こんにちは。koraです。
今回はオドメトリ（自己位置推定）をできるようにします。
以前の制御では迷路の壁に沿って進んでいたので、正確な自己位置を把握する必要はなかったのですが、今回は目標軌道に追従させるような制御をしたいので実装することにしました。
オドメトリの実装
計算方法
制御周期を1msとしますので、制御周期ごとに車体の角度と移動距離を計算して積算していけば、ロボットの自己位置を推定することができます。
- ジャイロの出力値から[度/秒]の車体の角速度が得られるので、単位を[rad/ms]に変換して1ms前の車体の角度[rad]に足し合わせることで、現在の車体の角度[rad]になります。
- エンコーダ出力値（タイヤの絶対角度）に対して、1ms前の値との差分をとるとタイヤの角速度[rad/ms]が得られます。これにタイヤの直径[mm]を掛けると車体の進行方向の移動距離[mm/ms = m/s]が得られます。
- 車体の角度と進行方向の移動距離から、X方向、Y方向の移動距離をそれぞれ求めます
- X方向の移動距離 = 車体の進行方向の移動距離 * cos(車体の角度)
- Y方向の移動距離 = 車体の進行方向の移動距離 * sin(車体の角度)
実際には1ms間も角度が変わり続けているのですが、ここでは計算を簡単にするため角度一定と仮定しています。また、タイヤの滑りなどで誤差が出ますので、後々補正を入れる必要があります。
モジュールの実装
エンコーダのモジュールを作ります。Srcフォルダにodom.cを、Incフォルダにodom.hを作って、上記の計算を実装します。また、タイマ割り込みで制御周期ごとに呼び出されるようにします。
動作確認
実際にマウスを動かして、角度と座標を計算できていることを確認できました！
次回
次回から、目標軌道の生成とその追従制御に取り掛かりたいと思います。