May 17, 2017 @ Chubu Univ.

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

2. 本日の話の流れ
• 自己位置推定問題
– 最も一般的な問題を提示
• Monte Carlo localization
– 基本的な手法
• より難しい問題と対策
– 大域的自己位置推定・誘拐ロボット問題・リセット
May 17, 2017 ロボットフロンティア第2回@中部大学 2

3. 本題の前に
• 動くサンプルコードを用意しました
– https://github.com/ryuichiueda/probrobo_practice
• Jupyter notebook + Pythonで作成
• セットアップの上、実行してみましょう
• 読むだけならGitHubのサイトで読めます
– 便利ですね
– 本日の話もこれに基づきます
• あとでサンプルコードを使って復習を
May 17, 2017 ロボットフロンティア第2回@中部大学 3

4. 自己位置推定問題
May 17, 2017 ロボットフロンティア第2回@中部大学 4

5. ロボットが移動
しようとした経路
（灰色矢印）
姿勢と姿勢変化
May 17, 2017 ロボットフロンティア第2回@中部大学 5
• 𝒙 = 𝑥, 𝑦, 𝜃 で姿勢が表されるロボット
• 移動で𝒙が変化していく
– 𝒙0を初期姿勢として離散時刻で𝒙1, 𝒙2, 𝒙3, ⋯と変化
– 移動モデル： 𝑝 𝒙 𝑡|𝒙 𝑡−1, 𝑎 𝑡 に従う
ロボットが実際
に移動した経路
（赤色矢印）

6. 観測
• 環境中にランドマーク
– 次の値が計測可能
• 𝑑: ロボットからの距離
• 𝜑:見える方角
– どちらも雑音でばらつく
– 観測モデル 𝑝 𝒛|𝒙 は既知
• ここで 𝒛 = (𝑑, 𝜑)
May 17, 2017 ロボットフロンティア第2回@中部大学 6
オレンジの線が観測結果
（ばらつき）

7. 問題
• 真の姿勢𝒙∗に関する確率分布 𝑏𝑒𝑙 𝑡(𝒙)をどう計算する？
– 𝑏𝑒𝑙 𝑡 𝒙 = 𝑝(𝒙 𝑡 = 𝒙∗|𝑎1:𝑡, 𝒛1:𝑡, 𝒙0)
• 課題
– 𝑏𝑒𝑙(𝒙)の式が難解に
• なんらかの近似が必要
– 推定対象の次元が3で、
観測で得られる値の次元が2
May 17, 2017 ロボットフロンティア第2回@中部大学 7
𝒙0
𝒛1:𝑡
𝒙∗
𝑎1:𝑡

8. MONTE CARLO LOCALIZATION
May 17, 2017 ロボットフロンティア第2回@中部大学 8

9. パーティクルフィルタによる近似
• パーティクル（粒子）
– ロボットの分身
• ロボットが移動したら
– 𝑝 𝒙|𝒙′, 𝑎 𝑡 に従って
各パーティクルを移動（右図）
– ロボット自身が計算機の中で
シミュレート
• ランドマークを観測したら
– ベイズの定理で反映
May 17, 2017 ロボットフロンティア第2回@中部大学 9
一つ一つが
真の姿勢の「分身」

10. パーティクルの定義
• 𝜉𝑡−1
(𝑖)
= 𝒙 𝑡−1
(𝑖)
, 𝑤𝑡
(𝑖)
𝑖 = 1,2, ⋯ , 𝑁
– 姿勢と、もう一つ変数を持つ構造体 or クラスを𝑁個準備
– 変数
• 𝒙 𝑡−1
(𝑖)
= (𝑥𝑡−1
𝑖
, 𝑦𝑡−1
𝑖
, 𝜃𝑡−1
𝑖
)
• 𝑤𝑡
(𝑖)
: 重み（初期値1/𝑁）
• 右図
– 赤が実際のロボットの姿勢
– 青がパーティクル
May 17, 2017 ロボットフロンティア第2回@中部大学 10
姿勢に加えて、観測に
合致している度合いを
表す「重み」を持つ

11. パーティクルの性質
• 𝑥𝑦𝜃空間を適当に囲った中にあるパーティクルの
重みの合計が、その囲いの中に𝒙∗
が存在する確率
– そうなるように計算しなければならない
• 数で計算能力と近似能力が変化
– 少ないと右図のようにムラ
– 周辺部で不足がち
May 17, 2017 ロボットフロンティア第2回@中部大学 11
本来ないはずの
空白地帯

