(57)【特許請求の範囲】
【請求項１】
車両の方位変化量に応じた信号を出力する相対方位センサと、
車両の移動距離を検出する移動距離検出手段と、
前記相対方位センサからの信号に基づき、オフセット補正量によりオフセット補正して方位変化量を求めるとともにこの方位変化量により車両の方位を特定し、この車両の方位と前記移動距離検出手段にて検出した車両の移動距離に基づいて車両の位置を検出する位置検出手段とから構成される推測航法手段と、
ＧＰＳ衛星からの衛星電波を受信して車両の方位に関する情報を出力するＧＰＳと、
少なくとも前記相対方位センサのオフセット誤差および車両の方位誤差と、前記移動距離検出手段の距離係数誤差若しくは車両の絶対位置誤差とを状態量Ｘ（ｔ）とし、この誤差の時間的変化を与えるプロセス行列φおよび信号生成過程で発生する雑音ωにより、状態量Ｘ（ｔ＋１）をφ・Ｘ（ｔ）＋ωにて関係付ける信号生成過程と、状態量Ｘ（ｔ）と観測値Ｙ（ｔ）とを、観測行列Ｈおよび観測過程で発生する雑音ｖにより、観測値Ｙ（ｔ）をＨ・Ｘ（ｔ）＋ｖにて関係付ける観測過程を形成するモデルを基に、前記推測航法手段における車両の方位に関する情報と前記ＧＰＳから出力される車両の方位に関する情報との差により、上記観測値Ｙ（ｔ）を演算するとともに、その演算値を基に上記モデルから状態量Ｘ（ｔ）を演算する状態量演算手段と、
この状態量演算手段にて求めた状態量Ｘ（ｔ）によるオフセット誤差および車両の方位誤差により、前記オフセット補正量を修正するとともに前記位置検出手段における車両の方位を修正する修正手段と、
車両の絶対方位を検出する絶対方位センサと
を備え、
前記状態量演算手段は、少なくとも前記オフセット誤差の大きさの見積もりと、前記車両の方位誤差の大きさの見積もりと、前記距離係数誤差の大きさの見積もり若しくは前記車両の絶対位置誤差の大きさの見積もりと、それらの誤差の相互相関値から構成される誤差共分散行列を計算する手段を有して、前記状態量Ｘ（ｔ）の演算を行うものであって、
前記ＧＰＳが長時間測位できず、前記車両の方位誤差の大きさの見積もりにより、車両の絶対方位の精度が前記絶対方位センサで期待できる精度より低下した状態を検出した時に、前記位置検出手段にて特定される車両の方位を前記絶対方位センサにて検出される方位にする変更手段と
を更に備え、
前記変更手段は、前記位置検出手段にて特定される車両の方位を前記絶対方位センサにて検出される方位にする時に前記誤差共分散行列のうちの前記車両の方位誤差と相関のある相互相関値を０に設定する手段を有することを特徴とする車両用現在位置検出装置。
【請求項２】
車両の方位変化量に応じた信号を出力する相対方位センサと、
車両の移動距離を検出する移動距離検出手段と、
前記相対方位センサからの信号に基づき、オフセット補正量によりオフセット補正して方位変化量を求めるとともにこの方位変化量により車両の方位を特定し、この車両の方位と前記移動距離検出手段にて検出した車両の移動距離に基づいて車両の位置を検出する位置検出手段とから構成される推測航法手段と、
ＧＰＳ衛星からの衛星電波を受信して車両の方位に関する情報を出力するＧＰＳと、
少なくとも前記相対方位センサのオフセット誤差および車両の方位誤差と、前記移動距離検出手段の距離係数誤差若しくは車両の絶対位置誤差とを状態量Ｘ（ｔ）とし、この誤差の時間的変化を与えるプロセス行列φおよび信号生成過程で発生する雑音ωにより、状態量Ｘ（ｔ＋１）をφ・Ｘ（ｔ）＋ωにて関係付ける信号生成過程と、状態量Ｘ（ｔ）と観測値Ｙ（ｔ）とを、観測行列Ｈおよび観測過程で発生する雑音ｖにより、観測値Ｙ（ｔ）をＨ・Ｘ（ｔ）＋ｖにて関係付ける観測過程を形成するモデルを基に、前記推測航法手段における車両の方位に関する情報と前記ＧＰＳから出力される車両の方位に関する情報との差により、上記観測値Ｙ（ｔ）を演算するとともに、その演算値を基に上記モデルから状態量Ｘ（ｔ）を演算する状態量演算手段と、
この状態量演算手段にて求めた状態量Ｘ（ｔ）によるオフセット誤差および車両の方位誤差により、前記オフセット補正量を修正するとともに前記位置検出手段における車両の方位を修正する修正手段と、
を備え、
前記状態量演算手段は、少なくとも前記オフセット誤差の大きさの見積もりと、前記車両の方位誤差の大きさの見積もりと、前記距離係数誤差の大きさの見積もり若しくは前記車両の絶対位置誤差の大きさの見積もりと、それらの誤差の相互相関値から構成される誤差共分散行列を計算する手段を有して、前記状態量Ｘ（ｔ）の演算を行うものであって、
前記オフセット誤差の大きさの見積もりにより前記オフセット補正の精度が低下した状態を検出した時に、車両停止時の前記相対方位センサからの出力に基づき、オフセット補正量を変更する変更手段と
を更に備え、
前記変更手段は、オフセット補正量を変更する時に前記誤差共分散行列のうちの前記オフセット誤差と相関のある相互相関値を０に設定する手段を有することを特徴とする車両用現在位置検出装置。
【発明の詳細な説明】
【０００１】
【産業上の利用分野】
本発明は、車両の方位および移動距離により車両の現在位置を検出する車両用現在位置検出装置に関する。
【０００２】
【従来の技術】
従来、この種の車載用ナビゲーション装置においては、車両の方位変化量を検出する相対方位センサ（ジャイロ、ステアリングセンサ、車輪センサ等）と、車両の速度（距離）を検出する距離センサ（車速センサ、車輪センサ等）の出力から、車両の位置・方位・車速等を検出する推測航法が用いられている。
【０００３】
この推測航法の出力（位置・方位・車速等）には、センサの誤差が含まれるため、誤差が生じる。特に、位置・方位は積分的に求められるため、誤差が徐々に増大してしまう。
これに対し、ＧＰＳは、絶対的な位置・方位・車速を求めることができるため、ＧＰＳが測位した場合に推測航法の出力をＧＰＳの出力に合わせることにより補正が可能である。例えば、推測航法で得られた位置をマップマッチングにより道路地図上の道路位置に位置合わせした時の位置と、ＧＰＳで得られた位置との差が所定値より大きくなった時に、道路地図上の位置をＧＰＳで得られた位置に修正するようにすることができる。
【０００４】
【発明が解決しようとする課題】
しかしながら、推測航法で求められる位置については、ＧＰＳの出力により補正することはできるが、センサの補正はできない。このため、ＧＰＳ非受信時においてセンサ出力の誤差により推測航法の出力精度が悪いという問題がある。
本発明は、センサ出力の誤差を修正して現在位置を求めるようにすることを第１の目的とする。
【０００５】
本発明者等は、上記目的を達成するため、後述するように、カルマンフィルタを用い、推測航法から求められる車両の方位に関する情報と、ＧＰＳから出力される車両の方位に関する情報との差に基づいて、センサ誤差量を求め、センサ出力の誤差修正を行うものを考えた。
このカルマンフィルタによる誤差修正においては、ＧＰＳからの測位データが必要となる。しかしながら、ＧＰＳが長時間測位できない場合は、センサ誤差が大きくなるため、センサ誤差を何らかの方法で補正する必要がある。
【０００６】
そこで、本発明は、ＧＰＳが長時間測位できないような場合に、ＧＰＳ以外のデータにより補正を行うようにすることを第２の目的とする。
【０００７】
【課題を解決するための手段】
本発明は上記目的を達成するため、請求項１に記載の発明においては、
車両の方位変化量に応じた信号を出力する相対方位センサ（２）と、
車両の移動距離を検出する移動距離検出手段（１および１０３）と、
前記相対方位センサからの信号に基づき、オフセット補正量によりオフセット補正して方位変化量を求めるとともにこの方位変化量により車両の方位を特定し、この車両の方位と前記移動距離検出手段にて検出した車両の移動距離に基づいて車両の位置を検出する位置検出手段（４，５およびそれに対する演算処理）とから構成される推測航法手段（１，２，４，５およびそれに対する演算処理）と、
ＧＰＳ衛星からの衛星電波を受信して車両の方位に関する情報を出力するＧＰＳ（３）と、
少なくとも前記相対方位センサのオフセット誤差および車両の方位誤差と、前記移動距離検出手段の距離係数誤差若しくは車両の絶対位置誤差とを状態量Ｘ（ｔ）とし、この誤差の時間的変化を与えるプロセス行列φおよび信号生成過程で発生する雑音ωにより、状態量Ｘ（ｔ＋１）をφ・Ｘ（ｔ）＋ωにて関係付ける信号生成過程と、状態量Ｘ（ｔ）と観測値Ｙ（ｔ）とを、観測行列Ｈおよび観測過程で発生する雑音ｖにより、観測値Ｙ（ｔ）をＨ・Ｘ（ｔ）＋ｖにて関係付ける観測過程を形成するモデルを基に、前記推測航法手段における車両の方位に関する情報と前記ＧＰＳから出力される車両の方位に関する情報との差により、上記観測値Ｙ（ｔ）を演算するとともに、その演算値を基に上記モデルから状態量Ｘ（ｔ）を演算する状態量演算手段（６およびそれに対する４０１〜４０８，４１０，４１１の処理）と、
この状態量演算手段にて求めた状態量Ｘ（ｔ）によるオフセット誤差および車両の方位誤差により、前記オフセット補正量を修正するとともに前記位置検出手段における車両の方位を修正する修正手段（４０９）と、
車両の絶対方位を検出する絶対方位センサ（７）と
を備え、
前記状態量演算手段は、少なくとも前記オフセット誤差の大きさの見積もりと、前記車両の方位誤差の大きさの見積もりと、前記距離係数誤差の大きさの見積もり若しくは前記車両の絶対位置誤差の大きさの見積もりと、それらの誤差の相互相関値から構成される誤差共分散行列を計算する手段（４０５，４１１）を有して、前記状態量Ｘ（ｔ）の演算を行うものであって、
前記ＧＰＳが長時間測位できず、前記車両の方位誤差の大きさの見積もりにより、車両の絶対方位の精度が前記絶対方位センサで期待できる精度より低下した状態を検出した時に、前記位置検出手段にて特定される車両の方位を前記絶対方位センサにて検出される方位にする変更手段（６０１〜６０３）と
を更に備え、
前記変更手段は、前記位置検出手段にて特定される車両の方位を前記絶対方位センサにて検出される方位にする時に前記誤差共分散行列のうちの前記車両の方位誤差と相関のある相互相関値を０に設定する手段（６０３）を有することを特徴としている。
【００１０】
請求項２に記載の発明においては、
車両の方位変化量に応じた信号を出力する相対方位センサ（２）と、
車両の移動距離を検出する移動距離検出手段（１および１０３）と、
前記相対方位センサからの信号に基づき、オフセット補正量によりオフセット補正して方位変化量を求めるとともにこの方位変化量により車両の方位を特定し、この車両の方位と前記移動距離検出手段にて検出した車両の移動距離に基づいて車両の位置を検出する位置検出手段（４，５およびそれに対する演算処理）とから構成される推測航法手段（１，２，４，５およびそれに対する演算処理）と、
ＧＰＳ衛星からの衛星電波を受信して車両の方位に関する情報を出力するＧＰＳ（３）と、
少なくとも前記相対方位センサのオフセット誤差および車両の方位誤差と、前記移動距離検出手段の距離係数誤差若しくは車両の絶対位置誤差とを状態量Ｘ（ｔ）とし、この誤差の時間的変化を与えるプロセス行列φおよび信号生成過程で発生する雑音ωにより、状態量Ｘ（ｔ＋１）をφ・Ｘ（ｔ）＋ωにて関係付ける信号生成過程と、状態量Ｘ（ｔ）と観測値Ｙ（ｔ）とを、観測行列Ｈおよび観測過程で発生する雑音ｖにより、観測値Ｙ（ｔ）をＨ・Ｘ（ｔ）＋ｖにて関係付ける観測過程を形成するモデルを基に、前記推測航法手段における車両の方位に関する情報と前記ＧＰＳから出力される車両の方位に関する情報との差により、上記観測値Ｙ（ｔ）を演算するとともに、その演算値を基に上記モデルから状態量Ｘ（ｔ）を演算する状態量演算手段（６およびそれに対する４０１〜４０８，４１０，４１１の処理）と、
この状態量演算手段にて求めた状態量Ｘ（ｔ）によるオフセット誤差および車両の方位誤差により、前記オフセット補正量を修正するとともに前記位置検出手段における車両の方位を修正する修正手段（４０９）と、
を備え、
前記状態量演算手段は、少なくとも前記オフセット誤差の大きさの見積もりと、前記車両の方位誤差の大きさの見積もりと、前記距離係数誤差の大きさの見積もり若しくは前記車両の絶対位置誤差の大きさの見積もりと、それらの誤差の相互相関値から構成される誤差共分散行列を計算する手段（４０５，４１１）を有して、前記状態量Ｘ（ｔ）の演算を行うものであって、
前記オフセット誤差の大きさの見積もりにより前記オフセット補正の精度が低下した状態を検出した時に、車両停止時の前記相対方位センサからの出力に基づき、オフセット補正量を変更する変更手段（７０１〜７０３）と
を更に備え、
前記変更手段は、オフセット補正量を変更する時に前記誤差共分散行列のうちの前記オフセット誤差と相関のある相互相関値を０に設定する手段（７０３）を有することを特徴としている。
なお、上記した、車両の方位に関する情報とは、方位そのものに限らず、方位に関係する情報も含む概念であり、後述するように方位と相関のある絶対位置等も含まれるものである。
【００１３】
また、上記各手段のカッコ内の符号は、後述する実施例記載の具体的手段との対応関係を示すものである。
また、後述するフローチャート内の各ステップはそれぞれの機能を実現する機能実現手段を構成するものである。
【００１４】
【発明の作用効果】
請求項１、２に記載の発明によれば、推測航法から求められる車両の方位に関する情報とＧＰＳから出力される車両の方位に関する情報との差により、オフセット誤差を求め、相対方位センサのオフセット補正を行うようにしているから、相対方位センサに対するセンサ出力の誤差修正を行うことができる。
【００１５】
さらに、請求項１に記載の発明においては、ＧＰＳが長時間測位できないような場合で車両の絶対方位の精度が絶対方位センサで期待できる精度より低下した時に絶対方位センサからの出力により車両の方位を決めるようにしているから、そのような場合の車両の方位の補正を行うことができる。さらに、絶対方位センサを用いた車両の方位の設定時に、誤差共分散行列のうちの車両の方位誤差と相関のある相互相関値を０に設定するようにしているから、再びＧＰＳが測位した後の誤差修正を良好にすることができる。
【００１６】
また、請求項２に記載の発明においては、ＧＰＳが長時間測位できないような場合でオフセット補正の精度が低下した時に、車両停止時の相対方位センサからの出力により、オフセット補正量を設定するようにしているから、そのような場合のオフセット補正量を補正することができる。さらに、上記のような車両停止時のオフセット補正量の設定時に、誤差共分散行列のうちのオフセット誤差と相関のある相互相関値を０に設定するようにしているから、再びＧＰＳが測位した後の誤差修正を良好にすることができる。
【００１７】
【実施例】
以下、本発明を図に示す実施例について説明する。
この実施例は、推測航法とＧＰＳとの複合化を図るため、カルマンフィルタを用いている。このカルマンフィルタの概要について説明する。
このカルマンフィルタにおいては、図７に示すように、信号生成過程と観測過程に分けられる。図において、線形システム（φ）があり、そのシステムの状態Ｘ（ｔ）に対して、観測行列Ｈで関係付けられるＸ（ｔ）の一部が観測できる場合に、フィルタはＸ（ｔ）の最適な推定値を与える。ここで、ωは信号生成過程にて発生する雑音であり、ｖは観測過程にて発生する雑音である。このフィルタの入力はＹ（ｔ）であり、出力はＸ（ｔ）の最適推定値である。
【００１８】
時刻ｔまでの情報を用いた状態Ｘの最適推定値、すなわち状態量Ｘ（ｔ｜ｔ）は、数１により求められる。
【００１９】
【数１】
Ｘ（ｔ｜ｔ）＝Ｘ（ｔ｜ｔ−１）＋Ｋ（ｔ）｛Ｙ（ｔ）−ＨＸ（ｔ｜ｔ−１）｝
ここで、Ｘ（ｔ｜ｔ−１）は事前推定値、Ｋ（ｔ）はカルマンゲインであり、それぞれ数２、数３により表される。
【００２０】
【数２】
Ｘ（ｔ｜ｔ−１）＝φＸ（ｔ−１｜ｔ−１）
【００２１】
【数３】
Ｋ（ｔ）＝Ｐ（ｔ｜ｔ−１）ＨＴ  （ＨＰ（ｔ｜ｔ−１）ＨＴ  ＋Ｖ）−１
ここで、Ｐは状態量Ｘの誤差共分散であり、Ｐ（ｔ｜ｔ−１）は誤差共分散の予測値、Ｐ（ｔ−１｜ｔ−１）は誤差共分散であり、それぞれ数４、数５により表される。
【００２２】
【数４】
Ｐ（ｔ｜ｔ−１）＝φＰ（ｔ−１｜ｔ−１）φＴ  ＋Ｗ
【００２３】
【数５】
Ｐ（ｔ−１｜ｔ−１）＝（Ｉ−Ｋ（ｔ−１）Ｈ）Ｐ（ｔ−１｜ｔ−２）
なお、Ｖは観測過程で発生する雑音ｖの分散、Ｗは信号過程で発生する雑音ωの分散である。また、Ａ（ｉ｜ｊ）は時刻ｊまでの情報に基づく時刻ｉでのＡの推定値を表す。なお、添字のＴ  は転置行列を意味し、−１は逆行列を意味する。Ｉは単位行列である。
【００２４】
さらに、ＶとＷは平均０の白色ガウス雑音であり、互いに無相関である。
上記のようなカルマンフィルタにおいて、状態量Ｘと誤差共分散Ｐの初期値に適当な誤差を与えてやり、新しい観測が行われる度に以上の計算を繰り返し行うことにより、状態量Ｘの精度が向上する。
このようなカルマンフィルタを推測航法へ適用したのが本実施例である。
【００２５】
まず、上記の信号生成過程の定義について説明する。
推測航法でのカルマンフィルタは、推測航法の誤差の補正を目的とするので、状態量Ｘは以下の５つの誤差値を定義する。この誤差値の時間的な変化を与えるものがプロセス行列φである。
▲１▼オフセット誤差（εＧ）
【００２６】
【数６】
εＧｔ  ＝εＧｔ−１  ＋ω０  
確定的な変化はなく、前回の誤差にノイズが付加される。
▲２▼絶対方位誤差（εＡ）
【００２７】
【数７】
εＡｔ  ＝Ｔ×εＧｔ−１  ＋εＡｔ−１  ＋ω１  
前回の誤差に、オフセット誤差に前回からの経過時間をかけて求める方位誤差とノイズが付加される。
▲３▼距離係数誤差（εＫ）
【００２８】
【数８】
εＫｔ  ＝εＫｔ−１  ＋ω２  
確定的な変化はなく、前回の誤差にノイズが付加される。
▲４▼絶対位置北方向誤差（εＹ）
【００２９】
【数９】
εＹｔ  ＝ｓｉｎ（ＡＴ  ＋εＡｔ−１  ＋εＧｔ−１  ×Ｔ／２）×Ｌ×（１＋εＫｔ−１  ）−ｓｉｎ（ＡＴ  ）×Ｌ＋εＹｔ−１  
前回の誤差に方位誤差・距離誤差によって生じる誤差が付加される。
▲５▼絶対位置東方向誤差（εＸ）
【００３０】
【数１０】
εＸｔ  ＝ｃｏｓ（ＡＴ  ＋εＡｔ−１  ＋εＧｔ−１  ×Ｔ／２）×Ｌ×（１＋εＫｔ−１  ）−ｃｏｓ（ＡＴ  ）×Ｌ＋εＸｔ−１  
前回の誤差に方位誤差・距離誤差によって生じる誤差が付加される。
上記の定義において、ＡＴ  は真の絶対方位、Ｌは前回からの移動距離、Ｔは前回からの経過時間である。
【００３１】
上記の各式を状態量で偏微分し線形化すると信号生成過程は以下のように定義される。
【００３２】
【数１１】
0003628046.tif 000002
【００３３】
上記Ａは、絶対方位ＡＴ  ＋εＡｔ−１  ＋εＧｔ−１  ×Ｔ／２を意味する。この値は真の絶対方位ＡＴ  にセンサ誤差が加わったものであり、後述するように、方位変化量から求められる絶対方位Ａとする。
また、ω０  は、オフセット雑音（温度ドリフト等によるオフセットの変動分）、ω１  は絶対方位雑音（ジャイロのゲイン的な誤差）、ω２  は距離係数雑音（経年変化）を意味する。
【００３４】
次に、上記観測過程の定義について説明する。
観測値は推測航法の出力と、ＧＰＳの出力の差より求める。それぞれの出力には誤差が含まれるため、観測値において、推測航法の誤差とＧＰＳの誤差の和が得られる。この観測値Ｙと状態量Ｘを関係付け、数１２のように定義される。
【００３５】
【数１２】
0003628046.tif 000003
【００３６】
但し、観測過程で発生する雑音ｖはＧＰＳの雑音であり、数１３のように定義される。
【００３７】
【数１３】
0003628046.tif 000004
【００３８】
以上の定義を基に、カルマンフィルタを用いた推測航法について説明する。図１に本実施例における概略構成を示す。
この図に示すように、車速センサ１、ジャイロ２からの信号を基に、相対軌跡演算部４、絶対位置演算部５での演算が行われ、それらの演算（推測航法演算）により、車速、相対軌跡、絶対位置、絶対方位が出力される。また、ＧＰＳ３からは位置・方位・車速の出力が得られる。カルマンフィルタ６は、推測航法により得られた車速、絶対位置・絶対方位の情報およびＧＰＳ３からの車速、位置・方位の情報を基に、車速センサ１の距離係数補正、ジャイロ２のオフセット補正、絶対位置補正、絶対方位補正を行う。
【００３９】
このような車載用ナビゲーション装置へカルマンフィルタを適用すると、車速センサ１の距離係数補正、ジャイロ２のオフセット補正、および絶対方位補正、絶対位置補正により、数２に示す、事前推定Ｘ（ｔ｜ｔ−１）は０となる。従って、数１は数１４に示すようになる。
【００４０】
【数１４】
Ｘ（ｔ｜ｔ）＝Ｋ（ｔ）Ｙ（ｔ）
従って、上記信号生成過程にて定義された５つの誤差値による状態量Ｘは、数３〜数５によって求められるカルマンゲインＫ（ｔ）および観測値Ｙ（ｔ）により求められる。
【００４１】
ここで、数３における誤差共分散Ｐは、数１５により定義される。
【００４２】
【数１５】
0003628046.tif 000005
【００４３】
この誤差共分散ＰにおけるσＧＧ２  はオフセット誤差の大きさの見積もりを表し、σＡＡ  ２は絶対方位誤差の大きさの見積もりを表し、σＫＫ２  は距離係数誤差の大きさの見積もりを表し、σＹＹ２  は絶対位置北方向誤差の大きさの見積もりを表し、σＸＸ２  は絶対位置東方向誤差の大きさの見積もりを表す。それら以外のσｉｊ２  はｉ行とｊ列の相互相関値を表す。例えばσＡＧ  ２はオフセット誤差と絶対方位誤差の相互相関値を表す。
【００４４】
この誤差共分散Ｐの値は、数４の計算によって更新される。なお、初期値においては、σＧＧ２  、σＡＡ  ２、σＫＫ２  、σＹＹ２  、σＸＸ２  の各値を誤差が最大となる値に設定しておき、また相互相関値については全て０に設定しておく。
また、数３におけるＨは数１２で示される行列を用い、Ｖについては数１３に示されるものを用いる。また、数４におけるＷは数１１に示されるωの分散を用いる。
【００４５】
観測過程における観測値Ｙとしては、数１２に示すように、εＡＤＲｔ  −εＡＧＰＳｔ、εＫＤＲｔ  −εＫＧＰＳｔ、εＹＤＲｔ  −εＹＧＰＳｔ、εＸＤＲｔ  −εＸＧＰＳｔを用いている。ここで、添字のＤＲｔ  は時刻ｔにおいて車速センサ１、ジャイロ２からの信号に基づく推測航法にて求められた値を意味し、ＧＰＳｔは時刻ｔにおいてＧＰＳ３から出力される値を意味する。
【００４６】
εＡＤＲｔ  −εＡＧＰＳｔは、推測航法により求められた絶対方位とＧＰＳ３から出力される方位の差、すなわち推測航法により求められた絶対方位には真の絶対方位とその誤差εＡＤＲｔ  が含まれており、またＧＰＳ３から出力される方位には真の絶対方位とその誤差εＡＧＰＳｔが含まれているため、それらの差を取ることによりεＡＤＲｔ  −εＡＧＰＳｔが得られる。
【００４７】
同様に、εＫＤＲｔ  −εＫＧＰＳｔは、推測航法により求められる速度とＧＰＳ３から出力される速度の差から求まる距離係数誤差であり、具体的には、（推測航法による速度−ＧＰＳによる速度）／（推測航法による速度）により求められる。また、εＹＤＲｔ  −εＹＧＰＳｔは、推測航法により求められる絶対位置のＹ成分とＧＰＳ３から出力される位置のＹ成分の誤差の差であり、εＸＤＲｔ  −εＸＧＰＳｔは、推測航法により求められる絶対位置のＸ成分とＧＰＳ３から出力される位置のＸ成分の誤差の差である。
【００４８】
また、数１３に示す、観測過程で発生する雑音ｖはＧＰＳ３の雑音であり、以下のようにして求められる。ＧＰＳ３における擬似距離の計測誤差（ＵＥＲＥ）とＨＤＯＰ（Ｈｏｒｉｚｏｎｔａｌ  Ｄｉｌｕｔｉｏｎ  ｏｆ  Ｐｒｅｓｉｓｉｏｎ）の関係により測位精度が、ＵＥＲＥ×ＨＤＯＰで求められ、この測位精度を２乗することにより、ｖ２ｔ、ｖ３ｔが求められる。また、ドップラー周波数の計測誤差とＨＤＯＰの関係より速度精度が、ドップラー周波数の計測誤差×ＨＤＯＰで求められ、この速度精度／車速にて距離係数計測誤差が求められ、これを２乗することによりｖ１ｔが求められる。さらに、車両の速度Ｖｃ  と速度精度から方位精度がｔａｎ−１（速度精度／Ｖｃ  ）で求められ、この方位精度を２乗するこによりｖ０ｔが求められる。
【００４９】
従って、観測過程におけるεＡＤＲｔ  −εＡＧＰＳｔ、εＫＤＲｔ  −εＫＧＰＳｔ、εＹＤＲｔ  −εＹＧＰＳｔ、εＸＤＲｔ  −εＸＧＰＳｔおよび上記雑音Ｖを入力とし、数３〜数５および数１を実行することにより、信号生成過程にて定義された５つの誤差値による状態量Ｘが求められ、これらにより車速センサ１の距離係数補正、ジャイロ２のオフセット補正、絶対位置補正、絶対方位補正が行われる。
【００５０】
上記の相対軌跡演算、絶対位置演算、カルマンフィルタはマイクロコンピュータによる演算処理にて行われるため、以下これについて説明する。
図２に推測航法のメインルーチンの演算処理を示す。
ステップ１００にて方位変化量・移動距離の演算を行う。この処理の詳細を図３に示す。まず、ステップ１０１にてジャイロ２の出力角速度にメインルーチンの起動周期ＴＭ  を掛けて方位変化量を算出する。次のステップ１０２にて、その方位変化量から、オフセット補正量（この補正量については後述する）にメインルーチンの起動周期ＴＭ  を掛けたものを引き、方位変化量のオフセット補正を行う。次のステップ１０３では、車速センサ１からの車速パルス数に距離係数（この距離係数についても後述する）を掛けて移動距離を算出する。
【００５１】
このステップ１００の次に、ステップ２００の相対軌跡演算処理を行う。この処理の詳細を図４に示す。まず、ステップ２０１にて、方位変化量（ステップ１０２にて求めたもの）を基に相対方位を更新する。この更新した相対方位およびステップ１０３にて求めた移動距離によりステップ２０２にて相対位置座標の更新を行う。この更新は、移動距離に対する相対方位のＸ，Ｙ成分をそれまでの相対位置座標に加算することにより行う。この相対位置座標は相対軌跡を求めるたに行うもので、その相対軌跡と道路形状との関係により、いわゆるマップマッチングが行われる。
【００５２】
このステップ２００の次に、ステップ３００の絶対方位・絶対位置の演算処理を行う。この処理の詳細を図５に示す。まず、ステップ３０１にて、方位変化量（ステップ１０２にて求めたもの）を基に絶対方位を更新する。この更新した絶対方位およびステップ１０３にて求めた移動距離によりステップ２０２にて絶対位置座標の更新を行う。このステップ２００の処理にて更新された絶対方位Ａと絶対位置は後述するＧＰＳとの複合化処理にて利用される。
【００５３】
このＧＰＳとの複合化処理を行うステップ４００の詳細を図６に示す。まず、ステップ４０１にて前回の測位又は予測計算からＴ１  秒経過したか否かを判定する。これはＧＰＳ３の測位が行われる毎にステップ４０３〜４０９にて推測航法の誤差を補正する処理を行うが、ＧＰＳ３の測位ができない場合には誤差が大きくなるため、それに対応した誤差の予測計算をステップ４１０、４１１にて定期的に行うために設けられている。
【００５４】
ステップ４０１の判定がＮＯになると、ステップ４０２にてＧＰＳ３からの測位データがあるか否かを行う。ＧＰＳ３からの測位データがあると、ステップ４０３以降のカルマンフィルタの演算処理に進む。
まず、ステップ４０３にて観測値Ｙの計算を行う。これは、ＧＰＳ３から出力される速度、位置、方位データおよび推測航法におけるステップ３００の処理にて求めた絶対方位、絶対位置および図示しない速度演算処理により車速センサ１からの車速パルスに基づく車両の速度とから、数１２に示した、εＡＤＲｔ  −εＡＧＰＳｔ、εＫＤＲｔ  −εＫＧＰＳｔ、εＹＤＲｔ  −εＹＧＰＳｔ、εＸＤＲｔ  −εＸＧＰＳｔを計算するとともに、数１３に示す、観測過程で発生する雑音ｖをＧＰＳ３の測位データ等を基に計算する。
【００５５】
ステップ４０４では、プロセス行列φの計算を行う。これは、前回のプロセス行列の計算時点からの移動距離Ｌ、経過時間Ｔ（これらは図示しない計測処理により別途求められている）およびステップ３０１にて求めた絶対方位Ａにより、数１１に示すプロセス行列φを求める。
このようにして計算した観測値Ｙおよびプロセス行列φを基に、上述した数３〜数５の計算を行って数１４に示す状態量Ｘを求める。すなわち、ステップ４０５では、数３により誤差共分散Ｐの予測計算を行う。ステップ４０６では、数４によりカルマンゲインＫの計算を行う。ステップ４０７では、数５により誤差共分散Ｐの計算を行う。この後、カルマンゲインＫおよび観測値Ｙに基づき、ステップ４０８にて、数１４の計算により状態量Ｘを求める。この状態量Ｘは、数１１の左辺に示すように、オフセット誤差（εＧ）、絶対方位誤差（εＡ）、距離係数誤差（εＫ）、絶対位置北方向誤差（εＹ）、絶対位置東方向誤差（εＸ）を表している。
【００５６】
これらの誤差により、ステップ４０９にて、図に示す計算にて推測航法誤差の修正、すなわちジャイロ２のオフセット補正、車速センサ１の距離係数補正、絶対方位補正、絶対位置補正が行われる。ジャイロ２のオフセット補正により、ステップ１０２にて用いられるオフセット補正量が修正され、車速センサ１の距離係数補正により、ステップ１０３にて用いられる距離係数が修正され、絶対方位補正により、ステップ３０１にて用いられる絶対方位Ａが修正され、絶対位置補正によりステップ３０２にて用いられる絶対位置が修正される。
【００５７】
上記の処理を、ＧＰＳ３からの測位データが有る毎に繰り返し行い、上記誤差修正を行って、より正確なる推測航法データを得ることができる。
また、ＧＰＳ３の測位が長時間できない場合で、ステップ４０１の判定がＹＥＳとなると、ステップ４１０、４１１に進み、プロセス行列φの計算および誤差共分散Ｐの予測計算を行う。これによって、ＧＰＳ３の測位ができない場合の誤差に対応した誤差共分散の予測計算を行い、その後にＧＰＳ３が測位できた時に行われるカルマンフィルタの処理を正確に行えるようにする。
【００５８】
上記したカルマンフィルタによる誤差修正においてはＧＰＳ３からの測位データが必要となる。しかしながら、ＧＰＳ３が長時間測位できない場合は、センサ誤差が大きくなるため、センサ誤差を何らかの方法で補正する必要がある。
そこで、絶対方位に対しては地磁気センサ７を用いて補正を行う。この場合、推測航法の絶対方位と地磁気方位を比較し、差が一定値以上ある場合に推測航法の絶対方位を地磁気方位に合わせるようにすることができる。
【００５９】
このような補正により、方位の誤差は減少するが、方位精度の予測値は大きなままであり、信頼性のないデータとなる。そこで、絶対方位を地磁気方位に補正した時に、絶対方位の精度予測値を地磁気センサで期待できる値まで減少させることが考えられる。しかしながら、この場合、絶対方位分散の部分のみ小さくすると、絶対方位誤差と他の状態量共分散の大きさの関係が異常になり、次にＧＰＳ３が測位しカルマンフィルタが動作した時に、間違った補正をし更に誤差共分散が負の数字となり演算不能になってしまう。
【００６０】
そこで、地磁気方位を利用して絶対方位の補正を行った時には、その補正により、絶対方位誤差と、絶対位置の誤差、オフセット誤差および距離係数誤差とのそれぞれの関係は相関のないものとなるため、誤差共分散行列の内の絶対方位とそれ以外の状態量との共分散項の相互相関値をクリア（０を設定）する。
具体的には、図８に示す処理を行う。
【００６１】
図８において、ステップ６０１にて絶対方位精度が地磁気センサで期待できる精度より低いか否かを判定する。ここで、絶対方位精度は誤差共分散行列におけるσＡＡ  ２により把握することができる。また、地磁気センサで期待できる精度は実験等で求めることができ、比較判定に際しては実験等で求めた値とすることができるが、車両が着磁しているような場合にはその値をそれに応じて変化させるようにしてもよい。従って、このステップ６０１ではσＡＡ  ２とその比較値が比較される。
【００６２】
このステップ６０１の判定がＹＥＳになると、ステップ６０２にて、絶対方位Ａを地磁気センサ７にて検出した方位に補正する。この後、ステップ６０３にて数１５に示す誤差共分散行列の内、σＡＡ  ２を地磁気センサで期待できる方位精度の分散値に設定するとともに、σＡＧ  ２、σＡＫ  ２、σＡＹ  ２、σＡＸ  ２、σＧ  Ａ  ２  、σＫＡ  ２、σＹＡ  ２、σＸＡ  ２の各値を０に設定する。
【００６３】
上記のような制御により、地磁気センサ７による補正の効果を精度予測値に反映させることができるとともに、次にＧＰＳ３が測位でき補正が行われた場合にも正常な補正が可能となる。
次に、ジャイロオフセットの補正について説明する。ＧＰＳ３が長時間測位できず推測航法のオフセット補正精度が低下し、車両停止時に行うジャイロオフセット補正で期待できるオフセット補正精度より低い精度となった場合は、車両停止によるオフセット補正が可能となった時に、推測航法のオフセット補正量を更新する。
【００６４】
この時、誤差共分散行列のオフセット補正誤差分散の部分を車両停止時の補正で期待できる値まで小さくする。
また、車両停止時のオフセット補正を行ったため、オフセット補正誤差とその他の状態量の誤差の関係は相関のないものとなるため、誤差共分散行列のオフセット補正誤差とそれ以外の状態量との共分散項の相互相関値をクリア（０に設定）する。
【００６５】
具体的には、図９に示す処理を行う。
図９において、ステップ７０１にてオフセット補正精度が車両停止時のオフセット補正で期待できる精度より低いか否かを判定する。ここで、オフセット補正精度は誤差共分散行列におけるσＧＧ２  により把握することができる。また、車両停止時のオフセット補正で期待できる精度は実験等で求めた値とすることができるため、σＧＧ２  とその比較値が比較される。
【００６６】
このステップ７０１の判定がＹＥＳになると、ステップ７０２にて、車両停止時、すなわち車速センサ１からの信号に基づいて車両停止を判定し、この時のジャイロ３の出力を求めてその値を０にするような値にオフセット補正量を補正する。この後、ステップ７０３にて数１５に示す誤差共分散行列の内、σＧＧ２  を車両停止時のオフセット補正で期待できるオフセット補正精度の分散値に設定するとともに、σＧＡ  ２、σＧＫ  ２、σＧＹ  ２、σＧＸ  ２、σＡＧ  ２、σＫＧ２  、σＹＧ２  、σＸＧ２  の各値を０に設定する。
【００６７】
上記のような制御により、車両停止時のオフセット補正の効果を精度良く値に反映させることができるとともに、次にＧＰＳ３が測位でき補正が行われた場合にも正常な補正が可能となる。
【図面の簡単な説明】
【図１】本発明の一実施例を示す概略構成図である。
【図２】推測航法のメインルーチンの演算処理を示すフローチャートである。
【図３】方位変化量・移動距離の演算処理を示すフローチャートである。
【図４】相対軌跡の演算処理を示すフローチャートである。
【図５】絶対方位・絶対位置の演算処理を示すフローチャートである。
【図６】ＧＰＳとの複合化処理を示すフローチャートである。
【図７】カルマンフィルタのモデルを示す構成図である。
【図８】地磁気方位を利用して絶対方位の補正を行う処理を示すフローチャートである。
【図９】車両停止時のオフセット補正を行う処理を示すフローチャートである。
【符号の説明】
１  距離センサ
２  相対方位センサ
３  ＧＰＳ
４  相対軌跡演算部
５  絶対位置演算部
６  カルマンフィルタ
７  地磁気センサ
【図１】
0003628046.tif 000006
【図２】
0003628046.tif 000007
【図３】
0003628046.tif 000008
【図４】
0003628046.tif 000009
【図５】
0003628046.tif 000010
【図６】
0003628046.tif 000011
【図７】
0003628046.tif 000012
【図８】
0003628046.tif 000013
【図９】
0003628046.tif 000014