JP5147036B2

JP5147036B2 - 位置推定装置、位置推定方法及び位置推定プログラム

Info

Publication number: JP5147036B2
Application number: JP2006062190A
Authority: JP
Inventors: 竜基坂本; 周大田; 潔小暮; 努藤波
Original assignee: ATR Advanced Telecommunications Research Institute International
Current assignee: ATR Advanced Telecommunications Research Institute International
Priority date: 2006-03-08
Filing date: 2006-03-08
Publication date: 2013-02-20
Anticipated expiration: 2026-03-08
Also published as: JP2007240295A

Description

本発明は、複数の対象物の位置を推定する位置推定装置、位置推定方法及び位置推定プログラムに関するものである。

ユビキタス・パーベイシブコンピューティングにおいて、コンテキストアウェアな環境の構築を目的とした研究開発が行われている。この対象は、生産現場、看護・介護業務など多岐に渡っているが、共通して重要な役割を果たす要素技術に、人物や物体の位置の推定及び追跡がある。物体の位置推定及びその追跡のためには、対象の個体識別と物理位置とを連続的に推定する必要があり、この実現に向けて様々なセンサを用いたシステムが開発及び試用されている。例えば、ＲＦＩＤを利用したシステムや、超音波センサを用いたシステム、ビデオカメラの画像処理によるシステム等が開発されている（例えば、非特許文献１参照）。
西田佳史、他３名、超音波センサを用いた対象物のセンサ化に基づく人の日常活動の認識、日本機械学会、ロボティクス・メカトロニクス講演会、’０２講演会論文集、２００２年、１Ａ１−Ｊ０７

しかしながら、上記のシステムに用いられるセンサは、その特性によって利点と欠点とがあり、予め想定されている環境や状況では、対象物の位置を正しく推定できるが、検出精度が環境の特性に依存するため、様々な環境において実用レベルで対象物の位置を安定的に推定し続けることは困難である。例えば、カメラで撮影された画像を入力とする場合は、オクルージョンや環境光の影響に強く左右され、電波を用いる場合は、位置精度が悪くなることが多い。

また、情報支援をおこなう適用先によっては、環境上の制約やコスト問題等により、理想的なセンサすなわち高性能なセンサではなく、代替となる低性能なセンサを使用せざるえない場面も想定される。

本発明の目的は、比較的低性能なセンサを用いる場合でも、複数の対象物の位置を安定して推定することができる位置推定装置、位置推定方法及び位置推定プログラムを提供することである。

本発明に係る位置推定装置は、複数の対象物を識別することなく、前記複数の対象物の位置を検出する第１の検出手段と、前記複数の対象物の各々に対して一意的に割り付けられた識別情報を検出する第２の検出手段と、前記第１の検出手段により検出された位置を用いて対象物の位置の尤度を算出する第１の算出手段と、前記第２の検出手段により検出された識別情報を用いて対象物の識別情報の尤度を算出する第２の算出手段と、前記第１の算出手段により算出された対象物の位置の尤度と、前記第２の算出手段により算出された対象物の識別情報の尤度とを基に各対象物の位置を推定する推定手段とを備え、前記第１の算出手段は、Ｃｏｎｄｅｎｓａｔｉｏｎアルゴリズムに従って、前記第１の検出手段により検出された位置と、対象物の位置に対するシステムモデル及び観測モデルとを用いて対象物の位置の尤度を算出し、前記第２の算出手段は、Ｃｏｎｄｅｎｓａｔｉｏｎアルゴリズムに従って、前記第２の検出手段により検出された識別情報と、対象物の識別情報に対するシステムモデル及び観測モデルとを用いて対象物の識別情報の尤度を算出し、前記第２の検出手段は、各対象物に取り付けられ、当該対象物に対して一意的に割り付けられた識別情報に応じて光を発光する複数の発光手段と、前記発光手段を含む所定領域の画像を撮影する撮影手段と、前記撮影手段により撮影された画像を用いて前記発光手段の識別情報を検出する識別情報検出手段とを含み、前記第１の算出手段は、前記第１の検出手段により検出された位置を検出領域上のパーティクルの分布として扱い、複数のパーティクルを用いて対象物の位置の尤度を算出し、前記第２の算出手段は、前記撮影手段により撮影された画像上の各発光手段の画素位置を射影変換によって前記第１の検出手段の検出座標上に投影し、投影された検出座標に対応付けられているパーティクルに対して各発光手段の識別情報の尤度を保持させ、前記推定手段は、前記第２の算出手段により各発光手段の識別情報の尤度が保持されたパーティクルを用いて、前記第１の算出手段により算出された対象物の位置の尤度と、前記第２の算出手段により算出された各発光手段の識別情報の尤度との双方の平均をとることにより、各対象物の位置を推定するものである。

この位置推定装置においては、複数の対象物の位置が検出され、検出された位置を用いて対象物の位置の尤度が算出され、この位置の尤度の算出処理と分離して、複数の対象物の各々に対して一意的に割り付けられた識別情報が検出され、検出された識別情報を用いて対象物の識別情報の尤度が算出され、算出された対象物の位置の尤度と対象物の識別情報の尤度とを基に各対象物の位置が推定される。このように、対象物の位置の尤度と対象物の識別情報の尤度とを個別に取り扱うことによって、頑健な識別情報付きの対象物追跡を実現することができるので、比較的低性能なセンサを用いる場合でも、複数の対象物の位置を安定して推定することができる。

