[GPSRCC]ロボットカーの制御について

先日、今回出場する大宮工業高校のはなしを少し聞いたりインタビューの記事を読んだところ、プログラムが難しいかもなぁって思いました。ので、今回はプログラムについて書きます。

こちらは機体がだいたいできあがりました！

【はじめに】
大前提として、使うのは「目的位置」と「現在位置」と「(現在)角度」の状態。今回は現在の状態をGPSと地磁気センサで取得しますが、GPSモジュールから出力されるデータの中に方位のデータがあるのでこれでもいいです。

そして作るのは「目的地にロボットを誘導するプログラム」で、特にルールにしばられるアルゴリズムではありません。巡回するルールでは目的地を順に変更していけばいいです。

【緯度経度の扱い方】
座標空間は緯度経度に定数を掛けて得る直交座標です。y軸正が北、x軸正が東となるようにとります。適当な定数を掛けた値なので単位はmでもmmでもなんでもよく、"単位距離"というのが妥当かもしれません。
ここで注意！世界地図や日本地図を見ると分かりますが方眼が北方向に伸びてますね。距離を正しく表そうとすると緯度には経度の1.2倍くらい掛ける必要があります。（1.2ってのは東京の緯度だとだいたいこれくらいという値です。）

図で見るとこんな感じ。

【目標方位の求め方】
　dx = (目標経度 - 現在経度)×定数
　dy = (目標緯度 - 現在緯度)×定数×1.2
まず目標位置と現在位置の差をこんな感じで定義します。
そしたら角度は逆三角関数arcTanをつかって
　θ = arcTan(dy/dx)
で求められます。（高校生だとここが難しいのかな）
プログラムを書く時はatan関数を使えばいいのですが、atan関数を使える状態であればatan2関数も使えるはずです。atan2はatanと違って値域を考えなくて良いので便利です。

図にして考えるといい。

【アルゴリズム】
上のような準備が済んだら
次の式で左右のモータに与えるゲイン（電圧）を決定します。

　左モータゲイン = 直進ゲイン - 回転ゲイン×(目標方位 - 現在方位)
　右モータゲイン = 直進ゲイン + 回転ゲイン×(目標方位 - 現在方位)

この式の直進ゲインと回転ゲインのパラメータを調整してください。簡単に説明するとこんな感じ。
直進ゲイン：目標にまっすぐ向かう時の速さ
回転ゲイン：目標の角度に向ける速さ

【これができたら】
次は競技に合わせて目標地点に着いたら、目標地点を変更するようにすればいいでしょう。
上で紹介したのは角度しか制御してませんが、距離や他のパラメータを制御して自分流にしてくのがいいと思います。が、GPSロボットカーコンテストについて言えるのはあまり凝った制御は必要ないということです。それよりはハードウェアの調整に力を注いだ方がいいとおもいます。

地磁気が指す北と緯度経度座標の北は違ったり、方位は北から右回り正の値なのに対して数学の角度は東から左回り正のあたいだったり、他にも注意しなきゃいけないことがあるはずです。