説明

車両姿勢角推定装置及びセンサドリフト推定装置

【課題】ドリフト誤差による影響を軽減して、精度よく姿勢角を推定することができるようにする。
【解決手段】前後車体速度を推定する前後速度推定手段12と、横車体速度を推定する横速度推定手段10と、ピッチ角速度を推定するピッチ角速度推定手段24と、ロール角速度センサ20及び上下加速度センサ14からのセンサ信号について、初期不感帯を設定するように補正する初期不感帯補正手段23と、各センサ信号、補正されたロール角速度センサ20及び上下加速度センサ14からのセンサ信号、前後車体速度の推定値、並びにピッチ角速度の推定値に基づいて、ロール角及びピッチ角を推定する姿勢角オブザーバ26と、を備えている。ピッチ角速度推定手段24は、前後車体速度の推定値、横車体速度の推定値、補正されたロール角速度及び上下加速度の各検出値、並びに推定されたロール角及びピッチ角の前回推定値に基づいて、ピッチ角速度を推定する。

【発明の詳細な説明】
【技術分野】
【0001】
本発明は、車両姿勢角推定装置及びセンサドリフト推定装置にかかり、特に、オブザーバによって車両物理量としての鉛直軸に対する車両の姿勢角であるロール角及びピッチ角を推定する車両姿勢角推定装置、及び車両運動の運動状態量の検出値に応じたセンサ信号のセンサドリフト量を推定するセンサドリフト量推定装置に関する。
【背景技術】
【0002】
従来より、車載の角速度センサ及び加速度センサを利用して車両の姿勢角を推定する姿勢角推定装置において、車載の角速度センサ及び加速度センサのゼロ点ドリフトは、姿勢角の推定への影響が大きいことから、ゼロ点ドリフトを逐次推定して補償する必要がある。
【0003】
このようなセンサのゼロ点ドリフトを推定する従来技術として、センサ出力が閾値内に一定時間以上ある場合に、物体が静止していると判定し、このときのセンサ出力値をセンサドリフト量とする手法(例えば、特許文献1)や、車輪速の左右差が小さいときに直進状態と判定し、このときの横加速度を横加速度センサドリフト量とする手法(例えば、特許文献2)などが知られている。
【特許文献1】特許第3795498号公報
【特許文献2】特開平7−40043号公報
【発明の開示】
【発明が解決しようとする課題】
【0004】
しかしながら、上記の特許文献1、2に記載の技術では、静止又は直進状態を判定し、このときのセンサ出力をドリフト量として演算しているため、センサが搭載された物体が静止していない場合や等速運動していない場合には、センサのドリフト量を推定することができず、精度よく姿勢角を推定することができない、という問題がある。
【0005】
本発明は、上記の問題点を解決するためになされたもので、ドリフト誤差による影響を軽減して、精度よく姿勢角を推定することができる姿勢角推定装置及びセンサドリフト量推定装置を提供することを目的とする。
【課題を解決するための手段】
【0006】
上記目的を達成するために第1の発明の車両姿勢角推定装置は、各輪の車輪速度に基づいて車両前後方向の車体速度である前後車体速度を推定する前後速度推定手段と、車両横方向の車体速度である横車体速度を推定する横速度推定手段と、ピッチ角速度を推定するピッチ角速度推定手段と、車両運動のロール角速度の検出値に応じたセンサ信号について、予め定められたセンサドリフト最大量、及び姿勢角の推定を開始してから現時点までにおける前記検出値の絶対値の最小値の何れか小さい方の分だけ、前記検出値の絶対値を小さくするように前記センサ信号を補正する補正手段と、車両運動の前後加速度、横加速度、上下加速度、及びヨー角速度の各検出値に応じたセンサ信号、前記補正手段によって補正された前記ロール角速度の検出値に応じたセンサ信号、前記前後車体速度の推定値、並びに前記ピッチ角速度の推定値に基づいて、車体の鉛直軸に対する姿勢角であるロール角及びピッチ角を推定する姿勢角推定手段と、を含む車両姿勢角推定装置であって、前記ピッチ角速度推定手段が、前記前後車体速度の推定値、前記横車体速度の推定値、前記上下加速度の検出値、前記補正された前記ロール角速度の検出値、前記姿勢角推定手段で推定された前記ロール角の前回推定値、及び前記姿勢角推定手段で推定された前記ピッチ角の前回推定値に基づいて、前記ピッチ角速度を推定するようにしたものである。
【0007】
第1の発明の車両姿勢角推定装置には、車両運動の前後加速度、横加速度、上下加速度、ヨー角速度、及びロール角速度の各々を検出して検出値に応じたセンサ信号を出力するセンサを設けることができる。
【0008】
第1の発明では、車両運動のロール角速度の検出値に応じたセンサ信号について、予め定められたセンサドリフト最大量、及び姿勢角の推定を開始してから現時点までにおける前記検出値の絶対値の最小値の何れか小さい方の分だけ、検出値の絶対値を小さくするようにセンサ信号を補正する。また、車両運動の前後加速度、横加速度、上下加速度、及びヨー角速度の各検出値に応じたセンサ信号、補正手段によって補正されたロール角速度の検出値に応じたセンサ信号、前後車体速度の推定値、並びにピッチ角速度の推定値に基づいて、車体の鉛直軸に対する姿勢角であるロール角及びピッチ角を推定する。
【0009】
ピッチ角速度は、前後車体速度の推定値、横車体速度の推定値、上下加速度の検出値、補正されたロール角速度の検出値、ロール角の前回推定値、及びピッチ角の前回推定値に基づいて、推定される。
【0010】
第1の発明では、車体の上下方向運動の釣り合いからピッチ角速度を推定し、ピッチ角速度の推定値を利用して車体の鉛直軸に対する姿勢角であるロール角及びピッチ角を推定しているので、上下加速度が増加するバンク走行時においても精度良くロール角及びピッチ角を推定することができる。また、初期不感帯を設定するように、ロール角速度の検出値の絶対値を小さくするようにセンサ信号を補正しているので、ドリフト誤差による影響を軽減して、精度良くロール角及びピッチ角を推定することができる。
【0011】
第2の発明の車両姿勢角推定装置は、各輪の車輪速度に基づいて車両前後方向の車体速度である前後車体速度を推定する前後速度推定手段と、車両横方向の車体速度である横車体速度を推定する横速度推定手段と、ピッチ角速度を推定するピッチ角速度推定手段と、車両運動の上下加速度の検出値に応じたセンサ信号について、予め定められたセンサドリフト最大量、及び姿勢角の推定を開始してから現時点までにおける前記検出値と標準値との差の絶対値の最小値の何れか小さい方の分だけ、前記検出値と前記標準値との差の絶対値を小さくするように前記センサ信号を補正する補正手段と、車両運動の前後加速度、横加速度、ヨー角速度、及びロール角速度の各検出値に応じたセンサ信号、前記補正手段によって補正された前記上下加速度の検出値に応じたセンサ信号、前記前後車体速度の推定値、並びに前記ピッチ角速度の推定値に基づいて、車体の鉛直軸に対する姿勢角であるロール角及びピッチ角を推定する姿勢角推定手段と、を含む車両姿勢角推定装置であって、前記ピッチ角速度推定手段が、前記前後車体速度の推定値、前記横車体速度の推定値、前記補正された前記上下加速度の検出値、前記ロール角速度の検出値、前記姿勢角推定手段で推定された前記ロール角の前回推定値、及び前記姿勢角推定手段で推定された前記ピッチ角の前回推定値に基づいて、前記ピッチ角速度を推定するようにしたものである。
【0012】
第2の発明の車両姿勢角推定装置には、車両運動の前後加速度、横加速度、上下加速度、ヨー角速度、及びロール角速度の各々を検出して検出値に応じたセンサ信号を出力するセンサを設けることができる。
【0013】
第2の発明では、補正手段によって、車両運動の上下加速度の検出値に応じたセンサ信号について、予め定められたセンサドリフト最大量、及び姿勢角の推定を開始してから現時点までにおける前記検出値と標準値との差の絶対値の最小値の何れか小さい方の分だけ、検出値と標準値との差の絶対値を小さくするようにセンサ信号を補正する。また、車両運動の前後加速度、横加速度、ロール角速度、及びヨー角速度の各検出値に応じたセンサ信号、補正手段によって補正された上下加速度の検出値に応じたセンサ信号、前後車体速度の推定値、並びにピッチ角速度の推定値に基づいて、車体の鉛直軸に対する姿勢角であるロール角及びピッチ角を推定する。
【0014】
ピッチ角速度は、前後車体速度の推定値、横車体速度の推定値、ロール角速度の検出値、補正された上下加速度の検出値、ロール角の前回推定値、及びピッチ角の前回推定値に基づいて、推定される。
【0015】
第2の発明では、車体の上下方向運動の釣り合いからピッチ角速度を推定し、ピッチ角速度の推定値を利用して車体の鉛直軸に対する姿勢角であるロール角及びピッチ角を推定しているので、上下加速度が増加するバンク走行時においても精度良くロール角及びピッチ角を推定することができる。また、初期不感帯を設定するように、上下加速度の検出値の標準値との差の絶対値を小さくするようにセンサ信号を補正しているので、ドリフト誤差による影響を軽減して、精度良くロール角及びピッチ角を推定することができる。
【0016】
第3の発明の車両姿勢角推定装置は、各輪の車輪速度に基づいて車両前後方向の車体速度である前後車体速度を推定する前後速度推定手段と、車両横方向の車体速度である横車体速度を推定する横速度推定手段と、車両運動のピッチ角速度の検出値に応じたセンサ信号について、予め定められたセンサドリフト最大量、及び姿勢角の推定を開始してから現時点までにおける前記検出値の絶対値の最小値の何れか小さい方の分だけ、前記検出値の絶対値を小さくするように前記センサ信号を補正する補正手段と、車両運動の前後加速度、横加速度、上下加速度、ヨー角速度、及びロール角速度の各検出値に応じたセンサ信号、前記補正手段によって補正された前記ピッチ角速度の検出値に応じたセンサ信号、並びに前記前後車体速度の推定値に基づいて、車体の鉛直軸に対する姿勢角であるロール角及びピッチ角を推定する姿勢角推定手段とを含んで構成されている。
【0017】
第3の発明の車両姿勢角推定装置には、車両運動の前後加速度、横加速度、上下加速度、ヨー角速度、ロール角速度、ピッチ角速度の各々を検出して検出値に応じたセンサ信号を出力するセンサを設けることができる。
【0018】
第3の発明では、車両運動のピッチ角速度の検出値に応じたセンサ信号について、予め定められたセンサドリフト最大量、及び姿勢角の推定を開始してから現時点までにおける前記検出値の絶対値の最小値の何れか小さい方の分だけ、検出値の絶対値を小さくするようにセンサ信号を補正する。また、車両運動の前後加速度、横加速度、上下加速度、ロール角速度、及びヨー角速度の各検出値に応じたセンサ信号、補正手段によって補正されたピッチ角速度の検出値に応じたセンサ信号、並びに前後車体速度の推定値に基づいて、車体の鉛直軸に対する姿勢角であるロール角及びピッチ角を推定する。
【0019】
第3の発明では、初期不感帯を設定するように、ピッチ角速度の検出値の絶対値を小さくするようにセンサ信号を補正しているので、ドリフト誤差による影響を軽減して、精度良くロール角及びピッチ角を推定することができる。
【0020】
第2の発明に係る補正手段は、上下加速度の検出値に応じたセンサ信号を補正すると共に、ロール角速度の検出値に応じたセンサ信号について、予め定められたセンサドリフト最大量、及び姿勢角の推定を開始してから現時点までにおける検出値の絶対値の最小値の何れか小さい方の分だけ、検出値の絶対値を小さくするようにセンサ信号を補正し、ピッチ角速度推定手段は、前後車体速度の推定値、横車体速度の推定値、補正された上下加速度の検出値、補正されたロール角速度の検出値、姿勢角推定手段で推定されたロール角の前回推定値、及び姿勢角推定手段で推定されたピッチ角の前回推定値に基づいて、ピッチ角速度を推定し、姿勢角推定手段は、車両運動の前後加速度、横加速度、及びヨー角速度の各検出値に応じたセンサ信号、補正手段によって補正された上下加速度の検出値に応じたセンサ信号、補正手段によって補正されたロール角速度の検出値に応じたセンサ信号、前後車体速度の推定値、並びにピッチ角速度の推定値に基づいて、車体の鉛直軸に対する姿勢角であるロール角及びピッチ角を推定することができる。これによって、初期不感帯を設定するように、ロール角速度の検出値の絶対値を小さくするようにセンサ信号を補正すると共に、上下加速度の検出値の標準値との差の絶対値を小さくするようにセンサ信号を補正しているので、ドリフト誤差による影響を軽減して、精度良くロール角及びピッチ角を推定することができる。
【0021】
第3の発明に係る補正手段は、ピッチ角速度の検出値に応じたセンサ信号を補正すると共に、ロール角速度の検出値に応じたセンサ信号について、予め定められたセンサドリフト最大量、及び姿勢角の推定を開始してから現時点までにおける検出値の絶対値の最小値の何れか小さい方の分だけ、検出値の絶対値を小さくするようにセンサ信号を補正し、姿勢角推定手段は、車両運動の前後加速度、横加速度、上下加速度、及びヨー角速度の各検出値に応じたセンサ信号、補正手段によって補正されたピッチ角速度の検出値に応じたセンサ信号、補正手段によって補正されたロール角速度の検出値に応じたセンサ信号、並びに前後車体速度の推定値に基づいて、車体の鉛直軸に対する姿勢角であるロール角及びピッチ角を推定することができる。これによって、初期不感帯を設定するように、ロール角速度の検出値の絶対値を小さくするようにセンサ信号を補正すると共に、ピッチ角速度の検出値の絶対値を小さくするようにセンサ信号を補正しているので、ドリフト誤差による影響を軽減して、精度良くロール角及びピッチ角を推定することができる。
【0022】
また、上記のピッチ角速度及びロール角速度の各検出値に応じたセンサ信号を補正する補正手段は、ピッチ角速度の検出値に応じたセンサ信号及びロール角速度の検出値に応じたセンサ信号を補正すると共に、上下加速度の検出値に応じたセンサ信号について、予め定められたセンサドリフト最大量、及び姿勢角の推定を開始してから現時点までにおける検出値と標準値との差の絶対値の最小値の何れか小さい方の分だけ、検出値と標準値との差の絶対値を小さくするようにセンサ信号を補正し、姿勢角推定手段は、車両運動の前後加速度、横加速度、及びヨー角速度の各検出値に応じたセンサ信号、補正手段によって補正された上下加速度の検出値に応じたセンサ信号、補正手段によって補正されたピッチ角速度の検出値に応じたセンサ信号、補正手段によって補正されたロール角速度の検出値に応じたセンサ信号、並びに前後車体速度の推定値に基づいて、車体の鉛直軸に対する姿勢角であるロール角及びピッチ角を推定することができる。これによって、初期不感帯を設定するように、ロール角速度の検出値の絶対値を小さくするようにセンサ信号を補正し、ピッチ角速度の検出値の絶対値を小さくするようにセンサ信号を補正すると共に、上下加速度の検出値の標準値との差の絶対値を小さくするようにセンサ信号を補正しているので、ドリフト誤差による影響を軽減して、精度良くロール角及びピッチ角を推定することができる。
【0023】
第3の発明に係る補正手段は、ピッチ角速度の検出値に応じたセンサ信号を補正すると共に、上下加速度の検出値に応じたセンサ信号について、予め定められたセンサドリフト最大量、及び姿勢角の推定を開始してから現時点までにおける検出値と標準値との差の絶対値の最小値の何れか小さい方の分だけ、検出値と標準値との差の絶対値を小さくするようにセンサ信号を補正し、姿勢角推定手段は、車両運動の前後加速度、横加速度、ロール角速度、及びヨー角速度の各検出値に応じたセンサ信号、補正手段によって補正されたピッチ角速度の検出値に応じたセンサ信号、補正手段によって補正された上下加速度の検出値に応じたセンサ信号、並びに前後車体速度の推定値に基づいて、車体の鉛直軸に対する姿勢角であるロール角及びピッチ角を推定することができる。これによって、初期不感帯を設定するように、ピッチ角速度の検出値の絶対値を小さくするようにセンサ信号を補正すると共に、上下加速度の検出値の標準値との差の絶対値を小さくするようにセンサ信号を補正しているので、ドリフト誤差による影響を軽減して、精度良くロール角及びピッチ角を推定することができる。
【0024】
第4の発明に係るセンサドリフト量推定装置は、車両運動のロール角速度の検出値に応じたセンサ信号について、予め定められたセンサドリフト最大量、及び姿勢角の推定を開始してから現時点までにおける検出値の絶対値の最小値の何れか小さい方の分だけ、前記検出値の絶対値を小さくするように前記センサ信号を補正すると共に、上下加速度の検出値に応じたセンサ信号について、予め定められたセンサドリフト最大量、及び姿勢角の推定を開始してから現時点までにおける検出値と標準値との差の絶対値の最小値の何れか小さい方の分だけ、前記検出値と前記標準値との差の絶対値を小さくするように前記センサ信号を補正する補正手段と、車両運動のヨー角速度、前後加速度、及び横加速度の各検出値に応じたセンサ信号と、前記補正手段によって補正された前記ロール角速度及び前記上下加速度の各検出値に応じたセンサ信号とに基づいて、車体の鉛直軸に対するロール角及びピッチ角の各々の微分量を算出し、算出した前記ロール角及び前記ピッチ角の各々の微分量を積分して、前記ロール角及び前記ピッチ角を推定する姿勢角推定手段と、前記上下加速度、前記前後加速度、前記横加速度、前記ロール角速度、及び前記ヨー角速度の各検出値に応じたセンサ信号と、前記姿勢角推定手段によって推定された前記ロール角及び前記ピッチ角とに基づいて、車両運動の運動方程式より得られる前記ロール角及び前記ピッチ角の各々の微分量を算出する算出手段と、前記センサ信号のセンサドリフト量を考慮したときに、前記姿勢角推定手段により算出された前記ロール角及び前記ピッチ角の各々の微分量と、前記算出手段により算出された前記ロール角及び前記ピッチ角の各々の微分量に前記センサドリフト量を考慮した値とが等しくなる関係を用いて、前記上下加速度の検出値に応じたセンサ信号及び前記ロール角速度の検出値に応じたセンサ信号の各々のセンサドリフト量を推定するドリフト量推定手段とを含んで構成されている。
【0025】
第4の発明に係るセンサドリフト量推定装置によれば、補正手段によって、車両運動のロール角速度の検出値に応じたセンサ信号について、予め定められたセンサドリフト最大量、及び姿勢角の推定を開始してから現時点までにおける検出値の絶対値の最小値の何れか小さい方の分だけ、検出値の絶対値を小さくするようにセンサ信号を補正すると共に、上下加速度の検出値に応じたセンサ信号について、予め定められたセンサドリフト最大量、及び姿勢角の推定を開始してから現時点までにおける検出値と標準値との差の絶対値の最小値の何れか小さい方の分だけ、検出値と標準値との差の絶対値を小さくするようにセンサ信号を補正する。
【0026】
そして、姿勢角推定手段によって、車両運動のヨー角速度、前後加速度、及び横加速度の各検出値に応じたセンサ信号と、補正手段によって補正されたロール角速度及び上下加速度の各検出値に応じたセンサ信号とに基づいて、車体の鉛直軸に対するロール角及びピッチ角の各々の微分量を算出し、算出したロール角及びピッチ角の各々の微分量を積分して、ロール角及びピッチ角を推定する。また、算出手段によって、上下加速度、前後加速度、横加速度、ロール角速度、及びヨー角速度の各検出値に応じたセンサ信号と、姿勢角推定手段によって推定されたロール角及びピッチ角とに基づいて、車両運動の運動方程式より得られるロール角及びピッチ角の各々の微分量を算出する。
【0027】
そして、ドリフト量推定手段によって、センサ信号のセンサドリフト量を考慮したときに、姿勢角推定手段により算出されたロール角及びピッチ角の各々の微分量と、算出手段により算出されたロール角及びピッチ角の各々の微分量にセンサドリフト量を考慮した値とが等しくなる関係を用いて、上下加速度の検出値に応じたセンサ信号及びロール角速度の検出値に応じたセンサ信号の各々のセンサドリフト量を推定する。
【0028】
このように、上下加速度及びロール角速度の各々を検出するセンサのセンサドリフト量を推定することにより、車両運動の状態に関わらず、安定して、上下加速度及びロール角速度の各々を検出するセンサのドリフト量を推定することができ、また、初期不感帯を設定するように、ロール角速度の検出値の絶対値を小さくするようにセンサ信号を補正すると共に、上下加速度の検出値の標準値との差の絶対値を小さくするようにセンサ信号を補正しているので、ドリフト誤差による影響を軽減して、精度良くロール角及びピッチ角を推定することができる。
【0029】
第5の発明に係るセンサドリフト量推定装置は、車両運動のロール角速度の検出値に応じたセンサ信号について、予め定められたセンサドリフト最大量、及び姿勢角の推定を開始してから現時点までにおける検出値の絶対値の最小値の何れか小さい方の分だけ、前記検出値の絶対値を小さくするように前記センサ信号を補正すると共に、ピッチ角速度の検出値に応じたセンサ信号について、予め定められたセンサドリフト最大量、及び姿勢角の推定を開始してから現時点までにおける検出値の絶対値の最小値の何れか小さい方の分だけ、前記検出値の絶対値を小さくするように前記センサ信号を補正する補正手段と、車両運動の上下加速度、前後加速度、横加速度、及びヨー角速度の各検出値に応じたセンサ信号と、前記補正手段によって補正された前記ロール角速度及び前記ピッチ角速度の各検出値に応じたセンサ信号とに基づいて、車体の鉛直軸に対するロール角及びピッチ角の各々の微分量を算出し、算出した前記ロール角及び前記ピッチ角の各々の微分量を積分して、前記ロール角及び前記ピッチ角を推定する姿勢角推定手段と、前記ロール角速度、前記ヨー角速度、及び前記ピッチ角速度の各検出値に応じたセンサ信号と、前記姿勢角推定手段によって推定された前記ロール角及び前記ピッチ角とに基づいて、車両運動の運動方程式より得られる前記ロール角及び前記ピッチ角の各々の微分量を算出する算出手段と、前記センサ信号のセンサドリフト量を考慮したときに、前記姿勢角推定手段により算出された前記ロール角及び前記ピッチ角の各々の微分量と、前記算出手段により算出された前記ロール角及び前記ピッチ角の各々の微分量に前記センサドリフト量を考慮した値とが等しくなる関係を用いて、前記ピッチ角速度の検出値に応じたセンサ信号及び前記ロール角速度の検出値に応じたセンサ信号の各々のセンサドリフト量を推定するドリフト量推定手段とを含んで構成されている。
【0030】
第5の発明に係るセンサドリフト量推定装置によれば、補正手段によって、車両運動のロール角速度の検出値に応じたセンサ信号について、予め定められたセンサドリフト最大量、及び姿勢角の推定を開始してから現時点までにおける検出値の絶対値の最小値の何れか小さい方の分だけ、検出値の絶対値を小さくするようにセンサ信号を補正すると共に、ピッチ角速度の検出値に応じたセンサ信号について、予め定められたセンサドリフト最大量、及び姿勢角の推定を開始してから現時点までにおける検出値の絶対値の最小値の何れか小さい方の分だけ、検出値の絶対値を小さくするようにセンサ信号を補正する。
【0031】
そして、姿勢角推定手段によって、車両運動の上下加速度、前後加速度、横加速度、及びヨー角速度の各検出値に応じたセンサ信号と、補正手段によって補正されたロール角速度及びピッチ角速度の各検出値に応じたセンサ信号とに基づいて、車体の鉛直軸に対するロール角及びピッチ角の各々の微分量を算出し、算出したロール角及びピッチ角の各々の微分量を積分して、ロール角及びピッチ角を推定する。また、算出手段によって、ロール角速度、ヨー角速度、及びピッチ角速度の各検出値に応じたセンサ信号と、姿勢角推定手段によって推定されたロール角及びピッチ角とに基づいて、車両運動の運動方程式より得られるロール角及びピッチ角の各々の微分量を算出する。
【0032】
そして、ドリフト量推定手段によって、センサ信号のセンサドリフト量を考慮したときに、姿勢角推定手段により算出されたロール角及びピッチ角の各々の微分量と、算出手段により算出されたロール角及びピッチ角の各々の微分量にセンサドリフト量を考慮した値とが等しくなる関係を用いて、ピッチ角速度の検出値に応じたセンサ信号及びロール角速度の検出値に応じたセンサ信号の各々のセンサドリフト量を推定する。
【0033】
このように、ピッチ角速度及びロール角速度の各々を検出するセンサのセンサドリフト量を推定することにより、車両運動の状態に関わらず、安定して、ピッチ角速度及びロール角速度の各々を検出するセンサのドリフト量を推定することができ、また、初期不感帯を設定するように、ロール角速度の検出値の絶対値を小さくするようにセンサ信号を補正すると共に、ピッチ角速度の検出値の絶対値を小さくするようにセンサ信号を補正しているので、ドリフト誤差による影響を軽減して、精度良くロール角及びピッチ角を推定することができる。
【0034】
第4の発明に係るセンサドリフト量推定装置は、ドリフト量推定手段によって推定されたセンサドリフト量に基づいて、上下加速度の検出値に応じたセンサ信号及びロール角速度の検出値に応じたセンサ信号の各々を補正するドリフト補正手段を更に含み、姿勢角推定手段は、ヨー角速度の検出値に応じたセンサ信号と、ドリフト補正手段によって補正され、かつ、補正手段によって補正されたロール角速度及び上下加速度の各検出値に応じたセンサ信号とに基づいて、ロール角及びピッチ角を推定することができる。
【0035】
第5の発明に係るセンサドリフト量推定装置は、ドリフト量推定手段によって推定されたセンサドリフト量に基づいて、ピッチ角速度の検出値に応じたセンサ信号及びロール角速度の検出値に応じたセンサ信号の各々を補正するドリフト補正手段を更に含み、姿勢角推定手段は、上下加速度、前後加速度、横加速度、及びヨー角速度の各検出値に応じたセンサ信号と、ドリフト補正手段によって補正され、かつ、補正手段によって補正されたロール角速度及びピッチ角速度の各検出値に応じたセンサ信号とに基づいて、ロール角及びピッチ角を推定することができる。
【0036】
本発明は、コンピュータを以下のように機能させるプログラムで構成することができる。
【0037】
第1のプログラムは、コンピュータを、各輪の車輪速度に基づいて車両前後方向の車体速度である前後車体速度を推定する前後速度推定手段、車両横方向の車体速度である横車体速度を推定する横速度推定手段、ピッチ角速度を推定するピッチ角速度推定手段、車両運動のロール角速度の検出値に応じたセンサ信号について、予め定められたセンサドリフト最大量、及び姿勢角の推定を開始してから現時点までにおける前記検出値の絶対値の最小値の何れか小さい方の分だけ、前記検出値の絶対値を小さくするように前記センサ信号を補正する補正手段、及び車両運動の前後加速度、横加速度、上下加速度、及びヨー角速度の各検出値に応じたセンサ信号、前記補正手段によって補正された前記ロール角速度の検出値に応じたセンサ信号、前記前後車体速度の推定値、並びに前記ピッチ角速度の推定値に基づいて、車体の鉛直軸に対する姿勢角であるロール角及びピッチ角を推定する姿勢角推定手段として機能させるためのプログラムであって、前記ピッチ角速度推定手段が、前記前後車体速度の推定値、前記横車体速度の推定値、前記上下加速度の検出値、前記補正された前記ロール角速度の検出値、前記姿勢角推定手段で推定された前記ロール角の前回推定値、及び前記姿勢角推定手段で推定された前記ピッチ角の前回推定値に基づいて、前記ピッチ角速度を推定するプログラムである。
【0038】
第2のプログラムは、コンピュータを、各輪の車輪速度に基づいて車両前後方向の車体速度である前後車体速度を推定する前後速度推定手段、車両横方向の車体速度である横車体速度を推定する横速度推定手段、ピッチ角速度を推定するピッチ角速度推定手段、車両運動の上下加速度の検出値に応じたセンサ信号について、予め定められたセンサドリフト最大量、及び姿勢角の推定を開始してから現時点までにおける前記検出値と標準値との差の絶対値の最小値の何れか小さい方の分だけ、前記検出値と前記標準値との差の絶対値を小さくするように前記センサ信号を補正する補正手段、及び車両運動の前後加速度、横加速度、ヨー角速度、及びロール角速度の各検出値に応じたセンサ信号、前記補正手段によって補正された前記上下加速度の検出値に応じたセンサ信号、前記前後車体速度の推定値、並びに前記ピッチ角速度の推定値に基づいて、車体の鉛直軸に対する姿勢角であるロール角及びピッチ角を推定する姿勢角推定手段として機能させるためのプログラムであって、前記ピッチ角速度推定手段が、前記前後車体速度の推定値、前記横車体速度の推定値、前記補正された前記上下加速度の検出値、前記ロール角速度の検出値、前記姿勢角推定手段で推定された前記ロール角の前回推定値、及び前記姿勢角推定手段で推定された前記ピッチ角の前回推定値に基づいて、前記ピッチ角速度を推定するプログラムである。
【0039】
第3のプログラムは、コンピュータを、各輪の車輪速度に基づいて車両前後方向の車体速度である前後車体速度を推定する前後速度推定手段、車両横方向の車体速度である横車体速度を推定する横速度推定手段、車両運動のピッチ角速度の検出値に応じたセンサ信号について、予め定められたセンサドリフト最大量、及び姿勢角の推定を開始してから現時点までにおける前記検出値の絶対値の最小値の何れか小さい方の分だけ、前記検出値の絶対値を小さくするように前記センサ信号を補正する補正手段、及び車両運動の前後加速度、横加速度、上下加速度、ヨー角速度、及びロール角速度の各検出値に応じたセンサ信号、前記補正手段によって補正された前記ピッチ角速度の検出値に応じたセンサ信号、並びに前記前後車体速度の推定値に基づいて、車体の鉛直軸に対する姿勢角であるロール角及びピッチ角を推定する姿勢角推定手段として機能させるためのプログラムである。
【0040】
第4のプログラムは、コンピュータを、車両運動のロール角速度の検出値に応じたセンサ信号について、予め定められたセンサドリフト最大量、及び姿勢角の推定を開始してから現時点までにおける前記検出値の絶対値の最小値の何れか小さい方の分だけ、前記検出値の絶対値を小さくするように前記センサ信号を補正すると共に、上下加速度の検出値に応じたセンサ信号について、予め定められたセンサドリフト最大量、及び姿勢角の推定を開始してから現時点までにおける前記検出値と標準値との差の絶対値の最小値の何れか小さい方の分だけ、前記検出値と前記標準値との差の絶対値を小さくするように前記センサ信号を補正する補正手段、車両運動のヨー角速度、前後加速度、横加速度の各検出値に応じたセンサ信号と、前記補正手段によって補正された前記ロール角速度及び前記上下加速度の各検出値に応じたセンサ信号とに基づいて、車体の鉛直軸に対するロール角及びピッチ角の各々の微分量を算出し、算出した前記ロール角及び前記ピッチ角の各々の微分量を積分して、前記ロール角及び前記ピッチ角を推定する姿勢角推定手段、前記上下加速度、前記前後加速度、前記横加速度、前記ロール角速度、及び前記ヨー角速度の各検出値に応じたセンサ信号と、前記姿勢角推定手段によって推定された前記ロール角及び前記ピッチ角とに基づいて、車両運動の運動方程式より得られる前記ロール角及び前記ピッチ角の各々の微分量を算出する算出手段、及び前記センサ信号のセンサドリフト量を考慮したときに、前記姿勢角推定手段により算出された前記ロール角及び前記ピッチ角の各々の微分量と、前記算出手段により算出された前記ロール角及び前記ピッチ角の各々の微分量に前記センサドリフト量を考慮した値とが等しくなる関係を用いて、前記上下加速度の検出値に応じたセンサ信号及び前記ロール角速度の検出値に応じたセンサ信号の各々のセンサドリフト量を推定するドリフト量推定手段として機能させるためのプログラムである。
【0041】
第5のプログラムは、コンピュータを、車両運動のロール角速度の検出値に応じたセンサ信号について、予め定められたセンサドリフト最大量、及び姿勢角の推定を開始してから現時点までにおける前記検出値の絶対値の最小値の何れか小さい方の分だけ、前記検出値の絶対値を小さくするように前記センサ信号を補正すると共に、ピッチ角速度の検出値に応じたセンサ信号について、予め定められたセンサドリフト最大量、及び姿勢角の推定を開始してから現時点までにおける前記検出値の絶対値の最小値の何れか小さい方の分だけ、前記検出値の絶対値を小さくするように前記センサ信号を補正する補正手段、車両運動の上下加速度、前後加速度、横加速度、及びヨー角速度の各検出値に応じたセンサ信号と、前記補正手段によって補正された前記ロール角速度及び前記ピッチ角速度の各検出値に応じたセンサ信号とに基づいて、車体の鉛直軸に対するロール角及びピッチ角の各々の微分量を算出し、算出した前記ロール角及び前記ピッチ角の各々の微分量を積分して、前記ロール角及び前記ピッチ角を推定する姿勢角推定手段、前記ロール角速度、前記ヨー角速度、及び前記ピッチ角速度の各検出値に応じたセンサ信号と、前記姿勢角推定手段によって推定された前記ロール角及び前記ピッチ角とに基づいて、車両運動の運動方程式より得られる前記ロール角及び前記ピッチ角の各々の微分量を算出する算出手段、及び前記センサ信号のセンサドリフト量を考慮したときに、前記姿勢角推定手段により算出された前記ロール角及び前記ピッチ角の各々の微分量と、前記算出手段により算出された前記ロール角及び前記ピッチ角の各々の微分量に前記センサドリフト量を考慮した値とが等しくなる関係を用いて、前記ピッチ角速度の検出値に応じたセンサ信号及び前記ロール角速度の検出値に応じたセンサ信号の各々のセンサドリフト量を推定するドリフト量推定手段として機能させるためのプログラムである。
【発明の効果】
【0042】
以上説明したように本発明によれば、初期不感帯を設定するように、ロール角速度、上下加速度、又はピッチ角速度のセンサ信号を補正しているので、ドリフト誤差による影響を軽減して、精度良くロール角及びピッチ角を推定することができる、という効果が得られる。
【発明を実施するための最良の形態】
【0043】
以下、図面を参照して本発明の実施の形態を詳細に説明する。本実施の形態は、鉛直軸に対する車両の姿勢角であるピッチ角及びロール角を推定する姿勢角推定装置に本発明を適用したものである。
【0044】
図1に示すように、第1の実施の形態の姿勢角推定装置には、車両横方向の車体速度である横車体速度Vを推定する横速度推定手段10、及び各輪の車輪速度に基づいて車両前後方向の車体速度である前後車体速度Uを推定する前後速度推定手段12が設けられている。
【0045】
横車体速度Vは、カルマンフィルタ等を用いて推定したり、横加速度センサの検出値から推定したりすることができる。
【0046】
各輪の車輪速度は、各輪に対応して設けられた車輪速センサにより検出することができ、前後車体速度Uは各輪の車輪速度、または各輪の車輪速度及び車輪速度の微分値から推定することができる。例えば、制動時には、4輪の車輪速度の最大値を前後車体速度Uとして出力し、駆動時には、従動輪の車輪速度の平均値を前後車体速度Uとして出力することができる。
【0047】
また、本実施の形態の姿勢角推定装置には、車両運動のxyz3軸の加速度である上下加速度Gz、横加速度Gy、前後加速度Gxの各々を検出する上下加速度センサ14、横加速度センサ16、及び前後加速度センサ18が設けられている。図2に示すように、x軸は車両前後方向、y軸は車両幅方向(横方向)、z軸は車両上下方向に対応している。
【0048】
また、本実施の形態の姿勢角推定装置には、ロール角速度Pを検出するロール角速度センサ20、及びヨー角速度Rを検出するヨー角速度センサ22が設けられている。
【0049】
上下加速度センサ14及びロール角速度センサ20は、初期不感帯を設けるように、上下加速度センサ14及びロール角速度センサ20からの各検出値に応じた各センサ信号を補正する初期不感帯補正手段23に接続されている。
【0050】
横速度推定手段10、前後速度推定手段12、及び初期不感帯補正手段23は、ピッチ角速度Qを推定するピッチ角速度推定手段24に接続されている。
【0051】
ピッチ角速度推定手段24は、車体の鉛直軸に対する姿勢角であるロール角φ及びピッチ角θを推定する姿勢角オブザーバ26に接続されている。姿勢角オブザーバ26には、更に、前後速度推定手段12、初期不感帯補正手段23、横加速度センサ16、前後加速度センサ18、及びヨー角速度センサ22が接続されている。
【0052】
姿勢角オブザーバ26は、車両運動の前後加速度Gx、横加速度Gy、及びヨー角速度Rの各検出値に応じたセンサ信号、補正された上下加速度Gzの検出値に応じたセンサ信号、補正されたロール角速度Pの検出値に応じたセンサ信号、前後車体速度Uの推定値Vso、及びピッチ角速度Qの推定値に基づいて、車体の鉛直軸に対する姿勢角であるロール角φ及びピッチ角θを推定し、ロール角φ及びピッチ角θの推定値を前回値としてピッチ角速度推定手段24に入力するように、ピッチ角速度推定手段24に接続されている。
【0053】
横速度推定手段10、前後速度推定手段12、初期不感帯補正手段23、ピッチ角速度推定手段24、及び姿勢角オブザーバ26は、各手段の機能を実現する1つまたは複数のコンピュータ、または1つまたは複数の電子回路で構成することができる。
【0054】
次に、初期不感帯を設けるようにセンサ信号を補正する原理について説明する。
【0055】
姿勢角は増大し続けないという自動車の性質から、姿勢角推定のベースとなるロール角速度とピッチ角速度とは、「短時間の間に必ずゼロ点に戻る」という特徴を有している。これは、ロール角速度とピッチ角速度とが「ゼロ点に戻らなければ最低、センサ信号の最小値分のドリフトがセンサ信号に印加されている」ことを意味している。本実施の形態では、このようなセンサ信号の特徴に着目し、現在までのセンサ信号値の履歴に応じた初期不感帯を、姿勢角推定の入力信号に対して設定して、センサ信号を補正することにより、推定される姿勢角のドリフト誤差による影響を軽減する。
【0056】
初期不感帯補正手段23は、車両運動のロール角速度の検出値に応じたセンサ信号について、ロール角速度センサ20について予め定められたセンサドリフト最大量、及び姿勢角の推定を開始してから現時点までの履歴におけるセンサ信号の絶対値の最小値の何れか小さい方を、初期不感帯の幅として設定し、初期不感帯の幅の分だけ検出値の絶対値を小さくするように、ロール角速度センサ20からのセンサ信号を補正する。
【0057】
初期不感帯補正手段23は、初期不感帯の幅の初期値として、予め求められたセンサ信号のセンサドリフト最大量(ドリフト誤差の絶対値の最大値)を設定する。例えば、センサ信号の符号が正の場合には、誤差上限値を設定し、センサ信号の符号が負の場合には、誤差下限値を設定する。また、初期不感帯補正手段23は、姿勢角推定開始から現在までのセンサ信号の履歴におけるセンサ信号の絶対値の最小値を超えないように、初期不感帯の幅を逐次修正する。この結果、図3に示すように、センサ信号値の絶対値が、センサドリフト最大量より大きい場合には、センサドリフト最大量が初期不感帯の幅として設定される。センサ信号値の絶対値が、センサドリフト最大量より小さい場合には、センサ信号値の絶対値が、初期不感帯の幅として設定される。
【0058】
図4に示すように、設定される初期不感帯の領域の面積が、推定される姿勢角の値の修正値に相当しており、例えば、ロール角速度センサからのセンサ信号が、真値にドリフト誤差の上限値が印加された信号である場合には、ロール角速度の検出値が負の方向に最も大きく補正される。センサ信号が真値であり、本来補正する必要のないドリフト誤差がない信号である場合にも、初期不感帯の設定によりロール角速度の検出値の補正が行われ、真値よりも小さい値が、ロール角速度の検出値として出力されるが、補正幅は、ドリフト誤差の上限値が印加された信号である場合に比べて小さくなる。
【0059】
また、センサ信号の符号と印加されるドリフトの符号が異なり、ロール角速度センサのセンサ信号が、真値にドリフト誤差の下限値(上限値と逆符号の誤差)が印加された信号である場合には、本来増加側に補正されるべきロール角速度の検出値が減少側に補正されてしまうが、補正による減少幅は比較的小さい。
【0060】
なお、上記のような、ドリフト誤差の下限値がセンサ信号に印加されている状況で、大きなロール角速度が出力される場合には、センサ信号の符号と印加されるドリフト誤差の符号が異なっており、図5に示すように、横転時を除き比較的早いタイミングでロール角速度のセンサ信号の符号は必ず反転する。符号の反転は、初期不感帯補償による補正が終了することを意味しており、時間積分として寄与する姿勢角推定への影響は小さい。
【0061】
一方、ロール角速度の真値がほぼ0となる走行条件では、信号に占めるドリフト誤差の割合が大きく、長時間、ドリフト誤差によりセンサ信号が0にならない状況もある。このような場合、図6に示すように、ロール角速度の信号の符号とドリフト誤差の符号とが一致するため、設定された初期不感帯によって、長時間適切な信号の補正が行われる。
【0062】
また、初期不感帯補正手段23は、車両運動の上下加速度の検出値に応じたセンサ信号について、上下加速度センサ14について予め定められたセンサドリフト最大量、及び姿勢角の推定を開始してから現時点までの履歴におけるセンサ信号の標準値(9.8m/s2)との差の絶対値の最小値の何れか小さい方を、初期不感帯の幅として設定し、初期不感帯の幅の分だけ、標準値との差の絶対値を小さくするように、上下加速度センサ14からのセンサ信号を補正する。
【0063】
次に、ピッチ角速度の推定について説明する。剛体に固定された3軸加速度及び3軸角速度を検出する3軸センサから出力されるセンサ信号と運動状態量との関係を表す剛体の運動方程式は、以下のように記述することができる。
【0064】
【数1】

