JPH06341848A - ナビゲーション装置 - Google Patents

Abstract

(57)【要約】 【目的】 各種センサ特有のモデル化を行うことによ
り、センサノイズを適正に、且つ迅速に見積もり、より
精度の高い位置情報を得るナビゲーション装置を提供す
る。 【構成】 移動体の方位を検出する方位センサ１１、１
２からのセンサ信号をフィルタ処理手段１に入力し、推
定方位５を求め、速度センサ１５の速度データと合わせ
て、移動体の走行位置を求める。その時、方位データ、
速度データ、フィルタ出力の推定値７のいずれかを入力
として、各方位センサの誤差をセンサ誤差評価手段３に
おいて評価し、その出力をもとにフィルタゲインをフィ
ルタゲイン算出手段２で算出することにより、最終的に
出力される推定方位５の精度を向上させる。 【効果】 車速によるフィルタの収束性やセンサ誤差等
が、迅速かつ適正に評価でき、その結果を用いてフィル
タゲインが決められるため、より精度の高い推定量を得
ることができる。

Description

【発明の詳細な説明】

【０００１】

【産業上の利用分野】本発明は、移動体に備えられた種
々のセンサにより移動体の運動を検出し、それらの検出
信号を組み合わせ、フィルタ処理を施すことにより、移
動体の位置情報の精度を向上させるナビゲーション装置
に関する。

【０００２】

【従来の技術】従来から道路交通網の任意の個所を走行
している、車両の位置を検出する方式として、距離セン
サと方位センサと、それら両センサからの出力信号を用
いて、処理を施す処理装置とを具備し、車両の走行に伴
って生ずる距離変化量および方位変化量を積算しなが
ら、車両の現在位置データを得る推測航法がある。

【０００３】しかし、この推測航法では、距離センサお
よび方位センサが、必然的に有している誤差が、走行距
離に伴って累積される。そのため、得られる現在位置デ
ータに含まれる誤差も、累積されてしまうという問題が
あった。

【０００４】特に、方位センサは、従来から、車両の絶
対方位を検出することのできる地磁気センサと、車両の
相対的方位変化を検出することのできるジャイロセンサ
とを、組み合わせて用いることにより、車両の走行方位
を検出している。

【０００５】しかし、各々のセンサ特有の誤差特性を持
っている。地磁気センサは、高圧線やビルディング等の
影響を受ける、磁場環境の悪い場所において、短期的な
ノイズが発生するが、長期的には、比較的正しい方位を
出力する。一方、ジャイロセンサは、磁場環境の影響は
全く受けず、短期的には、正しい相対的方位変化を出力
するが、長期的にはドリフト（角速度バイアスの変動）
の影響により、時間の経過とともに方位誤差を累積す
る。

【０００６】このような問題点を解決する一方法とし
て、特開平１−２１９６１０号公報に示されている、セ
ンサ信号にフィルタリング処理を施す方法がある。これ
は、単位時間毎の地磁気センサとジャイロセンサの方位
変化量の差から、磁場環境の状況を検出し、それに応じ
て真の地磁気方位に近づけるため、フィルタゲインの大
きさを調節する方法である。

【０００７】また、これと類似な方法として、上記フィ
ルタゲインの算出に、カルマンフィルタ処理を導入し
た、特開平３−１８８３１６号公報に示されている例も
ある。この方式では、例えば、直線走行時における地磁
気センサとジャイロセンサの方位データの分散値から、
各センサのノイズ成分を決定する。その後、ノイズ成分
の値をもとに、カルマンフィルタアルゴリズムにより、
ノイズの少ないセンサによって決定された方位に、重み
を置いたフィルタゲインを算出する。

【０００８】これらの両従来方式は、定性的には、前述
した各方位センサの特性から、短期的にはジャイロセン
サに重みを置き、長期的には徐々に地磁気センサに重み
を移してゆく方式である。そして、地磁気方位に収束さ
せる速さ（フィルタゲイン）を、前者は各センサの方位
変化量の差から、後者は直線走行時の各センサデータの
分散値から、それぞれ決定している。すなわち、地磁気
方位に収束させる速さは、地磁気センサおよびジャイロ
センサの出力データのみから決められている。

【０００９】

【発明が解決しようとする課題】しかし、ナビゲーショ
ン装置において、地磁気方位に収束させる速さは、車速
データにも密接に関係している。すなわち、車速を積分
した走行距離と、推定方位とにより、走行位置を随時求
めていくナビゲーション装置において、地磁気方位への
収束性を考える場合、時間の経過に対する収束性ではな
く、走行距離に対する収束性を考えるべきである。つま
り、車速が大きく、走行距離が増大する程、より速く正
確な走行方位が必要になる。

【００１０】それに対して、従来例では、極端な例を挙
げれば、車両が停止している場合（方位誤差があっても
位置誤差にはつながらない）でも、高速走行している場
合（僅かな方位誤差でも大きな位置誤差になる）でも、
地磁気センサおよびジャイロのノイズ特性が同じである
限り、地磁気方位への収束速度が同じになるという問題
があった。

【００１１】また、上述した前者の従来方式では、両セ
ンサの方位変化量の差からフィルタゲインを算出する式
の物理的根拠が乏しく、このことに起因する方位誤差の
発生は免れえない。

【００１２】それに対して、後者の方式においては、フ
ィルタゲインを決定する合理的な枠組みとして、カルマ
ンフィルタアルゴリズムを採用している。しかし、その
反面、フィルタゲインを変更する機会は、直線走行時の
データを積み重ねて統計処理が可能となった時のみであ
る。つまり、フィルタゲインの決定頻度が極めて低く、
リアルタイムとは言い難い。逆にいうと、統計処理が可
能となって、適切なフィルタゲインが決定されるまでの
間は、不適切なゲインを使用し続けることになり、位置
検出精度の悪化につながるという問題があった。

【００１３】また、このセンサノイズを見積もる統計処
理方法にも問題がある。センサノイズは、各々のセンサ
に特有のものがある。例えば、地磁気センサには、着磁
ノイズがあり、一瞬の内にセンサ出力に大きなオフセッ
ト誤差が発生する。ジャイロセンサの場合は、徐々に一
方向の方位誤差が累積する。

【００１４】さらに、上記従来例では、これらのノイズ
のモデル化が一切ない。従来技術では、センサが出力す
る生の方位データを統計処理し、分散値を算出して、そ
れをもってフィルタゲインを求めている。この方法で
は、上記に述べた各センサ特有のノイズが発生した場合
に、適切な対応ができず、結果として、大きな方位誤差
を発生させる原因になりかねない。

【００１５】以上は、方位推定に適用したフィルタ処理
における従来例の問題点を述べた。しかし、これは方位
推定に限ったものではない。同様に、センサを用いて、
ナビゲーションに関する、ある特定の物理量を推定する
場合、例えば、位置推定の場合でも共通の課題である。

【００１６】本発明は、上記の問題点を鑑みてなされた
ものであり、各種センサ特有のモデル化を行うことによ
り、センサノイズを適正かつ迅速に見積もり、より精度
の高い位置情報を得るナビゲーション装置を提供するこ
とを目的としている。

【００１７】

