JP7455004B2 - 位置推定方法及び位置推定システム - Google Patents

Description

本発明は、車両の位置を推定する位置推定方法及び位置推定システムに関する。

従来、車両の位置を推定する技術が存在する。例えば、車両に設けられた画像センサの出力に基づいてローカルマップにおける自己の位置を推定し、この推定された自己の位置と、ローカルマップにおける他の車両の位置とから、他の車両に対する自己の相対位置を推定する技術が提案されている。この技術では、第１の搬送波位相距離を用いたＧＮＳＳ測位情報と、他の車両において受信された第２の搬送波位相距離を用いたＧＮＳＳ測位情報と、自己の相対位置に基づいて自己の絶対位置を推定する技術もさらに提案されている（例えば、特許文献１参照）。

特開２０１９－１００９４２号公報

上述した従来技術では、測位装置により位置情報、例えばＧＮＳＳ測位情報を取得して車両の位置を推定することができる。なお、車両内の制御システムでは、ＧＮＳＳ測位情報の時刻と異なるローカルな時刻が使用されることが多い。このため、測位装置を備える車両内のローカルな時刻と、測位装置により取得されるＧＮＳＳ測位情報の時刻とが同期していないことも想定される。このように、車両内のローカルな時刻とＧＮＳＳ測位情報の時刻とが同期していない場合には、測位装置から遅れをもって得られた位置情報と、車載センサ（オドメトリなど）により取得された位置情報とを適切にカップリング（統合）することができない。仮に、測位装置からの遅れがないものと仮定して推定した車両の位置は、その遅れ時間内に車両が移動した分だけずれてしまう。このため、車両の位置を適切に推定することが困難となるおそれがある。

本発明は、車両の位置を適切に推定することを目的とする。

本発明の一形態は、車両情報に基づいて取得された車両の通過位置とその通過位置の車両の通過時刻とを関連付けて記憶する第１記憶部と、測位装置によって測位された車両の絶対位置とその絶対位置の測位時刻とを関連付けて記憶する第２記憶部とを備える車両の位置を推定する位置推定方法である。この位置推定方法は、複数の通過位置からなる第１走行軌跡と、複数の絶対位置からなる第２走行軌跡とを比較して、第１走行軌跡における通過位置と第２走行軌跡における絶対位置との対応を特定し、その対応する通過位置の通過時刻と絶対位置の測位時刻との時刻差を算出する時刻差算出ステップと、絶対位置と車両情報と算出された時刻差とに基づいて車両の位置を推定する位置推定ステップと、を含み、位置推定ステップでは、絶対位置の測位時刻を、算出された時刻差で補正し、当該測位時刻が補正された絶対位置と、直前に推定された車両の位置と、車両情報とをカルマンフィルタを用いて合成して、車両の位置を推定する。

本発明によれば、車両の位置を適切に推定することができる。

図１は、第１実施形態における位置推定システムの機能構成例を示すブロック図である。 図２Ａは、相対位置推定部により推定された相対座標系における位置の軌跡を示す図である。 図２Ｂは、絶対位置推定部により推定された絶対座標系における位置の軌跡を示す図である。 図３は、軌跡照合部による軌跡照合処理により２つの軌跡が照合された状態を模式的に示す図である。 図４は、軌跡照合部による照合開始判定処理の処理手順の一例を示すフローチャートである。 図５は、時刻差算出部による時刻差算出処理における相対位置の点の選択処理の一例を示す図である。 図６は、時刻差算出部による時刻差算出処理の処理手順の一例を示すフローチャートである。 図７は、位置推定システムによる位置推定処理の処理手順の一例を示すフローチャートである。 図８は、第２実施形態における時刻差算出部による時刻差算出処理の処理手順の一例を示すフローチャートである。 図９は、第３実施形態における軌跡照合部による照合開始判定処理の処理手順の一例を示すフローチャートである。

以下、添付図面を参照しながら本発明の実施形態について説明する。

［第１実施形態］
［位置推定システムの構成例］
図１は、第１実施形態における位置推定システム１００の機能構成例を示すブロック図である。位置推定システム１００は、車両２０（図３参照）に搭載され、車両２０の現在位置を推定するシステムである。

図１に示すように、位置推定システム１００は、角速度センサ１０１と、車速センサ１０２と、相対位置推定部１０３と、ＧＮＳＳ（Global Navigation Satellite System）アンテナ１０４と、絶対位置推定部１０５と、軌跡照合部１０６と、第１走行軌跡記憶部１０７と、第２走行軌跡記憶部１０８と、時刻差算出部１０９と、位置推定部１１０とを備える。なお、相対位置推定部１０３、軌跡照合部１０６、第１走行軌跡記憶部１０７、第２走行軌跡記憶部１０８、時刻差算出部１０９及び位置推定部１１０は、車両２０が備えるコントローラ１０により実現される。また、ＧＮＳＳアンテナ１０４及び絶対位置推定部１０５は、例えば、測位装置１１により実現される。測位装置１１は、例えばＧＰＳ（Global Positioning System）受信器である。また、角速度センサ１０１、車速センサ１０２、ＧＮＳＳアンテナ１０４及び絶対位置推定部１０５は、車両２０に搭載され、車両２０に関する車両情報を取得するセンサの一例である。なお、絶対位置推定部１０５はコントローラ１０により実現されるようにしてもよい。

コントローラ１０は、各種機器を制御する制御装置であり、例えば中央演算装置（ＣＰＵ（Central Processing Unit））、読み出し専用メモリ（ＲＯＭ（Read Only Memory））、ランダムアクセスメモリ（ＲＡＭ（Random Access Memory））及び入出力インタフェース（Ｉ／Ｏ（input/output）インタフェース）を備えたマイクロコンピュータで構成される。コントローラ１０は、特定のプログラムを実行することにより、車両２０に備えられるエンジン、モータ、インバータ、バッテリ等の各種機器の動作を制御する制御部として機能する。なお、コントローラ１０は、一つのマイクロコンピュータで構成されるのではなく、複数のマイクロコンピュータにより構成されてもよい。例えば、各種機器を制御する車両制御装置と、エンジンを制御するエンジンＥＣＵ（Engine Control Unit）とにより構成されてもよい。また、コントローラ１０により車両２０の制御システムが構成される。

角速度センサ１０１は、車両２０のヨーレート、すなわち、車両２０の旋回方向への回転角の速さを計測するセンサであり、計測されたヨーレートを相対位置推定部１０３に出力する。なお、角速度センサ１０１として、例えば、車両２０が回転するスピードであるヨーレートを検出可能な各機器、例えばジャイロセンサ等のヨーレートセンサを用いることができる。また、角速度センサ１０１の代わりに、複数の機器を用いるようにしてもよい。例えば、車両２０の車速及び舵角に基づいてヨーレートを検出可能であるため、これらを検出可能な車速センサ及び舵角センサを、角速度センサ１０１の代わりに用いるようにしてもよい。

車速センサ１０２は、車輪速パルスに基づいて車両２０の速度を計測するセンサであり、計測された速度を相対位置推定部１０３に出力する。

相対位置推定部１０３は、角速度センサ１０１で計測されたヨーレートと、車速センサ１０２で計測された速度とに基づいて、車両２０の位置を推定するものであり、推定された位置に関する位置情報を軌跡照合部１０６及び位置推定部１１０に出力する。すなわち、相対位置推定部１０３は、いわゆる、ホイールオドメトリを計算する。ここで、相対位置推定部１０３により推定される位置は、車両２０の制御システムの起動時の位置のような、任意の基準となる位置を原点として、原点からの移動方向及び移動量に基づいて推定される、原点を基準とした相対的な位置である。従って、相対位置推定部１０３により推定される位置は、基準となる位置（本実施形態では制御システムの起動時の位置とする）を原点に持つ座標系（相対座標系）における車両２０の位置である。また、本実施形態では、相対位置推定部１０３により推定された相対座標系における位置を相対位置またはローカルな位置と称して説明する。なお、相対位置には、その位置を車両２０が通過した際の車両２０の制御システムの時刻、すなわち通過時刻がタイムスタンプとして付与される。また、相対位置推定部１０３により推定された車両２０の相対位置の一例を図２Ａに示す。

