JP5455037B2 - 画像から平面を検出する平面検出装置及び検出方法 - Google Patents

Description

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

移動体に搭載した一対の撮像手段により撮像した画像から、移動体が走行可能な平面を検出する手段として、特許文献１が既に開示されている。

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

図２は一対の撮像手段間での２次元射影変換を説明するための模式図であり、
図３は平面領域を抽出する手順の説明図である。

先ず、本発明に用いられる道路平面領域の抽出及び２次元射影変換の原理を説明する。

図２に示すように、空間中にある平面Π上の観測点Ｍ１が基準画像Ｉ_ｂ及び参照画像Ｉ_ｒに投影されるとき、基準画像Ｉ_ｂ上での同次座標をｍ_ｂ、参照画像Ｉ_ｒ上での同次座標をｍ_ｒとすると、各同次座標ｍ_ｂ、ｍ_ｒは、２次元射影変換行列Ｈによって関係付けられる。

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

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

図４は図１の演算処理装置１６における処理の全体の流れを説明するフローチャートである。
この図において、先ず、ステップＳ０において、基準カメラ１２及び参照カメラ１４により、車両１０が走行する道路平面領域を含む基準画像及び参照画像を所定の時間間隔で撮像し、所定の画像処理を行った後、各時間毎の画像情報として記憶する。

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

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

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

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

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

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

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

特許第３９３７４１４号公報、「平面検出装置及び検出方法」

上述した特許文献１の手段により、低コスト並びに安定した道路平面領域及び障害物情報が取得可能となる。

特許文献１の手段では、平面領域の検出を行うために、ステップＳ１において、記憶された基準画像Ｉ_ｂ及び参照画像Ｉ_ｒを用いて、路面に対する射影変換行列Ｈを動的に推定している。

この動的推定を成功させるためには、初期値として解に近いものを与える必要があり、特許文献１の手段では、ある時刻ｔにおける射影変換行列Ｈの推定の際に、前時刻（ｔ−１）までの推定結果を利用している。

しかし車両の揺動が激しく、前時刻（ｔ−１）と現時刻（ｔ）で画像の撮影姿勢が大きく変化する場合には、前時刻の射影変換行列Ｈが初期値として妥当ではない場合がでてくる。
すなわち、前時刻の射影変換行列Ｈを初期値として用いた場合、射影変換行列Ｈの探索が別の値に収束したり、解が求まらない可能性がある。その場合、幅広くパラメータを振って解の推定を行う必要があり、推定に時間を要し、これにより、円滑な走行に支障をきたすことがある。

なお、初期値として妥当でなくなる条件は、撮影される画像のテクスチャにより異なるが、過去の試験により、１フレーム間で車両のピッチ角２°以上の変化で生じている。

本発明は、上述した問題点を解決するために創案されたものである。すなわち、本発明の目的は、車両の揺動が激しい場合でも、射影変換行列Ｈの初期値を短時間かつ正確に推定することができる画像から平面を検出する平面検出装置及び検出方法を提供することにある。

本発明によれば、撮像手段により撮像した画像から平面を検出する平面検出装置であって、
移動体に所定の位置関係で固定して配置され、平面を含む相互に重複する領域の画像を撮像する一対の撮像手段と、
前記一対の撮像手段の一方の座標系を他方の座標系に変換する回転行列及び並進ベクトルを固定パラメータとして記憶するパラメータ記憶手段と、
前記移動体の姿勢角を計測するジャイロセンサと、
前記姿勢角を用いて記憶された前記平面の法線ベクトルを修正する法線ベクトル修正手段と、を備え、前記法線ベクトル修正手段は、
前時刻（ｔ−１）においてジャイロセンサにより計測された姿勢角を用いてカメラ座標系Ｃの法線ベクトルｎ（ｔ−１）を慣性座標系Ｅの法線ベクトルｎ _Ｅ （ｔ−１）に変換し、
次いで、現時刻（ｔ）においてジャイロセンサにより計測された姿勢角を用いて慣性座標系Ｅの法線ベクトルｎ _Ｅ （ｔ−１）をカメラ座標系Ｃの法線ベクトルｎ（ｔ）に変換し、
さらに、前記平面の修正された法線ベクトルと、基準とする一方の前記撮像手段から前記平面までの距離とからなる平面パラメータを変動パラメータとし、前記固定パラメータとして記憶されている前記回転行列及び前記並進ベクトルを用いて、一方の前記撮像手段により撮像された画像を変換し、前記画像に含まれる平面を他方の前記撮像手段により撮像された画像に含まれる平面に最適に重畳させる射影変換行列を動的に推定する射影変換行列推定手段を備え、
推定された前記射影変換行列を用いて平面を検出することを特徴とする平面検出装置が提供される。

