特許 6552448 車両位置検出装置、車両位置検出方法及び車両位置検出用コンピュータプログラム

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

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

(11)【特許番号】6552448

(24)【登録日】2019年7月12日

(45)【発行日】2019年7月31日

(54)【発明の名称】車両位置検出装置、車両位置検出方法及び車両位置検出用コンピュータプログラム

(51)【国際特許分類】

   G01C 21/28 20060101AFI20190722BHJP

   G08G 1/16 20060101ALI20190722BHJP

   G06T 7/00 20170101ALI20190722BHJP

【ＦＩ】

   G01C21/28

   G08G1/16 C

   G06T7/00 650A

【請求項の数】7

【全頁数】25

(21)【出願番号】特願2016-73531(P2016-73531)

(22)【出願日】2016年3月31日

(65)【公開番号】特開2017-181476(P2017-181476A)

(43)【公開日】2017年10月5日

【審査請求日】2018年7月31日

(73)【特許権者】

【識別番号】502324066

【氏名又は名称】株式会社デンソーアイティーラボラトリ

(74)【代理人】

【識別番号】100099759

【弁理士】

【氏名又は名称】青木 篤

(74)【代理人】

【識別番号】100092624

【弁理士】

【氏名又は名称】鶴田 準一

(74)【代理人】

【識別番号】100114018

【弁理士】

【氏名又は名称】南山 知広

(74)【代理人】

【識別番号】100133835

【弁理士】

【氏名又は名称】河野 努

(72)【発明者】

【氏名】原 孝介

【審査官】 藤村 泰智

(56)【参考文献】

【文献】 国際公開第２０１５／０７２２１７（ＷＯ，Ａ１）

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

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

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

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

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

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

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

【文献】 米国特許出願公開第２０１７／０１６９３００（ＵＳ，Ａ１）

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

Ｇ０１Ｃ ２１／２６ 〜 ２１／３６

Ｇ０８Ｇ １／００ 〜 １／１６

Ｇ０６Ｔ ７／００

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

【請求項1】

道路上に標示される線の位置及び当該線の種類を表す地図情報を記憶する記憶部（４１）と、
車両（１０）の直前の位置から推定される当該車両の現時刻における予測位置に応じて前記道路上に標示される線を前記地図情報から抽出し、抽出した線のそれぞれについて、当該線の種類に応じて、当該線を横切る方向の輝度変化を表すパーティクルを、当該線に沿った方向に応じた位置を変えながら複数生成するパーティクル生成部（２３）と、
前記抽出した線のそれぞれについての前記複数のパーティクルのそれぞれについて、前記予測位置を想定される誤差範囲内で変化させながら前記車両（１０）を基準とする位置を求め、当該位置にしたがって前記車両（１０）に搭載された撮像部（２）により得られた現時刻の画像に投影する投影部（２４）と、
前記抽出した線のそれぞれについての前記複数のパーティクルのうち、前記画像との一致度合が所定の一致条件を満たすパーティクルを選択する選択部（２５）と、
前記選択されたパーティクルのそれぞれを、現時刻における前記車両（１０）の推定位置に基づいて対応する前記線に射影したときの誤差の総和に応じて前記推定位置を求める位置更新部（２６）と、
を有することを特徴とする車両位置検出装置。

【請求項2】

前記位置更新部（２６）は、前記選択されたパーティクルのそれぞれを対応する前記線に射影したときの誤差の総和が最小となるように前記推定位置を求める、請求項１に記載の車両位置検出装置。

【請求項3】

前記位置更新部（２６）は、前記選択されたパーティクルのそれぞれを対応する前記線に射影したときの誤差の総和と、過去の複数の時刻のそれぞれにおける前記車両（１０）の推定位置からオドメトリ情報を用いて予測した次の時刻における前記車両の位置と当該次の時刻における前記車両の推定位置との誤差の総和との和が最小となるように、現時刻及び前記過去の複数の時刻のそれぞれにおける前記車両（１０）の推定位置を求める、請求項１に記載の車両位置検出装置。

【請求項4】

前記パーティクル生成部（２３）は、前記抽出した線の種類が点線または破線を含む線を含む場合、当該点線または破線のブロックが有る場合に相当する当該線を横切る方向の輝度変化を表す第１のパーティクルと、当該点線または破線のブロックが無い場合に相当する当該線を横切る方向の輝度変化を表す第２のパーティクルとを生成する、請求項１〜３の何れか一項に記載の車両位置検出装置。

【請求項5】

前記撮像部（２）は、二つのカメラを含むステレオカメラであり、
前記選択部（２５）は、前記二つのカメラのうちの一方により現時刻に得られた第１の画像上で前記一致度合が前記一致条件を満たし、かつ、前記二つのカメラのうちの他方により現時刻に得られた第２の画像上で前記一致度合が前記一致条件を満たすパーティクルを選択する、請求項１〜４の何れか一項に記載の車両位置検出装置。

【請求項6】

車両（１０）の直前の位置から推定される当該車両の現時刻における予測位置に応じて道路上に標示される線を地図情報から抽出し、抽出した線のそれぞれについて、前記地図情報に表された当該線の種類に応じて、当該線を横切る方向の輝度変化を表すパーティクルを、当該線に沿った方向に応じた位置を変えながら複数生成するステップと、
前記抽出した線のそれぞれについての前記複数のパーティクルのそれぞれについて、前記予測位置を想定される誤差範囲内で変化させながら前記車両（１０）を基準とする位置を求め、当該位置にしたがって前記車両（１０）に搭載された撮像部（２）により得られた現時刻の画像に投影するステップと、
前記抽出した線のそれぞれについての前記複数のパーティクルのうち、前記画像との一致度合が所定の一致条件を満たすパーティクルを選択するステップと、
前記選択されたパーティクルのそれぞれを、現時刻における前記車両（１０）の推定位置に基づいて対応する前記線に射影したときの誤差の総和に応じて前記推定位置を求めるステップと、
を含むことを特徴とする車両位置検出方法。

【請求項7】

車両（１０）の直前の位置から推定される当該車両の現時刻における予測位置に応じて道路上に標示される線を地図情報から抽出し、抽出した線のそれぞれについて、前記地図情報に表された当該線の種類に応じて、当該線を横切る方向の輝度変化を表すパーティクルを、当該線に沿った方向に応じた位置を変えながら複数生成するステップと、
前記抽出した線のそれぞれについての前記複数のパーティクルのそれぞれについて、前記予測位置を想定される誤差範囲内で変化させながら前記車両（１０）を基準とする位置を求め、当該位置にしたがって前記車両（１０）に搭載された撮像部（２）により得られた現時刻の画像に投影するステップと、
前記抽出した線のそれぞれについての前記複数のパーティクルのうち、前記画像との一致度合が所定の一致条件を満たすパーティクルを選択するステップと、
前記選択されたパーティクルのそれぞれを、現時刻における前記車両（１０）の推定位置に基づいて対応する前記線に射影したときの誤差の総和に応じて前記推定位置を求めるステップと、
を前記車両（１０）に搭載されたプロセッサ（４３）に実行させる命令を含むことを特徴とする車両位置検出用コンピュータプログラム。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、車両に搭載されたカメラにより撮影された画像と地図を利用してその車両の位置を検出する車両位置検出装置、車両位置検出方法及び車両位置検出用コンピュータプログラムに関する。

【背景技術】

【0002】

