JP6488226B2 - 走路パラメータ推定装置及びプログラム - Google Patents

Description

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

従来より、車両が走行車線を変更したとき、前回算出した横変位に車線幅を加算又は減算することによって、横変位を補正する技術が知られている（例えば、特許文献１）。

上記の特許文献１に記載の技術では、自車から見た車線中心の横変位（横位置）に着目し、横変位の絶対値が設定閾値以上であれば車線変更と判定する。そして、車線変更と判定した場合は、以下のように横位置を補正する。

横位置＝横位置＋車線幅；右車線変更の場合
横位置＝横位置−車線幅；左車線変更の場合

特開２０１５−１４０１１４号公報

車両が追跡中の白線は、車線変更の前後で、右白線だったものは左白線に、左白線だったものは右白線に切り替わる。

上記特許文献１に記載の技術のように、車両の車線変更時に横位置を補正しただけでは、ノイズに対するロバスト性が劣化して、推定パラメータの大きな変動が生じる場合がある。特に、左右白線のどちらかが検出できずに、片側単独白線を追跡している状態で車線変更した場合に影響が大きい。

本発明は、上記の事情に鑑みてなされたもので、移動体の車線変更が行われた場合でも、走路パラメータを精度良く推定することができる走路パラメータ推定装置及びプログラムを提供することを目的とする。

上記の目的を達成するために本発明に係る走路パラメータ推定装置は、移動体が走行する走路の画像に基づいて、前記移動体が走行する走行車線の境界の位置を検出し、検出された前記走行車線の境界の位置に基づいて、予め定められた走路モデルに関する走路パラメータであって、かつ前記移動体に対する車線の横位置を含む走路パラメータを、拡張カルマンフィルタを用いて推定する推定手段と、前記移動体の走行状態が車線変更である場合に、前記走路パラメータの前記車線の横位置と、前記拡張カルマンフィルタにおける共分散行列とを補正する補正手段と、を含んで構成されている。

本発明に係るプログラムは、コンピュータに、移動体が走行する走路の画像に基づいて、前記移動体が走行する走行車線の境界の位置を検出し、検出された前記走行車線の境界の位置に基づいて、予め定められた走路モデルに関する走路パラメータであって、かつ前記移動体に対する車線の横位置を含む走路パラメータを、拡張カルマンフィルタを用いて推定する推定手段、及び前記移動体の走行状態が車線変更である場合に、前記走路パラメータの前記車線の横位置と、前記拡張カルマンフィルタにおける共分散行列とを補正する補正手段として機能させるためのプログラムである。

本発明によれば、推定手段によって、移動体が走行する走路の画像に基づいて、移動体が走行する走行車線の境界の位置を検出し、検出された走行車線の境界の位置に基づいて、予め定められた走路モデルに関する走路パラメータであって、かつ移動体に対する車線の横位置を含む走路パラメータを、拡張カルマンフィルタを用いて推定する。

そして、補正手段によって、移動体の走行状態が車線変更である場合に、走路パラメータの車線の横位置と、拡張カルマンフィルタにおける共分散行列とを補正する。

このように、移動体の走行状態が車線変更である場合に、走路パラメータの車線の横位置と、拡張カルマンフィルタにおける共分散行列とを補正することにより、移動体の車線変更が行われた場合でも、走路パラメータを精度良く推定することができる。

また、本発明は、前記走路パラメータは車線幅に関するパラメータを含み、前記補正手段は、前記移動体の走行状態が車線変更である場合に、前記拡張カルマンフィルタにおける共分散行列の部分行列であって、前記車線幅に関するパラメータと、前記車線幅に関するパラメータと異なる他の走路パラメータとの相関を表す部分行列の符号を反転するように、前記拡張カルマンフィルタにおける共分散行列を補正するようにすることができる。

また、本発明は、前記推定手段によって検出された前記走行車線の境界の位置に基づいて、前記移動体に対する車線中心の横位置の絶対値が、車線幅の１／２以上の位置である場合に、前記移動体の走行状態が車線変更であると判定する判定手段を更に含み、前記補正手段は、前記判定手段によって前記移動体の走行状態が車線変更と判定された場合に、前記走路パラメータの前記車線の横位置と、前記拡張カルマンフィルタにおける共分散行列とを補正するようにすることができる。

また、本発明の前記補正手段は、前記移動体の走行状態が右側車線への車線変更である場合に、前記車線の横位置に車線幅を加算して補正し、前記移動体の走行状態が左側車線への車線変更と判定された場合に、前記車線の横位置から前記車線幅を減算して補正するようにすることができる。

また、本発明のプログラムは、記憶媒体に格納して提供することも可能である。

以上説明したように、本発明の走路パラメータ推定装置及びプログラムによれば、移動体の走行状態が車線変更である場合に、走路パラメータの車線の横位置と、拡張カルマンフィルタにおける共分散行列とを補正することにより、移動体の車線変更が行われた場合でも、走路パラメータを精度良く推定することができる、という効果が得られる。

