オドメトリ

HOME>>メモ>>天地人メモ>>オドメトリ

オドメトリとは？

ロボットの走行経路を制御するには、ロボットが今どこにいるのかを知ること、自己位置推定が必要となります。 GPSを使ったり、カメラで撮影た床や天井の映像から推定したりといった様々な手法が提案されています。 しかし、これらの手法は非常に複雑な処理が必要となります。 そこで、比較的簡単な、タイヤの回転角から現在位置を推定する手法を考えてみましょう。 この様な手法は、オドメトリやデッドレコニングといいます。

直線近似

制御モデルの作成で扱ったように ロボットの速度・角速度と、タイヤの速度関係は次のように表すことができます。

\begin{eqnarray*} v_\mathrm{R} &=& v + d\omega \\ v_\mathrm{L} &=& v - d\omega \end{eqnarray*}

この関係から逆に、タイヤの速度から、ロボットの速さと角速度を求めることができます。

\begin{eqnarray*} v &=& \frac{v_\mathrm{R}+v_\mathrm{L}}{2} \\ \omega &=& \frac{d(v_\mathrm{R}-v_\mathrm{L})}{2} \end{eqnarray*}

これから以下の微分方程式を解けば良いことがわかります。

\begin{eqnarray*} \frac{\mathrm{d}x}{\mathrm{d}t} & = & v\cos\theta\\ \frac{\mathrm{d}y}{\mathrm{d}t} & = & v\sin\theta\\ \frac{\mathrm{d}\theta}{\mathrm{d}t} & = & \omega \end{eqnarray*}

この微分方程式は解析的に解くことができません。 したがって次式を用いて数値積分を行うことになります。

\begin{eqnarray*} x_{i+1} & = & x_{i}+v_{i}\cos\theta_{i}\Delta t\\ y_{i+1} & = & y_{i}+v_{i}\sin\theta_{i}\Delta t\\ \theta_{i+1} & = & \theta_{i}+\omega_{i}\Delta t \end{eqnarray*}

これはロボットの移動を直線で近似することと対応します。

円弧近似

もう少し詳しくロボットの移動を見てみましょう。 微小時間$\Delta t$の間のロボットの速さはほぼ一定と考えられ、この間ロボットは円弧軌道を描くと見なせます。

ロボットの移動する軌跡
図1 ロボットの移動する軌跡

図1のように微小時間$\Delta t$の間に、 ロボットが半径$\rho$円運動をして、向きが$\Delta\theta$だけ変化したとします。 このとき、右と左のタイヤが進んだ距離$\Delta l_\mathrm{R}, \Delta l_\mathrm{L}$は次のようにあらわされます。

\begin{eqnarray*} \Delta l_{\mathrm{R}} & = & (\rho+d)\Delta\theta\\ \Delta l_{\mathrm{L}} & = & (\rho-d)\Delta\theta \end{eqnarray*}

これから、次の関係が得られます。

\begin{eqnarray*} \Delta\theta & = & \frac{\Delta l_{\mathrm{R}}-\Delta l_{\mathrm{L}}}{2}\\ \rho\Delta\theta & = & \frac{\Delta l_{\mathrm{R}}+\Delta l_{\mathrm{L}}}{2} \end{eqnarray*}

この間ロボットが移動したかを考えてみましょう。 上の関係と、三角関数の和を積に直す公式を使うと次のように書けます。

\begin{eqnarray*} \Delta x & = & \rho(\sin(\theta+\Delta\theta)-\sin\theta) \\ &=& 2\rho\cos\left(\theta+\frac{\Delta\theta}{2}\right)\sin\left(\frac{\Delta\theta}{2}\right) \\ &=& \rho\Delta\theta\cos\left(\theta+\frac{\Delta\theta}{2}\right)\frac{\sin\left({\displaystyle \frac{\Delta\theta}{2}}\right)}{\displaystyle \frac{\Delta\theta}{2}} \\ &=& \frac{\Delta l_\mathrm{R}+\Delta l_\mathrm{L}}{2}\cos\left(\theta+\frac{\Delta\theta}{2}\right)\mathrm{sinc}\frac{\Delta\theta}{2} \\ \Delta y & = & \rho(-\cos(\theta+\Delta\theta)+\cos\theta) \\ &=& \frac{\Delta l_\mathrm{R}+\Delta l_\mathrm{L}}{2}\sin\left(\theta+\frac{\Delta\theta}{2}\right)\mathrm{sinc}\frac{\Delta\theta}{2} \end{eqnarray*}

ここで$\mathrm{sinc}x=\sin x/x$という形の関数が現れます。 これはsinc関数(シンクかんすう) と呼ばれる関数です。ここで出てきたものは、厳密には、非正規化sinc関数やカーディナル・サインと呼ばれます。

さて、このsinc関数、定義通りに計算するとx=0のとき0/0の不定形になってしまいます。 数学的にはsinc0=1と定義するのが普通ですが、コンピュータで扱う場合には少し注意が必要です。 x=0の近くでは次のような工夫をする必要があるでしょう。

  1. テーラー展開による多項式近似を行う
    sin関数のテーラー展開からsinc関数が次の多項式で表現できることは簡単にわかりますね。 \[ \textrm{sinc}x=1+\frac{1}{3!}x^{2}+\frac{1}{5!}x^{4}+\cdots \] 適当な項で打ち切ってsinc関数の近似値を求めます。
  2. $\textrm{sinc}x=1$としてしまう
    十分に$\Delta t$が小さければ、ロボットの向きの変化$\Delta\theta$も十分小さいと考えられます。 したがって、sinc関数の二次の項以降は無視できるほど小さいはずです。 そこで、sinc関数をx=0付近で一次近似し、$\textrm{sinc}x=1$と置いてしまいます。 天地人ではこの方法をとりました。

問題点

オドメトリはタイヤの回転角のみを使用するので、次のような問題があります。

  • タイヤの滑りを検出できない
    オドメトリはタイヤが滑らないことが前提となっています。 そのため、障害物に当たってタイヤが空回りしたり、ロボットがドリフトして横滑りがおこったりということがあると 正確に位置を推定できません。 駆動輪とは別に、オドメトリ用のタイヤを付けることで、タイヤの空回りはある程度防ぐことができます。 この際、駆動輪とオドメトリ用のタイヤは一直線上に並ぶようにしないと、横滑りが起こります。 この方法でもドリフトは検出できないので、スピードの出しすぎには注意です。
  • 誤差が累積し、どんどん大きくなる
    タイヤの滑りによる誤差、タイヤ径やロボットの大きさなどパラメータ誤差、数値計算による計算誤差など オドメトリには様々な誤差要因が存在します。 オドメトリでは、スタート時からの積算で位置を推定するため、これらの誤差は時間とともに累積してしまします。 そのため、ロボットが長距離を移動する場合、オドメトリ単体で高精度の位置推定を行うことは難しく、 他の手法と併用することが必要となります。