JP4669581B2 - 物体位置推定システム、物体位置推定装置、物体位置推定方法、及び、物体位置推定プログラム - Google Patents

Description

本発明は、観測手段が観測したデータを基に環境内に存在する物体の位置を、例えば、観測時刻の早い順に推定する物体位置推定システム、物体位置推定装置、物体位置推定方法、及び、物体位置推定プログラムに関する。

カメラ等のセンサを利用して、環境内に存在する対象物体を識別して、対象物体のＩＤ及び環境内における位置を推定するためには、大きく分けて、５つの工程が必要である。

第１工程では、カメラにより対象物体を撮像し、対象物体を含む画像データを得る。

第２工程では、予め保持する背景画像のテンプレートと、対象物体を含む画像データとを比較して、差分画像を得る。

第３工程では、予め保持する画像データ中の画素位置と環境内位置とを対応させたデータベースを用いて、得られた差分画像の環境内の物体の位置候補を得る。

第４工程では、予め保持する物体のＩＤと画像データとを対応させたデータベースを用いて、得られた差分画像に対応する物体のＩＤ候補を得る。以下、第３工程（物体の位置候補を得る工程）と第４工程（物体のＩＤ候補を得る工程）とを合わせて、「物体識別」と呼ぶ。

第５工程では、個別の物体のＩＤ候補及び位置候補のデータには不確実性があるため、過去にカメラで得た物体のＩＤと位置のデータを用いて、尤もらしい各物体の位置を推定する。以下、第５工程を「位置推定」と呼ぶ。

センサが環境を観測して得られたデータから、物体位置推定処理が取り扱うことのできる物体のＩＤ候補と位置候補に関するデータへと変換する物体識別処理を行う必要がある。

位置推定に必要な時間（第５工程）は、センサが環境を観測し、物体識別が終わるまで必要な時間（第１工程から第４工程）と比べて長い。よって、環境内に存在する全ての対象物体に対して、物体識別後のデータを個別に、位置推定を行った場合、センサが環境を観測した時刻と物体識別処理が完了する時刻との遅延時間（応答待ち時間）が発生する。図１３に例を示す。観測装置１の観測データ１が物体識別処理され、物体位置推定処理可能となるのが時刻１４：１７：２０（１４時１７分２０秒）である。観測装置２の観測データ２が物体識別処理され、物体位置推定処理可能となるのが時刻１４：１７：２６である。しかし、時刻１４：１７：２６の時点では観測データ１の物体位置推定処理はまだ終了しておらず、観測データ２の物体位置推定処理が開始できるのは、観測データ１の物体位置推定処理が終了する時刻１４：１７：３０からとなってしまっている。よって、観測データ２の物体位置推定処理は、観測データ１の物体位置推定処理が終了するのを待たなければならない。

このような装置等からの応答待ち時間を低減する方法として、例えば特許文献１では、現在までに収集された通信状態又は通信機器状態を統計処理することで異常応答の確率を求め、該異常応答の確率データに基づいてタイムアウト時間を決定する技術が開示されている。

特開平１１−３５３２５６号公報

しかしながら、物体識別処理に要する時間は観測装置に備え付けられているセンサの種類、及び、センサが観測した環境状態にも依存しており、センサの種類の情報と環境状態の情報とが無いと、物体識別処理に要する時間を正確に判断することができない。

その結果、物体識別処理に要する正確な時間に対して、物体位置推定処理タイミングの周期を速く設定してしまうと、物体識別処理の結果待ちが発生することとなる一方、物体位置推定処理タイミングの周期を遅く設定してしまうと、物体位置推定処理の結果待ちが発生する可能性がある。ここで、物体位置推定処理タイミングの周期を速くするためには、例えば物体位置推定処理を行うときのパラメータの調整、又は、物体位置推定処理に用いる物体識別処理結果の情報量を削減する必要があり、物体位置推定処理タイミングの周期を速く設定しすぎると前記物体識別処理結果の情報量を削減しすぎることになり、物体位置推定精度の低下に繋がる。逆に、物体位置推定処理タイミングの周期を遅く設定しすぎると、物体の移動軌跡を粗くしか推定することができない。

本発明は、前記課題を解決するためになされ、その目的とするところは、観測手段が観測した観測生データを基に、物体識別処理に要する時間を事前に判断し、現在の時刻から次に観測生データを取得する時刻までの物体識別処理可能な時間内に未処理の観測生データを確実に処理できるパラメータを用いて物体位置推定処理を確実に行うことによって、実時間処理と物体位置推定の高精度化を両立することのできる物体位置推定システム、物体位置推定装置、物体位置推定方法、及び、物体位置推定プログラムを提供することにある。

前記目的を達成するために、本発明は以下のように構成する。

本発明の第１態様によれば、環境内に存在する物体を含む前記環境の状況を観測して、第１及び第２観測生データを取得する観測手段と、
物体データベースに予め記憶された前記物体のＩＤ毎の物体識別データと、前記第１及び第２の観測生データのそれぞれの観測生データとに基づいて、前記物体のＩＤ及び前記物体の位置候補をそれぞれ取得して、前記取得した物体のＩＤ及び前記物体の位置候補の情報を観測データベースにそれぞれ記憶する物体識別手段と、
前記第１及び第２観測生データのそれぞれの観測生データから前記環境内に存在する前記物体の数を取得し、前記物体の数をそれぞれ取得した時刻と、前記それぞれ取得した物体の数と、予め記憶された前記物体あたりの前記物体のＩＤ及び前記物体の位置候補をそれぞれ取得する物体識別処理に要する時間とに基づいて、前記物体識別処理の終了予定時刻をそれぞれ予測し、前記物体識別処理の終了予定時刻を前記観測データベースにそれぞれ記憶する物体識別処理終了時間予測手段と、
前記観測データベースに記憶された前記物体のＩＤと前記物体の位置候補とを取得し、前記取得した物体のＩＤ及び物体の位置候補と、前記物体の位置候補の分散状況とに基づいて、前記物体の位置を推定する物体位置推定手段と、
前記物体識別手段が前記第１観測生データの前記物体識別処理を終了して前記物体のＩＤ及び前記物体の位置候補を取得した時刻に、前記観測データベースに記憶された前記第２観測生データの前記物体識別処理の終了予定時刻を参照して、前記参照した時刻から前記第２観測生データの前記物体識別処理の終了予定時刻が記憶されているか否かを判断し、前記第２観測生データの前記物体識別処理の終了予定時刻が記憶されていると判断した場合には、予め記憶されかつ前記物体あたりの前記物体の位置を推定する位置推定処理を行うのに要する時間を参照して、前記参照した時刻から前記第２観測生データの前記物体識別処理の終了予定時刻までの時間において、前記物体識別処理が終了した前記第１観測生データの前記物体の位置推定処理を行うパラメータを決定する一方、前記第２観測生データの前記物体識別処理の終了予定時刻が記憶されていなかったと判断した場合には、前記物体識別処理が終了した前記第１観測生データの前記物体の位置推定処理を行うパラメータに、予め設定しておいた基準となるパラメータを設定する、パラメータ決定手段とを備えて、
前記物体位置推定手段は、前記パラメータ決定手段が決定した前記パラメータを使用して、前記物体識別処理が終了したデータから、そのデータに含まれている前記物体の位置推定処理を行い、前記物体ＩＤに関する前記物体の位置を、前記物体識別手段が取得した前記物体のＩＤと位置候補とに基づいて推定する物体位置推定システムを提供する。

本発明の第６態様によれば、少なくとも、環境内に存在する物体のＩＤ毎の物体識別データと、前記物体を含む前記環境の状況を観測手段で観測して取得した第１及び第２の観測生データのそれぞれの観測生データとに基づいて、それぞれ取得された前記物体あたりの前記物体のＩＤ及び前記物体の位置候補を入力可能であるとともに、前記第１及び第２観測生データのそれぞれの観測生データから前記環境内に存在する前記物体の数を取得し、前記物体の数をそれぞれ取得した時刻と、前記それぞれ取得した物体の数と、前記物体あたりの前記物体のＩＤ及び前記物体の位置候補をそれぞれ取得する物体識別処理に要する時間とに基づいてそれぞれ予測された前記物体識別処理の終了予定時刻を入力可能なデータ入力部と、
前記データ入力部より受け取った前記物体のＩＤと位置候補と前記終了予定時刻に関するデータとを記憶する観測データベースと、
前記観測データベースに記憶されている前記物体のＩＤと位置候補と、前記物体の位置候補の分散状況とに基づいて、前記物体の位置を推定する物体位置推定手段と、
前記観測データベースに記憶された前記物体識別処理の終了予定時刻を参照して、前記参照した時刻から最も早い時刻に終了する前記物体識別処理の終了予定時刻が記憶されているか否かを判断し、前記観測生データの前記物体識別処理の終了予定時刻が記憶されていると判断した場合には、予め記憶されかつ前記物体あたりの前記物体の位置を推定する位置推定処理を行うのに要する時間を参照して、前記参照した時刻から前記観測生データの前記物体識別処理の終了予定時刻までの時間において、前記物体の位置推定処理を行うパラメータを決定する一方、前記観測生データの前記物体識別処理の終了予定時刻が記憶されていなかったと判断した場合には、前記物体の位置推定処理を行うパラメータに、予め設定しておいた基準となるパラメータを設定する、パラメータ決定手段と、
を備えて、前記物体位置推定手段は、前記パラメータ決定手段が決定した前記パラメータを使用して、前記観測データベースに記憶された前記物体のＩＤと位置候補と前記物体の位置候補の分散状況とから前記物体の位置推定処理を行い、前記物体の位置を推定する物体位置推定装置を提供する。

本発明の第７態様によれば、環境内の状況を観測し、複数の観測生データを観測手段で取得し、
前記複数の観測生データを基に、テンプレートマッチング処理を使用して、前記観測手段の観測範囲内に存在する物体のＩＤと位置候補とを物体識別手段で算出する物体識別処理を行い、
前記複数の観測生データを基に前記物体識別手段でそれぞれ行われる前記物体識別処理が終了する時刻を、前記それぞれの観測生データから前記環境内に存在する前記物体の数を取得し、前記物体の数をそれぞれ取得した時刻と、前記それぞれ取得した物体の数と、予め記憶された前記物体あたりの前記物体のＩＤ及び前記物体の位置候補をそれぞれ取得する物体識別処理に要する時間とに基づいて、物体識別処理終了時間予測手段でそれぞれ予測して観測データベースにそれぞれ記憶し、
前記観測データベースに記憶された前記物体識別処理の終了予定時刻を参照して、前記参照した時刻から最も早い時刻に終了する前記物体識別処理の終了予定時刻が記憶されているか否かをパラメータ決定手段で判断し、
前記観測生データの前記物体識別処理の終了予定時刻が記憶されていると判断した場合には、予め記憶されかつ前記物体あたりの前記物体の位置を推定する位置推定処理を行うのに要する時間を参照して、前記参照した時刻から前記観測生データの前記物体識別処理の終了予定時刻までの時間において、前記物体の位置推定処理を行うパラメータを前記パラメータ決定手段で決定する一方、
前記観測生データの前記物体識別処理の終了予定時刻が記憶されていなかったと判断した場合には、前記物体の位置推定処理を行うパラメータに、予め設定しておいた基準となるパラメータを前記パラメータ決定手段で設定し、
前記パラメータ決定手段で設定した前記パラメータを使用して、前記物体識別処理で算出した前記物体のＩＤと位置候補と、前記物体の位置候補の分散状況とに基づいて、前記物体の位置を物体位置推定手段で推定する、
ことを備える物体位置推定方法を提供する。

