以下、本発明の実施形態を図面に基づいて説明する。
図1は、本実施形態に係る車両位置算出システムを示す構成図である。図1に示すように、本実施形態に係る車両位置算出システム100は、車両に搭載される車両の位置を算出するためのシステムであり、車載カメラ10と、データベース20と、制御装置30と、車両センサ群40とを備えている。これら車載カメラ10、データベース20、制御装置30、および車両センサ群40は、互いに情報の授受を行うことができるようにCAN(Controller Area Network)その他の車載LANによって接続されている。
車載カメラ10は、車両周辺を撮影するための装置であり、車両周辺の撮像画像を取得し、取得した撮像画像を制御装置30へ送出する。車載カメラ10は、車両の任意の位置に取付けることができるが、たとえば、車両フロント部に設置して、車両前方を撮影することができる。また、車載カメラ10の設置数は特に限定されず、1つでもよいし、あるいは複数設置してもよい。車載カメラ10としては、たとえば、CCD等の固体撮像素子、および魚眼レンズのような広角レンズを備えるものなどを用いることができる。
データベース20は、路面表示および路面の周囲に存在する構造物に対応するエッジの情報を、その3次元位置の情報とともに記憶された3次元地図を備えている。具体的には、データベース20に備えられた3次元地図は、白線、停止線、横断歩道、路面マーク等の路面表示に対応するエッジの情報を、その3次元位置の情報とともに記憶している。また、3次元地図は、これら路面表示に対応するエッジの情報に加えて、縁石、建物等の構造物のエッジの情報についても、その3次元位置の情報とともに記憶している。
なお、データベース20に備えられた3次元地図においては、エッジが直線状のものである場合には、該エッジの情報を、所定距離毎(たとえば1m毎)に区切られたエッジの集合体とし、これら所定距離毎に区切られたエッジの両端点の位置を評価点として記憶している。たとえば、上述した所定距離が1mである場合に、長さ4mの停止線を例示して説明すると、3次元地図は、該停止線に対応するエッジの情報を、1mの長さに分割された4本のエッジの各端点の3次元座標を評価点とし、これらの評価点を結んだものを、長さ4mの停止線として記憶している。あるいは、エッジが直線状で1m未満である場合には、該エッジの両端点の3次元座標を評価点とし、該評価点を結んだものをエッジとして記憶している。また、エッジが曲線状の場合には、1m毎に区切られたエッジの両端点に加えて、エッジの中間点位置にも評価点が設定されており、これらの評価点を結んだものがエッジとして記憶されている。
また、3次元地図は、エッジ毎に、エッジが所定のランドマークに該当するか否かを示す情報をさらに記憶していてもよい。なお、所定のランドマークとしては、たとえば、エッジの検出が比較的容易である横断歩道、横断歩道予告、および進行方向別通行区分等の路面表示や、案内板や建物などの立体物等が挙げられる。
車両センサ群40は、GPS、ステアリングセンサ、車速センサ、加速度センサ、車輪速センサ、ヨーレートセンサ等の各種車載センサで構成され、各種車載センサにより取得された信号は制御装置30へ送出される。制御装置30は送出された信号から、車両のおおよその位置や、単位時間での車両が進んだ移動距離および車両が方向転換した際の回転量などを算出する。
制御装置30は、3次元地図上の情報と撮像画像とを用いて車両の位置を算出する電子制御ユニットである。制御装置30は、本実施形態に係る車両位置算出処理を実行するためのプログラムを格納したROM(Read Only Memory)31と、このROM31に格納されたプログラムを実行するCPU(Central Processing Unit)32と、アクセス可能な記憶装置として機能するRAM(Random Access Memory)33と、を備える。なお、動作回路としては、CPU(Central Processing Unit)32に代えて、またはこれとともに、MPU(Micro Processing Unit)、DSP(Digital Signal Processor)、ASIC(Application Specific Integrated Circuit)、FPGA(Field Programmable Gate Array)などを用いることができる。また、本実施形態における制御装置30は、他の制御に用いる制御装置と兼用してもよい。
次に、制御装置30が備える処理機能について説明する。制御装置30は、3次元地図上の情報と撮像画像とを用いて車両の位置を算出するために、エッジ画像算出機能、オドメトリ算出機能、パーティクル生成機能、位置予測機能、評価点投影機能、位置評価機能、位置統合機能、評価点追加機能および評価点削除機能を有する。制御装置30は、上記各機能を実現するためのソフトウェアと、上述したハードウェアの協働により各機能を実行することができる。
以下、上述した制御装置30が実現する各機能についてそれぞれ説明する。
制御装置30のエッジ画像算出機能は、車載カメラ10により撮像された撮像画像中の路面表示に対応するエッジを検出することで、エッジ画像を生成する機能である。ここで、エッジとしては、たとえば、画素の輝度が鋭敏に変化している箇所などが挙げられる。また、エッジの検出方法としては、たとえば、Canny法や、微分エッジ検出法などの公知の方法を採用することができる。ここで、図2は、撮像画像からエッジ画像を生成する方法を説明するための図であり、図2(A)は車載カメラ10により撮像された画像の一例であり、図2(B)は、図2(A)の画像から検出されたエッジに基づいて生成されたエッジ画像である。図2(A)、図2(B)においては、車載カメラ10により撮像された撮像画像中の白線部分がエッジとして検出され、白線部分のエッジを含むエッジ画像が生成された場面を示している。
制御装置30のオドメトリ算出機能は、車両センサ群40から送出された信号に基づいて、単位時間当たりに車両が進んだ移動距離および車両が方向転換した際の回転量を算出する機能である。車両の移動距離および回転量の算出方法としては、たとえば、車両の運動を平面上に限定した上で、車両センサ群40を構成する車輪速センサにより検出された車輪速とヨーレートセンサにより検出された車輪の回転速度とから算出することができる。また、車輪速の代わりに車速センサにより検出された車速を用いたり、車輪の回転速度の代わりにステアリングセンサにより検出された操舵角を用いても、上記と同様に車両の移動距離と回転量とを算出することができる。
制御装置30のパーティクル生成機能、位置予測機能、評価点投影機能、位置評価機能、および位置統合機能は、パーティクルフィルタ手法を用いて車両位置を算出するための機能である。パーティクルフィルタ手法は、移動体の追跡等を行うアルゴリズムであり、一般的には、前状態における尤度に基づいて、パーティクルを選び直す「リサンプリング」と、パーティクルを状態方程式等に基づいて移動させる「予測」と、移動させた各パーティクルの尤度を求める「重み付け」と、算出した尤度から現在の状態を算出する「状態推定」との4ステップからなる。本実施形態においては、過去に算出した車両位置から、現在までに車両が移動しうる多数の予測位置を多数のパーティクルに見立て、全パーティクルの確からしさである尤度を算出し、その尤度に基づいて現在の車両位置を算出するという処理を繰り返し、逐次車両の追跡を行っていく。
以下、上述した各機能を説明しながら、パーティクルフィルタ手法を用いた車両位置の算出方法を詳細に説明する。
まず、制御装置30は、パーティクル生成機能により、パーティクルフィルタ手法の「リサンプリング」を実行する。具体的には、制御装置30は、まず、3次元地図上において、前回算出した車両位置の周囲に、ランダムにばらつかせた複数のパーティクルを生成する。ここで、図3は本実施形態におけるパーティクルフィルタ手法を用いた車両位置算出の手順を示す図である。たとえば、制御装置30は、図3(A)に示すように、前回算出した車両位置の周囲に、車両位置の候補地点となる複数のパーティクルを生成する。
次いで、制御装置30は、位置予測機能により、パーティクルフィルタ手法の「予測」を実行する。具体的には、制御装置30は、図3(A)に示すように、パーティクル生成機能により生成した車両位置の候補地点となる複数のパーティクルに対して、上述したオドメトリ算出機能により算出した車両の移動距離および回転量に応じて、移動させた位置を算出し、算出した位置に、図3(B)に示すように、予測車両位置のパーティクルを生成する。
次いで、制御装置30は、評価点投影機能により、データベース20に備えられている3次元地図に記憶されている情報に基づき、評価点投影画像を生成する。具体的には、まず、制御装置30は、データベース20に備えられた3次元地図上において、上述した位置予測機能により生成した予測車両位置に対応する位置を求める。たとえば、図3(B)に示す例においては、同図に示された予測車両位置のパーティクルが生成された位置を求める。そして、制御装置30は、3次元地図中に記憶されている各エッジに対応する評価点について、予測車両位置から車載カメラ10により撮像を行なったと仮定した場合に得られる撮像画像中において写る位置を算出し、この算出結果に基づいて、各エッジに対応する評価点の3次元地図上の位置と撮像画像上の位置とを変換する処理を行うことで、評価点投影画像を生成する。
ここで、図4は、3次元地図上の各エッジに対応する評価点を、投影変換して評価点投影画像を生成する方法を説明するための図であり、図4(A)は3次元地図を示しており、図4(B)は図4(A)に示す3次元地図を投影変換して得られる評価点投影画像を示している。なお、図4(A)においては、3次元地図中に含まれる各エッジおよび評価点を、真上から見た場合における図を示している。また、図4(A)においては、特に図示していないが、同図において、自車両の予測車両位置が図4(A)の下端の左車線側にあると仮定している。そして、本実施形態においては、このような図4(A)に示すような3次元地図中に含まれる評価点を、図4(B)に示すように、予測車両位置からの投影画像に変換することで、各エッジに対応する複数の評価点が投影された評価点投影画像を得る。なお、図4(B)中においては、理解を容易なものとするために、各評価点に対応するエッジも示したが、通常は、価点投影画像中には、評価点のみが存在し、エッジは存在しないこととなる。また、制御装置30は、位置予測機能により生成した全ての予測車両位置に対して、このような評価点投影画像の生成を行なう。
なお、本実施形態においては、投影変換を行う際には、3次元地図上における車載カメラ10に対する位置、および車載カメラ10の特性等(たとえば、レンズの歪み等)の情報を用いて、投影変換を行なう。なお、3次元地図上における車載カメラ10の位置は、たとえば、自車両の重心位置から車載カメラ10までの相対位置を予め計測しておくことで、予測車両位置から算出することができる。また、車載カメラ10の特性等の情報は、車載カメラ10を予めキャリブレーションしておくことにより、取得することができる。
さらに、本実施形態において、評価点投影機能により評価点投影画像を生成する際には、車載カメラ10により撮像された撮像画像中において、上述したエッジ画像算出機能により、エッジとして検出される可能性が低い位置に対応する評価点を除いて、評価点投影画像を生成するような構成としてもよい。
具体的には、本実施形態においては、評価点投影画像を生成する際に、3次元地図上において、予測車両位置から所定の遠方距離以上にある評価点を除いて評価点投影画像を生成してもよい。すなわち、3次元地図上において、位置予測機能により算出された予測車両位置と各評価点との3次元距離を算出し、該当する評価点を除いて、評価点投影画像を生成することができる。特に、車載カメラ10により撮像された撮像画像中においては、車載カメラ10からの3次元距離が遠いほど、エッジの検出が困難となるため、評価点投影画像を生成する際にも、予測車両位置から所定の遠方距離以上となる評価点を予め除いておくことで、本実施形態における車両位置算出を行う際の計算効率を高めることができる。
あるいは、本実施形態においては、3次元地図上において、位置予測機能により算出された予測車両位置に対して、広角側に位置する評価点を除いて評価点投影画像を生成してもよい。具体的には、予測車両位置から車載カメラ10を用いて撮影したと仮定した場合の車載カメラ10の光軸と、車載カメラ10と評価点とを結ぶ直線との成す角度を算出し、算出された角度が所定の閾値以上となる場合には、該評価点を除いて評価点投影画像を生成することができる。ここで、所定の閾値は、たとえば、車載カメラ10の画角に応じて決定することができ、これにより、車載カメラ10により撮像できない範囲に位置する評価点を除くことができるため、本実施形態における車両位置算出を行う際の計算効率を高めることが可能となる。
さらに、本実施形態においては、3次元地図に予め記憶された立体物情報に基づいて、その立体物があることによって遮蔽されてしまう評価点を除いて、評価点投影画像を生成してもよい。具体的には、3次元地図上において、路面の周囲に存在する立体物を検出し、位置予測機能により算出された予測車両位置から車載カメラ10を用いて撮影したと仮定した場合に、立体物があることによって遮蔽される評価点を除いて、評価点投影画像を生成することができる。これにより、車載カメラ10により撮像される撮像画像中において、立体物によって遮蔽されてしまい、撮像画像中においてエッジとして検出されない領域に位置する評価点を除くことができるため、本実施形態における車両位置算出を行う際の計算効率を高めることが可能となる。
次いで、制御装置30は、位置評価機能により、パーティクルフィルタ手法の「重み付け」を実行する。具体的には、制御装置30は、上述したエッジ画像算出機能により生成されたエッジ画像(図2(B)参照)と、評価点投影機能により生成された評価点投影画像(図4(B)参照)とを比較する処理を、全ての予測車両位置(図3(B)参照)について行い、全ての予測車両位置ごとに、一致度合いを示す尤度を算出する。なお、本実施形態においては、尤度の算出は、評価点投影画像(図4(B)参照)中の全ての評価点について、エッジ画像(図2(B)参照)中の対応する位置にエッジが存在しているか否かを判定することにより行なわれる。たとえば、上述したように評価点投影画像(図4(B)参照)上の評価点について、エッジ画像(図2(B)参照)中の対応する位置にエッジが存在している場合に1を加算する、という処理を、全評価点に対して実施し、その総和である一致評価点の数を尤度とすることができる。そして、全ての予測車両位置(図3(B)参照)における評価点投影画像(図4(B)参照)について、尤度を算出したら、全ての予測車両位置の尤度の合計値が1になるよう、尤度を正規化する。なお、尤度の算出方法としては、このような方法に特に限定されるものではなく、たとえば、評価点投影画像上の全評価点のうち、エッジ画像中の対応する位置にエッジが存在している評価点の割合を求め、これを尤度としてもよい。
次いで、制御装置30は、位置統合機能により、パーティクルフィルタ手法の「予測」を実行する。具体的には、制御装置30は、上述した位置評価機能により算出した全ての予測車両位置の尤度に基づいて、現在の車両位置の算出を行なう。なお、現在の車両位置の算出方法としては、たとえば、全ての予測車両位置のうち、最も尤度が高い位置を、現在の車両位置とする方法などが挙げられる。すなわち、たとえば、図3(C)に示すように、図3(B)において算出された複数の予測車両位置のなかから、複数の予測車両位置のうち最も尤度が高い位置を、現在の車両位置とすることができる。あるいは、複数の予測車両位置を尤度に応じて重み付き平均を取ることにより算出することとしてもよい。
なお、本実施形態では、このようにして算出された各予測車両位置における尤度を、上述するパーティクル生成機能により、車両位置の候補地点となる複数のパーティクルを生成する際に用いてもよい。具体的には、各予測車両位置における尤度に基づいて、尤度の高い予測車両位置に対応する位置については、その近傍により多くのパーティクルを生成させ、一方で、尤度の低い予測車両位置に対応する位置については、生成するパーティクルの数を少なく設定するような構成としてもよい。なお、この際において、生成するパーティクルの数としては、任意の数とすることができるが、たとえば、100個程度とすることができる。さらに、この際においては、尤度が所定値以下である予測車両位置に対応する位置については、パーティクルを全く生成しないような構成としてもよい。
本実施形態においては、このようにして、パーティクルフィルタ手法を用いて、現在の車両位置を算出する。
次いで、制御装置30の備える評価点追加機能および評価点削除機能について説明する。
制御装置30は、評価点追加機能により、上述した評価点投影画像(図4(B)参照)を得る際に、データベース20に備えられた3次元地図上において、評価点の追加を行なう。具体的には、制御装置30は、全体として評価点の密度が高い評価点投影画像が得られるように、3次元地図上におけるエッジの角度に応じて、エッジ上に評価点を設定する。
ここで、図5は、3次元地図上の評価点を投影変換して評価点投影画像を生成する際の評価点の偏りについて説明するための図である。図5(A)は、3次元地図上において、車両の進行方向をx軸方向をとし、車両の進行方向と直交し、路面と平行な方向をy軸方向とし、路面と垂直な方向をz軸方向とし、それぞれの軸の矢印が向く方向を正方向とした場合に、x軸に平行な一対の直線状に並んだエッジと、y軸に平行な2つの直線状のエッジとが存在している場面を例示している。ここで、図5(A)に示すような、x軸に平行な直線状のエッジとしては、たとえば、車両の進行方向と平行に存在している走行レーンを示す白線等が該当し、y軸に平行なエッジとしては、たとえば、車両の進行方向と垂直に存在している停止線等が該当する。なお、図5(A)においては、3次元地図は、エッジの情報を、所定距離毎に区切られたエッジの集合体とし、これら所定距離毎に区切られたエッジの両端点の位置を評価点として記憶している。そのため、図5(A)中においては、一対の直線状のエッジ上には、それぞれ複数の評価点が設定されている。
そして、図5(A)中に示すように、たとえば、車載カメラ10の光軸(車載カメラ10に備えられたレンズ、撮像素子の中心を通る光の軸)がx軸方向に平行な方向に向いており、車載カメラ10によりx軸方向に平行な方向に撮像するとした場合に、得られる投影変換画像は、図5(B)に示すものとなる。すなわち、図5(A)に示すような、エッジ上に設定された各評価点を単純に投影変換した際に、車載カメラ10からの3次元距離が同程度であるy軸に平行なエッジおよびx軸に平行なエッジに着目すると、y軸に平行なエッジは、投影変換画像上で大きく投影される一方、x軸に平行なエッジは、投影変換画像上で小さく投影されることとなる。すなわち、3次元距離が同程度である場合でも、y軸に平行なエッジは、投影変換画像上において、エッジ上に設定される評価点の密度が低くなる一方で、x軸に平行なエッジは、投影変換画像上において、エッジ上に設定される評価点の密度が高くなることとなる。本実施形態では、このような傾向は、車載カメラ10の方向と相関しており、車載カメラ10の光軸方向(予測車両位置におけるz軸の正方向から俯瞰して見た場合の光軸の方向)に対する角度が大きいエッジ(たとえば、y軸に平行なエッジ)ほど、3次元距離が同程度であっても、投影変換画像上で大きく投影され、そのため、評価点の密度が低くなる傾向にある。その一方で、車載カメラ10の光軸方向に対する角度が小さいエッジ(たとえば、x軸に平行なエッジ)ほど、投影変換画像上で小さく投影され、そのため、評価点の密度が高くなる傾向にある。なお、図5(B)において、エッジが湾曲して投影されているのは、車載カメラ10が備えるレンズの歪みを考慮しているためである。そこで、本実施形態では、車載カメラ10の光軸方向に対するエッジの角度に応じて生じる、評価点投影画像上における各エッジ上の評価点の密度の差を緩和し、得られる評価点投影画像を、図5(C)に示すように、評価点が偏りなく配置された状態とするために、制御装置30の評価点追加機能により、3次元地図上において評価点の追加を行う。
ここで、図6は、エッジ上に評価点を追加する方法を説明するための図である。なお、図6(A)においては、直線状のエッジの両端点のみに評価点が設定されている例を示している。そして、図6(A)中に示すように、たとえば、車載カメラ10により撮像したとした場合に、得られる投影変換画像は、図6(B)に示すものとなる。そして、このような場面において、評価点を追加する場合には、図6(B)に示すように、各評価点のちょうど中間に位置するように配置することが理想的である。
しかしその一方で、評価点投影画像上において、投影された評価点の中間点に、評価点を追加すると、図6(C)に示すように、追加した評価点がエッジ上からはずれてしまう。これは、車載カメラ10のレンズには歪みが存在するために直線状のエッジが投影画像上では歪むことに起因するもので、特に魚眼レンズ等の広角レンズを搭載したカメラでは顕著となる。また、3次元地図上で直線状のエッジの中間点に評価点を追加してから投影変換すると、上述したように、車載カメラ10からの3次元距離が近いほど、評価点の密度が低くなる傾向にあるため、図6(D)に示すように、追加した評価点が3次元距離の遠い方向に偏った位置に配置されてしまうこととなる。ここで、投影変換画像を図6(B)に示すように理想的なものとするためには、投影変換後の評価点の位置を計算した上で、評価点を追加する方法が考えられるが、評価点毎に投影変換後の評価点の位置を計算しなければならず、計算コストの増大を招く。
そのため、本実施形態においては、制御装置30は、評価点追加機能により、以下に説明する方法にしたがって、データベース20に記憶されている3次元地図上において、評価点の追加を行う。
図7(A)は、3次元地図において、互いに同じxy平面上に位置し、長さが同じで、向きが異なるエッジα,βが存在する場面を例示する図である。なお、図7(A)においては、車載カメラ10の光軸がx軸方向に平行な方向に向いており、車載カメラ10によりx軸方向に平行な方向に撮像する場面を示している。ここで、図7(A)におけるx,y,z軸の各軸は、上述した図5(A)のx,y,z軸と同様に設定されている。そして、図7(A)中に示すように、車載カメラ10により、x軸方向に平行な方向に撮像したとした場合に、得られる投影変換画像は、図7(B)に示すものとなる。図7(B)においては、車載カメラ10からの3次元距離が同程度であるにもかかわらず、エッジαは、エッジβと比較して大きく投影され、評価点の密度も低くなることとなる。これは、上述したように、3次元地図上において、車載カメラ10の光軸方向に対するエッジα,βの向きがそれぞれ異なることに起因している。
すなわち、図7(A)に示す場面を、z軸の正方向、すなわち、車載カメラ10の真上(予測車両位置の真上)からxy平面を俯瞰して見た場面を示す図8(A)に示すように、車載カメラ10の真上(予測車両位置の真上)から、xy平面を俯瞰して見た場合における、車載カメラ10の光軸とエッジαとの成す角度θα、車載カメラ10の光軸とエッジβとの成す角度θβが異なるため、投影変換画像中における大きさが異なることとなり、結果として、評価点の密度も異なることとなる。
そのため、本実施形態においては、車載カメラ10の真上(予測車両位置の真上)から、xy平面を俯瞰して見た場合における、車載カメラ10の光軸と各エッジとの成す角度である俯瞰角度θを求め、俯瞰角度θが大きいエッジほど、追加される評価点の数が多くなるような態様で、データベース20に記憶されている3次元地図上において、評価点の追加を行なう。そして、これにより、各エッジの俯瞰角度θに拘わらず、全体として評価点の密度が高い評価点投影画像を得ることが可能となる。すなわち、図8(A)に示す場面においては、車載カメラ10の光軸とエッジαとの成す角度θαの方が、車載カメラ10の光軸とエッジβとの成す角度θβよりも大きいため、エッジαの方が追加される評価点の数が多くなるように、評価点の追加を行ない、そして、追加した評価点と、3次元地図上に予め設定されている評価点とを投影変換することで、図8(B)に示すように、全体として評価点の密度が高い評価点投影画像を得ることができる。なお、図8(B)中においては、予め設定されている評価点は黒丸で示し、追加した評価点は白丸で示している。すなわち、図8(B)においては、エッジα上には白丸で示す5つの評価点が等間隔で追加され、エッジβ上にはエッジβの中間点に白丸で示す1つの評価点が追加された例を示している。これにより、各エッジの俯瞰角度θに拘わらず、全体として評価点の密度が高い評価点投影画像を得ることができる。なお、評価点を追加する際には、評価点投影画像全体としての評価点の数が、上述した位置評価機能によりエッジ画像と評価点投影画像とを比較する際に必要となる評価点の数よりも、多い数となるように、評価点を追加することが望ましい。
また、本実施形態において、評価点を追加する具体的な方法としては、特に限定されず、俯瞰角度θが大きいエッジほど、追加される評価点の数が多くなるような方法であれば何でもよいが、たとえば、図8(A)に示すように、俯瞰角度θを算出し、算出された角度に応じて評価点を追加していく方法が挙げられる。具体的には、評価点を追加する際に基準とする評価点間の間隔を追加点間距離Deとし、この追加点間距離Deを、De=K1×(K2−θ/90)に従って求め、各評価点間の間隔が追加点間距離Deとなるように評価点を追加する方法を採用することができる。ここで、K1およびK2は係数であり、任意の値とすることができる。なお、エッジが曲線である場合には、エッジ両端点を結ぶ直線を求め、車載カメラ10の光軸と、エッジ両端点を結ぶ直線とを車載カメラ10の真上(予測車両位置の真上)から見た場合に、車載カメラ10の光軸と、エッジ両端点を結ぶ直線とが成す角度を、俯瞰角度θとして算出してもよい。
ここで、たとえば、K1を1.5とし、K2を1.2と定めると、車載カメラ10の光軸と、エッジとを車載カメラ10の真上(予測車両位置の真上)から見た場合における、車載カメラ10の光軸方向と平行なエッジ(すなわち、θ=0°であるエッジ)では1.8mおきに評価点を追加し、車載カメラ10の光軸方向に対し垂直なエッジ(すなわち、θ=90°であるエッジ)では0.3mおきに評価点を追加することとなり、俯瞰角度θが大きいエッジほど追加点間距離Deは短くなることとなる。そのため、俯瞰角度θが大きく、画像上では大きく写るために評価点の密度が低くなってしまうエッジにおいては、追加点間距離Deは短くなり、より多くの評価点が設定されることとなる。このように、本実施形態においては、全てのエッジについて一様に評価点を追加するのではなく、俯瞰角度θが大きいほど、評価点投影画像上の評価点の密度が低くなることを利用して、俯瞰角度θが大きいほど、追加される評価点の数が多くなるような手法を採用することにより、車両位置算出を行う際の計算効率を高めることが可能となる。
なお、追加点間距離Deには、上限値および/または下限値を定めてもよく、この場合には、追加点間距離Deが極端に小さい値や、大きい値になることを防ぐことができる。また、追加点間距離Deの算出方法としては、上述した方法に代えて、De=K1×(K2−θ/90)×(K2−θ/90)のようなθの2次式で算出する方法を採用してもよいし、あるいは、車載カメラ10の特性を考慮に入れた上で予め用意されたθとDeとの関係を示すマップに基づいて算出する方法を採用してもよい。そして、上記方法にしたがって求めた追加点間距離Deと、エッジの長さLとを用いて、各エッジ上に追加する評価点の数を、たとえば、L/Deで算出することができる。なお、この場合において、少数点以下の値は、切り捨ててもよいし、あるいは、切り上げてもよいし、さらには四捨五入してもよい。そして、制御装置30は、追加する評価点の数を決定した後、エッジの両端の評価点の3次元位置に基づいて、3次元地図上に追加する評価点の3次元位置を決定する。なお、3次元地図上に追加する評価点の3次元位置の決定方法としては、3次元地図上において、等間隔に配置されるような位置としてもよいし、あるいは、投影後の位置を考慮したような位置としてもよい。
ここで、図5(A)に示す場面において、上述した本実施形態における方法により評価点を追加した例を図9(A)に示す。具体的には、制御装置30は、俯瞰角度θが大きいエッジであるy軸に平行なエッジ上に白丸で示す5つの評価点を等間隔で追加し、俯瞰角度θが小さいエッジであるx軸に平行なエッジ上にエッジの中間点に白丸で示す1つの評価点を追加している。これにより、各エッジの俯瞰角度θに拘わらず、全体として評価点の密度が高い評価点投影画像を得ることができる。
そして、制御装置30は、上述した方法にしたがい、3次元地図上に予め設定されている評価点(たとえば、図9(A)中の黒丸)に加えて、上述した評価点追加機能により追加された評価点(たとえば、図9(A)中の白丸)を、投影変換して図9(B)に示すような評価点投影画像を生成する。
次いで、制御装置30は、評価点削除機能により、3次元地図上に予め設定されている評価点および追加された評価点を投影変換することで得られた評価点投影画像について、該評価点投影画像中に含まれる全ての評価点のうち、一部の評価点を削除する処理を行なう。具体的には、制御装置30は、図9(B)に示す評価点投影画像中において、該評価点投影画像中に存在する各評価点間の間隔が予め設定した所定の間隔Dmin以上となるように、一部の評価点を削除することで、図9(C)に示すような評価点削除後の評価点投影画像を得る。なお、この際においては、図9(B)、図9(C)に示すように、各評価点間の間隔が予め設定した所定の間隔Dmin以上となるように、評価点追加機能により追加された評価点(図9(B)、図9(C)中において白丸で示した。)だけでなく、3次元地図上に予め設定されている評価点(図9(B)、図9(C)中において黒丸で示した。)についても削除を行なう。このように不要な評価点の削除を行うことで、図9(C)に示すように、残された評価点同士が所定の間隔Dmin以上の間隔で配置されることとなり、その結果、評価点投影画像上の評価点の密度が低くなる傾向にある、車載カメラ10からの3次元距離が近いエッジに対応する評価点の配置が、エッジ画像との比較に適した状態となるように評価点が削除され、評価点が偏りなく配置された評価点投影画像が得られる。
なお、予め設定した所定の間隔Dminは、たとえば、評価点投影画像上の所定のピクセル数で設定することができ、これにより、エッジ画像との比較に適した状態となるように評価点が削除され、評価点が偏りなく配置された評価点投影画像を得ることができる。また、本実施形態においては、評価点を削除する際には、評価点投影画像上の任意の評価点を選択して、その評価点の周囲の評価点を削除しようとすると、処理の順番によって、削除されずに残る評価点が変わってしまうことがある。そのため、本実施形態においては、予測車両位置と評価点との3次元距離を算出した上で、予測車両位置に近い評価点から順に評価点の削除を行うことで、予測車両位置に近いエッジに対応した評価点を残すこととしている。これにより、エッジ検出が比較的容易である予測車両位置に最も近いエッジに対応する評価点を残すことができ、車両位置算出の精度向上に寄与する。たとえば、図9(B)に示した評価点のうち、予測車両位置に近い評価点から順に、上述した評価点の削除を行うことにより、図9(C)に示すように、予測車両位置に最も近い評価点(図9(C)中で最も下にある評価点)を残すことができる。
ここで、図5(A)に示す場面において、上述した本実施形態に係る方法を用いずに、評価点を追加する方法、すなわち、全てのエッジについて、一様に、評価点を追加する方法を用いた場合を例示した結果を図10(A)、図10(B)に示す。すなわち、図10(A)においては、全てのエッジについて、それぞれのエッジの中間点に1つの評価点を追加しており、そして、図10(A)における全ての評価点を投影変換して生成された評価点投影画像は図10(B)に示すものとなる。ここで、図10(B)に示すように、俯瞰角度θによらず、全てのエッジについて一様に評価点を追加した場合には、俯瞰角度θが大きいエッジである、y軸に平行なエッジ上の評価点密度が低下してしまうこととなり、俯瞰角度θが小さいエッジである、x軸に平行なエッジ上に評価点が集中して存在することとなってしまい、結果として評価点投影画像中において、評価点が偏ってしまう。そして、評価点が偏ってしまうことにより、正確な車両位置が算出できなくなるおそれがある。これに対し、本実施形態においては、上述した方法に従って、評価点の追加を行なうことにより、俯瞰角度θが大きいほどエッジ上に評価点を多く設定することで、全体として評価点の密度が高い評価点投影画像を得ることができる。
そして、本実施形態においては、このようにして、制御装置30の備える評価点追加機能および評価点削除機能により、3次元地図上において評価点の追加を行い、追加した評価点も含めて評価点投影画像を生成し、生成した評価点投影画像から一部の評価点を削除することで、評価点削除後の評価点投影画像を得る。そして、このようにして得られた評価点削除後の評価点投影画像を用いて、上述したパーティクルフィルタ手法を用いて、現在の車両位置の算出を行なう。
なお、本実施形態においては、評価点削除機能により、評価点を削除する処理を行なう際には、上述した所定の間隔Dminを、評価点に対応するエッジに応じて変化させてもよい。たとえば、評価点に対応するエッジが、位置予測機能により算出された予測車両位置における車両進行方向に対して、略垂直であり、かつ地平線と略水平であるエッジに該当する場合には、該評価点については、所定の間隔Dminを、他の評価点よりも小さい値としてもよい。すなわち、このような評価点については、他の評価点と比較して、評価点間の間隔を狭く設定してもよい。なお、この場合おいて、車両進行方向に対して厳密に垂直な方向、あるいは地平線に対して厳密に水平な方向である必要はなく、たとえば、±20°程度ずれている場合でも、略垂直または略水平と判断してもよい。たとえば、車載カメラ10が車両フロント部に設置されており、車両前方を撮影するものであるような場合は、路面上の停止線のエッジ等が、車両進行方向に略垂直であり、かつ地平線と略水平であるエッジとなる。具体的には、図7(A)に示す場面において、エッジαが、車両進行方向に対して、略垂直であり、かつ地平線と略水平であるエッジに該当すると判断された場合には、エッジαに対応する評価点については、上記の所定の間隔Dminを小さく設定し、エッジαに対応する評価点の数を比較的多く設定することができる。特に、車両進行方向に略垂直であり、かつ地平線と略水平であるエッジは、車両が前後方向にわずかに移動した場合にも、評価点投影画像上において、他のエッジよりも大きく移動するものであることから、このようなエッジに対応する評価点の密度を高く設定しておくことで、車両の前後方向のわずかな移動を検出することができるため、より正確に車両位置算出を行うことができる。なお、上述した方法による、略垂直であり、かつ地平線と略水平であるエッジ上の評価点についての所定の間隔Dminの設定においては、さらに、評価点投影画像における予測車両位置からの3次元距離が近いエッジほど、所定の間隔Dminを小さく設定してもよい。
また、本実施形態においては、評価点削除機能により、評価点を削除する処理を行なう際には、評価点に対応するエッジが所定のランドマークである場合には、所定の間隔Dminを、他の評価点よりも小さく設定してもよい。なお、このような所定のランドマークは、上述したように、予め3次元地図に記憶させておくことができ、たとえば、所定のランドマークとしては、車載カメラ10により撮像された撮像画像中においてエッジの検出が比較的容易である横断歩道、横断歩道予告、および進行方向別通行区分等の路面表示や、案内板や建物などの立体物等が挙げられる。これにより、エッジの検出が比較的容易である所定のランドマークにおけるエッジ上の評価点密度を高く設定しておくことができ、より正確に車両位置算出を行うことができる。なお、上述した方法による、所定のランドマークにおけるエッジ上の評価点についての所定の間隔Dminの設定においては、さらに、評価点投影画像における予測車両位置からの3次元距離が近いエッジほど、所定の間隔Dminを小さく設定してもよい。
さらに、本実施形態においては、評価点削除機能により、評価点を削除する処理を行なう際には、3次元地図に記憶されたエッジ情報から、エッジ検出のし易さを示すエッジ検出強度を算出し、算出したエッジ検出強度に基づいてエッジ毎に上記の所定の間隔Dminを変化させてもよい。具体的には、エッジ検出強度が0〜1で表現され、値が大きいほどエッジを検出し易いとした場合、所定の間隔Dminを、下記式(1)にしたがってエッジ毎に算出して、エッジ毎に算出した所定の間隔Dminを用いて、評価点の削除を行なうことができる。
Dmin=D0/(E+K) ・・・(1)
なお、上記式(1)において、Eはエッジ検出強度であり、K、D0は任意の値に設定できる係数である。ここで、たとえば、Kを1と定めると、E=0である場合は、E=1である場合と比較して所定の間隔Dminの値は2倍の大きさとなる。これにより、エッジ検出をし易いエッジ上に多くの評価点を設定することができるため、光学環境の変化によりエッジを検出し難い環境においても、より正確に車両位置算出を行うことができる。
次いで、本実施形態の動作例を説明する。図11は本実施形態における車両位置算出方法の一例を示すフローチャートである。図11に示す処理内容は、制御装置30において、たとえば、100ms程度の間隔で連続的に繰返し行われる。
まず、ステップS110では、車載カメラ10は、車両周囲の路面表示を含む撮像画像を取得し、取得された撮像画像は制御装置30へ送出される。そして、制御装置30は、エッジ画像算出機能により、車載カメラ10が取得した撮像画像中の路面表示に対応するエッジを検出し、検出したエッジに基づいてエッジ画像を生成する。たとえば、車載カメラ10が車両周辺を撮像し、図2(A)に示すような撮像画像を取得し、制御装置30は、図2(A)の画像から路面表示に対応するエッジを検出し、検出したエッジに基づいて図2(B)に示すエッジ画像を生成する。
ステップS120では、制御装置30は、パーティクル生成機能により、3次元地図上において、前回算出した車両位置の周囲に、ランダムにばらつかせた複数のパーティクルを生成する。たとえば、制御装置30は、図3(A)に示すように、前回算出した車両位置の周囲に、車両位置の候補地点となる複数のパーティクルを生成する。なお、本処理を開始して初めて車両位置を算出する場合は、前回の予測車両位置の情報がないため、車両センサ群40に含まれるGPSが取得したデータを予測車両位置情報に代わる情報として活用することができる。
ステップS130では、制御装置30は、オドメトリ算出機能により、車両センサ群40から送出された信号に基づき、単位時間当たりに車両が進んだ移動距離および車両が方向転換した際の回転量を算出する。なお、本処理を開始して初めて車両位置を算出する場合は、車両の移動距離および回転量をゼロとして算出する。
ステップS140では、制御装置30は、位置予測機能により、ステップS120で生成した複数のパーティクルを、ステップS130で算出した車両の移動距離および回転量に応じて、移動させた位置を算出し、算出した位置に予測車両位置のパーティクルを生成する。たとえば、図3(B)に示すように、ステップS120で生成した白色の丸で表したパーティクルを、ステップS130で算出した車両の移動距離および回転量に応じて移動させ、灰色の丸で表した新たなパーティクルを生成する。
ステップS150では、制御装置30は、評価点追加機能により、データベース20に記憶されている3次元地図上において、評価点の追加を行う。具体的には、制御装置30は、俯瞰角度θが大きいエッジほど、追加される評価点の数が多くなるような態様で、データベース20に記憶されている3次元地図上において、評価点の追加を行なう。たとえば、図9(A)に示すように、3次元地図上にx軸に平行な一対の直線状に並んだエッジと、y軸に平行な2つの直線状のエッジとが存在している場面において、予め記憶された評価点(図9(A)中の黒丸で示す)が属するエッジ上に、新たな評価点(図9(A)中の白丸で示す)を追加する処理を行う。
ステップS160では、制御装置30は、評価点投影機能により、3次元地図中の評価点を、予測車両位置から投影して得られる評価点投影画像の生成を行う。具体的には、まず、制御装置30は、3次元地図上において、ステップS140で生成した予測車両位置に対応する位置を求める。たとえば、図3(B)に示す例においては、同図に示された予測車両位置のパーティクルが生成された位置を求める。そして、制御装置30は、予め3次元地図中に記憶されている評価点、およびステップS150で追加した評価点を投影変換することで、評価点投影画像を生成する。たとえば、図9(A)に示すように、3次元地図上に予め設定されている評価点(たとえば、図9(A)中の黒丸)に加えて、ステップS150で追加した評価点(たとえば、図9(A)中の白丸)を、投影変換して図9(B)に示すような評価点投影画像を生成する。
ステップS170では、制御装置30は、評価点削除機能により、3次元地図上に予め設定されている評価点および追加された評価点を投影変換することで得られた評価点投影画像について、該評価点投影画像中に含まれる全ての評価点のうち、一部の評価点を削除する処理を行なう。たとえば、図9(B)に示す評価点投影画像中において、該評価点投影画像中に存在する各評価点間の間隔が予め設定した所定の間隔以上となるように、一部の評価点を削除することで、図9(C)に示すような評価点削除後の評価点投影画像を得る。
ステップS180では、制御装置30は、位置評価機能により、ステップS110で生成したエッジ画像(図2(B)参照)と、ステップS160〜170で予測車両位置毎に生成した各評価点投影画像(図4(B)参照)とを比較する処理を、全ての予測車両位置(図3(B)参照)について行い、全ての予測車両位置毎に尤度を算出する。
ステップS190では、制御装置30は、位置統合機能により、ステップS180で算出した全ての予測車両位置の尤度情報に基づいて、現在の車両位置の算出を行う。なお、現在の車両位置の算出方法としては、制御装置30は、全ての予測車両位置のうち、最も尤度が高い位置を、現在の車両位置とする方法などが挙げられる。すなわち、たとえば、図3(C)に示すように、図3(B)において算出された複数の予測車両位置のなかから、複数の予測車両位置のうち最も尤度が高い位置を、現在の車両位置とすることができる。あるいは、複数の予測車両位置を尤度に応じて重み付き平均を取ることにより現在の車両位置を算出することとしてもよい。現在の車両位置が算出されたら、ステップS110に戻り、上述した車両位置算出処理が繰り返し実行され、逐次車両の追跡が行われる。
以上のとおり、本実施形態においては、実際の撮像画像からエッジを検出することでエッジ画像を生成し、前回算出した車両位置と、単位時間当たりに車両が移動した移動距離および回転量とに基づいて、現在の予測車両位置を算出し、3次元地図における評価点群を予測車両位置から投影して得られる評価点投影画像を予測車両位置毎に生成し、エッジ画像と評価点投影画像とを比較することで得られる算出した全ての予測車両位置の尤度情報に基づいて、現在の車両位置の算出を行う。ここで、評価点投影画像を生成する際に、3次元地図上において、予測車両位置から車載カメラ10を用いて撮影したと仮定した場合の車載カメラ10の光軸と、エッジとを予測車両位置の真上から見た場合に、車載カメラ10の光軸と、エッジとが成す角度が大きいほど、追加される評価点の数を多くし、設定した評価点を投影して得た評価点投影画像上において、予測車両位置と評価点との距離に応じて評価点を削除することで、評価点投影画像上の評価点の密度が低くなる傾向にある、車載カメラ10からの3次元距離が近いエッジに対応する評価点の配置が、エッジ画像との比較に適した状態となるように評価点が削除され、評価点が偏りなく配置された評価点投影画像を得ることができ、安定的に正確な車両位置算出を行うことが可能となる。
また、エッジ上に評価点を追加する際に、全てのエッジについて一様に評価点を追加することは計算コストの増大を招くことから、本実施形態においては、3次元地図上において、予測車両位置から車載カメラ10を用いて撮影したと仮定した場合の車載カメラ10の光軸と、エッジとを予測車両位置の真上から見た場合に、車載カメラ10の光軸と、エッジとが成す角度が大きいエッジほど、評価点投影画像上の評価点の密度が低くなることを利用して、該角度が大きいほど、追加される評価点の数が多くなるような手法を採用することにより、車両位置算出を行う際の計算効率を高めることが可能となる。
なお、本実施形態においては、3次元地図上において、予測車両位置から車載カメラ10を用いて撮影したと仮定した場合の車載カメラ10の光軸と、エッジとを予測車両位置の真上から見た場合に、車載カメラ10の光軸と、エッジとが成す角度に基づいて、評価点の追加を行う例を示したが、該角度を求める際には、予測車両位置に任意のカメラが存在すると仮定し、該カメラの光軸と、エッジとが成す角度を求めるような構成としてもよい。
以上、本発明の実施形態について説明したが、これらの実施形態は、本発明の理解を容易にするために記載されたものであって、本発明を限定するために記載されたものではない。したがって、上記の実施形態に開示された各要素は、本発明の技術的範囲に属する全ての設計変更や均等物をも含む趣旨である。
たとえば、制御装置30は、3次元地図上において、車載カメラ10の撮像範囲に他の車両等が映りこむ可能性が高い領域であるオクルージョン頻発領域を検出し、評価点削除機能により、評価点を削除する処理を行なう際に、オクルージョン頻発領域内にある評価点については、所定の間隔Dminを、他の評価点よりも大きい値としてもよい。ここで、オクルージョン頻発領域は、予測車両位置から車載カメラ10を用いて撮影したと仮定した場合に、車載カメラ10の撮像範囲に他の車両等が映りこむ可能性が高い領域である。また、図12にオクルージョン頻発領域の一例を示す。図12においては、車載カメラ10により車両進行方向を撮像した際に、撮像画像に写った路面上の白線と、先行車両とを示し、先行車両が写っている領域をオクルージョン頻発領域として、点線で囲んでいる。ここで、図12に示すようなオクルージョン頻発領域を、3次元地図上に設定する方法は、たとえば、車両走行時に撮影された動画または画像を目視で確認した上で、任意に定めてもよく、車載カメラ10により撮像された撮像画像を元に移動体を検出し、その移動体が映りこむ可能性が高い領域を設定してもよい。そして、制御装置30は、評価点がオクルージョン頻発領域に属する場合には、上述した評価点削除機能の説明における、予め設定した所定の間隔Dminを、他の評価点よりも大きく設定する。これにより、車載カメラ10により撮像される撮像画像中において、他の車両等が映りこむことにより、撮像画像中においてエッジとして検出されない可能性が高い領域に位置する評価点については、評価点の密度を低くすることができるため、より正確に車両位置算出を行うことが可能となる。
なお、車載カメラ10により撮像される撮像画像を元に移動体を検出し、オクルージョン頻発領域を設定する方法としては、まず、前回の車両位置算出処理によって得られた撮像画像と現在の撮像画像とを比較することで、各ピクセルの移動方向と移動量を示すオプティカルフローを算出する。オプティカルフローの算出方法は、たとえば、KLTトラッカーを用いて特徴点を追跡する方法や、勾配法などの公知の方法を採用することができる。次いで、車両の動きによって生じると予測されるオプティカルフローと異なる方向、もしくは大きく異なる移動量をもつ領域を抽出することにより、該当領域に移動体がいるか否かを判定し、頻繁に移動体が認識される領域をオクルージョン頻発領域として設定する。また、オクルージョン頻発領域を設定する別の方法としては、たとえば、自車両に他の車両や人等を認識するセンサを備え、センサにより車両や人等が認識される可能性が高い領域を設定する方法が挙げられる。
さらに、制御装置30は、評価点削除機能により、評価点を削除する処理を行なう際には、他の車両等の移動体を検知し、検知された移動体によってエッジが遮蔽されてしまう領域を抽出し、抽出された領域に対応する評価点を評価点投影画像上から削除してもよい。具体的には、制御装置30は、車載カメラ10によって撮像された撮像画像から他の車両を含む移動体を検知し、撮像画像上において移動体によってエッジが遮蔽される領域を遮蔽領域として抽出し、遮蔽領域に対応する評価点投影画像における領域を不可視領域として算出し、評価点投影画像から不可視領域内の評価点を削除することができる。これにより、車載カメラ10により撮像される撮像画像中において、他の車両を含む移動体によって遮蔽されてしまい、撮像画像中においてエッジとして検出されない領域に位置する評価点を、評価点投影画像上から削除することができるため、より正確に車両位置算出を行うことが可能となる。なお、他の車両等の移動体を検知する方法は、上述した車載カメラ10により撮像される撮像画像を元に移動体を検出する方法を用いることができる。
また、上述した実施形態においては、車両位置算出手法として、パーティクルフィルタ手法を用いる例を例示したが、車両位置算出手法としてはこのような例に限定されず、たとえば、車載カメラ10によって撮像された撮像画像と、評価点投影機能によって生成された評価点投影画像との比較から画像のずれ量を算出し、評価点の3次元位置と、算出されたずれ量とに基づいて車両位置を算出する手法を採用してもよい。
なお、上述した実施形態において、車載カメラ10は本発明の撮像装置に、データベース20は本発明の記憶手段に、制御装置30のエッジ画像算出機能は本発明のエッジ画像生成手段に、制御装置30の位置予測機能は本発明の予測車両位置算出手段に、制御装置30の評価点投影機能は本発明の評価点設定手段、および評価点投影画像生成手段に、制御装置30の評価点削除機能は、エッジ検出強度算出手段、オクルージョン頻発領域検出手段、立体物検出手段、および不可視領域設定手段に、制御装置30の位置評価機能は本発明の車両位置算出手段に、それぞれ相当する。