JP2013002883A - 測距装置 - Google Patents

Abstract

【課題】対象物が存在する路面の勾配の影響を適切に補償しつつ、車載カメラによる撮像画像を用いて対象物と自車両との間の距離を精度よく測定することができる測距装置を提供する。
【解決手段】互いに異なる撮像時刻での車載カメラ２の２つの撮像画像から、自車両１からの距離が第１撮像画像の撮像時刻での対象物５３の接地点Ｐ53と同じになる静止点Ｐの投影点となる特徴点を抽出する。計測したカメラ運動パラメータと、各撮像画像の特徴点の位置と、車載カメラ２の高さＨcの値とから、車載カメラ２の下方に位置する路面５１上の点から対象物存在路面５２上の静止点に至る直線Ｌ２が車載カメラ２の光軸Ｌcに対してなす角度θを所定の演算式に基づいて推定する。角度θの推定値を用いて対象物５３と自車両１との間の距離Ｄを推定する。
【選択図】図２

Description

本発明は車載カメラの撮像画像を用いて車両周辺に存在する対象物と自車両との間の距離を測定する測距装置に関する。

車両の走行支援等を目的として、車両前部等に搭載される車載カメラによって車両周辺の撮像画像を取得し、その撮像画像を基に、車両周辺に存在する歩行者等の対象物と自車両との間の距離を測定する技術が従来より知られている。

例えば、車載カメラの光軸が、車両が走行している路面（車両存在路面）とほぼ平行である場合において、車両存在路面上の前方箇所に存在する対象物を車載カメラにより撮像した場合、その撮像画像での対象物の接地点の位置と画像中心との上下方向の偏差量と、車載カメラの焦点距離と、車載カメラの高さとから、対象物と自車両との間の距離を推定することができる。

ただし、この場合、対象物が存在する路面（対象物が接地している路面）が車両存在路面に対して勾配を有するような場合には、その勾配に依存して、撮像画像中での対象物の接地点の位置が上下にずれることとなる。

このため、撮像画像での対象物の接地点の位置と画像中心との上下方向の偏差量を利用して、対象物と自車両との間の距離を精度よく測定するためには、対象物が存在する路面の勾配に伴う影響を適切に補償する必要がある。

一方、路面の勾配に関する情報を取得する技術としては、例えば特許文献１、２に見られる技術が知られている。

特許文献１に見られる技術は、車両に搭載した加速度センサの出力に基づいて路面の傾斜角度を推定するものである。また、特許文献２に見られる技術は、レーザ光を車両前方の路面に投光し、その反射光の受光強度の時間変化パターンに基づいて、車両前方の路面の勾配の有無や、その勾配が登り勾配であるか降り勾配であるかを判定するものである。

特開２００６−１９４６７７号公報 特開２００３−６５７４０号公報

しかるに、特許文献１に見られる技術では、自車両が存在する路面の傾斜角度が判るだけなので、車両周辺の対象物が存在する路面の勾配に関する情報を取得することはできない。

また、特許文献２に見られる技術では、車両前方の路面の勾配の有無や、その勾配が登り勾配であるか降り勾配であるかが判るだけであり、その勾配の度合いや、その勾配が撮像画像における対象物の接地点の位置にどのような影響を及ぼすかを把握することは困難である。

従って、これらの特許文献１、２に見られる技術により得られる路面の勾配情報を、撮像画像に基づいて対象物と自車両との間の距離を測定する技術に適用することは困難である。

本発明はかかる背景に鑑みてなされたものであり、対象物が存在する路面の勾配の影響を適切に補償しつつ、車載カメラによる撮像画像を用いて対象物と自車両との間の距離を精度よく測定することができる測距装置を提供することを目的とする。

まず、本発明に関する基礎的な技術事項を説明しておく。

図１（ａ）に示すように、車載カメラ２を搭載した車両１が、平坦な路面５１上を走行している状況を想定する。この状況では、車両１の前方の路面５２上に、車両１（自車両１）からの距離Ｄを測定しようとする対象物５３（図示例では歩行者）が存在している。

この場合、対象物５３が存在する路面５２（対象物５３が接地している路面５２）は、車両１が存在する路面５１（車両１が接地している路面５１）と同じ平面上の路面（路面５１と面一の路面）となっている。以降、路面５１，５２をそれぞれ自車両存在路面５１、対象物存在路面５２ということがある。

また、図１（ｂ）は、図１（ａ）に示す状況において、車載カメラ２により撮像された撮像画像を概略的に示している。参照符号５１ａ，５２ａ，５３ａを付した部分が、それぞれ、自車両存在路面５１、対象物存在路面５２、対象物５３の画像を示している。

図１（ａ）に示す状況において、図示の如く、車載カメラ２の光軸Ｌcが自車両存在路面５１に対してなす角度をα、車載カメラ２から対象物５３の接地点Ｐ53（対象物５３と対象物存在路面５２との接触点）に至る直線Ｌ１が車載カメラ２の光軸Ｌcに対してなす角度をγ、車載カメラ２の自車両存在路面５１からの高さをＨcとする。

ここで、車載カメラ２の高さＨc（以降、カメラ高Ｈcということがある）は、より詳しくは、車載カメラ２の光学中心Ｃの自車両存在路面５１からの高さである。また、角度γに係わる上記直線Ｌ１は、より詳しくは、車載カメラ２の光学中心Ｃから対象物５３の接地点Ｐ53に至る直線である。また、角度α，γは、より詳しくは、車載カメラ２の光軸Ｌcを含み、且つ、自車両存在路面５１に対して垂直に立する平面の法線方向の軸周りでの角度である。その角度α，γは、図１（ａ）に示す状況では、車両１のピッチ方向での角度（すなわち車両１の車幅方向の軸周りの角度）に相当する。

そして、角度αは、車載カメラ２の光軸Ｌcが、車載カメラ２の光学中心Ｃを通って自車両存在路面５１に平行な平面に対して斜め下向きに延在する状況（図１（ａ）に示す状況）で正の角度とし、車載カメラ２の光軸Ｌcが当該平面に対して斜め上向きに延在する状況で負の角度とする。また、角度γは、上記直線Ｌ１が、車載カメラ２の光軸Ｌcに対して斜め下向きに延在する状況で正の角度とし、該直線Ｌ１が車載カメラ２の光軸Ｌcに対して斜め下向きに延在する状況で負の角度とする。

なお、図１（ａ）では、α≠０[deg]となっているが、α＝０[deg]であってもよい。

上記のように角度α，γを定義したとき、対象物５３の自車両１からの距離Ｄ（自車両存在路面５１に平行な方向での距離）と、角度α，γと、カメラ高Ｈcとの間には、次式（１）の関係が成立する。

Ｄ＝Ｈc／tan(α＋γ) ……（１）

次に、図２（ａ）に示すように、車両１の前方の対象物存在路面５２が、自車両存在路面５１に対して勾配を有する路面（傾斜した路面）となっている状況を想定する。この状況は、対象物存在路面５２の勾配状態だけが、図１（ａ）に示した状況と相違するものである。

なお、図２（ｂ）は、図２（ａ）に示す状況において、車載カメラ２により撮像された撮像画像を概略的に示している。そして、図１（ｂ）の場合と同様に、参照符号５１ａ，５２ａ，５３ａを付した部分が、それぞれ、自車両存在路面５１、対象物存在路面５２、対象物５３の画像を示している。

図２（ａ）に示す状況において、図中の角度α，γは、図１（ａ）の状況と同様に定義される角度である。加えて、この状況において、自車両存在路面５１のうちの車載カメラ２の下方に位置する点（詳しくは、車載カメラ２の光学中心Ｃから自車両存在路面５１に降ろした垂線が自車両存在路面５１と交わる点）から対象物５３の接地点Ｐ53に至る直線Ｌ２が自車両存在路面５１に対してなす角度をβ（＞０）とする。

なお、角度βは、角度α、γと同様に、車載カメラ２の光軸Ｌcを含み、且つ、自車両存在路面５１に対して垂直に起立する平面の法線方向の軸周りでの角度（図１（ｂ）に示す状況では、車両１のピッチ方向の角度）である。

上記のように角度α，γに加えて、角度βを定義したとき、対象物５３の自車両１からの距離Ｄと、角度α，γ，βと、カメラ高Ｈcとの間には、次式（２）の関係が成立する。

Ｄ＝Ｈc／（tan(α＋γ)＋tanβ） ……（２）

この場合、式（２）において、β＝０とすれば、式（２）は前記式（１）に一致する。そして、図１（ａ）に示した状況は、図２（ａ）におけるβを“０”とした状況に相当する。

従って、図１（ａ）及び図２（ａ）のいずれの状況であっても、車載カメラ２のカメラ高Ｈcの値と、角度α，γ，βの値（[rad]の単位での値）とが判れば、それらの値から上記式（２）の右辺の演算によって、対象物５３と自車両１との間の距離Ｄを特定できることとなる。

