JPH0727541A - 道路形状及び自車両位置の計測装置 - Google Patents

Abstract

(57)【要約】 【目的】 正確な道路形状及び自車両位置の計測を行え
る道路形状及び自車両位置の計測装置の提供。 【構成】 地図情報記憶手段１０３は道路の３次元形状
に対応するデータを含む地図情報を持っているので、道
路モデル演算手段１０５により現在の車両位置データに
基づいて地図情報記憶手段１０３の地図情報を参照し、
３次元道路モデルを求め、入力画像とのずれから、グロ
ーバルかつローカルな自車両位置を同時推定することを
特徴とする。

Description

【発明の詳細な説明】

【０００１】

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

【０００２】

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

【０００３】また、グローバルな自車両位置を求めて自
車両がどのあたりを走行中であるかも併せて知ることが
必要である。

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

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

【０００６】ところが、車両に搭載されたカメラでは、
車両の近傍のデータのみを対象とした画像処理しかでき
ないため、グローバルな自車両位置を計測できない。そ
こで、従来においては、グローバルな自車両位置を知る
ため、例えばＧＰＳ（GlobalPositioning System)を採
用していた。しかし、このＧＰＳの場合、精度は３０〜
１００ｍ位なので、道路端からの距離、ヨー角、どの車
線を自車両が走行中か等のローカルな自車両姿勢を計測
することができない。

【０００７】

【発明が解決しようとする課題】上記したように、従来
の場合においては、グローバルな自車両位置を求めるた
めに、実質的に更に精度向上が不可能なＧＰＳを採用し
ており、またカメラの高さ、ピッチ角、ロール角を一定
としたカメラ画像処理による道路形状やローカルな自車
両位置計測を行っていたため、正確な道路形状及び自車
両位置の計測を行うことができないという問題点があっ
た。

【０００８】本発明は、上記した従来の問題点に着目し
てなされたものであり、その目的とするところは、正確
な道路形状及び自車両位置の計測を行える道路形状及び
自車両位置の計測装置を提供することにある。

【０００９】

【課題を解決するための手段】本発明は、上記の目的を
達成するため、図１に示すように、車両の進行方向前方
の道路を撮像する撮像手段１０１と、該撮像手段で撮像
した画像から車両の進行方向前方の道路形状を表わす特
徴を抽出する道路形状検出手段１０２と、道路の３次元
形状に対応するデータを含む地図情報を予め記憶した地
図情報記憶手段１０３と、１回前に計測された車両位置
パラメータ及び前記撮像手段１０１の姿勢パラメータを
保持するパラメータ保持手段１０４と、前記車両位置パ
ラメータに基づいて、前記地図情報を参照し、３次元座
標上で定義された３次元道路モデルを演算する道路モデ
ル演算手段１０５と、該道路モデル演算手段１０５で演
算された３次元道路モデルを前記撮像手段の姿勢パラメ
ータに基づいて画像座標に変換する座標変換手段１０６
と、前記道路形状検出手段１０２で検出された前記道路
形状を、前記座標変換手段１０６で座標変換された前記
３次元道路モデルと比較して、両者間の位置ずれを検出
する位置ずれ検出手段１０７と、該位置ずれ検出手段で
検出した位置ずれから前記車両位置パラメータの変化量
および前記撮像手段の姿勢パラメータの変化量を演算す
る変化量演算手段１０８と、前記両パラメータの変化量
に基づき前記パラメータ保持手段１０４の各パラメータ
を更新する更新手段１０９とを有することを特徴とす
る。

【００１０】

【作用】本発明による道路形状及び自車両位置の計測装
置であれば、地図情報記憶手段１０３は道路の３次元形
状に対応するデータを含む地図情報を持っているので、
道路モデル演算手段１０５によりパラメータ保持手段１
０４の車両位置パラメータに基づいて地図情報記憶手段
１０３の地図情報を参照して、３次元道路モデルが求め
られる。

【００１１】他方、撮像手段１０１で撮像した画像から
道路形状検出手段１０２が車両の進行方向前方の道路形
状を検出しているので、位置ずれ検出手段１０７におい
て、道路モデル演算手段１０５で求めた３次元道路モデ
ルの値を座標変換手段１０６により画像座標に変換して
受ける一方、道路形状検出手段１０２の道路形状の値を
受けて、その両者間の位置ずれを検出し、この検出結果
から変化量演算手段１０８により自車両の車両位置パラ
メータの変化量および撮像手段１０１の姿勢パラメータ
の変化量を演算し、更新手段１０９によりパラメータ保
持手段１０４の各パラメータを更新することができる。

【００１２】

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

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

