(58)【調査した分野】(Int.Cl.,DB名)
前記(S2)において、前記第1指標値の大きさが、第1閾値より小さい場合に「収束済み」と判定し、それ以外を「収束中」と判定する、ことを特徴とする請求項1に記載の物体の位置推定方法。
前記(S5)において、いずれの仮説も第2閾値を超える尤度を持たない場合、(S3)における観測値を誤りと判定する、ことを特徴とする請求項1に記載の物体の位置推定方法。
計測体以外の物体のうち少なくとも1つは、位置が既知の基準マーカであり、基準マーカは常に収束済みと判定する、ことを特徴とする請求項1に記載の物体の位置推定方法。
前記センサは、他の物体の方位を計測するカメラ、又は、レーザにより他の物体の距離を計測するレーザセンサである、ことを特徴とする請求項7に記載の物体の位置推定装置。
【発明の概要】
【発明が解決しようとする課題】
【0006】
計測範囲内に2以上の物体がある場合、特許文献1の手段では、それぞれの物体を識別することはできない。
【0007】
特許文献2の光学マーカを用いる場合、一般的なカメラとレンズの解像度で距離の離れた対象エリアを広い視野範囲で計測しようとすると、物体は画像上で数Pixel程度の大きさとなる。そのため、計測範囲内に2以上の物体がある場合、光学マーカを用いてもそれぞれの物体を識別することは困難である。
【0008】
また、レーザセンサを用いた場合でも、一般的なレーザ計測の分解能では物体に数点しかレーザ光が当たらないため、形状の特徴等から2以上の物体を識別することは困難である。
【0009】
特許文献3、4は、いわゆる多重仮説トラッキングを用いており、計測範囲内に2以上の物体がある場合でも、それぞれの物体を識別することができる。
しかし多重仮説トラッキングは、観測値と予測値をつき合わせることで対象を識別するため、予測値に誤りがあると正しく識別できない問題点がある。特に観測値が一度も得られていない計測開始直後などに精度の高い予測値が得られないため識別ミスが起きやすい問題点があった。
【0010】
また、例えば、各物体にGPSを搭載すれば、各物体の正確な位置が取得できるが、GPSで正確な位置が取得できないような環境(屋内、市街地、森林等)においても、各物体の位置を推定する必要がある。
【0011】
本発明は上述した問題点を解決するために創案されたものである。すなわち本発明の目的は、計測範囲内に2以上の物体があり、物体までの距離が離れている場合において、それぞれの物体を識別することができ、かつ検出されたデータに基づく予測値に誤りがある場合でも物体を正しく識別でき、これにより物体の位置の推定精度を高めることができる物体の位置推定方法と装置を提供することにある。
【課題を解決するための手段】
【0012】
本発明によれば、3以上の物体と、各物体の位置を推定する推定演算装置とを用いた物体の位置推定方法であって、
前記物体のうち少なくとも1つは、他の物体の方位又は距離を計測可能なセンサを有する計測体であり、
前記推定演算装置は、前記計測体と通信可能であり、該推定演算装置により、
(S1)各物体の位置の推定値とその誤差の大きさの第1指標値を算出し、
(S2)各物体の前記第1指標値から前記推定値が「収束済み」か「収束中」かを判定し、
(S3)前記計測体から他の物体の方位又は距離を計測してその観測値を取得し、
(S4)前記他の物体としてあり得る1以上の仮説を作成し、
(S5)仮説ごとに、(S1)で算出した位置の推定値と前記第1指標値から、観測値の予測値とその誤差の大きさの第2指標値を算出し、得られた予測値と前記観測値を比較して、最も尤度が高い他の物体を選択し、
(S6)観測値を計測した前記計測体、及び、選択された他の物体の両方が(S2)において「収束中」の場合は、位置を修正せずに(S1)に戻り、
(S7)観測値を計測した前記計測体、又は、選択された他の物体のどちらかが「収束済み」の場合は、各物体の位置を修正し、
(S1)〜(S7)を繰り返して各物体の位置を推定する、ことを特徴とする物体の位置推定方法が提供される。
【0013】
前記(S2)において、前記第1指標値の大きさが、第1閾値より小さい場合に「収束済み」と判定し、それ以外を「収束中」と判定する。
【0014】
前記第1指標値の大きさは、誤差分散行列の対角項の和であり、前記第1閾値は、
初期状態の前記対角項の和より小さい値である。
【0015】
前記(S5)において、いずれの仮説も第2閾値を超える尤度を持たない場合、(S3)における観測値を誤りと判定する。
【0016】
前記計測体は、移動可能な移動体、又は、移動不能な固定体である。
【0017】
計測体以外の物体のうち少なくとも1つは、位置が既知の基準マーカであり、基準マーカは常に収束済みと判定する。
【0018】
また本発明によれば、3以上の物体と、各物体の位置を推定する推定演算装置とを備えた物体の位置推定装置であって、
前記物体のうち少なくとも1つは、他の物体の方位又は距離を計測可能なセンサを有する計測体であり、
前記推定演算装置は、前記計測体と通信可能であり、
各物体の位置の推定値とその誤差の大きさの第1指標値を算出する予測処理部と、
各物体の前記第1指標値から前記推定値が「収束済み」か「収束中」かを判定する収束判定部と、
前記計測体から他の物体の方位又は距離を計測してその観測値を取得する計測値取得部と、
前記他の物体としてあり得る1以上の仮説を作成する仮説作成部と、
仮説ごとに、予測処理部で算出した位置の推定値と前記第1指標値から、観測値の予測値とその誤差の大きさの第2指標値を算出し、得られた予測値と前記観測値を比較して、最も尤度が高い他の物体を選択する仮説選択部と、
観測値を計測した前記計測体、及び、選択された他の物体の両方が収束判定部において「収束中」と判定した場合は、位置を修正せずに予測処理部に戻す修正判定部と、
観測値を計測した前記計測体、又は、選択された他の物体のどちらかが「収束済み」の場合は、各物体の位置を修正する修正処理部と、を有し、
前記推定演算装置により、各物体の位置を推定する、ことを特徴とする物体の位置推定装置が提供される。
【0019】
前記センサは、他の物体の方位を計測するカメラ、又は、レーザにより他の物体の距離を計測するレーザセンサである。
【発明の効果】
【0020】
上記本発明の方法と装置は、(S2)と(S6)を省略したものが、一般的な多重仮説トラッキングの応用である。従って計測範囲内に2以上の物体があり、物体までの距離が離れている場合において、画像や距離データから物体を識別できない場合でも、予測値と観測値の比較により物体を識別できる。
【0021】
また、観測側と被観測側の両方が収束していない場合は、予測値の精度が低く、(S5)の選択が間違っている可能性が高い。本発明では、(S6)において、観測値を計測した計測体、及び、選択された他の物体の両方が「収束中」の場合、位置を修正せずに(S1)に戻り、(S7)において識別ミスの少ない条件でのみ選択された他の物体の位置を修正処理するため、結果的に推定精度が高くなる。
【発明を実施するための形態】
【0023】
本発明の好ましい実施形態を図面に基づいて説明する。なお、各図において共通する部分には同一の符号を付し、重複した説明を省略する。
【0024】
図1は、本発明の使用例を示す図である。
本発明の位置推定装置は、3以上の物体10と、各物体10の位置を推定する推定演算装置20とを備える。
【0025】
3以上の物体10のうち少なくとも1つは、他の物体10の方位又は距離を計測可能なセンサ12を有する計測体14である。計測体14は、移動可能な移動体15又は移動不能な固定体16である。また、計測体14以外の物体10のうち少なくとも1つは、位置が既知の基準マーカ17(
図5参照)であるのがよい。
【0026】
基準マーカ17は、物体10の位置を基準座標上で求めるために用いる。また、計測体14が基準マーカ17となってもよい。さらに基準マーカ17でも計測体14ではないが位置を推定したい物体10(位置が未知の物体10)があってもよい。
基準マーカ17を含む3以上の物体10の位置がわかれば、3次元空間での位置を求めることができる。また、この効果に加えて、基準マーカ17は始めから「収束済み」(後述する)と判定できる効果がある。
【0027】
すなわち、本発明において、「3以上の物体10」とは、以下の3通りの組み合わせを意味する。
(1)1以上の計測体14(移動体15又は固定体16)と2以上の基準マーカ17。この組み合わせに、単一の固定体16と2つの基準マーカ17の組み合わせを含む。単一の固定体16から、2つの基準マーカ17を識別する必要があるからである。
(2)2以上の計測体14と1以上の基準マーカ17。この組み合わせに、2つの固定体16と単一の基準マーカ17の組み合わせを含む。一方の固定体16から、基準マーカ17と他方の固定体16を識別する必要があるからである。
(3)3以上の計測体14。この組み合わせに、3つの固定体16の組み合わせを含む。固定体16の1つから、2つの固定体16を識別する必要があるからである。
なお「3以上の物体10」以外に、上述したように、基準マーカ17でも計測体14ではない物体10があってもよい。
【0028】
図1の例では、エリア内に移動体15(例えば移動ロボット)や固定体16(例えば固定デバイス)を配置し、移動体15と固定体16の位置を求める。得られた物体10の位置は、移動体15の制御やエリアの地図作成、監視などに用いる。
【0029】
図2は、推定演算装置20と計測体14の模式的構成図である。
推定演算装置20は、例えばコンピュータ(PC)であり、運動推定部20aと記憶装置20bを有し、計測体14と例えば無線LANで通信可能に構成されている。
計測体14(移動体15又は固定体16)は、計測体上PC14a、コントローラ14b及びセンサ12を有し、推定演算装置20と好ましくはリアルタイムに無線LANで通信可能に構成されている。計測体上PC14aは、推定演算装置20と無線LANで通信し、コントローラ14b及びセンサ12を制御する。コントローラ14bは、計測体14(例えば移動体15の操舵等)を制御する。
【0030】
図3は、推定演算装置20の具体的構成図である。
この図に示すように、推定演算装置20は、予測処理部21a、収束判定部21b、計測値取得部21c、仮説作成部21d、仮説選択部21e、修正判定部21f、及び修正処理部21gを有し、各物体10の位置X
i(後述する)を推定する。
なお、推定演算装置20は、予測処理部21a、収束判定部21b、計測値取得部21c、仮説作成部21d、仮説選択部21e、修正判定部21f、及び修正処理部21gの処理の順番に制約はない。
【0031】
予測処理部21aは、各物体10の位置X
iの推定値とその誤差の大きさの第1指標値CX
ii(後述する)を算出する。
収束判定部21bは、各物体10の第1指標値CX
iiから推定値が「収束済み」か「収束中」かを判定する。
計測値取得部21cは、計測体14から他の物体10の方位又は距離を計測してその観測値Y
ij(後述する)を取得する。
仮説作成部21dは、他の物体10としてあり得る1以上の仮説を作成する。
仮説選択部21eは、仮説ごとに、予測処理部21aで算出した位置X
iの推定値と第1指標値CX
iiから、観測値Y
ijの予測値Y
ij*とその誤差の大きさの第2指標値V
ij(後述する)を算出し、得られた予測値Y
ij*と観測値Y
ijを比較して、最も尤度(後述する)が高い他の物体10を選択する。
修正判定部21fは、観測値Y
ijを計測した計測体14、及び、選択された他の物体10の両方が収束判定部21bにおいて「収束中」と判定した場合は、位置を修正せずに予測処理部21aに戻す。
修正処理部21gは、観測値Y
ijを計測した計測体14、又は、選択された他の物体10のどちらかが「収束済み」の場合は、各物体10の位置X
iを修正する。
【0032】
図4は、計測体14による計測の模式図である。
計測体14(移動体15又は固定体16)のセンサ12は、他の物体10の方位を計測するカメラ12a、又は、レーザにより他の物体10の距離を計測するレーザセンサ12bである。カメラ12aとレーザセンサ12bの両方を計測体14に搭載することが好ましいが、一方のみでもよい。
また、センサ12は、カメラ12a、又は、レーザセンサ12bの他にジャイロ(図示せず)を有してもよい。
【0033】
カメラ12aにより、周囲の物体10のある方位を方位角と仰角として計測することができる。なお方位角と仰角は一般的な極座標表現と同じ定義である。物体10には、カメラ12aで検出するためのマーカ10aを設置するのが好ましい。マーカ10aには、例えば特許文献2に開示したものを用いる。
レーザセンサ12bにより、周囲の物体10までの距離を計測することができる。また、レーザセンサ12bにより、距離だけでなく他の物体10の方位も計測できる。
【0034】
図5は、本発明の実施形態を示す図である。
この例では、3以上の物体10は、単一のターゲット11、4台の移動体15、2つの基準マーカ17である。ターゲット11は、この例では移動体15である。従ってこの例は、上述した(1)の組み合わせに該当する。
【0035】
図5において、2つの基準マーカ17は位置が既知であり、この例では一方がx−y座標系の原点Oに位置し、他方がx軸上のx=Lの位置に位置する。
【0036】
以下、この例において、各物体10の物体番号を符号i(i=0〜6)で表す。すなわち、この例で、i=0はターゲット11、i=1〜4は4台の移動体15、i=5は原点の基準マーカ17、i=6はx軸上の基準マーカ17である。
【0037】
図6は、本発明の位置推定方法の全体フロー図である。この図において、
本発明の位置推定方法は、S1〜S7の各ステップ(工程)からなる。
【0038】
ステップS1では、各物体iの位置X
iの推定値とその誤差の大きさの第1指標値CX
iiを算出する。
[数1]の(1a)〜(1c)のように、各物体iの位置X
iとその誤差の大きさの第1指標値CX
iiを定義する。
【0040】
通常、各物体iがエリア内に配置された時点で大まかな配置は分かっている。そのため、位置X
iの初期値x
0〜rz
0は、例えば位置誤差2m,姿勢誤差2度程度の精度で設定する。
【0041】
また、CX
xx〜CX
rzrzは、設定した位置X
iの初期値誤差の大きさとして想定される値である。すなわち、位置X
iの位置誤差2m、姿勢誤差2度程度の標準偏差が想定される場合、CX
xx〜CX
rzrzは、以下のようになる。
CX
xx〜CX
zz=2
2[m
2]・・・(1d)
CX
rxrx〜CX
rzrz=(2×π/180)
2[rad
2]・・・(1e)
【0042】
また、基準マーカ17(i=5、6)は、固定マーカであるため、以下の値とする。
X
5の初期値:(0 0 0 0 0 0)
T・・・(1f)
X
6の初期値:(L 0 0 0 0 0)
T・・・(1g)
L:基準マーカ間の距離
CX
55、CX
66:全て0・・・(1h)
【0043】
上述したステップS1の処理は、以下の3通りに該当する。
(S1−1)推定値と誤差分散の初期値を設定する。この場合は、最初の1回だけである。
(S1−2)前回修正した推定値と誤差分散をメモリから読み出す。この場合は、そのままデータを保持し続ける。
(S1−3)センサ12の観測時刻に合うように、推定値と誤差分散データを時間遷移させる。この場合は、推定値と誤差分散データを予測する。
これらの処理(S1−1〜3)を併せて、「各物体iの位置X
iの推定値とその誤差の大きさの第1指標値CX
iiを算出する。」ことになる。
【0044】
(S1−3)の場合、「移動する物体の場合、位置X
iと誤差の大きさの第1指標値CX
iiは、時間の経過と共に変化する。このような時間遷移を考慮して、X
iとCX
iiを変化させても良い。」
【0045】
ステップS2では、各物体iの誤差の大きさの第1指標値CX
iiから各物体iの位置X
iの推定値が「収束済み」か「収束中」かを判定する。
「収束済み」とは、位置X
iの推定値が、所定の精度内にあることを意味する。また「収束中」とは、位置X
iの推定値が、所定の精度外にあることを意味する。
【0046】
例えばステップS2において、第1指標値CX
iiの大きさが、第1閾値より小さい場合に「収束済み」と判定し、それ以外を「収束中」と判定する。この場合、第1指標値CX
iiの大きさは、例えば誤差分散行列の対角項の和であり、第1閾値は、初期状態の誤差分散行列の対角項の和より小さい値であるのがよい。
【0047】
各物体iが観測した又は観測された回数をカウントして、この回数が予め設定した回数以上になれば「収束済み」としてもよい。
また、一度「収束済み」となった物体iは、以降の処理では常に「収束済み」としても良い。なお、2つの基準マーカ17は、計測開始直後から「収束済み」とする。
【0048】
ステップS3では、計測体14から他の物体j(物体i以外の物体)の方位又は距離を計測してその観測値Y
ijを取得する。すなわち、物体i(計測体14)上のセンサ12(カメラ12a又はレーザセンサ12b)で他の物体jの観測値Y
ijを得る。
【0049】
ステップS4では、他の物体j(物体i以外の物体)としてあり得る1以上の仮説を作成する。すなわち、計測体14の観測対象としてあり得るすべての他の物体jを仮説として仮定する。すなわち「仮説」とは、あり得るすべての他の物体jを意味する。
例えば、ステップS3にて、第3の移動体15(i=3)が観測した場合、第3の移動体15(i=3)以外の全て(i=3以外の0〜6)を観測対象の候補とする。また、「センサ12が誤検出した」ことを仮定した仮説も作成する。
【0050】
ステップS5では、上記仮説ごとに、ステップS1で算出した位置X
iの推定値と第1指標値CX
iiから、観測値Y
ijの予測値Y
ij*とその誤差の大きさの第2指標値V
ijを算出し、得られた予測値Y
ij*と実際の観測値Y
ijを比較して、最も尤度が高い他の物体j(物体i以外の物体)を選択する。ここで、「最も尤度が高い」とは「最も可能性が高い」ことを意味する。
【0051】
すなわち、仮説ごとに、位置X
iの推定値と、位置X
iの第1指標値CX
iiから、以下の予測値Y
ij*と予測値Y
ij*の誤差の大きさの第2指標値V
ijを算出する。
【0052】
予測値Y
ij*(=g(X
i*,X
j*)は、物体iから他の物体jを見た時の距離、方位角、仰角の予測値であり、[数2]の(2a)〜(2b)で求めることができる。ここで、X
i*、X
j*は、観測時点での位置X
iを予測したものであり、x
i*、x
j*などはX
i*、X
j*の成分である。
【0054】
C
nbiは、全体座標系から物体iのローカル座標系に変換する行列であり、[数3]の(3a)により、物体iの位置X
iの姿勢成分から算出する。ここで、cos(x)=c(x)、sin(x)=s(x)、tan(x)=t(x)と略記する。
さらに予測値Y
ij*の誤差の大きさの第2指標値V
ijは、[数3]の(3b)(3c)により求める。ここで、Rは、距離、方位角、仰角の計測誤差として想定される分散値である。
【0056】
得られた予測値Y
ij*と実際の観測値Y
ijを比較して、仮説の尤度を算出し、最も尤度が高いものを選出する。尤度は、例えば[数4]の(4)より求める。ここで、err_yは、予測した観測値Y
ij*と、実際の観測値Y
ijとの差(Y
ij−Y
ij*)である。
【0058】
ステップS6では、観測値Y
ijを計測した計測体14、及び、選択された他の物体j(物体i以外の物体)の両方がステップS2において「収束中」の場合は、ステップS6−2で位置X
iを修正せずにステップS1に戻る。
すなわち、選出した仮説が、観測側、被観測側共に「収束中」だった場合は、位置X
iを修正せずにステップS1に戻る。
また、選出した仮説が「誤検出仮説」の場合もステップS1に戻る。すなわち、ステップS5において、いずれの仮説も第2閾値を超える尤度を持たない場合、ステップS3における観測値Y
ijを誤りと判定して、位置X
iを修正せずにステップS1に戻る。第2閾値は、誤検出の場合の尤度を参考に設定するのがよい。
【0059】
ステップS7では、観測値Y
ijを計測した計測体14(物体i)、又は、選択された他の物体jのどちらかが「収束済み」の場合は、選択された各物体iの位置X
iを修正する。
すなわち、ステップS6で選出した仮説に基づいて、各物体iの位置X
iの推定値を修正する。
【0060】
位置X
iの修正式は、[数5]の(5a)で示すことができる。また、位置X
iの分散共分散の更新式は、[数5]の(5b)で示すことができる。ここで、Cは観測値Y
ijが得られたときのカルマンゲイン、B
ijは観測値Y
ijに対応する観測行列である。
【0062】
(5a)(5b)の各項は具体的には、[数6]の(6a)〜(6d)で示すことができる。
なお、(6c)の各成分Bnの定義は式(3c)、(6d)のV
ijは式(3b)による。
【0064】
ステップS1〜S7を繰り返すことで、観測対象を識別しながら、各物体iの位置X
iを推定する。
以上のステップS1〜S7を繰り返すことで、基準座標上での各移動体15やターゲット11の位置X
iを同定できる。
【0065】
図7は、本発明による推定の収束過程を示す模式図である。この図において、(A)は初期状態、(B)は移動体15−1、15−2間での計測、(C)は移動体15−2、15−3間での計測、(D)は移動体15−3による基準マーカ17の計測を示している。
また、この図において、移動体15−1、15−2、15−3は、真の位置にあり、それぞれの推定位置を18−1、18−2、18−3とする。各推定位置18−1、18−2、18−3は、推定誤差の期待値(誤差範囲)を有している。図中の破線の円は、誤差範囲の大きさを模式的に示している。
【0066】
以下、上述したステップS2、S6によって推定が収束していく過程を説明する。
【0067】
(1)ステップS2に記載したように、計測開始直後は基準マーカ17のみが「収束済み」である。
【0068】
(2)
図7(B)に示すように、基準マーカ17から離れた移動体15−1、15−2間で観測した場合は、移動体15−1、15−2の両方が「収束中」(誤差範囲が大きい状態)であるため、ステップS6、S6−2にて、修正処理が省略される。
図7(C)の場合も同様である。
従って、
図7(D)に示すように、計測開始直後は、基準マーカ17に近い移動体15−3が基準マーカ17を観測した場合にのみ、どちらかが「収束済み」(誤差範囲が小さい状態)であり、ステップS6を通過してステップS7で修正処理される。
【0069】
(3)(2)の結果、移動体15−3の位置X
iが収束していき、ステップS2で「収束済み」と判定される。
【0070】
(4)
図7(C)に示すように、移動体15−3が移動体15−2を観測した場合、あるいは、移動体15−2が移動体15−3を観測した場合も修正処理される。これにより、移動体15−2の位置X
iが収束して行き、ステップS2で「収束済み」と判定される。
(5)
図7(B)に示すように、移動体15−2が移動体15−1を観測した場合、あるいは、移動体15−1が移動体15−2を観測した場合も修正処理される。これにより、移動体15−1の位置X
iが収束して行き、ステップS2で「収束済み」と判定される。
(6)移動体15−1がターゲット11(図示せず、
図5参照)を観測した場合も、修正処理される。これにより、ターゲット11の位置X
iが収束して、全ての移動体15、ターゲット11の位置X
iが推定される。
【0071】
上述した実施形態では、観測側、被観測側ともに「収束中」の場合は修正処理をしないとしている。しかし、本発明はこれに限定されず、通常よりも少ない量で修正しても良い。例えば、カルマンゲインに1未満の係数を掛けて修正処理してもよい。
【0072】
上述した本発明の方法と装置は、(S2)と(S6)を省略したものが、一般的な多重仮説トラッキングの応用である。従って計測範囲内に2以上の物体iがあり、物体iまでの距離が離れている場合において、画像や距離データから物体iを識別できない場合でも、予測値と観測値Y
ijの比較により物体iを識別できる。
【0073】
また、観測側と被観測側の両方が収束していない場合は、予測値の精度が低く、(S5)の選択が間違っている可能性が高い。本発明では、(S6)において、観測値Y
ijを計測した計測体14、及び、選択された他の物体j(物体i以外の物体)の両方が「収束中」の場合、位置X
iを修正せずに(S1)に戻り、(S7)において識別ミスの少ない条件でのみ選択された他の物体jの位置X
iを修正処理するため、結果的に推定精度が高くなる。
【0074】
2以上の物体iの位置X
i(例えば位置)を同定する際、位置X
iが既知の基準マーカ17から離れた物体iで観測すると、仮に正しく識別できても推定誤差が大きい状態で収束することがある。そのため、この誤差の影響で他の物体j(物体i以外の物体)間での観測を識別できなくなる可能性がある。
これに対し、本発明では、基準マーカ17から近い順に推定値が収束していくため、推定値が正しく収束し、識別ミスを起こしにくい。
【0075】
本発明は上述した実施形態に限定されず、本発明の要旨を逸脱しない範囲で種々変更を加え得ることは勿論である。