JP5046687B2 - ナビゲーション・システム内での反復拡張カルマン・フィルタを実施する方法及び装置 - Google Patents

Description

本発明は、概してナビゲーション・システムに関し、より詳細には、ＩＮＳ／ＧＰＳをベースにしたナビゲーション・システム内において反復され拡張されるカルマン・フィルタを実現する方法及び装置に関する。

ナビゲーション・システムは、非線形動的システムの一例である。ナビゲーション・システムの開発に関係する問題の１つは、動的システムの様々な状態を推定することにある。そのような推定には、典型的には、ナビゲーション・システムのソフトウェアを用いる。拡張カルマン・フィルタ（extended Kalman filter）（ＥＫＦ）をナビゲーション・システム・ソフトウェア内で使用して、斯かる推定を行って来た。幾つかのナビゲーション・システムにおいて、ＥＫＦは、非線形システムに対してのテイラー級数展開及び観察方程式を適用し、一次項を用いて公知の線形カルマン・フィルタ理論を適用し、確率密度関数（ＰＤＦ）はガウス形であると推定される。

しかしながら、実際には、ＥＫＦには幾つかの制限があった。斯かる制限の１つは、ＥＫＦへ入力されることを許容される誤差は、小さいもののみであることである。さもなければ、非線形の誤差の挙動がある場合、共分散行列の更新において、一次の近似値が原因で、偏った解及び矛盾を生じさせることになり、これによりフィルタが不安定となる。ＥＫＦの二次バージョンも存在するが、実施及び計算が一段と複雑になるので、使用が困難な傾向にある。

一次の手法を改善する一般的な方法は、繰り返しＥＫＦ（反復ＥＫＦ、iterated EKF）であり、これは、名目状態推定（nominal state estimate）を再定義し、測定式を再び線形化することにより、現時点での観察においてＥＫＦ式を効果的に反復する。反復ＥＫＦでは、基本のＥＫＦと比べて、より良い性能を提供することが可能となり、特に、測定関数において著しい非線形性がある場合において、良い性能を提供することが可能である。

近年、低コストのＭＥＭＳをベースにしたセンサが利用できるようになり、慣性ナビゲーション・システム（ＩＮＳ）に利用できるようになっている。斯かるＩＮＳの応用は、航空機のナビゲーション、位置の決定および案内を含む。大抵のナビゲーション・システムはＧＰＳ範囲測定装置およびＩＮＳを含んでおり、ＩＮＳは、角速度、速度及び方位測定に関するデータを提供し、これらのデータを組み合わせて使用して移動体（例えば、航空機）の動きの測定を行なう。ナビゲーション・システムはまた、範囲誤差推定装置を含む。この誤差推定装置の出力に基づいて、移動体の位置を決定できる。誤差推定装置は、時にはカルマン・フィルタ及び平均化プロセスを使用して実施されることがある。

様々な測定装置の出力は、カルマン・フィルタ等を使用して補正され、それにより、移動体の位置を、高精度のセンサを使用することなく比較的高いレベルの精度で推定することができる。しかしながら、低コストのＭＥＭＳセンサは、ノイズ・レベルが高く、非線形の影響があり、測定の精度が低いので、従来のＥＫＦ推定は時間の経過と共に低下して信頼性が失われることになる。従って、ＩＮＳの精度は制限されたものになり、特に、ＧＰＳデータが利用できない場合には、制限されたものとなる。殆どの非線形カルマン・フィルタは、推定誤差を改善するために使用できるが、その実施は困難である。より詳しく述べると、その実施では、チューニングが困難であり、また、非線形の影響は或る状況においてのみ出現するので、推定方式を切り替えるのが困難である。

従って、現在利用可能である高価なナビゲーション・システムと同等のレベルの精度を提供できる低コストのナビゲーション・システムの必要性があり、斯かるナビゲーション・システムは未だ実現されていない。

