JPH0868653A - 車両用現在位置検出装置 - Google Patents

Abstract

(57)【要約】 【目的】 ＧＰＳにより推測航法の補正を行うようにし
た場合に、推測航法における相対方位センサ、距離セン
サそれぞれにメインセンサ、サブセンサを設けて切換使
用する。 【構成】 距離センサ１、相対方位センサ２、相対軌跡
演算部４、絶対軌跡演算部５による推測航法に対し、推
測航法から求められる車両の位置、方位、車速の情報と
ＧＰＳ３から出力される車両の位置、方位、車速の情報
により、カルマンフィルタ６にて、推測航法におけるオ
フセット誤差、距離係数誤差、絶対方位誤差、絶対位置
誤差を求めて補正する。ここで、距離センサ１、相対方
位センサ２として車輪センサをメインにし、車速セン
サ、ステアリングセンサをそれぞれのサブセンサとして
切換使用する。その場合に、車輪センサからの信号によ
り得られるデータを基に、車速センサの距離係数、ステ
アリングセンサのセンタ値を求める。

Description

【発明の詳細な説明】

【０００１】

【産業上の利用分野】本発明は、車両の方位および移動
距離により車両の現在位置を検出する車両用現在位置検
出装置に関する。

【０００２】

【従来の技術】従来、この種の車載用ナビゲーション装
置においては、車両の方位変化量を検出する相対方位セ
ンサ（ジャイロ、ステアリングセンサ、車輪センサ等）
と、車両の速度（距離）を検出する距離センサ（車速セ
ンサ、車輪センサ等）の出力から、車両の位置・方位・
車速等を検出する推測航法が用いられている。

【０００３】この推測航法の出力（位置・方位・車速
等）には、センサの誤差が含まれるため、誤差が生じ
る。特に、位置・方位は積分的に求められるため、誤差
が徐々に増大してしまう。これに対し、ＧＰＳは、絶対
的な位置・方位・車速を求めることができるため、ＧＰ
Ｓが測位した場合に推測航法の出力をＧＰＳの出力に合
わせることにより補正が可能である。例えば、推測航法
で得られた位置をマップマッチングにより道路地図上の
道路位置に位置合わせした時の位置と、ＧＰＳで得られ
た位置との差が所定値より大きくなった時に、道路地図
上の位置をＧＰＳで得られた位置に修正するようにする
ことができる。

【０００４】

【発明が解決しようとする課題】しかしながら、推測航
法で求められる位置については、ＧＰＳの出力により補
正することはできるが、センサの補正はできない。この
ため、ＧＰＳ非受信時においてセンサ出力の誤差により
推測航法の出力精度が悪いという問題がある。本発明
は、センサ出力の誤差を修正して現在位置を求めるよう
にすることを第１の目的とする。

【０００５】本発明者等は、上記目的を達成するため、
推測航法から求められる車両の方位、速度に関する情報
と、ＧＰＳから出力される車両の方位、車両の速度に関
する情報とのそれぞれの差に基づいて、センサ誤差量を
求め、センサ出力の誤差修正を行うものを考えた。この
ものの詳細については後述する。しかしながら、相対方
位センサとして、左右の車輪の車輪センサをメインのセ
ンサとして車両の方位、移動距離を求めるようにした場
合、車輪センサは車両の走行速度が低いと精度上問題が
あり、このため他のサブセンサとの切換使用が考えられ
る。この場合、相対方位センサとしてはステアリングセ
ンサが、また距離センサとしては車速センサが考えられ
る。

【０００６】このような場合に、それぞれのサブセンサ
に対してもメインセンサと特性を合わせる必要があり、
その場合にサブセンサに対する誤差量の演算を独自に行
っていたのではそれに対する演算量が多くなる。そこ
で、本発明は、相対方位センサ及び／又は距離センサに
メインセンサとサブセンサを設けて切換使用する場合の
サブセンサの誤差量演算を容易に行うようにすることを
第２の目的とする。

【０００７】

【課題を解決するための手段】本発明は上記目的を達成
するため、請求項１に記載の発明においては、車両の方
位変化量に応じた信号を出力する相対方位センサ（２）
と、車両の移動距離を検出する移動距離検出手段（１お
よび１０３）と、前記相対方位センサからの信号に基づ
き、オフセット補正量によりオフセット補正して方位変
化量を求めるとともにこの方位変化量により車両の方位
を特定し、この車両の方位と前記距離検出手段にて検出
した車両の移動距離に基づいて車両の位置を検出する位
置検出手段（４，５およびそれに対する演算処理）とか
ら構成される推測航法手段（１，２，４，５およびそれ
に対する演算処理）と、ＧＰＳ衛星からの衛星電波を受
信して車両の方位に関する情報を出力するＧＰＳ（３）
と、前記推測航法手段における車両の方位に関する情報
と前記ＧＰＳから出力される車両の方位に関する情報と
の差に基づいて、オフセット誤差を推定する推定手段
（６およびそれに対する４０１〜４０８，４１０，４１
１の処理）と、この推定手段にて推定したオフセット誤
差により前記オフセット補正量を修正する修正手段（４
０９）とを備え、前記相対方位センサは、車両の方位変
化量に応じた信号を出力する第１、第２のセンサを有
し、それぞれのセンサは車両の方位に関する異なる物理
量を検出するものであって、前記位置検出手段は、前記
第１のセンサからの信号により求められる方位変化量と
前記第２のセンサからの信号により求められる方位変化
量を所定のタイミングで切換使用するとともに、前記第
１の方位センサからの信号により求められる方位変化量
に対し前記オフセット補正して得られた方位変化量と前
記第２のセンサからの信号により求められる方位変化量
とが一致するように、前記第２のセンサに対するオフセ
ット補正量を設定する設定手段（５０１〜５０６）を有
することを特徴としている。

