(57)【要約】【課題】角速度に対する出力電圧特性が非線形のジャイロスコープを用いても、精度よく車両の走行軌跡を推定できるようにする。【解決手段】角速度センサ２２の出力値のゲイン誤差として、角速度センサ２２の車両への取り付け角度を要因とする取り付け角ゲイン誤差と、角速度センサ２２の角速度に対する非線形ゲイン特性を要因とする角速度ゲイン誤差とを区別して推定する。すなわち、カルマンフィルタＡにより取り付け角ゲイン誤差を推定した後、カルマンフィルタＢにより取り付け角ゲイン誤差および角速度ゲイン誤差の推定を実施する。【選択図】図８
2012002794.tif 000002
【特許請求の範囲】
【請求項１】
車両の回転角速度に応じた信号を出力する角速度センサと、
前記車両の走行速度に応じた信号を出力する速度センサと、
ＧＰＳ衛星からの電波を受信し、前記車両の絶対位置および絶対方位を特定するための信号を出力するＧＰＳ受信装置と、
前記角速度センサの出力値に基づいて算出される方位変化量と、前記速度センサの出力値に基づいて算出される移動距離から、前記車両の相対軌跡を推定するとともに当該車両の相対軌跡と前記ＧＰＳ受信装置からの出力値とに基づいて前記車両の走行軌跡を推定する走行軌跡推定手段と、
前記車両の相対軌跡および前記車両の走行軌跡と、前記ＧＰＳ受信装置の出力値との差を観測値として前記角速度センサの出力値、前記速度センサの出力値および前記ＧＰＳ受信装置の出力値の各誤差を推定するとともに推定した前記各誤差に基づいて前記角速度センサの出力値、前記速度センサの出力値および前記ＧＰＳ受信装置の出力値の各誤差を補正する誤差推定手段と、を備えた車両用現在位置検出装置であって、
前記誤差推定手段は、前記角速度センサの出力値のゲイン誤差として、前記角速度センサの前記車両への取り付け角度を要因とする取り付け角ゲイン誤差と、前記角速度センサの角速度に対する非線形ゲイン特性を要因とする角速度ゲイン誤差とを区別して推定することを特徴とする車両用現在位置検出装置。
【請求項２】
前記誤差推定手段は、前記取り付け角ゲイン誤差を用いて前記角速度センサの前記車両への取り付け角度による誤差を補正するための第１の補正量を算出するとともに前記角速度ゲイン誤差を用いて前記角速度センサの非線形ゲイン特性による誤差を補正するための第２の補正量を算出し、前記第１の補正量および第２の補正量を用いて前記ＧＰＳ受信装置の出力値より算出される絶対方位量を補正するための絶対方位誤差を推定することを特徴とする請求項１に記載の車両用現在位置検出装置。
【請求項３】
前記誤差推定手段は、前記方位変化量をＤ、前回の角速度ゲイン誤差をεＳａｔ−１とすると、角速度ゲイン誤差により発生する方位誤差をＤ２・εＳａｔ−１とモデル化し、前記第２の補正量を推定することを特徴とする請求項２に記載の車両用現在位置検出装置。
【請求項４】
前記誤差推定手段は、前記方位変化量をＤ、前回の取り付け角ゲイン誤差をεＳｂｔ−１とすると、前記取り付け角ゲイン誤差により発生する方位誤差をＤ・εＳｂｔ−１としてモデル化し、前記第１の補正量を推定することを特徴とする請求項２または３に記載の車両用現在位置検出装置。
【請求項５】
前記誤差推定手段は、第１の関数を用いて取り付け角ゲイン誤差を推定するとともに、前記第１の関数と異なる第２の関数を用いて前記角速度ゲイン誤差を推定することを特徴とする請求項１ないし４のいずれか１つに記載の車両用現在位置検出装置。
【請求項６】
前記誤差推定手段は、前記取り付け角ゲイン誤差の推定および前記第１の補正量の算出が可能な第１カルマンフィルタにより前記取り付け角ゲイン誤差の推定および前記第１の補正量の算出を実施する取り付け角ゲイン誤差推定手段と、
前記取り付け角ゲイン誤差推定手段による前記取り付け角ゲイン誤差の推定および前記第１の補正量の算出を実施した後、前記第１カルマンフィルタの誤差共分散行列における取り付け角ゲイン誤差に対応する対角成分が予め定められた閾値未満になったか否かを判定する判定手段と、
前記判定手段により前記第１カルマンフィルタの誤差共分散行列における取り付け角ゲイン誤差に対応する対角成分が予め定められた閾値未満になったと判定された後、前記角速度ゲイン誤差の推定および前記第２の補正量の算出が可能な第２カルマンフィルタにより前記角速度ゲイン誤差の推定および前記第２の補正量の算出を実施する角速度ゲイン誤差推定手段と、を備えたことを特徴とする請求項１ないし５のいずれか１つに記載の車両用現在位置検出装置。
【請求項７】
前記角速度ゲイン誤差推定手段は、前記第１カルマンフィルタにより前記取り付け角ゲイン誤差の推定および前記第１の補正量の算出を実施した際に、前記車両の相対軌跡および前記車両の走行軌跡と、前記ＧＰＳ受信装置の出力値との差を観測値として推定した前記角速度センサの出力値、前記速度センサの出力値および前記ＧＰＳ受信装置の出力値の各推定誤差および、前記第１の補正量の算出を実施した際に算出された誤差共分散値を用いて、前記第２カルマンフィルタによる前記角速度ゲイン誤差を推定することを特徴とする請求項６に記載の車両用現在位置検出装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、車両の走行軌跡を推定する車両用現在位置検出装置に関するものである。
【背景技術】
【０００２】
従来、車両の現在位置を検出するカーナビゲーション装置等の車両用位置検出装置においては、ＧＰＳに代表される人工衛星を利用したＧＰＳ受信機と、車速に応じた車速信号を出力する車速センサ、車両の回転角速度を検出するジャイロスコープ等の自立センサとを組み合わせて推測航法軌跡を生成し、推測航法軌跡と地図データを照合（マップマッチング）して地図上における自車位置を特定するようになっている。
【０００３】
一般に、こうした車両用位置検出装置では、ジャイロスコープにより検出される車両の角速度と車速センサにより検出される移動距離から車両の運動ベクトルを生成し、それを積分することにより車両の走行軌跡を推定する。しかしながら、ジャイロスコープや車速センサは誤差を有するため、走行軌跡が正しく推定できないといった問題がある。具体的な誤差要因としては、ジャイロスコープのオフセット誤差、ジャイロスコープのゲイン誤差、タイヤ径の変化による距離係数誤差などが挙げられる。
【０００４】
そこで、このような誤差を補正するため、カルマンフィルタを用いて方位変化量のオフセット誤差、方位変化量のゲイン誤差、車速センサの距離係数誤差等の誤差要因を１つの状態量ｘとしてモデル化し、ＧＰＳ受信機からの出力値から各誤差を逐次推定し、推定した各誤差に基づいて、ジャイロスコープによる検出値のゲイン補正に用いるゲイン補正量を修正するようにしたものがある（例えば、特許文献１参照）。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特開２００９−１１５５４５号公報
【発明の概要】
【発明が解決しようとする課題】
【０００６】
しかしながら、上記特許文献１に記載された装置では、角速度に対して線形の関係を有する取り付け角ゲイン誤差のみをゲイン誤差として各誤差を逐次推定し、推定した各誤差に基づいてジャイロスコープによる検出値のゲイン補正に用いるゲイン補正量を修正する構成となっているので、車両の走行軌跡を精度良く推定できないといった問題があった。
【０００７】
発明者の検討によれば、上記したジャイロスコープの誤差要因のうち、方位変化量のゲイン誤差の誤差要因には、ジャイロスコープの検出軸が傾いた状態で設置されることにより生じる取り付け角ゲイン誤差の他に、角速度に応じてゲインが変化することにより生じる角速度ゲイン誤差があり、取り付け角ゲイン誤差は角速度に対して線形の関係を有するのに対し、角速度ゲイン誤差は角速度に対して非線形の関係を有する傾向があることが分かった。
【０００８】
図１０は、ジャイロスコープの取り付け角ゲイン誤差について説明するための図である。この図には、ジャイロスコープの検出軸が傾くことなく設置された理想的なジャイロスコープの角速度に対する出力電圧特性（実線で示す）と、ジャイロスコープの検出軸が傾いて設置されたジャイロスコープの角速度に対する出力電圧特性（点線で示す）が示されている。これらの出力電圧特性は、いずれも角速度に対して一定の傾きを有している。取り付け角ゲイン誤差は角速度に対して一定の割合で変化する。すなわち、取り付け角ゲイン誤差は角速度に対して線形の関係を有する。なお、ジャイロスコープが車両に取り付けられた後、取り付け角度と取り付け角ゲイン誤差はともに変化しない。
【０００９】
図１１は、ジャイロスコープの角速度ゲイン誤差について説明するための図である。この図には、理想的なジャイロスコープの出力電圧特性（実線で示す）と実際のジャイロスコープの出力電圧特性（点線で示す）が示されている。実際のジャイロスコープの出力電圧特性としては、図中Ａに示すような非線形性を有するものや、図中Ｂに示すような非線形性を有するものがある。このように、実際のジャイロスコープのゲイン特性および角速度ゲイン誤差は固体毎に異なる。また、安価なジャイロスコープは、角速度に対する出力電圧特性が非線形となる傾向が高い。角速度ゲイン誤差は、実際のジャイロスコープの出力電圧と理想的なジャイロスコープの出力電圧の差分となる。
【００１０】
なお、図１１と図１２に示される各出力電圧を合計した電圧がジャイロスコープより出力される。
【００１１】
図１２に、ジャイロスコープの角速度に対するゲイン特性の一例を示す。この図には、ジャイロスコープの検出軸が傾くことなく設置されたジャイロスコープのゲイン特性（実線で示す）と、ジャイロスコープの検出軸が傾いた状態で設置されたジャイロスコープのゲイン特性（点線で示す）とが示されている。ジャイロスコープの検出軸が傾くことなく設置されたジャイロスコープのゲイン特性は、角速度と関係なく一定（０．０２５［Ｖ］）となっている。これに対し、ジャイロスコープの検出軸が傾いた状態で設置されたジャイロスコープの出力電圧（点線で示す）は、０．０２５［Ｖ］よりも低く、かつ、角速度と関係なく一定となっている。
【００１２】
図１３に、ジャイロスコープの角速度に対するゲイン特性の一例を示す。この図には、理想的なジャイロスコープのゲイン特性（実線で示す）と実際のジャイロスコープのゲイン特性（点線で示す）が示されている。理想的なジャイロスコープのゲイン特性は、角速度と関係なく一定（０．０２５［Ｖ］）となっている。しかし、実際のジャイロスコープのゲイン特性は、角速度に対して一定の傾きを有している。
【００１３】
図１２と図１３とを対比すると分かるように、ジャイロスコープのゲイン特性は、取り付け角ゲイン誤差に関しては角速度に対して一定であるのに対し、角速度ゲイン誤差に関しては角速度に応じて変化する傾向にある。
【００１４】
したがって、上記特許文献１に記載された装置では、角速度に対して非線形の関係を有する角速度ゲイン誤差を考慮することなく、角速度に対して線形の関係を有する取り付け角ゲイン誤差のみをゲイン誤差として各誤差を逐次推定し、推定した各誤差に基づいて、ジャイロスコープによる検出値のゲイン補正に用いるゲイン補正量を修正するような構成となっているので、角速度に対する出力電圧特性が非線形のジャイロスコープを用いた場合、車両の走行軌跡を精度良く推定することができないといった問題があった。
【００１５】
本発明は上記問題に鑑みたもので、角速度に対する出力電圧特性が非線形のジャイロスコープを用いても、精度よく車両の走行軌跡を推定できるようにすることを可能とすることを目的とする。
【課題を解決するための手段】
【００１６】
上記目的を達成するため、請求項１に記載の発明は、車両の回転角速度に応じた信号を出力する角速度センサと、車両の走行速度に応じた信号を出力する速度センサと、ＧＰＳ衛星からの電波を受信し、車両の絶対位置および絶対方位を特定するための信号を出力するＧＰＳ受信装置と、角速度センサの出力値に基づいて算出される方位変化量と、速度センサの出力値に基づいて算出される移動距離から、車両の相対軌跡を推定するとともに当該車両の相対軌跡とＧＰＳ受信装置からの出力値とに基づいて車両の走行軌跡を推定する走行軌跡推定手段と、車両の相対軌跡および車両の走行軌跡と、ＧＰＳ受信装置の出力値との差を観測値として角速度センサの出力値、速度センサの出力値およびＧＰＳ受信装置の出力値の各誤差を推定するとともに推定した各誤差に基づいて角速度センサの出力値、速度センサの出力値およびＧＰＳ受信装置の出力値の各誤差を補正する誤差推定手段と、を備えた車両用現在位置検出装置であって、誤差推定手段は、角速度センサの出力値のゲイン誤差として、角速度センサの車両への取り付け角度を要因とする取り付け角ゲイン誤差と、角速度センサの角速度に対する非線形ゲイン特性を要因とする角速度ゲイン誤差とを区別して推定することを特徴としている。
【００１７】
このような構成によれば、角速度センサの出力値のゲイン誤差として、角速度センサの車両への取り付け角度を要因とする取り付け角ゲイン誤差と、角速度センサの角速度に対する非線形ゲイン特性を要因とする角速度ゲイン誤差とが区別して推定されるので、角速度に対する出力電圧特性が非線形のジャイロスコープを用いても、精度よく車両の走行軌跡を推定することができる。
【００１８】
また、請求項２に記載の発明では、誤差推定手段は、取り付け角ゲイン誤差を用いて角速度センサの車両への取り付け角度による誤差を補正するための第１の補正量を算出するとともに角速度ゲイン誤差を用いて角速度センサの非線形ゲイン特性による誤差を補正するための第２の補正量を算出し、第１の補正量および第２の補正量を用いてＧＰＳ受信装置の出力値より算出される絶対方位量を補正するための絶対方位誤差を推定することを特徴としている。
【００１９】
このような構成では、取り付け角ゲイン誤差を用いて角速度センサの車両への取り付け角度による誤差を補正するための第１の補正量を算出するとともに角速度ゲイン誤差を用いて角速度センサの非線形ゲイン特性による誤差を補正するための第２の補正量を算出し、第１の補正量および第２の補正量を用いてＧＰＳ受信装置の出力値より算出される絶対方位量を補正するための絶対方位誤差が推定されるので、ＧＰＳ受信装置の出力値より算出される絶対方位量を精度よく補正することができる。
【００２０】
なお、請求項３に記載の発明のように、方位変化量をＤ、前回の角速度ゲイン誤差をεＳａｔ−１とすると、角速度ゲイン誤差により発生する方位誤差をＤ２・εＳａｔ−１としてモデル化して第２の補正量を推定することができる。
【００２１】
また、請求項４に記載の発明のように、方位変化量をＤ、前回の取り付け角ゲイン誤差をεＳｂｔ−１とすると、取り付け角ゲイン誤差により発生する方位誤差をＤ・εＳｂｔ−１としてモデル化し、第１の補正量を推定することができる。
【００２２】
また、請求項５に記載の発明のように、誤差推定手段は、第１の関数を用いて取り付け角ゲイン誤差を推定するとともに、第１の関数と異なる第２の関数を用いて角速度ゲイン誤差を推定することができる。
【００２３】
また、請求項６に記載の発明は、誤差推定手段は、取り付け角ゲイン誤差の推定および第１の補正量の算出が可能な第１カルマンフィルタにより取り付け角ゲイン誤差の推定および第１の補正量の算出を実施する取り付け角ゲイン誤差推定手段と、取り付け角ゲイン誤差推定手段による取り付け角ゲイン誤差の推定および第１の補正量の算出を実施した後、第１カルマンフィルタの誤差共分散行列における取り付け角ゲイン誤差に対応する対角成分が予め定められた閾値未満になったか否かを判定する判定手段と、判定手段により第１カルマンフィルタの誤差共分散行列における取り付け角ゲイン誤差に対応する対角成分が予め定められた閾値未満になったと判定された後、角速度ゲイン誤差の推定および第２の補正量の算出が可能な第２カルマンフィルタにより角速度ゲイン誤差の推定および第２の補正量の算出を実施する角速度ゲイン誤差推定手段と、を備えたことを特徴としている。
【００２４】
このような構成によれば、誤差推定手段は、取り付け角ゲイン誤差の推定および第１の補正量の算出が可能な第１カルマンフィルタにより取り付け角ゲイン誤差の推定および第１の補正量の算出を実施した後、第１カルマンフィルタの誤差共分散行列における取り付け角ゲイン誤差に対応する対角成分が予め定められた閾値未満になったか否かを判定し、第１カルマンフィルタの誤差共分散行列における取り付け角ゲイン誤差に対応する対角成分が予め定められた閾値未満になったと判定された後、角速度ゲイン誤差の推定および第２の補正量の算出が可能な第２カルマンフィルタにより角速度ゲイン誤差の推定および第２の補正量の算出が実施される。つまり、第１カルマンフィルタにより取り付け角ゲイン誤差の推定および第１の補正量の算出を実施した後、すなわち、第１カルマンフィルタの誤差共分散行列における取り付け角ゲイン誤差に対応する対角成分が収束して取り付け角ゲイン誤差が正確に推定された後、第２カルマンフィルタにより角速度ゲイン誤差の推定および第２の補正量の算出が実施されるので、取り付け角ゲイン誤差、角速度ゲイン誤差の推定、および第１、第２の補正量の算出を精度良く行うことができる。
【００２５】
また、請求項６に記載の発明では、角速度ゲイン誤差推定手段は、第１カルマンフィルタにより取り付け角ゲイン誤差の推定および第１の補正量の算出を実施した際に、車両の相対軌跡および車両の走行軌跡と、ＧＰＳ受信装置の出力値との差を観測値として推定した角速度センサの出力値、速度センサの出力値およびＧＰＳ受信装置の出力値の各推定誤差および第１カルマンフィルタにより算出された誤差共分散値を用いて、第２カルマンフィルタによる角速度ゲイン誤差を推定することを特徴としている。
【００２６】
このような構成によれば、第１カルマンフィルタにより取り付け角ゲイン誤差の推定および第１の補正量の算出を実施した際に、車両の相対軌跡および車両の走行軌跡と、ＧＰＳ受信装置の出力値との差を観測値として推定した角速度センサの出力値、速度センサの出力値およびＧＰＳ受信装置の出力値の各推定誤差と第１カルマンフィルタにより算出された誤差共分散値を用いて、第２カルマンフィルタによる角速度ゲイン誤差が推定されるので、第１カルマンフィルタによる取り付け角ゲイン誤差等の各誤差を推定した後、再度、取り付け角ゲイン誤差以外の各誤差を初期値として第２カルマンフィルタによる角速度ゲイン誤差を推定する場合と比較して、高速で、かつ精度良く角速度ゲイン誤差を推定することができる。
【図面の簡単な説明】
【００２７】
【図１】本発明の一実施形態に係る車両用現在位置検出装置の構成を示す図である。
【図２】カルマンフィルタのシグナルフローグラフである。
【図３】実際のジャイロスコープにおける角速度に対するゲイン特性例を示す図である。
【図４】制御部による推測航法処理のフローチャートである。
【図５】ジャイロスコープのオフセット補正について説明するための図である。
【図６】ジャイロスコープのゲイン補正について説明するための図である。
【図７】制御部によるＧＰＳ・複合化処理のフローチャートである。
【図８】制御部によるカルマンフィルタＡとカルマンフィルタＢの切替処理のフローチャートである。
【図９】カルマンフィルタＡによるゲイン誤差のオフセットの推定について説明するための図である。
【図１０】ジャイロスコープの取り付け角ゲイン誤差について説明するための図である。
【図１１】ジャイロスコープの角速度ゲイン誤差について説明するための図である。
【図１２】ジャイロスコープの角速度に対するゲイン特性の一例を示した図である。
【図１３】ジャイロスコープの角速度に対するゲイン特性の一例を示した図である。
【発明を実施するための形態】
【００２８】
本発明の一実施形態に係る車両用現在位置検出装置の構成を図１に示す。本実施形態に係る車両用現在位置検出装置は車両用ナビゲーション装置として構成されており、ＧＰＳ受信装置１０、自立センサ２０、地図データ記憶装置３０、ディスプレイ部４０および制御部５０を備えている。
【００２９】
ＧＰＳ受信装置１０は、複数のＧＰＳ衛星１００（図中では、１つのみ示す）からの電波を受信するＧＰＳアンテナ部１１、ＧＰＳアンテナ部１１から入力される高周波信号を中間周波数帯の受信信号に変換するＧＰＳ―ＲＦ部１２、ＧＰＳ―ＲＦ部１２から入力される受信信号の同期捕捉や同期保持等の信号処理を行うＧＰＳ信号処理部１３、ＧＰＳ信号処理部１３から出力されるデータに基づいてＧＰＳ測位演算を行い、このＧＰＳ測位演算により求めた測位データを出力するＧＰＳ測位演算部１４を備えている。
【００３０】
ＧＰＳ受信装置１０は、ＧＰＳ衛星１００からの電波を受信し、車両の絶対的な現在位置を表すＧＰＳ位置、車両の進行方向を特定するための速度ベクトル等を算出し、算出結果を測位データとして出力する。
【００３１】
自立センサ２０は、車両の走行速度に応じた車速パルス信号を出力する速度センサとしての車速センサ２１と、車両の回転角速度に応じた角速度信号を出力する角速度センサとしてのジャイロスコープ２２を備えている。
【００３２】
地図データ記憶装置３０は、地図データが記憶された地図データ記憶媒体から地図データを読み出すための装置である。地図データには、各リンクの距離、道路種別、車線数、一方通行規制、信号機の位置等を表す道路データ、位置検出精度を向上するためのいわゆるマップマッチングデータ、川、湖、海、鉄道、施設などの位置、形状、名称を表す背景データ、各地の施設の名称、所在位置、施設種類等を示す施設データ等が含まれる。なお、地図データ記憶媒体としては、ＣＤ−ＲＯＭ、ＤＶＤ−ＲＯＭ、ハードディスクドライブ、メモリカード等がある。
【００３３】
ディスプレイ部４０は、液晶等の表示部を備え、コンピュータから入力される映像信号に応じた映像を表示部に表示させる。
【００３４】
制御部５０は、ＣＰＵ、ＲＯＭ、ＲＡＭ、不揮発性メモリ、Ｉ／Ｏ等を有するコンピュータにより構成されており、ＣＰＵは、ＲＯＭに記憶されたプログラムに従って各種処理を実施する。
【００３５】
制御部５０の処理としては、ジャイロスコープ２２の出力値に基づいて算出される方位変化量と、速度センサ２１の出力値に基づいて算出される距離係数から、車両の相対軌跡を推定するとともに当該車両の相対軌跡とＧＰＳ受信装置１０からの出力値とに基づいて車両の走行軌跡を推定する走行軌跡推定処理、ジャイロスコープ２２の出力値およびＧＰＳ受信装置１０からの出力値に基づいて方位変化量のオフセット誤差、方位変化量のゲイン誤差、絶対方位誤差等を推定する誤差推定処理、自車の走行軌跡と地図メモリ７０に格納されている地図情報の道路データとを照合することによって、自車位置（ＧＰＳ位置により補正された推測位置）を道路上の位置に修正するマップマッチング処理等がある。なお、走行軌跡推定処理では、誤差推定処理により推定された各誤差を用いて走行軌跡推定処理による車両の走行軌跡の推定に用いられる方位変化量のオフセット、方位変化量のゲインおよび絶対方位等を補正するようになっている。
【００３６】
制御部５０は、誤差推定処理においてカルマンフィルタを用いて推測航法における各誤差要因を１つの状態量ｘとしてモデル化し、各誤差の推定値を求める誤差推定処理を実施する。
【００３７】
ここで、カルマンフィルタの概要について説明する。カルマンフィルタにおいては、推定したい誤差信号を状態量ｘとする。状態量は次の状態方程式（数式１）により更新される。
【００３８】
【数１】
2012002794.tif 000003
更に、観測過程を次のように示す。
【００３９】
【数２】
2012002794.tif 000004
数式１、２の関係をシグナルフローグラフで表すと図２のようになる。
【００４０】
カルマンフィルタでは、観測信号を用いて次のような巡回計算による状態量を最小二乗推定する。
【００４１】
【数３】
2012002794.tif 000005
ただし、Ｇ（ｔ）は、カルマンゲイン、Ｐ（ｔ｜ｔ−１）は状態量ｘの誤差共分散予測値、Ｐ（ｔ｜ｔ）は誤差共分散、Ｖ（ｔ）は雑音ｖ（ｔ）の共分散行列、Ｗ（ｔ）は雑音ｗ（ｔ）の誤差共分散行列を表す。
【００４２】
カルマンフィルタでは、推測航法における誤差を状態量として、観測信号から推定する。この誤差の推定値を推測航法にフィードバックして補正することにより、数式５に示した事前推定値ｘ（ｔ｜ｙｔ−１）を零と仮定することができる。したがって、数式６は、次の数式８のように変形することができる。
【００４３】
【数４】
2012002794.tif 000006
次に、推測航法のモデル化とカルマンフィルタの定式化について説明する。本実施形態では、ジャイロスコープ２２の取り付け角ゲイン誤差のみをゲイン誤差として各誤差を推定するカルマンフィルタＡを用いてジャイロスコープの取り付け各誤差を推定した後、ジャイロスコープ２２の取り付け角ゲイン誤差と角速度ゲイン誤差を区別して各誤差を推定するカルマンフィルタＢを用いてジャイロスコープの角速度ゲイン誤差を推定する。
【００４４】
まず、カルマンフィルタＡについて説明する。カルマンフィルタＡでは、オフセット誤差（εＧ）、ゲイン誤差（εＳ）、絶対方位誤差（εθ）、距離係数誤差（εＫ）、絶対位置北方位誤差（εＹ）、絶対位置東方位誤差（εＸ）の６つの誤差を状態量ｘとしてモデル化し、各誤差の推定値を求める。
【００４５】
【数５】
2012002794.tif 000007
ただし、θＴは真の絶対方位、Ｄは前回からの方位変化量、Ｌは前回からの移動距離、Ｔは前回からの経過時間を表す。また、θは真の絶対方位θＴに、センサ誤差が加わったものであり、方位変化量から求められる絶対方位θとする。
【００４６】
数式９〜１４の各式を状態量で偏微分すると、状態量生成過程は次のように表される。
【００４７】
【数６】
2012002794.tif 000008
ただし、ε０はオフセット雑音（温度ドリフト等によるオフセットの変動分）、ε１はゲイン雑音（温度ドリフト等によるジャイロゲインの変動分）、ε２は絶対方位雑音（ジャイロのクロスカップリング等による変動分）、ε３は距離計数雑音（速度センサの経年変化によるの変動分）を意味する。
【００４８】
次に、観測過程のモデル化について説明する。観測値は推測航法の出力と、ＧＰＳの出力の差により求める。それぞれの差には誤差が含まれるため、観測値においては推測航法の誤差とＧＰＳの誤差の和が得られる。
【００４９】
この観測値ｙと状態量ｘを関係づけ、以下のようにモデル化する。
【００５０】
【数７】
2012002794.tif 000009
ここで、添字のＤＲｔは時刻ｔにおいて車速センサ２１、ジャイロスコープ２２からの信号に基づく推測航法にて求められた値を意味し、ＧＰＳｔは時刻ｔにおいてＧＰＳから出力される値を意味する。
【００５１】
数式１６におけるεθＤＲｔ−εθＧＰＳｔは、推測航法により求められた絶対方位とＧＰＳ受信装置１０から出力される方位の差である。すなわち、推測航法により求められた絶対方位には、真の絶対方位と誤差εθＤＲｔが含まれており、ＧＰＳ受信装置１０から出力される方位には、真の絶対方位と誤差εθＤＲｔが含まれているので、推測航法により求められた絶対方位とＧＰＳ受信装置１０から出力される方位の差εθＤＲｔ−εθＧＰＳｔが算出される。
【００５２】
同様に、数式１６におけるεＫＤＲｔ−εＫＧＰＳｔは、推測航法により求められる速度とＧＰＳ受信装置１０から出力される速度との差から求められる距離係数誤差である。また、数式１６におけるεＸＤＲｔ−εＸＧＰＳｔ、εＹＤＲｔ−εＹＧＰＳｔは、それぞれ推測航法により求められる絶対位置とＧＰＳ受信装置１０から出力される絶対位置とのＸ成分、Ｙ成分の誤差である。
【００５３】
次に、カルマンフィルタＢについて説明する。カルマンフィルタＢでは、ゲイン誤差（εＳ）を角速度ゲイン誤差（εＳａ）と取り付け角ゲイン誤差（εＳｂ）に区別する。すなわち、オフセット誤差（εＧ）、角速度ゲイン誤差（εＳａ）、取り付け角ゲイン誤差（εＳｂ）、絶対方位誤差（εθ）、距離係数誤差（εＫ）、絶対位置北方位誤差（εＹ）、絶対位置東方位誤差（εＸ）の７つの誤差を状態量ｘとしてモデル化し、各誤差の推定値を求める。なお、取り付け角ゲイン誤差（εＳｂ）は、カルマンフィルタＡにおけるゲイン誤差（εＳ）に相当する。
【００５４】
【数８】
2012002794.tif 000010
ただし、θＴは真の絶対方位、Ｄは前回からの方位変化量、Ｌは前回からの移動距離、Ｔは前回からの経過時間を表す。また、θは真の絶対方位θＴに、センサ誤差が加わったものであり、方位変化量から求められる絶対方位θとする。
【００５５】
数式９〜１４の各式を状態量で偏微分すると、状態量生成過程は次のように表される。
【００５６】
図３に、理想的なジャイロスコープのゲインを１とした場合の実際のジャイロスコープにおける角速度に対するゲイン特性例を示す。縦軸はゲインであり、横軸は角速度である。横軸のＣＷは時計回りの角速度を意味し、ＣＣＷは反時計回りの角速度を意味し、ＣＷ６０は＋６０ｄｅｇ／ｓ、ＣＣＷ６０は−６０ｄｅｇ／ｓを意味する。実際のジャイロスコープには、パターンＡに示すような特性を有するものや、パターンＢに示すような特性を有するものがある。
【００５７】
カルマンフィルタＡでは、数式１０を用いて取り付け角ゲイン誤差を推定するとともに、数式１１を用いて絶対方位誤差を推定している。数式１１の第２項であるＤ・εＳｔ−１は、取り付け角ゲイン誤差のみを誤差要因として算出したゲイン誤差に関する項である。すなわち、カルマンフィルタＡにおいては、方位変化量Ｄを変数とする一次関数で表されるゲイン誤差が絶対方位誤差に作用する。
【００５８】
しかし、このようなカルマンフィルタＡを用いた場合、図３中、パターンＡまたはパターンＢに示したように、角速度に応じてゲインが変化すると、絶対方位誤差を正確に推定することができない。
【００５９】
そこで、カルマンフィルタＢでは、数式１８−１、数式１８−２を用いてそれぞれ角速度ゲイン誤差と取り付け角ゲイン誤差を区別して推定するとともに、数式１９を用いて絶対方位誤差を推定している。数式１９の第２項であるＤ２・εＳａｔ−１は角速度ゲイン誤差に関する項であり、数式１９の第３項であるＤ・εＳｂｔ−１は取り付け角ゲイン誤差に関する項である。すなわち、カルマンフィルタＢにおいては、方位変化量Ｄを変数とする二次関数で表される角速度ゲイン誤差と方位変化量Ｄを変数とする一次関数で表される取り付け角ゲイン誤差とが絶対方位誤差に作用する。
【００６０】
数式１７〜２２の各式を状態量で偏微分すると、状態量生成過程は次のように表される。
【００６１】
【数９】
2012002794.tif 000011
ただし、ε０はオフセット雑音（温度ドリフト等によるオフセットの変動分）、ε１１は第１ゲイン雑音（ジャイロの取り付け角等によるジャイロゲインの変動分）、ε１２は第２ゲイン雑音（ジャイロの角速度等によるジャイロゲインの変動分）、ε２は絶対方位雑音（ジャイロの変動分）、ε３は距離計数雑音（タイヤ径の経年変化によるの変動分）を意味する。
【００６２】
観測値は推測航法の出力と、ＧＰＳの出力の差により求める。それぞれの差には誤差が含まれるため、観測値においては推測航法の誤差とＧＰＳの誤差の和が得られる。
【００６３】
次に、この観測値ｙと状態量ｘを関係づけ、以下のようにモデル化する。
【００６４】
【数１０】
2012002794.tif 000012
ここで、添字のＤＲｔは時刻ｔにおいて車速センサ２１、ジャイロスコープ２２からの信号に基づく推測航法にて求められた値を意味し、ＧＰＳｔは時刻ｔにおいてＧＰＳから出力される値を意味する。
【００６５】
数式２４におけるεθＤＲｔ−εθＧＰＳｔは、推測航法により求められた絶対方位とＧＰＳ受信装置１０から出力される方位の差である。すなわち、推測航法により求められた絶対方位には、真の絶対方位と誤差εθＤＲｔが含まれており、ＧＰＳ受信装置１０から出力される方位には、真の絶対方位と誤差εθＤＲｔが含まれているので、推測航法により求められた絶対方位とＧＰＳ受信装置１０から出力される方位の差εθＤＲｔ−εθＧＰＳｔが算出される。
【００６６】
同様に、数式２４におけるεＫＤＲｔ−εＫＧＰＳｔは、推測航法により求められる速度とＧＰＳ受信装置１０から出力される速度との差から求められる距離係数誤差である。また、数式２４におけるεＸＤＲｔ−εＸＧＰＳｔ、εＹＤＲｔ−εＹＧＰＳｔは、それぞれ推測航法により求められる絶対位置とＧＰＳ受信装置１０から出力される絶対位置とのＸ成分、Ｙ成分の誤差である。
【００６７】
カルマンフィルタＡでの演算を実行することにより、状態量生成過程にて定義された６つの誤差値による状態量ｘが求められ、これらによりジャイロスコープ２２の出力値に基づいて算出される方位変化量のオフセット補正、ゲイン補正、速度センサ２１の出力値に基づいて算出される距離係数補正、更には、絶対位置補正が行われる。
【００６８】
また、カルマンフィルタＡでの演算の実行後、カルマンフィルタＢでの演算を実行することにより、状態量生成過程にて定義された７つの誤差値による状態量ｘが求められ、これらによりジャイロスコープ２２の出力値に基づいて算出される方位変化量のオフセット補正、取り付け角ゲイン補正、角速度ゲイン補正、速度センサ２１の出力値に基づいて算出される距離係数補正、更には、絶対位置補正が正確に行われる。
【００６９】
図４に、制御部１０による推測航法処理のフローチャートを示す。本車両用現在位置検出装置が動作状態になると、制御部１０は、図４に示す処理を一定周期ＴＭ毎に繰り返し実行する。
【００７０】
Ｓ１００の方位変化量・移動距離の演算処理では、まず、ジャイロスコープ２２の出力角を基に方位変化量を算出する。
【００７１】
方位変化量＝ジャイロ出力角速度×ＴＭ・・・数式２５
なお、ジャイロ出力角速度は、図５のグラフから明らかなように、ジャイロスコープ２２の出力をサンプリングした値から、オフセット値(図５では２．５Ｖ) を減じたものに、変換ゲイン(グラフの傾きに相当する) を乗じることにより算出されたものである。
【００７２】
次に、後述するオフセット補正量に一定周期ＴＭを乗じたものを、数式２５により求めた方位変化量を減じることにより、方位変化量のオフセット補正を行う。
【００７３】
方位変化量＝方位変化量−オフセット補正量×ＴＭ・・・数式２６
すなわち、図５に示すように、角速度０［ｄｅｇ／ｓｅｃ] に対応するジャイロスコープ２２の出力電圧(ゼロ点) の変動に基づく誤差を補正する。
【００７４】
次に、オフセット補正された方位変化量に、後述するゲイン補正量を減じることにより、方位変化量のゲイン補正を行う。
【００７５】
方位変化量＝方位変化量×ゲイン補正量・・・数式２７
すなわち、図６に示すように、グラフの傾き(変換ゲイン) の変動に基づく誤差を補正する。
【００７６】
なお、カルマンフィルタＢを用いた場合、数式２７は、方位変化量＝方位変化量×（角速度ゲイン補正量×方位変化量＋取り付け角ゲイン補正量）となる。
【００７７】
そして、一定周期ＴＭの間に検出された車速センサ２１からの出力パルス数に、後述する距離係数を乗じることにより移動距離を算出する。
【００７８】
移動距離＝車速センサパルス数×距離係数・・・数式２８
このように、移動距離を算出すると、方位変化量・移動距離の演算処理を終了する。
【００７９】
次のＳ２００の相対軌跡の演算処理では、まず、数式２７を用いて算出した方位変化量を、それまでに求められている相対方位に加算することにより、相対方位を更新する。
【００８０】
相対方位＝相対方位＋方位変化量・・・数式２９
次に、この更新した相対方位および数式２８を用いて算出した移動距離に基づき、相対位置座標の更新を行う。具体的には、南北方向をｘ座標軸とした相対座標ｒｅｌ．ｘ、東西方向をｙ座標軸とした相対座標ｒｅｌ．ｙを、数式３０に従って更新する。ただし、θｓは数式２９を用いて算出した相対方位である。
【００８１】
ｒｅｌ．ｘ＝ｒｅｌ．ｘ＋移動距離×ｃｏｓ（θｓ）
ｒｅｌ．ｙ＝ｒｅｌ．ｙ＋移動距離×ｓｉｎ（θｓ）・・・数式３０
すなわち、この更新は、移動距離に対する相対方位のＸ，Ｙ成分をそれまでの相対位置座標に加算することにより行う。この相対位置座標は相対軌跡を求めるために算出するもので、その相対軌跡と道路形状との関係により、いわゆるマップマッチングが行われる。
【００８２】
次に、数式２８を用いて算出した移動距離を、メインルーチンの起動周期ＴＭで除することにより車速を算出する。
【００８３】
車速＝移動距離／ＴＭ・・・数式３１
このように、車速を算出すると、相対軌跡演算処理を終了する。
【００８４】
次のＳ３００の絶対方位・絶対位置の演算処理では、まず、数式２７を用いて算出した方位変化量を、それまでに求められている絶対方位に加算することにより、絶対方位を更新する。
【００８５】
絶対方位＝絶対方位＋方位変化量・・・数式３２
次に、この更新した絶対方位および数式２８を用いて算出した移動距離に基づき、絶対位置座標の更新を行う。具体的には、南北方向をｘ座標軸とした絶対座標ａｂｓ．ｘ、東西方向をｙ座標軸とした絶対座標ａｂｓ．ｙを、数式３３に従って更新する。ただし、θは数式３２を用いて算出した絶対方位である。
【００８６】
ａｂｓ．ｘ＝ａｂｓ．ｘ＋移動距離×ｃｏｓ（θ）
ａｂｓ．ｙ＝ａｂｓ．ｙ＋移動距離×ｓｉｎ（θ）・・・数式３３
すなわち、この更新は、移動距離に対する絶対方位のＸ，Ｙ成分をそれまでの絶対位置座標に加算することにより行う。このようにして更新した絶対方位と絶対位置は後述するＧＰＳとの複合化処理にて利用される。
【００８７】
次に、Ｓ４００のＧＰＳ・複合化処理を実施する。このＧＰＳ・複合化処理のフローチャートを図７に示す。このＧＰＳ・複合化処理では、まず、前回の測位または予測計算からＴ１秒経過したか否かを判定する（Ｓ４０２）。ここで、前回の測位または予測計算からＴ１秒経過していない場合、Ｓ４０２の判定はＮＯとなり、次にＧＰＳ受信措置１０からの測位データがあるか否かを判定する（Ｓ４０４）。
【００８８】
ここで、ＧＰＳ受信措置１０からの測位データがある場合、Ｓ４０４の判定はＹＥＳとなり、次に、カルマンフィルタの演算処理を実施する（Ｓ４０６）。本実施形態では、このカルマンフィルタの演算処理において、カルマンフィルタＡとカルマンフィルタＢを切り替えてカルマンフィルタの演算処理を行うようになっている。このカルマンフィルタの切替処理については後で説明するが、この切替処理により設定されるカルマンフィルタ切替用フラグに従ってカルマンフィルタの切替を実施する。
【００８９】
次に、カルマンフィルタの演算処理により求められた状態量ｘに含まれる各誤差により、以下に示す推測航法誤差の補正を実施する（Ｓ４０８）。
【００９０】
オフセット補正量＝オフセット補正量＋εＧ      ・・・数式３４
ゲイン補正量＝ゲイン補正量×（１−εＳ  ）    ・・・数式３５
距離係数＝距離係数×（１−εＫ）              ・・・数式３６
絶対方位＝絶対方位−εθ                      ・・・数式３７
ａｂｓ．ｙ（絶対位置）＝ａｂｓ．ｙ−εＹ      ・・・数式３８
ａｂｓ．ｘ（絶対位置）＝ａｂｓ．ｘ−εＸ      ・・・数式３９
ただし、カルマンフィルタＢを用いた場合、以下に示すようにゲイン補正量を角速度ゲイン補正量と取り付け角ゲイン補正量に分けて補正する。
【００９１】
角速度ゲイン補正量＝角速度ゲイン補正量×（１−εＳａ）・・・数式３５−１
取り付け角ゲイン補正量＝取り付け角ゲイン補正量×（１−εＳｂ）・・・数式３５−２
数式３４のジャイロスコープ２２のオフセット補正により、数式２６で用いられるオフセット補正量が修正され、数式３５のゲイン補正により、数式２７で用いられるゲイン補正量が修正され、数式３６の車速センサ２１の距離係数補正により数式２７にて用いられる距離係数が修正され、数式３７の絶対方位補正により、数式３２にて用いられる絶対方位θ が修正され、数式３８の絶対位置補正により、数式３３にて用いられる絶対位置が修正される。
【００９２】
このように、カルマンフィルタＢを用いて方位変化量の角速度ゲイン誤差と取り付け角ゲイン誤差を推定する状態方程式は、εＳａとεＳｂの２変数でモデル化し、ＧＰＳ受信機からの出力値から各誤差を逐次推定するようになっているので、正確にゲイン誤差を推定することが可能である。
【００９３】
また、ＧＰＳ受信装置１０の測位データが長時間(Ｔ１秒以上)得られなかった場合、Ｓ４０２の判定はＹＥＳとなり、状態遷移行列Ａの計算と誤差共分散Ｐの予測計算を行う（Ｓ４１０、Ｓ４１２）。これは、ＧＰＳ測位データが得られない場合、何も補正を行わないと誤差が大きくなってしまうため、誤差共分散Ｐの見積りだけは定期的に行っておくことにより、その後にＧＰＳ測位データが得られた時に行われるカルマンフィルタの処理を正確に行えるようにするためのものである。
【００９４】
上記処理をＧＰＳ受信装置１０からの測位データがある毎に繰り返し行い、誤差修正を行いながら推測航法データを得るようになっている。
【００９５】
図８に、カルマンフィルタＡとカルマンフィルタＢの切替処理のフローチャートを示す。制御部５０は、図４に示した推測航法処理と並行して図８に示す切替処理を実施する。
【００９６】
まず、取り付け角ゲイン誤差推定が既に完了しているか否かを判定する（Ｓ５００）。
【００９７】
ここで、取り付け角ゲイン誤差推定が完了していない場合、Ｓ５００の判定はＮＯとなり、次に、カルマンフィルタＡを用いた取り付け角ゲイン誤差を推定するようにカルマンフィルタ切替用フラグを設定する（Ｓ５０２）。具体的には、図７のＳ４０６にて、カルマンフィルタＡを用いた演算処理が行われるようにカルマンフィルタ切替用フラグを設定する。これにより、図７のＳ４０６にて、カルマンフィルタＡを用いた演算処理が行われ、取り付け角ゲイン誤差のオフセットが推定され、Ｓ４０８にて、取り付け角ゲイン補正量が修正される。
【００９８】
次に、取り付け角ゲイン誤差推定が完了したか否かを判定する（Ｓ５０４）。数式４０に、カルマンフィルタＡの誤差共分散行列を示す。
【００９９】
【数１１】
2012002794.tif 000013
このカルマンフィルタＡの誤差共分散行列の対角成分は、各変数の推定誤差量を表しており、この推定誤差量が小さく収束すると正確に各変数が推定できたことを示す。例えば、カルマンフィルタＡの誤差共分散行列の１行１列目はオフセット誤差に対応する対角成分で、２行２列目は取り付け角ゲイン誤差に対応する対角成分で、３行３列目は絶対方位誤差に対応する対角成分で、４行４列目は距離係数誤差に対応する対角成分となっている。ここでは、カルマンフィルタＡの誤差共分散行列における取り付け角ゲイン誤差に対応する対角成分Ｐｓが閾値Ｔｈ＿ｓ未満となったか否かに基づいて取り付け角ゲイン誤差推定が完了したか否かを判定する。
【０１００】
ここで、カルマンフィルタＡの誤差共分散行列における取り付け角ゲイン誤差に対応する対角成分Ｐｓが閾値Ｔｈ＿ｓ未満になっていない場合、Ｓ５０４の判定はＮＯとなり、Ｓ５０２へ戻る。
【０１０１】
また、カルマンフィルタＡの誤差共分散行列における取り付け角ゲイン誤差に対応する対角成分Ｐｓが閾値Ｔｈ＿ｓ未満になると、Ｓ５０４の判定はＹＥＳとなり、次に、誤差共分散値の継承を行う（Ｓ５０５）。本実施形態では、６行６列のカルマンフィルタＡを用いて繰り返しオフセット誤差（εＧ）、取り付け角ゲイン誤差（εＳ）、絶対方位誤差（εθ）、距離係数誤差（εＫ）、絶対位置北方位誤差（εＹ）、絶対位置東方位誤差（εＸ）の６つの誤差を推定した後、このカルマンフィルタＡで推定した各誤差を用いて７行７列の第２カルマンフィルタによる角速度ゲイン誤差の推定を実施する。このようにして、高速で、かつ精度良く角速度ゲイン誤差を推定する。具体的には、数式４１に示すカルマンフィルタＡの誤差共分散行列Ｐ１から数式４２に示すカルマンフィルタＢの誤差共分散行列Ｐ２となるように誤差共分散値の継承を行う。なお、数式４２に示すカルマンフィルタＢの誤差共分散行列Ｐ２におけるσa_iniはジャイロゲイン(角速度誤差)の誤差共分散の初期値である。
【０１０２】
【数１２】
2012002794.tif 000014
【０１０３】
【数１３】
2012002794.tif 000015
すなわち、数式４２に示すように、カルマンフィルタＢの誤差共分散行列Ｐ２には、数式４１に示したカルマンフィルタＡの誤差共分散行列Ｐ１における各成文σ１１〜σ１６、σ２１〜σ２６、σ３１〜σ３６、σ４１〜σ４６、σ５１〜σ５６、σ６１〜σ６６が用いられている。また、カルマンフィルタＢの誤差共分散行列Ｐ２における２行２列目は、ジャイロゲイン(角速度誤差)の誤差共分散の初期値σａ＿ｉｎｉとなっている。また、カルマンフィルタＢの誤差共分散行列Ｐ２における２行１列目および２行３列目〜２行７列目は、いずれも０となっており、１行２列目および３行２列目〜７行２列目も、いずれも０となっている。
【０１０４】
このような構成により、カルマンフィルタＡにより取り付け角ゲイン誤差を推定した際に、車両の相対軌跡および車両の走行軌跡と、ＧＰＳ受信装置の出力値との差を観測値として推定した角速度センサの出力値、速度センサの出力値およびＧＰＳ受信装置の出力値の各推定誤差およびカルマンフィルタＡにより取り付け角ゲイン誤差を推定した際に算出された誤差共分散値を用いて、カルマンフィルタＢによる角速度ゲイン誤差を推定することができる。すなわち、カルマンフィルタＡによる取り付け角ゲイン誤差等の各誤差の推定後、カルマンフィルタＡにより推定された取り付け角ゲイン誤差等の各誤差とカルマンフィルタＡにより推定された誤差共分散値を用いてカルマンフィルタＢによる角速度ゲイン誤差を推定するので、カルマンフィルタＡによる取り付け角ゲイン誤差等の各誤差を推定した後、再度、取り付け角ゲイン誤差以外の各誤差を初期値としてカルマンフィルタＢによる角速度ゲイン誤差を推定する場合と比較して、高速で、かつ精度良く角速度ゲイン誤差を推定することができる。
【０１０５】
次に、カルマンフィルタＢを用いた角速度ゲイン誤差を推定するようにカルマンフィルタ切替用フラグを設定する（Ｓ５０６）。具体的には、図７のＳ４０６にて、カルマンフィルタＢを用いた演算処理が行われるようにカルマンフィルタ切替用フラグを設定する。これにより、図７のＳ４０６にて、カルマンフィルタＢを用いた演算処理が行われ、角速度ゲイン誤差のオフセットが推定され、Ｓ４０８にて、角速度ゲイン補正量が修正される。
【０１０６】
カルマンフィルタＡによって、図９（ａ）に示すゲイン誤差のオフセットが推定される。このゲイン誤差のオフセットは、取り付け角ゲイン誤差に相当する。
【０１０７】
次に、カルマンフィルタＢによって、図９（ｂ）に示すゲイン誤差の傾きが推定される。このゲイン誤差の傾きは、角速度ゲイン誤差に相当する。
また、カルマンフィルタＢに切り替わる際、角速度ゲイン誤差に関する項目は初期値が設定されるが、そのたの項目は、カルマンフィルタＡによって推定された補正量と、誤差共分散行列の対角成分はカルマンフィルタＢに継承される。これによりカルマンフィルタＢによって角速度ゲイン誤差を早く精度よく推定可能となる。
【０１０８】
このように、取り付け角ゲイン誤差と角速度ゲイン誤差を区別して正確にゲイン誤差を推定することにより、現実のジャイロスコープのゲイン特性を精度良く推定することが可能となる。
【０１０９】
上記した構成によれば、角速度センサの出力値に基づいて算出される方位変化量と、速度センサの出力値に基づいて算出される移動距離から、車両の相対軌跡を推定するとともに当該車両の相対軌跡とＧＰＳ受信装置からの出力値とに基づいて車両の走行軌跡を推定し、車両の相対軌跡および車両の走行軌跡と、ＧＰＳ受信装置の出力値との差を観測値として角速度センサの出力値、速度センサの出力値およびＧＰＳ受信装置の出力値の各誤差を推定するとともに推定した各誤差に基づいて角速度センサの出力値、速度センサの出力値およびＧＰＳ受信装置の出力値の各誤差を補正する車両用現在位置検出装置であって、角速度センサの出力値のゲイン誤差として、角速度センサの車両への取り付け角度を要因とする取り付け角ゲイン誤差と、角速度センサの角速度に対する非線形ゲイン特性を要因とする角速度ゲイン誤差とが区別して推定されるので、角速度に対する出力電圧特性が非線形のジャイロスコープを用いても、精度よく車両の走行軌跡を推定することができる。
【０１１０】
また、取り付け角ゲイン誤差を用いて角速度センサの車両への取り付け角度による誤差を補正するための第１の補正量を算出するとともに角速度ゲイン誤差を用いて角速度センサの非線形ゲイン特性による誤差を補正するための第２の補正量を算出し、第１の補正量および第２の補正量を用いてＧＰＳ受信装置の出力値より算出される絶対方位量を補正するための絶対方位誤差を推定することができる。
【０１１１】
なお、方位変化量をＤ、前回の角速度ゲイン誤差をεＳａｔ−１とすると、角速度ゲイン誤差により発生する方位誤差をＤ２・εＳａｔ−１としてモデル化して第２の補正量を推定することができ、方位変化量をＤ、前回の取り付け角ゲイン誤差をεＳｂｔ−１とすると、取り付け角ゲイン誤差により発生する方位誤差をＤ・εＳｂｔ−１としてモデル化し、第１の補正量を推定することができる。
【０１１２】
また、第１の関数を用いて取り付け角ゲイン誤差を推定するとともに、第１の関数と異なる第２の関数を用いて角速度ゲイン誤差を推定することができる。
【０１１３】
また、取り付け角ゲイン誤差の推定および第１の補正量の算出が可能な第１カルマンフィルタにより取り付け角ゲイン誤差の推定および第１の補正量の算出を実施した後、第１カルマンフィルタの誤差共分散行列における取り付け角ゲイン誤差に対応する対角成分が予め定められた閾値未満になったか否かを判定し、第１カルマンフィルタの誤差共分散行列における取り付け角ゲイン誤差に対応する対角成分が予め定められた閾値未満になったと判定された後、角速度ゲイン誤差の推定および第２の補正量の算出が可能な第２カルマンフィルタにより角速度ゲイン誤差の推定および第２の補正量の算出が実施される。つまり、第１カルマンフィルタにより取り付け角ゲイン誤差の推定および第１の補正量の算出を実施した後、すなわち、第１カルマンフィルタの誤差共分散行列における取り付け角ゲイン誤差に対応する対角成分が収束して取り付け角ゲイン誤差が正確に推定された後、第２カルマンフィルタにより角速度ゲイン誤差の推定および第２の補正量の算出が実施されるので、取り付け角ゲイン誤差、角速度ゲイン誤差の推定、および第１、第２の補正量の算出を精度良く行うことができる。
【０１１４】
なお、本発明は上記実施形態に限定されるものではなく、本発明の趣旨に基づいて種々なる形態で実施することができる。
【０１１５】
例えば、上記実施形態では、カルマンフィルタＢにおいて、方位変化量Ｄを変数とする二次関数で表される角速度ゲイン誤差と、方位変化量Ｄを変数とする一次関数で表される取り付け角ゲイン誤差とが絶対方位誤差に作用するように、絶対方位誤差を推定したが、必ずしも方位変化量Ｄを変数とする二次関数で角速度ゲイン誤差を表す必要はなく、例えば、方位変化量Ｄを変数とする三次関数で表される取り付け角ゲイン誤差が絶対方位誤差に作用するように、絶対方位誤差を推定するようにしてもよい。
【０１１６】
なお、上記実施形態における構成と特許請求の範囲の構成との対応関係について説明すると、ジャイロスコープ２２が角速度センサに相当し、車速センサ２１が速度センサに相当し、図４のＳ１００〜Ｓ３００が走行軌跡推定手段に相当し、図４のＳ４００、図７のＳ４０２〜Ｓ４１２、図８のＳ５００〜Ｓ５０６が誤差推定手段に相当し、Ｓ５０２が取り付け角ゲイン誤差推定手段に相当し、Ｓ５０４が判定手段に相当し、Ｓ５０６が角速度ゲイン誤差推定手段に相当する。
【符号の説明】
【０１１７】
１０    ＧＰＳ受信装置
１１    ＧＰＳアンテナ部
１２    ＧＰＳ−ＲＦ部
１３    ＧＰＳ信号処理部
１４    ＧＰＳ測位演算部
２０    自立センサ
２１    車速センサ
２２    ジャイロスコープ
３０    地図データ記憶装置
４０    ディスプレイ部
５０    制御部
１００  ＧＰＳ衛星 
【図１】
2012002794.tif 000016
【図２】
2012002794.tif 000017
【図３】
2012002794.tif 000018
【図４】
2012002794.tif 000019
【図５】
2012002794.tif 000020
【図６】
2012002794.tif 000021
【図７】
2012002794.tif 000022
【図８】
2012002794.tif 000023
【図９】
2012002794.tif 000024
【図１０】
2012002794.tif 000025
【図１１】
2012002794.tif 000026
【図１２】
2012002794.tif 000027
【図１３】
2012002794.tif 000028