AbudoriLab.

自律ロボットで誰でも遊べるよう試行錯誤するブログです。

SLAMを理解する - 累積誤差の最小化 -

AbudoriLab.です。

ロボットの自律移動を実現するために必要不可欠な技術となるSLAM(Simultainius Localization and Mapping)の概念を難しい数式などは使用せずにまとめていきます。

3回目となる本稿はSLAMの高精度化に必要な計算「局所最適化と全体最適化」についてご紹介します。

過去にまとめたSLAMの記事から読み始めてもらえると、今回の話をより理解していただけると思います。

SLAMの「自己位置の推定」と「地図の作成」について簡単な例を用いて紹介しています。

www.abudorilab.com

自己位置と地図作成の相互依存関係がSLAMの精度向上にはとても大事で、その利点を効果的に活用するには同時実行が有効だということについて紹介しています。

www.abudorilab.com

自己位置のずれを修正するために地図を用いることで補正できることを先の記事で紹介しましたが、それでも自己位置がずれてしまいます。

理由はとてもシンプルで、
センサの計測値にノイズが乗ってしまい、頼り所にしていた特徴点の位置が正確でないからです。

計測誤差があった場合でも、SLAMの精度を向上させる方法を検討しなければなりません。

はじめに

タイヤの回転数などから移動量を推定するオドメトリは、走行するにつれて自己位置に誤差が累積するため自立移動が実現できなくなるという問題があります。

自己位置のずれ・・・位置が不正確なため自律移動で目的地へ到達できない

地図のゆがみ・・・物の位置が現実と異なり、計画した移動経路が無効になる(地図が信頼できないため、あちこち探索しながら目的地を目指すことになります)

よって、累積誤差は極力なくす必要があります。

そこで局所最適化と全体最適化の登場です。

局所最適化

局所最適化は地図に登録した特徴点を参照して、一致させることで自己位置を修正する方法です。

表現こそ異なりますが、2回目のSLAM紹介記事でも局所最適化について触れているのでおさらいします。

wheel encoderのような内界センサから計測した移動量で自己位置を推定した場合、図1のように精度が低下します。

 

f:id:nekoze-nya:20210508172157p:plain

図1:オドメトリから推定した自己位置

各時刻t-1,t,t+1で計測した特徴点を識別できるようp^{t-1},p^{t},p^{t+1}として示します。

自己位置が正確に推定できていれば特徴点は一致します。

したがって、図2のように特徴点が一致するように自己位置を修正することで推定精度を向上させます。

 

f:id:nekoze-nya:20210508172638p:plain

図2:特徴点を一致させて自己位置を修正

上述の処理を毎時刻に実施することで内界センサから計測した移動量の精度低下を抑制できます。

しかし、局所最適化だけでは自己位置の修正は不十分で、誤差は累積し続けてSLAMが破綻します。

その理由は、計測誤差がある特徴点に一致させようとして、修正したはずの自己位置がずれてしまうからです。

自己位置は正確に推定できているが、計測誤差によって特徴点が一致しない例を図3に示します。

計測誤差が乗った時刻tの特徴点p_3,p_4を青枠のスターで示します。

 

f:id:nekoze-nya:20210530164630p:plain

図3: 計測誤差による特徴点のずれ

局所最適化でずれを無理やり修正した結果を図4に示します。*1

 

f:id:nekoze-nya:20210530164718p:plain

図3: 計測誤差による自己位置のずれ

自己位置の修正が失敗することがわかります。

シンプルな解決策としては高額で高精度な計測機器を使用する方法がありますが、できればコストを抑えてSLAMの高精度化を目指したいです。

そこで、全体最適化の出番になります。

全体最適

ロボットが一度訪れた場所を再訪したことをきっかけとして、図5のようにループ内の全地点の自己位置を修正します。

 

f:id:nekoze-nya:20210531204803p:plain

図5: 全体最適化による自己位置修正

