JP6845124B2 - 走路推定装置及びプログラム - Google Patents

Description

本発明は、走路推定装置及びプログラムに関する。

従来より、車載カメラで前方の走路の平面線形とともに、勾配変化等の縦断線形を推定する方法として、（ａ）ステレオカメラを利用して道路の三次元構造を復元する方法（特許文献１〜３）、（ｂ）単眼カメラで地図情報を利用する方法（特許文献４）、（ｃ）単眼カメラで車線幅一定の仮定を利用する方法（特許文献５）、（ｄ）単眼カメラで白線の間隔が一定の破線であることを仮定する方法（特許文献６）、（ｅ）車載単眼カメラの移動ステレオにより路面勾配変化を推定する方法（特許文献７）などが提案されている。

特開平９−５０５９２号広報 特開平９−３２５０２６号広報 特開２０１１−２８６５９号広報 特開平１１−３９４６４号広報 特開平９−４８２９９号広報 特開２０１２−２５５７０３号広報 特開２０１３−２３９１４３号広報

しかしながら、上述の方法には、以下のような問題がある。

上記特許文献１〜３記載の方法では、単眼カメラに比べて、カメラ数が増加し、ステレオカメラ画像間のマッチング計算に専用ハードが必要で、コストが増大する。

上記特許文献４記載の方法では、道路の高さを含む３次元構造を表現する地図情報が必要となり、その地図の作成コストが増大する。車両位置と地図とマッチングをとるために測位装置が必要となる。

上記特許文献５記載の方法では、左右の車線境界線（白線）が無いと計算できない。すなわち、片方の白線が摩耗して掠れていたり、分岐や合流箇所で片方の白線が無かったりする箇所では適用できない。

上記特許文献６記載の方法では、白線が実線の場合に適用できない。すなわち、実線の場合は、白線は単調な形状であるため、連続する二時刻間で白線上の点の一対一照合が困難であり、画像処理の分野で窓問題（Ａｐｅｒｔｕｒｅ Ｐｒｏｂｌｅｍ）に陥ってしまう。そのため、単眼ステレオによる三次元復元が困難である。

上記特許文献７記載の方法では、２時刻間の画像のマッチング計算に専用ハードが必要で、コストが増大する。また、特に白線に関しては、画像処理の窓問題（Ａｐｅｒｔｕｒｅ Ｐｒｏｂｌｅｍ）が発生して、２時刻の白線候補点間のマッチング点が一意に決まらず計算困難である。

すなわち、撮像画像に写る道路上の車線境界線（白線）が左右どちらか一方だけ単独の場合、かつ、路面勾配変化がある場合には、精度良く走路を推定することができない、という問題があった。

本発明は、上記問題点を解決するためになされたもので、走路に勾配変化がある場合であっても、精度良く走路の境界線を推定することができる走路推定装置及びプログラムを提供することを目的とする。

上記の目的を達成するために、本発明の走路推定装置は、移動体に搭戦された撮像装置によって撮像された前記移動体の走行する走路の画像から、線状に並んだ複数の境界線候補点を抽出する境界線候補点抽出部と、前記複数の境界線候補点を観測値として、少なくとも１つの境界線候補点の各々までの実空間上の距離と、境界線の線形に関するパラメータと、前記境界線に対する前記移動体のオフセット、及び姿勢角とを含む状態ベクトルを前記境界線候補点と前記状態ベクトルから画像上へ投影した予測点との間の誤差を最小化して推定する走路パラメータ推定部と、を含んで構成される。

また、本発明のプログラムは、コンピュータを、移動体に搭戦された撮像装置によって撮像された前記移動体の走行する走路の画像から、線状に並んだ複数の境界線候補点を抽出する境界線候補点抽出部、及び前記複数の境界線候補点を観測値として、少なくとも１つの境界線候補点の各々までの実空間上の距離と、境界線の線形に関するパラメータと、前記境界線に対する前記移動体のオフセット、及び姿勢角とを含む状態ベクトルを前記境界線候補点と前記状態ベクトルから画像上へ投影した予測点との間の誤差を最小化して推定する走路パラメータ推定部として機能させるためのプログラムである。

本発明の走路推定装置及びプログラムによれば、境界線候補点抽出部が、移動体に搭戦された撮像装置によって撮像された前記移動体の走行する走路の画像から、線状に並んだ複数の境界線候補点を抽出し、走路パラメータ推定部が、前記複数の境界線候補点を観測値として、少なくとも１つの境界線候補点の各々までの実空間上の距離と、境界線の線形に関するパラメータと、前記境界線に対する前記移動体のオフセット、及び姿勢角とを含む状態ベクトルを前記境界線候補点と前記状態ベクトルから画像上へ投影した予測点との間の誤差を最小化して推定する。

このように、走路の画像から、線状に並んだ複数の境界線候補点を抽出し、複数の境界線候補点を観測値として、少なくとも１つの境界線候補点の各々までの実空間上の距離と、境界線の線形に関するパラメータと、境界線に対する移動体のオフセット、及び姿勢角とを含む状態ベクトルを境界線候補点と状態ベクトルから画像上へ投影した予測点との間の誤差を最小化して推定することにより、走路に勾配変化がある場合であっても、精度良く走路を推定することができる。

