オドメトリによる移動ロボットの自己位置推定

オドメトリによる移動ロボットの自己位置推定アイキャッチ画像

はじめに

マイクロマウスを始めた2000年頃は、まだオドメトリを計算して自己位置を割り出すと言う事をほとんど見つけられなかったと思います。(書いてらっしゃった方がおられたらすいません。)

当時、僕はオドメトリによる自己位置推定をして、ライントレースロボットのように仮想の走行ラインを追随して高速走行する技術の獲得を夢見たものです。

そのために作ったマイクロマウスが「３式改」というマウスなのですが、このロボットは計測のためのロータリーエンコーダーに取り付けた計測輪がつけられていてほぼスリップすることなく走行軌跡を取得することができました。

マイクロマウスのオドメトリ走行

今回は、マイクロマウスのような、２輪ロボットのオドメトリについて話したいと思います。

はじめに
自己位置推定
- スターレコニング
- デッドレコニング
カウント値は離散的にしか得られない
移動ロボットの運動学（キネマティクス）
ホイールエンコーダから速度を知る
- 速度計算の実際
オドメトリ円弧近似
オドメトリ直線近似
オドメトリと誤差
おわりに

自己位置推定

ロボットにとって自分がどこにいるのかを知ることは大変重要です。移動ロボットならば目的地に着くためには今自分がいる位置を知らなければ、次にどこに行くかを決めることもできないですし地図を作ることもできません。ロボットがどこにいるのかをセンサから得られる情報から推定することを自己位置推定と言います。

スターレコニング

大航海時代の昔、船が航海するための指標になるものに星がありました。北極星などの目標になる星を六分儀を用いて角度をはかり、自己位置推定を行っていました。星やランドマークを使った自己位置推定をスターレコニングと呼びます。

六分儀とその使い方を知る素晴らしいつぶやきがあったので、共有させていただきました。

六分儀にはハーフミラーが組み込まれていて、こうやって水平線を基準にして太陽の位置を合わせると簡単に角度を知ることができる。 pic.twitter.com/S0AmkY5rS4
— MASA🌈🕒 (@masa_0083) January 5, 2019

六分儀で緯度はわかりますが、経度を知るには狂いのない時計が必要でそのような時計を作ることが大変難しく、安全な航海にはクロノメータの発明を待たなければなりませんでした。そのあたりの話は「経度への挑戦」と言う本に面白くまとめられています。

デッドレコニング

スターレコニングは外界を検知するセンサすなわち外界センサを用いて自分の位置を割り出します。それに対して、ロボットの速度や角速度、走行距離、方向の変位を検知する内界センサによる自己位置推定をデッドレコニングといいます。

速度が判っていれば、走行時間と合わせて考えることで走行距離が判ります。角速度がわかると同様に角度を求めることができます。

走行距離と角度が判れば、そこから自分が今どこにいるのかを割り出すことができます。

マイクロマウスのような２輪移動ロボットは、タイヤ回転を取得できるインクリメンタルロータリーエンコーダーを持つことで、移動量と回転量を知ることができます。

ロータリーエンコーダーを用いて移動した距離を測って自己位置を知ることをオドメトリと呼びます。

ちなみに距離計の事をオドメーターと呼びます。このオドという言葉は古代ギリシア語の「経路」をあらわす言葉だという事です。

ちなみに、ステッピングモータで走行するマイクロマウスは一般にはエンコーダを持ちませんが脱調等が起こらない限り、指令ステップ数からモータの回転角が判るので、エンコーダが付いているのと同じように考えることができます。

今回の記事では、エンコーダやステッピングモータステップ指令から得られるタイヤの回転角（走行距離）から如何にして自己位置の推定の計算をするのかを以下でひも解いていきたいと思っています。

カウント値は離散的にしか得られない

エンコーダ出力は通常マイコンに内蔵されているカウンタ回路に接続されます。カウンタ回路はその名の通り、エンコーダパルスを数えてくれます。マイコンは今何カウントになったか定期的に問い合わせをします。通常はカウンタの桁数が有限ですので、前の値との差分をとってメモリに積算していきます。

重要なことは、カウンタ値は一定の時間間隔でしか得られないことです。間隔の間にどのように変化したかはわからないためその間の走行経路はわかりません。したがって、何らかの仮定をおいて、その間の走り方を想定しなければ自己位置を推定することはできません、ただし闇雲にそれを決めることもできないので、それを考えるためにロボットの運動学を考えていこうと思います。

移動ロボットの運動学（キネマティクス）

