平面検出装置及び検出方法
技術分野
[0001] 本発明は、例えば、自動車の運転者支援や自律移動車の自動運転を実現するた め、これらの移動体に搭載した一対の撮像手段により撮像した画像から、前記移動 体が走行可能な平面を検出する平面検出装置及び検出方法に関する。
背景技術
[0002] 自律移動車等の移動体の誘導にお!、て、走行可能な道路平面領域を検出し、ある いは、走行環境に存在する障害物を検出する方法は、レーザレーダ、超音波、ミリ波 レーダ等を利用する方法と、画像を利用する方法とに大別することができる。
[0003] レーザレーダやミリ波レーダを用いる検出方法では、一般に装置が高価であり、ま た、十分な空間分解能が得られないという問題がある。また、超音波を用いる検出方 法では、遠くの計測が困難であって、空間分解能も低いという問題がある。
[0004] 一方、画像を用いる方法では、単眼による方法と、複眼を用いる方法とに分けること ができる。従来、画像を用いる方法の多くは単眼、すなわち、 1つの視点から得られる 画像を用いるもので、主に、高速道路等の整備された環境で用いることを想定し、路 面の白線 (例えば分離線や中央線)等を検出することにより、走行領域を検出してい る。しかしながら、白線等の存在が保証されず、また路面の色やパターンがさまざま であるような一般道や駐車場等では、単眼画像力も得られる濃度パターンのみから、 走行可能領域と障害物を安定に区別することは困難である。
[0005] これに対して、複眼、すなわち、ステレオ画像を利用する検出方法では、原理的に は環境の 3次元構造を利用できるため、より安定に走行環境を認識できる可能性が ある。特に、走行可能な領域は空間中でほぼ平面であるとみなせることから、画像間 の射影変換行列を算出し、この射影変換行列により一方の画像を 2次元射影変換し て他方の画像に重畳させ、画像同士の重なり具合から、平面領域か否か、あるいは、 障害物か否かを検出する方法が提案されている (特許文献 1、非特許文献 1参照)。 特許文献 1:特開 2000— 293693号公報
非特許文献 1 :ホイング—ヤング'シャム(Heung-Yeung Shum)、リチャード'セー リスキ(Richard Szeliski)、「"パノラミック 'イメージ'モザイク"テク-カル 'レポート(" Panoramic Image Mosaics fechnical report)」、 1997年、ェム ·エス ·ァ ~~ ノレ一ティ^ ~ ·アーノレー 97— 23、マイクロソフト'リサーチ(MSR— TR— 97- 23, Micr osoft Researcn)
発明の開示
[0006] ところで、特許文献 1に開示された検出方法は、各画像から対応する 4点以上の特 徴点を抽出し、それらの対応点に基づいて射影変換行列の各パラメータを算出する もので、射影変換行列を得るために対応点の存在が前提となっている。そのため、例 えば、テクスチャの少ない画像では、対応点を見出すことができず、射影変換行列を 算出できなくなるおそれがある。
[0007] 一方、非特許文献 1に開示された検出方法は、一般に「濃度勾配法」と称されるも のであり、推定した射影変換行列により一方の画像を 2次元射影変換して得た画像と 、他方の画像とを比較して濃度パターンが一致するか否かを判定し、所望の一致範 囲となるまで射影変換行列を繰り返し修正して求めるようにしたものである。この場合 、非特許文献 1では、移動体に対する走行面 (道路面)の変動を考慮しておらず、一 且算出された射影変換行列は、当該移動体に固有のものとして決定されることを前 提としている。
[0008] しかしながら、移動体は、移動に伴ってピッチングやローリングの挙動を示すため、 一般に移動体と走行面との位置関係は一定とはならない。したがって、適切な射影 変換行列を得るためには、走行面に対する移動体の状態に応じて算出する必要が ある。この場合、所望の射影変換行列の算出に要する時間は、射影変換行列を構成 する未知のパラメータの数に依存する。そのため、例えば、移動体の状態が大きく変 動し、且つ、演算時間が制約されるような場合、射影変換行列の繰り返し演算を中途 で打ち切ってしまうと、十分な精度を有する射影変換行列を得ることができなくなって しまう。
[0009] 本発明の一般的な目的は、画像間の射影変換行列を高精度且つ短時間で算出し 、平面を高精度に検出することのできる平面検出装置及び検出方法を提供すること
にある。
[0010] 本発明の主たる目的は、テクスチャの少な 、画像であっても、射影変換行列を確実 に算出することのできる平面検出装置及び検出方法を提供することにある。
[0011] 本発明の他の目的は、移動体が走行可能な平面を高精度且つ短時間で算出し、 算出された平面情報に従って移動体の運転を支援することのできる平面検出装置及 び検出方法を提供することにある。
[0012] 本発明に係る平面検出装置及び検出方法では、所定の位置関係に配置された一 対の撮像手段を用いて、平面を含む相互に重複する領域の画像を撮像した後、予 め求めた回転行列及び並進ベクトルを用いて、一方の撮像手段により撮像された画 像に含まれる平面を、他方の撮像手段により撮像された画像に含まれる平面に重畳 させることのできる射影変換行列を算出する。この場合、回転行列及び並進ベクトル を固定されたパラメータとすることで、射影変換行列を構成する未知のパラメータ数 が削減され、射影変換行列が高精度且つ短時間で算出される。同様に、射影変換 行列とともに、平面を規定する法線ベクトル及び撮像手段から平面までの距離が高 精度且つ短時間で算出される。
[0013] なお、射影変換行列力 求めた平面領域を平面パラメータを用いて変換することに より、撮像手段に対する平面領域の空間位置を検出することができるとともに、空間 位置が検出された平面上に存在する物体を検出することができる。
図面の簡単な説明
[0014] [図 1]本発明の平面検出装置及び検出方法が適用される車両の説明図である。
[図 2]図 1に示す車両に搭載される演算処理装置の機能構成ブロック図である。
[図 3]図 1に示す演算処理装置における処理の全体の流れを説明するフローチャート である。
[図 4]一対の撮像手段間での 2次元射影変換を説明するための模式図である。
[図 5]平面領域を抽出する手順の説明図である。
[図 6]物体抽出処理の処理内容を示すフローチャートである。
[図 7]候補点を高さの制限に基づいて抽出する様子を示す模式図である。
[図 8]投票処理の処理の様子を示す模式図である。
圆 9]投票面上で行うクラスタリング処理の様子を示す模式図である。
[図 10]投票面を示す図である。
[図 11]物体の各エッジ点のワールド座標を求める処理内容を示すフローチャートであ る。
[図 12]エッジが抽出されたエッジ画像を示す図である。
[図 13]エッジが抽出されたエッジ画像の実際の画像データである。
[図 14]平面領域が除去された修正エッジ画像を示す図である。
圆 15]ェピポーララインが設定された参照画像を示す図である。
[図 16]ワールド座標系とカメラ座標系との関係を示す模式図である。
[図 17]奥行き画像を示す図である。
[図 18]物体の相対速度を算出する処理内容を示すフローチャートである。
[図 19]物体領域の重なり具合を調べる処理内容を示すフローチャートである。
[図 20]クラスタに基づいてテンプレートを作成する処理内容を示す模式図である。 圆 21]テンプレートが設定された基準画像を示す図である。
圆 22]重心移動距離を算出する処理内容を示すフローチャートである。
[図 23]物体同一判定の処理内容を示すフローチャートである。
[図 24]同一物体の位置を再計算する処理内容を示すフローチャートである。
[図 25]物体の相対速度が記録された投票面を示す図である。
[図 26]物体の相対速度を算出する処理内容を示すフローチャートである。
[図 27]ェピポーララインを設定する処理を説明するための模式図である。
[図 28]VPP画像を生成するための処理内容を示すフローチャートである。
[図 29]VPP画像変換式を算出する処理内容を示すフローチャートである。
[図 30]法線ベクトル、カメラ座標系、及び基準カメラの光軸との関係を示す座標図で ある。
圆 31]カメラ座標系基準画像を道路平面に平行となるように変換する処理の説明の ための図である。
圆 32]第 1変換画像を回転変換する様子を示す模式図である。
圆 33]基準画像を VPP変換する様子を示す模式図である。
[図 34]道路平面領域を VPP変換する様子を示す模式図である。
[図 35]自車速度を算出する処理内容を示すフローチャートである。
[図 36]VPP画像マッチングの処理内容を示すフローチャートである。
[図 37]VPP画像マッチングの処理内容を示す模式ブロック図である。
[図 38]物体の絶対速度を算出するための処理内容を示す模式ブロック図である。
[図 39]物体の絶対速度が記録された投票面を示す図である。
発明を実施するための最良の形態
[0015] 図 1は、本発明の平面検出装置及び検出方法が適用される実施形態の車両 10 ( 移動体)を示す。車両 10のウィンドシールド上部の左右には、車両 10が走行する道 路平面領域を含む画像を撮像する一対の撮像手段である基準カメラ 12及び参照力 メラ 14が固定配置される。基準カメラ 12及び参照カメラ 14は、共通の撮像領域が設 定された CCDカメラ等カゝらなるステレオカメラであり、これらには、撮像した画像を処 理することで道路平面領域と障害物となる可能性のある物体とを検出する演算処理 装置 16が接続される。なお、以下の説明では、基準カメラ 12によって撮像された画 像を基準画像と称し、参照カメラ 14によって撮像された画像を参照画像と称する。
[0016] 図 2は、演算処理装置 16の機能構成ブロックを示す。演算処理装置 16は、基準力 メラ 12によって撮像した基準画像を処理する画像処理部 18と、参照カメラ 14によつ て撮像した参照画像を処理する画像処理部 20と、所定の撮像時間間隔で撮像され た基準画像及び参照画像を記憶する画像記憶部 22とを備える。
[0017] また、演算処理装置 16は、演算制御部 24を備え、演算制御部 24には、画像記憶 部 22、パラメータ記憶部 26 (パラメータ記憶手段)、射影変換行列算出部 28 (射影変 換行列算出手段、射影変換行列推定手段)、平面パラメータ算出部 30 (平面パラメ ータ算出手段、平面パラメータ推定手段)、平面領域抽出部 32、物体検出部 34、物 体相対速度算出部 36、仮想投影面画像生成部 38 (空間位置算出手段)、自車速度 算出部 40、物体絶対速度算出部 42及び表示処理部 44が接続される。
[0018] パラメータ記憶部 26は、基準カメラ 12の座標系を参照カメラ 14の座標系に変換す るための回転行列及び並進ベクトルを既知のパラメータとして記憶するとともに、平面 の法線ベクトルの初期値、基準カメラ 12から当該平面までの距離の初期値等、演算
に必要な初期値パラメータ、基準カメラ 12及び参照カメラ 14の内部パラメータ、前回 の演算や演算中に算出された各パラメータ等を記憶する。射影変換行列算出部 28 は、参照画像中の道路平面領域を基準画像中の道路平面領域に 2次元射影変換す る射影変換行列を算出する。平面パラメータ算出部 30は、道路平面の法線ベクトル 及び基準カメラ 12の光学中心力 道路平面までの距離を平面パラメータとして算出 する。平面領域抽出部 32は、基準画像と射影変換行列によって射影変換した参照 画像とを用いて、車両 10が走行可能な道路平面領域を抽出する。物体検出部 34は 、道路平面領域外の領域に存在する物体を検出する。物体相対速度算出部 36は、 抽出された物体の車両 10に対する相対速度ベクトルを算出する。仮想投影面画像 生成部 38は、抽出された道路平面領域を実際の道路面に平行な仮想平面に投影し た仮想投景面(VPP)画像(VPP :Virtual Projection Plane)を生成する。自車 速度算出部 40は、 VPP画像上で車両 10の自車速度ベクトルを算出する。物体絶対 速度算出部 42は、車両 10に対する物体の相対速度ベクトルと車両 10の自車速度 ベクトルとから、各物体の絶対速度ベクトルを算出する。表示処理部 44は、上記の各 演算結果をディスプレイに表示する処理を行うとともに、演算結果に基づいて必要に 応じた警告等の報知を行う。
[0019] 本発明の平面検出装置及び検出方法が適用される車両 10及び演算処理装置 16 は、基本的には以上のように構成されるものであり、次に、図 3に示すフローチャート に基づき、演算処理装置 16による処理の全体の流れを説明する。
[0020] 先ず、基準カメラ 12及び参照カメラ 14により、車両 10が走行する道路平面領域を 含む基準画像及び参照画像を所定の時間間隔で撮像し、画像処理部 18、 20にお V、て所定の画像処理を行った後、画像記憶部 22に各時間毎の画像情報として記憶 させる (ステップ SO)。
[0021] 次に、射影変換行列算出部 28において、パラメータ記憶部 26に記憶されている初 期値パラメータを用いて、参照画像中の道路平面領域を基準画像中の道路平面領 域に 2次元射影変換する射影変換行列を動的に算出する (ステップ Sl)。
[0022] 平面領域抽出部 32は、ステップ S1で算出した射影変換行列を用いて、基準画像 力も道路平面領域を抽出する (ステップ S2)。
[0023] 平面パラメータ算出部 30は、道路平面の法線ベクトル及び基準カメラ 12の光学中 心から道路平面までの距離を平面パラメータとして算出する (ステップ S3)。
[0024] 次に、物体検出部 34は、ステップ S3で算出した平面パラメータを用いて、ステップ
S2で抽出した道路平面領域を道路面上に変換して自分の車両 10以外の障害物と なる可能性のある物体を検出し、若しくは、ステップ S 2で抽出した道路平面領域以 外の領域に対してステレオ計測することにより物体を検出する (ステップ S4)。
[0025] 物体相対速度算出部 36は、抽出された各物体の時間毎の位置変化から、車両 10 に対する各物体の相対速度ベクトルを算出する (ステップ S 5)。
[0026] さらに、仮想投影面画像生成部 38は、ステップ S3で算出した道路平面の傾きであ る法線ベクトルを用いて、道路面を上方から見た画像である仮想投影面 (VPP)画像 を生成する (ステップ S6)。
[0027] そして、自車速度算出部 40は、ステップ S6で生成された VPP画像の各時刻での 位置から車両 10の自車速度ベクトルを算出する (ステップ S7)。
[0028] さらに、物体絶対速度算出部 42は、ステップ S5で算出した物体の相対速度べタト ルと、ステップ S7で算出した車両 10の自車速度ベクトルとを用いて、各物体の絶対 速度ベクトルを算出する (ステップ S8)。
[0029] なお、表示処理部 44は、以上のようにして抽出した道路平面領域、障害物となる可 能性のある物体、車両 10及び物体の絶対速度等をディスプレイに表示するとともに、 例えば、車両 10と物体との相対的な位置関係、速度関係に基づき、必要に応じて警 告等の報知を行うことができる。
[0030] 次に、本実施形態をステップ毎に詳細に説明する。
[0031] 先ず、本発明に用いられる道路平面領域の抽出及び 2次元射影変換の原理を説 明する。なお、以下の説明において、同次座標は、文章中では、符号に記号门を付 カロして表し、数式中では、符号の上に記号「〜」を付加して表すものとする。また、射 影変換された画像については、文章中では、符号に記号に 」を付加して表し、数式 中では、符号の上に記号「〜」を付加して表すものとする。
[0032] 図 4に示すように、空間中にある平面 Π上の観測点 Mlが基準画像 I及び参照画
b
像 Iに投影されるとき、基準画像 I上での同次座標を π 、参照画像 I上での同次座 r b b r
標を π とすると、各同次座標 π 、 m"は、(1 1)式に示すように、 2次元射影変換 r b r
によって関係付けられることはよく知られている。
[0033] [数 1] mr^Hmb - " ( 1-1 )
(左辺と右辺との間の「〜」は、定数倍の不定性を許して等しいものとする。 ) [0034] ここで、 Hは 3 X 3の射影変換行列である。また、図 4において、 Oは基準カメラ 12の 光学中心、 O' は参照カメラ 14の光学中心、 Rは基準カメラ座標系から参照カメラ座 標系への回転行列、 tは、参照カメラ座標系において参照カメラ 14の光学中心から 基準カメラ 12の光学中心へ向かう並進ベクトル、 dは基準カメラ 12の光学中心 Oと平 面 Πとの距離、 nは平面 Πの法線ベクトルである。
[0035] 道路等の走行可能な領域は、空間中でほぼ平面であると見なすことができれば、 参照画像 Iに対して適切な射影変換を施すことにより、平面の部分に対して基準画 像 I
bに一致するような画像を得ることができる。一方、平面上にない物体は、射影変 換した際に画像が一致しないため、その物体の領域は走行可能な領域でない、ある いは、障害物となり得る領域であると判断することができる。したがって、平面の拡がり を算出することにより、走行可能な領域及び障害物となる物体の検出が可能となる。
[0036] くステップ SO >
基準画像 I及び参照画像 Iに係る画像情報を取得する。
b r
[0037] 基準カメラ 12及び参照カメラ 14を用いて画像を撮像し、各画像間の明るさの違い 等を除くため、画像処理部 18及び 20において、各画像に LOG (Laplacian of Ga ussian)フィル夕をかける。また、 LOGフィルタを掛けた画像は、コントラストが低いた め、さらに、ヒストグラム平坦化処理を施す。これらの処理が施された基準画像 I及び
b 参照画像 Iとその原画像は、画像記憶部 22に記憶される。
[0038] くステップ Sl >
射影変換行列算出部 28において、画像記憶部 22に記憶された基準画像 I及び参
b 照画像 Iを用いて、路面に対する射影変換行列 Hを動的に推定する。
[0039] すなわち、基準画像 I内のある領域 R1で、射影変換行列 Hを微少量変化させ、以 b
下の(1 2)式の評価関数 E (H)を最小化する射影変換行列 Hを、繰り返し最適化 によって推定する。
[0040] [数 2]
E(H) = ^[/r(Hmi,) - /i,(m;>)r . . . ( 1-2 )
' なお、 I (π )、 I (ΗπΓ )は、同次座標 πΓ、 Ηπ における基準画像 I及び参照 b b r b b b b 画像 Iの輝度値を表す。
[0041] 上記の推定には、適当に真値に近い初期値としての射影変換行列 Hと、領域 R" ( 本発明では、以後「計算領域」と呼ぶ。)とを必要とする。そこで、時系列情報を利用 し、以下に述べる手順により、射影変換行列 Hと計算領域 R1とを求める。
[0042] すなわち、同時刻における基準画像 I及び参照画像 I間の射影変換行列 Hと、連 b r S 続する時間で取得した 2つの基準画像 I間の射影変換行列 Hと、基準画像 Iに対す b m b る計算領域 R1とを求める。そして、ある時刻 tにおける射影変換行列 H (H、 H
s m )の推 定の際には、前時刻(t 1)までの推定結果を利用する。
[0043] 先ず、連続する基準画像 I (t 1)、 I (t)間の射影変換行列 H (t)を (1 2)式に b b m
基づく繰り返し最適化によって推定する。このとき、射影変換行列 H (t)の初期値と m
しては、前時刻に推定された射影変換行列 H (t— 1)を、また、計算領域 R1の初期 m
値としては、基準画像 I (t— 1)に対して前時刻で求められている計算領域 R' (t— 1) b
を用いることができる。
[0044] 次に、求められた射影変換行列 H (t)を用いて、前時刻の計算領域 R" (t— 1)を変 m
換することにより、現時刻における計算領域 R' (t)の予測値を求める。
[0045] 基準画像 I及び参照画像 I間の射影変換行列 H (t)は、前時刻にお ヽて推定され b r S
た射影変換行列 H (t— 1)を初期値とし、射影変換行列 H (t)から求めた計算領域 m
R1 (t)を用いて、(1 2)式に基づく繰り返し最適化によって推定する。
[0046] このように、時系列画像を用いた連続推定において、十分に真値に近い初期値とし ての射影変換行列 Hと計算領域 R1とを用いて、時刻 tにおける基準画像 I及び参照
画像 I間の動的な射影変換行列 H (以下、射影変換行列 Hという。)を安定して推定 r S
することができる。
[0047] 次に、 (1 - 2)式を用いた繰り返し最適化 (濃度勾配法)による射影変換行列 Hの 推定方法について詳細に説明する。
[0048] 濃度勾配法は、 2つの画像の一方を微小量変化させ、画像の重なり具合 (濃度差) を表す評価関数を最小化することにより、画像が最も重なるような射影変換行列 Hの 変化量を推定する方法である。
[0049] 座標 x(x, y)における参照画像 Iの輝度値を I (x)とすると、適当な射影変換行列 H により射影変換された参照画像 I ' は、次の(1— 3)式で表される。なお、参照画像 I の上の「〜」は、射影変換された後の画像を表し、 Xの上の「〜」は、座標 x (x, y)の同 次座標を表す。
[0050] [数 3] ΐΓ (χ) =ΙΓ (Ηχ) ー(1.3 )
[0051] なお、射影変換行列 Ηは、 3 X 3のパラメータを要素とする自由度 8 (1パラメータを 固定)の行列とする。
[0052] 射影変換行列 Ηの 8つのパラメータを微小量変化させたとき、射影変換された参照 画像 I ' 上の座標 x(x, y)が座標 X" , γ" )に変化するものとすると、
[0053] [数 4] "〜(l + Dx)ii …( 〜」は、定数倍の不定性を許して等しいものとする。 ) の関係が得られる。なお、 Iは 3 X 3の単位行列、 D
Xは射影変換行列 Hの各パラメ一 タの微小変化量をパラメータとする 3 X 3の微小変化行列とする。
[0054] (1 - 3) , (1 4)式の関係を用いて(1 2)式の評価関数 E (H)を書き替えると、射 影変換された参照画像 I
r ' と基準画像 Iとの
b 重なり状態を示す評価関数 E (H)は、射
影変換行列 Hの微小変化行列 Dのパラメータを変数として、
[数 5]
2
•(1-5)
E(dx) = 2[ ) - 1 )
(「i」は、画素番号を表す。)
と表すことができる。なお、 dは、微小変化行列 Dのパラメータをベクトルで表したも
X X
のである。
[0055] (1 5)式をテーラ展開して一次近似すると、評価関数 E(d )は、
X
[0056] [数 6]
但し 9j =VIr(xi) ei = ( i) Ib )
Jdxi = J dxi (xiノ
(「 」は、(1— 5)式の一次近似の範囲で等しいものとする。 )
となる。
[0057] (1-6)式の評価関数 E (d )を最小化するベクトル dは、評価関数 E (d )をベクトル
X X X
dで微分し、その微分値を 0とおいて、
X
[0058] [数 7]
Adx -b … - 7 ) 但し
dxi dxi
1
を解くことにより求めることができる。
[0059] この場合、 g (輝度勾配)、 e (輝度差分)は、基準画像 I及び射影変換された参照
i i b
画像 I '
r 力も算出され、ヤコビアン J
dXiは、座標 x (x, y)力も算出できる。したがって、 ベクトル dは、最小二乗法を用いて(1— 7)式から求めることができる。
X
[0060] 以上のようにして求めたベクトル d、すなわち、微小変化行列 Dを用いて、射影変
X X
換行列 Hを次の(1— 8)式にしたがって更新する。なお、 Iは、 3 X 3の単位行列とする
[0061] [数 8]
H H(l + Dx) ' ( 1-8 )
[0062] そして、(1— 8)式で求めた射影変換行列 Hを用いて、再び(1— 6)式の評価関数 E (d )の値を算出する処理を繰り返し、評価関数 E (d )の値が所定の値以下に収束
X X
したときの射影変換行列 Hを所望の射影変換行列 Hとすることができる。
[0063] ところで、上記の説明では、射影変換行列 Hを構成する 8つのパラメータが不明で あるものとして射影変換行列 Hを推定しているが、基準カメラ 12及び参照カメラ 14間 の回転行列 R及び並進ベクトル tが車両 10に対して固定されているものとすれば、予 め求めた回転行列 R及び並進ベクトル tを用いて、平面パラメータ(法線ベクトル n及 び距離 d)を変化させることにより、動的な射影変換行列 Hを迅速且つ高精度に推定 することができる。
[0064] この場合、回転行列 R及び並進ベクトル tは、例えば、基準カメラ 12及び参照カメラ 14を車両 10に固定した後、整備場のように、車両 10を停止させた理想的な環境下 で高精度に求めておくことが可能である。そして、走行中等の任意の時刻ににおい て射影変換行列 Hを求める際、理想的な環境下で求めた高精度な回転行列 R及び 並進ベクトル tを用いるため、射影変換行列 Hを高精度に算出することができる。
[0065] そこで、先ず、回転行列 R及び並進ベクトル tの算出方法について説明する。なお、 回転行列 R及び並進ベクトル tは、以下のようにして算出する代わりに、その他のキヤ リブレーシヨン方法を利用して求め、あるいは、基準カメラ 12及び参照カメラ 14の位 置関係を直接計測して求めることもできる。
[0066] 車両 10を停止させた状態において、基準カメラ 12及び参照カメラ 14によって平面 Πを撮像した後、(1— 5)式の評価関数 E (d )を用いて射影変換行列 Hを推定する。
X
[0067] ここで、射影変換行列 Hは、基準カメラ座標系から参照カメラ座標系への回転行列 R、参照カメラ 14の光学中心力も基準カメラ 12の光学中心へ向力 並進ベクトル t、 基準カメラ 12及び参照カメラ 14の内部パラメータ A、 Aを用いて、
b r
[0068] [数 9]
H = Ar · k(R +―) Ab1 …" )
d と表される。なお、 kは、 0でない係数であって、画像から得られた射影変換行列 Hに 定数倍の自由度が存在することを表す。内部パラメータ A、 Aが既知であるとすると b r
、次の(1 10)式が成立する。
[0070] そこで、射影変換行列 H' を特異値分解する。 U、 Vを正規直交行列、∑を射影変 換行列 H' の特異値 σ 〜σ ( σ ≥ σ ≥ σ >0)を要素とする対角行列として、射
1 3 1 2 3
影変換行列 H' は、
[0071] [数 11]
と書き替えることができる。なお、
[数 12]
[0073] (1-10) 11)式から、
[0075] ここで、
[0076] [数 14]
UtRV = R , = t , n V-n (ι ΐ4)
とすると、次の(1 15)式が得られる。
[0077] [数 15]
∑ = ■(1-15)
基準 kカメラ座標系の基底ベクトル (e , e , e )を導入し、
1 2 3
2
[0078] [数 16]
σ
2 j
2
n = ei +n +つ +n3 '·-(ι-ΐ6)
2
σ
i 2
とすると、(1— 15)式力 得られる 3つのベクトル方程式と、η、η' が単位ベクトル、 V
2
が正規直交ベクトル、 力 ルム変化のない回転行列であることから、次の(1— 17 o
)式が得られる。
[0079] [数 17]
(i≠ 1) ···( 1-17)
(1 17)式を11' η' η' の連立方程式と考えれば、 0でない解を持つには
1 2 3
、行列式が 0であるから、
[0080] [数 18]
(k2 - o ){ 2 - o )( 2 - of ) = 0 ·'·(ι-ΐ8)
となる。
[0081] そこで、射影変換行列 H' の特異値 σ 〜σ 力
1 3
(I) 重解を持たない場合(σ ≠ σ ≠ σ )
1 2 3
(II) 重解を持つ場合(σ = σ ≠ σ 又は σ ≠ σ = σ )
1 2 3 1 2 3
(III) 3重解を持つ場合(σ = σ = σ )
1 2 3
に場合分けして考える。(I)及び (II)の場合は、 k=± a 若しくは k=± a とすると、(
1 3
3— 9)式力 算出される が単位ベクトルであることに矛盾する。したがって、いず れの場合でも k=± σ として求められることになる。また、 k=— σ の場合は 2台の力
2 2
メラ(基準カメラ 12及び参照カメラ 14)が平面を挟むような配置となるため除外され、 k
は σ として求められることになる。なお、(III)の 3重解の場合は、 2台の基準カメラ 12
2
及び参照カメラ 14の光学中心が一致し、回転のみの場合であるため、除外する。
[0082] (I)及び (II)の場合、(1 17)式より、 は、
[0083] [数 19]
[0084] σ ≠ σ ≠ σ のとき、 η ' =0である力ら、
1 2 3 2
[0085] [数 20]
を得る。すなわち、 R' は、 eの回りの回転行列である。したがって、次の(1— 21)、 (
2
1— 22)、(1— 23)式を得る。
[0086] [数 21]
[0088] [数 23]
σ1 "σ3 ο •(1-23)
d σ2
(Π)の場合、 σ = σ (又は、 σ = σ )として、(1— 21)式から、
[0089] [数 24]
Ι1ι = Ά — 0, ΙΙ3 = 土丄 (Hi = ±1,11 = ιι^ = 0)
■■•(1-24)
となり、
[0090] [数 25]
[0091] [数 26]
—― ― n ■■ ( 1"26 )
d σ2 となる。
[0092] ここで、(1 19)式における ε 、 ε の符号の違いにより、 4つの解の候補が存在す
る。この 4つの候補のうち、「平面上の点が画像に映る」条件を満たす を選択する ことで 2つの候補に絞り込む。さらに、基準カメラ 12及び参照カメラ 14の位置関係が 撮影中に変化 d 1 tしない条件を加え、解を一意に決定する。
[0093] 以上のようにして一 d t、(1 21)式で求めた!^ を(1 14)式に代入することにより、回 転行列 Rが求められる。また、(1 23)又は(1— 26)式で求めた を(1— 14)式に 代入して tZdを求め、 tZdを正規化し、並進ベクトル tの単位ベクトル tを、
e
[0094] [数 27]
■( 1-27 )
として求める。並進ベクトル tの長さ I t I (基準カメラ 12及び参照カメラ 14間の距離) は、設計値や実測値から予め求めることができるため、(1— 27)式の単位ベクトル t
e に I t Iを掛けることで、並進ベクトル tが求められる。このようにして算出された回転 行列 R及び並進ベクトル tは、パラメータ記憶部 26に記憶される。
[0095] 次いで、以上のようにして算出された回転行列 R及び並進ベクトル tをパラメータ記 憶部 26から読み出し、任意の時刻において、動的な射影変換行列 Hを算出する。
[0096] (1 9)式を書き替えると、射影変換行列 Hは、
[0097] [数 28]
— t
Η = ΑΓΙΙΑ^ + ΑΓΙ— Ab1 … )
と展開することができる。なお、射影変換行列 Hは、構成するパラメータ力も算出され るため、定数倍の不定性を示す係数 kを 1とすることができる。この場合、回転行列 R 及び並進ベクトル tが既に求められているため、射影変換行列 Hの第 1項は定数であ り、第 2項は nZdの変数となる。したがって、第 2項を推定することで、射影変換行列
Hを求めることができる。
[0098] そこで、 nZdを微少量変化させたときの射影変換行列 Hの微小変化行列を Dとす ると、
[0099] [数 29]
D = Art5n"tA^1 … ) 但し δη" = δ
である。なお、 n〃 =nZdとし、その微小変化量を δ η〃 とする。
[0100] 微小変化行列 Dを nZdの微小変化量 δ η〃 に書き替え、(1 5)式に対応する評 価関数 Ε(δη〃)をテーラ展開して一次近似すると、
[0101] [数 30]
但し ^ δ
η", J^ =J^(d
xi) = ^
don don
(「^」は、(1— 5)式の一次近似の範囲で等しいものとする。)となる。
[0102] (1 30)式の評価関数 Είδη" )を最小化する微小変化量 δη〃 は、評価関数 Ε( δη )を微小変化量 δη で微分し、その微分値を 0とおいて、
[0103] [数 31]
Adn" = - b '-'(ι-3ΐ) 但し = ^ ' ^ ^"
b =∑ei Jdxi9i
を解くことにより求めることができる。
[0104] この場合、 g (輝度勾配)、 e (輝度差分)は、基準画像 I及び射影変換された参照 i i b
画像 I力ら算出され、ヤコビアン J は、座標 x (x, y)力ら算出され、ヤコビアン J δ " r dXi n は、基準カメラ 12及び参照カメラ 14の内部パラメータ A、 A、並進ベクトル t力も算出 b r
できる。したがって、微小変化量 δ η〃 は、最小二乗法を用いて、(1 31)式から求 めることができる。
[0105] 以上のようにして求めた微小変化量 δ η〃力 、(1— 29)式を用いて射影変換行 列 Ηの微小変化行列 Dを求め、射影変換行列 Ηを次の(1 32)式にしたがって更新 する。
[0106] [数 32]
H H + D " - ( 1-32 ) また、微小変化量 δ η〃を前回の演算で求めた n〃 (=n/d)に加算し、次の(1 33)式にしたがって更新する。
[0107] [数 33] η" η" + δ η" - - - ( i-33 )
そして、(1— 32)式で求めた射影変換行列 Hと、(1— 33)で求めた n" とを用いて 、再び(1 30)式の評価関数 E ( δ n〃)の値を算出する処理を繰り返し、評価関数 Ε ( δ )の値が所定の値以下に収束したときの射影変換行列 Ηを所望の射影変換 行列 Ηとする。
[0108] このように、予め求めた回転行列 R及び並進ベクトル tを用いることにより、射影変換 行列 Hを構成するパラメータの自由度は、 nZdを構成する 3つのパラメータだけとな るため、射影変換行列 Hを迅速且つ高精度に推定することができる。
[0109] <ステップ S2>
平面領域抽出部 32において、ステップ S1で推定した射影変換行列 Hを用いて、車
両 10が走行可能な道路平面領域を抽出する。この抽出処理について、図 5を参照し て説明する。
[0110] ステップ SOで LOGフィルタ処理及びヒストグラム平坦ィ匕処理が施された参照画像 I に対して、ステップ S1で推定した射影変換行列 Hが作用され、射影変換画像 I ' が 生成される。
次!、で、ステップ SOで LOGフィルタ処理及びヒストグラム平坦ィ匕処理が施された基 準画像 I
bと、生成された射影変換画像 I ' との
r 差分画像 (1 ' I )
r bを求める。
[0111] この場合、平面上に存在する点は、基準画像 Iに正確に投影されるため、差分画
b
像 (I ' I )における輝度値は小さくなる。一方、平面上にない点の差分画像 (I ' r b r I )における輝度値は大きくなる。したがって、所定の閾値を設定して差分画像 (I b r I )を 2値ィ匕することにより、道路平面領域 Πを抽出することができる。
b f
[0112] <ステップ S3 >
平面パラメータ算出部 30において、ステップ SIで推定した射影変換行列 Hを用い て、平面 Πの平面パラメータを算出する。この場合、平面パラメータである法線べタト ル n及び距離 dは、射影変換行列 Hが収束した際に更新されて ヽる(1— 33)式の n "から、
[0113] [数 34]
n
n •( 3-1 )
n
として直ちに求めることができる。
[0115] このように、予め求めた回転行列 R及び並進ベクトル tを用いることにより、射影変換
行列 Hを迅速且つ高精度に推定できるとともに、法線ベクトル n及び距離 dを同時に 算出することができる。
[0116] <ステップ S4>
ステップ S4は物体を検出する処理であり、図 6に示すステップ S41〜ステップ S46 のサブルーチン処理からなる。
[0117] ステップ S41において、基準カメラ 12及び参照カメラ 14によって、立体領域をステ レオ計測し、物体の各エッジ点のワールド座標 Cwを求める。
[0118] ステップ S42において、ワールド座標 Cwの高さ方向の座標 Yw (図 7参照)に基づ き、平面からの高さにより物体の候補点を抽出する。具体的には、図 7に示すように、 平面からの高さが Tmin (例えば、 20cm)以上であって Tmax (例えば、 2. 5m)未満 の空間に存在する候補点 (Tmin≤Ywく Tmax)を抽出する。これにより、道路上に 設けられた歩道橋、トンネルの入口部、看板等の少なくとも一部が除外され、他の車 両ゃガードレール等の車両 10の走行に関わり合いのあり得る部分のみが抽出され やすくなる。これにより、図 7の候補点 Paは抽出され、候補点 Pbは除外される。
[0119] また、車幅方向である Xw方向及び前後方向である Zw方向についても、所定範囲 内に制限して候補点の抽出を行うようにしてもよい。これにより、車両 10が通過する 可能性のな!ヽ箇所に対して無駄な投票処理を行うことが防止され、計算量を低減す ることができる。この場合、例えば、図 8に示すように X方向については一 10〜: LOm、 z方向につ!ヽては 0〜40mの範囲に制限するとよ!/、。
[0120] ステップ S43において、物体の各候補点をその Xw、 Zw座標に応じて図 8に示す投 票面 50上に設けた一定サイズの桝目(セル)単位に投射する。投票面 50は、ワール ド座標系の直交する 2軸である座標 Xwと座標 Zw (図 16参照)の座標面として表され る。このとき、同物体のセルに対して複数の候補点が重複して投射される場合には、 その重複数を記録する。このように重複数を考慮しながら候補点を投射する処理を 投票と呼び、重複数を投票数と呼ぶ。投票の処理では、前記ステップ S42において Tmin未満の点及び Tmax以上の点が予め除外されているため、歩道橋、トンネルの 入口部、看板等に対応する点の投票数は 0又は十分に小さい値となる。なお、図 8及 び図 9に示す投票面 50は、説明上の模式的なものであって、他の図 5等とは関連付
けられていない。
[0121] ステップ S44において、投票面 50上に投票された候補点を隣接する群毎にグルー プ分けするクラスタリング処理を行う。クラスタリング処理によって分けられた群をクラ スタと呼ぶ。クラスタリング処理としては、最短距離法(Nearest Neighbor Metho d)を挙げることができ、最短距離法で用いる特徴量としては幅 (座標 Xw)及び奥行き (座標 Zw)を用いるとよい。このようなクラスタリング処理によれば、検出物体のモデル が不要であって、種々の形状の物体を検出することができる。
[0122] このステップ S44のクラスタリング処理によって、例えば、図 9に示すように、投票面 50上で 3つのクラスタ 52a、 52b及び 52c力得られる。
[0123] なお、ステレオ計測における対応探索のミス (マッチングミス)によるノイズを除去す ることを目的として、クラスタリング処理を行った後にクラスタに対して膨張収縮処理を 行うようにしてもよい。物体をステレオ計測する際にマッチングミスがあると、計測され る 3次元位置には誤差が生じるため、計測された物体形状には凹凸(ノイズ)が生じ、 このノイズによりクラスタ形状が異なることがある。膨張収縮処理では、膨張により凹凸 部をつなげ、収縮により元の大きさに戻すため、ノイズの影響を抑えた形状にすること ができる。
[0124] ステップ S45において、クラスタリング処理によって分けられた各クラスタ 52a、 52b 、 52cが物体であるか又はノイズであるかを判断し、ノイズによるクラスタを除去する。
[0125] 具体的には、先ず、得られた各クラスタ 52a、 52b及び 52cに対してそれぞれ合計 投票数を求める。つまり、クラスタ 52aの合計投票数は 38であり、クラスタ 52bの合計 投票数は 21であり、クラスタ 52cの合計投票数は 3である。また、各クラスタ 52a、 52b 及び 52cにおける面積上の重心 gの奥行きである座標 Zwの値 Zgを基準とした閾値 T hzと各合計投票数とを比較し、合計投票数が閾値 Thz以下であるクラスタはノイズで あるとして除去する。閾値 Thzは以下の(4—1)式で表される。
[0126] [数 36]
Thz = Kgx a/Zg)2 - -( 4-i )
ここで、 Kgは係数であって、例えば、 lm先に物体が存在するとした場合の最小の 合計投票数である。 (4 1)式は、画像中に映し出される物体の面積が距離の二乗 に反比例して小さくなると 、う現象に基づ 、て設定されて 、る。
[0127] このような、ステップ S45の処理によって、例えば、合計投票数の少ないクラスタ 52 cはノイズとして除去される。また、歩道橋、トンネルの入口部、看板等の合計投票数 力 S小さ 、物体もノイズとして除去される。
[0128] ノイズが除去された投票面は、図 5の例に則してより実際に近いデータとして示すと 、図 10に示すようになり、この投票面 50が所定の記憶部に記録される。図 10から明 らかなように、この投票面 50には、比較的近い前方で車線変更を行っているトラック に対応したクラスタ 54aと、該トラックより前方を走る乗用車に対応したクラスタ 54bと、 ガードレールに対応したクラスタ 54cが抽出されている。また、投票面 50には、設計 者の確認用に座標 Xw、座標 Zw及び自車である車両 10が模式的に示されて ヽる。
[0129] ステップ S46において、残ったクラスタ 54a〜54cを物体として認識し、対応する物 体位置領域を切り出して、所定の記憶部に記憶する。
[0130] ステップ S4 (図 3参照)の物体抽出処理はこのようなステップ S41〜S46により構成 されている力 各ステップについて以下さらに詳細に説明する。
[0131] ステップ S41は、図 11に示すステップ S411〜ステップ S417のサブルーチン処理 からなる。
[0132] ステップ S411にお 、て、基準画像 Iカゝらエッジ及び特徴点を抽出するソ一べルフィ
b
ルタ処理を行う。このソーベルフィルタ処理とは、画像輪郭抽出のためのフィルタ処 理であって、画素毎にその近傍の画素に所定の係数をそれぞれ乗算し、結果を合計 する処理であって、垂直方向、水平方向の二つの係数行列を用いてこの処理を行う
[0133] すなわち、自動車のように、表面が鏡面反射している物体は、見る角度によって反 射光が変化するため、同じ点を左右の画像で観測しても輝度が異なってしまう。その ため、左右の画像間での領域ベースマッチングがうまく行かず、ステレオ距離計測が 不正確となることがある。そこで、テクスチャレスな部分や鏡面反射している部分はス テレオマッチングをせずに、エッジ部分のみステレオマッチングを行い、距離の計測
をする。そのため、先ず入力画像にソーベルフィルタ処理を行い、エッジや特徴点を 抽出し、この後 2値ィ匕し、図 12に示すエッジ画像 60を作成する。図 12から明らかなよ うに、この時点のエッジ画像 60には、路面の白線等の物体検出の処理には無関係な 部分も抽出されている。
[0134] なお、図 12では、説明の便宜上エッジ画像 60を模式的に示しており、実際には、 図 13で示すような画像データが得られる。
[0135] ステップ S412において、白線等の道路平面にあるエッジは検出対象ではないため 、エッジ画像 60から道路平面領域抽出処理 (ステップ S 2)で得られた平面領域 Π ( f 図 5参照)の部分を除去する処理を行う。具体的には、エッジ画像 60と平面領域 Πと f の二値的な排他処理を行い図 14に示す修正エッジ画像 62を得る。この際、エッジ画 像 60における上部の所定幅の領域や下部のボンネットの領域等は検出対象がない ことが明らかであって、し力もこれらの領域はエッジ画像 60上の場所が固定されてい ることから、これらの部分に対しても所定の領域指定手段により除去する。
[0136] なお、エッジ画像 60から平面部分が完全に取り除けなくとも、後述するように車両 1 0の通過する空間に限定した処理を行うので、検出対象として誤検出されることはな い。
[0137] ステップ S413において、修正エッジ画像 62上に残った各エッジ点に対応する参照 画像 I上の探索範囲としてのェピポーラライン EPを設定する(図 15参照)。ェピポー ラライン EPは、修正エッジ画像 62上におけるエッジ点の参照画像 I上に対応する点 を含む直線として、参照カメラ 14の内部パラメータ、基準カメラ 12の内部パラメータ、 参照カメラ 14と基準カメラ 12との回転行列 R及び並進ベクトル tに基づいて算出され るが、詳しくは後述する。
[0138] ステップ S414にお 、て、道路平面領域抽出処理 (ステップ S2)で得られた平面領 域 Πに射影変換行列の逆行列をかけて参照画像 I上における参照平面領域 Πを f r fr 求めて、参照画像 I上に投影する。
[0139] ステップ S415において、参照画像 I上におけるェピポーラライン EP力も参照平面 領域 Π の部分を除去する処理を行い、ェピポーラライン EPは非平面領域に限定さ fr
れる。
[0140] ステップ S416において、エッジ点に対応する基準画像 I上の小領域に対して、参
b
照画像 Iのェピポーラライン EP上で領域ベースマッチング処理を行う。この領域べ一 スマッチング処理とは、基準画像 Iにおけるエッジ点を中心とした小さいマッチングゥ
b
インドウを作成し、該マッチングウィンドウと参照画像 Iにおけるェピポーラライン EP上 の小ウィンドウとの類似度の計算を順次行う処理である。領域ベースマッチング処理 によって最も類似度の高い点がエッジ点の対応点(ウィンドウ)として求められる。
[0141] この際、先ず、基準画像 Iから参照画像 Iへ領域ベースマッチング処理を行 、、計
b r
算された類似度を閾値と比較し、マッチングが成功したと判断されたとき、今度は参 照画像 Iから基準画像 Iへ領域ベースマッチングを行い、先の基準画像 Iの点に r b b 一 致した場合に、マッチングが成立すると判断する。このような領域ベースマッチングを 行うことで、繰り返しパターンによるマッチングミスを抑えることができる。ここで用いる 閾値としては、類似度の最大値に対する第 1閾値と、類似度の最大値から類似度の 2 番目の最大値との差に基づく第 2閾値を設けて判断してもよい。
[0142] 類似度の計算には、例えば、正規ィ匕相互相関関数 (ZNCC)を類似度関数をして 用いることができる。正規ィ匕相互相関関数ではパターンの類似度を計算しているため 、基準画像 Iと参照画像 Iとに全体的な輝度のオフセットずれがある場合であっても
b r
マッチングのミスが発生しにくい。正規ィ匕相互相関関数は、次の (4— 2)式により求め られる。
[0143] [数 37]
' " ( 4-2 )
[0144] ここで、 I 。ま
b、 Iはそれぞれ基準画像と参照画像を示し、 Rはウィンドウ領域を示す
r
た xはウィンドウ内のピクセルの位置を示す。また、 u及び Vはそれぞれ基準画像、参 照画像のウィンドウの基準位置を示す。
[0145] また、基準画像 I及び参照画像 Iの種類や必要な計算速度等に基づ!、て他の類似
b r
度関数 (SSD (2乗残差和)、 NSSD及び SAD (絶対値和)等)を用いてもよ!、。
[0146] このステップ S416における領域ベースマッチング処理は、基準画像 Iに関しては
b
平面領域 Πが除かれた部分のエッジ点のみが処理対象となり、一方、参照画像 Iに
f r 関してはェピポーラライン EP上のうち参照平面領域 Πが除かれた部分のみが処理 対象となる。したがって、計算負荷が大幅に削減されるとともに、平面領域の点を検 出してしまうことがない。また、計算負荷が低減されることから、計算のためのハードウ エアに関して低廉ィ匕を図ることができる。
[0147] ステップ S417において、領域ベースマッチングの成立した基準画像 I上の点(ウイ
b
ンドウ)と参照画像 I上の点(ウィンド)に基づいて、三角測量の原理に基づくステレオ 計測により対応する点のカメラ座標 Ccを計算する。
[0148] ステップ 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)式で表される。
[0149] [数 38]
Cw = w · Cc + lw "'( 4'3 )
[0150] ここで、法線ベクトル nを (4—4)式で表したとき、回転行列 は、(4— 5)式で表さ れる。
[0151] [数 39]
[o刚 [seio]
( り… rzaijiucXu) = u
83
L8L lO/SOOZdr/13d 1^99910/900^ OAV
J TTL ώ
O O ) ( } mlヽe?。JE Π—
O
o
C 2
お、説明を簡単にするために、図 11のフローチャートでは、ステップ S413〜ステ
ップ S418の処理を直列的に示した力 実際上、修正エッジ画像 62に表される各エツ ジ点毎にステップ S413〜ステップ S418を実行し、対応するワールド座標 Cwを求め る。
[0154] このようにして、修正エッジ画像 62上の各エッジ点は奥行きである座標軸 Zwに基 づいて、例えば、図 17に示すような奥行き画像 64として表すことができる。この奥行 き画像 64では、座標軸 Zwの値 (又はワールド座標系の原点力ゝらの距離)に応じた大 きさの点が示されている。各点は、座標軸 Zwの値に応じて色の変化するグラデーシ ヨン状の点で表してもょ ヽ。奥行き画像 64は設計者の動作確認のために供されるも のであり実際上は設けなくてもよいが、該奥行き画像 64を生成するために必要なヮ 一ルド座標 Cwは所定の記憶部に記録しておく。
[0155] 上述したように、ステップ S41の処理 (ステップ S411〜S418)によれば、基準画像 I 及び参照画像 Iに基づいて、物体のエッジ点毎にワールド座標 Cwを求めることがで b r
きる。この際、平面領域 Πを除外した部分について処理を行うので、計算負荷の低
f
減が図られる。このうち、ステップ S413〜S416は参照画像対応部探索手段というこ とがでさる。
[0156] <ステップ S5 >
ステップ S5は物体の相対速度を算出する処理であり、図 18に示すステップ S51〜 ステップ S55のサブルーチン処理からなる。
[0157] ステップ S51にお 、て、連続する時刻の基準画像 I力も物体領域の重なり割合 Rat
b
eを算出する。
[0158] ステップ S52において、連続する時刻における物体の重心移動距離 Lgを算出する
[0159] ステップ S53にお 、て、重なり割合 Rate及び重心移動距離 Lgに基づ 、て閾値判 定による同一物体の判定を行う。
[0160] ステップ S54において、物体の位置を高精度に求めるため再計算して位置ベクトル
Pを求める。
t
[0161] ステップ S55において、位置ベクトル Pに基づいて、物体の相対速度ベクトル V
t obst-r を算出する。
[0162] ステップ S5 (図 3参照)はこのようなステップ S51〜S55により構成されている力 各 ステップについて以下さらに詳細に説明する。なお、ステップ S51〜S55についての 説明では、検出された各物体のうち、基準画像 Iにおけるトラック 70 (図 5参照)に対
b
する処理を例示的に説明する力 他の検出物体である乗用車 72やガードレール 74 についても同様の処理が行われる。ステップ S 51は、図 19に示すステップ S 511〜ス テツプ S517のサブルーチン処理からなる。
[0163] ステップ S511において、前時刻のステップ S517で記憶されたテンプレート 80aを 所定の記憶領域から呼び出す。ここで、テンプレート 80aとは投票面 50 (図 20参照) におけるクラスタ 54aに対応したトラック 70の基準画像 Iにおける像を領域指定して
b
記録したものであり、例えば、平面から 1. 5mの高さの領域として記録されている。
[0164] 概念的には、図 20に示すように、点 POは、投票面 50上の点であり、光学中心 Oか ら一定角度で走査し、クラスタ 54aに当たる点とする。点 P0に基づいてワールド座標 Cwが計算できる。点 Prを点 P0と同じ幅位置と奥行位置であり、且つ道路平面 Π上 の点とする。点 Prは、道路平面 Π上の点であることから高さが 0である。点 Pxは、点 P 0と同じ幅位置と奥行位置であり、且つ道路平面 Πから 1. 5mの高さをもつ点とする。 次に点 Pr、 Pxを用いてカメラ座標 Ccに変換する。カメラ座標 Ccに変換後、透視投影 行列を用いて基準画像 I上の座標に変換すればよい。このようにして前時刻のクラス
b
タ 54aに対応したテンプレート 80aが求められて、記録されている。同様に、乗用車 7 2に対応した前時刻のクラスタ 54b及びガードレール 74に対応した前時刻のクラスタ 54cに対しても図 21に示すテンプレート 80b及びテンプレート 80cが記録されている
[0165] また、前記ステップ S418で各物体の高さが求められていることから、各テンプレート 80a、 80b及び 80cの高さは求められた物体の高さに応じて設定してもよ!/、。
[0166] ステップ S512において、現時刻のメインルーチン処理におけるステップ SOで取得 した基準画像 Iを読み出す。なお、ここでは LOGフィルタやヒストグラム平坦ィ匕などの
b
処理を施す前の原画像を用いるものとする。
[0167] ステップ S513において、各テンプレート 80aを用いて、現時刻に取得した基準画 像 I上でテクスチャに基づくテンプレートマッチングを行い、類似度の最も高い位置
に各テンプレート 80aを移動させる。テンプレートマッチングの具体的手法としては、 前記の ZNCC (4— 2)式や、 SSD、 SAD等を用いたマッチングを挙げることができる
[0168] ステップ S514にお!/、て、前記ステップ S46で記録したデータに基づ!/、て、現時刻 に求められたクラスタ 54aを呼び出す。
[0169] ステップ S515において、呼び出したクラスタ 54aに対してそれぞれ対応する物体位 置領域 82aを求める。この物体位置領域 82aは前記ステップ S511で説明した手順 によって求められる。物体位置領域 82aは、平面的なクラスタ 54aに基づいて求める 方法以外にも、図 17に示されるような奥行き画像 64における各エッジ点の 3次元的 な空間位置に基づ 、て求める方法を採ってもょ 、。
[0170] ステップ S516において、前記ステップ S513で移動したテンプレート 80aと、前記ス テツプ S515で求められた物体位置領域 82aとの重なり割合 Rateを次の(5— 1)式で 算出する。
[0171] [数 41]
Rate = (Rt„1 H Rt)/ min(R t—, R t ) …( 5- 1 )
[0172] ここで、 R は、移動したテンプレート 80aの領域であり、 Rは、対応する物体位置領
t-i t
域 82aである。また、 Πは領域の重なり部分を示す演算子であり、 min(R 、 R )は R
t-1 t t と Rの面積の小さ 、ずれか一方を選択する関数である。
- 1 t
[0173] (5— 1)式によれば、移動したテンプレート 80aと物体位置領域 82aが完全に一致 する場合には Rate= lとなり、一致する部分が全くない場合には Rate = 0となる。ま た、概念的には、図 21に示すように、基準画像 I上でテンプレートを示す R と物体
b t-1 位置領域を示す R
tに対して重複する部分 R Π Rの
t-1 t 割合を求めることになる。
[0174] 重なり割合 Rateが所定の閾値 Tha (0<Tha< 1)以上である場合には、対応する 前時刻の投票面 50におけるクラスタと現時刻の投票面 50におけるクラスタを同一物 体に基づくものであると仮判定されてトラッキングされる。トラッキングとは、異なった時 刻に得られたクラスタの対応付けを行う処理であり、これにより物体の動作が検出可 能となる。
[0175] ステップ S517において、前記ステップ S515で求められた物体位置領域 82aを新 たなテンプレート 80aとして所定の記憶部に記録する。この新たなテンプレート 80aは 、前記のとおり次回処理時のステップ S511で呼び出される。
[0176] このように、ステップ S51 (ステップ S511〜S517)においては、テンプレート 80aに 基づいてテンプレートマッチングを行うとともに、投票面 50のクラスタ 54aから物体位 置領域 82aを求めるため、 2重のトラッキング処理を行っており、これらの結果の重な り割合 Rateを求めてトラッキング処理の信頼性を高めている。実際には、このステツ プ S51で求めたトラッキングの関連づけはステップ S53で最終的に確認される。
[0177] 次に、ステップ S52の処理内容について説明する。ステップ S52は、図 22に示すス テツプ S521〜ステップ S524のサブルーチン処理からなる。
[0178] ステップ S521にお!/、て、前記ステップ S46で記録したデータに基づ!/、て、現時刻 に求められたクラスタ 54aを呼び出す。
[0179] ステップ S522において、クラスタ 54aの現時刻の重心 gを求めて、図 10に示すよう
T
に投票面 50に記録する。
[0180] ステップ S523において、前時刻にステップ S533を実行したときに記録したクラスタ
54aの前時刻の重心 gを読み出す。
o
[0181] ステップ S524において、図 10に示すように、クラスタ 54aの重心 gと重心 gとの重
O T
心移動距離 Lgを算出する。
[0182] このように、ステップ S52 (ステップ S521〜S524)においては、クラスタ 54aの前時 刻の重心 gと現時刻の重心 gとの重心移動距離 Lgを求めており、短時間の移動距
O T
離として合理的なもののクラスタ同士を対応づけることができる。実際には、このステツ プ S52で求めたトラッキングの関連づけは次のステップ S53で最終的に確認される。
[0183] 次に、ステップ S53の処理内容について説明する。ステップ S53は、図 23に示すス テツプ S531〜ステップ S534のサブルーチン処理からなる。
[0184] ステップ S531において、対応する物体毎に、前記ステップ S516で求めた重なり割 合 Rateと閾値 Thaとを比較し、 Rate≥Thaであればステップ S 532へ移り、 Rate<T haであればステップ S534へ移る。
[0185] ステップ S532において、対応する物体毎に、前記ステップ S524で求めた重心移
動距離 Lgと閾値 Thbとを比較し、 Lg≤Thbであればステップ S533へ移り、 Lg>Th bであればステップ S534へ移る。この閾値 Thbは、自車の走行速度と対向車の走行 速度を考慮して、処理上の演算周期に物体が移動可能な合理的な距離として固定 値で設定されている。
[0186] ステップ S533においては、対応付けられた 2つの物体は同一物体であるとして確 定認識され、記録される。また、重心 gを g として入れ替えて所定の記憶部に記
T
憶する。
[0187] 一方、ステップ S534においては、対応付けられた 2つの物体は別物体であったも のとして認識される。この場合、対応付けられなかった物体については、画像の前後 関係に基づいて撮像範囲内に入ってきた新たな物体として認識する登録処理を行 い、又は撮像範囲力 抜け出た物体として所定の削除処理を行う。
[0188] このように、ステップ S53 (ステップ S531〜S534)においては、前記ステップ S52ま での処理で対応付けられた 2つの物体につ ヽて、重なり割合 Rateと重心移動距離 L gに基づいて、同一物体力否かの確認を一層確実に行うことができる。
[0189] なお、このステップ S531〜S534は、予め仮に対応付けられた 2つの物体に対して 行うものとして説明したが、前時刻検出の各物体と、現時刻検出の各物体に対して総 当たり的に行って、対応付けを行ってもよい。
[0190] また、ステップ S532で用いられる閾値 Thbは固定値であるものとして説明したが、 車両 10から重心 gまでの座標 Zw方向の距離 Zgに基づ 、て変化する関数としての o
閾値 Th (Zg)を閾値判定に用いてもよい。この場合、ある物体の重心 gまでの座標 Z
O
w方向の距離力 ¾glであったとすると、閾値は Th (Zgl)として表される。また同様に
、閾値を自車速 I V I に基いて変化する関数としての閾値 Th( I V
V V I )を閾値判定 に用いてもよい。
[0191] 次に、ステップ S54及びステップ S55の処理内容について説明する。ステップ S54 は、図 24に示すステップ S541〜ステップ S544からなり、ステップ S55は、図 26に示 すステップ S551〜S553力らなる。
[0192] ステップ S541にお!/、て、基準画像 Iにおける物体の注目点 Pcを特定する。この注 b
目点 Pcは、ステップ S513のテンプレートマッチングにより移動したテンプレート 80a
の領域内に設定し、例えば、テンプレート 80aの中心点や、物体の角部などの特徴 的な点として特定すると、常に物体上の略同一点に特定される。
[0193] ステップ S542にお!/、て、注目点 Pcに対応させたェピポーラライン EPを参照画像 I 上に設定する。このとき、クラスタ 54aを利用することで、トラック 70までの概略距離を 算出できることから、これに基づいてェピポーラライン EPを必要十分な程度に短く設 定できる。
[0194] クラスタを利用した注目点 Pcの概略距離の算出方法を説明する。先ず、クラスタ 54 aの重心 g (図 10参照)より Zw座標を求める。これを注目点 Pcの Zw座標に仮設定し
T
、注目点 Pcの画像座標(u , V )ととも〖こ、次の(5— 2)式に代入して対応する Xw座 pc pc
標を算出する。 ( λは定数倍の自由度を表す)
[0195] [数 42]
•( 5-2 )
[0196] こうして求めた投票面 50上の位置 (Xw, Zw)と基準カメラの位置(twx, twz)を通 る直線を設定し、その直線に沿ってクラスタ 54aの点を探索して、車両 10に最も近い 点の Zw座標を注目点 Pcの概略距離とすればょ 、。
[0197] ステップ S543において、前記ステップ S416と同様に注目点 Pcに対応する基準画 像 I上の点に対して、参照画像 Iのェピポーラライン EP上で領域ベースマッチング処 b r
理を行う。
[0198] ステップ S544において、前記ステップ S417と同様に、マッチングの成立した基準 画像 I上の点と参照画像 I上の点に基づいて、ステレオ計測によって対応する点の力 b r
メラ座標 Ccを計算する。さらに、前記ステップ S418と同様に、求められたカメラ座標 Ccをワールド座標 Cwに変換する。求められた物体の位置であるワールド座標 Cwは 、図 25に示すように、投票面 50上において 2次元的な位置ベクトル Pとして表す。
t
[0199] 次に、図 26のステップ S551において、物体の前時刻に検出した位置ベクトル P
を読み出す。この位置ベクトル P は、前時刻処理時のステップ S553で所定の記憶 t-1
部に記録されている。
[0200] ステップ S552において、次の(5— 3)式により物体の相対速度ベクトル (移動べタト ル) V を算出する。
obst-rel
[0201] [数 43]
Vobst rel = (Pt - Pt-l) At …( )
[0202] ここで、 A tは処理の演算周期であり、単位時間あたりの位置ベクトル Pの変化とし t て相対速度ベクトル V が求められる。
obst-rel
[0203] ステップ S553において、次回の処理に供するために、位置ベクトル Pを P Pと t t-1 t 代入して記憶部に記録する。ここで記録する位置ベクトル Pは、ステップ S517にお t
V、て新たなテンプレート 80aとして記憶した物体位置領域 82aに対して、ステップ S5 41〜S544と同様の処理を行い更新したものである。
[0204] このようなステップ S54及び S55の処理によれば、ェピポーラライン EPを十分に短 く設定していることから、ステップ S543における領域ベースマッチングでマッチングミ スの発生する頻度が十分に低くなり物体の距離を一層正確に算出することができる。 また、注目点 Pcは常に物体上の略同一点に設定されることから、幅方向(座標 Xw方 向)の移動を含めて物体の経時的な相対移動を示す相対速度ベクトル V を正確 obst-rel に算出することができる。
[0205] この結果、トラック 70、乗用車 72及びガードレール 74の相対速度ベクトル V で obst-rel あるベクトル Vrl、 Vr2及び Vr3力 図 25に示すようにベクトル表示される。図 25から 明らかなように、ベクトル Vrlからはトラック 70が左方向に向力つて進路変更している ことが判断され、ベクトル Vr2からは乗用車 72が前方に向力つて車両 10よりもやや速 い速度で進行していることが判断される。また、ベクトル Vr3からは静止物であるガー ドレール 74が車両 10の進行方向と逆方向に相対的に移動していることが判断される 。なお、図 25における自車速度ベクトル Vはステップ S6及び S7において求められる
[0206] 次に、ェピポーララインの設定方法について説明する。ステレオ 3次元計測をする ためには基準画像 Iに映る観測点 Mlの位置と参照画像 Iに映る観測点 Mlの位置
b r
をそれぞれ求める必要がある。図 27に示すように、観測点 Mlは基準画像 I上の点
b mに投影されており、点 mは観測点 Mlと基準カメラ 12の光学中心 Oを通る直線 Le b b
上に存在する。直線 Le上の点を探せば、観測点 Mlを見つけることができ、参照画 像 I上に投影される直線 Leがェピポーラライン EPとなる。つまり、基準画像 I上に投 r b 影された観測点 Mlを参照画像 Iと対応をつけるには、参照画像 I中でのェピポーラ ライン EP上を探索すればよいこととなる。なお、図 27における点 MO及び M2は、同 一の点 mに投影される他の点であって直線 Le上の点である。
b
[0207] このェピポーラ拘束の関係式を求める。参照カメラ座標 Xrと基準カメラ座標 Xbの間 には、前記の回転行列 R及び並進ベクトル tを用いて、 Xr=R'Xb+tの関係が成り 立つ。
[0208] このとき、基準カメラ 12の正規ィ匕画像座標 X"ba、参照カメラ 14の正規ィ匕画像座標 X"ra及び並進ベクトル tは同一平面上に存在する。すなわち、スカラー三重積が 0に なるので、
[0209] [数 44]
Xra -(t x -Xba) =
- Xba … "
[0210] となる。
[0211] ただし、
[0212] [数 45]
と表すことにする。ただし、 tl、 t2、 t3は並進ベクトル tの要素を示す。ここで、正規ィ匕 画像座標と画像座標との関係より、 πΓ =A -X'ba. m" =A X"raを代入して整理
b b r r
すると、基礎行列(Fundamental Matrix) Fは次の(5— 6)式及び(5— 7)式で表 される。
[0213] [数 46]
F = Ar"1[t]x Ab"1 … 6 )
[0214] [数 47]
[0215] (5— 7)式により、空間の観測点 Mlが基準画像 Iに投影される位置を mとしたとき、 b
l' =FnTとすると、 m'T = 0より、参照画像 I中では、
[0217] となり、参照画像 I中における観測点 Mlに対応したェピポーラライン ΕΡとなる。ここ で、「 は参照画像であることを示し、 Γ, Γ, 1 'は参照画像のェピポーラライン EPの
1 2 3
パラメータを示す。また、 U'、 Vは参照画像の画像座標を示す。
[0218] また、観測点 Mlの概略位置が既知である場合には、直線 Leの長さを制限すること によりェピポーラライン EPを短く設定することができる。
[0219] 参照画像 Iに投影される位置を m'としたとき、 1=^πι'とすると、 1 =0より、参照画 像 I中では、
[0220] [数 49]
となり、観測点 Mlにおける基準画像 Iにおけるェピポーラライン EPとなる。なお、こ b
のェピポーラライン EPの説明で用いたパラメータ u及び u'は、後述するベクトル u及
び u'とは異なるものである。
[0221] くステップ S6 >
このステップ S6は、自車である車両 10の自車速度ベクトル V (図 25参照)を求める
V
ために、道路平面 Πを仮想的に上方から見た VPP画像を生成するための処理であり 、図 28に示すステップ S61〜ステップ S63のサブルーチン処理からなる。すなわち、 ステップ S61において、基準画像 Iを VPP画像に変換 (以下、 VPP変換とも呼ぶ)す b
るための変換式 H を算出する。この後、ステップ S62において、求められた変換式
VPP
H を用いて、前記基準画像 Iを VPP画像 Iに変換し、記録する(図 33参照)。さら
VPP b V
に、ステップ S63において、同じ変換式 H を用いて前記平面領域 Πを VPP変換し
VPP f
て記録する(図 34参照)。以下、各ステップ S61〜S63について詳細に説明する。
[0222] なお、 VPP画像 Iを撮像する仮想的なカメラを想定し、説明上これを仮想カメラと呼
V
[0223] 先ず、図 29のステップ S611において、基準カメラ 12の内部パラメータ Aの逆行列 b
A—1を読み込む。内部パラメータ Aは、次の(6— 1)式で表される。
b b
[0224] [数 50]
[0225] ここで、 ひ 、 j8 、 s、C 及び C は、基準カメラ 12のキャリブレーションにより定まる
1 1 1 XI Y1
定数である。
[0226] ステップ S612において、道路平面 Πに対する位置姿勢パラメータである法線べク トル n及び距離 d (図 16参照)を読み込む。
[0227] ステップ S613において、法線ベクトル n及び距離 dを用いて、基準カメラ 12の光軸 であるベクトル e (図 30参照)を法線ベクトル nに一致させる回転行列 Rを次の(6—
2)式〜(6— 5)式に基づ 、て算出する。
[0228] [数 51]
[0229] [数 52]
cos6n = JZ •(6-3)
n e oz
[0230] [数 53]
[0231] [数 54]
(
Rn =
■(6-5)
[0232] これらの(6— 2)式〜(6— 5)式を図 30も参照しながら説明すると、式中で縦二重線 で囲まれた部分はノルムを示し、ベクトル e は、カメラ座標系における光軸である座 標 Zc (図 16参照)の単位ベクトルを示す。また、 cは、法線ベクトル nとベクトル e に n oz 垂直な単位ベクトルであり、角度 0 は法線ベクトル nとベクトル e とのなす角度であ n OZ
る。なお、図 30におけるベクトル e' e' 及び e' は、仮想カメラ座標系を示す直交
OX OY OZ
単位ベクトルであり、一転鎖線は車両 10の前後方向を示す。
[0233] ステップ S614において、投影距離、スケール、画像中心を指定し、道路平面 Πを 上方から見た仮想カメラの内部パラメータ A を設定する。内部パラメータ A は、仮
VPP VPP
想カメラ座標上の画像を画像座標上に変換するための行列であり、次の(6— 6)式で 表される。
[0234] [数 55]
[0235] ここで、 a = d - k β = d - k である。また、 d は投影
VPP VPP X-VPP VPP VPP Y-VPP vpp 距離 [m]であり、 k 及び k は VPP画像 Iの X座標方向及び Y座標方向のスケー χ-νρρ y-vpp V
ル [pixel/m]である。さらに、 C 及び C は VPP画像 I中心の X座標及び Y座
- vpp -vpp V
標である。
[0236] この内部パラメータ A により投影距離に道路平面のパラメータである距離 dを指定
VPP
すると、検出した道路平面 Π上に投影される。
[0237] ステップ S615にお!/、て、車両前後方向と VPP画像の縦軸を一致させるために画 像を角度 Θ 及び角度 φだけ回転させる変換式である回転行列 Rを算出する。す
V V
なわち、図 30及び(6— 7)式に基づいて基準カメラ 12の光軸方向の単位ベクトル e
OZ
を VPPカメラ座標系カゝらみたベクトル uを設定し、該ベクトル uを仮想カメラ座標系に おける e' e' 平面に正射影したベクトル u'を (6— 8)式に基づいて設定する。
OX OY
[0238] [数 56]
U = Rne0Z = (Ux,Uy,Uz) ·'·(6-7)
[0239] [数 57]
U' = (Ux,Uy,0)t '"(6-8) ベクトル u及び u'をこのように定義するとき、角度 Θ はベクトル u'と仮想カメラ座標 e'
V
とのなす角度であり(6— 9)式で表される。
oy
[0240] [数 58] θν = arctan ■(6-9)
ノ
[0241] また、角度 φは車両前後方向に対するカメラ光軸であるベクトル e とのなす角度で
OZ
あり予め求めておく。求める回転行列 Rは、次の(6— 10)式で表される。
V
[0242] [数 59]
= Rotate(e0
z— (|))Rotate(e0
z,6
v)
ステップ S616において、 A A に基づいて、 VPP変換のための変 b、 R
n、 R及び
V VPP
換式 H を算出する。変換式 H は、次の(6— 11)式で表される。なお、 R =RR
VPP VPP VPP V
nである。
[0244] [数 60]
Hvpp = AVppRvR]iAb = AvppRVppAb
[0245] つまり、変換式 H では、基準画像 Iを行列 A— R、 R、及び A で順に変換す
VPP b b n V VPP
ることと等価な処理が行われる。実際には 1つの変換式 H で一度に変換が可能で
VPP
あるが、変換の過程を概念的な理解が容易となるように、各行列 A— R、 R、及び A
b n V
VPPによる変換を説明する。
[0246] 先ず、基準画像 I (図 5参照)に対して逆行列 A 1による変換を行うと画像座標から
b b
カメラ座標系への変換が行われ、図 31に示すカメラ座標系基準画像 I が得られる。
be
[0247] 次に、このカメラ座標系基準画像 I に対して回転行列 Rnによる変換を行うと、カメラ
be
座標系基準画像 I は角度 θ nだけ回転して道路平面 Πに平行になるとともに遠方部
be
分が拡大され、第 1変換画像 I が得られる。図 32に示すように、この第 1変換画像 I
VI VI
は縦軸に対して角度( Θ — Φ
V )だけ傾!ヽた画像として得られる。
[0248] さらに、第 1変換画像 I に対して回転行列 Rによる変換を行うと、第 1変換画像 I
VI V VI
は角度( Θ — Φ )だけ回転して車両前後方向と VPP画像の縦軸が一致することにな
V
り第 2変換画像 I が得られる。
V2
[0249] 最後に、第 2変換画像 I に対して仮想カメラの内部パラメータ A による変換を行う
V2 VPP
と、スケール変換が行われて所望の大きさの VPP画像 I (図 33参照)が得られる。
V
[0250] このような一連の行列式 A R R A— 1によって、変換式 H が構成されており、基
VPP V n b VPP
準画像 Iに対して変換式 H による VPP変換を行うことにより VPP画像 I (図 33参照 b VPP V
)が得られる。前記のとおり、この VPP変換は、ステップ S62において行われる。得ら れた VPP画像 Iは記憶部に記録される。
V
[0251] なお、この VPP画像 Iは、内部パラメータ A の設定により、図 33に示すように画
V VPP
像の左上部が原点であって、車幅方向が X方向(図 33における右方向)、車両前後 方向が Y方向(図 33における下方向)として規定される。
[0252] 図 33から了解されるように、 VPP画像 Iは、道路平面 Πを上方の高い距離から眺
V
めたような画像となり、道路の幅は略一定に映し出される。また、基準画像 I上のトラ
ック 70や乗用車 72は、画像上方部が拡大するように歪曲して映し出される。
[0253] また、この後ステップ S63においては、平面領域 Π (図 5参照)に対して変換式 H f VPP による VPP変換を行い、上方から眺めた平面領域 Πである VPP平面画像 I (図 34 f Vf 参照)を求めて記録する。この VPP平面画像 I は VPP画像 Iにおける道路平面 Πの
Vf V
領域を示すことになる。得られた VPP平面画像 I は記憶部に記憶される。この VPP
Vf
平面画像 I は、道路平面と同じ形状となっており、図 34に示す例では、前方の他のト
Vf
ラック 70ゃ路側部等の水平でない構造物や、道路平面と同一レベルでない面が除 外された略 V字形状となって 、る。
[0254] <ステップ S7>
ステップ S7は、前記ステップ S6で求めた VPP画像 I及び VPP平面画像 I に基づ
V Vf いて、車両 10の自車速度ベクトル V (図 25参照)を求めるための処理であり、図 35
V
に示すステップ S71〜ステップ S77のサブルーチン処理からなる。なお、以下の説明 においては、現時刻の処理で得られた VPP画像 I及び VPP平面画像 I を VPP画像
V Vf
I (t)及び VPP平面画像 I (t)と表し、前時刻に得られたものを VPP画像 I (t— 1)及
V Vf V び VPP平面画像 I (t— 1)と表して区別する。また、ワールド座標系では車両 10の前
Vf
後方向が座標 Zw (図 10参照)として規定されている力 前記のとおり、 VPP画像 Iで
V
は前後方向が Y軸と規定されているため、以下のステップ S7に関する説明では車両 10の前後方向に対応するパラメータには添え字「Y」又は「y」を付けて表す。
[0255] ステップ S71にお!/、て、前時刻処理時の VPP画像 I (t 1)、 VPP平面画像 I (t
V Vf
1)及び画像マッチング移動量 ΔΧ、 ΔΥ、 Δ Θ を呼び出す。画像マッチング移
0 0 yaw-0
動量 ΔΧ、 ΔΥ、 Δ 0 は、前時刻処理時のステップ S 77で記録されたパラメータ
0 0 yaw-0
であり、前時刻における VPP画像 I (t)と VPP画像 I (t—l)とのマッチングによるず
V V
れ量を表す。
[0256] ステップ S72において、 VPP画像 I (t)と VPP画像 I (t— 1)とのマッチングを行うた
V V
めの探索範囲を設定する。具体的には、画像マッチング移動量 ΔΧ、 ΔΥ、 Δ 0
0 0 yaw- を中心とした所定幅を有する範囲として ΔΧ= ΔΧ 〜ΔΧ 、及び ΔΥ= ΔΥ 〜ΔΥ , Δ θ = Δ θ 〜Δ 0 が設定される。ステップ S73において、 V
MAX yaw yaw- IN yaw— MAX
PP画像 I (t)と VPP画像 I (t—l)とを前記ステップ S72で設定した探索範囲内で相
対的に移動させて、 VPP画像マッチングを行う。探索範囲は画像
ΔΧ、 ΔΥ、 Δ 0 を基準とした範囲に設定されているため、 VPP画像
に要する時間は短時間で足りる。また、自車速度ベクトル Vは急激に変化することは ないため、類似度が最大となる箇所が画像 ΔΧ、 ΔΥ、 Δ 0 を
0 0 ya«^0 中心とした探索範囲力もずれることはない。ステップ S73における VPP画像マツチン グの詳細な処理については後述する(図 36参照)。
[0257] ステップ S74において、前記ステップ S73で求めた VPP画像マッチングの結果を参 照して、 VPP画像 I (t)と VPP画像 I (t—1)との類似度が最大となる箇所 (後述する
V V
NSSD ( AX、 ΔΥ、 Δ θ )が最小となる箇所)を検出し、その箇所における VPP画 yaw
像 I (t)と VPP画像 I (t—l)とのずれ量を移動量 move ( AX、 ΔΥ、 Δ θ )として
V V f f
求める。ここで、 ΔΧ、 ΔΥ及び Δ θ は VPP画像 I (t)と VPP画像 I (t—1)との類 f f yav^f V V
似度が最大となる箇所における ΔΧ、 ΔΥ及び Δ Θ の値である。
yaw
[0258] ステップ S75において、移動量 move ( AX、 Δ Y)に基づいて下記の(7— 1)〜(7 f f
3)式により車両 10の自車速度ベクトル Vの大きさを求める。
V
[0259] [数 61] d vpp AXf
V [m/s] •( 7-1 )
α, At
[0261] [数 63]
V, v = Λ V, + vz [m/s] ...( 7-3 )
[0262] ここで、 Vx及び Vzは、車両 10の幅方向及び車両前後方向の速度であり、 d a vpp V 及び β は前記(6— 6)式にお 、て定義される係数及び数値である。また、前記の
PP VPP
とおり A tは演算周期である。(7— 2)式及び(7— 3)式で、車両前後方向の速度を V zとして添え字「y」でなく「z」を用いて!/、るのは、他の処理に対して整合性を考慮し V PP画像の座標力もワールド座標系に戻して 、るためである。
[0263] 自車速度ベクトル Vの大きさは、周知のとおり直交する 2方向速度成分で形成され
V
る斜辺部の長さとして表されることから (7- 3)式が成り立つ。
[0264] (7— 3)式及び(7— 4)式により、自車速度ベクトル Vの大きさと角度 0 が求まる
V YAW
ことから自車速度ベクトル Vが決定され、該自車速度ベクトル Vを図 25上に表す。
V V
なお、角度 Θ は、車両 10の回転角度として用いることもできる。
YAW
[0265] ところで、車両 10の回転角度をより厳密に求めるためには、 VPP画像 I (t—l)を
V
ΔΧ及び ΔΥ方向に並進的に移動させるだけではなぐ回転を含んだ移動をさせた 上で類似度の演算を行うとよい。この場合、画像マッチング移動量 ΔΧ ΔΥを中心
0 0 とした所定幅を有する範囲として ΔΧ= ΔΧ ΔΧ ΔΥ ΔΥ ΔΥ 及
ΙΝ MAX ΙΝ MAX
び回転角度としての ω = Δ Θ Δ Θ
IN MAXが設定され、 3自由度の移動を行う。
[0266] ステップ S76において、自車速度ベクトル V、角度 Θ を記録する。また、ステツ
V YAW
プ S74で求めた ΔΧ ΔΥ Δ Θ を次回処理時の画像マッチング移動量 ΔΧ f f yaw-f 0
ΔΥ Δ 0 として用いるために、 ΔΧ ΔΧ ΔΥ ΔΥ Δ 0 θ と
0 yaw— 0 0 f 0 f yaw-0 yaw-f 代入した後に記録する。
[0267] 次に、ステップ S73における VPP画像マッチングの第 1の処理方法について説明 する。この第 1の処理方法は図 36におけるステップ S731 S735の処理からなる。 なお、このステップ S731 S735の処理は、前記ステップ S 72で設定された探索範 囲としての ΔΧ= ΔΧ ΔΧ 、及び ΔΥ= ΔΥ ΔΥ , Δ θ = Α Θ
MIN MAX MIN MAX yaw yaw- MIN Δ Θ の領域について個別に実行される力 繰り返し制御についての説明を yaw- AX
省略して 1回分の処理についてのみ説明する。
[0268] 先ず、ステップ S731において、 VPP平面画像 I (t- 1)を現時刻で設定されてい
Vf
る Δ 0 回転し、 ΔΧ及び ΔΥの方向に移動させる。(図 37の「S731」参照)。なお、 yaw
回転の中心は、 VPP画像における光学中心の位置(c 、 )とする。
χ-νρρ c y-vpp
[0269] ステップ S732にお!/、て、 VPP平面画像 I (t)と前記ステップ S731で移動した VPP
Vf
平面画像 I (t—l)との共通領域 R(AX、 ΔΥ, Δ θ )を求める(図 37の「S732」参
Vf yaw
照)。
[0270] ステップ S733において、 VPP画像 I (t- 1)をその時点で設定されている Δ 0 回
V yaw 転し、 Δ X及び Δ Yの方向に移動させる(図 37の「S733」参照)。
[0271] ステップ S734において、共通領域 R(AX, ΔΥ, Δ Θ ;)、 VPP画像 I (t)及び移 yaw V 動した VPP画像 I (t 1)に基づいて、次の(7— 4)式により VPP画像マッチングとし
V
て NSSDを計算する。
[0272] [数 64]
NSSD(Ax, Ay, ΔΘ yaw )
2
- !) move(Ax, Ay, A0yaw )(xj)- Iv (t)(x i)}
N 2 ilv (l
;£Κ(Δχ,Δγ,ΔΘ^)
■(7-4)
[0273] ここで、 xiは共通領域 R( ΔΧ, ΔΥ, Δ θ )に含まれるピクセルの座標であり、 Ν yaw
は、共通領域 R(AX, ΔΥ, Δ Θ )に含まれるピクセルの総数であり、 I (t-l)mo yaw V ve(AX, ΔΥ)は、ステップ S 734で VPP画像 I (t— 1)を Δ θ 回転し、 ΔΧ及び Δ
V yaw
Υだけ移動した画像を示す。
[0274] この(7—4)式により、 VPP画像 I (t)と VPP画像 I (t— 1)を Δ 0 回転し、 ΔΧ及
V V yaw
び Δ Yだけ移動した画像とのピクセル毎の輝度差の二乗和が正規ィ匕された値として 求められ、次のステップ S735において、移動量 move (Δχ、 Ay、 Δ θ )毎に類似 yaw 度として NSSD(Ax、 Δγ, Δ θ )が記録される。 NSSD(Ax、 Δγ, Δ θ )は正 yaw yaw 規ィ匕されていることから共通領域 R(AX, ΔΥ, Δ Θ )の面積の大小に拘わらずに yaw
相互比較が可能となる。
[0275] このようなステップ S7の処理によれば、対応点探索を行うことがないため、ロバスト 性が高 ヽ。また平面の位置姿勢パラメータに基づ ヽて平面に平行な VPP画像を生 成するため、車両姿勢の変動等による影響を受けず耐振動性が高い。実際上、道路 平面 Πには不規則であっても地模様等何らかの模様や特徴部が存在していることか ら、このような部分に基づいて自車速度ベクトル Vを求めることができる。
V
[0276] さらに、平面領域 Πに基づく共通領域 R ( AX, ΔΥ, Δ 0 )内で VPP画像マッチ f yaw
ングを行うことから、他の車両ゃ路側部等の余分な部分が削除されて高精度且つ高 速な演算が遂行される。
[0277] また、 VPP画像マッチングにおける類似度として、 NSSDに代えて前記の ZNCC 等を用いてもよい。
[0278] 次に、ステップ S73における VPP画像マッチングの第 2の処理方法について説明 する。この第 2の処理方法は、基本的には前記ステップ S1及び前記非特許文献 1で 説明した濃度勾配法と同じ方法であり、前記射影変換行列 Hに相当する移動回転行 列 Mを微小量変化させながら 2つの画像が最も重なるように収束させる。
[0279] 先ず、適当に真値に近い初期値としての移動回転行列 Mを設定しておき、次の(7 — 5)式により、 VPP画像 I (t— 1)を移動回転変換する。ここで、 I (X)を VPP画像 I (
V r V t— 1)の座標 x (x, y)における輝度値とする。なお、移動回転行列 Mは、 3 X 3の行 列である。また、移動回転行列 Mの初期値としては、前時刻のステップにて算出され た値を用いるとよい。
[0280] [数 65]
Ivpp(t-l)(x) =
(「叫は、変換された後の輝度値を表す。 )
[0281] 移動回転行列 Μの各パラメータを微小量変化させたとき、変換された VPP画像 I (t
V
1)上の座標 x (x, y)が座標 X" (χ〃 , γ" )に変化するものとすると、
[0282] [数 66] "〜(I + DX) …( )
[0283] の関係が得られる。なお、 Iは 3 X 3の単位行列であり、 Dは移動回転行列 Mの各パラ メータの微小変化量をパラメータとする 3 X 3の微小変化行列であり次の(7— 7)式で 表される。
[0284] [数 67]
[0285] ここで、 ωは微小回転角度、 dx及び dyは X方向及び y方向の微小移動量である。ま た、 ω、 dx及び dyをまとめて 1つのパラメータ Θとしても表す。このパラメータ Θを引 数とし、(7— 5)、(7— 6)式の関係を用いて、移動回転変換された VPP画像 I (t- 1
V
)と VPP画像 I (t)との重なり状態を示す評価関数 Ε ( Θ)は、次の(7— 8)式で表され
V
る。
[0286] [数 68]
Ε(Θ) = ∑[TVPP(T— D( - IvpP(t) ( )] 2 … - 8 )
plane
(「i」は、画素番号を表す。)
[0287] と表すことができる。ここで、 R は、 VPP平面画像 I (t 1)、I (t)における道路平 plane Vf Vf
面 Πに相当する領域である。
[0288] この(7— 8)式は前記(1 5)式と等価なものであり、基準画像 Iと VPP画像 I (t)、 b V 参照画像 Iと VPP画像 I (t- 1)、行列 Dxと行列 Dがそれぞれ対応する。
r V
[0289] したがって、前記と同様の方法により、 g; (輝度勾配)、 e; (輝度差分)、ヤコビアン を算出し、微小変化行列 Dを最小二乗法を用いて求めることができる。
[0290] 以上のようにして求めた微小変化行列 Dを用いて、移動回転行列 Mを次の(7— 9) 式にしたがって更新する。
[0291] [数 69]
M M(l + D) · ' · ( 7-9)
[0292] そして、(7— 9)式で求めた移動回転行列 Μを用いて、再び(7— 8)式の評価関数 Ε ( Θ )の値を算出する処理を繰り返す。この繰り返しの処理にぉ 、て移動回転行列 Μは最小二乗法を解いて得られる微小変化行列 Dを用いて、 M^M (I + D)として更 新される。評価関数 Ε ( Θ)の値が所定の値以下に収束したときのを Mの各成分より Δ θ 、 ΔΧ、 ΔΥを式(7— 10)を用いて求めることができる。
yaw
[0293] [数 70]
JI« l
QV 9 VS8 s日 _ ¾> χ M
このようなステップ S73の第 2の処理方法では、前記の第 1の処理方法で求めた Δ θ 、 ΔΧ、 ΔΥを初期値として第 2の処理方法を用いることで、一層正確に Δ Θ
ΔΧ、 ΔΥを求めることができる。
[0295] <ステップ S8 >
ステップ S8は、検出した物体が静止物体であるか移動物体であるかを判断する処 理であり、図 38に示すステップ S81〜S84のサブルーチン処理からなる。
[0296] ステップ S81において、前記ステップ S55で求められた各物体の相対速度であるべ タトル Vrl、 Vr2及び Vr3を読み出す。これらのベクトルは図 25における各矢印で示 されている。
[0297] ステップ S82において、前記ステップ S7において求めた車両 10の自車速度べタト ル Vを読み出す。この車両 10の自車速度ベクトル Vは図 25の原点を基点に示され
V V
ている。
[0298] ステップ S83において、検出された各物体の絶対速度ベクトル V N (N= 1、 2、 3
ABS
…;)を算出する。絶対速度 V Nは V N=V N+V 'としてベクトル加算により求め
ABS ABS r V
られる。ただし、 V 'は相対速度 V Nに含まれる、 自車両のモーションによる速度分で
V r
ある。図 39に各物体毎に求められた絶対速度ベクトル V Nを示す。図 39から明ら
ABS
かなように、静止物体であるガードレール 74については、 V 3 0となる。
ABS
[0299] ステップ S84において、絶対速度 V Nに基づいて、検出された各物体が静止物
ABS
体であるか移動物体であるかを調べる。具体的には、各物体の絶対速度のベクトル の大きさ I V N Iと閾値 Th を比較して、 Th ≥
BS ABS ABS I V N ABS Iであるとき対象となる
A
物体は静止物体であり、 I V N
ABS I >Th であるときは移動物体であると判断され
ABS
る。これにより、 V 3 0であるガードレール 74は静止物体として判別され、トラック 7
ABS
0及び乗用車 72は移動物体として識別される。
[0300] このように、ステップ S8の処理によれば、検出された物体の絶対速度を精度よく求 めることができ、物体が静止物か否かを正確に判断することができる。この判断結果 の情報は、例えば、車両 10と物体との所定の接近注意喚起手段に供することができ る。
[0301] また、ステップ S 7及び S8の処理によれば、 VPP画像 Iの平面領域 Πのみを用い
V f
て移動量を算出することにより、移動する物体と静止物とが混在する環境下において も正確に自車速度ベクトル Vを求めることができる。
[0302] さらに、例えば路面など一定の高さ(距離)にある物体を撮像して、路面を基準とし て速度計測を行うように定めた場合に、路面に対して高さのある物体が撮像され、該 物を基準として速度計測を行うと高さに基づく速度検出の誤差が発生することが知ら れているが、ステップ S7及び S8の処理によれば、平面領域 Πのみを用いることによ
f
り高さのある物体を除外して算出できるので、高さに基づく誤差が発生しない。
[0303] またステップ S7及び S8の処理によれば、平面までの距離も検出するため、例えば 歩道など路面に対して高さのある平面が画像中の大半を占める場合にも、その高さ を考慮して速度を算出できる。
[0304] なお、本実施の形態において求められる平面領域 Πは、車両 10が走行する略水
f
平な道路平面 Πに対応する領域であって、「平面」であっても垂直な面やレベルの異 なる他の面は除外される。
[0305] また、上記の説明では、基準カメラ 12と参照カメラ 14が設けられているものとしたが 、必要に応じてより多くのカメラを設けてもよい。