また、本発明の走路推定装置は、移動体に搭載された撮像装置によって撮像された前記移動体の走行する走路の画像から、線状に並んだ複数の境界線候補点を抽出する境界線候補点抽出部と、前記複数の境界線候補点を観測値として、カルマンフィルタにより、少なくとも１つの境界線候補点の各々までの実空間上の距離と、境界線の線形に関するパラメータと、前記境界線に対する前記移動体のオフセット、及び姿勢角とを含む状態ベクトルを推定する走路パラメータ推定部と、を含んで構成される。

本発明に係る走路推定装置によれば、境界線候補点抽出部が、移動体に搭載された撮像装置によって撮像された前記移動体の走行する走路の画像から、線状に並んだ複数の境界線候補点を抽出し、走路パラメータ推定部が、前記複数の境界線候補点を観測値として、カルマンフィルタにより、少なくとも１つの境界線候補点の各々までの実空間上の距離と、境界線の線形に関するパラメータと、前記境界線に対する前記移動体のオフセット、及び姿勢角とを含む状態ベクトルを推定する。

このように、走路の画像から、線状に並んだ複数の境界線候補点を抽出し、複数の境界線候補点を観測値として、カルマンフィルタにより、少なくとも１つの境界線候補点の各々までの実空間上の距離と、境界線の線形に関するパラメータと、境界線に対する移動体のオフセット、及び姿勢角とを含む状態ベクトルを推定することにより、走路に勾配変化がある場合であっても、精度良く走路を推定することができる。

また、本発明の走路推定装置は、前記境界線の線形に関するパラメータは、水平面における前記境界線の曲率及び曲率変化率と、垂直断面における前記境界線の曲率とであるとすることができる。

また、本発明の走路推定装置は、前記境界線の線形に関するパラメータは、水平面における前記境界線の曲率及び曲率変化率と、及び垂直断面における前記境界線の曲率及び曲率変化率とであるとすることができる。

また、本発明の走路推定装置は、前記境界線候補点抽出部は、前記複数の境界線候補点から境界線幅を算出し、前記状態ベクトルは、前記境界線幅を更に含むことができる。

また、本発明の走路推定装置は、前記状態ベクトルは、前記複数の境界線候補点の各々までの実空間上の距離のうち、最遠点である前記境界線候補点の実空間上の距離と、前記境界線の線形に関するパラメータと、前記境界線に対する前記移動体のオフセット及び姿勢角とを含むことができる。

また、本発明の走路推定装置は、前記走路パラメータ推定部は、カルマンフィルタの時間更新行列として、前記境界線の線形に関するパラメータと、前記境界線に対する前記移動体のオフセット、及び姿勢角と、前記複数の境界線候補点の各々までの実空間上の距離との各々に対応する要素を含む行列を用いて、前記状態ベクトルを推定することができる。

また、本発明の走路推定装置は、前記移動体の車速と、前記移動体の姿勢角とを取得する取得部を更に含み、前記走路パラメータ推定部は、前記車速と、前記姿勢角とに対応する要素を更に含む前記時間更新行列を用いて、前記状態ベクトルを推定することができる。

以上説明したように、本発明の走路推定装置及びプログラムによれば、走路の画像から、線状に並んだ複数の境界線候補点を抽出し、複数の境界線候補点を観測値として、少なくとも１つの境界線候補点の各々までの実空間上の距離と、境界線の線形に関するパラメータと、境界線に対する移動体のオフセット、及び姿勢角とを含む状態ベクトルを境界線候補点と状態ベクトルから画像上へ投影した予測点との間の誤差を最小化して推定することにより、走路に勾配変化がある場合であっても、精度良く走路を推定することができる、という効果が得られる。

従来技術と本発明の実施の形態との違いを示すイメージ図である。 本発明の実施の形態における処理の流れの概要を表す図である。 本発明の実施の形態における走路推定装置の概略構成を示すブロック図である。 本発明の第１の実施の形態における道路モデルの例を示す図である。 本発明の実施の形態における走路パラメータ推定処理ルーチンの内容を示すフローチャートである。 本発明の実施の形態における数値実験（Ａ）のシミュレーション結果を示す図である。 本発明の実施の形態における数値実験（Ｂ）のシミュレーション結果を示す図である。 本発明の第２の実施の形態における道路モデルの例を示す図である。

以下、図面を参照して本発明の実施の形態を詳細に説明する。

＜本発明の実施の形態の概要＞
従来技術（特許文献７）では、単眼移動ステレオを用いて路面の三次元構造を復元して、路面の凹凸を推定する方法が提案されていた。この従来技術では、２時刻間で同一の路面特徴点を照合して、その照合結果と車速から、三角測量の原理で距離を算出し、路面の垂直輪郭を検出していた。

しかし、白線は単調な形状であるため、２時刻間の白線点を一対一で高精度に照合することは難しかった（図１）。そのため、個々の白線点の照合に基づく、白線の３次元構造の復元は困難だった。

本発明の実施の形態では、２時刻間の個々の白線点を照合せずに、白線の３次元構造（平面線形、縦断線形）を推定する。このことにより、路面勾配変化（路面の起伏）箇所での、平面曲率の誤差を低減し、走路認識に基づく自動走行を安定化させる。

