JP2013129289A - 車両状態量推定装置、車両用操舵制御装置 - Google Patents

Abstract

【課題】精度良く車両の状態量を推定することが可能な車両用状態量推定装置等を提供すること。
【解決手段】車両に搭載され、車両に関する観測を行い観測量を出力する複数の観測手段と、複数の観測手段により出力される観測量の信頼度を、観測量毎に算出する信頼度算出手段と、観測手段により出力された観測量、及び信頼度算出手段により算出された観測量の信頼度を推定モデルに入力して車両の状態量を推定する推定手段と、を備える車両状態量推定装置であって、複数の観測手段には、同一種類の観測量を複数出力可能な観測手段が含まれ、信頼度算出手段は、同一の観測手段により出力された同一種類の複数の観測量を含めた複数の観測量について信頼度を算出し、推定手段は、同一の観測手段により出力された同一種類の複数の観測量のそれぞれを前記推定モデルに入力して車両の状態量を推定することを特徴とする車両状態量推定装置。
【選択図】図１

Description

本発明は、車両に搭載されたセンサ等により観測された観測量に基づき車両の状態量を推定する車両状態量推定装置、及びこれを利用した車両用操舵制御装置に関する。

従来、車両に搭載された複数のセンサ等（加速度センサや車速センサ等、いわゆるセンサ類の他、カメラセンサ、レーダセンサ、通信により情報を取得する装置等をいう）により得られた情報を総合的に加味して車両の状態量を推定する技術について研究が進められている。

特許文献１には、車両の運動状態を表す所定状態量を観測するＧＰＳ受信機等のセンサ等と、それらのセンサ等によって観測された所定状態量を車両の運動状態のモデルに入力して所定状態量を含む車両の運動状態を表す状態量を推定するカルマンフィルタとを有する車両状態量推定装置について記載されている。この装置では、複数のセンサ等によって観測された所定状態量の信頼度が評価され、所定状態量のカルマンフィルタにおける反映度合いが、信頼度に基づいて設定される。

特開２００７−３３１７１５号公報

しかしながら、上記従来の装置は、同一のセンサ等が複数の観測量を出力する可能性についての考慮がなされていない。この結果、推定精度が十分なものとならない場合がある。

本発明は、一側面によれば、精度良く車両の状態量を推定することが可能な車両用状態量推定装置等を提供することを目的とする。

上記目的を達成するための本発明の第１の態様は、
車両に搭載され、車両に関する観測を行い観測量を出力する複数の観測手段と、
前記複数の観測手段により出力される観測量の信頼度を、観測量毎に算出する信頼度算出手段と、
前記観測手段により出力された観測量、及び前記信頼度算出手段により算出された観測量の信頼度を推定モデルに入力して車両の状態量を推定する推定手段と、
を備える車両状態量推定装置であって、
前記複数の観測手段には、同一種類の観測量を複数出力可能な観測手段が含まれ、
前記信頼度算出手段は、前記同一の観測手段により出力された同一種類の複数の観測量を含めた複数の観測量について信頼度を算出し、
前記推定手段は、前記同一の観測手段により出力された同一種類の複数の観測量のそれぞれを前記推定モデルに入力して車両の状態量を推定することを特徴とする、
車両状態量推定装置である。

この本発明の一態様によれば、複数の観測手段には、同一種類の観測量を複数出力可能な観測手段が含まれ、信頼度算出手段は、同一の観測手段により出力された同一種類の複数の観測量を含めた複数の観測量について信頼度を算出し、推定手段は、同一の観測手段により出力された同一種類の複数の観測量のそれぞれを推定モデルに入力して車両の状態量を推定するため、精度良く車両の状態量を推定することができる。

本発明の第１の態様において、
前記推定モデルは、カルマンフィルタであり、
該カルマンフィルタにおける観測方程式の各行列は、前記同一の観測手段により出力された複数の観測量を含めた複数の観測量分の行数を有し、前記同一の観測手段により出力された複数の観測量に対応する行は同一の成分を有することを特徴とするものとしてもよい。

また、本発明の第１の態様において、
前記観測方程式には、前記複数の観測値の観測誤差の標準偏差の項が追加され、
該標準偏差は、前記信頼度算出手段により算出された信頼度が低くなる程、大きく設定されることを特徴とするものとしてもよい。

