JP2006053757A - 平面検出装置及び検出方法 - Google Patents

Abstract

【課題】平面を検出するための画像間の射影変換行列を高精度且つ短時間で算出する。
【解決手段】基準カメラ１２及び参照カメラ１４間の回転行列及び並進ベクトルをパラメータ記憶部２６から読み出し、射影変換行列算出部２８において、前記回転行列及び前記並進ベクトルを用いて、基準カメラ１２及び参照カメラ１４によって撮像された画像に含まれる道路平面領域を重畳させることのできる射影変換行列を算出する。
【選択図】図２

Description

本発明は、例えば、自動車の運転者支援や自律移動車の自動運転を実現するため、これらの移動体に搭載した一対の撮像手段により撮像した画像から、前記移動体が走行可能な平面を検出する平面検出装置及び検出方法に関する。

自律移動車等の移動体の誘導において、走行可能な道路平面領域を検出し、あるいは、走行環境に存在する障害物を検出する方法は、レーザレーダ、超音波、ミリ波レーダ等を利用する方法と、画像を利用する方法とに大別することができる。

レーザレーダやミリ波レーダを用いる検出方法では、一般に装置が高価であり、また、十分な空間分解能が得られないという問題がある。また、超音波を用いる検出方法では、遠くの計測が困難であって、空間分解能も低いという問題がある。

一方、画像を用いる方法では、単眼による方法と、複眼を用いる方法とに分けることができる。従来、画像を用いる方法の多くは単眼、すなわち、１つの視点から得られる画像を用いるもので、主に、高速道路等の整備された環境で用いることを想定し、路面の白線（例えば分離線や中央線）等を検出することにより、走行領域を検出している。しかしながら、白線等の存在が保証されず、また路面の色やパターンがさまざまであるような一般道や駐車場等では、単眼画像から得られる濃度パターンのみから、走行可能領域と障害物を安定に区別することは困難である。

これに対して、複眼、すなわち、ステレオ画像を利用する検出方法では、原理的には環境の３次元構造を利用できるため、より安定に走行環境を認識できる可能性がある。特に、走行可能な領域は空間中でほぼ平面であるとみなせることから、画像間の射影変換行列を算出し、この射影変換行列により一方の画像を２次元射影変換して他方の画像に重畳させ、画像同士の重なり具合から、平面領域か否か、あるいは、障害物か否かを検出する方法が提案されている（特許文献１、非特許文献１参照）。

特開２０００−２９３６９３号公報 ホイング−ヤング・シャム（Ｈｅｕｎｇ‐Ｙｅｕｎｇ Ｓｈｕｍ）、リチャード・セーリスキ（Ｒｉｃｈａｒｄ Ｓｚｅｌｉｓｋｉ）、「"パノラミック・イメージ・モザイク"テクニカル・レポート（"Ｐａｎｏｒａｍｉｃ Ｉｍａｇｅ Ｍｏｓａｉｃｓ"Ｔｅｃｈｎｉｃａｌ ｒｅｐｏｒｔ）」、１９９７年、エム・エス・アール−テイー・アール−９７−２３、マイクロソフト・リサーチ（ＭＳＲ−ＴＲ−９７‐２３，Mｉｃｒｏｓｏｆｔ Ｒｅｓｅａｒｃｈ）

ところで、特許文献１に開示された検出方法は、各画像から対応する４点以上の特徴点を抽出し、それらの対応点に基づいて射影変換行列の各パラメータを算出するもので、射影変換行列を得るために対応点の存在が前提となっている。そのため、例えば、テクスチャの少ない画像では、対応点を見出すことができず、射影変換行列を算出できなくなるおそれがある。

一方、非特許文献１に開示された検出方法は、一般に「濃度勾配法」と称されるものであり、推定した射影変換行列により一方の画像を２次元射影変換して得た画像と、他方の画像とを比較して濃度パターンが一致するか否かを判定し、所望の一致範囲となるまで射影変換行列を繰り返し修正して求めるようにしたものである。この場合、非特許文献１では、移動体に対する走行面（道路面）の変動を考慮しておらず、一旦算出された射影変換行列は、当該移動体に固有のものとして決定されることを前提としている。

しかしながら、移動体は、移動に伴ってピッチングやローリングの挙動を示すため、一般に移動体と走行面との位置関係は一定とはならない。したがって、適切な射影変換行列を得るためには、走行面に対する移動体の状態に応じて算出する必要がある。この場合、所望の射影変換行列の算出に要する時間は、射影変換行列を構成する未知のパラメータの数に依存する。そのため、例えば、移動体の状態が大きく変動し、且つ、演算時間が制約されるような場合、射影変換行列の繰り返し演算を中途で打ち切ってしまうと、十分な精度を有する射影変換行列を得ることができなくなってしまう。

本発明は、上述した課題に鑑みてなされたものであり、画像間の射影変換行列を高精度且つ短時間で算出し、平面を高精度に検出することのできる平面検出装置及び検出方法を提供することを目的とする。

本発明に係る平面検出装置及び検出方法では、所定の位置関係に配置された一対の撮像手段を用いて、平面を含む相互に重複する領域の画像を撮像した後、予め求めた回転行列及び並進ベクトルを用いて、一方の撮像手段により撮像された画像に含まれる平面を、他方の撮像手段により撮像された画像に含まれる平面に重畳させることのできる射影変換行列を算出する。この場合、回転行列及び並進ベクトルを固定されたパラメータとすることで、射影変換行列を構成する未知のパラメータ数が削減され、射影変換行列が高精度且つ短時間で算出される。同様に、射影変換行列とともに、平面を規定する法線ベクトル及び撮像手段から平面までの距離が高精度且つ短時間で算出される。

なお、射影変換行列から求めた平面領域を平面パラメータを用いて変換することにより、撮像手段に対する平面領域の空間位置を検出することができるとともに、空間位置が検出された平面上に存在する物体を検出することができる。

本発明の平面検出装置及び検出方法によれば、一方の撮像手段の座標を他方の撮像手段の座標に変換するための回転行列及び並進ベクトルを予め求めておき、これらの回転行列及び並進ベクトルを用いて、各撮像手段により撮像された画像に含まれる平面を重畳させる射影変換行列を高精度且つ短時間で算出することができる。また、平面の法線ベクトルと、撮像手段から平面までの距離とを高精度且つ短時間に算出することができる。この結果、平面を高精度且つ短時間に検出することができる。

図１は、本発明の平面検出装置及び検出方法が適用される実施形態の車両１０（移動体）を示す。車両１０のウィンドシールド上部の左右には、車両１０が走行する道路平面領域を含む画像を撮像する一対の撮像手段である基準カメラ１２及び参照カメラ１４が固定配置される。基準カメラ１２及び参照カメラ１４は、共通の撮像領域が設定されたＣＣＤカメラ等からなるステレオカメラであり、これらには、撮像した画像を処理することで道路平面領域と障害物となる可能性のある物体とを検出する演算処理装置１６が接続される。なお、以下の説明では、基準カメラ１２によって撮像された画像を基準画像と称し、参照カメラ１４によって撮像された画像を参照画像と称する。

図２は、演算処理装置１６の機能構成ブロックを示す。演算処理装置１６は、基準カメラ１２によって撮像した基準画像を処理する画像処理部１８と、参照カメラ１４によって撮像した参照画像を処理する画像処理部２０と、所定の撮像時間間隔で撮像された基準画像及び参照画像を記憶する画像記憶部２２とを備える。

また、演算処理装置１６は、演算制御部２４を備え、演算制御部２４には、画像記憶部２２、パラメータ記憶部２６（パラメータ記憶手段）、射影変換行列算出部２８（射影変換行列算出手段、射影変換行列推定手段）、平面パラメータ算出部３０（平面パラメータ算出手段、平面パラメータ推定手段）、平面領域抽出部３２、物体検出部３４、物体相対速度算出部３６、仮想投影面画像生成部３８（空間位置算出手段）、自車速度算出部４０、物体絶対速度算出部４２及び表示処理部４４が接続される。

パラメータ記憶部２６は、基準カメラ１２の座標系を参照カメラ１４の座標系に変換するための回転行列及び並進ベクトルを既知のパラメータとして記憶するとともに、平面の法線ベクトルの初期値、基準カメラ１２から当該平面までの距離の初期値等、演算に必要な初期値パラメータ、基準カメラ１２及び参照カメラ１４の内部パラメータ、前回の演算や演算中に算出された各パラメータ等を記憶する。射影変換行列算出部２８は、参照画像中の道路平面領域を基準画像中の道路平面領域に２次元射影変換する射影変換行列を算出する。平面パラメータ算出部３０は、道路平面の法線ベクトル及び基準カメラ１２の光学中心から道路平面までの距離を平面パラメータとして算出する。平面領域抽出部３２は、基準画像と射影変換行列によって射影変換した参照画像とを用いて、車両１０が走行可能な道路平面領域を抽出する。物体検出部３４は、道路平面領域外の領域に存在する物体を検出する。物体相対速度算出部３６は、抽出された物体の車両１０に対する相対速度ベクトルを算出する。仮想投影面画像生成部３８は、抽出された道路平面領域を実際の道路面に平行な仮想平面に投影した仮想投影面（ＶＰＰ）画像（ＶＰＰ：Ｖｉｒｔｕａｌ Ｐｒｏｊｅｃｔｉｏｎ Ｐｌａｎｅ）を生成する。自車速度算出部４０は、ＶＰＰ画像上で車両１０の自車速度ベクトルを算出する。物体絶対速度算出部４２は、車両１０に対する物体の相対速度ベクトルと車両１０の自車速度ベクトルとから、各物体の絶対速度ベクトルを算出する。表示処理部４４は、上記の各演算結果をディスプレイに表示する処理を行うとともに、演算結果に基づいて必要に応じた警告等の報知を行う。

本発明の平面検出装置及び検出方法が適用される車両１０及び演算処理装置１６は、基本的には以上のように構成されるものであり、次に、図３に示すフローチャートに基づき、演算処理装置１６による処理の全体の流れを説明する。

先ず、基準カメラ１２及び参照カメラ１４により、車両１０が走行する道路平面領域を含む基準画像及び参照画像を所定の時間間隔で撮像し、画像処理部１８、２０において所定の画像処理を行った後、画像記憶部２２に各時間毎の画像情報として記憶させる（ステップＳ０）。

次に、射影変換行列算出部２８において、パラメータ記憶部２６に記憶されている初期値パラメータを用いて、参照画像中の道路平面領域を基準画像中の道路平面領域に２次元射影変換する射影変換行列を動的に算出する（ステップＳ１）。

平面領域抽出部３２は、ステップＳ１で算出した射影変換行列を用いて、基準画像から道路平面領域を抽出する（ステップＳ２）。

平面パラメータ算出部３０は、道路平面の法線ベクトル及び基準カメラ１２の光学中心から道路平面までの距離を平面パラメータとして算出する（ステップＳ３）。

次に、物体検出部３４は、ステップＳ３で算出した平面パラメータを用いて、ステップＳ２で抽出した道路平面領域を道路面上に変換して自分の車両１０以外の障害物となる可能性のある物体を検出し、若しくは、ステップＳ２で抽出した道路平面領域以外の領域に対してステレオ計測することにより物体を検出する（ステップＳ４）。

物体相対速度算出部３６は、抽出された各物体の時間毎の位置変化から、車両１０に対する各物体の相対速度ベクトルを算出する（ステップＳ５）。
さらに、仮想投影面画像生成部３８は、ステップＳ３で算出した道路平面の傾きである法線ベクトルを用いて、道路面を上方から見た画像である仮想投影面（ＶＰＰ）画像を生成する（ステップＳ６）。

そして、自車速度算出部４０は、ステップＳ６で生成されたＶＰＰ画像の各時刻での位置から車両１０の自車速度ベクトルを算出する（ステップＳ７）。

さらに、物体絶対速度算出部４２は、ステップＳ５で算出した物体の相対速度ベクトルと、ステップＳ７で算出した車両１０の自車速度ベクトルとを用いて、各物体の絶対速度ベクトルを算出する（ステップＳ８）。

なお、表示処理部４４は、以上のようにして抽出した道路平面領域、障害物となる可能性のある物体、車両１０及び物体の絶対速度等をディスプレイに表示するとともに、例えば、車両１０と物体との相対的な位置関係、速度関係に基づき、必要に応じて警告等の報知を行うことができる。

