JP2007302204A - 走行制御装置 - Google Patents

Abstract

【課題】ＬＱ制御を用いたフィードバック制御において、状態量に対する誤差による制御への影響を抑制する走行制御装置を提供することを課題とする。
【解決手段】車両の状態量にかかるフィードバックゲインをＬＱ制御の線形二次形式評価関数に基づいて算出する走行制御装置１であって、ＬＱ制御の線形二次形式評価関数で用いる状態量にかかる重みを状態量の検出精度が低いほど小さく設定する設定手段９ｂを備えることを特徴とし、検出精度が低下している状態量のフィードバック量を少なくし、その状態量に対する誤差による制御への影響を抑制する。
【選択図】図１

Description

本発明は、ＬＱ[Linear Quadratic]制御を用いてフィードバック制御を行う走行制御装置に関する。

走行制御装置としては、運転者を支援するために、レーンキープ装置などの様々な装置が開発されている。レーンキープ装置では、車両が車線に沿って走行するように、車両の状態量（目標軌跡に対する横偏差、ヨー角など）をフィードバックすることによって制御量（操舵角）を求め、制御量に応じた操舵制御を行う。特に、フィードバック制御における収束性と応答性を両立させるとともに制御の安定性を向上させるために、ＬＱ制御を用いるものがある（非特許文献１参照）。ＬＱ制御では、状態量と制御量を含む線形二次形式評価関数を最小化するようなフィードバックゲインを算出し、そのフィードバックゲインと状態量から制御量を算出する。
毛利宏、古性裕之著、「自動車線追従走行の検討」、日本機械学会第６回交通・物流部門大会講演論文集（部門大会編）、１９９７年７月２８〜３０日

車両の実際の状態量はセンサなどによって検出されるので、検出精度が低いほど状態量に誤差が含まれる場合がある。状態量に誤差が含まれる場合、その状態量をフィードバックすることにより、誤差を含む制御量が算出される。

そこで、本発明は、ＬＱ制御を用いたフィードバック制御において、状態量に対する誤差による制御への影響を抑制する走行制御装置を提供することを課題とする。

本発明に係る走行制御装置は、車両の状態量にかかるフィードバックゲインをＬＱ制御の線形二次形式評価関数に基づいて算出する走行制御装置であって、ＬＱ制御の線形二次形式評価関数で用いる状態量にかかる重みを状態量の検出精度が低いほど小さく設定する設定手段を備えることを特徴とする。

この走行制御装置では、ＬＱ制御を利用し、車両の状態量と車両への制御量を含む線形二次形式評価関数を最小化するフィードバックゲインを求め、その求めたフィードバックゲインと状態量から制御量を求める。特に、走行制御装置では、設定手段により、ある状態量の検出精度が低いほど線形二次形式評価関数におけるその状態量にかかる重みを通常より小さくする。これによって、この走行制御装置では、検出精度が低下している状態量のフィードバック量が少なくなり、その状態量に対する誤差による制御への影響が抑制される。また、この走行制御装置では、ＬＱ制御による制御の安定性が確保され、フィードバック制御における収束性と応答性も向上する。

なお、状態量の検出精度については、予め設定された固定値でもよいしあるいは車両走行中に求められる可変値でもよい。したがって、状態量にかかる重みも、検出精度が固定値の場合には予め設定され、検出精度が可変値の場合には値が変わる毎に設定される。

本発明の上記走行制御装置では、状態量の検出精度を評価する評価手段を備える構成としてもよい。

この走行制御装置では、評価手段により各状態量の検出精度を評価し、設定手段によりその評価された検出精度を応じて各状態量にかかる重みを設定する。このように、走行制御装置では、評価手段を備えることにより車両走行中に状態量の検出精度が変化する場合でもその検出精度の変化を動的に検出することができ、状態量にかかる重みも動的に変えることができる。その結果、状態量の検出状況に応じて、状態量に対する誤差による制御への影響をより抑制することができる。

本発明の上記走行制御装置では、評価手段は、状態量の検出精度としてカルマンフィルタの推定誤差共分散行列における状態量に対応する対角値を評価し、設定手段は、評価手段で評価した対角値が大きいほど、ＬＱ制御の線形二次形式評価関数で用いる状態量にかかる重みを小さく設定する構成としてもよい。