また、撮像手段により撮像した画像から平面を検出する平面検出装置であって、
移動体に所定の位置関係で固定して配置され、平面を含む相互に重複する領域の画像を撮像する一対の撮像手段と、
前記一対の撮像手段の一方の座標系を他方の座標系に変換する回転行列及び並進ベクトルを固定パラメータとして記憶するパラメータ記憶手段と、
前記移動体の姿勢角を計測するジャイロセンサと、
前記姿勢角を用いて記憶された前記平面の法線ベクトルを修正する法線ベクトル修正手段と、を備え、前記法線ベクトル修正手段は、
カメラ座標系Ｃを車体座標系Ｂに変換する変換行列Ｔ _ＢＣ とその逆変換行列Ｔ _ＣＢ とを予め記憶し、
前時刻（ｔ−１）における姿勢角を用いて車体座標系Ｂから慣性座標系Ｅに変換する変換行列Ｔ _ＥＢ （ｔ−１）を求め、
前時刻（ｔ−１）におけるカメラ座標系Ｃの法線ベクトルｎ（ｔ−１）を変換行列Ｔ _ＢＣ と変換行列Ｔ _ＥＢ （ｔ−１）を用いて慣性座標系Ｅの法線ベクトルｎ _Ｅ （ｔ−１）に変換し、
次いで、現時刻（ｔ）における姿勢角を用いて慣性座標系Ｅから車体座標系Ｂに変換する逆変換行列Ｔ _ＢＥ （ｔ）を求め、
前時刻（ｔ−１）における前記慣性座標系Ｅの法線ベクトルｎ _Ｅ （ｔ−１）を現時刻（ｔ）における逆変換行列Ｔ _ＢＥ （ｔ）と逆変換行列Ｔ _ＣＢ を用いてカメラ座標系Ｃの法線ベクトルｎ（ｔ）に変換し、
さらに、前記平面の修正された法線ベクトルと、基準とする一方の前記撮像手段から前記平面までの距離とからなる平面パラメータを変動パラメータとし、前記固定パラメータとして記憶されている前記回転行列及び前記並進ベクトルを用いて、一方の前記撮像手段により撮像された画像を変換し、前記画像に含まれる平面を他方の前記撮像手段により撮像された画像に含まれる平面に最適に重畳させる射影変換行列を動的に推定する射影変換行列推定手段を備え、
推定された前記射影変換行列を用いて平面を検出することを特徴とする平面検出装置が提供される。

また、本発明によれば、撮像手段により撮像した画像から平面を検出する平面検出方法であって、
移動体に所定の位置関係で固定して配置された一対の撮像手段を用いて、平面を含む相互に重複する領域の画像を撮像するステップと、
ジャイロセンサにより、前記移動体の姿勢角を検出するステップと、
法線ベクトル修正手段により、前記姿勢角を用いて記憶された前記平面の法線ベクトルを修正するステップと、
射影変換行列推定手段により、前記平面の修正された法線ベクトルと、基準とする一方の前記撮像手段から前記平面までの距離とからなる平面パラメータを変動パラメータとし、一方の前記撮像手段の座標系を他方の前記撮像手段の座標系に変換する固定パラメータとしての回転行列及び並進ベクトルを用いて、一方の前記撮像手段により撮像された画像を変換し、前記画像に含まれる平面を他方の前記撮像手段により撮像された画像に含まれる平面に最適に重畳させる射影変換行列を動的に推定するステップと、を有し、
前記法線ベクトルを修正するステップにおいて、
ジャイロセンサにより姿勢角を計測し、
前時刻（ｔ−１）における姿勢角を用いてカメラ座標系Ｃの法線ベクトルｎ（ｔ−１）を慣性座標系Ｅの法線ベクトルｎ_Ｅ（ｔ−１）に変換し、
次いで、現時刻（ｔ）における姿勢角を用いて慣性座標系Ｅの法線ベクトルｎ_Ｅ（ｔ−１）をカメラ座標系Ｃの法線ベクトルｎ（ｔ）に変換し、
推定された前記射影変換行列を用いて平面を検出することを特徴とする平面検出方法が提供される。