ＧＮＳＳアンテナ１０４は、ＧＮＳＳ衛星からの信号を受信するアンテナであり、受信した信号を絶対位置推定部１０５に出力する。

絶対位置推定部１０５は、ＧＮＳＳアンテナ１０４で受信した信号に基づいて、車両２０の位置を推定するものであり、推定された位置に関する位置情報を軌跡照合部１０６及び位置推定部１１０に出力する。ここで、絶対位置推定部１０５により推定される位置は、ある特定の（不変の）地点を原点に持つ座標系（絶対座標系）における車両２０の位置である。また、本実施形態では、絶対位置推定部１０５により推定された絶対座標系における位置を絶対位置またはグローバルな位置と称して説明する。

ここで、絶対座標系は例えば直交座標系であり、東京周辺であれば平面直角座標系の東京９系を用いることができる。また、絶対位置にはＧＮＳＳの時刻、すなわち測位時刻がタイムスタンプとして付与される。また、絶対位置推定部１０５により推定された車両２０の絶対位置の一例を図２Ｂに示す。

軌跡照合部１０６は、相対位置推定部１０３により推定された車両２０の相対位置の軌跡と、絶対位置推定部１０５により推定された車両２０の絶対位置の軌跡とを幾何学的に照合（マッチング）するものであり、照合結果を時刻差算出部１０９に出力する。具体的には、軌跡照合部１０６は、相対位置推定部１０３から出力された車両２０の相対位置と通過時刻とを第１走行軌跡記憶部１０７に保持させる。また、軌跡照合部１０６は、絶対位置推定部１０５から出力された車両２０の絶対位置と測位時刻とを第２走行軌跡記憶部１０８に保持させる。また、軌跡照合部１０６は、所定のタイミングで、第１走行軌跡記憶部１０７に保持された車両２０の相対位置及び通過時刻と、第２走行軌跡記憶部１０８に保持された車両２０の絶対位置及び測位時刻とを取得する。そして、軌跡照合部１０６は、取得した車両２０の相対位置の軌跡と、車両２０の絶対位置の軌跡とについて軌跡照合処理を行うことにより、２つの軌跡を照合する。なお、軌跡照合処理については、図３を参照して詳細に説明する。また、軌跡照合処理を開始するタイミングを判定する照合開始判定処理については、図４を参照して詳細に説明する。

第１走行軌跡記憶部１０７は、相対位置推定部１０３により推定された車両２０の相対位置と、その位置を車両２０が通過した通過時刻とを関連付けて記憶するメモリであり、例えばＲＡＭにより実現される。

第２走行軌跡記憶部１０８は、絶対位置推定部１０５により推定された車両２０の絶対位置と、その位置を測位した測位時刻とを関連付けて記憶するメモリであり、例えばＲＡＭにより実現される。

時刻差算出部１０９は、軌跡照合部１０６により照合された２つの軌跡を比較して、相対位置の各通過時刻と、絶対位置の各測位時刻との時刻差を算出するものであり、その算出結果を位置推定部１１０に出力する。すなわち、時刻差算出部１０９は、軌跡照合部１０６により照合された２つの軌跡に含まれる点群同士を対応付けし、対応付けられた各点の通過時刻、測位時刻を比較することで車両２０の制御システムの時刻とＧＮＳＳの時刻との時刻差を算出する。なお、時刻差算出部１０９による時刻差算出処理については、図５、図６を参照して詳細に説明する。

位置推定部１１０は、絶対位置推定部１０５により推定された絶対位置と車両情報と時刻差算出部１０９により算出された時刻差とに基づいて、車両２０の現在位置を推定するものである。なお、車両情報は、車両２０に搭載されている機器により取得可能な情報であり、例えば、角速度センサ１０１により計測されたヨーレートと、車速センサ１０２により計測された車速である。具体的には、位置推定部１１０は、時刻差算出部１０９により算出された時刻差を用いて、絶対位置推定部１０５により推定された絶対位置の測位時刻と車両２０の制御システムの時刻とのズレを補正する。そして、位置推定部１１０は、ズレが補正された絶対位置と、直前に推定された車両２０の位置と、車両情報とをカルマンフィルタを用いて統合して車両２０の現在位置を推定する。なお、位置推定部１１０による位置推定処理については、図７を参照して詳細に説明する。

ここで、絶対位置推定部１０５により推定される絶対位置とこれに付与される測位時刻とは精度が高いものである。しかし、車両２０が備える制御システムが、絶対位置推定部１０５からその絶対位置と測位時刻とを取得する際には、各種処理や各種情報のやり取りが発生するため遅延が生じる。また、その遅延時間は固定的な値ではなく、ばらつきが生じる。すなわち、絶対位置推定部１０５による絶対位置及び測位時刻により、車両２０がどの位置にいつの時刻に存在したかを正確に把握することは可能であるが、車両２０の制御システムがその絶対位置及び測位時刻を受け取る際には、ばらつきのある遅延が発生する。

また、一般に、車両の内部では、車両が備える制御システムにより計時されたローカルな時刻が用いられている。また、自動運転機能を備える車両は、外部の時間に同期する処理がされていないことが多い。このため、車両２０の制御システムと、ＧＮＳＳの測位時刻とが同期していないと、絶対位置推定部１０５から遅れをもって得られた絶対位置と、車両情報、例えば車載センサの情報（オドメトリなど）とをカップリング（統合）することができない。仮に、絶対位置推定部１０５からの遅れがないものと仮定して推定した車両の位置は、その遅れ時間内に車両が移動した分だけずれてしまう。なお、車両２０の制御システムの時刻を、ＧＮＳＳの測位時刻に同期させればよいとも考えられる。しかし、様々な制約から車両２０では、ＧＮＳＳとは異なるシステム時間が採用されている場合が多く、時刻同期という単純な方策が成立しないことが多い。そこで、本実施形態では、特徴的な軌跡、例えば、車両２０がカーブする際の軌跡を用いて、相対位置の各通過時刻と絶対位置の各測位時刻との時刻差を算出する。そして、この時刻差を用いて、絶対位置の測位時刻と車両２０の制御システムの時刻とのズレを補正し、車両２０の現在位置を推定する。

［相対座標系と絶対座標系における軌跡例］
図２Ａは、相対位置推定部１０３により推定された相対座標系における位置の軌跡１を示す図である。図２Ｂは、絶対位置推定部１０５により推定された絶対座標系における位置２の軌跡を示す図である。なお、図２Ａ及び図２Ｂでは、所定時間内に推定された位置の軌跡１、２を簡略化して示す。また、図２Ａ及び図２Ｂでは、軌跡照合部１０６により２つの軌跡１、２が照合される前の状態、すなわち初期状態を示す。

図２Ａには、車両２０のシステム起動時を原点とする座標系（相対座標系）Ｘｒ、Ｙｒにおいて、相対位置推定部１０３により求められた車両２０の相対位置ＶＴ１乃至ＶＴ１０と、相対位置ＶＴ１乃至ＶＴ１０を結ぶ軌跡１とを示す。なお、相対位置ＶＴ１乃至ＶＴ１０には車両２０の制御システムの時刻がタイムスタンプとして付与されている。

