Apr 25, 2018 @中部大学

1. 確率ロボティクス入門
第3回
上田隆一

2. 本日の話
• SLAM（simultaneous localization and mapping）
– どんな問題を解くのか
• FastSLAM
– 解法の一つ
– 対応するサンプルコード・ノート
• https://github.com/ryuichiueda/probrobo2017/blob/master/12.ip
ynb
• その他話題
May 2, 2018 ロボットフロンティア第3回@中部大学 2

3. SLAM
May 2, 2018 ロボットフロンティア第3回@中部大学 3

4. 移動ロボットの地図生成
• 白地図を準備
• 自身の姿勢を原点に
• 以下の繰り返し
– センサで壁などの位置を
計測して白地図に書く
– 移動して白地図上の
自身の姿勢を更新
• 歪む！
May 2, 2018 ロボットフロンティア第3回@中部大学 4

5. なぜ地図を作るのか
• 場所と動きを結びつける
– ロボット自身の「空間に対する知識」
– 歪んでいても問題ない
– 人間も歪んだ地図をたくさん持っているようだ
• 人や他のロボットとコミュニケーションをとる
– 「ここからここまで行って」
– 歪んでいると困る
– 環境の形状だけでなく、言語化できるとなお良い
May 2, 2018 ロボットフロンティア第3回@中部大学 5

6. SLAM（simultaneous localization and mapping）
• 自己位置推定と地図生成を同時に行う問題・
手法を指す
• 右図
– 先週使ったランドマーク
– 自己位置推定
• ランドマークの位置は既知
– SLAM
• ランドマークの位置は未知
May 2, 2018 ロボットフロンティア第3回@中部大学 6

7. SLAM問題の種類
• オンライン/オフライン
– ロボットの動作中に地図を作るか、後で作るか
• ランドマークベース/格子地図ベース
• 2D/3D: 平面か立体か
• 移動モデルへの依存度
• ・・・
May 24, 2017 ロボットフロンティア第3回@中部大学 7

8. 問題の定式化（その1）
• 各時刻のロボットの姿勢: 𝒙0:𝑡 = {𝒙0, 𝒙1, … , 𝒙 𝑡 }
– ロボットの姿勢: 𝒙 = 𝑥, 𝑦, 𝜃
• 各時刻のロボットの行動: 𝒖1:𝑡 = {𝒖1, 𝒖2, … , 𝒖 𝑡 }
– 前回は𝑎で表していましたが今回は𝒖で
• 各時刻の観測: 𝒛0:𝑡 = {𝒛0, 𝒛1, … , 𝒛 𝑡 }
– 前回はランドマークの距離と向きでしたが、
今回は姿勢𝒙から導かれるランドマークの絶対位置とします
• 地図（未知）: 𝒎 = {𝒎1, 𝒎2, … , 𝒎 𝑀 }
– 𝒎𝑖 = (𝑥 𝒎 𝑖
, 𝑦 𝒎 𝑖
)
May 2, 2018 ロボットフロンティア第3回@中部大学 8

9. 問題の定式化（その2）
• 移動モデル： 𝑝 𝒙 𝑡|𝒙 𝑡−1, 𝒖 𝑡
• 観測モデル: 𝑝 𝒛|𝒙
• SLAM問題: 𝑝 𝒎, 𝒙1:𝑡|𝒙0, 𝒖1:𝑡, 𝒛1:𝑡 を計算
– 完全（Full） SLAM問題: 𝒎, 𝒙1:𝑡を（後から）解く
– 逐次SLAM問題: 𝒎, 𝒙 𝑡を更新し続ける
May 2, 2018 ロボットフロンティア第3回@中部大学 9