【0065】
ただし、 Gx:前後加速度、Gy:横加速度、Gz:上下加速度、P:ロール角速度、Q:ピッチ角速度、R:ヨー角速度、U:前後速度、V:横速度、W:上下速度、φ:ロール角、θ:ピッチ角、g:重力加速度である。
【0066】
本実施の形態において車両を剛体とした場合、前後加速度Gx、横加速度Gy、及びヨー角速度Rの各々は、前後加速度センサ18、横加速度センサ16、及びヨー角速度センサ22によって検出される。また、上下加速度Gz及びロール角速度Pは、上下加速度センサ14及びロール角速度センサ20によって検出された信号を初期不感帯補正手段23によって補正した補正信号である。横速度Vは、上記で説明したように横速度推定手段10で推定することができ、前後速度Uは各輪の車輪速に基づいて上記で説明したように前後速度推定手段12で推定することができる。
【0067】
なお、本実施の形態では、図2に示すように車体の上向きをz軸の正方向とする右手系で座標を記述すると共に角度はオイラー角で表現している。
【0068】
バンク走行中は、遠心力によって上下加速度Gzが増加する。上下加速度Gzの増加は、上記(3)式において左辺の−QUが大きくなることと対応しており、この性質を利用し、(3)式において上下速度Wの変化を無視(W=0とする)すると共に、ロール角及びピッチ角の各々の前回値、及び横速度の推定値、前後速度Uとして前後車体速度の推定値Vsoを用いることによりことにより、ピッチ角速度の推定値を以下の式から求めることができる。ピッチ角速度推定手段24は、下記(6)式の演算を実行することによりピッチ角速度の推定値を求める。
【0069】
【数2】