【００１４】図２に示す画像処理部３は、カメラ１で撮
像された２車線道路の画像を連続的に入力し、この画像
からカメラ１の高さ（Ｄｙ）、ヨー角（θ）、ピッチ角
（φ）、ロール角（ψ）、道路端からの距離（Ｄｘ）と
いう車速を除くカメラの５軸の姿勢パラメータに加え
て、自車両の車両位置の単位時間当りの変動量を同時に
推定するものである。更に詳しくは、本実施例では、現
在の走行地点から地図情報を参照して道路形状パラメー
タを求め、次に、車両前方の３次元形状を計算して道路
モデルとし、さらに、１画面前（前時刻）に推定された
姿勢パラメータを用いて、各モデル点を車両座標系に座
標変換し、さらに画像座標系に透視変換する。そして、
入力された画像中の白線位置と、モデル点とを対応付け
ることによって、各姿勢パラメータと車両位置の単位時
間当りの変動量を推定し、補正更新する。なお、レンズ
の焦点距離は固定とし既知とする。

【００１５】図３は、図２に示す画像処理部３の詳細な
構成を示すブロック図である。同図に示すように、画像
処理部３は、前記カメラ１で撮像された画像が入力され
る画像入力部１１、この画像入力部１１から入力された
画像情報から道路の白線の右端のエッジを検出する道路
形状検出手段としての前処理部１３、道路の３次元形状
に対応するデータを含む地図情報を予め記憶しておく地
図情報記憶部１５、１回前に求めた車両位置における水
平曲率や垂直曲率などの道路形状パラメータを計算する
道路形状パラメータ計算部１９、１回前に求めた車両位
置における道路形状パラメータを用いて道路中心線と白
線の座標を計算する道路中心線および白線の座標計算部
２１、道路中心線と白線の座標を片勾配分だけ回転させ
る片勾配回転部２３、片勾配分だけ回転された道路中心
線と白線の座標を１回前のカメラ姿勢パラメータで座標
変換する座標変換部２５、この座標変換部２５によって
画像座標系に座標変換された道路モデルと前処理部１３
で検出された道路のエッジ画像とを線対応関係で対応づ
ける線対応マッチング部２７、この線対応マッチング部
２７によって線対応づけられた画像の位置ずれを検出
し、この位置ずれから前記車両位置パラメータおよび前
記カメラ姿勢パラメータを最小二乗法を適用して推定す
る最小二乗推定部２９、この最小二乗推定部２９で推定
されたパラメータを１回前の各パラメータ値に加えるこ
とにより各パラメータを更新するパラメータ更新部３
１、更新された各パラメータを次回の計測に用いるため
に保持しておくパラメータ保持部３５から構成されてい
る。

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

【００１７】

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

【００１８】地図情報記憶部１５は、道路形状パラメー
タを予め記憶しておくものである。図４に示す道路形状
の例において、道路形状パラメータは次にように定義で
きる。

【００１９】道路中心線に沿った道のり座標をＬで表
す。Ｌは車両の走行距離に対応する。Ｌにおける道路の
水平曲率をρ（Ｌ）、垂直曲率をμ（Ｌ）、片勾配をλ
（Ｌ）で表す。また、単位距離当たりの水平曲率の変化
率をρ'(Ｌ）（＝ｄρ（Ｌ）／ｄＬ）、片勾配の変化率
をλ'(Ｌ）で表す。

【００２０】道路の平面曲線は、一定曲率である円弧部
や直線部と、曲率がＬに対して直線的に変化する緩和曲
線部とに分類される。図４に水平曲率と道路形状との関
係の例を示す。縦断曲線は一定勾配部と円弧部がある
が、円弧部の垂直曲率は水平曲率に比べて1/10程度と小
さいため、緩和曲線部に相当する部分は考慮しない。片
勾配は水平曲率の大きさに応じて設定され、緩和曲線部
ではＬに対して直線的に変化する。

【００２１】このような道路形状パラメータを図５
（ａ）に示すような形式で記憶する。すなわち、水平曲
率、垂直曲率、片勾配の３つの独立したデータをもち、
それぞれの形状変化点の座標Ｌ_ρ(n) （ｎ＝１〜
Ｎ_ρ），Ｌ_λ(n) （ｎ＝１〜Ｎ_λ）と、その地点のパラ
メータ値ρ（Ｌ_ρ(n) ），λ（Ｌ_λ(n) ）からなる。

【００２２】時刻ｋにおいて、車両がＬ_k に位置してい
るとする。Ｌ_k における各パラメータ値は、地図情報か
ら以下のように求められる。水平曲率においてＬ_ρ(n)
≦Ｌ_k ＜Ｌ_ρ(n+1) である時、

