JP2021081272A - 位置推定装置および位置推定用コンピュータプログラム - Google Patents

Abstract

【課題】高い精度で移動物体の位置を推定できる位置推定装置を提供する。【解決手段】位置推定装置は、第１及び第２の周期で設定される第１及び第２情報取得時刻ごとに、移動物体の位置を表す第１及び第２位置関係情報を取得する第１及び第２位置情報取得部と、第１及び第２位置関係情報に基づいて、第１及び第２情報取得時刻における移動物体の第１及び第２推定位置を算出する第１及び第２位置算出部と、直前の第１情報取得時刻の第１推定位置が求められた場合、現時刻の移動物体の位置を、第１推定位置と、第１情報取得時刻と現時刻との間における移動物体の移動量及び移動方向とに基づいて推定し、直前の第１情報取得時刻の第１推定位置が求められない場合、現時刻の移動物体の位置を、第２推定位置と、直前の第２情報取得時刻と現時刻との間における移動物体の移動量及び移動方向とに基づいて推定する位置推定部と、を有する。【選択図】図４

Description

本発明は、位置推定装置および位置推定用コンピュータプログラムに関する。

車両の運転支援又は自動運転制御に利用するために、車両の位置を推定することが研究されている。

例えば、車両に搭載されたカメラを用いて車両の前方の画像を取得して画像内の車線区画線を認識し、この車線区画線と地図とを比較することにより車両の位置を推定することが提案されている（例えば、特許文献１参照）。

ここで、カメラを用いて取得された画像に基づいて求められる車両の位置は、画像が取得された時刻における車両の位置である。

特開２０１８−７７１６２号公報

推定される車両の位置には少なからず誤差が生じる場合があるが、より高い精度で車両の位置を推定できることが好ましい。

そこで、本発明は、高い精度で移動物体の位置を求められる位置推定装置を提供することを目的とする。

一の実施形態によれば、位置推定装置を提供される。この位置推定装置は、第１の周期で設定される第１情報取得時刻ごとに、移動物体の位置を表す第１位置関係情報を取得する第１位置情報取得部と、第１位置関係情報に基づいて、第１情報取得時刻における移動物体の第１推定位置を算出する第１位置算出部と、第２の周期で設定される第２情報取得時刻ごとに、移動物体の位置を表す第２位置関係情報を取得する第２位置情報取得部と、第２位置関係情報に基づいて、第２情報取得時刻における移動物体の第２推定位置を算出する第２位置算出部と、直前の第１情報取得時刻に取得された第１位置関係情報に基づいて第１推定位置が求められた場合、移動物体の挙動を表す情報に基づいて、当該第１情報取得時刻と、現時刻との間における移動物体の第１移動量及び第１移動方向を算出し、現時刻における移動物体の位置を、第１推定位置と、移動物体の第１移動量及び第１移動方向とに基づいて推定し、直前の第１情報取得時刻に取得された第１位置関係情報に基づいて第１推定位置が求められない場合、移動物体の挙動を表す情報に基づいて、直前の第２情報取得時刻と、現時刻との間における移動物体の第２移動量及び第２移動方向を算出し、現時刻における移動物体の位置を、第２推定位置と、移動物体の第２移動量及び第２移動方向とに基づいて推定する位置推定部と、を有する。

この位置推定装置において、位置推定部が移動物体の位置を推定する間隔は、第１の周期及び第２の周期よりも短いことが好ましい。

また、この位置推定装置において、第１推定位置の精度は、第２推定位置の精度よりも高いことが好ましい。

また、他の実施形態によれば、位置推定用コンピュータプログラムを提供される。この位置推定用コンピュータプログラムは、第１の周期で設定される第１情報取得時刻ごとに、移動物体の位置を表す第１位置関係情報を取得し、第１位置関係情報に基づいて、第１情報取得時刻における移動物体の第１推定位置を算出し、第２の周期で設定される第２情報取得時刻ごとに、移動物体の位置を表す第２位置関係情報を取得し、第２位置関係情報に基づいて、第２情報取得時刻における移動物体の第２推定位置を算出し、直前の第１情報取得時刻に取得された第１位置関係情報に基づいて第１推定位置が求められた場合、移動物体の挙動を表す情報に基づいて、当該第１情報取得時刻と、現時刻との間における移動物体の第１移動量及び第１移動方向を算出し、現時刻における移動物体の位置を、第１推定位置と、移動物体の第１移動量及び第１移動方向とに基づいて推定し、直前の第１情報取得時刻に取得された第１位置関係情報に基づいて第１推定位置が求められない場合、移動物体の挙動を表す情報に基づいて、直前の第２情報取得時刻と、現時刻との間における移動物体の第２移動量及び第２移動方向を算出し、現時刻における移動物体の位置を、第２推定位置と、移動物体の第２移動量及び第２移動方向とに基づいて推定する、ことをプロセッサに実行させる。

本発明に係る位置推定装置は、高い精度で移動物体の位置を求められるという効果を奏する。

位置推定装置が実装される車両制御システムの概略構成図である。 位置推定装置の一つの実施形態である電子制御装置のハードウェア構成図であり、 メモリが含むデータバッファを示す図である。 位置推定処理を含む車両制御処理に関する、電子制御装置のプロセッサの機能ブロック図である。 プロセッサの処理を説明するタイミング図である。 位置算出部の動作を説明するフローチャートである。 位置推定部の動作を説明するフローチャート（その１）である。 位置推定部の動作を説明するフローチャート（その２）である。 位置推定部の動作を説明するフローチャート（その３）である。

以下、図を参照しつつ、位置推定装置について説明する。この位置推定装置は、第１の周期で設定される第１情報取得時刻ごとに、移動物体の位置を表す第１位置関係情報を取得する第１位置情報取得部を有する。第１位置情報取得部は、例えば車両の周囲を撮影するカメラであり、第１位置関係情報は例えば車両の周囲が表された画像である。位置推定装置は、第１位置関係情報に基づいて、第１情報取得時刻における移動物体の第１推定位置を算出する第１位置算出部を有する。また、位置推定装置は、第２の周期で設定される第２情報取得時刻ごとに、移動物体の位置を表す第２位置関係情報を取得する第２位置情報取得部を有する。第２位置情報取得部は、第１位置情報取得部とは異なるハードウェアを有することが好ましい。位置推定装置は、第２位置関係情報に基づいて、第２情報取得時刻における移動物体の第２推定位置を算出する第２位置算出部を有する。位置推定装置は、直前の第１情報取得時刻に取得された第１位置関係情報に基づいて第１推定位置が求められた場合、移動物体の挙動を表す情報に基づいて、この第１情報取得時刻と、現時刻との間における移動物体の第１移動量及び第１移動方向を算出し、現時刻における移動物体の位置を、第１推定位置と、移動物体の第１移動量及び第１移動方向とに基づいて推定位置推定部を有する。一方、この位置推定部は、直前の第１情報取得時刻に取得された第１位置関係情報に基づいて第１推定位置が求められない場合、移動物体の挙動を表す情報に基づいて、直前の第２情報取得時刻と、現時刻との間における移動物体の第２移動量及び第２移動方向を算出し、現時刻における移動物体の位置を、第２推定位置と、移動物体の第２移動量及び第２移動方向とに基づいて推定する。これにより、位置推定装置は、第１位置関係情報を用いて移動物体の位置を求められない場合でも、第２位置関係情報を用いて、高い精度で移動物体の位置を求められる。