【0070】
なお、上記(6)式において、Pは、ロール角速度センサ20のセンサ信号を、初期不感帯補正手段23によって補正した補正信号を表わし、Gは、上下加速度センサ14のセンサ信号Gを、初期不感帯補正手段23によって補正した補正信号を表わしている。
【0071】
ピッチ角速度が上記のように推定されると、ピッチ角もロール角と同時に姿勢角オブザーバ26によって推定することができる。姿勢角オブザーバ26を用いた場合の自動車固有の運動の拘束条件について説明する。上記の運動方程式を利用して、姿勢角オブザーバ26を構成する場合、積分演算によって推定される速度及び角度の状態量が発散しないように、測定できる物理量のフィードバックが必要となる。本実施の形態では、自動車運動固有の特徴を以下のようにフィードバックする物理量に利用する。なお、以下の式では、ロール角及びピッチ角として各々の前回推定値、横速度として横車体速度の推定値、前後速度Uとして前後車体速度の推定値Vsoを各々用いている。
【0072】
前後車体速度の推定値Vsoを微分して上記(1)式に代入し、自動車運動固有の特徴として、車体の前後方向に関しては「車輪のスリップは長時間増加し続けない」という性質を利用すると、上記(1)式から、上下速度の変化を無視したフィードバックに利用する以下の(7)式に示す条件が得られる。
【0073】
【数3】

