(57)【特許請求の範囲】
【請求項１】
撮像手段により撮像した画像から平面を検出する平面検出装置であって、
移動体に所定の位置関係で固定して配置され、平面を含む相互に重複する領域の画像を撮像する一対の撮像手段と、
前記一対の撮像手段の一方の座標系を他方の座標系に変換する回転行列及び並進ベクトルを固定パラメータとして記憶するパラメータ記憶手段と、
前記移動体の姿勢角を計測するジャイロセンサと、
前記姿勢角を用いて記憶された前記平面の法線ベクトルを修正する法線ベクトル修正手段と、を備え、前記法線ベクトル修正手段は、
前時刻（ｔ−１）においてジャイロセンサにより計測された姿勢角を用いてカメラ座標系Ｃの法線ベクトルｎ（ｔ−１）を慣性座標系Ｅの法線ベクトルｎＥ（ｔ−１）に変換し、
次いで、現時刻（ｔ）においてジャイロセンサにより計測された姿勢角を用いて慣性座標系Ｅの法線ベクトルｎＥ（ｔ−１）をカメラ座標系Ｃの法線ベクトルｎ（ｔ）に変換し、
さらに、前記平面の修正された法線ベクトルと、基準とする一方の前記撮像手段から前記平面までの距離とからなる平面パラメータを変動パラメータとし、前記固定パラメータとして記憶されている前記回転行列及び前記並進ベクトルを用いて、一方の前記撮像手段により撮像された画像を変換し、前記画像に含まれる平面を他方の前記撮像手段により撮像された画像に含まれる平面に最適に重畳させる射影変換行列を動的に推定する射影変換行列推定手段を備え、
推定された前記射影変換行列を用いて平面を検出することを特徴とする平面検出装置。
【請求項２】
撮像手段により撮像した画像から平面を検出する平面検出装置であって、
移動体に所定の位置関係で固定して配置され、平面を含む相互に重複する領域の画像を撮像する一対の撮像手段と、
前記一対の撮像手段の一方の座標系を他方の座標系に変換する回転行列及び並進ベクトルを固定パラメータとして記憶するパラメータ記憶手段と、
前記移動体の姿勢角を計測するジャイロセンサと、
前記姿勢角を用いて記憶された前記平面の法線ベクトルを修正する法線ベクトル修正手段と、を備え、前記法線ベクトル修正手段は、
カメラ座標系Ｃを車体座標系Ｂに変換する変換行列ＴＢＣとその逆変換行列ＴＣＢとを予め記憶し、
前時刻（ｔ−１）における姿勢角を用いて車体座標系Ｂから慣性座標系Ｅに変換する変換行列ＴＥＢ（ｔ−１）を求め、
前時刻（ｔ−１）におけるカメラ座標系Ｃの法線ベクトルｎ（ｔ−１）を変換行列ＴＢＣと変換行列ＴＥＢ（ｔ−１）を用いて慣性座標系Ｅの法線ベクトルｎＥ（ｔ−１）に変換し、
次いで、現時刻（ｔ）における姿勢角を用いて慣性座標系Ｅから車体座標系Ｂに変換する逆変換行列ＴＢＥ（ｔ）を求め、
前時刻（ｔ−１）における前記慣性座標系Ｅの法線ベクトルｎＥ（ｔ−１）を現時刻（ｔ）における逆変換行列ＴＢＥ（ｔ）と逆変換行列ＴＣＢを用いてカメラ座標系Ｃの法線ベクトルｎ（ｔ）に変換し、
さらに、前記平面の修正された法線ベクトルと、基準とする一方の前記撮像手段から前記平面までの距離とからなる平面パラメータを変動パラメータとし、前記固定パラメータとして記憶されている前記回転行列及び前記並進ベクトルを用いて、一方の前記撮像手段により撮像された画像を変換し、前記画像に含まれる平面を他方の前記撮像手段により撮像された画像に含まれる平面に最適に重畳させる射影変換行列を動的に推定する射影変換行列推定手段を備え、
推定された前記射影変換行列を用いて平面を検出することを特徴とする平面検出装置。
【請求項３】
撮像手段により撮像した画像から平面を検出する平面検出方法であって、
移動体に所定の位置関係で固定して配置された一対の撮像手段を用いて、平面を含む相互に重複する領域の画像を撮像するステップと、
ジャイロセンサにより、前記移動体の姿勢角を検出するステップと、
法線ベクトル修正手段により、前記姿勢角を用いて記憶された前記平面の法線ベクトルを修正するステップと、
射影変換行列推定手段により、前記平面の修正された法線ベクトルと、基準とする一方の前記撮像手段から前記平面までの距離とからなる平面パラメータを変動パラメータとし、一方の前記撮像手段の座標系を他方の前記撮像手段の座標系に変換する固定パラメータとしての回転行列及び並進ベクトルを用いて、一方の前記撮像手段により撮像された画像を変換し、前記画像に含まれる平面を他方の前記撮像手段により撮像された画像に含まれる平面に最適に重畳させる射影変換行列を動的に推定するステップと、を有し、
前記法線ベクトルを修正するステップにおいて、
ジャイロセンサにより姿勢角を計測し、
前時刻（ｔ−１）における姿勢角を用いてカメラ座標系Ｃの法線ベクトルｎ（ｔ−１）を慣性座標系Ｅの法線ベクトルｎＥ（ｔ−１）に変換し、
次いで、現時刻（ｔ）における姿勢角を用いて慣性座標系Ｅの法線ベクトルｎＥ（ｔ−１）をカメラ座標系Ｃの法線ベクトルｎ（ｔ）に変換し、
推定された前記射影変換行列を用いて平面を検出することを特徴とする平面検出方法。
【請求項４】
撮像手段により撮像した画像から平面を検出する平面検出方法であって、
移動体に所定の位置関係で固定して配置された一対の撮像手段を用いて、平面を含む相互に重複する領域の画像を撮像するステップと、
ジャイロセンサにより、前記移動体の姿勢角を検出するステップと、
法線ベクトル修正手段により、前記姿勢角を用いて記憶された前記平面の法線ベクトルを修正するステップと、
射影変換行列推定手段により、前記平面の修正された法線ベクトルと、基準とする一方の前記撮像手段から前記平面までの距離とからなる平面パラメータを変動パラメータとし、一方の前記撮像手段の座標系を他方の前記撮像手段の座標系に変換する固定パラメータとしての回転行列及び並進ベクトルを用いて、一方の前記撮像手段により撮像された画像を変換し、前記画像に含まれる平面を他方の前記撮像手段により撮像された画像に含まれる平面に最適に重畳させる射影変換行列を動的に推定するステップと、を有し、
前記法線ベクトルを修正するステップにおいて、
カメラ座標系Ｃを車体座標系Ｂに変換する変換行列ＴＢＣとその逆変換行列ＴＣＢとを予め記憶し、
前時刻（ｔ−１）における姿勢角を用いて車体座標系Ｂから慣性座標系Ｅに変換する変換行列ＴＥＢ（ｔ−１）を求め、
前時刻（ｔ−１）におけるカメラ座標系Ｃの法線ベクトルｎ（ｔ−１）を変換行列ＴＢＣと変換行列ＴＥＢ（ｔ−１）を用いて慣性座標系Ｅの法線ベクトルｎＥ（ｔ−１）に変換し、
次いで、現時刻（ｔ）における姿勢角を用いて慣性座標系Ｅから車体座標系Ｂに変換する逆変換行列ＴＢＥ（ｔ）を求め、
前時刻（ｔ−１）における前記慣性座標系Ｅの法線ベクトルｎＥ（ｔ−１）を現時刻（ｔ）における逆変換行列ＴＢＥ（ｔ）と逆変換行列ＴＣＢを用いてカメラ座標系Ｃの法線ベクトルｎ（ｔ）に変換し、
推定された前記射影変換行列を用いて平面を検出することを特徴とする平面検出方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、例えば、自動車の運転者支援や自律移動車の自動運転を実現するため、これらの移動体に搭載した一対の撮像手段により撮像した画像から、前記移動体が走行可能な平面を検出する平面検出装置及び検出方法に関する。
【背景技術】
【０００２】
移動体に搭載した一対の撮像手段により撮像した画像から、移動体が走行可能な平面を検出する手段として、特許文献１が既に開示されている。
【０００３】
図１は特許文献１の手段が適用される車両の説明図である。
この図において、特許文献１の手段が適用される車両１０（移動体）のウィンドシールド上部の左右には、車両１０が走行する道路平面領域を含む画像を撮像する一対の撮像手段である基準カメラ１２及び参照カメラ１４が固定配置される。基準カメラ１２及び参照カメラ１４は、共通の撮像領域が設定されたＣＣＤカメラ等からなるステレオカメラであり、これらには、撮像した画像を処理することで道路平面領域と障害物となる可能性のある物体とを検出する演算処理装置１６が接続される。なお、以下の説明では、基準カメラ１２によって撮像された画像を「基準画像」と称し、参照カメラ１４によって撮像された画像を「参照画像」と称する。
【０００４】
図２は一対の撮像手段間での２次元射影変換を説明するための模式図であり、
図３は平面領域を抽出する手順の説明図である。
【０００５】
先ず、本発明に用いられる道路平面領域の抽出及び２次元射影変換の原理を説明する。
【０００６】
図２に示すように、空間中にある平面Π上の観測点Ｍ１が基準画像Ｉｂ及び参照画像Ｉｒに投影されるとき、基準画像Ｉｂ上での同次座標をｍｂ、参照画像Ｉｒ上での同次座標をｍｒとすると、各同次座標ｍｂ、ｍｒは、２次元射影変換行列Ｈによって関係付けられる。
【０００７】
ここで、Ｈは３×３の射影変換行列である。また、図２において、Ｏは基準カメラ１２の光学中心、Ｏ’は参照カメラ１４の光学中心、Ｒは基準カメラ座標系から参照カメラ座標系への回転行列、ｔは、参照カメラ座標系において参照カメラ１４の光学中心から基準カメラ１２の光学中心へ向かう並進ベクトル、ｄは基準カメラ１２の光学中心Ｏと平面Πとの距離、ｎは平面Πの法線ベクトルである。
【０００８】
道路等の走行可能な領域は、空間中でほぼ平面であると見なすことができれば、図３（Ａ）の参照画像Ｉｒに対して適切な射影変換（Ｈ１）を施すことにより、平面の部分に対して図３（Ｂ）（Ｃ）の基準画像Ｉｂに一致するような画像（図３（Ｃ））を得ることができる。
一方、平面上にない物体は、Ｈ１により射影変換した際に画像（図３（Ｃ）と図３（Ｄ））が一致しないため、その物体の領域は走行可能な領域でない、あるいは、障害物となり得る領域であると判断することができる。したがって、図３（Ｅ）のように、平面の拡がりを算出することにより、走行可能な領域及び障害物となる物体の検出が可能となる。
【０００９】
図４は図１の演算処理装置１６における処理の全体の流れを説明するフローチャートである。
この図において、先ず、ステップＳ０において、基準カメラ１２及び参照カメラ１４により、車両１０が走行する道路平面領域を含む基準画像及び参照画像を所定の時間間隔で撮像し、所定の画像処理を行った後、各時間毎の画像情報として記憶する。
【００１０】
次に、ステップＳ１において、記憶されている初期値パラメータを用いて、参照画像中の道路平面領域を基準画像中の道路平面領域に２次元射影変換する射影変換行列Ｈを動的に算出する。
【００１１】
次に、ステップＳ２において、ステップＳ１で算出した射影変換行列Ｈを用いて、基準画像から道路平面領域を抽出する。
【００１２】
ステップＳ３では、道路平面の法線ベクトルｎ及び基準カメラ１２の光学中心から道路平面までの距離ｄを平面パラメータとして算出する。
【００１３】
次に、ステップＳ４において、ステップＳ３で算出した平面パラメータを用いて、ステップＳ２で抽出した道路平面領域を道路面上に変換して自分の車両１０以外の障害物となる可能性のある物体を検出し、若しくは、ステップＳ２で抽出した道路平面領域以外の領域に対してステレオ計測することにより物体を検出する。
【００１４】
ステップＳ５では、抽出された各物体の時間毎の位置変化から、車両１０に対する各物体の相対速度ベクトルを算出する。
さらに、ステップＳ６では、ステップＳ３で算出した道路平面の傾きである法線ベクトルを用いて、道路面を上方から見た画像である仮想投影面（ＶＰＰ）画像を生成する。
【００１５】
そして、ステップＳ７では、ステップＳ６で生成されたＶＰＰ画像の各時刻での位置から車両１０の自車速度ベクトルを算出する。
【００１６】
さらに、ステップＳ８では、ステップＳ５で算出した物体の相対速度ベクトルと、ステップＳ７で算出した車両１０の自車速度ベクトルとを用いて、各物体の絶対速度ベクトルを算出する。
【先行技術文献】
【特許文献】
【００１７】
【特許文献１】特許第３９３７４１４号公報、「平面検出装置及び検出方法」
【発明の概要】
【発明が解決しようとする課題】
【００１８】
上述した特許文献１の手段により、低コスト並びに安定した道路平面領域及び障害物情報が取得可能となる。
【００１９】
特許文献１の手段では、平面領域の検出を行うために、ステップＳ１において、記憶された基準画像Ｉｂ及び参照画像Ｉｒを用いて、路面に対する射影変換行列Ｈを動的に推定している。
【００２０】
この動的推定を成功させるためには、初期値として解に近いものを与える必要があり、特許文献１の手段では、ある時刻ｔにおける射影変換行列Ｈの推定の際に、前時刻（ｔ−１）までの推定結果を利用している。
【００２１】
しかし車両の揺動が激しく、前時刻（ｔ−１）と現時刻（ｔ）で画像の撮影姿勢が大きく変化する場合には、前時刻の射影変換行列Ｈが初期値として妥当ではない場合がでてくる。
すなわち、前時刻の射影変換行列Ｈを初期値として用いた場合、射影変換行列Ｈの探索が別の値に収束したり、解が求まらない可能性がある。その場合、幅広くパラメータを振って解の推定を行う必要があり、推定に時間を要し、これにより、円滑な走行に支障をきたすことがある。
【００２２】
なお、初期値として妥当でなくなる条件は、撮影される画像のテクスチャにより異なるが、過去の試験により、１フレーム間で車両のピッチ角２°以上の変化で生じている。
【００２３】
本発明は、上述した問題点を解決するために創案されたものである。すなわち、本発明の目的は、車両の揺動が激しい場合でも、射影変換行列Ｈの初期値を短時間かつ正確に推定することができる画像から平面を検出する平面検出装置及び検出方法を提供することにある。
【課題を解決するための手段】
【００２４】
本発明によれば、撮像手段により撮像した画像から平面を検出する平面検出装置であって、
移動体に所定の位置関係で固定して配置され、平面を含む相互に重複する領域の画像を撮像する一対の撮像手段と、
前記一対の撮像手段の一方の座標系を他方の座標系に変換する回転行列及び並進ベクトルを固定パラメータとして記憶するパラメータ記憶手段と、
前記移動体の姿勢角を計測するジャイロセンサと、
前記姿勢角を用いて記憶された前記平面の法線ベクトルを修正する法線ベクトル修正手段と、を備え、前記法線ベクトル修正手段は、
前時刻（ｔ−１）においてジャイロセンサにより計測された姿勢角を用いてカメラ座標系Ｃの法線ベクトルｎ（ｔ−１）を慣性座標系Ｅの法線ベクトルｎＥ（ｔ−１）に変換し、
次いで、現時刻（ｔ）においてジャイロセンサにより計測された姿勢角を用いて慣性座標系Ｅの法線ベクトルｎＥ（ｔ−１）をカメラ座標系Ｃの法線ベクトルｎ（ｔ）に変換し、
さらに、前記平面の修正された法線ベクトルと、基準とする一方の前記撮像手段から前記平面までの距離とからなる平面パラメータを変動パラメータとし、前記固定パラメータとして記憶されている前記回転行列及び前記並進ベクトルを用いて、一方の前記撮像手段により撮像された画像を変換し、前記画像に含まれる平面を他方の前記撮像手段により撮像された画像に含まれる平面に最適に重畳させる射影変換行列を動的に推定する射影変換行列推定手段を備え、
推定された前記射影変換行列を用いて平面を検出することを特徴とする平面検出装置が提供される。
【００２５】
また、撮像手段により撮像した画像から平面を検出する平面検出装置であって、
移動体に所定の位置関係で固定して配置され、平面を含む相互に重複する領域の画像を撮像する一対の撮像手段と、
前記一対の撮像手段の一方の座標系を他方の座標系に変換する回転行列及び並進ベクトルを固定パラメータとして記憶するパラメータ記憶手段と、
前記移動体の姿勢角を計測するジャイロセンサと、
前記姿勢角を用いて記憶された前記平面の法線ベクトルを修正する法線ベクトル修正手段と、を備え、前記法線ベクトル修正手段は、
カメラ座標系Ｃを車体座標系Ｂに変換する変換行列ＴＢＣとその逆変換行列ＴＣＢとを予め記憶し、
前時刻（ｔ−１）における姿勢角を用いて車体座標系Ｂから慣性座標系Ｅに変換する変換行列ＴＥＢ（ｔ−１）を求め、
前時刻（ｔ−１）におけるカメラ座標系Ｃの法線ベクトルｎ（ｔ−１）を変換行列ＴＢＣと変換行列ＴＥＢ（ｔ−１）を用いて慣性座標系Ｅの法線ベクトルｎＥ（ｔ−１）に変換し、
次いで、現時刻（ｔ）における姿勢角を用いて慣性座標系Ｅから車体座標系Ｂに変換する逆変換行列ＴＢＥ（ｔ）を求め、
前時刻（ｔ−１）における前記慣性座標系Ｅの法線ベクトルｎＥ（ｔ−１）を現時刻（ｔ）における逆変換行列ＴＢＥ（ｔ）と逆変換行列ＴＣＢを用いてカメラ座標系Ｃの法線ベクトルｎ（ｔ）に変換し、
さらに、前記平面の修正された法線ベクトルと、基準とする一方の前記撮像手段から前記平面までの距離とからなる平面パラメータを変動パラメータとし、前記固定パラメータとして記憶されている前記回転行列及び前記並進ベクトルを用いて、一方の前記撮像手段により撮像された画像を変換し、前記画像に含まれる平面を他方の前記撮像手段により撮像された画像に含まれる平面に最適に重畳させる射影変換行列を動的に推定する射影変換行列推定手段を備え、
推定された前記射影変換行列を用いて平面を検出することを特徴とする平面検出装置が提供される。
【００２６】
また、本発明によれば、撮像手段により撮像した画像から平面を検出する平面検出方法であって、
移動体に所定の位置関係で固定して配置された一対の撮像手段を用いて、平面を含む相互に重複する領域の画像を撮像するステップと、
ジャイロセンサにより、前記移動体の姿勢角を検出するステップと、
法線ベクトル修正手段により、前記姿勢角を用いて記憶された前記平面の法線ベクトルを修正するステップと、
射影変換行列推定手段により、前記平面の修正された法線ベクトルと、基準とする一方の前記撮像手段から前記平面までの距離とからなる平面パラメータを変動パラメータとし、一方の前記撮像手段の座標系を他方の前記撮像手段の座標系に変換する固定パラメータとしての回転行列及び並進ベクトルを用いて、一方の前記撮像手段により撮像された画像を変換し、前記画像に含まれる平面を他方の前記撮像手段により撮像された画像に含まれる平面に最適に重畳させる射影変換行列を動的に推定するステップと、を有し、
前記法線ベクトルを修正するステップにおいて、
ジャイロセンサにより姿勢角を計測し、
前時刻（ｔ−１）における姿勢角を用いてカメラ座標系Ｃの法線ベクトルｎ（ｔ−１）を慣性座標系Ｅの法線ベクトルｎＥ（ｔ−１）に変換し、
次いで、現時刻（ｔ）における姿勢角を用いて慣性座標系Ｅの法線ベクトルｎＥ（ｔ−１）をカメラ座標系Ｃの法線ベクトルｎ（ｔ）に変換し、
推定された前記射影変換行列を用いて平面を検出することを特徴とする平面検出方法が提供される。
【００２７】
また、撮像手段により撮像した画像から平面を検出する平面検出方法であって、
移動体に所定の位置関係で固定して配置された一対の撮像手段を用いて、平面を含む相互に重複する領域の画像を撮像するステップと、
ジャイロセンサにより、前記移動体の姿勢角を検出するステップと、
法線ベクトル修正手段により、前記姿勢角を用いて記憶された前記平面の法線ベクトルを修正するステップと、
射影変換行列推定手段により、前記平面の修正された法線ベクトルと、基準とする一方の前記撮像手段から前記平面までの距離とからなる平面パラメータを変動パラメータとし、一方の前記撮像手段の座標系を他方の前記撮像手段の座標系に変換する固定パラメータとしての回転行列及び並進ベクトルを用いて、一方の前記撮像手段により撮像された画像を変換し、前記画像に含まれる平面を他方の前記撮像手段により撮像された画像に含まれる平面に最適に重畳させる射影変換行列を動的に推定するステップと、を有し、前記法線ベクトルｎを修正するステップにおいて、
カメラ座標系Ｃを車体座標系Ｂに変換する変換行列ＴＢＣとその逆変換行列ＴＣＢとを予め記憶し、
前時刻（ｔ−１）における姿勢角を用いて車体座標系Ｂから慣性座標系Ｅに変換する変換行列ＴＥＢ（ｔ−１）を求め、
前時刻（ｔ−１）におけるカメラ座標系Ｃの法線ベクトルｎ（ｔ−１）を変換行列ＴＢＣと変換行列ＴＥＢ（ｔ−１）を用いて慣性座標系Ｅの法線ベクトルｎＥ（ｔ−１）に変換し、
次いで、現時刻（ｔ）における姿勢角を用いて慣性座標系Ｅから車体座標系Ｂに変換する逆変換行列ＴＢＥ（ｔ）を求め、
前時刻（ｔ−１）における前記慣性座標系Ｅの法線ベクトルｎＥ（ｔ−１）を現時刻（ｔ）における逆変換行列ＴＢＥ（ｔ）と逆変換行列ＴＣＢを用いてカメラ座標系Ｃの法線ベクトルｎ（ｔ）に変換し、
推定された前記射影変換行列を用いて平面を検出することを特徴とする平面検出方法が提供される。
【発明の効果】
【００２８】
上記本発明の装置及び方法によれば、車体の姿勢角を計測し、記憶された平面の法線ベクトルｎを修正するので、車両に揺動が生じた場合でも、射影変換行列Ｈの探索に使用する初期値を補正することができる。
【００２９】
従って、未舗装路のように車両の揺動が激しい場所においても、前時刻（ｔ−１）における平面領域を基に現時刻（ｔ）の法線ベクトルｎ（ｔ）を求めるので、より安定して平面領域を求めることができる。また、安定して平面領域が検出できることにより、計算コストの高い初期探索を行う回数が低減でき、よりスムーズな走行が可能となる。
【図面の簡単な説明】
【００３０】
【図１】特許文献１の手段が適用される車両の説明図である。
【図２】一対の撮像手段間での２次元射影変換を説明するための模式図である。
【図３】特許文献１による平面領域を抽出する手順の説明図である。
【図４】図１の演算処理装置１６における処理の全体の流れを説明するフローチャートである。
【図５】本発明が適用される車両の説明図である。
【図６】本発明による平面検出装置の機能構成ブロック図である。
【図７】本発明による平面検出方法のフローチャートである。
【発明を実施するための形態】
【００３１】
以下、本発明の好ましい実施形態を図面を参照して説明する。なお各図において、共通する部分には同一の符号を付し、重複した説明は省略する。
【００３２】
図５は、本発明が適用される車両の説明図である。
この図において、本発明が適用される車両１０（移動体）の上部の左右には、車両１０が走行する道路平面領域を含む画像を撮像する一対の撮像手段である基準カメラ１２及び参照カメラ１４が固定配置される。基準カメラ１２及び参照カメラ１４は、共通の撮像領域が設定されたＣＣＤカメラ等からなるステレオカメラであり、これらには、撮像した画像を処理することで道路平面領域と障害物となる可能性のある物体とを検出する演算処理装置１６が接続される。  以上の構成は、特許文献１の構成と同一である。
【００３３】
図５において、本発明が適用される車両１０（移動体）は、さらに、ジャイロセンサ５２と法線ベクトル修正手段５４を備える。
ジャイロセンサ５２は、移動体１０に固定されており、移動体１０のオイラー角を常時検出する。
法線ベクトル修正手段５４は、検出されたオイラー角を用いて記憶された前記平面の法線ベクトルｎを修正する。
【００３４】
図５において、ＸＣ−ＹＣ−ＺＣはカメラ座標系Ｃであり、ＸＢ−ＹＢ−ＺＢは車体座標系Ｂであり、ＸＥ−ＹＥ−ＺＥは慣性座標系Ｅである。
【００３５】
一対の撮像手段（基準カメラ１２及び参照カメラ１４）は、移動体に所定の位置関係で固定して配置されているので、カメラ座標系Ｃと車体座標系Ｂの相対位置は一定であり、カメラ座標系Ｃを車体座標系Ｂに変換する変換行列ＴＢＣとその逆変換行列ＴＣＢとは予め既知である。
【００３６】
また、移動体１０の慣性座標系Ｅに対するオイラー角は、ロール角α、仰角β、ヨー角γからなり、車体座標系Ｂから慣性座標系Ｅに変換する変換行列ＴＥＢは、数１の式（１）で与えられる。また、慣性座標系Ｅから車体座標系Ｂに変換する逆変換行列ＴＢＥは、式（１）の逆行列ＴＥＢ-1として与えられる。
【００３７】
【数１】
0005455037.tif 000002
【００３８】
図６は、本発明による平面検出装置の機能構成ブロック図である。この図において、演算処理装置１６は、パラメータ記憶手段２６と射影変換行列推定手段２８を有する。
演算処理装置１６は、さらに特許文献１と同様に、画像処理部、画像記憶部、演算制御部、平面パラメータ算出部、平面領域抽出部、物体検出部、物体相対速度算出部、仮想投影面画像生成部（空間位置算出手段）、自車速度算出部、物体絶対速度算出部及び表示処理部を備える。これらの機能は、特許文献１と同様である。
【００３９】
図７は、本発明による平面検出方法のフローチャートである。
この図において、先ず、ステップＳ０において、基準カメラ１２及び参照カメラ１４により、車両１０が走行する道路平面領域を含む基準画像及び参照画像を所定の時間間隔で撮像し、画像処理部において所定の画像処理を行った後、画像記憶部に各時間毎の画像情報として記憶する。
【００４０】
次に、ステップＳ１において、パラメータ記憶手段２６に記憶されている初期値パラメータを用いて、参照画像中の道路平面領域を基準画像中の道路平面領域に２次元射影変換する射影変換行列Ｈを動的に算出する。
【００４１】
次に、ステップＳ２において、平面領域抽出部はステップＳ１で算出した射影変換行列Ｈを用いて、基準画像から道路平面領域を抽出する。
【００４２】
ステップＳ３では、道路平面の法線ベクトルｎ及び基準カメラ１２の光学中心から道路平面までの距離ｄを平面パラメータとして算出する。
以上のステップは、特許文献１の構成と同一である。
【００４３】
本発明による平面検出方法では、さらに法線ベクトル修正ステップＳ３１と、初期値算出ステップＳ３２とを有する。
法線ベクトル修正ステップＳ３１では、ジャイロセンサ５２で検出したオイラー角を用いて記憶された道路平面領域の法線ベクトルｎを修正する。
すなわち、法線ベクトル修正ステップＳ３１では、前時刻（ｔ−１）におけるオイラー角を用いてカメラ座標系Ｃの法線ベクトルｎ（ｔ−１）を慣性座標系Ｅの法線ベクトルｎＥ（ｔ−１）に変換し、
次いで、現時刻（ｔ）におけるオイラー角を用いて慣性座標系Ｅの法線ベクトルｎＥ（ｔ−１）をカメラ座標系Ｃの法線ベクトルｎ（ｔ）に変換する。
【００４４】
局所的に凹凸は存在しても、大局的に平坦な路面を走行している場合、慣性座標系において、前時刻（ｔ−１）における慣性座標系Ｅの法線ベクトルｎＥ（ｔ−１）と現時刻（ｔ）における慣性座標系Ｅの法線ベクトルｎＥ（ｔ）は、ほぼ同一となる。従って、ｎＥ（ｔ）＝ｎＥ（ｔ−１）が成り立つ。
また、前時刻（ｔ−１）における慣性座標系Ｅの法線ベクトルｎＥ（ｔ−１）は、前時刻（ｔ−１）におけるカメラ座標系Ｃの法線ベクトルｎ（ｔ−１）が既知であり、かつ前時刻（ｔ−１）におけるオイラー角も既知であるから、これらを用いて、以下のように算出することができる。
【００４５】
（１）カメラ座標系Ｃを車体座標系Ｂに変換する変換行列ＴＢＣとその逆変換行列ＴＣＢとを予め記憶する。前述したように、カメラ座標系Ｃと車体座標系Ｂの相対位置は一定であり、カメラ座標系Ｃを車体座標系Ｂに変換する変換行列ＴＢＣとその逆変換行列ＴＣＢとは予め既知である。また、ＴＣＢ＝ＴＢＣ−１である。
（２）前時刻（ｔ−１）におけるオイラー角（ロール角α、仰角β、ヨー角γ）を用いて、上述した式（１）により、車体座標系Ｂから慣性座標系Ｅに変換する変換行列ＴＥＢ（ｔ−１）を求める。
（３）前時刻（ｔ−１）におけるカメラ座標系Ｃの法線ベクトルｎ（ｔ−１）を変換行列ＴＢＣと変換行列ＴＥＢ（ｔ−１）を用いて、数２の式（２）により、慣性座標系Ｅの法線ベクトルｎＥ（ｔ−１）に変換する。
【００４６】
【数２】
0005455037.tif 000003
【００４７】
また、時刻ｔと時刻ｔ−１で、同一の平面を走行しているとすれば、現時刻（ｔ）における法線ベクトルｎ（ｔ）は、現時刻（ｔ）における慣性座標系Ｅの法線ベクトルｎＥ（ｔ）が前時刻（ｔ−１）における慣性座標系Ｅの法線ベクトルｎＥ（ｔ−１）と同一とみなすことができ、かつ現時刻（ｔ）におけるオイラー角も既知であるから、これらを用いて、以下のように算出することができる。
【００４８】
（４）現時刻（ｔ）におけるオイラー角（ロール角α、仰角β、ヨー角γ）を用いて慣性座標系Ｅから車体座標系Ｂに変換する逆変換行列ＴＢＥ（ｔ）を求める。なお、ＴＢＥ（ｔ）＝ＴＥＢ−１（ｔ）である。
（５）前時刻（ｔ−１）における前記慣性座標系Ｅの法線ベクトルｎＥ（ｔ−１）を現時刻（ｔ）における逆変換行列ＴＢＥ（ｔ）と逆変換行列ＴＣＢを用いて、数３の式（３）により、カメラ座標系Ｃの法線ベクトルｎ（ｔ）に変換する。
【００４９】
【数３】
0005455037.tif 000004
【００５０】
初期値算出ステップＳ３２では、数４の式（４）を用いて、射影変換行列Ｈを算出する。
ここで、Ｒは前記回転行列、ｔは前記並進ベクトル、Ａｂ、Ａｒは前記一対の撮像手段の内部パラメータ、ｎは前記平面の法線ベクトル、ｄは基準とする一方の前記撮像手段から前記平面までの距離、ｋは０でない係数である。
【００５１】
【数４】
0005455037.tif 000005
【００５２】
式（４）において、Ｒ、ｔ、Ａｂ、Ａｒ、ｋは既知の定数である。従って、数３の式（３）により求めたカメラ座標系Ｃの法線ベクトルｎ（ｔ）と、前時刻（ｔ−１）における距離ｄを初期値として、法線ベクトルｎ及び距離ｄを変化させ、前記一対の撮像手段により撮像された画像同士が最も重畳されるときの前記射影変換行列Ｈとして推定することにより、上述したステップＳ２において、ステップＳ１で算出した射影変換行列Ｈを用いて、基準画像から道路平面領域を抽出することができる。
【００５３】
上述した本発明の装置及び方法によれば、移動体１０に固定されたジャイロセンサ５２を用いて移動体１０の慣性座標系Ｅに対するオイラー角（ロール角α、仰角β、ヨー角γ）を検出し、このオイラー角を用いて記憶された前記平面の法線ベクトルｎを修正するので、車両１０に揺動が生じた場合でも、この揺動をジャイロセンサ５２で検出して射影変換行列Ｈの探索に使用する初期値を補正することができる。
【００５４】
すなわち、移動体（車両）１０にジャイロセンサ５２を搭載し、これにより計測される車体の姿勢角（オイラー角）を用いて、車両の揺動による初期値Ｈの変動を補正する。
【００５５】
従って、大局的に平坦な場所であれば、未舗装路のように車両の揺動が激しい場所においても、前時刻（ｔ−１）における平面領域を元に現時刻（ｔ）の法線ベクトルｎ（ｔ）を求めるので、より安定して平面領域を求めることができる。また、安定して平面領域が検出できることにより、計算コストの高い初期探索を行う回数が低減でき、よりスムーズな走行が可能となる。
【００５６】
なお、本発明は上述した実施形態に限定されず、本発明の要旨を逸脱しない範囲で種々に変更することができることは勿論である。
【符号の説明】
【００５７】
１０  車両（移動体）、１２  基準カメラ、１４  参照カメラ、
１６  演算処理装置、
２６  パラメータ記憶手段、２８  射影変換行列推定手段、
５２  ジャイロセンサ、５４  法線ベクトル修正手段
【図１】
0005455037.tif 000006
【図２】
0005455037.tif 000007
【図３】
0005455037.tif 000008
【図４】
0005455037.tif 000009
【図５】
0005455037.tif 000010
【図６】
0005455037.tif 000011
【図７】
0005455037.tif 000012