以下では、位置推定装置を、車両制御システムに適用した例について説明する。この例では、位置推定装置は、車両に搭載されたカメラにより取得された画像などに基づいて、位置推定処理を実行することで、車両の位置を推定し、その推定された位置に基づいて、車両の運転計画を生成して、車両の運転制御に利用する。但し、本発明の技術範囲はそれらの実施形態に限定されず、特許請求の範囲に記載された発明とその均等物に及ぶものである。

図１は、位置推定装置が実装される車両制御システムの概略構成図である。また、図２は、位置推定装置の一つの実施形態である電子制御装置のハードウェア構成図である。

本実施形態では、車両１０に搭載され、且つ、車両１０を制御する車両制御システム１は、車両の前方の画像を取得する第１カメラ２と、第１カメラ２とは別に配置され、車両１０の前方の画像を取得する第２カメラ３を有する。また、車両制御システム１は、車両１０の前後左右の画像を取得する第３カメラ４ａ〜４ｄと、第３カメラ４ａ〜４ｄが撮影した画像を処理するカメラ画像処理装置４ｅを有する。また、車両制御システム１は、車両１０の前後左右に配置されるLiDARセンサ５ａ〜５ｄと、車両１０の前遠方の画像を取得する第４カメラ６とを有する。また、車両制御システム１は、測位情報受信機７と、測位情報受信機７が出力する測位情報に基づいて地図情報を生成する地図情報記憶装置８と、位置推定装置の一例である電子制御装置（ＥＣＵ）９とを有する。

第１カメラ２と、第２カメラ３と、カメラ画像処理装置４ｅと、LiDARセンサ５ａ〜５ｄと、第４カメラ６と、地図情報記憶装置８と、ＥＣＵ９とは、コントローラエリアネットワークといった規格に準拠した車内ネットワーク１１を介して通信可能に接続される。

第１カメラ２は、位置情報取得部の一例である。第１カメラ２は、所定の周期（例えば１００ｍ秒）で設定される第１情報取得時刻において、位置関係情報の一例である、車両１０の前方の所定の領域が表された第１画像を取得する。取得された第１画像には、車両１０の前方の所定の領域内に含まれる路面上の車線区画線などの地物が表わされる。第１画像は、車両１０と車両１０の周辺の地物との相対的な位置情報を有する。第１カメラ２により得られた第１画像は、カラー画像であってもよく、又は、グレー画像であってもよい。第１カメラ２は、撮像部の一例であり、CCDあるいはC-MOSなど、可視光に感度を有する光電変換素子のアレイで構成された２次元検出器と、その２次元検出器上に撮影対象となる領域の像を結像する結像光学系を有する。そして第１カメラ２は、車両１０の前方を向くように、例えば、車両１０の車室内に取り付けられる。

第１カメラ２は、第１画像を取得する度に、第１画像及び第１画像を取得した第１情報取得時刻を、車内ネットワーク１１を介してＥＣＵ９へ出力する。第１画像は、ＥＣＵ９において、位置関係情報として車両の位置を決定する処理に使用されるのと共に、車両１０の周囲の他の物体を検出する処理に使用される。

第２カメラ３は、位置情報取得部の他の一例である。第２カメラ３は、所定の周期（例えば１５０ｍ秒）で設定される第２情報取得時刻において、位置関係情報の一例である、車両の前方の所定の領域が表された第２画像を取得する。この第２画像は、車両１０と車両１０の周辺の地物との相対的な位置情報を有する。第２カメラ３は、第１カメラ２とは異なる位置に、第１カメラ２とは異なる方向に向かって設置される。例えば、第２カメラ３は、車両１０の前方を向くように、第１カメラ２と同様に車両１０の車室内に取り付けられる。取得された画像には、車両１０の前方の所定の領域内に含まれる路面上の車線区画線などの地物が表わされる。第２カメラ３により得られた画像は、カラー画像であってもよく、又は、グレー画像であってもよい。第２カメラ３は、撮像部の一例であり、CCDあるいはC-MOSなど、可視光に感度を有する光電変換素子のアレイで構成された２次元検出器と、その２次元検出器上に撮影対象となる領域の像を結像する結像光学系を有する。第２カメラ３は、第２画像を取得する度に、第２画像及び第２画像を取得した第２情報取得時刻を、車内ネットワーク１１を介してＥＣＵ９へ出力する。第２画像は、ＥＣＵ９において、位置関係情報として車両の位置を決定する処理に使用される。

第３カメラ４ａ〜４ｄは、所定の周期（例えば１８０ｍ秒）で設定される第３情報取得時刻において、車両１０の前方、左側方、後方、右側方の所定の領域が表された画像を取得して、カメラ画像処理御装置４ｅへ出力する。これらの画像は、車両１０と車両１０の周辺の地物との相対的な位置情報を有する。第３カメラ４ａ〜４ｄは、撮像部の一例であり、ＣＣＤあるいはＣ−ＭＯＳなど、可視光に感度を有する光電変換素子のアレイで構成された２次元検出器と、その２次元検出器上に撮影対象となる領域の像を結像する結像光学系を有する。そして第３カメラ４ａ〜４ｄは、車両１０の前方、左側方、後方、右側方を向くように、例えば、車両１０の前後左右に取り付けられる。第３カメラ４ａ〜４ｄにより得られた画像は、カラー画像であってもよく、又は、グレー画像であってもよい。

カメラ画像処理装置４ｅは、プロセッサ（図示せず）を有する。カメラ画像処理装置４ｅのプロセッサは、第３カメラ４ａ〜４ｄが生成した画像を合成して、車両１０の周囲が写った合成画像を生成する。カメラ画像処理装置４ｅのプロセッサは、合成画像を生成する度に、合成画像及び合成画像を取得した第３情報取得時刻を、車内ネットワーク１１を介してＥＣＵ９へ出力する。車両１０の周囲が写った合成画像は、ＥＣＵ９において、車両１０の周囲の他の物体を検出する処理に使用される。

LiDARセンサ５ａ〜５ｄのそれぞれは、所定の周期で設定される第４情報取得時刻において、車両１０の前方、左側方、後方、右側方に向けてパルス状のレーザを同期して発射して、反射物により反射された反射波を受信して、反射波の情報及びレーザを放射した方位情報を含む第４情報を、レーザを発射した第４情報取得時刻と共に、車内ネットワーク１１を介してＥＣＵ９へ出力する。反射波は、車両１０と車両１０の周辺の地物との相対的な位置情報を有する。

