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

Description

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

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

角速度を正確に求めるためには、角速度センサのオフセット値と、感度係数を正確に求める必要がある。角速度センサの感度係数は、一般的に角速度センサの個体差や経年劣化の影響により異なる。また、オフセット値は温度変化によって変化する可能性がある。つまり、オフセット値は、車両用ナビゲーション装置に使用される基板等の発熱や、車両用ナビゲーション装置が車両のダッシュボード等に取り付けられた場合の車両エンジン等の発熱による温度上昇の影響を受ける。従来、角速度センサのオフセット値は、角速度が「０」になる車両停止時や直進走行時の角速度センサからの出力電圧を用いて補正されていた。しかしながら、高速道路の走行や、交通量の少ない地域における長時間の走行等、車両の停止頻度が少ないケースでは、角速度センサのオフセット値を定期的に補正することが困難になり、オフセット値の精度が悪化しやすくなる。正確に角速度センサからの出力電圧が「０」になるような直進走行時のオフセット値補正についても、道路形状やドライバの運転状況に左右されやすいので、定期的に行うことが困難になる。また、角速度センサの感度係数は、単位期間内での方位変化量と、角速度センサの出力電圧とから導出される。そのため、式（１）から明らかなように、角速度センサの感度係数は、オフセット値の誤差の影響を受ける。

直進走行以外の走行中であっても角速度センサのオフセットおよび感度係数を補正する技術が提案されている。そこでは、所定期間の角速度センサの出力電圧の平均値と、その平均値が算出される期間の車両の方位変化量をもとに、角速度センサのオフセット値および感度係数が補正されている。具体的に説明すると、角速度センサのオフセット値Ｖｏｆｆｓｅｔは、次のように導出される。
Ｖｏｆｆｓｅｔ＝１／ｎ・ΣＶｏｕｔ−１／Δｔ・Δθ／ｎ・Ｓ・ｃｏｓα・・・（２）
ここで、ｎは、角速度センサの出力電圧のサンプル数であり、Δｔ（ｓｅｃ）は、サンプリング間隔であり、Δθ（ｄｅｇ）は、方位変化量であり、αは、鉛直軸に対する角速度センサの検出軸の傾きである。方位変化量は、ＧＰＳ衛星から取得したＧＰＳ方位、あるいは地図データをもとに求められる。鉛直軸に対する角速度センサの検出軸の傾きは、角速度センサの取付角と道路傾斜角を加算して求められる。角速度センサの取付角については、加速度センサから求められ、道路傾斜角は、ＧＰＳ衛星から取得したＧＰＳ高度変化量等から求められる。また、角速度センサの感度係数については、補正されたオフセット値の変化量が小さい、つまり安定した状態において、次のように導出される。
Ｓ＝（１／ｎ・ΣＶｏｕｔ−Ｖｏｆｆｓｅｔ）・ｎ／Δθ・Δｔ／ｃｏｓα・・・（３）
ここで、Ｖｏｆｆｓｅｔは、補正されたオフセット値の変化量が小さいような安定した状態において、既知であり定数とされる（例えば、特許文献１参照）。

特開２００１−３３０４５４号公報

このような状況下において、車両用ナビゲーション装置の位置の推定精度を向上するためには、角速度センサの出力信号を高精度に補正することが望まれる。

本発明はこうした状況に鑑みてなされたものであり、その目的は、角速度センサの出力信号を高精度に補正する技術を提供することにある。

上記課題を解決するために、本発明のある態様の角速度センサ補正装置は、ＧＰＳ衛星からの信号をもとに測位された対象物の測位データと、角速度センサから出力された対象物の角速度とを取得する取得部と、取得部において取得した測位データと角速度とをもとに、角速度センサの仮のオフセット値を逐次導出するオフセット値導出部と、オフセット値導出部において逐次導出した角速度センサの仮のオフセット値に対して統計処理を実行することによって、角速度センサから出力された角速度を補正するための角速度センサのオフセット値を導出するオフセット値フィルタ処理部と、取得部において取得した測位データと角速度とをもとに、相関係数を導出する相関係数導出部とを備える。オフセット値フィルタ処理部は、相関係数導出部において導出した相関係数に応じて、統計処理の際の忘却係数を変更し、相関係数導出部は、角速度の変化量に応じて、相関係数を計算する際の計算期間を調節する。

本発明の別の態様は、角速度センサ補正方法である。この方法は、ＧＰＳ衛星からの信号をもとに測位された対象物の測位データと、角速度センサから出力された対象物の角速度とを取得するステップと、取得した測位データと角速度とをもとに、角速度センサの仮のオフセット値を逐次導出するステップと、逐次導出した角速度センサの仮のオフセット値に対して統計処理を実行することによって、角速度センサから出力された角速度を補正するための角速度センサのオフセット値を導出するステップと、取得した測位データと角速度とをもとに、相関係数を導出するステップとを備える。角速度センサのオフセット値を導出するステップは、導出した相関係数に応じて、統計処理の際の忘却係数を変更し、相関係数を導出するステップは、角速度の変化量に応じて、相関係数を計算する際の計算期間を調節する。

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

本発明によれば、角速度センサの出力信号を高精度に補正できる。