本発明の第１の実施の形態に係る走路パラメータ推定装置を示すブロック図である。 本発明の第１の実施の形態に係る走路モデルを説明するための図である。 車線変更の判定処理を説明するための図である。 本発明の第１の実施の形態に係る走路パラメータ推定装置における走路認識処理ルーチンの内容を示すフローチャートである。 本発明の第１の実施の形態に係る走路パラメータ推定装置における走路パラメータ推定処理ルーチンの内容を示すフローチャートである。 従来手法を説明するための図である。 本実施の形態に係る提案手法を説明するための図である。 本発明の第２の実施の形態に係る走路モデルを説明するための図である。 従来手法と本実施の形態に係る手法との比較を説明するための図である。

以下、図面を参照して、本発明の実施の形態について説明する。本発明の実施の形態では、走路パラメータを推定する走路パラメータ推定装置に本発明を適用した場合を例に説明する。

［第１の実施の形態］
＜走路パラメータ推定装置のシステム構成＞
図１に示すように、第１の実施の形態に係る走路パラメータ推定装置１０は、自車両の前方を撮像する撮像装置１２と、撮像装置１２によって撮像された自車両の前方の道路画像に基づいて、走路パラメータを推定するコンピュータ１４とを備えている。

撮像装置１２は、自車両の前方の道路を撮像する車載カメラである。撮像装置１２は、例えば車両のリアビューミラー付近に設置され、自車両が走行する走路の画像の一例として、自車両の前方の道路画像を撮像する。なお、以下では説明の簡易化のため、撮像装置１２は車両の中心に設置され、かつ車両の進行方向（ｚ軸方向向き）へ向けて設置され、かつ路面に対して水平に設置されているものとする。

コンピュータ１４は、ＣＰＵと、ＲＡＭと、後述する走路パラメータ推定処理ルーチンを実行するためのプログラムを記憶したＲＯＭとを備え、機能的には次に示すように構成されている。コンピュータ１４は、撮像装置１２によって撮像された道路画像に基づいて、自車両が走行する走路に関する走路パラメータを推定する走路認識部１６と、自車両の走行状態が車線変更であるか否かを判定する車線変更判定部１８と、自車両に対する車線の中心の横位置を補正する横位置補正部２０と、拡張カルマンフィルタにおける共分散行列を補正する共分散行列補正部２２と、走路パラメータを出力する出力部２４とを備えている。なお、走路認識部１６は、推定手段の一例であり、車線変更判定部１８は、判定手段の一例であり、横位置補正部２０及び共分散行列補正部２２は、補正手段の一例である。

走路認識部１６は、撮像装置１２によって撮像された道路画像に基づいて、自車両が走行する走行車線の境界の位置を検出する。そして、走路認識部１６は、検出された走行車線の境界の位置に基づいて、予め定められた走路モデルに関する走路パラメータを、拡張カルマンフィルタを用いて推定する。

具体的には、まず走路認識部１６は、前処理として、道路画像から走行車線の境界の候補点の位置の座標値を抽出する。前処理では、自車両が走行中の車線の左右の境界を抽出する。

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

さらに、白線レーンマークは、局所的には進行方向に直線状に連続しているため、Ｈｏｕｇｈ変換等の手法により、直線上に連続している箇所を、車線の境界の候補として絞り込んでもよい。また、白線レーンマークの幅は局所的には一定なので、白線レーンマークの左右の車線の境界の候補対を照合して、車線の境界の候補点として選別してもよい。

次に、走路認識部１６は、後処理として、走行車線の境界の候補点の座標値に対して予め設定された走路モデルを当てはめ、拡張カルマンフィルタを用いて走路パラメータを推定する。

第１の実施の形態では、走路モデルとして平行直線モデルを用いる場合について説明する。図２に平行直線モデルを説明するための図を示す。

図２に示すように、平行直線モデルは、走路が直線であることを想定し、走路の曲率・曲率変化率を無視したモデルである。平行直線モデルでは、道路画像上の車線の境界の位置を記述する観測モデルと、車両の時間更新を記述する時間更新モデルとが設定される。

以下では、まず、観測モデルについて説明する。観測モデルでは、走路は平面上にあると仮定して、上記図２に示すように、自車両を基準としたｘ−ｚ車両座標系を設定する。さらに、走路境界を規定する白線は平行直線で表現されると仮定する。この場合、走行平面上の走路境界である白線の位置ｘは、以下の式（１．１）で表現することができる。なお、以下では、自車両が走行している走路を走行車線とし、車線の境界を白線として説明する。

上記式（１．１）における、ｘ_０は自車両が走行している走行車線の中心の横位置［ｍ］を表し、θはヨー角（車線の傾き）［ｒａｄ］を表し、ｗは車線幅［ｍ］を表し、ｓは観測点が右白線の場合＋１、左白線の場合−１となる変数を表す。ｘ−ｚ車両座標上の白線は、道路画像上に設定されたＩ−Ｊ座標系へ透視変換することにより、撮像装置１２に撮像されて観測される白線点の座標値（Ｉ_ｉ，Ｊ_ｉ）として得られる。

なお、上記透視変換は、以下の式（１．２）及び（１．３）によって表される。

