JP2015031656A - 物体の位置推定方法と装置 - Google Patents

Abstract

【課題】計測範囲内に２以上の物体があり距離が離れている場合において、物体を識別することができ、かつ予測値に誤りがある場合でも正しく識別できる物体の位置推定方法と装置を提供する。【解決手段】（１）各物体ｉの位置Ｘｉの推定値とその誤差の大きさの第１指標値ＣＸｉｉを算出し、（２）推定値が「収束済み」か「収束中」かを判定し、（３）他の物体ｊの観測値Ｙｉｊを取得し、（４）他の物体ｊとしてあり得る仮説を作成し、（５）仮説ごとに予測値Ｙｉｊ＊を算出し、予測値Ｙｉｊ＊と観測値Ｙｉｊを比較して、最も尤度が高い他の物体ｊを選択し、（６）計測した計測体１４及び選択された他の物体ｊの両方が「収束中」の場合は、位置Ｘｉを修正せずに（１）に戻り、（７）どちらかが「収束済み」の場合は、他の物体ｊの位置Ｘｉを修正し、（１）〜（７）を繰り返して各物体ｉの位置Ｘｉを推定する。【選択図】図６

Description

本発明は、複数の物体の位置を推定する方法と装置に関する。

複数の物体（例えば移動体）が、互いに独立してエリア内を移動する場合に、複数の移動体が互いに衝突することを避ける制御や、先行する移動体に他の移動体を追従させる制御などを行うことがある。
なお、複数の移動体は、例えば、物品を搬送する移動ロボットであり、作業エリア内で自在に移動可能である。

上述の制御を行うために、特許文献１〜４が既に開示されている。

特許文献１は、各移動体に方位センサを設け、互いに計測した方位の計測結果を統合して、相対姿勢を求めるものである。
特許文献２は、光学マーカを用いて画像中の物体を抽出するものである。
特許文献３、４は、レーダーで何らかの目標を観測した値（観測ベクトル）と、追尾中の複数の目標位置を予測した値（予測ベクトル）とを比較して、差の小さいものを選出することで、観測ベクトルが、どの追尾目標のものかを判別するものである。

特開２０１３−６１３０７号公報 特開２０１３−５７５３０号公報 特開２０１１−１４５０９６号公報 特開２０１２−１８４９８９号公報

計測範囲内に２以上の物体がある場合、特許文献１の手段では、それぞれの物体を識別することはできない。

特許文献２の光学マーカを用いる場合、一般的なカメラとレンズの解像度で距離の離れた対象エリアを広い視野範囲で計測しようとすると、物体は画像上で数Ｐｉｘｅｌ程度の大きさとなる。そのため、計測範囲内に２以上の物体がある場合、光学マーカを用いてもそれぞれの物体を識別することは困難である。

また、レーザセンサを用いた場合でも、一般的なレーザ計測の分解能では物体に数点しかレーザ光が当たらないため、形状の特徴等から２以上の物体を識別することは困難である。

特許文献３、４は、いわゆる多重仮説トラッキングを用いており、計測範囲内に２以上の物体がある場合でも、それぞれの物体を識別することができる。
しかし多重仮説トラッキングは、観測値と予測値をつき合わせることで対象を識別するため、予測値に誤りがあると正しく識別できない問題点がある。特に観測値が一度も得られていない計測開始直後などに精度の高い予測値が得られないため識別ミスが起きやすい問題点があった。

また、例えば、各物体にＧＰＳを搭載すれば、各物体の正確な位置が取得できるが、ＧＰＳで正確な位置が取得できないような環境（屋内、市街地、森林等）においても、各物体の位置を推定する必要がある。

本発明は上述した問題点を解決するために創案されたものである。すなわち本発明の目的は、計測範囲内に２以上の物体があり、物体までの距離が離れている場合において、それぞれの物体を識別することができ、かつ検出されたデータに基づく予測値に誤りがある場合でも物体を正しく識別でき、これにより物体の位置の推定精度を高めることができる物体の位置推定方法と装置を提供することにある。

