ナビゲーションシステムの位置検出装置および位置検出方法
【目的】センサー数が少ないシステムであっても、位置精度や方位精度を向上できるようにする「ナビゲーションシステムの位置検出装置および位置検出方法」を提供することである。
【構成】車速センサーと加速度センサーと角速度センサーとを含むセンサー部、これらセンサーから出力される信号を用いて所定の周期で車両の現在位置、車両速度、車体の姿勢角を含む状態量を計算する自律航法計算部を備え、車両の位置を検出するナビゲーションシステムの位置検出装置である。自律航法計算部は、車速センサーから出力される信号を用いて前回の状態量計算時刻から今回の状態量計算時刻までに移動した移動量を計算する移動量算出部、車体の姿勢変化量を計算する変化量検出部、姿勢変化量を用いて移動量を車体の前後方向、車体の左右方向、車体の上下方向の方向成分に分解する移動量分解部、各方向成分を用いて今回の状態量計算時刻における車両の位置を計算する状態量計算部を備えている。
【構成】車速センサーと加速度センサーと角速度センサーとを含むセンサー部、これらセンサーから出力される信号を用いて所定の周期で車両の現在位置、車両速度、車体の姿勢角を含む状態量を計算する自律航法計算部を備え、車両の位置を検出するナビゲーションシステムの位置検出装置である。自律航法計算部は、車速センサーから出力される信号を用いて前回の状態量計算時刻から今回の状態量計算時刻までに移動した移動量を計算する移動量算出部、車体の姿勢変化量を計算する変化量検出部、姿勢変化量を用いて移動量を車体の前後方向、車体の左右方向、車体の上下方向の方向成分に分解する移動量分解部、各方向成分を用いて今回の状態量計算時刻における車両の位置を計算する状態量計算部を備えている。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明はナビゲーションシステムの位置検出装置および位置検出方法に関わり、特に、自律航法計算部が計算する状態量（車両の位置、車両速度及びセンサーあるいは車体の姿勢を含む状態量）を所定周期で補正し、該補正された状態量を用いて自律航法計算部が状態量を計算して出力するナビゲーションシステムの位置検出装置および位置検出方法に関する。
【背景技術】
【０００２】
自律航法システムは、車両の距離センサーである車速パルスセンサーや、加速度計と角速度計(ジャイロスコープ)などの慣性センサーから成り、動体である車両の位置、速度、及びセンサー姿勢あるいは車体姿勢の推定のために広く使用されている。
【０００３】
慣性航法システム（Inertial Navigation System：INS）は、通常３軸加速度計と３軸ジャイロスコープからなり、動体の６自由度、即ち３次元の並進運動と３次元の回転運動を同時に推定するために使用される。例えば、航空・宇宙機のナビゲーションは、かかる慣性航法システムを用いて、動体の６自由度、即ち３次元の並進運動と３次元の回転運動を同時に推定する。車両ナビゲーションにおいては車速パルスセンサーを使用することで、コストの高い慣性センサーを削減する場合が多い。
【０００４】
自律航法は外部信号に頼らずにセンサー出力に基づいて推定位置の更新を行うことができる。しかし、センサー出力に含まれるノイズなどのエラーが時間の経過とともに積分計算に伴って蓄積するため、長時間は使用できない。そこで、衛星信号の届く範囲では有限誤差の位置推定を常時与える全地球測位システム（Global Positioning System: GPS受信機）と自律航法システムを、カルマンフィルタなどの最適確率アルゴリズムで結合することにより、両システムの性能上の問題を補間・解決する手法が考案された(特許文献１)。このようなシステムは複合ナビゲーションシステム（Integrated Navigation System）と呼ばれ、現在では車両ナビゲーションから航空宇宙ナビゲーションまで、幅広く利用されている。
【０００５】
カルマンフィルタ処理は所定時刻における所定状態量の予測値(推定値)と観測値との誤差を修正しながら、各時刻における最適な推定値を逐次求める方法である。カルマンフィルタ処理においては、予め、ある値を推定するための算出式を設定し、この算出式を用いて観測値Z(t)が得られる時刻ｎまで推定を繰り返す。時刻nで観測値が取得できれば、該観測値を用いて時刻ｎでの前記推定値について、確率論的に定義された誤差を最小化させるような推定値補正計算を行う。
図１４はカルマンフィルタ処理の概要説明図である。カルマンフィルタにおいては、図１４に示すように、信号生成過程１と観測過程２に分けられる。図において、線形システムF(前記算出式により同定されるシステム)があり、その線形システムの状態をX(t)とするとき、観測行列Hを介してX(t)の一部が観測できる場合、カルマンフィルタはX(t)の最適な推定値を与える。ここで、ｗは信号生成過程にて発生する雑音であり、ｖは観測過程にて発生する雑音である。カルマンフィルタは、入力を観測値Z(t)としてカルマン処理を所定周期で繰り返し実行することにより、最適推定値X(t)を求める。
【０００６】
カルマンフィルタ処理におけるシステムモデルの状態式は次式
【数１】
により表現される。システム状態変数δXは各軸速度、各軸位置、センサー姿勢等の状態量を含み後述するように
【数２】
と表現できる。観測値Z(t)が得られる周期より短い周期T毎に(1)式の演算が実行され、得られた値が状態量として出力される。そして、観測値Z(t)が得られれば、最適推定値X(t)をカルマンフィルタ処理により演算し、以後、該最適推定値を用いて(1)式の演算を実行する。
【０００７】
カルマンフィルタの観測式は
【数３】
と表現される。(1）式により得られるシステム状態変数は時間の経過と共に誤差が累積してゆく。そこで、観測値δZ(k)が得られる毎に(3)式を用いてシステム状態変数δX(k)の最適推定値を求め、以後、該最適推定値を用いて(1)式により、次の観測値が得られるまでシステム状態量δX(k)を計算して出力する。
【０００８】
すなわち、カルマンフィルタは、観測値Z(t)(=δZ(t))が得られる毎に（観測値Z(t)の入力周期で）、以下の(4)式を実行することにより最適推定値X(t|t)(=δX(t|t))を求める。ただし、時刻ｊまでの情報に基づく時刻iでのAの推定値をＡ(i|j)と表記するものとし、次式においてX(t｜t−1)は事前推定値、K(t)はカルマンゲインである。
【数４】
(4)式の右辺{ }内の差にカルマンゲインを乗算して最適推定値X(t|t)を計算する。事前推定値およびカルマンゲインはそれぞれ
【数５】
と表現でき、事前推定値X(t｜t−1)は、Z(t)の入力周期より短い周期で(1)式に相当する(5a)式により更新される。
【０００９】
また、(5b)式におけるＰは状態量Xの誤差共分散行列であり、P(t|t-1)は誤差共分散の予測値、P(t-1|t-1)は誤差共分散であり、それぞれ
【数６】
である。Ｖは観測過程で発生する雑音ｖの分散、Ｗは信号過程で発生する雑音wの分散である。添字の（・）Ｔ は転置行列を意味し、(・)−１は逆行列を意味する。また、Ｉは単位行列である。さらに、ＶとＷは平均０の白色ガウス雑音であり、互いに無相関である。上記のようなカルマンフィルタにおいて、状態量Ｘと誤差共分散Pの初期値に適当な誤差を与えてやり、新しい観測が行われる度に(4)式により最適推定値を求め、以後、(5a)式により、状態量Ｘを計算することにより該状態量の精度を向上することができる。
【００１０】
図１５は本願出願人により出願済みの車載ナビゲーションシステムの位置検出部の構成図である(特許文献２)。自律系の入力信号としては、センサーボード１０上の慣性センサーから得られる信号と、別のケーブルを通じて車体から取り込まれる車速パルスがある。慣性センサーとしては加速度計(加速度３軸)１０a、ジャイロスコープ（角速度３軸)１０bを採用し、車速パルスを発生するセンサーとしては車両が所定距離移動する毎に１個のパルスを発生する車速センサー１１を採用する。図１６(Ａ)はセンサーボード１０に固定したセンサー座標系Xs-Ys-Zsを示している。なお、以下の数式の中でセンサー（ボード）座標系に関する表現であることを、添え字の”ｓ”によって表現することに注意すべきである。図１６(B)は、センサーボード１０において、加速度計１０ａの3軸（Acc x, Acc y, Acc z）およびジャイロスコープ１０ｂの3軸（Gyro x, Gyro y, Gyro z）から成る、即ち6自由度の基本システムのセンサー構成を示したものである。３軸加速度計Acc x〜Acc zは、センサー座標系の三つの座標方向（ｘ，ｙ，ｚ）における加速度を検出し、また３軸ジャイロスコープGyro x〜Gyro zは、センサー座標系の三つの座標軸（ｘ，ｙ，ｚ）周りの角速度P,Q,Rを検出する。
なお、図１７（A）は、車両に固定した座標系Xb-Yb-Zbを示しており、以下の数式の中で車両固定座標系に関する表現であることを、添え字の”b”によって表現する。又、図１７(B)は、ある緯度、経度における、地表固定座標系North-East-Down（NED座標系）を示しおり、以下の数式の中でNED座標系に関する表現であることを、添え字の”n”によって表現する。
【００１１】
自律航法計算部１２は６自由度の基本システムのアルゴリズムを用いて、車両の現在の位置、速度及び姿勢を含む車両状態量を、所定の計算式に従って高周波数で、例えば２５Hzで更新して出力する。
GPS受信機１３は、複数の人工衛星から距離と距離変化率に関する信号を受信することで、車両のアンテナの位置(緯度Ｎ、経度Ｅ、高さＤ)と車両速度(北方向速度ｖＮ、東方向速度ｖＥ、上下方向速度ｖＤ)の測定値を１Hzでカルマンフィルタ補正部14に入力する。
カルマンフィルタ演算部１４は車両の現在の位置、速度及び姿勢を含む車両状態量の補正を行う。例えば、カルマンフィルタ演算部１４は５Hzの速度で、車速センサー１１から得られる速度(走行時)あるいはジャイロ１０ｂから出力される角速度オフセット(静止時)を観測値として用いて状態量補正処理を行って得られた補正状態量を自律航法計算部１２に入力する。又、カルマンフィルタ補正部１４は１Hzの速度でGPS受信機１３から得られる位置・速度（Ｎ，Ｅ，Ｄ，ｖＮ，ｖＥ，ｖＤ)を観測値として用いて状態量補正処理を行って得られた補正状態量を自律航法計算部１２に入力する。
【００１２】
図１８は図１５の位置検出部の全体の処理フローである。
はじめに、自律航法計算部１２に３次元車両位置Ｎ、Ｅ、Ｄ、車両速度Ｖx、ピッチ角θ、車両取り付けピッチ角Ａ、ヨー角Ｙ、車両取り付けヨー角Ａ2、ジャイロのオフセットωＯＦ、加速度センサーのオフセットαＯＦの初期値を設定する（ステップ１０１）。ジャイロ１０aおよび加速度センサー１０bは、側面から見たとき、車両方向と平行して車両に取り付けられるのが理想であるが、図１９（Ａ）に示すように取り付け誤差があり、センサー方向は車両方向に角度Ａ（取り付けピッチ角）を成して取り付けられる。なお、水平方向とセンサー方向の角度θをピッチ角といい、ピッチ角は傾斜角と取り付けピッチ角の和である。また、ジャイロ１０aおよび加速度センサー１０bは、平面に投影したとき、車両方向と一致して車両に取り付けられるのが理想であるが、取り付け誤差があり、図１９（Ｂ）に示すように、センサー方向は車両方向に角度Ａ2（取り付けヨー角）を成して取り付けられる。なお、北方向とセンサー方向の角度Ｙをヨー角といい、ヨー角Ｙは車両方向と取り付けヨー角の和である。
【００１３】
以後、自律航法計算部１２は、ジャイロ１０a、加速度センサー１０b、車速センサー１１の出力を取り込み（ステップ１０２）、第１周期（２５Hz周期）毎に自律航法計算アルゴリズムにしたがって(２)式に示す各軸速度、各軸位置、センサー姿勢等の状態量を計算して出力する(ステップ１０３)。
ついで、第２周期(５Hz周期)になったかチェックし(ステップ１０４)、第２周期になっていなければ、ステップ１０２以降の処理を繰り返す。
第２周期になっていれば、車両速度Ｖxが零である状態が2秒以上続いていたかどうかによって停車判定を行う（ステップ１０５）。
停車中でなければ、第３周期(１Hz周期=GPS測位周期)になっているかチェックし（ステップ１０６）、第３周期でなければカルマンフィルタ補正部１４は、車速センサー１１の出力より計算した車両前後方向の速度(観測値)Ｖxと自律航法計算部１２が計算した車両速度を用いてカルマンフィルタ処理により車両状態量を補正し、補正状態量を自律航法計算部１２に入力する（ステップ１０７）。このステップ１０７では、第１のカルマンフィルタの観測行列H1を用いた第１補正処理が行われる。
【００１４】
ステップ１０６において、第３周期であればカルマンフィルタ演算部１４はGPSレシーバ１４が出力する３次元の車両位置Ｎ，Ｅ，Ｄと３次元の車両速度ｖＮ，ｖＥ，ｖＤを観測値として用いて車両状態量を補正し、補正状態量を自律航法計算部１２に入力する(ステップ１０８)。このステップ１０８では第２のカルマンフィルタの観測行列H2を用いた第２補正処理が行われる。
ステップ１０５において、停車中であれば、第３周期(１Hz周期=GPS測位周期)になっているかチェックし（ステップ１０９）、第３周期でなければカルマンフィルタ演算部１４は前記ステップ１０７の補正処理を行うと共に、ジャイロの角速度出力信号(観測値)と自律航法計算部１２で計算した角速度信号オフセットにより車両状態量を補正し、補正状態量を自律航法計算部１２に入力する（ステップ１１０）。このステップ１１０では第３のカルマンフィルタの観測行列H3を用いた第３補正処理が行われる。
ステップ１０９において、第３周期であれば、カルマンフィルタ演算部１４は前記ステップ１０８の補正処理を行うと共に、ジャイロの角速度出力信号と自律航法計算部１２で計算した角速度信号オフセットとの差に基づいて角速度オフセット補正を行う（ステップ１１１）。このステップ１１１では第４のカルマンフィルタの観測行列H4を用いた第４補正処理が行われる（ステップ１１１）。
【００１５】
以上は３軸の加速度計、３軸のジャイロスコープを有する６自由度の基本システムの場合であるが、センサー数が多くなって高価となる。このため、図１５の位置検出部ではセンサー数を減らした構成に対応できるようになっている。図２０、図２１は種々のセンサー構成を示す説明図であり、図２０（Ａ）、（Ｂ）はセンサー座標系および６自由度の基本システムの説明図であり、図１６(Ａ)，（Ｂ）に示したものと同一である。
図２０（Ｃ）は、（Ａ）のセンサーボード１０において、加速度計3軸、ジャイロスコープ２軸から成るシステムのセンサー構成の説明図である。なお、ジャイロスコープが２軸しか検出できないときのシステムのジャイロスコープはGyro xおよび Gyro zである。
図２０（Ｄ）は、（Ａ）のセンサーボード１０において、加速度計3軸、ジャイロスコープ1軸から成るシステムのセンサー構成を示したものである。ジャイロスコープが１軸しか検出できないときのシステムのジャイロスコープはGyro zである。
図２１（Ａ）は、図２０（Ａ）のセンサーボード１０において、加速度計２軸、ジャイロスコープ1軸から成るシステムのセンサー構成の説明図であり、加速度計が２軸しか検出できないとき、システムの加速度計はAcc x, Acc yであり、ジャイロスコープが１軸しか検出できないときのシステムのジャイロスコープはGyro zである。
図２１（Ｂ）は図２０（Ａ）のセンサーボード１０において、加速度計１軸、ジャイロスコープ１軸ら成るシステムのセンサー構成の説明図であり、加速度計が１軸しか検出できないときのシステムの加速度計はAcc xであり、またジャイロスコープが１軸しか検出できないとき、システムのジャイロスコープはGyro zである。
図２１（Ｃ）は、図２０（Ａ）のセンサーボード１０において、加速度計０軸（不使用）、ジャイロスコープ１軸から成るシステムのセンサー構成の説明図であり、ジャイロスコープが１軸しか検出できないときのシステムのジャイロスコープはGyro zである。
【００１６】
図１５の自律航法計算部１２は常に同じ６自由度の基本システムのアルゴリズムを用いて、車両の現在の位置、速度及び姿勢を含む車両状態量を、高周波数で更新する。このため、センサー・コンフィギュレーション・コントローラ（SCコントローラ）１５は、車載ナビゲーションシステムの実際のセンサー構成を識別し、基本システムから欠けているセンサーの出力値を推定して自律航法計算部１２に入力する。たとえば、欠けているセンサーの出力値として、定数（０を含む）あるいはホワイトノイズあるいは他センサーの出力値を変数とする関数の関数値を採用する。
【００１７】
しかし、センサー数が減少すると精度が劣化する問題がある。図２２は、センサー構成と精度の説明図であり、（１）ＧＰＳ受信できない地上１０階建ての螺旋駐車場において最上階で道路と平行して停車したときの道路方位と推定車両方位との方位誤差（ｄｅｇ）と、（２）地上の入り口から最上階まで行ってから地上の出口に戻ってきたときの推定高度の高度差（ｍ）を示している。加速度計3軸、ジャイロスコープ２軸から成るシステムでは、方位誤差が３度、高度さが５ｍであるが、加速度計１軸、ジャイロスコープ１軸から成るシステムでは、方位誤差が２０度（図７Ｂ）参照）、高度差が−１１．６ｍ（図８の曲線Ｂ参照）と非常に大きくなってしまう。
【特許文献１】特許３４７３１１７号
【特許文献２】特願２００８−０３９５６３
【発明の開示】
【発明が解決しようとする課題】
【００１８】
以上から、本発明の目的は、センサー数が少ないシステムであっても、例えば加速度計１軸、ジャイロスコープ１軸のシステムであっても、位置精度や方位精度を向上できるようにすることである。
【課題を解決するための手段】
【００１９】
本発明は車両の位置を検出するナビゲーションシステムの位置検出装置および位置検出方法である。
・位置検出装置
本発明の位置検出装置は、車両の速度を検出する車速センサーと、車両の加速度を検出する加速度センサーと、車両の角速度を検出する角速度センサーとを含むセンサー部、これらセンサーから出力される信号を用いて所定の周期で車両の現在位置、車両速度、車体の姿勢角を含む状態量を計算する自律航法計算部を備え、前記自律航法計算部は、前記車速センサーから出力される信号を用いて前回の状態量計算時刻から今回の状態量計算時刻までに移動した移動量を計算する移動量算出部、車体の姿勢変化量を計算する変化量検出部、前記姿勢変化量を用いて前記移動量を車体の前後方向、車体の左右方向、車体の上下方向の方向成分に分解する移動量分解部、前記各方向成分を用いて今回の状態量計算時刻における車両の位置を計算する状態量計算部を備えている。
前記変化量検出部は、前記車体の姿勢変化量として、（１）車体のピッチ角の変化量と車体のヨー角の変化量、あるいは（２）車体のヨー角の変化量を検出する。この際、変化量検出部は、前記状態量計算部が計算した車体の姿勢角を用いて前記車体のピッチ角の変化量と車体のヨー角の変化量を計算する。
【００２０】
車体の姿勢変化量として車体のピッチ角変化量と車体のヨー角変化量が検出され、これらをそれぞれ
とし、移動量をNｐLとすれば、前記移動量分解部は該移動量を次式
【数７】
により車体の前後方向、車体の左右方向、車体の上下方向の方向成分に分解する。
また、車体の姿勢変化量として車体のヨー角変化量が検出され、これを
とし、移動量をNｐLとすれば、前記移動量分解部は次式
【数８】
により前記移動量を車体の前後方向、車体の左右方向、車体の上下方向の方向成分に分解する。
【００２１】
本発明の位置検出装置は更に、前記車速センサーの出力信号より測定した車両の移動速度と前記自律航法計算部が計算した前記状態量とを用いて、該自律航法計算部が状態量を計算する前記周期より長い第２の周期で状態量補正処理を実行して該自律航法計算部が計算する状態量を補正する状態量補正部、例えばカルマンフィルタを備え、該状態量補正部は、前記姿勢変化量を用いて前記測定した移動速度を車体の前後方向、車体の左右方向、車体の上下方向の方向成分に分解する速度分解部、前記各方向の速度成分と前記自律航法計算部が計算した前記状態量を用いて補正状態量を計算する補正状態量計算部を備えている。
前記姿勢変化量が車体のピッチ角の変化量と車体のヨー角の変化量であり、該車体のピッチ角変化量と車体のヨー角変化量をそれぞれ
とし、前記測定した移動速度をVとすれば、前記速度分解部は次式
【数９】
により該移動速度を車体の前後方向、車体の左右方向、車体の上下方向の方向成分に分解する。
また、前記速度分解部は、前記姿勢変化量が車体のヨー角の変化量であり、該車体のヨー角変化量を
とし、前記測定した移動速度をVとすれば、前記速度分解部は次式
【数１０】
により該移動速度を車体の前後方向、車体の左右方向、車体の上下方向の方向成分に分解する。
【００２２】
・位置検出方法
本発明の位置検出方法は、車両の速度を検出する車速センサーと、車両の加速度を検出する加速度センサーと、車両の角速度を検出する角速度センサーから出力される信号を用いて、所定の周期で車両の現在位置、車両速度、車体の姿勢角を含む状態量を自律航法により計算するナビゲーションシステムの位置検出方法であり、前記車速センサーから出力される信号を用いて前回の状態量計算時刻から今回の状態量計算時刻までに移動した移動量を計算する第１ステップ、車体の姿勢変化量を計算する第２ステップ、前記姿勢変化量を用いて前記移動量を車体の前後方向、車体の左右方向、車体の上下方向の方向成分に分解する第３ステップ、前記各方向成分を用いて今回の状態量計算時刻における車両の位置を計算する第４ステップを備えている。
第２ステップは、上記位置検出装置の変化量検出部が行う処理であり、前記車体の姿勢変化量として、（１）車体のピッチ角の変化量と車体のヨー角の変化量、あるいは（２）車体のヨー角の変化量を検出する。
第３ステップは、上記位置検出装置の移動量分解部が行う処理であり、(7)式あるいは(8)式により移動量を車体の前後方向、車体の左右方向、車体の上下方向の方向成分に分解する。
本発明の位置検出方法は更に、前記自律航法により状態量を計算する前記周期より長い第２の周期で、前記車速センサーの出力信号より車両の移動速度を測定する第５ステップ、
該測定した移動速度と前記自律航法により計算した状態量とを用いて状態量補正処理を実行し、該自律航法により計算する状態量を補正する第６ステップを備え、該第６ステップは、前記姿勢変化量を用いて前記測定した移動速度を車体の前後方向、車体の左右方向、車体の上下方向の方向成分に分解する分解ステップ、前記各方向の速度成分と前記自律航法計算部が計算した前記状態量を用いて補正状態量を計算する計算ステップを備えている。
前記分解ステップは、上記位置検出装置の速度分解部が行う処理であり、(9)式あるいは(10)式により測定した移動速度を車体の前後方向、車体の左右方向、車体の上下方向の方向成分に分解する。
【発明の効果】
【００２３】
本発明によれば、車両走行中に車体の姿勢角、例えばピッチ角やヨー角が変化する場合、これらピッチ角やヨー角の変化量を考慮して車両の移動距離を車体の前後方向、車体の左右方向、車体の上下方向の成分に分解したから、移動距離ベクトルの精度を向上でき、車両位置の計算精度を改善することができる。
また、本発明によれば、車両走行中にピッチ角やヨー角が変化する場合、これらピッチ角やヨー角の変化量を考慮して車両移動速度を車体の前後方向、車体の左右方向、車体の上下方向の成分vｘｂ-obs、vｙｂ-obs、vｚｂ-obsに分解したから、移動距離ベクトルの精度を向上することができ、速度、センサー姿勢角、センサー取付角精度を改善することができる。この結果、高度精度を向上することができる。
また、本発明によれば、少ないセンサー構成で位置および方位精度を向上できるため、ナビゲーションシステムのコストを抑えることが出来る。
さらに、本発明によれば、相対高度精度を向上することができるため、一般道と高速道路の上下分岐を判定する手段に応用することができる。
【発明を実施するための最良の形態】
【００２４】
（Ａ）本発明の原理
(ａ)従来の問題点の解析
図１５の従来の自律航法計算部１２は、自律航法計算アルゴリズムにしたがって各軸速度、各軸位置、センサー姿勢等の状態量を計算する。この従来の自律航法計算アルゴリズムでは、以下の位置方程式
【数１１】
によりＮＥＤ座標系における位置Ｐｎを計算する。但し、(11)式において
【数１２】
であり、Ｔｎｂは車両固定座標系からＮＥＤ座標系に変換する変換マトリックスである。又、(12a)は位置ベクトル、(12b)は車両固定座標系における車両の移動距離ベクトルであり、 (12b)式において、Npはサンプル時間当たりのパルス数、Ｌはパルス間距離である。
【００２５】
しかし、(12b)式は車両移動距離ベクトルを正しく表現していない。というのは、車両が勾配走行中においてピッチ角θを増減して走行している場合には、速度ベクトルの成分は車両前後方向(車両固定座標系のｘ軸方向)だけでなく、上下方向(車両固定座標系のｚ軸方向)にも発生する。また、車両がカーブで回転走行している場合やタイヤがスリップしている場合にはヨー角Ｙが増減する。かかるヨー角Ｙが増減して走行している場合には、速度ベクトルの成分は車両前後方向(車両固定座標系のｘ軸方向)だけでなく、左右方向(車両固定座標系のｙ軸方向)にも発生する。したがって、ピッチ角θおよびヨー角Ｙが増減して走行している場合には、速度ベクトルの成分は車両前後方向(車両固定座標系のｘ軸方向)だけでなく、上下方向(車両固定座標系のｚ軸方向)および左右方向(車両固定座標系のｙ軸方向)にも発生する。このため、(12)式は正しく車輌の移動距離ベクトルを表現しておらず、従来の自律航法計算部は精度よく状態量(車両位置、車両方位)を計算できなかったのである。
【００２６】
また、従来のカルマンフィルタ補正部１４は通常走行状態において、５Hzの速度で車速センサー１１から得られる速度を観測値として用いて状態量補正処理を行ない、得られた補正状態量を自律航法計算部１２に入力している。この状態量補正処理において、カルマンフィルタ補正部１４は、速度の観測値(測定値)として次式の測定方程式
【数１３】
を用い、ｖｘｂ(車両固定座標系の前後方向速度成分)は車速パルス間距離をパルス間隔(時間)で割ることで求め、ｖｙｂ(車両固定座標系の左右方向速度成分)= 0、ｖｚｂ(車両固定座標系の上下方向速度成分) = 0としている。なお、
は、ホワイトノイズでモデル化された瞬間車速値の測定ノイズである。
【００２７】
そして、カルマンフィルタ補正部１４は、（13）式で求まる観測値と自律航法計算部１２で計算された計算結果との差分に基づいて補正状態量を計算する。
しかし、前述のように、ピッチ角θおよびヨー角Ｙが変化している場合には、速度ベクトルの成分は車両前後方向だけでなく、上下方向および左右方向にも発生する。このため、従来のカルマンフィルタ補正部１４においては正確な観測値を使用しておらず、算出した補正状態量の信頼性が低く、結果的に自律航法計算部１２は精度よく状態量(車両位置、車両方位)を計算できなかった。
【００２８】
（ｂ）本発明の概略
本発明では、車両がピッチ角θおよびヨー角Ｙが増減して走行している場合、これらの増減率を考慮して（11）式の位置方程式における車両の移動距離ベクトルｓｂの各軸成分を計算し、また、（13）式の測定方程式における速度ベクトル（ｖｘｂ、ｖｙｂ、ｖｚｂ）を計算する。
図１、図２は本発明の車速分解の説明図であり、図１は車速ベクトルＶを車両固定座標系のｘｙ平面(水平面)の速度成分ｖｘｙと上下方向(車両固定座標系のｚ軸方向) の速度成分ｖｚに分解する車速分解説明図、図２はｘｙ平面(水平面)の速度成分ｖｘｙを車両前後方向(車両固定座標系のｘ軸方向) の速度成分ｖｘと左右方向(車両固定座標系のｙ軸方向) の速度成分ｖｙに分解する車速分解説明図である。
【００２９】
図１に示すようにピッチ角θおよびヨー角Ｙを増減して走行している場合において、ピッチ角θの変化量を
とし、速度ベクトルの大きさを｜Ｖ｜とすれば、ｘｙ平面(車体水平面)の速度成分ｖｘｙ，車体上下方向(車両固定座標系のｚ軸方向) における速度成分ｖｚはそれぞれ次式
【数１４】
で表現される。また、図２に示すようにヨー角Ｙの変化量を
とし、速度ベクトルの大きさ(速度)を｜ｖｘｙ｜とすれば、車両前後方向(車両固定座標系のｘ軸方向)における速度成分ｖｘ，左右方向(車両固定座標系のｙ軸方向) における速度成分ｖｙはそれぞれ次式
【数１５】
で表現される。(14)式を(15)式に代入してまとめると、
【数１６】
となる。
【００３０】
以上より、本発明では、(11)式の位置方程式における車両の移動距離ベクトルｓｂとして次式
【数１７】
を使用する。すなわち、移動量をNｐLとすれば、該移動量を車体の前後方向、車体の左右方向、車体の上下方向の方向成分に分解する。また、（13）式の測定方程式として次式
【数１８】
を使用する。すなわち、上式により瞬間車速値(測定した移動速度V)を車体の前後方向成分vｘｂ-obs、車体の左右方向成分vｙｂ-obs、車体の上下方向の方向成分vｚｂ-obsに分解する。
【００３１】
（Ｂ）本発明のナビゲーションシステム
（a）全体の構成
図３は本発明のナビゲーションシステムの全体の構成図であり、図１５と類似した構成を有しており、同一部分には同一符号を付している。
センサーボード１０上には、加速度計１０ａ、ジャイロスコープ１０ｂが設けられている。加速度計１０ａとして１軸〜3軸（Acc x, Acc y, Acc z）の構成が可能であり、また、ジャイロスコープ１０ｂとして１軸〜3軸（Gyro x, Gyro y, Gyro z）の構成が可能である。加速度計３軸およびジャイロスコープ３軸を備えたシステムが６自由度の基本システムでとなる。
車速センサー１１は車両が所定距離走行する毎に１個のパルスを発生する構成になっている。自律航法計算部１２は６自由度の基本システムのアルゴリズム（後述する）を用いて、車両の現在位置、速度及び姿勢を含む車両状態量を、所定の計算式に従って高周波数で、例えば２５Hzで更新して出力する。自律航法計算部１２は車両状態量の更新に際して、ピッチ角の変動率およびヨー角の変動率
をそれぞれ計算し、これらを用いて(17)式により位置方程式における車両の移動距離ベクトルｓｂを計算する。
【００３２】
センサー・コンフィギュレーション・コントローラ（SCコントローラ）１５は車載ナビゲーションシステムの実際のセンサー構成を識別し、基本システムから欠けているセンサーの出力値を推定して自律航法計算部１２に入力する。GPS受信機１３は、複数の人工衛星から距離と距離変化率（速度）に関する信号を受信することで、車両のアンテナの位置(緯度Ｎ、経度Ｅ、高さＤ)と車両速度(北方向速度ｖＮ、東方向速度ｖＥ、上下方向速度ｖＤ)の測定値を１Hzでカルマンフィルタ補正部１４に入力する。
カルマンフィルタ演算部１４は車両の現在位置、速度及び姿勢を含む車両状態量の補正を所定周期で行う。すなわち、カルマンフィルタ演算部１４は５Hzの周期で車速センサー１１から得られる速度(走行時)あるいはジャイロ１０ｂから出力される角速度オフセット(静止時)を観測値として用いて状態量補正処理を行い、得られた補正状態量を自律航法計算部１２に入力する。走行時における補正処理において、カルマンフィルタ演算部１４は、自律航法計算部１２が計算するピッチ角およびヨー角の増減量を用いて(18)式により車両固定座標系の各軸速度成分を演算する。
又、カルマンフィルタ補正部１４は１Hzの速度でGPS受信機１３から得られる位置・速度（Ｎ，Ｅ，Ｄ，ｖＮ，ｖＥ，ｖＤ)を観測値として用いて状態量補正処理を行って得られた補正状態量を自律航法計算部１２に入力する。
【００３３】
（ｂ）自律航法計算部
図４は自律航法計算部１２の構成図であり、状態量更新部２１は６自由度の基本システムのアルゴリズムに従って、２５Ｈｚ周期で車両の現在位置、速度及び姿勢を含む車両の状態量
【数１９】
を計算して出力すると共に、所定周期でカルマンフィルタ補正部１４から入力する状態量補正データにより状態量を補正する。なお、状態量のうちｃ００，ｃ１０，ｃ２０，ｃ２１はセンサー座標系からＮＥＤ座標系に変換する姿勢パラメータであり、ロール角Ｅ１とピッチ角Ｅ２とヨー角Ｅ３を用いて次式
【数２０】
で表現することができる。
【００３４】
変化量計算部２２は、状態量計算周期（２５Hz）で(20)式よりピッチ角Ｅ２およびヨー角Ｅ３を計算し、それぞれの変化量
を計算して、移動量ベクトル算出部２３に入力する。計数部４は状態量計算周期（２５Hz）の間に車速センサー１１から入力する車速パルスの数Ｎpを計数し，移動距離計算部２５は車速パルス数Ｎpに車速１パルス当たりの移動距離Ｌを乗算して移動距離Ｎp×Ｌを移動ベクトル算出部２３に入力する。移動量ベクトル算出部２３は(17)式により車両の移動距離ベクトルｓｂを計算して状態量更新部２１に入力する。状態量更新部２１は、加速度計１０ａ、ジャイロスコープ１０ｂおよびＳＣコントローラ１５から入力される６軸分のセンサー出力信号および移動距離ベクトルｓｂを用いて車両の現在位置、速度及び姿勢を含む車両の状態量を計算して出力する。
【００３５】
（ｃ）カルマンフィルタ補正部
図５はカルマンフィルタ補正部１４の構成図であり、状態量補正データ演算部３１は所定周期で状態量ｘｋとカルマンゲインＫｋと測定ベクトルｚｋを用いて補正状態量を計算し、状態補正データを自律航法計算部１２に入力する。カルマンゲイン算出部３２はカルマンゲインＫｋを計算して状態量補正データ演算部３１に入力する。測定ベクトル発生部３３は所定周期で測定ベクトルｚｋを発生して状態量補正データ演算部３１に入力する。
変化量受信部３４は自律航法計算部１２が計算したピッチ角Ｅ２およびヨー角Ｅ３の変化量を受信して速度ベクトル算出部３５に入力する。速度算出部３６は１車速パルス当たりの移動距離Ｌをパルス間隔時間ｔで除算することにより瞬間車速Ｖを計算して速度ベクトル算出部３５に入力する。速度ベクトル算出部３５は、(18)式により車両固定座標系の各軸の観測速度vｘｂ-obs、vｙｂ-obs、vｚｂ-obsを計算して測定ベクトル発生部３３に入力する。なお、(18)式における瞬間車速値は｜Ｖ｜である。状態監視部３７は、通常走行状態であるか静止状態であるかを監視し、状態信号を測定ベクトル発生部３３に入力する。
【００３６】
測定ベクトル発生部３３は、１Hzの周期でGPS受信信号Ｎ，Ｅ，Ｄ，ｖＮ，ｖＥ，ｖＤを用いて測定ベクトルｚｋを発生し、５Hzの周期で、通常走行状態であれば各軸の観測速度vｘｂ-obs、vｙｂ-obs、vｚｂ-obsを用いて測定ベクトルｚｋを発生し、静止行状態であれば各軸のジャイロ出力を用いて測定ベクトルｚｋを発生し、それぞれを状態量補正データ演算部３１に入力する。
状態量補正データ演算部３１は、所定のアルゴリズム（後述する）に従って所定周期で状態量ｘｋとカルマンゲインＫｋと測定ベクトルｚｋを用いて補正状態量を計算し、状態補正データを自律航法計算部１２に入力する。
【００３７】
（ｄ）本発明の効果
図６は従来技術と本発明の精度比較説明図表、図７は方位誤差説明図、図８は高度差説明図である。図６の精度比較説明図表において、（１）はＧＰＳ受信できない地上１０階建ての螺旋駐車場において最上階で道路と平行して停車したときの道路方位と推定車両方位との方位誤差（ｄｅｇ）を示し、（２）は地上の入り口から最上階まで行ってから地上の出口に戻ってきたときの推定高度の高度差（ｍ）を示している。
本発明によれば、加速度計１軸、ジャイロスコープ１軸から成るシステムであっても、方位誤差が４度（図７（A）参照）、高度差が２．１６ｍ（図８の曲線Ａ参照）と小さくなり、従来技術の加速度計１軸、ジャイロスコープ１軸から成るシステムの方位誤差、高度差と比較すると格段に精度が向上し、従来技術の加速度計３軸、ジャイロスコープ２軸から成るシステムと同等の精度が得られている。なお、図７は車両の走行軌跡を示しており、Aから表示エリアに進入し、ついで地上１０階建ての螺旋駐車場を最上階まで行って、道路に平行に駐車し、その後、地上まで戻ってBから脱出している。図７（Ａ)，（Ｂ）におけるひげ部分が道路と平行駐車したときの走行軌跡である。
以上のように、本発明によれば、自律航法計算部１２は(17)式により計算した車両の移動距離ベクトルｓｂを用いて状態量を計算したから、移動距離ベクトルの精度を向上でき、位置精度を改善することができる。
また、本発明によれば、カルマンフィルタ補正部１４は通常走行状態において５Hzの周期で(18)式により計算した各軸の観測速度vｘｂ-obs、vｙｂ-obs、vｚｂ-obsを用いて測定ベクトルｚｋを発生するようにしたから、各軸の観測速度を向上することができ、速度、センサー姿勢角、センサー取付角精度を改善することができる。この結果、相対高度精度を飛躍的に向上することができる。
また、本発明によれば、少ないセンサー構成で位置および方位精度を向上できるため、ナビゲーションシステムのコストを抑えることが出来る。
さらに、相対高度精度が飛躍的に向上するため、一般道と高速道路の上下分岐を判定する手段に応用することができる。
【００３８】
（ｅ）変形例
以上の車速分解法では、ピッチ角の変化量およびヨー角の変化量の両方を考慮したが、センサ姿勢ヨー角のみを考慮する簡略方法を用いてもよい。これは、次の理由による。
（１）車速の誤差が大きい場合（低速中、またはパルス取得方法の違いなどによる）、センサー姿勢ピッチ角の推定精度が悪くなり、また、（２）車体の姿勢角は、ヨー角の変化量が支配的で、短時間におけるピッチ角の変化量はほぼ０に近い、からである。
以上より、自律航法計算部１２は(17)式に替えて次式
【数２１】
により車両の移動距離ベクトルｓｂを計算し、該移動距離ベクトルを用いて状態量の計算を行なう。
また、カルマンフィルタ補正部１４は、測定方程式として次式
【数２２】
により各軸の観測速度vｘｂ-obs、vｙｂ-obs、vｚｂ-obsを計算して状態量補正処理を行う。
【００３９】
図９は変形例の自律航法計算部１２の構成図であり、図４の自律航法計算部１２と異なる点は、変化量演算部２２がヨー角の変化量のみ演算する点、移動量ベクトル算出部２３が(21)式により移動距離ベクトルｓｂを計算して状態量更新部２１に入力する点である。
図１０は変形例のカルマンフィルタ補正部１４の構成図であり、図５のカルマンフィルタ補正部１４と異なる点は、変化量受信部３４が自律航法計算部１２で計算されたヨー角の変化量を受信して速度ベクトル算出部３５に入力する点、速度ベクトル算出部35が(22)式により各軸の観測速度vｘｂ-obs、vｙｂ-obs、vｚｂ-obsを計算して測定ベクトル発生部３３に入力する点である。
以上では、自律航法計算部１２における自律航法計算アルゴリズムやカルマンフィルタ演算部１４における計算アルゴリズムの詳細説明をしなかったが、以下にその詳細を説明する。
【００４０】
（Ｃ）自律航法計算部における自律航法計算アルゴリズム
以下において自律航法計算部における自律航法計算アルゴリズムを説明するが、以降の式において、
で示すように変数の上にドットがついているものは、その変数ｖsの時間変化率を表しているものとする。
【００４１】
（ａ）速度方程式
速度方程式は次式
【数２３】
で表現される。但し、
【数２４】
であり、(24a)はセンサー固定座標系で表現される速度ベクトル、(24b)はセンサー固定座標系で表現されるジャイロスコープ出力ベクトル、(24c)はセンサー固定座標系で表現される加速度計出力ベクトル、(24d)はセンサー固定座標系で表現される重力ベクトルである。ここで、
【数２５】
ここで、センサーの“感度”は、設置角の影響などを含めないセンサー自体の電圧出力から物理量への変換定数を意味する。ジャイロに関する感度誤差はナビゲーションの方位計算に大きな影響を及ぼすので、本ナビゲーションシステムではジャイロ感度をカルマンフィルタの状態量に含め、常時推定・補正を行う。
【００４２】
（ｂ） 姿勢方程式
姿勢方程式は次式で表現される。
【数２６】
ただし、
【数２６ａ】
はセンサー座標系からNED座標系への変換行列であり、パラメータｃ００，ｃ１０，ｃ２０，ｃ２１は（20）式に示すようにピッチ角とヨー角により表現することが出来る。
（26）式は、慣性航法システム（Inertial Navigation System: INS）の一般的な手法である以下の姿勢方程式
【数２７】
から、車両ナビゲーション適用の際に独立な4つのパラメータだけを抜き出したものである。(27)式はより一般的なINS姿勢方程式である。
【００４３】
（ｃ）位置方程式
位置方程式は次式
【数２８】
により表現できる。但し、(28)式において、Ｔｎｂは車両固定座標系からＮＥＤ座標系への変換行列である。また、(28)式において、
【数２９】
であり、(29a)は位置ベクトル、(29b)は車両移動距離ベクトルである。さらに、(29b)式において、Np はサンプル時間当たりの車速パルス数、Lはパルス間距離である。
【００４４】
（ｄ） 固定パラメータ方程式
固定パラメータ方程式は次式
【数３０】
により表現される。但し、
【数３１】
であり、(31a)はセンサー座標系ジャイロスコープ出力に含まれるバイアス（Volt）、(31b)はセンサー座標系加速度計出力に含まれるバイアス（m/s2）、(31c)はセンサー座標系ジャイロスコープ出力に関する感度（rad/sec/Volt）、(27d)はセンサー座標系から車両座標系への変換行列である。また、(30）式中p００、p１０、p２０は取り付け角に関する行列の要素から、独立な3つのパラメータだけを抜き出したものである。
【００４５】
（ｅ） 非線形状態方程式
上記（23）、(26)、(28)、（30）式をまとめて、簡単に（32）式の様に行列式で表すことができる
【数３２】
但し、ｘは以下に与えられる非線形状態量ベクトルである。
【数３２ａ】
【００４６】
（ｆ） 状態量更新
CPU上で(32)式を数値積分するには、例えば、次式の状態量更新式
【数３３】
を用いて行えばよい。精度要求の高い場合は、周知の数値積分方法であるルンゲ・クッタ4次式(下記文献参照)などを用いてもよい。
Kreyszig, E., Advanced Engineering Mathematics, John Wiley & Sons, 1999, New York, NY.
但し、(33)式において、Tはサンプルタイムで、例えば25HzならばT=0.04秒である。以上は一般的な慣性航法システム（INS）に用いられる手法を基に、MEMSセンサー使用を考慮して、地球の丸みなど微小な項を簡略化した車両用慣性航法システム（INS）である(下記文献参照)。
Hoshizaki, T., Computational Scheme for MEMS Inertial Navigation Systems, AOAMR Patent, August, 2006.
【００４７】
（ｇ） 微小擾乱方程式
さて、GPS受信機などの測定値をもとに、カルマンフィルタ補正部１４がカルマンフィルタ処理で状態量ｘを補正を行うためには、自律航法計算部１２が状態量を非線形状態方程式により更新を行い、かつ、誤差量が線形方程式によって増加すると仮定して該誤差量（微小擾乱量）のコバリアンス値も更新する必要がある。そのために、（29）式の線形化を行い、微小擾乱方程式である行列式（34）を用意する。
【数３４】
但し、
は以下に与えられる微小擾乱ベクトルである。
【数３５】
ただし、(35b)は
の現在推定値、(35c)はホワイトノイズでモデル化された3軸ジャイロスコープ出力
に含まれる高周波ノイズと、3軸加速度計出力
に含まれる高周波ノイズである。それぞれのノイズの標準偏差(σ）はセンサースペックや、測定実験などによって得られ、ここではそれらの大きさを次のようにＮで表す。
【数３６】
また、
に関する微小擾乱量であり、
には、慣性航法システム（INS）の一般的な手法に基づき、以下の関係がある。
【数３７】
また、
に関する微小擾乱量であり、
には、慣性航法システム（INS）の一般的な手法に基づき、以下の関係がある。
【数３８】
ここで車両に対するセンサーの取り付けロール角＝0と仮定すれば、取り付けロール角に関する補正量
となる。
(34)式における線形システムを表わす行列
はそれぞれ図１１、図１２に示すように得られる。ただし、図１１において、
【数３９】
【００４８】
（ｈ） コバリアンスの更新
以上のように得られた行列
を使って、カルマンフィルタ処理の定式に従い、高周波で以下のように微小擾乱ベクトル
のコバリアンス行列(誤差共分散行列)を次式により更新する。
【数４０】
但し、右肩のT は転地行列（Transpose）を意味し、右肩の−はカルマンフィルタの補正前の状態を意味し、右肩の＋はカルマンフィルタの補正後の状態を意味する。また、
の相関値の期待値（E[ ]）からなるコバリアンス行列であり、
に関するコバリアンス行列であり、それぞれ次式で表現される。
【数４１】
以上の（33）式及び（40）式が、６自由度の基本システムにおいて自律航法計算部１２が高周波で(例えば25Hzで)実行する演算式である。以下に(33)式及び（40）式を再掲する。
【数４２】
【００４９】
（Ｄ）カルマンフィルタ演算部の計算
次に、低周波の測定値入力に伴う、カルマンフィルタ補正部１２の計算方法について説明する。なお、以下の測定方程式で、
のように表されているのは、ホワイトノイズでモデル化された測定ノイズであり、その標準偏差は
の様に表されている。
【００５０】
（ａ）通常走行状態
通常走行状態においては、次式の測定方程式
【数４３】
を用いる。但し、瞬間車速値は車速パルス間距離Ｌをパルス間隔（時間）で割ることで得られる。これらの標準偏差は例えば
である。ただし、これらの値はデザイン値であり、ここに記した値は目安である。
【００５１】
（ｂ）静止状態
車速パルスが2秒以上入ってこないとき、車両が静止状態にあると判定する。静止状態において、測定値及びそれらの標準偏差は
であるとし、これに加えて以下の測定方程式を用いる。
【数４４】
である。
【００５２】
（ｃ）GPS受信機測定状態
以下のGPS受信機測定値が得られるときには、上記通常走行状態と静止状態のいずれかの測定に加え、以下のGPS受信機測定結果を利用する。
【数４５】
但し、測定値と測定誤差標準偏差はGPS受信機５０から得られる。
【００５３】
なお、加速度計１軸、およびジャイロスコープ１軸のシステムでは、ＳＣコントローラ１５は基本システムにかけているセンサー出力を推定して自律航法計算部１２に入力するが、かかる場合、ＳＣコントローラ１５はロール角=０の測定量をカルマンフィルタ１４に入力する。かかるロール角＝０の情報が送られてきたときには、カルマンフィルタ１４は次の測定方程式を使用する。
【数４６】
これは、
c21 = sin(センサーロール角)・cos(センサーピッチ角)
に相当し、c21 =0を満たすことで、ロール角＝0が自動的に満たされるからである。
【００５４】
（ｄ）非線形測定方程式
上記（43）−（46）式をまとめて、簡単に（47）式の非線形測定方程式で示すように行列式で表すことができる。
【数４７】
但し、測定ベクトル
は以下のように与えられる。
【数４８】
【００５５】
（ｅ）線形測定方程式（カルマンフィルタの観測式）
カルマンフィルタで使用するために（47）式を線形化すると、次式の線形測定方程式（カルマンフィルタの観測式）が得られる。
【数４９】
但し、微小擾乱測定ベクトル
は以下のように与えられる。
【数５０】
また、行列（観測行列という）
は図１３に示すように与えられる。通常走行状態では図１３の行列内の（1）の行を観測行列として使用し、静止状態では（1）の行と（2）の行を観測行列として使用する。もしもGPS受信機測定が同時に得られるときには（1）の行又は（1）の行と（2）の行に（3）の行を合わせて観測行列として使用する（特願２００７−２３３５１５参照）。さらに、センサー構成によっては（４）の行を加える。
【００５６】
測定値が得られたとき、以下の（51）−(53)式の反復計算を、所定の条件を満足するまで繰り返す。
【数５１】
【数５２】
【数５３】
但し、上式において、
【数５４】
である。また、
は測定値に関するコバリアンス行列であり、使用する
に応じて変化する。すなわち、コバリアンス行列は通常走行状態であれば(51a)で表現され、静止状態であれば(51b)式で表現される。
【数５５】
GPS受信機測位が得られるときには、上記いずれかの走行状態に加え、GPS受信機の測定誤差に関するコバリアンスを以下のように付け加える。
【数５６】
以上の実施例では、カルマンフィルタ演算により状態量を補正する場合について説明したが、本発明はカルマンフィルタ処理に限らず同等の処理を行う補正方法にも適用することが出来る。
【図面の簡単な説明】
【００５７】
【図１】車速ベクトルＶを車両固定座標系のｘｙ平面(水平面)の速度成分ｖｘｙと上下方向(車両固定座標系のｚ軸方向) の速度成分ｖｚに分解する車速分解説明図である。
【図２】ｘｙ平面(水平面)の速度成分ｖｘｙを車両前後方向(車両固定座標系のｘ軸方向) の速度成分ｖｘと左右方向(車両固定座標系のｙ軸方向) の速度成分ｖｙに分解する車速分解説明図である。
【図３】本発明のナビゲーションシステムの全体の構成図である。
【図４】自律航法計算部の構成図である。
【図５】カルマンフィルタ補正部の構成図である。
【図６】従来技術と本発明の精度比較説明図表である。
【図７】方位誤差説明図である。
【図８】高度差説明図である。
【図９】自律航法計算部の変形例である。
【図１０】カルマンフィルタ補正部の変形例ある。
【図１１】行列Ｆの構成要素説明図である。
【図１２】行列Gkの構成要素説明図である。
【図１３】行列Ｈの構成要素説明図である。
【図１４】カルマンフィルタ処理の概要説明図である。
【図１５】ナビゲーションシステムにおける位置検出部の構成図である。
【図１６】センサーボードに固定したセンサー座標系Xs-Ys-Zs及び加速度計3軸（Acc x, Acc y, Acc z）およびジャイロスコープ3軸（Gyro x, Gyro y, Gyro z）から成る6自由度の基本システムのセンサー説明図である。
【図１７】車両固定座標系Xb-Yb-Zb及び地表固定座標系North-East-Down（NED座標系）の説明図である。
【図１８】図１５の位置検出部の全体の処理フローである。
【図１９】センサー姿勢ピッチ角及びセンサー姿勢ヨー角説明図である。
【図２０】種々のセンサー構成を示す第１の説明図である。
【図２１】種々のセンサー構成を示す第２の説明図である。
【図２２】従来技術におけるセンサー構成と精度との対応説明図である。
【符号の説明】
【００５８】
１０ センサーボード
１０a 加速度計
１０b ジャイロスコープ
１１ 車速センサー
１２ 自律航法計算部
１３ GPS受信機
１４ カルマンフィルタ演算部
１５ センサー・コンフィギュレーション・コントローラ（SCコントローラ）
２１ 状態量更新部
２２ 変化量演算部
２３ 移動量ベクトル算出部
３１ 状態量補正データ演算部
３３ 測定ベクトル発生部
３４ 速度ベクトル算出部
【技術分野】
【０００１】
本発明はナビゲーションシステムの位置検出装置および位置検出方法に関わり、特に、自律航法計算部が計算する状態量（車両の位置、車両速度及びセンサーあるいは車体の姿勢を含む状態量）を所定周期で補正し、該補正された状態量を用いて自律航法計算部が状態量を計算して出力するナビゲーションシステムの位置検出装置および位置検出方法に関する。
【背景技術】
【０００２】
自律航法システムは、車両の距離センサーである車速パルスセンサーや、加速度計と角速度計(ジャイロスコープ)などの慣性センサーから成り、動体である車両の位置、速度、及びセンサー姿勢あるいは車体姿勢の推定のために広く使用されている。
【０００３】
慣性航法システム（Inertial Navigation System：INS）は、通常３軸加速度計と３軸ジャイロスコープからなり、動体の６自由度、即ち３次元の並進運動と３次元の回転運動を同時に推定するために使用される。例えば、航空・宇宙機のナビゲーションは、かかる慣性航法システムを用いて、動体の６自由度、即ち３次元の並進運動と３次元の回転運動を同時に推定する。車両ナビゲーションにおいては車速パルスセンサーを使用することで、コストの高い慣性センサーを削減する場合が多い。
【０００４】
自律航法は外部信号に頼らずにセンサー出力に基づいて推定位置の更新を行うことができる。しかし、センサー出力に含まれるノイズなどのエラーが時間の経過とともに積分計算に伴って蓄積するため、長時間は使用できない。そこで、衛星信号の届く範囲では有限誤差の位置推定を常時与える全地球測位システム（Global Positioning System: GPS受信機）と自律航法システムを、カルマンフィルタなどの最適確率アルゴリズムで結合することにより、両システムの性能上の問題を補間・解決する手法が考案された(特許文献１)。このようなシステムは複合ナビゲーションシステム（Integrated Navigation System）と呼ばれ、現在では車両ナビゲーションから航空宇宙ナビゲーションまで、幅広く利用されている。
【０００５】
カルマンフィルタ処理は所定時刻における所定状態量の予測値(推定値)と観測値との誤差を修正しながら、各時刻における最適な推定値を逐次求める方法である。カルマンフィルタ処理においては、予め、ある値を推定するための算出式を設定し、この算出式を用いて観測値Z(t)が得られる時刻ｎまで推定を繰り返す。時刻nで観測値が取得できれば、該観測値を用いて時刻ｎでの前記推定値について、確率論的に定義された誤差を最小化させるような推定値補正計算を行う。
図１４はカルマンフィルタ処理の概要説明図である。カルマンフィルタにおいては、図１４に示すように、信号生成過程１と観測過程２に分けられる。図において、線形システムF(前記算出式により同定されるシステム)があり、その線形システムの状態をX(t)とするとき、観測行列Hを介してX(t)の一部が観測できる場合、カルマンフィルタはX(t)の最適な推定値を与える。ここで、ｗは信号生成過程にて発生する雑音であり、ｖは観測過程にて発生する雑音である。カルマンフィルタは、入力を観測値Z(t)としてカルマン処理を所定周期で繰り返し実行することにより、最適推定値X(t)を求める。
【０００６】
カルマンフィルタ処理におけるシステムモデルの状態式は次式
【数１】
により表現される。システム状態変数δXは各軸速度、各軸位置、センサー姿勢等の状態量を含み後述するように
【数２】
と表現できる。観測値Z(t)が得られる周期より短い周期T毎に(1)式の演算が実行され、得られた値が状態量として出力される。そして、観測値Z(t)が得られれば、最適推定値X(t)をカルマンフィルタ処理により演算し、以後、該最適推定値を用いて(1)式の演算を実行する。
【０００７】
カルマンフィルタの観測式は
【数３】
と表現される。(1）式により得られるシステム状態変数は時間の経過と共に誤差が累積してゆく。そこで、観測値δZ(k)が得られる毎に(3)式を用いてシステム状態変数δX(k)の最適推定値を求め、以後、該最適推定値を用いて(1)式により、次の観測値が得られるまでシステム状態量δX(k)を計算して出力する。
【０００８】
すなわち、カルマンフィルタは、観測値Z(t)(=δZ(t))が得られる毎に（観測値Z(t)の入力周期で）、以下の(4)式を実行することにより最適推定値X(t|t)(=δX(t|t))を求める。ただし、時刻ｊまでの情報に基づく時刻iでのAの推定値をＡ(i|j)と表記するものとし、次式においてX(t｜t−1)は事前推定値、K(t)はカルマンゲインである。
【数４】
(4)式の右辺{ }内の差にカルマンゲインを乗算して最適推定値X(t|t)を計算する。事前推定値およびカルマンゲインはそれぞれ
【数５】
と表現でき、事前推定値X(t｜t−1)は、Z(t)の入力周期より短い周期で(1)式に相当する(5a)式により更新される。
【０００９】
また、(5b)式におけるＰは状態量Xの誤差共分散行列であり、P(t|t-1)は誤差共分散の予測値、P(t-1|t-1)は誤差共分散であり、それぞれ
【数６】
である。Ｖは観測過程で発生する雑音ｖの分散、Ｗは信号過程で発生する雑音wの分散である。添字の（・）Ｔ は転置行列を意味し、(・)−１は逆行列を意味する。また、Ｉは単位行列である。さらに、ＶとＷは平均０の白色ガウス雑音であり、互いに無相関である。上記のようなカルマンフィルタにおいて、状態量Ｘと誤差共分散Pの初期値に適当な誤差を与えてやり、新しい観測が行われる度に(4)式により最適推定値を求め、以後、(5a)式により、状態量Ｘを計算することにより該状態量の精度を向上することができる。
【００１０】
図１５は本願出願人により出願済みの車載ナビゲーションシステムの位置検出部の構成図である(特許文献２)。自律系の入力信号としては、センサーボード１０上の慣性センサーから得られる信号と、別のケーブルを通じて車体から取り込まれる車速パルスがある。慣性センサーとしては加速度計(加速度３軸)１０a、ジャイロスコープ（角速度３軸)１０bを採用し、車速パルスを発生するセンサーとしては車両が所定距離移動する毎に１個のパルスを発生する車速センサー１１を採用する。図１６(Ａ)はセンサーボード１０に固定したセンサー座標系Xs-Ys-Zsを示している。なお、以下の数式の中でセンサー（ボード）座標系に関する表現であることを、添え字の”ｓ”によって表現することに注意すべきである。図１６(B)は、センサーボード１０において、加速度計１０ａの3軸（Acc x, Acc y, Acc z）およびジャイロスコープ１０ｂの3軸（Gyro x, Gyro y, Gyro z）から成る、即ち6自由度の基本システムのセンサー構成を示したものである。３軸加速度計Acc x〜Acc zは、センサー座標系の三つの座標方向（ｘ，ｙ，ｚ）における加速度を検出し、また３軸ジャイロスコープGyro x〜Gyro zは、センサー座標系の三つの座標軸（ｘ，ｙ，ｚ）周りの角速度P,Q,Rを検出する。
なお、図１７（A）は、車両に固定した座標系Xb-Yb-Zbを示しており、以下の数式の中で車両固定座標系に関する表現であることを、添え字の”b”によって表現する。又、図１７(B)は、ある緯度、経度における、地表固定座標系North-East-Down（NED座標系）を示しおり、以下の数式の中でNED座標系に関する表現であることを、添え字の”n”によって表現する。
【００１１】
自律航法計算部１２は６自由度の基本システムのアルゴリズムを用いて、車両の現在の位置、速度及び姿勢を含む車両状態量を、所定の計算式に従って高周波数で、例えば２５Hzで更新して出力する。
GPS受信機１３は、複数の人工衛星から距離と距離変化率に関する信号を受信することで、車両のアンテナの位置(緯度Ｎ、経度Ｅ、高さＤ)と車両速度(北方向速度ｖＮ、東方向速度ｖＥ、上下方向速度ｖＤ)の測定値を１Hzでカルマンフィルタ補正部14に入力する。
カルマンフィルタ演算部１４は車両の現在の位置、速度及び姿勢を含む車両状態量の補正を行う。例えば、カルマンフィルタ演算部１４は５Hzの速度で、車速センサー１１から得られる速度(走行時)あるいはジャイロ１０ｂから出力される角速度オフセット(静止時)を観測値として用いて状態量補正処理を行って得られた補正状態量を自律航法計算部１２に入力する。又、カルマンフィルタ補正部１４は１Hzの速度でGPS受信機１３から得られる位置・速度（Ｎ，Ｅ，Ｄ，ｖＮ，ｖＥ，ｖＤ)を観測値として用いて状態量補正処理を行って得られた補正状態量を自律航法計算部１２に入力する。
【００１２】
図１８は図１５の位置検出部の全体の処理フローである。
はじめに、自律航法計算部１２に３次元車両位置Ｎ、Ｅ、Ｄ、車両速度Ｖx、ピッチ角θ、車両取り付けピッチ角Ａ、ヨー角Ｙ、車両取り付けヨー角Ａ2、ジャイロのオフセットωＯＦ、加速度センサーのオフセットαＯＦの初期値を設定する（ステップ１０１）。ジャイロ１０aおよび加速度センサー１０bは、側面から見たとき、車両方向と平行して車両に取り付けられるのが理想であるが、図１９（Ａ）に示すように取り付け誤差があり、センサー方向は車両方向に角度Ａ（取り付けピッチ角）を成して取り付けられる。なお、水平方向とセンサー方向の角度θをピッチ角といい、ピッチ角は傾斜角と取り付けピッチ角の和である。また、ジャイロ１０aおよび加速度センサー１０bは、平面に投影したとき、車両方向と一致して車両に取り付けられるのが理想であるが、取り付け誤差があり、図１９（Ｂ）に示すように、センサー方向は車両方向に角度Ａ2（取り付けヨー角）を成して取り付けられる。なお、北方向とセンサー方向の角度Ｙをヨー角といい、ヨー角Ｙは車両方向と取り付けヨー角の和である。
【００１３】
以後、自律航法計算部１２は、ジャイロ１０a、加速度センサー１０b、車速センサー１１の出力を取り込み（ステップ１０２）、第１周期（２５Hz周期）毎に自律航法計算アルゴリズムにしたがって(２)式に示す各軸速度、各軸位置、センサー姿勢等の状態量を計算して出力する(ステップ１０３)。
ついで、第２周期(５Hz周期)になったかチェックし(ステップ１０４)、第２周期になっていなければ、ステップ１０２以降の処理を繰り返す。
第２周期になっていれば、車両速度Ｖxが零である状態が2秒以上続いていたかどうかによって停車判定を行う（ステップ１０５）。
停車中でなければ、第３周期(１Hz周期=GPS測位周期)になっているかチェックし（ステップ１０６）、第３周期でなければカルマンフィルタ補正部１４は、車速センサー１１の出力より計算した車両前後方向の速度(観測値)Ｖxと自律航法計算部１２が計算した車両速度を用いてカルマンフィルタ処理により車両状態量を補正し、補正状態量を自律航法計算部１２に入力する（ステップ１０７）。このステップ１０７では、第１のカルマンフィルタの観測行列H1を用いた第１補正処理が行われる。
【００１４】
ステップ１０６において、第３周期であればカルマンフィルタ演算部１４はGPSレシーバ１４が出力する３次元の車両位置Ｎ，Ｅ，Ｄと３次元の車両速度ｖＮ，ｖＥ，ｖＤを観測値として用いて車両状態量を補正し、補正状態量を自律航法計算部１２に入力する(ステップ１０８)。このステップ１０８では第２のカルマンフィルタの観測行列H2を用いた第２補正処理が行われる。
ステップ１０５において、停車中であれば、第３周期(１Hz周期=GPS測位周期)になっているかチェックし（ステップ１０９）、第３周期でなければカルマンフィルタ演算部１４は前記ステップ１０７の補正処理を行うと共に、ジャイロの角速度出力信号(観測値)と自律航法計算部１２で計算した角速度信号オフセットにより車両状態量を補正し、補正状態量を自律航法計算部１２に入力する（ステップ１１０）。このステップ１１０では第３のカルマンフィルタの観測行列H3を用いた第３補正処理が行われる。
ステップ１０９において、第３周期であれば、カルマンフィルタ演算部１４は前記ステップ１０８の補正処理を行うと共に、ジャイロの角速度出力信号と自律航法計算部１２で計算した角速度信号オフセットとの差に基づいて角速度オフセット補正を行う（ステップ１１１）。このステップ１１１では第４のカルマンフィルタの観測行列H4を用いた第４補正処理が行われる（ステップ１１１）。
【００１５】
以上は３軸の加速度計、３軸のジャイロスコープを有する６自由度の基本システムの場合であるが、センサー数が多くなって高価となる。このため、図１５の位置検出部ではセンサー数を減らした構成に対応できるようになっている。図２０、図２１は種々のセンサー構成を示す説明図であり、図２０（Ａ）、（Ｂ）はセンサー座標系および６自由度の基本システムの説明図であり、図１６(Ａ)，（Ｂ）に示したものと同一である。
図２０（Ｃ）は、（Ａ）のセンサーボード１０において、加速度計3軸、ジャイロスコープ２軸から成るシステムのセンサー構成の説明図である。なお、ジャイロスコープが２軸しか検出できないときのシステムのジャイロスコープはGyro xおよび Gyro zである。
図２０（Ｄ）は、（Ａ）のセンサーボード１０において、加速度計3軸、ジャイロスコープ1軸から成るシステムのセンサー構成を示したものである。ジャイロスコープが１軸しか検出できないときのシステムのジャイロスコープはGyro zである。
図２１（Ａ）は、図２０（Ａ）のセンサーボード１０において、加速度計２軸、ジャイロスコープ1軸から成るシステムのセンサー構成の説明図であり、加速度計が２軸しか検出できないとき、システムの加速度計はAcc x, Acc yであり、ジャイロスコープが１軸しか検出できないときのシステムのジャイロスコープはGyro zである。
図２１（Ｂ）は図２０（Ａ）のセンサーボード１０において、加速度計１軸、ジャイロスコープ１軸ら成るシステムのセンサー構成の説明図であり、加速度計が１軸しか検出できないときのシステムの加速度計はAcc xであり、またジャイロスコープが１軸しか検出できないとき、システムのジャイロスコープはGyro zである。
図２１（Ｃ）は、図２０（Ａ）のセンサーボード１０において、加速度計０軸（不使用）、ジャイロスコープ１軸から成るシステムのセンサー構成の説明図であり、ジャイロスコープが１軸しか検出できないときのシステムのジャイロスコープはGyro zである。
【００１６】
図１５の自律航法計算部１２は常に同じ６自由度の基本システムのアルゴリズムを用いて、車両の現在の位置、速度及び姿勢を含む車両状態量を、高周波数で更新する。このため、センサー・コンフィギュレーション・コントローラ（SCコントローラ）１５は、車載ナビゲーションシステムの実際のセンサー構成を識別し、基本システムから欠けているセンサーの出力値を推定して自律航法計算部１２に入力する。たとえば、欠けているセンサーの出力値として、定数（０を含む）あるいはホワイトノイズあるいは他センサーの出力値を変数とする関数の関数値を採用する。
【００１７】
しかし、センサー数が減少すると精度が劣化する問題がある。図２２は、センサー構成と精度の説明図であり、（１）ＧＰＳ受信できない地上１０階建ての螺旋駐車場において最上階で道路と平行して停車したときの道路方位と推定車両方位との方位誤差（ｄｅｇ）と、（２）地上の入り口から最上階まで行ってから地上の出口に戻ってきたときの推定高度の高度差（ｍ）を示している。加速度計3軸、ジャイロスコープ２軸から成るシステムでは、方位誤差が３度、高度さが５ｍであるが、加速度計１軸、ジャイロスコープ１軸から成るシステムでは、方位誤差が２０度（図７Ｂ）参照）、高度差が−１１．６ｍ（図８の曲線Ｂ参照）と非常に大きくなってしまう。
【特許文献１】特許３４７３１１７号
【特許文献２】特願２００８−０３９５６３
【発明の開示】
【発明が解決しようとする課題】
【００１８】
以上から、本発明の目的は、センサー数が少ないシステムであっても、例えば加速度計１軸、ジャイロスコープ１軸のシステムであっても、位置精度や方位精度を向上できるようにすることである。
【課題を解決するための手段】
【００１９】
本発明は車両の位置を検出するナビゲーションシステムの位置検出装置および位置検出方法である。
・位置検出装置
本発明の位置検出装置は、車両の速度を検出する車速センサーと、車両の加速度を検出する加速度センサーと、車両の角速度を検出する角速度センサーとを含むセンサー部、これらセンサーから出力される信号を用いて所定の周期で車両の現在位置、車両速度、車体の姿勢角を含む状態量を計算する自律航法計算部を備え、前記自律航法計算部は、前記車速センサーから出力される信号を用いて前回の状態量計算時刻から今回の状態量計算時刻までに移動した移動量を計算する移動量算出部、車体の姿勢変化量を計算する変化量検出部、前記姿勢変化量を用いて前記移動量を車体の前後方向、車体の左右方向、車体の上下方向の方向成分に分解する移動量分解部、前記各方向成分を用いて今回の状態量計算時刻における車両の位置を計算する状態量計算部を備えている。
前記変化量検出部は、前記車体の姿勢変化量として、（１）車体のピッチ角の変化量と車体のヨー角の変化量、あるいは（２）車体のヨー角の変化量を検出する。この際、変化量検出部は、前記状態量計算部が計算した車体の姿勢角を用いて前記車体のピッチ角の変化量と車体のヨー角の変化量を計算する。
【００２０】
車体の姿勢変化量として車体のピッチ角変化量と車体のヨー角変化量が検出され、これらをそれぞれ
とし、移動量をNｐLとすれば、前記移動量分解部は該移動量を次式
【数７】
により車体の前後方向、車体の左右方向、車体の上下方向の方向成分に分解する。
また、車体の姿勢変化量として車体のヨー角変化量が検出され、これを
とし、移動量をNｐLとすれば、前記移動量分解部は次式
【数８】
により前記移動量を車体の前後方向、車体の左右方向、車体の上下方向の方向成分に分解する。
【００２１】
本発明の位置検出装置は更に、前記車速センサーの出力信号より測定した車両の移動速度と前記自律航法計算部が計算した前記状態量とを用いて、該自律航法計算部が状態量を計算する前記周期より長い第２の周期で状態量補正処理を実行して該自律航法計算部が計算する状態量を補正する状態量補正部、例えばカルマンフィルタを備え、該状態量補正部は、前記姿勢変化量を用いて前記測定した移動速度を車体の前後方向、車体の左右方向、車体の上下方向の方向成分に分解する速度分解部、前記各方向の速度成分と前記自律航法計算部が計算した前記状態量を用いて補正状態量を計算する補正状態量計算部を備えている。
前記姿勢変化量が車体のピッチ角の変化量と車体のヨー角の変化量であり、該車体のピッチ角変化量と車体のヨー角変化量をそれぞれ
とし、前記測定した移動速度をVとすれば、前記速度分解部は次式
【数９】
により該移動速度を車体の前後方向、車体の左右方向、車体の上下方向の方向成分に分解する。
また、前記速度分解部は、前記姿勢変化量が車体のヨー角の変化量であり、該車体のヨー角変化量を
とし、前記測定した移動速度をVとすれば、前記速度分解部は次式
【数１０】
により該移動速度を車体の前後方向、車体の左右方向、車体の上下方向の方向成分に分解する。
【００２２】
・位置検出方法
本発明の位置検出方法は、車両の速度を検出する車速センサーと、車両の加速度を検出する加速度センサーと、車両の角速度を検出する角速度センサーから出力される信号を用いて、所定の周期で車両の現在位置、車両速度、車体の姿勢角を含む状態量を自律航法により計算するナビゲーションシステムの位置検出方法であり、前記車速センサーから出力される信号を用いて前回の状態量計算時刻から今回の状態量計算時刻までに移動した移動量を計算する第１ステップ、車体の姿勢変化量を計算する第２ステップ、前記姿勢変化量を用いて前記移動量を車体の前後方向、車体の左右方向、車体の上下方向の方向成分に分解する第３ステップ、前記各方向成分を用いて今回の状態量計算時刻における車両の位置を計算する第４ステップを備えている。
第２ステップは、上記位置検出装置の変化量検出部が行う処理であり、前記車体の姿勢変化量として、（１）車体のピッチ角の変化量と車体のヨー角の変化量、あるいは（２）車体のヨー角の変化量を検出する。
第３ステップは、上記位置検出装置の移動量分解部が行う処理であり、(7)式あるいは(8)式により移動量を車体の前後方向、車体の左右方向、車体の上下方向の方向成分に分解する。
本発明の位置検出方法は更に、前記自律航法により状態量を計算する前記周期より長い第２の周期で、前記車速センサーの出力信号より車両の移動速度を測定する第５ステップ、
該測定した移動速度と前記自律航法により計算した状態量とを用いて状態量補正処理を実行し、該自律航法により計算する状態量を補正する第６ステップを備え、該第６ステップは、前記姿勢変化量を用いて前記測定した移動速度を車体の前後方向、車体の左右方向、車体の上下方向の方向成分に分解する分解ステップ、前記各方向の速度成分と前記自律航法計算部が計算した前記状態量を用いて補正状態量を計算する計算ステップを備えている。
前記分解ステップは、上記位置検出装置の速度分解部が行う処理であり、(9)式あるいは(10)式により測定した移動速度を車体の前後方向、車体の左右方向、車体の上下方向の方向成分に分解する。
【発明の効果】
【００２３】
本発明によれば、車両走行中に車体の姿勢角、例えばピッチ角やヨー角が変化する場合、これらピッチ角やヨー角の変化量を考慮して車両の移動距離を車体の前後方向、車体の左右方向、車体の上下方向の成分に分解したから、移動距離ベクトルの精度を向上でき、車両位置の計算精度を改善することができる。
また、本発明によれば、車両走行中にピッチ角やヨー角が変化する場合、これらピッチ角やヨー角の変化量を考慮して車両移動速度を車体の前後方向、車体の左右方向、車体の上下方向の成分vｘｂ-obs、vｙｂ-obs、vｚｂ-obsに分解したから、移動距離ベクトルの精度を向上することができ、速度、センサー姿勢角、センサー取付角精度を改善することができる。この結果、高度精度を向上することができる。
また、本発明によれば、少ないセンサー構成で位置および方位精度を向上できるため、ナビゲーションシステムのコストを抑えることが出来る。
さらに、本発明によれば、相対高度精度を向上することができるため、一般道と高速道路の上下分岐を判定する手段に応用することができる。
【発明を実施するための最良の形態】
【００２４】
（Ａ）本発明の原理
(ａ)従来の問題点の解析
図１５の従来の自律航法計算部１２は、自律航法計算アルゴリズムにしたがって各軸速度、各軸位置、センサー姿勢等の状態量を計算する。この従来の自律航法計算アルゴリズムでは、以下の位置方程式
【数１１】
によりＮＥＤ座標系における位置Ｐｎを計算する。但し、(11)式において
【数１２】
であり、Ｔｎｂは車両固定座標系からＮＥＤ座標系に変換する変換マトリックスである。又、(12a)は位置ベクトル、(12b)は車両固定座標系における車両の移動距離ベクトルであり、 (12b)式において、Npはサンプル時間当たりのパルス数、Ｌはパルス間距離である。
【００２５】
しかし、(12b)式は車両移動距離ベクトルを正しく表現していない。というのは、車両が勾配走行中においてピッチ角θを増減して走行している場合には、速度ベクトルの成分は車両前後方向(車両固定座標系のｘ軸方向)だけでなく、上下方向(車両固定座標系のｚ軸方向)にも発生する。また、車両がカーブで回転走行している場合やタイヤがスリップしている場合にはヨー角Ｙが増減する。かかるヨー角Ｙが増減して走行している場合には、速度ベクトルの成分は車両前後方向(車両固定座標系のｘ軸方向)だけでなく、左右方向(車両固定座標系のｙ軸方向)にも発生する。したがって、ピッチ角θおよびヨー角Ｙが増減して走行している場合には、速度ベクトルの成分は車両前後方向(車両固定座標系のｘ軸方向)だけでなく、上下方向(車両固定座標系のｚ軸方向)および左右方向(車両固定座標系のｙ軸方向)にも発生する。このため、(12)式は正しく車輌の移動距離ベクトルを表現しておらず、従来の自律航法計算部は精度よく状態量(車両位置、車両方位)を計算できなかったのである。
【００２６】
また、従来のカルマンフィルタ補正部１４は通常走行状態において、５Hzの速度で車速センサー１１から得られる速度を観測値として用いて状態量補正処理を行ない、得られた補正状態量を自律航法計算部１２に入力している。この状態量補正処理において、カルマンフィルタ補正部１４は、速度の観測値(測定値)として次式の測定方程式
【数１３】
を用い、ｖｘｂ(車両固定座標系の前後方向速度成分)は車速パルス間距離をパルス間隔(時間)で割ることで求め、ｖｙｂ(車両固定座標系の左右方向速度成分)= 0、ｖｚｂ(車両固定座標系の上下方向速度成分) = 0としている。なお、
は、ホワイトノイズでモデル化された瞬間車速値の測定ノイズである。
【００２７】
そして、カルマンフィルタ補正部１４は、（13）式で求まる観測値と自律航法計算部１２で計算された計算結果との差分に基づいて補正状態量を計算する。
しかし、前述のように、ピッチ角θおよびヨー角Ｙが変化している場合には、速度ベクトルの成分は車両前後方向だけでなく、上下方向および左右方向にも発生する。このため、従来のカルマンフィルタ補正部１４においては正確な観測値を使用しておらず、算出した補正状態量の信頼性が低く、結果的に自律航法計算部１２は精度よく状態量(車両位置、車両方位)を計算できなかった。
【００２８】
（ｂ）本発明の概略
本発明では、車両がピッチ角θおよびヨー角Ｙが増減して走行している場合、これらの増減率を考慮して（11）式の位置方程式における車両の移動距離ベクトルｓｂの各軸成分を計算し、また、（13）式の測定方程式における速度ベクトル（ｖｘｂ、ｖｙｂ、ｖｚｂ）を計算する。
図１、図２は本発明の車速分解の説明図であり、図１は車速ベクトルＶを車両固定座標系のｘｙ平面(水平面)の速度成分ｖｘｙと上下方向(車両固定座標系のｚ軸方向) の速度成分ｖｚに分解する車速分解説明図、図２はｘｙ平面(水平面)の速度成分ｖｘｙを車両前後方向(車両固定座標系のｘ軸方向) の速度成分ｖｘと左右方向(車両固定座標系のｙ軸方向) の速度成分ｖｙに分解する車速分解説明図である。
【００２９】
図１に示すようにピッチ角θおよびヨー角Ｙを増減して走行している場合において、ピッチ角θの変化量を
とし、速度ベクトルの大きさを｜Ｖ｜とすれば、ｘｙ平面(車体水平面)の速度成分ｖｘｙ，車体上下方向(車両固定座標系のｚ軸方向) における速度成分ｖｚはそれぞれ次式
【数１４】
で表現される。また、図２に示すようにヨー角Ｙの変化量を
とし、速度ベクトルの大きさ(速度)を｜ｖｘｙ｜とすれば、車両前後方向(車両固定座標系のｘ軸方向)における速度成分ｖｘ，左右方向(車両固定座標系のｙ軸方向) における速度成分ｖｙはそれぞれ次式
【数１５】
で表現される。(14)式を(15)式に代入してまとめると、
【数１６】
となる。
【００３０】
以上より、本発明では、(11)式の位置方程式における車両の移動距離ベクトルｓｂとして次式
【数１７】
を使用する。すなわち、移動量をNｐLとすれば、該移動量を車体の前後方向、車体の左右方向、車体の上下方向の方向成分に分解する。また、（13）式の測定方程式として次式
【数１８】
を使用する。すなわち、上式により瞬間車速値(測定した移動速度V)を車体の前後方向成分vｘｂ-obs、車体の左右方向成分vｙｂ-obs、車体の上下方向の方向成分vｚｂ-obsに分解する。
【００３１】
（Ｂ）本発明のナビゲーションシステム
（a）全体の構成
図３は本発明のナビゲーションシステムの全体の構成図であり、図１５と類似した構成を有しており、同一部分には同一符号を付している。
センサーボード１０上には、加速度計１０ａ、ジャイロスコープ１０ｂが設けられている。加速度計１０ａとして１軸〜3軸（Acc x, Acc y, Acc z）の構成が可能であり、また、ジャイロスコープ１０ｂとして１軸〜3軸（Gyro x, Gyro y, Gyro z）の構成が可能である。加速度計３軸およびジャイロスコープ３軸を備えたシステムが６自由度の基本システムでとなる。
車速センサー１１は車両が所定距離走行する毎に１個のパルスを発生する構成になっている。自律航法計算部１２は６自由度の基本システムのアルゴリズム（後述する）を用いて、車両の現在位置、速度及び姿勢を含む車両状態量を、所定の計算式に従って高周波数で、例えば２５Hzで更新して出力する。自律航法計算部１２は車両状態量の更新に際して、ピッチ角の変動率およびヨー角の変動率
をそれぞれ計算し、これらを用いて(17)式により位置方程式における車両の移動距離ベクトルｓｂを計算する。
【００３２】
センサー・コンフィギュレーション・コントローラ（SCコントローラ）１５は車載ナビゲーションシステムの実際のセンサー構成を識別し、基本システムから欠けているセンサーの出力値を推定して自律航法計算部１２に入力する。GPS受信機１３は、複数の人工衛星から距離と距離変化率（速度）に関する信号を受信することで、車両のアンテナの位置(緯度Ｎ、経度Ｅ、高さＤ)と車両速度(北方向速度ｖＮ、東方向速度ｖＥ、上下方向速度ｖＤ)の測定値を１Hzでカルマンフィルタ補正部１４に入力する。
カルマンフィルタ演算部１４は車両の現在位置、速度及び姿勢を含む車両状態量の補正を所定周期で行う。すなわち、カルマンフィルタ演算部１４は５Hzの周期で車速センサー１１から得られる速度(走行時)あるいはジャイロ１０ｂから出力される角速度オフセット(静止時)を観測値として用いて状態量補正処理を行い、得られた補正状態量を自律航法計算部１２に入力する。走行時における補正処理において、カルマンフィルタ演算部１４は、自律航法計算部１２が計算するピッチ角およびヨー角の増減量を用いて(18)式により車両固定座標系の各軸速度成分を演算する。
又、カルマンフィルタ補正部１４は１Hzの速度でGPS受信機１３から得られる位置・速度（Ｎ，Ｅ，Ｄ，ｖＮ，ｖＥ，ｖＤ)を観測値として用いて状態量補正処理を行って得られた補正状態量を自律航法計算部１２に入力する。
【００３３】
（ｂ）自律航法計算部
図４は自律航法計算部１２の構成図であり、状態量更新部２１は６自由度の基本システムのアルゴリズムに従って、２５Ｈｚ周期で車両の現在位置、速度及び姿勢を含む車両の状態量
【数１９】
を計算して出力すると共に、所定周期でカルマンフィルタ補正部１４から入力する状態量補正データにより状態量を補正する。なお、状態量のうちｃ００，ｃ１０，ｃ２０，ｃ２１はセンサー座標系からＮＥＤ座標系に変換する姿勢パラメータであり、ロール角Ｅ１とピッチ角Ｅ２とヨー角Ｅ３を用いて次式
【数２０】
で表現することができる。
【００３４】
変化量計算部２２は、状態量計算周期（２５Hz）で(20)式よりピッチ角Ｅ２およびヨー角Ｅ３を計算し、それぞれの変化量
を計算して、移動量ベクトル算出部２３に入力する。計数部４は状態量計算周期（２５Hz）の間に車速センサー１１から入力する車速パルスの数Ｎpを計数し，移動距離計算部２５は車速パルス数Ｎpに車速１パルス当たりの移動距離Ｌを乗算して移動距離Ｎp×Ｌを移動ベクトル算出部２３に入力する。移動量ベクトル算出部２３は(17)式により車両の移動距離ベクトルｓｂを計算して状態量更新部２１に入力する。状態量更新部２１は、加速度計１０ａ、ジャイロスコープ１０ｂおよびＳＣコントローラ１５から入力される６軸分のセンサー出力信号および移動距離ベクトルｓｂを用いて車両の現在位置、速度及び姿勢を含む車両の状態量を計算して出力する。
【００３５】
（ｃ）カルマンフィルタ補正部
図５はカルマンフィルタ補正部１４の構成図であり、状態量補正データ演算部３１は所定周期で状態量ｘｋとカルマンゲインＫｋと測定ベクトルｚｋを用いて補正状態量を計算し、状態補正データを自律航法計算部１２に入力する。カルマンゲイン算出部３２はカルマンゲインＫｋを計算して状態量補正データ演算部３１に入力する。測定ベクトル発生部３３は所定周期で測定ベクトルｚｋを発生して状態量補正データ演算部３１に入力する。
変化量受信部３４は自律航法計算部１２が計算したピッチ角Ｅ２およびヨー角Ｅ３の変化量を受信して速度ベクトル算出部３５に入力する。速度算出部３６は１車速パルス当たりの移動距離Ｌをパルス間隔時間ｔで除算することにより瞬間車速Ｖを計算して速度ベクトル算出部３５に入力する。速度ベクトル算出部３５は、(18)式により車両固定座標系の各軸の観測速度vｘｂ-obs、vｙｂ-obs、vｚｂ-obsを計算して測定ベクトル発生部３３に入力する。なお、(18)式における瞬間車速値は｜Ｖ｜である。状態監視部３７は、通常走行状態であるか静止状態であるかを監視し、状態信号を測定ベクトル発生部３３に入力する。
【００３６】
測定ベクトル発生部３３は、１Hzの周期でGPS受信信号Ｎ，Ｅ，Ｄ，ｖＮ，ｖＥ，ｖＤを用いて測定ベクトルｚｋを発生し、５Hzの周期で、通常走行状態であれば各軸の観測速度vｘｂ-obs、vｙｂ-obs、vｚｂ-obsを用いて測定ベクトルｚｋを発生し、静止行状態であれば各軸のジャイロ出力を用いて測定ベクトルｚｋを発生し、それぞれを状態量補正データ演算部３１に入力する。
状態量補正データ演算部３１は、所定のアルゴリズム（後述する）に従って所定周期で状態量ｘｋとカルマンゲインＫｋと測定ベクトルｚｋを用いて補正状態量を計算し、状態補正データを自律航法計算部１２に入力する。
【００３７】
（ｄ）本発明の効果
図６は従来技術と本発明の精度比較説明図表、図７は方位誤差説明図、図８は高度差説明図である。図６の精度比較説明図表において、（１）はＧＰＳ受信できない地上１０階建ての螺旋駐車場において最上階で道路と平行して停車したときの道路方位と推定車両方位との方位誤差（ｄｅｇ）を示し、（２）は地上の入り口から最上階まで行ってから地上の出口に戻ってきたときの推定高度の高度差（ｍ）を示している。
本発明によれば、加速度計１軸、ジャイロスコープ１軸から成るシステムであっても、方位誤差が４度（図７（A）参照）、高度差が２．１６ｍ（図８の曲線Ａ参照）と小さくなり、従来技術の加速度計１軸、ジャイロスコープ１軸から成るシステムの方位誤差、高度差と比較すると格段に精度が向上し、従来技術の加速度計３軸、ジャイロスコープ２軸から成るシステムと同等の精度が得られている。なお、図７は車両の走行軌跡を示しており、Aから表示エリアに進入し、ついで地上１０階建ての螺旋駐車場を最上階まで行って、道路に平行に駐車し、その後、地上まで戻ってBから脱出している。図７（Ａ)，（Ｂ）におけるひげ部分が道路と平行駐車したときの走行軌跡である。
以上のように、本発明によれば、自律航法計算部１２は(17)式により計算した車両の移動距離ベクトルｓｂを用いて状態量を計算したから、移動距離ベクトルの精度を向上でき、位置精度を改善することができる。
また、本発明によれば、カルマンフィルタ補正部１４は通常走行状態において５Hzの周期で(18)式により計算した各軸の観測速度vｘｂ-obs、vｙｂ-obs、vｚｂ-obsを用いて測定ベクトルｚｋを発生するようにしたから、各軸の観測速度を向上することができ、速度、センサー姿勢角、センサー取付角精度を改善することができる。この結果、相対高度精度を飛躍的に向上することができる。
また、本発明によれば、少ないセンサー構成で位置および方位精度を向上できるため、ナビゲーションシステムのコストを抑えることが出来る。
さらに、相対高度精度が飛躍的に向上するため、一般道と高速道路の上下分岐を判定する手段に応用することができる。
【００３８】
（ｅ）変形例
以上の車速分解法では、ピッチ角の変化量およびヨー角の変化量の両方を考慮したが、センサ姿勢ヨー角のみを考慮する簡略方法を用いてもよい。これは、次の理由による。
（１）車速の誤差が大きい場合（低速中、またはパルス取得方法の違いなどによる）、センサー姿勢ピッチ角の推定精度が悪くなり、また、（２）車体の姿勢角は、ヨー角の変化量が支配的で、短時間におけるピッチ角の変化量はほぼ０に近い、からである。
以上より、自律航法計算部１２は(17)式に替えて次式
【数２１】
により車両の移動距離ベクトルｓｂを計算し、該移動距離ベクトルを用いて状態量の計算を行なう。
また、カルマンフィルタ補正部１４は、測定方程式として次式
【数２２】
により各軸の観測速度vｘｂ-obs、vｙｂ-obs、vｚｂ-obsを計算して状態量補正処理を行う。
【００３９】
図９は変形例の自律航法計算部１２の構成図であり、図４の自律航法計算部１２と異なる点は、変化量演算部２２がヨー角の変化量のみ演算する点、移動量ベクトル算出部２３が(21)式により移動距離ベクトルｓｂを計算して状態量更新部２１に入力する点である。
図１０は変形例のカルマンフィルタ補正部１４の構成図であり、図５のカルマンフィルタ補正部１４と異なる点は、変化量受信部３４が自律航法計算部１２で計算されたヨー角の変化量を受信して速度ベクトル算出部３５に入力する点、速度ベクトル算出部35が(22)式により各軸の観測速度vｘｂ-obs、vｙｂ-obs、vｚｂ-obsを計算して測定ベクトル発生部３３に入力する点である。
以上では、自律航法計算部１２における自律航法計算アルゴリズムやカルマンフィルタ演算部１４における計算アルゴリズムの詳細説明をしなかったが、以下にその詳細を説明する。
【００４０】
（Ｃ）自律航法計算部における自律航法計算アルゴリズム
以下において自律航法計算部における自律航法計算アルゴリズムを説明するが、以降の式において、
で示すように変数の上にドットがついているものは、その変数ｖsの時間変化率を表しているものとする。
【００４１】
（ａ）速度方程式
速度方程式は次式
【数２３】
で表現される。但し、
【数２４】
であり、(24a)はセンサー固定座標系で表現される速度ベクトル、(24b)はセンサー固定座標系で表現されるジャイロスコープ出力ベクトル、(24c)はセンサー固定座標系で表現される加速度計出力ベクトル、(24d)はセンサー固定座標系で表現される重力ベクトルである。ここで、
【数２５】
ここで、センサーの“感度”は、設置角の影響などを含めないセンサー自体の電圧出力から物理量への変換定数を意味する。ジャイロに関する感度誤差はナビゲーションの方位計算に大きな影響を及ぼすので、本ナビゲーションシステムではジャイロ感度をカルマンフィルタの状態量に含め、常時推定・補正を行う。
【００４２】
（ｂ） 姿勢方程式
姿勢方程式は次式で表現される。
【数２６】
ただし、
【数２６ａ】
はセンサー座標系からNED座標系への変換行列であり、パラメータｃ００，ｃ１０，ｃ２０，ｃ２１は（20）式に示すようにピッチ角とヨー角により表現することが出来る。
（26）式は、慣性航法システム（Inertial Navigation System: INS）の一般的な手法である以下の姿勢方程式
【数２７】
から、車両ナビゲーション適用の際に独立な4つのパラメータだけを抜き出したものである。(27)式はより一般的なINS姿勢方程式である。
【００４３】
（ｃ）位置方程式
位置方程式は次式
【数２８】
により表現できる。但し、(28)式において、Ｔｎｂは車両固定座標系からＮＥＤ座標系への変換行列である。また、(28)式において、
【数２９】
であり、(29a)は位置ベクトル、(29b)は車両移動距離ベクトルである。さらに、(29b)式において、Np はサンプル時間当たりの車速パルス数、Lはパルス間距離である。
【００４４】
（ｄ） 固定パラメータ方程式
固定パラメータ方程式は次式
【数３０】
により表現される。但し、
【数３１】
であり、(31a)はセンサー座標系ジャイロスコープ出力に含まれるバイアス（Volt）、(31b)はセンサー座標系加速度計出力に含まれるバイアス（m/s2）、(31c)はセンサー座標系ジャイロスコープ出力に関する感度（rad/sec/Volt）、(27d)はセンサー座標系から車両座標系への変換行列である。また、(30）式中p００、p１０、p２０は取り付け角に関する行列の要素から、独立な3つのパラメータだけを抜き出したものである。
【００４５】
（ｅ） 非線形状態方程式
上記（23）、(26)、(28)、（30）式をまとめて、簡単に（32）式の様に行列式で表すことができる
【数３２】
但し、ｘは以下に与えられる非線形状態量ベクトルである。
【数３２ａ】
【００４６】
（ｆ） 状態量更新
CPU上で(32)式を数値積分するには、例えば、次式の状態量更新式
【数３３】
を用いて行えばよい。精度要求の高い場合は、周知の数値積分方法であるルンゲ・クッタ4次式(下記文献参照)などを用いてもよい。
Kreyszig, E., Advanced Engineering Mathematics, John Wiley & Sons, 1999, New York, NY.
但し、(33)式において、Tはサンプルタイムで、例えば25HzならばT=0.04秒である。以上は一般的な慣性航法システム（INS）に用いられる手法を基に、MEMSセンサー使用を考慮して、地球の丸みなど微小な項を簡略化した車両用慣性航法システム（INS）である(下記文献参照)。
Hoshizaki, T., Computational Scheme for MEMS Inertial Navigation Systems, AOAMR Patent, August, 2006.
【００４７】
（ｇ） 微小擾乱方程式
さて、GPS受信機などの測定値をもとに、カルマンフィルタ補正部１４がカルマンフィルタ処理で状態量ｘを補正を行うためには、自律航法計算部１２が状態量を非線形状態方程式により更新を行い、かつ、誤差量が線形方程式によって増加すると仮定して該誤差量（微小擾乱量）のコバリアンス値も更新する必要がある。そのために、（29）式の線形化を行い、微小擾乱方程式である行列式（34）を用意する。
【数３４】
但し、
は以下に与えられる微小擾乱ベクトルである。
【数３５】
ただし、(35b)は
の現在推定値、(35c)はホワイトノイズでモデル化された3軸ジャイロスコープ出力
に含まれる高周波ノイズと、3軸加速度計出力
に含まれる高周波ノイズである。それぞれのノイズの標準偏差(σ）はセンサースペックや、測定実験などによって得られ、ここではそれらの大きさを次のようにＮで表す。
【数３６】
また、
に関する微小擾乱量であり、
には、慣性航法システム（INS）の一般的な手法に基づき、以下の関係がある。
【数３７】
また、
に関する微小擾乱量であり、
には、慣性航法システム（INS）の一般的な手法に基づき、以下の関係がある。
【数３８】
ここで車両に対するセンサーの取り付けロール角＝0と仮定すれば、取り付けロール角に関する補正量
となる。
(34)式における線形システムを表わす行列
はそれぞれ図１１、図１２に示すように得られる。ただし、図１１において、
【数３９】
【００４８】
（ｈ） コバリアンスの更新
以上のように得られた行列
を使って、カルマンフィルタ処理の定式に従い、高周波で以下のように微小擾乱ベクトル
のコバリアンス行列(誤差共分散行列)を次式により更新する。
【数４０】
但し、右肩のT は転地行列（Transpose）を意味し、右肩の−はカルマンフィルタの補正前の状態を意味し、右肩の＋はカルマンフィルタの補正後の状態を意味する。また、
の相関値の期待値（E[ ]）からなるコバリアンス行列であり、
に関するコバリアンス行列であり、それぞれ次式で表現される。
【数４１】
以上の（33）式及び（40）式が、６自由度の基本システムにおいて自律航法計算部１２が高周波で(例えば25Hzで)実行する演算式である。以下に(33)式及び（40）式を再掲する。
【数４２】
【００４９】
（Ｄ）カルマンフィルタ演算部の計算
次に、低周波の測定値入力に伴う、カルマンフィルタ補正部１２の計算方法について説明する。なお、以下の測定方程式で、
のように表されているのは、ホワイトノイズでモデル化された測定ノイズであり、その標準偏差は
の様に表されている。
【００５０】
（ａ）通常走行状態
通常走行状態においては、次式の測定方程式
【数４３】
を用いる。但し、瞬間車速値は車速パルス間距離Ｌをパルス間隔（時間）で割ることで得られる。これらの標準偏差は例えば
である。ただし、これらの値はデザイン値であり、ここに記した値は目安である。
【００５１】
（ｂ）静止状態
車速パルスが2秒以上入ってこないとき、車両が静止状態にあると判定する。静止状態において、測定値及びそれらの標準偏差は
であるとし、これに加えて以下の測定方程式を用いる。
【数４４】
である。
【００５２】
（ｃ）GPS受信機測定状態
以下のGPS受信機測定値が得られるときには、上記通常走行状態と静止状態のいずれかの測定に加え、以下のGPS受信機測定結果を利用する。
【数４５】
但し、測定値と測定誤差標準偏差はGPS受信機５０から得られる。
【００５３】
なお、加速度計１軸、およびジャイロスコープ１軸のシステムでは、ＳＣコントローラ１５は基本システムにかけているセンサー出力を推定して自律航法計算部１２に入力するが、かかる場合、ＳＣコントローラ１５はロール角=０の測定量をカルマンフィルタ１４に入力する。かかるロール角＝０の情報が送られてきたときには、カルマンフィルタ１４は次の測定方程式を使用する。
【数４６】
これは、
c21 = sin(センサーロール角)・cos(センサーピッチ角)
に相当し、c21 =0を満たすことで、ロール角＝0が自動的に満たされるからである。
【００５４】
（ｄ）非線形測定方程式
上記（43）−（46）式をまとめて、簡単に（47）式の非線形測定方程式で示すように行列式で表すことができる。
【数４７】
但し、測定ベクトル
は以下のように与えられる。
【数４８】
【００５５】
（ｅ）線形測定方程式（カルマンフィルタの観測式）
カルマンフィルタで使用するために（47）式を線形化すると、次式の線形測定方程式（カルマンフィルタの観測式）が得られる。
【数４９】
但し、微小擾乱測定ベクトル
は以下のように与えられる。
【数５０】
また、行列（観測行列という）
は図１３に示すように与えられる。通常走行状態では図１３の行列内の（1）の行を観測行列として使用し、静止状態では（1）の行と（2）の行を観測行列として使用する。もしもGPS受信機測定が同時に得られるときには（1）の行又は（1）の行と（2）の行に（3）の行を合わせて観測行列として使用する（特願２００７−２３３５１５参照）。さらに、センサー構成によっては（４）の行を加える。
【００５６】
測定値が得られたとき、以下の（51）−(53)式の反復計算を、所定の条件を満足するまで繰り返す。
【数５１】
【数５２】
【数５３】
但し、上式において、
【数５４】
である。また、
は測定値に関するコバリアンス行列であり、使用する
に応じて変化する。すなわち、コバリアンス行列は通常走行状態であれば(51a)で表現され、静止状態であれば(51b)式で表現される。
【数５５】
GPS受信機測位が得られるときには、上記いずれかの走行状態に加え、GPS受信機の測定誤差に関するコバリアンスを以下のように付け加える。
【数５６】
以上の実施例では、カルマンフィルタ演算により状態量を補正する場合について説明したが、本発明はカルマンフィルタ処理に限らず同等の処理を行う補正方法にも適用することが出来る。
【図面の簡単な説明】
【００５７】
【図１】車速ベクトルＶを車両固定座標系のｘｙ平面(水平面)の速度成分ｖｘｙと上下方向(車両固定座標系のｚ軸方向) の速度成分ｖｚに分解する車速分解説明図である。
【図２】ｘｙ平面(水平面)の速度成分ｖｘｙを車両前後方向(車両固定座標系のｘ軸方向) の速度成分ｖｘと左右方向(車両固定座標系のｙ軸方向) の速度成分ｖｙに分解する車速分解説明図である。
【図３】本発明のナビゲーションシステムの全体の構成図である。
【図４】自律航法計算部の構成図である。
【図５】カルマンフィルタ補正部の構成図である。
【図６】従来技術と本発明の精度比較説明図表である。
【図７】方位誤差説明図である。
【図８】高度差説明図である。
【図９】自律航法計算部の変形例である。
【図１０】カルマンフィルタ補正部の変形例ある。
【図１１】行列Ｆの構成要素説明図である。
【図１２】行列Gkの構成要素説明図である。
【図１３】行列Ｈの構成要素説明図である。
【図１４】カルマンフィルタ処理の概要説明図である。
【図１５】ナビゲーションシステムにおける位置検出部の構成図である。
【図１６】センサーボードに固定したセンサー座標系Xs-Ys-Zs及び加速度計3軸（Acc x, Acc y, Acc z）およびジャイロスコープ3軸（Gyro x, Gyro y, Gyro z）から成る6自由度の基本システムのセンサー説明図である。
【図１７】車両固定座標系Xb-Yb-Zb及び地表固定座標系North-East-Down（NED座標系）の説明図である。
【図１８】図１５の位置検出部の全体の処理フローである。
【図１９】センサー姿勢ピッチ角及びセンサー姿勢ヨー角説明図である。
【図２０】種々のセンサー構成を示す第１の説明図である。
【図２１】種々のセンサー構成を示す第２の説明図である。
【図２２】従来技術におけるセンサー構成と精度との対応説明図である。
【符号の説明】
【００５８】
１０ センサーボード
１０a 加速度計
１０b ジャイロスコープ
１１ 車速センサー
１２ 自律航法計算部
１３ GPS受信機
１４ カルマンフィルタ演算部
１５ センサー・コンフィギュレーション・コントローラ（SCコントローラ）
２１ 状態量更新部
２２ 変化量演算部
２３ 移動量ベクトル算出部
３１ 状態量補正データ演算部
３３ 測定ベクトル発生部
３４ 速度ベクトル算出部
【特許請求の範囲】
【請求項１】
車両の位置を検出するナビゲーションシステムの位置検出装置において、
車両の速度を検出する車速センサーと、車両の加速度を検出する加速度センサーと、車両の角速度を検出する角速度センサーとを含むセンサー部、
これらセンサーから出力される信号を用いて所定の周期で車両の現在位置、車両速度、車体の姿勢角を含む状態量を計算する自律航法計算部を備え、
前記自律航法計算部は、
前記車速センサーから出力される信号を用いて前回の状態量計算時刻から今回の状態量計算時刻までに移動した移動量を計算する移動量算出部、
車体の姿勢変化量を計算する変化量検出部、
前記姿勢変化量を用いて前記移動量を車体の前後方向、車体の左右方向、車体の上下方向の方向成分に分解する移動量分解部、
前記各方向成分を用いて今回の状態量計算時刻における車両の位置を計算する状態量計算部、
を備えたことを特徴とする位置検出装置。
【請求項２】
前記変化量検出部は、前記車体の姿勢変化量として、車体のピッチ角の変化量と車体のヨー角の変化量を検出する、
ことを特徴とする請求項１記載の位置検出装置。
【請求項３】
前記変化量検出部は、前記状態量計算部が計算した車体の姿勢角を用いて前記車体のピッチ角の変化量と車体のヨー角の変化量を計算する、
ことを特徴とする請求項２記載の位置検出装置。
【請求項４】
車体のピッチ角変化量と車体のヨー角変化量をそれぞれ
とし、移動量をNｐLとすれば、前記移動量分解部は該移動量を次式
【数１】
により車体の前後方向、車体の左右方向、車体の上下方向の方向成分に分解する、
ことを特徴とする請求項２記載の位置検出装置。
【請求項５】
前記変化量検出部は、前記車体の姿勢変化量として、車体のヨー角の変化量を検出する、
ことを特徴とする請求項１記載の位置検出装置。
【請求項６】
前記変化量検出部は、前記状態量計算部が計算した車体の姿勢角を用いて前記車体のヨー角の変化量を計算する、
ことを特徴とする請求項５記載の位置検出装置
【請求項７】
車体のヨー角変化量を
とし、移動量をNｐLとすれば、前記移動量分解部は次式
【数２】
により前記移動量を車体の前後方向、車体の左右方向、車体の上下方向の方向成分に分解する、
ことを特徴とする請求項４記載の位置検出装置。
【請求項８】
前記位置検出装置は、前記車速センサーの出力信号より測定した車両の移動速度と前記自律航法計算部が計算した前記状態量とを用いて、該自律航法計算部が状態量を計算する前記周期より長い第２の周期で状態量補正処理を実行して該自律航法計算部が計算する状態量を補正する状態量補正部、
を備え、前記状態量補正部は、
前記姿勢変化量を用いて前記測定した移動速度を車体の前後方向、車体の左右方向、車体の上下方向の方向成分に分解する速度分解部、
前記各方向の速度成分と前記自律航法計算部が計算した前記状態量を用いて補正状態量を計算する補正状態量計算部、
を備えたことを特徴とする請求項１記載の位置検出装置。
【請求項９】
前記状態量補正部はカルマンフィルタ処理により補正状態量を計算するカルマンフィルタ補正部である、
ことを特徴とする請求項８記載の位置検出装置。
【請求項１０】
前記姿勢変化量が車体のピッチ角の変化量と車体のヨー角の変化量であり、該車体のピッチ角変化量と車体のヨー角変化量をそれぞれ
とし、前記測定した移動速度をVとすれば、前記速度分解部は次式
【数３】
により該移動速度を車体の前後方向、車体の左右方向、車体の上下方向の方向成分に分解する、
ことを特徴とする請求項８または９記載の位置検出装置。
【請求項１１】
前記姿勢変化量が車体のヨー角の変化量であり、該車体のヨー角変化量を
とし、前記測定した移動速度をVとすれば、前記速度分解部は次式
【数４】
により該移動速度を車体の前後方向、車体の左右方向、車体の上下方向の方向成分に分解する、
ことを特徴とする請求項８または９記載の位置検出装置。
【請求項１２】
車両の速度を検出する車速センサーと、車両の加速度を検出する加速度センサーと、車両の角速度を検出する角速度センサーから出力される信号を用いて、所定の周期で車両の現在位置、車両速度、車体の姿勢角を含む状態量を自律航法により計算するナビゲーションシステムの位置検出方法において、
前記車速センサーから出力される信号を用いて前回の状態量計算時刻から今回の状態量計算時刻までに移動した移動量を計算する第１ステップ、
車体の姿勢変化量を計算する第２ステップ、
前記姿勢変化量を用いて前記移動量を車体の前後方向、車体の左右方向、車体の上下方向の方向成分に分解する第３ステップ、
前記各方向成分を用いて今回の状態量計算時刻における車両の位置を計算する第４ステップ、
を備えたことを特徴とする位置検出方法。
【請求項１３】
前記第２ステップにおいて、前記車体の姿勢変化量として、車体のピッチ角の変化量と車体のヨー角の変化量を検出する、
ことを特徴とする請求項１２記載の位置検出方法。
【請求項１４】
前記第２ステップにおいて、前記自律航法により計算した車体の姿勢角を用いて前記車体のピッチ角の変化量と車体のヨー角の変化量を計算する、
ことを特徴とする請求項１３記載の位置検出方法。
【請求項１５】
車体のピッチ角変化量と車体のヨー角変化量をそれぞれ
とし、移動量をNｐLとすれば、前記第３ステップにおいて該移動量を次式
【数５】
により車体の前後方向、車体の左右方向、車体の上下方向の方向成分に分解する、
ことを特徴とする請求項１３記載の位置検出方法。
【請求項１６】
前記第２ステップにおいて、前記車体の姿勢変化量として、車体のヨー角の変化量を検出する、
ことを特徴とする請求項１２記載の位置検出方法。
【請求項１７】
前記第２ステップにおいて、前記自律航法により計算した車体の姿勢角を用いて前記車体のヨー角の変化量を計算する、
ことを特徴とする請求項１６記載の位置検出方法。
【請求項１８】
車体のヨー角変化量を
とし、移動量をNｐLとすれば、前記第３ステップにおいて該移動量を次式
【数６】
により車体の前後方向、車体の左右方向、車体の上下方向の方向成分に分解する、
ことを特徴とする請求項１５記載の位置検出方法。
【請求項１９】
前記位置検出方法は更に、前記自律航法により状態量を計算する前記周期より長い第２の周期で、前記車速センサーの出力信号より車両の移動速度を測定する第５ステップ、
該測定した移動速度と前記自律航法により計算した状態量とを用いて状態量補正処理を実行し、該自律航法により計算する状態量を補正する第６ステップ、
を備え、該第６ステップは、
前記姿勢変化量を用いて前記測定した移動速度を車体の前後方向、車体の左右方向、車体の上下方向の方向成分に分解する分解ステップ、
前記各方向の速度成分と前記自律航法計算部が計算した前記状態量を用いて補正状態量を計算する計算ステップ、
を備えたことを特徴とする請求項１２記載の位置検出方法。
【請求項２０】
前記状態量補正をカルマンフィルタ処理により行う、
ことを特徴とする請求項１９記載の位置検出方法。
【請求項２１】
前記姿勢変化量が車体のピッチ角の変化量と車体のヨー角の変化量であり、該車体のピッチ角変化量と車体のヨー角変化量をそれぞれ
とし、前記測定した移動速度をVとすれば、前記分解ステップにおいて該移動速度を次式
【数７】
により車体の前後方向、車体の左右方向、車体の上下方向の方向成分に分解する、
ことを特徴とする請求項１９または２０記載の位置検出方法。
【請求項２２】
前記姿勢変化量が車体のヨー角の変化量であり、該車体のヨー角変化量を
とし、前記測定した移動速度をVとすれば、前記分解ステップにおいて該移動速度を次式
【数８】
により車体の前後方向、車体の左右方向、車体の上下方向の方向成分に分解する、
ことを特徴とする請求項１９または２０記載の位置検出方法。
【請求項１】
車両の位置を検出するナビゲーションシステムの位置検出装置において、
車両の速度を検出する車速センサーと、車両の加速度を検出する加速度センサーと、車両の角速度を検出する角速度センサーとを含むセンサー部、
これらセンサーから出力される信号を用いて所定の周期で車両の現在位置、車両速度、車体の姿勢角を含む状態量を計算する自律航法計算部を備え、
前記自律航法計算部は、
前記車速センサーから出力される信号を用いて前回の状態量計算時刻から今回の状態量計算時刻までに移動した移動量を計算する移動量算出部、
車体の姿勢変化量を計算する変化量検出部、
前記姿勢変化量を用いて前記移動量を車体の前後方向、車体の左右方向、車体の上下方向の方向成分に分解する移動量分解部、
前記各方向成分を用いて今回の状態量計算時刻における車両の位置を計算する状態量計算部、
を備えたことを特徴とする位置検出装置。
【請求項２】
前記変化量検出部は、前記車体の姿勢変化量として、車体のピッチ角の変化量と車体のヨー角の変化量を検出する、
ことを特徴とする請求項１記載の位置検出装置。
【請求項３】
前記変化量検出部は、前記状態量計算部が計算した車体の姿勢角を用いて前記車体のピッチ角の変化量と車体のヨー角の変化量を計算する、
ことを特徴とする請求項２記載の位置検出装置。
【請求項４】
車体のピッチ角変化量と車体のヨー角変化量をそれぞれ
とし、移動量をNｐLとすれば、前記移動量分解部は該移動量を次式
【数１】
により車体の前後方向、車体の左右方向、車体の上下方向の方向成分に分解する、
ことを特徴とする請求項２記載の位置検出装置。
【請求項５】
前記変化量検出部は、前記車体の姿勢変化量として、車体のヨー角の変化量を検出する、
ことを特徴とする請求項１記載の位置検出装置。
【請求項６】
前記変化量検出部は、前記状態量計算部が計算した車体の姿勢角を用いて前記車体のヨー角の変化量を計算する、
ことを特徴とする請求項５記載の位置検出装置
【請求項７】
車体のヨー角変化量を
とし、移動量をNｐLとすれば、前記移動量分解部は次式
【数２】
により前記移動量を車体の前後方向、車体の左右方向、車体の上下方向の方向成分に分解する、
ことを特徴とする請求項４記載の位置検出装置。
【請求項８】
前記位置検出装置は、前記車速センサーの出力信号より測定した車両の移動速度と前記自律航法計算部が計算した前記状態量とを用いて、該自律航法計算部が状態量を計算する前記周期より長い第２の周期で状態量補正処理を実行して該自律航法計算部が計算する状態量を補正する状態量補正部、
を備え、前記状態量補正部は、
前記姿勢変化量を用いて前記測定した移動速度を車体の前後方向、車体の左右方向、車体の上下方向の方向成分に分解する速度分解部、
前記各方向の速度成分と前記自律航法計算部が計算した前記状態量を用いて補正状態量を計算する補正状態量計算部、
を備えたことを特徴とする請求項１記載の位置検出装置。
【請求項９】
前記状態量補正部はカルマンフィルタ処理により補正状態量を計算するカルマンフィルタ補正部である、
ことを特徴とする請求項８記載の位置検出装置。
【請求項１０】
前記姿勢変化量が車体のピッチ角の変化量と車体のヨー角の変化量であり、該車体のピッチ角変化量と車体のヨー角変化量をそれぞれ
とし、前記測定した移動速度をVとすれば、前記速度分解部は次式
【数３】
により該移動速度を車体の前後方向、車体の左右方向、車体の上下方向の方向成分に分解する、
ことを特徴とする請求項８または９記載の位置検出装置。
【請求項１１】
前記姿勢変化量が車体のヨー角の変化量であり、該車体のヨー角変化量を
とし、前記測定した移動速度をVとすれば、前記速度分解部は次式
【数４】
により該移動速度を車体の前後方向、車体の左右方向、車体の上下方向の方向成分に分解する、
ことを特徴とする請求項８または９記載の位置検出装置。
【請求項１２】
車両の速度を検出する車速センサーと、車両の加速度を検出する加速度センサーと、車両の角速度を検出する角速度センサーから出力される信号を用いて、所定の周期で車両の現在位置、車両速度、車体の姿勢角を含む状態量を自律航法により計算するナビゲーションシステムの位置検出方法において、
前記車速センサーから出力される信号を用いて前回の状態量計算時刻から今回の状態量計算時刻までに移動した移動量を計算する第１ステップ、
車体の姿勢変化量を計算する第２ステップ、
前記姿勢変化量を用いて前記移動量を車体の前後方向、車体の左右方向、車体の上下方向の方向成分に分解する第３ステップ、
前記各方向成分を用いて今回の状態量計算時刻における車両の位置を計算する第４ステップ、
を備えたことを特徴とする位置検出方法。
【請求項１３】
前記第２ステップにおいて、前記車体の姿勢変化量として、車体のピッチ角の変化量と車体のヨー角の変化量を検出する、
ことを特徴とする請求項１２記載の位置検出方法。
【請求項１４】
前記第２ステップにおいて、前記自律航法により計算した車体の姿勢角を用いて前記車体のピッチ角の変化量と車体のヨー角の変化量を計算する、
ことを特徴とする請求項１３記載の位置検出方法。
【請求項１５】
車体のピッチ角変化量と車体のヨー角変化量をそれぞれ
とし、移動量をNｐLとすれば、前記第３ステップにおいて該移動量を次式
【数５】
により車体の前後方向、車体の左右方向、車体の上下方向の方向成分に分解する、
ことを特徴とする請求項１３記載の位置検出方法。
【請求項１６】
前記第２ステップにおいて、前記車体の姿勢変化量として、車体のヨー角の変化量を検出する、
ことを特徴とする請求項１２記載の位置検出方法。
【請求項１７】
前記第２ステップにおいて、前記自律航法により計算した車体の姿勢角を用いて前記車体のヨー角の変化量を計算する、
ことを特徴とする請求項１６記載の位置検出方法。
【請求項１８】
車体のヨー角変化量を
とし、移動量をNｐLとすれば、前記第３ステップにおいて該移動量を次式
【数６】
により車体の前後方向、車体の左右方向、車体の上下方向の方向成分に分解する、
ことを特徴とする請求項１５記載の位置検出方法。
【請求項１９】
前記位置検出方法は更に、前記自律航法により状態量を計算する前記周期より長い第２の周期で、前記車速センサーの出力信号より車両の移動速度を測定する第５ステップ、
該測定した移動速度と前記自律航法により計算した状態量とを用いて状態量補正処理を実行し、該自律航法により計算する状態量を補正する第６ステップ、
を備え、該第６ステップは、
前記姿勢変化量を用いて前記測定した移動速度を車体の前後方向、車体の左右方向、車体の上下方向の方向成分に分解する分解ステップ、
前記各方向の速度成分と前記自律航法計算部が計算した前記状態量を用いて補正状態量を計算する計算ステップ、
を備えたことを特徴とする請求項１２記載の位置検出方法。
【請求項２０】
前記状態量補正をカルマンフィルタ処理により行う、
ことを特徴とする請求項１９記載の位置検出方法。
【請求項２１】
前記姿勢変化量が車体のピッチ角の変化量と車体のヨー角の変化量であり、該車体のピッチ角変化量と車体のヨー角変化量をそれぞれ
とし、前記測定した移動速度をVとすれば、前記分解ステップにおいて該移動速度を次式
【数７】
により車体の前後方向、車体の左右方向、車体の上下方向の方向成分に分解する、
ことを特徴とする請求項１９または２０記載の位置検出方法。
【請求項２２】
前記姿勢変化量が車体のヨー角の変化量であり、該車体のヨー角変化量を
とし、前記測定した移動速度をVとすれば、前記分解ステップにおいて該移動速度を次式
【数８】
により車体の前後方向、車体の左右方向、車体の上下方向の方向成分に分解する、
ことを特徴とする請求項１９または２０記載の位置検出方法。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図１６】
【図１７】
【図１８】
【図１９】
【図２０】
【図２１】
【図２２】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図１６】
【図１７】
【図１８】
【図１９】
【図２０】
【図２１】
【図２２】
【公開番号】特開２０１０−３２３９８（Ｐ２０１０−３２３９８Ａ）
【公開日】平成２２年２月１２日（２０１０．２．１２）
【国際特許分類】
【出願番号】特願２００８−１９５５７７（Ｐ２００８−１９５５７７）
【出願日】平成２０年７月３０日（２００８．７．３０）
【出願人】（０００１０１７３２）アルパイン株式会社 (2,424)
【Ｆターム（参考）】
【公開日】平成２２年２月１２日（２０１０．２．１２）
【国際特許分類】
【出願日】平成２０年７月３０日（２００８．７．３０）
【出願人】（０００１０１７３２）アルパイン株式会社 (2,424)
【Ｆターム（参考）】
[ Back to top ]