【０００８】請求項２に記載の発明においては、車両の
方位変化量に応じた信号を出力する相対方位センサ
（２）と、車両の移動距離を検出する移動距離検出手段
（１および１０３）と、前記相対方位センサからの信号
に基づき、オフセット補正量によりオフセット補正して
方位変化量を求めるとともにこの方位変化量により車両
の方位を特定し、この車両の方位と前記距離検出手段に
て検出した車両の移動距離に基づいて車両の位置を検出
する位置検出手段（４，５およびそれに対する演算処
理）とから構成される推測航法手段（１，２，４，５お
よびそれに対する演算処理）と、ＧＰＳ衛星からの衛星
電波を受信して車両の方位に関する情報を出力するＧＰ
Ｓ（３）と、前記推測航法手段における車両の方位に関
する情報と前記ＧＰＳから出力される車両の方位に関す
る情報との差に基づいて、オフセット誤差を推定する推
定手段（６およびそれに対する４０１〜４０８，４１
０，４１１の処理）と、この推定手段にて推定したオフ
セット誤差により前記オフセット補正量を修正する修正
手段（４０９）とを備え、前記相対方位センサは、車両
の左右輪の回転に応じた信号を出力する車輪センサと車
両のステアリング操作量に応じた信号を出力するステア
リングセンサにより構成されるものであって、前記位置
検出手段は、前記車輪センサからの信号により求められ
る方位変化量と前記ステアリングセンサにより求められ
る方位変化量を所定のタイミングで切換使用するととも
に、前記車輪センサからの信号により求められる方位変
化量に対し前記オフセット補正して得られた方位変化量
を基に、前記ステアリングセンサに対するステアリング
・センタ値を設定する設定手段（５０１〜５０６）を有
して、ステアリング・センタ補正した値にて前記ステア
リングセンサによる方位変化量を求めるものであること
を特徴としている。

【０００９】ここで、上記車両の方位に関する情報と
は、方位そのものに限らず、方位に関係する他の情報も
含む概念であり、後述するように方位と相関のある絶対
位置等も含まれるものである。請求項３に記載の発明で
は、請求項１又は２に記載の発明において、前記推定手
段は、オフセット誤差および車両の方位誤差を状態量Ｘ
（ｔ）とし、この誤差の時間的変化を与えるプロセス行
列φおよび信号生成過程で発生する雑音ωにより、状態
量Ｘ（ｔ＋１）をφ・Ｘ（ｔ）＋ωにて関係付ける信号
生成過程と、状態量Ｘ（ｔ）と観測値Ｙ（ｔ）とを、観
測行列Ｈおよび観測過程で発生する雑音ｖにより、観測
値Ｙ（ｔ）をＨ・Ｘ（ｔ）＋ｖにて関係付ける観測過程
を形成するモデルを基に、前記推測航法手段における車
両の方位に関する情報と前記ＧＰＳから出力される車両
の方位に関する情報との差により、上記観測値Ｙ（ｔ）
を演算するとともに、その演算値を基に上記モデルから
状態量Ｘ（ｔ）を演算する状態量演算手段を有すること
を特徴としている。

【００１０】請求項４に記載の発明においては、車両の
方位を検出する方位検出手段（２，１０１，１０２）
と、車両の走行速度に応じた信号を出力する距離センサ
（１）と、前記距離センサからの信号により距離係数を
乗じて車両の移動距離を求め、前記方位検出手段にて検
出した車両の方位と前記移動距離に基づいて車両の位置
を検出する位置検出手段（４，５およびそれに対する演
算処理）と、から構成される推測航法手段（１，２，
４，５およびそれに対する演算処理）と、ＧＰＳ衛星か
らの衛星電波を受信して車両の速度に関する情報を出力
するＧＰＳ（３）と、前記推測航法手段における車両の
速度に関する情報と前記ＧＰＳから出力される車両の速
度に関する情報との差に基づいて、距離係数誤差を推定
する推定手段（６およびそれに対する４０１〜４０８，
４１０，４１１の処理）と、この推定手段にて推定した
距離係数誤差により前記距離係数を修正する修正手段
（４０９）とを備え、前記距離センサは、車両の走行速
度に応じた信号を出力する第１、第２のセンサを有し、
それぞれのセンサは車両の走行速度に関する異なる物理
量を検出するものであって、前記位置検出手段は、前記
第１のセンサからの信号により求められる車両の移動距
離と前記第２のセンサにより求められる車両の移動距離
を所定のタイミングで切換使用するとともに、前記第１
のセンサからの信号に対し前記修正された距離係数を乗
じて得られる移動距離と第２のセンサからの信号により
求められる移動距離が一致するように前記第２のセンサ
に対する距離係数を設定する設定手段（６０１〜６０
５）を有することを特徴としている。

【００１１】請求項５に記載の発明においては、車両の
方位を検出する方位検出手段（２，１０１，１０２）
と、車両の走行速度に応じた信号を出力する距離センサ
（１）と、前記距離センサからの信号により距離係数を
乗じて車両の移動距離を求め、前記方位検出手段にて検
出した車両の方位と前記移動距離に基づいて車両の位置
を検出する位置検出手段（４，５およびそれに対する演
算処理）と、から構成される推測航法手段（１，２，
４，５およびそれに対する演算処理）と、ＧＰＳ衛星か
らの衛星電波を受信して車両の速度に関する情報を出力
するＧＰＳ（３）と、前記推測航法手段における車両の
速度に関する情報と前記ＧＰＳから出力される車両の速
度に関する情報との差に基づいて、距離係数誤差を推定
する推定手段（６およびそれに対する４０１〜４０８，
４１０，４１１の処理）と、この推定手段にて推定した
距離係数誤差により前記距離係数を修正する修正手段
（４０９）とを備え、前記距離センサは、車輪の回転に
応じた信号を出力する車輪センサと車両の走行速度に応
じた車速パルスを発生する車速センサにより構成される
ものであって、前記位置検出手段は、前記車輪センサか
らの信号により求められる車両の移動距離と前記車速セ
ンサにより求められる車両の移動距離を所定のタイミン
グで切換使用するとともに、前記車輪センサからの信号
に対し前記修正された距離係数を乗じて得られる移動距
離と車速センサからの車速パルス数による移動距離が一
致するように車速センサに対する距離係数を設定する設
定手段（６０１〜６０５）を有することを特徴としてい
る。