本発明の実施例１に係る角速度算出装置の構成を示す図である。 図１のオフセット値演算部の構成を示す図である。 図２のオフセット値フィルタ処理部の構成を示す図である。 図３の忘却係数制御部において記憶されるテーブルのデータ構造を示す図である。 図１の感度係数演算部の構成を示す図である。 図５の忘却係数制御部において記憶されるテーブルのデータ構造を示す図である。 図１の相関係数演算部において計算される相関係数の概要を示す図である。 図１の角速度算出装置による忘却係数の導出手順を示すフローチャートである。 本発明の実施例２に係る角速度算出装置の構成を示す図である。 図９のオフセット値演算部の構成を示す図である。 図１０のオフセット値フィルタ処理部の構成を示す図である。 図１１の忘却係数制御部において記憶されるテーブルのデータ構造を示す図である。 図９の相関係数演算部において計算される相関係数の概要を示す図である。 本発明の実施例３に係る方位推定装置の構成を示す図である。 図１４のＧＰＳ有効度判断部において記憶されるテーブルのデータ構造を示す図である。 図１４の合成比率決定部において記憶されるテーブルのデータ構造を示す図である。

（実施例１）
本発明を具体的に説明する前に、まず概要を述べる。本発明の実施例は、車両等に搭載され、車両の旋回による角速度を導出する角速度算出装置に関する。角速度算出装置は、角速度センサからの出力電圧に対して、オフセット値と感度係数とを使用しながら、角速度を導出する。角速度センサの感度係数を導出するための式（３）において、Ｖｏｆｆｓｅｔが温度変化等の影響により安定しないことによって、サンプル数を増やすと、感度係数の誤差が増大する可能性がある。また、角速度センサの感度係数を導出するための式（３）において、方位変化量Δθが、ＧＰＳ衛星から取得したＧＰＳ方位から求められる場合、ＧＰＳ衛星からの電波の受信状況によりＧＰＳ方位の精度が悪化することがあり、感度係数に含まれる誤差が大きくなる。

また、式（３）において、方位変化量Δθが、地図データにもとづいて求められる場合、地図データにもとづく道路方位と車両の走行方位とが完全に一致するとは限らないので、感度係数に含まれる誤差が大きくなる可能性がある。さらに、角速度センサの感度係数を導出するための式（３）において、鉛直軸に対する角速度センサの検出軸の傾きに含まれる道路傾斜角が、ＧＰＳ衛星から取得したＧＰＳ高度変化量から求められる場合、ＧＰＳ衛星からの電波の受信状況によりＧＰＳ方位の精度が悪化することがあり、感度係数に含まれる誤差が大きくなる。

これらの場合、角速度の導出精度を向上させるために、感度係数に含まれる誤差の低減が要求される。そのため、本実施例に係る角速度算出装置は、逐次導出した角速度センサの感度の過去の値と現在の値に対して重み付け平均を実行する際に、忘却係数を変更させながら使用する。具体的に説明すると、角速度算出装置は、所定の期間における角速度センサから計算した方位変化とＧＰＳから計算した方位変化との相関係数を算出し、その相関係数をもとに忘却係数を変更する。相関係数が大きくなるほど、角速度センサ、ＧＰＳという異なる手段から計算した方位変化が近くなり、方位変化の測定精度が高くなっている。そのため、このような場合には、角速度センサの感度の現在の値の影響が大きくなるように重み付け平均が計算される。一方、相関係数が小さくなると、方位変化の測定精度が低くなっているので、角速度センサの感度の現在の値の影響が小さくなるように重み付け平均が計算される。

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

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

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

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

取付角検出部２４は、例えば、図示しない加速度センサ等であり、有効性判定部２２からのＧＰＳ測位データ２００を逐次入力し、車両が水平状態での鉛直軸に対する角速度センサの検出軸の傾きを取付角度２０２として算出する。取付角検出部２４は、入力したＧＰＳ測位データ２００のＧＰＳ高度に変化が微小であるときに車両が水平状態であると判断し、このとき、車両が停止から発進したときに検出される加速度にもとづいて取付角度２０２を算出する。なお、加速度センサ装置には、公知の技術が使用されればよいので、ここでは説明を省略する。取付角検出部２４は、検出した取付角度２０２を傾斜角検出部２６へ出力する。

傾斜角検出部２６は、有効性判定部２２からのＧＰＳ測位データ２００、特にＧＰＳ測位データ２００に含まれたＧＰＳ高度と、取付角検出部２４からの取付角度２０２とを逐次入力する。傾斜角検出部２６は、逐次入力したＧＰＳ高度をもとに、サンプリング間隔における車両の平均傾斜角度（以下、「傾斜角度２０４」という）を検出する。具体的に説明すると、傾斜角検出部２６は、連続したＧＰＳ高度の差異を逐次計算してから、計算結果を平均した後に、平均値をサンプリング間隔で除算することによって、車両の傾斜角度を導出する。ここで、連続したＧＰＳ高度の間隔が、サンプリング間隔に相当する。傾斜角検出部２６は、算出した車両の傾斜角度に取付角度２０２を加算し、傾斜角度２０４としてオフセット値演算部３０、感度係数演算部３２、測位データ記憶部３６へ逐次出力する。

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

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

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

感度係数演算部３２は、有効性判定部２２からのＧＰＳ測位データ２００、傾斜角検出部２６からの傾斜角度２０４、角速度センサ２８からの出力信号２０６を入力する。また、感度係数演算部３２は、オフセット値演算部３０からのオフセット値２０８、相関係数演算部３４からの相関係数２１２も入力する。感度係数演算部３２は、所定期間、例えば１０秒間にわたって入力された、ＧＰＳ測位データ２００、出力信号２０６、傾斜角度２０４、オフセット値２０８とをもとに、角速度センサ２８の感度係数（以下、前述の「感度係数２１０」という）を算出する。なお、感度係数演算部３２での処理の詳細、特に相関係数２１２を使用した処理の詳細は後述する。感度係数演算部３２は、感度係数２１０を角速度変換部１４、オフセット値演算部３０へ出力する。