【数２】 ρ（Ｌ_k ）＝ρ'(Ｌ_k ）・（Ｌ_k −Ｌ_ρ(n) ）＋ρ（Ｌ_ρ(n) ） ρ'(Ｌ_k ）＝｛ρ（Ｌ_ρ(n+1) ）−ρ（Ｌ_ρ(n) ）｝ /(Ｌ_ρ(n+1) −Ｌ_ρ(n) ） （２） である。なお、一定曲率部ではρ'(Ｌ_k ）＝０、緩和曲
線部ではρ'(Ｌ_k ）≠０である。

【００２３】同様に、Ｌ_μ(n) ≦Ｌ_k ＜Ｌ_μ(n+1) であ
る時、

【数３】 μ（Ｌ_k ）＝μ（Ｌ_μ(n) ） （３） さらに、Ｌ_λ(n) ≦Ｌ_k ＜Ｌ_λ(n+1) である時、

【数４】 λ（Ｌ_k ）＝λ'(Ｌ_k ）・（Ｌ_k −Ｌ_λ(n) ）＋λ（Ｌ_λ(n) ） λ'(Ｌ_k ）＝｛λ（Ｌ_λ(n+1) ）−λ（Ｌ_λ(n) ）｝ /(Ｌ_λ(n+1) −Ｌ_λ(n) ） （４） とする。図５（ｂ）は、（２）〜（４）式によって、図
５（ａ）のパラメータから、ρ（Ｌ），μ（Ｌ），λ
（Ｌ）を復元したものである。

【００２４】道路形状パラメータ計算部１９、道路中心
線および白線の座標計算部２１、片勾配回転部２３、座
標変換部２５の処理過程では、Ｎ×Ｍ（Ｎは白線１本当
りのモデル点数，Ｍは白線の本数）２点からなる道路モ
デルを作成する。具体的には、道路形状パラメータ計算
部１９において、前時刻の車両位置Ｌから地図データを
参照し、（２）〜（４）式によって道路形状パラメータ
を求める。次に道路中心線及び白線の座標計算部２１に
よって３次元の道路モデルを計算する。

【００２５】まず、道路形状モデルを求める座標系につ
いて説明する。

【００２６】車両Ｌ_k に位置しているとする。Ｌ_k の地
点の道路中央に原点Ｏをとり、Ｌ軸接線方向にＺ軸、地
表面に平行で進行方向左手にＸ軸、上方にＹ軸をとり、
道路座標系とする。カメラは常にＺ＝０にあるものとす
る。道路座標系は、主に道路の中心線を定義するための
もので、原点における道路中心線の接線はＺ軸上にあ
る。Ｌ_k の地点に片勾配が付与されていると、道路面は
Ｘ軸に対して角度ξだけ傾く。車両の道路に対する姿勢
は、道路面を基準とするのが自然であるため、道路座標
系をＺ軸回りにξだけ回転させた道路面座標系（Ｘ' ，
Ｙ' ，Ｚ）も考える。すなわち、Ｚ軸回りの回転マトリ
クスをＲ_ξとすると、

【数５】 である。なお、片勾配は、

【数６】 λ（Ｌ_k ）＝tan ξ （６） なる関係とする。

【００２７】さらに、カメラ位置を原点とする車両座標
系（Ｕ，Ｖ，Ｗ）と、ｘ軸とＵ軸、ｙ軸とＶ軸がそれぞ
れ平行になるよう画像座標系（ｘ，ｙ）をとる。車両座
標系は道路面座標系から次式で変換される。

【００２８】

【数７】 ここで、Ｄ_X はカメラのＸ軸方向の偏位を、Ｄ_Y はカメ
ラの高さを表す。θφψはそれぞれＸ' Ｙ' Ｚ軸回りの
回転角を表し、Ｒ_θφψはθφψの順に回転させる回転
マトリクスである。Ｄ_X ，Ｄ_Y ，θ，φ，ψは本システ
ムで画像から求める姿勢パラメータである。画像座標系
と車両座標との関係は、レンズの焦点距離をＦとする
と、

【数８】 ｘ＝−Ｆ・Ｕ／Ｗ ｙ＝−Ｆ・Ｖ／Ｗ （８） である。

【００２９】こうした座標系において、道路の３次元座
標は次の通りとなる。

【００３０】カメラから観測できる道路は、例えば前方
１００ｍまでと限られている。この画像処理範囲内で、
道路の３次元座標を前述の道路形状パラメータから求め
る。まず、＜１＞と＜２＞で、画像処理範囲内にＬ
_ρ(n) などの形状変化点を含まない（以下同一パラメー
タ区間と呼ぶ）場合について述べ、＜３＞では異なるパ
ラメータの区間が画像処理範囲内にある場合の手法につ
いて述べる。

【００３１】以下の説明において、現在地点の曲率をρ
₀ （＝ρ（Ｌ_k ))，μ₀ （＝μ（Ｌ_k ))、同様に、λ₀
（＝λ（Ｌ_k ))，ρ' ₀ （＝ρ'(Ｌ_k ))，λ' ₀ （＝
λ'(Ｌ_k ))と表す。

【００３２】＜１＞道路中心線 道路座標系における道
路中心線の３次元座標を、Ｓを媒介変数とする次の多項
式で表す。

【００３３】

【数９】 なお、（９）式は以下のようにして導かれた。まずＸＺ
平面上の曲線を、Ｓを媒介変数とする次の多項式で表
す。

【００３４】

【数１０】 Ｘ＝ａ₂ Ｓ² ＋ａ₃ Ｓ³ ＋ａ₄ Ｓ⁴ Ｚ＝Ｓ＋ｂ₂ Ｓ² ＋ｂ₃ Ｓ³ （１０） この曲線の曲率は、

【数１１】 Ｋ（Ｓ）＝（Ｘ''Ｚ' −Ｚ''Ｘ')（Ｘ' ² ＋Ｚ' ² ）^-3/2 （１１） である。（１１）式をテーラー展開によって２次の項ま
で近似すると次式が得られる。