また、本発明の第１の態様において、
前記複数の観測手段は、車両の周囲の道路を撮像して道路区画線を認識し、走行車線と車両の位置関係を前記観測量として出力する手段を含み、
前記推定手段は、走行車線と車両の位置関係を前記車両の状態量として推定する手段であるものとしてもよい。

この場合、
前記複数の観測手段が出力する観測量は、横偏差、道路曲率、及びヨー角のいずれかを含み、
前記推定手段が出力する車両の状態量は、車両重心の横偏差、横方向速度、ヨー角、及びヨーレートのいずれかを含むものとしてもよい。

本発明の第２の態様は、
本発明の第１の態様の車両状態量推定装置と、
該車両状態量推定装置によって推定された車両の状態量に基づいて操舵制御を行う制御装置と、
を備える車両用操舵制御装置である。

この本発明の第２の態様によれば、精度良く操舵制御を行うことができる。

本発明は、一側面によれば、精度良く車両の状態量を推定することが可能な車両用状態量推定装置等を提供することができる。

本発明の一実施例に係る車両用状態量推定装置１、及びこれを利用した車両用操舵制御装置１００のシステム構成例である。 各カメラセンサによる撮像範囲の一例である。 単体信頼度算出部１０Ａａ〜１４Ａが各センサに付随する構成を例示した図である。 単体信頼度算出部１０Ａａ〜１４Ａが推定処理用ＥＣＵ３０の機能である構成を例示した図である。 本実施例のカルマンフィルタ処理部３２が扱う各種パラメータの関係を簡単に示す図である。 カルマンフィルタ処理部３２により実行される処理の流れを示すフローチャートである。

以下、本発明を実施するための形態について、添付図面を参照しながら実施例を挙げて説明する。

［構成、観測量］
以下、図面を参照し、本発明の一実施例に係る車両用状態量推定装置、及びこれを利用した車両用操舵制御装置について説明する。

図１は、本発明の一実施例に係る車両用状態量推定装置１、及びこれを利用した車両用操舵制御装置１００のシステム構成例である。車両用状態量推定装置１は、主要な構成として、前方カメラセンサ１０と、後方カメラセンサ１１と、右側方カメラセンサ１２と、左側方カメラセンサ１３と、ヨーレートセンサ１４と、操舵角センサ２０と、ナビゲーション装置２１と、推定処理用ＥＣＵ（Electronic Control Unit）３０と、を備える。推定処理用ＥＣＵ３０は、例えばＣＰＵ（Central Processing Unit）を中央としてＲＯＭ（Read Only Memory）やＲＡＭ（Random Access Memory）等がバスを介して相互に接続されたマイクロコンピュータであり、その他、ＨＤＤ（Hard Disc Drive）やＤＶＤ−Ｒ（Digital Versatile Disk-Recordable）ドライブ、ＣＤ−Ｒ（Compact Disc‐Recordable）ドライブ、ＥＥＰＲＯＭ（Electronically Erasable and Programmable Read Only Memory）等の記憶装置やＩ／Ｏポート、タイマー、カウンター等を備える。記憶装置には、ＣＰＵが実行するプログラムやデータが格納されている。また、推定処理用ＥＣＵ３０は、係るプログラムをＣＰＵが実行することにより機能する主要な機能ブロックとして、総合信頼度算出部３１と、カルマンフィルタ処理部３２と、を備える。なお、これらの機能ブロックが明確に別のプログラムに基づくものである必要はなく、同一プログラムの中に複数の機能ブロックを実現する部分が含まれていてもよい。また、機能ブロックの一部が、ＬＳＩ（Large Scale Integrated circuit）、ＩＣ（Integrated Circuit）やＦＰＧＡ（Field Programmable Gate Array）等のハードウエア手段であっても構わない。

前方カメラセンサ１０は、例えば車両のフロントウインドシールド上部等に取り付けられ、車両前方を撮像するカメラを有する。また、前方カメラセンサ１０は、撮像した画像を解析して道路区画線を認識し、画像において道路区画線と判断された画素の配置に基づいて、車両における観測量の一例である横偏差、曲率、ヨー角を算出して推定処理用ＥＣＵ３０に出力する画像解析装置を有する。

画像解析装置は、例えば、画像において隣接画素との間で所定以上の輝度差を有する画素を特徴点として抽出し、直線状に並ぶ特徴点が平行に存在する場合に、当該直線状に並ぶ特徴点で画成される領域を道路区画線候補として認識する。

