JPH0620189A

JPH0620189A - 道路形状計測装置

Info

Publication number: JPH0620189A
Application number: JP4171240A
Authority: JP
Inventors: Kazunori Noso; 千典農宗
Original assignee: Nissan Motor Co Ltd
Current assignee: Nissan Motor Co Ltd
Priority date: 1992-06-29
Filing date: 1992-06-29
Publication date: 1994-01-28
Anticipated expiration: 2014-07-12
Also published as: JP2917681B2

Abstract

(57)【要約】【目的】車両に搭載した撮像手段であるカメラの姿勢
が車両の走行により変動するという条件を考慮しなが
ら、多次曲線による３次元道路モデルを作成して道路の
３次元形状データを精度良く高速に計測し得る道路形状
計測装置を提供する。【構成】カメラで撮像した画像から車両の進行方向前
方の道路形状を前処理部１３で検出するとともに、道路
モデル作成部１５において３次元曲線パラメータに基づ
いて３次元座標上で定義された３次元道路モデルを演算
し、この３次元道路モデルを座標変換部１７においてカ
メラの姿勢パラメータに基づいて画像座標に変換し、線
対応マッチング部１９において前記道路形状と座標変換
された３次元道路モデルとを比較し線対応マッチングを
行って、両者間の位置ずれを検出し、この位置ずれから
３次元曲線パラメータの変化量および姿勢パラメータの
変化量をパラメータ推定部２１で推定し、パラメータ更
新部２３において各パラメータを更新している。

Description

【発明の詳細な説明】

【０００１】

【産業上の利用分野】本発明は、例えば車両の自動操縦
や予防安全運転等のために車両の進行方向前方の道路の
カーブや勾配等の形状を計測する道路形状計測装置に関
する。

【０００２】

【従来の技術】車両の自動操縦や追突防止等の予防安全
運転等のためには、車両の進行方向前方における道路の
形状データである例えば車両の道路端からの距離、道路
方向と車両の方向とのなすヨー角、道路のカーブ曲率で
ある水平曲率、道路の勾配変化である垂直曲率等を精度
良く高速に計測することが必要である。

【０００３】このような道路の３次元形状データは、車
両に搭載したカメラで撮像した画像から推定している
が、この場合、従来はカメラの道路に対する姿勢のう
ち、カメラの高さ、ピッチ角、ロール角が一定であると
いう仮定のもとに道路の形状データを計測している。

【０００４】ところで、車両に搭載したカメラは、車両
が走行すると、路面の凹凸等の影響でカメラ姿勢は変動
するが、カメラ姿勢が変動すると、道路の３次元形状デ
ータにも誤差が含まれることになる。しかしながら、実
際には車両が走行すると、路面の段差等の影響でカメラ
姿勢は変動するので、道路の３次元形状データを精度良
く求めるには、カメラの高さ、ピッチ角、ロール角等も
変動するものとしてカメラ姿勢を正確に求めることが必
要である。

【０００５】

【発明が解決しようとする課題】上述したように、従来
は、カメラの道路に対する姿勢のうち、カメラの高さ、
ピッチ角、ロール角が一定であるとともに、またカーブ
の曲率も一定であるという仮定のもとに道路の３次元形
状データを求めているので、車両が走行して、カメラ姿
勢が変動することにより、計測される道路の３次元形状
データに誤差が含まれるとともに、また曲率の変化する
緩和曲線部やＳ字カーブ等では推定誤差が避けられず、
道路の形状データを正確に計測することができないとい
う問題がある。

【０００６】本発明は、上記に鑑みてなされたもので、
その目的とするところは、車両に搭載した撮像手段であ
るカメラの姿勢が車両の走行により変動するという条件
を考慮しながら、多次曲線による３次元道路モデルを作
成して道路の３次元形状データを精度良く高速に計測し
得る道路形状計測装置を提供することにある。

【０００７】

【課題を解決するための手段】上記目的を達成するた
め、本発明の請求項１記載の道路形状計測装置は、図１
に示すように、車両の進行方向前方の道路を撮像する撮
像手段８１と、該撮像手段で撮像した画像から車両の進
行方向前方の道路形状を検出する道路形状検出手段８３
と、３次元曲線パラメータに基づいて３次元座標上で定
義された３次元道路モデルを演算する道路モデル演算手
段８５と、前記道路モデル演算手段で演算された３次元
道路モデルを前記撮像手段の姿勢パラメータに基づいて
画像座標に変換する座標変換手段８７と、前記道路形状
検出手段で検出された前記道路形状と前記座標変換手段
で座標変換された前記３次元道路モデルとを比較して、
両者間の位置ずれを検出する位置ずれ検出手段８９と、
該位置ずれ検出手段で検出した位置ずれから前記３次元
曲線パラメータの変化量および前記撮像手段の姿勢パラ
メータの変化量を推定し、前記各パラメータを更新する
推定更新手段９１とを有することを要旨とする。