本発明の第８態様によれば、コンピュータに、
環境内の状況を観測手段で観測して取得された複数の観測生データを基に、テンプレートマッチング処理を使用して、前記観測手段の観測範囲内に存在する物体のＩＤと位置候補とを算出する物体識別処理を行う物体識別処理機能と、
前記複数の観測生データを基に前記物体識別処理機能による前記物体識別処理が終了する時刻を、前記それぞれの観測生データから前記環境内に存在する前記物体の数を取得し、前記物体の数をそれぞれ取得した時刻と、前記それぞれ取得した物体の数と、予め記憶された前記物体あたりの前記物体のＩＤ及び前記物体の位置候補をそれぞれ取得する物体識別処理に要する時間とに基づいて、予測して観測データベースにそれぞれ記憶する処理終了時刻予測機能と、
前記観測データベースに記憶された前記物体識別処理の終了予定時刻を参照して、前記参照した時刻から最も早い時刻に終了する前記物体識別処理の終了予定時刻が記憶されているか否かを判断し、前記観測生データの前記物体識別処理の終了予定時刻が記憶されていると判断した場合には、予め記憶された前記物体あたりの前記物体の位置を推定する位置推定処理を行うのに要する時間を参照して、前記参照した時刻から前記観測生データの前記物体識別処理の終了予定時刻までの時間において、前記物体の位置推定処理を行うパラメータを決定する一方、前記観測生データの前記物体識別処理の終了予定時刻が記憶されていなかったと判断した場合には、前記物体の位置推定処理を行うパラメータに、予め設定しておいた基準となるパラメータを設定するパラメータ決定機能と、
前記パラメータ決定手段で設定した前記パラメータを使用して、前記物体識別処理機能による前記物体識別処理で算出した前記物体のＩＤと位置候補と、前記物体の位置候補の分散状況とに基づいて、前記物体の位置を推定する物体位置推定機能と、
を実現させるための物体位置推定プログラムを提供する。

以上のように、本発明は、観測手段が観測した観測生データを基に、事前に物体識別処理に要する時間、つまり、次回の観測データ（物体識別処理されたデータ）が得られる時間を判断し、現在得られている観測データを用いて位置の推定を行う物体数を決定することによって、観測手段の観測に対して遅れが出ることなく、物体の位置を推定することのできる物体位置推定システム、物体位置推定装置、物体位置推定方法、及び、物体位置推定プログラムを提供することができる。

本発明のこれらと他の目的と特徴は、添付された図面についての好ましい実施形態に関連した次の記述から明らかになる。
図１は、本発明の第１及び第２実施形態にかかる物体位置推定システムの構成を示すブロック図である。 図２は、本発明の前記第１及び第２実施形態にかかる物体位置推定システムの物体データベースの一例を示す図である。 図３は、本発明の前記第１及び第２実施形態にかかる観測手段に測域センサを用いたときの観測生データの一例を示す図である。 図４は、本発明の前記第１及び第２実施形態にかかる観測手段に測域センサを用いたときの一観測状況例を示す図である。 図５は、本発明の前記第１及び第２実施形態にかかる物体位置推定システムの観測データベースの一例を示す図である。 図６Ａは、前記第１実施形態にかかる物体位置推定システムに使用可能なパーティクルフィルタによる物体位置推定方法の処理を示すフローチャートである。 図６Ｂは、前記第２実施形態にかかる物体位置推定システムに使用可能なカルマンフィルタの処理状況例を示す図である。 図７は、本発明の前記第１及び第２実施形態にかかる物体位置推定システムにおける物体観測状況例を示す図である。 図８は、本発明の前記第１実施形態にかかる物体位置推定結果の一例を示す図である。 図９は、本発明の前記第１実施形態にかかる物体位置推定システムの処理を示すフローチャートである。 図１０は、本発明の前記第１実施形態にかかる物体位置推定システムの処理を示すフローチャートである。 図１１Ａは、本発明の前記第１実施形態にかかる物体位置推定システムにおけるパーティクル数と処理時間の関係の情報を示す図である。 図１１Ｂは、図５に示した観測データ、及び、観測生データ処理終了予定時刻を基に、物体の位置を推定したときのタイミングチャートである。 図１２は、図１１Ｂを簡略化した、本発明の前記第１実施形態にかかるタイミングチャート図である。 図１３は、図１２との比較における、本発明を用いないときのタイミングチャート図である。 図１４Ａは、パーティクル数別の近似精度の比較図である。 図１４Ｂは、パーティクル数別の近似精度の比較図である。 図１５は、本発明の前記第１実施形態にかかる物体位置推定システムにおいて観測装置にカメラを用いたときの環境背景画像の一例の説明図である。 図１６は、本発明の前記第１実施形態にかかる観測装置にカメラを用いたときのカメラ入力画像の一例の説明図である。 図１７は、本発明の前記第１実施形態にかかる観測装置にカメラを用いたときの差分領域の画像の一例の説明図である。 図１８は、本発明の第１実施形態にかかるパーティクル数と処理時間との関係の情報の一例を示す説明図である。 図１９は、本発明の第１実施形態にかかる物体位置推定システムのＵＷＢタグリーダのタグ位置の測位方法を示した図である。 図２０Ａは、本発明の第１実施形態にかかる物体位置推定システムの観測データベースの一例を示す図である。 図２０Ｂは、本発明の第１実施形態にかかる物体位置推定システムの観測データベースの別の例を示す図である。 図２１は、本発明の第３実施形態にかかる物体位置推定システムの構成を示すブロック図である。 図２２は、本発明の第３実施形態にかかる物体位置推定システムの表示手段の一例を示す図である。 図２３は、本発明の第３実施形態にかかる物体位置推定システムの表示手段の一例を示す図である。 図２４は、本発明の第３実施形態にかかる物体位置推定システムの表示手段の一例を示す図である。 図２５は、本発明の第３実施形態にかかる物体位置推定システムの観測データベースの一例を示す図である。 図２６は、本発明の第１実施形態にかかる物体位置推定システムのマッチングスコアが記録された観測データベースの例を示す図である。

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

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

本発明の第１態様によれば、環境内に存在する物体を含む前記環境の状況を観測して、第１及び第２観測生データを取得する観測手段と、
物体データベースに予め記憶された前記物体のＩＤ毎の物体識別データと、前記第１及び第２の観測生データのそれぞれの観測生データとに基づいて、前記物体のＩＤ及び前記物体の位置候補をそれぞれ取得して、前記取得した物体のＩＤ及び前記物体の位置候補の情報を観測データベースにそれぞれ記憶する物体識別手段と、
前記第１及び第２観測生データのそれぞれの観測生データから前記環境内に存在する前記物体の数を取得し、前記物体の数をそれぞれ取得した時刻と、前記それぞれ取得した物体の数と、予め記憶された前記物体あたりの前記物体のＩＤ及び前記物体の位置候補をそれぞれ取得する物体識別処理に要する時間とに基づいて、前記物体識別処理の終了予定時刻をそれぞれ予測し、前記物体識別処理の終了予定時刻を前記観測データベースにそれぞれ記憶する物体識別処理終了時間予測手段と、
前記観測データベースに記憶された前記物体のＩＤと前記物体の位置候補とを取得し、前記取得した物体のＩＤ及び物体の位置候補と、前記物体の位置候補の分散状況とに基づいて、前記物体の位置を推定する物体位置推定手段と、
前記物体識別手段が前記第１観測生データの前記物体識別処理を終了して前記物体のＩＤ及び前記物体の位置候補を取得した時刻に、前記観測データベースに記憶された前記第２観測生データの前記物体識別処理の終了予定時刻を参照して、前記参照した時刻から前記第２観測生データの前記物体識別処理の終了予定時刻が記憶されているか否かを判断し、前記第２観測生データの前記物体識別処理の終了予定時刻が記憶されていると判断した場合には、予め記憶されかつ前記物体あたりの前記物体の位置を推定する位置推定処理を行うのに要する時間を参照して、前記参照した時刻から前記第２観測生データの前記物体識別処理の終了予定時刻までの時間において、前記物体識別処理が終了した前記第１観測生データの前記物体の位置推定処理を行うパラメータを決定する一方、前記第２観測生データの前記物体識別処理の終了予定時刻が記憶されていなかったと判断した場合には、前記物体識別処理が終了した前記第１観測生データの前記物体の位置推定処理を行うパラメータに、予め設定しておいた基準となるパラメータを設定する、パラメータ決定手段とを備えて、
前記物体位置推定手段は、前記パラメータ決定手段が決定した前記パラメータを使用して、前記物体識別処理が終了したデータから、そのデータに含まれている前記物体の位置推定処理を行い、前記物体ＩＤに関する前記物体の位置を、前記物体識別手段が取得した前記物体のＩＤと位置候補とに基づいて推定する物体位置推定システムを提供する。

本発明の第２態様によれば、前記物体識別データは前記物体の形状情報であり、
前記観測生データとは、カメラで撮像された画像データ、又は、測域センサで取得したレンジデータ、又は、タグリーダで取得した位置データである、第１の態様に記載の物体位置推定システムを提供する。

本発明の第３態様によれば、前記カメラ又は前記測域センサの物体識別処理は、テンプレートマッチング処理であって、
更に、前記物体識別手段は、前記テンプレートマッチング処理の過程にて出力されるマッチングスコアを前記観測データベースに記録し、
前記パラメータ決定手段は、前記マッチングスコアの高い順に前記位置推定処理を行う物体を決定する、第２の態様に記載の物体位置推定システムを提供する。

本発明の第４態様によれば、前記物体位置推定手段は、パーティクルフィルタを使用するとき、物体１個あたりの処理時間とパーティクルの個数との関係情報を予め用意しておき、前記参照した時刻から前記第２観測生データの前記物体識別処理の終了予定時刻までの時間から前記パーティクルフィルタで処理可能な物体の個数を算出し、算出された物体の個数が、前記物体識別処理で識別された物体の個数と同じならば、そのままパーティクルフィルタを使用して処理を行う一方、
前記算出された物体の個数が、前記物体識別処理で識別された物体の個数より少ないときには、物体１個あたりの前記パーティクルの個数を増やして処理を行う一方、
前記算出された物体の個数が、前記物体識別処理で識別された物体の個数より多いときには、物体１個あたりの前記パーティクルの個数を減らして処理を行う、第１又は２の態様に記載の物体位置推定システムを提供する。

本発明の第５態様によれば、更に前記物体位置推定手段が推定した結果を表示する表示手段を備え、
前記表示手段は、前記観測データベースを参照し、次回の物体識別処理の終了予定時刻を、次回の前記物体位置推定手段の推定結果の出力時刻として表示すると共に、現在の前記物体位置推定手段の推定結果を出力するに至った観測生データが得られた時刻を、現在の前記物体位置推定手段の推定結果が得られた時刻として表示する第１〜４のいずれか１つの態様に記載の物体位置推定システムを提供する。

本発明の第６態様によれば、少なくとも、物体識別処理された物体のＩＤと位置とに関するデータと、前記データの物体識別処理が終了したのちに入力される前記物体識別処理の終了予定時刻を入力可能なデータ入力部と、
前記データ入力部より受け取った前記物体のＩＤと位置とに関するデータと前記入力予定時刻に関するデータとを記憶する観測データベースと、
前記観測データベースに記憶されている前記物体のＩＤと位置とに関するデータから、前記物体の位置を推定する物体位置推定手段と、
前記観測データベースに記憶された前記物体識別処理の終了予定時刻を参照して、前記参照した時刻から最も早い時刻に終了する前記物体識別処理の終了予定時刻が記憶されているか否かを判断し、前記第２観測生データの前記物体識別処理の終了予定時刻が記憶されていると判断した場合には、予め記憶されかつ前記物体あたりの前記物体の位置を推定する前記物体識別処理を行うのに要する時間を参照して、前記参照した時刻から前記第２観測生データの前記物体識別処理の終了予定時刻までの時間において、前記物体の位置推定処理を行うパラメータを決定する一方、前記第２観測生データの前記物体識別処理の終了予定時刻が記憶されていなかったと判断した場合には、前記物体の位置推定処理を行うパラメータに、予め設定しておいた基準となるパラメータを設定する、パラメータ決定手段と、
を備えて、前記物体位置推定手段は、前記パラメータ決定手段が決定した前記パラメータを使用して、前記観測データベースに記憶された前記物体のＩＤと位置とに関するデータから前記物体の位置推定処理を行い、前記物体の位置を推定する物体位置推定装置を提供する。

