確率ロボティクス

Sebastian Thrun (著), Wolfram Burgard (著), Dieter Fox (著), 上田 隆一 (翻訳) 2007.

参考図書

第12回 確率システム制御特論

Probabilistic Robotics

Sebastian Thrun, Wolfram Burgard and Dieter Fox

PDFファイルをインターネットで読むことができる．

粒子𝒙𝑘−1(𝑙)

を𝑓 𝒙𝑘 𝒙𝑘−1(𝑙)

, 𝒖𝑘−1 に従って状態推移させ

粒子集合 𝒙𝑘(𝑙)

𝑙 = 1,2,⋯ ,𝑁 を発生させる．

運動モデル

3.1 尤度計算

粒子 𝒙𝑘(𝑙)

の尤度𝑤𝑘(𝑙)

= ℎ 𝒚𝑘 𝒙𝑘(𝑙),𝒎 を計算する．

一期先予測

ろ波

どのパラメータにどのような誤差が混入するのかを考慮した離散時間モデルを構築する必要がある．

センサモデルセンサの計測誤差がどのように生ずるのかを確率的にモデル化する．

12.1 制御系の状態推定にパーティクルフィルタを導入する

第12回 確率システム制御特論

12.1 制御系の状態推定にパーティクルフィルタを導入する

𝒙𝑘−1(𝑙)

𝒙𝑘(𝑙)

𝑓 𝒙𝑘 𝒙𝑘−1, 𝒖𝑘−1, 𝝃𝑘−1

① サンプリング

② 尤度評価

𝒎 𝒙𝑘(𝑙)

𝑤𝑘(𝑙)

𝒚𝑘

③ リサンプリング

𝒙𝑘(𝑙)

𝑤𝑘(𝑙)

𝒙𝑘

第12回 確率システム制御特論

システムモデルやインポータンスサンプリングによってパーティクルを遷移させる

計測や地図を頼りに粒子の尤度を求める

すべての粒子を有効利用するために

定期的に重みの小さな粒子を再配置する

4

k

ロボットへの入力

𝑣𝑘 : 時刻𝑘における並進速度

例題として，最も単純な平面移動ロボットの機構のモデルを考える。

x

y

(0,0)

( , )x y

ロボットの状態

時刻𝑘における慣性座標系における位置と方向

𝒙𝑘 = 𝑥𝑘 𝑦𝑘 𝜃𝑘𝑇 𝒖𝑘 = 𝑣𝑘 𝜔𝑘

𝑇

𝜔𝑘 : 時刻𝑘における並進速度

𝑣𝑘

第12回 確率システム制御特論

12.2 平面移動ロボットの確率状態方程式

時刻𝑘 − 1におけるロボットの状態𝒙𝑘−1に，制御入力𝒖𝑘−1が実行され，

システムノイズ𝝃𝑘−1が印可された場合のロボットの姿勢の事後確率分布

システムモデル

1kx1kx

実線で描かれた動作コマンドを実行した後のロボットの姿勢の事後分布。色が濃いほど尤度が高いことを示している。

確率分布は2次元写像されているが、本来はロボットの方向も考慮した三次元の分布である。

複雑な動作コマンドが与えられるほど不確かさは広がる。

𝑓 𝒙𝑘 𝒙𝑘−1, 𝒖𝑘−1, 𝝃𝑘−1

𝒖𝑘−1

𝒖𝑘−1

𝑓 𝒙𝑘 𝒙𝑘−1, 𝒖𝑘−1, 𝝃𝑘−1

統計・試行錯誤でシステムノイズ𝝃𝑘をモデル化する必要がある．

𝑓 𝒙𝑘 𝒙𝑘−1, 𝒖𝑘−1, 𝝃𝑘−1

第12回 確率システム制御特論

12.2 平面移動ロボットの確率状態方程式

6

( , )c cx y

𝒙𝑘−1 = 𝑥𝑘−1 𝑦𝑘−1 𝜃𝑘−1𝑇を初期姿勢とみなし ∆𝑡 の間ロボットの

入力を 𝑣𝑘−1 𝜔𝑘−1𝑇に保つとき，回転円の中心 𝑥𝑐 𝑦𝑐

𝑇を以下のように定めることができる。

①

②

ロボットの動作モデルの解析的導出

𝑥𝑐 = 𝑥𝑘−1 −𝑣𝑘−1𝜔𝑘−1