第４カメラ６は、所定の周期で設定される第５情報取得時刻において、第１カメラ２及び第２カメラ３よりも車両１０の前遠方の所定の領域が表された第３画像を取得する。第３画像には、車両１０の前遠方の所定の領域内に含まれる路面上の車線区画線などの地物が表わされる。第３画像は、車両１０と車両１０の周辺の地物との相対的な位置関係情報を有する。第４カメラ６により得られた第３画像は、カラー画像であってもよく、又は、グレー画像であってもよい。第４カメラ６は、撮像部の一例であり、CCDあるいはC-MOSなど、可視光に感度を有する光電変換素子のアレイで構成された２次元検出器と、その２次元検出器上に撮影対象となる領域の像を結像する結像光学系を有する。そして第４カメラ６は、車両１０の前遠方を向くように、例えば、第１カメラ２と同様に車両１０の車室内に取り付けられる。

第４カメラ６は、第３画像を取得する度に、位置関係情報である第３画像及び第３画像を取得した第５情報取得時刻を、車内ネットワーク１１を介してＥＣＵ９へ出力する。第３画像は、ＥＣＵ９において、車両１０の周囲の物体を検出する処理に使用される。

測位情報受信機７は、車両１０の現在位置を表す測位情報を出力する。例えば、測位情報受信機７は、ＧＰＳ受信機とすることができる。測位情報受信機７は、ＧＰＳ電波を受信する測位情報受信部７ａと、測位情報受信部７ａが受信したＧＰＳ電波に基づいて、車両１０の現在位置を表す測位情報を出力するプロセッサ７ｂとを有する。測位情報受信部７ａは、位置情報取得部の他の一例である。プロセッサ７ｂは、測位情報受信部７ａが所定の受信周期で測位情報を取得する度に、位置関係情報である測位情報及び測位情報を取得した第６情報取得時刻を、地図情報記憶装置８へ出力する。

地図情報記憶装置８は、プロセッサ（図示せず）と、磁気ディスクドライブ又は不揮発性の半導体メモリなどの記憶装置（図示せず）とを有しており、この記憶装置は、車両１０の現在位置を含む広い範囲（例えば数ｋｍ四方の範囲）の広域地図情報を記憶する。この広域地図情報は、道路上の車線区画線などの地物、構造物の種類及び位置を表す情報を含む高精度地図情報であることが好ましい。なお、道路上の地物、構造物の位置は、例えば、実空間における所定の基準位置を原点とする世界座標系で表される。地図情報記憶装置８のプロセッサは、車両１０の現在位置に応じて、無線通信装置を介した無線通信により、基地局を介して外部のサーバから広域地図情報を受信して記憶装置に記憶する。地図情報記憶装置８のプロセッサは、測位情報受信機７から測位情報を入力する度に、記憶部に記憶している広域地図情報を参照して、測位情報により表される現在位置を含む狭い領域（例えば、数十〜数百ｍ四方の範囲）の地図情報及び第６情報取得時刻を、車内ネットワーク１１を介してＥＣＵ９へ出力する。

ＥＣＵ９は、車両１０を制御する。本実施形態では、ＥＣＵ９は、第１カメラ２などが出力する位置関係情報に基づいて車両１０の位置を推定して、車両１０を自動運転するように車両１０を制御する。そのために、ＥＣＵ９は、通信インターフェース２１と、メモリ２２と、プロセッサ２３とを有する。

通信インターフェース（Ｉ／Ｆ）２１は、通信部の一例であり、ＥＣＵ９を車内ネットワーク１１に接続するためのインターフェース回路を有する。すなわち、通信インターフェース２１は、車内ネットワーク１１を介して、第１カメラ２及び地図情報記憶装置８などと接続される。そして通信インターフェース２１は、例えば、第１カメラ２から第１画像及び第１情報取得時刻を受信する度に、受信した第１画像及び第１情報取得時刻をプロセッサ２３へわたす。また通信インターフェース２１は、地図情報記憶装置８から測位情報、第６情報取得時刻及び地図情報を受信する度に、受信した測位情報、第６情報取得時刻及び地図情報をプロセッサ２３へわたす。また通信インターフェース２１は、図示しない車両速度センサ及びヨーレートセンサから受信した車両速度及びヨーレートを、プロセッサ２３へわたす。

メモリ２２は、記憶部の一例であり、例えば、揮発性の半導体メモリ及び不揮発性の半導体メモリを有する。そしてメモリ２２は、ＥＣＵ９のプロセッサ２３により実行される位置推定処理において使用される各種のデータ、各カメラ及び各LiDARセンサの光軸方向及び取り付け位置などの設置位置情報、結像光学系の焦点距離及び画角といった内部パラメータなどを記憶する。また、メモリ２２は、第１カメラ２などから受信した第１画像などの位置関係情報及び情報取得時刻、地図情報記憶装置８から受信した測位情報、第６情報取得時刻及び地図情報などを記憶する。

また、メモリ２２は、図３に示すように、データバッファ３００を含む。データバッファ３００は、リング型のデータバッファであり、図３に示す例では、１０個のバッファを有する。各バッファには、位置決定時刻と、車両速度と、ヨーレートと、前回の位置決定時刻に求められた車両１０の位置が初期位置として記憶される。データバッファ３００には、バッファ番号１から順番にデータが記憶されていき、１０個目のデータがバッファ番号１０に記憶された後、１１個目のデータはバッファ番号１に上書きされる。プロセッサ２３は、入力した車両速度及びヨーレートに基づいて、前回の位置決定時刻と今回の位置決定時刻との間の車両速度及びヨーレートの平均値を求めて、この平均値を、メモリ２２が含むデータバッファに今回の位置決定時刻と関連づけて記憶する。

プロセッサ２３は、制御部の一例であり、１個または複数個のＣＰＵ(Central Processing Unit)及びその周辺回路を有する。プロセッサ２３は、論理演算ユニット、数値演算ユニットあるいはグラフィック処理ユニットといった他の演算回路をさらに有していてもよい。プロセッサ２３が複数個のＣＰＵを有する場合、ＣＰＵごとにメモリを有していてもよい。プロセッサ２３は、位置関係情報に基づいて、この位置関係情報が取得された情報取得時刻における車両１０の位置を推定する位置推定処理を実行する。また、プロセッサ２３は、所定の周期（例えば、３２ｍ秒）で設定される位置決定時刻において、車両１０の位置を推定する位置推定処理を実行する。この位置決定時刻の周期は、通常、第１〜第５情報取得時刻の周期よりも短い。プロセッサ２３は、前回の位置決定時刻における車両１０の位置と、情報取得時刻における車両１０の位置と、車両速度及びヨーレートなどの車両１０の挙動を表す情報に基づいて、今回の位置決定時刻における車両１０の位置を求める。さらに、プロセッサ２３は、推定された車両１０の位置と、車両１０の周囲の他の物体との相対的な位置関係に基づいて、車両１０を自動運転するよう、車両１０を制御する。