【0074】
上記(7)式は、車輪速度から推定される加速度(前後車体速度の推定値Vsoの微分値)と前後加速度Gxとの差と、ピッチ角θとの関係を記述している。
【0075】
また、車体の横方向に関しては、「スリップ角は長時間増加し続けない」という性質からロール角速度及び横加速度が無視できることから、フィードバックに利用するための(2)式から、以下の(8)式に示す条件が得られる。
【0076】
【数4】

【0077】
さらに、一般的な道路勾配を考慮すると、「鉛直方向の加速度は、重力加速度に略一致している」とみなすことができる。この条件は前後加速度Gx、横加速度Gy、上下加速度Gz、ピッチ角θ、及びロール角φを用いて、以下の(9)式に示す代数方程式によって記述することができる。
【0078】
【数5】

【0079】
上記(7)〜(9)式の関係は、いずれもある程度長い時間を考慮したときに満足する条件であることから、オブザーバ測定量のフィードバックには、以下で説明するように上記(7)〜(9)式の両辺をローパスフィルタ処理した値を利用する。
【0080】
次に、基本的な非線形オブザーバの構成について説明する。ここでは、センサから出力されるセンサ信号、初期不感帯を設定するように補正された補正信号、及びピッチ角速度の推定値を含む値uを下記(10)式のように表現している。
【0081】
【数6】

【0082】
また、対象となる車両運動を下記(11)式のように表現し、オブザーバを構成するために測定できる物理量を(12)式のように表現すると、非線形オブザーバは、以下の(13)式及び(14)式の非線形運動方程式によって記述することができる。
【0083】
【数7】

【0084】
ただし、xのチルダ、yのチルダは、それぞれx、yの推定値を表わし、k(xのチルダ、u)は設計するオブザーバゲインを表わしている。
【0085】
上記(7)〜(9)式は、何れもある程度長い時間を考慮したときに満足する条件であることから、オブザーバの測定量のフィードバックには、下記(15)〜(17)式に示す(7)〜(9)式の両辺をローパスフィルタでローパスフィルタ処理した値を利用する。
【0086】
【数8】

【0087】
ただし、τx、 τy、 τgは各々(7)〜(9)式で考慮するローパスフィルタの数秒から数十秒以上の時定数を表している。
【0088】
次に、上記の基本的な非線形オブザーバを用いてロール角φ及びピッチ角θを推定する本実施の形態のロール角φ及びピッチ角θの姿勢角オブザーバ26について説明する。上記(4)、(5)式の角度に関する状態方程式には、速度の状態量が含まれていないことから、速度の推定と分離して、角度としてロール角φ及びピッチ角θを推定することができる。
【0089】
このため、まず、上記のローパスフィルタによって生じる状態量を含め、ロール角及びピッチ角を推定するためのオブザーバを構成する。本実施の形態では、オブザーバの状態量を下記(18)式で表し、
【0090】
【数9】

フィードバックに用いるオブザーバ出力を下記(19)式で表し、
【0091】
【数10】

【0092】
さらに、センサ信号等から演算される車両出力を以下の(20)式で表わし、適切なオブザーバゲインk(xのチルダ,u)を設定することによって、ロール角及びピッチ角を推定する姿勢角オブザーバ26を構成している。
【0093】
【数11】

ただし、
【0094】
【数12】

である。
【0095】
なお、(21)式右辺第2項の分子は、前後車体速度の推定値Vs0の微分値から、3軸センサで検出されたヨー角速度値Rと横車体速度の推定値との積と、3軸センサで検出された前後加速度値Gxとの和を減算した前後加速度状態量の偏差であり、(22)式右辺第2項の分子は、3軸センサで検出されたヨー角速度値Rと前後車体速度の推定値Vs0との積から3軸センサで検出された横加速度値Gyを減算した横加速度状態量の偏差である。
【0096】
オブザーバゲインの一例として、本実施の形態では、オブザーバの安定性を確保するため、線形化を行ったときの対角成分が負の係数を持つように、下記(23)式のように表す。
【0097】
【数13】

【0098】
ただし、Kφy、 Kφg、 Kθx、 Kθg、 Kx、 Ky、 Kgは適切な正の定数である。したがって、ロール角及びピッチ角を推定する本実施の形態の非線形オブザーバは、以下の(24)式に示す運動方程式で記述される。
【0099】
【数14】

ただし、xのチルダは、以下の(25)式で表される。
【0100】
【数15】