また、撮像手段により撮像した画像から平面を検出する平面検出方法であって、
移動体に所定の位置関係で固定して配置された一対の撮像手段を用いて、平面を含む相互に重複する領域の画像を撮像するステップと、
ジャイロセンサにより、前記移動体の姿勢角を検出するステップと、
法線ベクトル修正手段により、前記姿勢角を用いて記憶された前記平面の法線ベクトルを修正するステップと、
射影変換行列推定手段により、前記平面の修正された法線ベクトルと、基準とする一方の前記撮像手段から前記平面までの距離とからなる平面パラメータを変動パラメータとし、一方の前記撮像手段の座標系を他方の前記撮像手段の座標系に変換する固定パラメータとしての回転行列及び並進ベクトルを用いて、一方の前記撮像手段により撮像された画像を変換し、前記画像に含まれる平面を他方の前記撮像手段により撮像された画像に含まれる平面に最適に重畳させる射影変換行列を動的に推定するステップと、を有し、前記法線ベクトルｎを修正するステップにおいて、
カメラ座標系Ｃを車体座標系Ｂに変換する変換行列Ｔ_ＢＣとその逆変換行列Ｔ_ＣＢとを予め記憶し、
前時刻（ｔ−１）における姿勢角を用いて車体座標系Ｂから慣性座標系Ｅに変換する変換行列Ｔ_ＥＢ（ｔ−１）を求め、
前時刻（ｔ−１）におけるカメラ座標系Ｃの法線ベクトルｎ（ｔ−１）を変換行列Ｔ_ＢＣと変換行列Ｔ_ＥＢ（ｔ−１）を用いて慣性座標系Ｅの法線ベクトルｎ_Ｅ（ｔ−１）に変換し、
次いで、現時刻（ｔ）における姿勢角を用いて慣性座標系Ｅから車体座標系Ｂに変換する逆変換行列Ｔ_ＢＥ（ｔ）を求め、
前時刻（ｔ−１）における前記慣性座標系Ｅの法線ベクトルｎ_Ｅ（ｔ−１）を現時刻（ｔ）における逆変換行列Ｔ_ＢＥ（ｔ）と逆変換行列Ｔ_ＣＢを用いてカメラ座標系Ｃの法線ベクトルｎ（ｔ）に変換し、
推定された前記射影変換行列を用いて平面を検出することを特徴とする平面検出方法が提供される。

上記本発明の装置及び方法によれば、車体の姿勢角を計測し、記憶された平面の法線ベクトルｎを修正するので、車両に揺動が生じた場合でも、射影変換行列Ｈの探索に使用する初期値を補正することができる。

従って、未舗装路のように車両の揺動が激しい場所においても、前時刻（ｔ−１）における平面領域を基に現時刻（ｔ）の法線ベクトルｎ（ｔ）を求めるので、より安定して平面領域を求めることができる。また、安定して平面領域が検出できることにより、計算コストの高い初期探索を行う回数が低減でき、よりスムーズな走行が可能となる。

特許文献１の手段が適用される車両の説明図である。 一対の撮像手段間での２次元射影変換を説明するための模式図である。 特許文献１による平面領域を抽出する手順の説明図である。 図１の演算処理装置１６における処理の全体の流れを説明するフローチャートである。 本発明が適用される車両の説明図である。 本発明による平面検出装置の機能構成ブロック図である。 本発明による平面検出方法のフローチャートである。

以下、本発明の好ましい実施形態を図面を参照して説明する。なお各図において、共通する部分には同一の符号を付し、重複した説明は省略する。

図５は、本発明が適用される車両の説明図である。
この図において、本発明が適用される車両１０（移動体）の上部の左右には、車両１０が走行する道路平面領域を含む画像を撮像する一対の撮像手段である基準カメラ１２及び参照カメラ１４が固定配置される。基準カメラ１２及び参照カメラ１４は、共通の撮像領域が設定されたＣＣＤカメラ等からなるステレオカメラであり、これらには、撮像した画像を処理することで道路平面領域と障害物となる可能性のある物体とを検出する演算処理装置１６が接続される。 以上の構成は、特許文献１の構成と同一である。

