パーティクルフィルタを用いた全方向移動ロボットの自己位置同定

における効率的な探索法

Efficient Exploration for Localization of Omnidirectional Mobile Robotsusing Particle Filter

○非山下遼，学項警宇，正田崎勇一，

正稲垣伸吉，正鈴木達也（名古屋大）

Ryo YAMASHITA,r yamashita@nuem.nagoya-u.ac.jp,Jingyu XIANG, Yuichi TAZAKI, Shinkichi INAGAKI and Tatsuya SUZUKI,Nagoya University

This paper presents an efficient exploration strategy for the self-localization of omnidirectionalmobile robots based on particle filtering.It is a challenging task to identify the robot’s global position ina workspace based only on local sensory information,especially if there exist multiple locations wheresimilar measurement signals are obtained.A popular existing technique called Active Localization, whichchooses the next action so as to minimize the entropy of the expected posterior distribution of the robot’sposition, cannot be directly applied to particle-based localization, since entropy cannot be evaluated easilywhen a probability distribution is expressed by particles.Instead, in the proposed method, the robot movesto a position where the variance of the expected measurement signal is maximized.The variance of aparticle distribution is easy to compute and therefore the proposed method is well suited to particle-basedlocalization.The proposed technique is tested in an in-door environment.

Key Words: Localization, Laser Range Finder, Particle Filter

1. は じ め に

近年，福祉，オフィスなどにおける自律型ロボットへの期待が高まっている．ロボットが自律移動をするためには自己位置同定，地図生成，経路計画などさまざまな要素が必要であるが，何らかのタスクを実行するには環境中の自己位置を把握する必要があり，自己位置同定は特に重要であると考えられる．　初期位置が未知であり，作業環境におけるロボット自身の大域的位置をセンサ情報に基づき特定する大域的自己位置同定において，ロボットは移動と観測を繰り返しながら自己位置同定を行うこととなる．　ロボットの移動法としては，ランダムに移動する，または決まった経路の移動などが広くとられているが，自己位置と周囲環境が非常に似通った場所が存在した場合，これらの手法では，自己位置を精度よく同定できるまでに非常に多くの移動回数を要すると考えられる．　効率的な移動法について Active Localization[1]という手法が存在する．この手法では，ベイズ推定に基づく自己位置同定の枠組みにおいて，仮想的な制御を加えた時の自己位置の事後分布を予測し，エントロピーが最も大きく減少する場所へ移動する．　一方，現在はパーティクルフィルタを自己位置同定に用いた手法が広く用いられている．この手法は実装が容易であり有用であると考えられる．しかし，パーティクルフィルタは粒子の数で確率分布を表現するので，エントロピーを直接計算することが困難であり，Active Localizationは必ずしも適しているとは言えない．　よって本論文では，環境地図は既知であり，初期位置，姿勢は未知である大域的位置同定問題においてロボットが地図中の数か所で自己位置を特定できない場合，それぞれ

Omni-directional camera

Personal computer

DC motor ×3

Omni-wheel ×3

(L×W×H)=(420×400×630)

Laser range finder ×2

Fig. 1 The robot used for testing: Omnia

の場所にいたと仮定した時の移動後に得られるであろう観測値を予測し，その情報を元にした効率的な移動法を提案し，実験においてその有用性を検証する．　観測には距離センサである Laser Range Finder（以下LRF）を用いる．これによりランドマークを用いることなく自己位置同定を行える．

2. 問 題 設 定

環境は大域的には異なるが局所的に似通った場所が存在するものを想定し，移動ロボットは 3輪駆動式の全方向移動ロボット (図.1)を使用する．前後の 2か所に LRFを実装しており，２つのデータを合わせることで全方位の観測結果を得ることができる．

xxxt ,yyyt ,uuut はそれぞれ時刻 tにおける状態，観測信号，制御入力を表すとする.状態は 2次元作業空間におけるロボットの位置 x,yと向き θ の 3次元ベクトルであり，観測信号

Fig. 2 An observation by laser range finder

は図 2のように LRFで観測する方向数分のベクトルであるので，周囲M方向分の障害物までの距離を観測する場合，

xxx= [x,y,θ ]T ,yyy= [y1,y2, ..,yM]T (1)

となる．制御 uuuはロボットの移動量を表す．移動モデル fはシステムノイズ vvvt を用いて

xxxt+1 = f (xxxt ,uuut ,vvvt)

= xxxt +uuut +vvvt (2)

