JP5906379B2 - 移動体位置推定装置、移動体位置推定方法、及び、移動体位置推定プログラム - Google Patents

Description

本発明は、センサからの対象物体に関する観測値に基づいて、観測対象の空間内に存在する複数の移動物体（人などの移動体）の位置を推定する移動体位置推定装置、移動体位置推定方法、及び、移動体位置推定プログラムに関するものである。

センサの観測値に基づいて人などの移動体の位置を推定する方式として、センシング性能の異なる異種のセンサの観測値を統合することにより、単体のセンサのみによるセンシングの弱点を補完する方式が存在する。

例えば、カメラと画像認識処理による画像センサにより人の識別と位置との推定をしようとした場合、画像認識処理だけでは、複数の人をお互いに高精度に識別する事が困難なケースが多い。そこで、カメラとは別に、人の識別性能の高いセンサ（無線タグ）の観測値と、カメラの観測値とを組み合わせることで、単体のセンサの弱点を解消しつつ、移動体の識別と位置の推定を行う従来技術（非特許文献１）がある。

非特許文献１では、対象物の位置情報は出力できるが、識別能力がないカメラと画像認識によるセンサの観測値と、対象物の識別能力が高く、位置情報も出力できる無線タグによるセンサの観測値とを統合することにより、移動する対象物を互いに識別しながら、逐次変化する位置を推定し対象物の移動軌跡を出力する方式が開示されている。

具体的には、非特許文献１で開示される方式は、オンラインベイズ推定の枠組みに、複数の観測値と、推定処理内部の複数の位置推定分布の対応付けを確率的に表現するアソシエーション変数とを導入することで、複数の人などの移動体の逐次位置推定を同時に行うものである。

さらに具体的には、対象物の識別情報（ＩＤ）の離散分布であるＩＤ尤度、及び、対象物の観測位置と、推定処理内部の各対象物の推定位置分布との距離的な近さから算出される距離尤度（距離が近い方が観測値と位置推定分布との対応の可能性が大きいとして距離尤度が大きい値を取る）の２つの値の積を規格化した値を、アソシエーション値として算出する。

このアソシエーション値は、観測値と推定位置分布の対応関係に１つ割り当てられる値であり、各観測値に含まれる位置情報を、各対象物の推定位置の更新にどれだけ寄与させればよいかの重みに相当する。

各観測値の観測値位置に前記アソシエーション値を重みづけした値で、推定分布を更新することにより、観測値と対象物の対応関係の曖昧性を考慮した位置推定を行っている。

特に、非特許文献１は、識別情報（ＩＤ）を出力できないセンサ（例えば認識機能をもつカメラセンサ）を用いた場合にも、前記のアソシエーション値に基づく、複数物体の移動軌跡を推定するための構成と方式を開示している。カメラの撮像データの画像認識処理では、対象物の識別を高精度に行うことは難しいが、１観測周期において時間的に前後する２つの観測値が同じ対象物を観測したかどうかについては、その時点で撮像した対象物の画像特徴の類似性を評価するなどにより容易に判別できる。

そこで、観測周期前後で同じ対象物を観測していると判別できた場合は、前回の観測時点での観測値と対象物の対応関係が維持されているとして、カメラセンサが出力できないＩＤ尤度を、前の観測値に基づいて得られたアソシエーション値で近似する。一方、観測周期前後で同じ対象物ではないと判別された場合は、前回の観測時点での観測値と対象物の対応関係が維持されていないため、カメラセンサが出力できないＩＤ尤度を、一様分布で近似する（ＩＤ尤度が一様分布であるということは観測値と対象物の対応関係が不明であるということに対応する）。前記対象物が同一であるかどうかの判別状態が曖昧な場合は、判別状態を確率値（追跡尤度）で表現し、前記２つの場合を、この追跡尤度により結合し、ＩＤ尤度を近似する。

（数１）
そのときのカメラセンサ観測値のＩＤ尤度＝追跡尤度×前回のアソシエーション値＋（１−追跡尤度）×一様分布
．．．．（式１）

このように、式１で示されるＩＤ尤度の近似により、対象物の識別情報が出力できないセンサに対して、識別情報の近似値を与えることで、複数物体の位置推定を行えるようにしている。

谷川徹、山上勝義、"カメラと無線タグのセンサ情報統合による移動体トラッキング技術の開発"、第２８回日本ロボット学会学術講演会、２０１０年９月２２〜２４日

しかしながら、非特許文献１で開示されている方式は、観測対象である移動体同士の距離が接近するような状況においては、観測値と対象物との対応関係が曖昧になるために、位置推定誤差が蓄積し、移動体の移動経路を誤って推定してしまうという課題を有する。

この課題について、２人の人が交差するように移動した例を用いて説明する。

図１７は、空間１７０１内を北から南へ移動する人（図１７中でＩＤが「Ａ」の人）、西から東へ移動する人（図１７中でＩＤが「Ｂ」の人）が真ん中付近ですれ違うように移動した場合の、人の移動経路１７０２，１７０３を示す図である。

図１７の１７０１Ａは、破線状の矩形で表した、空間１７０１の中心付近の領域である。この中心付近の領域１７０１Ａで起こる現象を、図１８を用いて説明する。

図１８の（Ａ）、（Ｂ）、（Ｃ）は、図１７の中心付近の領域１７０１Ａに対応する領域でのＡの人及びＢの人の位置推定分布（図中丸で示す）と、カメラの観測値（図中三角で示す）を示したものである。（Ａ）、（Ｂ）、（Ｃ）の順に観測が進んだものとする。

非特許文献１で開示されている方式では、移動体同士が接近する状況では、観測値と位置推定分布との近さに基づく距離尤度の観点では、観測値が真に対応する位置推定分布との距離尤度の値と、観測値が真に対応しない位置推定分布との距離尤度の値が均等に近づいていく。例えば、図１８の（Ｂ）に示される状況においては、Ａの人のカメラ観測値とＡの人の位置推定分布との距離と、Ａの人のカメラ観測値とＢの人の位置推定分布との距離は、相対的に近くなるため、Ａの人のカメラ観測値のＡの人の位置推定分布との距離尤度の値と、Ａの人のカメラ観測値とＢの人の位置推定分布との距離尤度の値は相対的に近くなる。

また、非特許文献１で開示されている方式では、移動体同士が接近する状況では、画像識別処理において２つの移動体の分別が難しくなることで、同じ対象を観測出来ているかどうかが曖昧になる。よって、式１の追跡尤度の確率値が小さい値になり、結果的に一様分布がＩＤ尤度に反映される割合が大きくなり、近似されたＩＤ尤度の観点でも観測値が真に観測値した対象物とのＩＤ尤度の値と、観測値が真に観測した対象物以外とのＩＤ尤度の値が、相対的に値が近くなる。

距離尤度の値、及び、ＩＤ尤度の値が均等に近づくと、その結果、距離尤度とＩＤ尤度の積から算出されるアソシエーション値についても、位置推定分布と対応するはずの観測値との間のアソシエーション値と、位置推定分布と対応しないはずの観測値との間のアソシエーション値が、相対的に近くなる。

このため、位置推定分布の更新時に、真に対象物を観測した観測値以外の観測値の位置情報が更新に寄与する割合が相対的に高まることにより、真に対象物を観測した観測値の方向とはずれた方向に位置推定分布の更新が行われ、位置推定誤差が増大する。

図１８の実線矢印は、観測値により位置推定分布が更新される方向を例示したものである。図１８の（Ｂ）の状況では、Ａの人の位置推定分布、Ｂの人の位置推定分布、それぞれ更新される方向が、Ａの人のカメラ観測値の位置、Ｂの人のカメラ観測値の位置の方向からずれている。このため、次の観測時刻（（Ｃ）の状況）では、Ａの人の位置推定分布は東側にずれ、Ｂの人の位置推定分布は南側にずれる。

図１８の（Ｃ）の状況では、観測値と、その観測値と対応するはずの位置推定分布の距離ははなれ、むしろ、その観測値と対応しないはずの位置推定分布との距離が近くなり、結果的に、観測値による位置推定分布の方向はさらに、誤った方向になり、さらに位置推定誤差が増大する。

このように、非特許文献１の方式では、特に対象物が接近した状況では位置推定の誤りが起こりやすく、その誤りに起因して、真に人が移動した経路とはまったく異なる経路を推定してしまうという課題を有す。

図１９は、図１７で示した２人の人Ａ，Ｂの移動に対して非特許文献１の方式に基づいて位置推定した結果、２人の人Ａ，Ｂの移動経路１７０２，１７０３とは全く異なる移動経路を誤って推定した結果を示す図である。

図１８に示したように、２人の人Ａ，Ｂが接近し交差する付近での位置推定誤差の増大により、Ａ，Ｂのそれぞれの人の位置推定分布の移動履歴は交差せずに、中心付近で誤った方向に移動してしまった結果である。

図１８の（Ｃ）の状況からさらに先に進むと、対象物の識別情報を出力する無線タグ観測値に関しても、観測値が出力する位置情報と、その観測値の識別情報に対応する対象物の位置推定分布との距離も大きく離れ、反対に、当該観測値の識別情報に対応しない対象物の位置推定分布に近くなるという状況が起こる。

非特許文献１で開示される方式では、観測値の位置情報で対象物の位置情報の推定値を更新する際の重み（アソシエーション値）の計算を、距離尤度（観測値の位置情報と対象物の位置推定分布との近さ）とＩＤ尤度の積に基づいて計算するため、ＩＤ尤度における重みが、距離尤度によって、割り引かれてしまう。そのため、対象物の識別性能が高く位置推定誤差の訂正に寄与するはずの無線タグ観測値の識別情報が、位置推定分布の推定誤差の訂正に有効に寄与しない。よって、一度、位置推定分布が真の人の位置から大きくずれると、そのずれが増大し結果的にまったく異なる推定結果を導くという課題を有す。

さらに、無線タグの観測頻度が、カメラの観測頻度より低い場合は、時間の観点で見たときの無線タグの観測値の対象物の推定位置へ更新の寄与はさらに弱まるため、推定誤差の訂正が起こりにくいという課題を有す。

ならびに、非特許文献１では、カメラ観測値が常に人の数だけ得られている場合の方式のみ開示されており、カメラ観測値が人数分得られないケース、つまり、カメラ観測値の消失又はカメラ観測値が実際の人数よりも多く得られるケース場合の対応について記載はない。カメラによる人認識処理では、遮蔽物により観測対象の人が遮られる、接近した２人を１人と検知するなどの理由により、カメラの観測値が人数分得られないことが起こりうる。また、人ではない物体を人であると誤認識することで、カメラの観測値が人数分よりも多く得られる場合も起こりうる。しかしながら、非特許文献１ではこのようなケースにおける対応方法が開示されておらず、仮にカメラ観測値が消失し、観測値の個数が人数に満たない場合は、１つのカメラ観測値をその観測値に対応しない人も含めて複数の人の位置推定分布の更新に同時に反映させることになり、誤った推定結果を招くという課題を有す。

従って、本発明の目的は、上記問題を解決することにあって、移動体がすれ違うなどの観測値と対象物の対応関係が曖昧になるシーンでも、位置推定精度の低下を招くこと無く、かつ、対象物の移動経路の推定誤りを引き起こさない、移動体位置推定装置、移動体位置推定方法、及び、移動体位置推定プログラムを提供することにある。

前記課題を解決するために、本発明は以下のように構成される。

本発明の１つの態様によれば、観測空間に存在する複数の移動体を観測して、移動体毎に逐次得られる前記移動体の識別情報及び前記移動体の位置情報を含む観測値を入力として、観測対象である前記複数の移動体の識別情報、及び、位置情報を推定する移動体位置推定装置であって、
前記移動体の識別情報と位置情報を観測値として所定時間間隔毎に出力する第１の観測手段と、
前記移動体の位置情報を観測値として、前記第１の観測手段の間隔とは異なる時間間隔で出力する第２の観測手段と、
前記第１の観測手段と前記第２の観測手段のそれぞれの前記時間間隔のうちの前記第１の観測手段の前記観測値が得られた時刻における、前記第１の観測手段の前記観測値の位置情報と前記第２の観測手段の前記観測値の位置情報の相対距離の合計、及び、前記第１観測手段の前記観測値が得られた時刻での、前記第２観測手段の前記観測値への識別情報の割り当てに基づいて、前記第１観測手段の観測値が得られない時刻における、前記第２観測手段の前記観測値に対する識別情報の割り当ての考えうる組み合わせの中で、前記第１の観測手段の前記観測値が得られた時刻から次の前記第１の観測手段の前記観測値が得られた時刻までの前記第２の観測手段の観測値の移動量の合計が最小になるように、前記第２の観測手段の観測値に対して前記第１の観測手段から得られた識別情報を割り当てる識別情報割り当て手段と、
前記第１の観測手段の観測値、及び、前記識別情報割り当て手段によって割り当てられた前記識別情報を含む前記第２の観測手段の観測値に基づいて、前記移動体の位置を推定する位置推定手段を、
有する移動体位置推定装置を提供する。

