(57)【要約】【課題】ＬＱ制御を用いたフィードバック制御において、状態量に対する誤差による制御への影響を抑制する走行制御装置を提供することを課題とする。【解決手段】車両の状態量にかかるフィードバックゲインをＬＱ制御の線形二次形式評価関数に基づいて算出する走行制御装置１であって、ＬＱ制御の線形二次形式評価関数で用いる状態量にかかる重みを状態量の検出精度が低いほど小さく設定する設定手段９ｂを備えることを特徴とし、検出精度が低下している状態量のフィードバック量を少なくし、その状態量に対する誤差による制御への影響を抑制する。【選択図】図１
2007302204.tif 000002
【特許請求の範囲】
【請求項１】
車両の状態量にかかるフィードバックゲインをＬＱ制御の線形二次形式評価関数に基づいて算出する走行制御装置であって、
ＬＱ制御の線形二次形式評価関数で用いる状態量にかかる重みを状態量の検出精度が低いほど小さく設定する設定手段を備えることを特徴とする走行制御装置。
【請求項２】
状態量の検出精度を評価する評価手段を備えることを特徴とする請求項１に記載する走行制御装置。
【請求項３】
前記評価手段は、状態量の検出精度としてカルマンフィルタの推定誤差共分散行列における状態量に対応する対角値を評価し、
前記設定手段は、前記評価手段で評価した対角値が大きいほど、ＬＱ制御の線形二次形式評価関数で用いる状態量にかかる重みを小さく設定することを特徴とする請求項２に記載する走行制御装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ＬＱ[Linear Quadratic]制御を用いてフィードバック制御を行う走行制御装置に関する。
【背景技術】
【０００２】
走行制御装置としては、運転者を支援するために、レーンキープ装置などの様々な装置が開発されている。レーンキープ装置では、車両が車線に沿って走行するように、車両の状態量（目標軌跡に対する横偏差、ヨー角など）をフィードバックすることによって制御量（操舵角）を求め、制御量に応じた操舵制御を行う。特に、フィードバック制御における収束性と応答性を両立させるとともに制御の安定性を向上させるために、ＬＱ制御を用いるものがある（非特許文献１参照）。ＬＱ制御では、状態量と制御量を含む線形二次形式評価関数を最小化するようなフィードバックゲインを算出し、そのフィードバックゲインと状態量から制御量を算出する。
【非特許文献１】毛利宏、古性裕之著、「自動車線追従走行の検討」、日本機械学会第６回交通・物流部門大会講演論文集（部門大会編）、１９９７年７月２８〜３０日
【発明の開示】
【発明が解決しようとする課題】
【０００３】
車両の実際の状態量はセンサなどによって検出されるので、検出精度が低いほど状態量に誤差が含まれる場合がある。状態量に誤差が含まれる場合、その状態量をフィードバックすることにより、誤差を含む制御量が算出される。
【０００４】
そこで、本発明は、ＬＱ制御を用いたフィードバック制御において、状態量に対する誤差による制御への影響を抑制する走行制御装置を提供することを課題とする。
【課題を解決するための手段】
【０００５】
本発明に係る走行制御装置は、車両の状態量にかかるフィードバックゲインをＬＱ制御の線形二次形式評価関数に基づいて算出する走行制御装置であって、ＬＱ制御の線形二次形式評価関数で用いる状態量にかかる重みを状態量の検出精度が低いほど小さく設定する設定手段を備えることを特徴とする。
【０００６】
この走行制御装置では、ＬＱ制御を利用し、車両の状態量と車両への制御量を含む線形二次形式評価関数を最小化するフィードバックゲインを求め、その求めたフィードバックゲインと状態量から制御量を求める。特に、走行制御装置では、設定手段により、ある状態量の検出精度が低いほど線形二次形式評価関数におけるその状態量にかかる重みを通常より小さくする。これによって、この走行制御装置では、検出精度が低下している状態量のフィードバック量が少なくなり、その状態量に対する誤差による制御への影響が抑制される。また、この走行制御装置では、ＬＱ制御による制御の安定性が確保され、フィードバック制御における収束性と応答性も向上する。
【０００７】
なお、状態量の検出精度については、予め設定された固定値でもよいしあるいは車両走行中に求められる可変値でもよい。したがって、状態量にかかる重みも、検出精度が固定値の場合には予め設定され、検出精度が可変値の場合には値が変わる毎に設定される。
【０００８】
本発明の上記走行制御装置では、状態量の検出精度を評価する評価手段を備える構成としてもよい。
【０００９】
この走行制御装置では、評価手段により各状態量の検出精度を評価し、設定手段によりその評価された検出精度を応じて各状態量にかかる重みを設定する。このように、走行制御装置では、評価手段を備えることにより車両走行中に状態量の検出精度が変化する場合でもその検出精度の変化を動的に検出することができ、状態量にかかる重みも動的に変えることができる。その結果、状態量の検出状況に応じて、状態量に対する誤差による制御への影響をより抑制することができる。
【００１０】
本発明の上記走行制御装置では、評価手段は、状態量の検出精度としてカルマンフィルタの推定誤差共分散行列における状態量に対応する対角値を評価し、設定手段は、評価手段で評価した対角値が大きいほど、ＬＱ制御の線形二次形式評価関数で用いる状態量にかかる重みを小さく設定する構成としてもよい。
【００１１】
この走行制御装置では、カルマンフィルタを利用し、各検出手段による検出量から各状態量を推定する。走行制御装置では、評価手段により、各状態量の検出精度としてカルマンフィルタの推定誤差共分散行列における各状態量に対応する対角値で評価する。そして、走行制御装置では、設定手段により各状態量に対応する対角値が大きいほど（つまり、検出精度が低いほど）、線形二次形式評価関数におけるその状態量にかかる重みを通常より小さくする。このように、走行制御装置では、カルマンフィルタを利用することにより、各検出手段による検出値から各状態量を推定できるとともに、その各状態量に対する確からしさ（推定精度、信頼度）も得ることができる。
【発明の効果】
【００１２】
本発明は、状態量の検出精度に応じてＬＱ制御の線形二次形式評価関数におけるその状態量にかかる重みを設定することにより、状態量に対する誤差による制御への影響を抑制することができる。
【発明を実施するための最良の形態】
【００１３】
以下、図面を参照して、本発明に係る走行制御装置の実施の形態を説明する。
【００１４】
本実施の形態では、本発明に係る走行制御装置を、車両に搭載されるレーンキープ装置に適用する。本実施の形態に係るレーンキープ装置は、車線（一対の白線）に沿って走行するように（例えば、車両重心が車線の中心線上になるように）、操舵制御を行う。特に、本実施の形態に係るレーンキープ装置では、カルマンフィルタによって車両の状態量を推定し、ＬＱ制御によってその推定した状態量をフィードバック制御する。
【００１５】
図１〜図３を参照して、本実施の形態に係るレーンキープ装置１について説明する。図１は、本実施の形態に係るレーンキープ装置の構成図である。図２は、本実施の形態に係る道路−車両座標系のモデル図である。図３は、本実施の形態に係るＬＱ制御を用いたフィードバック制御の概要図である。
【００１６】
レーンキープ装置１は、４つの車両の運動状態（観測量）と３つの車両への入力（入力量）を検出し、カルマンフィルタにより４つの観測量と３つの入力量から４つの車両の状態量を推定する。そして、レーンキープ装置１では、ＬＱ制御により、カルマンフィルタによる４つの状態量と制御量（操舵角）を含む線形二次形式評価関数を最小化するフィードバックゲインを求め、そのフィードゲインと状態量から制御量を求める。特に、レーンキープ装置１では、観測量や入力量の各検出誤差（ひいては、状態量の推定誤差）による制御量への影響を抑制するために、カルマンフィルタの推定誤差共分散行列における各状態量に対応する対角値を用いて、線形二次形質評価関数における各状態量に対する重みに各状態量の推定誤差を常時反映させる。そのために、レーンキープ装置１は、ＧＰＳ[Global Positioning System]受信機２、白線認識センサ３、磁気センサ４、ヨーレートセンサ５、操舵角センサ６、道路マップデータベース７、操舵アクチュエータ８及びＥＣＵ[Electronic Control Unit]９を備えている。
【００１７】
ＧＰＳ受信機２は、ＧＰＳアンテナや信号処理部などを備えている。ＧＰＳ受信機２では、アンテナでＧＰＳ衛星からの電波を受信し、信号処理部で３個以上（高度も求める場合には４個以上）のＧＰＳ衛星からの電波により緯度、経度を算出する。さらに、ＧＰＳ受信機２では、道路マップデータベース７に格納されている現在位置の緯度、経度（車線中心）を用いて、道路マップデータの車線中心からのＧＰＳによる緯度、経度の横偏差Ｄｇｐｓを算出し、その横偏差Ｄｇｐｓを示すＧＰＳ横偏差信号をＥＣＵ９に送信する。なお、車両にナビゲーションシステムが搭載される場合、ナビゲーションシステムのＧＰＳ受信機を共有する。
【００１８】
白線認識センサ３は、カメラや画像処理部などを備えている。白線認識センサ３では、カメラで車両の前方の道路を撮像し、画像処理部でその撮像画像から左右一対の白線を認識し、一対の白線の中心線を算出する。さらに、白線認識センサ３では、その左右一対の白線の中心線からの車両の横偏差Ｄｗｌを算出し、その横偏差Ｄｗｌを示す白線横偏差信号をＥＣＵ９に送信する。
【００１９】
磁気センサ４では、車線中心に埋め込まれている磁気マーカからの磁束密度を検出する。そして、磁気センサ４では、その検出した磁束密度に基づいて車線中心（つまり、磁気マーカの位置）からの車両の横偏差Ｄｍａｇを算出し、その横偏差Ｄｍａｇを示す磁気マーカ横偏差信号をＥＣＵ９に送信する。
【００２０】
ヨーレートセンサ５では、車両に作用しているヨーレートγを検出する。そして、ヨーレートセンサ５では、その検出したヨーレートγを示すヨーレート信号をＥＣＵ９に送信する。
【００２１】
これらのＧＰＳ受信機２で検出した横偏差Ｄｇｐｓ、白線認識センサ３で検出した横偏差Ｄｗｌ、磁気センサ４で検出した横偏差Ｄｍａｇ、ヨーレートセンサ５で検出したヨーレートγは、車両の運動状態を示し、４つの観測量である。
【００２２】
図２には、車両座標系と道路座標系を示している。車両座標系は、車両の重心ＣＧを原点とし、車両の前後方向がｘ軸であり、左右方向がｙ軸である。道路座標系は、車両の重心ＣＧから目標軌跡ＴＣへ下ろした垂点を原点とし、目標軌跡ＴＣに対する接線がξ軸であり、その接線に対する垂線がη軸である。目標軌跡ＴＣは、レーンキープを行う際に車両を走行させる目標となる軌跡であり、例えば、左右一対の白線（車線）の中心線である。各横偏差Ｄｇｐｓ、横偏差Ｄｗｌ、横偏差Ｄｍａｇは、道路座標系におけるη座標の値であり、横偏差Ｄｇｐｓの場合にはＧＰＳ受信機２のアンテナの配設位置の横偏差であり、横偏差Ｄｗｌの場合には白線認識センサ３のカメラの配設位置の横偏差であり、横偏差Ｄｍａｇの場合には磁気センサ４の配設位置の横偏差である。
【００２３】
操舵角センサ６では、車両の操舵角δを検出する。そして、操舵角センサ６では、その検出した操舵角δを示す操舵角信号をＥＣＵ９に送信する。
【００２４】
道路マップデータベース７は、道路マップに関する様々なデータが格納されたデータベースであり、例えば、道路（車線）の形状、道路の位置（緯度、経度）、道路の曲率(道路半径)、道路のカントである。ＥＣＵ９では、道路マップデータベース７を参照し、現在位置走行中の道路の曲率κとカントαを抽出する（図２参照）。なお、車両にナビゲーションシステムが搭載される場合にはナビゲーションシステムの道路マップデータベースを利用するが、ナビゲーションシステムが搭載されていない場合にはＥＣＵ９に道路マップデータベース７を構成する。
【００２５】
これらの操舵角センサ６で検出した操舵角δ、道路マップデータベース７から抽出される道路の曲率κとカントαは、車両への入力を示し、３つの入力量である。
【００２６】
操舵アクチュエータ８は、操舵機構における操舵角を変化させるアクチュエータである。操舵アクチュエータ８では、ＥＣＵ９から操舵制御信号を受信し、操舵制御信号に示される操舵角δｃになるように操舵角を変化させる。
【００２７】
ＥＣＵ９は、ＣＰＵ[Central Processing Unit]、ＲＯＭ[Read Only Memory]、ＲＡＭ[Random Access Memory]などからなり、ＲＯＭに格納される各プログラムをＣＰＵで実行することによってカルマンフィルタ（状態推定部）９ａ及びＬＱ制御部９ｂが構成される。ＥＣＵ９では、一定時間毎に、カルマンフィルタ９ａにより、式（１）で表される４次元ベクトルの車両の観測量Ｙと式（２）で表される３次元ベクトルの車両への入力量Ｕを取り入れ、式（３）で表される４次元ベクトルの車両の状態量Ｘ（横偏差η、横速度ｄη／ｄｔ、ヨー角θ、ヨーレートγ）を推定するとともにカルマンフィルタの推定誤差共分散行列ＰＫを求める（図１参照）。そして、ＥＣＵ９では、一定時間毎に、ＬＱ制御部９ｂにより、推定誤差共分散行列ＰＫの各対角値を利用してＬＱ制御の線形二次形式評価関数における状態量Ｘにかかる重み行列ＱＬＱを設定し、線形二次形式評価関数を最小にするフィードバックゲインＫＬＱを求める（図３参照）。さらに、ＥＣＵ９では、一定時間毎に、フィードバックゲインＫＬＱと状態量Ｘから制御量（操舵角δｃ）を求め、この操舵角δｃになるように操舵アクチュエータ８を制御する。なお、本実施の形態ではＬＱ制御部９ｂが特許請求の範囲に記載する設定手段に相当し、カルマンフィルタ９ａが特許請求の範囲に記載する評価手段に相当する。
【００２８】
【数１】
2007302204.tif 000003
【００２９】
図２に示すように、状態量Ｘの横偏差ηは車両の重心ＣＧの道路座標系におけるη座標の値であり、横速度ｄη／ｄｔは横偏差ηの時間微分値である。状態量Ｘのヨー角θは、車両の前後方向の目標軌跡ＴＣに対する傾きであり、道路座標系のξ軸と車両座標系のｘ軸とのなす角となる。状態量Ｘのヨーレートγは、車両の重心ＣＧ周りのヨーレートである。
【００３０】
カルマンフィルタについて説明する。カルマンフィルタでは、入力量Ｕ及び推定する状態量Ｘを用いて、式（４）で表される状態方程式を立てる。この状態方程式には、外乱入力によるシステムノイズベクトルＷが加味される。
【００３１】
【数２】
2007302204.tif 000004
【００３２】
状態方程式の行列Ａは、式（５）で表される。式（５）の行列ＡにおけるＡ１は式（６）で求められ、Ａ２は式（７）で求められ、Ａ３は式（８）で求められ、Ａ４は式（９）で求められる。状態方程式の行列Ｂは、式（１０）で表される。状態方程式の行列Ｇは、式（１１）で表される。システムノイズベクトルＷは、式（１２）で表され、横偏差η、横速度ｄη／ｄｔ、ヨー角θ、ヨーレートγに対応して各値が設定される。このシステムノイズベクトルＷの各値は、予め設定された固定値でもよいし、あるいは、外乱の変化などに応じて設定される可変値としてもよい。
【００３３】
なお、Ｋｆは前輪側のコーナリングパワー（２輪分）であり、Ｋｒは後輪側のコーナリングパワー（２輪分）である。Ｉは、ヨー慣性モーメントである。ｌｆは前輪の車軸と車両の重心ＣＧ間の距離であり、ｌｒは後輪の車軸と車両の重心ＣＧ間の距離である（図２参照）。ｍは、車両重量である。Ｖは、車速である。ｇは、重力加速度である。Ｋｆ，Ｋｒ，Ｉ，ｍは、車両のジオメトリやタイヤ特性から予め設定された固定値でもよいし、あるいは、車両の荷重などの変化に応じて設定される可変値としてもよい。Ｖは、図示しない車速センサにおける検出値である。ｌｒ，ｌｆは、車両のジオメトリから決まる固定値である。
【００３４】
また、カルマンフィルタでは、観測量Ｙと入力量Ｕ及び推定する状態量Ｘを用いて、式（１３）で示す観測方程式を立てる。この観測方程式には、各センサ２〜５での観測における観測ノイズベクトルＶが加味される。なお、カルマンフィルタにおけるノイズは、ホワイトノイズが前提である。
【００３５】
【数３】
2007302204.tif 000005
【００３６】
状態方程式の行列Ｃは、式（１４）で表される。状態方程式の行列Ｄは、式（１５）で表される。観測ノイズベクトルＶは、式（１６）で表され、磁気センサ４による横偏差Ｄｍａｇ、ＧＰＳ受信機２による横偏差Ｄｇｐｓ、白線認識センサ３による横偏差Ｄｗｌ、ヨーレートセンサ５によるヨーレートγに対応して各値が設定される。この観測ノイズベクトルＶの各値は、予め設定された固定値でもよいし、あるいは、各観測信号のレベルの変化などに応じて設定される可変値としてもよい。なお、Ｌは、車両の重心ＣＧと観測用のセンサの配設位置間の距離であり、固定値である（図２参照）。
【００３７】
そして、カルマンフィルタでは、今回の入力量Ｕと観測量Ｙを用いて式（４）の状態方程式と式（１３）の観測方程式を解き、今回の状態量Ｘ（横偏差η、横速度ｄη／ｄｔ、ヨー角θ、ヨーレートγ）を求める。
【００３８】
さらに、カルマンフィルタでは、前回予測した推定誤差共分散行列ＰＫ（ｔ／ｔ−１）を用いて、今回の推定誤差共分散行列ＰＫ（ｔ／ｔ）を求めるために、式（１７）に示す共分散方程式を立てる。そして、カルマンフィルタでは、式（１７）の共分散方程式を解き、今回の推定誤差共分散行列ＰＫ（ｔ／ｔ）を決定する。さらに、カルマンフィルタでは、今回決定した推定誤差共分散行列ＰＫ（ｔ／ｔ）を用いて、次回の推定誤差共分散行列ＰＫ（ｔ＋１／ｔ）を予測するために、式（１８）に示す共分散方程式を立てる。そして、カルマンフィルタでは、式（１８）の共分散方程式を解き、次回の推定誤差共分散行列ＰＫ（ｔ＋１／ｔ）を予測する。この次回の推定誤差共分散行列ＰＫ（ｔ＋１／ｔ）は、次回の推定誤差共分散行列ＰＫを決定する際に用いられる。
【００３９】
【数４】
2007302204.tif 000006
【００４０】
推定誤差共分散行列ＰＫは、式（１９）で表される。推定誤差共分散行列ＰＫの各対角値は、カルマンフィルタによって推定された横偏差、横速度、ヨー角、ヨーレートに対する各推定誤差（ひいては、これらを推定するために用いた観測量や入力量の検出誤差に相当する）を示す。式（１７）のＫＫ（ｔ）は、カルマンゲインであり、式（２０）によって求められる。式（１７）の行列Ｃは、上記した式（１４）で表される。式（１８）の行列Ａは、上記した式（５）で表される。式（１８）の行列Ｇは、上記した式（１１）で表される。式（１８）の行列Ｑｋは、式（２１）で表され、その対角値がシステムノイズの標準偏差である。行列Ｑｋの各値は、横偏差η、横速度ｄη／ｄｔ、ヨー角θ、ヨーレートγに対応して各値が設定される。この各値は、予め設定された固定値でもよいし、あるいは、外乱の変化などに応じて設定される可変値としてもよい。
【００４１】
【数５】
2007302204.tif 000007
【００４２】
式（１９）の行列内の・は、任意の値を表す。式（２０）の行列Ｃは、上記した式（１４）で表される。式（２０）の行列ＲＫは、式（２２）で表され、その対角値が観測ノイズの標準偏差である。行列ＲＫの各値は、横偏差Ｄｍａｇ、横偏差Ｄｇｐｓ、横偏差Ｄｗｌ、ヨーレートγに対応して各値が設定される。この各値は、予め設定された固定値でもよいし、あるいは、各観測信号のレベルの変化などに応じて設定される可変値としてもよい。行列ＱＫ及び行列ＲＫと上記したシステムノイズベクトルＷ（式（１２））及び観測ノイズベクトルＶ（式（１６））とは、式（２３）の関係にある。式（２３）のＥは、相関演算を意味する記号である。式（２３）のδｔτは、クロネッカーのデルタを意味する記号であり、ｔ＝τのときにはδｔτ＝１であり、ｔ＝τでないときにはδｔτ＝０である。
【００４３】
次に、ＬＱ制御について説明する。ＬＱ制御では、入力量Ｕとカルマンフィルタで推定した状態量Ｘを用いて、式（２４）で表される状態方程式を立てる。この状態方程式による可制御で線形な時不変システムのもとで、ある評価関数を最小にする制御則が線形な状態フィードバック制御則となるためには、式（２５）で表される二次形式とする必要がある。ＬＱ制御では、この式（２５）で表される状態量Ｘと制御量（操舵角）δｃを含む線形二次形式評価関数Ｊを最小にするフィードバックゲインＫＬＱを算出する（図３参照）。ＬＱ制御に入力される状態量Ｘとしては、カルマンフィルタで推定された各値から目標状態を減算した各値が用いられる。ただし、目標状態の各値が０の場合（例えば、横偏差を０にする場合（車両の重心ＣＧを車線の中心線上にする場合））、カルマンフィルタで推定された値がそのまま用いられる。そして、ＬＱ制御では、フィードバックゲインＫＬＱを算出すると、そのフィードバックゲインＫＬＱと状態量Ｘの乗算値によって制御量δｃを求める。
【００４４】
【数６】
2007302204.tif 000008
【００４５】
式（２４）の行列Ａは、上記した式（５）で表される。式（２４）の行列Ｂは、上記した式（１０）で表される。式（２５）の行列ＱＬＱは、状態量Ｘにかかる重みであり、式（２６）で表される。式（２５）のＲＬＱは、制御量δｃにかかる重みである。この重みＲＬＱは、予め設定された固定値でもよいし、あるいは、重みＱＬＱなどに応じて設定される可変値としてもよい。
【００４６】
【数７】
2007302204.tif 000009
【００４７】
式（２６）の重み行列ＱＬＱの各対角値は、状態量Ｘである横偏差、横速度、ヨー角、ヨーレートにそれぞれかかる重みであり、カルマンフィルタで求めた推定誤差共分散行列ＰＫの対角値（ｐｌａｔ，ｐｄｌａｔ，ｐｙａｗ，ｐｙｒ）からそれぞれ求められる。式（２６）におけるｐａｌｌは、推定誤差共分散行列ＰＫの全ての対角値を用いて、式（２７）で算出される。式（２６）におけるｑｌａｔ，ｑｄｌａｔ，ｑｙａｗ，ｑｙｒは、横偏差、横速度、ヨー角、ヨーレートにそれぞれかかる基本重みであり、実験などによって予め決定される。式（２６）におけるｋｌａｔ，ｋｄｌａｔ，ｋｙａｗ，ｋｙｒは、横偏差、横速度、ヨー角、ヨーレートに対する推定誤差共分散行列ＰＫの各対角値の反映度をそれぞれ決める係数であり、実験などによって予め決定される。
【００４８】
推定誤差共分散行列ＰＫの各対角値は、その値が大きいほど、その対角値に対応する状態量Ｘの各値の推定精度（ひいては、これらを推定するために用いた観測量や入力量の検出精度）が低いこと（推定誤差が大きいこと）を示す。また、重みＱＬＱの各対角値は、式（２６）から判るように、推定誤差共分散行列ＰＫの対応する対角値が大きいほど、小さくなる。したがって、状態量Ｘのある値の推定精度が低いほど（推定誤差が大きいほど）、ＬＱ制御における状態量Ｘのその値にかかる重みが小さくなる。つまり、状態量Ｘのある値に対する推定誤差（検出誤差）が大きい場合、ＬＱ制御において状態量Ｘのその値の反映度合いが小さくなり、その値のフィードバック量が少なくなる。
【００４９】
それでは、ＥＣＵ９におけるカルマンフィルタ９ａとＬＱ制御部９ｂで実際に行う処理について説明する。
【００５０】
カルマンフィルタ９ａでは、式（４）の状態方程式、式（１３）の観測方程式を解くために、式（２８）で表されるフィルタ方程式を立てる。カルマンフィルタ９ａでは、今回の観測量Ｙ（ｔ）と前回予測した状態量Ｘ（ｔ／ｔ−１）を用いて、式（２８）のフィルタ方程式を解き、今回の状態量Ｘ（ｔ／ｔ）を決定する。さらに、カルマンフィルタ９ａでは、今回決定した状態量Ｘ（ｔ／ｔ）と今回の入力量Ｕ（ｔ）を用いて、次回の状態量Ｘ（ｔ＋１／ｔ）を予測するために、式（２９）に示すフィルタ方程式を立てる。そして、カルマンフィルタ９ａでは、式（２９）のフィルタ方程式を解き、次回の状態量Ｘ（ｔ＋１／ｔ）を予測する。この次回の状態量Ｘ（ｔ＋１／ｔ）は、次回の状態量Ｘを決定する際に用いられる。
【００５１】
【数８】
2007302204.tif 000010
【００５２】
式（２８）のＫＫ（ｔ）は、今回のカルマンゲインである。カルマンフィルタ９ａでは、式（１４）の行列Ｃ、式（２２）の行列ＲＫ及び前回予測された推定誤差共分散行列ＰＫ（ｔ／ｔ−１）を用いて、上記した式（２０）から今回のカルマンゲインＫＫ（ｔ）を求める。式（２８）の行列Ｃは、上記した式（１４）で表される。式（２９）の行列Ａは、上記した式（５）で表される。式（２９）の行列Ｂは、上記した式（１０）で表される。
【００５３】
さらに、カルマンフィルタ９ａでは、前回予測した推定誤差共分散行列ＰＫ（ｔ／ｔ−１）を用いて、式（１７）の共分散方程式を解き、今回の推定誤差共分散行列ＰＫ（ｔ／ｔ）を決定する。さらに、カルマンフィルタ９ａでは、今回決定した推定誤差共分散行列ＰＫ（ｔ／ｔ）を用いて、式（１８）の共分散方程式を解き、次回の推定誤差共分散行列ＰＫ（ｔ＋１／ｔ）を予測する。このように、カルマンフィルタ９ａでは、今回の状態量Ｘと推定誤差共分散行列ＰＫを求める。
【００５４】
ＬＱ制御部９ｂでは、式（２５）の線形二次形式評価関数Ｊを解くために、状態量Ｘにかかる重み行列ＱＬＱと制御量δｃにかかる重みＲＬＱを用いて、式（３０）で表されるリカッチ方程式を立てる。この際、ＬＱ制御部９ｂでは、カルマンフィルタ９ａで求めた今回の推定誤差共分散行列ＰＫの対角値を用いて、重み行列ＱＬＱ（式（２６））の各対角値を求める。そして、ＬＱ制御部９ｂでは、式（３０）のリカッチ方程式を解き、正定対称行列ＰＬＱを求める。さらに、ＬＱ制御部９ｂでは、式（３１）により、正定対称行列ＰＬＱを用いて、フィードバックゲインＫＬＱを求める。
【００５５】
【数９】
2007302204.tif 000011
【００５６】
式（３０）の行列Ａは、上記した式（５）で表される。式（３０）の行列Ｂは、上記した式（１０）で表される。式（３１）のＲＬＱは、上記した制御量δｃにかかる重みである。
【００５７】
そして、ＬＱ制御部９ｂでは、求めたフィードバックゲインＫＬＱとカルマンフィルタ９ａで求めた今回の状態量Ｘを用いて、制御量（操舵角）δｃを求める。さらに、ＬＱ制御部９ｂでは、車両の操舵角が操舵角δｃになるように操舵制御信号を生成し、この操舵制御信号を操舵アクチュエータ８に送信する。
【００５８】
次に、図１〜図３を参照して、レーンキープ装置１の動作について説明する。
【００５９】
ＧＰＳ受信機２では、各ＧＰＳ衛星からの電波をそれぞれ受信し、各電波を用いて緯度、経度を算出し、この緯度、経度の車線中心からの横偏差Ｄｇｐｓを算出し、その横偏差Ｄｇｐｓを示すＧＰＳ横偏差信号をＥＣＵ９に送信している。白線認識センサ３では、車両の前方の道路を撮像し、その撮像画像から左右一対の白線を認識し、その白線の中心線からの車両の横偏差Ｄｗｌを算出し、その横偏差Ｄｗｌを示す白線横偏差信号をＥＣＵ９に送信している。磁気センサ４では、磁気マーカからの磁束密度を検出し、その磁束密度に基づいて車線中心からの車両の横偏差Ｄｍａｇを算出し、その横偏差Ｄｍａｇを示す磁気マーカ横偏差信号をＥＣＵ９に送信している。ヨーレートセンサ５では、車両に作用しているヨーレートγを検出し、そのヨーレートγを示すヨーレート信号をＥＣＵ９に送信している。
【００６０】
操舵角センサ６では、車両の操舵角δを検出し、その操舵角δを示す操舵角信号をＥＣＵ９に送信している。ＥＣＵ９では、道路マップデータベース７から現在位置走行中の道路（車線）の曲率κとカントαを取り出している。
【００６１】
一定時間毎に、ＥＣＵ９では、式（２０）により、前回予測した推定誤差共分散行列ＰＫ（ｔ／ｔ−１）を用いて、今回のカルマンゲインＫＫ（ｔ）を求める。そして、ＥＣＵ９では、式（２８）のフィルタ方程式により、前回予測した状態量Ｘ（ｔ／ｔ−１）、今回のカルマンゲインＫＫ（ｔ）と入力された今回の観測量Ｙ（ｔ）を用いて、今回の状態量Ｘ（ｔ／ｔ）を求める。さらに、ＥＣＵ９では、式（２９）のフィルタ方程式により、今回の状態量Ｘ（ｔ／ｔ）と今回の入力量Ｕ（ｔ）を用いて、次回の状態量Ｘ（ｔ＋１／ｔ）を予測する。また、ＥＣＵ９では、式（１７）の共分散方程式により、今回のカルマンゲインＫＫ（ｔ）と前回予測した推定誤差共分散行列ＰＫ（ｔ／ｔ−１）を用いて、今回の推定誤差共分散行列ＰＫ（ｔ／ｔ−１）を求める。さらに、ＥＣＵ９では、式（１８）の共分散方程式により、今回の推定誤差共分散行列ＰＫ（ｔ／ｔ）を用いて、次回の推定誤差共分散行列ＰＫ（ｔ＋１／ｔ）を予測する。
【００６２】
ここまでの処理により、今回の観測量Ｙ（Ｄｍａｇ，Ｄｇｐｓ，Ｄｗｌ，γ）から今回の状態量Ｘ（η，ｄη／ｄｔ，θ，γ）が推定され、この状態量Ｘの各値についての誤差が今回の推定誤差共分散行列ＰＫ（ｔ／ｔ）の各対角値で表される。この対角値が大きいほど、誤差が大きく、推定精度（検出精度）が低いことを示す。
【００６３】
次に、ＥＣＵ９では、今回の推定誤差共分散行列ＰＫ（ｔ／ｔ）の各対角値を用いて、式（２６）の重み行列ＱＬＱの各対角値を求める。推定誤差共分散行列ＰＫ（ｔ／ｔ）の各対角値が大きいほど、重み行列ＱＬＱの各対角値は小さくなり、ＬＱ制御における状態量Ｘの各値にかかる重みが小さくなる。
【００６４】
ＥＣＵ９では、式（３０）のリカッチ方程式により、重み行列ＱＬＱを用いて、正定対称行列ＰＬＱを求める。さらに、ＥＣＵ９では、式（３１）により、正定対称行列ＰＬＱを用いて、フィードバックゲインＫＬＱを求める。
【００６５】
そして、ＥＣＵ９では、フィードバックゲインＫＬＱと今回の状態量Ｘを用いて、操舵角δｃを求める。さらに、ＥＣＵ９では、操舵角δｃになるように操舵制御信号を生成し、この操舵制御信号を操舵アクチュエータ８に送信する。この操舵制御信号を受信すると、操舵アクチュエータ８では、操舵角δｃになるように、転舵輪を転舵させる。
【００６６】
以上の動作が繰り返し実行され、車両は、車両重心が左右一対の白線の中心線上になるように走行する。
【００６７】
このレーンキープ装置１では、状態量Ｘの推定誤差共分散行列Ｐｋを求め、推定精度（ひいては、観測量Ｙや入力量Ｕの検出精度）が低いほど線形二次形式評価関数における状態量Ｘにかかる重みを小さくすることにより、精度が低下している状態量のフィードバック量を少なくする。その結果、レーンキープ装置１では、状態量に対する誤差による制御への影響を抑制することができ、高精度なフィードバック制御を行うことができる。また、レーンキープ装置１では、ＬＱ制御を用いているので、制御の安定性が確保され、フィードバック制御における収束性と応答性も向上する。
【００６８】
また、レーンキープ装置１では、カルマンフィルタを用いて状態量Ｘと推定誤差共分散行列Ｐｋを観測量Ｙや入力量Ｕに応じて更新していくので、状態量Ｘの推定精度（ひいては、状態量Ｘに対する重み）として適切な値を毎回求めることができ、より高精度なフィードバック制御を行うことができる。
【００６９】
以上、本発明に係る実施の形態について説明したが、本発明は上記実施の形態に限定されることなく様々な形態で実施される。
【００７０】
例えば、本実施の形態ではレーンキープ制御に適用したが、定速制御、挙動安定化制御、車間制御などの様々な走行制御に適用可能である。
【００７１】
また、本実施の形態では状態量として横偏差、横速度、ヨー角、ヨーレートを推定する構成としたが、状態量としてはこの４つに特に限定するものでない。また、車両の位置を検出するためにＧＰＳによる位置検出、白線認識による位置検出、磁気マーカによる位置検出の３つの手段を用いる構成としたが、この３つのうちの１つあるいは２つの位置検出だけでもよいし、あるいは、他の位置検出で行ってもよい。
【００７２】
また、本実施の形態では評価手段としてカルマンフィルタを適用したが、他の手段によって状態量の精度を評価してもよい。また、センサの検出精度などから状態量の精度が予め判っている場合、評価手段が無くてもよく、状態量に対する重みを予め設定しておくようにしてもよい。
【図面の簡単な説明】
【００７３】
【図１】本実施の形態に係るレーンキープ装置の構成図である。
【図２】本実施の形態に係る道路−車両座標系のモデル図である。
【図３】本実施の形態に係るＬＱ制御を用いたフィードバック制御の概要図である。
【符号の説明】
【００７４】
１…レーンキープ装置、２…ＧＰＳ受信機、３…白線認識センサ、４…磁気センサ、５…ヨーレートセンサ、６…操舵角センサ、７…道路マップデータベース、８…操舵アクチュエータ、９…ＥＣＵ、９ａ…カルマンフィルタ（状態制御部）、９ｂ…ＬＱ制御部
【図１】
2007302204.tif 000012
【図２】
2007302204.tif 000013
【図３】
2007302204.tif 000014