JPWO2017037753A1

JPWO2017037753A1 - 車両位置推定装置、車両位置推定方法

Info

Publication number: JPWO2017037753A1
Application number: JP2017537033A
Authority: JP
Inventors: 俊弘浅井
Original assignee: Nissan Motor Co Ltd
Current assignee: Nissan Motor Co Ltd
Priority date: 2015-08-28
Filing date: 2015-08-28
Publication date: 2018-02-22
Anticipated expiration: 2035-08-28
Also published as: CN107923758A; JP6414337B2; CA2997046A1; EP3343174A1; RU2678960C1; MX364577B; EP3343174A4; KR20180041741A; KR101927038B1; WO2017037753A1; US20190033082A1; BR112018004048A2; CN107923758B; BR112018004048B1; MX2018002267A; EP3343174B1; CA2997046C; US10508923B2

Abstract

車両の周囲に存在する物標の位置を検出すると共に、車両の移動量を検出し、物標の位置を移動量に基づいて物標位置データとして蓄積する。また、物標位置データの一部を、車両の旋回状態に応じてグループ化し、物標位置データを検出した際の車両の移動量に基づいて、グループの調整範囲を設定する。そして、物標の位置を含む地図情報を取得し、物標位置データ、及び地図情報における物標の位置を、設定された調整範囲に基づいて照合することにより、車両の自己位置を推定する。

Description

本発明は、車両位置推定装置、車両位置推定方法に関するものである。

特許文献１に記載の従来技術は、自律移動する移動ロボットが、移動量に応じて自己位置を推定するものであって、レーザスキャンによって検出した通路と、予め取得している地図情報とを照合することにより、推定した自己位置の補正を行なっている。なお、検出した通路と地図情報とは、平面視の二次元データとして扱い、その照合には、現在位置から所定範囲内のデータだけを用いている。

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

移動量に誤差が発生することで、検出した通路の情報に誤差が生じた場合に、検出した通路の形状と実際の通路の形状とにずれが生じて、検出した通路の情報と地図情報と照合度合いが低くなり、自己位置を高精度に推定することができない。
本発明の課題は、自己位置の推定精度を向上させることである。

本発明の一態様に係る車両位置推定装置は、車両の周囲に存在する物標の位置を検出すると共に、車両の移動量を検出し、物標の位置を移動量に基づいて物標位置データとして蓄積する。また、物標位置データの一部を、車両の旋回状態に応じてグループ化し、物標位置データを検出した際の車両の移動量に基づいて、グループの調整範囲を設定する。そして、物標の位置を含む地図情報を取得し、物標位置データ、及び地図情報における物標の位置を、設定された調整範囲に基づいて照合することにより、車両の自己位置を推定する。

本発明によれば、物標位置データを調整範囲で調整しながら、地図情報と照合して、車両の自己位置を推定するので、その推定精度を向上させることができる。

車両位置推定装置の構成図である。レーダ装置とカメラの配置を示す図である。レーダ装置の走査範囲、及びカメラの撮像範囲を示す図である。車両座標系を示す図である。オドメトリ座標系を示す図である。車両座標系の物標位置を示す図である。走行軌跡と物標位置とを対応付けた図である。旋回地点Ｐｔの検出方法を示す図である。グループ化を示す図である。許容範囲αの設定に用いるマップである。旋回半径Ｒに応じた許容範囲β1、β２の設定に用いるマップである。調整前の物標位置データを示す図である。調整後の物標位置データを示す図である。車両位置推定処理を示すフローチャートである。直線を抽出することの効果を示す図である。走行距離ｄｍに応じた許容範囲β１、β２の設定に用いるマップである。

以下、本発明の実施形態を図面に基づいて説明する。なお、各図面は模式的なものであって、現実のものとは異なる場合がある。また、以下の実施形態は、本発明の技術的思想を具体化するための装置や方法を例示するものであり、構成を下記のものに特定するものでない。すなわち、本発明の技術的思想は、特許請求の範囲に記載された技術的範囲内において、種々の変更を加えることができる。

《第１実施形態》
《構成》
図１は、車両位置推定装置の構成図である。
車両位置推定装置１１は、車両の自己位置を推定するものであり、レーダ装置１２と、カメラ１３と、地図データベース１４と、センサ群１５と、コントローラ１６と、を備える。
図２は、レーダ装置とカメラの配置を示す図である。
図３は、レーダ装置の走査範囲、及びカメラの撮像範囲を示す図である。

レーダ装置１２は、例えばＬＲＦ（Laser Range Finder）からなり、車両２１の側方に存在する対象物の距離及び方位を測定し、測定したデータをコントローラ１６へと出力する。レーダ装置１２は、車両２１の左側面及び右側面の計二箇所に設けられている。二つのレーダ装置１２を区別する際には、車両２１の左側面に設けたレーダ装置を左側レーダ装置１２Ｌとし、車両２１の右側面に設けたレーダ装置を右側レーダ装置１２Ｒとする。左側レーダ装置１２Ｌは、下方から左方にかけてスキャンし、右側レーダ装置１２Ｒは、下方から右方にかけてスキャンする。すなわち、何れも車体前後方向の回転軸を有し、その軸直角方向にスキャンする。これにより、車体側方の路面２２や縁石２３までの距離及び方位を検出する。縁石２３は、車道と歩道との境界線として、道路の路肩に、走行車線に沿って設けられている。