【0101】
姿勢角オブザーバ26は、上記(24)式を用いることにより、車体鉛直方向に対する姿勢角であるロール角φのチルダの微分量dφのチルダとピッチ角θのチルダの微分量dθのチルダとを算出し、算出したロール角φのチルダの微分量dφのチルダ及びピッチ角θのチルダの微分量dθのチルダの各々を積分することにより、ロール角φのチルダ及びピッチ角θのチルダを算出することができる。
【0102】
コンピュータを、横速度推定手段10、前後速度推定手段12、初期不感帯補正手段23、ピッチ角速度推定手段24、及び姿勢角オブザーバ26の各手段として機能させるプログラムによる情報処理は、図7のフローチャートに示す姿勢角推定処理ルーチンで実現することができる。コンピュータは、相互にバスによって接続されたCPU,ROM,及びRAM,並びに必要に応じて接続されたHDDで構成され、これらのプログラムは、コンピュータのCPUに接続されているROMまたはHDD等の記録媒体に記録される。
【0103】
この姿勢角推定処理ルーチンを説明すると、ステップ100で、上下加速度センサ14、横加速度センサ16、前後加速度センサ18、ロール角速度センサ20、及びヨー角速度センサ22から、各検出値に応じたセンサ信号を取得する。このとき、上下加速度センサ14からのセンサ信号の検出値を、上下加速度センサ14からのセンサ信号の検出値の履歴として記録すると共に、ロール角速度センサ20からのセンサ信号の検出値を、ロール角速度センサ20からのセンサ信号の検出値の履歴として記録する。
【0104】
そして、ステップ102において、上記ステップ100で取得した上下加速度センサ14の検出値に応じたセンサ信号について、姿勢角推定を開始してから得られた上下加速度センサ14の検出値の履歴のうち、検出値と標準値との差の絶対値が最小となるセンサ信号であるか否かを判定する。上記ステップ100で取得した上下加速度センサ14の検出値に応じたセンサ信号について、検出値と標準値との差の絶対値が最小でない場合には、ステップ106へ移行する。一方、上記ステップ102において、上記ステップ100で取得した上下加速度センサ14の検出値に応じたセンサ信号について、検出値と標準値との差の絶対値が最小となる場合には、ステップ104において、標準値との差の絶対値が最小になるセンサ信号の検出値として、上記ステップ100で取得した上下加速度センサ14の検出値を記憶する。
【0105】
次のステップ108では、上記ステップ100で取得したロール角速度センサ20の検出値に応じたセンサ信号について、姿勢角推定を開始してから得られたロール角速度センサ20の検出値の履歴のうち、検出値の絶対値が最小となるセンサ信号であるか否かを判定する。上記ステップ100で取得したロール角速度センサ20の検出値に応じたセンサ信号について、検出値の絶対値が最小でない場合には、ステップ110へ移行する。一方、上記ステップ106において、上記ステップ100で取得したロール角速度センサ20の検出値に応じたセンサ信号について、検出値の絶対値が最小になる場合には、ステップ108において、検出値の絶対値が最小になるセンサ信号の検出値として、上記ステップ100で取得したロール角速度センサ20の検出値を記憶する。
【0106】
そして、ステップ110では、上記ステップ100で取得した上下加速度センサ14の検出値に応じたセンサ信号について、予め定められた上下加速度センサ14のセンサドリフト最大量、及び姿勢角の推定を開始してから現時点までの履歴における標準値との差の絶対値の最小値の何れか小さい方の分だけ、検出値と標準値との差の絶対値を小さくするようにセンサ信号を補正する。また、上記ステップ100で取得したロール角速度センサ20の検出値に応じたセンサ信号について、予め定められたロール角速度センサ20のセンサドリフト最大量、及び姿勢角の推定を開始してから現時点までの履歴における検出値の絶対値の最小値の何れか小さい方の分だけ、検出値の絶対値を小さくするようにセンサ信号を補正する。
【0107】
次のステップ112では、上記で説明したように横車体速度を推定し、ステップ114において各輪の車輪速度に基づいて前後車体速度を推定する。
【0108】
ステップ116において、最初の演算か否かを判断し、最初の演算の場合には、ロール角及びピッチ角の前回値が存在しないので、ステップ118でロール角及びピッチ角の前回値として予め定められた初期値を設定し、ステップ122において、上記で説明したように上記(6)式を用いてピッチ角速度Qの推定値を演算する。
【0109】
一方、2回目以降の演算の場合には、後述するステップ126でロール角及びピッチ角の推定値が演算されるので、ステップ120においてロール角及びピッチ角の値として前回演算された前回値を設定し、ステップ122において、上記で説明したように上記(6)式を用いてピッチ角速度Qの推定値を演算する。
【0110】
ステップ124において、上記で説明したように、車両運動の前後加速度Gx、横加速度Gy、及びヨー角速度Rの検出値に応じたセンサ信号、前後車体速度Uの推定値Vsoに基づいて、上記(21)、(22)式で記述されたローパスフィルタ処理を行う。
【0111】
次のステップ126では、ステップ122で演算されたピッチ角速度Qの推定値、ステップ124で演算されたローパスフィルタ処理値を用い、上記(24)式にしたがって車体の鉛直軸に対する姿勢角であるロール角φ及びピッチ角θを推定し、ロール角φ及びピッチ角θの推定値をコンピュータのメモリに記憶して、上記ステップ100へ戻る。
【0112】
なお、上記の実施の形態では、ロール角速度センサ及び上下加速度センサからの各センサ信号に対して、初期不感帯を設定するように補正する場合を例に説明したが、これに限定されるものではなく、ロール角速度センサ及び上下加速度センサの何れか一方のセンサ信号に対してのみ、初期不感帯を設定するように補正してもよい。
【0113】
次に、本発明の第2の実施の形態について説明する。なお、第1の実施の形態と同様の構成となる部分については、同一符号を付して説明を省略する。
【0114】
第2の実施の形態では、各種センサに加えて、ピッチ角速度センサを用いて、姿勢角を推定している点が第1の実施の形態と異なっている。
【0115】
図8に示すように、第2の実施の形態の姿勢角推定装置には、上下加速度センサ14、横加速度センサ16、前後加速度センサ18、ロール角速度センサ20、ヨー角速度センサ22、及びピッチ角速度を検出するピッチ角速度センサ220が設けられている。
【0116】
上下加速度センサ14、ロール角速度センサ20、及びピッチ角速度センサ220は、初期不感帯を設けるように、上下加速度センサ14、ロール角速度センサ20、及びピッチ角速度センサ220からの各検出値に応じた各センサ信号を補正する初期不感帯補正手段223に接続されている。
【0117】
姿勢角オブザーバ26には、横速度推定手段10、前後速度推定手段12、初期不感帯補正手段223、横加速度センサ16、前後加速度センサ18、及びヨー角速度センサ22が接続されている。
【0118】
初期不感帯補正手段223は、車両運動のロール角速度の検出値に応じたセンサ信号について、第1の実施の形態と同様に、初期不感帯を設定し、初期不感帯の幅の分だけ、絶対値を小さくするように、ロール角速度センサ20からのセンサ信号を補正する。また、初期不感帯補正手段223は、車両運動の上下加速度の検出値に応じたセンサ信号について、第1の実施の形態と同様に、初期不感帯を設定し、初期不感帯の幅の分だけ、標準値との差の絶対値を小さくするように、上下加速度センサ14からのセンサ信号を補正する。
【0119】
また、初期不感帯補正手段223は、車両運動のピッチ角速度の検出値に応じたセンサ信号について、ピッチ角速度センサ220について予め定められたセンサドリフト最大量、及び姿勢角の推定を開始してから現時点までにおけるセンサ信号の絶対値の最小値の何れか小さい方を、初期不感帯の幅として設定し、初期不感帯の幅の分だけ、絶対値を小さくするように、ピッチ角速度センサ220からのセンサ信号を補正する。
【0120】
姿勢角オブザーバ26は、上記の第1の実施の形態で説明した(24)式を以下の(26)式に置き換えて、前後加速度センサ18、横加速度センサ16、及びヨー角速度センサ22によって検出される前後加速度Gx、横加速度Gy、及びヨー角速度Rと、初期不感帯補正手段23によって補正された上下加速度Gz、ロール角速度P、ピッチ角速度Qと、横速度推定手段10で推定された横速度Vのチルダと、前後速度推定手段12で推定された前後速度Vs0とを用いて、車体鉛直方向に対する姿勢角であるロール角の微分量dφのチルダとピッチ角の微分量dθのチルダとを算出する。また、姿勢角オブザーバ26は、算出したロール角の微分量dφのチルダ及びピッチ角の微分量dθのチルダの各々を積分することにより、ロール角φのチルダ及びピッチ角θのチルダを算出する。
【0121】
【数16】

【0122】
なお、上記の実施の形態では、ロール角速度センサ、ピッチ角速度センサ、及び上下加速度センサからの各センサ信号に対して、初期不感帯を設定するように補正する場合を例に説明したが、これに限定されるものではなく、ピッチ角速度センサからのセンサ信号に対してのみ、初期不感帯を設定するように補正してもよい。また、ロール角速度センサ、ピッチ角速度センサ、及び上下加速度センサのうちの任意の2つのセンサの組み合わせからの各センサ信号に対して、初期不感帯を設定するように補正してもよい。例えば、ロール角速度センサ及びピッチ角速度センサからの各センサ信号に対して、初期不感帯を設定するように補正してもよく、また、ピッチ角速度センサ及び上下加速度センサからの各センサ信号に対して、初期不感帯を設定するように補正してもよい。
【0123】
次に、本発明の第3の実施の形態について説明する。なお、第1の実施の形態と同様の構成となる部分については、同一符号を付して説明を省略する。第3の実施の形態では、
第3の実施の形態では、センサドリフト量を補正した後に、初期不感帯を設定するように補正している点が、第1の実施の形態と主に異なっている。
【0124】
図9に示すように、第3の実施の形態に係る姿勢角推定装置310は、上下加速度センサ14、横加速度センサ16、前後加速度センサ18、ロール角速度センサ20、及びヨー角速度センサ22が設けられている。
【0125】
上下加速度センサ14、及びロール角速度センサ20は、後述するドリフト量推定手段328によって推定されたセンサドリフト量に基づいて、各センサからのセンサ信号を補正するドリフト量補正手段322に接続されている。ドリフト量補正手段322は、ドリフト量推定手段328に接続されている。
【0126】
ドリフト量補正手段322は、初期不感帯補正手段23に接続されている。初期不感帯補正手段23は、車体の鉛直軸に対する姿勢角であるロール角φ及びピッチ角θを推定する姿勢角オブザーバ324に接続されている。
【0127】
上下加速度センサ14、ロール角速度センサ20、ヨー角速度センサ22、及び姿勢角オブザーバ324は、車両運動の運動方程式により得られるロール角及びピッチ角の各々の微分量を算出する運動方程式微分量算出手段326に接続されている。
【0128】
姿勢角オブザーバ324及び運動方程式微分量算出手段326は、上下加速度センサ14及びロール角速度センサ20の各々のセンサドリフト量を推定するドリフト量推定手段328に接続されている。
【0129】
ドリフト量補正手段322、初期不感帯補正手段23、姿勢角オブザーバ324、運動方程式微分量算出手段326、及びドリフト量推定手段328は、各手段の機能を実現する1つまたは複数のコンピュータ、または1つまたは複数の電子回路で構成することができる。
【0130】
姿勢角オブザーバ324は、車両横方向の車両速度である横車体速度Vを推定すると共に、各輪の車輪速度に基づいて車両前後方向の車両速度である前後車体速度Uを推定する。また、姿勢角オブザーバ324は、横車体速度Vの推定値、前後車体速度Uの推定値、ドリフト量補正手段322で補正された後に初期不感帯補正手段23によって補正された上下加速度Gの検出値に応じた補正信号、及びドリフト量補正手段322で補正された後に初期不感帯補正手段23によって補正されたロール角速度Pの検出値に応じた補正信号に基づいて、ピッチ角速度Qを推定する。なお、横車体速度Vは、カルマンフィルタ等を用いて推定したり、横加速度センサの検出値から推定したりすることができる。
【0131】
姿勢角オブザーバ324は、ドリフト量補正手段322で補正された後に初期不感帯補正手段23によって補正された、車両運動の上下加速度G及びロール角速度Pの各検出値に応じた補正信号と、車両運動の前後加速度G、横加速度G、及びヨー角速度Rの各検出値に応じたセンサ信号と、前後車体速度Uの推定値Vsoと、ピッチ角速度Qの推定値とに基づいて、車体の鉛直軸に対する姿勢角であるロール角φ及びピッチ角θを推定する。
【0132】
姿勢角オブザーバ324は、上記第1の実施の形態で説明した(6)式を以下の(27)式に置き換えて、ピッチ角速度の推定値Qのチルダを求める。
【0133】
【数17】

【0134】
なお、上記(27)式において、P−Pdrは、ロール角速度センサ20のセンサ信号Pのドリフト量Pdrを補正した後に初期不感帯を設定するように補正した補正信号を表わし、G−Gzdrは、上下加速度センサ14のセンサ信号Gのドリフト量Gzdrを補正した後に初期不感帯を設定するように補正した補正信号を表わしている。
【0135】
ピッチ角速度が上記のように推定されると、ピッチ角もロール角と同時に姿勢角オブザーバ324によって推定することができる。本実施の形態では、姿勢角オブザーバ324を用いた場合の自動車固有の運動の拘束条件について、一般的な道路勾配を考慮した、上記(9)式に示す条件を、以下の(28)式に示す条件に置き換える。
【0136】
【数18】

【0137】
次に、基本的な非線形オブザーバの構成について説明する。ここでは、上記(10)式を、以下の(29)式に置き換えて、ドリフト量を補正された後に初期不感帯を設定するように補正された補正信号と、センサ信号と、ピッチ角速度の推定値とを含む値uを表現している。
【0138】
【数19】

【0139】
オブザーバの測定量のフィードバックに利用するローパスフィルタ処理を示す、上記(15)〜(17)式を、以下の(30)式〜(32)式に置き換えて用いている。
【0140】
【数20】

