確率ロボティクス入門第2回

上田隆一

本日の話の流れ

• 自己位置推定問題

– 最も一般的な問題を提示

• Monte Carlo localization

– 基本的な手法

• より難しい問題と対策

– 大域的自己位置推定・誘拐ロボット問題・リセット

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

本題の前に

• 動くサンプルコードを用意しました

– https://github.com/ryuichiueda/probrobo_practice

• Jupyter notebook + Pythonで作成

• セットアップの上、実行してみましょう

• 読むだけならGitHubのサイトで読めます– 便利ですね

–本日の話もこれに基づきます

• あとでサンプルコードを使って復習を

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

自己位置推定問題

May 17, 2017 ロボットフロンティア第2回@中部大学 4

ロボットが移動しようとした経路（灰色矢印）

姿勢と姿勢変化

May 17, 2017 ロボットフロンティア第2回@中部大学 5

• 𝒙 = 𝑥, 𝑦, 𝜃 で姿勢が表されるロボット

• 移動で𝒙が変化していく

– 𝒙0を初期姿勢として離散時刻で𝒙1, 𝒙2, 𝒙3, ⋯と変化

– 移動モデル： 𝑝 𝒙𝑡|𝒙𝑡−1, 𝑎𝑡 に従う

ロボットが実際に移動した経路（赤色矢印）

観測

• 環境中にランドマーク

– 次の値が計測可能

• 𝑑: ロボットからの距離

• 𝜑:見える方角

– どちらも雑音でばらつく

– 観測モデル 𝑝 𝒛|𝒙 は既知

• ここで 𝒛 = (𝑑, 𝜑)

May 17, 2017 ロボットフロンティア第2回@中部大学 6

オレンジの線が観測結果（ばらつき）

問題• 真の姿勢𝒙∗に関する確率分布 𝑏𝑒𝑙𝑡(𝒙)をどう計算する？

– 𝑏𝑒𝑙𝑡 𝒙 = 𝑝(𝒙𝑡 = 𝒙∗|𝑎1:𝑡 , 𝒛1:𝑡 , 𝒙0)

• 課題

– 𝑏𝑒𝑙(𝒙)の式が難解に

• なんらかの近似が必要

– 推定対象の次元が3で、観測で得られる値の次元が2

May 17, 2017 ロボットフロンティア第2回@中部大学 7

𝒙0

𝒛1:𝑡

𝒙∗

𝑎1:𝑡

MONTE CARLO LOCALIZATION

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

パーティクルフィルタによる近似

• パーティクル（粒子）

– ロボットの分身

• ロボットが移動したら– 𝑝 𝒙|𝒙′, 𝑎𝑡 に従って各パーティクルを移動（右図）

– ロボット自身が計算機の中でシミュレート

• ランドマークを観測したら

– ベイズの定理で反映

May 17, 2017 ロボットフロンティア第2回@中部大学 9

一つ一つが真の姿勢の「分身」

パーティクルの定義

• 𝜉𝑡−1(𝑖)= 𝒙𝑡−1

(𝑖), 𝑤𝑡(𝑖)

𝑖 = 1,2,⋯ ,𝑁

– 姿勢と、もう一つ変数を持つ構造体 or クラスを𝑁個準備

– 変数

• 𝒙𝑡−1(𝑖)= (𝑥𝑡−1

𝑖, 𝑦𝑡−1𝑖, 𝜃𝑡−1𝑖)

• 𝑤𝑡(𝑖)

: 重み（初期値1/𝑁）

• 右図

– 赤が実際のロボットの姿勢

– 青がパーティクル

May 17, 2017 ロボットフロンティア第2回@中部大学 10

姿勢に加えて、観測に合致している度合いを表す「重み」を持つ

パーティクルの性質

• 𝑥𝑦𝜃空間を適当に囲った中にあるパーティクルの重みの合計が、その囲いの中に𝒙∗が存在する確率

– そうなるように計算しなければならない

• 数で計算能力と近似能力が変化

– 少ないと右図のようにムラ

– 周辺部で不足がち

May 17, 2017 ロボットフロンティア第2回@中部大学 11

本来ないはずの空白地帯

移動時の処理

• 移動時の処理: 𝒙𝑡(𝑖)~𝑝 𝒙|𝒙𝑡−1

𝑖, 𝑎𝑡

– 移動モデルに従って一つずつパーティクルを動かす

– 全体でこの式を近似