【００３５】

【数１２】 Ｋ（Ｓ）＝Ａ₀ ＋Ａ₁ ・Ｓ＋Ａ₂ ・Ｓ² Ａ₀ ＝２ａ₂ Ａ₁ ＝６ａ₃ −１２ａ₂ ｂ₂ （１２） Ａ₂ ＝４８ａ₂ ｂ₃ ＋２４ａ₄ −２４ａ₂ ² −６０ａ³ ｂ² ＋９６ａ₂ ｂ₂ ² 一方、同一パラメータ区間内では、曲率は道路中心線に
沿った道のり距離に対して直線的に変化するので、

【数１３】 Ｋ（Ｓ）＝ρ（Ｌ_k ）＋ρ'(Ｌ_k ）・Ｓ （１３） となるべきである。簡単のためにｂ₂ ＝０とおいて、

【数１４】 ａ₂ ＝ρ（Ｌ_k )/２，ａ₃ ＝ρ'(Ｌ_k )/６ ａ₄ ＝−ρ（Ｌ_k ）³ / ２４，ｂ₃ ＝−ρ（Ｌ_k ）² / ６ （１４） とすると、Ａ₀ ＝ρ（Ｌ_k ），Ａ₁ ＝ρ'(Ｌ_k ），Ａ₂
＝０となる。

【００３６】Ｓは図６に示すように、道路中心線をＸＺ
平面に射影したときの、原点０からの道のり距離であ
り、画像処理範囲内の値（例えば前方１００ｍまで）を
とる。道路を（９）式で表すと、三角関数を使わず、ク
ロソイド曲線の計算のための数値積分も行わずに、した
がって高速に演算できる。さらに、前方の形状を、原地
点Ｌ^k におけるパラメータのみで計算でき、高速道路の
ように曲率が小さく曲率変化も小さい道路では、誤差は
十分小さく抑えられ、そして、Ｓ軸とＬ軸とはほぼ一致
する。（９）式のＸ^c とＹ^c に０次と１次の項がないの
は、Ｚ＝０における道路中心線をＺ軸と一致させるため
である。

【００３７】＜２＞白線のエッジ座標 入力画像中の白
線との対応のために、複数の白線の位置に対応する３次
元座標を求め、道路モデルとする。図６のように道路中
心線と直角方向にＢ軸をとり、道路中心から白線ｊ（ｊ
＝１〜３）までの距離をＢ_jで表すと、

【数１５】 によって、計算される。ここで、

【数１６】 である。片勾配はｄＹによって付加される。

【００３８】

【数１７】 ＜３＞パラメータの変化点の接続 ＜１＞では同一パラ
メータ区間内について道路形状を高速かつ精度よく計算
する手法を述べた。しかしながら、原地点Ｌ_kにおける
パラメータのみから、前方の道路形状を計算するため、
画像処理範囲内に原地点とは異なるパラメータの区間が
含まれると、（９）式のみで表すことはできない。

【００３９】そこで、パラメータの異なる区間が画像処
理範囲内で接続される場合には、以下の処理を行う。接
続点までの形状をまず計算し、接続点以遠の形状を、接
続点に車両があるものとみなして計算する。そして、接
続点における接線方向が一致するよう平行移動と回転を
行って接続する。

【００４０】以下の説明において、Ｓ₀ における曲率を
ρ₁ （＝ρ（Ｓ₀ ）＝ρ（Ｌ_{ρ(n+1 )} ))なる記号（添え
字１）を用いる。ρ' ₁ やμ、λについても同様であ
る。

【００４１】(1) 平面曲線の接続 Ｓ₀ ＝Ｌ_ρ(n+1) −
Ｌ_k としてＳ₀ が画像処理範囲内にある時、すなわち、
一定曲率部と緩和曲率部との変化点がＳ₀ に現われる場
合、Ｌ＝Ｌ_ρ(n+1) に車両があるとみなして（１１）式
を計算しＸ_p ，Ｚ_p を得るとすると、Ｓ₀ 以遠の形状
は、

【数１８】 である。Ｘ₀ ，Ｚ₀ は（９）式で、ｄＸ₀ とｄＺ₀ は
（１０）式で、それぞれＳ＝Ｓ₀ とおいたものである。
Ｘ_p ，Ｚ_p は、（１７）式でＳ＝Ｓ−Ｓ₀ とおいたもの
である。

【００４２】(2) 縦断曲線の接続 縦断曲線がＳ＝Ｓ₀
にて接続される場合は、道路中心線Ｙｃを接続する。縦
断曲線と、平面曲線や片勾配は独立に設定されるからで
ある。接続点Ｌ＝Ｌ_μ(n+1) に車両があるとみなして計
算した道路中心のＹ座標をＹ_cpとすると、Ｓ₀ 以遠の道
路中心線は、