【０００８】

【作用】本発明の請求項１記載の道路形状計測装置で
は、撮像手段で撮像した画像から車両の進行方向前方の
道路形状を検出し、３次元曲線パラメータに基づいて３
次元座標上で定義された３次元道路モデルを演算し、こ
の３次元道路モデルを撮像手段の姿勢パラメータに基づ
いて画像座標に変換し、前記道路形状と座標変換された
３次元道路モデルとを比較して、両者間の位置ずれを検
出し、この位置ずれから３次元曲線パラメータの変化量
および姿勢パラメータの変化量を推定し、各パラメータ
を更新している。

【０００９】

【実施例】以下、図面を用いて本発明の実施例を説明す
る。

【００１０】図２は、本発明の一実施例に係わる道路形
状計測装置の全体構成を概略的に示すブロック図であ
る。同図に示す道路形状計測装置は、車両の進行方向前
方の道路を撮像すべく車両に搭載された撮像手段である
カメラ１、該カメラ１で撮像した画像信号を入力し、こ
の入力された画像信号を画像処理する画像処理部３、お
よび該画像処理部３で得られた計測結果である道路の３
次元形状データを表示するとともに、該形状データに基
づいて車両の制御および警報判断等を行う制御部５を有
する。

【００１１】図１に示す画像処理部３は、カメラ１で撮
像された２車線道路の画像を連続的に入力され、この画
像からカメラ１の高さ（Ｄｙ）、ヨー角（θ）、ピッチ
角（φ）、ロール角（φ）道路端からの距離（Ｄｘ）と
いう車速を除くカメラの５軸挙動パラメータに加えて、
道路の水平曲線（ρ）および道路の勾配を表す垂直曲線
（μ）の道路形状パラメータを有する３次元道路構造を
同時にオンラインで推定するものである。更に詳しく
は、本実施例では、水平曲線（ρ）および垂直曲線
（μ）を道路形状パラメータとする３次元道路モデルを
作成するとともに、１画面前のカメラの５軸挙動パラメ
ータに基づいて前記３次元道路モデルを２次元画像に座
標変換し、この変換された２次元道路モデルとカメラ１
で撮像した道路の入力画像との対応関係から、道路形状
パラメータとカメラの挙動パラメータの変化量を推定し
て更新している。なお、道路モデルは折れ線グラフで表
され、道路モデルとカメラからの入力画像との対応は線
対応としている。また、道路形状パラメータとカメラの
挙動パラメータの各パラメータの変化量が小さいと仮定
すると、線形方程式で推定でき、これにより線対応マッ
チングや方程式の作成および解法等の計算が簡単に行え
るので、高速処理に適している。また、前方の道路曲線
は車両の挙動に左右されないので、高精度で求めること
ができる。

【００１２】図３は、図１に示す画像処理部３の詳細な
構成を示すブロック図である。同図に示すように、画像
処理部３は、前記カメラ１で撮像された画像を入力され
る画像入力部１１、この画像入力部１１から入力された
画像情報から道路の白線の右端のエッジを検出する前処
理部１３、１画面前（前時刻）に推定された道路形状パ
ラメータに基づいて３次元の道路モデルを作成する道路
モデル作成部１５、前時刻に推定されたカメラの挙動パ
ラメータを用いて、道路モデル作成部１５で作成された
３次元の道路モデルを車両座標系に座標変換し、更に画
像座標系に透視変換する座標変換部１７、この座標変換
部１７によって画像座標系に座標変換された道路モデル
と前記前処理部１３で検出された道路のエッジ画像とを
線対応関係で対応づける線対応マッチング部１９、この
線対応マッチング部１９によって線対応づけられた画像
の位置ずれを検出し、この位置ずれから道路形状パラメ
ータおよびカメラの挙動パラメータの各パラメータの変
化量を推定するパラメータ推定部２１、およびこのパラ
メータ推定部２１で推定されたパラメータを更新するパ
ラメータ更新部２３から構成されている。

【００１３】前記前処理部１３における前処理は、カメ
ラ１で撮像した道路の入力画像から道路の白線のエッジ
を検出するために、入力画像をＡ（ｘ，ｙ）とし、エッ
ジ画像をＧ（ｘ，ｙ）とした場合、次式の演算を行い、
エッジ画像を検出する。

【００１４】

【数１】このような演算式を使用することにより、カーブ路にお
いて遠方の白線はほとんど水平で、線幅も１画素以下で
撮影されてもエッジを抽出することができる。

