特許 5966747 車両走行制御装置及びその方法

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】特許公報(B2)

(11)【特許番号】5966747

(24)【登録日】2016年7月15日

(45)【発行日】2016年8月10日

(54)【発明の名称】車両走行制御装置及びその方法

(51)【国際特許分類】

   B60R 21/00 20060101AFI20160728BHJP

   G08G 1/16 20060101ALI20160728BHJP

   G01C 21/26 20060101ALI20160728BHJP

   B60W 30/06 20060101ALI20160728BHJP

   B60W 40/02 20060101ALI20160728BHJP

   B62D 6/00 20060101ALI20160728BHJP

   B62D 101/00 20060101ALN20160728BHJP

   B62D 111/00 20060101ALN20160728BHJP

   B62D 113/00 20060101ALN20160728BHJP

   B62D 137/00 20060101ALN20160728BHJP

【ＦＩ】

   B60R21/00 628D

   G08G1/16 CZYW

   G01C21/26 A

   B60W30/06

   B60W40/02

   B62D6/00

   B60R21/00 624C

   B62D101:00

   B62D111:00

   B62D113:00

   B62D137:00

【請求項の数】7

【全頁数】19

(21)【出願番号】特願2012-175547(P2012-175547)

(22)【出願日】2012年8月8日

(65)【公開番号】特開2014-34251(P2014-34251A)

(43)【公開日】2014年2月24日

【審査請求日】2015年6月29日

(73)【特許権者】

【識別番号】000003997

【氏名又は名称】日産自動車株式会社

(74)【代理人】

【識別番号】100083806

【弁理士】

【氏名又は名称】三好 秀和

(74)【代理人】

【識別番号】100100712

【弁理士】

【氏名又は名称】岩▲崎▼ 幸邦

(74)【代理人】

【識別番号】100101247

【弁理士】

【氏名又は名称】高橋 俊一

(74)【代理人】

【識別番号】100095500

【弁理士】

【氏名又は名称】伊藤 正和

(74)【代理人】

【識別番号】100098327

【弁理士】

【氏名又は名称】高松 俊雄

(72)【発明者】

【氏名】西羅 光

(72)【発明者】

【氏名】藤木 教彰

【審査官】 飯島 尚郎

(56)【参考文献】

【文献】 特開２００５−２６５４９４（ＪＰ，Ａ）

【文献】 特開２００５−３１３７１０（ＪＰ，Ａ）

【文献】 特開２０００−３１０５４１（ＪＰ，Ａ）

【文献】 特開２００３−２７９３６１（ＪＰ，Ａ）

【文献】 特開２０１２−１２７８９６（ＪＰ，Ａ）

【文献】 特開昭６４−０１２２１８（ＪＰ，Ａ）

【文献】 特開２００４−２８６５１８（ＪＰ，Ａ）

(58)【調査した分野】（Int.Cl.，ＤＢ名）

Ｂ６０Ｒ ２１／００

Ｂ６０Ｗ ３０／０６

Ｂ６０Ｗ ４０／０２

Ｂ６２Ｄ ６／００

Ｇ０１Ｃ ２１／２６

Ｇ０８Ｇ １／１６

Ｂ６２Ｄ １０１／００

Ｂ６２Ｄ １１１／００

Ｂ６２Ｄ １１３／００

Ｂ６２Ｄ １３７／００

(57)【特許請求の範囲】

【請求項1】

自車の周囲環境の画像を撮影する周囲環境撮影手段と、
自車の操作量及び運動状態を検出する自車状態検出手段と、
自車が走行可能な道路の線形情報及び道路周辺の構造物の位置情報が取得された地図情報に基づき、自車の出発地点から移動目標地点に至る目標走行経路を算出する走行経路算出手段と、
前記周囲環境撮影手段の出力に基づき地上固定座標系上での自車の位置と姿勢の推定値及び推定精度を算出する第１の自車位置推定手段と、
前記自車状態検出手段の出力に基づき推定される自車位置の変化量を積算することで基準点及び方位と自車位置との相対的な位置関係を推定する第２の自車位置推定手段と、
前記目標走行経路上で前記第１の自車位置推定手段による位置推定の難度が高い区間を推定する自車位置難度区間推定手段と、
前記自車位置難度区間推定手段で難度が高いと推定された区間の手前側領域内で、前記周囲環境撮影手段の撮影範囲に入る可能性が高いランドマークを前記地図情報から検出するランドマーク検出手段と、
前記ランドマーク検出手段で検出されたランドマークに基づき位置推定難度の高い区間を走行中、前記第１の自車位置推定手段から前記第２の自車位置推定手段に切り替える自車位置推定切替手段と、
位置推定難度の高い区間を走行中、前記自車位置推定切替手段の結果に基づく前記第２の自車位置推定手段の出力に基づき自車の走行制御を実行し、位置推定難度の高い区間以外の区間では前記自車位置推定切替手段の結果に基づく前記第１の自車位置推定手段の出力に基づき自車の走行制御を実行する自動走行制御手段と、
を備えることを特徴とする車両走行制御装置。

【請求項2】

前記ランドマーク検出手段は、前記第１の自車位置推定手段の位置推定精度が所定の水準になるまで、検出されたランドマークが前記周囲環境撮影手段の撮影範囲内に入るように自車位置を補正し、
前記自車位置推定切替手段は、前記ランドマーク検出手段が起動された状態で前記第１の自車位置推定手段の位置推定精度が前記所定の水準以上である場合には、前記ランドマーク検出手段の起動を解除し、位置推定難度の高い区間を走行中、前記第１の自車位置推定手段から前記第２の自車位置推定手段に切り替えることを特徴とする請求項１記載の車両走行制御装置。

【請求項3】

前記第１の自車位置推定手段は、前記周囲環境撮影手段で撮影された自車の周囲環境の撮影画像と、前記地図情報を仮想位置及び仮想姿勢角から撮影した仮想画像とを比較することにより、実際の車両の姿勢角及び位置を推定することを特徴とする請求項１又は請求項２記載の車両走行制御装置。

【請求項4】

前記第１の自車位置推定手段は、前記撮影画像と前記仮想画像とを比較し、前記撮影画像内の遠方位置画素と前記仮想画像内の遠方位置画素とが一致した場合には前記仮想画像の姿勢角尤度を高くし、前記撮影画像内の近傍位置画素と前記仮想画像内の近傍位置画素とが一致した場合には前記仮想画像の位置尤度を高くする尤度設定手段と、
前記尤度設定手段により設定された前記仮想画像の姿勢角尤度に基づき当該仮想画像の仮想姿勢角に対する実際の自車の姿勢角を推定し、前記尤度設定手段により設定された前記仮想画像の位置尤度に基づき当該仮想画像の仮想位置に対する実際の自車の位置を推定する自車位置姿勢推定手段と、
を備えることを特徴とする請求項３記載の車両走行制御装置。

【請求項5】

前記ランドマーク検出手段は、前記ランドマークを検出したときには、自車を一時停止させることを特徴とする請求項１乃至請求項４のいずれか１項記載の車両走行制御装置。

【請求項6】

