導入
move_base (ナビゲーション)
1. ROSで遊んでみる
2. ソフトウェア構成をみる
amcl (自己位置推定)
gmapping (地図生成)
1. ROSで遊んでみる
2. ソフトウェア構成をみる
3. 原理をみる(応用編) ← いまココ
4. 原理をみる(準備編その1)
5. 原理をみる(準備編その2)

はじめに

Navigation Stack を理解する - 4.2 gmapping: ソフトウェア構成をみるの続きです．頑張っているgmappingの中で何が起きているのかを見ていきます．

主に参考とした文献は下記の2です．

S Thrun, et al., "Probabilistic Robotics", the MIT Press, 2005.
- 特徴ベースFast SLAM の基本理論，Rao-Blackwell 化，有占格子地図等
Grisetti, Giorgio, Cyrill Stachniss, and Wolfram Burgard. "Improved techniques for grid mapping with rao-blackwellized particle filters." IEEE transactions on Robotics 23.1 (2007): 34-46.
- 格子ベースFast SLAM の元論文(gmappingはこれを実装)

本シリーズでは，数式ガチャガチャのイメージが強いSLAM 問題に必要な技術について，できるだけ数式を排除してイラストを用いて直観的に理解できるように記述することを心がけています．しかし，SLAM 問題に入り込もうと思うと，数式を完全に排除して説明しようとすると雲をつかむような議論となってしまいます．

そこで，本編においては，Fast SLAM で扱う数式と向かい合いながら，その数式の背後にある原理や，それによって実世界での動作に与える影響を考察していくこととします．いわば，Fast SLAM の数式に対するリバースエンジニアリングです．従って，数式の証明自体は追ったりはしません．解析的な証明は，それこそ様々な文献に記述されているので，そちらに譲ることとします．

ROS Wiki の図中
導入部で示したパッケージの図中

本編の構成

応用編
1. Fast SLAM 1.0
  1. イラストによる挙動の理解
  2. 数式との対応
2. Fast SLAM 2.0
  1. FastSLAM 1.0 との比較（イラスト版）
  2. FastSLAM 1.0 との比較（数式版）
  3. 特徴ベースから格子ベースへの展開
準備編その1
1. Fast SLAM の基本フロー
2. Rao-Blackwell 化
準備編その2
1. オンラインSLAMと完全SLAM
2. 有占格子地図

本編においても，3.3 amcl: 原理をみる (準備編)と3.4 amcl: 原理をみる (応用編)と同様に，準備編と応用編に分けて説明をしていきます．ただし，最初に応用編，その後に準備編です．

え？先に応用？となりますよね．実は，準備編を書いていたら随分と紛糾をしてしまい，本当に面白いSLAMの挙動にたどり着くまでに疲れてしまうことは必至だと思ったのです．

だから，まずは応用上どのような挙動になるのかをイラストで先に確認しちゃったほうが，頭に入りやすいと思ったのです．そのイラストの背後にある数式は本稿後半に記してあります．更に，FastSLAM アルゴリズム全体を理論付けるより詳細な背景を知りたくなった人だけが，次回以降の準備編を見てみればよいと思ったのです．この辺は，退屈な感じは否めないので^^;

では，本題に移ります．

概要

扱うSLAMは，ROSの"gmapping"で実装されているFastSLAM です．なお，内部的には"gmapping", OpenSLAMをラップしているものとなっています．

FastSLAM のざっくりとしたフローを確認しておきます．注意点も含めた詳細は次回述べることとして，ここでは早くイラストにいってしまいましょう．青い吹き出してコメントした部分は，FastSLAM 2.0でのみ扱う点です．1.0 と 2.0 を順番に説明する過程で，詳細を説明します．

「注意点も含めた詳細」については，「4.3. 準備編その1 Fast SLAM の基本フロー」で述べる予定です．

Fast SLAM 1.0

それでは、FastSLAM 1.0 の挙動をイラストで解釈していきましょう。

一応前提の確認です．時刻tでロボットが移動しています．今使える地図は，時刻t-1までに作成された中途半端なものです．この状態で，ロボットの位置と地図を更新したいのです．

イラストによる挙動の理解

ステップ1: 事前推定

動作モデルに従って粒子をばら撒きます．「オンラインSLAM問題」がキーワードとなります．FastSLAM 1.0では観測による更新は行いません．このまま次のステップに進みます．

「オンラインSLAM問題」については，「4.4. 準備編その2 オンラインSLAMと完全SLAM」で述べる予定です．

ステップ2: 観測更新1

観測と作成途中の地図を元に尤度を計算し，各粒子の重みを更新します．ここでも「オンラインSLAM」がキーワードとなります．FastSLAM1.0では事前分布の情報は使いません．