測位データ記憶部３６は、メモリ上のリングバッファ等で構成され、有効性判定部２２からのＧＰＳ測位データ２００、傾斜角検出部２６からの傾斜角度２０４、角速度センサ２８からの出力信号２０６を逐次入力する。測位データ記憶部３６は、有効性判定部２２において有効であると判定されたＧＰＳ測位データ２００を取得し、所定期間、例えば感度係数演算部３２が感度係数２１０を算出する期間において連続して有効であった場合、ＧＰＳ測位データ２００、傾斜角度２０４、出力信号２０６を記憶データ２１４として記憶し、記憶データの有効性フラグを有効とする。また、有効なＧＰＳ測位データが入力されるごとに、記憶データ２１４を更新する。測位データ記憶部３６は、入力されたＧＰＳ測位データ２００が無効であった場合、記憶データ２１４をクリアし、有効性フラグを無効とする。測位データ記憶部３６は、記憶した記憶データ２１４に有効性フラグを付加し、相関係数演算部３４へ出力する。

相関係数演算部３４は、オフセット値演算部３０からのオフセット値２０８、測位データ記憶部３６からの記憶データ２１４を逐次入力する。相関係数演算部３４は、記憶データ２１４に付加された有効性フラグが有効であった場合、出力信号２０６の積算値とオフセット値２０８との差分を傾斜角度２０４の余弦値で除算した値と、ＧＰＳ測位データ２００に含まれるＧＰＳ方位変化量とについて、所定期間、例えば感度係数演算部３２が感度係数２１０を算出する期間において、相関係数２１２を算出する。なお、相関係数演算部３４での処理の詳細は後述する。相関係数演算部３４は、相関係数２１２を感度係数演算部３２へ出力する。

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

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

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

状態推定部４０は、ＧＰＳ測位データ２００、傾斜角度２０４、出力信号２０６を入力する。状態推定部４０は、停止推定部５０、直進走行推定部５２、非直進走行推定部５４において、車両の走行状態を推定する。ここでは、車両の走行状態として、車両が停止あるいは直進している状態であるか、残りの状態、つまり非直進走行している状態であるかを推定する。また、状態推定部４０は、判定結果を走行状態情報２１８としてオフセット値フィルタ処理部４４へ出力する。

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

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

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

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

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

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

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

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

オフセット値フィルタ処理部４４は、状態別オフセット値導出部４２において逐次導出した仮オフセット値２１６を入力する。オフセット値フィルタ処理部４４は、仮オフセット値２１６に対して統計処理を実行することによって、角速度センサ２８のオフセット値２０８を導出する。なお、オフセット値２０８は、Ｇｏｆｆｓｅｔと示され、これは、式（１）のＶｏｆｆｓｅｔに対応する。以下では、図３を使用しながら、オフセット値フィルタ処理部４４での処理を説明する。

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

加算部７２は、αｉ乗算部７０からの乗算結果と、１−αｉ乗算部７４からの乗算結果とを逐次加算する。加算部７２は、加算結果をオフセット値２０８として逐次出力する。１−αｉ乗算部７４は、オフセット値２０８に係数「１−αｉ」を乗算する。なお、係数「１−αｉ」のうちの「αｉ」は、αｉ乗算部７０でのαｉと同様であるので、ここでは説明を省略する。１−αｉ乗算部７４は、加算部７２へ乗算結果をフィードバックする。忘却係数制御部７６は、走行状態情報２１８を入力する。また、忘却係数制御部７６は、走行状態情報２１８にて示された状態に応じて、忘却係数「αｉ」の値を決定する。さらに、忘却係数制御部７６は、決定した忘却係数「αｉ」をαｉ乗算部７０および１−αｉ乗算部７４へ設定する。

図４は、忘却係数制御部７６において記憶されるテーブルのデータ構造を示す。図示のごとく、走行状態欄３００、忘却係数欄３０２が含まれる。走行状態欄３００には、走行状態情報２１８にて示される各状態が含まれる。忘却係数欄３０２には、各状態に対応した忘却係数「αｉ」を記憶する。つまり、停止状態と直進走行状態に対して忘却係数「α１」が対応づけられ、非直進走行状態に対して忘却係数「α２」が対応づけられる。例えば、α１＞α２である。図３に戻る。忘却係数制御部７６は、図４のテーブルを参照しながら、走行状態情報２１８にて示された状態から忘却係数「α１」あるいは「α２」を選択する。このように、忘却係数制御部７６は、車両の走行状態に応じて、フィルタ処理の際の忘却係数を変更する。

図５は、感度係数演算部３２の構成を示す。感度係数演算部３２は、感度係数導出部９０、感度係数フィルタ部９２、忘却係数制御部９４を含む。感度係数導出部９０は、ＧＰＳ測位データ２００、傾斜角度２０４、出力信号２０６、相関係数２１２を入力する。また、感度係数演算部３２は、オフセット値２０８も入力する。また、感度係数導出部９０は、ＧＰＳ測位データ２００、傾斜角度２０４、出力信号２０６、相関係数２１２、オフセット値２０８とをもとに、角速度センサ２８の仮の感度係数を逐次導出する。