本発明によれば、３以上の物体と、各物体の位置を推定する推定演算装置とを用いた物体の位置推定方法であって、
前記物体のうち少なくとも１つは、他の物体の方位又は距離を計測可能なセンサを有する計測体であり、
前記推定演算装置は、前記計測体と通信可能であり、該推定演算装置により、
（Ｓ１）各物体の位置の推定値とその誤差の大きさの第１指標値を算出し、
（Ｓ２）各物体の前記第１指標値から前記推定値が「収束済み」か「収束中」かを判定し、
（Ｓ３）前記計測体から他の物体の方位又は距離を計測してその観測値を取得し、
（Ｓ４）前記他の物体としてあり得る１以上の仮説を作成し、
（Ｓ５）仮説ごとに、（Ｓ１）で算出した位置の推定値と前記第１指標値から、観測値の予測値とその誤差の大きさの第２指標値を算出し、得られた予測値と前記観測値を比較して、最も尤度が高い他の物体を選択し、
（Ｓ６）観測値を計測した前記計測体、及び、選択された他の物体の両方が（Ｓ２）において「収束中」の場合は、位置を修正せずに（Ｓ１）に戻り、
（Ｓ７）観測値を計測した前記計測体、又は、選択された他の物体のどちらかが「収束済み」の場合は、各物体の位置を修正し、
（Ｓ１）〜（Ｓ７）を繰り返して各物体の位置を推定する、ことを特徴とする物体の位置推定方法が提供される。

前記（Ｓ２）において、前記第１指標値の大きさが、第１閾値より小さい場合に「収束済み」と判定し、それ以外を「収束中」と判定する。

前記第１指標値の大きさは、誤差分散行列の対角項の和であり、前記第１閾値は、前記対角項の和より小さい値である。

前記（Ｓ５）において、いずれの仮説も第２閾値を超える尤度を持たない場合、（Ｓ３）における観測値を誤りと判定する。

前記計測体は、移動可能な移動体、又は、移動不能な固定体である。

計測体以外の物体のうち少なくとも１つは、位置が既知の基準マーカであり、基準マーカは常に収束済みと判定する。

また本発明によれば、３以上の物体と、各物体の位置を推定する推定演算装置とを備えた物体の位置推定装置であって、
前記物体のうち少なくとも１つは、他の物体の方位又は距離を計測可能なセンサを有する計測体であり、
前記推定演算装置は、前記計測体と通信可能であり、
各物体の位置の推定値とその誤差の大きさの第１指標値を算出する予測処理部と、
各物体の前記第１指標値から前記推定値が「収束済み」か「収束中」かを判定する収束判定部と、
前記計測体から他の物体の方位又は距離を計測してその観測値を取得する計測値取得部と、
前記他の物体としてあり得る１以上の仮説を作成する仮説作成部と、
仮説ごとに、予測処理部で算出した位置の推定値と前記第１指標値から、観測値の予測値とその誤差の大きさの第２指標値を算出し、得られた予測値と前記観測値を比較して、最も尤度が高い他の物体を選択する仮説選択部と、
観測値を計測した前記計測体、及び、選択された他の物体の両方が収束判定部において「収束中」と判定した場合は、位置を修正せずに予測処理部に戻す修正判定部と、
観測値を計測した前記計測体、又は、選択された他の物体のどちらかが「収束済み」の場合は、各物体の位置を修正する修正処理部と、を有し、
前記推定演算装置により、各物体の位置を推定する、ことを特徴とする物体の位置推定装置が提供される。

前記センサは、他の物体の方位を計測するカメラ、又は、レーザにより他の物体の距離を計測するレーザセンサである。

上記本発明の方法と装置は、（Ｓ２）と（Ｓ６）を省略したものが、一般的な多重仮説トラッキングの応用である。従って計測範囲内に２以上の物体があり、物体までの距離が離れている場合において、画像や距離データから物体を識別できない場合でも、予測値と観測値の比較により物体を識別できる。

また、観測側と被観測側の両方が収束していない場合は、予測値の精度が低く、（Ｓ５）の選択が間違っている可能性が高い。本発明では、（Ｓ６）において、観測値を計測した計測体、及び、選択された他の物体の両方が「収束中」の場合、位置を修正せずに（Ｓ１）に戻り、（Ｓ７）において識別ミスの少ない条件でのみ選択された他の物体の位置を修正処理するため、結果的に推定精度が高くなる。

本発明の使用例を示す図である。 推定演算装置と計測体の模式的構成図である。 推定演算装置の具体的構成図である。 計測体による計測の模式図である。 本発明の実施形態を示す図である。 本発明の位置推定方法の全体フロー図である。 本発明による推定の収束過程を示す模式図である。

本発明の好ましい実施形態を図面に基づいて説明する。なお、各図において共通する部分には同一の符号を付し、重複した説明を省略する。

図１は、本発明の使用例を示す図である。
本発明の位置推定装置は、３以上の物体１０と、各物体１０の位置を推定する推定演算装置２０とを備える。