「オンラインSLAM問題」については，「4.4. 準備編その2 オンラインSLAMと完全SLAM」で述べる予定です．

ステップ3: 地図更新

リサンプリングの前に地図を更新します．「有占格子地図」がキーワードとなります．このとき，粒子毎に異なる地図を持つことになります．状態が異なるN個粒子があれば，N種類の地図が出来上がることになります．この点については，「Rao-Blackwel 化」がキーワードとなります．

「有占格子地図」については，「4.4. 準備編その2 有占格子地図」で述べる予定です．

ステップ4: 観測更新2

地図を更新した後に，現時刻の推定値を確定します．このとき最大重み推定を行うことは，「Rao-Blackwell化」と関係することになります．

「注意点も含めた詳細」については，「4.3. 準備編その1 Rao-Blackwell化」で述べる予定です．

その後リサンプリングを行います．ここが「完全SLAM問題」と関連すると，解釈しました．※この点は当方の考察です．

「完全SLAM問題」については，「4.4. 準備編その2 オンラインSLAMと完全SLAM」で述べる予定です．

さて，リサンプリングによって重みの大きい粒子だけを生き残らせるのですが，上図では生き残れそうな粒子が少ないのが分かると思います．これは，粒子は観測とは関係ない方向にばらまかれること，重み関数の分散が小さく生き残らせられる粒子が少ないという点が原因と考えられます．これでは，粒子の多様性や動作モデルの分布に関する情報が失われ，パーティクルフィルタの特性を有効活用できなくなってしまいます．

じゃあ，「粒子をもっと観測に近い方にばらまいて」，かつ「重み関数の分散を大きくして粒子をすくう網を広げれば良い」ではないか，という発想が生まれます．この発想が，FastSLAM 2.0 における式展開へと繋がるのです．

数式との対応

ここで，数式との対応を見てみましょう．ただし，ここでは特徴ベースでの式展開を扱います．

いいわけ

イラストについて散々格子ベースを扱ってきたくせに，ここで特徴ベースを出すだなんて言うちゃぶ台返しをするのは，ひとえに私の力不足故です．Probabilistic Robotics のFastSLAM 1.0と2.0は特徴ベースなので，これに全面的に依存することにしたのです．

今からやりたいのは，これらの特長を比較することなのですが，格子ベースだとFastSLAM 2.0しか見つからなかったので，これを直接1.0 → 2.0に展開させるのは控えたいのです（これを自信を持ってできない点が，私の力不足に他なりません）．文献ベースで証明されている情報を基にリバースエンジニアリングをしたいのです．

そこで，まずは数式については特徴ベースでFastSLAM 1.0 → 2.0へ展開させます．その後，FastSLAM 2.0 において特徴ベース → 格子ベースへと展開させていくことにします．

数式

それでは，まずざっと特徴ベースFastSLAM 1.0の式を確認します．変数の説明，解析的証明等はProbabilistic Roboticsに預け，結論だけを抜粋します．ここではざっと数式と挙動の対応関係を掴みたいのです．また，FastSLAM 1.0 → 2.0への展開を理解する上で，大事なのは「提案分布」と「重み」ですので，これらの式だけを抜粋します．

ごちゃっとしてしまいましたが，ここで把握しておくべきことを下記のとおりです．

提案分布
- 中心も分散も「動作モデル」に従う
重み
- 隠れていた「地図」に関する変数を表舞台に出している
- 中心は「観測モデル」に従う
- 分散は「観測」の不確かさ＋「地図」の不確かさの重ね合わせである

提案分布の方はこれまでの自己位置推定と同様ですが，重みがちょっと変わります．重みの式では周辺化されていたはずの隠れていた変数「地図」をワザワザ取り扱うように式を変形しています．こうすると，ランドマークの位置推定の誤差を重みの計算式で陽に扱うことができるのです．

ここで注目すべきが，隠れ変数「地図」を出すと，重みの「中心」には影響を与えず，「分散」にだけ影響をあたえると言う点です．「分散」が大きくなる点は，どこの馬の骨とも知らない変数を突然出てきたのだから不確かさが増すのは当然だろう．ただし，その隠れ変数で積分したら元の重みの数式に戻るような分布を形成しているはずなのだから，「中心」まで変えるのはおかしい．直観的にはそんな感じかな，と解釈しています．

この感覚は，FastSLAM 1.0 → 2.0 への展開を理解する上で非常に重要なので，ここで述べておきました．

FastSLAM 2.0

それでは，ここからはFastSLAM 2.0 の話に進みます．FastSLAM 1.0と比較して，何がどう良くなっているのかに着目しながら説明をします．まずはぱっと見て理解できるようにイラストを，次にそのイラストの挙動の背景に潜む原理を見るために数式を比較する，という順で構成します．