従来より、ナビゲーションまたは運転支援のために、Global Positioning System(GPS)信号を利用して、走行中の自車両の位置を検出することが行われている。しかし、車両の近くに、高層建築物など、GPSの測位衛星からの測位信号を遮る構造物が存在する場合、車両において測位信号を受信することが困難となり、自車両の位置を正確に検出できないことがあった。そこで、車載カメラにより撮影された、車両周囲の画像と、事前に準備された地図などの情報とを対応付けることで、自車両の位置の検出精度を向上させる方法が提案されている（例えば、特許文献１〜３及び非特許文献１及び２を参照）。

【0003】

例えば、特許文献１に記載の位置測位装置は、車両の前方を撮影する撮影手段により撮影した画像の中にある道路標示に基づいて現在位置を特定する。

【0004】

また、特許文献２に記載された移動体位置測定装置は、地図上の参照地点付近で移動体に搭載された撮像部が撮影して得た画像から抽出した特徴情報と、地図の参照地点付近の画像から抽出した特徴情報とをマッチングして推定した参照地点付近の複数の特徴の位置に応じて移動体の現在位置を推定する。

【0005】

また、特許文献３に記載された方法は、車両の進行方向の画像データに対してテンプレートマッチングを適用することで、画像内で識別された光学指標に関して車両の位置を決定する。

【0006】

さらに、非特許文献１に記載された方法は、縁石及び道路標示を表す線分含む地図とステレオカメラにより得られた画像から抽出された特徴点とのマッチングにより、自車両の位置を検出する。

【0007】

さらにまた、非特許文献２に記載された方法は、GPS、IMU、オドメトリ、LIDARなど、車載された機器により取得される情報を用いて、高解像度の環境地図を生成する。そしてこの方法は、車両をこれらの地図上で位置決めする際に、LIDER測定値と地図との相関をとるパーティクルフィルタを利用する。

【先行技術文献】

【特許文献】

【0008】

【特許文献1】特開２００７−１０８０４３号公報

【特許文献2】特開２０１２−１２７８９６号公報

【特許文献3】特開２００５−１３６９４６号公報

【非特許文献】

【0009】

【非特許文献1】Schreiber他、「LaneLoc: Lane Marking based Localization using Highly Accurate Maps」、Intelligent Vehicles Symposium (IV)、2013 IEEE、IEEE、2013

【非特許文献2】Lenvision他、「Map-Based Precision Vehicle Localization in Urban Environments」、Robotics: Science and Systems (RSS)、2007年

【発明の概要】

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

【0010】

しかし、特許文献１に記載の装置は、撮影時の環境の影響などにより、道路標示の特徴点の検出が困難な場合、自車両の位置を正確に特定できないおそれがある。特に、特許文献１に記載の装置は、道路標示の端点などを特徴点として抽出し、その特徴点を用いて車両の現在位置を求めるので、道路標示の経年劣化などにより、道路標示の端点がわずかに掠れるだけでも抽出された特徴点の位置がずれてしまい、自車両の位置の検出精度の低下を招いてしまう。

【0011】

また、特許文献２に記載の装置は、地図の参照地点付近で撮影された画像を自車両の位置の特定に利用するが、参照地点の付近の建物の建て替え、あるいは、撮影時の環境（例えば、天候、撮影時間帯など）によって、同じ場所について撮影された画像であっても、事前登録時に撮影された画像に写っている景色と、自車両位置の検出時に撮影された画像に写っている景色は大きく異なることがある。このような場合、画像間のマッチングに失敗する可能性が高くなり、そのため、自車両の位置を正確に特定できないおそれがある。

【0012】

さらに、特許文献３に記載の方法は、テンプレートマッチングを利用している。テンプレートマッチングでは、比較対象となる画像とテンプレート間の位置を変えつつ、繰り返しマッチング演算する必要が有るため、自車両の位置の検出に要する演算量が多い。また、非特許文献１に記載の方法では、縁石の検出と道路標示の検出に異なるアルゴリズムが利用されており、それらの検出にステレオ画像を利用するので、演算量が多い。そのためこれらの方法では、自車両の位置の検出に必要なハードウェアリソースが増えてしまうか、あるいは、自車両の位置の検出に時間を要するので、自車両の位置の更新間隔が長くなってしまう。さらに、非特許文献２に記載の方法では、自車両の位置の検出にレーザ測距システムが必要となり、システム全体のコストが高くなる。

【0013】

そこで、本発明は、低い処理負荷で、自車両の位置を正確に推定できる車両位置検出装置、車両位置検出方法及び車両位置検出用コンピュータプログラムを提供することを目的とする。

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

【0014】

請求項１の記載によれば、本発明の一つの形態として、車両位置検出装置が提供される。この車両位置検出装置は、道路上に標示される線の位置及びその線の種類を表す地図情報を記憶する記憶部（４１）と、車両（１０）の直前の位置から推定される車両の現時刻における予測位置に応じて道路上に標示される線を地図情報から抽出し、抽出した線のそれぞれについて、その線の種類に応じて、その線を横切る方向の輝度変化を表すパーティクルを、その線に沿った方向に応じた位置を変えながら複数生成するパーティクル生成部（２３）と、抽出した線のそれぞれについての複数のパーティクルのそれぞれについて、予測位置を想定される誤差範囲内で変化させながら車両（１０）を基準とする位置を求め、その位置にしたがって車両（１０）に搭載された撮像部（２）により得られた現時刻の画像に投影する投影部（２４）と、抽出した線のそれぞれについての複数のパーティクルのうち、画像との一致度合が所定の一致条件を満たすパーティクルを選択する選択部（２５）と、選択されたパーティクルのそれぞれを、現時刻における車両（１０）の推定位置に基づいて対応する道路上に標示される線に射影したときの誤差の総和に応じて車両（１０）の推定位置を求める位置更新部（２６）とを有する。
本発明による車両位置検出装置は、上記の構成を有することにより、低い処理負荷で、自車両の位置を正確に推定できる。

【0015】

また請求項２の記載によれば、位置更新部（２６）は、選択されたパーティクルのそれぞれを対応する道路上に標示される線に射影したときの誤差の総和が最小となるように車両（１０）の推定位置を求めることが好ましい。
この構成を有することで、車両位置検出装置は、自車両の位置をより正確に推定できる。

【0016】

また請求項３の記載によれば、位置更新部（２６）は、選択されたパーティクルのそれぞれを対応する道路上に標示される線に射影したときの誤差の総和と、過去の複数の時刻のそれぞれにおける車両（１０）の推定位置からオドメトリ情報を用いて予測した次の時刻における車両の位置と次の時刻における車両の推定位置との誤差の総和との和が最小となるように、現時刻及び過去の複数の時刻のそれぞれにおける車両（１０）の推定位置を求めることが好ましい。
この構成を有することで、車両位置検出装置は、オドメトリ情報も利用するので、自車両の位置をより正確に推定できる。

【0017】

さらに、請求項４の記載によれば、パーティクル生成部（２３）は、抽出した線の種類が点線または破線を含む線を含む場合、点線または破線のブロックが有る場合に相当する道路上に標示される線を横切る方向の輝度変化を表す第１のパーティクルと、点線または破線のブロックが無い場合に相当する道路上に標示される線を横切る方向の輝度変化を表す第２のパーティクルとを生成することが好ましい。
この構成を有することで、車両位置検出装置は、実際に道路上に標示される線と一致する可能性が高いパーティクルを生成できる。

【0018】

さらに、請求項５の記載によれば、撮像部（２）は、二つのカメラを含むステレオカメラであり、選択部（２５）は、二つのカメラのうちの一方により現時刻に得られた第１の画像上で一致度合が一致条件を満たし、かつ、二つのカメラのうちの他方により現時刻に得られた第２の画像上で一致度合が一致条件を満たすパーティクルを選択することが好ましい。
この構成を有することで、車両位置検出装置は、他の車両などによるオクルージョンにより路面が隠れているところに投影されたパーティクルを誤って選択する可能性を低減することができる。

