JP5029442B2 - 車両姿勢角推定装置及びプログラム - Google Patents

Description

本発明は、車両姿勢角推定装置及びプログラムにかかり、特に、ピッチ角速度の推定値を利用してオブザーバによって車両物理量としての鉛直軸に対する車両の姿勢角であるロール角及びピッチ角を推定する車両姿勢角推定装置及びプログラムに関する。

剛体運動の加速度及び角速度とオイラー角で記述される車両の姿勢角との関係は、非特許文献１等で知られており、この関係に基づいて積分演算を行うことによって、車両の姿勢角を導出することができる。しかしながら、単純な積分演算ではセンサドリフト誤差が蓄積されて正確な姿勢角を導出することはできない。このような積分演算での誤差の蓄積を回避するためには、他のセンサ信号の出力と比較し、略一致するように出力を補正するオブザーバを構成する必要がある。

また、車体の鉛直軸に対するピッチ角を推定する従来技術として、車体の前後加速度と車輪速とから演算される前後加速度の差がピッチ角に依存することを利用して、ピッチ角を推定する技術が知られている（特許文献１）。
航空機力学入門（p12、加藤、大屋、柄沢：東京大学出版会、1982） 特開２００１−８２９５６号公報

しかしながら、上記特許文献１の技術では、旋回時には車体スリップ角が増大することから前後加速度の誤差が大きくなり、ピッチ角推定精度が劣化する、という問題がある。

本発明は、上記問題を解決すべく成されたもので、走行中の車両の上下方向の速度は略０であり、上下方向の速度の変化も小さい、という車両運動固有の性質に着目し、車体の上下方向運動の釣り合いからピッチ角速度を推定し、このピッチ角速度の推定値を利用して車体の鉛直軸に対する姿勢角であるロール角及びピッチ角を推定することにより、上下加速度が増加するバンク走行時においても精度良くピッチ角を推定することができる車両姿勢角推定装置及びプログラムを提供することを目的とする。

上記目的を達成するために第１の発明の車両姿勢角推定装置は、各輪の車輪速度に基づいて車両前後方向の車体速度である前後車体速度を推定する前後速度推定手段と、車両横方向の車体速度である横車体速度を推定する横速度推定手段と、ピッチ角速度を推定するピッチ角速度推定手段と、車両運動の前後加速度、横加速度、上下加速度、ヨー角速度、及びロール角速度の各検出値に応じたセンサ信号、前記前後車体速度の推定値、並びに前記ピッチ角速度の推定値に基づいて、車体の鉛直軸に対する姿勢角であるロール角及びピッチ角を推定する姿勢角推定手段と、を含む車両姿勢角推定装置であって、前記ピッチ角速度推定手段が、前記前後車体速度の推定値、前記横車体速度の推定値、前記上下加速度の検出値、前記ロール角速度の検出値、前記姿勢角推定手段で推定された前記ロール角の前回推定値、及び前記姿勢角推定手段で推定された前記ピッチ角の前回推定値に基づいて、前記ピッチ角速度を推定するようにしたものである。

また、第２の発明のプログラムは、コンピュータを、各輪の車輪速度に基づいて車両前後方向の車体速度である前後車体速度を推定する前後速度推定手段と、車両横方向の車体速度である横車体速度を推定する横速度推定手段と、ピッチ角速度を推定するピッチ角速度推定手段と、車両運動の前後加速度、横加速度、上下加速度、ヨー角速度、及びロール角速度の各検出値に応じたセンサ信号、前記前後車体速度の推定値、並びに前記ピッチ角速度の推定値に基づいて、車体の鉛直軸に対する姿勢角であるロール角及びピッチ角を推定する姿勢角推定手段と、して機能させるためのプログラムであって、前記ピッチ角速度推定手段が、前記前後車体速度の推定値、前記横車体速度の推定値、前記上下加速度の検出値、前記ロール角速度の検出値、前記姿勢角推定手段で推定された前記ロール角の前回推定値、及び前記姿勢角推定手段で推定された前記ピッチ角の前回推定値に基づいて、前記ピッチ角速度を推定するようにしたものである。

第１の発明の車両姿勢角推定装置には、車両運動の前後加速度、横加速度、上下加速度、ヨー角速度、及びロール角速度の各々を検出して検出値に応じたセンサ信号を出力するセンサを設けることができる。

第１の発明及び第２の発明では、車両運動の前後加速度、横加速度、上下加速度、ヨー角速度、及びロール角速度の各検出値に応じたセンサ信号、前後車体速度の推定値、並びにピッチ角速度の推定値に基づいて、車体の鉛直軸に対する姿勢角であるロール角及びピッチ角を推定する。

ピッチ角速度は、前後車体速度の推定値、横車体速度の推定値、上下加速度の検出値、ロール角速度の検出値、ロール角の前回推定値、及びピッチ角の前回推定値に基づいて、推定される。

第１の発明及び第２の発明では、車体の上下方向運動の釣り合いからピッチ角速度を推定し、ピッチ角速度の推定値を利用して車体の鉛直軸に対する姿勢角であるロール角及びピッチ角を推定しているので、上下加速度が増加するバンク走行時においても精度良くロール角及びピッチ角を推定することができる。

第１の発明及び第２の発明では、前記姿勢角推定手段によって推定された姿勢角の推定値、前記センサによって検出された車両運動の前後加速度、横加速度、上下加速度、ヨー角速度、及びロール角速度、ピッチ角速度の推定値、前後車体速度の推定値、及び操舵角に基づいて、車両物理量としての新たな前後車体速度及び横車体速度を推定する車体速度推定手段を更に設けることができる。

これにより、新たな前後車体速度及び横車体速度が高精度に推定され、スピン抑制やタイヤスリップ抑制に必要な車体スリップ角や各輪のスリップ速度推定を高精度に行うことが可能となる。

