ジャイロコンパス+測距輪でのナビゲーション(4)
ROSさんお手やわらかに/花岡ちゃんのウィークエンド
今回の実験機にはPIC24FJ64GA002-I/SPを使用し、スタンドアロンで自己位置推定をさせています。まず、その部分の概要を説明します。
最初に実験機の座標系です。移動面の座標系と方位の関係は図のようになっています。
つまり、方位0がY軸方向、方位90がX軸方向になっています。自己位置推定のコードは、シェーキーのモノを流用しました。ROSでの座標系と合わせようかとも思いましたが、手早く実験したかったので効率優先です。
目標位置への方位計算(目標方位計算)のコードもシェーキーのものを流用しましたが、遠距離射撃方式のシェーキーとは異なり、測距輪の1カウントごとに自己位置と目標方位を再計算させています。実験機がシェーキーのようにきちんと直進できないためですが、もちろん、このほうが普通の方式です。シェーキーは60年代のテクノロジーを再現したものなので、当時の計算機で処理できるよう、計算量の少ない方式をとっているのです。
計算はdoubleの変数を使い、三角関数を使っていますが、PIC24Fで実時間的に処理が出来ています。安いマイコンにしてはなかなかの性能ではと思います。
このシンプルなアルゴリズムで、もう少し複雑な経路を動かしたときの動画を撮りました。14の目標地点を順番にたどらせています。どの目標地点もまあまあの精度でクリアしていきます。目標点を中心に半径15cmほどの円を描いたとき、ロボットの中心がだいたいその中に収まる程度ですが、実用上は何とか使えそうです。ジャイロコンパスを使用しているため、旋回誤差が累積しないのが効いています。