JP2021017073A

JP2021017073A - 位置推定装置

Info

Publication number: JP2021017073A
Application number: JP2019131901A
Authority: JP
Inventors: 将紘坂本; Masahiro Sakamoto
Original assignee: Toyota Motor Corp
Current assignee: Toyota Motor Corp
Priority date: 2019-07-17
Filing date: 2019-07-17
Publication date: 2021-02-15
Anticipated expiration: 2039-07-17
Also published as: JP7234840B2

Abstract

【課題】急な操舵量の変化が抑制されるように、移動物体の位置及び方位角を推定する位置推定装置を提供する。【解決手段】位置推定装置は、現時刻の移動物体のヨーレートに基づいて単位時間あたりの許容ヨーレート変化量を決定し、前時刻から現時刻間の移動物体の進行方向を表す方位角の変化に対して許容される許容方位角変化量を決定し、移動物体の進行方向と直交する方向への移動が許容される許容横移動範囲を決定し、現時刻の移動物体の推定位置及び推定方位角を求め、推定方位角と前時刻における移動物体の方位角との差が許容方位角変化量内にない場合、前時刻の移動物体の方位角との差が許容方位角変化量内になるように推定方位角を補正して、現時刻の移動物体の方位角を決定し、推定位置が許容横移動範囲内にない場合、許容横移動範囲内になるように推定位置を補正して、現時刻の移動物体の位置を決定する。【選択図】図３

Description

本発明は、位置推定装置に関する。

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

例えば、車両の位置は、車両に搭載されたカメラを用いて車両の周辺を撮影した画像と、車線区画線などの地物が表された地図とを比較して、画像中の地物と地図に表された地物とをマッチングさせて推定されている。

また、車両の位置は、前回の位置決定時刻における車両の位置と、前回から今回の位置決定時刻間における車両の車両速度及びヨーレートにより算出された移動量及び方位角変化量とに基づいて求められている（例えば、特許文献１）。

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

車両が横滑りにより移動すると、推定される今回の時刻の位置および方位角が、前回の時刻の位置および方位角に対して異常に大きく変化する場合があり、このような位置に基づいて、車両の走行が自動制御されると、ステアリングが振動するような急な操舵量の変化が発生することがあった。

そこで、本発明は、急な操舵量の変化が抑制されるように、移動物体の位置及び方位角を推定する位置推定装置を提供することを目的とする。

一の実施形態によれば、位置推定装置を提供される。この位置推定装置は、地物の位置を表す位置情報を記憶する記憶部と、現時刻における移動物体のヨーレートに基づいて、移動物体のヨーレートの変化に対して許容される単位時間あたりの許容ヨーレート変化量を決定する許容ヨーレート変化量決定部と、前時刻における移動物体のヨーレートと、許容ヨーレート変化量とに基づいて、前時刻から現時刻間の移動物体の進行方向を表す方位角の変化に対して許容される許容方位角変化量を決定する許容方位角変化量決定部と、前時刻における移動物体の位置と、前時刻から現時刻間の移動物体の移動量及び方位角変化量と、許容方位角変化量とに基づいて、移動物体の進行方向と直交する方向への移動が許容される許容横移動範囲を決定する許容横移動範囲決定部と、移動物体に設けられた撮像部により得られた現時刻における移動物体の周囲の路面上の地物を表す画像と、記憶部に記憶される地物の位置を表す位置情報と、前時刻における移動物体の位置及び方位角と、前時刻から現時刻間の移動物体の移動量及び方位角変化量とに基づいて、現時刻における移動物体の推定位置及び推定方位角を求める推定部と、推定方位角と前時刻における移動物体の方位角との差が許容方位角変化量内にない場合、前時刻における移動物体の方位角との差が許容方位角変化量内になるように推定方位角を補正して、現時刻における移動物体の方位角を決定する方位角決定部と、推定位置が許容横移動範囲内にない場合、許容横移動範囲内になるように推定位置を補正して、現時刻における移動物体の位置を決定する位置決定部と、を有する。

この位置推定装置において、許容ヨーレート変化量決定部は、許容ヨーレート変化量を、円軌道で走行していると仮定した移動物体に対して動径方向に許容される位置の変動幅が所定の範囲内になるように、現時刻における移動物体のヨーレートに基づいて決定することが好ましい。

この位置推定装置において、許容方位角変化量決定部は、前時刻における移動物体のヨーレートに対して、許容ヨーレート変化量だけオフセットした値に前時刻から現時刻間の時間を乗じて、許容方位角変化量を決定することが好ましい。

この位置推定装置において、許容横移動範囲決定部は、前時刻における移動物体の位置と、前時刻から現時刻間の移動物体の移動量及び方位角変化量とに基づいて、現時刻における移動物体の第２推定位置を求め、前時刻における移動物体の位置から第２推定位置へ向かう方向を中心として、許容方位角変化量の幅を持つ領域を、許容横移動範囲として決定することが好ましい。

この位置推定装置において、許容横移動範囲決定部は、前時刻から現時刻間の移動量が所定の距離よりも大きい場合、許容横移動範囲の幅を一定にすることが好ましい。