横偏差とは、センサ位置の基準位置（例えば走行車線中央）からの変位である。横偏差は、上記認識された道路区画線の画像上の位置から、カメラの設置パラメータ（ロール、パン、ピッチ、設置された高さ、焦点距離等）を考慮した幾何学的な関係を解析して導出することができる。また、走行車線中央を基準位置とする場合、横偏差を算出する際には車線幅（幅員）の情報が必要となるが、この情報については、各カメラセンサの出力。ナビゲーション装置２１からの情報等に基づき取得することができる。

曲率とは、走行車線の曲率である。また、ヨー角とは、基準方向（例えば走行車線の中央線の方向）と車両中央軸とのなす角度である。これらの観測量についても、認識された道路区画線の画像上の位置から、カメラの設置パラメータを考慮した幾何学的な関係を解析して導出することができる。

前方カメラセンサ１０は、観測量｛横偏差、曲率、ヨー角｝の組み合わせを、複数セット（図１では２セット）分出力することができる。このように観測量が複数セット出力される状況としては、カメラの画像において、上記手法によって道路区画線と認識され得る要素（日陰と日なたの切り替わり部分、道路区画線以外の道路ペイント、他車線の白線、路肩、中央分離帯等）が存在するような状況である。このような場合に前方カメラセンサ１０は、いずれが真に道路区画線であるかを判断して絞り込むことなく、複数の観測量として出力する。なお、前方カメラセンサ１０は、画像上で道路区画線と認識されるものが一つしか存在しない場合は、１セットの観測量を出力する。

後方カメラセンサ１１は、例えば車両後方のナンバープレート付近等に取り付けられ、車両後方を広角度に撮像するカメラを有する。また、後方カメラセンサ１１は、撮像した画像を解析して道路区画線を認識し、画像において道路区画線と判断された画素の配置に基づいて、車両における観測量の一例である横偏差（同上）を算出して推定処理用ＥＣＵ３０に出力する画像解析装置を有する。

右側方カメラセンサ１２は、例えば車両右側のドアミラー付近等に取り付けられ、車両右側方を撮像するカメラを有する。また、右側方カメラセンサ１２は、撮像した画像を解析して道路区画線を認識し、画像において道路区画線と判断された画素の配置に基づいて、車両における観測量の一例である横偏差（同上）を算出して推定処理用ＥＣＵ３０に出力する画像解析装置を有する。

左側方カメラセンサ１３は、例えば車両左側のドアミラー付近等に取り付けられ、車両左側方を撮像するカメラを有する。また、左側方カメラセンサ１３は、撮像した画像を解析して道路区画線を認識し、画像において道路区画線と判断された画素の配置に基づいて、車両における観測量の一例である横偏差（同上）を算出して推定処理用ＥＣＵ３０に出力する画像解析装置を有する。

図２は、各カメラセンサによる撮像範囲の一例である。なお、各カメラセンサが有する画像解析装置は、物理的にそれぞれのカメラに付随する必要はなく、より少数の装置に統合されてもよい（推定処理用ＥＣＵ３０の機能であってもよい）。

ヨーレートセンサ１４は、例えば、車両のセンターコンソール部の下方に取り付けられ、圧電セラミックスの歪み量と方向により、車両の鉛直軸まわりの回転角速度（ヨーレート）を検出する。

操舵角センサ２０は、例えば、ステアリングコラム内部に取り付けられ、操舵角信号を推定処理用ＥＣＵ３０に出力する。

ナビゲーション装置２１は、例えば、ＧＰＳ受信機、地図データを格納した記憶装置、ナビゲーションコンピュータ等を備える。ナビゲーション装置２１は、ＧＰＳ受信機によって特定された車両の位置を用いて地図データを検索し、道路曲率やカント角（バンク角）を取得して推定処理用ＥＣＵ３０に出力する。

［信頼度算出］
これらのカメラセンサやヨーレートセンサ等は、誤差が重畳することが想定される。そこで、本実施例の車両用状態量推定装置１は、各センサの出力する観測量（前方カメラセンサ１０のように複数セットの観測量を出力する場合は、各観測量）に対応した単体信頼度算出部１０Ａａ〜１４Ａを備える。観測量毎の単体信頼度は、総合信頼度算出部３１によって各センサの観測量毎の総合信頼度に加工された上で、カルマンフィルタ処理部３２に供給される。