sin 𝜃𝑘−1 , 𝑦𝑐 = 𝑦𝑘−1 +𝑣𝑘−1𝜔𝑘−1

cos 𝜃𝑘−1

𝑣𝑘 = 𝜔𝑘 ∙ 𝑟 より

𝑟 =𝑣𝑘𝜔𝑘

𝑥𝑘−1 𝑦𝑘−1

𝜃𝑘−1𝜃𝑘−1 − π/2

𝑟

第12回 確率システム制御特論

12.2 平面移動ロボットの確率状態方程式

7

③

これらより移動時間∆𝑡の後、ロボットの姿勢𝒙𝑘 = 𝑥𝑘 𝑦𝑘 𝜃𝑘

𝑇は

( , )c cx y

ロボットの動作モデルの解析的導出

𝑥𝑘−1 𝑦𝑘−1

𝜃𝑘−1𝜃𝑘−1 − π/2

𝑟 𝑥𝑘𝑦𝑘𝜃𝑘

=

𝑥𝑐 +𝑣𝑘−1𝜔𝑘−1

sin(𝜃𝑘−1 + 𝜔𝑘−1∆𝑡)

𝑦𝑐 −𝑣𝑘−1𝜔𝑘−1

cos(𝜃𝑘−1 + 𝜔𝑘−1∆𝑡)

𝜃𝑘−1 + 𝜔𝑘−1∆𝑡

=

𝑥𝑘−1𝑦𝑘−1𝜃𝑘−1

+

−𝑣𝑘−1𝜔𝑘−1

sin 𝜃𝑘−1 +𝑣𝑘−1𝜔𝑘−1

sin(𝜃𝑘−1 + 𝜔𝑘−1∆𝑡)

−𝑣𝑘−1𝜔𝑘−1

cos(𝜃𝑘−1 + 𝜔𝑘−1∆𝑡)

𝜔𝑘−1∆𝑡

第12回 確率システム制御特論

12.2 平面移動ロボットの確率状態方程式

12.2 平面移動ロボットの確率状態方程式

8

④ ロボットの動作に加わる雑音を、平均値が０で有限な分布に従う確率変数でモデル化する。

⑤

1 4 ～ はロボットに固有な誤差パラメータ。ロボット動作の不正確さを示しており、不正確なほど値が大きくなる。

回転半径に加わる雑音を、上と同様にモデル化する。

と は、回転雑音の標準偏差を決定するためのロボット固有のパラメータ。5 6

⑥ すべて考慮した最終的なロボットの動作モデルは

𝑓 𝒙𝑘 𝒙𝑘−1, 𝒖𝑘−1, 𝝃𝑘−1

휀𝛼𝑏2 =1

2𝜋𝑏2exp −

1

2

𝛼2

𝑏2

𝜃𝑘 = 𝜃𝑘−1 + 𝜔𝑘−1∆𝑡 + 𝛾∆𝑡 𝛾 = 휀𝛼5𝑣2+𝛼6𝜔2

𝑥𝑘𝑦𝑘𝜃𝑘

=

𝑥𝑘−1𝑦𝑘−1𝜃𝑘−1

+

− 𝑣𝑘−1 𝜔𝑘−1

sin 𝜃𝑘−1 + 𝑣𝑘−1 𝜔𝑘−1

sin( 𝜃𝑘−1 + 𝜔𝑘−1∆𝑡)

𝑣𝑘−1 𝜔𝑘−1

cos 𝜃𝑘−1 − 𝑣𝑘−1 𝜔𝑘−1

cos( 𝜃𝑘−1 + 𝜔𝑘−1∆𝑡)

𝜔𝑘−1∆𝑡 + 𝛾∆𝑡

𝑣𝑘𝜔𝑘

=휀𝛼1𝑣2+𝛼2𝜔2

휀𝛼3𝑣2+𝛼4𝜔2

第12回 確率システム制御特論

9

1

2

3

4

5

6

7

8

Algorithm sample_motion_velocity

return

表１姿勢 𝒙𝑘−1= 𝑥𝑘−1, 𝑦𝑘−1, 𝜃𝑘−1

𝑇と制御入力 𝒖𝑘−1= 𝑣𝑘−1, 𝜔𝑘−1𝑇

から姿勢 𝒙𝑘= 𝑥𝑘 , 𝑦𝑘 , 𝜃𝑘𝑇 をサンプリングするためのアルゴリズム