【００１５】道路モデル作成部１５および座標変換部１
７では、前時刻に推定される道路形状パラメータを基に
３次元の道路モデルを作成し、また前時刻に推定される
カメラの挙動パラメータを用いて車両座標系に座標変換
し、更に２次元の画像座標系に透視変換する。なお、本
実施例では、道路のカーブや勾配のパラメータは時間的
に変化するものと考える。カメラ１の焦点距離は既知と
し、また車両の速度成分は推定しないものとする。これ
は白線と平行に移動しても、見かけ上の白線位置は変化
しないためである。従って、推定すべきパラメータは、
前述したカメラ１の高さ（Ｄｙ）、ヨー角（θ）、ピッ
チ角（φ）、ロール角（φ）、道路端からの距離（Ｄ
ｘ）のカメラの５軸挙動パラメータに加えて、水平曲率
（ρ）および垂直曲率（μ）を含む道路形状パラメータ
の７つのパラメータである。

【００１６】まず、座標系について図４を参照して説明
する。座標系としては、道路座標系（Ｘ，Ｙ，Ｚ）、車
両座標系（Ｕ，Ｖ，Ｗ）および２次元の画像座標系
（ｘ，ｙ）の３つの座標系を定義する。

【００１７】道路座標系（Ｘ，Ｙ，Ｚ）は、現在車両が
置かれている地点を基準に道路の形状を表すものであ
り、図４に示すように、道路中央の路面上に原点を取
り、道路接続および路面と平行で車両の進行方向にＺ
軸、左方向に路面と平行にＸ軸、路面と垂直で上方にＹ
軸を取る右手系である。なお、カメラ１のレンズの中心
は常にＺ＝０の位置にあるものとする。

【００１８】車両座標系（Ｕ，Ｖ，Ｗ）は、カメラ１の
レンズの中心を原点とし、該レンズの主軸方向にＷ軸、
撮像面と平行にＵ軸およびＶ軸を取る右手系である。

【００１９】画像座標系（ｘ，ｙ）は、撮像面上で定義
され、ｘ軸がＵ軸と平行、ｙ軸がＶ軸と平行で、それぞ
れ逆向きであるものとする。

【００２０】また、車両座標系で表された３次元空間上
の点Ｐ（Ｕ，Ｖ，Ｗ）は、画像上の画像座標系で次式の
ように透視変換されて表される。

【００２１】ｘ＝−Ｆ・Ｕ／Ｗｙ＝−Ｆ／Ｖ／Ｗ（１）但し、Ｆはカメラ１のレンズの焦点距離である。

【００２２】道路座標系と車両座標系との関係が車両の
挙動、すなわちカメラ１の姿勢に対応する。以下、カメ
ラ１の道路端からの距離をＤｘ、カメラ１の高さをＤ
ｙ、ヨー角をθ、ピッチ角をφ、ロール角をφで表す。

【００２３】車両座標系は、まず道路座標系で（Ｄｘ，
Ｄｙ，０）だけ平行移動した後、ヨー角θ、ピッチ角
φ、ロール角φの順で回転させる。なお、ヨー角θは、
Ｗ軸をＸＺ平面に射影した時のＺ軸とのなす角度であ
り、ピッチ角φはＷ軸とＸＺ平面との角度であり、ロー
ル角φはＷ軸回りの回転角でＵ軸とＸＺ平面との角度で
ある。回転マトリックスをＲとすると、道路座標系で表
された点は、次式により車両座標系へ変換される。

【００２４】

【数２】次に、道路モデル作成部１５で作成される道路モデルに
ついて説明する。

【００２５】道路は、横断曲線（カーブ）と縦断曲線
（勾配）に分けて構造が決定されている。横断道路は曲
率一定の円弧部、直線部、およびこれらを滑らかに結ぶ
ための緩和曲線部によって定義される。また、縦断曲線
は一定勾配である直線部を放物線によって滑らかに結ぶ
ものとされている。

【００２６】３次元の道路座標上では、横断曲線、縦断
曲線ともに多次曲線によって近似される。そして、路面
上に描かれた白線の道路座標系における座標は次式のよ
うに定義される。

【００２７】Ｘ＝ｆ（Ｚ）＝ａＺ⁴＋ｂＺ³＋ｃＺ²＋ＢＹ＝ｇ（Ｚ）＝ｄＺ³＋ｅＺ² （４）ここで、Ｂは、道路中心から白線までの距離を表し、３
車線道路の場合には、左の白線に対してはＢは正の定数
であり、中央に対しては０である。ａ〜ｅは求める道路
形状パラメータである。なお、水平方向の式ｆ（Ｚ）を
４次式としたのは、Ｓ字カーブにも対応するためであ
る。

【００２８】また、道路の水平曲率ρおよび垂直曲率μ
は、次式で表される。

【００２９】