３以上の物体１０のうち少なくとも１つは、他の物体１０の方位又は距離を計測可能なセンサ１２を有する計測体１４である。計測体１４は、移動可能な移動体１５又は移動不能な固定体１６である。また、計測体１４以外の物体１０のうち少なくとも１つは、位置が既知の基準マーカ１７（図５参照）であるのがよい。

基準マーカ１７は、物体１０の位置を基準座標上で求めるために用いる。また、計測体１４が基準マーカ１７となってもよい。さらに基準マーカ１７でも計測体１４ではないが位置を推定したい物体１０（位置が未知の物体１０）があってもよい。
基準マーカ１７を含む３以上の物体１０の位置がわかれば、３次元空間での位置を求めることができる。また、この効果に加えて、基準マーカ１７は始めから「収束済み」（後述する）と判定できる効果がある。

すなわち、本発明において、「３以上の物体１０」とは、以下の３通りの組み合わせを意味する。
（１）１以上の計測体１４（移動体１５又は固定体１６）と２以上の基準マーカ１７。この組み合わせに、単一の固定体１６と２つの基準マーカ１７の組み合わせを含む。単一の固定体１６から、２つの基準マーカ１７を識別する必要があるからである。
（２）２以上の計測体１４と１以上の基準マーカ１７。この組み合わせに、２つの固定体１６と単一の基準マーカ１７の組み合わせを含む。一方の固定体１６から、基準マーカ１７と他方の固定体１６を識別する必要があるからである。
（３）３以上の計測体１４。この組み合わせに、３つの固定体１６の組み合わせを含む。固定体１６の１つから、２つの固定体１６を識別する必要があるからである。
なお「３以上の物体１０」以外に、上述したように、基準マーカ１７でも計測体１４ではない物体１０があってもよい。

図１の例では、エリア内に移動体１５（例えば移動ロボット）や固定体１６（例えば固定デバイス）を配置し、移動体１５と固定体１６の位置を求める。得られた物体１０の位置は、移動体１５の制御やエリアの地図作成、監視などに用いる。

図２は、推定演算装置２０と計測体１４の模式的構成図である。
推定演算装置２０は、例えばコンピュータ（ＰＣ）であり、運動推定部２０ａと記憶装置２０ｂを有し、計測体１４と例えば無線ＬＡＮで通信可能に構成されている。
計測体１４（移動体１５又は固定体１６）は、計測体上ＰＣ１４ａ、コントローラ１４ｂ及びセンサ１２を有し、推定演算装置２０と好ましくはリアルタイムに無線ＬＡＮで通信可能に構成されている。計測体上ＰＣ１４ａは、推定演算装置２０と無線ＬＡＮで通信し、コントローラ１４ｂ及びセンサ１２を制御する。コントローラ１４ｂは、計測体１４（例えば移動体１５の操舵等）を制御する。

図３は、推定演算装置２０の具体的構成図である。
この図に示すように、推定演算装置２０は、予測処理部２１ａ、収束判定部２１ｂ、計測値取得部２１ｃ、仮説作成部２１ｄ、仮説選択部２１ｅ、修正判定部２１ｆ、及び修正処理部２１ｇを有し、各物体１０の位置Ｘ_ｉ（後述する）を推定する。
なお、推定演算装置２０は、予測処理部２１ａ、収束判定部２１ｂ、計測値取得部２１ｃ、仮説作成部２１ｄ、仮説選択部２１ｅ、修正判定部２１ｆ、及び修正処理部２１ｇの処理の順番に制約はない。

予測処理部２１ａは、各物体１０の位置Ｘ_ｉの推定値とその誤差の大きさの第１指標値ＣＸ_ｉｉ（後述する）を算出する。
収束判定部２１ｂは、各物体１０の第１指標値ＣＸ_ｉｉから推定値が「収束済み」か「収束中」かを判定する。
計測値取得部２１ｃは、計測体１４から他の物体１０の方位又は距離を計測してその観測値Ｙ_ｉｊ（後述する）を取得する。
仮説作成部２１ｄは、他の物体１０としてあり得る１以上の仮説を作成する。
仮説選択部２１ｅは、仮説ごとに、予測処理部２１ａで算出した位置Ｘ_ｉの推定値と第１指標値ＣＸ_ｉｉから、観測値Ｙ_ｉｊの予測値Ｙ_ｉｊ ^＊とその誤差の大きさの第２指標値Ｖ_ｉｊ（後述する）を算出し、得られた予測値Ｙ_ｉｊ ^＊と観測値Ｙ_ｉｊを比較して、最も尤度（後述する）が高い他の物体１０を選択する。
修正判定部２１ｆは、観測値Ｙ_ｉｊを計測した計測体１４、及び、選択された他の物体１０の両方が収束判定部２１ｂにおいて「収束中」と判定した場合は、位置を修正せずに予測処理部２１ａに戻す。
修正処理部２１ｇは、観測値Ｙ_ｉｊを計測した計測体１４、又は、選択された他の物体１０のどちらかが「収束済み」の場合は、各物体１０の位置Ｘ_ｉを修正する。