sample(𝑏)は、原点を中心とする標準偏差𝑏の分布からランダム標本を生成するための関数。

この関数は、例えばのようなアルゴリズで実装される。

注１

表２

注２

注１

注２

乱数の項 𝛾を加えて事後状態に摂動を加えていることに注意。

𝛼1から𝛼6は動作の雑音パラメータである。

第12回 確率システム制御特論

12.2 平面移動ロボットの確率状態方程式

𝑣𝑘−1 = 𝑣𝑘−1 + sample 𝛼1𝑣𝑘−12 + 𝛼2𝜔𝑘−1

2

𝜔𝑘−1 = 𝜔𝑘−1 + sample 𝛼3𝑣𝑘−12 + 𝛼4𝜔𝑘−1

2

𝛾𝑘−1 = sample 𝛼5𝑣𝑘−12 + 𝛼6𝜔𝑘−1

2

𝑥𝑘 = 𝑥𝑘−1 − 𝑣𝑘−1 𝜔𝑘−1

sin 𝜃𝑘−1 + 𝑣𝑘−1 𝜔𝑘−1

sin( 𝜃𝑘−1 + 𝜔𝑘−1∆𝑡)

𝑦𝑘 = 𝑦𝑘−1 + 𝑣𝑘−1 𝜔𝑘−1

cos 𝜃𝑘−1 − 𝑣𝑘−1 𝜔𝑘−1

cos( 𝜃𝑘−1 + 𝜔𝑘−1∆𝑡)

𝜃𝑘 = 𝜃𝑘−1 + 𝜔𝑘−1∆𝑡 + 𝛾∆𝑡

𝒙𝑘 = 𝑥𝑘 , 𝑦𝑘 , 𝜃𝑘𝑇

10

1

2

Algorithm sample_normal_distribution (𝑏2)

return

原点を中心とする正規分布について確率密度を計算するアルゴリズム表２

正規分布からのサンプリングの近似として一般的なアルゴリズム。この近似は中心極限定理を利用したもの。

12個の一様分布からの値を平均することで正規分布に従う値を近似的に生成するが厳密には生成される値は −2𝑏, 2𝑏 内に限定される。

速度動作モデルによるサンプリングの例。標本数はそれぞれ500。パラメータによって動作の摂動は様々に変化する。

第12回 確率システム制御特論

1

2

𝑖=1

12

rand(−𝑏, 𝑏)

11

オドメトリ動作モデル

時間(𝑘 − 1, 𝑘] においてロボットは姿勢 𝒙𝑘−1から姿勢 𝒙𝑘まで進む。

動作情報

𝒙𝑘 = 𝑥𝑘 , 𝑦𝑘 , 𝜃𝑘𝑇

ロボットの動作を，回転 𝛿rot1，直進 𝛿trans，回転 𝛿rot2の三つに変換して考える。

rot1

trans

rot2

𝑘 − 1

𝑘

第12回 確率システム制御特論

12.2 平面移動ロボットの確率状態方程式

オドメトリ情報

𝒙𝑘−1 = 𝑥𝑘−1, 𝑦𝑘−1, 𝜃𝑘−1𝑇

𝒖𝑘−1 = 𝒙𝑘−1 𝒙𝑘

オドメトリ情報に基づいて 𝑝 𝒙𝑘 𝒖𝑘−1, 𝒙𝑘−1からシステムノイズを考慮した𝒙𝑘= 𝑥𝑘, 𝑦𝑘 , 𝜃𝑘

𝑇 をサンプリングするにはどうすればいい？

12

rot1

trans

rot2

①

②

③

𝛿rot1= atan2 𝑦𝑘 − 𝑦𝑘−1, 𝑥𝑘 − 𝑥𝑘−1

第12回 確率システム制御特論

12.2 平面移動ロボットの確率状態方程式

𝛿rot1= 𝛿rot1−휀𝛼1𝛿rot12 +𝛼2𝛿trans2

𝛿trans= 𝛿trans−휀𝛼3𝛿trans2 +𝛼4𝛿rot12 +𝛼4𝛿rot2

2

𝛿rot2= 𝛿rot2−휀𝛼1𝛿rot22 +𝛼2𝛿trans2

𝑥𝑘𝑦𝑘𝜃𝑘

=

𝑥𝑘−1𝑦𝑘−1𝜃𝑘−1

+