【課題を解決するための手段】上記目的は、移動体の速
度を検出する速度センサと、速度以外の移動体の運動を
表す物理量を検出する１つ以上のセンサと、このセンサ
および速度センサからなるセンサ群からの出力を用い
て、移動体の走行位置を求めるナビゲーション装置にお
いて、センサ群からの出力に対して、予め定めたフィル
タ特性を持つフィルタ処理を施し、その処理結果を用い
て、推定値を算出するフィルタ処理手段と、速度センサ
以外の１つ以上のセンサからの出力、および、フィルタ
処理手段から出力される推定値のうち、いずれか一つ以
上を用いて、一つ以上のセンサからの出力誤差を評価す
るセンサ誤差評価手段と、センサ誤差評価手段の出力を
もとに、フィルタ処理手段のフィルタ特性を調整する、
フィルタゲインを算出するフィルタゲイン算出手段とを
有することを特徴とする、ナビゲーション装置によって
達成できる。

【００１８】上記目的は、また、移動体の運動を表す物
理量を検出する複数のセンサからなるセンサ群と、セン
サ群からの出力を用いて、移動体の位置を求めるナビゲ
ーション装置において、センサ群からの複数個の出力に
対して、予め定めた特性を持つフィルタ処理を施し、そ
の処理結果を用いて、少なくとも一つの移動体の運動に
関する物理量の推定値を算出するフィルタ処理手段と、
センサ群からの複数個の出力およびフィルタ処理手段か
ら出力される推定値のうち、いずれか一つ以上を入力と
して、一つ以上のセンサの誤差を評価するセンサ誤差評
価手段と、センサ誤差評価手段の出力を用いて、ウイン
ドウ特性を決定し、その特性によって、一つ以上のセン
サからの出力における、異常値を除外するウインドウ処
理手段とを有することを特徴とする、ナビゲーション装
置によっても達成できる。

【００１９】

【作用】本発明では、移動体の速度を検出する速度セン
サと、速度以外の移動体の運動を表す物理量を検出する
１つ以上のセンサと、このセンサおよび速度センサから
なるセンサ群からの出力を用いて、移動体の走行位置を
求めるナビゲーション装置において、フィルタ処理手段
は、これらセンサ群からの出力に対して、予め定めたフ
ィルタ特性を持つフィルタ処理を施し、その処理結果を
用いて、速度以外の移動体の運動を表す物理量に関する
推定値を算出する。

【００２０】このフィルタ処理手段から出力される推定
値、および、速度センサ以外の１つ以上のセンサからの
出力のうち、いずれか一つ以上を用いて、センサ誤差評
価手段は、一つ以上のセンサからの出力誤差を適正に評
価することができる。

【００２１】このセンサ誤差評価手段の出力をもとに、
フィルタゲイン算出手段は、フィルタ処理手段のフィル
タ特性を調整する、フィルタゲインを算出する。

【００２２】このように、算出されたフィルタゲインを
用いることにより、フィルタ処理手段の特性を適正に調
整し、最終的に出力される推定値の精度を向上させる。
したがって、このように決定された推定値を用いれば、
高精度に移動体の位置または方位を決定することでき
る。

【００２３】本発明では、また、移動体の運動を表す物
理量を検出する複数のセンサからなるセンサ群と、セン
サ群からの出力を用いて、移動体の位置を求めるナビゲ
ーション装置において、フィルタ処理手段は、センサ群
からの複数個の出力に対して、予め定めた特性を持つフ
ィルタ処理を施し、その処理結果を用いて、少なくとも
一つの移動体の運動に関する物理量の推定値を算出す
る。

【００２４】次に、センサ誤差評価手段は、これらセン
サ群からの複数個の出力、および、フィルタ処理手段か
ら出力される推定値のうち、いずれか一つ以上を入力と
して、一つ以上のセンサの誤差を評価する。

【００２５】最後に、ウインドウ処理手段が、センサ誤
差評価手段の出力を用いて、ウインドウ特性を決定し、
その特性によって、一つ以上のセンサからの出力におけ
る、異常値を除外する。

【００２６】このように、センサの誤差評価の結果を用
いて、異常値を除外するウインドウ処理手段を採用する
ことで、最終的に、フィルタ処理手段から出力される推
定値の精度を向上させることができる。したがって、こ
のように決定された推定値を用いれば、高精度に移動体
の位置または方位を決定することできる。

【００２７】

【実施例】以下、図面を用いて、本発明に係わる装置の
実施例を説明する。本発明を適用した、車両用ナビゲー
ション装置の構成図の一例を、図１８に示す。本実施例
は、移動体の速度を測定する車速センサ２３と、移動体
の方位を測定する地磁気センサ２１および旋回角速度セ
ンサ２２と、それらセンサからの信号を用いて、以下に
説明する処理手段を行ない位置を算出するコントローラ
２０とを有する。さらに、本実施例は、地図データを記
憶する地図メモリ２５と、コントローラ２０によって算
出された移動体の位置を、地図メモリ２５から読みだし
た地図上に表示する表示装置２６と、初期位置等を入力
する初期位置入力装置２７とを備える。

【００２８】ここで、地磁気センサ２１および旋回角速
度センサ２２は方位センサとして、車速センサ２３は速
度センサとして用いる。

【００２９】方位センサは、例えば、地球磁界（地磁
気）を検出して絶対方位を求める地磁気センサ２１が代
表的なセンサである。しかし、ＧＰＳ測位装置も、それ
によって求められた位置の時間変化から進行方位を求め
る、絶対方位センサとして、利用可能である。

【００３０】旋回角速度センサ２２は、移動体の旋回角
速度を測定するものである。具体的には、機械式ジャイ
ロ、振動ジャイロ、光ファイバジャイロ等があり、航空
機、宇宙機、自動車のナビゲーション装置に使われてい
る。これらも、基準方位を与えれば、方位センサとして
用いることができる。

【００３１】車速センサ２３には、自動車、電車等の車
両では車輪回転数に応じたパルス信号を出力する、光電
式あるいは電磁式の速度センサがある。また、航空機で
は、移動体に加わる気圧変化を利用した速度センサがあ
る。

【００３２】地図メモリ２５は、所定範囲にわたる道路
地図データが、予め格納されているものである。具体的
には、半導体メモリ、カセットテープ、ＣＤ−ＲＯＭ、
ＩＣメモリ、ＤＡＴ等が使用可能である。

【００３３】表示装置２６は、ＣＲＴ、液晶表示器等を
使用して、車両走行中の道路地図と車両位置とを表示す
るものである。

【００３４】コントローラ２０は、地磁気センサ２１お
よび旋回角速度センサ２２より検出されたデータに基づ
いて、車両の推定方位を算出するとともに、車速センサ
２３のデータと合わせて、車両の位置を演算する。さら
に、図形処理プロセッサ、画像処理メモリ等も備えるこ
とができ、表示装置２６上における地図の検索、縮尺切
り替え、スクロール、上記演算結果の車両位置の表示等
を行わせることが可能である。

【００３５】初期位置入力装置２７は、キーボードやジ
ョイステック等、車両位置を入力できるものとする。

【００３６】本実施例の動作を、図１５のフローチャー
トを用いて説明する。本実施例のシステムがスタートす
ると、まず、初期処理が行われる（ステップ２００）。
ここでは、フィルタ処理手段にカルマンフィルタ処理が
適用される場合は、後に説明するカルマンフィルタ処理
の初期処理も行われる。キー入力等の初期位置入力装置
２７により、現在地が設定されると（ステップ２０
２）、表示装置２６上に、車両の現在地、および、その
周辺の地図が表示される（ステップ２０４）。

