(57)【特許請求の範囲】
【請求項１】
移動体が走行する走路の画像に基づいて、前記移動体が走行する走行車線の境界の位置を検出し、検出された前記走行車線の境界の位置に基づいて、予め定められた走路モデルに関する走路パラメータであって、かつ車線幅に関するパラメータ及び前記移動体に対する車線の横位置を含む走路パラメータを、拡張カルマンフィルタを用いて推定する推定手段と、
前記移動体の走行状態が車線変更である場合に、前記走路パラメータの前記車線の横位置と、前記拡張カルマンフィルタにおける共分散行列とを補正する補正手段と、
を含み、
前記補正手段は、前記移動体の走行状態が車線変更である場合に、前記拡張カルマンフィルタにおける共分散行列の部分行列であって、前記車線幅に関するパラメータと、前記車線幅に関するパラメータと異なる他の走路パラメータとの相関を表す部分行列の符号を反転するように、前記拡張カルマンフィルタにおける共分散行列を補正する、
走路パラメータ推定装置。
【請求項２】
前記推定手段によって検出された前記走行車線の境界の位置に基づいて、前記移動体に対する車線中心の横位置の絶対値が、車線幅の１／２以上の位置である場合に、前記移動体の走行状態が車線変更であると判定する判定手段を更に含み、
前記補正手段は、前記判定手段によって前記移動体の走行状態が車線変更と判定された場合に、前記走路パラメータの前記車線の横位置と、前記拡張カルマンフィルタにおける共分散行列とを補正する
請求項１に記載の走路パラメータ推定装置。
【請求項３】
前記補正手段は、前記移動体の走行状態が右側車線への車線変更である場合に、前記車線の横位置に車線幅を加算して補正し、前記移動体の走行状態が左側車線への車線変更と判定された場合に、前記車線の横位置から前記車線幅を減算して補正する
請求項１又は請求項２に記載の走路パラメータ推定装置。
【請求項４】
コンピュータを、
移動体が走行する走路の画像に基づいて、前記移動体が走行する走行車線の境界の位置を検出し、検出された前記走行車線の境界の位置に基づいて、予め定められた走路モデルに関する走路パラメータであって、かつ車線幅に関するパラメータ及び前記移動体に対する車線の横位置を含む走路パラメータを、拡張カルマンフィルタを用いて推定する推定手段、及び
前記移動体の走行状態が車線変更である場合に、前記走路パラメータの前記車線の横位置と、前記拡張カルマンフィルタにおける共分散行列とを補正する補正手段
として機能させるためのプログラムであって、
前記補正手段は、前記移動体の走行状態が車線変更である場合に、前記拡張カルマンフィルタにおける共分散行列の部分行列であって、前記車線幅に関するパラメータと、前記車線幅に関するパラメータと異なる他の走路パラメータとの相関を表す部分行列の符号を反転するように、前記拡張カルマンフィルタにおける共分散行列を補正する、
プログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、走路パラメータ推定装置及びプログラムに関する。
【背景技術】
【０００２】
従来より、車両が走行車線を変更したとき、前回算出した横変位に車線幅を加算又は減算することによって、横変位を補正する技術が知られている（例えば、特許文献１）。
【０００３】
上記の特許文献１に記載の技術では、自車から見た車線中心の横変位（横位置）に着目し、横変位の絶対値が設定閾値以上であれば車線変更と判定する。そして、車線変更と判定した場合は、以下のように横位置を補正する。
【０００４】
横位置＝横位置＋車線幅；右車線変更の場合
横位置＝横位置−車線幅；左車線変更の場合
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特開２０１５−１４０１１４号公報
【発明の概要】
【発明が解決しようとする課題】
【０００６】
車両が追跡中の白線は、車線変更の前後で、右白線だったものは左白線に、左白線だったものは右白線に切り替わる。
【０００７】
上記特許文献１に記載の技術のように、車両の車線変更時に横位置を補正しただけでは、ノイズに対するロバスト性が劣化して、推定パラメータの大きな変動が生じる場合がある。特に、左右白線のどちらかが検出できずに、片側単独白線を追跡している状態で車線変更した場合に影響が大きい。
【０００８】
本発明は、上記の事情に鑑みてなされたもので、移動体の車線変更が行われた場合でも、走路パラメータを精度良く推定することができる走路パラメータ推定装置及びプログラムを提供することを目的とする。
【課題を解決するための手段】
【０００９】
上記の目的を達成するために本発明に係る走路パラメータ推定装置は、移動体が走行する走路の画像に基づいて、前記移動体が走行する走行車線の境界の位置を検出し、検出された前記走行車線の境界の位置に基づいて、予め定められた走路モデルに関する走路パラメータであって、かつ前記移動体に対する車線の横位置を含む走路パラメータを、拡張カルマンフィルタを用いて推定する推定手段と、前記移動体の走行状態が車線変更である場合に、前記走路パラメータの前記車線の横位置と、前記拡張カルマンフィルタにおける共分散行列とを補正する補正手段と、を含んで構成されている。
【００１０】
本発明に係るプログラムは、コンピュータに、移動体が走行する走路の画像に基づいて、前記移動体が走行する走行車線の境界の位置を検出し、検出された前記走行車線の境界の位置に基づいて、予め定められた走路モデルに関する走路パラメータであって、かつ前記移動体に対する車線の横位置を含む走路パラメータを、拡張カルマンフィルタを用いて推定する推定手段、及び前記移動体の走行状態が車線変更である場合に、前記走路パラメータの前記車線の横位置と、前記拡張カルマンフィルタにおける共分散行列とを補正する補正手段として機能させるためのプログラムである。
【００１１】
本発明によれば、推定手段によって、移動体が走行する走路の画像に基づいて、移動体が走行する走行車線の境界の位置を検出し、検出された走行車線の境界の位置に基づいて、予め定められた走路モデルに関する走路パラメータであって、かつ移動体に対する車線の横位置を含む走路パラメータを、拡張カルマンフィルタを用いて推定する。
【００１２】
そして、補正手段によって、移動体の走行状態が車線変更である場合に、走路パラメータの車線の横位置と、拡張カルマンフィルタにおける共分散行列とを補正する。
【００１３】
このように、移動体の走行状態が車線変更である場合に、走路パラメータの車線の横位置と、拡張カルマンフィルタにおける共分散行列とを補正することにより、移動体の車線変更が行われた場合でも、走路パラメータを精度良く推定することができる。
【００１４】
また、本発明は、前記走路パラメータは車線幅に関するパラメータを含み、前記補正手段は、前記移動体の走行状態が車線変更である場合に、前記拡張カルマンフィルタにおける共分散行列の部分行列であって、前記車線幅に関するパラメータと、前記車線幅に関するパラメータと異なる他の走路パラメータとの相関を表す部分行列の符号を反転するように、前記拡張カルマンフィルタにおける共分散行列を補正するようにすることができる。
【００１５】
また、本発明は、前記推定手段によって検出された前記走行車線の境界の位置に基づいて、前記移動体に対する車線中心の横位置の絶対値が、車線幅の１／２以上の位置である場合に、前記移動体の走行状態が車線変更であると判定する判定手段を更に含み、前記補正手段は、前記判定手段によって前記移動体の走行状態が車線変更と判定された場合に、前記走路パラメータの前記車線の横位置と、前記拡張カルマンフィルタにおける共分散行列とを補正するようにすることができる。
【００１６】
また、本発明の前記補正手段は、前記移動体の走行状態が右側車線への車線変更である場合に、前記車線の横位置に車線幅を加算して補正し、前記移動体の走行状態が左側車線への車線変更と判定された場合に、前記車線の横位置から前記車線幅を減算して補正するようにすることができる。
【００１７】
また、本発明のプログラムは、記憶媒体に格納して提供することも可能である。
【発明の効果】
【００１８】
以上説明したように、本発明の走路パラメータ推定装置及びプログラムによれば、移動体の走行状態が車線変更である場合に、走路パラメータの車線の横位置と、拡張カルマンフィルタにおける共分散行列とを補正することにより、移動体の車線変更が行われた場合でも、走路パラメータを精度良く推定することができる、という効果が得られる。
【図面の簡単な説明】
【００１９】
【図１】本発明の第１の実施の形態に係る走路パラメータ推定装置を示すブロック図である。
【図２】本発明の第１の実施の形態に係る走路モデルを説明するための図である。
【図３】車線変更の判定処理を説明するための図である。
【図４】本発明の第１の実施の形態に係る走路パラメータ推定装置における走路認識処理ルーチンの内容を示すフローチャートである。
【図５】本発明の第１の実施の形態に係る走路パラメータ推定装置における走路パラメータ推定処理ルーチンの内容を示すフローチャートである。
【図６】従来手法を説明するための図である。
【図７】本実施の形態に係る提案手法を説明するための図である。
【図８】本発明の第２の実施の形態に係る走路モデルを説明するための図である。
【図９】従来手法と本実施の形態に係る手法との比較を説明するための図である。
【発明を実施するための形態】
【００２０】
以下、図面を参照して、本発明の実施の形態について説明する。本発明の実施の形態では、走路パラメータを推定する走路パラメータ推定装置に本発明を適用した場合を例に説明する。
【００２１】
［第１の実施の形態］
＜走路パラメータ推定装置のシステム構成＞
図１に示すように、第１の実施の形態に係る走路パラメータ推定装置１０は、自車両の前方を撮像する撮像装置１２と、撮像装置１２によって撮像された自車両の前方の道路画像に基づいて、走路パラメータを推定するコンピュータ１４とを備えている。
【００２２】
撮像装置１２は、自車両の前方の道路を撮像する車載カメラである。撮像装置１２は、例えば車両のリアビューミラー付近に設置され、自車両が走行する走路の画像の一例として、自車両の前方の道路画像を撮像する。なお、以下では説明の簡易化のため、撮像装置１２は車両の中心に設置され、かつ車両の進行方向（ｚ軸方向向き）へ向けて設置され、かつ路面に対して水平に設置されているものとする。
【００２３】
コンピュータ１４は、ＣＰＵと、ＲＡＭと、後述する走路パラメータ推定処理ルーチンを実行するためのプログラムを記憶したＲＯＭとを備え、機能的には次に示すように構成されている。コンピュータ１４は、撮像装置１２によって撮像された道路画像に基づいて、自車両が走行する走路に関する走路パラメータを推定する走路認識部１６と、自車両の走行状態が車線変更であるか否かを判定する車線変更判定部１８と、自車両に対する車線の中心の横位置を補正する横位置補正部２０と、拡張カルマンフィルタにおける共分散行列を補正する共分散行列補正部２２と、走路パラメータを出力する出力部２４とを備えている。なお、走路認識部１６は、推定手段の一例であり、車線変更判定部１８は、判定手段の一例であり、横位置補正部２０及び共分散行列補正部２２は、補正手段の一例である。
【００２４】
走路認識部１６は、撮像装置１２によって撮像された道路画像に基づいて、自車両が走行する走行車線の境界の位置を検出する。そして、走路認識部１６は、検出された走行車線の境界の位置に基づいて、予め定められた走路モデルに関する走路パラメータを、拡張カルマンフィルタを用いて推定する。
【００２５】
具体的には、まず走路認識部１６は、前処理として、道路画像から走行車線の境界の候補点の位置の座標値を抽出する。前処理では、自車両が走行中の車線の左右の境界を抽出する。
【００２６】
一般には、走路には白線レーンマークが敷設されているため、走路認識部１６は、白線レーンマークを走行車線の境界の候補として抽出すればよい。白線レーンマークは、道路画像上では走路面に対して輝度が高いため、走路認識部１６は、例えば、道路画像を既知のＳｏｂｅｌフィルタ等によって処理し、得られた処理結果が閾値よりも大きい点を走行車線の境界の候補点として抽出する。
【００２７】
さらに、白線レーンマークは、局所的には進行方向に直線状に連続しているため、Ｈｏｕｇｈ変換等の手法により、直線上に連続している箇所を、車線の境界の候補として絞り込んでもよい。また、白線レーンマークの幅は局所的には一定なので、白線レーンマークの左右の車線の境界の候補対を照合して、車線の境界の候補点として選別してもよい。
【００２８】
次に、走路認識部１６は、後処理として、走行車線の境界の候補点の座標値に対して予め設定された走路モデルを当てはめ、拡張カルマンフィルタを用いて走路パラメータを推定する。
【００２９】
第１の実施の形態では、走路モデルとして平行直線モデルを用いる場合について説明する。図２に平行直線モデルを説明するための図を示す。
【００３０】
図２に示すように、平行直線モデルは、走路が直線であることを想定し、走路の曲率・曲率変化率を無視したモデルである。平行直線モデルでは、道路画像上の車線の境界の位置を記述する観測モデルと、車両の時間更新を記述する時間更新モデルとが設定される。
【００３１】
以下では、まず、観測モデルについて説明する。観測モデルでは、走路は平面上にあると仮定して、上記図２に示すように、自車両を基準としたｘ−ｚ車両座標系を設定する。さらに、走路境界を規定する白線は平行直線で表現されると仮定する。この場合、走行平面上の走路境界である白線の位置ｘは、以下の式（１．１）で表現することができる。なお、以下では、自車両が走行している走路を走行車線とし、車線の境界を白線として説明する。
【００３２】
【数１】
0006488226.tif 000002
【００３３】
上記式（１．１）における、ｘ０は自車両が走行している走行車線の中心の横位置［ｍ］を表し、θはヨー角（車線の傾き）［ｒａｄ］を表し、ｗは車線幅［ｍ］を表し、ｓは観測点が右白線の場合＋１、左白線の場合−１となる変数を表す。ｘ−ｚ車両座標上の白線は、道路画像上に設定されたＩ−Ｊ座標系へ透視変換することにより、撮像装置１２に撮像されて観測される白線点の座標値（Ｉｉ，Ｊｉ）として得られる。
【００３４】
なお、上記透視変換は、以下の式（１．２）及び（１．３）によって表される。
【００３５】
【数２】
0006488226.tif 000003
【００３６】
上記式（１．２）及び（１．３）において、Ｉｉは画像上のｉ番目の白線点の垂直方向の座標値［画素］を表し、Ｊｉは道路画像上のｉ番目の白線点の水平方向の座標値［画素］を表し、φはピッチ角［ｒａｄ］（ここでは変数ではなく、固定値と見なす）を表し、Ｈｃは路面平面からの撮像装置の高さ［ｍ］を表し、ｆは焦点距離［ｍ］を表し、Ｉｃは画像中心の垂直方向の座標値［画素］を表し、Ｊｃは画像中心の水平方向の座標値［画素］を表す。また、Ｒｖは道路画像の垂直方向分解能を表し、Ｒｈは道路画像の水平方向分解能を表す。
【００３７】
上記式（１．１）及び上記式（１．３）より、以下の式（１．４）が得られる。
【００３８】
【数３】
0006488226.tif 000004
【００３９】
上記式（１．４）中のｚは、上記式（１．２）を変形した以下の式（１．５）より白線点毎に求められる。
【００４０】
【数４】
0006488226.tif 000005
【００４１】
次に、時間更新モデルについて説明する。時刻ｔの走路パラメータＸｔを、以下の式（１．６）で定義する。以下の式（１．６）に示すように、走路パラメータは、自車両に対する走行車線の中心の横位置ｘ０が含まれている。
【００４２】
【数５】
0006488226.tif 000006
【００４３】
時刻ｔ−１の走路パラメータＸｔ−１から時刻ｔの走路パラメータＸｔの時間更新は、以下の式（１．７）で表現される。
【００４４】
【数６】
0006488226.tif 000007
【００４５】
このとき、上記式（１．７）における更新行列Ｆとシステムに付加される入力ｕとは、以下の式（１．８）及び（１．９）で表現される。また、上記式（１．７）におけるｖはシステムノイズ項を表す。
【００４６】
【数７】
0006488226.tif 000008
【００４７】
上記式（１．８）及び（１．９）において、Δｚは車速から算出した更新周期時間Δｔ内の自車両の移動距離［ｍ］を表し、Δｔは更新周期時間［ｓ］を表し、Ｙｒはヨーレイト［ｒａｄ/ｓ］を表す。
【００４８】
また、観測行列Ｈは以下の式（１．１０）で表現される。
【００４９】
【数８】
0006488226.tif 000009
【００５０】
また、上記式（１．４）を走路パラメータの各要素で偏微分することにより、以下の式（１．１１）〜式（１．１４）が得られる。
【００５１】
【数９】
0006488226.tif 000010
【００５２】
以上をまとめると、以下の式（１．１５）が得られる。
【００５３】
【数１０】
0006488226.tif 000011
【００５４】
上記式（１．６）に示す走路パラメータＸと、上記式（１．４）と上記式（１．５）から予測される道路上の左右の白線位置と観測値との偏差の二乗和をコストとして、当該コストを最小化する最適化計算をすれば、走路パラメータＸの最適値が得られる。この最適化計算には、観測データの一時的部分的な欠陥にロバストな拡張カルマンフィルタを利用することができる。拡張カルマンフィルタにより、画像上の観測値に付加される観測ノイズや自車両運動によって発生するシステムノイズを考慮した走路パラメータＸの最適な推定値が得られる。拡張カルマンフィルタの計算方法は既知であり、例えば以下の参考文献等に記載されている。
【００５５】
参考文献：片山徹，“応用カルマンフィルタ”，朝倉書店，１９８３
【００５６】
拡張カルマンフィルタにより、走路パラメータＸを推定されると共に、その共分散行列Ｐも更新される。拡張カルマンフィルタの適用には、ダイナミクスモデルを状態遷移行列として設定する必要があるが、変動が緩やかな場合は単位行列を設定すればよい。
【００５７】
また、車両運動に関する知見が得られた場合、その知見をダイナミクスモデルに組み込むことにより推定性能を改善することができる。また、本実施の形態では、左右白線がｘ−ｚ車両座標系で平行な直線を想定したが、道路線形の曲率や曲率変化率を推定ベクトルとして考慮すれば、提案手法は容易に適用することができる。
【００５８】
次に、拡張カルマンフィルタへの適用について説明する。
【００５９】
以上で説明した観測モデルと時間更新モデルとを、以下に示す拡張カルマンフィルタの更新式（１．１６）〜（１．２３）に適用することにより、ｍ個の観測値から時々刻々の走路パラメータＸ＾ｔと、時々刻々の共分散行列Ｐｔが最新値に更新される。
【００６０】
【数１１】
0006488226.tif 000012
【００６１】
上記式（１．１６）〜（１．２３）において、Ｋはカルマンゲイン、Ｈは観測行列、Ｊｔｏｂｓ，ｉは時刻ｔの画像中のｉ番目の白線点の観測値である。また、Ｊｔｉは時刻ｔの画像中のｉ番目の白線点の予測値ベクトルであり上記式（１．４）と式（１．５）とＸ￣ｔとＩｉとから算出される。
【００６２】
また、上記式（１．１８）に含まれるＲは観測ノイズの分散行列で、以下の式（１．２４）に示すように画像上の白線点位置の観測誤差の標準偏差二乗σ２ｏｂｓを要素とした対角行列である。
【００６３】
【数１２】
0006488226.tif 000013
【００６４】
また、上記式（１．１７）に含まれるＱは、以下の式（１．２５）で示されるシステムノイズ分散行列である。
【００６５】
【数１３】
0006488226.tif 000014
【００６６】
拡張カルマンフィルタによる推定処理は、予測ステップとフィルタリングステップとを含んで構成される。以下、走路認識部１６によって実行される拡張カルマンフィルタによる予測ステップとフィルタリングステップとについて説明する。
【００６７】
（１）予測ステップ
まず、予測ステップにおける処理について説明する。走路認識部１６は、予測ステップにおいて、前回のステップで算出された時刻ｔ−１の走路パラメータＸｔ−１と、上記式（１．８）に示す更新行列Ｆ＾とに基づいて、上記式（１．１６）に従って、時刻ｔの走路パラメータＸ￣ｔを算出する。
【００６８】
そして、走路認識部１６は、上記式（１．８）に示す更新行列Ｆ＾と、前回のステップで推定された共分散行列Ｐｔ−１と、システムノイズ分散行列Ｑとに基づいて、上記式（１．１７）に従って、共分散行列Ｐ￣ｔを算出する。
【００６９】
（２）フィルタリングステップ
次に、フィルタリングステップにおける処理について説明する。走路認識部１６は、フィルタリングステップにおいて、予測ステップで算出された共分散行列Ｐ￣ｔと、観測行列Ｈと、観測ノイズの分散行列Ｒとに基づいて、上記式（１．１８）に従って、カルマンゲインＫを算出する。
【００７０】
次に、走路認識部１６は、算出されたカルマンゲインＫと、予測ステップで算出された時刻ｔにおける状態ベクトルＸ￣ｔと、上記式（１．２１）で示す観測値ｈｏｂｓと、上記式（１．２２）で示す予測値ベクトルと、上記式（１．２３）に示す観測行列Ｈとに基づいて、上記式（１．１９）に従って、時刻ｔにおける走路パラメータＸ＾ｔを推定する。
【００７１】
そして、走路認識部１６は、予測ステップで推定された共分散行列Ｐ￣ｔと、算出されたカルマンゲインＫと、上記式（１．２３）に示す観測行列Ｈとに基づいて、上記式（１．２０）に従って、共分散行列Ｐｔを算出する。
【００７２】
フィルタリングステップで算出された各値（カルマンゲインＫ、走路パラメータＸ＾ｔ、共分散行列Ｐｔ）は、原則として次回の予測ステップにおける処理で用いられる。しかし、本実施の形態では、後述する車線変更判定部１８による判定処理に応じて、走路パラメータＸ＾ｔのうち自車両に対する走行車線の中心の横位置ｘ０と共分散行列Ｐｔとが補正される。
【００７３】
車線変更判定部１８は、走路認識部１６によって検出された走行車線の境界の位置に基づいて、自車両の走行状態が車線変更であるか否かを判定する。具体的には、車線変更判定部１８は、走路認識部１６によって検出された走行車線の境界の位置に基づいて、自車両に対する車線中心の横位置の絶対値が、車線幅の１／２以上の位置である場合に、自車両の走行状態が車線変更であると判定する。
【００７４】
具体的には、撮像装置１２によって撮像された道路画像の観測フレームの拡張カルマンフィルタによる走路パラメータの更新が終了した時点で、車線変更の判定と車線の変更時の各処理とが実行される。車線変更の判定式は以下の式（１．２６）で表現される。
【００７５】
【数１４】
0006488226.tif 000015
【００７６】
なお、上記式（１．２６）のαは車線の境界付近で車線変更の判定の頻発を防ぐためのヒステリシス項である。
【００７７】
横位置補正部２０は、車線変更判定部１８によって自車両の走行状態が右側車線への変更と判定された場合に、車線の横位置に車線幅を加算して補正する。また、横位置補正部２０は、車線変更判定部１８によって自車両の走行状態が左側車線への変更と判定された場合に、車線の横位置から車線幅を減算して補正する。
【００７８】
図３に、車両の車線変更の判定処理の一例を示す。図３では、自車両が車線１を走行している状態から、白線を跨ぐ状態に移り、最終的に車線２を走行する状態に移行する。車線の変更の前後で、観測している左右の白線はａ，ｂからｂ，ｃに切り替わるため、自車両が走行している走行車線の中心の横位置は、以下の式（１．２７）のように修正する必要があることが分かる。
【００７９】
【数１５】
0006488226.tif 000016
【００８０】
なお、上記式（１．２７）におけるプラスマイナスの符号は、右側車線へ変更する場合が正で、左側車線へ変更する場合は負である。
【００８１】
共分散行列補正部２２は、車線変更判定部１８によって自車両の走行状態が車線変更であると判定された場合に、車線幅に関するパラメータと車線幅に関するパラメータと異なる他の走路パラメータとの相関を表す部分行列の符号を反転するように、拡張カルマンフィルタにおける共分散行列を補正する。
【００８２】
具体的には、共分散行列補正部２２は、車線変更が行われた場合に、以下の式（１．２８）で表される拡張カルマンフィルタの共分散行列Ｐを、以下の式（１．２９）に従って、車線変更後の共分散行列Ｐｌｃへ補正する。
【００８３】
【数１６】
0006488226.tif 000017
【００８４】
上記式（１．２８）及び（１．２９）に基づく補正処理は、車線変更の左右の方向にかかわらず処理は同一である。また、上記式（１．２８）及び（１．２９）において、Ｐ０は車線幅ｗを除いた共分散行列Ｐの部分行列、Ｐｗは車線幅ｗに対応する共分散行列Ｐの部分行列、Ｐ１は共分散行列Ｐの車線幅ｗに対応する列から対角項を除いた部分行列である。また、Ｐ１は車線幅に関するパラメータと車線幅に関するパラメータと異なる他の走路パラメータとの相関を表す部分行列である。なお、推定する走路パラメータの個数をｎ個とするとＰ０のサイズは（ｎ−１）×（ｎ−１）、Ｐ１のサイズは（ｎ−１）×１、Ｐｗのサイズは１×１である。
【００８５】
出力部２４は、車線変更判定部１８によって自車両の走行状態が車線変更であると判定された場合には、横位置補正部２０によって補正された走行車線の中心の横位置と、走路認識部１６によって推定された走行車線の中心の横位置以外の走路パラメータと、共分散行列補正部２２によって補正された共分散行列とを結果として出力し、メモリ（図示省略）に格納する。一方、出力部２４は、車線変更判定部１８によって自車両の走行状態が車線変更でないと判定された場合には、走路認識部１６によって推定された走路パラメータと共分散行列とを結果として出力し、メモリ（図示省略）に格納する。
【００８６】
＜走路パラメータ推定装置の１０の動作＞
次に、第１の実施の形態に係る走路パラメータ推定装置の１０の動作について説明する。まず、自車両が走行し、撮像装置１２によって自車両の前方が逐次撮像されているときに、コンピュータ１４において、図４に示す走路認識処理ルーチンが実行される。
【００８７】
ステップＳ１００において、走路認識部１６は、撮像装置１２によって撮像された道路画像を取得する。
【００８８】
ステップＳ１０２において、走路認識部１６は、前処理として、上記ステップＳ１００で受け付けた道路画像から、走行車線の境界の候補点の位置の座標値を抽出する。
【００８９】
ステップＳ１０４において、走路認識部１６は、後処理として、上記ステップＳ１０２で抽出された走行車線の境界の候補点の座標値に対して走路モデルを当てはめ、拡張カルマンフィルタを用いて走路パラメータを推定する。ステップＳ１０４は、図５に示す走路パラメータ推定処理ルーチンによって実現される。
【００９０】
＜走路パラメータ推定処理ルーチン＞
ステップＳ２００において、走路認識部１６は、予測ステップにおいて、前回のステップＳ１１２でメモリ（図示省略）に格納された走路パラメータＸｔ−１と、上記式（１．８）に示す更新行列Ｆ＾とに基づいて、上記式（１．１６）に従って、時刻ｔの走路パラメータＸ￣ｔを算出する。
【００９１】
ステップＳ２０２において、走路認識部１６は、上記式（１．８）に示す更新行列Ｆ＾と、前回のステップＳ１１２でメモリ（図示省略）に格納された共分散行列Ｐｔ−１と、システムノイズ分散行列Ｑとに基づいて、上記式（１．１７）に従って、共分散行列Ｐ￣ｔを算出する。
【００９２】
ステップＳ２０４において、走路認識部１６は、フィルタリングステップにおいて、上記ステップＳ２０２で算出された共分散行列Ｐ￣ｔと、観測行列Ｈと、観測ノイズの分散行列Ｒとに基づいて、上記式（１．１８）に従って、カルマンゲインＫを算出する。
【００９３】
ステップＳ２０６において、走路認識部１６は、上記ステップＳ２０４で算出されたカルマンゲインＫと、予測ステップで算出された時刻ｔにおける状態ベクトルＸ￣ｔと、上記式（１．２１）で示す観測値ｈｏｂｓと、上記式（１．２２）で示す予測値ベクトルと、上記式（１．２３）に示す観測行列Ｈとに基づいて、上記式（１．１９）に従って、時刻ｔにおける走路パラメータＸ＾ｔを推定する。
【００９４】
ステップＳ２０８において、走路認識部１６は、上記ステップＳ２０２で算出された共分散行列Ｐ￣ｔと、上記ステップＳ２０４で算出されたカルマンゲインＫと、上記式（１．２３）に示す観測行列Ｈとに基づいて、上記式（１．２０）に従って、共分散行列Ｐｔを算出する。
【００９５】
ステップＳ２１０において、走路認識部１６は、上記ステップＳ２０６で算出された走路パラメータＸ＾ｔと、上記ステップＳ２０８で算出された共分散行列Ｐｔを結果として出力する。
【００９６】
次に、走路認識処理ルーチンに戻り、ステップＳ１０６において、車線変更判定部１８は、上記ステップＳ１０２で検出された走行車線の境界の位置に基づいて、自車両の走行状態が車線変更であるか否かを判定する。自車両に対する車線中心の横位置の絶対値が、車線幅の１／２以上の位置である場合に、自車両の走行状態が車線変更であると判定し、ステップＳ１０８へ進む。一方、自車両に対する車線中心の横位置の絶対値が、走路幅の１／２未満の位置である場合に、自車両の走行状態は車線変更でないと判定し、ステップＳ１１２へ進む。
【００９７】
ステップＳ１０８において、横位置補正部２０は、自車両の走行状態が右側車線への変更と判定された場合に、上記ステップＳ１０４で得られた走路パラメータの車線の横位置ｘ０に車線幅ｗを加算して補正する。また、横位置補正部２０は、自車両の走行状態が左側車線への変更と判定された場合に、上記ステップＳ１０４で得られた走路パラメータの車線の横位置ｘ０から車線幅ｗを減算して補正する。
【００９８】
ステップＳ１１０において、共分散行列補正部２２は、上記式（１．２９）に示すように、車線幅に関するパラメータｗと車線幅に関するパラメータｗと異なる他の走路パラメータとの相関を表す部分行列の符号を反転するように、上記ステップＳ１０４得られた共分散行列Ｐｔを補正する。
【００９９】
ステップＳ１１２において、出力部２４は、走路パラメータと共分散行列Ｐｔとを結果として出力し、メモリ（図示省略）に格納する。
【０１００】
そして、ステップＳ１１４において、時刻ｔを１インクリメントして、ステップＳ１００へ戻る。
【０１０１】
なお、補正された走路パラメータの車線の横位置ｘ０と共分散行列Ｐｔとは、次回の走路パラメータの推定で用いられる。
【０１０２】
以上説明したように、第１の実施の形態に係る走路パラメータ推定装置によれば、自車両の走行状態が車線変更と判定された場合に、走路パラメータの車線の横位置と、拡張カルマンフィルタにおける共分散行列とを補正することにより、車両の車線変更が行われた場合でも、走路パラメータを精度良く推定することができる。
【０１０３】
また、自車両からみて片側の車線境界しか検出されない場合であっても走路パラメータを算出することができる。図６に、従来手法において片側の車線境界しか検出されない場合の例を示し、図７に、本実施の形態による提案法において片側の車線境界しか検出されない場合の例を示す。
【０１０４】
図６に示すように、片側白線のみを観測する状態が継続した場合、従来法では車線変更の直前（Ａ）と直後（Ｂ）とで白線に投影された共分散が不連続に増大するため観測ノイズの影響を受けやすくなる。一方、図７に示す本実施の形態による提案法では、直前（Ａ）と直後（Ｂ）とで白線に投影された共分散が連続して変化しないので、観測ノイズの影響を受けにくい。
【０１０５】
また、第１の実施の形態に係る走路パラメータ推定装置では、車両の走路変更時に、車両に対する走路の中心の横位置の補正に合わせて、同時に、拡張カルマンフィルタにおける共分散行列を補正する。具体的には、車線変更後に追跡中の車線境界（例えば白線）は、右白線だったものは左白線に、左白線だったものは右白線に切り替わる。拡張カルマンフィルタによって更新された共分散行列には、左右の白線の情報が保存されているため、走路変更に合わせて共分散行列も補正することにより、左右の白線の情報の整合性をとることができる。
これにより、車線変更の直後のノイズに対するロバスト性を向上させることができる。また、車線変更の直後の走路パラメータの異常値の発生を低減させることができ、車線変更の直後の走路パラメータのばたつきを低減させることができる。
【０１０６】
［第２の実施の形態］
次に、第２の実施の形態について説明する。なお、第２の実施の形態に係る走路パラメータ推定装置は、第１の実施の形態と同様の構成となるため、同一符号を付して説明を省略する。
【０１０７】
第２の実施の形態では、走路モデルとして曲線モデルを用いる点が第１の実施の形態と異なっている。
【０１０８】
第１の実施の形態では、最低限の走路パラメータ（車線幅と車線の横位置との２つのパラメータを含む走路パラメータ）の単純な場合で説明したが、実際のシステムでは曲率等を含む走路パラメータが推定されるため、第２の実施の形態において説明する。また、第２の実施の形態では、第１の実施の形態で説明を省略した車線変更時の共分散行列の補正式の導出証明についても説明する。
【０１０９】
第２の実施の形態に係る走路パラメータ推定装置の走路認識部１６は、走行車線の境界の候補点の座標値に対して予め設定された走路モデルを当てはめ、拡張カルマンフィルタを用いて走路パラメータを推定する。第２の実施の形態では、走路モデルとして曲線モデルを用いるため、第２の実施の形態走路パラメータは、第１の実施の形態の走路パラメータと異なる。
【０１１０】
図８に、第２の実施の形態で用いる曲線モデルを説明するための図を示す。図８に示す曲線モデルにおいて、拡張カルマンフィルタにより走路パラメータを推定するためには、道路画像上の白線位置を記述する観測モデルと、車両と走路曲率の時間更新を記述する時間更新モデルとが設定される。
【０１１１】
以下では、まず観測モデルについて説明する。
【０１１２】
上記図８に示すように、走路は平面上にあると仮定して、自車両を基準としたｘ‐ｚ車両座標系を設定する。さらに、走路境界を規定する白線は３次関数で表現されると仮定する。この場合、走行平面上の白線位置は以下の式（２．１）で表現できる。
【０１１３】
【数１７】
0006488226.tif 000018
【０１１４】
上記式（２．１）において、ｘ０は自車両が走行している走行車線の中心の横位置［ｍ］を表し、θはヨー角（走路の傾き）［ｒａｄ］を表し、ｃ０は撮像装置の位置直下（（ｘ，ｚ）＝（０，０））の曲率［１／ｍ］を表し、ｃ１は曲率変化率［１／ｍ２］を表し、ｗは車線幅［ｍ］を表し、ｓは観測点が右白線の場合＋１、左白線の場合−１となる変数を表す。
【０１１５】
ｘ−ｚ車両座標上の白線は、道路画像上に設定されたＩ−Ｊ座標系へ透視変換することにより、撮像装置１２によって撮像されて観測される白線点の座標値（Ｉｉ，Ｊｉ）として得られる。
【０１１６】
なお、上記透視変換は、以下の式（２．２）及び（２．３）によって表される。
【０１１７】
【数１８】
0006488226.tif 000019
【０１１８】
このとき，Ｉｉは画像上のｉ番目の白線点の垂直方向の座標値［画素］を表し、Ｊｉは道路画像上のｉ番目の白線点の水平方向の座標値［画素］を表し、φはピッチ角［ｒａｄ］を表し、Ｈｃは路面平面からの撮像装置の高さ［ｍ］を表し、ｆは焦点距離［ｍ］を表し、Ｉｃは画像中心の垂直方向の座標値［画素］を表し、Ｊｃは画像中心の水平方向の座標値［画素］を表す。また、Ｒｖは道路画像の垂直方向分解能を表し、Ｒｈは道路画像の水平方向分解能を表す。
【０１１９】
上記式（２．１）及び上記式（２．３）より、以下の式（２．４）が得られる。
【０１２０】
【数１９】
0006488226.tif 000020
【０１２１】
上記式（２．４）中のｚは、上記式（２．２）を変形した以下の式（２．５）より白線点毎に求められる。
【０１２２】
【数２０】
0006488226.tif 000021
【０１２３】
次に、時間更新モデルについて説明する。時刻ｔの走路パラメータＸｔを、以下の式（２．６）で定義する。以下の式（２．６）に示すように、走路パラメータは、上記式（２．４）と上記式（２．５）の中で使用した自車両の位置姿勢と白線形状を規定したパラメータである。ここで、Ｔは転置記号を表す。
【０１２４】
【数２１】
0006488226.tif 000022
【０１２５】
時刻ｔ−１の走路パラメータＸｔ−１から時刻ｔの走路パラメータＸｔの時間更新は、以下の式（２．７）で表現される。
【０１２６】
【数２２】
0006488226.tif 000023
【０１２７】
このとき、上記式（２．７）における更新行列Ｆとシステムに付加される入力ｕとは、以下の式（２．８）及び（２．９）で表現される。また、上記式（２．７）におけるｖはシステムノイズ項を表す。
【０１２８】
【数２３】
0006488226.tif 000024
【０１２９】
上記式（２．８）及び（２．９）において、Δｚは車速から算出した更新周期時間Δｔ内の自車両の移動距離［ｍ］を表し、Δｔは更新周期時間［ｓ］を表し、Ｙｒはヨーレイト［ｒａｄ/ｓ］を表す。
【０１３０】
拡張カルマンフィルタへの適用については、上記第１の実施の形態と同様であるため、説明を省略する。
【０１３１】
＜共分散行列の補正式の導出＞
次に、拡張カルマンフィルタの共分散行列Ｐの補正式を導出する。
【０１３２】
導出の前提として、推定値Ｘ＾の場合の白線ｂ上の任意の点の水平方向座標Ｊの分散を計算する。白線点の水平方向座標Ｊは式（２．４）で表現されるが、以下では式展開の簡易化のために式（３．１）のように関数ｈで表す。
【０１３３】
【数２４】
0006488226.tif 000025
【０１３４】
ここで、線形の範囲で推定値Ｘ＾の微小変化分をΔＸ＾とすると、それによって生じるＪの微小変化分ΔＪは次式で得られる。
【０１３５】
【数２５】
0006488226.tif 000026
【０１３６】
既知の分散の公式より、以下の式（３．３）が既知である。
【０１３７】
【数２６】
0006488226.tif 000027
【０１３８】
ただし、Ｖａｒは分散を表し，Ａは行列で、Ｘとαとはベクトルを表す。上記式（３．３）を上記式（３．２）へ適用することにより、以下の式（３．４）が得られる。
【０１３９】
【数２７】
0006488226.tif 000028
【０１４０】
式（３．５）のＶａｒ（ΔＸ＾）を誤差共分散行列Ｐと見なすと次式が得られる。
【０１４１】
【数２８】
0006488226.tif 000029
【０１４２】
以上をまとめると、以下の式（３．７）が得られる。
【０１４３】
【数２９】
0006488226.tif 000030
【０１４４】
このとき、Ｈは以下の式（３．８）に示す観測行列である。
【０１４５】
【数３０】
0006488226.tif 000031
【０１４６】
上記式（２．４）より、画像上の白線点Ｊ座標値は、以下の式（３．９）で表される。
【０１４７】
【数３１】
0006488226.tif 000032
【０１４８】
上記式（３．９）を走路パラメータの各要素で偏微分することにより、以下の式(３．１０)〜式(３．１６)が得られる。
【０１４９】
【数３２】
0006488226.tif 000033
【０１５０】
上記式（３．８）、上記式（３．１０）〜式（３．１６）より以下の式（３．１７）が得られる。
【０１５１】
【数３３】
0006488226.tif 000034
【０１５２】
特に車線変更時は、白線を跨いでいる位置にあるので、以下の式（３．１８）が成立する。
【０１５３】
【数３４】
0006488226.tif 000035
【０１５４】
上記式（３．１７）と上記式（３．１８）より，以下の式（３．１９）が得られる。
【０１５５】
【数３５】
0006488226.tif 000036
【０１５６】
上記式（３．１９）より、右車線変更時の観測行列Ｈｒは以下の式（３．２０）で表される。
【０１５７】
【数３６】
0006488226.tif 000037
【０１５８】
また、上記式（３．１９）より、左車線変更時の観測行列Ｈｌは、以下の式（３．２１）で表される。
【０１５９】
【数３７】
0006488226.tif 000038
【０１６０】
上記式（３．２０）と上記式（３．２１）とを比較すると、右車線変更時と左車線変更時の観測行列は、車線幅に関する項の符号が正負反転している点だけが違っていることが分かる。
【０１６１】
上記式（３．２０）と上記式（３．２１）との中のｈｗは車線幅の偏微分項で、ｈ０は車線幅以外の項を要素としたベクトルである。
【０１６２】
ここで、車線変更前の共分散行列をＰとして、その要素を以下の式（３．２２）で設定する。
【０１６３】
【数３８】
0006488226.tif 000039
【０１６４】
このとき、Ｐ０は車線幅以外に対応する部分行列、Ｐｗは車線幅に対応する部分行列である。また、Ｐ１は車線幅の非対角項の部分行列であり、車線幅に関するパラメータｗと車線幅に関するパラメータｗと異なる他の走路パラメータとの相関を表す部分行列である。また、車線変更後の共分散行列をＰｌｃとして、その要素を以下の式（３．２３）で設定する。
【０１６５】
【数３９】
0006488226.tif 000040
【０１６６】
このとき、Ｐｌｃ０は車線幅以外の部分行列、Ｐｌｃｗは車線幅に対応する部分行列、Ｐｌｃ１は車線幅の非対角項の部分行列である。図３の右車線変更の例では、車線変更の直前・直後で、白線ｂの観測が継続している。そのため、車線変更の直前・直後で白線ｂの分散も連続している必要がある。よって、車線変更の直前・直後で注目した全ての白線点の分散は変化しないと仮定すると、以下の式（３．２４）が得られる。
【０１６７】
【数４０】
0006488226.tif 000041
【０１６８】
上記式（３．２４）に、上記式（３．２０）〜式（３．２３）を代入すると、以下の式（３．２５）が得られる。
【０１６９】
【数４１】
0006488226.tif 000042
【０１７０】
上記式（３．２５）を展開すると、以下の式（３．２６）が得られる。
【０１７１】
【数４２】
0006488226.tif 000043
【０１７２】
上記式（３．２６）を整理すると、以下の式（３．２７）が得られる。
【０１７３】
【数４３】
0006488226.tif 000044
【０１７４】
上記式（３．２７）の２番目と３番目の項はスカラーなので、以下の式（３．２８）のように整理することができる。
【０１７５】
【数４４】
0006488226.tif 000045
【０１７６】
上記式（３．２８）が白線ｂ上の任意の一点について成立するためには、以下の式（３．２９）〜式（３．３２）が成立すればよい。
【０１７７】
【数４５】
0006488226.tif 000046
【０１７８】
以上より、車線変更後の共分散行列Ｐｌｃは次式で表される。
【０１７９】
【数４６】
0006488226.tif 000047
【０１８０】
すなわち、車両の車線変更時、共分散行列は、走路パラメータの「車線幅」に対応する行と列の非対角項の符号を反転するように補正すればよい。以上は右車線変更で例示したが、左車線変更時も同様である。
【０１８１】
なお、第２の実施の形態に係る走路パラメータ推定装置の他の構成及び作用については、第１の実施の形態と同様であるため、説明を省略する。
【０１８２】
以上説明したように、第２の実施の形態に係る車両挙動予測装置によれば、走路モデルとして曲線モデルを用い、自車両の走行状態が走路の変更と判定された場合に、走路パラメータの走路の横位置と、拡張カルマンフィルタにおける共分散行列とを補正することにより、車両の車線変更が行われた場合でも、走路パラメータを精度良く推定することができる。
【０１８３】
＜実施例＞
上記実施の形態に係る走路パラメータ推定装置による、車線変更処理の効果の例を図９に示す。片側白線観測時の車線変更直後の白線位置推定誤差σ（標準偏差）を比較したシミュレーション結果を図９に示した。図９では観測ノイズσの設定が１画素と１０画素の場合について示している。図９より、推定された白線位置σの推定誤差σが半減していることから、本実施の形態に係る提案法の効果が確認される。
【０１８４】
なお、上記の実施の形態では、移動体として車両を対象とする場合を例に説明したが、これに限定されるものではなく、他の移動体を対象としてもよい。
【０１８５】
また、撮像装置１２に代えてレーザレーダを用いることもできる。レーザレーダは対象までの距離を計測するアクティブセンサであるが反射波の受光強度により道路画像を生成することも可能である。
【０１８６】
なお、本発明のプログラムは、記録媒体に格納して提供することができる。
【符号の説明】
【０１８７】
１０   走路パラメータ推定装置
１２   撮像装置
１４   コンピュータ
１６   走路認識部
１８   車線変更判定部
２０   横位置補正部
２２   共分散行列補正部
２４   出力部
【図１】
0006488226.tif 000048
【図２】
0006488226.tif 000049
【図３】
0006488226.tif 000050
【図４】
0006488226.tif 000051
【図５】
0006488226.tif 000052
【図６】
0006488226.tif 000053
【図７】
0006488226.tif 000054
【図８】
0006488226.tif 000055
【図９】
0006488226.tif 000056