JP6673106B2 - 角速度センサ補正装置および角速度センサ補正方法 - Google Patents

Description

本発明は、角速度センサ補正技術に関し、特に角速度センサの出力を補正する角速度センサ補正装置および角速度センサ補正方法に関する。

車両用ナビゲーション装置では、一般的に、自律航法から算出された位置と、ＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）から算出された位置とが合成されることによって、最適な位置が推定される。自律航法では、車両の速度を示す速度パルスと、角速度センサによって計測された車両の旋回角速度とをもとに、前回の測位位置を更新することによって、現在の位置が算出される。このような方式のナビゲーション装置によれば、ＧＰＳ衛星からの電波の受信が困難なトンネル、地下駐車場や高層ビルの谷間であっても、自律航法によって自車位置の導出が可能である。車両の旋回による角速度ωは、次の式によって導出される。
ω＝（Ｖｏｕｔ−Ｖｏｆｆｓｅｔ）／Ｓ ・・・（１）
ここで、Ｖｏｕｔは、角速度センサの出力電圧であり、Ｖｏｆｆｓｅｔは、角速度センサのオフセット値、Ｓ（ｍＶ／ｄｅｇ／ｓｅｃ）は、角速度センサの感度係数である（例えば、特許文献１）。

特開２０１０−１０１８１０号公報

角速度を正確に求めるためには、角速度センサのオフセット値と、感度係数を正確に求める必要がある。角速度センサの感度係数は、一般的に角速度センサの個体差や角速度センサの車両への取り付け角度により異なる。また、オフセット値は温度変化によって変化する可能性がある。つまり、オフセット値は、車両用ナビゲーション装置に使用される基板等の発熱や、車両用ナビゲーション装置が車両のダッシュボード等に取り付けられた場合の車両エンジン等の発熱による温度変化の影響を受ける。このようにオフセット値が変化する場合、それに追従するようにオフセット値が求められるべきである。しかしながら、オフセット値が正常な変化の範囲内で急激に変化した場合、変化後のオフセット値が真のオフセット値であるか、ノイズであるかを区別する必要がある。例えば、角速度センサの出力にはホワイトノイズが含まれるが、それ以外にも、不規則なステップ状のシフトが存在する場合である。

本発明はこうした状況に鑑みてなされたものであり、その目的は、角速度センサのオフセット値が急激に変化する場合でも角速度の導出精度の低下を抑制する技術を提供することにある。

上記課題を解決するために、本発明のある態様の角速度センサ補正装置は、ＧＰＳ衛星からの信号をもとに測位された対象物の測位データと、角速度センサから出力された対象物の角速度とを取得する取得部と、取得部において取得した測位データと角速度とを使用することによって、角速度センサの仮のオフセット値を逐次導出するオフセット値導出部と、オフセット値導出部において逐次導出した角速度センサの仮のオフセット値に対して統計処理を実行することによって、角速度センサから出力された角速度を補正するための角速度センサのオフセット値を導出するオフセット値フィルタ処理部と、オフセット値導出部において逐次導出した角速度センサの仮のオフセット値に対する第１期間における変化の傾きと、第１期間よりも短い第２期間における角速度センサの仮のオフセット値の変化の絶対値とをもとに、ステップ状非変化、ステップ状変化、ステップ状を越えた変化のいずれかを検出するオフセット値変化検出部と、オフセット値変化検出部においてステップ状非変化を検出した場合、（１）非変化が連続する期間と、オフセット値フィルタ処理部において導出した角速度センサのオフセット値とを記憶するとともに、（２）角速度センサのオフセット値を出力するオフセット値判定部とを備える。オフセット値判定部は、オフセット値変化検出部においてステップ状を越えた変化を検出した場合、あるいは仮のオフセット値の導出が不能になった場合、（１）連続する期間が最大であるときに記憶された角速度センサのオフセット値を出力するとともに、（２）非変化が連続する期間をリセットする。

本発明の別の態様は、角速度センサ補正方法である。この方法は、ＧＰＳ衛星からの信号をもとに測位された対象物の測位データと、角速度センサから出力された対象物の角速度とを取得するステップと、取得した測位データと角速度とを使用することによって、角速度センサの仮のオフセット値を逐次導出するステップと、逐次導出した角速度センサの仮のオフセット値に対して統計処理を実行することによって、角速度センサから出力された角速度を補正するための角速度センサのオフセット値を導出するステップと、逐次導出した角速度センサの仮のオフセット値に対する第１期間における変化の傾きと、第１期間よりも短い第２期間における角速度センサの仮のオフセット値の変化の絶対値とをもとに、ステップ状非変化、ステップ状変化、ステップ状を越えた変化のいずれかを検出するステップと、ステップ状非変化を検出した場合、（１）非変化が連続する期間と、導出した角速度センサのオフセット値とを記憶するとともに、（２）角速度センサのオフセット値を出力するステップと、ステップ状を越えた変化を検出した場合、あるいは仮のオフセット値の導出が不能になった場合、（１）連続する期間が最大であるときに記憶された角速度センサのオフセット値を出力するとともに、（２）非変化が連続する期間をリセットするステップと、を備える。

なお、以上の構成要素の任意の組合せ、本発明の表現を方法、装置、システム、記録媒体、コンピュータプログラムなどの間で変換したものもまた、本発明の態様として有効である。

本発明によれば、角速度センサのオフセット値が急激に変化する場合でも角速度の導出精度の低下を抑制できる。

本発明の実施例に係る角速度センサからの出力信号の一例を示す図である。 本発明の実施例に係る角速度算出装置の構成を示す図である。 図２のオフセット値演算部の構成を示す図である。 図３のオフセット値補正部の構成を示す図である。 図４のオフセット値変化検出部において記憶されるテーブルのデータ構造を示す図である。 図４のオフセット値フィルタ処理部の構成を示す図である。 図７（ａ）−（ｂ）は、図４のオフセット値判定部において記憶されるデータ構造を示す図である。 図４のオフセット値補正部による処理の概要を示す図である。 図４のオフセット値判定部による処理の別の概要を示す図である。 図１の角速度算出装置によるオフセット値の導出手順を示すフローチャートである。