次に、本実施形態をステップ毎に詳細に説明する。

先ず、本発明に用いられる道路平面領域の抽出及び２次元射影変換の原理を説明する。なお、以下の説明において、同次座標は、文章中では、符号に記号「＾」を付加して表し、数式中では、符号の上に記号「〜」を付加して表すものとする。また、射影変換された画像については、文章中では、符号に記号「′」を付加して表し、数式中では、符号の上に記号「〜」を付加して表すものとする。

図４に示すように、空間中にある平面Π上の観測点Ｍ１が基準画像Ｉ_b及び参照画像Ｉ_rに投影されるとき、基準画像Ｉ_b上での同次座標をｍ＾_b、参照画像Ｉ_r上での同次座標をｍ＾_rとすると、各同次座標ｍ＾_b、ｍ＾_rは、（１−１）式に示すように、２次元射影変換によって関係付けられることはよく知られている。

（「〜」は、定数倍の不定性を許して等しいものとする。）

ここで、Ｈは３×３の射影変換行列である。また、図４において、Ｏは基準カメラ１２の光学中心、Ｏ′は参照カメラ１４の光学中心、Ｒは基準カメラ座標系から参照カメラ座標系への回転行列、ｔは、参照カメラ座標系において参照カメラ１４の光学中心から基準カメラ１２の光学中心へ向かう並進ベクトル、ｄは基準カメラ１２の光学中心Ｏと平面Πとの距離、ｎは平面Πの法線ベクトルである。

道路等の走行可能な領域は、空間中でほぼ平面であると見なすことができれば、参照画像Ｉ_rに対して適切な射影変換を施すことにより、平面の部分に対して基準画像Ｉ_bに一致するような画像を得ることができる。一方、平面上にない物体は、射影変換した際に画像が一致しないため、その物体の領域は走行可能な領域でない、あるいは、障害物となり得る領域であると判断することができる。したがって、平面の拡がりを算出することにより、走行可能な領域及び障害物となる物体の検出が可能となる。

＜ステップＳ０＞
基準画像Ｉ_b及び参照画像Ｉ_rに係る画像情報を取得する。

基準カメラ１２及び参照カメラ１４を用いて画像を撮像し、各画像間の明るさの違い等を除くため、画像処理部１８及び２０において、各画像にＬＯＧ（Ｌａｐｌａｃｉａｎ ｏｆ Ｇａｕｓｓｉａｎ）フィル夕をかける。また、ＬＯＧフィルタを掛けた画像は、コントラストが低いため、さらに、ヒストグラム平坦化処理を施す。これらの処理が施された基準画像Ｉ_b及び参照画像Ｉ_rとその原画像は、画像記憶部２２に記憶される。

＜ステップＳ１＞
射影変換行列算出部２８において、画像記憶部２２に記憶された基準画像Ｉ_b及び参照画像Ｉ_rを用いて、路面に対する射影変換行列Ｈを動的に推定する。

すなわち、基準画像Ｉ_b内のある領域Ｒ^Iで、射影変換行列Ｈを微少量変化させ、以下の（１−２）式の評価関数Ｅ（Ｈ）を最小化する射影変換行列Ｈを、繰り返し最適化によって推定する。

なお、Ｉ_b（ｍ＾_b）、Ｉ_r（Ｈｍ＾_b）は、同次座標ｍ＾_b、Ｈｍ＾_bにおける基準画像Ｉ_b及び参照画像Ｉ_rの輝度値を表す。

上記の推定には、適当に真値に近い初期値としての射影変換行列Ｈと、領域Ｒ^I（本発明では、以後「計算領域」と呼ぶ。）とを必要とする。そこで、時系列情報を利用し、以下に述べる手順により、射影変換行列Ｈと計算領域Ｒ^Iとを求める。

すなわち、同時刻における基準画像Ｉ_b及び参照画像Ｉ_r間の射影変換行列Ｈ_Sと、連続する時間で取得した２つの基準画像Ｉ_b間の射影変換行列Ｈ_mと、基準画像Ｉ_bに対する計算領域Ｒ^Iとを求める。そして、ある時刻ｔにおける射影変換行列Ｈ（Ｈ_s、Ｈ_m）の推定の際には、前時刻（ｔ−１）までの推定結果を利用する。

先ず、連続する基準画像Ｉ_b（ｔ−１）、Ｉ_b（ｔ）間の射影変換行列Ｈ_m（ｔ）を（１−２）式に基づく繰り返し最適化によって推定する。このとき、射影変換行列Ｈ_m（ｔ）の初期値としては、前時刻に推定された射影変換行列Ｈ_m（ｔ−１）を、また、計算領域Ｒ^Iの初期値としては、基準画像Ｉ_b（ｔ−１）に対して前時刻で求められている計算領域Ｒ^I（ｔ−１）を用いることができる。

次に、求められた射影変換行列Ｈ_m（ｔ）を用いて、前時刻の計算領域Ｒ^I（ｔ−１）を変換することにより、現時刻における計算領域Ｒ^I（ｔ）の予測値を求める。

基準画像Ｉ_b及び参照画像Ｉ_r間の射影変換行列Ｈ_S（ｔ）は、前時刻において推定された射影変換行列Ｈ_S（ｔ−１）を初期値とし、射影変換行列Ｈ_m（ｔ）から求めた計算領域Ｒ^I（ｔ）を用いて、（１−２）式に基づく繰り返し最適化によって推定する。

このように、時系列画像を用いた連続推定において、十分に真値に近い初期値としての射影変換行列Ｈと計算領域Ｒ^Iとを用いて、時刻ｔにおける基準画像Ｉ_b及び参照画像Ｉ_r間の動的な射影変換行列Ｈ_S（以下、射影変換行列Ｈという。）を安定して推定することができる。

次に、（１−２）式を用いた繰り返し最適化（濃度勾配法）による射影変換行列Ｈの推定方法について詳細に説明する。

濃度勾配法は、２つの画像の一方を微小量変化させ、画像の重なり具合（濃度差）を表す評価関数を最小化することにより、画像が最も重なるような射影変換行列Ｈの変化量を推定する方法である。

座標ｘ（ｘ，ｙ）における参照画像Ｉ_rの輝度値をＩ_r（ｘ）とすると、適当な射影変換行列Ｈにより射影変換された参照画像Ｉ_r′は、次の（１−３）式で表される。なお、参照画像Ｉ_rの上の「〜」は、射影変換された後の画像を表し、ｘの上の「〜」は、座標ｘ（ｘ，ｙ）の同次座標を表す。

なお、射影変換行列Ｈは、３×３のパラメータを要素とする自由度８（１パラメータを固定）の行列とする。

射影変換行列Ｈの８つのパラメータを微小量変化させたとき、射影変換された参照画像Ｉ_r′上の座標ｘ（ｘ，ｙ）が座標ｘ″（ｘ″，ｙ″）に変化するものとすると、

（「〜」は、定数倍の不定性を許して等しいものとする。）
の関係が得られる。なお、Ｉは３×３の単位行列、Ｄ_Xは射影変換行列Ｈの各パラメータの微小変化量をパラメータとする３×３の微小変化行列とする。

（１−３）、（１−４）式の関係を用いて（１−２）式の評価関数Ｅ（Ｈ）を書き替えると、射影変換された参照画像Ｉ_r′と基準画像Ｉ_bとの重なり状態を示す評価関数Ｅ（Ｈ）は、射影変換行列Ｈの微小変化行列Ｄ_Xのパラメータを変数として、

（「ｉ」は、画素番号を表す。）
と表すことができる。なお、ｄ_Xは、微小変化行列Ｄ_Xのパラメータをベクトルで表したものである。

（１−５）式をテーラ展開して一次近似すると、評価関数Ｅ（ｄ_X）は、

（「≒」は、（１−５）式の一次近似の範囲で等しいものとする。）
となる。

（１−６）式の評価関数Ｅ（ｄ_X）を最小化するベクトルｄ_Xは、評価関数Ｅ（ｄ_X）をベクトルｄ_Xで微分し、その微分値を０とおいて、

を解くことにより求めることができる。

この場合、ｇ_i（輝度勾配）、ｅ_i（輝度差分）は、基準画像Ｉ_b及び射影変換された参照画像Ｉ_r′から算出され、ヤコビアンＪ_dXiは、座標ｘ（ｘ，ｙ）から算出できる。したがって、ベクトルｄ_Xは、最小二乗法を用いて（１−７）式から求めることができる。 以上のようにして求めたベクトルｄ_X、すなわち、微小変化行列Ｄ_Xを用いて、射影変換行列Ｈを次の（１−８）式にしたがって更新する。なお、Ｉは、３×３の単位行列とする。

そして、（１−８）式で求めた射影変換行列Ｈを用いて、再び（１−６）式の評価関数Ｅ（ｄ_X）の値を算出する処理を繰り返し、評価関数Ｅ（ｄ_X）の値が所定の値以下に収束したときの射影変換行列Ｈを所望の射影変換行列Ｈとすることができる。

ところで、上記の説明では、射影変換行列Ｈを構成する８つのパラメータが不明であるものとして射影変換行列Ｈを推定しているが、基準カメラ１２及び参照カメラ１４間の回転行列Ｒ及び並進ベクトルｔが車両１０に対して固定されているものとすれば、予め求めた回転行列Ｒ及び並進ベクトルｔを用いて、平面パラメータ（法線ベクトルｎ及び距離ｄ）を変化させることにより、動的な射影変換行列Ｈを迅速且つ高精度に推定することができる。

この場合、回転行列Ｒ及び並進ベクトルｔは、例えば、基準カメラ１２及び参照カメラ１４を車両１０に固定した後、整備場のように、車両１０を停止させた理想的な環境下で高精度に求めておくことが可能である。そして、走行中等の任意の時刻ににおいて射影変換行列Ｈを求める際、理想的な環境下で求めた高精度な回転行列Ｒ及び並進ベクトルｔを用いるため、射影変換行列Ｈを高精度に算出することができる。

そこで、先ず、回転行列Ｒ及び並進ベクトルｔの算出方法について説明する。なお、回転行列Ｒ及び並進ベクトルｔは、以下のようにして算出する代わりに、その他のキャリブレーション方法を利用して求め、あるいは、基準カメラ１２及び参照カメラ１４の位置関係を直接計測して求めることもできる。

車両１０を停止させた状態において、基準カメラ１２及び参照カメラ１４によって平面Πを撮像した後、（１−５）式の評価関数Ｅ（ｄ_X）を用いて射影変換行列Ｈを推定する。

ここで、射影変換行列Ｈは、基準カメラ座標系から参照カメラ座標系への回転行列Ｒ、参照カメラ１４の光学中心から基準カメラ１２の光学中心へ向かう並進ベクトルｔ、基準カメラ１２及び参照カメラ１４の内部パラメータＡ_b、Ａ_rを用いて、

と表される。なお、ｋは、０でない係数であって、画像から得られた射影変換行列Ｈに定数倍の自由度が存在することを表す。内部パラメータＡ_b、Ａ_rが既知であるとすると、次の（１−１０）式が成立する。

そこで、射影変換行列Ｈ′を特異値分解する。Ｕ、Ｖを正規直交行列、Σを射影変換行列Ｈ′の特異値σ₁〜σ₃（σ₁≧σ₂≧σ₃＞０）を要素とする対角行列として、射影変換行列Ｈ′は、

と書き替えることができる。なお、

である。

（１−１０）、（１−１１）式から、

となる。

ここで、

とすると、次の（１−１５）式が得られる。

基準カメラ座標系の基底ベクトル（ｅ₁，ｅ₂，ｅ₃）を導入し、

とすると、（１−１５）式から得られる３つのベクトル方程式と、ｎ、ｎ′が単位ベクトル、Ｖが正規直交ベクトル、Ｒ′がノルム変化のない回転行列であることから、次の（１−１７）式が得られる。

（１−１７）式をｎ₁′²、ｎ₂′²、ｎ₃′²の連立方程式と考えれば、０でない解を持つには、行列式が０であるから、

となる。