解決のアプローチは、白線の３次元構造（平面線形、縦断線形）を滑らかな関数（３次関数）でモデル表現して、自車両の移動による画像上の白線パターン全体の形状変化を追跡することにより３次元構造（平面線形、縦断線形）を復元する。また、白線パターン全体の形状パターンを追跡するので、個々の白線点の照合は不要となる。

車載単眼カメラ画像による走路認識において、３次元の白線パターン全体を２時刻間で照合・追跡することにより、従来推定困難だった路面勾配変化・片側単独白線の場合でも走路推定が可能となる。

本実施の形態の処理の流れの概略を図２に示す。車載単眼カメラによって道路画像を撮像し、そして入力された道路画像上をスキャンして、背景（路面）から輝度が明るく変化する点（白線の稜線部）から数点の観測点（以後、白線点）を抽出する。

この白線点を観測値として、道路モデルのパラメータ推定値をカルマンフィルタで更新して、後段の警報や制御へ推定値を出力する。

また、本実施の形態における道路モデルは、パラメータとして、白線の平面線形（平面曲線曲率とその曲率変化率）、白線の縦断線形（縦断曲線曲率とその曲率変化率）、白線上の着目点までの三次元距離、および、白線とカメラ（車両）との相対位置姿勢、を含むものである。

当該道路モデルに、パラメータとして、白線の縦断線形と、白線上の着目点までの三次元距離とを道路モデルに含むことにより、路面勾配変化・片側単独白線の場合の曲率、ヨー角、曲率等の推定精度が改善することができる。

以下、車載単眼カメラ画像による走路推定へ適用する場合について説明し、実験により、路面勾配変化・片側単独白線の場合の曲率、ヨー角、曲率等の推定精度が改善できたことを説明する。なお、本実施形態では、簡単のために白線上の着目点は観測点と一致させている。

＜本発明の第１の実施形態に係る走路推定装置１００の構成＞
図３に示すように、本発明の第１の実施の形態に係る走路推定装置１００は、自車両の前方を撮像する撮像装置１１０と、ヨーレイトセンサ１２０と、車速センサ１３０と、撮像装置１１０によって撮像された自車両の前方の道路画像、ヨーレイトセンサ１２０によって検出された自車両のヨーレイト、及び車速センサ１３０によって検出された自車両の車速に基づいて、走路パラメータを推定するコンピュータ１４０とを備えている。

撮像装置１１０は、自車両の前方の道路を撮像する車載単眼カメラ画像である。撮像装置１１０は、例えば車両のリアビューミラー付近に設置され、自車両が走行する走路の画像の一例として、自車両の前方の道路画像を撮像する。

コンピュータ１４０は、ＣＰＵと、ＲＡＭと、後述する走路パラメータ推定処理ルーチンを実行するためのプログラムを記憶したＲＯＭとを備え、機能的には次に示すように構成されている。

コンピュータ１４０は、境界線候補点抽出部１４２と、走路パラメータ推定部１４４と、走路パラメータ記憶部１４６とを備えている。

境界線候補点抽出部１４２は、撮像装置１１０によって撮像された道路画像に基づいて、自車両が走行する走行車線の境界として、線状に並んだ複数の白線の候補点である白線点を抽出する。

具体的には、境界線候補点抽出部１４２は、道路画像から走行車線と白線との境界線の候補点の位置の座標値を抽出する。例えば、自車両が走行中の車線の左又は右の白線との境界線を抽出する。

一般には、走路には白線が敷設されているため、境界線候補点抽出部１４２は、白線点を走行車線の境界線の候補点として抽出する。白線は、道路画像上では走路面に対して輝度が高いため、境界線候補点抽出部１４２は、例えば、道路画像を既知のＳｏｂｅｌフィルタ等によって処理し、得られた処理結果が閾値よりも大きい点を走行車線の白線点として抽出する。なお、白線点は、走行車線と隣接する車線と白線との境界線の候補点として抽出されてもよい。

さらに、白線は、局所的には進行方向に直線状に連続しているため、Ｈｏｕｇｈ変換等の手法により、直線状に連続している箇所を、車線の白線点として絞り込んでもよい。

そして、境界線候補点抽出部１４２は、抽出した複数の白線点の道路画像上の座標値を算出し、走路パラメータ推定部１４４に渡す。

走路パラメータ推定部１４４は、境界線候補点抽出部１４２により抽出した複数の白線点の座標に基づいて、走路パラメータを、境界線候補点と状態ベクトルから画像上へ投影した予測点との間の誤差を最小化して推定する。例えば、走路パラメータ推定部１４４は、カルマンフィルタを用いて、当該状態ベクトルを推定する。

ここで、走路パラメータを、カルマンフィルタを用いて推定する原理について説明する。

まず、本実施の形態における道路モデルについて説明する。

道路上の白線の平面線形と縦断線形は、カメラ位置を原点として三次元直交座標系ＸＹＺ上の曲線として表現する（図４）。

白線の平面線形は、下記式（１）で表す。

また、白線の縦断線形は、下記式（２）で表す。

