平面検出装置及び検出方法
【課題】平面を検出するための画像間の射影変換行列を高精度且つ短時間で算出する。
【解決手段】基準カメラ１２及び参照カメラ１４間の回転行列及び並進ベクトルをパラメータ記憶部２６から読み出し、射影変換行列算出部２８において、前記回転行列及び前記並進ベクトルを用いて、基準カメラ１２及び参照カメラ１４によって撮像された画像に含まれる道路平面領域を重畳させることのできる射影変換行列を算出する。
【解決手段】基準カメラ１２及び参照カメラ１４間の回転行列及び並進ベクトルをパラメータ記憶部２６から読み出し、射影変換行列算出部２８において、前記回転行列及び前記並進ベクトルを用いて、基準カメラ１２及び参照カメラ１４によって撮像された画像に含まれる道路平面領域を重畳させることのできる射影変換行列を算出する。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、例えば、自動車の運転者支援や自律移動車の自動運転を実現するため、これらの移動体に搭載した一対の撮像手段により撮像した画像から、前記移動体が走行可能な平面を検出する平面検出装置及び検出方法に関する。
【背景技術】
【０００２】
自律移動車等の移動体の誘導において、走行可能な道路平面領域を検出し、あるいは、走行環境に存在する障害物を検出する方法は、レーザレーダ、超音波、ミリ波レーダ等を利用する方法と、画像を利用する方法とに大別することができる。
【０００３】
レーザレーダやミリ波レーダを用いる検出方法では、一般に装置が高価であり、また、十分な空間分解能が得られないという問題がある。また、超音波を用いる検出方法では、遠くの計測が困難であって、空間分解能も低いという問題がある。
【０００４】
一方、画像を用いる方法では、単眼による方法と、複眼を用いる方法とに分けることができる。従来、画像を用いる方法の多くは単眼、すなわち、１つの視点から得られる画像を用いるもので、主に、高速道路等の整備された環境で用いることを想定し、路面の白線（例えば分離線や中央線）等を検出することにより、走行領域を検出している。しかしながら、白線等の存在が保証されず、また路面の色やパターンがさまざまであるような一般道や駐車場等では、単眼画像から得られる濃度パターンのみから、走行可能領域と障害物を安定に区別することは困難である。
【０００５】
これに対して、複眼、すなわち、ステレオ画像を利用する検出方法では、原理的には環境の３次元構造を利用できるため、より安定に走行環境を認識できる可能性がある。特に、走行可能な領域は空間中でほぼ平面であるとみなせることから、画像間の射影変換行列を算出し、この射影変換行列により一方の画像を２次元射影変換して他方の画像に重畳させ、画像同士の重なり具合から、平面領域か否か、あるいは、障害物か否かを検出する方法が提案されている（特許文献１、非特許文献１参照）。
【０００６】
【特許文献１】特開２０００−２９３６９３号公報
【非特許文献１】ホイング−ヤング・シャム（Ｈｅｕｎｇ‐Ｙｅｕｎｇ Ｓｈｕｍ）、リチャード・セーリスキ（Ｒｉｃｈａｒｄ Ｓｚｅｌｉｓｋｉ）、「“パノラミック・イメージ・モザイク”テクニカル・レポート（“Ｐａｎｏｒａｍｉｃ Ｉｍａｇｅ Ｍｏｓａｉｃｓ”Ｔｅｃｈｎｉｃａｌ ｒｅｐｏｒｔ）」、１９９７年、エム・エス・アール−テイー・アール−９７−２３、マイクロソフト・リサーチ（ＭＳＲ−ＴＲ−９７‐２３，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が既知であるとすると、次の（１−１０）式が成立する。
【００６５】
【数１０】
【００６６】
そこで、射影変換行列Ｈ′を特異値分解する。Ｕ、Ｖを正規直交行列、Σを射影変換行列Ｈ′の特異値σ1〜σ3（σ1≧σ2≧σ3＞０）を要素とする対角行列として、射影変換行列Ｈ′は、
【００６７】
【数１１】
と書き替えることができる。なお、
【数１２】
である。
【００６８】
（１−１０）、（１−１１）式から、
【数１３】
となる。
【００６９】
ここで、
【数１４】
とすると、次の（１−１５）式が得られる。
【００７０】
【数１５】
【００７１】
基準カメラ座標系の基底ベクトル（ｅ1，ｅ2，ｅ3）を導入し、
【数１６】
【００７２】
とすると、（１−１５）式から得られる３つのベクトル方程式と、ｎ、ｎ′が単位ベクトル、Ｖが正規直交ベクトル、Ｒ′がノルム変化のない回転行列であることから、次の（１−１７）式が得られる。
【００７３】
【数１７】
（１−１７）式をｎ1′2、ｎ2′2、ｎ3′2の連立方程式と考えれば、０でない解を持つには、行列式が０であるから、
【数１８】
となる。
【００７４】
そこで、射影変換行列Ｈ′の特異値σ1〜σ3が、
（I） 重解を持たない場合（σ1≠σ2≠σ3）
（II） 重解を持つ場合（σ1＝σ2≠σ3 又は σ1≠σ2＝σ3）
（III） ３重解を持つ場合（σ1＝σ2＝σ3）
に場合分けして考える。（I）及び（II）の場合は、ｋ＝±σ1若しくはｋ＝±σ3とすると、（３−９）式から算出されるｎ′が単位ベクトルであることに矛盾する。したがって、いずれの場合でもｋ＝±σ2として求められることになる。また、ｋ＝−σ2の場合は２台のカメラ（基準カメラ１２及び参照カメラ１４）が平面を挟むような配置となるため除外され、ｋはσ2として求められることになる。なお、（III）の３重解の場合は、２台の基準カメラ１２及び参照カメラ１４の光学中心が一致し、回転のみの場合であるため、除外する。
【００７５】
（I）及び（II）の場合、（１−１７）式より、ｎ′は、
【数１９】
として求まる。
【００７６】
σ1≠σ2≠σ3のとき、ｎ2′＝０であるから、
【数２０】
を得る。すなわち、Ｒ′は、ｅ2の回りの回転行列である。したがって、次の（１−２１）、（１−２２）、（１−２３）式を得る。
【００７７】
【数２１】
【００７８】
【数２２】
【００７９】
【数２３】
（II）の場合、σ1＝σ2（又は、σ2＝σ3）として、（１−２１）式から、
【数２４】
となり、
【数２５】
【数２６】
となる。
【００８０】
ここで、（１−１９）式におけるε1、ε2の符号の違いにより、４つの解の候補が存在する。この４つの候補のうち、「平面上の点が画像に映る」条件を満たすｎ′を選択することで２つの候補に絞り込む。さらに、基準カメラ１２及び参照カメラ１４の位置関係が撮影中に変化しない条件を加え、解を一意に決定する。
【００８１】
以上のようにして、（１−２１）式で求めたＲ′を（１−１４）式に代入することにより、回転行列Ｒが求められる。また、（１−２３）又は（１−２６）式で求めたｔ′を（１−１４）式に代入してｔ／ｄを求め、ｔ／ｄを正規化し、並進ベクトルｔの単位ベクトルｔ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中における観測点Ｍ１に対応したエピポーララインＥＰとなる。ここで、「’」は参照画像であることを示し、ｌ1’，ｌ2’，ｌ3’は参照画像のエピポーララインＥＰのパラメータを示す。また、ｕ’、ｖ’は参照画像の画像座標を示す。
【０１９６】
また、観測点Ｍ１の概略位置が既知である場合には、直線Ｌｅの長さを制限することによりエピポーララインＥＰを短く設定することができる。
【０１９７】
参照画像Ｉrに投影される位置をｍ’としたとき、ｌ＝Ｆtｍ’とすると、ｌtｍ＝０より、参照画像Ｉr中では、
【数４９】
となり、観測点Ｍ１における基準画像ＩbにおけるエピポーララインＥＰとなる。なお、このエピポーララインＥＰの説明で用いたパラメータｕ及びｕ’は、後述するベクトルｕ及びｕ’とは異なるものである。
【０１９８】
＜ステップＳ６＞
このステップＳ６は、自車である車両１０の自車速度ベクトルＶV（図２５参照）を求めるために、道路平面Πを仮想的に上方から見たＶＰＰ画像を生成するための処理であり、図２８に示すステップＳ６１〜ステップＳ６３のサブルーチン処理からなる。すなわち、ステップＳ６１において、基準画像ＩbをＶＰＰ画像に変換（以下、ＶＰＰ変換とも呼ぶ）するための変換式ＨVPPを算出する。この後、ステップＳ６２において、求められた変換式ＨVPPを用いて、前記基準画像ＩbをＶＰＰ画像ＩVに変換し、記録する（図３３参照）。さらに、ステップＳ６３において、同じ変換式ＨVPPを用いて前記平面領域ΠfをＶＰＰ変換して記録する（図３４参照）。以下、各ステップＳ６１〜Ｓ６３について詳細に説明する。
【０１９９】
なお、ＶＰＰ画像ＩVを撮像する仮想的なカメラを想定し、説明上これを仮想カメラと呼ぶ。
【０２００】
先ず、図２９のステップＳ６１１において、基準カメラ１２の内部パラメータＡbの逆行列Ａb-1を読み込む。内部パラメータＡbは、次の（６−１）式で表される。
【０２０１】
【数５０】
【０２０２】
ここで、α1、β1、ｓ1、Ｃ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（ｔ−１）及び画像マッチング移動量ΔＸ0、ΔＹ0、Δθyaw-0を呼び出す。画像マッチング移動量ΔＸ0、ΔＹ0、Δθyaw-0は、前時刻処理時のステップＳ７７で記録されたパラメータであり、前時刻におけるＶＰＰ画像ＩV（ｔ）とＶＰＰ画像ＩV（ｔ−１）とのマッチングによるずれ量を表す。
【０２３３】
ステップＳ７２において、ＶＰＰ画像ＩV（ｔ）とＶＰＰ画像ＩV（ｔ−１）とのマッチングを行うための探索範囲を設定する。具体的には、画像マッチング移動量ΔＸ0、ΔＹ0、Δθyaw-0を中心とした所定幅を有する範囲としてΔＸ＝ΔＸMIN〜ΔＸMAX、及びΔＹ＝ΔＹMIN〜ΔＹMAX、Δθyaw＝Δθyaw-MIN〜Δθyaw-MAXが設定される。ステップＳ７３において、ＶＰＰ画像ＩV（ｔ）とＶＰＰ画像ＩV（ｔ−１）とを前記ステップＳ７２で設定した探索範囲内で相対的に移動させて、ＶＰＰ画像マッチングを行う。探索範囲は画像マッチング移動量ΔＸ0、ΔＹ0、Δθyaw-0を基準とした範囲に設定されているため、ＶＰＰ画像マッチングに要する時間は短時間で足りる。また、自車速度ベクトルＶVは急激に変化することはないため、類似度が最大となる箇所が画像マッチング移動量ΔＸ0、ΔＹ0、Δθ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（ｔ−１）をΔＸ及びΔＹ方向に並進的に移動させるだけではなく、回転を含んだ移動をさせた上で類似度の演算を行うとよい。この場合、画像マッチング移動量ΔＸ0、ΔＹ0を中心とした所定幅を有する範囲としてΔＸ＝ΔＸMIN〜ΔＸMAX、ΔＹ＝ΔＹMIN〜ΔＹMAX及び回転角度としてのω＝ΔθMIN〜ΔθMAXが設定され、３自由度の移動を行う。
【０２４３】
ステップＳ７６において、自車速度ベクトルＶV、角度θYAWを記録する。また、ステップＳ７４で求めたΔＸf、ΔＹf、Δθyaw-fを次回処理時の画像マッチング移動量ΔＸ0、ΔＹ0、Δθyaw-0として用いるために、ΔＸ0←ΔＸf、ΔＹ0←ΔＹ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…参照画像 Π…平面
ｄ…距離 ｎ…法線ベクトル
ｔ…並進ベクトル
【技術分野】
【０００１】
本発明は、例えば、自動車の運転者支援や自律移動車の自動運転を実現するため、これらの移動体に搭載した一対の撮像手段により撮像した画像から、前記移動体が走行可能な平面を検出する平面検出装置及び検出方法に関する。
【背景技術】
【０００２】
自律移動車等の移動体の誘導において、走行可能な道路平面領域を検出し、あるいは、走行環境に存在する障害物を検出する方法は、レーザレーダ、超音波、ミリ波レーダ等を利用する方法と、画像を利用する方法とに大別することができる。
【０００３】
レーザレーダやミリ波レーダを用いる検出方法では、一般に装置が高価であり、また、十分な空間分解能が得られないという問題がある。また、超音波を用いる検出方法では、遠くの計測が困難であって、空間分解能も低いという問題がある。
【０００４】
一方、画像を用いる方法では、単眼による方法と、複眼を用いる方法とに分けることができる。従来、画像を用いる方法の多くは単眼、すなわち、１つの視点から得られる画像を用いるもので、主に、高速道路等の整備された環境で用いることを想定し、路面の白線（例えば分離線や中央線）等を検出することにより、走行領域を検出している。しかしながら、白線等の存在が保証されず、また路面の色やパターンがさまざまであるような一般道や駐車場等では、単眼画像から得られる濃度パターンのみから、走行可能領域と障害物を安定に区別することは困難である。
【０００５】
これに対して、複眼、すなわち、ステレオ画像を利用する検出方法では、原理的には環境の３次元構造を利用できるため、より安定に走行環境を認識できる可能性がある。特に、走行可能な領域は空間中でほぼ平面であるとみなせることから、画像間の射影変換行列を算出し、この射影変換行列により一方の画像を２次元射影変換して他方の画像に重畳させ、画像同士の重なり具合から、平面領域か否か、あるいは、障害物か否かを検出する方法が提案されている（特許文献１、非特許文献１参照）。
【０００６】
【特許文献１】特開２０００−２９３６９３号公報
【非特許文献１】ホイング−ヤング・シャム（Ｈｅｕｎｇ‐Ｙｅｕｎｇ Ｓｈｕｍ）、リチャード・セーリスキ（Ｒｉｃｈａｒｄ Ｓｚｅｌｉｓｋｉ）、「“パノラミック・イメージ・モザイク”テクニカル・レポート（“Ｐａｎｏｒａｍｉｃ Ｉｍａｇｅ Ｍｏｓａｉｃｓ”Ｔｅｃｈｎｉｃａｌ ｒｅｐｏｒｔ）」、１９９７年、エム・エス・アール−テイー・アール−９７−２３、マイクロソフト・リサーチ（ＭＳＲ−ＴＲ−９７‐２３，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が既知であるとすると、次の（１−１０）式が成立する。
【００６５】
【数１０】
【００６６】
そこで、射影変換行列Ｈ′を特異値分解する。Ｕ、Ｖを正規直交行列、Σを射影変換行列Ｈ′の特異値σ1〜σ3（σ1≧σ2≧σ3＞０）を要素とする対角行列として、射影変換行列Ｈ′は、
【００６７】
【数１１】
と書き替えることができる。なお、
【数１２】
である。
【００６８】
（１−１０）、（１−１１）式から、
【数１３】
となる。
【００６９】
ここで、
【数１４】
とすると、次の（１−１５）式が得られる。
【００７０】
【数１５】
【００７１】
基準カメラ座標系の基底ベクトル（ｅ1，ｅ2，ｅ3）を導入し、
【数１６】
【００７２】
とすると、（１−１５）式から得られる３つのベクトル方程式と、ｎ、ｎ′が単位ベクトル、Ｖが正規直交ベクトル、Ｒ′がノルム変化のない回転行列であることから、次の（１−１７）式が得られる。
【００７３】
【数１７】
（１−１７）式をｎ1′2、ｎ2′2、ｎ3′2の連立方程式と考えれば、０でない解を持つには、行列式が０であるから、
【数１８】
となる。
【００７４】
そこで、射影変換行列Ｈ′の特異値σ1〜σ3が、
（I） 重解を持たない場合（σ1≠σ2≠σ3）
（II） 重解を持つ場合（σ1＝σ2≠σ3 又は σ1≠σ2＝σ3）
（III） ３重解を持つ場合（σ1＝σ2＝σ3）
に場合分けして考える。（I）及び（II）の場合は、ｋ＝±σ1若しくはｋ＝±σ3とすると、（３−９）式から算出されるｎ′が単位ベクトルであることに矛盾する。したがって、いずれの場合でもｋ＝±σ2として求められることになる。また、ｋ＝−σ2の場合は２台のカメラ（基準カメラ１２及び参照カメラ１４）が平面を挟むような配置となるため除外され、ｋはσ2として求められることになる。なお、（III）の３重解の場合は、２台の基準カメラ１２及び参照カメラ１４の光学中心が一致し、回転のみの場合であるため、除外する。
【００７５】
（I）及び（II）の場合、（１−１７）式より、ｎ′は、
【数１９】
として求まる。
【００７６】
σ1≠σ2≠σ3のとき、ｎ2′＝０であるから、
【数２０】
を得る。すなわち、Ｒ′は、ｅ2の回りの回転行列である。したがって、次の（１−２１）、（１−２２）、（１−２３）式を得る。
【００７７】
【数２１】
【００７８】
【数２２】
【００７９】
【数２３】
（II）の場合、σ1＝σ2（又は、σ2＝σ3）として、（１−２１）式から、
【数２４】
となり、
【数２５】
【数２６】
となる。
【００８０】
ここで、（１−１９）式におけるε1、ε2の符号の違いにより、４つの解の候補が存在する。この４つの候補のうち、「平面上の点が画像に映る」条件を満たすｎ′を選択することで２つの候補に絞り込む。さらに、基準カメラ１２及び参照カメラ１４の位置関係が撮影中に変化しない条件を加え、解を一意に決定する。
【００８１】
以上のようにして、（１−２１）式で求めたＲ′を（１−１４）式に代入することにより、回転行列Ｒが求められる。また、（１−２３）又は（１−２６）式で求めたｔ′を（１−１４）式に代入してｔ／ｄを求め、ｔ／ｄを正規化し、並進ベクトルｔの単位ベクトルｔ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中における観測点Ｍ１に対応したエピポーララインＥＰとなる。ここで、「’」は参照画像であることを示し、ｌ1’，ｌ2’，ｌ3’は参照画像のエピポーララインＥＰのパラメータを示す。また、ｕ’、ｖ’は参照画像の画像座標を示す。
【０１９６】
また、観測点Ｍ１の概略位置が既知である場合には、直線Ｌｅの長さを制限することによりエピポーララインＥＰを短く設定することができる。
【０１９７】
参照画像Ｉrに投影される位置をｍ’としたとき、ｌ＝Ｆtｍ’とすると、ｌtｍ＝０より、参照画像Ｉr中では、
【数４９】
となり、観測点Ｍ１における基準画像ＩbにおけるエピポーララインＥＰとなる。なお、このエピポーララインＥＰの説明で用いたパラメータｕ及びｕ’は、後述するベクトルｕ及びｕ’とは異なるものである。
【０１９８】
＜ステップＳ６＞
このステップＳ６は、自車である車両１０の自車速度ベクトルＶV（図２５参照）を求めるために、道路平面Πを仮想的に上方から見たＶＰＰ画像を生成するための処理であり、図２８に示すステップＳ６１〜ステップＳ６３のサブルーチン処理からなる。すなわち、ステップＳ６１において、基準画像ＩbをＶＰＰ画像に変換（以下、ＶＰＰ変換とも呼ぶ）するための変換式ＨVPPを算出する。この後、ステップＳ６２において、求められた変換式ＨVPPを用いて、前記基準画像ＩbをＶＰＰ画像ＩVに変換し、記録する（図３３参照）。さらに、ステップＳ６３において、同じ変換式ＨVPPを用いて前記平面領域ΠfをＶＰＰ変換して記録する（図３４参照）。以下、各ステップＳ６１〜Ｓ６３について詳細に説明する。
【０１９９】
なお、ＶＰＰ画像ＩVを撮像する仮想的なカメラを想定し、説明上これを仮想カメラと呼ぶ。
【０２００】
先ず、図２９のステップＳ６１１において、基準カメラ１２の内部パラメータＡbの逆行列Ａb-1を読み込む。内部パラメータＡbは、次の（６−１）式で表される。
【０２０１】
【数５０】
【０２０２】
ここで、α1、β1、ｓ1、Ｃ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（ｔ−１）及び画像マッチング移動量ΔＸ0、ΔＹ0、Δθyaw-0を呼び出す。画像マッチング移動量ΔＸ0、ΔＹ0、Δθyaw-0は、前時刻処理時のステップＳ７７で記録されたパラメータであり、前時刻におけるＶＰＰ画像ＩV（ｔ）とＶＰＰ画像ＩV（ｔ−１）とのマッチングによるずれ量を表す。
【０２３３】
ステップＳ７２において、ＶＰＰ画像ＩV（ｔ）とＶＰＰ画像ＩV（ｔ−１）とのマッチングを行うための探索範囲を設定する。具体的には、画像マッチング移動量ΔＸ0、ΔＹ0、Δθyaw-0を中心とした所定幅を有する範囲としてΔＸ＝ΔＸMIN〜ΔＸMAX、及びΔＹ＝ΔＹMIN〜ΔＹMAX、Δθyaw＝Δθyaw-MIN〜Δθyaw-MAXが設定される。ステップＳ７３において、ＶＰＰ画像ＩV（ｔ）とＶＰＰ画像ＩV（ｔ−１）とを前記ステップＳ７２で設定した探索範囲内で相対的に移動させて、ＶＰＰ画像マッチングを行う。探索範囲は画像マッチング移動量ΔＸ0、ΔＹ0、Δθyaw-0を基準とした範囲に設定されているため、ＶＰＰ画像マッチングに要する時間は短時間で足りる。また、自車速度ベクトルＶVは急激に変化することはないため、類似度が最大となる箇所が画像マッチング移動量ΔＸ0、ΔＹ0、Δθ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（ｔ−１）をΔＸ及びΔＹ方向に並進的に移動させるだけではなく、回転を含んだ移動をさせた上で類似度の演算を行うとよい。この場合、画像マッチング移動量ΔＸ0、ΔＹ0を中心とした所定幅を有する範囲としてΔＸ＝ΔＸMIN〜ΔＸMAX、ΔＹ＝ΔＹMIN〜ΔＹMAX及び回転角度としてのω＝ΔθMIN〜ΔθMAXが設定され、３自由度の移動を行う。
【０２４３】
ステップＳ７６において、自車速度ベクトルＶV、角度θYAWを記録する。また、ステップＳ７４で求めたΔＸf、ΔＹf、Δθyaw-fを次回処理時の画像マッチング移動量ΔＸ0、ΔＹ0、Δθyaw-0として用いるために、ΔＸ0←ΔＸf、ΔＹ0←ΔＹ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…参照画像 Π…平面
ｄ…距離 ｎ…法線ベクトル
ｔ…並進ベクトル
【特許請求の範囲】
【請求項１】
撮像手段により撮像した画像から平面を検出する平面検出装置であって、
所定の位置関係に配置され、平面を含む相互に重複する領域の画像を撮像する一対の撮像手段と、
前記一対の撮像手段の一方の座標系を他方の座標系に変換する回転行列及び並進ベクトルをパラメータとして記憶するパラメータ記憶手段と、
前記回転行列及び前記並進ベクトルを用いて、一方の前記撮像手段により撮像された画像を変換し、前記画像に含まれる平面を他方の前記撮像手段により撮像された画像に含まれる平面に重畳させる射影変換行列を算出する射影変換行列算出手段と、
を備え、前記射影変換行列を用いて平面を検出することを特徴とする平面検出装置。
【請求項２】
請求項１記載の装置において、
前記平面の法線ベクトルと、基準とする一方の前記撮像手段から前記平面までの距離とからなる平面パラメータを算出する平面パラメータ算出手段を備えることを特徴とする平面検出装置。
【請求項３】
請求項１記載の装置において、
前記撮像手段は、移動体に固定されることを特徴とする平面検出装置。
【請求項４】
撮像手段により撮像した画像から平面を検出する平面検出方法であって、
所定の位置関係に配置された一対の撮像手段を用いて、平面を含む相互に重複する領域の画像を撮像する第１ステップと、
一方の前記撮像手段の座標系を他方の前記撮像手段の座標系に変換する回転行列及び並進ベクトルを用いて、一方の前記撮像手段により撮像された画像を変換し、前記画像に含まれる平面を他方の前記撮像手段により撮像された画像に含まれる平面に重畳させる射影変換行列を算出する第２ステップと、
を有し、前記射影変換行列を用いて平面を検出することを特徴とする平面検出方法。
【請求項５】
請求項４記載の方法において、
前記射影変換行列Ｈは、前記回転行列Ｒ、前記並進ベクトルｔ、前記一対の撮像手段の内部パラメータＡb、Ａr、前記平面の法線ベクトルｎ、基準とする一方の前記撮像手段から前記平面までの距離ｄとを用いて、
【数１】
（但し、ｋは、０でない係数）
と定義し、前記法線ベクトルｎ及び前記距離ｄを変化させ、前記一対の撮像手段により撮像された画像同士が最も重畳されるときの前記射影変換行列Ｈとして算出することを特徴とする平面検出方法。
【請求項６】
撮像手段により撮像した画像から平面を検出する平面検出装置であって、
所定の位置関係に配置され、平面を含む相互に重複する領域の画像を撮像する一対の撮像手段と、
前記一対の撮像手段の一方の座標系を他方の座標系に変換する回転行列及び並進ベクトルと、前記一対の撮像手段の内部パラメータと、平面の法線ベクトルの初期値と、基準とする一方の前記撮像手段から平面までの距離の初期値とをパラメータとして記憶するパラメータ記憶手段と、
前記回転行列、前記並進ベクトル及び前記内部パラメータを用いて、前記法線ベクトルの初期値及び前記距離の初期値を濃度勾配法により変化させ、前記一対の撮像手段によって撮像されたステレオ画像に含まれる平面を重畳させる射影変換行列を推定する射影変換行列推定手段と、
を備え、前記射影変換行列を用いて平面を検出することを特徴とする平面検出装置。
【請求項７】
請求項６記載の装置において、
平面の前記法線ベクトルと、基準とする一方の前記撮像手段から平面までの前記距離とからなる平面パラメータを推定する平面パラメータ推定手段を備え、
前記平面パラメータ推定手段は、前記パラメータ記憶手段に記憶された前記法線ベクトルの初期値及び前記距離の初期値に対して、前記射影変換行列推定手段によって前記射影変換行列を推定する際に変化した前記法線ベクトルの変化量と、前記距離の変化量とを用いて前記平面パラメータを推定することを特徴とする平面検出装置。
【請求項８】
請求項６記載の装置において、
前記法線ベクトルの初期値及び前記距離の初期値は、前記一対の撮像手段が固定される移動体が停止している状態での初期値であることを特徴とする平面検出装置。
【請求項９】
請求項６記載の装置において、
前記平面パラメータ推定手段により推定された前記平面パラメータを用いて、前記平面の空間位置を算出する空間位置算出手段を備えることを特徴とする平面検出装置。
【請求項１】
撮像手段により撮像した画像から平面を検出する平面検出装置であって、
所定の位置関係に配置され、平面を含む相互に重複する領域の画像を撮像する一対の撮像手段と、
前記一対の撮像手段の一方の座標系を他方の座標系に変換する回転行列及び並進ベクトルをパラメータとして記憶するパラメータ記憶手段と、
前記回転行列及び前記並進ベクトルを用いて、一方の前記撮像手段により撮像された画像を変換し、前記画像に含まれる平面を他方の前記撮像手段により撮像された画像に含まれる平面に重畳させる射影変換行列を算出する射影変換行列算出手段と、
を備え、前記射影変換行列を用いて平面を検出することを特徴とする平面検出装置。
【請求項２】
請求項１記載の装置において、
前記平面の法線ベクトルと、基準とする一方の前記撮像手段から前記平面までの距離とからなる平面パラメータを算出する平面パラメータ算出手段を備えることを特徴とする平面検出装置。
【請求項３】
請求項１記載の装置において、
前記撮像手段は、移動体に固定されることを特徴とする平面検出装置。
【請求項４】
撮像手段により撮像した画像から平面を検出する平面検出方法であって、
所定の位置関係に配置された一対の撮像手段を用いて、平面を含む相互に重複する領域の画像を撮像する第１ステップと、
一方の前記撮像手段の座標系を他方の前記撮像手段の座標系に変換する回転行列及び並進ベクトルを用いて、一方の前記撮像手段により撮像された画像を変換し、前記画像に含まれる平面を他方の前記撮像手段により撮像された画像に含まれる平面に重畳させる射影変換行列を算出する第２ステップと、
を有し、前記射影変換行列を用いて平面を検出することを特徴とする平面検出方法。
【請求項５】
請求項４記載の方法において、
前記射影変換行列Ｈは、前記回転行列Ｒ、前記並進ベクトルｔ、前記一対の撮像手段の内部パラメータＡb、Ａr、前記平面の法線ベクトルｎ、基準とする一方の前記撮像手段から前記平面までの距離ｄとを用いて、
【数１】
（但し、ｋは、０でない係数）
と定義し、前記法線ベクトルｎ及び前記距離ｄを変化させ、前記一対の撮像手段により撮像された画像同士が最も重畳されるときの前記射影変換行列Ｈとして算出することを特徴とする平面検出方法。
【請求項６】
撮像手段により撮像した画像から平面を検出する平面検出装置であって、
所定の位置関係に配置され、平面を含む相互に重複する領域の画像を撮像する一対の撮像手段と、
前記一対の撮像手段の一方の座標系を他方の座標系に変換する回転行列及び並進ベクトルと、前記一対の撮像手段の内部パラメータと、平面の法線ベクトルの初期値と、基準とする一方の前記撮像手段から平面までの距離の初期値とをパラメータとして記憶するパラメータ記憶手段と、
前記回転行列、前記並進ベクトル及び前記内部パラメータを用いて、前記法線ベクトルの初期値及び前記距離の初期値を濃度勾配法により変化させ、前記一対の撮像手段によって撮像されたステレオ画像に含まれる平面を重畳させる射影変換行列を推定する射影変換行列推定手段と、
を備え、前記射影変換行列を用いて平面を検出することを特徴とする平面検出装置。
【請求項７】
請求項６記載の装置において、
平面の前記法線ベクトルと、基準とする一方の前記撮像手段から平面までの前記距離とからなる平面パラメータを推定する平面パラメータ推定手段を備え、
前記平面パラメータ推定手段は、前記パラメータ記憶手段に記憶された前記法線ベクトルの初期値及び前記距離の初期値に対して、前記射影変換行列推定手段によって前記射影変換行列を推定する際に変化した前記法線ベクトルの変化量と、前記距離の変化量とを用いて前記平面パラメータを推定することを特徴とする平面検出装置。
【請求項８】
請求項６記載の装置において、
前記法線ベクトルの初期値及び前記距離の初期値は、前記一対の撮像手段が固定される移動体が停止している状態での初期値であることを特徴とする平面検出装置。
【請求項９】
請求項６記載の装置において、
前記平面パラメータ推定手段により推定された前記平面パラメータを用いて、前記平面の空間位置を算出する空間位置算出手段を備えることを特徴とする平面検出装置。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図１６】
【図１７】
【図１８】
【図１９】
【図２０】
【図２１】
【図２２】
【図２３】
【図２４】
【図２５】
【図２６】
【図２７】
【図２８】
【図２９】
【図３０】
【図３１】
【図３２】
【図３３】
【図３４】
【図３５】
【図３６】
【図３７】
【図３８】
【図３９】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図１６】
【図１７】
【図１８】
【図１９】
【図２０】
【図２１】
【図２２】
【図２３】
【図２４】
【図２５】
【図２６】
【図２７】
【図２８】
【図２９】
【図３０】
【図３１】
【図３２】
【図３３】
【図３４】
【図３５】
【図３６】
【図３７】
【図３８】
【図３９】
【公開番号】特開２００６−５３７５７（Ｐ２００６−５３７５７Ａ）
【公開日】平成１８年２月２３日（２００６．２．２３）
【国際特許分類】
【出願番号】特願２００４−２３４９４８（Ｐ２００４−２３４９４８）
【出願日】平成１６年８月１１日（２００４．８．１１）
【出願人】（０００００５３２６）本田技研工業株式会社 (23,863)
【出願人】（３０４０２１４１７）国立大学法人東京工業大学 (1,821)
【Ｆターム（参考）】
【公開日】平成１８年２月２３日（２００６．２．２３）
【国際特許分類】
【出願日】平成１６年８月１１日（２００４．８．１１）
【出願人】（０００００５３２６）本田技研工業株式会社 (23,863)
【出願人】（３０４０２１４１７）国立大学法人東京工業大学 (1,821)
【Ｆターム（参考）】
[ Back to top ]