この走行制御装置では、カルマンフィルタを利用し、各検出手段による検出量から各状態量を推定する。走行制御装置では、評価手段により、各状態量の検出精度としてカルマンフィルタの推定誤差共分散行列における各状態量に対応する対角値で評価する。そして、走行制御装置では、設定手段により各状態量に対応する対角値が大きいほど（つまり、検出精度が低いほど）、線形二次形式評価関数におけるその状態量にかかる重みを通常より小さくする。このように、走行制御装置では、カルマンフィルタを利用することにより、各検出手段による検出値から各状態量を推定できるとともに、その各状態量に対する確からしさ（推定精度、信頼度）も得ることができる。

本発明は、状態量の検出精度に応じてＬＱ制御の線形二次形式評価関数におけるその状態量にかかる重みを設定することにより、状態量に対する誤差による制御への影響を抑制することができる。

以下、図面を参照して、本発明に係る走行制御装置の実施の形態を説明する。

本実施の形態では、本発明に係る走行制御装置を、車両に搭載されるレーンキープ装置に適用する。本実施の形態に係るレーンキープ装置は、車線（一対の白線）に沿って走行するように（例えば、車両重心が車線の中心線上になるように）、操舵制御を行う。特に、本実施の形態に係るレーンキープ装置では、カルマンフィルタによって車両の状態量を推定し、ＬＱ制御によってその推定した状態量をフィードバック制御する。

図１〜図３を参照して、本実施の形態に係るレーンキープ装置１について説明する。図１は、本実施の形態に係るレーンキープ装置の構成図である。図２は、本実施の形態に係る道路−車両座標系のモデル図である。図３は、本実施の形態に係るＬＱ制御を用いたフィードバック制御の概要図である。

レーンキープ装置１は、４つの車両の運動状態（観測量）と３つの車両への入力（入力量）を検出し、カルマンフィルタにより４つの観測量と３つの入力量から４つの車両の状態量を推定する。そして、レーンキープ装置１では、ＬＱ制御により、カルマンフィルタによる４つの状態量と制御量（操舵角）を含む線形二次形式評価関数を最小化するフィードバックゲインを求め、そのフィードゲインと状態量から制御量を求める。特に、レーンキープ装置１では、観測量や入力量の各検出誤差（ひいては、状態量の推定誤差）による制御量への影響を抑制するために、カルマンフィルタの推定誤差共分散行列における各状態量に対応する対角値を用いて、線形二次形質評価関数における各状態量に対する重みに各状態量の推定誤差を常時反映させる。そのために、レーンキープ装置１は、ＧＰＳ[Global Positioning System]受信機２、白線認識センサ３、磁気センサ４、ヨーレートセンサ５、操舵角センサ６、道路マップデータベース７、操舵アクチュエータ８及びＥＣＵ[Electronic Control Unit]９を備えている。

ＧＰＳ受信機２は、ＧＰＳアンテナや信号処理部などを備えている。ＧＰＳ受信機２では、アンテナでＧＰＳ衛星からの電波を受信し、信号処理部で３個以上（高度も求める場合には４個以上）のＧＰＳ衛星からの電波により緯度、経度を算出する。さらに、ＧＰＳ受信機２では、道路マップデータベース７に格納されている現在位置の緯度、経度（車線中心）を用いて、道路マップデータの車線中心からのＧＰＳによる緯度、経度の横偏差Ｄ_ｇｐｓを算出し、その横偏差Ｄ_ｇｐｓを示すＧＰＳ横偏差信号をＥＣＵ９に送信する。なお、車両にナビゲーションシステムが搭載される場合、ナビゲーションシステムのＧＰＳ受信機を共有する。

白線認識センサ３は、カメラや画像処理部などを備えている。白線認識センサ３では、カメラで車両の前方の道路を撮像し、画像処理部でその撮像画像から左右一対の白線を認識し、一対の白線の中心線を算出する。さらに、白線認識センサ３では、その左右一対の白線の中心線からの車両の横偏差Ｄ_ｗｌを算出し、その横偏差Ｄ_ｗｌを示す白線横偏差信号をＥＣＵ９に送信する。

磁気センサ４では、車線中心に埋め込まれている磁気マーカからの磁束密度を検出する。そして、磁気センサ４では、その検出した磁束密度に基づいて車線中心（つまり、磁気マーカの位置）からの車両の横偏差Ｄ_ｍａｇを算出し、その横偏差Ｄ_ｍａｇを示す磁気マーカ横偏差信号をＥＣＵ９に送信する。

ヨーレートセンサ５では、車両に作用しているヨーレートγを検出する。そして、ヨーレートセンサ５では、その検出したヨーレートγを示すヨーレート信号をＥＣＵ９に送信する。

