尤度を共有する複数のパーティクルフィルタを用いたシステム制御
齋藤

1

碧(指導教員：小林一郎)

はじめに

3

システムの制御は，運動方程式を用いた様々な手法においてその有効性が示されている．しかし，環境には外乱が含まれており，非線形の状態を線形近似した運動方程式では制御できない場合もある．パーティク
ルフィルタは制御対象の状態を確率分布で近似して表現するため, 近年，システム制御などにも応用され，注目を集めている [2][3]．

(k)

パーティクルフィルタは, 観測できない状態ベクトル xt を観測可能な観測ベクトル yt から推定する手法である. 状態 xt と観測値 yt は, それぞれ以下に示す
システムモデル (1), 観測モデル (2) によって得られる.

yt= Ft xt−1 + Gt vt(1)= Ht xt + wt

(2)

vt はシステムノイズ, wt は観測ノイズを表す. Ft ，Gt ，Ht はそれぞれ変数に対応する係数を表す行列である．また, K 個の重み付けされた粒子の集合 Xt =(k)

(k)

(k) K

step1 初期設定：ランダムな K 個の xt−1 , zt−1 を生成する
step2 予測：式 (1) に従い，それぞれノイズを乗せる
step3 ソート：x̃t , z̃t をそれぞれ昇順にソートする
step4 観測値取得：観測可能な x を取得，z は観測値なし(k)
step5 尤度計算：重み p(xt |x̂t−1 )(1≤k≤K) を計算
step6 尤度共有：粒子のインデックスに重みを依存した形で ẑ にも x̂ と同じ尤度を与える
step7 リサンプリング：πt に比例した確率で xt , zt をK 個抽出する
step8 時間更新：t→t + 1 として step2〜step7 を繰返す

パーティクルフィルタ

xt(k)粒子の集合を Xt = {x̂t , ẑt , πt }k=1 として x とz が依存関係にある場合，x のみの観測から z の値を推定する手法を提案する．提案手法のアルゴリズムを示す．

本研究では, 振り子の倒立安定化問題において，パーティクルフィルタを用い，外乱に対して頑健かつ運動方程式を用いない制御手法を提案する．

2

尤度共有による状態推定

パーティクルフィルタは，観測可能な値に対する追従を行う．しかし，実時間内に複雑な制御対象の複数の情報を取得，近似計算をするには制御速度の問題がある．本研究では，適用するパーティクルフィルタのK{(xt , πt )}k=1 で状態 xt を表す. ここで πt (k) は個々のパーティクルの重みである. 一般的なパーティクルフィルタのアリゴリズムを以下に示す.
step1 初期設定：ランダムな K 個の xt−1 を生成する
step2 予測：式 (1) に従い Ft xt−1 + Gt vt → xt として xt を予測値とする
step3 尤度計算：xt と yt を比較し重み πt を計算する
step4 リサンプリング：πt に比例した確率で xt を K個抽出する
step5 時間更新：t→t + 1 として step2〜step4 を繰返す

図 2: 尤度を共有する２連のパーティクルフィルタ

4

実験：倒立振子の安定化制御

本研究では，倒立振子を制御対象とし，地面に障害物が存在する場合に，運動方程式による制御と提案手法である運動方程式を用いずに振り子の傾きと台車にかける力をパーティクルフィルタで推定する制御とを比較する．実験では文献 [2] を参考に, 倒立振子の外観(図 3) や運動方程式を設定した. 長さ l = 0.5(m), 質量m = 0.3(kg) の回転軸を振り子とみなし, 台車の質量mc = 3.0(kg) とする. 振り子の角度を ϕ(rad) とし, その角速度を ϕ̇(rad/s), 角加速度を ϕ̈(rad/s2 ) とする. また, 台車の移動距離を p(m), その速度を ṗ(m/s), 加速度を p̈(m/s2 ) とする. 重力加速度 g は 9.8(m/s2 ) とした. 倒立振子の運動方程式を満たすため使用シミュレー
タに物理演算エンジン PhysX[4] を用いた. 外乱を表現するために倒立振子の初期位置付近に一辺 5.0(mm)の立方体を 22 個設置し，反発係数を 0.0, 動摩擦係数・静止摩擦係数を 0.5 とした.

図 1: パーティクルフィルタ
パーティクルフィルタは, 図 1 に示すように，予測,尤度計算，リサンプリングを繰返して時間更新を行う.

57