そこで、射影変換行列Ｈ′の特異値σ₁〜σ₃が、
（I） 重解を持たない場合（σ₁≠σ₂≠σ₃）
（II） 重解を持つ場合（σ₁＝σ₂≠σ₃ 又は σ₁≠σ₂＝σ₃）
（III） ３重解を持つ場合（σ₁＝σ₂＝σ₃）
に場合分けして考える。（I）及び（II）の場合は、ｋ＝±σ₁若しくはｋ＝±σ₃とすると、（３−９）式から算出されるｎ′が単位ベクトルであることに矛盾する。したがって、いずれの場合でもｋ＝±σ₂として求められることになる。また、ｋ＝−σ₂の場合は２台のカメラ（基準カメラ１２及び参照カメラ１４）が平面を挟むような配置となるため除外され、ｋはσ₂として求められることになる。なお、（III）の３重解の場合は、２台の基準カメラ１２及び参照カメラ１４の光学中心が一致し、回転のみの場合であるため、除外する。

（I）及び（II）の場合、（１−１７）式より、ｎ′は、

として求まる。

σ₁≠σ₂≠σ₃のとき、ｎ₂′＝０であるから、

を得る。すなわち、Ｒ′は、ｅ₂の回りの回転行列である。したがって、次の（１−２１）、（１−２２）、（１−２３）式を得る。

（II）の場合、σ₁＝σ₂（又は、σ₂＝σ₃）として、（１−２１）式から、

となり、

となる。

ここで、（１−１９）式におけるε₁、ε₂の符号の違いにより、４つの解の候補が存在する。この４つの候補のうち、「平面上の点が画像に映る」条件を満たすｎ′を選択することで２つの候補に絞り込む。さらに、基準カメラ１２及び参照カメラ１４の位置関係が撮影中に変化しない条件を加え、解を一意に決定する。

以上のようにして、（１−２１）式で求めたＲ′を（１−１４）式に代入することにより、回転行列Ｒが求められる。また、（１−２３）又は（１−２６）式で求めたｔ′を（１−１４）式に代入してｔ／ｄを求め、ｔ／ｄを正規化し、並進ベクトルｔの単位ベクトルｔ_eを、

として求める。並進ベクトルｔの長さ｜ｔ｜（基準カメラ１２及び参照カメラ１４間の距離）は、設計値や実測値から予め求めることができるため、（１−２７）式の単位ベクトルｔ_eに｜ｔ｜を掛けることで、並進ベクトルｔが求められる。このようにして算出された回転行列Ｒ及び並進ベクトルｔは、パラメータ記憶部２６に記憶される。

次いで、以上のようにして算出された回転行列Ｒ及び並進ベクトルｔをパラメータ記憶部２６から読み出し、任意の時刻において、動的な射影変換行列Ｈを算出する。

（１−９）式を書き替えると、射影変換行列Ｈは、

と展開することができる。なお、射影変換行列Ｈは、構成するパラメータから算出されるため、定数倍の不定性を示す係数ｋを１とすることができる。この場合、回転行列Ｒ及び並進ベクトルｔが既に求められているため、射影変換行列Ｈの第１項は定数であり、第２項はｎ／ｄの変数となる。したがって、第２項を推定することで、射影変換行列Ｈを求めることができる。

そこで、ｎ／ｄを微少量変化させたときの射影変換行列Ｈの微小変化行列をＤとすると、

である。なお、ｎ″＝ｎ／ｄとし、その微小変化量をδｎ″とする。

微小変化行列Ｄをｎ／ｄの微小変化量δｎ″に書き替え、（１−５）式に対応する評価関数Ｅ（δｎ″）をテーラ展開して一次近似すると、

（「≒」は、（１−５）式の一次近似の範囲で等しいものとする。）
となる。

（１−３０）式の評価関数Ｅ（δｎ″）を最小化する微小変化量δｎ″は、評価関数Ｅ（δｎ″）を微小変化量δｎ″で微分し、その微分値を０とおいて、

を解くことにより求めることができる。

この場合、ｇ_i（輝度勾配）、ｅ_i（輝度差分）は、基準画像Ｉ_b及び射影変換された参照画像Ｉ_rから算出され、ヤコビアンＪ_dXiは、座標ｘ（ｘ，ｙ）から算出され、ヤコビアンＪδ_n″は、基準カメラ１２及び参照カメラ１４の内部パラメータＡ_b、Ａ_r、並進ベクトルｔから算出できる。したがって、微小変化量δｎ″は、最小二乗法を用いて、（１−３１）式から求めることができる。

以上のようにして求めた微小変化量δｎ″から、（１−２９）式を用いて射影変換行列Ｈの微小変化行列Ｄを求め、射影変換行列Ｈを次の（１−３２）式にしたがって更新する。

また、微小変化量δｎ″を前回の演算で求めたｎ″（＝ｎ／ｄ）に加算し、次の（１−３３）式にしたがって更新する。

そして、（１−３２）式で求めた射影変換行列Ｈと、（１−３３）で求めたｎ″とを用いて、再び（１−３０）式の評価関数Ｅ（δｎ″）の値を算出する処理を繰り返し、評価関数Ｅ（δｎ″）の値が所定の値以下に収束したときの射影変換行列Ｈを所望の射影変換行列Ｈとする。

このように、予め求めた回転行列Ｒ及び並進ベクトルｔを用いることにより、射影変換行列Ｈを構成するパラメータの自由度は、ｎ／ｄを構成する３つのパラメータだけとなるため、射影変換行列Ｈを迅速且つ高精度に推定することができる。

＜ステップＳ２＞
平面領域抽出部３２において、ステップＳ１で推定した射影変換行列Ｈを用いて、車両１０が走行可能な道路平面領域を抽出する。この抽出処理について、図５を参照して説明する。

ステップＳ０でＬＯＧフィルタ処理及びヒストグラム平坦化処理が施された参照画像Ｉ_rに対して、ステップＳ１で推定した射影変換行列Ｈが作用され、射影変換画像Ｉ_r′が生成される。

次いで、ステップＳ０でＬＯＧフィルタ処理及びヒストグラム平坦化処理が施された基準画像Ｉ_bと、生成された射影変換画像Ｉ_r′との差分画像（Ｉ_r′−Ｉ_b）を求める。

この場合、平面上に存在する点は、基準画像Ｉ_bに正確に投影されるため、差分画像（Ｉ_r′−Ｉ_b）における輝度値は小さくなる。一方、平面上にない点の差分画像（Ｉ_r′−Ｉ_b）における輝度値は大きくなる。したがって、所定の閾値を設定して差分画像（Ｉ_r′−Ｉ_b）を２値化することにより、道路平面領域Π_fを抽出することができる。

＜ステップＳ３＞
平面パラメータ算出部３０において、ステップＳ１で推定した射影変換行列Ｈを用いて、平面Πの平面パラメータを算出する。この場合、平面パラメータである法線ベクトルｎ及び距離ｄは、射影変換行列Ｈが収束した際に更新されている（１−３３）式のｎ″から、

として直ちに求めることができる。

このように、予め求めた回転行列Ｒ及び並進ベクトルｔを用いることにより、射影変換行列Ｈを迅速且つ高精度に推定できるとともに、法線ベクトルｎ及び距離ｄを同時に算出することができる。

＜ステップＳ４＞
ステップＳ４は物体を検出する処理であり、図６に示すステップＳ４１〜ステップＳ４６のサブルーチン処理からなる。

ステップＳ４１において、基準カメラ１２及び参照カメラ１４によって、立体領域をステレオ計測し、物体の各エッジ点のワールド座標Ｃｗを求める。

ステップＳ４２において、ワールド座標Ｃｗの高さ方向の座標Ｙｗ（図７参照）に基づき、平面からの高さにより物体の候補点を抽出する。具体的には、図７に示すように、平面からの高さがＴｍｉｎ（例えば、２０ｃｍ）以上であってＴｍａｘ（例えば、２．５ｍ）未満の空間に存在する候補点（Ｔｍｉｎ≦Ｙｗ＜Ｔｍａｘ）を抽出する。これにより、道路上に設けられた歩道橋、トンネルの入口部、看板等の少なくとも一部が除外され、他の車両やガードレール等の車両１０の走行に関わり合いのあり得る部分のみが抽出されやすくなる。これにより、図７の候補点Ｐａは抽出され、候補点Ｐｂは除外される。

また、車幅方向であるＸｗ方向及び前後方向であるＺｗ方向についても、所定範囲内に制限して候補点の抽出を行うようにしてもよい。これにより、車両１０が通過する可能性のない箇所に対して無駄な投票処理を行うことが防止され、計算量を低減することができる。この場合、例えば、図８に示すようにｘ方向については−１０〜１０ｍ、ｚ方向については０〜４０ｍの範囲に制限するとよい。

ステップＳ４３において、物体の各候補点をそのＸｗ、Ｚｗ座標に応じて図８に示す投票面５０上に設けた一定サイズの桝目（セル）単位に投射する。投票面５０は、ワールド座標系の直交する２軸である座標Ｘｗと座標Ｚｗ（図１６参照）の座標面として表される。このとき、同物体のセルに対して複数の候補点が重複して投射される場合には、その重複数を記録する。このように重複数を考慮しながら候補点を投射する処理を投票と呼び、重複数を投票数と呼ぶ。投票の処理では、前記ステップＳ４２においてＴｍｉｎ未満の点及びＴｍａｘ以上の点が予め除外されているため、歩道橋、トンネルの入口部、看板等に対応する点の投票数は０又は十分に小さい値となる。なお、図８及び図９に示す投票面５０は、説明上の模式的なものであって、他の図５等とは関連付けられていない。

ステップＳ４４において、投票面５０上に投票された候補点を隣接する群毎にグループ分けするクラスタリング処理を行う。クラスタリング処理によって分けられた群をクラスタと呼ぶ。クラスタリング処理としては、最短距離法（Ｎｅａｒｅｓｔ Ｎｅｉｇｈｂｏｒ Ｍｅｔｈｏｄ）を挙げることができ、最短距離法で用いる特徴量としては幅（座標Ｘｗ）及び奥行き（座標Ｚｗ）を用いるとよい。このようなクラスタリング処理によれば、検出物体のモデルが不要であって、種々の形状の物体を検出することができる。

このステップＳ４４のクラスタリング処理によって、例えば、図９に示すように、投票面５０上で３つのクラスタ５２ａ、５２ｂ及び５２ｃが得られる。

なお、ステレオ計測における対応探索のミス（マッチングミス）によるノイズを除去することを目的として、クラスタリング処理を行った後にクラスタに対して膨張収縮処理を行うようにしてもよい。物体をステレオ計測する際にマッチングミスがあると、計測される３次元位置には誤差が生じるため、計測された物体形状には凹凸（ノイズ）が生じ、このノイズによりクラスタ形状が異なることがある。膨張収縮処理では、膨張により凹凸部をつなげ、収縮により元の大きさに戻すため、ノイズの影響を抑えた形状にすることができる。

ステップＳ４５において、クラスタリング処理によって分けられた各クラスタ５２ａ、５２ｂ、５２ｃが物体であるか又はノイズであるかを判断し、ノイズによるクラスタを除去する。

具体的には、先ず、得られた各クラスタ５２ａ、５２ｂ及び５２ｃに対してそれぞれ合計投票数を求める。つまり、クラスタ５２ａの合計投票数は３８であり、クラスタ５２ｂの合計投票数は２１であり、クラスタ５２ｃの合計投票数は３である。また、各クラスタ５２ａ、５２ｂ及び５２ｃにおける面積上の重心ｇの奥行きである座標Ｚｗの値Ｚｇを基準とした閾値Ｔｈｚと各合計投票数とを比較し、合計投票数が閾値Ｔｈｚ以下であるクラスタはノイズであるとして除去する。閾値Ｔｈｚは以下の（４−１）式で表される。

ここで、Ｋｇは係数であって、例えば、１ｍ先に物体が存在するとした場合の最小の合計投票数である。（４−１）式は、画像中に映し出される物体の面積が距離の二乗に反比例して小さくなるという現象に基づいて設定されている。

このような、ステップＳ４５の処理によって、例えば、合計投票数の少ないクラスタ５２ｃはノイズとして除去される。また、歩道橋、トンネルの入口部、看板等の合計投票数が小さい物体もノイズとして除去される。