上記式（１．２）及び（１．３）において、Ｉ_ｉは画像上のｉ番目の白線点の垂直方向の座標値［画素］を表し、Ｊ_ｉは道路画像上のｉ番目の白線点の水平方向の座標値［画素］を表し、φはピッチ角［ｒａｄ］（ここでは変数ではなく、固定値と見なす）を表し、Ｈ_ｃは路面平面からの撮像装置の高さ［ｍ］を表し、ｆは焦点距離［ｍ］を表し、Ｉ_ｃは画像中心の垂直方向の座標値［画素］を表し、Ｊ_ｃは画像中心の水平方向の座標値［画素］を表す。また、Ｒ_ｖは道路画像の垂直方向分解能を表し、Ｒ_ｈは道路画像の水平方向分解能を表す。

上記式（１．１）及び上記式（１．３）より、以下の式（１．４）が得られる。

上記式（１．４）中のｚは、上記式（１．２）を変形した以下の式（１．５）より白線点毎に求められる。

次に、時間更新モデルについて説明する。時刻ｔの走路パラメータＸ^ｔを、以下の式（１．６）で定義する。以下の式（１．６）に示すように、走路パラメータは、自車両に対する走行車線の中心の横位置ｘ_０が含まれている。

時刻ｔ−１の走路パラメータＸ^ｔ−１から時刻ｔの走路パラメータＸ^ｔの時間更新は、以下の式（１．７）で表現される。

このとき、上記式（１．７）における更新行列Ｆとシステムに付加される入力ｕとは、以下の式（１．８）及び（１．９）で表現される。また、上記式（１．７）におけるｖはシステムノイズ項を表す。

上記式（１．８）及び（１．９）において、Δｚは車速から算出した更新周期時間Δｔ内の自車両の移動距離［ｍ］を表し、Δｔは更新周期時間［ｓ］を表し、Ｙ_ｒはヨーレイト［ｒａｄ/ｓ］を表す。

また、観測行列Ｈは以下の式（１．１０）で表現される。

また、上記式（１．４）を走路パラメータの各要素で偏微分することにより、以下の式（１．１１）〜式（１．１４）が得られる。

以上をまとめると、以下の式（１．１５）が得られる。

上記式（１．６）に示す走路パラメータＸと、上記式（１．４）と上記式（１．５）から予測される道路上の左右の白線位置と観測値との偏差の二乗和をコストとして、当該コストを最小化する最適化計算をすれば、走路パラメータＸの最適値が得られる。この最適化計算には、観測データの一時的部分的な欠陥にロバストな拡張カルマンフィルタを利用することができる。拡張カルマンフィルタにより、画像上の観測値に付加される観測ノイズや自車両運動によって発生するシステムノイズを考慮した走路パラメータＸの最適な推定値が得られる。拡張カルマンフィルタの計算方法は既知であり、例えば以下の参考文献等に記載されている。

参考文献：片山徹，“応用カルマンフィルタ”，朝倉書店，１９８３

拡張カルマンフィルタにより、走路パラメータＸを推定されると共に、その共分散行列Ｐも更新される。拡張カルマンフィルタの適用には、ダイナミクスモデルを状態遷移行列として設定する必要があるが、変動が緩やかな場合は単位行列を設定すればよい。

また、車両運動に関する知見が得られた場合、その知見をダイナミクスモデルに組み込むことにより推定性能を改善することができる。また、本実施の形態では、左右白線がｘ−ｚ車両座標系で平行な直線を想定したが、道路線形の曲率や曲率変化率を推定ベクトルとして考慮すれば、提案手法は容易に適用することができる。

次に、拡張カルマンフィルタへの適用について説明する。

以上で説明した観測モデルと時間更新モデルとを、以下に示す拡張カルマンフィルタの更新式（１．１６）〜（１．２３）に適用することにより、ｍ個の観測値から時々刻々の走路パラメータＸ＾^ｔと、時々刻々の共分散行列Ｐ^ｔが最新値に更新される。

上記式（１．１６）〜（１．２３）において、Ｋはカルマンゲイン、Ｈは観測行列、Ｊ^ｔ _{ｏｂｓ，ｉ}は時刻ｔの画像中のｉ番目の白線点の観測値である。また、Ｊ^ｔ _ｉは時刻ｔの画像中のｉ番目の白線点の予測値ベクトルであり上記式（１．４）と式（１．５）とＸ￣^ｔとＩ_ｉとから算出される。

また、上記式（１．１８）に含まれるＲは観測ノイズの分散行列で、以下の式（１．２４）に示すように画像上の白線点位置の観測誤差の標準偏差二乗σ^２ _ｏｂｓを要素とした対角行列である。

また、上記式（１．１７）に含まれるＱは、以下の式（１．２５）で示されるシステムノイズ分散行列である。

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

（１）予測ステップ
まず、予測ステップにおける処理について説明する。走路認識部１６は、予測ステップにおいて、前回のステップで算出された時刻ｔ−１の走路パラメータＸ^ｔ−１と、上記式（１．８）に示す更新行列Ｆ＾とに基づいて、上記式（１．１６）に従って、時刻ｔの走路パラメータＸ￣^ｔを算出する。