第３の発明の車両姿勢角推定装置は、各輪の車輪速度に基づいて車両前後方向の車体速度である前後車体速度を推定する前後速度推定手段と、車両横方向の車体速度である横車体速度を推定する横速度推定手段と、ピッチ角速度を推定するピッチ角速度推定手段と、車両運動の前後加速度、横加速度、上下加速度、ヨー角速度、及びロール角速度の各検出値に応じたセンサ信号、前記前後車体速度の推定値、前記横速度推定手段で推定された前記横車体速度の微分値の推定値又は前記横車体速度の推定値の微分値、並びに前記ピッチ角速度の推定値に基づいて、車体の鉛直軸に対する姿勢角であるロール角及びピッチ角を推定する姿勢角推定手段と、を含む車両姿勢角推定装置であって、前記ピッチ角速度推定手段が、前記前後車体速度の推定値、前記横車体速度の推定値、前記上下加速度の検出値、前記ロール角速度の検出値、前記姿勢角推定手段で推定された前記ロール角の前回推定値、及び前記姿勢角推定手段で推定された前記ピッチ角の前回推定値に基づいて、前記ピッチ角速度を推定するようにしたものである。

また、第４の発明のプログラムは、コンピュータを、各輪の車輪速度に基づいて車両前後方向の車体速度である前後車体速度を推定する前後速度推定手段と、車両横方向の車体速度である横車体速度を推定する横速度推定手段と、ピッチ角速度を推定するピッチ角速度推定手段と、車両運動の前後加速度、横加速度、上下加速度、ヨー角速度、及びロール角速度の各検出値に応じたセンサ信号、前記前後車体速度の推定値、前記横速度推定手段で推定された前記横車体速度の微分値の推定値又は前記横車体速度の推定値の微分値、並びに前記ピッチ角速度の推定値に基づいて、車体の鉛直軸に対する姿勢角であるロール角及びピッチ角を推定する姿勢角推定手段と、して機能させるためのプログラムであって、前記ピッチ角速度推定手段が、前記前後車体速度の推定値、前記横車体速度の推定値、前記上下加速度の検出値、前記ロール角速度の検出値、前記姿勢角推定手段で推定された前記ロール角の前回推定値、及び前記姿勢角推定手段で推定された前記ピッチ角の前回推定値に基づいて、前記ピッチ角速度を推定するようにしたものである。

第３の発明の車両姿勢角推定装置には、車両運動の前後加速度、横加速度、上下加速度、ヨー角速度、及びロール角速度の各々を検出して検出値に応じたセンサ信号を出力するセンサを設けることができる。

第３の発明及び第４の発明では、車両運動の前後加速度、横加速度、上下加速度、ヨー角速度、及びロール角速度の各検出値に応じたセンサ信号、前後車体速度の推定値、横速度推定手段で推定された横車体速度の微分値の推定値又は横車体速度の推定値の微分値、並びにピッチ角速度の推定値に基づいて、車体の鉛直軸に対する姿勢角であるロール角及びピッチ角を推定する。

ピッチ角速度は、前後車体速度の推定値、横車体速度の推定値、上下加速度の検出値、ロール角速度の検出値、ロール角の前回推定値、及びピッチ角の前回推定値に基づいて、推定される。

第３の発明及び第４の発明では、車体の上下方向運動の釣り合いからピッチ角速度を推定し、ピッチ角速度の推定値を利用して車体の鉛直軸に対する姿勢角であるロール角及びピッチ角を推定しているので、上下加速度が増加するバンク走行時においても精度良くロール角及びピッチ角を推定することができる。また、横車体速度の微分値の推定値又は横車体速度の推定値の微分値を利用してロール角及びピッチ角を推定しているので、車体の横方向運動の釣り合いを利用して、精度よくロール角及びピッチ角を推定することができる。

以上説明したように本発明によれば、車体の上下方向運動の釣り合いからピッチ角速度を推定し、このピッチ角速度の推定値を利用して車体の鉛直軸に対する姿勢角であるロール角及びピッチ角を推定しているので、上下加速度が増加するバンク走行時においても精度良くピッチ角を推定することができる、という効果が得られる。

以下、図面を参照して本発明の実施の形態を詳細に説明する。本実施の形態は、鉛直軸に対する車両の姿勢角であるピッチ角及びロール角を推定する姿勢角推定装置に本発明を適用したものである。

図１に示すように、本実施の形態の姿勢角推定装置には、車両横方向の車体速度である横車体速度Ｖを推定する横速度推定手段１０、及び各輪の車輪速度に基づいて車両前後方向の車体速度である前後車体速度Ｕを推定する前後速度推定手段１２が設けられている。

横車体速度Ｖは、カルマンフィルタ等を用いて推定したり、横加速度センサの検出値から推定したりすることができる。

各輪の車輪速度は、各輪に対応して設けられた車輪速センサにより検出することができ、前後車体速度Ｕは各輪の車輪速度、または各輪の車輪速度及び車輪速度の微分値から推定することができる。例えば、制動時には、４輪の車輪速度の最大値を前後車体速度Ｕとして出力したり、駆動時には従動輪の車輪速度の平均値を前後車体速度Ｕとして出力することができる。

また、本実施の形態の姿勢角推定装置には、車両運動のｘｙｚ３軸の加速度である上下加速度Ｇｚ、横加速度Ｇｙ、前後加速度Ｇｘの各々を検出する上下加速度センサ１４、横加速度センサ１６、及び前後加速度センサ１８が設けられている。図２に示すように、ｘ軸は車両前後方向、ｙ軸は車両幅方向（横方向）、ｚ軸は車両上下方向に対応している。

また、本実施の形態の姿勢角推定装置には、ロール角速度Ｐを検出するロール角速度センサ２０、及びヨー角速度Ｒを検出するヨー角速度センサ２２が設けられている。

横速度推定手段１０、前後速度推定手段１２、上下加速度センサ１４、及びロール角速度センサ２０は、ピッチ角速度Ｑを推定するピッチ角速度推定手段２４に接続されている。