ノイズが除去された投票面は、図５の例に則してより実際に近いデータとして示すと、図１０に示すようになり、この投票面５０が所定の記憶部に記録される。図１０から明らかなように、この投票面５０には、比較的近い前方で車線変更を行っているトラックに対応したクラスタ５４ａと、該トラックより前方を走る乗用車に対応したクラスタ５４ｂと、ガードレールに対応したクラスタ５４ｃが抽出されている。また、投票面５０には、設計者の確認用に座標Ｘｗ、座標Ｚｗ及び自車である車両１０が模式的に示されている。

ステップＳ４６において、残ったクラスタ５４ａ〜５４ｃを物体として認識し、対応する物体位置領域を切り出して、所定の記憶部に記憶する。

ステップＳ４（図３参照）の物体抽出処理はこのようなステップＳ４１〜Ｓ４６により構成されているが、各ステップについて以下さらに詳細に説明する。

ステップＳ４１は、図１１に示すステップＳ４１１〜ステップＳ４１７のサブルーチン処理からなる。

ステップＳ４１１において、基準画像Ｉ_bからエッジ及び特徴点を抽出するソーベルフィルタ処理を行う。このソーベルフィルタ処理とは、画像輪郭抽出のためのフィルタ処理であって、画素毎にその近傍の画素に所定の係数をそれぞれ乗算し、結果を合計する処理であって、垂直方向、水平方向の二つの係数行列を用いてこの処理を行う。

すなわち、自動車のように、表面が鏡面反射している物体は、見る角度によって反射光が変化するため、同じ点を左右の画像で観測しても輝度が異なってしまう。そのため、左右の画像間での領域ベースマッチングがうまく行かず、ステレオ距離計測が不正確となることがある。そこで、テクスチャレスな部分や鏡面反射している部分はステレオマッチングをせずに、エッジ部分のみステレオマッチングを行い、距離の計測をする。そのため、先ず入力画像にソーベルフィルタ処理を行い、エッジや特徴点を抽出し、この後２値化し、図１２に示すエッジ画像６０を作成する。図１２から明らかなように、この時点のエッジ画像６０には、路面の白線等の物体検出の処理には無関係な部分も抽出されている。

なお、図１２では、説明の便宜上エッジ画像６０を模式的に示しており、実際には、図１３で示すような画像データが得られる。

ステップＳ４１２において、白線等の道路平面にあるエッジは検出対象ではないため、エッジ画像６０から道路平面領域抽出処理（ステップＳ２）で得られた平面領域Π_f（図５参照）の部分を除去する処理を行う。具体的には、エッジ画像６０と平面領域Π_fとの二値的な排他処理を行い図１４に示す修正エッジ画像６２を得る。この際、エッジ画像６０における上部の所定幅の領域や下部のボンネットの領域等は検出対象がないことが明らかであって、しかもこれらの領域はエッジ画像６０上の場所が固定されていることから、これらの部分に対しても所定の領域指定手段により除去する。

なお、エッジ画像６０から平面部分が完全に取り除けなくとも、後述するように車両１０の通過する空間に限定した処理を行うので、検出対象として誤検出されることはない。

ステップＳ４１３において、修正エッジ画像６２上に残った各エッジ点に対応する参照画像Ｉ_r上の探索範囲としてのエピポーララインＥＰを設定する（図１５参照）。エピポーララインＥＰは、修正エッジ画像６２上におけるエッジ点の参照画像Ｉ_r上に対応する点を含む直線として、参照カメラ１４の内部パラメータ、基準カメラ１２の内部パラメータ、参照カメラ１４と基準カメラ１２との回転行列Ｒ及び並進ベクトルｔに基づいて算出されるが、詳しくは後述する。

ステップＳ４１４において、道路平面領域抽出処理（ステップＳ２）で得られた平面領域Π_fに射影変換行列の逆行列をかけて参照画像Ｉ_r上における参照平面領域Π_frを求めて、参照画像Ｉ_r上に投影する。

ステップＳ４１５において、参照画像Ｉ_r上におけるエピポーララインＥＰから参照平面領域Π_frの部分を除去する処理を行い、エピポーララインＥＰは非平面領域に限定される。

ステップＳ４１６において、エッジ点に対応する基準画像Ｉ_b上の小領域に対して、参照画像Ｉ_rのエピポーララインＥＰ上で領域ベースマッチング処理を行う。この領域ベースマッチング処理とは、基準画像Ｉ_bにおけるエッジ点を中心とした小さいマッチングウィンドウを作成し、該マッチングウィンドウと参照画像Ｉ_rにおけるエピポーララインＥＰ上の小ウィンドウとの類似度の計算を順次行う処理である。領域ベースマッチング処理によって最も類似度の高い点がエッジ点の対応点（ウィンドウ）として求められる。

この際、先ず、基準画像Ｉ_bから参照画像Ｉ_rへ領域ベースマッチング処理を行い、計算された類似度を閾値と比較し、マッチングが成功したと判断されたとき、今度は参照画像Ｉ_rから基準画像Ｉ_bへ領域ベースマッチングを行い、先の基準画像Ｉ_bの点に一致した場合に、マッチングが成立すると判断する。このような領域ベースマッチングを行うことで、繰り返しパターンによるマッチングミスを抑えることができる。ここで用いる閾値としては、類似度の最大値に対する第１閾値と、類似度の最大値から類似度の２番目の最大値との差に基づく第２閾値を設けて判断してもよい。

類似度の計算には、例えば、正規化相互相関関数（ＺＮＣＣ）を類似度関数をして用いることができる。正規化相互相関関数ではパターンの類似度を計算しているため、基準画像Ｉ_bと参照画像Ｉ_rとに全体的な輝度のオフセットずれがある場合であってもマッチングのミスが発生しにくい。正規化相互相関関数は、次の（４−２）式により求められる。

ここで、Ｉ_b、Ｉ_rはそれぞれ基準画像と参照画像を示し、Ｒはウィンドウ領域を示す。またｘはウィンドウ内のピクセルの位置を示す。また、ｕ及びｖはそれぞれ基準画像、参照画像のウィンドウの基準位置を示す。

また、基準画像Ｉ_b及び参照画像Ｉ_rの種類や必要な計算速度等に基づいて他の類似度関数（ＳＳＤ（２乗残差和）、ＮＳＳＤ及びＳＡＤ（絶対値和）等）を用いてもよい。

このステップＳ４１６における領域ベースマッチング処理は、基準画像Ｉ_bに関しては平面領域Π_fが除かれた部分のエッジ点のみが処理対象となり、一方、参照画像Ｉ_rに関してはエピポーララインＥＰ上のうち参照平面領域Π_frが除かれた部分のみが処理対象となる。したがって、計算負荷が大幅に削減されるとともに、平面領域の点を検出してしまうことがない。また、計算負荷が低減されることから、計算のためのハードウェアに関して低廉化を図ることができる。

ステップＳ４１７において、領域ベースマッチングの成立した基準画像Ｉ_b上の点（ウィンドウ）と参照画像Ｉ_r上の点（ウィンド）に基づいて、三角測量の原理に基づくステレオ計測により対応する点のカメラ座標Ｃｃを計算する。

ステップＳ４１８において、カメラ座標Ｃｃを自車である車両１０を基準としたワールド座標Ｃｗに変換する。具体的には、図１６に示すように、カメラ座標系を基準カメラ１２の光学中心を原点とし光軸を座標軸Ｚｃとして含む直交座標Ｘｃ、Ｙｃ、Ｚｃと表す。またワールド座標系は座標軸Ｙｗを法線ベクトルｎ方向に、座標軸Ｘｗ、Ｚｗを平面Π上に設けた直交座標系Ｘｗ、Ｙｗ、Ｚｗと表す。なお、座標軸Ｚｗは車両の前後方向とし、原点は、車両に対する物体位置を求める際に基準とする車両の特定位置（の平面Π上の位置）とする。さらに、ワールド座標系の原点からカメラ座標系の原点へ向かうベクトルを［ｔｗｘ、ｔｗｙ、ｔｗｚ］^t＝ｔｗとして表す（ｔｗｙは距離ｄに等しい）。各パラメータをこのように表したとき、ワールド座標Ｃｗは、以下の（４−３）式で表される。

ここで、法線ベクトルｎを（４−４）式で表したとき、回転行列Ｒ_wは、（４−５）式で表される。

なお、説明を簡単にするために、図１１のフローチャートでは、ステップＳ４１３〜ステップＳ４１８の処理を直列的に示したが、実際上、修正エッジ画像６２に表される各エッジ点毎にステップＳ４１３〜ステップＳ４１８を実行し、対応するワールド座標Ｃｗを求める。

このようにして、修正エッジ画像６２上の各エッジ点は奥行きである座標軸Ｚｗに基づいて、例えば、図１７に示すような奥行き画像６４として表すことができる。この奥行き画像６４では、座標軸Ｚｗの値（又はワールド座標系の原点からの距離）に応じた大きさの点が示されている。各点は、座標軸Ｚｗの値に応じて色の変化するグラデーション状の点で表してもよい。奥行き画像６４は設計者の動作確認のために供されるものであり実際上は設けなくてもよいが、該奥行き画像６４を生成するために必要なワールド座標Ｃｗは所定の記憶部に記録しておく。

上述したように、ステップＳ４１の処理（ステップＳ４１１〜Ｓ４１８）によれば、基準画像Ｉ_b及び参照画像Ｉ_rに基づいて、物体のエッジ点毎にワールド座標Ｃｗを求めることができる。この際、平面領域Π_fを除外した部分について処理を行うので、計算負荷の低減が図られる。このうち、ステップＳ４１３〜Ｓ４１６は参照画像対応部探索手段ということができる。

＜ステップＳ５＞
ステップＳ５は物体の相対速度を算出する処理であり、図１８に示すステップＳ５１〜ステップＳ５５のサブルーチン処理からなる。

ステップＳ５１において、連続する時刻の基準画像Ｉ_bから物体領域の重なり割合Ｒａｔｅを算出する。

ステップＳ５２において、連続する時刻における物体の重心移動距離Ｌｇを算出する。

ステップＳ５３において、重なり割合Ｒａｔｅ及び重心移動距離Ｌｇに基づいて閾値判定による同一物体の判定を行う。

ステップＳ５４において、物体の位置を高精度に求めるため再計算して位置ベクトルＰ_tを求める。

ステップＳ５５において、位置ベクトルＰ_tに基づいて、物体の相対速度ベクトルＶ_obst-relを算出する。

ステップＳ５（図３参照）はこのようなステップＳ５１〜Ｓ５５により構成されているが、各ステップについて以下さらに詳細に説明する。なお、ステップＳ５１〜Ｓ５５についての説明では、検出された各物体のうち、基準画像Ｉ_bにおけるトラック７０（図５参照）に対する処理を例示的に説明するが、他の検出物体である乗用車７２やガードレール７４についても同様の処理が行われる。ステップＳ５１は、図１９に示すステップＳ５１１〜ステップＳ５１７のサブルーチン処理からなる。

ステップＳ５１１において、前時刻のステップＳ５１７で記憶されたテンプレート８０ａを所定の記憶領域から呼び出す。ここで、テンプレート８０ａとは投票面５０（図２０参照）におけるクラスタ５４ａに対応したトラック７０の基準画像Ｉ_bにおける像を領域指定して記録したものであり、例えば、平面から１．５ｍの高さの領域として記録されている。

概念的には、図２０に示すように、点Ｐ０は、投票面５０上の点であり、光学中心Ｏから一定角度で走査し、クラスタ５４ａに当たる点とする。点Ｐ０に基づいてワールド座標Ｃｗが計算できる。点Ｐｒを点Ｐ０と同じ幅位置と奥行位置であり、且つ道路平面Π上の点とする。点Ｐｒは、道路平面Π上の点であることから高さが０である。点Ｐｘは、点Ｐ０と同じ幅位置と奥行位置であり、且つ道路平面Πから１．５ｍの高さをもつ点とする。次に点Ｐｒ、Ｐｘを用いてカメラ座標Ｃｃに変換する。カメラ座標Ｃｃに変換後、透視投影行列を用いて基準画像Ｉ_b上の座標に変換すればよい。このようにして前時刻のクラスタ５４ａに対応したテンプレート８０ａが求められて、記録されている。同様に、乗用車７２に対応した前時刻のクラスタ５４ｂ及びガードレール７４に対応した前時刻のクラスタ５４ｃに対しても図２１に示すテンプレート８０ｂ及びテンプレート８０ｃが記録されている。