と表され，観測モデル hは観測ノイズ εt を用いて

yyyt = h(xxxt ,εt) (3)

と表される.

3. Monte Carlo Localizaton

近年，ベイズ推定に基づく自己位置同定手法の研究が盛んに行われている[2]．ベイズ推定による自己位置同定は一般的に，移動モデルによる状態予測

p(xxxt+1|uuut) =∫

p(xxxt+1|xxxt ,uuut)p(xxxt)dxxxt (4)

と，観測モデルによる更新（η :正規化定数)

p(xxxt+1) = η p(yyyt |xxxt+1)p(xxxt+1|uuut) (5)

により成り立つ．その中でも自己位置同定にパーティクルフィルタ[3]を用いる手法はMonte Carlo Localization[4]と呼ばれる．パーティクルフィルタとは状態の確率分布を粒子（パーティクル）の集団で表現する手法である.前述のように実装が容易でほとんどどのような分布であっても表現できるといった点で優れている．パーティクルフィルタは以下のアルゴリズムに従って，状態推定と粒子の更新を繰り返していく．また，その概念図を図 3に示す．状態ベクトル xxxt ,観測ベクトル yyyt ,外乱 vvvt

とする．STEP1：粒子数 Nとして，i = 1, · · · ,Nについて粒子の集団の初期状態 XXX0 = {xxx(1)0 , · · ·xxx(N)

0 }を生成する．STEP2：粒子の分布が所定の条件（後述）を満たすまで以下の 1～4を実行する．vvvt には疑似乱数を用いる．

1. 状態予測モデル f，制御 ut を決定する（後述）.

xxx(i)t = f (xxx(i)t−1,vvvt ,uuut)

を用いて次の状態 xxx(i)t を推定する．

Fig. 3 A schematic

description

of particle filter

Fig. 4 example of environment

2. 観測によって得た yyyt を用い，

w(i)t = p(yyyt |xxx

(i)t )

によって粒子 i の尤度 w(i)t を計算する．

3. Wt = ∑Ni=1w(i)

t を計算する．4. 全ての粒子の中で i 番目の粒子が占める尤度

w(i)t =

w(i)t

Wt

を計算し，時刻 t における粒子の集団 XXXt ={x(1)t , · · · x(N)

t } の中から x(i)t を w(i)t の確率でリサ

ンプリングを行う．このとき抽出された x(i)t から，時刻 t における新たな粒子の集団 XXXt = {x(1)t , · · ·x(N)

t }として生成する．

各粒子の状態で観測した時に得られるであろう LRFの観測値を計算し，実際の観測値と比較し尤度を決定する．この際に LRFの観測のノイズに正規分布を仮定して計算する．例えば，図 2で，円マークをロボットとし，各線はその方向に得られた観測を表す．ロボットは地図上向きとして，右から反時計回りに yyy = [y1,..,y8]

T のような観測が得られたとする．i番目の粒子の状態での LRFの観測値の予測が y j ( j=1,2,…8)であるとすると，i 番目の粒子の尤度w(i)

t は分散 σ2として，

w(i)t = Π8

j=1exp(−(y j − y j)

2

2∗σ2 ) (6)

となる．これを正規化して最終的な尤度とする．

4. 観測量の期待分散最大化に基づく探索法の提案

前述のように，自己位置が一意に特定できないということは真の位置と同じような観測結果が得られる場所が地図中に存在するということが大きな原因の一つである．これらの場所を区別するためにはそれぞれの場所で異なる観測値が得られるような方向に移動すればよいと考えられる．地図は既知であり，移動する前に移動先での観測値を予測することができることを利用し，そのような移動を実現することを考える．

4·1 基本的な考え方 図 4を用いて本研究の基本的な考えについて説明する．図の aの位置にロボットが存在するとする．灰色部は LRFで観測できる範囲であるとする．この範囲では aは図の bの位置と周囲環境が非常に似ているため，この時点ではロボットは自己位置を一意に特

定することができない．よって，自己位置同定するためには bと区別がつくところまで移動し観測を行う必要がある．今，aから 1,2の方向のどちらに動く方が自己位置を一意に同定しやすいかを考える．今，1の方向に動いたとする，すると移動後の観測は aと bでまたしても同じような結果が得られてしまう．そうなると，自己位置を絞り込むことはできない．次に，2の方向に移動した場合を考える．この場合，移動後に bでは障害物の斜線部が観測されるが，aでは観測されない．よって，aと bの区別ができるので自己位置を一意に特定できる可能性が高いと考えられる．このように，地図が既知であり自己位置が数か所で区別できなかった場合には自己位置を特定しやすいような，つまりそれらの場所で周囲環境が異なるような場所に移動させればよいというのが提案手法の基本的な考えである．観測値の異なりを分散という形で定量化する．以下に扱う問題の定式化を図る．ロボットの状態が xt であり，制御入力 uuut が決定した時，移動後の観測量は式 (2),(3)より