カメラ１３は、例えばＣＣＤ（Charge Coupled Device）イメージセンサを用いた広角カメラからなり、車両２１の側方を撮像し、撮像したデータをコントローラ１６へと出力する。カメラ１３は、車両２１における左のドアミラー及び右のドアミラーの計二箇所に設けられている。二つのカメラ１３を区別する際には、左のドアミラーに設けたカメラを左サイドカメラ１３Ｌとし、右のドアミラーに設けたカメラを右サイドカメラ１３Ｒとする。左サイドカメラ１３Ｌは、車両２１における左方の路面２２を撮像し、右サイドカメラ１３Ｒは、車両２１における右方の路面２２を撮像する。これにより、車体側方の通行区分線２４を検出する。通行区分線２４は、車両２１が通行すべき走行車線（車両通行帯）を標示するために、路面２２にペイントされた白線などの区画線であり、走行車線に沿って標示されている。なお、図３においては、便宜的に通行区分線２４を立体的に描いているが、実際には厚みをゼロと見なすことができるため、路面２２と同一面であるものとする。

地図データベース１４は、道路地図情報を取得する。道路地図情報には、縁石２３や通行区分線２４の位置情報も含まれる。縁石２３は、高さを持つ物体であるが、平面視の二次元データとして取得されている。縁石２３や通行区分線２４は、直線の集合体としてデータ化されており、各直線は両端点の位置情報として取得され、カーブした円弧曲線は折れ線によって近似した直線として扱う。なお、地図データベース１４は、カーナビゲーションシステムで道路地図情報を記憶している記憶媒体であっても良いし、無線通信（路車間通信又は車車間通信でも可）等の通信システムを介して外部から地図情報を取得しても良い。この場合、地図データベース１４は、定期的に最新の地図情報を入手して、保有する地図情報を更新しても良い。また、地図データベース１４は、自車が実際に走行した走路を地図情報として蓄積しても良い。

センサ群１５は、例えばＧＰＳ受信機、アクセルセンサ、操舵角センサ、ブレーキセンサ、車速センサ、加速度センサ、車輪速センサ、ヨーレートセンサ等を含み、検出した各データをコントローラ１６へと出力する。ＧＰＳ受信機は、車両２１の現在位置情報を取得する。アクセルセンサは、アクセルペダルの操作量を検出する。操舵角センサは、ステアリングホイールの操作量を検出する。ブレーキセンサは、ブレーキペダルの操作量やブレーキブースタ内の圧力を検出する。車速センサは、車速を検出する。加速度センサは、車両前後方向の加減速度や横加速度を検出する。車輪速センサは、各車輪の車輪速度を検出する。ヨーレートセンサは、車両のヨーレートを検出する。

コントローラ１６は、例えばＥＣＵ（Electronic Control Unit）からなり、ＣＰＵ、ＲＯＭ、ＲＡＭ等を備え、ＲＯＭには後述する車両位置推定処理等の各種演算処理を実行するプログラムが記録されている。なお、車両位置推定処理のために専用のコントローラ１６を設けてもよいし、他の制御コントローラと兼用にしてもよい。
コントローラ１６は、機能ブロックの構成として、物標位置検出部３１と、移動量検出部３２と、物標位置蓄積部３３と、調整範囲設定部３４と、自己位置推定部３５と、を備える。

物標位置検出部３１は、車両の周囲に存在する縁石２３や通行区分線２４等の物標の位置を、車両を基準とした車両座標系で、車両に対する相対位置として検出する。
図４は、車両座標系を示す図である。
車両座標系は、平面視の二次元座標であり、車両２１の例えば後輪車軸の中心を原点Ｏとし、前後方向をＸ_ＶＨＣ軸、左右方向をＹ_ＶＨＣ軸とする。レーダ装置１２の座標系、及びカメラ１３の座標系を、夫々、車両座標系に変換する式は、予め求められている。また、車両座標系における路面２２のパラメータも既知とする。

レーダ装置１２により、路面２２を車幅方向外側に向かってレーザスキャンし、高さに大きな変化（段差）の生じる位置を、縁石２３の幅方向における車道側の端点として検出する。すなわち、三次元データから縁石２３の位置を検出し、二次元の車両座標系に投影する。ここでは、縁石２３の検出点をＰｃとし、黒塗りのひし形で表示している。
カメラ１３により、路面２２を撮像し、そのグレースケール画像において、車体左右方向に沿って暗部から明部へ、且つ明部から暗部へと輝度が変化するパターンを抽出することで、通行区分線２４を検出する。例えば、通行区分線２４における幅方向の中心点を検出する。すなわち、カメラ１３によって撮像した画像データを俯瞰変換し、俯瞰画像から通行区分線２４を検出し、車両座標系に投影する。ここでは、通行区分線２４の検出点をＰｗとし、黒丸で表示している。

移動量検出部３２は、センサ群１５で検出した各種情報から単位時間における車両２１の移動量であるオドメトリを検出する。これを積分することにより、車両の走行軌跡をオドメトリ座標系として算出することができる。
図５は、オドメトリ座標系を示す図である。
オドメトリ座標系は、例えばシステムの電源を投入した、又は切断した時点での車両位置を座標原点とし、その時点の車体姿勢（方位角）を０度とする。このオドメトリ座標系で、演算周期毎に車両の座標位置［Ｘ_ＯＤＭ，Ｙ_ＯＤＭ］、及び車体姿勢［θ_ＯＤＭ］の三つのパラメータを蓄積することで、走行軌跡を検出する。図５では、時点ｔ１〜ｔ４における車両の座標位置、及び車体姿勢を描いている。なお、現在の車両位置を原点として、蓄積されている物標位置データを、毎回、座標変換してもよい。つまり、同一座標系で物標位置データが蓄積されればよい。