【００３７】次に、ステップ２０６により、ステップ２
０８以下の割り込みを許可し、以下のメインループに入
る。このメインループでは、単位走行距離毎の走行量を
積分加算して、車両現在地と地図が更新される。

【００３８】走行方位算出割込処理（ステップ２０８）
および走行ベクトル積分割込処理（ステップ２１０）に
より、新しい車両位置が算出され、車両位置が移動した
と判断された場合（ステップ２１２でYes）、現在地お
よび周辺地図が更新される（ステップ２１４）。

【００３９】走行方位算出割込処理（ステップ２０８）
では、方位を測定するセンサ２１および２２のでが読み
込まれ、フィルタ処理により、方位が算出される。この
ステップ２０８の具体的動作については、後のフィルタ
処理手段にカルマンフィルタ処理を用いた、実施例１に
おいて説明する。

【００４０】走行ベクトル積分割込処理（ステップ２１
０）の処理手順を、図１７を用いて説明する。この処理
は、車両が、所定距離ΔＤを走行する毎に実行される。
すなわち、ステップ４００により、車両方位θが読み込
まれる。この車両方位θは、図１５のステップ２０８の
処理により算出され、記憶されているものである。

【００４１】この車両方位を用いて、距離ΔＤのｘ方向
成分とｙ方向成分が求めらる（ステップ４０２）。最後
に、求められた成分が、ｘ方向積分距離とｙ方向積分距
離とに加算されて、積算距離が更新される（ステップ４
０４）。

【００４２】以上の処理が行われると、上記図１５の処
理で、最新のｘ方向積算処理とｙ方向積算処理で算出さ
れた座標位置が、車両の現在走行位置として決定され
る。

【００４３】本発明によるナビゲーション装置の基本的
特徴を、図１、２および３を用いて説明する。以下に示
す実施例１以降では、これらの基本的特徴を組み合わせ
た構成について説明する。

【００４４】本発明を適用したナビゲーション装置の一
例における特徴部分を図１に示す。ここで、本発明の基
本的な構成は、上記で説明した図１８のような構成を持
つ。ここで、ナビゲーション装置のコントローラ２０
（図１８）には、図１が示す、推定方位５と、方位を含
む推定値７とを出力するフィルタ処理手段１と、フィル
タ処理手段１のゲインを決定するフィルタゲイン算出手
段２と、方位センサ１１、１２、および、速度センサ１
５の誤差を評価するセンサ誤差評価手段３とが含まれ
る。ここで、方位センサ１１および１２は、図１８の地
磁気センサ２１および旋回角速度センサ２２に、速度セ
ンサ１５は、車速センサ２３に対応する。

【００４５】この例において、移動体の方位と回転角を
計測する、複数個の方位センサ１１と１２とからのセン
サ信号を用いて、フィルタ処理手段１は、推定方位５を
求める。このフィルタ処理手段１からの出力と同期し
て、速度センサ１５から求められる速度データと合わせ
て、移動体の走行位置を求める。

【００４６】ここで、方位データ、速度データ、フィル
タ出力の推定値７のいずれかを一つ以上を入力として、
方位センサ１１、１２の測定誤差を、センサ誤差評価手
段３において評価する。さらに、その出力をもとに、フ
ィルタゲイン算出手段２で算出することにより、最終的
に出力される推定方位５の精度を向上させる。

【００４７】本発明を適用したナビゲーション装置の他
の一例における特徴部分を、図２に示す。ここでは、図
１のようにセンサとして、方位センサに限らず、一般的
なセンサ１３、１４を用いて、移動体の運動を表す物理
量を計測する。

【００４８】この例の全体構成は、図１の例と同じであ
る。しかし、この例では、フィルタ処理手段１は、測定
された物理量のデータと、速度センサ１５の速度データ
とを入力として用い、移動体の推定位置６を出力する。

【００４９】この例では、各センサ１３および１４から
のデータ、速度データおよび推定値７のうち、いずれか
一つ以上を入力として、各センサ１３、１４の誤差を、
センサ誤差評価手段３で評価する。その出力をもとに、
フィルタ処理手段１のフィルタゲインを、フィルタゲイ
ン算出手段２で算出することにより、最終的に、出力さ
れる推定位置６の精度を向上させる。

【００５０】本発明を適用したナビゲーション装置の他
の一例における特徴部分を、図３に示す。この手段は、
一般的なセンサ１３および１４と、それらの出力信号を
用いて、方位や位置などの推定値７を求めるフィルタ処
理手段１と、各センサからのデータおよび推定値７のう
ち、いずれか一つ以上を入力として、各センサ誤差を評
価するセンサ誤差評価手段３と、その出力をもとに、セ
ンサ信号の異常値を除外するウインドウ処理手段４とか
ら構成される。この構成によって、最終的に出力される
推定値７の精度を向上させる。

【００５１】本発明によれば、以上のような特徴によ
り、センサによる計測信号そのものの誤差を小さくでき
る。そのため、例えば、車両用ナビゲーション装置にお
ける地図マッチング方式の誤差補正のような最終段の補
正とは異なり、補正した後に同様な誤差がまたすぐに発
生するようなことはない。従って、本発明は精度の高い
センサを用いていることと等価になり、システム的な信
頼性も向上させることができる。

【００５２】図２、３に示される各手段におけるセンサ
１３および１４の例としては、上記で述べた方位センサ
や速度センサと、それ以外に、位置検出センサやレンジ
検出センサ等が挙げられる。

【００５３】位置検出センサは、ＧＰＳ衛星や地上局等
の送信局より、送信された信号から、移動体の現在位置
を演算により求めるものである。具体的には、ＧＰＳ、
ＧＬＯＮＡＳＳ、ビーコン等があり、航空機、宇宙機、
自動車等のナビゲーション装置に用いられている。

【００５４】レンジ検出センサは、送信局と移動体に搭
載した受信機との距離（レンジ）、あるいは、その変化
率（レンジレート）を測定し、送信機の位置が既知であ
ればその位置を送信するものである。現在、利用可能な
ものには、ＧＰＳ、ＧＬＯＮＡＳＳ、ＬＯＲＡＮ−Ｃ、
ビーコン等がある。

【００５５】ＧＰＳは、航空機、宇宙機、自動車等のナ
ビゲーション装置として利用されている。ＧＬＯＮＡＳ
Ｓは、旧ソ連の管理運用しているもので、ＧＰＳと同様
の衛星測距システムである。ＬＯＲＡＮ−Ｃは、船舶等
によく装備されている。ビーコンは、地上に設置され
た、いわゆる電波灯台である。これは、ビーコン位置を
局所的に送信するもので、その信号を受信することによ
って、移動体がビーコン付近に位置することが分かる。
主に、自動車、航空機の位置検出に利用される。

【００５６】〔実施例１〕本実施例は、図４に示される
構成を有する。これは、図１および図３に示された本発
明の基本的特徴部分を組み合わせ、フィルタ処理手段１
をカルマンフィルタ処理手段で実現したナビゲーション
装置である。

【００５７】本実施例のハードウエア構成は、上記で説
明した図１８の構成である。ここで、地磁気センサ２１
および旋回角速度センサ２２を方位センサとして、車速
センサ２３を速度センサとして用いる。

【００５８】コントローラ２０における、カルマンフィ
ルタ処理を用いたフィルタ処理手段１（図４参照）で
は、推定する量（状態量）の時間変化と、状態量と、セ
ンサ出力として測定可能な量（観測量）との関係を、モ
デル化する必要がある。