本発明の別の態様によれば、観測空間に存在する複数の移動体を観測して、移動体毎に逐次得られる前記移動体の識別情報及び前記移動体の位置情報を含む観測値を入力として、観測対象である前記複数の移動体の識別情報、及び、位置情報を推定する移動体位置推定方法であって、
前記移動体の識別情報と位置情報を観測値として所定時間間隔毎に第１の観測手段により出力し、
前記移動体の位置情報を観測値として、前記第１の観測手段の間隔とは異なる時間間隔で第２の観測手段により出力し、
前記第１の観測手段と前記第２の観測手段のそれぞれの前記時間間隔のうちの前記第１の観測手段の前記観測値が得られた時刻における、前記第１の観測手段の前記観測値の位置情報と前記第２の観測手段の前記観測値の位置情報の相対距離の合計、及び、前記第１観測手段の前記観測値が得られた時刻での、前記第２観測手段の前記観測値への識別情報の割り当てに基づいて、前記第１観測手段の観測値が得られない時刻における、前記第２観測手段の前記観測値に対する識別情報の割り当ての考えうる組み合わせの中で、前記第１の観測手段の前記観測値が得られた時刻から次の前記第１の観測手段の前記観測値が得られた時刻までの前記第２の観測手段の観測値の移動量の合計が最小になるように、前記第２の観測手段の観測値に対して前記第１の観測手段から得られた識別情報を識別情報割り当て手段により割り当て、
前記第１の観測手段の観測値、及び、前記識別情報割り当て手段によって割り当てられた前記識別情報を含む前記第２の観測手段の観測値に基づいて、前記移動体の位置を位置推定手段により推定する、
移動体位置推定方法を提供する。

本発明のさらに別の態様によれば、観測空間に存在する複数の移動体を観測して、移動体毎に逐次得られる前記移動体の識別情報及び前記移動体の位置情報を含む観測値を入力として、観測対象である前記複数の移動体の識別情報、及び、位置情報を推定する移動体位置推定プログラムであって、
前記移動体の識別情報と位置情報を観測値として所定時間間隔毎に出力する第１の観測手段と、前記移動体の位置情報を観測値として、前記第１の観測手段の間隔とは異なる時間間隔で出力する第２の観測手段とのそれぞれの前記時間間隔のうちの前記第１の観測手段の前記観測値が得られた時刻における、前記第１の観測手段の前記観測値の位置情報と前記第２の観測手段の前記観測値の位置情報の相対距離の合計、及び、前記第１観測手段の前記観測値が得られた時刻での、前記第２観測手段の前記観測値への識別情報の割り当てに基づいて、前記第１観測手段の観測値が得られない時刻における、前記第２観測手段の前記観測値に対する識別情報の割り当ての考えうる組み合わせの中で、前記第１の観測手段の前記観測値が得られた時刻から次の前記第１の観測手段の前記観測値が得られた時刻までの前記第２の観測手段の観測値の移動量の合計が最小になるように、前記第２の観測手段の観測値に対して前記第１の観測手段から得られた識別情報を割り当てる識別情報割り当て手段と、
前記第１の観測手段の観測値、及び、前記識別情報割り当て手段によって割り当てられた前記識別情報を含む前記第２の観測手段の観測値に基づいて、前記移動体の位置を推定する位置推定手段と
の機能をコンピュータに実現させるための移動体位置推定プログラムを提供する。

本発明の移動体位置推定装置、移動体位置推定方法、及び、移動体位置推定プログラムは、観測対象物の識別能力が高いセンサの識別情報に基づいて、観測対象物の識別能力を持たない（あるいは識別能力が低い）センサの観測値への妥当な識別情報の割り当てを行うことができるので、観測値と観測対象物との対応関係を常に確定できる。従って、移動体がすれ違うなどの観測値と対象物の対応関係が曖昧になるシーンでも、位置推定精度の低下を招くこと無く、かつ、対象物の移動経路の推定誤りを引き起こさないという格別の効果を有す。

また、本発明の移動体位置推定装置、移動体位置推定方法、及び、移動体位置推定プログラムは、観測エラーによる観測値の消失又は過観測が発生する状況においても、観測対象物の識別能力を持たない（あるいは識別能力が低い）センサの観測値への妥当な識別情報の割り当てを行うことができる。従って、実環境において常に観測対象が正しくセンシングされるとは限らない状況においても、位置推定精度の低下を招くことなく、かつ、対象物の移動経路の推定誤りを引き起こさないという格別の効果を有す。

本発明の第１実施形態の移動体位置推定装置の構成を示すブロック図。 本発明の第１実施形態の移動体位置推定装置に係る、人位置トラッキング装置の構成を示すブロック図。 本発明の第１実施形態に係る、人位置トラッキング装置で人の位置を検出する状況の例を示す図。 本発明の第１実施形態に係る、観測値バッファに記録された観測値の一例を示す図。 本発明の第１実施形態に係る、観測値バッファに記録された観測値の一例を示す図。 本発明の第１実施形態に係る、人位置推定値記録部バッファに記録された人位置情報の例を示す図。 本発明の第１実施形態に係る、人位置表示部に表示された人の位置、及び、移動軌跡の例を示す図。 本発明の第１実施形態に係る、人位置トラッキング処理のメイン処理のフローチャート。 本発明の第１実施形態に係る、ステップＳ１０５の処理方法を説明するための人の移動軌跡例を示す図。 本発明の第１実施形態に係る、ステップＳ１０５の処理方法を説明するためのＩＤ割り当て処理の対象となる観測値の例を示す図。 本発明の第１実施形態に係る、ステップＳ１０５の処理方法を説明するためのある時刻ｔにおける観測値へのＩＤ割り当ての定式化を示す図。 本発明の第１実施形態に係る、ステップＳ１０５の処理方法を説明するためのある時刻ｔにおける観測値へのＩＤ割り当ての組み合わせを示す図。 本発明の第１実施形態に係る、ステップＳ１０５の処理のＩＤ割り当て処理が最小コスト経路探索問題となることの説明図。 本発明の第１実施形態に係る、ステップＳ１０５の処理のＩＤ割り当て処理の最小コスト経路探索問題をＡ＊アルゴリズムで解く場合の予測コスト設定を説明する図。 本発明の第１実施形態に係る、ステップＳ１０５の処理のＩＤ割り当て処理の最小コスト経路探索問題をＡ＊アルゴリズムで解く場合の各ノードに格納される情報の例を示す図。 本発明の第１実施形態に係る、ステップＳ１０５の処理のＩＤ割り当て処理の最小コスト経路探索問題をＡ＊アルゴリズムで解く場合の開始ノードの作成と、目標ノードの作成に関わる動作のフローチャート。 本発明の第１実施形態に係る、ステップＳ１０５の処理のＩＤ割り当て処理の最小コスト経路探索問題をＡ＊アルゴリズムで解く場合の、最小コスト経路を導出する手順のフローチャート。 本発明の第１実施形態に係る、ステップＳ１０５の処理のＩＤ割り当て処理の最小コスト経路探索問題でＩＤ割り当ての仮説ノードを枝刈りする方法を説明する図。 本発明の第１実施形態に係る、ステップＳ１０５の処理のＩＤ割り当て処理の最小コスト経路探索問題でＩＤ割り当ての仮説ノードを枝刈りする方法を説明する図。 従来技術における人位置推定の課題を説明するための２つの移動体の移動軌跡の例を示す図。 従来技術での課題を説明するための２つの移動体と観測値の位置関係において人位置推定間違いの原因を示す図。 従来技術における人位置移動経路の推定間違いの例を示す図。 本発明の第２実施形態に係る、人位置トラッキング装置の構成を示すブロック図。 本発明の第２実施形態に係る、人位置トラッキング処理のメイン処理のフローチャート。 本発明の第２実施形態に係る、ステップＳ１０５Ａ内のＩＤ割り当て仮説を生成する処理の流れを示すフローチャート。 本発明の第２実施形態に係る、観測値移動可能性判定部の処理内容に関わる人認識カメラ観測値の例を示す図。 本発明の第２実施形態に係る、観測値移動可能性判定部の判定結果の例を示す図。 本発明の第２実施形態に係る、仮想観測値生成部の処理内容の例を示す図。 本発明の第２実施形態に係る、ＩＤの割り当て制御部が観測値に対してＩＤを割り当てた例を示す図。 本発明の第２実施形態に係る、ＩＤの割り当て仮説のノード表現を表す図。 本発明の第１実施形態における人位置移動経路の推定結果の例を示す図。

以下に、本発明にかかる実施の形態を図面に基づいて詳細に説明する。

以下、図面を参照して本発明における実施形態を詳細に説明する前に、本発明の種々の態様について説明する。

本発明の第１態様によれば、観測空間に存在する複数の移動体を観測して、移動体毎に逐次得られる前記移動体の識別情報及び前記移動体の位置情報を含む観測値を入力として、観測対象である前記複数の移動体の識別情報、及び、位置情報を推定する移動体位置推定装置であって、
前記移動体の識別情報と位置情報を観測値として所定時間間隔毎に出力する第１の観測手段と、
前記移動体の位置情報を観測値として、前記第１の観測手段の間隔とは異なる時間間隔で出力する第２の観測手段と、
前記第１の観測手段と前記第２の観測手段のそれぞれの前記時間間隔のうちの前記第１の観測手段の前記観測値が得られた時刻おける、前記第１の観測手段の前記観測値の位置情報と前記第２の観測手段の前記観測値の位置情報の相対距離の合計、及び、前記第１観測手段の前記観測値が得られた時刻での、前記第２観測手段の前記観測値への識別情報の割り当てに基づいて、前記第１観測手段の観測値が得られない時刻における、前記第２観測手段の前記観測値に対する識別情報の割り当ての考えうる組み合わせの中で、前記第１の観測手段の前記観測値が得られた時刻から次の前記第１の観測手段の前記観測値が得られた時刻までの前記第２の観測手段の観測値の移動量の合計が最小になるように、前記第２の観測手段の観測値に対して前記第１の観測手段から得られた識別情報を割り当てる識別情報割り当て手段と、
前記第１の観測手段の観測値、及び、前記識別情報割り当て手段によって割り当てられた前記識別情報を含む前記第２の観測手段の観測値に基づいて、前記移動体の位置を推定する位置推定手段を、
有する移動体位置推定装置を提供する。

本発明の第２態様によれば、前記第１観測手段は、前記移動体の識別情報を含みかつ前記移動体が有する無線タグから発信される電波を受信して前記移動体の識別情報と位置情報を観測値として出力する無線タグ観測部、又は、前記移動体に超音波を発信して反射した超音波を受信して前記移動体の識別情報と位置情報を観測値として出力する超音波センサであり、
前記第２観測手段は、画像を撮像して前記移動体の位置情報を観測値として出力するカメラ、又は、前記移動体にミリ波を発信して反射したミリ波を受信して前記移動体の位置情報を観測値として出力するミリ波送受信器、又は、前記観測空間の床面に複数配置して前記移動体の位置情報を観測値として出力する圧力センサである、第１の態様に記載の移動体位置推定装置を提供する。

本発明の第３態様によれば、前記識別情報割り当て手段において、前記観測間隔の時間内に前記移動体が移動可能な移動可能範囲外にある前記第２観測手段の観測値を削除して、前記観測間隔の時間内に前記移動体が移動可能な移動可能範囲内にある前記第２観測手段の観測値を基に、識別情報割り当て処理を行うときに対する前回の前記第２観測手段の観測値の位置から移動不可能な、前記識別情報割り当てを行うときの観測値を特定し、又は、前記識別情報割り当てを行うときの前記第２観測手段の観測値の位置へ移動可能なものが１つもない、前記識別情報割り当てを行うときに対する前回の観測値を特定し、特定された前記第２の観測手段の観測値に対して前記第１の観測手段から得られた識別情報を割り当てる、第１又は２の態様に記載の移動体位置推定装置を提供する。

