もみあげあしめ

組み込みとかIoTとか、いろいろ

倒立振子でライントレースする 自己位置推定編

右も左もわからないロボットに、右も左を教えてあげようではないか。

f:id:shima_nigoro:20161002221939p:plain:w300

この記事では倒立振子の現在位置を求める計算方法をまとめる。

目的と制約事項

  1. 倒立振子の中心(左右の車輪の中心)の現在位置を求める
  2. 左右の車輪の回転角度がわかる

材料

  1. 倒立振子の車輪の大きさ(半径) {r}
  2. 左右の車輪の間の幅 {w}
  3. 左右の車輪の単位時間での回転角度 {\theta_L(t)} と {\theta_R(t)}

計算する値

  1. 単位時間で進む距離 {L(t)}
  2. 単位時間で変わる方位 {O(t)}

数式

・進む距離
{L(t) = 2 \pi r * \frac{(\theta_L(t) + \theta_R(t)) / 2}{360}}

・進む方位
{O(t) = \arctan \frac{2 \pi r * ( (\theta_L(t) - \theta_R(t) ) / 360)}{w}}

シンプルにこれだけ。現在位置は毎単位時間で進んだ距離と方位から求まる。

サンプルコード

後日きれいにしてから載せます。