本発明の第７態様によれば、環境内の状況を観測し、複数の観測生データを観測手段で取得する観測し、
前記複数の観測生データを基に、前記観測手段の観測範囲内に存在する物体のＩＤと位置とを物体識別手段で算出して物体識別処理を行い、
前記複数の観測生データを基に前記物体識別手段でそれぞれ行われる前記物体識別処理が終了する時刻を物体識別処理終了時間予測手段でそれぞれ予測して観測データベースにそれぞれ記憶し、
前記観測データベースに記憶された前記物体識別処理の終了予定時刻を参照して、前記参照した時刻から最も早い時刻に終了する前記物体識別処理の終了予定時刻が記憶されているか否かをパラメータ決定手段で判断し、
前記第２観測生データの前記物体識別処理の終了予定時刻が記憶されていると判断した場合には、予め記憶されかつ前記物体あたりの前記物体の位置を推定する位置推定処理を行うのに要する時間を参照して、前記参照した時刻から前記第２観測生データの前記物体識別処理の終了予定時刻までの時間において、前記物体の位置推定処理を行うパラメータを前記パラメータ決定手段で決定する一方、
前記第２観測生データの前記物体識別処理の終了予定時刻が記憶されていなかったと判断した場合には、前記物体の位置推定処理を行うパラメータに、予め設定しておいた基準となるパラメータを前記パラメータ決定手段で設定し、
前記パラメータ決定手段で設定した前記パラメータを使用して、前記物体識別処理で算出した前記物体のＩＤと位置とに関するデータから、前記物体の位置を物体位置推定手段で推定する、
ことを備える物体位置推定方法を提供する。

本発明の第８態様によれば、コンピュータに、
環境内の状況を観測手段で観測して取得された複数の観測生データを基に、前記観測手段の観測範囲内に存在する物体のＩＤと位置とを算出して物体識別処理を行う物体識別処理機能と、
前記複数の観測生データを基に前記物体識別処理機能による前記物体識別処理が終了する時刻を予測して観測データベースにそれぞれ記憶する処理終了時刻予測機能と、
前記観測データベースに記憶された前記物体識別処理の終了予定時刻を参照して、前記参照した時刻から最も早い時刻に終了する前記物体識別処理の終了予定時刻が記憶されているか否かを判断し、前記第２観測生データの前記物体識別処理の終了予定時刻が記憶されていると判断した場合には、予め記憶された前記物体あたりの前記物体の位置を推定する位置推定処理を行うのに要する時間を参照して、前記参照した時刻から前記第２観測生データの前記物体識別処理の終了予定時刻までの時間において、前記物体の位置推定処理を行うパラメータを決定する一方、前記第２観測生データの前記物体識別処理の終了予定時刻が記憶されていなかったと判断した場合には、前記物体の位置推定処理を行うパラメータに、予め設定しておいた基準となるパラメータを設定するパラメータ決定機能と、
前記パラメータ決定手段で設定した前記パラメータを使用して、前記物体識別処理機能による前記物体識別処理で算出した前記物体のＩＤと位置とに関するデータから、前記物体の位置を推定する物体位置推定機能と、
を実現させるための物体位置推定プログラムを提供する。

この構成によると、物体位置推定処理に必要となる次に観測生データが得られるまでの待ち時間内に処理可能な物体数から、物体位置推定処理を行う物体を決定することによって、実時間処理と物体位置推定の高精度化を両立することのできる物体位置推定システム、物体位置推定装置、物体位置推定方法、及び、物体位置推定プログラムを提供することができる。

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

（第１実施形態）
図１には、本発明の第１実施形態にかかる物体位置推定システムを示したブロック図を示す。

本システムは、観測手段（観測部）１０１と、物体位置推定装置とで大略構成されて、物体位置推定方法を実施するものである。前記物体位置推定装置は、物体データベース１０２と、物体識別手段（物体識別部）１０３と、識別処理終了時刻予測手段（識別処理終了時刻予測部）（以下、単に、「処理終了時刻予測手段」と称する。）１０４と、観測データベース１０５と、物体位置推定手段（物体位置推定部）１０６と、パラメータ決定手段（パラメータ決定部）１０７とで構成される。なお、観測手段１０１と物体識別手段１０３と処理終了時刻予測手段１０４とを合わせて、少なくとも、物体のＩＤと位置とに関するデータと、前記データの入力予定時刻を観測データベース１０５に入力可能なデータ入力部として取り扱うことも可能である。

観測手段１０１は、環境（観測対象となる空間、例えば、生活空間（より具体的には、リビングルームなどの部屋）など）内に存在する物体（観測物体）を含む環境の状況を観測して、観測生データを取得する。具体的には、環境内の物体を観測して、観測生データを得る。観測手段１０１としては、カメラや測域センサ、ＵＷＢ(Ultra Wide Band)タグリーダなどを利用して実現することができる。

物体データベース１０２には、物体のＩＤの情報（以下、単に、「物体のＩＤ」と称する。）と、物体を識別するための情報とが記憶（記憶）されている。図２に物体データベースの１０２の一例を示す。図２の物体データベースの１０２に記憶されている情報は、「Ｏｂｊ＿００１」から「Ｏｂｊ＿００５」までの物体ＩＤと、物体ＩＤが「Ｏｂｊ＿００１」から「Ｏｂｊ＿００５」までの５個の物体のそれぞれにおける形状情報とである。形状情報には、物体の外観に関する情報と大きさ（寸法）とに関する情報とが記憶されている。一例として、物体ＩＤ「Ｏｂｊ＿００１」の物体の外観と大きさとに関する情報を図２に拡大して示している。ここでは、物体ＩＤ「Ｏｂｊ＿００１」の物体は、大きな直径の円柱と、その上に固定された小さな直径の円柱とで構成され、大きな直径の円柱の直径は８ｃｍ、高さは１２ｃｍであり、小さな直径の円柱の直径は４ｃｍ、高さは２ｃｍである。

物体識別手段１０３は、観測手段１０１が観測した観測生データを物体識別処理することにより、観測生データ中に含まれる物体のＩＤと位置とに関する観測データを算出する。より具体的には、物体識別手段１０３は、物体データベース１０２に予め記憶された物体のＩＤ毎の物体を識別するための情報（物体識別データ）と、観測生データとに基づいて、物体のＩＤ及び物体の位置候補を取得して、取得した物体のＩＤ及び物体の位置候補の情報を観測データベース１０５に記憶する。「観測生データ」とは、観測手段１０１の観測に対して何も処理を施していない状態のデータであり、例えばカメラなどであれば、カメラで撮像された画像データに相当する。測域センサの場合、レンジデータに相当し、ＵＷＢタグリーダの場合、物体ＩＤ毎の位置データに相当する。一方、「観測データ」とは、観測生データに物体識別手段１０３を適用することにより、物体識別手段１０３で観測生データを使用して算出されるデータであって、物体位置推定手段１０６で取り扱うことのできる物体のＩＤと位置とに関するデータのことである。物体識別手段１０３と物体位置推定手段１０６とに関しては、後に説明を行う。

観測手段１０１がカメラである場合、観測生データは画像データであり、物体識別手段１０３は、カメラで撮像された画像データの画像処理を行うこととなる。画像処理には、例えばテンプレートマッチング処理などの手法が挙げられる。予め取得しておきかつ物体が環境内に置かれていないときの環境の背景画像と、現在の入力画像とを比較し、画素値が異なる領域を１つの差分領域として取り出す。ただし、物体識別手段１０３で差分領域として取り出すとき、入力画像にはノイズが加わる可能性もあるため、画素値が予め決められた個数以上連結している領域を差分領域とする等の制約を加えても良い。環境中にＮ個物体（ただし、Ｎは整数で１個以上。）が置かれていた場合、物体識別手段１０３で、差分領域はＮ個得られることになる。その後、物体識別手段１０３で得られた差分領域と、物体データベース１０２に記憶されている形状情報の外観データ（テンプレート画像）とのマッチング処理を物体識別手段１０３で行うことにより、物体のＩＤを物体識別手段１０３で識別することが可能となる。具体的には、差分領域と外観データとのマッチング率が最も高かった物体を、識別結果として、観測データベース１０５に登録する。尚、前記マッチングスコア（差分領域とテンプレート画像との間でマッチした割合）も、観測データベース１０５に登録しても良い。マッチングスコアの利用方法については後述する。

また、物体の環境中の位置に関しては、画像中の座標と環境中の座標とを対応付ける座標変換式などを物体識別手段１０３に予め用意しておくことで、物体識別手段１０３で算出できる。

図１５に環境の背景画像（環境背景画像）の一例を、図１６に入力画像の一例を、図１７に差分領域の画像の一例をそれぞれ示す。ここでは、環境の一例として生活環境であって、リビングなどの部屋１１０であるとする。準備する環境背景画像は、環境内に物体が何も置かれていない状態の背景を用いる。なお、図１５の環境背景画像のように、ほとんど移動しない本棚Ｂｓなどの家具又は家電機器を環境背景画像中に含めても構わない。ただし、この場合、本棚Ｂｓなどの家具又は家電機器を背景差分で取り出すことができなくなるため、本棚Ｂｓなどの家具又は家電機器は物体位置推定の対象から除外することとなる。図１６の入力画像は、食卓Ｄｔの上に財布Ｏ_３が、テーブルＴａの上に本Ｏ_２が置かれている状況を、部屋１１０の天井などに配置したカメラが観測したことを表している。図１７の差分領域は、図１５の環境背景画像と図１６の入力画像とで画素値が異なる領域、つまり、財布Ｏ_３の領域と本Ｏ_２の領域をそれぞれ表している。ここでは、一例として、財布Ｏ_３は、物体ＩＤが「Ｏｂｊ＿００３」の物体とし、本Ｏ_２は、物体ＩＤが「Ｏｂｊ＿００２」の物体としている。

観測手段１０１が、カメラの代わりに測域センサ４０１（図４参照）である場合、観測生データはレンジデータとなる。図３にレンジデータの一例を示す。図３は、センサ４０１の周り３６０度方向に対し３６０の分解能で距離を取得した場合のレンジデータであり、図４に示す状況において測域センサ４０１で観測を行ったときの観測結果である。前記レンジデータから物体のＩＤを識別するには、例えば物体の幅情報を用いて識別することができる。説明を簡単にするために、測域センサ４０１から１００ｃｍより離れた場所には壁４０２しか存在しないと仮定する。このとき、距離１００ｃｍ以内の観測値が得られているのは３６０度のうちの８５度〜９５度までの間であり、８５度における観測値と９５度における観測値が物体Ｏ_１の両端であると考える。測域センサ４０１の位置を（０ｃｍ，０ｃｍ，５ｃｍ）、＋Ｘ軸方向を０度、＋Ｙ軸方向を９０度とすると、物体Ｏ_１の両端の座標は（ｓｉｎ８５°×４６ｃｍ， ｃｏｓ８５°×４６ｃｍ）＝（４５．８３ｃｍ，４．０１ｃｍ）と、（ｓｉｎ９５°×４６ｃｍ， ｃｏｓ９５°×４６ｃｍ）＝（４５．８３ｃｍ，−４．０１ｃｍ）となり、設置面（測域センサの設置位置（０ｃｍ、０ｃｍ、５ｃｍ））から上部５ｃｍにおける物体Ｏ_１の幅は約８ｃｍであると推定できる。

この物体Ｏ_１の物体幅の推定結果と物体データベース１０２に記憶されている物体Ｏ_１の形状情報とから、例えば前記物体Ｏ_１はＩＤが「Ｏｂｊ＿００１」の物体であるとすることができる。また、物体Ｏ_１の環境中の位置は、両端の座標の中心値とすることができる。例えば、前記例では、（４５．８３ｃｍ，４．０１ｃｍ）の座標と（４５．８３ｃｍ，−４．０１ｃｍ）の座標との中心値である（４５．８３ｃｍ，０ｃｍ）を物体Ｏ_１の環境中の位置とすることができる。

観測手段１０１がＵＷＢタグリーダである場合、観測生データは物体ＩＤ毎の位置データとなる。ＵＷＢタグリーダは少なくとも３個使用する。ＵＷＢタグリーダを用いて物体を検出するためには、物体にタグを予め付与させておく必要がある。

