前回はAMCL（適応モンテカルロ位置推定）によって自己位置推定を行いながらロボットが移動する様子を眺めた。
ktd-prototype.hatenablog.com
前回時点では、地図は既知のものとして予め与えられていたが、今回はその地図を予め保持せず、移動と並行して地図作成も行うSLAM（Simultanious Localization And Mapping）について取り扱う。
多分今まで以上に雰囲気でしか理解できないと思う。
この記事は自分用のメモです。現時点では間違っている可能性も大いにあり、自分で使ってみたり書籍を当たったりして知識を修正していくつもりです。間違っても参考になどしないようにお願いします。
SLAMとは
SLAMの概要
繰り返しになるが、Simultanious Localization And Mappingの頭文字を取ってSLAMで、和訳すれば「自己位置推定と地図作成の同時実施」ということになる。
今までは予め地図が与えられた上で、LRFによるスキャンデータとのマッチング（この部分をAMCLが担う）によって自己位置を推定していた。
今回は地図自体もLRFによるスキャンデータを用いて作成していくことになる。
人間も、地図を与えられなくても、大体の距離感や地点々々からの景色を見ながら、自分で建物内の見取り図を作ることができる。野外でも、景色の流れやどこで曲がったか、どれくらい歩いたかを推測しながら大体同じことができそうだ。
それをロボットにやらせるという話のようだ。ロボットの研究と言ったら大体SLAMをやっている・・・と言えば過言だろうが、でもかなりメジャーで、でも今でも改良のネタには事欠かない部分なのだろうと予想している。
なんとなくのSLAMの仕組み
LRFを始めとするSLAMに用いられる測域センサは、少なくとも1フレームの間にロボットが覆域外に出ない程度には広い、測定範囲を持っている。
そこで、あるフレームでのセンシング結果をそのまま「地図化」し、次のフレームでは、新たなセンシング結果を前フレームで地図化したデータとマッチングして自己位置推定を行う。
それによってある程度の正確さを持つ自己位置推定ができれば、再びその地点からのセンシング結果を前フレームで作った地図に統合して地図を広げる・・・ということの繰り返しを行っていく。
・・・というのが僕の理解だ。ここまでは概ね間違っていないだろう。
少しずつ少しずつ地図を繋げていくと、ある時点から前のエリアはセンサの覆域外となり、マッチング対象からは外れていく。人間でも同じで、ずっとこれまで通ってきた領域の全範囲を視野内に収めておくことはできない。
従って、例えば角を曲がる毎の角度誤差（方向転換はセンサ覆域を大幅に変えるため）とか、そういった誤差は少しずつ蓄積していくことになるだろう。屋内であれば「そもそも通路は直角にしか交わらないから」とかそういった「常識」を元に脳内補正をすると思うが、人間界の常識をロボットに教える方が余程大変な気もする。
それが使えないとしたら、例えばある領域を一周して戻ってきて同じ景色が見えた時に、手元で逐次作ってきた地図を、今いる地点がが元居た地点であるという揺るぎない事実から、作ってきた地図に載っている誤差を補正するのではないだろうか。
人間の場合は地図を描き直すため結構な手間になるが、電子的な地図なら多少は緩和されるような気もする。
この方式はループ閉じ込み（Loop Closure）という方式で、結構な割合のSLAMアルゴリズムに含まれているようだ。
実際に動かしてみる
例によって、参考しているwebサイトに沿ってシミュレータを動かしてみる（再掲）
qiita.com
シミュレータ稼働の様子
この手順も手慣れたものだが、物理シミュレータ環境であるGazebo、センサデータ等の可視化ツールであるRvizと、相互に連動するツールを立ち上げていく。
今回はgmappingという方式を使ってSLAMを行うようで、この方式を使ったデモのロボット制御スクリプトを走らせる。
若干サイズ感は異なるが、下に貼った前回記事の「地図は事前に保持している場合」との違いは一目瞭然で、今回の図では地図がごく一部（現時点で見えている範囲）しか表示されていない。
移動しながら地図を作っていくということなので、例によって2D Nav Goalによって目標位置を指定して移動させてみると、確かに地図ができてくる。
作成の様子を一応キャプチャしてみた。自分は既にこの目で見ているし、わざわざこのブログを参考にする人も居ないだろうから意味があるかはわからないが。
SLAM Gmapping試してみた
メッセージのやりとり
rqt_graphによってノードとそれらの間でやり取りされるトピックを可視化してみる。
前回記事で見たrqt_graphとそう大きくは変わらない。相変わらず自己位置推定に/ekf_localizationは使われていないし。
これはどういう時に使うものなんだろうな。まだブログには書いてないけど、これはこれで少し試した（そして、全く扱えなかった）もので思い入れが深いので、使われていてほしいものだ。
特筆すべきは、/move_baseノードに流れ込む情報（トピック）の部分だろう。前は単純にLRFデータである/scanと、既知の地図を格納した/mapが/move_baseに流れ込んでいた。
/mapに格納された地図情報は既知であり、何かから影響を受けたり新しい情報で更新する必要が無いため、別のノードから発行されるような形にはなっておらず/move_baseへの一方通行の矢印となっていた。
環境認識と自律移動（２）- とりあえずweb記事に沿ってシミュレータを動かしてみた（自己位置推定編） - 技術開発日記
今回も/move_baseノードを動かすのに/scanと/mapトピックが流れ込む部分は変わらないのだが、マップが未知のため、/mapトピックが別のノードで生成され、発行される形になっている。
そしてそれを発行しているノードが今回着目している/slam_gmappingであり、そのノードは/scanトピックと/clockトピックによって動いている。後者は時間管理の情報だろうから、重要ではあるだろうとは言え空間データではなく、事実上このノードは/scanだけで動いていると言って良いだろう。
代わりに/amclノードが消えている。/slam_gmappingノードが/mapトピックを発行しつつ、自身もその情報と受け取った/scanトピックの情報から自己位置推定をするということらしい。
gmappingの動作原理
おそらくここに書くことがあるほどの理解には到達しないと思う。後で詳解確率ロボティクスを写経したらちょっとは理解できるだろうか。
とりあえず僕が読んでいるこの記事、大人になったら理解できるようになるからと未来の自分に伝言を残す。
qiita.com
一応、参考にさせてもらったwebサイトの見出しを辿りつつ自分なりの理解を記録することに努めたい。
自己位置の候補点をばらまいて最も尤度の高い候補点を新しい自己位置の推定値とする部分はAMCLと似たようなやりかたらしい。この「点を撒く」からパーティクルフィルタと呼ぶのだと思っていたが、違うかも？
それともこのSLAMもまたパーティクルフィルタを使っているということなのだろうか。
ステップ０：前提知識（独自追加）
gmappingはROSパッケージの名称、アルゴリズムとしてはFast SLAMという名前なのか？と思ったがROS wikiを見ると、gmappingのROS wrapperがROSのgmappingパッケージであると書いてある。
wiki.ros.org
gmappingはGrid Mappingの略であるということらしいので、そうなるとかなり広い概念な気もするし、gmappingが大まかな手法の種類、実際の計算アルゴリズムがFast SLAMである、といったところかもしれない。
Fast SLAMには1.0と2.0があり、2.0では候補点の撒き方の効率が良くなっているとか。
ステップ１：事前推定
ある時刻フレームにおいて、それまでの情報から「既知の地図」と「その中での自己の位置」を保持し、次の時刻フレームでそれを更新することを考える。
それまでに作った地図が無い場合、つまり動き出しや地図をリセットした直後の場合は、LRFデータとの相対位置関係をそのまま使うのだと思われる。
まずは今の時刻フレームでの地図を作成する前に、ロボットの動作モデル（このように制御指令を送ったらこう動くはず、という予測体系のこと、要はオドメトリなど？）に従って、地図上に今の時刻フレームにおける自己位置の候補点（位置と姿勢）を粒子として撒く。
動作モデルがホイールオドメトリなのであれば、予めホイールオドメトリにどのような誤差がどの程度の確率で出るか明らかであれば、それに従って点の撒き方を決められそうだ。
ROSの/Odometryトピックにcovariance（共分散）というデータが格納されるのはそのためか？
ステップ２：観測更新①
事前推定において撒いた各候補点から既知の地図を観測した場合にどのように見えるかという予測と、現在のセンサの実観測値を照合し、各候補点の尤度を計算する。
現時点では、最高尤度の点だけを残して後は棄却、とかそういうわけでもないようだ。
ステップ３：地図更新
各候補点それぞれを自己位置・姿勢であると仮定して、その下で地図を更新する。これは今の位置・姿勢におけるLRFのデータを現有地図と統合すれば行けそうだ。
今の位置・姿勢は各候補点が情報として持っているし、1フレーム前の現有地図における位置・姿勢は1フレーム前でのSLAMによって推定されているから、統合自体はできそうだ。
ただ、上記によって候補点の数だけ地図を保有することになる。
ステップ４：観測更新②
最終的に、各候補点から「重みが最大」の粒子を選択肢、現在における自己位置推定結果として確定する。
この「重み」とやらは一体何なのだろうか。単なる尤度なのであれば、地図更新をする前に自己位置だけ推定できたような気もする。
それならば全候補点に対して地図を保持する必要はなくなり、尤度最高の候補点から観たセンサ情報を地図に統合すればよい。
この時、重みが小さかった粒子は推定結果として選ばれた粒子の周辺に集約される、この操作をリサンプリングという・・・と記事中には書かれている。が、この粒子は一体この後何に使うのかよくわからない。
更に次の時間フレームにおいて使用する自己位置は、現在時間フレームで確定してしまえば良いのであって、重みが小さかった点を（重み最大の点の周囲にリサンプリングするとは言え）残しておく意味はあまりない気がするが・・・
次の時間フレームで使用する自己位置としては複数の点を使うとすると、等比級数的に自己位置候補が増えていくことになるので、どこかで削減する必要があるけど。
まとめ
尻切れトンボ感はあるが、元々参考にしていた記事も大体ここまでであるため、いったんここまでとする。（あとは数式の話が続くが、全く理解できなかった）
結局のところ動作原理を解析的に、というか数式的に理解することは全く叶わなかったことになるが、大体何の情報があれば何のパッケージが動かせそうかというところまではわかった。
GAZEBOとRvizとROSノート群の連携などの実装上の困難さとか、普通に/scanを発行できるセンサは手に入るのかとか、/mapを既知として扱う場合はどうやってデータを用意したら良いのとか、まだ悩みは尽きないが、とりあえず第一歩は踏み出したと言えるだろう。