これらのＧＰＳ受信機２で検出した横偏差Ｄ_ｇｐｓ、白線認識センサ３で検出した横偏差Ｄ_ｗｌ、磁気センサ４で検出した横偏差Ｄ_ｍａｇ、ヨーレートセンサ５で検出したヨーレートγは、車両の運動状態を示し、４つの観測量である。

図２には、車両座標系と道路座標系を示している。車両座標系は、車両の重心ＣＧを原点とし、車両の前後方向がｘ軸であり、左右方向がｙ軸である。道路座標系は、車両の重心ＣＧから目標軌跡ＴＣへ下ろした垂点を原点とし、目標軌跡ＴＣに対する接線がξ軸であり、その接線に対する垂線がη軸である。目標軌跡ＴＣは、レーンキープを行う際に車両を走行させる目標となる軌跡であり、例えば、左右一対の白線（車線）の中心線である。各横偏差Ｄ_ｇｐｓ、横偏差Ｄ_ｗｌ、横偏差Ｄ_ｍａｇは、道路座標系におけるη座標の値であり、横偏差Ｄ_ｇｐｓの場合にはＧＰＳ受信機２のアンテナの配設位置の横偏差であり、横偏差Ｄ_ｗｌの場合には白線認識センサ３のカメラの配設位置の横偏差であり、横偏差Ｄ_ｍａｇの場合には磁気センサ４の配設位置の横偏差である。

操舵角センサ６では、車両の操舵角δを検出する。そして、操舵角センサ６では、その検出した操舵角δを示す操舵角信号をＥＣＵ９に送信する。

道路マップデータベース７は、道路マップに関する様々なデータが格納されたデータベースであり、例えば、道路（車線）の形状、道路の位置（緯度、経度）、道路の曲率(道路半径)、道路のカントである。ＥＣＵ９では、道路マップデータベース７を参照し、現在位置走行中の道路の曲率κとカントαを抽出する（図２参照）。なお、車両にナビゲーションシステムが搭載される場合にはナビゲーションシステムの道路マップデータベースを利用するが、ナビゲーションシステムが搭載されていない場合にはＥＣＵ９に道路マップデータベース７を構成する。

これらの操舵角センサ６で検出した操舵角δ、道路マップデータベース７から抽出される道路の曲率κとカントαは、車両への入力を示し、３つの入力量である。

操舵アクチュエータ８は、操舵機構における操舵角を変化させるアクチュエータである。操舵アクチュエータ８では、ＥＣＵ９から操舵制御信号を受信し、操舵制御信号に示される操舵角δ_ｃになるように操舵角を変化させる。

ＥＣＵ９は、ＣＰＵ[Central Processing Unit]、ＲＯＭ[Read Only Memory]、ＲＡＭ[Random Access Memory]などからなり、ＲＯＭに格納される各プログラムをＣＰＵで実行することによってカルマンフィルタ（状態推定部）９ａ及びＬＱ制御部９ｂが構成される。ＥＣＵ９では、一定時間毎に、カルマンフィルタ９ａにより、式（１）で表される４次元ベクトルの車両の観測量Ｙと式（２）で表される３次元ベクトルの車両への入力量Ｕを取り入れ、式（３）で表される４次元ベクトルの車両の状態量Ｘ（横偏差η、横速度ｄη／ｄｔ、ヨー角θ、ヨーレートγ）を推定するとともにカルマンフィルタの推定誤差共分散行列Ｐ_Ｋを求める（図１参照）。そして、ＥＣＵ９では、一定時間毎に、ＬＱ制御部９ｂにより、推定誤差共分散行列Ｐ_Ｋの各対角値を利用してＬＱ制御の線形二次形式評価関数における状態量Ｘにかかる重み行列Ｑ_ＬＱを設定し、線形二次形式評価関数を最小にするフィードバックゲインＫ_ＬＱを求める（図３参照）。さらに、ＥＣＵ９では、一定時間毎に、フィードバックゲインＫ_ＬＱと状態量Ｘから制御量（操舵角δ_ｃ）を求め、この操舵角δ_ｃになるように操舵アクチュエータ８を制御する。なお、本実施の形態ではＬＱ制御部９ｂが特許請求の範囲に記載する設定手段に相当し、カルマンフィルタ９ａが特許請求の範囲に記載する評価手段に相当する。

図２に示すように、状態量Ｘの横偏差ηは車両の重心ＣＧの道路座標系におけるη座標の値であり、横速度ｄη／ｄｔは横偏差ηの時間微分値である。状態量Ｘのヨー角θは、車両の前後方向の目標軌跡ＴＣに対する傾きであり、道路座標系のξ軸と車両座標系のｘ軸とのなす角となる。状態量Ｘのヨーレートγは、車両の重心ＣＧ周りのヨーレートである。