この場合、Ｃｏｎｄｅｎｓａｔｉｏｎアルゴリズムに従った対象物の位置の尤度の算出と、Ｃｏｎｄｅｎｓａｔｉｏｎアルゴリズムに従った対象物の識別情報の尤度の算出とを個別に取り扱っているので、元来複数対象の個別追跡が考慮されていないＣｏｎｄｅｎｓａｔｉｏｎアルゴリズムを識別情報付きのマルチトラッキング対応へと単純に拡張できるとともに、入力の欠損による一時的な誤り状態からの復帰が容易となり、さらに、複数の対象物追跡にとって理想的な識別情報付き位置情報が精度よく取得可能なセンサを用いることができない場合でも、各センサがもつ位置精度や識別情報認識の有無といった特性を織り込むことが可能となる。

この場合、複数のパーティクルを用いて対象物の位置の尤度が算出されるとともに、撮影された画像上の各発光手段の画素位置が射影変換によって第１の検出手段の検出座標上に投影され、投影された検出座標に対応付けられているパーティクルに対して各発光手段の識別情報の尤度が保持されるので、各発光手段の識別情報の尤度すなわち対象物の識別情報の尤度を、当該識別情報を有する対象物の位置の尤度と統合することができ、頑健な識別情報付きの対象物追跡を実現することができる。

本発明に係る位置推定方法は、第１及び第２の検出手段、第１及び第２の算出手段、並びに推定手段を備える位置推定装置を用いた位置推定方法であって、前記第１の検出手段が、複数の対象物を識別することなく、前記複数の対象物の位置を検出する第１のステップと、前記第２の検出手段が、前記複数の対象物の各々に対して一意的に割り付けられた識別情報を検出する第２のステップと、前記第１の算出手段が、前記第１の検出手段により検出された位置を用いて対象物の位置の尤度を算出する第３のステップと、前記第２の算出手段が、前記第２の検出手段により検出された識別情報を用いて対象物の識別情報の尤度を算出する第４のステップと、前記推定手段が、前記第１の算出手段により算出された対象物の位置の尤度と、前記第２の算出手段により算出された対象物の識別情報の尤度とを基に各対象物の位置を推定する第５のステップとを含み、前記第３のステップは、Ｃｏｎｄｅｎｓａｔｉｏｎアルゴリズムに従って、前記第１の検出手段により検出された位置と、対象物の位置に対するシステムモデル及び観測モデルとを用いて対象物の位置の尤度を算出するステップを含み、前記第４のステップは、Ｃｏｎｄｅｎｓａｔｉｏｎアルゴリズムに従って、前記第２の検出手段により検出された識別情報と、対象物の識別情報に対するシステムモデル及び観測モデルとを用いて対象物の識別情報の尤度を算出するステップを含み、前記第２の検出手段は、各対象物に取り付けられ、当該対象物に対して一意的に割り付けられた識別情報に応じて光を発光する複数の発光手段と、前記発光手段を含む所定領域の画像を撮影する撮影手段と、前記撮影手段により撮影された画像を用いて前記発光手段の識別情報を検出する識別情報検出手段とを含み、前記第３のステップは、前記第１の検出手段により検出された位置を検出領域上のパーティクルの分布として扱い、複数のパーティクルを用いて対象物の位置の尤度を算出するステップを含み、前記第４のステップは、前記撮影手段により撮影された画像上の各発光手段の画素位置を射影変換によって前記第１の検出手段の検出座標上に投影し、投影された検出座標に対応付けられているパーティクルに対して各発光手段の識別情報の尤度を保持させるステップを含み、前記第５のステップは、前記第４のステップにおいて各発光手段の識別情報の尤度が保持されたパーティクルを用いて、前記第３のステップにおいて算出された対象物の位置の尤度と、前記第４のステップにおいて算出された各発光手段の識別情報の尤度との双方の平均をとることにより、各対象物の位置を推定するステップを含むものである。

本発明に係る位置推定プログラムは、複数の対象物を識別することなく、前記複数の対象物の位置を検出する第１の検出手段により検出された位置を用いて対象物の位置の尤度を算出する第１の算出手段と、前記複数の対象物の各々に対して一意的に割り付けられた識別情報を検出する第２の検出手段により検出された識別情報を用いて対象物の識別情報の尤度を算出する第２の算出手段と、前記第１の算出手段により算出された対象物の位置の尤度と、前記第２の算出手段により算出された対象物の識別情報の尤度とを基に各対象物の位置を推定する推定手段としてコンピュータを機能させ、前記第１の算出手段は、Ｃｏｎｄｅｎｓａｔｉｏｎアルゴリズムに従って、前記第１の検出手段により検出された位置と、対象物の位置に対するシステムモデル及び観測モデルとを用いて対象物の位置の尤度を算出し、前記第２の算出手段は、Ｃｏｎｄｅｎｓａｔｉｏｎアルゴリズムに従って、前記第２の検出手段により検出された識別情報と、対象物の識別情報に対するシステムモデル及び観測モデルとを用いて対象物の識別情報の尤度を算出し、前記第２の検出手段は、各対象物に取り付けられ、当該対象物に対して一意的に割り付けられた識別情報に応じて光を発光する複数の発光手段と、前記発光手段を含む所定領域の画像を撮影する撮影手段と、前記撮影手段により撮影された画像を用いて前記発光手段の識別情報を検出する識別情報検出手段とを含み、前記第１の算出手段は、前記第１の検出手段により検出された位置を検出領域上のパーティクルの分布として扱い、複数のパーティクルを用いて対象物の位置の尤度を算出し、前記第２の算出手段は、前記撮影手段により撮影された画像上の各発光手段の画素位置を射影変換によって前記第１の検出手段の検出座標上に投影し、投影された検出座標に対応付けられているパーティクルに対して各発光手段の識別情報の尤度を保持させ、前記推定手段は、前記第２の算出手段により各発光手段の識別情報の尤度が保持されたパーティクルを用いて、前記第１の算出手段により算出された対象物の位置の尤度と、前記第２の算出手段により算出された各発光手段の識別情報の尤度との双方の平均をとることにより、各対象物の位置を推定するものである。