【００１２】ここで、上記車両の速度に関する情報と
は、速度そのものに限らず、速度に関係する他の情報も
含む概念であり、後述するように速度と相関のある絶対
位置等も含まれるものである。請求項６に記載の発明で
は、請求項４又は５に記載の発明において、前記推定手
段は、距離係数誤差を状態量Ｘ（ｔ）とし、この誤差の
時間的変化を与えるプロセス行列φおよび信号生成過程
で発生する雑音ωにより、状態量Ｘ（ｔ＋１）をφ・Ｘ
（ｔ）＋ωにて関係付ける信号生成過程と、状態量Ｘ
（ｔ）と観測値Ｙ（ｔ）とを、観測行列Ｈおよび観測過
程で発生する雑音ｖにより、観測値Ｙ（ｔ）をＨ・Ｘ
（ｔ）＋ｖにて関係付ける観測過程を形成するモデルを
基に、前記推測航法手段における車両の速度に関する情
報と前記ＧＰＳから出力される車両の速度に関する情報
との差により、上記観測値Ｙ（ｔ）を演算するととも
に、その演算値を基に上記モデルから状態量Ｘ（ｔ）を
演算する状態量演算手段を有することを特徴としてい
る。

【００１３】なお、上記各手段のカッコ内の符号は、後
述する実施例記載の具体的手段との対応関係を示すもの
である。また、後述するフローチャート内の各ステップ
はそれぞれの機能を実現する機能実現手段として構成す
るものである。

【００１４】

【発明の作用効果】請求項１〜３に記載の発明によれ
ば、推測航法から求められる車両の方位に関する情報
と、ＧＰＳから出力される車両の方位に関する情報との
それぞれの差に基づいて、オフセット誤差を求め、相対
方位センサのオフセット補正行うようにしているから、
相対方位センサに対するセンサ出力の誤差修正を行うこ
とができる。

【００１５】さらに、この場合に相対方位センサとし
て、第１、第２のセンサ（請求項２においては左右の車
輪の車輪センサとステアリングセンサ）を用い、第１の
センサからの信号により求められる方位変化量に対しオ
フセット補正して得られた方位変化量と、第２のセンサ
から求められる方位変化量とが一致するように、第２の
センサに対するオフセット補正量を設定するようにして
いから、第２のセンサのオフセット補正量を容易に求め
ることができる。

【００１６】請求項４〜６に記載の発明によれば、推測
航法から求められる車両の速度に関する情報と、ＧＰＳ
から出力される車両の速度に関する情報とのそれぞれの
差に基づいて、距離係数誤差を求め、距離センサに対す
るセンサ出力の誤差修正を行うことができる。さらに、
この場合に距離センサとして、第１、第２のセンサ（請
求項５においては車輪センサと車速センサ）を用い、第
１のセンサからの信号に対し修正された距離係数を乗じ
て得られる移動距離と第２のセンサから求められる移動
距離が一致するように第２のセンサに対する距離係数を
設定するようにしているから、第１のセンサに対する距
離係数を第２のセンサに対する距離係数に合わせ込ん
で、それを容易に求めることができる。

【００１７】

【実施例】以下、本発明を図に示す実施例について説明
する。この実施例は、推測航法とＧＰＳとの複合化を図
るため、カルマンフィルタを用いている。このカルマン
フィルタの概要について説明する。このカルマンフィル
タにおいては、図９に示すように、信号生成過程と観測
過程に分けられる。図において、線形システム（φ）が
あり、そのシステムの状態Ｘ（ｔ）に対して、観測行列
Ｈで関係付けられるＸ（ｔ）の一部が観測できる場合
に、フィルタはＸ（ｔ）の最適な推定値を与える。ここ
で、ωは信号生成過程にて発生する雑音であり、ｖは観
測過程にて発生する雑音である。このフィルタの入力は
Ｙ（ｔ）であり、出力はＸ（ｔ）の最適推定値である。

【００１８】時刻ｔまでの情報を用いた状態Ｘの最適推
定値、すなわち状態量Ｘ（ｔ｜ｔ）は、数１により求め
られる。

【００１９】

【数１】Ｘ（ｔ｜ｔ）＝Ｘ（ｔ｜ｔ−１）＋Ｋ（ｔ）
｛Ｙ（ｔ）−ＨＸ（ｔ｜ｔ−１）｝ ここで、Ｘ（ｔ｜ｔ−１）は事前推定値、Ｋ（ｔ）はカ
ルマンゲインであり、それぞれ数２、数３により表され
る。

【００２０】

【数２】Ｘ（ｔ｜ｔ−１）＝φＸ（ｔ−１｜ｔ−１）

【００２１】

【数３】Ｋ（ｔ）＝Ｐ（ｔ｜ｔ−１）Ｈ^T （ＨＰ（ｔ｜
ｔ−１）Ｈ^T ＋Ｖ）^-1 ここで、Ｐは状態量Ｘの誤差共分散であり、Ｐ（ｔ｜ｔ
−１）は誤差共分散の予測値、Ｐ（ｔ−１｜ｔ−１）は
誤差共分散であり、それぞれ数４、数５により表され
る。

【００２２】

【数４】 Ｐ（ｔ｜ｔ−１）＝φＰ（ｔ−１｜ｔ−１）φ^T ＋Ｗ

【００２３】