前記ランドマーク検索手段は、前記地図情報から所定の条件を満たす複数のランドマークを検索し、検索された複数のランドマークから状況に適したランドマークを選択し、選択されたランドマークを前記ランドマーク検出走行制御手段の目標ランドマークとして使用することを特徴とする請求項１乃至請求項５のいずれか１項記載の車両走行制御装置。

【請求項7】

周囲環境撮影手段により自車の周囲環境の画像を撮影するステップと、
自車状態検出手段により自車の操作量及び運動状態を検出するステップと、
自車が走行可能な道路の線形情報及び道路周辺の構造物の位置情報が取得された地図情報に基づき自車の出発地点から移動目標地点に至る目標走行経路を算出するステップと、
第１の自車位置推定手段により前記周囲環境撮影手段の出力に基づき地上固定座標系上での自車の位置と姿勢の推定値及び推定精度を推定するステップと、
第２の自車位置推定手段により前記自車状態検出手段の出力に基づき推定される自車位置の変化量を積算することで基準点及び方位と自車位置との相対的な位置関係を推定するステップと、
前記目標走行経路上で前記第１の自車位置推定手段による位置推定の難度が高い区間を推定するステップと、
難度が高いと推定された区間の手前側領域内で前記周囲環境撮影手段の撮影範囲に入る可能性が高いランドマークを前記地図情報から検出するステップと、
検出されたランドマークに基づき位置推定難度の高い区間を走行中、前記第１の自車位置推定手段から前記第２の自車位置推定手段に切り替える自車位置推定切替ステップと、
位置推定難度の高い区間を走行中、前記自車位置推定切替ステップの結果に基づく前記第２の自車位置推定手段の出力に基づき自車の走行制御を実行し、位置推定難度の高い区間以外の区間では前記自車位置推定切替ステップの結果に基づく前記第１の自車位置推定手段の出力に基づき自車の走行制御を実行するステップと、
を含むことを特徴とする車両走行制御方法。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、車両を目標走行経路に追従させる車両走行制御装置及びその方法に関し、特に、駐車・狭路通過等の高難度区間を走行する際にも、安定した推定精度を得る車両走行制御装置及びその方法に関する。

【背景技術】

【0002】

従来、この種の技術として、例えば特許文献１に記載された駐車支援装置が知られている。この駐車支援装置は、駐車位置に設置されたマークを撮影するカメラを備え、検出されたマークと自車現在位置との相対位置関係を画像処理によって認識して駐車支援を行う。

【0003】

駐車支援装置は、駐車開始位置から駐車完了位置までの理想走行経路を予め車両を走行させることで記録する。理想走行経路を記録した後に、設定された駐車位置に駐車する場合に、駐車開始時に設置されたマークをカメラで検出して自車位置を算出する。算出された自車位置と駐車開始位置との誤差を推定し、推定された誤差に基づき理想走行経路を補正して駐車誘導経路を算出して自動駐車を実行する。

【先行技術文献】

【特許文献】

【0004】

【特許文献1】特開2008-174000号公報

【発明の概要】

【発明が解決しようとする課題】

【0005】

しかしながら、特許文献１に記載された駐車支援装置においては、予め駐車位置にマークを設置する必要があった。また、過去に一度、駐車した場所でないと、自動駐車させることができなかった。また、マークがカメラ視野内に入るまでは、ドライバーが運転するか、あるいは別の方法で車両を誘導する必要があった。

【0006】

即ち、駐車・狭路通過等の高難度区間を走行する際には、安定した推定精度を得ることができず、高精度に自動駐車させることができなかった。

【0007】

そこで、本発明は、上述した実情に鑑みて提案されたものであり、駐車・狭路通過等の高難度区間を走行する際にも、安定した推定精度を得ることができる車両走行制御装置及びその方法を提供することを目的とする。

【課題を解決するための手段】

【0008】

本発明は、第１の自車位置推定部により自車の周囲環境の画像に基づき地上固定座標系上での自車の位置と姿勢の推定値及び推定精度を推定する。第２の自車位置推定部により自車の操作量及び運動状態に基づき推定される自車位置の変化量を積算することで基準点及び方位と自車位置との相対的な位置関係を推定し、目標走行経路上で第１の自車位置推定部による位置推定の難度が高い区間を推定する。難度が高いと推定された区間の手前側領域内で周囲環境撮影部の撮影範囲に入る可能性が高いランドマークを地図情報から検出する。検出されたランドマークに基づき位置推定難度の高い区間を走行中、自車位置推定切替部により第１の自車位置推定部から第２の自車位置推定部に切り替える。位置推定難度の高い区間を走行中、自車位置推定切替部の結果に基づく第２の自車位置推定部の出力に基づき自車の走行制御を実行し、位置推定難度の高い区間以外の区間では自車位置推定切替部の結果に基づく第１の自車位置推定部の出力に基づき自車の走行制御を実行する。

【発明の効果】

【0009】

本発明によれば、位置推定難度の高い区間を走行中、第１の自車位置推定部から第２の自車位置推定部に切り替え、位置推定難度の高い区間を走行中、自車位置推定切替部の結果に基づく第２の自車位置推定部の出力に基づき自車の走行制御を実行する。位置推定難度の高い区間以外の区間では自車位置推定切替部の結果に基づく第１の自車位置推定部の出力に基づき自車の走行制御を実行する。従って、駐車・狭路通過等の高難度区間を走行する際にも、安定した推定精度を得ることができる

【図面の簡単な説明】

【0010】

【図1】本発明の第１の実施形態の車両走行制御装置の構成を示すブロック図である。

【図2】本発明の第１の実施形態の車両走行制御装置が適用される例を示す図である。

【図3】図２に示す第１の実施形態の車両走行制御装置が適用される例において性質の異なる２つの自己位置推定方法をランドマークの検出により使い分ける例を示す図である。

【図4】本発明の第１の実施形態の車両走行制御装置の動作手順を示すフローチャートである。

【図5】本発明の第２の実施形態の自車位置姿勢推定装置の構成を示すブロック図である。

【図6】（ａ）は撮影画像、（ｂ）は（ａ）のエッジ画像、（ｃ）は仮想画像、（ｄ）は（ｃ）の仮想位置が右方向にずれた場合の仮想画像である。

【図7】本発明の第２の実施形態の自車位置姿勢推定装置の動作手順を示すフローチャートである。

【図8】本発明の第２の実施形態の自車位置姿勢推定装置においてパーティクルを移動させる動作を説明する説明図である。

【図9】本発明の第２の実施形態の自車位置姿勢推定装置において、車両からの距離と尤度like_pとの関係を示す図である。

【図10】本発明の第２の実施形態の自車位置姿勢推定装置において、車両からの距離と尤度like_aとの関係を示す図である。

【発明を実施するための形態】

【0011】

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

【0012】

〔第１の実施形態〕
図１は、本発明の第１の実施形態の車両走行制御装置の構成を示すブロック図である。車両走行制御装置は、周囲環境撮影部１、自車状態検出部２、対地自車位置推定部３、相対自車位置推定部４、地図データベース（地図ＤＢ）５、走行経路算出部６を備える。また、車両走行制御装置は、自動走行制御部７、自車位置難度区間推定部８、ランドマーク検索部９、ランドマーク検出走行制御部１０、自車位置推定切替部１１を備える。