図４は、計測体１４による計測の模式図である。
計測体１４（移動体１５又は固定体１６）のセンサ１２は、他の物体１０の方位を計測するカメラ１２ａ、又は、レーザにより他の物体１０の距離を計測するレーザセンサ１２ｂである。カメラ１２ａとレーザセンサ１２ｂの両方を計測体１４に搭載することが好ましいが、一方のみでもよい。
また、センサ１２は、カメラ１２ａ、又は、レーザセンサ１２ｂの他にジャイロ（図示せず）を有してもよい。

カメラ１２ａにより、周囲の物体１０のある方位を方位角と仰角として計測することができる。なお方位角と仰角は一般的な極座標表現と同じ定義である。物体１０には、カメラ１２ａで検出するためのマーカ１０ａを設置するのが好ましい。マーカ１０ａには、例えば特許文献２に開示したものを用いる。
レーザセンサ１２ｂにより、周囲の物体１０までの距離を計測することができる。また、レーザセンサ１２ｂにより、距離だけでなく他の物体１０の方位も計測できる。

図５は、本発明の実施形態を示す図である。
この例では、３以上の物体１０は、単一のターゲット１１、４台の移動体１５、２つの基準マーカ１７である。ターゲット１１は、この例では移動体１５である。従ってこの例は、上述した（１）の組み合わせに該当する。

図５において、２つの基準マーカ１７は位置が既知であり、この例では一方がｘ−ｙ座標系の原点Ｏに位置し、他方がｘ軸上のｘ＝Ｌの位置に位置する。

以下、この例において、各物体１０の物体番号を符号ｉ（ｉ＝０〜６）で表す。すなわち、この例で、ｉ＝０はターゲット１１、ｉ＝１〜４は４台の移動体１５、ｉ＝５は原点の基準マーカ１７、ｉ＝６はｘ軸上の基準マーカ１７である。

図６は、本発明の位置推定方法の全体フロー図である。この図において、
本発明の位置推定方法は、Ｓ１〜Ｓ７の各ステップ（工程）からなる。

ステップＳ１では、各物体ｉの位置Ｘ_ｉの推定値とその誤差の大きさの第１指標値ＣＸ_ｉｉを算出する。
［数１］の（１ａ）〜（１ｃ）のように、各物体ｉの位置Ｘ_ｉとその誤差の大きさの第１指標値ＣＸ_ｉｉを定義する。

通常、各物体ｉがエリア内に配置された時点で大まかな配置は分かっている。そのため、位置Ｘ_ｉの初期値ｘ_０〜ｒｚ_０は、例えば位置誤差２ｍ，姿勢誤差２度程度の精度で設定する。

また、ＣＸ_ｘｘ〜ＣＸ_ｒｚｒｚは、設定した位置Ｘ_ｉの初期値誤差の大きさとして想定される値である。すなわち、位置Ｘ_ｉの位置誤差２ｍ、姿勢誤差２度程度の標準偏差が想定される場合、ＣＸ_ｘｘ〜ＣＸ_ｒｚｒｚは、以下のようになる。
ＣＸ_ｘｘ〜ＣＸ_ｚｚ＝２^２［ｍ^２］・・・（１ｄ）
ＣＸ_ｒｘｒｘ〜ＣＸ_ｒｚｒｚ＝（２×π／１８０）^２［ｒａｄ^２］・・・（１ｅ）

また、基準マーカ１７（ｉ＝５、６）は、固定マーカであるため、以下の値とする。
Ｘ_５の初期値：（０ ０ ０ ０ ０ ０）^Ｔ・・・（１ｆ）
Ｘ_６の初期値：（Ｌ ０ ０ ０ ０ ０）^Ｔ・・・（１ｇ）
Ｌ：基準マーカ間の距離
ＣＸ_５５、ＣＸ_６６：全て０・・・（１ｈ）