𝛿transcos(𝜃𝑘−1+ 𝛿rot1) 𝛿transsin(𝜃𝑘−1+ 𝛿rot1)

𝛿rot1+ 𝛿rot2

𝑘 − 1

𝑘

𝛿trans= 𝑥𝑘 − 𝑥𝑘−12 + 𝑦𝑘 − 𝑦𝑘−1

2

𝛿rot2= 𝜃𝑘 − 𝜃𝑘−1 − 𝛿rot1

13

1

2

3

4

5

6

7

8

9

10

11

Algorithm sample_motion_model_odometry (𝒖𝑘 , 𝒙𝑘−1)

return 𝒙𝑘= 𝑥𝑘 , 𝑦𝑘 , 𝜃𝑘𝑇

表３ オドメトリ情報に基づいて 𝑝 𝒙𝑘 𝒖𝑘 , 𝒙𝑘−1 から

𝒙𝑘= 𝑥𝑘 , 𝑦𝑘 , 𝜃𝑘𝑇 をサンプリングするためのアルゴリズム

𝛿rot1= atan2( 𝑦𝑘 − 𝑦𝑘−1, 𝑥𝑘 − 𝑥𝑘−1)

𝛿trans= 𝑥𝑘 − 𝑥𝑘−12 + 𝑦𝑘 − 𝑦𝑘−1

2

𝛿rot2= 𝜃𝑘 − 𝜃𝑘−1 − 𝛿rot1

𝛿rot1= 𝛿rot1−sample 𝛼1𝛿rot12 + 𝛼2𝛿trans

2

𝛿trans= 𝛿trans−sample 𝛼3𝛿trans2 + 𝛼4𝛿rot1

2 + 𝛼4𝛿rot22

𝛿rot2= 𝛿rot2−sample 𝛼1𝛿rot22 + 𝛼2𝛿trans

2

𝑥𝑘 = 𝑥𝑘−1 + 𝛿transcos(𝜃𝑘−1+ 𝛿rot1)

𝑦𝑘 = 𝑦𝑘−1 + 𝛿transsin(𝜃𝑘−1+ 𝛿rot1)

𝜃𝑘 = 𝜃𝑘−1 + 𝛿rot1+ 𝛿rot2

14

動作と地図

1

2

3

4

5

6

Algorithm sample_motion_model_with_map(𝒖𝑘−1, 𝒙𝑘−1,𝒎)

return

𝑝 𝒙𝑘 𝒖𝑘−1, 𝒙𝑘−1,𝒎 を計算するアルゴリズム表４

do

until

この動作モデルでは、環境の地図𝒎が利用される。このアルゴリズムは、障害物に占有された地図𝒎中の空間にはロボットが存在できないことを反映するために、前述の動作モデルに付け加えられる。

第12回 確率システム制御特論

12.2 平面移動ロボットの確率状態方程式

𝒙𝑘 = sample_motion_model(𝒖𝑘−1, 𝒙𝑘−1)

𝜋 = 𝑝 𝒙𝑘−1 𝒎

𝜋 > 0

𝒙𝑘 , 𝜋

得られた計測データは、与えられた地図中のどの場所・姿勢で得られたのだろうか？

ロボットの知覚

15

環境計測モデルは次の条件付き確率分布で定義される。

時刻𝑘における計測

地図情報

時刻𝑘における姿勢

このモデルは、カメラ、バーコードソナー、レーザレンジファインダなど、任意のセンサに適用できる。

周囲の障害物の認識

地図作成

SICK LMS 北陽電機 URG

問い

第12回 確率システム制御特論

ℎ 𝒚𝑘 𝒙𝑘(𝑙),𝒎

センサ信号センサの多くは、問い合わ

せがあると複数の数値𝑦𝑘𝑗

からなる計測結果 𝒚𝑘を生成して返す。

環境中でロボットが姿勢 𝒙𝑘(𝑙)

と推定され，

環境地図の情報𝒎が与えられている状況で

センサの計測値𝒚𝑘の尤もらしさを

評価することを考える。

知っていること：センサモデル

12.3 ロボットの観測モデル（レーザレンジファインダの場合）

𝒚𝑘 = 𝑦𝑘1, 𝑦𝑘

2, ⋯ , 𝑦𝑘𝑗, ⋯ , 𝑦𝑘

𝐽

ℎ 𝒚𝑘 𝒙𝑘(𝑙),𝒎