また、他の実施形態によれば、位置推定用コンピュータプログラムを提供される。この位置推定用コンピュータプログラムは、現時刻における移動物体のヨーレートに基づいて、移動物体のヨーレートの変化に対して許容される単位時間あたりの許容ヨーレート変化量を決定することと、前時刻における移動物体のヨーレートと、許容ヨーレート変化量とに基づいて、前時刻から現時刻間の移動物体の進行方向を表す方位角の変化に対して許容される許容方位角変化量を決定することと、前時刻における移動物体の位置と、前時刻から現時刻間の移動物体の移動量及び方位角変化量と、許容方位角変化量とに基づいて、移動物体の進行方向と直交する方向への移動が許容される許容横移動範囲を決定することと、移動物体に設けられた撮像部により得られた現時刻における移動物体の周囲の路面上の地物を表す画像と、記憶部に記憶される地物の位置を表す位置情報と、前時刻における移動物体の位置及び方位角と、前時刻から現時刻間の移動物体の移動量及び方位角変化量とに基づいて、現時刻における移動物体の推定位置及び推定方位角を求めることと、推定方位角と前時刻における移動物体の方位角との差が許容方位角変化量内にない場合、前時刻における移動物体の方位角との差が許容方位角変化量内になるように推定方位角を補正して、現時刻における移動物体の方位角を決定することと、推定位置が許容横移動範囲内にない場合、許容横移動範囲内になるように推定位置を補正して、現時刻における移動物体の位置を決定すること、をプロセッサに実行させる。

本発明に係る位置推定装置は、急な操舵量の変化が抑制されるように、移動物体の位置及び方位角を推定できるという効果を奏する。このように推定された移動物体の位置及び方位角に基づいて、移動物体の運転計画を生成して、この運転計画に基づいて車両の動作が制御されることが好ましい。

位置推定装置が実装される車両制御システムの概略構成図である。位置推定装置の一つの実施形態である電子制御装置のハードウェア構成図である。位置推定装置を含む車両制御処理に関する、電子制御装置のプロセッサの機能ブロック図である。許容ヨーレート変化量決定部の動作を説明する図（その１）である。許容ヨーレート変化量決定部の動作を説明する図（その２）である。許容方位角変化量決定部の動作を説明する図である。許容横移動範囲決定部の動作を説明する図である。推定部の動作を説明する図である。方位角決定部の動作を説明する図である。位置決定部の動作を説明する図である。許容横移動範囲決定部の動作の変型例を説明する図である。位置推定処理を含む、車両制御システムの動作フローチャートである。

以下、図を参照しつつ、位置推定装置について説明する。この位置推定装置は、現時刻における移動物体のヨーレートに基づいて、移動物体のヨーレートの変化に対して許容される単位時間あたりの許容ヨーレート変化量を決定する。位置推定装置は、前時刻における移動物体のヨーレートと、許容ヨーレート変化量とに基づいて、前時刻から現時刻間の移動物体の進行方向を表す方位角の変化に対して許容される許容方位角変化量を決定する。前時刻及び現時刻は、例えば、移動物体の周囲の地物を表す画像が撮影される前回の画像情報取得時刻と今回の画像情報取得時刻である。さらに、位置推定装置は、前時刻における移動物体の位置と、前時刻から現時刻間の移動物体の移動量及び方位角変化量と、許容方位角変化量とに基づいて、移動物体の進行方向と直交する方向への移動が許容される許容横移動範囲を決定する。位置推定装置は、移動物体の移動量を、例えば、移動物体に設けられたセンサにより検知された車速情報を用いて算出する。また、位置推定装置は、移動物体の方位角変化量を、例えば、移動物体に設けられたセンサにより検知されたヨーレートを用いて算出する。これにより、位置推定装置は、車両の走行状況に応じて、車両の位置を決定する時刻間の移動物体の移動量に対する許容範囲を決定する。位置推定装置は、現時刻における移動物体の周囲の地物を表す画像と、記憶部に記憶される地物の位置を表す位置情報と、前時刻における移動物体の位置及び方位角と、前時刻から現時刻間の移動物体の移動量及び方位角変化量とに基づいて、現時刻における移動物体の推定位置及び移動物体の進行方向を表す推定方位角を推定する。位置推定装置は、推定方位角と前時刻における移動物体の方位角との差が許容方位角変化量内にない場合、前時刻における移動物体の方位角との差が許容方位角変化量内になるように推定方位角を補正して、現時刻における移動物体の方位角を決定する。また、位置推定装置は、推定位置が許容横移動範囲内にない場合、許容横移動範囲内になるように推定位置を補正して、現時刻における移動物体の位置を決定する。これにより、位置推定装置は、急な操舵量の変化が抑制されるように、移動物体の位置及び方位角を推定できる。

以下では、位置推定装置を、車両制御システムに適用した例について説明する。この例では、位置推定装置は、位置推定処理を実行することで、移動物体の一例である車両の現時刻における車両の方位角を、前時刻における車両の方位角との差が許容方位角変化量内になるように決定し、かつ、車両の現時刻における車両の位置を、許容横移動範囲内になるように決定して、車両の位置及び方位角を推定する。但し、本発明の技術範囲はそれらの実施形態に限定されず、特許請求の範囲に記載された発明とその均等物に及ぶものである。

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