本発明の一態様によれば、車両用ナビゲーション・システムが提供され、このシステムは、車両の運転に関する第１のセットのデータを提供するように構成された慣性測定装置と、車両の運転に関する第２のセットのデータを提供するように構成された位置決め装置とを含む。斯かるナビゲーション・システムは、更に、前記の慣性測定装置及び位置決め装置が提供するデータのセットを受信するように構成された処理装置を含む。斯かる処理装置は、更に、受信したデータのセットに関係する時間更新誤差状態及び測定更新誤差状態を用いてプログラムされる誤差処理装置を含む。誤差処理装置は、時間更新誤差状態及び測定更新誤差状態の双方を第１の基準に基づいて繰り返し、また、測定更新誤差状態のみを第２の基準に基づいて繰り返すように構成される。処理装置は、更新した誤差状態に基づいて、少なくとも車両の位置及び速度を推定するように構成される。

本発明の別の形態によると、物体の位置を推定する方法が提供される。この方法は、複数の外部ソースからのセンサ・データを受信し、外部ソースの少なくとも１つから受信したデータが物体の変化する位置を示すものであるか否かを判定し、受信したデータが変化する位置を示すものでない場合、受信したセンサ・データについて時間更新誤差状態及び測定更新誤差状態の双方を繰り返して物体の位置を決定し、受信したデータが変化する位置を示すものである場合、受信したセンサ・データについて測定更新誤差状態を繰り返して物体の位置を決定することを含む。

本発明の更に別の形態によれば、受信したセンサ・データに関しての時間更新誤差状態及び測定更新誤差状態を用いてプログラムされたカルマン・フィルタが、提供される。カルマン・フィルタは、第一の基準に基づいて時間更新誤差状態及び測定更新誤差状態の双方を繰り返し、第二の基準に基づいて測定更新誤差状態のみを繰り返すように構成される。

図１は、カルマン・フィルタ１２を組み込んだナビゲーション・システム１０のブロック図である。ナビゲーション・システム１０は、グローバル・ポジショニング・システム（ＧＰＳ）及び慣性ナビゲーション・システム（ＩＮＳ）２２を含む。ＧＰＳ２０からのＧＰＳデータ及びＩＮＳ２２からのセンサ・データは加算装置２４へ供給される。ＧＰＳデータはＧＰＳ測定誤差を含み、センサ・データは慣性誤差を含む。加算装置２４は、ＧＰＳ２０からのＧＰＳデータから、ＩＮＳ２２からのセンサ・データを減算し、測定誤差２６を出力する。測定誤差２６はカルマン・フィルタ１２へ入力され、カルマン・フィルタ１２は、前に提供された測定誤差に基づいて慣性誤差推定値を算出し、算出された慣性誤差推定値は加算装置３０へ供給される。加算装置３０はまた、ＩＮＳ２２からセンサ・データを受信する。慣性誤差推定値が、加算装置３０において、慣性誤差を含んだＩＮＳ２２からのセンサ・データから減算され、加算装置３０から位置推定値３２が出力されるが、慣性システムを使用しての位置決定に関係した誤差の少なくとも一部は、ＧＰＳ２０が生成したデータを用いて低減されている。

動作について、カルマン・フィルタ１２は、例えばＧＰＳ２０及びＩＮＳ２２から受信したデータの許容誤差に基づいた誤差状態を用いてプログラムされる。誤差状態の一例は、ＧＰＳの衛星内の原子時計と、ＧＰＳ受信機内の精度の落ちる時計との間での不正確性である。一実施例では、カルマン・フィルタ１２は拡張カルマン・フィルタ（ＥＫＦ）である。上述のように、非線形誤差の挙動とともに、大きな誤差がＥＫＦへ入力されると、一次近似が、偏った慣性誤差推定値及び共分散行列更新における不安定性をもたらし得る。非線形誤差のソースには、例えばＧＰＳデータのレイテンシが含まれ、このレイテンシは、ＧＰＳ受信機２２に対する衛星の位置に基づくものである。上述のように、二次バージョンのＥＫＦに関係した実施及び計算の複雑さにより、かかる二次バージョンのＥＫＦは使用されない傾向にある。