𝑦𝑘𝑗

𝒎

𝒙𝑘(𝑙)

第12回 確率システム制御特論

環境の地図は、環境中に存在する物体とそれらの置かれ方のリスト環境地図

𝑛は特徴に対するインデクスを表す。

𝒎𝑛はデカルト座標系における特徴の位置を表す。

𝑛 はある特徴の位置と対応付けるインデクス。例えば世界座標系 𝑥, 𝑦 を直接表現するために

𝒎𝑛を𝒎𝑥,𝑦と表現する。

地図に記述されている物体の場所における環境の形状を記録したもの。センシングによって情報が追加された時に地図を書き換えるので、ロボットによる地図生成によく用いられる。

ボリュームメトリック（容積測量的）な地図であり、環境中の物体に関する情報だけでなく物体のない場所に関する情報も記述される。

位置ベース

特徴ベース

占有格子地図はこれであり、ロボットのナビゲーションなどに向く地図の形式。

第12回 確率システム制御特論

𝒎 = 𝒎1,𝒎2, ⋯ ,𝒎𝑁

求めたい観測モデルは4種類の誤差を考慮した確率分布の混合分布で表される。

正しい計測時の局所的な計測雑音１

２

３

４

想定外の物体による計測誤差

計測失敗による計測誤差

原因不明のランダムな雑音

例：レーザスキャナの観測モデル ℎ 𝒚𝑘 𝒙𝑘(𝑙),𝒎

第12回 確率システム制御特論

12.3 ロボットの観測モデル（レーザレンジファインダの場合）

19

１. 正しい計測時の局所的な計測雑音

距離センサの解像度が有限であることや、計測信号に対する大気の影響

などが原因で発生する測定雑音は、たいてい平均が𝑦𝑘𝑗∗，標準偏差𝜎hit

の狭いガウス分布でモデル化できる。

はセンサ固有の誤差パラメータ

𝑝hit 𝑦𝑘𝑗𝒙𝑘 , 𝒎 = 𝜂𝒩 𝑦𝑘

𝑗; 𝑦𝑘

𝑗∗, 𝜎hit

2 if 0 ≤ 𝑦𝑘𝑗≤ 𝑦max

0 otherwith

𝒩 𝑦𝑘𝑗; 𝑦𝑘

𝑗∗, 𝜎hit

2 =1

2𝜋𝜎hit2

exp −1

2

𝑦𝑘𝑗− 𝑦𝑘

𝑗∗ 2

𝜎hit2

𝜂 = 0

𝑦max

𝒩 𝑦𝑘𝑗; 𝑦𝑘

𝑗∗, 𝜎hit

2 d𝑦𝑘𝑗

−1

第12回 確率システム制御特論

𝑝hit 𝑦𝑘𝑗𝒙𝑘 ,𝒎

𝑦𝑘𝑗∗ 𝑦max

𝜎hit

12.3 ロボットの観測モデル（レーザレンジファインダの場合）

20

２. 想定外の物体による計測誤差

地図に含むべきではない物体をセンサが検知して極端に短い距離を出力する場

合がある。そのような典型例として、ロボット周囲の人間が挙げられる。

𝜆shortはセンサ固有の誤差パラメータ

対処法の一つとして、それらをセンサの誤差要因として扱う方法がある。その場

合、モデル化されていない物体は、距離を𝑦𝑘𝑗∗

よりも短くするという性質を持つ。

第12回 確率システム制御特論

𝑦𝑘𝑗∗ 𝑦max

𝑝short 𝑦𝑘𝑗𝒙𝑘 ,𝒎

𝑝hit 𝑦𝑘𝑗𝒙𝑘 ,𝒎

= 𝜂𝜆shortexp(−𝜆short𝑦𝑘𝑗) if 0 ≤ 𝑦𝑘

𝑗≤ 𝑦𝑘

𝑗∗

0 otherwith

𝜂 =1

1 − 𝑒−𝜆short𝑦𝑘𝑗∗

12.3 ロボットの観測モデル（レーザレンジファインダの場合）

３. 計測失敗による計測誤差

時々、障害物が全く検知されないことがある。レーザレンジファインダでは、黒

くて赤外線を吸収する物体の計測中や、計測対象が日光の中にあるような場合に

計測失敗が生ずる。このような失敗は高頻度で起こる。

𝑝maxが確率密度関数となるように、𝑦𝑘𝑗が𝑦max