本発明によれば、対象物の位置の尤度と対象物の識別情報の尤度とを個別に取り扱うことによって頑健な識別情報付きの対象物追跡を実現することができるので、比較的低性能なセンサを用いる場合でも、複数の対象物の位置を安定して推定することができる。

以下、本発明の一実施の形態による位置推定装置について図面を参照しながら説明する。図１は、本発明の一実施の形態による位置推定装置の構成を示すブロック図である。なお、本実施の形態では、人物を対象物として複数の人物の位置を推定する例について説明するが、対象物はこの例に特に限定されず、本発明は種々の対象物の位置を以下と同様にして推定することができる。

図１に示す位置推定装置は、位置センサ１、ＩＤセンサ２、光ＩＤタグ３、及び推定装置４を備える。ＩＤセンサ２は、ビデオカメラ２１及び画像処理装置２２を備え、光ＩＤタグ３は、駆動回路３１及びＬＥＤ３２を備え、推定装置４は、判断部４１、位置尤度算出部４２、ＩＤ尤度算出部４３及び位置推定部４４を備える。

位置センサ１は、各人物を識別することなく、複数の人物の位置を検出し、例えば、低解像度なフロアセンサを用いることができる。このフロアセンサとしては、ヴィストン社製フロアセンサ（型式：ＶＳ−ＳＦ５５）が用いられ、人物が踏んだ箇所の圧力荷重を感知し、ＯＮ／ＯＦＦの２値（１ビット）で人物の位置を出力する。但し、フロアセンサの解像度は低く、取得される位置情報の最小単位は、一辺８ｃｍの正方形をした単位領域であり、各単位領域を表す番号毎にＯＮ／ＯＦＦの２値で位置情報が出力される。推定装置４は、位置センサ１の位置情報を９Ｈｚで定期的に取得する。したがって、位置センサ１の位置情報から、細かな足形を取得してトラッキングや人物認識等をおこなうことは非常に困難である。

光ＩＤタグ３は、各人物の胸部等に装着され、駆動回路３１が各人物（各光ＩＤタグ３）に対して一意的に割り付けられたＩＤ番号（識別情報）をＬＥＤ３２の点滅により送信する。なお、図１では、図示を容易にするために、１個の光ＩＤタグ３のみを図示しているが、本実施の形態では、人物毎に光ＩＤタグ３が装着されており、複数の光ＩＤタグ３が用いられている。また、一人の人物に対して異なる位置に複数の光ＩＤタグ３を装着し、１台のビデオカメラが少なくとも１個の光ＩＤタグ３を常時撮影できるようにしてもよい。

ＬＥＤ３２としては、可視光を発光する通常のＬＥＤを用いることができるが、この例に特に限定されず、指向性が弱く且つ可視光に近い８００ｎｍ程度の赤外光を発光する光通信用高出力発光ダイオード（スタンレイ社製ＤＮ３１１）等を用いてもよい。この場合、ビデオカメラ２１として、赤外線画像を撮影する赤外線カメラが用いられる。

駆動回路３１は、マイクロコンピュータ等から構成され、例えば、Ａｔｍｅｌ社製４ＭＨｚ駆動マイコンＡＴ９０Ｓ２３２３等を用いることができ、光ＩＤタグ３が取り付けられた人物に対して一意的に割り付けられたＩＤ番号が識別可能なようにＬＥＤ３２を点滅制御する。なお、駆動回路３１及びＬＥＤ３２は、内部電池（図示省略）から電源を供給されている。

ＩＤセンサ２のビデオカメラ２１は、複数の人物が位置する検出空間中の所定位置（例えば、部屋の天井の一方端）に取り付けられ、各人物の光ＩＤタグ３を含む所定領域の画像を撮影し、画像処理装置２２は、ビデオカメラ２１により撮影された画像を用いて、光ＩＤタグ３から送信される対象物のＩＤ番号及び光ＩＤタグ３の画像内のＸＹ座標を検出して推定装置４へ出力する。ＩＤセンサ２は、受光部分に対してＩＤタグが存在するか否かしか判断できないＩｒＤＡとは異なり、光ＩＤタグ３の画素位置を取得可能であることが特徴である。

ビデオカメラ２１としては、ＵＳＢカメラが用いられ、例えば、Ｌｕｍｅｎｅｒａ社製ＵＳＢカメラを用いることができ、６４０×４８０ｐｉｘｅｌの画像を撮影することができる。また、光ＩＤタグ３は３０Ｈｚで点滅し、新たな光ＩＤタグ３の発見には最速でも０．５秒かかり、連続してＩＤ番号を認識する場合でも、約０．１秒の間隔が必要となる。なお、本実施の形態では、１台のビデオカメラ２１を用いているが、この例に特に限定されず、複数台のビデオカメラを検出空間の異なる位置に配置するようにしてもよい。

図２は、図１に示すＩＤセンサ２及び光ＩＤタグ３によるＩＤ番号の生成方法を説明するための模式図である。光ＩＤタグ３は、マンチェスタ符号化方式を用い、図示のような点滅パターンを繰り返して点滅し、ＩＤセンサ２は、点滅パターンを撮影して画像処理によってデコードすることにより、光ＩＤタグ３のＩＤ番号を読み取る。図示の例では、点滅パターンから１、１、０、１、０の２進数がデコードされ、ＩＤ番号１０が取得される。このときのＩＤセンサ２のサンプリング周波数は６０ｆｐｓであり、光ＩＤタグ３の点滅パターンは３０ｆｐｓである。