カルマンフィルタについて説明する。カルマンフィルタでは、入力量Ｕ及び推定する状態量Ｘを用いて、式（４）で表される状態方程式を立てる。この状態方程式には、外乱入力によるシステムノイズベクトルＷが加味される。

状態方程式の行列Ａは、式（５）で表される。式（５）の行列ＡにおけるＡ_１は式（６）で求められ、Ａ_２は式（７）で求められ、Ａ_３は式（８）で求められ、Ａ_４は式（９）で求められる。状態方程式の行列Ｂは、式（１０）で表される。状態方程式の行列Ｇは、式（１１）で表される。システムノイズベクトルＷは、式（１２）で表され、横偏差η、横速度ｄη／ｄｔ、ヨー角θ、ヨーレートγに対応して各値が設定される。このシステムノイズベクトルＷの各値は、予め設定された固定値でもよいし、あるいは、外乱の変化などに応じて設定される可変値としてもよい。

なお、Ｋ_ｆは前輪側のコーナリングパワー（２輪分）であり、Ｋ_ｒは後輪側のコーナリングパワー（２輪分）である。Ｉは、ヨー慣性モーメントである。ｌ_ｆは前輪の車軸と車両の重心ＣＧ間の距離であり、ｌ_ｒは後輪の車軸と車両の重心ＣＧ間の距離である（図２参照）。ｍは、車両重量である。Ｖは、車速である。ｇは、重力加速度である。Ｋ_ｆ，Ｋ_ｒ，Ｉ，ｍは、車両のジオメトリやタイヤ特性から予め設定された固定値でもよいし、あるいは、車両の荷重などの変化に応じて設定される可変値としてもよい。Ｖは、図示しない車速センサにおける検出値である。ｌ_ｒ，ｌ_ｆは、車両のジオメトリから決まる固定値である。

また、カルマンフィルタでは、観測量Ｙと入力量Ｕ及び推定する状態量Ｘを用いて、式（１３）で示す観測方程式を立てる。この観測方程式には、各センサ２〜５での観測における観測ノイズベクトルＶが加味される。なお、カルマンフィルタにおけるノイズは、ホワイトノイズが前提である。

状態方程式の行列Ｃは、式（１４）で表される。状態方程式の行列Ｄは、式（１５）で表される。観測ノイズベクトルＶは、式（１６）で表され、磁気センサ４による横偏差Ｄ_ｍａｇ、ＧＰＳ受信機２による横偏差Ｄ_ｇｐｓ、白線認識センサ３による横偏差Ｄ_ｗｌ、ヨーレートセンサ５によるヨーレートγに対応して各値が設定される。この観測ノイズベクトルＶの各値は、予め設定された固定値でもよいし、あるいは、各観測信号のレベルの変化などに応じて設定される可変値としてもよい。なお、Ｌは、車両の重心ＣＧと観測用のセンサの配設位置間の距離であり、固定値である（図２参照）。

そして、カルマンフィルタでは、今回の入力量Ｕと観測量Ｙを用いて式（４）の状態方程式と式（１３）の観測方程式を解き、今回の状態量Ｘ（横偏差η、横速度ｄη／ｄｔ、ヨー角θ、ヨーレートγ）を求める。

さらに、カルマンフィルタでは、前回予測した推定誤差共分散行列Ｐ_{Ｋ（ｔ／ｔ−１）}を用いて、今回の推定誤差共分散行列Ｐ_{Ｋ（ｔ／ｔ）}を求めるために、式（１７）に示す共分散方程式を立てる。そして、カルマンフィルタでは、式（１７）の共分散方程式を解き、今回の推定誤差共分散行列Ｐ_{Ｋ（ｔ／ｔ）}を決定する。さらに、カルマンフィルタでは、今回決定した推定誤差共分散行列Ｐ_{Ｋ（ｔ／ｔ）}を用いて、次回の推定誤差共分散行列Ｐ_{Ｋ（ｔ＋１／ｔ）}を予測するために、式（１８）に示す共分散方程式を立てる。そして、カルマンフィルタでは、式（１８）の共分散方程式を解き、次回の推定誤差共分散行列Ｐ_{Ｋ（ｔ＋１／ｔ）}を予測する。この次回の推定誤差共分散行列Ｐ_{Ｋ（ｔ＋１／ｔ）}は、次回の推定誤差共分散行列Ｐ_Ｋを決定する際に用いられる。