単体信頼度算出部１０Ａａ〜１４Ａは、図３に示すように、それぞれが各センサに付随する構成であってもよいし、図４に示すように、推定処理用ＥＣＵ３０の機能であってもよい。前者の場合、単体信頼度算出部は、各センサに取り付けられたマイクロコンピュータ（画像解析装置と同じものであってよい）、ＬＳＩ、ＩＣ、ＦＰＧＡ等の処理手段である。また、図３と図４の中間として、各センサに付随する単体信頼度算出部と、推定処理用ＥＣＵ３０の機能である単体信頼度算出部が混在しても構わない。

ここで、各観測量についての単体信頼度について説明する。単体信頼度は、各観測量の正確さを示す値である。

各カメラセンサが出力する観測量の単体信頼度は、輝度差、道路区画線の平行度、トラッキング度等の道路区画線認識結果に応じて設定される。単体信頼度算出部１０Ａａ〜１３Ａは、道路区画線認識の際の輝度差（平均）が大きいほど単体信頼度を高く算出し、道路区画線の両側の輪郭と考えられる直線の平行度が高いほど単体信頼度を高く算出し、トラッキング度（微小時間における変動の小ささ）が高いほど単体信頼度を高く算出する。また、単体信頼度算出部１０Ａａ〜１３Ａは、道路区画線と認識されるものの数が多ければ単体信頼度を低く算出し、少なければ単体信頼度を高く算出する。

ヨーレートセンサ１４が出力する観測量の単体信頼度は、例えば、検出信号の周波数成分のスペクトル強度に応じて設定される。単体信頼度算出部１４Ａは、車両の動きとは考えにくい高周波成分（例えば、１０Ｈｚ）以上の周波数成分が多くなるほど単体信頼度を低く算出して推定処理用ＥＣＵ３０に出力する。

総合信頼度算出部３１は、各観測量の単体信頼度に基づき、観測量毎の総合信頼度を算出する。総合信頼度は、例えば０〜１００までの値で設定される。

総合信頼度算出部３１は、単体信頼度算出部１０Ａａ〜１３Ａから入力された単体信頼度を、例えば、天候、日照、路面材質等に基づいて修正する。総合信頼度算出部３１は、道路区画線が認識しにくい検出状況（悪天候、夜間、路面材質がアスファルトではなくコンクリートである）の場合には、単体信頼度を低い値に修正した値を総合信頼度とする。これらの天候、時刻、路面材質等の情報は、例えば車載インターネット設備、車載時計、ナビゲーション装置２１からの情報に基づいて取得する。

また、総合信頼度算出部３１は、単体信頼度算出部１４Ａから入力された単体信頼度を、例えば、路面の粗さ、路面のうねり、路面材質などに基づいて修正する。総合信頼度算出部３１は、路面の粗さや路面のうねりが大きい場所では小さい場所に比べて単体信頼度を低い値に修正した値を総合信頼度とする。

総合信頼度算出部３１は、上記各修正基準に限らず、車両形状（車高、車幅、全長）、車種、ボディ材質等、他の要素を加味して総合信頼度を算出してもよい。

［カルマンフィルタ］
（定義、原理等）
カルマンフィルタ処理部３２は、各センサにより出力された観測量、及び観測量毎の信頼度をカルマンフィルタに入力して車両の状態量を推定する。車両の状態量には、車両重心の横偏差（基準位置からの変位）、車両の横方向速度、ヨー角、及びヨーレートが含まれる。

各センサが出力する観測量には、測定雑音が定常的に含まれ、更に、車両に固有の雑音成分（システム雑音）も含まれている。カルマンフィルタ処理部３２は、カルマンフィルタによって状態量の推定値を繰り返し演算することによってカルマンゲインを最適化し、当該状態量の推定値が、雑音成分が除かれた真の値に収束するように処理する。

カルマンフィルタには、入力量として、操舵角センサ２０から入力される操舵角、ナビゲーション装置２１から入力される道路曲率やカント角が入力される。

カルマンフィルタ処理部３２は、状態方程式と観測方程式を設定する。カルマンフィルタ処理部３２が設定する状態方程式は次式（１）で表され、観測方程式は次式（２）で表される。

（ｄ／ｄｔ）（状態量）^T＝Ａ・（状態量）^T＋Ｂ・（入力量（影響因子））^T＋Ｇ・（システム雑音） …（１）
（観測量）^T＝Ｃ・（状態量）^T＋Ｄ・（入力量（影響因子））^T＋（観測誤差標準偏差） …（２）