本構成によって、第２観測手段において想定される観測エラー、たとえば、観測対象の隠蔽又は、観測対象同士の接近等に起因する観測値の消失、あるいは、観測対象以外の物体を観測対象だと誤検知することに起因する誤観測値の発生の状況を把握することができ、第２の観測手段の観測値のうち、第１の観測手段から得られた識別情報を割り当てるべきものとそうではないものとを区別可能である。従って、観測エラーに起因する誤った識別情報の割り当てを回避でき、観測エラーによる最終的な移動体の位置推定結果の精度低下を防ぐことができる。

本発明の第４態様によれば、前記識別情報割り当て手段は、前記識別情報割り当てを行うときの前記第２観測手段の観測値の位置へ移動可能なものが１つもないと特定された、前記識別情報割り当てを行うときに対する前回の観測値に対して、前記当該観測値の移動履歴に基づいて、前記識別情報割り当てを行うときの移動先位置を予測し、前記予測位置に仮想的な観測値を生成する仮想観測値生成部を備える、第３の態様に記載の移動体位置推定装置を提供する。

本構成によって、当該観測対象の移動履歴から妥当であると思われる位置に観測対象が移動したと仮定して、仮想的に観測値を生成し、第１観測手段から得られた識別情報を割り当てることにより、観測対象が隠蔽されるなどの原因で、ある観測時刻において当該観測対象の第２の観測手段の観測値が得られなかった場合にも、当該観測対象の移動経路を推定することが可能である。

本発明の第５態様によれば、前記識別情報割り当て手段において、前記観測対象の前記移動体の前記移動可能範囲を、当該移動体の移動能力の計測結果に基づく最大の移動速度と、前記観測間隔の積を半径とする円の範囲と設定する第３又は４の態様に記載の移動体位置推定装置を提供する。

本発明の第６態様によれば、前記識別情報割り当て手段において、前記観測対象の前記移動体の前記移動可能範囲を、前記第２の観測手段の観測値の移動履歴の、前記識別情報割り当てを行うときに対する前回の観測値の位置と、前記識別情報割り当てを行うときに対する前々回の観測値の位置から算出される移動速度と移動方向とが維持されると仮定して得られる移動先位置を中心とした所定の半径を持つ円と定める、第３又は４の態様に記載の移動体位置推定装置を提供する。

本発明の第７態様によれば、観測空間に存在する複数の移動体を観測して、移動体毎に逐次得られる前記移動体の識別情報及び前記移動体の位置情報を含む観測値を入力として、観測対象である前記複数の移動体の識別情報、及び、位置情報を推定する移動体位置推定方法であって、
前記移動体の識別情報と位置情報を観測値として所定時間間隔毎に第１の観測手段により出力し、
前記移動体の位置情報を観測値として、前記第１の観測手段の間隔とは異なる時間間隔で第２の観測手段により出力し、
前記第１の観測手段と前記第２の観測手段のそれぞれの前記時間間隔のうちの前記第１の観測手段の前記観測値が得られた時刻における、前記第１の観測手段の前記観測値の位置情報と前記第２の観測手段の前記観測値の位置情報の相対距離の合計、及び、前記第１観測手段の前記観測値が得られた時刻での、前記第２観測手段の前記観測値への識別情報の割り当てに基づいて、前記第１観測手段の観測値が得られない時刻における、前記第２観測手段の前記観測値に対する識別情報の割り当ての考えうる組み合わせの中で、前記第１の観測手段の前記観測値が得られた時刻から次の前記第１の観測手段の前記観測値が得られた時刻までの前記第２の観測手段の観測値の移動量の合計が最小になるように、前記第２の観測手段の観測値に対して前記第１の観測手段から得られた識別情報を識別情報割り当て手段により割り当て、
前記第１の観測手段の観測値、及び、前記識別情報割り当て手段によって割り当てられた前記識別情報を含む前記第２の観測手段の観測値に基づいて、前記移動体の位置を位置推定手段により推定する、
移動体位置推定方法を提供する。

本発明の第８態様によれば、前記移動体の識別情報と位置情報を含みかつ前記第１の観測手段から出力された第１の観測値と、前記移動体の位置情報を含みかつ前記第２の観測手段から出力された第２の観測値とが同時に得られた第１の時刻から、次に、前記第１の観測値と前記第２の観測値が同時に得られた第２の時刻の間に得られた第２の観測値に対して、
前記第１の時刻において、前記第１の観測値の位置と、前記第２の観測値の位置の距離の合計が最も短い組み合わせを前記識別情報割り当て手段で求め、
前記第１の時刻における組み合わせに基づいて、前記第１の観測値の識別情報を前記第２の観測値の識別情報として前記識別情報割り当て手段で割り当て、
さらに、前記第２の時刻において、前記第１の観測値の位置と、前記第２の観測値の位置の距離の合計が最も短い組み合わせを前記識別情報割り当て手段で求め、
前記第２の時刻における組み合わせにおいて前記第１の観測値の識別情報を前記第２の観測値の識別情報として前記識別情報割り当て手段で割り当て、
前記識別情報割り当て手段において、前記第１の時刻における前記第２の観測値への識別情報の割り当て状態を開始ノードとし、また、前記第２の時刻における前記第２の観測値への識別情報の割り当て状態を目標ノードとし、前記第１の時刻と前記第２の時刻の間の前記第２の観測値への識別情報の割り当ての組み合わせを、前記開始ノードと前記目標ノードの間に位置するノードとすることにより得られる探索グラフにおいて、前記ノード間の遷移コストを前記第２の観測値の移動量の合計と置くことで、前記探索グラフの前記開始ノードから前記目標ノードに至る最小コスト経路を求めることにより、前記第１の時刻から前記第２の時刻までの前記第２の観測値に前記第１の観測値の識別情報を前記識別情報割り当て手段で割り当てる、
第７の態様に記載の移動体位置推定方法を提供する。

本発明の第９態様によれば、前記探索グラフ上の探索処理において、前記識別情報割り当て手段により、ある観測時刻における識別情報の割り当て状態に相当するノードから、次の観測時刻への割り当て状態の仮説に相当するノードを生成する場合に、前記観測対象の前記移動体の移動可能範囲を考慮し、前記観測間隔の時間内に移動可能な前記第２観測手段の観測値の対応関係を判定し、探索処理を行うときに対する前回の前記第２観測手段の観測値の位置から移動不可能な、前記探索処理を行うときの観測値を特定することで、前記探索処理を行うときに対する前回の観測値の位置から移動不可能な観測値を含んだ割り当ての仮説ノードを生成しない、第８の態様に記載の移動体位置推定方法を提供する。

本発明の第１０態様によれば、前記探索グラフ上の探索処理において、前記識別情報割り当て手段により、ある観測時刻における識別情報の割り当て状態に相当するノードから、次の観測時刻への割り当て状態の仮説に相当するノードを生成する場合に、前記観測対象の前記移動体の移動可能範囲を考慮し、探索処理を行うときの観測値の位置へ移動可能なものが１つもないと特定された、前記探索処理を行うときに対する前回の観測値に対して、前記当該観測値の移動履歴に基づいて前記探索処理を行うときの移動先位置を予測し、前記予測位置に仮想的な観測値を生成し、前記仮想的な観測値に前記当該の観測値が移動したとする仮説ノードを生成する、第８又は９の態様に記載の移動体位置推定方法を提供する。

本発明の第１１態様によれば、観測空間に存在する複数の移動体を観測して、移動体毎に逐次得られる前記移動体の識別情報及び前記移動体の位置情報を含む観測値を入力として、観測対象である前記複数の移動体の識別情報、及び、位置情報を推定する移動体位置推定プログラムであって、
前記移動体の識別情報と位置情報を観測値として所定時間間隔毎に出力する第１の観測手段と、前記移動体の位置情報を観測値として、前記第１の観測手段の間隔とは異なる時間間隔で出力する第２の観測手段とのそれぞれの前記時間間隔のうちの前記第１の観測手段の前記観測値が得られた時刻における、前記第１の観測手段の前記観測値の位置情報と前記第２の観測手段の前記観測値の位置情報の相対距離の合計、及び、前記第１観測手段の前記観測値が得られた時刻での、前記第２観測手段の前記観測値への識別情報の割り当てに基づいて、前記第１観測手段の観測値が得られない時刻における、前記第２観測手段の前記観測値に対する識別情報の割り当ての考えうる組み合わせの中で、前記第１の観測手段の前記観測値が得られた時刻から次の前記第１の観測手段の前記観測値が得られた時刻までの前記第２の観測手段の観測値の移動量の合計が最小になるように、前記第２の観測手段の観測値に対して前記第１の観測手段から得られた識別情報を割り当てる識別情報割り当て手段と、
前記第１の観測手段の観測値、及び、前記識別情報割り当て手段によって割り当てられた前記識別情報を含む前記第２の観測手段の観測値に基づいて、前記移動体の位置を推定する位置推定手段と
の機能をコンピュータに実現させるための移動体位置推定プログラムを提供する。

本発明の第１２態様によれば、前記移動体の識別情報と位置情報を含みかつ前記第１の観測手段から出力された第１の観測値と、前記移動体の位置情報を含みかつ前記第２の観測手段から出力された第２の観測値とが同時に得られた第１の時刻から、次に、前記第１の観測値と前記第２の観測値とが同時に得られた第２の時刻の間に得られた第２の観測値に対して、
前記第１の時刻において、前記第１の観測値の位置と、前記第２の観測値の位置の距離の合計が最も短い組み合わせを前記識別情報割り当て手段で求める機能と、
前記第１の時刻における組み合わせに基づいて、前記第１の観測値の識別情報を前記第２の観測値の識別情報として前記識別情報割り当て手段で割り当てる機能と、
さらに、前記第２の時刻において、前記第１の観測値の位置と、前記第２の観測値の位置の距離の合計が最も短い組み合わせを前記識別情報割り当て手段で求める機能と、
前記第２の時刻における組み合わせにおいて前記第１の観測値の識別情報を前記第２の観測値の識別情報として前記識別情報割り当て手段で割り当てる機能と、
前記第１の時刻における前記第２の観測値への識別情報の割り当て状態を開始ノードとし、また、前記第２の時刻における前記第２の観測値への識別情報の割り当て状態を目標ノードとし、前記第１の時刻と前記第２の時刻の間の前記第２の観測値への識別情報の割り当ての組み合わせを、前記開始ノードと前記目標ノードの間に位置するノードとすることにより得られる探索グラフにおいて、前記ノード間の遷移コストを前記第２の観測値の移動量の合計と置くことで、前記探索グラフの前記開始ノードから前記目標ノードに至る最小コスト経路を求めることにより、前記第１の時刻から前記第２の時刻までの前記第２の観測値に前記第１の観測値の識別情報を前記識別情報割り当て手段で割り当てる機能と、
をコンピュータに実現させるための、第１１の態様に記載の移動体位置推定プログラムを提供する。

以下に、本発明にかかる実施形態を図面に基づいて詳細に説明する。

（第１実施形態）
図１は、本発明の第１実施形態にかかる移動体位置推定装置の構成を示すブロック図である。移動体位置推定装置は、第１観測手段１０１と、第２観測手段１０２と、識別情報割り当て手段１０３と、位置推定手段１０４とを備えて構成している。

第１観測手段１０１は、複数の観測対象を区別する識別情報と、観測対象の位置情報を識別情報割り当て手段１０３と位置推定手段１０４とに出力する。

第２観測手段１０２は、第１観測手段１０１と同じ観測対象に対して、位置情報のみを識別情報割り当て手段１０３に出力する。

識別情報割り当て手段１０３は、対象物の識別精度が高い第１観測手段１０１の各観測値と、対象物の識別ができない第２観測手段１０２の各観測値の対応関係を、対応させる観測値の距離の合計が最も小さくなるように対応付ける。この識別情報割り当て手段１０３での対応関係に基づき、第１観測手段１０１の観測値が得られた時刻において、第２観測手段１０２の観測値への識別情報の割り当てを識別情報割り当て手段１０３で行う。さらに、識別情報割り当て手段１０３は、第１観測手段１０１の観測値が得られた時刻での、第２観測手段１０２の観測値への識別情報の割り当てに基づいて、第１観測手段１０１の観測値が得られない時刻における、第２観測手段１０２の観測値に対する識別情報の割り当ての考えうる組み合わせの中で、観測時刻毎の第２観測手段１０２の識別情報を割り当てられた観測値の移動量の合計が最小になるような、識別情報の割り当ての組み合わせを決定し、第１観測手段１０１の観測値が得られない時刻における、第２観測手段１０２の観測値へも識別情報の割り当てを行う。識別情報割り当て手段１０３で得られた情報は、位置推定手段１０４に出力される。