推定誤差共分散行列Ｐ_Ｋは、式（１９）で表される。推定誤差共分散行列Ｐ_Ｋの各対角値は、カルマンフィルタによって推定された横偏差、横速度、ヨー角、ヨーレートに対する各推定誤差（ひいては、これらを推定するために用いた観測量や入力量の検出誤差に相当する）を示す。式（１７）のＫ_Ｋ（ｔ）は、カルマンゲインであり、式（２０）によって求められる。式（１７）の行列Ｃは、上記した式（１４）で表される。式（１８）の行列Ａは、上記した式（５）で表される。式（１８）の行列Ｇは、上記した式（１１）で表される。式（１８）の行列Ｑ_ｋは、式（２１）で表され、その対角値がシステムノイズの標準偏差である。行列Ｑ_ｋの各値は、横偏差η、横速度ｄη／ｄｔ、ヨー角θ、ヨーレートγに対応して各値が設定される。この各値は、予め設定された固定値でもよいし、あるいは、外乱の変化などに応じて設定される可変値としてもよい。

式（１９）の行列内の・は、任意の値を表す。式（２０）の行列Ｃは、上記した式（１４）で表される。式（２０）の行列Ｒ_Ｋは、式（２２）で表され、その対角値が観測ノイズの標準偏差である。行列Ｒ_Ｋの各値は、横偏差Ｄ_ｍａｇ、横偏差Ｄ_ｇｐｓ、横偏差Ｄ_ｗｌ、ヨーレートγに対応して各値が設定される。この各値は、予め設定された固定値でもよいし、あるいは、各観測信号のレベルの変化などに応じて設定される可変値としてもよい。行列Ｑ_Ｋ及び行列Ｒ_Ｋと上記したシステムノイズベクトルＷ（式（１２））及び観測ノイズベクトルＶ（式（１６））とは、式（２３）の関係にある。式（２３）のＥは、相関演算を意味する記号である。式（２３）のδ_ｔτは、クロネッカーのデルタを意味する記号であり、ｔ＝τのときにはδ_ｔτ＝１であり、ｔ＝τでないときにはδ_ｔτ＝０である。

次に、ＬＱ制御について説明する。ＬＱ制御では、入力量Ｕとカルマンフィルタで推定した状態量Ｘを用いて、式（２４）で表される状態方程式を立てる。この状態方程式による可制御で線形な時不変システムのもとで、ある評価関数を最小にする制御則が線形な状態フィードバック制御則となるためには、式（２５）で表される二次形式とする必要がある。ＬＱ制御では、この式（２５）で表される状態量Ｘと制御量（操舵角）δ_ｃを含む線形二次形式評価関数Ｊを最小にするフィードバックゲインＫ_ＬＱを算出する（図３参照）。ＬＱ制御に入力される状態量Ｘとしては、カルマンフィルタで推定された各値から目標状態を減算した各値が用いられる。ただし、目標状態の各値が０の場合（例えば、横偏差を０にする場合（車両の重心ＣＧを車線の中心線上にする場合））、カルマンフィルタで推定された値がそのまま用いられる。そして、ＬＱ制御では、フィードバックゲインＫ_ＬＱを算出すると、そのフィードバックゲインＫ_ＬＱと状態量Ｘの乗算値によって制御量δ_ｃを求める。

式（２４）の行列Ａは、上記した式（５）で表される。式（２４）の行列Ｂは、上記した式（１０）で表される。式（２５）の行列Ｑ_ＬＱは、状態量Ｘにかかる重みであり、式（２６）で表される。式（２５）のＲ_ＬＱは、制御量δ_ｃにかかる重みである。この重みＲ_ＬＱは、予め設定された固定値でもよいし、あるいは、重みＱ_ＬＱなどに応じて設定される可変値としてもよい。

式（２６）の重み行列Ｑ_ＬＱの各対角値は、状態量Ｘである横偏差、横速度、ヨー角、ヨーレートにそれぞれかかる重みであり、カルマンフィルタで求めた推定誤差共分散行列Ｐ_Ｋの対角値（ｐ_ｌａｔ，ｐ_ｄｌａｔ，ｐ_ｙａｗ，ｐ_ｙｒ）からそれぞれ求められる。式（２６）におけるｐ_ａｌｌは、推定誤差共分散行列Ｐ_Ｋの全ての対角値を用いて、式（２７）で算出される。式（２６）におけるｑ_ｌａｔ，ｑ_ｄｌａｔ，ｑ_ｙａｗ，ｑ_ｙｒは、横偏差、横速度、ヨー角、ヨーレートにそれぞれかかる基本重みであり、実験などによって予め決定される。式（２６）におけるｋ_ｌａｔ，ｋ_ｄｌａｔ，ｋ_ｙａｗ，ｋ_ｙｒは、横偏差、横速度、ヨー角、ヨーレートに対する推定誤差共分散行列Ｐ_Ｋの各対角値の反映度をそれぞれ決める係数であり、実験などによって予め決定される。