図２は、ナビゲーション・システム５０のブロック図であり、このナビゲーション・システム５０はナビゲーション処理装置５２を含み、カルマン・フィルタの実施を動的に切り替えて、上述の非線形誤差の処理の関係した問題の少なくとも一部に対処する。一実施例では、カルマン・フィルタの実施の動的切り替えが、ジャイロスコープのコンパスの整合時間に関係した問題に対処するために使用される。ナビゲーション・システムの実施に特有のことであるが、この動的切り替えにより、ナビゲーション処理装置５２は、位置の推定時に、受信した慣性データにおける非線形影響に対処し、また、ナビゲーション・システム５０のジャイロスコープのコンパスの整合時間を低減することができるようになる。

また、図２を参照すると、ナビゲーション・システム５０は、例えばＭＥＭＳベースのジャイロスコープ及び加速度計を有する慣性センサ６０、気圧高度計６２、ＧＰＳ受信機６４、真対気速度（ＴＡＳ）指示器６６、及び磁力計６８を含む。入力処理装置７０は、上述の感知装置からのデータを処理し、組み合わせたセンサ・データ７２をナビゲーション処理装置５２へ出力する。ナビゲーション処理装置５２は、反復拡張カルマン・フィルタ（ＩＥＫＦ）７４を使用して、上記の組み合わせたセンサ・データ７２から誤差を除去し、正確なナビゲーション出力データ７８を、ナビゲーション出力装置８０へ供給すると共に入力処理装置７０へ戻す。カルマン・フィルタ７４は、また、慣性センサ補正値８２を入力処理装置７０へ戻す。入力処理装置７０は、また、慣性センサ６０からのデータを処理して、組み合わせたセンサ・データ７２をナビゲーション処理装置５２へ出力する。ナビゲーション処理装置５２は、グローバル反復拡張カルマン・フィルタ（Ｇ−ＩＥＫＦ）を用いて、ジャイロスコープコンパスモードで作動する時の整合時間を低減する。

一実施例では、カルマン・フィルタ７４は、少なくとも位置誤差状態及び速度誤差状態を用いて初期設定されるものであり、推定技術を組み込んでおり、この推定技術によりナビゲーション・システム誤差の推定値が提供される。この推定技術は、カルマン・ゲイン行列の計算を含み、これにより、センサ６０、６２、６４、６６及び６８の一つ以上のものから受信した現在の測定値の精度と、センサ６０、６２、６４、６６及び６８に関係した誤差の以前の推定値の精度とを関連付ける。

一実施例では、ナビゲーション・システム５０の受信したホイール・センサ信号８４が、ナビゲーション・システム５０を組み込んだ航空機が地上にあるか否かを示す。説明する実施例では、ホイール・センサ信号８４がカルマン・フィルタ７４へ供給される。航空機が地上にあることを示す指示は、静止状態と呼ばれることがあり、ジャイロスコープのコンパスの整合は、航空機が静止状態にある時になされるのが典型的である。特定の実施例では、カルマン・フィルタ７４はホイール・センサ信号８４の状態を用いて、センサ６０、６２、６４、６６及び６８の時間更新誤差状態及び測定更新誤差状態（グローバル反復拡張カルマン・フィルタ（Ｇ−ＩＥＫＦ） モード）の双方の繰り返しと、センサ６０、６２、６４、６６及び６８の測定更新誤差状態（反復拡張カルマン・フィルタ（ＩＥＫＦ）モード）のみの繰り返しとの間で、切り替えを行なう。本書ではホイール・センサ８４について記載してあるが、センサからの信号やデータの組合せを利用して、カルマン・フィルタ７４を、センサ６０、６２、６４、６６及び６８の時間更新誤差状態及び測定更新誤差状態の双方の繰り返しと、センサ６０、６２、６４、６６及び６８の測定更新誤差状態のみの繰り返しとの間で切り替えるようにしても良いことは、当業者には明らかである。

