計測自動制御学会東北支部 第 346 回研究集会 (2023.12.20)
資料番号 346-16

粒子フィルタにおける粒子の位置情報を用いた移動ロボットの誘導制御
Guidance Control of Mobile Robots by Localilzation of Particles
in Particle Filter○篠崎佑介 ∗ ，村松鋭一 ∗○ Yusuke Shinozaki∗ , Eiichi Muramatsu∗*山形大学*Yamagata University
キーワード : 粒子フィルタ (particle ﬁlter), 状態推定 (state estimation), 移動ロボット (mobile robot)，誘導制御 (guidance control)

連絡先 : 〒 992-8510 米沢市城南町 4–3–16 山形大学大学院理工学研究科機械システム工学専攻
村松鋭一，Tel.: (0238)26-3327，Fax.: (0238)26-3327，E-mail: muramatu@yz.yamagata-u.ac.jp

1.

研究の背景と目的

2.

移動ロボットの運動方程式動的システムの状態推定法として粒子フィル

xy 平面上を移動するロボットの位置と姿勢をタの研究が進んでいる．移動ロボットに粒子フィ状態とし，(x, y, θ) と表す．その時間微分 (ẋ, ẏ, θ̇)Tルタを適用するとき，その目的は自己位置推定は，前方方向の速度 ν と 角速度 ω を用いて  ẋν cos θ  (1) ẏ  =  ν sin θ であり，多数の粒子の分布はロボットの位置と姿勢に関する確率分布を近似するものとなる．各粒子はロボットの状態に関する尤度を持っており，それはロボットの位置と姿勢に関する情報を信頼度とともに数値化したものとなっている．
本研究では粒子フィルタの粒子をロボットの自己位置推定のみだけでなく，障害物回避に用いる方法を提案する．各粒子の尤度を利用し，障害物に対する危険度を算出し，それに応じたベθ̇ωで記述される．これを刻み時間 ∆t で離散化した方程式は，ω = 0 のとき，  xt
xt−1
νt cos θt−1  
 yt  =  yt−1  +  νt sin θt−1  ∆t
θt
θt−1
ωt(2)

クトルを用いてロボットを誘導する．

–1–ω ̸= 0 のとき，
 

xt
xt−1
 
 yt  =  yt−1 θtθt−1νt ωr−1 {sin(θt−1 + ωt ∆t) − sin θt−1 }+  νt ωr−1 {− cos(θt−1 + ωt ∆t) + cos θt−1 } ωt ∆tといった関数で表せる. ランドマーク mj を特化した関数 hj を考えると,

zj = hj (x)と表せる.

(3)と表される．

3.

(8)

4.

パーティクルフィルタ

4.1

パーティクルフィルタについて

本研究では, ロボットの自己位置推定のためにパーティカルフィルタ 3) を適応する. この手

ランドマーク法では, ロボットの姿勢が示すパラメータ (位置本研究での環境では, ロボットがマップに点(x, y), 方向 θ), による空間に、確率的な重みを在しているランドマークを認識することにより,持たせた点を数多く分布させる. そして, その空位置や角度を把握し, パーティクルに生かすこ間内の任意の場所にロボットが存在する確率をとができる.

点ランドマークの座標は,mj =その領域内に存在するパーティクルの重みの合)T , ロボットの姿勢 x = (x, y, θ)T で計で近似表現する. そのパーティクルの重みは,あり, ロボットの姿勢から,zj = (lj , ϕj )T を求めセンサ情報が入力されると, ベイズの定理によていく. lj は, ロボットとランドマークの距離でり, パーティクルの重みが変更される. この手法あるため,は,Monte Carlo localization (MCL) と呼ばれて(mj,x , mj,yいるものである.

mj = |mj − x|2√= (mj,x − x)2 + (mj,y − y)2

4.2

状態推移モデル(4)
本研究における雑音は, ４つの標準偏差をパとなる.ϕj は, ２つの線分のなす角になるため,ラメータとして用いており,σνν = 0.19,σνω =0.00,σων = 0.13,σωω = 0.20 とし, それぞれ, 直

ϕj + θ

線で生じる道のりのばらつきの標準偏差, 回転= atan2(mj,x − y, mj,x − x)で生じる道のりのばらつきの標準偏差, 直線で

ϕj = atan2(mj,y − y, mj,x − x) − θ

生じる向きのばらつきの標準偏差, 回転で生じとなる. 上記式をまとめると,) ( √()lj(mj,x − x)2 + (mj,y − y)2=
ϕjatan2(mj,y − y, mj,x − x) − θ(5)zj = h(x, mj )(6)
( √)(mj,x − x)2 + (mj,y − y)2h(x, mj ) =atan2(mj,y − y, mj,x − x) − θ(7)

る向きのばらつきの標準偏差となっている. これにより, 雑音を考慮した速度, 角速度は,
() ()
ν′
ν
=ω′ω
)
(√√δνν { |ν|∆t + δνω { |ω|∆t)}√√+δων { |ν|∆t + δωω { |ω|∆t)}–2–(9)

と表される. この際, パーティクルの分布は, 信と定義し,lj (x) は hj (x) の距離の成分,lj (x)σl , σϕ念分布に近似していると言え,はそれぞれ lj ,ϕj に期待されるばらつきの標準偏∫P (x∗t ∈ X) = x∈X bt (x)dx∑ −1(i)≈ N1 Ni=0 δ(xt ∈ X)差である. また lj ,ϕj の雑音はお互いに独立と仮定する.lj (x)σl , σϕ の値はどのランドマークでも共通であるとする. 尤度関数は, ガウス分布を使(10)用し,となる.

4.3

Lj (x|zj ) = N [z = zj |hj (x), Qj (x)](16)と定義される. 本研究では,lj (x)σl , σϕ の値は σl =センサ値の反映

0.14,σϕ = 0.05 とする.

パーティクルは, ロボットがランドマークを観測したときのセンサ値,zj = (lj ϕj )T , パーティク

4.5

ルの姿勢 x(i) ,x(k) によって尤もらしさを求め,Lj (x|z) = ηpj (z|x)

リサンプリングとは, 重みの大きいパーティク(11)となる.Lj は尤度関数であり,η は正規化定数である.

MCL では, それぞれのパーティクルに重みと呼ばれる変数 wt をもたせる.MCL のパーティク
ルは姿勢と重みの２つの変数の組で,(i)(i)(i)
ξt = (xt , wt )(i)

wt = 1

ルに偏ってしまった現象を重みがあまりに小さすぎるパーティクルを消して, 重みが大きいパーティクルを分割することで, 重みが集中している領域のパーティクルの数を多く保つ処理のことである. 本研究では, 系統リサンプリングを採用している. 系統リサンプリング 3) は, パーティク
ルの重みを積み上げ, その先頭から重みの累積値(12)が r のところに目安をつける.(r 〜 U(0,W/N’))
そして, 以下の処理を N 回繰り返す. 目安をつけとされ, 全パーティクルの合計は,
N−1∑

