カルマンフィルタ処理における繰り返し演算制御方法及び装置
【目的】少ない繰り返し演算回数で高精度の位置、速度等の推定を可能にする「カルマンフィルタ処理における繰り返し演算制御方法及び装置」を提供することである。
【構成】車両の位置、速度及びセンサーの姿勢を含む状態量を推定するカルマンフィルタ処理において、カルマンフィルタ処理部１４は車両の位置、速度及びセンサーの姿勢を含む状態量を演算し、カルマンフィルタ繰り返し演算停止制御部２２は、演算された状態量のうち所定の状態量あるいは該状態量に応じた値について前回と今回との差分を演算し、該差分が前回の差分より大きくなったとき、あるいは、該差分が設定値以下になったとき、カルマンフィルタ処理部１４による状態量の繰り返し演算を停止し、補正状態量出力部21はカルマンフィルタ処理部１４により演算された状態量を補正状態量として出力する
【構成】車両の位置、速度及びセンサーの姿勢を含む状態量を推定するカルマンフィルタ処理において、カルマンフィルタ処理部１４は車両の位置、速度及びセンサーの姿勢を含む状態量を演算し、カルマンフィルタ繰り返し演算停止制御部２２は、演算された状態量のうち所定の状態量あるいは該状態量に応じた値について前回と今回との差分を演算し、該差分が前回の差分より大きくなったとき、あるいは、該差分が設定値以下になったとき、カルマンフィルタ処理部１４による状態量の繰り返し演算を停止し、補正状態量出力部21はカルマンフィルタ処理部１４により演算された状態量を補正状態量として出力する
【発明の詳細な説明】
【技術分野】
【０００１】
本発明はカルマンフィルタ処理における繰り返し演算制御方法及び装置に関わり、特に、車両の位置、速度及びセンサーの姿勢を含む状態量を推定するカルマンフィルタ処理における繰り返し演算を制御する繰り返し制御方法及び繰り返し演算制御装置に関する。
【背景技術】
【０００２】
自律航法システムは、車両の距離センサーである車速パルスセンサーや、加速度計と角速度計(ジャイロスコープ)などの慣性センサーから成り、動体である車両の位置、速度、及びセンサー姿勢の推定のために広く使用されている。
【０００３】
慣性航法システム（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は各軸速度、各軸位置、センサー姿勢等の状態量を含み後述するように
【数２】
と表現できる。観測値が得られる周期より短い周期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の分散である。添字の（・）T は転置行列を意味し、(・)-1は逆行列を意味する。また、Ｉは単位行列である。さらに、ＶとＷは平均０の白色ガウス雑音であり、互いに無相関である。上記のようなカルマンフィルタにおいて、状態量Ｘと誤差共分散Pの初期値に適当な誤差を与えてやり、新しい観測が行われる度に(4)式により最適推定値を求め、以後、(5a)式により、状態量Ｘを計算することにより該状態量の精度を向上することができる。
【００１０】
ところで、(4)式により得られる状態量は1回の演算では誤差が大きい。このため、繰り返し演算を行なうことにより状態量を誤差の少ない値に収束させることが行なわれている。観測値が得られるたびに反復計算を行うこの方法は、反復拡張カルマンフィルタ（Iterated Extended Kalman Filter）と呼ばれる周知の方法である(非特許文献１)。
しかし、繰り返し演算(イタレーション)回数が多いほど、状態量の収束は早くなるが、逆に時間（処理負荷）がかかるという問題がある。このため、従来は、イタレーション処理中、状態量の前回補正量dx1と今回補正量dx0の差分（補正効果）をチェックし、補正効果が閾値（0に非常に近い値）TH以下になった場合、十分に収束したとみなし、イタレーション終了していた。
【００１１】
カルマンフィルタ処理により状態量を繰り返し補正すると、徐々に真値に近づくため、通常はイタレーションを実行する度に、補正効果（補正量）は0に近づいていく。しかし、実際の動きとして、補正効果が前回より大きくなったりして閾値以下にならない場合がある。図１５はセンサー(加速度計及びジャイロ)の姿勢を表わす状態量より計算されるセンサー姿勢値c10の補正量dc10とイタレーション回数との関係特性A、補正量dc10の変化量とイタレーション回数の関係特性Bであり、補正量dc10の変化量（特性Ｂ）は大きくなったり、小さくなったりして補正値変化量の閾値TH以下にならない。この現象が発生すると、いつまでたってもイタレーションが終了せず、カルマンフィルタにおける演算量が増大すると共に、補正した状態量を出力できない問題が生じる。この根本原因は不明であるが、状態量をfloat型で扱うため、数値の桁数が少なく（荒く）なり、補正量の誤差が大きくなることが考えられる。
【００１２】
従来技術として、GPS測位位置又は自律航法測位位置のいずれか、及び、過去の自律航法測位位置の履歴に基づいて予測した予測位置を用いて移動体の位置をカルマンフィルタにより推定する技術がある（特許文献２）。また、電波航法で得られる絶対測位情報が高信頼度であれば、電波航法による絶対測位情報を利用して、自律航法による測位結果の測定誤差を推定し、電波航法による絶対測位情報が高信頼度でない場合は、捕捉衛星の中から受信パワーや仰角がしきい値よりも大きい高信頼度衛星を選択し、この高信頼度衛星との間の擬似距離及びドップラ周波数を算出しこれを利用して、自律航法による測位結果の測定誤差を推定する従来技術（特許文献３）がある。
しかし、これらの従来技術はカルマンフィルタの少ない繰り返し回数で高精度の位置、速度等の推定を可能にするものではない。
【特許文献１】特許３４７３１１７号
【特許文献２】特開２００７−３３３６５２号公報
【特許文献３】特開２００７−２４８２７１号公報
【非特許文献１】Gelb, A., Applied Optimal Estimation, M.I.T. Press, Cambridge, MA, 1974, pp. 182-91.
【発明の開示】
【発明が解決しようとする課題】
【００１３】
以上から、本発明の目的は、少ない繰り返し回数で高精度の位置、速度等の状態量の推定を可能にすることである。
本発明の別の目的は、補正量の変化量が閾値TH以下にならない場合であっても、繰り返し演算を停止してカルマンフィルタの演算量を減少することである。
【課題を解決するための手段】
【００１４】
本発明は車両の位置、速度及びセンサーの姿勢を含む状態量を推定するカルマンフィルタ処理における繰り返し演算制御方法及び繰り返し演算制御装置である。
・繰り返し演算制御方法
本発明のカルマンフィルタ処理における繰り返し演算制御方法は、車両の位置、速度及びセンサーの姿勢を含む状態量を演算するステップ、演算された状態量のうち所定の状態量あるいは該状態量に応じた値について前回と今回との差分を演算するステップ、該差分が前回の差分より大きくなったとき、あるいは、該差分が設定値以下になったとき、状態量の繰り返し演算を停止し、演算された状態量を出力するステップ、を有している。なお、前記状態量に応じた値は、加速度センサーの姿勢ピッチ角およびジャイロセンサの姿勢ヨー角に依存する状態量に応じた値である。
・繰り返し演算制御装置
本発明のカルマンフィルタ処理における繰り返し演算制御装置は、車両の位置、速度及びセンサーの姿勢を含む状態量を演算する状態量演算部、演算された状態量のうち所定の状態量あるいは該状態量に応じた値について前回と今回の差分を演算する差分演算部、該差分と前回の差分の大小を比較する比較部、該差分が設定値以下になったか否かを検出する検出部、該差分が前回の差分より大きくなったとき、あるいは、該差分が設定値以下になったとき、前記状態量の繰り返し演算を停止するよう制御すると共に、演算された状態量を出力するよう制御する制御部を備えている。
【発明の効果】
【００１５】
以上から本発明によれば、車両の位置、速度及びセンサーの姿勢を含む状態量を演算し、演算された状態量のうち所定の状態量あるいは該状態量に応じた値について前回と今回との差分を演算し、該差分が前回の差分より大きくなったとき、あるいは、該差分が設定値以下になったとき、状態量の繰り返し演算を停止し、演算された状態量を出力するように構成したから、補正効果が前回より大きくなったり、小さくなったりして閾値以下にならない場合でも、少ない繰り返し回数で高精度の位置、速度等の推定が可能になる。又、本発明によれば、補正量の変化量が閾値TH以下にならない場合であっても、繰り返し演算を停止するため、カルマンフィルタの演算量を減少することが出来、しかも状態量を出力することができる。
【発明を実施するための最良の形態】
【００１６】
（A）車載ナビゲーション装置の位置検出部の構成
図１は本発明を適用できる車載ナビゲーション装置の位置検出部の構成図である。自律系の入力信号としては、センサーボード１０上の慣性センサーから得られる信号と、別のケーブルを通じて車体から取り込まれる車速パルスがある。慣性センサーとしては加速度計(加速度３軸)１０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で提供する。
カルマンフィルタ演算部１４は車両の現在の位置、速度及び姿勢を含む車両状態量の補正を行う。例えば、カルマンフィルタ演算部１４は１０Hzの速度で、車速センサーから得られる速度(走行時)あるいはジャイロから出力される角速度オフセット(静止時)を観測値として用いて状態量補正処理を行って得られた補正状態量を自律航法計算部１２に入力する。又、カルマンフィルタ演算部１４は１Hzの速度でGPS受信機１３から得られる位置・速度を観測値として用いて状態量補正処理を行って得られた補正状態量を自律航法計算部１２に入力する。
【００１８】
（B）位置検出部の処理
図４は図１の位置検出部の全体の処理フローである。
はじめに、自律航法計算部１２に３次元車両位置N、E、D、車両速度Vsp、ピッチ角θ、車両取り付けピッチ角A、ヨー角Y、車両取り付けヨー角A2、ジャイロのオフセットωOF、加速度センサーのオフセットαOFの初期値を設定する（ステップ１０１）。ジャイロ１０aおよび加速度センサー１０bは、側面から見たとき、車両方向と平行して車両に取り付けられるのが理想であるが、図５（Ａ）に示すように取り付け誤差があり、センサー方向は車両方向に角度A（取り付けピッチ角）を成して取り付けられる。なお、水平方向とセンサー方向の角度θをピッチ角といい、ピッチ角は傾斜角と取り付けピッチ角の和である。また、ジャイロ１０aおよび加速度センサー１０bは、平面に投影したとき、車両方向と一致して車両に取り付けられるのが理想であるが、取り付け誤差があり、図５（Ｂ）に示すように、センサー方向は車両方向に角度A2（取り付けヨー角）を成して取り付けられる。なお、北方向とセンサー方向の角度Ｙをヨー角といい、ヨー角Ｙは車両方向と取り付けヨー角の和である。
【００１９】
以後、自律航法計算部１２はジャイロ１０a、加速度センサー１０b、車速センサー１１の出力を取り込み（ステップ１０２）、第１周期（２５Hz周期）で後述する計算式により(２)式に示す各軸速度、各軸位置、センサー姿勢等の状態量を計算して出力する(ステップ１０３)。ついで、第２周期(１０Hz周期)になったかチェックし(ステップ１０４)、第２周期になっていなければ、ステップ１０２以降の処理を繰り返す。
第２周期になっていれば、車両速度Vxが零である状態が2秒以上続いていたかどうかによって停車判定を行う（ステップ１０５）。
停車中でなければ、第３周期(1Hz周期=GPS測位周期)になっているかチェックし（ステップ１０６）、第３周期でなければカルマンフィルタ演算部１４は、車速センサー１１の出力より計算した車両速度(観測値)Vxと自律航法計算部１２が計算した車両速度を用いてカルマンフィルタ処理により車両状態量を補正し、補正状態量を自律航法計算部１２に入力する（ステップ１０７）。このステップ１０７では、後述するカルマンフィルタの観測行列H1を用いた第１補正処理が行われる。
【００２０】
ステップ１０６において、第３周期であればカルマンフィルタ演算部１４はGPSレシーバ１４が出力する３次元の車両位置と３次元の車両速度を観測値として用いて車両状態量を補正し、補正状態量を自律航法計算部１２に入力する(ステップ１０８)。このステップ１０８では後述するカルマンフィルタの観測行列H2を用いた第２補正処理が行われる。
ステップ１０５において、停車中であれば、第３周期(１Hz周期=GPS測位周期)になっているかチェックし（ステップ１０９）、第３周期でなければカルマンフィルタ演算部１４は前記ステップ１０７の補正処理を行うと共に、ジャイロの角速度出力信号(観測値)と自律航法計算部１２で計算した角速度信号オフセットにより車両状態量を補正し、補正状態量を自律航法計算部１２に入力する（ステップ１１０）。このステップ１１０では後述するカルマンフィルタの観測行列H3を用いた第３補正処理が行われる。
ステップ１０９において、第３周期であれば、カルマンフィルタ演算部１４は前記ステップ１０８の補正処理を行うと共に、ジャイロの角速度出力信号と自律航法計算部１２で計算した角速度信号オフセットとの差に基づいて角速度オフセット補正を行う（ステップ１１１）。このステップ１１１では後述するカルマンフィルタの観測行列H4を用いた第４補正処理が行われる。（ステップ１１１）
以上の位置検出処理によれば、GPSによる推定誤差補正より早い周波数で、誤差累積の補正を行うため精度の高い位置検出ができる。
【００２１】
（Ｃ）本発明の繰り返し演算制御装置
図６は本発明のカルマンフィルタ処理における繰り返し演算制御の構成図であり、カルマンフィルタ演算部１４は観測値が得られる毎に状態量（車両の位置、速度及びセンサーの姿勢を含む）を繰り返し演算し、補正状態量出力部２１はカルマンフィルタ繰り返し演算停止制御部２２からの指示により、カルマンフィルタ演算部１４が出力する補正状態量を自律航法計算部１２に出力する。カルマンフィルタ繰り返し演算停止制御部２２は、
(1)カルマンフィルタ演算部１４で演算された状態量のうち所定の状態量、あるいは該状態量に応じた値(例えばセンサー姿勢値)について前回と今回との差分を演算し、該差分が設定値以下になったとき、あるいは、
(2)該差分が前回の差分より大きくなったとき、あるいは
(3)繰り返し演算回数が設定最大回数に達したとき、
繰り返し演算停止信号CSPを発生してカルマンフィルタ演算部１４による状態量の繰り返し演算を停止する。すなわち、カルマンフィルタ繰り返し演算停止制御部２２は、上記(1)，(2)、(3)により状態量が収束したことを検出したとき、カルマンフィルタ演算部１４による状態量の繰り返し演算を停止制御すると共に、補正状態量出力部２１を制御して補正状態量を自律航法計算部１２に出力させる。
なお、前記状態量に応じた値は、例えばセンサー姿勢値ｃ10であり、加速度センサーの姿勢ピッチ角およびジャイロセンサの姿勢ヨー角に基づいて計算される。
【００２２】
(Ｄ) カルマンフィルタ処理部
図７はカルマンフィルタ処理部１４の構成図であり、カルマンゲイン計算部１４aは観測値が発生したとき、後述する状態量コバリアンスＰ、測定値コバリアンスＲ、観測行列Ｈ、ｋ番目の状態量xkを用いてｋ番目の状態量に応じたカルマンゲインＫk,i（ただし、iは繰り返し回数で、i=0,1,2….）を次式
【数７】
により計算する。上式において、
【数８】
である。カルマンゲインが求まれば、状態量補正部１４bは、ｋ番目の状態量を次式
【数９】
により計算し、状態量コバリアンス補正部１４cは状態量コバリアンスを次式
【数１０】
により補正してカルマンゲイン計算部１４aに入力する。測定値コバリアンス発生部１４dは測定値に関するコバリアンス行列Ｒkを発生し、観測行列発生部１４eは所定の観測行列Hkを発生する。
図７のカルマンフィルタ処理部１４は、カルマンフィルタ繰り返し演算停止制御部２２(図６)から繰り返し演算停止信号CSPが発生するまで、iを歩進しながら上記の(7)~(10)式の演算を繰り返し演算する。
【００２３】
（Ｅ）繰り返し演算停止制御
図８は図６のカルマンフィルタ繰り返し演算停止制御部の構成図である。ｃ10計算部２１aは、カルマンフィルタ処理部１４より状態量が計算されて出力される毎に、次式
【数１１】
によりセンサー姿勢値ｃ10を計算する。センサー姿勢ピッチ角は水平面に対するセンサーピッチ角(図５(Ａ）参照)、センサー姿勢ヨー角は北を基準方向としたときのセンサーヨー角(図５（Ｂ）)であり、それぞれ（２）式の状態量を構成する1つの要素である。状態量が収束しているか否かに対してセンサー姿勢値ｃ10が敏感であるため、このセンサー姿勢値ｃ10を状態量が収束しているか否かを判断するために使用する。演算部２１bにおいて、保持部３１は前回のセンサー姿勢値ｃ10を保存し、差演算部３２は前回と今回のセンサー姿勢値ｃ10の差dcを計算し、保存部３３は前回の差分dcを保存し、差演算部３４は今回の差分と前回の差分の差を演算すると共に、その大小を比較し、比較結果を繰り返し演算終了決定部２１cに入力する。繰り返し演算終了決定部２１cは、今回の差分が前回の差分より大きくなったとき、図１５の特性Ａ，Ｂで示すように差分は収束したものとみなして、繰り返し演算停止信号CSPを出力する。これにより、カルマンフィルタ処理部１４はカルマンフィルタ演算処理を停止し、また、補正状態量出力部２１は最後にカルマンフィルタ処理部１４から入力されている補正状態量を自律航法計算部１２に入力する。
【００２４】
以上と並行して、目標値到達検出部２１dは演算部３４により演算された今回の差分と前回の差分の差ddcが予め設定されている変化量閾値(図１５参照)TH以下になったか監視しており、繰り返し演算終了決定部２１cに監視結果を入力する。繰り返し演算終了決定部２１cは、差ddcが変化量閾値以下になれば（ddc<TH）、状態量は収束しているから繰り返し演算停止信号CSPを出力する。これにより、カルマンフィルタ処理部１４はカルマンフィルタ演算処理を停止し、また、補正状態量出力部２１は最後にカルマンフィルタ処理部１４から入力されている補正状態量を自律航法計算部１２に入力する。
また、最大繰り返し検出部２１eは繰り返し演算回数を監視しており、該繰り返し演算回数nを繰り返し演算終了決定部２１cに入力する。繰り返し演算終了決定部２１cは、繰り返し演算回数が設定最大演算回数（例えば２０回）に到達すれば、状態量は収束したものとみなして、繰り返し演算停止信号CSPを出力する。これにより、カルマンフィルタ処理部１４はカルマンフィルタ演算処理を停止し、また、補正状態量出力部２１は最後にカルマンフィルタ処理部１４から入力されている補正状態量を自律航法計算部１２に入力する。
【００２５】
（Ｆ）本発明の効果
図９は状態量が収束したとみなして繰り返し演算を終了したときの本発明と従来技術の繰り返し演算回数の頻度分布説明図であり、カルマンフィルタ処理を５０００回行なった場合である。従来の平均繰り返し回数は8.7回であったのが、本発明によれば平均繰り返し回数は4.8回とほぼ半減している。特に、従来技術では最大繰り返し回数まで繰り返し演算する頻度が非常に高いが、本発明によれば、最大繰り返し回数まで繰り返し演算する頻度はゼロ回である。この結果、本発明によれば時間のかかるカルマンフィルタ処理に要する演算回数を低減できる利点がある。
図１０は走行軌跡説明図で、(A)は従来技術の走行軌跡説明図、（B）は本発明の走行軌跡説明図であり、共に車両がENTより進入し、２つの螺旋駐車場を通ってEXTから脱出した走行軌跡を示している。なお、白丸が繋がって見える軌跡がカルマンフィルタ演算位置、不連続な丸がGPS３次元測位位置である。螺旋駐車場の外では走行軌跡とGPS３次元測位位置はほぼ一致している。又、本発明と従来技術の走行軌跡の精度はほぼ同じであり、本発明のようにカルマンフィルタ演算の処理負荷を削減しても精度劣化が生じない。
以上より、本発明によれば、カルマンフィルタの特性を利用し、計算結果（走行軌跡精度）をほとんど変えることなく、処理負荷を約50％以上削減できる。
以上では、自律航法計算部１２における自律航法計算アルゴリズムやカルマンフィルタ演算部１４における計算アルゴリズムの詳細説明をしなかったが、以下にその詳細を説明する。
【００２６】
（Ｇ）自律航法計算部における自律航法計算アルゴリズム
まず、基本となる６自由度システムの、高周波自律航法計算部３０のアルゴリズムについて説明する。なお、以降の式においては、
で示すように変数の上にドットがついているものは、その変数ｖsの時間変化率を表しているものとする。
【００２７】
（ａ）速度方程式
速度方程式は次式
【数１２】
で表現される。但し、
【数１３】
であり、(13a)はセンサー固定座標系で表現される速度ベクトル、(13b)はセンサー固定座標系で表現されるジャイロスコープ出力ベクトル、(13c)はセンサー固定座標系で表現される加速度計出力ベクトル,(13d)はセンサー固定座標系で表現される重力ベクトルである。ここで、
【数１４】
ここで、センサーの“感度”は、設置角の影響などを含めない、センサー自体の電圧出力から物理量への変換定数を意味する。通常センサーの生の出力は、出力がないことを示す０点電圧（しばしば２．５Ｖｏｌｔ）を中心とした電圧値（単位はＶｏｌｔ）であり、角度変化率や加速度などの物理量を得るためには、感度とよばれる変換用の定数を、出力電圧値と０点電圧の差に乗算する必要がある。感度は通常仕様値がセンサー毎に決められているが、低コストのセンサーの場合、実際の値が仕様値と大きく異なることや、温度補正が正確でないために感度が仕様値とずれるといったことがしばしば起こる。ことにジャイロに関する感度誤差はナビゲーションの方位計算に大きな影響を及ぼすので、このシステムではジャイロ感度をカルマンフィルタの状態量に含め、常時推定・補正を行う。
【００２８】
（ｂ） 姿勢方程式
姿勢方程式は次式で表現される。
【数１５】
ただし、
【数１５ａ】
はセンサー座標系からNED座標系への変換行列である。
（15）式は、慣性航法システム（Inertial Navigation System: INS）の一般的な手法である以下の姿勢方程式
【数１６】
から、車両ナビゲーション適用の際に独立な4つのパラメータだけを抜き出したものである。(16)式はより一般的なINS姿勢方程式である。
【００２９】
（ｃ） 位置方程式
位置方程式は次式
【数１７】
により表現できる。但し、(17)式において
【数１８】
であり、(18a)は位置ベクトル、(18b)は車両移動距離ベクトルである。又、(18b)式において、Np はサンプル時間当たりのパルス数、Lはパルス間距離である。
【００３０】
（ｄ） 固定パラメータ方程式
固定パラメータ方程式は次式
【数１９】
により表現される。但し、
【数２０】
であり、(20a)はセンサー座標系ジャイロスコープ出力に含まれるバイアス（Volt）、(20b)はセンサー座標系加速度計出力に含まれるバイアス（m/s2）、(20c)はセンサー座標系ジャイロスコープ出力に関する感度（rad/sec/Volt）、(20d)はセンサー座標系から車両座標系への変換行列である。また、(19）式中p00、p10、及びp20、は取り付け角に関する行列の要素から、独立な3つのパラメータだけを抜き出したものである。
【００３１】
（ｅ） 非線形状態方程式
上記（12）、(15)、(17)、（19）式をまとめて、簡単に（21）式の様に行列式で表すことができる
【数２１】
但し、ｘは以下に与えられる非線形状態量ベクトルである。
【数２１ａ】
【００３２】
（ｆ） 状態量更新
CPU上で(21)式を数値積分するには、例えば、次式の状態量更新式
【数２２】
を用いて行えばよい。精度要求の高い場合は、周知の数値積分方法であるルンゲ・クッタ4次式(下記文献参照)などを用いてもよい。
Kreyszig, E., Advanced Engineering Mathematics, John Wiley & Sons, 1999, New York, NY.
但し、(22)式において、Tはサンプルタイムで、例えば25HzならばT=0.04秒である。以上は一般的な慣性航法システム（INS）に用いられる手法を基に、MEMSセンサー使用を考慮して、地球の丸みなど微小な項を簡略化した車両用慣性航法システム（INS）である(下記文献参照)。
Hoshizaki, T., Computational Scheme for MEMS Inertial Navigation Systems, AOAMR Patent, August, 2006.
【００３３】
（ｇ） 微小擾乱方程式
さて、GPS受信機などの測定値をもとに、カルマンフィルタ補正部４０がカルマンフィルタ処理で状態量ｘを補正を行うためには、自律航法計算部３０が状態量を非線形状態方程式により更新を行い、かつ、誤差量が線形方程式によって増加すると仮定して該誤差量（微小擾乱量）のコバリアンス値も更新する必要がある。そのために、（22）式の線形化を行い、微小擾乱方程式である行列式（23）を用意する。
【数２３】
但し、
は以下に与えられる微小擾乱ベクトルである。
【数２４】
ただし、(24b)は
の現在推定値、(24c)はホワイトノイズでモデル化された3軸ジャイロスコープ出力
に含まれる高周波ノイズと、3軸加速度計出力
に含まれる高周波ノイズである。それぞれのノイズの標準偏差(σ）はセンサースペックや、測定実験などによって得られ、ここではそれらの大きさを次のようにＮで表す。
【数２５】
また、
に関する微小擾乱量であり、
には、慣性航法システム（INS）の一般的な手法に基づき、以下の関係がある。
【数２６】
上式より
δc10＝−δγ・c00＋δα・c20
が得られ、この式により図８のc10計算部２１aは状態量δγ、δαを用いてセンサー姿勢値を計算することが出来る。
また、
に関する微小擾乱量であり、
には、慣性航法システム（INS）の一般的な手法に基づき、以下の関係がある。
【数２７】
ここで車両に対するセンサーの取り付けロール角＝0と仮定すれば、取り付けロール角に関する補正量
となる。
(23)式における線形システムを表わす行列
はそれぞれ図１１、図１２に示すように得られる。ただし、図１１において、
【数２８】
【００３４】
（ｈ） コバリアンスの更新
以上のように得られた行列
を使って、カルマンフィルタ処理の定式に従い、高周波で以下のように微小擾乱ベクトル
のコバリアンス行列(誤差共分散行列)を次式により更新する。
【数２９】
但し、右肩のT は転地行列（Transpose）を意味し、右肩の−はカルマンフィルタの補正前の状態を意味し、右肩の＋はカルマンフィルタの補正後の状態を意味する。また、
の相関値の期待値（E[ ]）からなるコバリアンス行列であり、
に関するコバリアンス行列であり、それぞれ次式で表現される。
【数３０】
以上の（22）式及び（29）式が、6自由度の基本システムにおいて自律航法計算部１２が高周波で(例えば25Hzで)実行する演算式である。以下に(22)式及び（29）式を再掲する。
【数３１】
【００３５】
（Ｈ）カルマンフィルタ演算部の計算
次に、低周波の測定値入力に伴う、カルマンフィルタ補正部４０の計算方法について説明する。なお、以下の測定方程式で、
のように表されているのは、ホワイトノイズでモデル化された測定ノイズであり、その標準偏差は
の様に表されている。
【００３６】
（ａ）通常走行状態
通常走行状態においては、次式の測定方程式
【数３２】
を用いる。但し、
ｖxb_obs = 車速パルス間距離をパルス間隔で割ることで得られ、ｖyb_obs = 0、ｖzb_obs = 0である。これらの標準偏差は例えば
である。ただし、これらの値はデザイン値であり、ここに記した値は目安である。
【００３７】
（ｂ）静止状態
車速パルスが2秒以上入ってこないとき、車両が静止状態にあると判定する。静止状態において、測定値及びそれらの標準偏差は
であるとし、これに加えて以下の測定方程式を用いる。
【数３３】
である。
【００３８】
（ｃ）GPS受信機測定状態
以下のGPS受信機測定値が得られるときには、上記通常走行状態と静止状態のいずれかの測定に加え、以下のGPS受信機測定結果を利用する。
【数３４】
但し、測定値と測定誤差標準偏差はGPS受信機５０から得られる。
【００３９】
ロール角＝0の情報が送られてきたときには、次の測定方程式を使用する。
【数３５】
これは、
c21 = sin(センサーロール角)・cos(センサーピッチ角)
に相当し、c21 = 0を満たすことで、ロール角＝0が自動的に満たされるからである。
【００４０】
（ｄ）非線形測定方程式
上記（32）−（35）式をまとめて、簡単に（36）式の非線形測定方程式で示すように行列式で表すことができる。
【数３６】
但し、測定ベクトル
は以下のように与えられる。
【数３７】
【００４１】
（ｅ）線形測定方程式（カルマンフィルタの観測式）
カルマンフィルタで使用するために（36）式を線形化すると、次式の線形測定方程式（カルマンフィルタの観測式）が得られる。
【数３８】
但し、微小擾乱測定ベクトル
は以下のように与えられる。
【数３９】
また、行列（観測行列という）
は図１３に示すように与えられる。通常走行状態では図１３の行列内の（1）の行を観測行列として使用し、静止状態では（1）の行と（2）の行を観測行列として使用する。もしもGPS受信機測定が同時に得られるときには（1）の行又は（1）の行と（2）の行に（3）の行を合わせて観測行列として使用する（特願２００７−２３３５１５参照）。さらに、センサー構成によっては（４）の行を加える。
【００４２】
測定値が得られたとき、以下の（40）−(42)式の反復計算を、所定の条件を満足するまで繰り返す。本発明では、所定の状態量、あるいは該状態量に応じた値について前回と今回との差分を演算し、該差分が設定値以下になったとき、あるいは、該差分が前回の差分より大きくなったとき、繰り返し演算を停止する。なお、以下の（40）、(41)、(42)式は、図７のカルマンフィルタ処理部１４の説明に際して使用した（7）、(9)、(10）式と同じである。
【数４０】
【数４１】
【数４２】
但し、上式において、
【数４３】
である。また、
は測定値に関するコバリアンス行列であり、使用する
に応じて変化する。すなわち、コバリアンス行列は通常走行状態であれば(44a)で表現され、静止状態であれば(44b)式で表現される。
【数４４】
GPS受信機測位が得られるときには、上記いずれかの走行状態に加え、GPS受信機の測定誤差に関するコバリアンスを以下のように付け加える。
【数４５】
本発明によれば、カルマンフィルタの特性を利用し、計算結果（走行軌跡精度）をほとんど変えることなく、処理負荷を約50％以上削減できる。
以上では、所定の状態量に応じた値、すなわち、センサー姿勢値について前回と今回との差分を演算し、該差分が前回の差分より大きくなったとき、あるいは、該差分が設定値以下になったとき、状態量の繰り返し演算を停止し、演算された状態量を出力する場合について説明したが、所定の状態量について前回と今回との差分を演算し、該差分が前回の差分より大きくなったとき、あるいは、該差分が設定値以下になったとき、状態量の繰り返し演算を停止し、演算された状態量を出力するように構成することもできる。
【図面の簡単な説明】
【００４３】
【図１】本発明を適用できる車載ナビゲーション装置の位置検出部の構成図である。
【図２】センサーボードに固定したセンサー座標系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座標系）の説明図である。
【図４】図１の位置検出部の全体の処理フローである。
【図５】センサー姿勢ピッチ角及びセンサー姿勢ヨー角説明図である。
【図６】本発明のカルマンフィルタ処理における繰り返し演算制御の構成図である。
【図７】カルマンフィルタ演算部の構成図である。
【図８】図６のカルマンフィルタ繰り返し演算停止制御部の構成図である。
【図９】状態量が収束したとみなして繰り返し演算を終了したときの本発明と従来技術の繰り返し演算回数の頻度分布説明図である。
【図１０】走行軌跡説明図である。
【図１１】行列Ｆの構成要素説明図である。
【図１２】行列Gkの構成要素説明図である。
【図１３】行列Ｈの構成要素説明図である。
【図１４】カルマンフィルタ処理の概要説明図である。
【図１５】センサー姿勢角c10の補正量dc10とイタレーション回数との関係特性A、補正量dc10の変化量とイタレーション回数の関係特性Bである。
【符号の説明】
【００４４】
１４ カルマンフィルタ演算部
１４a カルマンゲイン計算部
１４b 状態量補正部
１４c 状態量コバリアンス補正部
１４d 測定値コバリアンス発生部
１４e 観測行列発生部
２１ 補正状態量出力部
２１a ｃ10計算部
２１b 演算部
２１c 繰り返し演算終了決定部
２１d 目標値到達検出部
２１e 最大繰り返し検出部
２２ カルマンフィルタ繰り返し演算停止制御部
【技術分野】
【０００１】
本発明はカルマンフィルタ処理における繰り返し演算制御方法及び装置に関わり、特に、車両の位置、速度及びセンサーの姿勢を含む状態量を推定するカルマンフィルタ処理における繰り返し演算を制御する繰り返し制御方法及び繰り返し演算制御装置に関する。
【背景技術】
【０００２】
自律航法システムは、車両の距離センサーである車速パルスセンサーや、加速度計と角速度計(ジャイロスコープ)などの慣性センサーから成り、動体である車両の位置、速度、及びセンサー姿勢の推定のために広く使用されている。
【０００３】
慣性航法システム（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は各軸速度、各軸位置、センサー姿勢等の状態量を含み後述するように
【数２】
と表現できる。観測値が得られる周期より短い周期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の分散である。添字の（・）T は転置行列を意味し、(・)-1は逆行列を意味する。また、Ｉは単位行列である。さらに、ＶとＷは平均０の白色ガウス雑音であり、互いに無相関である。上記のようなカルマンフィルタにおいて、状態量Ｘと誤差共分散Pの初期値に適当な誤差を与えてやり、新しい観測が行われる度に(4)式により最適推定値を求め、以後、(5a)式により、状態量Ｘを計算することにより該状態量の精度を向上することができる。
【００１０】
ところで、(4)式により得られる状態量は1回の演算では誤差が大きい。このため、繰り返し演算を行なうことにより状態量を誤差の少ない値に収束させることが行なわれている。観測値が得られるたびに反復計算を行うこの方法は、反復拡張カルマンフィルタ（Iterated Extended Kalman Filter）と呼ばれる周知の方法である(非特許文献１)。
しかし、繰り返し演算(イタレーション)回数が多いほど、状態量の収束は早くなるが、逆に時間（処理負荷）がかかるという問題がある。このため、従来は、イタレーション処理中、状態量の前回補正量dx1と今回補正量dx0の差分（補正効果）をチェックし、補正効果が閾値（0に非常に近い値）TH以下になった場合、十分に収束したとみなし、イタレーション終了していた。
【００１１】
カルマンフィルタ処理により状態量を繰り返し補正すると、徐々に真値に近づくため、通常はイタレーションを実行する度に、補正効果（補正量）は0に近づいていく。しかし、実際の動きとして、補正効果が前回より大きくなったりして閾値以下にならない場合がある。図１５はセンサー(加速度計及びジャイロ)の姿勢を表わす状態量より計算されるセンサー姿勢値c10の補正量dc10とイタレーション回数との関係特性A、補正量dc10の変化量とイタレーション回数の関係特性Bであり、補正量dc10の変化量（特性Ｂ）は大きくなったり、小さくなったりして補正値変化量の閾値TH以下にならない。この現象が発生すると、いつまでたってもイタレーションが終了せず、カルマンフィルタにおける演算量が増大すると共に、補正した状態量を出力できない問題が生じる。この根本原因は不明であるが、状態量をfloat型で扱うため、数値の桁数が少なく（荒く）なり、補正量の誤差が大きくなることが考えられる。
【００１２】
従来技術として、GPS測位位置又は自律航法測位位置のいずれか、及び、過去の自律航法測位位置の履歴に基づいて予測した予測位置を用いて移動体の位置をカルマンフィルタにより推定する技術がある（特許文献２）。また、電波航法で得られる絶対測位情報が高信頼度であれば、電波航法による絶対測位情報を利用して、自律航法による測位結果の測定誤差を推定し、電波航法による絶対測位情報が高信頼度でない場合は、捕捉衛星の中から受信パワーや仰角がしきい値よりも大きい高信頼度衛星を選択し、この高信頼度衛星との間の擬似距離及びドップラ周波数を算出しこれを利用して、自律航法による測位結果の測定誤差を推定する従来技術（特許文献３）がある。
しかし、これらの従来技術はカルマンフィルタの少ない繰り返し回数で高精度の位置、速度等の推定を可能にするものではない。
【特許文献１】特許３４７３１１７号
【特許文献２】特開２００７−３３３６５２号公報
【特許文献３】特開２００７−２４８２７１号公報
【非特許文献１】Gelb, A., Applied Optimal Estimation, M.I.T. Press, Cambridge, MA, 1974, pp. 182-91.
【発明の開示】
【発明が解決しようとする課題】
【００１３】
以上から、本発明の目的は、少ない繰り返し回数で高精度の位置、速度等の状態量の推定を可能にすることである。
本発明の別の目的は、補正量の変化量が閾値TH以下にならない場合であっても、繰り返し演算を停止してカルマンフィルタの演算量を減少することである。
【課題を解決するための手段】
【００１４】
本発明は車両の位置、速度及びセンサーの姿勢を含む状態量を推定するカルマンフィルタ処理における繰り返し演算制御方法及び繰り返し演算制御装置である。
・繰り返し演算制御方法
本発明のカルマンフィルタ処理における繰り返し演算制御方法は、車両の位置、速度及びセンサーの姿勢を含む状態量を演算するステップ、演算された状態量のうち所定の状態量あるいは該状態量に応じた値について前回と今回との差分を演算するステップ、該差分が前回の差分より大きくなったとき、あるいは、該差分が設定値以下になったとき、状態量の繰り返し演算を停止し、演算された状態量を出力するステップ、を有している。なお、前記状態量に応じた値は、加速度センサーの姿勢ピッチ角およびジャイロセンサの姿勢ヨー角に依存する状態量に応じた値である。
・繰り返し演算制御装置
本発明のカルマンフィルタ処理における繰り返し演算制御装置は、車両の位置、速度及びセンサーの姿勢を含む状態量を演算する状態量演算部、演算された状態量のうち所定の状態量あるいは該状態量に応じた値について前回と今回の差分を演算する差分演算部、該差分と前回の差分の大小を比較する比較部、該差分が設定値以下になったか否かを検出する検出部、該差分が前回の差分より大きくなったとき、あるいは、該差分が設定値以下になったとき、前記状態量の繰り返し演算を停止するよう制御すると共に、演算された状態量を出力するよう制御する制御部を備えている。
【発明の効果】
【００１５】
以上から本発明によれば、車両の位置、速度及びセンサーの姿勢を含む状態量を演算し、演算された状態量のうち所定の状態量あるいは該状態量に応じた値について前回と今回との差分を演算し、該差分が前回の差分より大きくなったとき、あるいは、該差分が設定値以下になったとき、状態量の繰り返し演算を停止し、演算された状態量を出力するように構成したから、補正効果が前回より大きくなったり、小さくなったりして閾値以下にならない場合でも、少ない繰り返し回数で高精度の位置、速度等の推定が可能になる。又、本発明によれば、補正量の変化量が閾値TH以下にならない場合であっても、繰り返し演算を停止するため、カルマンフィルタの演算量を減少することが出来、しかも状態量を出力することができる。
【発明を実施するための最良の形態】
【００１６】
（A）車載ナビゲーション装置の位置検出部の構成
図１は本発明を適用できる車載ナビゲーション装置の位置検出部の構成図である。自律系の入力信号としては、センサーボード１０上の慣性センサーから得られる信号と、別のケーブルを通じて車体から取り込まれる車速パルスがある。慣性センサーとしては加速度計(加速度３軸)１０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で提供する。
カルマンフィルタ演算部１４は車両の現在の位置、速度及び姿勢を含む車両状態量の補正を行う。例えば、カルマンフィルタ演算部１４は１０Hzの速度で、車速センサーから得られる速度(走行時)あるいはジャイロから出力される角速度オフセット(静止時)を観測値として用いて状態量補正処理を行って得られた補正状態量を自律航法計算部１２に入力する。又、カルマンフィルタ演算部１４は１Hzの速度でGPS受信機１３から得られる位置・速度を観測値として用いて状態量補正処理を行って得られた補正状態量を自律航法計算部１２に入力する。
【００１８】
（B）位置検出部の処理
図４は図１の位置検出部の全体の処理フローである。
はじめに、自律航法計算部１２に３次元車両位置N、E、D、車両速度Vsp、ピッチ角θ、車両取り付けピッチ角A、ヨー角Y、車両取り付けヨー角A2、ジャイロのオフセットωOF、加速度センサーのオフセットαOFの初期値を設定する（ステップ１０１）。ジャイロ１０aおよび加速度センサー１０bは、側面から見たとき、車両方向と平行して車両に取り付けられるのが理想であるが、図５（Ａ）に示すように取り付け誤差があり、センサー方向は車両方向に角度A（取り付けピッチ角）を成して取り付けられる。なお、水平方向とセンサー方向の角度θをピッチ角といい、ピッチ角は傾斜角と取り付けピッチ角の和である。また、ジャイロ１０aおよび加速度センサー１０bは、平面に投影したとき、車両方向と一致して車両に取り付けられるのが理想であるが、取り付け誤差があり、図５（Ｂ）に示すように、センサー方向は車両方向に角度A2（取り付けヨー角）を成して取り付けられる。なお、北方向とセンサー方向の角度Ｙをヨー角といい、ヨー角Ｙは車両方向と取り付けヨー角の和である。
【００１９】
以後、自律航法計算部１２はジャイロ１０a、加速度センサー１０b、車速センサー１１の出力を取り込み（ステップ１０２）、第１周期（２５Hz周期）で後述する計算式により(２)式に示す各軸速度、各軸位置、センサー姿勢等の状態量を計算して出力する(ステップ１０３)。ついで、第２周期(１０Hz周期)になったかチェックし(ステップ１０４)、第２周期になっていなければ、ステップ１０２以降の処理を繰り返す。
第２周期になっていれば、車両速度Vxが零である状態が2秒以上続いていたかどうかによって停車判定を行う（ステップ１０５）。
停車中でなければ、第３周期(1Hz周期=GPS測位周期)になっているかチェックし（ステップ１０６）、第３周期でなければカルマンフィルタ演算部１４は、車速センサー１１の出力より計算した車両速度(観測値)Vxと自律航法計算部１２が計算した車両速度を用いてカルマンフィルタ処理により車両状態量を補正し、補正状態量を自律航法計算部１２に入力する（ステップ１０７）。このステップ１０７では、後述するカルマンフィルタの観測行列H1を用いた第１補正処理が行われる。
【００２０】
ステップ１０６において、第３周期であればカルマンフィルタ演算部１４はGPSレシーバ１４が出力する３次元の車両位置と３次元の車両速度を観測値として用いて車両状態量を補正し、補正状態量を自律航法計算部１２に入力する(ステップ１０８)。このステップ１０８では後述するカルマンフィルタの観測行列H2を用いた第２補正処理が行われる。
ステップ１０５において、停車中であれば、第３周期(１Hz周期=GPS測位周期)になっているかチェックし（ステップ１０９）、第３周期でなければカルマンフィルタ演算部１４は前記ステップ１０７の補正処理を行うと共に、ジャイロの角速度出力信号(観測値)と自律航法計算部１２で計算した角速度信号オフセットにより車両状態量を補正し、補正状態量を自律航法計算部１２に入力する（ステップ１１０）。このステップ１１０では後述するカルマンフィルタの観測行列H3を用いた第３補正処理が行われる。
ステップ１０９において、第３周期であれば、カルマンフィルタ演算部１４は前記ステップ１０８の補正処理を行うと共に、ジャイロの角速度出力信号と自律航法計算部１２で計算した角速度信号オフセットとの差に基づいて角速度オフセット補正を行う（ステップ１１１）。このステップ１１１では後述するカルマンフィルタの観測行列H4を用いた第４補正処理が行われる。（ステップ１１１）
以上の位置検出処理によれば、GPSによる推定誤差補正より早い周波数で、誤差累積の補正を行うため精度の高い位置検出ができる。
【００２１】
（Ｃ）本発明の繰り返し演算制御装置
図６は本発明のカルマンフィルタ処理における繰り返し演算制御の構成図であり、カルマンフィルタ演算部１４は観測値が得られる毎に状態量（車両の位置、速度及びセンサーの姿勢を含む）を繰り返し演算し、補正状態量出力部２１はカルマンフィルタ繰り返し演算停止制御部２２からの指示により、カルマンフィルタ演算部１４が出力する補正状態量を自律航法計算部１２に出力する。カルマンフィルタ繰り返し演算停止制御部２２は、
(1)カルマンフィルタ演算部１４で演算された状態量のうち所定の状態量、あるいは該状態量に応じた値(例えばセンサー姿勢値)について前回と今回との差分を演算し、該差分が設定値以下になったとき、あるいは、
(2)該差分が前回の差分より大きくなったとき、あるいは
(3)繰り返し演算回数が設定最大回数に達したとき、
繰り返し演算停止信号CSPを発生してカルマンフィルタ演算部１４による状態量の繰り返し演算を停止する。すなわち、カルマンフィルタ繰り返し演算停止制御部２２は、上記(1)，(2)、(3)により状態量が収束したことを検出したとき、カルマンフィルタ演算部１４による状態量の繰り返し演算を停止制御すると共に、補正状態量出力部２１を制御して補正状態量を自律航法計算部１２に出力させる。
なお、前記状態量に応じた値は、例えばセンサー姿勢値ｃ10であり、加速度センサーの姿勢ピッチ角およびジャイロセンサの姿勢ヨー角に基づいて計算される。
【００２２】
(Ｄ) カルマンフィルタ処理部
図７はカルマンフィルタ処理部１４の構成図であり、カルマンゲイン計算部１４aは観測値が発生したとき、後述する状態量コバリアンスＰ、測定値コバリアンスＲ、観測行列Ｈ、ｋ番目の状態量xkを用いてｋ番目の状態量に応じたカルマンゲインＫk,i（ただし、iは繰り返し回数で、i=0,1,2….）を次式
【数７】
により計算する。上式において、
【数８】
である。カルマンゲインが求まれば、状態量補正部１４bは、ｋ番目の状態量を次式
【数９】
により計算し、状態量コバリアンス補正部１４cは状態量コバリアンスを次式
【数１０】
により補正してカルマンゲイン計算部１４aに入力する。測定値コバリアンス発生部１４dは測定値に関するコバリアンス行列Ｒkを発生し、観測行列発生部１４eは所定の観測行列Hkを発生する。
図７のカルマンフィルタ処理部１４は、カルマンフィルタ繰り返し演算停止制御部２２(図６)から繰り返し演算停止信号CSPが発生するまで、iを歩進しながら上記の(7)~(10)式の演算を繰り返し演算する。
【００２３】
（Ｅ）繰り返し演算停止制御
図８は図６のカルマンフィルタ繰り返し演算停止制御部の構成図である。ｃ10計算部２１aは、カルマンフィルタ処理部１４より状態量が計算されて出力される毎に、次式
【数１１】
によりセンサー姿勢値ｃ10を計算する。センサー姿勢ピッチ角は水平面に対するセンサーピッチ角(図５(Ａ）参照)、センサー姿勢ヨー角は北を基準方向としたときのセンサーヨー角(図５（Ｂ）)であり、それぞれ（２）式の状態量を構成する1つの要素である。状態量が収束しているか否かに対してセンサー姿勢値ｃ10が敏感であるため、このセンサー姿勢値ｃ10を状態量が収束しているか否かを判断するために使用する。演算部２１bにおいて、保持部３１は前回のセンサー姿勢値ｃ10を保存し、差演算部３２は前回と今回のセンサー姿勢値ｃ10の差dcを計算し、保存部３３は前回の差分dcを保存し、差演算部３４は今回の差分と前回の差分の差を演算すると共に、その大小を比較し、比較結果を繰り返し演算終了決定部２１cに入力する。繰り返し演算終了決定部２１cは、今回の差分が前回の差分より大きくなったとき、図１５の特性Ａ，Ｂで示すように差分は収束したものとみなして、繰り返し演算停止信号CSPを出力する。これにより、カルマンフィルタ処理部１４はカルマンフィルタ演算処理を停止し、また、補正状態量出力部２１は最後にカルマンフィルタ処理部１４から入力されている補正状態量を自律航法計算部１２に入力する。
【００２４】
以上と並行して、目標値到達検出部２１dは演算部３４により演算された今回の差分と前回の差分の差ddcが予め設定されている変化量閾値(図１５参照)TH以下になったか監視しており、繰り返し演算終了決定部２１cに監視結果を入力する。繰り返し演算終了決定部２１cは、差ddcが変化量閾値以下になれば（ddc<TH）、状態量は収束しているから繰り返し演算停止信号CSPを出力する。これにより、カルマンフィルタ処理部１４はカルマンフィルタ演算処理を停止し、また、補正状態量出力部２１は最後にカルマンフィルタ処理部１４から入力されている補正状態量を自律航法計算部１２に入力する。
また、最大繰り返し検出部２１eは繰り返し演算回数を監視しており、該繰り返し演算回数nを繰り返し演算終了決定部２１cに入力する。繰り返し演算終了決定部２１cは、繰り返し演算回数が設定最大演算回数（例えば２０回）に到達すれば、状態量は収束したものとみなして、繰り返し演算停止信号CSPを出力する。これにより、カルマンフィルタ処理部１４はカルマンフィルタ演算処理を停止し、また、補正状態量出力部２１は最後にカルマンフィルタ処理部１４から入力されている補正状態量を自律航法計算部１２に入力する。
【００２５】
（Ｆ）本発明の効果
図９は状態量が収束したとみなして繰り返し演算を終了したときの本発明と従来技術の繰り返し演算回数の頻度分布説明図であり、カルマンフィルタ処理を５０００回行なった場合である。従来の平均繰り返し回数は8.7回であったのが、本発明によれば平均繰り返し回数は4.8回とほぼ半減している。特に、従来技術では最大繰り返し回数まで繰り返し演算する頻度が非常に高いが、本発明によれば、最大繰り返し回数まで繰り返し演算する頻度はゼロ回である。この結果、本発明によれば時間のかかるカルマンフィルタ処理に要する演算回数を低減できる利点がある。
図１０は走行軌跡説明図で、(A)は従来技術の走行軌跡説明図、（B）は本発明の走行軌跡説明図であり、共に車両がENTより進入し、２つの螺旋駐車場を通ってEXTから脱出した走行軌跡を示している。なお、白丸が繋がって見える軌跡がカルマンフィルタ演算位置、不連続な丸がGPS３次元測位位置である。螺旋駐車場の外では走行軌跡とGPS３次元測位位置はほぼ一致している。又、本発明と従来技術の走行軌跡の精度はほぼ同じであり、本発明のようにカルマンフィルタ演算の処理負荷を削減しても精度劣化が生じない。
以上より、本発明によれば、カルマンフィルタの特性を利用し、計算結果（走行軌跡精度）をほとんど変えることなく、処理負荷を約50％以上削減できる。
以上では、自律航法計算部１２における自律航法計算アルゴリズムやカルマンフィルタ演算部１４における計算アルゴリズムの詳細説明をしなかったが、以下にその詳細を説明する。
【００２６】
（Ｇ）自律航法計算部における自律航法計算アルゴリズム
まず、基本となる６自由度システムの、高周波自律航法計算部３０のアルゴリズムについて説明する。なお、以降の式においては、
で示すように変数の上にドットがついているものは、その変数ｖsの時間変化率を表しているものとする。
【００２７】
（ａ）速度方程式
速度方程式は次式
【数１２】
で表現される。但し、
【数１３】
であり、(13a)はセンサー固定座標系で表現される速度ベクトル、(13b)はセンサー固定座標系で表現されるジャイロスコープ出力ベクトル、(13c)はセンサー固定座標系で表現される加速度計出力ベクトル,(13d)はセンサー固定座標系で表現される重力ベクトルである。ここで、
【数１４】
ここで、センサーの“感度”は、設置角の影響などを含めない、センサー自体の電圧出力から物理量への変換定数を意味する。通常センサーの生の出力は、出力がないことを示す０点電圧（しばしば２．５Ｖｏｌｔ）を中心とした電圧値（単位はＶｏｌｔ）であり、角度変化率や加速度などの物理量を得るためには、感度とよばれる変換用の定数を、出力電圧値と０点電圧の差に乗算する必要がある。感度は通常仕様値がセンサー毎に決められているが、低コストのセンサーの場合、実際の値が仕様値と大きく異なることや、温度補正が正確でないために感度が仕様値とずれるといったことがしばしば起こる。ことにジャイロに関する感度誤差はナビゲーションの方位計算に大きな影響を及ぼすので、このシステムではジャイロ感度をカルマンフィルタの状態量に含め、常時推定・補正を行う。
【００２８】
（ｂ） 姿勢方程式
姿勢方程式は次式で表現される。
【数１５】
ただし、
【数１５ａ】
はセンサー座標系からNED座標系への変換行列である。
（15）式は、慣性航法システム（Inertial Navigation System: INS）の一般的な手法である以下の姿勢方程式
【数１６】
から、車両ナビゲーション適用の際に独立な4つのパラメータだけを抜き出したものである。(16)式はより一般的なINS姿勢方程式である。
【００２９】
（ｃ） 位置方程式
位置方程式は次式
【数１７】
により表現できる。但し、(17)式において
【数１８】
であり、(18a)は位置ベクトル、(18b)は車両移動距離ベクトルである。又、(18b)式において、Np はサンプル時間当たりのパルス数、Lはパルス間距離である。
【００３０】
（ｄ） 固定パラメータ方程式
固定パラメータ方程式は次式
【数１９】
により表現される。但し、
【数２０】
であり、(20a)はセンサー座標系ジャイロスコープ出力に含まれるバイアス（Volt）、(20b)はセンサー座標系加速度計出力に含まれるバイアス（m/s2）、(20c)はセンサー座標系ジャイロスコープ出力に関する感度（rad/sec/Volt）、(20d)はセンサー座標系から車両座標系への変換行列である。また、(19）式中p00、p10、及びp20、は取り付け角に関する行列の要素から、独立な3つのパラメータだけを抜き出したものである。
【００３１】
（ｅ） 非線形状態方程式
上記（12）、(15)、(17)、（19）式をまとめて、簡単に（21）式の様に行列式で表すことができる
【数２１】
但し、ｘは以下に与えられる非線形状態量ベクトルである。
【数２１ａ】
【００３２】
（ｆ） 状態量更新
CPU上で(21)式を数値積分するには、例えば、次式の状態量更新式
【数２２】
を用いて行えばよい。精度要求の高い場合は、周知の数値積分方法であるルンゲ・クッタ4次式(下記文献参照)などを用いてもよい。
Kreyszig, E., Advanced Engineering Mathematics, John Wiley & Sons, 1999, New York, NY.
但し、(22)式において、Tはサンプルタイムで、例えば25HzならばT=0.04秒である。以上は一般的な慣性航法システム（INS）に用いられる手法を基に、MEMSセンサー使用を考慮して、地球の丸みなど微小な項を簡略化した車両用慣性航法システム（INS）である(下記文献参照)。
Hoshizaki, T., Computational Scheme for MEMS Inertial Navigation Systems, AOAMR Patent, August, 2006.
【００３３】
（ｇ） 微小擾乱方程式
さて、GPS受信機などの測定値をもとに、カルマンフィルタ補正部４０がカルマンフィルタ処理で状態量ｘを補正を行うためには、自律航法計算部３０が状態量を非線形状態方程式により更新を行い、かつ、誤差量が線形方程式によって増加すると仮定して該誤差量（微小擾乱量）のコバリアンス値も更新する必要がある。そのために、（22）式の線形化を行い、微小擾乱方程式である行列式（23）を用意する。
【数２３】
但し、
は以下に与えられる微小擾乱ベクトルである。
【数２４】
ただし、(24b)は
の現在推定値、(24c)はホワイトノイズでモデル化された3軸ジャイロスコープ出力
に含まれる高周波ノイズと、3軸加速度計出力
に含まれる高周波ノイズである。それぞれのノイズの標準偏差(σ）はセンサースペックや、測定実験などによって得られ、ここではそれらの大きさを次のようにＮで表す。
【数２５】
また、
に関する微小擾乱量であり、
には、慣性航法システム（INS）の一般的な手法に基づき、以下の関係がある。
【数２６】
上式より
δc10＝−δγ・c00＋δα・c20
が得られ、この式により図８のc10計算部２１aは状態量δγ、δαを用いてセンサー姿勢値を計算することが出来る。
また、
に関する微小擾乱量であり、
には、慣性航法システム（INS）の一般的な手法に基づき、以下の関係がある。
【数２７】
ここで車両に対するセンサーの取り付けロール角＝0と仮定すれば、取り付けロール角に関する補正量
となる。
(23)式における線形システムを表わす行列
はそれぞれ図１１、図１２に示すように得られる。ただし、図１１において、
【数２８】
【００３４】
（ｈ） コバリアンスの更新
以上のように得られた行列
を使って、カルマンフィルタ処理の定式に従い、高周波で以下のように微小擾乱ベクトル
のコバリアンス行列(誤差共分散行列)を次式により更新する。
【数２９】
但し、右肩のT は転地行列（Transpose）を意味し、右肩の−はカルマンフィルタの補正前の状態を意味し、右肩の＋はカルマンフィルタの補正後の状態を意味する。また、
の相関値の期待値（E[ ]）からなるコバリアンス行列であり、
に関するコバリアンス行列であり、それぞれ次式で表現される。
【数３０】
以上の（22）式及び（29）式が、6自由度の基本システムにおいて自律航法計算部１２が高周波で(例えば25Hzで)実行する演算式である。以下に(22)式及び（29）式を再掲する。
【数３１】
【００３５】
（Ｈ）カルマンフィルタ演算部の計算
次に、低周波の測定値入力に伴う、カルマンフィルタ補正部４０の計算方法について説明する。なお、以下の測定方程式で、
のように表されているのは、ホワイトノイズでモデル化された測定ノイズであり、その標準偏差は
の様に表されている。
【００３６】
（ａ）通常走行状態
通常走行状態においては、次式の測定方程式
【数３２】
を用いる。但し、
ｖxb_obs = 車速パルス間距離をパルス間隔で割ることで得られ、ｖyb_obs = 0、ｖzb_obs = 0である。これらの標準偏差は例えば
である。ただし、これらの値はデザイン値であり、ここに記した値は目安である。
【００３７】
（ｂ）静止状態
車速パルスが2秒以上入ってこないとき、車両が静止状態にあると判定する。静止状態において、測定値及びそれらの標準偏差は
であるとし、これに加えて以下の測定方程式を用いる。
【数３３】
である。
【００３８】
（ｃ）GPS受信機測定状態
以下のGPS受信機測定値が得られるときには、上記通常走行状態と静止状態のいずれかの測定に加え、以下のGPS受信機測定結果を利用する。
【数３４】
但し、測定値と測定誤差標準偏差はGPS受信機５０から得られる。
【００３９】
ロール角＝0の情報が送られてきたときには、次の測定方程式を使用する。
【数３５】
これは、
c21 = sin(センサーロール角)・cos(センサーピッチ角)
に相当し、c21 = 0を満たすことで、ロール角＝0が自動的に満たされるからである。
【００４０】
（ｄ）非線形測定方程式
上記（32）−（35）式をまとめて、簡単に（36）式の非線形測定方程式で示すように行列式で表すことができる。
【数３６】
但し、測定ベクトル
は以下のように与えられる。
【数３７】
【００４１】
（ｅ）線形測定方程式（カルマンフィルタの観測式）
カルマンフィルタで使用するために（36）式を線形化すると、次式の線形測定方程式（カルマンフィルタの観測式）が得られる。
【数３８】
但し、微小擾乱測定ベクトル
は以下のように与えられる。
【数３９】
また、行列（観測行列という）
は図１３に示すように与えられる。通常走行状態では図１３の行列内の（1）の行を観測行列として使用し、静止状態では（1）の行と（2）の行を観測行列として使用する。もしもGPS受信機測定が同時に得られるときには（1）の行又は（1）の行と（2）の行に（3）の行を合わせて観測行列として使用する（特願２００７−２３３５１５参照）。さらに、センサー構成によっては（４）の行を加える。
【００４２】
測定値が得られたとき、以下の（40）−(42)式の反復計算を、所定の条件を満足するまで繰り返す。本発明では、所定の状態量、あるいは該状態量に応じた値について前回と今回との差分を演算し、該差分が設定値以下になったとき、あるいは、該差分が前回の差分より大きくなったとき、繰り返し演算を停止する。なお、以下の（40）、(41)、(42)式は、図７のカルマンフィルタ処理部１４の説明に際して使用した（7）、(9)、(10）式と同じである。
【数４０】
【数４１】
【数４２】
但し、上式において、
【数４３】
である。また、
は測定値に関するコバリアンス行列であり、使用する
に応じて変化する。すなわち、コバリアンス行列は通常走行状態であれば(44a)で表現され、静止状態であれば(44b)式で表現される。
【数４４】
GPS受信機測位が得られるときには、上記いずれかの走行状態に加え、GPS受信機の測定誤差に関するコバリアンスを以下のように付け加える。
【数４５】
本発明によれば、カルマンフィルタの特性を利用し、計算結果（走行軌跡精度）をほとんど変えることなく、処理負荷を約50％以上削減できる。
以上では、所定の状態量に応じた値、すなわち、センサー姿勢値について前回と今回との差分を演算し、該差分が前回の差分より大きくなったとき、あるいは、該差分が設定値以下になったとき、状態量の繰り返し演算を停止し、演算された状態量を出力する場合について説明したが、所定の状態量について前回と今回との差分を演算し、該差分が前回の差分より大きくなったとき、あるいは、該差分が設定値以下になったとき、状態量の繰り返し演算を停止し、演算された状態量を出力するように構成することもできる。
【図面の簡単な説明】
【００４３】
【図１】本発明を適用できる車載ナビゲーション装置の位置検出部の構成図である。
【図２】センサーボードに固定したセンサー座標系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座標系）の説明図である。
【図４】図１の位置検出部の全体の処理フローである。
【図５】センサー姿勢ピッチ角及びセンサー姿勢ヨー角説明図である。
【図６】本発明のカルマンフィルタ処理における繰り返し演算制御の構成図である。
【図７】カルマンフィルタ演算部の構成図である。
【図８】図６のカルマンフィルタ繰り返し演算停止制御部の構成図である。
【図９】状態量が収束したとみなして繰り返し演算を終了したときの本発明と従来技術の繰り返し演算回数の頻度分布説明図である。
【図１０】走行軌跡説明図である。
【図１１】行列Ｆの構成要素説明図である。
【図１２】行列Gkの構成要素説明図である。
【図１３】行列Ｈの構成要素説明図である。
【図１４】カルマンフィルタ処理の概要説明図である。
【図１５】センサー姿勢角c10の補正量dc10とイタレーション回数との関係特性A、補正量dc10の変化量とイタレーション回数の関係特性Bである。
【符号の説明】
【００４４】
１４ カルマンフィルタ演算部
１４a カルマンゲイン計算部
１４b 状態量補正部
１４c 状態量コバリアンス補正部
１４d 測定値コバリアンス発生部
１４e 観測行列発生部
２１ 補正状態量出力部
２１a ｃ10計算部
２１b 演算部
２１c 繰り返し演算終了決定部
２１d 目標値到達検出部
２１e 最大繰り返し検出部
２２ カルマンフィルタ繰り返し演算停止制御部
【特許請求の範囲】
【請求項１】
車両の位置、速度及びセンサーの姿勢を含む状態量を推定するカルマンフィルタ処理における繰り返し演算制御方法において、
車両の位置、速度及びセンサーの姿勢を含む状態量を演算し、
演算された状態量のうち所定の状態量あるいは該状態量に応じた値について前回と今回との差分を演算し、
該差分が前回の差分より大きくなったとき、あるいは、該差分が設定値以下になったとき、状態量の繰り返し演算を停止し、演算された状態量を出力する、
ことを特徴とするカルマンフィルタ処理における繰り返し演算制御方法。
【請求項２】
前記状態量に応じた値は、加速度センサーの姿勢ピッチ角およびジャイロセンサの姿勢ヨー角に依存する状態量に応じた値であることを特徴とする請求項１記載のカルマンフィルタ処理における繰り返し演算制御方法。
【請求項３】
車両の位置、速度及びセンサーの姿勢を含む状態量を推定するカルマンフィルタ処理における繰り返し演算制御装置において、
車両の位置、速度及びセンサーの姿勢を含む状態量を演算する状態量演算部、
演算された状態量のうち所定の状態量あるいは該状態量に応じた値について前回と今回の差分を演算する差分演算部、
該差分と前回の差分の大小を比較する比較部、
該差分が設定値以下になったか否かを検出する検出部、
該差分が前回の差分より大きくなったとき、あるいは、該差分が設定値以下になったとき、前記状態量の繰り返し演算を停止するよう制御すると共に、演算された状態量を出力するよう制御する制御部、
を備えたことを特徴とするカルマンフィルタ処理における繰り返し演算制御装置。
【請求項１】
車両の位置、速度及びセンサーの姿勢を含む状態量を推定するカルマンフィルタ処理における繰り返し演算制御方法において、
車両の位置、速度及びセンサーの姿勢を含む状態量を演算し、
演算された状態量のうち所定の状態量あるいは該状態量に応じた値について前回と今回との差分を演算し、
該差分が前回の差分より大きくなったとき、あるいは、該差分が設定値以下になったとき、状態量の繰り返し演算を停止し、演算された状態量を出力する、
ことを特徴とするカルマンフィルタ処理における繰り返し演算制御方法。
【請求項２】
前記状態量に応じた値は、加速度センサーの姿勢ピッチ角およびジャイロセンサの姿勢ヨー角に依存する状態量に応じた値であることを特徴とする請求項１記載のカルマンフィルタ処理における繰り返し演算制御方法。
【請求項３】
車両の位置、速度及びセンサーの姿勢を含む状態量を推定するカルマンフィルタ処理における繰り返し演算制御装置において、
車両の位置、速度及びセンサーの姿勢を含む状態量を演算する状態量演算部、
演算された状態量のうち所定の状態量あるいは該状態量に応じた値について前回と今回の差分を演算する差分演算部、
該差分と前回の差分の大小を比較する比較部、
該差分が設定値以下になったか否かを検出する検出部、
該差分が前回の差分より大きくなったとき、あるいは、該差分が設定値以下になったとき、前記状態量の繰り返し演算を停止するよう制御すると共に、演算された状態量を出力するよう制御する制御部、
を備えたことを特徴とするカルマンフィルタ処理における繰り返し演算制御装置。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【公開番号】特開２００９−２５０７７８（Ｐ２００９−２５０７７８Ａ）
【公開日】平成２１年１０月２９日（２００９．１０．２９）
【国際特許分類】
【出願番号】特願２００８−９８８５６（Ｐ２００８−９８８５６）
【出願日】平成２０年４月７日（２００８．４．７）
【出願人】（０００１０１７３２）アルパイン株式会社 (2,424)
【Ｆターム（参考）】
【公開日】平成２１年１０月２９日（２００９．１０．２９）
【国際特許分類】
【出願日】平成２０年４月７日（２００８．４．７）
【出願人】（０００１０１７３２）アルパイン株式会社 (2,424)
【Ｆターム（参考）】
[ Back to top ]