上述のように、ナビゲーション・システム５０は高精度のリアルタイムのシステムであり、反復拡張カルマン・フィルタ（ＩＥＫＦ）７４を組み込んである。本書で更に説明するように、ＩＥＫＦ７４は、一回の繰り返しを実施する基本の線形のカルマン・フィルタと、何回かの繰り返しを実施する非線形のカルマン・フィルタとの双方として作動するように構成されている。単数回の繰り返しを行なうか又は複数回の繰り返しを行なうかの決定は、例えば、処理装置７０で受信する一以上の状態に基づいてなされる。この決定はまた、剰余や、共分散行列値や、誤差状態間の差や、入力処理装置７０で受信する一以上の信号に基づくものとしても良い。ナビゲーションの間、センサ６０、６２、６４、６６及び６８から受信するセンサ・データは変化し続け、線形及び非線形の双方の傾向を示す特性がる。システム５０は、非線形の傾向を動的に特徴づけるように、および収束情報に基づいてカルマン・フィルタ７４を適合させるように、構成されている。システム５０はまた、静止状態と非静止状態との間でモード切替を行って、カルマン・フィルタ７４を、Ｇ−ＩＥＫＦモードでの動作とＩＥＫＦモードでの動作とに適合させるように構成される。

本書に記載したカルマン・フィルタ７４を用いることで、様々な誤差状態に関係した非線形の影響を低減し、且つ、非線形カルマン・フィルタを、収束状態に基づいて、動的にオンまたはオフにすることができる。別な言い方をすれば、システム５０内のカルマン・フィルタ７４を、システム５０の受信するセンサ挙動または他の信号に基づいて、グローバル反復拡張カルマン・フィルタとして又は反復拡張カルマン・フィルタとして適合させて繰り返しを実施することができる。センサ挙動の分析の一例としては、現在の誤差状態ベクトルを前の誤差状態ベクトルと比較することが挙げられる。より詳細には、一実施例では、第１及び第２の誤差状態反復からのジャイロスコープ・データおよび誤差の比較を用いて、カルマン・フィルタ７４をどちらのモードで動作させるかを決定する。この分析には、センサ挙動が誤差の大きさの関数として線形であるか又は非線形であるかの判定が含まれ、また、カルマン・フィルタ７４の繰り返し時間でのジャイロスコープの非線形性の校正を更に含むようにしても良い。斯かる実施例は、システム５０による位置決定の精度を改善するものと考えられる。

反復拡張カルマン・フィルタの基本式は下記の通りである。

時間更新推定（グローバル）：

状態推定伝播

誤差共分散伝播

測定更新（グローバル＋ローカル）：

状態推定の初期設定

状態推定値更新

カルマン・ゲイン更新

誤差共分散更新

上記において、ｘは状態ベクトルであり、Ｆは状態遷移行列であり、Ｐは共分散行列であり、Ｑは動的攪乱ノイズの共分散であり、Ｒは測定ノイズの共分散であり、Ｈは測定感度行列であり、Ｋはカルマン・ゲインである。指数「ｉ」は反復に関して使用され、ｋは時間関連の指数である。上記式（１）乃至（６）から決定できるように、ローカル反復拡張カルマン・フィルタの実施では、反復の間に測定式（４）乃至（６）のみが更新される。

しかしながら、グローバル反復拡張カルマン・フィルタの実施では、全ての観測データに適用される。状態推定値が逆伝播されることから、一実施例では、ジャイロ・コンパスの整合のために、グローバル反復拡張カルマン・フィルタを用いる。グローバル反復拡張カルマン・フィルタの実施についての基本式は、ローカル拡張カルマン・フィルタの実施の基本式と類似であるが、グローバル反復に関しては、時間推定式（１）乃至（２）と測定式（４）乃至（６）との双方が更新される。グローバル反復の間に時間ステップを「仮想時間」として維持しているときに、静止状態に関しては測定数を低減することが可能であり、従来の拡張カルマン・フィルタ手法と比較して整合時間が大幅に短くなる。収束が達成されると、繰り返し（反復）が停止される。