12. 移動時の処理
• 移動時の処理: 𝒙 𝑡
(𝑖)
~𝑝 𝒙|𝒙 𝑡−1
𝑖
, 𝑎 𝑡
– 移動モデルに従って一つずつ
パーティクルを動かす
– 全体でこの式を近似
• 𝑏𝑒𝑙 𝑡 𝒙 = 𝑝 𝒙 𝑡 𝑎1:𝑡, 𝒛1:𝑡−1, 𝒙0
= 𝑝 𝒙|𝒙 𝑡−1, 𝑎 𝑡 𝑝 𝒙 𝑡−1 𝑎1:𝑡−1, 𝒛1:𝑡−1, 𝒙0 𝑑𝒙 𝑡−1
= 𝑝 𝒙|𝒙′, 𝑎 𝑡 𝑏𝑒𝑙 𝑡−1(𝒙′) 𝑑𝒙′
• 𝑏𝑒𝑙 𝑡−1(𝒙′)からサンプリングして、再度𝑝 𝒙|𝒙′, 𝑎 𝑡 からサンプリング
May 17, 2017 ロボットフロンティア第2回@中部大学 12

13. 観測時の処理
• 重みを変更することでベイズの定理を近似
1. 𝑤𝑡
(𝑖)
← 𝑤𝑡
(𝑖)
𝑝 𝒛|𝒙 𝑡
(𝑖)
2. その後、重みの和が1になるように正規化
– 計測値と矛盾の大きいパーティクルの
重みが減少
– 具体的な計算は次のページで
• リサンプリングという処理が必要
– 後述
May 17, 2017 ロボットフロンティア第2回@中部大学 13

14. 具体的な計算の例
• 状況
– パーティクルの姿勢は 𝒙 𝑡
(𝑖)
– センサの値は𝒛 = (𝑑, 𝜑)
– 𝒙 𝑡
(𝑖)
からは𝒛′ = (𝑑′, 𝜑′)で
観測されるはず
• 事前知識
– 計測値𝑑,𝜑は、それぞれ標準偏差𝜎 𝑑,𝜎 𝜑でばらつく
• 𝑝 𝒛|𝒙 𝑡
(𝑖)
∝
exp[−
1
2𝜎 𝑑
𝑑−𝑑′ 2
]
𝜎 𝑑 2𝜋
exp[−
1
2𝜎 𝜑
𝜑−𝜑′ 2
]
𝜎 𝜑 2𝜋
May 17, 2017 ロボットフロンティア第2回@中部大学 14
パーティクル
の姿勢 𝒙 𝑡
(𝑖)
真の姿勢𝒙∗
𝑑
𝜑
𝑑′
観測
𝜑′
注意：Jupyter notebookの
例では違う計算をしています

15. リサンプリング
• 𝑁個の重みの異なるパーティクルから
𝑁個選び直して重みを1/𝑁に戻す処理
• 近似能力の維持のため
• 観測後に毎回行うのが標準的
• こういう処理に似ている
– A, B, C室の面積はそれぞれ10m2
, 17m2
, 1m2
– 15人をA, B, C室に割り振るには？
– 考慮すべきこと
• 計算量（パーティクルの数に比例すること。それより大きいとまずい。）
• バイアス（何回もリサンプリングしたときに確率的に公平なこと。）
May 17, 2017 ロボットフロンティア第2回@中部大学 15
処理前
（大きさ = 重み）
処理後

16. 具体的な処理
• 系統サンプリングの例
– （systematic sampling。確率ロボティクスでは「等間隔サンプリング」と翻訳）
1. パーティクルの重みを並べて累積確率分布を作成
2. [0,1/𝑁)から1つ乱数𝛾を選ぶ
3. 𝑃 = 𝛾, 𝛾 +
1
𝑁
, 𝛾 +
2
𝑁
, ⋯ , 𝛾 +
𝑁−1
𝑁
地点のパーティクルを選択
May 17, 2017 ロボットフロンティア第2回@中部大学 16
0 1
確率の数直線
・・・
・・・
𝛾
・・・
1
𝑁
1
𝑁
1
𝑁

17. より難しい問題と対策
May 17, 2017 ロボットフロンティア第2回@中部大学 17

18. 自己位置推定の3つの問題
May 17, 2017 ロボットフロンティア第2回@中部大学 18
• トラッキング
– 𝒙0は分かっているという仮定（これまで喋ってきたもの）
• 大域的自己位置推定（global localization）
– 𝒙0は分からないという前提
– 𝑏𝑒𝑙0が一様分布
• 誘拐ロボット問題
（kidnapped robot problem）
– 移動モデルが破綻
– 今まで信じていた𝑏𝑒𝑙 𝑡が
一瞬で間違いに
誘拐ロボット問題が頻発する例
 人による置き直し
 足が絡んで想定外の移動誤差