図４は、位置推定処理を含む車両制御処理に関する、ＥＣＵ９のプロセッサ２３の機能ブロック図である。プロセッサ２３は、第１〜２横位置算出部３１ａ、３１ｂと、第１〜２縦位置算出部３２ａ、３２ｂと、位置推定部３３と、検出部３４と、運転計画部３５と、車両制御部３６とを有する。プロセッサ２３が有するこれらの各部は、例えば、プロセッサ２３上で動作するコンピュータプログラムにより実現される機能モジュールである。あるいは、プロセッサ２３が有するこれらの各部は、プロセッサ２３に設けられる、専用の演算回路であってもよい。また、プロセッサ２３が有するこれらの各部のうち、第１〜２横位置算出部３１ａ、３１ｂ、第１〜２縦位置算出部３２ａ、３２ｂ及び位置推定部３３が、位置推定処理を実行する。

プロセッサ２３の第１〜２横位置算出部３１ａ、３１ｂは、入力した位置関係情報などの情報に基づいて、この位置関係情報が取得された情報取得時刻における車両１０の位置を推定し、この車両１０の位置と、車両１０の参照位置との差における車両１０の進行方向と直交する方向の成分である横ずれ量を求めて、位置推定部３３へ通知する。参照位置は、情報取得時刻の直前の位置決定時刻における車両の位置と、この位置決定時刻と情報取得時刻との間の車両１０の移動量及び移動方向とに基づいて、情報取得時刻における車両１０の位置が推定されたものである。

プロセッサ２３の第１〜２縦位置算出部３２ａ、３２ｂは、入力した位置関係情報に基づいて、この位置関係情報が取得された情報取得時刻における車両１０の位置を推定し、この車両１０の位置と、車両１０の参照位置との差における車両１０の進行方向の成分である縦ずれ量を求めて、位置推定部３３へ通知する。

図５は、プロセッサ２３の処理を説明するタイミング図である。プロセッサ２３の位置推定部３３は、所定の周期（図５に示す例では、３２ｍ秒）で設定される位置決定時刻ごとに、第１〜２横位置算出部３１ａ、３１ｂの中から選択された一の横位置算出部から通知された横ずれ量、及び、第１〜２縦位置算出部３２ａ、３２ｂの中から選択された一の縦位置算出部から通知された縦ずれ量と、位置決定時刻における車両１０の位置と、車両１０の挙動を表す情報である車両速度及びヨーレートに基づいて、今回の位置決定時刻における車両１０の位置を推定する。そして、位置推定部３３は、推定された車両１０の位置を、運転計画部３５及び車両制御部３６へ通知する。位置決定時刻は、現時刻の一例である。

第１横位置算出部３１ａは、第１カメラ２が第１画像を取得する第１情報取得時刻の周期（図５に示す例では、１００ｍ秒）で、横ずれ量を求めて位置推定部３３へ通知する。第１横位置算出部３１ａが横ずれ量を求める周期は、位置推定部３３が車両１０の位置を求める位置決定時刻の周期（図５に示す例では、３２ｍ秒）よりも長い。同様に、第２横位置算出部３１ｂは、第２カメラ３が画像を取得する第２情報取得時刻の周期（図５に示す例では、１５０ｍ秒）で、横ずれ量を求めて位置推定部３３へ通知する。第２横位置算出部３１ｂが横ずれ量を求める周期も、位置推定部３３が車両１０の位置を求める位置決定時刻の周期（図５に示す例では、３２ｍ秒）よりも長い。

次に、第１横位置算出部３１ａが車両１０の位置の横ずれ量を算出する処理を、図６を参照して、以下に説明する。図６に示す処理は、第１情報取得時刻に第１カメラ２が第１画像を取得するごとに、第１横位置算出部３１ａが実行する。

まず、第１横位置算出部３１ａは、位置関係情報の一例である第１画像と、この第１画像を取得した第１情報取得時刻ｓ１と、測位情報により示される位置を含む所定の領域の地図情報を取得する（ステップＳ６０１）。

次に、第１横位置算出部３１ａは、位置関係情報の一例である第１画像を取得した第１情報取得時刻ｓ１における車両１０の位置を示す参照位置を算出する（ステップＳ６０３）。まず、第１横位置算出部３１ａは、メモリ２２が含むデータバッファ（図３参照）を参照して、第１情報取得時刻ｓ１よりも前の時刻であり且つ第１情報取得時刻ｓ１と最も近い位置決定時刻ｓ２を選択する。第１横位置算出部３１ａが車両１０の位置の横ずれ量を求める周期（図５に示す例では、１００ｍ秒）は、位置決定時刻の周期（図５に示す例では、３２ｍ秒）よりも長いので、第１情報取得時刻ｓ１は、データバッファに記憶されている最新の位置決定時刻よりも過去の時刻となる。この観点から、データバッファは、各位置算出部が車両１０の位置のずれ量を算出する周期の２倍以上の間の位置決定時刻のデータを記憶することが好ましい。そして、第１横位置算出部３１ａは、第１情報取得時刻ｓ１と位置決定時刻ｓ２とが一致していない場合、データバッファを参照して、第１情報取得時刻ｓ１と位置決定時刻ｓ２との間における車両速度及びヨーレートを取得して、第１情報取得時刻ｓ１と位置決定時刻ｓ２との間における車両の移動量及び移動方向を求める。第１横位置算出部３１ａは、車両１０の車両速度を時間積分して、第１情報取得時刻ｓ１と位置決定時刻ｓ２との間の車両１０の移動量を求め、車両１０のヨーレートを時間積分して、第１情報取得時刻ｓ１と位置決定時刻ｓ２との間の車両１０の移動方向を求める。そして、第１横位置算出部３１ａは、位置決定時刻ｓ２の初期位置と、車両１０の移動量及び移動方向とに基づいて、第１情報取得時刻ｓ１における車両１０の位置ｑ１を、車両１０の参照位置として算出する。一方、情報取得時刻ｓ１と位置決定時刻ｓ２とが一致する場合には、第１横位置算出部３１ａは、位置決定時刻ｓ２の次の位置決定時刻における初期位置を、車両１０の参照位置とする。