ここで、カメラ高Ｈcは、車両１の車体に対する車載カメラ２の取付位置によって規定されるので、一般には、その値は、既知の値として事前に設定しておくことができる。

また、角度γは、車載カメラ２の撮像画像を基に推定することができる。具体的には、図１（ｂ）又は図２（ｂ）に示すように、対象物５３の接地点Ｐ53を撮像画像に投影してなる点である画像上対象物接地点Ｐ53aと、撮像画像の画像中心Ｐca（車載カメラ２の撮像面と光軸Ｌcとの交点）との間の上下方向の位置偏差をΔｙとすると、次式（３）が成立する。なお、Ｆは車載カメラ２の焦点距離である。

tanγ＝Δｙ／Ｆ ……（３）

従って、撮像画像において、画像上対象物接地点Ｐ53aと画像中心Ｐcaとの間の上下方向の位置偏差Δｙを特定すれば、そのΔｙの値と焦点距離Ｆの値（これは一般に既知の値としてあらかじめ設定できる）とから上記式（３）に基づいて角度γを推定できることとなる。

補足すると、対象物存在路面５２上で、車両１からの距離が対象物５３の接地点Ｐ53と同じになる任意の静止点（対象物存在路面５２に対して固定された点）を車載カメラ２の撮像画像に投影してなる点は、例えば図１（ｂ）又は図２（ｂ）に示す如く、画像上対象物接地点Ｐ53aを通って撮像画像の横方向（水平方向）に延在するラインＬ53a上の点（例えば図中の点Ｐ１）となる。

この場合、この点（例えばＰ１）と、画像中心Ｐcaとの上下方向の間隔は、画像上対象物接地点Ｐ53aと画像中心Ｐcaとの上下方向の間隔Δｙに一致する。従って、角度γは、上記静止点を撮像画像に投影してなる点（例えばＰ１）と画像中心Ｐcaとの間隔Δｙから上記式（３）に基づいて推定することもできる。

また、角度αは、車両１の車体の姿勢（角度αの増減方向での姿勢）に応じて規定される。従って、車両１の車体の姿勢をジャイロセンサ等の適宜のセンサ等を用いて計測（適当なモデルを使用した推定でもよい）すれば、角度αの値を推定できることとなる。

例えば、図１（ａ）又は図２（ａ）に示す状況では、車両１の車体のピッチ方向の傾斜角度（ピッチ角）を計測すれば、そのピッチ角の計測値と、車両１の車体に対する車載カメラ２の取付姿勢により定まる光軸Ｌcの向き（車体に対するピッチ方向での向き）とから、角度αを推定できることとなる。

なお、車両１が、滑らかな路面５１（自車両存在路面５１）を定常走行しているような状況では、車両１の車体の姿勢は、一般に、車体の前後方向が自車両存在路面５１とほぼ平行になるような姿勢に維持される。従って、このような状況では、車両１の車体の姿勢を計測せずとも、角度αの値を、既知の値として事前に設定できることとなる。例えば、車載カメラ２の光軸Ｌcの方向が車両１の車体の前後方向とほぼ同方向である場合には、角度αの値を近似的に“０”であると見なすことができる。

また、角度βは、車載カメラ２によって、互いに異なる時刻で撮像した２つの撮像画像を利用することで、以下に説明する如く推定することが可能である。

まず、図３（ａ）に示すように、互いに異なる時刻ｔ1，ｔ2において、車載カメラ２の前方に存在する平坦な路面Ｓ（平面）を撮像した場合を想定する。そして、路面Ｓ上の任意の１つの静止点Ｐを、時刻ｔ1での撮像画像（以下、第１撮像画像という）に投影してなる点をＰ１、時刻ｔ2での撮像画像（以下、第２撮像画像という）に投影してなる点をＰ２とする。

この場合、第１撮像画像における投影点Ｐ１と、第２撮像画像における投影点Ｐ２とは、それぞれ各撮像画像における特徴点として抽出され、その点の各撮像画像での位置が特定されるものとする。

そして、第１撮像画像における投影点Ｐ１の位置を同次座標で表現してなる位置ベクトルを↑Ｐ１（≡[ｘ1,ｙ1，１]^T）、第２撮像画像における投影点Ｐ２の位置を同次座標で表現してなる位置ベクトルを↑Ｐ２（≡[ｘ2,ｙ2，１]^T）と表記する。

なお、ｘ1，ｙ1はそれぞれ第１撮像画像における投影点Ｐ１の横方向の座標位置、縦方向（上下方向）の座標位置である。同様に、ｘ2，ｙ2はそれぞれ第２撮像画像における投影点Ｐ２の横方向の座標位置、縦方向（上下方向）の座標位置である。また、添え字“Ｔ”は、転置を意味する。

また、時刻ｔ1と時刻ｔ2との間の期間での車載カメラ２の位置の空間的な並進変位を表す並進移動ベクトルを↑ｔ、時刻ｔ1と時刻ｔ2との間の期間での車載カメラ２の姿勢の空間的な回転変位を表す回転行列をＲとする。これらの並進移動ベクトル↑ｔ及び回転行列Ｒは、換言すれば、時刻ｔ1と時刻ｔ2との間の期間でのカメラ座標系の空間的な並進変位と回転変位とをそれぞれ表すものである。

なお、回転行列Ｒは直交行列である。また、上記カメラ座標系は、該車載カメラ２に対して固定された座標系である。より具体的には、カメラ座標系は、例えば、車載カメラ２の光学中心Ｃを原点として、車載カメラ２の撮像画像の横方向をＸ軸方向、縦方向（上下方向）をＹ軸方向、光軸Ｌcの方向をＺ軸方向とする３軸座標系である。

さらに、図３（ｂ）に示すように、路面Ｓの法線方向の単位ベクトル（以下、単に法線ベクトルという）を↑ｎ、時刻ｔ1，ｔ2のいずれか一方の時刻、例えば時刻ｔ1での車載カメラ２の光学中心Ｃと路面Ｓとの距離をｄとする。これらの↑ｎ，ｄは、それぞれ路面Ｓ（平面）の空間的な姿勢及び位置を規定する平面パラメータである。

なお、路面Ｓは、通常、車幅方向とほぼ平行であるので、車載カメラ２の撮像画像の横方向（カメラ座標系のＸ軸方向）にほぼ平行である。このため、時刻ｔ1における車載カメラ２の光軸Ｌcに対する路面Ｓの傾き角度を図３（ｂ）に示す如くθとした場合、路面Ｓの法線ベクトル↑ｎは、時刻ｔ1でのカメラ座標系における座標成分を用いて、↑ｎ＝[０，cosθ，sinθ]^Tと表記される。

ここで、第１撮像画像の投影点Ｐ１の位置ベクトル↑Ｐ１と、第２撮像画像の投影点Ｐ２の位置ベクトル↑Ｐ２との間の関係は、平面射影変換によって対応付けられる。具体的には、第１撮像画像の投影点Ｐ１の位置ベクトル↑Ｐ１を第２撮像画像の投影点Ｐ２の位置ベクトル↑Ｐ２に変換するためのホモグラフィ行列（平面射影変換行列）をＨとおくと、次式（４）に示す如く、↑Ｐ２は、Ｈ・↑Ｐ１に、ある比例定数ｋを乗じたベクトルとなる。

↑Ｐ２＝ｋ・Ｈ・↑Ｐ１ ……（４）

また、ホモグラフィ行列Ｈは、上記並進移動ベクトル↑ｔ、回転行列Ｒ、路面Ｓの平面パラメータ↑ｎ，ｄを用いて、次式（５）により与えられる。

Ｈ＝Ｒ^T−Ｒ^T・↑ｔ・(↑ｎ／ｄ)^T ……（５）

従って、式（５）を式（４）に適用することで、次式（６）が得られる。

↑Ｐ２＝ｋ・（Ｒ^T−Ｒ^T・↑ｔ・(↑ｎ／ｄ)^T）・↑Ｐ１ ……（６）

この式（６）において、↑Ｐ１、↑Ｐ２、Ｒ、↑ｔが既知であれば、↑ｎ／ｄの成分である(cosθ)／ｄ又は(sinθ)／ｄの値を次のように求めることができる。

すなわち、時刻ｔ1でのカメラ座標系で見た並進移動ベクトル↑ｔが座標成分を用いて↑ｔ＝[ｔ1，ｔ2，ｔ3]^Tと表記されるものとする。また、回転行列Ｒの転置行列Ｒ^Tの成分表示は、次式（７）により与えられるものとする。

さらに、変数ａ，ｂをそれぞれａ≡(cosθ)／ｄ、ｂ≡(sinθ)／ｄと定義し、↑ｎ／ｄ＝[０，ａ，ｂ]^Tとおく。このとき、式（６）は、↑Ｐ１，↑Ｐ２，Ｒ^T，↑ｔ，↑ｎ／ｄのそれぞれの成分を用いて、次式（８）により表される。