• 𝑏𝑒𝑙𝑡 𝒙 = 𝑝 𝒙𝑡 𝑎1:𝑡 , 𝒛1:𝑡−1, 𝒙0= 𝑝 𝒙|𝒙𝑡−1, 𝑎𝑡 𝑝 𝒙𝑡−1 𝑎1:𝑡−1, 𝒛1:𝑡−1, 𝒙0 𝑑𝒙𝑡−1

= 𝑝 𝒙|𝒙′, 𝑎𝑡 𝑏𝑒𝑙𝑡−1(𝒙′) 𝑑𝒙′

• 𝑏𝑒𝑙𝑡−1(𝒙′)からサンプリングして、再度𝑝 𝒙|𝒙′, 𝑎𝑡 からサンプリング

May 17, 2017 ロボットフロンティア第2回@中部大学 12

観測時の処理

• 重みを変更することでベイズの定理を近似

1. 𝑤𝑡(𝑖)← 𝑤𝑡

(𝑖)𝑝 𝒛|𝒙𝑡

(𝑖)

2. その後、重みの和が1になるように正規化

– 計測値と矛盾の大きいパーティクルの重みが減少

– 具体的な計算は次のページで

• リサンプリングという処理が必要

– 後述

May 17, 2017 ロボットフロンティア第2回@中部大学 13

具体的な計算の例• 状況

– パーティクルの姿勢は 𝒙𝑡(𝑖)

– センサの値は𝒛 = (𝑑, 𝜑)

– 𝒙𝑡(𝑖)からは𝒛′ = (𝑑′, 𝜑′)で

観測されるはず

• 事前知識

– 計測値𝑑,𝜑は、それぞれ標準偏差𝜎𝑑,𝜎𝜑でばらつく

• 𝑝 𝒛|𝒙𝑡(𝑖)∝exp[−

1

2𝜎𝑑𝑑−𝑑′ 2]

𝜎𝑑 2𝜋

exp[−1

2𝜎𝜑𝜑−𝜑′ 2]

𝜎𝜑 2𝜋

May 17, 2017 ロボットフロンティア第2回@中部大学 14

パーティクル

の姿勢 𝒙𝑡(𝑖)

真の姿勢𝒙∗

𝑑

𝜑

𝑑′ 観測

𝜑′

注意：Jupyter notebookの例では違う計算をしています

リサンプリング

• 𝑁個の重みの異なるパーティクルから𝑁個選び直して重みを1/𝑁に戻す処理

• 近似能力の維持のため

• 観測後に毎回行うのが標準的

• こういう処理に似ている

– A, B, C室の面積はそれぞれ10m2, 17m2, 1m2

– 15人をA, B, C室に割り振るには？

– 考慮すべきこと• 計算量（パーティクルの数に比例すること。それより大きいとまずい。）

• バイアス（何回もリサンプリングしたときに確率的に公平なこと。）

May 17, 2017 ロボットフロンティア第2回@中部大学 15

処理前（大きさ = 重み）

処理後

具体的な処理

• 系統サンプリングの例– （systematic sampling。確率ロボティクスでは「等間隔サンプリング」と翻訳）

1. パーティクルの重みを並べて累積確率分布を作成

2. [0,1/𝑁)から1つ乱数𝛾を選ぶ

3. 𝑃 = 𝛾, 𝛾 +1

𝑁, 𝛾 +

2

𝑁, ⋯ , 𝛾 +

𝑁−1

𝑁地点のパーティクルを選択

May 17, 2017 ロボットフロンティア第2回@中部大学 16

0 1確率の数直線

・・・

・・・

𝛾

・・・

1

𝑁

1

𝑁

1

𝑁

より難しい問題と対策

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

自己位置推定の3つの問題

May 17, 2017 ロボットフロンティア第2回@中部大学 18

• トラッキング

– 𝒙0は分かっているという仮定（これまで喋ってきたもの）

• 大域的自己位置推定（global localization）

– 𝒙0は分からないという前提

– 𝑏𝑒𝑙0が一様分布

• 誘拐ロボット問題（kidnapped robot problem）

– 移動モデルが破綻

– 今まで信じていた𝑏𝑒𝑙𝑡が一瞬で間違いに

誘拐ロボット問題が頻発する例 人による置き直し 足が絡んで想定外の移動誤差

大域的自己位置推定

• 初期姿勢が不明なのでパーティクルの初期姿勢もランダムに