図５において、本発明が適用される車両１０（移動体）は、さらに、ジャイロセンサ５２と法線ベクトル修正手段５４を備える。
ジャイロセンサ５２は、移動体１０に固定されており、移動体１０のオイラー角を常時検出する。
法線ベクトル修正手段５４は、検出されたオイラー角を用いて記憶された前記平面の法線ベクトルｎを修正する。

図５において、Ｘ_Ｃ−Ｙ_Ｃ−Ｚ_Ｃはカメラ座標系Ｃであり、Ｘ_Ｂ−Ｙ_Ｂ−Ｚ_Ｂは車体座標系Ｂであり、Ｘ_Ｅ−Ｙ_Ｅ−Ｚ_Ｅは慣性座標系Ｅである。

一対の撮像手段（基準カメラ１２及び参照カメラ１４）は、移動体に所定の位置関係で固定して配置されているので、カメラ座標系Ｃと車体座標系Ｂの相対位置は一定であり、カメラ座標系Ｃを車体座標系Ｂに変換する変換行列Ｔ_ＢＣとその逆変換行列Ｔ_ＣＢとは予め既知である。

また、移動体１０の慣性座標系Ｅに対するオイラー角は、ロール角α、仰角β、ヨー角γからなり、車体座標系Ｂから慣性座標系Ｅに変換する変換行列Ｔ_ＥＢは、数１の式（１）で与えられる。また、慣性座標系Ｅから車体座標系Ｂに変換する逆変換行列Ｔ_ＢＥは、式（１）の逆行列Ｔ_ＥＢ ^-1として与えられる。

図６は、本発明による平面検出装置の機能構成ブロック図である。この図において、演算処理装置１６は、パラメータ記憶手段２６と射影変換行列推定手段２８を有する。
演算処理装置１６は、さらに特許文献１と同様に、画像処理部、画像記憶部、演算制御部、平面パラメータ算出部、平面領域抽出部、物体検出部、物体相対速度算出部、仮想投影面画像生成部（空間位置算出手段）、自車速度算出部、物体絶対速度算出部及び表示処理部を備える。これらの機能は、特許文献１と同様である。

図７は、本発明による平面検出方法のフローチャートである。
この図において、先ず、ステップＳ０において、基準カメラ１２及び参照カメラ１４により、車両１０が走行する道路平面領域を含む基準画像及び参照画像を所定の時間間隔で撮像し、画像処理部において所定の画像処理を行った後、画像記憶部に各時間毎の画像情報として記憶する。

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

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

ステップＳ３では、道路平面の法線ベクトルｎ及び基準カメラ１２の光学中心から道路平面までの距離ｄを平面パラメータとして算出する。
以上のステップは、特許文献１の構成と同一である。

本発明による平面検出方法では、さらに法線ベクトル修正ステップＳ３１と、初期値算出ステップＳ３２とを有する。
法線ベクトル修正ステップＳ３１では、ジャイロセンサ５２で検出したオイラー角を用いて記憶された道路平面領域の法線ベクトルｎを修正する。
すなわち、法線ベクトル修正ステップＳ３１では、前時刻（ｔ−１）におけるオイラー角を用いてカメラ座標系Ｃの法線ベクトルｎ（ｔ−１）を慣性座標系Ｅの法線ベクトルｎ_Ｅ（ｔ−１）に変換し、
次いで、現時刻（ｔ）におけるオイラー角を用いて慣性座標系Ｅの法線ベクトルｎ_Ｅ（ｔ−１）をカメラ座標系Ｃの法線ベクトルｎ（ｔ）に変換する。

局所的に凹凸は存在しても、大局的に平坦な路面を走行している場合、慣性座標系において、前時刻（ｔ−１）における慣性座標系Ｅの法線ベクトルｎ_Ｅ（ｔ−１）と現時刻（ｔ）における慣性座標系Ｅの法線ベクトルｎ_Ｅ（ｔ）は、ほぼ同一となる。従って、ｎ_Ｅ（ｔ）＝ｎ_Ｅ（ｔ−１）が成り立つ。
また、前時刻（ｔ−１）における慣性座標系Ｅの法線ベクトルｎ_Ｅ（ｔ−１）は、前時刻（ｔ−１）におけるカメラ座標系Ｃの法線ベクトルｎ（ｔ−１）が既知であり、かつ前時刻（ｔ−１）におけるオイラー角も既知であるから、これらを用いて、以下のように算出することができる。