ピッチ角速度推定手段２４は、車体の鉛直軸に対する姿勢角であるロール角φ及びピッチ角θを推定する姿勢角推定手段２６に接続されている。姿勢角推定手段２６には、更に、前後速度推定手段１２、上下加速度センサ１４、横加速度センサ１６、前後加速度センサ１８、ロール角速度センサ２０、及びヨー角速度センサ２２が接続されている。

姿勢角推定手段２６は、車両運動の前後加速度Ｇｘ、横加速度Ｇｙ、上下加速度Ｇｚ、ヨー角速度Ｒ、及びロール角速度Ｐの検出値に応じたセンサ信号、前後車体速度Ｕの推定値Ｖｓｏ、及びピッチ角速度Ｑの推定値に基づいて、車体の鉛直軸に対する姿勢角であるロール角φ及びピッチ角θを推定し、ロール角φ及びピッチ角θの推定値を前回値としてピッチ角速度推定手段２４に入力するように、ピッチ角速度推定手段２４に接続されている。

横速度推定手段１０、前後速度推定手段１２、ピッチ角速度推定手段２４、及び姿勢角推定手段２６は、各手段の機能を実現する１つまたは複数のコンピュータ、または１つまたは複数の電子回路で構成することができる。

次に、ピッチ角速度の推定について説明する。剛体に固定された３軸加速度及び３軸角速度を検出する３軸センサから出力されるセンサ信号と運動状態量との関係を表す剛体の運動方程式は、以下のように記述することができる。

ただし、 Ｇｘ：前後加速度、Ｇｙ：横加速度、Ｇｚ：上下加速度、Ｐ：ロール角速度、Ｑ：ピッチ角速度、Ｒ：ヨー角速度、Ｕ：前後速度、Ｖ：横速度、Ｗ：上下速度、φ：ロール角、θ：ピッチ角、g：重力加速度である。

本実施の形態において車両を剛体とした場合、前後加速度Ｇｘ、横加速度Ｇｙ、及び上下加速度Ｇｚの３軸速度、並びにロール角速度Ｐ、及びヨー角速度Ｒの２軸角速度の各々は、前後加速度センサ１８、横加速度センサ１６、上下加速度センサ１４、ロール角速度センサ２０、及びヨー角速度センサ２２によって検出され、横速度Ｖは、上記で説明したように横速度推定手段１０で推定することができ、前後速度Ｕは各輪の車輪速に基づいて上記で説明したように前後速度推定手段１２で推定することができる。

なお、本実施の形態では、図２に示すように車体の上向きをｚ軸の正方向とする右手系で座標を記述すると共に角度はオイラー角で表現している。

バンク走行中は、遠心力によって上下加速度Ｇｚが増加する。上下加速度Ｇｚの増加は、上記（３）式において左辺の−ＱＵが大きくなることと対応しており、この性質を利用し、（３）式において上下速度Ｗの変化を無視（Ｗ＝０とする）すると共に、ロール角及びピッチ角の各々の前回値、及び横速度の推定値、前後速度Ｕとして前後車体速度の推定値Ｖｓｏを用いることによりことにより、ピッチ角速度の推定値を以下の式から求めることができる。ピッチ角速度推定手段２４は、下記（６）式の演算を実行することによりピッチ角速度の推定値を求める。

図３は、バンク走行時の３軸加速度Ｇｘ，Ｇｙ，Ｇｚの変化を示しており、上下加速度は２０〜４０ｓ付近でバンク走行時の遠心力の影響を受けて、重力が速度９．８ｍ／ｓ^２以上の値を示している。図４は、このときの３軸角速度Ｐ，Ｑ，Ｒと上記（６）式を用いて推定したピッチ角速度Ｑの推定値との関係を示している。図４からピッチ角速度の推定値は、真値とよく一致しており、このことからバンク走行時のピッチ角速度が正確に推定できていることが理解できる。

ピッチ角速度が上記のように推定されると、ピッチ角もロール角と同時にオブザーバによって推定することができる。オブザーバを用いた場合の自動車固有の運動の拘束条件について説明する。上記の運動方程式を利用して図５に示すオブザーバ３０を構成する場合、積分演算によって推定される速度及び角度の状態量が発散しないように、測定できる物理量のフィードバックが必要となる。本実施の形態では、自動車運動固有の特徴を以下のようにフィードバックする物理量に利用する。なお、以下の式では、ロール角及びピッチ角として各々の前回値、横速度として横車体速度の推定値、前後速度Ｕとして前後車体速度の推定値Ｖｓｏを各々用いている。

前後車体速度の推定値Ｖｓｏを微分して上記(１)式に代入し、自動車運動固有の特徴として、車体の前後方向に関しては「車輪のスリップは長時間増加し続けない」という性質を利用すると、（１）式から上下速度の変化を無視したフィードバックに利用する以下の（７）式に示す条件が得られる。

（７）式は、車輪速度から推定される加速度（前後車体速度の推定値Ｖｓｏの微分値）と前後加速度Ｇｘとの差と、ピッチ角θとの関係を記述している。

また、車体の横方向に関しては、「スリップ角は長時間増加し続けない」という性質からロール角速度及び横加速度が無視できることから、フィードバックに利用するための(２)式から得られる以下の（８）式に示す条件が得られる。

さらに、一般的な道路勾配を考慮すると、「鉛直方向の加速度は、重力加速度に略一致している」とみなすことができる。この条件は前後加速度Ｇｘ、横加速度Ｇｙ、上下加速度Ｇｚ、ピッチ角θ、及びロール角φを用いて次式の代数方程式によって記述することができる。

上記（７）〜（９）式の関係は、いずれもある程度長い時間を考慮したときに満足する条件であることから、オブザーバ測定量のフィードバックには、以下で説明するように上記（７）〜（９）式の両辺をローパスフィルタ処理した値を利用する。

次に、図５に示した、基本的な非線形オブザーバの構成について説明する。ここでは、センサから出力されるセンサ信号、及びピッチ角速度の推定値を含む値ｕを下記（１０）式のように表現している。