本実施形態では、車両１０に搭載され、かつ、車両１０を制御する車両制御システム１は、車両の前方の画像を撮影するカメラ２と、車両１０の車両速度情報を検知する車両速度センサ３と、車両１０のヨーレートを検知するヨーレートセンサ４とを有する。また、車両制御システム１は、測位情報受信機５を有する。また、車両制御システム１は、所定の領域の広域地図情報を記憶しており、測位情報受信機５が出力する測位情報に基づいて、測位情報により表される現在位置を含む領域の地図情報を出力する地図情報生成装置６を有する。さらに、車両制御システム１は、位置推定装置の一例であり、車両１０の位置及び方位角を推定する電子制御装置（ＥＣＵ）７を有する。また、車両１０は、無線通信装置（図示せず）を有しており、この無線通信装置を介した無線通信により、基地局を介して外部のサーバと通信可能である。

カメラ２は、所定の撮影周期ごとに車両１０の前方の所定の領域を撮影し、その前方の所定の領域が写った画像を生成する。生成された画像には、車両１０の前方の所定の領域内に含まれる路面上の車線区画線などの地物が表わされる。カメラ２により生成された画像は、カラー画像であってもよく、又は、グレー画像であってもよい。カメラ２は、撮像部の一例であり、ＣＣＤあるいはＣ−ＭＯＳなど、可視光に感度を有する光電変換素子のアレイで構成された２次元検出器と、その２次元検出器上に撮影対象となる領域の像を結像する結像光学系を有する。そしてカメラ２は、車両１０の前方を向くように、例えば、車両１０の車室内に取り付けられる。そしてカメラ２は、画像を生成する度に、画像及び画像を取得した画像情報取得時刻を、車内ネットワーク８を介してＥＣＵ７へ出力する。カメラ２により生成された画像は、ＥＣＵ７において、位置推定処理と、車両１０の周囲の物体を検出する処理に使用される。

車両速度センサ３は、車両１０の車両速度情報を検知して、車両速度情報及び車両速度情報を取得した速度情報取得時刻を、車内ネットワーク８を介してＥＣＵ７へ出力する。車両速度センサ３は、例えば、車軸に取り付けられ、車軸の回転数を検知して、回転数に比例したパルス信号を出力する。

ヨーレートセンサ４は、車両１０のヨーレートを検知して、ヨーレート及びヨーレートを取得したヨーレート情報取得時刻を、車内ネットワーク８を介してＥＣＵ７へ出力する。ヨーレートセンサ４として、例えば、ジャイロスコープなどの加速度センサを用いることができる。

測位情報受信機５は、車両１０の現在位置を表す測位情報を取得する。例えば、測位情報受信機５は、ＧＰＳ受信機とすることができる。そして測位情報受信機５は、所定の受信周期で測位情報を取得する度に、測位情報及び測位情報を取得した測位情報取得時刻を、地図情報生成装置６へ出力する。

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

ＥＣＵ７は、車両１０を制御する。本実施形態では、ＥＣＵ７は、カメラ２が生成する画像、車両速度及びヨーレートに基づいて車両１０の位置及び進行方向を表す方位角を推定して、車両１０を自動運転するように車両１０を制御する。そのために、ＥＣＵ７は、通信インターフェース２１と、メモリ２２と、プロセッサ２３とを有する。

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

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

プロセッサ２３は、制御部の一例であり、１個または複数個のＣＰＵ（ＣｅｎｔｒａｌＰｒｏｃｅｓｓｉｎｇＵｎｉｔ）及びその周辺回路を有する。プロセッサ２３は、論理演算ユニット、数値演算ユニットあるいはグラフィック処理ユニットといった他の演算回路をさらに有していてもよい。プロセッサ２３が複数個のＣＰＵを有する場合、ＣＰＵごとにメモリを有していてもよい。そして、プロセッサ２３は、カメラ２により画像が生成される度に、車両の方位角を、カメラ２により画像が生成された前回の画像情報取得時刻における車両の方位角との差が許容方位角変化量内になるように決定し、かつ、車両の位置を、許容横移動範囲内になるように決定して、車両の位置及び方位角を推定する位置推定処理を実行する。また、プロセッサ２３は、カメラ２により画像が生成される周期よりも短い周期で設定される位置決定時刻において、今回の位置決定時刻における車両１０の方位角を、前回の位置決定時刻における車両の方位角との差が最新の許容方位角変化量内になるように決定し、かつ、車両の位置を、最新の許容横移動範囲内になるように決定する。さらに、プロセッサ２３は、推定された車両１０の位置及び方位角と、車両１０の周囲の他の物体との相対的な位置関係に基づいて、車両１０を自動運転するよう、車両１０を制御する。