本発明を具体的に説明する前に、まず概要を述べる。本発明の実施例は、車両等に搭載され、車両の旋回による角速度を導出する角速度算出装置に関する。角速度算出装置は、角速度センサからの出力電圧に対して、オフセット値と感度係数とを使用しながら、角速度を導出する。前述のごとく、角速度センサのオフセット値は、温度の変動の影響を受ける。また、オフセット値の誤差は、感度係数の算出精度に影響を与える。従来、角速度センサのオフセット値は、角速度が「０」になる車両停止時や直進走行時の角速度センサからの出力電圧を用いて補正される。車両の走行状態は、車速センサや加速度センサ、角速度センサの出力値の組合せから判断されるが、正確に走行状態を判断することが困難である場合がある。例えば、微速で移動している場合には、所定期間における車速センサの出力が「０」、かつ、加速度センサと角速度センサの出力変化が非常に小さいため停止と判断される場合があり、このときに角速度センサのオフセット値を補正すると補正誤差が含まれる可能性がある。

前述のごとく、オフセット値が正常な変化の範囲内で急激に変化した場合、真のオフセット値に追随するのに時間を要する可能性がある。例えば、角速度センサの出力にはホワイトノイズが含まれるが、それ以外にも、不規則なステップ状のシフトが存在する場合がある。出力値がステップ状に変化すると、変化後のオフセット値が真のオフセット値であるか、ノイズであるかを区別する必要がある。不規則なステップ状のシフトが存在するような状況であっても角速度の導出精度を高めるためには、角速度センサのオフセット値を短期間で高精度に導出することが要求される。これに対応するために、本実施例に係る角速度算出装置は、次の処理を実行する。

角速度算出装置は、車両に搭載された角速度センサから出力電圧を入力するとともに、車両に搭載されたＧＰＳ受信機から測位データを入力する。ここで、角速度センサからの出力電圧が、車両の角速度に相当する。また、測位データには、車両の方位、車両の速度、車両の高度等が含まれる。角速度算出装置は、測位データと出力電圧とをもとに、車両の走行状態を推定する。走行状態として、例えば、停止状態、直進走行状態、非直進走行状態のいずれかが特定される。また、角速度算出装置は、走行状態に応じた導出方法にて仮のオフセット値を逐次導出し、逐次導出した仮のオフセット値に対してローパスフィルタによるフィルタ処理を実行する。角速度算出装置は、仮のオフセット値をもとに、ステップ状の変化がない状態（以下、「ステップ状非変化」という）、ステップ状の変化がある状態（以下、「ステップ状変化」という）、ステップ状の変化を越えるノイズが含まれる状態（以下、「ステップ状を越えた変化」という）のいずれかを検出する。角速度算出装置は、ステップ状非変化、ステップ状変化、ステップ状を越えた変化に応じてオフセット値を出力する。

図１は、本発明の実施例に係る角速度センサからの出力信号の一例を示す。横軸は、時間を示し、縦軸は、角速度センサのオフセット値を示す。０［ｓｅｃ］から２００［ｓｅｃ］の全体として、オフセット値は、温度変化の影響を受けて緩やかに上昇している。しかしながら、８０［ｓｅｃ］付近から１３０［ｓｅｃ］付近において、オフセット値ではステップ状のシフトが発生している。

図２は、本発明の実施例に係る角速度算出装置１００の構成を示す。角速度算出装置１００は、測定部１０、パラメータ演算部１２、角速度変換部１４、制御部１６を含む。また、測定部１０は、ＧＰＳ測位部２０、有効性判定部２２、角速度センサ２４を含み、パラメータ演算部１２は、オフセット値演算部２６、感度係数演算部２８を含む。さらに信号として、ＧＰＳ測位データ２００、出力信号２０２、オフセット値２０４、感度係数２０６が含まれる。ここで、パラメータ演算部１２は角速度センサ補正装置であるともいえる。

ＧＰＳ測位部２０は、図示しないＧＰＳ衛星からの信号を受信して、ＧＰＳ測位データ２００を算出する。ＧＰＳ測位データ２００には、経緯度、車両の高度であるＧＰＳ高度、移動速度であるＧＰＳ速度、車両の方位であるＧＰＳ方位、ＰＤＯＰ（Ｐｏｓｉｔｉｏｎ Ｄｉｌｕｔｉｏｎ Ｐｒｅｃｉｓｉｏｎ）、捕捉衛星数等が含まれる。ここで、ＰＤＯＰは、ＧＰＳ測位データ２００におけるＧＰＳ衛星位置の誤差が受信点位置にどのように反映されるかの指標であり、測位誤差に相当する。なお、ＧＰＳ測位データ２００には、これら以外の値が含まれていてもよい。また、ＧＰＳ測位データ２００の算出は、公知の技術によってなされればよいので、ここでは説明を省略する。また、ＧＰＳ測位部２０は、ＧＰＳ測位データ２００をサンプリング間隔ごとに、つまり周期的に算出する。ＧＰＳ測位部２０は、ＧＰＳ測位データ２００を有効性判定部２２へ逐次出力する。

有効性判定部２２は、ＧＰＳ測位部２０からのＧＰＳ測位データ２００を逐次入力する。有効性判定部２２は、ＧＰＳ測位データ２００から、ＧＰＳ測位データ２００それぞれの有効性を判定する。例えば、有効性判定部２２は、ＰＤＯＰの値が第１のしきい値以下であり、かつＧＰＳ速度が第２のしきい値以上である場合に、それらに対応したＧＰＳ方位が有効であると判定する。また、有効性判定部２２は、上記の条件が満たされない場合に、対応したＧＰＳ方位が無効であると判定する。これは、一般的にＰＤＯＰの値が大きい場合やＧＰＳ速度が小さい場合に、ＧＰＳ方位の精度が低くなる傾向があるからである。さらに具体的に説明すると、ＰＤＯＰの値が６以下であり、かつＧＰＳ速度が２０ｋｍ／ｈ以上である場合に、有効性判定部２２は、ＧＰＳ方位の有効性をフラグで表す。

