JP2018084492A - 自己位置推定方法及び自己位置推定装置 - Google Patents

Abstract

【課題】車両が旋回してオドメトリの誤差が発生した場合でも自己位置の推定精度を向上させる。【解決手段】本発明の自己位置推定方法は、自車両の周囲に存在する物標と自車両との間の相対位置を自車両の移動量だけ移動させて物標位置データとして蓄積し、自車両が走行してきた方向に観測される観測物標を検出し、観測物標の物標位置データと蓄積された物標位置データとを比較して物標位置の差が所定値未満となる区間に蓄積された物標位置データを選択し、選択された物標位置データと、地図情報に含まれる物標位置情報とを照合して自車両の自己位置を推定する。【選択図】図３

Description

本発明は、自車両の自己位置を推定する自己位置推定方法及び自己位置推定装置に関する。

従来では、移動体を基準として予め定めた領域内に存在する環境情報に限定し、この限定された環境情報を、予め保持している環境地図と照合することによって自己位置を推定していた（特許文献１参照）。

特開２００８−２５０９０６号公報

しかしながら、上述した従来の自己位置推定方法では、車両が旋回した場合にはオドメトリの誤差が発生して自己位置の推定精度が低下してしまうという問題点があった。

本発明は上記課題に鑑みて成されたものであり、その目的は、車両が旋回してオドメトリの誤差が発生した場合でも自己位置の推定精度を向上させることのできる自己位置推定方法及びその装置を提供することにある。

上述の課題を解決するために、本発明の一態様に係る自己位置推定方法及びその装置は、自車両の周囲に存在する物標と自車両との間の相対位置を自車両の移動量だけ移動させて物標位置データとして蓄積する。そして、自車両が走行してきた方向に観測される観測物標を検出し、観測物標の物標位置データと、蓄積された物標位置データとを比較して物標位置の差が所定値未満となる区間に蓄積された物標位置データを選択する。この結果から、選択された物標位置データと、地図情報に含まれる物標位置情報とを照合して自車両の自己位置を推定する。

本発明によれば、車両が旋回してオドメトリの誤差が発生した場合でも自己位置の推定精度を向上させることができる。

図１は、本発明の第１実施形態に係る自己位置推定装置を備えた自己位置推定システムの構成を示すブロック図である。 図２は、車両に搭載されたカメラの一例を示す図である。 図３は、本発明の第１実施形態に係る自己位置推定装置による自己位置推定処理の処理手順を示すフローチャートである。 図４は、本発明の第１実施形態に係る自己位置推定装置による観測物標の物標位置データと蓄積された物標位置データとの比較方法を説明するための図である。 図５は、本発明の第２実施形態に係る自己位置推定装置を備えた自己位置推定システムの構成を示すブロック図である。 図６は、本発明の第２実施形態に係る自己位置推定装置による自己位置推定処理の処理手順を示すフローチャートである。 図７は、本発明の第２実施形態に係る自己位置推定装置による観測物標の物標位置データと蓄積された物標位置データとの比較方法を説明するための図である。

［第１実施形態］
以下、本発明の第１実施形態について図面を参照して説明する。図面の記載において、同一又は類似の部分には同一又は類似の符号を付し、重複する説明を省略する。

［自己位置推定システム］
図１は、本実施形態に係る自己位置推定装置を備えた自己位置推定システムの構成を示すブロック図である。図１に示すように、本実施形態に係る自己位置推定システム１００は、周囲センサ群１と、自己位置推定装置３と、記憶装置４と、車両センサ群５とを備えている。本実施形態に係る自己位置推定システム１００は、車両Ｖ（図２参照）に搭載され、車両Ｖの自己位置を推定する。

