May 24, 2017 @ Chubu Univ.

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

2. 本日の話の流れ
• SLAM
– SLAMとは何か
• SLAM手法の例
– よく使われるものと最新のもの
• graph-based SLAM
– 理論
May 24, 2017 ロボットフロンティア第3回@中部大学 2

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

4. SLAM（simultaneous localization and mapping）
• こういう問題
– 自己位置推定の問題について、
ランドマークの位置を未知に
– ロボットの初期姿勢𝒙0を原点に置く
– ロボットの姿勢とランドマークの位置（姿勢）を
両方求めてください
• 可能なのか？→ 可能
May 24, 2017 ロボットフロンティア第3回@中部大学 4

5. 人間（素人）の平面地図の作り方
• 作り方1
– 白い紙を用意
– 見えたものを地図に書く
– 歩く
– 見えたものを前に書いたものと矛盾なく書く
– 以後繰り返し
• 作り方2
– 散歩して見えたものを覚えておいて後からまとめて書く
May 24, 2017 ロボットフロンティア第3回@中部大学 5
 そんなに難しくない

6. ロボットに地図を作らせる
• 課題
– 見たものを矛盾なくつなぎ合わせる → 大変
• どれくらい大変か
– ここ20年間、ロボティクス分野で一番数学の
できる連中の扱う課題のひとつだった
– 今は使う・理解するのはさほど難しくない
• 計算機、センサの進化
• 研究成果
• ROSを中心としたモジュール化
May 24, 2017 ロボットフロンティア第3回@中部大学 6

7. SLAMの手法の例
May 24, 2017 ロボットフロンティア第3回@中部大学 7

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

9. FastSLAM [Montemerlo 2003]
• MCLのパーティクル一つ一つに地図を持たせる
– 地図の種類
• その1: 点ランドマークの位置リスト
• その2: 占有格子地図（動画）
– 2次元平面を格子状に
区切り壁の有無を記録
したもの
• アルゴリズム
– 移動でパーティクルを移動
– センサの値と観測モデル、地図を比較して重みを変更
– 地図をセンサの値に基づいて変更
May 24, 2017 ロボットフロンティア第3回@中部大学 9

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

11. graph-based SLAM
• 情報フィルタを用いた最適化手法
• 他のものにも応用が利く
• 本日扱うアルゴリズム
– この解説 [Grisetti 2010]の数式を簡単な例に適用したもの
– サンプルコード
• https://github.com/ryuichiueda/probrobo_practice/blob/master/gra
ph-based_SLAM/graph-based_slam.ipynb
– 数式を交えた解説も書きました
• https://github.com/ryuichiueda/commentary_on_graph-
based_slam/blob/master/commentary_on_graph-based_slam.pdf
May 24, 2017 ロボットフロンティア第3回@中部大学 11

12. GRAPH-BASED SLAM
May 24, 2017 ロボットフロンティア第3回@中部大学 12

13. 問題
• ロボットがパス（姿勢遷移） 𝒙0:𝑇で移動したと
思っている
– 図の灰色の矢印
– 実際のパス（赤色の矢印）とずれている
• 各姿勢でランドマークを観測
– 得られる値のシーケンス: 𝒛0:𝑇
（右図の桃色の線）
• 𝒛 = (𝑑, 𝜑, 𝜓)
– 𝑑: ロボットからの距離
– 𝜑:見える方角
– 𝜓: ランドマークの向き
（問題を簡単にするため追加）
May 24, 2017 ロボットフロンティア第3回@中部大学 13

14. 問題（続き）
• 問題:
– 𝒙0:𝑇と𝒛0:𝑇と観測モデル𝑝 𝒛|𝒙 から
真のパス𝒙0:𝑇
∗
を推定してみましょう
– つまり、右の図の桃色の線が
ランドマークのところに合うように、
灰色の矢印の位置を修正する問題
– 𝒛0:𝑇に雑音が混入しているので完全には一致しない
– 最適化問題を適切に定義して解く
May 24, 2017 ロボットフロンティア第3回@中部大学 14

15. 解き方の例
A) 各時刻の姿勢をグラフのノードに
B) 同じランドマークを見た2姿勢をエッジで結ぶ
– 移動履歴から計算される姿勢と
観測から計算される姿勢に差
C) 差を解消するようにノードを初期値から移動
– エッジが互いに矛盾することも
– 観測値の信頼性で差に重み付けしなければならない
May 24, 2017 ロボットフロンティア第3回@中部大学 15
図: 拘束のグラフ（互いに歪み）
𝒙 𝑡 𝒙 𝑡′
𝑧𝑡 𝑧 𝑡′
図: 2つの観測から2姿勢間の位置関係を計算