反復がなされている間、誤差状態を判定するのに用いられる測定式における剰余は、誤差を低減するために何を低減する必要があるかについての指示を提供する。加えて、カルマン・フィルタ７４に関係したカルマン・ゲインは、次の誤差状態判定へのステップの大きさを決定するときに、因数分解される。特に、カルマン・フィルタ７４は、時間更新式及び測定更新式の双方を繰り返すことで、ジャイロスコープのコンパスの整合時間を低減する。斯かる繰り返しは、基本の拡張カルマン・フィルタを使用するときの短時間ステップの使用と実質的に等しい。例えばマイクロ電気機械装置（ＭＥＭＳ）ベースの慣性システムからのデータのような、様々な低価格のセンサからのデータに存在するノイズ・レベルが原因で、拡張カルマン・フィルタの時間ステップを非常に小さいものとすることができない。しかしながら、斯かる時間ステップを使用して、校正中にセンサ・データの非線形性を測定するようにしても良い。

非線形性を測定するための別の基準は、現在の誤差ベクトルと前の誤差ベクトルとの比較である。例えば、反復拡張カルマン・フィルタの第１の反復は基本拡張カルマン・フィルタと同じである。この第１の反復で、受信したセンサ・データの非線形の影響が除去できない場合は、フィルタが更に反復を行う。一実施例では、図２を再び参照すると、リアルタイム・ナビゲーション・システム５０が、２つのレート処理装置（図示せず）を備えて構成されて、上記の反復を可能にする。一方のレート処理装置を使用して上記のカルマン・フィルタを実施して比較的低速度で動作するようにし、他方のレート処理装置は高レート処理装置でありセンサ・データの更新に使用される。カルマン・フィルタの実施において、一実施例では、ｆｏｒループを使用して反復を実施する。反復の間に、センサ・データの測定を除いての、他のナビゲーション・パラメータが更新されるが、これには誤差状態ベクトル及び動的共分散が含まれる。

図３は、特定の実施例、例えば、物体の位置の推定値を提供するプロセスにおいてカルマン・フィルタ７４により行われるプロセスを例示したフローチャート１００である。カルマン・フィルタ７４は外部ソース（即ち、センサ６０、６２、６４、６６、６８及び８４）からセンサ・データを受信する（１０２）。センサ・データの少なくとも幾つかには、時間更新誤差状態及び測定更新誤差状態が含まれる。カルマン・フィルタ７４は、更に、外部ソースの少なくとも１つからのデータが、物体が位置を変更していないことを示すか否かを判定する（１０４）。

物体が位置を変更しているようであれば、例えば、測定更新誤差状態のみを繰り返し（１０６）、非線形の影響を除去するようにし、反復数は収束状態に応じて動的に変更される。物体が位置を変更していない（本書の前段において静止状態と呼んだ）ようであれば、時間更新誤差状態及び測定更新誤差状態の双方を繰り返し（１０８）、ジャイロスコープのコンパスの整合時間を低減する。

上述のように、また、一実施例において、ＩＮＳデータ、ＧＰＳデータ及び他のセンサ・データの統合したものに反復拡張カルマン・フィルタを提供する方法が説明され、また、ＩＮＳ、ＧＰＳ及び他のセンサからのセンサ・データのリアルタイムの統合のための反復拡張カルマン・フィルタを組み込んだＩＮＳ／ＧＰＳベースのナビゲーション・システムが説明されている。本書に記載したナビゲーション・システムは、カルマン・フィルタにより処理される他のセンサ・データのソースを含み、斯かるデータは、例えば、真対気速度及び気圧高度データなどである。本発明のナビゲーション・システムは、反復拡張カルマン・フィルタの動作に基づき、公知のＩＮＳ／ＧＰＳベースのナビゲーション・システムよりも堅牢である。カルマン・フィルタが、ＧＰＳから受信した更新におけるレイテンシや、例えば、反復拡張カルマン・フィルタにより誤差状態が生じる気圧高度計の非線形機能性に、対処することが出来ることから、更なる堅牢性がもたらされる。