また、対象となる車両運動を下記（１１）式のように表現し、オブザーバを構成するために測定できる物理量を（１２）式のように表現すると、非線形オブザーバは、以下の（１３）式及び（１４）式の非線形運動方程式によって記述することができる。

上記(７)〜(９)式は、何れもある程度長い時間を考慮したときに満足する条件であることから、オブザーバの測定量のフィードバックには、下記（１５）〜（１７）式に示す(７)〜(９)式の両辺をローパスフィルタでローパスフィルタ処理した値を利用する。

ただし、τ_x、 τ_y、 τ_gは各々（７）〜（９）式で考慮するローパスフィルタの数秒から数十秒以上の時定数を表している。

次に、上記の基本的な非線形オブザーバを用いてロール角φ及びピッチ角θを推定する本実施の形態のロール角φ及びピッチ角θのオブザーバについて説明する。上記(４)、(５)式の角度に関する状態方程式には、速度の状態量が含まれていないことから、角度としてロール角φ及びピッチ角θを速度の推定と分離して推定することができる。

このため、まず、上記のローパスフィルタによって生じる状態量を含め、ロール角及びピッチ角を推定するためのオブザーバを構成する。本実施の形態では、オブザーバの状態量を下記（１８）式で表し、

フィードバックに用いるオブザーバ出力を下記（１９）式で表し、

ただし、

である。

なお、（２１）式右辺第２項の分子は、前後車体速度の推定値Ｖ_ｓ０の微分値から、３軸センサで検出されたヨー角速度値Ｒと横車体速度の推定値との積と、３軸センサで検出された前後加速度値Ｇｘとの和を減算した前後加速度状態量の偏差であり、（２２）式右辺第２項の分子は、３軸センサで検出されたヨー角速度値Ｒと前後車体速度の推定値Ｖ_ｓ０との積から３軸センサで検出された横加速度値Ｇｙを減算した横加速度状態量の偏差である。

オブザーバゲインの一例として、本実施の形態では、オブザーバの安定性を確保するため、線形化を行ったときの対角成分が負の係数を持つように、下記（２３）式のように表す。

ただし、Ｋ_φｙ、 Ｋ_φg、 Ｋ_θｘ、 Ｋ_θg、 Ｋ_x、 Ｋ_y、 Ｋ_gは適切な正の定数である。したがって、ロール角及びピッチ角を推定する本実施の形態の非線形オブザーバは、以下の運動方程式で記述される。

ただし、

である。

上記のオブザーバを用いることにより車両鉛直方向に対する姿勢角であるロール角φ及びピッチ角θを推定することができる。

図６は、ロール角φ及びピッチ角θの推定結果を示したものである。

図６では、本実施の形態の推定値、（２４）式のピッチ角速度に計測値を代入した推定値、下記で説明する２軸角速度及び３軸加速度を利用した推定方法によって推定した推定値を比較例として示している。ピッチ角速度の計測値を使用して推定された姿勢角が最も真値に近いと仮定すると、本実施の形態では比較例と比較して推定精度が向上していることが理解できる。

次に、比較例としての２軸角速度及び３軸加速度を利用した推定方法について説明する。この推定方法では、「前後運動の時間微分と前後加速度の差は、ピッチ角による重力成分に等しい」ということを示している、すなわち前後方向の加速度の釣り合いを記述する下記（２６）式と、「鉛直方向の加速度は、重力加速である」ということを示す以下の(２７)式で示す関係を利用している。

ノイズ等の影響を考慮すると、（２６）式及び（２７）式を同時に満足するピッチ角θが存在することを保証することができない。このため、（２６）式及び（２７）式の誤差を最小にするピッチ角θを求める。この誤差を最小にするためには、前ステップで求めたθの回りで線形化し、擬似逆行列を用いて演算すればよい。

まず、（２６）式及び（２７）式を線形化して行列で表わすと、以下の式が得られる。

ただし、θ_０は、θの前回値であり、（２６）式のＱＷは、Ｗが微小であることから無視するものとする。上記（２８）式を次の（２９）式のように表現すると、θは以下の（３０）式で求めることができる。

なお、ここでは、演算値の安定性を確保するために１次のローパスフィルタを通した以下の演算を行っている。

ただし、Ｔはサンプル時間、τは例えば、０．１ｓの時定数である。

次に、本発明の第２の実施の形態について説明する。なお、第１の実施の形態と同様の構成となる部分については、同一符号を付して説明を省略する。

第２の実施の形態では、スロースピン走行等を行う場合、ある程度長時間の挙動での横方向加速度の釣り合いにおいてスリップ角の変動を無視できないという点と、車体速度オブザーバによってある程度精度の良い車体横速度の微分値の推定値が得られるという点とに着目して、車体の横方向運動の釣り合いを利用した、ロール角及びピッチ角を推定するオブザーバによって姿勢角を推定する。

図７に示すように、本実施の形態の姿勢角推定装置には、上下加速度センサ１４、横加速度センサ１６、前後加速度センサ１８、ロール角速度センサ２０、ヨー角速度センサ２２、及び前輪実操舵角δ_ｆを検出する操舵角センサ２２９が設けられている。

上下加速度センサ１４、及びロール角速度センサ２０は、ピッチ角速度Ｑを推定するピッチ角速度推定手段２２４に接続されている。

ピッチ角速度推定手段２２４は、車体の鉛直軸に対する姿勢角であるロール角φ及びピッチ角θを推定する姿勢角推定手段２２６、及び車両前後方向の車体速度である前後車体速度Ｕと車両横方向の車体速度である横車体速度Ｖとを推定する車体速度推定手段２２８に接続されている。姿勢角推定手段２２６には、更に、上下加速度センサ１４、横加速度センサ１６、前後加速度センサ１８、ロール角速度センサ２０、ヨー角速度センサ２２、及び車体速度推定手段２２８が接続されている。