図２Ｂには、特定の地点を原点とする座標系（絶対座標系）Ｘｇ、Ｙｇにおいて、絶対位置推定部１０５により求められた車両２０の絶対位置ＧＴ１乃至ＧＴ３と、絶対位置ＧＴ１乃至ＧＴ３を結ぶ軌跡２とを示す。なお、絶対座標系として、平成１４年国土交通省告示第９号で定義された平面直角座標系を用いることができる。例えば、東京周辺であれば平面直角座標系の東京９系を用いることができる。なお、絶対位置ＧＴ１乃至ＧＴ３にはＧＮＳＳの測位時刻がタイムスタンプとして付与されている。

ここで、一般に、車両に搭載されている角速度センサや車速センサは、ＧＮＳＳの測位周期に比べて高速に動作している。このため、角速度センサや車速センサを用いて取得される相対位置は、ＧＮＳＳの測位に基づいて取得される絶対位置に比べて多くなる。そこで、図２Ａ及び図２Ｂでは、所定時間内に取得された相対位置ＶＴ１乃至ＶＴ１０が、所定時間内に取得された絶対位置ＧＴ１乃至ＧＴ３に比べて多い例を示す。

図２Ａ及び図２Ｂに示すように、所定時間内に取得された相対位置ＶＴ１乃至ＶＴ１０と絶対位置ＧＴ１乃至ＧＴ３とは、点群をなす。また、相対位置ＶＴ１乃至ＶＴ１０と絶対位置ＧＴ１乃至ＧＴ３とのそれぞれには、時刻がタイムスタンプとして付与されている。

［軌跡照合処理例］
図３は、軌跡照合部１０６による軌跡照合処理により２つの軌跡が照合された状態を模式的に示す図である。

軌跡照合部１０６は、図２Ａに示す軌跡１と、図２Ｂに示す軌跡２とについて、位置合わせを行い、照合する軌跡照合処理を行う。なお、点群と点群の位置合わせはｒｅｇｉｓｔｒａｔｉｏｎと呼ばれ、様々な手法が提案されている。

例えば、以下の非特許文献１に記載されているＩＣＰ（Iterative Closest Point）アルゴリズムは、２つの点群を位置合わせする位置合わせ手法として広く用いられている。

非特許文献１：P.J. Besl and N.D. McKay: A Method for Registration of 3-D Shapes, IEEE Trans. Pattern Analysis and Machine Intelligence, Vol. 14, No. 2, pp. 239 - 256 (1992).

このＩＣＰアルゴリズムは、一方の点群（第一の点群と称する）を他方の点群（第二の点群と称する）にできるだけ近付けるような回転と並進を求める手法である。

具体的には、第一の点群に含まれる各点について、第二の点群に含まれるその最近傍の点を対応付け、対応点間の距離の総和を最小化する回転と並進を求める。次に、求めた回転と並進を第一の点群に施し、この回転と並進を施した後に上述した処理を繰り返す。そして、所定条件を満たす場合に計算を終了し、最終的に最適な回転と並進を得る。なお、所定条件を満たす場合は、例えば、上述した処理を所定回数繰り返したタイミング、または、何らかの収束判定による終了タイミングである。

このような軌跡照合処理により２つの軌跡１、２の位置合わせが行われた後の状態を図３に示す。また、このような軌跡照合処理を行うタイミングを図４に示す。

［照合開始判定処理例］
図４は、軌跡照合部１０６による照合開始判定処理の処理手順の一例を示すフローチャートである。なお、この処理手順は、記憶装置（図示省略）に記憶されているプログラムに基づいて実行される。また、図４では、車両２０がカーブを曲がり始める前の位置を始点とし、車両２０がそのカーブを所定量だけ曲がった位置を終点とする走行軌跡を用いて軌跡照合処理を実行する場合の例を示す。例えば、車両２０がカーブを曲がるときの軌跡を用いて軌跡照合処理を実行することができる。

まず、軌跡照合部１０６は、ヨーレートフラグを用意し、その初期値はオフとしておく。ヨーレートフラグは、特徴的な軌跡、例えば車両２０がカーブを曲がるときの軌跡を取得するために用いられるフラグである。なお、図４に示すヨーレートフラグは、車両２０がカーブを曲がり始める前の位置を始点とし、車両２０がそのカーブを所定量だけ曲がった位置を終点とする走行軌跡を取得するためのフラグである。また、ヨーレートフラグがオンの場合に取得された絶対位置からなる走行軌跡と、相対位置からなる走行軌跡とを用いて、軌跡照合部１０６は軌跡照合処理を行う。

ステップＳ２０１において、軌跡照合部１０６は、ヨーレートフラグの判定を行う。すなわち、軌跡照合部１０６は、ヨーレートフラグの状態に基づいて条件分岐を行う。そして、ヨーレートフラグがオフの場合には、ステップＳ２０２に進み、ヨーレートフラグがオンの場合には、ステップＳ２０４に進む。

ステップＳ２０２において、軌跡照合部１０６は、角速度センサ１０１により計測されたヨーレートの判定を行う。すなわち、軌跡照合部１０６は、現在のヨーレートの値に基づいて条件分岐を行う。そして、ヨーレートが閾値未満の場合には、ステップＳ２０３に進み、軌跡照合部１０６は、ヨーレートフラグをオンにする。一方、ヨーレートが閾値以上の場合には、何もせず照合開始判定処理を終了する。なお、ステップＳ２０２で用いるヨーレートの閾値として、車両２０が直進していることが判定できるレベルを各種の実験データを用いて設定する。例えば０．１乃至０．１５（ｒａｄ／ｓ）程度の値を用いることができる。

ステップＳ２０４において、軌跡照合部１０６は、角速度センサ１０１により計測されたヨーレートの時間積分を行う。すなわち、軌跡照合部１０６は、車両２０のヨー角変化の累積値を求める。このように、ヨーレートの時間積分を行うことにより、ヨーレートフラグがオンされてから車両２０がどの程度曲がったかを判定することが可能となる。なお、車両２０が左右方向の一方に曲がることも想定されるが、車両２０がＳ字に曲がることも想定される。また、車両２０がＳ字に曲がった場合の軌跡についても軌跡照合処理に用いることができるため、ヨーレートの絶対値を積分して用いるようにする。

ステップＳ２０５において、軌跡照合部１０６は、ステップＳ２０４での演算結果である積分値を判定する。すなわち、軌跡照合部１０６は、積分値に基づいて条件分岐を行う。そして、積分値が閾値以上である場合には、ステップＳ２０６に進む。一方、積分値が閾値未満である場合には、何もせず照合開始判定処理を終了する。なお、ステップＳ２０５で用いる積分値の閾値として、軌跡照合処理におけるマッチング精度を高めることが可能なレベルを各種の実験データを用いて設定する。例えばπ／２（ｒａｄ）程度の値を用いることができる。なお、ステップＳ２０５で用いる積分値の閾値を大きな値とした場合には、マッチング精度を高めることができるが、軌跡照合処理の頻度は減少する。一方、ステップＳ２０５で用いる積分値の閾値を小さな値とした場合には、マッチング精度は低下するが、軌跡照合処理の頻度を増加させることができる。

ステップＳ２０６において、軌跡照合部１０６は、軌跡照合処理を実行する。すなわち、図２Ａ、図２Ｂ、図３を参照して説明した軌跡照合処理が実行される。

ステップＳ２０７において、軌跡照合部１０６は、ヨーレートフラグをオフにする。

このように、本実施形態では、特徴的な軌跡、例えば車両２０が交差点やカーブを曲がるときの軌跡が現れる度に軌跡照合処理を実施する。これにより、軌跡のマッチングの誤差等によって時刻差の推定に失敗しても、その後にマッチングしやすい軌跡が現れた時点で正しい時刻差を推定できる。また、安定して推定された時刻差を採用することができる。