また、前記ステップＳ４１８で各物体の高さが求められていることから、各テンプレート８０ａ、８０ｂ及び８０ｃの高さは求められた物体の高さに応じて設定してもよい。

ステップＳ５１２において、現時刻のメインルーチン処理におけるステップＳ０で取得した基準画像Ｉ_bを読み出す。なお、ここではＬＯＧフィルタやヒストグラム平坦化などの処理を施す前の原画像を用いるものとする。

ステップＳ５１３において、各テンプレート８０ａを用いて、現時刻に取得した基準画像Ｉ_b上でテクスチャに基づくテンプレートマッチングを行い、類似度の最も高い位置に各テンプレート８０ａを移動させる。テンプレートマッチングの具体的手法としては、前記のＺＮＣＣ（４−２）式や、ＳＳＤ、ＳＡＤ等を用いたマッチングを挙げることができる。

ステップＳ５１４において、前記ステップＳ４６で記録したデータに基づいて、現時刻に求められたクラスタ５４ａを呼び出す。

ステップＳ５１５において、呼び出したクラスタ５４ａに対してそれぞれ対応する物体位置領域８２ａを求める。この物体位置領域８２ａは前記ステップＳ５１１で説明した手順によって求められる。物体位置領域８２ａは、平面的なクラスタ５４ａに基づいて求める方法以外にも、図１７に示されるような奥行き画像６４における各エッジ点の３次元的な空間位置に基づいて求める方法を採ってもよい。

ステップＳ５１６において、前記ステップＳ５１３で移動したテンプレート８０ａと、前記ステップＳ５１５で求められた物体位置領域８２ａとの重なり割合Ｒａｔｅを次の（５−１）式で算出する。

ここで、Ｒ_t-1は、移動したテンプレート８０ａの領域であり、Ｒ_tは、対応する物体位置領域８２ａである。また、∩は領域の重なり部分を示す演算子であり、ｍｉｎ（Ｒ_t-1、Ｒ_t）はＲ_t-1とＲ_tの面積の小さいいずれか一方を選択する関数である。
（５−１）式によれば、移動したテンプレート８０ａと物体位置領域８２ａが完全に一致する場合にはＲａｔｅ＝１となり、一致する部分が全くない場合にはＲａｔｅ＝０となる。また、概念的には、図２１に示すように、基準画像Ｉ_b上でテンプレートを示すＲ_t-1と物体位置領域を示すＲ_tに対して重複する部分Ｒ_t-1∩Ｒ_tの割合を求めることになる。

重なり割合Ｒａｔｅが所定の閾値Ｔｈａ（０＜Ｔｈａ＜１）以上である場合には、対応する前時刻の投票面５０におけるクラスタと現時刻の投票面５０におけるクラスタを同一物体に基づくものであると仮判定されてトラッキングされる。トラッキングとは、異なった時刻に得られたクラスタの対応付けを行う処理であり、これにより物体の動作が検出可能となる。

ステップＳ５１７において、前記ステップＳ５１５で求められた物体位置領域８２ａを新たなテンプレート８０ａとして所定の記憶部に記録する。この新たなテンプレート８０ａは、前記のとおり次回処理時のステップＳ５１１で呼び出される。

このように、ステップＳ５１（ステップＳ５１１〜Ｓ５１７）においては、テンプレート８０ａに基づいてテンプレートマッチングを行うとともに、投票面５０のクラスタ５４ａから物体位置領域８２ａを求めるため、２重のトラッキング処理を行っており、これらの結果の重なり割合Ｒａｔｅを求めてトラッキング処理の信頼性を高めている。実際には、このステップＳ５１で求めたトラッキングの関連づけはステップＳ５３で最終的に確認される。

次に、ステップＳ５２の処理内容について説明する。ステップＳ５２は、図２２に示すステップＳ５２１〜ステップＳ５２４のサブルーチン処理からなる。

ステップＳ５２１において、前記ステップＳ４６で記録したデータに基づいて、現時刻に求められたクラスタ５４ａを呼び出す。

ステップＳ５２２において、クラスタ５４ａの現時刻の重心ｇ_Tを求めて、図１０に示すように投票面５０に記録する。

ステップＳ５２３において、前時刻にステップＳ５３３を実行したときに記録したクラスタ５４ａの前時刻の重心ｇ_Oを読み出す。

ステップＳ５２４において、図１０に示すように、クラスタ５４ａの重心ｇ_Oと重心ｇ_Tとの重心移動距離Ｌｇを算出する。

このように、ステップＳ５２（ステップＳ５２１〜Ｓ５２４）においては、クラスタ５４ａの前時刻の重心ｇ_Oと現時刻の重心ｇ_Tとの重心移動距離Ｌｇを求めており、短時間の移動距離として合理的なもののクラスタ同士を対応づけることができる。実際には、このステップＳ５２で求めたトラッキングの関連づけは次のステップＳ５３で最終的に確認される。

次に、ステップＳ５３の処理内容について説明する。ステップＳ５３は、図２３に示すステップＳ５３１〜ステップＳ５３４のサブルーチン処理からなる。

ステップＳ５３１において、対応する物体毎に、前記ステップＳ５１６で求めた重なり割合Ｒａｔｅと閾値Ｔｈａとを比較し、Ｒａｔｅ≧ＴｈａであればステップＳ５３２へ移り、Ｒａｔｅ＜ＴｈａであればステップＳ５３４へ移る。

ステップＳ５３２において、対応する物体毎に、前記ステップＳ５２４で求めた重心移動距離Ｌｇと閾値Ｔｈｂとを比較し、Ｌｇ≦ＴｈｂであればステップＳ５３３へ移り、Ｌｇ＞ＴｈｂであればステップＳ５３４へ移る。この閾値Ｔｈｂは、自車の走行速度と対向車の走行速度を考慮して、処理上の演算周期に物体が移動可能な合理的な距離として固定値で設定されている。

ステップＳ５３３においては、対応付けられた２つの物体は同一物体であるとして確定認識され、記録される。また、重心ｇ_oをｇ_o←ｇ_Tとして入れ替えて所定の記憶部に記憶する。

一方、ステップＳ５３４においては、対応付けられた２つの物体は別物体であったものとして認識される。この場合、対応付けられなかった物体については、画像の前後関係に基づいて撮像範囲内に入ってきた新たな物体として認識する登録処理を行い、又は撮像範囲から抜け出た物体として所定の削除処理を行う。

このように、ステップＳ５３（ステップＳ５３１〜Ｓ５３４）においては、前記ステップＳ５２までの処理で対応付けられた２つの物体について、重なり割合Ｒａｔｅと重心移動距離Ｌｇに基づいて、同一物体か否かの確認を一層確実に行うことができる。

なお、このステップＳ５３１〜Ｓ５３４は、予め仮に対応付けられた２つの物体に対して行うものとして説明したが、前時刻検出の各物体と、現時刻検出の各物体に対して総当たり的に行って、対応付けを行ってもよい。

また、ステップＳ５３２で用いられる閾値Ｔｈｂは固定値であるものとして説明したが、車両１０から重心ｇ_Oまでの座標Ｚｗ方向の距離Ｚｇに基づいて変化する関数としての閾値Ｔｈ（Ｚｇ）を閾値判定に用いてもよい。この場合、ある物体の重心ｇ_Oまでの座標Ｚｗ方向の距離がＺｇ１であったとすると、閾値はＴｈ（Ｚｇ１）として表される。また同様に、閾値を自車速｜Ｖ_V｜に基いて変化する関数としての閾値Ｔｈ（｜Ｖ_V｜）を閾値判定に用いてもよい。

次に、ステップＳ５４及びステップＳ５５の処理内容について説明する。ステップＳ５４は、図２４に示すステップＳ５４１〜ステップＳ５４４からなり、ステップＳ５５は、図２６に示すステップＳ５５１〜Ｓ５５３からなる。

ステップＳ５４１において、基準画像Ｉ_bにおける物体の注目点Ｐｃを特定する。この注目点Ｐｃは、ステップＳ５１３のテンプレートマッチングにより移動したテンプレート８０ａの領域内に設定し、例えば、テンプレート８０ａの中心点や、物体の角部などの特徴的な点として特定すると、常に物体上の略同一点に特定される。

ステップＳ５４２において、注目点Ｐｃに対応させたエピポーララインＥＰを参照画像Ｉ_r上に設定する。このとき、クラスタ５４ａを利用することで、トラック７０までの概略距離を算出できることから、これに基づいてエピポーララインＥＰを必要十分な程度に短く設定できる。

クラスタを利用した注目点Ｐｃの概略距離の算出方法を説明する。先ず、クラスタ５４ａの重心ｇ_T（図１０参照）よりＺｗ座標を求める。これを注目点ＰｃのＺｗ座標に仮設定し、注目点Ｐｃの画像座標（ｕ_pc，ｖ_pc）とともに、次の（５−２）式に代入して対応するＸｗ座標を算出する。（λは定数倍の自由度を表す）

こうして求めた投票面５０上の位置（Ｘｗ，Ｚｗ）と基準カメラの位置（ｔｗｘ，ｔｗｚ）を通る直線を設定し、その直線に沿ってクラスタ５４ａの点を探索して、車両１０に最も近い点のＺｗ座標を注目点Ｐｃの概略距離とすればよい。

ステップＳ５４３において、前記ステップＳ４１６と同様に注目点Ｐｃに対応する基準画像Ｉ_b上の点に対して、参照画像Ｉ_rのエピポーララインＥＰ上で領域ベースマッチング処理を行う。

ステップＳ５４４において、前記ステップＳ４１７と同様に、マッチングの成立した基準画像Ｉ_b上の点と参照画像Ｉ_r上の点に基づいて、ステレオ計測によって対応する点のカメラ座標Ｃｃを計算する。さらに、前記ステップＳ４１８と同様に、求められたカメラ座標Ｃｃをワールド座標Ｃｗに変換する。求められた物体の位置であるワールド座標Ｃｗは、図２５に示すように、投票面５０上において２次元的な位置ベクトルＰ_tとして表す。

次に、図２６のステップＳ５５１において、物体の前時刻に検出した位置ベクトルＰ_t-1を読み出す。この位置ベクトルＰ_t-1は、前時刻処理時のステップＳ５５３で所定の記憶部に記録されている。

ステップＳ５５２において、次の（５−３）式により物体の相対速度ベクトル（移動ベクトル）Ｖ_obst-relを算出する。

ここで、Δｔは処理の演算周期であり、単位時間あたりの位置ベクトルＰ_tの変化として相対速度ベクトルＶ_obst-relが求められる。

ステップＳ５５３において、次回の処理に供するために、位置ベクトルＰ_tをＰ_t-1←Ｐ_tと代入して記憶部に記録する。ここで記録する位置ベクトルＰ_tは、ステップＳ５１７において新たなテンプレート８０ａとして記憶した物体位置領域８２ａに対して、ステップＳ５４１〜Ｓ５４４と同様の処理を行い更新したものである。

このようなステップＳ５４及びＳ５５の処理によれば、エピポーララインＥＰを十分に短く設定していることから、ステップＳ５４３における領域ベースマッチングでマッチングミスの発生する頻度が十分に低くなり物体の距離を一層正確に算出することができる。また、注目点Ｐｃは常に物体上の略同一点に設定されることから、幅方向（座標Ｘｗ方向）の移動を含めて物体の経時的な相対移動を示す相対速度ベクトルＶ_obst-relを正確に算出することができる。

この結果、トラック７０、乗用車７２及びガードレール７４の相対速度ベクトルＶ_obst-relであるベクトルＶｒ１、Ｖｒ２及びＶｒ３が、図２５に示すようにベクトル表示される。図２５から明らかなように、ベクトルＶｒ１からはトラック７０が左方向に向かって進路変更していることが判断され、ベクトルＶｒ２からは乗用車７２が前方に向かって車両１０よりもやや速い速度で進行していることが判断される。また、ベクトルＶｒ３からは静止物であるガードレール７４が車両１０の進行方向と逆方向に相対的に移動していることが判断される。なお、図２５における自車速度ベクトルＶ_VはステップＳ６及びＳ７において求められる。