上式における状態量は、カルマンフィルタ処理部３２が推定する車両の状態量であり、（η，η｀，θ，γ）で表される。ηは車両重心の横偏差であり、η｀はηの微分、すなわち車両の横方向速度であり、θはヨー角であり、γはヨーレートである。なお、η｀は数式中におけるηにドットが付されたものと同様、微分を意味するものとする。

また、上式における観測量は、各センサが出力した観測量であり、（Ｄ_ｆｒ１，Ｄ_ｆｒ２，Ｄ_ｒｒ，Ｄ_ｓｒ，Ｄ_ｓｌ，γ_ｙ）で表される。Ｄ_ｆｒ１は前方カメラセンサ１０が出力した横偏差（その１）であり、Ｄ_ｆｒ２は前方カメラセンサ１０が出力した横偏差（その２）であり、Ｄ_ｒｒは後方カメラセンサ１１が出力した横偏差であり、Ｄ_ｓｒは右側方カメラセンサ１２が出力した横偏差であり、Ｄ_ｓｌは左側方カメラセンサ１３が出力した横偏差であり、γ_ｙはヨーレートセンサ１４が出力したヨーレートである。

また、上式における入力量は、（δ，κ，α）で表される。δは操舵角センサ２０が出力した操舵角であり、κ、αはナビゲーション装置が出力した曲率、カント率である。

また、上式における観測誤差標準偏差は、（ν_ｆｒ，ν_ｆｒ，ν_ｒｒ，ν_ｓｒ，ν_ｓｌ，ν_γｙ）で表される。観測誤差標準偏差νは、観測量の誤差が大きくなるほど大きくなる値である。観測誤差標準偏差νは、総合信頼度算出部３１により算出された総合信頼度が大きくなるほど大きくなるように、カルマンフィルタ処理部３２によって設定される。

観測誤差標準偏差νは、例えばセンサ毎に最良の計測状態における誤差の標準偏差をシミュレーションなどによって予め算出しておき、その最良計測状態における誤差の標準偏差をνｍｉｎとすると、（１００／総合信頼度）×νｍｉｎ等と設定される。これによって、冗長的に観測された観測量のうち、観測誤差標準偏差の小さいものが大きいウエイトで演算に用いられ、観測誤差標準偏差の大きいものが小さいウエイトで演算に用いられることになる。

図５は、上記説明した、本実施例のカルマンフィルタ処理部３２が扱う各種パラメータの関係を簡単に示す図である。図中、ξは基準位置の軌跡の接線であり、ｘは車両中心軸方向であり、Ｌは重心から車両前端までの距離であり、ｌｆ、ｌｒは重心と前、後輪との間の距離である。

カルマンフィルタについて、より詳細に説明する。次式（３）、（４）は、上式（１）、（２）における行列Ａ、Ｂ、Ｃ、Ｄ、Ｇを具体的に示したものである。

上式（３）、（４）等に用いた各パラメータの一覧を以下に示す。
・η ：車両重心の横偏差
・η｀：車両重心の横方向速度
・θ ：ヨー角
・γ ：ヨーレート（状態量）
・γ_ｙ：ヨーレート（観測量）
・δ ：操舵角
・κ ：道路曲率
・α ：カント角
・Ｄ_ｆｒ１、Ｄ_ｆｒ２、Ｄ_ｒｒ、Ｄ_ｓｒ、Ｄ_ｓｌ：横偏差（観測値）
・ｗη，ｗη｀，ｗθ，ｗγ：状態量のシステム雑音
・Ｖ ：車速
・ｇ ：重力加速度
・ｍ ：車両質量
・Ｉ ：ヨー慣性モーメント
・Ｋｆ，Ｋｒ ：前・後輪のコーナリングパワー
・ｌｆ，ｌｒ ：重心と前・後輪との間の距離
・Ｌ ：重心から車両前端までの距離

カルマンフィルタ処理部３２は、状態方程式（３）と観測方程式（４）に基づき、状態量（η，η｀，θ，γ）を繰り返し演算することによって、状態量の推定値を雑音成分が除かれた真の値に収束させる。カルマンフィルタ処理部３２は、推定値を繰り返し演算する過程で、状態方程式（３）に示す微分方程式を解く際に前回得られた推定値を用いる。