［相対位置の選択処理例］
図５は、時刻差算出部１０９による時刻差算出処理における相対位置の点の選択処理の一例を示す図である。具体的には、図５には、絶対位置の点ＧＴｉと、点ＧＴｉから半径Ｒ１内に存在する相対位置の点ＶＴｉ１乃至ＶＴｉ５との関係を模式的に示す。なお、図５では、半径Ｒ１の円Ｒ２を点線で示す。また、相対位置の点ＶＴｉ１乃至ＶＴｉ５に関連付けられている通過時刻のうち、点ＶＴｉ１に関連付けられている通過時刻が最も古い時刻であり、点ＶＴｉ５に関連付けられている通過時刻が最も新しい時刻であるものとする。

なお、絶対位置の点ＧＴｉは、上述した軌跡照合処理によりマッチングされた２つの軌跡のうち、複数の絶対位置からなる軌跡に含まれる。また、相対位置の点ＶＴｉ１乃至ＶＴｉ５は、上述した軌跡照合処理によりマッチングされた２つの軌跡のうち、複数の相対位置からなる軌跡に含まれる。なお、図５については、図６を参照して詳細に説明する。

［時刻差算出処理例］
図６は、時刻差算出部１０９による時刻差算出処理の処理手順の一例を示すフローチャートである。なお、この処理手順は、記憶装置（図示省略）に記憶されているプログラムに基づいて実行される。また、時刻差算出処理の対象となる２つの軌跡は、上述した軌跡照合処理によりマッチングされた後の軌跡である。

図６では、絶対位置点群に含まれる各点についてステップＳ５０２からＳ５０５の処理を繰り返す。すなわち、絶対位置点群に含まれる各点から１つの点が順次選択され、その選択された点についてステップＳ５０２からＳ５０５の処理を繰り返す。なお、絶対位置点群は、上述した軌跡照合処理によりマッチングされた２つの軌跡のうち、複数の絶対位置からなる軌跡における各絶対位置を意味する。

例えば、図３に示す例では、ＧＴ１乃至ＧＴ３のそれぞれについてステップＳ５０１からＳ５０５の処理を繰り返す。なお、図６では、絶対位置点群に含まれる各点のうち、ステップＳ５０２からＳ５０５の処理の対象となる点を対象点と称して説明する。

ステップＳ５０１では、絶対位置点群のうちの未選択の１つの点を対象点として選択する。

ステップＳ５０２において、時刻差算出部１０９は、絶対位置の対象点から所定範囲内にある相対位置の点（対応点群）を選択して対応付ける。例えば、図５では、絶対位置の対象点ＧＴｉから半径Ｒ１の円Ｒ２を所定範囲とする例を示す。すなわち、図５に示す例では、絶対位置の対象点ＧＴｉから半径Ｒ１内に存在する相対位置の点ＶＴｉ１乃至ＶＴｉ５が、絶対位置の対象点ＧＴｉに対応した相対位置の点ＶＴｉ１乃至ＶＴｉ５として選択される。なお、時刻差算出処理の対象となる２つの軌跡は、上述した軌跡照合処理によりマッチングされている。このため、所定範囲は、数十ｃｍ程度、例えば１０乃至２０ｃｍ程度とすることができる。なお、所定範囲は、各種の実験データを用いて設定することができる。このように、ステップＳ５０２では、絶対位置の対象点と相対位置の点との対応が特定される。

ステップＳ５０３において、時刻差算出部１０９は、ステップＳ５０２で選択された相対位置の各点（対応点群）に関連付けられている通過時刻のうち、最も古い時刻と最も新しい時刻とを求める。そして、時刻差算出部１０９は、最も古い通過時刻と最も新しい通過時刻との差（時刻幅）を求める。上述したように、図５に示す例では、相対位置の点ＶＴｉ１乃至ＶＴｉ５に関連付けられている通過時刻のうち、点ＶＴｉ１に関連付けられている通過時刻が最も古い時刻であり、点ＶＴｉ５に関連付けられている通過時刻が最も新しい時刻である。この場合には、時刻差算出部１０９は、点ＶＴｉ１に関連付けられている通過時刻と、点ＶＴｉ５に関連付けられている通過時刻との差（時刻幅）を求める。

ステップＳ５０４において、時刻差算出部１０９は、ステップＳ５０３で求められた時刻幅が閾値以上であるか否かを判定する。そして、ステップＳ５０３で求められた時刻幅が閾値未満である場合には、ステップＳ５０５に進む。一方、ステップＳ５０３で求められた時刻幅が閾値以上である場合には、何もせずにステップＳ５０６に進む。なお、ステップＳ５０４で用いる時刻幅の閾値は、各種の実験データを用いて設定することができる。

例えば、車両２０が交差点を曲がる場合に、その交差点を歩行者が横断していることも想定される。この場合には、車両２０は、その歩行者が横断するまでの間、停止していることになる。このような状況では、その停止中に略同じ位置において、複数の絶対位置と複数の相対位置とが記録されることになる。上述したように、角速度センサや車速センサを用いて取得される相対位置は、ＧＮＳＳの測位に基づいて取得される絶対位置に比べて多くなる。このため、１つの絶対位置から所定範囲内にある相対位置の数が多くなり、かつ、これらの相対位置の通過時刻の時刻差が大きくなると想定される。このような場合には、絶対位置の測位時刻と、これに対応する相対位置の通過時刻との時刻差として、正確な値を算出することが困難であると想定される。そこで、このような状況で取得された絶対位置とこれに対応する相対位置については、時間差算出に使用しないようにする。

ステップＳ５０５において、時刻差算出部１０９は、ステップＳ５０２で選択された相対位置の各点（対応点群）の中から絶対位置の対象点に最も近い点を選択し、絶対位置の対象点の測位時刻と相対位置の通過時刻との差を算出する。例えば、図５に示す例では、相対位置の点ＶＴｉ１乃至ＶＴｉ５のうち、絶対位置の対象点ＧＴｉに最も近い点は、点ＶＴｉ３である。このため、時刻差算出部１０９は、絶対位置の対象点ＧＴｉに関連付けられている測位時刻と、相対位置の点ＶＴｉ３に関連付けられている通過時刻との差を算出する。また、時刻差算出部１０９は、算出された時刻差と絶対位置の対象点ＧＴｉとを関連付けて保持する。なお、この例では、絶対位置の対象点に最も近い相対位置の通過時刻を用いて時刻差を算出する例を示すが、他の演算方法により時刻差を算出するようにしてもよい。例えば、ステップＳ５０２で選択された相対位置の各点の通過時刻の平均値を算出し、この平均値と、絶対位置の対象点の測位時刻との差を時刻差として算出するようにしてもよい。このように、ステップＳ５０５では、対応する絶対位置の対象点の測位時刻と相対位置の通過時刻との時刻差が算出される。

ステップＳ５０６において、絶対位置点群に含まれる全ての点について、ステップＳ５０２からＳ５０５の処理が終了しているか否か、すなわち絶対位置点群に含まれる全ての点がステップＳ５０１で対象点として選択済であるか否かを判定して、絶対位置点群に含まれる全ての点がステップＳ５０１で対象点として選択済である場合には、ステップＳ５０７に進む。一方、絶対位置点群に含まれる全ての点がステップＳ５０１で対象点として選択済でない場合は、ステップＳ５０１に戻って未選択の点を対象点として選択し、ステップＳ５０２からステップＳ５０５の処理を行う。

ステップＳ５０７において、時刻差算出部１０９は、ステップＳ５０５で絶対位置の対象点毎に求められた時刻差の平均を算出する。このように算出された平均値が、上述した軌跡照合処理によりマッチングされた２つの軌跡の時刻差の推定値とされる。すなわち、複数の絶対位置からなる軌跡と、複数の相対位置からなる軌跡との時刻差の推定値とされる。なお、ステップＳ５０３で求められた時刻幅が閾値以上となった絶対位置の対象点については、ステップＳ５０５で時刻差が算出されないため、ステップＳ５０７での平均値の算出に用いられない。