次に、エピポーララインの設定方法について説明する。ステレオ３次元計測をするためには基準画像Ｉ_bに映る観測点Ｍ１の位置と参照画像Ｉ_rに映る観測点Ｍ１の位置をそれぞれ求める必要がある。図２７に示すように、観測点Ｍ１は基準画像Ｉ_b上の点ｍ_bに投影されており、点ｍ_bは観測点Ｍ１と基準カメラ１２の光学中心Ｏを通る直線Ｌｅ上に存在する。直線Ｌｅ上の点を探せば、観測点Ｍ１を見つけることができ、参照画像Ｉ_r上に投影される直線ＬｅがエピポーララインＥＰとなる。つまり、基準画像Ｉ_b上に投影された観測点Ｍ１を参照画像Ｉ_rと対応をつけるには、参照画像Ｉ_r中でのエピポーララインＥＰ上を探索すればよいこととなる。なお、図２７における点Ｍ０及びＭ２は、同一の点ｍ_bに投影される他の点であって直線Ｌｅ上の点である。

このエピポーラ拘束の関係式を求める。参照カメラ座標Ｘｒと基準カメラ座標Ｘｂの間には、前記の回転行列Ｒ及び並進ベクトルｔを用いて、Ｘｒ＝Ｒ・Ｘｂ＋ｔの関係が成り立つ。

このとき、基準カメラ１２の正規化画像座標Ｘ＾ｂａ、参照カメラ１４の正規化画像座標Ｘ＾ｒａ及び並進ベクトルｔは同一平面上に存在する。すなわち、スカラー三重積が０になるので、

となる。

ただし、

と表すことにする。ただし、ｔ１、ｔ２、ｔ３は並進ベクトルｔの要素を示す。ここで、正規化画像座標と画像座標との関係より、ｍ＾_b＝Ａ_b・Ｘ＾ｂａ、ｍ＾_r＝Ａ_rＸ＾ｒａを代入して整理すると、基礎行列（Ｆｕｎｄａｍｅｎｔａｌ Ｍａｔｒｉｘ）Ｆは次の（５−６）式及び（５−７）式で表される。

（５−７）式により、空間の観測点Ｍ１が基準画像Ｉ_bに投影される位置をｍとしたとき、ｌ’＝Ｆｍ＾とすると、ｍ＾’ｌ’＝０より、参照画像Ｉ_r中では、

となり、参照画像Ｉ_r中における観測点Ｍ１に対応したエピポーララインＥＰとなる。ここで、「’」は参照画像であることを示し、ｌ₁’，ｌ₂’，ｌ₃’は参照画像のエピポーララインＥＰのパラメータを示す。また、ｕ’、ｖ’は参照画像の画像座標を示す。

また、観測点Ｍ１の概略位置が既知である場合には、直線Ｌｅの長さを制限することによりエピポーララインＥＰを短く設定することができる。

参照画像Ｉ_rに投影される位置をｍ’としたとき、ｌ＝Ｆ^tｍ’とすると、ｌ^tｍ＝０より、参照画像Ｉ_r中では、

となり、観測点Ｍ１における基準画像Ｉ_bにおけるエピポーララインＥＰとなる。なお、このエピポーララインＥＰの説明で用いたパラメータｕ及びｕ’は、後述するベクトルｕ及びｕ’とは異なるものである。

＜ステップＳ６＞
このステップＳ６は、自車である車両１０の自車速度ベクトルＶ_V（図２５参照）を求めるために、道路平面Πを仮想的に上方から見たＶＰＰ画像を生成するための処理であり、図２８に示すステップＳ６１〜ステップＳ６３のサブルーチン処理からなる。すなわち、ステップＳ６１において、基準画像Ｉ_bをＶＰＰ画像に変換（以下、ＶＰＰ変換とも呼ぶ）するための変換式Ｈ_VPPを算出する。この後、ステップＳ６２において、求められた変換式Ｈ_VPPを用いて、前記基準画像Ｉ_bをＶＰＰ画像Ｉ_Vに変換し、記録する（図３３参照）。さらに、ステップＳ６３において、同じ変換式Ｈ_VPPを用いて前記平面領域Π_fをＶＰＰ変換して記録する（図３４参照）。以下、各ステップＳ６１〜Ｓ６３について詳細に説明する。

なお、ＶＰＰ画像Ｉ_Vを撮像する仮想的なカメラを想定し、説明上これを仮想カメラと呼ぶ。

先ず、図２９のステップＳ６１１において、基準カメラ１２の内部パラメータＡ_bの逆行列Ａ_b ^-1を読み込む。内部パラメータＡ_bは、次の（６−１）式で表される。

ここで、α₁、β₁、ｓ₁、Ｃ_X1及びＣ_Y1は、基準カメラ１２のキャリブレーションにより定まる定数である。

ステップＳ６１２において、道路平面Πに対する位置姿勢パラメータである法線ベクトルｎ及び距離ｄ（図１６参照）を読み込む。

ステップＳ６１３において、法線ベクトルｎ及び距離ｄを用いて、基準カメラ１２の光軸であるベクトルｅ_oz（図３０参照）を法線ベクトルｎに一致させる回転行列Ｒ_nを次の（６−２）式〜（６−５）式に基づいて算出する。

これらの（６−２）式〜（６−５）式を図３０も参照しながら説明すると、式中で縦二重線で囲まれた部分はノルムを示し、ベクトルｅ_ozは、カメラ座標系における光軸である座標Ｚｃ（図１６参照）の単位ベクトルを示す。また、ｃ_nは、法線ベクトルｎとベクトルｅ_ozに垂直な単位ベクトルであり、角度θ_nは法線ベクトルｎとベクトルｅ_OZとのなす角度である。なお、図３０におけるベクトルｅ’_OX、ｅ’_OY及びｅ’_OZは、仮想カメラ座標系を示す直交単位ベクトルであり、一転鎖線は車両１０の前後方向を示す。

ステップＳ６１４において、投影距離、スケール、画像中心を指定し、道路平面Πを上方から見た仮想カメラの内部パラメータＡ_VPPを設定する。内部パラメータＡ_VPPは、仮想カメラ座標上の画像を画像座標上に変換するための行列であり、次の（６−６）式で表される。

ここで、α_VPP＝ｄ_VPP・ｋ_X-VPP、β_VPP＝ｄ_VPP・ｋ_Y-VPPである。また、ｄ_vppは投影距離［ｍ］であり、ｋ_x-vpp及びｋ_y-vppはＶＰＰ画像Ｉ_VのＸ座標方向及びＹ座標方向のスケール［ｐｉｘｅｌ／ｍ］である。さらに、Ｃ_x-vpp及びＣ_y-vppはＶＰＰ画像Ｉ_V中心のＸ座標及びＹ座標である。

この内部パラメータＡ_VPPにより投影距離に道路平面のパラメータである距離ｄを指定すると、検出した道路平面Π上に投影される。

ステップＳ６１５において、車両前後方向とＶＰＰ画像の縦軸を一致させるために画像を角度θ_V及び角度−φだけ回転させる変換式である回転行列Ｒ_Vを算出する。すなわち、図３０及び（６−７）式に基づいて基準カメラ１２の光軸方向の単位ベクトルｅ_OZをＶＰＰカメラ座標系からみたベクトルｕを設定し、該ベクトルｕを仮想カメラ座標系におけるｅ’_OX、ｅ’_OY平面に正射影したベクトルｕ’を（６−８）式に基づいて設定する。

ベクトルｕ及びｕ’をこのように定義するとき、角度θ_Vはベクトルｕ’と仮想カメラ座標ｅ’_oyとのなす角度であり（６−９）式で表される。

また、角度φは車両前後方向に対するカメラ光軸であるベクトルｅ_OZとのなす角度であり予め求めておく。求める回転行列Ｒ_Vは、次の（６−１０）式で表される。

ステップＳ６１６において、Ａ_b ^-1、Ｒ_n、Ｒ_V及びＡ_VPPに基づいて、ＶＰＰ変換のための変換式Ｈ_VPPを算出する。変換式Ｈ_VPPは、次の（６−１１）式で表される。なお、Ｒ_VPP＝Ｒ_VＲ_nである。

つまり、変換式Ｈ_VPPでは、基準画像Ｉ_bを行列Ａ_b ^-1、Ｒ_n、Ｒ_V、及びＡ_VPPで順に変換することと等価な処理が行われる。実際には１つの変換式Ｈ_VPPで一度に変換が可能であるが、変換の過程を概念的な理解が容易となるように、各行列Ａ_b ^-1、Ｒ_n、Ｒ_V、及びＡ_VPPによる変換を説明する。

先ず、基準画像Ｉ_b（図５参照）に対して逆行列Ａ_b ^-1による変換を行うと画像座標からカメラ座標系への変換が行われ、図３１に示すカメラ座標系基準画像Ｉ_bcが得られる。

次に、このカメラ座標系基準画像Ｉ_bcに対して回転行列Ｒｎによる変換を行うと、カメラ座標系基準画像Ｉ_bcは角度θｎだけ回転して道路平面Πに平行になるとともに遠方部分が拡大され、第１変換画像Ｉ_V1が得られる。図３２に示すように、この第１変換画像Ｉ_V1は縦軸に対して角度（θ_V−φ）だけ傾いた画像として得られる。

さらに、第１変換画像Ｉ_V1に対して回転行列Ｒ_Vによる変換を行うと、第１変換画像Ｉ_V1は角度（θ_V−φ）だけ回転して車両前後方向とＶＰＰ画像の縦軸が一致することになり第２変換画像Ｉ_V2が得られる。

最後に、第２変換画像Ｉ_V2に対して仮想カメラの内部パラメータＡ_VPPによる変換を行うと、スケール変換が行われて所望の大きさのＶＰＰ画像Ｉ_V（図３３参照）が得られる。

このような一連の行列式Ａ_VPPＲ_VＲ_nＡ_b ^-1によって、変換式Ｈ_VPPが構成されており、基準画像Ｉ_bに対して変換式Ｈ_VPPによるＶＰＰ変換を行うことによりＶＰＰ画像Ｉ_V（図３３参照）が得られる。前記のとおり、このＶＰＰ変換は、ステップＳ６２において行われる。得られたＶＰＰ画像Ｉ_Vは記憶部に記録される。

なお、このＶＰＰ画像Ｉ_Vは、内部パラメータＡ_VPPの設定により、図３３に示すように画像の左上部が原点であって、車幅方向がＸ方向（図３３における右方向）、車両前後方向がＹ方向（図３３における下方向）として規定される。

図３３から了解されるように、ＶＰＰ画像Ｉ_Vは、道路平面Πを上方の高い距離から眺めたような画像となり、道路の幅は略一定に映し出される。また、基準画像Ｉ_b上のトラック７０や乗用車７２は、画像上方部が拡大するように歪曲して映し出される。

また、この後ステップＳ６３においては、平面領域Π_f（図５参照）に対して変換式Ｈ_VPPによるＶＰＰ変換を行い、上方から眺めた平面領域Π_fであるＶＰＰ平面画像Ｉ_Vf（図３４参照）を求めて記録する。このＶＰＰ平面画像Ｉ_VfはＶＰＰ画像Ｉ_Vにおける道路平面Πの領域を示すことになる。得られたＶＰＰ平面画像Ｉ_Vfは記憶部に記憶される。このＶＰＰ平面画像Ｉ_Vfは、道路平面と同じ形状となっており、図３４に示す例では、前方の他のトラック７０や路側部等の水平でない構造物や、道路平面と同一レベルでない面が除外された略Ｖ字形状となっている。

＜ステップＳ７＞
ステップＳ７は、前記ステップＳ６で求めたＶＰＰ画像Ｉ_V及びＶＰＰ平面画像Ｉ_Vfに基づいて、車両１０の自車速度ベクトルＶ_V（図２５参照）を求めるための処理であり、図３５に示すステップＳ７１〜ステップＳ７７のサブルーチン処理からなる。なお、以下の説明においては、現時刻の処理で得られたＶＰＰ画像Ｉ_V及びＶＰＰ平面画像Ｉ_VfをＶＰＰ画像Ｉ_V（ｔ）及びＶＰＰ平面画像Ｉ_Vf（ｔ）と表し、前時刻に得られたものをＶＰＰ画像Ｉ_V（ｔ−１）及びＶＰＰ平面画像Ｉ_Vf（ｔ−１）と表して区別する。また、ワールド座標系では車両１０の前後方向が座標Ｚｗ（図１０参照）として規定されているが、前記のとおり、ＶＰＰ画像Ｉ_Vでは前後方向がＹ軸と規定されているため、以下のステップＳ７に関する説明では車両１０の前後方向に対応するパラメータには添え字「Ｙ」又は「ｙ」を付けて表す。

