 パーティクルフィルタを用いた

カメラとレーザレンジファインダによる複数移動体の同時追跡 
山田 弘幸 £倉爪 亮 £ 村上 剛司 £ 長谷川 勉 £                        £ 










£





£





£

  

                   !"#$"% &'                     !"#$"% &'                      
                                                                  !                                              "    "              ( )  *  +     ,  +  *   
は

じ

め

に

る "%$ ．そこで本研究では，これまで開発した  !

ロボットの活動の場が工場内から屋外環境へ移るにつれて，ロボットが処理すべき情報量は飛躍的に増加する．この膨大な情報処理を，センサによる情報獲得

ンダからの距離情報を統合し，各センサ特有のノイズに対する頑健性の高い移動体追跡システムを構築する．
移動体追跡ではカルマンフィルタやパーティクルフィ

から認識，判断，動作計画，そして実行まで全てロボッ
ト単体で行わせるのは，ロボットの可搬計算機の処理能力から非現実的である．そこで，ロボットの代わりに環境側があらかじめ情報を取得，管理することで，ロボットは自らセンシングしなくとも必要な情報を環境側から得られるようになり，他の必要な処理に計算機資源を費やすことができる．本研究はこの考えに立ち，環境に固定したセンサによってロボットに必要な環境情報の一つである人間などの移動体の位置を追跡!



による複数移動体の追跡システムを開発してきた "#$．しかし近年，ビデオカメラに比べて測定精度や分解能に優れたレーザレンジファインダがよく用いられ，高い精度で追跡できることが報告されてい£原稿受付  年  月  日
九州大学システム情報科学府 〒  福岡県福岡市
西区元岡  番地£¾ 九州大学システム情報科学研究院 〒  福岡県福岡市西区元岡  番地     £½

ルタなどの時系列フィルタを適用することでノイズや隠れに対して頑健になることが知られている．そこで
本論文では，モデルに対する少ない事前知識で異種センサ情報を統合できるパーティクルフィルタを採用する．しかし，パーティクルフィルタには複数対象を追跡する場合に一部の対象にだけ粒子が集中し，粒子が全く集まらない対象は追跡を見逃してしまうという問題&! '()がある．この問題を解決する

ため，% 種類のパーティクルフィルタ（ するシステムを構築する．
これまで我々は，ビデオカメラを用いた と，環境に置かれたレーザレンジファイ

'!
&) +,&*

パーティクルフィルタ， +)++

パーティクルフィルタ "-$）

（付録参照）を結合し，複数移動体を頑健に同時追跡する手法を提案する．

 関

連

研

究

レーザレンジファインダを用いた移動体追跡は，環境に固定したシステム "% ./0$ と，移動ロボットに搭載したシステム "1/#-$ に分類できる．
環境に固定したレーザレンジファインダによる歩行

Environment
Sensor node

t=0s

Hub

Sensor nodet=0.77s

Sensor nodeSensor node

CameraLaser range finder

t=1.82s23 #



23 %

 !

者の追跡には，中村らによる一連の研究 "% .$ がある．
彼らはレーザレンジファインダ & ! 
5

%44)

を最大

台用いて足首位置を計測し，歩行動作の解析から作成された複数状態遷移を有するカルマンフィルタにより足断面上の点をクラスタリングすることで，通常時（#.1 人），通勤ラッシュ時 046の追跡成功率を#446

達成している "%$．またこの手法はカメラ画像と統合され，レーザ計測による追跡に失敗した時には
,7



法によるカメラ画像上での追跡に切り替えることで，ロバストな歩行者追跡を実現している ".$．この他にも，8!!'! 9 を用いた手法 ":$，腰位置をカルマンフィルタおよびパーティクルフィルタで追跡したもの "5$，レーザで膝上を追跡し，カメラの肌色抽出と組み合わせて身長や位置をカルマンフィルタで推定する手法 ";$，あるいはレーザレンジファイン
ダとカメラを用いた応接システム "0$ などが提案されている．
一方，移動体にレーザレンジファインダを搭載し，環境モデル構築や障害物回避，追跡を行ったものとして