位置推定手段１０４は、第１観測手段１０１の観測値に含まれる識別情報及び位置情報、そして、識別情報割り当て手段１０３により識別情報が割り当てられた第２観測手段１０２の観測値に含まれる識別情報及び位置情報に基づいて、観測値が得られた時刻毎に、観測対象物の推定位置を更新することで、移動する対象物の位置を捕捉する。

このような構成によって、識別情報割り当て手段１０３において、第１観測手段１０１の観測値の識別情報に基づいて、第２観測手段１０２の観測値には含まれない識別情報を割り当てる。このように構成することにより、従来技術における識別情報を近似する方式では、観測値同士が接近する状況になると、識別情報を持たないセンサの識別情報が曖昧になるという問題を回避することができる。従って、識別情報が曖昧になることに起因する対象物の位置推定誤差の増大、さらには、位置推定誤差の増大による、観測対象物の移動経路の推定間違いという問題を、この第１実施形態では回避することができる。

また、識別情報割り当て手段１０３においては、第１の観測手段１０１の観測値の観測頻度が、第２の観測手段１０２の観測頻度よりも低い場合においても、第２の観測手段１０２の観測値への識別情報の割り当てを行うことが可能なので、従来技術では、観測頻度が低いセンサの情報が位置推定の誤りを補正する方向に働きにくいという問題も、同時に回避することができる。

図２は、本発明の第１実施形態における移動体位置推定装置の具体的な一例としての人位置トラッキング装置９０の構成を示すブロック図である。人位置トラッキング装置９０は、第１の観測手段の一例としての無線タグ観測部２０１と、第２の観測手段の一例としての人認識カメラ２０２と、識別情報割り当て手段の一例としてのＩＤ割り当て処理部２０５と、位置推定手段の一例としての人位置推定部２０６とを備えるように構成している。人位置トラッキング装置９０は、さらに、観測値取得部２０３と、観測値バッファ２０４と、人位置推定値記録部２０７と、人位置表示部２０８とを備えている。

無線タグ観測部２０１は、観測対象である人が所持する無線タグから発信される電波を複数のアンテナで受信し、アンテナ毎の到達時間の差からタグの位置を測位する。同時に、無線タグ観測部２０１は、個々の無線タグに割り当てられた固有のＩＤ情報を、タグの位置の情報と組み合わせ、観測値として観測値取得部２０３に出力する。

人認識カメラ２０２は、観測対象である人と人以外のものとを識別することにより、撮像範囲内に存在する人の位置を観測値として観測値取得部２０３に出力する。

図３に、第１実施形態における人位置トラッキング装置９０の無線タグ観測部２０１と人認識カメラ２０２とを、観測空間９１に配置した例を示す。観測対象となる人８９は、無線タグ（無線発信器）３０１，３０２，３０３を所持することで、無線タグ３０１，３０２，３０３から発信される電波を、無線タグ観測部２０１が受信することにより、無線タグ３０１，３０２，３０３の位置と識別情報とが無線タグ観測部２０１で観測される。また、人認識カメラ２０２の撮像範囲が観測対象となる観測空間９１の床面９１ｆの全体を含むように、天井から床面方向に向けて人認識カメラ２０２を配置することで、人認識カメラ２０２により、床面９１ｆ上を移動する人８９の位置を観測する。

観測値取得部２０３は、内部に無線タグ観測部２０１及び人認識カメラ２０２の観測値をそれぞれ取得するタイミングを管理するタイマ２０３Ａを持っている。観測値取得部２０３は、無線タグ観測部２０１の観測周期としてタイマ２０３Ａで設定された時間間隔、及び、人認識カメラ２０２の観測周期としてタイマ２０３Ａで設定された時間間隔で、それぞれの観測値を取得し、観測値バッファ２０４に追加記録する。

観測値バッファ２０４は、観測値取得部２０３が取得した観測値を観測時刻毎に記録する。図４Ａ及び図４Ｂに、観測値バッファ２０４に記録される観測値の例を示す。

図４Ａ及び図４Ｂでは、観測対象の人が３人存在する場合の例で、各観測時刻において、無線タグ観測値及び人認識カメラ観測値は、それぞれ３個ずつ記録される。１つの観測値は、位置情報とＩＤ情報とで構成される。位置情報は、観測空間９１内に人８９の存在する座標である。第１実施形態では、人８９の存在する空間９１の床面９１ｆの平面における２次元座標を位置座標とした場合の例を示す。ＩＤ情報は、観測された人８９を互いに区別するための情報である。第１実施形態では、３人の人８９をそれぞれＡ，Ｂ，ＣというＩＤ情報の値で区別するものとする。

観測値バッファ２０４に記録される観測値は、無線タグ観測値が得られた時刻から次の無線タグ観測値が得られた時刻までの観測値とする。第１実施形態においては、人認識カメラ２０２の観測周期をΔｔとし、無線タグ観測部２０１の観測周期を４Δｔとする。すなわち、無線タグ観測部２０１と人認識カメラ２０２との観測頻度の比は、１：４であるとする。従って、図４Ａ及び図４Ｂでは観測時刻が０から４Δｔまでの５つの時刻の観測値が記録される。

図４Ａは、観測値取得部２０３が、無線タグ観測部２０１及び人認識カメラ２０２から観測値を取得した直後の状態を示す。無線タグ観測値には、位置情報とＩＤ情報とが含まれるが、人認識カメラ観測値には識別情報は含まれない。このため、図４Ａでは、人認識カメラ２０２の観測値のＩＤ情報は不定（Ｕｎｋｎｏｗｎ）を意味する「ＵＫ」という値である。

図４Ｂは、後述するＩＤ割り当て処理部２０５が、人認識カメラ観測値に対しＩＤ情報を割り当てた直後の状態を示す。図４Ｂでは、人認識カメラ２０２の観測値のＩＤ情報は、それぞれ割り当てられたＩＤ情報が格納されている。

ＩＤ割り当て処理部２０５は、観測値バッファ２０４に記録された観測値の内容を読み出し、無線タグ観測値の位置情報と人認識カメラ観測値の位置情報との対応関係、及び、人認識カメラ観測値の観測時刻毎の観測値の移動量の評価に基づいて、人認識カメラ観測値へのＩＤ識別情報の割り当てを決定し、割り当てられたＩＤ識別情報を観測値バッファ２０４の人認識カメラ観測値のＩＤ情報に書き込む。

人位置推定部２０６は、ＩＤ情報が書き込まれた状態の観測値バッファ２０４の観測値に基づいて、各観測時刻における人８９の位置を推定する。人位置推定部２０６は、その内部に、識別される人毎に推定位置を保持している。観測値と推定位置との対応関係は、観測値バッファ２０４の観測値の識別情報から直ちに分かるので、対応する観測値により人８９の推定位置を観測時刻毎に更新することで、移動する人８９の位置を推定し、人位置の推定値を人位置推定値記録部２０７に記録する。

人位置推定値記録部２０７には、人位置推定部２０６で推定された人８９の推定位置の情報が観測時刻毎に人８９の人数分だけ記録される。

図５に、人位置推定値記録部２０７に記録される人８９の位置の推定位置の例を示す。各観測値時刻におけるＩＤで区別された３人Ａ，Ｂ，Ｃの人８９の推定位置（２次元座標）が記録される。

人位置表示部２０８は、人位置推定値記録部２０７に記録された人位置の推定値の任意の時間範囲の人位置推定値、さらに、任意の人８９の人位置推定値を、指定して読み出し、各人の位置の軌跡を表示する。人位置表示部２０８は、例えばＣＲＴ又は液晶ディスプレイのようなディスプレイを含んだ装置として構成される。

図６に人位置表示部２０８の表示例を示す。ここでは、第１実施形態において３人の人（ＩＤ＝Ａ，Ｂ，Ｃ）６０４，６０５，６０６が移動しており、６０１、６０２、６０３で示される丸印と丸印間を結ぶ矢印線が３人それぞれの人６０４，６０５，６０６の移動軌跡を表す。最新の観測値時刻（ｔ＝４Δｔ）から７回前の観測時刻までの３人６０４，６０５，６０６それぞれの推定位置が丸印で表示されており、各観測時刻間の移動は、丸印を結ぶ矢印線で表される。

なお、人認識カメラ２０２で天井から床面方向に撮像した映像を人位置表示部２０８へ送り、人位置表示部２０８において、人認識カメラ２０２からの映像に、図６に示す人の移動軌跡６０１、６０２、６０３を重ねて表示する構成を取ることもできる。

図６の６０４、６０５、６０６は、人認識カメラ２０２で撮影した映像中に映る人Ａ，Ｂ，Ｃを示す。天井方向から床面方向の撮影なので、頭頂部分と肩が主に映る。

次に、図７に示すフロー図に基づいて、第１実施形態における人位置トラッキング装置９０の動作の流れについて説明する。

図７に示すステップＳ１０１からステップＳ１０７は、無線タグ観測部１０１及び人認識カメラ１０２から観測値をそれぞれ取得し、ＩＤ割り当て処理部１０３で人認識カメラ観測値へのＩＤ情報の割り当て処理を行い、ＩＤ情報割り当て済みの観測値に対して、人位置推定部２０６で人の位置推定を行うまでの動作フローに相当する。

まず、ステップＳ１０１では、観測値取得部２０３が、無線タグ観測部２０１及び人認識カメラ２０２のそれぞれにタイマー２０３Ａで設定した観測周期に同期した時刻において、無線タグ観測部２０１及び人認識カメラ２０２から観測値を取得する。観測周期に同期した時刻以外では、観測値の取得を行わない。第１実施形態においては、無線タグ２０１の観測周期は、人認識カメラ２０２の観測周期の４倍に設定しているので、無線タグ観測値と人認識カメラ観測値とが同時に取得できるときと、人認識カメラ観測値のみ取得できるときとがある。

次いで、ステップＳ１０２では、観測値取得部２０３が、ステップＳ１０１で取得した観測値を観測値バッファ２０４に記録する。

次いで、ステップＳ１０３では、ステップＳ１０１で無線タグ観測値が取得できたかどうかをＩＤ割り当て処理部２０５で判断する。ステップＳ１０１で無線タグ観測値が取得できなかったと観測値取得部２０３で判断した場合は、フローはステップＳ１０４へ進む。ステップＳ１０４では、人認識カメラの観測周期であるΔｔ時間ウェイトし、ステップＳ１０１へ戻る。

ステップＳ１０１において、無線タグ観測値が取得できたと観測値取得部２０３で判断した場合は、ステップＳ１０５へ進む。この場合は、図４Ａに示した例と同様に、観測値バッファ２０４には、そのときの観測時刻と前回の観測値時刻との間に観測された無線タグ観測値及び人認識カメラ観測値が記録されている。

ステップＳ１０５では、ＩＤ割り当て処理部２０５が、観測値バッファ２０４に記録された無線タグ観測値と人認識カメラ観測値とに含まれる、位置情報及びＩＤ情報に基づいて、人認識カメラ観測値へのＩＤ情報の割り当て処理を行う。ステップＳ１０５の処理の詳細については、後ほど述べる。

次いで、ステップＳ１０６では、人位置推定部２０６が、観測値バッファ２０４に記録されている無線タグ観測値、及び、ステップＳ１０５でＩＤ情報が割り当てられた人認識カメラ観測値の位置情報、及び、ＩＤ情報に基づいて、人８９の位置の推定処理を行う。人位置推定部２０６の人位置推定の方法としては、既に提案されている逐次推定手法を用いることができる。例えば、オンラインベイズフィルタの一種であるＶａｒｉａｔｉｏｎａｌ Ｄａｔａ Ａｓｓｏｃｉａｔｉｏｎ Ｆｉｌｔｅｒ（ＶＡＤＡＦ）を用いることができる。