yyyt+1|t(xxxt ,uuut) = h( f (xxxt ,uuut)) (7)

，xxxt の確率分布を用いて期待値を計算すると，

E[yyyt+1|t |uuut ] =∫

yyyt+1|t(xxxt ,uuut)p(xxxt)dxxxt (8)

と書け，その分散は

var[yyyt+1|t |uuut ] =∫

||yyyt+1|t(xxxt ,uuut)−E[yyyt+1|t |uuut ]||2p(xxxt)dxxxt(9)

である．提案手法では分散が最大となる場所に移動するので，

finduuu∗t = argmax(var[yyyt+1|t |uuut ]) (10)

と表せる．

4·2 k-meansクラスタリングによる自己位置候補地の抽出 真の位置と数か所で自己位置の判断がつかない時，ぞれぞれの場所を自己位置候補地と呼ぶこととする．前述の通り粒子の集団は確率分布を表しており，すべての粒子を自己位置候補地と考えることもできる．しかし，大域的位置同定問題においては粒子数が膨大であるため，計算量も膨大になる．また，自己位置同定の際には粒子がいくつかの塊を形成することが多いので，クラスタリングを利用し自己位置候補地の抽出を行うことが有効であると考えられる．これにより高々数か所自己位置候補地について考えることができ，大幅な計算量低減につながる．クラスタリングには非階層的クラスタリングの 1種である k-means法[5]を用いる．以下にその概略を示す．

1. K個のクラスタの代表点（重心）ccc j をランダムに振り分ける

2. 粒子 xxxi について各クラスタの代表点 ccc j との距離∥xi −ccc j∥2を計算し，最も小さい値をとったクラスタに割り当てる

3. 2をすべての粒子について行い，各クラスタごとに重心を計算して新たな代表点とし 2へ戻る

4. 代表点の変動がなくなったら終了する確率分布にこの k-means法を適用し，それぞれのクラスタの代表点を自己位置候補地とする．k-means法を使用する際には以下の点を考慮しなければならない．

Fig. 5 Generation of a set of candidate destinations

1. 初期値依存性2. 初期クラスタ数を決定する必要がある前者には乱数を元にした異なる初期値でクラスタリング

を複数回行い，すべての粒子と粒子の所属するクラスタの代表点との距離の総和を評価関数として，クラスタ j に所属する粒子の集合をCj とすると，

K

∑j

∑xxxi∈Cj

∥xxxi −ccc j∥2 (11)

を最小とするものを採用する．後者にはクラスタ数を充分と考えられる数よりも多く設定しておき，クラスタリングの結果 2つの代表点同士の距離がある閾値よりも小さかった場合，それらのクラスタを統合するという手法をとる．

4·3 移動候補地の生成 ロボットが移動できる場所はほぼ無限に存在するが，移動先を決定する際にそれぞれの移動先での比較検討を行うためには移動個所を有限個に絞り込むことが必要である．図 5に候補地生成の流れを示す．観測結果からロボット本体の半径を引いた空間を生成する．そして，観測可能な 1024方向について，候補地とする．移動により幅を持たせるために，移動距離もある程度持たせたいと考え，移動距離も可変とする．厳密には，観測値を等分して，候補地を生成できるようにし，本論文では距離は 10分割とする．つまり，ある方向のＬＲＦ観測値が 4000mmであったとすると，400mmごとに候補点を生成する．この手順で LRFが観測できる 1024方向,距離も 10分割するので，計 10240個の候補地について移動先を検討する.

4·4 期待分散最大化 クラスタリングにより，上記の式 (9)を以下のように近似的に解くことができる．yyyccci をi 番目のクラスタの代表点の位置，姿勢において予想される観測とすると

var[yyyt+1|t |uuut ]≈K

∑i∥yyyccci

t+1|t(ccci ,uuut)−K

∑j(yyy

ccc j

t+1|t(ccc j ,uuut)/K)∥2(12)

となり，これを解くこととなる．以上より，uuut を決定する方法は