【数３】特に、Ｚ＝０の付近の曲率は、水平曲率ρ＝２ｃおよび
垂直曲率μ＝２ｅである。

【００３０】このようにして、Ｎ点からなる道路モデル
を作成するＺｉ（ｉ＝１〜Ｎ）を与えれば、白線の３次
元座標を計算することができる。また、Ｚの地点の曲率
を求めることも可能である。

【００３１】上述した式（４）で示した道路形状パラメ
ータａ，ｂ，ｃ，ｄ，ｅを前画面（前時刻）から得て、
道路モデルを作成し、この道路モデルを前時刻における
カメラ１の挙動パラメータに基づく車両座標系に変換す
る。そして、道路モデル上の各点が現画面（現時刻）で
どう移動するかを観測することによって、車両座標系に
おける挙動変動量および道路パラメータの変動量を求め
る。すなわち、道路モデル上の点（ｘ，ｙ）が現画面で
（Δｘ，Δｙ）だけ変動したとし、この変動Δｘ，Δｙ
がカメラの挙動パラメータの変動と道路形状パラメータ
の変動とに起因すると考えて、各パラメータの変動量と
ΔｘおよびΔｙとの関係を導く。なお、パラメータは各
時刻毎に変動量を積分することになるが、モデルを基準
とした変動量であるので、誤差の蓄積はない。

【００３２】パラメータの変動によって画像上の道路モ
デル上の点（ｘ，ｙ）が（ｘ’，ｙ’）に移動したと考
え、点の移動量を（Δｘ，Δｙ）とすると、次式で表さ
れる。

【００３３】ｘ’＝ｘ＋Δｘｙ’＝ｙ＋Δｙ（５）カメラの挙動パラメータの変動を車両座標系での点の移
動として考える。すなわち、車両座標系で挙動変動量を
求め、前時刻の挙動パラメータと合成することにより、
現時刻におけるカメラの挙動パラメータに変換し、次時
刻で更に新たな車両座標系を作成する。なお、挙動変動
量を道路座標系でなく、車両座標系で求めるのは、回転
角が常に０からの変動となるため、近似の精度を維持し
ながら線形解を求められるからである。

【００３４】車両座標系において、点Ｐ（Ｕ，Ｖ，Ｗ）
が点Ｐ’（Ｕ’，Ｖ’，Ｗ’）に移動するとすると、

【数４】で表される。なお、Ｄ_U，Ｄ_Uは平行移動成分であり、
α，β，γはそれぞれＶ軸、Ｕ軸、Ｗ軸回りの回転角で
ある。但し、各パラメータの値は、十分小さいものと
し、ｓｉｎδ＝δ，ｃｏｓδ＝１，ｓｉｎα＝α，ｃｏ
ｓα＝１，ｓｉｎβ＝β，ｃｏｓβ＝１，ｓｉｎγ＝
γ，ｃｏｓγ＝１とすると共に、２次以上の項は無視し
た。

【００３５】（６）式と（１）式とから、次式が得られ
る。

【００３６】

【数５】各挙動パラメータの微小変動による画像座標の変動は、
（５）式のＴａｌｅｒ展開の１次の項から次式のように
表される。

【００３７】

【数６】（８）式から、α，β，γ＝０なる条件を近似的に計算
すると、次式に示すようにカメラの挙動パラメータの変
動分が得られる。

【００３８】

【数７】次に、道路パラメータの変動分について考える。上述し
た（２）〜（４）式から、Ｕ＝Ｒ₁₁（Ｘ−Ｄ_x）＋Ｒ₁₂（Ｙ−Ｄ_Y）＋Ｒ₁₃ＺＶ＝Ｒ₂₁（Ｘ−Ｄ_x）＋Ｒ₂₂（Ｙ−Ｄ_Y）＋Ｒ₂₃ＺＸ＝ｆ（Ｚ），Ｙ＝ｇ（Ｚ）であるので、これを（８）式に代入するとともに、また
α，β，γ＝０とおいて

【数８】を計算すると、次式に示すように道路パラメータの変動
分が得られる。

【００３９】

【数９】従って、（９）式と（１０）式を加算すると、カメラの
挙動パラメータと道路パラメータによる画像上での点の
移動量の関係式が得られる。

【００４０】

【数１０】次に、線対応マッチング部１９における線対応マッチン
グについて説明する。

【００４１】モデルを用いたカメラ姿勢の推定手法に
は、点対応と線対応とがあるが、本実施例では線対応、
すなわち道路モデルにおける接線と画面中から抽出され
る白線の線成分とを対応づける。白線は滑らかな曲線で
あるので、点の対応は困難であるからである。

