車載ナビゲーションシステム及びナビゲーション方法
【目的】開発効率化と高精度を達成するために任意のセンサー構成のシステムでも基本システムと同一のソフトウェアで制御を行なえるようにする「車載ナビゲーションシステム及びナビゲーション方法」を提供する。
【構成】車速パルスセンサー、加速度計や角速度計（ジャイロスコープ）およびGPS受信機を含む複数のセンサーを複合した車載ナビゲーションシステムにおいて、いくつかのセンサーの組み合わせで構成される複数のシステムの中から選択した所定のセンサー構成システムと全センサーを備えた基本システムとの差異を補完し、該補完された値及びシステムのセンサー出力信号を用いて自律航法計算し、基本システムの制御アルゴリズムに基づいてカルマンフィルタ処理を行って車両の位置、速度及び姿勢の推定値を得る。
【構成】車速パルスセンサー、加速度計や角速度計（ジャイロスコープ）およびGPS受信機を含む複数のセンサーを複合した車載ナビゲーションシステムにおいて、いくつかのセンサーの組み合わせで構成される複数のシステムの中から選択した所定のセンサー構成システムと全センサーを備えた基本システムとの差異を補完し、該補完された値及びシステムのセンサー出力信号を用いて自律航法計算し、基本システムの制御アルゴリズムに基づいてカルマンフィルタ処理を行って車両の位置、速度及び姿勢の推定値を得る。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、車載ナビゲーションシステム及びナビゲーション方法に係わり、特に、任意のセンサー構成システムにおいても6自由度の基本システムのアルゴリズムを共通に利用できるようにし、かつセンサー軸数が減少しても大幅な精度減少がない、車載ナビゲーションシステム及びナビゲーション方法に関する。
【背景技術】
【０００２】
自律航法システムは、車両の距離センサーである車速パルスセンサーや、加速度計と角速度計などの慣性センサーから成り、動体である車両の位置、速度、及び姿勢の推定のために広く使用されている。
【０００３】
慣性航法システム（Inertial Navigation System：INS）は、通常３軸加速度計と３軸ジャイロスコープからなり、動体の６自由度、即ち３次元の並進運動と３次元の回転運動を同時に推定するために使用される。例えば、航空・宇宙機のナビゲーションは、かかる慣性航法システムを用いて、動体の６自由度、即ち３次元の並進運動と３次元の回転運動を同時に推定する。車両ナビゲーションにおいては車速パルスセンサーを使用することで、コストの高い慣性センサーを削減する場合が多い。
【０００４】
自律航法は外部信号に頼らずにセンサー出力に基づいて推定位置の更新を行うことができるが、センサー出力に含まれるノイズなどのエラーが時間の経過とともに積分計算に伴って蓄積するため、長時間は使用できない。そこで、衛星信号の届く範囲では有限誤差の位置推定を常時与える全地球測位システム（Global Positioning System: GPS受信機）と自律航法システムを、カルマンフィルタなどの最適確率アルゴリズムで結合することにより、両システムの性能上の問題を補間・解決する手法が考案された(特許文献１)。このようなシステムは複合ナビゲーションシステム（Integrated Navigation System）と呼ばれ、現在では車両ナビゲーションから航空宇宙ナビゲーションまで、幅広く利用されている。
【０００５】
近年の車両ナビゲーションにおいては、GPS受信機単体を使用するローコスト商品から、GPS受信機と任意のセンサーを組み合わせて使用するハイエンド商品まで、様々なセンサー構成のナビゲーションシステムが存在する。しかし、一社でローコスト商品からハイエンド商品まで扱う場合、各商品におけるセンサーの組み合わせは異なり、通常、商品毎に異なるソフトウェア（SW）を開発する必要があり、開発とメンテナンスの手間が商品の種類の数に比例して増大する問題があった。
【特許文献１】特許３４７３１１７号
【発明の開示】
【発明が解決しようとする課題】
【０００６】
従って、センサーの組み合わせが異なる複数のシステムに共通に対応できる単一のプラットフォームソフトウェア（SW）を備え、しかもセンサー軸数が減少しても大幅な精度減少がないナビゲーション方法及び車載ナビゲーションシステムが要望されている。
【０００７】
本発明の目的は、単一プラットフォームソフトウェア（SW）でセンサーの組み合わせが異なる複数のシステムに対応できるようにし、しかもセンサー軸数が減少しても大きな精度減少がないようにすることである。
本発明の別の目的は、任意のセンサー構成システムにおいて6自由度の基本システムのアルゴリズムを共通に利用できるようにすることである。
【課題を解決するための手段】
【０００８】
本発明は車速パルスセンサー、加速度計や角速度計（ジャイロスコープ）およびGPS受信機の複数のセンサーを複合した車載ナビゲーションシステムおよびナビゲーション方法である。
・ナビゲーション方法
本発明のナビゲーション方法は、いくつかのセンサーの組み合わせで構成される複数のシステムの中から所定のセンサー構成システムを選択するステップ、前記選択されたセンサー構成システムと全センサーを備えた基本システムとの差異を補完するステップ、前記基本システムの制御アルゴリズムに基づいてカルマンフィルタ処理を行って車両の位置、速度及び姿勢の推定値を得るステップを備えている。
前記センサー構成システムは少なくとも、
GPS受信機、車速パルスセンサー、加速度計３軸、ジャイロスコープ３軸、
GPS受信機、車速パルスセンサー、加速度計３軸、ジャイロスコープ２軸、
GPS受信機、車速パルスセンサー、加速度計３軸、ジャイロスコープ１軸、
GPS受信機、車速パルスセンサー、加速度計２軸、ジャイロスコープ１軸、
GPS受信機、車速パルスセンサー、加速度計１軸、ジャイロスコープ１軸、
GPS受信機、車速パルスセンサー、加速度計不使用(０軸)、ジャイロスコープ１軸、
の内のいずれかであり、前記基本システムはGPS受信機、車速パルスセンサー、加速度計３軸、ジャイロスコープ３軸を備えた６自由度のシステムである。
本発明のナビゲーション方法は更に、前記選択されたセンサー構成システムをシステム設計時に予め設定し、あるいは、センサーから得られる信号をもとに自動的に判別するステップを備えている。
又、本発明のナビゲーション方法において、前記差異を補完する補完手段は、前記選択されたセンサー構成システムにおける基本システムから欠けているセンサーの出力値として、０またはホワイトノイズを採用する。あるいは、補完手段は、前記選択されたセンサー構成システムにおける基本システムから欠けているセンサーの出力値として、定数を採用する。あるいは補完手段は、前記選択されたセンサー構成システムにおける基本システムから欠けているセンサーの出力値として、他センサーの出力値を変数とする関数の関数値を採用する。
【０００９】
・車載ナビゲーションシステム
本発明の車載ナビゲーションシステムは、いくつかのセンサーの組み合わせで構成される複数のシステムの中から選択した所定のセンサー構成システムと全センサーを備えた基本システムとの差異を補完する補完部、補完された値及びシステムのセンサー出力信号を用いて自律航法計算する自律航法計算部、前記基本システムの制御アルゴリズムに基づいて自律航法計算結果及びGPS測定結果を用いてカルマンフィルタ処理を行い、車両の位置、速度及び姿勢の推定値を得る処理部を備えている。前記センサー構成システムは、前記のナビゲーション方法において示した複数の構成が可能であり、前記基本システムはGPS受信機、車速パルスセンサー、加速度計３軸、ジャイロスコープ３軸を備えた６自由度のシステムである。
本発明の車載ナビゲーションシステムは、更に、前記選択されたセンサー構成システムをシステム設計時に予め設定する手段、あるいは、センサーから得られる信号をもとに選択されたセンサー構成システムを自動的に判別する手段を備えている。
又、本発明の車載ナビゲーションシステムにおいて、前記補完部は、前記選択されたセンサー構成システムにおける基本システムから欠けているセンサーの出力値として、０またはホワイトノイズを採用する。あるいは、補完部は、前記選択されたセンサー構成システムにおける基本システムから欠けているセンサーの出力値として、定数を採用する。あるいは補完部は、前記選択されたセンサー構成システムにおける基本システムから欠けているセンサーの出力値として、他センサーの出力値を変数とする関数の関数値を採用する。
【発明の効果】
【００１０】
本発明によれば、予め定義されたセンサー構成に応じて、基本の6自由度システムから欠けているセンサーの出力値として、力学的な知識に基づいて所定の値を採用することにより、自律航法計算部は常に同じ6自由度の基本システムの計算を行うことができる。結果として同じソフトウェア（SW）で異なるセンサー構成システムに対応することができ、開発工数を削減できる。また、個別のセンサー構成システムにだけ対応するアルゴリズムよりも、本発明では力学的に推定・補完したセンサー信号の分だけ多くの情報を使用することになり、自律航法計算の精度が向上する。このため本発明によれば、センサー軸数を削減しても、個別のセンサー構成システムにだけ対応するアルゴリズムに比べて精度減少が少ない。
【発明を実施するための最良の形態】
【００１１】
(Ａ) 複合ナビゲーションシステムの基本的な構成
図１は、車両に搭載される本発明の複合ナビゲーションシステムの基本的な構成の例を示す概略ブロック線図である。
【００１２】
自律系の入力信号は、センサーボード１０上の慣性センサーから得られる信号と、別のケーブルを通じて車体から直接ナビゲーションCPUに取り込まれる車速パルスである。慣性センサーとしては加速度計(加速度１〜３軸)、ジャイロスコープ（角速度１〜３軸)を採用し、車速パルスを発生するセンサーとしては車両が所定距離移動する毎に１個のパルスを発生する車速センサーを採用する。
【００１３】
センサー・コンフィギュレーション（SC）・コントローラ２０は、車載ナビゲーションシステムの実際のセンサー構成を設定部１５から設定され、あるいは、センサー出力から取得した情報を評価することで該センサー構成を判別する。なお、センサー出力から取得した情報の評価とは、「センサー出力信号が異常値であるとき、システムは該当するセンサーを使わないセンサー構成であると自動判別すること」を指す。センサー出力信号が異常値であるとは、例えば、（1）常時一定電圧を出力している、あるいは、（2）バイアスや感度が仕様範囲を超えている、ということである。
【００１４】
SCコントローラ２０は、前記判別されたセンサー構成に応じて、6自由度の基本システムから欠けているセンサーの出力値として、力学的な知識をもとに、定数(０を含む)、あるいはホワイトノイズ、あるいは他のセンサー出力値を変数とする関数の関数値を採用し、各センサーの出力値を自律航法計算部３０に送る。また、SCコントローラ２０は、前記判別されたセンサー構成がロール角を推定するセンサーをもたないとき、ロール角推定量が発散するのを防ぐために、ロール角＝0の測定量をカルマンフィルタ補正部４０に送る。このとき実際の車両では最大５度程度のロール角が生じるが、カルマンフィルタ補正部４０では実際値との誤差はホワイトノイズとして取り扱う。
【００１５】
自律航法計算部２０は常に同じ6自由度の基本システムのアルゴリズムを用いて、車両の現在の位置、速度及び姿勢を含む車両状態量を、高周波数で更新する。
【００１６】
GPS受信機５０は、複数の人工衛星から距離と距離変化率に関する信号を受信することで、車両のアンテナの位置と車両速度の測定値を提供する。
【００１７】
演算部６０は、自律航法計算部３０により推定された車両状態量と、GPS受信機５０から得られる位置・速度測定値を含む測定量との差分をとり、カルマンフィルタ補正部４０はカルマンフィルタを用いて該差分に基づいて車両の現在の位置、速度及び姿勢を含む車両状態量の補正を行う。
【００１８】
(Ｂ)種々のシステムにおけるセンサー構成
図２(A)は、図１におけるセンサーボード１０に固定した座標系Xs-Ys-Zsを示している。なお、以下の数式の中でセンサー（ボード）座標系に関する表現であることを、添え字の”ｓ”によって表現することに注意すべきである。
【００１９】
図２(B)は、(A)のセンサーボード１０において、加速度計3軸（Acc x, Acc y, Acc z）およびジャイロスコープ3軸（Gyro x, Gyro y, Gyro z）から成る、即ち6自由度の基本システムのセンサー構成を示したものである。3軸加速度計Acc x〜Acc zは、センサー座標系の三つの座標方向（ｘ，ｙ，ｚ）における加速度を検出し、また3軸ジャイロスコープGyro x〜Gyro zは、センサー座標系の三つの座標軸（ｘ，ｙ，ｚ）周りの角速度P,Q,Rを検出する。
【００２０】
図２（C）は、(A)のセンサーボード１０において、加速度計3軸、ジャイロスコープ2軸から成るシステムのセンサー構成を示したものである。ここで、ジャイロスコープが2軸しか検出できないとき、システムのジャイロスコープはGyro xおよび Gyro zである。
【００２１】
図２（D）は、(A)のセンサーボード１０において、加速度計3軸、ジャイロスコープ1軸から成るシステムのセンサー構成を示したものである。ここで、ジャイロスコープが１軸しか検出できないとき、システムのジャイロスコープはGyro zである。
【００２２】
図３（A）は、図２(A)のセンサーボード１０において、加速度計2軸、ジャイロスコープ1軸から成るシステムのセンサー構成を示したものである。ここで、加速度計が2軸しか検出できないとき、システムの加速度計はAcc x, Acc yであり、ジャイロスコープが１軸しか検出できないとき、システムのジャイロスコープはGyro zである。
【００２３】
図３(B)は図２（A）のセンサーボード１０において、加速度計1軸、ジャイロスコープ1軸ら成るシステムのセンサー構成を示したものである。ここで、加速度計が１軸しか検出できないとき、システムの加速度計はAcc xであり、ジャイロスコープが１軸しか検出できないとき、システムのジャイロスコープはGyro zである。
【００２４】
図３(C)は、図２（A）のセンサーボード１０において、加速度計0軸（不使用）、ジャイロスコープ1軸から成るシステムのセンサー構成を示したものであり、ジャイロスコープが１軸しか検出できないとき、システムのジャイロスコープはGyro zである。
【００２５】
図４（A）は、車両に固定した座標系Xb-Yb-Zbを示している。以下の数式の中で車両固定座標系に関する表現であることを、添え字の”b”によって表現する。
【００２６】
図４(B)は、ある緯度、経度における、地表固定座標系North-East-Down（NED座標系）を示している。以下の数式の中でNED座標系に関する表現であることを、添え字の”n”によって表現する。
【００２７】
（Ｃ）基本的作動プロセス
図５は、本発明に係るナビゲーションシステムの基本的作動プロセスの例を示しているフローチャートである。工程101において、車速パルスセンサー、慣性センサーである加速度計および角速度計をもとに予めいくつかのセンサー構成を定義する（図２、図３参照）。
【００２８】
次に工程102において、SCコントローラ２０は前記センサーから得られる信号をもとにナビゲーションシステムの前記センサー構成を判別する。
【００２９】
工程103において、SCコントローラ２０は前記判別したセンサー構成に応じて、6自由度の基本システムから欠けているセンサーの出力値として、力学的な知識をもとに、定数(０を含む)、あるいはホワイトノイズ、あるいは他のセンサー出力を変数とする関数の関数値を採用し、各センサーの出力を自律航法計算部３０に送る。また、SCコントローラ２０はロール角＝0の測定量をカルマンフィルタ補正部４０に送る。なお、ロール角＝0の測定量をカルマンフィルタ補正部４０に送らない場合もある。
【００３０】
工程104において、自律航法計算部３０は、該センサー出力として採用した値と実際のセンサー出力値を基に、常に同じ6自由度基本システムのアルゴリズムを使って、車両状態量を高周波で更新する自律航法計算を行う。
【００３１】
工程105において、演算部６０は自律航法計算部３０により推定された車両状態量と、GPS受信機５０から得られる位置・速度測定値を含む測定量との差分を演算し、カルマンフィルタ補正部４０はカルマンフィルタを用いて該差分に基づいて車両の現在の位置、速度及び姿勢を含む車両状態量の補正を行う。
【００３２】
工程106において、複合自律/GPS受信機ナビゲーションシステムは、ナビゲーション精度を連続して最適化するために前記の工程102から工程105を反復実行する。
【００３３】
以下では、センサー構成によらず常に基本となる6自由度システムのアルゴリズムを使用して、車両状態量の推定計算を行う具体的な手法を説明する。
【００３４】
（Ｄ）自律航法計算部における自律航法計算アルゴリズム
まず、基本となる６自由度システムの、高周波自律航法計算部３０のアルゴリズムについて説明する。なお、以降の式においては、
で示すように変数の上にドットがついているものは、その変数ｖsの時間変化率を表しているものとする。
【００３５】
（ａ）速度方程式
速度方程式は次式
【数１】
で表現される。但し、
【数２】
であり、(2a)はセンサー固定座標系で表現される速度ベクトル、(2b)はセンサー固定座標系で表現されるジャイロスコープ出力ベクトル、(2c)はセンサー固定座標系で表現される加速度計出力ベクトル,(2d)はセンサー固定座標系で表現される重力ベクトルである。ここで、
【数３】
ここで、センサーの“感度”は、設置角の影響などを含めない、センサー自体の電圧出力から物理量への変換定数を意味する。通常センサーの生の出力は、出力がないことを示す０点電圧（しばしば２．５Ｖｏｌｔ）を中心とした電圧値（単位はＶｏｌｔ）であり、角度変化率や加速度などの物理量を得るためには、感度とよばれる変換用の定数を、出力電圧値と０点電圧の差に乗算する必要がある。感度は通常仕様値がセンサー毎に決められているが、低コストのセンサーの場合、実際の値が仕様値と大きく異なることや、温度補正が正確でないために感度が仕様値とずれるといったことがしばしば起こる。ことにジャイロに関する感度誤差はナビゲーションの方位計算に大きな影響を及ぼすので、このシステムではジャイロ感度をカルマンフィルタの状態量に含め、常時推定・補正を行う。
【００３６】
（ｂ） 姿勢方程式
姿勢方程式は次式で表現される。
【数４】
ただし、
【数４ａ】
はセンサー座標系からNED座標系への変換行列である。
（4）式は、慣性航法システム（Inertial Navigation System: INS）の一般的な手法である以下の姿勢方程式
【数５】
から、車両ナビゲーション適用の際に独立な4つのパラメータだけを抜き出したものである。(5)式はより一般的なINS姿勢方程式である。
【００３７】
（ｃ） 位置方程式
位置方程式は次式
【数６】
により表現できる。但し、(6)式において
【数７】
であり、(7a)は位置ベクトル、(7b)は車両移動距離ベクトルである。又、(7b)式において、Np はサンプル時間当たりのパルス数、Lはパルス間距離である。
【００３８】
（ｄ） 固定パラメータ方程式
固定パラメータ方程式は次式
【数８】
により表現される。但し、
【数９】
であり、(9a)はセンサー座標系ジャイロスコープ出力に含まれるバイアス（Volt）、(9b)はセンサー座標系加速度計出力に含まれるバイアス（m/s2）、(9c)はセンサー座標系ジャイロスコープ出力に関する感度（rad/sec/Volt）、(9d)はセンサー座標系から車両座標系への変換行列である。また、(8）式中p00、p10、及びp20、は取り付け角に関する行列の要素から、独立な3つのパラメータだけを抜き出したものである。
【００３９】
（ｅ） 非線形状態方程式
上記（1）、(4)、(6)、（8）式をまとめて、簡単に（10）式の様に行列式で表すことができる
【数１０】
但し、ｘは以下に与えられる非線形状態量ベクトルである。
【数１０ａ】
【００４０】
（ｆ） 状態量更新
CPU上で(10)式を数値積分するには、例えば、次式の状態量更新式
【数１１】
を用いて行えばよい。精度要求の高い場合は、周知の数値積分方法であるルンゲ・クッタ4次式(下記文献参照)などを用いてもよい。
Kreyszig, E., Advanced Engineering Mathematics, John Wiley & Sons, 1999, New York, NY.
但し、(11)式において、Tはサンプルタイムで、例えば25HzならばT=0.04秒である。以上は一般的な慣性航法システム（INS）に用いられる手法を基に、MEMSセンサー使用を考慮して、地球の丸みなど微小な項を簡略化した車両用慣性航法システム（INS）である(下記文献参照)。
Hoshizaki, T., Computational Scheme for MEMS Inertial Navigation Systems, AOAMR Patent, August, 2006.
【００４１】
（ｇ） 微小擾乱方程式
さて、GPS受信機などの測定値をもとに、カルマンフィルタ補正部４０がカルマンフィルタ処理で状態量ｘを補正を行うためには、自律航法計算部３０が状態量を非線形状態方程式により更新を行い、かつ、誤差量が線形方程式によって増加すると仮定して該誤差量（微小擾乱量）のコバリアンス値も更新する必要がある。そのために、（11）式の線形化を行い、微小擾乱方程式である行列式（12）を用意する。
【数１２】
但し、
は以下に与えられる微小擾乱ベクトルである。
【数１３】
ただし、(13b)は
の現在推定値、(13c)はホワイトノイズでモデル化された3軸ジャイロスコープ出力
に含まれる高周波ノイズと、3軸加速度計出力
に含まれる高周波ノイズである。それぞれのノイズの標準偏差(σ）はセンサースペックや、測定実験などによって得られ、ここではそれらの大きさを次のようにＮで表す。
【数１４】
また、
に関する微小擾乱量であり、
には、慣性航法システム（INS）の一般的な手法に基づき、以下の関係がある。
【数１５】
また、
に関する微小擾乱量であり、
には、慣性航法システム（INS）の一般的な手法に基づき、以下の関係がある。
【数１６】
ここで車両に対するセンサーの取り付けロール角＝0と仮定すれば、取り付けロール角に関する補正量
となる。
(12)式における線形システムを表わす行列
はそれぞれ図６、図７に示すように得られる。ただし、図６において、
【数１７】
【００４２】
（ｈ） コバリアンスの更新
以上のように得られた行列
を使って、カルマンフィルタ処理の定式に従い、高周波で以下のように微小擾乱ベクトル
のコバリアンス行列(誤差共分散行列)を次式により更新する。
【数１８】
但し、右肩のT は転地行列（Transpose）を意味し、右肩の−はカルマンフィルタの補正前の状態を意味し、右肩の＋はカルマンフィルタの補正後の状態を意味する。また、
の相関値の期待値（E[ ]）からなるコバリアンス行列であり、
に関するコバリアンス行列であり、それぞれ次式で表現される。
【数１９】
以上の（11）式及び（18）式が、前記6自由度の基本システムにおいて自律航法計算部３０が高周波で(例えば25Hzで)実行する演算式である。以下に(11)式及び（18）式を再掲する。
【数２０】
【００４３】
（Ｅ）任意のシステムの基本システムから欠けているセンサーの出力決定法
ここで所定のセンサー構成システムにおいて、6自由度の基本システムから欠けているセンサーの出力値の決定法を説明する。SCコントローラ２０は、力学的な知識をもとに、基本システムから欠けているセンサーの出力値として、定数、あるいはホワイトノイズ、あるいは他のセンサー出力を変数とする関数の関数値を採用して自律航法計算部３０に入力する。また、SCコントローラ２０は、ロール角＝0の測定量をカルマンフィルタ補正部40に入力する。
【００４４】
（ａ） 車速パルスセンサー、加速度計3軸、及びジャイロスコープ3軸を備えた基本システム：
かかる基本システムの場合、SCコントローラ２０は各センサー出力を自律航法計算部３０に入力し、自律航法計算部３０は入力されたセンサー出力を用いて前記自律航法計算アルゴリズムを実行する((11)、（18）式の演算を実行する)。
【００４５】
(ｂ) 車速パルスセンサー、加速度計3軸及びジャイロスコープ2軸を備えたシステム：
かかるシステムでは以下の(1)〜(3)に従ってセンサー出力を決定する。
（1）ジャイロスコープ2軸はｘs及びｚs軸を使用するものとする（図2(C）)。
（2）ｙs軸ジャイロスコープ出力を0（ωys=Q=0）とし、
とする。即ち、車両モーションによるピッチ角速度を、平均値0（rad/s）と標準偏差0.05（rad/s）＝2.9（deg/s）のホワイトノイズの加算であると仮定する。標準偏差値は6自由度の基本システムを用いて得られたデータを統計処理した値を使用している。後述する図８の螺旋駐車場を含むテストコースを実際に走行して採取したｙs軸ジャイロスコープ出力データの標準偏差値は0.054 （rad/s）であり、上記の標準偏差0.05（rad/s）は乗用車の車両ピッチ角モーションのおおよその代表的な値であると仮定できる。
（3）ｙs軸のジャイロスコープ出力に含まれるバイアス及び感度をそれぞれ
で固定する。
【００４６】
(ｃ) 車速パルスセンサー、加速度計3軸及びジャイロスコープ1軸を備えたシステム、あるいは 車速パルスセンサー、加速度計2軸及びジャイロスコープ1軸を備えたシステム：
かかるシステムでは以下の(1)〜(3)に従ってセンサー出力を決定する。
（1）加速度計2軸はｘs及びｙs軸を使用するものする。また、ジャイロスコープ1軸は、ｚs軸を使用する（図２(D)，図３（A））。
（2）ｘs軸ジャイロスコープ出力を0（ωxs=P=0）とし、
とする。即ち、車両モーションによるロール角速度の大きさを平均値0（rad/s）と、標準偏差0.05（rad/s）＝2.9（deg/s）のホワイトノイズの加算である仮定する。標準偏差値は6自由度の基本システムを用いて得られたデータを統計処理した値を使用している。後述する図９の螺旋駐車場を含むテストコースを実際に走行して採取したｘs軸ジャイロスコープ出力データの標準偏差値（0.049 rad/s）であり、上記の標準偏差0.05（rad/s）は乗用車の車両ロール角モーションのおおよその代表的な値であると仮定できる。
また、ｙs軸ジャイロスコープ出力を0（ωys=Q=0）とし、
とする。更に、
ｚs軸加速度計出力を‐9.8（m/s2）とし、
とする。即ち、車両モーションによるセンサー垂直方向加速度の大きさを平均値‐9.8（m/s2）と、 標準偏差0.3（m/s2）のホワイトノイズの加算であると仮定する。標準偏差値は6自由度システムを用いて得られたデータを統計処理した値を使用している。後述する図９の螺旋駐車場を含むテストコースを実際に走行して採取したｚs軸加速度計出力データの標準偏差値は0.28（m/s2）であり、上記の標準偏差0.3（m/s2）は乗用車の車両垂直方向加速度モーションのおおよその代表的な値であると仮定できる。
（3）ｘs軸ジャイロスコープ出力に含まれるバイアス及び感度をそれぞれ
で固定する。また、ｙs軸ジャイロスコープ出力に含まれるバイアス及び感度をそれぞれ
で固定する。さらに、zs軸加速度計出力に含まれるバイアス
で固定する。
【００４７】
（ｄ） 車速パルスセンサー、加速度計1軸、及びジャイロスコープ1軸のシステム：
かかるシステムでは以下の(1)〜(4)に従ってセンサー出力を決定する。
（1）加速度計1軸はｘs軸をするものとし、ジャイロスコープ1軸はｚs軸を使用するものとする（図３（B）参照）。
（2）(ｃ)のシステムと同様にｘs軸ジャイロスコープ出力を0（ωxs=P=0）とし、
とする。また、ｙs軸ジャイロスコープ出力を0（ωys=Q=0）とし、
とする。更に、ys軸加速度計出力を、
車速パルスから得られる速度（ｖxb_obs）×ｚs軸ジャイロスコープ出力ωzs（=R)
とする（掛け算の結果の単位はm/s2）。速度ｖxb_obsについては後述するが、ys軸加速度計出力は、ｚs軸ジャイロスコープ出力ωzsを変数とする関数の関数値であるといえる。また
とする。即ち、車両モーションによるセンサー横方向加速度の大きさを、ｖxb_obs×ωzsの平均値と標準偏差0.7（m/s2）のホワイトノイズの加算であると仮定する。平均値を表す上式は、力学的に平面内回転運動において、横方向加速度が前方向速度と角速度の積で得られることから、定性的に推定されたものである。それ以外の横方向加速度出力はロール角による重力方向成分を含む。標準偏差値は6自由度システムを用いて得られたデータを統計処理した値を使用している。後述する図９の螺旋駐車場を含むテストコースを実際に走行して採取したｙs軸加速度計出力データとｖxb_obs×ωzsとの差の標準偏差値は0.72（m/s2）であり、上記の標準偏差0.7（m/s2）は、乗用車の横方向加速度モーションのおおよその代表的なモデルと仮定できる。また、(ｃ)のシステムと同様にｚs軸加速度計出力を‐9.8（m/s2）とし、Nwazs=0.3（m/s2）とする。
（3）ｘs軸ジャイロスコープ出力に含まれるバイアス及び感度をそれぞれ
で固定する。また、ｙs軸ジャイロスコープ出力に含まれるバイアス及び感度をそれぞれ
で固定する。さらに、ｙs軸加速度計、zs軸加速度計出力に含まれるバイアス
で固定する。
（4）ロール角＝0の測定量を後述のカルマンフィルタ補正部４０で使用する。
【００４８】
（ｅ） 車速パルスセンサー、加速度計0軸、及びジャイロスコープ1軸のシステム：
かかるシステムでは以下の(1)〜(4)に従ってセンサー出力を決定する。
（1）ジャイロスコープ1軸はｚs軸を使用するものとする（図3(C)。
（2） (ｃ)のシステムと同様にxs軸ジャイロスコープ出力を0（ωxs=P=0）とし、
とし、ｙs軸ジャイロスコープ出力を0（ωys=Q=0）とし、
とする。また、ｘs軸加速度計出力を0とし、
とする。即ち、車両モーションによるセンサー前方向加速度の大きさを、平均値0（m/s2）と、 標準偏差0.8（m/s2）のホワイトノイズの加算であると仮定する。この標準偏差値は坂道の多さやドライバーの加減速によって0.5〜1.0（m/s2）と異なる値であるが、ここでは後述する図９の螺旋駐車場を含むテストコースを実際に走行して採取したｘs軸加速度計出力データの標準偏差値0.84（m/s2）を採用する。
更に、(ｄ)のシステムと同様にｙs軸加速度計出力を
車速パルスから得られる速度（ｖxb_obs）×ｚs軸ジャイロスコープ出力ωzs（=R)
とし（掛け算の結果の単位はm/s2）、
とする。また、ｚs軸加速度計出力を‐9.8（m/s2）とし、
とする。
（3）ｘs軸ジャイロスコープ出力に含まれるバイアス及び感度をそれぞれ
で固定する。また、ｙs軸ジャイロスコープ出力に含まれるバイアス及び感度をそれぞれ
で固定する。さらに、xs軸加速度計、ｙs軸加速度計、zs軸加速度計出力に含まれるバイアス
で固定する。
（4）ロール角＝0の測定量を、後述のカルマンフィルタ補正部４０で使用する。
以上の（1）−（4）におけるパラメータの設定例は代表的なものであり、設計者が意図に応じてデザインパラメータとして変化させることも可能である。ただしオーダーが変わるほどの変化でない限り、システム性能はさほど変わらない。
【００４９】
（Ｆ）カルマンフィルタ補正部の計算
次に、低周波の測定値入力に伴う、カルマンフィルタ補正部４０の計算方法について説明する。なお、以下の測定方程式で、
のように表されているのは、ホワイトノイズでモデル化された測定ノイズであり、その標準偏差は
の様に表されている。
【００５０】
（ａ）通常走行状態
通常走行状態においては、次式の測定方程式
【数２１】
を用いる。但し、
ｖxb_obs = 車速パルス間距離をパルス間隔で割ることで得られ、ｖyb_obs = 0、ｖzb_obs = 0である。これらの標準偏差は例えば
である。ただし、これらの値はデザイン値であり、ここに記した値は目安である。
【００５１】
（ｂ）静止状態
車速パルスが2秒以上入ってこないとき、車両が静止状態にあると判定する。静止状態において、測定値及びそれらの標準偏差は
であるとし、これに加えて以下の測定方程式を用いる。
【数２２】
である。
【００５２】
（ｃ）GPS受信機測定状態
以下のGPS受信機測定値が得られるときには、上記通常走行状態と静止状態のいずれかの測定に加え、以下のGPS受信機測定結果を利用する。
【数２３】
但し、測定値と測定誤差標準偏差はGPS受信機５０から得られる。
【００５３】
ロール角＝0の情報が送られてきたときには、次の測定方程式を使用する。
【数２４】
これは、
c21 = sin(センサーロール角)・cos(センサーピッチ角)
に相当し、c21 = 0を満たすことで、ロール角＝0が自動的に満たされるからである。
【００５４】
（ｄ）非線形測定方程式
上記（21）−（24）式をまとめて、簡単に（25）式の非線形測定方程式で示すように行列式で表すことができる。
【数２５】
但し、測定ベクトル
は以下のように与えられる。
【数２６】
【００５５】
（ｅ）線形測定方程式（カルマンフィルタの観測式）
カルマンフィルタで使用するために（25）式を線形化すると、次式の線形測定方程式（カルマンフィルタの観測式）が得られる。
【数２７】
但し、微小擾乱測定ベクトル
は以下のように与えられる。
【数２８】
また、行列（観測行列という）
は図８に示すように与えられる。通常走行状態では図８の行列内の（1）の行を観測行列として使用し、静止状態では（1）の行と（2）の行を観測行列として使用する。もしもGPS受信機測定が同時に得られるときには（1）の行又は（1）の行と（2）の行に（3）の行を合わせて観測行列として使用する（特願２００７−２３３５１５参照）。さらに、センサー構成によっては（４）の行を加える。
【００５６】
測定値が得られたとき、以下の（29）−(31)式の反復計算をi = 0 から n回行う。通常n = 2回以上反復すれば反復しないときに比べ大幅に精度が向上し、10回以上行う必要はあまりない。測位が得られるたびに反復計算を行うこの方法は、反復拡張カルマンフィルタ（Iterated Extended Kalman Filter）と呼ばれる周知の方法である（以下の文献参照）。
Gelb, A., Applied Optimal Estimation, M.I.T. Press, Cambridge, MA, 1974, pp. 182-91.
【数２９】
【数３０】
【数３１】
但し、上式において、
【数３２】
である。また、
は測定値に関するコバリアンス行列であり、使用する
に応じて変化する。すなわち、コバリアンス行列は通常走行状態であれば(33a)で表現され、静止状態であれば(33b)式で表現される。
【数３３】
GPS受信機測位が得られるときには、上記いずれかの走行状態に加え、GPS受信機の測定誤差に関するコバリアンスを以下のように付け加える。
【数３４】
センサー構成によっては、SCコントローラ２０から入力するロール角＝0の測定量を使用する際、以下の測定誤差に関するコバリアンスを付け加える。
【数３５】
【００５７】
(Ｇ）実験結果
図９から図１３は、本発明の有効性を示す実験結果を示している。図１４は比較のための、本発明を使用しない現行ナビゲーションシステムの結果を示している。実験の方法は、まず路上走行によって各センサーとGPS受信機のデータを収集し、ついで机上パーソナルコンピュータ(PC)で本発明のナビゲーションシステムを構成し、路上収集センサーデータをPCに入力することでシミュレーションを行う、というプロセスからなる。様々な場面での路上走行テストの中から、システム毎の差異が大きく表れる螺旋駐車場走行を利用して実験結果を比較している。これは螺旋駐車場内においてGPS受信機に信号が届かず、補正がかからない状態で上り下りしながら何度も回転するため、方位角推定には最も厳しい条件であり、自律航法システムの性能差が顕著に現れるためである。
図９から図１４は全て平面図であり、円弧を描いて見えるところが螺旋駐車場である。実験走行は、各図の（Ａ）に示される螺旋駐車場を２回(コース１とコース２)、続いて各図の（Ｂ）に示される螺旋駐車場を２回（コース３とコース４)、図中1−2−3−４の順に連続に走行している。連続した点の軌跡は本発明の複合システムの推定軌跡であり、まばらな点の軌跡は、GPS受信機のみによる推定した位置の軌跡である。いずれの図でも、推定軌跡はGPS受信機軌跡に引き寄せられて道路リンク（直線）上にあるが、螺旋駐車場の入り口、出口での方位角に注目してもらいたい。なお、駐車場前後で車両は直線道路上を走行している。
【００５８】
図９は、車速パルスセンサー、3軸加速度計、3軸ジャイロのセンサー構成からなる基本システム（6自由度システム：Acc3 Gyro3システム）の結果である推定軌跡を示している。推定軌跡１〜４は、駐車場への進入方位角と脱出方位角が一直線になり、実際の走行軌跡と同じであることが判る。
【００５９】
図１０は、車速パルスセンサー、3軸加速度計、2軸ジャイロのセンサー構成からなるシステム(Acc3 Gyro2システム）の結果を示している。6自由度の基本システムとほぼ遜色のない結果が得られていることが確認できる。
【００６０】
図１１は、車速パルスセンサー、3軸あるいは2軸加速度計、1軸ジャイロのセンサー構成からなるシステム（Acc3 Gyro1システムまたはAcc2 Gyro1システム）の結果を示している。ここでは前述のように、加速度計が3軸でも2軸でも、同じアルゴリズムを用いている。このシステムでは出口方位角において10度〜20度の誤差が生じている推定軌跡１，３，４が確認できる。
【００６１】
図１２は、車速パルスセンサー、1軸加速度計、1軸ジャイロのセンサー構成（Acc1 Gyro1システム）からなるシステムの結果を示している。図１１のシステムと同様に出口方位角において10度〜15度の誤差が生じている推定軌跡１，３，４が確認できる。
【００６２】
図１３は、車速パルスセンサー、0軸加速度計、1軸ジャイロのセンサー構成からなるシステム(Acc0 Gyro1システム）の結果を示している。出口方位角において10度〜15度の誤差が生じている推定軌跡３，４が確認できる。なお、このセンサー構成システムはピッチ角推定性能をもたず、高度に関する推定は自律航法で行わず、GPS受信機の測位データによってのみ補正が行われる。
【００６３】
図１４は比較のため、本発明を使用していない、車速パルスセンサー、1軸加速度計、1軸ジャイロのセンサー構成からなる現行ナビゲーション商品の結果を示している。このシステムは車両のロール角、ピッチ角の影響は考慮されておらず、かつ、カルマンフィルタ処理を用いないため感度誤差補正、バイアス補正が不正確になり、螺旋駐車場のような厳しいケースで、出口方位角が大きく不正確になることを示している。すなわち、実際の走行軌跡は進入方位角と脱出方位角が一直線であるが、図１４では、推定された進入方位角と脱出方位角が一直線にならず、脱出方位が大幅にずれ、走行道路から大幅にずれている。
【００６４】
図１５から図１９は、シミュレーション計算中に使用した、コバリアンス行列Pの対角要素（σ2に相当）を用いて、位置と方位角の推定値に関する2σ値（2×標準偏差σ＝96％誤差範囲）をプロットしたもので、横軸は経過時間(sec)、縦軸は誤差距離（m）及び誤差角度（deg）である。この方法は、一つの走行データを使って統計的な精度を論じるための手段であり、ホワイトノイズの仮定が正しいときに成り立つものである。図１５−図１９は、それぞれ、対応する図９−図１３における螺旋駐車場走行コース３における入り口から出口付近までの位置誤差2σ値（図１５−図１９の(A)）、及び方位角誤差2σ値（図１５−図１９の(B)）を示しており、駐車場内でGPS受信機補正が掛からないために右上がりになっている。なお、駐車場外でGPS受信機の補正を受けた際は、いずれのシステム精度もGPS受信機精度以上に収束し、システム毎の差異は表れない。
【００６５】
図１５は、6自由度の基本システムの螺旋駐車場走行３における精度結果を示している。位置精度、方位角精度ともに５つのシステムの中で最高（２σ値が最小）であり、システムの中の基準精度を示す。
【００６６】
図１６は、Acc3 Gyro2システムの螺旋駐車場走行コース３における精度結果を示している。6自由度の基本システムに比べ、平面位置精度（Ｎ、Ｅ）はほぼ同等であり、高さ誤差が1.5倍程度大きくなるものの、方位角精度もほぼ同等であることが確認できる。
【００６７】
図１７は、Acc3 Gyro1システムの螺旋駐車場走行コース３における精度結果を示している。6自由度の基本システムに比べ、平面位置精度（Ｎ、Ｅ）はほぼ同等、高さ誤差が約２倍、方位角誤差が約２倍程度であることが確認できる。
【００６８】
図１８は、Acc1 Gyro1システムの螺旋駐車場走行コース３における精度結果を示している。6自由度の基本システムに比べ、平面位置精度（Ｎ、Ｅ）はほぼ同等、高さ誤差が約２倍、方位角誤差が約1.5倍程度であることが確認できる。この時点で、本案の具体例を用いた場合、安価なAcc1 Gyro1システムの方がAcc3 Gyro1システムよりも高性能であることがわかった。
【００６９】
図１９は、Acc0 Gyro1システムの螺旋駐車場走行コース３ににおける精度結果を示している。6自由度の基本システムに比べ、平面位置精度（Ｎ、Ｅ）はほぼ同等に対して、高さ誤差が約7倍と極端に悪いのは、Acc0 Gyro1システムは自律航法で高さ方向をトラックしないからである。一方、方位角誤差は約1.5倍程度と、Acc1 Gyro1システムとほぼ同等であることが確認できる。
【００７０】
図２０は前記精度性能をまとめた図表である。右端列に目安となるコストを示したが、これは、現在の一般的な車両スペックセンサーの量産値段をもとにして次式、
（加速度計１軸￥500）×軸数＋（ジャイロ１軸￥700）×軸数
から概算したものである。なお、左端のAiGjはACCi Gyrojと同じである。
【００７１】
なお、本発明方法と異なり、カルマンフィルタやパーティクルフィルタといった確率最適理論を使用しない方法では、図２０のような理論精度とコストを関連付けた議論ができず、このため、強い商品設計が困難である。
【００７２】
上述してきたように、本発明によれば、センサーから取得した情報を評価することで現在のセンサー構成を判別し、判別したセンサー構成と基本となる6自由度の基本システムとの差異を補完する手段を講じ、センサー構成によらず常に前記6自由度の基本システムのアルゴリズムを使用して、カルマンフィルタ処理を行い車両状態量の推定計算を行う。結果として、複数センサー構成に対して1つのソフトウェアで対応できるので開発効率が上がる。また、個別のセンサー構成にだけ対応するアルゴリズムよりも、力学的に推定・補間したセンサー信号の分だけ多くの情報を使用することになり、自律航法計算の精度が向上する。このためセンサー軸数を削減しても、個別のセンサー構成にだけ対応するアルゴリズムよりも精度減少が少ない。
【図面の簡単な説明】
【００７３】
【図１】本発明の複合ナビゲーションシステムの基本的な構成図である。
【図２】センサー固定座標系と、センサーボード内部のセンサー構成の第１の説明図である。
【図３】センサー固定座標系と、センサーボード内部のセンサー構成の第２の説明図である。
【図４】車両座標系およびNED座標系の説明図である。
【図５】本発明に係る複数センサー構成を単一ソフトウェアで対応する処理のフローチャートである。
【図６】行列Fの構成要素を説明する説明図である。
【図７】行列Gkの構成要素を説明する説明図である。
【図８】行列Hの構成要素を説明する説明図である。
【図９】車速パルスセンサー、3軸加速度計、3軸ジャイロのセンサー構成からなるシステム（6自由度の基本システム）の推定軌跡の説明図である。
【図１０】車速パルスセンサー、3軸加速度計、2軸ジャイロのセンサー構成からなるシステム(Acc3 Gyro2システム)の推定軌跡の説明図である。
【図１１】車速パルスセンサー、3軸あるいは2軸加速度計、1軸ジャイロのセンサー構成からなるシステム(Acc3 Gyro1システムあるいはAcc2 Gyro1システム)の推定軌跡の説明図である。
【図１２】車速パルスセンサー、1軸加速度計、1軸ジャイロのセンサー構成からなるシステム(Acc1 Gyro1システム)の推定軌跡の説明図である。
【図１３】車速パルスセンサー、0軸加速度計、1軸ジャイロのセンサー構成からなるシステム(Acc0 Gyro1システム)の推定軌跡の説明図である。
【図１４】本発明を用いない、車速パルスセンサー、1軸加速度計、1軸ジャイロのセンサー構成からなるシステムの推定軌跡の説明図である。
【図１５】6自由度の基本システムの位置、方位角に関する２σ理論値(精度)の説明図である。
【図１６】Acc3 Gyro2システムの位置、方位角に関する２σ理論値(精度)の説明図である。
【図１７】Acc3 Gyro1システムの位置、方位角に関する２σ理論値(精度)の説明図である。
【図１８】Acc1 Gyro1システムの位置、方位角に関する２σ理論値(精度)の説明図である。
【図１９】Acc0 Gyro1システムの位置、方位角に関する２σ理論値(精度)の説明図である。
【図２０】センサー構成ごとの精度性能とコストをまとめた図表である。
【符号の説明】
【００７４】
１０ センサーボード
２０ SCコントローラ
３０ 自律航法計算部
４０ カルマンフィルタ補正部
５０ GPS受信機
６０ 演算部
【技術分野】
【０００１】
本発明は、車載ナビゲーションシステム及びナビゲーション方法に係わり、特に、任意のセンサー構成システムにおいても6自由度の基本システムのアルゴリズムを共通に利用できるようにし、かつセンサー軸数が減少しても大幅な精度減少がない、車載ナビゲーションシステム及びナビゲーション方法に関する。
【背景技術】
【０００２】
自律航法システムは、車両の距離センサーである車速パルスセンサーや、加速度計と角速度計などの慣性センサーから成り、動体である車両の位置、速度、及び姿勢の推定のために広く使用されている。
【０００３】
慣性航法システム（Inertial Navigation System：INS）は、通常３軸加速度計と３軸ジャイロスコープからなり、動体の６自由度、即ち３次元の並進運動と３次元の回転運動を同時に推定するために使用される。例えば、航空・宇宙機のナビゲーションは、かかる慣性航法システムを用いて、動体の６自由度、即ち３次元の並進運動と３次元の回転運動を同時に推定する。車両ナビゲーションにおいては車速パルスセンサーを使用することで、コストの高い慣性センサーを削減する場合が多い。
【０００４】
自律航法は外部信号に頼らずにセンサー出力に基づいて推定位置の更新を行うことができるが、センサー出力に含まれるノイズなどのエラーが時間の経過とともに積分計算に伴って蓄積するため、長時間は使用できない。そこで、衛星信号の届く範囲では有限誤差の位置推定を常時与える全地球測位システム（Global Positioning System: GPS受信機）と自律航法システムを、カルマンフィルタなどの最適確率アルゴリズムで結合することにより、両システムの性能上の問題を補間・解決する手法が考案された(特許文献１)。このようなシステムは複合ナビゲーションシステム（Integrated Navigation System）と呼ばれ、現在では車両ナビゲーションから航空宇宙ナビゲーションまで、幅広く利用されている。
【０００５】
近年の車両ナビゲーションにおいては、GPS受信機単体を使用するローコスト商品から、GPS受信機と任意のセンサーを組み合わせて使用するハイエンド商品まで、様々なセンサー構成のナビゲーションシステムが存在する。しかし、一社でローコスト商品からハイエンド商品まで扱う場合、各商品におけるセンサーの組み合わせは異なり、通常、商品毎に異なるソフトウェア（SW）を開発する必要があり、開発とメンテナンスの手間が商品の種類の数に比例して増大する問題があった。
【特許文献１】特許３４７３１１７号
【発明の開示】
【発明が解決しようとする課題】
【０００６】
従って、センサーの組み合わせが異なる複数のシステムに共通に対応できる単一のプラットフォームソフトウェア（SW）を備え、しかもセンサー軸数が減少しても大幅な精度減少がないナビゲーション方法及び車載ナビゲーションシステムが要望されている。
【０００７】
本発明の目的は、単一プラットフォームソフトウェア（SW）でセンサーの組み合わせが異なる複数のシステムに対応できるようにし、しかもセンサー軸数が減少しても大きな精度減少がないようにすることである。
本発明の別の目的は、任意のセンサー構成システムにおいて6自由度の基本システムのアルゴリズムを共通に利用できるようにすることである。
【課題を解決するための手段】
【０００８】
本発明は車速パルスセンサー、加速度計や角速度計（ジャイロスコープ）およびGPS受信機の複数のセンサーを複合した車載ナビゲーションシステムおよびナビゲーション方法である。
・ナビゲーション方法
本発明のナビゲーション方法は、いくつかのセンサーの組み合わせで構成される複数のシステムの中から所定のセンサー構成システムを選択するステップ、前記選択されたセンサー構成システムと全センサーを備えた基本システムとの差異を補完するステップ、前記基本システムの制御アルゴリズムに基づいてカルマンフィルタ処理を行って車両の位置、速度及び姿勢の推定値を得るステップを備えている。
前記センサー構成システムは少なくとも、
GPS受信機、車速パルスセンサー、加速度計３軸、ジャイロスコープ３軸、
GPS受信機、車速パルスセンサー、加速度計３軸、ジャイロスコープ２軸、
GPS受信機、車速パルスセンサー、加速度計３軸、ジャイロスコープ１軸、
GPS受信機、車速パルスセンサー、加速度計２軸、ジャイロスコープ１軸、
GPS受信機、車速パルスセンサー、加速度計１軸、ジャイロスコープ１軸、
GPS受信機、車速パルスセンサー、加速度計不使用(０軸)、ジャイロスコープ１軸、
の内のいずれかであり、前記基本システムはGPS受信機、車速パルスセンサー、加速度計３軸、ジャイロスコープ３軸を備えた６自由度のシステムである。
本発明のナビゲーション方法は更に、前記選択されたセンサー構成システムをシステム設計時に予め設定し、あるいは、センサーから得られる信号をもとに自動的に判別するステップを備えている。
又、本発明のナビゲーション方法において、前記差異を補完する補完手段は、前記選択されたセンサー構成システムにおける基本システムから欠けているセンサーの出力値として、０またはホワイトノイズを採用する。あるいは、補完手段は、前記選択されたセンサー構成システムにおける基本システムから欠けているセンサーの出力値として、定数を採用する。あるいは補完手段は、前記選択されたセンサー構成システムにおける基本システムから欠けているセンサーの出力値として、他センサーの出力値を変数とする関数の関数値を採用する。
【０００９】
・車載ナビゲーションシステム
本発明の車載ナビゲーションシステムは、いくつかのセンサーの組み合わせで構成される複数のシステムの中から選択した所定のセンサー構成システムと全センサーを備えた基本システムとの差異を補完する補完部、補完された値及びシステムのセンサー出力信号を用いて自律航法計算する自律航法計算部、前記基本システムの制御アルゴリズムに基づいて自律航法計算結果及びGPS測定結果を用いてカルマンフィルタ処理を行い、車両の位置、速度及び姿勢の推定値を得る処理部を備えている。前記センサー構成システムは、前記のナビゲーション方法において示した複数の構成が可能であり、前記基本システムはGPS受信機、車速パルスセンサー、加速度計３軸、ジャイロスコープ３軸を備えた６自由度のシステムである。
本発明の車載ナビゲーションシステムは、更に、前記選択されたセンサー構成システムをシステム設計時に予め設定する手段、あるいは、センサーから得られる信号をもとに選択されたセンサー構成システムを自動的に判別する手段を備えている。
又、本発明の車載ナビゲーションシステムにおいて、前記補完部は、前記選択されたセンサー構成システムにおける基本システムから欠けているセンサーの出力値として、０またはホワイトノイズを採用する。あるいは、補完部は、前記選択されたセンサー構成システムにおける基本システムから欠けているセンサーの出力値として、定数を採用する。あるいは補完部は、前記選択されたセンサー構成システムにおける基本システムから欠けているセンサーの出力値として、他センサーの出力値を変数とする関数の関数値を採用する。
【発明の効果】
【００１０】
本発明によれば、予め定義されたセンサー構成に応じて、基本の6自由度システムから欠けているセンサーの出力値として、力学的な知識に基づいて所定の値を採用することにより、自律航法計算部は常に同じ6自由度の基本システムの計算を行うことができる。結果として同じソフトウェア（SW）で異なるセンサー構成システムに対応することができ、開発工数を削減できる。また、個別のセンサー構成システムにだけ対応するアルゴリズムよりも、本発明では力学的に推定・補完したセンサー信号の分だけ多くの情報を使用することになり、自律航法計算の精度が向上する。このため本発明によれば、センサー軸数を削減しても、個別のセンサー構成システムにだけ対応するアルゴリズムに比べて精度減少が少ない。
【発明を実施するための最良の形態】
【００１１】
(Ａ) 複合ナビゲーションシステムの基本的な構成
図１は、車両に搭載される本発明の複合ナビゲーションシステムの基本的な構成の例を示す概略ブロック線図である。
【００１２】
自律系の入力信号は、センサーボード１０上の慣性センサーから得られる信号と、別のケーブルを通じて車体から直接ナビゲーションCPUに取り込まれる車速パルスである。慣性センサーとしては加速度計(加速度１〜３軸)、ジャイロスコープ（角速度１〜３軸)を採用し、車速パルスを発生するセンサーとしては車両が所定距離移動する毎に１個のパルスを発生する車速センサーを採用する。
【００１３】
センサー・コンフィギュレーション（SC）・コントローラ２０は、車載ナビゲーションシステムの実際のセンサー構成を設定部１５から設定され、あるいは、センサー出力から取得した情報を評価することで該センサー構成を判別する。なお、センサー出力から取得した情報の評価とは、「センサー出力信号が異常値であるとき、システムは該当するセンサーを使わないセンサー構成であると自動判別すること」を指す。センサー出力信号が異常値であるとは、例えば、（1）常時一定電圧を出力している、あるいは、（2）バイアスや感度が仕様範囲を超えている、ということである。
【００１４】
SCコントローラ２０は、前記判別されたセンサー構成に応じて、6自由度の基本システムから欠けているセンサーの出力値として、力学的な知識をもとに、定数(０を含む)、あるいはホワイトノイズ、あるいは他のセンサー出力値を変数とする関数の関数値を採用し、各センサーの出力値を自律航法計算部３０に送る。また、SCコントローラ２０は、前記判別されたセンサー構成がロール角を推定するセンサーをもたないとき、ロール角推定量が発散するのを防ぐために、ロール角＝0の測定量をカルマンフィルタ補正部４０に送る。このとき実際の車両では最大５度程度のロール角が生じるが、カルマンフィルタ補正部４０では実際値との誤差はホワイトノイズとして取り扱う。
【００１５】
自律航法計算部２０は常に同じ6自由度の基本システムのアルゴリズムを用いて、車両の現在の位置、速度及び姿勢を含む車両状態量を、高周波数で更新する。
【００１６】
GPS受信機５０は、複数の人工衛星から距離と距離変化率に関する信号を受信することで、車両のアンテナの位置と車両速度の測定値を提供する。
【００１７】
演算部６０は、自律航法計算部３０により推定された車両状態量と、GPS受信機５０から得られる位置・速度測定値を含む測定量との差分をとり、カルマンフィルタ補正部４０はカルマンフィルタを用いて該差分に基づいて車両の現在の位置、速度及び姿勢を含む車両状態量の補正を行う。
【００１８】
(Ｂ)種々のシステムにおけるセンサー構成
図２(A)は、図１におけるセンサーボード１０に固定した座標系Xs-Ys-Zsを示している。なお、以下の数式の中でセンサー（ボード）座標系に関する表現であることを、添え字の”ｓ”によって表現することに注意すべきである。
【００１９】
図２(B)は、(A)のセンサーボード１０において、加速度計3軸（Acc x, Acc y, Acc z）およびジャイロスコープ3軸（Gyro x, Gyro y, Gyro z）から成る、即ち6自由度の基本システムのセンサー構成を示したものである。3軸加速度計Acc x〜Acc zは、センサー座標系の三つの座標方向（ｘ，ｙ，ｚ）における加速度を検出し、また3軸ジャイロスコープGyro x〜Gyro zは、センサー座標系の三つの座標軸（ｘ，ｙ，ｚ）周りの角速度P,Q,Rを検出する。
【００２０】
図２（C）は、(A)のセンサーボード１０において、加速度計3軸、ジャイロスコープ2軸から成るシステムのセンサー構成を示したものである。ここで、ジャイロスコープが2軸しか検出できないとき、システムのジャイロスコープはGyro xおよび Gyro zである。
【００２１】
図２（D）は、(A)のセンサーボード１０において、加速度計3軸、ジャイロスコープ1軸から成るシステムのセンサー構成を示したものである。ここで、ジャイロスコープが１軸しか検出できないとき、システムのジャイロスコープはGyro zである。
【００２２】
図３（A）は、図２(A)のセンサーボード１０において、加速度計2軸、ジャイロスコープ1軸から成るシステムのセンサー構成を示したものである。ここで、加速度計が2軸しか検出できないとき、システムの加速度計はAcc x, Acc yであり、ジャイロスコープが１軸しか検出できないとき、システムのジャイロスコープはGyro zである。
【００２３】
図３(B)は図２（A）のセンサーボード１０において、加速度計1軸、ジャイロスコープ1軸ら成るシステムのセンサー構成を示したものである。ここで、加速度計が１軸しか検出できないとき、システムの加速度計はAcc xであり、ジャイロスコープが１軸しか検出できないとき、システムのジャイロスコープはGyro zである。
【００２４】
図３(C)は、図２（A）のセンサーボード１０において、加速度計0軸（不使用）、ジャイロスコープ1軸から成るシステムのセンサー構成を示したものであり、ジャイロスコープが１軸しか検出できないとき、システムのジャイロスコープはGyro zである。
【００２５】
図４（A）は、車両に固定した座標系Xb-Yb-Zbを示している。以下の数式の中で車両固定座標系に関する表現であることを、添え字の”b”によって表現する。
【００２６】
図４(B)は、ある緯度、経度における、地表固定座標系North-East-Down（NED座標系）を示している。以下の数式の中でNED座標系に関する表現であることを、添え字の”n”によって表現する。
【００２７】
（Ｃ）基本的作動プロセス
図５は、本発明に係るナビゲーションシステムの基本的作動プロセスの例を示しているフローチャートである。工程101において、車速パルスセンサー、慣性センサーである加速度計および角速度計をもとに予めいくつかのセンサー構成を定義する（図２、図３参照）。
【００２８】
次に工程102において、SCコントローラ２０は前記センサーから得られる信号をもとにナビゲーションシステムの前記センサー構成を判別する。
【００２９】
工程103において、SCコントローラ２０は前記判別したセンサー構成に応じて、6自由度の基本システムから欠けているセンサーの出力値として、力学的な知識をもとに、定数(０を含む)、あるいはホワイトノイズ、あるいは他のセンサー出力を変数とする関数の関数値を採用し、各センサーの出力を自律航法計算部３０に送る。また、SCコントローラ２０はロール角＝0の測定量をカルマンフィルタ補正部４０に送る。なお、ロール角＝0の測定量をカルマンフィルタ補正部４０に送らない場合もある。
【００３０】
工程104において、自律航法計算部３０は、該センサー出力として採用した値と実際のセンサー出力値を基に、常に同じ6自由度基本システムのアルゴリズムを使って、車両状態量を高周波で更新する自律航法計算を行う。
【００３１】
工程105において、演算部６０は自律航法計算部３０により推定された車両状態量と、GPS受信機５０から得られる位置・速度測定値を含む測定量との差分を演算し、カルマンフィルタ補正部４０はカルマンフィルタを用いて該差分に基づいて車両の現在の位置、速度及び姿勢を含む車両状態量の補正を行う。
【００３２】
工程106において、複合自律/GPS受信機ナビゲーションシステムは、ナビゲーション精度を連続して最適化するために前記の工程102から工程105を反復実行する。
【００３３】
以下では、センサー構成によらず常に基本となる6自由度システムのアルゴリズムを使用して、車両状態量の推定計算を行う具体的な手法を説明する。
【００３４】
（Ｄ）自律航法計算部における自律航法計算アルゴリズム
まず、基本となる６自由度システムの、高周波自律航法計算部３０のアルゴリズムについて説明する。なお、以降の式においては、
で示すように変数の上にドットがついているものは、その変数ｖsの時間変化率を表しているものとする。
【００３５】
（ａ）速度方程式
速度方程式は次式
【数１】
で表現される。但し、
【数２】
であり、(2a)はセンサー固定座標系で表現される速度ベクトル、(2b)はセンサー固定座標系で表現されるジャイロスコープ出力ベクトル、(2c)はセンサー固定座標系で表現される加速度計出力ベクトル,(2d)はセンサー固定座標系で表現される重力ベクトルである。ここで、
【数３】
ここで、センサーの“感度”は、設置角の影響などを含めない、センサー自体の電圧出力から物理量への変換定数を意味する。通常センサーの生の出力は、出力がないことを示す０点電圧（しばしば２．５Ｖｏｌｔ）を中心とした電圧値（単位はＶｏｌｔ）であり、角度変化率や加速度などの物理量を得るためには、感度とよばれる変換用の定数を、出力電圧値と０点電圧の差に乗算する必要がある。感度は通常仕様値がセンサー毎に決められているが、低コストのセンサーの場合、実際の値が仕様値と大きく異なることや、温度補正が正確でないために感度が仕様値とずれるといったことがしばしば起こる。ことにジャイロに関する感度誤差はナビゲーションの方位計算に大きな影響を及ぼすので、このシステムではジャイロ感度をカルマンフィルタの状態量に含め、常時推定・補正を行う。
【００３６】
（ｂ） 姿勢方程式
姿勢方程式は次式で表現される。
【数４】
ただし、
【数４ａ】
はセンサー座標系からNED座標系への変換行列である。
（4）式は、慣性航法システム（Inertial Navigation System: INS）の一般的な手法である以下の姿勢方程式
【数５】
から、車両ナビゲーション適用の際に独立な4つのパラメータだけを抜き出したものである。(5)式はより一般的なINS姿勢方程式である。
【００３７】
（ｃ） 位置方程式
位置方程式は次式
【数６】
により表現できる。但し、(6)式において
【数７】
であり、(7a)は位置ベクトル、(7b)は車両移動距離ベクトルである。又、(7b)式において、Np はサンプル時間当たりのパルス数、Lはパルス間距離である。
【００３８】
（ｄ） 固定パラメータ方程式
固定パラメータ方程式は次式
【数８】
により表現される。但し、
【数９】
であり、(9a)はセンサー座標系ジャイロスコープ出力に含まれるバイアス（Volt）、(9b)はセンサー座標系加速度計出力に含まれるバイアス（m/s2）、(9c)はセンサー座標系ジャイロスコープ出力に関する感度（rad/sec/Volt）、(9d)はセンサー座標系から車両座標系への変換行列である。また、(8）式中p00、p10、及びp20、は取り付け角に関する行列の要素から、独立な3つのパラメータだけを抜き出したものである。
【００３９】
（ｅ） 非線形状態方程式
上記（1）、(4)、(6)、（8）式をまとめて、簡単に（10）式の様に行列式で表すことができる
【数１０】
但し、ｘは以下に与えられる非線形状態量ベクトルである。
【数１０ａ】
【００４０】
（ｆ） 状態量更新
CPU上で(10)式を数値積分するには、例えば、次式の状態量更新式
【数１１】
を用いて行えばよい。精度要求の高い場合は、周知の数値積分方法であるルンゲ・クッタ4次式(下記文献参照)などを用いてもよい。
Kreyszig, E., Advanced Engineering Mathematics, John Wiley & Sons, 1999, New York, NY.
但し、(11)式において、Tはサンプルタイムで、例えば25HzならばT=0.04秒である。以上は一般的な慣性航法システム（INS）に用いられる手法を基に、MEMSセンサー使用を考慮して、地球の丸みなど微小な項を簡略化した車両用慣性航法システム（INS）である(下記文献参照)。
Hoshizaki, T., Computational Scheme for MEMS Inertial Navigation Systems, AOAMR Patent, August, 2006.
【００４１】
（ｇ） 微小擾乱方程式
さて、GPS受信機などの測定値をもとに、カルマンフィルタ補正部４０がカルマンフィルタ処理で状態量ｘを補正を行うためには、自律航法計算部３０が状態量を非線形状態方程式により更新を行い、かつ、誤差量が線形方程式によって増加すると仮定して該誤差量（微小擾乱量）のコバリアンス値も更新する必要がある。そのために、（11）式の線形化を行い、微小擾乱方程式である行列式（12）を用意する。
【数１２】
但し、
は以下に与えられる微小擾乱ベクトルである。
【数１３】
ただし、(13b)は
の現在推定値、(13c)はホワイトノイズでモデル化された3軸ジャイロスコープ出力
に含まれる高周波ノイズと、3軸加速度計出力
に含まれる高周波ノイズである。それぞれのノイズの標準偏差(σ）はセンサースペックや、測定実験などによって得られ、ここではそれらの大きさを次のようにＮで表す。
【数１４】
また、
に関する微小擾乱量であり、
には、慣性航法システム（INS）の一般的な手法に基づき、以下の関係がある。
【数１５】
また、
に関する微小擾乱量であり、
には、慣性航法システム（INS）の一般的な手法に基づき、以下の関係がある。
【数１６】
ここで車両に対するセンサーの取り付けロール角＝0と仮定すれば、取り付けロール角に関する補正量
となる。
(12)式における線形システムを表わす行列
はそれぞれ図６、図７に示すように得られる。ただし、図６において、
【数１７】
【００４２】
（ｈ） コバリアンスの更新
以上のように得られた行列
を使って、カルマンフィルタ処理の定式に従い、高周波で以下のように微小擾乱ベクトル
のコバリアンス行列(誤差共分散行列)を次式により更新する。
【数１８】
但し、右肩のT は転地行列（Transpose）を意味し、右肩の−はカルマンフィルタの補正前の状態を意味し、右肩の＋はカルマンフィルタの補正後の状態を意味する。また、
の相関値の期待値（E[ ]）からなるコバリアンス行列であり、
に関するコバリアンス行列であり、それぞれ次式で表現される。
【数１９】
以上の（11）式及び（18）式が、前記6自由度の基本システムにおいて自律航法計算部３０が高周波で(例えば25Hzで)実行する演算式である。以下に(11)式及び（18）式を再掲する。
【数２０】
【００４３】
（Ｅ）任意のシステムの基本システムから欠けているセンサーの出力決定法
ここで所定のセンサー構成システムにおいて、6自由度の基本システムから欠けているセンサーの出力値の決定法を説明する。SCコントローラ２０は、力学的な知識をもとに、基本システムから欠けているセンサーの出力値として、定数、あるいはホワイトノイズ、あるいは他のセンサー出力を変数とする関数の関数値を採用して自律航法計算部３０に入力する。また、SCコントローラ２０は、ロール角＝0の測定量をカルマンフィルタ補正部40に入力する。
【００４４】
（ａ） 車速パルスセンサー、加速度計3軸、及びジャイロスコープ3軸を備えた基本システム：
かかる基本システムの場合、SCコントローラ２０は各センサー出力を自律航法計算部３０に入力し、自律航法計算部３０は入力されたセンサー出力を用いて前記自律航法計算アルゴリズムを実行する((11)、（18）式の演算を実行する)。
【００４５】
(ｂ) 車速パルスセンサー、加速度計3軸及びジャイロスコープ2軸を備えたシステム：
かかるシステムでは以下の(1)〜(3)に従ってセンサー出力を決定する。
（1）ジャイロスコープ2軸はｘs及びｚs軸を使用するものとする（図2(C）)。
（2）ｙs軸ジャイロスコープ出力を0（ωys=Q=0）とし、
とする。即ち、車両モーションによるピッチ角速度を、平均値0（rad/s）と標準偏差0.05（rad/s）＝2.9（deg/s）のホワイトノイズの加算であると仮定する。標準偏差値は6自由度の基本システムを用いて得られたデータを統計処理した値を使用している。後述する図８の螺旋駐車場を含むテストコースを実際に走行して採取したｙs軸ジャイロスコープ出力データの標準偏差値は0.054 （rad/s）であり、上記の標準偏差0.05（rad/s）は乗用車の車両ピッチ角モーションのおおよその代表的な値であると仮定できる。
（3）ｙs軸のジャイロスコープ出力に含まれるバイアス及び感度をそれぞれ
で固定する。
【００４６】
(ｃ) 車速パルスセンサー、加速度計3軸及びジャイロスコープ1軸を備えたシステム、あるいは 車速パルスセンサー、加速度計2軸及びジャイロスコープ1軸を備えたシステム：
かかるシステムでは以下の(1)〜(3)に従ってセンサー出力を決定する。
（1）加速度計2軸はｘs及びｙs軸を使用するものする。また、ジャイロスコープ1軸は、ｚs軸を使用する（図２(D)，図３（A））。
（2）ｘs軸ジャイロスコープ出力を0（ωxs=P=0）とし、
とする。即ち、車両モーションによるロール角速度の大きさを平均値0（rad/s）と、標準偏差0.05（rad/s）＝2.9（deg/s）のホワイトノイズの加算である仮定する。標準偏差値は6自由度の基本システムを用いて得られたデータを統計処理した値を使用している。後述する図９の螺旋駐車場を含むテストコースを実際に走行して採取したｘs軸ジャイロスコープ出力データの標準偏差値（0.049 rad/s）であり、上記の標準偏差0.05（rad/s）は乗用車の車両ロール角モーションのおおよその代表的な値であると仮定できる。
また、ｙs軸ジャイロスコープ出力を0（ωys=Q=0）とし、
とする。更に、
ｚs軸加速度計出力を‐9.8（m/s2）とし、
とする。即ち、車両モーションによるセンサー垂直方向加速度の大きさを平均値‐9.8（m/s2）と、 標準偏差0.3（m/s2）のホワイトノイズの加算であると仮定する。標準偏差値は6自由度システムを用いて得られたデータを統計処理した値を使用している。後述する図９の螺旋駐車場を含むテストコースを実際に走行して採取したｚs軸加速度計出力データの標準偏差値は0.28（m/s2）であり、上記の標準偏差0.3（m/s2）は乗用車の車両垂直方向加速度モーションのおおよその代表的な値であると仮定できる。
（3）ｘs軸ジャイロスコープ出力に含まれるバイアス及び感度をそれぞれ
で固定する。また、ｙs軸ジャイロスコープ出力に含まれるバイアス及び感度をそれぞれ
で固定する。さらに、zs軸加速度計出力に含まれるバイアス
で固定する。
【００４７】
（ｄ） 車速パルスセンサー、加速度計1軸、及びジャイロスコープ1軸のシステム：
かかるシステムでは以下の(1)〜(4)に従ってセンサー出力を決定する。
（1）加速度計1軸はｘs軸をするものとし、ジャイロスコープ1軸はｚs軸を使用するものとする（図３（B）参照）。
（2）(ｃ)のシステムと同様にｘs軸ジャイロスコープ出力を0（ωxs=P=0）とし、
とする。また、ｙs軸ジャイロスコープ出力を0（ωys=Q=0）とし、
とする。更に、ys軸加速度計出力を、
車速パルスから得られる速度（ｖxb_obs）×ｚs軸ジャイロスコープ出力ωzs（=R)
とする（掛け算の結果の単位はm/s2）。速度ｖxb_obsについては後述するが、ys軸加速度計出力は、ｚs軸ジャイロスコープ出力ωzsを変数とする関数の関数値であるといえる。また
とする。即ち、車両モーションによるセンサー横方向加速度の大きさを、ｖxb_obs×ωzsの平均値と標準偏差0.7（m/s2）のホワイトノイズの加算であると仮定する。平均値を表す上式は、力学的に平面内回転運動において、横方向加速度が前方向速度と角速度の積で得られることから、定性的に推定されたものである。それ以外の横方向加速度出力はロール角による重力方向成分を含む。標準偏差値は6自由度システムを用いて得られたデータを統計処理した値を使用している。後述する図９の螺旋駐車場を含むテストコースを実際に走行して採取したｙs軸加速度計出力データとｖxb_obs×ωzsとの差の標準偏差値は0.72（m/s2）であり、上記の標準偏差0.7（m/s2）は、乗用車の横方向加速度モーションのおおよその代表的なモデルと仮定できる。また、(ｃ)のシステムと同様にｚs軸加速度計出力を‐9.8（m/s2）とし、Nwazs=0.3（m/s2）とする。
（3）ｘs軸ジャイロスコープ出力に含まれるバイアス及び感度をそれぞれ
で固定する。また、ｙs軸ジャイロスコープ出力に含まれるバイアス及び感度をそれぞれ
で固定する。さらに、ｙs軸加速度計、zs軸加速度計出力に含まれるバイアス
で固定する。
（4）ロール角＝0の測定量を後述のカルマンフィルタ補正部４０で使用する。
【００４８】
（ｅ） 車速パルスセンサー、加速度計0軸、及びジャイロスコープ1軸のシステム：
かかるシステムでは以下の(1)〜(4)に従ってセンサー出力を決定する。
（1）ジャイロスコープ1軸はｚs軸を使用するものとする（図3(C)。
（2） (ｃ)のシステムと同様にxs軸ジャイロスコープ出力を0（ωxs=P=0）とし、
とし、ｙs軸ジャイロスコープ出力を0（ωys=Q=0）とし、
とする。また、ｘs軸加速度計出力を0とし、
とする。即ち、車両モーションによるセンサー前方向加速度の大きさを、平均値0（m/s2）と、 標準偏差0.8（m/s2）のホワイトノイズの加算であると仮定する。この標準偏差値は坂道の多さやドライバーの加減速によって0.5〜1.0（m/s2）と異なる値であるが、ここでは後述する図９の螺旋駐車場を含むテストコースを実際に走行して採取したｘs軸加速度計出力データの標準偏差値0.84（m/s2）を採用する。
更に、(ｄ)のシステムと同様にｙs軸加速度計出力を
車速パルスから得られる速度（ｖxb_obs）×ｚs軸ジャイロスコープ出力ωzs（=R)
とし（掛け算の結果の単位はm/s2）、
とする。また、ｚs軸加速度計出力を‐9.8（m/s2）とし、
とする。
（3）ｘs軸ジャイロスコープ出力に含まれるバイアス及び感度をそれぞれ
で固定する。また、ｙs軸ジャイロスコープ出力に含まれるバイアス及び感度をそれぞれ
で固定する。さらに、xs軸加速度計、ｙs軸加速度計、zs軸加速度計出力に含まれるバイアス
で固定する。
（4）ロール角＝0の測定量を、後述のカルマンフィルタ補正部４０で使用する。
以上の（1）−（4）におけるパラメータの設定例は代表的なものであり、設計者が意図に応じてデザインパラメータとして変化させることも可能である。ただしオーダーが変わるほどの変化でない限り、システム性能はさほど変わらない。
【００４９】
（Ｆ）カルマンフィルタ補正部の計算
次に、低周波の測定値入力に伴う、カルマンフィルタ補正部４０の計算方法について説明する。なお、以下の測定方程式で、
のように表されているのは、ホワイトノイズでモデル化された測定ノイズであり、その標準偏差は
の様に表されている。
【００５０】
（ａ）通常走行状態
通常走行状態においては、次式の測定方程式
【数２１】
を用いる。但し、
ｖxb_obs = 車速パルス間距離をパルス間隔で割ることで得られ、ｖyb_obs = 0、ｖzb_obs = 0である。これらの標準偏差は例えば
である。ただし、これらの値はデザイン値であり、ここに記した値は目安である。
【００５１】
（ｂ）静止状態
車速パルスが2秒以上入ってこないとき、車両が静止状態にあると判定する。静止状態において、測定値及びそれらの標準偏差は
であるとし、これに加えて以下の測定方程式を用いる。
【数２２】
である。
【００５２】
（ｃ）GPS受信機測定状態
以下のGPS受信機測定値が得られるときには、上記通常走行状態と静止状態のいずれかの測定に加え、以下のGPS受信機測定結果を利用する。
【数２３】
但し、測定値と測定誤差標準偏差はGPS受信機５０から得られる。
【００５３】
ロール角＝0の情報が送られてきたときには、次の測定方程式を使用する。
【数２４】
これは、
c21 = sin(センサーロール角)・cos(センサーピッチ角)
に相当し、c21 = 0を満たすことで、ロール角＝0が自動的に満たされるからである。
【００５４】
（ｄ）非線形測定方程式
上記（21）−（24）式をまとめて、簡単に（25）式の非線形測定方程式で示すように行列式で表すことができる。
【数２５】
但し、測定ベクトル
は以下のように与えられる。
【数２６】
【００５５】
（ｅ）線形測定方程式（カルマンフィルタの観測式）
カルマンフィルタで使用するために（25）式を線形化すると、次式の線形測定方程式（カルマンフィルタの観測式）が得られる。
【数２７】
但し、微小擾乱測定ベクトル
は以下のように与えられる。
【数２８】
また、行列（観測行列という）
は図８に示すように与えられる。通常走行状態では図８の行列内の（1）の行を観測行列として使用し、静止状態では（1）の行と（2）の行を観測行列として使用する。もしもGPS受信機測定が同時に得られるときには（1）の行又は（1）の行と（2）の行に（3）の行を合わせて観測行列として使用する（特願２００７−２３３５１５参照）。さらに、センサー構成によっては（４）の行を加える。
【００５６】
測定値が得られたとき、以下の（29）−(31)式の反復計算をi = 0 から n回行う。通常n = 2回以上反復すれば反復しないときに比べ大幅に精度が向上し、10回以上行う必要はあまりない。測位が得られるたびに反復計算を行うこの方法は、反復拡張カルマンフィルタ（Iterated Extended Kalman Filter）と呼ばれる周知の方法である（以下の文献参照）。
Gelb, A., Applied Optimal Estimation, M.I.T. Press, Cambridge, MA, 1974, pp. 182-91.
【数２９】
【数３０】
【数３１】
但し、上式において、
【数３２】
である。また、
は測定値に関するコバリアンス行列であり、使用する
に応じて変化する。すなわち、コバリアンス行列は通常走行状態であれば(33a)で表現され、静止状態であれば(33b)式で表現される。
【数３３】
GPS受信機測位が得られるときには、上記いずれかの走行状態に加え、GPS受信機の測定誤差に関するコバリアンスを以下のように付け加える。
【数３４】
センサー構成によっては、SCコントローラ２０から入力するロール角＝0の測定量を使用する際、以下の測定誤差に関するコバリアンスを付け加える。
【数３５】
【００５７】
(Ｇ）実験結果
図９から図１３は、本発明の有効性を示す実験結果を示している。図１４は比較のための、本発明を使用しない現行ナビゲーションシステムの結果を示している。実験の方法は、まず路上走行によって各センサーとGPS受信機のデータを収集し、ついで机上パーソナルコンピュータ(PC)で本発明のナビゲーションシステムを構成し、路上収集センサーデータをPCに入力することでシミュレーションを行う、というプロセスからなる。様々な場面での路上走行テストの中から、システム毎の差異が大きく表れる螺旋駐車場走行を利用して実験結果を比較している。これは螺旋駐車場内においてGPS受信機に信号が届かず、補正がかからない状態で上り下りしながら何度も回転するため、方位角推定には最も厳しい条件であり、自律航法システムの性能差が顕著に現れるためである。
図９から図１４は全て平面図であり、円弧を描いて見えるところが螺旋駐車場である。実験走行は、各図の（Ａ）に示される螺旋駐車場を２回(コース１とコース２)、続いて各図の（Ｂ）に示される螺旋駐車場を２回（コース３とコース４)、図中1−2−3−４の順に連続に走行している。連続した点の軌跡は本発明の複合システムの推定軌跡であり、まばらな点の軌跡は、GPS受信機のみによる推定した位置の軌跡である。いずれの図でも、推定軌跡はGPS受信機軌跡に引き寄せられて道路リンク（直線）上にあるが、螺旋駐車場の入り口、出口での方位角に注目してもらいたい。なお、駐車場前後で車両は直線道路上を走行している。
【００５８】
図９は、車速パルスセンサー、3軸加速度計、3軸ジャイロのセンサー構成からなる基本システム（6自由度システム：Acc3 Gyro3システム）の結果である推定軌跡を示している。推定軌跡１〜４は、駐車場への進入方位角と脱出方位角が一直線になり、実際の走行軌跡と同じであることが判る。
【００５９】
図１０は、車速パルスセンサー、3軸加速度計、2軸ジャイロのセンサー構成からなるシステム(Acc3 Gyro2システム）の結果を示している。6自由度の基本システムとほぼ遜色のない結果が得られていることが確認できる。
【００６０】
図１１は、車速パルスセンサー、3軸あるいは2軸加速度計、1軸ジャイロのセンサー構成からなるシステム（Acc3 Gyro1システムまたはAcc2 Gyro1システム）の結果を示している。ここでは前述のように、加速度計が3軸でも2軸でも、同じアルゴリズムを用いている。このシステムでは出口方位角において10度〜20度の誤差が生じている推定軌跡１，３，４が確認できる。
【００６１】
図１２は、車速パルスセンサー、1軸加速度計、1軸ジャイロのセンサー構成（Acc1 Gyro1システム）からなるシステムの結果を示している。図１１のシステムと同様に出口方位角において10度〜15度の誤差が生じている推定軌跡１，３，４が確認できる。
【００６２】
図１３は、車速パルスセンサー、0軸加速度計、1軸ジャイロのセンサー構成からなるシステム(Acc0 Gyro1システム）の結果を示している。出口方位角において10度〜15度の誤差が生じている推定軌跡３，４が確認できる。なお、このセンサー構成システムはピッチ角推定性能をもたず、高度に関する推定は自律航法で行わず、GPS受信機の測位データによってのみ補正が行われる。
【００６３】
図１４は比較のため、本発明を使用していない、車速パルスセンサー、1軸加速度計、1軸ジャイロのセンサー構成からなる現行ナビゲーション商品の結果を示している。このシステムは車両のロール角、ピッチ角の影響は考慮されておらず、かつ、カルマンフィルタ処理を用いないため感度誤差補正、バイアス補正が不正確になり、螺旋駐車場のような厳しいケースで、出口方位角が大きく不正確になることを示している。すなわち、実際の走行軌跡は進入方位角と脱出方位角が一直線であるが、図１４では、推定された進入方位角と脱出方位角が一直線にならず、脱出方位が大幅にずれ、走行道路から大幅にずれている。
【００６４】
図１５から図１９は、シミュレーション計算中に使用した、コバリアンス行列Pの対角要素（σ2に相当）を用いて、位置と方位角の推定値に関する2σ値（2×標準偏差σ＝96％誤差範囲）をプロットしたもので、横軸は経過時間(sec)、縦軸は誤差距離（m）及び誤差角度（deg）である。この方法は、一つの走行データを使って統計的な精度を論じるための手段であり、ホワイトノイズの仮定が正しいときに成り立つものである。図１５−図１９は、それぞれ、対応する図９−図１３における螺旋駐車場走行コース３における入り口から出口付近までの位置誤差2σ値（図１５−図１９の(A)）、及び方位角誤差2σ値（図１５−図１９の(B)）を示しており、駐車場内でGPS受信機補正が掛からないために右上がりになっている。なお、駐車場外でGPS受信機の補正を受けた際は、いずれのシステム精度もGPS受信機精度以上に収束し、システム毎の差異は表れない。
【００６５】
図１５は、6自由度の基本システムの螺旋駐車場走行３における精度結果を示している。位置精度、方位角精度ともに５つのシステムの中で最高（２σ値が最小）であり、システムの中の基準精度を示す。
【００６６】
図１６は、Acc3 Gyro2システムの螺旋駐車場走行コース３における精度結果を示している。6自由度の基本システムに比べ、平面位置精度（Ｎ、Ｅ）はほぼ同等であり、高さ誤差が1.5倍程度大きくなるものの、方位角精度もほぼ同等であることが確認できる。
【００６７】
図１７は、Acc3 Gyro1システムの螺旋駐車場走行コース３における精度結果を示している。6自由度の基本システムに比べ、平面位置精度（Ｎ、Ｅ）はほぼ同等、高さ誤差が約２倍、方位角誤差が約２倍程度であることが確認できる。
【００６８】
図１８は、Acc1 Gyro1システムの螺旋駐車場走行コース３における精度結果を示している。6自由度の基本システムに比べ、平面位置精度（Ｎ、Ｅ）はほぼ同等、高さ誤差が約２倍、方位角誤差が約1.5倍程度であることが確認できる。この時点で、本案の具体例を用いた場合、安価なAcc1 Gyro1システムの方がAcc3 Gyro1システムよりも高性能であることがわかった。
【００６９】
図１９は、Acc0 Gyro1システムの螺旋駐車場走行コース３ににおける精度結果を示している。6自由度の基本システムに比べ、平面位置精度（Ｎ、Ｅ）はほぼ同等に対して、高さ誤差が約7倍と極端に悪いのは、Acc0 Gyro1システムは自律航法で高さ方向をトラックしないからである。一方、方位角誤差は約1.5倍程度と、Acc1 Gyro1システムとほぼ同等であることが確認できる。
【００７０】
図２０は前記精度性能をまとめた図表である。右端列に目安となるコストを示したが、これは、現在の一般的な車両スペックセンサーの量産値段をもとにして次式、
（加速度計１軸￥500）×軸数＋（ジャイロ１軸￥700）×軸数
から概算したものである。なお、左端のAiGjはACCi Gyrojと同じである。
【００７１】
なお、本発明方法と異なり、カルマンフィルタやパーティクルフィルタといった確率最適理論を使用しない方法では、図２０のような理論精度とコストを関連付けた議論ができず、このため、強い商品設計が困難である。
【００７２】
上述してきたように、本発明によれば、センサーから取得した情報を評価することで現在のセンサー構成を判別し、判別したセンサー構成と基本となる6自由度の基本システムとの差異を補完する手段を講じ、センサー構成によらず常に前記6自由度の基本システムのアルゴリズムを使用して、カルマンフィルタ処理を行い車両状態量の推定計算を行う。結果として、複数センサー構成に対して1つのソフトウェアで対応できるので開発効率が上がる。また、個別のセンサー構成にだけ対応するアルゴリズムよりも、力学的に推定・補間したセンサー信号の分だけ多くの情報を使用することになり、自律航法計算の精度が向上する。このためセンサー軸数を削減しても、個別のセンサー構成にだけ対応するアルゴリズムよりも精度減少が少ない。
【図面の簡単な説明】
【００７３】
【図１】本発明の複合ナビゲーションシステムの基本的な構成図である。
【図２】センサー固定座標系と、センサーボード内部のセンサー構成の第１の説明図である。
【図３】センサー固定座標系と、センサーボード内部のセンサー構成の第２の説明図である。
【図４】車両座標系およびNED座標系の説明図である。
【図５】本発明に係る複数センサー構成を単一ソフトウェアで対応する処理のフローチャートである。
【図６】行列Fの構成要素を説明する説明図である。
【図７】行列Gkの構成要素を説明する説明図である。
【図８】行列Hの構成要素を説明する説明図である。
【図９】車速パルスセンサー、3軸加速度計、3軸ジャイロのセンサー構成からなるシステム（6自由度の基本システム）の推定軌跡の説明図である。
【図１０】車速パルスセンサー、3軸加速度計、2軸ジャイロのセンサー構成からなるシステム(Acc3 Gyro2システム)の推定軌跡の説明図である。
【図１１】車速パルスセンサー、3軸あるいは2軸加速度計、1軸ジャイロのセンサー構成からなるシステム(Acc3 Gyro1システムあるいはAcc2 Gyro1システム)の推定軌跡の説明図である。
【図１２】車速パルスセンサー、1軸加速度計、1軸ジャイロのセンサー構成からなるシステム(Acc1 Gyro1システム)の推定軌跡の説明図である。
【図１３】車速パルスセンサー、0軸加速度計、1軸ジャイロのセンサー構成からなるシステム(Acc0 Gyro1システム)の推定軌跡の説明図である。
【図１４】本発明を用いない、車速パルスセンサー、1軸加速度計、1軸ジャイロのセンサー構成からなるシステムの推定軌跡の説明図である。
【図１５】6自由度の基本システムの位置、方位角に関する２σ理論値(精度)の説明図である。
【図１６】Acc3 Gyro2システムの位置、方位角に関する２σ理論値(精度)の説明図である。
【図１７】Acc3 Gyro1システムの位置、方位角に関する２σ理論値(精度)の説明図である。
【図１８】Acc1 Gyro1システムの位置、方位角に関する２σ理論値(精度)の説明図である。
【図１９】Acc0 Gyro1システムの位置、方位角に関する２σ理論値(精度)の説明図である。
【図２０】センサー構成ごとの精度性能とコストをまとめた図表である。
【符号の説明】
【００７４】
１０ センサーボード
２０ SCコントローラ
３０ 自律航法計算部
４０ カルマンフィルタ補正部
５０ GPS受信機
６０ 演算部
【特許請求の範囲】
【請求項１】
車速パルスセンサー、加速度計や角速度計（ジャイロスコープ）およびGPS受信機を含む複数のセンサーを複合した車載ナビゲーションシステムのナビゲーション方法において、
いくつかのセンサーの組み合わせで構成される複数のシステムの中から所定のセンサー構成システムを選択し、
前記選択されたセンサー構成システムと全センサーを備えた基本システムとの差異を補完し、
前記基本システムの制御アルゴリズムに基づいてカルマンフィルタ処理を行って車両の位置、速度及び姿勢の推定値を得る、
ことを特徴とするナビゲーション方法。
【請求項２】
前記センサー構成システムは少なくとも、
GPS受信機、車速パルスセンサー、加速度計３軸、ジャイロスコープ３軸、
GPS受信機、車速パルスセンサー、加速度計３軸、ジャイロスコープ２軸、
GPS受信機、車速パルスセンサー、加速度計３軸、ジャイロスコープ１軸、
GPS受信機、車速パルスセンサー、加速度計２軸、ジャイロスコープ１軸、
GPS受信機、車速パルスセンサー、加速度計１軸、ジャイロスコープ１軸、
GPS受信機、車速パルスセンサー、加速度計不使用(０軸)、ジャイロスコープ１軸、
の内のいずれかであり、前記基本システムはGPS受信機、車速パルスセンサー、加速度計３軸、ジャイロスコープ３軸を備えた６自由度のシステムである、
ことを特徴とする請求項１記載のナビゲーション方法。
【請求項３】
前記選択されたセンサー構成システムを、システム設計時に予め設定し、あるいは、センサーから得られる信号をもとに、自動的に判別する、
ことを特徴とする請求項１または２記載のナビゲーション方法。
【請求項４】
前記差異を補完する補完手段を設け、該補完手段は、前記選択されたセンサー構成システムにおける基本システムから欠けているセンサーの出力値として、０またはホワイトノイズを採用する、
ことを特徴とする請求項１又は２記載のナビゲーション方法。
【請求項５】
前記差異を補完する補完手段を設け、該補完手段は、前記選択されたセンサー構成システムにおける基本システムから欠けているセンサーの出力値として、定数を採用する、
ことを特徴とする請求項１又は２記載のナビゲーション方法。
【請求項６】
前記差異を補完する補完手段を設け、該補完手段は、前記選択されたセンサー構成システムにおける基本システムから欠けているセンサーの出力値として、他センサーの出力値を変数とする関数の関数値を採用する、
ことを特徴とする請求項１又は２記載のナビゲーション方法。
【請求項７】
前記差異を補完する補完手段を設け、該補完手段は、前記選択されたセンサー構成システムがロール角を推定するセンサーを持たないとき、ロール角推定量が発散するのをふせぐために、ロール角が0の測定量をカルマンフィルタに入力する、
ことを特徴とする請求項１又は２記載のナビゲーション方法。
【請求項８】
車速パルスセンサー、加速度計や角速度計（ジャイロスコープ）およびGPS受信機を含む複数のセンサーを複合した車載ナビゲーションシステムにおいて、
いくつかのセンサーの組み合わせで構成される複数のシステムの中から選択した所定のセンサー構成システムと全センサーを備えた基本システムとの差異を補完する補完部、
補完された値及びシステムのセンサー出力信号を用いて自律航法計算する自律航法計算部、
前記基本システムの制御アルゴリズムに基づいて自律航法計算結果及びGPS測定結果を用いてカルマンフィルタ処理を行い、車両の位置、速度及び姿勢の推定値を得る処理部、
を備えたことを特徴とする車載ナビゲーションシステム。
【請求項９】
前記センサー構成システムは少なくとも、
GPS受信機、車速パルスセンサー、加速度計３軸、ジャイロスコープ３軸、
GPS受信機、車速パルスセンサー、加速度計３軸、ジャイロスコープ２軸、
GPS受信機、車速パルスセンサー、加速度計３軸、ジャイロスコープ１軸、
GPS受信機、車速パルスセンサー、加速度計２軸、ジャイロスコープ１軸、
GPS受信機、車速パルスセンサー、加速度計１軸、ジャイロスコープ１軸、
GPS受信機、車速パルスセンサー、加速度計不使用(０軸)、ジャイロスコープ１軸、
の内のいずれかであり、前記基本システムはGPS受信機、車速パルスセンサー、加速度計３軸、ジャイロスコープ３軸を備えた６自由度のシステムである、
ことを特徴とする請求項８記載の車載ナビゲーションシステム。
【請求項１０】
前記選択されたセンサー構成システムをシステム設計時に予め設定する手段、あるいは、センサーから得られる信号をもとに選択されたセンサー構成システムを自動的に判別する手段、
を備えたことを特徴とする請求項８または９記載の車載ナビゲーションシステム。
【請求項１１】
前記補完手段は、前記選択されたセンサー構成システムにおける基本システムから欠けているセンサーの出力値として、０またはホワイトノイズを採用する、
ことを特徴とする請求項８又は９記載の車載ナビゲーションシステム。
【請求項１２】
前記補完手段は、前記選択されたセンサー構成システムにおける基本システムから欠けているセンサーの出力値として、定数を採用する、
ことを特徴とする請求項８又は９記載のナビゲーションシステム。
【請求項１３】
前記補完手段は、前記選択されたセンサー構成システムにおける基本システムから欠けているセンサーの出力値として、他センサーの出力値を変数とする関数の関数値を採用する、
ことを特徴とする請求項８又は９記載の車載ナビゲーションシステム。
【請求項１４】
前記補完手段は前記選択されたセンサー構成システムがロール角を推定するセンサーを持たないとき、ロール角推定量が発散するのをふせぐために、ロール角が0の測定量をカルマンフィルタに入力する、
ことを特徴とする請求項８又は９記載の車載ナビゲーションシステム。
【請求項１】
車速パルスセンサー、加速度計や角速度計（ジャイロスコープ）およびGPS受信機を含む複数のセンサーを複合した車載ナビゲーションシステムのナビゲーション方法において、
いくつかのセンサーの組み合わせで構成される複数のシステムの中から所定のセンサー構成システムを選択し、
前記選択されたセンサー構成システムと全センサーを備えた基本システムとの差異を補完し、
前記基本システムの制御アルゴリズムに基づいてカルマンフィルタ処理を行って車両の位置、速度及び姿勢の推定値を得る、
ことを特徴とするナビゲーション方法。
【請求項２】
前記センサー構成システムは少なくとも、
GPS受信機、車速パルスセンサー、加速度計３軸、ジャイロスコープ３軸、
GPS受信機、車速パルスセンサー、加速度計３軸、ジャイロスコープ２軸、
GPS受信機、車速パルスセンサー、加速度計３軸、ジャイロスコープ１軸、
GPS受信機、車速パルスセンサー、加速度計２軸、ジャイロスコープ１軸、
GPS受信機、車速パルスセンサー、加速度計１軸、ジャイロスコープ１軸、
GPS受信機、車速パルスセンサー、加速度計不使用(０軸)、ジャイロスコープ１軸、
の内のいずれかであり、前記基本システムはGPS受信機、車速パルスセンサー、加速度計３軸、ジャイロスコープ３軸を備えた６自由度のシステムである、
ことを特徴とする請求項１記載のナビゲーション方法。
【請求項３】
前記選択されたセンサー構成システムを、システム設計時に予め設定し、あるいは、センサーから得られる信号をもとに、自動的に判別する、
ことを特徴とする請求項１または２記載のナビゲーション方法。
【請求項４】
前記差異を補完する補完手段を設け、該補完手段は、前記選択されたセンサー構成システムにおける基本システムから欠けているセンサーの出力値として、０またはホワイトノイズを採用する、
ことを特徴とする請求項１又は２記載のナビゲーション方法。
【請求項５】
前記差異を補完する補完手段を設け、該補完手段は、前記選択されたセンサー構成システムにおける基本システムから欠けているセンサーの出力値として、定数を採用する、
ことを特徴とする請求項１又は２記載のナビゲーション方法。
【請求項６】
前記差異を補完する補完手段を設け、該補完手段は、前記選択されたセンサー構成システムにおける基本システムから欠けているセンサーの出力値として、他センサーの出力値を変数とする関数の関数値を採用する、
ことを特徴とする請求項１又は２記載のナビゲーション方法。
【請求項７】
前記差異を補完する補完手段を設け、該補完手段は、前記選択されたセンサー構成システムがロール角を推定するセンサーを持たないとき、ロール角推定量が発散するのをふせぐために、ロール角が0の測定量をカルマンフィルタに入力する、
ことを特徴とする請求項１又は２記載のナビゲーション方法。
【請求項８】
車速パルスセンサー、加速度計や角速度計（ジャイロスコープ）およびGPS受信機を含む複数のセンサーを複合した車載ナビゲーションシステムにおいて、
いくつかのセンサーの組み合わせで構成される複数のシステムの中から選択した所定のセンサー構成システムと全センサーを備えた基本システムとの差異を補完する補完部、
補完された値及びシステムのセンサー出力信号を用いて自律航法計算する自律航法計算部、
前記基本システムの制御アルゴリズムに基づいて自律航法計算結果及びGPS測定結果を用いてカルマンフィルタ処理を行い、車両の位置、速度及び姿勢の推定値を得る処理部、
を備えたことを特徴とする車載ナビゲーションシステム。
【請求項９】
前記センサー構成システムは少なくとも、
GPS受信機、車速パルスセンサー、加速度計３軸、ジャイロスコープ３軸、
GPS受信機、車速パルスセンサー、加速度計３軸、ジャイロスコープ２軸、
GPS受信機、車速パルスセンサー、加速度計３軸、ジャイロスコープ１軸、
GPS受信機、車速パルスセンサー、加速度計２軸、ジャイロスコープ１軸、
GPS受信機、車速パルスセンサー、加速度計１軸、ジャイロスコープ１軸、
GPS受信機、車速パルスセンサー、加速度計不使用(０軸)、ジャイロスコープ１軸、
の内のいずれかであり、前記基本システムはGPS受信機、車速パルスセンサー、加速度計３軸、ジャイロスコープ３軸を備えた６自由度のシステムである、
ことを特徴とする請求項８記載の車載ナビゲーションシステム。
【請求項１０】
前記選択されたセンサー構成システムをシステム設計時に予め設定する手段、あるいは、センサーから得られる信号をもとに選択されたセンサー構成システムを自動的に判別する手段、
を備えたことを特徴とする請求項８または９記載の車載ナビゲーションシステム。
【請求項１１】
前記補完手段は、前記選択されたセンサー構成システムにおける基本システムから欠けているセンサーの出力値として、０またはホワイトノイズを採用する、
ことを特徴とする請求項８又は９記載の車載ナビゲーションシステム。
【請求項１２】
前記補完手段は、前記選択されたセンサー構成システムにおける基本システムから欠けているセンサーの出力値として、定数を採用する、
ことを特徴とする請求項８又は９記載のナビゲーションシステム。
【請求項１３】
前記補完手段は、前記選択されたセンサー構成システムにおける基本システムから欠けているセンサーの出力値として、他センサーの出力値を変数とする関数の関数値を採用する、
ことを特徴とする請求項８又は９記載の車載ナビゲーションシステム。
【請求項１４】
前記補完手段は前記選択されたセンサー構成システムがロール角を推定するセンサーを持たないとき、ロール角推定量が発散するのをふせぐために、ロール角が0の測定量をカルマンフィルタに入力する、
ことを特徴とする請求項８又は９記載の車載ナビゲーションシステム。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図１６】
【図１７】
【図１８】
【図１９】
【図２０】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図１６】
【図１７】
【図１８】
【図１９】
【図２０】
【公開番号】特開２００９−１９８２７９（Ｐ２００９−１９８２７９Ａ）
【公開日】平成２１年９月３日（２００９．９．３）
【国際特許分類】
【出願番号】特願２００８−３９５６３（Ｐ２００８−３９５６３）
【出願日】平成２０年２月２１日（２００８．２．２１）
【出願人】（０００１０１７３２）アルパイン株式会社 (2,424)
【Ｆターム（参考）】
【公開日】平成２１年９月３日（２００９．９．３）
【国際特許分類】
【出願日】平成２０年２月２１日（２００８．２．２１）
【出願人】（０００１０１７３２）アルパイン株式会社 (2,424)
【Ｆターム（参考）】
[ Back to top ]