ここで、ｅは白線のオフセット［ｍ］、ｃ_０は道路の曲率［１／ｍ］、ｃ_１は道路の曲率変化率（

）［１／ｍ^２］、ｃ_ｖ０は道路の縦断曲率［１／ｍ］、ｃ_ｖ１は道路の縦断曲率の変化率（

）［１／ｍ^２］、ｈ_ｃは路面平面からのカメラの高さ［ｍ］、ｘ_ｊはｊ番目の白線点（観測値）のＸ座標値［画素］、ｙ_ｊはｊ番目の白線点（観測値）のＹ座標値［画素］、ｚ_ｊはｊ番目の白線点（観測値）のＺ座標値（距離）［画素］である。

次に、下記式（３）〜（５）から、ヨー角θ（単位は［ｒａｄ］）による回転座標変換後の白線点座標値を得る。

次に、下記式（６）〜（８）から、ピッチ角φ（単位は［ｒａｄ］）による回転座標変換後の白線点座標値を得る。

そして、下記式（９）及び（１０）で表す射影変換により、画像上のｊ番目の白線点座標値（ｉｘ_ｊ，ｉｙ_ｊ）が計算できる。

このとき、ｉｘ_ｊは、画像上のｊ番目の白線点の水平方向の座標値［画素］、ｉｙ_ｊは、画像上のｊ番目の白線点の垂直方向の座標値［画素］である。

また、ｆは焦点距離［ｍ］、ｉｘ_ｃは画像中心の水平方向の座標値［画素］、ｉｙ_ｃは画像中心の垂直方向の座標値［画素］、ｒ_ｈは画像の水平方向の解像度［ｍ／画素］、ｒ_ｖは画像の垂直方向の解像度［ｍ／画素］である。

上記をまとめると、ｊ番目の白線点に対して、下記式（１１）の観測関数が得られる。

そして、観測するｍ個の白線点をまとめた観測関数ｈは、下記式（１２）で表される。

以上のカルマンフィルタの推定パラメータを要素とした状態ベクトルｘを、下記式（１３）に示す。

このとき、Ｔは転置記号である。

このように、本実施の形態に係る状態ベクトルｘは、パラメータとして、白線の平面線形に関するパラメータ（平面曲線曲率ｃ_０とその曲率変化率ｃ_１）、白線の縦断線形に関するパラメータ（縦断曲線曲率ｃ_ｖ０とその曲率変化率ｃ_ｖ１）、白線点までの三次元距離ｚ_０，・・・，ｚ_ｍ−１を含む。

次に、この状態ベクトルｘの時刻ｔ−１からｔへの離散時間更新モデルを下記式（１４）〜（１６）に示す。

このとき、Δｔは時刻ｔ−１から時刻ｔまでの更新時間［ｓ］、Δｚは更新時間Δｔ内の移動距離（＝車速×Δｔ）［ｍ］、ωはヨーレイト［ｒａｄ／ｓ］、ｕはシステムノイズ、Ｆは状態ベクトルｘの時間更新行列、Ｂはヨーレイトωから状態ベクトルｘへの寄与項である。

次に、カルマンフィルタの更新式を示す。カルマンフィルタはダイナミクスによる時間更新と、白線点の観測による観測更新の２ステップを更新時間毎に交互に実行する。

時間更新について、下記式（１７）及び（１８）に示す。

観測更新について、下記式（１９）〜式（２２）に示す。

このとき、Ｐは誤差共分散行列、Ｈは観測ヤコビアン行列、Ｒは観測ノイズ分散行列、Ｑはシステムノイズ共分散行列（Ｑ＝Ｅ［ｕｕ^Ｔ］，Ｋはカルマンゲイン、ｙは観測値ベクトルである。

ここで、観測値ベクトルｙは、道路画像から境界線候補点抽出部１４２により抽出されたｍ個の白線点の座標値（ｉｘ_ｊ，ｉｙ_ｊ）を、順に並べたベクトルである（下記式（２３））。

また、観測ヤコビアン行列Ｈは、下記式（２４）のように、０，・・・，ｍ−１番目の観測関数ｈ_ｉを状態ベクトルｘで偏微分した観測ヤコビアン行列Ｈ_ｊを縦に積み重ねた形となる。

このとき、ｊ番目の白線点の観測ヤコビアン行列Ｈ_ｊは、下記式（２５）で表される。

以上説明した原理により、走路パラメータ推定部１４４は、抽出した複数の白線点の座標である観測値ベクトルｙに基づいて、走路パラメータを含む状態ベクトルｘを推定する。

また、走路パラメータ推定部１４４は、状態ベクトルｘについて、曲率変化率ｃ_ｖ１を含まないものとすることができる。この場合、上記の各行列（Ｆ、Ｂ、Ｈ等）から曲率変化率ｃ_ｖ１に対応する要素を除くように構成すればよい。白線の縦断線形に関するパラメータ（縦断曲線曲率ｃ_ｖ０）を考慮しつつ、曲率変化率ｃ_ｖ１を含めた場合よりも計算量を抑えることができるためである。

