JP5674550B2

JP5674550B2 - 状態追跡装置、方法、及びプログラム

Info

Publication number: JP5674550B2
Application number: JP2011104192A
Authority: JP
Inventors: 弾三上; 大塚　和弘; 和弘大塚; 史朗熊野; 淳司大和
Original assignee: Nippon Telegraph and Telephone Corp; NTT Inc USA
Current assignee: NTT Inc; NTT Inc USA
Priority date: 2011-05-09
Filing date: 2011-05-09
Publication date: 2015-02-25
Anticipated expiration: 2031-05-09
Also published as: JP2012234466A

Description

本発明は、状態追跡装置、方法、及びプログラムに係り、特に、直接観測できない状態（パラメータ）を、観測可能な事象から推定するパーティクルフィルタを用いて、動的に変化する状態を追跡する状態追跡装置、方法、及びプログラムに関する。

時間的に変化する状態を推定するための手法としてパーティクルフィルタがある。パーティクルフィルタでは、追跡対象の状態の確率分布をパーティクルの分布で表現し、（１）現時刻における状態の事後確率分布を用いて次時刻における状態の事前確率分布を推定し、（２）推定された事前確率分布と、次時刻における実際の観測とを比較することで、事後確率分布を求める。次時刻以降、（１）及び（２）の処理を繰り返すことで、動的に変化する追跡対象の状態を逐次的に推定する。

このように、パーティクルフィルタとは、追跡対象の状態を表すパラメータの事前確率分布の予測と、事後確率分布の計算とを繰り返し行うことで、時々刻々変化するパラメータを推定するものである。このようなパーティクルフィルタは、しばしばオブジェクトの位置及び姿勢の追跡に利用される（例えば、非特許文献１参照）。オブジェクトの位置及び姿勢追跡の場合、追跡対象の状態を表すパラメータは、位置及び姿勢を表すパラメータであり、事後確率分布は、位置及び姿勢のパラメータから推定される観測（例えば、画像）と、実際の観測（カメラ等により取得される画像）とを比較することによって得ることができる。

三上弾、大塚和弘、大和淳司、「メモリベースパーティクルフィルタ：状態履歴に基づく事前分布予測を用いた頑健な対象追跡」、電子情報通信学会論文誌Ｄ Vol.J93-D No.8 pp.1313-1328

パーティクルフィルタでは、追跡対象の状態の事前及び事後確率分布を、有限個のパーティクル（粒子）の集合で表現する。粒子の密度が確率密度分布に相当する。パーティクルによる表現により、任意の分布を近似することが可能となる。しかしながら、有限個のパーティクルにより確率分布を表現するため、局所最適解に陥ってしまい、追跡に失敗してしまうことがある、という問題がある。

この問題を図９に模式的に示す。図９は横軸にパラメータ空間を、縦軸に事後確率をとったものである。曲線は事後確率関数を表し、真の事後確率である。また横軸上のドットは、事後確率関数をパーティクルにより表現したものである。パーティクルフィルタの枠組みでは、パーティクルは確率的に生成されるため、必ずしも同図（ａ）の様に全ての山（事後確率が高くなっている部分）にパーティクルが生成されるとは限らず、特に、事後確率関数が複雑な場合には、同図（ｂ）のようにパーティクルが生成されない山ができてしまう可能性がある。このような事後確率分布から、次時刻における事前確率分布を推定すると、同図（ｂ）の一番右側の山周辺には事前確率分布が生成されず、局所最適解に陥り、次時刻において追跡対象の追跡に失敗してしまう可能性がある、という問題がある。

本発明は上記問題点に鑑みてなされたものであり、局所最適解に陥ることを防止して、追跡対象の状態を追跡することができる状態追跡装置、方法、及びプログラムを提供することを目的とする。

上記目的を達成するために第１の発明の状態追跡装置は、取得手段と、類似性データ記憶部と、事後確率分布推定手段と、事前確率分布予測手段とを含む状態追跡装置であって、前記取得手段は、直接観測できない追跡対象の状態を推定するために、観測可能な事象から得られる観測を取得し、前記事後確率分布推定手段は、前時刻に予測された前記追跡対象の状態の事前確率分布を表す複数のパーティクル各々が示す状態から推定される観測の、前記取得手段により取得された観測に対する尤もらしさを示す尤度各々と、前記事前確率分布とに基づいて、前記追跡対象の状態の事後確率分布を推定し、前記類似性データ記憶部に、各パーティクルと、前記パーティクルの状態から推定される前記パーティクルの観測と類似した観測を持つ他のパーティクルとを対応付けた類似性データをあらかじめ学習して記憶し、前記事前確率分布予測手段は、前記事後確率分布推定手段により推定された事後確率分布に従ってサンプリングした現在のパーティクル、及び、前記類似性データ記憶部から検索した前記サンプリングした現在のパーティクルに対応付けられた他のパーティクルとに基づいて、次時刻の前記追跡対象の状態の事前確率分布を予測する状態追跡装置である。