具体的に説明すると、感度係数導出部９０は、ＧＰＳ測位データ２００においてＧＰＳ方位が有効であると示されている場合、次のように、ＧＰＳ方位のサンプリング間隔における角速度センサ２８の仮の感度係数を算出する。
Ｇｓｅｎｓｉｔｉｖｉｔｙ＝（（１／ｎ・ΣＧｏｕｔ−Ｇｏｆｆｓｅｔ）／ｃｏｓ（α））／Δθ ・・・（５）
ここで、Ｇｏｆｆｓｅｔは、図示しないオフセット値演算部３０から入力されるが、角速度算出装置１００の起動直後などのような状態において、オフセット値２０８が未だ算出されていないこともありえる。式（５）には、Δθによる除算が含まれるので、Δθが所定値以上であるときに感度係数を算出する。Δθの値が所定値以下であるときは、感度係数導出部９０は、直前に補正した感度係数を出力する。

感度係数フィルタ部９２は、感度係数導出部９０において逐次導出した仮の感度係数を入力する。感度係数フィルタ部９２は、仮の感度係数に対して統計処理を実行することによって、角速度センサ２８の感度係数２１０を導出する。感度係数２１０は、角速度センサ２８から出力された出力信号２０６を補正するための値といえる。感度係数フィルタ部９２は、図３に示されたオフセット値フィルタ処理部４４と同様に、ＩＩＲフィルタにて構成されており、ＩＩＲフィルタは、ローパスフィルタを構成し、忘却係数制御部９４からの指示をもとに、ＩＩＲフィルタでの忘却係数を設定する。

忘却係数制御部９４は、相関係数２１２に応じて、感度係数フィルタ部９２での忘却係数を変更する。また、忘却係数制御部９４は、変更した忘却係数の使用を感度係数フィルタ部９２へ指示する。図６は、忘却係数制御部９４において記憶されるテーブルのデータ構造を示す。図示のごとく、条件欄４００、忘却係数欄４０２が示される。条件欄４００には、忘却係数を決定するための相関係数２１２の条件として、「ｘ以上」、「ｘ未満」が示されている。忘却係数欄４０２には、条件欄４００での各条件に対応した忘却係数の値が示される。具体的には、「ｘ以上」の場合に対して、忘却係数α１０が設定されており、「ｘ未満」の場合に対して、忘却係数α１１が規定されている。また、α１１＜α１０の関係が規定されている。

ここでは、図７を用いて相関係数演算部３４が算出する相関係数を説明する。図７は、相関係数演算部３４において計算される相関係数の概要を示し、これは、相関係数演算部３４に入力された出力信号２０６とオフセット値２０８との差分を傾斜角度２０４の余弦値で除算した値と、ＧＰＳ測位データ２００に含まれるＧＰＳ方位変化量のグラフを示す。相関係数演算部３４は、次のように、出力信号２０６の積算値とオフセット値２０８との差分を傾斜角度２０４の余弦値で除算することによって、Ｄ_Ｇｙｒｏを導出する。

感度係数算出式である（式５）の右辺は、角速度センサ出力変化量とＧＰＳ方位変化量の比を表しており、Ｄ_Ｇｙｒｏは角速度センサ出力変化量に相当する。相関係数演算部３４は、次のように、角速度センサ出力変化量Ｄ_ＧｙｒｏとＧＰＳ方位変化量Δθとの間の相関係数Ｃを導出する。

感度係数２１０は、一般的に個体差や経年劣化の影響を受けるが、短期的には値の変化は少ない。したがって、相関係数Ｃが大きいということは、換言すると、（式５）右辺の各変数の導出精度が良好で、誤差が少ないといえる。つまり、図７において、Ｄ_ＧｙｒｏとΔθとの関連が高い場合には、各変数の導出精度が良好であり、Ｄ_ＧｙｒｏとΔθとの関連が低い場合には、各変数の導出精度が悪化している。相関係数演算部３４は、相関係数Ｃを相関係数２１２として、感度係数演算部３２へ出力する。なお、前述のごとく、感度係数フィルタ部９２において、相関係数２１２がｘ以上の場合は感度係数２１０の導出精度が高いので、ｘ未満の場合よりも、新たに導出された仮の感度係数の重み付けを大きくした平均化処理がなされる。

以上の構成による角速度算出装置１００の動作を説明する。図８は、角速度算出装置１００による忘却係数の導出手順を示すフローチャートである。相関係数演算部３４は、測位データ記憶部３６から記憶データ２１４が入力された場合（Ｓ１０のＹ）、所定期間のＧＰＳ方位変化量と、角速度センサ出力変化量とを算出する（Ｓ１２）。相関係数演算部３４は、測位データ記憶部３６から記憶データ２１４が入力されない場合（Ｓ１０のＮ）、待機する。相関係数演算部３４は、算出したＧＰＳ方位変化量と角速度センサ出力変化量との相関係数を演算し（Ｓ１４）、忘却係数制御部９４へ出力する。忘却係数制御部９４は、相関係数がしきい値以上であった場合（Ｓ１６のＹ）、感度係数フィルタ部９２へ忘却係数α１０を設定し、相関係数がしきい値未満であった場合（Ｓ１６のＮ）、感度係数フィルタ部９２へ忘却係数α１１を設定する。