物標位置蓄積部３３は、移動量検出部３２で検出した移動量に基づく走行軌跡、及び物標位置検出部３１で検出した物標の位置を対応付けてオドメトリ座標系で蓄積する。
図６は、車両座標系の物標位置を示す図である。
ここでは、時点ｔ１〜ｔ４において、物標位置検出部３１で検出した物標の、車両座標系における位置を示している。物標としては、車両２１の左方に存在する縁石２３の検出点Ｐｃと、車両２１の左方に存在する通行区分線２４の検出点Ｐｗと、車両２１の右方に存在する通行区分線２４の検出点Ｐｗと、を検出している。車両２１の変位、及び姿勢変化によって、車両座標系における各物標の位置は刻々と変化する。

図７は、車両の移動量に基づく走行軌跡と物標位置とを対応付けた図である。
すなわち、時点ｔ１〜ｔ４における車両の座標位置、及び車体姿勢に対応して、各時点における物標の位置を、オドメトリ座標系に投影している。すなわち、各時点において、車両２１の左方に存在する縁石２３の検出点Ｐｃと、車両２１の左方に存在する通行区分線２４の検出点Ｐｗと、車両２１の右方に存在する通行区分線２４の検出点Ｐｗと、を投影している。

物標位置蓄積部３３は、物標位置検出部３１で順に検出した物標において、単位時間Δｔの間に取得された点群から走行車線に沿った直線を抽出する。具体的には、オドメトリ座標系において、直線を表す［ａＸ_ＯＤＭ＋ｂＹ_ＯＤＭ＋ｃ＝０］の最適なパラメータａ、ｂ、ｃを求める。
単位時間Δｔを０.２秒とし、レーダ装置１２の動作を２５Ｈｚとし、カメラ１３の動作を３０Ｈｚとすると、単位時間Δｔの間に、縁石２３は５点分のデータ、通行区分線２４は６点分のデータが取得できる。また、各物標が車両２１の左側にあるか右側にあるかは、車両座標系のＹ_ＶＨＣ座標が正であるか負であるかで判定するものとする。こうして、車両２１の右側か左側かで点群を分けた後で、パラメータａ、ｂ、ｃを求める。

なお、直線と各検出点との距離の和が閾値以上となるときには、直線とは判定しない。一方、直線と各検出点との距離の和が閾値未満であるときには、直線と判定する。この直線に対する距離が最小になる検出点のうち、長さが最大となる二点を選び、この二点の情報を取得時刻と共に、物標位置蓄積部３３へ蓄積する。
ここでは、時点ｔ１〜ｔ４において、車両２１の左方に存在する縁石２３の検出点Ｐｃから直線Ｌ２３を抽出している。また、時点ｔ１〜ｔ４において、車両２１の左方に存在する通行区分線２４の検出点Ｐｗから直線Ｌ２４を抽出している。また、時点ｔ１〜ｔ４において、車両２１の右方に存在する通行区分線２４の検出点Ｐｗから直線Ｌ２４を抽出している。

調整範囲設定部３４は、物標位置蓄積部３３に蓄積された物標位置データの一部を、車両の旋回状態に応じてグループ化し、物標位置データを検出した際の車両の移動量に基づいて、グループの調整範囲を設定する。
先ず、移動量に基づく走行軌跡から車両の旋回地点Ｐｔを検出する。ここでは、下記の何れかの方法で旋回地点Ｐｔを検出する。
図８は、旋回地点Ｐｔの検出方法を示す図である。
図中の（ａ）では、現在位置Ｐｎと、現在位置Ｐｎから予め定めた設定距離Ｄ１だけ遡った地点Ｐｒとを直線Ｌ１で結んでいる。そして、現在位置Ｐｎから設定距離Ｄ１だけ遡った範囲で、直線Ｌ１に対して直交方向の距離ｄが予め定めた設定値ｄ１以上で且つ最も離れている地点を、旋回地点Ｐｔとして検出する。

図中の（ｂ）では、現在位置Ｐｎから遡って最初に車両の旋回角度θｔが予め定めた設定角度θ１以上となる地点を、旋回地点Ｐｎとして検出する。車両の旋回角度θｔとは、オドメトリ座標系において、現在の車体姿勢に至るまでの姿勢変化量であり、したがって現在の車体の向きを基準としたときの車体の角度差である。設定角度θ１は、例えば６０度である。
次に、旋回地点Ｐｔ以降の物標位置データをグループ化し、第一のグループＧｒ１とする。また、旋回地点Ｐｔよりも前の物標位置データをグループ化し、第二のグループＧｒ２とする。

図９は、グループ化を示す図である。
ここでは、旋回地点Ｐｔから現在位置Ｐｎまでを第一のグループＧｒ１とし、旋回地点Ｐｔよりも前の予め定めた範囲を第二のグループＧｒ２としている。
次に、グループ化した物標位置データを、地図情報に照合するために位置調整（移動）する際の許容範囲α、及び角度調整（回転）する際の許容範囲β１、β２を設定する。グループ化した物標位置データの位置［ｘｇ，ｙｇ］は、オドメトリ座標系において旋回地点Ｐｔを基点に半径αの範囲で位置調整が許容され、グループ化した物標位置データの角度φｇは、オドメトリ座標系において旋回地点Ｐｔを基点にβ１、β２の範囲で角度調整が許容される。なお、許容範囲β１、β２は、同じ角度範囲としてもよい。また、第一のグループＧｒ１のみで調整する場合には、許容範囲β２をゼロとして、許容範囲β１のみを設定してよく、第二のグループＧｒ２のみで調整する場合には、許容範囲β１をゼロとして、許容範囲β２のみを設定してもよい。