ロボットの運動学で検索したらたくさん優れた記事があると思うので僕が改めて書いたらかえって混乱してしまうかもしれません。と言っても書かないと、記事の流れが判らなくなるので頑張って書きます。

加速度・速度・位置

ロボットの加速度を $a(t)$ とするとロボットの速度 $V(t)$ は加速度の積分としてあらわすことができます。

$\displaystyle{ V(t)=\int a(t) dt }$

速度 $V(t)$ の方向を $\psi(t)$ として、速度を水平成分と、垂直成分に分解します。

水平方向の速度成分は

$\displaystyle{ v_x=V(t) \cos \psi(t) }$

垂直方向の速度成分は

$\displaystyle{ v_y=V(t) \sin \psi(t) }$

位置情報は速度の積分ですので

水平方向の位置は

$\displaystyle{ x=\int V(t) \cos \psi(t) dt }$

垂直方向の位置は

$\displaystyle{ y=\int V(t) \sin \psi(t) dt }$

さて、ロボットの速度の方向 $\psi(t)$ はロボットの角速度 $\omega(t)$ の積分になりますから。

$\displaystyle{ \psi(t)=\int \omega(t) dt }$

加速度の積分が速度、速度の積分が位置という高校物理で習う程度のお話とまったく同じものです。

２輪移動ロボットの速度と角速度

以上はロボットの移動形態は問わず、どんな場合でも当てはまることに関して書きました。さて、僕が今回取り上げたいのは２輪移動ロボットです。 2輪と言ってもバイクや自転車形状ではなく、リヤカーやセグウエイのようなタイヤが向かい合った形のロボットです。

以下はタイヤが滑らない前提でお話を進めます。

右のタイヤの周速度を $v_R(t)$ 、左のタイヤの周速度を $v_L(t)$ としたときロボットの速度 $V(t)$ は次のようにそれぞれの速度の平均となります。

$\displaystyle{ V(t)=\frac{v_R(t) + v_L(t)}{2} }$

速度は判りましたが、角速度はどうなるでしょうか？

タイヤの間の距離を $W$ とします。また、ロボットの回転は反時計回りが正方向とします。すると、ロボットの角速度 $\omega(t)$ は以下のようになります。

$\displaystyle{ \omega(t)=\frac{v_R(t) - v_L(t)}{W} }$

以上がロボットの運動学です。

積分できるか

自己位置を計算するためには $V(t)$ や $\psi(t)$ が判ったうえで積分をする必要があります。はたして、ロボットが得ることができる情報からそれは可能でしょうか？

ホイールエンコーダから速度を知る

運動学においては自己位置を知るためには速度を積分しなければならないことが判りましたがホイールエンコーダから速度を求めることを考えてみます。

エンコーダの回転とタイヤの回転が１：１であるとします。エンコーダ1周のパルス数を $N$ とします。タイヤの半径を $r$ とすると1パルスのあらわす距離は次のようになります。

$\displaystyle{ \frac{2\pi r}{N} }$

したがって、エンコーダから得られるパルスが $P$ としたとき、その時の走行距離 $l$ は以下となります。

$\displaystyle{ l=\frac{2\pi r}{N}P }$

あくまでエンコーダから得られる情報は、タイヤの転がり距離情報であって、速度ではありません。ここから速度を得るためには、速度は位置の微分ですが微分は実現不可能なため差分にによる近似を用いなければなりません。

差分とは引き算の事ですから、エンコーダから時間を空けて二つの値を取得する必要があります。エンコーダから $\Delta t$ の時間を空けて2回取得した一つ古いパルス $P_{k-1}$ と最新のパルス $P_{k}$ これらの差分からその間の転がり距離情報が得られます。

速度は距離（位置）の微分なので微分の定義に立ち返って考えると次のように表現できます。

$\displaystyle{ V(t)=\dot l(t)=\lim_{h \to 0}\frac{l(t+h)-l(t)}{h} }$

先ほども微分ができないと書いていましたが、これはロボットに装備されるコンピュータは四則演算しかできないためです。極限の計算は理解できないので、位置情報を一定間隔で得られるロボット（コンピュータ）は速度を得るためにはどうにかして上の微分を四則演算で近似しなければなりません。

そこで、極限では $h$ を限りなく０に近づけるわけなので、上の式から極限の記号を取り去った式の中で、できるだけ $h$ を小さくしてやれば微分値に近くなるだろうと考えます。

したがいまして、以下の式が速度の近似値となります。

$\displaystyle{ V(t)=\frac{l(t+h)-l(t)}{h} }$