本実施形態では、推定する車両Ｖの自己位置として、東西方向（Ｘ軸方向）の位置（Ｘ座標［ｍ］）と、南北方向（Ｙ軸方向）の位置（Ｙ座標［ｍ］）と、姿勢角情報として車両の方位角θ（ヨー角［ｒａｄ］）とを推定する。すなわち、二次元平面上における合計３自由度の位置及び姿勢角を推定する。

周囲センサ群１は、画像処理可能なデジタル画像を撮影する複数のカメラ１１、１３を備えている。ただし、その他のセンサを備えていてもよく、周囲センサ群１は、車両Ｖの周囲に存在する物標を検出するための複数のセンサから構成されていればよい。

図２は、周囲センサ群１を車両Ｖに搭載した状態の一例を示す図である。カメラ１１は、例えば、車両Ｖの前方を撮像する前方カメラであり、車両Ｖの前方の路面を撮影して走行レーンや縁石を検出する。ただし、カメラ１１は、車両Ｖの左右両側のドアミラーにそれぞれ搭載された側方カメラであってもよい。カメラ１３は、例えば、車両Ｖの後方を撮像する後方カメラであり、車両Ｖの後方の路面を撮影して走行レーンや縁石を検出する。カメラ１１、１３は、例えば、ＣＣＤ、ＣＭＯＳ等の固体撮像素子により画像を撮影し、逐次、撮影した画像を自己位置推定装置３に出力する。尚、本実施形態では、前方カメラと後方カメラの両方を備えた場合を一例として説明するが、後方カメラだけでもよいし、２つ以上のカメラを備えていてもよい。

記憶装置４は、地図上に存在する物標の物標位置情報を含む地図情報４１を記憶している。記憶装置４は、半導体メモリ、磁気ディスク等から構成可能である。地図情報４１に記録される物標は、例えば、停止線、横断歩道、横断歩道予告、区画線等を示す道路標示や、縁石等の構造物の他、周囲センサ群１により検出可能な種々の設備等を含む。地図情報４１において、縁石、白線等の位置情報は、両端点の二次元位置情報を有する直線情報の集合体で定義される。地図情報４１は、実環境の形状が曲線の場合、折れ線で近似された二次元平面上の直線情報として記述される。

車両センサ群５は、ＧＰＳ受信機５１、アクセルセンサ５２、ステアリングセンサ５３、ブレーキセンサ５４、車速センサ５５、加速度センサ５６、車輪速センサ５７及びヨーレートセンサ等のその他のセンサ５８等を備えている。各センサ５１〜５８は自己位置推定装置３に接続され、逐次、各種の検出結果を自己位置推定装置３に出力する。

自己位置推定装置３は、自車両の周囲に存在する物標の物標位置データと地図情報に含まれる物標位置情報とを照合して自車両の現在位置である自己位置を推定するコントローラである。自己位置推定装置３は、車両センサ群５の各検出結果を用いて、地図情報４１における車両Ｖの概位置を算出したり、単位時間における車両Ｖの移動量を示すオドメトリを算出したりする。自己位置推定装置３は、物標位置検出部３１と、移動量推定部３２と、物標位置蓄積部３３と、地図情報取得部３４と、物標選択部３５と、自己位置推定部３６とを有する。尚、自己位置推定装置３は、マイクロコンピュータ、マイクロプロセッサ、ＣＰＵを含む汎用の電子回路とメモリ等の周辺機器から構成されている。そして、特定のプログラムを実行することにより、上述した物標位置検出部３１、移動量推定部３２、物標位置蓄積部３３、地図情報取得部３４、物標選択部３５及び自己位置推定部３６として動作する。このような自己位置推定装置３の各機能は、１または複数の処理回路によって実装することができる。処理回路は、例えば電気回路を含む処理装置等のプログラムされた処理装置を含み、また実施形態に記載された機能を実行するようにアレンジされた特定用途向け集積回路（ＡＳＩＣ）や従来型の回路部品のような装置も含んでいる。また、処理回路は、例えば自動運転制御等の車両Ｖの他の制御に用いる電子制御ユニット（ＥＣＵ）と兼用されてもよい。