第１の発明の状態追跡装置によれば、取得手段が、直接観測できない追跡対象の状態を推定するために、観測可能な事象から得られる観測を取得する。そして、事後確率分布推定手段が、前時刻に予測された追跡対象の状態の事前確率分布を表す複数のパーティクル各々が示す状態から推定される観測の、取得手段により取得された観測に対する尤もらしさを示す尤度各々と、事前確率分布とに基づいて、追跡対象の状態の事後確率分布を推定する。また、類似性データ記憶部に、各パーティクルと、パーティクルの状態から推定されるパーティクルの観測と類似した観測を持つ他のパーティクルとを対応付けた類似性データをあらかじめ学習して記憶しておく。そして、事前確率分布予測手段が、事後確率分布推定手段により推定された事後確率分布に従ってサンプリングした現在のパーティクル、及び、前記類似性データ記憶部から検索した前記サンプリングした現在のパーティクルに対応付けられた他のパーティクルに基づいて、次時刻の前記追跡対象の状態の事前確率分布を予測する。

このように、次時刻の追跡対象の状態の事前確率分布を予測する際に、現在の観測と類似する観測に対応する状態が示す確率分布も加えることにより、局所最適解に陥ることを防止して、追跡対象の状態を追跡することができる。

また、前記事前確率分布予測手段の処理をパーティクルにより記述すると、前記事前確率分布予測手段は、前記事後確率分布推定手段により推定された事後確率分布に従ってサンプリングされた現在のパーティクルに基づく事前確率分布を示すパーティクル、及び前記現在の観測と類似する観測に対応する状態を示すパーティクルとして、前記状態の類似性に従ってサンプリングされたパーティクルを用いて、前記次時刻の前記追跡対象の状態の事前確率分布を予測することができる。

また、前記類似性データを、複数の前記追跡対象の状態各々から推定された観測各々の類似度に基づいて、前記観測同士が類似する前記追跡対象の状態同士を対応付けたテーブルとして予め定めておいたり、前記追跡対象の状態、及び該追跡対象の状態から推定された観測を含む多次元データが各セルに埋め込まれる共に、セル間の距離が近いほど該セルに埋め込まれた前記多次元データ同士の類似度が高くなる自己組織化マップとして予め定めておいたりすることができる。これにより、現在の観測と類似する観測に対応する状態が示す確率分布も用いた事前確率分布の予測が可能になる。

また、第１の発明の状態追跡装置は、前記追跡対象の状態と該追跡対象の状態から推定された観測とを対応付けた学習用データを学習することにより、前記類似性データを学習する類似性学習手段を更に含んで構成することもできる。

また、第２の発明の状態追跡方法は、取得手段と、類似性データ記憶部と、事後確率分布推定手段と、事前確率分布予測手段とを含む状態追跡装置における状態追跡方法であって、前記取得手段は、直接観測できない追跡対象の状態を推定するために、観測可能な事象から得られる観測を取得し、前記事後確率分布推定手段は、前時刻に予測された前記追跡対象の状態の事前確率分布を表す複数のパーティクル各々が示す状態から推定される観測の、前記取得手段により取得された観測に対する尤もらしさを示す尤度各々と、前記事前確率分布とに基づいて、前記追跡対象の状態の事後確率分布を推定し、前記類似性データ記憶部に、各パーティクルと、前記パーティクルの状態から推定される前記パーティクルの観測と類似した観測を持つ他のパーティクルとを対応付けた類似性データをあらかじめ学習して記憶し、前記事前確率分布予測手段は、前記事後確率分布推定手段により推定された事後確率分布に従ってサンプリングした現在のパーティクル、及び、前記類似性データ記憶部から検索した前記サンプリングした現在のパーティクルに対応付けられた他のパーティクルに基づいて、次時刻の前記追跡対象の状態の事前確率分布を予測する方法である。

また、第２の発明において、前記状態追跡装置は、更に類似性学習手段を含み、前記類似性学習手段は、前記追跡対象の状態と該追跡対象の状態から推定された観測とを対応付けた学習用データを学習することにより、前記類似性データを学習するようにしてもよい。

また、第３の発明の状態追跡プログラムは、コンピュータを、第１の発明の状態追跡装置を構成する各手段として機能させるためのプログラムである。

以上説明したように、本発明の状態追跡装置、方法、及びプログラムによれば、パーティクルフィルタに基づいて、動的に変化する状態を追跡する場合において、次時刻の追跡対象の状態の事前確率分布を予測する際に、現在の観測と類似する観測に対応する状態が示す確率分布も加えることにより、局所最適解に陥ることを防止して、追跡対象の状態を追跡することができる、という効果が得られる。

第１及び第２の実施の形態の状態追跡装置の機能的構成を示すブロック図である。テーブル形式の類似性データの一例を示す図である。第１の実施の形態の状態追跡装置における類似性学習処理ルーチンの内容を示すフローチャートである。第１及び第２の実施の形態の状態追跡装置における追跡処理ルーチンの内容を示すフローチャートである。第１の実施の形態の状態追跡装置における事前確率分布予測処理ルーチンの内容を示すフローチャートである。ＳＯＭ形式の類似性データの一例を示す図である。第２の実施の形態の状態追跡装置における類似性学習処理ルーチンの内容を示すフローチャートである。第２の実施の形態の状態追跡装置における事前確率分布予測処理ルーチンの内容を示すフローチャートである。従来のパーティクルフィルタの問題点を説明するための図である。