先ず、車両の移動量の走行距離が長いほど、オドメトリ情報に誤差が生じやすい。そこで、走行距離ｄｍが長いほど、許容範囲αを大きくする。
図１０は、許容範囲αの設定に用いるマップである。
このマップは、横軸を物標位置データにおける車両の移動量の走行距離ｄｍとし、縦軸を位置調整の許容範囲αとしている。走行距離ｄｍについては、０よりも大きなｄｍ１と、ｄｍ１よりも大きなｄｍ２とを予め定めている。許容範囲αについては、０よりも大きなα_ＭＩＮと、α_ＭＩＮよりも大きなα_ＭＡＸとを予め定めている。そして、走行距離ｄｍがｄｍ１からｄｍ２の範囲にあるときには、走行距離ｄｍが長いほど、許容範囲αがα_ＭＩＮからα_ＭＡＸの範囲で大きくなる。また、走行距離ｄｍがｄｍ２以上であるときには、許容範囲αがα_ＭＡＸを維持する。

また、車両の移動量の旋回量が大きいほど横すべりが大きく発生して、センサの特性によってオドメトリ情報に誤差が生じやすい。なお、車両の移動量の旋回量が大きいほど、旋回半径Ｒは小さいことを意味する。そこで、旋回半径Ｒが小さいほど、許容範囲β１、β２を大きくする。
図１１は、旋回半径Ｒに応じた許容範囲β１、β２の設定に用いるマップである。
このマップは、横軸を物標位置データにおける車両の旋回半径Ｒとし、縦軸を角度補正の許容範囲β１、β２としている。旋回半径Ｒについては、０よりも大きなＲ１と、Ｒ１よりも大きなＲ２とを予め定めている。許容範囲β１、β２については、０よりも大きなβ_ＭＩＮと、β_ＭＩＮよりも大きなβ_ＭＡＸとを予め定めている。そして、旋回半径ＲがＲ２以上であるときには、許容範囲β１、β２がβ_ＭＩＮを維持する。また、旋回半径ＲがＲ２からＲ１の範囲にあるときには、旋回半径Ｒが小さいほど、許容範囲β１、β２がβ_ＭＩＮからβ_ＭＡＸの範囲で大きくなる。

自己位置推定部３５は、物標位置蓄積部３３に蓄積された物標位置データ、及び地図データベース１４に格納された地図情報を、調整範囲設定部３４で設定した調整範囲に基づいて照合することにより、地図座標系における車両２１の自己位置を推定する。
先ず、物標位置データと地図情報とを照合する際に、旋回地点Ｐｔを支点として調整範囲内で動かしながら、最小誤差となる各グループの配置を割り出す。ここでは、各グループ内における物標位置データの相対的な位置関係は保持し、旋回地点Ｐｔを支点として、グループ間の相対関係を調整する。すなわち、第一のグループＧｒ１も第二のグループＧｒも、夫々の中身は動かさずに、第一のグループＧｒ１ごと移動及び回転させ、第二のグループＧｒ２ごと移動及び回転させる。なお、グループ化した物標位置データを地図情報に合わせる際には、前述したように、縁石２３の検出点Ｐｃから直線Ｌ２３を抽出したり、通行区分線２４の検出点Ｐｗから直線Ｌ２４を抽出したりして、これら直線Ｌ２３や直線Ｌ２４を用いて地図情報に合わせる。

図１２は、調整前の物標位置データを示す図である。
地図座標系は、平面視の二次元座標であり、東西方向をＸ_ＭＡＰ軸、南北方向をＹ_ＭＡＰ軸とする。車体姿勢（方位角）は、東方向を０度とし、反時計回りの角度として表す。ここでは、十字路の交差点を右折したときの状況を示している。右折前も右折後も、黒塗りのひし形で表示した縁石の検出点や、黒丸で表示した通行区分線の検出点が、地図座標系の縁石２３や通行区分線２４の位置と異なっている。ここでは、物標位置データを調整可能な許容範囲も示しており、物標位置データはグループごとに、調整前の旋回地点Ｐｔ０を基点に、半径αの範囲で位置調整が許容され、且つβ１、β２の範囲で角度調整が許容される。

図１３は、調整後の物標位置データを示す図である。
ここでは、旋回地点Ｐｔを支点としてグループ化した物標位置データを地図情報と照合している。すなわち、縁石の検出点や通行区分線の検出点が、地図座標系の縁石２３や通行区分線２４の位置に対して、位置ずれが最小となるように旋回地点Ｐｔを支点として各グループの配置を調整している。先ず、旋回地点をＰｔ０からＰｔ１へと移動調整しており、その調整量は、許容範囲α以内となるΔＰｔ［Δｘ，Δｙ］である。また、旋回地点Ｐｔ１以降のグループについては、旋回地点Ｐｔ１を基点に回転調整しており、その調整量は、許容範囲β１、β２以内となるΔθ１である。同様に、旋回地点Ｐｔ１より前のグループについても、旋回地点Ｐｔ１を基点に調整しており、その調整量は、許容範囲β１、β２以内となるΔθ２である。これにより、右折前の物標位置データも右折後の物標位置データも、縁石の検出点や通行区分線の検出点が、地図座標系の縁石２３や通行区分線２４の位置に略一致している。