推定誤差共分散行列Ｐ_Ｋの各対角値は、その値が大きいほど、その対角値に対応する状態量Ｘの各値の推定精度（ひいては、これらを推定するために用いた観測量や入力量の検出精度）が低いこと（推定誤差が大きいこと）を示す。また、重みＱ_ＬＱの各対角値は、式（２６）から判るように、推定誤差共分散行列Ｐ_Ｋの対応する対角値が大きいほど、小さくなる。したがって、状態量Ｘのある値の推定精度が低いほど（推定誤差が大きいほど）、ＬＱ制御における状態量Ｘのその値にかかる重みが小さくなる。つまり、状態量Ｘのある値に対する推定誤差（検出誤差）が大きい場合、ＬＱ制御において状態量Ｘのその値の反映度合いが小さくなり、その値のフィードバック量が少なくなる。

それでは、ＥＣＵ９におけるカルマンフィルタ９ａとＬＱ制御部９ｂで実際に行う処理について説明する。

カルマンフィルタ９ａでは、式（４）の状態方程式、式（１３）の観測方程式を解くために、式（２８）で表されるフィルタ方程式を立てる。カルマンフィルタ９ａでは、今回の観測量Ｙ_（ｔ）と前回予測した状態量Ｘ_{（ｔ／ｔ−１）}を用いて、式（２８）のフィルタ方程式を解き、今回の状態量Ｘ_{（ｔ／ｔ）}を決定する。さらに、カルマンフィルタ９ａでは、今回決定した状態量Ｘ_{（ｔ／ｔ）}と今回の入力量Ｕ_（ｔ）を用いて、次回の状態量Ｘ_{（ｔ＋１／ｔ）}を予測するために、式（２９）に示すフィルタ方程式を立てる。そして、カルマンフィルタ９ａでは、式（２９）のフィルタ方程式を解き、次回の状態量Ｘ_{（ｔ＋１／ｔ）}を予測する。この次回の状態量Ｘ_{（ｔ＋１／ｔ）}は、次回の状態量Ｘを決定する際に用いられる。

式（２８）のＫ_Ｋ（ｔ）は、今回のカルマンゲインである。カルマンフィルタ９ａでは、式（１４）の行列Ｃ、式（２２）の行列Ｒ_Ｋ及び前回予測された推定誤差共分散行列Ｐ_{Ｋ（ｔ／ｔ−１）}を用いて、上記した式（２０）から今回のカルマンゲインＫ_Ｋ（ｔ）を求める。式（２８）の行列Ｃは、上記した式（１４）で表される。式（２９）の行列Ａは、上記した式（５）で表される。式（２９）の行列Ｂは、上記した式（１０）で表される。

さらに、カルマンフィルタ９ａでは、前回予測した推定誤差共分散行列Ｐ_{Ｋ（ｔ／ｔ−１）}を用いて、式（１７）の共分散方程式を解き、今回の推定誤差共分散行列Ｐ_{Ｋ（ｔ／ｔ）}を決定する。さらに、カルマンフィルタ９ａでは、今回決定した推定誤差共分散行列Ｐ_{Ｋ（ｔ／ｔ）}を用いて、式（１８）の共分散方程式を解き、次回の推定誤差共分散行列Ｐ_{Ｋ（ｔ＋１／ｔ）}を予測する。このように、カルマンフィルタ９ａでは、今回の状態量Ｘと推定誤差共分散行列Ｐ_Ｋを求める。

ＬＱ制御部９ｂでは、式（２５）の線形二次形式評価関数Ｊを解くために、状態量Ｘにかかる重み行列Ｑ_ＬＱと制御量δ_ｃにかかる重みＲ_ＬＱを用いて、式（３０）で表されるリカッチ方程式を立てる。この際、ＬＱ制御部９ｂでは、カルマンフィルタ９ａで求めた今回の推定誤差共分散行列Ｐ_Ｋの対角値を用いて、重み行列Ｑ_ＬＱ（式（２６））の各対角値を求める。そして、ＬＱ制御部９ｂでは、式（３０）のリカッチ方程式を解き、正定対称行列Ｐ_ＬＱを求める。さらに、ＬＱ制御部９ｂでは、式（３１）により、正定対称行列Ｐ_ＬＱを用いて、フィードバックゲインＫ_ＬＱを求める。