ここで、オンラインベイズフィルターに基づく位置推定処理を行うことで、無線タグ観測部２０１及び人認識カメラ２０２の観測値にそれぞれ含まれる位置情報の誤差特性、つまり、位置センサとしての観測値のバラツキを考慮した位置推定を行うことができ、観測値の位置情報をそのまま人の位置として取り扱うのに較べて、真の人の位置との誤差を低減できる。観測値の位置情報をそのまま人の位置としても、誤差が問題にならない場合には、位置推定処理を行わなくてもよく、本発明の効果を損なうものではない。

次いで、ステップＳ１０７では、人位置推定部２０６が推定した人位置の情報を、人位置推定値記録部２０７に書き込む。

以上が、人位置トラッキング装置９０の動作のフローである。

次に、ステップＳ１０５のＩＤ割り当て処理部２０５によるＩＤ割り当て処理の詳細について説明する。

まず、ステップＳ１０５のＩＤ割り当て処理が、組み合わせ探索問題の一種である最小コスト経路探索問題として定式化できることを説明する。

その後で、最小コスト経路探索問題を解く手順として、Ａ＊アルゴリズムが適用できることを述べ、Ａ＊アルゴリズムにより最小コスト経路を導出する手順を説明する。

最小コスト経路問題としてＩＤ割り当て処理を定式化する説明を行う。

図８は、以下の定式化の説明において、Ａ，Ｂ，ＣのＩＤ情報で区別される３人の人８９が実際に移動した軌跡８０１，８０２，８０３の例を示す図である。

図９は、図８の灰色の矩形領域８０４を３人の人が移動した際に得られた観測値の例を示す図である。Ａ、Ｂ，Ｃの人の無線タグの観測値は、それぞれ、黒く塗りつぶされた正方形と、内部が斜線模様の正方形と、正方形とで示す。黒く塗りつぶされた丸は、人認識カメラ２０２の観測値である。

図９において、点線の矩形で囲んだ領域９１０に含まれる観測値は、それぞれ、同じ観測時刻に得られた観測値である。５つの点線矩形領域９１０は、無線タグ観測値が得られた時刻ｔ＝０から、次の無線タグ観測値が得られた時刻ｔ＝４Δｔまでの各観測時刻に同時に得られた観測値である。ここで、人認識カメラ観測値へＩＤ情報をＩＤ割り当て処理部２０５で割り当てるということは、各観測時刻ごとに３つある人認識カメラ２０２の観測値の各々にＡ，Ｂ，ＣのどのＩＤ情報をＩＤ割り当て処理部２０５で割り当てるかということである。このことを定式化するために、各観測時刻の３つの観測値を区別するために観測値のインデックスとして変数ｉを用いる。ｉ＝１は１番目、ｉ＝２は２番目、ｉ＝３は３番目の観測値を指す。次に、ｉ番目の観測値に対するＩＤ情報の割り当ての定式化のために変数ＩＤ_ｉ（ｔ）を用いる。例えば、ＩＤ_ｉ（ｔ）＝Ａは時刻ｔのｉ番目の観測値にＩＤ情報Ａが割り当てられていることを表す。

次に、後に最小コスト経路問題として定式化するために観測値の位置情報（座標）を用いるので、ｘ_ｉ（ｔ）を時刻ｔのｉ番目の観測値の座標とする。

ここで述べた、観測値とｉ， ＩＤ_ｉ（ｔ）， ｘ_ｉ（ｔ）の定式化に関しては、観測値との対応関係を図１０に示す。

図１１は、３人の人がいるときに得られる３つの人認識カメラ観測値へのＩＤ情報の割り当ての組み合わせを示す図である。３つの観測値へのＡ，Ｂ，ＣのＩＤ情報の割り当ての組み合わせは３！＝６通りあることがわかる。ｎ人の場合は、ｎ！通りである。

まず、無線タグ観測値が得られる時刻ｔ＝０、及び、時刻ｔ＝４Δｔにおいては、これら６通りの人認識カメラの観測値へのＩＤ割り当ての組み合わせの中で最適な組み合わせを決定することができる。無線タグ観測値の位置と、その無線タグ観測値のＩＤ情報と同じＩＤ情報が割り当てられた人認識カメラ観測値の位置の距離の合計（ここでは観測値間の３つの距離の合計）が最も小さくなるような、ＩＤ割り当ての組み合わせをＩＤ割り当て処理部２０５で選ぶ。例えば、ｔ＝０においては、図９の観測値９０１にはＩＤ情報Ａを割り当て、観測値９０２にはＩＤ情報Ｂを割り当て、観測値９０３にはＩＤ情報Ｃを割り当てるという組み合わせをＩＤ割り当て処理部２０５で選べばよい。ｔ＝４Δｔにおいては、観測値９０４にはＩＤ情報Ｂを割り当て、観測値９０５にはＩＤ情報Ｃを割り当て、観測値９０６にはＩＤ情報Ａを割り当てる組み合わせをＩＤ割り当て処理部２０５で選べばよい。

ここまでで、時刻ｔ＝０と時刻ｔ＝４Δｔでは、１つのＩＤ割り当ての組み合わせ、それ以外の時刻では６つのＩＤ割り当ての組み合わせが考えられることが分かる。

ここで、ｉ＝１〜３の観測値へのＩＤ情報の割り当て状態を、３つの横に並んだ矩形にＡ，Ｂ，Ｃを並べる順番で表現する。

図１２は、ｔ＝０からｔ＝４Δｔに到るまでの、人認識カメラ観測値へのＩＤ割り当ての組み合わせの探索空間を探索グラフで表した図である。各観測時刻における人認識カメラ観測値へのＩＤの割り当て状態が探索グラフのノードとなる。

ノード間を結ぶエッジは、観測時刻が１つ進む度に、エッジが始まるノードが表す観測値へのＩＤの割り当て状態から、エッジが終わるノードが表す観測値へのＩＤの割り当て状態に変化した時の遷移を表す。また各エッジにはＣｏｓｔというコスト値を割り当てる。Ｃｏｓｔを定義する式を以下に（式２）として掲げる。

（数２）

前記（式２）の左辺のＣｏｓｔ（ｔ，ｔ＋Δｔ）は、時刻ｔのｎ個の観測値、及び、次の観測時刻ｔ＋Δｔのｎ個の観測値にそれぞれあるＩＤが割り当てられているとし、そのＩＤ割当状態において、同じＩＤを持つ観測値間の距離を合計したものである。

図１２に示す探索グラフのｔ＝０のノードからエッジとノードを辿って、ｔ＝４Δｔのノードへ辿る経路のうち、Ｃｏｓｔの合計が最小となる経路（最小コスト経路）をＩＤ割り当て処理部２０５で考える。その経路上に存在するノードが表す観測値へのＩＤ割り当ては、観測時刻間での同じＩＤをもつ観測値の移動距離の合計が最も小さいＩＤ割り当てになる。すなわち、時刻ｔ＝０、ｔ＝４Δｔでは、無線タグ観測値のＩＤ情報との位置関係から最適な人認識カメラへのＩＤ割り当てがなされた上で、その制約を満たす中で、時刻ｔ＝０、ｔ＝４Δｔ以外の人認識カメラの観測値へは移動距離の観点から最適なＩＤ割り当てがＩＤ割り当て処理部２０５で求まる。従って、人認識カメラ観測値へのＩＤ割り当て問題が、ＩＤ割り当て処理部２０５で、最小コスト経路探索問題として定式化できる。

次に、ここまでで定式化した最小コスト経路探索問題を解く手順として、ＩＤ割り当て処理部２０５において、Ａ＊アルゴリズムが適用できることを説明する。Ａ＊アルゴリズムはコスト最小に基づいて探索問題を解くアルゴリズムの一種として知られており、探索の手順の中に予測コストを導入することにより、ダイクストラ法などの予測コストによらない方法よりも、効率的に最小コスト経路をＩＤ割り当て処理部２０５で導出できる。

以下、Ａ＊アルゴリズムの説明を行う。図１２に示すように、探索グラフの各エッジにコストが設定されているとし、探索グラフの中の任意のノードｎを経由しかつ最小コストとなる経路のコストをｆ（ｎ）とおくと、ｆ（ｎ）は、
ｆ（ｎ）＝ｇ（ｎ）＋ｈ（ｎ）
と表すことができる。ｇ（ｎ）は開始ノードからｎまでの最小コスト、ｈ（ｎ）はｎから目標ノードまでの最小コストである。ノードｎを通る経路でコストが最小の経路は未知であり、ｇ（ｎ），ｈ（ｎ），ｆ（ｎ）も未知である。よって、ｆ（ｎ）の推定値ｆ＊（ｎ）を考える。

ｆ＊（ｎ）＝ｇ＊（ｎ）＋ｈ＊（ｎ）
ここで、ｇ＊（ｎ）は開始ノードからｎまでの最小コストの推定値であり、ｈ＊（ｎ）はｎから目標ノードまでの最小コストの推定値である。ｇ＊（ｎ）については、開始ノードから探索を順次進めることにより、エッジに割り当てられたコストの積算から推定できるが、ｈ＊（ｎ）については、探索の途中で求めることはできない。従って、ｈ＊（ｎ）に対して適当な推定値を与え、ノードｎを経由する経路の推定コストｆ＊（ｎ）を評価し、より小さいｆ＊（ｎ）を与える経路を辿ることにより、最小コスト経路を探索するのがＡ＊アルゴリズムである。

ｈ＊（ｎ）はＡ＊アルゴリズムのヒューリスティクス関数と呼ばれ、すべてのｎについて、ｈ＊（ｎ）が、
ｈ＊（ｎ）≦ｈ（ｎ）
を満たすとき、Ａ＊アルゴリズムにより求められた開始ノードから目標ノードに至る経路が、最小コスト経路であることが保証される。逆に言うと、この式を満たすｈ＊（ｎ）が設定できれば、その問題はＡ＊アルゴリズムで解くことができる。

第１実施形態において、ｈ＊（ｎ）の設定方法を示す。時刻ΔｔにおけるＩＤ割り当て状態のノードのいずれかをｎとする。ｈ＊（ｎ）として、図１３の破線で示すように、時刻Δｔの観測値から時刻４Δｔの観測値への同じＩＤを持つ観測値間の距離の合計をｈ＊（ｎ）と考える。あるＩＤの観測値ついて、時刻ｔの観測値の位置から時刻４Δｔの観測値の位置へ移動する距離の中で、破線の距離は最短であることは、ＩＤ割り当て処理部２０５で、すぐわかるので、ｎから時刻４Δｔの目標ノードまでの真の最小コストｈ（ｎ）に対して、
ｈ＊（ｎ）≦ｈ（ｎ）
が成り立つ。従って、ＩＤ割り当て処理部２０５により、第１実施形態のＩＤ割り当て問題をＡ＊アルゴリズムで解くことができる。

次に、ＩＤ割り当て処理部２０５において、ＩＤ割り当て問題をＡ＊アルゴリズムで解く手順について説明する。図１４は、ＩＤ割り当て処理部２０５において、Ａ＊アルゴリズムで解く場合の、ノードの表現形式を示す図である。１つのノードは、時刻ｔ、各観測値へのＩＤの割り当て状態、そのノードの推定コストｆ＊の値、１つ前のノードへのポインタからなる。ノードへのポインタを辿ることにより、開始ノードから経由してきたノードを知ることができる。図１４では、時刻Δｔのノードのポインタが時刻０のノード（開始ノード）へのポインタを持つ例を示している。

図１５Ａ及び図１５Ｂは、ＩＤ割り当て処理部２０５において、Ａ＊アルゴリズムで解く場合の手順のフロー図である。図１５Ａ及び図１５Ｂのフローの手順は、ＩＤ割り当て処理部２０５で実行される。

図１５Ａのフロー中のｔｓ、ｔｅは、それぞれ無線タグ観測値が得られた時刻であり、ｔｓ＜ｔｅである。

ステップＳ２０１〜Ｓ２０３までは、ＩＤ割り当て処理部２０５により、ＩＤ割り当ての探索問題の開始ノードを設定する処理である。

まず、ステップＳ２０１では、無線タグ観測値が得られた時刻ｔｓにおいて、無線タグ観測値と人認識カメラ観測値との距離が最小になる対応関係をＩＤ割り当て処理部２０５で求める。

次いで、ステップＳ２０２では、その対応に基づいて、人認識カメラ観測値に対応する無線タグ観測値のＩＤ情報をＩＤ割り当て処理部２０５で割り当てる。

次いで、ステップＳ２０３で、その割当状態から開始ノードをＩＤ割り当て処理部２０５で作成する。

