(57)【要約】【目的】高精度を達成するためにGPS測定結果に加えて車両走行状態を利用する「ナビゲーション・システムおよびナビゲーション方法」を提供する。【構成】MEMS（超小型電子・機械式システム、Micro Electro Mechanical System）と呼ばれる安価で低精度の慣性センサーを使用する慣性航法システム（INS）とGPSを複合したナビゲーション・システムにおいて、(1)慣性センサーから得られる加速度と角速度を用いて予め複数の車両走行状態を定義し、(2)INSに搭載された慣性センサーから車両の各座標軸における加速度と角速度を受信し、(3)該加速度と角速度を評価することで車両の現在の走行状態を判別し、(4)判別された車両走行状態に応じた値を求め、該値を測定値としてINSとGPSを複合するカルマン・フィルタに送り、(5)車両走行状態を考慮してカルマン・フィルタ処理を行って車両の位置、速度及び姿勢の最適な推定値を得る。【選択図】図１
2008076389.tif 000002
【特許請求の範囲】
【請求項１】
慣性航法システム（Inertial Navigatoin System: INS）とGPSを複合したナビゲーション・システムにおけるナビゲーション方法において、
慣性センサーから得られる加速度と角速度を用いて予め複数の車両走行状態を定義し、
前記INSに搭載された慣性センサーから車両の各座標軸における加速度と角速度を検出し、
前記慣性センサーから取得した前記加速度と角速度を評価することで前記車両の現在の走行状態を判別し、
前記判別された車両の走行状態に応じた値を求め、該値を測定値として前記INSとGPSを複合するカルマン・フィルタに送ってカルマン・フィルタ処理を行い、
現在の車両位置、速度及び姿勢の最適な推定値を得るために上記処理を繰り返す、
ことを特徴とするナビゲーション方法。
【請求項２】
前記車両走行状態の内の一つは、車両座標系に関して側方と下方の速度がゼロと推測される通常走行状態である、
ことを特徴とする請求項１記載のナビゲーション方法。
【請求項３】
前記車両走行状態の内の一つは、コーナリング状態である、
ことを特徴とする請求項１記載のナビゲーション方法。
【請求項４】
前記車両走行状態の内の一つは、車両静止状態である、
ことを特徴とする請求項１記載のナビゲーション方法。
【請求項５】
前記慣性センサーは、車両座標系の各軸方向の加速度を検出する三軸加速度計と、車両座標系の各軸周りの角速度を検出する三軸ジャイロスコープを備えている、
ことを特徴とする請求項１記載のナビゲーション方法。
【請求項６】
前記コーナリング状態で,車両座標系に関して前方向の速度を、車両座標系側方向の加速度と垂直軸周りの角速度とによって推定し、前記測定値としてカルマン・フィルタに入力する、
ことを特徴とする請求項３記載のナビゲーション方法。
【請求項７】
前記慣性センサーは三軸ジャイロスコープを備えており、車両静止状態と判定された時には、三軸ジャイロスコープの全ての出力はゼロであると仮定し、前記測定値としてカルマン・フィルタに入力する、
ことを特徴とする請求項４記載のナビゲーション方法。
【請求項８】
前記検出された車両走行状態に応じた測定値を、GPS測定結果の出力周期よりも高周期でカルマン・フィルタに入力する
ことを特徴とする請求項１記載のナビゲーション方法。
【請求項９】
前記検出された車両の走行状態に応じた測定値を、10Hzの周期でカルマン・フィルタに入力し、また前記GPS測定結果を1Ｈｚの周期でカルマン・フィルタに入力する
ことを特徴とする請求項８記載のナビゲーション方法。
【請求項１０】
慣性航法システム（INS）とGPSを複合したナビゲーション・システムにおいて、
加速度センサーと角速度センサーを組み込んでなる慣性測定ユニット（IMU）を有する慣性航法システム（INS）、
車両の絶対位置及び速度を検出するために複数の人工衛星から人工衛星信号を受信するGPS受信機機、
前記INSの出力と前記GPS受信機機の出力とを複合すると共にカルマン・フィルタ処理を実行するカルマン・フィルタ、
前記慣性センサーの出力から車両の走行状態を検出し、該走行状態に応じた処理を前記カルマン・フィルタに実行させるコントローラ、
を備え、前記車両走行状態は、前記車両の通常走行状態、コーナリング状態及び静止状態である、
ことを特徴とする複合ナビゲーション・システム。
【請求項１１】
前記慣性センサーは、車両座標系の各軸方向の加速度と各軸周りの角速度を検出できるように三軸の加速度計と三軸のジャイロスコープを備えている、
ことを特徴とする請求項１０記載のナビゲーション・システム。
【請求項１２】
前記通常走行状態は車両座標系に関して側方と下方の速度がゼロと推測される状態である、
ことを特徴とする請求項１０記載のナビゲーション・システム。
【請求項１３】
前記コーナリング状態は、車両座標系に関して前方向の速度が側方向の加速度と垂直軸周りの角速度とから力学的に推測される、かつ、側方向の速度が前方向の推測速度の関数から推測される、かつ、下方向の速度がゼロと推測される状態である、
ことを特徴とする請求項１０記載のナビゲーション・システム。
【請求項１４】
前記静止状態は、車両座標系の三軸方向全ての速度が、停車状態を検出後にゼロと推測される状態である、
ことを特徴とする請求項１０記載のナビゲーション・システム。
【請求項１５】
慣性航法システム（INS）とGPSナビゲーション・システムを複合したナビゲーション・システムにおいて、
慣性センサーから得られる加速度/角速度を用いて予め複数の車両走行状態を定義する手段、
前記INSに搭載された慣性センサーから車両の各座標軸における加速度と角速度を検出する手段、
前記慣性センサーから取得した前記加速度と角速度を評価することで前記車両の現在の走行状態を判別する手段、
前記判別された車両の走行状態に応じた値を求め、該値を測定値として前記INSとGPSを複合するカルマン・フィルタに送り、該測定値を用いたカルマン・フィルタ処理を行う手段、
現在の車両の位置、速度及び向きの最適な推定値を得るために上記処理を繰り返す手段、
を備えたことを特徴とするナビゲーション・システム。
【請求項１６】
前記車両走行状態の内の一つは、車両座標系に関して側方と下方の速度がゼロと推測される通常走行状態である、
ことを特徴とする請求項１５記載のナビゲーション・システム。
【請求項１７】
前記車両走行状態の内の一つは、コーナリング状態である、ことを特徴とする請求項１５記載のナビゲーション・システム。
【請求項１８】
前記車両走行状態の内の一つは、車両静止状態である、ことを特徴とする請求項１５記載のナビゲーション・システム。
【請求項１９】
前記慣性センサーは、前記車両座標系の各軸方向の加速度と各軸周りの角速度を検出できるように三軸の加速度計と三軸のジャイロスコープを備えている、
ことを特徴とする請求項１５記載のナビゲーション方法。
【請求項２０】
前記コーナリング状態で、車両座標系に関して前方向の速度を、車両座標系側方向の加速度と垂直軸周りの角速度とによって力学的に推測し、測定値としてカルマン・フィルタに入力する、
ことを特徴とする請求項１７記載のナビゲーション・システム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ナビゲーション・システムおよびナビゲーション方法に係わり、特にＭＥＭＳ（超小型電子・機械式システム、MicroＥlectro Mechanical System）と呼ばれる安価で低精度の慣性センサーを用い、かつ、信頼できるGPS信号が長時間得られないときでも、高い位置推定精度を維持するためのナビゲーション・システムおよびナビゲーション方法に関する。
【背景技術】
【０００２】
慣性航法システム（Inertia Navigation System: INS）は、車両の位置、速度、及び姿勢の推定（ナビゲーション）及び誘導（ガイダンス）のために広く使用されている技術である。INSは慣性測定ユニット（Inertial Measurement Unit: IMU）とプロセッサとから構成されている。ここでIMUは加速度計とジャイロスコープを備え、これらは慣性座標系に対するプラットフォームの動きを検出する慣性センサーである。INSの重要な長所は、外部の支援から独立している、即ちそれ自身で自律している点である。しかし、INSは、長い移動距離では高い精度を提供できない。なぜなら慣性センサーの出力に含まれるノイズなどのエラーが時間の経過とともに積分計算に伴って蓄積するため、運用時間が長ければ長い程、不正確さが大きくなるからである。
【０００３】
一方全地球測位システム（Global Positioning System: GPS）における最近の開発によって高精度な動体ナビゲーションが低コストで実現できるようになった。しかし、GPSは人工衛星からの信号に依存しているので、妨害電波やRF（無線周波数）干渉あるいはマルチパス問題といった環境からの影響を受けやすい。即ちGPSは、長時間に渡って有限の誤差範囲で位置と速度の推定を与えてくれるが、しばしば大きなマルチパス・エラーを引き起こしたり、受信不能の状態に陥ったりする。そこで、GPSとINSの出力をカルマン・フィルタを使用して結合することにより、両システムの性能上の問題を補間・解決するための努力が行われている。
【０００４】
慣性航法システム（INS）はかつては高価でおおがかりな装置であったので、航空宇宙や軍事用ナビゲーションなど、精密さを問われるアプリケーションにのみ使用されていた。そこでIMUパッケージをコンパクトで安価な方式で設置可能とするために、超小型電子・機械式システム（MicroElectro Mechanical System: MEMS）のセンサーを開発する努力がなされてきた。低コストで小さい、しかしノイズが大きいMEMS慣性センサーが商品化されて以来、多くの研究者がMEMS慣性測定ユニット（IMU）をベースにしたINS/GPSナビゲーション・システムにおいて、GPS信号が喪失した際、短時間でナビゲーション精度が劣化するのを防ぐ方法を研究をしてきた。例えばビデオ・センサー、高度計又は磁力計などの補助ソースの使用が提案される一方、カルマン・フィルタのような新しいフィルタ理論を創造することでより一般的な解決策を見出そうとする動向もある。ただ、そのような新しいフィルタ技術を使用したとしても、MEMSをベースにしたINS/GPSシステムは、GPS信号喪失が10秒ほど続くと発散する傾向にある。
【発明の開示】
【発明が解決しようとする課題】
【０００５】
従って、GPS信号が失われたり、又は長時間に渡って使用不能になっても、高いナビゲーション精度を維持することができるMEMSベースの複合INS/GPSナビゲーション・システムが必要とされている。
【０００６】
本発明の目的は、MEMSベースの複合INS/GPSナビゲーション・システムにおいて高精度を達成するために、GPS測定結果に加えて車両走行状態を利用する方法を提供することである。
【０００７】
本発明の別の目的は、信号を取るための接続が困難な速度パルス・センサーを使用する必要がなく、しかも低コストのMEMSセンサーを使用でき、更にGPSの信号喪失期間が長く続いた場合でも高精度な位置、速度、及び姿勢の推定（ナビゲーション）を行うことができる低コストの複合INS/GPSナビゲーション・システムを提供することである。
【０００８】
本発明の別の目的は、車両への適用に焦点を当て、車両走行状態を識別してナビゲーション精度劣化を抑制し、結果的にGPS信号喪失が長時間続いたとしても正確に車両の軌跡を推定することである。
【課題を解決するための手段】
【０００９】
本発明の第１は安価なMEMS慣性センサーを使用した慣性航法システム（INS）とGPSを複合したナビゲーション・システムにおけるナビゲーション方法であり、慣性センサーから得られる加速度/角速度を用いて予め複数の車両走行状態を定義するステップ、前記INSに搭載された慣性センサーから車両の各座標軸における加速度と角速度を受信するステップ、前記慣性センサーから取得した前記加速度と角速度を評価することで前記車両の現在の走行状態を判別するステップ、前記判別された車両の走行状態に応じた値を求め、該値を測定値として前記INSとGPSを複合するカルマン・フィルタに送ってカルマン・フィルタ処理を行うステップ、現在の車両位置、速度及び姿勢の最適な推定値を得るために上記処理を繰り返すステップ、を備えている。
【００１０】
本発明の第２は安価なMEMS慣性センサーを使用した慣性航法システム（INS）とGPSを複合したナビゲーション・システムであり、MEMS加速度センサーと角速度センサーからなる慣性測定ユニット（IMU）を有する慣性航法システム（INS）、車両の絶対位置及び速度を検出するために複数の人工衛星から人工衛星信号を受信するGPS受信機、前記INSの出力と前記GPS受信機の出力とを複合する処理を実行するカルマン・フィルタ、前記センサーの出力から車両の走行状態を検出し、該走行状態に応じた処理を前記カルマン・フィルタに実行させるコントローラ、を備えている。なお、前記車両走行状態は、前記車両の通常走行状態、コーナリング状態及び静止状態である。
【発明の効果】
【００１１】
本発明によれば、カルマン・フィルタを使用することでGPSの出力とINSの出力とが複合される従来の複合INS/GPSナビゲーション・システムに加えて、予め定義された車両走行状態に応じた値がカルマン・フィルタに与えられ、カルマン・フィルタ処理により車両の現在位置、速度及び姿勢の最適推定値を得るための処理を行う。結果的に、車両の運動を推定する際に起きるエラーは、GPSによって修正されるばかりではなく、車両走行状態にに応じた値よっても修正される。車両の走行状態に応じた値はGPS測定結果の出力周波数よりも高い周波数で参照されるため、エラーが短時間内に修正され、蓄積されるエラーの量は少なくなる。このため、低コストでノイズの多いMEMESセンサーを複合INS/GPSナビゲーション・システムに使用することができる。また、本発明によれば、速度パルス・センサーを使用する必要がない。
【発明を実施するための最良の形態】
【００１２】
本発明を添付図面を参照して詳細に説明する。
【００１３】
図１は、車両に搭載される本発明の複合INS/GPSナビゲーション・システム２０の基本的な構成の例を示す概略ブロック線図である。この図は、GPSの内部カルマン・フィルタと、GPS出力と慣性センサ出力を複合するための複合カルマン・フィルタ、合計２つのカルマン・フィルタがシステムに存在する緩散結合INS/GPS (Loosely Coupled INS/GPS) と呼ばれるシステムである。これに対して、GPSの受信する擬似距離（衛星と受信機の距離）、擬似距離変化率と慣性センサー出力を直接１つの複合カルマン・フィルタに入力する緊密結合INS/GPS (Tightly Coupled INS/GPS) システムも存在するが、本発明による付加的な条件は、いずれのシステムにおいても同じように参照するので、等しく効果的である。さて、ここで例にあげる緩散結合INS/GPSナビゲーション・システム２０は、車両走行状態を検出し、カルマン・フィルタに該車両走行状態に応じた処理を行わせるVDC(Vehicle Dynamics Conditioning）コントローラ２５と、慣性航法システム（INS）３０と、GPS受信機４０と、カルマン・フィルタ５０（KF−2）とを有している。今日自動車に広く使用されている通常のGPSナビゲーション・システムと異なって、本発明の複合INS/GPSナビゲーション・システム2０は、自動車の移動距離を検出するために速度パルス・センサーを使用していない。従って、速度パルス・センサーをナビゲーション・システムのプロセッサに接続するための複雑な配線を車両内に設ける必要は無い。更に、タイヤ一回転当たりの速度パルス数の定義が自動車メーカ毎に異なっているために従来技術で必要であった、自動車メーカに対応したソフトウエア上の速度パルス数の定義変更をする必要が無い。
【００１４】
図１では、VDCコントローラ２５は、本発明の車両走行状態制御を含む本ナビゲーション・システム全般に渡る動作を制御するための主CPUの一部分となっている。INS３０は、慣性測定ユニット（IMU）３２と、ローパス・フィルタ・ユニット３４と、ナビゲーション計算ユニット３６とによって構成されている。GPS受信機４０は、GPS信号の受信機回路とマイクロプロセッサ（図示されていない）の他にカルマン・フィルタ４２（KF−1）を有している。カルマン・フィルタ５０（KF−2）は、測定値と推定値の差から、補正量を計算するためのゲイン（係数）を与える周知のカルマン・ゲイン・ユニット５２を有している。
【００１５】
IMU３２は、慣性センサーとマイクロプロセッサとを有している。慣性センサーは、車両の3つの座標軸X，Y，Zの加速度と各軸周りの角速度を検出するためにMEMS（超小型電子・機械式システム）技術により造られる。マイクロプロセッサは、慣性センサーからの検出された信号を処理する。IMU３２は、例えば100回/秒（100Hz）で出力データを発生する。IMU３２からの出力データは、その高周波成分が除去されるローパス・フィルタ・ユニット３４に送られる。ローパス・フィルタ・ユニット３４からの出力データは、車両の現在の位置、速度及び姿勢を慣性航法技術により推定するナビゲーション計算ユニット３６に送られる。
【００１６】
GPS受信機４０は、複数の人工衛星から信号を受信し、時計信号と人工衛星からの位置及び速度のデータを比較することで車両の所在場所と速度とを推定する。一般的に、GPS受信機40は、人工衛星信号に含まれるノイズの影響を最少にするためにカルマン・フィルタ（KF−1）によって得られる位置と速度の推定を最適化する。通常、GPS受信機４０は、１秒毎に（1Hzで）位置と速度の推定データを発生する。
【００１７】
INS３０により推定された位置、速度、姿勢のデータとGPS受信機４０により推定された位置と速度データは、カルマン・フィルタ（KF−2）50によって複合される。このカルマン・フィルタはノイズの多い測定データに基づいてナビゲーション・システムの状態量を実時間で推定誤差が最小になるように推定するものである。カルマン・ゲイン・ユニット５２は、測定値と推定値の差から、補正量を計算するためのゲイン計算する。カルマン・フィルタ５０の出力は、ナビゲーションの地図表示スクリーン（図示されていない）上に表示する車両位置を算定するナビゲーション計算ユニット３６に与えられる。
【００１８】
また、VDCコントローラ２５は、ローパス・フィルタ・ユニット３４を介して得られたIMU３２の出力データから、車両が本発明で定義した走行状態の内の何れの状態に現在なっているのか決める。後で詳細に説明するように、本発明は、現在の車両走行状態を通常走行状態、コーナリング状態、静止状態のいずれかの状態として分類する。そして、VDCコントローラ２５は、予め定義した車両走行状態の内の一つに応じた値を測定値としてカルマン・フィルタ５０に与える。これにより、カルマン・フィルタ５０は推定値を更に最適化するために該測定値を利用する。VDCコントローラ２５は、GPS測定よりも高速で、例えば10回/秒（10Hz）の速度で測定データを発生する。
【００１９】
図２Aは、図１のINS３０に使用されるIMU３２の基本的な構成を示した概略ブロック線図である。IMU３２は、プロセッサ３３と、３台の加速度計Acc1−3と3台のジャイロスコープGyro1−3から成る慣性センサーとを有している。加速度計Acc1−3は、車両の三つ座標方向（X，Y，Z）における加速度を検出し、またジャイロスコープGyro1−3は、車両の三つ座標軸（X，Y，Z）周りの角速度を検出する。プロセッサ３３は、慣性センサーAcc1−3、Gyro1−3からの信号に基づいて加速度と角速度を出力する。
【００２０】
図２Ｂと図２Ｃは、IMU本体座標系と車両座標系とを各々描いている概略線図である。図２ＢのP、Q、Rは、IMU座標系ｘb、ｙb、ｚbの軸周りの角速度を表している。図２Ｃのp、q、rは、車両座標系ｘv、ｙv、ｚvの軸周りの角速度を表しており、IMU３２が車両に固定されているのでP、Q、Rから車両座標系のp、q、rに簡単に変換されうる。
【００２１】
図３は、本発明に係るナビゲーションシステムの基本的作動プロセスの例を示しているフローチャートである。上述のように、本発明の方法は、低コストでノイズの多いＭＥＭＳセンサーを使用しているにも拘らず高いナビゲーション精度を達成するために、GPS測定に加えて車両走行状態を利用する。先ず工程１０１では、本プロセスは、加速度と角速度を用いて三つの車両走行状態を前もって定義している。上述のように、三つの走行状態とは、車両の通常走行状態とコーナリング状態と静止状態である。
【００２２】
次に工程１０２では、複合INS/GPSナビゲーション・システムは、車両に搭載されたIMU３２において３台の加速度計と３台のジャイロスコープの出力を評価する。３台の加速度計の出力に基づいて、IMU３２は、車両の三つの座標方向（X，Y，Z）における加速度を検出する。また、３台のジャイロスコープの出力に基づいて、IMU３２は、車両の三つの座標軸（X，Y，Z）周りの角速度を検出する。車両の三つの座標の加速度と角速度に基づいて、工程１０３において複合INS/GPSナビゲーション・システム２０のVDCコントローラ２５（図１）は、どの車両走行状態が車両の現状に相当しているかについて決定し、決定された走行状態に応じた値を測定値として引き出す。
【００２３】
工程１０４では、複合INS/GPSナビゲーション・システム２０のVDCコントローラ２５は、走行状態に応じた測定値を該ナビゲーション・システムのカルマン・フィルタ５０（図１のＫＦ２）に送る。カルマン・フィルタ５０は、工程１０５においてカルマン・フィルタ処理を実行し、その際現在の車両走行状態に応じた測定値を使用し、それによってナビゲーション・システムの推定エラーを最少にする。
【００２４】
工程１０６では、複合INS/GPSナビゲーション・システム２０は、ナビゲーション精度を連続して最適化するために前記の工程１０２から工程１０５を反復する。その際、慣性センサー（加速度計とジャイロスコープ）は、センサー出力を25回/秒（25Ｈｚ）程度発生し、またVDCコントローラ２５は、車両走行状態に応じた測定値をカルマン・フィルタに5〜10HzなどでGPS測定周期よりも高い周期で提供する。GPS受信機からは一般的には車両の絶対位置と速度を示すデータが1Hzの周期で発生するため、車両走行状態に基づいたエラー修正はGPS出力の参照受信機よりも数倍早く実施され、それによってエラー蓄積を最少化する。その結果、たとえGPS信号が２分以上の長時間に渡って利用不能な場合でも、本発明の複合INS/GPSナビゲーション・システム２０は、比較的高い推定精度を維持することができる。
【００２５】
上述のように、本発明は車両走行状態を通常走行状態、コーナリング状態、静止状態の三つの状態により定義する。
・通常走行状態：車両が真っ直に運転されている走行状態であり、車両座標系のyv軸方向（側方）の速度ｖyvとzv軸方向（下方）の速度ｖzvはゼロであると推測する。
・コーナリング状態：車両がカーブを切っている走行状態であり、車両座標系のｘv軸方向（前方）の速度ｖxvは、車両座標系のyv軸方向（側方）の加速度とｚv軸（下方）周りの角速度から力学的に推測する。かつ、yv軸方向（側方）の速度ｖyv（横滑り速度）を、推測されたｘv軸方向（前方）の速度ｖxvの関数で推測する。さらに、zv軸方向（下方）の速度ｖzvはゼロであると推測する。
・静止状態：車両が静止していると識別される状態であり、車両座標系の三軸方向全ての速度と角速度はゼロであると推測する。
【００２６】
各状態について以下に詳細に説明する：
（１）通常走行状態
ｒ＝車両固定座標系ｚ軸（図２Ｃ）回りの角速度とした時、|ｒ|≦9.2度/秒（0.16rad/s）を通常走行状態の条件として設定する。その際、以下の条件を定義する：
・車両座標系のyv軸方向（側方）の速度ｖyvとzv軸方向（下方）の速度ｖzvはゼロであると力学的に推測する。
但し、ｖyv及びｖzvは、各々車両座標系ｙv、ｚv（図２Ｂ、図２Ｃ参照）に関する車両速度である。このときｘv軸方向（前方）に関しては自由に運動している状態であり、力学的な条件は存在しないので、速度ｖxvに関する条件付けは行わない。
これらの力学的な条件をカルマン・フィルタで実現するために、次の(1)式のｖyv_obsおよびｖzv_obsをvyvおよびvzvの測定値として誤差指標と共にカルマン・フィルタ５０で使用する。添え字_obsがついたパラメータは、それらの測定値（Observation）であることを示す。
【００２７】
【数１】
2008076389.tif 000003
この式は、0≦ |ｒ|≦2.3度/秒のときに、横滑り速度の推測であるｖyv_obs = 0 m/sが、
2008076389.tif 000004
程度の誤差を含み、また、下方向速度の推測であるｖzv_obs = 0 m/sが
2008076389.tif 000005
程度の誤差を含むということを意味している。更に2.3 <|ｒ|≦9.2度/秒のときには、回転運動に伴いより大きな横滑り速度が発生する可能性があるため、ｖyv_obs = 0 m/sの推測に関する誤差指標を
2008076389.tif 000006
と大きくしている。
通常走行状態が適用されたとき、GPS測定値の他に上記(1)式の測定値がカルマン・フィルタ５０で使用される。これにより、車両が直線走行するだけでGPS測定値に加え2つの測定値を得ることができ、軌跡や姿勢の推定を含むナビゲーション精度が向上する。
（注１）ｒを求めるために使われるジャイロスコープの出力データは、カルマン・フィルタで推定しているバイアスを基に修正した後に使用する。
（注２）上記条件設定中、
2008076389.tif 000007
及び閾値は、センサー性能と設計者の選択に左右される設計パラメータである。
【００２８】
（2）コーナリング状態
図４は、車両がカーブを切る際のベクトル・パターンを図示しており、図４Ａは左にカーブを切る状態を示し、図４Ｂは右にカーブを切る状態を示し、また図４Ｃは、パターン(1)が釣り合いターンを示し、パターン(2)が横滑りターンを示し、パターン(3)が横滑りターンとオーバーシュートを示している場合のコーナリング状態を示している。
【００２９】
先ず、コーナリング状態とみなす条件を|ｒ|＞9.2度/秒（0.16rad/s）として設定する。なお、ｒは車両座標系垂直軸周りの角速度である。その際、以下の条件を定義する：
・車両座標系のｘv軸方向（前方）の速度ｖxvは、車両座標系のyv軸方向（側方）の加速度とｚv軸（下方）周りの角速度から力学的に推測する。かつ、yv軸方向（側方）の速度ｖyv（横滑り速度）を、推測されたｘv軸方向（前方）の速度ｖxvの関数で推測する。さらに、zv軸方向（下方）の速度ｖzvはゼロであると推測する。
これらの力学的な条件をカルマン・フィルタで実現するために、次の(2)式のvxv_obs、ｖyv_obs及びｖzv_obsを、vxv、vyvおよびvzvの測定値として誤差指標と共にカルマン・フィルタ５０で使用する。
【００３０】
【数２】
2008076389.tif 000008
但し、ａxv、ａyv及びａzvは、車両座標系に変換された加速度計の出力であり、その場合バイアスと重力はカルマン推定値によって較正される。
【００３１】
第一行のｖxv推定値は、以下に説明されているようにａyvとｒとの間の関係によって得ることができる（図４Ａ、図４Ｂを参照）。
・左へターン(r<0)の場合
左へターンするとき、次式
【数３】
2008076389.tif 000009
が成立する。ただし、Lは回転半径、
2008076389.tif 000010
は、Lの一次時間微分、二次時間微分値である。なお(3)式は、以下に示されるように、回転する座標系上の微分方法によって導かれた一般的な式である。
2008076389.tif 000011
但しｐは位置（Position）を示す。
(3)式より、次式が得られる。
【数４】
2008076389.tif 000012
(4)式において、車両プラットフォームでは、通常右辺第1項は小さいと仮定でき
【数４ａ】
2008076389.tif 000013
となる。
【００３２】
・右へターン(r>0)の場合
右へターンするとき、次式
【数５】
2008076389.tif 000014
が成立する。なお数式（5）は、以下に示されるように、回転する座標系上の微分方法によって導かれた一般的な式である。
2008076389.tif 000015
(5)式より(６)式
【数６】
2008076389.tif 000016
が得られる。車両プラットフォームでは、通常右辺第1項は小さいと仮定でき
【数６ａ】
2008076389.tif 000017
となる。以上より、右ターンでも左ターンでも共通して、
2008076389.tif 000018
という式により前方向の速度を推定することが正当化された。このとき、この推定の確からしさを(2)式中第１行目の
2008076389.tif 000019
によって表している。
【００３３】
逆符号により定義される重要な例外があるということに注意すべきである。すなわち、車両が顕著な横滑り速度でしばしばオーバシュートすることがあり、オーバーシュートから立ち直る際、ｒの符号がコーナリングの途中で変化する。このような走行例は右ターンのコーナリング状態を示す図４Ｃの軌跡(3)によって示されている。ｒの符号が反転すると次の式を使う。
【数７】
2008076389.tif 000020
(7)式は以下により正当化される。瞬間速度ｖyvがdT秒後に瞬間加速度ａyvで直線的に０に減少すると仮定すれば次式、
【数８】
2008076389.tif 000021
が成立する。更に、瞬間横滑り角度aと角速度ｒとの間に、正の角速度ｒに対して横滑り角aは減少することに注意して、同様な関係を導出すると、
a−r・dT＝０、又は  a＝r・dT
が成立する。ここで、小さなスリップ角度aが車両速度に次式で示すように関係付けられる。
【数９】
2008076389.tif 000022
それゆえ、次式
【数１０】
2008076389.tif 000023
が得られる(8)、(10)式より
【数１１】
2008076389.tif 000024
が成立する。
コーナリング状態が適用されたとき、GPS測定値の他に上記(2)式の測定値がカルマン・フィルタ５０で使用される。これにより、車両がカーブをきるだけでGPS測定値に加え３つの測定値を得ることができ、軌跡や姿勢の推定を含むナビゲーション精度が向上する。
【００３４】
(3) 静止状態
先ず、図５のフローチャートによって説明されている手順を介して静止状態を識別する。図５では例として10Hzでフローが実行されており、説明中、様々な条件のOn/Offフラグのカウント数１回につき0.1秒、カウント数20回は2秒に相当する。フローチャートにおける主要な状態(静粛(Quiet)状態、再スタート状態、ピーク状態など)は、次のように定義されている。
(3a) 静粛条件
以下の条件を静粛条件と定義する。A∩Bは積集合であり、Ａであり、かつBであることを意味する。
|p| < 0.46 deg/s ∩ |q| < 0.46 deg/s ∩ |r|< 0.57 deg/s ∩ |axv|< 0.4 m/s2 ∩ |ayv| < 0.1 m/s2 ∩ |axv| < 0.1 m/s2 ∩ |ｖxv|< 3 m/s
これは、IMUの出力が小さく（静粛であり）、車両が静止している可能性を示唆する基本的な状態である。ただしセンサー出力に関する閾値は、IMU性能に左右されるパラメータである。
なお、フローチャートにおけるｉは静粛フラグ、ｊは静粛状態の連続カウント値である。静粛フラグは、上記静粛条件に加え、後述の(3d) 過去ピーク条件、あるいは(3e) 過去静粛条件を満たした時初めて、ｉ= 1(On)となり、それ以外ではｉ= 0(Off)となる。また、jは一回の静止事象中において静粛条件が満たされた連続回数である。
【００３５】
(3b) 静止条件
静止条件は
ｊ＝20
を満足することである。すなわち、静粛フラグｉ= 1(On)の連続カウント値ｊが２０に達した時、車両は静止していると見なす。
【００３６】
(3c) 再スタート条件
再スタート条件は次式
ｊ ≧ 20 ∩ |ａxv| > 0.15 m/s2
を満足することである。(3a)静粛条件を満たしていても、車両は依然動いているかもしれない。例えば、静止状態からの再スタートが、加速度に何らピークの無い非常に滑らかな場合がある。再スタート条件はこのケースを識別するものである。
【００３７】
(3d) 過去ピーク条件
過去ピーク条件は次式
max_ax > 0.6m/s2
を満足することである。但しmax_ax は過去1.5秒間の車両座標系ｘ方向加速度の最大絶対値である。閾値の0.6m/s2は、設計者の決定に依存する設計パラメータである。
車両がブレーキングによって停止すると、前後方向加速度に鋭いピークが生じる。max_ax は、過去1.5秒間にピークが存在するかどうかを判断するために使用される。
【００３８】
(3e) 過去静粛条件
過去静粛条件は
過去0.5sの間に静粛条件が成立した(ｉ＝１)時が存在した
を満足することである。
【００３９】
(3f) 非常に静粛な状態
これは静粛状態において、更に次の条件
|ａxv| < 0.1 m/s2
を満足する状態である。
フローチャートにおいて、ｍは一回の静止事象中において「非常に静粛な状態」になってトータル回数、ｎは一回の静止事象中において「非常に静粛な状態」になった連続回数である。この「非常に静粛な状態」は、IMU出力のより厳格な状態であり、常時ではないが、幾つかのケースにおいて静止状態を知らせてくれる。
通常の静止条件は、まず(3a)静粛条件が満たされ、次いで(3d) 過去ピーク条件が検出されることで静粛フラグｉ= 1(On)になり、一旦静粛フラグｉ= 1(On)になった後(3a)静粛条件が満たされたときは(3e)過去静粛条件より静粛フラグｉ= 1(On)となり、ｉ= 1(On)が20回続く事で、停車後に静止していると識別することになる。
図５では例として10Hzでフローが実行される。また、フローチャートにおいて「静粛状態」になれば、ブロックI,IIの両方の処理が行われる。そして、ｊが２０以上に達すると、車両は停止していると見なされる。
ｊ＝２０以上になったことによって静止状態が検出された際、以下の条件を定義する
・車両座標系の3軸方向全ての速度は0であると推測する。また車両座標系の3軸方向角速度は0であるはずなので、0でない出力は全てバイアスとホワイトノイズであると推測する。
この力学的な条件をカルマン・フィルタで実現するために、次の式の
2008076389.tif 000025
の測定値として誤差情報と共にカルマン・フィルタ５０で使用する。添え字_obsがついたパラメータは、それらの測定値（Observation）であることを示す。
2008076389.tif 000026
但し、Ｎωxb、Nωyb及びNωzbは、或る一定の周波数で測定されるジャイロ角速度のホワイトノイズの標準偏差値である。
静止状態が適用されたとき、GPS測定値の他に上記６つの測定値がカルマン・フィルタ５０で使用される。これにより、車両が静止している間、信頼性のない位置推定を止めることができ、なおかつジャイロ出力に含まれる0点のずれの元であるバイアスの値を短時間に推定できる。
【００４０】
本発明で説明した車両状態を適用するには１HzのGPS測定サイクルを待つ必要が無い。VDCコントローラ２５は、三つの車両状態の内の一つを表した測定データを10HzなどのGPS測定値よりも高い周波数でカルマン・フィルタ（KF2）５０に送る。かくして、カルマン・フィルタの更新は、例えば通常走行状態とコーナリング状態と静止状態に基づいて10Hzで実行されると共に、GPS測定値に基づいて1Hzで実行される。これは、車両の走行状態に基づいたエラー修正がGPS測定値の取得速度よりも速く行われ、それによってエラーの蓄積を最少にすることを意味している。
【００４１】
図６は、本発明の複合INS/GPSナビゲーション・システムにおけるエラー推定パターンと修正タイミングの説明図である。図６Ａは、従来技術に係わるパターンであり、図６Ｂは、本発明の車両走行状態を考慮した修正処理を組み込んだパターンである。上述のように、GPS測定値は、１Ｈｚの反復速度でカルマン・フィルタ(KF2)５０に与えられる。その期間（１秒）中に、図６Ａに示されているように、INSの位置推定エラーが蓄積され、GPS測定値が車両の絶対位置と速度を与える１秒毎に修正される。図６Ａに示されているように、１秒の期間の最終段階で、蓄積されたエラーはレベルEaに到達する。
【００４２】
一方、本発明の車両走行状態修正処理は、GPS測定値の修正処理よりも数倍高い反復速度で行われる。かくして、図６Ｂに示されているように、本発明の修正処理サイクルの最終段階で、蓄積されたエラーは、レベルEaより遥かに小さいレベルEbに到達する。その結果、たとえ十分なGPS信号が２分などの長い時間に渡って利用不能な場合も、本発明の複合INS/GPSナビゲーション・システムは、比較的高い推定精度を維持することができる。
【００４３】
これまで述べてきた車両走行状態に基づいた測定データをGPS測定に加えて、カルマン・フィルタリング処理に組み込む具体的な手法を以下に述べる。まず、慣性航法システムで使われる手法を以下に示す。即ち、速度、位置、及び姿勢の時間変化率を数式から求め、それらの値にサンプル時間Tを掛け、前の値に足していく方法である。以下の式で、
2008076389.tif 000027
のように変数の上にドットがついているものは、その変数の時間変化率を表している。
【数１２】
2008076389.tif 000028
但し、
2008076389.tif 000029
【数１３】
2008076389.tif 000030
但し、
2008076389.tif 000031
・・・センサー座標系(添え字ｂ表記、図2B)から地表NED（North-East-Down）座標系(添え字ｎ表記、図省略)への変換行列
(13)式は、慣性航法システム（INS）の一般的な手法から求められる、以下の姿勢方程式から、車両プラットフォーム適用の際に独立な4つのパラメータだけを抜き出したものである。
2008076389.tif 000032
【数１４】
2008076389.tif 000033
但し、
N：               ・・・北方向距離
E：               ・・・東方向距離
D：               ・・・下方向距離
2008076389.tif 000034
・・・地表NED（北-東-下：North-East-Down）座標系に変換された速度ベクトル
最後に、固定パラメータに関する式を以下に示す。
【数１５】
2008076389.tif 000035
但し、
2008076389.tif 000036
(15)式中p00、p10、及びp20、は取り付け角に関する行列の要素から、独立な3つのパラメータだけを抜き出したものである。
さて、上記(12)−(15)式をまとめて、簡単に(16)式の様に行列式で表すことができる
【数１６】
2008076389.tif 000037
・・・状態量ベクトル
CPU上で(16)式を数値積分するには、例えば、
【数１７】
2008076389.tif 000038
のように行えばよい。但しTはサンプルタイムで、例えば25HzならばT=0.04秒である。以上が一般的な慣性航法システム（INS）に用いられる手法を基に、MEMSセンサー使用を考慮して、地球の丸みなど微小な項を簡略化した車両用慣性航法システム（INS）である。
【００４４】
さらに、慣性航法システム（INS）で得られた状態量ｘを、GPSなどの測定値をもとに、カルマン・フィルタリングによって補正する方法を以下に示す。
まず(17)式の線形化を行い、微小擾乱方程式である行列式(18)を用意する。
【数１８】
2008076389.tif 000039
2008076389.tif 000040
2008076389.tif 000041
【数１９】
2008076389.tif 000042
【数２０】
2008076389.tif 000043
ここで車両に対するセンサーの取り付けロール角＝0を仮定し、取り付けロール角に関する補正量
2008076389.tif 000044
である。そして、行列
2008076389.tif 000045
は次の(21)、(22)式のように得られる。
但し行列中で以下の表記を利用している。
2008076389.tif 000046
また、表記のない要素は0である。
【数２１】
2008076389.tif 000047
【数２２】
2008076389.tif 000048
【００４５】
次に測定方程式を用意する。但し以下の測定方程式で、uvyvのように表されているのは、ホワイトノイズでモデル化された測定ノイズである。uvyvの標準偏差は
2008076389.tif 000049
の様に表されている。
通常走行状態
【数２３】
2008076389.tif 000050
但し、測定値については(1)式を参照。
コーナリング状態
【数２４】
2008076389.tif 000051
但し、測定値については(2)式を参照。
静止状態
【数２５】
2008076389.tif 000052
【数２６】
2008076389.tif 000053
但し、
2008076389.tif 000054
GPS測定
【数２７】
2008076389.tif 000055
【数２８】
2008076389.tif 000056
但し、測定値と測定誤差標準偏差は、GPS レシーバから得られる。
【００４６】
上記(23)−(28)式をまとめて、簡単に(29)式の様に行列式で表すことができる
【数２９】
2008076389.tif 000057
カルマン・フィルタで使用するために(29)式を線形化する。
【数３０】
2008076389.tif 000058
また、行列
2008076389.tif 000059
は次の(31)式のように得られる。
【数３１】
2008076389.tif 000060
【００４７】
慣性航法システム（INS）とGPS及び車両素行状態による測定を複合するためには、以下の一般的なカルマンフィルタリングの定式化に従う。まず前述まで得られた行列を使って、高周波で、例えば25Hzで、(17)式と(32)式を更新する。
【数３２】
2008076389.tif 000061
但し、
右肩のT ：              転地行列（Transpose）
右肩の-：               カルマンフィルタの補正前の状態
右肩の+：               カルマンフィルタの補正後の状態
2008076389.tif 000062
【００４８】
測定値が得られたとき、以下の反復計算をi = 0 から n回行う。通常n = 2回以上反復すれば反復しないときに比べ大幅に精度が向上し、10回以上行う必要はあまりない。測位が得られるたびに反復計算を行うこの方法は、反復拡張カルマン・フィルタ（Iterated Extended Kalman Filter）と呼ばれる周知の方法である。
【数３３】
2008076389.tif 000063
【数３４】
2008076389.tif 000064
【数３５】
2008076389.tif 000065
である。
【００４９】
(12)式から(35)式まで、周知の慣性航法システム（INS）の手法と、周知のカルマン・フィルタリングの手法を示した。本発明で特異な手法の具体例を以下に説明する。
通常の複合INS/GPSナビゲーションシステムにおいて、
2008076389.tif 000066
行列は(4)((31)式では丸付き数字４、以下同様)の行でしか構成されていない。本発明では、通常走行状態では(1)の行、コーナリング状態では(2)の行、静止状態では(2)と(3)の行を使用して
2008076389.tif 000067
行列を構成する。もしもGPS測定が同時に得られるときには(1)〜(3)のいずれかと(4)の行を合わせて
2008076389.tif 000068
行列を構成する。その際には
2008076389.tif 000069
も、以下のように使用する
2008076389.tif 000070
に応じて変化する。
2008076389.tif 000071
GPS測位が得られるときには、上記いずれかの走行状態に加え、GPSの測定誤差に関するコバリアンスを以下のように付け加える。
2008076389.tif 000072
【００５０】
上述のように、従来のカルマン・フィルタリング処理に加えて、慣性センサーによって測定される車両走行状態に基づいた測定データがカルマン・フィルタリング処理に組み込まれる。該測定データは三つの予め定義された車両走行状態の内の一つを示しており、カルマン・フィルタリング処理を介して状態量推定を最適化するのを補助する。図７から図１０は、本発明の有効性を示す実験結果を示している。実験の方法は、まず路上走行によってIMUとGPSのデータを収集し、ついで机上パーソナルコンピュータ(PC)で緩結合(Loosely Coupled)INS/GPSシステムを構成し、最後に路上収集センサーデータをPCに入力することでシミュレーションを行う、というプロセスからなる。図７から図１０は、複合INS/GPSシステムによって推定される車両の走行軌跡であり、車両は、多層階の駐車場建築物（図内PS）の一階と地下を通って走行している。
【００５１】
図７は、従来の緩結合INS/GPSシステムを使用した結果(走行軌跡)を示している。車両位置の推定が細い破線と実線とで示されており、実線は衛星が１つも見え無い状況を表している。参考のためGPSのみを使用してINSを使用しない場合の走行軌跡も比較のために太い破線で示されている。入口(Entrance)から出口（Exit）までの全体の運転に約7.5分掛かっており、衛星からの信号が完全に途絶える状態（実線）が断続的に続き、最大で136秒にも及んでいる。このような好ましくないGPS環境の故に、従来のINS/GPSシステムによって与えられる車両の推定位置は、実際の軌跡から遠く離れている。
それに対して、図８と図９は、本発明を組み込んでいる緩結合INS/GPSシステムの結果を示しており、図９は図８の拡大図である。水平方向の逸脱挙動が大幅に改善されており、そこでは車両の動きが妥当な正確さで回復されており、ヨー角（方位角）の発散量も大幅に縮小している。太い点で示されているGPSのみを使用する場合、GPS信号が利用可能になっている駐車構造物PSの窓近くの領域においてのみ正確な位置推定を行い、その他の場所では推定不可能となっている。本発明によれば、車両が駐車構造物PSの地下に在っても、車両の移動は十分な精度で推定できる。
【００５２】
図９の３次元図は、図１０に示されており、垂直方向のズレも非常に小さくなっていることがわかる（入口と出口における高さの差は±２ｍ以内）。地下レベルに接続された中央の傾斜路も正確に復元されている。GPSだけによる垂直方向の推定位置精度は、複合INS/GPSの精度よりも遥かに低く、図１０からはみだしていて見えない。
【００５３】
上述してきたように、本発明によれば、GPSとINSの出力がカルマン・フィルタによって結合される従来の複合INS/GPSナビゲーション・システムに加えて、所定の車両の走行状態に応じた測定値をカルマン・フィルタに与え、これにより車両の現在位置、速度及び姿勢の最適な推定値を得ることができ、結果的に、車両の運動の推定に関するエラーは、GPSによって修正されるばかりでなく、車両の走行状態を加味した修正処理によっても修正される。車両の走行状態による修正処理は、GPS出力の周波数よりも遥かに高い周波数によって反復されるので、蓄積されるエラーの量は、エラーが短時間内に修正されるので少なくなる。更に、蓄積されるエラーが少なくなるので、低コストでノイズの多いMEMSセンサーを複合INS/GPSナビゲーション・システムにおいて使用することができる。
【図面の簡単な説明】
【００５４】
【図１】本発明の複合されたINS/GPSナビゲーション・システムの基本的な構成図である。
【図２】IMUの構成と該IMUに組み込まれた慣性センサーの説明図である。
【図３】本発明に係る車両の走行状態を考慮した処理のフローチャートである。
【図４】座標系が地上に固定されている場合に車両がカーブを切る際(コーナリング時)に係わるベクトル・パターンの説明図である。
【図５】本発明の走行状態を考慮した修正処理における車両静止状態の検出フローチャートである。
【図６】従来と本発明の複合INS/GPSナビゲーション・システムにおける時間に対するエラーパターンとその修正パターンを示した概略線図である。
【図７】車両が多層階の駐車構造物の中を通過して走行している場合における従来の緩結合INS/GPSナビゲーション・システムによる走行軌跡である。
【図８】本発明の走行状態を考慮した修正処理方法を組み込んだ緩散結合INS/GPSナビゲーション・システムによる走行軌跡である。
【図９】図８の拡大図である。
【図１０】図９の３次元走行軌跡である。
【符号の説明】
【００５５】
２０  INS/GPSナビゲーション・システム
２５  コントローラ（VDCコントローラ）
３０  慣性ナビゲーション・システム（INS）
３２  慣性測定ユニット（IMU）
３４  ローパス・フィルタ・ユニット
３６  ナビゲーション等化ユニット
４０  GPS受信機
５０  カルマン・フィルタ
５２  カルマン・ゲイン・ユニット
【図１】
2008076389.tif 000073
【図２】
2008076389.tif 000074
【図３】
2008076389.tif 000075
【図４】
2008076389.tif 000076
【図５】
2008076389.tif 000077
【図６】
2008076389.tif 000078
【図７】
2008076389.tif 000079
【図８】
2008076389.tif 000080
【図９】
2008076389.tif 000081
【図１０】
2008076389.tif 000082