そして、走路認識部１６は、上記式（１．８）に示す更新行列Ｆ＾と、前回のステップで推定された共分散行列Ｐ^ｔ−１と、システムノイズ分散行列Ｑとに基づいて、上記式（１．１７）に従って、共分散行列Ｐ￣^ｔを算出する。

（２）フィルタリングステップ
次に、フィルタリングステップにおける処理について説明する。走路認識部１６は、フィルタリングステップにおいて、予測ステップで算出された共分散行列Ｐ￣^ｔと、観測行列Ｈと、観測ノイズの分散行列Ｒとに基づいて、上記式（１．１８）に従って、カルマンゲインＫを算出する。

次に、走路認識部１６は、算出されたカルマンゲインＫと、予測ステップで算出された時刻ｔにおける状態ベクトルＸ￣^ｔと、上記式（１．２１）で示す観測値ｈ_ｏｂｓと、上記式（１．２２）で示す予測値ベクトルと、上記式（１．２３）に示す観測行列Ｈとに基づいて、上記式（１．１９）に従って、時刻ｔにおける走路パラメータＸ＾^ｔを推定する。

そして、走路認識部１６は、予測ステップで推定された共分散行列Ｐ￣^ｔと、算出されたカルマンゲインＫと、上記式（１．２３）に示す観測行列Ｈとに基づいて、上記式（１．２０）に従って、共分散行列Ｐ^ｔを算出する。

フィルタリングステップで算出された各値（カルマンゲインＫ、走路パラメータＸ＾^ｔ、共分散行列Ｐ^ｔ）は、原則として次回の予測ステップにおける処理で用いられる。しかし、本実施の形態では、後述する車線変更判定部１８による判定処理に応じて、走路パラメータＸ＾^ｔのうち自車両に対する走行車線の中心の横位置ｘ_０と共分散行列Ｐ^ｔとが補正される。

車線変更判定部１８は、走路認識部１６によって検出された走行車線の境界の位置に基づいて、自車両の走行状態が車線変更であるか否かを判定する。具体的には、車線変更判定部１８は、走路認識部１６によって検出された走行車線の境界の位置に基づいて、自車両に対する車線中心の横位置の絶対値が、車線幅の１／２以上の位置である場合に、自車両の走行状態が車線変更であると判定する。

具体的には、撮像装置１２によって撮像された道路画像の観測フレームの拡張カルマンフィルタによる走路パラメータの更新が終了した時点で、車線変更の判定と車線の変更時の各処理とが実行される。車線変更の判定式は以下の式（１．２６）で表現される。

なお、上記式（１．２６）のαは車線の境界付近で車線変更の判定の頻発を防ぐためのヒステリシス項である。

横位置補正部２０は、車線変更判定部１８によって自車両の走行状態が右側車線への変更と判定された場合に、車線の横位置に車線幅を加算して補正する。また、横位置補正部２０は、車線変更判定部１８によって自車両の走行状態が左側車線への変更と判定された場合に、車線の横位置から車線幅を減算して補正する。

図３に、車両の車線変更の判定処理の一例を示す。図３では、自車両が車線１を走行している状態から、白線を跨ぐ状態に移り、最終的に車線２を走行する状態に移行する。車線の変更の前後で、観測している左右の白線はａ，ｂからｂ，ｃに切り替わるため、自車両が走行している走行車線の中心の横位置は、以下の式（１．２７）のように修正する必要があることが分かる。

なお、上記式（１．２７）におけるプラスマイナスの符号は、右側車線へ変更する場合が正で、左側車線へ変更する場合は負である。

共分散行列補正部２２は、車線変更判定部１８によって自車両の走行状態が車線変更であると判定された場合に、車線幅に関するパラメータと車線幅に関するパラメータと異なる他の走路パラメータとの相関を表す部分行列の符号を反転するように、拡張カルマンフィルタにおける共分散行列を補正する。

具体的には、共分散行列補正部２２は、車線変更が行われた場合に、以下の式（１．２８）で表される拡張カルマンフィルタの共分散行列Ｐを、以下の式（１．２９）に従って、車線変更後の共分散行列Ｐ_ｌｃへ補正する。

上記式（１．２８）及び（１．２９）に基づく補正処理は、車線変更の左右の方向にかかわらず処理は同一である。また、上記式（１．２８）及び（１．２９）において、Ｐ_０は車線幅ｗを除いた共分散行列Ｐの部分行列、Ｐ_ｗは車線幅ｗに対応する共分散行列Ｐの部分行列、Ｐ_１は共分散行列Ｐの車線幅ｗに対応する列から対角項を除いた部分行列である。また、Ｐ_１は車線幅に関するパラメータと車線幅に関するパラメータと異なる他の走路パラメータとの相関を表す部分行列である。なお、推定する走路パラメータの個数をｎ個とするとＰ_０のサイズは（ｎ−１）×（ｎ−１）、Ｐ_１のサイズは（ｎ−１）×１、Ｐ_ｗのサイズは１×１である。