【0013】

周囲環境撮影部１は、自車の周囲環境の画像を撮影し、撮影された画像を対地自車位置推定部３に出力するもので、カメラ等である。自車状態検出部２は、自車の操作量及び運動状態を検出し、検出値を相対自車位置推定部４に出力するもので、エンコーダ、速度センサ等である。

【0014】

対地自車位置推定部３は、第１の自車位置推定手段に対応し、カメラを用いたＶＭＭ（Visual Map Matching）で実現され、高難度区間として駐車エリアやランドマークが乏しい狭路区間を想定する。対地自車位置推定部３は、周囲環境撮影部１で検出された自車の周囲環境の画像に基づき、地上固定座標系上での自車の位置と姿勢の推定値及び推定精度を算出する。対地自車位置推定部３は、地図データが利用可能な走行区間では、継続的に自車位置を推定できるが、高難度区間では位置推定精度が低下する。

【0015】

相対自車位置推定部４は、第２の自車位置推定手段に対応し、自車状態検出部２の検出値に基づき推定される自車位置の変化量を積算することで、基準点及び方位と自車位置との相対的な位置関係を推定する。相対自車位置推定部４は、場所によらず高い位置推定精度が期待できるが、走行距離とともに誤差が積算されるので、継続的な使用が困難である。

【0016】

地図ＤＢ５は、自車が走行可能な道路の線形情報及び道路周辺の構造物の位置情報を地図情報として取得して記憶するもので、ハードディスク、メモリなどの記憶媒体からなる。走行経路算出部６は、地図ＤＢ５に記憶された地図情報に基づき、自車の出発地点から移動目標地点に至る目標走行経路を算出するもので、ナビゲーション等である。

【0017】

自動走行制御部７は、走行経路算出部６で算出された目標走行経路に自車が追従するように操舵系及び制駆動系を自動で制御する。自車位置難度区間推定部８は、走行経路算出部６で算出された目標走行経路上で対地自車位置推定部３による位置推定の難度が高い区間を推定する。

【0018】

ランドマーク検索部９は、ランドマーク検出手段に対応し、自車位置難度区間推定部８で難度が高いと推定された区間の手前側領域内で、周囲環境撮影部１の撮影範囲に入る可能性が高いランドマークを地図ＤＢ５の地図情報から検出する。

【0019】

ランドマーク検出走行制御部１０は、ランドマーク検出手段に対応し、対地自車位置推定部３による自車位置推定精度が所定の水準になるまで、検出されたランドマークが周囲環境撮影部１の撮影範囲内に入るように自車位置を補正するように制御する。

【0020】

自車位置推定切替部１１は、ランドマーク検出走行制御部１０が起動された状態で対地自車位置推定部３の位置推定精度が所定の水準以上になった場合には、ランドマーク検出走行制御部１０の起動を解除する。自車位置推定切替部１１は、位置推定難度の高い区間を走行中、対地自車位置推定部３から相対自車位置推定部４に切り替える。

【0021】

自動走行制御部７は、位置推定難度の高い区間を走行中、自車位置推定切替部１１の結果に基づく相対自車位置推定部４の出力に基づき自車を走行制御し、位置推定難度の高い区間以外の区間では自車位置推定切替部１１の結果に基づく対地自車位置推定部３の出力に基づき自車を走行制御する。

【0022】

対地自車位置推定部３、相対自車位置推定部４、走行経路算出部６、自動走行制御部７、自車位置難度区間推定部８、ランドマーク検索部９、ランドマーク検出走行制御部１０、自車位置推定切替部１１はＥＣＵが自動走行制御用プログラムを実行して実現される。

【0023】

次にこのように構成された第１の実施形態の車両走行制御装置の動作を図２乃至図４を参照しながら詳細に説明する。図２は、本発明の第１の実施形態の車両走行制御装置が適用される例を示す図である。図３は、図２に示す第１の実施形態の車両走行制御装置が適用される例において性質の異なる２つの自己位置推定方法をランドマークの検出により使い分ける例を示す図である。図４は、本発明の第１の実施形態の車両走行制御装置の動作手順を示すフローチャートである。

【0024】

まず、ステップＳ１において、操作部により目的地が入力される。ステップＳ２において、走行経路算出部６は、入力された目的地と、地図ＤＢ５からの地図情報とに基づき、自車の出発地から目的地に至る目標走行経路を算出する。

【0025】

次に、ステップＳ３において、自車位置難度区間推定部８は、走行経路算出部６で算出された目標走行経路上で対地自車位置推定部３による位置推定の難度が高い区間を推定して抽出する。ステップＳ４において、ランドマーク検索部９は、自車位置難度区間推定部８で難度が高いと推定された区間の手前側領域内で、周囲環境撮影部１の撮影範囲に入る可能性が高いランドマークを地図ＤＢ５から地図情報から検出する。ステップＳ５において、自車の走行を開始する。

【0026】

次に、対地自車位置推定部３は、周囲環境撮影部１で検出された自車の周囲環境の画像に基づき、地上固定座標系上での自車の位置と姿勢の推定値及び推定精度を算出する。相対自車位置推定部４は、自車状態検出部２の検出値に基づき推定される自車位置の変化量を積算することで、基準点及び方位と自車位置との相対的な位置関係を推定する。ステップＳ６において、ＥＣＵは、対地自車位置推定部３及び相対自車位置推定部４の自車位置推定結果を取得する。

【0027】

次に、ステップＳ７において、ＥＣＵは自車が目的地に到着したかどうかを判定し、ステップＳ８において、自車が目的地に到着していない場合には、ＥＣＵは自車が高難度区間を走行中かどうかを判定する。

【0028】

ステップＳ９において、ＥＣＵは自車が高難度区間を走行中でない場合には、自車位置推定切替部１１は、相対自車位置推定部４から対地自車位置推定部３に切り替える。ステップＳ１０において、自動走行制御部７は、自車位置推定切替部１１により切り替えられた対地自車位置推定部３からの自車位置に基づき、自車の自動走行を制御するための走行制御指令値を算出する。

【0029】

図２に示す適用例では、自車１３が道路１２ａ上を走行しているときには（図３の時刻ｔ１前）、対地自車位置推定部３により、それなりの推定位置精度で自車位置が推定される。

【0030】

一方、ステップＳ８において、ＥＣＵは自車が高難度区間を走行中である場合には、ステップＳ１１において、前方に高難度区間が接近したかどうかを判定する。ステップＳ１２において、高難度区間が接近した場合には、ランドマーク検出走行制御部１０に切り替えられて、ランドマーク検出走行制御部１０が起動する。

【0031】

ステップＳ１３において、ランドマーク検出走行制御部１０は、ランドマーク検索部９で検索されたランドマークを検出したかどうかを判定する。ステップＳ１４において、ランドマーク検出走行制御部１０は、ランドマークが検出された場合、対地自車位置推定部３による自車位置推定精度が所定水準以上がどうかを判定する。即ち自車位置推定精度が良好かどうかを判定する。

【0032】