なお、本実施の形態では、位置推定のためのセンサとしてフロアセンサを用い、ＩＤ番号推定のためのセンサとして光ＩＤタグを用いているが、各センサは、これらの例に特に限定されず、例えば、ＩＤセンサとして、ＩＤ付き超音波センサ、ＲＦＩＤ、ＩＣタグ、ＩｒＤＡ、画像処理による人物（物体）認識等を用いたり、位置センサとして、超音波センサ、画像処理によるトラッキング、電磁誘導センサ、静電容量計測技術を用いたセンサ（ＳｍａｒｔＳｋｉｎ）等を用いてもよい。また、３次元位置が取得可能な超音波ＩＤセンサや、上記のＩＤ付き超音波センサ、ＲＦＩＤ、ＩＣタグ、ＩｒＤＡ、画像処理による人物（物体）認識等を用いて位置及びＩＤ番号の双方が取得可能な場合、１種類のセンサの出力（位置及びＩＤ番号）から、位置の尤度及びＩＤ番号の尤度を個別に算出するようにしてもよい。

推定装置４は、ＲＯＭ（リードオンリメモリ）、ＣＰＵ（中央演算処理装置）、ＲＡＭ（ランダムアクセスメモリ）、外部記憶装置、入力装置、入出力インタフェース装置及び表示装置等を備える通常のコンピュータから構成され、後述する位置推定処理を実行するための位置推定プログラムをＣＰＵ等で実行することにより上記の判断部４１、位置尤度算出部４２、ＩＤ尤度算出部４３及び位置推定部４４の機能が実現される。なお、推定装置４の構成は、この例に特に限定されず、図示の各ブロックの機能を専用のハードウエア回路から構成したり、上記の各機能の一部又は全部を１台又は複数台のコンピュータを用いて実行するようにしてもよい。また、画像処理装置２２の機能を取り込んで実行するようにしてもよい。

判断部４１は、センサ入力として、位置センサ１から人物の位置情報を取得するとともに、ＩＤセンサ２から人物すなわち光ＩＤタグ３のＩＤ番号及び画像内のＸＹ座標を取得し、人物の位置情報を位置尤度算出部４２へ出力し、光ＩＤタグ３のＩＤ番号及び画像内のＸＹ座標をＩＤ尤度算出部４３へ出力する。

位置尤度算出部４２は、Ｃｏｎｄｅｎｓａｔｉｏｎアルゴリズムに従って、位置センサ１により検出された人物の位置情報と、人物の位置に対するシステムモデル及び観測モデルとを用いて人物の位置の尤度を算出して位置推定部４４へ出力する。ＩＤ尤度算出部４３は、Ｃｏｎｄｅｎｓａｔｉｏｎアルゴリズムに従って、検出された光ＩＤタグ３のＩＤ番号及び画像内のＸＹ座標と、人物の識別情報に対するシステムモデル及び観測モデルとを用いて光ＩＤタグ３すなわち人物のＩＤ番号の尤度を算出して位置推定部４４へ出力する。位置推定部４４は、位置尤度算出部４２により算出された人物の位置の尤度と、ＩＤ尤度算出部４３により算出された人物のＩＤ番号の尤度とを基に検出空間内の各人物の位置を推定する。

上記のＣｏｎｄｅｎｓａｔｉｏｎアルゴリズムは、パーティクルによって確率分布を近似し、パーティクルの移動・拡散・観測を逐次繰り返すことによって、追跡対象の確率密度を推定していく手法である。Ｃｏｎｄｅｎｓａｔｉｏｎアルゴリズムは、研究分野によっては、モンテカルロフィルタ、ブートストラップフィルタ等とも呼ばれ、人物追跡、画像を用いたトラッキング、ロボットの位置推定等の様々な分野で応用が試されている。

また、Ｃｏｎｄｅｎｓａｔｉｏｎアルゴリズムは、位置推定対象の状態遷移についてマルコフ性を仮定したベイズ推論の一種であり、まず、追跡対象をｘ、観測結果をｚとした場合の、時刻ｔ−１における位置推定対象の確率分布ｐ（ｘ_ｔ−１｜ｚ_ｔ−１）と、時刻ｔ−１から時刻ｔへの状態推移確率ｐ（ｘ_ｔ−１｜ｘ_ｔ−１）とにより、時刻ｔにおける事前確率ｐ（ｘ_ｔ｜ｚ_ｔ−１）を求める。次に、この事前確率に基づき、実際の観測結果から尤度ｐ（ｚ_ｔ｜ｘ_ｔ）を算出し、時刻ｔにおける確率分布ｐ（ｘ_ｔ｜ｚ_ｔ）を求める。時刻ｔ＋１においても同様に時刻ｔにおける確率密度に従い、上記の計算を繰り返す。この状態推移確率を求めるモデルをシステムモデルと呼び、尤度を求めるモデルを観測モデルと呼ぶ。

本実施の形態では、このＣｏｎｄｅｎｓａｔｉｏｎアルゴリズムにおける仮説生成に際して、位置に関する尤度と、ＩＤ番号に関する尤度とを分離し、位置推定に関するシステムモデル及び観測モデルとは別に、個別認識のためのシステムモデル及び観測モデルを組み込み、個別に計算を行っている。この利点は、（１）元来複数対象の個別追跡が考慮されていないＣｏｎｄｅｎｓａｔｉｏｎアルゴリズムをＩＤ番号付きのマルチトラッキング対応へと単純に拡張できること、（２）入力の欠損による一時的な誤り状態からの復帰が容易であること、（３）複数人物追跡にとって理想的なＩＤ番号付き位置情報が精度よく取得可能なセンサを用いることができない場合でも、各センサが持つ位置精度やＩＤ認識の有無といった特性を織り込むことが可能なことである。

本実施の形態では、位置センサ１が第１の検出手段の一例に相当し、ＩＤセンサ２及び光ＩＤタグ３が第２の検出手段の一例に相当し、位置尤度算出部４２が第１の算出手段の一例に相当し、ＩＤ尤度算出部４３が第２の算出手段の一例に相当し、位置推定部４４が推定手段の一例に相当する。また、光ＩＤタグ３が発光手段の一例に相当し、ビデオカメラ２１が撮影手段の一例に相当し、画像処理装置２２が識別情報検出手段の一例に相当する。