出力部２４は、車線変更判定部１８によって自車両の走行状態が車線変更であると判定された場合には、横位置補正部２０によって補正された走行車線の中心の横位置と、走路認識部１６によって推定された走行車線の中心の横位置以外の走路パラメータと、共分散行列補正部２２によって補正された共分散行列とを結果として出力し、メモリ（図示省略）に格納する。一方、出力部２４は、車線変更判定部１８によって自車両の走行状態が車線変更でないと判定された場合には、走路認識部１６によって推定された走路パラメータと共分散行列とを結果として出力し、メモリ（図示省略）に格納する。

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

ステップＳ１００において、走路認識部１６は、撮像装置１２によって撮像された道路画像を取得する。

ステップＳ１０２において、走路認識部１６は、前処理として、上記ステップＳ１００で受け付けた道路画像から、走行車線の境界の候補点の位置の座標値を抽出する。

ステップＳ１０４において、走路認識部１６は、後処理として、上記ステップＳ１０２で抽出された走行車線の境界の候補点の座標値に対して走路モデルを当てはめ、拡張カルマンフィルタを用いて走路パラメータを推定する。ステップＳ１０４は、図５に示す走路パラメータ推定処理ルーチンによって実現される。

＜走路パラメータ推定処理ルーチン＞
ステップＳ２００において、走路認識部１６は、予測ステップにおいて、前回のステップＳ１１２でメモリ（図示省略）に格納された走路パラメータＸ^ｔ−１と、上記式（１．８）に示す更新行列Ｆ＾とに基づいて、上記式（１．１６）に従って、時刻ｔの走路パラメータＸ￣^ｔを算出する。

ステップＳ２０２において、走路認識部１６は、上記式（１．８）に示す更新行列Ｆ＾と、前回のステップＳ１１２でメモリ（図示省略）に格納された共分散行列Ｐ^ｔ−１と、システムノイズ分散行列Ｑとに基づいて、上記式（１．１７）に従って、共分散行列Ｐ￣^ｔを算出する。

ステップＳ２０４において、走路認識部１６は、フィルタリングステップにおいて、上記ステップＳ２０２で算出された共分散行列Ｐ￣^ｔと、観測行列Ｈと、観測ノイズの分散行列Ｒとに基づいて、上記式（１．１８）に従って、カルマンゲインＫを算出する。

ステップＳ２０６において、走路認識部１６は、上記ステップＳ２０４で算出されたカルマンゲインＫと、予測ステップで算出された時刻ｔにおける状態ベクトルＸ￣^ｔと、上記式（１．２１）で示す観測値ｈ_ｏｂｓと、上記式（１．２２）で示す予測値ベクトルと、上記式（１．２３）に示す観測行列Ｈとに基づいて、上記式（１．１９）に従って、時刻ｔにおける走路パラメータＸ＾^ｔを推定する。

ステップＳ２０８において、走路認識部１６は、上記ステップＳ２０２で算出された共分散行列Ｐ￣^ｔと、上記ステップＳ２０４で算出されたカルマンゲインＫと、上記式（１．２３）に示す観測行列Ｈとに基づいて、上記式（１．２０）に従って、共分散行列Ｐ^ｔを算出する。

ステップＳ２１０において、走路認識部１６は、上記ステップＳ２０６で算出された走路パラメータＸ＾^ｔと、上記ステップＳ２０８で算出された共分散行列Ｐ^ｔを結果として出力する。

次に、走路認識処理ルーチンに戻り、ステップＳ１０６において、車線変更判定部１８は、上記ステップＳ１０２で検出された走行車線の境界の位置に基づいて、自車両の走行状態が車線変更であるか否かを判定する。自車両に対する車線中心の横位置の絶対値が、車線幅の１／２以上の位置である場合に、自車両の走行状態が車線変更であると判定し、ステップＳ１０８へ進む。一方、自車両に対する車線中心の横位置の絶対値が、走路幅の１／２未満の位置である場合に、自車両の走行状態は車線変更でないと判定し、ステップＳ１１２へ進む。

ステップＳ１０８において、横位置補正部２０は、自車両の走行状態が右側車線への変更と判定された場合に、上記ステップＳ１０４で得られた走路パラメータの車線の横位置ｘ_０に車線幅ｗを加算して補正する。また、横位置補正部２０は、自車両の走行状態が左側車線への変更と判定された場合に、上記ステップＳ１０４で得られた走路パラメータの車線の横位置ｘ_０から車線幅ｗを減算して補正する。

ステップＳ１１０において、共分散行列補正部２２は、上記式（１．２９）に示すように、車線幅に関するパラメータｗと車線幅に関するパラメータｗと異なる他の走路パラメータとの相関を表す部分行列の符号を反転するように、上記ステップＳ１０４得られた共分散行列Ｐ^ｔを補正する。

ステップＳ１１２において、出力部２４は、走路パラメータと共分散行列Ｐ^ｔとを結果として出力し、メモリ（図示省略）に格納する。

