オドメトリ


オドメトリ(odmetry)とは,独立二輪型車両ロボットの車輪の回転速度から移動速度を求め,それを積分して位置と姿勢を求める自己位置推定法.

概要

厳密な表記

以下のようにモデル化される独立二輪型車両ロボットを考える.
 \begin{bmatrix} \dot{x}\\\dot{y}\\ \dot{\theta} \end{bmatrix} = \begin{bmatrix} v \cos\theta\\ v \sin\theta\\ \omega \end{bmatrix}
ただし,平面空間M:=\mathbb{R}^2に対し,(x,y,\theta)\in M\times S^1,\displaystyleは状態量であり,ロボットの位置,ロボットの姿勢角を意味している.また,v,\omega\in \mathbb{R}は制御入力であり,ロボットの併進速度,ロボットの角速度を意味している.

時間の分割\{t_i|t_1=0,t_\infty = \infty,t_i<t_{i+1},i \in \mathbb{N} \}に対し,v,\omega\displaystyle[t_i,t_{i+1})\displaystyleの時間で一定の値v(t_i),\omega(t_i)\displaystyleをとるとする.また,時刻0におけるロボットの初期位置・姿勢角x(0),y(0),\theta(0)\displaystyleは既知であるとする.すると,区間<a href="t_i,t),(t\in" target="_blank">[t_i,t_{i+1}))において\dot{x},\dot{y},\dot{\theta}\displaystyleは連続であるから[http://ja.wikipedia.org/wiki/%E7%A9%8D%E5%88%86%E6%B3%95#.E3.83.AA.E3.83.BC.E3.83.9E.E3.83.B3.E7.A9.8D.E5.88.86 リーマン積分可能である.t\in [t_i,t_{i+1})\displaystyleに対し,ロボットの位置,姿勢角は以下のように求まる.

(1)    \begin{align*} \begin{bmatrix} x(t)\\y(t)\\ \theta(t) \end{bmatrix} &= \begin{bmatrix} x(0)\\y(0)\\ \theta(0) \end{bmatrix} + \int_{t_i}^{t} \begin{bmatrix} v(t_i) \cos\theta(s)\\ v(t_i) \sin\theta(s)\\ \omega(t_i) \end{bmatrix} ds + \sum_{j=0}^{j=i-1} \int_{t_j}^{t_{j+1}} \begin{bmatrix} v(t_j) \cos\theta(s)\\ v(t_j) \sin\theta(s)\\ \omega(t_j) \end{bmatrix} ds\\ &= \begin{bmatrix} x(t_i)\\y(t_i)\\ \theta(t_i) \end{bmatrix} + \int_{t_i}^{t} \begin{bmatrix} v(t_i) \cos\theta(s)\\ v(t_i) \sin\theta(s)\\ \omega(t_i) \end{bmatrix} ds \end{align*}

第2式右辺第2項である積分式は解析的に解くことが出来る.\theta(t)=\theta(t_i)+\int_{t_i}^{t}\omega(t_i)ds=\theta(t_i)+(t-t_i)\omega(t_i)\displaystyleに注意し,三角関数の和積公式を使うと,

(2)    \begin{align*} \int_{t_i}^{t} \begin{bmatrix} v(t_i) \cos\theta(s)\\ v(t_i) \sin\theta(s)\\ \omega(t_i) \end{bmatrix} ds = \left\{ \begin{array}{lr} \begin{bmatrix} (t-t_i)v(t_i)\cos\theta(t_i)\\ (t-t_i)v(t_i)\sin\theta(t_i)\\ (t-t_i)\omega(t_i)\\ \end{bmatrix}&(\omega(t_i)=0)\\ & \\ \begin{bmatrix} 2\frac{(t-t_i)v(t_i)}{(t-t_i)\omega(t_i)}\sin(\frac{(t-t_i)\omega(t_i)}{2})\cos(\theta(t_i)+\frac{(t-t_i)\omega(t_i)}{2})\\ 2\frac{(t-t_i)v(t_i)}{(t-t_i)\omega(t_i)}\sin(\frac{(t-t_i)\omega(t_i)}{2})\sin(\theta(t_i)+\frac{(t-t_i)\omega(t_i)}{2})\\ (t-t_i)\omega(t_i)\\ \end{bmatrix}&(\omega(t_i)\neq 0) \end{array} \right. \end{align*}

ただし,右辺(\omega(t_i)\neq 0) \displaystyleの場合に,\frac{(t-t_i)v(t_i)}{(t-t_i)\omega(t_i)} \displaystyleを意図的に約分せずに残した.

多くの独立二輪型車両ロボットは左右それぞれのタイヤの移動量を計測することが出来る.左右のタイヤに対し,時刻0\displaystyleから時刻t\displaystyleまでのタイヤ移動量をD_l,D_r:\mathbb{R}_{\geq 0}\to \mathbb{R}\displaystyleとすると,本稿でおいた仮定のもとで,以下が成立する.

(3)    \begin{align*} (t-t_i)     v(t_i)&=\frac{(D_r(t)-D_r(t_i))+(D_l(t)-D_l(t_i))}{2}\\ (t-t_i)\omega(t_i)&=\frac{(D_r(t)-D_r(t_i))-(D_l(t)-D_l(t_i))}{W} \end{align*}

ただし,W\displaystyleはトレッド(左右のタイヤの中心間距離)である.

以上により,区間[t_i,t)\displaystyleでの状態量x,y,\theta\displaystyleの変化量は測定可能な物理量D_l,D_r\displaystyleと状態量のみで表現できることが分かる.

考察

仮定の物理的意味

本記事でおいた仮定は,時刻の分割\{t_i\}\displaystyleに対し,各区間[t_i,t_{i+1}]\displaystyleでロボットは円運動/直進運動をしているという物理的な仮定を定式化したものである.この仮定の元では,ロボットの位置を正確に特定することが出来る.

しかし,実際のロボットは円運動と直進運動を不連続に切り替えることは出来ない.(分割の幅を十分小さく取れば,仮定と実際の挙動の間のずれは軽減される)そのため,各区間[t_i,t_{i+1}]\displaystyleで微小な誤差が累積する.ゆえに,自己位置推定法として使うためには,他のセンシング法とセンサーフュージョンをかけることで,誤差をキャンセルする必要がある.

数値計算上の問題

\omega\to 0\displaystyleの際に,\frac{\sin(\frac{(t-t_i)\omega(t_i)}{2})}{(t-t_i)\omega(t_i)}\displaystyleの分母・分子の項が0に収束する.ゆえに,\omega\simeq 0\displaystyle時,数値計算上で0割りが発生する.

これに対し,\omega \cong 0 \displaystyleの場合に,\omega = 0\displaystyleとみなす方法が考えられる.この場合分けは\omega \cong 0\displaystyleの時に,限りなく半径が∞に近い円運動を直進運動に近似しているという幾何的な解釈が出来る.(近似の精度は\omega \displaystyleに対して1次)ここで,t-t_i\displaystyleが微小な内は問題はないが,そうでないときに誤差は増大する.ゆえに,オドメトリは数値計算を考えると,分割幅t_{i+1}-t_i\displaystyleを十分小さくすることで,誤差の累積を防止する必要がある.

他にも,意図的に0割りを発生させることで,自動で条件分岐をさせるコーディングも考えられる.

テイラー展開を利用した積分項の近似計算

\omega\to 0\displaystyleの際に,\frac{\sin(\frac{(t-t_i)\omega(t_i)}{2})}{(t-t_i)\omega(t_i)}\displaystyleの分母・分子の項が0に収束するため,場合分けが必要となる.

どうせオドメトリはt-t_i\displaystyleが微小である必要があるので,この部分を近似してしまえば,場合分けは不要である.

例えば,\sin \displaystyle関数をテイラー展開して,

(4)    \begin{align*} \frac{\sin(\frac{(t-t_i)\omega(t_i)}{2})}{(t-t_i)\omega(t_i)} &=\frac{1}{(t-t_i)\omega(t_i)}\left( \frac{(t-t_i)\omega(t_i)}{2}-\frac{1}{3!}(\frac{(t-t_i)\omega(t_i)}{2})^3+\frac{1}{5!}(\frac{(t-t_i)\omega(t_i)}{2})^5\cdots\right)\\ &=\frac{1}{2}-\frac{1}{2^3\cdot 3!}((t-t_i)\omega(t_i))^2+\frac{1}{2^5\cdot5!}((t-t_i)\omega(t_i))^4\cdots \\ &\cong \frac{1}{2}-\frac{1}{48}((t-t_i)\omega(t_i))^2+\frac{1}{3840}((t-t_i)\omega(t_i))^4  \end{align*}

とすればよい.

このように,数値計算を前提とすると,与えられた時間の分割において円運動/直進運動をするという仮定に加え,ここで挙げた近似を追加でかけることになる.すなわち,\omega \cong 0\displaystyleの時場合分けする手法なら,\omega \cong 0\displaystyleの時のみに,\omega \displaystyleに対して1次の近似を行う.それに対し,この手法は,常に,(t-t_i)\omega \displaystyleに対してn次の近似を行うことになる.(nはテイラー展開で打ち切った次数.本稿の例だと5次)

物理モデルに対する仮定の流れ

物理モデルからここまでの議論でなされてきた仮定を整理すると,以下のような流れになる.

物理
↓仮定1:車両は横滑りしない,平面上を動く等
微分方程式でシステムを表現

この段階では積分不可能(v,\omega\displaystyleが不連続で,方程式全体がルベーグ積分不可能になるかもしれない)

 \begin{bmatrix} \dot{x}\\\dot{y}\\ \dot{\theta} \end{bmatrix} = \begin{bmatrix} v \cos\theta\\ v \sin\theta\\ \omega \end{bmatrix}
↓仮定2:車両は時刻の分割に対して円運動/直進運動
積分可能になった.解軌道が具体的に得られる.

この段階までなら時刻の分割を小さくとる必要がない.その間円運動を行い続けるなら,t_{i+t}-t_i\displaystyleの長さを1時間としても良い.
ロボットがクロソイド曲線を描くような挙動の場合は,t_{i+t}-t_i\displaystyleを小さくすることで,ある程度対応できる.
\omega \cong 0\displaystyleの時に数値計算誤差が増大.
0割りとそうでない場合の分岐は,0割りエラーによる割り込み機能を使った実装がスマート.(マイコンがその機能を持っていれば)

(5)    \begin{align*} \int_{t_i}^{t} \begin{bmatrix} v(t_i) \cos\theta(s)\\ v(t_i) \sin\theta(s)\\ \omega(t_i) \end{bmatrix} ds = \left\{ \begin{array}{lr} \begin{bmatrix} (t-t_i)v(t_i)\cos\theta(t_i)\\ (t-t_i)v(t_i)\sin\theta(t_i)\\ (t-t_i)\omega(t_i)\\ \end{bmatrix}&(\omega(t_i)=0)\\ & \\ \begin{bmatrix} 2\frac{(t-t_i)v(t_i)}{(t-t_i)\omega(t_i)}\sin(\frac{(t-t_i)\omega(t_i)}{2})\cos(\theta(t_i)+\frac{(t-t_i)\omega(t_i)}{2})\\ 2\frac{(t-t_i)v(t_i)}{(t-t_i)\omega(t_i)}\sin(\frac{(t-t_i)\omega(t_i)}{2})\sin(\theta(t_i)+\frac{(t-t_i)\omega(t_i)}{2})\\ (t-t_i)\omega(t_i)\\ \end{bmatrix}&(\omega(t_i)\neq 0) \end{array} \right. \end{align*}

↓仮定3:\omega \displaystyleが十分小さい時は,円弧運動を直進運動に近似 ↓仮定3:(t-t_i)\omega \displaystyleは常に十分小さい
数値計算によって生じる誤差を軽減.

(\omega(t_i)=0)\displaystyleor(\omega(t_i)\neq 0)\displaystyleの場合分けを,

(\omega(t_i)\not\cong 0)\displaystyleor(\omega(t_i)\cong 0)\displaystyle
と解釈する.

(6)    \begin{align*} &\frac{\sin(\frac{(t-t_i)\omega(t_i)}{2})}{(t-t_i)\omega(t_i)}\\ &\cong \frac{1}{2}-\frac{1}{48}((t-t_i)\omega(t_i))^2+\frac{1}{3840}((t-t_i)\omega(t_i))^4  \end{align*}

と解釈する

仮定2によって生じる誤差が大きい場合,いくら仮定3によって数値計算による誤差を軽減したとしても,無意味であることに注意する.実際にロボットはどのような動きをするのか,コントローラーの演算精度はどの程度なのかを考えて,どの程度の仮定をしている理論を採用するのか検討した方が良い.

実装

分割はt_{i+1}-t_i=10msec\displaystyle程度にとればよい.
10msec毎に割り込みをかけ,逐次積分項を計算すればよい.

参考文献

外部リンク