次に、上記のように構成された推定装置４による位置推定処理について説明する。図３は、図１に示す推定装置４の位置推定処理を説明するためのフローチャートである。図３に示す位置推定処理では、入力（各センサの出力）の特性に合わせて処理を二分化し、位置推定のためのシステムモデル適用及び尤度計算と、ＩＤ番号推定のためのシステムモデル適用及び尤度計算とを分離することにより、ＣｏｎｄｅｎｓａｔｉｏｎアルゴリズムによるＩＤ番号付き位置推定を実現している。

まず、ステップＳ１１において、判断部４１は、センサ入力の有無を判断し、センサ入力がなければ、ステップＳ１１の処理を継続し、センサ入力があった場合にステップＳ１２へ処理を移行する。

センサ入力があった場合、ステップＳ１２において、判断部４１は、位置センサ１又はＩＤセンサ２のいずれの入力であるかを判断し、位置センサ１からの入力の場合に、位置センサ１からの人物の位置情報を位置尤度算出部４２へ出力して処理をステップＳ１３へ移行し、ＩＤセンサ２からの入力の場合に、ＩＤセンサ２からの光ＩＤタグ３のＩＤ番号及び画像内のＸＹ座標をＩＤ尤度算出部４３へ出力して処理をステップＳ１６へ移行する。

位置センサ１からの入力の場合、ステップＳ１３において、位置尤度算出部４２は、位置に対する下記のシステムモデルを用いて、人物の位置に対する仮説を生成する。次に、ステップＳ１４において、位置尤度算出部４２は、位置センサ１により検出された人物の位置情報と、パーティクル（例えば、１０００個）の分布とから、位置に対する下記の観測モデルを用いて、位置の尤度を算出する。

一方、ＩＤセンサ２からの入力の場合、ステップＳ１６において、ＩＤ尤度算出部４３は、ＩＤ番号に対する下記のシステムモデルを用いて、ＩＤ番号に対する仮説を生成する。次に、ステップＳ１７において、ＩＤ尤度算出部４３は、ＩＤセンサ２により検出されたＩＤ番号、及びＩＤ番号に対する下記の観測モデルを用いて、ＩＤ番号の尤度を算出する。また、ＩＤ尤度算出部４３は、ＩＤセンサ２からの光ＩＤタグ３の画像内のＸＹ座標すなわち撮影された画像上の各光ＩＤタグ３の画素位置を射影変換によって位置センサ１の検出座標上に投影し、投影された検出座標に位置する単位領域に対応付けられているパーティクルに対して光ＩＤタグ３のＩＤ番号の尤度を保持させる。

図４は、ＩＤ番号が取得された画素位置からの直線投影を説明するための模式図であり、図５は、パーティクルへＩＤ番号の尤度を保持させる方法を説明するための模式図であり、図６は、パーティクルのＩＤ番号の判別例を説明するための模式図である。

ＩＤセンサ２によって検出される位置情報は、画像上の画素位置であるため、この出力結果をそのまま位置センサ１の位置情報（単位領域を表す番号毎のＯＮ／ＯＦＦの２値）と対応付けることはできない。このため、以下のようにして、このＩＤ番号の画素位置を射影変換行列によってフロアセンサ上に投影している。なお、この射影変換行列はカメラを強校正することで獲得している。

まず、ＩＤ尤度算出部４３は、図４に示すように、ビデオカメラ２１により撮影された画像上の光ＩＤタグ３の画素位置から射影変換によって直線を投影し、ブレゼンハムアルゴリズムにより位置センサ１の検出座標上の直線Ｌ１を求める。

例えば、ビデオカメラ２１によりＩＤ番号３、６、１０を有する３個の光ＩＤタグ３を含む画像が撮影された場合、図５に示すように、各ＩＤ番号３、６、１０に対して３本の直線が位置センサ１の検出座標上（複数の単位領域上）に投影される。ここで、ＩＤ番号３に着目すると、下段の拡大図に示すように、ＩＤ番号３の直線が交差する各単位領域ＵＡ（ハッチング部分）に対応付けられているパーティクルＰＡに対してＩＤ番号３の尤度を保持させる。

このようにして、各パーティクルが対応するＩＤ番号の尤度を保持する場合、図６に示す例（ＩＤ番号６のパーティクルを赤色で表し、ＩＤ番号３のパーティクルを緑色で表している）のように、位置センサ１のパーティクルをＩＤ番号で判別することができる。

再び、図３を参照して、上記のいずれかの処理が終了した後、ステップＳ１５において、位置推定部４４は、位置尤度算出部４２により算出された人物の位置の尤度と、ＩＤ尤度算出部４３により算出された人物のＩＤ番号の尤度とを基に検出空間内の各人物の位置を推定する。その後、ステップＳ１１に戻って上記の処理が繰り返され、複数の人物の位置が追跡される。

次に、上記のステップＳ１３〜Ｓ１７の処理をさらに詳細に説明する。この説明では、時刻ｔにおける追跡対象である人物の位置をｘ_ｔ、位置センサ１からの観測結果である人物の位置情報をｚ_ｔ、ＩＤセンサ２からの観測結果である光ＩＤタグ３のＩＤ番号をｙ_ｔ、サンプル数をＮ個とするとともに、時刻ｔ−１における仮説ｓ^（ｎ） _ｔ−１と、位置の尤度π^（ｎ） _ｔ−１と、ＩＤ情報の尤度π（ｕ）^（ｎ） _ｔ−１とから成る仮説群｛ｓ^（ｎ） _ｔ−１，π^（ｎ） _ｔ−１，π（ｕ）^（ｎ） _ｔ−１｝（ｎ＝１，…，Ｎ）により表現された確率分布が存在するものとする。