そして、ステップＳ１１４において、時刻ｔを１インクリメントして、ステップＳ１００へ戻る。

なお、補正された走路パラメータの車線の横位置ｘ_０と共分散行列Ｐ^ｔとは、次回の走路パラメータの推定で用いられる。

以上説明したように、第１の実施の形態に係る走路パラメータ推定装置によれば、自車両の走行状態が車線変更と判定された場合に、走路パラメータの車線の横位置と、拡張カルマンフィルタにおける共分散行列とを補正することにより、車両の車線変更が行われた場合でも、走路パラメータを精度良く推定することができる。

また、自車両からみて片側の車線境界しか検出されない場合であっても走路パラメータを算出することができる。図６に、従来手法において片側の車線境界しか検出されない場合の例を示し、図７に、本実施の形態による提案法において片側の車線境界しか検出されない場合の例を示す。

図６に示すように、片側白線のみを観測する状態が継続した場合、従来法では車線変更の直前（Ａ）と直後（Ｂ）とで白線に投影された共分散が不連続に増大するため観測ノイズの影響を受けやすくなる。一方、図７に示す本実施の形態による提案法では、直前（Ａ）と直後（Ｂ）とで白線に投影された共分散が連続して変化しないので、観測ノイズの影響を受けにくい。

また、第１の実施の形態に係る走路パラメータ推定装置では、車両の走路変更時に、車両に対する走路の中心の横位置の補正に合わせて、同時に、拡張カルマンフィルタにおける共分散行列を補正する。具体的には、車線変更後に追跡中の車線境界（例えば白線）は、右白線だったものは左白線に、左白線だったものは右白線に切り替わる。拡張カルマンフィルタによって更新された共分散行列には、左右の白線の情報が保存されているため、走路変更に合わせて共分散行列も補正することにより、左右の白線の情報の整合性をとることができる。
これにより、車線変更の直後のノイズに対するロバスト性を向上させることができる。また、車線変更の直後の走路パラメータの異常値の発生を低減させることができ、車線変更の直後の走路パラメータのばたつきを低減させることができる。

［第２の実施の形態］
次に、第２の実施の形態について説明する。なお、第２の実施の形態に係る走路パラメータ推定装置は、第１の実施の形態と同様の構成となるため、同一符号を付して説明を省略する。

第２の実施の形態では、走路モデルとして曲線モデルを用いる点が第１の実施の形態と異なっている。

第１の実施の形態では、最低限の走路パラメータ（車線幅と車線の横位置との２つのパラメータを含む走路パラメータ）の単純な場合で説明したが、実際のシステムでは曲率等を含む走路パラメータが推定されるため、第２の実施の形態において説明する。また、第２の実施の形態では、第１の実施の形態で説明を省略した車線変更時の共分散行列の補正式の導出証明についても説明する。

第２の実施の形態に係る走路パラメータ推定装置の走路認識部１６は、走行車線の境界の候補点の座標値に対して予め設定された走路モデルを当てはめ、拡張カルマンフィルタを用いて走路パラメータを推定する。第２の実施の形態では、走路モデルとして曲線モデルを用いるため、第２の実施の形態走路パラメータは、第１の実施の形態の走路パラメータと異なる。

図８に、第２の実施の形態で用いる曲線モデルを説明するための図を示す。図８に示す曲線モデルにおいて、拡張カルマンフィルタにより走路パラメータを推定するためには、道路画像上の白線位置を記述する観測モデルと、車両と走路曲率の時間更新を記述する時間更新モデルとが設定される。

以下では、まず観測モデルについて説明する。

上記図８に示すように、走路は平面上にあると仮定して、自車両を基準としたｘ‐ｚ車両座標系を設定する。さらに、走路境界を規定する白線は３次関数で表現されると仮定する。この場合、走行平面上の白線位置は以下の式（２．１）で表現できる。

上記式（２．１）において、ｘ_０は自車両が走行している走行車線の中心の横位置［ｍ］を表し、θはヨー角（走路の傾き）［ｒａｄ］を表し、ｃ_０は撮像装置の位置直下（（ｘ，ｚ）＝（０，０））の曲率［１／ｍ］を表し、ｃ_１は曲率変化率［１／ｍ^２］を表し、ｗは車線幅［ｍ］を表し、ｓは観測点が右白線の場合＋１、左白線の場合−１となる変数を表す。

ｘ−ｚ車両座標上の白線は、道路画像上に設定されたＩ−Ｊ座標系へ透視変換することにより、撮像装置１２によって撮像されて観測される白線点の座標値（Ｉ_ｉ，Ｊ_ｉ）として得られる。

なお、上記透視変換は、以下の式（２．２）及び（２．３）によって表される。

このとき，Ｉ_ｉは画像上のｉ番目の白線点の垂直方向の座標値［画素］を表し、Ｊ_ｉは道路画像上のｉ番目の白線点の水平方向の座標値［画素］を表し、φはピッチ角［ｒａｄ］を表し、Ｈ_ｃは路面平面からの撮像装置の高さ［ｍ］を表し、ｆは焦点距離［ｍ］を表し、Ｉ_ｃは画像中心の垂直方向の座標値［画素］を表し、Ｊ_ｃは画像中心の水平方向の座標値［画素］を表す。また、Ｒ_ｖは道路画像の垂直方向分解能を表し、Ｒ_ｈは道路画像の水平方向分解能を表す。