また、走路パラメータ推定部１４４は、白線点までの実空間上の距離のパラメータを、最遠点の白線点に限定する構成としてもよい。この場合、状態ベクトルｘについて、最遠点以外の白線点までの実空間上の距離のパラメータを含まないものとし、上記の各行列（Ｆ、Ｂ、Ｈ等）から最遠点以外の白線点までの実空間上の距離のパラメータに対応する要素を除くように構成すればよい。最遠点の境界線候補点の有無が推定精度に最も寄与するものであるため、最遠点の境界線候補点以外の境界候補点については、走路パラメータの推定に用いない構成とすることができる。このような構成により、走路パラメータの推定精度を維持しつつ、状態ベクトルｘのパラメータ数と、観測値数とを低減することができ、計算量の低減を図ることができるからである。

最遠点の白線点に限定する構成とする場合、例えば、状態ベクトルｘを示す上記式（１３）において、白線点までの三次元距離ｚ_０，・・・，ｚ_ｍ−１のうち、最も遠い三次元距離（例えば、ｚ_ｍ−１）のみを用い、他の三次元距離（ｚ_０，・・・，ｚ_ｍ−２）については、状態ベクトルｘに含めないものとする。この場合、上記の各行列（Ｆ、Ｂ、Ｈ等）から他の三次元距離（ｚ_０，・・・，ｚ_ｍ−２）に対応する要素を除くように構成する。また、この場合、上記式（２３）において、観測値ベクトルｙは、ｉｘ_ｍ−１と、ｉｙ_ｍ−１とを要素とする。

具体的には、走路パラメータ推定部１４４は、境界線候補点抽出部１４２により抽出された走行車線の境界の位置（道路画像上の走行車線の白線点の座標値）に基づいて、カルマンフィルタにより走路パラメータの推定処理を行う。

カルマンフィルタによる推定処理は、予測ステップとフィルタリングステップとを含んで構成される。以下、走路パラメータ推定部１４４によって実行されるカルマンフィルタによる予測ステップとフィルタリングステップとについて説明する。

（１）予測ステップ
まず、予測ステップにおける処理について説明する。走路パラメータ推定部１４４は、予測ステップにおいて、走路パラメータ記憶部１４６に記憶されている前回のフィルタリングステップで算出された時刻ｔ−１の走路パラメータｘ_ｔ−１と、上記式（１６）に示す時間更新行列Ｆとに基づいて、上記式（１７）に従って、時刻ｔの走路パラメータｘ_{ｔ，ｔ−１}を算出する。

そして、走路パラメータ推定部１４４は、上記式（１６）に示す時間更新行列Ｆと、走路パラメータ記憶部１４６に記憶されている前回のフィルタリングステップで推定された誤差共分散行列Ｐ_ｔ−１と、システムノイズ共分散行列Ｑとに基づいて、上記式（１８）に従って、誤差共分散行列Ｐ_{ｔ，ｔ−１}を算出する。

（２）フィルタリングステップ
次に、フィルタリングステップにおける処理について説明する。走路パラメータ推定部１４４は、フィルタリングステップにおいて、予測ステップで算出された誤差共分散行列Ｐ_{ｔ，ｔ−１}と、観測ヤコビアン行列Ｈと、観測ノイズ分散行列Ｒとに基づいて、上記式（２１）に従って、カルマンゲインＫを算出する。

走路パラメータ推定部１４４は、算出されたカルマンゲインＫと、予測ステップで算出された時刻ｔにおける状態ベクトルｘ_{ｔ，ｔ−１}と、境界線候補点抽出部１４２により算出された白線点の座標値を用いて得られる観測値ベクトルｙとに基づいて、上記式（１９）に従って、時刻ｔにおける走路パラメータｘ_ｔを推定する。

そして、走路パラメータ推定部１４４は、予測ステップで推定された誤差共分散行列Ｐ_{ｔ，ｔ−１}と、算出されたカルマンゲインＫと、上記式（２２）に示す観測ヤコビアン行列Ｈとに基づいて、上記式（２０）に従って、誤差共分散行列Ｐ_ｔを算出する。

走路パラメータ推定部１４４は、フィルタリングステップで算出された各値（カルマンゲインＫ、走路パラメータｘ_ｔ、誤差共分散行列Ｐ_ｔ）を走路パラメータ記憶部１４６へ渡す。当該各値は、次回の予測ステップにおける処理で用いられる。

また、走路パラメータ推定部１４４は、推定された走路パラメータを、警報装置１５０及び車両制御装置１６０へ出力する。

走路パラメータ記憶部１４６は、走路パラメータ推定部１４４で算出された各値（カルマンゲインＫ、走路パラメータｘ_ｔ、誤差共分散行列Ｐ_ｔ）を記憶する。

警報装置１５０は、走路パラメータに基づいて、自車両のドライバーに対し警報を出力する。例えば、走路パラメータに含まれる自車両のオフセット（横位置）と、自車両のヨー角に基づいて、車線逸脱を示す警報を出力する。

車両制御装置１６０は、走路パラメータの各々に基づいて、運転支援や自動運転を行う。

＜本発明の第１の実施の形態に係る走路推定装置１００の動作＞
次に、本実施形態に係る走路推定装置１００の動作について説明する。まず、自車両が走行し、撮像装置１１０によって自車両の前方が逐次撮像され、ヨーレイトセンサ１２０によって自車両のヨーレイトが逐次検出され、車速センサ１３０によって自車両の車速が逐次検出されているときに、コンピュータ１４０において、図５に示す走路パラメータ推定処理ルーチンが実行される。