【0019】

請求項６の記載によれば、本発明の他の形態として、車両位置検出方法が提供される。この車両位置検出方法は、車両（１０）の直前の位置から推定されるその車両の現時刻における予測位置に応じて道路上に標示される線を地図情報から抽出し、抽出した線のそれぞれについて、地図情報に表されたその線の種類に応じて、その線を横切る方向の輝度変化を表すパーティクルを、その線に沿った方向に応じた位置を変えながら複数生成するステップと、抽出した線のそれぞれについての複数のパーティクルのそれぞれについて、予測位置を想定される誤差範囲内で変化させながら車両（１０）を基準とする位置を求め、その位置にしたがって車両（１０）に搭載された撮像部（２）により得られた現時刻の画像に投影するステップと、抽出した線のそれぞれについての複数のパーティクルのうち、画像との一致度合が所定の一致条件を満たすパーティクルを選択するステップと、選択されたパーティクルのそれぞれを、現時刻における車両（１０）の推定位置に基づいて対応する道路上に標示される線に射影したときの誤差の総和に応じて車両（１０）の推定位置を求めるステップとを含む。
本発明による車両位置検出方法は、上記の構成を有することにより、低い処理負荷で、自車両の位置を正確に推定できる。

【0020】

請求項６の記載によれば、本発明の他の形態として、車両位置検出用コンピュータプログラムが提供される。この車両位置検出用コンピュータプログラムは、車両（１０）の直前の位置から推定されるその車両の現時刻における予測位置に応じて道路上に標示される線を地図情報から抽出し、抽出した線のそれぞれについて、地図情報に表されたその線の種類に応じて、その線を横切る方向の輝度変化を表すパーティクルを、その線に沿った方向に応じた位置を変えながら複数生成するステップと、抽出した線のそれぞれについての複数のパーティクルのそれぞれについて、予測位置を想定される誤差範囲内で変化させながら車両（１０）を基準とする位置を求め、その位置にしたがって車両（１０）に搭載された撮像部（２）により得られた現時刻の画像に投影するステップと、抽出した線のそれぞれについての複数のパーティクルのうち、画像との一致度合が所定の一致条件を満たすパーティクルを選択するステップと、選択されたパーティクルのそれぞれを、現時刻における車両（１０）の推定位置に基づいて対応する道路上に標示される線に射影したときの誤差の総和に応じて車両（１０）の推定位置を求めるステップとを車両（１０）に搭載されたプロセッサ（４３）に実行させる命令を含む。
本発明による車両位置検出用コンピュータプログラムは、上記の構成を有することにより、低い処理負荷で、自車両の位置を正確に推定できる。

【0021】

上記各部に付した括弧内の符号は、後述する実施形態に記載の具体的手段との対応関係を示す一例である。

【図面の簡単な説明】

【0022】

【図1】本発明の一つの実施形態に係る車両位置検出システムの概略構成図である。

【図2】地図座標系と、車両座標系と、カメラ座標系の関係を示す図である。

【図3】（ａ）〜（ｄ）は、地図情報の一例を示す図である。

【図4】車両位置検出システムの制御部の機能ブロック図である。

【図5】地図線分の種類が「白実線（細）」である場合のパーティクルの一例を示す図である。

【図6】地図線分の種類が「白実線（細）＋右側減速標示」である場合のパーティクルの一例を示す図である。

【図7】地図線分の種類が「白点線（細）＋両側減速標示」である場合のパーティクルの一例を示す図である。

【図8】各パーティクルが投影された画像の一例を示す図である。

【図9】車両座標系における、選択された各パーティクルについての共分散行列に相当する誤差楕円の一例を表す図である。

【図10】車両位置検出処理の動作フローチャートである。

【図11】変形例により生成されるパーティクルと道路上に標示される線との関係の一例を示す図である。

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

【0023】

以下、図を参照しつつ、車両位置検出システムについて説明する。
この車両位置検出システムは、区画線または道路標示などの道路上に標示された線の種類に関する情報を含む地図情報を参照して、直前の自車両の位置などから推定される現時刻における予測位置の周辺の地図上の線に沿って、線の種類に応じた、線を横切る方向の輝度変化を表す輝度プロファイルを持つパーティクルを複数生成する。そしてこの車両位置検出システムは、地図上の座標系で表される各パーティクルを、自車両の現時刻における予測位置及び予測方位に応じて車両を基準とする座標系での位置に変換してから車両に搭載されたカメラにより得られる画像上へ投影する。そしてこの車両位置検出システムは、画像上に投影された各パーティクルのうち、画像との一致度合が所定の一致条件を満たすパーティクルを選択し、選択したパーティクルの地図上の線への投影誤差が小さくなるように、現時刻における自車両の位置及び方位を推定する。

【0024】

図１は、一つの実施形態による車両位置検出システムの概略構成図である。図１に示すように、車両位置検出システム１は、車両１０に搭載され、カメラ２と、コントローラ４とを有する。カメラ２と、コントローラ４とは、コントロールエリアネットワーク（以下、ＣＡＮという）３によって互いに接続されている。なお、図１では、説明の都合のため、車両位置検出システム１の各構成要素及び車両１０の形状、サイズ及び配置は、実際のものとは異なっている。

【0025】

カメラ２は、撮像部の一例であり、車両の前方領域を撮影し、その前方領域の画像を生成する。そのために、カメラ２は、CCDあるいはC-MOSなど、可視光に感度を有する光電変換素子のアレイで構成された２次元検出器と、その２次元検出器上に車両１０の前方に存在する地面または構造物などの像を結像する結像光学系を有する。そしてカメラ２は、例えば、結像光学系の光軸が地面に対して略平行となり、かつ車両１０の前方を向くように、車両１０の車室内に配置される。そしてカメラ２は、一定の時間間隔（例えば1/30秒）ごとに撮影し、車両１０の前方領域を撮影したカラー画像を生成する。なお、カメラ２は、近赤外光に感度を有する２次元検出器を有し、その撮像範囲内の近赤外光の照度に応じたモノクロ画像を生成してもよい。

【0026】

図２は、地図座標系と、車両座標系と、カメラ座標系の関係を示す図である。本実施形態では、便宜上、地図上の任意の位置を原点とする地図座標系(Xm, Ym, Zm)と、車両１０を原点とする車両座標系(Xv, Yv, Zv)と、カメラ２を原点とするカメラ座標系(Xc, Yc, Zc)を利用する。本実施形態では、車両座標系(Xv, Yv, Zv)は、車両１０の左右の後輪間の中点かつ地面上の点を原点とする。そして車両の進行方向をZv軸とし、Zv軸と直交し、かつ、地面に平行な方向をXv軸とし、鉛直方向をYv軸とする。また地図座標系(Xm, Ym, Zm)においても、傾きの無い地面に平行な面内にXm軸及びZm軸が設定され、その地面に対する鉛直方向にYm軸が設定される。また、カメラ座標系(Xc, Yc, Zc)では、説明の簡単化のために、車両座標系の原点から鉛直方向に沿って上方かつカメラ２が設置される高さの位置に撮像面の中心があると仮定して、その撮像面の中心を原点とする。そして車両座標系と同様に、車両１０の進行方向をZc軸とし、Zc軸と直交し、かつ、地面に平行な方向をXc軸とし、鉛直方向をYc軸とする。
なお、実際にカメラ２が取り付けられる位置は、車両１０の左右の後輪間の中点の上方からずれていることもあるが、このずれは、単純な平行移動によって補正すればよい。