次に、第１横位置算出部３１ａは、位置関係情報の一例である第１画像及び地図情報に基づいて、第１画像を取得した第１情報取得時刻における車両１０の位置を算出する（ステップＳ６０５）。まず、第１横位置算出部３１ａは、第１画像に表される物体の座標を、第１カメラ２の設置位置情報及び内部パラメータなどの情報を用いて視点変換処理を実行することで、第１カメラ２の位置を原点とするカメラ座標系で表す。第１画像上に表示される物体の座標は、第１画像の左上の隅の位置を原点として、右方に伸びるＸｉ軸と、下方に伸びるＹｉ軸とを有する画像座標系で表すことができる。また、カメラ座標系では、撮像面の中心を原点として、車両１０の進行方向にＺｃ軸が設定され、Ｚｃ軸と直交し、かつ、地面に平行な方向をＸｃ軸が設定され、鉛直方向にＹｃ軸が設定され、原点は、地面から第１カメラ２が設置される高さにある。そして、第１横位置算出部３１ａは、カメラ座標系で表された第１画像に表される物体の座標を、世界座標系で表す。世界座標系では、実空間における所定の基準位置を原点として、地面に平行な面内にＸｗ軸及びＺｗが設定され、鉛直方向にＹｗ軸が設定される。第１横位置算出部３１ａは、第１情報取得時刻よりも前の時刻であり且つ第１情報取得時刻と最も近い位置決定時刻における車両１０の位置と、この位置決定時刻と第１情報取得時刻との間の車両１０の移動量及び移動方向とに基づいて、第１情報取得時刻における車両１０の位置及び進行方向を推定する。第１横位置算出部３１ａは、第１情報取得時刻における車両１０の位置及び進行方向として、車両１０の参照位置を算出する際に求めた値を用いることができる。第１横位置算出部３１ａは、第１カメラ２の設置位置情報及び内部パラメータなどの情報と、推定された車両１０の位置及び移動方向とを用いて、第１情報取得時刻における第１カメラ２の仮定位置及び仮定姿勢を推定する。第１横位置算出部３１ａは、第１情報取得時刻における第１カメラ２の仮定位置及び仮定姿勢に従って、カメラ座標系から、世界座標系への変換式を求める。そのような変換式は、座標系間の回転を表す回転行列と座標系間の平行移動を表す並進ベクトルの組み合わせで表される。そして第１横位置算出部３１ａは、その変換式に従って、カメラ座標系で表された第１画像に表される物体の座標を、世界座標系で表される座標に変換する。

第１横位置算出部３１ａは、世界座標系で表される第１画像を、地図情報により表される地図上に投影して、地図に表された車両１０の周囲の地物の一例である車線区画線と、第１画像内に検出された候補点との一致度合を算出する。例えば，第１横位置算出部３１ａは、第１画像内に設定された照合領域内の候補点の数（インライア数）に基づいて、地図に表された車両１０の周囲の車線区画線と、第１画像内に検出された候補点との一致度合を算出してもよい。第１横位置算出部３１ａは、仮定位置及び仮定姿勢を所定量ずつ変化させながら、上記と同様の第１画像のカメラ座標系から世界座標系への位置変換、車線区画線との一致度合の算出の各処理を実行することで、複数の仮定位置及び仮定姿勢のそれぞれについて、地図情報に表された車両１０の周囲の車線区画線と第１画像との一致度合を算出する。第１横位置算出部３１ａは、一致度合が最大となるときの仮定位置及び仮定姿勢を特定し、その第１カメラ２の仮定位置及び仮定姿勢に基づいて、第１カメラ２が画像を取得した第１情報取得時刻における車両１０の位置及び進行方向を推定する。第１横位置算出部３１ａは、車両１０の位置と、地図上の車線区画線の位置とに基づいて、第１情報取得時刻における車両１０の位置を算出する。第１横位置算出部３１ａは、第１情報取得時刻における車両１０の位置及び参照位置を、第１縦位置算出部３２ａに通知する。また、第１横位置算出部３１ａは、第１情報取得時刻における車両１０の位置及び参照位置を、位置推定部３３へ通知してもよい。

次に、第１横位置算出部３１ａは、第１情報取得時刻における車両１０の位置を算出できたか否かを判定する（ステップＳ６０７）。例えば，第１横位置算出部３１ａは、第１画像内の車線区画線を照合する照合領域内のインライア数に基づいて、第１画像内の車線区画線を検出可能か否かを判定してもよい。第１横位置算出部３１ａは、インライア数が所定の閾値以上の場合、車両１０の位置を算出できたと判定し、インライア数が所定の閾値未満の場合、車両１０の位置を算出できないと判定してもよい。

車両１０の位置を算出できた場合（ステップＳ６０７−Ｙｅｓ）、第１横位置算出部３１ａは、車両１０の位置と参照位置との差に基づいて、車両１０の進行方向と直交する方向である横方向の成分である横ずれ量を求める（ステップＳ６０９）。第１横位置算出部３１ａは、車両１０の位置を、車両１０の進行方向と直交する方向である横方向の中心でありかつ車両１０の進行方向の中心の位置としてもよい。

次に、第１横位置算出部３１ａは、横ずれ量及び第１情報取得時刻を、位置推定部３３へ通知する（ステップＳ６１１）。

一方、車両１０の位置を算出できなかった場合（ステップＳ６０７−Ｎｏ）、第１横位置算出部３１ａは、車両１０の位置を算出できないことを示す情報を、位置推定部３３及び第１縦位置算出部３２ａへ通知する（ステップＳ６１３）。第１横位置算出部３１ａが車両１０の位置を算出できない理由として、第１画像内に十分な数の候補点を検出できないこと、または、カメラが故障していることが挙げられる。第１画像内の候補点を検出できない理由としては、例えば、逆光で第１画像が取得されたこと、カメラのレンズが汚れていること、雨又は霧などの気象条件下で第１画像が取得されたことなどが挙げられる。以上が、第１横位置算出部３１ａの説明である。

第２横位置算出部３１ｂは、第２画像及び地図情報に基づいて、第２情報取得時刻における車両１０の位置、参照位置、及び、横ずれ量を求める。例えば、第２横位置算出部３１ｂは、第１横位置算出部３１ａと同様の処理を用いて、第２画像及び地図情報に基づいて、第２情報取得時刻における車両１０の位置、参照位置、及び、横ずれ量を求めてもよい。第２横位置算出部３１ｂは、車両１０の位置、参照位置、及び、横ずれ量及び第２情報取得時刻を、位置推定部３３へ通知する。また、第２横位置算出部３１ｂは、第２情報取得時刻における車両１０の位置を算出できない場合、車両１０の位置を算出できないことを示す情報を、位置推定部３３へ通知する。

第１縦位置算出部３２ａは、第１横位置算出部３１ａから第１情報取得時刻における車両１０の位置及び参照位置が通知される。第１縦位置算出部３２ａは、第１情報取得時刻における車両１０の位置と参照位置との差に基づいて、車両１０の進行方向である縦方向の成分である縦ずれ量を求める。第１縦位置算出部３２ａは、縦ずれ量及び第１情報取得時刻を、位置推定部３３へ通知する。また、第１縦位置算出部３２ａは、第１横位置算出部３１ａから、第１情報取得時刻における車両１０の位置を算出できないことを示す情報が通知された場合、車両１０の位置を算出できないことを示す情報を、位置推定部３３へ通知する。