【数５】Ｐ（ｔ−１｜ｔ−１）＝（Ｉ−Ｋ（ｔ−１）
Ｈ）Ｐ（ｔ−１｜ｔ−２） なお、Ｖは観測過程で発生する雑音ｖの分散、Ｗは信号
過程で発生する雑音ωの分散である。また、Ａ（ｉ｜
ｊ）は時刻ｊまでの情報に基づく時刻ｉでのＡの推定値
を表す。なお、添字の^T は転置行列を意味し、^-1は逆行
列を意味する。Ｉは単位行列である。

【００２４】さらに、ＶとＷは平均０の白色ガウス雑音
であり、互いに無相関である。上記のようなカルマンフ
ィルタにおいて、状態量Ｘと誤差共分散Ｐの初期値に適
当な誤差を与えてやり、新しい観測が行われる度に以上
の計算を繰り返し行うことにより、状態量Ｘの精度が向
上する。このようなカルマンフィルタを推測航法へ適用
したのが本実施例である。

【００２５】まず、上記の信号生成過程の定義について
説明する。推測航法でのカルマンフィルタは、推測航法
の誤差の補正を目的とするので、状態量Ｘは以下の５つ
の誤差値を定義する。この誤差値の時間的な変化を与え
るものがプロセス行列φである。 オフセット誤差（εＧ）

【００２６】

【数６】εＧ_t ＝εＧ_t-1 ＋ω₀ 確定的な変化はなく、前回の誤差にノイズが付加され
る。 絶対方位誤差（εＡ）

【００２７】

【数７】εＡ_t ＝Ｔ×εＧ_t-1 ＋εＡ_t-1 ＋ω₁ 前回の誤差に、オフセット誤差に前回からの経過時間を
かけて求める方位誤差とノイズが付加される。 距離係数誤差（εＫ）

【００２８】

【数８】εＫ_t ＝εＫ_t-1 ＋ω₂ 確定的な変化はなく、前回の誤差にノイズが付加され
る。 絶対位置北方向誤差（εＹ）

【００２９】

【数９】εＹ_t ＝ｓｉｎ（Ａ_T ＋εＡ_t-1 ＋εＧ_t-1 ×
Ｔ／２）×Ｌ×（１＋εＫ_t-1 ）−ｓｉｎ（Ａ_T ）×Ｌ
＋εＹ_t-1 前回の誤差に方位誤差・距離誤差によって生じる誤差が
付加される。 絶対位置東方向誤差（εＸ）

【００３０】

【数１０】εＸ_t ＝ｃｏｓ（Ａ_T ＋εＡ_t-1 ＋εＧ_t-1
×Ｔ／２）×Ｌ×（１＋εＫ_t-1 ）−ｃｏｓ（Ａ_T ）×
Ｌ＋εＸ_t-1 前回の誤差に方位誤差・距離誤差によって生じる誤差が
付加される。上記の定義において、Ａ_T は真の絶対方
位、Ｌは前回からの移動距離、Ｔは前回からの経過時間
である。

【００３１】上記の各式を状態量で偏微分し線形化する
と信号生成過程は以下のように定義される。

【００３２】

【数１１】

【００３３】上記Ａは、絶対方位Ａ_T ＋εＡ_t-1 ＋εＧ
_t-1 ×Ｔ／２を意味する。この値は真の絶対方位Ａ_T に
センサ誤差が加わったものであり、後述するように、方
位変化量から求められる絶対方位Ａとする。また、ω₀
は、オフセット雑音（温度ドリフト等によるオフセット
の変動分）、ω₁ は絶対方位雑音（ジャイロのゲイン的
な誤差）、ω₂ は距離係数雑音（経年変化）を意味す
る。

【００３４】次に、上記観測過程の定義について説明す
る。観測値は推測航法の出力と、ＧＰＳの出力の差より
求める。それぞれの出力には誤差が含まれるため、観測
値において、推測航法の誤差とＧＰＳの誤差の和が得ら
れる。この観測値Ｙと状態量Ｘを関係付け、数１２のよ
うに定義される。

【００３５】

【数１２】

【００３６】但し、観測過程で発生する雑音ｖはＧＰＳ
の雑音であり、数１３のように定義される。

【００３７】

【数１３】

【００３８】以上の定義を基に、カルマンフィルタを用
いた推測航法について説明する。図１に本実施例におけ
る概略構成を示す。この図に示すように、距離センサ
１、相対方位センサ２からの信号を基に、相対軌跡演算
部４、絶対位置演算部５での演算が行われ、それらの演
算（推測航法演算）により、車速、相対軌跡、絶対位
置、絶対方位が出力される。また、ＧＰＳ３からは位置
・方位・車速の出力が得られる。カルマンフィルタ６
は、推測航法により得られた車速、絶対位置・絶対方位
の情報およびＧＰＳ３からの車速、位置・方位の情報を
基に、距離センサ１の距離係数補正、相対方位センサ２
のオフセット補正、絶対位置補正、絶対方位補正を行
う。なお、本実施例においては、距離センサ１として車
速センサおよび左右輪の車輪センサを用い、相対方位セ
ンサ２として左右輪の車輪センサおよびステアリングセ
ンサを用いている。

【００３９】このような車載用ナビゲーション装置へカ
ルマンフィルタを適用すると、距離センサ１の距離係数
補正、相対方位センサ２のオフセット補正、および絶対
方位補正、絶対位置補正により、数２に示す、事前推定
Ｘ（ｔ｜ｔ−１）は０となる。従って、数１は数１４に
示すようになる。

【００４０】

【数１４】Ｘ（ｔ｜ｔ）＝Ｋ（ｔ）Ｙ（ｔ） 従って、上記信号生成過程にて定義された５つの誤差値
による状態量Ｘは、数３〜数５によって求められるカル
マンゲインＫ（ｔ）および観測値Ｙ（ｔ）により求めら
れる。

【００４１】ここで、数３における誤差共分散Ｐは、数
１５により定義される。

【００４２】

【数１５】

