図1は、本発明の移動体移動量算出装置が適用される実施形態の車両10を示す。車両10のウインドシールド上部の左右には、車両10が走行する道路平面領域を含む画像を撮像する一対の撮像手段である基準カメラ12及び参照カメラ14が固定配置される。基準カメラ12及び参照カメラ14は、共通の撮像領域が設定されたCCDカメラ等からなるステレオカメラであり、これらには、撮像した画像を処理することで道路平面領域と障害物となる可能性のある物体とを検出する演算処理装置16が接続される。なお、以下の説明では、基準カメラ12によって撮像された画像を基準画像と称し、参照カメラ14によって撮像された画像を参照画像と称する。
図2は、演算処理装置16の機能構成ブロックを示す。演算処理装置16は、基準カメラ12によって撮像した基準画像を処理する画像処理部18と、参照カメラ14によって撮像した参照画像を処理する画像処理部20と、所定の撮像時間間隔で撮像された基準画像及び参照画像を記憶する画像記憶部22とを備える。
また、演算処理装置16は、演算制御部24を備え、演算制御部24には、画像記憶部22、パラメータ記憶部26、射影変換行列算出部28、平面パラメータ算出部30、平面領域抽出部32、物体検出部34、物体相対速度算出部36、仮想投影面画像生成部38、自車速度算出部40、物体絶対速度算出部42及び表示処理部44が接続される。
パラメータ記憶部26は、演算に必要な初期値パラメータ、前回の演算や演算中に算出された各パラメータを記憶する。射影変換行列算出部28は、参照画像中の道路平面領域を基準画像中の道路平面領域に2次元射影変換する射影変換行列を算出する。平面パラメータ算出部30は、道路平面の法線ベクトル及び基準カメラ12の光学中心から道路平面までの距離を平面パラメータとして算出する。平面領域抽出部32は、基準画像と射影変換行列によって射影変換した参照画像とを用いて、車両10が走行可能な道路平面領域を抽出する。物体検出部34は、道路平面領域外の領域に存在する物体を検出する。物体相対速度算出部36は、抽出された物体の車両10に対する相対速度ベクトルを算出する。仮想投影面画像生成部38は、抽出された道路平面領域を実際の道路面に平行な仮想平面に投影した仮想投影面(VPP)画像(VPP:Virtual Projection Plane)を生成する。自車速度算出部40は、VPP画像上で車両10の自車速度ベクトルを算出する。物体絶対速度算出部42は、車両10に対する物体の相対速度ベクトルと車両10の自車速度ベクトルとから、各物体の絶対速度ベクトルを算出する。表示処理部44は、上記の各演算結果をディスプレイに表示する処理を行うとともに、演算結果に基づいて必要に応じた警告等の報知を行う。
本発明の移動体移動量算出装置が適用される車両10及び演算処理装置16は、基本的には以上のように構成されるものであり、次に、図3に示すフローチャートに基づき、演算処理装置16による処理の全体の流れを説明する。
先ず、基準カメラ12及び参照カメラ14により、車両10が走行する道路平面領域を含む基準画像及び参照画像を所定の時間間隔で撮像し、画像処理部18、20において所定の画像処理を行った後、画像記憶部22に各時間毎の画像情報として記憶させる(ステップS0)。
次に、射影変換行列算出部28において、パラメータ記憶部26に記憶されている初期値パラメータを用いて、参照画像中の道路平面領域を基準画像中の道路平面領域に2次元射影変換する射影変換行列を動的に算出する(ステップS1)。
平面領域抽出部32は、ステップS1で算出した射影変換行列を用いて、基準画像から道路平面領域を抽出する(ステップS2)。
平面パラメータ算出部30は、道路平面の法線ベクトル及び基準カメラ12の光学中心から道路平面までの距離を平面パラメータとして算出する(ステップS3)。
次に、物体検出部34は、ステップS3で算出した平面パラメータを用いて、ステップS2で抽出した道路平面領域を道路面上に変換して自分の車両10以外の障害物となる可能性のある物体を検出し、若しくは、ステップS2で抽出した道路平面領域以外の領域に対してステレオ計測することにより物体を検出する(ステップS4)。
物体相対速度算出部36は、抽出された各物体の時間毎の位置変化から、車両10に対する各物体の相対速度ベクトルを算出する(ステップS5)。
さらに、仮想投影面画像生成部38は、ステップS3で算出した道路平面の傾きである法線ベクトルを用いて、道路面を上方から見た画像である仮想投影面(VPP)画像を生成する(ステップS6)。
そして、自車速度算出部40は、ステップS6で生成されたVPP画像の各時刻での位置から車両10の自車速度ベクトルを算出する(ステップS7)。
さらに、物体絶対速度算出部42は、ステップS5で算出した物体の相対速度ベクトルと、ステップS7で算出した車両10の自車速度ベクトルとを用いて、各物体の絶対速度ベクトルを算出する(ステップS8)。
なお、表示処理部44は、以上のようにして抽出した道路平面領域、障害物となる可能性のある物体、車両10及び物体の絶対速度等をディスプレイに表示するとともに、例えば、車両10と物体との相対的な位置関係、速度関係に基づき、必要に応じて警告等の報知を行うことができる。
次に、本実施形態をステップ毎に詳細に説明する。
先ず、本発明に用いられる道路平面領域の抽出及び2次元射影変換の原理を説明する。なお、以下の説明において、同次座標は、文章中では、符号に記号「^」を付加して表し、数式中では、符号の上に記号「〜」を付加して表すものとする。また、射影変換された画像については、文章中では、符号に記号「′」を付加して表し、数式中では、符号の上に記号「〜」を付加して表すものとする。
図4に示すように、空間中にある平面(道路平面ともいう)Π上の観測点M1が基準画像Ib及び参照画像Irに投影されるとき、基準画像Ib上での同次座標をm^b、参照画像Ir上での同次座標をm^rとすると、各同次座標m^b、m^rは、(1−1)式に示すように、2次元射影変換によって関係付けられることはよく知られている。
(「〜」は、定数倍の不定性を許して等しいものとする。)
ここで、Hは3×3の射影変換行列である。また、図4において、Oは基準カメラ12の光学中心、O′は参照カメラ14の光学中心、Rは基準カメラ座標系から参照カメラ座標系への回転行列、tは、参照カメラ座標系において参照カメラ14の光学中心から基準カメラ12の光学中心へ向かう並進ベクトル、dは基準カメラ12の光学中心Oと平面Πとの距離、nは平面Πの法線ベクトルである。
道路等の走行可能な領域は、空間中でほぼ平面であると見なすことができれば、参照画像Irに対して適切な射影変換を施すことにより、平面の部分に対して基準画像Ibに一致するような画像を得ることができる。一方、平面上にない物体は、射影変換した際に画像が一致しないため、その物体の領域は走行可能な領域でない、あるいは、障害物となり得る領域であると判断することができる。したがって、平面の拡がりを算出することにより、走行可能な領域及び障害物となる物体の検出が可能となる。
<ステップS0>
基準画像Ib及び参照画像Irに係る画像情報を取得する。
基準カメラ12及び参照カメラ14を用いて画像を撮像し、各画像間の明るさの違い等を除くため、画像処理部18及び20において、各画像にLOG(Laplacian of Gaussian)フィル夕をかける。また、LOGフィルタを掛けた画像は、コントラストが低いため、さらに、ヒストグラム平坦化処理を施す。これらの処理が施された基準画像Ib及び参照画像Irとその原画像は、画像記憶部22に記憶される。
<ステップS1>
射影変換行列算出部28において、画像記憶部22に記憶された基準画像Ib及び参照画像Irを用いて、路面に対する射影変換行列Hを動的に推定する。
すなわち、基準画像Ib内のある領域RIで、射影変換行列Hを微少量変化させ、以下の(1−2)式の評価関数E(H)を最小化する射影変換行列Hを、繰り返し最適化によって推定する。
なお、Ib(m^b)、Ir(Hm^b)は、同次座標m^b、Hm^bにおける基準画像Ib及び参照画像Irの輝度値を表す。
上記の推定には、適当に真値に近い初期値としての射影変換行列Hと、領域RI(本発明では、以後「計算領域」と呼ぶ。)とを必要とする。そこで、時系列情報を利用し、以下に述べる手順により、射影変換行列Hと計算領域RIとを求める。
すなわち、同時刻における基準画像Ib及び参照画像Ir間の射影変換行列HSと、連続する時間で取得した2つの基準画像Ib間の射影変換行列Hmと、基準画像Ibに対する計算領域RIとを求める。そして、ある時刻tにおける射影変換行列H(Hs,Hm)の推定の際には、前時刻(t−1)までの推定結果を利用する。
先ず、連続する基準画像Ib(t−1)、Ib(t)間の射影変換行列Hm(t)を(1−2)式に基づく繰り返し最適化によって推定する。このとき、射影変換行列Hm(t)の初期値としては、前時刻に推定された射影変換行列Hm(t−1)を、また、計算領域RIの初期値としては、基準画像Ib(t−1)に対して前時刻で求められている計算領域RI(t−1)を用いることができる。
次に、求められた射影変換行列Hm(t)を用いて、前時刻の計算領域RI(t−1)を変換することにより、現時刻における計算領域RI(t)の予測値を求める。
基準画像Ib及び参照画像Ir間の射影変換行列HS(t)は、前時刻において推定された射影変換行列HS(t−1)を初期値とし、射影変換行列Hm(t)から求めた計算領域RI(t)を用いて、(1−2)式に基づく繰り返し最適化によって推定する。
このように、時系列画像を用いた連続推定において、十分に真値に近い初期値としての射影変換行列Hと計算領域RIとを用いて、時刻tにおける基準画像Ib及び参照画像Ir間の動的な射影変換行列HS(以下、射影変換行列Hという。)を安定して推定することができる。
次に、(1−2)式を用いた繰り返し最適化(濃度勾配法)による射影変換行列Hの推定方法について詳細に説明する。
濃度勾配法は、2つの画像の一方を微小量変化させ、画像の重なり具合(濃度差)を表す評価関数を最小化することにより、画像が最も重なるような射影変換行列Hの変化量を推定する方法である。
座標x(x,y)における参照画像Irの輝度値をIr(x)とすると、適当な射影変換行列Hにより射影変換された参照画像Ir′は、次の(1−3)式で表される。なお、参照画像Irの上の「〜」は、射影変換された後の画像を表し、xの上の「〜」は、座標x(x,y)の同次座標を表す。
なお、射影変換行列Hは、3×3のパラメータを要素とする自由度8(1パラメータを固定)の行列とする。
射影変換行列Hの8つのパラメータを微小量変化させたとき、射影変換された参照画像I
r′上の座標x(x,y)が座標x″(x″,y″)に変化するものとすると、
(「〜」は、定数倍の不定性を許して等しいものとする。)の関係が得られる。なお、Iは3×3の単位行列、D
Xは射影変換行列Hの各パラメータの微小変化量をパラメータとする3×3の微小変化行列とする。
(1−3)、(1−4)式の関係を用いて(1−2)式の評価関数E(H)を書き替えると、射影変換された参照画像I
r′と基準画像I
bとの重なり状態を示す評価関数E(H)は、射影変換行列Hの微小変化行列D
Xのパラメータを変数として、
(「i」は、画素番号を表す。)
と表すことができる。なお、d
Xは、微小変化行列D
Xのパラメータをベクトルで表したものである。
(1−5)式をテーラ展開して一次近似すると、評価関数E(d
X)は、
(「≒」は、(1−5)式の一次近似の範囲で等しいものとする。)
となる。
(1−6)式の評価関数E(d
X)を最小化するベクトルd
Xは、評価関数E(d
X)をベクトルd
Xで微分し、その微分値を0とおいて、
を解くことにより求めることができる。
この場合、gi(輝度勾配)、ei(輝度差分)は、基準画像Ib及び射影変換された参照画像Ir′から算出され、ヤコビアンJdXiは、座標x(x,y)から算出できる。したがって、ベクトルdXは、最小二乗法を用いて(1−7)式から求めることができる。
以上のようにして求めたベクトルdX、すなわち、微小変化行列DXを用いて、射影変換行列Hを次の(1−8)式にしたがって更新する。なお、Iは、3×3の単位行列とする。
そして、(1−8)式で求めた射影変換行列Hを用いて、再び(1−6)式の評価関数E(dX)の値を算出する処理を繰り返し、評価関数E(dX)の値が所定の値以下に収束したときの射影変換行列Hを所望の射影変換行列Hとする。
<ステップS2(第一の道路平面領域抽出手段)>
平面領域抽出部32において、ステップS1で推定した射影変換行列Hを用いて、車両10が走行可能な道路平面領域を抽出する。この抽出処理について、図5を参照して説明する。
ステップS0でLOGフィルタ処理及びヒストグラム平坦化処理が施された参照画像Irに対して、ステップS1で推定した射影変換行列Hが作用され、射影変換画像Ir′が生成される。
次いで、ステップS0でLOGフィルタ処理及びヒストグラム平坦化処理が施された基準画像Ibと、生成された射影変換画像Ir′との差分画像(Ir′−Ib)を求める。
この場合、平面上に存在する点は、基準画像Ibに正確に投影されるため、差分画像(Ir′−Ib)における輝度値は小さくなる。一方、平面上にない点の差分画像(Ir′−Ib)における輝度値は大きくなる。したがって、所定の閾値を設定して差分画像(Ir′−Ib)を2値化することにより、道路平面領域Πfを抽出することができる。
<ステップS3>
平面パラメータ算出部30において、ステップS1で推定した射影変換行列Hを用いて、平面Πの平面パラメータである法線ベクトルn及び基準カメラ12の光学中心Oから平面Πまでの距離dを求める。
図4に示す関係において、基準カメラ座標系から参照カメラ座標系への回転行列R、参照カメラ14の光学中心から基準カメラ12の光学中心へ向かう並進ベクトルt、基準カメラ12及び参照カメラ14の内部パラメータA
b、A
rを用いて、射影変換行列Hは、
と表される。なお、kは、0でない係数であって、画像から得られた射影変換行列Hに定数倍の自由度が存在することを表す。内部パラメータA
b、A
rが既知であるとすると、次の(3−2)式が成立する。
そこで、射影変換行列H′を特異値分解する。U、Vを正規直交行列、Σを射影変換行列H′の特異値σ
1〜σ
3(σ
1≧σ
2≧σ
3>0)を要素とする対角行列として、射影変換行列H′は、
と書き替えることができる。なお、
である。
基準カメラ座標系の基底ベクトル(e
1,e
2,e
3)を導入し、
とすると、(3−7)式から得られる3つのベクトル方程式と、n、n′が単位ベクトル、Vが正規直交ベクトル、R′がノルム変化のない回転行列であることから、次の(3−9)式が得られる。
(3−9)式をn
1′
2、n
2′
2、n
3′
2の連立方程式と考えれば、0でない解を持つには、行列式が0であるから、
となる。
そこで、射影変換行列H′の特異値σ1〜σ3が、
(I) 重解を持たない場合(σ1≠σ2≠σ3)
(II) 重解を持つ場合(σ1=σ2≠σ3 又は σ1≠σ2=σ3)
(III) 3重解を持つ場合(σ1=σ2=σ3)
に場合分けして考える。(I)及び(II)の場合は、k=±σ1若しくはk=±
σ3とすると、(3−9)式から算出されるn′が単位ベクトルであることに矛盾する。したがって、いずれの場合でもk=±σ2として求められることになる。また、k=−σ2の場合は2台のカメラ(基準カメラ12及び参照カメラ14)が平面を挟むような配置となるため除外され、kはσ2として求められることになる。なお、(III)の3重解の場合は、2台の基準カメラ12及び参照カメラ14の光学中心が一致し、回転のみの場合であるため、除外する。
(I)及び(II)の場合、(3−9)式より、n′は、
として求まる。
σ
1≠σ
2≠σ
3のとき、n
2′=0であるから、
を得る。すなわち、R′は、e
2の回りの回転行列である。したがって、次の(3−13)、(3−14)、(3−15)式を得る。
(II)の場合、σ
1=σ
2(又は、σ
2=σ
3)として、(3−13)式から、
となり、
となる。
ここで、(3−11)式におけるε1、ε2の符号の違いにより、4つの解の候補が存在する。この4つの候補のうち、「平面上の点が画像に映る」条件を満たすn′を選択することで2つの候補に絞り込む。さらに、基準カメラ12及び参照カメラ14の位置関係が撮影中に変化しない条件を加え、解を一意に決定する。
以上のようにして、(3−11)、(3−13)、(3−15)式、又は、(3−18)式で求めたR′、t′/d、n′を(3−6)式に代入することにより、回転行列Rと、平面パラメータを構成するt/d及び法線ベクトルnとが求められる。なお、並進ベクトルtの長さ|t|(基準カメラ12及び参照カメラ14間の距離)は、設計値や実測値から予め求めることができるため、得られたt/dから、平面パラメータである距離dを求めることができる。
ところで、上述した平面パラメータの算出方法(ステップS3)では、基準カメラ12及び参照カメラ14間の回転行列R及び並進ベクトルtが変動することを前提としているが、これらが車両10に対して固定されているとすれば、予め求めた回転行列R及び並進ベクトルtを用いて、以下に示す方法により、平面パラメータを迅速に算出することができる。
この場合、回転行列R及び並進ベクトルtは、例えば、基準カメラ12及び参照カメラ14を車両10に固定した後、整備場のように、車両10を停止させた理想的な環境下で高精度に求めておくことが可能である。そして、走行中等の任意の時刻に算出した平面の射影変換行列Hから平面パラメータを求める際、理想的な環境下で求めた高精度な回転行列R及び並進ベクトルtを用いるため、平面の射影変換行列Hが誤差を含む場合であっても、平面パラメータの精度を悪化させることなく算出することができる。
そこで、先ず、車両10を停止させた状態において、基準カメラ12及び参照カメラ14によって平面Πを撮像し、例えば、ステップS1の処理によって射影変換行列Hを推定する。次いで、上述した平面パラメータの算出手法を用いて回転行列R及び並進ベクトルtの単位ベクトルt
eを求める。すなわち、(3−6)式及び(3−13)、(3−14)式を用いて回転行列Rを求める。また、(3−6)式及び(3−15)式、又は、(3−18)式を用いてt/dを求め、このt/dを正規化し、並進ベクトルtの単位ベクトルt
eを、
として求める。並進ベクトルtの長さ|t|(基準カメラ12及び参照カメラ14間の距離)は、設計値や実測値から予め求めることができるため、(3−19)式の単位ベクトルt
eに|t|を掛けることで、並進ベクトルtが求められる。
ここで、ステップS1の処理で射影変換行列Hを高精度に推定できるようにするため、画像の重なり具合を評価し易いような模様を付けた十分広い面積を持つ平面Πを用意するとよい。また、平面Πの向きを変えて上記方法により回転行列R及び並進ベクトルtを求め、その結果を平均化して回転行列R及び並進ベクトルtを一層高精度に求めるとよい。
このようにして算出された回転行列R及び並進ベクトルtは、パラメータ記憶部26に記憶される。並進ベクトルtは、単位ベクトルteとベクトルの大きさ|t|とに分けて記憶させてもよい。回転行列R及び並進ベクトルtは、前記のようにして算出する代わりに、その他のキャリブレーション方法を利用して算出し、あるいは、基準カメラ12及び参照カメラ14の位置関係を直接計測して求めることもできる。
次いで、車両10の走行中等の任意の時刻において、動的な射影変換行列Hを算出する(ステップS1)。
(3−2)式を変形して、
の関係が得られる。法線ベクトルnは単位ベクトル、|t|/dはスカラであるので、(3−20)式で算出したベクトルを正規化することで、平面Πの法線ベクトルnを求めることができる。
また、並進ベクトルtの長さ|t|は、設計値や実測値から分かっているため、基準カメラ12の光学中心から平面Πまでの距離dは、次の(3−21)式から算出することができる。
このように、予め求めた回転行列R及び並進ベクトルtを用いることにより、任意の方法で推定した射影変換行列Hから、法線ベクトルn及び距離dを迅速に算出することができる。
さらに、基準カメラ12及び参照カメラ14間の回転行列R及び並進ベクトルtが予め求められている場合、射影変換行列H及び平面パラメータ(法線ベクトルn、距離d)を、これらの既知のパラメータを用いて迅速且つ高精度に求めることができる。
すなわち、(3−1)式を書き替えると、射影変換行列Hは、
と展開することができる。なお、射影変換行列Hは、構成するパラメータから算出されるため、定数倍の不定性を示す係数kを1とすることができる。この場合、射影変換行列Hの第1項は、並進ベクトルtが既知であるため定数であり、第2項はn/dの変数となる。したがって、第2項を推定することで、射影変換行列Hを求めることができる。
そこで、n/dを微少量変化させたときの射影変換行列Hの微小変化行列をDとすると、
である。なお、n″=n/dとし、その微小変化量をδn″とする。
微小変化行列Dをn/dの微小変化量δn″に書き替え、(1−5)式に対応する評価関数E(δn″)をテーラ展開して一次近似すると、
(「≒」は、(1−5)式の一次近似の範囲で等しいものとする。)
となる。
(1′−3)式の評価関数E(δn″)を最小化する微小変化量δn″は、評価関数E(δn″)を微小変化量δn″で微分し、その微分値を0とする微小変化量δn″として求めることができる。この場合、g
i(輝度勾配)、e
i(輝度差分)は、基準画像I
b及び参照画像I
rから算出され、ヤコビアンJ
dXiは、座標x(x,y)から算出され、ヤコビアンJδ
n″は、基準カメラ12及び参照カメラ14の内部パラメータA
b、A
r、並進ベクトルtから算出できる。したがって、微小変化量δn″は、最小二乗法を用いて、
から求めることができる。
以上のようにして求めた微小変化量δn″から、(1′―2)式を用いて射影変換行列Hの微小変化行列Dを求め、射影変換行列Hを次の(1′−5)式にしたがって更新する。
また、微小変化量δn″を前回の演算で求めたn″(=n/d)に加算し、次の(1′−6)式にしたがって更新する。
そして、(1′−5)式で求めた射影変換行列Hと、(1′−6)で求めたn″とを用いて、再び(1′−3)式の評価関数E(δn″)の値を算出する処理を繰り返し、評価関数E(δn″)の値が所定の値以下に収束したときの射影変換行列Hを所望の射影変換行列Hとする。
さらに、射影変換行列Hが収束した際のn″から、平面パラメータである法線ベクトルn及び距離dは、
として求めることができる。
このように、予め求めた回転行列R及び並進ベクトルtを用いることにより、射影変換行列Hを迅速且つ高精度に推定できるとともに、法線ベクトルn及び距離dを射影変換行列Hから直ちに算出することができる。
<ステップS4>
ステップS4は物体を検出する処理であり、図6に示すステップS41〜ステップS46のサブルーチン処理からなる。
ステップS41において、基準カメラ12及び参照カメラ14によって、立体領域をステレオ計測し、物体の各エッジ点のワールド座標Cwを求める。
ステップS42において、ワールド座標Cwの高さ方向の座標Yw(図7参照)に基づき、平面からの高さにより物体の候補点を抽出する。具体的には、図7に示すように、平面からの高さがTmin(例えば、20cm)以上であってTmax(例えば、2.5m)未満の空間に存在する候補点(Tmin≦Yw<Tmax)を抽出する。これにより、道路上に設けられた歩道橋、トンネルの入口部、看板等の少なくとも一部が除外され、他の車両やガードレール等の車両10の走行に関わり合いのあり得る部分のみが抽出されやすくなる。これにより、図7の候補点Paは抽出され、候補点Pbは除外される。
また、車幅方向であるXw方向及び前後方向であるZw方向についても、所定範囲内に制限して候補点の抽出を行うようにしてもよい。これにより、車両10が通過する可能性のない箇所に対して無駄な投票処理を行うことが防止され、計算量を低減することができる。この場合、例えば、図8に示すようにx方向については−10〜10m、z方向については0〜40mの範囲に制限するとよい。
ステップS43において、物体の各候補点をそのXw、Zw座標に応じて図8に示す投票面50上に設けた一定サイズの桝目(セル)単位に投射する。投票面50は、ワールド座標系の直交する2軸である座標Xwと座標Zw(図16参照)の座標面として表される。このとき、同物体のセルに対して複数の候補点が重複して投射される場合には、その重複数を記録する。このように重複数を考慮しながら候補点を投射する処理を投票と呼び、重複数を投票数と呼ぶ。投票の処理では、前記ステップS42においてTmin未満の点及びTmax以上の点が予め除外されているため、歩道橋、トンネルの入口部、看板等に対応する点の投票数は0又は十分に小さい値となる。なお、図8及び図9に示す投票面50は、説明上の模式的なものであって、他の図5等とは関連付けられていない。
ステップS44において、投票面50上に投票された候補点を隣接する群毎にグループ分けするクラスタリング処理を行う。クラスタリング処理によって分けられた群をクラスタと呼ぶ。クラスタリング処理としては、最短距離法(Nearest Neighbor Method)を挙げることができ、最短距離法で用いる特徴量としては幅(座標Xw)及び奥行き(座標Zw)を用いるとよい。このようなクラスタリング処理によれば、検出物体のモデルが不要であって、種々の形状の物体を検出することができる。
このステップS44のクラスタリング処理によって、例えば、図9に示すように、投票面50上で3つのクラスタ52a、52b及び52cが得られる。
なお、ステレオ計測における対応探索のミス(マッチングミス)によるノイズを除去することを目的として、クラスタリング処理を行った後にクラスタに対して膨張収縮処理を行うようにしてもよい。物体をステレオ計測する際にマッチングミスがあると、計測される3次元位置には誤差が生じるため、計測された物体形状には凹凸(ノイズ)が生じ、このノイズによりクラスタ形状が異なることがある。膨張収縮処理では、膨張により凹凸部をつなげ、収縮により元の大きさに戻すため、ノイズの影響を抑えた形状にすることができる。
ステップS45において、クラスタリング処理によって分けられた各クラスタ52a、52b、52cが物体であるか又はノイズであるかを判断し、ノイズによるクラスタを除去する。
具体的には、先ず、得られた各クラスタ52a、52b及び52cに対してそれぞれ合計投票数を求める。つまり、クラスタ52aの合計投票数は38であり、クラスタ52bの合計投票数は21であり、クラスタ52cの合計投票数は3である。また、各クラスタ52a、52b及び52cにおける面積上の重心gの奥行きである座標Zwの値Zgを基準とした閾値Thzと各合計投票数とを比較し、合計投票数が閾値Thz以下であるクラスタはノイズであるとして除去する。閾値Thzは以下の(4−1)式で表される。
ここで、Kgは係数であって、例えば、1m先に物体が存在するとした場合の最小の合計投票数である。(4−1)式は、画像中に映し出される物体の面積が距離の二乗に反比例して小さくなるという現象に基づいて設定されている。
このような、ステップS45の処理によって、例えば、合計投票数の少ないクラスタ52cはノイズとして除去される。また、歩道橋、トンネルの入口部、看板等の合計投票数が小さい物体もノイズとして除去される。
ノイズが除去された投票面は、図5の例に則してより実際に近いデータとして示すと、図10に示すようになり、この投票面50が所定の記憶部に記録される。図10から明らかなように、この投票面50には、比較的近い前方で車線変更を行っているトラックに対応したクラスタ54aと、該トラックより前方を走る乗用車に対応したクラスタ54bと、ガードレールに対応したクラスタ54cが抽出されている。また、投票面50には、設計者の確認用に座標Xw、座標Zw及び自車である車両10が模式的に示されている。
ステップS46において、残ったクラスタ54a〜54cを物体として認識し、対応する物体位置領域を切り出して、所定の記憶部に記憶する。
ステップS4(図3参照)の物体抽出処理はこのようなステップS41〜S46により構成されているが、各ステップについて以下さらに詳細に説明する。
ステップS41は、図11に示すステップS411〜ステップS417のサブルーチン処理からなる。
ステップS411(基準画像特徴点抽出手段)において、基準画像Ibからエッジ及び特徴点を抽出するソーベルフィルタ処理を行う。このソーベルフィルタ処理とは、画像輪郭抽出のためのフィルタ処理であって、画素毎にその近傍の画素に所定の係数をそれぞれ乗算し、結果を合計する処理であって、垂直方向、水平方向の二つの係数行列を用いてこの処理を行う。
すなわち、自動車のように、表面が鏡面反射している物体は、見る角度によって反射光が変化するため、同じ点を左右の画像で観測しても輝度が異なってしまう。そのため、左右の画像間での領域ベースマッチングがうまく行かず、ステレオ距離計測が不正確となることがある。そこで、テクスチャレスな部分や鏡面反射している部分はステレオマッチングをせずに、エッジ部分のみステレオマッチングを行い、距離の計測をする。そのため、先ず入力画像にソーベルフィルタ処理を行い、エッジや特徴点を抽出し、この後二値化し、図12に示すエッジ画像60を作成する。図12から明らかなように、この時点のエッジ画像60には、路面の白線等の物体検出の処理には無関係な部分も抽出されている。
なお、図12では、説明の便宜上エッジ画像60を模式的に示しており、実際には、図13で示すような画像データが得られる。
ステップS412において、白線等の道路平面にあるエッジは検出対象ではないため、エッジ画像60から道路平面領域抽出処理(ステップS2)で得られた平面領域Πf(図5参照)の部分を除去する処理を行う。具体的には、エッジ画像60と平面領域Πfとの二値的な排他処理を行い図14に示す修正エッジ画像62を得る。この際、エッジ画像60における上部の所定幅の領域や下部のボンネットの領域等は検出対象がないことが明らかであって、しかもこれらの領域はエッジ画像60上の場所が固定されていることから、これらの部分に対しても所定の領域指定手段により除去する。
なお、エッジ画像60から平面部分が完全に取り除けなくとも、後述するように車両10の通過する空間に限定した処理を行うので、検出対象として誤検出されることはない。
ステップS413において、修正エッジ画像62上に残った各エッジ点に対応する参照画像Ir上の探索範囲としてのエピポーララインEPを設定する(図15参照)。エピポーララインEPは、修正エッジ画像62上におけるエッジ点の参照画像Ir上に対応する点を含む直線として、参照カメラ14の内部パラメータ、基準カメラ12の内部パラメータ、参照カメラ14と基準カメラ12との回転行列R及び並進ベクトルtに基づいて算出されるが、詳しくは後述する。
ステップS414において、道路平面領域抽出処理(ステップS2)で得られた平面領域Πfに射影変換行列の逆行列をかけて参照画像Ir上における参照平面領域Πfrを求めて、参照画像Ir上に投影する。
ステップS415において、参照画像Ir上におけるエピポーララインEPから参照平面領域Πfrの部分を除去する処理を行い、エピポーララインEPは非平面領域に限定される。
ステップS416において、エッジ点に対応する基準画像Ib上の小領域に対して、参照画像IrのエピポーララインEP上で領域ベースマッチング処理を行う。この領域ベースマッチング処理とは、基準画像Ibにおけるエッジ点を中心とした小さいマッチングウィンドウを作成し、該マッチングウィンドウと参照画像IrにおけるエピポーララインEP上の小ウィンドウとの類似度の計算を順次行う処理である。領域ベースマッチング処理によって最も類似度の高い点がエッジ点の対応点(ウィンドウ)として求められる。
この際、先ず、基準画像Ibから参照画像Irへ領域ベースマッチング処理を行い、計算された類似度を閾値と比較し、マッチングが成功したと判断されたとき、今度は参照画像Irから基準画像Ibへ領域ベースマッチングを行い、先の基準画像Ibの点に一致した場合に、マッチングが成立すると判断する。このような領域ベースマッチングを行うことで、繰り返しパターンによるマッチングミスを抑えることができる。ここで用いる閾値としては、類似度の最大値に対する第1閾値と、類似度の最大値から類似度の2番目の最大値との差に基づく第2閾値を設けて判断してもよい。
類似度の計算には、例えば、正規化相互相関関数(ZNCC)を類似度関数として用いることができる。正規化相互相関関数ではパターンの類似度を計算しているため、基準画像Ibと参照画像Irとに全体的な輝度のオフセットずれがある場合であってもマッチングのミスが発生しにくい。正規化相互相関関数は、次の(4−2)式により求められる。
ここで、Ib、Irはそれぞれ基準画像と参照画像を示し、Rはウィンドウ領域を示す。またxはウィンドウ内のピクセルの位置を示す。また、u及びvはそれぞれ基準画像、参照画像のウィンドウの基準位置を示す。
また、基準画像Ib及び参照画像Irの種類や必要な計算速度等に基づいて他の類似度関数(SSD(2乗残差和)、NSSD及びSAD(絶対値和)等)を用いてもよい。
このステップS416における領域ベースマッチング処理は、基準画像Ibに関しては平面領域Πfが除かれた部分のエッジ点のみが処理対象となり、一方、参照画像Irに関してはエピポーララインEP上のうち参照平面領域Πfrが除かれた部分のみが処理対象となる。したがって、計算負荷が大幅に削減されるとともに、平面領域の点を検出してしまうことがない。また、計算負荷が低減されることから、計算のためのハードウェアに関して低廉化を図ることができる。
ステップS417(第一の物体位置算出手段)において、領域ベースマッチングの成立した基準画像Ib上の点(ウィンドウ)と参照画像Ir上の点(ウィンド)に基づいて、三角測量の原理に基づくステレオ計測により対応する点のカメラ座標Ccを計算する。
ステップS418において、カメラ座標Ccを自車である車両10を基準としたワールド座標Cwに変換する。具体的には、図16に示すように、カメラ座標系を基準カメラ12の光学中心を原点とし、光軸を座標軸Zcとして含む直交座標Xc、Yc、Zcと表す。また、ワールド座標系は座標軸Ywを法線ベクトルn方向に、座標軸Xw、Zwを平面Π上に設けた直交座標系Xw、Yw、Zwと表す。なお、座標軸Zwは車両の前後方向とし、原点は、車両に対する物体位置を求める際に基準とする車両の特定位置(の平面Π上の位置)とする。さらに、ワールド座標系の原点からカメラ座標系の原点へ向かうベクトルを[twx、twy、twz]t=twとして表す(twyは距離dに等しい)。各パラメータをこのように表したとき、ワールド座標Cwは、以下の(4−3)式で表される。
ここで、法線ベクトルnを(4−4)式で表したとき、回転行列Rwは、(4−5)式で表される。
なお、説明を簡単にするために、図11のフローチャートでは、ステップS413〜ステップS418の処理を直列的に示したが、実際上、修正エッジ画像62に表される各エッジ点毎にステップS413〜ステップS418を実行し、対応するワールド座標Cwを求める。
このようにして、修正エッジ画像62上の各エッジ点は奥行きである座標軸Zwに基づいて、例えば、図17に示すような奥行き画像64として表すことができる。この奥行き画像64では、座標軸Zwの値(又はワールド座標系の原点からの距離)に応じた大きさの点が示されている。各点は、座標軸Zwの値に応じて色の変化するグラデーション状の点で表してもよい。奥行き画像64は設計者の動作確認のために供されるものであり実際上は設けなくてもよいが、該奥行き画像64を生成するために必要なワールド座標Cwは所定の記憶部に記録しておく。
上述したように、ステップS41の処理(ステップS411〜S418)によれば、基準画像Ib及び参照画像Irに基づいて、物体のエッジ点毎にワールド座標Cwを求めることができる。この際、平面領域Πfを除外した部分について処理を行うので、計算負荷の低減が図られる。このうち、ステップS413〜S416は参照画像対応部探索手段ということができる。
<ステップS5>
ステップS5は物体の相対速度を算出する処理であり、図18に示すステップS51〜ステップS55のサブルーチン処理からなる。
ステップS51において、連続する時刻の基準画像Ibから物体領域の重なり割合Rateを算出する。
ステップS52において、連続する時刻における物体の重心移動距離Lgを算出する。
ステップS53(同一物体判定手段)において、重なり割合Rate及び重心移動距離Lgに基づいて閾値判定による同一物体の判定を行う。
ステップS54において、物体の位置を高精度に求めるため再計算して位置ベクトルPtを求める。
ステップS55において、位置ベクトルPtに基づいて、物体の相対速度ベクトルVobst−relを算出する。
ステップS5(図3参照)はこのようなステップS51〜S55により構成されているが、各ステップについて以下さらに詳細に説明する。なお、ステップS51〜S55についての説明では、検出された各物体のうち、基準画像Ibにおけるトラック70(図5参照)に対する処理を例示的に説明するが、他の検出物体である乗用車72やガードレール74についても同様の処理が行われる。ステップS51は、図19に示すステップS511〜ステップS517のサブルーチン処理からなる。
ステップS511において、前時刻のステップS517で記憶されたテンプレート80aを所定の記憶領域から呼び出す。ここで、テンプレート80aとは投票面50(図20参照)におけるクラスタ54aに対応したトラック70の基準画像Ibにおける像を領域指定して記録したものであり、例えば、平面から1.5mの高さの領域として記録されている。
概念的には、図20に示すように、点P0は、投票面50上の点であり、光学中心Oから一定角度で走査し、クラスタ54aに当たる点とする。点P0に基づいてワールド座標Cwが計算できる。点Prを点P0と同じ幅位置と奥行位置であり、且つ道路平面Π上の点とする。点Prは、道路平面Π上の点であることから高さが0である。点Pxは、点P0と同じ幅位置と奥行位置であり、且つ道路平面Πから1.5mの高さをもつ点とする。次に点Pr、Pxを用いてカメラ座標Ccに変換する。カメラ座標Ccに変換後、透視投影行列を用いて基準画像Ib上の座標に変換すればよい。このようにして前時刻のクラスタ54aに対応したテンプレート80aが求められて、記録されている。同様に、乗用車72に対応した前時刻のクラスタ54b及びガードレール74に対応した前時刻のクラスタ54cに対しても図21に示すテンプレート80b及びテンプレート80cが記録されている。
また、前記ステップS418で各物体の高さが求められていることから、各テンプレート80a、80b及び80cの高さは求められた物体の高さに応じて設定してもよい。
ステップS512において、現時刻のメインルーチン処理におけるステップS0で取得した基準画像Ibを読み出す。なお、ここではLOGフィルタやヒストグラム平坦化などの処理を施す前の原画像を用いるものとする。
ステップS513(テンプレートマッチング手段)において、各テンプレート80aを用いて、現時刻に取得した基準画像Ib上でテクスチャに基づくテンプレートマッチングを行い、類似度の最も高い位置に各テンプレート80aを移動させる。テンプレートマッチングの具体的手法としては、前記のZNCC(4−2)式や、SSD、SAD等を用いたマッチングを挙げることができる。
ステップS514において、前記ステップS46で記録したデータに基づいて、現時刻に求められたクラスタ54aを呼び出す。
ステップS515(物体位置領域算出手段)において、呼び出したクラスタ54aに対してそれぞれ対応する物体位置領域82aを求める。この物体位置領域82aは前記ステップS511で説明した手順によって求められる。物体位置領域82aは、平面的なクラスタ54aに基づいて求める方法以外にも、図17に示されるような奥行き画像64における各エッジ点の3次元的な空間位置に基づいて求める方法を採ってもよい。
ステップS516(重なり割合算出手段)において、前記ステップS513で移動したテンプレート80aと、前記ステップS515で求められた物体位置領域82aとの重なり割合Rateを次の(5−1)式で算出する。
ここで、Rt-1は、移動したテンプレート80aの領域であり、Rtは、対応する物体位置領域82aである。また、∩は領域の重なり部分を示す演算子であり、min(Rt-1、Rt)はRt-1とRtの面積の小さいいずれか一方を選択する関数である。
(5−1)式によれば、移動したテンプレート80aと物体位置領域82aが完全に一致する場合にはRate=1となり、一致する部分が全くない場合にはRate=0となる。また、概念的には、図21に示すように、基準画像Ib上でテンプレートを示すRt-1と物体位置領域を示すRtに対して重複する部分Rt-1∩Rtの割合を求めることになる。
重なり割合Rateが所定の閾値Tha(0<Tha<1)以上である場合には、対応する前時刻の投票面50におけるクラスタと現時刻の投票面50におけるクラスタを同一物体に基づくものであると仮判定されてトラッキングされる。トラッキングとは、異なった時刻に得られたクラスタの対応付けを行う処理であり、これにより物体の動作が検出可能となる。
ステップS517において、前記ステップS515で求められた物体位置領域82aを新たなテンプレート80aとして所定の記憶部に記録する。この新たなテンプレート80aは、前記のとおり次回処理時のステップS511で呼び出される。
このように、ステップS51(ステップS511〜S517)においては、テンプレート80aに基づいてテンプレートマッチングを行うとともに、投票面50のクラスタ54aから物体位置領域82aを求めるため、2重のトラッキング処理を行っており、これらの結果の重なり割合Rateを求めてトラッキング処理の信頼性を高めている。実際には、このステップS51で求めたトラッキングの関連づけはステップS53で最終的に確認される。
次に、ステップS52の処理内容について説明する。ステップS52は、図22に示すステップS521〜ステップS524のサブルーチン処理からなる。
ステップS521において、前記ステップS46で記録したデータに基づいて、現時刻に求められたクラスタ54aを呼び出す。
ステップS522(物体重心位置算出手段)において、クラスタ54aの現時刻の重心gTを求めて、図10に示すように投票面50に記録する。
ステップS523において、前時刻にステップS533を実行したときに記録したクラスタ54aの前時刻の重心gOを読み出す。
ステップS524(物体重心間移動量算出手段)において、図10に示すように、クラスタ54aの重心gOと重心gTとの重心移動距離Lgを算出する。
このように、ステップS52(ステップS521〜S524)においては、クラスタ54aの前時刻の重心gOと現時刻の重心gTとの重心移動距離Lgを求めており、短時間の移動距離として合理的なもののクラスタ同士を対応づけることができる。実際には、このステップS52で求めたトラッキングの関連づけは次のステップS53で最終的に確認される。
次に、ステップS53の処理内容について説明する。ステップS53は、図23に示すステップS531〜ステップS534のサブルーチン処理からなる。
ステップS531において、対応する物体毎に、前記ステップS516で求めた重なり割合Rateと閾値Thaとを比較し、Rate≧ThaであればステップS532へ移り、Rate<ThaであればステップS534へ移る。
ステップS532において、対応する物体毎に、前記ステップS524で求めた重心移動距離Lgと閾値Thbとを比較し、Lg≦ThbであればステップS533へ移り、Lg>ThbであればステップS534へ移る。この閾値Thbは、自車の走行速度と対向車の走行速度を考慮して、処理上の演算周期に物体が移動可能な合理的な距離として固定値で設定されている。
ステップS533においては、対応付けられた2つの物体は同一物体であるとして確定認識され、記録される。また、重心goをgo←gTとして入れ替えて所定の記憶部に記憶する。
一方、ステップS534においては、対応付けられた2つの物体は別物体であったものとして認識される。この場合、対応付けられなかった物体については、画像の前後関係に基づいて撮像範囲内に入ってきた新たな物体として認識する登録処理を行い、又は撮像範囲から抜け出た物体として所定の削除処理を行う。
このように、ステップS53(ステップS531〜S534)においては、前記ステップS52までの処理で対応付けられた2つの物体について、重なり割合Rateと重心移動距離Lgに基づいて、同一物体か否かの確認を一層確実に行うことができる。
なお、このステップS531〜S534は、予め仮に対応付けられた2つの物体に対して行うものとして説明したが、前時刻検出の各物体と、現時刻検出の各物体に対して総当たり的に行って、対応付けを行ってもよい。
また、ステップS532で用いられる閾値Thbは固定値であるものとして説明したが、車両10から重心gOまでの座標Zw方向の距離Zgに基づいて変化する関数としての閾値Th(Zg)を閾値判定に用いてもよい。この場合、ある物体の重心gOまでの座標Zw方向の距離がZg1であったとすると、閾値はTh(Zg1)として表される。また同様に、閾値を自車速|VV|に基いて変化する関数としての閾値Th(|VV|)を閾値判定に用いてもよい。
次に、ステップS54及びステップS55の処理内容について説明する。ステップS54は、図24に示すステップS541〜ステップS544からなり、ステップS55は、図26に示すステップS551〜S553からなる。
ステップS541(注目点決定手段)において、基準画像Ibにおける物体の注目点Pcを特定する。この注目点Pcは、ステップS513のテンプレートマッチングにより移動したテンプレート80aの領域内に設定し、例えば、テンプレート80aの中心点や、物体の角部などの特徴的な点として特定すると、常に物体上の略同一点に特定される。
ステップS542において、注目点Pcに対応させたエピポーララインEPを参照画像Ir上に設定する。このとき、クラスタ54aを利用することで、トラック70までの概略距離を算出できることから、これに基づいてエピポーララインEPを必要十分な程度に短く設定できる。
クラスタを利用した注目点Pcの概略距離の算出方法を説明する。先ず、クラスタ54aの重心gT(図10参照)よりZw座標を求める。これを注目点PcのZw座標に仮設定し、注目点Pcの画像座標(upc,vpc)とともに、次の(5−2)式に代入して対応するXw座標を算出する。ここで、λは定数倍の自由度を表す。
こうして求めた投票面50上の位置(Xw,Zw)と基準カメラの位置(twx,twz)を通る直線を設定し、その直線に沿ってクラスタ54aの点を探索して、車両10に最も近い点のZw座標を注目点Pcの概略距離とすればよい。
ステップS543において、前記ステップS416と同様に注目点Pcに対応する基準画像Ib上の点に対して、参照画像IrのエピポーララインEP上で領域ベースマッチング処理を行う。
ステップS544(第二の物体位置算出手段)において、前記ステップS417と同様に、マッチングの成立した基準画像Ib上の点と参照画像Ir上の点に基づいて、ステレオ計測によって対応する点のカメラ座標Ccを計算する。さらに、前記ステップS418と同様に、求められたカメラ座標Ccをワールド座標Cwに変換する。求められた物体の位置であるワールド座標Cwは、図25に示すように、投票面50上において2次元的な位置ベクトルPtとして表す。
次に、図26のステップS551において、物体の前時刻に検出した位置ベクトルPt-1を読み出す。この位置ベクトルPt-1は、前時刻処理時のステップS553で所定の記憶部に記録されている。
ステップS552(物体移動ベクトル算出手段)において、次の(5−3)式により物体の相対速度ベクトル(移動ベクトル)Vobst−relを算出する。
ここで、Δtは処理の演算周期であり、単位時間あたりの位置ベクトルPtの変化として相対速度ベクトルVobst−relが求められる。
ステップS553において、次回の処理に供するために、位置ベクトルPtをPt-1←Ptと代入して記憶部に記録する。ここで記録する位置ベクトルPtは、ステップS517において新たなテンプレート80aとして記憶した物体位置領域82aに対して、ステップS541〜S544と同様の処理を行い更新したものである。
このようなステップS54及びS55の処理によれば、エピポーララインEPを十分に短く設定していることから、ステップS543における領域ベースマッチングでマッチングミスの発生する頻度が十分に低くなり物体の距離を一層正確に算出することができる。また、注目点Pcは常に物体上の略同一点に設定されることから、幅方向(座標Xw方向)の移動を含めて物体の経時的な相対移動を示す相対速度ベクトルVobst−relを正確に算出することができる。
この結果、トラック70、乗用車72及びガードレール74の相対速度ベクトルVobst−relであるベクトルVr1、Vr2及びVr3が、図25に示すようにベクトル表示される。図25から明らかなように、ベクトルVr1からはトラック70が左方向に向かって進路変更していることが判断され、ベクトルVr2からは乗用車72が前方に向かって車両10よりもやや速い速度で進行していることが判断される。また、ベクトルVr3からは静止物であるガードレール74が車両10の進行方向と逆方向に相対的に移動していることが判断される。なお、図25における自車速度ベクトルVVはステップS6及びS7において求められる。
次に、エピポーララインの設定方法について説明する。ステレオ3次元計測をするためには基準画像Ibに映る観測点M1の位置と参照画像Irに映る観測点M1の位置をそれぞれ求める必要がある。図27に示すように、観測点M1は基準画像Ib上の点mbに投影されており、点mbは観測点M1と基準カメラ12の光学中心Oを通る直線Le上に存在する。直線Le上の点を探せば、観測点M1を見つけることができ、参照画像Ir上に投影される直線LeがエピポーララインEPとなる。つまり、基準画像Ib上に投影された観測点M1を参照画像Irと対応をつけるには、参照画像Ir中でのエピポーララインEP上を探索すればよいこととなる。なお、図27における点M0及びM2は、同一の点mbに投影される他の点であって直線Le上の点である。
このエピポーラ拘束の関係式を求める。参照カメラ座標Xrと基準カメラ座標Xbの間には、前記の回転行列R及び並進ベクトルtを用いて、Xr=R・Xb+tの関係が成り立つ。
このとき、基準カメラ12の正規化画像座標X^ba、参照カメラ14の正規化画像座標X^ra及び並進ベクトルtは同一平面上に存在する。すなわち、スカラー三重積が0になるので、
となる。
ただし、
と表すことにする。ただし、t1、t2、t3は並進ベクトルtの要素を示す。ここで、正規化画像座標と画像座標との関係より、m^
b=A
bX^ba、m^
r=A
rX^raを代入して整理すると、基礎行列(Fundamental Matrix)Fは次の(5−6)式及び(5−7)式で表される。
(5−7)式により、空間の観測点M1が基準画像I
bに投影される位置をmとしたとき、l’=Fm^とすると、m^’l’=0より、参照画像I
r中では、
となり、参照画像I
r中における観測点M1に対応したエピポーララインEPとなる。ここで、「’」は参照画像であることを示し、l
1’,l
2’,l
3’は参照画像のエピポーララインEPのパラメータを示す。また、u’、v’は参照画像の画像座標を示す。
また、観測点M1の概略位置が既知である場合には、直線Leの長さを制限することによりエピポーララインEPを短く設定することができる。
参照画像I
rに投影される位置をm’としたとき、l=F
tm’とすると、l
tm=0より、参照画像I
r中では、
となり、観測点M1における基準画像I
bにおけるエピポーララインEPとなる。なお、このエピポーララインEPの説明で用いたパラメータu及びu’は、後述するベクトルu及びu’とは異なるものである。
<ステップS6>
このステップS6は、自車である車両10の自車速度ベクトルVV(図25参照)を求めるために、道路平面Πを仮想的に上方から見たVPP画像を生成するための処理であり、図28に示すステップS61〜ステップS63のサブルーチン処理からなる。すなわち、ステップS61において、基準画像IbをVPP画像に変換(以下、VPP変換とも呼ぶ)するための変換式HVPPを算出する。この後、ステップS62において、求められた変換式HVPPを用いて、前記基準画像IbをVPP画像IVに変換し、記録する(図33参照)。さらに、ステップS63において、同じ変換式HVPPを用いて前記平面領域ΠfをVPP変換して記録する(図34参照)。以下、各ステップS61〜S63について詳細に説明する。
なお、VPP画像IVを撮像する仮想的なカメラを想定し、説明上これを仮想カメラと呼ぶ。
先ず、図29のステップS611において、基準カメラ12の内部パラメータAbの逆行列Ab -1を読み込む。内部パラメータAbは、次の(6−1)式で表される。
ここで、α1、β1、s1、CX1及びCY1は、基準カメラ12のキャリブレーションにより定まる定数である。
ステップS612において、道路平面Πに対する位置姿勢パラメータである法線ベクトルn及び距離d(図16参照)を読み込む。
ステップS613において、法線ベクトルn及び距離dを用いて、基準カメラ12の光軸であるベクトルeoz(図30参照)を法線ベクトルnに一致させる回転行列Rnを次の(6−2)式〜(6−5)式に基づいて算出する。
これらの(6−2)式〜(6−5)式を図30も参照しながら説明すると、式中で縦二重線で囲まれた部分はノルムを示し、ベクトルeozは、カメラ座標系における光軸である座標Zc(図16参照)の単位ベクトルを示す。また、cnは、法線ベクトルnとベクトルeozに垂直な単位ベクトルであり、角度θnは法線ベクトルnとベクトルeOZとのなす角度である。なお、図30におけるベクトルe’OX、e’OY及びe’OZは、仮想カメラ座標系を示す直交単位ベクトルであり、一転鎖線は車両10の前後方向を示す。
ステップS614において、投影距離、スケール、画像中心を指定し、道路平面Πを上方から見た仮想カメラの内部パラメータAVPPを設定する。内部パラメータAVPPは、仮想カメラ座標上の画像を画像座標上に変換するための行列であり、次の(6−6)式で表される。
ここで、αVPP=dVPP・kX−VPP、βVPP=dVPP・kY−VPPである。また、dvppは投影距離[m]であり、kx−vpp及びky−vppはVPP画像IVのX座標方向及びY座標方向のスケール[pixel/m]である。さらに、Cx−vpp及びCy−vppはVPP画像IV中心のX座標及びY座標である。
この内部パラメータAVPPにより投影距離に道路平面のパラメータである距離dを指定すると、検出した道路平面Π上に投影される。
ステップS615において、車両前後方向とVPP画像の縦軸を一致させるために画像を角度θV及び角度−φだけ回転させる変換式である回転行列RVを算出する。すなわち、図30及び(6−7)式に基づいて基準カメラ12の光軸方向の単位ベクトルeOZをVPPカメラ座標系からみたベクトルuを設定し、該ベクトルuを仮想カメラ座標系におけるe’OX、e’OY平面に正射影したベクトルu’を(6−8)式に基づいて設定する。
ベクトルu及びu’をこのように定義するとき、角度θVはベクトルu’と仮想カメラ座標e’oyとのなす角度であり(6−9)式で表される。
また、角度φは車両前後方向に対するカメラ光軸であるベクトルeOZとのなす角度であり予め求めておく。求める回転行列RVは、次の(6−10)式で表される。
ステップS616において、Ab -1、Rn、RV及びAVPPに基づいて、VPP変換のための変換式HVPPを算出する。変換式HVPPは、次の(6−11)式で表される。なお、RVPP=RVRnである。
つまり、変換式HVPPでは、基準画像Ibを行列Ab -1、Rn、RV、及びAVPPで順に変換することと等価な処理が行われる。実際には1つの変換式HVPPで一度に変換が可能であるが、変換の過程を概念的な理解が容易となるように、各行列Ab -1、Rn、RV、及びAVPPによる変換を説明する。
先ず、基準画像Ib(図5参照)に対して逆行列Ab -1による変換を行うと画像座標からカメラ座標系への変換が行われ、図31に示すカメラ座標系基準画像Ibcが得られる。
次に、このカメラ座標系基準画像Ibcに対して回転行列Rnによる変換を行うと、カメラ座標系基準画像Ibcは角度θnだけ回転して道路平面Πに平行になるとともに遠方部分が拡大され、第1変換画像IV1が得られる。図32に示すように、この第1変換画像IV1は縦軸に対して角度(θV−φ)だけ傾いた画像として得られる。
さらに、第1変換画像IV1に対して回転行列RVによる変換を行うと、第1変換画像IV1は角度(θV−φ)だけ回転して車両前後方向とVPP画像の縦軸が一致することになり第2変換画像IV2が得られる。
最後に、第2変換画像IV2に対して仮想カメラの内部パラメータAVPPによる変換を行うと、スケール変換が行われて所望の大きさのVPP画像IV(図33参照)が得られる。
このような一連の行列式AVPPRVRnAb -1によって、変換式HVPPが構成されており、基準画像Ibに対して変換式HVPPによるVPP変換を行うことによりVPP画像IV(図33参照)が得られる。前記のとおり、このVPP変換は、ステップS62において行われる。得られたVPP画像IVは記憶部に記録される。
なお、このVPP画像IVは、内部パラメータAVPPの設定により、図33に示すように画像の左上部が原点であって、車幅方向がX方向(図33における右方向)、車両前後方向がY方向(図33における下方向)として規定される。
図33から了解されるように、VPP画像IVは、道路平面Πを上方の高い距離から眺めたような画像となり、道路の幅は略一定に映し出される。また、基準画像Ib上のトラック70や乗用車72は、画像上方部が拡大するように歪曲して映し出される。
また、この後ステップS63においては、平面領域Πf(図5参照)に対して変換式HVPPによるVPP変換を行い、上方から眺めた平面領域ΠfであるVPP平面画像IVf(図34参照)を求めて記録する。このVPP平面画像IVfはVPP画像IVにおける道路平面Πの領域を示すことになる。得られたVPP平面画像IVfは記憶部に記憶される。このVPP平面画像IVfは、道路平面と同じ形状となっており、図34に示す例では、前方の他のトラック70や路側部等の水平でない構造物や、道路平面と同一レベルでない面が除外された略V字形状となっている。
<ステップS7(移動体速度算出手段)>
ステップS7は、前記ステップS6で求めたVPP画像IV及びVPP平面画像IVfに基づいて、車両10の自車速度ベクトルVV(図25参照)を求めるための処理であり、図35に示すステップS71〜ステップS77のサブルーチン処理からなる。なお、以下の説明においては、現時刻の処理で得られたVPP画像IV及びVPP平面画像IVfをVPP画像IV(t)及びVPP平面画像IVf(t)と表し、前時刻に得られたものをVPP画像IV(t−1)及びVPP平面画像IVf(t−1)と表して区別する。また、ワールド座標系では車両10の前後方向が座標Zw(図10参照)として規定されているが、前記のとおり、VPP画像IVでは前後方向がY軸と規定されているため、以下のステップS7に関する説明では車両10の前後方向に対応するパラメータには添え字「Y」又は「y」を付けて表す。
ステップS71において、前時刻処理時のVPP画像IV(t−1)、VPP平面画像IVf(t−1)及び画像マッチング移動量ΔX0、ΔY0、Δθyaw−0を呼び出す。画像マッチング移動量ΔX0、ΔY0、Δθyaw−0は、前時刻処理時のステップS77で記録されたパラメータであり、前時刻におけるVPP画像IV(t)とVPP画像IV(t−1)とのマッチングによるずれ量を表す。
ステップS72において、VPP画像IV(t)とVPP画像IV(t−1)とのマッチングを行うための探索範囲を設定する。具体的には、画像マッチング移動量ΔX0、ΔY0、Δθyaw-0を中心とした所定幅を有する範囲としてΔX=ΔXMIN〜ΔXMAX、及びΔY=ΔYMIN〜ΔYMAX、Δθyaw=Δθyaw−MIN〜Δθyaw−MAXが設定される。ステップS73において、VPP画像IV(t)とVPP画像IV(t−1)とを前記ステップS72で設定した探索範囲内で相対的に移動させて、VPP画像マッチングを行う。探索範囲は画像マッチング移動量ΔX0、ΔY0、Δθyaw−0を基準とした範囲に設定されているため、VPP画像マッチングに要する時間は短時間で足りる。また、自車速度ベクトルVVは急激に変化することはないため、類似度が最大となる箇所が画像マッチング移動量ΔX0、ΔY0、Δθyaw−0を中心とした探索範囲からずれることはない。ステップS73におけるVPP画像マッチングの詳細な処理については後述する(図36参照)。
ステップS74において、前記ステップS73で求めたVPP画像マッチングの結果を参照して、VPP画像IV(t)とVPP画像IV(t−1)との類似度が最大となる箇所(後述するNSSD(ΔX、ΔY、Δθyaw)が最小となる箇所)を検出し、その箇所におけるVPP画像IV(t)とVPP画像IV(t−1)とのずれ量を移動量move(ΔXf、ΔYf、Δθyaw−f)として求める。ここで、ΔXf、ΔYf及びΔθyaw−fはVPP画像IV(t)とVPP画像IV(t−1)との類似度が最大となる箇所におけるΔX、ΔY及びΔθyawの値である。
ステップS75において、移動量move(ΔXf、ΔYf)に基づいて下記の(7−1)〜(7−3)式により車両10の自車速度ベクトルVVの大きさを求める。
ここで、Vx及びVzは、車両10の幅方向及び車両前後方向の速度であり、dvpp、αVPP及びβVPPは前記(6−6)式において定義される係数及び数値である。また、前記のとおりΔtは演算周期である。(7−2)式及び(7−3)式で、車両前後方向の速度をVzとして添え字「y」でなく「z」を用いているのは、他の処理に対して整合性を考慮しVPP画像の座標からワールド座標系に戻しているためである。
自車速度ベクトルVVの大きさは、周知のとおり直交する2方向速度成分で形成される斜辺部の長さとして表されることから(7−3)式が成り立つ。
(7−3)式及び(7−4)式により、自車速度ベクトルVVの大きさと角度θYAWが求まることから自車速度ベクトルVVが決定され、該自車速度ベクトルVVを図25上に表す。なお、角度θYAWは、車両10の回転角度として用いることもできる。
ところで、車両10の回転角度をより厳密に求めるためには、VPP画像IV(t−1)をΔX及びΔY方向に並進的に移動させるだけではなく、回転を含んだ移動をさせた上で類似度の演算を行うとよい。この場合、画像マッチング移動量ΔX0、ΔY0を中心とした所定幅を有する範囲としてΔX=ΔXMIN〜ΔXMAX、ΔY=ΔYMIN〜ΔYMAX及び回転角度としてのω=ΔθMIN〜ΔθMAXが設定され、3自由度の移動を行う。
ステップS76において、自車速度ベクトルVV、角度θYAWを記録する。また、ステップS74で求めたΔXf、ΔYf、Δθyaw−fを次回処理時の画像マッチング移動量ΔX0、ΔY0、Δθyaw−0として用いるために、ΔX0←ΔXf、ΔY0←ΔYf、Δθyaw−0←Δθyaw−fと代入した後に記録する。
次に、ステップS73におけるVPP画像マッチングの第1の処理方法について説明する。この第1の処理方法は図36におけるステップS731〜S735の処理からなる。なお、このステップS731〜S735の処理は、前記ステップS72で設定された探索範囲としてのΔX=ΔXMIN〜ΔXMAX、及びΔY=ΔYMIN〜ΔYMAX、Δθyaw=Δθyaw−MIN〜Δθyaw−MAXの領域について個別に実行されるが、繰り返し制御についての説明を省略して1回分の処理についてのみ説明する。
先ず、ステップS731において、VPP平面画像IVf(t−1)を現時刻で設定されているΔθyaw回転し、ΔX及びΔYの方向に移動させる。(図37の「S731」参照)。なお、回転の中心は、VPP画像における光学中心の位置(Cx−vpp、Cy−vpp)とする。
ステップS732(第二の道路平面領域抽出手段)において、VPP平面画像IVf(t)と前記ステップS731で移動したVPP平面画像IVf(t−1)との共通領域R(ΔX、ΔY,Δθyaw)を求める(図37の「S732」参照)。
ステップS733において、VPP画像IV(t−1)をその時点で設定されているΔθyaw回転し、ΔX及びΔYの方向に移動させる(図37の「S733」参照)。
ステップS734において、共通領域R(ΔX,ΔY,Δθyaw)、VPP画像IV(t)及び移動したVPP画像IV(t−1)に基づいて、次の(7−4)式によりVPP画像マッチングとしてNSSDを計算する。
ここで、xiは共通領域R(ΔX,ΔY,Δθyaw)に含まれるピクセルの座標であり、Nは、共通領域R(ΔX,ΔY,Δθyaw)に含まれるピクセルの総数であり、IV(t−1)move(ΔX,ΔY)は、ステップS734でVPP画像IV(t−1)をΔθyaw回転し、ΔX及びΔYだけ移動した画像を示す。
この(7−4)式により、VPP画像IV(t)とVPP画像IV(t−1)をΔθyaw回転し、ΔX及びΔYだけ移動した画像とのピクセル毎の輝度差の二乗和が正規化された値として求められ、次のステップS735において、移動量move(Δx、Δy、Δθyaw)毎に類似度としてNSSD(Δx、Δy、Δθyaw)が記録される。NSSD(Δx、Δy、Δθyaw)は正規化されていることから共通領域R(ΔX,ΔY,Δθyaw)の面積の大小に拘わらずに相互比較が可能となる。
このようなステップS7の処理によれば、対応点探索を行うことがないため、ロバスト性が高い。また平面の位置姿勢パラメータに基づいて平面に平行なVPP画像を生成するため、車両姿勢の変動等による影響を受けず耐振動性が高い。実際上、道路平面Πには不規則であっても地模様等何らかの模様や特徴部が存在していることから、このような部分に基づいて自車速度ベクトルVVを求めることができる。
さらに、平面領域Πfに基づく共通領域R(ΔX,ΔY,Δθyaw)内でVPP画像マッチングを行うことから、他の車両や路側部等の余分な部分が削除されて高精度且つ高速な演算が遂行される。
また、VPP画像マッチングにおける類似度として、NSSDに代えて前記のZNCC等を用いてもよい。
次に、ステップS73におけるVPP画像マッチングの第2の処理方法について説明する。この第2の処理方法は、基本的には前記ステップS1及び前記非特許文献1で説明した濃度勾配法と同じ方法であり、前記射影変換行列Hに相当する移動回転行列Mを微小量変化させながら2つの画像が最も重なるように収束させる。
先ず、適当に真値に近い初期値としての移動回転行列Mを設定しておき、次の(7−5)式により、VPP画像IV(t−1)を移動回転変換する。ここで、Ir(x)をVPP画像IV(t−1)の座標x(x,y)における輝度値とする。なお、移動回転行列Mは、3×3の行列である。また、移動回転行列Mの初期値としては、前時刻のステップにて算出された値を用いるとよい。
移動回転行列Mの各パラメータを微小量変化させたとき、変換されたVPP画像I
V(t−1)上の座標x(x,y)が座標x″(x″,y″)に変化するものとすると、
の関係が得られる。なお、Iは3×3の単位行列であり、Dは移動回転行列Mの各パラメータの微小変化量をパラメータとする3×3の微小変化行列であり次の(7−7)式で表される。
ここで、ωは微小回転角度、dx及びdyはx方向及びy方向の微小移動量である。また、ω、dx及びdyをまとめて1つのパラメータΘとしても表す。このパラメータΘを引数とし、(7−5)、(7−6)式の関係を用いて、移動回転変換されたVPP画像IV(t−1)とVPP画像IV(t)との重なり状態を示す評価関数E(Θ)は、次の(7−8)式で表される。
(「i」は、画素番号を表す。)
と表すことができる。ここで、R
planeは、VPP平面画像I
Vf(t−1)、I
Vf(t)における道路平面Πに相当する領域である。
この(7−8)式は前記(1−5)式と等価なものであり、基準画像IbとVPP画像IV(t)、参照画像IrとVPP画像IV(t−1)、行列Dxと行列Dがそれぞれ対応する。
したがって、前記と同様の方法により、gi(輝度勾配)、ei(輝度差分)、ヤコビアンJΘiを算出し、微小変化行列Dを最小二乗法を用いて求めることができる。
以上のようにして求めた微小変化行列Dを用いて、移動回転行列Mを次の(7−9)式にしたがって更新する。
そして、(7−9)式で求めた移動回転行列Mを用いて、再び(7−8)式の評価関数E(Θ)の値を算出する処理を繰り返す。この繰り返しの処理において移動回転行列Mは最小二乗法を解いて得られる微小変化行列Dを用いて、M←M(I+D)として更新される。評価関数E(Θ)の値が所定の値以下に収束したときのをMの各成分よりΔθyaw、ΔX、ΔYを式(7−10)を用いて求めることができる。
このようなステップS73の第2の処理方法では、前記の第1の処理方法で求めたΔθyaw、ΔX、ΔYを初期値として第2の処理方法を用いることで、一層正確にΔθyaw、ΔX、ΔYを求めることができる。
<ステップS8>
ステップS8は、検出した物体が静止物体であるか移動物体であるかを判断する処理であり、図38に示すステップS81〜S84のサブルーチン処理からなる。
ステップS81において、前記ステップS55で求められた各物体の相対速度であるベクトルVr1、Vr2及びVr3を読み出す。これらのベクトルは図25における各矢印で示されている。
ステップS82において、前記ステップS7において求めた車両10の自車速度ベクトルVVを読み出す。この車両10の自車速度ベクトルVVは図25の原点を基点に示されている。
ステップS83において、検出された各物体の絶対速度ベクトルVABSN(N=1、2、3…)を算出する。絶対速度VABSNはVABSN=VrN+VV'としてベクトル加算により求められる。ただし、VV'は相対速度VrNに含まれる、自車両のモーションによる速度分である。図39に各物体毎に求められた絶対速度ベクトルVABSNを示す。図39から明らかなように、静止物体であるガードレール74については、VABS3≒0となる。
ステップS84において、絶対速度VABSNに基づいて、検出された各物体が静止物体であるか移動物体であるかを調べる。具体的には、各物体の絶対速度のベクトルの大きさ|VABSN|と閾値ThABSを比較して、ThABS≧|VABSN|であるとき対象となる物体は静止物体であり、|VABSN|>ThABSであるときは移動物体であると判断される。これにより、VABS3≒0であるガードレール74は静止物体として判別され、トラック70及び乗用車72は移動物体として識別される。
このように、ステップS8の処理によれば、検出された物体の絶対速度を精度よく求めることができ、物体が静止物か否かを正確に判断することができる。この判断結果の情報は、例えば、車両10と物体との所定の接近注意喚起手段に供することができる。
また、ステップS7及びS8の処理によれば、VPP画像IVの平面領域Πfのみを用いて移動量を算出することにより、移動する物体と静止物とが混在する環境下においても正確に自車速度ベクトルVVを求めることができる。
さらに、例えば路面など一定の高さ(距離)にある物体を撮像して、路面を基準として速度計測を行うように定めた場合に、路面に対して高さのある物体が撮像され、該物を基準として速度計測を行うと高さに基づく速度検出の誤差が発生することが知られているが、ステップS7及びS8の処理によれば、平面領域Πfのみを用いることにより高さのある物体を除外して算出できるので、高さに基づく誤差が発生しない。
またステップS7及びS8の処理によれば、平面までの距離も検出するため、例えば歩道など路面に対して高さのある平面が画像中の大半を占める場合にも、その高さを考慮して速度を算出できる。
なお、本実施の形態において求められる平面領域Πfは、車両10が走行する略水平な道路平面Πに対応する領域であって、「平面」であっても垂直な面やレベルの異なる他の面は除外される。
また、上記の説明では、基準カメラ12と参照カメラ14が設けられているものとしたが、必要に応じてより多くのカメラを設けてもよい。