まず、ステップＳ１００において、コンピュータ１４０は、撮像装置１１０から道路画像と、ヨーレイトセンサ１２０からヨーレイトと、車速センサ１３０から車速とを取得する。

次に、ステップＳ１１０において、境界線候補点抽出部１４２は、撮像装置１１０によって撮像された道路画像に基づいて、線状に並んだ複数の白線の候補点である白線点を抽出する。

ステップＳ１２０において、境界線候補点抽出部１４２は、白線点の道路画像上の座標値を算出する。

ステップＳ１３０において、境界線候補点抽出部１４２は、全ての白線点について座標値を算出したか否かを判定する。

全ての白線点について座標値を算出していない場合（ステップＳ１３０のＮＯ）、ステップＳ１２０に戻り、次の白線点を選択して座標値を算出する。

全ての白線点について座標値を算出した場合（ステップＳ１３０のＹＥＳ）、ステップＳ１４０において、走路パラメータ推定部１４４は、カルマンフィルタの予測ステップにおいて、走路パラメータ記憶部１４６に記憶されている前回のフィルタリングステップで算出された時刻ｔ−１の走路パラメータｘ_ｔ−１と、上記式（１６）に示す時間更新行列Ｆとに基づいて、上記式（１７）に従って、時刻ｔの走路パラメータｘ_{ｔ，ｔ−１}を算出する。

ステップＳ１５０において、走路パラメータ推定部１４４は、上記式（１６）に示す時間更新行列Ｆと、走路パラメータ記憶部１４６に記憶されている前回のフィルタリングステップで推定された誤差共分散行列Ｐ_ｔ−１と、システムノイズ共分散行列Ｑとに基づいて、上記式（１８）に従って、誤差共分散行列Ｐ_{ｔ，ｔ−１}を算出する。

ステップＳ１６０において、走路パラメータ推定部１４４は、フィルタリングステップにおいて、予測ステップで算出された誤差共分散行列Ｐ_{ｔ，ｔ−１}と、観測ヤコビアン行列Ｈと、観測ノイズ分散行列Ｒとに基づいて、上記式（２１）に従って、カルマンゲインＫを算出する。

ステップＳ１７０において、走路パラメータ推定部１４４は、算出されたカルマンゲインＫと、予測ステップで算出された時刻ｔにおける状態ベクトルｘ_{ｔ，ｔ−１}と、上記ステップＳ１２０で算出された全ての白線点の座標値を用いて得られる観測値ベクトルｙとに基づいて、上記式（１９）に従って、時刻ｔにおける走路パラメータｘ_ｔを推定する。

ステップＳ１８０において、走路パラメータ推定部１４４は、予測ステップで推定された誤差共分散行列Ｐ_{ｔ，ｔ−１}と、算出されたカルマンゲインＫと、上記式（２２）に示す観測ヤコビアン行列Ｈとに基づいて、上記式（２０）に従って、誤差共分散行列Ｐ_ｔを算出する。

ステップＳ１９０において、走路パラメータ推定部１４４は、フィルタリングステップで算出された各値（カルマンゲインＫ、走路パラメータｘ_ｔ、誤差共分散行列Ｐ_ｔ）を走路パラメータ記憶部１４６へ渡すと共に、推定された走路パラメータを、警報装置１５０及び車両制御装置１６０へ出力して、ステップＳ１００に戻る。

＜本発明の第１の実施の形態に係る実験結果＞
次に、本実施の形態に係る手法を確認した数値実験について説明する。

＜＜数値実験の内容＞＞
数値実験は、（Ａ）４５０ｍＲの縦断曲線半径の凹型・凸型縦断線形が連続する直線路、（Ｂ）平坦路面で４６０ｍＲのＳ字カーブ路、のそれぞれについて、本実施の形態に係る手法と、従来法の動作とを比較して行った。

本数値実験は、認識距離内の時々刻々に観測される白線点座標値をシミュレーションし、カルマンフィルタで推定値を更新した。また、従来法は、平坦路面を仮定して、ピッチ角の推定値を０に固定して推定した。

観測値は、前方道路画像までの右側白線から５点を抽出した。また、オフセットは０．７５ｍ、ヨー角とピッチ角は０ｒａｄに設定した。

＜＜数値実験の結果＞＞
図６は、数値実験（Ａ）のシミュレーション結果を、図７は、数値実験（Ｂ）のシミュレーション結果を示した図である。

図６（ａ）及び図７（ａ）が、曲率についての従来法の推定値（破線）と、本実施の形態に係る手法の推定値（実線）と、真値（点線）とをプロットしたものである。図６（ｂ）及び図７（ｂ）が、縦断曲率についての従来法の推定値（破線）と、本実施の形態に係る手法の推定値（実線）と、真値（点線）とをプロットしたものである。図６（ｃ）及び図７（ｃ）が、オフセットについての従来法の推定値（破線）と、本実施の形態に係る手法の推定値（実線）と、真値（点線）とをプロットしたものである。図６（ｄ）及び図７（ｄ）が、ヨー角についての従来法の推定値（破線）と、本実施の形態に係る手法の推定値（実線）と、真値（点線）とをプロットしたものである。

