読者です 読者をやめる 読者になる 読者になる

MyEnigma

とあるエンジニアのブログです。#Robotics #Programing #C++ #Python #MATLAB #Vim #Mathematics #Book #Movie #Traveling #Mac #iPhone

ステアリング型ロボットの運動モデル

Robot


はじめに

よく忘れて何度も調べなおしていたので、

自分用のメモです。


使用する変数

ω: 回頭速度(ヨーレート) [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と近似できるため、

下記の式が得られます。

f:id:meison_amsl:20161011090828p:plain

ここで、曲率とステアリング角度は時刻で変化すると考えると、

それらをそれぞれ微分できます。

f:id:meison_amsl:20161011090914p:plain

上記の式によりステアリング速度と、

曲率の変化量を定式化することができました。

ステアリング速度を入力する場合もあるので、

その時にこの式を使う時があります。

 

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)の2つがあります。

 

下記の図において、

f:id:meison_amsl:20161106053105p:plain

力学的二輪モデルは下記の式で表されます。

f:id:meison_amsl:20161106053224p:plain

 

一方、動的二輪モデルは下記の式で表されます。

f:id:meison_amsl:20161106053352p:plain

前述の通り、全後輪にかかる横方向の力Fは、

各車輪のすべり角とコーナリングパワーで決まります。

f:id:meison_amsl:20161106053649p:plain

 

上記の2つのモデルを比較すると、

力学的モデルは、計算が単純で、高速に実行できますし、

車両パラメータが重心位置を決めるためのlrとlfのみなので

シンプルです。

また、動的二輪モデルで発生する速度が遅い時に、

すべり角の計算ができなくなり、

計算が不安定になると言った問題もありません。

また、タイヤパラメータも不要です。

しかし、

速度が早い場合、力学的モデルは、

横すべりを考慮していないため、

推定精度が低下することがあり、

その場合は動的モデルの方が精度がよくなります。

 

参考資料

myenigma.hatenablog.com

myenigma.hatenablog.com
myenigma.hatenablog.com
myenigma.hatenablog.com

Principles of Robot Motion: Theory, Algorithms, and Implementations (Intelligent Robotics and Autonomous Agents series)

Principles of Robot Motion: Theory, Algorithms, and Implementations (Intelligent Robotics and Autonomous Agents series)

  • 作者: Howie Choset,Kevin M. Lynch,Seth Hutchinson,George A. Kantor,Wolfram Burgard,Lydia E. Kavraki,Sebastian Thrun
  • 出版社/メーカー: A Bradford Book
  • 発売日: 2005/05/20
  • メディア: ハードカバー
  • この商品を含むブログを見る
自動車の運動と制御 第2版─車両運動力学の理論形成と応用

自動車の運動と制御 第2版─車両運動力学の理論形成と応用

車はなぜ曲がるか?―限界コーナリングのダイナミクス (MECHANISM SERIES)

車はなぜ曲がるか?―限界コーナリングのダイナミクス (MECHANISM SERIES)