物標位置検出部３１は、カメラ１１、１３で撮像された画像から自車両の周囲に存在する物標の物標情報を検出する物標検出センサである。特に、物標位置検出部３１は、カメラ１１、１３の撮像結果に基づいて、車両Ｖの周囲に存在する物標と車両Ｖとの間の相対位置を検出する。さらに、物標位置検出部３１は、後方カメラであるカメラ１３の撮像結果に基づいて、自車両が走行してきた方向に観測される観測物標を検出する。ここで、検出対象となる物標は、走路の周囲に存在する白線等の区画線や縁石、中央分離帯等の走路境界である。例えば、カメラ１１、１３の撮像結果である画像から画素間で輝度変化がある座標位置をエッジ点として検出して、区画線や縁石、中央分離帯等の走路境界をエッジ点群として検出する。

移動量推定部３２は、車両センサ群５のいずれかのセンサの検出結果に基づいて、単位時間における車両Ｖの移動量を推定する。自車両の移動量は、タイヤの回転数によるオドメトリの計測やジャイロと加速度センサを用いた慣性計測等を用いて計測すればよい。また、ＧＮＳＳ（全地球航法衛星システム）等の衛星からの電波を受信する方法や外界センサの計測値の変化から移動量の推定を行なうＳＬＡＭ等のさまざまな方法を用いてもよい。ただし、本実施形態では、移動量の推定方法を特に限定しない。

物標位置蓄積部３３は、物標位置検出部３１によって検出された相対位置を、移動量推定部３２によって推定された移動量だけ移動させて物標位置データとして蓄積する。この際、相対位置が検出された時刻Ｔｏを相対位置とともに蓄積する。さらに、物標位置蓄積部３３は、物標位置データを車両座標系に投影して、検出された物標の相対位置を、車両座標系の位置に変換する。車両座標系は、例えば、車両Ｖの後輪車軸中心を原点とし、前方向をｘ軸の正方向、左方向をｙ軸の正方向、上方向をｚ軸の正方向とする。尚、カメラ１１、１３の座標系から車両座標系への変換式は予め物標位置蓄積部３３に設定されている。車両座標系における路面のパラメータも同様である。

地図情報取得部３４は、自車両の自己位置を推定するために、記憶装置４から自車両の周囲の地図情報４１を取得する。特に、地図情報取得部３４は、ＧＰＳ受信機５１等から自車両の位置情報を取得し、取得した位置情報に基づいて、地図情報４１から自車両の周囲に存在する物標の物標位置情報を取得する。

物標選択部３５は、観測物標の物標位置データと、蓄積された物標位置データとを比較して、物標位置の差が所定値未満となる区間に蓄積された物標位置データを選択する。このとき、観測物標が走路境界である場合には、物標選択部３５は、観測物標として検出された走路境界と物標位置データとして蓄積された走路境界とのなす角に基づいて、物標位置の差が所定値未満となるか否かを判断する。尚、物標選択部３５は、観測物標が検出されている範囲内で、観測物標の物標位置データと、蓄積された物標位置データとを比較するようにしてもよい。これにより、例えば、後続車等によって遮蔽されている区間を除外して物標位置データの比較を行うことができるようになる。また、物標選択部３５は、物標位置の差が所定値以上となった地点の物標位置データが蓄積された時点以降に蓄積された物標位置データを選択するようにしてもよい。すなわち、物標位置の差が所定値未満となる自車両の近傍の物標位置データに限定して選択することで、簡易な方法で自己位置の推定精度を向上させるようにする。

自己位置推定部３６は、物標選択部３５によって選択された物標位置データと、地図情報４１に記録された物標の物標位置情報とを照合して車両Ｖの自己位置を推定する。

［自己位置推定方法］
次に、図３のフローチャートを参照して、本実施形態に係る自己位置推定装置により実行される自己位置推定処理の処理手順を説明する。

