パーティクルフィルタ


※上記の広告は60日以上更新のないWIKIに表示されています。更新することで広告が下部へ移動します。

パーティクルフィルタ(Particle Filter)

動的システムの状態推定などに使われる。マルコフ連鎖モンテカルロ法の逐次版であって、逐次モンテカルロ(Sequential Monte Carlo)法とも呼ばれる。過去の状態に依存する提案分布から得られたサンプル(これをパーティクル(粒子)と呼ぶ)に対して尤度を計算し、その尤度に基づいて現在の状態を推定する。提案分布の現在の状態に対する確率が0でなく、パーティクル数が無限大ならば真値に収束する。なお、提案分布の作り方、推定値を計算方法は設計者次第である。

ロボットでの応用

ロボットでは、自己位置推定と画像処理のコンテキストで用いられる。直感的で実装もしやすいので人気である(実際、つくばチャレンジ2010の参加チームの多くがパーティクルフィルタを利用していた)。

推定の手順(自己位置推定を例にして)

ここでは具体的にシンプルなロボットの自己位置推定を例に推定の手順を述べる。

ロボットの状態と動作

左右の動輪と補助輪から成るシンプルな移動ロボットを考える。この場合、ロボットの(位置に関する)状態Xは、「X座標の位置(x)」、「Y座標の位置(y)」、「ロボットの向き(\theta)」によって表現できる。一方、ロボットの動作は、向いている方向へまっすぐ進む「前進」と、向いている方向を変える「右回転」、「左回転」の動作で表現できる。

ロボットの状態方程式

誤差がない場合、状態遷移は各行動に応じて次の方程式で表せる。
  • 「前進」の場合
 x_t = f_x(x_{t-1}) = x_{t-1} + \Delta d * cos(\theta_{t-1})
 y_t = f_y(y_{t-1}) = y_{t-1} + \Delta d * sin(\theta_{t-1})
 \theta_t = f_\theta(\theta_{t-1}) = \theta_{t-1}
 ここで、\Delta dは移動距離。

  • 「右回転」「左回転」の場合
 x_t = f_x(x_{t-1}) = x_{t-1}
 y_t = f_y(y_{t-1}) = y_{t-1}
 \theta_t = f_\theta(\theta_{t-1}) = \theta_{t-1} + \Delta\theta
 ここで、\Delta\thetaは回転量。

問題設定

自己位置推定の目的は、LRFのデータを使って現在の状態X_tを過去の状態X_{t-1}から推定することである。そのためにロボットは、自己位置を推定するために測域センサ(Laser Range Finder : LRF)を有しており、さらに、ロボットは正確な地図を持っていて、任意の場所のLRFのデータが得られるとする。

提案分布

提案分布の一つの作り方は、状態方程式で予測される現在位置を中心とした正規分布を仮定することである。すなわち、パーティクルの状態は次のようになる。ここで、NormalRand()は正規分布からサンプリングされた値を表し、sigma_?はその分散である。

 x_p = NormalRand(f_x(x_{t-1}), \sigma_x)
 y_p = NormalRand(f_y(y_{t-1}), \sigma_y)
 \theta_p = NormalRand(f_\theta(\theta_{t-1}), \sigma_\theta)

推定の方針

測域センサの情報から自己位置が一か所に確定できるとすると、ロボットの測域センサの情報を利用して、パーティクルの尤もらしさを得ることができる。つまり、本来の現在位置の確率p(X_t)に対する条件付き確率(=尤度)p(X|\Theta)の定数倍の情報を得ることができると見なせる。そこで、この尤度の(定数倍の)情報を使ったSample Importance Resampling(SIR)を行い真の分布を推定する。

推定

(1)サンプリング
 提案分布に従いN個のパーティクルをサンプリングする。
(2)重み計算
 各パーティクルの測域センサの情報と現在の測域センサの情報を比較し、パーティクルの重みを計算する。ここで重みは、たとえば、2つのセンサの各方向における距離の差の絶対値を累積した値の逆数などで与えられる(実際にはこの重みの定義は工夫する必要がある)。
(3)リサンプリング
 重みに基づいて、N個のパーティクルからM個のパーティクルをリサンプリングする(つまり、重みを使ったルーレット選択をする)。
(4)リサンプリングされたM個のパーティクルの平均値を現在位置の推定値とする。

補足

なお、正統派?のパーティクルフィルタは全部もしくは重みの大きい複数のパーティクルを残しておいて、パーティクルに対して状態遷移を行ってから、重みに応じてパーティクルを生成しなおす(状態遷移は後でも良い)。しかし、本稿の例では、平均で与えられる代表のパーティクルだけを残して、そこから次のパーティクルを生成ししている。これは、ロボットが複数の位置には存在しないこと、尤度は真の位置で最大となることを仮定しているためである。また、状態推定において、重みつき平均を用いるのではなく、リサンプリングしたパーティクルの平均としているのも、あまり一般的ではないのかもしれない(いろいろ考えると、上記をパーティクルフィルタと言ってよいかわからなくなってくる)。

実装例

言語

C#(Visual Studio 2010 Express)

問題設定

上述のロボットの自己位置推定。

ソース

きれいじゃないのでなし(ソフトはアップロード)。
きれいじゃないけどアップロード。
https://bitbucket.org/t_style/machinelearning/src/23be211008bc/ParticleFilter

ソフトの使い方

概略

前記説明をソフトウェアにしたもの。青い三角形が推定位置。赤い三角形が真の位置。緑の小さな三角形がパーティクル。右上の小さなウィンドウは真の位置から計測した測域センサの情報を使って復元した部分地図。自己位置推定ON(OFF)というボタンを押すとパーティクルフィルタによる自己位置推定のON/OFFを切り替えられる。初めはOFFになっている。OFFの場合は、前記の状態方程式にのみ従って推定される。この世界のロボットはノイズと右上方向への歪みを持っているので、ONにしないとだんだん青と赤の三角形がずれていく。


実行方法

1.ParticleFilter.exe/dllとmap.bmpをダウンロード
2. ParticleFilter.exe/dllとmap.bmpを同じフォルダに置く
3. ParticleFilter.exeを実行

操作方法

  • 自己位置推定ON(OFF)ボタン
 パーティクルフィルタによる自己位置推定のON/OFFを切り替える。
  • 上下カーソルキー
 ロボットが向いている方向に前進もしくは後退する。その後、自己位置推定を行う。
  • 左右カーソルキー
 ロボットが左もしくは右に回転する。その後、自己位置推定を行う。
  • Uキー
 強制的に自己位置推定を行う。

補足

サンプリング数は100、リサンプリング数も100。地図は200x200。測域センサは50ピクセル先までを測定。それ以降はすべて50を返却。なので、長い直線に入ると自己位置推定がブレるのがよくわかります。


|新しいページ|検索|ページ一覧|RSS|@ウィキご利用ガイド | 管理者にお問合せ
|ログイン|


統計・機械学習


計算機


プログラミング



Raspberry Pi


その他



工事中


関係ない話題


管理者用



プロフィール

但馬慶行
(TAJIMA Yoshiyuki)

機械学習とロボットを趣味で勉強中。
連絡先はトップページ