最適化手法はこれまでにたくさん提案されており、1つ1つが専門知識が必要になりますが、今回はシンプルな例を紹介します。そのため、実際の手法とは多少異なります。

紹介する方法は3ステップになります。

手順-1:

ロボットが再訪した場合、既に計測した特徴点を再び計測することになります。

まずは、図6のように再訪を認識した地点の自己位置を特徴点が一致するように修正します。

 

f:id:nekoze-nya:20210606152304p:plain

図6: 再訪を認識した自己位置を修正

このままではループ内の最後の位置だけが修正されて、それ以前の位置は誤差が残っている状態です。

手順-2:

図7のように手順-1で修正した自己位置の移動量を、少しずつ重みを付けて他の自己位置も移動させます。

 

f:id:nekoze-nya:20210606152838p:plain

図7: 移動量の重み付け

重みの数値は、ループを遡るほど小さく設定します。
これは、ループが閉じない原因が蓄積誤差にあるために遡るほどその誤差は小さくなるという考えを元にしています。

これで全体で精度が向上しますが、本来の自己位置のずれは各箇所で大きさ,方向が異なるため、図8のように部分的に修正が必要になります。

 

f:id:nekoze-nya:20210606153735p:plain

図8: 線形的な修正だけでは不十分な理由

手順-3:

最後に、部分的に修正して最も整合がとれる位置を見つけていきます。

その整合性を特徴点の一致度で測ります。

図9のように手順-2の時点で一致度が80%だったとして、自己位置を少し動かすことで82%に上げることができます。

 

f:id:nekoze-nya:20210606154833p:plain

図9: 自己位置の微調整

自己位置の微調整によってより正確な位置に修正できることがわかります。

微調整を点数が最大になるように全体に実施したら最適化は終了です。

手順-3の注意点:

全体最適化はSLAMの精度向上にとても強力ですが、精度を高めることに重みを置く程に計算量が多くなります。

リアルタイム性を求めるなら妥協も必要です。

1. 自己位置を動かす範囲を限定する

特徴点の一致度を上げるために自己位置を動かしますが、範囲が無限になると正解が存在しない空間で正解を探し続ける可能性があります。

対策としては誤差からずれの範囲を想定してその中で移動させます。

範囲が限られる分、試行回数が減るため計算量を減らせることができます。

2. 一致度の点数を妥協する

まだ点数が高くなるのでは?と何度も何度も手順-3を繰り返すと計算量は大きくなります。

1,2%の改善が見込めると思われますが、リアルタイム性を重視するのであれば恩恵が小さすすぎます。

ある程度の妥協点を設定しておくことも重要です。

また、100%を目指すことはあまりおすすめできません。

計測誤差があることを前提で理想に近づけているので、図10のように一致させすぎて自己位置のずれが大きくなります。

 

f:id:nekoze-nya:20210606160732p:plain

図10: 過剰適合による最適化の失敗

これを過剰適合(オーバーフィッティング)と呼びます。

"完璧な地図を作成したい","リアルタイムに動く自律移動ロボットを製作したい"など目的に応じて、リアルタイム性と緻密性のどちらに重きを置くかを考えることが必要です。

おわりに

全体最適化の手順-3は専門的にはボーズグラフ最適化(pose graph optimization)と呼ばれます。

本や記事,論文等でこの言葉が出てきたら、紹介した内容がイメージされると嬉しいです。

3回分の記事でSLAMの主要なポイントを概念的にまとめることができました。

SLAMの実現には多くの専門知識が必要で、次々と新しい手法が提案されるのでついていくだけでも大変です。

今後は実装や論文などを記事にしていこうと考えています。

AbudoriLab.の記事によってSLAMを理解できる人口が増えるとうれしいです。

*1:完璧に一致しないため、実際ではp^{t-1}_3,p^{t}_3p^{t-1}_4,p^{t}_4のずれの距離合計が最小になるように修正する方法が取られたりします。