ステップＳ７１において、前時刻処理時のＶＰＰ画像Ｉ_V（ｔ−１）、ＶＰＰ平面画像Ｉ_Vf（ｔ−１）及び画像マッチング移動量ΔＸ₀、ΔＹ₀、Δθ_yaw-0を呼び出す。画像マッチング移動量ΔＸ₀、ΔＹ₀、Δθ_yaw-0は、前時刻処理時のステップＳ７７で記録されたパラメータであり、前時刻におけるＶＰＰ画像Ｉ_V（ｔ）とＶＰＰ画像Ｉ_V（ｔ−１）とのマッチングによるずれ量を表す。

ステップＳ７２において、ＶＰＰ画像Ｉ_V（ｔ）とＶＰＰ画像Ｉ_V（ｔ−１）とのマッチングを行うための探索範囲を設定する。具体的には、画像マッチング移動量ΔＸ₀、ΔＹ₀、Δθ_yaw-0を中心とした所定幅を有する範囲としてΔＸ＝ΔＸ_MIN〜ΔＸ_MAX、及びΔＹ＝ΔＹ_MIN〜ΔＹ_MAX、Δθ_yaw＝Δθ_yaw-MIN〜Δθ_yaw-MAXが設定される。ステップＳ７３において、ＶＰＰ画像Ｉ_V（ｔ）とＶＰＰ画像Ｉ_V（ｔ−１）とを前記ステップＳ７２で設定した探索範囲内で相対的に移動させて、ＶＰＰ画像マッチングを行う。探索範囲は画像マッチング移動量ΔＸ₀、ΔＹ₀、Δθ_yaw-0を基準とした範囲に設定されているため、ＶＰＰ画像マッチングに要する時間は短時間で足りる。また、自車速度ベクトルＶ_Vは急激に変化することはないため、類似度が最大となる箇所が画像マッチング移動量ΔＸ₀、ΔＹ₀、Δθ_yaw-0を中心とした探索範囲からずれることはない。ステップＳ７３におけるＶＰＰ画像マッチングの詳細な処理については後述する（図３６参照）。

ステップＳ７４において、前記ステップＳ７３で求めたＶＰＰ画像マッチングの結果を参照して、ＶＰＰ画像Ｉ_V（ｔ）とＶＰＰ画像Ｉ_V（ｔ−１）との類似度が最大となる箇所（後述するＮＳＳＤ（ΔＸ、ΔＹ、Δθ_yaw）が最小となる箇所）を検出し、その箇所におけるＶＰＰ画像Ｉ_V（ｔ）とＶＰＰ画像Ｉ_V（ｔ−１）とのずれ量を移動量ｍｏｖｅ（ΔＸ_f、ΔＹ_f、Δθ_yaw-f）として求める。ここで、ΔＸ_f、ΔＹ_f及びΔθ_yaw-fはＶＰＰ画像Ｉ_V（ｔ）とＶＰＰ画像Ｉ_V（ｔ−１）との類似度が最大となる箇所におけるΔＸ、ΔＹ及びΔθ_yawの値である。

ステップＳ７５において、移動量ｍｏｖｅ（ΔＸ_f、ΔＹ_f）に基づいて下記の（７−１）〜（７−３）式により車両１０の自車速度ベクトルＶ_Vの大きさを求める。

ここで、Ｖｘ及びＶｚは、車両１０の幅方向及び車両前後方向の速度であり、ｄ_vpp、α_VPP及びβ_VPPは前記（６−６）式において定義される係数及び数値である。また、前記のとおりΔｔは演算周期である。（７−２）式及び（７−３）式で、車両前後方向の速度をＶｚとして添え字「ｙ」でなく「ｚ」を用いているのは、他の処理に対して整合性を考慮しＶＰＰ画像の座標からワールド座標系に戻しているためである。

自車速度ベクトルＶ_Vの大きさは、周知のとおり直交する２方向速度成分で形成される斜辺部の長さとして表されることから（７−３）式が成り立つ。

（７−３）式及び（７−４）式により、自車速度ベクトルＶ_Vの大きさと角度θ_YAWが求まることから自車速度ベクトルＶ_Vが決定され、該自車速度ベクトルＶ_Vを図２５上に表す。なお、角度θ_YAWは、車両１０の回転角度として用いることもできる。

ところで、車両１０の回転角度をより厳密に求めるためには、ＶＰＰ画像Ｉ_V（ｔ−１）をΔＸ及びΔＹ方向に並進的に移動させるだけではなく、回転を含んだ移動をさせた上で類似度の演算を行うとよい。この場合、画像マッチング移動量ΔＸ₀、ΔＹ₀を中心とした所定幅を有する範囲としてΔＸ＝ΔＸ_MIN〜ΔＸ_MAX、ΔＹ＝ΔＹ_MIN〜ΔＹ_MAX及び回転角度としてのω＝Δθ_MIN〜Δθ_MAXが設定され、３自由度の移動を行う。

ステップＳ７６において、自車速度ベクトルＶ_V、角度θ_YAWを記録する。また、ステップＳ７４で求めたΔＸ_f、ΔＹ_f、Δθ_yaw-fを次回処理時の画像マッチング移動量ΔＸ₀、ΔＹ₀、Δθ_yaw-0として用いるために、ΔＸ₀←ΔＸ_f、ΔＹ₀←ΔＹ_f、Δθ_yaw-0←Δθ_yaw-fと代入した後に記録する。

次に、ステップＳ７３におけるＶＰＰ画像マッチングの第１の処理方法について説明する。この第１の処理方法は図３６におけるステップＳ７３１〜Ｓ７３５の処理からなる。なお、このステップＳ７３１〜Ｓ７３５の処理は、前記ステップＳ７２で設定された探索範囲としてのΔＸ＝ΔＸ_MIN〜ΔＸ_MAX、及びΔＹ＝ΔＹ_MIN〜ΔＹ_MAX、Δθ_yaw＝Δθ_yaw-MIN〜Δθ_yaw-MAXの領域について個別に実行されるが、繰り返し制御についての説明を省略して１回分の処理についてのみ説明する。

先ず、ステップＳ７３１において、ＶＰＰ平面画像Ｉ_Vf（ｔ−１）を現時刻で設定されているΔθ_yaw回転し、ΔＸ及びΔＹの方向に移動させる。（図３７の「Ｓ７３１」参照）。なお、回転の中心は、ＶＰＰ画像における光学中心の位置（Ｃ_x-vpp、Ｃ_y-vpp）とする。

ステップＳ７３２において、ＶＰＰ平面画像Ｉ_Vf（ｔ）と前記ステップＳ７３１で移動したＶＰＰ平面画像Ｉ_Vf（ｔ−１）との共通領域Ｒ（ΔＸ、ΔＹ，Δθ_yaw）を求める（図３７の「Ｓ７３２」参照）。

ステップＳ７３３において、ＶＰＰ画像Ｉ_V（ｔ−１）をその時点で設定されているΔθ_yaw回転し、ΔＸ及びΔＹの方向に移動させる（図３７の「Ｓ７３３」参照）。

ステップＳ７３４において、共通領域Ｒ（ΔＸ，ΔＹ，Δθ_yaw）、ＶＰＰ画像Ｉ_V（ｔ）及び移動したＶＰＰ画像Ｉ_V（ｔ−１）に基づいて、次の（７−４）式によりＶＰＰ画像マッチングとしてＮＳＳＤを計算する。

ここで、ｘｉは共通領域Ｒ（ΔＸ，ΔＹ，Δθ_yaw）に含まれるピクセルの座標であり
、Ｎは、共通領域Ｒ（ΔＸ，ΔＹ，Δθ_yaw）に含まれるピクセルの総数であり、Ｉ_V（ｔ−１）ｍｏｖｅ（ΔＸ，ΔＹ）は、ステップＳ７３４でＶＰＰ画像Ｉ_V（ｔ−１）をΔθ_yaw回転し、ΔＸ及びΔＹだけ移動した画像を示す。

この（７−４）式により、ＶＰＰ画像Ｉ_V（ｔ）とＶＰＰ画像Ｉ_V（ｔ−１）をΔθ_yaw回転し、ΔＸ及びΔＹだけ移動した画像とのピクセル毎の輝度差の二乗和が正規化された値として求められ、次のステップＳ７３５において、移動量ｍｏｖｅ（Δｘ、Δｙ、Δθ_yaw）毎に類似度としてＮＳＳＤ（Δｘ、Δｙ、Δθ_yaw）が記録される。ＮＳＳＤ（Δｘ、Δｙ、Δθ_yaw）は正規化されていることから共通領域Ｒ（ΔＸ，ΔＹ，Δθ_yaw）の面積の大小に拘わらずに相互比較が可能となる。

このようなステップＳ７の処理によれば、対応点探索を行うことがないため、ロバスト性が高い。また平面の位置姿勢パラメータに基づいて平面に平行なＶＰＰ画像を生成するため、車両姿勢の変動等による影響を受けず耐振動性が高い。実際上、道路平面Πには不規則であっても地模様等何らかの模様や特徴部が存在していることから、このような部分に基づいて自車速度ベクトルＶ_Vを求めることができる。

さらに、平面領域Π_fに基づく共通領域Ｒ（ΔＸ，ΔＹ，Δθ_yaw）内でＶＰＰ画像マッチングを行うことから、他の車両や路側部等の余分な部分が削除されて高精度且つ高速な演算が遂行される。

また、ＶＰＰ画像マッチングにおける類似度として、ＮＳＳＤに代えて前記のＺＮＣＣ等を用いてもよい。

次に、ステップＳ７３におけるＶＰＰ画像マッチングの第２の処理方法について説明する。この第２の処理方法は、基本的には前記ステップＳ１及び前記非特許文献１で説明した濃度勾配法と同じ方法であり、前記射影変換行列Ｈに相当する移動回転行列Ｍを微小量変化させながら２つの画像が最も重なるように収束させる。

先ず、適当に真値に近い初期値としての移動回転行列Ｍを設定しておき、次の（７−５）式により、ＶＰＰ画像Ｉ_V（ｔ−１）を移動回転変換する。ここで、Ｉ_r（ｘ）をＶＰＰ画像Ｉ_V（ｔ−１）の座標ｘ（ｘ，ｙ）における輝度値とする。なお、移動回転行列Ｍは、３×３の行列である。また、移動回転行列Ｍの初期値としては、前時刻のステップにて算出された値を用いるとよい。

（「〜」は、変換された後の輝度値を表す。）

移動回転行列Ｍの各パラメータを微小量変化させたとき、変換されたＶＰＰ画像Ｉ_V（ｔ−１）上の座標ｘ（ｘ，ｙ）が座標ｘ″（ｘ″，ｙ″）に変化するものとすると、

の関係が得られる。なお、Ｉは３×３の単位行列であり、Ｄは移動回転行列Ｍの各パラメータの微小変化量をパラメータとする３×３の微小変化行列であり次の（７−７）式で表される。

ここで、ωは微小回転角度、ｄｘ及びｄｙはｘ方向及びｙ方向の微小移動量である。また、ω、ｄｘ及びｄｙをまとめて１つのパラメータΘとしても表す。このパラメータΘを引数とし、（７−５）、（７−６）式の関係を用いて、移動回転変換されたＶＰＰ画像Ｉ_V（ｔ−１）とＶＰＰ画像Ｉ_V（ｔ）との重なり状態を示す評価関数Ｅ（Θ）は、次の（７−８）式で表される。

（「ｉ」は、画素番号を表す。）
と表すことができる。ここで、Ｒ_planeは、ＶＰＰ平面画像Ｉ_Vf（ｔ−１）、Ｉ_Vf（ｔ）における道路平面Πに相当する領域である。

この（７−８）式は前記（１−５）式と等価なものであり、基準画像Ｉ_bとＶＰＰ画像Ｉ_V（ｔ）、参照画像Ｉ_rとＶＰＰ画像Ｉ_V（ｔ−１）、行列Ｄｘと行列Ｄがそれぞれ対応する。