第２縦位置算出部３２ｂは、第２画像及び地図情報に基づいて、第２情報取得時刻における車両１０の縦ずれ量を求める。例えば、第２縦位置算出部３２ｂは、第１縦位置算出部３２ａと同様の処理を用いて、第２画像及び地図情報に基づいて、第２情報取得時刻における車両１０の縦ずれ量を求めてもよい。第２縦位置算出部３２ｂは、縦ずれ量及び第２情報取得時刻を、位置推定部３３へ通知する。また、第２縦位置算出部３２ｂは、第２横位置算出部３１ｂから、第２情報取得時刻における車両１０の位置を算出できないことを示す情報が通知された場合、車両１０の位置を算出できないことを示す情報を、位置推定部３３へ通知する。

次に、位置推定部３３が車両の位置を推定する処理を、図７〜図９を参照して、以下に説明する。

まず、位置推定部３３は、内部クロックに基づいて、前回の位置決定時刻を所定の周期だけ更新して今回の位置決定時刻ｔ０とする。また、位置推定部３３は、メモリ２２に含まれるデータバッファを参照して、前回の位置決定時刻に推定された車両１０の位置である今回の位置決定時刻ｔ０の初期位置ｐ０を取得する（ステップＳ７０１）。

次に、位置推定部３３は、車両１０の横方向の位置を算出する横位置算出部を選択する（ステップＳ７０３）。

以下、位置推定部３３が、横位置算出部を選択する処理を、図９を参照しながら、以下に説明する。

メモリ２２は、第１〜第２横位置算出部を選択する順番が記録された優先順位リスト（図示せず）を記憶している。位置推定部３３は、メモリ２２が記憶する優先順位リストを参照して、順位に従って、選択可能な横位置算出部の中で最も順位の高い横位置算出部を選択する（ステップＳ９０１）。横位置算出部の優先順位は、例えば、車両１０の位置を推定する精度の高い順番に決定される。本実施形態では、横位置算出部の優先順位は、第１横位置算出部３１ａから第２横位置算出部３１ｂの順番で決定されている。

次に、位置推定部３３は、選択された横位置算出部が、直前の情報取得時刻に取得された位置関係情報に基づいて、車両の位置を求められたか否かを判定する（ステップＳ９０３）。位置推定部３３は、選択されている横位置算出部から車両の位置を算出できないことを示す情報が通知されていない場合、車両の位置を算出できたと判定して（ステップＳ９０３−Ｙｅｓ）、選択された横位置算出部から通知された横ずれ量を用いる（ステップＳ９０５）。一方、位置推定部３３は、選択された横位置算出部から車両の位置を算出できないことを示す情報が通知されている場合、選択された横位置算出部は車両の位置を算出できないと判定して（ステップＳ９０３−Ｎｏ）、優先順位リストの順位に従って、選択可能な横位置算出部の中で最も順位の高い横位置算出部を選択する（ステップＳ９０７）。位置推定部３３は、車両の位置を算出できないと判定された横位置算出部を、以後の処理では、選択対象から除外する。

また、位置推定部３３は、上述した横位置算出部を決定する処理と同様にして、車両１０の縦方向の位置を算出する縦位置算出部を選択する（ステップＳ７０３）。以上が、ステップＳ７０３の説明である。

以下、位置推定部３３が、選択された横位置算出部から通知された横ずれ量に基づいて、横ずれ量が補正された車両１０の位置を推定する処理ステップＳ７０５〜Ｓ８０３を説明する。位置推定部３３が、選択された縦位置算出部から通知された縦ずれ量に基づいて、縦ずれ量が補正された車両１０の位置を推定する処理も、横ずれ量が補正された車両１０の位置を推定する処理と同様なので、縦ずれ量が補正された車両１０の位置を推定する処理の説明は省略する。

位置推定部３３は、選択された横位置算出部が通知する車両１０の位置の横ずれ量及び情報取得時刻ｔ１を取得する（ステップＳ７０５）。第１〜２横位置算出部３１ａ〜３１ｂのそれぞれは、位置推定部３３によって選択されているか否かには関わらず、車両１０の位置の横ずれ量を求める度に、横ずれ量及び横ずれ量を求めるための情報取得時刻を、位置推定部３３へ通知している。

次に、位置推定部３３は、メモリ２２が含むデータバッファ（図３参照）を参照して、情報取得時刻ｔ１よりも前の時刻であり且つ情報取得時刻ｔ１と最も近い位置決定時刻ｔ２を取得する（ステップＳ７０７）。情報取得時刻ｔ１の周期（例えば、１００ｍ秒〜１８０ｍ秒）は、位置決定時刻の周期（例えば、３２ｍ秒）よりも長いので、情報取得時刻ｔ１は、データバッファに記憶されている最新の位置決定時刻よりも前の時刻となる。

次に、位置推定部３３は、情報取得時刻ｔ１と位置決定時刻ｔ２とが一致していない場合、メモリ２２が含むデータバッファを参照して、位置決定時刻ｔ２と情報取得時刻ｔ１との間の時刻における車両速度及びヨーレートを取得して、位置決定時刻ｔ２と情報取得時刻ｔ１との間における車両の移動量及び移動方向を求める。そして、位置推定部３３は、初期位置ｐ０と、車両１０の移動量及び移動方向とに基づいて、情報取得時刻ｔ１における車両１０の位置ｐ１を算出する（ステップＳ７０９）。一方、情報取得時刻ｔ１と位置決定時刻ｔ２とが一致する場合には、位置推定部３３は、位置決定時刻ｔ２の次の位置決定時刻における初期位置に基づいて、情報取得時刻ｔ１における車両１０の位置ｐ１を得る。

次に、位置推定部３３は、車両１０の位置ｐ１に対して横ずれ量を補正して、情報取得時刻ｔ１における車両の位置ｐ２を求める（ステップＳ７１１）。なお、位置推定部３３は、情報取得時刻ｔ１における車両の位置ｐ２を、選択された横位置算出部から情報取得時刻ｔ１における車両の位置を通知されるようにしてもよい。この場合、上述したステップＳ７０５〜７１１の処理は省略される。

次に、位置推定部３３は、メモリ２２が含むデータバッファ（図３参照）を参照して、情報取得時刻ｔ１と位置決定時刻ｔ０との間における車両１０の移動量及び移動方向を求める（ステップＳ７１３）。ここで、位置決定時刻ｔ０は、情報取得時刻ｔ１よりも後の時刻である。

次に、位置推定部３３は、車両１０の位置ｐ２と、情報取得時刻ｔ１と位置決定時刻ｔ０との間における車両１０の移動量及び移動方向とに基づいて、位置決定時刻ｔ０における車両の位置ｐ３を求める（ステップＳ８０１）。なお、位置推定部３３は、車両の位置ｐ３に対して補正係数を乗算して、補正された車両の位置を求めてもよい。