上記式（２．１）及び上記式（２．３）より、以下の式（２．４）が得られる。

上記式（２．４）中のｚは、上記式（２．２）を変形した以下の式（２．５）より白線点毎に求められる。

次に、時間更新モデルについて説明する。時刻ｔの走路パラメータＸ^ｔを、以下の式（２．６）で定義する。以下の式（２．６）に示すように、走路パラメータは、上記式（２．４）と上記式（２．５）の中で使用した自車両の位置姿勢と白線形状を規定したパラメータである。ここで、Ｔは転置記号を表す。

時刻ｔ−１の走路パラメータＸ^ｔ−１から時刻ｔの走路パラメータＸ^ｔの時間更新は、以下の式（２．７）で表現される。

このとき、上記式（２．７）における更新行列Ｆとシステムに付加される入力ｕとは、以下の式（２．８）及び（２．９）で表現される。また、上記式（２．７）におけるｖはシステムノイズ項を表す。

上記式（２．８）及び（２．９）において、Δｚは車速から算出した更新周期時間Δｔ内の自車両の移動距離［ｍ］を表し、Δｔは更新周期時間［ｓ］を表し、Ｙ_ｒはヨーレイト［ｒａｄ/ｓ］を表す。

拡張カルマンフィルタへの適用については、上記第１の実施の形態と同様であるため、説明を省略する。

＜共分散行列の補正式の導出＞
次に、拡張カルマンフィルタの共分散行列Ｐの補正式を導出する。

導出の前提として、推定値Ｘ＾の場合の白線ｂ上の任意の点の水平方向座標Ｊの分散を計算する。白線点の水平方向座標Ｊは式（２．４）で表現されるが、以下では式展開の簡易化のために式（３．１）のように関数ｈで表す。

ここで、線形の範囲で推定値Ｘ＾の微小変化分をΔＸ＾とすると、それによって生じるＪの微小変化分ΔＪは次式で得られる。

既知の分散の公式より、以下の式（３．３）が既知である。

ただし、Ｖａｒは分散を表し，Ａは行列で、Ｘとαとはベクトルを表す。上記式（３．３）を上記式（３．２）へ適用することにより、以下の式（３．４）が得られる。

式（３．５）のＶａｒ（ΔＸ＾）を誤差共分散行列Ｐと見なすと次式が得られる。

以上をまとめると、以下の式（３．７）が得られる。

このとき、Ｈは以下の式（３．８）に示す観測行列である。

上記式（２．４）より、画像上の白線点Ｊ座標値は、以下の式（３．９）で表される。

上記式（３．９）を走路パラメータの各要素で偏微分することにより、以下の式(３．１０)〜式(３．１６)が得られる。

上記式（３．８）、上記式（３．１０）〜式（３．１６）より以下の式（３．１７）が得られる。

特に車線変更時は、白線を跨いでいる位置にあるので、以下の式（３．１８）が成立する。

上記式（３．１７）と上記式（３．１８）より，以下の式（３．１９）が得られる。

上記式（３．１９）より、右車線変更時の観測行列Ｈ_ｒは以下の式（３．２０）で表される。

また、上記式（３．１９）より、左車線変更時の観測行列Ｈ_ｌは、以下の式（３．２１）で表される。

上記式（３．２０）と上記式（３．２１）とを比較すると、右車線変更時と左車線変更時の観測行列は、車線幅に関する項の符号が正負反転している点だけが違っていることが分かる。

上記式（３．２０）と上記式（３．２１）との中のｈ_ｗは車線幅の偏微分項で、ｈ_０は車線幅以外の項を要素としたベクトルである。

ここで、車線変更前の共分散行列をＰとして、その要素を以下の式（３．２２）で設定する。

このとき、Ｐ_０は車線幅以外に対応する部分行列、Ｐ_ｗは車線幅に対応する部分行列である。また、Ｐ_１は車線幅の非対角項の部分行列であり、車線幅に関するパラメータｗと車線幅に関するパラメータｗと異なる他の走路パラメータとの相関を表す部分行列である。また、車線変更後の共分散行列をＰ_ｌｃとして、その要素を以下の式（３．２３）で設定する。

このとき、Ｐ_ｌｃ０は車線幅以外の部分行列、Ｐ_ｌｃｗは車線幅に対応する部分行列、Ｐ_ｌｃ１は車線幅の非対角項の部分行列である。図３の右車線変更の例では、車線変更の直前・直後で、白線ｂの観測が継続している。そのため、車線変更の直前・直後で白線ｂの分散も連続している必要がある。よって、車線変更の直前・直後で注目した全ての白線点の分散は変化しないと仮定すると、以下の式（３．２４）が得られる。

上記式（３．２４）に、上記式（３．２０）〜式（３．２３）を代入すると、以下の式（３．２５）が得られる。