観測方程式（４）における同一のセンサから出力された観測量に対応する行は、同じ値となっている。これは、センサの位置やセンサの誤差量がセンサの環境によって一意に決定されることに基づく。

以下、カルマンフィルタ処理部３２が行う具体的処理の中で必要となる数式等について更に説明する。なお、以下に示す行列Ａ、Ｂ、Ｃ、Ｄ、Ｇは、状態方程式（３）、観測方程式（４）と同じものである。

（必要な数式等）
システム雑音の標準偏差を成分とする行列Ｑｋと、観測誤差標準偏差を成分とする行列Ｒｋの関係は、次式（５）で表される。式（５）におけるＥは、相関演算を意味する記号であり、δｔτは、クロネッカーのデルタを意味する記号である。ｔ＝τのときにはδｔτ＝１であり、ｔ＝τではないときにはδｔτ＝０である。

行列Ｑｋは次式（６）で表され、行列Ｐｋは次式（７）で表される。式（６）における対角値ση、ση｀、σθ、σγは、それぞれシステム雑音ｗη、ｗη｀、ｗθ、ｗγの標準偏差である。システム雑音の標準偏差は、システムの特性に応じて任意に設定される設定値であり、固定値でもよいし、外乱の変化などに応じて変更される可変値としてもよい。

また、カルマンフィルタ処理部３２は、推定誤差共分散行列Ｐ_Ｋを繰り返し予測する処理を行う。推定誤差共分散行列Ｐ_Ｋは、次式（８）で表される。推定誤差共分散行列Ｐ_Ｋの対角値は、カルマンフィルタ処理部３２によって推定された車両重心の横偏差、車両重心の横方向速度、ヨー角、ヨーレートの推定誤差である。推定誤差共分散行列Ｐ_Ｋの対角値は、その値が大きいほど、対応する状態量の推定精度が低いことを示す。式（８）の・は、任意の値を示す。

カルマンフィルタ処理部３２は、前回予測した推定誤差共分散行列Ｐ_{Ｋ（ｔ／ｔ−１）}を用いて、今回の推定誤差共分散行列Ｐ_{Ｋ（ｔ／ｔ）}を求める。次式（９）は、前回予測した推定誤差共分散行列Ｐ_{Ｋ（ｔ／ｔ−１）}を用いて、今回の推定誤差共分散行列Ｐ_{Ｋ（ｔ／ｔ）}を求めるための共分散方程式である。

式（９）におけるＫ_Ｋ（ｔ）は、カルマンゲインであり、次式（１０）で求められる。

更に、カルマンフィルタ処理部３２は、今回決定した推定誤差共分散行列Ｐ_{Ｋ（ｔ／ｔ）}を用いて、次回の推定誤差共分散行列Ｐ_{Ｋ（ｔ＋１／ｔ）}を予測するために、次式（１１）で表される共分散方程式を解き、次回の推定誤差共分散行列Ｐ_{Ｋ（ｔ＋１／ｔ）}を予測する。

（具体的な処理内容）
カルマンフィルタ処理部３２は、状態方程式（３）と観測方程式（４）を解くために、次式（１２）で表されるフィルタ方程式を設定する。式中、「ハットｘ」は、ｘの推定値であることを示す。また、Ｋ_Ｋ（ｔ）は、今回のカルマンゲインである。カルマンフィルタ処理部３２は、まず、観測方程式（４）の行列Ｃ、式（７）の行列Ｒ_Ｋ、及び前回予測された推定誤差共分散行列Ｐ_{Ｋ（ｔ／ｔ−１）}を式（１０）に適用し、今回のカルマンゲインＫ_Ｋ（ｔ）を求める。そして、カルマンフィルタ処理部３２は、今回の観測量ｙ（ｔ）と前回予測した状態量ｘ（ｔ／ｔ−１）を用いて、式（１２）により今回の状態量ｘ（ｔ）を決定する。また、カルマンフィルタ処理部３２は、上式（９）を解き、今回の推定誤差共分散行列Ｐ_{Ｋ（ｔ／ｔ）}を求めておく。以上の処理を処理Ａとする。