本発明の実施例によれば、相関係数をもとに忘却係数を変化させるので、ＧＰＳの受信状態に適合したフィルタ処理によって感度係数を導出できる。また、ＧＰＳの受信状態に適合したフィルタ処理が実現されるので、感度係数の導出精度を向上できる。また、角速度センサ出力変化量とＧＰＳ方位変化量との相関係数を算出し、その相関係数にもとづいて忘却係数を制御するので、ＧＰＳ方位変化量から求められるΔθや、車両の平均傾斜角度αに含まれる誤差の影響を軽減でき、感度係数の導出精度を向上できる。また、相関係数に応じて忘却係数を設定するので、式（５）におけるＧＰＳ方位変化量から求められるΔθや、車両の平均傾斜角度αに含まれる誤差を低減できる。

（実施例２）
次に、実施例２を説明する。実施例２も、実施例１と同様に、角速度センサからの出力電圧に対して、オフセット値と感度係数とを使用しながら、角速度を導出する角速度算出装置に関する。角速度センサのオフセット値を導出するための式（２）において、方位変化量Δθが、ＧＰＳ衛星から取得したＧＰＳ方位から求められる場合、ＧＰＳ衛星からの電波の受信状況によりＧＰＳ方位の精度が悪化することがあり、オフセット値に含まれる誤差が大きくなる。

また、式（２）において、方位変化量Δθが、地図データにもとづいて求められる場合、地図データにもとづく道路方位と車両の走行方位とが完全に一致するとは限らないので、オフセット値に含まれる誤差が大きくなる可能性がある。さらに、角速度センサのオフセット値を導出するための式（２）において、鉛直軸に対する角速度センサの検出軸の傾きに含まれる道路傾斜角が、ＧＰＳ衛星から取得したＧＰＳ高度変化量から求められる場合、ＧＰＳ衛星からの電波の受信状況によりＧＰＳ方位の精度が悪化することがあり、オフセット値に含まれる誤差が大きくなる。

これらの場合、角速度の導出精度を向上させるために、オフセット値に含まれる誤差の低減が要求される。そのため、本実施例に係る角速度算出装置は、逐次導出した角速度センサのオフセット値の過去の値と現在の値に対して重み付け平均を実行する際に、忘却係数を変更させながら使用する。具体的に説明すると、角速度算出装置は、所定の期間における角速度センサから計算した方位変化とＧＰＳから計算した方位変化との相関係数を算出し、その相関係数をもとに忘却係数を変更する。

図９は、本発明の実施例２に係る角速度算出装置１００の構成を示す。角速度算出装置１００は、図１と同様の部材によって構成されているが、相関係数演算部３４が相関係数２１２をオフセット値演算部３０へ出力している点が異なる。以下では、これまでとの差異を中心に説明する。

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

相関係数演算部３４は、オフセット値演算部３０からのオフセット値２０８、測位データ記憶部３６からの記憶データ２１４を逐次入力する。相関係数演算部３４は、記憶データ２１４に付加された有効性フラグが有効であった場合、出力信号２０６を傾斜角度２０４の余弦値で除算した値と、ＧＰＳ測位データ２００に含まれるＧＰＳ方位変化量とについて、所定期間、例えば感度係数演算部３２が感度係数２１０を算出する期間において、相関係数２１２を算出する。なお、相関係数演算部３４での処理の詳細は後述する。相関係数演算部３４は、オフセット値演算部３０、感度係数演算部３２へ相関係数２１２を出力する。

図１０は、オフセット値演算部３０の構成を示す。オフセット値演算部３０は、図２と同様の部材によって構成されているが、オフセット値フィルタ処理部４４に相関係数２１２が入力されている点が異なる。以下では、これまでとの差異を中心に説明する。オフセット値フィルタ処理部４４は、状態別オフセット値導出部４２において逐次導出した仮オフセット値２１６を入力する。オフセット値フィルタ処理部４４は、仮オフセット値２１６に対して統計処理を実行することによって、角速度センサ２８のオフセット値２０８を導出する。その際、オフセット値フィルタ処理部４４は、相関係数２１２に応じて統計処理の際の忘却係数を変更する。以下では、図１１を使用しながら、オフセット値フィルタ処理部４４での処理を説明する。

図１１は、オフセット値フィルタ処理部４４の構成を示す。オフセット値フィルタ処理部４４は、図３と同様の部材によって構成されているが、忘却係数制御部７６に相関係数２１２が入力されている点が異なる。以下では、これまでとの差異を中心に説明する。忘却係数制御部７６は、相関係数２１２、走行状態情報２１８を入力する。また、忘却係数制御部７６は、走行状態情報２１８にて示された状態と相関係数２１２に応じて、忘却係数「αｉ」の値を決定する。さらに、忘却係数制御部７６は、決定した忘却係数「αｉ」をαｉ乗算部７０および１−αｉ乗算部７４へ設定する。

図１２は、忘却係数制御部７６において記憶されるテーブルのデータ構造を示す。図示のごとく、走行状態欄５００、条件欄５０２、忘却係数欄５０４が含まれる。走行状態欄５００には、走行状態情報２１８にて示される各状態が含まれる。条件欄５０２には、忘却係数を決定するための相関係数２１２の条件として、「ｘ以上」、「ｘ未満」が示されている。忘却係数欄５０４には、各状態に対応した忘却係数「αｉ」を記憶する。つまり、停止状態と直進走行状態に対して忘却係数「α１」が対応づけられる。また、非直進走行状態に対しては条件欄５０２に対応して、「ｘ以上」であれば「α２１」が、「ｘ未満」であれば「α２２」が対応づけられる。このように、忘却係数制御部７６は、図１２のテーブルを参照しながら、走行状態情報２１８にて示された状態から忘却係数「α１」あるいは「α２」を選択する。そのため、忘却係数制御部７６は、車両の走行状態に応じて、フィルタ処理の際の忘却係数を変更する。