【００４２】対応付けには画像の探索が必要であるが、
ここでは同一ｘ軸または同一ｙ軸上を走査する。探索処
理が簡単化できるため、高速化につながるからである。
ｘ軸走査かｙ軸走査かの選択は、対応付けする線の傾き
の大きさに応じて行う。

【００４３】画像座標系における道路モデル上の点Ｐ
（ｘ，ｙ）における接線の傾きをωで表し、ωを

【数１１】で定義すると、ωは次式で計算される。

【００４４】

【数１２】現時刻の白線は、点Ｐ付近の点Ｐ’（ｘ’，ｙ’）でモ
デルと同じωなる傾きを有すると仮定する。すなわち、
傾きの変化はないものとする。前時刻で求められたモデ
ルが現時刻で白線位置まで移動したと考える。この場合
の道路モデルと白線との関係は図５に示すようになる。
同一ｘ軸上の移動量をｐとし、同一ｙ軸上の移動量をｑ
とすると、ΔｘとΔｙとの関係は、 Δｘ／ｐ＋Δｙ／ｑ＝１であり、更にｑ／ｐ＝−ω であるから、 Δｘ−Δｙ／ω−ｐ＝０（１３ａ）または −ωΔｘ＋Δｙ−ｑ＝０（１３ｂ）が得られる。上式は直線の座標軸方向の（見かけの）移
動量と実際の移動量の関係を表す。｜ω｜が大きい時
（垂直に近い場合）は、（１３ａ）式が使用され、｜ω
｜が小さい時（水平に近い場合）は、（１３ｂ）式が使
用される。

【００４５】道路モデル上のいくつかの点でＰｉ（ｘ
ｉ，ｙｉ）とωｉを計算し、入力画像上における点との
対応からｐｉまたはｑｉを求め、最小二乗法を適用すれ
ば、連立方程式が得られ、各パラメータの変化量が算出
される。

【００４６】評価誤差は、上述した（１１）式および
（１３）式から次式で求められる。

【００４７】

【数１３】但し

【数１４】次に、本実施例の作用を説明する。

【００４８】まず、道路モデル作成部１５において、Ｎ
点からなる道路モデルを作成する。そして、前時刻のカ
メラの挙動パラメータ（Ｄｘ，Ｄｙ，θ，φ，φ）の推
定結果から、（３）式の変換行列を作成する。それか
ら、前時刻の道路形状パラメータ（ａ，ｂ，ｃ，ｄ，
ｅ）から（４）式に基づいて道路座標系（Ｘ，Ｙ，Ｚ）
で道路モデルの３次元座標を計算する。すなわち、前時
刻の道路形状パラメータ（ａ，ｂ，ｃ，ｄ，ｅ）から
（４）式にＺ＝Ｚｉ（ｉ＝１〜Ｎ）とおくことによって
道路モデルの点列を作成する。次に、前時刻のカメラの
挙動パラメータ（Ｄｘ，Ｄｙ，θ，φ，φ）により
（２）式で車両座標系（Ｕ，Ｖ，Ｗ）に変換し、更に
（１）式で画像座標系（ｘ，ｙ）に変換し、座標（ｘ
ｉ，ｙｉ）（ｉ＝１〜Ｎ）を得る。また、（１２）式で
接線の傾きωを計算する。なお、各座標変換は、座標変
換部１７で行われる。

【００４９】道路モデルから算出される値は、（１４）
式に現れているＺｉ，Ｕｉ，Ｖｉ，Ｗｉ，ωｉおよびｘ
ｉ，ｙｉ（ｉ＝１〜Ｎ）である。このうち、Ｚｉは道路
座標上で例えば３メートル間隔と予め定めておく。すな
わち、Ｚｉを与えれば、（１４）式中のＡｒｉ（ｒ＝１
〜１０）の値が定まる。次に、入力画像とのマッチング
を行い、Ｂｉを求める。

【００５０】線対応マッチング部１９は、画像入力部１
１を介してカメラ１から得られる入力画像であるエッジ
画像とモデル点接続との線対応マッチングを行う。｜ω
ｉ｜≧１のとき（垂直に近い場合）は、各モデル点（ｘ
ｉ，ｙｉ）を中心にｘ軸方向に探索して、ｐｉを求め、
｜ωｉ｜＜１のとき（水平に近い場合）は、ｙ軸方向に
探索して、ｑｉを求める。

【００５１】｜ωｉ｜≧１のときは、点（ｘｉ，ｙｉ）
を中心に幅Ｍ_x、高さＭ_yなるウインドウを考える。こ
の中で、傾きωｉの直線を発生させ、直線上のエッジ濃
度の和が最も大きい直線を選択し、直線のｘ座標値とｘ
ｉとの差をｐｉとする。すなわち、