このとき、ランドマーク検出走行制御部１０は、対地自車位置推定部３による自車位置推定精度が所定水準になるまで、ランドマークが周囲環境撮影部１の撮影範囲内に入るように自車位置を補正するように制御する。

【0033】

この場合には、図３の時刻ｔ１において、図２に示すように、ランドマーク１４の手前で減速制御し、自車１３がランドマーク１４を検出すると、ランドマーク検出走行制御部１０は、自車１３を一時停止させる。そして、時刻ｔ１〜ｔ２において、検出されたランドマーク１４に基づき自車位置を補正する。そして、自車位置推定精度が所定水準以上になった時刻ｔ２で自車１３を再発進させる。

【0034】

また、ステップＳ１５において、自車位置推定精度が所定水準以上になると、自車位置推定切替部１１は、対地自車位置推定部３から相対自車位置推定部４に切り替える。

【0035】

この場合には、時刻ｔ２以降において、難度が高い区間を走行中、即ち、自車１３が道路１２ｂの駐車場１５周辺を走行中、自車位置推定切替部１１は、対地自車位置推定部３から相対自車位置推定部４に切り替え、相対自車位置推定部４が自車１３を制御する。相対自車位置推定部４は、短距離では安定して高精度で自車の走行を制御することができる。

【0036】

このように第１の実施形態の車両走行制御装置によれば、地図ＤＢ５の地図情報が利用可能な走行区間では、対地自車位置推定部３により継続的に自車位置を推定し、高難度区間の手前でランドマークによる自車位置を補正する。そして、位置推定精度が高い区間を走行中、対地自車位置推定部３から相対自車位置推定部４に切り替えるので、駐車・狭路通過等の高難度区間も安定した自車位置推定精度で走行制御を継続できる。

【0037】

また、対地自車位置推定部３による自車位置推定精度が所定水準になるまで、検出されたランドマークがカメラ視野角に入るように自車位置を補正するので、確実に自車位置推定精度を改善することができる。

【0038】

また、対地自車位置推定部３は、周囲環境撮影部１で撮影された自車の周囲環境の撮影画像と、地図ＤＢ５の地図情報を仮想位置及び仮想姿勢角から撮影した仮想画像とを比較する。これにより、実際の車両の姿勢角及び位置を推定することもでき、撮影画像だけを用いて継続的に位置推定結果を得ることができる。

【0039】

また、ランドマーク検出走行制御部１０は、ランドマーク検索部９により検索されたランドマークを検出したときには、自車を一時停止させるので、ランドマークによる補正が行え、自車位置推定精度を高くすることができる。

【0040】

また、ランドマーク検索部９は、地図ＤＢ５から所定の条件を満たす複数のランドマークを検索し、検索された複数のランドマークから交通状況から検出可能性の高いランドマークを選択する。選択されたランドマークをランドマーク検出走行制御部１０の目標ランドマークとして使用する。これにより、ランドマークが他の車両等によつて自車の視界から遮蔽されている場合でも代替物を用いて補正を行うことで、対環境ロバスト性を向上することができる。

【0041】

また、本発明は、自車の前方に存在する障害物の位置と大きさを検出する障害物検出部を備えても良い。この場合には、抽出されたランドマークのうち障害物検出部で検出された障害物で遮蔽される可能性が高いランドマークを除外した上で適切なランドマークを選択することで、ランドマーク検出走行制御部１０の処理を実行することができる。

【0042】

また、ランドマーク近傍で十分な位置推定精度が達成されていると判断された場合には、ランドマーク検出走行制御部１０の処理をスキップして、直ちに相対自車位置推定部４への切り替えを実行しても良い。

【0043】

また、自車位置難度区間推定部８は、設定された目的地が複数の駐車枠から構成される駐車場である場合には、駐車場内を自車位置推定難度が高い区間として設定しても良い。同じような駐車枠が連続しているためにカメラによる位置推定精度が上がり難い駐車エリアでの位置推定精度を安定して改善できる。

【0044】

また、自車位置難度区間推定部８は、道路幅が所定値よりも小さい区間で、区間内に道路上から検出可能なランドマークが地図情報に記録されていない場合には、その区間を自車位置推定難度が高い区間として設定しても良い。

【0045】

また、ランドマーク検出走行制御部１０は、現在の自車位置推定精度の水準に基づき自車の目標走行速度を変更して良い。また、抽出されたランドマークのうち、最初に周囲環境撮影部１で検出されたランドマークを選択しても良い。

【0046】

また、対地自車位置推定部３は、カメラを用いたＶＭＭに代えて、高精度のＧＰＳ（Global Positioning System）で実現してもよく、高難度区間としてトンネル内、ビルの谷間、屋内駐車場等を想定しても良い。

【0047】

〔第２の実施形態〕
本発明の第２の実施形態の自車位置姿勢推定装置は、第１の実施形態の車両走行制御装置において説明した対地自車位置推定部３の具体的な構成を示すもので、例えば、図５に示すように構成される。

【0048】

この自車位置姿勢推定装置は、ＥＣＵ２１、カメラ（撮影手段）２２、３次元地図データベース２３、及び、車両センサ群４０を含む。車両センサ群４０は、ＧＰＳ受信機４１、アクセルセンサ４２、ステアリングセンサ４３、ブレーキセンサ４４、車速センサ４５、加速度センサ４６、車輪速センサ４７、及び、ヨーレートセンサ等のその他センサ４８を含む。なお、ＥＣＵ２１は、実際にはＲＯＭ、ＲＡＭ、演算回路等にて構成されている。ＥＣＵ２１がＲＯＭに格納された自車位置姿勢推定用のプログラムに従って処理をすることによって、後述する機能（仮想画像取得手段、尤度設定手段、自車位置姿勢推定手段）を実現する。

【0049】

カメラ２２は、例えばＣＣＤ等の固体撮影素子を用いたものである。カメラ２２は、例えば車両のフロント部分に設置され、車両前方を撮影可能な方向に設置される。カメラ２２は、所定時間毎に周辺を撮影して撮影画像を取得し、ＥＣＵ２１に供給する。

【0050】

３次元地図データベース２３は、例えば路面表示を含む周囲環境のエッジ等の三次元位置情報が記憶されている。本実施形態において、３次元地図データベース２３には、白線、停止線、横断歩道、路面マーク等の路面表示の他に、縁石、建物等の構造物のエッジ情報も含まれる。白線などの各地図情報は、エッジの集合体で定義される。エッジが長い直線の場合には、例えば１ｍ毎に区切られるため、極端に長いエッジは存在しない。直線の場合には、各エッジは、直線の両端点を示す３次元位置情報を持っている。曲線の場合には、各エッジは、曲線の両端点と中央点を示す３次元位置情報を持っている。

【0051】

車両センサ群４０は、ＥＣＵ２１に接続される。車両センサ群４０は、各センサ４１〜４８により検出した各種のセンサ値をＥＣＵ２１に供給する。ＥＣＵ２１は、車両センサ群４０の出力値を用いることで、車両の概位置の算出、単位時間に車両が進んだ移動量を示すオドメトリを算出する。

【0052】