ここでは、図１３を用いて相関係数演算部３４が算出する相関係数を説明する。図１３は、相関係数演算部３４において計算される相関係数の概要を示し、これは、相関係数演算部３４に入力された出力信号２０６を傾斜角度２０４の余弦値で除算した値と、ＧＰＳ測位データ２００に含まれるＧＰＳ方位変化量のグラフを示す。相関係数演算部３４は、次のように出力信号２０６の積算値を傾斜角度２０４の余弦値で除算することによって、Ｄ'_Ｇｙｒｏを導出する。

Ｄ'_Ｇｙｒｏも角速度センサ出力変化量に相当する。相関係数演算部３４は、次のように、角速度センサ出力変化量Ｄ'_ＧｙｒｏとＧＰＳ方位変化量Δθとの間の相関係数Ｃを導出する。

ここで、角速度センサ出力変化量Ｄ'_Ｇｙｒｏは角速度センサ出力変化量Ｄ_ＧｙｒｏからＧｏｆｆｓｅｔの項を除いた値であり、相関係数Ｃの算出期間においてＧｏｆｆｓｅｔが変化しなければ、方位変化量Δθとのそれぞれとの相関係数は一致する。したがって、前述のように、相関係数Ｃが大きい場合、式（４）右辺の「１／ｎ・ΣＧｏｕｔ」および「Δθ」、「ｃｏｓ（α）」の精度が良好であるといえる。相関係数演算部３４は、相関係数Ｃを相関係数２１２として、オフセット値演算部３０、感度係数演算部３２へ出力する。なお、感度係数演算部３２へ出力される相関係数２１２は、式（７）によって計算されたものであってもよい。

本発明の実施例によれば、相関係数をもとに忘却係数を変化させるので、ＧＰＳの受信状態に適合したフィルタ処理によってオフセット値を導出できる。また、ＧＰＳの受信状態に適合したフィルタ処理が実現されるので、オフセット値の導出精度を向上できる。また、角速度センサ出力変化量とＧＰＳ方位変化量との相関係数を算出し、その相関係数にもとづいて忘却係数を制御するので、ＧＰＳ方位変化量から求められるΔθや、車両の平均傾斜角度αに含まれる誤差の影響を軽減でき、オフセット値の導出精度を向上できる。また、相関係数に応じて忘却係数を設定するので、式（５）におけるＧＰＳ方位変化量から求められるΔθや、車両の平均傾斜角度αに含まれる誤差を低減できる。

（実施例３）
次に、実施例３を説明する。実施例３は、これまでの角速度算出装置において導出した角速度を使用して方位を推定する方位推定装置に関する。角速度センサのオフセットおよび感度係数を補正しても、わずかな誤差が累積し、自立航法による位置に含まれる誤差が顕著になる場合がある。このため、ＧＰＳの衛星捕捉状況が悪い地域等を走行するとき、長期間にわたって自立航法の合成比率が大きくなるので、推定した位置に誤差が含まれることがある。

この場合、ＧＰＳの衛星捕捉状況が良好になったときに、ＧＰＳから算出される位置の合成比率を大きくし、推定した位置の誤差を短期間で補正する必要がある。そのため、本実施例に係る方位推定装置は、ＧＰＳから算出した方位と角速度センサから算出した角速度とを合成する場合において、所定の期間における角速度センサから計算した方位変化とＧＰＳから計算した方位変化との相関係数を算出し、その相関係数にもとづいて合成の比率を変更する。

図１４は、本発明の実施例３に係る方位推定装置１５０の構成を示す。方位推定装置１５０は、角速度算出装置１００と、推測航法位置演算部１６を含む。推測航法位置演算部１６は、ＧＰＳ有効度判断部１１２、合成比率決定部１１４、推測方位導出部１１６を含む。また、信号として、ＧＰＳ測位データ２００、傾斜角度２０４、出力信号２０６、相関係数２１２、角速度２２０、ＧＰＳ有効度２２２、合成比率２２４を含む。角速度算出装置１００は、図１あるいは図９と同様に構成されるので、ここでは、説明を省略する。なお、パラメータ演算部１２に含まれた相関係数演算部３４は、相関係数２１２を推測航法位置演算部１６にも出力する。

推測航法位置演算部１６は、角速度算出装置１００からＧＰＳ測位データ２００、相関係数２１２、角速度２２０とを入力し、推測航法方位を導出する。ＧＰＳ有効度判断部１１２は、測定部１０からＧＰＳ測位データ２００を入力し、ＧＰＳ測位データ２００に含まれるＨＤＯＰ値、受信衛星数、速度に対しそれぞれ複数のしきい値を設け、それぞれの値に応じてＧＰＳ有効度２２２を段階的に判断する。ＧＰＳ有効度判断部１１２において設けられるしきい値は、有効性判定部２２において設けられるしきい値と同一であってもよく、異なっていてもよい。