【0141】
次に、上記の基本的な非線形オブザーバを用いてロール角φ及びピッチ角θを推定する本実施の形態の姿勢角オブザーバ324について説明する。
【0142】
また、ロール角及びピッチ角を推定する本実施の形態の非線形オブザーバは、以下の(33)式で表される運動方程式で記述される。
【0143】
【数21】

ただし、xのチルダは、以下の(34)式で表される。
【0144】
【数22】

【0145】
姿勢角オブザーバ324は、上記(33)式を用いることにより、車体鉛直方向に対する姿勢角であるロール角の微分量dφのチルダとピッチ角の微分量dθのチルダとを算出し、算出したロール角の微分量dφのチルダ及びピッチ角の微分量dθのチルダの各々を積分することにより、ロール角φのチルダ及びピッチ角θのチルダを算出することができる。
【0146】
次に、車両運動の運動方程式により得られるロール角φの微分量dφ及びピッチ角θの微分量dθの各々の算出方法について説明する。
【0147】
上記(1)式〜(5)式に対して、上記(6)式を代入し、上下車体速度を0と仮定すると共に、センサ信号G、G、G、P、Rに、所定のドリフト誤差Gxdr、Gydr、Gzdr、Pdr、Rdrが重畳していると仮定すると、以下の(35)式〜(38)式が得られる。
【0148】
【数23】

【0149】
ただし、ロール角φ及びピッチ角θには、姿勢角オブザーバ324によって推定されたロール角φ及びピッチ角θの推定値を用いる。また、上下加速度G−Gzdr、及びロール角速度P−Pdrの各々は、上下加速度センサ14及びロール角速度センサ20によって検出されたセンサ信号からセンサドリフト量を除去するように補正した補正信号である。また、前後加速度G−Gxdr、横加速度G−Gydr、及びヨー角速度R−Rdrの各々は、前後加速度センサ18、横加速度センサ16、及びヨー角速度センサ22の各々によって検出された各センサ信号に対してセンサドリフト量を除去するように補正した補正信号である。
【0150】
また、横車体速度Vは、カルマンフィルタ等を用いて推定したり、横加速度センサの検出値から推定したりすることができ、前後車体速度Uは各輪の車輪速に基づいて推定することができる。
【0151】
車両運動の運動方程式により得られるロール角φの微分量dφおよびピッチ角θの微分量dθは、上記(37)式、(38)式のドリフト量を無視した以下の(39)式、(40)式で算出される。
【0152】
【数24】

【0153】
また、上記(37)式のφのドット、上記(38)式のθのドットと、上記(39)式のdφ、上記(40)式のdθとの間には、以下の(41)式、(42)式で表される関係が存在する。
【0154】
【数25】

【0155】
運動方程式微分量算出手段326は、上記(39)式、(40)式に従って、車両運動の運動方程式より得られるロール角φ及びピッチ角θの微分量を算出する。
【0156】
本実施の形態では、ドリフト量推定手段328によって、姿勢角オブザーバ324で算出された姿勢角の微分量と運動方程式微分量算出手段326で算出された姿勢角の微分量との比較から、センサドリフト量を推定する。
【0157】
次に、センサドリフト量の推定方法について説明する。
【0158】
センサ信号G、G、G、P、Rに、所定のドリフト誤差Gxdr、Gydr、Gzdr、Pdr、Rdrが重畳していると仮定するとともに、上記(41)式、(42)式の右辺が表わす微分量について、姿勢角オブザーバ324によって真値が既知であると仮定すると、以下の(39)式、(40)式で表される関係が成立する。
【0159】
【数26】

【0160】
ただし、dφ、dθは、上記(33)式により得られるオブザーバ内部演算値としてのロール角の微分量、ピッチ角の微分量である。
【0161】
上記(43)式、(44)式は、センサ信号のセンサドリフト量を考慮したときに、姿勢角オブザーバ324により算出されたロール角及びピッチ角の微分量と、運動方程式により得られるロール角及びピッチ角の微分量にセンサドリフト量を考慮した値とが等しくなる関係を表わしている。
【0162】
ここで、上記(43)式、(44)式にG、Gが含まれておらず、かつ、Rの係数も比較的小さい(姿勢角が小さいことを仮定)ことから、上下加速度センサ14及びロール角速度センサ20によって検出されたセンサ信号がドリフト量補正手段322において補正された後に初期不感帯補正手段23によって補正された補正信号を用いて、上下加速度G及びロール角速度Pに関するセンサドリフト量の推定を行うことが可能である。
【0163】
ところで、横加速度に関しては、「スリップ角は長時間、非線形領域に留まらない」という条件から、以下の(45)式で表される関係を利用することができる。
【0164】
【数27】

【0165】
ただし、τは、長時間の運動のみを考慮するためのフィルタ時定数であり、c、cは、前後輪のコーナリングパワーであり、l、lは、前後軸−重心間距離である。また、lはホイールベースであり、mは車両質量であり、δは前輪実舵角である。
【0166】
また、上下加速度に関して、「鉛直方向の加速度は重力加速度」という条件から、以下の(46)式で表される関係を利用することができる。
【0167】
【数28】

【0168】
ただし、Dgf、Egfは、以下の式で表される。また、τは、長時間の運動のみを考慮するためのフィルタ時定数である。
【0169】
【数29】

【0170】
ここでは、上記(43)式、(44)式、(46)式から、以下の(47)式で記述することができる。
【0171】
【数30】

ただし、dD1、dE1、dE2、dE3は、以下の(48)式〜(51)式で表される。
【0172】
【数31】

【0173】
そして、上記(47)式の左辺の係数行列と右辺のベクトルとを一定時間積分すると、以下の(52)式が得られる。
【0174】
【数32】

ただし、D1、E1、E2、E3は、以下の(53)式〜(56)式で表される。
【0175】
【数33】

【0176】
上記(52)式を解くことによって、以下の(57)式を導出することができる。
【0177】
【数34】

ただし、Dは、Dの擬似逆行列である。
【0178】
ドリフト量推定手段328は、上記(57)式に従って、姿勢角オブザーバ324で算出されるロール角及びピッチ角の各々の微分量と、運動方程式微分量算出手段326で算出されたロール角及びピッチ角の各々の微分量とに基づいて、上下加速度センサ14のセンサドリフト量及びロール角速度センサ20のセンサドリフト量を推定することができる。
【0179】
また、本実施の形態に係るドリフト量推定手段328では、演算の安定化を図るため、前回の推定値を用いて、以下の(58)式、(59)式に従って、上記(57)式の演算結果により得られるセンサドリフト量の推定値を平滑化する。
【0180】
【数35】

【0181】
ただし、Gzdrのチルダ、Pdrのチルダは、平滑化後のセンサドリフト量の推定値であり、λ1は、忘却係数である。
【0182】
初期不感帯補正手段23は、ドリフト量補正手段322によって補正されたロール角速度の検出値に応じたセンサ信号について、ロール角速度センサ20について予め定められたセンサドリフト最大量、及び姿勢角の推定を開始してから現時点までの履歴における、ドリフト量補正手段322により補正されたセンサ信号の絶対値の最小値の何れか小さい方を、初期不感帯の幅として設定し、初期不感帯の幅の分だけ、絶対値を小さくするように、ドリフト量補正手段322によって補正されたロール角速度センサ20からのセンサ信号を補正する。
【0183】
また、初期不感帯補正手段23は、ドリフト量補正手段322によって補正された上下加速度の検出値に応じたセンサ信号について、上下加速度センサ14について予め定められたセンサドリフト最大量、及び姿勢角の推定を開始してから現時点までの履歴における、ドリフト量補正手段322により補正されたセンサ信号の標準値との差の絶対値の最小値の何れか小さい方を、初期不感帯の幅として設定し、初期不感帯の幅の分だけ、標準値との差の絶対値を小さくするように、ドリフト量補正手段322によって補正された上下加速度センサ14からのセンサ信号を補正する。
【0184】
次に、本実施の形態の初期不感帯を設定するようにセンサ信号を補正する方法による推定結果について説明する。なお、センサ信号を補正する方法の効果を確認するため、0.3G程度の定常円旋回データにドリフト誤差を印加して姿勢角の推定を行った。また、ロール角速度センサ20と上下加速度センサ14とのドリフト誤差推定のみを行うとともに、両センサからの信号に対する初期不感帯補償による補正を行う場合と、初期不感帯補償による補正を行わない場合とについて、姿勢角の推定を行った。
【0185】
まず、ロール角速度センサからのセンサ信号に3deg/sのドリフト誤差を印加した状態で、姿勢角の推定を行った。図10(A)、(B)に示すように、初期不感帯補償による補正を行わない場合には、ロール角の推定値は振動的になっているとともに、ピッチ角の推定値は、真値(ドリフトを印加しないときの推定値)から大きくずれてしまっている。これに対し、初期不感帯補償による補正を行った場合には、真値に近いロール角の推定値が演算され、また、真値に近いピッチ角の推定値が演算されている。
【0186】
また、図11(B)に示すように、ロール角速度センサからのセンサ信号に3deg/sのドリフト誤差を印加した場合には、ロール角速度のセンサドリフト量の推定の遅れを補償するように、初期不感帯補償による補正が行われていることがわかる。
【0187】
また、ロール角速度センサからのセンサ信号に−3deg/sのドリフト誤差を印加した状態で、姿勢角の推定を行った。図12(A)、(B)に示すように、初期不感帯補償による補正を行わない場合には、ロール角の推定値及びピッチ角の推定値の各々は、真値(ドリフトを印加しないときの推定値)から大きくずれてしまっている。
【0188】
また、図13(B)に示すように、ロール角速度センサからのセンサ信号に−3deg/sのドリフト誤差を印加した場合には、ロール角速度のセンサドリフト量の推定の遅れを補償するように、初期不感帯補償による補正が行われていることがわかる。
【0189】
上記の第3の実施の形態において、コンピュータを、ドリフト量補正手段322、初期不感帯補正手段23、姿勢角オブザーバ324、運動方程式微分量算出手段326、及びドリフト量推定手段328の各手段として機能させるプログラムによる情報処理は、図14のフローチャートに示す姿勢角推定処理ルーチンで実現することができる。コンピュータは、相互にバスによって接続されたCPU、ROM、及びRAM、並びに必要に応じて接続されたHDDで構成され、これらのプログラムは、コンピュータのCPUに接続されているROMまたはHDD等の記録媒体に記録される。
【0190】
以下に、この姿勢角推定処理ルーチンについて説明する。なお、第1の実施の形態と同様の処理については、同一符号を付して詳細な説明を省略する。
【0191】
まず、ステップ100で、前後加速度センサ18、横加速度センサ16、上下加速度センサ14、ロール角速度センサ20、及びヨー角速度センサ22から、各検出値に応じたセンサ信号を取得する。
【0192】
そして、ステップ350において、後述のステップ366で1演算周期前に得られた上下加速度センサ14のセンサドリフト量及びロール角速度センサ20のセンサドリフト量を用いて、上記ステップ100で取得された上下加速度センサ14及びロール角速度センサ20から出力されたセンサ信号を補正する。
【0193】
そして、ステップ352において、上記ステップ350で補正した上下加速度センサ14の検出値に応じたセンサ信号について、姿勢角推定を開始してから得られた、補正された上下加速度センサ14の検出値の履歴のうち、検出値と標準値との差の絶対値が最小になるか否かを判定する。上記ステップ350で補正した上下加速度センサ14の検出値に応じたセンサ信号について、検出値と標準値との差の絶対値が最小にならない場合には、ステップ356へ移行する。一方、上記ステップ352において、上記ステップ350で補正した上下加速度センサ14の検出値に応じたセンサ信号について、検出値と標準値との差の絶対値が最小になる場合には、ステップ354において、標準値との差の絶対値の最小値として、上記ステップ350で補正した上下加速度センサ14の検出値に応じたセンサ信号の検出値を記憶する。
【0194】
次のステップ356では、上記ステップ350で補正したロール角速度センサ20の検出値に応じたセンサ信号について、姿勢角推定を開始してから得られた、補正されたロール角速度センサ20の検出値の履歴のうち、検出値の絶対値が最小になるか否かを判定する。上記ステップ350で補正したロール角速度センサ20の検出値に応じたセンサ信号について、検出値の絶対値が最小にならない場合には、ステップ360へ移行する。一方、上記ステップ356において、上記ステップ350で補正したロール角速度センサ20の検出値に応じたセンサ信号について、検出値の絶対値が最小になる場合には、ステップ358において、検出値の絶対値の最小値として、上記ステップ350で補正したロール角速度センサ20からのセンサ信号の検出値を記憶する。
【0195】
そして、ステップ360では、上記ステップ350で補正した上下加速度センサ14の検出値に応じたセンサ信号について、予め定められたセンサドリフト最大量、及び姿勢角の推定を開始してから現時点までの履歴における標準値との差の絶対値の最小値の何れか小さい方の分だけ、検出値と標準値との差の絶対値を小さくするようにセンサ信号をさらに補正する。また、上記ステップ350で補正したロール角速度センサ20の検出値に応じたセンサ信号について、予め定められたセンサドリフト最大量、及び姿勢角の推定を開始してから現時点までの履歴における検出値の絶対値の最値の何れか小さい方の分だけ、検出値の絶対値を小さくするようにセンサ信号をさらに補正する。
【0196】
そして、ステップ362で、後述のステップ368で1演算周期前に得られた姿勢角推定値と、カルマンフィルタ等を用いて推定される横車体速度と、各輪の車輪速度に基づいて推定される前後車体速度とを用いて、上記で説明したように、姿勢角オブザーバ324によってロール角及びピッチ角を推定するために算出されるロール角及びピッチ角の各々の微分量を算出する。
【0197】
そして、ステップ364において、後述のステップ368で1演算周期前に得られた姿勢角推定値と、推定された横車体速度及び前後車体速度とを用いて、上記で説明したように、上記ステップ362で算出されたロール角及びピッチ角の各々の微分量と、車両運動の運動方程式から得られるロール角及びピッチ角の各々の微分量にセンサドリフト量を考慮した値とが等しくなる関係を用いて、上下加速度センサ14のセンサドリフト量及びロール角速度センサ20のセンサドリフト量を推定する。
【0198】
次のステップ366では、上記ステップ364で推定された上下加速度センサ14のセンサドリフト量及びロール角速度センサ20のセンサドリフト量を、前回のセンサドリフト量の推定値を用いて平滑化する。
【0199】
次のステップ368では、上記ステップ362で算出されたロール角及びピッチ角の各々の微分量を積分することで、ロール角及びピッチ角を推定して出力し、上記ステップ100へ戻る。
【0200】
次に、本発明の第4の実施の形態について説明する。なお、第1の実施の形態〜第3の実施の形態と同様の構成となる部分については、同一符号を付して説明を省略する。
【0201】
第4の実施の形態では、各種センサに加えて、ピッチ角速度センサを用いて、姿勢角を推定している点と、ピッチ角速度センサのセンサドリフト量を推定している点と、ピッチ角速度センサのセンサ信号に対して初期不感帯を設定するように補正している点とが、第3の実施の形態と主に異なっている。
【0202】
図15に示すように、第4の実施の形態の姿勢角推定装置410には、上下加速度センサ14、横加速度センサ16、前後加速度センサ18、ロール角速度センサ20、ヨー角速度センサ22、及びピッチ角速度センサ220が設けられている。
【0203】
ピッチ角速度センサ220及びロール角速度センサ20は、後述するドリフト量推定手段428によって推定されたセンサドリフト量に基づいて、各センサからのセンサ信号を補正するドリフト量補正手段422に接続されている。ドリフト量補正手段422は、ドリフト量推定手段428に接続されている。
【0204】
ドリフト量補正手段422は、初期不感帯を設けるように、ドリフト量補正手段422によって補正された、ピッチ角速度センサ220及びロール角速度センサ20からの各検出値に応じた各センサ信号を補正する初期不感帯補正手段423に接続されている。初期不感帯補正手段423は、車体の鉛直軸に対する姿勢角であるロール角φ及びピッチ角θを推定する姿勢角オブザーバ424に接続されている。
【0205】
ピッチ角速度センサ220、ロール角速度センサ20、ヨー角速度センサ22、及び姿勢角オブザーバ324は、車両運動の運動方程式により得られるロール角及びピッチ角の各々の微分量を算出する運動方程式微分量算出手段426に接続されている。
【0206】
姿勢角オブザーバ424には、初期不感帯補正手段423、上下加速度センサ14、横加速度センサ16、前後加速度センサ18、及びヨー角速度センサ22が接続されている。
【0207】
初期不感帯補正手段423は、ドリフト量補正手段422によって補正されたロール角速度の検出値に応じたセンサ信号について、ロール角速度センサ20について予め定められたセンサドリフト最大量、及び姿勢角の推定を開始してから現時点までの履歴における、ドリフト量補正手段422により補正されたセンサ信号の絶対値の最小値の何れか小さい方を、初期不感帯の幅として設定し、初期不感帯の幅の分だけ、絶対値を小さくするように、ドリフト量補正手段422によって補正されたロール角速度センサ20からのセンサ信号を補正する。
【0208】
また、初期不感帯補正手段423は、ドリフト量補正手段422によって補正されたピッチ角速度の検出値に応じたセンサ信号について、ピッチ角速度センサ220について予め定められたセンサドリフト最大量、及び姿勢角の推定を開始してから現時点までの履歴における、ドリフト量補正手段422により補正されたセンサ信号の絶対値の最小値の何れか小さい方を、初期不感帯の幅として設定し、初期不感帯の幅の分だけ、絶対値を小さくするように、ドリフト量補正手段422によって補正されたピッチ角速度センサ220からのセンサ信号を補正する。
【0209】
姿勢角オブザーバ424は、車両横方向の車両速度である横車体速度Vを推定すると共に、各輪の車輪速度に基づいて車両前後方向の車両速度である前後車体速度Uを推定する。
【0210】
また、姿勢角オブザーバ424は、上記の第3の実施の形態で説明した(33)式を以下の(60)式に置き換えて、ドリフト量補正手段322で補正された後に初期不感帯補正手段423によって補正された、車両運動のピッチ角速度Q及びロール角速度Pの各検出値に応じた補正信号Q−Qdr、P−Pdrと、車両運動の前後加速度G、横加速度G、上下加速度G、及びヨー角速度Rの各検出値に応じたセンサ信号と、前後車体速度Uの推定値Vsoとを用いて、車体鉛直方向に対する姿勢角であるロール角の微分量dφのチルダとピッチ角の微分量dθのチルダとを算出する。また、姿勢角オブザーバ424は、算出したロール角の微分量dφのチルダ及びピッチ角の微分量dθのチルダの各々を積分することにより、ロール角φのチルダ及びピッチ角θのチルダを算出することができる。
【0211】
【数36】