以下、図面を参照して本発明の実施の形態を詳細に説明する。

第１の実施の形態に係る状態追跡装置１０は、ＣＰＵ（Central Processing Unit）と、ＲＡＭ（Random Access Memory）と、後述する追跡処理ルーチン及び類似性学習処理ルーチンを実行するためのプログラムを記憶したＲＯＭ（Read Only Memory）とを備えたコンピュータで構成されている。このコンピュータは、機能的には、図１に示すように、追跡部２０及び学習部３０を含んだ構成で表すことができる。

追跡部２０は、初期設定部２２、観測取得部２４、事後確率分布推定部２６、及び事前確率分布予測部２８を含んだ構成で表すことができる。また、学習部３０は、類似性学習部３２及び類似性データ記憶部３４を含んだ構成で表すことができる。追跡対象の状態の追跡に先立って、類似性学習部３２により観測空間での追跡対象の状態の類似性を学習し、類似性データとして類似性データ記憶部３４に記憶しておく。

初期設定部２２は、追跡対象の状態（パーティクル）の初期状態を設定する。設定方法は周知技術を用いればよく、様々な方法が考えられる。例えば、パーティクルを広く分布させた初期状態を設定することができる。具体的には、可能性のある全ての状態を表すパーティクルを初期状態として作成する。そして、作成したパーティクルの集合を全探索することで追跡対象を発見し、追跡を開始する。ただし、この場合初期化に極めて時間がかかることが予想される。

また、別の設定方法として、検出手法を用いてもよい。例えば、カメラで撮影された画像上の顔を追跡対象とする場合では、顔検出装置の結果を用いてパーティクルの初期状態を設定する。パーティクルの状態は、パラメータの組で表現する。この場合は、正面を向いた顔を検出する装置を用いて、検出された顔の位置と、角度として正面を向いているという情報とをパーティクルの状態を表すパラメータの初期値として設定する。

また、さらに別の設定方法として、アプリケーションにより人手でパーティクルの初期状態を与えるようにしてもよい。パーティクルの初期状態の設定はどのような方法で行ってもよく、追跡対象に合わせて選択すればよい。

設定したパーティクルの初期状態の集合は、事後確率分布推定部２６において事前確率分布を表すパーティクルの集合として利用される。

観測取得部２４は、追跡対象の状態を推定するための実際の観測を取得する。例えば、上記のように画像上の顔を追跡対象とする場合には、カメラで撮影された画像の各画素値を観測として取得する。

事後確率分布推定部２６は、初期設定部２２で設定された事前確率分布、または後述する事前確率分布予測部２８により予測された事前確率分布を表すパーティクルの集合に含まれる各パーティクルについて、パーティクルの示す状態から推定される観測を求める。パーティクルの状態から推定した観測と観測取得部２４で取得した実際の観測とを比較することにより、推定した観測の実際の観測に対する尤もらしさを示す尤度を求める。そして、事前確率分布と尤度とに基づいて事後確率分布を算出する。

ここで、パーティクルの示す状態及び観測について具体例を挙げて説明する。カメラ映像を用いて物体を追跡する場合であれば、パーティクルの示す状態は追跡する物体の位置及び姿勢などを表すパラメータ（例えば、Ｘ，Ｙ，Ｚ座標やピッチ角度、ロール角度、ヨー角度等）であり、実際の観測はカメラにより撮影された画像、つまり実際に観測することができる事象である。また、他の例として、距離センサを用いて自走式ロボットの位置を追跡する場合であれば、パーティクルの示す状態は追跡する自走式ロボットの位置を表すパラメータであり、実際の観測は距離センサの出力となる。

カメラ映像を用いて物体を追跡する場合において、テンプレート照合に基づいた追跡をパーティクルフィルタで行う場合を例に、事後確率分布推定部２６の具体処理を説明する。まず、テンプレート画像に、事前確率分布を表すパーティクルの状態に従った幾何学変形（平行移動及び回転）を加えたテンプレートの観測（画像）を推定する。このような幾何学変形は一般的な行列演算によって実現できる。実際の観測としては、画像（画素値の集合）が取得される。事前確率分布を表すパーティクルの状態から推定されたテンプレートの観測と、実際の画像とを比較することで尤度を求める。この比較方法として、各画素の差の絶対値、各画素の自乗誤差、各画素の差に何らかの関数（ロバスト関数など）を適用したものなどが用いられる。ただし、比較方法はこれに限定されない。そして、尤度と事前確率分布とからパーティクルの事後確率分布を推定する。なお、顔姿勢を推定する場合の事後確率分布推定の例は、非特許文献１に詳しく述べられている。

類似性データ記憶部３４には、観測空間でのパーティクルの類似性を対応付けた類似性データが記憶されている。類似性データは、例えば、図２に示すようなテーブル形式で保持することができる。図２の場合の類似性データは、各パーティクルを識別するための識別番号であるパーティクルｉｄ、追跡対象の状態を示すパラメータ、及びそのパラメータから推定される観測と類似した観測を持つパーティクルのパーティクルｉｄ、及び観測同士の類似度を含む。ここでの追跡対象の状態を示すパラメータは、テンプレート照合に基づいて、パーティクルフィルタにより状態（位置及び姿勢）を求める場合のパラメータの一部であり、一例として、ｘ_０（ｘ位置）、・・・、ｘ_ｒ（ヨー角度）がパラメータとして定められている。