式（３０）の行列Ａは、上記した式（５）で表される。式（３０）の行列Ｂは、上記した式（１０）で表される。式（３１）のＲ_ＬＱは、上記した制御量δ_ｃにかかる重みである。

そして、ＬＱ制御部９ｂでは、求めたフィードバックゲインＫ_ＬＱとカルマンフィルタ９ａで求めた今回の状態量Ｘを用いて、制御量（操舵角）δ_ｃを求める。さらに、ＬＱ制御部９ｂでは、車両の操舵角が操舵角δ_ｃになるように操舵制御信号を生成し、この操舵制御信号を操舵アクチュエータ８に送信する。

次に、図１〜図３を参照して、レーンキープ装置１の動作について説明する。

ＧＰＳ受信機２では、各ＧＰＳ衛星からの電波をそれぞれ受信し、各電波を用いて緯度、経度を算出し、この緯度、経度の車線中心からの横偏差Ｄ_ｇｐｓを算出し、その横偏差Ｄ_ｇｐｓを示すＧＰＳ横偏差信号をＥＣＵ９に送信している。白線認識センサ３では、車両の前方の道路を撮像し、その撮像画像から左右一対の白線を認識し、その白線の中心線からの車両の横偏差Ｄ_ｗｌを算出し、その横偏差Ｄ_ｗｌを示す白線横偏差信号をＥＣＵ９に送信している。磁気センサ４では、磁気マーカからの磁束密度を検出し、その磁束密度に基づいて車線中心からの車両の横偏差Ｄ_ｍａｇを算出し、その横偏差Ｄ_ｍａｇを示す磁気マーカ横偏差信号をＥＣＵ９に送信している。ヨーレートセンサ５では、車両に作用しているヨーレートγを検出し、そのヨーレートγを示すヨーレート信号をＥＣＵ９に送信している。

操舵角センサ６では、車両の操舵角δを検出し、その操舵角δを示す操舵角信号をＥＣＵ９に送信している。ＥＣＵ９では、道路マップデータベース７から現在位置走行中の道路（車線）の曲率κとカントαを取り出している。

一定時間毎に、ＥＣＵ９では、式（２０）により、前回予測した推定誤差共分散行列Ｐ_{Ｋ（ｔ／ｔ−１）}を用いて、今回のカルマンゲインＫ_Ｋ（ｔ）を求める。そして、ＥＣＵ９では、式（２８）のフィルタ方程式により、前回予測した状態量Ｘ_{（ｔ／ｔ−１）}、今回のカルマンゲインＫ_Ｋ（ｔ）と入力された今回の観測量Ｙ_（ｔ）を用いて、今回の状態量Ｘ_{（ｔ／ｔ）}を求める。さらに、ＥＣＵ９では、式（２９）のフィルタ方程式により、今回の状態量Ｘ_{（ｔ／ｔ）}と今回の入力量Ｕ_（ｔ）を用いて、次回の状態量Ｘ_{（ｔ＋１／ｔ）}を予測する。また、ＥＣＵ９では、式（１７）の共分散方程式により、今回のカルマンゲインＫ_Ｋ（ｔ）と前回予測した推定誤差共分散行列Ｐ_{Ｋ（ｔ／ｔ−１）}を用いて、今回の推定誤差共分散行列Ｐ_{Ｋ（ｔ／ｔ−１）}を求める。さらに、ＥＣＵ９では、式（１８）の共分散方程式により、今回の推定誤差共分散行列Ｐ_{Ｋ（ｔ／ｔ）}を用いて、次回の推定誤差共分散行列Ｐ_{Ｋ（ｔ＋１／ｔ）}を予測する。

ここまでの処理により、今回の観測量Ｙ（Ｄ_ｍａｇ，Ｄ_ｇｐｓ，Ｄ_ｗｌ，γ）から今回の状態量Ｘ（η，ｄη／ｄｔ，θ，γ）が推定され、この状態量Ｘの各値についての誤差が今回の推定誤差共分散行列Ｐ_{Ｋ（ｔ／ｔ）}の各対角値で表される。この対角値が大きいほど、誤差が大きく、推定精度（検出精度）が低いことを示す。

次に、ＥＣＵ９では、今回の推定誤差共分散行列Ｐ_{Ｋ（ｔ／ｔ）}の各対角値を用いて、式（２６）の重み行列Ｑ_ＬＱの各対角値を求める。推定誤差共分散行列Ｐ_{Ｋ（ｔ／ｔ）}の各対角値が大きいほど、重み行列Ｑ_ＬＱの各対角値は小さくなり、ＬＱ制御における状態量Ｘの各値にかかる重みが小さくなる。