19. 大域的自己位置推定
• 初期姿勢が不明なのでパーティクルの初期姿勢も
ランダムに
– パーティクルが運よく𝒙∗付近に配置される期待は
パーティクルの数が多くないとできない
• 一度𝒙∗とパーティクルの分布が離れると
修正は期待できない
May 17, 2017 ロボットフロンティア第2回@中部大学 19
図: 長時間修正が効かない場合
このパーティクルの姿勢が
たまたま観測と合致

20. 誘拐ロボット問題
• 動いているロボットを持ち上げて置き直す
– デッドレコニングが破綻
– 大域的自己位置推定でパーティクルが不足するときと同
様に𝒙∗
とパーティクルの分布が分離
– パーティクルが多くてもダメ（下の例だと300個）
May 17, 2017 ロボットフロンティア第2回@中部大学 20
誘拐

21. 問題の本質
• パーティクルがない領域では𝑏𝑒𝑙 𝑡 𝒙 を表現できない
• 移動/観測モデルと違うことには対処できない
• 根本的な解決法（過去受理された論文に基づくが、個人的な考え）
– 間違いは許容すべき
• どんな手法を持ってきても実世界で動くエージェントが
実時間で全領域を意識することは不可能
– ロボットに限らず
– 誤りを認識する能力を持たせる: メタ認知
May 17, 2017 ロボットフロンティア第2回@中部大学 21

22. どこで誤りを見つけるか
• ベイズの定理の分母から見つける方法が簡単
– ベイズの定理:
• 𝑏𝑒𝑙 𝑡 𝒙|𝒛 𝑡 = 𝜂−1 𝑝 𝒛 𝑡|𝒙 𝑏𝑒𝑙 𝑡 𝒙
• where 𝜂 = 𝒳
𝑝 𝒛 𝑡|𝒙 𝑏𝑒𝑙 𝑡 𝒙 𝑑𝒙
– 𝜂の値は𝑝 𝒛|𝒙 と 𝑏𝑒𝑙 𝑡 𝒙 の分布が離れるほど小さく
– 参考: カルバック・ライブラー情報量
• 𝜂の値が閾値以下になったら誤っていると
考えてみましょう
– じゃあ誤ってたらどうすればいいのか？
May 17, 2017 ロボットフロンティア第2回@中部大学 22

23. 誤りからの仕切り直し（リセット）
• 仕切り直して信念を再構築
– 手がかり
• 直近の信念: 𝑏𝑒𝑙 𝑡 𝒙
• 直近の観測から推測される姿勢の確率分布: 𝑝 𝒙|𝒛 𝑡
– 観測モデル𝑝 𝒛 𝑡|𝒙 を流用し、𝒛 𝑡を固定して
𝒙をサンプリングすると得られる
• 信念のリセットとして妥当だと思われるもの
a. センサリセット: 𝑏𝑒𝑙 𝑡 𝒙 を破棄、𝑝 𝒙|𝒛 𝑡 を信念に[Lenser 2000]
b. 膨張リセット: 𝑏𝑒𝑙 𝑡 𝒙 をぼかす[上田 2005]
c. a,bを混ぜる[上田 2005]
May 17, 2017 ロボットフロンティア第2回@中部大学 23

24. センサリセット
• 𝑝 𝒙|𝒛 𝑡 に基づいてパーティクルを置きなおし
– 1回の観測得られる情報が多い場合に有利
– 小さい誘拐で発動するとこれまでの推定が台無しに
May 17, 2017 ロボットフロンティア第2回@中部大学 24
このランドマーク
の観測結果から
信念を再構築
リセットの起こった瞬間 アニメーション

25. 膨張リセット
May 17, 2017 ロボットフロンティア第2回@中部大学 25
• リセット直前の𝑏𝑒𝑙 𝑡 𝒙 をぼかして
パーティクルを再配置
– パーティクルをランダムに動かして分布拡大
– 小さい誘拐で発動しても問題ない
– 大きい誘拐では効率が悪い
– センサリセットと組み合わせるとよい[上田05]
誘拐 膨張1回目 膨張2回目 収束
アニメーション

26. まとめ
• 自己位置推定の問題
– 移動/観測モデルからロボットが自身で姿勢を推定
• パーティクルフィルタ
– 「パーティクル」で確率分布を近似
• マルコフ連鎖、ベイズの定理の演算を𝑂(𝑁)で
• 他の手法やパーティクルフィルタ出現の
背景についてはまた別の機会で
• 誘拐ロボット問題が解くことでロボットの自律性が向上
– 雑に置いても自身の位置を理解
– まだまだ研究されていないことが多い
May 17, 2017 ロボットフロンティア第2回@中部大学 26