(57)【要約】【課題】車両運動の状態に関わらず、安定してセンサのドリフト量を推定することができるようにする。【解決手段】センサ信号のセンサドリフト量を考慮したときに、姿勢角オブザーバ２６により算出された姿勢角の微分量と、運動方程式微分量算出手段３０により算出された姿勢角の微分量にセンサドリフト量を考慮した値とが等しくなる関係、スリップ角オブザーバ２８により算出された車両速度の微分量と、運動方程式微分量算出手段３０により算出された車両速度の微分量にセンサドリフト量を考慮した値とが等しくなる関係、及び車体ヨー角算出手段４４により算出された車体ヨー角と、運動方程式微分量算出手段３０により算出された車体ヨー角の微分量にセンサドリフト量を考慮した値を積分した値とが等しくなる関係を用いて、ドリフト量推定手段３２によって、センサ信号のセンサドリフト量を推定する。【選択図】図１
2014240266.tif 000002
【特許請求の範囲】
【請求項１】
車両運動の運動状態量の検出値に応じたセンサ信号に基づいて、車体の鉛直軸に対する姿勢角の微分量を算出し、算出した前記姿勢角の微分量を積分して、前記姿勢角を推定する姿勢角推定手段と、
前記センサ信号に基づいて、車両速度の微分量を算出し、前記車両速度の微分量を積分して、前記車両速度を推定し、前記推定された前記車両速度に基づいて、スリップ角を推定するスリップ角推定手段と、
前記センサ信号及び前記姿勢角推定手段によって推定された前記姿勢角に基づいて、車両運動の運動方程式より得られる前記姿勢角の微分量を算出し、前記センサ信号及び前記スリップ角推定手段により推定された前記車両速度に基づいて、車両運動の運動方程式より得られる前記車両速度の微分量を算出し、前記センサ信号、前記姿勢角推定手段によって推定された前記姿勢角、及び前記スリップ角推定手段により推定された前記車両速度に基づいて、車両運動の運動方程式より得られる、車体上下方向軸まわりの回転角の微分量を算出する算出手段と、
複数の情報発信源の各々から発信された、前記情報発信源の各々の位置に関する情報、前記情報発信源の各々と車両との距離に関する情報、及び前記情報発信源の各々に対する前記車両の相対速度に関する情報を含む発信源情報を取得する取得手段と、
前記取得手段により取得された前記発信源情報に基づいて、前記車両の速度ベクトルを推定する車両速度ベクトル推定手段と、
前記姿勢角推定手段によって推定された前記姿勢角、前記スリップ角推定手段によって推定された前記スリップ角、及び前記車両速度ベクトル推定手段によって推定された前記車両の速度ベクトルに基づいて、前記回転角を算出する車体回転角算出手段と、
前記センサ信号のセンサドリフト量を考慮したときに、前記姿勢角推定手段により算出された前記姿勢角の微分量と、前記算出手段により算出された前記姿勢角の微分量に前記センサドリフト量を考慮した値とが等しくなる関係、前記スリップ角推定手段により算出された前記車両速度の微分量と、前記算出手段により算出された前記車両速度の微分量に前記センサドリフト量を考慮した値とが等しくなる関係、及び前記車体回転角算出手段により算出された前記回転角と、前記算出手段により算出された前記回転角の微分量に前記センサドリフト量を考慮した値を積分した値とが等しくなる関係を用いて、前記センサ信号のセンサドリフト量を推定するドリフト量推定手段と、
を含むセンサドリフト量推定装置。
【請求項２】
前記ドリフト量推定手段によって推定された前記センサドリフト量に基づいて、前記センサ信号を補正する補正手段を更に含み、
前記姿勢角推定手段は、前記補正手段によって補正された前記センサ信号に基づいて、前記姿勢角を推定し、
前記スリップ角推定手段は、前記補正手段によって補正された前記センサ信号に基づいて、前記スリップ角を推定する請求項１記載のセンサドリフト量推定装置。
【請求項３】
車両運動の上下加速度、前後加速度、横加速度、ロール角速度、及びヨー角速度の各検出値に応じたセンサ信号に基づいて、車体の鉛直軸に対するロール角及びピッチ角の各々の微分量を算出し、算出した前記ロール角及び前記ピッチ角の各々の微分量を積分して、前記ロール角及び前記ピッチ角を推定する姿勢角推定手段と、
各検出値に応じたセンサ信号に基づいて、前後速度及び横速度の各々の微分量を算出し、前記前後速度及び前記横速度の各々の微分量を積分して、前記前後速度及び前記横速度を推定し、前記推定された前記前後速度及び前記横速度に基づいて、スリップ角を推定するスリップ角推定手段と、
前記センサ信号と、前記姿勢角推定手段によって推定された前記ロール角及び前記ピッチ角とに基づいて、車両運動の運動方程式より得られる前記ロール角及び前記ピッチ角の各々の微分量を算出し、前記センサ信号と、前記スリップ角推定手段により推定された前記前後速度及び前記横速度とに基づいて、車両運動の運動方程式より得られる前記前後速度及び前記横速度の各々の微分量を算出し、前記センサ信号、前記姿勢角推定手段によって推定された前記ロール角及び前記ピッチ角、及び前記スリップ角推定手段により推定された前記前後速度及び前記横速度に基づいて、車両運動の運動方程式より得られる、車体上下方向軸まわりの車体ヨー角の微分量を算出する算出手段と、
複数の情報発信源の各々から発信された、前記情報発信源の各々の位置に関する情報、前記情報発信源の各々と車両との距離に関する情報、及び前記情報発信源の各々に対する前記車両の相対速度に関する情報を含む発信源情報を取得する取得手段と、
前記取得手段により取得された前記発信源情報に基づいて、前記車両の速度ベクトルを推定する車両速度ベクトル推定手段と、
前記姿勢角推定手段によって推定された前記ロール角及び前記ピッチ角、前記スリップ角推定手段によって推定された前記スリップ角、及び前記車両速度ベクトル推定手段によって推定された前記車両の速度ベクトルに基づいて、前記車体ヨー角を算出する車体ヨー角算出手段と、
前記センサ信号のセンサドリフト量を考慮したときに、前記姿勢角推定手段により算出された前記ロール角及び前記ピッチ角の各々の微分量と、前記算出手段により算出された前記ロール角及び前記ピッチ角の各々の微分量に前記センサドリフト量を考慮した値とが等しくなる関係、前記スリップ角推定手段により算出された前記前後速度及び前記横速度の各々の微分量と、前記算出手段により算出された前記前後速度及び前記横速度の各々の微分量に前記センサドリフト量を考慮した値とが等しくなる関係、及び前記車体ヨー角算出手段により算出された前記車体ヨー角と、前記算出手段により算出された前記車体ヨー角の微分量に前記センサドリフト量を考慮した値を積分した値とが等しくなる関係を用いて、前記上下加速度、前記前後加速度、前記横加速度、前記ロール角速度、及び前記ヨー角速度の各検出値に応じたセンサ信号のセンサドリフト量を推定するドリフト量推定手段と、
を含むセンサドリフト量推定装置。
【請求項４】
前記ドリフト量推定手段は、前記上下加速度、前記前後加速度、前記横加速度、前記ロール角速度、及び前記ヨー角速度の各検出値に応じたセンサ信号のセンサドリフト量を推定し、前記推定された前記センサドリフト量と前回推定された前記センサドリフト量とに基づいて、前記センサドリフト量を更新する請求項３記載のセンサドリフト量推定装置。
【請求項５】
前記ドリフト量推定手段は、前記推定された前記ヨー角速度に応じたセンサ信号のセンサドリフト量を用いて該センサドリフト量を更新することにより、前記姿勢角推定手段によって推定される前記ロール角の絶対値が小さくなる場合のみ、前記推定された前記前後速度が小さいほど、該センサドリフト量の更新における、前記推定された前記ヨー角速度に応じたセンサ信号のセンサドリフト量の影響度合いを小さくして、前記ヨー角速度に応じたセンサ信号のセンサドリフト量を更新する請求項４記載のセンサドリフト量推定装置。
【請求項６】
前記ドリフト量推定手段は、前記車体ヨー角算出手段により算出された前記車体ヨー角と、前記算出手段により算出された前記車体ヨー角の微分量を積分した値との差分の絶対値が、予め定められた値未満である場合のみ、前記推定された前記ヨー角速度に応じたセンサ信号のセンサドリフト量と前回推定された前記ヨー角速度に応じたセンサ信号のセンサドリフト量とに基づいて、前記ヨー角速度に応じたセンサ信号のセンサドリフト量を更新する請求項４記載のセンサドリフト量推定装置。
【請求項７】
前記ドリフト量推定手段は、前記推定された前記横加速度に応じたセンサ信号のセンサドリフト量を用いて該センサドリフト量を更新することにより、前記姿勢角推定手段によって推定される前記ロール角の絶対値が小さくなる場合のみ、前記推定された前記横加速度に応じたセンサ信号のセンサドリフト量と前回推定された前記横加速度に応じたセンサ信号のセンサドリフト量とに基づいて、前記横加速度に応じたセンサ信号のセンサドリフト量を更新する請求項４〜請求項６の何れか１項記載のセンサドリフト量推定装置。
【請求項８】
前記ドリフト量推定手段は、前記推定された前記前後加速度に応じたセンサ信号のセンサドリフト量を用いて該センサドリフト量を更新することにより、前記姿勢角推定手段によって推定される前記ピッチ角の絶対値が小さくなる場合のみ、前記推定された前記前後加速度に応じたセンサ信号のセンサドリフト量と前回推定された前記前後加速度に応じたセンサ信号のセンサドリフト量とに基づいて、前記前後加速度に応じたセンサ信号のセンサドリフト量を更新する請求項４〜請求項７の何れか１項記載のセンサドリフト量推定装置。
【請求項９】
前記ドリフト量推定手段によって推定された前記センサドリフト量に基づいて、各検出値に応じたセンサ信号を補正する補正手段を更に含み、
前記姿勢角推定手段は、前記補正手段によって補正された各検出値に応じたセンサ信号に基づいて、前記ロール角及び前記ピッチ角を推定し、
前記スリップ角推定手段は、前記補正手段によって補正された各検出値に応じたセンサ信号に基づいて、前記前後速度及び前記横速度を推定し、推定された前記前後速度及び前記横速度に基づいて、スリップ角を推定する請求項４〜請求項８の何れか１項記載のセンサドリフト量推定装置。
【請求項１０】
コンピュータを、
車両運動の運動状態量の検出値に応じたセンサ信号に基づいて、車体の鉛直軸に対する姿勢角の微分量を算出し、算出した前記姿勢角の微分量を積分して、前記姿勢角を推定する姿勢角推定手段、
前記センサ信号に基づいて、車両速度の微分量を算出し、前記車両速度の微分量を積分して、前記車両速度を推定し、前記推定された前記車両速度に基づいて、スリップ角を推定するスリップ角推定手段、
前記センサ信号及び前記姿勢角推定手段によって推定された前記姿勢角に基づいて、車両運動の運動方程式より得られる前記姿勢角の微分量を算出し、前記センサ信号及び前記スリップ角推定手段により推定された前記車両速度に基づいて、車両運動の運動方程式より得られる前記車両速度の微分量を算出し、前記センサ信号、前記姿勢角推定手段によって推定された前記姿勢角、及び前記スリップ角推定手段により推定された前記車両速度に基づいて、車両運動の運動方程式より得られる、車体上下方向軸まわりの回転角の微分量を算出する算出手段、
複数の情報発信源の各々から発信された、前記情報発信源の各々の位置に関する情報、前記情報発信源の各々と車両との距離に関する情報、及び前記情報発信源の各々に対する前記車両の相対速度に関する情報を含む発信源情報を取得する取得手段、
前記取得手段により取得された前記発信源情報に基づいて、前記車両の速度ベクトルを推定する車両速度ベクトル推定手段、
前記姿勢角推定手段によって推定された前記姿勢角、前記スリップ角推定手段によって推定された前記スリップ角、及び前記車両速度ベクトル推定手段によって推定された前記車両の速度ベクトルに基づいて、前記回転角を算出する車体回転角算出手段、
前記センサ信号のセンサドリフト量を考慮したときに、前記姿勢角推定手段により算出された前記姿勢角の微分量と、前記算出手段により算出された前記姿勢角の微分量に前記センサドリフト量を考慮した値とが等しくなる関係、前記スリップ角推定手段により算出された前記車両速度の微分量と、前記算出手段により算出された前記車両速度の微分量に前記センサドリフト量を考慮した値とが等しくなる関係、及び前記車体回転角算出手段により算出された前記回転角と、前記算出手段により算出された前記回転角の微分量に前記センサドリフト量を考慮した値を積分した値とが等しくなる関係を用いて、前記センサ信号のセンサドリフト量を推定するドリフト量推定手段
として機能させるためのプログラム。
【請求項１１】
コンピュータを、
車両運動の上下加速度、前後加速度、横加速度、ロール角速度、及びヨー角速度の各検出値に応じたセンサ信号に基づいて、車体の鉛直軸に対するロール角及びピッチ角の各々の微分量を算出し、算出した前記ロール角及び前記ピッチ角の各々の微分量を積分して、前記ロール角及び前記ピッチ角を推定する姿勢角推定手段、
各検出値に応じたセンサ信号に基づいて、前後速度及び横速度の各々の微分量を算出し、前記前後速度及び前記横速度の各々の微分量を積分して、前記前後速度及び前記横速度を推定し、前記推定された前記前後速度及び前記横速度に基づいて、スリップ角を推定するスリップ角推定手段、
前記センサ信号と、前記姿勢角推定手段によって推定された前記ロール角及び前記ピッチ角とに基づいて、車両運動の運動方程式より得られる前記ロール角及び前記ピッチ角の各々の微分量を算出し、前記センサ信号と、前記スリップ角推定手段により推定された前記前後速度及び前記横速度とに基づいて、車両運動の運動方程式より得られる前記前後速度及び前記横速度の各々の微分量を算出し、前記センサ信号、前記姿勢角推定手段によって推定された前記ロール角及び前記ピッチ角、及び前記スリップ角推定手段により推定された前記前後速度及び前記横速度に基づいて、車両運動の運動方程式より得られる、車体上下方向軸まわりの車体ヨー角の微分量を算出する算出手段、
複数の情報発信源の各々から発信された、前記情報発信源の各々の位置に関する情報、前記情報発信源の各々と車両との距離に関する情報、及び前記情報発信源の各々に対する前記車両の相対速度に関する情報を含む発信源情報を取得する取得手段、
前記取得手段により取得された前記発信源情報に基づいて、前記車両の速度ベクトルを推定する車両速度ベクトル推定手段、
前記姿勢角推定手段によって推定された前記ロール角及び前記ピッチ角、前記スリップ角推定手段によって推定された前記スリップ角、及び前記車両速度ベクトル推定手段によって推定された前記車両の速度ベクトルに基づいて、前記車体ヨー角を算出する車体ヨー角算出手段、及び
前記センサ信号のセンサドリフト量を考慮したときに、前記姿勢角推定手段により算出された前記ロール角及び前記ピッチ角の各々の微分量と、前記算出手段により算出された前記ロール角及び前記ピッチ角の各々の微分量に前記センサドリフト量を考慮した値とが等しくなる関係、前記スリップ角推定手段により算出された前記前後速度及び前記横速度の各々の微分量と、前記算出手段により算出された前記前後速度及び前記横速度の各々の微分量に前記センサドリフト量を考慮した値とが等しくなる関係、及び前記車体ヨー角算出手段により算出された前記車体ヨー角と、前記算出手段により算出された前記車体ヨー角の微分量に前記センサドリフト量を考慮した値を積分した値とが等しくなる関係を用いて、前記上下加速度、前記前後加速度、前記横加速度、前記ロール角速度、及び前記ヨー角速度の各検出値に応じたセンサ信号のセンサドリフト量を推定するドリフト量推定手段  として機能させるためのプログラム。
【請求項１２】
車両運動の運動状態量の検出値に応じたセンサ信号に基づいて、車体の鉛直軸に対する姿勢角の微分量を算出し、算出した前記姿勢角の微分量を積分して、前記姿勢角を推定する姿勢角推定手段と、
前記センサ信号に基づいて、車両速度の微分量を算出し、前記車両速度の微分量を積分して、前記車両速度を推定し、前記推定された前記車両速度に基づいて、スリップ角を推定するスリップ角推定手段と、
前記センサ信号及び前記姿勢角推定手段によって推定された前記姿勢角に基づいて、車両運動の運動方程式より得られる前記姿勢角の微分量を算出し、前記センサ信号及び前記スリップ角推定手段により推定された前記車両速度に基づいて、車両運動の運動方程式より得られる前記車両速度の微分量を算出し、前記センサ信号、前記姿勢角推定手段によって推定された前記姿勢角、及び前記スリップ角推定手段により推定された前記車両速度に基づいて、車両運動の運動方程式より得られる、車体上下方向軸まわりの回転角の微分量を算出する算出手段と、
前記車体の外部を撮像した複数の画像の各々から複数の特徴点を抽出し、前記抽出した複数の特徴点に基づいて、前記複数の画像を撮像したときの車両の運動を推定し、前記推定された車両の運動に基づいて、前記回転角の微分量を算出する運動推定手段と、
前記センサ信号のセンサドリフト量を考慮したときに、前記姿勢角推定手段により算出された前記姿勢角の微分量と、前記算出手段により算出された前記姿勢角の微分量に前記センサドリフト量を考慮した値とが等しくなる関係、前記スリップ角推定手段により算出された前記車両速度の微分量と、前記算出手段により算出された前記車両速度の微分量に前記センサドリフト量を考慮した値とが等しくなる関係、及び前記運動推定手段により算出された前記回転角の微分量と、前記算出手段により算出された前記回転角の微分量に前記センサドリフト量を考慮した値とが等しくなる関係を用いて、前記センサ信号のセンサドリフト量を推定するドリフト量推定手段と、
を含むセンサドリフト量推定装置。
【請求項１３】
前記ドリフト量推定手段によって推定された前記センサドリフト量に基づいて、前記センサ信号を補正する補正手段を更に含み、
前記姿勢角推定手段は、前記補正手段によって補正された前記センサ信号に基づいて、前記姿勢角を推定し、
前記スリップ角推定手段は、前記補正手段によって補正された前記センサ信号に基づいて、前記スリップ角を推定する請求項１２記載のセンサドリフト量推定装置。
【請求項１４】
車両運動の上下加速度、前後加速度、横加速度、ロール角速度、及びヨー角速度の各検出値に応じたセンサ信号に基づいて、車体の鉛直軸に対するロール角及びピッチ角の各々の微分量を算出し、算出した前記ロール角及び前記ピッチ角の各々の微分量を積分して、前記ロール角及び前記ピッチ角を推定する姿勢角推定手段と、
各検出値に応じたセンサ信号に基づいて、前後速度及び横速度の各々の微分量を算出し、前記前後速度及び前記横速度の各々の微分量を積分して、前記前後速度及び前記横速度を推定し、前記推定された前記前後速度及び前記横速度に基づいて、スリップ角を推定するスリップ角推定手段と、
前記センサ信号と、前記姿勢角推定手段によって推定された前記ロール角及び前記ピッチ角とに基づいて、車両運動の運動方程式より得られる前記ロール角及び前記ピッチ角の各々の微分量を算出し、前記センサ信号と、前記スリップ角推定手段により推定された前記前後速度及び前記横速度とに基づいて、車両運動の運動方程式より得られる前記前後速度及び前記横速度の各々の微分量を算出し、前記センサ信号、前記姿勢角推定手段によって推定された前記ロール角及び前記ピッチ角、及び前記スリップ角推定手段により推定された前記前後速度及び前記横速度に基づいて、車両運動の運動方程式より得られる、車体上下方向軸まわりの車体ヨー角の微分量を算出する算出手段と、
前記車体の外部を撮像した複数の画像の各々から複数の特徴点を抽出し、前記抽出した複数の特徴点に基づいて、前記複数の画像を撮像したときの車両の運動を推定し、前記推定された車両の運動に基づいて、前記車体ヨー角の微分量を算出する運動推定手段と、
前記センサ信号のセンサドリフト量を考慮したときに、前記姿勢角推定手段により算出された前記ロール角及び前記ピッチ角の各々の微分量と、前記算出手段により算出された前記ロール角及び前記ピッチ角の各々の微分量に前記センサドリフト量を考慮した値とが等しくなる関係、前記スリップ角推定手段により算出された前記前後速度及び前記横速度の各々の微分量と、前記算出手段により算出された前記前後速度及び前記横速度の各々の微分量に前記センサドリフト量を考慮した値とが等しくなる関係、及び前記運動推定手段により算出された前記車体ヨー角の微分量と、前記算出手段により算出された前記車体ヨー角の微分量に前記センサドリフト量を考慮した値とが等しくなる関係を用いて、前記上下加速度、前記前後加速度、前記横加速度、前記ロール角速度、及び前記ヨー角速度の各検出値に応じたセンサ信号のセンサドリフト量を推定するドリフト量推定手段と、
を含むセンサドリフト量推定装置。
【請求項１５】
前記ドリフト量推定手段は、前記上下加速度、前記前後加速度、前記横加速度、前記ロール角速度、及び前記ヨー角速度の各検出値に応じたセンサ信号のセンサドリフト量を推定し、前記推定された前記センサドリフト量と前回推定された前記センサドリフト量とに基づいて、前記センサドリフト量を更新する請求項１４記載のセンサドリフト量推定装置。
【請求項１６】
前記ドリフト量推定手段は、前記運動推定手段により算出された前記車体ヨー角の微分量と、前記算出手段により算出された前記車体ヨー角の微分量との差分の絶対値が、予め定められた値未満である場合のみ、前記推定された前記ヨー角速度に応じたセンサ信号のセンサドリフト量と前回推定された前記ヨー角速度に応じたセンサ信号のセンサドリフト量とに基づいて、前記ヨー角速度に応じたセンサ信号のセンサドリフト量を更新する請求項１５記載のセンサドリフト量推定装置。
【請求項１７】
前記ドリフト量推定手段は、前記推定された前記横加速度に応じたセンサ信号のセンサドリフト量を用いて該センサドリフト量を更新することにより、前記姿勢角推定手段によって推定される前記ロール角の絶対値が小さくなる場合のみ、前記推定された前記横加速度に応じたセンサ信号のセンサドリフト量と前回推定された前記横加速度に応じたセンサ信号のセンサドリフト量とに基づいて、前記横加速度に応じたセンサ信号のセンサドリフト量を更新する請求項１５〜請求項１６の何れか１項記載のセンサドリフト量推定装置。
【請求項１８】
前記ドリフト量推定手段は、前記推定された前記前後加速度に応じたセンサ信号のセンサドリフト量を用いて該センサドリフト量を更新することにより、前記姿勢角推定手段によって推定される前記ピッチ角の絶対値が小さくなる場合のみ、前記推定された前記前後加速度に応じたセンサ信号のセンサドリフト量と前回推定された前記前後加速度に応じたセンサ信号のセンサドリフト量とに基づいて、前記前後加速度に応じたセンサ信号のセンサドリフト量を更新する請求項１５〜請求項１７の何れか１項記載のセンサドリフト量推定装置。
【請求項１９】
前記ドリフト量推定手段によって推定された前記センサドリフト量に基づいて、各検出値に応じたセンサ信号を補正する補正手段を更に含み、
前記姿勢角推定手段は、前記補正手段によって補正された各検出値に応じたセンサ信号に基づいて、前記ロール角及び前記ピッチ角を推定し、
前記スリップ角推定手段は、前記補正手段によって補正された各検出値に応じたセンサ信号に基づいて、前記前後速度及び前記横速度を推定し、推定された前記前後速度及び前記横速度に基づいて、スリップ角を推定する請求項１５〜請求項１８の何れか１項記載のセンサドリフト量推定装置。
【請求項２０】
コンピュータを
車両運動の運動状態量の検出値に応じたセンサ信号に基づいて、車体の鉛直軸に対する姿勢角の微分量を算出し、算出した前記姿勢角の微分量を積分して、前記姿勢角を推定する姿勢角推定手段、
前記センサ信号に基づいて、車両速度の微分量を算出し、前記車両速度の微分量を積分して、前記車両速度を推定し、前記推定された前記車両速度に基づいて、スリップ角を推定するスリップ角推定手段、
前記センサ信号及び前記姿勢角推定手段によって推定された前記姿勢角に基づいて、車両運動の運動方程式より得られる前記姿勢角の微分量を算出し、前記センサ信号及び前記スリップ角推定手段により推定された前記車両速度に基づいて、車両運動の運動方程式より得られる前記車両速度の微分量を算出し、前記センサ信号、前記姿勢角推定手段によって推定された前記姿勢角、及び前記スリップ角推定手段により推定された前記車両速度に基づいて、車両運動の運動方程式より得られる、車体上下方向軸まわりの回転角の微分量を算出する算出手段、
前記車体の外部を撮像した複数の画像の各々から複数の特徴点を抽出し、前記抽出した複数の特徴点に基づいて、前記複数の画像を撮像したときの車両の運動を推定し、前記推定された車両の運動に基づいて、前記回転角の微分量を算出する運動推定手段、及び
前記センサ信号のセンサドリフト量を考慮したときに、前記姿勢角推定手段により算出された前記姿勢角の微分量と、前記算出手段により算出された前記姿勢角の微分量に前記センサドリフト量を考慮した値とが等しくなる関係、前記スリップ角推定手段により算出された前記車両速度の微分量と、前記算出手段により算出された前記車両速度の微分量に前記センサドリフト量を考慮した値とが等しくなる関係、及び前記運動推定手段により算出された前記回転角の微分量と、前記算出手段により算出された前記回転角の微分量に前記センサドリフト量を考慮した値とが等しくなる関係を用いて、前記センサ信号のセンサドリフト量を推定するドリフト量推定手段、
として機能させるためのプログラム。
【請求項２１】
コンピュータを、
車両運動の上下加速度、前後加速度、横加速度、ロール角速度、及びヨー角速度の各検出値に応じたセンサ信号に基づいて、車体の鉛直軸に対するロール角及びピッチ角の各々の微分量を算出し、算出した前記ロール角及び前記ピッチ角の各々の微分量を積分して、前記ロール角及び前記ピッチ角を推定する姿勢角推定手段、
各検出値に応じたセンサ信号に基づいて、前後速度及び横速度の各々の微分量を算出し、前記前後速度及び前記横速度の各々の微分量を積分して、前記前後速度及び前記横速度を推定し、前記推定された前記前後速度及び前記横速度に基づいて、スリップ角を推定するスリップ角推定手段、
前記センサ信号と、前記姿勢角推定手段によって推定された前記ロール角及び前記ピッチ角とに基づいて、車両運動の運動方程式より得られる前記ロール角及び前記ピッチ角の各々の微分量を算出し、前記センサ信号と、前記スリップ角推定手段により推定された前記前後速度及び前記横速度とに基づいて、車両運動の運動方程式より得られる前記前後速度及び前記横速度の各々の微分量を算出し、前記センサ信号、前記姿勢角推定手段によって推定された前記ロール角及び前記ピッチ角、及び前記スリップ角推定手段により推定された前記前後速度及び前記横速度に基づいて、車両運動の運動方程式より得られる、車体上下方向軸まわりの車体ヨー角の微分量を算出する算出手段、
前記車体の外部を撮像した複数の画像の各々から複数の特徴点を抽出し、前記抽出した複数の特徴点に基づいて、前記複数の画像を撮像したときの車両の運動を推定し、前記推定された車両の運動に基づいて、前記車体ヨー角の微分量を算出する運動推定手段、及び
前記センサ信号のセンサドリフト量を考慮したときに、前記姿勢角推定手段により算出された前記ロール角及び前記ピッチ角の各々の微分量と、前記算出手段により算出された前記ロール角及び前記ピッチ角の各々の微分量に前記センサドリフト量を考慮した値とが等しくなる関係、前記スリップ角推定手段により算出された前記前後速度及び前記横速度の各々の微分量と、前記算出手段により算出された前記前後速度及び前記横速度の各々の微分量に前記センサドリフト量を考慮した値とが等しくなる関係、及び前記運動推定手段により算出された前記車体ヨー角の微分量と、前記算出手段により算出された前記車体ヨー角の微分量に前記センサドリフト量を考慮した値とが等しくなる関係を用いて、前記上下加速度、前記前後加速度、前記横加速度、前記ロール角速度、及び前記ヨー角速度の各検出値に応じたセンサ信号のセンサドリフト量を推定するドリフト量推定手段、
として機能させるためのプログラム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、センサドリフト量推定装置に係り、特に、車両運動の運動状態量の検出値に応じたセンサ信号のセンサドリフト量を推定するセンサドリフト量推定装置に関する。
【背景技術】
【０００２】
従来より、車載の加速度センサおよび加速度センサを利用して車両の姿勢角を推定する姿勢角推定装置において、車載の角速度センサおよび加速度センサのゼロ点ドリフトは、姿勢角の推定への影響が大きいことから、ゼロ点ドリフトを逐次推定して補償する必要がある。
【０００３】
このようなセンサゼロ点ドリフトを推定する従来技術として、姿勢がほぼ安定して長時間・長距離航行する移動体に用いる場合の角速度および加速度計のドリフト・バイアス誤差を低減する慣性航法システムが提案されている（特許文献１）。
【０００４】
また、ＧＰＳ衛星からの信号を利用したセンサドリフト量の推定方法も提案されている（非特許文献２、３）。非特許文献２では、ＧＰＳ受信機から車両の旋回角を求めるとともに、角速度計の出力データを積算して車両のヨー角を求め、これら２つの角度の差に基づいて車両の走行中でもセンサドリフト量を求めヨー角速度の出力を補正する技術が提案されている。また、特許文献３では、ヨー角速度計またはＧＰＳ受信機から得られるヨー角の微分値が所定の値以下となっている期間のピッチ角速度信号の平均値を求めることでピッチ角速度のセンサドリフト量を推定する手法が記載されている。
【０００５】
また、車両運動の状態に関わらず安定してセンサのドリフト量を推定することができるセンサドリフト量推定手法として、２軸角速度、および３軸加速度センサ信号に基づき、車体鉛直軸に対する姿勢角（ロール角、ピッチ角）を推定する姿勢角推定オブザーバと、オブザーバからの出力とセンサドリフト量を考慮した運動方程式との差からセンサドリフト量を推定する方法が提案されている(特許文献４）。この手法では、ヨー角速度と横加速度のセンサドリフト量を１つの式から推定するため、車速変化を利用し、ヨー角速度と横加速度のセンサドリフト量を配分している。
【０００６】
また、車載カメラから推定されるヨー角速度を利用し、車載ジャイロセンサのヨー角速度センサのゼロ点ドリフトのみを補正する技術についても提案されている（特許文献５、６、７）。特許文献５及び特許文献６の方法は、カメラを用いて車両が直進しているか否かを判定することでジャイロセンサのドリフトを補正する方法である。特許文献７の方法は、画像から得られる旋回時の消失点と直進時の消失点を基にヨーレートを推定し、ジャイロセンサのドリフトを補正する方法である。
【先行技術文献】
【特許文献】
【０００７】
【特許文献１】特開２００７‐１５５８４号公報
【特許文献２】特開平６−２６８６５号公報
【特許文献３】特開２０１１‐１９１２４３号公報
【特許文献４】特開２０１０‐７０１８４号公報
【特許文献５】特開２００６‐１９９２４２号公報
【特許文献６】特開２０１２‐２３３８５２号公報
【特許文献７】特開２０１１‐１６９７２８号公報
【発明の概要】
【発明が解決しようとする課題】
【０００８】
車両運動制御技術の高精度化には、車両運動の状態に関わらず安定してセンサドリフト量を推定する必要がある、という課題がある。
【０００９】
上記の特許文献１に記載の技術では、移動体の姿勢がほぼ一定に安定している状態を判定しドリフト量を推定するため、センサが搭載された物体が静止していない場合や等速運動していない場合には、センサドリフト量を推定することができない、という問題がある。また、上記の特許文献２、３に記載の技術では、車両の姿勢角（ロール角、ピッチ角）や、スリップ角を考慮していないため、カント路やスリップ角が大きくなるような走行では精度の劣化が懸念される、という問題がある。
【００１０】
上記の特許文献４では、上記課題に対し、車両運動の状態に関わらずセンサドリフト量を推定する技術を提案している。しかしながら、ヨー角速度と横加速度のセンサドリフト量の推定に車速変化を利用するため、車速変化の少ない走行条件では推定精度の劣化が懸念される、という問題がある。
【００１１】
本発明は、上記の問題点を解決するためになされたもので、車両運動の状態に関わらず、安定してセンサのドリフト量を精度よく推定することができるセンサドリフト量推定装置及びプログラムを提供することを目的とする。
【課題を解決するための手段】
【００１２】
上記の目的を達成するために第１の発明に係るセンサドリフト量推定装置は、車両運動の運動状態量の検出値に応じたセンサ信号に基づいて、車体の鉛直軸に対する姿勢角の微分量を算出し、算出した前記姿勢角の微分量を積分して、前記姿勢角を推定する姿勢角推定手段と、前記センサ信号に基づいて、車両速度の微分量を算出し、前記車両速度の微分量を積分して、前記車両速度を推定し、前記推定された前記車両速度に基づいて、スリップ角を推定するスリップ角推定手段と、前記センサ信号及び前記姿勢角推定手段によって推定された前記姿勢角に基づいて、車両運動の運動方程式より得られる前記姿勢角の微分量を算出し、前記センサ信号及び前記スリップ角推定手段により推定された前記車両速度に基づいて、車両運動の運動方程式より得られる前記車両速度の微分量を算出し、前記センサ信号、前記姿勢角推定手段によって推定された前記姿勢角、及び前記スリップ角推定手段により推定された前記車両速度に基づいて、車両運動の運動方程式より得られる、車体上下方向軸まわりの回転角の微分量を算出する算出手段と、複数の情報発信源の各々から発信された、前記情報発信源の各々の位置に関する情報、前記情報発信源の各々と前記車両との距離に関する情報、及び前記情報発信源の各々に対する前記車両の相対速度に関する情報を含む発信源情報を取得する取得手段と、前記取得手段により取得された前記発信源情報に基づいて、前記車両の速度ベクトルを推定する車両速度ベクトル推定手段と、前記姿勢角推定手段によって推定された前記姿勢角、前記スリップ角推定手段によって推定された前記スリップ角、及び前記車両速度ベクトル推定手段によって推定された前記車両の速度ベクトルに基づいて、前記回転角を算出する車体回転角算出手段と、前記センサ信号のセンサドリフト量を考慮したときに、前記姿勢角推定手段により算出された前記姿勢角の微分量と、前記算出手段により算出された前記姿勢角の微分量に前記センサドリフト量を考慮した値とが等しくなる関係、前記スリップ角推定手段により算出された前記車両速度の微分量と、前記算出手段により算出された前記車両速度の微分量に前記センサドリフト量を考慮した値とが等しくなる関係、及び前記車体回転角算出手段により算出された前記回転角と、前記算出手段により算出された前記回転角の微分量に前記センサドリフト量を考慮した値を積分した値とが等しくなる関係を用いて、前記センサ信号のセンサドリフト量を推定するドリフト量推定手段とを含んで構成されている。
【００１３】
第２の発明に係るプログラムは、コンピュータを、車両運動の運動状態量の検出値に応じたセンサ信号に基づいて、車体の鉛直軸に対する姿勢角の微分量を算出し、算出した前記姿勢角の微分量を積分して、前記姿勢角を推定する姿勢角推定手段、前記センサ信号に基づいて、車両速度の微分量を算出し、前記車両速度の微分量を積分して、前記車両速度を推定し、前記推定された前記車両速度に基づいて、スリップ角を推定するスリップ角推定手段、前記センサ信号及び前記姿勢角推定手段によって推定された前記姿勢角に基づいて、車両運動の運動方程式より得られる前記姿勢角の微分量を算出し、前記センサ信号及び前記スリップ角推定手段により推定された前記車両速度に基づいて、車両運動の運動方程式より得られる前記車両速度の微分量を算出し、前記センサ信号、前記姿勢角推定手段によって推定された前記姿勢角、及び前記スリップ角推定手段により推定された前記車両速度に基づいて、車両運動の運動方程式より得られる、車体上下方向軸まわりの回転角の微分量を算出する算出手段、複数の情報発信源の各々から発信された、前記情報発信源の各々の位置に関する情報、前記情報発信源の各々と前記車両との距離に関する情報、及び前記情報発信源の各々に対する前記車両の相対速度に関する情報を含む発信源情報を取得する取得手段、前記取得手段により取得された前記発信源情報に基づいて、前記車両の速度ベクトルを推定する車両速度ベクトル推定手段、前記姿勢角推定手段によって推定された前記姿勢角、前記スリップ角推定手段によって推定された前記スリップ角、及び前記車両速度ベクトル推定手段によって推定された前記車両の速度ベクトルに基づいて、前記回転角を算出する車体回転角算出手段、前記センサ信号のセンサドリフト量を考慮したときに、前記姿勢角推定手段により算出された前記姿勢角の微分量と、前記算出手段により算出された前記姿勢角の微分量に前記センサドリフト量を考慮した値とが等しくなる関係、前記スリップ角推定手段により算出された前記車両速度の微分量と、前記算出手段により算出された前記車両速度の微分量に前記センサドリフト量を考慮した値とが等しくなる関係、及び前記車体回転角算出手段により算出された前記回転角と、前記算出手段により算出された前記回転角の微分量に前記センサドリフト量を考慮した値を積分した値とが等しくなる関係を用いて、前記センサ信号のセンサドリフト量を推定するドリフト量推定手段として機能させるためのプログラムである。
【００１４】
第３の発明に係るセンサドリフト量推定装置は、車両運動の上下加速度、前後加速度、横加速度、ロール角速度、及びヨー角速度の各検出値に応じたセンサ信号に基づいて、車体の鉛直軸に対するロール角及びピッチ角の各々の微分量を算出し、算出した前記ロール角及び前記ピッチ角の各々の微分量を積分して、前記ロール角及び前記ピッチ角を推定する姿勢角推定手段と、各検出値に応じたセンサ信号に基づいて、前後速度及び横速度の各々の微分量を算出し、前記前後速度及び前記横速度の各々の微分量を積分して、前記前後速度及び前記横速度を推定し、前記推定された前記前後速度及び前記横速度に基づいて、スリップ角を推定するスリップ角推定手段と、前記センサ信号と、前記姿勢角推定手段によって推定された前記ロール角及び前記ピッチ角とに基づいて、車両運動の運動方程式より得られる前記ロール角及び前記ピッチ角の各々の微分量を算出し、前記センサ信号と、前記スリップ角推定手段により推定された前記前後速度及び前記横速度とに基づいて、車両運動の運動方程式より得られる前記前後速度及び前記横速度の各々の微分量を算出し、前記センサ信号、前記姿勢角推定手段によって推定された前記ロール角及び前記ピッチ角、及び前記スリップ角推定手段により推定された前記前後速度及び前記横速度に基づいて、車両運動の運動方程式より得られる、車体上下方向軸まわりの車体ヨー角の微分量を算出する算出手段と、複数の情報発信源の各々から発信された、前記情報発信源の各々の位置に関する情報、前記情報発信源の各々と前記車両との距離に関する情報、及び前記情報発信源の各々に対する前記車両の相対速度に関する情報を含む発信源情報を取得する取得手段と、前記取得手段により取得された前記発信源情報に基づいて、前記車両の速度ベクトルを推定する車両速度ベクトル推定手段と、前記姿勢角推定手段によって推定された前記姿勢角、前記スリップ角推定手段によって推定された前記スリップ角、及び前記車両速度ベクトル推定手段によって推定された前記車両の速度ベクトルに基づいて、前記車体ヨー角を算出する車体ヨー角算出手段と、前記センサ信号のセンサドリフト量を考慮したときに、前記姿勢角推定手段により算出された前記ロール角及び前記ピッチ角の各々の微分量と、前記算出手段により算出された前記ロール角及び前記ピッチ角の各々の微分量に前記センサドリフト量を考慮した値とが等しくなる関係、前記スリップ角推定手段により算出された前記前後速度及び前記横速度の各々の微分量と、前記算出手段により算出された前記前後速度及び前記横速度の各々の微分量に前記センサドリフト量を考慮した値とが等しくなる関係、及び前記車体ヨー角算出手段により算出された前記車体ヨー角と、前記算出手段により算出された前記車体ヨー角の微分量に前記センサドリフト量を考慮した値を積分した値とが等しくなる関係を用いて、前記上下加速度、前記前後加速度、前記横加速度、前記ロール角速度、及び前記ヨー角速度の各検出値に応じたセンサ信号のセンサドリフト量を推定するドリフト量推定手段とを含んで構成されている。
【００１５】
第４の発明に係るプログラムは、コンピュータを、車両運動の上下加速度、前後加速度、横加速度、ロール角速度、及びヨー角速度の各検出値に応じたセンサ信号に基づいて、車体の鉛直軸に対するロール角及びピッチ角の各々の微分量を算出し、算出した前記ロール角及び前記ピッチ角の各々の微分量を積分して、前記ロール角及び前記ピッチ角を推定する姿勢角推定手段、各検出値に応じたセンサ信号に基づいて、前後速度及び横速度の各々の微分量を算出し、前記前後速度及び前記横速度の各々の微分量を積分して、前記前後速度及び前記横速度を推定し、前記推定された前記前後速度及び前記横速度に基づいて、スリップ角を推定するスリップ角推定手段、前記センサ信号と、前記姿勢角推定手段によって推定された前記ロール角及び前記ピッチ角とに基づいて、車両運動の運動方程式より得られる前記ロール角及び前記ピッチ角の各々の微分量を算出し、前記センサ信号と、前記スリップ角推定手段により推定された前記前後速度及び前記横速度とに基づいて、車両運動の運動方程式より得られる前記前後速度及び前記横速度の各々の微分量を算出し、前記センサ信号、前記姿勢角推定手段によって推定された前記ロール角及び前記ピッチ角、及び前記スリップ角推定手段により推定された前記前後速度及び前記横速度に基づいて、車両運動の運動方程式より得られる、車体上下方向軸まわりの車体ヨー角の微分量を算出する算出手段、複数の情報発信源の各々から発信された、前記情報発信源の各々の位置に関する情報、前記情報発信源の各々と前記車両との距離に関する情報、及び前記情報発信源の各々に対する前記車両の相対速度に関する情報を含む発信源情報を取得する取得手段、前記取得手段により取得された前記発信源情報に基づいて、前記車両の速度ベクトルを推定する車両速度ベクトル推定手段、前記姿勢角推定手段によって推定された前記姿勢角、前記スリップ角推定手段によって推定された前記スリップ角、及び前記車両速度ベクトル推定手段によって推定された前記車両の速度ベクトルに基づいて、前記車体ヨー角を算出する車体ヨー角算出手段、及び前記センサ信号のセンサドリフト量を考慮したときに、前記姿勢角推定手段により算出された前記ロール角及び前記ピッチ角の各々の微分量と、前記算出手段により算出された前記ロール角及び前記ピッチ角の各々の微分量に前記センサドリフト量を考慮した値とが等しくなる関係、前記スリップ角推定手段により算出された前記前後速度及び前記横速度の各々の微分量と、前記算出手段により算出された前記前後速度及び前記横速度の各々の微分量に前記センサドリフト量を考慮した値とが等しくなる関係、及び前記車体ヨー角算出手段により算出された前記車体ヨー角と、前記算出手段により算出された前記車体ヨー角の微分量に前記センサドリフト量を考慮した値を積分した値とが等しくなる関係を用いて、前記上下加速度、前記前後加速度、前記横加速度、前記ロール角速度、及び前記ヨー角速度の各検出値に応じたセンサ信号のセンサドリフト量を推定するドリフト量推定手段として機能させるためのプログラムである。
【００１６】
第５の発明に係るセンサドリフト量推定装置は、車両運動の運動状態量の検出値に応じたセンサ信号に基づいて、車体の鉛直軸に対する姿勢角の微分量を算出し、算出した前記姿勢角の微分量を積分して、前記姿勢角を推定する姿勢角推定手段と、前記センサ信号に基づいて、車両速度の微分量を算出し、前記車両速度の微分量を積分して、前記車両速度を推定し、前記推定された前記車両速度に基づいて、スリップ角を推定するスリップ角推定手段と、前記センサ信号及び前記姿勢角推定手段によって推定された前記姿勢角に基づいて、車両運動の運動方程式より得られる前記姿勢角の微分量を算出し、前記センサ信号及び前記スリップ角推定手段により推定された前記車両速度に基づいて、車両運動の運動方程式より得られる前記車両速度の微分量を算出し、前記センサ信号、前記姿勢角推定手段によって推定された前記姿勢角、及び前記スリップ角推定手段により推定された前記車両速度に基づいて、車両運動の運動方程式より得られる、車体上下方向軸まわりの回転角の微分量を算出する算出手段と、前記車体の外部を撮像した複数の画像の各々から複数の特徴点を抽出し、前記抽出した複数の特徴点に基づいて、前記複数の画像を撮像したときの車両の運動を推定し、前記推定された車両の運動に基づいて、前記回転角の微分量を算出する運動推定手段と、前記センサ信号のセンサドリフト量を考慮したときに、前記姿勢角推定手段により算出された前記姿勢角の微分量と、前記算出手段により算出された前記姿勢角の微分量に前記センサドリフト量を考慮した値とが等しくなる関係、前記スリップ角推定手段により算出された前記車両速度の微分量と、前記算出手段により算出された前記車両速度の微分量に前記センサドリフト量を考慮した値とが等しくなる関係、及び前記運動推定手段により算出された前記回転角の微分量と、前記算出手段により算出された前記回転角の微分量に前記センサドリフト量を考慮した値とが等しくなる関係を用いて、前記センサ信号のセンサドリフト量を推定するドリフト量推定手段と、を含んで構成されている。
【００１７】
第６の発明に係るプログラムは、コンピュータを、車両運動の運動状態量の検出値に応じたセンサ信号に基づいて、車体の鉛直軸に対する姿勢角の微分量を算出し、算出した前記姿勢角の微分量を積分して、前記姿勢角を推定する姿勢角推定手段、前記センサ信号に基づいて、車両速度の微分量を算出し、前記車両速度の微分量を積分して、前記車両速度を推定し、前記推定された前記車両速度に基づいて、スリップ角を推定するスリップ角推定手段、前記センサ信号及び前記姿勢角推定手段によって推定された前記姿勢角に基づいて、車両運動の運動方程式より得られる前記姿勢角の微分量を算出し、前記センサ信号及び前記スリップ角推定手段により推定された前記車両速度に基づいて、車両運動の運動方程式より得られる前記車両速度の微分量を算出し、前記センサ信号、前記姿勢角推定手段によって推定された前記姿勢角、及び前記スリップ角推定手段により推定された前記車両速度に基づいて、車両運動の運動方程式より得られる、車体上下方向軸まわりの回転角の微分量を算出する算出手段、前記車体の外部を撮像した複数の画像の各々から複数の特徴点を抽出し、前記抽出した複数の特徴点に基づいて、前記複数の画像を撮像したときの車両の運動を推定し、前記推定された車両の運動に基づいて、前記回転角の微分量を算出する運動推定手段、及び前記センサ信号のセンサドリフト量を考慮したときに、前記姿勢角推定手段により算出された前記姿勢角の微分量と、前記算出手段により算出された前記姿勢角の微分量に前記センサドリフト量を考慮した値とが等しくなる関係、前記スリップ角推定手段により算出された前記車両速度の微分量と、前記算出手段により算出された前記車両速度の微分量に前記センサドリフト量を考慮した値とが等しくなる関係、及び前記運動推定手段により算出された前記回転角の微分量と、前記算出手段により算出された前記回転角の微分量に前記センサドリフト量を考慮した値とが等しくなる関係を用いて、前記センサ信号のセンサドリフト量を推定するドリフト量推定手段、として機能させるためのプログラム。
【００１８】
第７の発明に係るセンサドリフト量推定装置は、車両運動の上下加速度、前後加速度、横加速度、ロール角速度、及びヨー角速度の各検出値に応じたセンサ信号に基づいて、車体の鉛直軸に対するロール角及びピッチ角の各々の微分量を算出し、算出した前記ロール角及び前記ピッチ角の各々の微分量を積分して、前記ロール角及び前記ピッチ角を推定する姿勢角推定手段と、各検出値に応じたセンサ信号に基づいて、前後速度及び横速度の各々の微分量を算出し、前記前後速度及び前記横速度の各々の微分量を積分して、前記前後速度及び前記横速度を推定し、前記推定された前記前後速度及び前記横速度に基づいて、スリップ角を推定するスリップ角推定手段と、前記センサ信号と、前記姿勢角推定手段によって推定された前記ロール角及び前記ピッチ角とに基づいて、車両運動の運動方程式より得られる前記ロール角及び前記ピッチ角の各々の微分量を算出し、前記センサ信号と、前記スリップ角推定手段により推定された前記前後速度及び前記横速度とに基づいて、車両運動の運動方程式より得られる前記前後速度及び前記横速度の各々の微分量を算出し、前記センサ信号、前記姿勢角推定手段によって推定された前記ロール角及び前記ピッチ角、及び前記スリップ角推定手段により推定された前記前後速度及び前記横速度に基づいて、車両運動の運動方程式より得られる、車体上下方向軸まわりの車体ヨー角の微分量を算出する算出手段と、前記車体の外部を撮像した複数の画像の各々から複数の特徴点を抽出し、前記抽出した複数の特徴点に基づいて、前記複数の画像を撮像したときの車両の運動を推定し、前記推定された車両の運動に基づいて、前記車体ヨー角の微分量を算出する運動推定手段と、前記センサ信号のセンサドリフト量を考慮したときに、前記姿勢角推定手段により算出された前記ロール角及び前記ピッチ角の各々の微分量と、前記算出手段により算出された前記ロール角及び前記ピッチ角の各々の微分量に前記センサドリフト量を考慮した値とが等しくなる関係、前記スリップ角推定手段により算出された前記前後速度及び前記横速度の各々の微分量と、前記算出手段により算出された前記前後速度及び前記横速度の各々の微分量に前記センサドリフト量を考慮した値とが等しくなる関係、及び前記運動推定手段により算出された前記車体ヨー角の微分量と、前記算出手段により算出された前記車体ヨー角の微分量に前記センサドリフト量を考慮した値とが等しくなる関係を用いて、前記上下加速度、前記前後加速度、前記横加速度、前記ロール角速度、及び前記ヨー角速度の各検出値に応じたセンサ信号のセンサドリフト量を推定するドリフト量推定手段と、を含んで構成されている。
【００１９】
第８の発明に係るプログラムは、コンピュータを、車両運動の上下加速度、前後加速度、横加速度、ロール角速度、及びヨー角速度の各検出値に応じたセンサ信号に基づいて、車体の鉛直軸に対するロール角及びピッチ角の各々の微分量を算出し、算出した前記ロール角及び前記ピッチ角の各々の微分量を積分して、前記ロール角及び前記ピッチ角を推定する姿勢角推定手段、各検出値に応じたセンサ信号に基づいて、前後速度及び横速度の各々の微分量を算出し、前記前後速度及び前記横速度の各々の微分量を積分して、前記前後速度及び前記横速度を推定し、前記推定された前記前後速度及び前記横速度に基づいて、スリップ角を推定するスリップ角推定手段、前記センサ信号と、前記姿勢角推定手段によって推定された前記ロール角及び前記ピッチ角とに基づいて、車両運動の運動方程式より得られる前記ロール角及び前記ピッチ角の各々の微分量を算出し、前記センサ信号と、前記スリップ角推定手段により推定された前記前後速度及び前記横速度とに基づいて、車両運動の運動方程式より得られる前記前後速度及び前記横速度の各々の微分量を算出し、前記センサ信号、前記姿勢角推定手段によって推定された前記ロール角及び前記ピッチ角、及び前記スリップ角推定手段により推定された前記前後速度及び前記横速度に基づいて、車両運動の運動方程式より得られる、車体上下方向軸まわりの車体ヨー角の微分量を算出する算出手段、前記車体の外部を撮像した複数の画像の各々から複数の特徴点を抽出し、前記抽出した複数の特徴点に基づいて、前記複数の画像を撮像したときの車両の運動を推定し、前記推定された車両の運動に基づいて、前記車体ヨー角の微分量を算出する運動推定手段、及び前記センサ信号のセンサドリフト量を考慮したときに、前記姿勢角推定手段により算出された前記ロール角及び前記ピッチ角の各々の微分量と、前記算出手段により算出された前記ロール角及び前記ピッチ角の各々の微分量に前記センサドリフト量を考慮した値とが等しくなる関係、前記スリップ角推定手段により算出された前記前後速度及び前記横速度の各々の微分量と、前記算出手段により算出された前記前後速度及び前記横速度の各々の微分量に前記センサドリフト量を考慮した値とが等しくなる関係、及び前記運動推定手段により算出された前記車体ヨー角の微分量と、前記算出手段により算出された前記車体ヨー角の微分量に前記センサドリフト量を考慮した値とが等しくなる関係を用いて、前記上下加速度、前記前後加速度、前記横加速度、前記ロール角速度、及び前記ヨー角速度の各検出値に応じたセンサ信号のセンサドリフト量を推定するドリフト量推定手段、として機能させるためのプログラムである。
【００２０】
また、本発明は、上記のプログラムを記憶した記録媒体として構成することもできる。
【発明の効果】
【００２１】
以上説明したように、本発明のセンサドリフト量推定装置及びプログラムによれば、姿勢角を推定するために算出された姿勢角の微分量と車両運動の運動方程式より得られる姿勢角の微分量との関係、スリップ角を推定するために算出された車両速度の微分量と車両運動の運動方程式より得られる車両速度の微分量との関係、及び発信源情報に基づいて算出された回転角と、車両運動の運動方程式より得られる回転角の微分量を積算した値との関係を用いて、センサドリフト量を推定することにより、車両運動の状態に関わらず、安定してセンサのドリフト量を推定することができる、という効果が得られる。
【００２２】
また、本発明のセンサドリフト量推定装置及びプログラムによれば、姿勢角を推定するために算出された姿勢角の微分量と車両運動の運動方程式により得られる姿勢角の微分量との関係、スリップ角を推定するために算出された車両速度の微分量と車両運動の運動方程式より得られる車両速度の微分量との関係、及び車体の外部を撮像した複数の画像に基づいて算出された回転角の微分量と、車両運動の運動方程式により得られる回転角の微分量との関係を用いて、センサドリフト量を推定することにより、車両運動の状態に関わらず、安定してセンサのドリフト量を推定することができる、という効果が得られる。
【図面の簡単な説明】
【００２３】
【図１】本発明の第１の実施の形態に係る姿勢角スリップ角推定装置の構成を示す概略図である。
【図２】車両運動を説明する各軸の方向を示す図である。
【図３】角度を説明するための図である。
【図４】従来技術におけるドリフト量の推定手法を説明するための図である。
【図５】車両速度ベクトル推定手段の構成を示すブロック図である。
【図６】車体ヨー角を説明するための図である。
【図７】本発明の第１の実施の形態に係る姿勢角スリップ角推定装置における姿勢角推定処理ルーチンの内容を示すフローチャートである。
【図８】本発明の第１の実施の形態に係る姿勢角スリップ角推定装置における車体ヨー角推定処理ルーチンの内容を示すフローチャートである。
【図９】本発明の第１の実施の形態に係る姿勢角スリップ角推定装置における自車両速度推定処理の内容を示すフローチャートである。
【図１０】第２の実施の形態におけるドリフト量の推定値の更新方法を説明するための図である。
【図１１】ドリフトを印加した状態におけるヨー角誤差の算出結果、ロール角推定値の推定結果、ヨー角速度のセンサドリフト量の推定結果、及び連続補足時間を示すグラフである。
【図１２】本発明の第４の実施の形態に係る姿勢角スリップ角推定装置の構成を示す概略図である。
【図１３】平面運動する車両の座標系の例を示す図である。
【図１４】本発明の第４の実施の形態に係る運動推定手段の構成を示す概略図である。
【図１５】本発明の第４の実施の形態に係る姿勢角スリップ角推定装置における姿勢角推定処理ルーチンの内容を示すフローチャートである。
【図１６】本発明の第４の実施の形態に係る姿勢角スリップ角推定装置における車体ヨー角時間微分値推定処理ルーチンの内容を示すフローチャートである。
【図１７】本発明の第４の実施の形態に係る姿勢角スリップ角推定装置における運動推定処理ルーチンの内容を示すフローチャートである。
【図１８】ノイズ対策の例を示す図である。
【発明を実施するための形態】
【００２４】
以下、図面を参照して本発明の実施の形態を詳細に説明する。本実施の形態は、鉛直軸に対する車両の姿勢角であるピッチ角及びロール角を推定すると共に、車両のスリップ角を推定する姿勢角スリップ角推定装置に本発明を適用したものである。
【００２５】
まず、図２に示すように、車両運動の軸を重心から車両前方方向に向かってｘ軸、車両左方向に向かってｙ軸、鉛直上向きに向かってｚ軸としたときの各軸回りの回転角速度を各々ロール角速度、ピッチ角速度、ヨー角速度と定義する。
【００２６】
図１に示すように、第１の実施の形態に係る姿勢角スリップ角推定装置１０は、車両運動のｘｙｚ３軸の加速度である前後加速度Ｇｘ、横加速度Ｇｙ、及び上下加速度Ｇｚの各々を検出し、検出値に応じたセンサ信号を出力する前後加速度センサ１２、横加速度センサ１４、及び上下加速度センサ１６と、ロール角速度Ｐを検出し、検出値に応じたセンサ信号を出力するロール角速度センサ１８と、ヨー角速度Ｒを検出し、検出値に応じたセンサ信号を出力するヨー角速度センサ２０とを備えている。
【００２７】
上下加速度センサ１６、ロール角速度センサ１８、前後加速度センサ１２、横加速度センサ１４、及びヨー角速度センサ２０は、後述するドリフト量推定手段３２によって推定されたセンサドリフト量に基づいて、各センサからのセンサ信号を補正するドリフト量補正手段２２に接続されている。ドリフト量補正手段２２は、ドリフト量推定手段３２に接続されている。
【００２８】
ドリフト量補正手段２２は、車体の鉛直軸に対する姿勢角であるロール角φ及びピッチ角θを推定する姿勢角オブザーバ２６に接続されている。
【００２９】
ドリフト量補正手段２２は、車体スリップ角を推定するスリップ角オブザーバ２８に接続されている。
【００３０】
また、前後加速度センサ１２、横加速度センサ１４、及びヨー角速度センサ２０は、ドリフト量推定手段３２によって推定された前後加速度センサ１２、横加速度センサ１４、及びヨー角速度センサ２０の各々のセンサドリフト量に基づいて、前後加速度センサ１２、横加速度センサ１４、及びヨー角速度センサ２０の各々からのセンサ信号を補正するドリフト量補正手段２２に接続されている。
【００３１】
前後加速度センサ１２、横加速度センサ１４、上下加速度センサ１６、ロール角速度センサ１８、ヨー角速度センサ２０、姿勢角オブザーバ２６、及びスリップ角オブザーバ２８は、車両運動の運動方程式により得られるロール角、ピッチ角、前後車両速度、横車両速度、及び車体ヨー角の各々の微分量を算出する運動方程式微分量算出手段３０に接続されている。
【００３２】
姿勢角オブザーバ２６、スリップ角オブザーバ２８、及び運動方程式微分量算出手段３０は、前後加速度センサ１２、横加速度センサ１４、上下加速度センサ１６、ロール角速度センサ１８、及びヨー角速度センサ２０の各々のセンサドリフト量を推定するドリフト量推定手段３２に接続されている。
【００３３】
ドリフト量補正手段２２、姿勢角オブザーバ２６、スリップ角オブザーバ２８、運動方程式微分量算出手段３０、及びドリフト量推定手段３２は、各手段の機能を実現する１つまたは複数のコンピュータ、または１つまたは複数の電子回路で構成することができる。
【００３４】
姿勢角オブザーバ２６は、スリップ角オブザーバ２８で推定された横車体速度Ｖの推定値、前後車体速度Ｕの推定値、ドリフト量補正手段２２で補正された上下加速度Ｇｚの検出値に応じた補正信号、及びロール角速度Ｐの検出値に応じた補正信号に基づいて、ピッチ角速度Ｑを推定する。姿勢角オブザーバ２６は、ドリフト量補正手段２２で補正された、車両運動の前後加速度Ｇｘ、横加速度Ｇｙ、上下加速度Ｇｚ、ヨー角速度Ｒ、及びロール角速度Ｐの各検出値に応じた補正信号と、前後車体速度Ｕの推定値Ｖｓｏと、ピッチ角速度Ｑの推定値とに基づいて、車体の鉛直軸に対する姿勢角であるロール角φ及びピッチ角θを推定する。
【００３５】
上下加速度センサ１６、及びロール角速度センサ１８は、後述するドリフト量推定手段３２によって推定されたセンサドリフト量に基づいて、各センサからのセンサ信号を補正するドリフト量補正手段２２に接続されている。ドリフト量補正手段２２は、ドリフト量推定手段３２に接続されている。
【００３６】
ドリフト量補正手段２２は、車体の鉛直軸に対する姿勢角であるロール角φ及びピッチ角θを推定する姿勢角オブザーバ２６に接続されている。
【００３７】
姿勢角スリップ角推定装置１０は、ＧＰＳ衛星からの電波を受信するＧＰＳ受信手段４０を更に備えている。
【００３８】
ＧＰＳ受信手段４０は、車両速度ベクトルを推定する車両速度ベクトル推定手段４２に接続されている。ＧＰＳ受信手段４０は、複数のＧＰＳ衛星からの電波を受信して、受信した全てのＧＰＳ衛星からの受信信号から、ＧＰＳ衛星の情報として、ＧＰＳ衛星の衛星番号、ＧＰＳ衛星の軌道情報（エフェメリス）、ＧＰＳ衛星が電波を送信した時刻、受信信号の強度、周波数などを取得し、車両速度ベクトル推定手段４２に出力する。なお、ＧＰＳ衛星が情報発信源の一例である。
【００３９】
車両速度ベクトル推定手段４２は、車体ヨー角を算出する車体ヨー角算出手段４４に接続されている。車体ヨー角算出手段４４は、姿勢角オブザーバ２６、スリップ角オブザーバ２８、車両速度ベクトル推定手段４２に接続されており、算出した車体ヨー角を、ドリフト量推定手段３２に出力する。
【００４０】
次に、ピッチ角速度の推定について説明する。剛体に固定された３軸加速度及び３軸角速度を検出する３軸センサから出力されるセンサ信号と運動状態量との関係を表す剛体の運動方程式は、以下のように記述することができる。
【００４１】
【数１】
2014240266.tif 000003
【００４２】
ただし、 Ｇｘ：前後加速度、Ｇｙ：横加速度、Ｇｚ：上下加速度、Ｐ：ロール角速度、Ｑ：ピッチ角速度、Ｒ：ヨー角速度、Ｕ：前後車体速度、Ｖ：横車体速度、Ｗ：上下車体速度、φ：ロール角、θ：ピッチ角、g：重力加速度である。座標系は、図２のように車体固定座標系とし、車体の上向きをＺ軸の正方向とする右手系で記述する。角度は図３に示すように軸をＺ軸→Ｙ軸→Ｘ軸の順番で回転させたオイラー角で表現する。
【００４３】
本実施の形態において車両を剛体とした場合、前後加速度Ｇｘ、横加速度Ｇｙ、及び上下加速度Ｇｚの３軸速度、並びにロール角速度Ｐ及びヨー角速度Ｒの２軸角速度の各々は、前後加速度センサ１２、横加速度センサ１４、上下加速度センサ１６、ロール角速度センサ１８、及びヨー角速度センサ２０によって検出されたセンサ信号がドリフト量補正手段２２でドリフト量を補正された信号であり、横車体速度Ｖは、スリップ角オブザーバ２８によって推定でき、また、前後車体速度Ｕは各輪の車輪速に基づいて推定することができる。
【００４４】
本実施の形態では、上記（３）式において上下車体速度Ｗの変化を無視（Ｗ＝０とする）できると仮定し、ロール角φ及びピッチ角θの各々の前回推定値~φ、~θ、横車体速度Ｖの前回推定値~Ｖ、及び前後車体速度Ｕとして前後車体速度の推定値Ｖｓｏを用いることにより、ピッチ角速度Ｑの推定値~Ｑを以下の（６）式から求めることができる。姿勢角オブザーバ２６は、下記（６）式の演算を実行することによりピッチ角速度Ｑの推定値~Ｑを求める。
【００４５】
【数２】
2014240266.tif 000004
【００４６】
ピッチ角速度が上記のように推定されると、ピッチ角もロール角と同時に姿勢角オブザーバ２６によって推定することができる。姿勢角オブザーバ２６を用いた場合の自動車固有の運動の拘束条件について説明する。上記の運動方程式を利用して、姿勢角オブザーバ２６を構成する場合、積分演算によって推定される速度及び角度の状態量が発散しないように、測定できる物理量のフィードバックが必要となる。本実施の形態では、自動車運動固有の特徴を、以下のようにフィードバックする物理量に利用する。なお、以下の式では、ロール角及びピッチ角として各々の前回推定値~φ、~θ、横車体速度としての横車体速度の推定値~Ｖ、及び前後車体速度Ｕとしての前後車体速度の推定値Ｖｓｏを各々用いている。
【００４７】
前後車体速度の推定値Ｖｓｏを微分して上記（１）式に代入し、自動車運動固有の特徴として、車体の前後方向に関しては「車輪のスリップは長時間増加し続けない」という性質を利用すると、上記（１）式から上下車体速度の変化を無視した、フィードバックに利用する以下の（７）式に示す条件が得られる。
【００４８】
【数３】
2014240266.tif 000005
【００４９】
ここで、ＶＳ０は車輪速から推定される車体前後速度で、この車輪速度から推定される加速度（前後車体速度の推定値Ｖｓｏの微分値）と前後加速度Ｇｘとの差と、ピッチ角θとの関係を記述している。また、車体横速度Ｖはスリップ角の推定値~βを用い、以下の（８）式により求められるため、~Ｖと表す。
【００５０】
【数４】
2014240266.tif 000006
【００５１】
また、車体の横方向に関しては、「スリップ角は長時間増加し続けない」という性質からロール角速度及び横加速度を無視できることから、フィードバックに利用するための、上記（２）式から得られる以下の（９）式に示す条件が得られる。
【００５２】
【数５】
2014240266.tif 000007
【００５３】
さらに、一般的な道路勾配を考慮すると、「地面固定座標系の鉛直軸方向の加速度は、重力加速度と等しい」と考えられる。この条件は、前後加速度Ｇｘ、横加速度Ｇｙ、上下加速度Ｇｚ、ピッチ角θ、及びロール角φを用いて次の（１０）式の代数方程式によって記述することができる。
【００５４】
【数６】
2014240266.tif 000008
【００５５】
次に、姿勢角オブザーバについて説明する。
【００５６】
上記（７）、（９）、（１０）式の関係は、長い時間の中での変化を考慮したときに満足する条件であることから、オブザーバ測定量のフィードバックには、以下の（１１）〜（１３）式に示す、上記（７）〜（９）式の両辺をローパスフィルタ処理した値を利用する。
【００５７】
【数７】
2014240266.tif 000009
【００５８】
ただし、τx、 τy、 τgは、上記（７）、（９）、（１０）式で考慮するローパスフィルタの数秒から数十秒以上の時定数を各々表している。
【００５９】
次に、本実施の形態では、オブザーバの状態量~ｘを下記（１４）式で表す。
【００６０】
【数８】
2014240266.tif 000010
【００６１】
また、フィードバックに用いるオブザーバ出力を下記（１５）式で表す。
【００６２】
【数９】
2014240266.tif 000011
【００６３】
さらに、センサ信号等から演算される車両出力を以下の（１６）式で表わし、適切なオブザーバゲインｋ（~ｘ，ｕ）を設定することによって、ロール角及びピッチ角を推定する姿勢角オブザーバ２６を構成している。
【００６４】
【数１０】
2014240266.tif 000012
【００６５】
オブザーバゲインの一例として、本実施の形態では、オブザーバの安定性を確保するため、線形化を行ったときの対角成分が負の係数を持つように、下記（１９）式のように表す。
【００６６】
【数１１】
2014240266.tif 000013
【００６７】
Ｋｘ、Ｋｙ、Ｋｇは正の定数、Ｋφｇ（Ｇｙ）、Ｋθｇ（Ｇｘ）と、Ｋφｙ、Ｋθｘは横加速度、前後推定値、ピッチ角推定値に応じて変化する可変ゲインとする。
【００６８】
したがって、ロール角及びピッチ角を推定する本実施の形態の非線形オブザーバは、以下の（２０）式で表される運動方程式で記述される。
【００６９】
【数１２】
2014240266.tif 000014
【００７０】
姿勢角オブザーバ２６は、上記（２０）式を用いることにより、車体鉛直方向に対する姿勢角であるロール角~φの微分量~ｄφとピッチ角~θの微分量~ｄθとを算出し、算出したロール角~φの微分量~ｄφ及びピッチ角~θの微分量~ｄθの各々を積分することにより、ロール角~φ及びピッチ角~θを算出することができる。
【００７１】
スリップ角オブザーバ２８は、車両横方向の車体速度である横車体速度Ｖを推定すると共に、各輪の車輪速度に基づいて車両前後方向の車体速度である前後車体速度Ｕを推定し、また、横車体速度Ｖの推定値、前後車体速度Ｕの推定値、前後加速度Ｇｘの検出値に応じた補正信号、横加速度Ｇｙの検出値に応じた補正信号、及びヨー角速度Ｒの検出値に応じた補正信号に基づいて、前後車体速度Ｕ及び横車体速度Ｖを推定する。また、スリップ角オブザーバ２８は、前後車体速度Ｕの推定値及び横車体速度Ｖの推定値に基づいて、車体スリップ角βを推定する。
【００７２】
次に、前後車体速度及び横車体速度の推定について説明する。
【００７３】
従来、動特性を無視していた上記（１）式には、横車体速度Ｖの項が含まれており、この横車体速度を利用することによってオブザーバが構成できる。ここでは、ヨー角速度を独立したパラメータとみなして、上記（１）式及び（２）式を以下の（２１）式のように整理する。なお、上下車体速度Ｗに関しては、姿勢角オブザーバ２６と同様に０と仮定している。
【００７４】
【数１３】
2014240266.tif 000015
【００７５】
また、システムの出力ｙを前後車体速度Ｕ、すなわち以下の（２２）式で表わされると考えると、(２１)式及び(２２)式は可観測なシステムとなる。
【００７６】
【数１４】
2014240266.tif 000016
【００７７】
なお、前後車体速度Ｕは、上記で説明したように、各輪の車輪速に基づいて推定される。例えば、オブザーバゲインＫは、ヨー角速度の絶対値から得られる値を含む以下の（２３）式で表わされる。
【００７８】
【数１５】
2014240266.tif 000017
【００７９】
オブザーバゲインを上記の（２３）式のように表わすと、オブザーバは以下の（２４）式で表される運動方程式で記述される。
【００８０】
【数１６】
2014240266.tif 000018
【００８１】
ただし、~ｘは、オブザーバの状態量であり、以下の（２５）式で表される。
【００８２】
【数１７】
2014240266.tif 000019
【００８３】
本実施の形態のスリップ角オブザーバ２８は、上記（２４）式を用いて前後車体速度及び横車体速度を推定する。本実施の形態では、車体の姿勢角、すなわち鉛直軸に対する車体のロール角φ及びピッチ角θは既知という仮定の下で（例えば、ロール角、ピッチ角を０とする）、前後車体速度~Ｕ及び横車体速度~Ｖをオブザーバの状態量とし、前後車体速度の演算値Ｕから前後車体速度の推定値~Ｕ（オブザーバ出力）を減算した偏差、既知のロール角φ及びピッチ角θ、並びにドリフト量補正手段２２で補正された前後加速度Ｇｘ−Ｇｘｄｒ、横加速度Ｇｙ−Ｇｙｄｒ、及びヨー角速度Ｒ−Ｒｄｒを用いて、前後車体速度~Ｕの微分量~ｄＵ及び横車体速度~Ｖの微分量~ｄＶを算出している。また、スリップ角オブザーバ２８は、前後車体速度及び横車体速度の微分量を積分して、前後車体速度及び横車体速度を推定している。
【００８４】
また、スリップ角オブザーバ２８は、前後車体速度Ｕ及び横車体速度Ｖの関係から次の（２６）式に基づいて車体スリップ角~βを推定する。
【００８５】
【数１８】
2014240266.tif 000020
【００８６】
次に、ドリフト量の補正について説明する。
【００８７】
姿勢角オブザーバ２６およびスリップ角オブザーバ２８に、センサドリフトが重畳したセンサ信号を入力した場合、姿勢角および車体速度推定値に誤差が重畳し、推定誤差が増大する。したがって、ドリフト量推定手段３２で算出するドリフト量推定値をセンサ信号から差し引き、オブザーバの入力とする必要がある。
【００８８】
ドリフト量補正手段２２は、上下加速度センサ１６からのセンサ信号から、上下加速度ドリフト量推定値を差し引いて、姿勢角オブザーバ２６へ出力する。また、ドリフト量補正手段２２は、ロール角速度センサ１８からのセンサ信号から、ロール角速度ドリフト量推定値を差し引いて、姿勢角オブザーバ２６へ出力する。
【００８９】
ドリフト量補正手段２２は、前後加速度センサ１２からのセンサ信号から、前後加速度ドリフト量推定値を差し引いて、スリップ角オブザーバ２８へ出力する。また、ドリフト量補正手段２２は、横加速度センサ１４からのセンサ信号から、横加速度ドリフト量推定値を差し引いて、スリップ角オブザーバ２８へ出力する。また、ドリフト量補正手段２２は、ヨー角速度センサ２０からのセンサ信号から、ヨー角速度ドリフト量推定値を差し引いて、スリップ角オブザーバ２８へ出力する。
【００９０】
次に、車体ヨー角を利用したドリフト量の推定手法について説明する。
【００９１】
上記特許文献４に記載のドリフト量推定手段は、センサ誤差の影響を少なくする効果がオブザーバにあることを利用し、センサ信号の単純積分とオブザーバの出力とを比較することによってセンサゼロ点ドリフトを推定するという手法である（図４参照）。
【００９２】
従来手法では上記の概念をもとに横加速度とヨー角速度のセンサドリフトを１つの式から推定するため、車速変化によるヨー角速度ドリフト推定値の精度悪化が懸念される。
【００９３】
本実施の形態では、ＧＰＳ信号を利用し、車体ヨー方向に関する以下のつり合いの式を新たに加えることで、ヨー角速度ドリフトの推定精度を向上できる。
【００９４】
【数１９】
2014240266.tif 000021
【００９５】
ここで、ψは車体ヨー角とする。
【００９６】
次に、ＧＰＳ衛星の信号から得られる車体速度ベクトルを用いた車体ヨー角の算出方法について説明する。
【００９７】
ＧＰＳ衛星から得られる信号は、搬送波と搬送波上に０．１で付与されるコードで構成されており、搬送波のトップラー効果によって生じる周波数変化分を計測したものをＧＰＳドップラーという。このＧＰＳドップラーと衛星速度から車体の地球中心座標系に対する速度を推定する手法により、精度の高い車体速度ベクトルが得られる。
【００９８】
車両速度ベクトル推定手段４２は、特開２０１１‐２０９２６８と同様に、ＧＰＳドップラーによる車体速度ベクトルを利用し、移動体の位置を推定する。
【００９９】
車両速度ベクトル推定手段４２を、以下で説明する自車両速度推定処理ルーチンに従って機能ブロックで表すと、図５に示すように、ＧＰＳ受信手段４０から、電波を受信した全てのＧＰＳ衛星について、ＧＰＳ衛星の情報を取得すると共に、ＧＰＳ擬似距離データ、ドップラー周波数、及びＧＰＳ衛星の位置座標を算出して取得するＧＰＳ情報取得手段５０と、取得した各ＧＰＳ衛星のドップラー周波数に基づいて、各ＧＰＳ衛星に対する自車両の相対速度を算出する相対速度算出手段５２と、取得した各ＧＰＳ衛星の位置座標の時系列データに基づいて、各ＧＰＳ衛星の速度ベクトルを算出する衛星速度算出手段５４と、取得した各ＧＰＳ衛星のＧＰＳ擬似距離データに基づいて、自車両の位置を算出する自車位置算出手段５６と、算出された自車両の位置及び各ＧＰＳ衛星の位置座標に基づいて、各ＧＰＳ衛星の方向（角度の関係）を算出する衛星方向算出手段５８と、算出された相対速度、各ＧＰＳ衛星の速度ベクトル、及び各ＧＰＳ衛星の方向に基づいて、各ＧＰＳ衛星方向の自車両の速度を算出する衛星方向自車速算出手段６０と、算出された複数の各ＧＰＳ衛星方向の自車両の速度に基づいて、自車両の速度ベクトルを算出する自車両速度算出手段６２と、を備えた構成で表すことができる。
【０１００】
ＧＰＳ情報取得手段５０は、ＧＰＳ受信手段４０から、電波を受信した全てのＧＰＳ衛星について、ＧＰＳ衛星の情報を取得すると共に、ＧＰＳ衛星が電波を送信した時刻及び自車両で電波を受信した時刻に基づいて、ＧＰＳ擬似距離データを算出する。また、ＧＰＳ情報取得手段５０は、各ＧＰＳ衛星から送信される信号の既知の周波数と、各ＧＰＳ衛星から受信した受信信号の周波数とに基づいて、各ＧＰＳ衛星からの受信信号のドップラー周波数を各々算出する。なお、ドップラー周波数は、ＧＰＳ衛星と自車との相対速度による、搬送波周波数のドップラーシフト量を観測したものである。また、ＧＰＳ情報取得手段５０は、ＧＰＳ衛星の軌道情報及びＧＰＳ衛星が電波を送信した時刻に基づいて、ＧＰＳ衛星の位置座標を各々算出する。
【０１０１】
相対速度算出手段５２は、ドップラー周波数とＧＰＳ衛星に対する相対速度との関係を表わす以下の（２８）式に従って、各ＧＰＳ衛星からの受信信号のドップラー周波数から、各ＧＰＳ衛星に対する自車両の相対速度を算出する。
【０１０２】
【数２０】
2014240266.tif 000022
【０１０３】
ただし、ｖｊはＧＰＳ衛星ｊに対する相対速度であり、Ｄ１ｊはＧＰＳ衛星ｊから得られるドップラー周波数（ドップラーシフト量）である。また、Ｃは光速であり、Ｆ１は、ＧＰＳ衛星から送信される信号の既知のＬ１周波数である。
【０１０４】
衛星速度算出手段５４は、取得した各ＧＰＳ衛星の位置座標の時系列データから、ケプラーの方程式の微分を用いて、各ＧＰＳ衛星の速度ベクトル（３次元速度ＶＸｊ、ＶＹｊ、ＶＺｊ）を算出する。例えば、非特許文献（Ｐｒａｔａｐ  Ｍｉｓｒａ  ａｎｄ  Ｐｅｒ  Ｅｎｇｅ原著  日本航海学会ＧＰＳ研究会訳：“精説ＧＰＳ基本概念・測位原理・信号と受信機”正陽文庫，２００４．）に記載された方法を用いて、各ＧＰＳ衛星の速度ベクトルを算出することができる。
【０１０５】
自車位置算出手段５６は、以下のように、ＧＰＳ情報取得手段５０によって取得された各ＧＰＳ衛星のＧＰＳ擬似距離データを用いて、自車両の位置を算出する。
【０１０６】
ＧＰＳを用いた測位では、既知であるＧＰＳ衛星の位置座標と、各ＧＰＳ衛星から受信した受信信号の伝播距離である擬似距離とに基づいて、三角測量の原理に従って、自車両の位置が推定される。
【０１０７】
ここで、ＧＰＳ衛星までの真の距離ｒｊは、以下の（２９）式で表され、ＧＰＳで観測される擬似距離ρｊは、以下の（３０）式で表される。
【０１０８】
【数２１】
2014240266.tif 000023
【０１０９】
ただし、（Ｘｊ，Ｙｊ，Ｚｊ）がＧＰＳ衛星ｊの位置座標であり、（ｘ，ｙ，ｚ）が自車両の位置座標である。ｓは、ＧＰＳ受信手段４０の時計誤差による距離誤差である。
【０１１０】
上記（２９）式、（３０）式より、４つ以上のＧＰＳ衛星のＧＰＳ擬似距離データから得られる以下の（３１）式の連立方程式を解くことによって、自車両の位置（ｘ、ｙ、ｚ）が算出される。
【０１１１】
【数２２】
2014240266.tif 000024
【０１１２】
なお、本実施の形態では、自車両の位置を、ＧＰＳ衛星の方向（ＧＰＳ衛星と自車両との角度）を求めるために算出しているが、遠方に存在するＧＰＳ衛星の方向であるため、位置決めは大まかでよく、擬似距離を用いた位置決定を行わなくてもよい。時刻の影響が少なく、システム等で許容される推定精度に依存するが、自車の位置決めの誤差も数百ｍの範囲であれば、速度推定誤差も１ｍ／ｓｅｃ以下程度となり大きな問題はないため、例えば、地図などから位置を決定してもよく、また、過去の位置の測定履歴やビーコンなどの情報などから、自車両の位置を決定してもよい。
【０１１３】
衛星方向算出手段５８は、算出された自車両の位置及び各ＧＰＳ衛星の位置座標に基づいて、各ＧＰＳ衛星ｊの位置と自車両の位置との角度関係（水平方向に対する仰角θｊ、北方向に対する方位角φｊ）を、各ＧＰＳ衛星の方向として算出する。
【０１１４】
衛星方向自車速算出手段６０は、算出された各ＧＰＳ衛星に対する自車両の相対速度ｖｊ、各ＧＰＳ衛星の速度ベクトル（ＶＸｊ、ＶＹｊ、ＶＺｊ）、及び各ＧＰＳ衛星の方向Ｒｊ（θｊ、φｊ）に基づいて、以下の（３２）式に従って、各ＧＰＳ衛星ｊの方向の自車両の速度Ｖｖｊを算出する。
【０１１５】
【数２３】
2014240266.tif 000025
【０１１６】
ｖｊは、ＧＰＳ衛星ｊに対する自車両の相対速度（衛星方向におけるＧＰＳ衛星との相対速度）である。また、Ｖｓｊは、自車方向のＧＰＳ衛星ｊの速度であり、Ｖｓｊ＝Ｒｊ［ＶＸｊ，ＶＹｊ，ＶＺｊ］Ｔにより求まる。また、Ｖｖｊは、ＧＰＳ衛星ｊの方向の自車速であり、ｖＣｂは、クロックバイアス変動である。
【０１１７】
上述したように、ＧＰＳ衛星方向の自車速は、ＧＰＳ衛星位置の三次元位置ではなく、ＧＰＳ衛星との方位関係によってのみ算出される。ＧＰＳ衛星は遥か遠方にあり、１日で地球をほぼ２周するため１分間の角度変化は０．５度である。通常、ＧＰＳ衛星とＧＰＳ受信機との時計誤差は通常１ｍｓｅｃ以下であるため、ＧＰＳ衛星との方位関係に大きな影響はない。また、同じくＧＰＳ衛星は遥か遠方にあるため、自車の位置決定に数１００ｍ程度の誤差が生じていたとしても、ＧＰＳ衛星との方位関係に大きな影響はない。このため、擬似距離に誤差が乗りやすい状況であったとしても、ＧＰＳ衛星方向の自車速は、擬似距離に比較して正確に算出され得る。
【０１１８】
自車両速度算出手段６２は、以下に説明するように、自車両の速度ベクトルの最適推定を行う。
【０１１９】
まず、自車の速度ベクトルを（Ｖｘ，Ｖｙ，Ｖｚ）としたとき、ＧＰＳ衛星方向の自車両の速度Ｖｖｊとの関係は以下の（３３）式で表される。
【０１２０】
【数２４】
2014240266.tif 000026
【０１２１】
各ＧＰＳ衛星ｊについて得られる上記（３３）式より、Ｖｘ，Ｖｙ，Ｖｚ及びＣｂを推定値とした、以下の（３４）式で表される連立方程式が得られる。
【０１２２】
【数２５】
2014240266.tif 000027
【０１２３】
電波を受信したＧＰＳ衛星が４個以上である場合に、上記（３４）式の連立方程式を解くことによって、自車両の速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）の最適値を算出する。
【０１２４】
上記のように、ＧＰＳドップラーに基づいて算出される車体の速度ベクトルは、図６のような地球中心座標系に対する速度として求められる。地面固定座標系に対する車体速度ベクトルを、任意の位置を原点とした地面固定座標系Ｏ−ＸＹＺの速度ベクトルに変換すると、地面固定座標系Ｏ−ＸＹＺとＧＰＳドップラーによる車体速度ベクトルとのなす角ψＧＰＳが得られる。以下では、ψＧＰＳをＧＰＳ方位角とする。ＸＹ平面での運動のみを考えた場合、ψＧＰＳと車体スリップ角βおよび車体ヨー角ψの関係は、以下の（３５）式で表わされる。
【０１２５】
【数２６】
2014240266.tif 000028
【０１２６】
車体のロール角、ピッチ角を考慮した車両運動を扱うため、（３５）式の関係を３次元での車両運動を考慮した形に拡張する。車体姿勢角は、上記で定義したように軸をＺ軸→Ｙ軸→Ｘ軸の順番で回転させたオイラー角である。したがって、車体座標系をＸ軸→Ｙ軸の順に戻した座標系Ｏ−Ｘ１Ｙ１Ｚ１と、地面固定座標系とのＺ軸まわりの回転角が車体ヨー角である。このとき、車体の速度ベクトルをＯ−Ｘ１Ｙ１Ｚ１とで表した場合のＸ１方向成分とＹ１方向成分から作られるベクトルをＶＸ１Ｙ１Ｚ１とすると、Ｘ１軸とＶＸ１Ｙ１Ｚ１とがなす角は以下の（３６）式で表される。
【０１２７】
【数２７】
2014240266.tif 000029
【０１２８】
すなわち、車両の３次元運動を考慮した場合、β’を用いて（３５）式は、以下の（３７）式のように修正される。
【０１２９】
【数２８】
2014240266.tif 000030
【０１３０】
上記（３７）式により、ＧＰＳ信号から得られる車体速度ベクトルと車体スリップ角から、車両の３次元運動を考慮した車体ヨー角が得られる。
【０１３１】
車体ヨー角算出手段４４は、車両速度ベクトル推定手段４２によって推定された車両速度ベクトルから得られるＧＰＳ方位角ψＧＰＳと、スリップ角オブザーバ２８によって推定されたスリップ角βと、姿勢角オブザーバ２６によって推定されたロール角φ、ピッチ角θとに基づいて、上記（３６）式、（３７）式に従って、車体ヨー角ψを算出する。
【０１３２】
次に、車両運動の運動方程式により得られる各微分量の算出方法について説明する。
【０１３３】
上記（１）式、（２）式、（４）式、（５）式に対して、上下車体速度Ｗを０と仮定し、上記（６）式を代入すると、以下の（３８）式〜（４１）式が得られる。
【０１３４】
【数２９】
2014240266.tif 000031
【０１３５】
ただし、ロール角φ及びピッチ角θには、姿勢角オブザーバ２６によって推定されたロール角φ及びピッチ角θの推定値を用いる。前後加速度Ｇｘ、横加速度Ｇｙ、及び上下加速度Ｇｚの３軸速度、並びにロール角速度Ｐ及びヨー角速度Ｒの２軸角速度の各々は、前後加速度センサ１２、横加速度センサ１４、上下加速度センサ１６、ロール角速度センサ１８、及びヨー角速度センサ２０によって検出されたセンサ信号である。また、横車体速度Ｖは、カルマンフィルタ等を用いて推定したり、横加速度センサの検出値から推定したりすることができ、前後車体速度Ｕは各輪の車輪速に基づいて推定することができる。
【０１３６】
また、センサ信号ＧＸ、Ｇｙ、Ｇｚ、Ｐ、Ｒに一定のドリフト誤差Ｇｘｄｒ、Ｇｙｄｒ、Ｇｚｄｒ、Ｐｄｒ、Ｒｄｒが重畳していると仮定するとともに、上式の右辺はオブザーバによって真値が既知と仮定すると、以下の（４２）〜（４５）式の関係が成立する。
【０１３７】
【数３０】
2014240266.tif 000032
【０１３８】
ここで、ｄφ、ｄθは（４０）式、（４１）式の右辺に相当する姿勢角オブザーバ２６の内部値である。また、ｄＵ、ｄＶは（３８）式、（３９）式の右辺に相当し、車体前後、横速度推定手法から得られるオブザーバ内部値である。
【０１３９】
車体ヨー角と角速度センサ信号との関係式を表す上記（２７）式について、センサ信号に一定のドリフト誤差が重畳していると仮定し、両辺積分すると以下の（４６）式のように表わされる。
【０１４０】
【数３１】
2014240266.tif 000033
【０１４１】
ここで、ピッチ角速度Ｑには（６）式の推定値を利用した。
【０１４２】
運動方程式微分量算出手段３０は、上記（４４）式、（４５）式に従って、車両運動の運動方程式より得られるロール角φ及びピッチ角θの微分量を算出する。
【０１４３】
運動方程式微分量算出手段３０は、上記（４２）式、（４３）式に従って、車両運動の運動方程式より得られる前後車体速度Ｕ及び横車体速度Ｖの微分量を算出する。
【０１４４】
運動方程式微分量算出手段３０は、上記（４６）式右辺の括弧｛  ｝内の式に従って、車両運動の運動方程式より得られる車体ヨー角ψの微分量を算出する。
【０１４５】
次に、センサドリフト量の推定方法について説明する。
【０１４６】
上記（４２）、（４３）、（４４）、（４５）、および（４６）式をドリフトに関する式に整理すると、以下の（４７）〜（５１）式のように表される。
【０１４７】
【数３２】
2014240266.tif 000034
【０１４８】
ただし、ｄＵ、ｄＶは、上記（２４）式により得られるオブザーバ内部演算値としての前後車体速度の微分量、横車体速度の微分量であり、ｄφ、ｄθは、上記（２０）式により得られるオブザーバ内部演算値としてのロール角の微分量、ピッチ角の微分量である。
【０１４９】
上記（４９）式、（５０）式は、センサ信号のセンサドリフト量を考慮したときに、姿勢角オブザーバ２６により算出されたロール角及びピッチ角の微分量と、運動方程式により得られるロール角及びピッチ角の微分量にセンサドリフト量を考慮した値とが等しくなる関係を表わしている。また、上記（４７）式、（４８）式は、スリップ角オブザーバ２８により算出された前後車体速度及び横車体速度の微分量と、運動方程式により得られる前後車体速度及び横車体速度の微分量にセンサドリフト量を考慮した値とが等しくなる関係を表わしている。また、上記（５１）式は、車体ヨー角算出手段４４により算出された車体ヨー角と、運動方程式により得られる車体ヨー角の微分量にセンサドリフト量を考慮した値を積分した値とが等しくなる関係を表わしている。
【０１５０】
さらに、上記（１０）式の「地面固定座標系の鉛直方向の加速度は重力加速度」という条件を考慮し、センサドリフトを未知数とした連立方程式を解くことでセンサドリフト推定値を求める。ここで、（１０）式は長時間の運動に関する仮定であるため、フィルタ処理した次の（５２）式を用いる。
【０１５１】
【数３３】
2014240266.tif 000035
【０１５２】
上記（５２）式は以下の（５３）式のように書き直される。
【０１５３】
【数３４】
2014240266.tif 000036
【０１５４】
ただし、Ｄｇｆ、Ｅｇｆは、以下の（５４）式、（５５）式で表される。また、τｆは、長時間の運動のみを考慮するためのフィルタ時定数である。
【０１５５】
【数３５】
2014240266.tif 000037
【０１５６】
また、ｕｇｆは鉛直方向加速度の増加による推定精度の悪化を防止するため、ｇｖに上限を設けた信号とし、その範囲をｇｍを用いて設定する。
【０１５７】
また、（４７）、（４８）、（４９）、（５０）、（５１）、（５３）式から、以下の（５６）式のように記述できる。
【０１５８】
【数３６】
2014240266.tif 000038
【０１５９】
ここで、ｔｓｍｐはシステム全体の演算に用いるサンプリング時間とする。（５７）、（５８）式左辺の係数行列と右辺のベクトルを一定時間積分すると、以下の（５９）式が得られる。
【０１６０】
【数３７】
2014240266.tif 000039
【０１６１】
したがって、以下の（６１）式を解くことにより、Ｇｚｄｒ、Ｐｄｒ、Ｒｄｒ、Ｇｙｄｒ、Ｇｘｄｒを導出できる。
【０１６２】
【数３８】
2014240266.tif 000040
【０１６３】
ここで、Ｄ＋は疑似逆行列とする。
【０１６４】
ドリフト量推定手段３２は、上記（６１）式に従って、姿勢角オブザーバ２６で算出されるロール角及びピッチ角の各々の微分量と、スリップ角オブザーバ２８で算出される前後車体速度及び横車体速度の各々の微分量と、車体ヨー角算出手段４４によって算出された車両ヨー角と、運動方程式微分量算出手段３０で算出されたロール角、ピッチ角、前後車体速度、横車体速度、及び車体ヨー角の各々の微分量とに基づいて、前後加速度センサ１２のセンサドリフト量、横加速度センサ１４のセンサドリフト量、上下加速度センサ１６のセンサドリフト量、ロール角速度センサ１８のセンサドリフト量、及びヨー角速度センサ２０のセンサドリフト量を推定することができる。
【０１６５】
また、本実施の形態に係るドリフト量推定手段３２では、演算の安定化を図るため、上式の演算結果に対し、前回値を用いて以下の（６２）〜（６６）式のように平滑化した値をドリフト推定値とする。
【０１６６】
【数３９】
2014240266.tif 000041
【０１６７】
ただし、~Ｇｚｄｒ、~Ｐｄｒ、~Ｇｙｄｒ、~Ｒｄｒ、~Ｇｘｄｒは、平滑化後のセンサドリフト量の推定値であり、λ１、λ２、λ３は忘却係数である。（６４）、（６５）、（６６）式は、並進加速度と姿勢角のつり合い状態での姿勢角推定値の増加を抑制するため、並進加速度のドリフト推定値更新時に、姿勢角の絶対値を低下させる方向に変更される場合に限ってドリフト推定値を更新するロジックである。
【０１６８】
すわなち、上記（６４）式に示すように、ロール角の推定値~φが正の場合には、新たに演算されたドリフト誤差の推定値Ｇｙｄｒが前回値~Ｇｙｄｒ（ｉ）より大きければ、ドリフト誤差の更新によりロール角の推定値の絶対値を小さくすることができるため、ドリフト誤差学習値を更新する。また、ロール角の推定値~φが負の場合には、新たに演算されたドリフト誤差の推定値Ｇｙｄｒが前回値~Ｇｙｄｒ（ｉ）より小さければ、ドリフト誤差の更新によりロール角の推定値の絶対値を小さくすることができるため、ドリフト誤差学習値を更新する。
【０１６９】
上記（６５）式に示すように、ロール角の推定値~φが正の場合には、新たに演算されたドリフト誤差の推定値Ｒｄｒが前回値~Ｒｄｒ（ｉ）より小さければ、ドリフト誤差の更新によりロール角の推定値の絶対値を小さくすることができるため、ドリフト誤差学習値を更新する。また、ロール角の推定値~φが負の場合には、新たに演算されたドリフト誤差の推定値Ｒｄｒが前回値~Ｒｄｒ（ｉ）より大きければ、ドリフト誤差の更新によりロール角の推定値の絶対値を小さくすることができるため、ドリフト誤差学習値を更新する。
【０１７０】
上記（６６）式に示すように、ピッチ角の推定値~θが正の場合には、新たに演算されたドリフト誤差の推定値Ｇｘｄｒが前回値~Ｇｘｄｒ（ｉ）より小さければ、ドリフト誤差の更新によりピッチ角の推定値の絶対値を小さくすることができるため、ドリフト誤差学習値を更新する。また、ピッチ角の推定値~θが負の場合には、新たに演算されたドリフト誤差の推定値Ｇｘｄｒが前回値~Ｇｘｄｒ（ｉ）より大きければ、ドリフト誤差の更新によりピッチ角の推定値の絶対値を小さくすることができるため、ドリフト誤差学習値を更新する。
【０１７１】
上記の第１の実施の形態において、コンピュータを、ドリフト量補正手段２２、姿勢角オブザーバ２６、スリップ角オブザーバ２８、運動方程式微分量算出手段３０、及びドリフト量推定手段３２の各手段として機能させるプログラムによる情報処理は、図７のフローチャートに示す姿勢角推定処理ルーチンで実現することができる。コンピュータは、相互にバスによって接続されたＣＰＵ、ＲＯＭ、及びＲＡＭ、並びに必要に応じて接続されたＨＤＤで構成され、これらのプログラムは、コンピュータのＣＰＵに接続されているＲＯＭまたはＨＤＤ等の記録媒体に記録される。
【０１７２】
この姿勢角推定処理ルーチンを説明すると、ステップ１００で、前後加速度センサ１２、横加速度センサ１４、上下加速度センサ１６、ロール角速度センサ１８、及びヨー角速度センサ２０から、各検出値に応じたセンサ信号を取得する。
【０１７３】
そして、ステップ１０２において、後述のステップ１１０で１演算周期前に得られた前後加速度センサ１２のセンサドリフト量、横加速度センサ１４のセンサドリフト量、上下加速度センサ１６のセンサドリフト量、ロール角速度センサ１８のセンサドリフト量、及びヨー角速度センサ２０のセンサドリフト量を用いて、上記ステップ１００で取得された前後加速度センサ１２、横加速度センサ１４、上下加速度センサ１６、ロール角速度センサ１８、及びヨー角速度センサ２０から出力されたセンサ信号を補正する。
【０１７４】
そして、ステップ１０４で、後述のステップ１１４で１演算周期前に得られた姿勢角推定値と後述のステップ１１２で１演算周期前に得られた車体速度推定値とを用いて、上記で説明したように、姿勢角オブザーバ２６によってロール角及びピッチ角を推定するために算出されるロール角及びピッチ角の各々の微分量を算出する。
【０１７５】
ステップ１０６において、ステップ１１４で１演算周期前に得られた姿勢角推定値とステップ１１２で１演算周期前に得られた車体速度推定値とを用いて、上記で説明したように、前後車体速度及び横車体速度を推定するために算出される前後車体速度及び横車体速度の各々の微分量を算出する。
【０１７６】
そして、ステップ１０８において、ステップ１１２で１演算周期前に得られた姿勢角推定値とステップ１１４で１演算周期前に得られた車体速度推定値と後述するステップ１２４で１演算周期前に算出された車体ヨー角とに基づいて、上記で説明したように、上記ステップ１０６で算出された車両前後速度及び車両横速度の各々の微分量と、車両運動の運動方程式から得られる車両前後速度及び車両横速度の各々の微分量にセンサドリフト量を考慮した値とが等しくなる関係、上記ステップ１０４で算出されたロール角及びピッチ角の各々の微分量と、車両運動の運動方程式から得られるロール角及びピッチ角の各々の微分量にセンサドリフト量を考慮した値とが等しくなる関係、及び上記ステップ１２４で算出された車体ヨー角と、車両運動の運動方程式から得られる車両ヨー角の微分量にセンサドリフト量を考慮した値を積分した値とが等しくなる関係を用いて、前後加速度センサ１２のセンサドリフト量、横加速度センサ１４のセンサドリフト量、上下加速度センサ１６のセンサドリフト量、ロール角速度センサ１８のセンサドリフト量、及びヨー角速度センサ２０のセンサドリフト量を推定する。
【０１７７】
次のステップ１１０では、上記ステップ１０８で推定された前後加速度センサ１２のセンサドリフト量、横加速度センサ１４のセンサドリフト量、上下加速度センサ１６のセンサドリフト量、ロール角速度センサ１８のセンサドリフト量、及びヨー角速度センサ２０のセンサドリフト量を、前回のセンサドリフト量の推定値を用いて平滑化する。
【０１７８】
次のステップ１１２では、上記ステップ１０４で算出されたロール角及びピッチ角の各々の微分量を積分することで、ロール角及びピッチ角を推定して出力する。
【０１７９】
そして、ステップ１１４において、上記ステップ１０６で算出された前後車体速度及び横車体速度の各々の微分量を積分することで、前後車体速度及び横車体速度を推定し、推定された前後車体速度及び横車体速度に基づいて車体スリップ角を推定して出力し、上記ステップ１００へ戻る。
【０１８０】
また、コンピュータを、車両速度ベクトル推定手段４２、及び車体ヨー角算出手段４４の各手段として機能させるプログラムによる情報処理は、図８のフローチャートに示す車体ヨー角推定処理ルーチンで実現することができる。
【０１８１】
ステップ１２０で、ＧＰＳ受信手段４０から複数のＧＰＳ衛星の情報を取得すると共に、複数のＧＰＳ衛星のＧＰＳ擬似距離データ、ドップラー周波数、ＧＰＳ衛星の位置座標を算出して取得する。
【０１８２】
次に、ステップ１２２で、後述する自車両速度推定処理を実行して、自車両の速度ベクトルを算出し、次に、ステップ１２４で、上記ステップ１２２で算出した速度ベクトル、及び上記ステップ１１２で推定されたスリップ角およびステップ１１４で推定された姿勢角に基づいて、車体ヨー角を算出し、上記ステップ１２０へ戻る。
【０１８３】
次に、図９を参照して、自車両速度推定処理ルーチンについて説明する。
【０１８４】
ステップ１３０で、上記（２８）式に従って、各ＧＰＳ衛星からの受信信号のドップラー周波数から、各ＧＰＳ衛星に対する自車両の相対速度ｖｊを算出する。
【０１８５】
次に、ステップ１３２で、取得した各ＧＰＳ衛星の位置座標の時系列データから、ケプラーの方程式の微分を用いて、各ＧＰＳ衛星の速度ベクトル（ＶＸｊ、ＶＹｊ、ＶＺｊ）を算出する。
【０１８６】
次に、ステップ１３４で、各ＧＰＳ衛星のＧＰＳ擬似距離データを用いて、上記（２９）〜（３１）式に従って、自車両の位置を算出する。なお、ここでは、自車両の位置を、ＧＰＳ衛星の方向（ＧＰＳ衛星と自車両との角度）を求めるために算出しており、自車両の位置として大まかな位置が決定できればよく、例えば、地図などから位置を決定してもよく、また、過去の位置の測定履歴やビーコンなどの情報などから、自車両の位置を決定してもよい。
【０１８７】
次に、ステップ１３６で、上記ステップ１３４で算出された自車両の位置及び取得された各ＧＰＳ衛星の位置座標に基づいて、各ＧＰＳ衛星ｊの位置と自車両の位置との角度関係Ｒｊ（水平方向に対する仰角θｊ、北方向に対する方位角φｊ）を、各ＧＰＳ衛星の方向として算出する。
【０１８８】
次に、ステップ１３８で、上記ステップ１３０で算出された各ＧＰＳ衛星に対する自車両の相対速度ｖｊ、上記ステップ１３２で算出された各ＧＰＳ衛星の速度ベクトルＶｊ（ＶＸｊ、ＶＹｊ、ＶＺｊ）、及び上記ステップ１３６で算出された各ＧＰＳ衛星の方向Ｒｊ（θｊ、φｊ）に基づいて、上記（３２）式に従って、各ＧＰＳ衛星ｊの方向の自車両の速度Ｖｖｊを算出する。（３２）式における自車方向のＧＰＳ衛星ｊの速度Ｖｓｊは、Ｖｓｊ＝Ｒｊ［ＶＸｊ，ＶＹｊ，ＶＺｊ］Ｔにより算出する。
【０１８９】
次に、ステップ１４０で、上記（３３）式、及び（３４）式に従って、自車両の速度ベクトル（Ｖｘ，Ｖｙ，Ｖｚ）の最適値を算出して、リターンする。
【０１９０】
以上説明したように、第１の実施の形態に係る姿勢角スリップ角推定装置によれば、車体鉛直軸に対する姿勢角を推定する手法、車両前後速度を推定し車体スリップ角を推定する手法、およびＧＰＳ衛星から得られる車体ヨー角を利用し、ロール角及びピッチ角を推定するために算出されたロール角及びピッチ角の各々の微分量と車両運動の運動方程式より得られるロール角及びピッチ角の各々の微分量にセンサドリフト量を考慮した値とが等しくなる関係、スリップ角オブザーバによって算出された前後車体速度及び横車体速度の各々の微分量と車両運動の運動方程式より得られる前後車体速度及び横車体速度の各々の微分量にセンサドリフト量を考慮した値とが等しくなる関係、及びＧＰＳドップラーに基づいて算出された車体ヨー角と車両運動の運動方程式より得られる車体ヨー角の微分量にセンサドリフト量を考慮した値を積分した値とが等しくなる関係を用いて、前後加速度、横加速度、上下加速度、ヨー角速度、及びロール角速度の各々を検出するセンサのセンサドリフト量を推定することにより、車両運動の状態に関わらず、安定して、前後加速度、横加速度、上下加速度、ヨー角速度、及びロール角速度の各々を検出するセンサのセンサドリフト量を推定することができる。
【０１９１】
また、姿勢角およびスリップ角の推定値を車体ヨー角算出手段に反映することにより、車両の３次元運動を考慮した車体ヨー角を算出することができる。また、車速変化が少ない条件でも、ＧＰＳ衛星からの情報を利用することにより、精度の高いヨー角速度のセンサドリフト量を推定できる。
【０１９２】
また、ＧＰＳ衛星からの信号を利用した地球座標系に対する車両の速度ベクトル推定値に対し、姿勢角オブザーバおよびスリップ角オブザーバから得られる姿勢角・スリップ角の推定値を適用することによって、車両の３次元運動を考慮した車体ヨー角が得られることに着目し、車両状態量とドリフト量を考慮したときの運動方程式の関係からセンサ信号のドリフト量を推定する。ＧＰＳ衛星から得られる車体ヨー角信号は、衛星の受信環境や車速によってノイズが重畳するため、このノイズを除去し安定的にドリフト量を推定する。
【０１９３】
また、推定されたセンサドリフト量によって補正されたセンサ信号に基づいて、姿勢角としてのロール角及びピッチ角を精度よく推定することができる。
【０１９４】
また、推定されたセンサドリフト量によって補正されたセンサ信号に基づいて、前後車体速度及び横車体速度を精度よく推定することができ、また、車体スリップ角を精度よく推定することができる。
【０１９５】
次に、第２の実施の形態について説明する。なお、第２の実施の形態の姿勢角スリップ角推定装置の構成は、第１の実施の形態と同様の構成となるため、同一符号を付して説明を省略する。
【０１９６】
第２の実施の形態では、忘却係数によるドリフト推定値の平滑化を行う際、車速に応じて忘却係数を変化させている点が、第１の実施の形態と異なっている。
【０１９７】
まず、センサドリフト量を推定する原理について説明する。
【０１９８】
ＧＰＳ信号から得られる車体ヨー角は、衛星と車両との相対速度から求めるため、車速が低い場合は相対速度が小さくなり、ＧＰＳ信号のノイズの影響が大きくなる。したがって、ＧＰＳ信号から得られる車体ヨー角の推定値にノイズが重畳し、ヨー角速度のドリフト量の推定精度への影響が懸念される。そこで、忘却係数によるドリフト推定値の平滑化を行う際、車速に応じて忘却係数を変化させることで、車速低下によるノイズの影響を軽減する。すなわち、上記（６５）式を以下の（６７）式のように変更する。
【０１９９】
【数４０】
2014240266.tif 000042
【０２００】
上記（６７）式に、車速域をＵ0、２Ｕ0、３Ｕ0、３Ｕ0以上の４つの車速域に分割した場合について示す。ここで、Ｕminは、ＧＰＳ信号から演算可能な車速の最低値とし、Ｕ0は、Ｕminより大きな適当な値とする。また、忘却係数はλ２１＞λ２２＞λ２３＞λ２４とする。また、Ｕは、スリップ角オブザーバ２８により算出された前後車体速度である。
【０２０１】
本実施の形態に係るドリフト量推定手段３２では、演算の安定化を図るため、上記（６１）式の演算結果に対し、前回値と前後車体速度を用いて、上記の（６２）〜（６４）、（６６）、（６７）式のように平滑化した値をドリフト推定値とする。
【０２０２】
なお、第２の実施の形態に係る姿勢角スリップ角推定装置の他の構成及び作用については、第１の実施の形態と同様であるため、説明を省略する。
【０２０３】
このように、忘却係数によるドリフト推定値の平滑化を行う際、車速が低いほど、忘却係数を大きくして、センサドリフト量の更新における、ドリフト推定値の影響度合いを小さくすることにより、ＧＰＳ信号のノイズの影響を抑制することができ、より精度良く姿勢角及びスリップ角を推定することができる。
【０２０４】
次に、第３の実施の形態について説明する。なお、第３の実施の形態の姿勢角スリップ角推定装置の構成は、第１の実施の形態と同様の構成となるため、同一符号を付して説明を省略する。
【０２０５】
第３の実施の形態では、センサが補償するドリフト量の最大値を利用し、ノイズの影響を軽減している点が、第１の実施の形態と異なっている。
【０２０６】
まず、センサドリフト量を推定する原理について説明する。
【０２０７】
捕捉衛星数が安定しない場合や、建物や木などによってＧＰＳ信号を常時受信できない場合、ＧＰＳ信号のノイズが大きくなる。そこで、上記（４６）式の関係と、センサが補償するドリフト量の最大値を利用し、ノイズの影響を軽減する。すなわち、上下加速度センサ１６の検出値、ロール角速度センサ１８の検出値、ヨー角速度センサ２０の検出値、姿勢角オブザーバ２６によるロール角推定値、ヨー角推定値、スリップ角オブザーバ２８による前後車両速度推定値、横車両速度推定値から算出される車体ヨー角（上記（４６）式右辺からドリフト量Ｇzdr、Ｐdr、Ｒdrを除いた式から算出される車体ヨー角）と、車体ヨー角算出手段４４から得られる車体ヨー角との誤差が、センサで補償される最大ドリフト量よりも大きい値の場合、ヨー角速度ドリフト推定値の更新を行わない。
【０２０８】
本実施の形態に係るドリフト量推定手段３２では、上記（６１）式の演算結果に対し、前回値と前後車体速度を用いて、上記の（６２）〜（６４）、（６６）式のように平滑化した値をドリフト推定値とする。
【０２０９】
また、ドリフト量推定手段３２は、ヨー角速度センサ２０のドリフト推定値を更新する際、上記（４６）式右辺からドリフト量Ｇzdr、Ｐdr、Ｒdrを除いた式（Δｔ秒の積分値）から算出される車体ヨー角と、車体ヨー角算出手段４４から得られる車体ヨー角との誤差を、ドリフト推定値更新前のＮ点分を取得し、更新前Ｎ点分の平均Ｈψを算出する（図１０参照）。ヨー角速度センサ２０が補償するゼロ点ドリフト最大値をＲｄｒｍａｘとした場合、ドリフト量推定手段３２は、以下の（６８）式が成立する場合、ヨー角速度ドリフト量の推定値の更新を行わない。一方、以下の（６８）式が成立しない場合、上記（６５）式に従って、ヨー角速度ドリフト量の推定値の更新を行う。
【０２１０】
【数４１】
2014240266.tif 000043
【０２１１】
次に、第３の実施の形態のセンサドリフト量の推定方法による推定結果について説明する。なお、センサドリフト量の推定方法の効果を確認するため、ヨー角速度センサ信号にドリフトが重畳した場合を模擬して、センサドリフト量の推定、及びこの推定値を利用した姿勢角の推定を行った。
【０２１２】
図１１に推定結果を示す。ここで、連続衛星捕捉時間とは、途切れることなく連続してＧＰＳ衛星を捕えられた時間とする。すなわち、連続衛星捕捉時間は０ｓになるタイミングでは衛星からの信号が途切れたことを意味する、この結果から、衛星が連続して捕捉できない走行においても、ドリフト推定精度は良好であることが分かる。
【０２１３】
このように、センサが補償するドリフト量の最大値を利用し、車体ヨー角の誤差が、ドリフト量の最大値を超える場合に、ヨー角速度ドリフト量の推定値の更新を行わないようにすることにより、ノイズの影響を抑制することができ、より精度良く姿勢角及びスリップ角を推定することができる。
【０２１４】
次に、第４の実施の形態について説明する。なお、第１の実施の形態と同様の構成、及び作用となる点については、同一符号を付して説明を省略する。
【０２１５】
第４の実施の形態では、車載カメラを用いて取得した、車体ヨー角の時間微分値を利用してドリフト量を推定する点が、第１の実施の形態と異なっている。
【０２１６】
まず、第４の実施の形態に係る姿勢角スリップ角推定装置１００について説明する。図１２に示すように、第４の実施の形態に係る姿勢角スリップ角推定装置１００は、車両運動のｘｙｚ３軸の加速度である前後加速度Ｇｘ、横加速度Ｇｙ、及び上下加速度Ｇｚの各々を検出し、検出値に応じたセンサ信号を出力する前後加速度センサ１２、横加速度センサ１４、及び上下加速度センサ１６と、ロール角速度Ｐを検出し、検出値に応じたセンサ信号を出力するロール角速度センサ１８と、ヨー角速度Ｒを検出し、検出値に応じたセンサ信号を出力するヨー角速度センサ２０とを備えている。
【０２１７】
上下加速度センサ１６、ロール角速度センサ１８、前後加速度センサ１２、横加速度センサ１４、及びヨー角速度センサ２０は、後述するドリフト量推定手段３２によって推定されたセンサドリフト量に基づいて、各センサからのセンサ信号を補正するドリフト量補正手段２２に接続されている。ドリフト量補正手段２２は、ドリフト量推定手段１４０に接続されている。
【０２１８】
撮像装置１１０は、車両の前方又は後方を撮像し、画像の画像信号を生成する撮像部（図示省略）と、撮像部で生成された画像信号をＡ／Ｄ変換するＡ／Ｄ変換部（図示省略）と、Ａ／Ｄ変換された画像信号を一時的に格納するための画像メモリ（図示省略）とを備えている。撮像装置１１０は、Ａ／Ｄ変換された画像信号を運動推定手段１２０に出力する。なお、本実施の形態においては、撮像装置として車載カメラを用いることとする。また、本実施の形態においては、撮像装置は１つであるものとするが、撮像装置を２つ用いてもよい。
【０２１９】
運動推定手段１２０は、自車の運動を推定することにより、車体ヨー角の時間微分値を算出し、算出された車体ヨー角の時間微分値を、ドリフト量推定手段１４０に出力する。なお、車体ヨー角の時間微分値が、回転角の微分量、及び車体ヨー角の微分量の一例である。
【０２２０】
次に、車体ヨー角の時間微分値を利用したドリフト量の推定する原理について説明する。      
【０２２１】
本実施の形態においては、撮像装置１１０（車載カメラ）を利用し、車体ヨー方向に関する上記（２７）式のつり合いの式を新たに加えることで、ヨー角速度ドリフトの推定精度を向上できる。
【０２２２】
次に、撮像装置１１０により得られる画像信号を用いた車体ヨー角の時間微分値の算出方法について説明する。
【０２２３】
運動推定手段１２０は、撮像装置１１０から入力される画像信号に基づく画像を用いて、追跡しやすい特徴点を複数抽出し、次の時刻の画像信号に基づく画像において、当該特徴点がどのように変化したかを探索することによって、特徴点の信頼度を判定しながら、自車両の運動を推定し、当該推定された運動から車体ヨー角の時間微分値を算出する。具体的には、図１３のように、平面運動する車両の座標系を考えると、時刻ｔ１から時刻ｔ２での車体ヨー角はΔψ変化するものとすると、車体ヨー角の時間微分値は、
【０２２４】
【数４２】
2014240266.tif 000044
【０２２５】
となる。実際の車両では、図１３とは異なり、時刻ｔ１から時刻ｔ２の間にロールやピッチも伴うが、特徴点から運動を推定する手法では、これらの運動を分離した後のヨー角時間微分信号を推定することができる。なお、図１３においては、ロール運動、ピッチ運動は考えないものとする。
【０２２６】
運動推定手段１２０を、以下で説明する運動推定処理ルーチンに従って機能ブロックで表すと、図１４に示すように、画像取得部１２２と、特徴点抽出部１２４と、特徴点特徴量算出部１２６と、対応点検索部１２８と、特徴ベクトル生成部１３０と、学習結果記憶部１３２と、判定部１３４と、運動推定部１３６と、を備えた構成で表すことができる。
【０２２７】
画像取得部１２２は、撮像装置１１０から撮像された画像を取得する。
【０２２８】
特徴点抽出部１２４は、撮像装置１１０から得られる複数の撮像画像の各々から、特徴点を複数抽出する。特徴点は、例えば、ＤｏＧ（Ｄｉｆｆｅｒｅｎｃｅ  ｏｆ  Ｇａｕｓｓｉａｎ）、Ｈａｒｒｉｓ−Ｌａｐｌａｃｅなどを用いて、自動的に抽出される。
【０２２９】
特徴点特徴量算出部１２６は、特徴点抽出部１２４において抽出された特徴点の各々の特徴量を算出する。
【０２３０】
対応点検索部１２８は、特徴点特徴量算出部１２６から得られる各特徴点の特徴量を用いて、以下に説明するように、複数の画像間での特徴点の対応付けを行って、一つの画像の特徴点に対応する特徴点を他の画像から検索する。
【０２３１】
特徴点の抽出と特徴量の算出が行われた２枚の画像Ｉ，Ｉ´において、画像Ｉの特徴点ｆｉに対する画像Ｉ´の特徴点を求めるとき、画像Ｉ´から抽出された特徴点から、その特徴量が特徴点ｆｉの特徴量と最も近い特徴点ｆｊ´を求める。このとき、特徴点ｆｉの特徴量と特徴点ｆｊ´の特徴量との距離ｄ１及び、特徴点ｆｉの特徴量と２番目に近い特徴量ｆｋ´との距離ｄ２の比ｄ１／ｄ２が、ある所定のしきい値より小さい場合に、画像Ｉの特徴点ｆｉと画像Ｉ´の特徴点ｆｊ´を対応付ける。
【０２３２】
特徴ベクトル生成部１３０は、特徴点抽出部１２４により抽出された特徴点の各々について、注目特徴点とその周辺に存在する特徴点の特徴量の個数分布であるヒストグラムを用いて、特徴ベクトルを生成する。
【０２３３】
学習結果記憶部１３２は、以下に説明する学習処理により学習結果として得られた特徴ベクトルとインライア、アウトライアを示すラベルとの関係を記憶する。
【０２３４】
学習処理では、まず、予め用意された複数の学習用画像に対して、上述した方法と同様に、特徴点の抽出、特徴点の特徴量の算出、及び対応する特徴点の検索を行い、特徴点、特徴量、および画像間での特徴点の対応関係を求める。そして、オペレータの操作によって、学習用画像から抽出された特徴点のうち、画像間での特徴点の対応関係に誤りがある特徴点、および移動物体上に存在する特徴点をアウトライアとラベル付けし、そうでない特徴点をインライアとラベル付けする。次に、ラベル付けされた特徴点に対して、特徴ベクトル生成部１３０と同様に特徴ベクトルを生成し、そして、ラベル付けされた特徴点、特徴点の特徴ベクトル、およびラベルを入力として、特徴ベクトルに対してインライア、アウトライアを判定する学習処理を行い、特徴ベクトルとインライア、アウトライアを示すラベルとの関係を学習する。なお、特徴点に対してラベル付けされるインライア又はアウトライアは、運動推定の際に特徴点の対応が信頼できるか否かを表わし、特徴点を信頼できるか否かに対応している。
【０２３５】
判定部１３４は、特徴ベクトル生成部１３０によって生成された各特徴点の特徴ベクトル、及び学習結果記憶部１３２に記憶された学習結果としての特徴ベクトルとラベルとの関係に基づいて、各特徴点に対してインライア、アウトライアの判定を行う。
【０２３６】
運動推定部１３６は、判定部１３４によりインライアと判定された特徴点の画像間での対応関係を用いて、複数の画像間の相対的な位置及び姿勢を、複数の画像を撮像したときの自車両の運動として推定する。
【０２３７】
２枚の画像で対応する特徴点の座標から、２つの画像が撮像されたときの位置、姿勢の変化（３次元空間での移動量および回転量）を計算するとき、計算する位置姿勢の変化は、第１の画像から第２の画像への回転行列Ｒと並進ベクトルｔから構成される６自由度の運動である。
【０２３８】
ここで、第１の画像から第２の画像への回転行列Ｒと並進ベクトルｔとの計算方法について説明する。第１の画像におけるｎ点の対応点の画像座標Ｉｉと第２の画像におけるｎ点の対応点の画像座標Ｉ’ ｉとについて（ｎ≧８）、対応点の対応関係が正しくて誤差がなければ、位置及び姿勢の変化を示す行列として、以下の（７０）式を満たす３×３行列Ｆが存在する。
【０２３９】
【数４３】
2014240266.tif 000045
【０２４０】
ただし、Ｉｉ＝(ｕｉ，ｖｉ，１)Ｔ、Ｉ’ｉ＝(ｕ’ｉ，ｖ’ｉ，１)Ｔであり、第１の画像での画像座標（ｕｉ，ｖｉ）の点に対応する第２の画像での点の画像座標が（ｕ’ｉ，ｖ’ｉ）である。
【０２４１】
ここで、上記（７０）式を満たす行列Ｆは、定数倍の不定性を持っている。すなわち、Ｆｓが上記（７０）式を満たす場合には、αＦｓも上記（７０）式を満たす（ただし、αは実数）。よって、行列Ｆを以下の（７１）式のように表すことができる。
【０２４２】
【数４４】
2014240266.tif 000046
【０２４３】
また、上記（７０）式、（７１）式より、以下の（７２）式が得られる。
【０２４４】
【数４５】
2014240266.tif 000047
【０２４５】
ここで、８組以上の対応点Ｉｉ、Ｉ’ｉがあれば、上記（７２）式が少なくとも８つ得られるため、８つの変数ｆ１１〜ｆ３２を求めることができる。つまり、判定部１３４によりインライアと判定された特徴点が８点以上あれば、行列Ｆを計算できる。
【０２４６】
また、誤対応した特徴点や移動物体上の特徴点の中にも、インライアと誤判定された点が含まれることがあるため、インライアと判定された特徴点のうち、その対応関係が上記（７０）式を満たす特徴点の数が最も多くなる行列Ｆを求める。
【０２４７】
実画像で特徴点の対応を求めた場合には、対応する画像座標には誤差が含まれるため、正しい対応関係であっても上記（７０）式を厳密には満たさない。そこで、行列Ｆが与えられたとき、特徴点の対応関係が上記（７０）式を満たすか否かは以下の（７３）式に示す条件を満たすか否かにより判断する。
【０２４８】
【数４６】
2014240266.tif 000048
【０２４９】
ただし、ｄ（ａ，ｂ）はａとｂの距離を表し、τはあらかじめ設定するしきい値である。
【０２５０】
また、撮像装置１１０のキャリブレーション行列Ｋが既知である場合には、上述したように特定された行列Ｆに基づいて、以下の（７４）式、（７５）式に従って、回転行列Ｒと並進ベクトルｔとを計算し、第１の画像及び第２の画像の各々が撮像されたときの撮像装置１１０の運動（位置及び姿勢の変化）として出力する。
【０２５１】
【数４７】
2014240266.tif 000049
【０２５２】
上記の方法により、運動推定部１３６は、第１の画像及び第２の画像における複数組の対応する特徴点の画像座標と、各特徴点のインライア、アウトライアの判定結果とに基づいて、第１の画像及び第２の画像を撮像したときの撮像装置１１０の位置及び姿勢の変化を計算する。運動推定部１３６は、第１の画像が撮像された時間ｔ１と第２の画像が撮像された時間ｔ２との間での撮像装置１１０の姿勢の変化のうちの、車体ヨー角の変化量Δψに基づいて、上記（６９）式に従って、車体ヨー角の時間微分値を算出し、ドリフト量推定手段１４０に出力する。
【０２５３】
次に、車両運動の運動方程式により得られる各微分量の算出方法について説明する。
【０２５４】
本実施の形態において、車体ヨー角と角速度センサ信号との関係式を表す上記（２７）式について、センサ信号に一定のドリフト誤差が重畳していると仮定すると、上記（４６）式は、下記（７６）式に置き換えられる。なお、下記（７６）式のｄψは、運動推定部１３６により、上記(６９)式に従って、算出された車体ヨー角の時間微分値である。
【０２５５】
【数４８】
2014240266.tif 000050
【０２５６】
ここで、ピッチ角速度Ｑには（６）式の推定値を利用した。
【０２５７】
運動方程式微分量算出手段３０は、上記（４４）式、（４５）式に従って、車両運動の運動方程式より得られるロール角φ及びピッチ角θの微分量を算出する。
【０２５８】
運動方程式微分量算出手段３０は、上記（４２）式、（４３）式に従って、車両運動の運動方程式より得られる前後車体速度Ｕ及び横車体速度Ｖの微分量を算出する。
【０２５９】
運動方程式微分量算出手段３０は、上記（７６）式右辺に従って、車両運動の運動方程式より得られる車体ヨー角ψの微分量を算出する。
【０２６０】
次に、センサドリフト量の推定方法について説明する。
【０２６１】
上記（４２）、（４３）、（４４）、及び（４５）をドリフトに関する式に整理すると、上記の（４７）〜（５０）式のように表される。また、上記（７６）式をドリフトに関する式に整理すると下記（７７）式のように表される。
【０２６２】
【数４９】
2014240266.tif 000051
【０２６３】
上記（７７）式は、運動推定手段１２０により算出された車体ヨー角の時間微分値と、運動方程式により得られる車体ヨー角の微分量にセンサドリフト量を考慮した値とが等しくなる関係を表している。
【０２６４】
本実施の形態においては、上記（４７）〜（５０）、（７７）、及び（５３）式から、上記（５６）式のように記述できる。ただし、本実施の形態においては、上記（５８）式で表される条件は、下記（７８）式に置き換わる。
【０２６５】
【数５０】
2014240266.tif 000052
【０２６６】
ここで、ｔｓｍｐはシステム全体の演算に用いるサンプリング時間とする。（５７）、（７８）式左辺の係数行列と右辺のベクトルを一定時間積分すると、上記（５９）式が得られる。
【０２６７】
ドリフト量推定手段１４０は、上記（６１）式に従って、姿勢角オブザーバ２６で算出されるロール角及びピッチ角の各々の微分量と、スリップ角オブザーバ２８で算出される前後車体速度及び横車体速度の各々の微分量と、運動推定手段１２０によって算出された車体ヨー角の時間微分値と、運動方程式微分量算出手段３０で算出されたロール角、ピッチ角、前後車体速度、横車体速度、及び車体ヨー角の各々の微分量とに基づいて、前後加速度センサ１２のセンサドリフト量、横加速度センサ１４のセンサドリフト量、上下加速度センサ１６のセンサドリフト量、ロール角速度センサ１８のセンサドリフト量、及びヨー角速度センサ２０のセンサドリフト量を推定することができる。
【０２６８】
上記の第４の実施の形態において、コンピュータを、ドリフト量補正手段２２、姿勢角オブザーバ２６、スリップ角オブザーバ２８、運動方程式微分量算出手段３０、及びドリフト量推定手段３２の各手段として機能させるプログラムによる情報処理は、図１５のフローチャートに示す姿勢角推定処理ルーチンで実現することができる。コンピュータは、相互にバスによって接続されたＣＰＵ、ＲＯＭ、及びＲＡＭ、並びに必要に応じて接続されたＨＤＤで構成され、これらのプログラムは、コンピュータのＣＰＵに接続されているＲＯＭまたはＨＤＤ等の記録媒体に記録される。
【０２６９】
この姿勢角推定処理ルーチンを説明すると、ステップ２００において、ステップ１１２で１演算周期前に得られた姿勢角推定値と、ステップ１１４で１演算周期前に得られた車体速度推定値と、後述するステップ４０４で１演算周期前に算出された車体ヨー角の時間微分値とに基づいて、上記で説明したように、上記ステップ１０６で算出された車両前後速度及び車両横速度の各々の微分量と、車両運動の運動方程式から得られる車両前後速度及び車両横速度の各々の微分量にセンサドリフト量を考慮した値とが等しくなる関係、上記ステップ１０４で算出されたロール角及びピッチ角の各々の微分量と、車両運動の運動方程式から得られるロール角及びピッチ角の各々の微分量にセンサドリフト量を考慮した値とが等しくなる関係、及び上記ステップ４０４で算出された車体ヨー角の時間微分値と、車両運動の運動方程式から得られる車両ヨー角の微分量にセンサドリフト量を考慮した値とが等しくなる関係を用いて、前後加速度センサ１２のセンサドリフト量、横加速度センサ１４のセンサドリフト量、上下加速度センサ１６のセンサドリフト量、ロール角速度センサ１８のセンサドリフト量、及びヨー角速度センサ２０のセンサドリフト量を推定する。
【０２７０】
また、コンピュータを、運動推定手段１２０として機能させるプログラムによる情報処理は、図１６のフローチャートに示す車体ヨー角の時間微分値推定処理ルーチンで実現することができる。
【０２７１】
次に、図１６に示す、車体ヨー角の時間微分値推定処理ルーチンについて説明する。ステップ４００で、撮像装置１１０によって連続して撮像した第１の画像及び第２の画像の画像信号を取得する。
【０２７２】
次に、ステップ４０２で、後述する運動推定処理を実行して、自車両の運動を推定し、次に、ステップ４０４で、上記ステップ４０２で推定された自車両の運動に基づいて、車体ヨー角の時間微分値を算出し、上記ステップ４００へ戻る。
【０２７３】
次に、図１７を参照して、運動推定処理ルーチンについて説明する。
【０２７４】
ステップ５００において、上記ステップ４００で取得した第１の画像及び第２の画像から特徴点を複数抽出する。そして、ステップ５０２において、上記ステップ５００で第１の画像及び第２の画像から抽出された複数の特徴点の各々について、特徴量を算出する。
【０２７５】
次のステップ５０４では、上記ステップ５０２で算出された特徴量に基づいて、上記ステップ５００で抽出された第１の画像上の複数の特徴点の各々に対応する特徴点を、第２の画像上で検索して追跡する。
【０２７６】
そして、ステップ５０６において、第１の画像上の特徴点の各々について、特徴ベクトルを生成し、ステップ５０８において、学習結果記憶部１３２に記憶されている学習された特徴ベクトルとラベルとの関係、及び上記ステップ５０６で生成された各特徴点の特徴ベクトルに基づいて、第１の画像上の特徴点の各々について、インライアかアウトライアかを判定する。
【０２７７】
次のステップ５１０では、上記ステップ５０８でインライアと判定された特徴点と対応する特徴点との対応関係の任意の組み合わせに基づいて、行列Ｆを計算し、また、対応関係が上記（７３）式を満たす特徴点の数が最も多い行列Ｆを求める。そして、求められた行列Ｆに基づいて、上記（７４）式及び（７５）式に従って、第１の画像を撮像したときの撮像装置１１０の第１の位置及び第１の姿勢と、第２の画像を撮像したときの撮像装置１１０の第２の位置及び第２の姿勢との変化として、撮像装置１１０の運動のＸＹＺ軸方向の移動量及びＸＹＺ軸を基準とする回転量を算出し、撮像装置１１０を搭載した自車両のその２時刻間における運動の推定結果として外部装置に出力して、運動推定処理ルーチンを終了する。
【０２７８】
以上説明したように、第４の実施の形態に係る姿勢角スリップ角推定装置によれば、車体鉛直軸に対する姿勢角を推定する手法、車両前後速度を推定し車体スリップ角を推定する手法、および撮像装置から得られる車体ヨー角の時間微分値を利用し、ロール角及びピッチ角を推定するために算出されたロール角及びピッチ角の各々の微分量と車両運動の運動方程式より得られるロール角及びピッチ角の各々の微分量にセンサドリフト量を考慮した値とが等しくなる関係、スリップ角オブザーバによって算出された前後車体速度及び横車体速度の各々の微分量と車両運動の運動方程式より得られる前後車体速度及び横車体速度の各々の微分量にセンサドリフト量を考慮した値とが等しくなる関係、及び撮像装置を利用して算出された車体ヨー角の時間微分値と車両運動の運動方程式より得られる車体ヨー角の微分量にセンサドリフト量を考慮した値とが等しくなる関係を用いて、前後加速度、横加速度、上下加速度、ヨー角速度、及びロール角速度の各々を検出するセンサのセンサドリフト量を推定することにより、車両運動の状態に関わらず、安定して、前後加速度、横加速度、上下加速度、ヨー角速度、及びロール角速度の各々を検出するセンサのセンサドリフト量を推定することができる。
【０２７９】
また、車両に搭載されたカメラの画像から車両の運動を推定し、車体スリップ角の影響を受けない車体ヨー角の時間微分値が得られることにより、車両状態量とドリフト量を考慮したときの運動方程式の関係からセンサ信号のドリフト量を推定することができる。
【０２８０】
次に、第５の実施の形態について説明する。なお、第５の実施の形態の姿勢角スリップ角推定装置の構成及び作用について、第１〜第４の実施の形態と同様の構成となる部分については、同一符号を付して説明を省略する。
【０２８１】
第５の実施の形態では、センサが補償するドリフト量の最大値を利用し、ノイズの影響を軽減している点が、第１〜第４の実施の形態と異なっている。
【０２８２】
まず、センサドリフト量を推定する原理について説明する。
【０２８３】
周辺環境が極端に明るいまたは暗い場合や、車両は運動しているのに対し同じような画像が続く場合など、特徴点を捉えにくい環境下では、カメラから推定されるヨー角の時間微分値に大きなノイズが重畳しやすい。そこで、上記（７６）式の関係と、センサが保障するドリフト量の最大値を利用し、ノイズの影響を軽減する。すなわち、上下加速度センサ１６の検出値、ロール角速度センサ１８の検出値、ヨー角速度センサ２０の検出値、姿勢角オブザーバ２６によるロール角推定値、ヨー角推定値、スリップ角オブザーバ２８による前後車両速度推定値、横車両速度推定値から算出される車体ヨー角の微分量（上記（７６）式の右辺からドリフト量Ｇzdr、Ｐdr、Ｒdrを除いた式から算出される車体ヨー角の微分量）と、運動推定手段１２０から得られる車体ヨー角の時間微分値との誤差が、センサで補償される最大ドリフト量よりも大きい値の場合、ヨー角速度ドリフト推定値の更新を行わない。
【０２８４】
本実施の形態に係るドリフト量推定手段３２では、上記（６１）式の演算結果に対し、前回値と前後車体速度を用いて、上記の（６２）〜（６４）、（６６）式のように平滑化した値をドリフト推定値とする。
【０２８５】
また、ドリフト量推定手段１４０は、ヨー角速度センサ２０のドリフト推定値を更新する際、上記（７６）式右辺からドリフト量Ｇzdr、Ｐdr、Ｒdrを除いた式から算出される車体ヨー角の微分量と、運動推定手段１２０から得られる車体ヨー角の時間微分値との誤差を、ドリフト推定値更新前のＮ点分を取得し、更新前Ｎ点分の平均Ｈψを算出する（図１８参照）。ヨー角速度センサ２０が補償するゼロ点ドリフト最大値をＲｄｒｍａｘとした場合、ドリフト量推定手段１４０は、下記の（７９）式が成立する場合、ヨー角速度ドリフト量の推定値の更新を行わない。一方、下記の（７９）式が成立しない場合、上記（６５）式に従って、ヨー角速度ドリフト量の推定値の更新を行う。
【０２８６】
【数５１】
2014240266.tif 000053
【０２８７】
以上説明したように、第５の実施の形態に係る姿勢角スリップ角推定装置によれば、センサが補償するドリフト量の最大値を利用し、車体ヨー角の時間微分値の誤差が、ドリフト量の最大値を超える場合に、ヨー角速度ドリフト量の推定値の更新を行わないようにすることにより、ノイズの影響を抑制することができ、より精度良く姿勢角及びスリップ角を推定することができる。
【０２８８】
また、画像から得られる車体ヨー角の時間微分信号は、撮影環境によってノイズが重畳するため、このノイズを除去し、安定的にドリフト量を推定することができる。
【符号の説明】
【０２８９】
１０ 姿勢角スリップ角推定装置
１２ 前後加速度センサ
１４ 横加速度センサ
１６ 上下加速度センサ
１８ ロール角速度センサ
２０ ヨー角速度センサ
２２ ドリフト量補正手段
２６ 姿勢角オブザーバ
２８ スリップ角オブザーバ
３０ 運動方程式微分量算出手段
３２ ドリフト量推定手段
４０ ＧＰＳ受信手段
４２ 車両速度ベクトル推定手段
４４ 車体ヨー角算出手段
１００ 姿勢角スリップ角推定装置
１１０ 撮像装置
１２０ 運動推定手段
１４０ ドリフト量推定手段
【図１】
2014240266.tif 000054
【図２】
2014240266.tif 000055
【図３】
2014240266.tif 000056
【図４】
2014240266.tif 000057
【図５】
2014240266.tif 000058
【図６】
2014240266.tif 000059
【図７】
2014240266.tif 000060
【図８】
2014240266.tif 000061
【図９】
2014240266.tif 000062
【図１０】
2014240266.tif 000063
【図１１】
2014240266.tif 000064
【図１２】
2014240266.tif 000065
【図１３】
2014240266.tif 000066
【図１４】
2014240266.tif 000067
【図１５】
2014240266.tif 000068
【図１６】
2014240266.tif 000069
【図１７】
2014240266.tif 000070
【図１８】
2014240266.tif 000071