【数１９】 Ｙ_c （Ｓ）＝Ｙ_c （Ｓ₀ ）＋Ｙ_cp（Ｓ−Ｓ₀ ）＋ｄＹ₀ （Ｓ−Ｓ₀ ） ｄＹ₀ ＝μ（Ｌ_k ）・Ｓ₀ （１９） で近似される。（１８）式の道路中心線に（１５）（１
６）式で片勾配をつければ、道路モデルのＹ座標とな
る。すなわち、

【数２０】 (3) 片勾配の接続 片勾配がＳ＝Ｓ₀ で接続される場合
は、Ｓ₀ 以遠では、（１６）式のｄＹに次式を用いる。

【００４３】

【数２１】 ｄＹ＝λ₁ ＋λ' ₁ （Ｓ−Ｓ₀ ） （２１） このようにして道路の３次元座標が求まるから、道路中
心線および白線の座標部２１において、（９）式、（１
５）〜（２１）式に、Ｓ＝Ｓｉ（但し、ｉ＝１〜Ｎ）と
Ｂｊ（但しｊ＝１〜Ｍ）とをおくことによて道路モデル
の点列を作成することができる。例えば、Ｓｉは道路座
標上で３ｍ間隔にとり最も近い点は７ｍとする。Ｍ＝３
であれば２車線道路となる。

【００４４】次の片勾配回転部２３及び座標変換部２５
では、（５），（６）式で片勾配だけ回転させた後、計
算結果出力部３５からの前時刻の姿勢パラメータによ
り、（７）〜（８）式で道路モデルを画像座標系に変換
する。

【００４５】一方、カメラ１からの画像が画像入力部１
１を介して前処理部１３に加わり、前処理部１３におい
て、（１）式の演算がなされることによりエッジ画像を
検出しているので、線対応マッチング部２７において、
座標変換部２５からの道路モデルの画像座標を受ける一
方、前処理部１３のエッジ画像を受けて、その両者間の
位置ずれを検出し、この検出結果から最小二乗推定部２
９により自車両の車両位置パラメータの変化量およびカ
メラ１の姿勢パラメータの変化量を推定し、パラメータ
更新部３１によりその各パラメータを更新することがで
きる。

【００４６】このパラメ−タ推定更新の原理は次の通り
である。

【００４７】＜１＞パラメータ変動と画像上での点移動 前画面のパラメータから求めた画像上の道路モデルの１
点（ｘ，ｙ）が、現画面では（Δｘ，Δｙ）だけ移動し
たとする。ΔｘとΔｙは、姿勢パラメータ変動と車両位
置変動（ΔＬ：車速に相当）とに起因すると考える。本
節では、各パラメータの変動量と画像上での点の移動量
（Δｘ，Δｙ）との関係を導く。

【００４８】姿勢パラメータのうち回転変動について
は、車両座標系での点の移動として考える。道路座標系
でなく車両座標系で求めるのは、回転角が常に０からの
変動となるため、近似の精度を維持しながら線理解を求
められるからである。

【００４９】車両座標系において回転マトリクスＲ
_αβγを導入する。α，β，γをそれぞれＶ軸、Ｕ軸、
Ｗ軸回りの微小回転角とする。前時刻においては、α，
β，γはすべて０で、現時刻においてα＝Δα，β＝Δ
β，γ＝Δγに変化すると考える。こうして、Ｒ_θφψ
の要素を定数として扱い、Δα，Δβ，Δγを推定する
ものとする。（７）式は、次式で表される。

【００５０】

【数２２】 ただし、sin δ＝δ，cos δ＝１（δ＝α，β，γ）と
近似した。

【００５１】各パラメータの微小変動による画像座標の
変動（Δｘ，Δｙ）は、次式で表される。

【００５２】

【数２３】 （２２）式を（８）式に代入し、（２３）式中の偏微分
成分を、α，β，γ＝０なる条件で近似的に計算する。
右辺第１項から第５項までの偏微分の結果、すなわち姿
勢パラメータの変動項について以下に示す。

【００５３】

【数２４】 ただし、Ｕ，Ｖ，Ｗはα，β，γ＝０とした時（７）式
で得られる値、すなわち、前時刻の結果から計算される
値である。

【００５４】第６項については以下の通りである。

【００５５】まず、接続点が存在しない場合を考える。

【００５６】（１６）式において、車両位置Ｌの変動に
起因するのは、道路パラメータから求められる片勾配ξ
とＸＹＺである。（８）式を微分すると、

【数２５】 （１６）式でα，β，γ＝０とおくと、

【数２６】 ここで、

【数２７】 （９）式より

【数２８】 となる。ただし、ρの２乗以上の項は無視し、ρ' やμ
の微分項も０とおき、λも１より十分小さいものと仮定
した。（２４）式から（２８）式により、（１７）式の
第６項の偏微分が計算される。

【００５７】画像処理の範囲に接続点Ｓ₀ が含まれる場
合は、接続点までは前式で計算し、接続点以遠では以下
の通りとする。計算においては、ρやμの２乗以上の項
は無視し、ρ' やμやλ' の微分項も０とおいて計算す
る。また、ρ₀ （＝ρ（Ｌ_k）），μ₀ ，λ₀ はＬに対
して変数であるが、Ｓ₀ におけるρ₁ （＝ρ（Ｓ₀ ）＝
ρ（Ｌ_ρ(n+1) ))，μ₁ ，λ₁ は定数であり、Ｌの変化
によってＳ₀ が変数となることに注意する。