そして、この式（８）の第３行の式を用いて、第１行及び第２行からｋを消去すると、次式（１０ａ），（１０ｂ）が得られる。

なお、式（１０ａ），（１０ｂ）に関する変数ａ，ｂの係数行列（(ｅｆ)を第１行成分、(ｈｉ)を第２行成分とする２次の正方行列）の行列式は、“０”となるので、これらの式（１０ａ）、（１０ｂ）を連立方程式として、変数ａ，ｂの値を決定することはできない。

一方、ａ≡(cosθ)／ｄ、ｂ≡(sinθ)／ｄであるから、次式（１０ｃ）が成立する。

ａ²＋ｂ²＝１／ｄ² ……（１０ｃ）

従って、式（１０ａ）〜（１０ｃ）を連立方程式として用いることで、ａ，ｂの値を算出することができる。例えば、式（１０ａ），（１０ｂ）の両辺をそれぞれ加え合わせた式と、式（１０ｃ）とを用いてｂを消去すると、次式（１１）が得られる。

従って、変数ａの値は、式（１１）により示される二次方程式の解として求めることができる。この場合、式（１１）の解となるａの値は、一般には２つ存在することとなるものの、それらの２つの値のうち、式（１０ａ）、（１０ｂ）を満足し得るａの値を、最終的に、前記式（１０ａ）〜（１０ｃ）の全てを満たすａの値として選定すればよい。

なお、式（１０ａ）〜（１０ｃ）からａを消去して、上記と同様に、ｂの値を決定するようにすることもできることはもちろんである。また、ａ，ｂの一方の値から、式（１０ａ）又は（１０ｂ）により他方の値を決定することもできる。

このように↑Ｐ１、↑Ｐ２、Ｒ、↑ｔが既知であれば、前記式（１０ａ）〜（１０ｃ）に基づいて、変数ａ又はｂの値を算出することができることとなる。そして、ａ≡(cosθ)／ｄ、ｂ≡(sinθ)／ｄであるから、上記のように変数ａ又はｂの値を算出すれば、ｄの値を用いて次式（１２ａ）又は（１２ｂ）により、車載カメラ２の光軸Ｌcに対する路面Ｓの傾斜角度θを求めることができることとなる。

θ＝cos^-1(ａ・ｄ) ……（１２ａ）
θ＝sin^-1(ｂ・ｄ) ……（１２ｂ）

次に、時刻ｔ1で車載カメラ２により撮像された第１撮像画像が、前記した図２（ａ）の状況において撮像された画像であるとし、第２撮像画像の撮像時刻ｔ2が、時刻ｔ1の直後、又は直前の時刻であるとする。なお、時刻ｔ1，ｔ2の時間差は、その時間間隔内での対象物５３と自車両１との間の距離の変化が十分に微小なものとなるような短い時間差であるとする。

そして、時刻ｔ1、ｔ2において、それぞれ第１撮像画像、第２撮像画像に投影される前記静止点Ｐは、時刻ｔ1における自車両１からの距離が対象物５３の接地点Ｐ53と同一又はほぼ同一となるような前記対象物存在路面５２上の静止点であるとする。なお、該静止点Ｐは、時刻ｔ1での対象物５３の接地点Ｐ53と一致する静止点であってもよい。

さらに、図２（ａ）に示した状況で、車載カメラ２の光軸Ｌcを含んで自車両存在路面５１に対して垂直に起立する平面の法線方向（図２（ａ）の状況では車両１の車幅方向（紙面に垂直な方向））に平行で、且つ、直線Ｌ２を含むような仮想的な平面を、対象物５３が存在する仮想的な路面として想定し、以降、これを対象物存在仮想路面Ｓａという。そして、この対象物存在仮想路面Ｓａの法線ベクトルと、時刻ｔ1での車載カメラ２の光学中心Ｃから対象物存在仮想路面Ｓａまでの距離とをそれぞれ改めて↑ｎ、ｄとおく。

このとき、上記対象物存在仮想路面Ｓａの平面パラメータ↑ｎ、ｄを前記式（６）の右辺に適用した場合、第１撮像画像の投影点Ｐ１の位置ベクトル↑Ｐ１と、第２撮像画像の投影点Ｐ２の位置ベクトル↑Ｐ２との間には、式（６）により表される関係が近似的に成立すると考えられる。

従って、時刻ｔ1と時刻ｔ2との間の期間での車載カメラ２の位置及び姿勢の変化をそれぞれ表す移動並進ベクトル↑ｔ及び回転行列Ｒと、時刻ｔ1において自車両１からの距離が対象物５３の接地点Ｐ53と同一となるような対象物存在路面５２上の静止点Ｐを第１及び第２撮像画像にそれぞれ投影してなる投影点Ｐ１，Ｐ２の位置ベクトル↑Ｐ１，↑Ｐ２とを用いて、前記した如く変数ａ（≡(cosθ)／ｄ）又はｂ（≡(sinθ)／ｄ）値を算出した場合、その変数ａ又はｂの値は、近似的に、対象物存在仮想路面Ｓａに対応する↑ｎ／ｄの成分の値に相当するものとなる。

また、上記対象物存在仮想路面Ｓａと、時刻ｔ1での車載カメラ２の光学中心Ｃとの距離ｄは、近似的にカメラ高Ｈcに一致するものと考えられる。

従って、ｄの値として、カメラ高Ｈcを使用することで、そのｄの値（＝Ｈc）と、変数ａ又はｂの値とから前記式（１２ａ）又は式（１２ｂ）により、時刻ｔ1での車載カメラ２の光軸Ｌcに対する対象物存在仮想路面Ｓaの傾斜角度θ（換言すれば、図２（ａ）の直線Ｌ２の光軸Ｌcに対する傾斜角度）を推定することができることとなる。

一方、図２（ａ）を参照して判るように、車載カメラ２の光軸Ｌcに対する対象物存在仮想路面Ｓaの傾斜角度θは、θ＝α＋βとなる。従って、対象物５３の自車両１からの距離Ｄを前記式（２）に基づき測定するために必要となる前記角度βは、上記の如く推定した対象物存在仮想路面Ｓａの傾斜角度θと、前記角度αとから、次式（１３）により算出できることとなる。

β＝θ−α ……（１３）

以上のようにして、互いに異なる時刻ｔ1，ｔ2で車載カメラ２により撮像した２つの撮像画像を用いて、前記角度βの値を推定することが可能である。

そして、上記した如く角度γ、α、βの値を推定すれば、前記式（２）に基づいて、対象物５３と自車両１との間の距離Ｄを測定できることとなる。

以上説明した技術事項を前提として、以下に本発明を説明する。

本発明の測距装置は、車載カメラによって撮像された路面上の対象物と前記車載カメラが搭載された自車両との間の距離を測定する測距装置であって、
互いに異なる撮像時刻で前記車載カメラにより撮像された２つの撮像画像から、前記対象物が存在する路面である対象物存在路面上の静止点であって、自車両からの距離が前記２つの撮像画像のうちの一方である第１撮像画像の撮像時刻での前記対象物の接地点と同じになる静止点を前記２つの撮像画像にそれぞれ投影してなる特徴点を抽出して、前記２つの撮像画像のそれぞれにおける該特徴点の位置を特定する測距用特徴点抽出手段と、
前記２つの撮像画像のそれぞれの撮像時刻の間の期間における前記車載カメラの位置及び姿勢の変化を表すカメラ運動パラメータを計測するカメラ運動パラメータ計測手段と、
前記第１撮像画像において特定された前記特徴点の位置に基づき、前記車載カメラから前記静止点に至る直線が該車載カメラの光軸に対してなす角度である第１参照角度の値を推定する第１参照角度推定手段と、
前記カメラ運動パラメータと、前記２つの撮像画像のそれぞれにおける前記特徴点の位置と、前記車載カメラの高さと、前記第１撮像画像の撮像時刻において前記自車両が存在する路面である自車両存在路面のうちの前記車載カメラの下方に位置する点から前記静止点に至る直線が前記車載カメラの光軸に対してなす角度である第２参照角度との間の関係を表す演算式に基づいて、前記カメラ運動パラメータの計測値と、前記２つの撮像画像のそれぞれにおいて特定された前記特徴点の位置と、前記車載カメラの高さの設定値とから、該演算式における未知数としての前記第２参照角度の値を推定する第２参照角度推定手段と、
少なくとも前記第１参照角度の推定値と、前記第２参照角度の推定値と、前記車載カメラの高さの設定値とから前記対象物と自車両との間の距離を推定する距離推定手段とを備えることを特徴とする（第１発明）。

なお、第１発明における上記第１参照角度、第２参照角度は、より詳しくは、車載カメラの光軸を含む平面であって、且つ、自車両存在路面に対して垂直に起立する平面の法線方向又はこれとほぼ同方向の軸周りでの角度である。このことは、後述する第３参照角度についても同様である。