まず、ステップＳ１０において、周囲センサ群１のカメラ１１、１３によって車両Ｖの周囲の外界をセンシングする。

ステップＳ２０において、物標位置検出部３１は、カメラ１１、１３で撮像された画像から自車両の近傍に存在する物標の物標情報を検出する。特に、物標位置検出部３１は、カメラ１１、１３で撮像された画像から自車両の周囲に存在する物標と自車両との間の相対位置を検出する。具体的に、物標位置検出部３１は、カメラ１１、１３によって撮像された画像を取得し、白線等の区画線や縁石、中央分離帯等の走路境界を線分として検出して相対位置を求める。例えば、カメラ１１、１３により撮像された画像の輝度情報から、車両Ｖの左右に存在する白線を検出する。尚、白線等の所定距離区間毎に代表候補点を求め、その上で代表候補点の群から線分を求めても良い。また、代表候補点の群は、同時刻に検出してもよいし、異なる時刻に検出してもよい。本ステップでは、検出範囲を自車両の近傍に限定しているが、その理由は、できる限り正確な物標位置を検出するためである。尚、自車両の近傍とは、自車両の左右に隣接した白線を検出できる範囲である。さらに、本ステップでは、カメラ１１、１３で撮像された画像から物標情報を検出しているが、カメラ１１、１３のいずれか一方で撮像された画像から物標情報を検出してもよい。

ステップＳ３０において、まず移動量推定部３２が車両センサ群５の検出結果に基づいて車両Ｖの移動量を推定する。そして、物標位置蓄積部３３は、ステップＳ２０で検出された相対位置を、移動量推定部３２によって推定された移動量だけ移動させて物標位置データとして蓄積する。この際、相対位置が検出された時刻Ｔｏを相対位置とともに蓄積する。

ステップＳ４０において、物標位置蓄積部３３は、ステップＳ３０で蓄積された物標位置データを、車両位置を中心とした車両座標系に投影する。物標位置蓄積部３３は、物標の相対位置が検出された時刻Ｔｏにおける車両Ｖの位置情報Ｐｏに基づいて、ステップＳ３０で蓄積された物標位置データを、現時点での車両座標系に変換する。

ステップＳ５０において、物標位置検出部３１は、後方カメラであるカメラ１３で撮像された後方画像から自車両周囲の広範囲に存在する物標の物標情報を検出する。ここで、広範囲とは、例えば、カメラ１３で撮像された画像のうち、水平線の上側を除いた範囲とすることができる。また、画像の全範囲とすることもできる。したがって、物標位置検出部３１は、カメラ１３で撮像された画像から物標を検出し、自車両との間の相対位置を検出する。具体的に、物標位置検出部３１は、カメラ１３によって撮像された画像を取得し、画像内にある白線等の区画線や縁石、中央分離帯等の走路境界を線分として検出して相対位置を求める。特に、物標位置検出部３１は、カメラ１３で撮像された後方画像から自車両が走行してきた方向に観測される観測物標を検出し、この観測物標の物標情報及び相対位置を検出する。

ステップＳ６０において、物標選択部３５は、観測物標の物標位置データと、蓄積された物標位置データとを比較する。このとき、観測物標が走路境界である場合には、観測物標として検出された走路境界と物標位置データとして蓄積された走路境界とのなす角に基づいて比較を行う。例えば、図４は、蓄積された物標の位置を自車両の後方画像に重畳した図であり、白線６０ａ、６０ｂが撮像されている。図４に示すように、過去にカメラ１１、１３の画像から検出されて蓄積された物標の位置を、カメラ１３の画像上に投影し、画像座標系において投影された物標位置と、現在時刻のカメラ１３の画像から検出された広範囲の観測物標の位置とを比較する。図４では、蓄積された物標６２ａ、６２ｂ、６２ｃが画像上に投影され、現在時刻の観測物標６４ａ、６４ｂ、６４ｃが画像から検出されている。そして、画像から検出された観測物標の各線分６４ａ、６４ｂ、６４ｃについて、画像座標系上の距離に基づいて、最も近い蓄積された物標の線分６２ａ、６２ｂ、６２ｃを探索し、対応付けられた２つの線分のなす角θを求めて比較する。図４に示すように、車両の近傍（画像下部）では、蓄積された物標６２ａの位置は、白線６０ａにほぼ重なっているが、車両の遠方（画像上部）では、蓄積された物標６２ｃの位置と白線６０ａとの間に差が生じている。