また、有効性判定部２２は、ＧＰＳ速度が第３のしきい値以上である場合に、当該ＧＰＳ速度が有効であると判定する。ここで、第３のしきい値は、第２のしきい値と同じでもよい。このような処理の結果、有効性判定部２２は、ＧＰＳ測位データ２００に含まれたＧＰＳ方位等の各値に対して、有効あるいは無効が示されたフラグを付加する（以下、フラグが付加されたＧＰＳ測位データ２００もまた「ＧＰＳ測位データ２００」という）。有効性判定部２２は、オフセット値演算部２６、感度係数演算部２８へＧＰＳ測位データ２００を逐次出力する。

角速度センサ２４は、例えば、振動ジャイロ等のジャイロ装置に相当し、車両の進行方向の変化を車両の相対的な角度変化として検出する。つまり、角速度センサ２４は、車両の旋回角速度を検出する。検出された角速度は、例えば、０Ｖ〜５Ｖのアナログ信号として出力される。その際、時計回りの旋回に対応した正の角速度は５Ｖ側への２．５Ｖからの偏差電圧として出力され、反時計回りの旋回に対応した負の角速度は０Ｖ側への２．５Ｖからの偏差電圧として出力される。また、２．５Ｖは、角速度のオフセット値、つまり零点であり、温度等の影響を受けドリフトする。

また、２．５Ｖからの角速度の偏差程度である感度係数（ｍＶ／ｄｅｇ／ｓｅｃ）は、水平な状態において許容誤差内に収まる所定の値として定められている。この許容誤差原因は、ジャイロ装置の個体差や経年変化、温度による影響等である。ジャイロ装置の電圧値は、図示しないＡＤ（Ａｎａｌｏｇ ｔｏ Ｄｉｇｉｔａｌ）変換装置によって、例えば、サンプリング間隔１００ｍｓｅｃでＡＤ変換され、その結果のデジタル信号が出力される。当該デジタル信号は、前述の出力電圧に相当し、以下では、出力信号２０２という用語を使用する。なお、ジャイロ装置として、公知の技術が使用されればよいので、ここでは説明を省略する。角速度センサ２４は、角速度変換部１４、オフセット値演算部２６、感度係数演算部２８へ出力信号２０２を出力する。この出力信号２０２がオフセット値２０４において取得した角速度に相当する。ここで、オフセット値演算部２６、感度係数演算部２８は、有効性判定部２２からのＧＰＳ測位データ２００と、角速度センサ２４からの出力信号２０２とを取得するので、これらは「取得部」であるといえる。

オフセット値演算部２６は、有効性判定部２２からのＧＰＳ測位データ２００、角速度センサ２４からの出力信号２０２を入力する。また、オフセット値演算部２６は、感度係数演算部２８から感度係数２０６も入力する。オフセット値演算部２６は、ＧＰＳ測位データ２００、出力信号２０２、感度係数２０６をもとに、角速度センサ２４のオフセット値（以下、「オフセット値２０４」という）を算出する。なお、オフセット値演算部２６での処理の詳細は後述する。オフセット値演算部２６は、オフセット値２０４を感度係数演算部２８、角速度変換部１４へ出力する。

感度係数演算部２８は、有効性判定部２２からのＧＰＳ測位データ２００、角速度センサ２４からの出力信号２０２を入力する。また、感度係数演算部２８は、オフセット値演算部２６からオフセット値２０４も入力する。感度係数演算部２８は、所定期間、例えば１０秒間にわたって入力された、ＧＰＳ測位データ２００、出力信号２０２、オフセット値２０４をもとに、角速度センサ２４の感度係数（以下、前述の「感度係数２０６」という）を算出する。なお、感度係数の演算には公知の技術が使用されればよいので、ここでは説明を省略する。感度係数演算部２８は、感度係数２０６をオフセット値演算部２６、角速度変換部１４へ出力する。

角速度変換部１４は、角速度センサ２４からの出力信号２０２、オフセット値演算部２６からのオフセット値２０４、感度係数演算部２８からの感度係数２０６を入力する。角速度変換部１４は、出力信号２０２、オフセット値２０４、感度係数２０６をもとに、前述の式（１）を計算することによって、車両の角速度ωを算出する。角速度変換部１４は、角速度ωを出力する。制御部１６は、角速度算出装置１００全体の動作を制御する。

この構成は、ハードウエア的には、任意のコンピュータのＣＰＵ、メモリ、その他のＬＳＩで実現でき、ソフトウエア的にはメモリにロードされたプログラムなどによって実現されるが、ここではそれらの連携によって実現される機能ブロックを描いている。したがって、これらの機能ブロックがハードウエアのみ、ソフトウエアのみ、またはそれらの組合せによっていろいろな形で実現できることは、当業者には理解されるところである。

図３は、オフセット値演算部２６の構成を示す。オフセット値演算部２６は、状態推定部３０、状態別オフセット値導出部３２、オフセット値補正部３４を含む。また、状態推定部３０は、停止推定部４０、直進走行推定部４２、非直進走行推定部４４を含み、状態別オフセット値導出部３２は、停止時オフセット値導出部４６、直進走行時オフセット値導出部４８、非直進走行時オフセット値導出部５０を含む。さらに信号として、仮オフセット値２１０が含まれる。

状態推定部３０は、ＧＰＳ測位データ２００、出力信号２０２を入力する。状態推定部３０は、停止推定部４０、直進走行推定部４２、非直進走行推定部４４において、車両の走行状態を推定する。ここでは、車両の走行状態として、車両が停止あるいは直進している状態であるか、残りの状態、つまり非直進走行している状態であるかを推定する。

停止推定部４０は、図示しない有効性判定部２２において有効であると判定されたＧＰＳ測位データ２００を取得する。また、停止推定部４０は、ＧＰＳ測位データ２００からＧＰＳ速度を抽出し、ＧＰＳ速度が「０」であるかを確認する。一方、停止推定部４０は、所定期間内における出力信号２０２の分散値を計算し、分散値と第４のしきい値とを比較する。停止推定部４０は、ＧＰＳ速度が０であり、かつ分散値が第４のしきい値よりも小さい場合に、車両が停止状態であると判定する。前述のごとく、ＧＰＳ速度が小さい場合、その精度は低くなる傾向があるが、停止推定部４０は、出力信号２０２の分散値を併せて使用することによって停止と判断する。ここで、所定期間は、例えば、ＧＰＳ速度のサンプリング間隔である１ｓｅｃとされる。所定期間において、出力信号２０２の分散値が小さいときは、車両の揺れ等がない安定した状態であると推定される。停止推定部４０は、停止状態ではないと判定した場合、その旨を非直進走行推定部４４へ出力する。

直進走行推定部４２は、図示しない有効性判定部２２において有効であると判定されたＧＰＳ測位データ２００を取得する。また、直進走行推定部４２は、ＧＰＳ測位データ２００からＧＰＳ方位を抽出し、ＧＰＳ方位の所定期間にわたる変化（以下、「ＧＰＳ方位変化」という）を導出する。さらに、直進走行推定部４２は、ＧＰＳ方位変化が「０」であるかを確認する。また、直進走行推定部４２は、所定期間における出力信号２０２の分散値を計算し、分散値と第５のしきい値とを比較する。なお、第５のしきい値は、第４のしきい値と同一であってもよい。ここで、所定期間は、例えば、ＧＰＳ方位変化が連続して０であるような期間に設定される。

直進走行推定部４２は、ＧＰＳ方位変化が０であり、かつ分散値が第５のしきい値よりも小さい場合に、車両が直進走行状態であると判定する。所定期間において、出力信号２０２の分散値が小さいときは、微妙な蛇行等の影響がない直進走行状態であると推定される。なお、ドライバの運転状況や道路形状によるが、例えば、市街地等において、直進走行状態の検出頻度は、一般的に、停止推定部４０による停止状態の判定よりも少なく、その期間は数秒間程度である。直進走行推定部４２は、直進走行状態ではないと判定した場合、その旨を非直進走行推定部４４へ出力する。非直進走行推定部４４は、停止推定部４０から、停止状態ではない旨を入力し、かつ直進走行推定部４２から、直進走行状態ではない旨を入力した場合、車両が非直進走行状態であると判定する。

状態別オフセット値導出部３２は、ＧＰＳ測位データ２００、出力信号２０２、感度係数２０６を入力する。状態別オフセット値導出部３２は、状態推定部３０において推定した車両の走行状態に応じて、角速度センサ２４の仮オフセット値２１０を逐次導出する。ここで、停止推定部４０において停止状態と判定された場合、停止時オフセット値導出部４６が出力信号２０２をもとに仮オフセット値２１０を逐次導出する。また、直進走行推定部４２において直進走行状態と判定された場合、直進走行時オフセット値導出部４８が出力信号２０２をもとに仮オフセット値２１０を逐次導出する。

また、非直進走行推定部４４において非直進走行状態と判定された場合、非直進走行時オフセット値導出部５０がＧＰＳ測位データ２００、出力信号２０２、感度係数２０６をもとに仮オフセット値２１０を逐次導出する。つまり、車両の走行状態に応じて停止時オフセット値導出部４６から非直進走行時オフセット値導出部５０は、ＧＰＳ測位データ２００、出力信号２０２、感度係数２０６の組合せを変更しながら、仮オフセット値２１０を導出する。また、ＧＰＳ測位データ２００が無効であり、状態推定部３０が走行状態を推定できなかった場合、仮オフセット値２１０を導出不能として、その旨がオフセット値補正部３４に出力される。そのため、停止時オフセット値導出部４６、直進走行時オフセット値導出部４８、非直進走行時オフセット値導出部５０を含んだ状態別オフセット値導出部３２がオフセット値導出部であるといえる。

停止時オフセット値導出部４６は、停止状態と判定された場合に、出力信号２０２をもとに、角速度センサ２４の仮オフセット値２１０を逐次導出する。具体的に説明すると、停止時オフセット値導出部４６は、停止時に車両の旋回角速度が「０」になることを利用し、出力信号２０２の平均値を仮オフセット値２１０として算出する。直進走行時オフセット値導出部４８は、直進走行状態と判定された場合に、出力信号２０２をもとに、角速度センサ２４の仮オフセット値２１０を逐次導出する。具体的に説明すると、ここでも車両の旋回角速度が０であるので、直進走行時オフセット値導出部４８は、出力信号２０２の平均値を仮オフセット値２１０として算出する。

非直進走行時オフセット値導出部５０は、非直進走行状態であると判定された場合に、ＧＰＳ測位データ２００中のＧＰＳ方位、出力信号２０２、感度係数２０６とをもとに、例えば、ＧＰＳ方位のサンプリング間隔における仮オフセット値２１０を逐次導出する。仮オフセット値２１０は、次のように導出される。
Ｇｏｆｆｓｅｔ＝１／ｎ・ΣＧｏｕｔ−Δθ・Ｇｓｅｎｓｉｔｉｖｉｔｙ・・・（２）
ここで、ｎは、ＧＰＳ方位のサンプリング間隔における出力信号２０２のサンプル数であり、ΣＧｏｕｔ（ｍＶ）は、ＧＰＳ方位のサンプリング間隔における出力信号２０２の合計値である。また、Δθ（ｄｅｇ）は、ＧＰＳ方位変化量であり、Ｇｓｅｎｓｉｔｉｖｉｔｙ（ｍＶ／ｄｅｇ／ｓｅｃ）は、感度係数２０６である。

感度係数２０６は、通常、図示しない感度係数演算部２８から入力されるが、角速度算出装置１００の起動直後などのような状態において、感度係数２０６が未だ算出されていないこともありえる。そのような場合、非直進走行時オフセット値導出部５０は、図示しないジャイロ装置の仕様によって決定される感度係数２０６を初期値として使用する。また、非直進走行時オフセット値導出部５０は、前回の走行終了時に感度係数演算部２８からの感度係数２０６を記憶しておき、初期値として使用してもよい。

オフセット値補正部３４は、状態別オフセット値導出部３２において逐次導出した仮オフセット値２１０を入力する。オフセット値補正部３４は、仮オフセット値２１０に対して統計処理を実行するとともに、時間に対する仮オフセット値２１０を変化を検出し、変化が所定値より小さい期間を計数する。また、オフセット値補正部３４は、計数した期間と統計処理の結果とを関連付けて記憶する。オフセット値補正部３４は、状態別オフセット値導出部３２からの仮オフセット値２１０が導出された場合は統計処理の結果を出力し、導出不能となったときは、計数した期間が最大である統計処理の結果を出力する。以下では、図４を使用しながら、オフセット値補正部３４での処理を説明する。

図４は、オフセット値補正部３４の構成を示す。オフセット値補正部３４は、平均値算出部６０、差分絶対値算出部６２、傾き絶対値算出部６４、オフセット値変化検出部６６、オフセット値フィルタ処理部６８、オフセット値判定部７０を含む。また、信号として、差分絶対値２１２、傾き絶対値２１４が含まれる。

平均値算出部６０は、仮オフセット値２１０を入力する。平均値算出部６０は、入力した仮オフセット値２１０に対する期間Ｔａでの移動平均を計算することによって、仮オフセット値２１０の平均値を算出する。期間Ｔａとは、例えば、５［秒］等であり、車両に取り付けられた角速度算出装置１００内の温度の変化が大きくなく、角速度センサ２４のオフセットの温度による変動が微小であるような期間が設定される。平均値算出部６０は、算出した平均値Ｖａ［Ｖ］を差分絶対値算出部６２へ逐次出力する。

差分絶対値算出部６２は、平均値算出部６０から平均値Ｖａ［Ｖ］を逐次入力する。差分絶対値算出部６２は、逐次入力した平均値Ｖａ［Ｖ］の差分の絶対値を算出する。具体的に説明すると、差分絶対値算出部６２は、平均値を少なくとも期間Ｔａにわたって保持し、入力した平均値と期間Ｔａ以前に入力した平均値との差分の絶対値を算出する。差分絶対値算出部６２は、算出した絶対値｜Ｖａ’−Ｖａ｜［Ｖ］（以下、「差分絶対値２１２」ともいう）をオフセット値変化検出部６６へ逐次出力する。なお、Ｖａ’は以前の平均値を示す。

傾き絶対値算出部６４は、仮オフセット値２１０を入力する。また、傾き絶対値算出部６４は、入力した仮オフセット値２１０と、図示しないマイクロコンピュータ等に備わっている計時機能から取得される起動後の経過時間とを関連付けて、期間Ｔｓにわたってリングバッファ等に記憶する。期間Ｔｓとは、期間Ｔａよりも大きい値であり、例えば、１０秒等である。なお、期間Ｔｓを「第１期間」と呼ぶ場合、期間Ｔａは「第２期間」と呼ばれる。傾き絶対値算出部６４は、時間と仮オフセット値２１０に対し関数近似し、仮オフセット値２１０の傾きを算出する。傾き絶対値算出部６４は、例えば、時間をＸ軸、仮オフセット値２１０をＹ軸として、最小二乗法を用いて一次関数で近似する。最小二乗法を適用すると、仮オフセット値２１０の傾きγｎ（Ｖ／ｓｅｃ）は次のように算出される。

ここで、ｍは仮オフセット値２１０のサンプル数、ｔ［ｓｅｃ］は時間、Ｏｆｓ［Ｖ］は仮オフセット値２１０である。傾き絶対値算出部６４は、算出した仮オフセット値２１０の傾き絶対値｜γｎ｜（Ｖ／ｓｅｃ）（以下、「傾き絶対値２１４」ともいう）をオフセット値変化検出部６６へ逐次出力する。

オフセット値変化検出部６６は、差分絶対値算出部６２からの差分絶対値２１２と、傾き絶対値算出部６４からの傾き絶対値２１４とを入力する。オフセット値変化検出部６６は、差分絶対値２１２と傾き絶対値２１４とをもとに、仮オフセット値２１０における変化の状態を検出する。仮オフセット値２１０における変化の状態とは、「ステップ状非変化」、「ステップ状変化」、「ステップ状を越えた変化」のいずれかである。この検出のために、オフセット値変化検出部６６は、テーブルを保持しており、当該テーブルを参照することによって、仮オフセット値２１０における変化の状態を検出する。

図５は、オフセット値変化検出部６６において記憶されるテーブルのデータ構造を示す。図示のごとく、差分絶対値条件欄５００、傾き絶対値条件欄５０２、変化フラグ欄５０４が含まれる。差分絶対値条件欄５００には、変化フラグ欄５０４を決定するための差分絶対値２１２に対する条件として、「Ｖｔｈ以上」、「Ｖｔｈ未満」が示されている。ここで、「Ｖｔｈ」は差分絶対値２１２に対するしきい値であり、これには、仮オフセット値２１０に含まれるノイズによる変動幅以上の値が設定される。また、傾き絶対値条件欄５０２には、変化フラグを決定するための傾き絶対値２１４に対する条件として、「γｔｈ以上」、「γｔｈ未満」が示されている。ここで、「γｔｈ」は傾き絶対値２１４に対するしきい値であり、これには、例えば、予め測定した、角速度算出装置１００の起動後数秒間の温度変動に対する仮オフセット値２１０の傾き絶対値２１４以上の値が設定される。変化フラグ欄５０４には、各条件に対応した変化フラグを記憶する。ここで、変化フラグ欄５０４における「１」は「ステップ状変化」を示し、「０」は「ステップ状非変化」を示し、「−１」は「ステップ状を越えた変化」を示す。図４に戻る。

このようにオフセット値変化検出部６６は、時間経過に対する角速度センサ２４の仮オフセット値２１０の変化の傾きである傾き絶対値２１４が大きいときにステップ状の変化を検出する。オフセット値変化検出部６６は、テーブルを参照しながら、出力する変化フラグを決定する。また、オフセット値変化検出部６６は、期間Ｔａにおいて、角速度センサ２４の仮オフセット値２１０の変化の絶対値である差分絶対値２１２がしきい値以上である場合に、出力する変化フラグをノイズとして決定する。オフセット値変化検出部６６は、決定した変化フラグをオフセット値判定部７０へ出力する。

オフセット値フィルタ処理部６８は、仮オフセット値２１０を入力する。オフセット値フィルタ処理部６８は、仮オフセット値２１０に対して統計処理を実行することによって、角速度センサ２４から出力された角速度を補正するためのオフセット値２０４を導出する。オフセット値フィルタ処理部６８の構成を説明するために、ここでは図６を使用する。

図６は、オフセット値フィルタ処理部６８の構成を示す。オフセット値フィルタ処理部６８は、α乗算部８０、加算部８２、１−α乗算部８４を含む。図示のごとく、オフセット値フィルタ処理部６８は、ＩＩＲ（Ｉｎｆｉｎｉｔｅ Ｉｍｐｕｌｓｅ Ｒｅｓｐｏｎｃｅ）フィルタを含むように構成されており、ＩＩＲフィルタによってローパスフィルタを構成する。α乗算部８０は、仮オフセット値２１０に忘却係数「α」を乗算する。α乗算部８０は、乗算結果を加算部８２へ出力する。

加算部８２は、α乗算部８０からの乗算結果と、１−α乗算部８４からの乗算結果とを逐次加算する。加算部８２は、加算結果をオフセット値２０４として逐次出力する。１−α乗算部８４は、オフセット値２０４に係数「１−α」を乗算する。なお、係数「１−α」のうちの「α」は、α乗算部８０でのαと同様であるので、ここでは説明を省略する。１−α乗算部８４は、加算部８２へ乗算結果をフィードバックする。図４に戻る。オフセット値フィルタ処理部６８は、導出したオフセット値２０４をオフセット値判定部７０へ出力する。

オフセット値判定部７０は、仮オフセット値２１０と、オフセット値変化検出部６６からの変化フラグと、オフセット値フィルタ処理部６８からのオフセット値２０４とを入力する。ここで、差分絶対値算出部６２における差分絶対値２１２の導出処理には時間２×Ｔａ［秒］を要し、傾き絶対値算出部６４における傾き絶対値２１４の導出処理にはＴｓ［秒］を要する。そのため、オフセット値判定部７０は入力値をリングバッファ等に保存し、変化フラグとオフセット値２０４とを同期させる。

オフセット値判定部７０は、仮オフセット値２１０に付加されているフラグが有効であり、変化フラグが「０」または「１」であれば、入力したオフセット値２０４を出力する。特に、変化フラグ「０」である場合、オフセット値判定部７０は、変化フラグが「０」である期間、つまり仮オフセット値２１０の時間に対する変化が少ない期間を計数し、期間とオフセット値２０４とを関連付けて記憶する。また、オフセット値判定部７０は、仮オフセット値２１０に付加されているフラグが無効、あるいは変化フラグが「−１」であれば、記憶した期間が最大であるオフセット値２０４を出力する。ここでは、図７を使用してオフセット値判定部７０が記憶するデータの詳細を以下に説明する。

図７（ａ）−（ｂ）は、オフセット値判定部７０において記憶されるデータ構造を示す。図示のごとく、期間最大値と計測中の行と、期間欄７００とオフセット値欄７０２とが含まれる。期間最大値の行には、期間が最大であるときの期間とオフセット値２０４が記録され、計測中の行には、現在計測中である期間とオフセット値２０４が記録される。

オフセット値判定部７０は、仮オフセット値２１０のフラグが有効である場合に、変化フラグが連続して「０」である期間を図示しないマイクロコンピュータ等に備わっている計時機能から取得する。また、オフセット値判定部７０は、図７（ａ）のごとく、計時した期間と入力したオフセット値２０４を計測中の行へ記憶する。このとき、計測中の行の期間が、期間最大値の行の期間以上であれば、オフセット値判定部７０は、図７（ｂ）のごとく、期間最大値の行を計測中の行で置換する。

オフセット値判定部７０は、変化フラグが「１」または「−１」の場合に、計測中の行に記憶された期間欄７００をとオフセット値欄７０２とを消去する。また、オフセット値判定部７０は、期間に対してしきい値を設定し、しきい値以上の場合はオフセット値欄７０２のみを更新する。このしきい値とは、例えば、予め実験等で得られたステップ状の変化の期間より大きく、角速度センサ２４のオフセットの温度による変化がステップ状の変化と同じになる期間より小さくなるような値が設定される。このようにすることで、ステップ状の変化検出後も期間最大値を更新できるので、角速度センサ２４のオフセットの温度による変化に追随できる。

つまり、オフセット値判定部７０は、オフセット値変化検出部６６においてステップ状非変化を検出した場合、非変化が連続する期間と、オフセット値２０４とを記憶するとともに、入力したオフセット値２０４を出力する。一方、オフセット値判定部７０は、オフセット値変化検出部６６においてステップ状を越えた変化を検出した場合、連続する期間が最大であるときに記憶されたオフセット値２０４を出力するとともに、非変化が連続する期間をリセットする。なお、オフセット値判定部７０は、仮オフセット値２１０の導出が不能になった場合も、オフセット値変化検出部６６においてステップ状を越えた変化を検出した場合と同様の処理を実行する。オフセット値判定部７０は、オフセット値変化検出部６６においてステップ状変化を検出した場合、入力したオフセット値２０４を出力するとともに、非変化が連続する期間をリセットする。