【００４３】この誤差共分散Ｐにおけるσ_GG ² はオフセ
ット誤差の大きさの見積もりを表し、σ_AA ²は絶対方位
誤差の大きさの見積もりを表し、σ_KK ² は距離係数誤差
の大きさの見積もりを表し、σ_YY ² は絶対位置北方向誤
差の大きさの見積もりを表し、σ_XX ² は絶対位置東方向
誤差の大きさの見積もりを表す。それら以外のσ_ij ²は
ｉ行とｊ列の相互相関値を表す。例えばσ_AG ²はオフセ
ット誤差と絶対方位誤差の相互相関値を表す。

【００４４】この誤差共分散Ｐの値は、数４の計算によ
って更新される。なお、初期値においては、σ_GG ² 、σ
_AA ²、σ_KK ² 、σ_YY ² 、σ_XX ² の各値を誤差が最大とな
る値に設定しておき、また相互相関値については全て０
に設定しておく。また、数３におけるＨは数１２で示さ
れる行列を用い、Ｖについては数１３に示されるものを
用いる。また、数４におけるＷは数１１に示されるωの
分散を用いる。

【００４５】観測過程における観測値Ｙとしては、数１
２に示すように、εＡ_DRt −εＡ_{GP St}、εＫ_DRt −εＫ
_GPSt、εＹ_DRt −εＹ_GPSt、εＸ_DRt −εＸ_GPStを用い
ている。ここで、添字の_DRt は時刻ｔにおいて距離セン
サ１、相対方位センサ２からの信号に基づく推測航法に
て求められた値を意味し、_GPStは時刻ｔにおいてＧＰＳ
３から出力される値を意味する。

【００４６】εＡ_DRt −εＡ_GPStは、推測航法により求
められた絶対方位とＧＰＳ３から出力される方位の差、
すなわち推測航法により求められた絶対方位には真の絶
対方位とその誤差εＡ_DRt が含まれており、またＧＰＳ
３から出力される方位には真の絶対方位とその誤差εＡ
_GPStが含まれているため、それらの差を取ることにより
εＡ_DRt −εＡ_GPStが得られる。

【００４７】同様に、εＫ_DRt −εＫ_GPStは、推測航法
により求められる速度とＧＰＳ３から出力される速度の
差から求まる距離係数誤差であり、具体的には、（推測
航法による速度−ＧＰＳによる速度）／（推測航法によ
る速度）により求められる。また、εＹ_DRt −εＹ_GPSt
は、推測航法により求められる絶対位置のＹ成分とＧＰ
Ｓ３から出力される位置のＹ成分の誤差の差であり、ε
Ｘ_DRt −εＸ_GPStは、推測航法により求められる絶対位
置のＸ成分とＧＰＳ３から出力される位置のＸ成分の誤
差の差である。

【００４８】また、数１３に示す、観測過程で発生する
雑音ｖはＧＰＳ３の雑音であり、以下のようにして求め
られる。ＧＰＳ３における擬似距離の計測誤差（ＵＥＲ
Ｅ）とＨＤＯＰ（Horizontal Dilution of Presision）
の関係により測位精度が、ＵＥＲＥ×ＨＤＯＰで求めら
れ、この測位精度を２乗することにより、ｖ_2t、ｖ_3tが
求められる。また、ドップラー周波数の計測誤差とＨＤ
ＯＰの関係より速度精度が、ドップラー周波数の計測誤
差×ＨＤＯＰで求められ、この速度精度／車速にて距離
係数計測誤差が求められ、これを２乗することによりｖ
_1tが求められる。さらに、車両の速度Ｖc と速度精度か
ら方位精度がｔａｎ^-1（速度精度／Ｖc）で求められ、
この方位精度を２乗するこによりｖ_0tが求められる。

【００４９】従って、観測過程におけるεＡ_DRt −εＡ
_GPSt、εＫ_DRt −εＫ_GPSt、εＹ_{DR t} −εＹ_GPSt、εＸ
_DRt −εＸ_GPStおよび上記雑音Ｖを入力とし、数３〜数
５および数１を実行することにより、信号生成過程にて
定義された５つの誤差値による状態量Ｘが求められ、こ
れらにより距離センサ１の距離係数補正、相対方位セン
サ２のオフセット補正、絶対位置補正、絶対方位補正が
行われる。

【００５０】上記の相対軌跡演算、絶対位置演算、カル
マンフィルタはマイクロコンピュータによる演算処理に
て行われるため、以下これについて説明する。図２に推
測航法のメインルーチンの演算処理を示す。ステップ１
００にて方位変化量・移動距離の演算を行う。この処理
の詳細を図３に示す。まず、ステップ１０１にて相対方
位センサ２の方位変化量を求める。ここで、相対方位セ
ンサ２としては左右輪の車輪センサとステアリングセン
サを用いているため、左右輪の車輪センサからの信号に
基づきそのセンサ信号の平均により車速を求め、その車
速が所定値以下の低速時にはステリングセンサの出力お
よび前回からの移動距離により方位変化量を求め、車速
が所定値より大きい時には左右の車輪センサから方位変
化量を求める。この左右の車輪センサからの方位変化量
の求め方については公知のように、アッカーマン・ジャ
ント理論に基づき内輪と外輪の移動距離により回転半径
を求め、これとホイールベースにより方位変化量を求め
る。

【００５１】次のステップ１０２にて、その方位変化量
から、オフセット補正量（この補正量については後述す
る）に前回からの移動距離Ｌを掛けたものを引き、方位
変化量のオフセット補正を行う。次のステップ１０３で
は、上記したように左右の車輪センサからの求まる車速
により車速が所定値より大きい時には、車輪センサから
の車輪パルス数（左右の車輪センサの平均）に距離係数
（この距離係数についても後述する）を掛けて移動距離
を算出する。また、車速が所定値以下の時の低速時に
は、車速センサからの車速パルスに距離係数を掛けて移
動距離を算出する。この車速センサからの車速パルスに
対する距離係数は後述する図８に示す処理にて車輪セン
サからの車輪パルスに対する距離係数を基に算出され
る。この点については後述する。

