(57)【要約】【課題】道路の曲率を高精度に推定する曲率推定装置を提供することを課題とする。【解決手段】撮像手段で撮像した画像に基づいて道路の曲率を推定する曲率推定装置であって、車両の運転者の操舵による蛇行を考慮して、車両は運転者の操舵に応じて道路（車線）に沿って走行するが、走行中の車両から車線を観測したと仮定すると車線は周期的に微少な蛇行をしており、その車線の蛇行は運転者の操舵によって発生すると仮定し、操舵の舵角を入力とし、道路の曲率を出力とする道路モデルを構築し、その道路モデルを組み込んだカルマンフィルタに実際の車両走行データを適用し、画像から求めた道路の曲率に対してカルマンフィルタを駆動した結果に基づいて曲率を推定することを特徴とし、車両の運転者の操舵による蛇行の周波数を道路モデルに適用すると好適である。【選択図】図３
2013125327.tif 000002
【特許請求の範囲】
【請求項１】
撮像手段で撮像した画像に基づいて道路の曲率を推定する曲率推定装置であって、
車両の運転者の操舵による蛇行に基づいて、操舵の舵角を入力とし、道路の曲率を出力とする道路モデルを構築し、前記道路モデルを組み込んだカルマンフィルタに実際の車両走行データを適用し、前記撮像手段で撮像した画像から求めた道路の曲率に対して前記カルマンフィルタを駆動した結果に基づいて曲率を推定することを特徴とする曲率推定装置。
【請求項２】
車両の運転者の操舵による蛇行の周波数を計算し、該蛇行周波数を前記道路モデルに適用することを特徴とする請求項１に記載の曲率推定装置。
【請求項３】
道路がカーブの出入口の場合、単調増加又は単調減少の関数を用いて曲率を推定することを特徴とすることを特徴とする請求項１又は請求項２に記載の曲率推定装置。
【請求項４】
道路が直線の場合、カルマンフィルタを駆動しないことを特徴とする請求項１〜請求項３のいずれか１項に記載の曲率推定装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、撮像手段で撮像した画像に基づいて道路の曲率を推定する曲率推定装置に関する。
【背景技術】
【０００２】
特許文献１は、自車両の左右両側の車線区間線を検出し、車線区間線の検出結果に基づいて自車両の車線逸脱防止を支援する車線逸脱防止支援装置について記載されており、カメラで撮像した自車両前方の画像から車線の曲率を検出することが開示されている。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２０１１−１１８８２８号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
上記のように、道路（車線）の曲率を推定する場合、通常、カメラが用いられる。カメラで撮像した画像を処理して曲率を推定すると、ノイズ（例えば、画像に対するエッジ処理のノイズ、カメラノイズ）が含まれる場合がある。そのため、曲率の推定精度が低下する。
【０００５】
そこで、本発明は、道路の曲率を高精度に推定する曲率推定装置を提供することを課題とする。
【課題を解決するための手段】
【０００６】
本発明に係る曲率推定装置は、撮像手段で撮像した画像に基づいて道路の曲率を推定する曲率推定装置であって、車両の運転者の操舵による蛇行に基づいて、操舵の舵角を入力とし、道路の曲率を出力とする道路モデルを構築し、道路モデルを組み込んだカルマンフィルタに実際の車両走行データを適用し、撮像手段で撮像した画像から求めた道路の曲率に対してカルマンフィルタを駆動した結果に基づいて曲率を推定する。
【０００７】
幾何学的な道路はカルマンフィルタで構成できない。そこで、曲率推定装置では、車両の運転者の操舵による蛇行を考慮し、操舵の舵角を入力とし、道路の曲率を出力とする道路モデルを構築する。車両は運転者の操舵に応じて道路（車線）に沿って走行するが、走行中の車両から車線を観測したと仮定すると車線は周期的に微少な蛇行をしており、その車線の蛇行は運転者の操舵によって発生すると考えられる。そこで、この車線の変化（蛇行）は運転者の操舵に応じた舵角と関連付けられるので、車線の曲線と舵角とから構成される道路モデルを構築し、この道路モデルを用いることによってカルマンフィルタを可制御とすることができる。そして、この曲率推定モデルでは、道路モデルを組み込んだカルマンフィルタに実際の車両走行データ（舵角、ヨーレート（横加速度）、ヨー角（横速度）等）を適用し、画像から求めた道路の曲率に対してカルマンフィルタを駆動した結果に基づいて曲率を推定する。このように、曲率推定装置では、道路の曲率を運転者の操舵による蛇行と関連付けた道路モデルを組み込んだカルマンフィルタを構成することにより、簡易な構成（演算）で演算負荷が少なく、画像から求められる曲率に含まれるノイズを抑制でき、道路の曲率を高精度に推定することができる。
【０００８】
本発明の上記曲率推定装置では、車両の運転者の操舵による蛇行の周波数を計算し、該蛇行周波数を道路モデルに適用すると好適である。運転者によって操舵特性が異なるので、蛇行の周波数も異なる。そこで、この曲率推定装置では、各運転者についての操舵による蛇行周波数を求めて、その蛇行周波数を道路モデルに適用することにより、運転者の操舵特性に応じた道路モデルを組み込んだカルマンフィルタを構成することができ、道路の曲率をより高精度に推定することができる。
【０００９】
本発明の上記曲率推定装置では、道路がカーブの出入口の場合、単調増加又は単調減少の関数を用いて曲率を推定すると好適である。通常の車両走行では、カーブの入口でインカットとなり、カーブの出口で膨らむ。そこで、この曲率推定装置では、そのカーブ出入口での走行特性を考慮するために、単調増加又は単調減少の関数を用いて曲率を推定することにより、道路の曲率をより高精度に推定することができる。
【００１０】
本発明の上記曲率推定装置では、道路が直線の場合、カルマンフィルタを駆動しないと好適である。道路が直線路の場合、ノイズよりも蛇行のほうが大きくなり、上記のカルマンフィルタを用いると誤差が大きくなる。そこで、この曲率推定装置では、道路が直線の場合、画像から求めた道路の曲率に対してカルマンフィルタを駆動せずに、曲率の誤差を抑制する。
【発明の効果】
【００１１】
本発明によれば、道路の曲率を運転者の操舵による蛇行と関連付けた道路モデルを組み込んだカルマンフィルタを構成することにより、簡易な構成（演算）で演算負荷が少なく、画像から求められる曲率に含まれるノイズを抑制でき、道路の曲率を高精度に推定することができる。
【図面の簡単な説明】
【００１２】
【図１】本実施の形態に係る曲率推定装置の構成図である。
【図２】本実施の形態で用いる道路の曲率モデルを示す図である。
【図３】図１のＥＣＵにおける曲率推定処理の流れを示すフローチャートである。
【発明を実施するための形態】
【００１３】
以下、図面を参照して、本発明に係る曲率推定装置の実施の形態を説明する。なお、各図において同一又は相当する要素については同一の符号を付し、重複する説明を省略する。
【００１４】
本実施の形態に係る曲率推定装置は、車両に搭載され、車載カメラで撮像した画像に基づいて道路（特に、車線を構成する白線）の曲率を推定する。この推定された曲率情報は、車両に搭載される各種運転支援装置（例えば、車線逸脱防止装置、レーンキープ装置）や居眠り検出装置に用いる運転者モデル等で利用される。なお、本実施の形態では、道路の曲率として、道路上に描かれる車線を構成する白線の曲率を推定する。白線は道路に沿って描かれているので、白線の曲率は道路の曲率に相当する。
【００１５】
図１及び図２を参照して、本実施の形態に係る曲率推定装置１について説明する。図１は、本実施の形態に係る曲率推定装置の構成図である。図２は、本実施の形態で用いる曲率モデルを示す図である。
【００１６】
曲率推定装置１は、カメラ画像による道路の曲率に加えて道路の曲率と相関関係がある車両情報（舵角、ヨーレート（横加速度）等）を用いて、運転者の操舵による蛇行に関連付けた曲率モデル（道路モデル）を構築し、その曲率モデルをカルマンフィルタに組み込む。そしてに、曲率推定装置１は、運転者の操舵による舵角等を入力とし、曲率を出力としてそのカルマンフィルタを駆動し、カルマンフィルタの観測結果に基づいて曲率を推定する。
【００１７】
このようにカメラ画像による曲率情報に加えて曲率と相関関係がある車両情報（舵角、ヨーレート等）を併用するので、曲率と相関関係がある車両情報も用いることにより、カーブ出入口等における道路曲線の波形の鈍化に対応できる。
【００１８】
カメラ画像による曲率のノイズを抑制するためのカルマンフィルタを用いるが、カルマンフィルタが成立するための必要十分条件は、観測対象とする系が可制御かつ可観測であることである。しかし、幾何学的な道路をそのままモデル化した場合、運転者が右操舵した場合でも左操舵した場合でも道路の幾何学的な曲率が変化しないので、舵角やヨーレート（横加速度）では曲率は不可制御である。そのため、カルマンフィルタを構成できない。しかし、実際の車両の走行では、道路に沿った走行を行っているので、曲率と舵角（ヨーレート、横加速度）とには有意な相関関係がある。したがって、可制御な状態方程式であれば、カルマンフィルタを構成できる。
【００１９】
実際に車両が道路を走行する場合、車線を構成する白線に対して完全に平行な走行軌跡ではなく。道路の幅方向に対して微少の振幅（例えば、１０−２ｍオーダの振幅）で蛇行している。したがって、車両から白線を観測した場合、白線は蛇行して見える。そこで、車両は道路にほぼ沿って走行し、走行中の車両から白線を見ると白線はほぼ周期的に蛇行をしているように観測され、白線の蛇行は運転者の操舵によって発生すると仮定すると、白線の変化は舵角と関連付けられるので、白線（曲率）と舵角やヨーレート（横加速度）とで構成される系は可制御になり得る。
【００２０】
そこで、図２に示す道路（白線）の曲率モデルを考える。ここでは、車両固定座標系を用い、車両ＭＶから白線ＷＬを観測する。Ｖは、車両ＭＶの速度である。ａは、車両ＭＶの横加速度である（白線ＷＬの曲率ρにＶ２を乗算すればａの次元となる）。ｙは、車両ＭＶに対する白線ＷＬの横位置である。ｖは、車両ＭＶに対する白線ＷＬの横速度である。αは、車両ＭＶに対する白線ＷＬの横加速度である。白線ＷＬに対する車両ＭＶのヨー角度をθとすると、θ＝ｖ／Ｖの関係がある。曲率推定装置１では、このような車両固定座標系において道路（白線）が蛇行していると仮定して曲率モデルを構築し、この道路の曲率モデルを用いてカルマンフィルを構成する。
【００２１】
運転者の操舵による蛇行は、周期的な蛇行であるが、運転者によって蛇行の周期が異なる。そこで、蛇行の周波数を計算し、その蛇行周波数を曲率モデルに用いる。また、蛇行は、ＬＳＢ付近の蛇行なので、ＳＮ比が低い周波数分析となる。そこで、蛇行周波数は単振動なので、横加速度ａと横速度ｖ（白線のヨー角）の各値のオーバオールパワー比によって蛇行周波数を計算する。また、カメラ画像による検知で曲率を検知できなかった場合には、曲率の推定誤差が大きくなる。そこで、検知できなった場合にはカルマンフィルタの推定機能を重視して、曲率を推定する。
【００２２】
ただし、カーブの出入口の場合、通常のカーブ走行中のように蛇行せずに、入口ではインカットした走行となり、出口では膨らんだ走行となる。そこで、カーブの入口では単調増加の関数を用い、出口では単調減少の関数を用いる。また、道路が直線の場合、カメラ画像による曲率に含まれるノイズよりも蛇行のほうが大きくなるので、カルマンフィルタを駆動するとかえって誤差が大きくなる。そこで、道路が直線の場合、カルマンフィルタを駆動せずにカメラによる検知の曲率をそのまま用いるか、あるいは、カルマンフィルタにおけるノイズ設定値を調整する。
【００２３】
図２で示した白線の曲率モデルの式は、式（１）の微分方程式となる。また、カーブの出入口での単調増加や単調減少の関数となる白線の曲率モデルの式は、式（２）の微分方程式となる。式（１）、（２）におけるρｆｉｌｔは、カメラ画像による検知の曲率ρｃａｍに対して、カメラ画像による前方（又は後方）の検知位置と自車両位置（舵角等の検出位置）との時間差分を除去するためのフィルタがかけられた曲率であり、自車両位置での曲率となる。ζは、蛇行減衰比である。ωは、蛇行周波数である。Ｇは、舵角に対する横加速度のゲインである。τは、舵角に対する横加速度の時定数である。なお、各記号の上「・」は、１階時間微分を示す。
【数１】
2013125327.tif 000003
【００２４】
それでは、曲率推定装置１について具体的に説明する。曲率推定装置１は、図１に示すように、カメラセンサ１０、舵角センサ１１、横加速度センサ１２、車速センサ１３及びＥＣＵ[Electronic Control Unit]２０を備えている。
【００２５】
カメラセンサ１０は、車線を構成する白線を認識するセンサであり、カメラ（撮像手段）と画像処理装置からなる。カメラは、ＣＣＤ[Charge Coupled Device]等を用いたカメラである。カメラは、車両の前方又は後方に取り付けられる。この際、カメラは、その光軸方向が車両の進行方向と一致するように取り付けられる。カメラでは、一定時間毎に、車両の前方又は後方の道路を含む領域を撮像し、その撮像した画像を取得し、その画像情報を画像処理装置に出力する。カメラは、左右方向に撮像範囲が広く、走行している車線を示す左右両側（一対）の白線を十分に撮像可能である。なお、カメラは、カラーのカメラでもよいし、白黒のカメラでもよい。
【００２６】
画像処理装置では、カメラから画像情報を入力する毎に、画像から車両が走行している車線を示す一対の白線を認識する。この認識方法としては、例えば、路面とその上に描かれた白線との輝度差が大きいことからエッジ処理による方法がある。そして、画像処理装置では、認識した一対の白線のうちの一方の白線の曲率ρｃａｍを計算する。また、画像処理装置では、その一方の白線の横位置ｙ（車両から白線までの横方向の距離）を計算する。そして、画像処理装置では、その横位置ｙの時系列データを用いて横速度ｖを計算する。さらに、画像処理装置では、その横速度ｖの時系列データを用いて横加速度αを計算する。また、画像処理装置では、白線を認識でき、曲率を検知できた場合には曲率ロストフラグｆｌａｇ＿ｌｏｓｔに０を設定し、白線を認識できず、曲率を検知できなかった場合（ロストした場合）には曲率ロストフラグｆｌａｇ＿ｌｏｓｔに１を設定する。そして、カメラセンサ１０では、一定時間毎に、曲率ロストフラグｆｌａｇ＿ｌｏｓｔと、検知できた場合には曲率ρｃａｍ、横位置ｙ、横速度ｖ、横加速度αの各データをＥＣＵ２０に出力する。なお、曲率ρｃａｍ、横位置ｙ、横速度ｖ、横加速度αはカメラが撮像した前方（又は後方）の画像を用いているので、曲率ρｃａｍ、横位置ｙ、横速度ｖ、横加速度αが検知される位置は自車両前方（又は後方）の所定位置であるので、これらの前方（又は後方）の検知位置と自車両位置とでは時間差がある。
【００２７】
舵角センサ１１は、運転者による操舵に応じて変化する転舵輪の舵角を検出するセンサである。舵角センサ１１では、一定時間毎に、舵角δを検出し、その舵角δのデータをＥＣＵ２０に出力する。横加速度センサ１２は、車両の横加速度を検出するセンサである。横加速度センサ１２では、一定時間毎に、横加速度ａを検出し、その横加速度ａのデータをＥＣＵ２０に出力する。車速センサ１３は、車両の速度を検出するセンサである。車速センサ１３では、一定時間毎に、車速Ｖを検出し、その車速ＶのデータをＥＣＵ２０に出力する。
【００２８】
ＥＣＵ２０は、ＣＰＵ[CentralProcessing Unit]、ＲＯＭ[Read Only Memory]、ＲＡＭ[Random Access Memory]等からなる電子制御ユニットである。ＥＣＵ２０では、一定時間毎に、各センサ１０，１１，１２，１３からのデータをそれぞれ入力し、その入力した車両走行データを用いて白線の曲率ρを推定する。一定時間毎に入力するデータは、上記したように、カメラセンサ１０からの曲率ρｃａｍ、横位置ｙ、横速度ｖ、横加速度α、曲率ロストフラグｆｌａｇ＿ｌｏｓｔ、舵角センサ１１からの舵角δ、横加速度センサ１２からの横加速度ａ、車速センサ１３からの車速Ｖである。
【００２９】
ＥＣＵ２０では、起動時に、予め設定されている設定値を読み込む。設定値としては、スタビリティファクタＡ、ステアリングギヤ比Ｎ、ホイールベースｌ、カメラ〜曲率測定点間距離Ｌ、自車両位置での曲率閾値ρｏ、カメラセンサ１０で検知される前方位置（又は後方位置）での曲率閾値ρｏｃａｍ、蛇行減衰比ζ０（例えば、０．５）、クロソノイド発散係数ζｐ（例えば、０．５）、クロソノイド減衰係数ζｎ（例えば、０．５）、蛇行周波数観測時定数Ｔ、舵角に対する横加速度の時定数τ、曲率分散σρ２、横加速度分散σａ２、横速度分散σｖ２、横位置分散σｙ２、ロスト時重み付け係数の底Ｗ＿ｌｏｓｔ（＜＜１）、直線路重み付け係数Ｗ＿ｓｔｒ（＞＞１）がある。これらの各設定値は、ホイールベース等の車両固有の値についてはその値が設定され、それ以外の値については実験等によって予め設定され、ＥＣＵ２０内にＲＯＭに記憶されている。
【００３０】
また、ＥＣＵ２０では、起動時に、初期値を設定する。初期値としては、横速度の２乗平均値の増分Δｖ２＊として０を設定し、横加速度の２乗平均値の増分Δａ２＊として０を設定し、トランジェントフラグ（カーブ出入口フラグ）ｔｒに０を代入し、曲率ρに０を設定し、直線状態フラグｓｔｒａｉｇｈｔに０を設定する。ｔｒは、０／１のフラグであり、１の場合がカーブ出入口である。ｓｔｒａｉｇｈｔは、０／１のフラグであり、１の場合が直線路である。なお、増分Δは、前回値からの増分である。
【００３１】
ＥＣＵ２０では、横速度ｖの２乗値と横加速度ａの２乗値についての積分値を逐次的にそれぞれ求め、その各積分値の比（横加速度と横速度（白線のヨー角）の各値のオーバオールパワー比）から蛇行周波数ωを求める。具体的には、横速度の２乗平均値ｖ２＊（前回値）と今回入力した横速度ｖ及び蛇行周波数観測時定数Ｔを用いて式（３）により横速度の２乗平均値の増分Δｖ２＊を計算し、式（５）に示すように横速度の２乗平均値ｖ２＊（前回値）にその増分Δｖ２＊を加算して横速度の２乗平均値ｖ２＊の今回値とする。また、横加速度の２乗平均値ａ２＊（前回値）と今回入力した横加速度ａ及び蛇行周波数観測時定数Ｔを用いて式（４）により横加速度の２乗平均値の増分Δａ２＊を計算し、式（６）に示すように横加速度の２乗平均値ａ２＊（前回値）にその増分Δａ２＊を加算して横加速度の２乗平均値ａ２＊の今回値とする。そして、その横速度の２乗平均値ｖ２＊（今回値）と横加速度の２乗平均値ａ２＊（今回値）を用いて、式（７）により蛇行周波数ωを計算する。
【数２】
2013125327.tif 000004
【００３２】
また、ＥＣＵ２０では、カメラ画像による曲率ρｃａｍの検知位置（前方位置）と舵角δの検知位置（自車両位置）との時間差を調整するために、位相調整を行う。具体的には、位相調整した曲率ρｆｉｌｔ（前回値）と今回入力したカメラ画像による曲率ρｃａｍと車速Ｖ及びカメラ〜曲率測定点間距離を用いて式（８）により曲率の増分Δρｆｉｌｔを計算する。そして、式（９）に示すようにカメラ画像による曲率ρｃａｍの増分Δρｃａｍにその増分Δρｆｉｌｔを加算した値を、位相調整した曲率ρｆｉｌｔの今回値とする。位相調整した曲率ρｆｉｌｔ（今回値）は、今回検知されたカメラ画像による前方位置での曲率ρｃａｍが自車両位置になるように位相調整された値である。
【数３】
2013125327.tif 000005
【００３３】
また、ＥＣＵ２０では、今回入力した車速Ｖ及びスタビリティファクタＡ、ホイールベースｌ、ステアリングギヤ比Ｎを用いて、式（１０）により舵角に対する横加速度ゲインＧを計算する。
【数４】
2013125327.tif 000006
【００３４】
ＥＣＵ２０では、カーブの出入口を判定してトランジェントフラグｔｒを設定し、カーブ出入口の場合（ｔｒ＝１の場合）には蛇行減衰比ζと蛇行周波数ωを設定する。具体的には、式（１１）の条件式で示すように、カメラ検知の前方位置での曲率ρｃａｍの絶対値が閾値ρｏｃａｍより大きいか否かを判定する。曲率ρｃａｍの絶対値が閾値ρｏｃａｍより大きい場合には前方位置では白線が円を示す曲率であり、曲率ρｃａｍの絶対値が閾値ρｏｃａｍ以下の場合には前方位置では白線が直線を示す曲率である。さらに、式（１２）の条件式で示すように、自車両位置での曲率ρの絶対値が閾値ρｏより大きいか否かを判定する。曲率ρの絶対値が閾値ρｏより大きい場合には自車両位置では白線が円を示す曲率であり、曲率ρの絶対値が閾値ρｏ以下の場合には自車両位置では白線が直線を示す曲率である。
【数５】
2013125327.tif 000007
【００３５】
したがって、曲率ρｃａｍの絶対値が閾値ρｏｃａｍより大きい場合かつ曲率ρの絶対値が閾値ρｏより大きい場合、（１３）に示すように、円から円であり（カーブ走行中）、トランジェントフラグｔｒとして０を設定する。曲率ρｃａｍの絶対値が閾値ρｏｃａｍより大きい場合かつ曲率ρの絶対値が閾値ρｏ以下の場合、（１４）に示すように、円から直線であり（カーブ出口走行中）、トランジェントフラグｔｒとして１を設定し、蛇行減衰比ζとしてクロソノイド減衰係数ζｎを設定し、蛇行周波数ωとして１を設定する。曲率ρｃａｍの絶対値が閾値ρｏｃａｍ以下の場合かつ曲率ρの絶対値が閾値ρｏより大きい場合、（１５）に示すように、直線から円であり（カーブ入口走行中）、トランジェントフラグｔｒとして１を設定し、蛇行減衰比ζとしてクロソノイド発散係数ζｐを設定し、蛇行周波数ωとして１を設定する。曲率ρｃａｍの絶対値が閾値ρｏｃａｍ以下の場合かつ曲率ρの絶対値が閾値ρｏ以下の場合、（１６）に示すように、直線から直線であり（直線走行中）、トランジェントフラグｔｒとして０を設定し、直線状態フラグｓｔｒａｉｇｈｔとして１を設定する。
【００３６】
ＥＣＵ２０では、各設定値、各センサ１０，１１，１２，１３から入力した曲率ロストフラグｆｌａｇ＿ｌｏｓｔ、横速度ｖ、横位置ｙ、舵角δ、横加速度ａ、車速Ｖ、計算した蛇行周波数ωと舵角に対する横加速度ゲインＧ、位相調整した曲率ρｆｉｌｔ、設定したトランジェントフラグｔｒ、直線状態フラグｓｔｒａｉｇｈｔ、蛇行減衰比ζを用いて、カルマンフィルタのカルマンゲインを計算し、このカルマンゲインに基づいてカルマンゲインを駆動し、自車両位置での曲率ρを推定する。以下に、ＥＣＵ２０で用いるカルマンフィルタについて具体的に説明する。
【００３７】
カルマンフィルタは、トランジェントフラグｔｒ＝０の場合（カーブ出入口でない場合）には上記の式（１）のモデル式が適用され、トランジェントフラグｔｒ＝１の場合（カーブ出入口の場合）には上記の式（２）のモデル式が適用されるように構成される。また、カルマンフィルタは、直線状態フラグｓｔｒａｉｇｈｔ＝０の場合（直線路の場合）にはフィルタが作用しないように構成される。また、カルマンフィルタは、曲率ロストフラグｆｌａｇ＿ｌｏｓｔ＝１の場合（カメラ検知で曲率が検知されなかった場合）にはカルマンフィルタの推定機能が重視されるように構成される。
【００３８】
以上の条件を満たすようにカルマンフィルタを構成する。カルマンフィルタの状態方程式は、式（１７）である。状態方程式（１７）における状態量ｘは、式（１８）で示す各値である。状態方程式（１７）における入力量ｕは、式（１９）で示すように、舵角δである。状態方程式（１７）におけるＡは、式（２０）で示す４×４行列となる。状態方程式（１７）におけるＢは、式（２１）で示す４次元ベクトルとなる。
【数６】
2013125327.tif 000008
【００３９】
また、プロセスノイズＰは、式（２２）である。システムノイズＱは、式（２３）である。このプロセスノイズＰとシステムノイズＱのバランスによって、カルマンフィルタにおける修正機能と推定機能との重視する度合いが変わる。なお、σδ２は舵角δの分散である。
【数７】
2013125327.tif 000009
【００４０】
カルマンフィルタの出力方程式は、式（２４）である。出力方程式（２４）における観測量ｚは、式（２５）で示す各値である。特に、この中の舵角δが出力となる。出力方程式（２５）におけるＣは、式（２６）で示す４×４行列となる。なお、ｒは白線に対する車両のヨー角速度であり、θは白線に対する車両のヨー角である。
【数８】
2013125327.tif 000010
【００４１】
トランジェントフラグｔｒ＝１の場合、式（２０）の行列における−（１−ｔｒ）ω＝０となり、上記の式（２）のモデル式が適用される。トランジェントフラグｔｒ＝０の場合、式（２０）の行列における−（１−ｔｒ）ω＝−ωとなり、上記の式（１）のモデル式が適用される。
【００４２】
曲率ロストフラグｆｌａｇ＿ｌｏｓｔ＝１の場合（カメラ画像で曲率を検知できなかった場合）、式（２２）においてＰがほぼ０となる。曲率ロストフラグｆｌａｇ＿ｌｏｓｔ＝０の場合（カメラ画像で曲率を検知でき場合）、式（２２）においてＰ＝Ｗ＿ｓｔｒｓｔｒａｉｇｈｔとなる。これによって、プロセスノイズＰとシステムノイズＱとの比が変わるので、カルマンフィルタの推定機能と修正機能との重視する度合いが変わり、特に、カメラ画像で曲率を検知できなかった場合には推定機能が重視されることになる。
【００４３】
曲率ロストフラグｆｌａｇ＿ｌｏｓｔ＝０の場合に直線状態フラグｓｔｒａｉｇｈｔ＝１の場合（直線路の場合）、式（２２）においてＰ＝Ｗ＿ｓｔｒとなる。曲率ロストフラグｆｌａｇ＿ｌｏｓｔ＝０の場合に直線状態フラグｓｔｒａｉｇｈｔ＝０の場合（カーブ路の場合）、式（２２）においてＰ＝１となる。これによって、直線路の場合にはカルマンフィルタが作用しなくなり、カーブ路の場合にはカルマンフィルタが作用する。
【００４４】
そして、式（２０）によってＡ、式（２１）によってＢ、式（２６）によってＣ、式（２２）によってＰ、式（２４）によってＱを求めると、カルマンゲインはＡ、Ｂ、Ｃ、Ｐ、Ｑから一意に決まる。そして、この求めたＡ、Ｂ、Ｃ、Ｐ、Ｑに用いてカルマンフィルタを駆動する。その結果、式（２５）に示す各観測値が得られ、その中の曲率ρを出力する。
【００４５】
図１及び図２を参照して、曲率推定装置１の動作を説明する。特に、ＥＣＵ２０における処理については、図３のフローチャートに沿って説明する。図３は、図１のＥＣＵにおける曲率推定処理の流れを示すフローチャートである。曲率推定装置１は、イグニッションスイッチがＯＮすると起動し、イグニッションスイッチがＯＦＦすると停止し、イグニッションスイッチがＯＮ中は以下の動作を所定時間毎に繰り返し行う。
【００４６】
カメラセンサ１０では、カメラで自車両の前方を撮像し、画像処理装置で撮像画像に基づいて曲率ρｃａｍ、横位置ｙ、横速度ｖ、横加速度αを計算するとともに曲率ロストフラグｆｌａｇ＿ｌｏｓｔを設定し、曲率ρｃａｍ、横位置ｙ、横速度ｖ、横加速度α及び曲率ロストフラグｆｌａｇ＿ｌｏｓｔの各データをＥＣＵ２０に出力する。
【００４７】
舵角センサ１１では、舵角δを検出し、舵角δのデータをＥＣＵ２０に出力する。横加速度センサ１２では横加速度ａを検出し、横加速度ａのデータをＥＣＵ２０に出力する。車速センサ１３では、車速Ｖを検出し、車速ＶのデータをＥＣＵ２０に出力する。
【００４８】
起動時に、ＥＣＵ２０では、上記した各設定値を読み込む（Ｓ１）。また、ＥＣＵ２０では、上記した各初期値を設定する（Ｓ２）。
【００４９】
ＥＣＵ２０では、カメラセンサ１０、舵角センサ１１、横加速度センサ１２、車速センサ１３から各データが出力される毎に、その各データを読み込む（Ｓ３）。
【００５０】
ＥＣＵ２０では、今回読み込んだ横速度ｖを用いて式（３）によって横速度の２乗平均値の増分Δｖ２＊を計算し、その増分Δｖ２＊と横速度の２乗平均値ｖ２＊（前回値）を用いて式（５）によって横速度の２乗平均値ｖ２＊（今回値）を計算する（Ｓ４）。また、ＥＣＵ２０では、今回読み込んだ横加速度ａを用いて式（４）によって横加速度の２乗平均値の増分Δａ２＊を計算し、その増分Δａ２＊と横加速度の２乗平均値ａ２＊（前回値）を用いて式（６）によって横加速度の２乗平均値ａ２＊（今回値）を計算する（Ｓ４）。そして、ＥＣＵ２０では、その横速度の２乗平均値ｖ２＊（今回値）と横加速度の２乗平均値ａ２＊（今回値）を用いて式（７）によって蛇行周波数ωを計算する（Ｓ４）。
【００５１】
また、ＥＣＵ２０では、今回読み込んだカメラ画像による曲率ρｃａｍと車速Ｖを用いて式（８）によって位相調整した曲率の増分Δρｆｉｌｔを計算する。そして、ＥＣＵ２０では、その増分Δρｆｉｌｔを用いて式（９）によってカメラ画像による曲率ρｃａｍを位相調整した曲率ρｆｉｌｔ（今回値）を計算する（Ｓ４）。
【００５２】
また、ＥＣＵ２０では、今回読み込んだ車速Ｖを用いて式（１０）によって舵角に対する横加速度ゲインＧを計算する（Ｓ４）。
【００５３】
ＥＣＵ２０では、今回読み込んだ前方位置での曲率ρｃａｍの絶対値が曲率閾値ρｏｃａｍより大きいか否か（条件式（１１））を判定する（Ｓ５）。前方位置での曲率ρｃａｍの絶対値が曲率閾値ρｏｃａｍより大きいと判定した場合、ＥＣＵ２０では、自車両位置での曲率ρの絶対値が閾値ρｏより大きいか否か（条件式（１２））を判定する（Ｓ５）。前方位置での曲率ρｃａｍの絶対値が閾値ρｏｃａｍより大きいかつ自車両位置での曲率ρの絶対値が閾値ρｏより大きいと判定した場合、ＥＣＵ２０では、トランジェントフラグｔｒとして０を設定する（Ｓ５）。前方位置での曲率ρｃａｍの絶対値が閾値ρｏｃａｍより大きいかつ自車両位置での曲率ρの絶対値が閾値ρｏ以下と判定した場合、トランジェントフラグｔｒとして１を設定し、蛇行減衰比ζとしてクロソノイド減衰係数ζｎを設定し、蛇行周波数ωとして１を設定する（Ｓ５）。
【００５４】
また、前方位置での曲率ρｃａｍの絶対値が曲率閾値ρｏｃａｍ以下と判定した場合、ＥＣＵ２０では、自車両位置での曲率ρの絶対値が閾値ρｏより大きいか否か（条件式（１２））を判定する（Ｓ５）。前方位置での曲率ρｃａｍの絶対値が閾値ρｏｃａｍ以下かつ自車両位置での曲率ρの絶対値が閾値ρｏ以下と判定した場合、ＥＣＵ２０では、トランジェントフラグｔｒとして０を設定し、直線状態フラグｓｔｒａｉｇｈｔとして１を設定する（Ｓ５）。前方位置での曲率ρｃａｍの絶対値が閾値ρｏｃａｍ以下かつ自車両位置での曲率ρの絶対値が閾値ρｏより大きいと判定した場合、トランジェントフラグｔｒとして１を設定し、蛇行減衰比ζとしてクロソノイド発散係数ζｐを設定し、蛇行周波数ωとして１を設定する（Ｓ５）。
【００５５】
そして、ＥＣＵ２０では、舵角に対する横加速度の時定数τ、蛇行周波数ω、蛇行減衰比ζ、トランジェントフラグｔｒを用いて式（２０）によってＡを求める（Ｓ６）。また、ＥＣＵ２０では、舵角に対する横加速度の時定数τ、舵角に対する横加速度ゲインＧを用いて式（２１）によってＢを求める（Ｓ６）。また、ＥＣＵ２０では、車速Ｖを用いて式（２６）によってＣを求める（Ｓ６）。また、ＥＣＵ２０では、直線状態フラグｓｔｒａｉｇｈｔ、曲率ロストフラグｆｌａｇ＿ｌｏｓｔ、ロスト時重み付け係数の底Ｗ＿ｌｏｓｔ、直線路重み付け係数Ｗ＿ｓｔｒ、舵角分散σδ２を用いて式（２２）によってプロセスノイズＰを求める（Ｓ６）。また、ＥＣＵ２０では、曲率分散σρ２、横加速度分散σａ２、横速度分散σｖ２、横位置分散σｙ２を用いて式（２３）によってシステムノイズＱを求める（Ｓ６）。そして、ＥＣＵ２０では、このＡ，Ｂ，Ｃ，Ｐ，Ｑに基づいてカルマンゲインを計算する（Ｓ６）。さらに、ＥＣＵ２０では、今回読み込んだ横加速度ａ、車速Ｖ、横速度ｖ、横位置ｙ及び位相調整した曲率ρｆｉｌｔを用いて式（１８）によって状態量ｘを設定するとともに、今回読み込んだ舵角δを用いて式（１９）によって入力量ｕを設定する（Ｓ６）。そして、ＥＣＵ２０では、Ａ，Ｂ，Ｃ，Ｐ，Ｑ（カルマンゲイン）及び状態量ｘと入力量ｕを用いて、式（１７）を状態方程式とし、式（２４）を出力方程式とするカルマンフィルタを駆動し、式（２５）に示す観測量ｚを計算する（Ｓ６）。そして、ＥＣＵ２０では、観測量ｚに含まれる自車両位置での曲率ρを運転支援装置等に出力する（Ｓ７）。
【００５６】
ＥＣＵ２０では、イグニッションスイッチがＯＦＦしたか否かを判定する（Ｓ８）。イグニッションスイッチがＯＮを継続していると判定した場合、ＥＣＵ２０では、所定時間経過後、Ｓ３に戻って上記の処理を繰り返す。イグニッションスイッチがＯＦＦと判定した場合、ＥＣＵ２０では、処理を終了する。
【００５７】
この曲率推定装置１によれば、道路の曲率を運転者の操舵による蛇行と関連付けた曲率モデルを組み込んだカルマンフィルタを構成して曲率ρを推定することにより、カメラ画像から検知した曲率ρｃａｍに含まれるノイズを抑制でき、道路（車線を構成する白線）の曲率を高精度に推定することができる。また、カルマンフィルタによる簡易な構成なので、計算負荷も少ない。
【００５８】
また、曲率推定装置１によれば、各運転者についての操舵特性に応じた蛇行周波数ωを求めて、その蛇行周波数ωを曲率モデルに適用することにより、運転者の操舵特性に応じた曲率モデルを組み込んだカルマンフィルタを用いることができ、道路の曲率をより高精度に推定することができる。
【００５９】
また、曲率推定装置１によれば、カーブ出入口を判定し、カーブ出入口では単調増加又は単調減少の関数となる曲率モデルを組み込んだカルマンフィルタを構成して曲率ρを推定することにより、道路の曲率をより高精度に推定することができる。
【００６０】
また、曲率推定装置１によれば、直線路か否かを判定し、直線路ではカメラ画像から検知した曲率ρｃａｍに対してカルマンフィルタを働かせないので、直線路でカルマンフィルタを用いた場合の誤差を抑制できる。
【００６１】
以上、本発明に係る実施の形態について説明したが、本発明は上記実施の形態に限定されることなく様々な形態で実施される。
【００６２】
例えば、本実施の形態では曲率推定装置に適用したが、レーンキープ装置等の曲線情報を利用する各種装置に曲線推定機能として組み込まれる構成としてもよい。
【００６３】
また、本実施の形態では観測量の一例を示したが、他の観測量でもよい。但し、観測量には曲率ρが含まれることが必須である。また、本実施の形態では運転者毎に蛇行周波数を求める構成としたが、実験等によって求めた一般的な運転者の蛇行周波数を設定してもよい。
【００６４】
また、本実施の形態ではカーブ出入口の判定方法の一例を示したが、他の方法でカーブ出入口を判定してもよい。また、カーブ出入口の場合、カーブの曲率モデル式と異なる曲率モデル式を用いたが、同じモデル式を適用してもよい。
【符号の説明】
【００６５】
１…曲率推定装置、１０…カメラセンサ、１１…舵角センサ、１２…横加速度センサ、１３…車速センサ、２０…ＥＣＵ。
【図１】
2013125327.tif 000011
【図２】
2013125327.tif 000012
【図３】
2013125327.tif 000013