図６より、路面勾配変化がある場合、本実施の形態に係る手法により縦断曲率と白線点までの距離が推定されて、従来法に比べて推定パラメータの誤差が低減されていることが分かる。

また、図７より、平坦路面のカーブであっても本実施の形態に係る手法には副作用は無く、推定ができていることが確認された。

以上説明したように、本発明の実施の形態に係る走路推定装置によれば、走路の画像から、線状に並んだ複数の境界線候補点を抽出し、複数の境界線候補点を観測値として、カルマンフィルタにより、境界線の平面線形に関するパラメータと境界線の断面線形に関するパラメータと少なくとも１つの境界線候補点の各々までの実空間上の距離とを含む状態ベクトルを推定することにより、走路に勾配変化がある場合であっても、精度良く走路の境界線を推定することができる。

＜本発明の第２の実施の形態に係る走路推定装置１００の構成＞
次に、本発明の第２の実施の形態に係る走路推定装置１００の構成について説明する。単独白線の白線幅が画像上で観測できる場合には、白線幅は距離によって画像上の幅が変化して見えるため、距離推定の補助的な情報として有用である。そこで、本実施形態では、状態ベクトルに白線幅を更に含んで走路パラメータを推定する。以下、本実施形態において、上記の実施形態と異なる構成に係る部分について説明する。

本実施形態に係る境界線候補点抽出部１４２は、撮像装置１１０によって撮像された道路画像に基づいて、自車両が走行する走行車線の境界として、２本の線状に並んだ複数の白線点を抽出し、白線幅を抽出する。

具体的には、本実施形態に係る境界線候補点抽出部１４２は、２本のうちの一方として線状に並んだ複数の白線点の位置の座標値と、２本のうち一方として線状に並んだ複数の白線点の位置の座標値とから、白線の幅を算出する。例えば、２本のうち一方として線状に並んだ複数の白線点と、２本のうちの他方として線状に並んだ複数の白線点との間で、距離が近い白線点の組合せを抽出し、当該組合せから、当該白線点での画像上の白線幅を算出する。

そして、本実施形態に係る境界線候補点抽出部１４２は、２本のうち一方として線状に並んだ複数の白線点の位置の座標値と、複数の白線点の各々について算出した画像上の白線幅とを、走路パラメータ推定部１４４に渡す。

本実施形態に係る走路パラメータ推定部１４４は、白線幅を更に追加した状態ベクトルを推定する。

本実施形態における道路モデルについて説明する。

道路上の白線の平面線形と縦断線形は、カメラ位置を原点として三次元直交座標系ＸＹＺ上の曲線として表現する（図８）。

ここで、図８に示すｊ番目の白線点に対応する画面上の白線幅ｉｗ_ｊは、下記式（２６）で表すことができる。

ここで、ｗ_ｌｍは走行平面上の白線幅［ｍ］、ｉｗ_ｊは画像上のｊ番目の白線幅（観測値）［画素］、αｊは下記式（２７）で表せられるｊ番目の白線点における接線の傾きである。

白線幅パラメータｗ_ｌｍを含む状態ベクトルｘは、上記式（１３）を変形した下記式（２８）となる。

このとき、離散時間更新モデルの行列ＢとＦとは、下記式（２９）及び式（３０）となる。

また、ｊ番目の白線点に対して、下記式（３１）の観測関数ｈ_ｊ（ｘ）が得られる。

また、観測値ベクトルｙは、境界線候補点抽出部１４２で抽出された、２本のうちの一方として線状に並んだｍ個の白線点の座標値（ｉｘ_ｊ，ｉｙ_ｊ）と対応する白線幅ｉｗ_ｊを順に並べたベクトルであり、下記式（３２）で表される。

また、ｊ番目の白線点に対する観測ヤコビアン行列Ｈ_ｊは、下記式（３３）のように観測関数ｈ_ｊ（ｘ）を、状態ベクトルｘで偏微分した形となる。

そして、本実施形態に係る走路パラメータ推定部１４４は、以上説明した走路モデルにより、抽出した複数の白線点の座標である観測値ベクトルｙに基づいて、走路パラメータを含む状態ベクトルｘを推定する。

以上説明したように、本実施の形態における実施形態に係る走路推定装置によれば、境界線幅を追加した状態ベクトルを推定することにより、直線路とカーブ路の間に緩和曲線が無い場合のように、急峻に曲率が変化するカーブ路において、応答遅れやオーバーシュートを低減することができる。

＜本発明の第３の実施の形態に係る走路推定装置１００の構成＞
次に、本発明の第３の実施の形態に係る走路推定装置１００の構成について説明する。第２の実施形態においても、第１の実施形態と同様に、走路パラメータ推定部１４４は、状態ベクトルｘについて、曲率変化率ｃ_ｖ１を含まないものとして、計算量を削減することができる。さらに、走路パラメータ推定部１４４は、白線点までの実空間上のパラメータを最遠点の白線点に限定する構成として、計算量を削減することができる。