ＥＣＵ９では、式（３０）のリカッチ方程式により、重み行列Ｑ_ＬＱを用いて、正定対称行列Ｐ_ＬＱを求める。さらに、ＥＣＵ９では、式（３１）により、正定対称行列Ｐ_ＬＱを用いて、フィードバックゲインＫ_ＬＱを求める。

そして、ＥＣＵ９では、フィードバックゲインＫ_ＬＱと今回の状態量Ｘを用いて、操舵角δ_ｃを求める。さらに、ＥＣＵ９では、操舵角δ_ｃになるように操舵制御信号を生成し、この操舵制御信号を操舵アクチュエータ８に送信する。この操舵制御信号を受信すると、操舵アクチュエータ８では、操舵角δ_ｃになるように、転舵輪を転舵させる。

以上の動作が繰り返し実行され、車両は、車両重心が左右一対の白線の中心線上になるように走行する。

このレーンキープ装置１では、状態量Ｘの推定誤差共分散行列Ｐ_ｋを求め、推定精度（ひいては、観測量Ｙや入力量Ｕの検出精度）が低いほど線形二次形式評価関数における状態量Ｘにかかる重みを小さくすることにより、精度が低下している状態量のフィードバック量を少なくする。その結果、レーンキープ装置１では、状態量に対する誤差による制御への影響を抑制することができ、高精度なフィードバック制御を行うことができる。また、レーンキープ装置１では、ＬＱ制御を用いているので、制御の安定性が確保され、フィードバック制御における収束性と応答性も向上する。

また、レーンキープ装置１では、カルマンフィルタを用いて状態量Ｘと推定誤差共分散行列Ｐ_ｋを観測量Ｙや入力量Ｕに応じて更新していくので、状態量Ｘの推定精度（ひいては、状態量Ｘに対する重み）として適切な値を毎回求めることができ、より高精度なフィードバック制御を行うことができる。

以上、本発明に係る実施の形態について説明したが、本発明は上記実施の形態に限定されることなく様々な形態で実施される。

例えば、本実施の形態ではレーンキープ制御に適用したが、定速制御、挙動安定化制御、車間制御などの様々な走行制御に適用可能である。

また、本実施の形態では状態量として横偏差、横速度、ヨー角、ヨーレートを推定する構成としたが、状態量としてはこの４つに特に限定するものでない。また、車両の位置を検出するためにＧＰＳによる位置検出、白線認識による位置検出、磁気マーカによる位置検出の３つの手段を用いる構成としたが、この３つのうちの１つあるいは２つの位置検出だけでもよいし、あるいは、他の位置検出で行ってもよい。

また、本実施の形態では評価手段としてカルマンフィルタを適用したが、他の手段によって状態量の精度を評価してもよい。また、センサの検出精度などから状態量の精度が予め判っている場合、評価手段が無くてもよく、状態量に対する重みを予め設定しておくようにしてもよい。

本実施の形態に係るレーンキープ装置の構成図である。 本実施の形態に係る道路−車両座標系のモデル図である。 本実施の形態に係るＬＱ制御を用いたフィードバック制御の概要図である。

符号の説明

１…レーンキープ装置、２…ＧＰＳ受信機、３…白線認識センサ、４…磁気センサ、５…ヨーレートセンサ、６…操舵角センサ、７…道路マップデータベース、８…操舵アクチュエータ、９…ＥＣＵ、９ａ…カルマンフィルタ（状態制御部）、９ｂ…ＬＱ制御部

Claims (3)

1. 車両の状態量にかかるフィードバックゲインをＬＱ制御の線形二次形式評価関数に基づいて算出する走行制御装置であって、
   ＬＱ制御の線形二次形式評価関数で用いる状態量にかかる重みを状態量の検出精度が低いほど小さく設定する設定手段を備えることを特徴とする走行制御装置。
2. 状態量の検出精度を評価する評価手段を備えることを特徴とする請求項１に記載する走行制御装置。
3. 前記評価手段は、状態量の検出精度としてカルマンフィルタの推定誤差共分散行列における状態量に対応する対角値を評価し、
   前記設定手段は、前記評価手段で評価した対角値が大きいほど、ＬＱ制御の線形二次形式評価関数で用いる状態量にかかる重みを小さく設定することを特徴とする請求項２に記載する走行制御装置。
   

Images