自己位置推定部３５は、上記のように、物標位置蓄積部３３に蓄積された物標位置データ、及び地図データベース１４に格納された地図情報を照合したうえで、地図座標系における車両２１の自己位置を推定する。
地図座標系は、平面視の二次元座標であり、東西方向をＸ_ＭＡＰ軸、南北方向をＹ_ＭＡＰ軸とする。車体姿勢（方位角）は、東方向を０度とし、反時計回りの角度として表す。この地図座標系で、車両の座標位置［Ｘ_ＭＡＰ，Ｙ_ＭＡＰ］、及び車体姿勢［θ_ＭＡＰ］の三つのパラメータを推定する。この照合（マップマッチング）には、例えばＩＣＰ（Iterative Closest Point）アルゴリズムを用いる。このとき、直線同士を照合する場合には、両側の端点を評価点として照合させ、両側の端点の間隔が広いときには、補間することができる。

物標位置蓄積部３３は、自己位置推定部３５が物標位置データを許容範囲内で調整した際に、その照合度合が高いときには、蓄積されている物標位置データを、調整後の物標位置データに補正（更新）する。一方、自己位置推定部３５が物標位置データを許容範囲内で調整した際に、その照合度合が低いときには、蓄積されている物標位置データ、つまり調整前の物標位置データを維持する。

次に、コントローラ１６で所定時間（例えば１０ｍｓｅｃ）毎に実行する車両位置推定処理を説明する。
図１４は、車両位置推定処理を示すフローチャートである。
先ずステップＳ１０１は、物標位置検出部３１での処理に対応し、車両の周囲に存在する縁石２３や通行区分線２４等の物標の位置を、車両を基準とした車両座標系で、車両に対する相対位置として検出する。すなわち、レーダ装置１２で検出した縁石２３の検出点Ｐｃや、カメラ１３で検出した通行区分線２４の検出点Ｐｗを、車両座標系で検出する。

続くステップＳ１０２は、移動量検出部３２での処理に対応し、センサ群１５で検出した各種情報から単位時間における車両２１の移動量であるオドメトリを検出する。これを積分することで、車両の走行軌跡をオドメトリ座標系として算出することができる。すなわち、オドメトリ座標系で、演算周期毎に車両の座標位置［Ｘ_ＯＤＭ，Ｙ_ＯＤＭ］、及び車体姿勢［θ_ＯＤＭ］の三つのパラメータを蓄積する。
続くステップＳ１０３は、物標位置蓄積部３３での処理に対応し、移動量検出部３２で検出した移動量に基づく走行軌跡、及び物標位置検出部３１で検出した物標の位置を対応付けてオドメトリ座標系で蓄積する。すなわち、各時刻で検出した物標位置データを現在までの経過時間の車両の移動量だけ移動させ、各時刻の車両の座標位置、及び車体姿勢に対応して、夫々、縁石２３や通行区分線２４等の物標位置データを、オドメトリ座標系に投影して蓄積する。

続くステップＳ１０４は、調整範囲設定部３４での処理に対応し、物標位置蓄積部３３に蓄積された物標位置データの一部を、車両の旋回状態に応じてグループ化し、物標位置データを検出した際の車両の移動量に基づいて、グループの調整範囲を設定する。先ず、走行軌跡から車両の旋回地点Ｐｔを検出し、旋回地点Ｐｔ以降の物標位置データを第一のグループＧｒ１とする。また、旋回地点Ｐｔよりも前の物標位置データを第二のグループＧｒ２とする。次に、グループ化した物標位置データを、地図情報に合わせて位置調整する際の許容範囲α、及び角度調整する際の許容範囲β１、β２を設定する。

続くステップＳ１０５は、自己位置推定部３５での処理に対応し、蓄積された物標位置データ及び地図情報の物標の位置を調整範囲に基づいて照合することにより、地図座標系における車両２１の自己位置を推定する。先ず、物標位置データと地図情報の物標の位置とを照合する際に、旋回地点Ｐｔを支点として調整範囲内で動かしながら、最小誤差となる各グループの配置を割り出し、それから地図座標系における車両２１の自己位置を推定する。すなわち、地図座標系で、車両の座標位置［Ｘ_ＭＡＰ，Ｙ_ＭＡＰ］、及び車体姿勢［θ_ＭＡＰ］の三つのパラメータを推定する。

続くステップＳ１０６は、物標位置蓄積部３３での処理に対応し、物標位置データを適宜更新してから所定のメインプログラムに復帰する。すなわち、自己位置推定部３５が物標位置データを許容範囲内で調整した際に、その照合度合を判定する。そして、照合度合が高いときには、蓄積されている物標位置データを、調整後の物標位置データに補正（更新）する。一方、照合度合が低いときには、物標位置データの補正（更新）を行なわず、既に蓄積されている物標位置データ、つまり調整前の物標位置データを維持する。
上記が車両位置推定処理である。

《作用》
レーダ装置１２で検出した縁石２３やカメラ１３で検出した通行区分線２４等の物標の位置と、地図情報として予めデータ化された各物標の位置と、を照合することにより、車両２１の自己位置を推定する。本実施形態では、縁石２３や通行区分線２４等、検出が他の物標に比べ比較的容易で、且つ平面視の二次元データとして記述可能な物標のみで地図情報を作成し、それを用いて自己位置の推定を行う方法を例示している。なお、より自己位置の推定精度を高くする場合には、構造物の三次元（縦、横、高さ）データを有する地図情報用いてもよい。この場合でも、本実施形態を適用することができる。

