JP2015031656A - 物体の位置推定方法と装置 - Google Patents
物体の位置推定方法と装置 Download PDFInfo
- Publication number
- JP2015031656A JP2015031656A JP2013163261A JP2013163261A JP2015031656A JP 2015031656 A JP2015031656 A JP 2015031656A JP 2013163261 A JP2013163261 A JP 2013163261A JP 2013163261 A JP2013163261 A JP 2013163261A JP 2015031656 A JP2015031656 A JP 2015031656A
- Authority
- JP
- Japan
- Prior art keywords
- value
- measurement
- objects
- convergence
- error
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
Images
Landscapes
- Optical Radar Systems And Details Thereof (AREA)
Abstract
Description
なお、複数の移動体は、例えば、物品を搬送する移動ロボットであり、作業エリア内で自在に移動可能である。
特許文献2は、光学マーカを用いて画像中の物体を抽出するものである。
特許文献3、4は、レーダーで何らかの目標を観測した値(観測ベクトル)と、追尾中の複数の目標位置を予測した値(予測ベクトル)とを比較して、差の小さいものを選出することで、観測ベクトルが、どの追尾目標のものかを判別するものである。
しかし多重仮説トラッキングは、観測値と予測値をつき合わせることで対象を識別するため、予測値に誤りがあると正しく識別できない問題点がある。特に観測値が一度も得られていない計測開始直後などに精度の高い予測値が得られないため識別ミスが起きやすい問題点があった。
前記物体のうち少なくとも1つは、他の物体の方位又は距離を計測可能なセンサを有する計測体であり、
前記推定演算装置は、前記計測体と通信可能であり、該推定演算装置により、
(S1)各物体の位置の推定値とその誤差の大きさの第1指標値を算出し、
(S2)各物体の前記第1指標値から前記推定値が「収束済み」か「収束中」かを判定し、
(S3)前記計測体から他の物体の方位又は距離を計測してその観測値を取得し、
(S4)前記他の物体としてあり得る1以上の仮説を作成し、
(S5)仮説ごとに、(S1)で算出した位置の推定値と前記第1指標値から、観測値の予測値とその誤差の大きさの第2指標値を算出し、得られた予測値と前記観測値を比較して、最も尤度が高い他の物体を選択し、
(S6)観測値を計測した前記計測体、及び、選択された他の物体の両方が(S2)において「収束中」の場合は、位置を修正せずに(S1)に戻り、
(S7)観測値を計測した前記計測体、又は、選択された他の物体のどちらかが「収束済み」の場合は、各物体の位置を修正し、
(S1)〜(S7)を繰り返して各物体の位置を推定する、ことを特徴とする物体の位置推定方法が提供される。
前記物体のうち少なくとも1つは、他の物体の方位又は距離を計測可能なセンサを有する計測体であり、
前記推定演算装置は、前記計測体と通信可能であり、
各物体の位置の推定値とその誤差の大きさの第1指標値を算出する予測処理部と、
各物体の前記第1指標値から前記推定値が「収束済み」か「収束中」かを判定する収束判定部と、
前記計測体から他の物体の方位又は距離を計測してその観測値を取得する計測値取得部と、
前記他の物体としてあり得る1以上の仮説を作成する仮説作成部と、
仮説ごとに、予測処理部で算出した位置の推定値と前記第1指標値から、観測値の予測値とその誤差の大きさの第2指標値を算出し、得られた予測値と前記観測値を比較して、最も尤度が高い他の物体を選択する仮説選択部と、
観測値を計測した前記計測体、及び、選択された他の物体の両方が収束判定部において「収束中」と判定した場合は、位置を修正せずに予測処理部に戻す修正判定部と、
観測値を計測した前記計測体、又は、選択された他の物体のどちらかが「収束済み」の場合は、各物体の位置を修正する修正処理部と、を有し、
前記推定演算装置により、各物体の位置を推定する、ことを特徴とする物体の位置推定装置が提供される。
本発明の位置推定装置は、3以上の物体10と、各物体10の位置を推定する推定演算装置20とを備える。
基準マーカ17を含む3以上の物体10の位置がわかれば、3次元空間での位置を求めることができる。また、この効果に加えて、基準マーカ17は始めから「収束済み」(後述する)と判定できる効果がある。
(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があってもよい。
推定演算装置20は、例えばコンピュータ(PC)であり、運動推定部20aと記憶装置20bを有し、計測体14と例えば無線LANで通信可能に構成されている。
計測体14(移動体15又は固定体16)は、計測体上PC14a、コントローラ14b及びセンサ12を有し、推定演算装置20と好ましくはリアルタイムに無線LANで通信可能に構成されている。計測体上PC14aは、推定演算装置20と無線LANで通信し、コントローラ14b及びセンサ12を制御する。コントローラ14bは、計測体14(例えば移動体15の操舵等)を制御する。
この図に示すように、推定演算装置20は、予測処理部21a、収束判定部21b、計測値取得部21c、仮説作成部21d、仮説選択部21e、修正判定部21f、及び修正処理部21gを有し、各物体10の位置Xi(後述する)を推定する。
なお、推定演算装置20は、予測処理部21a、収束判定部21b、計測値取得部21c、仮説作成部21d、仮説選択部21e、修正判定部21f、及び修正処理部21gの処理の順番に制約はない。
収束判定部21bは、各物体10の第1指標値CXiiから推定値が「収束済み」か「収束中」かを判定する。
計測値取得部21cは、計測体14から他の物体10の方位又は距離を計測してその観測値Yij(後述する)を取得する。
仮説作成部21dは、他の物体10としてあり得る1以上の仮説を作成する。
仮説選択部21eは、仮説ごとに、予測処理部21aで算出した位置Xiの推定値と第1指標値CXiiから、観測値Yijの予測値Yij *とその誤差の大きさの第2指標値Vij(後述する)を算出し、得られた予測値Yij *と観測値Yijを比較して、最も尤度(後述する)が高い他の物体10を選択する。
修正判定部21fは、観測値Yijを計測した計測体14、及び、選択された他の物体10の両方が収束判定部21bにおいて「収束中」と判定した場合は、位置を修正せずに予測処理部21aに戻す。
修正処理部21gは、観測値Yijを計測した計測体14、又は、選択された他の物体10のどちらかが「収束済み」の場合は、各物体10の位置Xiを修正する。
計測体14(移動体15又は固定体16)のセンサ12は、他の物体10の方位を計測するカメラ12a、又は、レーザにより他の物体10の距離を計測するレーザセンサ12bである。カメラ12aとレーザセンサ12bの両方を計測体14に搭載することが好ましいが、一方のみでもよい。
また、センサ12は、カメラ12a、又は、レーザセンサ12bの他にジャイロ(図示せず)を有してもよい。
レーザセンサ12bにより、周囲の物体10までの距離を計測することができる。また、レーザセンサ12bにより、距離だけでなく他の物体10の方位も計測できる。
この例では、3以上の物体10は、単一のターゲット11、4台の移動体15、2つの基準マーカ17である。ターゲット11は、この例では移動体15である。従ってこの例は、上述した(1)の組み合わせに該当する。
本発明の位置推定方法は、S1〜S7の各ステップ(工程)からなる。
[数1]の(1a)〜(1c)のように、各物体iの位置Xiとその誤差の大きさの第1指標値CXiiを定義する。
CXxx〜CXzz=22[m2]・・・(1d)
CXrxrx〜CXrzrz=(2×π/180)2[rad2]・・・(1e)
X5の初期値:(0 0 0 0 0 0)T・・・(1f)
X6の初期値:(L 0 0 0 0 0)T・・・(1g)
L:基準マーカ間の距離
CX55、CX66:全て0・・・(1h)
(S1−1)推定値と誤差分散の初期値を設定する。この場合は、最初の1回だけである。
(S1−2)前回修正した推定値と誤差分散をメモリから読み出す。この場合は、そのままデータを保持し続ける。
(S1−3)センサ12の観測時刻に合うように、推定値と誤差分散データを時間遷移させる。この場合は、推定値と誤差分散データを予測する。
これらの処理(S1−1〜3)を併せて、「各物体iの位置Xiの推定値とその誤差の大きさの第1指標値CXiiを算出する。」ことになる。
「収束済み」とは、位置Xiの推定値が、所定の精度内にあることを意味する。また「収束中」とは、位置Xiの推定値が、所定の精度外にあることを意味する。
また、一度「収束済み」となった物体iは、以降の処理では常に「収束済み」としても良い。なお、2つの基準マーカ17は、計測開始直後から「収束済み」とする。
例えば、ステップS3にて、第3の移動体15(i=3)が観測した場合、第3の移動体15(i=3)以外の全て(i=3以外の0〜6)を観測対象の候補とする。また、「センサ12が誤検出した」ことを仮定した仮説も作成する。
さらに予測値Yij *の誤差の大きさの第2指標値Vijは、[数3]の(3b)(3c)により求める。ここで、Rは、距離、方位角、仰角の計測誤差として想定される分散値である。
ステップS6では、観測値Yijを計測した計測体14、及び、選択された他の物体j(物体i以外の物体)の両方がステップS2において「収束中」の場合は、ステップS6−2で位置Xiを修正せずにステップS1に戻る。
すなわち、選出した仮説が、観測側、被観測側共に「収束中」だった場合は、位置Xiを修正せずにステップS1に戻る。
また、選出した仮説が「誤検出仮説」の場合もステップS1に戻る。すなわち、ステップS5において、いずれの仮説も第2閾値を超える尤度を持たない場合、ステップS3における観測値Yijを誤りと判定して、位置Xiを修正せずにステップS1に戻る。第2閾値は、誤検出の場合の尤度を参考に設定するのがよい。
すなわち、ステップS6で選出した仮説に基づいて、各物体iの位置Xiの推定値を修正する。
なお、(6c)の各成分Bnの定義は式(3c)、(6d)のVijは式(3b)による。
以上のステップS1〜S7を繰り返すことで、基準座標上での各移動体15やターゲット11の位置Xiを同定できる。
また、この図において、移動体15−1、15−2、15−3は、真の位置にあり、それぞれの推定位置を18−1、18−2、18−3とする。各推定位置18−1、18−2、18−3は、推定誤差の期待値(誤差範囲)を有している。図中の破線の円は、誤差範囲の大きさを模式的に示している。
従って、図7(D)に示すように、計測開始直後は、基準マーカ17に近い移動体15−3が基準マーカ17を観測した場合にのみ、どちらかが「収束済み」(誤差範囲が小さい状態)であり、ステップS6を通過してステップS7で修正処理される。
(5)図7(B)に示すように、移動体15−2が移動体15−1を観測した場合、あるいは、移動体15−1が移動体15−2を観測した場合も修正処理される。これにより、移動体15−1の位置Xiが収束して行き、ステップS2で「収束済み」と判定される。
(6)移動体15−1がターゲット11(図示せず、図5参照)を観測した場合も、修正処理される。これにより、ターゲット11の位置Xiが収束して、全ての移動体15、ターゲット11の位置Xiが推定される。
これに対し、本発明では、基準マーカ17から近い順に推定値が収束していくため、推定値が正しく収束し、識別ミスを起こしにくい。
CXii 位置Xiの誤差の大きさの第1指標値、
CXxx〜CXrzrz 位置Xiの初期値誤差の想定値、
Yij 実際の観測値、
Yij * 得られた予測値、
Vij 予測値Yij *の誤差の大きさの第2指標値、
10 物体、10a マーカ、11 ターゲット、
12 センサ、12a カメラ、12b レーザセンサ、
14 計測体、14a 計測体上PC、14b コントローラ、
15、15−1、15−2、15−3 移動体(移動ロボット)、
16 固定体(固定デバイス)、17 基準マーカ、
18−1、18−2、18−3 推定位置、
20 推定演算装置(コンピュータ)、
20a 運動推定部、20b 記憶装置、
21a 予測処理部、21b 収束判定部、21c 計測値取得部、
21d 仮説作成部、21e 仮説選択部、21f 修正判定部、
21g 修正処理部
Claims (8)
- 3以上の物体と、各物体の位置を推定する推定演算装置とを用いた物体の位置推定方法であって、
前記物体のうち少なくとも1つは、他の物体の方位又は距離を計測可能なセンサを有する計測体であり、
前記推定演算装置は、前記計測体と通信可能であり、該推定演算装置により、
(S1)各物体の位置の推定値とその誤差の大きさの第1指標値を算出し、
(S2)各物体の前記第1指標値から前記推定値が「収束済み」か「収束中」かを判定し、
(S3)前記計測体から他の物体の方位又は距離を計測してその観測値を取得し、
(S4)前記他の物体としてあり得る1以上の仮説を作成し、
(S5)仮説ごとに、(S1)で算出した位置の推定値と前記第1指標値から、観測値の予測値とその誤差の大きさの第2指標値を算出し、得られた予測値と前記観測値を比較して、最も尤度が高い他の物体を選択し、
(S6)観測値を計測した前記計測体、及び、選択された他の物体の両方が(S2)において「収束中」の場合は、位置を修正せずに(S1)に戻り、
(S7)観測値を計測した前記計測体、又は、選択された他の物体のどちらかが「収束済み」の場合は、各物体の位置を修正し、
(S1)〜(S7)を繰り返して各物体の位置を推定する、ことを特徴とする物体の位置推定方法。 - 前記(S2)において、前記第1指標値の大きさが、第1閾値より小さい場合に「収束済み」と判定し、それ以外を「収束中」と判定する、ことを特徴とする請求項1に記載の物体の位置推定方法。
- 前記第1指標値の大きさは、誤差分散行列の対角項の和であり、前記第1閾値は、前記対角項の和より小さい値である、ことを特徴とする請求項2に記載の物体の位置推定方法。
- 前記(S5)において、いずれの仮説も第2閾値を超える尤度を持たない場合、(S3)における観測値を誤りと判定する、ことを特徴とする請求項1に記載の物体の位置推定方法。
- 前記計測体は、移動可能な移動体、又は、移動不能な固定体である、ことを特徴とする請求項1に記載の物体の位置推定方法。
- 計測体以外の物体のうち少なくとも1つは、位置が既知の基準マーカであり、基準マーカは常に収束済みと判定する、ことを特徴とする請求項1に記載の物体の位置推定方法。
- 3以上の物体と、各物体の位置を推定する推定演算装置とを備えた物体の位置推定装置であって、
前記物体のうち少なくとも1つは、他の物体の方位又は距離を計測可能なセンサを有する計測体であり、
前記推定演算装置は、前記計測体と通信可能であり、
各物体の位置の推定値とその誤差の大きさの第1指標値を算出する予測処理部と、
各物体の前記第1指標値から前記推定値が「収束済み」か「収束中」かを判定する収束判定部と、
前記計測体から他の物体の方位又は距離を計測してその観測値を取得する計測値取得部と、
前記他の物体としてあり得る1以上の仮説を作成する仮説作成部と、
仮説ごとに、予測処理部で算出した位置の推定値と前記第1指標値から、観測値の予測値とその誤差の大きさの第2指標値を算出し、得られた予測値と前記観測値を比較して、最も尤度が高い他の物体を選択する仮説選択部と、
観測値を計測した前記計測体、及び、選択された他の物体の両方が収束判定部において「収束中」と判定した場合は、位置を修正せずに予測処理部に戻す修正判定部と、
観測値を計測した前記計測体、又は、選択された他の物体のどちらかが「収束済み」の場合は、各物体の位置を修正する修正処理部と、を有し、
前記推定演算装置により、各物体の位置を推定する、ことを特徴とする物体の位置推定装置。 - 前記センサは、他の物体の方位を計測するカメラ、又は、レーザにより他の物体の距離を計測するレーザセンサである、ことを特徴とする請求項7に記載の物体の位置推定装置。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2013163261A JP6247860B2 (ja) | 2013-08-06 | 2013-08-06 | 物体の位置推定方法と装置 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2013163261A JP6247860B2 (ja) | 2013-08-06 | 2013-08-06 | 物体の位置推定方法と装置 |
Publications (2)
Publication Number | Publication Date |
---|---|
JP2015031656A true JP2015031656A (ja) | 2015-02-16 |
JP6247860B2 JP6247860B2 (ja) | 2017-12-13 |
Family
ID=52517077
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
JP2013163261A Active JP6247860B2 (ja) | 2013-08-06 | 2013-08-06 | 物体の位置推定方法と装置 |
Country Status (1)
Country | Link |
---|---|
JP (1) | JP6247860B2 (ja) |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JPWO2022102231A1 (ja) * | 2020-11-10 | 2022-05-19 |
Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US5798942A (en) * | 1994-04-05 | 1998-08-25 | Trw Inc. | N-best feasible hypotheses multitarget tracking system for space-based early warning systems |
JP2005227184A (ja) * | 2004-02-13 | 2005-08-25 | Mitsubishi Electric Corp | 多目標追尾装置 |
JP2007303841A (ja) * | 2006-05-08 | 2007-11-22 | Toyota Central Res & Dev Lab Inc | 車両位置推定装置 |
JP2008145295A (ja) * | 2006-12-11 | 2008-06-26 | Mitsubishi Electric Corp | センサシステム |
JP2009281865A (ja) * | 2008-05-22 | 2009-12-03 | Mitsubishi Electric Corp | 多目標追尾装置 |
JP2011065308A (ja) * | 2009-09-16 | 2011-03-31 | Hitachi Ltd | 自律移動システム及び自律移動装置 |
JP2013044602A (ja) * | 2011-08-23 | 2013-03-04 | Nec Corp | 目標運動推測システム及び方法 |
-
2013
- 2013-08-06 JP JP2013163261A patent/JP6247860B2/ja active Active
Patent Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US5798942A (en) * | 1994-04-05 | 1998-08-25 | Trw Inc. | N-best feasible hypotheses multitarget tracking system for space-based early warning systems |
JP2005227184A (ja) * | 2004-02-13 | 2005-08-25 | Mitsubishi Electric Corp | 多目標追尾装置 |
JP2007303841A (ja) * | 2006-05-08 | 2007-11-22 | Toyota Central Res & Dev Lab Inc | 車両位置推定装置 |
JP2008145295A (ja) * | 2006-12-11 | 2008-06-26 | Mitsubishi Electric Corp | センサシステム |
JP2009281865A (ja) * | 2008-05-22 | 2009-12-03 | Mitsubishi Electric Corp | 多目標追尾装置 |
JP2011065308A (ja) * | 2009-09-16 | 2011-03-31 | Hitachi Ltd | 自律移動システム及び自律移動装置 |
JP2013044602A (ja) * | 2011-08-23 | 2013-03-04 | Nec Corp | 目標運動推測システム及び方法 |
Cited By (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JPWO2022102231A1 (ja) * | 2020-11-10 | 2022-05-19 | ||
WO2022102231A1 (ja) * | 2020-11-10 | 2022-05-19 | 日立Astemo株式会社 | レーダ軸ずれ補正システム、および、レーダ軸ずれ補正方法 |
JP7464749B2 (ja) | 2020-11-10 | 2024-04-09 | 日立Astemo株式会社 | レーダ軸ずれ補正システム、および、レーダ軸ずれ補正方法 |
Also Published As
Publication number | Publication date |
---|---|
JP6247860B2 (ja) | 2017-12-13 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN107710094B (zh) | 自主车辆运行期间的在线校准检查 | |
EP3168705B1 (en) | Domestic robotic system | |
JP6442193B2 (ja) | 点群位置データ処理装置、点群位置データ処理システム、点群位置データ処理方法およびプログラム | |
KR102345886B1 (ko) | 알려진 이동 중에 이동하는 물체의 3차원 측정을 위한 방법 | |
US10627475B2 (en) | Pose estimation using radio frequency identification (RFID) tags | |
KR20160006898A (ko) | 레이저 거리 센서를 이용한 도로의 연석 추출 방법 및 도로의 연석 정보를 이용한 이동 로봇의 위치 측정 방법 | |
JP2015127664A (ja) | キャリブレーション装置、キャリブレーション方法およびキャリブレーションプログラム | |
JP6191484B2 (ja) | 移動ロボットの自己位置推定方法 | |
KR101468545B1 (ko) | 이동 로봇의 전역위치인식 및 지도 업데이트 장치 및 방법 | |
US20100265327A1 (en) | System for recording Surroundings | |
JP2013025401A (ja) | 自己位置推定装置、自己位置推定方法およびプログラム | |
CN111487960A (zh) | 一种基于定位能力估计的移动机器人路径规划方法 | |
US20160379365A1 (en) | Camera calibration device, camera calibration method, and camera calibration program | |
Kayhani et al. | Improved tag-based indoor localization of UAVs using extended Kalman filter | |
JP2017004228A (ja) | 軌跡推定方法、軌跡推定装置及び軌跡推定プログラム | |
Voges et al. | Interval-based visual-LiDAR sensor fusion | |
US11914062B2 (en) | Technique for calibrating a positioning system | |
CN109983299A (zh) | 工业机器人的测量系统和方法 | |
JP6974189B2 (ja) | 地図作成装置 | |
JP6247860B2 (ja) | 物体の位置推定方法と装置 | |
KR20170000282A (ko) | 센서를 이용한 로봇 위치 정확도 정보 제공장치 및 그 방법 | |
JP2018036151A (ja) | ランドマークを用いた測位方法 | |
JP5939395B2 (ja) | 移動体の相対姿勢計測方法と装置 | |
Ji et al. | An omni-vision based self-localization method for soccer robot | |
Araar et al. | Towards low-cost indoor localisation using a multi-camera system |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20160714 |
|
A977 | Report on retrieval |
Free format text: JAPANESE INTERMEDIATE CODE: A971007 Effective date: 20170406 |
|
A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20170601 |
|
A977 | Report on retrieval |
Free format text: JAPANESE INTERMEDIATE CODE: A971007 Effective date: 20170406 |
|
A521 | Request for written amendment filed |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20170728 |
|
TRDD | Decision of grant or rejection written | ||
A01 | Written decision to grant a patent or to grant a registration (utility model) |
Free format text: JAPANESE INTERMEDIATE CODE: A01 Effective date: 20171027 |
|
A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20171120 |
|
R150 | Certificate of patent or registration of utility model |
Ref document number: 6247860 Country of ref document: JP Free format text: JAPANESE INTERMEDIATE CODE: R150 |
|
R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |