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

Description

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

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

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

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

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

上記課題を解決するために、本発明のある態様の角速度センサ補正装置は、ＧＰＳ衛星からの信号をもとに測位された対象物の測位データと、角速度センサから出力された対象物の角速度とを取得する取得部と、取得部において取得した測位データと角速度とを使用することによって、角速度センサの仮のオフセット値を逐次導出するオフセット値導出部と、オフセット値導出部において逐次導出した角速度センサの仮のオフセット値に対して統計処理を実行することによって、角速度センサから出力された角速度を補正するための角速度センサのオフセット値を導出するオフセット値フィルタ処理部とを備える。オフセット値フィルタ処理部は、時間経過に対する角速度センサの仮のオフセット値の変化の傾きが大きくなるほど大きくなるような忘却係数を統計処理のために設定し、オフセット値フィルタ処理部は、時間経過に対する角速度センサの仮のオフセット値の変化の傾きを導出するための第１期間よりも短い第２期間において、角速度センサの仮のオフセット値の変化の絶対値がしきい値以上である場合に、忘却係数をゼロに設定する。

本発明の別の態様は、角速度センサ補正方法である。この方法は、ＧＰＳ衛星からの信号をもとに測位された対象物の測位データと、角速度センサから出力された対象物の角速度とを取得するステップと、取得した測位データと角速度とを使用することによって、角速度センサの仮のオフセット値を逐次導出するステップと、逐次導出した角速度センサの仮のオフセット値に対して統計処理を実行することによって、角速度センサから出力された角速度を補正するための角速度センサのオフセット値を導出するステップとを備える。角速度センサのオフセット値を導出するステップは、時間経過に対する角速度センサの仮のオフセット値の変化の傾きが大きくなるほど大きくなるような忘却係数を統計処理のために設定し、角速度センサのオフセット値を導出するステップは、時間経過に対する角速度センサの仮のオフセット値の変化の傾きを導出するための第１期間よりも短い第２期間において、角速度センサの仮のオフセット値の変化の絶対値がしきい値以上である場合に、忘却係数をゼロに設定する。

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

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

本発明の実施例に係る角速度センサからの出力信号の一例を示す図である。 本発明の実施例に係る角速度算出装置の構成を示す図である。 図２のオフセット値演算部の構成を示す図である。 図３のオフセット値補正部の構成を示す図である。 図４のオフセット値フィルタ処理部の構成を示す図である。 図５の忘却係数制御部において記憶されるテーブルのデータ構造を示す図である。 図４のオフセット値補正部による処理の概要を示す図である。 図１の角速度算出装置によるオフセット値の導出手順を示すフローチャートである。

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

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

角速度算出装置は、車両に搭載された角速度センサから出力電圧を入力するとともに、車両に搭載されたＧＰＳ受信機から測位データを入力する。ここで、角速度センサからの出力電圧が、車両の角速度に相当する。また、測位データには、車両の方位、車両の速度、車両の高度等が含まれる。角速度算出装置は、測位データと出力電圧とをもとに、車両の走行状態を推定する。走行状態として、例えば、停止状態、直進走行状態、非直進走行状態のいずれかが特定される。また、角速度算出装置は、走行状態に応じた導出方法にて仮のオフセット値を逐次導出する。さらに、角速度算出装置は、逐次導出した仮のオフセット値に対して、ローパスフィルタによるフィルタ処理を実行する。ここで、角速度算出装置は、時間経過に対する仮のオフセット値の変化の傾きが大きくなるほど、現在の仮のオフセット値の影響が大きくなるように忘却係数を制御する。これにより、仮のオフセット値のステップ状の変化に追随するようなフィルタ処理が実行される。フィルタ処理の結果がオフセット値である。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

オフセット値補正部３４は、状態別オフセット値導出部３２において逐次導出した仮オフセット値２１０を入力する。オフセット値補正部３４は、仮オフセット値２１０の時間に対する変化を検出するとともに、検出結果にもとづいて統計処理を実行することによって、角速度センサ２４のオフセット値２０４を導出する。以下では、図４を使用しながら、オフセット値補正部３４での処理を説明する。

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

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

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

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

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

オフセット値フィルタ処理部６６は、仮オフセット値２１０、差分絶対値２１２、傾き絶対値２１４を入力する。オフセット値フィルタ処理部６６は、仮オフセット値２１０に対して統計処理を実行することによって、角速度センサ２４から出力された角速度を補正するためのオフセット値２０４を導出する。その際、オフセット値フィルタ処理部６６は、差分絶対値２１２、傾き絶対値２１４をもとに、統計処理において使用する忘却係数を導出する。オフセット値フィルタ処理部６６の構成を説明するために、ここでは図５を使用する。

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

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

忘却係数制御部７６は、差分絶対値算出部６２からの差分絶対値２１２と、傾き絶対値算出部６４から傾き絶対値２１４とを入力する。また、忘却係数制御部７６は、差分絶対値２１２と傾き絶対値２１４に応じて、忘却係数「αｉ」の値を決定する。さらに、忘却係数制御部７６は、決定した忘却係数「αｉ」をαｉ乗算部７０および１−αｉ乗算部７４へ設定する。ここでは、忘却係数「αｉ」の値を決定するための処理を詳細に説明する。忘却係数制御部７６は、テーブルを保持しており、当該テーブルを参照することによって忘却係数「αｉ」の値を決定する。

図６は、忘却係数制御部７６において記憶されるテーブルのデータ構造を示す。図示のごとく、絶対値条件欄５００、傾き条件欄５０２、忘却係数欄５０４が含まれる。絶対値条件欄５００には、忘却係数を決定するための差分絶対値２１２に対する条件として、「Ｖｔｈ以上」、「Ｖｔｈ未満」が示されている。ここで、「Ｖｔｈ」は差分絶対値２１２に対するしきい値であり、これには、仮オフセット値２１０に含まれるノイズによる変動幅以上の値が設定される。また、傾き条件欄５０２には、忘却係数を決定するための傾き絶対値２１４に対する条件として、「γｔｈ以上」、「γｔｈ未満」が示されている。ここで、「γｔｈ」は傾き絶対値２１４に対するしきい値であり、これには、例えば、あらかじめ測定した、角速度算出装置１００の起動後数秒間の温度変動に対する仮オフセット値２１０の傾きの絶対値以上の値が設定される。忘却係数欄５０４には、各条件に対応した忘却係数「αｉ」を記憶する。「α１＞α２」である。また、差分絶対値２１２に対する条件が「Ｖｔｈ以上」である場合、忘却係数は「ゼロ」に設定される。図５に戻る。

このように忘却係数制御部７６は、時間経過に対する角速度センサ２４の仮オフセット値２１０の変化の傾きである傾き絶対値２１４が大きくなるほど大きくなるような忘却係数を統計処理のために設定する。特に、忘却係数制御部７６は、テーブルを参照しながら、忘却係数「α１」あるいは「α２」を選択する。また、忘却係数制御部７６は、傾き絶対値２１４を導出するための第１期間Ｔｓよりも短い第２期間Ｔａにおいて、角速度センサ２４の仮オフセット値２１０の変化の絶対値である差分絶対値２１２がしきい値以上である場合に、忘却係数をゼロに設定する。

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

以上の構成による角速度算出装置１００の動作を説明する。図８は、角速度算出装置１００によるオフセット値の導出手順を示すフローチャートである。差分絶対値２１２がＶｔｈよりも小さく（Ｓ１０のＹ）、傾き絶対値２１４がγｔｈ以上である場合（Ｓ１２のＹ）、忘却係数制御部７６は、忘却係数にα１を設定する（Ｓ１４）。傾き絶対値２１４がγｔｈ以上でない場合（Ｓ１２のＮ）、忘却係数制御部７６は、忘却係数にα２を設定する（Ｓ１６）。差分絶対値２１２がＶｔｈよりも小さくない場合（Ｓ１０のＮ）、忘却係数制御部７６は、忘却係数にゼロを設定する（Ｓ１８）。

本発明の実施例によれば、角速度センサの仮のオフセット値の変化の傾きが大きくなるほど大きくなるような忘却係数を統計処理のために設定するので、仮のオフセット値の変化の傾きが大きい場合に現在の仮のオフセット値の影響を大きくできる。また、仮のオフセット値の変化の傾きが大きい場合に現在の仮のオフセット値の影響が大きくなるので、オフセット値の変動に追従できる。また、オフセット値の変動に追従するので、角速度センサのオフセット値が急激に変化する場合でも角速度の導出精度の低下を抑制できる。

また、角速度センサの仮のオフセット値の変化の傾きが大きくなるほど大きくなるような忘却係数を統計処理のために設定するので、仮のオフセット値の変化の傾きが小さい場合に過去の仮のオフセット値の影響を大きくできる。また、仮のオフセット値の変化の傾きが小さい場合に過去の仮のオフセット値の影響が大きくなるので、雑音の影響を低減できる。また、雑音の影響が低減するので、角速度の導出精度の低下を抑制できる。

また、忘却係数を変化させるので、角速度センサの変動に適合したフィルタ処理を実現できる。また、角速度センサの変動に適合したフィルタ処理が実現されるので、角速度の導出精度を向上できる。また、短期間の仮のオフセット値の変化の絶対値がしきい値以上である場合に、忘却係数をゼロに設定するので、想定していない変化を無視できる。また、想定していない変化が無視されるので、角速度の導出精度の低下を抑制できる。

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

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

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

本発明の実施例において、忘却係数制御部７６は、忘却係数としてゼロ以外に２種類の値を設定する。しかしながらこれに限らず例えば、忘却係数制御部７６は、忘却係数としてゼロ以外に３種類以上の値を設定してもよい。本変形例によれば、構成の自由度を向上できる。

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

Claims (2)

1. ＧＰＳ衛星からの信号をもとに測位された対象物の測位データと、角速度センサから出力された対象物の角速度とを取得する取得部と、
   前記取得部において取得した測位データと角速度とを使用することによって、前記角速度センサの仮のオフセット値を逐次導出するオフセット値導出部と、
   前記オフセット値導出部において逐次導出した前記角速度センサの仮のオフセット値に対して統計処理を実行することによって、前記角速度センサから出力された角速度を補正するための前記角速度センサのオフセット値を導出するオフセット値フィルタ処理部とを備え、
   前記オフセット値フィルタ処理部は、時間経過に対する前記角速度センサの仮のオフセット値の変化の傾きが大きくなるほど大きくなるような忘却係数を統計処理のために設定し、
   前記オフセット値フィルタ処理部は、時間経過に対する前記角速度センサの仮のオフセット値の変化の傾きを導出するための第１期間よりも短い第２期間において、前記角速度センサの仮のオフセット値の変化の絶対値がしきい値以上である場合に、忘却係数をゼロに設定することを特徴とする角速度センサ補正装置。
2. ＧＰＳ衛星からの信号をもとに測位された対象物の測位データと、角速度センサから出力された対象物の角速度とを取得するステップと、
   取得した測位データと角速度とを使用することによって、前記角速度センサの仮のオフセット値を逐次導出するステップと、
   逐次導出した前記角速度センサの仮のオフセット値に対して統計処理を実行することによって、前記角速度センサから出力された角速度を補正するための前記角速度センサのオフセット値を導出するステップとを備え、
   前記角速度センサのオフセット値を導出するステップは、時間経過に対する前記角速度センサの仮のオフセット値の変化の傾きが大きくなるほど大きくなるような忘却係数を統計処理のために設定し、
   前記角速度センサのオフセット値を導出するステップは、時間経過に対する前記角速度センサの仮のオフセット値の変化の傾きを導出するための第１期間よりも短い第２期間において、前記角速度センサの仮のオフセット値の変化の絶対値がしきい値以上である場合に、忘却係数をゼロに設定することを特徴とする角速度センサ補正方法。

Images