このように、ステップＳ５０７において、ステップＳ５０５で絶対位置毎に算出された時刻差を用いて、上述した軌跡照合処理によりマッチングされた２つの軌跡の時刻差を算出する。

なお、時刻差算出部１０９は、軌跡照合部１０６による照合開始判定処理で照合開始と判定される度に図６に示す時刻差算出処理を行い、時刻差を算出する。そして、時刻差算出部１０９は、前回算出した時刻差と今回算出した時刻差が所定値以上変化している場合には、前回の算出結果が誤りであったとして今回の算出値を最終的に時刻差の推定値とする。この例を図７に示す。

［位置推定処理例］
図７は、位置推定システム１００による位置推定処理の処理手順の一例を示すフローチャートである。なお、この処理手順は、記憶装置（図示省略）に記憶されているプログラムに基づいて実行される。

ステップＳ６０１において、時刻差算出部１０９は、軌跡照合部１０６による軌跡照合処理が実行されたか否かを判定する。そして、軌跡照合部１０６による軌跡照合処理が実行された場合には、ステップＳ６０２に進む。一方、軌跡照合部１０６による軌跡照合処理が実行されていない場合には、ステップＳ６０６に進む。

ステップＳ６０２において、時刻差算出部１０９は、図６に示す時刻差算出処理を実行する。

ステップＳ６０３において、時刻差算出部１０９は、前回の時刻差算出処理により算出された時刻差と、今回の時刻差算出処理により算出された時刻差とが所定値以上変化したか否かを判定する。そして、前回と今回の時刻差が所定値以上変化した場合には、ステップＳ６０５に進む。一方、前回と今回の時刻差が所定値以上変化していない場合には、ステップＳ６０４に進む。

ステップＳ６０４において、時刻差算出部１０９は、前回の時刻差算出処理により算出された時刻差と、今回の時刻差算出処理により算出された時刻差との何れか一方または双方を、位置推定処理に用いる時刻差として決定する。例えば、前回と今回の時刻差算出処理により算出された２つの時刻差を用いる場合には、これらの平均値を用いることができる。

ステップＳ６０５において、時刻差算出部１０９は、今回の時刻差算出処理により算出された時刻差を、位置推定処理に用いる時刻差として決定する。すなわち、前回の時刻差算出処理の算出結果が誤りであったとして今回の算出値を最終的に時刻差の推定値とする。言い換えると、前回と今回の時刻差が閾値よりも大きい場合には、データをリセットにするため、今回の算出値を最終的に時刻差の推定値とする。なお、過去の複数の時刻差算出処理により算出された時刻差の平均値を算出しておき、この平均値と、今回の時刻差算出処理により算出された時刻差とを比較して決定するようにしてもよい。この場合には、過去の平均値と今回の時刻差とが所定値以上変化した場合には、今回の時刻差算出処理の算出結果が誤りであったとして、過去の平均値を用いるようにしてもよい。

ステップＳ６０６において、位置推定部１１０は、時刻差算出部１０９により算出された時刻差を用いて、車両２０の現在位置を推定する位置推定処理を行う。なお、位置推定部１１０による位置推定処理については以下で詳細に説明する。

このように、ステップＳ６０６では、軌跡照合部１０６による軌跡照合処理が実行され、ステップＳ６０４またはＳ６０５で位置推定処理に用いる時刻差が決定された場合には、その時刻差を用いた位置推定処理が行われる。また、軌跡照合部１０６による軌跡照合処理が実行されていない場合には、直前に決定された最新の時刻差を用いた位置推定処理が行われる。

このように、特徴的な軌跡が現れる度に、軌跡照合処理、時刻差算出処理を実施し、過去に推定した時刻差を順次更新する。

次に、時刻差算出部１０９が算出した時刻差を用いて車両２０の現在位置を推定する位置推定部１１０について説明する。

位置推定部１１０はカルマンフィルタを用いて車両２０の現在位置を推定する。すなわち、位置推定部１１０は、カルマンフィルタを用いて、過去の推定値と制御システムへの入力値と観測値とを合成して現在の推定値を算出する。なお、過去の推定値は、位置推定部１１０により推定された過去の車両２０の位置及び姿勢である。また、制御システムへの入力値は、車両２０に搭載されている機器により取得可能な車両情報であり、例えば、角速度センサ１０１により計測されたヨーレートと、車速センサ１０２により計測された車速である。また、観測値は、絶対位置推定部１０５により求められた絶対位置である。なお、カルマンフィルタについては、以下の非特許文献２等に開示されている。

非特許文献２：足立修一 他、カルマンフィルタの基礎、東京電機大学出版局、２０１２年。

また、位置推定部１１０による位置推定処理に用いられる状態方程式は、以下に示す式１で表される。状態ベクトルは、現在位置（ｘ、ｙ）と姿勢角（θ）を表す（ｘ、ｙ、θ）^Tであり、制御入力ベクトルは、車速ｖとヨーレートψからなる（ｖ、ψ）^Tである。なお、上付きのＴは行列の転置を表す。

ｘ＝ｘ＋ｖΔＴｃｏｓθ
ｙ＝ｙ＋ｖΔＴｓｉｎθ
θ＝θ＋φΔＴ …式１

位置推定部１１０は、カルマンフィルタを用いて、上述した式１に基づいて現在時刻の車両２０の位置及び姿勢を予測する。また、位置推定部１１０は、カルマンフィルタを用いて、その予測値を修正するために、絶対位置推定部１０５で算出された絶対位置（ｘｇ、ｙｇ）^Tを観測して、その観測値にその予測値を近づけるように修正する。

このように修正する場合に、ＧＮＳＳアンテナ１０４からのＧＮＳＳ信号を受け取ってから絶対位置推定部１０５が絶対位置を算出し、その絶対位置が位置推定部１１０に伝わるまでに遅延が生じる。このため、位置推定部１１０が扱う絶対位置は必ず過去のデータとなる。そこで、位置推定部１１０が受け取った絶対位置がいつの絶対位置かを知るために絶対位置の測位時刻（タイムスタンプ）が必要になる。ただし、同一または略同一の位置であっても、絶対位置の測位時刻と相対位置の通過時刻との間には、時刻差算出部１０９で算出した時刻差がある。このため、位置推定部１１０は、位置推定処理を行う前に、時刻差算出部１０９で算出した時刻差を用いて絶対位置の測位時刻を修正する。すなわち、絶対位置の測位時刻を相対位置の通過時刻に合わせるように、絶対位置の測位時刻のズレを補正する。この補正では、例えば、絶対位置の測位時刻と時刻差算出部１０９で算出した時刻差とを加算または減算することにより、絶対位置の測位時刻のズレを補正する。または、時刻差算出部１０９で算出した時刻差に所定の演算、例えば乗算や除算を施した後の値を用いて、絶対位置の測位時刻のズレを補正するようにしてもよい。

時刻差算出部１０９で算出された時刻差を用いて修正された時刻をＴとすると、位置推定部１１０は、式１に基づいて時刻Ｔの位置及び姿勢を予測する。その上で、位置推定部１１０は、予測された位置（ｘ、ｙ）^Tと、絶対位置推定部１０５で算出された絶対位置（ｘｇ、ｙｇ）^Tとの差を求め、この差に基づいて予測値をさらに修正する。

このように、位置推定部１１０は、絶対位置推定部１０５で算出された絶対位置の測位時刻を、時刻差算出部１０９で算出された時刻差で補正する。そして、位置推定部１１０は、その測位時刻が補正された絶対位置と、直前に推定された車両２０の位置と、車両情報とをカルマンフィルタを用いて合成して車両２０の位置を推定する。なお、このように推定された現在位置は、記憶部（図示省略）に保持され、次の位置推定処理に用いられる。