リサンプリングていたリストの要素を選び, その要素となった

(13)

i=0

パーティクルを選び, 新しいリストに追加 (重みは 1/N’) し,r に W/N’ を足す. これを採用こと
で, 増えやすい計算量を比較的抑えめにし, サンとなり, センサ値に重みを反映させると,プリングバイアスを考慮することができる.
(i)
wt

(i)
= Lj (x(i)
t |zj,t )ŵt(14)(i)となる.ŵt はセンサ値を反映する前のパーティクルの重みである.

4.4

5.

ロボット制御5.1

ロボット制御について

本研究では, 離散時間において (2),(3) で示し

尤度関数

た通りに動作を行っており, 動作を加えなけれ姿勢 x において, ランドマーク mj を観測した

ば, 雑音を考慮して速度と角速度の設定通りに時のセンサ値 zj = (lj ϕj )T のばらつきが,2 次元ほぼ直進し続ける. つまり, 制御しなければ, 障ガウス分布に従うとして, ばらつきの共分散行列を x の関数として Qj (x) と表すと,()[lj (x)σl ]2 0
Qj (x) =0
σϕ2

害物があった際, 回避することができない. そこで, 本研究では, パーティクルそれぞれに対し, 危(15)険度 Dj を算出させ, その値をもとに回避する方法を提案する.

–3–

5.2

危険度について危険度 D を算出する方法は, リサンプリング
前の尤度をすべて足し合わせて正規化し, その数値と障害物とパーティクルの距離 sj の逆数を取り, それを足し合わせることにより算出される.(16) から,j∑Lj (x|zj ) = w(17)Lj (x|zj )1+w
sj(18)i=0

Dj =と算出される.

Fig. 15.3

回避前の移動ロボットと粒子の様子

危険度による回避制御

危険度 Dj と障害物に対して一定の距離になったときに, 回避を行うようにする. その際, 危険度 Dj によって回避するベクトル方向を決定する. 例えば,L1 (x|z1 ) = 0.10,s1 = 2.0 であるとき,(18) から, D1 = 0.60 であると算出される. 現在のロボットの進行方向を 0 °とし,Dj が 1.0 の時に 180 °パーティクルの角度を変更すると,D1 =0.60 であるため, この計算上では,s1 <= 2.0 の時においてロボットの進行方向から 108 °変化する. この変化したパーティクルが存在した際,パーティクルとロボットのベクトル計算を行い,ロボットの進行方向は, 以前の状態から,54 °傾いた状態で回避運動を行う.

Fig. 2–4–

危険度による変化の様子

Fig. 5
Fig. 3

回避後の移動ロボットと粒子の様子

回避後以上から, 危険度 Dj を採用した回避運動を行うことができた.

6.

シミュレーション

下図は,5.3 での回避運動を採用した移動ロボッ

7.

まとめ

トとパーティクルの様子である. なお, 図中上に

本研究では粒子フィルタの粒子をロボットの描かれている (x, y, θ) は, 移動ロボットの座標と自己位置推定のみだけでなく, 障害物回避に用角度であり, 図中下に描かれている (x, y, θ) は,いる方法を提案した. そして, 各粒子の尤度を利最大尤度のパーティクルの座標と角度である.

用し, 障害物に対する危険度を算出し, それに応じたベクトルを用いてロボットを障害物から回避を行うことができた.

参考文献
1) 足立，丸田：カルマンフィルタの基礎，東京電機大学出版局 (2012)2) 片山徹：非線形カルマンフィルタ，朝倉書店 (2011)3) 上田隆一：詳解確率ロボティクス，講談社(2019)
Fig. 4回避前4) 上田隆一、新井民夫、浅沼和載、上田和昇、
大隈久：パーティクルフィルタを利用した自己位置推定に生じる致命的な推定誤りからの回復法、日本ロボット学会誌 Vol.23 No.4

pp466 473, 2005–5–5) 小林亮、星野智史：移動ロボットのための環境変化にロバストなパーティクルフィルタに基づく自己位置推定の観測モデル、日本ロボット学会誌 Vol.41 No.1, pp.92 101,2023

–6–