姿勢角推定手段２２６は、車両運動の前後加速度Ｇｘ、横加速度Ｇｙ、上下加速度Ｇｚ、ヨー角速度Ｒ、及びロール角速度Ｐの検出値に応じたセンサ信号、車体速度推定手段２２８によって推定された前後車体速度Ｕの推定値Ｖｓｏ及び横車体速度Ｖの微分値の推定値の前回値、並びにピッチ角速度推定手段２２４によって推定されたピッチ角速度Ｑの推定値に基づいて、車体の鉛直軸に対する姿勢角であるロール角φ及びピッチ角θを推定し、ロール角φ及びピッチ角θの推定値を前回値としてピッチ角速度推定手段２２４に入力するように、ピッチ角速度推定手段２２４に接続されている。

車体速度推定手段２２８には、上下加速度センサ１４、横加速度センサ１６、前後加速度センサ１８、ロール角速度センサ２０、ヨー角速度センサ２２、操舵角センサ２２９、ピッチ角速度推定手段２２４、及び姿勢角推定手段２２６が接続されている。

なお、車体速度推定手段２２８は、本発明の前後速度推定手段及び横速度推定手段に対応している。

ピッチ角速度推定手段２２４は、姿勢角推定手段２２６によって推定されたロール角及びピッチ角として各々の前回値、車体速度推定手段２２８によって推定された横車体速度の推定値及び前後車体速度の推定値Ｖｓｏの各々の前回値、上下加速度センサ１４からの検出値、並びにロール角速度センサ２０からの検出値の各々を用いて、第１の実施の形態と同様に、ピッチ角速度を推定する。

ピッチ角速度推定手段２２４、姿勢角推定手段２２６、及び車体速度推定手段２２８は、各手段の機能を実現する１つまたは複数のコンピュータ、または１つまたは複数の電子回路で構成することができる。

次に、前後車体速度及び横車体速度の推定について説明する。本実施の形態では、特に時変パラメータとしてのヨー角速度が小さいときの精度向上を目的として、「車両運動は長時間タイヤ非線形領域に留まらない」という自動車固有の特徴をオブザーバのフィードバック出力に利用するようにしたものである。

ここでは「車両運動は長時間タイヤ非線形領域に留まらない」という自動車固有の特徴をオブザーバの拘束条件に加えるため、「横方向のタイヤ発生力計測値＝モデル値」という以下の（３２）式で示される式の両辺をローパスフィルタ処理した関係をシステムに加えている。

ただし、ｍ：車両質量、ｃ_ｆ、ｃ_ｒ：前後輪コーナリングパワー、ｌ_ｆ、ｌ_ｒ：前後軸と重心との間の距離、δ_ｆ：前輪実操舵角である。

このとき、右辺をローパスフィルタ処理した状態量、すなわち横加速度の検出値をローパスフィルタ処理して得られた値が新たにオブザーバの状態量に加わる。ここでは、この状態量を横加速度フィルタ推定値ｇ_ｙｆのチルダとおく。このとき、オブザーバは、以下のように記述することができる。

ただし、ｇ_ｙｆは、横加速度Ｇ_ｙを以下のフィルタ処理した後の値である。

ただし、τ_ｙは、上記（３２）で考慮するローパスフィルタの数秒から数十秒以上の時定数を表わしている。

また、オブザーバゲインは、以下の（３７）式で表わされる。

ただし、Ｋ_ｖ、Ｋ_ｇは定数である。

従って、上記（３３）式の運動方程式に従って前後車体速度及び横車体速度を推定するオブザーバを用いた第２の実施の形態に係る車体速度推定手段２２８は、図８に示すように構成することができる。

車体速度推定手段２２８には、上記（３３）式の運動方程式に従って横車体速度の推定値及び前後車体速度の推定値を推定して出力する車体速度オブザーバ２３８、及び各輪の車輪速度に基づいて車両前後方向の車体速度である前後車体速度Ｕを演算する前後速度演算手段２３２を備えている。車体速度オブザーバ２３８は、姿勢角推定手段２２６、横加速度センサ１６、前後加速度センサ１８、ヨー角速度センサ２２、及び操舵角センサ２２９に接続されている。

ヨー角速度センサ２２は、車体速度オブザーバ２３８に接続されると共に、ヨー角速度の絶対値を演算する絶対値演算手段２４０に接続されている。前後速度演算手段２３２は、前後車体速度の演算値から前後車体速度の推定値を減算した偏差を演算する減算器２４４に接続されている。減算器２４４は、絶対値演算手段２４０で演算されたヨー角速度の絶対値と減算器２４４で演算された偏差との積を演算する乗算器２４２に接続されている。乗算器２４２は、ヨー角速度の絶対値と減算器２４４で演算された偏差との積をフィードバック量としてフィードバックするように車体速度オブザーバ２３８に接続されている。

上記（３２）式に従って横加速度の検出値をローパスフィルタ処理するために、前後加速度センサ１８及び横加速度センサ１６を用い、横加速度センサ１６には横加速度Ｇ_ｙの検出値をローパスフィルタ処理するためにローパスフィルタ２４６が接続されている。

ローパスフィルタ２４６は、横加速度Ｇ_ｙの検出値をローパスフィルタ処理したローパスフィルタ処理値からローパスフィルタ処理値の推定値（横加速度をローパスフィルタ処理した値の推定値）を減算した偏差を演算する減算器２４８に接続されている。減算器２４８は、ローパスフィルタ処理値からローパスフィルタ処理値の推定値を減算した偏差をフィードバック量として入力するように車体速度オブザーバ２３８に接続されている。

この車体速度オブザーバ２３８は、上記（３３）式に従って、横車体速度の推定値、前後車体速度の推定値、及びローパスフィルタ処理値の推定値を演算する。また、車体速度オブザーバ２３８は、上記（３３）式に従って、横車体速度の微分値の推定値を演算する。

次に、第２の実施の形態におけるロール角及びピッチ角の推定について説明する。上記の第１の実施の形態では、姿勢角推定オブザーバのフィードバック条件について車体の横方向に関して、「スリップ角は長時間増加し続けない」という性質から（８）式をフィードバックに利用している。

