はじめに
マイクロマウスを始めた2000年頃は、まだオドメトリを計算して自己位置を割り出すと言う事をほとんど見つけられなかったと思います。(書いてらっしゃった方がおられたらすいません。)
当時、僕はオドメトリによる自己位置推定をして、ライントレースロボットのように仮想の走行ラインを追随して 高速走行する技術の獲得を夢見たものです。
そのために作ったマイクロマウスが「3式改」というマウスなのですが、このロボットは計測のためのロータリーエンコーダーに取り付けた計測輪 がつけられていてほぼスリップすることなく走行軌跡を取得することができました。
今回は、マイクロマウスのような、2輪ロボットのオドメトリについて話したいと思います。
- はじめに
- 自己位置推定
- カウント値は離散的にしか得られない
- 移動ロボットの運動学(キネマティクス)
- ホイールエンコーダから速度を知る
- オドメトリ円弧近似
- オドメトリ直線近似
- オドメトリと誤差
- おわりに
自己位置推定
ロボットにとって自分がどこにいるのかを知ることは大変重要です。移動ロボットならば目的地に着くためには 今自分がいる位置を知らなければ、次にどこに行くかを決めることもできないですし 地図を作ることもできません。 ロボットがどこにいるのかをセンサから得られる情報から推定することを自己位置推定と言います。
スターレコニング
大航海時代の昔、船が航海するための指標になるものに星がありました。 北極星などの目標になる星を六分儀を用いて角度をはかり、自己位置推定を行っていました。 星やランドマークを使った自己位置推定をスターレコニングと呼びます。
六分儀とその使い方を知る素晴らしいつぶやきがあったので、共有させていただきました。
六分儀にはハーフミラーが組み込まれていて、こうやって水平線を基準にして太陽の位置を合わせると簡単に角度を知ることができる。 pic.twitter.com/S0AmkY5rS4
— MASA🌈🕒 (@masa_0083) January 5, 2019
六分儀で緯度はわかりますが、経度を知るには狂いのない時計が必要でそのような時計を作ることが大変難しく、安全な航海にはクロノメータの発明を待たなければなりませんでした。 そのあたりの話は「経度への挑戦」と言う本に面白くまとめられています。
デッドレコニング
スターレコニングは外界を検知するセンサすなわち外界センサを用いて自分の位置を割り出します。 それに対して、ロボットの速度や角速度、走行距離、方向の変位を検知する内界センサによる 自己位置推定をデッドレコニングといいます。
速度が判っていれば、走行時間と合わせて考えることで走行距離が判ります。角速度がわかると 同様に角度を求めることができます。
走行距離と角度が判れば、そこから自分が今どこにいるのかを割り出すことができます。
マイクロマウスのような2輪移動ロボットは、タイヤ回転を取得できるインクリメンタル ロータリーエンコーダーを持つことで、移動量と回転量を知ることができます。
ロータリーエンコーダーを用いて移動した距離を測って自己位置を知ることを オドメトリと呼びます。
ちなみに距離計の事をオドメーターと呼びます。このオドという言葉は 古代ギリシア語の「経路」をあらわす言葉だという事です。
ちなみに、ステッピングモータで走行するマイクロマウスは一般にはエンコーダを持ちませんが 脱調等が起こらない限り、指令ステップ数からモータの回転角が判るので、エンコーダ が付いているのと同じように考えることができます。
今回の記事では、エンコーダやステッピングモータステップ指令から得られるタイヤの回転角(走行距離) から如何にして自己位置の推定の計算をするのかを以下でひも解いていきたいと思っています。
カウント値は離散的にしか得られない
エンコーダ出力は通常マイコンに内蔵されているカウンタ回路に接続されます。 カウンタ回路はその名の通り、エンコーダパルスを数えてくれます。マイコンは今何カウントになったか 定期的に問い合わせをします。通常はカウンタの桁数が有限ですので、前の値との差分をとって メモリに積算していきます。
重要なことは、カウンタ値は一定の時間間隔でしか得られないことです。 間隔の間にどのように変化したかはわからないためその間の走行経路はわかりません。 したがって、何らかの仮定をおいて、その間の走り方を想定しなければ 自己位置を推定することはできません、ただし闇雲にそれを決めることも できないので、それを考えるためにロボットの運動学を考えていこうと思います。
移動ロボットの運動学(キネマティクス)
ロボットの運動学で検索したらたくさん優れた記事があると思うので 僕が改めて書いたらかえって混乱してしまうかもしれません。 と言っても書かないと、記事の流れが判らなくなるので 頑張って書きます。
加速度・速度・位置
ロボットの加速度をとすると ロボットの速度は加速度の積分としてあらわすことができます。
速度の方向をとして、速度を水平成分と、垂直成分に分解します。
水平方向の速度成分は
垂直方向の速度成分は
位置情報は速度の積分ですので
水平方向の位置は
垂直方向の位置は
さて、ロボットの速度の方向はロボットの角速度の積分になりますから。
加速度の積分が速度、速度の積分が位置という高校物理で習う程度のお話とまったく同じものです。
2輪移動ロボットの速度と角速度
以上はロボットの移動形態は問わず、 どんな場合でも当てはまることに関して書きました。 さて、僕が今回取り上げたいのは2輪移動ロボットです。 2輪と言ってもバイクや自転車形状ではなく、リヤカーやセグウエイのような タイヤが向かい合った形のロボットです。
以下はタイヤが滑らない前提でお話を進めます。
右のタイヤの周速度を、左のタイヤの周速度をとしたとき ロボットの速度は次のようにそれぞれの速度の平均となります。
速度は判りましたが、角速度はどうなるでしょうか?
タイヤの間の距離をとします。また、ロボットの回転は反時計回りが正方向とします。 すると、ロボットの角速度は以下のようになります。
以上がロボットの運動学です。
積分できるか
自己位置を計算するためにはやが判ったうえで積分をする必要があります。 はたして、ロボットが得ることができる情報からそれは可能でしょうか?
ホイールエンコーダから速度を知る
運動学においては自己位置を知るためには速度を積分しなければならないことが判りましたが ホイールエンコーダから速度を求めることを考えてみます。
エンコーダの回転とタイヤの回転が1:1であるとします。 エンコーダ1周のパルス数をとします。タイヤの半径をとすると1パルスのあらわす距離は次のようになります。
したがって、エンコーダから得られるパルスがとしたとき、その時の走行距離は以下となります。
あくまでエンコーダから得られる情報は、タイヤの転がり距離情報であって、速度ではありません。 ここから速度を得るためには、速度は位置の微分ですが微分は実現不可能なため差分にによる近似を用いなければなりません。
差分とは引き算の事ですから、エンコーダから時間を空けて二つの値を取得する必要があります。 エンコーダからの時間を空けて2回取得した一つ古いパルスと最新のパルス これらの差分からその間の転がり距離情報が得られます。
速度は距離(位置)の微分なので微分の定義に立ち返って考えると次のように表現できます。
先ほども微分ができないと書いていましたが、これはロボットに装備されるコンピュータは 四則演算しかできないためです。極限の計算は理解できないので、位置情報を一定間隔で得られる ロボット(コンピュータ)は速度を得るためにはどうにかして上の微分を四則演算で近似しなければ なりません。
そこで、極限ではを限りなく0に近づけるわけなので、上の式から極限の記号を取り去った式の中で、できるだけを小さくしてやれば微分値に近くなるだろうと考えます。
したがいまして、以下の式が速度の近似値となります。
速度計算の実際
実際にコンピュータの中でロボットの速度を割り出すことを考えます。
様々なセンサーからデータを取得できるロボットは一定間隔でデータを取得しています。 データは取得した順に並べることができるとすると、1番目のデータ、2番目のデータ、3番目のデータ、・・・、k番目のデータ、k+1番目のデータ、・・・・・・ とロボットが動き続ける間ずーっと何番目のデータというのが並んでいるのが確認できると思います。 実際には並べてみることは普通出来ませんが、イメージ的にはそんな感じです。
いま、同じ時間間隔秒ごとにデータを取得するとします。時刻の時にエンコーダから得れれる距離情報をとして、これを番目のデータであるとします。 そして秒経過して得られた、その次のデータを番目のデータという事にします。
そこでデータを次のように添え字付きで書きなおして、これまでの表現と対応させて書いてみると
速度についても同じように表現してみようと思いますが、一貫性を持たせるためにはやはり時刻の時の速度がデータとしては番目と考えるのがよさそうです。
これらで速度の近似式を書き換えてみましょう。
速度については、番目の距離のデータを得られた時点では番目の速度のデータしか推定できないことになります。
これはロボットの角速度も同様で両輪のエンコーダパルスから計算したロボットの角度データにおいて番目を、番目をとすると 以下のように表すことができます。
オドメトリ円弧近似
積分計算により自己位置を計算するためには離散的にしか得られないデータの間の走行状態を仮定するしかありません。 通常、ロボットでは一定時間間隔で制御を行います。その時間間隔はデータ取得の間隔(周期)と一致することが多いようです。
仮に、制御周期ごとにデータを取得しそれによって自己位置の推定をするとします。 制御周期の間は制御指令は変えられませんので、ロボットは同じ速度指令、角速度指令で移動します。 これは制御周期の間はロボットが同じ速度、角速度で動くことを意味するものでは無いのですが 近似的にそれを認めようというのが表題の円弧近似です。
ロボットが同じ速度、同じ角速度で動いている間は円弧軌道を描きます。
以下の式は運動学の水平方向の位置及び垂直方向の位置の計算式の再掲載です。
水平方向の位置は
垂直方向の位置は
速度、角速度ともに一定としたとき、上式においてロボットの角度が角速度に比例して変化します。それを踏まえて上式を書き換えます。 ここで積分範囲はからからとし積分結果はその間の増加量となります。
まずは水平方向からやってみます。
エンコーダに問い合わせをして得られる情報は制御周期間の変化量が得られることが多いので からの間に変化したロボットの角度をとし、上式に積和の公式を適用すると。
同様にして、垂直方向も求めると以下のようになります。
ただし角速度が0となるときは上記の式は発散してしまいます。その際は直線状に走行することになるので以下の式で計算します。
この計算を、制御周期ごとに行い毎回積算することで現在の自己位置を推定することができます。
オドメトリ直線近似
もう一つの近似の方法として直線近似があります。
こちらも運動学に戻って考えてみます。
水平方向の速度成分は
垂直方向の速度成分は
これから積分することで位置を割り出すのですが、ここで上式の速度は位置の微分であることから、位置の微分の近似式で書き直してみます。
まずは、水平方向からです。
これより一の変化量をとして、記号の使い方を円弧近似と揃えると
垂直方向もどうようにすると
これは区分求積法で速度のデータから距離を求めたことと同じです。 オイラー法で微分方程式を解いていることとも同じこととなります。
式をよく見ると決まった角度で直進した際の水平方向、垂直方向の移動量を計算しているのと同様になりますので 曲線の微小部位を直線近似し走行軌跡を折れ線で近似していることとなります。
オドメトリと誤差
オドメトリは積分を何らかの方法で近似して計算していますので、常に誤差が含まれており、積算するたびに誤差が増大します。 また、タイヤがスリップすると走行量がずれてしまうのでそれも誤差の要因になります。
したがってオドメトリだけでは時間とともに推定は大きく外れるので、違う方法で自己位置を修正する必要があります。
おわりに
オドメトリは 車輪で移動するロボットには重要ですよね。
計測輪でオドメトリをするのはかなりきれいに走行軌跡が計算でき誤差も少ない方法です。 マイクロマウスだけではなく屋外で走行するロボットにも計測輪をつけて数キロの自律走行にも 応用してよい結果を得られたことがあります。
さて、今回はこれで終わります。 書きなぐった感が強い記述になりました。後日図などを整備して充実していきたいと思います。
では、また!