斯かる装置の誤差状態を、従来の拡張カルマン・フィルタで生じさせようとするときには、斯かるレイテンシ及び非線形性は、典型的には不正確であるか又は実用的でない。反復拡張カルマン・フィルタに関しての本書に記載した実施例では、状態ベクトルを推定するプロセスの間に、非線形の影響を低減する。特に、カルマン・フィルタの非線形処理の能力を、ナビゲーション・システムが受信する信号又は他のセンサ・データに基づいて、動的にオン／オフすることが出来る。従って、反復拡張カルマン・フィルタ７４を備えたナビゲーション・システム５０は、１回の繰り返しのみが必要である場合の拡張カルマン・フィルタとしての動作と、例えば、反復されるデータに関係した収束状態に基づいて、より多くの繰り返しが必要とされる場合の反復拡張カルマン・フィルタとしての動作との、双方に適合する。

ナビゲーション・システム５０は、また、時間更新及び測定更新の双方を使用して、例えば、ジャイロスコープのコンパスの整合時間を低減するグローバル反復拡張カルマン・フィルタ（Ｇ−ＩＥＫＦ）としての動作と、測定式のみを更新して非線形の影響を除去する反復拡張カルマン・フィルタ（ＩＥＫＦ）としての動作との双方に適合するように構成されたカルマン・フィルタ７４を備える。Ｇ−ＩＥＫＦモードでの動作は、カルマン・フィルタ内で時間更新及び測定更新の式の双方の反復が行なわれ、それにより、静止状態の測定に関して、より効果的に非線形性の影響を低減する。上述のように、斯かる静止状態の測定の一例は、ジャイロスコープのコンパスを地上で整合させることである。

ナビゲーション・システムの動作に関して本発明を説明してきたが、本書に説明した実施例は、他のカルマン・フィルタを使用した装置における使用にも適用できる。特に、１回のみの反復が必要である場合における拡張カルマン・フィルタとしての動作と、例えば、反復されるデータに関係した収束状態に基づいて、より多くの繰り返しが必要とされる場合における反復拡張カルマン・フィルタとしての動作との双方に適合できるように構成されたカルマン・フィルタは、状態の推定を用いる任意の装置に適用できる。

本発明を特定の種々の実施例に関して説明してきたが、当業者には、本発明を、その趣旨及び範囲を逸脱することなく様々に変更して実施できることが、自明であろう。

図１は、ナビゲーション・システム内で実施されるカルマン・フィルタのブロック図である。 図２は、多数の動作モードにおいて位置解を提供するように動作する反復カルマン・フィルタを組み込んだ多入力ナビゲーション・システムのブロック図である。 図３は、図２のナビゲーション・システムのカルマン・フィルタの動作を例示したフローチャートである。

Claims (3)

1. 車両用のナビゲーション・システムであって、
   前記車両に関する慣性データを提供するように構成された慣性測定装置と、
   前記車両に関する位置データを提供するように構成された位置決め装置と、
   前記慣性測定装置が提供する前記慣性データ及び前記位置決め装置が提供する前記位置データを受信するように構成された処理装置であって、前記処理装置により受信した前記データに関連する時間更新誤差状態及び測定更新誤差状態を用いてプログラムされるカルマン・フィルタを備え、前記カルマン・フィルタは、前記車両が静止していることをデータが示すときに前記時間更新誤差状態及び前記測定更新誤差状態の双方を反復し、前記車両が位置を変更していることをデータが示すときに前記測定更新誤差状態のみを反復するように構成されるものであり、前記処理装置が更に、更新された前記時間更新誤差状態及び前記測定更新誤差状態に基づいて少なくとも前記車両の位置及び速度を推定するように構成された、処理装置と
   を備えるナビゲーション・システム。
2. 請求項１に記載のナビゲーション・システムであって、
   前記車両が静止していることをデータが示すということと、前記車両が位置を変更していることをデータが示すということとの判定は、前記車両から受信した１以上の信号に基づいてなされ、
   前記車両から受信した前記信号は、ホイール下降信号、ホイール加重信号、及びブレーキ信号のうちの１以上のものを含む、
   ナビゲーション・システム。
3. 請求項１に記載のナビゲーション・システムであって、
   前記処理装置が更に、気圧高度計、真対気速度指示器、及び磁力計のうちの１以上のものからデータを受信するように構成され、
   前記カルマン・フィルタが、少なくとも位置誤差状態及び速度誤差状態で初期設定される、
   ナビゲーション・システム。

Images