少なくとも３個のＵＷＢタグリーダを用いて検出した物体のローカル座標は、例えば、三点測量の原理を用いて決定することができる。図１９に三点測量によるタグの位置を検出する例を示す。図１９に示される環境内には、それぞれＵＷＢタグリーダと接続された基地局１９０１Ａと基地局１９０１Ｂと基地局１９０１Ｃが配置されている。基地局１９０１Ａは、ローカル座標（ｘ，ｙ）＝（５００，６００）に配置され、基地局１９０１Ｂは、ローカル座標（ｘ，ｙ）＝（７００，３００）に配置され、基地局１９０１Ｃは、ローカル座標（ｘ，ｙ）＝（３００，３００）に配置されている。ここで、ＵＷＢタグリーダは、３つの基地局の配置されているローカル座標を知っているものとする。そして、各基地局は、各基地局から射出した電波が、物体に付与されたタグから、射出した基地局に返ってくる時間を計測することによって、前記射出した基地局からタグまでの距離を各基地局で算出することができる。このような状況において、タグ１９０２が付与された物体が、ローカル座標（ｘ，ｙ）＝（４００，４００）に存在したとする。基地局１９０１Ａは、基地局１９０１Ａの位置を中心とし、タグ１９０２までの距離２２３．６１ｃｍを半径とした円弧上にタグ１９０２が存在することが分かる。同様に、基地局１９０１Ｂは、半径３１６．２３ｃｍの円弧上にタグ１９０２が存在することが分かり、基地局１９０１Ｃは、半径１４１．４２ｃｍの円弧上にタグ１９０２が存在することが分かる。以上の３つの円弧が全て重なり合う位置がタグ１９０２の存在する位置であると判断することができる。図１９では、二次元空間におけるタグ位置検出の説明を行っている。実空間（三次元空間）におけるタグ位置検出を行う場合には、図１９の円弧が球面となるだけであり、他の処理に変化は無い。

処理終了時刻予測手段１０４は、物体識別手段１０３による物体識別処理（ここでは、一例として、テンプレートマッチング処理）に要する時間を、予め算出して観測データベース１０５に記憶しておく。次に、処理終了時刻予測手段１０４は、物体識別手段１０３の観測生データから物体のＩＤと位置とに関する観測データの数（又は、量）を算出する処理アルゴリズム又は処理数を物体識別手段１０３から受け取ることによって、物体識別手段１０３の処理終了予定時刻を予測して観測データベース１０５に記憶する。詳細については、以下に説明を行う。

物体識別手段１０３が、一例としてカメラの観測生データを処理している場合、処理終了時刻予測手段１０４は、物体識別手段１０３が差分画像を取り出した段階で、差分画像が何枚得られたかという情報を物体識別手段１０３から受け取り、差分画像が何枚得られたかという情報から、テンプレートマッチング処理に要する時間を予測する。例えば、１つの差分画像に対して５つのテンプレートとマッチング処理を行うのに０．２秒かかるとすると、５つの差分画像が物体識別手段１０３で得られていた場合、テンプレートマッチング処理に要する時間は１秒となる。このとき、処理終了予定時刻は、０．２秒×５＝１．０秒であるから、背景差分を取り出した時刻の１秒後ということになる。つまり、処理終了時刻予測手段１０４は、物体識別処理（ここでは、テンプレートマッチング処理）に要する時間を、予め算出しておく。

ここで、図１に示すように、観測手段１０１と物体データベース１０２と物体識別手段１０３と処理終了時刻予測手段１０４とをまとめて、１つの観測装置５００と呼ぶこととする。また、観測装置５００を個別に区別するときには、第１観測装置５０１，第２観測装置５０２，第３観測装置５０３，・・・，第ｎ観測装置（ただし、ｎは１以上の整数。）と呼ぶこととする。

観測データベース１０５には、観測装置５００のＩＤの情報（以下、単に「観測装置のＩＤ」と称する。以下では、便宜上、第１〜第３観測装置５０１，５０２，５０３のＩＤをそれぞれ「５０１」，「５０２」，「５０３」とする。）と、観測物体ＩＤとその環境中の観測物体の位置（例えば位置座標）とに関する観測データと、観測生データ処理終了予定時刻（例えば年月日と時分秒）とが記憶される。

ここで、観測データベース１０５にデータが物体識別手段１０３及び処理終了時刻予測手段１０４によりそれぞれ記憶されるまでの流れを説明する。

まず、観測手段１０１が環境内を観測し、観測生データを得る。

次いで、処理終了時刻予測手段１０４により、物体識別手段１０３が観測生データを処理するのに要する時間（観測生データ処理終了予定時刻）を算出し、観測データベース１０５に生データ処理終了予定時刻として、観測生データを取得した観測手段１０１を含む観測装置５０１，５０２，５０３のＩＤと共に観測データベース１０５に記憶する。

次いで、観測手段１０１で得られた観測生データを使用して、物体識別手段１０３で物体識別処理を開始する。

次いで、物体識別手段１０３は、観測生データの物体識別処理を終了すると、観測データを、観測装置５０１，５０２，５０３のＩＤと共に観測データベース１０５に記憶する。

図５に観測データベース１０５の一例を示す。

図５の（ａ）は、２００７年９月１１日の１４時１７分２０秒時点での観測データベース１０５のデータ記憶状況を示すデータテーブルを図示している（このデータ記憶状況を示すデータテーブルのＩＤを「ＴＢＬ＿００１」とする。）。図５の（ｂ）は、その６秒後の１４時１７分２６秒時点でのデータ記憶状況（このデータ状況を示すデータテーブルのＩＤを「ＴＢＬ＿００２」とする。）を示すデータテーブルを図示している。図５の（ｂ）では、図５の（ａ）と比較して、下から１行目と２行目のデータが追加されている。図５の（ｃ）は、図５の（ｂ）の状況から８秒後の１４時１７分３４秒時点でのデータ記憶状況（このデータ状況を示すデータテーブルのＩＤを「ＴＢＬ＿００３」とする。）を示すデータテーブルを図示している。図５の（ｃ）では、図５の（ｂ）と比較して、下から１行目と２行目のデータが追加されている。

図５の（ａ）のデータテーブルＩＤ「ＴＢＬ＿００１」のデータテーブルには、物体識別処理が終了した観測装置ＩＤ５０１からの観測データ「Ｏｂｊ＿００１（６５，２０）」と「Ｏｂｊ＿００２（１２０，２６）」と、処理終了予定時刻「１４時１７分２６秒」と「１４時１７分３４秒」を予測した観測装置ＩＤ５０２と５０３からの観測生データ処理の終了予定時刻がそれぞれ記憶されている。

図５の（ｂ）のデータテーブルＩＤ「ＴＢＬ＿００２」のデータテーブルには、観測装置ＩＤ５０３から観測生データ処理終了予定時刻通りに観測データ「Ｏｂｊ＿００４（２６６，３７）」と「Ｏｂｊ＿００５（３２２，２１）」がそれぞれ記憶され（データテーブルの下から２行目参照）、観測装置ＩＤ５０１からは観測生データの物体識別処理の終了予定時刻が新たに記憶されている（データテーブルの下から１行目参照）。図５の（ｃ）のデータテーブルＩＤ「ＴＢＬ＿００３」のデータテーブルについても同様である。すなわち、図５の（ｃ）のデータテーブルＩＤ「ＴＢＬ＿００３」のデータテーブルには、観測装置ＩＤ５０２から観測生データ処理終了予定時刻通りに観測データ「Ｏｂｊ＿００２（１１９，２８）」と「Ｏｂｊ＿００５（３２２，２１）」が記憶され（データテーブルの下から２行目参照）、観測装置ＩＤ５０３からは観測生データ処理の終了予定時刻が新たに記憶されている（データテーブルの下から１行目参照）。

図２６に、観測物体位置と共に、前記したマッチングスコアが記録された観測データベース１０５の例を示す。

図２６のデータテーブルＩＤ「ＴＢＬ＿００７」のデータテーブルには、観測ＩＤと観測物体位置と生データ処理終了予定時刻と観測時刻と、更にマッチングスコアとが記録されている。

マッチングスコアの項目には、識別した物体のＩＤとスコアとが記録されている。例えば、マッチングスコア「Ｏｂｊ＿００１（０．８）」は、物体を検出して得られた差分領域と物体「Ｏｂｊ＿００１」のテンプレートとのマッチングスコア（マッチングした割合）が８０％だったことを示している。

物体位置推定手段１０６は、パラメータ決定手段１０７から指定がありかつ観測データベース１０５に記憶された物体のＩＤと物体の位置候補とに関する観測データから、物体の位置候補の分散状況（例えば、物体の位置と重み付け）などを考慮して（具体的には、パーティクルフィルタ又はカルマンフィルタを用いて）、物体の位置を推定する。

ここで、物体位置推定手段１０６が推定する物体の位置と観測データベース１０５に記憶されている位置候補との違いについて説明する。観測データベース１０５に記憶されている前記物体の位置は、各観測装置５０１，５０２，５０３が個々に算出した位置であって、各観測装置５０１，５０２，５０３に含まれるノイズ等による位置誤差のため、各観測装置５０１，５０２，５０３で、通常、それぞれ異なっているため、前記物体の位置候補として取り扱う。これに対して、物体位置推定手段１０６が推定する物体の位置は、前記物体の位置候補の中から、物体位置推定処理により、最も存在する確率の高い位置を意味するものであり、両者は、明確に異なるものである。

また、パラメータ決定手段１０７についての説明は後に行う。 物体位置推定手段１０６の物体位置推定処理は、一例として、パーティクルフィルタを用いて実現することができる（例えば、文献：J. Hightower, B. Borriello. ” The location Stack: A Layered Model for Location in Ubiquitous Computing”, 4^th IEEE Workshop on Mobile Computing Systems & Applications (WMCSA2002), pp. 22-28. 2002. を参照）。「パーティクルフィルタ」とは、状態量と重みとを持つ多数のパーティクルで確率分布を近似する手法であり、非線形な分布であっても近似が可能である。ここで、「状態量」とは、物体の位置を示す。また、全パーティクルの重みの和は１であり、パーティクルの数は予め設定されている。

物体位置推定手段１０６によりパーティクルフィルタを使用して実現する物体位置推定方法について図６Ａを基に説明する。

図６Ａの第１ステップＳ１では、前回の各パーティクルの重みに基づき、パーティクルのリサンプリングを物体位置推定手段１０６で行う。ただし、初回は、環境内にランダムにパーティクルを物体位置推定手段１０６で生成するものとする。

次いで、第２ステップＳ２では、各パーティクルについて、前回位置から現在位置を物体位置推定手段１０６で予測する。

次いで、第３ステップＳ３では、観測データから得られる物体の存在位置確率を基に、各パーティクルの重みを物体位置推定手段１０６で計算する。

次いで、第４ステップＳ４では、全パーティクルの重み付き平均を物体位置推定手段１０６で求め、物体位置推定手段１０６により、求められた重み付き平均を物体の推定位置とする。

しかしながら、パーティクルフィルタを使用した物体位置推定方法によれば、パーティクルの数が多いほど近似の精度は高まるが、物体位置推定処理に要する計算時間が長くなる一方、パーティクルの数が少ないほど物体位置推定処理に要する計算時間は短くなるが、近似の精度が低くなる、という問題がある。図１４Ａは、パーティクルを１５個使って財布の位置推定を行っている例であり（図１４Ａの１個の黒丸が１個のパーティクルを示す。）、図１４Ｂは、パーティクルを３０個使って財布の位置推定を行っている例を示している。これらのパーティクルの座標がそれぞれ物体の位置候補である。図１４Ａを見ると、１５個のパーティクルの中心付近である座標（４０００，４０００）に財布がありそうだということが判別できる。しかし、図１４Ｂを見ると、パーティクル数が多く近似精度が高くなっているため、財布は、座標（４０００，４０００）以外に、座標（５０００，５０００）にある確率も残されていることが判別できる。これらの座標が物体の位置候補である。

パラメータ決定手段１０７は、観測生データ処理（物体識別処理）終了予定時刻と、現在の時刻と、物体位置推定手段１０６の処理能力と、現在未処理の観測データとの各情報を用いて、現在の時刻から観測生データ処理終了予定時刻までに未処理観測データを処理できるパーティクルの数を決定する。

なお、パラメータ決定手段１０７はその内部にタイマーを備えており、現在の時刻などの時刻の情報を取得することができ、観測生データ処理終了予定時刻は観測データベース１０５に記憶されている。

パラメータ決定手段１０７により決定したパーティクル数が、例えば前回の処理で使用したパーティクル数に対してパーティクル数が多い場合、パラメータ決定手段１０７により、増えた分のパーティクルを環境内に一様にばらまいても良いし、元の各パーティクルの重みに応じてばらまくのでも良い。逆に、パラメータ決定手段１０７により決定したパーティクル数が、前回の処理で使用したパーティクル数に対して少ない場合、パラメータ決定手段１０７により、減少分のパーティクル数を、前回の位置推定結果であるパーティクルの分布からランダムに削除しても良いし、又は、元の各パーティクルの重みに応じて削除しても良い。

