(57)【要約】【課題】本発明は、車両の運動状態を表す状態量の推定精度を向上させる車両状態量推定装置の提供を目的とする。【解決手段】車両の運動状態を表す所定状態量を観測するＧＰＳ受信機１１等のセンサと、それらのセンサによって観測された所定状態量を車両の運動状態のモデルに入力して前記所定状態量を含む車両の運動状態を表す状態量を推定するカルマンフィルタ２００とを有する、車両状態量推定装置であって、前記推定された状態量を各センサのモデルに入力して各センサによって観測可能な所定状態量が推定され、前記所定状態量のカルマンフィルタ２００での反映度合いが前記推定された所定状態量と実際に観測された所定状態量との偏差に基づいて設定されることを特徴とする、車両状態量推定装置。【選択図】図２
2008077349.tif 000002
【特許請求の範囲】
【請求項１】
車両の運動状態を表す所定状態量を観測する観測手段と、
前記観測手段によって観測された所定状態量を車両の運動状態のモデルに入力して車両の運動状態を表す状態量を推定する状態量推定手段とを有する、車両状態量推定装置であって、
前記状態量推定手段によって推定された状態量を前記観測手段のモデルに入力して前記観測手段によって観測可能な所定状態量を推定する観測量推定手段を備え、
前記観測手段によって観測された所定状態量の車両の運動状態のモデルへの反映度合いが、前記観測手段によって観測された所定状態量と前記観測量推定手段によって推定された所定状態量との偏差に基づいて設定されることを特徴とする、車両状態量推定装置。
【請求項２】
前記観測手段によって観測された所定状態量のうち前記偏差が所定値を超えた所定状態量についての前記反映度合いは、前回の前記反映度合いより小さく設定される、請求項１記載の車両状態量推定装置。
【請求項３】
前記観測手段によって観測された所定状態量のうち前記偏差の度合いが大きい所定状態量ほど前記反映度合いが小さく設定される、請求項１または２に記載の車両状態量推定装置。
【請求項４】
前記状態量推定手段は、前記観測手段によってＮ−１回目に観測された所定状態量を車両の運動状態のモデルに入力して車両の運動状態を表す状態量を推定するものであり、
前記観測量推定手段は、前記状態量推定手段によって推定された状態量を前記観測手段のモデルに入力して前記観測手段によってＮ回目に観測可能な所定状態量を推定するものであって、
前記偏差が、前記観測手段によってＮ回目に観測された所定状態量と前記観測量推定手段によって推定されたＮ回目に観測可能な所定状態量との差に相当する（Ｎは２以上の整数）、請求項１から３のいずれかに記載の車両状態量推定装置。
【請求項５】
前記反映度合いは、更に、前記観測手段が自身の観測結果に基づいて判断した観測状態に基づいて設定され、前記観測状態が観測誤差の大きい状態であるほど前記反映度合いは小さく設定される、請求項１から４のいずれかに記載の車両状態量推定装置。
【請求項６】
前記反映度合いは、更に、前記観測手段によって所定状態量が観測された際の観測環境に基づいて設定され、前記観測環境が観測誤差の大きい環境であるほど前記反映度合いは小さく設定される、請求項１から５のいずれかに記載の車両状態量推定装置。
【請求項７】
前記反映度合いは、更に、前記観測手段による所定状態量の観測時点からの経過時間に基づいて設定され、前記経過時間が長くなるほど前記反映度合いは小さく設定される、請求項１から６のいずれかに記載の車両状態量推定装置。
【請求項８】
前記反映度合いは、更に、前記観測手段による所定状態量の観測時点からの走行距離に基づいて設定され、前記走行距離が長くなるほど前記反映度合いは小さく設定される、請求項１から６のいずれかに記載の車両状態量推定装置。
【請求項９】
前記状態量推定手段及び前記観測量推定手段は、カルマンフィルタであって、
前記観測手段によって観測される所定状態量の観測誤差の標準偏差の項が前記カルマンフィルタの観測方程式に追加され、
前記反映度が小さく設定されるほど前記標準偏差が大きくなる、請求項１から８のいずれかに記載の車両状態量推定装置。
【請求項１０】
請求項１から９のいずれかに記載の車両状態量推定装置と、
該車両状態量推定装置によって推定された車両の運動状態を表す状態量に基づいて操舵系における操舵アクチュエータに対する制御信号を生成する制御信号生成手段とを備える、車両操舵制御装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、走行する車両の運動状態を表す状態量を推定する車両状態量推定装置に係り、詳しくは、車両の運動状態をモデル化してその状態量を演算することにより実際の車両の状態量を推定するようにした車両状態量推定装置に関する。また、本発明は、その車両状態量推定装置を用いた車両操舵制御装置に関する。
【背景技術】
【０００２】
従来、車両を自動操舵制御にて予め定めた道路上を運行させる車両運行システムが提案されている。この車両運行システムでは、路面の車両走行軌道に沿って所定間隔にてマーカ（磁気マーカ）が設置され、車両が各マーカを通過するごとに車両に搭載したマーカセンサから出力されるマーカと当該車両との相対的な位置関係を反映した検出信号に基づいて当該車両の車両走行軌道からの横変位を検出している。そして、このマーカ通過ごとに検出される横変位に基づいて車両が車両走行軌道から外れないように操舵制御を行っている。
【０００３】
このような車両運行システムにおいて、より精度の高い操舵制御を実現するために、従来、カルマンフィルタを用いて操舵制御に必要な車両のヨー運動及び横運動の状態を表す状態量を推定する手法が提案されている（例えば、特許文献１参照）。この特許文献１に記載の従来技術では、車両がマーカ上を通過するごとに得られる横変位の観測値を用いて車両のヨー運動及び横運動を表す状態量として定義された４つの状態変数（ヨーレート、ヨー角、横変位速度、横変位）の推定値がカルマンフィルタによって演算される。
【０００４】
そして、その特許文献１に記載の従来技術は、磁気マーカを通過する毎に横変位とヨーレートを観測値として取得しその観測値を用いて車両の状態量の推定値を演算する距離軸カルマンフィルタと、所定周期毎にヨーレートを観測値として取得し前回演算した推定値を用いると共にその観測値を用いて車両の状態量の推定値を演算する時間軸カルマンフィルタとを備え、時間軸カルマンフィルタは、距離軸カルマンフィルタにて推定値が得られるごとに、前回演算した推定値に代えて距離軸カルマンフィルタにて得られた推定値を用いて車両の状態量の推定値を演算するものである。つまり、各磁気マーカで距離軸カルマンフィルタにて得られる推定値を時間軸カルマンフィルタにおける推定に反映させることによって、横変位の観測ができない磁気マーカ間であっても、車両の状態量の推定精度の向上を図ろうとしている。
【特許文献１】特開２００１−３４３４１号公報
【発明の開示】
【発明が解決しようとする課題】
【０００５】
しかしながら、上述の特許文献１に記載の従来技術では、次の磁気マーカを通過するまで、その直前の磁気マーカを通過するときに距離軸カルマンフィルタにて得られた推定値が、時間軸カルマンフィルタによる推定に反映されているため、磁気マーカ間において車両に横方向に大きな動きがある場合には、磁気マーカ間の状態量の推定誤差が大きくなりやすい傾向がある。すなわち、磁気マーカ間では横変位の観測ができないことにより、実道路形状の施工、ヨーレートドリフト、車両のモデル化などによる誤差が累積しやすいため、距離軸カルマンフィルタにて得られた推定値を時間軸カルマンフィルタに反映する周期が長くなる場合（例えば、曲率の大きなカーブにおいて極低速で走行するような場合）には、磁気マーカ間の時間軸カルマンフィルタによる推定誤差が大きくなりやすい傾向がある。
【０００６】
そこで、本発明は、車両の運動状態を表す状態量の推定精度を向上させる車両状態量推定装置の提供を第１の目的とする。また、本発明は、そのような車両状態量推定装置を用いた車両操舵制御装置の提供を第２の目的とする。
【課題を解決するための手段】
【０００７】
上記第１の目的を達成するため、本発明に係る車両状態量推定装置は、
車両の運動状態を表す所定状態量を観測する観測手段と、
前記観測手段によって観測された所定状態量を車両の運動状態のモデルに入力して車両の運動状態を表す状態量を推定する状態量推定手段とを有する、車両状態量推定装置であって、
前記状態量推定手段によって推定された状態量を前記観測手段のモデルに入力して前記観測手段によって観測可能な所定状態量を推定する観測量推定手段を備え、
前記観測手段によって観測された所定状態量の車両の運動状態のモデルへの反映度合いが、前記観測手段によって観測された所定状態量と前記観測量推定手段によって推定された所定状態量との偏差に基づいて設定されることを特徴とする。
【０００８】
これにより、車両の運動状態のモデルに入力する前記観測手段によって観測された所定状態量の反映度合いを当該偏差によって変化させることができるので、前記観測手段によって観測された所定状態量が一時的に大きく変化したとしても車両の運動状態を表す状態量の推定精度を向上させることができる。例えば、当該偏差が所定の基準値を超えた場合には、前記観測手段によって観測された所定状態量と前記観測量推定手段によって推定された所定状態量との間に一定レベルの乖離が発生したとして、前記観測手段によって観測された所定状態量の前記運動状態モデルへの入力の反映度合いを小さく設定して、車両の運動状態を表す状態量の推定に与える影響を小さくすることができる。その結果、前記観測手段によって観測された所定状態量が一時的に大きく変化したとしても、車両の運動状態を表す状態量の推定精度の向上を図ることができる。
【０００９】
また、本発明に係る車両状態量推定装置において、前記観測手段によって観測された所定状態量のうち前記偏差が所定値を超えた所定状態量についての前記反映度合いは、前回の前記反映度合いより小さく設定されるようにしてよい。すなわち、前記偏差が所定値を超えた場合には、当該所定値を超えた所定状態量の前記運動状態モデルへの入力の反映度合いを前回より小さく設定して、車両の運動状態を表す状態量の推定に与える影響を小さくすることができる。
【００１０】
また、前記観測手段によって観測された所定状態量のうち前記偏差の度合いが大きい所定状態量ほど前記反映度合いが小さく設定されるようにしてもよい。これにより、複数の所定状態量のうち他の所定状態量に比べて前記偏差の度合いが大きい所定状態量ほど、車両の運動状態を表す状態量の推定に与える影響を小さくすることができる。なお、複数の所定状態量の間の比較を前記偏差の度合いによって行うため、複数の所定状態量が互いに異なる物理量であっても比較可能となる。
【００１１】
ここで、本発明に係る車両状態量推定装置において、前記状態量推定手段は、前記観測手段によってＮ−１回目に観測された所定状態量を車両の運動状態のモデルに入力して車両の運動状態を表す状態量を推定するものであり、前記観測量推定手段は、前記状態量推定手段によって推定された状態量を前記観測手段のモデルに入力して前記観測手段によってＮ回目に観測可能な所定状態量を推定するものであって、前記偏差が、前記観測手段によってＮ回目に観測された所定状態量と前記観測量推定手段によって推定されたＮ回目に観測可能な所定状態量との差に相当する（Ｎは２以上の整数）ものであると好ましい。
【００１２】
また、本発明に係る車両状態量推定装置において、前記反映度合いは、更に、前記観測手段が自身の観測結果に基づいて判断した観測状態に基づいて設定され、前記観測状態が観測誤差の大きい状態であるほど前記反映度合いは小さく設定されると好ましい。これにより、観測手段自身の観測結果に基づいて判断された観測状態が観測誤差の大きい状態であれば前記運動状態モデルへの反映度合いを抑えることができるので、観測状態の悪化による観測手段の観測精度の低下に対してさらにロバストな状態量推定が可能となる。
【００１３】
また、前記反映度合いは、更に、前記観測手段によって所定状態量が観測された際の観測環境に基づいて設定され、前記観測環境が観測誤差の大きい環境であるほど前記反映度合いは小さく設定されると好ましい。これにより、客観的に把握され得る観測環境が観測誤差の大きい環境であれば前記運動状態モデルへの反映度合いを抑えることができるので、観測環境の悪化による観測手段の観測精度の低下に対してさらにロバストな状態量推定が可能となる。
【００１４】
また、前記反映度合いは、更に、前記観測手段による所定状態量の観測時点からの経過時間に基づいて設定され、前記経過時間が長くなるほど前記反映度合いは小さく設定されるとよい。これにより、観測時点からの時間経過により低下するその観測データの正確度を前記車両状態モデルに反映させることができるので、観測時点からの時間経過による観測手段の観測精度の低下に対してさらにロバストな状態量推定が可能となる。例えば、実道路の施工誤差や車両モデル誤差の累積が観測時点から生じたとしても、ロバストな状態量推定が可能となる。
【００１５】
また、前記反映度合いは、更に、前記観測手段による所定状態量の観測時点からの走行距離に基づいて設定され、前記走行距離が長くなるほど前記反映度合いは小さく設定されるとよい。これにより、観測時点からの走行距離により低下するその観測データの正確度を前記車両状態モデルに反映させることができるので、観測時点からの走行距離増加による観測手段の観測精度の低下に対してさらにロバストな状態量推定が可能となる。例えば、実道路の施工誤差や車両モデル誤差の累積が観測時点から生じたとしても、ロバストな状態量推定が可能となる。
【００１６】
ここで、本発明に係る車両状態量推定装置において、前記状態量推定手段及び前記観測量推定手段は、カルマンフィルタであって、前記観測手段によって観測される所定状態量の観測誤差の標準偏差の項が前記カルマンフィルタの観測方程式に追加され、前記反映度が小さく設定されるほど前記標準偏差が大きくなると好ましい。
【００１７】
また、上記第２の目的を達成するため、本発明に係る車両操舵制御装置は、
本発明に係る車両状態量推定装置と、
該車両状態量推定装置によって推定された車両の運動状態を表す状態量に基づいて操舵系における操舵アクチュエータに対する制御信号を生成する制御信号生成手段とを備えることを特徴とする。
【発明の効果】
【００１８】
本発明によれば、車両の運動状態を表す状態量の推定精度を向上させる車両状態量推定装置を実現することができる。また、そのような車両状態量推定装置を用いた車両操舵制御装置を実現することができる。
【発明を実施するための最良の形態】
【００１９】
以下、図面を参照して、本発明を実施するための最良の形態の説明を行う。本発明の実施の一形態に係る車両状態量推定装置は、カルマンフィルタを用いて車両の状態量、特に、車両の横方向に係る運動状態（ヨー運動及び横運動）を表す状態量を推定する。具体的には、この車両の横方向に係る運動状態を表す状態量として、例えば、ヨーレート、ヨー角、横変位速度及び横位置（横変位）を推定している。
【００２０】
その推定手法について、図１を用いて簡単に説明する。図１において、車両１００は、例えば、磁気マーカが離散的に設置された軌道に沿って走行するように操舵制御される。車両１００の横方向に係る運動状態は、操舵角、車輪速度、道路曲率、バンク角（カント）などの因子に影響される。その運動状態を表す状態量（ヨーレート、ヨー角、横変位速度、横位置）のうち、ヨーレートがヨーレートセンサからの検出信号に基づいて自律的に観測され、横位置が磁気センサ等のマーカセンサによって磁気マーカが検出されるごとにその検出信号に基づいて観測される。また、同時に、横位置は、車両の位置を特定可能なＧＰＳ受信機や軌道に沿って引かれた白線を認識可能な白線認識装置からの検出信号に基づいて自律的にも観測される。
【００２１】
カルマンフィルタ２００は、上記のような操舵角、車輪速度、道路曲率、バンク角などの因子に影響される車両１００の運動状態をモデル化し、これらの因子及び上記のように観測された横位置及びヨーレート、更に前回の推定値を用いてヨーレート、ヨー角、横変位速度及び横位置の推定値を演算する。実際に観測されるヨーレート及び横位置には、ヨーレートセンサやマーカセンサや白線認識測定やＧＰＳ測定の雑音成分（測定雑音）が定常的に含まれ、更に、車両１００固有の雑音成分（システム雑音）も含まれる。これに対して、カルマンフィルタ２００によって状態量（ヨーレート、ヨー角、横変位速度及び横位置）の各推定値を繰り返し演算することによって、当該状態量の各推定値は上記雑音成分が除かれた真の値に収束してゆく。
【００２２】
上記のようなカルマンフィルタを用いた車両状態量推定装置を用いた車両の操舵制御装置は、例えば、図２に示すように構成される。図２において、ＧＰＳ受信機１１、白線認識装置１２、マーカセンサ１３、ヨーレートセンサ１４、Ｇセンサ１５、車輪速センサ１６及び操舵角センサ１７が制御ユニット（ＥＣＵ）５０に接続されている。ＧＰＳ受信機１１は、ＧＰＳ衛星からの受信情報に基づいて特定された車両の位置に係る座標情報（例えば、緯度及び経度）に応じた検出信号を出力する。白線認識装置１２は、軌道に沿って引かれた白線との車両横方向における相対的な位置関係に応じた検出信号を出力する。マーカセンサ１３は、磁気マーカ（レーンマーカ）の発する磁気の大きさに応じた検出信号を出力する。したがって、マーカセンサ１３は、軌道に沿って離散的に設置された各磁気マーカ上を車両が通過するときにはその磁気マーカとの車両横方向における相対的な位置関係に応じた検出信号を出力する。ヨーレートセンサ１４は、走行中の車両のヨーレートに応じた検出信号を出力する。Ｇセンサ１５は、走行中の車両の横加速度に応じた検出信号を出力する。車輪速センサ１６は、走行する車両の車輪の回転速度（車両速度に対応）に応じたパルス信号を検出信号として出力する。操舵角センサ１７は、操舵輪（前輪）の操舵角度に応じた検出信号を出力する。
【００２３】
また、メモリユニット２０には、車両の走行する道路の形状（曲率、バンク角、道路の車線数、車線幅、標高等）や、周辺道路建造物（家屋、ビル、交差点、踏切、駐車場、有料道路の料金所等）に係る地図情報が座標データとともに予め格納されており、制御ユニット５０は、メモリユニット２０からこの地図情報を必要に応じて読み出す。メモリユニット２０内の地図情報は、車車間通信や路車間通信や所定の管理センターなどの外部との通信を介して、あるいは、ＣＤやＤＶＤなどの媒体を介して、更新可能にしてもよい。
【００２４】
制御ユニット５０は、上述したようなカルマンフィルタ２００の機能及び車両操舵制御の機能を有している。このカルマンフィルタ２００の機能により、車両の横方向に係る運動状態を表す状態量（ヨーレート、ヨー角、横変位速度、横位置）の推定演算が行われ、車両操舵制御部３１により、推定演算された状態量に基づいて操舵角が演算され、その操舵角に応じた操舵制御信号が出力される。このように制御ユニット５０から出力される操舵制御信号に基づいて操舵系に設けられた操舵アクチュエータ３２が駆動する。
【００２５】
制御ユニット５０のカルマンフィルタ２００は、
状態変数ｘ＝（η，η’，θ，γ）Ｔ
影響因子（入力量）ｕ＝（δ，κ，α）Ｔ
システム雑音ｗ＝（ｗη，ｗη’，ｗθ，ｗγ）Ｔ
観測量ｙ＝（Ｄｍａｇ，Ｄｇｐｓ，Ｄｗｌ，γｙ）Ｔ
観測誤差標準偏差σ＝（σｍａｇ，σｇｐｓ，σｗｌ，σｙγ）Ｔ
と定義したときに、式（１）に示される状態方程式と式（２）に示される観測方程式を定式化する。なお、「（）Ｔ」は、転置行列を示す。また、η’と数式中の「ドットη」は、同義である。
【００２６】
【数１】
2008077349.tif 000003
ここで、式（１）における（ｄ／ｄｔ）は、時間ｔに関する微分演算子である。また、状態方程式（１）と観測方程式（２）における各変数は、
η：車両重心の横位置
η’：車両重心の横変位速度（車両重心の横位置微分）
θ：道路接線方向に対するヨー角
γ：ヨーレート
δ：操舵角（操舵角センサ１７により検出）
κ：道路曲率（地図情報から取得）
α：バンク角（地図情報から取得）
Ｄｍａｇ，Ｄｇｐｓ，Ｄｗｌ：横位置（観測値）
γｙ：ヨーレート（観測値）
ｗη，ｗη’，ｗθ，ｗγ：状態量のシステム雑音
Ｖ：車速（車輪速センサ１１により検出）
ｇ：重力加速度
ｍ：車両質量
Ｉ：ヨー慣性質量
Ｋｆ，Ｋｒ：前・後輪のコーナリングパワー
ｌｆ，ｌｒ：重心と前・後輪との間の距離
Ｌ：重心から車両前端までの距離
と定義される。なお、観測方程式（２）における観測誤差標準偏差σについての説明は後述する。
【００２７】
上記各変数は、図３に示すような座標系において定義される。即ち、曲率κの軌道Ｒ（目標軌跡）の接線ξと垂直な方向ηに車両１００（二輪モデル）の重心Ｇが位置するように車両１００と軌道Ｒとの関係が設定される。そして、重心Ｇを原点とした車両前後左右方向の座標系ｘ−ｙにて重心Ｇと前・後車輪との距離ｌｆ，ｌｒが定義される。また、状態量としてのヨーレートγは、重心Ｇ回りのヨーレートとして定義される。横位置Ｄは、上記方向ηにおける軌道Ｒからマーカセンサ（車両先端に設置）までの距離として定義される。状態量としてのヨー角θは、軌道Ｒの接線方向ξと車両の前後方向ｘとのなす角度として定義される。
【００２８】
上記の状態方程式（１）と観測方程式（２）に基づき、カルマンフィルタ２００によって状態量（ヨーレート、ヨー角、横変位速度及び横位置）の各推定値を繰り返し演算することによって、当該状態量の各推定値は上記雑音成分が除かれた真の値に収束してゆく。なお、カルマンフィルタ２００での処理にて推定値を繰り返し演算する過程で、状態方程式（１）に示す微分方程式を解く際に前回得られた推定値が用いられる。
【００２９】
さらに、カルマンフィルタ２００では、前回予測した推定誤差共分散行列ＰＫ（ｔ／ｔ−１）を用いて、今回の推定誤差共分散行列ＰＫ（ｔ／ｔ）を求めるために、式（３）に示される共分散方程式を定式化する。そして、カルマンフィルタ２００では、共分散方程式（３）を解き、今回の推定誤差共分散行列ＰＫ（ｔ／ｔ）を決定する。さらに、カルマンフィルタ２００では、今回決定した推定誤差共分散行列ＰＫ（ｔ／ｔ）を用いて、次回の推定誤差共分散行列ＰＫ（ｔ＋１／ｔ）を予測するために、式（４）に示される共分散方程式を解き、次回の推定誤差共分散行列ＰＫ（ｔ＋１／ｔ）を予測する。
【００３０】
【数２】
2008077349.tif 000004
推定誤差共分散行列ＰＫは、式（５）で示される。推定誤差共分散行列ＰＫの各対角値は、カルマンフィルタ２００によって推定された横位置、横変位速度、ヨー角及びヨーレートに対する各推定誤差（ひいては、これらを推定するために用いた観測量や入力量の検出誤差に相当する）を示す。すなわち、推定誤差共分散行列ＰＫの各対角値は、その値が大きいほど、その対角値に対応する状態量ｘの各値の推定精度（ひいては、これらを推定するために用いた観測量や入力量の検出精度）が低いこと（推定誤差が大きいこと）を示す。式（３）のＫＫ（ｔ）は、カルマンゲインであり、式（６）によって求められる。式（３）の行列Ｃは、式（２）の行列Ｃに相当する。式（４）の行列Ａは式（１）内の行列Ａに相当する。式（４）の行列Ｇは式（１）内の行列Ｇに相当する。式（４）の行列ＱＫは、式（７）で表され、その対角値がシステム雑音の標準偏差である。行列ＱＫの各値は、横位置η，横変位速度η’，ヨー角θ，ヨーレートγに対応して各値が設定される。この各値は、予め設定された固定値でもよいし、あるいは、外乱の変化などに応じて設定される可変値としてもよい。
【００３１】
【数３】
2008077349.tif 000005
式（５）の行列ＰＫ内の・は、任意の値を示す。式（６）の行列Ｃは、式（２）の行列Ｃに相当する。式（６）の行列ＲＫは式（８）で表され、その対角値が観測誤差標準偏差である。行列ＲＫの各値は、横位置Ｄｍａｇ，横位置Ｄｇｐｓ，横位置Ｄｗｌ及びヨーレートγｙに対応して各値が設定される。この各値は、予め設定された固定値でもよいし、あるいは、後述のように各観測信号のレベルの変化などに応じて設定される可変値としてもよい。行列ＱＫ及び行列ＲＫと上記したシステム雑音ｗ及び観測誤差標準偏差σとは、式（９）の関係にある。式（９）のＥは、相関演算を意味する記号である。式（９）のδｔτは、クロネッカーのデルタを意味する記号であり、ｔ＝τのときにはδｔτ＝１であり、ｔ＝τではないときにはδｔτ＝０である。
【００３２】
それでは、カルマンフィルタ２００で実際に行う処理について説明する。カルマンフィルタ２００では、状態方程式（１）と観測方程式（２）を解くために、式（１０）で表されるフィルタ方程式を立てる。ただし、数式中の「ハットｘ」は、ｘの推定値であることを示す。カルマンフィルタ２００では、今回の観測量ｙ（ｔ）と前回予測した状態量ｘ（ｔ／ｔ−１）と今回の入力量ｕ（ｔ）を用いて、フィルタ方程式（１０）を解き、今回の状態量ｘ（ｔ／ｔ）を決定する。さらに、カルマンフィルタ２００では、今回決定した状態量ｘ（ｔ／ｔ）と今回の入力量ｕ（ｔ）を用いて、次回の状態量ｘ（ｔ＋１／ｔ）を予測するために、式（１１）に示すフィルタ方程式を立てる。そして、カルマンフィルタ２００では、フィルタ方程式（１１）の方程式を解き、次回の状態量ｘ（ｔ＋１／ｔ）を予測する。この次回の状態量ｘ（ｔ＋１／ｔ）は、次回の状態量ｘを決定する際に用いられる。なお、ｘ（ａ／ｂ）とは、ｂ回目演算時に予測されたａ回目演算時の状態量（推定値）を指す。
【００３３】
【数４】
2008077349.tif 000006
フィルタ方程式（１０）のＫＫ（ｔ）は、今回のカルマンゲインである。カルマンフィルタ２００では、観測方程式（２）の行列Ｃ、式（８）の行列ＲＫ及び前回予測された推定誤差共分散行列ＰＫ（ｔ／ｔ−１）を用いて、式（６）から今回のカルマンゲインＫＫ（ｔ）を求める。フィルタ方程式（１０）の行列Ｃは式（２）の行列Ｃに相当する。フィルタ方程式（１１）の行列Ａは式（１）内の行列Ａに相当する。フィルタ方程式（１１）の行列Ｂは式（１）内の行列Ｂに相当する。
【００３４】
さらに、カルマンフィルタ２００では、前回予測した推定誤差共分散行列ＰＫ（ｔ／ｔ−１）を用いて、（３）の共分散方程式を解き、今回の推定誤差共分散行列ＰＫ（ｔ／ｔ）を決定する。さらに、カルマンフィルタ２００では、今回決定した推定誤差共分散行列ＰＫ（ｔ／ｔ）を用いて、（４）の共分散方程式を解き、次回の推定誤差共分散行列ＰＫ（ｔ＋１／ｔ）を予測する。このように、カルマンフィルタ２００では、車両の状態量としての今回の状態量ｘと推定誤差共分散行列ＰＫを求める。具体的には、車両の状態量としての横位置η、横変位速度η’、ヨー角θ、ヨーレートγの推定値が得られる。
【００３５】
ところで、カルマンフィルタはノイズ等による観測量のばらつきの範囲をある程度想定した上で設計される。したがって、観測量がその範囲内にばらつく限り、カルマンフィルタは、そのばらつき誤差を除去して、状態量を精度良く推定することができる。しかしながら、図７に示されるように、想定外に大きな誤差（例えば、故障や瞬間的な劣環境による誤差）をもった観測量がカルマンフィルタに入力された場合には、その影響を受けて、状態量の推定値の誤差が大きくなってしまう。
【００３６】
そこで、本実施形態の車両状態量推定装置は、「観測量ｙの推定値」を演算し、実際の観測量ｙとその推定値との偏差に基づいて、カルマンフィルタ２００での状態量推定に観測量ｙを反映させる度合いを設定する。つまり、本実施形態の車両状態量推定装置は、想定外のばらつきを持つ観測値が状態推定に悪影響を与えないように、カルマンフィルタ２００による推定値との偏差が所定の基準値を超えた実観測量ｙについては異常値とみなす処理を実行した上で、状態量を推定する。
【００３７】
「観測量ｙの推定値」を算出するためには、観測方程式（２）を利用すればよい。観測量ｙの推定値は、観測方程式（２）を変形した推定観測量方程式（１２）によって算出される。なお、数式中の「ハットｙ」は、ｙの推定値であることを示す。
【００３８】
【数５】
2008077349.tif 000007
すなわち、観測誤差標準偏差σを除いた観測方程式（２）において、状態量ｘの部分に前回の演算時に予測した今回の状態量ｘ（ｔ／ｔ−１）を代入することによって、今回の観測量ｙの推定値を演算することができる。
【００３９】
観測量ｙは所定の演算周期で観測される。カルマンフィルタ２００は、前回演算時において、前回演算時の実観測量ｙ（ｔ−１）、前々回演算時に予測された前回演算時の状態量ｘ（ｔ−１／ｔ−２）及び前回演算時の入力量ｕ（ｔ−１）を用いて、フィルタ方程式（１０）を解くことで、前回演算時に予測された前回演算時の状態量ｘ（ｔ−１／ｔ−１）を決定している。それとともに、カルマンフィルタ２００は、前回演算時において、その決定した状態量ｘ（ｔ−１／ｔ−１）と前回演算時の入力量ｕ（ｔ−１）を用いて、フィルタ方程式（１１）を解くことで、今回演算時の状態量ｘ（ｔ／ｔ−１）を予測する。そして、カルマンフィルタ２００は、今回演算時において、前回演算時に予測された今回演算時の状態量ｘ（ｔ／ｔ−１）と今回演算時の入力量ｕ（ｔ）を推定観測量方程式（１２）に代入することによって、今回演算時のｙ（ｔ）の推定観測量を演算する。
【００４０】
制御ユニット５０は、実際の今回の観測量ｙ（ｔ）とカルマンフィルタ２００によって演算された今回の観測量ｙ（ｔ）の推定値との偏差が所定の基準値を超えた場合には、実際の今回の観測量ｙ（ｔ）は異常値であるとみなして、状態方程式（１）と観測方程式（２）によって定式化された車両の運動状態モデルからその異常値とみなされた観測量を除いたモデルに制御対象モデルを切り替える。例えば、観測量ｙのうち白線認識装置１２によって観測された横位置Ｄｗｌが横位置Ｄに関する基準範囲を超えたことにより当該横位置Ｄｗｌが異常値とみなされた場合には、観測方程式を式（２）から横位置Ｄｗｌの行を削除した式（１３）に変更する。
【００４１】
【数６】
2008077349.tif 000008
図８は、観測量ｙの異常値を状態量推定から排除する一方法のフローである。本フローは、所定の演算周期で繰り返される。カルマンフィルタ２００は、前回演算時に予測された今回演算時の状態量ｘ（ｔ／ｔ−１）と今回演算時の入力量ｕ（ｔ）を推定観測量方程式（１２）に代入することによって、今回演算時の観測量ｙ（ｔ）の推定観測量を演算する（ステップ２）。カルマンフィルタ２００は、今回演算時の観測量ｙ（ｔ）とステップ２で演算された今回演算時の観測量ｙ（ｔ）の推定観測量との差の絶対値が所定の基準値を超えるか否かを観測量ｙ（ｔ）の要素毎に判断する（ステップ４）。その差の絶対値が所定の基準値を超えている場合には（ステップ４；Ｙｅｓ）、その基準値を超えた今回演算時の観測量ｙ（ｔ）の要素を観測方程式（２）から削除する（ステップ６）。例えば、基準値を超えた今回演算時の観測量ｙ（ｔ）の要素が白線認識装置１２によって観測された横位置Ｄｗｌであるならば、観測方程式は式（１３）のように変形する。したがって、カルマンフィルタ２００は、状態方程式（１）と変形後の観測方程式（１３）の運動状態モデルに対して、フィルタ方程式（１０）を解くことで、今回演算時の状態量ｘ（ｔ）を推定するとともに、フィルタ方程式（１１）を解くことで、次回演算時の状態量ｘ（ｔ＋１）を予測する。一方、今回演算時の観測量ｙ（ｔ）とステップ２で演算された今回演算時の観測量ｙ（ｔ）の推定観測量との差の絶対値が所定の基準値を超えていない場合には（ステップ４；Ｎｏ）、観測方程式（２）の変形は行われない（ステップ８）。したがって、カルマンフィルタ２００は、状態方程式（１）と観測方程式（２）の運動状態モデルに対して、フィルタ方程式（１０）を解くことで、今回演算時の状態量ｘ（ｔ）を推定するとともに、フィルタ方程式（１１）を解くことで、次回演算時の状態量ｘ（ｔ＋１）を予測する。なお、ステップ６において観測方程式（２）の変形が行われたとしても、本フローが繰り返されて、今回演算時の観測量ｙ（ｔ）とステップ２で演算された今回演算時の観測量ｙ（ｔ）の推定観測量との差の絶対値が所定の基準値を超えないようになった場合には、式（２）に示される変形前の観測方程式に戻る。
【００４２】
したがって、今回の演算時に観測量ｙ（ｔ）が基準値を超えるような異常値で観測されたとしても、異常値に相当する観測量ｙ（ｔ）の要素を観測方程式から除くことで、状態量の推定演算にその異常値の影響が及ばないようにすることができる。その結果、想定外に大きな誤差をもった観測量が観測されても、状態量の推定精度に影響を与えずに、状態量の推定精度の向上が図れる。なお、観測方程式の要素が上記変形によって零にならない限り、カルマンフィルタによる状態量の推定は可能である。
【００４３】
ところで、観測誤差標準偏差σはシミュレーションなどによって予め任意の値に設定するようにしてもよいが、状態量ｘの推定値の精度を更に向上するため、後に詳細に説明する「信頼度」と「新鮮度」を使って、観測誤差標準偏差σを算出するのがよい。
【００４４】
制御ユニット５０は、例えば、図４に示す手順での処理を実行している。図４に示す手順での処理は、隣接する磁気マーカ間において複数回演算できるような所定の演算周期で、上記の状態方程式（１）と観測方程式（２）に基づき、カルマンフィルタ２００の推定演算により、車両の状態量の推定値を演算している。
【００４５】
図４において、車両の走行中に図２に示されるような各センサの検出信号が取得される（ステップ１０）。取得された検出信号に基づいて各センサによる車両の横位置Ｄが冗長的に演算される。すなわち、ＧＰＳ受信機１１からの検出信号に基づいて車両の横位置Ｄｇｐｓが演算され、白線認識装置１２からの検出信号に基づいて車両の横位置Ｄｗｌが演算され、マーカセンサ１３からの検出信号に基づいて車両の横位置Ｄｍａｇが演算される。また、ヨーレートセンサ１４からの検出信号に基づいて検出ヨーレートγｙが取得される。このように取得された各横位置Ｄ及び検出ヨーレートγｙを用いてカルマンフィルタ２００により車両の状態量の推定値が演算されることになる。
【００４６】
このステップ１０において、観測方程式（２）における観測誤差標準偏差σを算出する前提として、横位置Ｄや検出ヨーレートγｙなどに係る各センサの検出信号が取得されるとともに、その検出値に対する各センサの単体信頼度が取得される。
【００４７】
センサの単体信頼度とは、自身が出力する検出信号についてセンサ自身が主観的に判断した正確さの度合いを示すものである。すなわち、各センサは、自身の検出結果がその検出状況ではどのくらい正確なものであるのかを主観的に判断することが可能である。そこで、各センサは、自身の検出結果に関して主観的に判断した正確度を単体信頼度として制御ユニット５０に送信する。例えば、各センサは、自身が把握する検出状況が検出誤差の大きくなりやすい状況であれば、単体信頼度は低いと判断する。また、単体信頼度は、例えば０〜１００の間の値に数値化する。各センサの単体信頼度について、以下説明する。
【００４８】
ＧＰＳ受信機１１の単体信頼度は、例えば、ＧＰＳで使用される位置精度劣化度ＰＤＯＰ（Position  Dilution  of  Precision）に応じて設定される。ＰＤＯＰは、観測点位置の誤差と衛星位置の誤差の関係を表現する指数であって、数値が小さいほど位置精度がよいことを示す。ＧＰＳ受信機１１は、ＰＤＯＰが小さいほど単体信頼度を高く算出し、その算出された単体信頼度を検出信号とともに制御ユニット５０に出力する。例えば、ＧＰＳ受信機１１は、演算式『単体信頼度＝１００−ＰＤＯＰ×１０（ＰＤＯＰが１０以上の場合は１０とする）』に従って、単体信頼度を設定する。
【００４９】
白線認識装置１２の単体信頼度は、輝度差、白線の平行度、トラッキング度などの自身の白線認識結果に応じて設定される。白線認識装置１２は、輝度差が大きいほど単体信頼度を高く算出し、白線の平行度が高いほど単体信頼度を高く算出し、白線のトラッキング度が大きいほど単体信頼度を高く算出し、その算出された単体信頼度を検出信号とともに制御ユニット５０に出力する。
【００５０】
マーカセンサ（磁気センサ）１３の単体信頼度は、磁力密度分布形状、磁力強度などに応じて設定される。マーカセンサ１３は、検出された磁力密度分布形状が理想分布に近いほど単体信頼度を高く算出し、磁力強度が大きいほど単体信頼度を高く算出し、その算出された単体信頼度を検出信号とともに制御ユニット５０に出力する。
【００５１】
ヨーレートセンサ１４の単体信頼度は、例えば、検出信号の周波数成分のスペクトル強度に応じて設定される。ヨーレートセンサ１４は、車両の動きとは考えにくい高周波成分（例えば、１０Ｈｚ）以上の周波数成分が多くなるほど単体信頼度を低く算出し、その算出された単体信頼度を検出信号とともに制御ユニット５０に出力する。
【００５２】
Ｇセンサ１５の単体信頼度は、例えば、検出信号の周波数成分のスペクトル強度に応じて設定される。Ｇセンサ１５は、車両の動きとは考えにくい高周波成分（１０Ｈｚ）以上の周波数成分が多くなるほど単体信頼度を低く算出し、その算出された単体信頼度を検出信号とともに制御ユニット５０に出力する。
【００５３】
車輪速センサ１６の単体信頼度は、例えば、検出信号の出力パルスの形状に応じて設定される。パンク等によるタイヤの弾性変化やその他の異常が発生すると出力パルスのスペクトル分布が変化するので、車輪速センサ１６は、パターンマッチングなどを用いて出力パルスの形状がその車輪速における理想パルス形状に近いほど単体信頼度を高く算出し、その算出された単体信頼度を検出信号とともに制御ユニット５０に出力する。
【００５４】
また、上述のように図４のステップ１０において各センサの単体信頼度が取得される一方で、各センサの走行環境信頼度が算出される（ステップ２０）。
【００５５】
センサの走行環境信頼度とは、センサから取得した検出信号について制御ユニット５０が客観的に判断した正確さの度合いを示すものである。すなわち、制御ユニット５０は、センサの検出結果がその検出状況ではどのくらい正確なものであるのかを客観的に判断することが可能である。センサの検出結果がその検出状況によっては必ずしも正確な値であるとは限らないという考えに基づくものである。そこで、制御ユニット５０は、センサの検出結果に関して客観的に判断した正確度を走行環境信頼度として算出する。例えば、制御ユニット５０は、センサが検出動作をした際の検出環境が検出誤差の大きくなりやすい状況であれば、走行環境信頼度は低いと算出する。また、走行環境信頼度は、例えば０〜１００の間の値に数値化する。各センサの走行環境信頼度について、以下説明する。
【００５６】
ＧＰＳ受信機１１の走行環境信頼度は、例えば、車両周囲の建造物の状態（例えば、その形状や配置）に応じて設定される。制御ユニット５０は、メモリユニット２０内の地図情報に基づいて、トンネルやビル街などＧＰＳ衛星からの電波が届きにくい場所に車両が存在する場合には走行環境信頼度を低く算出する。
【００５７】
白線認識装置１２の走行環境信頼度は、天候、日照、路面材質などに応じて設定される。制御ユニット５０は、白線が認識しにくい検出状況の場合には走行環境信頼度を低めに設定する。雨や曇りの場合には晴れの場合に比べ走行環境信頼度を低く算出し、夜間の場合には昼間に比べ走行環境信頼度を低く算出し、路面材質がコンクリートの場合にはアスファルトの場合に比べ走行環境信頼度を低く算出する。
【００５８】
マーカセンサ１３の走行環境信頼度は、磁石埋設誤差、地磁気、道路構造材料などに応じて設定される。制御ユニット５０は、施工により磁石埋設誤差が大きい場所では小さい場所に比べ走行環境信頼度を低く算出し、地磁気が強い場所では弱い場所に比べ走行環境信頼度を低く算出し、道路構造材料に磁性体が使用されている橋梁などの場所では使用されていない場所に比べ走行環境信頼度を低く算出する。
【００５９】
ヨーレートセンサ１４やＧセンサ１５や車輪速センサ１６の走行環境信頼度は、路面粗さ、路面のうねり、路面材質などに応じて設定される。制御ユニット５０は、路面粗さや路面のうねりが大きい場所では小さい場所に比べ走行環境信頼度を低く算出する。
【００６０】
また、各センサの走行環境信頼度を設定するための共通の要素として、車両形状（車高、車幅、全長）、車種、ボディ材質などが挙げられる。制御ユニット５０は、例えば、センサの搭載車両がバスやトラックの場合には普通乗用車の場合に比べ、センサに車両の振動が伝わりやすいため、走行環境信頼度を低く算出する。
【００６１】
上述のように図４のステップ１０において各センサの単体信頼度が取得されるとともに、ステップ２０において各センサの走行環境信頼度が算出されると、各センサの総合信頼度が算出される（ステップ３０）。
【００６２】
センサの総合信頼度とは、上述の単体信頼度と走行環境信頼度を加味した、各センサの検出信号の正確さの度合いを示すものである。総合信頼度Ｒａは、
総合信頼度Ｒａ
＝（ｋ１×単体信頼度＋ｋ２×走行環境信頼度）×新鮮度  ・・・（１４）
新鮮度ｆ（ｔ）
＝−Ｋ（κ，γ）×ｔ／Ｔ＋１  ・・・（１５）
によって演算され得る。ここで、式（１４）におけるｋ１，ｋ２は『ｋ１＋ｋ２＝１，０≦ｋ１≦１，０≦ｋ２≦１』を満たす係数である。式（１５）におけるＴはセンサの計測周期、ｔは横位置検出後（あるいは、前回の計測時点からの）の経過時間である。また、Ｋは劣化係数であり、道路曲率κとヨーレートγに依存する。
【００６３】
劣化係数Ｋは、例えば、
Ｋ（κ，γ）＝ｊ１＋ｊ２×｜κ｜＋ｊ３×｜γ｜  ・・・（１６）
と定義できる。ただし、０≦Ｋ≦１、ｊ１，ｊ２，ｊ３は任意の係数である。
【００６４】
ここで、式（１５）の新鮮度について図５を参照しながら説明する。新鮮度はセンサ毎に決められる。各センサはそれぞれ所定の計測周期でデータを観測するところ、計測時点の観測データの信頼性は高いが、計測時点から時間が経過するにつれてその計測時点のおける観測データの信頼性は低下する（つまり、観測データの新鮮度が減少する）と考えることができる。
【００６５】
例えば、カーブを走行している場合や車両の走行が安定していない場合などにおける計測時点からの観測データの変化度合いは、直線道路を走行している場合や車両が安定走行している場合に比べて、大きいと考えられるため（特に、横位置は大きく変化している可能性が高い）、計測時点から時間が経過するにつれてその時点における観測データの新鮮度は減少しているとみなすことができる。
【００６６】
そこで、式（１５）に示されるように、車両の横方向に係る運動状態に影響を与える因子である道路曲率κとヨーレートγの項を加えることによって、道路曲率κが大きいほどヨーレートγが大きいほど新鮮度が下がるように新鮮度を算出する演算式が設定され、式（１４）に示されるように新鮮度が下がれば総合信頼度Ｒａも下がるように総合信頼度Ｒａを算出する演算式が設定されている。また、センサによっては、計測時点でその計測が失敗する場合もある。そこで、図５に示されるように、計測失敗時はそのまま新鮮度を減少させて、総合信頼度Ｒａの算出に反映する。
【００６７】
また、今回演算時の観測量ｙ（ｔ）と今回演算時の観測量ｙ（ｔ）の推定観測量との差の絶対値が所定の基準値を越えた場合には、今回演算時の観測量ｙ（ｔ）の計測は失敗したとみなして、上記同様に、図５に示される新鮮度ｆ（ｔ）に従って新鮮度を減少させて、総合信頼度Ｒａの算出に反映するようにしてもよい。
【００６８】
なお、式（１５）は計測間隔が時間ではなく距離で規定する場合（すなわち、所定間隔毎に設置されている磁気マーカを通過するタイミング毎に計測する場合）、経過時間ｔを経過距離ｓに、周期Ｔを磁気マーカ設置距離間隔Ｓに置き換えることができる。これは、マーカセンサ１３の場合に適用すればよく、
新鮮度ｆ（ｓ）＝−Ｋ（κ，γ）×ｓ／Ｓ＋１  ・・・（１７）
と定義可能である。この場合、図５の横軸は、時間ではなく走行距離で規定される。各センサについての新鮮度が算出されれば、式（１４）に従い、各センサの総合信頼度Ｒａが、０〜１００の間の値に数値化される。
【００６９】
上述のように図４のステップ３０において各センサの総合信頼度が算出されると、総合信頼度は上述の観測方程式（２）における観測誤差標準偏差σに反映される（ステップ４０）。観測誤差標準偏差σは、センサが計測した観測データが信頼できるかどうかを表す指標である。観測データの誤差が大きくなるほど観測誤差標準偏差σは大きくなる。この観測誤差標準偏差σに上述のように算出された総合信頼度Ｒａを反映させる。総合信頼度Ｒａを観測誤差標準偏差σに反映させる考え方として、総合信頼度Ｒａが小さくなるほど観測誤差標準偏差σが大きな値になるように設定する。したがって、総合信頼度Ｒａの観測誤差標準偏差σへの反映手法の一例として、センサ毎に最良の計測状態における標準偏差をシミュレーションなどによって予め算出しておき、その最良計測状態での標準偏差をσｍｉｎとすると、観測誤差標準偏差σは、
σ＝（１００／Ｒａ）×σｍｉｎ  ・・・（１８）
と表すことができる。総合信頼度Ｒａは本実施例では０〜１００までの値を想定しているので、観測誤差標準偏差σの最小値はσｍｉｎに相当する。このように、カルマンフィルタ２００の観測方程式（２）内の観測誤差標準偏差σの項を上述のように算出された各センサの総合信頼度Ｒａに応じて変化させることができる。なお、観測方程式（２）内において、ＧＰＳ受信機１１の観測誤差標準偏差をσｇｐｓとし、白線認識装置１２の観測誤差標準偏差をσｗｌとし、マーカセンサ１３の観測誤差標準偏差をσｍａｇとし、ヨーレートセンサ１４の観測誤差標準偏差をσｙγとする。
【００７０】
したがって、上述のように算出された観測誤差標準偏差σを観測方程式（２）に代入して、上述したカルマンフィルタ２００の推定演算により、状態量（横位置η、横変位速度η’，ヨー角θ，ヨーレートγ）の推定値が演算される（図４のステップ５０）。
【００７１】
このように観測方程式（２）に観測誤差標準偏差σの項が存在することによって、カルマンフィルタ２００は、観測誤差標準偏差σが大きくなると、その観測データは信頼がおけないとして、演算処理することになる。観測方程式（２）にもあるように、ＧＰＳ受信機１１に基づいて横位置Ｄｇｐｓが観測され、白線認識装置１２に基づいて横位置Ｄｗｌが観測され、マーカセンサ１３に基づいて横位置Ｄｍａｇが観測されており、横位置Ｄについては冗長的に観測されている。したがって、冗長的に観測された横位置Ｄのうち、観測誤差標準偏差の小さいセンサの観測データのほうが演算に使われやすくなり、観測誤差標準偏差の大きいセンサの観測データのほうが演算に使われにくくなる。
【００７２】
上記のようにしてカルマンフィルタ２００での処理により状態量の推定値が演算されると、その推定値を用いて制御すべき操舵角δが次式に従って演算される（図４のステップ６０）。操舵角δは、
δ＝Ｋη・η＋Ｋη’・η’＋Ｋθ（θ−θTARGET）＋Ｋγ（γ−γTARGET）＋δｆ
・・・（１９）
によって演算可能である。なお、式（１９）において、Ｋη、Ｋη’、Ｋθ及びＫγは定数、θTARGETは目標ヨー角、γTARGETは目標ヨーレート、δｆはフィードフォワード操舵角である。このフィードフォワード操舵角δｆは、車両モデル、道路曲率、バンク角、車速などに基づいて決定される。この制御すべき操舵角δが演算されると、図２に示されるように、その操舵角δに基づいた操舵制御信号が制御ユニット５０から操舵アクチュエータ３２に出力され、この操舵制御信号に従って操舵アクチュエータ３２は駆動する（図４のステップ７０）。以後、上記処理が繰り返し実行される。このようにカルマンフィルタ２００での処理が繰り返し実行されることにより、横位置η、ヨーレートγ等の状態量の推定値は、順次本来得られるべき状態量の値に収束していく。
【００７３】
ところで、観測量ｙの異常値を状態量推定から排除する上述の方法では、標準誤差標準偏差σを算出するために単体信頼度及び走行環境信頼度、並びに新鮮度を使っていたが、標準誤差標準偏差σを算出するために単体信頼度、走行環境信頼度及び連続性信頼度、並びに新鮮度を使って、観測量ｙの異常値を状態量推定から排除してもよい。この場合、制御ユニット５０は、図９に示す手順での処理を実行する。図９に示す手順での処理は、上述の図４と同様に、隣接する磁気マーカ間において複数回演算できるような所定の演算周期で、上記の状態方程式（１）と観測方程式（２）に基づき、カルマンフィルタ２００の推定演算により、車両の状態量の推定値を演算している。図４に対しステップ２５が追加された図９の説明については、ステップ２５以外のステップについてはその説明を省略または簡略する。
【００７４】
図９に示される連続性信頼度とは、各センサの観測量の連続性を観点とする当該観測値の信頼性を評価するための指標である。連続性信頼度は、カルマンフィルタ２００による状態量推定の演算に用いられる観測量ｙ（ｔ）についての推定観測量と観測量を利用することによって算出される。今回演算時の観測量ｙ（ｔ）と今回演算時の観測量ｙ（ｔ）の推定観測量との差の絶対値が所定の基準値を越えた場合には、今回演算時の観測量ｙ（ｔ）は基準値を超えない場合の観測量ｙ（ｔ）に比べて連続性信頼度は低いとみなすことができる。また、連続性信頼度は、単体信頼度と走行環境信頼度にあわせて０〜１００の間の値になるように数値化され、例えば、
【００７５】
【数７】
2008077349.tif 000009
のように、観測量ｙ（ｔ）についての推定観測量と観測量との差の関数Ｌによって定義する。関数Ｌは、観測量ｙ（ｔ）についての推定観測量と観測量との差が大きいほど連続性信頼度が小さくなるような関数とする。例えば、『関数Ｌ（Ｚ）＝ｌ／｜Ｚ｜』としてもよい。すなわち、関数Ｌは、Ｚの絶対値の逆数に比例する関数（ｌは定数）である。
【００７６】
上述の図９にしたがって、ステップ１０において各センサの単体信頼度が取得され、ステップ２０において各センサの走行環境信頼度が算出されるとともに、上述のように各センサの連続性信頼度が算出されると（ステップ２５）、各センサの総合信頼度が算出される（ステップ３０）。
【００７７】
図９に示される場合のセンサの総合信頼度とは、上述の単体信頼度と走行環境信頼度と連続性信頼度を加味した、各センサの検出信号の正確さの度合いを示すものである。図９に示される場合の総合信頼度Ｒａは、
総合信頼度Ｒａ＝
（ｋ１×単体信頼度＋ｋ２×走行環境信頼度＋ｋ３×連続性信頼度）×新鮮度
・・・（２１）
によって演算され得る。式（２１）における新鮮度は、上述の式（１５）と同じである。ここで、式（２１）におけるｋ１，ｋ２，ｋ３は『ｋ１＋ｋ２＋ｋ３＝１，０≦ｋ１≦１，０≦ｋ２≦１，０≦ｋ３≦１』を満たす係数である。
【００７８】
上述のように図９のステップ３０において各センサの総合信頼度が算出されると、総合信頼度は上述の観測方程式（２）における観測誤差標準偏差σに反映される（ステップ４０）。観測誤差標準偏差σは、式（１８）と同様に表すことができる。
【００７９】
したがって、上述のように算出された観測誤差標準偏差σを観測方程式（２）に代入して、上述したカルマンフィルタ２００の推定演算により、状態量（横位置η、横変位速度η’，ヨー角θ，ヨーレートγ）の推定値が演算される（図９のステップ５０）。
【００８０】
このように観測方程式（２）に観測誤差標準偏差σの項が存在することによって、カルマンフィルタ２００は、観測誤差標準偏差σが大きくなると、その観測データは信頼がおけないとして、演算処理することになる。観測方程式（２）にもあるように、ＧＰＳ受信機１１に基づいて横位置Ｄｇｐｓが観測され、白線認識装置１２に基づいて横位置Ｄｗｌが観測され、マーカセンサ１３に基づいて横位置Ｄｍａｇが観測されており、横位置Ｄについては冗長的に観測されている。したがって、冗長的に観測された横位置Ｄのうち、観測誤差標準偏差の小さいセンサの観測データのほうが演算に使われやすくなり、観測誤差標準偏差の大きいセンサの観測データのほうが演算に使われにくくなる。
【００８１】
上記のようにしてカルマンフィルタ２００での処理により状態量の推定値が演算されると、その推定値を用いて制御すべき操舵角δが式（１９）に従って演算される（図９のステップ６０）。この制御すべき操舵角δが演算されると、図２に示されるように、その操舵角δに基づいた操舵制御信号が制御ユニット５０から操舵アクチュエータ３２に出力され、この操舵制御信号に従って操舵アクチュエータ３２は駆動する（図９のステップ７０）。以後、上記処理が繰り返し実行される。このようにカルマンフィルタ２００での処理が繰り返し実行されることにより、横位置η、ヨーレートγ等の状態量の推定値は、順次本来得られるべき状態量の値に収束していく。
【００８２】
したがって、本実施形態によれば、各センサの処理結果に基づく単体信頼度及び計測状況に応じた各センサの長短所を制御ユニット５０によって考慮された走行環境信頼度から総合信頼度を算出し（または、単体信頼度及び走行環境信頼度、並びに各センサの観測値の連続性を考慮した連続性信頼度から総合信頼度を算出し）、また、非同期の各センサが計測したタイミングからの経過時間（あるいは経過距離）に従って減少する新鮮度を算出し、それらの算出された総合信頼度と新鮮度を観測誤差標準偏差σに反映させてカルマンフィルタの推定を行っているので、磁気マーカ間であっても直前に計測した横変位をカルマンフィルタの状態推定に使用し、カルマンフィルタによる状態量推定の精度や信頼性を向上させることができる。すなわち、磁気マーカ間での状態量の推定値の正確性を維持することができ、磁気マーカなどを設置した所定のチェックポイントにて状態量を観測する周期が長くなっても、より正確な車両の状態量の推定ができるような車両状態量推定装置を実現することができる。また、より高い精度での車両の操舵制御が可能な、そのような車両状態量推定装置を用いた車両操舵制御装置を実現することができる。
【００８３】
また、本実施形態によれば、想定外に大きな誤差（例えば、故障や瞬間的な劣環境による誤差）をもった観測量が観測されたとしても、推定観測量と実観測量との差が所定の基準値を超えることによって、その観測量に関する連続性信頼度が低下し、その観測値の状態推定における重みを下げることができる。すなわち、異常値を示す観測量の信頼度を一時的に落として、その異常観測値の状態量推定に対する影響を排除することで、図７の点線で示されるように、状態量の推定値の誤差を抑えて推定精度の悪化を防止することができる。
【００８４】
以上、本発明の好ましい実施例について詳説したが、本発明は、上述した実施例に制限されることはなく、本発明の範囲を逸脱することなく、上述した実施例に種々の変形及び置換を加えることができる。
【００８５】
例えば、本発明は、図６に示されるカルマンフィルタの構成にも実施することが可能である。図６に示されるカルマンフィルタは、磁気マーカが検出されるごとにカルマンフィルタ２００ａの機能に従って車両１００の状態量（ヨーレート、ヨー角、横変位速度、横位置）の推定値を演算する距離軸カルマンフィルタと，所定周期ごとにカルマンフィルタ２００ｂの機能に従って車両１００の状態量（ヨーレート、ヨー角、横変位速度、横位置）の推定値を演算する時間軸カルマンフィルタを備える。図６に示されるカルマンフィルタは、磁気マーカ通過時に横位置とヨーレートを観測値として状態推定する距離軸カルマンフィルタ２００ａの推定値で、ヨーレートのみを観測値として状態推定する時間軸カルマンフィルタ２００ｂの状態量を初期化することで、横位置の計測が不能な磁気マーカ間の状態量の推定精度を向上させるものである。距離軸カルマンフィルタ２００ａと時間軸カルマンフィルタ２００ｂのそれぞれに対して、上述の総合信頼度と新鮮度を観測誤差標準偏差σに反映させた上記の状態方程式（１）と観測方程式（２）と同様の定式化を行えば、状態量推定の精度や信頼性が向上する。
【００８６】
また、上述の実施例では、走行環境信頼度の算出を車両毎に行っていたが、走行環境信頼度のデータベースを構築して、各車両がそのデータベースから走行環境信頼度を取得してもよい。例えば、各センサの計測結果及びその単体信頼度、並びに、各センサの計測時の走行状態、天候及び路面状況などの同一地点における計測情報を各車両が所定の管理センターに送信する。一方、その管理センターは、各車両からそれらの計測情報を取得し、走行状況、天候、路面状況などを指標に各センサの信頼度データベースを統計的に構築する。そして、各車両は、管理センターから路車間通信などの通信回線を介して、信頼度データベース内の走行環境信頼度を取得し、上述の状態量推定に利用すればよい。これにより、各車両において算出される走行環境信頼度に比べ、より客観性の高い走行環境信頼度を導き出すことができるようになる。
【００８７】
また、上述の実施例では、カルマンフィルタを例に挙げたが、センサによって観測された所定状態量を車両の運動状態のモデルに入力して車両の運動状態を表す状態量を推定する推定器は、特にカルマンフィルタに限定されない。この推定器は、繰り返し演算することによって推定値が求まるべき真の値に収束するものであればよい。推定器は、その推定処理の中で、センサの計測値をどれぐらい信用して推定するかを決める数値（本実施例のカルマンフィルタでいえば、観測誤差標準偏差σ）にそのセンサの信頼度を反映させればよい。
【図面の簡単な説明】
【００８８】
【図１】カルマンフィルタを用いた手法を説明するためのブロック図である。
【図２】本発明の実施の一形態に係る車両状態量推定装置が適用される車両操舵制御装置の構成を示すブロック図である。
【図３】モデル化した車両に対して設定される座標系及び各変数の定義を示す図である。
【図４】制御ユニット５０の処理の第１の手順を示すフローチャートである。
【図５】新鮮度の算出を説明するための図である。
【図６】カルマンフィルタを用いた手法を説明するためのブロック図である。
【図７】観測値と状態推定値との関係を示した図である。
【図８】観測量ｙの異常値を状態量推定から排除する一方法のフローである。
【図９】制御ユニット５０の処理の第２の手順を示すフローチャートである。
【符号の説明】
【００８９】
１１  ＧＰＳ受信機
１２  白線認識装置
１３  マーカセンサ
１４  ヨーレートセンサ
１５  Ｇセンサ
１６  車輪速センサ
１７  操舵角センサ
２０  メモリユニット
３１  操舵制御部
３２  操舵アクチュエータ
５０  制御ユニット
１００  車両
２００  カルマンフィルタ
【図１】
2008077349.tif 000010
【図２】
2008077349.tif 000011
【図３】
2008077349.tif 000012
【図４】
2008077349.tif 000013
【図５】
2008077349.tif 000014
【図６】
2008077349.tif 000015
【図７】
2008077349.tif 000016
【図８】
2008077349.tif 000017
【図９】
2008077349.tif 000018