そこで、本実施形態では、第２の実施形態において、曲率変化率ｃ_ｖ１を含まず、かつ、白線点までの実空間上のパラメータを最遠点の白線点に限定する場合について説明する。なお、本実施形態において、第２の実施形態と異なる構成に係る部分について説明する。

走路パラメータ推定部１４４は、状態ベクトルｘを、下記式（３４）で表す。

このとき、縦断線形は、下記式（３５）に示す２次式で表現することができる。

また、本実施形態では、観測ベクトルｙを、下記式（３６）に示す白線点のｉｘ座標を５個、ｉｙ座標を１個、及び白線幅ｉｗを２個を順に並べたベクトルを用いることとする。

このとき、観測行列Ｈは、下記式（３７）のように示すことができる。

なお、推定パラメータから外したｚ０〜ｚ３は、観測した白線点のｉｙ座標から、以下の二次方程式の解として求めて、観測行列計算で使用する。

＜本発明の第３の実施の形態に係る実験結果＞
本実施形態に係るシミュレーションを行うことにより確認したところ、状態ベクトルｘの次元数を上記式（３４）のように少なくとも８とし、観測ベクトルｙの次元数を上記式（３６）のように少なくとも８とすることにより、第１の実施の形態に係る実験結果の性能を極端に劣化させることなく、かつ、計算量を低減させることができた。

以上説明したように、本実施の形態における実施形態に係る走路推定装置によれば、走路に勾配変化がある場合であっても、精度良く、かつ、高速に走路の境界線を推定することができる。

なお、本発明は、上述した実施の形態に限定されるものではなく、この発明の要旨を逸脱しない範囲内で様々な変形や応用が可能である。

例えば、上記の実施の形態では、移動体として車両を対象とする場合を例に説明したが、これに限定されるものではなく、他の移動体を対象としてもよい。

また、走路の境界線が白線である場合を例に説明したが、これに限定されるものではなく、橙線などの他の色で引かれた境界線であっても良い。

また、走路の境界線としての白線が片側にのみ１つ存在する場合を例に説明したが、これに限定されるものではなく、走路の境界線としての白線が両側にある場合であってもよい。この場合、白線毎に、カルマンフィルタによる推定を行って、走路パラメータを推定すればよい。

また、走路の境界線が曲線がある場合について説明したが、走路の境界線が直線である場合でも、本発明を適用することが可能である。この場合、走路パラメータ推定部１４４は、直線に近似できる近傍の境界線から、走路の境界線を推定することができる。

また、本願明細書中において、プログラムが予めインストールされている実施形態として説明したが、当該プログラムを、コンピュータ読み取り可能な記録媒体に格納して提供することも可能である。

１００ 走路推定装置
１１０ 撮像装置
１２０ ヨーレイトセンサ
１３０ 車速センサ
１４０ コンピュータ
１４２ 境界線候補点抽出部
１４４ 走路パラメータ推定部
１４６ 走路パラメータ記憶部
１５０ 警報装置
１６０ 車両制御装置

Claims (6)

1. 移動体に搭戦された撮像装置によって撮像された前記移動体の走行する走路の画像から、線状に並んだ複数の境界線候補点を抽出する境界線候補点抽出部と、
   前記複数の境界線候補点を観測値として、少なくとも１つの境界線候補点の各々までの実空間上の三次元距離と、境界線の線形に関するパラメータと、前記境界線に対する前記移動体の横位置を示すオフセット、及び姿勢角とを含む状態ベクトルを前記境界線候補点と前記状態ベクトルから画像上へ投影した予測点との間の誤差を最小化して推定する走路パラメータ推定部と、
   を含む走路推定装置。
2. 前記境界線の線形に関するパラメータは、水平面における前記境界線の曲率及び曲率変化率と、垂直断面における前記境界線の曲率とである請求項１記載の走路推定装置。
3. 前記境界線の線形に関するパラメータは、水平面における前記境界線の曲率及び曲率変化率と、及び垂直断面における前記境界線の曲率及び曲率変化率とである請求項１記載の走路推定装置。
4. 前記境界線候補点抽出部は、前記複数の境界線候補点から境界線幅を算出し、
   前記状態ベクトルは、前記境界線幅を更に含む
   請求項１乃至３の何れか１項記載の走路推定装置。
5. 前記状態ベクトルは、前記複数の境界線候補点の各々までの実空間上の距離のうち、最遠点である前記境界線候補点の実空間上の距離と、前記境界線の線形に関するパラメータと、前記境界線に対する前記移動体のオフセット及び姿勢角とを含む請求項１乃至４の何れか１項記載の走路推定装置。
6. コンピュータを、
   移動体に搭戦された撮像装置によって撮像された前記移動体の走行する走路の画像から、線状に並んだ複数の境界線候補点を抽出する境界線候補点抽出部、及び
   前記複数の境界線候補点を観測値として、少なくとも１つの境界線候補点の各々までの実空間上の三次元距離と、境界線の線形に関するパラメータと、前記境界線に対する前記移動体の横位置を示すオフセット、及び姿勢角とを含む状態ベクトルを前記境界線候補点と前記状態ベクトルから画像上へ投影した予測点との間の誤差を最小化して推定する走路パラメータ推定部
   として機能させるためのプログラム。