– パーティクルが運よく𝒙∗付近に配置される期待はパーティクルの数が多くないとできない

• 一度𝒙∗とパーティクルの分布が離れると修正は期待できない

May 17, 2017 ロボットフロンティア第2回@中部大学 19

図: 長時間修正が効かない場合

このパーティクルの姿勢がたまたま観測と合致

誘拐ロボット問題

• 動いているロボットを持ち上げて置き直す

– デッドレコニングが破綻

– 大域的自己位置推定でパーティクルが不足するときと同様に𝒙∗とパーティクルの分布が分離

– パーティクルが多くてもダメ（下の例だと300個）

May 17, 2017 ロボットフロンティア第2回@中部大学 20

誘拐

問題の本質

• パーティクルがない領域では𝑏𝑒𝑙𝑡 𝒙 を表現できない

• 移動/観測モデルと違うことには対処できない

• 根本的な解決法（過去受理された論文に基づくが、個人的な考え）

– 間違いは許容すべき

• どんな手法を持ってきても実世界で動くエージェントが実時間で全領域を意識することは不可能

–ロボットに限らず

– 誤りを認識する能力を持たせる: メタ認知

May 17, 2017 ロボットフロンティア第2回@中部大学 21

どこで誤りを見つけるか

• ベイズの定理の分母から見つける方法が簡単

– ベイズの定理: • 𝑏𝑒𝑙𝑡 𝒙|𝒛𝑡 = 𝜂

−1𝑝 𝒛𝑡|𝒙 𝑏𝑒𝑙𝑡 𝒙

• where 𝜂 = 𝒳 𝑝 𝒛𝑡|𝒙 𝑏𝑒𝑙𝑡 𝒙 𝑑𝒙

– 𝜂の値は𝑝 𝒛|𝒙 と 𝑏𝑒𝑙𝑡 𝒙 の分布が離れるほど小さく

– 参考: カルバック・ライブラー情報量

• 𝜂の値が閾値以下になったら誤っていると考えてみましょう

– じゃあ誤ってたらどうすればいいのか？

May 17, 2017 ロボットフロンティア第2回@中部大学 22

誤りからの仕切り直し（リセット）

• 仕切り直して信念を再構築– 手がかり

• 直近の信念: 𝑏𝑒𝑙𝑡 𝒙

• 直近の観測から推測される姿勢の確率分布: 𝑝 𝒙|𝒛𝑡– 観測モデル𝑝 𝒛𝑡|𝒙 を流用し、𝒛𝑡を固定して𝒙をサンプリングすると得られる

• 信念のリセットとして妥当だと思われるものa. センサリセット: 𝑏𝑒𝑙𝑡 𝒙 を破棄、𝑝 𝒙|𝒛𝑡 を信念に[Lenser 2000]

b. 膨張リセット: 𝑏𝑒𝑙𝑡 𝒙 をぼかす[上田 2005]

c. a,bを混ぜる[上田 2005]

May 17, 2017 ロボットフロンティア第2回@中部大学 23

センサリセット

• 𝑝 𝒙|𝒛𝑡 に基づいてパーティクルを置きなおし

– 1回の観測得られる情報が多い場合に有利

– 小さい誘拐で発動するとこれまでの推定が台無しに

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

このランドマークの観測結果から信念を再構築

リセットの起こった瞬間 アニメーション

膨張リセット

May 17, 2017 ロボットフロンティア第2回@中部大学 25

• リセット直前の𝑏𝑒𝑙𝑡 𝒙 をぼかしてパーティクルを再配置

– パーティクルをランダムに動かして分布拡大

– 小さい誘拐で発動しても問題ない

– 大きい誘拐では効率が悪い

– センサリセットと組み合わせるとよい[上田05]

誘拐 膨張1回目 膨張2回目 収束

アニメーション

まとめ• 自己位置推定の問題

– 移動/観測モデルからロボットが自身で姿勢を推定

• パーティクルフィルタ

– 「パーティクル」で確率分布を近似• マルコフ連鎖、ベイズの定理の演算を𝑂(𝑁)で

• 他の手法やパーティクルフィルタ出現の背景についてはまた別の機会で

• 誘拐ロボット問題が解くことでロボットの自律性が向上

– 雑に置いても自身の位置を理解

– まだまだ研究されていないことが多い

May 17, 2017 ロボットフロンティア第2回@中部大学 26