まず、センサ入力が位置センサ１の場合、ステップＳ１３において、位置尤度算出部４２は、位置の尤度π^（ｎ） _ｔ−１に従い仮説ｓ’^（ｎ） _ｔ−１の選択をし、予め定義されたパーティクルの移動特性を表すシステムモデルｐ（ｘ_ｔ｜ｘ_ｔ−１＝ｓ’^（ｎ） _ｔ−１）によって、位置に対する仮説群を生成する。

次に、ステップＳ１４において、位置尤度算出部４２は、位置センサ１から得られた実際の観測量から、観測モデルｐ_ｐｏｓ（ｚ_ｔ｜ｘ_ｔ）により各仮説の尤度π^（ｎ） _ｔを計算し、時刻ｔにおける仮説群｛ｓ^（ｎ） _ｔ，π^（ｎ） _ｔ｝を決定する。

一方、センサ入力がＩＤセンサ２の場合、ＩＤセンサ２に対するシステムモデルの適用と尤度計算とが行われ、ＩＤセンサ２のシステムモデルは、先に述べたパーティクルの移動特性を示すシステムモデルとは異なり、ＩＤ番号の尤度の減衰を制御する。このＩＤ番号の尤度は、各パーティクルにおいて観測されたＩＤ番号の数だけ存在するものとする。

まず、ステップＳ１６において、予め定義されたＩＤ番号のシステムモデルによって、ＩＤ番号に対する仮説群を生成する。次に、ステップＳ１７において、ＩＤ尤度算出部４３は、ＩＤセンサ２の観測結果ｙ_ｔから、ＩＤセンサ２の観測モデルｐ（ｙ_ｔ｜ｘ_ｔ）により各仮説の尤度π（ｕ）^（ｎ）を計算し、現時刻ｔにおけるＩＤに対する仮説群｛ｓ^（１） _ｔ，…，ｓ^（Ｎ） _ｔ｝のＩＤ番号の尤度を更新する。

最後に、ステップＳ１５において、位置推定部４４は、更新されたパーティクルによって、現時刻ｔにおけるＩＤ番号すなわちｕを有する人物の位置を次式から推定する。

ε_ｕ[ｆ（ｘ_ｔ）]＝Σπ^（ｎ） _ｔπ（ｕ）^（ｎ） _ｔｆ（ｓ^（ｎ） _ｔ）（１）
なお、ｆ（ｓ^（ｎ） _ｔ）としては、種々の関数を用いることができ、例えば、位置の尤度及びＩＤ番号の尤度の大きなものの平均や、最も大きい尤度を選択し、人物の位置を推定することができる。

次に、上記の処理に使用されるシステムモデル及び観測モデルの一例について説明する。まず、位置センサ１に対する仮説生成のシステムモデルとして、人物の移動速度に対応したモデルを考え、下式で表されるシステムモデルを用いることができる。

ｘ_ｎ＋１＝ｘ_ｎ＋ν_ｎΔｔ＋ω_ｎ（２）
ここで、ｘ_ｎはｎステップ目の追跡対象物である人物の位置であり、ν_ｎは過去の位置から推定した速度であり、Δｔは時間定数であり、ω_ｎはホワイトノイズである．
次に、位置センサ１に対する仮説生成の観測モデルとして、下式で表される観測モデルを用いることができる。

ここで、ｚ_ｎは観測結果として得られる追跡対象物である人物の位置であり、Σは共分散である。

一方、ＩＤセンサ２に対するシステムモデルは、位置変化に対して独立であり、時間経過に対する尤度変化に対応させ、下式で表されるシステムモデルを用いることができる。

π_ｎ＋１＝π_ｎｋ（４）
ここで、π_ｎはパーティクルの更新ｎステップ目におけるＩＤ番号の尤度であり、ｋ（０＜ｋ≦１）は定数である。ＩＤセンサ２に対する観測モデルは、上記の位置センサ１の観測モデルと同一の観測モデルを用いることができる。このモデルにより影響を受ける尤度は、各ＩＤ番号に対するＩＤ番号の尤度のみである。

上記のシステムモデルを用いて運用されたパーティクル群の確率分布により、人物の位置推定が実行される。通常のＣｏｎｄｅｎｓａｔｉｏｎでは、尤度の平均値を用いる方法が最も単純な位置推定となるが，本実施の形態では、位置の尤度とＩＤ番号の尤度との双方の平均をとる方法が、単純な位置推定方法となる。

上記の処理により、本実施の形態では、複数の人物の位置が検出され、検出された位置を用いて人物の位置の尤度が算出され、この位置の尤度の算出処理と分離して、複数の人物の各々に対して一意的に割り付けられたＩＤ番号が検出され、検出されたＩＤ番号を用いて人物のＩＤ番号の尤度が算出され、算出された人物の位置の尤度及びＩＤ番号の尤度を基に各人物の位置が推定されるので、位置の尤度とＩＤ番号の尤度とを個別に取り扱うことによって頑健なＩＤ番号付きの人物追跡を実現することができ、低解像度な位置センサ１とフレームレートの低いＩＤセンサ２とを用いた場合においても、複数の人物の位置を安定して推定することができる。

次に、上記の位置推定装置を用いて実際に複数の人物の位置を推定した結果について説明する。本推定では、光ＩＤタグ３に対する測定条件を以下のように変えて３種類の測定を行った。

図７は、第１の測定条件の一シーン及びそのときのパーティクル群による位置推定の様子を示す図である。図７に示すように、第１の測定条件として、光ＩＤタグ３に対してオクルージョンが発生しないように、カメラ視線に対して平行に２人の被測定者が並んで前後に約１分間歩行する条件を用いた。本実施の形態では、理論値として９．０９ＨｚでＩＤ番号を取得可能であるが、第１の測定条件では、検出できない場合を考慮して、平均して８．６１５ＨｚでＩＤ番号を取得した。