計測に失敗した場合に、典型的なセンサは最大距離𝑦maxを出力する。

よりも小さなとても小さな範囲で一様分布となるようにモデル化する。

第12回 確率システム制御特論

𝑦𝑘𝑚∗ 𝑦max

𝑝max 𝑦𝑘𝑗𝒙𝑘 ,𝒎 = 1 if 𝑦𝑘

𝑗≥ 𝑦max − 휀

0 otherwith

𝑝max 𝑦𝑘𝑗𝒙𝑘 ,𝒎

12.3 ロボットの観測モデル（レーザレンジファインダの場合）

22

４. 原因不明のランダムな雑音

距離センサは時折、完全に説明のしようのない計測値を出力することがある

（ビームの乱反射や混線など）。このような計測値は、センサ計測値の区間

全域にわたる一様分布を使ってモデル化できる。

第12回 確率システム制御特論

𝑦𝑘𝑚∗ 𝑦max

𝑝rand 𝑦𝑘𝑗𝒙𝑘 ,𝒎

𝑝rand 𝑦𝑘𝑗𝒙𝑘 ,𝒎 =

1

𝑦maxif 0 ≤ 𝑦𝑘

𝑗≤ 𝑦max

0 otherwith

12.3 ロボットの観測モデル（レーザレンジファインダの場合）

23

５．４種類の計測誤差モデルの混合分布

計測誤差は４種類の分布を重みづけして混合することで得られる。

センサ固有のパラメータΘ = 𝜎hit, 𝜆short, 𝑦hit, 𝑦short, 𝑦max, 𝑦rand𝑇は、

実際の計測データを利用した最尤推定によって求めることができる。

レーザレンジファインダの出力𝑦𝑘の尤度を表す計測誤差確率密度関数表５

第12回 確率システム制御特論

ℎ 𝒚𝑘 𝒙𝑘(𝑙),𝒎

𝑦𝑘𝑚∗ 𝑦max

𝑝 𝑦𝑘𝑗𝒙𝑘 ,𝒎 =

𝑦hit𝑦short𝑦max

𝑦rand

𝑇 𝑝hit 𝑦𝑘𝑗𝒙𝑘 ,𝒎

𝑝short 𝑦𝑘𝑗𝒙𝑘 ,𝒎

𝑝max 𝑦𝑘𝑗𝒙𝑘 ,𝒎

𝑝rand 𝑦𝑘𝑗𝒙𝑘 ,𝒎

𝑦hit + 𝑦short + 𝑦max + 𝑦rand = 1

12.3 ロボットの観測モデル（レーザレンジファインダの場合）

24

1

2

3

4

5

6

7

Algorithm beam_range_finder_model (𝒚𝑘 , 𝒙𝑘 ,𝒎)

Return 𝑞

レーザレンジファインダの出力𝑦𝑘の尤度を計算するアルゴリズム表６

注１

注１

スキャン中の個別の計測値は互いに独立であると仮定する。

注２

注２

雑音がない場合に得られるはずの距離を計算するために、マップ情報を利用したレイキャスティングを適用。

注３注３

表５ の混合分布

第12回 確率システム制御特論

𝑞 = 1

For 𝑗 = 1 to 𝐽 do

compute 𝑦𝑘𝑗∗

for the measurement 𝑦𝑘𝑗

using ray casting

𝑞 = 𝑞 ∙ 𝑝

𝑝 = 𝑦hit ∙ 𝑝hit 𝑦𝑘𝑗𝒙𝑘 ,𝒎 +𝑦short ∙ 𝑝short 𝑦𝑘

𝑗𝒙𝑘 ,𝒎

+𝑦max ∙ 𝑝max 𝑦𝑘𝑗𝒙𝑘 ,𝒎 + 𝑦rand ∙ 𝑝rand 𝑦𝑘

𝑗𝒙𝑘 ,𝒎

12.3 ロボットの観測モデル（レーザレンジファインダの場合）

25

1

2

3

4

5

6

7

8

9

10

11

12

Algorithm MCL (𝒳𝑘−1, 𝒖𝑘−1, 𝒚𝑘 ,𝒎)

return 𝒳𝑘

パーティクルフィルタに基づく位置推定アルゴリズム、モンテカルロ位置推定MCL表７

𝑀個のパーティクルのセット

初期のパーティクルのセット𝒳0