（１）カメラ座標系Ｃを車体座標系Ｂに変換する変換行列Ｔ_ＢＣとその逆変換行列Ｔ_ＣＢとを予め記憶する。前述したように、カメラ座標系Ｃと車体座標系Ｂの相対位置は一定であり、カメラ座標系Ｃを車体座標系Ｂに変換する変換行列Ｔ_ＢＣとその逆変換行列Ｔ_ＣＢとは予め既知である。また、Ｔ_ＣＢ＝Ｔ_ＢＣ ^−１である。
（２）前時刻（ｔ−１）におけるオイラー角（ロール角α、仰角β、ヨー角γ）を用いて、上述した式（１）により、車体座標系Ｂから慣性座標系Ｅに変換する変換行列Ｔ_ＥＢ（ｔ−１）を求める。
（３）前時刻（ｔ−１）におけるカメラ座標系Ｃの法線ベクトルｎ（ｔ−１）を変換行列Ｔ_ＢＣと変換行列Ｔ_ＥＢ（ｔ−１）を用いて、数２の式（２）により、慣性座標系Ｅの法線ベクトルｎ_Ｅ（ｔ−１）に変換する。

また、時刻ｔと時刻ｔ−１で、同一の平面を走行しているとすれば、現時刻（ｔ）における法線ベクトルｎ（ｔ）は、現時刻（ｔ）における慣性座標系Ｅの法線ベクトルｎ_Ｅ（ｔ）が前時刻（ｔ−１）における慣性座標系Ｅの法線ベクトルｎ_Ｅ（ｔ−１）と同一とみなすことができ、かつ現時刻（ｔ）におけるオイラー角も既知であるから、これらを用いて、以下のように算出することができる。

（４）現時刻（ｔ）におけるオイラー角（ロール角α、仰角β、ヨー角γ）を用いて慣性座標系Ｅから車体座標系Ｂに変換する逆変換行列Ｔ_ＢＥ（ｔ）を求める。なお、Ｔ_ＢＥ（ｔ）＝Ｔ_ＥＢ ^−１（ｔ）である。
（５）前時刻（ｔ−１）における前記慣性座標系Ｅの法線ベクトルｎ_Ｅ（ｔ−１）を現時刻（ｔ）における逆変換行列Ｔ_ＢＥ（ｔ）と逆変換行列Ｔ_ＣＢを用いて、数３の式（３）により、カメラ座標系Ｃの法線ベクトルｎ（ｔ）に変換する。

初期値算出ステップＳ３２では、数４の式（４）を用いて、射影変換行列Ｈを算出する。
ここで、Ｒは前記回転行列、ｔは前記並進ベクトル、Ａ_ｂ、Ａ_ｒは前記一対の撮像手段の内部パラメータ、ｎは前記平面の法線ベクトル、ｄは基準とする一方の前記撮像手段から前記平面までの距離、ｋは０でない係数である。

式（４）において、Ｒ、ｔ、Ａ_ｂ、Ａ_ｒ、ｋは既知の定数である。従って、数３の式（３）により求めたカメラ座標系Ｃの法線ベクトルｎ（ｔ）と、前時刻（ｔ−１）における距離ｄを初期値として、法線ベクトルｎ及び距離ｄを変化させ、前記一対の撮像手段により撮像された画像同士が最も重畳されるときの前記射影変換行列Ｈとして推定することにより、上述したステップＳ２において、ステップＳ１で算出した射影変換行列Ｈを用いて、基準画像から道路平面領域を抽出することができる。

上述した本発明の装置及び方法によれば、移動体１０に固定されたジャイロセンサ５２を用いて移動体１０の慣性座標系Ｅに対するオイラー角（ロール角α、仰角β、ヨー角γ）を検出し、このオイラー角を用いて記憶された前記平面の法線ベクトルｎを修正するので、車両１０に揺動が生じた場合でも、この揺動をジャイロセンサ５２で検出して射影変換行列Ｈの探索に使用する初期値を補正することができる。

すなわち、移動体（車両）１０にジャイロセンサ５２を搭載し、これにより計測される車体の姿勢角（オイラー角）を用いて、車両の揺動による初期値Ｈの変動を補正する。

従って、大局的に平坦な場所であれば、未舗装路のように車両の揺動が激しい場所においても、前時刻（ｔ−１）における平面領域を元に現時刻（ｔ）の法線ベクトルｎ（ｔ）を求めるので、より安定して平面領域を求めることができる。また、安定して平面領域が検出できることにより、計算コストの高い初期探索を行う回数が低減でき、よりスムーズな走行が可能となる。