上記第１発明によれば、前記測距用特徴点抽出手段によって、自車両からの距離が前記２つの撮像画像のうちの第１撮像画像の撮像時刻での前記対象物の接地点と同じになるような前記対象物存在路面上の静止点を前記２つの撮像画像にそれぞれ投影してなる特徴点が抽出される。そして、前記２つの撮像画像のそれぞれにおける該特徴点の位置が特定される。これにより、前記した位置ベクトル↑Ｐ１，↑Ｐ２が特定されることとなる。

なお、第１撮像画像における前記特徴点は、例えば、第１撮像画像の撮像時刻での前記対象物の接地点を第１撮像画像に投影してなる点（図１（ｂ）又は図２（ｂ）示した例では、点Ｐ53ａ）を通る横方向のライン上で、輝度等の局所特徴を有する点として抽出することできる。そして、他方の撮像画像（第２撮像画像）における前記特徴点は、該第２撮像画像において、第１撮像画像の特徴点と同じような特徴を有する点として抽出することができる。このような特徴点としては、例えばハリスコーナ点や、最小固有値点等を利用することができる。

また、前記カメラ運動パラメータ計測手段によって、２つの撮像画像のそれぞれの撮像時刻の間の期間における前記車載カメラの位置及び姿勢の変化を表すカメラ運動パラメータが計測される。該カメラ運動パラメータは、前記並進移動ベクトル↑ｔと回転行列Ｒとを規定し得るようなパラメータであり、並進移動ベクトル↑ｔ及び回転行列Ｒそのものであってもよい。

なお、上記並進移動ベクトル↑ｔや回転行列Ｒ等のカメラ運動パラメータは、例えば車両に搭載したジャイロセンサ等のセンサの出力を基に計測することが可能である。あるいは、車載カメラの撮像画像を利用して計測することも可能である。その場合には、例えばStructure from Motion（以下ＳｆＭということがある）に基づく公知の画像処理手法によってカメラ運動パラメータ（例えば上記並進移動ベクトル↑ｔ及び回転行列Ｒ）を算出することができる。

さらに、前記第１参照角度推定手段によって、前記第１撮像画像における前記特徴点の位置に基づき、前記第１参照角度の値が推定される。この第１参照角度は、図１（ａ）又は図２（ａ）における角度γを意味するものである。従って、前記第１撮像画像における前記特徴点の位置に基づいて、第１参照角度（γ）の値を推定することができる。より具体的には、前記式（３）に基づいて第１参照角度（γ）の値を推定することができる。

また、前記第２参照角度推定手段によって、前記第２参照角度の値が推定される。この第２参照角度は、第１撮像画像の撮像時刻において前記自車両が存在する路面である自車両存在路面のうちの前記車載カメラの下方に位置する点から前記静止点に至る直線が前記車載カメラの光軸に対してなす角度である。従って、該第２参照角度は、車載カメラの光軸に対する前記対象物存在仮想路面（Ｓａ）の傾斜角度θに相当するものである。

従って、前記カメラ運動パラメータと、前記２つの撮像画像のそれぞれにおける前記特徴点の位置と、前記車載カメラの高さと、第２参照角度との間には、前記式（１０ａ）〜（１０ｃ）により示される如き関係が近似的に成立する。そして、その関係を表す演算式に基づいて、前記カメラ運動パラメータの計測値と、前記２つの撮像画像のそれぞれにおいて特定された前記特徴点の位置と、前記車載カメラの高さの設定値とから、前記第２参照角度（θ）の値を推定することができる。

そして、第１発明では、少なくとも前記第１参照角度の推定値と、前記第２参照角度の推定値と、前記車載カメラの高さの設定値とから前記対象物と自車両との間の距離を前記距離推定手段により推定する。

この場合、例えば、車両が平坦な路面上を定常走行しているような状況では、前記式（２）における角度α（車載カメラの光軸が自車両存在路面に対してなす角度）が、ほぼ一定の値（既知の値）となる。このため、角度αの値をあらかじめ定めた定数値として、上記第１参照角度（γ）の推定値と、第２参照角度（θ）の推定値と、車載カメラの高さの設定値とから、前記式（２）及び式（１３）に基づいて前記対象物と自車両との間の距離を推定できることとなる。

あるいは、角度αを適宜の手法により推定してもよい。その場合には、その角度αの推定値と、上記第１参照角度（γ）の推定値と、第２参照角度（θ）の推定値と、車載カメラの高さの設定値とから、前記式（２）及び式（１３）に基づいて前記対象物と自車両との間の距離を推定できることとなる。

かかる第１発明によれば、対象物存在路面が、自車両存在路面に対して勾配を有するような状況であっても、その勾配の度合いを反映させて、対象物と自車両との間の距離を推定することができる。従って、対象物存在路面の勾配の影響を適切に補償しつつ、車載カメラによる撮像画像を用いて対象物と自車両との間の距離を精度よく測定することが可能となる。

かかる第１発明では、前記カメラ運動パラメータ計測手段は、例えば、前記２つの撮像画像のそれぞれの撮像時刻の間の期間における前記車載カメラの位置の変位ベクトルである並進移動ベクトルと、該車載カメラの姿勢の角度変化量を表す回転行列とを前記カメラ運動パラメータとして計測する手段である。これらの並進移動ベクトル及び回転行列は、それぞれ、前記した並進移動ベクトル↑ｔ、回転行列Ｒを意味するものである。

そして、この場合、前記第２参照角度推定手段は、具体的には、前記演算式としての前記式（１０ａ）〜（１０ｃ）に基づいて前記第２参照角度（θ）の値を推定することができる（第２発明）。

ここで、前記式（１０ａ）〜（１０ｃ）と、その各変数の意味もしくは定義を改めて記載すると次の通りである。

上記第２発明によれば、上記式（１０ａ）〜（１０ｃ）に基づいて、前記カメラ運動パラメータの計測値（前記並進移動ベクトル↑ｔの各成分の値、並びに回転行列Ｒの各成分の値）と、前記２つの撮像画像のそれぞれにおいて特定された前記特徴点の位置（位置ベクトル↑Ｐ１，↑Ｐ２のそれぞれの各成分の値）とから、前記した如く変数ａ又はｂの値を求めることができる。そして、この変数ａ又はｂの値と、ｄの値としての車載カメラの高さの設定値（Ｈc）とから、前記式（１２ａ）又は（１２ｂ）により第２参照角度（θ）の推定値を求めることができる。

前記第１発明では、前記第１撮像画像の撮像時刻での前記車載カメラの光軸が前記自車両存在路面に対してなす角度である第３参照角度の値を推定する第３参照角度推定手段をさらに備え、
前記距離推定手段は、前記第１参照角度の推定値と、前記第２参照角度の推定値と、前記第３参照角度の推定値と、前記車載カメラの高さの設定値とから前記対象物と自車両との間の距離を推定することが好ましい（第３発明）。

この第３発明によれば、前記第３参照角度推定手段により、前記第３参照角度を推定する。この第３参照角度は、図１（ａ）又は図１（ｂ）における角度αを意味するものである。そして、この第３参照角度（α）の推定値を、前記対象物と自車両との間の距離を推定するために用いる。これにより、車両の走行時に前記第３参照角度（α）の実際の値の変動が生じる場合でも、対象物と自車両との間の距離を精度よく推定することが可能となる。

この第３発明では、前記第３参照角度は、種々様々な手法で推定することが可能であるが、その手法としては、次のような手法を用いることができる。

すなわち、前記カメラ運動パラメータ計測手段が、前記２つの撮像画像のそれぞれの撮像時刻の間の期間における前記車載カメラの位置の変位ベクトルである並進移動ベクトルを前記カメラ運動パラメータの構成要素として計測する手段である場合には、前記第３参照角度推定手段は、少なくとも前記計測された並進移動ベクトルの、前記車載カメラの光軸に対する向きに基づいて前記第３参照角度の値を推定する（第４発明）。

この場合、上記並進移動ベクトルの向きは、概ね前記自車両存在路面と平行な方向となるので、前記第３参照角度（α）は、基本的には、上記並進移動ベクトルの、前記車載カメラの光軸に対する向きに応じて規定される。従って、車載カメラの光軸に対する上記並進移動ベクトルの向きに基づいて、前記第３参照角度（α）の値を推定することができる。

また、前記第３参照角度は、例えば、次のような手法により推定するようにしてもよい。すなわち、前記カメラ運動パラメータ計測手段が、前記２つの撮像画像のそれぞれの撮像時刻の間の期間における前記車載カメラの位置の変位ベクトルである並進移動ベクトルと、該車載カメラの姿勢の角度変化量を表す回転行列とを前記カメラ運動パラメータとして逐次計測する手段である場合に、前記計測された並進移動ベクトルの、前記車載カメラの光軸に対する向きに基づいて、前記第３参照角度の第Ａ暫定推定値を逐次求める第３参照角度第Ａ推定手段と、前記計測された回転行列により示される角度変化量を積算することによって前記第３参照角度の第Ｂ暫定推定値を逐次求める第３参照角度第Ｂ推定手段とをさらに備え、前記第３参照角度推定手段は、前記第３参照角度の第Ａ暫定推定値の所定期間分の平均値と第Ｂ暫定推定値の該所定期間分の平均値との偏差を算出し、該偏差に応じて前記第Ｂ暫定推定値を補正することにより前記第３参照角度の推定値を決定する（第５発明）。