［第１実施形態の作用効果］
第１実施形態に係る位置推定方法は、車両情報に基づいて取得された車両２０の通過位置（相対位置）とその通過位置の車両２０の通過時刻とを関連付けて記憶する第１走行軌跡記憶部１０７（第１記憶部の一例）と、測位装置１１によって測位された車両２０の絶対位置とその絶対位置の測位時刻とを関連付けて記憶する第２走行軌跡記憶部１０８（第２記憶部の一例）とを備える車両２０の位置を推定する位置推定方法である。この位置推定方法は、複数の通過位置からなる第１走行軌跡と、複数の絶対位置からなる第２走行軌跡とを比較して、第１走行軌跡における通過位置と第２走行軌跡における絶対位置との対応を特定し、その対応する通過位置の通過時刻と絶対位置の測位時刻との時刻差を算出する時刻差算出ステップ（ステップＳ６０２）と、絶対位置と車両情報と算出された時刻差とに基づいて車両２０の位置を推定する位置推定ステップ（ステップＳ６０６）と、を含む。

このような位置推定方法によれば、第１走行軌跡及び第２走行軌跡における多数の対応点から時刻差を推定することができるため、安定した時刻差の推定ができる。また、そのように求められた時刻差を用いて車両２０の現在位置を推定するため、位置推定精度を向上させることができる。

また、第１実施形態に係る位置推定方法では、位置推定ステップ（ステップＳ６０６）は、絶対位置の測位時刻を、ステップＳ６０２で算出された時刻差で補正し、その測位時刻が補正された絶対位置と、直前に推定された車両２０の位置と、車両情報とをカルマンフィルタを用いて合成して車両２０の位置を推定する。

このような位置推定方法によれば、求められた時刻差を用いて絶対位置の測位時刻を補正し、その測位時刻の補正後の絶対位置を用いて車両２０の現在位置を推定するため、位置推定精度を向上させることができる。

また、第１実施形態に係る位置推定方法では、時刻差算出ステップ（ステップＳ６０２）は、第２走行軌跡における絶対位置の測位時刻と、第１走行軌跡における当該絶対位置に対応する通過位置の通過時刻との差分値に基づいて時刻差を算出する。

このような位置推定方法によれば、第１走行軌跡及び第２走行軌跡における多数の対応点から時刻差を推定することができるため、安定した時刻差の推定ができる。

また、第１実施形態に係る位置推定方法では、時刻差算出ステップ（ステップＳ６０２）は、第２走行軌跡における各絶対位置について、その絶対位置から所定範囲に存在する第１走行軌跡における通過位置を、その絶対位置に対応する通過位置として選択し、その絶対位置の測位時刻とその選択された通過位置の通過時刻との差分値を算出し、その算出された各差分値に基づいて時刻差を算出する。

このような位置推定方法によれば、第１走行軌跡及び第２走行軌跡における多数の対応点から時刻差を推定することができるため、安定した時刻差の推定ができる。

また、第１実施形態に係る位置推定方法では、時刻差算出ステップ（ステップＳ６０２）は、選択された通過位置の通過時刻のうち最も古い時刻と最も新しい時刻との差分値が閾値を基準として大きい場合には、その選択された通過位置を時刻差の算出に用いない。なお、閾値を基準として大きい場合とは、閾値以上の場合と、閾値よりも大きい場合との双方の意味を含むものとする。また、閾値を基準として小さい場合とは、閾値以下の場合と、閾値未満の場合との双方の意味を含むものとする。ただし、閾値を基準として大きい場合が閾値以上を意味するときには、閾値を基準として小さい場合は閾値未満を意味するものとする。一方、閾値を基準として大きい場合が閾値よりも大きい場合を意味するときには、閾値を基準として小さい場合は閾値以下を意味するものとする。

このような位置推定方法によれば、曖昧性を排除することで時刻差の推定精度を向上させることができる。

また、第１実施形態に係る位置推定方法では、第２走行軌跡における各絶対位置について、第２走行軌跡における絶対位置と第１走行軌跡における当該絶対位置に対応する通過位置との対応をとり、その絶対位置及びその通過位置間の距離を最小化する処理を行うことにより第１走行軌跡と第２走行軌跡とをマッチングさせる軌跡照合ステップ（ステップ２０６）をさらに含む。また、時刻差算出ステップ（ステップＳ６０２）では、そのマッチング後の第１走行軌跡と第２走行軌跡とを比較して時刻差を算出する。

ここで、一般にＧＮＳＳの観測周期は長く、点が疎であるのに対して、オドメトリの観測周期は短く、点が密である。すなわち、第２走行軌跡に含まれる絶対位置は、点が疎であるのに対し、第１走行軌跡に含まれる通過位置は、点が密である。そこで、このような位置推定方法によれば、第２走行軌跡に含まれる絶対位置の全てについて、第１走行軌跡に含まれる適切な対応点を得ることができるため、マッチングの精度を向上させることができる。これにより、時刻差推定精度を向上させることができる。

また、第１実施形態に係る位置推定方法では、軌跡照合ステップ（ステップ２０６）は、車両２０がカーブを曲がり始める前の位置を始点とし、車両２０がカーブを所定量だけ曲がった位置を終点とする第１走行軌跡及び第２走行軌跡を用いてマッチングを実行する。

このような位置推定方法によれば、幾何学的にマッチングしやすい特徴的な走行軌跡を用いることができるため、マッチングの精度を向上させることができ、時刻差推定精度を向上させることができる。

また、第１実施形態に係る位置推定方法では、軌跡照合ステップ（ステップ２０６）は、車両２０のヨーレートが略ゼロとなる位置を車両２０がカーブを曲がり始める前の位置とし、そのヨーレートが略ゼロとなった後に車両２０のヨー角変化の累積が所定値を基準として大きくなった位置を車両２０がカーブを所定量だけ曲がった位置とする。

このような位置推定方法によれば、車両２０のヨーレートを考慮したカーブ区間の抽出を行うことができる。このように抽出された軌跡は、所定量以上の角度変化を含む曲線となるので、マッチング精度を向上させることができる。

また、第１実施形態に係る位置推定方法では、時刻差算出ステップ（ステップＳ６０２乃至Ｓ６０５）は、マッチングが行われる毎に時刻差を算出し、第１の時刻差と、第１の時刻差の算出後に新たに算出された第２の時刻差とを比較し、第１の時刻差と第２の時刻差との差分値が所定範囲内にある場合には、第１の時刻差及び第２の時刻差の何れか一方または双方を車両２０の位置を推定する値として用いる決定をし、一方、第１の時刻差と第２の時刻差との差分値が所定範囲内にない場合には、第２の時刻差を車両２０の位置を推定する値として用いる決定をする。

このような位置推定方法によれば、軌跡のマッチングの誤差等によって時刻差の推定に失敗しても、適切な時刻差を用いることができ、マッチングしやすい軌跡が現れた時点で正しい時刻差を推定できる。また、安定して推定された時刻差のみを採用することができる。

第１実施形態に係る位置推定システム１００は、車両２０の位置を推定するコントローラ１０を備える位置推定システムである。コントローラ１０は、車両情報に基づいて取得された車両２０の通過位置とその通過位置の車両２０の通過時刻とを関連付けて記憶する第１走行軌跡記憶部１０７（第１記憶部の一例）と、測位装置１１によって測位された車両２０の絶対位置とその絶対位置の測位時刻とを関連付けて記憶する第２走行軌跡記憶部１０８（第２記憶部の一例）と、複数の通過位置からなる第１走行軌跡と、複数の絶対位置からなる第２走行軌跡とを比較して、第１走行軌跡における通過位置と第２走行軌跡における絶対位置との対応を特定し、その対応する通過位置の通過時刻と絶対位置の測位時刻との時刻差を算出する時刻差算出部１０９と、絶対位置と車両情報と算出された時刻差とに基づいて車両２０の位置を推定する位置推定部１１０と、を備える。