尚、物標選択部３５は、観測物標が検出されている範囲内で、観測物標の物標位置データと、蓄積された物標位置データとを比較するようにしてもよい。これにより、例えば、後続車等によって遮蔽される区間を除外して物標位置データの比較を行うことができるようになる。

ステップＳ７０において、物標選択部３５は、物標位置の差が所定値未満となる区間に蓄積された物標位置データを選択する。このとき、観測物標が走路境界である場合には、観測物標として検出された走路境界と物標位置データとして蓄積された走路境界とのなす角に基づいて、物標位置の差が所定値未満となるか否かを判断する。例えば、図４では、観測物標の線分６４ａ、６４ｂ、６４ｃと、蓄積された物標の線分６２ａ、６２ｂ、６２ｃとのなす角θを、蓄積された物標の線分の検出時刻が新しい順に所定値と比較する。そして、なす角θが所定値未満であれば、その時刻の物標位置データを選択し、なす角θが所定値以上であれば、その時刻より過去（車両の遠方、画像上部）に蓄積された物標位置データを削除する。この結果、なす角θが所定値以上となった時点以降（車両の近傍、画像下部）に蓄積された物標位置データが選択される。例えば、図４では、観測物標の線分６４ｂと、蓄積された物標の線分６２ｂとのなす角θが所定値以上となった場合には、その時点以降に蓄積された物標（線分６２ａ）の物標位置データが選択される。つまり、なす角θが所定値以上となった地点では、オドメトリが大きくずれており、オドメトリに基づいて座標変換される物標位置は、その地点を境に不正確となる。そこで、このような不正確な物標位置データを除外し、その時点以降に蓄積された信頼できる物標位置データのみを選択する。

ステップＳ８０において、自己位置推定部３６は、ステップＳ７０で選択された物標の物標位置データと地図情報４１に含まれる物標位置情報とのマッチングを行って、自己位置の推定を実施する。地図情報取得部３４によって、地図情報４１から白線情報や走路境界情報等を含む物標位置情報が取得されると、自己位置推定部３６は、この物標位置情報と、ステップＳ７０で選択された物標の物標位置データとを比較して照合する。そして、最も整合するマップ上の位置を求める。尚、マップ上の位置を推定する手法は、自己位置推定手法として広く知られているものであればよく、ここでは特に限定しない。また、本実施形態では、物標が白線、縁石、中央分離帯等の走路境界である場合を例示して説明したが、物標がその他の路面標示（停止線、横断歩道、速度表示等）であっても適用可能である。すなわち、過去に検出された物標の物標位置データを蓄積しておき、その後に検出された物標の物標位置データと比較できるようにしておけばよい。

ステップＳ９０において、自己位置推定部３６は、ステップＳ８０で推定された自己位置の推定結果を出力する。この出力値は、例えば、ナビゲーション装置の地図上に自車両の位置を表示したり、自車両の位置から目的地までの経路を算出して経路案内を行うために使用される。また、自動運転制御で使用する場合には、地図上に自車両の位置を設定し、地図上の経路に沿って自車両の操舵制御や車速制御を行うために使用される。こうして自己位置の推定結果が出力されると、本実施形態に係る自己位置推定処理は終了する。