本実施の形態では、第１の実施の形態で無視していた横車体速度の微分値を、車体速度推定手段２２８から求めて、（８）式の代わりに、以下の（３８）式で表される条件をフィードバックに利用する。

ここで、Ｖのチルダのドットは、車体速度推定手段２２８で推定された横車体速度の微分値の前回値である。

また、ロール角、ピッチ角を推定する非線形オブザーバは、以下の（３９）式で表される運動方程式で記述される。

上記のオブザーバを用いることにより車両鉛直方向に対する姿勢角であるロール角φ及びピッチ角θを推定することができる。

図９（Ａ）、（Ｂ）は、ロール角φ及びピッチ角θの推定結果を示したものである。図９（Ａ）、（Ｂ）では、本実施の形態の推定値を、レーザ変位計で計測した４輪の高さから求めた姿勢角を比較例として示している。レーザ変位計の計測値から求める姿勢角が最も真値に近いと仮定すると、本実施の形態では、ロール角及びピッチ角を精度よく推定していることが理解できる。

上記の第２の実施の形態において、コンピュータを、ピッチ角速度推定手段２２４、姿勢角推定手段２２６、及び車体速度推定手段２２８の各手段として機能させ、更にロール角及びピッチ角を演算するプログラムによる情報処理は、図１０のフローチャートに示す手順で実現することができる。コンピュータは、相互にバスによって接続されたＣＰＵ，ＲＯＭ，及びＲＡＭ，並びに必要に応じて接続されたＨＤＤで構成され、これらのプログラムは、コンピュータのＣＰＵに接続されているＲＯＭまたはＨＤＤ等の記録媒体に記録される。

この手順を説明すると、ステップ２５０において、最初の演算か否かを判断し、最初の演算の場合には、ロール角、ピッチ角、横車体速度、横車体速度の微分値、及び前後車体速度の各々の前回値が存在しないので、ステップ２５２でロール角及びピッチ角の前回値として予め定められた初期値を設定し、ステップ２５４で、例えば、カルマンフィルタや、横加速度センサ１６の検出値を用いて、横車体速度及び横車体速度の微分値を推定し、横車体速度及び横車体速度の微分値の前回値として設定する。また、ステップ２５６で、各輪の車輪速度に基づいて前後車体速度を推定し、前後車体速度の前回値として設定する。

一方、２回目以降の演算の場合には、後述するステップ２６４、２６６で、ロール角、ピッチ角、前後車体速度、前後車体速度の微分値、及び横車体速度の各々の推定値が演算されるので、ステップ２５８においてロール角、ピッチ角、前後車体速度、前後車体速度の微分値、及び横車体速度の各々の値として前回演算された前回値を設定する。

そして、ステップ２６０で、上記第１の実施の形態で説明したように上記(６)式を用いてピッチ角速度Ｑの推定値を演算する。

ステップ２６２において、上記第１の実施の形態で説明したように、車両運動の前後加速度Ｇｘ、横加速度Ｇｙ、及びヨー角速度Ｒの検出値に応じたセンサ信号、前後車体速度Ｕの推定値Ｖｓｏに基づいて、上記（２１）、（２２）式で記述されたローパスフィルタ処理を行う。

次のステップ２６４では、ステップ２５４又はステップ２５８で設定された横車体速度の微分値の前回値、ステップ２６０で演算されたピッチ角速度Ｑの推定値、ステップ２６２で演算されたローパスフィルタ処理値を用い、上記（３９）式にしたがって車体の鉛直軸に対する姿勢角であるロール角φ及びピッチ角θを推定し、ロール角φ及びピッチ角θの推定値をコンピュータのメモリに記憶する。

ステップ２６６において、上記で説明したように推定された姿勢角の推定値、検出された３軸加速度及び２軸角速度、検出された操舵角、及び推定されたピッチ角速度に基づいて車両前後方向の新たな前後車体速度及び横車体速度を演算する。また、上記ステップ２６６において、横車体速度の微分値を演算する。

なお、上記の実施の形態では、上記（３３）式を用いた車体速度オブザーバによって、前後車体速度、横車体速度、及び横車体速度の微分値を推定した場合を例に説明したが、これに限定されるものではなく、後述する第３の実施の形態の車体速度オブザーバによって、前後車体速度、横車体速度、及び横車体速度の微分値を推定するようにしてもよい。また、各車輪速度から前後車体速度を推定し、カルマンフィルタや加速度センサの検出値を用いて横車体速度を推定すると共に、横車体速度の推定値を微分して、横車体速度の微分値を求めるようにしてもよい。

また、車体速度推定手段において、車体前後速度推定値及び車体横速度推定値が求まるので、車体スリップ角β及びスリップ速度ｓを演算するようにしてもよい。この場合には、後述する第３の実施の形態と同様に演算すればよい。

次に、本発明の第３の実施の形態について説明する。本実施の形態は、図１１に示すように、図１に示す姿勢角推定装置に、姿勢角推定手段２６によって推定されたピッチ角及びロール角の姿勢角、前後加速度センサ１８、横加速度センサ１６、及び上下加速度センサ１４の各々で検出された前後加速度Ｇｘ、横加速度Ｇｙ、及び上下加速度Ｇｚの３軸速度、ロール角速度センサ２０及びヨー角速度センサ２２の各々によって検出されたロール角速度Ｐ及びヨー角速度Ｒ、並びにピッチ角速度推定手段で推定されたピッチ角速度Ｑの推定値の３軸角速度、前後速度推定手段１２によって推定された車両の前後車体速度の推定値、及び操舵角を検出する操舵角センサ３４によって検出された操舵角である前輪実舵角に基づいて、新たな前後車体速度及び横車体速度を推定する車体速度推定手段３６を設け、車両物理量としての車体速度を推定する車体速度推定装置を構成したものである。

本実施の形態の車体速度を推定する車体速度オブザーバについて説明する。速度に関する拘束条件は、Ｕ＝Ｖｓｏの式及び(９)式で示されているが、この２つの条件のみでは、精度の良い推定は困難である。このため、車両運動の線形モデルを利用し、横加速度のモデル演算値が実測値に等しいという下記（４０）式で示される条件を拘束条件として利用する。