更に、カルマンフィルタ処理部３２は、今回決定した状態量ｘ（ｔ／ｔ）と今回の入力量ｕ（ｔ）を用いて次回の状態量ｘ（ｔ＋１／ｔ）を予測するために、次式（１３）で表されるフィルタ方程式を設定する。カルマンフィルタ処理部３２は、フィルタ方程式（１３）を解き、次回の状態量ｘ（ｔ＋１／ｔ）を予測しておく。また、カルマンフィルタ処理部３２は、今回決定した推定誤差共分散行列Ｐ_{Ｋ（ｔ／ｔ）}を用いて、式（１１）の共分散方程式を解き、次回の推定誤差共分散行列Ｐ_{Ｋ（ｔ＋１／ｔ）}を予測しておく。以上の処理を処理Ｂとする。これらの予測値は、次回の状態量ｘ（ｔ＋１）、推定誤差共分散行列Ｐ_{Ｋ（ｔ＋１／ｔ＋１）}を決定する際に用いられる。

カルマンフィルタ処理部３２は、観測量、入力量、総合信頼度の入力、処理Ａ、処理Ｂを１セットの処理として、この１セットの処理を繰り返し行うことにより、カルマンゲインを最適な値に収束させる。これによって、状態量（η、η’、θ、γ）の真の値を得ることができる。

図６は、カルマンフィルタ処理部３２により実行される処理の流れを示すフローチャートである。

まず、カルマンフィルタ処理部３２に対し、観測量、入力量、総合信頼度が入力されるまで待機する（Ｓ１００）。これらの値は、例えばソフトウエア割り込みによってカルマンフィルタ処理部３２に入力される。

カルマンフィルタ処理部３２は、観測量、入力量、総合信頼度が入力されると、総合信頼度に基づき観測誤差標準偏差を算出する（Ｓ１０２）。

次に、カルマンフィルタ処理部３２は、行列Ｃ、行列Ｒ_Ｋ、及び前回予測された推定誤差共分散行列Ｐ_{Ｋ（ｔ／ｔ−１）}を用いて今回のカルマンゲインＫ_Ｋ（ｔ）を求める（Ｓ１０４）。

次に、カルマンフィルタ処理部３２は、今回の観測量ｙ（ｔ）と前回予測した状態量ｘ（ｔ／ｔ−１）を用いて、式（１２）により今回の状態量ｘ（ｔ）を決定する（Ｓ１０６）。

次に、カルマンフィルタ処理部３２は、今回の推定誤差共分散行列Ｐ_{Ｋ（ｔ／ｔ）}を求める（Ｓ１０８）。

次に、カルマンフィルタ処理部３２は、フィルタ方程式（１３）を解き、次回の状態量ｘ（ｔ＋１／ｔ）を予測しておく（Ｓ１１０）。

次に、カルマンフィルタ処理部３２は、今回決定した推定誤差共分散行列Ｐ_{Ｋ（ｔ／ｔ）}を用いて、式（１１）の共分散方程式を解き、次回の推定誤差共分散行列Ｐ_{Ｋ（ｔ＋１／ｔ）}を予測しておく（Ｓ１１２）。Ｓ１１２の処理が終了すると、Ｓ１００に戻り、入力がなされるまで待機する。

［操舵制御］
操舵制御用ＥＣＵ４０は、車両用状態量推定装置１のカルマンフィルタ処理部３２が出力した状態量（η、η’、θ、γ）に基づき、アクチュエータ４１を制御する。アクチュエータ４１は、例えば、コラムハウジング部に取り付けられた直流モータであり、車両の操舵に必要なトルクを出力して運転者のステアリング操作をアシストする。アクチュエータ４１が出力するトルクは、ウォームギヤ及びホイールギヤによって偏向されると共に減速されてラックバーに伝達され、キングピンを支点として回転するナックルアームを回動させ、最終的に車輪の向きを変える。

操舵制御用ＥＣＵ４０は、例えば次式（１４）によって目標操舵角δを算出し、目標操舵角δが実現されるようにアクチュエータ４１の出力を決定する。式中、Ｋη、Ｋη’、Ｋθ及びＫγは定数であり、θ_TARGETは目標ヨー角であり、γ_TARGETは目標ヨーレートであり、δｆはフィードフォワード操舵角である。このフィードフォワード操舵角δｆは、車両モデル、道路曲率、バンク角、車速などに基づいて決定される。

以上説明した本実施の車両用状態量推定装置１によれば、観測量を複数セット出力するセンサを具備し、同一のセンサにより出力された同一種類の複数の観測量を含めた複数の観測量について信頼度を算出すると共に、同一種類の複数の観測量のそれぞれをカルマンフィルタに入力して状態量を推定するため、精度良く車両の状態量を推定することができる。