【0212】
運動方程式微分量算出手段426は、上記(39)式、(40)式を、以下の(61)式、(62)式に置き換えて、車両運動の運動方程式より得られるロール角φ及びピッチ角θの微分量を算出する。
【0213】
【数37】

【0214】
ただし、ロール角φ及びピッチ角θには、姿勢角オブザーバ424によって推定されたロール角φ及びピッチ角θの推定値を用いる。
【0215】
ドリフト量推定手段428は、姿勢角オブザーバ424で算出された姿勢角の微分量と運動方程式微分量算出手段426で算出された姿勢角の微分量との比較から、センサドリフト量を推定する。
【0216】
次に、センサドリフト量の推定方法について説明する。
【0217】
上記(4)式、(5)式において、センサ信号G,G,P,Q,Rに所定のドリフト誤差Gxdr,Gydr,Pdr,Qdr,Rdrが重畳していると仮定すると、以下の(63)式,(64)式が得られる。
【0218】
【数38】

【0219】
また、上記(63)式、(64)式が表す微分量について、姿勢角オブザーバ424によって真値が既知であると仮定すると、以下の(65)式、(66)式で表される関係が成立する。
【0220】
【数39】

【0221】
ただし、dφ、dθは、上記(60)式により得られるオブザーバ内部演算値としてのロール角の微分量、ピッチ角の微分量である。
【0222】
上記(65)式、(66)式は、ロール角速度センサ20及びピッチ角速度センサ220のセンサ信号のセンサドリフト量を考慮したときに、姿勢角オブザーバ424により算出されたロール角及びピッチ角の微分量と、運動方程式により得られるロール角及びピッチ角の微分量にセンサドリフト量を考慮した値とが等しくなる関係を表わしている。
【0223】
ここでは、上記(65)式、(66)式におけるヨー角速度ドリフトの係数が比較的小さく(姿勢角が小さいことを仮定)、影響が無視できることから、ヨー角速度ドリフトの項を無視して以下の(67)式のように整理する。
【0224】
【数40】

【0225】
ロール角速度ドリフトとピッチ角速度ドリフトについては、(67)式左辺の係数行列と右辺のベクトルを一定時間積分した以下の(68)式を解くことによって導出された以下の(69)式によって求めることができる。
【0226】
【数41】

【0227】
【数42】

【0228】
ドリフト量推定手段428は、上記(69)式に従って、姿勢角オブザーバ424で算出されるロール角及びピッチ角の各々の微分量と、運動方程式微分量算出手段426で算出されたロール角及びピッチ角の各々の微分量とに基づいて、ピッチ角速度センサ220のセンサドリフト量及びロール角速度センサ20のセンサドリフト量を推定することができる。
【0229】
また、本実施の形態に係るドリフト量推定手段428では、演算の安定化を図るため、前回の推定値を用いて、以下の(70)式、(71)式に従って、上記(69)式の演算結果により得られるセンサドリフト量の推定値を平滑化する。
【0230】
【数43】

【0231】
ただし、Qdrのチルダ、Pdrのチルダは、平滑化後のセンサドリフト量の推定値であり、λ1は、忘却係数である。
【0232】
なお、上記の実施の形態では、各推定値の前回値を用いて、ピッチ角速度やロール角及びピッチ角を推定する場合を例に説明したが、これに限定されるものではなく、各推定値の前々回値を用いて、ピッチ角速度やロール角及びピッチ角を推定するようにしてもよい。
【図面の簡単な説明】
【0233】
【図1】本発明の第1の実施の形態を示すブロック図である。
【図2】本実施の形態の座標系を示す説明図である。
【図3】ロール角速度センサからのセンサ信号と初期不感帯との関係を示す図である。
【図4】ロール角速度センサからのセンサ信号にドリフト誤差上限値が印加されている場合、センサ信号が真値である場合、及びセンサ信号にドリフト誤差下限値が印加されている場合の各々における、センサ信号と初期不感帯との関係を示す図である。
【図5】ロール角速度センサからのセンサ信号にドリフト誤差上限値が印加されている場合、センサ信号が真値である場合、及びセンサ信号にドリフト誤差下限値が印加されている場合の各々における、センサ信号と初期不感帯との関係を示す図である。
【図6】ロール角速度センサからのセンサ信号にドリフト誤差上限値が印加されている場合、センサ信号が真値である場合、及びセンサ信号にドリフト誤差下限値が印加されている場合の各々における、センサ信号と初期不感帯との関係を示す図である。
【図7】本発明の第1の実施の形態の処理を示す流れ図である。
【図8】本発明の第2の実施の形態を示すブロック図である。
【図9】本発明の第3の実施の形態を示すブロック図である。
【図10】第3の実施の形態におけるロール角及びピッチ角の推定結果を示す線図である。
【図11】第3の実施の形態における上下加速度及びロール角速度のセンサ信号に対するセンサドリフト量推定値及び初期不感帯を示す線図である。
【図12】第3の実施の形態におけるロール角及びピッチ角の推定結果を示す線図である。
【図13】第3の実施の形態における上下加速度及びロール角速度のセンサ信号に対するセンサドリフト量推定値及び初期不感帯を示す線図である。
【図14】本発明の第3の実施の形態の処理を示す流れ図である。
【図15】本発明の第4の実施の形態を示すブロック図である。
【符号の説明】
【0234】
10 横速度推定手段
12 前後速度推定手段
14 上下加速度センサ
16 横加速度センサ
18 前後加速度センサ
20 ロール角速度センサ
22 ヨー角速度センサ
23、223、423 初期不感帯補正手段
24 ピッチ角速度推定手段
26、324、424 姿勢角オブザーバ
220 ピッチ角速度センサ
310、410 姿勢角推定装置
322、422 ドリフト量補正手段
326、426 運動方程式微分量算出手段
328、428 ドリフト量推定手段