事前確率分布予測部２８は、事後確率分布推定部２６で推定された事後確率分布と、類似性データ記憶部３４に記憶された類似性データとに基づいて次時刻でのパーティクルの状態（事前確率分布）を予測する。

従来のパーティクルフィルタの枠組みでは、事前確率分布推定は、事後確率分布からパーティクルの集合をサンプリングし、各パーティクルの状態に対して、ランダムウォークのダイナミクスを仮定したガウシアンノイズを重畳して、事前確率分布を表すパーティクルの集合としている。なお、サンプリングとは、パーティクルの集合から、パーティクルを選択することを意味する。

ここで、従来のパーティクルフィルタは、事前確率分布の予測にあたって、パラメータの状態空間での類似性にのみ着目している。一方、本実施の形態では、類似性データ記憶部３４に蓄積された類似性データに基づいてサンプリングされるパーティクルを併せて利用して事前確率分布を予測する。

具体的には、従来のパーティクルフィルタの手法と同様に、事後確率分布に基づいてサンプリングされたパーティクルに、ダイナミクスを考慮した変換を加え、事前確率分布を表すパーティクルのひとつとする。ここで、ダイナミクスを考慮した変換とは、ダイナミクスとして、ランダムウォークを仮定する場合には、ガウスノイズを付加することに相当する。また、等速運動を仮定する場合には、一時刻前から現在までのパラメータの変化を加え、さらにガウスノイズを付加することに相当する。

さらに、類似性データ記憶部３４に記憶された類似性データに基づいて、現在のパーティクル（事後確率分布に基づいてサンプリングされたパーティクル）と類似したパーティクルを検索し、そのパーティクルの状態に対応するパラメータにガウスノイズを付加し、これを次時刻での事前確率分布を表すパーティクルのひとつに加える。図２に示した類似性データを用いる場合には、現在のパーティクルのパーティクルｉｄを検索し、そのパーティクルｉｄに対応付けられている類似したパーティクルｉｄに対応するパーティクルの状態に対応するパラメータにガウスノイズを付加し、これを次時刻での事前確率分布を表すパーティクルのひとつに加える。

このように、観測空間における類似性を考慮した類似性データに基づいて、現在のパーティクルに類似するパーティクルを事前確率分布の予測に加えることにより、図９（ｂ）で示したように、従来のパラメータ空間の類似性だけを考慮した場合には作成できなかったパーティクルについても作成できる可能性が高くなり、局所最適解に陥ることを抑制することができる。

類似性学習部３２は、類似性データを学習し、類似性データ記憶部３４に記憶する。パーティクルフィルタの枠組みでは、パーティクルの状態から観測を推定することが可能である。そのため、パーティクルの状態を変化させながら観測を推定し、観測空間においてパラメータがどのように類似しているかを学習する。具体的には、想定される全てのパーティクルについて観測を推定し、観測同士の類似度を計算し、類似度が閾値以上であれば、観測が類似するパーティクル同士を対応付けて、例えば、図２に示すようなテーブル形式で類似性データを記憶する。

次に、図３を参照して、第１の実施の形態の状態追跡装置１０において実行される類似性学習処理ルーチンについて説明する。ここでは、カメラ映像から顔の位置及び姿勢を追跡する場合を例に説明する。

ステップ１００で、想定される全てのパーティクルの状態の組（Ｓ＝｛ｓ_１，・・・，ｓ_Ｎ｝）を作成する。ここで、ｓ_ｋはｉｄがｋのパーティクルの状態を表し、ｓ_ｋ＝｛ｘ_０ ^（ｋ），ｘ_１ ^（ｋ）、・・・、ｘ_ｒ ^（ｋ）｝である。なお、ｘ_０、ｘ_１、・・・、ｘ_ｒは、パーティクルの状態を表すパラメータであり、例えば、ｘ_０＝ｘ座標、ｘ_１＝ｙ座標、・・・、ｘ_ｒ＝ヨー角度などとなる。ｘ_０ ^（ｋ）は、ｉｄがｋのパーティクルのパラメータｘ_０の値を示す。

次に、ステップ１０２で、パーティクルｉｄに対応する変数ｋに１を設定し、次に、ステップ１０４で、パーティクルの状態ｓ_ｋについて観測を推定する。具体的には、テンプレートをパーティクルの状態ｓ_ｋを示すパラメータの位置及び姿勢により変形（平行移動及び回転）することにより、パーティクルの状態からテンプレートの観測（画像）を推定する。

次に、ステップ１０６で、上記ステップ１０４で推定した観測を、パーティクルｉｄ（ｋ）と対応付けて、推定観測結果記憶部（図示省略）に記憶する。

次に、ステップ１０８で、変数ｋがＮになったか否かを判定することにより、パーティクルの状態の組Ｓに含まれる全てのパーティクルについて処理が終了したか否かを判定する。ｋ≠Ｎの場合には、ステップ１１０へ移行して、ｋを１インクリメントして、ステップ１０４へ戻り、ステップ１０４及びステップ１０６の処理を繰り返す。一方、ｋ＝Ｎとなった場合には、ステップ１１２へ移行する。