［第１実施形態の効果］
以上、詳細に説明したように、本実施形態に係る自己位置推定装置及びその方法では、自車両が走行してきた方向に観測される観測物標を検出し、観測物標の物標位置データと、蓄積された物標位置データとを比較する。そして、物標位置の差が所定値未満となる区間に蓄積された物標位置データを選択し、選択された物標位置データと、地図情報に含まれる物標位置情報とを照合して自車両の自己位置を推定する。これにより、観測物標と蓄積された物標の物標位置の差が所定値未満となる区間の物標位置データが選択されるので、車両が旋回してオドメトリの誤差が発生した場合でも自己位置の推定精度を向上させることができる。

また、本実施形態に係る自己位置推定装置及びその方法では、観測物標が検出されている範囲内で、観測物標の物標位置データと、蓄積された物標位置データとを比較する。これにより、例えば、後続車等によって遮蔽されている区間を除外して物標位置データの比較を行うことができるので、誤った比較を防止して自己位置の推定精度を向上させることができる。

さらに、本実施形態に係る自己位置推定装置及びその方法では、観測物標が走路境界である場合には、観測物標として検出された走路境界と物標位置データとして蓄積された走路境界とのなす角に基づいて、物標位置の差が所定値未満となるか否かを判断する。これにより、自車両の最も近傍に位置している白線等の走路境界を用いて物標位置の差を判定できるので、自己位置の推定精度を向上させることができる。

また、本実施形態に係る自己位置推定装置及びその方法では、物標位置の差が所定値以上となった地点の物標位置データが蓄積された時点以降に蓄積された物標位置データを選択する。これにより、自車両の近傍の物標位置データに限定して選択することができるので、簡易な方法で自己位置の推定精度を向上させることができる。

［第２実施形態］
以下、本発明の第２実施形態について図面を参照して説明する。図面の記載において、同一又は類似の部分には同一又は類似の符号を付し、重複する説明を省略する。

［自己位置推定システム］
図５は、本実施形態に係る自己位置推定装置を備えた自己位置推定システムの構成を示すブロック図である。図５に示すように、本実施形態に係る自己位置推定システム２００は、カメラの代わりにレーザレンジファインダ２１、２３を周囲センサ群１に備えたことが第１実施形態と相違している。

レーザレンジファインダ２１、２３は、車両周囲の外界をセンシングしてポイントクラウドを取得し、逐次、自己位置推定装置３に出力する。ポイントクラウドとは、レーザ光を照射し、その反射光から観測される反射点である観測点の集合であり、各点は３次元位置と反射強度を属性として有する。レーザレンジファインダ２１は、車両Ｖの前方及び側方の物標を検出できるように車両Ｖに搭載され、車両Ｖの前方及び側方の路面にレーザ光を照射し、その反射光からポイントクラウドを取得する。したがって、レーザレンジファインダ２１は、車両Ｖの左右両側のフロントフェンダー近傍に複数搭載されていればよい。また、レーザレンジファインダ２３は、車両Ｖの後方の物標を検出できるように車両Ｖに搭載され、車両Ｖの後方の路面にレーザ光を照射し、その反射光からポイントクラウドを取得する。尚、本実施形態では、車両Ｖの前方及び側方の物標を検出するレーザレンジファインダと車両Ｖの後方の物標を検出するレーザレンジファインダの両方を備えた場合を一例として説明するが、車両Ｖの後方の物標を検出するレーザレンジファインダだけでもよいし、２つ以上のレーザレンジファインダを備えていてもよい。

［自己位置推定方法］
次に、図６のフローチャートを参照して、本実施形態に係る自己位置推定装置により実行される自己位置推定処理の処理手順を説明する。ただし、第１実施形態と同一の処理が実行されるステップについては、重複する説明は省略する。

まず、ステップＳ１１０において、周囲センサ群１のレーザレンジファインダ２１、２３によって車両Ｖの周囲の外界をセンシングしてポイントクラウドを取得する。