【００５２】このステップ１００の次に、ステップ２０
０の相対軌跡演算処理を行う。この処理の詳細を図４に
示す。まず、ステップ２０１にて、方位変化量（ステッ
プ１０２にて求めたもの）を基に相対方位を更新する。
この更新した相対方位およびステップ１０３にて求めた
移動距離によりステップ２０２にて相対位置座標の更新
を行う。この更新は、移動距離に対する相対方位のＸ，
Ｙ成分をそれまでの相対位置座標に加算することにより
行う。この相対位置座標は相対軌跡を求めるたに行うも
ので、その相対軌跡と道路形状との関係により、いわゆ
るマップマッチングが行われる。

【００５３】このステップ２００の次に、ステップ３０
０の絶対方位・絶対位置の演算処理を行う。この処理の
詳細を図５に示す。まず、ステップ３０１にて、方位変
化量（ステップ１０２にて求めたもの）を基に絶対方位
を更新する。この更新した絶対方位およびステップ１０
３にて求めた移動距離によりステップ２０２にて絶対位
置座標の更新を行う。このステップ２００の処理にて更
新された絶対方位Ａと絶対位置は後述するＧＰＳとの複
合化処理にて利用される。

【００５４】このＧＰＳとの複合化処理を行うステップ
４００の詳細を図６に示す。まず、ステップ４０１にて
前回の測位又は予測計算からＴ1 秒経過したか否かを判
定する。これはＧＰＳ３の測位が行われる毎にステップ
４０３〜４０９にて推測航法の誤差を補正する処理を行
うが、ＧＰＳ３の測位ができない場合には誤差が大きく
なるため、それに対応した誤差の予測計算をステップ４
１０、４１１にて定期的に行うために設けられている。

【００５５】ステップ４０１の判定がＮＯになると、ス
テップ４０２にてＧＰＳ３からの測位データがあるか否
かを行う。ＧＰＳ３からの測位データがあると、ステッ
プ４０３以降のカルマンフィルタの演算処理に進む。ま
ず、ステップ４０３にて観測値Ｙの計算を行う。これ
は、ＧＰＳ３から出力される速度、位置、方位データお
よび推測航法におけるステップ３００の処理にて求めた
絶対方位、絶対位置および図示しない速度演算処理によ
り距離センサ１からのパルス信号に基づく車両の速度と
から、数１２に示した、εＡ_DRt −εＡ _GPSt、εＫ_DRt
−εＫ_GPSt、εＹ_DRt −εＹ_GPSt、εＸ_DRt −εＸ_GPSt
を計算するとともに、数１３に示す、観測過程で発生す
る雑音ｖをＧＰＳ３の測位データ等を基に計算する。

【００５６】ステップ４０４では、プロセス行列φの計
算を行う。これは、前回のプロセス行列の計算時点から
の移動距離Ｌ、経過時間Ｔ（これらは図示しない計測処
理により別途求められている）およびステップ３０１に
て求めた絶対方位Ａにより、数１１に示すプロセス行列
φを求める。このようにして計算した観測値Ｙおよびプ
ロセス行列φを基に、上述した数３〜数５の計算を行っ
て数１４に示す状態量Ｘを求める。すなわち、ステップ
４０５では、数３により誤差共分散Ｐの予測計算を行
う。ステップ４０６では、数４によりカルマンゲインＫ
の計算を行う。ステップ４０７では、数５により誤差共
分散Ｐの計算を行う。この後、カルマンゲインＫおよび
観測値Ｙに基づき、ステップ４０８にて、数１４の計算
により状態量Ｘを求める。この状態量Ｘは、数１１の左
辺に示すように、オフセット誤差（εＧ）、絶対方位誤
差（εＡ）、距離係数誤差（εＫ）、絶対位置北方向誤
差（εＹ）、絶対位置東方向誤差（εＸ）を表してい
る。

【００５７】これらの誤差により、ステップ４０９に
て、図に示す計算にて推測航法誤差の修正、すなわち相
対方位センサ２のオフセット補正、距離センサ１の距離
係数補正、絶対方位補正、絶対位置補正が行われる。相
対方位センサ２のオフセット補正により、ステップ１０
２にて用いられるオフセット補正量が修正され、距離セ
ンサ１の距離係数補正により、ステップ１０３にて用い
られる距離係数が修正され、絶対方位補正により、ステ
ップ３０１にて用いられる絶対方位Ａが修正され、絶対
位置補正によりステップ３０２にて用いられる絶対位置
が修正される。

【００５８】上記の処理を、ＧＰＳ３からの測位データ
が有る毎に繰り返し行い、上記誤差修正を行って、より
正確なる推測航法データを得ることができる。なお、上
記相対方位センサ２としては左右の車輪センサとスアリ
ングセンサを車速に応じて切換使用している。従って、
左右の車輪センサから求められる方位変化量とステアリ
ングセンサから得られる方位変化量を等しくする必要が
ある。そこで、このような方位変化量の合わせ込みを行
うため、図７に示すような処理を行う。

【００５９】まず、ステップ５０１にて走行距離、ステ
アリング・カウンタ積算数、車輪方位変化量をクリアす
る初期設定を行った後、ステップ５０２にて車速が所定
値以上で車輪センサの利用が可能かを判定する。この判
定がＹＥＳになると、ステップ５０３に進み、走行距
離、ステアリング・カウンタ、車輪方位変化量を積算す
る。この車輪方位変化量は、ステップ１０２にてオセッ
ト補正されたものを用いる。この積算をステップ５０４
により規定距離以上の走行が判定されるまで、繰り返し
行う。ステップ５０４の判定がＹＥＳになると、ステッ
プ５０５にて車輪方位変化量は一定値以下か否かを判定
する。この判定がＹＥＳの場合には、車輪方位変化量は
小さい場合なので直線走行とみなし、ステップ５０６に
てステアリング・カウンタ値の単純平均、すなわちステ
アリング・カウンタ積算値／積算回数によりステアリン
グ・センタ値を求める。この求めたステアリング・セン
タ値を基に、ステップ１０１においてステアリングセン
サによる方位変化量を求める場合には、ステアリング・
センタ値により補正して方位変化量を求める。