パーティクルフィルタを用いた歩行者の追跡 "#4 ##$，
全方位カメラとレーザレンジファインダによる歩行者の追跡 "#-$ など多数のシステムが提案されている．
このようにレーザレンジファインダを用いた移動体のトラッキングは多くの研究例があるが，本論文のようにカメラ画像上での移動体トラッキングとレーザによるトラッキングを密に融合した例は少ない．

   !

とは，背景差分等によって切り出された追跡対象領域に対して，能動輪郭モデルの１つである 



,

を適用し，ノイズや重なりに頑強に領域を追跡する手法である．図 # に複数人体を追跡している様子を示す．
8,

， , ら "#. #:$ によって提案された 

!   '  

,

は，  など従来の能動輪郭モデルの欠点であった複数領域の重なりに対して，本質的に適応可能であるという優れた特徴を有する．しかし一方で新たに導入された補助関数値を決定するための計算量が膨大であり，リアルタイム処理には不向きであると考えられてきた．これに対し我々はこれまでに，





,

,&2

の高速な実装方法である 2 )を開発し，画像上での実時間トラッキング "#$ や３次元モーションキャプチャシステム "#5$などを提案してきた．
図 # の例は，2 !

を % 次元画像に適用した の例である．画像のサイズ，入力速度がそれぞれ -%4<%.4'<，-4=，使用した計算機が> ? %9=の時，2の処理は約 54= で実行されている．この例では，当初 % つの閉曲面で表されていた移動物体の境界が，移動物体が交差したこと
で # つの閉曲面に統合され，次の時刻で再度 % つの閉曲面に分離しており，境界の位相変化，すなわち対象の重なりに柔軟に対応できていることがわかる．

 移動体追跡システム

は，車椅子搭載レンジファインダによる障害物回避 "1$，

Sensor nodeSensor nodeSensor unit
Computer

Host computer

t=1.60s

Sensor node

上述した  !

により，% 次元画像上では移動体領域をノイズや重なりに頑健に追跡できる．
しかし，% 次元画像から移動体の - 次元位置を正確に推定したり，重なった移動体領域を正確に切り分ける
ことは，単一のカメラでは困難である．これに対し，本システムで用いる平面スキャン型レーザレンジファ
インダは，% 次元平面上での対象物体までの正確な距離情報が容易に得られる．そこで，上述した 
!



と環境におかれた複数のレーザレンジファイ

ンダからのレンジデータをパーティクルフィルタの枠
組みで統合することで，交差時の位置関係を正確に推定しつつ頑健に対象追跡を行うシステムを構築する．
 

システム構成図 % に移動体追跡システムの構成を示す．カメラとレーザレンジファインダの組
を # つのセンサユニットとし，これが # 台のコンピューMoving object

である．しかし対象の出現，消失時に特別な処理が必要となり，また対象が一度接近してから離れる場合にImage plane

複数のパーティクルフィルタが同じ対象に集まり，他
Contour
Focal point

f

θl
θr
23 -の対象を見逃してしまうなどの問題がある．一方，提案する % つのパーティクルフィルタを用いる手法は，

ul ur

パーティクルフィルタそのものの性能向上を目指したものであり，対象出現のたびにパーティクルフィルタを生成するなどの特別な処理は不要である．以下，移<  7 

動体追跡の流れをセンサ端末，ホストコンピュータにタに接続されてセンサ端末を構成する．環境中に分散配置されたセンサ端末は，# つのホストコンピュータ
へ接続され，そこで各端末からの情報が統合される．
端末・ホスト間の通信は , を介して行われる．
移動体追跡処理

分けて述べる．
  

センサ端末での流れ

まず初期状態として，各センサ端末で移動体が環境中に存在しない静的環境の情報を取得する．このとき，センサ端末 で得本システムでは，上述しられる静的環境情報は，カメラから得られた静的環境

による移動体の追跡結果と異種

画像   ，レーザレンジファインダから，ミラー回

複数のレーザレンジファインダからのレンジデータを，

転軸を原点とし，放射状にあるステップ角度で離散的空間解像度の異なる % 種類のパーティクルフィルタをに得られるレンジデータ   用いて確率的に統合する．一般的なパーティクルフィだし，各センサ端末は世界座標系での自身のセンサ位ルタである

置・姿勢情報を保持しており， は世界座標系でのレ た  !