図１５は、ＧＰＳ有効度判断部１１２において記憶されるテーブルのデータ構造を示す。条件欄６００には、ＧＰＳ有効度を判断するためのＨＤＯＰ値、ＧＰＳ速度、ＧＰＳ受信衛星数の条件が示される。ここでは、「ＨＤＯＰ値がしきい値Ｃ１未満」かつ「ＧＰＳ速度がしきい値Ｃ２以上」かつ「ＧＰＳ受信衛星数がしきい値Ｃ３以上」であれば、ＧＰＳ測位データ２００の精度が高いと判断し、ＧＰＳ有効度欄６０２に「高」が示されている。また、「ＨＤＯＰ値がしきい値Ｃ１以上」または「ＧＰＳ速度が０より大きく、かつ、しきい値Ｃ２未満」または「ＧＰＳ受信衛星数がしきい値Ｃ４以上、かつ、しきい値Ｃ３未満」であれば、ＧＰＳ測位データ２００の精度が低いと判断し、ＧＰＳ有効度欄６０２に「低」が示されている。さらに、「ＧＰＳ受信衛星数がしきい値Ｃ４未満」であれば、ＨＤＯＰ値やＧＰＳ速度が無効値であると判断し、ＧＰＳ有効度欄に「無効」が示されている。図１４に戻る。ＧＰＳ有効度判断部１１２は、判断の結果をＧＰＳ有効度２２２として合成比率決定部１１４へ出力する。

合成比率決定部１１４は、パラメータ演算部１２からの相関係数２１２、ＧＰＳ有効度判断部１１２からのＧＰＳ有効度２２２を入力し、相関係数２１２とＧＰＳ有効度２２２とをもとに、ＧＰＳ測位データ２００に含まれるＧＰＳ方位変化量と角速度２２０とを合成する際の比率（以下、「合成比率」ともいう）を決定する。図１６は、合成比率決定部１１４において記憶されるテーブルのデータ構造を示す。ＧＰＳ有効度２２２が「高」である場合、相関係数２１２がしきい値Ｃ５以上であれば、合成比率は「ＧＰＳ方位変化量１００％、角速度０％」とされ、相関係数２１２がしきい値Ｃ５未満であれば、合成比率は「ＧＰＳ方位変化量５０％、角速度５０％」とされる。ＧＰＳ有効度２２２が「低」である場合、相関係数２１２がしきい値Ｃ５以上であれば、合成比率は「ＧＰＳ方位変化量８０％、角速度２０％」とされ、相関係数２１２がしきい値Ｃ５未満であれば、合成比率は「ＧＰＳ方位変化量３０％、角速度７０％」とされる。

ＧＰＳ有効度２２２が「無効ではない」である場合、相関係数２１２がしきい値Ｃ５以上であれば、合成比率は「ＧＰＳ方位変化量６０％、角速度４０％」とされ、相関係数２１２がしきい値Ｃ５未満であれば、合成比率は「ＧＰＳ方位変化量２０％、角速度８０％」とされる。ＧＰＳ有効度２２２が「無効」である場合、相関係数２１２がしきい値Ｃ５未満であれば、合成比率は「ＧＰＳ方位変化量０％、角速度１００％」とされる。つまり、合成比率決定部１１４は、ＧＰＳ有効度２２２が高くなるほど、ＧＰＳ方位変化量の比率を高くし、角速度の比率を低くする。また、合成比率決定部１１４は、相関係数２１２が高くなるほど、ＧＰＳ方位変化量の比率を高くし、角速度の比率を低くする。図１４に戻る。合成比率決定部１１４は、決定した合成比率２２４を推測方位導出部１１６へ出力する。

推測方位導出部１１６は、測定部１０からのＧＰＳ測位データ２００、合成比率決定部１１４からの合成比率２２４、角速度変換部１４からの角速度２２０とを入力する。推測方位導出部１１６は、合成比率２２４をもとに、ＧＰＳ測位データ２００に含まれたＧＰＳ方位変化量と、仮オフセット値２１６とを合成することによって、方位変化量を導出する。例えば、北向きの方位を０度／３６０度と表現すると、入力したＧＰＳ測位データ２００から算出したＧＰＳ方位変化量が１０（度／秒）、角速度２２０が９（度／秒）、合成比率２２４が「ＧＰＳ方位変化量８０％、角速度２０％」であった場合、推測方位導出部１１６は、方位変化量を１０×０．８＋９×０．２＝９．８（度／秒）のように算出する。さらに、推測方位導出部１１６は、算出した方位変化量を前回導出した推測方位へ加算することによって、推測方位を更新する。

なお、推測方位導出部１１６は、合成比率２２４が「ＧＰＳ方位変化量１００％、角速度０％」であった場合、つまり合成比率が、ＧＰＳ方位変化量のみの使用を示していた場合、更新前の推測方位として、ＧＰＳ測位データ２００に含まれた方位を使用してもよい。これは、更新前の推測方位をＧＰＳ方位で置換することに相当する。このようにすることで、ＧＰＳ測位精度が悪い、合成比率２２４が「ＧＰＳ方位変化量０％、角速度１００％」の期間が長く自立航法の誤差が蓄積された状態でも、ＧＰＳ測位状態がよくなった時点において短期間で推測方位の精度を向上することができる。