【0027】

なお、車両位置検出システム１は、撮像部として、車両１０の前方領域を撮影するカメラの代わりに、あるいはそのカメラとともに、車両の後方領域を撮影するリアカメラを有していてもよい。

【0028】

カメラ２は、生成した画像を逐次コントローラ４へ送信する。なお、車両１０の前方領域を撮影するカメラと車両１０の後方領域を撮影するカメラが取り付けられている場合、コントローラ４は、車両１０が進行している方向を撮影するカメラからの画像のみを選択的に取得してもよい。そのために、コントローラ４は、ＣＡＮ３を介して車両１０の電子制御ユニット(ECU)１１から、シフトレバーのポジションを表すシフトポジション信号を取得する。そしてコントローラ４は、シフトポジション信号が、車両１０が前進することを示すドライブポジジョンなどとなっている場合、車両１０の前方領域を撮影するカメラから画像を取得する。一方、コントローラ４は、シフトポジション信号が、車両１０が後進することを示すリバースポジションとなっている場合、車両１０の後方領域を撮影するカメラから画像を取得する。

【0029】

コントローラ４は、車両位置検出装置の一例であり、記憶部４１と、通信部４２と、制御部４３とを有する。記憶部４１は、例えば、電気的に書き換え可能な不揮発性メモリ及び揮発性メモリなどの半導体メモリを有する。そして記憶部４１は、車両位置検出システム１を制御するための各種プログラム、地図情報、カメラ２の地面からの高さ及び光軸方向といったカメラの位置情報、結像光学系の焦点距離及び画角といったカメラパラメータなどの各種パラメータ、及び制御部４３による一時的な演算結果などを記憶する。また記憶部４１は、カメラ２の結像光学系による歪曲収差を補正するためのパラメータ、例えば、画素ごとの歪曲収差の補正量（すなわち、歪曲収差を打ち消すための画像上での画素の移動量及び移動方向）を記憶してもよい。

【0030】

以下、車両位置検出に利用される、地図情報について説明する。
図３（ａ）〜図３（ｄ）は、地図情報の一例を示す図である。図３（ａ）に示される地図情報３００は、その地図情報で表された領域の位置を表す緯度経度情報と関連付けられている。そして地図情報３００は、白線、黄線などで表される区画線などの道路上に標示される線３０１に関する情報を含む。

【0031】

実際の道路上に標示される線には、複数の種類がある。例えば、図３（ｂ）において点線で示される線３１１は、実線の白線と一方の側に設けられる破線状の減速標示の組み合わせで表される。また図３（ｃ）において点線で示される線３１２は、点線の白線とその両側に設けられる減速標示の組み合わせで表される。また、図３（ｄ）において点線で示される線３１３は、相対的に太い実線の白線のみで表される。このように、道路上に標示される線には、様々な種類のものがある。しかし、地図情報３００では、例えば、それぞれの道路上の線について、一つの実線で表され、線ごとに、その線の種類を表すラベルが付されていることがある。例えば、図３（ｂ）に示される例では、線３１１は、実線３２１と、（白線（細）＋右減速標示）というラベルで表される。また、図３（ｃ）に示される例では、線３１２は、実線３２２と、（白点線（細）＋両側減速標示）というラベルで表される。そして図３（ｄ）に示される例では、線３１３は、実線３２３と、白線（太）というラベルで表される。さらに、地図情報３００では、道路上に標示される各線について、その両端点の３次元座標が含まれる。
なお、以下では、地図情報に含まれる、道路上に標示される線を地図線分と呼ぶ。

【0032】

通信部４２は、カメラ２、ＥＣＵ１１及び車輪速センサ（図示せず）などの各種センサとＣＡＮ３を通じて通信する通信インターフェース及びその制御回路を有する。そして通信部４２は、カメラ２から画像を受け取り、その画像を制御部４３へ渡す。また通信部４２は、位置推定処理を実行する周期ごとに、オドメトリ情報として、ＥＣＵ１１から、車両１０の速度及び移動量などを取得したり、あるいは、車輪速センサから車輪速を取得して、制御部４３へ渡す。

【0033】

制御部４３は、１個もしくは複数個の図示していないプロセッサ及びその周辺回路を有する。そして制御部４３は、車両位置検出システム１全体を制御する。
図４に、制御部４３の機能ブロック図を示す。図４に示すように、制御部４３は、初期値設定部２１と、予測位置算出部２２と、パーティクル生成部２３と、投影部２４と、選択部２５と、位置更新部２６とを有する。制御部４３が有するこれらの各部は、例えば、制御部４３が有するプロセッサ上で実行されるコンピュータプログラムによって実現される機能モジュールとして実装される。

【0034】

初期値設定部２１は、本実施形態による、車両１０の位置の検出、すなわち、自車両の位置の検出を開始する際の自車両の位置の初期値を設定する。例えば、初期値設定部２１は、GPSの測位信号が最後に受信できたときのその測位信号から求められる位置を、例えば、ナビゲーションシステムから取得し、その位置に基づいて、自車両の位置の初期値を設定する。そして得られた位置及び方位を、自車両の位置の初期値に設定する。
なお、初期値設定部２１により設定される初期値は、本実施形態による自車両の位置の検出精度よりも低い検出精度で得られる値であってもよい。

【0035】

本実施形態では、時刻tにおける、自車両の位置の初期値μ_t=(x_t,z_t,θ_t)^tは、ガウス分布に従って、次式で表される。

【数1】

ここで、(x_t,z_t)は、初期値を算出する時刻をtとしたときの、測位信号などから求められた地図座標系でのx方向及びz方向における車両１０の位置、すなわち、自車両の位置を表す。θ_tは、時刻tにおける、車両１０の方位を表す。なお、車両１０の方位は、例えば、車両１０が有する、車両１０の進行方向の方位を測定するセンサ（図示せず）からの信号をＣＡＮ３及び通信部４２を介して制御部４３が受信することにより得られる。あるいは、制御部４３は、自車両の位置の検出を開始する直前において異なる時刻に受信したGPSの測位信号から求めた自車両の位置の変化から算出された自車両の方位を、例えば、ナビゲーションシステムから受信してもよい。また、（１）式において、Σ_tは、共分散行列である。また鉛直方向の位置、すなわち標高y_tは、（１）式には含まれていないが、次式により算出できる。

【数2】

ここで、a_x^t、a_z^t、a_c^tは、それぞれ定数であり、例えば、地図座標(x_t,z_t)の周囲は平面であると仮定して、地図座標(x_t,z_t)の周囲の所定範囲（例えば、10m以内）に含まれる各地図線分上で等間隔の各点の地図座標(x_i,y_i,z_i)を（２）式に代入したときの二乗誤差が最小となるように、例えば最小二乗法を用いて設定される。

【0036】

初期値設定部２１により設定された自車両の位置の初期値は、記憶部４１に記憶される。

【0037】

予測位置算出部２２は、自車両の位置の初期値、あるいは、位置更新部２６により求められた直前の自車両の位置と、車両１０の車輪速等に基づいて、現時刻における自車両の予測位置を算出する。
本実施形態では、現時刻tにおける自車両の予測位置は、平均値μ_p^t、共分散Σ_p^tのガウス分布として表される。そこで予測位置算出部２２は、次式に従って、現時刻tにおける自車両の水平方向における予測位置と予測方位の平均値μ_p^t=(x_p^t,z_p^t,θ_p^t)と共分散行列Σ_p^tを算出する。