パーティクルフィルタを用いる場合，追跡対象が一つの時は真値（すなわち移動体の真の位置等）への収束性能は高く精度の良い位置推定が可能であるが，複数対象の追跡では，十分多数の粒子を用いないと一部の対象に粒子が偏り，他の追跡対象を     である．た

ンジデータである．
静的環境情報を取得後，移動体検出を行う．移動体検出は次の % つの方法で行われる．


時刻  におけるカメラ画像    を取得し，見逃すことがある．これは，全対象へ粒子が一様に収  

束せず，いくつかの対象にだけ粒子が集中しやすいとして，いう

パーティクルフィルタの性質による．一方，出する．領域の左端，右端の  座標  ， から，++

パーティクルフィルタ "-$ は基本的に一様なラカメラ焦点での方位角である移動体の存在範囲

ンダムサンプリングであり，真値への収束性能は低く，

との差分から切り出された対象領域に対

 



 !

により移動体領域を抽を求める（図 -）．ただしカメラ  軸

追跡空間の解像度が高くかつ対象が高速に移動する場とレーザレンジファインダのミラー回転軸（垂直合には追跡に失敗する可能性がある．しかし一方で，方向）は同方向であるとする．また，複数の領域

変化が急激でなければ，全対象へ一様に粒子が収束すを追跡している場合を考慮し，複数の移動体存在るという性質を持つ．そこで，提案するシステムでは，
これらの % 種類のパーティクルフィルタを空間解像度を変えて同時に用い，低解像度++範囲の集合を

パーティクルの初期粒子（ 

追跡精度と収束性，安定性を兼ね備えた複数移動体追跡システムを実現する．ただし両パーティクルフィル
タとも，観測すべき移動体の数は未知であり，また処
理時間の短縮のために，各粒子の保持する状態変数は#つの移動体の状態（位置，速度）のみとしている．
パーティクルフィルタを複数対象へ適応させる手法の一つとして，< >! 2 "#;$がある．この手法は，対象毎にパーティクルフィルタを適用し，複数のパーティクルフィルタが平行して動作するもの

    とする．

た点のみを抽出した @ を計算する．

パーティクルフィルタ

パーティクル）を生成することで，

 

ンジデータ  を取得し， から一定距離  離れ

フィルタにより大まかに得られた追跡対象候補位置の近傍に，重点的に高解像度

時刻  におけるレーザレンジファインダからのレ

@  




         A   # 

&#)

上記の % つのタスクは各センサ端末で独立に繰り返
し実行され，ホストコンピュータからの要求に対してはその時点で最新の情報を提供する．
  

ホストコンピュータでの流れ

ホストコン

ピュータでは，センサ端末から提供される情報を統合し，移動体の位置推定を行う．以下，その詳細を示す．
#3

各センサ端末 へ情報提供の要求コマンドを送信
し，  ，@ を取得する．次に各センサ端末からの情報を統合し，全センサ端末情報の集合@ @ 


を求める．







，



%3

High resolution SIR particle filter，@ を % 種類のパーティクルフィルタ（  パーティクルフィルタ，++パーティクルフィルタ）により統合し，移動体の位置を推定する．
 

種類のパーティクルフィルタによる移動体位

置推定

本節では，異種複数センサから得られた情報を % 種類のパーティクルフィルタにより統合し，移動体の位置推定を行う手法について述べる．時刻  での移動体の位置が値

である事象をとすると，観測23 .

，  を得た時の移動体の位置に関する事後確率
@


@




は，状態と重みを持つ粒子群によって再帰的に推定される．ただし
@





Low resolution MCMC particle filter，@ はそれぞれ観測値



53

，



















個の初期粒子









を 発 生 さ せ る ．た だ し ， は位置ベクトル，

は速度ベクトル，





は重

みを表す．
%3



個の粒子に対して動作モデル

 を適用し，粒子を遷移させる．動作モ



デルには，次式を用いる．










 



尤度計算：
@











@


@






&-)







 









&.)





 

 %



%



&:)



とした．ここで， は位置誤差， は      である，また 




は







&5)

への方位角がのい4

重み計算： 個の粒子に対して，重み にて計算する．






@




また，全ての粒子の重みの和 





を次式
&;)

 



 




リサンプリング：  個 4    # の粒子 




 



を



に

の確率で復元抽出し，ランダ

個の粒子は，後述する

+

+