上述したステップＳ１の処理は、以下の３通りに該当する。
（Ｓ１−１）推定値と誤差分散の初期値を設定する。この場合は、最初の１回だけである。
（Ｓ１−２）前回修正した推定値と誤差分散をメモリから読み出す。この場合は、そのままデータを保持し続ける。
（Ｓ１−３）センサ１２の観測時刻に合うように、推定値と誤差分散データを時間遷移させる。この場合は、推定値と誤差分散データを予測する。
これらの処理（Ｓ１−１〜３）を併せて、「各物体ｉの位置Ｘ_ｉの推定値とその誤差の大きさの第１指標値ＣＸ_ｉｉを算出する。」ことになる。

（Ｓ１−３）の場合、「移動する物体の場合、位置Ｘ_ｉと誤差の大きさの第１指標値ＣＸ_ｉｉは、時間の経過と共に変化する。このような時間遷移を考慮して、Ｘ_ｉとＣＸ_ｉｉを変化させても良い。」

ステップＳ２では、各物体ｉの誤差の大きさの第１指標値ＣＸ_ｉｉから各物体ｉの位置Ｘ_ｉの推定値が「収束済み」か「収束中」かを判定する。
「収束済み」とは、位置Ｘ_ｉの推定値が、所定の精度内にあることを意味する。また「収束中」とは、位置Ｘ_ｉの推定値が、所定の精度外にあることを意味する。

例えばステップＳ２において、第１指標値ＣＸ_ｉｉの大きさが、第１閾値より小さい場合に「収束済み」と判定し、それ以外を「収束中」と判定する。この場合、第１指標値ＣＸ_ｉｉの大きさは、例えば誤差分散行列の対角項の和であり、第１閾値は、初期状態の誤差分散行列の対角項の和より小さい値であるのがよい。

各物体ｉが観測した又は観測された回数をカウントして、この回数が予め設定した回数以上になれば「収束済み」としてもよい。
また、一度「収束済み」となった物体ｉは、以降の処理では常に「収束済み」としても良い。なお、２つの基準マーカ１７は、計測開始直後から「収束済み」とする。

ステップＳ３では、計測体１４から他の物体ｊ（物体ｉ以外の物体）の方位又は距離を計測してその観測値Ｙ_ｉｊを取得する。すなわち、物体ｉ（計測体１４）上のセンサ１２（カメラ１２ａ又はレーザセンサ１２ｂ）で他の物体ｊの観測値Ｙ_ｉｊを得る。

ステップＳ４では、他の物体ｊ（物体ｉ以外の物体）としてあり得る１以上の仮説を作成する。すなわち、計測体１４の観測対象としてあり得るすべての他の物体ｊを仮説として仮定する。すなわち「仮説」とは、あり得るすべての他の物体ｊを意味する。
例えば、ステップＳ３にて、第３の移動体１５（ｉ＝３）が観測した場合、第３の移動体１５（ｉ＝３）以外の全て（ｉ＝３以外の０〜６）を観測対象の候補とする。また、「センサ１２が誤検出した」ことを仮定した仮説も作成する。

ステップＳ５では、上記仮説ごとに、ステップＳ１で算出した位置Ｘ_ｉの推定値と第１指標値ＣＸ_ｉｉから、観測値Ｙ_ｉｊの予測値Ｙ_ｉｊ ^＊とその誤差の大きさの第２指標値Ｖ_ｉｊを算出し、得られた予測値Ｙ_ｉｊ ^＊と実際の観測値Ｙ_ｉｊを比較して、最も尤度が高い他の物体ｊ（物体ｉ以外の物体）を選択する。ここで、「最も尤度が高い」とは「最も可能性が高い」ことを意味する。

すなわち、仮説ごとに、位置Ｘ_ｉの推定値と、位置Ｘ_ｉの第１指標値ＣＸ_ｉｉから、以下の予測値Ｙ_ｉｊ ^＊と予測値Ｙ_ｉｊ ^＊の誤差の大きさの第２指標値Ｖ_ｉｊを算出する。

予測値Ｙ_ｉｊ ^＊（＝ｇ（Ｘ_ｉ ^＊，Ｘ_ｊ ^＊）は、物体ｉから他の物体ｊを見た時の距離、方位角、仰角の予測値であり、［数２］の（２ａ）〜（２ｂ）で求めることができる。ここで、Ｘ_ｉ ^＊、Ｘ_ｊ ^＊は、観測時点での位置Ｘ_ｉを予測したものであり、ｘ_ｉ ^＊、ｘ_ｊ ^＊などはＸ_ｉ ^＊、Ｘ_ｊ ^＊の成分である。