なお、パラメータ決定手段１０７のパラメータ決定処理（パラメータ設定処理）に関しては、以下に説明する。

パラメータ決定手段１０７は、観測データベース１０５に記憶されている物体識別手段１０３の物体識別処理が終了する時刻と現在の時刻との時間差内に、未処理の観測データである物体を処理可能となるパーティクル数を決定する。

ここで、図１１Ａにパーティクル数と物体識別処理に要する時間との関係の情報（関係情報）の一例を示す。この関係情報は、例えば、パラメータ決定手段１０７の内部記憶部に記憶されている。図１１Ａは、例えば、１００個のパーティクルを用いてパーティクルフィルタを適用した場合、物体１個の位置推定を行うのに０．８秒の時間を要することを示し、以下、２００個、３００個、５００個、１０００個のパーティクルをそれぞれ用いてパーティクルフィルタを適用した場合、処理速度として、物体１個の位置推定を行うのに１秒、２秒、３秒、５秒の時間をそれぞれ要することを示している。なお、図１１Ａに示した関係の情報は、事前実験により予め得られているものとする。

図１１Ａの関係情報と図５の（ａ）のデータテーブルＩＤ「ＴＢＬ＿００１」のデータテーブルとをパラメータ決定手段１０７で用いてパーティクル数を決定する方法について説明する。

まず、現在の時刻に対して、次に観測データが記憶される時刻が図５の観測データベース１０５に記憶されているか否かをパラメータ決定手段１０７で判断し、パラメータ決定手段１０７が、次に観測データが記憶される時刻が観測データベース１０５に記憶されていると判断したならば、以下の動作を行う。例えば、図５の（ａ）に示すように現在の時刻は１７分２０秒（ここでは、説明の簡略上、年月日時は省略する。）で、観測データベース１０５に記憶された情報を参照すると、次に観測データが記憶される時刻が１７分２６秒となっている（図５の（ｂ）参照）。現在の時刻１７分２０秒における未処理の観測データは、第１観測装置５０１で取得した第１観測物体に関する観測データ「Ｏｂｊ＿００１」と第２観測物体に関する観測データ「Ｏｂｊ＿００２」の２個であり（図５の（ａ）の２行目参照）、次に観測データ（この例では、第３観測装置５０３で取得する第４観測物体と第５観測物体のそれぞれの観測データ）が記憶されるまで（図５の（ｂ）のデータテーブルＩＤ「ＴＢＬ＿００２」のデータテーブルが記憶されるまで）に、（現在の時刻：１７分２０秒）−（次の時刻：１７分２６秒）＝６秒の余裕があることが処理終了予測手段１０４で算出できる。このとき、現在の時点での未処理の観測データは物体２個であるため、６秒／２個＝３秒より、物体１個あたりに費やせる処理時間は３秒までとパラメータ決定手段１０７で算出できる。よって、図１１Ａの関係情報から、物体１個あたり３秒で処理できる（物体１個あたり３秒の処理速度での）パーティクル数は５００個であると、パラメータ決定手段１０７で設定（決定）することができる。

よって、物体位置推定手段１０６が処理する観測データ数と、次に観測データが記憶されるまでの時間とに応じて、パーティクルフィルタのパーティクル数をパラメータ決定手段１０７により設定（決定）し、パラメータ決定手段１０７で設定した５００個のパーティクルを使用して、物体位置推定手段１０６によりパーティクルフィルタを使用して、２個の物体の位置推定を行うことができる。この結果、物体位置推定処理に必要となる次の観測データが記憶されるまでの待ち時間内に、前記観測生データの物体識別処理を終了させることができる。言い換えれば、観測手段１０１の観測に対して遅れが出ることなく、物体の位置を推定することができる。

ここで、もし、次に観測データが記録される時間（観測生データ処理終了予定時刻）が、図５の観測データベース１０５に未だ記憶されていなかったとパラメータ決定手段１０７で判断した場合には、パラメータ決定手段１０７は、パーティクルの数を事前に決定しておいた基準となるパーティクル数（例えば、２００個）に決定する。

観測データベースの一例を示す図２０Ａの「ＴＢＬ＿００４」は、観測生データ処理終了予定時刻が記録されていない状況において、観測データが記録された観測データベース１０５の例を示している。「ＴＢＬ＿００４」に示したデータが得られている場合、上述したように、パラメータ決定手段１０７は、パーティクルの数を事前に決定しておいた基準となるパーティクル数に決定し、物体の位置を推定する。ここで、観測データベースの別の例を示す図２０Ｂの「ＴＢＬ＿００５」は、前記「ＴＢＬ＿００４」に示されたデータを用いて物体の位置を推定した後の観測データベース１０５の例を示している。「ＴＢＬ＿００５」には、観測装置５０３が出力した観測値「Ｏｂｊ＿００４（２６６， ３７）」，「Ｏｂｊ＿００５（３２２， ２１）」と、観測装置５０２が出力した観測値「Ｏｂｊ＿００２（１１９， ２８）」，「Ｏｂｊ＿００３（１９７， ６９）」，「Ｏｂｊ＿００４（２６６， ３６）」の計５個の観測データが未処理の観測データとして存在している。また、現在時刻は、２００７年９月１１日１４時１７分３４秒であり、次に観測データが記録される時刻は、２００７年９月１１日１４時１７分３８秒となっている。つまり、次の観測データが記録されるまでの４秒間の間に５個の観測データを処理可能なパーティクル数を、パラメータ決定手段１０７が決定することとなる。

また、時間単位は秒に限らず、例えば、「次に観測データが記憶されるまで」の時間を、「多く時間がかかる」場合と、「中くらい時間がかかる」場合と、「少しだけ時間がかかる」場合等との複数の段階（例えば、ここでは、３段階）に分けて表し、パーティクル数と物体１個あたりの処理時間の関係情報もそれに合わせて作成しておいても良い。そのときの、パーティクル数と処理時間との関係情報を図１８に示す。図１８では、例えば、「多く時間がかかる」（図１８では「長い」と表現している。）場合はパーティクル数を３００個とし、「中くらい時間がかかる」（図１８では「中くらい」と表現している。）場合はパーティクル数を２００個（基準パーティクル数）とし、「少しだけ時間がかかる」（図１８では「短い」と表現している。）場合はパーティクル数を１００個としている。このような関係情報を基に、パラメータ決定手段１０７によりパーティクルの数を設定してもよい。

前記第１実施形態によれば、物体位置推定手段１０６が処理する観測データ数と、次に観測データが記憶されるまでの時間とに応じて、パーティクルフィルタのパーティクル数をパラメータ決定手段１０７により変更することができる。つまり、基準又は初期の設定として、現在の時刻から次に観測データが記憶される時刻までの観測生データ処理可能時間とその観測生データ処理可能時間に処理可能なパーティクル数との関係情報をパラメータ決定手段１０７の内部記憶部に予め記憶しておく。そして、現在の時刻から次に観測データが記憶される時刻までの観測生データ処理可能時間が所定時間よりも短い場合には、パラメータ決定手段１０７により、所定時間に対応するパーティクル数よりも減らして物体識別処理に要する時間を削減する一方、現在の時刻から次に観測データが記憶される時刻までの観測生データ処理可能時間が前記所定時間よりも長い場合には、パーティクル数を前記所定数より増やして物体の位置を高精度に推定することができる。よって、物体位置推定処理に必要となる次に観測データが記憶されるまでの待ち時間内に、前記観測生データの物体識別処理を確実に終了させることができる。

（第２実施形態）
図１は、本発明の第２実施形態にかかる物体位置推定システムを示したブロック図でもある。すなわち、第２実施形態にかかる物体位置推定システムの大略構成は、第１実施形態にかかる物体位置推定システムと同様であり、後述するように、物体位置推定手段１０６とパラメータ決定手段１０７との機能が異なるのみである。

本システムは、観測手段１０１と、物体データベース１０２と、物体識別手段１０３と、処理終了時刻予測手段１０４と、観測データベース１０５と、物体位置推定手段１０６と、パラメータ決定手段１０７とで構成される。

物体位置推定手段１０６とパラメータ決定手段１０７と以外の各手段、及び各データベースについては、第１実施形態にて説明した機能と同等のため説明を省略する。

物体位置推定手段１０６として、パーティクルフィルタの代わりに、例えばカルマンフィルタなどを用いることができる。「カルマンフィルタ」とは、システムの状態（本発明の第２実施形態においては、例えば、物体位置）の情報にノイズが含まれ、観測値にもノイズが含まれる場合に、システムの取りうる状態の候補の中から、最も確率の高い状態を推定するものである。図６Ｂに物体位置推定処理にカルマンフィルタを利用した例を示す。縦軸は確率、横軸は位置を表す。

物体が下記の（数１）の式に表されるような移動を行うとき、観測手段１０１は下記の（数２）の式で求められる観測値６０３を得ることができる。ここで、Ａは物体の運動モデル、ｘは物体位置、ｖは移動の際に生じるプロセスノイズを表している。また、ｙは観測値、Ｈは物体位置ｘと観測値ｙを対応付ける観測モデル、ｗは観測ノイズ、ｔは時間を表している。

ここで、プロセスノイズｖ及び観測ノイズｗは、白色ガウスノイズとすると、ｐ（ｗ）は（数３）の式、ｐ（ｖ）は（数４）の式のように表される。尚、Ｎ（０，Ｑ）は、平均０、分散Ｑのガウス分布を表している。Ｎ（０，Ｒ）も同様に、平均０、分散Ｒのガウス分布を表している。

観測値６０３が得られると、現在得られている物体の位置に関する事前確率分布６０１（以後、「事前分布」と呼ぶ。）を物体位置推定手段１０６で更新し、予測分布６０２を物体位置推定手段１０６で作成する。（数５）の式で予測分布６０２の位置を物体位置推定手段１０６で求め、（数６）の式で予測分布６０２の分散を物体位置推定手段１０６で求めることができる。尚、「Ｘ_ａ｜ｂ」は時刻ｂの情報を基にした時刻ａのＸの推定値を表す。例えば、（数５）の「ｘ_{ｔ｜ｔ-１}」は時刻ｔ-１の情報を基にした時刻ｔの物体位置ｘの推定値を表しており、（数６）の「Ｐ_{ｔ｜ｔ-１}」は時刻ｔ−１の情報を基にした時刻ｔのＰの推定値を表している。ここで、Ｐは分布の分散を表している。

予測分布６０２が物体位置推定手段１０６で得られると、観測値６０３と予測分布６０２とから事後分布６０４を物体位置推定手段１０６で求める。（数７）の式で事後分布６０４の位置を物体位置推定手段１０６で求め、（数８）の式で事後分布６０４の分散を物体位置推定手段１０６で求めることができる。ここで、Ｋはカルマンゲインと呼ばれる値であり、（数９）の式にて求められる。カルマンゲインは更新量を決定する値である。観測値の精度が良い（分散Ｒが非常に小さい）場合には、更新量を大きくするため、カルマンゲインの値は大きくなる。逆に、事前分布の精度が良い（Ｐが非常に小さい）場合には、更新量を小さくするため、カルマンゲインの値は小さくなる。

パラメータ決定手段１０７は、観測生データ処理終了予定時刻と、現在の時刻と、物体位置推定手段１０６の処理能力と、現在未処理の観測データとの各情報を用いて、現在の時刻から観測生データ処理終了予定時刻までに、何個の物体の未処理観測データを処理できるかを推定する。

具体的には、図５の（ａ）のデータテーブルＩＤ「ＴＢＬ＿００１」のデータテーブルを用いて説明すると、例えば、現在の時刻は１７分２０秒（ここでは、説明の簡略上、年月日時は省略する。）で、次に観測データが記憶されるのが１７分２６秒となっている（図５の（ｂ）参照）。第１実施形態と同様に、現在の時刻１７分２０秒における未処理の観測データは、第１観測装置５０１で取得した第１観測物体に関する観測データ「Ｏｂｊ＿００１」と第２観測物体に関する観測データ「Ｏｂｊ＿００２」の２個であり（図５の（ａ）の２行目参照）、次に観測データ（この例では、第３観測装置５０３で取得する第４観測物体と第５観測物体のそれぞれの観測データ）が記憶されるまで（図５の（ｂ）のデータテーブルＩＤ「ＴＢＬ＿００２」のデータテーブルが記憶されるまで）に、（現在の時刻：１７分２０秒）−（次の時刻：１７分２６秒）＝６秒の余裕があることがパラメータ決定手段１０７で算出できる。