したがって、前記と同様の方法により、ｇ_i（輝度勾配）、ｅ_i（輝度差分）、ヤコビアンＪΘ_iを算出し、微小変化行列Ｄを最小二乗法を用いて求めることができる。

以上のようにして求めた微小変化行列Ｄを用いて、移動回転行列Ｍを次の（７−９）式にしたがって更新する。

そして、（７−９）式で求めた移動回転行列Ｍを用いて、再び（７−８）式の評価関数Ｅ（Θ）の値を算出する処理を繰り返す。この繰り返しの処理において移動回転行列Ｍは最小二乗法を解いて得られる微小変化行列Ｄを用いて、Ｍ←Ｍ（Ｉ＋Ｄ）として更新される。評価関数Ｅ（Θ）の値が所定の値以下に収束したときのをＭの各成分よりΔθ_yaw、ΔＸ、ΔＹを式（７−１０）を用いて求めることができる。

このようなステップＳ７３の第２の処理方法では、前記の第１の処理方法で求めたΔθ_yaw、ΔＸ、ΔＹを初期値として第２の処理方法を用いることで、一層正確にΔθ_yaw、ΔＸ、ΔＹを求めることができる。

＜ステップＳ８＞
ステップＳ８は、検出した物体が静止物体であるか移動物体であるかを判断する処理であり、図３８に示すステップＳ８１〜Ｓ８４のサブルーチン処理からなる。

ステップＳ８１において、前記ステップＳ５５で求められた各物体の相対速度であるベクトルＶｒ１、Ｖｒ２及びＶｒ３を読み出す。これらのベクトルは図２５における各矢印で示されている。

ステップＳ８２において、前記ステップＳ７において求めた車両１０の自車速度ベクトルＶ_Vを読み出す。この車両１０の自車速度ベクトルＶ_Vは図２５の原点を基点に示されている。

ステップＳ８３において、検出された各物体の絶対速度ベクトルＶ_ABSＮ（Ｎ＝１、２、３…）を算出する。絶対速度Ｖ_ABSＮはＶ_ABSＮ＝Ｖ_rＮ＋Ｖ_V'としてベクトル加算により求められる。ただし、Ｖ_V'は相対速度Ｖ_rＮに含まれる、自車両のモーションによる速度分である。図３９に各物体毎に求められた絶対速度ベクトルＶ_ABSＮを示す。図３９から明らかなように、静止物体であるガードレール７４については、Ｖ_ABS３≒０となる。

ステップＳ８４において、絶対速度Ｖ_ABSＮに基づいて、検出された各物体が静止物体であるか移動物体であるかを調べる。具体的には、各物体の絶対速度のベクトルの大きさ｜Ｖ_ABSＮ｜と閾値Ｔｈ_ABSを比較して、Ｔｈ_ABS≧｜Ｖ_ABSＮ｜であるとき対象となる物体は静止物体であり、｜Ｖ_ABSＮ｜＞Ｔｈ_ABSであるときは移動物体であると判断される。これにより、Ｖ_ABS３≒０であるガードレール７４は静止物体として判別され、トラック７０及び乗用車７２は移動物体として識別される。

このように、ステップＳ８の処理によれば、検出された物体の絶対速度を精度よく求めることができ、物体が静止物か否かを正確に判断することができる。この判断結果の情報は、例えば、車両１０と物体との所定の接近注意喚起手段に供することができる。

また、ステップＳ７及びＳ８の処理によれば、ＶＰＰ画像Ｉ_Vの平面領域Π_fのみを用いて移動量を算出することにより、移動する物体と静止物とが混在する環境下においても正確に自車速度ベクトルＶ_Vを求めることができる。

さらに、例えば路面など一定の高さ（距離）にある物体を撮像して、路面を基準として速度計測を行うように定めた場合に、路面に対して高さのある物体が撮像され、該物を基準として速度計測を行うと高さに基づく速度検出の誤差が発生することが知られているが、ステップＳ７及びＳ８の処理によれば、平面領域Π_fのみを用いることにより高さのある物体を除外して算出できるので、高さに基づく誤差が発生しない。

またステップＳ７及びＳ８の処理によれば、平面までの距離も検出するため、例えば歩道など路面に対して高さのある平面が画像中の大半を占める場合にも、その高さを考慮して速度を算出できる。

なお、本実施の形態において求められる平面領域Π_fは、車両１０が走行する略水平な道路平面Πに対応する領域であって、「平面」であっても垂直な面やレベルの異なる他の面は除外される。

また、上記の説明では、基準カメラ１２と参照カメラ１４が設けられているものとしたが、必要に応じてより多くのカメラを設けてもよい。

本発明の平面検出装置及び検出方法が適用される車両の説明図である。 図１に示す車両に搭載される演算処理装置の機能構成ブロック図である。 図１に示す演算処理装置における処理の全体の流れを説明するフローチャートである。 一対の撮像手段間での２次元射影変換を説明するための模式図である。 平面領域を抽出する手順の説明図である。 物体抽出処理の処理内容を示すフローチャートである。 候補点を高さの制限に基づいて抽出する様子を示す模式図である。 投票処理の処理の様子を示す模式図である。 投票面上で行うクラスタリング処理の様子を示す模式図である。 投票面を示す図である。 物体の各エッジ点のワールド座標を求める処理内容を示すフローチャートである。 エッジが抽出されたエッジ画像を示す図である。 エッジが抽出されたエッジ画像の実際の画像データである。 平面領域が除去された修正エッジ画像を示す図である。 エピポーララインが設定された参照画像を示す図である。 ワールド座標系とカメラ座標系との関係を示す模式図である。 奥行き画像を示す図である。 物体の相対速度を算出する処理内容を示すフローチャートである。 物体領域の重なり具合を調べる処理内容を示すフローチャートである。 クラスタに基づいてテンプレートを作成する処理内容を示す模式図である。 テンプレートが設定された基準画像を示す図である。 重心移動距離を算出する処理内容を示すフローチャートである。 物体同一判定の処理内容を示すフローチャートである。 同一物体の位置を再計算する処理内容を示すフローチャートである。 物体の相対速度が記録された投票面を示す図である。 物体の相対速度を算出する処理内容を示すフローチャートである。 エピポーララインを設定する処理を説明するための模式図である。 ＶＰＰ画像を生成するための処理内容を示すフローチャートである。 ＶＰＰ画像変換式を算出する処理内容を示すフローチャートである。 法線ベクトル、カメラ座標系、及び基準カメラの光軸との関係を示す座標図である。 カメラ座標系基準画像を道路平面に平行となるように変換する処理の説明のための図である。 第１変換画像を回転変換する様子を示す模式図である。 基準画像をＶＰＰ変換する様子を示す模式図である。 道路平面領域をＶＰＰ変換する様子を示す模式図である。 自車速度を算出する処理内容を示すフローチャートである。 ＶＰＰ画像マッチングの処理内容を示すフローチャートである。 ＶＰＰ画像マッチングの処理内容を示す模式ブロック図である。 物体の絶対速度を算出するための処理内容を示す模式ブロック図である。 物体の絶対速度が記録された投票面を示す図である。

符号の説明

１０…車両 １２…基準カメラ
１４…参照カメラ １６…演算処理装置
１８、２０…画像処理部 ２２…画像記憶部
２４…演算制御部 ２６…パラメータ記憶部
２８…射影変換行列算出部 ３０…平面パラメータ算出部
３２…平面領域抽出部 ３４…物体検出部
３６…物体相対速度算出部 ３８…仮想投影面画像生成部
４０…自車速度算出部 ４２…物体絶対速度算出部
４４…表示処理部 ５０…投票面
５２ａ〜５２ｃ、５４ａ〜５４ｃ…クラスタ
６０…エッジ画像 ６２…修正エッジ画像
８０ａ〜８０ｃ…テンプレート ８２ａ…物体位置領域
Ａ_b、Ａ_r…内部パラメータ Ｃｃ…カメラ座標
Ｃｗ…ワールド座標 ＥＰ…エピポーラライン
Ｈ…射影変換行列 Ｉ_b…基準画像
Ｉ_r…参照画像 Π…平面
ｄ…距離 ｎ…法線ベクトル
ｔ…並進ベクトル

Claims (9)

1. 撮像手段により撮像した画像から平面を検出する平面検出装置であって、
   所定の位置関係に配置され、平面を含む相互に重複する領域の画像を撮像する一対の撮像手段と、
   前記一対の撮像手段の一方の座標系を他方の座標系に変換する回転行列及び並進ベクトルをパラメータとして記憶するパラメータ記憶手段と、
   前記回転行列及び前記並進ベクトルを用いて、一方の前記撮像手段により撮像された画像を変換し、前記画像に含まれる平面を他方の前記撮像手段により撮像された画像に含まれる平面に重畳させる射影変換行列を算出する射影変換行列算出手段と、
   を備え、前記射影変換行列を用いて平面を検出することを特徴とする平面検出装置。
2. 請求項１記載の装置において、
   前記平面の法線ベクトルと、基準とする一方の前記撮像手段から前記平面までの距離とからなる平面パラメータを算出する平面パラメータ算出手段を備えることを特徴とする平面検出装置。
3. 請求項１記載の装置において、
   前記撮像手段は、移動体に固定されることを特徴とする平面検出装置。
4. 撮像手段により撮像した画像から平面を検出する平面検出方法であって、
   所定の位置関係に配置された一対の撮像手段を用いて、平面を含む相互に重複する領域の画像を撮像する第１ステップと、
   一方の前記撮像手段の座標系を他方の前記撮像手段の座標系に変換する回転行列及び並進ベクトルを用いて、一方の前記撮像手段により撮像された画像を変換し、前記画像に含まれる平面を他方の前記撮像手段により撮像された画像に含まれる平面に重畳させる射影変換行列を算出する第２ステップと、
   を有し、前記射影変換行列を用いて平面を検出することを特徴とする平面検出方法。
5. 請求項４記載の方法において、
   前記射影変換行列Ｈは、前記回転行列Ｒ、前記並進ベクトルｔ、前記一対の撮像手段の内部パラメータＡ_b、Ａ_r、前記平面の法線ベクトルｎ、基準とする一方の前記撮像手段から前記平面までの距離ｄとを用いて、
   

   

   （但し、ｋは、０でない係数）
   と定義し、前記法線ベクトルｎ及び前記距離ｄを変化させ、前記一対の撮像手段により撮像された画像同士が最も重畳されるときの前記射影変換行列Ｈとして算出することを特徴とする平面検出方法。
6. 撮像手段により撮像した画像から平面を検出する平面検出装置であって、
   所定の位置関係に配置され、平面を含む相互に重複する領域の画像を撮像する一対の撮像手段と、
   前記一対の撮像手段の一方の座標系を他方の座標系に変換する回転行列及び並進ベクトルと、前記一対の撮像手段の内部パラメータと、平面の法線ベクトルの初期値と、基準とする一方の前記撮像手段から平面までの距離の初期値とをパラメータとして記憶するパラメータ記憶手段と、
   前記回転行列、前記並進ベクトル及び前記内部パラメータを用いて、前記法線ベクトルの初期値及び前記距離の初期値を濃度勾配法により変化させ、前記一対の撮像手段によって撮像されたステレオ画像に含まれる平面を重畳させる射影変換行列を推定する射影変換行列推定手段と、
   を備え、前記射影変換行列を用いて平面を検出することを特徴とする平面検出装置。
7. 請求項６記載の装置において、
   平面の前記法線ベクトルと、基準とする一方の前記撮像手段から平面までの前記距離とからなる平面パラメータを推定する平面パラメータ推定手段を備え、
   前記平面パラメータ推定手段は、前記パラメータ記憶手段に記憶された前記法線ベクトルの初期値及び前記距離の初期値に対して、前記射影変換行列推定手段によって前記射影変換行列を推定する際に変化した前記法線ベクトルの変化量と、前記距離の変化量とを用いて前記平面パラメータを推定することを特徴とする平面検出装置。
8. 請求項６記載の装置において、
   前記法線ベクトルの初期値及び前記距離の初期値は、前記一対の撮像手段が固定される移動体が停止している状態での初期値であることを特徴とする平面検出装置。
9. 請求項６記載の装置において、
   前記平面パラメータ推定手段により推定された前記平面パラメータを用いて、前記平面の空間位置を算出する空間位置算出手段を備えることを特徴とする平面検出装置。
   

Images