は事前分布𝑝(𝒙0)に従って生成し

各重みを𝑀−1に設定することで

得る。

注１

注２

注１ 注２

注３

注４

注３

注４

表１例えば を利用する。

表６例えば を利用する。

注５

注５ 表８を利用する。

𝒳𝑘 = 𝒙𝑘(1), 𝒙𝑘

(2), ⋯ , 𝒙𝑘

(𝑀)

𝒳𝑘−1 = 𝒳𝑘−1 = ∅

for 𝑚 = 1 to 𝑀 do

𝒙𝑘(𝑚)

= sample_motion_model(𝒖𝑘−1, 𝒙𝑘−1(𝑚)

)

𝑤𝑘(𝑚)

= beam_range_finder_model(𝒚𝑘 , 𝒙𝑘𝑚,𝒎)

𝒳𝑘 = 𝒳𝑘 + 𝒙𝑘𝑚, 𝑤𝑘

(𝑚)

for 𝑚 = 1 to 𝑀 do

end for

end for

draw 𝑖 with probability ∝ 𝑤𝑘(𝑖)

add 𝒙𝑘𝑖

to 𝒳𝑘

26

1

2

3

4

5

6

7

8

9

10

11

12

13

14

Algorithm Low_variance_sampler (𝒳𝑘, 𝒲𝑘)

return 𝒳𝑘

パーティクルフィルタのための等間隔リサンプリングアルゴリズム表８

このアルゴリズムでは、パーティクルの集合𝒳𝑘から重み𝒲𝑘に基づいてサンプリングを行うために、たった一つの乱数しか用いない。

それにもかかわらず、パーティクルがリサンプリングされる確率を、その重みに比例させることができる。さらに、このサンプラは計算効率が良く、𝑀個のパーティクルをサンプリングするために必要な計算量は𝑂(𝑀)である。

注

𝒳𝑘 = ∅

𝑟 = rand(0;𝑀−1)

𝑐 = 𝑤𝑘(1)

𝑖 = 1

for 𝑚 = 1 to 𝑀 do

𝑖 = 𝑖 + 1

while

end while

𝑈 = 𝑟 + (𝑚 − 1) ∙ 𝑀−1

𝑐 = 𝑐 + 𝑤𝑘(𝑖)

add 𝒙𝑘𝑖

to 𝒳𝑘

end for

27

1

2

3

4

5

6

7

8

9

10

11

12

13

14

15

16

17

18

19

20

Algorithm Augmented_MCL (𝒳𝑘−1, 𝒖𝑘−1, 𝒚𝑘 ,𝒎 )

ランダムパーティクルを加える適応的なＭＣＬ表９

slow faststatic ,w w

注

ランダムパーティクルの数が短時間と長時間のセンサ計測値の尤度を比較することで決定されている。

もし短期の尤度が長期の尤度以上であればランダムパーティクルは加えられない。逆に短期の尤度の方が低い場合には、その値に比例した数のランダムパーティクルが加えられる。

増えすぎたパーティクルの削減方法は別に考える必要がある。

注１

注１

𝛼slow, 𝛼longは長期、短期の平均を推定するための減衰率。

𝒳𝑘 = 𝒳𝑘 = ∅

𝒙𝑘(𝑚)

= sample_motion_model(𝒖𝑘−1, 𝒙𝑘−1(𝑚)

)

𝑤𝑘(𝑚)

= beam_range_finder_model(𝒚𝑘, 𝒙𝑘𝑚,𝒎)

𝒳𝑘 = 𝒳𝑘 + 𝒙𝑘𝑚, 𝑤𝑘

(𝑚)

𝑤𝑎𝑣𝑒 = 𝑤𝑎𝑣𝑒 + 1/𝑀 ∙ 𝑤𝑘(𝑚)

draw 𝑖 with probability ∝ 𝑤𝑘(𝑖)

add 𝒙𝑘𝑖

to 𝒳𝑘

return 𝒳𝑘

add random pose to 𝒳𝑘

else

end with

end for

end for

for 𝑚 = 1 to 𝑀 do

for 𝑚 = 1 to 𝑀 do

𝑤slow = 𝑤slow + 𝛼slow(𝑤avg +𝑤slow)

𝑤fast = 𝑤fast + 𝛼fast(𝑤avg −𝑤fast)

with probability max 0.0, 0.1 − 𝑤fast/𝑤slow do

0 < 𝛼slow, 𝛼long

