目次
はじめに
よく忘れて何度も調べなおしていたので、
自分用のメモです。
使用する変数
ω: 回頭速度(ヨーレート) [rad/s]
v: 前進速度 [m/s]
s: ステアリング角度 [rad]
κ:曲率 [1/m]
R: 旋回半径 [m]
W: ホイールベース(前後のタイヤの距離) [m]
T: トレッド(左右のタイヤの距離) [m]
x,y: ロボットの位置[m]
θ: ロボットの方位 [rad]
dt: 刻み時間 [s]
ヨーレートと曲率と前進速度の関係
Eq. 1
[rad/s] = [m/s] × [rad/m]
radは無次元であるため [1/m]=[rad/m]
旋回半径と曲率の関係
Eq. 2
[m] = 1 / [1/m]
曲率と円の半径の関係より
ステアリング角と旋回半径の関係
冒頭の図より、
Eq. 3
[m] / [m] = [1] (rad)
(ステアリング角が、RとWのサイン角になるのは、錯角の関係より)
ここで、Eq.2 を使用すると、
Eq.4
同様に,Eq. 1を使用すると,
Eq.5
となり、Eq.5を使用することにより、
ある前進速度の時に、任意のヨーレートで曲がりたい時に
制御すべき目標ステアリング角度が計算できる。
しかし、前進速度が0の時は、0割りになるので注意
ヨーレートと前進速度とステアリング角度の関係
Eq. 5を変形させると、
ある前進速度で、あるステアリング角度を切った時の
理論的なヨーレートを計算することができる。
これにより、ジャイロセンサを使用しなくても、
車速センサーとステアリングセンサを使用して、
ヨーレートを計算することができる。
このようにヨーレートを計算する方法を
ロボットの運動モデルによるヨーレート推定と呼ぶ。
移動ロボットではこの運動モデルによるヨーレート推定値と
ジャイロによるヨーレート推定値を組み合わせる方法として、
Gyrodometryという方法を使用するのが一般的
論文リンク：
Gyrodometry: A New Method for Combining ... - Valentiniweb.com
ステアリング速度と曲率変化量の関係
先程のEq3より、
ステアリング角度sが小さい時、
sinx=xと近似できるため、
下記の式が得られます。
ここで、曲率とステアリング角度は時刻で変化すると考えると、
それらをそれぞれ微分できます。
上記の式によりステアリング速度と、
曲率の変化量を定式化することができました。
ステアリング速度を入力する場合もあるので、
その時にこの式を使う時があります。
Wheel Odometryにおける運動モデル
Wheel Odometry (WO)はタイヤの回転数から
ロボットの相対移動量を計算する方法です。
相対的な移動量を使用してロボットの位置を計算する方法を
推測航法（デットレコニング）といい、
WOはその一種です。
デットレコニングによる位置計測システムの問題点に関しては、
下記の記事を参考にして下さい。
Dead Reckoning: デットレコニングの問題点 (ロボットの自己位置推定） - MY ENIGMA
WOによるデットレコニングに関しては、
微小時間のロボットの移動を
直線と近似するか、円運動と近似するかで
2つの代表的な運動モデルがあります。
下記でこの2つのモデルの説明をしたいと思います。
直線運動モデルによるWO
上図のように、直線運動モデルでは、
ある時刻tとその次の時刻t+1
の間の移動を直線であると仮定します。
まず始めに、時刻tの時の速度をvt,ヨーレートをωtとした時、
ヨーレートによる方位の変化を、
直線移動の後に生じたと仮定すると、
時刻t+1の時の位置と姿勢は下記のようになります。
速度とヨーレートに刻み時間dtをかけることにより、
WOの計算をする際の実際の移動量と方位変化量を計算し、
それらを幾何学的な関係から加えただけです。
方位の変化が移動の後に行われたと仮定しているため、
x,yの計算の際に、θtを使用しています。
もし方位の変化が移動の前に行われたと仮定すると
下記のように、
θt+1を先に計算し、x,yの計算の際にθt+1を使用することになります。
しかし、上記の方法ではdtが十分に短くない場合には、
常にロボットの移動方向が遅れるか早まる可能性があり、
WOの観測量にオフセット誤差が生じてしまう場合があります。
ここで、
より高精度にロボットの移動をモデル化する化するために、
微小時間の間のヨーレートは一定とし、
x,yの移動は微小時間の
ちょうど真ん中の時刻の方位を使用するようにします。
すると運動モデルは下記のようになります。
このような方法を使用すると、
モデルによるオフセット誤差はかなり少なくなります。
しかし、
微小時間の値が限りなく小さい場合、
上記のどのモデルを使用しても、
あまり精度の差はなくなります。
円運動モデルによるWO
続いて、円運動モデルによるWOの説明をします。
先ほどの直線運動モデルでは、
微小時間内のロボットの移動を直線移動で近似していましたが、
ヨーレートが大きいと近似誤差が生じてしまいます。
円運動モデルでは上記の図のように、
微小時間内の移動を円運動として近似します。
上図のように回転移動の中心を[xc,yc]とすると、
回転中心と、時刻tのロボットの位置、
そして回転中心のX軸とロボットの方位方向の交点がなす
直角三角形を考えます。
すると、上図のようにロボットの方位がθであり、
ロボットの位置を頂点とする角は直角三角形の直角であることを考えると、
中学生ぐらいの時に勉強した
ある外角は隣り合わない内角の和に等しいので、
回転中心と時刻tのロボットの位置がなす角度はθ−90度になります。
すると回転中心の位置[xc,yc]は、
となります。
ここで高校の時に習った加法定理を使うと、
という関係が導き出されるので、
先ほどの式は
となります。（Eq.3)
続いて、
時刻t+1の時の位置と姿勢は
先ほどの回転中心と時刻tのロボットの位置を考えた時と同様に
となります。(先ほどの式と回転中心と時刻t+1の時の位置が逆になっているので注意）
すると、先ほどの加法定理の関係と
Θt+1=Θt+ωdt
という関係から、
となります。
最後にこの式とEq.3を組み合わせて、回転中心の座標を消去し、
冒頭のEq.1とEq.2から、回転半径Rは
R=v/ω
となるので、最終的な運動モデルは
となり、時刻tの時の位置と速度、ヨーレートが分かれば、
時刻t+1の時の位置が計算されます。
力学二輪モデルと運動二輪モデルの比較
ステアリング型のロボットの二輪モデルは、
力学的関係から導出される力学的二輪モデル(Kinematic Bicycle Model)と、
動力学関係から導出される動的二輪モデル(Dynamic Bicycle Model)の２つがあります。
下記の図において、
力学的二輪モデルは下記の式で表されます。
一方、動的二輪モデルは下記の式で表されます。
前述の通り、全後輪にかかる横方向の力Fは、
各車輪のすべり角とコーナリングパワーで決まります。
上記の２つのモデルを比較すると、
力学的モデルは、計算が単純で、高速に実行できますし、
車両パラメータが重心位置を決めるためのlrとlfのみなので
シンプルです。
また、動的二輪モデルで発生する速度が遅い時に、
すべり角の計算ができなくなり、
計算が不安定になると言った問題もありません。
また、タイヤパラメータも不要です。
しかし、
速度が早い場合、力学的モデルは、
横すべりを考慮していないため、
推定精度が低下することがあり、
その場合は動的モデルの方が精度がよくなります。