16. 2方式で計算される姿勢の差
• 移動の履歴から計算される相対位置: 𝒙 𝑡′ − 𝒙 𝑡
– ここでは移動の誤差は考えない
– 当然誤差がある
• 観測𝑧𝑡, 𝑧 𝑡′から計算される相対位置: 𝝁 𝑐,𝑡,𝑡′
– 𝑐はランドマークのID
– 誤差の考慮は後で
• 二つの情報のズレ
– 𝒆 𝑐,𝑡,𝑡′ = 𝒙 𝑡′ − 𝒙 𝑡 − 𝝁 𝑐,𝑡,𝑡′
May 24, 2017 ロボットフロンティア第3回@中部大学 16
𝒙 𝑡 𝒙 𝑡′
𝑧𝑡 𝑧 𝑡′

17. 𝒆 𝑐,𝑡,𝑡′の計算
• 𝒆 𝑐,𝑡,𝑡′ = 𝒙 𝑡′ − 𝒙 𝑡 − 𝝁 𝑐,𝑡,𝑡′
=
𝑥 𝑡′ − 𝑥𝑡 − 𝑑 𝑐,𝑡cos 𝜃𝑡 + 𝜑 𝑐,𝑡 +𝑑 𝑐,𝑡′ cos 𝜃 𝑡′ + 𝜑 𝑐,𝑡′
𝑦 𝑡′ − 𝑦𝑡 − 𝑑 𝑐,𝑡sin 𝜃𝑡 + 𝜑 𝑐,𝑡 +𝑑 𝑐,𝑡′ sin 𝜃 𝑡′ + 𝜑 𝑐,𝑡′
𝜃 𝑡′ − 𝜃𝑡 − 𝜓 𝑐,𝑡 + 𝜓 𝑐,𝑡′
May 24, 2017 ロボットフロンティア第3回@中部大学 17

18. 𝒙 𝑡𝑡′を少し動かしたときの𝒆 𝑐,𝑡,𝑡′の変化
• 後から計算で使います
• 𝒆 𝑐,𝑡,𝑡′ 𝒙 𝑡𝑡′ + ∆𝒙 = 𝒆 𝑐,𝑡,𝑡′ 𝒙 𝑡𝑡′ +
𝝏𝒆 𝑐,𝑡,𝑡′
𝝏𝒙
| 𝒙=𝒙 𝑡𝑡′ ∆𝒙
•
𝝏𝒆 𝑐,𝑡,𝑡′
𝝏𝒙
| 𝒙=𝒙 𝑡𝑡′ = [𝐴 𝑐,𝑡,𝑡′ 𝐵 𝑐,𝑡,𝑡′]とすると
• 𝐴 𝑐,𝑡,𝑡′ =
−1 0 𝑑 𝑐,𝑡sin 𝜃𝑡 + 𝜑 𝑐,𝑡
0 −1 −𝑑 𝑐,𝑡cos 𝜃𝑡 + 𝜑 𝑐,𝑡
0 0 −1
• 𝐵 𝑐,𝑡,𝑡′ =
1 0 −𝑑 𝑐,𝑡′sin(𝜃 𝑡′ + 𝜑 𝑐,𝑡′)
0 1 𝑑 𝑐,𝑡′cos(𝜃 𝑡′ + 𝜑 𝑐,𝑡′)
0 0 1
May 24, 2017 ロボットフロンティア第3回@中部大学 18

19. エッジの歪み
• 𝒆 𝑐,𝑡,𝑡′の大きさだけでは評価できない
– ランドマークの計測値が信頼できる: 𝒆 𝑐,𝑡,𝑡′が大きいと問題
– 信頼性がないなら𝒆 𝑐,𝑡,𝑡′が大きくてもさほど問題でない
– マハラノビス距離 𝒆 𝑐,𝑡,𝑡′
𝑇
Ω 𝑐,𝑡,𝑡′ 𝒆 𝑐,𝑡,𝑡′ で評価
• 情報行列: Ω 𝑐,𝑡,𝑡′
– 共分散行列の逆行列
– この場合の共分散行列: 𝛴 𝑐,𝑡,𝑡′ = 𝑅 𝑐,𝑡 𝛴𝑐,𝑡 𝑅 𝑐,𝑡
𝑇
+ 𝑅 𝑐,𝑡′ 𝛴 𝑐,𝑡′ 𝑅 𝑐,𝑡′
𝑇
• 𝑅は回転行列
• 計測値のばらつきに関する共分散行列を
回転して和をとったもの
May 24, 2017 ロボットフロンティア第3回@中部大学 19
𝒙 𝑡 𝒙 𝑡′
𝑧𝑡 𝑧 𝑡′
𝛴𝑐,𝑡𝛴 𝑐,𝑡′

20. グラフ全体の歪み
• 𝐹 𝒙0:𝑇 = 𝒆 𝑐,𝑡,𝑡′∈ℰ
𝒆 𝑐,𝑡,𝑡′
𝑇
Ω 𝑐,𝑡,𝑡′ 𝒆 𝑐,𝑡,𝑡′
– ℰ: 2姿勢間の誤差の集合
– 𝒙0:𝑇: 初期姿勢から最後の姿勢までの姿勢の集合
– 評価関数𝐹を最小にする𝒙0:𝑇を求める
（ 𝒙0:𝑇 を最適な場所に動かす）
• どの方向に各𝒙 𝑡を動かすのか？
May 24, 2017 ロボットフロンティア第3回@中部大学 20