また、ＥＣＵ２１は、カメラ２２により撮影された撮影画像と３次元地図データベース２３に記憶された３次元位置情報とを用いて自車両の位置及び姿勢角の推定を行う電子制御ユニットである。なお、ＥＣＵ２１は、他の制御に用いるＥＣＵと兼用しても良い。
特に、この自車位置姿勢推定装置は、カメラ２２により撮影した撮影画像と、三次元地図データを仮想位置及び仮想姿勢角から撮影した画像に変換した仮想画像とを比較して、自車両の位置及び姿勢角を推定する。ここで、図６（ａ）に示すような撮影画像が得られ、図６（ｂ）のようなエッジ画像が得られたとする。一方、３次元位置情報をカメラ２２の位置及び姿勢角に投影した仮想画像が図６（ｃ）のようになったとする。図６（ｂ）の撮影画像と図６（ｃ）の仮想画像とを比較すると、遠方位置（Ａ）及び近傍位置（Ｂ）の双方で一致しているため、仮想画像を生成した仮想位置及び仮想姿勢角が、自車両の位置及び姿勢角に相当すると推定できる。しかし、仮想位置が右方向に位置ずれした場合には、仮想画像は、図６（ｄ）に示すようになる。この場合、図６（ａ）の撮影画像と図６（ｄ）の撮影画像とを比較すると、遠方位置（Ａ）は一致しているが、近傍位置（Ｂ）は大きくずれてしまう。逆に、図６（ｃ）の仮想画像の仮想姿勢角をずらし（図示せず）、図６（ａ）の撮影画像と比較すると、近傍位置（Ｂ）は一致するが、遠方位置（Ａ）は大きくずれる。

【0053】

このような現象に着目して、自車位置姿勢推定装置は、撮影画像内の近傍位置画素と仮想画像内の近傍位置画素とが一致した場合には仮想画像の仮想位置が尤もらしいと判断する。逆に、自車位置姿勢推定装置は、撮影画像内の遠方位置画素と仮想画像内の遠方位置画素とが一致した場合には仮想画像の仮想姿勢角が尤もらしいと判断する。

【0054】

以下、この自車位置姿勢推定装置の動作について、図７の位置姿勢推定アルゴリズムを参照して説明する。なお、本実施形態は、車両の６自由度の位置（前後方向，横方向，上下方向）及び姿勢角（ロール，ピッチ，ヨー）を推定するものとする。また、この図７の位置姿勢推定アルゴリズムは、ＥＣＵ２１によって、例えば１００ｍｓｅｃ程度の間隔で連続的に行われる。

【0055】

先ずステップＳ５１において、ＥＣＵ２１は、カメラ２２により撮影された映像を取得し、当該映像に含まれる撮影画像から、エッジ画像を算出する。本実施形態におけるエッジとは、画素の輝度が鋭敏に変化している箇所を指す。エッジ検出方法としては、例えばCanny法を用いることができる。これに限らず、エッジ検出手法は、他にも微分エッジ検出など様々な手法を使用してもよい。

【0056】

また、ＥＣＵ２１は、カメラ２２の撮影画像から、エッジの輝度変化の方向やエッジ近辺のカラーなど抽出することが望ましい。これにより、後述するステップＳ５５及びステップＳ５６において、３次元地図データベース２３にも記録しておいた、これらエッジ以外の情報も用いて位置推定用尤度、姿勢角推定用尤度を設定して、自車両の位置及び姿勢角を推定してもよい。

【0057】

次のステップＳ５２において、ＥＣＵ２１は、車両センサ群４０から得られるセンサ値から、１ループ前の位置姿勢推定アルゴリズムのステップＳ５２にて算出した時からの車両の移動量であるオドメトリを算出する。なお、位置姿勢推定アルゴリズムを開始して最初のループの場合は、オドメトリをゼロとして算出する。

【0058】

ＥＣＵ２１は、車両センサ群４０から得られる各種センサ値を用いて、車両が単位時間に進んだ移動量であるオドメトリを算出する。このオドメトリ算出方法としては、例えば、車両運動を平面上に限定した上で、各車輪の車輪速とヨーレートセンサから、単位時間での移動量と回転量を算出すれば良い。また、ＥＣＵ２１は、車輪速を車速やＧＰＳ受信機４１の測位値の差分で代用してもよく、ヨーレートセンサを操舵角で代用してもよい。なお、オドメトリの算出方法は、様々な算出手法が考えられるが、オドメトリが算出できればどの手法を用いても良い。

【0059】

次のステップＳ５３において、ＥＣＵ２１は、ステップＳ５２にて算出したオドメトリから、複数の仮想（予測）位置及び仮想（予測）姿勢角の候補を算出する。複数の仮想位置及び仮想姿勢角の候補は、自車位置及び姿勢角の候補である。このとき、ＥＣＵ２１は、１ループ前のステップＳ５６にて推定された車両位置から、今回のステップＳ５２で算出したオドメトリ分だけ移動させる。ＥＣＵ２１は、移動させた車両位置の近傍で、複数の仮想位置及び仮想姿勢角の候補を算出する。ただし、位置姿勢推定アルゴリズムを開始して初めてのループの場合には、ＥＣＵ２１は前回の車両位置情報を持っていない。このため、ＥＣＵ２１は、車両センサ群４０に含まれるＧＰＳ受信機４１からのデータを初期位置情報とする。又は、ＥＣＵ２１は、前回に停車時に最後に算出した車両位置及び姿勢角を記憶しておき、初期位置及び姿勢角情報にしてもよい。

【0060】

このとき、ＥＣＵ２１は、車両センサ群４０の測定誤差や通信遅れにより生じるオドメトリの誤差や、オドメトリで考慮できない車両の動特性を考慮に入れ、車両の位置や姿勢角の真値が取り得る可能性がある複数の仮想位置及び仮想姿勢角の候補を複数個生成する。この仮想位置及び仮想姿勢角の候補は、位置及び姿勢角の６自由度のパラメータに対してそれぞれ誤差の上下限を設定し、この誤差の上下限の範囲内で乱数表等を用いてランダムに設定する。

【0061】

なお、本実施形態では、仮想位置及び仮想姿勢角の候補を５００個作成する。また、位置及び姿勢角の６自由度のパラメータに対する誤差の上下限は、前後方向，横方向，上下方向、ロール，ピッチ，ヨーの順に±0.05[m]，±0.05[m]，±0.05[m]，±0.5[deg] ，±0.5[deg]，±0.5[deg]とする。この仮想位置及び仮想姿勢角の候補を作成する数や、位置及び姿勢角の６自由度のパラメータに対する誤差の上下限は、車両の運転状態や路面の状況を検出或いは推定して、適宜変更することが望ましい。例えば、急旋回やスリップなどが発生している場合には、平面方向（前後方向，横方向，ヨー）の誤差が大きくなる可能性が高いので、この３つのパラメータの誤差の上下限を大きくし、且つ仮想位置及び仮想姿勢角の候補の作成数を増やすことが望ましい。

【0062】