図８は、第１の測定条件に対する推定結果を示す図である。図８では、２人の被測定者の追跡軌跡をそれぞれ実線及び破線で表しており、２本の線がほぼ直線的に平行して移動している様子がわかる、図８の左側を歩行した被測定者の歩行方向に対して横方向の座標軸に計測された値（ｍｍ）の分散は１７６４であり、右側を歩行した被測定者の分散は１３６９であった。

図９は、第２の測定条件の一シーン及びそのときのパーティクル群による位置推定の様子を示す図である。図９に示すように、第２の測定条件として、カメラ視線方向に対して２人の被測定者が前後して並び、ビデオカメラ２１に対して横方向に往復移動する条件を用いた。この場合、２人の被測定者の重なりが往復運動の中央付近で常に発生するよう同期的に動いているため、第１の測定条件と異なり、ビデオカメラ２１から見て２人の被測定者が重なる瞬間にオクルージョンが発生する。このオクルージョンが発生する前後は、ＩＤ番号を取得し難くなるため、光ＩＤタグ３の平均認識頻度は７．８２Ｈｚとなり、第１の測定条件の場合より低下した。

図１０は、第２の測定条件に対する推定結果を示す図である。第２の測定条件に対する推定結果は、２本の直線が平行な形状となるべきであるが、図１０に示すように、三角形のような形状となって推定された。この結果における上部の被測定者の縦方向の分散は１０５９４であり、下部の被測定者の分散は１７４２４であり、第１の測定条件に比べて推定の誤りが多くなった。これは、オクルージョンが発生してＩＤ番号の尤度の更新がされなくなった場合に、正解となるＩＤ番号の尤度が全体的に減少し、広く散らばっている本来は無関係なパーティクルのＩＤ番号の尤度の影響が強くなることに起因すると考えられる。

しかしながら、往復運動の両端では正しく推定されており、往復運動の方向転換の位置で正しく推定された。この結果、オクルージョンによる混乱が発生した場合でも、オクルージョンが解消されれば、正しい推定に復帰可能な頑健な位置推定が可能であることがわかった。

図１１は、第３の測定条件の一シーン及びそのときのパーティクル群による位置推定の様子を示す図である。図１１に示すように、第３の測定条件として、部屋の中を２人の被測定者が点対称を維持したまま位置センサ１の縁に沿って歩く条件を用いた。この場合、２人の被測定者の間隔は約０．８ｍであり、カメラ視線に対して２人の被測定者が直線状に並ぶ瞬間にオクルージョンが発生する。

図１２は、第３の測定条件に対する推定結果を示す図である。図１２に示す推定結果から、多少の誤認識はあるものの、安定して方形の軌跡を描いていることがわかり、この場合も、頑健な位置推定が可能であることがわかった。

上記の結果、本実施の形態では、複数の人物の並び歩行、円弧歩行、交差歩行等の種々の条件において、光ＩＤタグ３に対してオクルージョンが発生する場合に、一時的に誤推定状態はあるものの、その状態から復帰可能であり、複数の人物の位置を安定して推定することができることがわかった。

本発明の一実施の形態による位置推定装置の構成を示すブロック図である。図１に示すＩＤセンサ及び光ＩＤタグによるＩＤ番号の生成方法を説明するための模式図である。図１に示す推定装置の位置推定処理を説明するためのフローチャートである。ＩＤ番号が取得された画素位置からの直線投影を説明するための模式図である。パーティクルへＩＤ番号の尤度を保持させる方法を説明するための模式図である。パーティクルのＩＤ番号の判別例を説明するための模式図である。第１の測定条件の一シーン及びそのときのパーティクル群による位置推定の様子を示す図である。第１の測定条件に対する推定結果を示す図である。第２の測定条件の一シーン及びそのときのパーティクル群による位置推定の様子を示す図である。第２の測定条件に対する推定結果を示す図である。第３の測定条件の一シーン及びそのときのパーティクル群による位置推定の様子を示す図である。第３の測定条件に対する推定結果を示す図である。

符号の説明

１位置センサ
２ＩＤセンサ
３光ＩＤタグ
４推定装置
２１ビデオカメラ
２２画像処理装置
３１駆動回路
３２ＬＥＤ
４１判断部
４２位置尤度算出部
４３ＩＤ尤度算出部
４４位置推定部

Claims