図３は、位置推定処理を含む車両制御処理に関する、ＥＣＵ７のプロセッサ２３の機能ブロック図である。プロセッサ２３は、許容ヨーレート変化量決定部３１と、許容方位角変化量決定部３２と、許容横移動範囲決定部３３と、推定部３４と、方位角決定部３５と、位置決定部３６と、物体検出部３７と、運転計画部３８と、車両制御部３９とを有する。プロセッサ２３が有するこれらの各部は、例えば、プロセッサ２３上で動作するコンピュータプログラムにより実現される機能モジュールである。あるいは、プロセッサ２３が有するこれらの各部は、プロセッサ２３に設けられる、専用の演算回路であってもよい。また、プロセッサ２３が有するこれらの各部のうち、許容ヨーレート変化量決定部３１と、許容方位角変化量決定部３２と、許容横移動範囲決定部３３と、推定部３４と、方位角決定部３５と、位置決定部３６とが、位置推定処理を実行する。

許容ヨーレート変化量決定部３１は、今回の画像情報取得時刻における車両１０のヨーレートに基づいて、車両１０のヨーレートの変化に対して許容される単位時間あたりの許容ヨーレート変化量を決定する。具体的には、許容ヨーレート変化量決定部３１は、円軌道で走行していると仮定した車両１０に対して動径方向に許容される位置の変動幅が所定の範囲内になるように、許容ヨーレート変化量を現時刻における車両１０のヨーレートに基づいて決定する。図４に示すように、車両１０は、カメラ２により画像が生成された前回の画像情報取得時刻において位置４００に位置しており、ヨーレートはω_preであったとする。カメラ２により画像が生成された今回の画像情報取得時刻と前回の画像情報取得時刻との間に車両１０は、円軌道で走行して、今回の画像情報取得時刻において位置４０１に位置しており、ヨーレートはωであったとする。許容ヨーレート変化量決定部３１は、今回の画像情報取得時刻における許容ヨーレート変化量Δω_guardを、今回の画像情報取得時刻におけるヨーレートωに基づいて、位置４０１からの動径方向の変動幅が所定の範囲内になるように決定する。本明細書では、カメラ２により画像が生成された今回の画像情報取得時刻は、現時刻の一例であり、カメラ２により画像が生成された前回の画像情報取得時刻は、前時刻の一例である。

例えば、許容ヨーレート変化量決定部３１は、車両１０が、遠心力が０．３Ｇとなる円軌道で走行しており、０．１秒後における車両１０の動径方向の変動幅が５ｃｍ変化する時のヨーレート変化量を、許容ヨーレート変化量としてもよい。図５は、このように決定されたヨーレート変化量Δωと、ヨーレートωとの関係を示す。

また、許容ヨーレート変化量決定部３１は、許容ヨーレート変化量の下限値を、ゼロではない値に設定することが好ましい。これは、許容ヨーレート変化量をゼロとすると、かえって車両の挙動が不安定になる場合があるからである。許容ヨーレート変化量決定部３１は、許容ヨーレート変化量の下限値として、車両１０がまっすぐに走行している時に許容される許容ふらつき量（例えば、３．５ｄｅｇ／ｓ²）に基づいて決定してもよい。

許容ヨーレート変化量決定部３１は、許容ヨーレート変化量を、下記の式（１）のように決定し得る。

ここで、αは、図５に示すΔωとωとの関係を表す直線の傾きであり、φは、単位がラジアン／ｓ²で表された許容ふらつき量であり、Δｔは、前回から今回の画像情報取得時刻間の時間である。そして、許容ヨーレート変化量決定部３１は、決定した許容ヨーレート変化量を、許容方位角変化量決定部３２へ通知する。

許容方位角変化量決定部３２は、カメラ２により画像が生成された前回の画像情報取得時刻における車両１０のヨーレートと、許容ヨーレート変化量とに基づいて、前回から今回の画像情報取得時刻間の車両１０の進行方向を表す方位角の変化に対して許容される許容方位角変化量を決定する。具体的には、許容方位角変化量決定部３２は、図６に示すように、前回の画像情報取得時刻における車両１０のヨーレートω_preに対して、許容ヨーレート変化量Δω_guardだけオフセットした値に前回から今回の画像情報取得時刻間の時間Δｔを乗じて、許容方位角変化量Δθ_guardを決定する。ここでθｃは、今回の画像情報取得時刻の車両１０の進行方向を表す方位角であり、θ_preは、前回の画像情報取得時刻の車両１０の進行方向を表す方位角である。前回から今回の画像情報取得時刻間の車両１０の進行方向を表す方位角の変化Δθは、θｃとθ_preとの差で表される。

許容方位角変化量決定部３２は、許容方位角変化量Δθ_guardを、下記の式（２）のように決定し得る。そして、許容方位角変化量決定部３２は、許容方位角変化量Δθ_guardを、許容横移動範囲決定部３３及び方位角決定部３５へ通知する。

