SLAMに関する覚書

はじめに

何故こんなものを書いたのか?

 近年,自律移動ロボットにまつわる言葉として
SLAM」という技術をよく耳にするようになった.
しかし(主に付け焼刃で解説"もどき"を披露する一部マスコミによって)
その技術はまるで街中をロボットが自由に動き回れるようになる魔法のようなものとして扱われ,
その本質を正しく理解しようとしている人の障害となっているように思える.
特にこれは,学生としてこれからロボティクスの研究を志す学生にとって有害である.
(逆に,既に技術としてこれを習得なり取り扱っている学生・研究者には無意味な話である)

 そこで私はここにSLAMの"より正しい"理解を目的として覚書を残しておくことにする. とはいえ,私がSLAMをどう理解しているか?という視点になってることは留意していただきたい.

技術の理解に関する個人的方法論

 個人的な方法論として,新しい技術を理解するには,
まずその歴史(何故その技術が必要とされたのか?)と
思想(どのようなアプローチで考えるのか?)
から考えることにしている.

 一般にこの手の工学的な分野では数式を用いて解説をする人も多いが,
その方式は正確で定量的ではあるが直観的な理解には劣り,
本来技術が目指していた方向を見失う可能性もある.
(逆に言えば,技術の水平思考の取っ掛かりとして考えるときには有用な方法である.)
また,工学分野における数式とは思想や現象のモデル化であり,
最初にその技術を編み出した人はまず間違いなく
定性的なアルゴリズムを数式モデル化するプロセスを踏んでいるはずである.

そもそもSLAMとは?

自己位置推定と環境地図

 閑話休題,SLAMの話に戻ろう.

 SLAMとは"Simultaneous Localization and Mapping",
「自己位置と環境推定を同時に行う」 ということ,言い換えると
「自己位置推定と環境地図作成の間の矛盾を解決する」
ということである.

 人間が未知の環境(たとえば初めて訪れる町)で目的地にたどり着くには
“自分の位置と目的地を含む地図”が事前の知識として必要になる.
(目的地までどのように向かうか?
例えば
“大通り優先で迷いにくいルート”
“裏道使ってでもとにかく所要時間を短くするルート”
“地形を無視して進路上の全てをぶち壊しながら進む最短距離ルート”
等と言ったルート選択の問題は別に考える.)

 また,人間が未知の環境に放り込まれた時も,
手元に「自分の位置を含む(但しどこかはわからない)地図」があれば,
周囲の地形と比較することで自分の位置を推定することが可能である.

 このように,地図は自律行動するロボットにとって最も重要と呼べるものであり,
その「正確さ(正確さの定義は別に考える必要があるが)」
ロボットの最終的なパフォーマンスに大きく影響する.

環境地図の構築

 この「地図(環境地図)」であるが,これはどのようにして生成するか?
例えば,3DCADを使って精密に環境を設計し,その設計どおりに環境を構築したならば
その3DCADデータをそのまま地図として使えるだろう.
また,環境の全てにメジャーと分度器を当て,それぞれの長さと角度を図面に書き写せたなら,
それもまた有用な環境地図として取り扱うことができるだろう.

 しかし,現実には環境は設計どおりではない上に,
(自然環境などにはそもそも設計図そのものがないし,
建物のであっても後付で棚や扉を設置するのは良くあることである.)
全てを人力で計測して環境地図を描くのは現実的な方法ではない.

 そこで考えられるのは,ロボットに搭載したセンサを用いて環境地図を得る方法である.
現在のロボットでは,センサとしてLRF(レーザー測距計)やステレオカメラを用いる方法が一般的だ.

環境図構築の問題点

視野の問題

 しかしこれで万事解決と言うわけにはいかない.
これらのセンサは環境の全てを見渡せるわけではないので,
当然得られる環境地図もきわめて限定的である.
また,精度も場所によって(特にセンサからの距離に依存して)低下する.
(例えばステレオカメラによる位置計測は,原理的にカメラからの距離の二乗に反比例して精度が下がる.)
環境の全てをセンサの有効範囲に収めようとすれば,
航空写真や衛星写真のように距離をとる必要がある.
しかしこれは先に述べたようにセンサの精度を低下させる事がほとんどであり,
また屋内のようにそもそも環境全てを見渡せる場所が存在しないこともある.

局所地図貼り合わせ

 ではどうするか?一番簡単な方法は「小さい地図を貼り合わせる」という方法である.
やっていることは伊能忠敬が日本地図を作ったときと同様,
センサの有効範囲で取れる小さい地図(一般にこれを局所地図あるいはローカルマップと呼ぶ)を
複数取得した上で,それぞれの地図をセンサを取得した位置ごとに貼り合わせ,
環境地図を作成する手法である.

 さあ今度こそ万事解決・・・ではない.
なぜなら,複数の地図を貼り合わせる際には
「その地図はどの位置(≒センサの位置)から見た地図なのか?」と言う情報が必要なためである.
位置がずれれば,当然貼り合わせた地図はゆがんだものとなり,
各々の局所地図が正確であったとしても,環境地図は正確にはならないのである.

 これはかなり大きな矛盾を持った問題である.