これによれば、前記第３参照角度第Ａ推定手段は、前記４発明の第３参照角度推定手段と同様に、前記並進移動ベクトルに向き（前記車載カメラの光軸に対する向き）に基づいて第３参照角度（α）の値を推定する。ただし、第５発明では、その推定値は、第Ａ暫定推定値（第３参照角度（α）の暫定的な推定値）とされる。

また、前記第３参照角度第Ｂ推定手段は、前記回転行列により示される前記車載カメラの姿勢の角度変化量として得られる第３参照角度（α）の角度変化量を積算することによって、第３参照角度（α）の暫定推定値（第Ｂ暫定推定値）を逐次求める。

ここで、前記第Ａ暫定推定値は、積算演算を必要とすることなく算出できることから、該第Ａ暫定推定値に誤差が累積していくようなことはない。ただし、前記第Ａ暫定推定値は、車両の姿勢の瞬時的な変動の影響を直接的に受けるために、第３参照角度（α）の実際の値に対して瞬時的な誤差を生じることがある。

これに対して、前記第Ｂ暫定推定値は、前記車載カメラの姿勢の異なる時刻間の相対的な変化量としての前記角度変化量を積算したものであるため、車両の姿勢の瞬時的な変動の影響を受け難い。ただし、第Ｂ暫定推定値は、該第Ｂ暫定推定値の初期値の誤差や累積的な誤差が含まれやすい。ひいては、該第Ｂ暫定推定値は、実際の第３参照角度（α）の値に対してオフセットを生じやすい。

そこで、第５発明では、前記第３参照角度推定手段は、前記第３参照角度（α）の第Ａ暫定推定値の所定期間分の平均値と第Ｂ暫定推定値の該所定期間分の平均値との偏差を算出し、該偏差に応じて前記第Ｂ暫定推定値を補正することにより前記第３参照角度（α）の推定値を決定する。

この場合、上記偏差は、第３参照角度（α）の実際の値に対する第Ｂ暫定推定値のオフセット分に相当するものとなる。このため、該偏差に応じて前記第Ａ暫定推定値を補正することによって、第Ｂ暫定推定値から上記オフセット分を除去することが可能となる。その結果、当該補正により得られる第３参照角度（α）の推定値を、第３参照角度（α）の実際の値に、より一層精度よく合致させることができることとなる。ひいては、対象物と自車両との間の距離の測定精度をより一層高めることができる。

なお、前記第３参照角度（α）の第Ａ暫定推定値と、累積していない第Ｂ暫定推定値（すなわち、車載カメラ２の姿勢の単位時間当たりの角度変化量）とからカルマンフィルタを用いて第３参照角度を推定するようにしてもよい。

また、上記第３〜第５発明は、前記第２発明と組み合わせてもよい。

図１（ａ）は車両と対象物との一例の状態を示す図、図１（ｂ）は図１（ａ）の状況で車載カメラにより撮像された撮像画像の概略を示す図。 図２（ａ）は車両と対象物との他の例の状態を示す図、図２（ｂ）は図２（ａ）の状況で車載カメラにより撮像された撮像画像の概略を示す図。 図３（ａ）車載カメラによる互いに異なる時刻ｔ1，ｔ2での撮像状態を示す図、図３（ｂ）は図３（ａ）の時刻ｔ1での撮像状態を示す図。 本発明の一実施形態における測距装置の構成を示すブロック図。 図５（ａ），（ｂ）は図４の第３Ａ参照角度第Ａ推定部の処理を説明するための図。

本発明の一実施形態を図１〜図５を参照して以下に説明する。

図４を参照して、本実施形態の測距装置１０は、先に図１（ａ）及び図２（ａ）に示した車両１に搭載されたものであり、前記車載カメラ２に接続された演算処理ユニット１１を備える。なお、図１（ａ）及び図２（ａ）では、車両１を模式的に図示したが、実際の車両１は例えば自動車である。

そして、本実施形態では、車載カメラ２は、車両１の前方を監視するために車両１の前部に搭載されており、車両１の前方の画像を撮像する。この場合、車載カメラ２は、その光軸Ｌcを車両１の前後方向に向けるようにして車両１の車体に取り付けられている。ただし、車載カメラ２の光軸Ｌcは、車両１の前後方向に対して多少の傾きを有していてもよい。

この車載カメラ２は、ＣＣＤカメラ又はＣＭＯＳカメラ等により構成され、撮像画像の画像信号を生成して出力する。その撮像画像は、カラー画像、モノトーン画像のいずれであってもよい。

なお、車載カメラ２は、車両１の後方又は側方の画像を撮像するように車両１に搭載されたカメラであってもよい。

演算処理ユニット１１は、図示を省略するＣＰＵ、ＲＡＭ、ＲＯＭ、インターフェース回路等を含む電子回路ユニットであり、車載カメラ２で生成された画像信号が入力される。

この演算処理ユニット１１は、実装されたプログラムを実行することによって実現される機能として、車載カメラ２の撮像画像を取得する画像取得部１２と、その撮像画像から対象物５３と自車両１との距離を測定するために使用する特徴点を抽出する測距用特徴点抽出部１３、車載カメラ２の運動パラメータを推定（計測）するカメラ運動推定部１４と、第１〜第３参照角度をそれぞれ推定する第１参照角度推定部１５、第２参照角度推定部１６及び第３参照角度推定部１７と、対象物５３と自車両１との間の距離の推定値（測定値）を求める距離推定部１８とを備える。該演算処理ユニット１１は、所定の演算処理周期でこれらの各機能部の処理を逐次実行することによって、車両１の前方に存在する対象物５３と自車両１との間の距離Ｄの推定値を逐次算出する。

以下に、演算処理ユニット１１の各機能部の処理を含めて、該演算処理ユニット１１の全体処理の詳細を説明する。

車載カメラ２は、演算処理ユニット１１から所定の演算処理周期で指示されるタイミングで車両１の前方の画像を撮像する。そして、その撮像により生成された画像信号が、車載カメラ２から演算処理ユニット１１の画像取得部１２に逐次取り込まれる。この画像取得部１２は、車載カメラ２から入力されるアナログ信号である画像信号（各画素毎の画像信号）をデジタルデータに変換し、図示しない画像メモリに記憶保持する。

この場合、画像取得部１２は、車載カメラ２によって互いに異なる時刻（所定の演算処理周期の時間間隔毎の時刻）で撮像された複数の撮像画像、例えば２つの撮像画像の組を定期的に更新しつつ、画像メモリに記憶保持する。以降の説明では、画像メモリに記憶保持される２つの撮像画像の撮像時刻を時刻ｔ1、ｔ2とし、時刻ｔ1での撮像画像を第１撮像画像、時刻ｔ2での撮像画像を第２撮像画像ということがある。なお、時刻ｔ2は時刻ｔ1の後の時刻及び前の時刻のいずれであってもよい。

画像取得部１２の処理によって画像メモリに記憶保持された２つの撮像画像（第１撮像画像及び第２撮像画像）は、測距用特徴点抽出部１３とカメラ運動推定部１４とに与えられ、それぞれの処理が次に実行される。

測距用特徴点抽出部１３の処理は、自車両１からの距離を測定しようとする対象物５３の接地点Ｐ53（対象物５３と対象物存在路面５２との接触点）を第１撮像画像に投影してなる画像上対象物接地点Ｐ53aを特定する画像上対象物接地点特定部１３ａの処理と、自車両１からの距離が上記接地点Ｐ53と同一（又はほぼ同一）となるような対象物存在路面５２上の静止点Ｐを第１撮像画像及び第２撮像画像に投影してなる特徴点Ｐ１，Ｐ２を各撮像画像から探索する特徴点探索部１３ｂとに大別される。

画像上対象物接地点特定部１３ａは、次のように画像上対象物接地点Ｐ53aを特定する処理を実行する。

画像上対象物接地点特定部１３ａは、まず、与えられた第１撮像画像において、自車両１との距離を測定しようとする所定種類の対象物５３の画像部分を探索する。本実施形態では、所定種類の対象物５３は、例えば歩行者（人）である。そして、画像上対象物接地点特定部１３ａは、第１撮像画像から、歩行者に特徴的な形状パターン等を有する画像部分を探索することで、該第１撮像画像に含まれる対象物５３（歩行者）の画像部分を抽出する。例えば、図１（ａ）又は図２（ａ）に示した状況では、図１（ｂ）又は図２（ｂ）の参照符号５３ａの部分が、対象物５３（歩行者）の画像部分として抽出される。