許容横移動範囲決定部３３は、前回の画像情報取得時刻における車両１０の位置と、前回から今回の画像情報取得時刻間の車両１０の移動量及び方位角変化量と、許容方位角変化量とに基づいて、車両１０の進行方向と直交する方向への移動が許容される許容横移動範囲を決定する。許容横移動範囲決定部３３は、図７に示すように、前回の画像情報取得時刻における車両１０の位置７００と、前回から今回の画像情報取得時刻間の車両１０の移動量及び方位角変化量とに基づいて、今回の画像情報取得時刻における車両１０の推定位置７０１（デッドレコニングにより推定される位置）を求める。許容横移動範囲決定部３３は、車両１０の車速速度情報により求められる車両速度を積分して、前回から今回の画像情報取得時刻間の車両１０の移動量を求め、車両１０のヨーレートを積分して、前回から今回の画像情報取得時刻間の車両１０の方位角変化量を求める。許容横移動範囲決定部３３は、前回の画像情報取得時刻における車両１０の位置及び移動方向と、移動量及び方位角変化量とを用いて、デッドレコニングにより推定される位置を求める。許容横移動範囲決定部３３は、推定位置７０１を、位置決定部３６から通知されている前回の画像情報取得時刻における車両１０の位置７００を原点とする車両座標系で表す。車両座標系は、車両の進行方向をＺｃ軸とし、Ｚｃ軸と直交し、かつ、地面に平行な方向をＸｃ軸とし、鉛直方向をＹｃ軸とする。許容横移動範囲決定部３３は、前回から今回の画像情報取得時刻間の車両１０の移動量及び方位角変化量を、その間に入力した車両速度情報及びヨーレートに基づいて算出する。許容横移動範囲決定部３３は、前回の画像情報取得時刻における車両１０の位置７００から今回の画像情報取得時刻における車両１０の推定位置７０１へ向かう方向αを中心として、許容方位角変化量の幅を持つ領域を、許容横移動範囲として決定する。図７に示す例では、許容横移動範囲決定部３３は、方向αを中心として、（ω_pre＋Δω_guard）Δｔ／２と、（ω_pre−Δω_guard）Δｔ／２との間の範囲を、許容横移動範囲として決定する。そして、許容横移動範囲決定部３３は、許容横移動範囲を位置決定部３６へ通知する。

推定部３４は、今回の画像情報取得時刻における車両１０の周囲の路面上の地物を表す画像情報と、地図情報生成装置６から受信した地図情報に含まれる地物の位置情報と、前回の画像情報取得時刻における車両１０の位置及び方位角と、前回から今回の画像情報取得時刻間の車両１０の移動量及び方位角変化量とに基づいて、今回の画像情報取得時刻における車両１０の推定位置及び推定方位角を求める。まず、推定部３４は、カメラ２により生成された画像と地図情報とを用いて、今回の画像情報取得時刻における車両１０の第１推定位置及び第１推定方位角を求める。そして、推定部３４は、デッドレコニングにより、今回の画像情報取得時刻における車両１０の第２推定位置及び第２推定方位角を求める。そして、推定部３４は、第１推定位置及び第１推定方位角と、第２推定位置及び第２推定方位角とを、予測フィルタに入力して、今回の画像情報取得時刻における車両１０の推定位置及び推定方位角を求める。

まず、推定部３４は、車両１０の位置及び姿勢を仮定して、地図情報生成装置６から受信した地図情報に表された車両１０の周囲の地物の一例である車線区画線を、今回の画像情報取得時刻に生成されたカメラ２の画像上に投影する。例えば、推定部３４は、今回の画像情報取得時刻に測位情報受信機５から受信した測位情報で表される車両１０の位置、及び、直近の複数の測位情報から求められる車両１０の進行方向に相当する車両１０の姿勢を、車両１０の仮定位置及び仮定姿勢とする。推定部３４は、その仮定位置及び仮定姿勢に従って、世界座標系から、カメラ２の位置を原点とし、カメラ２の光軸方向を一つの軸方向とするカメラ座標系への変換式を求める。そのような変換式は、座標系間の回転を表す回転行列と座標系間の平行移動を表す並進ベクトルの組み合わせで表される。そして推定部３４は、その変換式に従って、地図情報に含まれる、世界座標系で表された車両１０の周囲の道路上の車線区画線の座標を、カメラ座標系の座標に変換する。そして推定部３４は、カメラ２の焦点距離といったカメラ２の内部パラメータに基づいて、カメラ座標系で表された車両１０の周囲の車線区画線を、今回の画像情報取得時刻に生成されたカメラ２の画像上に投影して、地図に表された車両１０の周囲の車線区画線と、カメラ２の画像上に検出された検出点との一致度合を算出する。推定部３４は、仮定位置及び仮定姿勢を所定量ずつ変化させながら、上記と同様の座標系変換、投影及び一致度合算出の各処理を実行することで、複数の仮定位置及び仮定姿勢のそれぞれについて、地図情報に表された車両１０の周囲の車線区画線と画像から検出された検出点との一致度合を算出する。そして推定部３４は、一致度合が最大となるときの仮定位置及び仮定姿勢を特定して、その仮定位置を車両１０の第１推定位置とし、その仮定姿勢に基づいて車両１０の進行方向を表す第１推定方位角を求める。

また、推定部３４は、カメラ２により画像が生成された前回の画像情報取得時刻に推定された車両１０の位置及び方位角と、カメラ２により画像が生成された前回から今回の画像情報取得時刻間の車両１０の移動量及び移動方向とに基づいて、今回の画像情報取得時刻における車両１０の第２推定位置を車両１０の進行方向を表す第２推定方位角を推定する。推定部３４は、前回の画像情報取得時刻における車両１０の位置を、位置決定部３６から通知されており、前回の画像情報取得時刻における車両１０の方位角を方位角決定部３５から通知されている。推定部３４は、前回の画像情報取得時刻と今回の画像情報取得時刻との間の車両１０の移動量及び移動方向を、その間に入力した車両速度情報及びヨーレートに基づいて算出する。なお、推定部３４は、カメラ２により画像が生成された前回の画像情報取得時刻に推定された車両１０の位置及び方位角の代わりに、最新の位置決定時刻に推定された車両１０の位置及び方位角を用いてもよい。

