(cache) 走行位置推測 - チームまるの日記（ETロボコン2012には、、でない）

走行位置推測について考え方をまとめてみました。
実装にあたってはネットや過去のモデルを漁りましたが、大体どこでも似たような感じでやっているみたいです。
内容についてツッコミ歓迎です。

1. 下記のイメージで左右タイヤの回転角度（動いた角度）を取ります。

　

2. 次に回転角度から距離を算出します。
　タイヤの進んだ距離Ｌ＝ 2πr * (回転角度Ｌ / 360)
　タイヤの進んだ距離Ｒ＝ 2πr * (回転角度Ｒ / 360)

　「r」はタイヤの半径です。
　「2πr」で円周が出ます。それに対して「回転角度Ｌ(度) / 360(度)」の割合を掛ける事で扇形の弧の長さが出る考え方です。

3. 左右タイヤの動いた距離からNXT自体が移動した距離を出します。
　NXTの移動した距離＝ (タイヤの進んだ距離Ｌ + タイヤの進んだ距離Ｒ) / 2

　

4. NXTの旋回角度を出します。
　孤＝右タイヤ移動距離 - 左タイヤ移動距離
　旋回角度＝孤長 / 車軸長

　

　弧の求め方については、少々理解が出来ていません。
　左タイヤ移動距離がゼロの場合は良いのですが、ゼロでない場合は厳密な旋回角度は算出できていないと思う為です。
　ただ、この演算でそれなりのプロットが出来るので、許容誤差という感じでしょうか。
　なので、上図の赤の扇形のイメージは間違っているかも知れません。

　旋回角度は、「θ= 弧 ÷ 半径」で求めます。単位はrad（ラジアン）です。
　この式については、適当にググれば理解可能です。

　求まった旋回角度は上図のθ（シータ）となります。

5. 座標を求めるための要素を出します。

　

　座標を求めるにあたり、上図の「角度Ｂ」が必要になります。
　三角形の内角の和は180度なので、回りまわって「角度Ｂ＝θ/2」となります。

　
　
　これで座標を求める要素が揃ったことになります。
　
　※NXTの移動した距離が「弧」だったのに、ここで突然直線に変わっています。
　　このまま計算を進めると曲線をピンとさせた分、実際に移動した距離より遠くに移動する事になりそうです。
　　が、なぜがそれなりにプロットされます。誰か教えてください。許容出来る誤差範囲？

6. cos、sinを使って移動後の座標を算出します。
　分かりやすいように図の向き90度回転します。

　

　直角三角形の底辺は、斜辺にcos(角っちょの角度)をかけると求まります。
　これが、前方に進んだ距離となります。
　左右に進んだ距離もsinで同じ様に求まります。

　ただし、これだと１回分の旋回角度の座標しか求まらないので、連続した走行を考慮すると以下の形になります。

　

前方に進んだ距離、左右に進んだ距離をX、Yの座標に置き換える事により走行位置を知ることができます。