【００５９】以下の説明では、本実施例におけるカルマ
ンフィルタ処理において、最初、自動車ナビゲーション
において重要な、走行方位と、使用した方位センサの誤
差要因を、推定すべき状態量として、その時間変化をモ
デル化する。次に、観測量と状態量との関係をモデルで
表す。

【００６０】地磁気センサ２１の主な誤差要因は、着磁
と、車体のμ効果（以下車体効果と言う）である。着磁
は、地磁気センサ２１を取付けている車体が磁化し、大
きな方位誤差を与える現象である。これは、地磁気セン
サ２１の出力が描く、方位円の中心が、移動することに
対応する。また、車体効果は、磁性体である車体が地球
磁界に及ぼす効果であり、方位円が楕円に変形する現象
である。

【００６１】着磁成分と、車体効果は、それぞれ２つの
パラメタで記述でき、パラメタの時間変化は少ないとし
て、次のようにモデル化することができる。

【００６２】

【数１】

【００６３】

【数２】

【００６４】

【数３】

【００６５】

【数４】

【００６６】このモデルは、地磁気センサ２１の誤差要
因（着磁、車体効果）の挙動を表すもので、以下、地磁
気センサ誤差モデルと呼ぶ。

【００６７】旋回角速度センサ２２の誤差要因の一つ
は、角速度バイアスである。バイアスは、短時間では、
ほぼ一定と仮定でき、

【００６８】

【数５】

【００６９】とモデル化した。このモデルは、旋回角速
度センサ２２の誤差要因を記述したものであり、以下、
ジャイロ誤差モデルと呼ぶ。

【００７０】２つのカルマンフィルタのモデル（状態方
程式、観測方程式）を説明する。方位推定カルマンフィ
ルタの状態量ｘは、

【００７１】

【数６】

【００７２】とする。

【００７３】状態方程式は、

【００７４】

【数７】

【００７５】とした。システムノイズｖ（ｋ）は、状態
量の数７によるモデル化誤差である。

【００７６】観測方程式は、

【００７７】

【数８】

【００７８】

【数９】

【００７９】

【数１０】

【００８０】のとなり、まとめて表すと、

【００８１】

【数１１】

【００８２】となる。

【００８３】ここで、観測量ｙは、

【００８４】

【数１２】

【００８５】であり、測定ノイズｗは、

【００８６】

【数１３】

【００８７】である。

【００８８】ここで、地磁気センサ２１の誤差要因とし
て車体効果の影響が小さい場合、車体効果を表すＲｘ、
Ｒｙを一つの状態量（推定量）Ｒで表す、簡易的な方法
を取っても良い。

【００８９】上記の状態方程式と、観測方程式とに対し
て、カルマンフィルタを計算し、自動車の走行方位を推
定する。ここで、求めた観測方程式は非線形なため、拡
張カルマンフィルタのアルゴリズムを適用する。拡張カ
ルマンフィルタは、次の漸化式、

【００９０】

【数１４】

【００９１】

【数１５】

【００９２】

【数１６】

【００９３】

【数１７】

【００９４】

【数１８】

【００９５】

【数１９】

【００９６】で与えられる。

【００９７】カルマンフィルタでは、状態量ｘ（ｋ）の
最適推定値ｘ（ｋ｜ｋ）、予測値ｘ（ｋ｜ｋー１）と、
これらに対応した推定誤差共分散行列Ｐ（ｋ｜ｋ）、Ｐ
（ｋ｜ｋー１）を算出する。Ｖは、システムノイズｖの
共分散行列であり、Ｗは、観測ノイズｗの共分散行列で
ある。Ｈは、関数ｈの予測値ｘ（ｋ｜ｋー１）における
ヤコビアンである。なお、上記カルマンフィルタの繰返
し計算は、漸化式の形式を持ち、計算機のソフトウェア
により実現可能である。

【００９８】以上の拡張カルマンフィルタのアルゴリズ
ムを利用することにより、方位推定を行う。この処理手
順を、図４のブロック図と図２０のフローチャートを用
いて説明する。なお、本実施例による位置決定は、上記
で説明した図１５のフローに従って行なわれる。

【００９９】最初、初期処理（図１５のステップ２０
０）において、数１４の通り、予測値と誤差共分散行列
の初期値を設定する。その後、一定時間毎に走行方位を
算出する走行方位算出割込処理（図１５のステップ２０
８）に入る。

【０１００】このステップ２０８は、図４に示す機能手
段を用い、図２０に示すフローに従って方位を決定す
る。

【０１０１】最初、センサデータ（観測量）を読み込む
（ステップ３００）。次に、状態量の予測値を、観測方
程式に代入し、観測量の予測値を観測量予測値算出手段
３１（ステップ３０２）で算出し、その予測値と観測量
との差から観測量予測誤差を観測量予測誤差算出手段３
２（ステップ３０４）で算出する。

【０１０２】その後、観測量予測誤差、予測値、車速デ
ータ等を用いて、以下に説明するセンサ誤差評価手段３
（ステップ３０６）でセンサ誤差を評価する。ここで、
観測量予測誤差の代わりに観測量そのものを用いても良
いし、また、予測値の代わりに推定値を用いても良い。

【０１０３】センサ誤差評価手段３の一例として、図６
に示すような、地磁気センサ２１の誤差評価手段３とし
て、ファジィ推論処理手段５０を用いた例を説明する。

【０１０４】このファジイ推論処理手段５０の入力に
は、観測量予測誤差算出手段３２の出力である地磁気セ
ンサ２１の観測量予測誤差Ｉmag５１と、車速センサ２
３の出力データＶel５２を用いる。また、フィルタゲイ
ン算出手段３４への出力としては、地磁気センサ２１の
観測ノイズの共分散Ｗmag５３をとる。この場合、入力
値は、ある期間のそれぞれのデータ自身の統計量、例え
ば、平均値であってもよい。

【０１０５】入力値５１、５２の、それぞれの大きさに
対応する出力５３の大きさの、定性的な関係を、ファジ
ィ理論を用いて表す。

【０１０６】入力Ｉmag５１が大きい程、地磁気センサ
２１のノイズが大きい、と予想されるため、出力Ｗmag
５３は、相対的に大きくする必要がある。逆に、車速Ｖ
el５２が大きい程、最終的に、収束する地磁気センサ２
１が示す方位への収束速度を速める必要がある。そのた
め、出力Ｗmag５３は、相対的に小さくする必要があ
る。

【０１０７】この定性的な関係を、図７上に示すよう
に、５段階のファジィラベルを用いたファジィルールと
して表現し、それを同図下のようなメンバシップ関数を
用いて定量化した。ファジィ推論法として、例えば、マ
ックスミニ合成重心法を用いると、各々の入力値５１、
５２に対して、出力値５３が図７下のようにハッチング
した面積の、重心位置として求めることができる。ただ
し、演算量の節約から、上記の演算を予め行っておき、
マイクロコンピュータのメモリに書き込んでおいて、入
力値に対応する出力値をメモリから読み出す、テーブル
ルックアップ方式で、上記ファジィ推論を実現させても
よい。

【０１０８】また、上記ファジイ処理手段５０の例で
は、ファジィルールの前件部および後件部とも、ファジ
ィ変数を用いた。しかし、上記重心計算等の演算負荷を
減らすため、後件部のみファジィ変数を用いず、線形式
を適用する、菅野らの推論法によりファジィ推論を実現
させてもよい。