そして、推定部３４は、図８に示すように、第１推定位置及び第１推定方位角と、第２推定位置及び第２推定方位角とを、予測フィルタの一例であるカルマンフィルタに入力して、今回の画像情報取得時刻における車両１０の推定位置及び推定方位角θｃを推定する。推定部３４は、推定方位角θｃを方位角決定部３５へ通知し、推定位置を位置決定部３６へ通知する。

方位角決定部３５は、推定方位角θｃと、カメラ２により画像が生成された前回の画像情報取得時刻における車両１０の方位角との差が許容方位角変化量内にない場合、前回の画像情報取得時刻における車両１０の方位角との差が許容方位角変化量内になるように推定方位角を補正して、今回の画像情報取得時刻における車両１０の推定方位角を決定する。具体的には、方位角決定部３５は、図９に示すように、推定方位角θｃと前回の画像情報取得時刻における車両１０の方位角θ_preとの差Δθを求める。方位角決定部３５は、差Δθが、（ω_pre＋Δω_guard）Δｔよりも大きい場合、差Δθが許容方位角変化量内になるように推定方位角θｃを補正する。また、方位角決定部３５は、差Δθが、（ω_pre−Δω_guard）Δｔよりも小さい場合、差Δθが許容方位角変化量内になるように推定方位角θｃを補正する。また、方位角決定部３５は、差Δθが、許容方位角変化量内にある場合、推定方位角θｃに対する補正を行わない。方位角決定部３５は、推定方位角θｃを、下記の式（３）のように決定し得る。

そして、方位角決定部３５は、今回の画像情報取得時刻における推定方位角θｃを運転計画部３８及び車両制御部３９へ通知する。また、方位角決定部３５は、今回の画像情報取得時刻における推定方位角θｃを推定部３４へ通知する。

また、方位角決定部３５は、カメラ２により画像が生成される周期よりも短い周期で設定される位置決定時刻において、カメラ２により画像が生成された最新の画像情報取得時刻における車両１０の位置と、最新の画像情報取得時刻と位置決定時刻との間の車両１０の移動量及び移動方向とに基づいて、位置決定時刻における車両１０の推定位置及び推定方位角を求める。方位角決定部３５は、最新の画像情報取得時刻と位置決定時刻との間の車両１０の移動量及び移動方向を、その間に入力した車両速度情報及びヨーレートに基づいて算出する。方位角決定部３５は、今回の位置決定時刻における車両１０の推定方位角を、前回の位置決定時刻における車両１０の推定方位角との差が、今回の位置決定時刻の直前に決定された最新の画像情報取得時刻における許容方位角変化量内になるように補正して、推定方位角を決定する。そして、方位角決定部３５は、推定方位角を運転計画部３８及び車両制御部３９へ通知する。また、方位角決定部３５は、今回の位置決定時刻における車両１０の推定位置及び推定方位角を、位置決定時刻と共に、許容横移動範囲決定部３３、推定部３４及び位置決定部３６へ通知する。

位置決定部３６は、推定部３４から通知された推定位置が許容横移動範囲内にない場合、許容横移動範囲内になるように推定位置を補正して、カメラ２により画像が生成された今回の画像情報取得時刻における車両１０の推定位置を決定する。図１０では、推定部３４から通知された推定位置１００２は、前回の画像情報取得時刻における車両１０の位置１０００を原点とする車両座標系で表されている。位置決定部３６は、推定位置１００２を許容横移動範囲と比較して、推定位置１００２が許容横移動範囲内にない場合、車両１０の推定される位置を、推定位置１００２から許容横移動範囲の境界まで、車両１０の進行方向αと直行する方向に移動して、補正された推定位置１００３を求める。一方、位置決定部３６は、推定位置１００２が許容横移動範囲内にある場合、推定位置の補正は行わない。そして、位置決定部３６は、今回の画像情報取得時刻における車両１０の推定位置を、許容横移動範囲決定部３３及び推定部３４へ通知する。また、位置決定部３６は、今回の画像情報取得時刻における車両１０の推定位置を、世界座標系に変換して、運転計画部３８及び車両制御部３９へ通知する。

また、位置決定部３６は、方位角決定部３５から通知された、カメラ２により画像が生成される周期よりも短い周期で設定される位置決定時刻における車両１０の推定位置を、この推定位置が許容横移動範囲内にない場合、許容横移動範囲内になるように推定位置を補正する。位置決定部３６は、この許容横移動範囲として、今回の位置決定時刻の直前に決定された最新の許容横移動範囲を用いる。位置決定部３６は、推定位置を世界座標系に変換して、運転計画部３８及び車両制御部３９へ通知する。