複数の対象物を識別することなく、前記複数の対象物の位置を検出する第１の検出手段と、
前記複数の対象物の各々に対して一意的に割り付けられた識別情報を検出する第２の検出手段と、
前記第１の検出手段により検出された位置を用いて対象物の位置の尤度を算出する第１の算出手段と、
前記第２の検出手段により検出された識別情報を用いて対象物の識別情報の尤度を算出する第２の算出手段と、
前記第１の算出手段により算出された対象物の位置の尤度と、前記第２の算出手段により算出された対象物の識別情報の尤度とを基に各対象物の位置を推定する推定手段とを備え、
前記第１の算出手段は、Ｃｏｎｄｅｎｓａｔｉｏｎアルゴリズムに従って、前記第１の検出手段により検出された位置と、対象物の位置に対するシステムモデル及び観測モデルとを用いて対象物の位置の尤度を算出し、
前記第２の算出手段は、Ｃｏｎｄｅｎｓａｔｉｏｎアルゴリズムに従って、前記第２の検出手段により検出された識別情報と、対象物の識別情報に対するシステムモデル及び観測モデルとを用いて対象物の識別情報の尤度を算出し、
前記第２の検出手段は、
各対象物に取り付けられ、当該対象物に対して一意的に割り付けられた識別情報に応じて光を発光する複数の発光手段と、
前記発光手段を含む所定領域の画像を撮影する撮影手段と、
前記撮影手段により撮影された画像を用いて前記発光手段の識別情報を検出する識別情報検出手段とを含み、
前記第１の算出手段は、前記第１の検出手段により検出された位置を検出領域上のパーティクルの分布として扱い、複数のパーティクルを用いて対象物の位置の尤度を算出し、
前記第２の算出手段は、前記撮影手段により撮影された画像上の各発光手段の画素位置を射影変換によって前記第１の検出手段の検出座標上に投影し、投影された検出座標に対応付けられているパーティクルに対して各発光手段の識別情報の尤度を保持させ、
前記推定手段は、前記第２の算出手段により各発光手段の識別情報の尤度が保持されたパーティクルを用いて、前記第１の算出手段により算出された対象物の位置の尤度と、前記第２の算出手段により算出された各発光手段の識別情報の尤度との双方の平均をとることにより、各対象物の位置を推定することを特徴とする位置推定装置。
第１及び第２の検出手段、第１及び第２の算出手段、並びに推定手段を備える位置推定装置を用いた位置推定方法であって、
前記第１の検出手段が、複数の対象物を識別することなく、前記複数の対象物の位置を検出する第１のステップと、
前記第２の検出手段が、前記複数の対象物の各々に対して一意的に割り付けられた識別情報を検出する第２のステップと、
前記第１の算出手段が、前記第１の検出手段により検出された位置を用いて対象物の位置の尤度を算出する第３のステップと、
前記第２の算出手段が、前記第２の検出手段により検出された識別情報を用いて対象物の識別情報の尤度を算出する第４のステップと、
前記推定手段が、前記第１の算出手段により算出された対象物の位置の尤度と、前記第２の算出手段により算出された対象物の識別情報の尤度とを基に各対象物の位置を推定する第５のステップとを含み、
前記第３のステップは、Ｃｏｎｄｅｎｓａｔｉｏｎアルゴリズムに従って、前記第１の検出手段により検出された位置と、対象物の位置に対するシステムモデル及び観測モデルとを用いて対象物の位置の尤度を算出するステップを含み、
前記第４のステップは、Ｃｏｎｄｅｎｓａｔｉｏｎアルゴリズムに従って、前記第２の検出手段により検出された識別情報と、対象物の識別情報に対するシステムモデル及び観測モデルとを用いて対象物の識別情報の尤度を算出するステップを含み、
前記第２の検出手段は、
各対象物に取り付けられ、当該対象物に対して一意的に割り付けられた識別情報に応じて光を発光する複数の発光手段と、
前記発光手段を含む所定領域の画像を撮影する撮影手段と、
前記撮影手段により撮影された画像を用いて前記発光手段の識別情報を検出する識別情報検出手段とを含み、
前記第３のステップは、前記第１の検出手段により検出された位置を検出領域上のパーティクルの分布として扱い、複数のパーティクルを用いて対象物の位置の尤度を算出するステップを含み、
前記第４のステップは、前記撮影手段により撮影された画像上の各発光手段の画素位置を射影変換によって前記第１の検出手段の検出座標上に投影し、投影された検出座標に対応付けられているパーティクルに対して各発光手段の識別情報の尤度を保持させるステップを含み、
前記第５のステップは、前記第４のステップにおいて各発光手段の識別情報の尤度が保持されたパーティクルを用いて、前記第３のステップにおいて算出された対象物の位置の尤度と、前記第４のステップにおいて算出された各発光手段の識別情報の尤度との双方の平均をとることにより、各対象物の位置を推定するステップを含むことを特徴とする位置推定方法。
複数の対象物を識別することなく、前記複数の対象物の位置を検出する第１の検出手段により検出された位置を用いて対象物の位置の尤度を算出する第１の算出手段と、
前記複数の対象物の各々に対して一意的に割り付けられた識別情報を検出する第２の検出手段により検出された識別情報を用いて対象物の識別情報の尤度を算出する第２の算出手段と、
前記第１の算出手段により算出された対象物の位置の尤度と、前記第２の算出手段により算出された対象物の識別情報の尤度とを基に各対象物の位置を推定する推定手段としてコンピュータを機能させ、
前記第１の算出手段は、Ｃｏｎｄｅｎｓａｔｉｏｎアルゴリズムに従って、前記第１の検出手段により検出された位置と、対象物の位置に対するシステムモデル及び観測モデルとを用いて対象物の位置の尤度を算出し、
前記第２の算出手段は、Ｃｏｎｄｅｎｓａｔｉｏｎアルゴリズムに従って、前記第２の検出手段により検出された識別情報と、対象物の識別情報に対するシステムモデル及び観測モデルとを用いて対象物の識別情報の尤度を算出し、
前記第２の検出手段は、
各対象物に取り付けられ、当該対象物に対して一意的に割り付けられた識別情報に応じて光を発光する複数の発光手段と、
前記発光手段を含む所定領域の画像を撮影する撮影手段と、
前記撮影手段により撮影された画像を用いて前記発光手段の識別情報を検出する識別情報検出手段とを含み、
前記第１の算出手段は、前記第１の検出手段により検出された位置を検出領域上のパーティクルの分布として扱い、複数のパーティクルを用いて対象物の位置の尤度を算出し、
前記第２の算出手段は、前記撮影手段により撮影された画像上の各発光手段の画素位置を射影変換によって前記第１の検出手段の検出座標上に投影し、投影された検出座標に対応付けられているパーティクルに対して各発光手段の識別情報の尤度を保持させ、
前記推定手段は、前記第２の算出手段により各発光手段の識別情報の尤度が保持されたパーティクルを用いて、前記第１の算出手段により算出された対象物の位置の尤度と、前記第２の算出手段により算出された各発光手段の識別情報の尤度との双方の平均をとることにより、各対象物の位置を推定することを特徴とする位置推定プログラム。