【０１０９】さらに、センサ誤差処理手段３において、
ファジィ理論を用いずに、上記定性的関係を、単純にあ
る関数式で定量的に表すことにより、センサ誤差評価を
実現することもできる。また、センサ誤差評価手段３に
他の手段を用いる例を、以下の実施例２から７に説明す
る。

【０１１０】次に、センサ誤差評価手段３からの出力と
予測値とを用いて、カルマンフィルタゲインをフィルタ
ゲイン算出手段２（ステップ３２２、数１５）で求め
る。求められたゲインを用いて、状態量および誤差共分
散の推定値を推定値算出手段３５（ステップ３２４、数
１６、数１７）で計算する。求める走行方位は、この推
定値の一つ（推定方位５）として計算され、出力され
る。

【０１１１】また、センサ誤差評価手段３からの出力
は、ウインドウ処理手段４の判断基準にも使用される。
例えば、地磁気センサ２１における、ある一定レベル以
上のノイズを除去することが可能となる。

【０１１２】計算された推定値と、旋回角速度センサ２
２からの出力データをもと、次の一定時間後の予測値
（状態量、誤差共分散）を、予測値算出手段３６（ステ
ップ３２６、数１８、数１９）で算出し、割込み処理を
終了する。

【０１１３】また、本実施例は、図５に示すように、旋
回角速度センサ２２の出力を、観測量として用いずに、
予測値算出手段３６において、一定時間後の予測値を算
出する際の、方位変化量としてのみ使用する構成とする
ことができる。

【０１１４】以上のような処理手順により、地磁気セン
サ２１の誤差を適正に評価することができる。さらに、
その評価結果に、カルマンフィルタ処理を無理なく適応
させることができるため、高精度な方位推定が可能とな
る。

【０１１５】〔実施例２〕本実施例は、実施例１と同じ
ハードウエア構成（図１８参照）を有し、センサ誤差評
価手段３において対象とするセンサに、旋回角速度セン
サ２２を含む場合の構成を説明する。

【０１１６】本実施例は、図４に示される機能手段の構
成を、有するものである。本実施例におけるセンサ誤差
評価手段３の入出力に関する部分を、図８に示す。ここ
で、誤差評価には、実施例１同様に、ファジィ推論を適
用する。

【０１１７】センサ誤差処理手段３の入力には、観測量
予測誤差算出手段３２の出力である旋回角速度センサ２
２の観測量予測誤差Ｉgyro５４と、車速センサ２３の出
力データ５２をとり、フィルタゲイン算出手段３４への
出力として地磁気センサの観測ノイズの共分散Ｗgyro５
５をとる。この場合、入力値５２、５４は、ある期間の
それぞれのデータ自身の統計量、例えば平均値でもよ
い。

【０１１８】入力値５４および５２の、それぞれの大き
さに対応する出力５５の大きさの定性的な関係を、ファ
ジィ理論を用いて表す。入力Ｉgyro５４が大きい程、旋
回角速度センサ２２のノイズが大きいと予想されるた
め、出力Ｗgyro５５は相対的に大きくする必要がある。
逆に、車速データ５２が大きい程、最終的に、収束する
地磁気センサ２２の示す方位への収束速度を速める必要
があるため、出力Ｗgyro５５は相対的に大きくする必要
がある。

【０１１９】この定性的な関係を、図７と同様に、ファ
ジィルールとして表現（車速データ５２に対する出力５
５の関係は、図７と逆になる）し、それをメンバシップ
関数を用いて定量化すれば、実際の旋回角速度センサ誤
差に適応したフィルタゲインを選択することができる。
したがって、推定方位精度の高精度化を実現することが
できる。

【０１２０】〔実施例３〕本実施例は、実施例１と同様
なハードウエア構成（図１８参照）および機能手段構成
を有する（図４参照）。ただし、センサ誤差評価手段３
において、図９に示すように、地磁気センサ２１の誤差
評価にファジィ推論５０を用い、さらに、地磁気方位円
半径誤差を算出する手段を用いる。

【０１２１】本実施例のセンサ誤差評価手段３の入出力
は、図９に示されるような構成を有している。ここで、
入力には、観測量予測誤差算出手段３２の入力になって
いる地磁気センサの観測量Ｅｘ、Ｅｙ６１を、そのまま
出力させたものと、予測値算出手段３６の出力の内の着
磁成分Ｍｘ、Ｍｙ、車体効果Ｒｘ、Ｒｙ６２と、車速セ
ンサ２３の出力データ５２を用いる。また、センサ誤差
評価手段３の、フィルタゲイン算出手段３４への出力と
して、地磁気センサ２１の観測ノイズの共分散Ｗmag５
３をとる。この場合、入力値６１、６２、５２は、ある
期間の、それぞれのデータ自身の統計量、例えば平均値
でもよい。

【０１２２】本実施例では、入力される観測量６１と予
測値６２を用いて、

【０１２３】

【数２０】

【０１２４】

【数２１】

【０１２５】により、地磁気方位円半径誤差算出手段６
０により地磁気方位円半径誤差６３を算出する。ここ
で、数２０は、カルマンフィルタで推定した地磁気方位
円半径を基準にして誤差を求めている。また、数２１
は、例えば、日本における地磁気水平成分（３０マイク
ロテスラ）を基準にしている。

【０１２６】このように算出された誤差６３と車速デー
タ５２により、実施例１に示したようなファジィ推論を
構成する。

【０１２７】実施例１同様、こうして求めた地磁気方位
円半径誤差６３が大きい程、地磁気センサ２１のノイズ
が大きいと予想されるため、出力Ｗmag５３は相対的に
大きくする必要がある。逆に、車速５２が大きい程、最
終的に、収束する地磁気センサ２１が示す方位への収束
速度を速める必要がある。そのため、出力Ｗmag５３
は、相対的に小さくする必要がある。この定性的な関係
を、図７と同じようにファジィルールとして表現し、そ
れをメンバシップ関数を用いて定量化すれば、推定方位
精度の高精度化を実現することができる。

【０１２８】また、図６の地磁気センサ観測量予測誤差
Ｉmag５１と、地磁気方位円半径誤差６３を入力とする
ファジイ理論を構成することもできる。

【０１２９】〔実施例４〕本実施例は、センサ誤差評価
手段３として、地磁気センサ２１の着磁補正を行う、実
施例１と同じハードウエア構成（図１８参照）および機
能手段構成（図４参照）を有するナビゲーション装置で
ある。

【０１３０】本実施例における、センサ誤差評価手段３
の入出力の関係を、図１０に示す。ここで、センサ誤差
評価手段３としての着磁補正手段７０への入力は、観測
量予測誤差算出手段３２の出力の地磁気センサ観測量予
測誤差Ｉmag５１をとる。また、フィルタゲイン算出手
段３４への出力は、着磁量が大きく変化した場合に、数
１４〜１９に含まれる推定誤差共分散行列の着磁量に対
応する対角項Ｐｍｘ，ｍｙが、初期値にリセットされて
出力されるものである。