Ｃ_ｎ ^ｂｉは、全体座標系から物体ｉのローカル座標系に変換する行列であり、［数３］の（３ａ）により、物体ｉの位置Ｘ_ｉの姿勢成分から算出する。ここで、ｃｏｓ（ｘ）＝ｃ（ｘ）、ｓｉｎ（ｘ）＝ｓ（ｘ）、ｔａｎ（ｘ）＝ｔ（ｘ）と略記する。
さらに予測値Ｙ_ｉｊ ^＊の誤差の大きさの第２指標値Ｖ_ｉｊは、［数３］の（３ｂ）（３ｃ）により求める。ここで、Ｒは、距離、方位角、仰角の計測誤差として想定される分散値である。

得られた予測値Ｙ_ｉｊ ^＊と実際の観測値Ｙ_ｉｊを比較して、仮説の尤度を算出し、最も尤度が高いものを選出する。尤度は、例えば［数４］の（４）より求める。ここで、ｅｒｒ＿ｙは、予測した観測値Ｙ_ｉｊ ^＊と、実際の観測値Ｙ_ｉｊとの差（Ｙ_ｉｊ−Ｙ_ｉｊ ^＊）である。

ステップＳ６では、観測値Ｙ_ｉｊを計測した計測体１４、及び、選択された他の物体ｊ（物体ｉ以外の物体）の両方がステップＳ２において「収束中」の場合は、ステップＳ６−２で位置Ｘ_ｉを修正せずにステップＳ１に戻る。
すなわち、選出した仮説が、観測側、被観測側共に「収束中」だった場合は、位置Ｘ_ｉを修正せずにステップＳ１に戻る。
また、選出した仮説が「誤検出仮説」の場合もステップＳ１に戻る。すなわち、ステップＳ５において、いずれの仮説も第２閾値を超える尤度を持たない場合、ステップＳ３における観測値Ｙ_ｉｊを誤りと判定して、位置Ｘ_ｉを修正せずにステップＳ１に戻る。第２閾値は、誤検出の場合の尤度を参考に設定するのがよい。

ステップＳ７では、観測値Ｙ_ｉｊを計測した計測体１４（物体ｉ）、又は、選択された他の物体ｊのどちらかが「収束済み」の場合は、選択された各物体ｉの位置Ｘ_ｉを修正する。
すなわち、ステップＳ６で選出した仮説に基づいて、各物体ｉの位置Ｘ_ｉの推定値を修正する。

位置Ｘ_ｉの修正式は、［数５］の（５ａ）で示すことができる。また、位置Ｘ_ｉの分散共分散の更新式は、［数５］の（５ｂ）で示すことができる。ここで、Ｃは観測値Ｙ_ｉｊが得られたときのカルマンゲイン、Ｂ_ｉｊは観測値Ｙ_ｉｊに対応する観測行列である。

（５ａ）（５ｂ）の各項は具体的には、［数６］の（６ａ）〜（６ｄ）で示すことができる。
なお、（６ｃ）の各成分Ｂｎの定義は式（３ｃ）、（６ｄ）のＶ_ｉｊは式（３ｂ）による。

ステップＳ１〜Ｓ７を繰り返すことで、観測対象を識別しながら、各物体ｉの位置Ｘ_ｉを推定する。
以上のステップＳ１〜Ｓ７を繰り返すことで、基準座標上での各移動体１５やターゲット１１の位置Ｘ_ｉを同定できる。

図７は、本発明による推定の収束過程を示す模式図である。この図において、（Ａ）は初期状態、（Ｂ）は移動体１５−１、１５−２間での計測、（Ｃ）は移動体１５−２、１５−３間での計測、（Ｄ）は移動体１５−３による基準マーカ１７の計測を示している。
また、この図において、移動体１５−１、１５−２、１５−３は、真の位置にあり、それぞれの推定位置を１８−１、１８−２、１８−３とする。各推定位置１８−１、１８−２、１８−３は、推定誤差の期待値（誤差範囲）を有している。図中の破線の円は、誤差範囲の大きさを模式的に示している。

以下、上述したステップＳ２、Ｓ６によって推定が収束していく過程を説明する。

（１）ステップＳ２に記載したように、計測開始直後は基準マーカ１７のみが「収束済み」である。