なお、本発明は上述した実施形態に限定されず、本発明の要旨を逸脱しない範囲で種々に変更することができることは勿論である。

１０ 車両（移動体）、１２ 基準カメラ、１４ 参照カメラ、
１６ 演算処理装置、
２６ パラメータ記憶手段、２８ 射影変換行列推定手段、
５２ ジャイロセンサ、５４ 法線ベクトル修正手段

Claims (4)

1. 撮像手段により撮像した画像から平面を検出する平面検出装置であって、
   移動体に所定の位置関係で固定して配置され、平面を含む相互に重複する領域の画像を撮像する一対の撮像手段と、
   前記一対の撮像手段の一方の座標系を他方の座標系に変換する回転行列及び並進ベクトルを固定パラメータとして記憶するパラメータ記憶手段と、
   前記移動体の姿勢角を計測するジャイロセンサと、
   前記姿勢角を用いて記憶された前記平面の法線ベクトルを修正する法線ベクトル修正手段と、を備え、前記法線ベクトル修正手段は、
   前時刻（ｔ−１）においてジャイロセンサにより計測された姿勢角を用いてカメラ座標系Ｃの法線ベクトルｎ（ｔ−１）を慣性座標系Ｅの法線ベクトルｎ _Ｅ （ｔ−１）に変換し、
   次いで、現時刻（ｔ）においてジャイロセンサにより計測された姿勢角を用いて慣性座標系Ｅの法線ベクトルｎ _Ｅ （ｔ−１）をカメラ座標系Ｃの法線ベクトルｎ（ｔ）に変換し、
   さらに、前記平面の修正された法線ベクトルと、基準とする一方の前記撮像手段から前記平面までの距離とからなる平面パラメータを変動パラメータとし、前記固定パラメータとして記憶されている前記回転行列及び前記並進ベクトルを用いて、一方の前記撮像手段により撮像された画像を変換し、前記画像に含まれる平面を他方の前記撮像手段により撮像された画像に含まれる平面に最適に重畳させる射影変換行列を動的に推定する射影変換行列推定手段を備え、
   推定された前記射影変換行列を用いて平面を検出することを特徴とする平面検出装置。
2. 撮像手段により撮像した画像から平面を検出する平面検出装置であって、
   移動体に所定の位置関係で固定して配置され、平面を含む相互に重複する領域の画像を撮像する一対の撮像手段と、
   前記一対の撮像手段の一方の座標系を他方の座標系に変換する回転行列及び並進ベクトルを固定パラメータとして記憶するパラメータ記憶手段と、
   前記移動体の姿勢角を計測するジャイロセンサと、
   前記姿勢角を用いて記憶された前記平面の法線ベクトルを修正する法線ベクトル修正手段と、を備え、前記法線ベクトル修正手段は、
   カメラ座標系Ｃを車体座標系Ｂに変換する変換行列Ｔ _ＢＣ とその逆変換行列Ｔ _ＣＢ とを予め記憶し、
   前時刻（ｔ−１）における姿勢角を用いて車体座標系Ｂから慣性座標系Ｅに変換する変換行列Ｔ _ＥＢ （ｔ−１）を求め、
   前時刻（ｔ−１）におけるカメラ座標系Ｃの法線ベクトルｎ（ｔ−１）を変換行列Ｔ _ＢＣ と変換行列Ｔ _ＥＢ （ｔ−１）を用いて慣性座標系Ｅの法線ベクトルｎ _Ｅ （ｔ−１）に変換し、
   次いで、現時刻（ｔ）における姿勢角を用いて慣性座標系Ｅから車体座標系Ｂに変換する逆変換行列Ｔ _ＢＥ （ｔ）を求め、
   前時刻（ｔ−１）における前記慣性座標系Ｅの法線ベクトルｎ _Ｅ （ｔ−１）を現時刻（ｔ）における逆変換行列Ｔ _ＢＥ （ｔ）と逆変換行列Ｔ _ＣＢ を用いてカメラ座標系Ｃの法線ベクトルｎ（ｔ）に変換し、
   さらに、前記平面の修正された法線ベクトルと、基準とする一方の前記撮像手段から前記平面までの距離とからなる平面パラメータを変動パラメータとし、前記固定パラメータとして記憶されている前記回転行列及び前記並進ベクトルを用いて、一方の前記撮像手段により撮像された画像を変換し、前記画像に含まれる平面を他方の前記撮像手段により撮像された画像に含まれる平面に最適に重畳させる射影変換行列を動的に推定する射影変換行列推定手段を備え、
   推定された前記射影変換行列を用いて平面を検出することを特徴とする平面検出装置。