また、車両２１の運動情報を用い、過去のある程度の走行軌跡をオドメトリ座標系で蓄積し、物標位置データを投影したオドメトリ座標系と、物標の位置が予め格納された地図座標系とを照合することで自己位置の推定を行なうことができる。但し、オドメトリ座標系では、走行距離が長くなるほど、旋回量が大きくなるほど、累積誤差が大きくなるという問題がある。図１２は、調整前の物標位置データを示す図である。このシーンでは、車両が右折した際に、旋回量が大きくなり、オドメトリ誤差が発生して、実際よりも大きく旋回したように走行軌跡が算出され、それに従って蓄積された物標位置データが構成する走路形状は、実際の走路形状に対して、ずれた形状となっている。
そのため、このままの物標位置データが構成する走路形状を、地図情報の走路形状と照合した場合には、照合度合いが低くなり、最小誤差が大きくなって、自己位置を高精度に推定することができない。そこで、オドメトリ座標系において、走行軌跡と物標位置とを対応付けて蓄積している物標位置データの少なくとも一部のグループ間の位置及び角度を調整しながら、地図情報の物標の位置と照合して、車両の自己位置を推定する。

次に、具体的な手順について説明する。
先ず、走行車線に沿って存在する縁石２３や通行区分線２４等の物標の位置を、車両座標系として検出し（ステップＳ１０１）、センサ群１５で検出した各種情報を積分し、車両の走行軌跡をオドメトリ座標系として検出する（ステップＳ１０２）。また、検出した走行軌跡、及び検出した物標の位置を対応付けてオドメトリ座標系で蓄積する（ステップＳ１０３）。

また、蓄積された物標位置データの一部を、車両の旋回状態に応じてグループ化し、物標位置データを検出した際の車両の移動量に基づいて、グループの調整範囲を設定する（ステップＳ１０４）。ここでは、先ず走行軌跡から車両の旋回地点Ｐｔを検出する。例えば、現在位置Ｐｎと、現在位置Ｐｎから設定距離Ｄ１だけ遡った地点Ｐｒとを直線Ｌ１で結び、現在位置Ｐｎから設定距離Ｄ１だけ遡った範囲で、直線Ｌ１に対して直交方向の距離ｄが設定値ｄ１以上で且つ最も離れている地点を、旋回地点Ｐｔとして検出する。もしくは、現在位置Ｐｎから遡って最初に車両の旋回角度θｔが設定角度θ１以上となる地点を、旋回地点Ｐｎとして検出する。このように、直線Ｌ１に対する距離ｄや旋回角度θｔを用いることで、車両の旋回地点Ｐｔを容易に検出することができる。

そして、旋回地点Ｐｔ以降の物標位置データをグループ化して第一のグループＧｒ１を設定すると共に、旋回地点Ｐｔよりも前の物標位置データをグループ化して第二のグループＧｒ２を設定する。また、グループ化した物標位置データを、地図情報に照合するために位置調整する際の許容範囲α、及び角度調整する際の許容範囲β１、β２を設定する。ここでは、蓄積された物標位置データにおける車両の移動量の走行距離ｄｍが長いほど、許容範囲αを大きくし、車両の移動量の旋回量が大きいほど（旋回半径が小さいほど）、β１、β２を大きくする。

そして、蓄積された物標位置データ及び地図情報を、旋回地点Ｐｔを支点として調整範囲α及びβ１、β２に基づいて照合することにより、地図座標系における車両２１の自己位置を推定する（ステップＳ１０５）。すなわち、地図情報に対して旋回地点Ｐｔを支点として物標位置データを調整範囲内で動かしながら、最小誤差となる各グループの配置を求めたうえで、車両２１の自己位置を推定するので、オドメトリ情報の累積誤差の影響を抑制することができ、自己位置の推定精度を向上させることができる。すなわち、グループ化した物標位置データの位置［ｘｇ，ｙｇ］、及び角度φｇの三つの自由度を与えて旋回地点Ｐｔを支点として地図情報に照合させていることになるので、車両の座標位置［Ｘ_ＭＡＰ，Ｙ_ＭＡＰ］、及び車体姿勢［θ_ＭＡＰ］の推定精度が向上する。

また、特に旋回走行中は、車両の横すべりが発生するため、オドメトリ情報に誤差が生じやすい。したがって、上記のように旋回地点Ｐｔを検出し、旋回地点Ｐｔの前後でグループ分けし、そのグループごとに旋回地点Ｐｔを支点として位置調整及び角度調整を行なうことで、オドメトリ情報の誤差を効果的に排除することができる。一方、物標位置データの調整は、旋回地点Ｐｔを支点として許容範囲α及びβ１、β２内で行なうため、動かし過ぎることを抑制し、適正な範囲で補正することができる。走行距離ｄｍが長いほど、オドメトリ情報の誤差が大きくなりやすいので、走行距離ｄｍに応じて許容範囲αを設定し、車両の移動量の旋回量に応じて許容範囲β１、β２を設定することで、適正な許容範囲α及びβ１、β２を設定し、効果的な位置調整及び角度調整を行なうことができる。