（２）図７（Ｂ）に示すように、基準マーカ１７から離れた移動体１５−１、１５−２間で観測した場合は、移動体１５−１、１５−２の両方が「収束中」（誤差範囲が大きい状態）であるため、ステップＳ６、Ｓ６−２にて、修正処理が省略される。図７（Ｃ）の場合も同様である。
従って、図７（Ｄ）に示すように、計測開始直後は、基準マーカ１７に近い移動体１５−３が基準マーカ１７を観測した場合にのみ、どちらかが「収束済み」（誤差範囲が小さい状態）であり、ステップＳ６を通過してステップＳ７で修正処理される。

（３）（２）の結果、移動体１５−３の位置Ｘ_ｉが収束していき、ステップＳ２で「収束済み」と判定される。

（４）図７（Ｃ）に示すように、移動体１５−３が移動体１５−２を観測した場合、あるいは、移動体１５−２が移動体１５−３を観測した場合も修正処理される。これにより、移動体１５−２の位置Ｘ_ｉが収束して行き、ステップＳ２で「収束済み」と判定される。
（５）図７（Ｂ）に示すように、移動体１５−２が移動体１５−１を観測した場合、あるいは、移動体１５−１が移動体１５−２を観測した場合も修正処理される。これにより、移動体１５−１の位置Ｘ_ｉが収束して行き、ステップＳ２で「収束済み」と判定される。
（６）移動体１５−１がターゲット１１（図示せず、図５参照）を観測した場合も、修正処理される。これにより、ターゲット１１の位置Ｘ_ｉが収束して、全ての移動体１５、ターゲット１１の位置Ｘ_ｉが推定される。

上述した実施形態では、観測側、被観測側ともに「収束中」の場合は修正処理をしないとしている。しかし、本発明はこれに限定されず、通常よりも少ない量で修正しても良い。例えば、カルマンゲインに１未満の係数を掛けて修正処理してもよい。

上述した本発明の方法と装置は、（Ｓ２）と（Ｓ６）を省略したものが、一般的な多重仮説トラッキングの応用である。従って計測範囲内に２以上の物体ｉがあり、物体ｉまでの距離が離れている場合において、画像や距離データから物体ｉを識別できない場合でも、予測値と観測値Ｙ_ｉｊの比較により物体ｉを識別できる。

また、観測側と被観測側の両方が収束していない場合は、予測値の精度が低く、（Ｓ５）の選択が間違っている可能性が高い。本発明では、（Ｓ６）において、観測値Ｙ_ｉｊを計測した計測体１４、及び、選択された他の物体ｊ（物体ｉ以外の物体）の両方が「収束中」の場合、位置Ｘ_ｉを修正せずに（Ｓ１）に戻り、（Ｓ７）において識別ミスの少ない条件でのみ選択された他の物体ｊの位置Ｘ_ｉを修正処理するため、結果的に推定精度が高くなる。

２以上の物体ｉの位置Ｘ_ｉ（例えば位置）を同定する際、位置Ｘ_ｉが既知の基準マーカ１７から離れた物体ｉで観測すると、仮に正しく識別できても推定誤差が大きい状態で収束することがある。そのため、この誤差の影響で他の物体ｊ（物体ｉ以外の物体）間での観測を識別できなくなる可能性がある。
これに対し、本発明では、基準マーカ１７から近い順に推定値が収束していくため、推定値が正しく収束し、識別ミスを起こしにくい。

本発明は上述した実施形態に限定されず、本発明の要旨を逸脱しない範囲で種々変更を加え得ることは勿論である。

ｉ、ｊ 物体番号（物体）、Ｘ_ｉ 位置、
ＣＸ_ｉｉ 位置Ｘ_ｉの誤差の大きさの第１指標値、
ＣＸ_ｘｘ〜ＣＸ_ｒｚｒｚ 位置Ｘ_ｉの初期値誤差の想定値、
Ｙ_ｉｊ 実際の観測値、
Ｙ_ｉｊ ^＊ 得られた予測値、
Ｖ_ｉｊ 予測値Ｙ_ｉｊ ^＊の誤差の大きさの第２指標値、
１０ 物体、１０ａ マーカ、１１ ターゲット、
１２ センサ、１２ａ カメラ、１２ｂ レーザセンサ、
１４ 計測体、１４ａ 計測体上ＰＣ、１４ｂ コントローラ、
１５、１５−１、１５−２、１５−３ 移動体（移動ロボット）、
１６ 固定体（固定デバイス）、１７ 基準マーカ、
１８−１、１８−２、１８−３ 推定位置、
２０ 推定演算装置（コンピュータ）、
２０ａ 運動推定部、２０ｂ 記憶装置、
２１ａ 予測処理部、２１ｂ 収束判定部、２１ｃ 計測値取得部、
２１ｄ 仮説作成部、２１ｅ 仮説選択部、２１ｆ 修正判定部、
２１ｇ 修正処理部