10. 解き方（主要なもの）
• 完全SLAM
– graph-based SLAM
• デッドレコニングで得た𝒙1:𝑡をつなげたグラフを作る
– ランドマークの位置計測から互いの矛盾の度合いが分かる
（矛盾がエッジに記録される。矛盾の積算が歪み）
• グラフにある歪みを最小化するように𝒙1:𝑡を最適化
– 例
• その後地図を求める
• 逐次SLAM
– 本日解説するFastSLAM
May 2, 2018 ロボットフロンティア第3回@中部大学 10

11. FAST SLAM
May 2, 2018 ロボットフロンティア第3回@中部大学 11

12. FastSLAM
• 逐次SLAM手法
• Monte Carlo localizationの延長
– 各パーティクルに地図の推定情報を付加
• 地図の推定情報: 全ランドマークの推定位置情報（pdf）
• 本日はランドマークベースのものを説明
May 2, 2018 ロボットフロンティア第3回@中部大学 12

13. FastSLAMのパーティクル
• 𝜉𝑡
(𝑖)
= 𝒙 𝑡
(𝑖)
, 𝑤𝑡
(𝑖)
, 𝒎1
𝑖
, 𝛴1
𝑖
, 𝒎2
𝑖
, 𝛴2
𝑖
, … , 𝒎 𝑀
(𝑖)
, 𝛴 𝑀
(𝑖)
𝑖 = 1,2, ⋯ , 𝑁
– pdfのパラメータ: 𝒎𝑖, 𝛴𝑖: 平均と共分散行列
– 観測モデルに制限: 𝑝 𝒛|𝒙, 𝒎 𝑘 = 𝒩(𝒛|𝒛∗, 𝑄)
• 𝒛∗: 姿勢𝒙から導かれる𝒎 𝑘の位置
• 𝑄: 観測値に予想されるばらつきの共分散行列
May 2, 2018 ロボットフロンティア第3回@中部大学 13
図: 実際環境にパーティクル1個を描き入れたもの
（赤, 黄色の星: 実際の姿勢と各ランドマークの位置、
青: パーティクルの情報）

14. FastSLAMのアルゴリズム
• ロボットの移動時
– 𝒙 𝑡
(𝑖)
を移動（MCLと同じ）
• ランドマーク観測時
– 地図の各ランドマークの推定位置を更新
• 𝐾 ← 𝛴𝑗
𝑖
(𝛴𝑗
𝑖
+ 𝑄)−1
• 𝒎𝑗
𝑖
← 𝒎𝑗
𝑖
+ 𝐾(𝒛 − 𝒎𝑗
𝑖
)
• 𝛴𝑗
𝑖
← (𝐼 − 𝐾)𝛴𝑗
𝑖
– 各パーティクルの地図を使って𝑤𝑡
(𝑖)
を変更（MCLと同じ）
• リサンプリングもMCLと同じ
May 2, 2018 ロボットフロンティア第3回@中部大学 14

15. 推定の例
May 2, 2018 ロボットフロンティア第3回@中部大学 15
t=1 t=10
t=18 t=25
注意: 原点は揃わない
t=15
t=35

16. 他の話題
May 2, 2018 ロボットフロンティア第3回@中部大学 16

17. 格子ベースのFastSLAM
• 各パーティクルに占有格子地図を持たせる
• 例
May 2, 2018 ロボットフロンティア第3回@中部大学 17

18. Graph-based SLAM
• 昨年の第三回の資料
– https://www.slideshare.net/ryuichiueda/robot-
frontier-lesson3
May 2, 2018 ロボットフロンティア第3回@中部大学 18

19. 3D SLAM
• CNN-SLAM [Tateno 2017]
• 頭にVelodyneをつけて走る [Zhang 2017]
• rtabmap
– 上田研でもよく動かしてます
• 傾向
– 3Dのものはだんだん移動モデルを
使わなくなってきているようだ
• 画像処理重視に
– 2DのFastSLAMもまだまだ現役
• 結局、移動ロボットを動かすには十分な場合が多い
May 2, 2018 ロボットフロンティア第3回@中部大学 19