このとき、物体位置推定手段１０６が物体１個につき３秒の処理時間がかかるとすると、前記したように、次に観測データが記憶されるまでに６秒の余裕があることから、６秒／３秒＝２個であるから、２個の物体の位置を推定できることになり、第１観測装置ＩＤ「５０１」の観測装置５０１の観測手段１０１が観測した物体２個分の観測データは全て処理可能となる。しかし、物体位置推定手段１０６が物体１個につき５秒の処理時間がかかるとすると、次に観測データが記憶される前に６秒しか余裕が無いことから、６秒／５秒＝１．２個であるから、１個の物体の位置しか推定できないことになる。この場合、観測装置ＩＤ「５０１」の観測装置５０１の観測手段１０１が観測した２個の物体（Ｏｂｊ＿００１，Ｏｂｊ＿００２）のうち、どちらの物体位置を推定するかを物体位置推定手段１０６で選択する必要が出てくる。

物体位置推定手段１０６により、複数の物体の中から、位置推定を行う物体の選択方法について、図６Ｂと図７を用いて説明する。

図７は、３つの観測装置５０１、５０２、５０３を用いて環境内に存在する５個の物体を観測している様子を示しており、図８は、データテーブルＩＤ「ＴＢＬ＿００１」のデータテーブルが得られている時刻において、第１観測装置ＩＤ「５０１」の観測データを処理する直前の各物体の推定位置状況（例えば、物体位置推定手段１０６にカルマンフィルタを用いる場合、運動モデルを考慮した後の事前分布でも良い。）を示している。各観測装置５０１、５０２、５０３はカメラ又は測距センサである。

図８に物体位置の推定状況例を示す。図８に示されているのは、各物体の位置に関する確率分布であり、例えば、物体ＩＤが「Ｏｂｊ＿００１」の第１観測物体に関しては、平均位置（位置座標）が（ｘ，ｙ）＝（６０，２０）、分散共分散行列が（５^２，０，０，５^２）であることを示している。また、物体ＩＤが「Ｏｂｊ＿００２」の第２観測物体に関しては、平均位置（位置座標）が（ｘ，ｙ）＝（１２０，２５）、分散共分散行列が（２０^２，０，０，２０^２）であることを示している。

一例として、ここでは、物体ＩＤが「Ｏｂｊ＿００１」と「Ｏｂｊ＿００２」との２個の観測物体から、位置を推定する物体１つだけを物体位置推定手段１０６で選ぶ場合、分散がより大きい分布を持つ物体の位置を推定して物体位置推定手段１０６で選択することとする。この場合、第１観測物体の分散共分散行列が（５^２，０，０，５^２）であるのに対して第２観測物体の分散共分散行列が（２０^２，０，０，２０^２）であることから、物体ＩＤが「Ｏｂｊ＿００１」の第１観測物体の分布の分散より、物体ＩＤが「Ｏｂｊ＿００２」の第２観測物体の分布の分散の方が大きいため、物体ＩＤが「Ｏｂｊ＿００２」の物体の位置を物体位置推定手段１０６で推定させるとパラメータ決定手段１０７で決定する。これにより、位置が曖昧にしか推定できていない物体から、順に位置を推定することになり、全物体の位置推定精度について偏りを無くすことが可能となる。

また、位置を推定する物体を物体位置推定手段１０６で選択するのに、分散ではなく、分布の中心位置と観測位置との距離を物体位置推定手段１０６で比較して物体位置推定手段１０６で選択するようにしても良い。例えば、物体ＩＤが「Ｏｂｊ＿００１」の第１観測物体は分布の中心位置（位置座標）が（６０，２０）で第１観測装置５０１の観測手段１０１の観測位置（位置座標）が（６５，２０）であり、その差が５ｃｍである。それに対して、物体ＩＤが「Ｏｂｊ＿００２」の第２観測物体は分布の中心位置（位置座標）が（１２０，２５）で第２観測装置５０２の観測手段１０１の観測位置（位置座標）が（１２０，２６）であり、その差は１ｃｍしかないと仮定する。このとき、分布の中心位置と観測位置の差が大きな場合には、物体ＩＤが「Ｏｂｊ＿００１」の物体の位置を物体位置推定手段１０６で推定させると物体位置推定手段１０６で決定する。これにより、位置が曖昧にしか推定できていない物体から順に位置を物体位置推定手段１０６で推定することになり、全物体の位置推定精度についての偏りを無くすことが可能となる。物体位置推定手段１０６での物体位置推定後は、物体ＩＤが「Ｏｂｊ＿００１」の物体と物体ＩＤが「Ｏｂｊ＿００２」の物体は、共に処理済み観測データとなる。処理済み観測データか否かの物体位置推定手段１０６による判断は、パラメータ決定手段１０７が処理を行った観測データを観測データベース１０５で記憶しておいても良いし、又は、観測データベース１０５に処理済みであることを示す情報を物体位置推定手段１０６で更に記憶しておいて、物体位置推定手段１０６でそれを読み出すことにより判断を行うようにしても良い。なお、これらの場合には、図１において、パラメータ決定手段１０７から観測データベース１０５へのデータ（情報）の流れ、又は、物体位置推定手段１０６から観測データベース１０５へのデータ（情報）の流れが追加されることになる。

また、位置を推定する物体を物体位置推定手段１０６で選択するのに、マッチングスコアを利用しても良い。マッチングスコアが低いということは、検出した差分領域とテンプレート画像との間でマッチしなかった箇所が多く存在したことを表し、物体ＩＤの識別を誤っている可能性がある。つまり、マッチングスコアの低い物体ＩＤに関するデータを使用して位置推定を行うと、前記位置推定の結果も誤ってしまう恐れがある。そこで、マッチングスコアの高い物体ＩＤの順に、位置を推定することで、誤った位置推定を行うことを避けることも可能である。例えば、図２６の第一番目のデータ「Ｏｂｊ＿００１（６５，２０），Ｏｂｊ＿００２（１２０，２６）」の内、１つの物体しか位置推定を行う時間が無かったとする。このとき、「Ｏｂｊ＿００１」のマッチングスコア（０．８）が「Ｏｂｊ＿００２」のマッチングスコア（０．６）よりも高いため、「Ｏｂｊ＿００１」の位置推定を物体位置推定手段１０６で行うこととなる。

次に、物体位置推定システムの処理について、図９及び図１０に示すフローチャートを参照しながら説明する。図９は各観測装置５００の観測手段での観測動作及び物体識別手段１０３での物体識別処理及び処理終了時刻予測手段１０４での処理終了時刻予測処理のフローチャートであり、図１０は物体位置推定手段１０６での物体位置推定処理のフローチャートである。

始めに、図９の観測装置５００の観測動作などのフローチャートについて説明する。

ステップＳ９０１では、観測装置５００に備え付けられた観測手段１０１の一例であるセンサが環境を観測し、観測生データを取得する。

次いで、ステップＳ９０２では、物体識別手段１０３が、センサで取得した観測生データの物体識別処理を開始する。

次いで、ステップＳ９０３では、処理終了時刻予測手段１０４が、物体識別手段１０３の観測生データの物体識別処理内容から、物体識別処理の終了予定時刻を予測する。

次いで、ステップＳ９０４では、ステップＳ９０３で予測した観測生データの物体識別処理終了予定時刻を、処理終了時刻予測手段１０４が観測データベース１０５に記憶する。

次いで、ステップＳ９０５では、物体識別手段１０３は、前記観測生データの物体識別処理を終了し次第、観測データベース１０５に物体のＩＤと位置とに関する観測データを記憶する。

次に、図１０の物体位置推定処理に関するフローチャートについて説明する。

ステップＳ１００１では、パラメータ決定手段１０７が、観測データベース１０５に未処理の観測データが存在するか否かを判断する。観測データベース１０５に未処理の観測データが存在しないとパラメータ決定手段１０７で判断した場合には、未処理の観測データが存在するまで、このステップＳ１００１を繰り返す。観測データベース１０５に未処理の観測データが存在するとパラメータ決定手段１０７で判断した場合には、ステップＳ１００２に進む。

次いで、ステップＳ１００２では、パラメータ決定手段１０７が、観測データベース１０５から、次に物体のＩＤと位置とに関する観測データが記憶される時刻を取得する。

次いで、ステップＳ１００３では、パラメータ決定手段１０７が、ステップＳ１００２で取得した時刻とパラメータ決定手段１０７の内部のタイマーから取得した現在の時刻との時間差と、物体位置推定手段１０６の処理速度から物体位置推定処理を行うことのできる観測物体数を決定する。

次いで、ステップＳ１００４では、パラメータ決定手段１０７が、現時点で得られている各物体の物体推定位置状況から、推定位置と観測位置との差が大きい物体から順に、ステップＳ１００３で決定した前記観測物体数分だけ、位置推定処理を行う観測物体であると決定する。

次いで、ステップＳ１００５では、ステップＳ１００４でパラメータ決定手段１０７により位置推定処理を行う観測物体として決定した観測物体について、観測データベース１０５に記憶されている物体ＩＤと位置とに関する観測データを基に、物体の位置推定処理を物体位置推定手段１０６が行う。

図１１Ｂは、図５に示した観測データ、及び、観測生データ処理終了予定時刻を基に、物体の位置を推定したときのタイミングチャートである。尚、説明を簡単にするため、図１１Ｂでは第２観測装置５０２に関する処理は省略してあり、以下の説明でも第２観測装置５０２に関しては省略する。

図５に示すように、時刻１４：１７：２０（１４時１７分２０秒）（ここでは、説明の簡略上、年月日は省略する。）において、第１観測装置５０１で取得した観測生データの物体識別処理の結果である観測データとして、「Ｏｂｊ＿００１（６５，２０）」と「Ｏｂｊ＿００２（１２０，２６）」とが観測データベース１０５に記憶されると、観測データベース１０５に記憶されている観測生データ処理終了予定時刻をパラメータ決定手段１０７が参照して取得する。また、同じ時刻１４：１７：２０においては、図５に示すように、第３観測装置５０３の観測生データ処理終了予定時刻１４：１７：２６が観測データベース１０５に記憶されているため、次に、第３観測装置５０３の第４観測データ「Ｏｂｊ＿００４（２６６，３７）」及び第５観測データ「Ｏｂｊ＿００５（３２２，２１）」が記憶されるまでの６秒間、物体位置推定処理を行う時間があることとなる。このとき、１つの物体の位置を３秒以内で推定処理できるのであれば、「Ｏｂｊ＿００１（６５，２０）」と「Ｏｂｊ＿００２（１２０，２６）」の２つの観測データを用いて物体ＩＤが「Ｏｂｊ＿００１」と物体ＩＤが「Ｏｂｊ＿００２」との２つの物体の位置を物体位置推定処理手段１０６で推定処理できる。しかし、１つの物体の位置の推定処理に３秒より長い時間がかかる場合には、「Ｏｂｊ＿００１（６５，２０）」と「Ｏｂｊ＿００２（１２０，２６）」とのどちらかのデータを用いて、１つの物体の位置を物体位置推定処理手段１０６で推定処理することとなる。どちらのデータを用いるかの判断は既に説明済みのため、省略する。

もし、観測生データ処理終了予定時刻をパラメータ決定手段１０７が参照した結果、観測生データ処理終了予定時刻が記憶されていないとパラメータ決定手段１０７が判断した場合には、一例として、観測データに記憶されている「Ｏｂｊ＿００１（６５，２０）」と「Ｏｂｊ＿００２（１２０，２６）」との２つのデータを用いて物体位置推定処理を行う。以後、同じように処理を繰り返し、物体の位置の推定処理を行う。

図１１Ｂのタイミングチャートについて、詳細に説明する。
この図１１Ｂの例では、現在の時刻１４時１７分２０秒（ここでは、説明の簡略上、年月日は省略する。）までに、第１観測装置５０１の観測手段１０１により第１観測物体及び第２観測物体の観測生データをそれぞれ取得し（図１１Ｂの工程Ｐ_１参照）、処理終了時刻予測手段１０４により、物体識別手段１０３が観測生データをそれぞれ処理するのに要する時間（観測生データ処理終了予定時刻）の合計を算出し、観測データベース１０５に観測生データ処理終了予定時刻として、第１観測装置５０１のＩＤ「５０１」と共に観測データベース１０５に記憶する（図１１Ｂの工程Ｐ_２参照）。次いで、第１観測装置５０１で得られた観測生データを使用して、物体識別手段１０３で物体識別処理を開始し、観測生データの物体識別処理を終了して観測データを得ると、それらの観測データ「Ｏｂｊ＿００１（６５，２０）」と「Ｏｂｊ＿００２（１２０，２６）」とを第１観測装置５０１のＩＤ「５０１」と共に観測データベース１０５に記憶する（図５の（ａ）の２行目及び図１１Ｂの工程Ｐ_３参照）。この工程Ｐ_３の終了時刻が現在の時刻１４時１７分２０秒である。