ただし、ｍ：車両質量、ｃ_ｆ、 ｃ_ｒ：前後輪コーナリングパワー、ｌ_ｆ、 ｌ_ｒ：前後軸と重心との間の距離、δ_ｆ：前輪実舵角である。なお、上記(４０)式は、タイヤ特性に余裕のある線形領域のみで成立するものであることから、下記（４１）式で表される出力の偏差の絶対値に応じてフィードバックゲインを小さくし、限界領域でモデルの影響を小さくしていくことが必要となる。

本実施の形態では、オブザーバの状態量を下記（４２）式で表し、

フィードバックに用いるオブザーバ出力を下記（４３）式で表し、

さらに、センサ信号等から演算される車両出力を下記（４４）式のように表し、更に適切なオブザーバゲインｋ（ｘのチルダ，ｕ）を設定することによって車体速度を推定するオブザーバを構成している。

ただし、

である。オブザーバゲインの一例として、ここでは、オブザーバの安定性を確保するため、線形化を行ったときの対角成分が負の係数を持つように、下記（４６）式で表す。

ただし、Ｋ_Ｕ、 Ｋ_Ｗ、 Ｋ_Ｕ０、 Ｋ_Ｗ０は適切な正の定数である。また、Ｋ_Ｖは図１２に示すｙ_ｖ−ｙ_ｖのチルダの非線形関数である。なお、Ｋ_Ｖとして、適切な正の定数を用いてもよい。

したがって、車体速度を推定する非線形オブザーバは、姿勢角推定手段によって推定された姿勢角であるピッチ角θ及びロール角φ、センサによって検出された車両運動の３軸加速度及び２軸角速度、ピッチ角速度推定手段で推定されたピッチ角速度の推定値、前後車体速度推定手段によって推定された車両の前後車体速度の推定値、及び操舵角を用いた次の（４７）式の運動方程式によって記述される。

ただし、出力の偏差及びオブザーバの状態量は、上記（４１）式及び（４２）式で示したように、下記のように表される。

この演算で出力されるＵのチルダが車両前後方向の新たな車体速度の推定値（車体前後速度推定値）、Ｖのチルダが車両横方向の車体速度の推定値（車体横速度推定値）である。

本実施の形態では、車体前後速度推定値及び車体横速度推定値が求まるので、車体速度推定手段３６において、車体スリップ角β及びスリップ速度ｓを以下のようにして演算することができる。

車体スリップ角βは、オブザーバで上記のように推定された状態量を用いて下記（４９）式として求めることができ、

また、各輪の前後方向のスリップ、すなわちスリップ速度sは、下記（５０）式のように演算することができる。

ただし、r_tは各輪のタイヤ半径、ωは各輪回転角速度である。

上記の第３の実施の形態において、コンピュータを、横速度推定手段１０、前後速度推定手段１２、ピッチ角速度推定手段２４、姿勢角推定手段２６、及び車体速度推定手段３６の各手段として機能させ、更にスリップ角及びスリップ速度を演算するプログラムによる情報処理は図１３のフローチャートに示す手順で実現することができる。なお、コンピュータを第１の実施の形態の各手段として機能させるプログラムも図１３のフローチャートに示す手順と同様の手順で実現することができる。コンピュータは、相互にバスによって接続されたＣＰＵ，ＲＯＭ，及びＲＡＭ，並びに必要に応じて接続されたＨＤＤで構成され、これらのプログラムは、コンピュータのＣＰＵに接続されているＲＯＭまたはＨＤＤ等の記録媒体に記録される。

この手順を説明すると、ステップ１００で上記で説明したように横車体速度を推定し、ステップ１０２において各輪の車輪速度に基づいて前後車体速度を推定する。

ステップ１０４において、最初の演算か否かを判断し、最初の演算の場合には、ロール角及びピッチ角の前回値が存在しないので、ステップ１０６でロール角及びピッチ角の前回値として予め定められた初期値を設定し、ステップ１１０で上記で説明したように上記(６)式を用いてピッチ角速度Ｑの推定値を演算する。

一方、２回目以降の演算の場合には、後述するステップ１１４でロール角及びピッチ角の推定値が演算されるので、ステップ１０８においてロール角及びピッチ角の値として前回演算された前回値を設定し、ステップ１１０で上記で説明したように上記(６)式を用いてピッチ角速度Ｑの推定値を演算する。

ステップ１１２において、上記で説明したように、車両運動の前後加速度Ｇｘ、横加速度Ｇｙ、及びヨー角速度Ｒの検出値に応じたセンサ信号、前後車体速度Ｕの推定値Ｖｓｏに基づいて、上記（２１）、（２２）式で記述されたローパスフィルタ処理を行う。

次のステップ１１４では、ステップ１１０で演算されたピッチ角速度Ｑの推定値、ステップ１１２で演算されたローパスフィルタ処理値を用い上記（２４）式にしたがって車体の鉛直軸に対する姿勢角であるロール角φ及びピッチ角θを推定し、ロール角φ及びピッチ角θの推定値をコンピュータのメモリに記憶する。

ステップ１１６において、上記で説明したように推定された姿勢角の推定値、検出された３軸加速度及び２軸角速度、検出された操舵角、及び推定されたピッチ角速度に基づいて車両前後方向の新たな車体速度及び横方向速度を演算し、ステップ１１８において演算された前後方向の新たな車体速度及び横方向速度を用いて上記で説明したようにスリップ角及びスリップ速度を演算する。

なお、本実施の形態では、車体スリップ角β及びスリップ速度ｓのいずれかを推定するようにしてもよい。

また、各推定値の前回値を用いて、ピッチ角速度やロール角及びピッチ角を推定する場合を例に説明したが、これに限定されるものではなく、各推定値の前々回値を用いて、ピッチ角速度やロール角及びピッチ角を推定するようにしてもよい。