ステップＳ２０４〜Ｓ２０６までは、ＩＤ割り当て処理部２０５において、ＩＤ割り当ての探索問題の目標ノードを設定する処理である。

ステップＳ２０４では、無線タグ観測値が得られた時刻ｔｅにおいて、無線タグ観測値と人認識カメラ観測値の距離が最小になる対応関係をＩＤ割り当て処理部２０５で求める。

次いで、ステップＳ２０５では、その対応に基づいて、人認識カメラ観測値に対応する無線タグ観測値のＩＤ情報をＩＤ割り当て処理部２０５で割り当てる。

次いで、ステップＳ２０６では、その割当状態から目標ノードをＩＤ割り当て処理部２０５で作成する。

次に、図１５Ｂのフロー図で、ステップＳ２０６に続く手順について説明する。

図１５Ｂ中のｓは開始ノードであり、ＯＰＥＮリスト及びＣＬＯＳＥリストは、それぞれノードを格納するリストである。ＣＬＯＳＥリストは、自ノードから先のノードの探索が進んでいるノードを格納するリストであり、ＯＰＥＮリストは、自ノードから先のノードへの探索が進んでいないノードを格納するリストである。

ステップＳ２０７がＡ＊アルゴリズムの開始になり、ＩＤ割り当て処理部２０５において、ＯＰＥＮリストに開始ノードを入れる。このときの開始ノードの推定コストｆ＊（ｓ）は、ｆ＊（ｓ）＝ｈ＊（ｓ）にＩＤ割り当て処理部２０５で設定する。なお、第１実施形態では、開始ノードの観測値と目標ノードの観測値との同じＩＤ同士の観測値間の距離の合計である。

次いで、ステップＳ２０８では、ＩＤ割り当て処理部２０５において、ＯＰＥＮリストが空かどうかを調べ、ＯＰＥＮリストが空であるとＩＤ割り当て処理部２０５で判断すれば終了する。ＯＰＥＮリストが空である場合は、目標ノードに達する経路が見つからなかったケースに相当するが、第１実施形態の探索問題の場合は、必ず開始ノードから目標ノードへの経路がＩＤ割り当て処理部２０５で見つかる。ＯＰＥＮリストが空でないとＩＤ割り当て処理部２０５で判断する場合は、ステップＳ２０９に進む。

次いで、ステップＳ２０９で、ＩＤ割り当て処理部２０５において、ＯＰＥＮリストから最小のｆ＊の値を持つノードｎを取り出し、ＣＬＯＳＥリストに移す。

次いで、ステップＳ２１０で、ＩＤ割り当て処理部２０５において、ｎが目標ノードかどうかを判別し、ｎが目標ノードであるとＩＤ割り当て処理部２０５で判断すれば探索は終了である。このとき、ｎのポインタを開始ノードに達するまで辿って得られるノードの経路が、最小コスト経路である。ｎが目標ノードではないとＩＤ割り当て処理部２０５で判断する場合は、ステップＳ２１１に進む。

次いで、ステップＳ２１１では、ＩＤ割り当て処理部２０５において、ｎのノードの観測時刻の次の観測値時刻のＩＤ割り当て状態の仮説を生成し、それらをノードｎの継続ノードとしてノードの集合Ｍとする。Ｍの各ノードｍについて、ｆ’（ｍ）＝ｇ＊（ｎ）＋ｈ＊（ｍ）＋Ｃｏｓｔ（ｎ，ｍ）の値をＩＤ割り当て処理部２０５で計算する。Ｃｏｓｔ（ｎ，ｍ）はノードｎからノードｍへのエッジに割り当てられたコストであり、（式２）に基づいてＩＤ割り当て処理部２０５で計算する。

次いで、ステップＳ２１２では、ＩＤ割り当て処理部２０５において、Ｍから１つノードを取り出しｍとする。

次いで、ステップＳ２１３では、ＩＤ割り当て処理部２０５において、ノードｍがＯＰＥＮリストに含まれるノードに一致するか、あるいは、ＣＬＯＳＥリストに含まれるノードに一致するか、あるいは、そのどちらのリストのノードにも一致しないかを判断する。

ノードｍがどちらのリストにも含まれないとＩＤ割り当て処理部２０５で判断する場合は、ステップＳ２１４に進む。ステップＳ２１４では、ＩＤ割り当て処理部２０５において、ステップＳ２１１で計算したｆ’（ｍ）をノードｍの予測コストｆ＊として設定し、ノードｍのポインタをノードｎ設定してＯＰＥＮリストに追加する。

ノードｍがＯＰＥＮリストに含まれるとＩＤ割り当て処理部２０５で判断する場合は、ステップＳ２１５に進む。ステップＳ２１５では、ＩＤ割り当て処理部２０５において、含まれているノードの予測コストｆ＊（ｍ）とステップＳ２１１で計算したｆ’（ｍ）を比較し、ｆ’（ｍ）＜ｆ＊（ｍ）ならば、ｆ＊（ｍ）＝ｆ’（ｍ）でｆ＊の値を置き換えて、ノードｍのポインタをノードｎに設定する。

ノードｍがＣＬＯＳＥリストに含まれるとＩＤ割り当て処理部２０５で判断する場合は、ステップＳ２１６に進む。ステップＳ２１６では、ＩＤ割り当て処理部２０５において、含まれているノードの予測コストｆ＊（ｍ）とステップＳ２１１で計算したｆ’（ｍ）を比較し、ｆ’（ｍ）＜ｆ＊（ｍ）ならば、ｆ＊（ｍ）＝ｆ’（ｍ）でｆ＊の値を置き換えて、ノードｍのポインタをノードｎに設定した上で、ｍをＣＬＯＳＥリストからＯＰＥＮリストへ移す。

ステップＳ２１４、ステップＳ２１５、ステップＳ２１６の次は、ステップＳ２１７で、ＩＤ割り当て処理部２０５において、ノードの集合Ｍからすべてのｍを取り出したかを判断する。すべてのｍを取り出していないとＩＤ割り当て処理部２０５で判断する場合は、ステップＳ２１２に戻る。すべてのｍを取り出したとＩＤ割り当て処理部２０５で判断する場合は、ステップＳ２０８に戻る。

ステップＳ２１０で、ｎが目標ノードまで達して終了したときに得られる、目標ノードのポインタを辿り、開始ノードに至るまでの間のノードに表現されているＩＤ割り当て状態が、ＩＤ割り当て処理部２０５で求めるべき各観測値における人認識カメラ観測値へのＩＤ割り当て状態である。

さて、Ａ＊アルゴリズムでの解法の手順のステップＳ２１１において、ノードｎの次の観測時刻のＩＤ割り当て状態仮説をＩＤ割り当て処理部２０５で生成する際、ここまでの説明では、考えうるすべてのＩＤ割り当て状態をＩＤ割り当て処理部２０５で考えるとしている。例えば、観測値が３つある場合は、６通りの割り当て状態すべてを仮説としてＩＤ割り当て処理部２０５で生成するわけである。

しかし、第１実施形態の観測対象が人であるとすると、実際に観測周期Δｔの間に人が動ける距離には限界がある。よって、ステップＳ２１１で、ＩＤ割り当て状態の仮説をＩＤ割り当て処理部２０５で生成する際に、ノードｎのＩＤ割り当て状態から、次の観測時刻の観測値へのＩＤ割り当て状態として考えられる仮説の中には、実際に人の移動により実現不可能な仮説も含まれる場合がある。そのような仮説は人の移動では実現不可能であるため、生成して探索の対象としても無駄である。図１６Ａは、時刻０の観測値のＩＤ割り当てから、時刻ΔｔのＩＤ割り当てで、同じＩＤの観測値が移動する距離が、いずれも人の移動可能な範囲（図中の点線の円の範囲）になっている例を示している。

一方、図１６Ｂは、時刻０の観測値のＩＤ割り当てから、時刻ΔｔのＩＤ割り当てで、同じＩＤの観測値が移動する距離のうち、Ｂについては人の移動可能な範囲（図中の点線の円の範囲）になっているが、Ａ，Ｃについては、人の移動可能な範囲（図中の点線の円の範囲）を超えている例である。

第１実施形態においては、ある位置からの人の移動可能な範囲（図中の点線の円の範囲）の設定については、当該位置を中心とし、観測周期Δｔと観測対象空間において予め計測した最大の人の移動速度の積により算出される距離を半径とする円の内部とする。

もちろん、他の方法による人の移動可能範囲の設定も可能である。例えば、観測値の移動履歴から予想される移動速度と移動方向に基づいて、そのときの観測時刻における観測値の予想位置を設定することもできる。

よって、図１６Ａの時刻ΔｔのＩＤ割り当ての仮説は生成可能だが、図１６Ｂの時刻ΔｔのＩＤ割り当ての仮説は生成不可能である。

従って、ステップＳ２１１では、図１６Ｂの例のように人の移動可能距離の限界を超える同じＩＤの観測値の移動距離が生じてしまう仮説をＩＤ割り当て処理部２０５で捨てることにより、探索空間を減らし、探索を効率的に行うことができる。

かかる構成によって、ＩＤ割り当て処理部２０３により、無線タグ観測部２０１のＩＤ情報を手がかりにＩＤ情報を持たない人認識カメラ２０２の観測値に対して、ＩＤ情報を割り当てることで、観測対象が互いに接近するような状況を含む、任意の状況において、観測値のＩＤ情報を確定することができ、その結果として、人同士のすれ違いを含む人位置推定の誤りが発生しやすい状況での、推定誤りの問題を解決することができる。一例として、図１７の場合のように人が移動経路１７０２，１７０３に沿って移動した場合に、第１実施形態を使用して人位置移動経路の推定結果の例を図２８に示す。図２８より、従来技術の図１９とは大きく異なり、図１７の人の移動経路１７０２，１７０３とほぼ同じように、人位置移動経路を推定できることがわかる。

なお、本第１実施形態においては、人位置の推定位置をディスプレイなどの人位置表示部２０８に表示するとしている。しかしながら、人位置の推定値の利用形態は、人の位置を要求する他の装置への入力とすることも、もちろん可能であり、本発明の範囲を限定するものではない。

（第２実施形態）
図２０は、本発明の第２実施形態における人位置トラッキング装置９０Ａの構成を示すブロック図である。本発明の第２実施形態は、人認識カメラ観測値の個数が、隠蔽又は誤認識などにより無線タグ観測値の個数と一致しない場合にも人認識カメラ観測値へのＩＤ割り当て処理を行うよう構成したものである。本発明の第２実施形態は、本発明の第１実施形態のＩＤ割り当て処理部２０５を、内部構成が異なるＩＤ割り当て処理部２１０に置き換えたものであり、その他の構成部については、本発明の第１実施形態と同じである。第１実施形態の記述において説明済みの構成部分の説明は省略する。

ＩＤ割り当て処理部２１０は、内部の構成として、ＩＤ割り当て制御部２１０Ａと、観測値移動可能性判定部２１０Ｂと、仮想観測値生成部２１０Ｃとを備えるように構成される。

ＩＤ割り当て処理部２１０の全体の処理の制御を行うのが、ＩＤ割り当て制御部２１０Ａである。ＩＤ割り当て制御部２１０Ａは、本発明の第１実施形態のＩＤ割り当て処理部２０５と同様に、各観測時刻における人認識カメラ２０２の複数の観測値に対して、ＩＤ割り当ての仮説の探索グラフを生成したのち、その探索グラフ上で最もコストが小さい経路を導出することで、各観測時刻における人認識カメラ観測値へのＩＤ情報の最適な割り当てを決定する。

ただし、各観測時刻において人認識カメラ観測値の個数が無線タグ観測値の個数と一致しない場合、あるいは、観測対象の移動体（例えば、人）の移動能力又は移動履歴から予想される位置に観測値が無い場合、あるいは、予想される位置から外れた位置に観測値が存在する場合であっても、ＩＤ割り当ての仮説ノードを生成するために、観測値移動可能性判定部２１０Ｂにおいて、１単位の観測時刻間で移動可能な観測値の組み合わせを判定したのち、ＩＤを割り当てるべき観測値がない場合には、仮想観測値生成部２１０Ｃで、観測値の移動履歴から予想される位置に仮想の観測値を生成し、すべてのＩＤを割り当て可能にする。