このとき、工程Ｐ_１〜Ｐ_３と並行して、現在の時刻１４時１７分２０秒までに、第３観測装置５０３の観測手段１０１により第４観測物体及び第５観測物体の観測生データをそれぞれ取得し（図１１Ｂの工程Ｐ_４参照）、処理終了時刻予測手段１０４により、物体識別手段１０３が観測生データをそれぞれ処理するのに要する時間（観測生データ処理終了予定時刻）の合計を算出し、観測データベース１０５に観測生データ処理終了予定時刻として、第３観測装置５０３のＩＤ「５０３」と共に観測データベース１０５に記憶する（図５の（ａ）の３行目の「１４時１７分２６秒」及び図１１Ｂの工程Ｐ_５参照）。次いで、第３観測装置５０３で得られた観測生データを使用して、物体識別手段１０３で物体識別処理を開始する（図１１Ｂの工程Ｐ_６参照）。この工程Ｐ_６は、工程Ｐ_３が終了しても（言い換えれば、現在の時刻１４時１７分２０秒を過ぎても）続けられ、観測生データの物体識別処理を終了して観測データを得ると、その観測データ「Ｏｂｊ＿００４（２６６，３７）」及び「Ｏｂｊ＿００５（３２２，２１）」を第３観測装置５０３のＩＤ「５０３」と共に観測データベース１０５に記憶する（図５の（ｂ）の５行目及び図１１Ｂの工程Ｐ_６参照）。この工程Ｐ_６の終了時刻が現在の時刻１４時１７分２６秒である。

このようにして、現在の時刻１４時１７分２６秒までに、図５の（ａ）に示すように、第１観測装置５０１のＩＤ「５０１」と共に第１観測物体の観測データと第２観測物体の観測データとが観測データベース１０５に記憶されるとともに、第３観測装置５０３に関する観測生データ処理終了予定時刻が観測データベース１０５に記憶されることになる。

よって、パラメータ決定手段１０７により、図５の（ａ）のデータテーブルに示す第３観測装置５０３に関する観測生データ処理終了予定時刻を参照して、現在の時刻から観測生データ処理終了予定時刻までの時間｛（１４時１７分２６秒）−（１４時１７分２０秒）＝６秒｝を算出し、この算出された時間が物体位置推定処理を行う時間となる。このとき、１つの物体の位置を３秒以内で推定処理できるのであれば、「Ｏｂｊ＿００１（６５，２０）」と「Ｏｂｊ＿００２（１２０，２６）」の２つの観測データを用いて物体ＩＤが「Ｏｂｊ＿００１」と物体ＩＤが「Ｏｂｊ＿００２」との２つの物体の位置を物体位置推定処理手段１０６で推定処理できる（図１１Ｂの工程Ｐ_２１参照）。しかし、１つの物体の位置の推定処理に３秒より長い時間がかかる場合には、「Ｏｂｊ＿００１（６５，２０）」と「Ｏｂｊ＿００２（１２０，２６）」とのどちらかのデータを用いて、１つの物体の位置を物体位置推定処理手段１０６で推定処理することとなる（図１１Ｂの工程Ｐ_２１参照）。どちらのデータを用いるかの判断は既に説明済みのため、省略する。

また、現在の時刻１４時１７分２０秒以降では、第１観測装置５０１での物体識別処理が終了しているので、第１観測装置５０１により他の観測物体の観測動作を行う。すなわち、第１観測装置５０１の観測手段１０１により第３観測物体及び第４観測物体の観測生データをそれぞれ取得し（図１１Ｂの工程Ｐ_７参照）、処理終了時刻予測手段１０４により、物体識別手段１０３が観測生データをそれぞれ処理するのに要する時間（観測生データ処理終了予定時刻）の合計を算出し、観測データベース１０５に観測生データ処理終了予定時刻として、第１観測装置５０１のＩＤ「５０１」と共に観測データベース１０５に記憶する（図５の（ｂ）の最下行の「１４時１７分３８秒」及び図１１Ｂの工程Ｐ_８参照）。次いで、第１観測装置５０１で得られた観測生データを使用して、物体識別手段１０３で物体識別処理を開始し、観測生データの物体識別処理を終了して観測データを得ると、その観測データを第１観測装置５０１のＩＤ「５０１」と共に観測データベース１０５に記憶する（図１１Ｂの工程Ｐ_９参照）。この工程Ｐ_９の終了時刻が時刻１４時１７分３８秒である。

時刻１４時１７分２６秒になると、第１観測物体及び第２観測物体、又は、第１観測物体及び第２観測物体のいずれかの物体の物体位置推定処理工程Ｐ_２１が終了するので、続いて、パラメータ決定手段１０７により、図５の（ｂ）のデータテーブルに示す第１観測装置５０１に関する観測生データ処理終了予定時刻を参照して、１４時１７分２６秒から観測生データ処理終了予定時刻までの時間｛（１４時１７分３８秒）−（１４時１７分２６秒）＝８秒｝を算出し、この算出された時間が物体位置推定処理を行う時間となる。このとき、１つの物体の位置を４秒以内で推定処理できるのであれば、「Ｏｂｊ＿００４（２６６，３７）」と「Ｏｂｊ＿００５（３２２，２１）」の２つの観測データを用いて物体ＩＤが「Ｏｂｊ＿００４」と物体ＩＤが「Ｏｂｊ＿００５」との２つの物体の位置を物体位置推定処理手段１０６で推定処理できる（図１１Ｂの工程Ｐ_２２参照）。しかし、１つの物体の位置の推定処理に４秒より長い時間がかかる場合には、「Ｏｂｊ＿００４（２６６，３７）」と「Ｏｂｊ＿００５（３２２，２１）」とのどちらかのデータを用いて、１つの物体の位置を物体位置推定処理手段１０６で推定処理することとなる（図１１Ｂの工程Ｐ_２２参照）。どちらのデータを用いるかの判断は既に説明済みのため、省略する。

また、時刻１４時１７分２６秒以降では、第３観測装置５０３での物体識別処理が終了しているので、第３観測装置５０３により他の観測物体の観測動作を行う。すなわち、工程Ｐ_９と同時に、第３観測装置５０３の観測手段１０１により第１観測物体及び第２観測物体の観測生データをそれぞれ取得し（図１１Ｂの工程Ｐ_１０参照）、処理終了時刻予測手段１０４により、物体識別手段１０３が観測生データをそれぞれ処理するのに要する時間（観測生データ処理終了予定時刻）の合計を算出し、観測データベース１０５に観測生データ処理終了予定時刻として、第３観測装置５０３のＩＤ「５０３」と共に観測データベース１０５に記憶する（図５の（ｃ）の最下行の「１４時１７分４８秒」及び図１１Ｂの工程Ｐ_１１参照）。次いで、第３観測装置５０３で得られた観測生データを使用して、物体識別手段１０３で物体識別処理を開始する（図１１Ｂの工程Ｐ_１２参照）。この工程Ｐ_１２は、工程Ｐ_９が終了しても（言い換えれば、時刻１４時１７分３８秒を過ぎても）続けられ、観測生データの物体識別処理を終了して観測データを得ると、その観測データを第３観測装置５０３のＩＤ「５０３」と共に観測データベース１０５に記憶する（図１１Ｂの工程Ｐ_１２参照）。

その後、時刻１４時１７分３８秒になると、第４観測物体及び第５観測物体、又は、第４観測物体及び第５観測物体のいずれかの物体の物体位置推定処理工程Ｐ_２２が終了するので、続いて、パラメータ決定手段１０７により、前記したのと同様に、１４時１７分３８秒から観測生データ処理終了予定時刻までの時間を算出し、この算出された時間が、工程Ｐ_９で取得された観測データの物体位置推定処理を行う時間となる（図１１Ｂの工程Ｐ_２３参照）。

前記第２実施形態によれば、物体位置推定処理に必要となる次に観測データが記憶されるまでの待ち時間内に処理可能な物体数から、物体位置推定処理を行う物体を決定することによって、限られた時間内に、可能な限り物体位置の推定を行うことのできる物体位置推定システムを提供することができる。

（第３実施形態）
図２１は、本発明の第３実施形態にかかる物体位置推定システムを示すブロック図である。

本システムは、観測手段１０１と、物体データベース１０２と、物体識別手段１０３と、処理終了時刻予測手段１０４と、観測データベース１０５と、物体位置推定手段１０６と、パラメータ決定手段１０７と、表示手段１０８とで構成される。

表示手段１０８以外の各手段、及び各データベースについては、第１実施形態又は第２実施形態にて説明した機能と同等のため、それらの説明を省略する。

表示手段１０８は、物体位置推定手段１０６の物体位置の推定結果を受け取り、前記推定結果を表示する。表示手段１０８の一例としてはモニタなどを使用することができる。

また、表示手段１０８は、観測データベース１０５を参照し、現在推定されている物体の推定位置を出力するために使用した観測データが得られた時刻を、前回推定時刻として表示することができる。図２５の「ＴＢＬ＿００６」に、観測装置１０１が観測を行った時刻を記録可能な観測データベース１０５の例を示す。例えば、観測装置ＩＤ５０３の観測データ「Ｏｂｊ＿００４（２６６，３７），Ｏｂｊ＿００５（３２２，２１）」を使用して物体位置推定処理を行った場合、前記観測データが観測された時刻は、前回の観測装置ＩＤ５０３のデータから、時刻２００７年９月１１日１４時１７分１５秒であることが分かる。

さらに、表示手段１０８は、観測データベース１０５を参照し、次回の物体識別処理の終了予定時刻を、次回の前記物体位置推定手段１０６の物体位置推定結果の出力時刻として表示する。現在、図２５の「ＴＢＬ＿００６」の状態にある場合、次回の物体識別処理の終了予定時刻は、２００７年９月１１日１４時１７分３４秒であるため、表示手段１０８は、次回の物体位置の推定結果の出力時刻を２００７年９月１１日１４時１７分３４秒と表示する。

図２２に表示手段１０８の表示例を示す。図２２の表示例には、「Ｏｂｊ＿００１」，「Ｏｂｊ＿００２」，「Ｏｂｊ＿００３」，「Ｏｂｊ＿００４」，「Ｏｂｊ＿００５」の各物体の推定位置と、現在時刻、前回推定時刻、次回推定時刻の各情報とが表示されている。尚、表示手段１０８は、現在時刻を取得するためのタイマーを備えているものとする。

また、表示手段１０８は、物体の推定位置を確率密度分布で表示しても良い。物体の推定位置を確率密度分布で表示した例を図２３に示す。図２３には、「Ｏｂｊ＿００１」，「Ｏｂｊ＿００２」，「Ｏｂｊ＿００３」，「Ｏｂｊ＿００４」，「Ｏｂｊ＿００５」の各物体の位置の平均位置と分散とが数値とグラフとで表示されている。

また、表示手段１０８は、物体の推定位置を実環境(現在時刻での環境)の映像にオーバーレイさせて表示しても良い(ただし、図では、簡略化のため、部屋及び家屋などは図示を省略している。ここで言う実環境の映像とは、物体が映っている図を意味し、オーバーレイさせて表示しているのが、黒丸の推定位置と物体ＩＤになります。)。「Ｏｂｊ＿００１」，「Ｏｂｊ＿００２」，「Ｏｂｊ＿００３」，「Ｏｂｊ＿００４」，「Ｏｂｊ＿００５」の各物体の推定位置を実環境の映像にオーバーレイさせて表示した例を図２４に示す。尚、オーバーレイ表示を行う場合は、物体の推定位置と実環境における物体sの位置関係が理解し易いように、現在の実環境の映像を表示するのではなく、前回推定時刻に得られた映像を表示しても良い。

前記第３実施形態によれば、表示手段１０８を備えることによって、ユーザに物体の推定位置を提示することができ、次回の物体の位置推定の結果が出力される時刻もユーザに提示することができる。