本発明の第１の実施の形態を示すブロック図である。 本実施の形態の座標系を示す説明図である。 バンク走行時の３軸加速度の変化を示す線図である。 バンク走行時の３軸加速度とピッチ角速度の推定値との変化を示す線図である。 本発明の各実施の形態におけるオブザーバを示すブロック図である。 第１の実施の形態におけるロール角及びピッチ角の推定結果を示す線図である。 本発明の第２の実施の形態を示すブロック図である。 本発明の第２の実施の形態に係る車体速度推定手段を示すブロック図である。 第２の実施の形態におけるロール角及びピッチ角の推定結果を示す線図である。 本発明の第２の実施の形態の処理を示す流れ図である。 本発明の第３の実施の形態を示すブロック図である。 出力の偏差に対するオブザーバゲインを定める定数の１つを示すグラフである。 本発明の第３の実施の形態の処理を示す流れ図である。

符号の説明

１０ 横速度推定手段
１２ 前後速度推定手段
１４ 上下加速度センサ
１６ 横加速度センサ
１８ 前後加速度センサ
２０ ロール角速度センサ
２２ ヨー角速度センサ
２４、２２４ ピッチ角速度推定手段
２６、２２６ 姿勢角推定手段
２２４ ピッチ角速度推定手段
３６、２２８ 車体速度推定手段
２２９ 操舵角センサ
２３８ 車体速度オブザーバ

Claims (5)

1. 各輪の車輪速度に基づいて車両前後方向の車体速度である前後車体速度を推定する前後速度推定手段と、
   車両横方向の車体速度である横車体速度を推定する横速度推定手段と、
   ピッチ角速度を推定するピッチ角速度推定手段と、
   車両運動の前後加速度、横加速度、上下加速度、ヨー角速度、及びロール角速度の各検出値に応じたセンサ信号、前記前後車体速度の推定値、並びに前記ピッチ角速度の推定値に基づいて、車体の鉛直軸に対する姿勢角であるロール角及びピッチ角を推定する姿勢角推定手段と、
   を含む車両姿勢角推定装置であって、
   前記ピッチ角速度推定手段が、前記前後車体速度の推定値、前記横車体速度の推定値、前記上下加速度の検出値、前記ロール角速度の検出値、前記姿勢角推定手段で推定された前記ロール角の前回推定値、及び前記姿勢角推定手段で推定された前記ピッチ角の前回推定値に基づいて、前記ピッチ角速度を推定する車両姿勢角推定装置。
2. 各輪の車輪速度に基づいて車両前後方向の車体速度である前後車体速度を推定する前後速度推定手段と、
   車両横方向の車体速度である横車体速度を推定する横速度推定手段と、
   ピッチ角速度を推定するピッチ角速度推定手段と、
   車両運動の前後加速度、横加速度、上下加速度、ヨー角速度、及びロール角速度の各検出値に応じたセンサ信号、前記前後車体速度の推定値、前記横速度推定手段で推定された前記横車体速度の微分値の推定値又は前記横車体速度の推定値の微分値、並びに前記ピッチ角速度の推定値に基づいて、車体の鉛直軸に対する姿勢角であるロール角及びピッチ角を推定する姿勢角推定手段と、
   を含む車両姿勢角推定装置であって、
   前記ピッチ角速度推定手段が、前記前後車体速度の推定値、前記横車体速度の推定値、前記上下加速度の検出値、前記ロール角速度の検出値、前記姿勢角推定手段で推定された前記ロール角の前回推定値、及び前記姿勢角推定手段で推定された前記ピッチ角の前回推定値に基づいて、前記ピッチ角速度を推定する車両姿勢角推定装置。
3. 車両運動の前後加速度、横加速度、上下加速度、ヨー角速度、及びロール角速度の各々を検出して検出値に応じたセンサ信号を出力するセンサを更に含む請求項１又は２記載の車両姿勢角推定装置。
4. コンピュータを、
   各輪の車輪速度に基づいて車両前後方向の車体速度である前後車体速度を推定する前後速度推定手段と、
   車両横方向の車体速度である横車体速度を推定する横速度推定手段と、
   ピッチ角速度を推定するピッチ角速度推定手段と、
   車両運動の前後加速度、横加速度、上下加速度、ヨー角速度、及びロール角速度の各検出値に応じたセンサ信号、前記前後車体速度の推定値、並びに前記ピッチ角速度の推定値に基づいて、車体の鉛直軸に対する姿勢角であるロール角及びピッチ角を推定する姿勢角推定手段と、
   して機能させるためのプログラムであって、
   前記ピッチ角速度推定手段が、前記前後車体速度の推定値、前記横車体速度の推定値、前記上下加速度の検出値、前記ロール角速度の検出値、前記姿勢角推定手段で推定された前記ロール角の前回推定値、及び前記姿勢角推定手段で推定された前記ピッチ角の前回推定値に基づいて、前記ピッチ角速度を推定するプログラム。
5. コンピュータを、
   各輪の車輪速度に基づいて車両前後方向の車体速度である前後車体速度を推定する前後速度推定手段と、
   車両横方向の車体速度である横車体速度を推定する横速度推定手段と、
   ピッチ角速度を推定するピッチ角速度推定手段と、
   車両運動の前後加速度、横加速度、上下加速度、ヨー角速度、及びロール角速度の各検出値に応じたセンサ信号、前記前後車体速度の推定値、前記横速度推定手段で推定された前記横車体速度の微分値の推定値又は前記横車体速度の推定値の微分値、並びに前記ピッチ角速度の推定値に基づいて、車体の鉛直軸に対する姿勢角であるロール角及びピッチ角を推定する姿勢角推定手段と、
   して機能させるためのプログラムであって、
   前記ピッチ角速度推定手段が、前記前後車体速度の推定値、前記横車体速度の推定値、前記上下加速度の検出値、前記ロール角速度の検出値、前記姿勢角推定手段で推定された前記ロール角の前回推定値、及び前記姿勢角推定手段で推定された前記ピッチ角の前回推定値に基づいて、前記ピッチ角速度を推定するプログラム。

Images