ステップ１１２では、変数ｉに１を設定し、次に、ステップ１１４で、推定観測結果記憶部に記憶された観測のうち、パーティクルｉｄ（ｉ）に対応付けられた観測を検索し、パーティクルｉｄ（ｉ）の観測と、パーティクルｉｄ（ｊ）の観測との類似度ｄを計算する。なお、ｊは、ｊ∈Ｓ、ｉ≠ｊである。すなわち、パーティクルｉｄ（ｊ）は、推定観測結果記憶部に記憶されたパーティクルｉｄ（ｉ）以外の全てのパーティクルｉｄに相当する。また、類似度ｄは、例えば、各画素の差の絶対値、各画素の自乗誤差、各画素の差に何らかの関数（ロバスト関数など）を適用したものなどが用いて計算することができる。なお、類似度の計算はこれらの方法に限定されない。

次に、ステップ１１６で、上記ステップ１１４で計算した類似度ｄが、類似度閾値ｄｔｈ以上か否かを判定する。ｄ≧ｄｔｈの場合には、ステップ１１８へ移行して、類似性データ記憶部３４に記憶されたテーブル形式の類似性データ（例えば、図２）に、パーティクルｉｄ（ｉ）、パーティクルｉｄ（ｊ）、及び上記ステップ１１４で計算した類似度ｄを対応付けて記憶して、ステップ１２０へ移行する。また、図２に示したテーブルのように、パーティクルの状態ｓ_ｉを示す各パラメータの値も共に対応付けて記憶してもよい。一方、ｄ＜ｄｔｈの場合には、ステップ１１８をスキップして、ステップ１２０へ移行する。

ステップ１２０では、変数ｉがＮになったか否かを判定することにより、推定観測結果記憶部に記憶された全てのデータについて処理が終了したか否かを判定する。ｉ≠Ｎの場合には、ステップ１２２へ移行して、ｉを１インクリメントして、ステップ１１４へ戻り、ステップ１１４〜１１８の処理を繰り返す。一方、ｉ＝Ｎとなった場合には、処理を終了する。

次に、図４を参照して、第１の実施の形態の状態追跡装置１０において実行される追跡処理ルーチンについて説明する。ここでは、上記の類似性学習処理と同様に、カメラ映像から顔の位置及び姿勢を追跡する場合を例に説明する。

ステップ１５０で、追跡対象の状態を示すパーティクルの初期状態を設定する。例えば、想定される全ての状態を表すパーティクルを初期状態として作成する。次に、ステップ１５２で、実際にカメラで撮影された画像（観測）を取得する。

次に、ステップ１５４で、テンプレート画像に、上記ステップ１５０で設定されたパーティクルの初期状態に従った変形（平行移動及び回転）を加えて、テンプレートの観測（画像）を推定する。そして、推定された観測と上記ステップ１５２で取得された実際の観測とを比較して尤度を求め、求めた尤度とパーティクルの初期状態が示す事前確率分布に基づいて、事後確率分布を推定する。

次に、ステップ１５６で、後述する事前確率分布予測処理を実行して、次に、ステップ１５８で、追跡処理を終了するか否かを判定する。この判定は、実際の観測として次フレームの画像が取得されているか否か、処理終了を指示する信号が入力されているか否か等により判定することができる。追跡処理を終了しない場合には、ステップ１５２へ戻って、ステップ１５２〜１５６の処理を繰り返す。ステップ１５８で否定判定されタ後に、ステップ１５４へ移行した場合には、ステップ１５４の事後確率分布の推定に用いられる事前確率分布は、上記ステップ１５０の初期設定された事前確率分布ではなく、ステップ１５６で前時刻に予測された事前確率分布を用いる。ステップ１５８で肯定判定された場合には、処理を終了する。

次に、図５を参照して、事前確率分布予測処理ルーチンについて説明する。ここでは、図２に示すテーブル形式の類似性データを用いる場合について説明する。

ステップ１５６０で、上記ステップ１５４で推定された事後確率分布に従って、事後確率分布を表すパーティクル集合から、パーティクルをひとつサンプリングする。次に、ステップ１５６２で、従来のパーティクルフィルタの手法と同様に、上記ステップ１５６０でサンプリングしたパーティクルの状態を表すパラメータに、ダイナミクスを考慮した変換を加え、事前確率分布を表すパーティクルのひとつを作成する。

次に、ステップ１５６４で、乱数ｒを発生させ、次に、ステップ１５６６で、乱数ｒが乱数閾値ｒｔｈ以上か否かを判定する。ｒ≧ｒｔｈの場合には、ステップ１５６８へ移行し、ｒ＜ｒｔｈの場合には、ステップ１５６８をスキップして、ステップ１５７０へ移行する。

ステップ１５６８では、類似性データ記憶部３４に記憶されたテーブル形式の類似性データを参照して、上記ステップ１５６０でサンプリングされたパーティクルのパーティクルｉｄを検索し、そのパーティクルｉｄに対応付けられている類似したパーティクルｉｄに対応するパーティクルの状態を表すパラメータに、ダイナミクスを考慮した変換を加え、事前確率分布を表すパーティクルのひとつとして加える。