で得られ

た移動体の推定位置周辺にランダムに発生させる
図 .)．

&

のみである．よって，以下に &:) リサンプリング処理
リサンプリング：粒子 

における  ，

に対して，正規分布に従うランダムな変化量を加え，次状態候補 



¼

，

¼



及び次状態候補での尤度 を計算する．次に 

¼







に対して

¼



'D

を適用し新たな粒子   を決定する．これを一定回数繰り返すことによって粒子を収束さ
せる．

 実

験

によるジャンプエッジ近傍のノイズ除去 "#0$ を確認している．本章では，シミュレーション及び実機実験になお，使用した空間解像度は，  パーティクルフィ
+

+

パー

ティクルフィルタは対象空間を一辺 %44 ずつ格子状に区切った領域とした．使用したコンピュータはセンサ端末に >
+% %3.9=ムノイズを加えた新たな粒子   を発生させる．
#  

パーティクルフィ

ルタは連続値 &( 型浮動小数点），求める．
対して，



より提案手法の有効性検証実験を行った結果を示す．

を，存在しない場合には #   を与える関

数である．

:3

パーティクルフィルタがこれまで，カメラとレーザレンジファインダの統合  

ずれかの範囲内に存在する場合は定数値 .3+ルタと異なる点は，解像度と &:) リサンプリング処理と @

との最小ユークリッド距離，#

低 解 像 度  パ ー ティク ル フィル タ  

ただし，



パーティクルフィルタによって得られる推定位

置を，本システムでは最終的な移動体推定位置とする．

:3

  を次式にて計算する．


を推



定する．

&%)

個の粒子それぞれに対して，尤度@をクラスタリングし，クラスタ毎に粒子の重

のみ記す．



ただし， はサンプリング周期である．
-3+ '! C

み付き平均をとることで，移動体位置+

粒 子 遷 移：

高解像度  パーティクルフィルタ

初 期 粒 子 発 生：+

移動体位置推定：粒子群により近似的に得られる


フィルタによる移動体位置推定の手順について述べる．
#3

B

確率分布から，移動体位置の推定を行う．粒子群が得られる事象である．以下，% 種類のパーティクル
  



 

，ホストコンピュータに-349=

である．

シミュレーション実験

まず，複数の物体が同時に移動，交差する場合に対して，従来のィクルフィルタ，複数対象の同時追跡が可能なパーティクルフィルタ "#;$，提案する



B

+

パーテ
<
+パー

ティクルフィルタを用いてシミュレーション実験を行
った．

4000
Y coordinates [mm]Targets(a) SIR particle filter (particle 900)(b) Mixture particle filter (particle 300x3)

Sensor node 1

2000
Particle filters

0Sensor node 3-2000(c) SIR/MCMC particle filter (particle 300)23 :

Sensor node 4

Total station

Sensor node 202000
X coordinates [mm]23 54000!!! 

 

ンサ端末 &
実験は，- つの移動体がそれぞれ異なる初期位置か，E%<.

%44<.)

によって最大 ##人の歩行者を同時追跡した．ただし粒子数は

パー

ら同一の点を目指して移動し，交差，分離する場合にティクルフィルタが %444，

対して行った．粒子数は，  パーティクルフィルタ

ルタが #444 である．図 ; に追跡結果の軌跡の一部を，が 144，

パーティクルフィルタが -44 が - つ

図 0 に実験中の歩行者の様子 &左) と追跡結果 &右) をパーティクルフィルタが示す．全ての歩行者を測定範囲内において出現から消<

で計 144，  B
，

-44

+

+

+

+

に -44 の計 544 である．なお，に<

パーティクルフィルタのみ各移動体の初期位置は既知とし，各パーティクルフィルタの初期粒子をそれぞれの移動体に割り当てている．  パーティクルフィル
タ，  B++

パーティクルフィルタは，移動体の移

動開始位置は全く未知とした．

失まで１組の



B

+

+

+

+

パーティクルフィ

パーティクルフィルタで追跡できることが確認できた．
 

接近分離への対応-

分離する場合に対して， <

つの対象が接近後にパーティクルフィルタ，

パーティクルフィルタ，  B

+

パーティ

クルフィルタの追跡性能を比較した．粒子数は， 

実験結果を図 : に示す．通常の