【００６０】また、上記距離センサ１としては左右の車
輪センサと車速センサを車速に応じて切換使用してい
る。従って、左右の車輪センサから求められる移動距離
と車速センサから求められる移動距離を等しくする必要
がある。そこで、このような移動距離の合わせ込みを行
うため、図８に示すような処理を行う。まず、ステップ
６０１にて走行距離、車速パルス数をクリアする初期設
定を行った後、ステップ６０２にて車速が所定値以上で
車輪センサの利用が可能かを判定する。この判定がＹＥ
Ｓになると、ステップ６０３に進み、車輪走行距離、車
速パルス数を積算する。この車輪走行距離は、ステップ
４０９にて補正された距離係数をもとに求められるもの
を用いる。この積算をステップ６０４により規定距離以
上の走行が判定されるまで、繰り返し行う。ステップ６
０４の判定がＹＥＳになると、ステップ６０５にて車速
センサの距離係数補正を行う。

【００６１】すなわち、ステップ６０２より６０４の繰
り返しにより規定距離走行した時、車輪走行距離は、左
右の車輪センサのパルス数の平均×車輪センサの距離係
数で得られる。この値と、車速パルス数×車速センサの
距離係数が等しくなるようにすれば、左右の車輪センサ
と車速センサを切換えても上記したカルマンフィルタに
よる補正制御が正しく行われる。従って、上記両値が等
しくなるように車速センサの距離係数が左右の車輪セン
サのパルス数の平均×車輪センサの距離係数×／車速パ
ルス数により求められる。

【００６２】従って、車輪センサをメインとし、ステア
リングセンサ、車速センサをサブセンサとした場合に、
それぞれのサブセンサに対してカルマンフィルタが適合
するような個々の演算処理はなくなり、センサが切り換
わった時でも、それまでと同様の処理を行うことができ
る。なお、上記実施例ではカルマンフィルタを用いた誤
差修正を示したが、最小２乗法による制御あるいはフィ
ードバック制御を用いてセンサ等の誤差修正を行うよう
にしてもよい。

【図面の簡単な説明】

【図１】本発明の一実施例を示す概略構成図である。

【図２】推測航法のメインルーチンの演算処理を示すフ
ローチャートである。

【図３】方位変化量・移動距離の演算処理を示すフロー
チャートである。

【図４】相対軌跡の演算処理を示すフローチャートであ
る。

【図５】絶対方位・絶対位置の演算処理を示すフローチ
ャートである。

【図６】ＧＰＳとの複合化処理を示すフローチャートで
ある。

【図７】ステアリング・センタの設定処理を示すフロー
チャートである。

【図８】車速センサの距離係数の設定処理を示すフロー
チャートである。

【図９】カルマンフィルタのモデルを示す構成図であ
る。

【符号の説明】

１ 距離センサ ２ 相対方位センサ ３ ＧＰＳ ４ 相対軌跡演算部 ５ 絶対位置演算部 ６ カルマンフィルタ

Claims (6)

【特許請求の範囲】
1. 【請求項１】 車両の方位変化量に応じた信号を出力す
   る相対方位センサと、 車両の移動距離を検出する移動距離検出手段と、 前記相対方位センサからの信号に基づき、オフセット補
   正量によりオフセット補正して方位変化量を求めるとと
   もにこの方位変化量により車両の方位を特定し、この車
   両の方位と前記距離検出手段にて検出した車両の移動距
   離に基づいて車両の位置を検出する位置検出手段とから
   構成される推測航法手段と、 ＧＰＳ衛星からの衛星電波を受信して車両の方位に関す
   る情報を出力するＧＰＳと、 前記推測航法手段における車両の方位に関する情報と前
   記ＧＰＳから出力される車両の方位に関する情報との差
   に基づいて、オフセット誤差を推定する推定手段と、 この推定手段にて推定したオフセット誤差により前記オ
   フセット補正量を修正する修正手段とを備え、 前記相対方位センサは、車両の方位変化量に応じた信号
   を出力する第１、第２のセンサを有し、それぞれのセン
   サは車両の方位に関する異なる物理量を検出するもので
   あって、 前記位置検出手段は、前記第１のセンサからの信号によ
   り求められる方位変化量と前記第２のセンサからの信号
   により求められる方位変化量を所定のタイミングで切換
   使用するとともに、前記第１の方位センサからの信号に
   より求められる方位変化量に対し前記オフセット補正し
   て得られた方位変化量と前記第２のセンサからの信号に
   より求められる方位変化量とが一致するように、前記第
   ２のセンサに対するオフセット補正量を設定する設定手
   段を有することを特徴とする車両用現在位置検出装置。