ここで、同一のセンサは一種類の観測量について一つの観測量しか出力しないものとすると、信頼度の算出過程で誤った信頼度が算出された場合、誤差の大きい観測量の演算ウエイトが大きくなってしまう場合がある。従って、同一のセンサが同一種類の複数の観測量を出力する方が、仮に誤った信頼度が算出された場合であっても真の値を得るまでの期間を短くすることができ、ロバスト安定性を高めることができる。

また、本実施の車両用状態量推定装置１を利用した車両用操舵制御装置１００によれば、精度良く操舵制御を行うことができる。

以上、本発明を実施するための最良の形態について実施例を用いて説明したが、本発明はこうした実施例に何等限定されるものではなく、本発明の要旨を逸脱しない範囲内において種々の変形及び置換を加えることができる。

例えば、観測量を出力するセンサとして、カメラセンサとヨーレートセンサを例示したが、車両用状態量推定装置１が出力する状態量（実施例では横偏差等、走行車線と車両の位置関係に関するものである）に関連する観測量を出力するものであれば、如何なるセンサを採用してもよい。例えば。磁気マーカから信号を読み取るセンサ、高精度に車両の位置を測定可能なＧＰＳ受信機等が採用されてもよい。

１ 車両用状態量推定装置
１０ 前方カメラセンサ
１１ 後方カメラセンサ
１２ 右側方カメラセンサ
１３ 左側方カメラセンサ
１４ ヨーレートセンサ
１０Ａａ〜１３Ａ 単体信頼度算出部
２０ 操舵角センサ
２１ ナビゲーション装置
３０ 推定処理用ＥＣＵ
３１ 総合信頼度算出部
３２ カルマンフィルタ処理部
４０ 操舵制御用ＥＣＵ
４１ アクチュエータ
１００ 車両用操舵制御装置

Claims (6)

1. 車両に搭載され、車両に関する観測を行い観測量を出力する複数の観測手段と、
   前記複数の観測手段により出力される観測量の信頼度を、観測量毎に算出する信頼度算出手段と、
   前記観測手段により出力された観測量、及び前記信頼度算出手段により算出された観測量の信頼度を推定モデルに入力して車両の状態量を推定する推定手段と、
   を備える車両状態量推定装置であって、
   前記複数の観測手段には、同一種類の観測量を複数出力可能な観測手段が含まれ、
   前記信頼度算出手段は、前記同一の観測手段により出力された同一種類の複数の観測量を含めた複数の観測量について信頼度を算出し、
   前記推定手段は、前記同一の観測手段により出力された同一種類の複数の観測量のそれぞれを前記推定モデルに入力して車両の状態量を推定することを特徴とする、
   車両状態量推定装置。
2. 請求項１に記載の車両状態量推定装置であって、
   前記推定モデルは、カルマンフィルタであり、
   該カルマンフィルタにおける観測方程式の各行列は、前記同一の観測手段により出力された複数の観測量を含めた複数の観測量分の行数を有し、前記同一の観測手段により出力された複数の観測量に対応する行は同一の成分を有することを特徴とする、
   車両状態量推定装置。
3. 請求項２に記載の車両状態量推定装置であって、
   前記観測方程式には、前記複数の観測値の観測誤差の標準偏差の項が追加され、
   該標準偏差は、前記信頼度算出手段により算出された信頼度が低くなる程、大きく設定されることを特徴とする、
   車両状態量推定装置。
4. 請求項１ないし３のいずれか１項に記載の車両状態量推定装置であって、
   前記複数の観測手段は、車両の周囲の道路を撮像して道路区画線を認識し、走行車線と車両の位置関係を前記観測量として出力する手段を含み、
   前記推定手段は、走行車線と車両の位置関係を前記車両の状態量として推定する手段である、
   車両状態量推定装置。
5. 請求項４項に記載の車両状態量推定装置であって、
   前記複数の観測手段が出力する観測量は、横偏差、道路曲率、及びヨー角のいずれかを含み、
   前記推定手段が出力する車両の状態量は、車両重心の横偏差、横方向速度、ヨー角、及びヨーレートのいずれかを含む、
   車両状態量推定装置。
6. 請求項１ないし５のいずれか１項に記載の車両状態量推定装置と、
   該車両状態量推定装置によって推定された車両の状態量に基づいて操舵制御を行う制御装置と、
   を備える車両用操舵制御装置。

Images