位置推定部３３は、上述したのと同様の処理を行って、位置決定時刻ｔ０における縦ずれ量が補正された車両１０の縦位置を求める。縦ずれ量を求めるのに使用された位置関係情報が取得された情報取得時刻は、横ずれ量が求めるのに使用された位置関係情報が取得された情報取得時刻とは異なる場合があるので、縦ずれ量が補正された車両１０の位置を推定する処理は、別に行われる。

次に、位置推定部３３は、横ずれ量が補正された車両１０の位置と、縦ずれ量が補正された車両１０の位置とに基づいて、位置決定時刻ｔ０における車両１０の位置を推定する（ステップＳ８０３）。例えば、位置推定部３３は、横ずれ量が補正された車両１０の位置と、縦ずれ量が補正された車両１０の位置との平均値を、位置決定時刻ｔ０における車両１０の位置として求めてもよい。そして、位置推定部３３は、位置決定時刻ｔ０における車両１０の位置を、位置決定時刻ｔ０から所定の周期だけ更新した次の位置決定時刻と関連づけて、初期位置としてデータバッファに記録する。また、位置推定部３３は、入力した車両速度及びヨーレートに基づいて、前回の位置決定時刻と今回の位置決定時刻との間の車両速度及びヨーレートの平均値を求めて、この平均値を、メモリ２２が含むデータバッファ（図３参照）に今回の位置決定時刻ｔ０と関連づけて記憶する。

次に、位置推定部３３は、位置決定時刻ｔ０における車両１０の位置を、運転計画部３５及び車両制御部３６へ通知する（ステップＳ８０５）。以上が、位置推定部３３の処理の説明である。

検出部３４は、第１画像と、合成画像、第４情報と、第３画像とに基づいて、車両１０の周囲の他の物体を検出する。検出部３４は、例えば、第１画像を識別器に入力することで第１画像に表された物体を検出する。識別器として、例えば、入力された画像から、その画像に表された物体を検出するように予め学習されたディープニューラルネットワーク（ＤＮＮ）を用いることができる。検出部３４は、ＤＮＮ以外の識別器を用いてもよい。例えば、検出部３４は、識別器として、画像上に設定されるウィンドウから算出される特徴量（例えば、Histograms of Oriented Gradients, ＨＯＧ）を入力として、そのウィンドウに検出対象となる物体が表される確信度を出力するように予め学習されたサポートベクトルマシン（ＳＶＭ）を用いてもよい。あるいはまた、検出部３４は、検出対象となる物体が表されたテンプレートと画像との間でテンプレートマッチングを行うことで、物体領域を検出してもよい。同様にして、検出部３４は、合成画像及び第３画像に基づいて、各画像に表された物体を検出する。また、検出部３４は、第４情報に基づいて、所定の方位及び距離に存在する物体を検出する。また、検出部３４は、オプティカルフローに基づく追跡処理に従って、最新の画像から検出された物体を過去の画像から検出された物体と対応付けることで、最新の画像から検出された物体を追跡してもよい。そして検出部３４は、時間経過に伴う画像上での追跡中の物体のサイズの変化に基づいて、車両１０に対するその物体の相対速度を推定してもよい。そして、検出部３４は、各画像に基づいて検出された物体について同一物を判定し、同一と判定された物体は１つの物体として判断する。検出部３４は、検出された他の物体の位置を示す情報を、運転計画部３５へ通知する。

運転計画部３５は、位置決定時刻における車両１０の位置と、検出された物体の位置を示す情報と、地図情報とを取得する。運転計画部３５は、これらの情報に基づいて、車両１０の走行予定経路を１以上生成する。走行予定経路は、例えば、現時刻から所定時間先までの各時刻における、車両１０の目標位置の集合として表される。運転計画部３５は、車両１０の位置と、地図に表された道路上の構造物と、検出された他の物体との位置関係に応じて、車両１０と他の物体との相対的な位置関係を推定する。例えば、運転計画部３５は、地図に表された車線区画線と、他の物体との位置関係に応じて、他の物体が走行している車線を特定することで、他の物体と車両１０とが同じ車線を走行しているか否かを判定する。例えば、運転計画部３５は、他の物体の水平方向の中心位置を挟むように位置する互いに隣接する二つの車線区画線で特定される車線を他の物体が走行していると判定する。同様に、運転計画部３５は、車両１０を挟むように位置する互いに隣接する二つの車線区画線で特定される車線を車両１０が走行していると判定する。そして運転計画部３５は、車両１０が走行中の車線と他の物体が走行中の車線とが同一か否かを判定する。また、地図に表された隣接する二つの車線区画線間の間隔は既知であり、かつ、カメラの焦点距離といった内部パラメータが既知であるため、画像上での隣接する二つの車線区画線間の間隔により、車両１０からの他の物体までの距離が推定できる。そこで、運転計画部３５は、画像上での他の物体の位置における、地図に表された隣接する二つの車線区画線間の間隔に基づいて、車両１０から他の物体までの距離を推定してもよい。このように、運転計画部３５は、地図に表された道路上の構造物との位置関係で他の物体と車両１０との相対的な位置関係を推定する。そのため、運転計画部３５は、画像に写った車線区画線などの道路上の構造物が不明瞭でも、他の物体と車両１０との相対的な位置関係を正確に推定できる。

運転計画部３５は、検出された他の物体の現在及び過去の軌跡に基づいての将来の軌跡を推定し、検出された他の物体が走行中の車線及び相対距離に基づいて、他の物体と車両１０とが異なる車線を走行するか、あるいは、車両１０から他の物体までの相対距離が所定距離以上となるように、車両１０の走行予定経路を生成する。なお、運転計画部３５は、複数の走行予定経路を生成してもよい。この場合、運転計画部３５は、複数の走行予定経路のうち、車両１０の加速度の絶対値の総和が最小となる経路を選択してもよい。運転計画部３５は、生成した走行予定経路を車両制御部３６へ通知する。

車両制御部３６は、位置決定時刻における車両１０の位置と、車両速度及びヨーレートと、通知された走行予定経路とに基づいて、車両１０が通知された走行予定経路に沿って走行するように車両１０の各部を制御する。例えば、車両制御部３６は、通知された走行予定経路、及び、車両１０の現在の車両速度及びヨーレートに従って、車両１０の操舵角、加速度及び角加速度を求め、その操舵角、加速度及び角加速度となるように、操舵量、アクセル開度またはブレーキ量を設定する。そして車両制御部３６は、設定された操舵量に応じた制御信号を、車両１０の操舵輪を制御するアクチュエータ（図示せず）へ出力する。また、車両制御部３６は、設定されたアクセル開度に従って燃料噴射量を求め、その燃料噴射量に応じた制御信号を車両１０のエンジンの燃料噴射装置（図示せず）へ出力する。あるいは、車両制御部３６は、設定されたブレーキ量に応じた制御信号を車両１０のブレーキ（図示せず）へ出力する。

