(57)【要約】【課題】ドリフト誤差による影響を軽減して、精度よく姿勢角を推定することができるようにする。【解決手段】前後車体速度を推定する前後速度推定手段１２と、横車体速度を推定する横速度推定手段１０と、ピッチ角速度を推定するピッチ角速度推定手段２４と、ロール角速度センサ２０及び上下加速度センサ１４からのセンサ信号について、初期不感帯を設定するように補正する初期不感帯補正手段２３と、各センサ信号、補正されたロール角速度センサ２０及び上下加速度センサ１４からのセンサ信号、前後車体速度の推定値、並びにピッチ角速度の推定値に基づいて、ロール角及びピッチ角を推定する姿勢角オブザーバ２６と、を備えている。ピッチ角速度推定手段２４は、前後車体速度の推定値、横車体速度の推定値、補正されたロール角速度及び上下加速度の各検出値、並びに推定されたロール角及びピッチ角の前回推定値に基づいて、ピッチ角速度を推定する。【選択図】図１
2010043945.tif 000002
【特許請求の範囲】
【請求項１】
各輪の車輪速度に基づいて車両前後方向の車体速度である前後車体速度を推定する前後速度推定手段と、
車両横方向の車体速度である横車体速度を推定する横速度推定手段と、
ピッチ角速度を推定するピッチ角速度推定手段と、
車両運動のロール角速度の検出値に応じたセンサ信号について、予め定められたセンサドリフト最大量、及び姿勢角の推定を開始してから現時点までにおける前記検出値の絶対値の最小値の何れか小さい方の分だけ、前記検出値の絶対値を小さくするように前記センサ信号を補正する補正手段と、
車両運動の前後加速度、横加速度、上下加速度、及びヨー角速度の各検出値に応じたセンサ信号、前記補正手段によって補正された前記ロール角速度の検出値に応じたセンサ信号、前記前後車体速度の推定値、並びに前記ピッチ角速度の推定値に基づいて、車体の鉛直軸に対する姿勢角であるロール角及びピッチ角を推定する姿勢角推定手段と、
を含む車両姿勢角推定装置であって、
前記ピッチ角速度推定手段が、前記前後車体速度の推定値、前記横車体速度の推定値、前記上下加速度の検出値、前記補正された前記ロール角速度の検出値、前記姿勢角推定手段で推定された前記ロール角の前回推定値、及び前記姿勢角推定手段で推定された前記ピッチ角の前回推定値に基づいて、前記ピッチ角速度を推定する車両姿勢角推定装置。
【請求項２】
各輪の車輪速度に基づいて車両前後方向の車体速度である前後車体速度を推定する前後速度推定手段と、
車両横方向の車体速度である横車体速度を推定する横速度推定手段と、
ピッチ角速度を推定するピッチ角速度推定手段と、
車両運動の上下加速度の検出値に応じたセンサ信号について、予め定められたセンサドリフト最大量、及び姿勢角の推定を開始してから現時点までにおける前記検出値と標準値との差の絶対値の最小値の何れか小さい方の分だけ、前記検出値と前記標準値との差の絶対値を小さくするように前記センサ信号を補正する補正手段と、
車両運動の前後加速度、横加速度、ヨー角速度、及びロール角速度の各検出値に応じたセンサ信号、前記補正手段によって補正された前記上下加速度の検出値に応じたセンサ信号、前記前後車体速度の推定値、並びに前記ピッチ角速度の推定値に基づいて、車体の鉛直軸に対する姿勢角であるロール角及びピッチ角を推定する姿勢角推定手段と、
を含む車両姿勢角推定装置であって、
前記ピッチ角速度推定手段が、前記前後車体速度の推定値、前記横車体速度の推定値、前記補正された前記上下加速度の検出値、前記ロール角速度の検出値、前記姿勢角推定手段で推定された前記ロール角の前回推定値、及び前記姿勢角推定手段で推定された前記ピッチ角の前回推定値に基づいて、前記ピッチ角速度を推定する車両姿勢角推定装置。
【請求項３】
各輪の車輪速度に基づいて車両前後方向の車体速度である前後車体速度を推定する前後速度推定手段と、
車両横方向の車体速度である横車体速度を推定する横速度推定手段と、
車両運動のピッチ角速度の検出値に応じたセンサ信号について、予め定められたセンサドリフト最大量、及び姿勢角の推定を開始してから現時点までにおける前記検出値の絶対値の最小値の何れか小さい方の分だけ、前記検出値の絶対値を小さくするように前記センサ信号を補正する補正手段と、
車両運動の前後加速度、横加速度、上下加速度、ヨー角速度、及びロール角速度の各検出値に応じたセンサ信号、前記補正手段によって補正された前記ピッチ角速度の検出値に応じたセンサ信号、並びに前記前後車体速度の推定値に基づいて、車体の鉛直軸に対する姿勢角であるロール角及びピッチ角を推定する姿勢角推定手段と、
を含む車両姿勢角推定装置。
【請求項４】
前記補正手段は、上下加速度の検出値に応じたセンサ信号を補正すると共に、前記ロール角速度の検出値に応じたセンサ信号について、予め定められたセンサドリフト最大量、及び姿勢角の推定を開始してから現時点までにおける前記検出値の絶対値の最小値の何れか小さい方の分だけ、前記検出値の絶対値を小さくするように前記センサ信号を補正し、
前記ピッチ角速度推定手段は、前記前後車体速度の推定値、前記横車体速度の推定値、前記補正された前記上下加速度の検出値、前記補正された前記ロール角速度の検出値、前記姿勢角推定手段で推定された前記ロール角の前回推定値、及び前記姿勢角推定手段で推定された前記ピッチ角の前回推定値に基づいて、前記ピッチ角速度を推定し、
前記姿勢角推定手段は、車両運動の前後加速度、横加速度、及びヨー角速度の各検出値に応じたセンサ信号、前記補正手段によって補正された前記上下加速度の検出値に応じたセンサ信号、前記補正手段によって補正された前記ロール角速度の検出値に応じたセンサ信号、前記前後車体速度の推定値、並びに前記ピッチ角速度の推定値に基づいて、車体の鉛直軸に対する姿勢角であるロール角及びピッチ角を推定する請求項２記載の車両姿勢角推定装置。
【請求項５】
前記補正手段は、ピッチ角速度の検出値に応じたセンサ信号を補正すると共に、前記ロール角速度の検出値に応じたセンサ信号について、予め定められたセンサドリフト最大量、及び姿勢角の推定を開始してから現時点までにおける前記検出値の絶対値の最小値の何れか小さい方の分だけ、前記検出値の絶対値を小さくするように前記センサ信号を補正し、
前記姿勢角推定手段は、車両運動の前後加速度、横加速度、上下加速度、及びヨー角速度の各検出値に応じたセンサ信号、前記補正手段によって補正された前記ピッチ角速度の検出値に応じたセンサ信号、前記補正手段によって補正された前記ロール角速度の検出値に応じたセンサ信号、並びに前記前後車体速度の推定値に基づいて、車体の鉛直軸に対する姿勢角であるロール角及びピッチ角を推定する請求項３記載の車両姿勢角推定装置。
【請求項６】
前記補正手段は、ピッチ角速度の検出値に応じたセンサ信号及び前記ロール角速度の検出値に応じたセンサ信号を補正すると共に、前記上下加速度の検出値に応じたセンサ信号について、予め定められたセンサドリフト最大量、及び姿勢角の推定を開始してから現時点までにおける前記検出値と標準値との差の絶対値の最小値の何れか小さい方の分だけ、前記検出値と前記標準値との差の絶対値を小さくするように前記センサ信号を補正し、
前記姿勢角推定手段は、車両運動の前後加速度、横加速度、及びヨー角速度の各検出値に応じたセンサ信号、前記補正手段によって補正された前記上下加速度の検出値に応じたセンサ信号、前記補正手段によって補正された前記ピッチ角速度の検出値に応じたセンサ信号、前記補正手段によって補正された前記ロール角速度の検出値に応じたセンサ信号、並びに前記前後車体速度の推定値に基づいて、車体の鉛直軸に対する姿勢角であるロール角及びピッチ角を推定する請求項５記載の車両姿勢角推定装置。
【請求項７】
前記補正手段は、ピッチ角速度の検出値に応じたセンサ信号を補正すると共に、前記上下加速度の検出値に応じたセンサ信号について、予め定められたセンサドリフト最大量、及び姿勢角の推定を開始してから現時点までにおける前記検出値と標準値との差の絶対値の最小値の何れか小さい方の分だけ、前記検出値と前記標準値との差の絶対値を小さくするように前記センサ信号を補正し、
前記姿勢角推定手段は、車両運動の前後加速度、横加速度、ロール角速度、及びヨー角速度の各検出値に応じたセンサ信号、前記補正手段によって補正された前記ピッチ角速度の検出値に応じたセンサ信号、前記補正手段によって補正された前記上下加速度の検出値に応じたセンサ信号、並びに前記前後車体速度の推定値に基づいて、車体の鉛直軸に対する姿勢角であるロール角及びピッチ角を推定する請求項３記載の車両姿勢角推定装置。
【請求項８】
車両運動のロール角速度の検出値に応じたセンサ信号について、予め定められたセンサドリフト最大量、及び姿勢角の推定を開始してから現時点までにおける前記検出値の絶対値の最小値の何れか小さい方の分だけ、前記検出値の絶対値を小さくするように前記センサ信号を補正すると共に、上下加速度の検出値に応じたセンサ信号について、予め定められたセンサドリフト最大量、及び姿勢角の推定を開始してから現時点までにおける前記検出値と標準値との差の絶対値の最小値の何れか小さい方の分だけ、前記検出値と前記標準値との差の絶対値を小さくするように前記センサ信号を補正する補正手段と、
車両運動のヨー角速度、前後加速度、及び横加速度の各検出値に応じたセンサ信号と、前記補正手段によって補正された前記ロール角速度及び前記上下加速度の各検出値に応じたセンサ信号とに基づいて、車体の鉛直軸に対するロール角及びピッチ角の各々の微分量を算出し、算出した前記ロール角及び前記ピッチ角の各々の微分量を積分して、前記ロール角及び前記ピッチ角を推定する姿勢角推定手段と、
前記上下加速度、前記前後加速度、前記横加速度、前記ロール角速度、及び前記ヨー角速度の各検出値に応じたセンサ信号と、前記姿勢角推定手段によって推定された前記ロール角及び前記ピッチ角とに基づいて、車両運動の運動方程式より得られる前記ロール角及び前記ピッチ角の各々の微分量を算出する算出手段と、
前記センサ信号のセンサドリフト量を考慮したときに、前記姿勢角推定手段により算出された前記ロール角及び前記ピッチ角の各々の微分量と、前記算出手段により算出された前記ロール角及び前記ピッチ角の各々の微分量に前記センサドリフト量を考慮した値とが等しくなる関係を用いて、前記上下加速度の検出値に応じたセンサ信号及び前記ロール角速度の検出値に応じたセンサ信号の各々のセンサドリフト量を推定するドリフト量推定手段と、
を含むセンサドリフト量推定装置。
【請求項９】
車両運動のロール角速度の検出値に応じたセンサ信号について、予め定められたセンサドリフト最大量、及び姿勢角の推定を開始してから現時点までにおける前記検出値の絶対値の最小値の何れか小さい方の分だけ、前記検出値の絶対値を小さくするように前記センサ信号を補正すると共に、ピッチ角速度の検出値に応じたセンサ信号について、予め定められたセンサドリフト最大量、及び姿勢角の推定を開始してから現時点までにおける前記検出値の絶対値の最小値の何れか小さい方の分だけ、前記検出値の絶対値を小さくするように前記センサ信号を補正する補正手段と、
車両運動の上下加速度、前後加速度、横加速度、及びヨー角速度の各検出値に応じたセンサ信号と、前記補正手段によって補正された前記ロール角速度及び前記ピッチ角速度の各検出値に応じたセンサ信号とに基づいて、車体の鉛直軸に対するロール角及びピッチ角の各々の微分量を算出し、算出した前記ロール角及び前記ピッチ角の各々の微分量を積分して、前記ロール角及び前記ピッチ角を推定する姿勢角推定手段と、
前記ロール角速度、前記ヨー角速度、及び前記ピッチ角速度の各検出値に応じたセンサ信号と、前記姿勢角推定手段によって推定された前記ロール角及び前記ピッチ角とに基づいて、車両運動の運動方程式より得られる前記ロール角及び前記ピッチ角の各々の微分量を算出する算出手段と、
前記センサ信号のセンサドリフト量を考慮したときに、前記姿勢角推定手段により算出された前記ロール角及び前記ピッチ角の各々の微分量と、前記算出手段により算出された前記ロール角及び前記ピッチ角の各々の微分量に前記センサドリフト量を考慮した値とが等しくなる関係を用いて、前記ピッチ角速度の検出値に応じたセンサ信号及び前記ロール角速度の検出値に応じたセンサ信号の各々のセンサドリフト量を推定するドリフト量推定手段と、
を含むセンサドリフト量推定装置。
【請求項１０】
前記ドリフト量推定手段によって推定された前記センサドリフト量に基づいて、前記上下加速度の検出値に応じたセンサ信号及び前記ロール角速度の検出値に応じたセンサ信号の各々を補正するドリフト補正手段を更に含み、
前記姿勢角推定手段は、前記ヨー角速度の検出値に応じたセンサ信号と、前記ドリフト補正手段によって補正され、かつ、前記補正手段によって補正された前記ロール角速度及び前記上下加速度の各検出値に応じたセンサ信号とに基づいて、前記ロール角及び前記ピッチ角を推定する請求項８記載のセンサドリフト量推定装置。
【請求項１１】
前記ドリフト量推定手段によって推定された前記センサドリフト量に基づいて、前記ピッチ角速度の検出値に応じたセンサ信号及び前記ロール角速度の検出値に応じたセンサ信号の各々を補正するドリフト補正手段を更に含み、
前記姿勢角推定手段は、前記上下加速度、前記前後加速度、前記横加速度、及び前記ヨー角速度の各検出値に応じたセンサ信号と、前記ドリフト補正手段によって補正され、かつ、前記補正手段によって補正された前記ロール角速度及び前記ピッチ角速度の各検出値に応じたセンサ信号とに基づいて、前記ロール角及び前記ピッチ角を推定する請求項９記載のセンサドリフト量推定装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、車両姿勢角推定装置及びセンサドリフト推定装置にかかり、特に、オブザーバによって車両物理量としての鉛直軸に対する車両の姿勢角であるロール角及びピッチ角を推定する車両姿勢角推定装置、及び車両運動の運動状態量の検出値に応じたセンサ信号のセンサドリフト量を推定するセンサドリフト量推定装置に関する。
【背景技術】
【０００２】
従来より、車載の角速度センサ及び加速度センサを利用して車両の姿勢角を推定する姿勢角推定装置において、車載の角速度センサ及び加速度センサのゼロ点ドリフトは、姿勢角の推定への影響が大きいことから、ゼロ点ドリフトを逐次推定して補償する必要がある。
【０００３】
このようなセンサのゼロ点ドリフトを推定する従来技術として、センサ出力が閾値内に一定時間以上ある場合に、物体が静止していると判定し、このときのセンサ出力値をセンサドリフト量とする手法（例えば、特許文献１）や、車輪速の左右差が小さいときに直進状態と判定し、このときの横加速度を横加速度センサドリフト量とする手法（例えば、特許文献２）などが知られている。
【特許文献１】特許第３７９５４９８号公報
【特許文献２】特開平７−４００４３号公報
【発明の開示】
【発明が解決しようとする課題】
【０００４】
しかしながら、上記の特許文献１、２に記載の技術では、静止又は直進状態を判定し、このときのセンサ出力をドリフト量として演算しているため、センサが搭載された物体が静止していない場合や等速運動していない場合には、センサのドリフト量を推定することができず、精度よく姿勢角を推定することができない、という問題がある。
【０００５】
本発明は、上記の問題点を解決するためになされたもので、ドリフト誤差による影響を軽減して、精度よく姿勢角を推定することができる姿勢角推定装置及びセンサドリフト量推定装置を提供することを目的とする。
【課題を解決するための手段】
【０００６】
上記目的を達成するために第１の発明の車両姿勢角推定装置は、各輪の車輪速度に基づいて車両前後方向の車体速度である前後車体速度を推定する前後速度推定手段と、車両横方向の車体速度である横車体速度を推定する横速度推定手段と、ピッチ角速度を推定するピッチ角速度推定手段と、車両運動のロール角速度の検出値に応じたセンサ信号について、予め定められたセンサドリフト最大量、及び姿勢角の推定を開始してから現時点までにおける前記検出値の絶対値の最小値の何れか小さい方の分だけ、前記検出値の絶対値を小さくするように前記センサ信号を補正する補正手段と、車両運動の前後加速度、横加速度、上下加速度、及びヨー角速度の各検出値に応じたセンサ信号、前記補正手段によって補正された前記ロール角速度の検出値に応じたセンサ信号、前記前後車体速度の推定値、並びに前記ピッチ角速度の推定値に基づいて、車体の鉛直軸に対する姿勢角であるロール角及びピッチ角を推定する姿勢角推定手段と、を含む車両姿勢角推定装置であって、前記ピッチ角速度推定手段が、前記前後車体速度の推定値、前記横車体速度の推定値、前記上下加速度の検出値、前記補正された前記ロール角速度の検出値、前記姿勢角推定手段で推定された前記ロール角の前回推定値、及び前記姿勢角推定手段で推定された前記ピッチ角の前回推定値に基づいて、前記ピッチ角速度を推定するようにしたものである。
【０００７】
第１の発明の車両姿勢角推定装置には、車両運動の前後加速度、横加速度、上下加速度、ヨー角速度、及びロール角速度の各々を検出して検出値に応じたセンサ信号を出力するセンサを設けることができる。
【０００８】
第１の発明では、車両運動のロール角速度の検出値に応じたセンサ信号について、予め定められたセンサドリフト最大量、及び姿勢角の推定を開始してから現時点までにおける前記検出値の絶対値の最小値の何れか小さい方の分だけ、検出値の絶対値を小さくするようにセンサ信号を補正する。また、車両運動の前後加速度、横加速度、上下加速度、及びヨー角速度の各検出値に応じたセンサ信号、補正手段によって補正されたロール角速度の検出値に応じたセンサ信号、前後車体速度の推定値、並びにピッチ角速度の推定値に基づいて、車体の鉛直軸に対する姿勢角であるロール角及びピッチ角を推定する。
【０００９】
ピッチ角速度は、前後車体速度の推定値、横車体速度の推定値、上下加速度の検出値、補正されたロール角速度の検出値、ロール角の前回推定値、及びピッチ角の前回推定値に基づいて、推定される。
【００１０】
第１の発明では、車体の上下方向運動の釣り合いからピッチ角速度を推定し、ピッチ角速度の推定値を利用して車体の鉛直軸に対する姿勢角であるロール角及びピッチ角を推定しているので、上下加速度が増加するバンク走行時においても精度良くロール角及びピッチ角を推定することができる。また、初期不感帯を設定するように、ロール角速度の検出値の絶対値を小さくするようにセンサ信号を補正しているので、ドリフト誤差による影響を軽減して、精度良くロール角及びピッチ角を推定することができる。
【００１１】
第２の発明の車両姿勢角推定装置は、各輪の車輪速度に基づいて車両前後方向の車体速度である前後車体速度を推定する前後速度推定手段と、車両横方向の車体速度である横車体速度を推定する横速度推定手段と、ピッチ角速度を推定するピッチ角速度推定手段と、車両運動の上下加速度の検出値に応じたセンサ信号について、予め定められたセンサドリフト最大量、及び姿勢角の推定を開始してから現時点までにおける前記検出値と標準値との差の絶対値の最小値の何れか小さい方の分だけ、前記検出値と前記標準値との差の絶対値を小さくするように前記センサ信号を補正する補正手段と、車両運動の前後加速度、横加速度、ヨー角速度、及びロール角速度の各検出値に応じたセンサ信号、前記補正手段によって補正された前記上下加速度の検出値に応じたセンサ信号、前記前後車体速度の推定値、並びに前記ピッチ角速度の推定値に基づいて、車体の鉛直軸に対する姿勢角であるロール角及びピッチ角を推定する姿勢角推定手段と、を含む車両姿勢角推定装置であって、前記ピッチ角速度推定手段が、前記前後車体速度の推定値、前記横車体速度の推定値、前記補正された前記上下加速度の検出値、前記ロール角速度の検出値、前記姿勢角推定手段で推定された前記ロール角の前回推定値、及び前記姿勢角推定手段で推定された前記ピッチ角の前回推定値に基づいて、前記ピッチ角速度を推定するようにしたものである。
【００１２】
第２の発明の車両姿勢角推定装置には、車両運動の前後加速度、横加速度、上下加速度、ヨー角速度、及びロール角速度の各々を検出して検出値に応じたセンサ信号を出力するセンサを設けることができる。
【００１３】
第２の発明では、補正手段によって、車両運動の上下加速度の検出値に応じたセンサ信号について、予め定められたセンサドリフト最大量、及び姿勢角の推定を開始してから現時点までにおける前記検出値と標準値との差の絶対値の最小値の何れか小さい方の分だけ、検出値と標準値との差の絶対値を小さくするようにセンサ信号を補正する。また、車両運動の前後加速度、横加速度、ロール角速度、及びヨー角速度の各検出値に応じたセンサ信号、補正手段によって補正された上下加速度の検出値に応じたセンサ信号、前後車体速度の推定値、並びにピッチ角速度の推定値に基づいて、車体の鉛直軸に対する姿勢角であるロール角及びピッチ角を推定する。
【００１４】
ピッチ角速度は、前後車体速度の推定値、横車体速度の推定値、ロール角速度の検出値、補正された上下加速度の検出値、ロール角の前回推定値、及びピッチ角の前回推定値に基づいて、推定される。
【００１５】
第２の発明では、車体の上下方向運動の釣り合いからピッチ角速度を推定し、ピッチ角速度の推定値を利用して車体の鉛直軸に対する姿勢角であるロール角及びピッチ角を推定しているので、上下加速度が増加するバンク走行時においても精度良くロール角及びピッチ角を推定することができる。また、初期不感帯を設定するように、上下加速度の検出値の標準値との差の絶対値を小さくするようにセンサ信号を補正しているので、ドリフト誤差による影響を軽減して、精度良くロール角及びピッチ角を推定することができる。
【００１６】
第３の発明の車両姿勢角推定装置は、各輪の車輪速度に基づいて車両前後方向の車体速度である前後車体速度を推定する前後速度推定手段と、車両横方向の車体速度である横車体速度を推定する横速度推定手段と、車両運動のピッチ角速度の検出値に応じたセンサ信号について、予め定められたセンサドリフト最大量、及び姿勢角の推定を開始してから現時点までにおける前記検出値の絶対値の最小値の何れか小さい方の分だけ、前記検出値の絶対値を小さくするように前記センサ信号を補正する補正手段と、車両運動の前後加速度、横加速度、上下加速度、ヨー角速度、及びロール角速度の各検出値に応じたセンサ信号、前記補正手段によって補正された前記ピッチ角速度の検出値に応じたセンサ信号、並びに前記前後車体速度の推定値に基づいて、車体の鉛直軸に対する姿勢角であるロール角及びピッチ角を推定する姿勢角推定手段とを含んで構成されている。
【００１７】
第３の発明の車両姿勢角推定装置には、車両運動の前後加速度、横加速度、上下加速度、ヨー角速度、ロール角速度、ピッチ角速度の各々を検出して検出値に応じたセンサ信号を出力するセンサを設けることができる。
【００１８】
第３の発明では、車両運動のピッチ角速度の検出値に応じたセンサ信号について、予め定められたセンサドリフト最大量、及び姿勢角の推定を開始してから現時点までにおける前記検出値の絶対値の最小値の何れか小さい方の分だけ、検出値の絶対値を小さくするようにセンサ信号を補正する。また、車両運動の前後加速度、横加速度、上下加速度、ロール角速度、及びヨー角速度の各検出値に応じたセンサ信号、補正手段によって補正されたピッチ角速度の検出値に応じたセンサ信号、並びに前後車体速度の推定値に基づいて、車体の鉛直軸に対する姿勢角であるロール角及びピッチ角を推定する。
【００１９】
第３の発明では、初期不感帯を設定するように、ピッチ角速度の検出値の絶対値を小さくするようにセンサ信号を補正しているので、ドリフト誤差による影響を軽減して、精度良くロール角及びピッチ角を推定することができる。
【００２０】
第２の発明に係る補正手段は、上下加速度の検出値に応じたセンサ信号を補正すると共に、ロール角速度の検出値に応じたセンサ信号について、予め定められたセンサドリフト最大量、及び姿勢角の推定を開始してから現時点までにおける検出値の絶対値の最小値の何れか小さい方の分だけ、検出値の絶対値を小さくするようにセンサ信号を補正し、ピッチ角速度推定手段は、前後車体速度の推定値、横車体速度の推定値、補正された上下加速度の検出値、補正されたロール角速度の検出値、姿勢角推定手段で推定されたロール角の前回推定値、及び姿勢角推定手段で推定されたピッチ角の前回推定値に基づいて、ピッチ角速度を推定し、姿勢角推定手段は、車両運動の前後加速度、横加速度、及びヨー角速度の各検出値に応じたセンサ信号、補正手段によって補正された上下加速度の検出値に応じたセンサ信号、補正手段によって補正されたロール角速度の検出値に応じたセンサ信号、前後車体速度の推定値、並びにピッチ角速度の推定値に基づいて、車体の鉛直軸に対する姿勢角であるロール角及びピッチ角を推定することができる。これによって、初期不感帯を設定するように、ロール角速度の検出値の絶対値を小さくするようにセンサ信号を補正すると共に、上下加速度の検出値の標準値との差の絶対値を小さくするようにセンサ信号を補正しているので、ドリフト誤差による影響を軽減して、精度良くロール角及びピッチ角を推定することができる。
【００２１】
第３の発明に係る補正手段は、ピッチ角速度の検出値に応じたセンサ信号を補正すると共に、ロール角速度の検出値に応じたセンサ信号について、予め定められたセンサドリフト最大量、及び姿勢角の推定を開始してから現時点までにおける検出値の絶対値の最小値の何れか小さい方の分だけ、検出値の絶対値を小さくするようにセンサ信号を補正し、姿勢角推定手段は、車両運動の前後加速度、横加速度、上下加速度、及びヨー角速度の各検出値に応じたセンサ信号、補正手段によって補正されたピッチ角速度の検出値に応じたセンサ信号、補正手段によって補正されたロール角速度の検出値に応じたセンサ信号、並びに前後車体速度の推定値に基づいて、車体の鉛直軸に対する姿勢角であるロール角及びピッチ角を推定することができる。これによって、初期不感帯を設定するように、ロール角速度の検出値の絶対値を小さくするようにセンサ信号を補正すると共に、ピッチ角速度の検出値の絶対値を小さくするようにセンサ信号を補正しているので、ドリフト誤差による影響を軽減して、精度良くロール角及びピッチ角を推定することができる。
【００２２】
また、上記のピッチ角速度及びロール角速度の各検出値に応じたセンサ信号を補正する補正手段は、ピッチ角速度の検出値に応じたセンサ信号及びロール角速度の検出値に応じたセンサ信号を補正すると共に、上下加速度の検出値に応じたセンサ信号について、予め定められたセンサドリフト最大量、及び姿勢角の推定を開始してから現時点までにおける検出値と標準値との差の絶対値の最小値の何れか小さい方の分だけ、検出値と標準値との差の絶対値を小さくするようにセンサ信号を補正し、姿勢角推定手段は、車両運動の前後加速度、横加速度、及びヨー角速度の各検出値に応じたセンサ信号、補正手段によって補正された上下加速度の検出値に応じたセンサ信号、補正手段によって補正されたピッチ角速度の検出値に応じたセンサ信号、補正手段によって補正されたロール角速度の検出値に応じたセンサ信号、並びに前後車体速度の推定値に基づいて、車体の鉛直軸に対する姿勢角であるロール角及びピッチ角を推定することができる。これによって、初期不感帯を設定するように、ロール角速度の検出値の絶対値を小さくするようにセンサ信号を補正し、ピッチ角速度の検出値の絶対値を小さくするようにセンサ信号を補正すると共に、上下加速度の検出値の標準値との差の絶対値を小さくするようにセンサ信号を補正しているので、ドリフト誤差による影響を軽減して、精度良くロール角及びピッチ角を推定することができる。
【００２３】
第３の発明に係る補正手段は、ピッチ角速度の検出値に応じたセンサ信号を補正すると共に、上下加速度の検出値に応じたセンサ信号について、予め定められたセンサドリフト最大量、及び姿勢角の推定を開始してから現時点までにおける検出値と標準値との差の絶対値の最小値の何れか小さい方の分だけ、検出値と標準値との差の絶対値を小さくするようにセンサ信号を補正し、姿勢角推定手段は、車両運動の前後加速度、横加速度、ロール角速度、及びヨー角速度の各検出値に応じたセンサ信号、補正手段によって補正されたピッチ角速度の検出値に応じたセンサ信号、補正手段によって補正された上下加速度の検出値に応じたセンサ信号、並びに前後車体速度の推定値に基づいて、車体の鉛直軸に対する姿勢角であるロール角及びピッチ角を推定することができる。これによって、初期不感帯を設定するように、ピッチ角速度の検出値の絶対値を小さくするようにセンサ信号を補正すると共に、上下加速度の検出値の標準値との差の絶対値を小さくするようにセンサ信号を補正しているので、ドリフト誤差による影響を軽減して、精度良くロール角及びピッチ角を推定することができる。
【００２４】
第４の発明に係るセンサドリフト量推定装置は、車両運動のロール角速度の検出値に応じたセンサ信号について、予め定められたセンサドリフト最大量、及び姿勢角の推定を開始してから現時点までにおける検出値の絶対値の最小値の何れか小さい方の分だけ、前記検出値の絶対値を小さくするように前記センサ信号を補正すると共に、上下加速度の検出値に応じたセンサ信号について、予め定められたセンサドリフト最大量、及び姿勢角の推定を開始してから現時点までにおける検出値と標準値との差の絶対値の最小値の何れか小さい方の分だけ、前記検出値と前記標準値との差の絶対値を小さくするように前記センサ信号を補正する補正手段と、車両運動のヨー角速度、前後加速度、及び横加速度の各検出値に応じたセンサ信号と、前記補正手段によって補正された前記ロール角速度及び前記上下加速度の各検出値に応じたセンサ信号とに基づいて、車体の鉛直軸に対するロール角及びピッチ角の各々の微分量を算出し、算出した前記ロール角及び前記ピッチ角の各々の微分量を積分して、前記ロール角及び前記ピッチ角を推定する姿勢角推定手段と、前記上下加速度、前記前後加速度、前記横加速度、前記ロール角速度、及び前記ヨー角速度の各検出値に応じたセンサ信号と、前記姿勢角推定手段によって推定された前記ロール角及び前記ピッチ角とに基づいて、車両運動の運動方程式より得られる前記ロール角及び前記ピッチ角の各々の微分量を算出する算出手段と、前記センサ信号のセンサドリフト量を考慮したときに、前記姿勢角推定手段により算出された前記ロール角及び前記ピッチ角の各々の微分量と、前記算出手段により算出された前記ロール角及び前記ピッチ角の各々の微分量に前記センサドリフト量を考慮した値とが等しくなる関係を用いて、前記上下加速度の検出値に応じたセンサ信号及び前記ロール角速度の検出値に応じたセンサ信号の各々のセンサドリフト量を推定するドリフト量推定手段とを含んで構成されている。
【００２５】
第４の発明に係るセンサドリフト量推定装置によれば、補正手段によって、車両運動のロール角速度の検出値に応じたセンサ信号について、予め定められたセンサドリフト最大量、及び姿勢角の推定を開始してから現時点までにおける検出値の絶対値の最小値の何れか小さい方の分だけ、検出値の絶対値を小さくするようにセンサ信号を補正すると共に、上下加速度の検出値に応じたセンサ信号について、予め定められたセンサドリフト最大量、及び姿勢角の推定を開始してから現時点までにおける検出値と標準値との差の絶対値の最小値の何れか小さい方の分だけ、検出値と標準値との差の絶対値を小さくするようにセンサ信号を補正する。
【００２６】
そして、姿勢角推定手段によって、車両運動のヨー角速度、前後加速度、及び横加速度の各検出値に応じたセンサ信号と、補正手段によって補正されたロール角速度及び上下加速度の各検出値に応じたセンサ信号とに基づいて、車体の鉛直軸に対するロール角及びピッチ角の各々の微分量を算出し、算出したロール角及びピッチ角の各々の微分量を積分して、ロール角及びピッチ角を推定する。また、算出手段によって、上下加速度、前後加速度、横加速度、ロール角速度、及びヨー角速度の各検出値に応じたセンサ信号と、姿勢角推定手段によって推定されたロール角及びピッチ角とに基づいて、車両運動の運動方程式より得られるロール角及びピッチ角の各々の微分量を算出する。
【００２７】
そして、ドリフト量推定手段によって、センサ信号のセンサドリフト量を考慮したときに、姿勢角推定手段により算出されたロール角及びピッチ角の各々の微分量と、算出手段により算出されたロール角及びピッチ角の各々の微分量にセンサドリフト量を考慮した値とが等しくなる関係を用いて、上下加速度の検出値に応じたセンサ信号及びロール角速度の検出値に応じたセンサ信号の各々のセンサドリフト量を推定する。
【００２８】
このように、上下加速度及びロール角速度の各々を検出するセンサのセンサドリフト量を推定することにより、車両運動の状態に関わらず、安定して、上下加速度及びロール角速度の各々を検出するセンサのドリフト量を推定することができ、また、初期不感帯を設定するように、ロール角速度の検出値の絶対値を小さくするようにセンサ信号を補正すると共に、上下加速度の検出値の標準値との差の絶対値を小さくするようにセンサ信号を補正しているので、ドリフト誤差による影響を軽減して、精度良くロール角及びピッチ角を推定することができる。
【００２９】
第５の発明に係るセンサドリフト量推定装置は、車両運動のロール角速度の検出値に応じたセンサ信号について、予め定められたセンサドリフト最大量、及び姿勢角の推定を開始してから現時点までにおける検出値の絶対値の最小値の何れか小さい方の分だけ、前記検出値の絶対値を小さくするように前記センサ信号を補正すると共に、ピッチ角速度の検出値に応じたセンサ信号について、予め定められたセンサドリフト最大量、及び姿勢角の推定を開始してから現時点までにおける検出値の絶対値の最小値の何れか小さい方の分だけ、前記検出値の絶対値を小さくするように前記センサ信号を補正する補正手段と、車両運動の上下加速度、前後加速度、横加速度、及びヨー角速度の各検出値に応じたセンサ信号と、前記補正手段によって補正された前記ロール角速度及び前記ピッチ角速度の各検出値に応じたセンサ信号とに基づいて、車体の鉛直軸に対するロール角及びピッチ角の各々の微分量を算出し、算出した前記ロール角及び前記ピッチ角の各々の微分量を積分して、前記ロール角及び前記ピッチ角を推定する姿勢角推定手段と、前記ロール角速度、前記ヨー角速度、及び前記ピッチ角速度の各検出値に応じたセンサ信号と、前記姿勢角推定手段によって推定された前記ロール角及び前記ピッチ角とに基づいて、車両運動の運動方程式より得られる前記ロール角及び前記ピッチ角の各々の微分量を算出する算出手段と、前記センサ信号のセンサドリフト量を考慮したときに、前記姿勢角推定手段により算出された前記ロール角及び前記ピッチ角の各々の微分量と、前記算出手段により算出された前記ロール角及び前記ピッチ角の各々の微分量に前記センサドリフト量を考慮した値とが等しくなる関係を用いて、前記ピッチ角速度の検出値に応じたセンサ信号及び前記ロール角速度の検出値に応じたセンサ信号の各々のセンサドリフト量を推定するドリフト量推定手段とを含んで構成されている。
【００３０】
第５の発明に係るセンサドリフト量推定装置によれば、補正手段によって、車両運動のロール角速度の検出値に応じたセンサ信号について、予め定められたセンサドリフト最大量、及び姿勢角の推定を開始してから現時点までにおける検出値の絶対値の最小値の何れか小さい方の分だけ、検出値の絶対値を小さくするようにセンサ信号を補正すると共に、ピッチ角速度の検出値に応じたセンサ信号について、予め定められたセンサドリフト最大量、及び姿勢角の推定を開始してから現時点までにおける検出値の絶対値の最小値の何れか小さい方の分だけ、検出値の絶対値を小さくするようにセンサ信号を補正する。
【００３１】
そして、姿勢角推定手段によって、車両運動の上下加速度、前後加速度、横加速度、及びヨー角速度の各検出値に応じたセンサ信号と、補正手段によって補正されたロール角速度及びピッチ角速度の各検出値に応じたセンサ信号とに基づいて、車体の鉛直軸に対するロール角及びピッチ角の各々の微分量を算出し、算出したロール角及びピッチ角の各々の微分量を積分して、ロール角及びピッチ角を推定する。また、算出手段によって、ロール角速度、ヨー角速度、及びピッチ角速度の各検出値に応じたセンサ信号と、姿勢角推定手段によって推定されたロール角及びピッチ角とに基づいて、車両運動の運動方程式より得られるロール角及びピッチ角の各々の微分量を算出する。
【００３２】
そして、ドリフト量推定手段によって、センサ信号のセンサドリフト量を考慮したときに、姿勢角推定手段により算出されたロール角及びピッチ角の各々の微分量と、算出手段により算出されたロール角及びピッチ角の各々の微分量にセンサドリフト量を考慮した値とが等しくなる関係を用いて、ピッチ角速度の検出値に応じたセンサ信号及びロール角速度の検出値に応じたセンサ信号の各々のセンサドリフト量を推定する。
【００３３】
このように、ピッチ角速度及びロール角速度の各々を検出するセンサのセンサドリフト量を推定することにより、車両運動の状態に関わらず、安定して、ピッチ角速度及びロール角速度の各々を検出するセンサのドリフト量を推定することができ、また、初期不感帯を設定するように、ロール角速度の検出値の絶対値を小さくするようにセンサ信号を補正すると共に、ピッチ角速度の検出値の絶対値を小さくするようにセンサ信号を補正しているので、ドリフト誤差による影響を軽減して、精度良くロール角及びピッチ角を推定することができる。
【００３４】
第４の発明に係るセンサドリフト量推定装置は、ドリフト量推定手段によって推定されたセンサドリフト量に基づいて、上下加速度の検出値に応じたセンサ信号及びロール角速度の検出値に応じたセンサ信号の各々を補正するドリフト補正手段を更に含み、姿勢角推定手段は、ヨー角速度の検出値に応じたセンサ信号と、ドリフト補正手段によって補正され、かつ、補正手段によって補正されたロール角速度及び上下加速度の各検出値に応じたセンサ信号とに基づいて、ロール角及びピッチ角を推定することができる。
【００３５】
第５の発明に係るセンサドリフト量推定装置は、ドリフト量推定手段によって推定されたセンサドリフト量に基づいて、ピッチ角速度の検出値に応じたセンサ信号及びロール角速度の検出値に応じたセンサ信号の各々を補正するドリフト補正手段を更に含み、姿勢角推定手段は、上下加速度、前後加速度、横加速度、及びヨー角速度の各検出値に応じたセンサ信号と、ドリフト補正手段によって補正され、かつ、補正手段によって補正されたロール角速度及びピッチ角速度の各検出値に応じたセンサ信号とに基づいて、ロール角及びピッチ角を推定することができる。
【００３６】
本発明は、コンピュータを以下のように機能させるプログラムで構成することができる。
【００３７】
第１のプログラムは、コンピュータを、各輪の車輪速度に基づいて車両前後方向の車体速度である前後車体速度を推定する前後速度推定手段、車両横方向の車体速度である横車体速度を推定する横速度推定手段、ピッチ角速度を推定するピッチ角速度推定手段、車両運動のロール角速度の検出値に応じたセンサ信号について、予め定められたセンサドリフト最大量、及び姿勢角の推定を開始してから現時点までにおける前記検出値の絶対値の最小値の何れか小さい方の分だけ、前記検出値の絶対値を小さくするように前記センサ信号を補正する補正手段、及び車両運動の前後加速度、横加速度、上下加速度、及びヨー角速度の各検出値に応じたセンサ信号、前記補正手段によって補正された前記ロール角速度の検出値に応じたセンサ信号、前記前後車体速度の推定値、並びに前記ピッチ角速度の推定値に基づいて、車体の鉛直軸に対する姿勢角であるロール角及びピッチ角を推定する姿勢角推定手段として機能させるためのプログラムであって、前記ピッチ角速度推定手段が、前記前後車体速度の推定値、前記横車体速度の推定値、前記上下加速度の検出値、前記補正された前記ロール角速度の検出値、前記姿勢角推定手段で推定された前記ロール角の前回推定値、及び前記姿勢角推定手段で推定された前記ピッチ角の前回推定値に基づいて、前記ピッチ角速度を推定するプログラムである。
【００３８】
第２のプログラムは、コンピュータを、各輪の車輪速度に基づいて車両前後方向の車体速度である前後車体速度を推定する前後速度推定手段、車両横方向の車体速度である横車体速度を推定する横速度推定手段、ピッチ角速度を推定するピッチ角速度推定手段、車両運動の上下加速度の検出値に応じたセンサ信号について、予め定められたセンサドリフト最大量、及び姿勢角の推定を開始してから現時点までにおける前記検出値と標準値との差の絶対値の最小値の何れか小さい方の分だけ、前記検出値と前記標準値との差の絶対値を小さくするように前記センサ信号を補正する補正手段、及び車両運動の前後加速度、横加速度、ヨー角速度、及びロール角速度の各検出値に応じたセンサ信号、前記補正手段によって補正された前記上下加速度の検出値に応じたセンサ信号、前記前後車体速度の推定値、並びに前記ピッチ角速度の推定値に基づいて、車体の鉛直軸に対する姿勢角であるロール角及びピッチ角を推定する姿勢角推定手段として機能させるためのプログラムであって、前記ピッチ角速度推定手段が、前記前後車体速度の推定値、前記横車体速度の推定値、前記補正された前記上下加速度の検出値、前記ロール角速度の検出値、前記姿勢角推定手段で推定された前記ロール角の前回推定値、及び前記姿勢角推定手段で推定された前記ピッチ角の前回推定値に基づいて、前記ピッチ角速度を推定するプログラムである。
【００３９】
第３のプログラムは、コンピュータを、各輪の車輪速度に基づいて車両前後方向の車体速度である前後車体速度を推定する前後速度推定手段、車両横方向の車体速度である横車体速度を推定する横速度推定手段、車両運動のピッチ角速度の検出値に応じたセンサ信号について、予め定められたセンサドリフト最大量、及び姿勢角の推定を開始してから現時点までにおける前記検出値の絶対値の最小値の何れか小さい方の分だけ、前記検出値の絶対値を小さくするように前記センサ信号を補正する補正手段、及び車両運動の前後加速度、横加速度、上下加速度、ヨー角速度、及びロール角速度の各検出値に応じたセンサ信号、前記補正手段によって補正された前記ピッチ角速度の検出値に応じたセンサ信号、並びに前記前後車体速度の推定値に基づいて、車体の鉛直軸に対する姿勢角であるロール角及びピッチ角を推定する姿勢角推定手段として機能させるためのプログラムである。
【００４０】
第４のプログラムは、コンピュータを、車両運動のロール角速度の検出値に応じたセンサ信号について、予め定められたセンサドリフト最大量、及び姿勢角の推定を開始してから現時点までにおける前記検出値の絶対値の最小値の何れか小さい方の分だけ、前記検出値の絶対値を小さくするように前記センサ信号を補正すると共に、上下加速度の検出値に応じたセンサ信号について、予め定められたセンサドリフト最大量、及び姿勢角の推定を開始してから現時点までにおける前記検出値と標準値との差の絶対値の最小値の何れか小さい方の分だけ、前記検出値と前記標準値との差の絶対値を小さくするように前記センサ信号を補正する補正手段、車両運動のヨー角速度、前後加速度、横加速度の各検出値に応じたセンサ信号と、前記補正手段によって補正された前記ロール角速度及び前記上下加速度の各検出値に応じたセンサ信号とに基づいて、車体の鉛直軸に対するロール角及びピッチ角の各々の微分量を算出し、算出した前記ロール角及び前記ピッチ角の各々の微分量を積分して、前記ロール角及び前記ピッチ角を推定する姿勢角推定手段、前記上下加速度、前記前後加速度、前記横加速度、前記ロール角速度、及び前記ヨー角速度の各検出値に応じたセンサ信号と、前記姿勢角推定手段によって推定された前記ロール角及び前記ピッチ角とに基づいて、車両運動の運動方程式より得られる前記ロール角及び前記ピッチ角の各々の微分量を算出する算出手段、及び前記センサ信号のセンサドリフト量を考慮したときに、前記姿勢角推定手段により算出された前記ロール角及び前記ピッチ角の各々の微分量と、前記算出手段により算出された前記ロール角及び前記ピッチ角の各々の微分量に前記センサドリフト量を考慮した値とが等しくなる関係を用いて、前記上下加速度の検出値に応じたセンサ信号及び前記ロール角速度の検出値に応じたセンサ信号の各々のセンサドリフト量を推定するドリフト量推定手段として機能させるためのプログラムである。
【００４１】
第５のプログラムは、コンピュータを、車両運動のロール角速度の検出値に応じたセンサ信号について、予め定められたセンサドリフト最大量、及び姿勢角の推定を開始してから現時点までにおける前記検出値の絶対値の最小値の何れか小さい方の分だけ、前記検出値の絶対値を小さくするように前記センサ信号を補正すると共に、ピッチ角速度の検出値に応じたセンサ信号について、予め定められたセンサドリフト最大量、及び姿勢角の推定を開始してから現時点までにおける前記検出値の絶対値の最小値の何れか小さい方の分だけ、前記検出値の絶対値を小さくするように前記センサ信号を補正する補正手段、車両運動の上下加速度、前後加速度、横加速度、及びヨー角速度の各検出値に応じたセンサ信号と、前記補正手段によって補正された前記ロール角速度及び前記ピッチ角速度の各検出値に応じたセンサ信号とに基づいて、車体の鉛直軸に対するロール角及びピッチ角の各々の微分量を算出し、算出した前記ロール角及び前記ピッチ角の各々の微分量を積分して、前記ロール角及び前記ピッチ角を推定する姿勢角推定手段、前記ロール角速度、前記ヨー角速度、及び前記ピッチ角速度の各検出値に応じたセンサ信号と、前記姿勢角推定手段によって推定された前記ロール角及び前記ピッチ角とに基づいて、車両運動の運動方程式より得られる前記ロール角及び前記ピッチ角の各々の微分量を算出する算出手段、及び前記センサ信号のセンサドリフト量を考慮したときに、前記姿勢角推定手段により算出された前記ロール角及び前記ピッチ角の各々の微分量と、前記算出手段により算出された前記ロール角及び前記ピッチ角の各々の微分量に前記センサドリフト量を考慮した値とが等しくなる関係を用いて、前記ピッチ角速度の検出値に応じたセンサ信号及び前記ロール角速度の検出値に応じたセンサ信号の各々のセンサドリフト量を推定するドリフト量推定手段として機能させるためのプログラムである。
【発明の効果】
【００４２】
以上説明したように本発明によれば、初期不感帯を設定するように、ロール角速度、上下加速度、又はピッチ角速度のセンサ信号を補正しているので、ドリフト誤差による影響を軽減して、精度良くロール角及びピッチ角を推定することができる、という効果が得られる。
【発明を実施するための最良の形態】
【００４３】
以下、図面を参照して本発明の実施の形態を詳細に説明する。本実施の形態は、鉛直軸に対する車両の姿勢角であるピッチ角及びロール角を推定する姿勢角推定装置に本発明を適用したものである。
【００４４】
図１に示すように、第１の実施の形態の姿勢角推定装置には、車両横方向の車体速度である横車体速度Ｖを推定する横速度推定手段１０、及び各輪の車輪速度に基づいて車両前後方向の車体速度である前後車体速度Ｕを推定する前後速度推定手段１２が設けられている。
【００４５】
横車体速度Ｖは、カルマンフィルタ等を用いて推定したり、横加速度センサの検出値から推定したりすることができる。
【００４６】
各輪の車輪速度は、各輪に対応して設けられた車輪速センサにより検出することができ、前後車体速度Ｕは各輪の車輪速度、または各輪の車輪速度及び車輪速度の微分値から推定することができる。例えば、制動時には、４輪の車輪速度の最大値を前後車体速度Ｕとして出力し、駆動時には、従動輪の車輪速度の平均値を前後車体速度Ｕとして出力することができる。
【００４７】
また、本実施の形態の姿勢角推定装置には、車両運動のｘｙｚ３軸の加速度である上下加速度Ｇｚ、横加速度Ｇｙ、前後加速度Ｇｘの各々を検出する上下加速度センサ１４、横加速度センサ１６、及び前後加速度センサ１８が設けられている。図２に示すように、ｘ軸は車両前後方向、ｙ軸は車両幅方向（横方向）、ｚ軸は車両上下方向に対応している。
【００４８】
また、本実施の形態の姿勢角推定装置には、ロール角速度Ｐを検出するロール角速度センサ２０、及びヨー角速度Ｒを検出するヨー角速度センサ２２が設けられている。
【００４９】
上下加速度センサ１４及びロール角速度センサ２０は、初期不感帯を設けるように、上下加速度センサ１４及びロール角速度センサ２０からの各検出値に応じた各センサ信号を補正する初期不感帯補正手段２３に接続されている。
【００５０】
横速度推定手段１０、前後速度推定手段１２、及び初期不感帯補正手段２３は、ピッチ角速度Ｑを推定するピッチ角速度推定手段２４に接続されている。
【００５１】
ピッチ角速度推定手段２４は、車体の鉛直軸に対する姿勢角であるロール角φ及びピッチ角θを推定する姿勢角オブザーバ２６に接続されている。姿勢角オブザーバ２６には、更に、前後速度推定手段１２、初期不感帯補正手段２３、横加速度センサ１６、前後加速度センサ１８、及びヨー角速度センサ２２が接続されている。
【００５２】
姿勢角オブザーバ２６は、車両運動の前後加速度Ｇｘ、横加速度Ｇｙ、及びヨー角速度Ｒの各検出値に応じたセンサ信号、補正された上下加速度Ｇｚの検出値に応じたセンサ信号、補正されたロール角速度Ｐの検出値に応じたセンサ信号、前後車体速度Ｕの推定値Ｖｓｏ、及びピッチ角速度Ｑの推定値に基づいて、車体の鉛直軸に対する姿勢角であるロール角φ及びピッチ角θを推定し、ロール角φ及びピッチ角θの推定値を前回値としてピッチ角速度推定手段２４に入力するように、ピッチ角速度推定手段２４に接続されている。
【００５３】
横速度推定手段１０、前後速度推定手段１２、初期不感帯補正手段２３、ピッチ角速度推定手段２４、及び姿勢角オブザーバ２６は、各手段の機能を実現する１つまたは複数のコンピュータ、または１つまたは複数の電子回路で構成することができる。
【００５４】
次に、初期不感帯を設けるようにセンサ信号を補正する原理について説明する。
【００５５】
姿勢角は増大し続けないという自動車の性質から、姿勢角推定のベースとなるロール角速度とピッチ角速度とは、「短時間の間に必ずゼロ点に戻る」という特徴を有している。これは、ロール角速度とピッチ角速度とが「ゼロ点に戻らなければ最低、センサ信号の最小値分のドリフトがセンサ信号に印加されている」ことを意味している。本実施の形態では、このようなセンサ信号の特徴に着目し、現在までのセンサ信号値の履歴に応じた初期不感帯を、姿勢角推定の入力信号に対して設定して、センサ信号を補正することにより、推定される姿勢角のドリフト誤差による影響を軽減する。
【００５６】
初期不感帯補正手段２３は、車両運動のロール角速度の検出値に応じたセンサ信号について、ロール角速度センサ２０について予め定められたセンサドリフト最大量、及び姿勢角の推定を開始してから現時点までの履歴におけるセンサ信号の絶対値の最小値の何れか小さい方を、初期不感帯の幅として設定し、初期不感帯の幅の分だけ検出値の絶対値を小さくするように、ロール角速度センサ２０からのセンサ信号を補正する。
【００５７】
初期不感帯補正手段２３は、初期不感帯の幅の初期値として、予め求められたセンサ信号のセンサドリフト最大量（ドリフト誤差の絶対値の最大値）を設定する。例えば、センサ信号の符号が正の場合には、誤差上限値を設定し、センサ信号の符号が負の場合には、誤差下限値を設定する。また、初期不感帯補正手段２３は、姿勢角推定開始から現在までのセンサ信号の履歴におけるセンサ信号の絶対値の最小値を超えないように、初期不感帯の幅を逐次修正する。この結果、図３に示すように、センサ信号値の絶対値が、センサドリフト最大量より大きい場合には、センサドリフト最大量が初期不感帯の幅として設定される。センサ信号値の絶対値が、センサドリフト最大量より小さい場合には、センサ信号値の絶対値が、初期不感帯の幅として設定される。
【００５８】
図４に示すように、設定される初期不感帯の領域の面積が、推定される姿勢角の値の修正値に相当しており、例えば、ロール角速度センサからのセンサ信号が、真値にドリフト誤差の上限値が印加された信号である場合には、ロール角速度の検出値が負の方向に最も大きく補正される。センサ信号が真値であり、本来補正する必要のないドリフト誤差がない信号である場合にも、初期不感帯の設定によりロール角速度の検出値の補正が行われ、真値よりも小さい値が、ロール角速度の検出値として出力されるが、補正幅は、ドリフト誤差の上限値が印加された信号である場合に比べて小さくなる。
【００５９】
また、センサ信号の符号と印加されるドリフトの符号が異なり、ロール角速度センサのセンサ信号が、真値にドリフト誤差の下限値（上限値と逆符号の誤差）が印加された信号である場合には、本来増加側に補正されるべきロール角速度の検出値が減少側に補正されてしまうが、補正による減少幅は比較的小さい。
【００６０】
なお、上記のような、ドリフト誤差の下限値がセンサ信号に印加されている状況で、大きなロール角速度が出力される場合には、センサ信号の符号と印加されるドリフト誤差の符号が異なっており、図５に示すように、横転時を除き比較的早いタイミングでロール角速度のセンサ信号の符号は必ず反転する。符号の反転は、初期不感帯補償による補正が終了することを意味しており、時間積分として寄与する姿勢角推定への影響は小さい。
【００６１】
一方、ロール角速度の真値がほぼ０となる走行条件では、信号に占めるドリフト誤差の割合が大きく、長時間、ドリフト誤差によりセンサ信号が０にならない状況もある。このような場合、図６に示すように、ロール角速度の信号の符号とドリフト誤差の符号とが一致するため、設定された初期不感帯によって、長時間適切な信号の補正が行われる。
【００６２】
また、初期不感帯補正手段２３は、車両運動の上下加速度の検出値に応じたセンサ信号について、上下加速度センサ１４について予め定められたセンサドリフト最大量、及び姿勢角の推定を開始してから現時点までの履歴におけるセンサ信号の標準値（９．８ｍ／ｓ２）との差の絶対値の最小値の何れか小さい方を、初期不感帯の幅として設定し、初期不感帯の幅の分だけ、標準値との差の絶対値を小さくするように、上下加速度センサ１４からのセンサ信号を補正する。
【００６３】
次に、ピッチ角速度の推定について説明する。剛体に固定された３軸加速度及び３軸角速度を検出する３軸センサから出力されるセンサ信号と運動状態量との関係を表す剛体の運動方程式は、以下のように記述することができる。
【００６４】
【数１】
2010043945.tif 000003
【００６５】
ただし、 Ｇｘ：前後加速度、Ｇｙ：横加速度、Ｇｚ：上下加速度、Ｐ：ロール角速度、Ｑ：ピッチ角速度、Ｒ：ヨー角速度、Ｕ：前後速度、Ｖ：横速度、Ｗ：上下速度、φ：ロール角、θ：ピッチ角、g：重力加速度である。
【００６６】
本実施の形態において車両を剛体とした場合、前後加速度Ｇｘ、横加速度Ｇｙ、及びヨー角速度Ｒの各々は、前後加速度センサ１８、横加速度センサ１６、及びヨー角速度センサ２２によって検出される。また、上下加速度Ｇｚ及びロール角速度Ｐは、上下加速度センサ１４及びロール角速度センサ２０によって検出された信号を初期不感帯補正手段２３によって補正した補正信号である。横速度Ｖは、上記で説明したように横速度推定手段１０で推定することができ、前後速度Ｕは各輪の車輪速に基づいて上記で説明したように前後速度推定手段１２で推定することができる。
【００６７】
なお、本実施の形態では、図２に示すように車体の上向きをｚ軸の正方向とする右手系で座標を記述すると共に角度はオイラー角で表現している。
【００６８】
バンク走行中は、遠心力によって上下加速度Ｇｚが増加する。上下加速度Ｇｚの増加は、上記（３）式において左辺の−ＱＵが大きくなることと対応しており、この性質を利用し、（３）式において上下速度Ｗの変化を無視（Ｗ＝０とする）すると共に、ロール角及びピッチ角の各々の前回値、及び横速度の推定値、前後速度Ｕとして前後車体速度の推定値Ｖｓｏを用いることによりことにより、ピッチ角速度の推定値を以下の式から求めることができる。ピッチ角速度推定手段２４は、下記（６）式の演算を実行することによりピッチ角速度の推定値を求める。
【００６９】
【数２】
2010043945.tif 000004
【００７０】
なお、上記（６）式において、Ｐは、ロール角速度センサ２０のセンサ信号を、初期不感帯補正手段２３によって補正した補正信号を表わし、Ｇｚは、上下加速度センサ１４のセンサ信号Ｇｚを、初期不感帯補正手段２３によって補正した補正信号を表わしている。
【００７１】
ピッチ角速度が上記のように推定されると、ピッチ角もロール角と同時に姿勢角オブザーバ２６によって推定することができる。姿勢角オブザーバ２６を用いた場合の自動車固有の運動の拘束条件について説明する。上記の運動方程式を利用して、姿勢角オブザーバ２６を構成する場合、積分演算によって推定される速度及び角度の状態量が発散しないように、測定できる物理量のフィードバックが必要となる。本実施の形態では、自動車運動固有の特徴を以下のようにフィードバックする物理量に利用する。なお、以下の式では、ロール角及びピッチ角として各々の前回推定値、横速度として横車体速度の推定値、前後速度Ｕとして前後車体速度の推定値Ｖｓｏを各々用いている。
【００７２】
前後車体速度の推定値Ｖｓｏを微分して上記（１）式に代入し、自動車運動固有の特徴として、車体の前後方向に関しては「車輪のスリップは長時間増加し続けない」という性質を利用すると、上記（１）式から、上下速度の変化を無視したフィードバックに利用する以下の（７）式に示す条件が得られる。
【００７３】
【数３】
2010043945.tif 000005
【００７４】
上記（７）式は、車輪速度から推定される加速度（前後車体速度の推定値Ｖｓｏの微分値）と前後加速度Ｇｘとの差と、ピッチ角θとの関係を記述している。
【００７５】
また、車体の横方向に関しては、「スリップ角は長時間増加し続けない」という性質からロール角速度及び横加速度が無視できることから、フィードバックに利用するための（２）式から、以下の（８）式に示す条件が得られる。
【００７６】
【数４】
2010043945.tif 000006
【００７７】
さらに、一般的な道路勾配を考慮すると、「鉛直方向の加速度は、重力加速度に略一致している」とみなすことができる。この条件は前後加速度Ｇｘ、横加速度Ｇｙ、上下加速度Ｇｚ、ピッチ角θ、及びロール角φを用いて、以下の（９）式に示す代数方程式によって記述することができる。
【００７８】
【数５】
2010043945.tif 000007
【００７９】
上記（７）〜（９）式の関係は、いずれもある程度長い時間を考慮したときに満足する条件であることから、オブザーバ測定量のフィードバックには、以下で説明するように上記（７）〜（９）式の両辺をローパスフィルタ処理した値を利用する。
【００８０】
次に、基本的な非線形オブザーバの構成について説明する。ここでは、センサから出力されるセンサ信号、初期不感帯を設定するように補正された補正信号、及びピッチ角速度の推定値を含む値ｕを下記（１０）式のように表現している。
【００８１】
【数６】
2010043945.tif 000008
【００８２】
また、対象となる車両運動を下記（１１）式のように表現し、オブザーバを構成するために測定できる物理量を（１２）式のように表現すると、非線形オブザーバは、以下の（１３）式及び（１４）式の非線形運動方程式によって記述することができる。
【００８３】
【数７】
2010043945.tif 000009
【００８４】
ただし、ｘのチルダ、ｙのチルダは、それぞれｘ、ｙの推定値を表わし、ｋ（ｘのチルダ、ｕ）は設計するオブザーバゲインを表わしている。
【００８５】
上記（７）〜（９)式は、何れもある程度長い時間を考慮したときに満足する条件であることから、オブザーバの測定量のフィードバックには、下記（１５）〜（１７）式に示す（７）〜（９）式の両辺をローパスフィルタでローパスフィルタ処理した値を利用する。
【００８６】
【数８】
2010043945.tif 000010
【００８７】
ただし、τx、 τy、 τgは各々（７）〜（９）式で考慮するローパスフィルタの数秒から数十秒以上の時定数を表している。
【００８８】
次に、上記の基本的な非線形オブザーバを用いてロール角φ及びピッチ角θを推定する本実施の形態のロール角φ及びピッチ角θの姿勢角オブザーバ２６について説明する。上記（４）、（５）式の角度に関する状態方程式には、速度の状態量が含まれていないことから、速度の推定と分離して、角度としてロール角φ及びピッチ角θを推定することができる。
【００８９】
このため、まず、上記のローパスフィルタによって生じる状態量を含め、ロール角及びピッチ角を推定するためのオブザーバを構成する。本実施の形態では、オブザーバの状態量を下記（１８）式で表し、
【００９０】
【数９】
2010043945.tif 000011
フィードバックに用いるオブザーバ出力を下記（１９）式で表し、
【００９１】
【数１０】
2010043945.tif 000012
【００９２】
さらに、センサ信号等から演算される車両出力を以下の（２０）式で表わし、適切なオブザーバゲインｋ（ｘのチルダ，ｕ）を設定することによって、ロール角及びピッチ角を推定する姿勢角オブザーバ２６を構成している。
【００９３】
【数１１】
2010043945.tif 000013
ただし、
【００９４】
【数１２】
2010043945.tif 000014
である。
【００９５】
なお、（２１）式右辺第２項の分子は、前後車体速度の推定値Ｖｓ０の微分値から、３軸センサで検出されたヨー角速度値Ｒと横車体速度の推定値との積と、３軸センサで検出された前後加速度値Ｇｘとの和を減算した前後加速度状態量の偏差であり、（２２）式右辺第２項の分子は、３軸センサで検出されたヨー角速度値Ｒと前後車体速度の推定値Ｖｓ０との積から３軸センサで検出された横加速度値Ｇｙを減算した横加速度状態量の偏差である。
【００９６】
オブザーバゲインの一例として、本実施の形態では、オブザーバの安定性を確保するため、線形化を行ったときの対角成分が負の係数を持つように、下記（２３）式のように表す。
【００９７】
【数１３】
2010043945.tif 000015
【００９８】
ただし、Ｋφｙ、 Ｋφg、 Ｋθｘ、 Ｋθg、 Ｋx、 Ｋy、 Ｋgは適切な正の定数である。したがって、ロール角及びピッチ角を推定する本実施の形態の非線形オブザーバは、以下の（２４）式に示す運動方程式で記述される。
【００９９】
【数１４】
2010043945.tif 000016
ただし、ｘのチルダは、以下の（２５）式で表される。
【０１００】
【数１５】
2010043945.tif 000017
【０１０１】
姿勢角オブザーバ２６は、上記（２４）式を用いることにより、車体鉛直方向に対する姿勢角であるロール角φのチルダの微分量ｄφのチルダとピッチ角θのチルダの微分量ｄθのチルダとを算出し、算出したロール角φのチルダの微分量ｄφのチルダ及びピッチ角θのチルダの微分量ｄθのチルダの各々を積分することにより、ロール角φのチルダ及びピッチ角θのチルダを算出することができる。
【０１０２】
コンピュータを、横速度推定手段１０、前後速度推定手段１２、初期不感帯補正手段２３、ピッチ角速度推定手段２４、及び姿勢角オブザーバ２６の各手段として機能させるプログラムによる情報処理は、図７のフローチャートに示す姿勢角推定処理ルーチンで実現することができる。コンピュータは、相互にバスによって接続されたＣＰＵ，ＲＯＭ，及びＲＡＭ，並びに必要に応じて接続されたＨＤＤで構成され、これらのプログラムは、コンピュータのＣＰＵに接続されているＲＯＭまたはＨＤＤ等の記録媒体に記録される。
【０１０３】
この姿勢角推定処理ルーチンを説明すると、ステップ１００で、上下加速度センサ１４、横加速度センサ１６、前後加速度センサ１８、ロール角速度センサ２０、及びヨー角速度センサ２２から、各検出値に応じたセンサ信号を取得する。このとき、上下加速度センサ１４からのセンサ信号の検出値を、上下加速度センサ１４からのセンサ信号の検出値の履歴として記録すると共に、ロール角速度センサ２０からのセンサ信号の検出値を、ロール角速度センサ２０からのセンサ信号の検出値の履歴として記録する。
【０１０４】
そして、ステップ１０２において、上記ステップ１００で取得した上下加速度センサ１４の検出値に応じたセンサ信号について、姿勢角推定を開始してから得られた上下加速度センサ１４の検出値の履歴のうち、検出値と標準値との差の絶対値が最小となるセンサ信号であるか否かを判定する。上記ステップ１００で取得した上下加速度センサ１４の検出値に応じたセンサ信号について、検出値と標準値との差の絶対値が最小でない場合には、ステップ１０６へ移行する。一方、上記ステップ１０２において、上記ステップ１００で取得した上下加速度センサ１４の検出値に応じたセンサ信号について、検出値と標準値との差の絶対値が最小となる場合には、ステップ１０４において、標準値との差の絶対値が最小になるセンサ信号の検出値として、上記ステップ１００で取得した上下加速度センサ１４の検出値を記憶する。
【０１０５】
次のステップ１０８では、上記ステップ１００で取得したロール角速度センサ２０の検出値に応じたセンサ信号について、姿勢角推定を開始してから得られたロール角速度センサ２０の検出値の履歴のうち、検出値の絶対値が最小となるセンサ信号であるか否かを判定する。上記ステップ１００で取得したロール角速度センサ２０の検出値に応じたセンサ信号について、検出値の絶対値が最小でない場合には、ステップ１１０へ移行する。一方、上記ステップ１０６において、上記ステップ１００で取得したロール角速度センサ２０の検出値に応じたセンサ信号について、検出値の絶対値が最小になる場合には、ステップ１０８において、検出値の絶対値が最小になるセンサ信号の検出値として、上記ステップ１００で取得したロール角速度センサ２０の検出値を記憶する。
【０１０６】
そして、ステップ１１０では、上記ステップ１００で取得した上下加速度センサ１４の検出値に応じたセンサ信号について、予め定められた上下加速度センサ１４のセンサドリフト最大量、及び姿勢角の推定を開始してから現時点までの履歴における標準値との差の絶対値の最小値の何れか小さい方の分だけ、検出値と標準値との差の絶対値を小さくするようにセンサ信号を補正する。また、上記ステップ１００で取得したロール角速度センサ２０の検出値に応じたセンサ信号について、予め定められたロール角速度センサ２０のセンサドリフト最大量、及び姿勢角の推定を開始してから現時点までの履歴における検出値の絶対値の最小値の何れか小さい方の分だけ、検出値の絶対値を小さくするようにセンサ信号を補正する。
【０１０７】
次のステップ１１２では、上記で説明したように横車体速度を推定し、ステップ１１４において各輪の車輪速度に基づいて前後車体速度を推定する。
【０１０８】
ステップ１１６において、最初の演算か否かを判断し、最初の演算の場合には、ロール角及びピッチ角の前回値が存在しないので、ステップ１１８でロール角及びピッチ角の前回値として予め定められた初期値を設定し、ステップ１２２において、上記で説明したように上記（６）式を用いてピッチ角速度Ｑの推定値を演算する。
【０１０９】
一方、２回目以降の演算の場合には、後述するステップ１２６でロール角及びピッチ角の推定値が演算されるので、ステップ１２０においてロール角及びピッチ角の値として前回演算された前回値を設定し、ステップ１２２において、上記で説明したように上記（６）式を用いてピッチ角速度Ｑの推定値を演算する。
【０１１０】
ステップ１２４において、上記で説明したように、車両運動の前後加速度Ｇｘ、横加速度Ｇｙ、及びヨー角速度Ｒの検出値に応じたセンサ信号、前後車体速度Ｕの推定値Ｖｓｏに基づいて、上記（２１）、（２２）式で記述されたローパスフィルタ処理を行う。
【０１１１】
次のステップ１２６では、ステップ１２２で演算されたピッチ角速度Ｑの推定値、ステップ１２４で演算されたローパスフィルタ処理値を用い、上記（２４）式にしたがって車体の鉛直軸に対する姿勢角であるロール角φ及びピッチ角θを推定し、ロール角φ及びピッチ角θの推定値をコンピュータのメモリに記憶して、上記ステップ１００へ戻る。
【０１１２】
なお、上記の実施の形態では、ロール角速度センサ及び上下加速度センサからの各センサ信号に対して、初期不感帯を設定するように補正する場合を例に説明したが、これに限定されるものではなく、ロール角速度センサ及び上下加速度センサの何れか一方のセンサ信号に対してのみ、初期不感帯を設定するように補正してもよい。
【０１１３】
次に、本発明の第２の実施の形態について説明する。なお、第１の実施の形態と同様の構成となる部分については、同一符号を付して説明を省略する。
【０１１４】
第２の実施の形態では、各種センサに加えて、ピッチ角速度センサを用いて、姿勢角を推定している点が第１の実施の形態と異なっている。
【０１１５】
図８に示すように、第２の実施の形態の姿勢角推定装置には、上下加速度センサ１４、横加速度センサ１６、前後加速度センサ１８、ロール角速度センサ２０、ヨー角速度センサ２２、及びピッチ角速度を検出するピッチ角速度センサ２２０が設けられている。
【０１１６】
上下加速度センサ１４、ロール角速度センサ２０、及びピッチ角速度センサ２２０は、初期不感帯を設けるように、上下加速度センサ１４、ロール角速度センサ２０、及びピッチ角速度センサ２２０からの各検出値に応じた各センサ信号を補正する初期不感帯補正手段２２３に接続されている。
【０１１７】
姿勢角オブザーバ２６には、横速度推定手段１０、前後速度推定手段１２、初期不感帯補正手段２２３、横加速度センサ１６、前後加速度センサ１８、及びヨー角速度センサ２２が接続されている。
【０１１８】
初期不感帯補正手段２２３は、車両運動のロール角速度の検出値に応じたセンサ信号について、第１の実施の形態と同様に、初期不感帯を設定し、初期不感帯の幅の分だけ、絶対値を小さくするように、ロール角速度センサ２０からのセンサ信号を補正する。また、初期不感帯補正手段２２３は、車両運動の上下加速度の検出値に応じたセンサ信号について、第１の実施の形態と同様に、初期不感帯を設定し、初期不感帯の幅の分だけ、標準値との差の絶対値を小さくするように、上下加速度センサ１４からのセンサ信号を補正する。
【０１１９】
また、初期不感帯補正手段２２３は、車両運動のピッチ角速度の検出値に応じたセンサ信号について、ピッチ角速度センサ２２０について予め定められたセンサドリフト最大量、及び姿勢角の推定を開始してから現時点までにおけるセンサ信号の絶対値の最小値の何れか小さい方を、初期不感帯の幅として設定し、初期不感帯の幅の分だけ、絶対値を小さくするように、ピッチ角速度センサ２２０からのセンサ信号を補正する。
【０１２０】
姿勢角オブザーバ２６は、上記の第１の実施の形態で説明した（２４）式を以下の（２６）式に置き換えて、前後加速度センサ１８、横加速度センサ１６、及びヨー角速度センサ２２によって検出される前後加速度Ｇｘ、横加速度Ｇｙ、及びヨー角速度Ｒと、初期不感帯補正手段２３によって補正された上下加速度Ｇｚ、ロール角速度Ｐ、ピッチ角速度Ｑと、横速度推定手段１０で推定された横速度Ｖのチルダと、前後速度推定手段１２で推定された前後速度Ｖｓ０とを用いて、車体鉛直方向に対する姿勢角であるロール角の微分量ｄφのチルダとピッチ角の微分量ｄθのチルダとを算出する。また、姿勢角オブザーバ２６は、算出したロール角の微分量ｄφのチルダ及びピッチ角の微分量ｄθのチルダの各々を積分することにより、ロール角φのチルダ及びピッチ角θのチルダを算出する。
【０１２１】
【数１６】
2010043945.tif 000018
【０１２２】
なお、上記の実施の形態では、ロール角速度センサ、ピッチ角速度センサ、及び上下加速度センサからの各センサ信号に対して、初期不感帯を設定するように補正する場合を例に説明したが、これに限定されるものではなく、ピッチ角速度センサからのセンサ信号に対してのみ、初期不感帯を設定するように補正してもよい。また、ロール角速度センサ、ピッチ角速度センサ、及び上下加速度センサのうちの任意の２つのセンサの組み合わせからの各センサ信号に対して、初期不感帯を設定するように補正してもよい。例えば、ロール角速度センサ及びピッチ角速度センサからの各センサ信号に対して、初期不感帯を設定するように補正してもよく、また、ピッチ角速度センサ及び上下加速度センサからの各センサ信号に対して、初期不感帯を設定するように補正してもよい。
【０１２３】
次に、本発明の第３の実施の形態について説明する。なお、第１の実施の形態と同様の構成となる部分については、同一符号を付して説明を省略する。第３の実施の形態では、
第３の実施の形態では、センサドリフト量を補正した後に、初期不感帯を設定するように補正している点が、第１の実施の形態と主に異なっている。
【０１２４】
図９に示すように、第３の実施の形態に係る姿勢角推定装置３１０は、上下加速度センサ１４、横加速度センサ１６、前後加速度センサ１８、ロール角速度センサ２０、及びヨー角速度センサ２２が設けられている。
【０１２５】
上下加速度センサ１４、及びロール角速度センサ２０は、後述するドリフト量推定手段３２８によって推定されたセンサドリフト量に基づいて、各センサからのセンサ信号を補正するドリフト量補正手段３２２に接続されている。ドリフト量補正手段３２２は、ドリフト量推定手段３２８に接続されている。
【０１２６】
ドリフト量補正手段３２２は、初期不感帯補正手段２３に接続されている。初期不感帯補正手段２３は、車体の鉛直軸に対する姿勢角であるロール角φ及びピッチ角θを推定する姿勢角オブザーバ３２４に接続されている。
【０１２７】
上下加速度センサ１４、ロール角速度センサ２０、ヨー角速度センサ２２、及び姿勢角オブザーバ３２４は、車両運動の運動方程式により得られるロール角及びピッチ角の各々の微分量を算出する運動方程式微分量算出手段３２６に接続されている。
【０１２８】
姿勢角オブザーバ３２４及び運動方程式微分量算出手段３２６は、上下加速度センサ１４及びロール角速度センサ２０の各々のセンサドリフト量を推定するドリフト量推定手段３２８に接続されている。
【０１２９】
ドリフト量補正手段３２２、初期不感帯補正手段２３、姿勢角オブザーバ３２４、運動方程式微分量算出手段３２６、及びドリフト量推定手段３２８は、各手段の機能を実現する１つまたは複数のコンピュータ、または１つまたは複数の電子回路で構成することができる。
【０１３０】
姿勢角オブザーバ３２４は、車両横方向の車両速度である横車体速度Ｖを推定すると共に、各輪の車輪速度に基づいて車両前後方向の車両速度である前後車体速度Ｕを推定する。また、姿勢角オブザーバ３２４は、横車体速度Ｖの推定値、前後車体速度Ｕの推定値、ドリフト量補正手段３２２で補正された後に初期不感帯補正手段２３によって補正された上下加速度Ｇｚの検出値に応じた補正信号、及びドリフト量補正手段３２２で補正された後に初期不感帯補正手段２３によって補正されたロール角速度Ｐの検出値に応じた補正信号に基づいて、ピッチ角速度Ｑを推定する。なお、横車体速度Ｖは、カルマンフィルタ等を用いて推定したり、横加速度センサの検出値から推定したりすることができる。
【０１３１】
姿勢角オブザーバ３２４は、ドリフト量補正手段３２２で補正された後に初期不感帯補正手段２３によって補正された、車両運動の上下加速度Ｇｚ及びロール角速度Ｐの各検出値に応じた補正信号と、車両運動の前後加速度Ｇｘ、横加速度Ｇｙ、及びヨー角速度Ｒの各検出値に応じたセンサ信号と、前後車体速度Ｕの推定値Ｖｓｏと、ピッチ角速度Ｑの推定値とに基づいて、車体の鉛直軸に対する姿勢角であるロール角φ及びピッチ角θを推定する。
【０１３２】
姿勢角オブザーバ３２４は、上記第１の実施の形態で説明した（６）式を以下の（２７）式に置き換えて、ピッチ角速度の推定値Ｑのチルダを求める。
【０１３３】
【数１７】
2010043945.tif 000019
【０１３４】
なお、上記（２７）式において、Ｐ−Ｐｄｒは、ロール角速度センサ２０のセンサ信号Ｐのドリフト量Ｐｄｒを補正した後に初期不感帯を設定するように補正した補正信号を表わし、Ｇｚ−Ｇｚｄｒは、上下加速度センサ１４のセンサ信号Ｇｚのドリフト量Ｇｚｄｒを補正した後に初期不感帯を設定するように補正した補正信号を表わしている。
【０１３５】
ピッチ角速度が上記のように推定されると、ピッチ角もロール角と同時に姿勢角オブザーバ３２４によって推定することができる。本実施の形態では、姿勢角オブザーバ３２４を用いた場合の自動車固有の運動の拘束条件について、一般的な道路勾配を考慮した、上記（９）式に示す条件を、以下の（２８）式に示す条件に置き換える。
【０１３６】
【数１８】
2010043945.tif 000020
【０１３７】
次に、基本的な非線形オブザーバの構成について説明する。ここでは、上記（１０）式を、以下の（２９）式に置き換えて、ドリフト量を補正された後に初期不感帯を設定するように補正された補正信号と、センサ信号と、ピッチ角速度の推定値とを含む値ｕを表現している。
【０１３８】
【数１９】
2010043945.tif 000021
【０１３９】
オブザーバの測定量のフィードバックに利用するローパスフィルタ処理を示す、上記（１５）〜（１７）式を、以下の（３０）式〜（３２）式に置き換えて用いている。
【０１４０】
【数２０】
2010043945.tif 000022
【０１４１】
次に、上記の基本的な非線形オブザーバを用いてロール角φ及びピッチ角θを推定する本実施の形態の姿勢角オブザーバ３２４について説明する。
【０１４２】
また、ロール角及びピッチ角を推定する本実施の形態の非線形オブザーバは、以下の（３３）式で表される運動方程式で記述される。
【０１４３】
【数２１】
2010043945.tif 000023
ただし、ｘのチルダは、以下の（３４）式で表される。
【０１４４】
【数２２】
2010043945.tif 000024
【０１４５】
姿勢角オブザーバ３２４は、上記（３３）式を用いることにより、車体鉛直方向に対する姿勢角であるロール角の微分量ｄφのチルダとピッチ角の微分量ｄθのチルダとを算出し、算出したロール角の微分量ｄφのチルダ及びピッチ角の微分量ｄθのチルダの各々を積分することにより、ロール角φのチルダ及びピッチ角θのチルダを算出することができる。
【０１４６】
次に、車両運動の運動方程式により得られるロール角φの微分量ｄφｍ及びピッチ角θの微分量ｄθｍの各々の算出方法について説明する。
【０１４７】
上記（１）式〜（５）式に対して、上記（６）式を代入し、上下車体速度を０と仮定すると共に、センサ信号Ｇｘ、Ｇｙ、Ｇｚ、Ｐ、Ｒに、所定のドリフト誤差Ｇｘｄｒ、Ｇｙｄｒ、Ｇｚｄｒ、Ｐｄｒ、Ｒｄｒが重畳していると仮定すると、以下の（３５）式〜（３８）式が得られる。
【０１４８】
【数２３】
2010043945.tif 000025
【０１４９】
ただし、ロール角φ及びピッチ角θには、姿勢角オブザーバ３２４によって推定されたロール角φ及びピッチ角θの推定値を用いる。また、上下加速度Ｇｚ−Ｇｚｄｒ、及びロール角速度Ｐ−Ｐｄｒの各々は、上下加速度センサ１４及びロール角速度センサ２０によって検出されたセンサ信号からセンサドリフト量を除去するように補正した補正信号である。また、前後加速度Ｇｘ−Ｇｘｄｒ、横加速度Ｇｙ−Ｇｙｄｒ、及びヨー角速度Ｒ−Ｒｄｒの各々は、前後加速度センサ１８、横加速度センサ１６、及びヨー角速度センサ２２の各々によって検出された各センサ信号に対してセンサドリフト量を除去するように補正した補正信号である。
【０１５０】
また、横車体速度Ｖは、カルマンフィルタ等を用いて推定したり、横加速度センサの検出値から推定したりすることができ、前後車体速度Ｕは各輪の車輪速に基づいて推定することができる。
【０１５１】
車両運動の運動方程式により得られるロール角φの微分量ｄφｍおよびピッチ角θの微分量ｄθｍは、上記（３７）式、（３８）式のドリフト量を無視した以下の（３９）式、（４０）式で算出される。
【０１５２】
【数２４】
2010043945.tif 000026
【０１５３】
また、上記（３７）式のφのドット、上記（３８）式のθのドットと、上記（３９）式のｄφｍ、上記（４０）式のｄθｍとの間には、以下の（４１）式、（４２）式で表される関係が存在する。
【０１５４】
【数２５】
2010043945.tif 000027
【０１５５】
運動方程式微分量算出手段３２６は、上記（３９）式、（４０）式に従って、車両運動の運動方程式より得られるロール角φ及びピッチ角θの微分量を算出する。
【０１５６】
本実施の形態では、ドリフト量推定手段３２８によって、姿勢角オブザーバ３２４で算出された姿勢角の微分量と運動方程式微分量算出手段３２６で算出された姿勢角の微分量との比較から、センサドリフト量を推定する。
【０１５７】
次に、センサドリフト量の推定方法について説明する。
【０１５８】
センサ信号Ｇｘ、Ｇｙ、Ｇｚ、Ｐ、Ｒに、所定のドリフト誤差Ｇｘｄｒ、Ｇｙｄｒ、Ｇｚｄｒ、Ｐｄｒ、Ｒｄｒが重畳していると仮定するとともに、上記（４１）式、（４２）式の右辺が表わす微分量について、姿勢角オブザーバ３２４によって真値が既知であると仮定すると、以下の（３９）式、（４０）式で表される関係が成立する。
【０１５９】
【数２６】
2010043945.tif 000028
【０１６０】
ただし、ｄφ、ｄθは、上記（３３）式により得られるオブザーバ内部演算値としてのロール角の微分量、ピッチ角の微分量である。
【０１６１】
上記（４３）式、（４４）式は、センサ信号のセンサドリフト量を考慮したときに、姿勢角オブザーバ３２４により算出されたロール角及びピッチ角の微分量と、運動方程式により得られるロール角及びピッチ角の微分量にセンサドリフト量を考慮した値とが等しくなる関係を表わしている。
【０１６２】
ここで、上記（４３）式、（４４）式にＧｘ、Ｇｙが含まれておらず、かつ、Ｒの係数も比較的小さい（姿勢角が小さいことを仮定）ことから、上下加速度センサ１４及びロール角速度センサ２０によって検出されたセンサ信号がドリフト量補正手段３２２において補正された後に初期不感帯補正手段２３によって補正された補正信号を用いて、上下加速度Ｇｚ及びロール角速度Ｐに関するセンサドリフト量の推定を行うことが可能である。
【０１６３】
ところで、横加速度に関しては、「スリップ角は長時間、非線形領域に留まらない」という条件から、以下の（４５）式で表される関係を利用することができる。
【０１６４】
【数２７】
2010043945.tif 000029
【０１６５】
ただし、τｙは、長時間の運動のみを考慮するためのフィルタ時定数であり、ｃｆ、ｃｒは、前後輪のコーナリングパワーであり、ｌｆ、ｌｒは、前後軸−重心間距離である。また、ｌはホイールベースであり、ｍは車両質量であり、δｆは前輪実舵角である。
【０１６６】
また、上下加速度に関して、「鉛直方向の加速度は重力加速度」という条件から、以下の（４６）式で表される関係を利用することができる。
【０１６７】
【数２８】
2010043945.tif 000030
【０１６８】
ただし、Ｄｇｆ、Ｅｇｆは、以下の式で表される。また、τｆは、長時間の運動のみを考慮するためのフィルタ時定数である。
【０１６９】
【数２９】
2010043945.tif 000031
【０１７０】
ここでは、上記（４３）式、（４４）式、（４６）式から、以下の（４７）式で記述することができる。
【０１７１】
【数３０】
2010043945.tif 000032
ただし、ｄＤ１、ｄＥ１、ｄＥ２、ｄＥ３は、以下の（４８）式〜（５１）式で表される。
【０１７２】
【数３１】
2010043945.tif 000033
【０１７３】
そして、上記（４７）式の左辺の係数行列と右辺のベクトルとを一定時間積分すると、以下の（５２）式が得られる。
【０１７４】
【数３２】
2010043945.tif 000034
ただし、Ｄ１、Ｅ１、Ｅ２、Ｅ３は、以下の（５３）式〜（５６）式で表される。
【０１７５】
【数３３】
2010043945.tif 000035
【０１７６】
上記（５２）式を解くことによって、以下の（５７）式を導出することができる。
【０１７７】
【数３４】
2010043945.tif 000036
ただし、Ｄ＋は、Ｄの擬似逆行列である。
【０１７８】
ドリフト量推定手段３２８は、上記（５７）式に従って、姿勢角オブザーバ３２４で算出されるロール角及びピッチ角の各々の微分量と、運動方程式微分量算出手段３２６で算出されたロール角及びピッチ角の各々の微分量とに基づいて、上下加速度センサ１４のセンサドリフト量及びロール角速度センサ２０のセンサドリフト量を推定することができる。
【０１７９】
また、本実施の形態に係るドリフト量推定手段３２８では、演算の安定化を図るため、前回の推定値を用いて、以下の（５８）式、（５９）式に従って、上記（５７）式の演算結果により得られるセンサドリフト量の推定値を平滑化する。
【０１８０】
【数３５】
2010043945.tif 000037
【０１８１】
ただし、Ｇｚｄｒのチルダ、Ｐｄｒのチルダは、平滑化後のセンサドリフト量の推定値であり、λ１は、忘却係数である。
【０１８２】
初期不感帯補正手段２３は、ドリフト量補正手段３２２によって補正されたロール角速度の検出値に応じたセンサ信号について、ロール角速度センサ２０について予め定められたセンサドリフト最大量、及び姿勢角の推定を開始してから現時点までの履歴における、ドリフト量補正手段３２２により補正されたセンサ信号の絶対値の最小値の何れか小さい方を、初期不感帯の幅として設定し、初期不感帯の幅の分だけ、絶対値を小さくするように、ドリフト量補正手段３２２によって補正されたロール角速度センサ２０からのセンサ信号を補正する。
【０１８３】
また、初期不感帯補正手段２３は、ドリフト量補正手段３２２によって補正された上下加速度の検出値に応じたセンサ信号について、上下加速度センサ１４について予め定められたセンサドリフト最大量、及び姿勢角の推定を開始してから現時点までの履歴における、ドリフト量補正手段３２２により補正されたセンサ信号の標準値との差の絶対値の最小値の何れか小さい方を、初期不感帯の幅として設定し、初期不感帯の幅の分だけ、標準値との差の絶対値を小さくするように、ドリフト量補正手段３２２によって補正された上下加速度センサ１４からのセンサ信号を補正する。
【０１８４】
次に、本実施の形態の初期不感帯を設定するようにセンサ信号を補正する方法による推定結果について説明する。なお、センサ信号を補正する方法の効果を確認するため、０．３Ｇ程度の定常円旋回データにドリフト誤差を印加して姿勢角の推定を行った。また、ロール角速度センサ２０と上下加速度センサ１４とのドリフト誤差推定のみを行うとともに、両センサからの信号に対する初期不感帯補償による補正を行う場合と、初期不感帯補償による補正を行わない場合とについて、姿勢角の推定を行った。
【０１８５】
まず、ロール角速度センサからのセンサ信号に３ｄｅｇ／ｓのドリフト誤差を印加した状態で、姿勢角の推定を行った。図１０（Ａ）、（Ｂ）に示すように、初期不感帯補償による補正を行わない場合には、ロール角の推定値は振動的になっているとともに、ピッチ角の推定値は、真値（ドリフトを印加しないときの推定値）から大きくずれてしまっている。これに対し、初期不感帯補償による補正を行った場合には、真値に近いロール角の推定値が演算され、また、真値に近いピッチ角の推定値が演算されている。
【０１８６】
また、図１１（Ｂ）に示すように、ロール角速度センサからのセンサ信号に３ｄｅｇ／ｓのドリフト誤差を印加した場合には、ロール角速度のセンサドリフト量の推定の遅れを補償するように、初期不感帯補償による補正が行われていることがわかる。
【０１８７】
また、ロール角速度センサからのセンサ信号に−３ｄｅｇ／ｓのドリフト誤差を印加した状態で、姿勢角の推定を行った。図１２（Ａ）、（Ｂ）に示すように、初期不感帯補償による補正を行わない場合には、ロール角の推定値及びピッチ角の推定値の各々は、真値（ドリフトを印加しないときの推定値）から大きくずれてしまっている。
【０１８８】
また、図１３（Ｂ）に示すように、ロール角速度センサからのセンサ信号に−３ｄｅｇ／ｓのドリフト誤差を印加した場合には、ロール角速度のセンサドリフト量の推定の遅れを補償するように、初期不感帯補償による補正が行われていることがわかる。
【０１８９】
上記の第３の実施の形態において、コンピュータを、ドリフト量補正手段３２２、初期不感帯補正手段２３、姿勢角オブザーバ３２４、運動方程式微分量算出手段３２６、及びドリフト量推定手段３２８の各手段として機能させるプログラムによる情報処理は、図１４のフローチャートに示す姿勢角推定処理ルーチンで実現することができる。コンピュータは、相互にバスによって接続されたＣＰＵ、ＲＯＭ、及びＲＡＭ、並びに必要に応じて接続されたＨＤＤで構成され、これらのプログラムは、コンピュータのＣＰＵに接続されているＲＯＭまたはＨＤＤ等の記録媒体に記録される。
【０１９０】
以下に、この姿勢角推定処理ルーチンについて説明する。なお、第１の実施の形態と同様の処理については、同一符号を付して詳細な説明を省略する。
【０１９１】
まず、ステップ１００で、前後加速度センサ１８、横加速度センサ１６、上下加速度センサ１４、ロール角速度センサ２０、及びヨー角速度センサ２２から、各検出値に応じたセンサ信号を取得する。
【０１９２】
そして、ステップ３５０において、後述のステップ３６６で１演算周期前に得られた上下加速度センサ１４のセンサドリフト量及びロール角速度センサ２０のセンサドリフト量を用いて、上記ステップ１００で取得された上下加速度センサ１４及びロール角速度センサ２０から出力されたセンサ信号を補正する。
【０１９３】
そして、ステップ３５２において、上記ステップ３５０で補正した上下加速度センサ１４の検出値に応じたセンサ信号について、姿勢角推定を開始してから得られた、補正された上下加速度センサ１４の検出値の履歴のうち、検出値と標準値との差の絶対値が最小になるか否かを判定する。上記ステップ３５０で補正した上下加速度センサ１４の検出値に応じたセンサ信号について、検出値と標準値との差の絶対値が最小にならない場合には、ステップ３５６へ移行する。一方、上記ステップ３５２において、上記ステップ３５０で補正した上下加速度センサ１４の検出値に応じたセンサ信号について、検出値と標準値との差の絶対値が最小になる場合には、ステップ３５４において、標準値との差の絶対値の最小値として、上記ステップ３５０で補正した上下加速度センサ１４の検出値に応じたセンサ信号の検出値を記憶する。
【０１９４】
次のステップ３５６では、上記ステップ３５０で補正したロール角速度センサ２０の検出値に応じたセンサ信号について、姿勢角推定を開始してから得られた、補正されたロール角速度センサ２０の検出値の履歴のうち、検出値の絶対値が最小になるか否かを判定する。上記ステップ３５０で補正したロール角速度センサ２０の検出値に応じたセンサ信号について、検出値の絶対値が最小にならない場合には、ステップ３６０へ移行する。一方、上記ステップ３５６において、上記ステップ３５０で補正したロール角速度センサ２０の検出値に応じたセンサ信号について、検出値の絶対値が最小になる場合には、ステップ３５８において、検出値の絶対値の最小値として、上記ステップ３５０で補正したロール角速度センサ２０からのセンサ信号の検出値を記憶する。
【０１９５】
そして、ステップ３６０では、上記ステップ３５０で補正した上下加速度センサ１４の検出値に応じたセンサ信号について、予め定められたセンサドリフト最大量、及び姿勢角の推定を開始してから現時点までの履歴における標準値との差の絶対値の最小値の何れか小さい方の分だけ、検出値と標準値との差の絶対値を小さくするようにセンサ信号をさらに補正する。また、上記ステップ３５０で補正したロール角速度センサ２０の検出値に応じたセンサ信号について、予め定められたセンサドリフト最大量、及び姿勢角の推定を開始してから現時点までの履歴における検出値の絶対値の最値の何れか小さい方の分だけ、検出値の絶対値を小さくするようにセンサ信号をさらに補正する。
【０１９６】
そして、ステップ３６２で、後述のステップ３６８で１演算周期前に得られた姿勢角推定値と、カルマンフィルタ等を用いて推定される横車体速度と、各輪の車輪速度に基づいて推定される前後車体速度とを用いて、上記で説明したように、姿勢角オブザーバ３２４によってロール角及びピッチ角を推定するために算出されるロール角及びピッチ角の各々の微分量を算出する。
【０１９７】
そして、ステップ３６４において、後述のステップ３６８で１演算周期前に得られた姿勢角推定値と、推定された横車体速度及び前後車体速度とを用いて、上記で説明したように、上記ステップ３６２で算出されたロール角及びピッチ角の各々の微分量と、車両運動の運動方程式から得られるロール角及びピッチ角の各々の微分量にセンサドリフト量を考慮した値とが等しくなる関係を用いて、上下加速度センサ１４のセンサドリフト量及びロール角速度センサ２０のセンサドリフト量を推定する。
【０１９８】
次のステップ３６６では、上記ステップ３６４で推定された上下加速度センサ１４のセンサドリフト量及びロール角速度センサ２０のセンサドリフト量を、前回のセンサドリフト量の推定値を用いて平滑化する。
【０１９９】
次のステップ３６８では、上記ステップ３６２で算出されたロール角及びピッチ角の各々の微分量を積分することで、ロール角及びピッチ角を推定して出力し、上記ステップ１００へ戻る。
【０２００】
次に、本発明の第４の実施の形態について説明する。なお、第１の実施の形態〜第３の実施の形態と同様の構成となる部分については、同一符号を付して説明を省略する。
【０２０１】
第４の実施の形態では、各種センサに加えて、ピッチ角速度センサを用いて、姿勢角を推定している点と、ピッチ角速度センサのセンサドリフト量を推定している点と、ピッチ角速度センサのセンサ信号に対して初期不感帯を設定するように補正している点とが、第３の実施の形態と主に異なっている。
【０２０２】
図１５に示すように、第４の実施の形態の姿勢角推定装置４１０には、上下加速度センサ１４、横加速度センサ１６、前後加速度センサ１８、ロール角速度センサ２０、ヨー角速度センサ２２、及びピッチ角速度センサ２２０が設けられている。
【０２０３】
ピッチ角速度センサ２２０及びロール角速度センサ２０は、後述するドリフト量推定手段４２８によって推定されたセンサドリフト量に基づいて、各センサからのセンサ信号を補正するドリフト量補正手段４２２に接続されている。ドリフト量補正手段４２２は、ドリフト量推定手段４２８に接続されている。
【０２０４】
ドリフト量補正手段４２２は、初期不感帯を設けるように、ドリフト量補正手段４２２によって補正された、ピッチ角速度センサ２２０及びロール角速度センサ２０からの各検出値に応じた各センサ信号を補正する初期不感帯補正手段４２３に接続されている。初期不感帯補正手段４２３は、車体の鉛直軸に対する姿勢角であるロール角φ及びピッチ角θを推定する姿勢角オブザーバ４２４に接続されている。
【０２０５】
ピッチ角速度センサ２２０、ロール角速度センサ２０、ヨー角速度センサ２２、及び姿勢角オブザーバ３２４は、車両運動の運動方程式により得られるロール角及びピッチ角の各々の微分量を算出する運動方程式微分量算出手段４２６に接続されている。
【０２０６】
姿勢角オブザーバ４２４には、初期不感帯補正手段４２３、上下加速度センサ１４、横加速度センサ１６、前後加速度センサ１８、及びヨー角速度センサ２２が接続されている。
【０２０７】
初期不感帯補正手段４２３は、ドリフト量補正手段４２２によって補正されたロール角速度の検出値に応じたセンサ信号について、ロール角速度センサ２０について予め定められたセンサドリフト最大量、及び姿勢角の推定を開始してから現時点までの履歴における、ドリフト量補正手段４２２により補正されたセンサ信号の絶対値の最小値の何れか小さい方を、初期不感帯の幅として設定し、初期不感帯の幅の分だけ、絶対値を小さくするように、ドリフト量補正手段４２２によって補正されたロール角速度センサ２０からのセンサ信号を補正する。
【０２０８】
また、初期不感帯補正手段４２３は、ドリフト量補正手段４２２によって補正されたピッチ角速度の検出値に応じたセンサ信号について、ピッチ角速度センサ２２０について予め定められたセンサドリフト最大量、及び姿勢角の推定を開始してから現時点までの履歴における、ドリフト量補正手段４２２により補正されたセンサ信号の絶対値の最小値の何れか小さい方を、初期不感帯の幅として設定し、初期不感帯の幅の分だけ、絶対値を小さくするように、ドリフト量補正手段４２２によって補正されたピッチ角速度センサ２２０からのセンサ信号を補正する。
【０２０９】
姿勢角オブザーバ４２４は、車両横方向の車両速度である横車体速度Ｖを推定すると共に、各輪の車輪速度に基づいて車両前後方向の車両速度である前後車体速度Ｕを推定する。
【０２１０】
また、姿勢角オブザーバ４２４は、上記の第３の実施の形態で説明した（３３）式を以下の（６０）式に置き換えて、ドリフト量補正手段３２２で補正された後に初期不感帯補正手段４２３によって補正された、車両運動のピッチ角速度Ｑ及びロール角速度Ｐの各検出値に応じた補正信号Ｑ−Ｑｄｒ、Ｐ−Ｐｄｒと、車両運動の前後加速度Ｇｘ、横加速度Ｇｙ、上下加速度Ｇｚ、及びヨー角速度Ｒの各検出値に応じたセンサ信号と、前後車体速度Ｕの推定値Ｖｓｏとを用いて、車体鉛直方向に対する姿勢角であるロール角の微分量ｄφのチルダとピッチ角の微分量ｄθのチルダとを算出する。また、姿勢角オブザーバ４２４は、算出したロール角の微分量ｄφのチルダ及びピッチ角の微分量ｄθのチルダの各々を積分することにより、ロール角φのチルダ及びピッチ角θのチルダを算出することができる。
【０２１１】
【数３６】
2010043945.tif 000038
【０２１２】
運動方程式微分量算出手段４２６は、上記（３９）式、（４０）式を、以下の（６１）式、（６２）式に置き換えて、車両運動の運動方程式より得られるロール角φ及びピッチ角θの微分量を算出する。
【０２１３】
【数３７】
2010043945.tif 000039
【０２１４】
ただし、ロール角φ及びピッチ角θには、姿勢角オブザーバ４２４によって推定されたロール角φ及びピッチ角θの推定値を用いる。
【０２１５】
ドリフト量推定手段４２８は、姿勢角オブザーバ４２４で算出された姿勢角の微分量と運動方程式微分量算出手段４２６で算出された姿勢角の微分量との比較から、センサドリフト量を推定する。
【０２１６】
次に、センサドリフト量の推定方法について説明する。
【０２１７】
上記（４）式、（５）式において、センサ信号Ｇｘ，Ｇｙ，Ｐ，Ｑ，Ｒに所定のドリフト誤差Ｇｘｄｒ，Ｇｙｄｒ，Ｐｄｒ，Ｑｄｒ，Ｒｄｒが重畳していると仮定すると、以下の（６３）式，（６４）式が得られる。
【０２１８】
【数３８】
2010043945.tif 000040
【０２１９】
また、上記（６３）式、（６４）式が表す微分量について、姿勢角オブザーバ４２４によって真値が既知であると仮定すると、以下の（６５）式、（６６）式で表される関係が成立する。
【０２２０】
【数３９】
2010043945.tif 000041
【０２２１】
ただし、ｄφ、ｄθは、上記（６０）式により得られるオブザーバ内部演算値としてのロール角の微分量、ピッチ角の微分量である。
【０２２２】
上記（６５）式、（６６）式は、ロール角速度センサ２０及びピッチ角速度センサ２２０のセンサ信号のセンサドリフト量を考慮したときに、姿勢角オブザーバ４２４により算出されたロール角及びピッチ角の微分量と、運動方程式により得られるロール角及びピッチ角の微分量にセンサドリフト量を考慮した値とが等しくなる関係を表わしている。
【０２２３】
ここでは、上記（６５）式、（６６）式におけるヨー角速度ドリフトの係数が比較的小さく（姿勢角が小さいことを仮定）、影響が無視できることから、ヨー角速度ドリフトの項を無視して以下の（６７）式のように整理する。
【０２２４】
【数４０】
2010043945.tif 000042
【０２２５】
ロール角速度ドリフトとピッチ角速度ドリフトについては、（６７）式左辺の係数行列と右辺のベクトルを一定時間積分した以下の（６８）式を解くことによって導出された以下の（６９）式によって求めることができる。
【０２２６】
【数４１】
2010043945.tif 000043
【０２２７】
【数４２】
2010043945.tif 000044
【０２２８】
ドリフト量推定手段４２８は、上記（６９）式に従って、姿勢角オブザーバ４２４で算出されるロール角及びピッチ角の各々の微分量と、運動方程式微分量算出手段４２６で算出されたロール角及びピッチ角の各々の微分量とに基づいて、ピッチ角速度センサ２２０のセンサドリフト量及びロール角速度センサ２０のセンサドリフト量を推定することができる。
【０２２９】
また、本実施の形態に係るドリフト量推定手段４２８では、演算の安定化を図るため、前回の推定値を用いて、以下の（７０）式、（７１）式に従って、上記（６９）式の演算結果により得られるセンサドリフト量の推定値を平滑化する。
【０２３０】
【数４３】
2010043945.tif 000045
【０２３１】
ただし、Ｑｄｒのチルダ、Ｐｄｒのチルダは、平滑化後のセンサドリフト量の推定値であり、λ１は、忘却係数である。
【０２３２】
なお、上記の実施の形態では、各推定値の前回値を用いて、ピッチ角速度やロール角及びピッチ角を推定する場合を例に説明したが、これに限定されるものではなく、各推定値の前々回値を用いて、ピッチ角速度やロール角及びピッチ角を推定するようにしてもよい。
【図面の簡単な説明】
【０２３３】
【図１】本発明の第１の実施の形態を示すブロック図である。
【図２】本実施の形態の座標系を示す説明図である。
【図３】ロール角速度センサからのセンサ信号と初期不感帯との関係を示す図である。
【図４】ロール角速度センサからのセンサ信号にドリフト誤差上限値が印加されている場合、センサ信号が真値である場合、及びセンサ信号にドリフト誤差下限値が印加されている場合の各々における、センサ信号と初期不感帯との関係を示す図である。
【図５】ロール角速度センサからのセンサ信号にドリフト誤差上限値が印加されている場合、センサ信号が真値である場合、及びセンサ信号にドリフト誤差下限値が印加されている場合の各々における、センサ信号と初期不感帯との関係を示す図である。
【図６】ロール角速度センサからのセンサ信号にドリフト誤差上限値が印加されている場合、センサ信号が真値である場合、及びセンサ信号にドリフト誤差下限値が印加されている場合の各々における、センサ信号と初期不感帯との関係を示す図である。
【図７】本発明の第１の実施の形態の処理を示す流れ図である。
【図８】本発明の第２の実施の形態を示すブロック図である。
【図９】本発明の第３の実施の形態を示すブロック図である。
【図１０】第３の実施の形態におけるロール角及びピッチ角の推定結果を示す線図である。
【図１１】第３の実施の形態における上下加速度及びロール角速度のセンサ信号に対するセンサドリフト量推定値及び初期不感帯を示す線図である。
【図１２】第３の実施の形態におけるロール角及びピッチ角の推定結果を示す線図である。
【図１３】第３の実施の形態における上下加速度及びロール角速度のセンサ信号に対するセンサドリフト量推定値及び初期不感帯を示す線図である。
【図１４】本発明の第３の実施の形態の処理を示す流れ図である。
【図１５】本発明の第４の実施の形態を示すブロック図である。
【符号の説明】
【０２３４】
１０   横速度推定手段
１２   前後速度推定手段
１４   上下加速度センサ
１６   横加速度センサ
１８   前後加速度センサ
２０   ロール角速度センサ
２２   ヨー角速度センサ
２３、２２３、４２３  初期不感帯補正手段
２４   ピッチ角速度推定手段
２６、３２４、４２４  姿勢角オブザーバ
２２０ ピッチ角速度センサ
３１０、４１０  姿勢角推定装置
３２２、４２２  ドリフト量補正手段
３２６、４２６  運動方程式微分量算出手段
３２８、４２８  ドリフト量推定手段
【図１】
2010043945.tif 000046
【図２】
2010043945.tif 000047
【図３】
2010043945.tif 000048
【図４】
2010043945.tif 000049
【図５】
2010043945.tif 000050
【図６】
2010043945.tif 000051
【図７】
2010043945.tif 000052
【図８】
2010043945.tif 000053
【図９】
2010043945.tif 000054
【図１０】
2010043945.tif 000055
【図１１】
2010043945.tif 000056
【図１２】
2010043945.tif 000057
【図１３】
2010043945.tif 000058
【図１４】
2010043945.tif 000059
【図１５】
2010043945.tif 000060