【数１５】を求め、Ｃｉ（τ）の最大値近傍で重心を求め、ｐｉと
する。また、この時のＣｉ（τ）の値は確からしさを表
すので、あらためてＣｉとおく。

【００５２】また、｜ωｉ｜＜１の場合も同様にｑｉを
求める。なお、幅Ｍ_x，高さＭ_yについては数画素が適
当である。

【００５３】以上のようにして、（１４）式で示す評価
誤差のすべての係数が求められるので、最小二乗法を適
用することにより、各パラメータをパラメータ推定部２
１において推定することができる。

【００５４】ここで、別の誤差尺度を考える。１つは道
路パラメータが時間的に大きく変動しないことを利用す
るものである。すなわち、Δａ〜Δｅは０に近いといえ
る。そこで、Ｅ_G1＝Δａ，Ｅ_G2＝Δｂ，Ｅ_G3＝Δｃ，Ｅ_G4＝Δｄ，Ｅ_G5＝Δｅ，Ｅ_G5〜Ｅ_G10＝０（１５）として、それぞれの重みをＧｒ（ｒ＝１〜５）を定数と
して定義する。

【００５５】もう１つの誤差は、曲率に関するものであ
る。道路パラメータｃとｅはＺ＝０の地点の曲率の１／
２である。車両も速度が既知で、速度ｖ（ｍ（メート
ル）／１画面時間）で走行しているとすると、ｔ画面前
のＺ＝ｖｔの地点の曲率が現画面のＺ＝０の曲率になる
はずである。従って、

【数１６】但し

【数１７】とすることができる。なお、ｃ_k-1およびｅ_k-1はそれ
ぞれ前画面の結果である。また、ρ_k-1およびμ_k-1は
ｔ画面前の結果から得られる曲率である。

【００５６】各パラメータは最小二乗法で推定される。
すなわち、

【数１８】を最小化するように上式を各変数で微分し、それぞれ０
とおく。なお、ＨρおよびＨμは重み定数である。この
ようにして、次に示す１０元連立１次方程式

【数１９】が作成される。ここで、ｌ（Ｌの小文字）は１〜１０で
あり、ｍ＝１〜１０である。

【００５７】この１０元連立１次方程式は容易に解くこ
とができる。

【００５８】以上のようにして、パラメータ推定部２１
において最小二乗法により各パラメータの変化量が計算
されると、これらの結果を基にカメラの挙動パラメータ
（Ｄｘ，Ｄｙ，θ，φ，φ）および道路形状パラメータ
（ａ，ｂ，ｃ，ｄ，ｅ）がパラメータ更新部２３におい
て更新される。なお、前時刻のパラメータを添字ｋ−１
で表し、更新されたパラメータを添字ｋで表す。

【００５９】まず、道路形状パラメータａ，ｂ，ｃ，
ｄ，ｅは、Δａ〜Δｅをそのまま加えればよい。すなわ
ち、次式に示すようになる。

【００６０】ａ_(k)＝ａ_(K-1)＋Δａ，ｂ_(k)＝ｂ_(K-1)＋Δｂ，ｃ_(k)＝ｃ_(K-1)＋Δｃ，ｄ_(k)＝ｄ_(K-1)＋Δｄ，ｅ_(k)＝ｅ_(K-1)＋Δｅまた、カメラの挙動パラメータＤｘ，Ｄｙ，θ，φ，φ
は、以下のように更新される。（６）式中のパラメータ
（Ｄ_U，Ｄ_U，α，β，γ）はｋ−１においてすべて０
とみなせるので、Ｄ_U＝ΔＤ_U，Ｄ_U＝ΔＤ_U，α＝Δ
α，β＝Δβ，γ＝Δγとおく。（２）式を（６）式に
代入すると、

【数２０】となる。カメラの挙動パラメータの更新により、（Ｕ，Ｖ，Ｗ）＝（Ｕ’，Ｖ’，Ｗ’）とすればよい。（２）式と（１６）式を比較することに
より、