以上に説明してきたように、この位置推定装置は、移動物体に搭載されたカメラなどの第１位置情報取得部を用いて取得された移動物体の位置を表す第１位置関係情報に基づいて、第１位置関係情報を取得した第１情報取得時刻における移動物体の第１推定位置を算出する。また、位置推定装置は、第２位置情報取得部を用いて取得された移動物体の位置を表す第２位置関係情報に基づいて、第２位置関係情報を取得した第２情報取得時刻における移動物体の第２推定位置を算出する。位置推定装置は、直前の第１情報取得時刻に取得された第１位置関係情報に基づいて第１推定位置が求められた場合、現時刻における移動物体の位置を、第１推定位置と、この第１情報取得時刻と、現時刻との間における移動物体の第１移動量及び第１移動方向とに基づいて推定する。位置推定装置は、直前の第１情報取得時刻に取得された第１位置関係情報に基づいて第１推定位置が求められない場合、現時刻における移動物体の位置を、第２推定位置と、直前の第２情報取得時刻と、現時刻との間における移動物体の第２移動量及び第２移動方向とに基づいて推定する。これにより、位置推定装置は、移動物体の位置を算出するための情報を取得する位置情報取得装置を変更しても、高い精度で移動物体の位置を求められる。

本発明では、上述した実施形態の位置推定装置および位置推定用コンピュータプログラムは、本発明の趣旨を逸脱しない限り適宜変更が可能である。

例えば、上述した車両の横方向の位置を求める第１〜２横位置算出部、及び、車両の縦方向の位置を求める第１〜２縦位置算出部は一例であり、位置推定装置が車両の横位置及び縦位置を算出する方法は特に制限されない。位置推定装置は、車両の横位置及び縦位置を算出するための２つ以上の手段を有していればよい。例えば、位置推定装置は、第３カメラが生成した画像、LiDARセンサが生成した第４情報、又は、第４カメラ６が生成した第３画像に基づいて、車両の横ずれ量又は縦ずれ量を求めてもよい。

また、位置推定装置は、同じ位置関係情報取得装置が取得した位置関係情報に基づいて、異なるアルゴリズムを用いて車両の横ずれ量又は縦ずれ量を算出する複数の位置算出部を有していてもよい。例えば、上述した実施形態では、第２横位置算出部は、第１横位置算出部と同じアルゴリズムを用いて車両の横ずれ量を算出していたが、第２横位置算出部は、第１横位置算出部と異なるアルゴリズムを用いて車両の横ずれ量を算出してもよい。同様に、上述した実施形態では、第２縦位置算出部は、第１縦位置算出部と同じアルゴリズムを用いて車両の縦ずれ量を算出していたが、第２縦位置算出部は、第１縦位置算出部と異なるアルゴリズムを用いて車両の縦ずれ量を算出してもよい。

１ 車両制御システム
２ 第１カメラ
３ 第２カメラ
４ａ〜４ｄ 第３カメラ
４ｅ カメラ画像処理装置
５ａ〜５ｄ LiDARセンサ
６ 第４カメラ
７ 測位情報受信機
７ａ 測位情報受信部
７ｂ プロセッサ
８ 地図情報記憶装置
９ 電子制御装置
１０ 車両
１１ 車内ネットワーク
２１ 通信インターフェース
２２ メモリ
２３ プロセッサ
３１ａ 第１横位置算出部
３１ｂ 第２横位置算出部
３２ａ 第１縦位置算出部
３２ｂ 第２縦位置算出部
３３ 位置推定部
３４ 検出部
３５ 運転計画部
３６ 車両制御部

Claims (4)

1. 第１の周期で設定される第１情報取得時刻ごとに、移動物体の位置を表す第１位置関係情報を取得する第１位置情報取得部と、
   前記第１位置関係情報に基づいて、前記第１情報取得時刻における前記移動物体の第１推定位置を算出する第１位置算出部と、
   第２の周期で設定される第２情報取得時刻ごとに、前記移動物体の位置を表す第２位置関係情報を取得する第２位置情報取得部と、
   前記第２位置関係情報に基づいて、前記第２情報取得時刻における前記移動物体の第２推定位置を算出する第２位置算出部と、
   直前の前記第１情報取得時刻に取得された前記第１位置関係情報に基づいて前記第１推定位置が求められた場合、前記移動物体の挙動を表す情報に基づいて、当該第１情報取得時刻と、現時刻との間における前記移動物体の第１移動量及び第１移動方向を算出し、現時刻における前記移動物体の位置を、前記第１推定位置と、前記移動物体の前記第１移動量及び前記第１移動方向とに基づいて推定し、
   直前の前記第１情報取得時刻に取得された前記第１位置関係情報に基づいて前記第１推定位置が求められない場合、前記移動物体の挙動を表す情報に基づいて、直前の前記第２情報取得時刻と、現時刻との間における前記移動物体の第２移動量及び第２移動方向を算出し、現時刻における前記移動物体の位置を、前記第２推定位置と、前記移動物体の前記第２移動量及び前記第２移動方向とに基づいて推定する位置推定部と、
   を有する位置推定装置。
2. 前記位置推定部が前記移動物体の位置を推定する間隔は、前記第１の周期及び前記第２の周期よりも短い請求項１に記載の位置推定装置。
3. 前記第１推定位置の精度は、前記第２推定位置の精度よりも高い請求項１または２に記載の位置推定装置。
4. 第１の周期で設定される第１情報取得時刻ごとに、移動物体の位置を表す第１位置関係情報を取得し、
   前記第１位置関係情報に基づいて、前記第１情報取得時刻における前記移動物体の第１推定位置を算出し、
   第２の周期で設定される第２情報取得時刻ごとに、前記移動物体の位置を表す第２位置関係情報を取得し、
   前記第２位置関係情報に基づいて、前記第２情報取得時刻における前記移動物体の第２推定位置を算出し、
   直前の前記第１情報取得時刻に取得された前記第１位置関係情報に基づいて前記第１推定位置が求められた場合、前記移動物体の挙動を表す情報に基づいて、当該第１情報取得時刻と、現時刻との間における前記移動物体の第１移動量及び第１移動方向を算出し、現時刻における前記移動物体の位置を、前記第１推定位置と、前記移動物体の前記第１移動量及び前記第１移動方向とに基づいて推定し、
   直前の前記第１情報取得時刻に取得された前記第１位置関係情報に基づいて前記第１推定位置が求められない場合、前記移動物体の挙動を表す情報に基づいて、直前の前記第２情報取得時刻と、現時刻との間における前記移動物体の第２移動量及び第２移動方向を算出し、現時刻における前記移動物体の位置を、前記第２推定位置と、前記移動物体の前記第２移動量及び前記第２移動方向とに基づいて推定する、
   ことをプロセッサに実行させる位置推定用コンピュータプログラム。

Images