Claims (8)

1. ３以上の物体と、各物体の位置を推定する推定演算装置とを用いた物体の位置推定方法であって、
   前記物体のうち少なくとも１つは、他の物体の方位又は距離を計測可能なセンサを有する計測体であり、
   前記推定演算装置は、前記計測体と通信可能であり、該推定演算装置により、
   （Ｓ１）各物体の位置の推定値とその誤差の大きさの第１指標値を算出し、
   （Ｓ２）各物体の前記第１指標値から前記推定値が「収束済み」か「収束中」かを判定し、
   （Ｓ３）前記計測体から他の物体の方位又は距離を計測してその観測値を取得し、
   （Ｓ４）前記他の物体としてあり得る１以上の仮説を作成し、
   （Ｓ５）仮説ごとに、（Ｓ１）で算出した位置の推定値と前記第１指標値から、観測値の予測値とその誤差の大きさの第２指標値を算出し、得られた予測値と前記観測値を比較して、最も尤度が高い他の物体を選択し、
   （Ｓ６）観測値を計測した前記計測体、及び、選択された他の物体の両方が（Ｓ２）において「収束中」の場合は、位置を修正せずに（Ｓ１）に戻り、
   （Ｓ７）観測値を計測した前記計測体、又は、選択された他の物体のどちらかが「収束済み」の場合は、各物体の位置を修正し、
   （Ｓ１）〜（Ｓ７）を繰り返して各物体の位置を推定する、ことを特徴とする物体の位置推定方法。
2. 前記（Ｓ２）において、前記第１指標値の大きさが、第１閾値より小さい場合に「収束済み」と判定し、それ以外を「収束中」と判定する、ことを特徴とする請求項１に記載の物体の位置推定方法。
3. 前記第１指標値の大きさは、誤差分散行列の対角項の和であり、前記第１閾値は、前記対角項の和より小さい値である、ことを特徴とする請求項２に記載の物体の位置推定方法。
4. 前記（Ｓ５）において、いずれの仮説も第２閾値を超える尤度を持たない場合、（Ｓ３）における観測値を誤りと判定する、ことを特徴とする請求項１に記載の物体の位置推定方法。
5. 前記計測体は、移動可能な移動体、又は、移動不能な固定体である、ことを特徴とする請求項１に記載の物体の位置推定方法。
6. 計測体以外の物体のうち少なくとも１つは、位置が既知の基準マーカであり、基準マーカは常に収束済みと判定する、ことを特徴とする請求項１に記載の物体の位置推定方法。
7. ３以上の物体と、各物体の位置を推定する推定演算装置とを備えた物体の位置推定装置であって、
   前記物体のうち少なくとも１つは、他の物体の方位又は距離を計測可能なセンサを有する計測体であり、
   前記推定演算装置は、前記計測体と通信可能であり、
   各物体の位置の推定値とその誤差の大きさの第１指標値を算出する予測処理部と、
   各物体の前記第１指標値から前記推定値が「収束済み」か「収束中」かを判定する収束判定部と、
   前記計測体から他の物体の方位又は距離を計測してその観測値を取得する計測値取得部と、
   前記他の物体としてあり得る１以上の仮説を作成する仮説作成部と、
   仮説ごとに、予測処理部で算出した位置の推定値と前記第１指標値から、観測値の予測値とその誤差の大きさの第２指標値を算出し、得られた予測値と前記観測値を比較して、最も尤度が高い他の物体を選択する仮説選択部と、
   観測値を計測した前記計測体、及び、選択された他の物体の両方が収束判定部において「収束中」と判定した場合は、位置を修正せずに予測処理部に戻す修正判定部と、
   観測値を計測した前記計測体、又は、選択された他の物体のどちらかが「収束済み」の場合は、各物体の位置を修正する修正処理部と、を有し、
   前記推定演算装置により、各物体の位置を推定する、ことを特徴とする物体の位置推定装置。
8. 前記センサは、他の物体の方位を計測するカメラ、又は、レーザにより他の物体の距離を計測するレーザセンサである、ことを特徴とする請求項７に記載の物体の位置推定装置。

Images