本発明の実施例によれば、ＧＰＳ方位から算出した方位変化量と角速度センサから算出した角速度との相関係数にもとづいて方位変化量と角速度とを合成する際の比率を調節するので、測定精度に応じた方位変化量を導出できる。また、合成比率をもとに、推測航法方位を補正する度合いを変化させるので、位置の推定期間を短縮できる。また、合成比率をもとに、推測航法方位を補正する度合いを変化させるので、位置の推定精度を向上できる。また、合成比率がＧＰＳ方位変化量のみの使用を示していた場合、更新前の推測方位として、測位データに含まれた方位を使用するので、累積していた誤差を短期間のうちに低減できる。

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

本発明の実施例１から３において、相関係数演算部３４は、相関係数を計算する際の計算期間を固定している。しかしながらこれに限らず例えば、相関係数演算部３４は、角速度の変化量に応じて、相関係数を計算する際の計算期間を調節してもよい。ここで、角速度の変化量は、出力信号２０６から導出されてもよく、ＧＰＳ測位データ２００に含まれたＧＰＳ方位変化量Δθから導出されてもよい。これにより、角速度の変化量が小さい場合には、計算期間が長くされ、角速度の変化量が大きい場合には、計算期間が短くされる。本変形例によれば、角速度の変化量に応じた相関係数を導出できる。

本発明の実施例１から３において、状態推定部４０は、車両の走行状態判定として、ＧＰＳ測位データ２００に含まれたＧＰＳ速度を使用して、停止状態を判定している。しかしながらこれに限らず例えば、状態推定部４０は、図示しないパルス検出部からの車両の車速パルス信号を入力し、車速パルスをもとに停止状態を判定してもよい。ここで、パルス検出部は、図示しない速度センサに接続されており、速度センサは、ドライブシャフトの回転に対応して回転するスピードメータケーブルの中間に設置され、ドライブシャフトの回転に伴った車速パルス信号を出力する。本変形例によれば、さまざまな手段によって車両の速度を測定できる。

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

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

本発明の実施例１から３において、忘却係数制御部７６は、停止状態に対する忘却係数と、直進走行状態に対する忘却係数とに対して、同一の値を規定する。しかしながらこれに限らず例えば、忘却係数制御部７６は、これらに対して異なった値を設定してもよい。これは、３つの状態のそれぞれに対して異なった処理がなされているといえる。本変形例によれば、走行状態として３つの状態に対して、３つの忘却係数を用意するので、各状態に適合したフィルタ処理を実現できる。

１０ 測定部、 １２ パラメータ演算部、 １４ 角速度変換部、 １６ 推測航法位置演算部、 ２０ ＧＰＳ測位部、 ２２ 有効性判定部、 ２４ 取付角検出部、 ２６ 傾斜角検出部、 ２８ 角速度センサ、 ３０ オフセット値演算部、 ３２ 感度係数演算部、 ３４ 相関係数演算部、 ３６ 測位データ記憶部、 ３８ 制御部、 ４０ 状態推定部、 ４２ 状態別オフセット値導出部、 ４４ オフセット値フィルタ処理部、 ５０ 停止推定部、 ５２ 直進走行推定部、 ５４ 非直進走行推定部、 ５６ 停止時オフセット値導出部、 ５８ 直進走行時オフセット値導出部、 ６０ 非直進走行時オフセット値導出部、 ７０ αｉ乗算部、 ７２ 加算部、 ７４ １−αｉ乗算部、 ７６ 忘却係数制御部、 ９０ 感度係数導出部、 ９２ 感度係数フィルタ部、 ９４ 忘却係数制御部、 １００ 角速度算出装置。

Claims (3)

1. ＧＰＳ衛星からの信号をもとに測位された対象物の測位データと、角速度センサから出力された対象物の角速度とを取得する取得部と、
   前記取得部において取得した測位データと角速度とをもとに、前記角速度センサの仮のオフセット値を逐次導出するオフセット値導出部と、
   前記オフセット値導出部において逐次導出した前記角速度センサの仮のオフセット値に対して統計処理を実行することによって、前記角速度センサから出力された角速度を補正するための前記角速度センサのオフセット値を導出するオフセット値フィルタ処理部と、
   前記取得部において取得した測位データと角速度とをもとに、相関係数を導出する相関係数導出部とを備え、
   前記オフセット値フィルタ処理部は、前記相関係数導出部において導出した相関係数に応じて、統計処理の際の忘却係数を変更し、
   前記相関係数導出部は、角速度の変化量に応じて、相関係数を計算する際の計算期間を調節することを特徴とする角速度センサ補正装置。
2. 前記相関係数導出部は、前記角速度センサの傾斜角度の余弦値によって少なくとも角速度の積算値を除算した値と、測位データに含まれた方位角の変化量とをもとに、相関係数を計算することを特徴とする請求項１に記載の角速度センサ補正装置。
3. ＧＰＳ衛星からの信号をもとに測位された対象物の測位データと、角速度センサから出力された対象物の角速度とを取得するステップと、
   取得した測位データと角速度とをもとに、前記角速度センサの仮のオフセット値を逐次導出するステップと、
   逐次導出した前記角速度センサの仮のオフセット値に対して統計処理を実行することによって、前記角速度センサから出力された角速度を補正するための前記角速度センサのオフセット値を導出するステップと、
   取得した測位データと角速度とをもとに、相関係数を導出するステップとを備え、
   前記角速度センサのオフセット値を導出するステップは、導出した相関係数に応じて、統計処理の際の忘却係数を変更し、
   前記相関係数を導出するステップは、角速度の変化量に応じて、相関係数を計算する際の計算期間を調節することを特徴とする角速度センサ補正方法。

Images