1. LRFの観測により障害物に衝突しない移動先の候補地をいくつか作成する.

2. 各候補地へ移動した後の各クラスタの代表点にいたと仮定した時の LRF観測値を予測する．この際 LRFのノイズは無視する．

3. クラスタ間の LRF予測値の分散を計算し，分散が最大となる候補地へ移動する．

という方法をとる．

Fig. 6 An experimentation environment

Fig. 7 first resampling Fig. 8 second resampling

Table 1 Experimental resultsHHHHHH

提案手法 ランダムな移動

平均移動回数 1.75 4.50

平均時間 (s) 48.6 63.4

平均評価値 0.495 0.118

5. 検 証 実 験

5·1 実験環境 以下のような環境で検証実験を行った．提案手法と，従来手法の 1つであるランダムな移動を用いた方法と比較実験を行った．図の灰色部は障害物を表しており，高さによって形状の変化はないものとし，クラスタ数が 1になることを自己位置同定の終了条件とする．

5·2 評価関数 提案手法を組み込みランダムな移動と比較する際の評価として，自己位置同定できるまでの時刻 t,移動回数,制御前と制御後に真の状態付近に集まっている粒子の数について評価する．粒子の数については，クラスタリングによりロボットがいる集団に属する時刻 t における粒子の数 Nt とすると，評価関数 f は自然対数を用いて

f = log(Nt+1

Nt) (13)

とする．評価関数は 1回の移動によりどれだけ自己位置の不確定性を減少させられるかを表す．

5·3 実験結果 上図は提案手法での実験結果の 1例であり，図 7,図 8はそれぞれ初期位置でのリサンプリング後の分布，1回移動後の位置でのリサンプリング後の分布である．この例では図のように，初期位置で観測しただけ

では数か所に粒子が分かれているが，1回目の移動後に観測を行った結果，すべての粒子がロボットの位置周辺に集まっている．表 1にランダムな移動と提案手法との実験結果の比較

を示す．ランダムな移動は乱数を元に移動場所を選択する手法をとった．移動回数，時間，評価関数値について同じ初期位置から 4回の試行を行い，その平均値を計算した移動回数は，実験開始から自己位置が同定される（クラスタリングの結果クラスタ数が 1になる）までの移動回数を表し，時間は，自己位置同定を完了するまでの平均所要時間を表す．表 1を見ても分かる通り，平均試行回数，自己位置同定

までの平均時間，評価関数の値のすべてにおいて提案手法の方がランダムな移動より優れた結果を示した．評価値が 0.495という値は，式 (13)より，ロボットの真

の位置が範囲に入っているクラスタの時刻 tでの粒子数 Nt

とすると，Nt+1 = 1.640Nt となり，制御前に比べて，制御後は 1.640倍の数の粒子が真の位置周辺に集まることが期待できるということである．これは自己位置同定の度合いによる（自己位置がほぼ同定できているときは制御後に粒子の移動が少ない）ことも考えると（実際に提案手法の 1回目の移動後と 2回目の移動後の分布の変化は小さく評価関数の値も小さくなった）制御後にかなりの粒子が真の位置周辺に集まることが期待できると考えられる．

6. お わ り に

本研究では，全方向移動ロボットによる LRF，パーティクルフィルタを用いた自己位置同定を行い，地図が既知であることを利用し効率的な探索法を提案し，移動回数，時間，評価関数共にランダムな移動よりも効率的な結果を得ることができた．　今後の課題としてより広くかつ複雑で生活空間に適した場所への適応が挙げられる．

文 献

[1] D.Fox, W.Burgard; Active Markov Localization forMobile Robot, Elsevier Preprint,1998.

[2] D.Fox,J.Hightower, L.Liao,D.Schulz, Gaetano Bor-riello; Bayesian Filters for Location Estimation, IEEEPervasive Computing, 2003.

[3] 樋口　和之; 粒子フィルタ, 電子情報通信学会誌　Vol.88 No.12, pp.493-500, 2005.

[4] D.Fox, W.Burgard, F.Dellaert,S.Thrun; Monte CarloLocalization: Efficient Position Estimation forMobileRobots, the Sixteenth National Conference on ArtificialIntelligence, 1999.

[5] K.Wagsta,C.Cardie,S.Rogers,S.Schroedl; ConstrainedK-means Clustering with Background Knowledge,Proceedings of the Eighteenth International Conferenceon Machine Learning, pp. 577-584, 2001.