3. 撮像手段により撮像した画像から平面を検出する平面検出方法であって、
   移動体に所定の位置関係で固定して配置された一対の撮像手段を用いて、平面を含む相互に重複する領域の画像を撮像するステップと、
   ジャイロセンサにより、前記移動体の姿勢角を検出するステップと、
   法線ベクトル修正手段により、前記姿勢角を用いて記憶された前記平面の法線ベクトルを修正するステップと、
   射影変換行列推定手段により、前記平面の修正された法線ベクトルと、基準とする一方の前記撮像手段から前記平面までの距離とからなる平面パラメータを変動パラメータとし、一方の前記撮像手段の座標系を他方の前記撮像手段の座標系に変換する固定パラメータとしての回転行列及び並進ベクトルを用いて、一方の前記撮像手段により撮像された画像を変換し、前記画像に含まれる平面を他方の前記撮像手段により撮像された画像に含まれる平面に最適に重畳させる射影変換行列を動的に推定するステップと、を有し、
   前記法線ベクトルを修正するステップにおいて、
   ジャイロセンサにより姿勢角を計測し、
   前時刻（ｔ−１）における姿勢角を用いてカメラ座標系Ｃの法線ベクトルｎ（ｔ−１）を慣性座標系Ｅの法線ベクトルｎ _Ｅ （ｔ−１）に変換し、
   次いで、現時刻（ｔ）における姿勢角を用いて慣性座標系Ｅの法線ベクトルｎ _Ｅ （ｔ−１）をカメラ座標系Ｃの法線ベクトルｎ（ｔ）に変換し、
   推定された前記射影変換行列を用いて平面を検出することを特徴とする平面検出方法。
4. 撮像手段により撮像した画像から平面を検出する平面検出方法であって、
   移動体に所定の位置関係で固定して配置された一対の撮像手段を用いて、平面を含む相互に重複する領域の画像を撮像するステップと、
   ジャイロセンサにより、前記移動体の姿勢角を検出するステップと、
   法線ベクトル修正手段により、前記姿勢角を用いて記憶された前記平面の法線ベクトルを修正するステップと、
   射影変換行列推定手段により、前記平面の修正された法線ベクトルと、基準とする一方の前記撮像手段から前記平面までの距離とからなる平面パラメータを変動パラメータとし、一方の前記撮像手段の座標系を他方の前記撮像手段の座標系に変換する固定パラメータとしての回転行列及び並進ベクトルを用いて、一方の前記撮像手段により撮像された画像を変換し、前記画像に含まれる平面を他方の前記撮像手段により撮像された画像に含まれる平面に最適に重畳させる射影変換行列を動的に推定するステップと、を有し、
   前記法線ベクトルを修正するステップにおいて、
   カメラ座標系Ｃを車体座標系Ｂに変換する変換行列Ｔ _ＢＣ とその逆変換行列Ｔ _ＣＢ とを予め記憶し、
   前時刻（ｔ−１）における姿勢角を用いて車体座標系Ｂから慣性座標系Ｅに変換する変換行列Ｔ _ＥＢ （ｔ−１）を求め、
   前時刻（ｔ−１）におけるカメラ座標系Ｃの法線ベクトルｎ（ｔ−１）を変換行列Ｔ _ＢＣ と変換行列Ｔ _ＥＢ （ｔ−１）を用いて慣性座標系Ｅの法線ベクトルｎ _Ｅ （ｔ−１）に変換し、
   次いで、現時刻（ｔ）における姿勢角を用いて慣性座標系Ｅから車体座標系Ｂに変換する逆変換行列Ｔ _ＢＥ （ｔ）を求め、
   前時刻（ｔ−１）における前記慣性座標系Ｅの法線ベクトルｎ _Ｅ （ｔ−１）を現時刻（ｔ）における逆変換行列Ｔ _ＢＥ （ｔ）と逆変換行列Ｔ _ＣＢ を用いてカメラ座標系Ｃの法線ベクトルｎ（ｔ）に変換し、
   推定された前記射影変換行列を用いて平面を検出することを特徴とする平面検出方法。