なお、撮像画像から、歩行者の画像部分を探索する手法は種々様々な手法が公知となっており、その公知の手法を用いて第１撮像画像から対象物５３としての歩行者を抽出するようにすればよい。

そして、画像上対象物接地点特定部１３ａは、上記の如く抽出した第１撮像画像中の対象物５３の画像上対象物接地点Ｐ53aを特定する。この場合、図１（ｂ）又は図２（ｂ）に示す如く、例えば第１撮像画像中の対象物５３の画像５３ａの最下点が画像上対象物接地点Ｐ53aとして特定される。

なお、歩行者の脚の先端部の画像が明瞭に得られないような場合には、例えば、該歩行者の体型が標準的な体型であると仮定して、第１撮像画像における該歩行者の頭部や肩部の位置から画像上対象物接地点Ｐ53aの位置を推定するようにしてもよい。

次いで、特徴点探索部１３ｂの処理が次のように実行される。

本実施形態では、車載カメラ２は、その撮像画像の横方向（カメラ座標系のｘ軸方向）が自車両存在路面５１とほぼ平行になるように車両１に搭載されている。

このため、自車両１からの距離が対象物接地点Ｐ53と同一の距離となるような対象物存在路面５２上の静止点を第１撮像画像に投影してなる点は、基本的には、図１（ｂ）又は図２（ｂ）に示す如く、画像上対象物接地点Ｐ53を通って横方向（カメラ座標系のｘ軸方向）に延在する直線Ｌ53a上に存在する。

そこで、特徴点探索部１３ｂは、対象物５３の画像５３ａの近辺の画像領域内において、上記直線Ｌ53a上で、対象物存在路面５２の画像５２ａ中に存在する特徴点（例えば図１（ｂ）又は図２（ｂ）の点Ｐ１）を探索し、その点を対象物存在路面５２上の静止点（自車両１からの距離が対象物接地点Ｐ53と同じになる静止点）を第１撮像画像に投影してなる特徴点として抽出する。

このような特徴点は、一般的には、その点を含む局所領域における輝度、彩度、色相等の状態量が、特徴的な（他の局所領域と区別可能な）大きさもしくは変化を示すような点である。例えばハリスコーナ点や、最小固有値点等が上記特徴点して探索される。

このように第１撮像画像における特徴点を抽出した後、特徴点探索部１３ｂは、次に第２撮像画像において、第１撮像画像の特徴点と同じ特徴を有する点を探索し、その探索した点を、上記静止点（自車両１からの距離が対象物接地点Ｐ53と同じになる対象物存在路面５２上の静止点）を第２撮像画像に投影してなる特徴点として抽出する。この第２撮像画像における探索処理は、例えばブロックマッチング等の公知の画像処理手法を用いて行なえばよい。

画像上対象物接地点特定部１３ａ及び特徴点探索部１３ｂの上記の処理によって、自車両１からの距離が対象物接地点Ｐ53と同じになる対象物存在路面５２上の静止点を第１撮像画像に投影してなる特徴点と、該静止点を第２撮像画像に投影してなる特徴点とが抽出されることとなる。このようにして、第１撮像画像及び第２撮像画像から抽出される特徴点が、図３（ａ）に示した特徴点Ｐ１，Ｐ２である。

そして、測距用特徴点抽出部１３は、このように抽出した特徴点Ｐ１，Ｐ２の各撮像画像での位置（前記位置ベクトル↑Ｐ１，↑Ｐ２の座標成分）を求めて出力する。

補足すると、本実施形態では、対象物５３（歩行者）が移動体であるため、第１撮像画像の撮像時刻ｔ1と、第２撮像画像の撮像時刻ｔ2とでは、対象物接地点Ｐ53が一般には一致しない。このため、本実施形態では、対象物接地点Ｐ53とは別の静止点を各撮像画像に投影してなる点を、上記特徴点として各撮像画像から抽出するようにした。

ただし、距離を測定しようとする対象物５３は、歩行者等の移動体でなくてもよく、固定設置物（静止物）であってもよい。その場合には、対象物としての固定設置物の接地点を各撮像画像に投影してなる点を、上記特徴点として各撮像画像から抽出するようにしてもよい。

前記カメラ運動推定部１４は、本実施形態では、与えられた２つの撮像画像（第１撮像画像及び第２撮像画像）と、車載カメラ２の単位時間当たりの撮像数を示すフレームレートと、図示しない車速センサにより検出された車両１の車速とから、ＳｆＭ（Structure from Motion）に基づく公知の画像処理手法を用いて、車載カメラ２の空間的な位置の変位ベクトルの推定値（計測値）としての前記並進移動ベクトル↑ｔと、２つの撮像画像の撮像時刻t1，t2の間の期間における車載カメラ２の空間的な姿勢の角度変化量の推定値（計測値）を示す前記回転行列Ｒとを算出する。

これらの並進移動ベクトル↑ｔと回転行列Ｒとは、例えば、第１撮像画像及び第２撮像画像にそれぞれ投影されている静止物体（固定設置物）の画像の各撮像画像での特徴点から得られるオプティカルフローに関するＳｆＭに基づく公知の手法（例えば、「Structure from Motion without Correspondence」／Frank Dellaert, Steven M.Seitz, Charles E.Thorpe, Sebastian Thrun／Computer Sience Department & Robotics Institute Canegie Mellon University, Pittsborgh PA 15213を参照）によって算出される。

補足すると、ＳｆＭに基づいて算出される並進移動ベクトルは、スケールが不定なものとなるが、上記フレームレートと車速の検出値とを基に、並進移動ベクトル↑ｔのスケールが決定される。

なお、車載カメラ２は車両１の車体と一体的に動くので、車体の姿勢を検出するためのジャイロセンサ等の姿勢センサや、車体の加速度を検出するための加速度センサが車両１に搭載されている場合には、例えばそれらセンサの出力に基づいて、上記並進移動ベクトル↑ｔや、回転行列Ｒを計測するようにしてもよい。

演算処理ユニット１１は、上記の如く測距用特徴点算出部１３及びカメラ運動推定部１４の処理を実行した後、次に、第１参照角度推定部１５、第２参照角度推定部１６、第３参照角度推定部１７の処理をそれぞれ実行する。なお、第１参照角度推定部１５の処理は、カメラ運動推定部１４の処理と並行して、又はその前に実行してもよい。また、第３参照角度推定部１７の処理は、測距用特徴点算出部１３の処理と並行して、又はその前に実行してもよい。

第１参照角度推定部１５には、測距用特徴点抽出部１３から第１撮像画像における前記特徴点Ｐ１の位置（又は前記位置ベクトル↑Ｐ１）が入力される。そして、第１参照角度推定部１５は、第１撮像画像の画像中心Ｐcaと、入力された特徴点Ｐ１との間の、上下方向（カメラ座標系のｙ軸方向）の位置偏差Δｙ（図１（ｂ）又は図２（ｂ）を参照）を算出する。そして、第１参照角度推定部１５は、この位置偏差Δｙと、図示しない記憶装置にあらかじめ記憶保持された車載カメラ２の焦点距離Ｆの値とから、前記式（３）に基づいて、第１参照角度としての角度γの推定値（＝tan^-1(Δｙ／Ｆ)）を算出する。

第２参照角度推定部１６には、測距用特徴点抽出部１３から第１撮像画像及び第２撮像画像のそれぞれにおける前記特徴点Ｐ１，Ｐ２の位置（又は前記位置ベクトル↑Ｐ１，↑Ｐ２）が入力されると共に、カメラ運動推定部１４から並進移動ベクトル↑ｔ及び回転行列Ｒの各成分値が入力される。

そして、第２参照角度推定部１６は、測距用特徴点抽出部１３からの入力値により示される前記特徴点Ｐ１，Ｐ２の位置ベクトル↑Ｐ１，↑Ｐ２の各成分値と、カメラ運動推定部１４から入力された並進移動ベクトル↑ｔ及び回転行列Ｒの各成分値と、前記式（１０ｃ）のｄ（前記対象物存在仮想路面Ｓａと時刻ｔ1での車載カメラ２の光学中心Ｃとの間の距離）の近似的な設定値として図示しない記憶装置にあらかじめ記憶保持されたカメラ高Ｈcの値とから、前記式（１０ａ）〜（１０ｃ）に基づいて、第２参照角度としての角度θの推定値を算出する。

具体的には、例えば前記式（１１）の二次方程式の係数ｒ，ｓ，ｔの値が、前記式（１０ａ）〜（１０ｃ）に関するただし書きの定義と、式（１１）に関するただし書きの定義とに従って、↑Ｐ１，↑Ｐ２，↑ｔ，Ｒの各成分値の値から算出される。そして、係数ｒ，ｓ，ｔの値を用いて、式（１１）の二次方程式の解の値が前記変数ａ（≡(cosθ)／ｄ）の値として算出される。

