Apr 25, 2018 @中部大学

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

2. 前回のおさらい
• ロボットが確率という概念を持つと、
もっと賢くなるかもしれない。
Apr 25, 2018 ロボットフロンティア第2回@中部大学 2

3. 本日の話
• どうやってロボットで確率を扱うか
• 自己位置推定問題
– 最も一般的な問題を提示
• Monte Carlo localization
– 基本的な手法
• より難しい問題と対策
– 大域的自己位置推定・誘拐ロボット問題・リセット
Apr 25, 2018 ロボットフロンティア第2回@中部大学 3

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

5. 自己位置推定問題
Apr 25, 2018 ロボットフロンティア第2回@中部大学 5

6. 問題1
• Belが赤の分布のときにロボットが移動したら
次のBelはどうなるでしょうか？
Apr 25, 2018 ロボットフロンティア第2回@中部大学 6

7. 解答
• 𝐵𝑒𝑙(𝑥∗
= 𝐴) = 0.4 ∗ 0.5 = 0.2
• 𝐵𝑒𝑙 𝑥∗
= 𝐵 = 0.4 ∗ 0.5 + 0.6 ∗ 0.5 = 0.5
• 𝐵𝑒𝑙(𝑥∗
= 𝐶) = 0.6 ∗ 0.5 = 0.3
Apr 25, 2018 ロボットフロンティア第2回@中部大学 7
𝐴 𝐵 𝐶𝐴 𝐵 𝐶

8. 問題2
• Belが赤の分布のときにセンサ値dを得たら次のBel
はどうなるでしょうか？
– 各位置であるセンサの値を得る確率P(d|x)は既知
Apr 25, 2018 ロボットフロンティア第2回@中部大学 8
𝐴 𝐵 C

9. 解答（なんでこうなるかは次のページで）
• 0.4 × 0.2 = 0.08
• 0.6 × 0.5 = 0.3
• 𝐵𝑒𝑙 𝐴 =
0.08
0.08+0.3
= 0.21
• 𝐵𝑒𝑙 𝐵 =
0.3
0.08+0.3
= 0.79
Apr 25, 2018 ロボットフロンティア第2回@中部大学 9
𝐴 𝐵 C 𝐴 𝐵
観測前 観測後

10. ベイズの定理
• 離散系
– 𝑃 𝑥|𝑧 =
𝑃 𝑧|𝑥 𝑃(𝑥)
𝑥′∈𝑋
𝑃 𝑧|𝑥′ 𝑃(𝑥′)
= 𝜂𝑃 𝑧|𝑥 𝑃(𝑥)
– 𝑥が推定対象、𝑧が観測
– 𝜂は確率の総和を1に保つための定数（正規化定数）
• 尤度
– 𝑝 𝑧|𝑥 を𝑥の関数とみなしてℓ 𝑥|𝑧 とも表記
• 確率でなくても良い
– センサ情報𝑧が得られたとき
𝑥がどの程度尤もらしいか
Apr 25, 2018 ロボットフロンティア第2回@中部大学 10
更新前の信念
更新後の信念

11. 実際の移動ロボットの自己位置推定
• 問題
– ロボットは連続空間を動く
– 空間をマス目に切って自己位置推定する
方法もあるにはある
• ただし、スケールしない
• 次ページ以降で問題を整理してから
よく使われる方法を説明
Apr 25, 2018 ロボットフロンティア第2回@中部大学 11

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

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

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

15. MONTE CARLO LOCALIZATION
Apr 25, 2018 ロボットフロンティア第2回@中部大学 15

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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