なお、ここでは、乱数の閾値判断により類似性データに基づくパーティクルの追加を行うか否かを判定しているが、これは、次時刻の事前確率分布を予測するに当たり、多くの場合は、現在のパーティクルの近くのパーティクル、すなわちパラメータ空間において類似するパーティクルをサンプリングすることで対応できると考えられるため、現在のパーティクルの全てについて、類似性データに基づくパーティクルを追加する必要はないからである。また、類似性データに基づくパーティクルの追加を行うか否かの判定は、乱数の閾値判断で行う場合に限定されず、ランダムに行うようにしてもよいし、上記ステップ１５６０において、予め定めたｉｄのパーティクルがサンプリングされた際に行うようにしてもよい。

次に、ステップ１５７０で、作成された事前確率分布を表すパーティクルの数が、予め定めた所定個に達したか否かを判定する。所定個は、一般的なパーティクルフィルタと同様に、固定の値としてもよいし、追跡対象の状態に応じて適応的に変更してもよい。パーティクル数が所定個に達していない場合には、ステップ１５６０へ戻って、ステップ１５６０〜１５７０の処理を繰り返す。パーティクル数が所定個に達した場合には、追跡処理（図４）にリターンする。

以上説明したように、第１の実施の形態の状態追跡装置によれば、パーティクルフィルタに基づいて、動的に変化する状態を追跡する場合において、次時刻の事前確率分布を予測する際に、観測空間において現在のパーティクルと類似するパーティクルも加えることにより、局所最適解に陥ることを防止して、追跡対象の状態を追跡することができる。

次に、第２の実施の形態について説明する。第２の実施の形態の状態追跡装置は、類似性データをＳＯＭ（Self organizing feature map：自己組織化マップ）の形式で記憶しておく点が、第１の実施の形態と異なる。なお、第２の実施の形態の状態追跡装置において、第１の実施の形態の状態追跡装置１０と同一の構成については、同一符号を付して、詳細な説明を省略する。

図１に示すように、第２の実施の形態の状態追跡装置２１０を構成するコンピュータは、機能的には、追跡部２２０及び学習部２３０を含んだ構成で表すことができる。

追跡部２２０は、初期設定部２２、観測取得部２４、事後確率分布推定部２６、及び事前確率分布予測部２２８を含んだ構成で表すことができる。また、学習部２３０は、類似性学習部２３２及び類似性データ記憶部３４を含んだ構成で表すことができる。

類似性データ記憶部２３４には、観測空間でのパラメータの類似性を対応づけた類似性データが記憶されている。類似性データは、例えば、図６に示すようなＳＯＭとして保持することができる。

ＳＯＭとは、高次元データを低次元に圧縮する方法で、可視化の性能に優れた１次元または２次元の情報への圧縮が一般的である。ここでは、２次元データへの圧縮を例にとって説明する。図６は、ＳＯＭの例であり、Ｍ×Ｍの２次元データ（以下、「２次元マップ」と呼ぶ）へ、位置（Ｘ，Ｙ，Ｚの３次元）及び姿勢（ピッチ角度、ロール角度、ヨー角度の３次元）と、位置及び姿勢（状態）から推定された観測（Ｌ次元）とをあわせた多次元データを圧縮している。図６の各セルは、それぞれ位置及び姿勢の６次元と観測のＬ次元とを合わせた多次元データを持ち、隣接するセル同士が有する多次元データは、互いに類似する。ここで、観測のＬ次元は、画像の表現方法に依存する。観測を単純な画素値の集合とする場合、画像のピクセル数がＬとなる。

事前確率分布予測部２２８は、第１の実施の形態における事前確率分布予測部２８と、類似性データに基づくパーティクルを追加する処理が異なるだけであるので、異なる点について説明する。

事前確率分布予測部２２８は、現在のパーティクル（事後確率分布に基づいてサンプリングされたパーティクル）の状態から、そのときの観測を推定し、状態及び推定された観測を含む多次元データを得る。その多次元データと、ＳＯＭアルゴリズムに基づいて作成された２次元マップの各セル（多次元データを保持している）との類似性を比較し、最も類似したセルを選択する。この比較は、自乗誤差の逆数などにより行うのが一般的である。さらに、位置、姿勢、及び推定された観測各々を異なる重み付けで足し合わせてもよい。このようにして選択された最も類似したセルを勝ちニューロンと呼ぶ。そして、勝ちニューロンの近傍からセルをひとつサンプリングする。このサンプリングは、２次元マップ上で、勝ちニューロンに近ければ近いほど高い確率でサンプリングされ、勝ちニューロンから離れるに従って、サンプリングされ難くなるように行う。具体的には、正規乱数に従った乱数を用いることで、勝ちニューロンの近傍ほど高い確率でサンプリングされるようにすることができる。そして、サンプリングされたセルが保持する多次元データのうち、位置及び姿勢（状態）のデータを用い、そこに誤差成分を付加することで、事前確率分布を表すパーティクルに加える。誤差成分の付加には、正規乱数などが一般に利用される。

