(57)【特許請求の範囲】
【請求項１】
所定方向の移動体の加速度を計測する加速度計、前記移動体で発生する所定軸線周りの角速度を計測する角速度計、前記移動体の計器から取得した速度検出用データに基づいて移動速度を計測する速度計測機器、前記移動体の操舵角度を計測する操舵角度計測機器を有するセンサ部から出力されるデータを取得するデータ取得手段と、
操舵角度と移動速度との乗算により得られる推定角速度と前記角速度計で計測された計測角速度との差分値をフィルタリングすることで当該計測角速度に含まれるドリフト成分を検出する第１手段と、
加速度を移動速度で除算して得られる推定角速度と前記計測角速度との差分値をフィルタリングすることで当該計測角速度に含まれるドリフト成分を検出する第２手段と、
第１手段による検出結果と第２手段による検出結果とを移動速度に応じた割合でミキシングし、このミキシング結果に基づいて前記ドリフト成分を定量化する手段とを具備し、定量化されたドリフト成分を前記計測角速度から除去するように構成されている、
ドリフト除去装置。
【請求項２】
所定方向の移動体の加速度を計測する加速度計、前記移動体で発生する所定軸線周りの角速度を計測する角速度計、前記移動体の計器から取得した速度検出用データに基づいて移動速度を計測する速度計測機器、前記移動体の操舵角度を計測する操舵角度計測機器を有するセンサ部から出力されるデータを取得するデータ取得手段と、
操舵角度と移動速度との乗算により得られる推定角速度と前記角速度計で計測された計測角速度との差分値をフィルタリングすることで当該計測角速度に含まれるドリフト成分を検出する第１手段と、
加速度を移動速度で除算して得られる推定角速度と前記計測角速度との差分値をフィルタリングすることで当該計測角速度に含まれるドリフト成分を検出する第２手段と、
操舵角度が定点をクロスしたときの前記計測角速度を時系列に計測することで当該計測角速度に含まれるドリフト成分を検出する第３手段と、
第１手段による検出結果と第２手段による検出結果とを移動速度に応じた割合で第１ミキシングするとともに第１ミキシングの結果と第３手段による検出結果とを前記定点のクロス率に応じた割合で第２ミキシングし、この第２ミキシング結果に基づいて前記ドリフト成分を定量化する第４手段とを具備し、
第４手段により定量化されたドリフト成分を前記計測角速度から除去するように構成されている、ドリフト除去装置。
【請求項３】
前記第１手段と前記第２手段の少なくとも一方は、前記差分値をその時定数が移動速度に応じて変化可能な低域通過フィルタに入力することにより前記ドリフト成分を検出するように構成されることを特徴とする、
請求項１又は２記載のドリフト除去装置。
【請求項４】
前記第１手段と前記第２手段の少なくとも一方は、一定のレベル変動範囲内の計測角速度のみを前記推定角速度と比較することにより前記差分値を得るように構成されることを特徴とする、
請求項１、２又は３記載のドリフト除去装置。
【請求項５】
前記第４手段は、前記第２ミキシングの結果からオフセットを含むドリフト基線を検出するとともにこのドリフト基線を中心に変動する所定のバンド幅の信号成分をフィルタリングすることで以降のドリフト基線を推定することにより前記ドリフト成分を定量化するように構成されることを特徴とする、
請求項２記載のドリフト除去装置。
【請求項６】
前記第４手段は、移動体の停止が検出されたときはそのときの前記第２ミキシングの結果をオフセット及びドリフト成分を含むオフセット角速度として決定し、移動体が走行中は前記第２ミキシングの結果と任意の時点での収束度とをもとに収束すべきオフセット角速度を推定するとともに、決定したオフセット角速度及び推定したオフセット角速度に基づいて前記ドリフト基線を検出することを特徴とする、
請求項５記載のドリフト除去装置。
【請求項７】
前記第４手段は、前記センサ部から出力される計測データのばらつき度の大小により停止又は走行を検出することを特徴とする、
請求項６記載のドリフト除去装置。
【請求項８】
前記移動体が車両であり、前記加速度が横加速度であり、前記角速度が前記車両において発生した方位角速度であることを特徴とする、
請求項１乃至７のいずれかの項記載のドリフト除去装置。
【請求項９】
所定方向の移動体の加速度を計測する加速度計、前記移動体で発生する所定軸線周りの角速度を計測する角速度計、前記移動体の計器から取得した速度検出用データに基づいて移動速度を計測する速度計測機器、前記移動体の操舵角度を計測する操舵角度計測機器を有するセンサ部と、
このセンサ部から計測データを取得するとともにこの計測データに基づいて前記角速度計で計測した角速度のドリフト成分を除去する請求項１乃至８のいずれかの項記載のドリフト除去装置とを備え、
ドリフト除去された角速度を含む前記センサ部の計測データを実時間で出力することを特徴とする、
移動体の挙動検出センサ。
【請求項１０】
車両の横加速度を計測する加速度計、前記車両の方位角速度を計測する方位角速度計、前記車両の計器から取得した速度検出用データに基づいて走行速度を計測する速度計測機器、前記車両の操舵角度を計測する操舵角度計測機器を有するセンサ部から出力されるデータを用いて前記方位角速度計で計測した計測角速度に含まれるドリフト成分を除去する方法であって、
操舵角度と走行速度との乗算により得られる推定角速度と前記計測角速度との差分値をフィルタリングすることで当該計測角速度に含まれるドリフト成分を検出する第１処理、
加速度を走行速度で除算して得られる推定角速度と前記計測角速度との差分値をフィルタリングすることで当該計測角速度に含まれるドリフト成分を検出する第２処理、
操舵角度が定点をクロスしたときの前記計測角速度を時系列に計測することで当該計測角速度に含まれるドリフト成分を検出する第３処理を実行するとともに、
第１処理による検出結果と第２処理による検出結果とを走行速度に応じた割合で第１ミキシングした後、この第１ミキシングの結果と第３処理による検出結果とを前記定点のクロス率に応じた割合で第２ミキシングし、
さらに、第２ミキシング結果に基づいて前記ドリフト成分を定量化し、
定量化されたドリフト成分を前記計測角速度から除去することを特徴とする、
ドリフト除去方法。
【発明の詳細な説明】
【０００１】
【発明の属する技術分野】
本発明は、車両のような移動体の挙動の特徴を検出する挙動検出センサに係り、特に、そのセンサ部に角速度計を含む場合の当該角速度計の計測結果をより正確にするための手法に関する。
【０００２】
【発明の背景】
右折、左折、前進、後退、停止、発進等の車両の挙動特徴を検出するセンサとして、角速度計の一種であるヨーレートセンサを具備する挙動検出センサが知られている。ヨーレートセンサで計測される角速度（以下、「ヨーレート」）は、従来、次のように利用されている。
（１）操舵角度の推定。
運転者がハンドル操作したときの基準位置に対する操作量を検出する手段として、ハンドルとは分離された部位に非接触に配される操舵角計がある。操舵角計では、ハンドル操作によって生じた操舵角度（ハンドル角ともいう）をハンドルに連結されている機構からの信号によって検出するので、絶対的な操舵角度を正確に得ることはできない。
そこでヨーレートセンサで計測したヨーレートをリファレンスの一つとして、分解能の粗い操舵角計による計測結果を補完する。
【０００３】
（２）すべり角計測
ハンドル操作に応じて車両は旋回するが、旋回時の道路が雪道であったり、あるいは氷結したりして「すべり」が発生する場合がある。この場合は、操舵角計の検出結果とは異なる値のヨーレートが発生する。この現象を利用して、操舵角計の検出結果とヨーレートとの差分値から車両における「すべり」の発生とその「すべり」の大きさ、つまり「すべり角」を推定し、この推定結果を車両の挙動制御に利用する。
【０００４】
挙動検出センサにおけるヨーレートセンサに望まれる理想的な条件は、オフセットを含むドリフト成分が全くなく、計測値がそのまま実際の車両で発生したヨーレートを表すことである。ヨーレートの精度が悪いとリファレンスが狂い、操舵角度の検出結果やすべり角検出の結果に誤差が生じる。
しかしながら、ヨーレートセンサに角速度計を用いる限り、計器構造及び電子回路部分の温度特性等で、時間の経過と共に発生するドリフト成分を無視できない。そのため、従来は、角速度計をセンサ部に用いる場合は、車両の挙動制御を高精度で行うことができない、という問題があった。角速度計以外の計器を用いたり、あるいは上記の電子回路部分に温度補償回路を設けたりすることも考えられるが、そうすると、挙動検出センサのコストを下げることができない。
【０００５】
そこで本発明は、車両等の移動体に搭載される角速度計によるドリフト成分を簡易且つ正確に除去することができる、ドリフト除去装置を提供することを主たる課題とする。
本発明は、また、当該移動体の停止状態から高速走行までに柔軟に対応することができ、さらに、移動体に「すべり」が発生する状態にも柔軟に対応することができる、汎用的なドリフト除去装置を提供することをも課題とする。
【０００６】
【課題を解決するための手段】
本発明のドリフト除去装置は、所定方向の移動体の加速度を計測する加速度計、前記移動体で発生する所定軸線周りの角速度を計測する角速度計、前記移動体の計器から取得した速度検出用データに基づいて移動速度を計測する速度計測機器、前記移動体の操舵角度を計測する操舵角度計測機器を有するセンサ部から出力されるデータを取得するデータ取得手段と、操舵角度と移動速度との乗算により得られる推定角速度と前記角速度計で計測された計測角速度との差分値をフィルタリングすることで当該計測角速度に含まれるドリフト成分を検出する第１手段と、加速度を移動速度で除算して得られる推定角速度と前記計測角速度との差分値をフィルタリングすることで当該計測角速度に含まれるドリフト成分を検出する第２手段と、第１手段による検出結果と第２手段による検出結果とを移動速度に応じた割合でミキシングし、このミキシング結果に基づいて前記ドリフト成分を定量化する手段とを具備し、定量化されたドリフト成分を前記計測角速度から除去するように構成される。
【０００８】
上記のセンサ部の構成では、推定角速度の精度は移動速度によって異なる。そこで、低速移動時と高速移動時の検出結果をミキシング（混合）することで、移動速度に応じたドリフト成分の傾向を求めることができる。
【０００９】
本発明の他のドリフト除去装置は、上記のセンサ部から出力されるデータを取得するデータ取得手段と、操舵角度と移動速度との乗算により得られる推定角速度と前記角速度計で計測された計測角速度との差分値をフィルタリングすることで当該計測角速度に含まれるドリフト成分を検出する第１手段と、加速度を移動速度で除算して得られる推定角速度と前記計測角速度との差分値をフィルタリングすることで当該計測角速度に含まれるドリフト成分を検出する第２手段と、操舵角度が定点をクロスしたときの前記計測角速度を時系列に計測することで当該計測角速度に含まれるドリフト成分を検出する第３手段と、第１手段による検出結果と第２手段による検出結果とを移動速度に応じた割合で第１ミキシングするとともに第１ミキシングの結果と第３手段による検出結果とを前記定点のクロス率に応じた割合で第２ミキシングし、この第２ミキシング結果に基づいて前記ドリフト成分を定量化する第４手段とを具備し、第４手段により定量化されたドリフト成分を前記計測角速度から除去するように構成されたものである。
定点クロス時には計測角速度は本来的には一定値（零を含む）となるべきであるが、実際にはドリフト成分（オフセットを含む）が存在するため、これに基づいて変化する。そこで、この定点クロス時の計測角速度と、低速移動時と高速移動時の検出結果のミキシング結果（第１ミキシング結果）をさらにミキシング（第２ミキシング）することで、実際に発生しているドリフト成分に近似したドリフト成分を定量化する。
【００１０】
好ましい実施の形態では、上記の第１手段と第２手段の少なくとも一方を、差分値をその時定数が移動速度に応じて変化可能な低域通過フィルタに入力することによりドリフト成分の傾向を検出するように構成する。また、一定のレベル変動範囲内の計測角速度のみを前記推定角速度と比較することにより前記差分値を得るように構成する。
【００１１】
第４手段は、前記第２ミキシングの結果からオフセットを含むドリフト基線を検出するとともにこのドリフト基線を中心に変動する所定のバンド幅の信号成分をフィルタリングすることで以降のドリフト基線を推定することによりドリフト成分を定量化するように構成される。オフセットを含むドリフト基線の検出は、具体的には、移動体の停止が検出されたときはそのときの前記第２ミキシングの結果をオフセット及びドリフト成分を含むオフセット角速度として決定し、移動体が走行中は前記第２ミキシングの結果と任意の時点での収束度とをもとに収束すべきオフセット角速度を推定するとともに、決定したオフセット角速度及び推定したオフセット角速度に基づいて行う。停止又は走行の検出は、前記センサ部から出力される計測データのばらつき度の大小により行う。
【００１２】
本発明のドリフト除去方法は、車両の横加速度を計測する加速度計、車両の方位角速度を計測する方位角速度計、車両の計器から取得した速度検出用データに基づいて走行速度を計測する速度計測機器、車両の操舵角度を計測する操舵角度計測機器を有するセンサ部から出力されるデータを用いて方位角速度計で計測した計測角速度に含まれるドリフト成分を除去する方法であって、
操舵角度と走行速度との乗算により得られる推定角速度と前記計測角速度との差分値をフィルタリングすることで当該計測角速度に含まれるドリフト成分を検出する第１処理、加速度を走行速度で除算して得られる推定角速度と前記計測角速度との差分値をフィルタリングすることで当該計測角速度に含まれるドリフト成分を検出する第２処理、操舵角度が定点をクロスしたときの前記計測角速度を時系列に計測することで当該計測角速度に含まれるドリフト成分を検出する第３処理を実行するとともに、
第１処理による検出結果と第２処理による検出結果とを走行速度に応じた割合で第１ミキシングした後、この第１ミキシングの結果と第３処理による検出結果とを前記定点のクロス率に応じた割合で第２ミキシングし、さらに、第２ミキシング結果に基づいて前記ドリフト成分を定量化し、定量化されたドリフト成分を前記計測角速度から除去することを特徴とする。
【００１３】
【発明の実施の形態】
以下、本発明を車両の挙動特徴を検出するための挙動検出センサに適用した場合の実施の形態を説明する。
この挙動検出センサは、図１に示すように、同一又は複数の筐体に収容されるセンサ部１０とドリフト除去装置２０とからなり、ドリフト除去装置２０でドリフト除去されたヨーレートを含む計測データを、図示しない車両姿勢制御装置へ出力するものである。車両姿勢制御装置は、当該車両に取り付けられる安全運転のための挙動制御装置である。
センサ部１０は、Ｘ加速度計１１，Ｙ加速時計１２，ヨーレートセンサ１３及びこれらの計器で計測したデータ（計測データ）をドリフト除去装置２０に送出するための電子回路１４を有している。
【００１４】
Ｘ加速度計１１は、挙動検出センサを搭載した車両の前後方向（Ｘ方向）の加速度を計測するための加速度計であり、Ｙ加速度計１２は、車両の横方向（Ｙ方向）加速度を計測する加速度計であり、それぞれ三次元軸の該当軸線上に配置される。ヨーレートセンサ１３は、ヨーレートを計測するための方位角速度計である。以下、ヨーレートセンサ１３で実際に計測したヨーレートをこの実施形態では「計測角速度」と称する。
ドリフト除去装置２０は、Ａ／Ｄコンバータ２１、車速パルスカウンタ２２、操舵角度エンコーダ２３及び主制御部２４を含んで構成される。
Ａ／Ｄコンバータ２１は、センサ部１０から出力される各種計測データ（アナログ信号）を主制御部２４が処理可能な形態のディジタル信号に変換する。車速パルスカウンタ２２は、図示しない車両計器から入力される車速パルスの数を計測する速度計測用計器である。車速パルスは、車両の走行速度Ｖｘを求めるために入力する。操舵角度エンコーダ２３は、運転者によるハンドルの操舵角度をハンドルの操舵角を表す信号に基づいて検出する操舵角計測用計器である。
【００１５】
本実施形態では、Ｘ、Ｙ加速度計１１，１２で計測した加速度、車速パルスカウンタ２２のカウント値に基づいて演算された走行速度、操舵角度エンコーダ２３で検出された操舵角度をリファレンスとして用い、当該車両で発生したであろうヨーレートを演算によって推定する。以下、演算によって推定したヨーレートを上記の「計測角速度」に対して「推定角速度」と称する。
主制御部２４は、ＣＰＵ、ＲＡＭ、ＲＯＭを有し、ＲＯＭに格納されているプログラムをＲＡＭに展開してＣＰＵが実行することによって形成される制御機構であり、上記の推定角速度と計測角速度とを比較して当該計測角速度に含まれるドリフト成分の時系列な発生傾向を導出し、このドリフト成分の発生傾向に基づいて当該ドリフト成分を定量化するとともに、定量化したドリフト成分を計測角速度から除去する機能を有する。
【００１６】
具体的には、演算に際して不要となる成分を除去する低域通過フィルタ機能（以下、「ＬＰＦ」）、高域通過フィルタ機能（以下、「ＨＰＦ」）、及びフィルタ時定数制御を制御する機能を形成し、車両の停止、旋回、直進等の車両の挙動の際にセンサ部１０で計測された計測データに基づいて推定角速度を演算する。推定角速度はドリフトが発生しない計器からの計測データに基づいて演算されたものなので、計測角速度との差分値は、ヨーレートセンサ１３及び電子回路１４で発生したドリフト成分（オフセット込み）ということになる。本実施形態では、このドリフト成分を正確に定量化し、これを計測角速度から除去する仕組みを実現するものである。
【００１７】
次に、上記のように構成される挙動検出センサの動作を、ドリフト除去装置２０における処理内容を中心に説明する。
本実施形態では、δ−γ方式、遠心力方式、定点クロス方式、基線バンド方式という要素処理技術を採用し、これらを適宜アレンジすることによってドリフト成分を定量化する。まず、これらの要素処理技術について説明する。
【００１８】
［δ−γ方式］
操舵角度エンコーダ２３で検出した操舵角度δと車速パルスカウンタ２２により求めた走行速度Ｖｘとの乗算により推定角速度を求め、この推定角速度と計測角速度との差分値から時定数可変のＬＰＦで高周波信号を除去することで、ドリフト成分の傾向を検出する。上記の乗算は、ヨーレートγがδ・Ｖｘの関係式より導かれることに基づく。従って、このδ−γ方式は、実際に車両において発生するヨーレートの変化が滑らかな状況、具体的には、１．０度／秒／秒（角速度）以内で変動する計測角速度が得られる運転状態の範囲で実施する。図２の破線部分のような激しい動きの部分ではγ＝δ・Ｖｘの関係式が成り立たなくなり、また、図３のように大きな旋回のためにハンドル角の変化が過大になる場合（非直線的に変化）でもやはりγ＝δ・Ｖｘの関係式が成り立たなくなるので、ハンドル角の変化勾配が直線的となる領域（図３の破線部分の領域）でのみ上記の乗算を行う。
【００１９】
ＬＰＦの時定数τ（＝κ・γ／Ｖｘ：κは実測により定めた関数）は、走行速度Ｖｘが大きいほど短くしてヨーレートγが大きい動作時に長くし、上記差分値の変化が計測角速度の変動傾向を的確に捉えるような値にする。つまり、カーブ走行のような低速走行時にはヨーレートの振幅やハンドル角が大きく、かつ計測角速度の変動が低周波になる傾向があるため、時定数τを大きくし、ＬＰＦのカットオフ周波数を低くする。一方、直線走行のような高速走行時は、ハンドル角は小さいが計測角速度が小刻みに変動するのが一般的なので、時定数τを小さくし、ＬＰＦのカットオフ周波数を高くする。このことを図で示したのが図４である。
図４（Ａ）は、低速走行時の計測角速度（Ａ／Ｄ変換されたもの）の変動の様子、図４（Ｂ）は、高速走行時の計測角速度（Ａ／Ｄ変換されたもの）の変動の様子を示した図である。図中、ＳＴは時定数τが大きい場合の変動カーブ、ＦＴは時定数が小さい場合の変動カーブである。
【００２０】
時定数τを変化する場合は、計測角速度の周波数成分の解析処理が前提となる。この解析処理では、入力された計測角速度を２０秒程度の固定時定数のＨＰＦでフィルタリングした後、その絶対値を一定時間累積する。通常、絶対値が低振幅且つ小刻みに変動する高周波系では累積値はほぼ零になるのに対し、絶対値の振幅差が大きい低周波系では累積値は有限になるので、この累積値を正規化した値Ｈｓを周波系を切り分ける際のパラメータとしてＬＰＦの時定数τを決定する。
なお、絶対値の累積時間は、正規化した値Ｈｓの変化が一定のレベル範囲に入る時間とする。
δ−γ方式では、高速走行時に比べて低速走行時には操舵角度δの誤差を無視することができる。そのため、この方式は、低速走行時における推定角速度の演算に適した方式といえる。特に、車両停止時にはヨーレートが零になるので（なるはずなので）、ドリフト成分の傾向を観測する上では、より好ましい手法である。走行速度Ｖｘが変化する際に、操舵角度δの誤差を適宜調整することで、中速走行時にも対応可能な手法となる。
【００２１】
［遠心力方式］
Ｙ加速度計１２で計測した横加速度Ｇｙと車速パルスカウンタ２２により求めた走行速度Ｖｘとに基づき、Ｇｙ÷Ｖｘの除算式から推定角速度を演算する。この演算は、車両の旋回時に発生する横加速度Ｇｙ、ヨーレートγ、走行速度Ｖｘとの間に、Ｇｙ＝γ・Ｖｘの関係式（遠心力の定義より）が成立することに基づく。
【００２２】
遠心力方式では、走行速度Ｖｘで除算を実行すること、低速走行時の遠心力の誤差が絶対値に比べ大きくなることから、１．０Ｋｍ／ｈ以下の低速走行時での除算結果は採用しない（演算を実行しない）。除算により得られた推定角速度と計測角速度との差分値を求め、この差分値をＬＰＦを通過させることでドリフト成分の傾向を検出する点、除算する操舵角度δの範囲を図３の直線領域（破線領域）とする点、ＬＰＦの時定数τを周波数帯に応じて可変にする点については、δ−γ方式の場合と同様である。
この遠心力方式は、高速走行になるほど横加速度Ｇｙの誤差の影響がなくなるので、高速走行時での演算にはより適した手法となる。
【００２３】
［定点クロス方式］
操舵角度δが定点、例えば零点をクロスしたときのヨーレートが、理想的には零となることを利用してドリフト成分を求める方式である。簡単な例として直進走行する場合を想定する。操舵角度δが零点付近をクロスするとき（ゼロクロス）の計測角速度が有限値であった場合、理想的には、それはドリフト成分に起因したものである。そこで、操舵角度δが零点のときの計測角速度を時系列に沿って計測することで、ドリフト成分の傾向を容易に検出することができる。
【００２４】
また、零点でなくとも、ある特定のハンドル角を最も多くクロスする点のヨーレートがほぼ一定である（べきである）ことを利用して、ドリフト成分の傾向を検出することもできる。例えば滑らかなカーブが連続したり、特殊な実験円周回などのコースでは、操舵角度δがゼロクロスせず、特定のハンドル角を一定時間維持する。この場合は、操舵角度δをＬＰＦに通すことで最多ハンドル角を特定し、この最多ハンドル角の点（これも定点となる）で、計測角速度を時経列に観測する。図５は直線走行から緩やかなカーブ走行に移行し、その後に直線走行に戻った場合の操舵角度δの変化を示した図であり、図６の破線は、このときの最多ハンドル角である。直線走行の場合の最多ハンドル角は零点付近となるのに対し、カーブ走行の場合はあるハンドル角を一定時間維持する。
零点と最多ハンドル角でのクロス点とを併用した場合のドリフト成分の発生傾向は、図７のようになる。すなわち、零点（操舵角度δ０）を通過するときのドリフト成分（計測角速度）を累積させると図７（Ａ）のようになる。点ａの時点で緩やかなカーブにさしかかり、操舵角度がδ１となったとする。この時点以降のドリフト成分は図７（Ｂ）のように変化する。最終的には、図７（Ｃ）のように上記の２つのドリフト成分をつなげることにより、全体的なドリフト成分の傾向を求めることができる。
【００２５】
［基線バンド方式］
基線バンド方式は、図８に示すように、ドリフト成分を含む信号の変動幅の中心の変化線であるドリフト基線を求め、このドリフト基線を中心に変動する所定の振幅内（バンド幅）の信号成分をもとに、以降のドリフト基線を推定し、これによって当該ドリフト成分を定量化する方式である。ドリフト基線は、上記の３つの方式による計測結果をミキシングしてドリフト成分の発生傾向を求め、これにオフセット成分を考慮することによって求めることができる。
【００２６】
この基線バンド方式による処理の内容を、図９〜図１１をもとに詳細に説明する。
図９は、基線バンド方式の全体的な処理手順図である。
基線バンド方式は、δ−γ方式によるドリフト成分の傾向と遠心力方式によるドリフト成分の傾向とを同時に検出することを前提とし、これらの検出結果を互いに補完するために第１ミキシングを行う。上述のように、各方式による発生傾向の検出精度は、車両の走行速度Ｖｘによって大きく異なるため、両者のミキシング割合を走行速度Ｖｘをパラメータとして随時変化させる。δ−γ方式は低速走行時に高精度が維持されるため、その結果のデータに低速走行用の強調パラメータ（１−κ１（Ｖｘ））を乗算する。反対に高速走行時に高精度化される遠心力方式の場合は、その結果データに高速走行用の強調パラメータ（κ１（Ｖｘ））を乗算する。κ１は走行速度に依存した関数である。
次に、定点クロス方式により検出したドリフト成分の傾向を第１ミキシングの結果に反映させる。すなわち、これらのミキシング（第２ミキシング）を行う。これにより、運転者による実際のハンドル操作がドリフト成分の傾向検出に反映されるので、検出精度がさらに高まる。具体的には、第２ミキシングに際しては、定点クロス方式により検出したドリフト成分と第１ミキシングの結果の割合を定点クロス率Ｃｐ、すなわち定点（ゼロクロス点＋最多ハンドル角の点）をクロスした計測データ数と全体の計測データ数との比率に応じて随時変化させる。第１ミキシング結果には（１−κ２（Ｃｐ））を乗算し、定点クロス方式により求めたドリフト成分には、κ２（Ｃｐ）を乗算し、これらの乗算結果をミキシングする。κ２は定点クロス率Ｃｐに依存した関数である。
【００２７】
いま、低速走行か高速走行かを決める走行速度Ｖｘが８０ｋｍ／ｈであったとする。この場合、８０ｋｍ／ｈまでは走行速度Ｖｘの増加に対応させて関数κ１（Ｖｘ）を増加させ、８０ｋｍ／ｈ以降はその関数κ１（Ｖｘ）を一定レベルにホールドする。このようにして、δ−γ方式によるドリフト成分と遠心力方式によるドリフト成分とをつなげる。
【００２８】
第２ミキシング後は、オフセット検出を行う。第１及び第２ミキシングにより得られるのはドリフト成分の傾向（変化勾配）であり、オフセット成分については考慮されていない。そのため、以上のような演算によってドリフト基線を求めると、図１０に示されるように、本当に求めたいドリフト基線との間にずれが生じる場合がある。これは、計測の初期段階でのオフセットがそのまま維持されるためである。このオフセットは、車両停止時にゼロクリアしても、走行開始後は、図１０の拡大図に示されるようにオフセットを伴う計測角速度（以下、「オフセット角速度」）に収束してしまう。本実施形態では、この点を逆に利用し、後述する車両の停止状態検出処理と収束結果予測処理とを併用することにより、オフセット角速度に含まれるオフセットを検出する。
【００２９】
−停止状態検出処理−
車両の停止状態を検出する処理であるが、実際には車両が停止したかどうかを挙動検出センサによる計測データだけで判断することは難しい。車両停止が判定される場合を想定すると、ブレーキを踏んで停止した直後の場合もあれば、アイドリング状態で車両には一定の振動はあるが一時停止の状態にある場合、あるいは急発進直前の場合もあるし、逆に、実際には走行しているが、静かな挙動で、センサによる計測データがまったく変動していない場合があるためである。そこで、本実施形態の停止状態検出処理では、ヨーレートセンサ１３及び各加速度計１１，１２を利用し、これらの計測データのばらつき度の大小で停止／走行の判定を行う。ばらつき度がある閾値よりも小さい場合は車両が停止していると判定し、その時点のオフセット角速度をゼロクリアする。
なお、計測データのばらつき度は、標準偏差ＳＤにより表現される。下記式は、標準偏差ＳＤの一般的な定義式である。
【数１】
0004354607.tif 000002
標準偏差ＳＤを求めるアルゴリズムをモデル化すると図１１（Ａ）のようになる。但し、このモデルで処理するには、大量のメモリ容量など主制御部の処理能力が追いつかない。そこで、実際には、図１１（Ｂ）のモデルのように、平均演算処理に代えて、時定数が調整されたＬＰＦを使用する。これにより、主制御部２４による処理量を低減でき、連続的且つ実時間で信号処理を実現することができ、頻繁に生じる停止状態検出処理を容易に実行することができる。
【００３０】
−収束結果予測処理−
ゼロクリア時（オフセット角速度を零にリセット）から最終的に到達するオフセット角速度に至るまでの収束度を演算し、この収束度をもとにオフセット量を予測する処理である。挙動検出センサの電源オン時、上述の停止状態検出処理で停止と判定された場合等にこれを実行する。
図１２はこの処理の概念説明図である。すなわち、上述したドリフト成分の傾向検出のロジック、例えばγ＝δ・Ｖｘの演算で求めた推定角速度（ヨーレートγ）を時定数τのＬＰＦで推定したときにそのＬＰＦの出力がどこまで収束しているかによって収束度を求める。次に、現在計測できているオフセット角速度÷収束度の演算によって最終的なオフセット角速度（収束結果）を求める。例えば収束度が５０％であった場合は、現在計測できているオフセット角速度を２倍にすれば、最終的なオフセット角速度が直ちに求められる。
【００３１】
上記のようにしてオフセットを検出することにより、オフセットが反映されたドリフト基線（図１０における「本当に求めたいドリフト基線」）を正確に求めることができる。このようにしてドリフト基線を求めた後、図８に示したように、そのドリフト基線を中心とした最大／最小のバンド幅を設定し、このハンド幅内の信号の変化成分をＬＰＦで取り出す。
大きなハンドル角が発生した時など、ドリフト成分以外の振幅の大きな信号成分が計測されるとＬＰＦの出力が大きくなり、誤差を生じさせるので、ドリフト基線付近の振幅変動の小さな信号のみを抽出するため狭いバンド幅を設定し、その狭いバンド幅で変動する信号成分をフィルタリングする。つまり、ハンドル操作に関わる信号成分を一切キャンセルし、ドリフト成分に関わる信号成分のみをフィルタリングする。これによりオフセットが考慮された正確なドリフト基線を求めることができる。
その後、ドリフト基線によって特定されるドリフト成分を計測角速度から除去して正確な計測角速度を演算するとともに、この計測加速度と、前後加速度、横加速度、操舵角度、走行速度とを車両姿勢制御装置に出力する。
これにより車両の正確な姿勢制御が可能になる。
【００３２】
このように、本実施形態では、ドリフト成分が発生しない計器に基づく推定角速度を求め、この推定角速度と計測角速度とを比較することでドリフト成分の傾向を検出し、さらに、低速走行時に精度が高まるδ−γ方式による計測結果と高速走行時に精度が高まる遠心力方式による計測結果とを走行速度Ｖｘに応じた割合で第１ミキシングするようにしたので、走行速度Ｖｘに応じた精度で計測角速度に含まれるドリフト成分を正確に検出することができる。
【００３３】
また、第１ミキシングの結果と定点クロス方式の計測結果を定点クロス率に応じて配分して第２ミキシングし、これを定量化の元データとして利用するようにしたので、走行速度Ｖｘのみならず、運転者によるハンドル操作が考慮された場合のドリフト成分の変化を正確に検出することができる。
【００３４】
さらに、第２ミキシングの結果にオフセットを加算するとともにドリフト基線を中心とするバンド幅内の信号成分のみをフィルタリングするようにしたので、車両の停止状態から高速走行までに柔軟に対応することができ、また、車両に「すべり」が発生する状態にも柔軟に対応することができるようになる。
【００３５】
なお、本実施形態による上述のドリフト除去のアルゴリズムは、ヨーレートセンサ１３のみならず、他の角速度計、例えばロール（横方向揺動）レート、ピッチ（前後揺動）レートを計測するための角速度計の計測結果に含まれるドリフト成分の除去にも適用が可能なものである。
また、本実施形態では、移動体として車両を例に挙げて説明したが、航空機その他の移動体においても同様に適用が可能なものである。
【００３６】
【発明の効果】
以上の説明から明らかなように、本発明のドリフト除去装置によれば、移動体に搭載される角速度計によるドリフト成分を正確に除去することができる。しかも、その移動体の停止状態から高速走行までに柔軟に対応することができ、さらに、移動体に「すべり」が発生する状態にも柔軟に対応することができる。
【図面の簡単な説明】
【図１】本発明を適用した挙動検出センサの構成図。
【図２】車両の挙動に応じて変化するヨーレートの説明図。
【図３】ハンドル角の変化が過大になる場合と直線的になる場合を示した説明図。
【図４】（Ａ）は、低速走行時の計測角速度（Ａ／Ｄ変換されたもの）の変動の様子、（Ｂ）は、高速走行時の計測角速度（Ａ／Ｄ変換されたもの）の変動の様子を示した図である。
【図５】直線走行から緩やかなカーブ走行に移った場合の操舵角度δの変化を示した図。
【図６】図５の変化図における最多ハンドル角を説明するための図。
【図７】定点クロス方式の場合（ハンドル角が零の場合と最多ハンドル角の場合）のドリフト成分の傾向を示した図で、（Ａ）はハンドル角がゼロクロス点でのドリフト成分、（Ｂ）は最多ハンドル角でのクロス点、（Ｃ）は２つを連続させたドリフト成分の例である。
【図８】基線バンド方式の処理の概念説明図。
【図９】基線バンド方式の全体的な処理手順図。
【図１０】オフセットを考慮しない場合のドリフト基線と本当に求めたいドリフト基線との関係を示した図。
【図１１】（Ａ）は標準偏差ＳＤを求めるアルゴリズムをモデル化した図、（Ｂ）はその改良例を示した図。
【図１２】オフセット角速度の収束結果予測の概要を示す説明図。
【符号の説明】
１０  センサ部
１１，１２  加速度計
１３  ヨーレートセンサ
１４  電子回路
２０  ドリフト除去装置
２１  Ａ／Ｄコンバータ
２２  車速パルスカウンタ
２３  操舵角度エンコーダ
２４  主制御部
【図１】
0004354607.tif 000003
【図２】
0004354607.tif 000004
【図３】
0004354607.tif 000005
【図４】
0004354607.tif 000006
【図５】
0004354607.tif 000007
【図６】
0004354607.tif 000008
【図７】
0004354607.tif 000009
【図８】
0004354607.tif 000010
【図９】
0004354607.tif 000011
【図１０】
0004354607.tif 000012
【図１１】
0004354607.tif 000013
【図１２】
0004354607.tif 000014