車両用現在位置検出装置
【課題】角速度センサの出力値を車両の回転角速度に換算する比率を示す換算ゲインを精度良く補正する。
【解決手段】ナビゲーション装置１０において、演算部２２は、換算ゲインを用いジャイロ１２の出力値に基づいて車両の方位変化量を算出する。演算部２１は、車速センサ１１の出力値に基づいて車両の移動距離を算出する。現在位置検出部１４は、算出される方位変化量および移動距離に基づいて車両の現在位置や進行方向を算出する。誤差推定部２５は、ＧＰＳ受信機１３からの出力値と現在位置検出部１４での算出値との差を観測値として算出し、その観測値に基づいて換算ゲインの誤差を状態量として推定する。補正部２６は、推定される状態量に基づいて換算ゲインを補正する。換算ゲインの誤差共分散値が所定値よりも大きい場合には、当該換算ゲインの誤差共分散値を所定値に置き換える。
【解決手段】ナビゲーション装置１０において、演算部２２は、換算ゲインを用いジャイロ１２の出力値に基づいて車両の方位変化量を算出する。演算部２１は、車速センサ１１の出力値に基づいて車両の移動距離を算出する。現在位置検出部１４は、算出される方位変化量および移動距離に基づいて車両の現在位置や進行方向を算出する。誤差推定部２５は、ＧＰＳ受信機１３からの出力値と現在位置検出部１４での算出値との差を観測値として算出し、その観測値に基づいて換算ゲインの誤差を状態量として推定する。補正部２６は、推定される状態量に基づいて換算ゲインを補正する。換算ゲインの誤差共分散値が所定値よりも大きい場合には、当該換算ゲインの誤差共分散値を所定値に置き換える。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、車両の進行方向および走行距離に基づいて車両の現在位置を検出する車両用現在位置検出装置に関する。
【背景技術】
【０００２】
ジャイロスコープなどの角速度センサ（方位センサ）の出力情報を補正する技術に関して、例えば特許文献１には、以下のような車両用現在位置検出装置に関する技術が開示されている。この車両用現在位置検出装置は、車両の回転角速度に応じた信号を出力するジャイロスコープ（以下、ジャイロと称する）と、車両の走行速度に応じたパルス信号を出力する速度センサと、ＧＰＳ（Global Positioning System）衛星からの送信電波を受信し、車両の位置、方位（進行方向）、速度などを出力するＧＰＳ受信機と、ジャイロ、速度センサ、ＧＰＳ受信機からの出力に基づいて車両の現在位置や進行方向などのデータ（推測航法を行うためのデータ）を検出する現在位置検出部と、を備えている。
【０００３】
そして、現在位置検出部は、速度センサからのパルス信号に基づいて車両の移動距離を算出する移動距離演算部と、ジャイロからの検出信号に基づいて車両の方位変化量を算出する方位変化量演算部と、算出された方位変化量と移動距離とに基づいて車両の相対軌跡および車速を算出する相対軌跡演算部と、同じく方位変化量と移動距離とに基づいて車両の絶対方位および絶対位置を算出する絶対位置演算部と、相対軌跡演算部および絶対位置演算部での算出値とＧＰＳ受信機での検出値との差を観測値として算出し、車速、方位、位置の算出に使用する各種算出パラメータや算出結果の誤差を状態量として、その状態量の推定値を求めるカルマンフィルタからなる誤差推定部と、誤差推定部により算出された状態量（即ち誤差）の推定値に従って、各演算部での算出パラメータや算出値を補正する補正部とを備えている。誤差推定部は、状態量の一つとして、ジャイロ出力から回転角速度への換算比率を示す換算ゲインの誤差（ゲイン誤差）を推定する。そして、補正部は、この誤差推定部により求められるゲイン誤差の推定値により、ジャイロ出力および換算ゲインを用いて算出された方位変化量を補正する。
ところで、この種の装置においては、車両が直進走行を継続した場合（車両が旋回しない場合）には車両に回転角速度が発生しないことから、換算ゲインの誤差を適切に推定することができず、その結果、換算ゲインが必要以上に補正されて精度が低下してしまう場合がある。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開２０００−５５６７８号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
そこで、本発明の目的は、角速度センサの出力値を車両の回転角速度に換算する比率を示す換算ゲインを精度良く補正することができる車両用現在位置検出装置を提供することにある。
【課題を解決するための手段】
【０００６】
請求項１の車両用現在位置検出装置によれば、角速度センサは、車両の回転角速度に応じた信号を出力する。速度センサは、車両の走行速度に応じた信号を出力する。方位変化量算出手段は、前記角速度センサの出力値を前記回転角速度に換算する比率を示す換算ゲインを算出パラメータとして用い、前記角速度センサの出力値に基づいて車両の方位変化量を算出する。移動距離算出手段は、前記速度センサの出力値に基づいて車両の移動距離を算出する。現在位置算出手段は、前記方位変化量算出手段によって算出される方位変化量および前記移動距離算出手段によって算出される移動距離に基づいて車両の現在位置や進行方向を算出する。ＧＰＳ受信機は、ＧＰＳ衛星からの電波を受信し、車両の現在位置および進行方向を出力する。誤差推定手段は、前記ＧＰＳ受信機からの出力値と前記現在位置算出手段での算出値との差を観測値として算出し、その観測値に基づいて前記換算ゲインの誤差を状態量として推定する。補正手段は、前記誤差推定手段によって推定される前記状態量に基づいて前記換算ゲインを補正する。そして、前記換算ゲインの誤差共分散値が所定値よりも大きい場合には、当該換算ゲインの誤差共分散値は前記所定値に置き換えられる。
これにより、換算ゲインが必要以上に補正されてしまうことがなくなり、当該換算ゲインの補正を精度良く行うことができる。
【０００７】
請求項２の車両用現在位置検出装置によれば、誤差発生要因判定手段は、前記換算ゲインの誤差の発生要因を判定する。そして、前記所定値は、前記誤差発生要因判定手段によって判定された前記発生要因に基づいて設定される。
これにより、換算ゲインの誤差の発生要因に基づいて当該換算ゲインの補正量の度合いが誤差発生要因別に設定されるので、換算ゲインの補正を一層精度良く行うことができる。
【図面の簡単な説明】
【０００８】
【図１】本発明の一実施形態に係るものであり、車両用現在位置検出装置の構成を示すブロック図
【図２】カルマンフィルタの概要を示す図
【図３】推測航法処理の内容を示すフローチャート
【図４】方位変化量・移動距離の演算処理の内容を示すフローチャート
【図５】相対軌跡の演算処理の内容を示すフローチャート
【図６】絶対方位・絶対位置の演算処理の内容を示すフローチャート
【図７】ＧＰＳとの複合化処理の内容を示すフローチャート
【図８】学習度の更新処理の内容を示すフローチャート
【図９】システム雑音と誤差共分散との関係を示す図
【図１０】（１２）式に相当する行列式を示す図
【図１１】（１３）式に相当する行列式を示す図
【図１２】観測過程で発生する雑音を定義する行列式を示す図
【図１３】誤差共分散を定義する行列式を示す図
【図１４】ジャイロスコープの出力値と回転角速度との関係および誤差の補正手順を示す説明図
【発明を実施するための形態】
【０００９】
以下、本発明の一実施形態について図面を参照しながら説明する。図１に示すように、ナビゲーション装置１０（車両用現在位置検出装置に相当）は、車両の走行速度に応じた間隔でパルス信号を出力する車速センサ１１（速度センサに相当）と、車両に加わる回転運動の角速度に応じた検出信号を出力するジャイロスコープ１２（角速度センサに相当、以下、単にジャイロと称する）と、ＧＰＳ（Global Positioning System）用の人工衛星から送信される電波（衛星電波）をＧＰＳアンテナ（図示せず）を介して受信し、車両の絶対的な位置，方位（進行方向），速度などを出力するＧＰＳ受信機１３と、車速センサ１１，ジャイロ１２，ＧＰＳ受信機１３からの出力に基づいて車両の現在位置や進行方向などのデータ（推測航法を行うためのデータ）を検出する現在位置検出部１４（現在位置算出手段に相当）と、現在位置検出部１４での検出結果に基づき、表示画面（図示せず）の地図上に自車両の位置を表示したり、設定された目的地までの経路案内などを行うナビゲーション実行部１５とを備えている。なお、現在位置検出部１４およびナビゲーション実行部１５は、周知のマイクロコンピュータを中心に構成された電子制御装置（ＥＣＵ）の処理として実現されるものである。
【００１０】
現在位置検出部１４は、車速センサ１１からのパルス信号に基づいて車両の移動距離を算出する移動距離演算部２１（移動距離算出手段に相当）と、ジャイロ１２からの検出信号に基づいて車両の方位変化量を算出する方位変化量演算部２２（方位変化量算出手段に相当）と、算出された方位変化量と移動距離とに基づいて車両の相対軌跡および車速を算出する相対軌跡演算部２３と、同じく方位変化量と移動距離とに基づいて車両の絶対方位および絶対位置を算出する絶対位置演算部２４と、相対軌跡演算部２３および絶対位置演算部２４での算出値とＧＰＳ受信機１３での検出値との差を観測値として算出し、車速，方位，位置の算出に使用する各種算出パラメータや算出結果の誤差を状態量として、その状態量の推定値を求めるカルマンフィルタからなる誤差推定部２５（誤差推定手段に相当）と、誤差推定部２５により算出された状態量（即ち誤差）の推定値に従って、各演算部２１〜２４での算出パラメータや算出値を補正する補正部２６（補正手段に相当）とを備えている。
【００１１】
ここで、誤差推定部２５を構成するカルマンフィルタの概要について説明する。図２に示すように、カルマンフィルタは、信号生成過程および観測過程からなる。即ち、カルマンフィルタは、プロセス行列φで定義される線形システム（信号生成過程）の状態Ｘ（ｔ）に対して、観測行列Ｈで関係付けられた状態Ｘ（ｔ）の一部が観測可能（観測過程）である場合に、この観測値Ｙ（ｔ）を入力として、状態Ｘ（ｔ）の最適な推定値を与える。なお、ωは信号生成過程にて発生する雑音であり、νは観測過程にて発生する雑音である。
【００１２】
そして、時刻ｔまでの情報を用いた状態Ｘの最適推定値、即ち、状態量Ｘ（ｔ｜ｔ）は、次の（１）〜（５）式により求めることができる。
[式（１）]
Ｘ（ｔ｜ｔ）＝Ｘ（ｔ｜ｔ−１）＋Ｋ（ｔ）[Ｙ（ｔ）−ＨＸ（ｔ｜ｔ−１）]
[式（２）]
Ｘ（ｔ｜ｔ−１）＝φＸ（ｔ−１｜ｔ−１）
[式（３）]
Ｋ（ｔ）＝Ｐ（ｔ｜ｔ−１）ＨT[ＨＰ（ｔ｜ｔ−１）ＨT＋Ｖ]-1
[式（４）]
Ｐ（ｔ｜ｔ−１）＝φＰ（ｔ−１｜ｔ−１）φT＋Ｗ
[式（５）]
Ｐ（ｔ−１｜ｔ−１）＝[Ｉ−Ｋ（ｔ−１）Ｈ]Ｐ（ｔ−１｜ｔ−２）
なお、Ｋはカルマンゲインであり、Ｐは状態量Ｘの共分散（以下、誤差共分散と称する）であり、Ｖは観測過程で発生する雑音νの分散であり、Ｗは信号生成過程で発生する雑音ωの分散である。また、Ａ（i｜j）は時刻ｊまでの情報に基づく時刻ｉでのＡの推定値である。なお、雑音ω，νは、いずれも平均０の白色ガウス雑音であり、互いに無相関である。また、Ｉは単位行列を意味し、添字の「Ｔ」は転置行列を意味し、添字の「−１」は逆行列を意味する。また、Ｗは本システムにおけるシステム雑音を意味する。
【００１３】
このように定義されたカルマンフィルタでは、状態量Ｘを適当な初期状態に定義すると共に誤差共分散Ｐに適当な初期値を与え、その後、観測値Ｙに基づいて上述の計算を実行することにより、状態量の最適推定値Ｘ（ｔ｜ｔ）が得られる。そして、観測が行われて観測値Ｙが得られるごとに計算を繰り返すことにより、状態量の最適推定値Ｘ（ｔ｜ｔ）の精度が向上する。
【００１４】
そして、本実施形態では、状態量Ｘとして、オフセット誤差（εＧ）、ゲイン誤差（εＡ）、距離係数誤差（εＫ）、絶対方位誤差（εθ）、絶対位置北方向誤差（εＹ）、絶対位置東方向誤差（εＸ）の６つの誤差値を用いる。
オフセット誤差（εＧ）は、ジャイロ１２の出力値の大きさによらず、出力値の全領域にて均等に付加される誤差である。ゲイン誤差（εＡ）は、ジャイロ１２の出力値を回転角速度に換算（変換）する比率として設定される換算ゲインの誤差である。距離係数誤差（εＫ）は、走行距離を算出する際に、車速センサ１１から出力された単位時間当たりのパルス数に乗じられる距離係数の誤差である。絶対方位誤差（εθ）は、絶対方位を算出する際に上述のオフセット誤差（εＧ）、ゲイン誤差（εＡ）、距離係数誤差（εＫ）などに基づいて付与される誤差である。絶対位置北方向誤差（εＹ）および絶対位置東方向誤差（εＸ）は、絶対位置を算出する際に同様に付与される誤差である。これら誤差値εＧ，εＡ，εＫ，εθ，εＹ，εＸは、次の（６）〜（１１）式により定義される。
[式（６）]
εＧt＝εＧt-1＋ω0
[式（７）]
εＡt＝εＡt-1＋ω1
[式（８）]
εＫt＝εＫt-1＋ω2
[式（９）]
εθt＝ＴT・εＧt-1＋ＤT・εＡt-1＋εθt-1＋ω3
[式（１０）]
εＹt＝ｓｉｎ（θ）・ＬT・（１＋εＫt-1）−ｓｉｎ（θT）・ＬT＋εＹt-1
[式（１１）]
εＸt＝ｃｏｓ（θ）・ＬT・（１＋εＫt-1）−ｃｏｓ（θT）・ＬT＋εＸt-1
【００１５】
なお、θは次の式により定義される。
θ＝θT＋εθt-1＋ＴT・εＧt-1／２＋ＤT・εＡt-1／２
また、ＴTは前回（前回の観測）からの経過時間であり、ＤTは前回からの方位変化量であり、ＬTは前回からの移動距離であり、θTは真の絶対方位である。
また、ω0は温度ドリフトなどによるオフセットの変動分（オフセット雑音と称する）であり、ω1は温度ドリフトなどによるジャイロゲインの変動分（ゲイン雑音と称する）であり、ω2は車速センサ１１の経年変化による変動分（距離係数雑音と称する）であり、ω3はジャイロ１２のクロスカップリングなどによる変動分（絶対方位雑音と称する）である。
【００１６】
なお、（６）〜（８）式に示すように、オフセット誤差（εＧ）、ゲイン誤差（εＡ）、距離係数誤差（εＫ）については、確定的な変化はなく、前回の誤差にそれぞれノイズω0〜ω2が付加されたものとなっている。また、（９）式に示すように、絶対方位誤差（εθ）については、前回の誤差に、オフセット誤差（εＧ）に基づく方位誤差と、ゲイン誤差（εＡ）に基づく方位誤差と、ノイズω3が付加されたものとなっている。さらに、（１０）式，（１１）式に示すように、絶対位置誤差（εＹ），（εＸ）については、前回の誤差に、オフセット誤差（εＧ）および絶対方位誤差（εθ）に基づく方位誤差や距離係数誤差（εＫ）に基づく距離誤差によって生じる誤差が付加されたものとなっている。また、θは絶対位置演算部２４にて算出される絶対方位を意味しており、真の絶対方位θTにセンサ誤差に基づく誤差が加わったものとなっている。
【００１７】
そして、これら誤差値εＧ，εＡ，εＫ，εθ，εＹ，εＸの時間的な変化を規定するものがプロセス行列φであり、上記（６）〜（１１）式を各誤差値にて偏微分し線形化することにより求められる。その結果、信号生成過程は、次に示す（１２）式、即ち、図１０に示す行列式により定義される。
[式（１２）]
Ｘ（ｔ＋１）＝φ・Ｘ（ｔ）＋Ｗ
【００１８】
また、観測値Ｙ（ｔ）は、相対軌跡演算部２３および絶対位置演算部２４での算出値（以下、推測航法データと称する）と、ＧＰＳ受信機１３での検出値（以下、ＧＰＳ測位データと称する）との差より求められる。それぞれの出力には誤差が含まれるため、観測値Ｙ（ｔ）において、推測航法データの誤差とＧＰＳ測位データの誤差との和が得られる。この観測値Ｙ（ｔ）と状態量Ｘ（ｔ）とを関係付けることにより、観測過程は、次に示す（１３）式、即ち、図１１に示す行列式により定義される。
[式（１３）]
Ｙ（ｔ）＝Ｈ・Ｘ（ｔ）＋Ｖ
なお、観測過程で発生する雑音νは、ＧＰＳ受信機１３からの検出値に含まれる雑音であり、図１２に示す行列式により定義される。
【００１９】
このようなカルマンフィルタを有する現在位置検出部１４では、移動距離演算部２１が車速センサ１１からの信号に基づいて車両の移動距離を算出し、方位変化量演算部２２がジャイロ１２からの信号に基づいて車両の方位変化量を算出する。すると、これら算出された移動距離および方位変化量に基づいて、相対軌跡演算部２３および絶対位置演算部２４が、推測航法データとして、車速，相対軌跡，絶対位置，絶対方位を算出する。一方、ＧＰＳ受信機１３は、ＧＰＳ測位データとして、位置，方位，車速を随時出力している。
そして、誤差推定部２５（カルマンフィルタ）は、推測航法データおよびＧＰＳ測位データに基づいて、各誤差εＧ，εＡ，εＫ，εθ，εＹ，εＸの推定値を算出する。補正部２６は、この推定値に基づいて、各演算部１１〜１４での算出パラメータや算出値の補正を行う。
【００２０】
ところで、このように推定誤差に基づいて、車速センサ１１の距離係数補正、ジャイロ１２のオフセット補正，ゲイン補正、絶対方位補正、絶対位置補正を行った場合、（２）式に示す状態量の事前推定値Ｘ（ｔ｜ｔ−１）は０となる。つまり、（１）式は次の（１ａ）式に示すようなものとなり、上述のように状態量Ｘとして定義された６つの誤差値は、この（１ａ）式および上述の（３）〜（５）式によって算出されることになる。
[式（１ａ）]
Ｘ（ｔ｜ｔ）＝Ｘ（ｔ｜ｔ−１）＋Ｋ（t）Ｙ（t）
【００２１】
ここで、（３）〜（５）式にて用いられている誤差共分散Ｐは、図１３に示すように定義される。
なお、図１３に示すσGG2，σAA2，σKK2，σθθ2，σYY2，σXX2は、それぞれ各誤差εＧ，εＡ，εＫ，εθ，εＹ，εＸの大きさの見積もりを表すものである。また、これら以外のσij2はｉ行とｊ列との相互相関値であり、例えばσAG2であれば、ゲイン誤差（εＡ）とオフセット誤差（εＧ）との相互相関値を表すものである。
この誤差共分散Ｐは上述の（４）式により更新される。但し、誤差共分散の初期値Ｐ（０｜０）では、対角要素σij2（ｉ＝ｊ）をそれぞれ誤差が最大となる値に設定し、それ以外の要素σij2（ｉ≠ｊ）を全て０に設定しておく。また、（３）式中の行列Ｈは（１３）式にて定義されたものを使用する。同じく（３）式中の行列Ｖは図１２に示す行列式にて定義されたものを使用する。さらに（４）式中の行列Ｗは（１２）式にて定義されたものを使用する。
【００２２】
次に、観測過程における観測値Ｙ（ｔ）としては、図１１に示すように、εＫDRt−εＫGPSt、εθDRt−εθGPSt、εＹDRt−εＹGPSt、εＸDRt−εＸGPStを用いる。ここで、添字の「ＤＲｔ」は時刻ｔにおける推測航法データに基づく値であることを意味し、添字の「ＧＰＳｔ」は時刻ｔにおけるＧＰＳ測位データに基づく値であることを意味する。
εＫDRt−εＫGPStは、推測航法データの速度とＧＰＳ測位データの速度との差から求まる距離係数誤差であり、具体的には、｛（推測航法データの速度）−（ＧＰＳ測位データの速度）｝／（推測航法データの速度）により求められる。
εθDRt−εθGPStは、推測航法データの絶対方位とＧＰＳ測位データの方位との差を取ることにより得られる。即ち、推測航法データの絶対方位には真の絶対方位θTとその誤差εθDRtが含まれており、また、ＧＰＳ測位データの方位には真の絶対方位θTとその誤差εθGPStが含まれている。そのため、それらの差を取ることによりεθDRt−εθGPStが得られる。
εＹDRt−εＹGPStは、推測航法データの絶対位置のＹ成分とＧＰＳ測位データの位置のＹ成分の誤差の差である。
εＸDRt−εＸGPStは、推測航法データの絶対位置のＸ成分とＧＰＳ測位データのＸ成分との誤差の差である。
【００２３】
さらに、図１２に示す行列式により定義される観測過程で発生する雑音νはＧＰＳ受信機１３での雑音であり、以下のようにして求められる。即ち、ＧＰＳ受信機１３における擬似距離の計測誤差（ＵＥＲＥ）とＨＤＯＰ（Horizontal Dilution of Presision）の関係により、測位精度は、ＵＥＲＥ×ＨＤＯＰにより求められる。そして、この測位精度を２乗することにより、ν2t、ν3tが求められる。また、ドップラー周波数の計測誤差とＨＤＯＰの関係より、速度精度は、ドップラー周波数の計測誤差×ＨＤＯＰにより求められる。そして、この速度精度／車速により距離係数計測誤差が求められ、これを２乗することによりν1tが求められる。さらに、車両の速度Ｖｃと速度精度から方位精度が求められる。即ち、方位精度は、ｔａｎ-1（速度精度／Ｖｃ）により求められる。そして、この方位精度を２乗することによりν0tが求められる。
【００２４】
従って、観測過程におけるεＫDRt−εＫGPSt、εθDRt−εθGPSt、εＹDRt−εＹGPSt、εＸDRt−εＸGPStおよび上記雑音Ｖを入力として、上述の（３）〜（５）式および（１）式を実行することにより、信号生成過程にて定義された６つの誤差値εＧ，εＡ，εＫ，εθ，εＹ，εＸからなる状態量Ｘが求められる。これらにより、車速センサ１１の距離係数補正、ジャイロ１２のオフセット補正，ゲイン補正、さらには、絶対位置補正、絶対方位補正が行われる。
【００２５】
上述したように、現在位置検出部１４は、マイクロコンピュータの処理として実現されるものである。以下、この現在位置検出部１４が実行する推測航法処理を、図３〜図８に示すフローチャートに沿って説明する。なお、本処理は、一定周期ＴＭで繰り返し実行される。
図３に示すように、まず、ステップＡ１では、現在位置検出部１４は、移動距離演算部２１および方位変化量演算部２２により方位変化量・移動距離の演算処理を実行する。この演算処理の詳細を図４に沿って説明する。即ち、ステップＢ１では、ジャイロ１２により検出されたジャイロ出力角速度にメインルーチンの起動周期ＴＭを乗じることにより方位変化量を算出する。なお、ジャイロ出力角速度は、図１４（ａ）のグラフから明らかなように、ジャイロ１２の出力をサンプリングした値からオフセット値（この場合、２．５Ｖ）を減じたものに、換算ゲイン（グラフの傾きに相当）を乗じることにより算出されたものである。
次に、ステップＢ２では、後述するオフセット補正量にメインルーチンの起動周期ＴＭを乗じたものを、ステップＢ１にて求めた方位変化量から減じることにより、方位変化量のオフセット補正を行う。即ち、図１４（ｂ）に示すように、角速度０［ｄｅｇ／ｓｅｃ］に対応するジャイロ１２の出力電圧（ゼロ点）の変動に基づく誤差を補正する。
次に、ステップＢ３では、ステップＢ２にてオフセット補正された方位変化量に、後述するゲイン補正量を乗じることにより、方位変化量のゲイン補正を行う。即ち、図１４（ｃ）に示すように、グラフの傾き（換算ゲイン）の変動に基づく誤差を補正する。
次に、ステップＢ４では、本処理が前回起動されてから今回起動されるまでの間に検出された車速センサ１１からの出力パルス数に、後述する距離係数を乗じることにより移動距離を算出する。以上により、方位変化量・移動距離の演算処理を終了する。
【００２６】
方位変化量・移動距離の演算処理を終了すると、現在位置検出部１４は、図３のステップＡ２に移行し、相対軌跡演算部２３により相対軌跡の演算処理を実行する。
この演算処理の詳細を図５に沿って説明する。即ち、ステップＣ１では、先のステップＢ３にて算出された方位変化量を、それまでに求められている相対方位に加算することにより、相対方位を更新する。次に、ステップＣ２では、この更新された相対方位、および、先のステップＢ４にて算出された移動距離に基づき、相対位置座標の更新を行う。具体的には、南北方向をｘ座標軸とした相対座標ｒｅｌ．ｘを次に示す（１６）式に従って更新し、東西方向をｙ座標軸とした相対座標ｒｅｌ．ｙを次に示す（１７）式に従って更新する。但し、θはステップＣ１にて算出された相対方位である。
[式（１６）]
ｒｅｌ．ｘ←ｒｅｌ．ｘ＋移動距離×ｃｏｓθ
[式（１７）]
ｒｅｌ．ｙ←ｒｅｌ．ｙ＋移動距離×ｓｉｎθ
【００２７】
即ち、この更新は、移動距離に対する相対方位のｘ，ｙ成分を、それまでの相対位置座標に加算することにより行っている。この相対位置座標は相対軌跡を求めるために行うもので、その相対軌跡と道路形状との関係により、いわゆるマップマッチングが行われる。
次に、ステップＣ３では、ステップＢ４にて算出された移動距離を、メインルーチンの起動周期ＴＭで除することにより車速を算出する。以上により、相対軌跡の演算処理を終了する。
【００２８】
相対軌跡の演算処理を終了すると、現在位置検出部１４は、図３のステップＡ３に移行し、絶対位置演算部２４により絶対方位・絶対位置の演算処理を実行する。
この演算処理の詳細を図６に沿って説明する。即ち、ステップＤ１では、先のステップＢ３にて算出された方位変化量を、それまでに求められている絶対方位に加算することにより、絶対方位を更新する。次に、ステップＤ２では、この更新された絶対方位、および先のステップＢ４にて算出された移動距離に基づき、絶対位置座標の更新を行う。具体的には、南北方向をｘ座標軸とした絶対座標ａｂｓ．ｘを次に示す（１８）式に従って更新し、東西方向をｙ座標軸とした絶対座標ａｂｓ．ｙを次に示す（１９）式に従って更新する。但し、θはステップＤ１にて算出された絶対方位である。
[式（１８）]
ａｂｓ．ｘ←ａｂｓ．ｘ＋移動距離×ｃｏｓθ
[式（１９）]
ａｂｓ．ｙ←ａｂｓ．ｙ＋移動距離×ｓｉｎθ
【００２９】
即ち、この更新は、移動距離に対する絶対方位のｘ，ｙ成分を、それまでの絶対位置座標に加算することにより行っている。このようにして更新された絶対方位と絶対位置は後述するＧＰＳとの複合化処理にて利用される。
【００３０】
絶対方位・絶対位置の演算処理を終了すると、現在位置検出部１４は、図３のステップＡ４に移行し、誤差推定部２５および補正部２６によりＧＰＳとの複合化処理を実行する。この複合化処理の詳細を図７に沿って説明する。まず、ステップＥ１では、現在位置検出部１４は、後述するＧＰＳ測位データに基づくカルマンフィルタの処理（ステップＥ３〜Ｅ１２）または予測計算処理（ステップＥ１３〜Ｅ１７）が前回実行されてからＴ１秒経過したか否かを判定する。現在位置検出部１４は、否定判定した場合（ＮＯ）にはステップＥ２に移行する。
ステップＥ２では、現在位置検出部１４は、ＧＰＳ受信機１３が測位を行うことによりＧＰＳ受信機１３からＧＰＳ測位データが入力されたか否かを判断する。現在位置検出部１４は、肯定判定した場合（ＹＥＳ）には、ステップＥ３に移行して、誤差推定部２５（カルマンフィルタ）による処理を開始する。一方、現在位置検出部１４は、否定判定した場合（ＮＯ）には、そのまま本処理を終了する。
【００３１】
ステップＥ３では、現在位置検出部１４は、ＧＰＳ受信機１３からのＧＰＳ測位データ（速度，位置，方位）、および、先の推測航法処理により算出された推測航法データ（ステップＣ３により算出した車速，ステップＤ２により更新した絶対位置，ステップＤ１により更新した絶対方位）に基づいて観測値Ｙ（εＫDRt−εＫGPSt、εθDRt−εθGPSt、εＹDRt−εＹGPSt、εＸDRt−εＸGPSt）の計算を行う。これと共に、観測過程で発生する雑音νをＧＰＳ受信機１３のＧＰＳ測位データなどを基に計算する。
次に、現在位置検出部１４は、ステップＥ４に移行し、プロセス行列φの計算を行う。このステップＥ４では、前回のプロセス行列の計算時点からの移動距離Ｌ、経過時間Ｔ（これらは図示しない計測処理により別途求められている）、および、ステップＤ１にて求めた絶対方位θにより、上記した（１２）式に示すプロセス行列φを求める。
【００３２】
このようにして計算した観測値Ｙおよびプロセス行列φに基づき、以下では、上記した（３）〜（５）式の計算を行って（１ａ）式に示す状態量Ｘを求める。まず、現在位置検出部１４は、ステップＥ５に移行して、学習度の更新処理を実行する。この更新処理の詳細を図８に沿って説明する。即ち、ステップＦ１では、現在位置検出部１４は、ジャイロ１２によって検出される回転角速度が閾値Ｗ１以上であるか否かを判断する。現在位置検出部１４は、肯定判断をした場合（ＹＥＳ）には、ステップＦ２に移行して、累積旋回回数を更新する。この累積旋回回数は、車両にジャイロ１２が取り付けられた後（例えば、車両の出荷後）において当該車両が旋回した回数（ジャイロ１２によって検出される回転角速度が閾値Ｗ１以上となった回数）の累積値を記憶したものである。そして、現在位置検出部１４は、累積旋回回数を更新すると、ステップＦ３に移行する。なお、現在位置検出部１４は、ステップＦ１において否定判断した場合（ＮＯ）には、累積旋回回数を更新することなくステップＦ３に移行する。
【００３３】
ステップＦ３では、現在位置検出部１４は、累積旋回回数が閾値Ｎ１以上であるか否かを判断する（誤差発生要因判定手段に相当）。現在位置検出部１４は、否定判断をした場合（ＮＯ）には、ステップＦ４に移行して、システム雑音ＷをＱｓ１に設定するとともに、誤差共分散Ｐの上限値ＰｍａｘをＰｓ１に設定する。
一方、現在位置検出部１４は、ステップＦ３において肯定判断した場合（ＹＥＳ）には、ステップＦ５に移行して、ナビゲーション装置１０が起動してからＴ２時間経過したか否かを判断する（誤差発生要因判定手段）。現在位置検出部１４は、否定判断した場合（ＮＯ）には、ステップＦ６に移行して、システム雑音ＷをＱｓ２に設定するとともに、誤差共分散Ｐの上限値ＰｍａｘをＰｓ２に設定する。
一方、現在位置検出部１４は、ステップＦ５において肯定判断した場合（ＹＥＳ）には、ステップＦ７に移行して、システム雑音ＷをＱｓ３に設定するとともに、誤差共分散Ｐの上限値ＰｍａｘをＰｓ３に設定する。
【００３４】
ここで、図９に示すように、上記のシステム雑音Ｗとして設定されるＱｓ１，Ｑｓ２，Ｑｓ３は、Ｑｓ１＞Ｑｓ２＞Ｑｓ３の関係を満たすように設定される。また、上記の誤差共分散Ｐの上限値Ｐｍａｘとして設定されるＰｓ１，Ｐｓ２，Ｐｓ３は、Ｐｓ１＞Ｐｓ２＞Ｐｓ３の関係を満たすように設定される。なお、システム雑音Ｗの値が大きいほど、誤差共分散Ｐの増加は急峻となる。
【００３５】
車両にジャイロ１２が取り付けられてから間もない場合、即ち、車両の累積旋回回数が少ない場合（ステップＦ３にてＮＯ）には、換算ゲインの誤差の発生要因としては、ジャイロ１２の取り付け角度の誤差（ジャイロ１２の検出軸が傾いた状態で設置されることに基づく誤差）、或いは、ジャイロ１２の製造ばらつきの誤差（ジャイロ１２が製造される過程で各製品に固有に発生する誤差）が主な要因となる。このような要因により発生する誤差（オフセット性の誤差）は、変動するものではなく、補正をしなければ一定の誤差が発生し続けるが、補正をすれば除去することができ誤差が発生し続けることはない。そのため、このような誤差は早急に補正しておくことが望ましい。そこで、現在位置検出部１４は、車両の累積旋回回数が少ない場合（ステップＦ３にてＮＯ）には、システム雑音Ｗを大きく設定し（システム雑音Ｗ＝Ｑｓ１）、また、誤差共分散Ｐの上限値Ｐｍａｘを大きく設定する（誤差共分散Ｐの上限値Ｐｍａｘ＝Ｐｓ１）。これにより、換算ゲインの誤差の補正量が大きくなり、換算ゲインを迅速に補正することができる。
【００３６】
一方、車両にジャイロ１２が取り付けられてからある程度の時間が経過し、換算ゲインの補正もある程度繰り返された場合（ステップＦ３にてＹＥＳ）には、換算ゲインの誤差の発生要因としては、温度変化に伴う誤差（ジャイロ１２自体の温度変化に起因する誤差）が主な要因となる。このような要因により発生する誤差は、温度に応じて変動するものであり、補正をしたとしても、その後の温度変化により誤差が再び生じ得る。そのため、このような誤差は実際の状況（温度状況）に応じて随時補正することが望ましい。そこで、現在位置検出部１４は、換算ゲインの補正がある程度繰り返された後においては、システム雑音Ｗを小さく設定し（システム雑音Ｗ＝Ｑｓ２，Ｑｓ３）、また、誤差共分散Ｐの上限値Ｐｍａｘを小さく設定する（誤差共分散Ｐの上限値Ｐｍａｘ＝Ｐｓ２，Ｐｓ３）。これにより、換算ゲインの誤差の補正量は小さくなるが、換算ゲインを実際の温度状況に応じてきめ細かく補正することができる。
【００３７】
なお、ジャイロ１２の温度は、ナビゲーション装置１０が起動した後に徐々に上昇し始め、ある温度でほぼ一定に収束する傾向にある。そのため、起動からある程度の時間（この場合、Ｔ２時間）が経過していない場合（ステップＦ５にてＮＯ）には、現在位置検出部１４は、ジャイロ１２の温度が上昇中（温度変化中）であると判断し、システム雑音ＷをＱｓ３よりも大きめに設定し（システム雑音Ｗ＝Ｑｓ２）、また、誤差共分散Ｐの上限値ＰｍａｘをＰｓ３よりも大きめに設定する（誤差共分散Ｐの上限値Ｐｍａｘ＝Ｐｓ２）。これにより、換算ゲインの誤差の補正量が比較的大きくなり、換算ゲインを温度変化に追従させながら補正することができる。
【００３８】
一方、ナビゲーション装置１０の起動からある程度の時間（Ｔ２時間）が経過した場合（ステップＦ５にてＹＥＳ）には、現在位置検出部１４は、ジャイロ１２の温度がほぼ一定に収束したと判断し、システム雑音ＷをＱｓ２よりも小さく設定し（システム雑音Ｗ＝Ｑｓ３）、また、誤差共分散Ｐの上限値ＰｍａｘをＰｓ２よりも小さく設定する（誤差共分散Ｐの上限値＝Ｐｓ３）。これにより、換算ゲインの誤差の補正量が比較的小さくなり、収束した一定温度に応じて換算ゲインを精度良く補正することができる。また、ジャイロ１２の温度がほぼ一定に収束し補正量を大きくする必要がない場合に、必要以上に補正量が大きくなってしまうことがない。
【００３９】
以上のように換算ゲインの誤差の発生要因に基づいてシステム雑音Ｗおよび誤差共分散Ｐの上限値Ｐｍａｘの値を設定すると、現在位置検出部１４は、学習度の更新処理を終了する。そして、図７のステップＥ６に移行して、（４）式により誤差共分散Ｐの予測計算を行う。そして、現在位置検出部１４は、ステップＥ７に移行し、算出した誤差共分散Ｐの値が、上記のステップＦ４，Ｆ６，Ｆ７の何れかにて設定した誤差共分散Ｐの上限値Ｐｍａｘの値よりも大きいか否かを判断する。現在位置検出部１４は、肯定判断した場合（ＹＥＳ）には、ステップＥ８に移行して、誤差共分散Ｐの値を上限値Ｐｍａｘに置き換える。そして、ステップＥ９に移行する。一方、現在位置検出部１４は、ステップＥ７において否定判断した場合（ＮＯ）には、誤差共分差Ｐの値を上限値Ｐｍａｘに置き換えることなく、ステップＥ９に移行する。
【００４０】
ステップＥ９では、現在位置検出部１４は、（３）式によりカルマンゲインＫの計算を行う。続くステップＥ１０では、現在位置検出部１４は、（５）式により誤差共分散Ｐの計算を行う。続くステップＥ１１では、現在位置検出部１４は、算出したカルマンゲインＫおよび観測値Ｙに基づき、（１ａ）式により状態量Ｘを求める。この状態量Ｘは、（１２）式の左辺に示すオフセット誤差（εＧ）、ゲイン誤差（εＡ），絶対方位誤差（εθ）、距離係数誤差（εＫ）、絶対位置北方向誤差（εＹ）、絶対位置東方向誤差（εＸ）を表している。
【００４１】
そして、ステップＥ１２では、状態量Ｘとして算出されたこれらの誤差値εＧ，εＡ，εＫ，εθ，εＹ，εＸに基づき、推測航法誤差の補正（ジャイロ１２のオフセット補正、同じくゲイン補正、車速センサ１１の距離係数補正、絶対方位補正、絶対位置補正）を、次に示す（２０）〜（２５）式に従って行う。
[式（２０）]
オフセット補正量＝オフセット補正量−εＧ
[式（２１）]
ゲイン補正量＝ゲイン補正量×（１−εＡ）
[式（２２）]
距離係数＝距離係数×（１−εＫ）
[式（２３）]
絶対方位＝絶対方位−εθ
[式（２４）]
ａｂｓ．ｙ（絶対位置）＝ａｂｓ．ｙ−εＹ
[式（２５）]
ａｂｓ．ｘ（絶対位置）＝ａｂｓ．ｘ−εＸ
【００４２】
即ち、ジャイロ１２のオフセット補正およびゲイン補正により、ステップＢ２，Ｂ３にて用いられるオフセット補正量およびゲイン補正量が修正される。車速センサ１１の距離係数補正により、ステップＢ４にて用いられる距離係数が修正される。絶対方位補正により、ステップＤ１にて用いられる絶対方位θが修正される。絶対位置補正により、ステップＤ２にて用いられる絶対位置が修正される。
そして、上記ステップＥ３〜Ｅ１２の処理が、ＧＰＳ受信機１３からのＧＰＳ測位データが有るごとに繰り返されることにより、上記誤差が随時修正される。これにより、一層正確な推測航法データが得られることになる。
【００４３】
なお、ＧＰＳ受信機１３からのＧＰＳ測位データが長時間（Ｔ１秒以上）に渡って得ることができず、先のステップＥ１にて肯定判定された場合（ＹＥＳ）には、現在位置検出部１４は、プロセス行列φの計算（ステップＥ１３）、学習度の更新処理（ステップＥ１４）、誤差共分散Ｐの予測計算（ステップＥ１５）を行い、算出した誤差共分散Ｐの値が設定した誤差共分散Ｐの上限値Ｐｍａｘの値よりも大きい場合には誤差共分散Ｐの値を上限値Ｐｍａｘに置き換える（ステップＥ１６，Ｅ１７）。なお、ステップＥ１４における処理は、上述のステップＥ５における学習度の更新処理（図８参照）と同様の内容である。
ＧＰＳ受信機１３からＧＰＳ測位データが得られない場合、何も補正を行わないと誤差が大きくなってしまう。そのため、上記のステップＥ１３〜Ｅ１７の処理を実行することにより、誤差共分散Ｐの予測計算、即ち、誤差の見積もりと、システム雑音Ｗおよび誤差共分散Ｐの上限値Ｐｍａｘの設定については、定期的に行っておく。これにより、その後にＧＰＳ受信機１３からのＧＰＳ測位データの入力が再開された時に行われるカルマンフィルタの処理を正確に行うことができる。
【００４４】
以上に説明したように本実施形態によれば、誤差共分散Ｐに上限値Ｐｍａｘを設定し、算出された誤差共分散Ｐの値が設定した上限値Ｐｍａｘよりも大きい場合には、誤差共分差Ｐの値を上限値Ｐｍａｘに置き換えるように構成した。これにより、ジャイロ１２の出力値を車両の回転角速度に換算する比率を示す換算ゲインが必要以上に補正されてしまうことがなくなり、当該換算ゲインの補正を精度良く行うことができる。
また、換算ゲインの誤差の発生要因に基づいて誤差共分散Ｐの上限値Ｐｍａｘ、換言すれば、換算ゲインの補正量の度合い（上限値）を誤差発生要因別に異なる値に設定するように構成した。これにより、換算ゲインの補正量を誤差の発生要因に応じた量とすることができ、換算ゲインの補正を一層精度良く行うことができる。
【００４５】
なお、本発明は、上述した一実施形態にのみ限定されるものではなく、例えば次のように変形または拡張することができる。
システム雑音Ｗとして設定されるＱｓ１，Ｑｓ２，Ｑｓ３の具体的な値、および、誤差共分散Ｐの上限値Ｐｍａｘとして設定されるＰｓ１，Ｐｓ２，Ｐｓ３の具体的な値は、例えばナビゲーション装置１０やジャイロ１２の性能などに応じて適宜変更して設定することができる。また、推測航法処理における時間Ｔ１，Ｔ２、閾値Ｗ１，Ｎ１などの値は、適宜変更して設定することができる。
例えばジャイロ１２を取り付け直した場合、つまり、ジャイロ１２の取り付け角度が変化してしまうような場合には、記憶している累積旋回回数をリセットすることで、図８に示すステップＦ３，Ｆ４の処理、即ち、オフセット性の誤差を補正する処理を再び実行するようにするとよい。
【符号の説明】
【００４６】
図面中、１０はナビゲーション装置（車両用現在位置検出装置）、１１は車速センサ（速度センサ）、１２はジャイロスコープ（角速度センサ）、１３はＧＰＳ受信機、１４は現在位置検出部（現在位置算出手段、誤差発生要因判定手段）、２１は移動距離演算部（移動距離算出手段）、２２は方位変化量演算部（方位変化量算出手段）、２５は誤差推定部（誤差推定手段）、２６は補正部（補正手段）を示す。
【技術分野】
【０００１】
本発明は、車両の進行方向および走行距離に基づいて車両の現在位置を検出する車両用現在位置検出装置に関する。
【背景技術】
【０００２】
ジャイロスコープなどの角速度センサ（方位センサ）の出力情報を補正する技術に関して、例えば特許文献１には、以下のような車両用現在位置検出装置に関する技術が開示されている。この車両用現在位置検出装置は、車両の回転角速度に応じた信号を出力するジャイロスコープ（以下、ジャイロと称する）と、車両の走行速度に応じたパルス信号を出力する速度センサと、ＧＰＳ（Global Positioning System）衛星からの送信電波を受信し、車両の位置、方位（進行方向）、速度などを出力するＧＰＳ受信機と、ジャイロ、速度センサ、ＧＰＳ受信機からの出力に基づいて車両の現在位置や進行方向などのデータ（推測航法を行うためのデータ）を検出する現在位置検出部と、を備えている。
【０００３】
そして、現在位置検出部は、速度センサからのパルス信号に基づいて車両の移動距離を算出する移動距離演算部と、ジャイロからの検出信号に基づいて車両の方位変化量を算出する方位変化量演算部と、算出された方位変化量と移動距離とに基づいて車両の相対軌跡および車速を算出する相対軌跡演算部と、同じく方位変化量と移動距離とに基づいて車両の絶対方位および絶対位置を算出する絶対位置演算部と、相対軌跡演算部および絶対位置演算部での算出値とＧＰＳ受信機での検出値との差を観測値として算出し、車速、方位、位置の算出に使用する各種算出パラメータや算出結果の誤差を状態量として、その状態量の推定値を求めるカルマンフィルタからなる誤差推定部と、誤差推定部により算出された状態量（即ち誤差）の推定値に従って、各演算部での算出パラメータや算出値を補正する補正部とを備えている。誤差推定部は、状態量の一つとして、ジャイロ出力から回転角速度への換算比率を示す換算ゲインの誤差（ゲイン誤差）を推定する。そして、補正部は、この誤差推定部により求められるゲイン誤差の推定値により、ジャイロ出力および換算ゲインを用いて算出された方位変化量を補正する。
ところで、この種の装置においては、車両が直進走行を継続した場合（車両が旋回しない場合）には車両に回転角速度が発生しないことから、換算ゲインの誤差を適切に推定することができず、その結果、換算ゲインが必要以上に補正されて精度が低下してしまう場合がある。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開２０００−５５６７８号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
そこで、本発明の目的は、角速度センサの出力値を車両の回転角速度に換算する比率を示す換算ゲインを精度良く補正することができる車両用現在位置検出装置を提供することにある。
【課題を解決するための手段】
【０００６】
請求項１の車両用現在位置検出装置によれば、角速度センサは、車両の回転角速度に応じた信号を出力する。速度センサは、車両の走行速度に応じた信号を出力する。方位変化量算出手段は、前記角速度センサの出力値を前記回転角速度に換算する比率を示す換算ゲインを算出パラメータとして用い、前記角速度センサの出力値に基づいて車両の方位変化量を算出する。移動距離算出手段は、前記速度センサの出力値に基づいて車両の移動距離を算出する。現在位置算出手段は、前記方位変化量算出手段によって算出される方位変化量および前記移動距離算出手段によって算出される移動距離に基づいて車両の現在位置や進行方向を算出する。ＧＰＳ受信機は、ＧＰＳ衛星からの電波を受信し、車両の現在位置および進行方向を出力する。誤差推定手段は、前記ＧＰＳ受信機からの出力値と前記現在位置算出手段での算出値との差を観測値として算出し、その観測値に基づいて前記換算ゲインの誤差を状態量として推定する。補正手段は、前記誤差推定手段によって推定される前記状態量に基づいて前記換算ゲインを補正する。そして、前記換算ゲインの誤差共分散値が所定値よりも大きい場合には、当該換算ゲインの誤差共分散値は前記所定値に置き換えられる。
これにより、換算ゲインが必要以上に補正されてしまうことがなくなり、当該換算ゲインの補正を精度良く行うことができる。
【０００７】
請求項２の車両用現在位置検出装置によれば、誤差発生要因判定手段は、前記換算ゲインの誤差の発生要因を判定する。そして、前記所定値は、前記誤差発生要因判定手段によって判定された前記発生要因に基づいて設定される。
これにより、換算ゲインの誤差の発生要因に基づいて当該換算ゲインの補正量の度合いが誤差発生要因別に設定されるので、換算ゲインの補正を一層精度良く行うことができる。
【図面の簡単な説明】
【０００８】
【図１】本発明の一実施形態に係るものであり、車両用現在位置検出装置の構成を示すブロック図
【図２】カルマンフィルタの概要を示す図
【図３】推測航法処理の内容を示すフローチャート
【図４】方位変化量・移動距離の演算処理の内容を示すフローチャート
【図５】相対軌跡の演算処理の内容を示すフローチャート
【図６】絶対方位・絶対位置の演算処理の内容を示すフローチャート
【図７】ＧＰＳとの複合化処理の内容を示すフローチャート
【図８】学習度の更新処理の内容を示すフローチャート
【図９】システム雑音と誤差共分散との関係を示す図
【図１０】（１２）式に相当する行列式を示す図
【図１１】（１３）式に相当する行列式を示す図
【図１２】観測過程で発生する雑音を定義する行列式を示す図
【図１３】誤差共分散を定義する行列式を示す図
【図１４】ジャイロスコープの出力値と回転角速度との関係および誤差の補正手順を示す説明図
【発明を実施するための形態】
【０００９】
以下、本発明の一実施形態について図面を参照しながら説明する。図１に示すように、ナビゲーション装置１０（車両用現在位置検出装置に相当）は、車両の走行速度に応じた間隔でパルス信号を出力する車速センサ１１（速度センサに相当）と、車両に加わる回転運動の角速度に応じた検出信号を出力するジャイロスコープ１２（角速度センサに相当、以下、単にジャイロと称する）と、ＧＰＳ（Global Positioning System）用の人工衛星から送信される電波（衛星電波）をＧＰＳアンテナ（図示せず）を介して受信し、車両の絶対的な位置，方位（進行方向），速度などを出力するＧＰＳ受信機１３と、車速センサ１１，ジャイロ１２，ＧＰＳ受信機１３からの出力に基づいて車両の現在位置や進行方向などのデータ（推測航法を行うためのデータ）を検出する現在位置検出部１４（現在位置算出手段に相当）と、現在位置検出部１４での検出結果に基づき、表示画面（図示せず）の地図上に自車両の位置を表示したり、設定された目的地までの経路案内などを行うナビゲーション実行部１５とを備えている。なお、現在位置検出部１４およびナビゲーション実行部１５は、周知のマイクロコンピュータを中心に構成された電子制御装置（ＥＣＵ）の処理として実現されるものである。
【００１０】
現在位置検出部１４は、車速センサ１１からのパルス信号に基づいて車両の移動距離を算出する移動距離演算部２１（移動距離算出手段に相当）と、ジャイロ１２からの検出信号に基づいて車両の方位変化量を算出する方位変化量演算部２２（方位変化量算出手段に相当）と、算出された方位変化量と移動距離とに基づいて車両の相対軌跡および車速を算出する相対軌跡演算部２３と、同じく方位変化量と移動距離とに基づいて車両の絶対方位および絶対位置を算出する絶対位置演算部２４と、相対軌跡演算部２３および絶対位置演算部２４での算出値とＧＰＳ受信機１３での検出値との差を観測値として算出し、車速，方位，位置の算出に使用する各種算出パラメータや算出結果の誤差を状態量として、その状態量の推定値を求めるカルマンフィルタからなる誤差推定部２５（誤差推定手段に相当）と、誤差推定部２５により算出された状態量（即ち誤差）の推定値に従って、各演算部２１〜２４での算出パラメータや算出値を補正する補正部２６（補正手段に相当）とを備えている。
【００１１】
ここで、誤差推定部２５を構成するカルマンフィルタの概要について説明する。図２に示すように、カルマンフィルタは、信号生成過程および観測過程からなる。即ち、カルマンフィルタは、プロセス行列φで定義される線形システム（信号生成過程）の状態Ｘ（ｔ）に対して、観測行列Ｈで関係付けられた状態Ｘ（ｔ）の一部が観測可能（観測過程）である場合に、この観測値Ｙ（ｔ）を入力として、状態Ｘ（ｔ）の最適な推定値を与える。なお、ωは信号生成過程にて発生する雑音であり、νは観測過程にて発生する雑音である。
【００１２】
そして、時刻ｔまでの情報を用いた状態Ｘの最適推定値、即ち、状態量Ｘ（ｔ｜ｔ）は、次の（１）〜（５）式により求めることができる。
[式（１）]
Ｘ（ｔ｜ｔ）＝Ｘ（ｔ｜ｔ−１）＋Ｋ（ｔ）[Ｙ（ｔ）−ＨＸ（ｔ｜ｔ−１）]
[式（２）]
Ｘ（ｔ｜ｔ−１）＝φＸ（ｔ−１｜ｔ−１）
[式（３）]
Ｋ（ｔ）＝Ｐ（ｔ｜ｔ−１）ＨT[ＨＰ（ｔ｜ｔ−１）ＨT＋Ｖ]-1
[式（４）]
Ｐ（ｔ｜ｔ−１）＝φＰ（ｔ−１｜ｔ−１）φT＋Ｗ
[式（５）]
Ｐ（ｔ−１｜ｔ−１）＝[Ｉ−Ｋ（ｔ−１）Ｈ]Ｐ（ｔ−１｜ｔ−２）
なお、Ｋはカルマンゲインであり、Ｐは状態量Ｘの共分散（以下、誤差共分散と称する）であり、Ｖは観測過程で発生する雑音νの分散であり、Ｗは信号生成過程で発生する雑音ωの分散である。また、Ａ（i｜j）は時刻ｊまでの情報に基づく時刻ｉでのＡの推定値である。なお、雑音ω，νは、いずれも平均０の白色ガウス雑音であり、互いに無相関である。また、Ｉは単位行列を意味し、添字の「Ｔ」は転置行列を意味し、添字の「−１」は逆行列を意味する。また、Ｗは本システムにおけるシステム雑音を意味する。
【００１３】
このように定義されたカルマンフィルタでは、状態量Ｘを適当な初期状態に定義すると共に誤差共分散Ｐに適当な初期値を与え、その後、観測値Ｙに基づいて上述の計算を実行することにより、状態量の最適推定値Ｘ（ｔ｜ｔ）が得られる。そして、観測が行われて観測値Ｙが得られるごとに計算を繰り返すことにより、状態量の最適推定値Ｘ（ｔ｜ｔ）の精度が向上する。
【００１４】
そして、本実施形態では、状態量Ｘとして、オフセット誤差（εＧ）、ゲイン誤差（εＡ）、距離係数誤差（εＫ）、絶対方位誤差（εθ）、絶対位置北方向誤差（εＹ）、絶対位置東方向誤差（εＸ）の６つの誤差値を用いる。
オフセット誤差（εＧ）は、ジャイロ１２の出力値の大きさによらず、出力値の全領域にて均等に付加される誤差である。ゲイン誤差（εＡ）は、ジャイロ１２の出力値を回転角速度に換算（変換）する比率として設定される換算ゲインの誤差である。距離係数誤差（εＫ）は、走行距離を算出する際に、車速センサ１１から出力された単位時間当たりのパルス数に乗じられる距離係数の誤差である。絶対方位誤差（εθ）は、絶対方位を算出する際に上述のオフセット誤差（εＧ）、ゲイン誤差（εＡ）、距離係数誤差（εＫ）などに基づいて付与される誤差である。絶対位置北方向誤差（εＹ）および絶対位置東方向誤差（εＸ）は、絶対位置を算出する際に同様に付与される誤差である。これら誤差値εＧ，εＡ，εＫ，εθ，εＹ，εＸは、次の（６）〜（１１）式により定義される。
[式（６）]
εＧt＝εＧt-1＋ω0
[式（７）]
εＡt＝εＡt-1＋ω1
[式（８）]
εＫt＝εＫt-1＋ω2
[式（９）]
εθt＝ＴT・εＧt-1＋ＤT・εＡt-1＋εθt-1＋ω3
[式（１０）]
εＹt＝ｓｉｎ（θ）・ＬT・（１＋εＫt-1）−ｓｉｎ（θT）・ＬT＋εＹt-1
[式（１１）]
εＸt＝ｃｏｓ（θ）・ＬT・（１＋εＫt-1）−ｃｏｓ（θT）・ＬT＋εＸt-1
【００１５】
なお、θは次の式により定義される。
θ＝θT＋εθt-1＋ＴT・εＧt-1／２＋ＤT・εＡt-1／２
また、ＴTは前回（前回の観測）からの経過時間であり、ＤTは前回からの方位変化量であり、ＬTは前回からの移動距離であり、θTは真の絶対方位である。
また、ω0は温度ドリフトなどによるオフセットの変動分（オフセット雑音と称する）であり、ω1は温度ドリフトなどによるジャイロゲインの変動分（ゲイン雑音と称する）であり、ω2は車速センサ１１の経年変化による変動分（距離係数雑音と称する）であり、ω3はジャイロ１２のクロスカップリングなどによる変動分（絶対方位雑音と称する）である。
【００１６】
なお、（６）〜（８）式に示すように、オフセット誤差（εＧ）、ゲイン誤差（εＡ）、距離係数誤差（εＫ）については、確定的な変化はなく、前回の誤差にそれぞれノイズω0〜ω2が付加されたものとなっている。また、（９）式に示すように、絶対方位誤差（εθ）については、前回の誤差に、オフセット誤差（εＧ）に基づく方位誤差と、ゲイン誤差（εＡ）に基づく方位誤差と、ノイズω3が付加されたものとなっている。さらに、（１０）式，（１１）式に示すように、絶対位置誤差（εＹ），（εＸ）については、前回の誤差に、オフセット誤差（εＧ）および絶対方位誤差（εθ）に基づく方位誤差や距離係数誤差（εＫ）に基づく距離誤差によって生じる誤差が付加されたものとなっている。また、θは絶対位置演算部２４にて算出される絶対方位を意味しており、真の絶対方位θTにセンサ誤差に基づく誤差が加わったものとなっている。
【００１７】
そして、これら誤差値εＧ，εＡ，εＫ，εθ，εＹ，εＸの時間的な変化を規定するものがプロセス行列φであり、上記（６）〜（１１）式を各誤差値にて偏微分し線形化することにより求められる。その結果、信号生成過程は、次に示す（１２）式、即ち、図１０に示す行列式により定義される。
[式（１２）]
Ｘ（ｔ＋１）＝φ・Ｘ（ｔ）＋Ｗ
【００１８】
また、観測値Ｙ（ｔ）は、相対軌跡演算部２３および絶対位置演算部２４での算出値（以下、推測航法データと称する）と、ＧＰＳ受信機１３での検出値（以下、ＧＰＳ測位データと称する）との差より求められる。それぞれの出力には誤差が含まれるため、観測値Ｙ（ｔ）において、推測航法データの誤差とＧＰＳ測位データの誤差との和が得られる。この観測値Ｙ（ｔ）と状態量Ｘ（ｔ）とを関係付けることにより、観測過程は、次に示す（１３）式、即ち、図１１に示す行列式により定義される。
[式（１３）]
Ｙ（ｔ）＝Ｈ・Ｘ（ｔ）＋Ｖ
なお、観測過程で発生する雑音νは、ＧＰＳ受信機１３からの検出値に含まれる雑音であり、図１２に示す行列式により定義される。
【００１９】
このようなカルマンフィルタを有する現在位置検出部１４では、移動距離演算部２１が車速センサ１１からの信号に基づいて車両の移動距離を算出し、方位変化量演算部２２がジャイロ１２からの信号に基づいて車両の方位変化量を算出する。すると、これら算出された移動距離および方位変化量に基づいて、相対軌跡演算部２３および絶対位置演算部２４が、推測航法データとして、車速，相対軌跡，絶対位置，絶対方位を算出する。一方、ＧＰＳ受信機１３は、ＧＰＳ測位データとして、位置，方位，車速を随時出力している。
そして、誤差推定部２５（カルマンフィルタ）は、推測航法データおよびＧＰＳ測位データに基づいて、各誤差εＧ，εＡ，εＫ，εθ，εＹ，εＸの推定値を算出する。補正部２６は、この推定値に基づいて、各演算部１１〜１４での算出パラメータや算出値の補正を行う。
【００２０】
ところで、このように推定誤差に基づいて、車速センサ１１の距離係数補正、ジャイロ１２のオフセット補正，ゲイン補正、絶対方位補正、絶対位置補正を行った場合、（２）式に示す状態量の事前推定値Ｘ（ｔ｜ｔ−１）は０となる。つまり、（１）式は次の（１ａ）式に示すようなものとなり、上述のように状態量Ｘとして定義された６つの誤差値は、この（１ａ）式および上述の（３）〜（５）式によって算出されることになる。
[式（１ａ）]
Ｘ（ｔ｜ｔ）＝Ｘ（ｔ｜ｔ−１）＋Ｋ（t）Ｙ（t）
【００２１】
ここで、（３）〜（５）式にて用いられている誤差共分散Ｐは、図１３に示すように定義される。
なお、図１３に示すσGG2，σAA2，σKK2，σθθ2，σYY2，σXX2は、それぞれ各誤差εＧ，εＡ，εＫ，εθ，εＹ，εＸの大きさの見積もりを表すものである。また、これら以外のσij2はｉ行とｊ列との相互相関値であり、例えばσAG2であれば、ゲイン誤差（εＡ）とオフセット誤差（εＧ）との相互相関値を表すものである。
この誤差共分散Ｐは上述の（４）式により更新される。但し、誤差共分散の初期値Ｐ（０｜０）では、対角要素σij2（ｉ＝ｊ）をそれぞれ誤差が最大となる値に設定し、それ以外の要素σij2（ｉ≠ｊ）を全て０に設定しておく。また、（３）式中の行列Ｈは（１３）式にて定義されたものを使用する。同じく（３）式中の行列Ｖは図１２に示す行列式にて定義されたものを使用する。さらに（４）式中の行列Ｗは（１２）式にて定義されたものを使用する。
【００２２】
次に、観測過程における観測値Ｙ（ｔ）としては、図１１に示すように、εＫDRt−εＫGPSt、εθDRt−εθGPSt、εＹDRt−εＹGPSt、εＸDRt−εＸGPStを用いる。ここで、添字の「ＤＲｔ」は時刻ｔにおける推測航法データに基づく値であることを意味し、添字の「ＧＰＳｔ」は時刻ｔにおけるＧＰＳ測位データに基づく値であることを意味する。
εＫDRt−εＫGPStは、推測航法データの速度とＧＰＳ測位データの速度との差から求まる距離係数誤差であり、具体的には、｛（推測航法データの速度）−（ＧＰＳ測位データの速度）｝／（推測航法データの速度）により求められる。
εθDRt−εθGPStは、推測航法データの絶対方位とＧＰＳ測位データの方位との差を取ることにより得られる。即ち、推測航法データの絶対方位には真の絶対方位θTとその誤差εθDRtが含まれており、また、ＧＰＳ測位データの方位には真の絶対方位θTとその誤差εθGPStが含まれている。そのため、それらの差を取ることによりεθDRt−εθGPStが得られる。
εＹDRt−εＹGPStは、推測航法データの絶対位置のＹ成分とＧＰＳ測位データの位置のＹ成分の誤差の差である。
εＸDRt−εＸGPStは、推測航法データの絶対位置のＸ成分とＧＰＳ測位データのＸ成分との誤差の差である。
【００２３】
さらに、図１２に示す行列式により定義される観測過程で発生する雑音νはＧＰＳ受信機１３での雑音であり、以下のようにして求められる。即ち、ＧＰＳ受信機１３における擬似距離の計測誤差（ＵＥＲＥ）とＨＤＯＰ（Horizontal Dilution of Presision）の関係により、測位精度は、ＵＥＲＥ×ＨＤＯＰにより求められる。そして、この測位精度を２乗することにより、ν2t、ν3tが求められる。また、ドップラー周波数の計測誤差とＨＤＯＰの関係より、速度精度は、ドップラー周波数の計測誤差×ＨＤＯＰにより求められる。そして、この速度精度／車速により距離係数計測誤差が求められ、これを２乗することによりν1tが求められる。さらに、車両の速度Ｖｃと速度精度から方位精度が求められる。即ち、方位精度は、ｔａｎ-1（速度精度／Ｖｃ）により求められる。そして、この方位精度を２乗することによりν0tが求められる。
【００２４】
従って、観測過程におけるεＫDRt−εＫGPSt、εθDRt−εθGPSt、εＹDRt−εＹGPSt、εＸDRt−εＸGPStおよび上記雑音Ｖを入力として、上述の（３）〜（５）式および（１）式を実行することにより、信号生成過程にて定義された６つの誤差値εＧ，εＡ，εＫ，εθ，εＹ，εＸからなる状態量Ｘが求められる。これらにより、車速センサ１１の距離係数補正、ジャイロ１２のオフセット補正，ゲイン補正、さらには、絶対位置補正、絶対方位補正が行われる。
【００２５】
上述したように、現在位置検出部１４は、マイクロコンピュータの処理として実現されるものである。以下、この現在位置検出部１４が実行する推測航法処理を、図３〜図８に示すフローチャートに沿って説明する。なお、本処理は、一定周期ＴＭで繰り返し実行される。
図３に示すように、まず、ステップＡ１では、現在位置検出部１４は、移動距離演算部２１および方位変化量演算部２２により方位変化量・移動距離の演算処理を実行する。この演算処理の詳細を図４に沿って説明する。即ち、ステップＢ１では、ジャイロ１２により検出されたジャイロ出力角速度にメインルーチンの起動周期ＴＭを乗じることにより方位変化量を算出する。なお、ジャイロ出力角速度は、図１４（ａ）のグラフから明らかなように、ジャイロ１２の出力をサンプリングした値からオフセット値（この場合、２．５Ｖ）を減じたものに、換算ゲイン（グラフの傾きに相当）を乗じることにより算出されたものである。
次に、ステップＢ２では、後述するオフセット補正量にメインルーチンの起動周期ＴＭを乗じたものを、ステップＢ１にて求めた方位変化量から減じることにより、方位変化量のオフセット補正を行う。即ち、図１４（ｂ）に示すように、角速度０［ｄｅｇ／ｓｅｃ］に対応するジャイロ１２の出力電圧（ゼロ点）の変動に基づく誤差を補正する。
次に、ステップＢ３では、ステップＢ２にてオフセット補正された方位変化量に、後述するゲイン補正量を乗じることにより、方位変化量のゲイン補正を行う。即ち、図１４（ｃ）に示すように、グラフの傾き（換算ゲイン）の変動に基づく誤差を補正する。
次に、ステップＢ４では、本処理が前回起動されてから今回起動されるまでの間に検出された車速センサ１１からの出力パルス数に、後述する距離係数を乗じることにより移動距離を算出する。以上により、方位変化量・移動距離の演算処理を終了する。
【００２６】
方位変化量・移動距離の演算処理を終了すると、現在位置検出部１４は、図３のステップＡ２に移行し、相対軌跡演算部２３により相対軌跡の演算処理を実行する。
この演算処理の詳細を図５に沿って説明する。即ち、ステップＣ１では、先のステップＢ３にて算出された方位変化量を、それまでに求められている相対方位に加算することにより、相対方位を更新する。次に、ステップＣ２では、この更新された相対方位、および、先のステップＢ４にて算出された移動距離に基づき、相対位置座標の更新を行う。具体的には、南北方向をｘ座標軸とした相対座標ｒｅｌ．ｘを次に示す（１６）式に従って更新し、東西方向をｙ座標軸とした相対座標ｒｅｌ．ｙを次に示す（１７）式に従って更新する。但し、θはステップＣ１にて算出された相対方位である。
[式（１６）]
ｒｅｌ．ｘ←ｒｅｌ．ｘ＋移動距離×ｃｏｓθ
[式（１７）]
ｒｅｌ．ｙ←ｒｅｌ．ｙ＋移動距離×ｓｉｎθ
【００２７】
即ち、この更新は、移動距離に対する相対方位のｘ，ｙ成分を、それまでの相対位置座標に加算することにより行っている。この相対位置座標は相対軌跡を求めるために行うもので、その相対軌跡と道路形状との関係により、いわゆるマップマッチングが行われる。
次に、ステップＣ３では、ステップＢ４にて算出された移動距離を、メインルーチンの起動周期ＴＭで除することにより車速を算出する。以上により、相対軌跡の演算処理を終了する。
【００２８】
相対軌跡の演算処理を終了すると、現在位置検出部１４は、図３のステップＡ３に移行し、絶対位置演算部２４により絶対方位・絶対位置の演算処理を実行する。
この演算処理の詳細を図６に沿って説明する。即ち、ステップＤ１では、先のステップＢ３にて算出された方位変化量を、それまでに求められている絶対方位に加算することにより、絶対方位を更新する。次に、ステップＤ２では、この更新された絶対方位、および先のステップＢ４にて算出された移動距離に基づき、絶対位置座標の更新を行う。具体的には、南北方向をｘ座標軸とした絶対座標ａｂｓ．ｘを次に示す（１８）式に従って更新し、東西方向をｙ座標軸とした絶対座標ａｂｓ．ｙを次に示す（１９）式に従って更新する。但し、θはステップＤ１にて算出された絶対方位である。
[式（１８）]
ａｂｓ．ｘ←ａｂｓ．ｘ＋移動距離×ｃｏｓθ
[式（１９）]
ａｂｓ．ｙ←ａｂｓ．ｙ＋移動距離×ｓｉｎθ
【００２９】
即ち、この更新は、移動距離に対する絶対方位のｘ，ｙ成分を、それまでの絶対位置座標に加算することにより行っている。このようにして更新された絶対方位と絶対位置は後述するＧＰＳとの複合化処理にて利用される。
【００３０】
絶対方位・絶対位置の演算処理を終了すると、現在位置検出部１４は、図３のステップＡ４に移行し、誤差推定部２５および補正部２６によりＧＰＳとの複合化処理を実行する。この複合化処理の詳細を図７に沿って説明する。まず、ステップＥ１では、現在位置検出部１４は、後述するＧＰＳ測位データに基づくカルマンフィルタの処理（ステップＥ３〜Ｅ１２）または予測計算処理（ステップＥ１３〜Ｅ１７）が前回実行されてからＴ１秒経過したか否かを判定する。現在位置検出部１４は、否定判定した場合（ＮＯ）にはステップＥ２に移行する。
ステップＥ２では、現在位置検出部１４は、ＧＰＳ受信機１３が測位を行うことによりＧＰＳ受信機１３からＧＰＳ測位データが入力されたか否かを判断する。現在位置検出部１４は、肯定判定した場合（ＹＥＳ）には、ステップＥ３に移行して、誤差推定部２５（カルマンフィルタ）による処理を開始する。一方、現在位置検出部１４は、否定判定した場合（ＮＯ）には、そのまま本処理を終了する。
【００３１】
ステップＥ３では、現在位置検出部１４は、ＧＰＳ受信機１３からのＧＰＳ測位データ（速度，位置，方位）、および、先の推測航法処理により算出された推測航法データ（ステップＣ３により算出した車速，ステップＤ２により更新した絶対位置，ステップＤ１により更新した絶対方位）に基づいて観測値Ｙ（εＫDRt−εＫGPSt、εθDRt−εθGPSt、εＹDRt−εＹGPSt、εＸDRt−εＸGPSt）の計算を行う。これと共に、観測過程で発生する雑音νをＧＰＳ受信機１３のＧＰＳ測位データなどを基に計算する。
次に、現在位置検出部１４は、ステップＥ４に移行し、プロセス行列φの計算を行う。このステップＥ４では、前回のプロセス行列の計算時点からの移動距離Ｌ、経過時間Ｔ（これらは図示しない計測処理により別途求められている）、および、ステップＤ１にて求めた絶対方位θにより、上記した（１２）式に示すプロセス行列φを求める。
【００３２】
このようにして計算した観測値Ｙおよびプロセス行列φに基づき、以下では、上記した（３）〜（５）式の計算を行って（１ａ）式に示す状態量Ｘを求める。まず、現在位置検出部１４は、ステップＥ５に移行して、学習度の更新処理を実行する。この更新処理の詳細を図８に沿って説明する。即ち、ステップＦ１では、現在位置検出部１４は、ジャイロ１２によって検出される回転角速度が閾値Ｗ１以上であるか否かを判断する。現在位置検出部１４は、肯定判断をした場合（ＹＥＳ）には、ステップＦ２に移行して、累積旋回回数を更新する。この累積旋回回数は、車両にジャイロ１２が取り付けられた後（例えば、車両の出荷後）において当該車両が旋回した回数（ジャイロ１２によって検出される回転角速度が閾値Ｗ１以上となった回数）の累積値を記憶したものである。そして、現在位置検出部１４は、累積旋回回数を更新すると、ステップＦ３に移行する。なお、現在位置検出部１４は、ステップＦ１において否定判断した場合（ＮＯ）には、累積旋回回数を更新することなくステップＦ３に移行する。
【００３３】
ステップＦ３では、現在位置検出部１４は、累積旋回回数が閾値Ｎ１以上であるか否かを判断する（誤差発生要因判定手段に相当）。現在位置検出部１４は、否定判断をした場合（ＮＯ）には、ステップＦ４に移行して、システム雑音ＷをＱｓ１に設定するとともに、誤差共分散Ｐの上限値ＰｍａｘをＰｓ１に設定する。
一方、現在位置検出部１４は、ステップＦ３において肯定判断した場合（ＹＥＳ）には、ステップＦ５に移行して、ナビゲーション装置１０が起動してからＴ２時間経過したか否かを判断する（誤差発生要因判定手段）。現在位置検出部１４は、否定判断した場合（ＮＯ）には、ステップＦ６に移行して、システム雑音ＷをＱｓ２に設定するとともに、誤差共分散Ｐの上限値ＰｍａｘをＰｓ２に設定する。
一方、現在位置検出部１４は、ステップＦ５において肯定判断した場合（ＹＥＳ）には、ステップＦ７に移行して、システム雑音ＷをＱｓ３に設定するとともに、誤差共分散Ｐの上限値ＰｍａｘをＰｓ３に設定する。
【００３４】
ここで、図９に示すように、上記のシステム雑音Ｗとして設定されるＱｓ１，Ｑｓ２，Ｑｓ３は、Ｑｓ１＞Ｑｓ２＞Ｑｓ３の関係を満たすように設定される。また、上記の誤差共分散Ｐの上限値Ｐｍａｘとして設定されるＰｓ１，Ｐｓ２，Ｐｓ３は、Ｐｓ１＞Ｐｓ２＞Ｐｓ３の関係を満たすように設定される。なお、システム雑音Ｗの値が大きいほど、誤差共分散Ｐの増加は急峻となる。
【００３５】
車両にジャイロ１２が取り付けられてから間もない場合、即ち、車両の累積旋回回数が少ない場合（ステップＦ３にてＮＯ）には、換算ゲインの誤差の発生要因としては、ジャイロ１２の取り付け角度の誤差（ジャイロ１２の検出軸が傾いた状態で設置されることに基づく誤差）、或いは、ジャイロ１２の製造ばらつきの誤差（ジャイロ１２が製造される過程で各製品に固有に発生する誤差）が主な要因となる。このような要因により発生する誤差（オフセット性の誤差）は、変動するものではなく、補正をしなければ一定の誤差が発生し続けるが、補正をすれば除去することができ誤差が発生し続けることはない。そのため、このような誤差は早急に補正しておくことが望ましい。そこで、現在位置検出部１４は、車両の累積旋回回数が少ない場合（ステップＦ３にてＮＯ）には、システム雑音Ｗを大きく設定し（システム雑音Ｗ＝Ｑｓ１）、また、誤差共分散Ｐの上限値Ｐｍａｘを大きく設定する（誤差共分散Ｐの上限値Ｐｍａｘ＝Ｐｓ１）。これにより、換算ゲインの誤差の補正量が大きくなり、換算ゲインを迅速に補正することができる。
【００３６】
一方、車両にジャイロ１２が取り付けられてからある程度の時間が経過し、換算ゲインの補正もある程度繰り返された場合（ステップＦ３にてＹＥＳ）には、換算ゲインの誤差の発生要因としては、温度変化に伴う誤差（ジャイロ１２自体の温度変化に起因する誤差）が主な要因となる。このような要因により発生する誤差は、温度に応じて変動するものであり、補正をしたとしても、その後の温度変化により誤差が再び生じ得る。そのため、このような誤差は実際の状況（温度状況）に応じて随時補正することが望ましい。そこで、現在位置検出部１４は、換算ゲインの補正がある程度繰り返された後においては、システム雑音Ｗを小さく設定し（システム雑音Ｗ＝Ｑｓ２，Ｑｓ３）、また、誤差共分散Ｐの上限値Ｐｍａｘを小さく設定する（誤差共分散Ｐの上限値Ｐｍａｘ＝Ｐｓ２，Ｐｓ３）。これにより、換算ゲインの誤差の補正量は小さくなるが、換算ゲインを実際の温度状況に応じてきめ細かく補正することができる。
【００３７】
なお、ジャイロ１２の温度は、ナビゲーション装置１０が起動した後に徐々に上昇し始め、ある温度でほぼ一定に収束する傾向にある。そのため、起動からある程度の時間（この場合、Ｔ２時間）が経過していない場合（ステップＦ５にてＮＯ）には、現在位置検出部１４は、ジャイロ１２の温度が上昇中（温度変化中）であると判断し、システム雑音ＷをＱｓ３よりも大きめに設定し（システム雑音Ｗ＝Ｑｓ２）、また、誤差共分散Ｐの上限値ＰｍａｘをＰｓ３よりも大きめに設定する（誤差共分散Ｐの上限値Ｐｍａｘ＝Ｐｓ２）。これにより、換算ゲインの誤差の補正量が比較的大きくなり、換算ゲインを温度変化に追従させながら補正することができる。
【００３８】
一方、ナビゲーション装置１０の起動からある程度の時間（Ｔ２時間）が経過した場合（ステップＦ５にてＹＥＳ）には、現在位置検出部１４は、ジャイロ１２の温度がほぼ一定に収束したと判断し、システム雑音ＷをＱｓ２よりも小さく設定し（システム雑音Ｗ＝Ｑｓ３）、また、誤差共分散Ｐの上限値ＰｍａｘをＰｓ２よりも小さく設定する（誤差共分散Ｐの上限値＝Ｐｓ３）。これにより、換算ゲインの誤差の補正量が比較的小さくなり、収束した一定温度に応じて換算ゲインを精度良く補正することができる。また、ジャイロ１２の温度がほぼ一定に収束し補正量を大きくする必要がない場合に、必要以上に補正量が大きくなってしまうことがない。
【００３９】
以上のように換算ゲインの誤差の発生要因に基づいてシステム雑音Ｗおよび誤差共分散Ｐの上限値Ｐｍａｘの値を設定すると、現在位置検出部１４は、学習度の更新処理を終了する。そして、図７のステップＥ６に移行して、（４）式により誤差共分散Ｐの予測計算を行う。そして、現在位置検出部１４は、ステップＥ７に移行し、算出した誤差共分散Ｐの値が、上記のステップＦ４，Ｆ６，Ｆ７の何れかにて設定した誤差共分散Ｐの上限値Ｐｍａｘの値よりも大きいか否かを判断する。現在位置検出部１４は、肯定判断した場合（ＹＥＳ）には、ステップＥ８に移行して、誤差共分散Ｐの値を上限値Ｐｍａｘに置き換える。そして、ステップＥ９に移行する。一方、現在位置検出部１４は、ステップＥ７において否定判断した場合（ＮＯ）には、誤差共分差Ｐの値を上限値Ｐｍａｘに置き換えることなく、ステップＥ９に移行する。
【００４０】
ステップＥ９では、現在位置検出部１４は、（３）式によりカルマンゲインＫの計算を行う。続くステップＥ１０では、現在位置検出部１４は、（５）式により誤差共分散Ｐの計算を行う。続くステップＥ１１では、現在位置検出部１４は、算出したカルマンゲインＫおよび観測値Ｙに基づき、（１ａ）式により状態量Ｘを求める。この状態量Ｘは、（１２）式の左辺に示すオフセット誤差（εＧ）、ゲイン誤差（εＡ），絶対方位誤差（εθ）、距離係数誤差（εＫ）、絶対位置北方向誤差（εＹ）、絶対位置東方向誤差（εＸ）を表している。
【００４１】
そして、ステップＥ１２では、状態量Ｘとして算出されたこれらの誤差値εＧ，εＡ，εＫ，εθ，εＹ，εＸに基づき、推測航法誤差の補正（ジャイロ１２のオフセット補正、同じくゲイン補正、車速センサ１１の距離係数補正、絶対方位補正、絶対位置補正）を、次に示す（２０）〜（２５）式に従って行う。
[式（２０）]
オフセット補正量＝オフセット補正量−εＧ
[式（２１）]
ゲイン補正量＝ゲイン補正量×（１−εＡ）
[式（２２）]
距離係数＝距離係数×（１−εＫ）
[式（２３）]
絶対方位＝絶対方位−εθ
[式（２４）]
ａｂｓ．ｙ（絶対位置）＝ａｂｓ．ｙ−εＹ
[式（２５）]
ａｂｓ．ｘ（絶対位置）＝ａｂｓ．ｘ−εＸ
【００４２】
即ち、ジャイロ１２のオフセット補正およびゲイン補正により、ステップＢ２，Ｂ３にて用いられるオフセット補正量およびゲイン補正量が修正される。車速センサ１１の距離係数補正により、ステップＢ４にて用いられる距離係数が修正される。絶対方位補正により、ステップＤ１にて用いられる絶対方位θが修正される。絶対位置補正により、ステップＤ２にて用いられる絶対位置が修正される。
そして、上記ステップＥ３〜Ｅ１２の処理が、ＧＰＳ受信機１３からのＧＰＳ測位データが有るごとに繰り返されることにより、上記誤差が随時修正される。これにより、一層正確な推測航法データが得られることになる。
【００４３】
なお、ＧＰＳ受信機１３からのＧＰＳ測位データが長時間（Ｔ１秒以上）に渡って得ることができず、先のステップＥ１にて肯定判定された場合（ＹＥＳ）には、現在位置検出部１４は、プロセス行列φの計算（ステップＥ１３）、学習度の更新処理（ステップＥ１４）、誤差共分散Ｐの予測計算（ステップＥ１５）を行い、算出した誤差共分散Ｐの値が設定した誤差共分散Ｐの上限値Ｐｍａｘの値よりも大きい場合には誤差共分散Ｐの値を上限値Ｐｍａｘに置き換える（ステップＥ１６，Ｅ１７）。なお、ステップＥ１４における処理は、上述のステップＥ５における学習度の更新処理（図８参照）と同様の内容である。
ＧＰＳ受信機１３からＧＰＳ測位データが得られない場合、何も補正を行わないと誤差が大きくなってしまう。そのため、上記のステップＥ１３〜Ｅ１７の処理を実行することにより、誤差共分散Ｐの予測計算、即ち、誤差の見積もりと、システム雑音Ｗおよび誤差共分散Ｐの上限値Ｐｍａｘの設定については、定期的に行っておく。これにより、その後にＧＰＳ受信機１３からのＧＰＳ測位データの入力が再開された時に行われるカルマンフィルタの処理を正確に行うことができる。
【００４４】
以上に説明したように本実施形態によれば、誤差共分散Ｐに上限値Ｐｍａｘを設定し、算出された誤差共分散Ｐの値が設定した上限値Ｐｍａｘよりも大きい場合には、誤差共分差Ｐの値を上限値Ｐｍａｘに置き換えるように構成した。これにより、ジャイロ１２の出力値を車両の回転角速度に換算する比率を示す換算ゲインが必要以上に補正されてしまうことがなくなり、当該換算ゲインの補正を精度良く行うことができる。
また、換算ゲインの誤差の発生要因に基づいて誤差共分散Ｐの上限値Ｐｍａｘ、換言すれば、換算ゲインの補正量の度合い（上限値）を誤差発生要因別に異なる値に設定するように構成した。これにより、換算ゲインの補正量を誤差の発生要因に応じた量とすることができ、換算ゲインの補正を一層精度良く行うことができる。
【００４５】
なお、本発明は、上述した一実施形態にのみ限定されるものではなく、例えば次のように変形または拡張することができる。
システム雑音Ｗとして設定されるＱｓ１，Ｑｓ２，Ｑｓ３の具体的な値、および、誤差共分散Ｐの上限値Ｐｍａｘとして設定されるＰｓ１，Ｐｓ２，Ｐｓ３の具体的な値は、例えばナビゲーション装置１０やジャイロ１２の性能などに応じて適宜変更して設定することができる。また、推測航法処理における時間Ｔ１，Ｔ２、閾値Ｗ１，Ｎ１などの値は、適宜変更して設定することができる。
例えばジャイロ１２を取り付け直した場合、つまり、ジャイロ１２の取り付け角度が変化してしまうような場合には、記憶している累積旋回回数をリセットすることで、図８に示すステップＦ３，Ｆ４の処理、即ち、オフセット性の誤差を補正する処理を再び実行するようにするとよい。
【符号の説明】
【００４６】
図面中、１０はナビゲーション装置（車両用現在位置検出装置）、１１は車速センサ（速度センサ）、１２はジャイロスコープ（角速度センサ）、１３はＧＰＳ受信機、１４は現在位置検出部（現在位置算出手段、誤差発生要因判定手段）、２１は移動距離演算部（移動距離算出手段）、２２は方位変化量演算部（方位変化量算出手段）、２５は誤差推定部（誤差推定手段）、２６は補正部（補正手段）を示す。
【特許請求の範囲】
【請求項１】
車両の回転角速度に応じた信号を出力する角速度センサと、
車両の走行速度に応じた信号を出力する速度センサと、
前記角速度センサの出力値を前記回転角速度に換算する比率を示す換算ゲインを算出パラメータとして用い、前記角速度センサの出力値に基づいて車両の方位変化量を算出する方位変化量算出手段と、
前記速度センサの出力値に基づいて車両の移動距離を算出する移動距離算出手段と、
前記方位変化量算出手段によって算出される方位変化量および前記移動距離算出手段によって算出される移動距離に基づいて車両の現在位置や進行方向を算出する現在位置算出手段と、
ＧＰＳ衛星からの電波を受信し、車両の現在位置および進行方向を出力するＧＰＳ受信機と、
前記ＧＰＳ受信機からの出力値と前記現在位置算出手段での算出値との差を観測値として算出し、その観測値に基づいて前記換算ゲインの誤差を状態量として推定する誤差推定手段と、
前記誤差推定手段によって推定される前記状態量に基づいて前記換算ゲインを補正する補正手段と、
を備え、
前記換算ゲインの誤差共分散値が所定値よりも大きい場合には、当該換算ゲインの誤差共分散値を前記所定値に置き換えることを特徴とする車両用現在位置検出装置。
【請求項２】
前記換算ゲインの誤差の発生要因を判定する誤差発生要因判定手段を備え、
前記誤差発生要因判定手段によって判定された前記発生要因に基づいて前記所定値を設定することを特徴とする請求項１記載の車両用現在位置検出装置。
【請求項１】
車両の回転角速度に応じた信号を出力する角速度センサと、
車両の走行速度に応じた信号を出力する速度センサと、
前記角速度センサの出力値を前記回転角速度に換算する比率を示す換算ゲインを算出パラメータとして用い、前記角速度センサの出力値に基づいて車両の方位変化量を算出する方位変化量算出手段と、
前記速度センサの出力値に基づいて車両の移動距離を算出する移動距離算出手段と、
前記方位変化量算出手段によって算出される方位変化量および前記移動距離算出手段によって算出される移動距離に基づいて車両の現在位置や進行方向を算出する現在位置算出手段と、
ＧＰＳ衛星からの電波を受信し、車両の現在位置および進行方向を出力するＧＰＳ受信機と、
前記ＧＰＳ受信機からの出力値と前記現在位置算出手段での算出値との差を観測値として算出し、その観測値に基づいて前記換算ゲインの誤差を状態量として推定する誤差推定手段と、
前記誤差推定手段によって推定される前記状態量に基づいて前記換算ゲインを補正する補正手段と、
を備え、
前記換算ゲインの誤差共分散値が所定値よりも大きい場合には、当該換算ゲインの誤差共分散値を前記所定値に置き換えることを特徴とする車両用現在位置検出装置。
【請求項２】
前記換算ゲインの誤差の発生要因を判定する誤差発生要因判定手段を備え、
前記誤差発生要因判定手段によって判定された前記発生要因に基づいて前記所定値を設定することを特徴とする請求項１記載の車両用現在位置検出装置。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【公開番号】特開２０１２−５８０８７（Ｐ２０１２−５８０８７Ａ）
【公開日】平成２４年３月２２日（２０１２．３．２２）
【国際特許分類】
【出願番号】特願２０１０−２０１９４１（Ｐ２０１０−２０１９４１）
【出願日】平成２２年９月９日（２０１０．９．９）
【出願人】（０００００４２６０）株式会社デンソー (27,639)
【Ｆターム（参考）】
【公開日】平成２４年３月２２日（２０１２．３．２２）
【国際特許分類】
【出願日】平成２２年９月９日（２０１０．９．９）
【出願人】（０００００４２６０）株式会社デンソー (27,639)
【Ｆターム（参考）】
[ Back to top ]