【００５８】(1) 縦断曲線の接続 まず、簡単のために
縦断曲線Ｙの接続について説明する。μがＳ＝Ｓ₀ で接
続される場合、接続点Ｓ₀ 以遠では、（２０）式で表さ
れ、Ｌでの偏微分は、

【数２９】 ここで、

【数３０】 であるから、

【数３１】 が得られる。（３１）式を計算し、（２６）式に代入す
ればよい。

【００５９】(2) 平面曲線の接続 平面曲線が接続され
る場合も同様の手順で計算できる。ρがＳ＝Ｓ₀ で接続
される場合、接続点Ｓ₀ 以遠で（１２）式をＬで偏微分
すると、

【数３２】 ρの２乗以上の項は無視し、ρ' やμの微分項も０とお
いて計算すると、

【数３３】 が得られ、（２７）式を（２０）式に代入すればよい。
なお、（２７）式の算出にあたって、

【数３４】 などの関係を用いた。

【００６０】(3) 片勾配の接続 接続点以遠における片
勾配とＬでの偏微分はそれぞれ

【数３５】 で表される。よって、（２２）式のλ' ₀ の代わりに
λ' ₁ を用いればよい。（２７）式のλ' ₀ は現在地点
の片勾配変化であるから、接続のある場合においても
λ' ₀ である。

【００６１】前時刻のパラメータ推定結果を現時刻では
定数として扱うため、（２４）（２５）に示す各項
（Ａ）はＳに数値（Ｓ_i ）を与えればすべて計算でき
る。

【００６２】＜２＞線対応マッチング 道路モデルの接線と画像中から抽出される白線の線成分
とを対応づける。白線は滑らかな曲線であるため、点の
対応は困難であるからである。

【００６３】対応付けには画像の探索が必要であるが、
同一ｘ軸、または同一ｙ軸上を走査すれば、探索処理が
簡単化できるため高速化につながる。後述する最小二乗
推定の際の有限語長演算誤差を少なくするため、本手法
では、対応付けする線の傾きの大きさに応じて、ｘ軸走
査かｙ軸走査かを選択する。

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

【数３６】 で表せる。（７）式から

【数３７】 であるので、（１１）式より

【数３８】 から計算できる。

【００６５】なお、平面曲線がＳ₀ で接続される場合
は、Ｓ₀ 以降では、

【数３９】 で計算され、∂Ｘ_p ／∂Ｓと∂Ｚ_p ／∂Ｓは（３２）式
にＳ＝（Ｓ−Ｓ₀ ）とおいて計算したものである。

【００６６】縦断曲線の接続では、

【数４０】 となる。また、片勾配の接続の場合は、（３２）式の
λ' にＳ₀ 以遠のλ' を代入すればよい。

