目次

• 始めに
• 動作原理
• 動作の流れ
• 動作試験

1.始めに

本ドキュメントは、MIRSSTNDの行動制御プログラム中で使用する中心位置補正用関数の機能と動作原理について述べたものである。

2.動作原理

始めの座標系の原点を0とし、ポスト探索＋獲得動作後に戻ってきた座標系の原点を0'とする。MIRS標準機はポストの位置と角度を一回目と二回目それぞれ二つ取れたとする。このときのポストと本体の関係は以下のように表せる。ただし、角度のずれをδθ,位置のずれをそれぞれδx,δyとし、ポストまでの距離をr,その角度をθとする。

1. 全体の関係
   
2. 補正式の導出のための図
   
3. 補正式の導出(余弦定理より)
   

求められたδθ,δx,δyによって、中心位置を補正するが、これは1回目と2回目のポスト探索が正確である必要がある。また、ポストの数が最低二個ない場合や角度が大きくずれた場合は補正が適用できなくなるので注意する。

3.動作の流れ

1. 構造体post及びpost_tmpからポストの角度と距離を取得する。(データ列の先頭から二つ分のみ)
2. データからδθを計算する。(解は二つ得られる)
3. 二つの解から最適解を導く。(誤差が少ないという仮定により、一方にしぼる)
4. 一つに決められたδθよりδx,δyを計算する。
5. δθ,δx,δyより、元の位置へ戻る最短の行動を計算する。
6. 回転、直進、回転の最短の行動をとることで、もとの位置へ戻る。

4.デバッグメッセージについて

correct_center()を実行すると、計算結果をデバッグメッセージとして出力する。その出力例を以下に示す。

=============Debug Message=============
ans_b + ans_a = [deg]   // 求めたδθ1
ans_b - ans_a = [deg]   // 求めたδθ2
dtheta     : [deg]      // 誤差が少ないと仮定したδθ
dx         : [mm]       // そのδθから求めたx方向のズレ
dy         : [mm]       // そのδθから求めたy方向のズレ
alpha      : [deg]      // tan(dy/dx)
beta(deg1) : [deg]      // 元に戻るときの最初の回転角度
deg2       : [deg]      // 直線移動後の回転角度
distance   : [mm]       // 元の位置への直線移動距離
==============End Message==============