【数3】

ここで、平均値μ_t-1、共分散Σ_t-1で表されるガウス分布は、時刻(t-1)における、自車両の推定位置を表す。現時刻tが制御部４３が自車両位置の検出を行う最初の時刻である場合、N(x_t-1,z_t-1,θ_t-1|μ_t-1,Σ_t-1)は、初期値設定部２１により設定された自車両の位置の初期値である。また、時刻(t-1)において、制御部４３が自車両位置を検出している場合、N(x_t-1,z_t-1,θ_t-1|μ_t-1,Σ_t-1)は、時刻(t-1)において、制御部４３により検出された自車両の推定位置である。

【0038】

（３）式において、u_t=(v_t,ω_t)^tは、は、車輪速から定められる制御入力を表す。v_tは、現時刻tにおける車両１０の速度であり、左右の後輪に取り付けられた車輪速センサから通信部４２を介して受信した、最新の右後輪の車輪速v^R_tと左後輪の車輪速v^L_tの平均値(v^R_t+v^L_t)/2として算出される。またω_tは、現時刻tにおける車両１０の角速度であり、ωt=(v^R_t-v^L_t)/2daである。daは、左右の後輪間の距離である。
またΔtは、現時刻tと直前の時刻(t-1)の時間間隔、すなわち、制御部４３が自車両位置の検出処理を行う時間間隔である。行列Q_tは、後輪車輪速による自車両の位置の予測に含まれる誤差の度合いを表している。行列Q_tの各要素σ_x、σ_z、σ_θは、それぞれ、地図座標系におけるXm方向の位置の誤差の標準偏差、Zm方向の位置の誤差の標準偏差、及び方位の誤差の標準偏差に相当し、例えば、σ_x=σ_z=1(m)、σ_θ=10(°)に設定される。
なお、予測位置算出部２２は、現時刻tにおける自車両の標高y_tについては、例えば、（２）式に従って算出すればよい。

【0039】

予測位置算出部２２は、μ_p^t及びΣ_p^tを記憶部４１に記憶する。

【0040】

パーティクル生成部２３は、車両１０の現時刻における予測位置に基づいて、その予測位置におけるカメラ２の撮影範囲に含まれる地図線分を横切る方向の輝度変化を表す輝度プロファイルを持つ輝度パターンであるパーティクルを、地図情報に含まれるその地図線分の種類に応じて生成する。

【0041】

本実施形態では、パーティクル生成部２３は、パーティクルの生成範囲及び密度を適切に設定するために、予測位置の周囲の所定範囲（例えば、30m以内）、かつ、予測位置を基準としたカメラ２の撮影範囲内にある各地図線分を地図情報から抽出し、抽出した各地図線分をカメラ２により得られる画像上に投影する。なお、一端が所定範囲及び撮影範囲内にあり、かつ他端が所定範囲または撮影範囲から外れる地図線分については、パーティクル生成部２３は、その所定の範囲の境界及び撮影範囲の境界のうちの予測位置から近い方と交差した点を地図線分の他端として、その地図線分を画像上に投影する。以下、各地図線分に対して同一の処理が行われるので、r番目の地図線分を例として説明する。

【0042】

地図座標系で表される地図上の点p_m=(x_m,y_m,z_m)と画像上の点u_c=(u,v)との関係は、次式で表される。

【数4】

ここで、p_vは、点p_mに対応する車両座標系の点であり、p_cは、点p_mに対応するカメラ座標系の点である。そして行列{R_slope^tR_vm^t}は、時刻tにおける、車両座標系から地図座標系への座標変換における回転成分を表す。ここで、ロールピッチの回転成分を表す回転行列R_slope^tは、（２）式における(a_x^t、a_z^t、a_c^t)から算出される。また並進行列t_vm^t= (x_vm^t,y_vm^t,z_vm^t)は、時刻tにおける、車両座標系から地図座標系への座標変換における平行移動成分を表す。そしてh(p_c,k_c)は、カメラ座標系における点p_c=(x_c,y_c,z_c)からカメラ２により得られる画像上の点u_cとの関係を表し、k_cは、カメラ座標系から画像への変換パラメータを表す。カメラ２の歪曲収差を無視できる場合、h(p_c,k_c)は、次式で表される。

【数5】

【0043】

パーティクル生成部２３は、μp_tに基づいて(x_vm^t,z_vm^t,θ_vm^t)を設定し、（４）式に従って、r番目の地図線分の両端点(m_s^r,m_e^r)をカメラ２により得られる画像上に投影する。そしてパーティクル生成部２３は、画像上でのその地図線分の長さl_img^rを求める。パーティクル生成部２３は、その地図線分の長さl_img^rを一本の地図線分あたりに発生させるパーティクルの総数（例えば、500〜1000）で等分して得られる位置のそれぞれをパーティクルを発生させる位置の候補とする。パーティクル生成部２３は、各候補の中から所定数の候補を一様分布に従ってランダムに選択する。そしてパーティクル生成部２３は、選択した候補に対応する地図座標系での３次元位置にパーティクルを発生させる。あるいは、パーティクル生成部２３は、全ての候補についてパーティクルを発生させてもよい。

【0044】

パーティクル生成部２３は、パーティクルを発生させる地図線分の種類に応じて、パーティクルのタイプを決定する。なお、地図線分の種類は、上述したように、地図情報に含まれる。その際、パーティクル生成部２３は、地図線分の種類によっては、複数の種類のパーティクルを、そのパーティクルの種類に応じた確率に従って発生させる。

【0045】

図５は、地図線分の種類が「白線（細）」である場合のパーティクルの一例を示す図である。図５において、横軸は、実線である白線の位置を原点とする、地図線分と直交する方向に沿った位置を表し、縦軸は輝度を表す。そしてプロファイル５００は、発生させるパーティクルの輝度プロファイルを表す。地図線分の種類が「白線（細）」である場合には、一本の白線が道路上に標示されているので、地図線分に沿った方向の位置によらず、地図線分を横切る方向の輝度プロファイルは変化しない。そこでこの場合には、パーティクル生成部２３は、発生確率p1=1.0にて、プロファイル５００で表される、白線の位置に相当し、かつ、白線の幅と同じ幅を持つ中央部分だけが高い輝度を持ち、その両側が低い輝度となるパーティクルを発生させる。

【0046】

図６は、地図線分の種類が「白線（細）＋右側減速標示」である場合のパーティクルの一例を示す図である。図６において、横軸は、実線である白線の位置を原点とする、地図線分と直交する方向に沿った位置を表し、縦軸は輝度を表す。そしてプロファイル６００及び６１０は、それぞれ、発生させるパーティクルの輝度プロファイルを表す。地図線分の種類が「白線（細）＋右側減速標示」である場合には、一本の白線だけでなく、減速標示である破線が道路上に標示されているので、地図線分に沿った方向の位置によって、減速標示のブロックの有無により、地図線分を横切る方向の輝度プロファイルは変化する。そこでこの場合には、パーティクル生成部２３は、発生確率p1=0.4にて、プロファイル６００で表される、白線だけが存在する場合に相当する、白線の位置に相当し、かつ、白線の幅と同じ幅を持つ中央部分だけが高い輝度を持ち、その両側が低い輝度となるパーティクルを発生させ、発生確率p2=0.6にて、プロファイル６１０で表される、白線とともに減速標示のブロックが存在する場合に相当する、白線に相当する位置と減速標示のブロックに相当する位置の２か所で相対的に高い輝度を持つパーティクルを発生させる。なお、発生確率p1及びp2は、減速標示のブロックの長さとブロック間の長さの比に応じて、予め設定される。