この場合、前記した如く、式（１１）の解となるａの値は、一般には２つ存在することとなるものの、それらの２つの値のうち、式（１０ａ）、（１０ｂ）を満足し得るａの値が選定される。

そして、この変数ａの値と、ｄの設定値（Ｈc）とから、前記式（１２ａ）により第２参照角度θの推定値が算出される。

なお、前記変数ａの代わりに、前記変数ｂ（≡(sinθ)／ｄ）の値を算出し、この変数ｂの値と、ｄの設定値（Ｈc）とから、前記式（１２ｂ）により第２参照角度θの推定値を算出するようにしてもよい。

第３参照角度推定部１７には、カメラ運動推定部１４から並進移動ベクトル↑ｔ及び回転行列Ｒの各成分値が入力される。

この第３参照角度推定部１７は、第３参照角度としての角度αの第Ａ暫定推定値αａと第Ｂ暫定推定値αｂとをそれぞれ算出する処理を実行する第３参照角度第Ａ暫定推定部１７ａ、第３参照角度第Ｂ暫定推定部１７ｂを備えている。

第３参照角度第Ａ暫定推定部１７ａは、入力された並進移動ベクトル↑ｔの向き（撮像時刻ｔ1での車載カメラ２の光軸Ｌcに対する向き）に基づいて、第３参照角度αの第Ａ暫定推定値αａを算出する。

この場合、並進移動ベクトル↑ｔは、車両１が時刻ｔ1で走行している自車両存在路面５１にほぼ平行（車両１の進行方向とほぼ平行）なベクトルと見なすことができる。

従って、車載カメラ２の光軸Ｌcに対する並進移動ベクトル↑ｔの向き（あるいは、並進移動ベクトル↑ｔに対する光軸Ｌcの向き）は、自車両存在路面５１に対して車載カメラ２の光軸Ｌcがなす角度、すなわち、第３参照角度αに応じたものとなる。

すなわち、図５（ａ），（ｂ）に示す如く、時刻ｔ1での車載カメラ２のカメラ座標系で見た並進移動ベクトル↑ｔが、車載カメラ２の光軸Ｌc（Ｚ軸）に対してなす角度αａが、第３参照角度αに相当するものとなる。

そこで、第３参照角度第Ａ暫定推定部１７ａは、本実施形態では、入力された並進移動ベクトル↑ｔが、時刻t1における車載カメラ２のカメラ座標系のＸＺ平面に対してなす角度αａを、第３参照角度αの暫定推定値として算出する。

具体的には、第３参照角度第Ａ暫定推定部１７ａは、時刻t1における車載カメラ２のカメラ座標系での並進移動ベクトル↑ｔ（＝[ｔ1，ｔ2，ｔ3]^T）の成分値から、次式（１４）により、第Ａ暫定推定値αａを算出する。

また、第３参照角度第Ｂ暫定推定部１７ｂは、入力された回転行列Ｒに基づいて、第３参照角度αの第Ｂ暫定推定値αｂを算出する。

具体的には、第３参照角度第Ｂ暫定推定部１７ｂは、入力された回転行列Ｒにより示される車載カメラ２の空間的な姿勢の角度変化量（２つの撮像画像の撮像時刻t1，t2の間の期間における角度変化量）のうちの車両１のピッチ方向（第３参照角度αの変化が生じる軸周り方向）の角度変化量を演算処理ユニット１１の演算処理周期で逐次積算（累積加算）することによって、第３参照角度αの第Ｂ暫定推定値αｂを算出する。

なお、第Ｂ暫定推定値αｂは、車両１の運転開始時等に初期化される。この場合、第Ｂ暫定推定値αｂの初期値としては、例えば、車両１が水平な路面上に停車した状態での第３参照角度αの値としてあらじめ定められた値（例えば“０”）、あるいは、前回の車両１の運転時に最終的に算出された第３参照角度αの値が設定される。

以上説明した第３参照角度第Ａ推定部１７ａ及び第３参照角度第Ｂ推定部１７ｂの処理によって、並進移動ベクトル↑ｔに基づく第３参照角度αの第Ａ暫定推定値αａと、回転行列Ｒに基づく第３参照角度αの第Ｂ暫定推定値αｂとが所定の演算処理周期で逐次算出される。

ここで、並進移動ベクトル↑ｔに基づく第Ａ暫定推定値αａは、車両１の車体の姿勢の瞬時的な変動の影響を直接的に受けるため、実際の第３参照角度に対して瞬時的なずれを生じやすいものの、実際の第３参照角度αに対する定常的なオフセットは一般には生じない。

一方、回転行列Ｒに基づく第Ｂ暫定推定値αｂの波形は、実際の第３参照角度αの波形に比較的精度よく合致するものの、実際の第３参照角度αに対して定常的なオフセットを生じやすい。このオフセットは、αｂの初期値の誤差、あるいは、瞬時的な誤差の累積に起因するものである。

そこで、本実施形態では、第Ａ暫定推定値αａ及び第Ｂ暫定推定値αｂの上記の性状を踏まえて、第３参照角度推定部１７は、以下に説明する如く第３参照角度αの推定値を逐次決定する。

すなわち、第３参照角度推定部１７は、現在時刻から所定時間前までの期間で算出された第Ａ暫定推定値αａと第Ｂ暫定推定値αｂとを時系列的に記憶保持する。この場合、記憶保持される第Ａ暫定推定値αａと第Ｂ暫定推定値αｂとは、新たな（最新の）αａ，αｂが算出される毎に更新される。

そして、第３参照角度推定部１７は、まず、記憶保持したαａ，αｂの時系列データのうち、所定の時間間隔の期間分の最新のデータ（現在時刻から所定の時間間隔前の過去時刻までの期間内のデータ）を抽出し、その抽出したαａ，αｂのそれぞれのデータの平均値を算出する。すなわち、第３参照角度推定部１７は、現在時刻から所定の時間間隔前の過去時刻までの期間内のαａの平均値αa_aveとαｂの平均値αb_aveとを算出する。なお、上記所定の時間間隔は、一定時間でもよいが、本実施形態では、例えば車両１の車速に応じて設定する（車速が大きいほど、上記所定の時間間隔を短い時間間隔に設定する）。

次いで、第３参照角度推定部１７は、第Ａ暫定推定値αａの平均値αa_aveと第Ｂ暫定推定値αｂの平均値αb_aveとの偏差（＝αa_ave−αb_ave）を算出する。そして、第３参照角度推定部１７は、この偏差によって、現在時刻での第Ｂ暫定推定値αｂ（αｂの最新値）を補正することによって、現在時刻での第３参照角度αの推定値を決定する。具体的には、第３参照角度推定部１７は、上記偏差を現在時刻での第Ｂ暫定推定値αｂに加算することによって、現在時刻での第３参照角度αの推定値を決定する。

以上の第３参照角度推定部１７の処理により、第３参照角度αの推定値が逐次決定される。なお、現在時刻から所定の時間間隔前の過去時刻までの期間内における各時刻でのαａとαｂとの偏差を求め、その偏差の該期間内での平均値を求めることによって、第Ａ暫定推定値αａの平均値αa_aveと第Ｂ暫定推定値αｂの平均値αb_aveとの偏差（＝αa_ave−αb_ave）を算出するようにしてもよい。

第１〜第３参照角度推定部１５〜１７でそれぞれ上記の如く求められた第１参照角度γの推定値、第２参照角度θの推定値、第３参照角度αの推定値は、前記距離推定部１８に入力される。そして、該距離推定部１８は、これらの入力値から対象物５３と自車両１との間の距離Ｄの推定値を算出する。

具体的には、距離推定部１８は、第２参照角度θの推定値から第３参照角度αの推定値を減算することにより（前記式（１３）により）、図２（ａ）に示した角度β、すなわち図中の直線Ｌ２（又は前記対象物存在仮想路面Ｓａ）が自車両存在路面５１に対してなす角度βの推定値を算出する。そして、距離測定値算出部１８は、この角度βの推定値と、第１参照角度γの推定値と、第３参照角度αの推定値と、カメラ高Ｈcの値とから、前記式（２）により、対象物５３と自車両１との間の距離Ｄの推定値を算出する。

以上が本実施形態における演算処理ユニット１１が実行する処理の詳細である。この処理によって、車両１の前方の路面に自車両１との間の距離を測定しようとする対象物５３が存在する場合に、その対象物５３と自車両１との間の距離Ｄが逐次、測定されることとなる。

ここで、本実施形態と本発明との対応関係について補足しておく。本実施形態では、前記測距用特徴点抽出部１３、カメラ運動推定部１４、第１参照角度推定部１５、第２参照角度推定部１６、第３参照角度推定部１７、距離推定部１８によって、それぞれ、本発明における測距用特徴点抽出手段、カメラ運動パラメータ計測手段、第１参照角度推定手段、第２参照角度推定手段、第３参照角度推定手段、距離推定手段が実現される。