また、ステップＳ５３において、ＥＣＵ２１は、所謂パーティクルフィルタを用いて複数の仮想位置及び仮想姿勢角の候補を設定してもよい。このとき、ＥＣＵ２１は、１ループ前のステップＳ５６で生成された複数の仮想位置及び仮想姿勢角の候補である各パーティクル（候補点）の位置及び姿勢角を、オドメトリ分だけ移動させる。具体的には、図８に示すように、１ループ前に推定されていた車両Ｖ（ｔ１）の位置及び姿勢角のパーティクルＰと周囲のパーティクルＰ１〜Ｐ５を、オドメトリ分だけ移動させる。この結果、ＥＣＵ２１は、新たな車両Ｖ（ｔ２）の位置及び姿勢角を推定するためのパーティクルＰ１０〜１５を設定する。これにより、ＥＣＵ２１は、今回における複数の仮想位置及び仮想姿勢角の候補を算出する。すなわち、各パーティクルの位置及び姿勢角を、複数の仮想位置及び仮想姿勢角の候補とする。なお、車両センサ群４０の測定誤差や通信遅れによって生じるオドメトリの誤差や、オドメトリで考慮できない車両の動特性を考慮に入れて、各パーティクルの位置情報及び姿勢角情報をオドメトリ分だけ移動させる。その後に、上記のように位置及び姿勢角の６自由度のパラメータに対する誤差の上下限の範囲内で乱数表等を用いてランダムに変化させるとなおよい。

【0063】

ただし、位置姿勢推定アルゴリズムを開始して初めてのループの場合には、各パーティクルは位置情報及び姿勢角情報を持っていない。このため、車両センサ群４０に含まれるＧＰＳ受信機４１の検出データを初期位置情報としてもよい。又は、前回停車時に最後に推定した車両位置から、各パーティクルの位置情報及び姿勢角情報を設定してもよい。本実施形態では、初めてのループの場合、前回停車時に最後に推定した車両位置から、位置及び姿勢角の６自由度のパラメータに対して位置及び姿勢角の６自由度のパラメータに対する誤差の上下限を設定する。この誤差の上下限の範囲内で乱数表等を用いてランダムに各パーティクルの位置情報及び姿勢角を設定する。本実施形態では、初めてのループの場合はパーティクルを５００個作成する。また、各パーティクルの６自由度のパラメータに対する誤差の上下限は、前後方向，横方向，上下方向、ロール，ピッチ，ヨーの順に±0.05[m]，±0.05[m]，±0.05[m]，±0.5[deg] ，±0.5[deg]，±0.5[deg]とする。

【0064】

次のステップＳ５４において、ＥＣＵ２１は、ステップＳ５３で作成した複数の仮想位置及び仮想姿勢角の候補のそれぞれについて仮想（投影）画像を作成する。このとき、ＥＣＵ２１は、例えば３次元地図データベース２３に記憶されたエッジ等の三次元位置情報を、仮想位置及び仮想姿勢角から撮影したカメラ画像となるよう投影変換して、仮想画像を作成する。この仮想画像は、各仮想位置及び仮想姿勢角の候補が実際の自車両の位置及び姿勢角と合致しているかを評価する評価用画像である。投影変換では、カメラ２２の位置を示す外部パラメータと、カメラ２２の内部パラメータが必要となる。外部パラメータは、車両位置（例えば中心位置）からカメラ２２までの相対位置を予め計測しておくことで、仮想位置及び仮想姿勢角の候補から算出すればよい。また内部パラメータは、予めキャリブレーションをしておけばよい。

【0065】

なお、ステップＳ５１においてカメラ２２で撮影した撮影画像からエッジの輝度変化の方向やエッジ近辺のカラーなどについてもカメラ画像から抽出できる場合、それらについても三次元位置情報を３次元地図データベース２３に記録し、仮想画像を作成しても良い。

【0066】

ステップＳ５５において、ＥＣＵ２１は、ステップＳ５３で設定した複数の仮想位置及び仮想姿勢角の候補それぞれにおいて、ステップＳ５１で作成したエッジ画像と、ステップＳ５４で作成した仮想画像とを比較する。ＥＣＵ２１は、比較した結果に基づき、各仮想位置及び仮想姿勢角の候補ごとに、位置推定用尤度及び姿勢角推定用尤度を設定する。この尤度とは、仮想位置及び仮想姿勢角の候補が、どれぐらい実際の車両の位置及び姿勢角に対して尤もらしいかを示す指標である。ＥＣＵ２１は、仮想画像とエッジ画像との一致度が高いほど、尤度が高くなるように設定する。なお、尤度の求め方については後述する。

【0067】

特に、ＥＣＵ２１は、撮影画像と仮想画像とを比較し、撮影画像内の遠方位置画素と仮想画像内の遠方位置画素とが一致した場合には仮想画像の姿勢角尤度を高くする（尤度設定手段）。ＥＣＵ２１は、撮影画像と仮想画像とを比較し、撮影画像内の近傍位置画素と仮想画像内の近傍位置画素とが一致した場合には仮想画像の位置尤度を高くする（尤度設定手段）。遠方位置画素及び近傍位置画素は、例えば、撮影画像及び仮想画像の位置によって設定してもよい。例えば、撮影画像及び仮想画像内の縦方向における中間位置より所定幅だけ下方向の範囲を、遠方位置画素が存在する画像領域に設定してもよい。また、撮影画像及び仮想画像内の縦方向における底面位置より所定幅だけ上方向の範囲を、近傍位置画素が存在する画像領域に設定してもよい。この距離は、三次元地図をカメラ２２に投影したときの仮想映像を求める際、カメラ２２の位置を仮定しているため求めることが可能である。

【0068】

具体的には、ＥＣＵ２１は、仮想画像上とエッジ画像上のエッジ部が一致するか否か、すなわち、仮想画像上のエッジが存在する画素座標位置に、エッジ画像上のエッジが存在しているかを判断する。仮想画像上とエッジ画像上のエッジ部が一致する場合には、その一致したエッジ部分について３次元地図データベース２３を参照して、その一致したエッジ部の三次元空間上での位置を求める。そして、位置推定用尤度を求めている仮想位置及び仮想姿勢角の候補の位置情報と、その一致したエッジ部との距離Ｌ（単位：ｍ）を求め、その距離Ｌの逆数を尤度like_p（単位：なし）とする。なお、仮想画像上とエッジ画像上のエッジ部が一致しない場合には尤度like_pに０を設定する。

【0069】

ＥＣＵ２１は、この処理を仮想画像上の全画素に対して実施する。ＥＣＵ２１は、車両から近い部分でエッジ画像と仮想画像とが一致していた場合、当該画素に大きい尤度like_pを設定する。ＥＣＵ２１は、逆に、車両から遠い部分でエッジ画像と仮想画像とが一致していた場合、当該画素に小さい尤度like_pを設定する。ＥＣＵ２１は、全画素の尤度like_pの総和を、仮想画像についての位置推定用尤度LIKE_P（単位：なし）とする。

【0070】

また、ＥＣＵ２１は、尤度like_pを求めるときに、図９に示すように、車両からの距離に応じて上限値を設けてもよい。ＥＣＵ２１は、所定距離Ｌthp以上の近傍でエッジ画像（撮影画像）の画素と仮想画像の画素が一致した場合には、所定の上限値lthpとなるように尤度like_pを設定する。又は、ＥＣＵ２１は、車両からの距離が近くなるほど、尤度like_pの増加幅を少なくしてもよい。なお、本実施形態では距離Ｌthpは１．０［ｍ］が設定される。