次に、第２実施形態における人位置トラッキング装置９０Ａの動作の流れについて説明する。図２１は、第２実施形態の人位置トラッキング装置９０Ａの動作の流れを示すフロー図である。

第２実施形態の動作は、第１実施形態における人位置トラッキング装置９０の動作の流れと基本的には同じであり、ステップＳ１０５Ａでの処理の内容を除いて、各ステップの処理内容、及び、動作フローの構成は、図７のフロー図に示される動作フローと同一である。

ステップＳ１０５ＡがステップＳ１０５と異なるのは、上述の通り、無線タグ観測値の個数と人認識カメラ観測値の個数が一致しない場合への対応を含む処理内容になっている点である。具体的には、人認識カメラ観測値へのＩＤ割り当て処理のうち、人認識カメラ観測値へのＩＤ割り当ての仮説ノードを生成する部分の処理が異なる部分なので、この部分に絞って動作フローを説明する。

観測値バッファ２０４には、図４に示すように、人認識カメラ観測値が観測間隔Δｔで得られている。ここで、観測時刻ｔ＝ｔ_０における人認識カメラ観測値へのＩＤ割り当て状態から、観測時刻ｔ＝ｔ_０＋Δｔにおける人認識カメラ観測値へのＩＤ割り当て状態の仮説をＩＤ割り当て処理部２１０で生成する状況を考える。

図２２は、この状況での人認識カメラ観測値へのＩＤ割り当て仮説ノードをＩＤ割り当て処理部２１０で生成する動作のフロー図である。

まず、ステップＳ２２０１では、観測値移動可能性判定部２１０Ｂが、時刻ｔ＝ｔ_０における人認識カメラ２０２の各観測値から時刻ｔ＝ｔ_０＋Δｔにおける人認識カメラ２０２の各観測値への移動可能性を判定する。図２３は、観測時刻ｔ＝ｔ_０から観測時刻ｔ＝ｔ_０＋Δｔへの観測値間の移動を表す例である。観測値の側の数字は、各観測時刻における観測値を区別するための数字である。

図２３の観測時刻ｔ＝ｔ_０における参照符号１〜５までの観測値を中心に点線で示される円２３０１は、各観測値が観測時間間隔Δｔの間に、人（移動体）が移動可能な範囲の境界を表す。第２実施形態では、観測対象の人８９が移動可能な範囲の指定については、本発明の第１実施形態と同様に、観測時間間隔Δｔと観測対象の人８９の移動速度の限界から定めた半径を持つ円２３０１内とする。

この移動可能性の判定基準により、観測値移動可能性判定部２１０Ｂは、観測時刻ｔ＝ｔ_０おける各観測値が、観測時刻ｔ＝ｔ_０＋Δｔのどの観測値の位置まで移動可能かを判定する。

図２４に、観測値移動可能性判定部２１０Ｂの移動可能性判定結果を表にしたものを示す。図２４の左側の表２４０１は、ステップＳ２２０１での移動可能性判定の結果を表の形式で示したものである。表２４０１において、縦軸が観測時刻ｔ＝ｔ_０における観測値の識別番号、横軸が観測時刻ｔ＝ｔ_０＋Δｔにおける観測値の識別番号である。観測時刻ｔ＝ｔ_０におけるある観測値の位置から、観測時刻ｔ＝ｔ_０＋Δｔにおける観測値の位置に移動可能である場合は「○」で示し、移動可能ではない場合は「×」で示す。

次に、ステップＳ２２０２では、ＩＤ割り当て制御部２１０Ａが、ステップＳ２２０１の処理で求めた観測値の移動可能性判定結果に基づいて、観測時刻ｔ＝ｔ_０のどの観測値からも移動不可能な観測時刻ｔ＝ｔ_０＋Δｔの観測値を、ＩＤ割り当ての対象から除外する。

図２４に示す観測値の移動可能性判定結果では、観測時刻ｔ＝ｔ_０＋Δｔの識別番号３の観測値は、観測時刻ｔ＝ｔ_０のどの観測値からも移動不可能となっているため、この観測値をＩＤ割り当ての対象からＩＤ割り当て制御部２１０Ａで除外する。

次に、ステップＳ２２０３では、仮想観測値生成部２１０Ｃが、観測時刻ｔ＝ｔ_０＋Δｔのどの観測値にも移動可能なものがない観測時刻ｔ＝ｔ_０の観測値に対して、当該の観測値の移動履歴から予想される移動位置に仮想の観測値を生成する。

図２４に示す観測値の移動可能性判定結果では、観測時刻ｔ＝ｔ_０の識別番号５の観測値は、観測時刻ｔ＝ｔ_０＋Δｔのどの観測値にも移動可能ではない。よって、仮想観測値生成部２１０Ｃにおいて、当該観測値が観測時刻ｔ＝ｔ_０＋Δｔにおける移動予測位置を当該観測値の移動履歴から求め、その位置に仮想観測値を生成し、観測値バッファ２０４に生成した仮想観測値の情報を追加する。

図２５は、図２３に示す観測時刻での観測値に加え、観測時刻での観測値と、生成した仮想観測値（図２５中の識別番号５＊を付与した○）を示す図である。観測時刻の識別番号５の観測値は、観測時刻の識別番号５の観測値が移動したという仮説（すなわち両者に同じＩＤを割り当てるという仮説）が仮想観測値生成部２１０Ｃで生成されているので、仮想観測値生成部２１０Ｃにおいて、その仮説に基づき、観測時刻から観測時刻への移動速度と移動方向が維持されると仮定して、観測時刻における移動予測位置を図２５の「○」で表す位置と定め、そこに仮想観測値（図中「○」で示す）を生成する。

次に、ステップＳ２２０４において、観測値移動可能性判定部２１０Ｂは、ＩＤ割り当て仮想観測値生成部２１０Ｃが生成した観測時刻における仮想観測値に対して、移動可能性を再評価する。観測値移動可能性判定部２１０Ｂで再評価した結果を図２４の右側の表２４０２に示す。仮想的に生成した観測値を真に得られた観測値の個数の次の数字（仮想観測値が複数あれば順次数字を加算する）に＊を付けた形の識別番号で区別する。

次に、ステップＳ２２０５において、ＩＤ割り当て制御部２１０Ａは、ステップＳ２２０４までの処理で、評価された観測値の移動可能性判定結果として得られる観測時刻の観測値と観測時刻の観測値の移動に関する対応関係に基づき、対応関係にある観測時刻の観測値に観測時刻の観測時刻の観測値に割り当てられたＩＤを割り当てた仮説を複数生成する。

図２６は、図２４の観測値の移動可能性判定結果から得られる観測値へのＩＤ割り当ての仮説の組み合わせを示す表である。ＩＤ割り当て制御部２１０Ａにおいて、観測時刻の観測値のうち識別番号１，２の観測値には、ＩＤの多重の割り当てを許容したＩＤ割り当てを行うことになる。観測時刻の識別番号４，識別番号５＊へは、それぞれＩＤ＝Ｄ、ＩＤ＝ＥがＩＤ割り当て制御部２１０Ａで割り当てられる。

図２７は、図２６に示した３つのＩＤ割り当て仮説に対応する仮説ノードの表現を示す図である。仮説ノードの内部構造は、第１実施形態の図１４で説明した仮説ノードの構造同じである。１つの観測値に対して２つ以上のＩＤを割り当てた場合は、観測値の識別番号に「’」（ダッシュ）を追加して、同じ識別番号にＩＤが複数割り当てられていることを示す。ＩＤ割り当て仮説１の場合は、識別番号２の観測値にＩＤ＝Ｂ，ＩＤ＝Ｃの２つのＩＤが多重割り当てされるので、ＩＤ＝Ｃの割り当てに関しては、識別番号を２’としてノードに記録する。２つ以上のＩＤが割り当てられる場合は、「’」を順次増やして区別する。

かかる構成により、ＩＤ割り当て処理部２１０の内部において、観測値移動可能性判定部２１０Ｂが、１単位の観測間隔において人の移動可能性に基づいて、観測エラーによる観測値の消失の状態と、誤検出による誤観測値の状態を特定しつつ、移動可能な観測値間の対応関係を判定し、観測値の消失に関しても、仮想観測値生成手段２１０Ｃが観測値の移動履歴に基づいて予想される移動先位置に仮想的に観測値生成することにより、人認識カメラ２０２における観測エラーに対応可能な人位置トラッキング装置９０Ａを実現できる。

なお、第２実施形態においては、人位置の推定位置をディスプレイなどの人位置表示部２０８に表示するとしている。しかしながら、人位置の推定の利用形態は、人の位置を要求する他の装置のへの入力とすることも、もちろん可能であり、本発明の範囲を限定するものではない。

第１実施形態及び第２実施形態において、第１観測手段１０１は、移動体（例えば、人）の識別情報を含みかつ移動体が有する無線タグ３０１〜３０３から発信される電波を受信して移動体の識別情報と位置情報を観測値として出力する無線タグ観測部２０１で構成しているが、これに限るものではなく、移動体に超音波を発信して反射した超音波を受信して移動体の識別情報と位置情報を観測値として出力する超音波センサで構成することもできる。

第１実施形態及び第２実施形態において、第２観測手段１０２は、画像を撮像して移動体（例えば、人）の位置情報を観測値として出力するカメラ２０２で構成しているが、これに限るものではなく、移動体にミリ波を発信して反射したミリ波を受信して移動体の位置情報を観測値として出力するミリ波送受信器、又は、観測空間の床面に複数配置して移動体の位置情報を観測値として出力する圧力センサで構成することもできる。

なお、上記様々な実施形態又は変型例のうちの任意の実施形態又は変型例を適宜組み合わせることにより、それぞれの有する効果を奏するようにすることができる。

本発明に係る移動体位置推定装置、移動体位置推定方法、及び、移動体位置推定プログラムは、観測対象の人の行動を記録・監視するシステム又は、作業者の移動履歴を分析し作業手順の効率性を評価するシステム、物流現場における荷物の位置をトレースするシステムなどに応用が可能である。

８９ 人
９０，９０Ａ 人位置トラッキング装置
９１ 観測空間
９１ｆ 床面
１０１ 第１観測手段
１０２ 第２観測手段
１０３ 識別情報割り当て手段
１０４ 位置推定手段
２０１ 無線タグ観測部
２０２ 人認識カメラ
２０３ 観測値取得部
２０４ 観測値バッファ
２０５ ＩＤ割り当て処理部
２０６ 人位置推定部
２０７ 人位置推定部記録部
２０８ 人位置表示部
２１０ ＩＤ割り当て処理部
２１０Ａ ＩＤ割り当て制御部
２１０Ｂ 観測値移動可能性判定部
２１０Ｃ 仮想観測値生成部
３００ 部屋
３０１ 無線タグ
３０２ 無線タグ
３０３ 無線タグ
４０１ 観測値バッファに記録される観測値の例
４０２ 観測値バッファに記録された観測値にＩＤ割り当て処理が行われた例
６０１ 人位置表示部の人の移動軌跡の表示例
６０２ 人位置表示部の人の移動軌跡の表示例
６０３ 人位置表示部の人の移動軌跡の表示例
６０４ 人位置表示部に表示される人の頭頂方向からの被写映像
６０５ 人位置表示部に表示される人の頭頂方向からの被写映像
６０６ 人位置表示部に表示される人の頭頂方向からの被写映像
９０１ 人認識カメラ観測値の例
９０２ 人認識カメラ観測値の例
９０３ 人認識カメラ観測値の例
９０４ 人認識カメラ観測値の例
９０５ 人認識カメラ観測値の例
９０６ 人認識カメラ観測値の例
１７０１ 従来技術での課題を説明するための対象物の移動経路の例
１７０１Ａ 従来技術での課題を説明するための対象物が接近する領域
１７０２，１７０３ 移動経路
２３０１ 各観測値が観測時間間隔Δｔの間に、人（移動体）が移動可能な範囲の境界
２４０１ 観測値の移動可能性の判定結果
２４０２ 仮想観測値に対する移動可能性の判定結果

Claims (12)

