JP7455004B2 - 位置推定方法及び位置推定システム - Google Patents
位置推定方法及び位置推定システム Download PDFInfo
- Publication number
- JP7455004B2 JP7455004B2 JP2020103232A JP2020103232A JP7455004B2 JP 7455004 B2 JP7455004 B2 JP 7455004B2 JP 2020103232 A JP2020103232 A JP 2020103232A JP 2020103232 A JP2020103232 A JP 2020103232A JP 7455004 B2 JP7455004 B2 JP 7455004B2
- Authority
- JP
- Japan
- Prior art keywords
- vehicle
- time difference
- time
- trajectory
- passing
- 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.)
- Active
Links
- 238000000034 method Methods 0.000 title claims description 179
- 238000004364 calculation method Methods 0.000 claims description 93
- 230000001186 cumulative effect Effects 0.000 claims description 5
- 238000012545 processing Methods 0.000 description 37
- 238000010586 diagram Methods 0.000 description 10
- 238000013519 translation Methods 0.000 description 5
- 230000006870 function Effects 0.000 description 4
- 230000001360 synchronised effect Effects 0.000 description 4
- 230000000694 effects Effects 0.000 description 3
- 238000012795 verification Methods 0.000 description 3
- 238000012937 correction Methods 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000004458 analytical method Methods 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 230000007423 decrease Effects 0.000 description 1
- 230000001934 delay Effects 0.000 description 1
- 238000007689 inspection Methods 0.000 description 1
- 230000010354 integration Effects 0.000 description 1
- 239000011159 matrix material Substances 0.000 description 1
- 238000005259 measurement Methods 0.000 description 1
- 230000000737 periodic effect Effects 0.000 description 1
- 230000017105 transposition Effects 0.000 description 1
Images
Landscapes
- Navigation (AREA)
- Position Fixing By Use Of Radio Waves (AREA)
Description
[位置推定システムの構成例]
図1は、第1実施形態における位置推定システム100の機能構成例を示すブロック図である。位置推定システム100は、車両20(図3参照)に搭載され、車両20の現在位置を推定するシステムである。
図2Aは、相対位置推定部103により推定された相対座標系における位置の軌跡1を示す図である。図2Bは、絶対位置推定部105により推定された絶対座標系における位置2の軌跡を示す図である。なお、図2A及び図2Bでは、所定時間内に推定された位置の軌跡1、2を簡略化して示す。また、図2A及び図2Bでは、軌跡照合部106により2つの軌跡1、2が照合される前の状態、すなわち初期状態を示す。
図3は、軌跡照合部106による軌跡照合処理により2つの軌跡が照合された状態を模式的に示す図である。
図4は、軌跡照合部106による照合開始判定処理の処理手順の一例を示すフローチャートである。なお、この処理手順は、記憶装置(図示省略)に記憶されているプログラムに基づいて実行される。また、図4では、車両20がカーブを曲がり始める前の位置を始点とし、車両20がそのカーブを所定量だけ曲がった位置を終点とする走行軌跡を用いて軌跡照合処理を実行する場合の例を示す。例えば、車両20がカーブを曲がるときの軌跡を用いて軌跡照合処理を実行することができる。
図5は、時刻差算出部109による時刻差算出処理における相対位置の点の選択処理の一例を示す図である。具体的には、図5には、絶対位置の点GTiと、点GTiから半径R1内に存在する相対位置の点VTi1乃至VTi5との関係を模式的に示す。なお、図5では、半径R1の円R2を点線で示す。また、相対位置の点VTi1乃至VTi5に関連付けられている通過時刻のうち、点VTi1に関連付けられている通過時刻が最も古い時刻であり、点VTi5に関連付けられている通過時刻が最も新しい時刻であるものとする。
図6は、時刻差算出部109による時刻差算出処理の処理手順の一例を示すフローチャートである。なお、この処理手順は、記憶装置(図示省略)に記憶されているプログラムに基づいて実行される。また、時刻差算出処理の対象となる2つの軌跡は、上述した軌跡照合処理によりマッチングされた後の軌跡である。
図7は、位置推定システム100による位置推定処理の処理手順の一例を示すフローチャートである。なお、この処理手順は、記憶装置(図示省略)に記憶されているプログラムに基づいて実行される。
y=y+vΔTsinθ
θ=θ+φΔT …式1
第1実施形態に係る位置推定方法は、車両情報に基づいて取得された車両20の通過位置(相対位置)とその通過位置の車両20の通過時刻とを関連付けて記憶する第1走行軌跡記憶部107(第1記憶部の一例)と、測位装置11によって測位された車両20の絶対位置とその絶対位置の測位時刻とを関連付けて記憶する第2走行軌跡記憶部108(第2記憶部の一例)とを備える車両20の位置を推定する位置推定方法である。この位置推定方法は、複数の通過位置からなる第1走行軌跡と、複数の絶対位置からなる第2走行軌跡とを比較して、第1走行軌跡における通過位置と第2走行軌跡における絶対位置との対応を特定し、その対応する通過位置の通過時刻と絶対位置の測位時刻との時刻差を算出する時刻差算出ステップ(ステップS602)と、絶対位置と車両情報と算出された時刻差とに基づいて車両20の位置を推定する位置推定ステップ(ステップS606)と、を含む。
第1実施形態では、絶対位置の対象点から所定範囲内に存在する相対位置の各点に関連付けられている通過時刻のうち最も古い時刻と最も新しい時刻との差(時刻幅)を用いて時刻差算出処理を行う例を示した。ただし、他の判定基準を用いて時刻差算出処理を行うようにしてもよい。そこで、第2実施形態では、絶対位置の対象点から所定範囲内に存在する相対位置の点の個数を用いて時刻差算出処理を行う例を示す。なお、第2実施形態は、第1実施形態の一部を変形した例であり、第1実施形態と共通する部分については、図示及びその説明の一部を省略する。
図8は、第2実施形態における時刻差算出部109による時刻差算出処理の処理手順の一例を示すフローチャートである。なお、図8に示す例は、図6の一部を変形したものであり、図6と共通する部分については、同一の符号を付してその説明の一部を省略する。具体的には、図8では、図6に示すステップS503、S504の代わりに、ステップS511の処理を行う点が異なる。
第2実施形態に係る位置推定方法は、時刻差算出ステップ(S501、S502、S505乃至S507、S511)は、ステップS502で選択された通過位置の数が閾値を基準として大きい場合には、その選択された通過位置を時刻差の算出に用いない。
第1実施形態では、図4に示す照合開始判定処理において、角速度センサ101により計測されたヨーレートの時間積分が閾値以上であるか否かに基づいて軌跡照合処理を実行するか否かを判定する例を示した。この例では、車両がカーブを曲がり始める前の位置から、そのカーブを所定角以上曲がった位置までの区間の軌跡を用いて軌跡照合処理を実行することができる。ただし、車両がカーブを曲がり始める前から、そのカーブを曲がり終わった後までの区間の軌跡を用いることにより軌跡照合処理の精度を高めることができると想定される。そこで、第3実施形態では、軌跡照合処理の精度を高めるため、さらに特徴的な軌跡を抽出する例を示す。なお、第3実施形態は、第1実施形態の一部を変形した例であり、第1実施形態と共通する部分については、図示及びその説明の一部を省略する。
図9は、第3実施形態における軌跡照合部106による照合開始判定処理の処理手順の一例を示すフローチャートである。なお、図9に示す例は、図4の一部を変形したものであり、図4と共通する部分については、同一の符号を付してその説明の一部を省略する。具体的には、図9では、図4に示すステップS205の処理後にステップS211を追加した点が異なる。なお、図9では、車両20がカーブを曲がり始める前の位置を始点とし、車両20がカーブを曲がり終わった後の位置を終点とする走行軌跡を用いて軌跡照合処理を実行する場合の例を示す。
第3実施形態に係る位置推定方法は、軌跡照合ステップ(ステップS206)では、車両20がカーブを曲がり始める前の位置を始点とし、車両20がカーブを曲がり終わった後の位置を終点とする第1走行軌跡及び第2走行軌跡を用いてマッチングを実行する。
11 測位装置
20 車両
100 位置推定システム
101 角速度センサ
102 車速センサ
103 相対位置推定部
104 GNSSアンテナ
105 絶対位置推定部
106 軌跡照合部
107 第1走行軌跡記憶部
108 第2走行軌跡記憶部
109 時刻差算出部
110 位置推定部
Claims (12)
- 車両情報に基づいて取得された車両の通過位置と当該通過位置の前記車両の通過時刻とを関連付けて記憶する第1記憶部と、測位装置によって測位された前記車両の絶対位置と当該絶対位置の測位時刻とを関連付けて記憶する第2記憶部とを備える車両の位置を推定する位置推定方法であって、
複数の前記通過位置からなる第1走行軌跡と、複数の前記絶対位置からなる第2走行軌跡とを比較して、前記第1走行軌跡における前記通過位置と前記第2走行軌跡における前記絶対位置との対応を特定し、対応する前記通過位置の通過時刻と前記絶対位置の測位時刻との時刻差を算出する時刻差算出ステップと、
前記絶対位置と前記車両情報と前記算出された時刻差とに基づいて前記車両の位置を推定する位置推定ステップと、を含み、
前記位置推定ステップは、前記絶対位置の測位時刻を前記算出された時刻差で補正し、当該測位時刻が補正された絶対位置と、直前に推定された前記車両の位置と、前記車両情報とをカルマンフィルタを用いて合成して前記車両の位置を推定する、
位置推定方法。 - 請求項1に記載の位置推定方法であって、
前記時刻差算出ステップは、前記第2走行軌跡における前記絶対位置の測位時刻と、前記第1走行軌跡における当該絶対位置に対応する通過位置の通過時刻との差分値に基づいて前記時刻差を算出する、
位置推定方法。 - 請求項2に記載の位置推定方法であって、
前記時刻差算出ステップは、前記第2走行軌跡における各絶対位置について、当該絶対位置から所定範囲に存在する前記第1走行軌跡における通過位置を、当該絶対位置に対応する通過位置として選択し、当該絶対位置の測位時刻と当該選択された通過位置の通過時刻との差分値を算出し、当該算出された各差分値に基づいて前記時刻差を算出する、
位置推定方法。 - 請求項3に記載の位置推定方法であって、
前記時刻差算出ステップは、前記選択された通過位置の通過時刻のうち最も古い時刻と最も新しい時刻との差分値が閾値を基準として大きい場合には、当該選択された通過位置を前記時刻差の算出に用いない、
位置推定方法。 - 請求項3に記載の位置推定方法であって、
前記時刻差算出ステップは、前記選択された通過位置の数が閾値を基準として大きい場合には、当該選択された通過位置を前記時刻差の算出に用いない、
位置推定方法。 - 請求項1乃至5の何れかに記載の位置推定方法であって、
前記第2走行軌跡における各絶対位置について、前記第2走行軌跡における前記絶対位置と前記第1走行軌跡における当該絶対位置に対応する通過位置との対応をとり、当該絶対位置及び当該通過位置間の距離を最小化する処理を行うことにより前記第1走行軌跡と前記第2走行軌跡とをマッチングさせる軌跡照合ステップをさらに含み、
前記時刻差算出ステップは、前記マッチング後の前記第1走行軌跡と前記第2走行軌跡とを比較して前記時刻差を算出する、
位置推定方法。 - 請求項6に記載の位置推定方法であって、
前記軌跡照合ステップは、前記車両がカーブを曲がり始める前の位置を始点とし、前記車両が前記カーブを所定量だけ曲がった位置を終点とする前記第1走行軌跡及び前記第2走行軌跡を用いて前記マッチングを実行する、
位置推定方法。 - 請求項6に記載の位置推定方法であって、
前記軌跡照合ステップは、前記車両がカーブを曲がり始める前の位置を始点とし、前記車両が前記カーブを曲がり終わった後の位置を終点とする前記第1走行軌跡及び前記第2走行軌跡を用いて前記マッチングを実行する、
位置推定方法。 - 請求項7に記載の位置推定方法であって、
前記軌跡照合ステップは、前記車両のヨーレートが略ゼロとなる位置を前記車両が前記カーブを曲がり始める前の位置とし、当該ヨーレートが略ゼロとなった後に前記車両のヨー角変化の累積が所定値を基準として大きくなった位置を前記車両が前記カーブを所定量だけ曲がった位置とする、
位置推定方法。 - 請求項8に記載の位置推定方法であって、
前記軌跡照合ステップは、前記車両のヨーレートが略ゼロとなる位置を前記車両が前記カーブを曲がり始める前の位置とし、当該ヨーレートが略ゼロとなった後に前記車両のヨー角変化の累積が所定値を基準として大きくなり、かつ、前記車両のヨーレートが再度略ゼロとなる位置を前記車両が前記カーブを曲がり終わった後の位置とする、
位置推定方法。 - 請求項6乃至10の何れかに記載の位置推定方法であって、
前記時刻差算出ステップは、前記マッチングが行われる毎に前記時刻差を算出し、第1の時刻差と、当該第1の時刻差の算出後に新たに算出された第2の時刻差とを比較し、前記第1の時刻差と前記第2の時刻差との差分値が所定範囲内にある場合には、前記第1の時刻差及び前記第2の時刻差の何れか一方または双方を前記車両の位置を推定する値として用いる決定をし、一方、前記第1の時刻差と前記第2の時刻差との差分値が所定範囲内にない場合には、前記第2の時刻差を前記車両の位置を推定する値として用いる決定をする、
位置推定方法。 - 車両の位置を推定するコントローラを備える位置推定システムであって、
前記コントローラは、
車両情報に基づいて取得された車両の通過位置と当該通過位置の前記車両の通過時刻とを関連付けて記憶する第1記憶部と、
測位装置によって測位された前記車両の絶対位置と当該絶対位置の測位時刻とを関連付けて記憶する第2記憶部と、
複数の前記通過位置からなる第1走行軌跡と、複数の前記絶対位置からなる第2走行軌跡とを比較して、前記第1走行軌跡における前記通過位置と前記第2走行軌跡における前記絶対位置との対応を特定し、対応する前記通過位置の通過時刻と前記絶対位置の測位時刻との時刻差を算出する時刻差算出部と、
前記絶対位置と前記車両情報と前記算出された時刻差とに基づいて前記車両の位置を推定する位置推定部と、を備え、
前記位置推定部は、前記絶対位置の測位時刻を前記算出された時刻差で補正し、当該測位時刻が補正された絶対位置と、直前に推定された前記車両の位置と、前記車両情報とをカルマンフィルタを用いて合成して前記車両の位置を推定する、
位置推定システム。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2020103232A JP7455004B2 (ja) | 2020-06-15 | 2020-06-15 | 位置推定方法及び位置推定システム |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2020103232A JP7455004B2 (ja) | 2020-06-15 | 2020-06-15 | 位置推定方法及び位置推定システム |
Publications (2)
Publication Number | Publication Date |
---|---|
JP2021196278A JP2021196278A (ja) | 2021-12-27 |
JP7455004B2 true JP7455004B2 (ja) | 2024-03-25 |
Family
ID=79197951
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
JP2020103232A Active JP7455004B2 (ja) | 2020-06-15 | 2020-06-15 | 位置推定方法及び位置推定システム |
Country Status (1)
Country | Link |
---|---|
JP (1) | JP7455004B2 (ja) |
Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2002350157A (ja) | 2001-05-30 | 2002-12-04 | Honda Motor Co Ltd | 位置補正装置 |
JP2009222438A (ja) | 2008-03-13 | 2009-10-01 | Toyota Motor Corp | 移動体用測位装置 |
JP2009229293A (ja) | 2008-03-24 | 2009-10-08 | Seiko Epson Corp | 測位方法、プログラム及び測位装置 |
JP2012007939A (ja) | 2010-06-23 | 2012-01-12 | Aisin Aw Co Ltd | 軌跡情報生成装置、方法およびプログラム |
JP2013061320A (ja) | 2011-08-24 | 2013-04-04 | Denso Corp | 走行軌跡記憶装置 |
-
2020
- 2020-06-15 JP JP2020103232A patent/JP7455004B2/ja active Active
Patent Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2002350157A (ja) | 2001-05-30 | 2002-12-04 | Honda Motor Co Ltd | 位置補正装置 |
JP2009222438A (ja) | 2008-03-13 | 2009-10-01 | Toyota Motor Corp | 移動体用測位装置 |
JP2009229293A (ja) | 2008-03-24 | 2009-10-08 | Seiko Epson Corp | 測位方法、プログラム及び測位装置 |
JP2012007939A (ja) | 2010-06-23 | 2012-01-12 | Aisin Aw Co Ltd | 軌跡情報生成装置、方法およびプログラム |
JP2013061320A (ja) | 2011-08-24 | 2013-04-04 | Denso Corp | 走行軌跡記憶装置 |
Also Published As
Publication number | Publication date |
---|---|
JP2021196278A (ja) | 2021-12-27 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
JP4453728B2 (ja) | 位置補正装置 | |
US9952597B2 (en) | Apparatus for correcting vehicle location | |
US10145690B2 (en) | Map information generation system, method, and program | |
JP5419665B2 (ja) | 位置標定装置、位置標定方法、位置標定プログラム、速度ベクトル算出装置、速度ベクトル算出方法および速度ベクトル算出プログラム | |
CN110914711B (zh) | 定位装置 | |
US9864067B2 (en) | Method for determining a current position of a motor vehicle in a geodetic coordinate system and motor vehicle | |
JP2007139601A (ja) | 移動体用位置推定装置 | |
JP6464978B2 (ja) | 位置推定装置 | |
CN111751857A (zh) | 一种车辆位姿的估算方法、装置、存储介质及*** | |
JP2015094690A (ja) | 車両用走行軌跡算出装置 | |
JP6248559B2 (ja) | 車両用走行軌跡算出装置 | |
JP6539129B2 (ja) | 自車位置推定装置、及びそれを用いた操舵制御装置、並びに自車位置推定方法 | |
JP7455004B2 (ja) | 位置推定方法及び位置推定システム | |
EP3971525B1 (en) | Self-position correction method and self-position correction device | |
JP6531768B2 (ja) | センサ誤差補正装置及び方法 | |
JP6820762B2 (ja) | 位置推定装置 | |
Wang et al. | A comparison of loosely-coupled mode and tightly-coupled mode for INS/VMS | |
WO2017168588A1 (ja) | 測定装置、測定方法、及び、プログラム | |
JP2017181195A (ja) | 測定装置、測定方法、及び、プログラム | |
EP1749183A1 (en) | Alignment of a flicht vehicle based on recursive matrix inversion | |
RU2781373C1 (ru) | Способ коррекции собственного местоположения и устройство коррекции собственного местоположения | |
CN114440892B (zh) | 一种基于拓扑地图和里程计的自定位方法 | |
WO2017026108A1 (ja) | 補正方法およびそれを利用した補正装置 | |
WO2022168353A1 (ja) | 車両位置推定装置、自動運転装置および車両位置推定方法 | |
JP2021135090A (ja) | 車両用測位装置および車両用測位装置用のプログラム |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20230206 |
|
A977 | Report on retrieval |
Free format text: JAPANESE INTERMEDIATE CODE: A971007 Effective date: 20231018 |
|
A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20231031 |
|
A521 | Request for written amendment filed |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20240119 |
|
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: 20240213 |
|
A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20240312 |
|
R150 | Certificate of patent or registration of utility model |
Ref document number: 7455004 Country of ref document: JP Free format text: JAPANESE INTERMEDIATE CODE: R150 |