【０１３１】着磁は、地磁気センサを取り付けている車
体が磁化し、大きな方位誤差を与える現象である。これ
は、地磁気センサ２１の出力が描く、方位円の中心が移
動することに対応している。このような場合、入力にと
った観測量予測誤差Ｉmag５１にも、大きな誤差が定常
的に発生する。そのため、観測量予測誤差Ｉmag５１を
検出して、着磁補正が必要かどうかを判断する。すなわ
ち、ある一定レベル以上の誤差が、ある期間継続するこ
とを検出して、着磁補正を行えばよい。

【０１３２】実施例１のカルマンフィルタ処理において
は、大きな着磁量変化は、状態量として推定している着
磁成分Ｍｘ、Ｍｙに大きな推定誤差を発生させる。着磁
補正が必要と判断された時、正しい着磁量推定を復活さ
せるためには、ここで、出力にとったように、着磁成分
の誤差共分散Ｐｍｘ，ｍｙを大きな値（例えば初期値）
にリセットすればよい。

【０１３３】以上の着磁補正手段７０により、踏切等で
大きな着磁量変化が発生しても、瞬時にそれを検出する
ことができる。さらに、上記方法で着磁量推定も正しく
修整することができるため、高精度な方位推定も維持す
ることが可能となる。

【０１３４】また、上記推定誤差共分散行列の着磁量に
対応する対角項Ｐｍｘ，ｍｙだけでなく、すべての推定
量に対応する対角項は、その大きさがある一定レベル以
下に小さくなるか、もしくは、ある一定時間を経過する
ことをもって、その時点の値よりも大きな値（例えば初
期値）にリセットするのが望ましい。

【０１３５】〔実施例５〕本実施例は、図１１に示すよ
うな、入出力関係を有するセンサ誤差評価手段３を有す
るナビゲーション装置である。これは、実施例４（図１
０）と同様にセンサ誤差評価手段３として、着磁補正を
行う別の例であり、その他の機能手段構成およびハード
ウエア構成は、実施例１と同様である（図４、１８参
照）。

【０１３６】本実施例と実施例４とが異なる部分は、着
磁量変化を検出する点である。本実施例では、着磁量変
化を検出する方法として、図９で用いた、地磁気方位円
半径誤差算出手段６０を適用した。

【０１３７】着磁量の大きな変化により、着磁成分Ｍ
ｘ、Ｍｙは、大きな推定誤差を持つことになる。したが
って、その着磁成分を用いて算出する、地磁気方位円半
径にも大きな誤差が発生することになる。その半径誤差
をもとに、実施例４と同様に、ある一定レベル以上の誤
差が、ある期間継続することを検出して、着磁補正を行
うのである。地磁気方位円半径誤差６３を求める方法と
しては、上記数２０、数２１のどちらを用いてもよい。
本実施例も実施例４と同様な効果が期待できる。

【０１３８】〔実施例６〕本実施例は、実施例１（図４
参照）のセンサ誤差評価手段３として、ウインドウ処理
手段４を制御するものである。本実施例のハードウエア
および機能手段構成は、実施例１と同じである。本実施
例のセンサ誤差評価手段３における入出力の関係を、図
１２に示す。

【０１３９】ここで、地磁気センサのバーストノイズを
除去するために、図９、１１で用いた、地磁気方位円半
径誤差算出手段６０を適用し、その誤差６３が、ある一
定レベル以上になるデータを、ウインドウ処理手段４に
おいて除去する。なお、地磁気方位円半径誤差６３を求
める方法としては、上記の数２０、数２１のどちらを用
いても構わない。

【０１４０】本実施例におけるウインドウ処理手段４に
より、地磁気センサ２１等のバーストノイズを、瞬時に
判断して、除去することが可能になるため、推定方位精
度の向上を達成することができる。

【０１４１】〔実施例７〕以上述べた実施例１〜６のセ
ンサ誤差評価手段３は、各々単独でも構成できるが、複
数個組み合わせて構成してもよい。むしろ、その方が種
々のセンサ誤差に適応できるため、より一層の方位精度
向上が期待できる。本実施例では、以上の実施例で説明
したセンサ評価手段３の構成要素を組み合わせたもの
を、センサ誤差評価手段として用いた例について説明す
る。

【０１４２】本実施例のハードウエアおよび機能手段構
成は、実施例１（図４参照）と同じであり、センサ誤差
評価手段３における入出力の関係は、図１９に示すもの
である。

【０１４３】ここで、センサ誤差評価手段３は、上記で
説明した、ファジイ推論処理手段５０と、着磁補正手段
７０と、地磁気方位円誤差算出手段６０とから構成され
る。

【０１４４】ファジイ推論処理手段５０の入力には、観
測量予測誤差算出手段３２の出力である地磁気センサ２
１の観測量予測誤差Ｉmag５１および旋回角速度センサ
２２の観測量予測誤差Ｉgyro５４と、車速センサ２３の
出力データＶel５２とを用いる。その出力には、旋回角
速度センサの観測ノイズの共分散Ｗgyro５５と、地磁気
センサ２１の観測ノイズの共分散Ｗmag５３をとる。こ
の場合、入力値は、ある期間のそれぞれのデータ自身の
統計量、例えば、平均値であってもよい。

【０１４５】入力値５１および５２の、それぞれの大き
さに対応する出力５３の大きさの定性的な関係と、入力
値５２および５４の、それぞれの大きさに対応する出力
５５の大きさの定性的な関係を、それぞれ、実施例１お
よび２と同様に、ファジィ理論を用いて表す。

【０１４６】着磁補正手段７０の入力には、観測量予測
誤差算出手段３２の出力の地磁気センサ観測量予測誤差
Ｉmag５１をとる。フィルタゲイン算出手段３４への出
力は、着磁量が大きく変化した場合に、数１４〜１９に
含まれる推定誤差共分散行列の着磁量に対応する対角項
Ｐｍｘ，ｍｙが、初期値にリセットされて出力されるも
のである。これによって、実施例４と同様に、踏切等で
大きな着磁量変化が発生しても、瞬時にそれを検出する
ことができる。さらに、上記方法で着磁量推定も正しく
修整することができる。

【０１４７】地磁気方位円半径誤差算出手段６０の入力
は、観測量予測誤差算出手段３２の入力になっている地
磁気センサの観測量Ｅｘ、Ｅｙ６１を、そのまま出力さ
せたものと、予測値算出手段３６の出力の内の着磁成分
Ｍｘ、Ｍｙ、車体効果Ｒｘ、Ｒｙ６２とを用いる。その
出力には、地磁気方位円半径誤差６３を用い、その誤差
６３が、ある一定レベル以上になるデータを、ウインド
ウ処理手段４において除去する。実施例６と同様に、地
磁気方位円半径誤差算出方法６０とウインドウ処理手段
４とを用いるため、地磁気センサ２１等のバーストノイ
ズを、瞬時に判断して、除去することが可能になる。

【０１４８】本実施例におけるセンサ誤差評価手段３の
処理手順を、図１６のフローチャートを用いて説明す
る。

【０１４９】まず、カルマンフィルタ処理のステップ３
０４で求めた観測量予測誤差を用い、実施例４（図１
０）に示した着磁補正処理を行う。すなわち、ステップ
３０６において、前述した着磁補正の判断をする。ここ
で、Yesの場合のみ、ステップ３０８において、着磁量
の誤差共分散Ｐｍｘ，ｍｙを初期値リセットする。その
後ステップ３１０で、地磁気方位円半径誤差６３を、数
２０または数２１により、計算する（実施例５の着磁補
正手段を実行する場合は、先に、ステップ３１０を実行
してからステップ３０６に移行すればよい）。