図８を使用しながらオフセット値補正部３４に入力される仮オフセット値２１０に対する差分の絶対値と傾きの絶対値を説明する。図８は、オフセット値補正部３４による処理の概要を示す。図示のごとく、オフセット値補正部３４に入力される仮オフセット値２１０においてステップ状のシフトが発生している。ここでは、時間Ａ［秒］付近において、仮オフセット値２１０がステップ状にシフトしている。平均値算出部６０は、時間Ａ［秒］前後の期間Ｔａにおける仮オフセット値２１０の平均値Ｖ１と平均値Ｖ２を算出する。また、差分絶対値算出部６２は、絶対値｜Ｖ１−Ｖ２｜を算出する。さらに、傾き絶対値算出部６４は、期間Ｔｓにおける仮オフセット値２１０の傾きを算出する。オフセット値補正部３４は、絶対値｜Ｖ１−Ｖ２｜が「Ｖｔｈ」未満、かつ、傾きの絶対値が「γｔｈ」以上であった場合に、仮オフセット値２１０に発生したステップ状のシフトを検出する。

図９は、オフセット値補正部３４による処理の別の概要を示す。図９では、時間Ａ［秒］において仮オフセット値２１０のフラグが有効となり、時間Ｄ［秒］において仮オフセット値２１０のフラグが無効となった場合を示している。また、時間Ｂ［秒］付近と時間Ｃ［秒］付近において、仮オフセット値２１０がステップ状にシフトしている。オフセット値判定部７０は、時間Ａ［秒］からオフセット値の記録を開始し、時間Ｂ［秒］付近においてステップ状のシフトが検出されるまで、期間とオフセット値２０４を記憶する。時間Ｂ［秒］直前では、期間最大値および計測中共に、期間Ｔ_ＡＢ［秒］、オフセット値Ｖ_Ｂ［Ｖ］が記録される。ステップ状のシフトを検出したタイミングで、計測中の行のデータは初期化される。

また、時間Ｂ［秒］から時間Ｃ［秒］までは計測中の行にデータが記憶され、時間Ｃ［秒］直前では、期間Ｔ_ＢＣ［秒］、オフセット値Ｖ_Ｃ［Ｖ］が記憶されるが、ステップ状のシフトを検出したタイミングでＴ_ＡＢ＞Ｔ_ＢＣであるので、期間最大値の行のデータは更新されない。さらに、時間Ｃ［秒］から時間Ｄ［秒］まで、計測中の行にデータが記憶され、時間Ｄ［秒］では、期間Ｔ_ＣＤ［秒］、オフセット値Ｖ_Ｄ［Ｖ］が記憶されるが、Ｔ_ＡＢ＞Ｔ_ＣＤであるので、期間最大値の行のデータは更新されない。したがって、オフセット値判定部７０は、仮オフセット値２１０のフラグが無効となった時間Ｄ［秒］以降、仮オフセット値２１０のフラグが有効となるまで、期間最大値の行に記憶されたＶ_Ｂ［Ｖ］をオフセット値２０４として出力する。

以上の構成による角速度算出装置１００の動作を説明する。図１０は、角速度算出装置１００によるオフセット値の導出手順を示すフローチャートである。差分絶対値２１２がＶｔｈよりも小さく（Ｓ１０のＹ）、傾き絶対値２１４がγｔｈ以上である場合（Ｓ１２のＹ）、オフセット値判定部７０は、入力したオフセット値２０４を出力するとともに、期間をリセットする（Ｓ１４）。傾き絶対値２１４がγｔｈ以上でない場合（Ｓ１２のＮ）、オフセット値判定部７０は、入力したオフセット値２０４、期間を記憶するとともに、入力したオフセット値２０４を出力する（Ｓ１６）。差分絶対値２１２がＶｔｈよりも小さくない場合（Ｓ１０のＮ）、オフセット値判定部７０は、最大の期間に対応したオフセット値２０４を出力するとともに、期間をリセットする（Ｓ１８）。

本発明の実施例によれば、差分絶対値と傾き絶対値とをもとに、ステップ状非変化、ステップ状変化、ステップ状を越えた変化のいずれかを検出するので、ノイズの影響が大きい環境であるか否か、ステップ状のシフトを伴うノイズが含まれる環境であるか否かを判定できる。また、ノイズの影響が大きい環境であるか否か、ステップ状のシフトを伴うノイズが含まれる環境であるか否かに応じて、オフセット値に関する処理を実行するので、角速度センサのオフセット値が急激に変化する場合でも角速度の導出精度の低下を抑制できる。また、ステップ状非変化が検出された場合、非変化が連続する期間と、オフセット値とを記憶するので、定常的なオフセット値を取得できる。また、ステップ状非変化が検出された場合、入力したオフセット値を出力するので、最新のオフセット値を使用させることができる。

また、ステップ状を越えた変化が検出された場合、連続する期間が最大であるときに記憶されたオフセット値を出力するので、ステップ状を越えた変化が検出されない場合における確からしいオフセット値を出力できる。また、ステップ状を越えた変化が検出された場合、非変化が連続する期間をリセットするので、環境の変化を処理に反映できる。また、ステップ状変化が検出された場合、入力したオフセット値を出力するので、最新のオフセット値を使用させることができる。また、ステップ状変化が検出された場合、非変化が連続する期間をリセットするので、環境の変化を処理に反映できる。

また、時間経過に対する角速度センサの仮のオフセット値の変化の傾きが所定値よりも小さい期間を計数し、計数した期間とフィルタ処理後のオフセット値とを関連付けて記憶するので、両者の値を対応付けることができる。また、計数した期間とフィルタ処理後のオフセット値とを関連付けて記憶するとともに、計数した期間が最大となるオフセット値を出力するので、仮のオフセット値が導出不能な期間でも、ステップ状のシフトを伴うノイズがオフセット値へ与える影響を軽減できる。また、仮のオフセット値が導出可能な期間において、仮のオフセット値に対してローパスフィルタによるフィルタ処理を実行するので、角速度センサのオフセット値が急激に変化する場合でもオフセット値の変動に追従できる。また、オフセット値の変動に追従するので、角速度センサのオフセット値が急激に変化する場合でも角速度の導出精度の低下を抑制できる。