2. 【請求項２】 車両の方位変化量に応じた信号を出力す
   る相対方位センサと、 車両の移動距離を検出する移動距離検出手段と、 前記相対方位センサからの信号に基づき、オフセット補
   正量によりオフセット補正して方位変化量を求めるとと
   もにこの方位変化量により車両の方位を特定し、この車
   両の方位と前記距離検出手段にて検出した車両の移動距
   離に基づいて車両の位置を検出する位置検出手段とから
   構成される推測航法手段と、 ＧＰＳ衛星からの衛星電波を受信して車両の方位に関す
   る情報を出力するＧＰＳと、 前記推測航法手段における車両の方位に関する情報と前
   記ＧＰＳから出力される車両の方位に関する情報との差
   に基づいて、オフセット誤差を推定する推定手段と、 この推定手段にて推定したオフセット誤差により前記オ
   フセット補正量を修正する修正手段とを備え、 前記相対方位センサは、車両の左右輪の回転に応じた信
   号を出力する車輪センサと車両のステアリング操作量に
   応じた信号を出力するステアリングセンサにより構成さ
   れるものであって、 前記位置検出手段は、前記車輪センサからの信号により
   求められる方位変化量と前記ステアリングセンサにより
   求められる方位変化量を所定のタイミングで切換使用す
   るとともに、前記車輪センサからの信号により求められ
   る方位変化量に対し前記オフセット補正して得られた方
   位変化量を基に、前記ステアリングセンサに対するステ
   アリング・センタ値を設定する設定手段を有して、ステ
   アリング・センタ補正した値にて前記ステアリングセン
   サによる方位変化量を求めるものであることを特徴とす
   る車両用現在位置検出装置。
3. 【請求項３】 前記推定手段は、オフセット誤差および
   車両の方位誤差を状態量Ｘ（ｔ）とし、この誤差の時間
   的変化を与えるプロセス行列φおよび信号生成過程で発
   生する雑音ωにより、状態量Ｘ（ｔ＋１）をφ・Ｘ
   （ｔ）＋ωにて関係付ける信号生成過程と、状態量Ｘ
   （ｔ）と観測値Ｙ（ｔ）とを、観測行列Ｈおよび観測過
   程で発生する雑音ｖにより、観測値Ｙ（ｔ）をＨ・Ｘ
   （ｔ）＋ｖにて関係付ける観測過程を形成するモデルを
   基に、前記推測航法手段における車両の方位に関する情
   報と前記ＧＰＳから出力される車両の方位に関する情報
   との差により、上記観測値Ｙ（ｔ）を演算するととも
   に、その演算値を基に上記モデルから状態量Ｘ（ｔ）を
   演算する状態量演算手段を有することを特徴とする請求
   項１又は２に記載の車両用現在位置検出装置。
4. 【請求項４】 車両の方位を検出する方位検出手段と、 車両の走行速度に応じた信号を出力する距離センサと、 前記距離センサからの信号により距離係数を乗じて車両
   の移動距離を求め、前記方位検出手段にて検出した車両
   の方位と前記移動距離に基づいて車両の位置を検出する
   位置検出手段と、 から構成される推測航法手段と、 ＧＰＳ衛星からの衛星電波を受信して車両の速度に関す
   る情報を出力するＧＰＳと、 前記推測航法手段における車両の速度に関する情報と前
   記ＧＰＳから出力される車両の速度に関する情報との差
   に基づいて、距離係数誤差を推定する推定手段と、 この推定手段にて推定した距離係数誤差により前記距離
   係数を修正する修正手段とを備え、 前記距離センサは、車両の走行速度に応じた信号を出力
   する第１、第２のセンサを有し、それぞれのセンサは車
   両の走行速度に関する異なる物理量を検出するものであ
   って、 前記位置検出手段は、前記第１のセンサからの信号によ
   り求められる車両の移動距離と前記第２のセンサにより
   求められる車両の移動距離を所定のタイミングで切換使
   用するとともに、前記第１のセンサからの信号に対し前
   記修正された距離係数を乗じて得られる移動距離と第２
   のセンサからの信号により求められる移動距離が一致す
   るように前記第２のセンサに対する距離係数を設定する
   設定手段を有することを特徴とする車両用現在位置検出
   装置。
5. 【請求項５】 車両の方位を検出する方位検出手段と、 車両の走行速度に応じた信号を出力する距離センサと、 前記距離センサからの信号により距離係数を乗じて車両
   の移動距離を求め、前記方位検出手段にて検出した車両
   の方位と前記移動距離に基づいて車両の位置を検出する
   位置検出手段と、 から構成される推測航法手段と、 ＧＰＳ衛星からの衛星電波を受信して車両の速度に関す
   る情報を出力するＧＰＳと、 前記推測航法手段における車両の速度に関する情報と前
   記ＧＰＳから出力される車両の速度に関する情報との差
   に基づいて、距離係数誤差を推定する推定手段と、 この推定手段にて推定した距離係数誤差により前記距離
   係数を修正する修正手段とを備え、 前記距離センサは、車輪の回転に応じた信号を出力する
   車輪センサと車両の走行速度に応じた車速パルスを発生
   する車速センサにより構成されるものであって、 前記位置検出手段は、前記車輪センサからの信号により
   求められる車両の移動距離と前記車速センサにより求め
   られる車両の移動距離を所定のタイミングで切換使用す
   るとともに、前記車輪センサからの信号に対し前記修正
   された距離係数を乗じて得られる移動距離と車速センサ
   からの車速パルス数による移動距離が一致するように車
   速センサに対する距離係数を設定する設定手段を有する
   ことを特徴とする車両用現在位置検出装置。
6. 【請求項６】 前記推定手段は、距離係数誤差を状態量
   Ｘ（ｔ）とし、この誤差の時間的変化を与えるプロセス
   行列φおよび信号生成過程で発生する雑音ωにより、状
   態量Ｘ（ｔ＋１）をφ・Ｘ（ｔ）＋ωにて関係付ける信
   号生成過程と、状態量Ｘ（ｔ）と観測値Ｙ（ｔ）とを、
   観測行列Ｈおよび観測過程で発生する雑音ｖにより、観
   測値Ｙ（ｔ）をＨ・Ｘ（ｔ）＋ｖにて関係付ける観測過
   程を形成するモデルを基に、前記推測航法手段における
   車両の速度に関する情報と前記ＧＰＳから出力される車
   両の速度に関する情報との差により、上記観測値Ｙ
   （ｔ）を演算するとともに、その演算値を基に上記モデ
   ルから状態量Ｘ（ｔ）を演算する状態量演算手段を有す
   ることを特徴とする請求項４又は５に記載の車両用現在
   位置検出装置。