【０１５０】次に、計算した地磁気方位円半径誤差６３
を用い、ステップ３１２において、実施例６（図１２）
に示したウインドウ処理を行う。すなわち、前述したよ
うに、地磁気方位円半径誤差６３の大きさが、あるレベ
ル以上になった場合、フィルタゲイン計算（ステップ３
２２）および推定値計算（ステップ３２４）を省略して
ステップ３２６へジャンプする。

【０１５１】上記条件を満たさない場合、ステップ３１
４に移り、実施例１〜３（図６、８、９）に示した、フ
ァジィ推論による観測ノイズの共分散の制御を行う。前
述したファジィ推論処理手段５０への入力が、統計量の
場合、その入力値がそろったときのみ、メンバシップ値
の計算（ステップ３１６）に移り、ステップ３１８で重
心値を計算し、その重心値をステップ３２０にて、観測
ノイズ共分散値におく。

【０１５２】以上の処理手順において、着磁量の誤差共
分散（ステップ３０８）かまたは観測ノイズの共分散
（ステップ３２０）が変更された場合には、その変更さ
れた値をもとに、ステップ３２２において、カルマンフ
ィルタゲインが計算される。ここで、センサ誤差の影響
が加味されるため、方位の推定や、その他の推定の精度
を向上させることができる。

【０１５３】〔実施例８〕本実施例における、フィルタ
処理手段１と、センサ誤差評価手段３と、ウインドウ処
理手段４と、センサ２１、２２、２３、２４との構成を
図１３に示す。

【０１５４】本実施例は、図２に示した車速センサデー
タを含む、すべてのセンサデータを用いて、上述のカル
マンフィルタ処理を施し、求めたい推定位置を得るもの
である。ここで、センサとして、新たにＧＰＳ２４を加
えている。このＧＰＳ２４は、絶対位置検出センサ、絶
対方位検出センサ、またはレンジ検出センサとしても使
用できる。

【０１５５】したがって、実施例１〜７に示したのと同
様な形式で、センサ出力モデルおよびセンサ誤差モデル
と車両の運動モデルを用いて、状態方程式と観測方程式
を作ることが可能である。

【０１５６】〔実施例９〕本実施例は、従来例の項で説
明した特開平１−２１９６１０号公報記載の例におい
て、センサ誤差評価手段３に相当する磁場環境検出手段
に、単に、車速情報を加えるだけでも、従来例の課題が
改善できることを示した例である。

【０１５７】すなわち、地磁気センサ２１と旋回角速度
センサ２２の方位変化量の差からのみ、ローパスフィル
タ処理手段８０のゲインを算出していた従来方式に対
し、車速が大きいときは、フィルタゲインを相対的に大
きくして、地磁気方位への収束を速めるような処理を加
えるようにすればよい。

【０１５８】

【発明の効果】本発明のナビゲーション装置によれば、
推定量算出のために、センサの誤差特性をモデル化した
カルマンフィルタ処理を採用し、そのフィルタ処理中に
得られるセンサ誤差パラメータを使ってセンサ誤差を評
価するため、より迅速に、より正確に誤差をとらえるこ
とが可能になる。さらに、その評価結果を用い、カルマ
ンフィルタゲインを変更するため、適切なゲインがリア
ルタイムに算出でき、より精度の高い位置情報等を得る
ことが可能となる。

【０１５９】さらに、本発明では、車速データをも考慮
して推定量算出のためのフィルタのゲインを決めるた
め、車速を積分した走行距離の増加に伴って走行位置を
随時求めていかなければならない、というナビゲーショ
ン装置特有の性質に合理的に対応することができる。

【０１６０】

【図面の簡単な説明】

【図１】本発明のナビゲーション装置の基本的特徴を説
明する構成図。

【図２】本発明のナビゲーション装置の基本的特徴を説
明する構成図。

【図３】本発明のナビゲーション装置の基本的特徴を説
明する構成図。

【図４】図１および３を組み合わせた構成による、走行
方位検出にカルマンフィルタ処理を適用した本発明の一
実施例を示す構成図。

【図５】図４の構成を一部変更した一実施例を示す構成
図。

【図６】図４のセンサ誤差評価手段の一実施例を示す構
成図。

【図７】図６のセンサ誤差評価手段に適用したファジィ
推論の原理を説明する図。

【図８】図４のセンサ誤差評価手段の一実施例を示す構
成図。

【図９】図４のセンサ誤差評価手段の一実施例を示す構
成図。

【図１０】図４のセンサ誤差評価手段の一実施例を示す
構成図。

【図１１】図４のセンサ誤差評価手段の一実施例を示す
構成図。

【図１２】図４のセンサ誤差評価手段の一実施例を示す
構成図。

【図１３】図２の構成による走行位置検出にカルマンフ
ィルタ処理を適用した本発明の一実施例を示す構成図。

【図１４】従来例のローパスフィルタ方式に、本発明の
車速データの効果を加えた一実施例を示す構成図。

【図１５】本発明を適用した車両用ナビゲーション装置
のゼネラルフローを説明する図。

【図１６】図１５中の走行方位算出割込処理のフローチ
ャート。

【図１７】図１５中の走行ベクトル積分割込処理のフロ
ーチャート。

【図１８】本発明が適用されたナビゲーション装置のハ
ードウエア構成を示すブロック図。

【図１９】図４のセンサ誤差評価手段の一実施例を示す
構成図。

【図２０】図１５中の走行方位算出割込処理のフローチ
ャート。

【符号の説明】

１…フィルタ処理手段、 ２…フィルタゲイン算出手
段、 ３…センサ誤差評価手段、 ４…ウインドウ処理
手段、 ５…推定方位、 ６…推定位置、 ７…推定
値、 １１、１２…方位センサ、 １３、１４…セン
サ、 １５…速度センサ、 ２０…コントローラ、 ２
１…地磁気センサ、 ２２…旋回角速度センサ、 ２３
…車速センサ、 ２４…ＧＰＳ、 ２５…地図メモリ、
２６…表示装置、 ２７…初期位置入力手段、 ３１
…観測量予測値算出手段、 ３２…観測量予測誤差算出
手段、 ３５…推定値算出手段、 ３６…予測値算出手
段、 ５０…ファジィ推論処理手段、 ６０…地磁気方
位円半径誤差算出手段、 ７０…着磁補正手段、 ８０
…ローパスフィルタ処理手段。

───────────────────────────────────────────────────── フロントページの続き (72)発明者 星野 雅俊 茨城県日立市大みか町七丁目１番１号 株 式会社日立製作所日立研究所内 (72)発明者 黒田 浩司 茨城県日立市大みか町七丁目１番１号 株 式会社日立製作所日立研究所内 (72)発明者 長島 嘉正 神奈川県座間市広野台２丁目4991番地 株 式会社ザナヴィ・インフォマティクス内 (72)発明者 高野 憲治 神奈川県座間市広野台２丁目4991番地 株 式会社ザナヴィ・インフォマティクス内 (72)発明者 大成 幹彦 神奈川県座間市広野台２丁目4991番地 株 式会社ザナヴィ・インフォマティクス内

Claims (18)