前記各実施形態において、観測手段１０１と、物体データベース１０２と、物体識別手段１０３と、処理終了時刻予測手段１０４と、観測データベース１０５と、物体位置推定手段１０６と、パラメータ決定手段１０７となどのそれぞれ、又は、そのうちの任意の一部は、それ自体がソフトウェアで構成することができる。よって、例えば、前記第１，２，３実施形態の物体位置推定システム又は物体位置推定装置の物体位置推定動作のそれぞれのステップを有するコンピュータプログラムとして、記憶装置（ハードディスク等）などの記録媒体に読み取り可能に記憶させ、そのコンピュータプログラムをコンピュータの一時記憶装置（半導体メモリ等）に読み込んでＣＰＵを用いて実行することにより、前記した各ステップを実行することができる。

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

本発明の物体位置推定システム、物体位置推定装置、物体位置推定方法、及び、物体位置推定プログラムは、物体位置推定処理に必要となる次に観測データが記憶されるまでの待ち時間内を有効に活用すべく、物体推定処理のパラメータを可変にすることによって実時間処理と物体位置推定の高精度化を両立できるため、家庭内における物品の検索システム、又は、ロボットなどの移動体の追従に有用である。

本発明は、添付図面を参照しながら好ましい実施形態に関連して充分に記載されているが、この技術の熟練した人々にとっては種々の変形又は修正は明白である。そのような変形又は修正は、添付した請求の範囲による本発明の範囲から外れない限りにおいて、その中に含まれると理解されるべきである。

Claims (8)

1. 環境内に存在する物体を含む前記環境の状況を観測して、第１及び第２観測生データを取得する観測手段と、
   物体データベースに予め記憶された前記物体のＩＤ毎の物体識別データと、前記第１及び第２の観測生データのそれぞれの観測生データとに基づいて、前記物体のＩＤ及び前記物体の位置候補をそれぞれ取得して、前記取得した物体のＩＤ及び前記物体の位置候補の情報を観測データベースにそれぞれ記憶する物体識別手段と、
   前記第１及び第２観測生データのそれぞれの観測生データから前記環境内に存在する前記物体の数を取得し、前記物体の数をそれぞれ取得した時刻と、前記それぞれ取得した物体の数と、予め記憶された前記物体あたりの前記物体のＩＤ及び前記物体の位置候補をそれぞれ取得する物体識別処理に要する時間とに基づいて、前記物体識別処理の終了予定時刻をそれぞれ予測し、前記物体識別処理の終了予定時刻を前記観測データベースにそれぞれ記憶する物体識別処理終了時間予測手段と、
   前記観測データベースに記憶された前記物体のＩＤと前記物体の位置候補とを取得し、前記取得した物体のＩＤ及び物体の位置候補と、前記物体の位置候補の分散状況とに基づいて、前記物体の位置を推定する物体位置推定手段と、
   前記物体識別手段が前記第１観測生データの前記物体識別処理を終了して前記物体のＩＤ及び前記物体の位置候補を取得した時刻に、前記観測データベースに記憶された前記第２観測生データの前記物体識別処理の終了予定時刻を参照して、前記参照した時刻から前記第２観測生データの前記物体識別処理の終了予定時刻が記憶されているか否かを判断し、前記第２観測生データの前記物体識別処理の終了予定時刻が記憶されていると判断した場合には、予め記憶されかつ前記物体あたりの前記物体の位置を推定する位置推定処理を行うのに要する時間を参照して、前記参照した時刻から前記第２観測生データの前記物体識別処理の終了予定時刻までの時間において、前記物体識別処理が終了した前記第１観測生データの前記物体の位置推定処理を行うパラメータを決定する一方、前記第２観測生データの前記物体識別処理の終了予定時刻が記憶されていなかったと判断した場合には、前記物体識別処理が終了した前記第１観測生データの前記物体の位置推定処理を行うパラメータに、予め設定しておいた基準となるパラメータを設定する、パラメータ決定手段とを備えて、
   前記物体位置推定手段は、前記パラメータ決定手段が決定した前記パラメータを使用して、前記物体識別処理が終了したデータから、そのデータに含まれている前記物体の位置推定処理を行い、前記物体ＩＤに関する前記物体の位置を、前記物体識別手段が取得した前記物体のＩＤと位置候補とに基づいて推定する物体位置推定システム。
2. 前記物体識別データは前記物体の形状情報であり、
   前記観測生データとは、カメラで撮像された画像データ、又は、測域センサで取得したレンジデータ、又は、タグリーダで取得した位置データである、請求項１に記載の物体位置推定システム。
3. 前記カメラ又は前記測域センサの物体識別処理は、テンプレートマッチング処理であって、
   更に、前記物体識別手段は、前記テンプレートマッチング処理の過程にて出力されるマッチングスコアを前記観測データベースに記録し、
   前記パラメータ決定手段は、前記マッチングスコアの高い順に前記位置推定処理を行う物体を決定する、請求項２に記載の物体位置推定システム。
4. 前記物体位置推定手段は、パーティクルフィルタを使用するとき、物体１個あたりの処理時間とパーティクルの個数との関係情報を予め用意しておき、前記参照した時刻から前記第２観測生データの前記物体識別処理の終了予定時刻までの時間から前記パーティクルフィルタで処理可能な物体の個数を算出し、算出された物体の個数が、前記物体識別処理で識別された物体の個数と同じならば、そのままパーティクルフィルタを使用して処理を行う一方、
   前記算出された物体の個数が、前記物体識別処理で識別された物体の個数より少ないときには、物体１個あたりの前記パーティクルの個数を増やして処理を行う一方、
   前記算出された物体の個数が、前記物体識別処理で識別された物体の個数より多いときには、物体１個あたりの前記パーティクルの個数を減らして処理を行う、請求項１又は２に記載の物体位置推定システム。
5. 更に前記物体位置推定手段が推定した結果を表示する表示手段を備え、
   前記表示手段は、前記観測データベースを参照し、次回の物体識別処理の終了予定時刻を、次回の前記物体位置推定手段の推定結果の出力時刻として表示すると共に、現在の前記物体位置推定手段の推定結果を出力するに至った観測生データが得られた時刻を、現在の前記物体位置推定手段の推定結果が得られた時刻として表示する請求項１〜４のいずれか１つに記載の物体位置推定システム。
6. 少なくとも、環境内に存在する物体のＩＤ毎の物体識別データと、前記物体を含む前記環境の状況を観測手段で観測して取得した第１及び第２の観測生データのそれぞれの観測生データとに基づいて、それぞれ取得された前記物体あたりの前記物体のＩＤ及び前記物体の位置候補を入力可能であるとともに、前記第１及び第２観測生データのそれぞれの観測生データから前記環境内に存在する前記物体の数を取得し、前記物体の数をそれぞれ取得した時刻と、前記それぞれ取得した物体の数と、前記物体あたりの前記物体のＩＤ及び前記物体の位置候補をそれぞれ取得する物体識別処理に要する時間とに基づいてそれぞれ予測された前記物体識別処理の終了予定時刻を入力可能なデータ入力部と、
   前記データ入力部より受け取った前記物体のＩＤと位置候補と前記終了予定時刻に関するデータとを記憶する観測データベースと、
   前記観測データベースに記憶されている前記物体のＩＤと位置候補と、前記物体の位置候補の分散状況とに基づいて、前記物体の位置を推定する物体位置推定手段と、
   前記観測データベースに記憶された前記物体識別処理の終了予定時刻を参照して、前記参照した時刻から最も早い時刻に終了する前記物体識別処理の終了予定時刻が記憶されているか否かを判断し、前記観測生データの前記物体識別処理の終了予定時刻が記憶されていると判断した場合には、予め記憶されかつ前記物体あたりの前記物体の位置を推定する位置推定処理を行うのに要する時間を参照して、前記参照した時刻から前記観測生データの前記物体識別処理の終了予定時刻までの時間において、前記物体の位置推定処理を行うパラメータを決定する一方、前記観測生データの前記物体識別処理の終了予定時刻が記憶されていなかったと判断した場合には、前記物体の位置推定処理を行うパラメータに、予め設定しておいた基準となるパラメータを設定する、パラメータ決定手段と、
   を備えて、前記物体位置推定手段は、前記パラメータ決定手段が決定した前記パラメータを使用して、前記観測データベースに記憶された前記物体のＩＤと位置候補と前記物体の位置候補の分散状況とから前記物体の位置推定処理を行い、前記物体の位置を推定する物体位置推定装置。
7. 環境内の状況を観測し、複数の観測生データを観測手段で取得し、
   前記複数の観測生データを基に、テンプレートマッチング処理を使用して、前記観測手段の観測範囲内に存在する物体のＩＤと位置候補とを物体識別手段で算出する物体識別処理を行い、
   前記複数の観測生データを基に前記物体識別手段でそれぞれ行われる前記物体識別処理が終了する時刻を、前記それぞれの観測生データから前記環境内に存在する前記物体の数を取得し、前記物体の数をそれぞれ取得した時刻と、前記それぞれ取得した物体の数と、予め記憶された前記物体あたりの前記物体のＩＤ及び前記物体の位置候補をそれぞれ取得する物体識別処理に要する時間とに基づいて、物体識別処理終了時間予測手段でそれぞれ予測して観測データベースにそれぞれ記憶し、
   前記観測データベースに記憶された前記物体識別処理の終了予定時刻を参照して、前記参照した時刻から最も早い時刻に終了する前記物体識別処理の終了予定時刻が記憶されているか否かをパラメータ決定手段で判断し、
   前記観測生データの前記物体識別処理の終了予定時刻が記憶されていると判断した場合には、予め記憶されかつ前記物体あたりの前記物体の位置を推定する位置推定処理を行うのに要する時間を参照して、前記参照した時刻から前記観測生データの前記物体識別処理の終了予定時刻までの時間において、前記物体の位置推定処理を行うパラメータを前記パラメータ決定手段で決定する一方、
   前記観測生データの前記物体識別処理の終了予定時刻が記憶されていなかったと判断した場合には、前記物体の位置推定処理を行うパラメータに、予め設定しておいた基準となるパラメータを前記パラメータ決定手段で設定し、
   前記パラメータ決定手段で設定した前記パラメータを使用して、前記物体識別処理で算出した前記物体のＩＤと位置候補と、前記物体の位置候補の分散状況とに基づいて、前記物体の位置を物体位置推定手段で推定する、
   ことを備える物体位置推定方法。
8. コンピュータに、
   環境内の状況を観測手段で観測して取得された複数の観測生データを基に、テンプレートマッチング処理を使用して、前記観測手段の観測範囲内に存在する物体のＩＤと位置候補とを算出する物体識別処理を行う物体識別処理機能と、
   前記複数の観測生データを基に前記物体識別処理機能による前記物体識別処理が終了する時刻を、前記それぞれの観測生データから前記環境内に存在する前記物体の数を取得し、前記物体の数をそれぞれ取得した時刻と、前記それぞれ取得した物体の数と、予め記憶された前記物体あたりの前記物体のＩＤ及び前記物体の位置候補をそれぞれ取得する物体識別処理に要する時間とに基づいて、予測して観測データベースにそれぞれ記憶する処理終了時刻予測機能と、
   前記観測データベースに記憶された前記物体識別処理の終了予定時刻を参照して、前記参照した時刻から最も早い時刻に終了する前記物体識別処理の終了予定時刻が記憶されているか否かを判断し、前記観測生データの前記物体識別処理の終了予定時刻が記憶されていると判断した場合には、予め記憶された前記物体あたりの前記物体の位置を推定する位置推定処理を行うのに要する時間を参照して、前記参照した時刻から前記観測生データの前記物体識別処理の終了予定時刻までの時間において、前記物体の位置推定処理を行うパラメータを決定する一方、前記観測生データの前記物体識別処理の終了予定時刻が記憶されていなかったと判断した場合には、前記物体の位置推定処理を行うパラメータに、予め設定しておいた基準となるパラメータを設定するパラメータ決定機能と、
   前記パラメータ決定手段で設定した前記パラメータを使用して、前記物体識別処理機能による前記物体識別処理で算出した前記物体のＩＤと位置候補と、前記物体の位置候補の分散状況とに基づいて、前記物体の位置を推定する物体位置推定機能と、
   を実現させるための物体位置推定プログラム。

Images