物体検出部３７は、カメラ２により生成された画像を取得し、この画像に基づいて、車両１０の周囲の他の物体を検出する。物体検出部３７は、例えば、画像を識別器に入力することで画像に表された物体を検出する。識別器として、例えば、入力された画像から、その画像に表された物体を検出するように予め学習されたディープニューラルネットワーク（ＤＮＮ）を用いることができる。物体検出部３７は、ＤＮＮ以外の識別器を用いてもよい。例えば、物体検出部３７は、識別器として、画像上に設定されるウィンドウから算出される特徴量（例えば、ＨｉｓｔｏｇｒａｍｓｏｆＯｒｉｅｎｔｅｄＧｒａｄｉｅｎｔｓ，ＨＯＧ）を入力として、そのウィンドウに検出対象となる物体が表される確信度を出力するように予め学習されたサポートベクトルマシン（ＳＶＭ）を用いてもよい。あるいはまた、物体検出部３７は、検出対象となる物体が表されたテンプレートと画像との間でテンプレートマッチングを行うことで、物体領域を検出してもよい。また、物体検出部３７は、オプティカルフローに基づく追跡処理に従って、最新の画像から検出された物体を過去の画像から検出された物体と対応付けることで、最新の画像から検出された物体を追跡してもよい。そして物体検出部３７は、時間経過に伴う画像上での追跡中の物体のサイズの変化に基づいて、車両１０に対するその物体の相対速度を推定してもよい。そして、物体検出部３７は、各画像に基づいて検出された物体について同一物を判定し、同一と判定された物体は１つの物体として判断する。物体検出部３７は、検出された物体の位置を示す情報を、運転計画部３８へ通知する。なお、物体検出部３７は、ライダセンサ等の距離画像を取得するセンサが測定した測定結果に基づいて、車両１０の周囲の他の物体を検出してもよい。

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

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

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

図１２は、プロセッサ２３により実行される、位置推定処理を含む、車両制御処理の動作フローチャートである。プロセッサ２３は、所定の周期で設定される画像情報取得時刻ごとに図１２に示される動作フローチャートに従って車両制御処理を実行する。なお、以下に示される動作フローチャートにおいて、ステップＳ１２０１〜Ｓ１２０６の処理が位置姿勢推定処理に対応する。

まず、プロセッサ２３の許容ヨーレート変化量決定部３１は、今回の画像情報取得時刻における車両１０のヨーレートに基づいて、車両１０のヨーレートの変化に対して許容される単位時間あたりの許容ヨーレート変化量を決定する（ステップＳ１２０１）。

次に、プロセッサ２３の許容方位角変化量決定部３２は、前回の画像情報取得時刻における車両１０のヨーレートと、許容ヨーレート変化量とに基づいて、前回から今回の画像情報取得時刻間の車両１０の進行方向を表す方位角の変化に対して許容される許容方位角変化量を決定する（ステップＳ１２０２）。

次に、プロセッサ２３の許容横移動範囲決定部３３は、前回の画像情報取得時刻における車両１０の位置と、前回から今回の画像情報取得時刻間の車両１０の移動量及び方位角変化量と、許容方位角変化量とに基づいて、車両１０の進行方向と直交する方向への移動が許容される許容横移動範囲を決定する（ステップＳ１２０３）。

次に、プロセッサ２３の推定部３４は、車両１０に設けられた撮像部により得られた今回の画像情報取得時刻における車両１０の周囲の地物を表す画像と、地図情報生成装置６の記憶部に記憶される地物の位置を表す位置情報と、前回の画像情報取得時刻における車両１０の位置及び方位角と、前回から今回の画像情報取得時刻間の車両１０の移動量及び方位角変化量とに基づいて、今回の画像情報取得時刻における車両１０の推定位置及び推定方位角を求める（ステップＳ１２０４）。

次に、プロセッサ２３の方位角決定部３５は、推定方位角と前回の画像情報取得時刻における車両１０の方位角との差が許容方位角変化量内にない場合、前回の画像情報取得時刻における車両１０の方位角との差が許容方位角変化量内になるように推定方位角を補正して、今回の画像情報取得時刻における車両１０の方位角を決定する（ステップＳ１２０５）。また、プロセッサ２３の方位角決定部３５は、位置決定時刻における車両１０の方位角を決定する。

次に、プロセッサ２３の位置決定部３６は、推定位置が許容横移動範囲内にない場合、許容横移動範囲内になるように推定位置を補正して、今回の画像情報取得時刻における車両１０の位置を決定する（ステップＳ１２０６）。また、プロセッサ２３の方位角決定部３５は、位置決定時刻における車両１０の位置を決定する。

次に、プロセッサ２３の物体検出部３７は、カメラ２により生成された画像などに基づいて、車両１０の周囲の他の物体を検出する（ステップＳ１２０７）。

次に、プロセッサ２３の運転計画部３８は、今回の画像情報取得時刻及び位置決定時刻における車両１０の位置と、検出された物体の位置を示す情報と、地図情報などに基づいて、車両１０の走行予定経路を生成する（ステップＳ１２０８）。

次に、プロセッサ２３の車両制御部３９は、今回の画像情報取得時刻及び位置決定時刻における車両１０の位置と、車両速度及びヨーレートと、走行予定経路とに基づいて、走行予定経路に沿って走行するように車両１０の各部を制御する（ステップＳ１２０９）。