ステップＳ１２０において、物標位置検出部３１は、レーザレンジファインダ２１、２３で取得したポイントクラウドから自車両の近傍に存在する物標の物標情報を検出する。特に、物標位置検出部３１は、レーザレンジファインダ２１、２３のポイントクラウドから自車両の周囲に存在する物標と自車両との間の相対位置を検出する。具体的に、物標位置検出部３１は、レーザレンジファインダ２１、２３のポイントクラウドを取得し、縁石や中央分離帯等の走路境界の段差を検出して相対位置を求める。第１実施形態では走路境界を線分として検出していたが、本実施形態では走路境界に対応したポイントクラウドを検出する。本ステップでは、検出範囲を自車両の近傍に限定しているが、その理由は、できる限り正確な物標位置を検出するためである。尚、自車両の近傍とは、自車両の左右に隣接した縁石や中央分離帯の段差を検出できる範囲である。さらに、本ステップでは、レーザレンジファインダ２１、２３のポイントクラウドから物標情報を検出しているが、レーザレンジファインダ２１、２３のいずれか一方のポイントクラウドから物標情報を検出してもよい。

ステップＳ１３０、Ｓ１４０の処理は、第１実施形態のステップＳ３０、Ｓ４０と同様の処理が実行されるので、説明は省略する。ステップＳ１５０において、物標位置検出部３１は、後方の物標を検出するためのレーザレンジファインダ２３で取得したポイントクラウドから自車両周囲の広範囲に存在する物標の物標情報を検出する。ここで、広範囲とは、例えば、レーザレンジファインダ２３からレーザ光が照射され、その反射光が観測できる範囲とすることができる。したがって、物標位置検出部３１は、レーザ光が照射され、その反射光が観測できる範囲から物標を検出し、自車両との間の相対位置を検出する。具体的に、物標位置検出部３１は、レーザレンジファインダ２３からポイントクラウドを取得し、縁石、中央分離帯等の走路境界の段差を検出して相対位置を求める。特に、物標位置検出部３１は、レーザレンジファインダ２３で取得したポイントクラウドから自車両が走行してきた方向に観測される観測物標を検出し、この観測物標の物標情報及び相対位置を検出する。

ステップＳ１６０において、物標選択部３５は、観測物標の物標位置データと、蓄積された物標位置データとを車両座標系で比較する。例えば、図７は、蓄積された物標の位置を、ポイントクラウドから検出された観測物標の位置に重畳した図である。図７に示すように、過去に検出されて蓄積された物標７０の位置と、ポイントクラウドから検出された広範囲の観測物標７２の位置が車両座標系に設定されている。そして、現在時刻の観測物標７２について、車両座標系上の距離に基づいて、最も近い蓄積された物標７０を探索し、対応付けられた２点間の距離Ｌを求める。図７に示すように、自車両Ｖの近傍では、２点間の距離Ｌは小さいが、自車両Ｖから遠方へ離れるにしたがって距離Ｌが大きくなっている。

ステップＳ１７０において、物標選択部３５は、物標位置の差が所定値未満となる区間に蓄積された物標位置データを選択する。例えば、図７では、２点間の距離Ｌを、蓄積された物標の検出時刻が新しい順に所定値と比較する。そして、距離Ｌが所定値未満であれば、その時刻の物標位置データを選択し、距離Ｌが所定値以上であれば、その時刻より過去（車両の遠方、画像下部）に蓄積された物標位置データを削除する。この結果、距離Ｌが所定値以上となった時点以降（車両の近傍、画像上部）に蓄積された物標位置データが選択される。つまり、距離Ｌが所定値以上となった地点では、オドメトリが大きくずれており、オドメトリに基づいて座標変換される物標位置は、その地点を境に不正確となる。そこで、このような不正確な物標位置データを除外し、その時点以降に蓄積された信頼できる物標位置データのみを選択する。