以上、本発明を実施例をもとに説明した。この実施例は例示であり、それらの各構成要素や各処理プロセスの組合せにいろいろな変形例が可能なこと、またそうした変形例も本発明の範囲にあることは当業者に理解されるところである。

本発明の実施例において、有効性判定部２２は、ＧＰＳ測位データ２００の有効性を判定するために、ＰＤＯＰを使用している。しかしながらこれに限らず例えば、有効性判定部２２は、ＧＤＯＰ（Ｇｅｏｍｅｔｒｉｃ Ｄｉｌｕｔｉｏｎ Ｏｆ Ｐｒｅｃｉｓｉｏｎ）、ＨＤＯＰ（Ｈｏｒｉｚｏｎｔａｌ Ｄｉｌｕｔｉｏｎ Ｏｆ Ｐｒｅｃｉｓｉｏｎ）等や、これらの組合せを使用してもよい。本変形例によれば、さまざまなパラメータを判定に使用できる。

本発明の実施例において、オフセット値フィルタ処理部６８は、ＩＩＲフィルタを含むように形成されている。しかしながらこれに限らず例えば、オフセット値フィルタ処理部６８は、ＦＩＲ（Ｆｉｎｉｔｅ Ｉｍｐｕｌｓｅ Ｒｅｓｐｏｎｓｅ）フィルタを含むように形成されていてもよい。その際、忘却係数は、タップ係数として設定される。本変形例によれば、フィルタ構成の自由度を向上できる。

１０ 測定部、 １２ パラメータ演算部、 １４ 角速度変換部、 １６ 制御部、 ２０ ＧＰＳ測位部、 ２２ 有効性判定部、 ２４ 角速度センサ、 ２６ オフセット値演算部、 ２８ 感度係数演算部、 ３０ 状態推定部、 ３２ 状態別オフセット値導出部、 ３４ オフセット値補正部、 ４０ 停止推定部、 ４２ 直進走行推定部、 ４４ 非直進走行推定部、 ４６ 停止時オフセット値導出部、 ４８ 直進走行時オフセット値導出部、 ５０ 非直進走行時オフセット値導出部、 ６０ 平均値算出部、 ６２ 差分絶対値算出部、 ６４ 傾き絶対値算出部、 ６６ オフセット値変化検出部、 ６８ オフセット値フィルタ処理部、 ７０ オフセット値判定部、 ８０ α乗算部、 ８２ 加算部、 ８４ １−α乗算部、 １００ 角速度算出装置。

Claims (3)

1. ＧＰＳ衛星からの信号をもとに測位された対象物の測位データと、角速度センサから出力された対象物の角速度とを取得する取得部と、
   前記取得部において取得した測位データと角速度とを使用することによって、前記角速度センサの仮のオフセット値を逐次導出するオフセット値導出部と、
   前記オフセット値導出部において逐次導出した前記角速度センサの仮のオフセット値に対して統計処理を実行することによって、前記角速度センサから出力された角速度を補正するための前記角速度センサのオフセット値を導出するオフセット値フィルタ処理部と、
   前記オフセット値導出部において逐次導出した前記角速度センサの仮のオフセット値に対する第１期間における変化の傾きと、第１期間よりも短い第２期間における前記角速度センサの仮のオフセット値の変化の絶対値とをもとに、ステップ状非変化、ステップ状変化、ステップ状を越えた変化のいずれかを検出するオフセット値変化検出部と、
   前記オフセット値変化検出部においてステップ状非変化を検出した場合、（１）非変化が連続する期間と、前記オフセット値フィルタ処理部において導出した前記角速度センサのオフセット値とを記憶するとともに、（２）前記角速度センサのオフセット値を出力するオフセット値判定部とを備え、
   前記オフセット値判定部は、前記オフセット値変化検出部においてステップ状を越えた変化を検出した場合、あるいは仮のオフセット値の導出が不能になった場合、（１）連続する期間が最大であるときに記憶された前記角速度センサのオフセット値を出力するとともに、（２）非変化が連続する期間をリセットすることを特徴とする角速度センサ補正装置。
2. 前記オフセット値判定部は、前記オフセット値変化検出部においてステップ状変化を検出した場合、（１）前記オフセット値フィルタ処理部において導出した前記角速度センサのオフセット値を出力するとともに、（２）非変化が連続する期間をリセットすることを特徴とする請求項１に記載の角速度センサ補正装置。
3. ＧＰＳ衛星からの信号をもとに測位された対象物の測位データと、角速度センサから出力された対象物の角速度とを取得するステップと、
   取得した測位データと角速度とを使用することによって、前記角速度センサの仮のオフセット値を逐次導出するステップと、
   逐次導出した前記角速度センサの仮のオフセット値に対して統計処理を実行することによって、前記角速度センサから出力された角速度を補正するための前記角速度センサのオフセット値を導出するステップと、
   逐次導出した前記角速度センサの仮のオフセット値に対する第１期間における変化の傾きと、第１期間よりも短い第２期間における前記角速度センサの仮のオフセット値の変化の絶対値とをもとに、ステップ状非変化、ステップ状変化、ステップ状を越えた変化のいずれかを検出するステップと、
   ステップ状非変化を検出した場合、（１）非変化が連続する期間と、導出した前記角速度センサのオフセット値とを記憶するとともに、（２）前記角速度センサのオフセット値を出力するステップと、
   ステップ状を越えた変化を検出した場合、あるいは仮のオフセット値の導出が不能になった場合、（１）連続する期間が最大であるときに記憶された前記角速度センサのオフセット値を出力するとともに、（２）非変化が連続する期間をリセットするステップと、
   を備えることを特徴とする角速度センサ補正方法。

Images