Never Stop Questioning

パーティクルフィルタ

最終更新：2011年10月01日 23:31

t-style

- view

メンバー限定登録/ログイン

パーティクルフィルタ(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)重み計算
　各パーティクルの測域センサの情報と現在の測域センサの情報を比較し、パーティクルの重みを計算する。ここで重みは、たとえば、２つのセンサの各方向における距離の差の絶対値を累積した値の逆数などで与えられる（実際にはこの重みの定義は工夫する必要がある）。
(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にしないとだんだん青と赤の三角形がずれていく。