さらに、第３参照角度第Ａ推定部１７ａ、第３参照角度第Ｂ推定部１７ｂによって、それぞれ、本発明における第３参照角度第Ａ推定手段、第３参照角度第Ｂ推定手段が実現される。

以上説明した実施形態によれば、第１参照角度γ及び第３参照角度αに加えて、前記第２参照角度θを推定することにより、車両１の前方の対象物存在路面５２の勾配の度合いを対象物５３と自車両１との間の距離Ｄの推定処理に反映させて、該距離Ｄの推定値を前記式（２）により求めることができる。このため、自車両存在路面５１に対する対象物存在路面５３の勾配の影響を適切に補償して、対象物５３と自車両１との間の距離Ｄを精度よく測定することができる。

また、対象物５３と自車両１との間の距離Ｄを推定するために用いる第２参照角度θを、単一の車載カメラ２による撮像画像を利用して推定することができるので、その推定のための専用的なセンサ等を必要とすることなく、第３参照角度θを推定することができる。

また、対象物５３と自車両１との間の距離Ｄを推定するために用いる第３参照角度αの推定処理においては、所定の演算処理周期で算出される前記第Ｂ暫定推定値αｂを、現在時刻から所定の時間間隔前の過去時刻までの期間内における前記第Ａ暫定推定値αａの平均値αa_aveと第Ｂ暫定推定値αｂの平均値αb_aveとの偏差（＝αa_ave−αb_ave）の分だけ補正することによって、最終的な第３参照角度αの推定値が逐次決定される。

これにより、第３参照角度αの推定値は、第Ｂ暫定推定値αｂの初期値の誤差や、瞬時的な誤差の累積に起因して該第Ｂ暫定推定値αｂが第３参照角度αの実際の値に対して生じるオフセット分を、第Ｂ暫定推定値αｂから除去したものとなるように決定されることとなる。このため、第３参照角度αの実際の値に精度よく合致する推定値を得ることができる。

また、上記偏差（＝αa_ave−αb_ave）を求めるために使用するαａ、αｂのサンプリング期間を規定する前記所定の時間間隔が、車速が大きいほど、短い時間に設定される。このため、該所定の時間間隔の期間は、その期間内での自車両１の車体の姿勢（特にピッチ方向の姿勢）に大きな変動が生じ難い期間に設定される。

この結果、第Ａ暫定推定値αａ及び第Ｂ暫定推定値αｂの信頼性の高い（Ｓ／Ｎ比の高い）平均値αa_ave，αb_aveを使用して、上記偏差（＝αa_ave−αb_ave）を算出することができる。このため、第３参照角度αの推定値の精度を効果的に高めることができる。

ひいては、本実施形態によれば、単一の車載カメラ２を使用した安価な構成で、対象物５３と自車両１との間の距離Ｄを精度よく推定できる。

なお、以上説明した実施形態では、第３参照角度αを逐次推定するようにしたが、車両１が平坦な路面上を定常走行しているような状況では、第３参照角度αの値は、ほぼ一定に維持される。そして、その第３参照角度αの値は、車体に対する車載カメラ２の取付姿勢により規定される既知の値となる。従って、このような状況では、第３参照角度αを逐次推定せずとも、該αの値をあらかじめ定められた既定値として、対象物５３と自車両１との距離Ｄを推定するようにしてもよい。

また、前記実施形態では、対象物５３を歩行者とした場合を例にとって説明したが、対象物５３は歩行者以外の移動体、例えば、歩行者以外の動物や他車両等であってもよい。あるいは、対象物５３は、固定接地物（静止物）であってもよい。対象物５３が固定接地物である場合には、測距用特徴点抽出部１３の処理では、前記した如く、対象物５３の接地点を各撮像画像に投影してなる点を前記特徴点Ｐ１，Ｐ２として使用してもよい。

１…車両、２…車載カメラ、１０…測距装置、１３…測距用特徴点抽出部（測距用特徴点抽出手段）、１４…カメラ運動推定部（カメラ運動パラメータ計測手段）、１５…第１参照角度推定部（第１参照角度推定手段）、１６…第２参照角度推定部（第２参照角度推定手段）、１７…第３参照角度推定部（第３参照角度推定手段）、１７ａ…第３参照角度第Ａ推定部（第３参照角度第Ａ推定手段）、１７ｂ…第３参照角度第Ｂ推定部（第３参照角度第Ｂ推定手段）、１８…距離推定部（距離推定手段）。

Claims (5)

1. 車載カメラによって撮像された路面上の対象物と前記車載カメラが搭載された自車両との間の距離を測定する測距装置であって、
   互いに異なる撮像時刻で前記車載カメラにより撮像された２つの撮像画像から、前記対象物が存在する路面である対象物存在路面上の静止点であって、自車両からの距離が前記２つの撮像画像のうちの一方である第１撮像画像の撮像時刻での前記対象物の接地点と同じになる静止点を前記２つの撮像画像にそれぞれ投影してなる特徴点を抽出して、前記２つの撮像画像のそれぞれにおける該特徴点の位置を特定する測距用特徴点抽出手段と、
   前記２つの撮像画像のそれぞれの撮像時刻の間の期間における前記車載カメラの位置及び姿勢の変化を表すカメラ運動パラメータを計測するカメラ運動パラメータ計測手段と、
   前記第１撮像画像において特定された前記特徴点の位置に基づき、前記車載カメラから前記静止点に至る直線が該車載カメラの光軸に対してなす角度である第１参照角度の値を推定する第１参照角度推定手段と、
   前記カメラ運動パラメータと、前記２つの撮像画像のそれぞれにおける前記特徴点の位置と、前記車載カメラの高さと、前記第１撮像画像の撮像時刻において前記自車両が存在する路面である自車両存在路面のうちの前記車載カメラの下方に位置する点から前記静止点に至る直線が前記車載カメラの光軸に対してなす角度である第２参照角度との間の関係を表す演算式に基づいて、前記カメラ運動パラメータの計測値と、前記２つの撮像画像のそれぞれにおいて特定された前記特徴点の位置と、前記車載カメラの高さの設定値とから、該演算式における未知数としての前記第２参照角度の値を推定する第２参照角度推定手段と、
   少なくとも前記第１参照角度の推定値と、前記第２参照角度の推定値と、前記車載カメラの高さの設定値とから前記対象物と自車両との間の距離を推定する距離推定手段とを備えることを特徴とする測距装置。
2. 請求項１記載の測距装置において、
   前記カメラ運動パラメータ計測手段は、前記２つの撮像画像のそれぞれの撮像時刻の間の期間における前記車載カメラの位置の変位ベクトルである並進移動ベクトルと、該車載カメラの姿勢の角度変化量を表す回転行列とを前記カメラ運動パラメータとして計測する手段であり、
   前記第２参照角度推定手段は、前記演算式としての次式（１０ａ）〜（１０ｃ）に基づいて前記第２参照角度の値を推定することを特徴とする測距装置。
   
3. 請求項１記載の測距装置において、
   前記第１撮像画像の撮像時刻での前記車載カメラの光軸が前記自車両存在路面に対してなす角度である第３参照角度の値を推定する第３参照角度推定手段をさらに備え、
   前記距離推定手段は、前記第１参照角度の推定値と、前記第２参照角度の推定値と、前記第３参照角度の推定値と、前記車載カメラの高さの設定値とから前記対象物と自車両との間の距離を推定することを特徴とする測距装置。
4. 請求項３記載の測距装置において、
   前記カメラ運動パラメータ計測手段は、前記２つの撮像画像のそれぞれの撮像時刻の間の期間における前記車載カメラの位置の変位ベクトルである並進移動ベクトルを前記カメラ運動パラメータの構成要素として計測する手段であり、
   前記第３参照角度推定手段は、少なくとも前記計測された並進移動ベクトルの、前記車載カメラの光軸に対する向きに基づいて前記第３参照角度の値を推定することを特徴とする測距装置。
5. 請求項３記載の測距装置において、
   前記カメラ運動パラメータ計測手段は、前記２つの撮像画像のそれぞれの撮像時刻の間の期間における前記車載カメラの位置の変位ベクトルである並進移動ベクトルと、該車載カメラの姿勢の角度変化量を表す回転行列とを前記カメラ運動パラメータとして逐次計測する手段であり、
   前記計測された並進移動ベクトルの、前記車載カメラの光軸に対する向きに基づいて、前記第３参照角度の第Ａ暫定推定値を逐次求める第３参照角度第Ａ推定手段と、前記計測された回転行列により示される角度変化量を積算することによって前記第３参照角度の第Ｂ暫定推定値を逐次求める第３参照角度第Ｂ推定手段とをさらに備え、
   前記第３参照角度推定手段は、前記第３参照角度の第Ａ暫定推定値の所定期間分の平均値と第Ｂ暫定推定値の該所定期間分の平均値との偏差を算出し、該偏差に応じて前記第Ｂ暫定推定値を補正することにより前記第３参照角度の推定値を決定することを特徴とする測距装置。