何故なら,センサの位置を正確に知ることができているのなら,
それは環境地図作成の目的である「自己位置の推定が達成されている」
と言うことと全く同義
であり,そもそも環境地図が必要ないのである.

SLAMとは

 正確な自己位置のためには正確な環境地図が必要
正確な環境地図作成のためには,正確な自己位置が必要・・・
この矛盾に対処するための技術が“SLAM”なのである.

 したがって「自律移動ロボットにSLAM技術を搭載!」というのは少々正確ではない.
自動運転車などの自律移動ロボットは,
事前に正確な環境地図が与えられていることが大半であり,
正確な環境地図があれば後はそれを元に自己位置を推定し,
目的地を設定し,ルートを決定し走行するだけである.
そこに上記の矛盾を解決する技術など必要ない.

SLAMの実装

理解の出発点:ICPアルゴリズム

 さて,SLAM技術が何のために必要とされているのか?と言う点については上記の通りである.
では,具体的にどのようなアルゴリズムによってこれを実現しているのだろうか?

 最も直観的な考えでこの問題について考えるとするならば,その答えは
「局所地図同士をオーバーラップさせ,そのすりあわせによって地図と位置を更新する」
方法であろう.

 一例として2つの局所地図(A,B)を貼り合わせる事を考える.
2つの局所地図はある程度(6割~7割)が同じ地形を捉えていると考えた場合,
その部分は局所地図ABの間で一致しているはずである.

 そこで,片方の局所地図Aを基準として,
もう片方の地図Bをいろいろと捻ったりずらしたりして
もっともぴったり合うような合わせ方を模索する.

 このとき局所地図同士の「もっともピッタリな合わせ方」を見つけ出したら
両者の地図の和を取ることで局所地図Aを拡張することができる.
(これは「未完成の環境地図」であると考えることができる)
この際,局所地図Bの中心(つまりセンサの位置)が,
未完成の局所地図の何処にあるか?を考えれば,
そこが新しい自己位置であると考えることができる.

 さらにロボットが移動し局所地図Cを取得したならば,
(当然,局所地図CはBとある程度同じ地形を捉えている)
「未完成の環境地図」と局所地図Cの間で「もっともピッタリな合わせ方」を模索し
未完成の環境地図を拡張することができる.

 このように,地図同士を繰り返し重ね合わせて,最も収まりの良いあわせ方を
模索するアルゴリズムを,ICP(Iterrative Closest Point)という.

 これはSLAM実装における最も直観的な考え方だろう.

ICPの欠点

 ICPアルゴリズムは最も直観的にわかりやすいアルゴリズムだが,
当然コレだけで話が終われば苦労はない.ICPの問題点を挙げていこう

時間がたつにつれ計算時間が指数関数的に増大する

 ICPの基礎は,局所地図と未完成の環境地図を重ね合わせて,
最も収まりの良い点を探すことである.当然,未完成の環境地図は
局所地図を重ね合わせるたびに広がっていく.
仮に局所地図を未完成の環境地図の全領域に重ね合わせて調べようとすれば,
未完成の環境地図が広がれば広がるほど計算時間が発散してしまう.
また,格子状の地形のように,別の場所に同じような地形が存在している場合
局所地図のあわせ込みが間違った場所になってしまうことにもなりかねない.

 これに関しては,マップの重ねあわせを
「前回局所地図を環境地図に重ね合わせた際に推定した位置」
周辺に限定することである程度の解決が可能である.

精度が下がる

 局所地図を環境地図に統合する際,ほとんどの地形は既に環境地図に
書き込まれている場所なため書き足す必要はない.
またそれら「書き足す必要のない」領域は局所地図の中心付近である.
したがって新しく「書き足される」領域は地図の端っことなるが,
先に述べたように,センサから遠い領域は一般的に精度が悪い.
つまり,環境地図に書き足される領域は常に精度の悪い領域であり,
その領域を環境地図として組み込んだ地図を元に,次の局所地図を
組み込もうとすれば,その位置の合わせこみの精度はどうしても下がってしまう.

ループ閉じこみ問題

 ICPのアルゴリズムは,一般に環境地図と局所地図は常に正確であることを前提としている.
定かではないのはセンサの位置≒自己位置であり,環境地図と局所地図を重ね合わせることで
自己位置を推定しようと試みる.

 しかし,先に挙げた理由により,(未完成の)環境地図は広がれば広がるほど誤差を多く含む.
当然,誤差を含む環境地図を元に局所地図を取り込んで拡張した環境地図はより多くの誤差を含む
したがって,地図は徐々にゆがんでいくこととなる.

 この問題が一番良くわかるのがループ閉じこみ問題である. 下図では,建物の周りを一周してその環境地図(とロボットの軌跡)を推定している. 当然,一周しているのだからスタートとゴールは同じ点になり,軌跡は長方形を描くはずだが, 局所地図を重ね合わせる際に徐々にずれていった結果,その軌跡はループを描くことなく
そのスタートとゴールは「違う地点」と認識されてしまっている.

次あたりで「確率ロボティクス」の概念を組み込みながら
SLAMの定性的な理解と基礎的なモデリングを考えてみる.

それでは,また