グループ化した物標位置データのグループ間の位置及び角度を調整しながら、地図情報と照合する際には、グループ内の縁石２３の検出点Ｐｃから直線Ｌ２３を抽出したり、通行区分線２４の検出点Ｐｗから直線Ｌ２４を抽出したりして、これら直線Ｌ２３や直線Ｌ２４を用いて地図情報と照合する。
図１５は、直線を抽出することの効果を示す図である。
図中の（ａ）は、直線を抽出せず、黒塗りのひし形で表示した縁石２３の検出点Ｐｃのまま地図情報に照合させた場合を示す。Ａの領域にある検出点Ｐｃは、縁石２３ｘのラインに照合させなければならないところを、最も近い縁石２３ｙのラインに照合させてしまっている。図中の（ｂ）は、直線Ｌ２３を抽出したことで、縁石２３ｘのラインに照合させることができる。このように、直線を抽出し、この直線を用いて地図情報に照合させることで、自己位置の推定精度を向上させることができる。

なお、物標位置データを許容範囲内で調整した際に、その照合度合が高いときには、蓄積されている物標位置データを、調整後の物標位置データに補正（更新）する（ステップＳ１０６）。これにより、精度の高い物標位置データを蓄積することができる。逆に、照合度合が低いときには、蓄積されている物標位置データ、つまり調整前の物標位置データを維持する。これにより、精度の低い物標位置データを蓄積してしまう、といった事態を抑制できる。

《変形例》
第１実施形態では、旋回地点Ｐｔ以降の物標位置データをグループ化すると共に、旋回地点Ｐｔより前の物標位置データをグループ化し、夫々、グループ化した物標位置データを調整しているが、これに限定されるものではない。すなわち、旋回地点Ｐｔを通過する前の時点で、既に旋回地点Ｐｔより前の物標位置データは、地図情報との照合を経て調整済みであるなら、旋回地点Ｐｔを通過した後に、さらに地図情報との照合を経て調整する必要はない。したがって、少なくとも旋回地点Ｐｔ以降の物標位置データだけをグループ化し、旋回地点Ｐｔを支点としてグループの位置及び角度を調整しながら、地図情報と照合してもよい。すなわち、旋回地点Ｐｔより前の物標位置データである第二のグループＧｒ２は固定し、旋回地点Ｐｔ以降の物標位置データである第一のグループＧｒ１だけを調整してもよい。これにより、演算負荷を抑制し、より効率的に照合を行なうことができる。

《応用例》
第１実施形態では、車両の移動量の旋回量に応じて角度補正の許容範囲β１、β２を設定しているが、これに限定されるものではない。車両の速度変化が発生するときに、センサの特性によってオドメトリ情報に誤差が生じやすく、走行距離が長くなるほど、この誤差が蓄積するため、車両の走行距離ｄｍに応じて角度補正の許容範囲β１、β２を設定してもよい。
図１６は、走行距離ｄｍに応じた許容範囲β１、β２の設定に用いるマップである。
このマップは、横軸を物標位置データにおける車両の移動量の移動距離ｄｍとし、縦軸を角度調整の許容範囲β１、β２としている。移動距離ｄｍについては、０よりも大きなｄｍ１と、ｄｍ１よりも大きなｄｍ２とを予め定めている。許容範囲β１、β２については、０よりも大きなβ_ＭＩＮと、β_ＭＩＮよりも大きなβ_ＭＡＸとを予め定めている。そして、移動距離ｄｍがｄｍ１からｄｍ２の範囲にあるときには、移動距離ｄｍが長いほど、許容範囲βがβ_ＭＩＮからβ_ＭＡＸの範囲で大きくなる。また、移動距離ｄｍがｄｍ２以上であるときには、許容範囲β１、β２がβ_ＭＡＸを維持する。
このように、移動距離ｄｍに応じて角度補正の許容範囲β１、β２を設定することで、適正な許容範囲β１、β２を設定し、効果的な角度調整を行なうことができる。
なお、物標位置データにおける車両の移動量の旋回量と移動距離ｄｍの両者を加味して、許容範囲β１、β２を設定するようにしても良い。また、車両の移動量として加減速度も加味するようにしても良い。

《対応関係》
第１実施形態では、物標位置検出部３１、及びステップＳ１０１の処理が「物標位置検出部」に対応する。移動量検出部３２、及びステップＳ１０２の処理が「走行軌跡検出部」に対応する。物標位置蓄積部３３、及びステップＳ１０３、Ｓ１０６の処理が「物標位置データ蓄積部」に対応する。地図データベース１４が「地図情報蓄積部」に対応する。調整範囲設定部３４、及びステップＳ１０４の処理が「物標位置データ補正部」に対応する。自己位置推定部３５、及びステップＳ１０５の処理が「自己位置推定部」に対応する。

《効果》
次に、第１実施形態における主要部の効果を記す。
（１）第１実施形態に係る車両位置推定装置は、車両の周囲に存在する物標の位置を検出すると共に、車両の移動量を検出し、物標の位置を移動量に基づいて物標位置データとして蓄積する。また、物標位置データの一部を、車両の旋回状態に応じてグループ化し、物標位置データを検出した際の車両の移動量に基づいてグループの調整範囲を設定する。そして、物標の位置を含む地図情報を取得し、物標位置データ、及び地図情報における物標の位置を、設定された調整範囲に基づいて照合することにより、車両の自己位置を推定する。
このように、物標位置データを調整範囲で調整しながら、地図情報と照合して、車両の自己位置を推定するので、その推定精度を向上させることができる。

（２）第１実施形態に係る車両位置推定装置は、グループ間の位置範囲及び角度範囲として、調整範囲を設定する。
このように、グループ間の位置及び角度を調整することにより、オドメトリ情報の誤差の影響を抑制することができる。