上記式（３．２５）を展開すると、以下の式（３．２６）が得られる。

上記式（３．２６）を整理すると、以下の式（３．２７）が得られる。

上記式（３．２７）の２番目と３番目の項はスカラーなので、以下の式（３．２８）のように整理することができる。

上記式（３．２８）が白線ｂ上の任意の一点について成立するためには、以下の式（３．２９）〜式（３．３２）が成立すればよい。

以上より、車線変更後の共分散行列Ｐ_ｌｃは次式で表される。

すなわち、車両の車線変更時、共分散行列は、走路パラメータの「車線幅」に対応する行と列の非対角項の符号を反転するように補正すればよい。以上は右車線変更で例示したが、左車線変更時も同様である。

なお、第２の実施の形態に係る走路パラメータ推定装置の他の構成及び作用については、第１の実施の形態と同様であるため、説明を省略する。

以上説明したように、第２の実施の形態に係る車両挙動予測装置によれば、走路モデルとして曲線モデルを用い、自車両の走行状態が走路の変更と判定された場合に、走路パラメータの走路の横位置と、拡張カルマンフィルタにおける共分散行列とを補正することにより、車両の車線変更が行われた場合でも、走路パラメータを精度良く推定することができる。

＜実施例＞
上記実施の形態に係る走路パラメータ推定装置による、車線変更処理の効果の例を図９に示す。片側白線観測時の車線変更直後の白線位置推定誤差σ（標準偏差）を比較したシミュレーション結果を図９に示した。図９では観測ノイズσの設定が１画素と１０画素の場合について示している。図９より、推定された白線位置σの推定誤差σが半減していることから、本実施の形態に係る提案法の効果が確認される。

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

また、撮像装置１２に代えてレーザレーダを用いることもできる。レーザレーダは対象までの距離を計測するアクティブセンサであるが反射波の受光強度により道路画像を生成することも可能である。

なお、本発明のプログラムは、記録媒体に格納して提供することができる。

１０ 走路パラメータ推定装置
１２ 撮像装置
１４ コンピュータ
１６ 走路認識部
１８ 車線変更判定部
２０ 横位置補正部
２２ 共分散行列補正部
２４ 出力部

Claims (4)

1. 移動体が走行する走路の画像に基づいて、前記移動体が走行する走行車線の境界の位置を検出し、検出された前記走行車線の境界の位置に基づいて、予め定められた走路モデルに関する走路パラメータであって、かつ車線幅に関するパラメータ及び前記移動体に対する車線の横位置を含む走路パラメータを、拡張カルマンフィルタを用いて推定する推定手段と、
   前記移動体の走行状態が車線変更である場合に、前記走路パラメータの前記車線の横位置と、前記拡張カルマンフィルタにおける共分散行列とを補正する補正手段と、
   を含み、
   前記補正手段は、前記移動体の走行状態が車線変更である場合に、前記拡張カルマンフィルタにおける共分散行列の部分行列であって、前記車線幅に関するパラメータと、前記車線幅に関するパラメータと異なる他の走路パラメータとの相関を表す部分行列の符号を反転するように、前記拡張カルマンフィルタにおける共分散行列を補正する、
   走路パラメータ推定装置。
2. 前記推定手段によって検出された前記走行車線の境界の位置に基づいて、前記移動体に対する車線中心の横位置の絶対値が、車線幅の１／２以上の位置である場合に、前記移動体の走行状態が車線変更であると判定する判定手段を更に含み、
   前記補正手段は、前記判定手段によって前記移動体の走行状態が車線変更と判定された場合に、前記走路パラメータの前記車線の横位置と、前記拡張カルマンフィルタにおける共分散行列とを補正する
   請求項１に記載の走路パラメータ推定装置。
3. 前記補正手段は、前記移動体の走行状態が右側車線への車線変更である場合に、前記車線の横位置に車線幅を加算して補正し、前記移動体の走行状態が左側車線への車線変更と判定された場合に、前記車線の横位置から前記車線幅を減算して補正する
   請求項１又は請求項２に記載の走路パラメータ推定装置。
4. コンピュータを、
   移動体が走行する走路の画像に基づいて、前記移動体が走行する走行車線の境界の位置を検出し、検出された前記走行車線の境界の位置に基づいて、予め定められた走路モデルに関する走路パラメータであって、かつ車線幅に関するパラメータ及び前記移動体に対する車線の横位置を含む走路パラメータを、拡張カルマンフィルタを用いて推定する推定手段、及び
   前記移動体の走行状態が車線変更である場合に、前記走路パラメータの前記車線の横位置と、前記拡張カルマンフィルタにおける共分散行列とを補正する補正手段
   として機能させるためのプログラムであって、
   前記補正手段は、前記移動体の走行状態が車線変更である場合に、前記拡張カルマンフィルタにおける共分散行列の部分行列であって、前記車線幅に関するパラメータと、前記車線幅に関するパラメータと異なる他の走路パラメータとの相関を表す部分行列の符号を反転するように、前記拡張カルマンフィルタにおける共分散行列を補正する、
   プログラム。