以上に説明してきたように、この位置推定装置は、車両の現時刻における車両の方位角を、前時刻における車両の方位角との差が許容方位角変化量内になるように決定し、かつ、車両の現時刻における車両の位置を、許容横移動範囲内になるように決定して、車両の位置及び進行方向を表す方位角を推定する。これにより、急な操舵量の変化が抑制されるように、移動物体の位置及び方位角を推定できる。

車両の自動運転制御では、現在の車両の位置に基づいて車両の制御が行われる。横滑りなどに起因して、推定される今回の時刻の位置が、前回の時刻の位置に対して異常に大きく変化する場合があり、このような位置に基づいて、車両の走行が自動制御されると、ステアリングが振動するような急な操舵量の変化が発生することがあった。

このように、ステアリングの急な操舵量の変化が発生することを抑制する対策として、例えば、今回の位置を推定する処理において、前回と今回との時刻間の車両の推定移動量に対して、一定の制限を設けることがあり得る。

しかし、車両の走行状況とは無関係に、車両の推定移動量に対して一定の制限を設けることは、移動量の推定に対して、車両の走行状態に対する追従性が十分でなくなる場合があるので、必ずしも車両の位置の精度を向上するとは限らない。

本実施形態の位置推定装置では、車両の走行状態に応じて、車両の位置を決定する時刻間の移動物体の移動量に対する許容範囲を決定して、車両を制御することにより、ステアリングが振動するような急な操舵量の変化の発生が抑制される。

次に、許容横移動範囲決定部が許容横移動範囲を決定する他の例を、図１１を参照しながら、以下に説明する。

許容横移動範囲決定部３３は、カメラ２により画像が生成された前回から今回の画像情報取得時刻間の移動量が所定の距離よりも大きい場合、車両１０の進行方向αと直行する方向における許容横移動範囲の幅を一定にする。図１１に示すように、許容横移動範囲決定部３３は、カメラ２により画像が生成された前回の画像情報取得時刻における車両の位置１１００と、今回の画像情報取得時刻における車両の位置１１０１との距離を求めて、この距離が、所定の距離（例えば、５〜１５ｍ）よりも大きい場合、車両１０の進行方向αと直行する方向における許容横移動範囲の幅を一定にする。このように、車両１０の横方向の移動量に対して上限を設定することにより、カメラ２により画像が生成された今回の画像情報取得時刻における車両１０の位置及び方位角が、前回の画像情報取得時刻における車両１０位置及び方位角に対して異常に大きく変化しないような現時刻の移動物体の位置を決定できる。

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

例えば、上述した実施形態では、移動物体の進行方向と直行する横方向への移動が許容される許容横移動範囲内になるように、移動物体の推定位置が補正されていた。これは、ステアリングが振動するような急な操舵量の変化が発生に対して、横方向の移動が与える影響が多いためである。同様に、移動物体の進行方向への移動が許容される許容移動範囲を設定して、移動物体の進行方向への移動が許容される許容移動範囲内になるように、移動物体の位置を推定してもよい。

１車両制御システム
２カメラ
３車両速度センサ
４ヨーレートセンサ
５測位情報受信機
６地図情報生成装置
７電子制御装置
８車内ネットワーク
１０車両
２１通信インターフェース
２２メモリ
２３プロセッサ
３１許容ヨーレート変化量決定部
３２許容方位角変化量決定部
３３許容横移動範囲決定部
３４推定部
３５方位角決定部
３６位置決定部
３７物体検出部
３８運転計画部
３９車両制御部

Claims

地物の位置を表す位置情報を記憶する記憶部と、
現時刻における移動物体のヨーレートに基づいて、前記移動物体のヨーレートの変化に対して許容される単位時間あたりの許容ヨーレート変化量を決定する許容ヨーレート変化量決定部と、
前時刻における前記移動物体のヨーレートと、前記許容ヨーレート変化量とに基づいて、前時刻から現時刻間の前記移動物体の進行方向を表す方位角の変化に対して許容される許容方位角変化量を決定する許容方位角変化量決定部と、
前時刻における前記移動物体の位置と、前時刻から現時刻間の前記移動物体の移動量及び方位角変化量と、前記許容方位角変化量とに基づいて、前記移動物体の進行方向と直交する方向への移動が許容される許容横移動範囲を決定する許容横移動範囲決定部と、
前記移動物体に設けられた撮像部により得られた現時刻における前記移動物体の周囲の地物を表す画像と、前記記憶部に記憶される地物の位置を表す位置情報と、前時刻における前記移動物体の位置及び方位角と、前時刻から現時刻間の前記移動物体の移動量及び方位角変化量とに基づいて、現時刻における前記移動物体の推定位置及び推定方位角を求める推定部と、
前記推定方位角と前時刻における前記移動物体の方位角との差が前記許容方位角変化量内にない場合、前時刻における前記移動物体の方位角との差が前記許容方位角変化量内になるように前記推定方位角を補正して、現時刻における前記移動物体の方位角を決定する方位角決定部と、
前記推定位置が前記許容横移動範囲内にない場合、前記許容横移動範囲内になるように前記推定位置を補正して、現時刻における前記移動物体の位置を決定する位置決定部と、
を有する位置推定装置。