【0047】

図７は、地図線分の種類が「白点線（細）＋両側減速標示」である場合のパーティクルの一例を示す図である。図７において、横軸は、白点線の位置を原点とする、地図線分と直交する方向に沿った位置を表し、縦軸は輝度を表す。そしてプロファイル７００〜７２０は、それぞれ、発生させるパーティクルの輝度プロファイルを表す。地図線分の種類が「白点線（細）＋両側減速標示」である場合には、一本の白点線だけでなく、減速標示である破線が白点線の両側に道路上に標示されているので、地図線分に沿った方向の位置によって、白点線のブロックの有無、及び、減速標示のブロックの有無により、地図線分を横切る方向の輝度プロファイルは変化する。そこでこの場合には、パーティクル生成部２３は、発生確率p1=0.3にて、プロファイル７００で表される、白点線のブロックだけが存在する場合に相当する、白点線に相当する中央部分だけが高い輝度を持ち、その両側が低い輝度となるパーティクルを発生させる。またパーティクル生成部２３は、発生確率p2=0.2にて、プロファイル７１０で表される、白点線のブロックとともに減速標示のブロックが存在する場合に相当する、白点線のブロック及び減速標示のブロックに相当する３ヵ所で相対的に高い輝度を持つパーティクルを発生させる。そしてパーティクル生成部２３は、発生確率p3=0.5にて、プロファイル７２０で表される、減速標示のブロックのみが存在する場合に相当する、減速標示のブロックに相当する２か所で高い輝度を持つパーティクルを発生させる。なお、発生確率p1〜p3は、白点線のブロックの長さとブロック間の長さの比、及び、減速標示のブロックの長さとブロック間の長さの比に応じて、予め設定される。

【0048】

パーティクル生成部２３は、生成した各パーティクルを投影部２４へ渡す。

【0049】

投影部２４は、各パーティクルをカメラ２により現時刻に得られた画像上へ投影する。なお、車両位置検出処理の実行周期とカメラ２による撮影周期とが同期していない場合には、カメラ２により得られた最新の画像を、現時刻に得られた画像とすればよい。その際、投影部２４は、車両１０の予測位置の不確実性を考慮して、予測位置を算出した際の共分散行列により表される正規分布にしたがって、パーティクルごとに予測位置及び予測方位を修正する。例えば、修正された予測位置及び予測方位μ_q^ti=(x_q^ti,z_q^ti,θ_q^ti)は次式で表される。

【数6】

ここで、εⁱは、i番目のパーティクルについての予測位置及び予測方位の修正量を表し、正規分布N(0,Σ_p^t)にパーティクルごとに発生させた乱数を入力することで得られる。

【0050】

投影部２４は、各パーティクルについて、修正された予測位置及び予測方位に基づいてそのパーティクルの車両座標系での位置を次式に従って算出する。

【数7】

ここで、gⁱは、i番目のパーティクルの地図座標系での位置（例えば、パーティクルの両端点の位置、あるいは、パーティクル上で輝度値が変化する位置）を表し、qⁱは、i番目のパーティクルの車両座標系での位置を表す。また回転行列R_mv^tiは、時刻tにおける、地図座標系から車両座標系への座標変換における回転成分であり、i番目のパーティクルについての修正された予測方位θ_q^tiに基づいて決定される。同様に、並進行列t_mv^tiは、時刻tにおける、地図座標系から車両座標系への座標変換における平行移動成分であり、i番目のパーティクルについての修正された予測位置(x_q^ti,z_q^ti)に基づいて決定される。

【0051】

投影部２４は、車両座標系上の位置で表された各パーティクルを（５）式にしたがって、時刻tにおけるカメラ２により得られた画像上に投影する。

【0052】

図８は、各パーティクルが投影された画像の一例を示す図である。図８に示された画像８００において、道路上の白線８０１〜８０３のそれぞれごとに、その白線と交差するように複数のパーティクル８１０が投影されている。なお、この例では、パーティクル生成部２３は、１本の白線ごとに、最大で1000個のパーティクルを発生させている。パーティクル８１０によって、白線と直交する方向の位置及び白線となす角が少しずつ変化していることが分かる。なお、各パーティクル８１０において、相対的に白い部分は輝度が相対的に低く、相対的に黒い部分が輝度が相対的に高いことを表す。

【0053】

投影部２４は、画像上に投影された各パーティクルを選択部２５にわたす。

【0054】

選択部２５は、画像上に投影された各パーティクルについて、画像との一致度合を表す評価値を算出する。そして選択部２５は、評価値が所定の一致条件を満たすパーティクルを選択する。

【0055】

例えば、選択部２５は、各パーティクルについて、そのパーティクルの輝度プロファイル上で所定数（例えば、50点）の位置の輝度値と画像の対応位置の輝度値との間での正規化相互相関値を評価値として算出する。そして選択部２５は、評価値が所定の閾値（例えば、0.8）以上となるパーティクルを、一致条件を満たすとして選択する。なお、画像の画質が悪くても一定数以上のパーティクルが選択されるように、所定の閾値は、例えば、評価値の最大値に1未満の所定数（例えば、0.8）を乗じて得られた値としてもよい。

【0056】

あるいは、選択部２５は、評価値が最も高い方から順に所定数（例えば、生成されたパーティクルの総数の1/5〜1/10）のパーティクルを、一致条件を満たすとして選択してもよい。

【0057】

なお、選択部２５は、各パーティクルについて、正規化相互相関値を算出する代わりに、誤差二乗和(SSD)あるいは誤差絶対値和(SAD)を評価値として算出してもよい。この場合には、選択部２５は、評価値が所定の閾値以下となるパーティクルを、一致条件を満たすとして選択する。

【0058】

選択部２５は、選択された各パーティクルについて、位置の検出誤差を推定する。i番目のパーティクルについての位置の検出誤差は、例えば、画像上でのカメラ２の結像光学系の歪曲収差による誤差成分、あるいは、画素単位で区切られることによる誤差成分を含み、共分散行列R_Iⁱで表されるものとする。共分散行列R_Iⁱは、画像の中心、すなわち、カメラ２の光軸から離れるほど２次で大きくなると仮定される場合、例えば、次式で表される。

【数8】

ここで、nⁱ=(n_uⁱ,n_vⁱ)(n_uⁱ=(c_u-uⁱ)/f_u,n_vⁱ=(c_v-vⁱ)/f_v)は、正規化カメラ座標系でのi番目のパーティクルの画像上での中心位置を表す。なお、(uⁱ,vⁱ)は、i番目のパーティクルの画像上での中心座標であり、(c_u,c_v)は、画像上での中心座標であり、(f_u,f_v)は、カメラ２の焦点距離を表すパラメータである。c1、c2はカメラ２の結像光学系の歪曲収差などに応じた定数であり、例えば、c1=0.001、c2=0.005に設定される。またIは２行２列の単位行列である。

【0059】

選択部２５は、選択された各パーティクルについて、画像上での検出誤差の共分散行列を、車両座標系での共分散行列に変換する。この場合、選択部２５は、車両座標系でのパーティクルの位置qⁱにおける共分散行列R_qⁱは、画像上での対応する検出誤差の共分散行列R_Iⁱを鳥瞰変換したものとして算出できる。この鳥瞰変換は、例えば、次式で表される。

【数9】