1. 観測空間に存在する複数の移動体を観測して、移動体毎に逐次得られる前記移動体の識別情報及び前記移動体の位置情報を含む観測値を入力として、観測対象である前記複数の移動体の識別情報、及び、位置情報を推定する移動体位置推定装置であって、
   前記移動体の識別情報と位置情報を観測値として所定時間間隔毎に出力する第１の観測手段と、
   前記移動体の位置情報を観測値として、前記第１の観測手段の間隔とは異なる時間間隔で出力する第２の観測手段と、
   前記第１の観測手段と前記第２の観測手段のそれぞれの前記時間間隔のうちの前記第１の観測手段の前記観測値が得られた時刻における、前記第１の観測手段の前記観測値の位置情報と前記第２の観測手段の前記観測値の位置情報の相対距離の合計、及び、前記第１観測手段の前記観測値が得られた時刻での、前記第２観測手段の前記観測値への識別情報の割り当てに基づいて、前記第１観測手段の観測値が得られない時刻における、前記第２観測手段の前記観測値に対する識別情報の割り当ての考えうる組み合わせの中で、前記第１の観測手段の前記観測値が得られた時刻から次の前記第１の観測手段の前記観測値が得られた時刻までの前記第２の観測手段の観測値の移動量の合計が最小になるように、前記第２の観測手段の観測値に対して前記第１の観測手段から得られた識別情報を割り当てる識別情報割り当て手段と、
   前記第１の観測手段の観測値、及び、前記識別情報割り当て手段によって割り当てられた前記識別情報を含む前記第２の観測手段の観測値に基づいて、前記移動体の位置を推定する位置推定手段を、
   有する移動体位置推定装置。
2. 前記第１観測手段は、前記移動体の識別情報を含みかつ前記移動体が有する無線タグから発信される電波を受信して前記移動体の識別情報と位置情報を観測値として出力する無線タグ観測部、又は、前記移動体に超音波を発信して反射した超音波を受信して前記移動体の識別情報と位置情報を観測値として出力する超音波センサであり、
   前記第２観測手段は、画像を撮像して前記移動体の位置情報を観測値として出力するカメラ、又は、前記移動体にミリ波を発信して反射したミリ波を受信して前記移動体の位置情報を観測値として出力するミリ波送受信器、又は、前記観測空間の床面に複数配置して前記移動体の位置情報を観測値として出力する圧力センサである、請求項１に記載の移動体位置推定装置。
3. 前記識別情報割り当て手段において、前記観測間隔の時間内に前記移動体が移動可能な移動可能範囲外にある前記第２観測手段の観測値を削除して、前記観測間隔の時間内に前記移動体が移動可能な移動可能範囲内にある前記第２観測手段の観測値を基に、識別情報割り当て処理を行うときに対する前回の前記第２観測手段の観測値の位置から移動不可能な、前記識別情報割り当てを行うときの観測値を特定し、又は、前記識別情報割り当てを行うときの前記第２観測手段の観測値の位置へ移動可能なものが１つもない、前記識別情報割り当てを行うときに対する前回の観測値を特定し、特定された前記第２の観測手段の観測値に対して前記第１の観測手段から得られた識別情報を割り当てる、請求項１又は２に記載の移動体位置推定装置。
4. 前記識別情報割り当て手段は、前記識別情報割り当てを行うときの前記第２観測手段の観測値の位置へ移動可能なものが１つもないと特定された、前記識別情報割り当てを行うときに対する前回の観測値に対して、前記当該観測値の移動履歴に基づいて、前記識別情報割り当てを行うときの移動先位置を予測し、前記予測位置に仮想的な観測値を生成する仮想観測値生成部を備える、請求項３に記載の移動体位置推定装置。
5. 前記識別情報割り当て手段において、前記観測対象の前記移動体の前記移動可能範囲を、当該移動体の移動能力の計測結果に基づく最大の移動速度と、前記観測間隔の積を半径とする円の範囲と設定する請求項３又は４に記載の移動体位置推定装置。
6. 前記識別情報割り当て手段において、前記観測対象の前記移動体の前記移動可能範囲を、前記第２の観測手段の観測値の移動履歴の、前記識別情報割り当てを行うときに対する前回の観測値の位置と、前記識別情報割り当てを行うときに対する前々回の観測値の位置から算出される移動速度と移動方向とが維持されると仮定して得られる移動先位置を中心とした所定の半径を持つ円と定める、請求項３又は４に記載の移動体位置推定装置。
7. 観測空間に存在する複数の移動体を観測して、移動体毎に逐次得られる前記移動体の識別情報及び前記移動体の位置情報を含む観測値を入力として、観測対象である前記複数の移動体の識別情報、及び、位置情報を推定する移動体位置推定方法であって、
   前記移動体の識別情報と位置情報を観測値として所定時間間隔毎に第１の観測手段により出力し、
   前記移動体の位置情報を観測値として、前記第１の観測手段の間隔とは異なる時間間隔で第２の観測手段により出力し、
   前記第１の観測手段と前記第２の観測手段のそれぞれの前記時間間隔のうちの前記第１の観測手段の前記観測値が得られた時刻における、前記第１の観測手段の前記観測値の位置情報と前記第２の観測手段の前記観測値の位置情報の相対距離の合計、及び、前記第１観測手段の前記観測値が得られた時刻での、前記第２観測手段の前記観測値への識別情報の割り当てに基づいて、前記第１観測手段の観測値が得られない時刻における、前記第２観測手段の前記観測値に対する識別情報の割り当ての考えうる組み合わせの中で、前記第１の観測手段の前記観測値が得られた時刻から次の前記第１の観測手段の前記観測値が得られた時刻までの前記第２の観測手段の観測値の移動量の合計が最小になるように、前記第２の観測手段の観測値に対して前記第１の観測手段から得られた識別情報を識別情報割り当て手段により割り当て、
   前記第１の観測手段の観測値、及び、前記識別情報割り当て手段によって割り当てられた前記識別情報を含む前記第２の観測手段の観測値に基づいて、前記移動体の位置を位置推定手段により推定する、
   移動体位置推定方法。
8. 前記移動体の識別情報と位置情報を含みかつ前記第１の観測手段から出力された第１の観測値と、前記移動体の位置情報を含みかつ前記第２の観測手段から出力された第２の観測値とが同時に得られた第１の時刻から、次に、前記第１の観測値と前記第２の観測値が同時に得られた第２の時刻の間に得られた第２の観測値に対して、
   前記第１の時刻において、前記第１の観測値の位置と、前記第２の観測値の位置の距離の合計が最も短い組み合わせを前記識別情報割り当て手段で求め、
   前記第１の時刻における組み合わせに基づいて、前記第１の観測値の識別情報を前記第２の観測値の識別情報として前記識別情報割り当て手段で割り当て、
   さらに、前記第２の時刻において、前記第１の観測値の位置と、前記第２の観測値の位置の距離の合計が最も短い組み合わせを前記識別情報割り当て手段で求め、
   前記第２の時刻における組み合わせにおいて前記第１の観測値の識別情報を前記第２の観測値の識別情報として前記識別情報割り当て手段で割り当て、
   前記識別情報割り当て手段において、前記第１の時刻における前記第２の観測値への識別情報の割り当て状態を開始ノードとし、また、前記第２の時刻における前記第２の観測値への識別情報の割り当て状態を目標ノードとし、前記第１の時刻と前記第２の時刻の間の前記第２の観測値への識別情報の割り当ての組み合わせを、前記開始ノードと前記目標ノードの間に位置するノードとすることにより得られる探索グラフにおいて、前記ノード間の遷移コストを前記第２の観測値の移動量の合計と置くことで、前記探索グラフの前記開始ノードから前記目標ノードに至る最小コスト経路を求めることにより、前記第１の時刻から前記第２の時刻までの前記第２の観測値に前記第１の観測値の識別情報を前記識別情報割り当て手段で割り当てる、
   請求項７に記載の移動体位置推定方法。
9. 前記探索グラフ上の探索処理において、前記識別情報割り当て手段により、ある観測時刻における識別情報の割り当て状態に相当するノードから、次の観測時刻への割り当て状態の仮説に相当するノードを生成する場合に、前記観測対象の前記移動体の移動可能範囲を考慮し、前記観測間隔の時間内に移動可能な前記第２観測手段の観測値の対応関係を判定し、探索処理を行うときに対する前回の前記第２観測手段の観測値の位置から移動不可能な、前記探索処理を行うときの観測値を特定することで、前記探索処理を行うときに対する前回の観測値の位置から移動不可能な観測値を含んだ割り当ての仮説ノードを生成しない、請求項８に記載の移動体位置推定方法。
10. 前記探索グラフ上の探索処理において、前記識別情報割り当て手段により、ある観測時刻における識別情報の割り当て状態に相当するノードから、次の観測時刻への割り当て状態の仮説に相当するノードを生成する場合に、前記観測対象の前記移動体の移動可能範囲を考慮し、探索処理を行うときの観測値の位置へ移動可能なものが１つもないと特定された、前記探索処理を行うときに対する前回の観測値に対して、前記当該観測値の移動履歴に基づいて前記探索処理を行うときの移動先位置を予測し、前記予測位置に仮想的な観測値を生成し、前記仮想的な観測値に前記当該の観測値が移動したとする仮説ノードを生成する、請求項８又は９に記載の移動体位置推定方法。
11. 観測空間に存在する複数の移動体を観測して、移動体毎に逐次得られる前記移動体の識別情報及び前記移動体の位置情報を含む観測値を入力として、観測対象である前記複数の移動体の識別情報、及び、位置情報を推定する移動体位置推定プログラムであって、
    前記移動体の識別情報と位置情報を観測値として所定時間間隔毎に出力する第１の観測手段と、前記移動体の位置情報を観測値として、前記第１の観測手段の間隔とは異なる時間間隔で出力する第２の観測手段とのそれぞれの前記時間間隔のうちの前記第１の観測手段の前記観測値が得られた時刻における、前記第１の観測手段の前記観測値の位置情報と前記第２の観測手段の前記観測値の位置情報の相対距離の合計、及び、前記第１観測手段の前記観測値が得られた時刻での、前記第２観測手段の前記観測値への識別情報の割り当てに基づいて、前記第１観測手段の観測値が得られない時刻における、前記第２観測手段の前記観測値に対する識別情報の割り当ての考えうる組み合わせの中で、前記第１の観測手段の前記観測値が得られた時刻から次の前記第１の観測手段の前記観測値が得られた時刻までの前記第２の観測手段の観測値の移動量の合計が最小になるように、前記第２の観測手段の観測値に対して前記第１の観測手段から得られた識別情報を割り当てる識別情報割り当て手段と、
    前記第１の観測手段の観測値、及び、前記識別情報割り当て手段によって割り当てられた前記識別情報を含む前記第２の観測手段の観測値に基づいて、前記移動体の位置を推定する位置推定手段と
    の機能をコンピュータに実現させるための移動体位置推定プログラム。
12. 前記移動体の識別情報と位置情報を含みかつ前記第１の観測手段から出力された第１の観測値と、前記移動体の位置情報を含みかつ前記第２の観測手段から出力された第２の観測値とが同時に得られた第１の時刻から、次に、前記第１の観測値と前記第２の観測値とが同時に得られた第２の時刻の間に得られた第２の観測値に対して、
    前記第１の時刻において、前記第１の観測値の位置と、前記第２の観測値の位置の距離の合計が最も短い組み合わせを前記識別情報割り当て手段で求める機能と、
    前記第１の時刻における組み合わせに基づいて、前記第１の観測値の識別情報を前記第２の観測値の識別情報として前記識別情報割り当て手段で割り当てる機能と、
    さらに、前記第２の時刻において、前記第１の観測値の位置と、前記第２の観測値の位置の距離の合計が最も短い組み合わせを前記識別情報割り当て手段で求める機能と、
    前記第２の時刻における組み合わせにおいて前記第１の観測値の識別情報を前記第２の観測値の識別情報として前記識別情報割り当て手段で割り当てる機能と、
    前記第１の時刻における前記第２の観測値への識別情報の割り当て状態を開始ノードとし、また、前記第２の時刻における前記第２の観測値への識別情報の割り当て状態を目標ノードとし、前記第１の時刻と前記第２の時刻の間の前記第２の観測値への識別情報の割り当ての組み合わせを、前記開始ノードと前記目標ノードの間に位置するノードとすることにより得られる探索グラフにおいて、前記ノード間の遷移コストを前記第２の観測値の移動量の合計と置くことで、前記探索グラフの前記開始ノードから前記目標ノードに至る最小コスト経路を求めることにより、前記第１の時刻から前記第２の時刻までの前記第２の観測値に前記第１の観測値の識別情報を前記識別情報割り当て手段で割り当てる機能と、
    をコンピュータに実現させるための、請求項１１に記載の移動体位置推定プログラム。

Images