パーティクル

パーティクルフィルタが 5444，

<

パーティクル

フィルタでは追跡開始直後から # つの対象に全ての粒フィルタが %444 が - つで 5444，  B+

子が集中した．またクルフィルタが+<

+パーティクルフィルタは，



粒子 %444 と

+

+

パーティ

粒子 #444 で追跡開始直後はそれぞれを分離して追跡できている

ある．

が，交差した時点でそれぞれの粒子が重なり合い，分ション実験と同じように各パーティクルフィルタの初離後は # つの対象のみを追跡している．一方提案した期粒子を手動で移動体に割り当てている．

<

パーティクルフィルタでは，シミュレー

パーティクルフィルタは，最も粒子数が少

図 1 に実験結果を示す．  パーティクルフィルタ

ないにも関わらず，移動開始後，交差時，分離後とも

では，最初に現れた移動体にほとんどの粒子が集中にそれぞれを分離して追跡できていることがわかる．

し，- つ目の移動体には粒子がほとんど集まらず，追



B

+

 

+

移動体追跡精度の検証

提案手法による追跡精度を検証するため，実際に ;

:

社製 

パーティクルフィルタでは，移動体同士が接近するまでは精度よく追跡できて，いるが，移動体同士が離れた時に片方の対象に複数の，北陽電機社製  94.F<%）をパーティクルフィルタが集まってしまい，その後の追%44<%

配置し，追跡実験を行った．粒子数はルフィルタが %444，#444

<

E%<.

間に . つのセンサ端末（> 9 社製
!

跡に失敗している．

の対象空



パーティク

跡を失敗している．提案手法である



B

+

+

パー

パーティクルフィルタがティクルフィルタでは，対象を見失うことなく追跡でである．実験では一つの移動体に対して提案手

き，かつ対象の出現，接近後の分離に対応しているこ++法により位置計測を行い，同時にトプコン社製トータ
ルステーション 9 D0%: により移動体上のプリズムを自動追尾して，位置を計測した．図 5 にトータルス
テーション及び提案手法による追跡結果を示す．両者の差は平均 051，標準偏差 :1; であった．
複数移動体同時追跡性能の検証とがわかる．

 ま と め
これまでに開発した  !

と，環境におかれた複数のレーザレンジファインダからの距離情

次に提案

報を，空間解像度の異なる % 種類のパーティクルフィ

手法における複数移動体の同時追跡実験を行った．実

ルタを用いて統合することで，複数の移動体の位置関験では，対象空間 #.

係を正確に推定しつつ，ノイズや重なりに頑健に追跡 #4

に配置した . つのセ[mm]Images3000

SIR

MixtureSIR/MCMC

0-5000

Tracking trajectories
Sensor nodes-5000
23 ;

05000 [mm]>, 7 ## 

miss

87.7s
miss

89.3s

23 191.0s

<'


B+

7

<

+ '! Cパーティクル) の分布から推定する手法である．

&

92.7s

    &  # はターゲットの数) を同時確

 に従う確率変数とする．  を時刻率分布 

4  

までの観測結果， を時刻  での観測結果とすると，

94.3sそれぞれの観測が独立の場合，ベイズの定理より，


 



    が成り立つ．ただし 96.0s

観測モデル，



 











23 0!  7 ##  (



B

+

+

'! C 跡システムの有効性をシミュレーションと実システム

 を運動モデ














と表す．これを式 &


 



    











#)

に代入すると，

















金基盤 G「ロボット群と環境固定センサ群の相互支援による人間・ロボット共生環境の情報構造化 &#0-54#%.)」
および「５感センサネットワークによる被災建造物内システムの開発 &#1-54##1)」の助成を受けて行われました．

付録  パーティクルフィルタ
パーティクルフィルタとは，対象の状態をセンサによる観測値に対する事後確率分布として，多数の粒子



を





&







を用いた実験により確認した．
謝 辞 本研究の一部は，文部科学省科研費補助

は

用いて，


を行うシステムを提案した．また，提案した移動体追 は事前確率である．ここで状態推

 と前時刻での事後確率 #) は事後確率，  

移にマルコフ性を仮定し，事前確率 
ル

&

&

%)



-)

が得られる．
 パーティクルフィルタ

パーティクルフィルタである)

&