ここで、２次元マップ上では、位置及び姿勢だけでなく、推定された観測を含めた多次元データが類似しているセル同士が近傍に配置されている。つまり、２次元マップ上で近傍を探索することは、単に、位置及び姿勢が類似しているパーティクルをサンプリングする一般的なパーティクルフィルタと比較して、観測までを含めた類似性を考慮した事前確率分布予測となっているといえる。

次に、図７を参照して、第２の実施の形態の状態追跡装置２１０における類似性学習処理ルーチンについて説明する。なお、第１の実施の形態の類似性学習処理と同一の処理については、同一符号を付して詳細な説明を省略する。

ステップ１００〜１０４を実行して、パーティクルの状態ｓ_ｋについて観測を推定し、次に、ステップ２００で、パーティクルの状態ｓ_ｋを示すパラメータ、及び上記ステップ１０４で推定された観測を含む多次元データｋを、推定観測結果記憶部（図示省略）に記憶する。

次に、ステップ１０８及びステップ１１０を経て、パーティクルの状態の組Ｓに含まれる全てのパーティクルについて多次元データを作成し、ステップ１０８で肯定判定されると、ステップ２０２へ移行する。

ステップ２０２では、ＳＯＭの各セルにランダムに多次元データを付与して、ＳＯＭを初期化する。次に、ステップ１１２で、変数ｉに１を設定し、次に、ステップ２０４で、推定観測結果記憶部に記憶された多次元データから、多次元データｉを選択し、上記ステップ２０２で初期化されたＳＯＭの各セルの中で最も類似したセル（勝ちニューロン）を求める。なお、多次元データの選択は、上記のように多次元データの先頭から順番に１つ選択する場合に限定されず、ランダムに選択するようにしてもよい。また、類似の尺度は、多次元データの各次元の値の自乗和や重要度に従った重みを付けたものなどを用いることができる。

次に、ステップ２０６で、２次元マップ上の各セルについて、上記ステップ２０４で選択された最も類似したセルからの２次元マップ上での距離、及び繰り返し回数（ステップ１２０の判定による繰り返し回数）に応じた重みに従って、各セルの多次元データの値を、多次元データｉの値に近づけるように更新する。２次元マップ上の距離は、例えば、勝ちニューロンの２次元マップ上での位置（Ｗｘ，Ｗｙ）と、各セルの２次元マップ上での位置（Ｓｘ，Ｓｙ）により、（（Ｗｘ-Ｓｘ）^２+（Ｗｙ-Ｓｙ）^２）^０．５などにより求めることができる。

次に、ステップ１２０及びステップ１２２を経て、推定観測記憶部に記憶された全ての多次元データについて処理し、ステップ１２０で肯定判定されると、ステップ２０８へ移行して、現在のＳＯＭを類似性データとして類似性データ記憶部２３４へ記憶して、処理を終了する。

なお、ステップ２０６において、繰り返し回数が少ない場合には、２次元マップ上の勝ちニューロンを中心としたより広い範囲のセルの多次元データを更新する。繰り返し回数が多くなるに従い、２次元マップ上の勝ちニューロンを中心としたより狭い範囲、つまり勝ちニューロンの近傍セルの多次元データのみを更新する。また、各繰り返しにおいて、２次元マップ上で勝ちニューロンの近傍にあるセルほど、多次元データｉの値により近づくように更新する。これを繰り返し行うことによって、２次元マップ上で、近傍にあるセルが持つ状態を示すパラメータはお互い類似するようになる（詳細は、非特許文献２「http://ja.wikipedia.org/wiki/%E8%87%AA%E5%B7%B1%E7%B5%84%E7%B9%94%E5%8C%96%E5%86%99%E5%83%8F」、及び非特許文献３「Ｔ．コホネン著、徳高平蔵、堀尾恵一、大北正昭、大薮又茂、藤村喜久郎訳『自己組織化マップ』、シュプリンガーフェアラーク東京、２００５年」参照）。

次に、第２の実施の形態の状態追跡装置２１０において実行される追跡処理ルーチンについて説明する。第２の実施の形態における追跡処理ルーチンは、第１の実施の形態における追跡処理ルーチン（図４）と、ステップ１５６で実行される事前確率分布予測処理が異なるだけであるので、図８を参照して、第２の実施の形態における事前確率分布予測処理ルーチンについて説明する。なお、第１の実施の形態の事前確率分布処理と同一の処理については、同一符号を付して詳細な説明を省略する。

ステップ１５６０で、上記ステップ１５４で推定された事後確率分布に従って、事後確率分布を表すパーティクル集合から、パーティクルをひとつサンプリングする。次に、ステップ１５６２で、上記ステップ１５６０でサンプリングしたパーティクルの状態を表すパラメータから事前確率分布を表すパーティクルのひとつを作成する。

次に、ステップ２５６０で、上記ステップ１５６０でサンプリングされた現在のパーティクルの状態から、そのときの観測を推定し、状態及び推定された観測を含む多次元データを得る。その多次元データと、ＳＯＭアルゴリズムに基づいて作成された２次元マップの各セルが保持する多次元データとの類似性を比較し、最も類似したセル（勝ちニューロン）を選択する。