このような位置推定システム１００によれば、第１走行軌跡及び第２走行軌跡における多数の対応点から時刻差を推定することができるため、安定した時刻差の推定ができる。また、そのように求められた時刻差を用いて車両２０の位置を推定するため、位置推定精度を向上させることができる。

［第２実施形態］
第１実施形態では、絶対位置の対象点から所定範囲内に存在する相対位置の各点に関連付けられている通過時刻のうち最も古い時刻と最も新しい時刻との差（時刻幅）を用いて時刻差算出処理を行う例を示した。ただし、他の判定基準を用いて時刻差算出処理を行うようにしてもよい。そこで、第２実施形態では、絶対位置の対象点から所定範囲内に存在する相対位置の点の個数を用いて時刻差算出処理を行う例を示す。なお、第２実施形態は、第１実施形態の一部を変形した例であり、第１実施形態と共通する部分については、図示及びその説明の一部を省略する。

［時刻差算出処理例］
図８は、第２実施形態における時刻差算出部１０９による時刻差算出処理の処理手順の一例を示すフローチャートである。なお、図８に示す例は、図６の一部を変形したものであり、図６と共通する部分については、同一の符号を付してその説明の一部を省略する。具体的には、図８では、図６に示すステップＳ５０３、Ｓ５０４の代わりに、ステップＳ５１１の処理を行う点が異なる。

ステップＳ５１１において、時刻差算出部１０９は、ステップＳ５０２で選択された相対位置の点（対応点群に含まれる点）の個数を判定する。そして、ステップＳ５０２で選択された相対位置の点の個数が１である場合には、ステップＳ５０５に進む。一方、ステップＳ５０２で選択された相対位置の点の個数が２以上である場合には、ステップＳ５０６に進む。

例えば、図５に示す例では、絶対位置の対象点ＧＴｉから半径Ｒ１内に存在する相対位置の点は、点ＶＴｉ１乃至ＶＴｉ５であるため、ステップＳ５０２で選択された相対位置の点の個数は５となる。この場合には、他の絶対位置を対象点として、次のループを繰り返す。

ステップＳ５０５において、時刻差算出部１０９は、ステップＳ５０２で選択された相対位置の点の通過時刻と、絶対位置の点の測位時刻との差を求める。すなわち、図８に示す例では、ステップＳ５０２で選択された相対位置の点の個数が１である場合にのみ、ステップＳ５０５の処理が実行されるため、その相対位置の通過時刻が時刻差の算出対象となる。

このように、ステップＳ５０２で選択された相対位置の点の個数が２以上である場合には、その相対位置の通過時刻を時刻差の算出に用いない。なお、相対位置の点の個数が２以上と判定されることを少なくするため、ステップＳ５０２で用いられる所定範囲の値を小さく設定するようにしてもよい。

なお、図８に示す例では、ステップＳ５０２で選択された相対位置の点の個数が１であるか否かに基づいて、ステップＳ５０５の処理を実行するか否かを判定する例を示したが、他の判定基準を用いてもよい。例えば、ステップＳ５０２で選択された相対位置の点の個数が所定の数、例えば２以下であるか否かに基づいて、ステップＳ５０５の処理を実行するか否かを判定してもよい。

［第２実施形態の作用効果］
第２実施形態に係る位置推定方法は、時刻差算出ステップ（Ｓ５０１、Ｓ５０２、Ｓ５０５乃至Ｓ５０７、Ｓ５１１）は、ステップＳ５０２で選択された通過位置の数が閾値を基準として大きい場合には、その選択された通過位置を時刻差の算出に用いない。

このような位置推定方法によれば、曖昧性を排除することで時刻差の推定精度を向上させることができる。

［第３実施形態］
第１実施形態では、図４に示す照合開始判定処理において、角速度センサ１０１により計測されたヨーレートの時間積分が閾値以上であるか否かに基づいて軌跡照合処理を実行するか否かを判定する例を示した。この例では、車両がカーブを曲がり始める前の位置から、そのカーブを所定角以上曲がった位置までの区間の軌跡を用いて軌跡照合処理を実行することができる。ただし、車両がカーブを曲がり始める前から、そのカーブを曲がり終わった後までの区間の軌跡を用いることにより軌跡照合処理の精度を高めることができると想定される。そこで、第３実施形態では、軌跡照合処理の精度を高めるため、さらに特徴的な軌跡を抽出する例を示す。なお、第３実施形態は、第１実施形態の一部を変形した例であり、第１実施形態と共通する部分については、図示及びその説明の一部を省略する。

［照合開始判定処理例］
図９は、第３実施形態における軌跡照合部１０６による照合開始判定処理の処理手順の一例を示すフローチャートである。なお、図９に示す例は、図４の一部を変形したものであり、図４と共通する部分については、同一の符号を付してその説明の一部を省略する。具体的には、図９では、図４に示すステップＳ２０５の処理後にステップＳ２１１を追加した点が異なる。なお、図９では、車両２０がカーブを曲がり始める前の位置を始点とし、車両２０がカーブを曲がり終わった後の位置を終点とする走行軌跡を用いて軌跡照合処理を実行する場合の例を示す。

ステップＳ２０５において、積分値が閾値以上、例えばπ／２（ｒａｄ）以上である場合には、ステップＳ２１１に進む。

ステップＳ２１１において、軌跡照合部１０６は、角速度センサ１０１により計測されたヨーレートの判定を行う。すなわち、軌跡照合部１０６は、現在のヨーレートの値に基づいて条件分岐を行う。そして、ヨーレートが閾値未満、例えば０．１（ｒａｄ／ｓ）未満の場合には、ステップＳ２０６に進む。一方、ヨーレートが閾値以上の場合には、何もせず照合開始判定処理を終了する。

このように、ステップＳ２０２、Ｓ２０３の処理によりカーブに入る直前の直線の位置が検出され、ステップＳ２０５、Ｓ２１１の処理によりカーブが終わってから直線になった位置を検出することができる。これにより、車両がカーブを曲がり始める前からそのカーブを曲がり終わった後までの区間を適切に検出することができるため、その区間における軌跡を用いた軌跡照合処理を行うことができる。このため、軌跡照合処理の精度を向上させることができる。

［第３実施形態の作用効果］
第３実施形態に係る位置推定方法は、軌跡照合ステップ（ステップＳ２０６）では、車両２０がカーブを曲がり始める前の位置を始点とし、車両２０がカーブを曲がり終わった後の位置を終点とする第１走行軌跡及び第２走行軌跡を用いてマッチングを実行する。

このような位置推定方法によれば、幾何学的にマッチングしやすい特徴的な走行軌跡を用いることができるため、マッチングの精度を向上させることができ、時刻差推定精度を向上させることができる。

また、第３実施形態に係る位置推定方法では、軌跡照合ステップ（ステップＳ２０６）では、車両２０のヨーレートが略ゼロとなる位置を車両２０がカーブを曲がり始める前の位置とし、そのヨーレートが略ゼロとなった後に車両２０のヨー角変化の累積が所定値を基準として大きくなり、かつ、車両２０のヨーレートが再度略ゼロとなる位置を車両２０がカーブを曲がり終わった後の位置とする。

このような位置推定方法によれば、車両２０のヨーレートを考慮したカーブ区間の抽出を行うことができる。このように抽出された軌跡は、所定量以上の角度変化を含みつつ、その両端が直線となる走行軌跡となるので、マッチング精度を向上させることができる。