ここで、カメラ２の光軸方向は、路面に対して平行となるように取り付けられているものとした。またycは、路面からカメラ２までの高さを表す。またこの例では、車両１０の位置の推定とは無関係な、車両座標系のy軸に関連する成分については省略されている。このとき、共分散行列R_qⁱは、次式で表される。

【数10】

ここでBは、ヤコビアンである。

【0060】

図９は、車両座標系における、選択された各パーティクルについての共分散行列R_qⁱに相当する誤差楕円の一例を表す図である。図９において、横軸は、車両１０の進行方向と直交する方向の位置を表し、縦軸は車両１０の進行方向に沿った、車両１０からの距離を表す。なお、上側ほど、車両１０から離れているとする。個々の楕円９０１が、それぞれ、一つのパーティクルに対応する共分散行列R_qⁱに相当する。また個々の楕円９０１の中心の点９０２は、対応するパーティクルの中心位置を表す。図９に示されるように、パーティクルの位置が車両１０から離れるほど誤差楕円９０１は大きくなることが分かる。

【0061】

選択部２５は、選択された各パーティクルについて、そのパーティクルの中心位置の車両座標系における位置qⁱ及び共分散行列R_qⁱと、対応する地図線分の番号を位置更新部２６へわたす。

【0062】

位置更新部２６は、直近の過去の１以上の時刻における車両１０の推定位置と、現時刻における、選択された各パーティクルに基づいて、現時刻における車両１０の位置を推定する。

【0063】

例えば、位置更新部２６は、次式に従って評価値Eを算出し、その評価値Eが最小となるように、直近の過去の１以上の時刻及び現時刻における車両の推定位置を算出する。

【数11】

ここで、時刻t∈{τ_s〜τ_e}は、車両１０の位置を推定する時刻であり、τ_eが現時刻tに相当する。μ^t及びμ^t-1は、それぞれ、時刻t、(t-1)における車両１０の推定位置を表す。なお、評価値Eの算出において、μ^t-1については、前時刻(t-1)において求められた車両１０の推定位置が入力される。すなわち、μ^t-1については、既知の定数として扱われる。またu^tは、（３）式におけるu_tに対応する、オドメトリ情報である。そしてδ_x(μ^t,μ^t-1,u^t)は、オドメトリによる車両１０の位置の予測の誤差を表し、（３）式より、次式で表される。

【数12】

なお、関数g(μ^t-1,u^t)は、入力される変数が時刻(t-1)における車両１０の推定位置及び時刻tにおけるオドメトリ情報である点を除いて、（３）式における関数g(μ_t-1,u_t)と同じ関数である。すなわち、（１１）式の右辺の第１項は、オドメトリ情報による車両１０の位置の予測の誤差の二乗和を表す。

【0064】

また、右辺の第２項におけるδ_I(μ^cj,q^j,χ^rj)は、時刻cj(すなわち、選択されたj番目のパーティクルに対応する時刻であり、時刻τ_s〜τ_eの何れか)の車両１０の推定位置μ^cjにおける、j番目(j∈{1,...,I}、Iは、時刻τ_s〜τ_eにおいて選択されたパーティクルの総数)の地図線分r^jについてのパーティクルq^jを、推定位置μ^cjを用いて地図線分r^jに投影したときの地図座標系における射影誤差を表す。すなわち、第２項は、選択されたパーティクルについての射影誤差の二乗和を表す。なお、cjは、番号jに対応する時刻を表す。またR_δ^jは、地図座標系での誤差の共分散行列を表す。射影誤差δ_I(μ^cj,q^j,χ^rj)及び共分散行列R_δ^jは、次式で表される。

【数13】

ここで、χ_x、χ_z、χ_cは、地図座標系における地図線分を直線方程式で表したときの係数及び定数であり、その直線方程式は、次式で表される。

【数14】

なお、x_m、z_mは、地図座標系上でのx_m方向の座標値、z_m方向の座標値である。

【0065】

位置更新部２６は、評価値Eが最小となるように、各時刻の車両１０の推定位置{μ^τs,...,μ^τe}を算出する。すなわち、（１１）式において、第１項のμ^t及び第２項μ^cjが変数となる。その際、位置更新部２６は、例えば、勾配法、レーベンバーグマーカート法、あるいはニュートン法を（１１）式に適用することで各時刻の車両１０の推定位置{μ^τs,...,μ^τe}を算出できる。あるいは、位置更新部２６は、RANSAC法またはM推定といったロバストな推定法を（１１）式に適用して、各時刻の車両１０の推定位置{μ^τs,...,μ^τe}を算出してもよい。これにより、評価値Eが多峰性を持つパーティクルによる局所解とならないように、位置更新部２６は、評価値Eの最小値をもとめることができる。

【0066】

位置更新部２６は、求めた各時刻の車両１０の推定位置{μ^τs,...,μ^τe}を記憶部４１に保存する。

【0067】

図１０は、車両位置検出処理の動作フローチャートである。車両位置検出システム１は、所定の周期ごとに、例えば、カメラ２の撮影周期ごとに、以下に示す動作フローチャートに従って自車両の位置を推定する。

【0068】

予測位置算出部２２は、前時刻(t-1)における自車両の位置とオドメトリ情報に基づいて、現時刻tにおける自車両の予測位置を算出する（ステップＳ１０１）。なお、前時刻(t-1)における自車両の位置は、現時刻tにおいて行われる車両位置検出処理が初回であれば、初期値設定部２１により設定された自車両の位置の初期値である。一方、前時刻(t-1)において、既に車両位置検出処理が行われている場合には、前時刻(t-1)において位置更新部２６により求められた自車両の位置である。

【0069】

パーティクル生成部２３は、その予測位置から所定の範囲内、かつ、カメラ２の撮影範囲内含まれる地図線分を、地図情報に基づいて抽出する（ステップＳ１０２）。そしてパーティクル生成部２３は、抽出した地図線分ごとに、その地図線分の種類に応じた、地図線分を横切る方向の輝度プロファイルを持つパーティクルを地図線分に沿って複数発生させる（ステップＳ１０３）。

【0070】

投影部２４は、各パーティクルを、車両１０の予測位置の誤差による位置の揺らぎを加味して、カメラ２により得られた現時刻における画像上に投影する（ステップＳ１０４）。

【0071】

選択部２５は、画像上に投影された各パーティクルについて、そのパーティクルと画像との一致度合を表す評価値を算出し、その評価値が所定の一致条件を満たすパーティクルを選択する（ステップＳ１０５）。そして選択部２５は、選択したパーティクルのそれぞれについて、車両座標系での検出誤差を表す共分散行列を算出する（ステップＳ１０６）。

【0072】

位置更新部２６は、過去の複数の時刻における推定位置とオドメトリ情報に基づく車両の予測位置間の誤差の総和と、検出誤差に基づいて算出される、選択された各パーティクルの射影誤差の総和との和に基づく評価値Ｅが最小となるように、各時刻の車両の推定位置を更新する（ステップＳ１０７）。そして制御部４３は、車両位置検出処理を終了する。

【0073】

以上説明してきたように、車両位置検出システムは、テンプレートマッチングのように、繰り返し試行する処理を伴わずに自車両の位置を検出できる。そのため、この車両位置検出システムは、処理負荷を抑制できる。またこの車両位置検出システムは、車両の周囲の道路上に標示された線を横切る方向の輝度プロファイルを持つ複数のパーティクルを生成して実際に撮影された画像上に投影し、その画像との一致度合が良好なパーティクルを利用して、自車両の位置を更新するので、自車両の位置を正確に検出できる。さらに、この車両位置検出システムは、地図情報を参照して、車両の周囲の道路上に標示された線の種類に応じてパーティクルを生成するので、道路に実際に標示されている線と一致する可能性が高いパーティクルを生成できる。そのため、この車両位置検出システムは、車両位置の推定に有用なパーティクルの数を増やすことができるので、車両位置の推定精度を向上できる。