最も一般的な* '!

パーティクルフィルタの実装手順を示す．

まず時刻   # での事後確率 



 



を個

のパーティクルで表現する．ただし，各パーティクル
は自身の状態
ウエイト   #  とその生起確率を表すを有する．これにより，式 &

-)

の積分

は，各パーティクルの生起確率の和として以下のように書かれる．



 



    



















&

.)



'

'

ルタでは，



に基づく通常のパーティクルフィ









子から，その生起確率 として得られる  個の粒を生成する．また同時に，新たな生起確率 



¼

 

¼

 

   







  



 +,

分布 





を
&



 





:)

を推定する．

 +) に従うサンプル

法である．





パーティクルフィルタ 

+

+ &

¼

 



と求め，これを繰り返すことで 
 

¼

 

に応じて新たな粒子

#'D







とは，ある確率 を生成する方

を用いた

+

+

の手

順を示す．
#3

まず適当な推移行列  を決める．

%3

初期サンプル-3 ¼

 を適当に決める．
 に従ってを遷移させ，候補

¼ を計

算する．
.3

確率  で¼ と遷移する．ただし



¼




¼






¼





&

5)

&

;)

+

+

である．特に  が対称の場合には， は



¼









とより簡単に計算できる．
  



パーティクルフィルタ

パーティクルフィルタでは，通常のパーティクルフィ
ルタにおける '
'D

'

を用いたの代わりに，上述の+わち，各粒子の持つ生起確率 +を用いる．すなとは無関係に全ての粒子を一様にサンプリングし，それぞれの粒子 に対して上述の'D

を適用して新たな粒子を生成する．

文献-!. /0 1 0 2 )0 )3 20 4  0 5 )0 54( 6          73   8 9 & ,&   ''0 ::;::% -:##$.0-:. 0 <</< 50 =5<> /0 5 ?<< 0 <<>)> )0 >5 <  0 =<@<(6) '   '    4      7    9 & 
 A B    ''0 !!$;!!%: -:##%.0-$. =0   )0 ?   0 B ( 67 ' +     7  7 
9  )   , <       !! ''0 ! #%;!"! -:##%.0-. 0 <</< 50 =5<>  /0 5 ?<< (6) '     
 9 !! '        ''0 !;! # -:##%.0-%. 0 ,  &0  1  <0  ( 6) ' '    4     9  50 0A 
  , 0 A ' C     A   AC "" ''0 !D:;!" -!""".0-D. <0   <0 5 4  0 &0 ( 6< 
7 ' '  9 , 0   A   / 7   <   :##: ''0 $#:;$#:" -:##:.0-. <0 ?   0 @ ( 6) ' '  4
4      9 , 0 < 
A    / 7   <   :##$ -:##$.0- . 0 ,0   4  /0  ( 6   '      + 9 , 0 5/ #D-:##D.0-". ?0   A0    0 , ( 6   7  '    73 4   
+9 , 0   A    / 7  <   :##! ''0 !D $;!D -:##!.0-!#. B0   1 @0 ? B0  E  <0 ?0 A(6)  '     4   7   7  '  +       9 , 0  A    / 7   <  :##! ''0 !DD%;!D# -:##!.0-!!. 0    0 )   @0 @ ( 6A  '  +       7   7   1 
 ' ' 9 , 0   A   / 7   <   :##: ''0 D"%;#! -:##:.0-!:. 0 *   &0>0   ( 6B    73     7  '      9 0 , 0   F/&  A      / 7    - />.
-:##!.0-!$. 0  7   &0 5 ,0 ?  0 0  (6, '    4 4  7   7        9 , 0  A    / 7   <   :##D -:##D.0-!. 0 >   &0 <0  ( 6  ' ' 4 ' '( <    7    3 7  9 &  A '  ,  ''0 !:;" -!" .0-!%. &0  ( 6*    ! 09 A7
 , 4 2  -!""D.0-!D. 20 4  /0 1 0 5  )0 54(
6/ 7    '           9 , 0   A     / 7   <   ''0 !D ;!-:##D.0-!. &0 C <0 B   ,0 ,1( 6        E 9   A    A ' C  -:##$.0-! . 50 2 /0 1 0   )0 54(6,      7  '     +9 !: / 7 '  ''0 :" ;$# -:##.0