【００６７】いま、現時刻の白線は、点Ｐ付近の点Ｐ'
(ｘ' ，ｙ' ）でモデルと同じωなる傾きをもつと仮定
する。つまり、傾きの変化はないものとする。この場合
の道路モデルと白線との関係は図５のようになる。同一
ｘ軸上の移動量をρ、同一ｙ軸上の移動量をｑとする
と、ΔｘとΔｙとの関係は、

【数４１】 であり、さらに

【数４２】 であるから、

【数４３】 Δｘ−Δｙ／ω＝ｐ （４３） または

【数４４】 −ωΔｘ＋Δｙ＝ｑ （４４） が得られる。｜ω｜が大きい時（垂直に近い場合）は画
像からｐを求めて（７）式を用い、｜ω｜が小さい時
（水平に近い場合）は画像からｑを求めて（４３）式を
用いる。例えば｜ω｜が０の時（水平な線）、ｘ軸方向
に探索しても画像からｐは求められないからである。

【００６８】＜３＞パラメータ推定と更新 （４３）または（４４）式に（２３）式を代入すれば、
各パラメータ変動量と同一座標上での線の移動量との関
係式が得られる。道路モデル上のＮ個の点で（２４）と
（２５）式とω((３６）式）を計算し、各点の画像上で
の移動量ｐまたはｑを求める。そして、（４３）または
（４４）の右辺と左辺の差を評価誤差と考え、最小二乗
法を適用すれば、各パラメータの変化量が求められる。
各点ｉ（ｉ＝１〜Ｎ）における誤差は次式で表され、Σ
Ｅ_i ² を最小にする変化量を求めればよい。

【００６９】

【数４５】 Ｅ_i ＝Ａ_X(i)ΔＤ_X ＋Ａ_Y(i)ΔＤ_Y ＋Ａ_α(i) Δα＋Ａ_β(i) Δβ ＋Ａ_γ(i) Δγ＋Ａ_L(i)ΔＬ−Ｂ_(i) （４５） ここで、例えば

【数４６】 である。Ｔは１位のしきい値。

【００７０】パラメータの変化量が算出された後、前時
刻のパラメータを更新する。前時刻のパラメータを添字
ｋ−１で表し、更新されたパラメータを添字ｋで表す。

【００７１】座標回転を表すパラメータ（θ，φ，ψ）
以外は、前回のパラメータに推定された変化量を加えれ
ばよい。すなわち、

【数４７】 Ｌ_k ＝Ｌ_k-1 ＋ΔＬ Ｄ_X(k)＝Ｄ_X(k-1)＋ΔＤ_X Ｄ_Y(k)＝Ｄ_Y(k-1)＋ΔＤ_Y （４７） 回転を表すパラメータについては、

【数４８】 Ｒ_θφψ(k) ＝Ｒ_αβγＲ_θφψ(k) （４８） により更新される。ただし、Ｒ_αβγは（１６）式にお
いてα＝Δα，β＝Δβ，γ＝Δγとおいたものであ
る。パラメータ_θ(k) ，_φ(k) ，_ψ(k) は、更新された
マトリクスＲ_θφψ(k) の要素から求められる。

【００７２】このようなパラメータ推定更新の原理でそ
の処理がなされるので、上記したように３次元道路モデ
ルの点列の作成後、次に、前時刻の姿勢パラメータによ
り、（５）式〜（７）式で車両座標系に変換し、また、
（３６）〜（４０）式で接線の傾きを計算する。そし
て、係数Ａ_(i) を（２４）〜（３５）式と（４５）式に
従って計算する。さらに（８）式でモデル点を車両座標
系から画像座標系に変換し、各モデル点の座標（ｘ_i ，
ｙ_i ）（ｉ＝１〜Ｎ）を得る。

【００７３】次に、線対応マッチング部２７によりモデ
ル点接線とエッジ画像とのマッチングを行う。すなわ
ち、図７において、｜ω｜≧Ｔのときは、各モデル点
（ｘ_i ，ｙ_i ）を中心にｘ軸方向に探索してρ_i を求
め、｜ω_i ｜＜Ｔのときは、ｙ軸方向に探索してｑ_i を
求める。

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

【数４９】 を求め、Ｃ_i(τ) の最大値近傍で重心を求めｐ_i とす
る。またＣ_i(τ) の最大値を確からしさＣ_i とする。ｙ
軸方向に探索の場合も同様にしてｑ_i を求める。

【００７５】探索ウインドウは、ｘ軸方向の探索時は、
Ｍ_x ＝８，Ｍ_y ＝３（画素）とし、ｙ軸方向に探索する
場合は、共に３画素程度でよい。ただし、これらの値は
モデルと白線が充分マッチした時点の値で、イニシャラ
イズ時は大きな値に設定した。

【００７６】次に、最小二乗推定部２９において、車両
位置パラメータの変化量およびカメラの姿勢パラメータ
の変化量を推定する。

【００７７】ここでは、（４５）式に基づいて最小二乗
法を適用する。この際、確からしさＣ_i を各点の重みと
して用いる。すなわち、

【数５０】 を評価誤差として最小化する。（５０）を最小化するよ
う、６個の変数（ΔＤ_X，ΔＤ_Y ，Δα，Δβ，Δγ，
ΔＬ）でそれぞれ偏微分し０とおく。結果として６元連
立１次方程式が得られ、方程式を解いた後、（４７）〜
（４８）に従ってパラメータの更新を行う。

【００７８】ΔＬは、車速信号が目安となるので、

【数５１】 Ｅ_ＶＥＬ＝Ｃ_Ｖ・（Ｖ_ＶＩＨＣ−ΔＬ）² （５１） とおいて、（４５）式とあわせた、

【数５２】 Ｅ_{ＭＯＤＥＬ}＋Ｅ_ＶＥＬ （５２） を最小化してもよい。この場合、車両の車速センサから
得られる車速の１倍から１．２倍程度の値をＶ_VIHCとす
るのが好ましい。

【００７９】以上のように本発明の一実施例において
は、地図情報として、水平曲率、垂直曲率、バンク角等
の道路の３次元形状に対応するデータを含む地図情報を
予め記憶手段に格納しておき、上記地図情報を参照して
道路形状を算出する。従って、カメラの姿勢パラメータ
が変化しても道路形状を精度良く求めることができ、同
時にその求めた道路形状と入力された画像とのずれか
ら、グローバルかつローカルな自車両位置を推定するこ
とができる。

【００８０】なお、上記した本発明の一実施例の説明で
は、地図情報として、水平曲率、垂直曲率、バンク角と
して説明したが、車線数や車線の幅（Ｂｊに対応）など
の情報も付加情報として追加できる。また、一般道にお
いては、交差点や分岐・分流を表わすノードと、２つの
ノードを結ぶパスとで構成し、パスの情報として、前記
地図情報を用いればよい。さらにガソリンスタンドや、
信号機の有無などの情報も容易に付加でき、ナビゲーシ
ョン装置の地図と自車位置検出としても使える。

【００８１】また、垂直曲率にあわせて勾配（何％とい
う）値も格納しておけばスロットルなどの予見制御にも
つながる。なお、現在位置Ｌk の推定には初期位置が既
知である必要があるラビゲーション装置の現在位置を用
いてもよいし、高速道路の入口などスイッチ操作による
現在位置を設定するようにしてもよい。

【００８２】また、本発明の道路形状及び自車両位置計
測装置は、追突警報装置および車線逸脱警報装置に適用
することができ、これらを図８、図９にそれぞれ従って
以下説明する。

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

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

【００８５】なお、前車両が自車両と同じ車線を走行し
ているか否かの判断は以下のように行う。前車両の自車
両に対する位置が（Ｘｓ，Ｙ＝０，Ｚｓ）として求まっ
たとする。

【００８６】道路中心線は（９）式や（１８）式で求め
られる。曲率が小さい場合Ｓ＝Ｚと近似できるので
（９）式や（１８）式でＳ＝Ｚ_s においてＸ（Ｓ_z ）を
計算する。この値をＸ_p をおくとＸ_p は Ｘｐ＋Ｂ＞Ｘｓ＞Ｘｐ の場合は、左車線 Ｘｐ−Ｂ＜Ｘｓ＜Ｘｐ の場合は、右車線 を走行していると判断できる。Ｂは車線幅である自車両
の位置はＤ_X で与えられ、 Ｄ_X ＞０ の場合には、左車線 Ｄ_X ＜０ の場合には、右車線 である。そして、同一車線を走行していて、かつＺｓが
所定の距離以下まで近づいた場合に、上述したように警
報を発生すればよい。なお、例えばＸｓ＜Ｘｐ＋Ｂ（車
線幅）の場合には、路側帯の障害物と判断できる。Ｓ字
道路でも正確な判断を行うことができる。

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

【００８８】更に、自動操縦について説明すると、本発
明の計測装置では、地図情報から自車両の位置の曲率の
他にも前方の曲率も求めることができるので、予め速度
を制御する自動操縦が可能となる。例えば、次のように
制御することができる。

【００８９】 ｜前方の水平曲率｜＞｜現在の水平曲率｜→減速 ｜前方の水平曲率｜＜｜現在の水平曲率｜→加速 前方の垂直曲率（勾配）＞現在の垂直曲率（勾配）→加
速（スロットル開） 前方の垂直曲率（勾配）＜現在の垂直曲率（勾配）→減
速（スロットル閉） なお、前方の曲率は現在地点Ｌ_k の代わりにＬ_k ＋Ｌ´
において地図情報を参照し（２）〜（４）式を計算すれ
ばよい。

【００９０】また、例えば、ピッチ角φ、ロール角ψ、
高さＤｙの変動が大きい場合には、減速すること等も考
えられる。乗り心地のフィードバックと言える。

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

【００９２】

【発明の効果】以上説明したように本発明によれば、地
図情報として道路の３次元形状に対応するデータを持
ち、現在位置パラメータに基づいてその地図情報を参照
して道路形状を算出し、この道路形状を撮像手段の姿勢
パラメータに基づいて画像座標に変換し、撮像手段の入
力画像との対応関係から、車両の現在位置と撮像手段の
姿勢とを更新するようにしたため、道路形状の計測およ
び自車両位置の推定を正確に行える。

【図面の簡単な説明】

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

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

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

【図４】道路形状の一例を示す図である。

【図５】地図情報と道路形状パラメータの一例を示す図
である。

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

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

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

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

【符号の説明】

１０１ 撮像手段 １０２ 道路形状検出手段 １０３ 地図情報記憶手段 １０４ パラメータ保持手段 １０５ 道路モデル演算手段 １０６ 座標変換手段 １０７ 位置ずれ検出手段 １０８ 変化量演算手段 １０９ 更新手段

Claims (1)

【特許請求の範囲】
1. 【請求項１】 車両の進行方向前方の道路を撮像する撮
   像手段と、 該撮像手段で撮像した画像から車両の進行方向前方の道
   路形状を表わす特徴を抽出する道路形状検出手段と、 道路の３次元形状に対応するデータを含む地図情報を予
   め記憶した地図情報記憶手段と、 １回前に計測された車両位置パラメータ及び前記撮像手
   段の姿勢パラメータを保持するパラメータ保持手段と、 前記車両位置パラメータに基づいて、前記地図情報を参
   照し、３次元座標上で定義された３次元道路モデルを演
   算する道路モデル演算手段と、 該道路モデル演算手段で演算された３次元道路モデルを
   前記撮像手段の姿勢パラメータに基づいて画像座標に変
   換する座標変換手段と、 前記道路形状検出手段で検出された前記道路形状を、前
   記座標変換手段で座標変換された前記３次元道路モデル
   と比較して、両者間の位置ずれを検出する位置ずれ検出
   手段と、 該位置ずれ検出手段で検出した位置ずれから前記車両位
   置パラメータの変化量および前記撮像手段の姿勢パラメ
   ータの変化量を演算する変化量演算手段と、 前記両パラメータの変化量に基づき前記パラメータ保持
   手段の各パラメータを更新する更新手段とを有すること
   を特徴とする道路形状及び自車両位置の計測装置。