【0074】

なお、変形例によれば、パーティクル生成部２３は、パーティクル生成対象となる地図線分のそれぞれについて、線の太さ、あるいは、線間の間隔を互いに異ならせたパーティクルを生成してもよい。これは、道路上に標示される線（白線、減速線など）の太さが道路規格で定めされた範囲内で変動する可能性が有ること、及び、車線に対するカメラ２の向きにより、画像上でのその線の太さが変化する可能性があるためである。例えば、パーティクル生成部２３は、着目する地図線分に含まれる、道路上に標示される線の幅の変動が正規分布に従うと仮定して、その正規分布に従って線の幅あるいは線間の間隔を変えたパーティクルを複数生成すればよい。これにより、パーティクル生成部２３は、車両１０が実際に走行している位置での道路上に標示される線と一致するパーティクルの数を増やすことができる。

【0075】

他の変形例によれば、パーティクル生成部２３は、車両１０の左側に位置する地図線分と右側に位置する地図線分とを一つの組として、その組ごとにパーティクルを生成してもよい。これにより、道路上に描画されている線の一部の形状が、道路上の補修痕、あるいは、摩耗などによって本来の形状から変化している場合でも、誤って選択される可能性が低いパーティクルを生成できる。またこの場合、車両１０の左側に位置する地図線分と右側に位置する地図線分間の間隔は、車線に対するカメラ２の向き、あるいはキャリブレーションの誤差などにより変化することがある。そこで上記の変形例と同様に、パーティクル生成部２３は、車両１０の左側に位置する地図線分と右側に位置する地図線分間の間隔の変動が正規分布に従うと仮定して、その正規分布に従って間隔を変えたパーティクルを複数生成すればよい。

【0076】

また他の変形例によれば、パーティクル生成部２３は、道路の路面に対して平行な面状のパーティクルを生成してもよい。

【0077】

図１１は、この変形例により生成されるパーティクルと道路上に標示される線との関係の一例を示す図である。図１１では、道路上に標示される線１１００は、鋸波状の形状を有している。そして縦軸は、線１１００の延伸方向を表し（縦軸の位置は、延伸方向と直交する方向における線１１００の重心位置に相当）、横軸は、線１１００の延伸方向と直交する方向を表す。この例では、線１１００の延伸方向に沿って、その延伸方向と直交する方向に沿った輝度変化を持つプロファイルを持つ３個の線の組が一つのパーティクル１１０１、１１０２として生成される。なお、図１１では、パーティクル１１０１、１１０２を構成する個々の線において、相対的に明るい部分は相対的に低い輝度を持ち、相対的に暗い部分は相対的に高い輝度を持つ。そして各線において、輝度が高い部分は、線１１００の対応部分に応じて設定される。このようなパーティクルを生成することで、車両位置検出システムは、道路上に標示される線が複雑な形状を有している場合でも、その線と一致するパーティクルを適切に選択できるので、車両１０の位置を推定できる。

【0078】

さらに他の変形例によれば、カメラ２がカラー画像を生成する場合には、パーティクル生成部２３は、地図線分の種類に応じて、パーティクルにおける、道路上に標示される線に相当する部分の色をその線の色に応じて設定してもよい。あるいは、パーティクル生成部２３は、道路上に標示される線の色の確率分布に従って、パーティクルにおける、道路上に標示される線に相当する部分の色を設定してもよい。

【0079】

さらに他の変形例によれば、カメラ２はステレオカメラであってもよい。この場合には、投影部２４は、各パーティクルを、例えば、ステレオカメラにより得られた左右の画像のそれぞれに投影する。また選択部２５は、例えば、ステレオカメラにより得られた左右の画像のそれぞれから路面に相当する路面領域を判定する。そして選択部２５は、左右の画像の何れかにおいて、パーティクルの一部が路面領域に含まれないパーティクルを選択しない。これにより、選択部２５は、例えば、先行車両などに隠れた道路上の線に対応するパーティクルを、車両１０の位置の推定から排除できるので、車両１０の位置の推定精度をより向上できる。なお、選択部２５は、路面領域の判定方法として、例えば、関他、「ステレオ動画像を用いた車両の前方環境認識」、情報処理学会研究報告コンピュータビジョンとイメージメディア（CVIM）2007、1-16、2007年などに開示された方法を利用できる。

【0080】

また、選択部２５は、各パーティクルについて、左右それぞれの画像についての評価値のうちの小さい方（評価値として正規化相互相関値を利用する場合、評価値としてSSDまたはSADを利用する場合には、左右それぞれの画像についての評価値のうちの大きい方）、あるいは、評価値の平均値が所定の一致条件を満たすものを選択してもよい。この変形例によれば、着目するパーティクルが投影された位置が先行車両により隠されるオクルージョンが発生している場合、その投影された位置と重なる先行車両上の位置は、左右それぞれの画像において異なるため、その先行車両において２か所以上、着目するパーティクルと類似するパターンが存在しない限り、そのパーティクルは選択されないことになる。したがって、ロバスト性が向上する。

【0081】

なお、カメラ２がステレオカメラである場合も、位置更新部２６は、（１１）式で表される評価値Eが最小となるように、各時刻における車両１０の推定位置を算出すればよい。

【0082】

さらに他の変形例によれば、パーティクル生成部２３は、直前の時刻において選択されたパーティクルに基づいて、現時刻のパーティクルを発生させてもよい。この場合、パーティクル生成部２３は、一般的なパーティクルフィルタに従って、現時刻のパーティクルを発生させればよい。これにより、パーティクル生成部２３は、生成するパーティクルの数を減らすことができる。すなわち、選択部２５による評価対象となるパーティクルの数を減らすことができる。

【0083】

さらに他の変形例によれば、位置更新部２６は、要求される精度によっては、（１１）式における第１項を省略し、第２項のみに基づいて、評価値Ｅが最小となるように、現時刻における車両１０の位置を推定してもよい。この場合には、位置更新部２６は、第２項においても、現時刻において選択されたパーティクルのみに基づいて第２項を算出してもよい。これにより、位置更新部２６は、演算量を削減できる。

【0084】

上記の実施形態または変形例による車両位置検出システムから出力された自車両の位置は、例えば、ＣＡＮ３を介して運転支援システムの制御回路（図示せず）へ送信される。運転支援システムの制御回路は、例えば、自車両の位置とその周囲の情報とを比較して、自車両から所定距離範囲内に特定の構造物（例えば、高速道路の料金所、ナビゲーション中の経路において左折または右折が必要な交差点など）が有れば、車内に設置されたディスプレイまたはスピーカを介して、その構造物が近いことをドライバに通知する。あるいは、運転支援システムの制御回路は、ＥＣＵ１１に、速度を落とす命令を出力してもよい。

【0085】

以上のように、当業者は、本発明の範囲内で、実施される形態に合わせて様々な変更を行うことができる。

【符号の説明】

【0086】

１ 車両位置検出システム
２ カメラ
３ コントロールエリアネットワーク（ＣＡＮ）
４ コントローラ（車両位置検出装置）
１０ 車両
１１ ＥＣＵ
４１ 記憶部
４２ 通信部
４３ 制御部
２１ 初期値設定部
２２ 予測位置算出部
２３ パーティクル生成部
２４ 投影部
２５ 選択部
２６ 位置更新部

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】

【図9】

【図10】

【図11】