なお、第１乃至第３実施形態で示した各処理は、各処理手順をコンピュータに実行させるためのプログラムに基づいて実行されるものである。このため、第１乃至第３実施形態は、それらの各処理を実行する機能を実現するプログラム、そのプログラムを記憶する記録媒体の実施形態としても把握することができる。例えば、そのプログラムについては、車両に新機能を追加する際にアップデートにより車両の記憶装置に記憶させることができる。このアップデートは、例えば、車両の定期点検時等に行うことができる。また、ワイヤレス通信によりそのプログラムをアップデートするようにしてもよい。

以上、本発明の実施形態について説明したが、上記実施形態は本発明の適用例の一部を示したに過ぎず、本発明の技術的範囲を上記実施形態の具体的構成に限定する趣旨ではない。

１０ コントローラ
１１ 測位装置
２０ 車両
１００ 位置推定システム
１０１ 角速度センサ
１０２ 車速センサ
１０３ 相対位置推定部
１０４ ＧＮＳＳアンテナ
１０５ 絶対位置推定部
１０６ 軌跡照合部
１０７ 第１走行軌跡記憶部
１０８ 第２走行軌跡記憶部
１０９ 時刻差算出部
１１０ 位置推定部

Claims (12)

1. 車両情報に基づいて取得された車両の通過位置と当該通過位置の前記車両の通過時刻とを関連付けて記憶する第１記憶部と、測位装置によって測位された前記車両の絶対位置と当該絶対位置の測位時刻とを関連付けて記憶する第２記憶部とを備える車両の位置を推定する位置推定方法であって、
   複数の前記通過位置からなる第１走行軌跡と、複数の前記絶対位置からなる第２走行軌跡とを比較して、前記第１走行軌跡における前記通過位置と前記第２走行軌跡における前記絶対位置との対応を特定し、対応する前記通過位置の通過時刻と前記絶対位置の測位時刻との時刻差を算出する時刻差算出ステップと、
   前記絶対位置と前記車両情報と前記算出された時刻差とに基づいて前記車両の位置を推定する位置推定ステップと、を含み、
   前記位置推定ステップは、前記絶対位置の測位時刻を前記算出された時刻差で補正し、当該測位時刻が補正された絶対位置と、直前に推定された前記車両の位置と、前記車両情報とをカルマンフィルタを用いて合成して前記車両の位置を推定する、
   位置推定方法。
2. 請求項１に記載の位置推定方法であって、
   前記時刻差算出ステップは、前記第２走行軌跡における前記絶対位置の測位時刻と、前記第１走行軌跡における当該絶対位置に対応する通過位置の通過時刻との差分値に基づいて前記時刻差を算出する、
   位置推定方法。
3. 請求項２に記載の位置推定方法であって、
   前記時刻差算出ステップは、前記第２走行軌跡における各絶対位置について、当該絶対位置から所定範囲に存在する前記第１走行軌跡における通過位置を、当該絶対位置に対応する通過位置として選択し、当該絶対位置の測位時刻と当該選択された通過位置の通過時刻との差分値を算出し、当該算出された各差分値に基づいて前記時刻差を算出する、
   位置推定方法。
4. 請求項３に記載の位置推定方法であって、
   前記時刻差算出ステップは、前記選択された通過位置の通過時刻のうち最も古い時刻と最も新しい時刻との差分値が閾値を基準として大きい場合には、当該選択された通過位置を前記時刻差の算出に用いない、
   位置推定方法。
5. 請求項３に記載の位置推定方法であって、
   前記時刻差算出ステップは、前記選択された通過位置の数が閾値を基準として大きい場合には、当該選択された通過位置を前記時刻差の算出に用いない、
   位置推定方法。
6. 請求項１乃至５の何れかに記載の位置推定方法であって、
   前記第２走行軌跡における各絶対位置について、前記第２走行軌跡における前記絶対位置と前記第１走行軌跡における当該絶対位置に対応する通過位置との対応をとり、当該絶対位置及び当該通過位置間の距離を最小化する処理を行うことにより前記第１走行軌跡と前記第２走行軌跡とをマッチングさせる軌跡照合ステップをさらに含み、
   前記時刻差算出ステップは、前記マッチング後の前記第１走行軌跡と前記第２走行軌跡とを比較して前記時刻差を算出する、
   位置推定方法。
7. 請求項６に記載の位置推定方法であって、
   前記軌跡照合ステップは、前記車両がカーブを曲がり始める前の位置を始点とし、前記車両が前記カーブを所定量だけ曲がった位置を終点とする前記第１走行軌跡及び前記第２走行軌跡を用いて前記マッチングを実行する、
   位置推定方法。
8. 請求項６に記載の位置推定方法であって、
   前記軌跡照合ステップは、前記車両がカーブを曲がり始める前の位置を始点とし、前記車両が前記カーブを曲がり終わった後の位置を終点とする前記第１走行軌跡及び前記第２走行軌跡を用いて前記マッチングを実行する、
   位置推定方法。
9. 請求項７に記載の位置推定方法であって、
   前記軌跡照合ステップは、前記車両のヨーレートが略ゼロとなる位置を前記車両が前記カーブを曲がり始める前の位置とし、当該ヨーレートが略ゼロとなった後に前記車両のヨー角変化の累積が所定値を基準として大きくなった位置を前記車両が前記カーブを所定量だけ曲がった位置とする、
   位置推定方法。
10. 請求項８に記載の位置推定方法であって、
    前記軌跡照合ステップは、前記車両のヨーレートが略ゼロとなる位置を前記車両が前記カーブを曲がり始める前の位置とし、当該ヨーレートが略ゼロとなった後に前記車両のヨー角変化の累積が所定値を基準として大きくなり、かつ、前記車両のヨーレートが再度略ゼロとなる位置を前記車両が前記カーブを曲がり終わった後の位置とする、
    位置推定方法。
11. 請求項６乃至１０の何れかに記載の位置推定方法であって、
    前記時刻差算出ステップは、前記マッチングが行われる毎に前記時刻差を算出し、第１の時刻差と、当該第１の時刻差の算出後に新たに算出された第２の時刻差とを比較し、前記第１の時刻差と前記第２の時刻差との差分値が所定範囲内にある場合には、前記第１の時刻差及び前記第２の時刻差の何れか一方または双方を前記車両の位置を推定する値として用いる決定をし、一方、前記第１の時刻差と前記第２の時刻差との差分値が所定範囲内にない場合には、前記第２の時刻差を前記車両の位置を推定する値として用いる決定をする、
    位置推定方法。
12. 車両の位置を推定するコントローラを備える位置推定システムであって、
    前記コントローラは、
    車両情報に基づいて取得された車両の通過位置と当該通過位置の前記車両の通過時刻とを関連付けて記憶する第１記憶部と、
    測位装置によって測位された前記車両の絶対位置と当該絶対位置の測位時刻とを関連付けて記憶する第２記憶部と、
    複数の前記通過位置からなる第１走行軌跡と、複数の前記絶対位置からなる第２走行軌跡とを比較して、前記第１走行軌跡における前記通過位置と前記第２走行軌跡における前記絶対位置との対応を特定し、対応する前記通過位置の通過時刻と前記絶対位置の測位時刻との時刻差を算出する時刻差算出部と、
    前記絶対位置と前記車両情報と前記算出された時刻差とに基づいて前記車両の位置を推定する位置推定部と、を備え、
    前記位置推定部は、前記絶対位置の測位時刻を前記算出された時刻差で補正し、当該測位時刻が補正された絶対位置と、直前に推定された前記車両の位置と、前記車両情報とをカルマンフィルタを用いて合成して前記車両の位置を推定する、
    位置推定システム。

Images