(57)【要約】【課題】単眼の画像撮像手段を外界情報取得手段とし、他の移動距離測定手段等を装置構成の必須要件としない、距離計測装置を実現すること。【解決手段】この距離計測装置１００の主要構成は、外界の画像を撮像する単眼の画像撮像手段１と、撮像時刻が相異なる複数の画像に基づいてそれらの画像上に撮像された外界の不動点の三次元空間上における空間座標Ｑを任意のスケールで暫定的に求める空間座標暫定手段２と、この暫定された不動点の空間座標Ｑに基づいて、走行中の路面を近似的に表す近似平面の平面方程式を暫定的に求める路面方程式暫定手段３と、該移動体における画像撮像手段１の路面からの取り付け高さｈと上記の平面方程式に基づいて、上記のスケールを求めるスケール決定手段４と、このスケールに基づいて上記の空間座標Ｑを修正する空間座標修正手段５から成っている。【選択図】図１
2006349607.tif 000002
【特許請求の範囲】
【請求項１】
路面上を走行する移動体に搭載して該移動体の外界に位置する物体までの距離を計測する距離計測装置において、
前記外界の画像を撮像する単眼の画像撮像手段と、
撮像時刻が相異なる複数の前記画像に基づいて、前記画像上に撮像された前記外界の不動点の三次元空間上における空間座標Ｑを任意のスケールで暫定的に求める空間座標暫定手段と、
暫定された前記不動点の前記空間座標Ｑに基づいて、走行中の路面を近似的に表す近似平面の平面方程式を暫定的に求める路面方程式暫定手段と、
前記移動体における前記画像撮像手段の路面からの取り付け高さｈと前記平面方程式に基づいて、前記スケールを求めるスケール決定手段と、
前記スケールに基づいて、前記空間座標Ｑを修正する空間座標修正手段と
を有する
ことを特徴とする距離計測装置。
【請求項２】
前記路面方程式暫定手段は、
前記不動点の前記空間座標Ｑの集合から、走行中の路面上に位置する可能性が高い路面候補点の空間座標ＱG の集合を選り抜く路面候補点選抜手段
を有し、
前記路面候補点選抜手段は、
前記画像上で定義される所定の路面候補領域上に撮像された不動点を前記路面候補点として選抜する
ことを特徴とする請求項１に記載の距離計測装置。
【請求項３】
前記路面候補点選抜手段によって選抜される前記路面候補点が、前記近似平面上又は前記近似平面の近傍に多く集中する様に、前記路面候補領域の範囲を随時動的に再定義する候補領域最適化手段を有する
ことを特徴とする請求項２に記載の距離計測装置。
【請求項４】
前記空間座標暫定手段は、
相異なる２つの前記画像間で特徴が一致する２点１組から成る特徴点の組を少なくとも８組以上求める対応点検出手段と、
前記画像上における各前記組の各特徴点の平面座標（ｘ１，ｘ２）に基づいて、相異なる２つの前記撮像時刻の間における前記移動体の相対的な位置姿勢を求める位置姿勢検出手段と
を有する
ことを特徴とする請求項１乃至請求項３の何れか１項に記載の距離計測装置。
【請求項５】
前記路面方程式暫定手段は、
ＬＭｅｄＳ法を用いて前記平面方程式を求める路面平面推定手段を有する
ことを特徴とする請求項１乃至請求項４の何れか１項に記載の距離計測装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、路面上を走行する移動体に搭載して該移動体の外界に位置する物体までの距離を計測する距離計測装置に関する。
本発明は、単眼の画像撮像手段を備えた移動体から外界に位置する物体までの距離を走行中の外乱などに対してロバストに測定する際に大いに有用なものである。
【背景技術】
【０００２】
移動体に搭載された単眼の画像撮像手段を用いてその移動体の移動量を測ることを可能とする従来技術としては、例えば下記の特許文献１に開示されているものがあるが、この従来技術においては、走行中の移動体のピッチ角の動的な変動に対する考慮がなされていないので、走行中にその移動体のピッチ角が動的に変動した場合や、或いは、路面の勾配又は曲率が大きな場合などには、その移動量や外界の物体までの距離を正確に求めることはできない。
【０００３】
この様な問題を解決するために提案されたその他の従来装置としては、例えば下記の特許文献２に開示されているものがある。この従来装置は、車両に搭載された単眼の撮像手段と、自車両の車速を検出する車速センサと、撮像手段によって得られた画像内で、道路近傍に固定された構造体を検出する構造体検出手段と、その構造体の画像変化の状態と自車両の車速とに基づいて、撮像手段からその構造体までの距離を推定する構造体距離推定手段と、画像上で走行路上に存在する計測対象の物体と上記の構造体との画像上の位置関係に基づいて撮像手段からその計測対象の物体までの距離を推定する物体距離推定手段とを備える点に特徴がある。
【０００４】
この従来装置では、移動体の移動中に相異なる位置姿勢で撮像された２枚の画像を用いてそれら２時刻間における該移動体の相対的な位置姿勢を求めるが、その際に、その位置関係における並進成分の絶対値、即ち、その２点間の距離をもこの２枚の画像から求めることは困難であるので、車速センサとタイマーを使った距離演算処理によってその２点間の距離を補足している。
【特許文献１】特開２００３−１７８３０９
【特許文献２】特開２００３−２４７８２４
【発明の開示】
【発明が解決しようとする課題】
【０００５】
しかしながら、上記の特許文献２に記載されている従来装置では、車速センサを必須の構成要素としているので、車速センサとのインターフェイスを具備しなければ、この様な従来装置を有効に動作させることはできない。
また、このため、車速センサを備えていない例えばロボットなどの様な各種の移動体へのこの従来装置の応用を期待することはできない。
【０００６】
本発明は、上記の課題を解決するために成されたものであり、その目的は、単眼の画像撮像手段を外界情報取得手段とし、車速センサとのインターフェイス又は車速センサ自身、或いはＧＰＳなどを利用した測位装置等を装置構成の必須要件としない、距離計測装置を実現することである。
【課題を解決するための手段】
【０００７】
上記の課題を解決するためには、以下の手段が有効である。
即ち、本発明の第１の手段は、路面上を走行する移動体に搭載して該移動体の外界に位置する物体までの距離を計測する距離計測装置において、外界の画像を撮像する単眼の画像撮像手段と、撮像時刻が相異なる複数の画像に基づいてそれらの画像上に撮像された外界の不動点の三次元空間上における空間座標Ｑを任意のスケールで暫定的に求める空間座標暫定手段と、この暫定された不動点の空間座標Ｑに基づいて、走行中の路面を近似的に表す近似平面の平面方程式を暫定的に求める路面方程式暫定手段と、該移動体における画像撮像手段の路面からの取り付け高さｈと上記の平面方程式に基づいて、上記のスケールを求めるスケール決定手段と、このスケールに基づいて上記の空間座標Ｑを修正する空間座標修正手段とを備えることである。
【０００８】
ただし、上記の空間座標暫定手段は、不動点の三次元空間上における上記の空間座標Ｑを求めるものであるが、上記の不動点に対する各種の取り扱いは、同時または並行に略同様に実施し得るその他の動点の取り扱いを特段妨げるものではない。その様な動点に係わる実施形態は、付加的または付随的なもので有り得、任意で良い。
【０００９】
また、本発明の第２の手段は、上記の第１の手段において、上記の路面方程式暫定手段に、上記の不動点の空間座標Ｑの集合から走行中の路面上に位置する可能性が高い路面候補点の空間座標ＱG の集合を選り抜く路面候補点選抜手段を設け、この路面候補点選抜手段によって、画像上で定義される所定の路面候補領域上に撮像された不動点を路面候補点として選抜することである。
【００１０】
また、本発明の第３の手段は、上記の第２の手段において、路面候補点選抜手段によって選抜される路面候補点が上記の近似平面上又はその近傍に多く集中する様に、上記の路面候補領域の範囲を随時動的に再定義する候補領域最適化手段を設けることである。
【００１１】
ただし、この様な最適化の方式としては、例えば、撮像された画像の各部の濃淡によってその路面候補領域を再定義（最適化）する方式や、路面上に描かれた白線などを検出してそれらを利用することによってその路面候補領域を再定義する方式や、例外値を継続的に与え続ける画像上の部分領域を随時除外しつつその路面候補領域を再定義する方式などが考えられる。勿論、これらの方式は、任意に適当に組み合わせても良い。
【００１２】
また、本発明の第４の手段は、上記の第１乃至第３の何れか１つの手段において、相異なる２つの画像間で特徴が一致する２点１組から成る特徴点の組を少なくとも８組以上求める対応点検出手段と、それらの画像上における各組の各特徴点の平面座標（ｘ１，ｘ２）に基づいて、相異なる２つの撮像時刻の間における上記の移動体の相対的な位置姿勢を求める位置姿勢検出手段とを上記の空間座標暫定手段に設けることである。
【００１３】
また、本発明の第５の手段は、上記の第１乃至第４の何れか１つの手段において、周知のＬＭｅｄＳ法（The Least Median Squares method ）を用いて上記の平面方程式を求める路面平面推定手段を上記の路面方程式暫定手段に設けることである。
【００１４】
ただし、本発明に適用可能なロバストな推定方法は、ＬＭｅｄＳ法に限定されるものではなく、周知または任意のロバスト推定方法を適用することができる。その様なロバスト推定方法としては例えば、誤差に適当な重み付けをするＭ-estimator等の各種の推定方法が存在する。
以上の本発明の手段により、前記の課題を効果的、或いは合理的に解決することができる。
【発明の効果】
【００１５】
以上の本発明の手段によって得られる効果は以下の通りである。
即ち、本発明の第１の手段によれば、移動体が走行中の路面に対する近似平面の傾きを動的に随時正確に求めることができるので、画像撮像手段のカメラ視点の路面に対する高さｈが既知定数であることを利用して上記の空間座標Ｑのスケールを正しく算出することができる。したがって、本発明の第１の手段によれば、走行中にその移動体のピッチ角が動的に変動した場合や、或いは、路面の勾配又は曲率が大きな場合であっても、走行中の該移動体の外界に位置する物体までの距離を、車速センサを用いなくても正確に計測することができる。
【００１６】
また、本発明の第２の手段によれば、路面の傾きを推定するために用いる不動点の座標データを、走行中の路面上に位置する可能性が高い不動点（路面候補点）の座標データだけに簡単に絞り込むことができる。このため、本発明の第２の手段によれば、路面の傾きを推定する処理のオーバーヘッドを効果的に削減することができると同時に、その推定精度を効果的に高めることができる。
【００１７】
また、本発明の第３の手段によれば、上記の第２の手段が随時動的に最適化されるので、路面の傾きを推定するために用いる不動点の座標データを、走行中の路面上に位置する可能性がより高い不動点（路面候補点）の座標データだけに、更に効果的に絞り込むことができる。このため、本発明の第３の手段によれば、路面の傾きを推定する処理のオーバーヘッドを更に効果的に削減することができると同時に、その推定精度を更に効果的に高めることができる。
【００１８】
また、本発明の第４の手段によれば、キャリブレーション行列や基礎行列などを用いた周知の数学的手法に準拠して、上記の空間座標暫定手段を最も簡単かつ合理的に構成することができる。したがって、本発明の第４の手段によれば、オーバーヘッドの少ない空間座標暫定手段を簡潔に構成することができる。
【００１９】
また、本発明の第５の手段によれば、路面の傾きをロバストに推定することが容易となる。即ち、例えば最小二乗法を用いた場合には、例外値を与えるデータが多い時や、例外値の外れ方が大きい時などに、所望の推定値（路面の傾き）に対してそれらの例外値が大きな影響を及ぼすことがあるが、本発明の第５の手段は、ロバストな推定方法に準拠するものであるので、本発明の第５の手段によれば、所望の推定値が例外値から大きな影響を受けることがない。
【発明を実施するための最良の形態】
【００２０】
以下、本発明を具体的な実施例に基づいて説明する。
ただし、本発明の実施形態は、以下に示す個々の実施例に限定されるものではない。
【実施例１】
【００２１】
図１に、本実施例１の距離計測装置１００の論理的な基本構造を示す。この距離計測装置１００は、路面上を走行する移動体に搭載して該移動体の外界に位置する物体までの距離を計測するための装置である。この距離計測装置１００の主要構成は、外界の画像を撮像する単眼の画像撮像手段１と、撮像時刻が相異なる複数の画像に基づいてそれらの画像上に撮像された外界の不動点の三次元空間上における空間座標Ｑを任意のスケールで暫定的に求める空間座標暫定手段２と、この暫定された不動点の空間座標Ｑに基づいて、走行中の路面を近似的に表す近似平面の平面方程式を暫定的に求める路面方程式暫定手段３と、該移動体における画像撮像手段１の路面からの取り付け高さｈと上記の平面方程式に基づいて、上記のスケールを求めるスケール決定手段４と、このスケールに基づいて上記の空間座標Ｑを修正する空間座標修正手段５から成っている。
【００２２】
また、上記の空間座標暫定手段２は、相異なる２つの画像間で特徴が一致する２点１組から成る特徴点の組を少なくとも８組以上求める対応点検出手段２１と、それらの画像上における各特徴点の組の各特徴点の平面座標（ｘ１，ｘ２）に基づいて相異なる２つの撮像時刻の間における上記の移動体の相対的な位置姿勢を求める位置姿勢検出手段２２を備えている。以下、この２点１組の平面座標（ｘ１，ｘ２）のことを対応点と言う。更にこの空間座標暫定手段２は、これらの各手段（対応点検出手段２１、位置姿勢検出手段２２）の出力結果に基づいて、それらの各対応点の三次元空間上における空間座標Ｑを算出する空間座標算出手段２３を備えている。
【００２３】
また、上記の路面方程式暫定手段３は、検出された不動点の空間座標Ｑの集合から、走行中の路面上に位置する可能性が高い路面候補点の空間座標ＱG の集合を選り抜く路面候補点選抜手段３１を備えている。更に、この路面方程式暫定手段３には、周知のＬＭｅｄＳ法（The Least Median Squares method ）を用いて上記の平面方程式を求める路面平面推定手段３２が備えられている。
【００２４】
図２のハードウェア構成図は、上記の距離計測装置１００のシステム構成を具体的な物理的構成に対応させて書いたものである。上記の距離計測装置１００は、物理的には図中の電子制御ユニット１００Ａと、上記の画像撮像手段１に対応する単眼のカメラ１００Ｂから構成されている。一方、上記の各演算処理手段（２，３，４，５などの各手段）は、図中の電子制御ユニット１００Ａによって実現されている。
例えば、ＲＯＭ１０２には、これらの各手段（２，３，４，５）を実現する制御プログラムが格納されている。ＣＰＵ１０１はＲＡＭ１０３が供する記憶領域を制御プログラムのロード領域や作業領域などとして用いる。インターフェイス部１０４は、入出力チャネルなどから成り、例えばカメラ１００Ｂ等との入出力処理に介在する。
【００２５】
以下、図３のゼネラルフローチャートを用いて、上記の距離計測装置１００の制御手順を説明する。上記の距離計測装置１００は、以下に説明するステップ３１０〜ステップ３５０をそれぞれ順次所定の撮像周期にしたがって、周期的に繰り返し実行する装置である。
まず最初に、ステップ３１０では、図１の画像撮像手段１により、互いに相異なる位置で撮像された複数の画像を、空間座標暫定手段２の初段を構成する対応点検出手段２１と、路面方程式暫定手段３の初段を構成する路面候補点選抜手段３１に出力する。
【００２６】
次に、ステップ３２０では、図１の空間座標暫定手段２によって、微分処理などによる特徴点の抽出処理やその特徴点に対する追跡処理などを実行して、各画像が撮像された２時刻間で対応する特徴点の組（即ち、上記の対応点（ｘ１，ｘ２））を求め、各対応点の３次元空間における空間座標Ｑを任意のスケールＳで暫定的に算出する。
【００２７】
この時、まず最初に、上記の対応点検出手段２１では、入力された複数の画像から特徴点を抽出する。ただし、ここで特徴点とは、周囲のその他の点とは区別できる追跡の容易な点のことを言う。これらの特徴点は２次元的に濃淡変化の勾配値が大きくなる画素を検出する方法（例えばTomasi-Kanade の方法やHarrisオペレータ）などを用いて上記の抽出処理を実行する。この時、各特徴点に対して、撮像時刻の異なる複数の画像間で対応する点とその周囲の点の輝度は変化しないという仮定を用いると、画像間で同定すべき特徴点、即ち、同一の不動点であることが期待される点の画像間での対応を容易に検出することができる。画像間で対応する点（即ち、上記の対応点（ｘ１，ｘ２））が求まると、それらの各平面座標（ｘ１，ｘ２）は、位置姿勢検出手段２２に出力される。
【００２８】
以下、図４を用いて、抽出された特徴点（不動点）と移動体の運動との関係を説明する。位置姿勢検出手段２２は、対応点検出手段２１により検出された対応点から、画像が撮像されたときの撮像装置の相対的な位置姿勢を計算する。この画像間の位置姿勢は図４に示すように、回転行列Ｒと並進ベクトルｔで構成される。例えば、第一の画像上の特徴点の平面座標ｘ１に対応する第二の画像上の特徴点の平面座標をｘ２とする。これらは同一の不動点を示す点である。この時、カメラの撮像特性に依る画像の歪みを補正するキャリブレーション行列Ｋを用いれば、それらの関係は次式の様に表すことができる。ただし、ここで、行列Ｆは周知の基礎行列を表すものである。
【数１】
2006349607.tif 000003
【００２９】
以上の構成に従えば、上記の２時刻間における８組以上の互いに対応する特徴点の組（即ち、同一の不動点）が求まっていれば、それに基づいて上記の基礎行列Ｆを算出することができ、更に、この時、カメラのキャリブレーション行列Ｋが既知であれば、上記の式からも分かる様に、その２時刻間における移動体の運動（＝その２時刻間における該移動体の回転行列Ｒと並進ベクトルｔ）を求めることができる。
言い換えれば、２枚の画像間に８点以上の対応点（不動点）があれば、上記の位置姿勢検出手段２２によって、回転行列Ｒと並進ベクトルｔを計算することができる。
【００３０】
ただし、実際には、求めた対応点が不動点を表していない場合も有り得るので、対応点検出手段２１によって、２０点〜２００点程度の対応点を求めておくことが望ましい。また、より望ましくは１００点前後の対応点を検出しておくと良い。この値を適当に定めておけば、処理の高速化と高い推定精度の確保を両立させることができる。
また、空間座標暫定手段２を実行する段階では、並進ベクトルｔの実際の大きさ（即ち、撮像実行の２時刻間における該移動体の移動距離）や、空間座標Ｑが定義されるべき座標系のスケールＳについては未知のままで良い。
【００３１】
その後、空間座標算出手段２３では、対応点検出手段２１により検出された対応点と、位置姿勢検出手段２２で検出された画像間の位置姿勢から、３角測量の原理により対応点の空間座標Ｑを計算する。このとき、撮像実行時の２時刻間における該移動体の移動距離は未知であるため、並進ベクトルｔの大きさをある適当な定数に暫定して、対応点の空間座標Ｑの計算を行う。
【００３２】
次に、図３のステップ３３０では、図１の路面方程式暫定手段３によって、走行中の路面を平面近似して、その近似平面の方程式を暫定的に求める。言い換えれば、走行中の路面の３次元空間における傾きを表すパラメータ（ａ，ｂ，ｃ）を求める。
この時、まず最初に、前述の路面候補点選抜手段３１では、画像上で定義される所定の路面候補領域上に撮像されていた不動点の空間座標Ｑを路面候補点の空間座標ＱG として選抜する。画像上に定義される路面候補領域の定義形態を図５に例示する。例えばこの様に、画像上の路面候補領域は、予め画像平面領域の下部に適当な大きさで定義しておく。ただし、この路面候補領域は、随時動的に再定義しても良い（本発明の第３の手段）。その様な再定義の実現方式としては、例えば、画像の濃淡情報により路面候補領域を再定義（最適化）する方式や、路面上に描かれた白線を検出して路面候補領域を再定義する方式などが考えられる。
【００３３】
次に、次段の路面平面推定手段３２では、路面候補点選抜手段３１から得られた路面候補点の空間座標ＱG を用いて、該移動体が走行中の路面に対する近似平面を推定する。図６は、この時に実行される制御手順を表すフローチャートである。ここで推定すべき近似平面の方程式は、直交座標系（Ｘ，Ｙ，Ｚ）において、次式（１）の様に暫定することができる。
（近似平面の方程式）
ａＸ＋ｂＹ＋ｃＺ＝１                                                …（１）
【００３４】
即ち、図１の路面平面推定手段３２では、この近似平面の傾きを示すパラメータａ，ｂ，ｃを求める。路面候補点の空間座標ＱG の集合には、路面上には無い空間座標も含まれ得るが、例えば図６の様にして、ＬＭｅｄＳ推定（ロバストな推定方法）を導入することによって、これらのパラメータａ，ｂ，ｃを正確に決定することができる。
【００３５】
以下、図６を用いて、路面平面推定手段３２を実現するための制御手順を詳細に例示的に説明する。
まず最初にステップ５０５及びステップ５０８では、この路面平面推定処理の初期化、即ち変数ｎ，ｅ，ｉの初期設定を行う。例えば、実数Ｅ（＞０）は後述の中央値ｅＭとして許容可能な誤差の最大値である。また、自然数Ｉは上記の処理によって求められた路面候補点の数である。
【００３６】
次に、ステップ５１０では、路面候補点選抜手段３１によって求められた路面候補点の空間座標ＱG の集合から、ランダムサンプリングによって３点をランダムに選択する。そして、この３点によって、次のステップ５１５では、所望の近似平面としてより相応しいか否かが評価されるべき平面が特定される。即ち、次のステップ５１５では、この３点を通る平面の傾き、即ち式（１）を満たす各係数ａ，ｂ，ｃを求める。
例えばこの様に、特にＬＭｅｄＳ法などを用いる場合には、最終的に求めるべきパラメータに対する推定処理は非線形の最適化問題に帰着するので、その最適化演算の高速化のためには準最適解を、例えば上記のステップ５１０の様なランダムサンプリングによって探索するRandom Sample Consensus 法が有効となる。
【００３７】
次に、ステップ５２０〜５３０では、全ての路面候補点の各空間座標ＱG ＝（ｘi ，ｙi ，ｚi ）（１≦ｉ≦Ｉ）について、次式（２）を満たす誤差評価関数ｅ（ｉ）をそれぞれ求める。ただし、この時、これらの算出結果はその値の上昇順に配列して退避しておくものとする。
（誤差評価関数ｅ（ｉ））
ｅ（ｉ）＝（ａｘi ＋ｂｙi ＋ｃｚi −１）2                           …（２）
【００３８】
次に、ステップ５４０では、上昇順に退避された上記の算出結果（：Ｉ個のｅ（ｉ））の中央値を変数ｅＭに格納する。ただし、ここで言う中央値とは、上昇順に配列されたＩ／２番目または（Ｉ＋１）／２番目の誤差評価関数ｅ（ｉ）のことを言う。
次に、ステップ５４５では、変数ｅと変数ｅＭとの大小関係を判定し、ｅＭ＜ｅならばステップ５５０に、そうでなければステップ５７０に処理を移す。
【００３９】
ステップ５５０では、以下に示す代入操作によって、各変数の内容を更新する。
（代入操作）
ｅ＝ｅＭ，Ａ＝ａ，Ｂ＝ｂ，Ｃ＝ｃ                                    …（３）
ただし、上記の各退避領域Ａ，Ｂ，Ｃは、所望の近似平面の傾きを表す前述のパラメータ（ａ，ｂ，ｃ）の各値を該サブルーチンの呼出元に引き渡すための各引数領域を表している。
【００４０】
ステップ５６０では、このサブルーチンの終了条件を判定する。即ち、所定の正定数εよりも、上記の変数ｅの値が小さければ、本サブルーチンの呼出元に復帰し、そうでなければステップ５７０に処理を移す。また、ステップ５７０とステップ５７５によっても、終了条件が確認される。即ち、定数Ｎは、以上の処理の最大繰り返し回数を規定する自然数であり、この終了条件が成立しない場合には、再度ステップ５０８に戻って、もう一度次の平面の特定処理などからそれ以下の処理を繰り返す。そして、これらの繰り返しによって、最適なパラメータ（ａ，ｂ，ｃ）が最終的に選別される。即ち、以上の路面平面推定処理（路面平面推定手段３２）によって、所望の近似平面の傾き（ａ，ｂ，ｃ）が、ステップ５５０の出力領域（Ａ，Ｂ，Ｃ）に書き込まれる。
【００４１】
ただし、撮像状況や撮像状態などの各種の不測の事情によって、何れの中央値ｅＭも許容可能な誤差の最大値Ｅを下回ることがなかった場合には、上記の出力領域（Ａ，Ｂ，Ｃ）は更新されない。したがって、この様な場合には、前回の撮像周期で求めたパラメータ（ａ，ｂ，ｃ）の値が継続的に採用される。
例えば以上の様にして、ロバストな推定演算を実行することができる。また、上記のパラメータＩ，Ｎ，Ｅ，εや、このサブルーチンの実行周期（即ち、撮像周期）などを最適化することにより、本サブルーチンの推定処理における演算処理オーバーヘッドの削減と推定精度の向上とを同時に合理的に両立することができる。
【００４２】
次に、図３のステップ３４０では、図１のスケール決定手段４によって、路面平面推定手段３２により推定した路面平面から、空間座標算出手段２３により計算された空間座標ＱのスケールＳを求める。この時、例えば、走行中の路面を近似する近似平面の傾きを図７のグラフに例示した様に、画像撮像手段１のカメラ視点を原点、光軸方向をＺ、画像の上向きの方向をＹとした座標系における、その近似平面のＹ軸切片の絶対値、即ち、暫定された空間座標Ｑの座標系におけるカメラ視点の該近似平面からの高さをＨとする。
この時、実際の撮像装置の設置高さをｈとすると、空間座標算出手段２３により計算された、即ち、空間座標暫定手段２によって暫定された空間座標ＱのスケールＳは、Ｓ＝Ｈ／ｈとして計算することができる。
【００４３】
次に、図３のステップ３５０では、空間座標算出手段２３により計算された空間座標Ｑを修正対象として、図１の空間座標修正手段５によって、上記の様にして求めたスケールＳを用いて、各座標値を修正する。即ち、暫定された上記の空間座標ＱのスケールＳを用いれば、上記の各空間座標Ｑ：（Ｘ，Ｙ，Ｚ）＝ (ｘi ，ｙi ，ｚi ) をそのスケールＳに基づいて修正することができ、その結果、画像撮像手段１のカメラ視点を原点とした、移動体の外界における実際の３次元空間に対応する所望の直交座標（ｘ，ｙ，ｚ）を改めて、次式（４）の様にして得ることができる。
（修正後の空間座標Ｑ′）
Ｑ′：（ｘ，ｙ，ｚ）＝（ｘi ／Ｓ，ｙi ／Ｓ，ｚi ／Ｓ）              …（４）
【００４４】
以上の手段を用いることにより、上記の距離計測装置１００を移動体に搭載すれば、走行中に路面の傾斜が動的に変化したり、移動体のピッチ角が動揺したりした場合にも、画像撮像手段１のカメラ視点からの、該移動体の外界にある、例えば電柱やガードレールや道路標識などの物体までの距離ｒを、次式（５）に従って正確に求めることができる。
（物体までの距離ｒ）
ｒ＝（ｘ2 ＋ｙ2 ＋ｚ2 ）1/2 ＝（ｘi 2 ＋ｙi 2 ＋ｚi 2 ）1/2 ／Ｓ    …（５）
【００４５】
〔その他の変形例〕
本発明の実施形態は、上記の形態に限定されるものではなく、その他にも以下に例示される様な変形を行っても良い。この様な変形や応用によっても、本発明の作用に基づいて本発明の効果を得ることができる。
【００４６】
（変形例１）
例えば、上記の実施例１では、路面候補領域を画像平面上の固定領域としたが、路面候補領域は、随時動的に再定義することがより望ましい。その様な最適化技法を用いた構成（本発明の第３の手段）によれば、演算時間の短縮や近似平面の推定精度の向上を同時に効果的に図ることができる。その様な最適化の方式としては、例えば、撮像された画像の各部の濃淡によってその路面候補領域を再定義（最適化）する方式や、路面上に描かれた白線などを検出してそれらを利用することによってその路面候補領域を再定義する方式や、或いは、画像上に既に定義されている路面候補領域の中から、例外値を継続的に与え続ける部分領域を随時除外しつつその路面候補領域を再定義する方式などが考えられる。勿論、これらの方式は、任意に適当に組み合わせても良い。
【産業上の利用可能性】
【００４７】
本発明の適用対象は、道路上を走行する移動体に限定されるものではなく、例えば床面上を移動するロボットなどにも適用することができる。また、路面の代わりに水面を考えることにより、船舶などにも応用することが可能である。
【図面の簡単な説明】
【００４８】
【図１】距離計測装置１００の論理的な基本構造を示す制御ブロック図
【図２】距離計測装置１００の物理的な基本構造を示すハードウェア構成図
【図３】距離計測装置１００の制御手順を例示するゼネラルフローチャート
【図４】抽出された特徴点（不動点）と移動体の運動との関係を説明する概念図
【図５】画像上に定義される路面候補領域の定義形態を例示する画面平面図
【図６】路面平面推定手段３２を実現する制御手順を例示するフローチャート
【図７】走行中の路面を近似する近似平面の傾きを例示するグラフ
【符号の説明】
【００４９】
１００  ：  距離計測装置
１００Ａ：  電子制御ユニット
１００Ｂ：  カメラ
Ｒ  ：  回転行列
ｔ  ：  並進ベクトル
【図１】
2006349607.tif 000004
【図２】
2006349607.tif 000005
【図３】
2006349607.tif 000006
【図５】
2006349607.tif 000007
【図６】
2006349607.tif 000008
【図７】
2006349607.tif 000009
【図４】
2006349607.tif 000010