この後、ステップＳ１８０、Ｓ１９０の処理は、第１実施形態のステップＳ８０、Ｓ９０と同様の処理が実行され、自己位置の推定結果が出力されると、本実施形態に係る自己位置推定処理は終了する。尚、本実施形態では、縁石等の道路境界の段差のように並行して連続した物標を、ポイントクラウドを用いて検出する場合を例示して説明した。しかし、標識や看板、電柱、塀等であっても、ポイントクラウド群として検出し、マップ情報に位置や境界線、外形等を登録しておけば、道路境界等と同様に本実施形態の自己位置推定処理を適用することができる。

なお、上述の実施形態は本発明の一例である。このため、本発明は、上述の実施形態に限定されることはなく、この実施形態以外の形態であっても、本発明に係る技術的思想を逸脱しない範囲であれば、設計などに応じて種々の変更が可能であることは勿論である。

Ｖ 車両
１ 周囲センサ群
３ 自己位置推定装置
４ 記憶装置
５ 車両センサ群
１１、１３ カメラ
２１、２３ レーザレンジファインダ
３１ 物標位置検出部
３２ 移動量推定部
３３ 物標位置蓄積部
３４ 地図情報取得部
３５ 物標選択部
３６ 自己位置推定部
４１ 地図情報
５１ ＧＰＳ受信機
５２ アクセルセンサ
５３ ステアリングセンサ
５４ ブレーキセンサ
５５ 車速センサ
５６ 加速度センサ
５７ 車輪速センサ
５８ その他のセンサ
１００、２００ 自己位置推定システム

Claims (5)

1. 自車両に搭載され、自車両の周囲に存在する物標と自車両との間の相対位置を検出する物標検出センサを備え、前記物標検出センサによって検出された相対位置を自車両の移動量だけ移動させて物標位置データとして蓄積し、蓄積された前記物標位置データと、地図上に存在する物標の物標位置情報を含む地図情報とを照合して、自車両の現在位置である自己位置を推定する自己位置推定装置による自己位置推定方法であって、
   自車両が走行してきた方向に観測される観測物標を検出し、前記観測物標の物標位置データと、蓄積された物標位置データとを比較して物標位置の差が所定値未満となる区間に蓄積された物標位置データを選択し、選択された物標位置データと、前記地図情報に含まれる物標位置情報とを照合して自車両の自己位置を推定することを特徴とする自己位置推定方法。
2. 前記観測物標が検出されている範囲内で、前記観測物標の物標位置データと、蓄積された物標位置データとを比較することを特徴とする請求項１に記載の自己位置推定方法。
3. 前記観測物標が走路境界である場合には、前記観測物標として検出された走路境界と前記物標位置データとして蓄積された走路境界とのなす角に基づいて、物標位置の差が所定値未満となるか否かを判断することを特徴とする請求項１または２に記載の自己位置推定方法。
4. 前記物標位置の差が所定値以上となった地点の物標位置データが蓄積された時点以降に蓄積された物標位置データを選択することを特徴とする請求項１〜３のいずれか１項に記載の自己位置推定方法。
5. 自車両に搭載され、自車両の周囲に存在する物標と自車両との間の相対位置を検出する物標検出センサを備え、前記物標検出センサによって検出された相対位置を自車両の移動量だけ移動させて物標位置データとして蓄積し、蓄積された前記物標位置データと、地図上に存在する物標の物標位置情報を含む地図情報とを照合して、自車両の現在位置である自己位置を推定する自己位置推定装置であって、
   自車両が走行してきた方向に観測される観測物標を検出し、前記観測物標の物標位置データと、蓄積された物標位置データとを比較して物標位置の差が所定値未満となる区間に蓄積された物標位置データを選択し、選択された物標位置データと、前記地図情報に含まれる物標位置情報とを照合して自車両の自己位置を推定するコントローラを備えたことを特徴とする自己位置推定装置。

Images