（３）第１実施形態に係る車両位置推定装置は、蓄積されている物標位置データにおける車両の移動量が大きいほど、許容範囲α及びβ１、β２を大きくする。
このように、車両の移動量（旋回半径Ｒや走行距離ｄｍ）に応じて許容範囲α及びβ１、β２を設定することで、適正な許容範囲を設定し、効果的な位置調整及び角度調整を行なうことができる。

（４）第１実施形態に係る車両位置推定装置は、車両の移動量に基づく走行軌跡から車両の旋回地点Ｐｔを検出し、旋回地点Ｐｔに基づいて物標位置データをグループ化する。
このように、旋回地点Ｐｔを検出し、旋回地点Ｐｔに基づいて物標位置データをグループ化することで、旋回地点Ｐｔを支点として、オドメトリ情報の誤差を効率的に排除することができる。

（５）第１実施形態に係る車両位置推定装置は、地図情報における物標の位置と照合された場合、照合結果に基づいて物標位置データを補正（更新）する。
このように、照合結果に基づいて物標位置データを補正するか否かを判断するので、精度の高い物標位置データを蓄積することができる。

（６）第１実施形態に係る車両位置推定装置は、物標位置データから直線を抽出し、抽出した直線と地図情報における物標の位置とを照合する。
このように、直線を抽出し、抽出した直線を用いて地図情報に合わせることにより、照合精度を向上させることができる。

（７）第１実施形態に係る車両位置推定方法は、車両の周囲に存在する物標の位置を検出すると共に、車両の移動量を検出し、物標の位置を移動量に基づいて物標位置データとして蓄積する。また、物標位置データの一部を、車両の旋回状態に応じてグループ化し、物標位置データを検出した際の車両の移動量に基づいてグループの調整範囲を設定する。そして、物標の位置を含む地図情報を取得し、物標位置データ、及び地図情報における物標の位置を、設定された調整範囲に基づいて照合することにより、車両の自己位置を推定する。
このように、物標位置データを調整範囲で調整しながら、地図情報と照合して、車両の自己位置を推定するので、その推定精度を向上させることができる。

以上、限られた数の実施形態を参照しながら説明したが、権利範囲はそれらに限定されるものではなく、上記の開示に基づく実施形態の改変は、当業者にとって自明のことである。

１１車両位置推定装置
１２レーダ装置
１３カメラ
１４地図データベース
１５センサ群
１６コントローラ
２１車両
２２路面
２３縁石
２４通行区分線
３１物標位置検出部
３２移動量検出部
３３物標位置蓄積部
３４調整範囲設定部
３５自己位置推定部

Claims

車両の周囲に存在する物標の位置を検出する物標位置検出部と、
前記車両の移動量を検出する移動量検出部と、
前記物標位置検出部で検出した物標の位置を、前記移動量検出部により検出された移動量に基づいて、物標位置データとして蓄積する物標位置蓄積部と、
前記物標の位置を含む地図情報を取得する地図情報取得部と、
前記物標位置蓄積部に蓄積された物標位置データの一部を、車両の旋回状態に応じてグループ化し、前記物標位置データを検出した際の車両の移動量に基づいて、前記グループの調整範囲を設定する調整範囲設定部と、
前記物標位置蓄積部に蓄積された物標位置データ、及び前記地図情報取得部で取得した地図情報における物標の位置を、前記調整範囲設定部で設定した調整範囲に基づいて照合することにより、前記車両の自己位置を推定する自己位置推定部と、を備えることを特徴とする車両位置推定装置。
前記調整範囲設定部は、
前記グループ間の位置範囲及び角度範囲として、前記調整範囲を設定することを特徴とする請求項１に記載の車両位置推定装置。
前記調整範囲設定部は、
前記物標位置蓄積部で蓄積されている物標位置データにおける前記車両の移動量が大きいほど、前記調整範囲を大きくすることを特徴とする請求項１又は２に記載の車両位置推定装置。
前記調整範囲設定部は、
前記車両の移動量に基づく走行軌跡から車両の旋回地点を検出し、前記旋回地点に基づいて前記物標位置データをグループ化することを特徴とする請求項１〜３の何れか一項に記載の車両位置推定装置。
前記物標位置蓄積部は、
前記自己位置推定部で前記地図情報における物標の位置と照合された場合、照合結果に基づいて前記物標位置データを補正することを特徴とする請求項１〜４の何れか一項に記載の車両位置推定装置。
前記自己位置推定部は、
前記物標位置データから直線を抽出し、抽出した直線と前記地図情報における物標の位置と照合することを特徴とする請求項１〜５の何れか一項に記載の車両位置推定装置。
物標位置検出部が、車両の周囲に存在する物標の位置を検出し、
移動量検出部が、前記車両の移動量を検出し、
物標位置蓄積部が、前記物標位置検出部で検出した物標の位置を、前記移動量検出部により検出された移動量に基づいて物標位置データとして蓄積し、
地図情報取得部が、前記物標の位置を含む地図情報を取得し、
調整範囲設定部が、前記物標位置蓄積部に蓄積された物標位置データの一部を、車両の旋回状態に応じてグループ化し、前記物標位置データを検出した際の車両の移動量に基づいて、前記グループの調整範囲を設定し、
自己位置推定部が、前記物標位置蓄積部に蓄積された物標位置データ、及び前記地図情報取得部で取得した地図情報における物標の位置を、前記調整範囲設定部で設定した調整範囲に基づいて照合することにより、前記車両の自己位置を推定することを特徴とする車両位置推定方法。