【特許請求の範囲】
1. 【請求項１】移動体の速度を検出する速度センサと、速
   度以外の移動体の運動を表す物理量を検出する１つ以上
   のセンサと、このセンサおよび速度センサからなるセン
   サ群からの出力を用いて、移動体の走行位置を求めるナ
   ビゲーション装置において、 センサ群からの出力に対して、予め定めたフィルタ特性
   を持つフィルタ処理を施し、その処理結果を用いて、推
   定値を算出するフィルタ処理手段と、 速度センサ以外の１つ以上のセンサからの出力、およ
   び、フィルタ処理手段から出力される推定値のうち、い
   ずれか一つ以上を用いて、一つ以上のセンサからの出力
   誤差を評価するセンサ誤差評価手段と、 センサ誤差評価手段の出力をもとに、フィルタ処理手段
   のフィルタ特性を調整する、フィルタゲインを算出する
   フィルタゲイン算出手段とを有することを特徴とするナ
   ビゲーション装置。
2. 【請求項２】請求項１において、前記センサは、移動体
   の方位を異なった方法で測定する複数個のセンサである
   ことを特徴とするナビゲーション装置。
3. 【請求項３】請求項１において、前記センサは、地球磁
   場を検出する地磁気センサ、移動体の旋回角速度を検出
   する旋回角速度センサ、および、衛星信号を用い地球上
   の絶対位置または絶対方位を検出するＧＰＳ受信装置の
   うち、いずれか一つ以上を含み、 前記フィルタ処理手段は、少なくとも、移動体の位置
   を、推定値の一つとして算出することを特徴とするナビ
   ゲーション装置。
4. 【請求項４】請求項２において、前記センサは、地球磁
   場を検出する地磁気センサ、および、移動体の旋回角速
   度を検出する旋回角速度センサのうち、いずれか一つ以
   上を含み、 前記フィルタ処理手段は、少なくとも、移動体の進行方
   位を、推定値の一つとして算出することを特徴とするナ
   ビゲーション装置。
5. 【請求項５】請求項１から４のいずれかにおいて、前記
   センサ誤差評価手段は、さらに、前記速度センサからの
   出力を用いて、前記フィルタ処理手段から出力される推
   定値を、予め定めた値に収束させる速度を調整する、収
   束速度調整機能を有することを特徴とするナビゲーショ
   ン装置。
6. 【請求項６】請求項１から５のいずれかにおいて、前記
   フィルタ処理手段は、少なくとも前記センサの誤差要因
   を推定すべき状態量として用いた、カルマンフィルタ処
   理を含むことを特徴とするナビゲーション装置。
7. 【請求項７】請求項６において、前記センサ誤差評価手
   段の入力される、前記フィルタ処理手段から出力される
   推定値は、カルマンフィルタ処理における推定値、その
   推定値から計算される次の時間ステップでの予測値、お
   よび、その予測値と前記センサ出力から計算される観測
   量の予測誤差のうち、いずれか一つ以上であることを特
   徴とするナビゲーション装置。
8. 【請求項８】請求項６において、前記センサ誤差評価手
   段の出力は、カルマンフィルタ処理における、前記セン
   サの観測誤差の共分散値および推定値の誤差共分散値の
   うち、いずれか一つ以上であることを特徴とするナビゲ
   ーション装置。
9. 【請求項９】請求項６において、前記センサ誤差評価手
   段は、その入力が観測量の予測誤差と前記速度センサの
   出力とであり、その出力が前記センサの観測誤差の共分
   散値であり、入力値のそれぞれの大きさに応じて、出力
   値を調節する機能を、さらに、有することを特徴とする
   ナビゲーション装置。
10. 【請求項１０】請求項１から３のいずれかにおいて、前
    記センサの一つは、地磁気センサであり、 前記フィルタ処理手段は、少なくとも、地磁気センサの
    誤差要因を推定すべき状態量として用いたカルマンフィ
    ルタ処理であり、 前記センサ誤差評価手段は、地磁気センサの着磁量変化
    を補正するため、その入力としてカルマンフィルタ処理
    における地磁気センサに関する観測量の予測誤差を用
    い、出力として着磁量の誤差共分散値を用い、その入力
    値が急激に変化する場合、出力を予め定めた値に設定す
    る機能を、さらに、有することを特徴とするナビゲーシ
    ョン装置。
11. 【請求項１１】請求項１０において、前記センサ誤差評
    価手段は、さらに、カルマンフィルタ処理における予測
    値と観測量とから算出される地磁気方位円半径値を用い
    て、前記地磁気センサの着磁量変化を検出する、着磁量
    変化検出機能を有することを特徴とするナビゲーション
    装置。
12. 【請求項１２】移動体の運動を表す物理量を検出する複
    数のセンサからなるセンサ群と、センサ群からの出力を
    用いて、移動体の位置を求めるナビゲーション装置にお
    いて、 センサ群からの複数個の出力に対して、予め定めた特性
    を持つフィルタ処理を施し、その処理結果を用いて、少
    なくとも一つの移動体の運動に関する物理量の推定値を
    算出するフィルタ処理手段と、 センサ群からの複数個の出力およびフィルタ処理手段か
    ら出力される推定値のうち、いずれか一つ以上を入力と
    して、一つ以上のセンサの誤差を評価するセンサ誤差評
    価手段と、 センサ誤差評価手段の出力を用いて、ウインドウ特性を
    決定し、その特性によって、一つ以上のセンサからの出
    力における、異常値を除外するウインドウ処理手段とを
    有することを特徴とするナビゲーション装置。
13. 【請求項１３】請求項１２において、前記センサは、地
    球磁場を検出する地磁気センサ、移動体の旋回角速度を
    検出する旋回角速度センサ、衛星信号を用い地球上の絶
    対位置または絶対方位を検出するＧＰＳ受信装置、およ
    び、移動体の速度を検出する速度センサのうち、いずれ
    か一つ以上を含むことを特徴とするナビゲーション装
    置。
14. 【請求項１４】請求項１２または１３において、前記フ
    ィルタ処理手段は、少なくとも前記センサの誤差要因を
    推定すべき状態量として用いた、カルマンフィルタ処理
    であることを特徴とするナビゲーション装置。
15. 【請求項１５】請求項１２において、前記センサの一つ
    は、地球磁場を検出する地磁気センサであり、 前記ウインドウ処理手段は、地磁気センサの異常値を除
    外する手段であることを特徴とするナビゲーション装
    置。
16. 【請求項１６】請求項１５において、前記フィルタ処理
    手段は、少なくとも前記センサの誤差要因を推定すべき
    状態量として用いた、カルマンフィルタ処理であり、 前記センサ誤差評価手段は、入力としてカルマンフィル
    タ処理における予測値と観測量とを用い、出力として地
    磁気方位円半径誤差を算出し、 前記ウインドウ処理手段は、前記センサ誤差評価手段の
    出力を用いて、地磁気センサの異常値を除外することを
    特徴とするナビゲーション装置。
17. 【請求項１７】請求項１から１６において、前記センサ
    誤差評価手段は、さらに、その入力値および出力値のあ
    いまいさを考慮したファジィ推論処理機能を含むことを
    特徴とするナビゲーション装置。
18. 【請求項１８】請求項６または１４において、前記セン
    サ誤差評価手段は、推定誤差共分散行列の対角項の大き
    さがある特定値以下になった場合、対角項をその特定値
    以上に大きくするか、または、すべての対角項を、シス
    テム起動時およびある特定時間経過毎に、前回値に比
    べ、大きな値になるように動作することを特徴とするナ
    ビゲーション装置。