21. 解法
• 𝐹 𝒙0:𝑇 + ∆𝒙0:𝑇 が最小になる変化量∆𝒙0:𝑇を求めると
良いが、どうやって解くのか？
– 以下、証明は無しでアルゴリズムだけ示します
• 情報行列𝐻と情報ベクトル𝒃を準備
– 𝐻: 3𝑇 + 1 × 3𝑇 + 1
– 𝒃: (3𝑇 + 1)次元
– 3𝑇 + 1: 𝒙0:𝑇の全変数の数
• ただしここでは観測のない時刻の姿勢を𝒙0:𝑇から除外
• ある手続きで𝐻, 𝒃を作ると∆𝒙0:𝑇が以下の式で求まる
– ∆𝒙0:𝑇 = 𝐻−1
𝒃
May 24, 2017 ロボットフロンティア第3回@中部大学 21

22. 𝐻, 𝒃の計算（1）
• 𝐻 の 𝒙 𝑡と𝒙 𝑡′に対応する部分を𝐻[𝑡𝑡′]、
𝒃の𝒙 𝑡に対応する部分を𝒃[𝑡]と表す
– 𝐻 =
𝐻[00] ⋯ 𝐻[0𝑇]
⋮ ⋱ ⋮
𝐻[𝑇0] ⋯ 𝐻[𝑇𝑇]
, 𝒃 =
𝒃[0]
⋮
𝒃[𝑇]
• 初期化
– 𝐻, 𝒃の全要素を0で初期化
– ただし𝐻[00]の部分だけ大きな数をかけた単位行列で初期化
• 座標を固定するため
May 24, 2017 ロボットフロンティア第3回@中部大学 22

23. 𝐻, 𝒃の計算（2）
• 各エッジの歪みの情報を足しこむ
– 𝐻[𝑡𝑡] += 𝐴 𝑐,𝑡,𝑡′
𝑇
Ω 𝑐,𝑡,𝑡′ 𝐴 𝑐,𝑡,𝑡′
– 𝐻[𝑡𝑡′] += 𝐴 𝑐,𝑡,𝑡′
𝑇
Ω 𝑐,𝑡,𝑡′ 𝐵 𝑐,𝑡,𝑡′
– 𝐻[𝑡′ 𝑡] += 𝐵𝑐,𝑡,𝑡′
𝑇
Ω 𝑐,𝑡,𝑡′ 𝐴 𝑐,𝑡,𝑡′
– 𝐻[𝑡′ 𝑡′] += 𝐵𝑐,𝑡,𝑡′
𝑇
Ω 𝑐,𝑡,𝑡′ 𝐵 𝑐,𝑡,𝑡′
– 𝒃[𝑡] += 𝐴 𝑐,𝑡,𝑡′
𝑇
Ω 𝑐,𝑡,𝑡′ 𝒆 𝑐,𝑡,𝑡′
– 𝒃[𝑡′] += 𝐵𝑐,𝑡,𝑡′
𝑇
Ω 𝑐,𝑡,𝑡′ 𝒆 𝑐,𝑡,𝑡′
• 大雑把に考えると、各エッジについて、
マハラノビス距離の変化量を足しているイメージ
May 24, 2017 ロボットフロンティア第3回@中部大学 23
𝐴 𝑐,𝑡,𝑡′, 𝐵 𝑐,𝑡,𝑡′のおさらい:
𝝏𝒆 𝑐,𝑡,𝑡′
𝝏𝒙
| 𝒙=𝒙 𝑡𝑡′ = [𝐴 𝑐,𝑡,𝑡′ 𝐵 𝑐,𝑡,𝑡′]

24. 𝐻, 𝒃の計算（3）
• 𝒙0:𝑇を∆𝒙0:𝑇だけ動かして更新
⦁ 𝒙0:𝑇 += 𝐻−1 𝒃
• 以上の手続きを収束まで何回か繰り返す
– 線形化の際に誤差があるので1回で終わらない
May 24, 2017 ロボットフロンティア第3回@中部大学 24

25. 実行例
May 24, 2017 ロボットフロンティア第3回@中部大学 25
初期値 収束後
アニメーション
ロボットの動作と観測
• 青の点: 𝒆 𝑐,𝑡,𝑡′から導かれる
𝒙 𝑡 から見たときの 𝒙 𝑡′の姿勢
• 楕円: 𝚺 𝑐,𝑡,𝑡′

26. まとめ
• SLAM
– センサデータを貼り合わせて地図を作成
• 様々な確率的な手法、最適化手法
– 高度な画像処理に基づくものが
最近の研究のトレンド
• 基本は最適化問題を作って解くというものなので、
根本の理屈を抑えることが重要です。
• graph-based SLAM
– 情報フィルタを用いた最適化
May 24, 2017 ロボットフロンティア第3回@中部大学 26