【0071】

具体的には、ＥＣＵ２１は、予め車両からの距離と尤度like_pとの関係を記述した位置尤度設定マップを用意しておく。ＥＣＵ２１は、位置尤度設定マップを参照して、エッジ画像と仮想画像とが一致した車両からの距離に応じて尤度like_pを設定する。

【0072】

これにより、極端に車両（カメラ２２）からの距離が近い部分に対して抽出したエッジにノイズや誤差があっても、その影響を抑制することができ、位置推定誤差を小さくすることができる。

【0073】

ＥＣＵ２１は、姿勢角推定用尤度も求める。ＥＣＵ２１は、仮想画像上とエッジ画像上のエッジ部が一致する場合には、位置推定用尤度LIKE_Pを求めた時と同様に、一致した画素の車両からの距離Ｌ（単位：ｍ）を求める。ＥＣＵ２１は、当該車両からの距離Ｌを１０で除した値を尤度like_a（単位：なし）とする。なお、仮想画像上とエッジ画像上のエッジ部が一致しない場合には尤度like_aに０を設定する。

【0074】

ＥＣＵ２１は、この処理を仮想画像上の全画素に対して実施する。ＥＣＵ２１は、車両から遠い部分でエッジ画像と仮想画像とが一致していた場合、当該画素に大きい尤度like_aを設定する。ＥＣＵ２１は、逆に、車両から近い部分でエッジ画像と仮想画像とが一致していた場合、当該画素に小さい尤度like_aを設定する。ＥＣＵ２１は、全画素の尤度like_aの総和を、仮想画像についての姿勢角推定用尤度LIKE_A（単位：なし）とする。

【0075】

また、ＥＣＵ２１は、尤度like_aを求めるときに、図１０に示すように、車両からの距離に応じて上限値を設けてもよい。ＥＣＵ２１は、所定距離Ｌtha以上の遠方でエッジ画像（撮影画像）の画素と仮想画像の画素が一致した場合には、所定の上限値lthaとなるように尤度尤度like_aを設定する。又は、ＥＣＵ２１は、車両からの距離が遠くなるほど、尤度like_aの増加幅を少なくしてもよい。なお、本実施形態では距離Ｌthaは３０．０［ｍ］が設定される。

【0076】

具体的には、ＥＣＵ２１は、予め車両からの距離と尤度like_aとの関係を記述した位置尤度設定マップを用意しておく。ＥＣＵ２１は、位置尤度設定マップを参照して、エッジ画像と仮想画像とが一致した車両からの距離に応じて尤度like_aを設定する。

【0077】

これにより、極端に車両（カメラ２２）からの距離が遠い部分に対して抽出したエッジにノイズや誤差があっても、その影響を抑制することができ、姿勢角推定誤差を小さくすることができる。

【0078】

ＥＣＵ２１は、各仮想画像について位置推定用尤度LIKE_P及び姿勢角推定用尤度LIKE_Aを求め、更に、仮想位置及び仮想姿勢角の候補について位置推定用尤度LIKE_P及び姿勢角推定用尤度LIKE_Aを算出する。ＥＣＵ２１は、複数の仮想位置及び仮想姿勢角の候補についての全結果を用いて、位置推定用尤度LIKE_P、姿勢角推定用尤度LIKE_Aのそれぞれの合計値が１になるよう正規化する。

【0079】

次のステップＳ５６において、ＥＣＵ２１は、ステップＳ５５にて位置推定用尤度LIKE_P及び姿勢角推定用尤度LIKE_Aが求められた複数の仮想位置及び仮想姿勢角の候補を用いて、最終的な自車両の位置及び姿勢角を算出する。ＥＣＵ２１は、仮想画像の姿勢角推定用尤度LIKE_Aに基づき当該仮想画像の仮想姿勢角に対する実際の車両の姿勢角を推定する（自車位置姿勢推定手段）。ＥＣＵ２１は、仮想画像の位置推定用尤度LIKE_Pに基づき当該仮想画像の仮想位置に対する実際の車両の位置を推定する（自車位置姿勢推定手段）。

【0080】

このとき、ＥＣＵ２１は、例えば、位置推定用尤度LIKE_Pが最も高い仮想位置及び仮想姿勢角の候補を用いて、当該仮想位置及び仮想姿勢角を、車両の実際の位置及び姿勢角として算出してもよい。又は、ＥＣＵ２１は、姿勢角推定用尤度LIKE_Aが最も高い仮想位置及び仮想姿勢角の候補を用いて、当該仮想位置及び仮想姿勢角を、車両の実際の位置及び姿勢角として算出してもよい。又は、ＥＣＵ２１は、位置推定用尤度LIKE_Pと姿勢角推定用尤度LIKE_Aとの和が最も高い仮想位置及び仮想姿勢角の候補を用いて、当該仮想位置及び仮想姿勢角を、車両の実際の位置及び姿勢角として算出してもよい。又は、ＥＣＵ２１は、複数の仮想位置及び仮想姿勢角の候補に対して、各仮想画像の位置推定用尤度LIKE_Pに応じた重み付き平均を取って車両の実際の位置及び姿勢角を算出してもよい。又は、ＥＣＵ２１は、複数の仮想位置及び仮想姿勢角の候補に対して、各仮想画像の姿勢角推定用尤度LIKE_Aに応じた重み付き平均を取って車両の実際の位置及び姿勢角を算出してもよい。又は、ＥＣＵ２１は、各仮想画像の位置推定用尤度LIKE_Pと姿勢角推定用尤度LIKE_Aの和に応じた重み付き平均を取って車両の実際の位置及び姿勢角を算出してもよい。

【0081】

また、ステップＳ５３においてパーティクルフィルタを用いた場合、ステップＳ５６において、先ず各パーティクルの重みに、位置推定用尤度LIKE_P及び姿勢角推定用尤度LIKE_Aの２つを設定すればよい。そして、ＥＣＵ２１は、位置推定用尤度LIKE_Pが最も高いパーティクルの位置情報を、実際の車両の位置とする。又は、ＥＣＵ２１は、複数のパーティクルの位置情報に対して位置推定用尤度LIKE_Pに応じた重み付き平均を取って実際の車両の位置を算出してもよい。又は、ＥＣＵ２１は、姿勢角推定用尤度LIKE_Aが最も高いパーティクルの姿勢角情報を、実際の車両の姿勢角としてもよい。又は、ＥＣＵ２１は、複数のパーティクルの姿勢角情報に対して姿勢角推定用尤度LIKE_Aに応じた重み付き平均を取って実際の車両の姿勢角としてもよい。

【0082】

更に、ＥＣＵ２１は、各パーティクルのリサンプリングを、位置推定用尤度LIKE_Pと姿勢角推定用尤度LIKE_Aに基づき行う。すなわち、複数の仮想画像の姿勢角尤度及び複数の仮想画像の位置尤度に基づき、複数の仮想位置及び仮想姿勢角を再設定する。