4.3

実験結果および考察

運動方程式を用いた制御
図 4 と図 5 は，それぞれ，運動方程式を用いた際の倒立振子の振り子の角度 ϕ，振り子の重心の支点からの距離を示した. 運動方程式を用いた制御では，初期においては制御できていることが観察されるが，時間が経過するにつれて外乱による誤差が蓄積されてしまい，最終的に振り子が倒れてしまっている．
図 3: 倒立振子外観

4.1

運動方程式を用いた制御

倒立振子の運動方程式は以下の式で定義される [2].
sin ϕgsin ϕ + cos ϕ −f −mlϕmc +m2ϕ̈=p̈ =2ϕl( 43 − mcosmc +m )f + ml(ϕ2 sin ϕ − ϕ̈cos ϕ)
mc + m(3)図 4: 角度 ϕ(4)尤度共有による状態推定を用いた制御

ここで振り子が倒立状態 (ϕ = 0, 2π, ϕ̇ = 0) 付近の方程式を近似する.

f1 (ϕ)=−0.806314ϕ2 + 5.06622ϕ

式 (9) において r = 5.0 として実験と行った．図 6は，濃い太線が実際に観測された ϕ の値，薄い細線はパーティクルフィルタによる ϕ の推定値を表した．振り子の位置を示す図 7 により外乱が発生している場合でも左右に振り子が揺れていて制御可能であることが伺える．図 8 は，ϕ̂ と尤度を共有して得られた fˆ の値である．これにより，実際に観測されない値も提案手法を用いることにより推定できることが示された．

(5)

f1 の運動方程式は，式 (5) の様に ϕ の２次方程式で近似される [2]．よって, 制御対象を ϕ とし, 式 (5) よりϕ を f に変換することで台車に加える力とした. また,
ϕ = 0 付近において式 (5) を振り子の位置と角速度により, 以下に示す４つに場合分けをして制御した.
−f1 (ϕ)if (p > 0, ϕ̇ > 0)
 f (−ϕ)if (p > 0, ϕ̇ < 0)
1f (ϕ) =(6)f(ϕ)
if (p ≤ 0, ϕ̇ > 0)1 −f (−ϕ)
if (p ≤ 0, ϕ̇ < 0)1

図 6: 角度 ϕ4.2

尤度共有による状態推定を用いた制御

5倒立振子が外乱に影響される場合，制御量 f は，振り子の直立状態からの角度 ϕ に依存する．本研究では 角度 ϕ と台車にかける力 f を推定するパーティク
ルをそれぞれ 500 個用意する．パーティクルの集合を(k)(k)(k) K
Xt = {ϕ̂t , fˆt , πt }k=1 と設定し，観測可能な ϕ から fˆ を導く．推定するにあたり，ϕ̂ と，fˆ の初期値の(k)0k=1

図 7: 振り子位置

図 8: 推定値 fˆ

まとめと今後の課題

シミュレータ上でパーティクルフィルタの倒立振子安定化制御への性能を評価した. 解析的に近似困難な環境外乱に対し，パーティクルフィルタの状態推定が有効であり，提案手法では，尤度を共有したパーティクルフィルタにおいて，直接観測値が得られない場合でも，観測可能な値の重みを用いることにより推定が可能となることを示した．今後の課題は, 安定性の検討, 初期位置設定やノイズの加え方によるパーティクルフィルタの推定精度の改善が挙げられる.

範囲を経験的に以下の様に設定する．

0.0 ≤ {ϕ̂0 }K
k=1 ≤ 0.5
(k)K0.0 ≤ {fˆ }≤ 10.0

図 5: 振り子位置(7)
(8)

参考文献

このように得られた推定値 fˆ を f ∝ fˆ として出力する．振り子が傾く方向により２つに場合分けをする．
{rfˆif (p < 0.0)f=(9)ˆ−rf if (p ≥ 0.0)

[1] 樋口知之，粒子フィルタ，電子情報通信学会誌，
Vol.88，No.12，pp.989-994，2005．
[2] D.Stahl,J.Hauth,“PF-MPC:Particle FilterModel Predictive Control”,Berichte des Fraunhofer
ITWM,Nr.201(2011)．

上記，ϕ̂，fˆ の初期値と r は経験的に求められた値である．

[3] 西田健, パーティクルフィルタとロボット制御, 第３回横幹連合総合シンポジウム 2010．
[4] http://www.nvidia.com/58