【特許請求の範囲】
【請求項1】
各輪の車輪速度に基づいて車両前後方向の車体速度である前後車体速度を推定する前後速度推定手段と、
車両横方向の車体速度である横車体速度を推定する横速度推定手段と、
ピッチ角速度を推定するピッチ角速度推定手段と、
車両運動のロール角速度の検出値に応じたセンサ信号について、予め定められたセンサドリフト最大量、及び姿勢角の推定を開始してから現時点までにおける前記検出値の絶対値の最小値の何れか小さい方の分だけ、前記検出値の絶対値を小さくするように前記センサ信号を補正する補正手段と、
車両運動の前後加速度、横加速度、上下加速度、及びヨー角速度の各検出値に応じたセンサ信号、前記補正手段によって補正された前記ロール角速度の検出値に応じたセンサ信号、前記前後車体速度の推定値、並びに前記ピッチ角速度の推定値に基づいて、車体の鉛直軸に対する姿勢角であるロール角及びピッチ角を推定する姿勢角推定手段と、
を含む車両姿勢角推定装置であって、
前記ピッチ角速度推定手段が、前記前後車体速度の推定値、前記横車体速度の推定値、前記上下加速度の検出値、前記補正された前記ロール角速度の検出値、前記姿勢角推定手段で推定された前記ロール角の前回推定値、及び前記姿勢角推定手段で推定された前記ピッチ角の前回推定値に基づいて、前記ピッチ角速度を推定する車両姿勢角推定装置。
【請求項2】
各輪の車輪速度に基づいて車両前後方向の車体速度である前後車体速度を推定する前後速度推定手段と、
車両横方向の車体速度である横車体速度を推定する横速度推定手段と、
ピッチ角速度を推定するピッチ角速度推定手段と、
車両運動の上下加速度の検出値に応じたセンサ信号について、予め定められたセンサドリフト最大量、及び姿勢角の推定を開始してから現時点までにおける前記検出値と標準値との差の絶対値の最小値の何れか小さい方の分だけ、前記検出値と前記標準値との差の絶対値を小さくするように前記センサ信号を補正する補正手段と、
車両運動の前後加速度、横加速度、ヨー角速度、及びロール角速度の各検出値に応じたセンサ信号、前記補正手段によって補正された前記上下加速度の検出値に応じたセンサ信号、前記前後車体速度の推定値、並びに前記ピッチ角速度の推定値に基づいて、車体の鉛直軸に対する姿勢角であるロール角及びピッチ角を推定する姿勢角推定手段と、
を含む車両姿勢角推定装置であって、
前記ピッチ角速度推定手段が、前記前後車体速度の推定値、前記横車体速度の推定値、前記補正された前記上下加速度の検出値、前記ロール角速度の検出値、前記姿勢角推定手段で推定された前記ロール角の前回推定値、及び前記姿勢角推定手段で推定された前記ピッチ角の前回推定値に基づいて、前記ピッチ角速度を推定する車両姿勢角推定装置。
【請求項3】
各輪の車輪速度に基づいて車両前後方向の車体速度である前後車体速度を推定する前後速度推定手段と、
車両横方向の車体速度である横車体速度を推定する横速度推定手段と、
車両運動のピッチ角速度の検出値に応じたセンサ信号について、予め定められたセンサドリフト最大量、及び姿勢角の推定を開始してから現時点までにおける前記検出値の絶対値の最小値の何れか小さい方の分だけ、前記検出値の絶対値を小さくするように前記センサ信号を補正する補正手段と、
車両運動の前後加速度、横加速度、上下加速度、ヨー角速度、及びロール角速度の各検出値に応じたセンサ信号、前記補正手段によって補正された前記ピッチ角速度の検出値に応じたセンサ信号、並びに前記前後車体速度の推定値に基づいて、車体の鉛直軸に対する姿勢角であるロール角及びピッチ角を推定する姿勢角推定手段と、
を含む車両姿勢角推定装置。
【請求項4】
前記補正手段は、上下加速度の検出値に応じたセンサ信号を補正すると共に、前記ロール角速度の検出値に応じたセンサ信号について、予め定められたセンサドリフト最大量、及び姿勢角の推定を開始してから現時点までにおける前記検出値の絶対値の最小値の何れか小さい方の分だけ、前記検出値の絶対値を小さくするように前記センサ信号を補正し、
前記ピッチ角速度推定手段は、前記前後車体速度の推定値、前記横車体速度の推定値、前記補正された前記上下加速度の検出値、前記補正された前記ロール角速度の検出値、前記姿勢角推定手段で推定された前記ロール角の前回推定値、及び前記姿勢角推定手段で推定された前記ピッチ角の前回推定値に基づいて、前記ピッチ角速度を推定し、
前記姿勢角推定手段は、車両運動の前後加速度、横加速度、及びヨー角速度の各検出値に応じたセンサ信号、前記補正手段によって補正された前記上下加速度の検出値に応じたセンサ信号、前記補正手段によって補正された前記ロール角速度の検出値に応じたセンサ信号、前記前後車体速度の推定値、並びに前記ピッチ角速度の推定値に基づいて、車体の鉛直軸に対する姿勢角であるロール角及びピッチ角を推定する請求項2記載の車両姿勢角推定装置。
【請求項5】
前記補正手段は、ピッチ角速度の検出値に応じたセンサ信号を補正すると共に、前記ロール角速度の検出値に応じたセンサ信号について、予め定められたセンサドリフト最大量、及び姿勢角の推定を開始してから現時点までにおける前記検出値の絶対値の最小値の何れか小さい方の分だけ、前記検出値の絶対値を小さくするように前記センサ信号を補正し、
前記姿勢角推定手段は、車両運動の前後加速度、横加速度、上下加速度、及びヨー角速度の各検出値に応じたセンサ信号、前記補正手段によって補正された前記ピッチ角速度の検出値に応じたセンサ信号、前記補正手段によって補正された前記ロール角速度の検出値に応じたセンサ信号、並びに前記前後車体速度の推定値に基づいて、車体の鉛直軸に対する姿勢角であるロール角及びピッチ角を推定する請求項3記載の車両姿勢角推定装置。
【請求項6】
前記補正手段は、ピッチ角速度の検出値に応じたセンサ信号及び前記ロール角速度の検出値に応じたセンサ信号を補正すると共に、前記上下加速度の検出値に応じたセンサ信号について、予め定められたセンサドリフト最大量、及び姿勢角の推定を開始してから現時点までにおける前記検出値と標準値との差の絶対値の最小値の何れか小さい方の分だけ、前記検出値と前記標準値との差の絶対値を小さくするように前記センサ信号を補正し、
前記姿勢角推定手段は、車両運動の前後加速度、横加速度、及びヨー角速度の各検出値に応じたセンサ信号、前記補正手段によって補正された前記上下加速度の検出値に応じたセンサ信号、前記補正手段によって補正された前記ピッチ角速度の検出値に応じたセンサ信号、前記補正手段によって補正された前記ロール角速度の検出値に応じたセンサ信号、並びに前記前後車体速度の推定値に基づいて、車体の鉛直軸に対する姿勢角であるロール角及びピッチ角を推定する請求項5記載の車両姿勢角推定装置。
【請求項7】
前記補正手段は、ピッチ角速度の検出値に応じたセンサ信号を補正すると共に、前記上下加速度の検出値に応じたセンサ信号について、予め定められたセンサドリフト最大量、及び姿勢角の推定を開始してから現時点までにおける前記検出値と標準値との差の絶対値の最小値の何れか小さい方の分だけ、前記検出値と前記標準値との差の絶対値を小さくするように前記センサ信号を補正し、
前記姿勢角推定手段は、車両運動の前後加速度、横加速度、ロール角速度、及びヨー角速度の各検出値に応じたセンサ信号、前記補正手段によって補正された前記ピッチ角速度の検出値に応じたセンサ信号、前記補正手段によって補正された前記上下加速度の検出値に応じたセンサ信号、並びに前記前後車体速度の推定値に基づいて、車体の鉛直軸に対する姿勢角であるロール角及びピッチ角を推定する請求項3記載の車両姿勢角推定装置。
【請求項8】
車両運動のロール角速度の検出値に応じたセンサ信号について、予め定められたセンサドリフト最大量、及び姿勢角の推定を開始してから現時点までにおける前記検出値の絶対値の最小値の何れか小さい方の分だけ、前記検出値の絶対値を小さくするように前記センサ信号を補正すると共に、上下加速度の検出値に応じたセンサ信号について、予め定められたセンサドリフト最大量、及び姿勢角の推定を開始してから現時点までにおける前記検出値と標準値との差の絶対値の最小値の何れか小さい方の分だけ、前記検出値と前記標準値との差の絶対値を小さくするように前記センサ信号を補正する補正手段と、
車両運動のヨー角速度、前後加速度、及び横加速度の各検出値に応じたセンサ信号と、前記補正手段によって補正された前記ロール角速度及び前記上下加速度の各検出値に応じたセンサ信号とに基づいて、車体の鉛直軸に対するロール角及びピッチ角の各々の微分量を算出し、算出した前記ロール角及び前記ピッチ角の各々の微分量を積分して、前記ロール角及び前記ピッチ角を推定する姿勢角推定手段と、
前記上下加速度、前記前後加速度、前記横加速度、前記ロール角速度、及び前記ヨー角速度の各検出値に応じたセンサ信号と、前記姿勢角推定手段によって推定された前記ロール角及び前記ピッチ角とに基づいて、車両運動の運動方程式より得られる前記ロール角及び前記ピッチ角の各々の微分量を算出する算出手段と、
前記センサ信号のセンサドリフト量を考慮したときに、前記姿勢角推定手段により算出された前記ロール角及び前記ピッチ角の各々の微分量と、前記算出手段により算出された前記ロール角及び前記ピッチ角の各々の微分量に前記センサドリフト量を考慮した値とが等しくなる関係を用いて、前記上下加速度の検出値に応じたセンサ信号及び前記ロール角速度の検出値に応じたセンサ信号の各々のセンサドリフト量を推定するドリフト量推定手段と、
を含むセンサドリフト量推定装置。
【請求項9】
車両運動のロール角速度の検出値に応じたセンサ信号について、予め定められたセンサドリフト最大量、及び姿勢角の推定を開始してから現時点までにおける前記検出値の絶対値の最小値の何れか小さい方の分だけ、前記検出値の絶対値を小さくするように前記センサ信号を補正すると共に、ピッチ角速度の検出値に応じたセンサ信号について、予め定められたセンサドリフト最大量、及び姿勢角の推定を開始してから現時点までにおける前記検出値の絶対値の最小値の何れか小さい方の分だけ、前記検出値の絶対値を小さくするように前記センサ信号を補正する補正手段と、
車両運動の上下加速度、前後加速度、横加速度、及びヨー角速度の各検出値に応じたセンサ信号と、前記補正手段によって補正された前記ロール角速度及び前記ピッチ角速度の各検出値に応じたセンサ信号とに基づいて、車体の鉛直軸に対するロール角及びピッチ角の各々の微分量を算出し、算出した前記ロール角及び前記ピッチ角の各々の微分量を積分して、前記ロール角及び前記ピッチ角を推定する姿勢角推定手段と、
前記ロール角速度、前記ヨー角速度、及び前記ピッチ角速度の各検出値に応じたセンサ信号と、前記姿勢角推定手段によって推定された前記ロール角及び前記ピッチ角とに基づいて、車両運動の運動方程式より得られる前記ロール角及び前記ピッチ角の各々の微分量を算出する算出手段と、
前記センサ信号のセンサドリフト量を考慮したときに、前記姿勢角推定手段により算出された前記ロール角及び前記ピッチ角の各々の微分量と、前記算出手段により算出された前記ロール角及び前記ピッチ角の各々の微分量に前記センサドリフト量を考慮した値とが等しくなる関係を用いて、前記ピッチ角速度の検出値に応じたセンサ信号及び前記ロール角速度の検出値に応じたセンサ信号の各々のセンサドリフト量を推定するドリフト量推定手段と、
を含むセンサドリフト量推定装置。
【請求項10】
前記ドリフト量推定手段によって推定された前記センサドリフト量に基づいて、前記上下加速度の検出値に応じたセンサ信号及び前記ロール角速度の検出値に応じたセンサ信号の各々を補正するドリフト補正手段を更に含み、
前記姿勢角推定手段は、前記ヨー角速度の検出値に応じたセンサ信号と、前記ドリフト補正手段によって補正され、かつ、前記補正手段によって補正された前記ロール角速度及び前記上下加速度の各検出値に応じたセンサ信号とに基づいて、前記ロール角及び前記ピッチ角を推定する請求項8記載のセンサドリフト量推定装置。
【請求項11】
前記ドリフト量推定手段によって推定された前記センサドリフト量に基づいて、前記ピッチ角速度の検出値に応じたセンサ信号及び前記ロール角速度の検出値に応じたセンサ信号の各々を補正するドリフト補正手段を更に含み、
前記姿勢角推定手段は、前記上下加速度、前記前後加速度、前記横加速度、及び前記ヨー角速度の各検出値に応じたセンサ信号と、前記ドリフト補正手段によって補正され、かつ、前記補正手段によって補正された前記ロール角速度及び前記ピッチ角速度の各検出値に応じたセンサ信号とに基づいて、前記ロール角及び前記ピッチ角を推定する請求項9記載のセンサドリフト量推定装置。

【図1】
image rotate

【図2】
image rotate

【図3】
image rotate

【図4】
image rotate

【図5】
image rotate

【図6】
image rotate

【図7】
image rotate

【図8】
image rotate

【図9】
image rotate

【図10】
image rotate

【図11】
image rotate

【図12】
image rotate

【図13】
image rotate

【図14】
image rotate

【図15】
image rotate


【公開番号】特開2010−43945(P2010−43945A)
【公開日】平成22年2月25日(2010.2.25)
【国際特許分類】
【出願番号】特願2008−208153(P2008−208153)
【出願日】平成20年8月12日(2008.8.12)
【出願人】(000003609)株式会社豊田中央研究所 (4,200)
【出願人】(000003207)トヨタ自動車株式会社 (59,920)
【Fターム(参考)】