【0083】

具体的には、ＥＣＵ２１は、位置推定用尤度LIKE_Pと姿勢角推定用尤度LIKE_Aの和が最も高いパーティクルを中心として、各パーティクルのリサンプリングを行う。又は、ＥＣＵ２１は、各パーティクルの位置情報と姿勢角情報とを一旦分離し、位置情報のみのパーティクルについては位置推定用尤度LIKE_Pに基づきリサンプリングを行う。姿勢角情報のみのパーティクルについては姿勢角推定用尤度LIKE_Aに基づきリサンプリングを行う。その後、ＥＣＵ２１は、ランダムに、位置情報のみのパーティクルと姿勢角情報のみのパーティクルの位置情報と姿勢角情報を組み合わせて、新たに位置情報と姿勢角情報を持ったパーティクルを再構成してもよい。

【0084】

ＥＣＵ２１は、以上のようなステップＳ５１乃至ステップＳ５６を繰り返して行うことによって、逐次、車両の位置と姿勢角を算出できる。

【0085】

以上詳細に説明したように、第２の実施形態として示した自車位置姿勢推定装置によれば、撮影画像と仮想画像とを比較し、撮影画像内の遠方位置画素と仮想画像内の遠方位置画素とが一致した場合には仮想画像の姿勢角尤度を高くする。一方、撮影画像内の近傍位置画素と仮想画像内の近傍位置画素とが一致した場合には仮想画像の位置尤度を高くする。そして、自車位置姿勢推定装置は、仮想画像の姿勢角尤度に基づき当該仮想画像の仮想姿勢角に対する実際の自車の姿勢角を推定する。一方、仮想画像の位置尤度に基づき当該仮想画像の仮想位置に対する実際の自車の位置を推定する。

【0086】

したがって、この自車位置姿勢推定装置によれば、位置と姿勢角に対して別々の尤度を考え、現実の映像と仮想映像とが一致した場所までの距離に応じて、位置と姿勢角を別々に調整して推定することができる。具体的には、カメラ２２から遠い場所でこの２つの映像が一致していた場合には、仮想画像の姿勢角は真値に近いが仮想画像の位置情報の誤差が大きい可能性があるので、姿勢角情報の尤度をより大きく設定できる。逆に、位置情報の尤度はあまり大きく設定しない。一方、カメラ２２から近い場所でこの２つの映像が一致していた場合には、位置情報の尤度をより大きく設定し、逆に、姿勢角情報の尤度はあまり大きく設定しない。

【0087】

２つの映像が一致した場所がカメラ２２から遠い場合には位置の誤差が大きい場合がある。逆に、２つの映像が一致した位置がカメラ２２から近い場合には姿勢角の誤差が大きい場合があるが、この自車位置姿勢推定装置によれば、精度良く自車の位置及び姿勢角を推定することができる。

【0088】

また、この自車位置姿勢推定装置によれば、所定距離以上の遠方で撮影画像の画素と仮想画像の画素が一致した場合には、尤度の増加幅を少なく又は所定の上限値となるように仮想画像の姿勢角尤度を設定する。これにより、自車位置姿勢推定装置によれば、一致させる映像の中に極端にカメラ２２からの距離が遠い部分が含まれていた時に、この部分での一致度合いによって姿勢角情報の尤度が決定されないようにできる。このため、極端にカメラ２２からの距離が遠い部分に対して抽出したエッジにノイズや誤差があっても、その影響を抑制することができ、姿勢角の推定誤差を小さくすることができる。

【0089】

更に、この自車位置姿勢推定装置によれば、所定距離以上の近傍で撮影画像の画素と仮想画像の画素が一致した場合には、尤度の増加幅を少なく又は所定の上限値となるように仮想画像の位置尤度を設定する。これにより、カメラ２２から一定距離以内で２つの映像が一致していた場合に位置情報の尤度を大きくしすぎないようにする。したがって、この自車位置姿勢推定装置によれば、極端にカメラ２２からの距離が近い部分に対して抽出したエッジにノイズや誤差があっても、その影響を抑制することができ、位置の推定誤差を小さくすることができる。

【0090】

更にまた、この自車位置姿勢推定装置によれば、複数のパーティクル（候補点）を設定して各パーティクルについての位置推定用尤度LIKE_P及び姿勢角推定用尤度LIKE_Aを設定して車両の位置及び姿勢角を求める。当該位置推定用尤度LIKE_P及び姿勢角推定用尤度LIKE_Aに基づきパーティクルをリサンプリングできる。

【0091】

ここで、パーティクルフィルタでは、推定する次数がｎとした場合、推定精度をａ倍とするためには、散布するパーティクルの数も原理的にａのｎ乗倍に増やす必要がある（確率ロボティクス・第4章3節（（著）Sebastian Thrun／Wolfram Burgard／Dieter Fox，（訳）上田隆一，（発行）（株）毎日コミュニケーションズ））。既存技術では、例えば三次元での位置情報と姿勢角情報を同時に推定する場合、その次数は６次となり、推定精度を２倍に上げようとすると演算時間は６４倍となり、推定精度を３倍に上げようとすると演算時間は７２９倍に増加してしまう。

【0092】

これに対し、自車位置姿勢推定装置によれば、位置情報と姿勢角情報とを別々に扱うことができる。したがって、原理的に推定精度を２倍に上げようとした場合には演算時間は２３×２=１６倍となり、推定精度を３倍に上げようとした場合で演算時間は３３×２=５４倍となるため、演算負荷を大きく低減できる。

【0093】

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

【0094】

上述した実施形態では車両を例にしたが、少なくとも１台以上のカメラを搭載した移動体であれば、航空機や船舶などにも適用可能である。

【0095】

また、上述した実施形態では車両の６自由度の位置（前後方向，横方向，上下方向）及び姿勢角（ロール，ピッチ，ヨー）を求めているが、これに限らない。例えば、サスペンション等を有さない、工場などで用いられる無人搬送車などのように３自由度での位置（前後方向，横方向）及び姿勢角（ヨー）を推定する場合にも、本実施形態が適用可能である。具体的には、このような車両であれば、上下方向の位置と、ロール及びピッチといった姿勢角は固定であるので、予めこれらのパラメータを計測しておいたり、３次元地図データベース２３を参照して求めるようにすればよい。

【符号の説明】

【0096】

１ 周囲環境撮影部
２ 自車状態検出部
３ 対地自車位置推定部
４ 相対自車位置推定部
５ 地図ＤＢ（地図データベース）
６ 走行経路算出部
７ 自動走行制御部
８ 自車位置難度区間推定部
９ ランドマーク検索部
１０ ランドマーク検出走行制御部
１１ 自車位置推定切替部
１２ａ，１２ｂ 道路
１３ 自車
１４ ランドマーク
１５ 駐車場
２１ ＥＣＵ
２２ カメラ
２３ 次元地図データベース
４０ 車両センサ群
４１ ＧＰＳ受信機
４２ アクセルセンサ
４３ ステアリングセンサ
４４ ブレーキセンサ
４５ 車速センサ
４６ 加速度センサ
４７ 車輪速センサ
４８ その他センサ

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】

【図9】

【図10】