(57)【特許請求の範囲】
【請求項１】
少なくともGPS受信機と慣性センサで構成されるGPS複合航法装置において、
移動体の少なくとも位置、速度及び方位のいずれかを演算する航法演算部と、
前記航法演算部出力と前記GPS受信機出力に基づいて、前記航法演算部出力の少なくとも位置、速度及び方位のいずれかの誤差を推定するカルマン・フィルタ誤差推定部と、
移動体の静止状態を判定する静止判定部とを備え、
前記静止判定部で静止状態と判定された場合に、カルマン・フィルタの観測更新に用いる移動時用観測モデルを静止時用観測モデルに変更すると共に、静止判定前後の誤差共分散行列の観測更新の変化量を用いて誤差共分散行列を補正することを特徴とするGPS複合航法装置。
【請求項２】
請求項１に記載のGPS複合航法装置において、
前記静止時用観測モデルの内、位置及び/或いは方位の観測量は、現在推定値と前回推定値との差とすることを特徴とするGPS複合航法装置。
【請求項３】
請求項１に記載のGPS複合航法装置において、
前記誤差共分散行列の補正は、誤差共分散行列の、静止判定前後で得られる現在観測更新値と前回観測更新値との差を補正量とし、該補正量を前記現在観測更新値、前記現在観測更新値の時間更新値またはダイナミック・モデルの誤差分散行列のいずれかに加減算することを特徴とするGPS複合航法装置。
【請求項４】
請求項１に記載のGPS複合航法装置において、
前記誤差共分散行列の補正は、該誤差共分散行列の対角要素である、静止判定前後で得られる現在観測更新値と前回観測更新値との差を補正量とし、該補正量を前記現在観測更新値、前記現在観測更新値の時間更新値またはダイナミック・モデルの誤差分散行列のいずれかの対角要素に加減算することを特徴とするGPS複合航法装置。
【請求項５】
請求項２に記載のGPS複合航法装置において、
前記前回推定値は、静止時の推定値または静止直前の推定値とすることを特徴とするGPS複合航法装置。
【請求項６】
請求項３または４に記載のGPS複合航法装置において、
前記前回観測更新値は、静止時の観測更新値または静止直前の観測更新値とすることを特徴とするGPS複合航法装置。
【請求項７】
請求項１に記載のGPS複合航法装置において、
前記航法演算部は少なくとも、前記GPS受信機出力の位置、移動体の進行方向成分の加速度を計測する加速度センサ及び移動体の方位軸回転成分を計測する角速度センサ出力に基づいて、移動体の位置、速度及び方位を演算することを特徴とするGPS複合航法装置。
【請求項８】
請求項７に記載のGPS複合航法装置において、
移動体の進行方向成分の速度を計測する速度センサを追加し、前記加速度センサ出力を前記速度センサ出力で置換することを特徴とするGPS複合航法装置。
【請求項９】
請求項２または５に記載のGPS複合航法装置において、
前記静止時用観測モデルの観測量として、速度及び/或いは移動体の方位軸回転成分角速度の観測量を追加し、前記速度及び/或いは前記角速度の観測量は現在推定値とすることを特徴とするGPS複合航法装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、移動体の位置、速度、方位を計測するGPS複合航法装置に関し、特に静止時において位置及び方位の推定性能を改良したGPS複合航法装置に関する。
【背景技術】
【０００２】
従来、GPSと慣性センサ、速度センサなどの外部支援センサを統合して移動体の位置、速度、姿勢及び方位などを演算するGPS/INS統合装置では、誤差推定にカルマン・フィルタが広く使用される。カルマン・フィルタによる誤差の推定性能は、ダイナミック・モデルや観測モデルのモデル誤差に大きく影響し、特に移動体の移動中と静止時ではモデル誤差が大きく異なる。また、静止時は観測量の可観測性問題に起因して一部の誤差を推定できなくなるか、または推定精度が劣化する。これらの結果、移動体が静止したにも係わらず、位置、速度、姿勢及び方位が変化する不具合が生じる。
【０００３】
前記不具合を避けるために、通常のGPS/INS統合航法装置は複数のGPSアンテナで構成されるGPS受信機と高精度の３軸の軸加速度及び角速度センサで構成することを要求するが、高価（数十万円〜数百万円）となる。特に安価（数万円）が要求される陸上用途では、単一のGPSアンテナを有するGPS受信機と、安価な１軸の慣性センサと、安価な速度センサによる構成が望まれている。
【０００４】
しかし、安価なシステム構成では、少なくとも上述のモデル誤差、可観測性問題、特に、静止時の可観測性の弱さに起因する推定性能の劣化が避けられないために、移動体が静止したにも係わらず、位置、速度、姿勢及び方位が変化する不具合が顕著になる。
【０００５】
前記安価なシステム構成の有する不具合の対策として、静止時の速度観測量、角速度観測量などを零とする観測モデル式を採用する方法が公知である(例えば、非特許文献１、 非特許文献２)。
【非特許文献１】Anastasia Olegovna Salytcheva," Medium Accuracy INS/GPS Integration in Various GPS Environments"UCGE Reports Number 20200,canada,UNIVERSITY OF CALGARY,2004,pp107
【非特許文献２】Yufeng Zhang and Yang Gao, " Observability Analysis of Initial Alignment and  Its Accuracy Improvement",ION GNSS 18th International Technical Meeting of the Satellite Division, 13-16 September 2005, Long Beach, CA,pp1
【発明の開示】
【発明が解決しようとする課題】
【０００６】
しかしながら、非特許文献１に示されているような静止時の速度観測量をゼロにする方法は、理論上、該速度観測量の観測誤差を零にする必要がある。しかし、カルマン・フィルタ安定化の点から、観測誤差を零にすることはできないために、極めて小さい値に設定される。この結果、カルマン・フィルタで計算される誤差共分散行列はきわめて小さくなる。この状態で、移動体の移動時の観測モデル式に切り替えると応答性が悪くなる問題が新たに生じる。
【０００７】
応答性の劣化対策として、移動開始の観測モデル変更時に、静止時の誤差共分散行列を大きな値で初期化し直す方法が一般的に用いられる。しかし、この方法では、誤差共分散行列の初期化時に移動体の位置及、速度、方位などがジャンプする。
【０００８】
本発明はかかる問題に鑑みてなされたものであり、位置、速度、方位の静止時における変化を解消すると、同時に、静止時から移動時における応答性の劣化を解消することを目的とする。
【課題を解決するための手段】
【０００９】
前述の課題を解決するために、本発明は、少なくともGPS受信機と慣性センサで構成されるGPS複合航法装置において、移動体の少なくとも位置、速度及び方位のいずれかを演算する航法演算部と、該航法演算部出力と前記GPS受信機出力に基づいて、前記航法演算部出力の少なくとも位置、速度及び方位のいずれかの誤差を推定するカルマン・フィルタ誤差推定部と、移動体の静止状態を判定する静止判定部とを備え、前記静止判定部で静止状態と判定された場合に、カルマン・フィルタの観測更新に用いる移動時用観測モデルを静止時用観測モデルに変更すると共に、静止判定前後の誤差共分散行列の観測更新の変化量を用いて誤差共分散行列を補正することを特徴とする。これによって、移動体を静止状態に維持し、且つ移動開始時の応答性が改善することができるようになる。
【００１０】
また、本発明は、前記静止時用観測モデルの内、位置及び/或いは方位の観測量を、現在推定値と前回推定値との差とすることを特徴とする。これによって、静止時における移動体の位置が変動しないようにすることができる。
【００１１】
また、本発明は、前記誤差共分散行列の、静止判定前後で得られる現在観測更新値と前回観測更新値との差を補正量とし、該補正量を前記現在観測更新値、前記現在観測更新値の時間更新値、ダイナミック・モデルの誤差分散行列のいずれかに加減算補正する。この補正処理により、移動体の移動開始時における応答性が改善できる。
【００１２】
また、本発明は、前記誤差共分散行列の対角要素である、静止判定前後で得られる現在観測更新値と前回観測更新値との差を補正量とし、該補正量を前記現在観測更新値、前記現在観測更新値の時間更新値またはダイナミック・モデルの誤差分散行列のいずれかの対角要素に可算または減算することで行なわれることを特徴とする。
【００１３】
また、本発明は、前記前回推定値が静止時の推定値または静止直前の推定値で代用できることを特徴とする。また、本発明は、前記前回観測更新値が静止時の観測更新値または静止直前の観測更新値で代用できることを特徴とする。
【００１４】
また、本発明は、前記航法演算部が、少なくとも、前記GPS受信機出力の位置、移動体の進行方向成分の加速度を計測する加速度センサ及び移動体の方位軸回転成分を計測する角速度センサ出力に基づいて、移動体の位置、速度及び方位の演算処理を行なうことを特徴とする。
【００１５】
また、本発明は、移動体の進行方向成分の速度を計測する速度センサを追加し、前記加速度センサ出力を前記速度センサ出力で置換することで移動体の位置、速度及び方位を演算することもできる。
【００１６】
また、本発明は、前記静止時用観測モデルの観測量として、速度及び/或いは移動体の方位軸回転成分角速度の観測量を追加し、前記速度及び/或いは前記角速度の観測量は現在推定値とすることを特徴とする。
【発明の効果】
【００１７】
本発明によれば、カルマン・フィルタによる位置誤差、方位誤差の推定において、静止直前の誤差共分散を
静止中において維持するようにすることによって、移動体が静止にも係らず、推定位置や推定方位が変化する不具合を解消することができる。
【発明を実施するための最良の形態】
【００１８】
（実施の形態１）
図１は単一のGPSアンテナで構成されるGPS受信機と、１軸の加速度センサ及び１軸の角速度センサから構成される慣性センサと、速度センサで構成されるGPS複合航法装置の機能構成図を示している。図１において、GPSアンテナ１で受信されたGPS測位信号はGPS受信機２に導かれ、GPS受信機２において、GPS位置及びGPS速度が演算され、エフェメリス情報やGPS時刻などを含むGPS航法データと共に、GPS/INS統合演算部５に送出される。
【００１９】
一方、移動体の進行方向成分加速度を検出する加速度センサと移動体の垂直成分角速度を検出する角速度センサとで構成される慣性センサ３出力と、移動体の進行方向速度を検出する速度センサ４出力とがGPS/INS統合演算部５に送出される。
【００２０】
GPS/INS統合演算部５は、位置、速度及び方位を演算する航法演算部５１と、位置誤差、速度誤差及び方位誤差や加速度センサ誤差及び角速度センサ誤差を推定する誤差推定部５２、静止判定部５３で構成される。
【００２１】
航法演算部５１は、 [数１]〜[数３]及び[数５]〜[数６]を離散積分することで、測地座標系における移動体の位置、移動体の進行方向速度、移動体の北基準の方位を算定し、当該進行方向速度および方位を用いて航法座標系における速度を算定する。このとき、位置、速度の初期値はGPS位置、GPS速度を用い、方位の初期値はGPS水平速度成分より算定した方位を用いるか、または任意の初期値とすることができる。また、装置の電源OFF直前の航法演算部５１出力を記憶しておき、これ等の値を初期値として用いることも可能である。
【数１】
0005398120.tif 000002
【数２】
0005398120.tif 000003
【数３】
0005398120.tif 000004
【数４】
0005398120.tif 000005
【００２２】
ここで、vxは移動体の進行方向速度、vN、vE、vDは、それぞれ、航法座標系における北方向速度、東方向速度、垂直成分速度である。また、wn、we、whは、それぞれのモデル誤差である。また、Ψは方位、θはピッチであり、例えば、GPS速度から算定できるが、ピッチ角が小さい場合は零で近似してもよい。ωe、re、eは、それぞれ、地球自転角速度、地球赤道半径、地球の離心率である。
【００２３】
また、移動体の進行方向速度及び方位の演算は、それぞれ、 [数５],[数６]とする。
【数５】
0005398120.tif 000006
【数６】
0005398120.tif 000007
【００２４】
ここで、axは加速度センサ出力、waは誤差である。ωzは角速度センサ出力、wωは誤差である。
【００２５】
GPS/INS統合演算部５の誤差推定部５２では、カルマン・フィルタを用いて、 [数８]のダイナミック・モデル式と [数１０]の観測モデル式に基づいて、[数７]の状態変数を演算エポックk毎に推定演算する。
【数７】
0005398120.tif 000008
【００２６】
ここで、δφ、δλ、δhは、それぞれ、緯度誤差、経度誤差、高度誤差である。また、δvx、δΨは、それぞれ、移動体の進行方向速度誤差、方位誤差であり、δax、δωzは、それぞれ、加速度出力誤差、角速度出力誤差である。
【数８】
0005398120.tif 000009
【００２７】
ここで、Φは[数１]〜[数３]と、 [数５]、[数６]の偏微分から導かれた離散系表現の遷移行列であり、Gは誤差の駆動行列である。内のkは演算エポックｋにおける計算値とする。また、wは下記の[数9]の分散を持つダイナミック・モデルの誤差である。
【数９】
0005398120.tif 000010
【００２８】
ここで、Qp、Qv、Qψ、Qa、Qωは、それぞれ、位置誤差、速度誤差、方位誤差、加速度誤差、角速度誤差の分散である。
【数１０】
0005398120.tif 000011
ここで、νは観測雑音である。
【００２９】
観測ベクトルY(k)は移動体の移動時と静止時で切り替え、移動時は[数１１]の観測ベクトルY(k)とし、静止時は [数１３]とする。これに応じて、 [数１１]に対応した観測雑音分散を[数１２]、[数１３]に対応した観測雑音分散を[数１４]に変える。尚、式は示していないが観測行列Ｈ(k)も観測ベクトルY(k)の切り替えに応じて変わる。
【００３０】
移動体の静止状態の判定を行なう静止判定部５３は、速度センサ４出力を用いて決定する。尚、図示していないが、静止判定部５３の静止判定はGPS受信機２出力または航法演算部５１出力の位置または速度を速度センサ４出力に追加し、または代用し決定することもできる。
【数１１】
0005398120.tif 000012
【数１２】
0005398120.tif 000013
【００３１】
ここで、  yp（ｋ）は航法演算部５１で推定された現時刻ｋエポックの位置とGPS位置の差、yv（ｋ）は航法演算部５１で推定されたエポックｋの速度とGPS速度の差、yvx（ｋ）は航法演算部５１で推定されたエポックｋの移動体の進行方向速度と速度センサ４で得られた速度の差である。Rp_GPS、Rv_GPS、Rvxはそれぞれ、GPS位置、GPS速度、速度センサ４で得られた速度の誤差の分散であり、その他は零行列である。
【数１３】
0005398120.tif 000014
【数１４】
0005398120.tif 000015
【００３２】
ここで、yp（ｋ）は航法演算部５１で推定された位置（緯度、経度、高度）のkエポックと(k-1)エポックの差、yvx（ｋ）は航法演算部５１で推定された移動体の進行方向速度のkエポックと零の差、yψ（ｋ）は航法演算部５１で推定された方位のkエポックと(k-1)エポックの差、yω（ｋ）は角速度センサ出力を誤差推定部５２で推定された角速度誤差で補正した角速度のkエポックと零の差とする。
【００３３】
尚、移動体が移動から静止するときは、yp（ｋ）及びyψ（ｋ）は、それぞれ、kエポックと(k-1)エポックの差の代わりに、kエポックと静止判定時または静止直前の推定値の差とすることも可能である。静止直前の推定値は、例えば、予め決めた設定値以下になったときの値とすることができる。Rp_sは静止時の位置の誤差分散、σvx_s、σψ_s、σω_sは、それぞれ、静止時の移動体進行方向速度誤差の標準偏差、静止時の方位の標準偏差、静止時の角速度の標準偏差である。
【００３４】
通常のカルマン・フイルタのアルゴリズムは[数１５]に基づいて、  [数７]の状態変数のエポックｋにおける誤差共分散行列の観測更新値P(k)からエポックｋ＋１の誤差共分散行列の予測値、即ち、時間更新値P(k+1)を算出する。
【数１５】
0005398120.tif 000016
【００３５】
[数１５]で算定される誤差共分散行列は大きな初期値P(0)から、[数８]と[数１０]のモデル誤差で決まる値に向かって、観測更新毎に暫時、減少する。
【００３６】
ところが、前述のように、ダイナミック・モデル誤差及び/或いは観測モデルの観測雑音の大きい安価なシステム構成や、移動体静止時の可観測性の弱さに起因する推定性能劣化のために、移動体が静止したにも係わらず、位置、速度、姿勢及び方位が変化する不具合が生じる。
【００３７】
静止に速度観測量をゼロにする公知技法では、上述のこの不具合が十分に解消できない。この理由は、静止における速度観測量（ゼロ）の観測雑音はゼロとすべきであるが、カルマン・フイルタ安定性のため、ゼロにできないところにある。
【００３８】
本発明は、この問題を解決するために、静止時における観測モデルの位置観測量は航法演算部５１で得られる位置の現在推定値と静止判定時の推定値との差とする。そして、速度観測量は航法演算部５１で得られる速度の現在推定値とする。方位観測量は位置観測量と、また角速度観測量は速度観測量と同様な考え方の観測量とする。尚、位置観測量及び方位観測量の決め方において、静止判定時の推定値の代わりに、静止判定時の直前の推定値または前回の推定値とすることも可能である。
【００３９】
そして、カルマン・フイルタの観測更新で得られる誤差共分散行列の位置及び方位の誤差分散の変化分ΔP(k)を[数１６]〜[数１８]に基づいて算出する。
【数１６】
0005398120.tif 000017
【００４０】
ここで、ΔQp、ΔQψはそれぞれ、位置誤差分散補正量、方位誤差分散補正量であり、以下のように算定する。
【数１７】
0005398120.tif 000018
【数１８】
0005398120.tif 000019
【００４１】
ここで、Pp、Pψはそれぞれ、誤差共分散行列の位置、方位の誤差分散の観測更新値である。また、ksは静止判定時、静止判定時直前または前回のいずれかのエポックであり、kは現在のエポックである。
また、Pp、Pψは誤差共分散行列の観測更新値の代わりに誤差共分散行列の時間更新値を用いてもよい。
【００４２】
そして、ΔP(k)を現在観測更新値、該現在観測更新値の時間更新値、ダイナミック・モデル誤差のいずれかに加算または減算する。この補正方法によって、移動時と静止時の位置及び方位の誤差分散値をほぼ一定にすることができ、この結果、静止時に位置及び方位が変化しないようにすることが可能となる。
【００４３】
また、[数１６]によるΔP(k)の算定は、位置及び方位の誤差分散要素について算出しているが、用途や要求精度によっては、位置、方位のいずれかのみに対応するΔP(k)を算定し、補正を行なうようにしてもよい。また、誤差共分散行列の全要素について、現在観測更新値と静止判定時、静止判定時の直前または前回のいずれかの該観測更新値との差として算定し、補正を行なうようにしてもよい。
【００４４】
また、 [数７]は用途や要求精度によって、適宜に状態変数を変更してもよい。また、航法演算部５１における位置、速度、方位演算は速度センサ４出力と慣性センサ３出力の角速度のみを用いて算出することも可能である。この場合は、[数５]は不要となり、直接、速度センサ４から得られた速度で代用できる。この場合、[数７]のδaxは削除または速度センサ４出力の誤差で置換すればよい。
【００４５】
（実施の形態２）
実施の形態２の構成を図２に示している。これは複数のGPSアンテナ１で構成されるGPS受信機２と、３軸の加速度センサ及び３軸加速度センサで構成された慣性センサ３を用いたGPS複合航法装置の構成機能図である。複数のGPSアンテナ構成の場合、少なくとも１個以上の基線長ベクトルが生成できるから、GPS受信機２は少なくとも方位とピッチまたはロールが算出できる。また、上述の構成の慣性センサ３を用いると、[数１]〜[数６]とは違う公知の航法演算式で位置、速度、姿勢及び方位などが算出できる。
【００４６】
図２において、複数のGPSアンテナ１で受信した測位信号はGPS受信機２に導かれ、該GPS受信機２でキャリア位相観測量が生成される。姿勢演算部１０はキャリア位相観測量に基づいて、移動体の姿勢及び方位を算出する。航法演算部５１はGPS受信機２からの位置及び速度と、慣性センサ３出力、姿勢演算部１０出力を用いて、[数１]〜[数６]とは別の公知の演算技法で位置、速度、姿勢及び方位を算出する。
【００４７】
誤差推定部５２はGPS受信機２出力と、姿勢演算部１０出力と、航法演算部５１出力に基づいて、位置誤差、速度誤差、慣性センサ誤差、姿勢誤差及び方位誤差を公知の技法で推定する。
【００４８】
静止判定部５３は航法演算部５１出力、GPS受信機２出力または慣性センサ３出力のいずれか、または併用して移動体の動静を検出する。
【００４９】
上述のように、図２と図１の構成では、姿勢演算部１０から姿勢及び方位情報が得られること及び、航法演算部５１の演算方法が異なり、これに伴って、[数７]の状態変数の数、[数８]の遷移行列、[数10]の観測行列などが異なるのみである。図１の構成で説明した本発明の、静止判定部５３で静止状態と判定された場合に、カルマン・フィルタの観測更新に用いる観測モデルを変更すると共に、誤差共分散行列の更新による変化分を補正する方法は図２の構成においても適用できる。
【図面の簡単な説明】
【００５０】
【図１】本発明に係る単一GPSアンテナを用いたGPS複合航法装置の構成説明図
【図２】本発明に係る複数のGPSアンテナを用いたGPS複合航法装置の構成説明図
【符号の説明】
【００５１】
１    GPSアンテナ
２    GPS受信機
３    慣性センサ
４    速度センサ
５    GPS/INS統合演算部
１０  姿勢演算部
５１  航法演算部
５２  誤差推定部
５３  静止判定部
【図１】
0005398120.tif 000020
【図２】
0005398120.tif 000021