次に、ステップ２５６２で、上記ステップ２５６０で選択された勝ちニューロンの近傍から、正規乱数に従った乱数を用いることで、勝ちニューロンの近傍ほど高い確率でサンプリングされるように、セルをひとつサンプリングする。

次に、ステップ２５６４で、上記ステップ２５６４でサンプリングされたセルが保持する多次元データのうち、位置及び姿勢（状態）のデータを用い、そこに正規乱数などを用いて誤差成分を付加することで、事前確率分布を表すパーティクルとして追加する。

次に、ステップ１５７０で、作成された事前確率分布を表すパーティクルの数が、予め定めた所定個に達したか否かを判定する。パーティクル数が所定個に達していない場合には、ステップ１５６０へ戻って、ステップ１５６０〜１５７０の処理を繰り返し、所定個に達した場合には、追跡処理（図４）にリターンする。

以上説明したように、第２の実施の形態の状態追跡装置によれば、パーティクルフィルタに基づいて、動的に変化する状態を追跡する場合において、次時刻の事前確率分布を予測する際に、観測空間において現在のパーティクルと類似するパーティクルも加えることにより、局所最適解に陥ることを防止して、追跡対象の状態を追跡することができる。

なお、上記第１及び第２の実施の形態では、追跡部と学習部とを同一のコンピュータで実現する場合について説明したが、個々のコンピュータで構成するようにしてもよい。この場合、類似性データ記憶部は、学習部を構成するコンピュータに設けてもよいし、追跡部を構成するコンピュータに設けてもよいし、また他の外部装置に設けてもよい。追跡部を構成するコンピュータ以外に類似性データ記憶部を設けた場合には、追跡処理の際に、ネットワーク等を介して学習部を構成するコンピュータに設けられた類似性データ記憶部、または他の外部装置に設けられた類似性データ記憶部に記憶された類似性データを読み出すようにするとよい。

また、本発明は、上記実施の形態に限定されるものではなく、この発明の要旨を逸脱しない範囲内で様々な変形や応用が可能である。

また、上述の状態追跡装置は、内部にコンピュータシステムを有しているが、「コンピュータシステム」は、ＷＷＷシステムを利用している場合であれば、ホームページ提供環境（あるいは表示環境）も含むものとする。

また、本願明細書中において、プログラムが予めインストールされている実施形態として説明したが、当該プログラムを、コンピュータ読み取り可能な記録媒体に格納して提供することも可能である。

１０、２１０状態追跡装置
２０、２２０追跡部
２２初期設定部
２４観測取得部
２６事後確率分布推定部
２８、２２８事前確率分布予測部
３０、２３０学習部
３２、２３２類似性学習部
３４、２３４類似性データ記憶部

Claims

取得手段と、類似性データ記憶部と、事後確率分布推定手段と、事前確率分布予測手段とを含む状態追跡装置であって、
前記取得手段は、直接観測できない追跡対象の状態を推定するために、観測可能な事象から得られる観測を取得し、
前記事後確率分布推定手段は、前時刻に予測された前記追跡対象の状態の事前確率分布を表す複数のパーティクル各々が示す状態から推定される観測の、前記取得手段により取得された観測に対する尤もらしさを示す尤度各々と、前記事前確率分布とに基づいて、前記追跡対象の状態の事後確率分布を推定し、
前記類似性データ記憶部に、各パーティクルと、前記パーティクルの状態から推定される前記パーティクルの観測と類似した観測を持つ他のパーティクルとを対応付けた類似性データをあらかじめ学習して記憶し、
前記事前確率分布予測手段は、前記事後確率分布推定手段により推定された事後確率分布に従ってサンプリングした現在のパーティクル、及び、前記類似性データ記憶部から検索した前記サンプリングした現在のパーティクルに対応付けられた他のパーティクルに基づいて、次時刻の前記追跡対象の状態の事前確率分布を予測する
状態追跡装置。
前記類似性データを自己組織化マップの形式で記憶する請求項１記載の状態追跡装置。
取得手段と、類似性データ記憶部と、事後確率分布推定手段と、事前確率分布予測手段とを含む状態追跡装置における状態追跡方法であって、
前記取得手段は、直接観測できない追跡対象の状態を推定するために、観測可能な事象から得られる観測を取得し、
前記事後確率分布推定手段は、前時刻に予測された前記追跡対象の状態の事前確率分布を表す複数のパーティクル各々が示す状態から推定される観測の、前記取得手段により取得された観測に対する尤もらしさを示す尤度各々と、前記事前確率分布とに基づいて、前記追跡対象の状態の事後確率分布を推定し、
前記類似性データ記憶部に、各パーティクルと、前記パーティクルの状態から推定される前記パーティクルの観測と類似した観測を持つ他のパーティクルとを対応付けた類似性データをあらかじめ学習して記憶し、
前記事前確率分布予測手段は、前記事後確率分布推定手段により推定された事後確率分布に従ってサンプリングした現在のパーティクル、及び、前記類似性データ記憶部から検索した前記サンプリングした現在のパーティクルに対応付けられた他のパーティクルに基づいて、次時刻の前記追跡対象の状態の事前確率分布を予測する
状態追跡方法。
コンピュータを、請求項１または請求項２記載の状態追跡装置を構成する各手段として機能させるための状態追跡プログラム。