1

2

3

4

5

6

7

8

9

10

Algorithm motion_model_velocity (𝒙𝑘 , 𝒖𝑘−1, 𝒙𝑘−1)

return

表＊ 速度情報に基づいて𝑝 𝒙𝑘 𝒙𝑘−1, 𝒖𝑘−1 を計算するアルゴリズム

注１

atan2( , )

atan( / ) if 0

sign(y)( -atan(| / |)) if 0

0 if 0

sign( ) / 2 if 0, 0

y x

y x x

y x x

x y

y x y

注２

1 ( ) , ( )T T

t tx y v x u

( ' ' ')T

t x y x

から

をサンプリング

するには を利用する。表２

2prob(a, b ) は、原点を中心とする標準偏差ｂの分布において、引数ａの確率密度関数を計算する関数。

注３

表３

注２

注３

注１𝜇 =

1

2

𝑥𝑘−1 − 𝑥𝑘 cos 𝜃 − 𝑦𝑘−1 − 𝑦𝑘 sin 𝜃

𝑦𝑘−1 − 𝑦𝑘 cos 𝜃 − 𝑥𝑘−1 − 𝑥𝑘 sin 𝜃

𝑥∗ =𝑥𝑘−1 + 𝑥𝑘

2+ 𝜇(𝑦𝑘−1 − 𝑦𝑘)

𝑦∗ =𝑦𝑘−1 + 𝑦𝑘

2+ 𝜇(𝑥𝑘−1 − 𝑥𝑘)

𝑟∗ = (𝑥𝑘 − 𝑥∗)2+(𝑦𝑘 − 𝑦∗)2

∆𝜃 = atan2 𝑦𝑘 − 𝑦∗, 𝑥𝑘 − 𝑥∗ − atan2 𝑦𝑘−1 − 𝑦∗, 𝑥𝑘−1 − 𝑥∗

𝑣 =∆𝜃

∆𝑡𝑟∗

𝜔 =∆𝜃

∆𝑡

𝛾 =𝜃𝑘 − 𝜃𝑘−1

∆𝑡− 𝜔

prob(𝑣𝑘 − 𝑣, 𝛼1𝑣2 + 𝛼2𝜔

2) ∙ prob(𝜔𝑘 − 𝜔, 𝛼3𝑣2 + 𝛼4𝜔

2)∙ prob( 𝛾, 𝛼5𝑣

2 + 𝛼6𝜔2)

29

1 初期の不確かさは姿勢空間全体で一様にランダムに選ばれたパーティクルで表現される。

2 ロボットがドアを検知したとき、各パーティクルの重みを変更する。ここではパーティクルの位置は変化していないことに注意。

3

x

x

x

x

( | , )p z x m

w

w

w

次の時刻の様子。ロボットが移動している。リサンプリング後のパーティクルは、ロボットがいそうな三か所で増加している。

12.4 ロボットの状態推定

第12回 確率システム制御特論

30

4

5

x

x

w

w

( | , )p y x m

x

新たな計測値がロボットに与えられると、各パーティクルの重みが不均一になる。この時点で、ほとんどの確率が二番目のドアに集中していることが分かる。この場所が、ロボットが最もいそうな場所である。

さらにロボットが移動すると、もう一度リサンプリングが行われ、新たなパーティクルのセットが事後信念を近似する。

第12回 確率システム制御特論

12.4 ロボットの状態推定

31/36

材料・状態 𝒙𝑘 → 位置，角度…

・観測値 𝒚𝑘 → センサ信号…

・運動モデル 𝑓 𝒙𝑘 𝒙𝑘−1, 𝒖𝑘−1, 𝝃𝑘−1 →ロボットの動作から解析的に導出

・システム雑音 𝝃𝑘−1 → ガウス分布，コーシー分布，t分布…

・初期分布 𝑝(𝒙0) → ガウス分布，一様分布…

・観測モデルℎ 𝒚𝑘 𝒙𝑘(𝑙)

→ センサの特性に応じて実験的に決定

・制御入力 𝒖𝑘 → 並進速度，回転角速度…

・地図情報 𝒎 → トポロジカルマップ，グリッドマップ…

第12回 確率システム制御特論

12.4 ロボットの状態推定

32

ロボットの動作モデルの例

1

33

ロボットの動作モデルの例

2

34

3

35

4

36

5

37

6

38

7-1

39

7-2