FastSLAMが何をやっているかをざっくり説明。

1. 確率ロボティクスと移動ロボットの行動生成
第12回
上田隆一

2. 本日の内容
• FastSLAM
– オンラインSLAMの一手法
Oct. 7, 2015 確率ロボティクスと移動ロボットの行動生成 2

3. 前回
• 占有格子地図を使った、移動量の誤差を
考慮しない地図生成手法
• 移動量の誤差を考慮するには？
– オンラインSLAMではRao-Blackwellizationという
方法が用いられる
Oct. 7, 2015 確率ロボティクスと移動ロボットの行動生成 3

4. Rao-Blackwellization
• 自己位置推定と地図の推定の問題を
数式上で切り分ける
– ある推定姿勢を真として地図の不確かさを表現
• 地図中の要素（壁やランドマーク）の位置情報は
互いに独立
– その上で姿勢推定
Oct. 7, 2015 確率ロボティクスと移動ロボットの行動生成 4

5. 具体的な手法
• パーティクルフィルタによる自己位置推定を
思い出してみましょう
– 各パーティクルに着目すると、姿勢は真の姿勢扱い
→各パーティクルに推定中の地図を持たせる
→FastSLAM
Oct. 7, 2015 確率ロボティクスと移動ロボットの行動生成 5

6. FastSLAM [Montemerlo2003]
• パーティクルフィルタで地図を作成
• ランドマークベースのものと
占有格子地図ベースのもの
– もともと特徴（点ランドマーク）ベース
→その後占有格子地図ベース
– 占有格子地図ベースのものの方が簡単
• FastSLAM1.0とFastSLAM2.0が存在
– 2.0の改善点: 役に立たないパーティクルの発生を抑制
Oct. 7, 2015 確率ロボティクスと移動ロボットの行動生成 6

7. 占有格子地図ベースのFastSLAM
• 各パーティクルの変数
– 重み
– 姿勢（最初の座標を原点として）
– 占有格子地図
– 数式によるパーティクルの表現
• ある時刻tのi番目のパーティクル
Oct. 7, 2015 確率ロボティクスと移動ロボットの行動生成 7

8. アルゴリズム
• 位置推定の部分と地図の更新の部分に綺麗に分離
• 時刻t-1のパーティクルの集合から時刻tの集合への
更新手続き
1. 各パーティクルの姿勢xをデッドレコニング通りに移動
• 誤差モデルも考慮
2. 地図とセンサ値を比較してベイズ則で重みwを更新
3. センサ値を使って地図を更新
Oct. 7, 2015 確率ロボティクスと移動ロボットの行動生成 8

9. 例
• http://ais.informatik.uni-
freiburg.de/teaching/ws12/mapping/pdf/slam13-
gridfastslam.pdf
– 10ページ目
– パーティクル数: 500
• Stachniss et al.: Exploration with Active Loop-Closing
for FastSLAM, IROS2004.
– ループが閉じる時に推定が進む
Oct. 7, 2015 確率ロボティクスと移動ロボットの行動生成 9

10. ランドマークベースのFastSLAM
（FastSLAM1.0）
• 拡張カルマンフィルタを用いる
– ランドマーク位置の推定値を平均と共分散で
Oct. 7, 2015 確率ロボティクスと移動ロボットの行動生成 10

11. パーティクルの変数
• 占有格子地図版のFastSLAMと同様、各パーティクルが
地図（=ランドマークの位置情報）を持つ
1. 重み
2. 各時刻の姿勢のシーケンス
• オンラインSLAMとしては最新の姿勢だけ必要
3. 各ランドマークの推定位置と推定位置の共分散
Oct. 7, 2015 確率ロボティクスと移動ロボットの行動生成 11

12. アルゴリズム
• ロボットがランドマークを観測
– そのランドマークを初めて観測した場合
• ランドマークの位置μと共分散Σを初期化
• 重みwを初期化
– 二回目以降の観測の場合
• 拡張カルマンフィルタでμとΣを更新
• 重みwを共分散とセンサ値の誤差から計算
• ロボットが移動
– 重みwにしたがってリサンプリング
Oct. 7, 2015 確率ロボティクスと移動ロボットの行動生成 12

13. より難しい問題
• ランドマークが識別できない場合
– 何かランドマーク情報を得たら、各パーティクルが持つ
地図上のランドマークで一番それっぽいものを選ぶ
• 「それっぽい」=「最尤」
• ランドマーク数が未知
– 各パーティクル内で閾値処理してランドマークを追加
（確率ロボティクス p.413）
Oct. 7, 2015 確率ロボティクスと移動ロボットの行動生成 13

14. 高速な演算
• 一度の観測で更新の対象となるランドマークは
一個、アルゴリズムを凝っても数個
• 新しいパーティクルを作る時に更新の対象と
ならなかったランドマークのデータをコピーせず
ポインタで指しておく
（確率ロボティクス p.419）
Oct. 7, 2015 確率ロボティクスと移動ロボットの行動生成 14

15. FastSLAM2.0
• リサンプリングの際にセンサ情報も反映
– FastSLAM1.0のように、ただデッドレコニングで
パーティクルを散らすと無駄なパーティクルが
増えるので
– あくまでパーティクルの数を節約する方法
– 計算は複雑に
• デッドレコニングの誤差がセンサの雑音より
大きい場合に有効
Oct. 7, 2015 確率ロボティクスと移動ロボットの行動生成 15