【数２１】にて求められる。なお、Ｒ^-1＝Ｒ^Tなる関係式を用い
た。θ_(k)，φ_(k)，φ_(k ₎は（２）式中のＲ_31(k)，
Ｒ_32(k)，Ｒ_12(k)から容易に求められる。

【００６１】以上のようにして、各パラメータは更新さ
れるが、出力は（４）’から求められる前方Ｚにおける
カーブと勾配のそれぞれの曲率、すなわち水平曲率
（ρ）および垂直曲率（μ）、および車両の挙動、すな
わちカメラの挙動パラメータ（Ｄｘ，Ｄｙ，θ，φ，
φ）である。カメラの挙動パラメータのうち、道路に対
する道路端からの距離Ｄｘおよびヨー角θについては自
動操縦や予防安全における車線逸脱警報に重要である。

【００６２】図６は、上述した本発明の道路形状計測装
置を利用した追突警報装置の構成を示すブロック図であ
る。同図に示す追突警報装置は、カメラ１で撮像した車
両の進行方向前方の画像を本発明の道路形状計測装置１
００に供給する。該道路形状計測装置１００はカメラ１
からの画像情報から車両の前方の道路形状および自車両
の位置に関する情報を判断部５３に供給する。

【００６３】また、図６に示す装置は、前方の車両を検
出し、該車両までの距離と方向を検出するものとして、
スキャニングレーザレーダ５１を使用しており、該スキ
ャニングレーザレーダ５１で検出した前車両の位置情報
を前記判断部５３に供給する。判断部５３は、道路形状
計測装置１００から供給される道路形状および自車両の
位置情報とスキャニングレーザレーダ５１から供給され
る前車両の位置情報から前車両と自車両とが同じ車線か
否かを判断するとともに、同じ車線であって、両車両の
間の距離が所定値以下に近づいた場合、警報信号を警報
装置５５に供給し、例えばブザー、チャイム、音声等で
警報信号を発生する。

【００６４】なお、前車両が自車両と同じ車線を走行し
ているか否かの判断は、前車両の自車両に対する位置が
（Ｘｓ，Ｙ＝０，Ｚｓ）として求まったとして、道路パ
ラメータの水平情報、（４）式においてＢ＝０、すなわ
ちＸｐ＝ｆ（Ｚｓ）＝ａＺｓ⁴＋ｂＺｓ³＋ｃＺｓ² が道路中央のＸ座標であるから、Ｘｐ＋Ｂ＞Ｘｓ＞Ｘｐの場合は、左車線Ｘｐ−Ｂ＜Ｘｓ＜Ｘｐの場合は、右車線を走行していると判断できる。自車両の位置はＤ_xで与
えられ、Ｄ_x＞０の場合には、左車線Ｄ_x＜０の場合には、右車線である。そして、同一車線を走行していて、かつＺｓが
所定の距離以下まで近づいた場合に、上述したように警
報を発生すればよい。なお、例えばＸｓ＜Ｘｐ＋Ｂ（車
線幅）の場合には、路側帯の障害物と判断できる。Ｓ字
道路でも正確な判断を行うことができる。

【００６５】図７は、上述した本発明の道路形状計測装
置を利用した車線逸脱警報装置の構成を示すブロック図
である。同図に示す車線逸脱警報装置は、カメラ１で撮
像した車両の進行方向前方の画像を本発明の道路形状計
測装置１００に供給する。該道路形状計測装置１００は
カメラ１からの画像情報から車両の前方の道路形状およ
び自車両の位置に関する情報を判断部５７に供給する。
また、該判断部５７には、車両の方向指示器であるウイ
ンカ５９からの方向変更信号が供給されている。今、例
えば車両が左車線を走行しているとして、車両が例えば
ウインカ５９を出さずに、上述した道路端からの距離Ｄ
ｘがＢより大きくなったり（Ｄ_x＞Ｂ）、またはゼロよ
りも小さくなった場合（Ｄ_x＜０）には、自車線を逸脱
したと見なし、判断部５７は警報装置５５に警報信号を
供給し、該警報装置５５から警報を発生する。

【００６６】更に、自動操縦について説明すると、本道
路形状計測装置では、自車両の位置の曲率と前方の曲率
が同時に求められるので、予め速度を制御する自動操縦
が可能となる。例えば、次のように制御することができ
る。

【００６７】｜前方の水平曲率｜＞｜現在の水平曲率｜→減速｜前方の水平曲率｜＜｜現在の水平曲率｜→加速前方の垂直曲率（勾配）＞現在の垂直曲率（勾配）→加
速（スロットル開）前方の垂直曲率（勾配）＜現在の垂直曲率（勾配）→減
速（スロットル閉）また、例えば、ピッチ角φ、ロール角φ、高さＤｙの変
動が大きい場合には、減速すること等も考えられる。乗
り心地のフィードバックと言える。

【００６８】また、操舵制御は、例えば（１）現在の曲率に比例した操舵角 Φ_k1＝Ｋ・ρ （２）ヨー角が正の場合には左に操舵 Φ_k2＝Φ_k-1−δ （３）ヨー角が負の場合には右に操舵 Φ_k3＝Φ_k-1＋δ （４）Ｄｘが所定値より大きい場合右に操舵 Φ_k4＝Φ_k-1＋δ （５）Ｄｘが所定値より小さい場合左に操舵 Φ_k5＝Φ_k-1−δ とし、Φ_k1〜Φ_k5を平均した操舵角に設定すればよい。

【００６９】なお、前記実施例で説明した道路座標系
（Ｘ，Ｙ，Ｚ）における道路モデルを定義した（４）式
では、道路モデルは画像で観測できる範囲、すなわち前
方１００メートル位までを近似できればよいものとし
て、緩やかな曲率を仮定しているが、この（４）式は、
一般式として次のように表される。

【００７０】

【数２２】また、（６）式は次式のようにしてもよい。

【００７１】

【数２３】更に、（１４）式で示した評価誤差は次式のように一般
式で表され、係数Ａω，Ｂωは表１に示されるものとな
る。

【００７２】

【数２４】

【表１】また、（１６）式および（１７）式はそれぞれ次に示す
（１６−Ｔ）式および（１７−Ｔ）式を使用して一般式
として表される。

【００７３】

【数２５】

【数２６】

【００７４】

【発明の効果】以上説明したように、本発明によれば、
撮像手段で撮像した画像から車両の進行方向前方の道路
形状を検出し、３次元曲線パラメータに基づいて３次元
座標上で定義された３次元道路モデルを演算し、この３
次元道路モデルを撮像手段の姿勢パラメータに基づいて
画像座標に変換し、前記道路形状と座標変換された３次
元道路モデルとを比較して、両者間の位置ずれを検出
し、この位置ずれから３次元曲線パラメータの変化量お
よび姿勢パラメータの変化量を推定し、各パラメータを
更新しているので、比較的簡単な数式で表されるため、
少ない計算量で高速かつ精度良く各パラメータの推定を
行い、車両前方の道路形状を適確に計測することができ
る上に、Ｓ字カーブや曲率の変化するカーブにおいても
精度良く道路形状を求めることができる。更に、車両の
地点での曲率や道路形状も推定されるので、自車両位置
の推定結果も高精度である。

【図面の簡単な説明】

【図１】本発明のクレーム対応図である。

【図２】本発明の一実施例に係わる道路形状計測装置の
全体構成を概略的に示すブロック図である。

【図３】図２に示す道路形状計測装置に使用される画像
処理部の詳細な構成を示すブロック図である。

【図４】図３に示す画像処理部に使用される座標系を示
す説明図である。

【図５】図３に示す線対応マッチング部における道路モ
デルと白線との線対応マッチングを示す説明図である。

【図６】本発明の道路形状計測装置を利用した追突警報
装置の構成を示すブロック図である。

【図７】本発明の道路形状計測装置を利用した車線逸脱
警報装置の構成を示すブロック図である。

【符号の説明】

１カメラ３画像処理部５制御部１１画像入力部１３前処理部１５道路モデル作成部１７座標変換部１９線対応マッチング部２１パラメータ推定部２３パラメータ更新部

─────────────────────────────────────────────────────

【手続補正書】

【提出日】平成４年７月２７日

【手続補正１】

【補正対象書類名】明細書

【補正対象項目名】００１５

【補正方法】変更

【補正内容】

【００１５】道路モデル作成部１５および座標変換部１
７では、前時刻に推定される道路形状パラメータを基に
３次元の道路モデルを作成し、また前時刻に推定される
カメラの挙動パラメータを用いて車両座標系に座標変換
し、更に２次元の画像座標系に透視変換する。なお、本
実施例では、道路のカーブや勾配のパラメータは時間的
に変化するものと考える。カメラ１の焦点距離は既知と
し、また車両の速度成分は推定しないものとする。これ
は白線と平行に移動しても、見かけ上の白線位置は変化
しないためである。従って、推定すべきパラメータは、
前述したカメラ１の高さ（Ｄｙ）、ヨー角（θ）、ピッ
チ角（φ）、ロール角（φ）、道路端からの距離（Ｄ
ｘ）のカメラの５軸挙動パラメータに加えて、水平曲率
（ρ）および垂直曲率（μ）を含む道路形状パラメータ
である。

Claims

【特許請求の範囲】

【請求項１】車両の進行方向前方の道路を撮像する撮
像手段と、該撮像手段で撮像した画像から車両の進行方
向前方の道路形状を検出する道路形状検出手段と、３次
元曲線パラメータに基づいて３次元座標上で定義された
３次元道路モデルを演算する道路モデル演算手段と、前
記道路モデル演算手段で演算された３次元道路モデルを
前記撮像手段の姿勢パラメータに基づいて画像座標に変
換する座標変換手段と、前記道路形状検出手段で検出さ
れた前記道路形状と前記座標変換手段で座標変換された
前記３次元道路モデルとを比較して、両者間の位置ずれ
を検出する位置ずれ検出手段と、該位置ずれ検出手段で
検出した位置ずれから前記３次元曲線パラメータの変化
量および前記撮像手段の姿勢パラメータの変化量を推定
し、前記各パラメータを更新する推定更新手段とを有す
ることを特徴とする道路形状計測装置。