速度計算の実際

実際にコンピュータの中でロボットの速度を割り出すことを考えます。

様々なセンサーからデータを取得できるロボットは一定間隔でデータを取得しています。データは取得した順に並べることができるとすると、1番目のデータ、2番目のデータ、3番目のデータ、・・・、ｋ番目のデータ、ｋ＋１番目のデータ、・・・・・・とロボットが動き続ける間ずーっと何番目のデータというのが並んでいるのが確認できると思います。実際には並べてみることは普通出来ませんが、イメージ的にはそんな感じです。

いま、同じ時間間隔 $h$ 秒ごとにデータを取得するとします。時刻 $t$ の時にエンコーダから得れれる距離情報を $l(t)$ として、これを $k$ 番目のデータであるとします。そして $h$ 秒経過して得られた、その次のデータを $k+1$ 番目のデータという事にします。

そこでデータを次のように添え字付きで書きなおして、これまでの表現と対応させて書いてみると

$\displaystyle{ l_k=l(t) }$

$\displaystyle{ l_{k+1}=l(t+h) }$

速度についても同じように表現してみようと思いますが、一貫性を持たせるためにはやはり時刻 $t$ の時の速度がデータとしては $k$ 番目と考えるのがよさそうです。

$\displaystyle{ V_{k}=V(t) }$

これらで速度の近似式を書き換えてみましょう。

$\displaystyle{ V_k=\frac{l_{k+1}-l_k}{h} }$

速度については、 $k+1$ 番目の距離のデータを得られた時点では $k$ 番目の速度のデータしか推定できないことになります。

これはロボットの角速度 $\omega$ も同様で両輪のエンコーダパルスから計算したロボットの角度データにおいて $k$ 番目を $\psi_k$ 、 $k+1$ 番目を $\psi_{k+1}$ とすると以下のように表すことができます。

$\displaystyle{ \omega_k=\frac{\psi_{k+1}-\psi_k}{h} }$

オドメトリ円弧近似

積分計算により自己位置を計算するためには離散的にしか得られないデータの間の走行状態を仮定するしかありません。通常、ロボットでは一定時間間隔で制御を行います。その時間間隔はデータ取得の間隔（周期）と一致することが多いようです。

仮に、制御周期ごとにデータを取得しそれによって自己位置の推定をするとします。制御周期の間は制御指令は変えられませんので、ロボットは同じ速度指令、角速度指令で移動します。これは制御周期の間はロボットが同じ速度、角速度で動くことを意味するものでは無いのですが近似的にそれを認めようというのが表題の円弧近似です。

ロボットが同じ速度、同じ角速度で動いている間は円弧軌道を描きます。

以下の式は運動学の水平方向の位置及び垂直方向の位置の計算式の再掲載です。

水平方向の位置は

$\displaystyle{ x=\int V(t) \cos \psi(t) dt }$

垂直方向の位置は

$\displaystyle{ y=\int V(t) \sin \psi(t) dt }$

速度、角速度ともに一定としたとき、上式においてロボットの角度が角速度に比例して変化します。それを踏まえて上式を書き換えます。ここで積分範囲は $t_k$ から $t_{k+1}$ からとし積分結果はその間の増加量となります。

まずは水平方向からやってみます。

$\displaystyle{ }$

$\begin{eqnarray} \Delta x &=& V_k\int_{t_k}^{t_{k+1}} \cos \{\omega_k ( t-t_k) + \psi_k\} dt\\ &=& \frac{V_k}{\omega_k} \left[ \sin \{ \omega_k (t-t_k) + \psi_k \} \right ]_{t_k}^{t_{k+1}}\\ &=& \frac{V_k}{\omega_k}(\sin \phi_{k+1} - \sin \phi_k) \end{eqnarray}$

エンコーダに問い合わせをして得られる情報は制御周期間の変化量が得られることが多いので $t_k$ から $t_{k+1}$ の間に変化したロボットの角度を $\Delta \psi_k$ とし、上式に積和の公式を適用すると。

$\begin{eqnarray} \Delta x = \frac{2V_k} {\omega_k }\cos \left( \psi_k + \frac{\Delta \psi_k}{2} \right ) \sin \frac{\Delta \psi_k}{2} \end{eqnarray}$

同様にして、垂直方向も求めると以下のようになります。

$\begin{eqnarray} \Delta y = \frac{2V_k} {\omega_k }\sin \left( \psi_k + \frac{\Delta \psi_k}{2} \right ) \sin \frac{\Delta \psi_k}{2} \end{eqnarray}$