ＧＰＳ複合航法装置
【課題】GPS受信機を含む構成のGPS複合航法装置において、GPS擬似距離或観測量及びドップラー周波数観測量との差に基づいて、GPS受信機出力の異常を検知するとき、GPS受信機出力の異常を正確に判定すると共に、GPS擬似距離或観測量及びドップラー周波数観測量の推定誤差に起因する異常判定時の異常継続を回避するGPS複合航法装置を提供することを目的とする。
【解決手段】GPS受信機出力の異常を検知したとき、異常期間を計数し、該計数値が予め決められた閾値以下ではGPS受信機出力を異常として扱い、前記閾値を超えた以後はGPS受信機出力を正常として扱うことによって、GPS受信機出力の異常を正確に判定することを特徴とする。
【解決手段】GPS受信機出力の異常を検知したとき、異常期間を計数し、該計数値が予め決められた閾値以下ではGPS受信機出力を異常として扱い、前記閾値を超えた以後はGPS受信機出力を正常として扱うことによって、GPS受信機出力の異常を正確に判定することを特徴とする。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、移動体の位置、速度、方位を計測するGPS複合航法装置に関し、特にGPS出力の異常検出及び異常発生時の処理を適切に行なうGPS複合航法装置に関する。
【背景技術】
【０００２】
従来、GPS受信機と慣性センサを統合して、移動体の位置、速度等を演算するGPS/INS統合装置はGPS受信機出力の位置（GPS位置）、速度（GPS速度）等が利用される。これらのGPS受信機出力はGPS受信機で生成されるGPS擬似距離観測量やGPSドップラー周波数観測量（以下、GPS観測量）やエフエメリス・データ等に基づいて演算され出力される。
【０００３】
信頼できるGPS航法出力を得るためにはGPS観測量の異常の有無を検出し、異常値を排除する必要がある。通常、GPS受信機でGPS受信信号の良否判定に基づいて、異常なGPS観測量を排除するようにしているが、完全に排除できないのが現状である。
【０００４】
従って、受信機外部でもGPS受信機出力の異常判定を行なう必要がある。従来の異常判定方法は特許文献１に記載されているように、観測された擬似距離とその推定値間の残差に基づく方法、また、特許文献２に記載のGPS位置と外部装置から得た位置との残差に基づく方法、更に、特許文献３に記載の測距コードから算定した擬似距離とキャリア位相から算定した擬似距離の残差に基づく方法などによって行われる。
【０００５】
特許文献２に記載の方法はGPS以外の外部支援装置を必要とするために高価になるため、適用範囲に制限を受ける。また、特許文献３に記載の方法は、例えば、マルチパスのように、片方により強く影響をより強く受ける場合の検出には有効である。しかし、電離層遅延誤差のように逆極性で影響を受ける場合や、同じ影響を受けるような誤差がある場合は残差による異常検出は効力を失う。
【０００６】
特許文献１の方法はRAIM（Receiver Autonomous Integrity Monitoring）技法に見られるように、よく利用される技法である。
【非特許文献１】Masato Kawai, Shigeki Hara, Osamu Arai,"A New Step Detector Algorithm For GPS/SBAS Receiver"ION GPS 2001,14th International Technical Meeting of the Satellite Division of the Institute of NavigationSeptember 11-14, 2001
【特許文献１】特開２００４−３０１７２５号公報
【特許文献２】米国特許第 ５９１７４４５ 号
【発明の開示】
【発明が解決しようとする課題】
【０００７】
しかしながら、前記のいずれの方法においても、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受信機出力は正常であるにも係わらず、異常と判定し、統合演算部出力から得たGPS観測量推定値やGPS位置推定値の誤差要因が解消されない限り、異常判定ループから抜け出せない従来の欠点は解消することができる。
【００１２】
また、本発明は、前記GPS異常検出部が、前記GPS受信機出力から得られるGPS受信信号のS/N、C/NoもしくはDOP値のいずれかを用いて、前記GPS受信機出力の信頼度を判定するGPS信頼度判定部をさらに備え、前記GPS信頼度判定部で非信頼と判定されたときは前記異常期間の計数を停止またはリセットすることを特徴とする。
【００１３】
この構成により、前記異常期間の計数値が前記閾値を超えた以後に、真に異常の観測量を正常と見なす確率を低減させることできる。
【００１４】
また、本発明は、前記GPS異常検出部における異常検知が、GPS擬似距離及び/或いはGPSドップラー周波数観測量の代わりに、前記GPS受信機出力の位置及び/或いは速度と、前記統合演算部出力の位置及び/或いは速度との差に基づいて異常を検知することを特徴とする。
【００１５】
また、本発明は、前記統合演算部が、少なくとも前記GPS受信機出力の位置と、少なくとも移動体の進行方向成分の加速度を計測する加速度センサと、少なくとも移動体の方位軸回転成分を計測する角速度センサ出力に基づいて、移動体の位置、速度及び方位を演算することを特徴とする。
【００１６】
また、本発明は、前記統合演算部が、少なくとも移動体の進行方向成分の速度を計測する速度センサと、少なくとも移動体の方位軸回転成分を計測する角速度センサ出力に基づいて、移動体の位置、速度及び方位を演算することを特徴とする。
【００１７】
また、本発明は、電源OFF直前の前記統合演算部出力を記憶する記憶部を設けて、電源ON時に前記記憶部出力を少なくとも位置、速度の初期値として用いることを特徴とする。この構成により、電源ON時のGPS受信機出力から前記初期値が得られない場合でも、前記記憶部出力から初期値を得ることができるので、前記統合演算部出力の早期の提供が可能となる。
【発明の効果】
【００１８】
本発明によれば、安価且つ最少構成の慣性センサを用いて、GPS受信機出力の異常を検出することが可能となり、しかも、GPS受信機出力が正常であるにも係わらず、GPS観測量の推定値の異常に起因して、異常と判定したとき、異常判定ループから抜け出せないという欠点が解消できる。
【００１９】
また、本発明は、GPS観測量残差による異常判定に先立って、信頼度指数によるGPS受信機出力の信頼度を判定するGPS信頼度判定部を追加することで、前記異常期間の計数後の、真に異常の観測量を正常と見なす確率を低減できる。
【発明を実施するための最良の形態】
【００２０】
（実施の形態１）
図１に本発明の実施の形態１に係るGPS複合航法装置の機能構成を示す。複数のGPS衛星からの測位信号をアンテナ１で受信し、GPS受信機２は、アンテナ１からの受信信号を受けて、衛星とアンテナ１間のGPS観測量を算出し、該GPS観測量に基づいてGPS位置及びGPS速度やGPS観測量、エフェメリス情報、GPS時刻などを算定し、GPS/INS統合演算部５に出力する。
【００２１】
一方、移動体の進行方向成分加速度を計測する加速度センサと移動体の垂直成分角速度を計測する角速度センサで構成される慣性センサ３と、移動体の進行方向速度を計測する速度センサ４等の出力がGPS/INS統合演算部５に送出される。
【００２２】
統合演算部５１は、GPS受信機２出力のGPS位置、慣性センサ３出力の加速度及び角速度に基づいて、下記の[数１]、[数２]、[数３]を積分し、測地座標系における統合位置（緯度、経度、高度）、移動体の進行方向速度、北基準の統合方位を算定する。また、移動体の進行方向速度および統合方位を用いて航法座標系における統合速度を算定する。
【００２３】
尚、下記の[数１]、[数２]、[数３]の積分演算に必要な統合位置の初期値はGPS受信機２出力のGPS位置を、また移動体の進行方向速度の初期値はGPS受信機２出力の速度を、また統合方位の初期値はGPS受信機２出力の水平速度より算定したGPS方位を用いることができる。尚、後述のように、誤差推定部５３で方位誤差を推定し、補正することにより、統合方位の初期値は必ずしも必要としない。
【００２４】
また、装置の電源OFF直前の統合位置、統合速度、統合方位を記憶しておき、電源ON時に統合位置、移動体の進行方向速度及び統合方位の初期値として使用するようにすることもできる。
移動体位置（緯度、経度、高度）のダイナミック方程式は[数１]、[数２]、[数３]で表せる。
【数１】
【数２】
【数３】
【数４】
ここで、φ、λ、hは、それぞれ、緯度、経度、高度、vxは移動体の進行方向速度、vN、vE、はそれぞれ、航法座標系における北方向速度、東方向速度である。また、wn、we、whは、それぞれ、誤差である。また、Ψは方位、θはピッチであるが、移動体のピッチ角が小さい場合は零で近似してもよい。ωe、re、eは、それぞれ、地球自転角速度、地球赤道半径、地球の離心率である。
また、移動体の進行方向速度及び方位のダイナミック方程式は、それぞれ、[数５]および[数６]で表す。
【数５】
ここで、axは加速度センサの観測値、waは該観測誤差である。
【数６】
ここで、ωzは角速度センサの観測値、wωは誤差である。
【００２５】
GPS異常検出部５２は、GPS受信機２出力の異常の有無を検出し、異常発生時において、異常判定ループから抜け出られない問題を解消する処理部である。
【００２６】
GPS異常検出部５２において、GPS受信機２出力から得られるエフェメリス情報（i衛星の位置及び速度）と、統合演算部５１出力の統合位置及び統合速度に基づいて、i衛星と移動体間のGPS観測量推定値(i)が計算される。
【００２７】
GPS観測量推定値(i)はGPS受信機２出力のGPS観測量(i)と比較され、GPS観測量残差(i)が計算される。該GPS観測量残差(i)が予め設定された閾値より大きいとき、GPS受信機２出力が異常と検知する。尚、GPS観測量残差(i)と閾値等しいときは異常とするか正常とするかはいずれでもよい。
【００２８】
GPS異常検出部５２で異常を検知したとき、残差異常カウンタ(i)はi(=0,1,---N)番目のGPS観測量(i)の異常期間を計数するために、計数を開始し、当該計数値が予め決められた閾値以下ではGPS受信機２出力を異常として扱い、計数値を超えた以後はGPS受信機２出力を正常として扱うようにする。
【００２９】
誤差推定部５３は、例えば、最小２乗法またはカルマン・フィルタを用いて、[数１]〜[数３]と[数５]〜[数６]のダイナミック方程式と、後述の[数８]の観測方程式に基づいて、[数７]の誤差を推定する。
【数７】
【００３０】
ここで、δφ、δλ、δhは、それぞれ、緯度誤差、経度誤差、高度誤差である。また、δvx、δΨは、それぞれ、移動体の進行方向速度誤差、方位誤差であり、δax、δωzはそれぞれ、加速度センサ出力の加速度バイアス誤差、角速度センサ出力の角速度バイアス誤差である。
【数８】
ここで、νは観測雑音である。観測ベクトルYは以下のとおりとする。
【数９】
【００３１】
また、観測マトリックスHは統合演算部５１の出力の及びGPS受信機２出力を用いて計算できる。
yi_ρは、i番目のGPS擬似距離観測量(i)と、該観測量推定値(i)との差である。yi_φは、i番目のGPSドップラー周波数観測量(i)と、該観測量推定値(i)との差である。yvxは速度センサ４の観測量と、統合演算部５１で推定された移動体の進行方向速度との差である。尚、速度センサ４の観測量の代わりにGPS受信機２出力のGPS速度で代用することも可能である。
【００３２】
ここで、i番目のGPS擬似距離推定値(i)ρi_est及びGPSドップラー周波数観測量推定値(i)fi_estは、それぞれ、[数１０]、[数１１]で計算できる。
【数１０】
【数１１】
【００３３】
ここで、pe_estは、それぞれ、ECEF（Earth-Centered, Earth-Fixed）座標系における移動体の統合位置であり、統合位置（緯度、経度、高度）より公知の方法で変換できる。pe_i_svはECEF座標系におけるi衛星の位置であり、GPSデータに含まれるエフェメリス情報より既知である。また、HeはECEF座標系における移動体位置から見た基準衛星及びi衛星の方向余弦である。
【００３４】
尚、[数１０]、[数１１]はi衛星と移動体間の観測モデルに限定するものではなく、i衛星および基準衛星と移動体間で構成する一重位相差観測モデルに相当する式で置換しても良い。
また、移動体の進行方向速度の推定値ve_estはECEF座標系における移動体の速度推定値であり、[数１２]で求まる。
【数１２】
ここで、Cgeは航法座標系からECEF座標系への変換マトリックスであり、統合位置を用いて求めることができる。また、θはピッチ角であるが、移動体のピッチ角が小さい場合は零で近似してもよい。
【００３５】
誤差推定部５３で推定された統合位置誤差、移動体進行速度誤差、統合方位誤差、慣性センサ誤差はそれぞれ、統合位置、移動体進行速度、統合速度、統合方位、慣性センサ３出力に対して、GPS受信機２出力の更新周期毎に補正される。
また、誤差推定部５３において、[数９]の観測ベクトルＹを[数１３]で置換して前述の諸誤差を推定することもできる。
【数１３】
ここで、ypは統合位置とGPS受信機２出力のGPS位置の差、yvは統合速度とGPS受信機２出力の速度の差、yvxは速度センサ４の観測値と移動体の統合速度の差である。
【００３６】
本発明は図２に示す構成でも適用できる。図２の構成と図１の構成との違いは、移動体の進行方位軸回転成分を計測する角速度センサ出力と移動体の進行方向成分を計測する速度センサ４出力を用いて統合演算部５１の処理を行なうようにしたことである。図２の構成では、[数５]が不要となり、直接、速度センサ４から得られた速度で代用できる。この場合、[数７]のδaxは削除または速度センサ誤差で置換すればよい。図２の構成においても、統合演算部５１の演算に必要な初期値の与え方は図１の構成で説明した方法と同じである。
【００３７】
図３に図１の構成のGPS/INS統合演算部５の処理フローを示している。該処理フローは図２の構成でも同じであるため、特に差異がある場合を除き説明を省略する。図３のステップ１０において、誤差推定部５３で推定された誤差の補正を行なう。
【００３８】
ステップ２０では、所定の周期で読み込まれたGPS受信機２出力、慣性センサ３出力、速度センサ４出力に基づいて、統合演算部５１の演算処理が行われる。ここで、所定の周期とは、GPS受信機２出力はGPS受信２出力の更新周期（通常、1sec〜0.02sec）であり、慣性センサ３出力はGPS受信機２出力の更新周期同等以上の周期である。尚、速度センサ４出力は図１の構成ではGPS受信機２出力の更新周期でよいが、図２の構成では慣性センサ３出力の読み込み周期と同じとする。
【００３９】
ステップ３０において、GPS受信機２出力の更新判定を行ない、更新タイミングであれば、GPS異常検出部５２の処理部であるステップ４０に至り、全てのチャンネル番号iについて、GPS異常検出の処理を行なう。そして、ステップ５０で、誤差推定部５３の演算処理が行なわれる。
【００４０】
尚、誤差を推定するための最小必要数の正常な観測数が得られない期間が、予め設定された期間を超える場合はステップ１０における統合推定値の誤差補正停止や、ステップ２０における統合推定値の時間更新停止などの適切な処理が行なわれるかもしれない。
【００４１】
図４は図３のステップ４０詳細処理フローである。図４のステップ４１で、GPS受信機２で観測可能な全てのチャンネルの内、判定すべきチャンネルi(=1.2.3.---)の設定を行なった後、ステップ４２において、チャンネル番号iのGPS観測量推定値(i)を算定する。
【００４２】
ステップ４３で、選定されたチャンネル番号i毎に、GPS観測量残差(i)による判定と残差異常カウンタ(i)による判定に基づいて、GPS観測量(i)の異常検出を行なう。
【００４３】
GPS観測量残差(i)の判定は[数１４]に基づいて判定を行なう。
【数１４】
【００４４】
ここで、ρi_mes、fi_estは、それぞれ、GPS擬似距離観測量(i)、GPSドップラー周波数観測量(i)である。
【００４５】
そして、GPS観測量(i)の異常の判定は [数１５]に示すように残差異常カウンタ(i)値が予め設定された閾値と比較することによって行なわれる。
【数１５】
【００４６】
GPS観測量残差(i)による判定の閾値と残差異常カウンタ(i)による判定の閾値はそれぞれ、独立に設定される。また、GPS観測量残差(i)による判定の閾値及び残差異常カウンタ(i)の各閾値はチャンネル番号i毎に独自に設定してもよい。また、[数１４]では、GPS擬似距離観測量残差(i)とGPSドップラー周波数観測量残差(i)の判定を独立に行なっているが、GPS擬似距離観測量残差(i)とGPSドップラー周波数観測量残差(i)のいずれか、または併用で行なっても良い。また、[数１４]及び[数１５]は等号を含める式でもよい。
【００４７】
前述のように、GPS観測量推定値(i)の推定誤差に起因して生じるGPS観測量(i)の異常は、推定誤差の原因が解消されない限り継続する。これに対して、通常、受信信号の劣化、マルチパス、サイクルスリップ等の要因で発生するGPS観測量(i)の異常は比較的短時間で正常に復帰する確率が高い。
従って、設定計数を適切な値に設定すれば、異常の観測量(i)を正常と見なす確率は実質的に問題なき程度に低減できる。
【００４８】
ステップ４３において、異常と判定されると、ステップ４４に至り、残差異常カウンタ(i)を１つカウントアップし、ステップ４５に至る。正常の場合は残差異常カウンタ(i)をゼロにリセットした後、ステップ４５に至る。
【００４９】
（実施の形態２）
通常、GPS観測量(i)の異常は比較的短時間で正常に復帰する確率が高いため、適切な閾値を設定しておけば、残差異常カウンタ(i)の設定期間経過後に、強制的に正常とする図４のステップ４３の判定方法でも、真に異常のGPS観測量(i)を正常と見なす確率は実質的に問題なき程度に低減できる。
【００５０】
しかし、低S/Nや、高DOPとなる衛星を使用するときなどはGPS観測量(i)の異常が短時間でない場合があるため、真に異常のGPS観測量(i)を正常と見なす確率はゼロではない。真に異常のGPS観測量(i)を正常と見なす確率を、更に改善するために、GPS信頼度判定部を設け、当該判定結果に応じて残差異常カウンタ(i)を制御するようにした実施を図５に示す。
【００５１】
図５と図４の構成の違いは、図４のステップ４２とステップ４３の間にステップ４３aとステップ４３bの処理を追加したことである。
【００５２】
ステップ４３aはGPS信頼度判定部であり、GPS受信機２出力の品質を示すパラメータを用いて信頼度指数を生成し、該信頼度指数と予め設定された閾値とを比較することによって、GPS受信機２出力から得られる観測量の信頼度を判定する。
【００５３】
この結果、非信頼の場合はステップ４３bで残差異常カウンタ(i)をゼロにリセットし、信頼の場合は残差異常カウンタ(i)のリセットは行なわない。ここで、信頼度指数はGPS受信機２出力から得られる受信信号のS/NまたはC/Noとするか、もしくはDOP値の逆比例値に基づいて決定する。
【００５４】
これによって、信頼できないGPS観測量(i)は残差異常カウンタ(i)をゼロにリセットするので、次のステップ４３において、異常と判定されたとき、異常の一定期間が経過した後に、強制的に正常としないため、真に異常であるGPS観測量(i)を正常とする確率を低減することができる。
【００５５】
（実施の形態３）
図４及び図５に示した構成において、GPS観測量残差(i)による異常判定の代わりに、移動体の位置及び/或いは速度と、統合演算部５１出力の統合位置及び/或いは統合速度との比較差、即ち、位置残差及び/或いは速度残差を用いて、GPS観測量残差(i)の異常判定を行なうことも可能である。
【００５６】
また、GPS観測量残差(i)と、位置残差及び/或いは速度残差とのOR結合またはAND結合により新たな残差信号を生成し、当該残差信号で図４のステップ３及び図５のステップ４に示すGPS観測量残差(i)の代用することも可能である。
【図面の簡単な説明】
【００５７】
【図１】本発明に係るGPS/INS統合GPS複合航法装置構成例１の説明図
【図２】本発明に係るGPS/INS統合GPS複合航法装置構成例２の説明図
【図３】本発明に係るGPS/INS統合演算部の処理フローについての説明図
【図４】本発明に係る異常検出判定実施例１の処理フローについての説明図
【図５】本発明に係る異常検出判定実施例２の処理フローについての説明図
【符号の説明】
【００５８】
１ アンテナ
２ GPS受信機
３ 慣性センサ
４ 速度センサ
５ GPS/INS統合演算部
５１ 統合演算部
５２ GPS異常検出部
５３ 誤差推定部
【技術分野】
【０００１】
本発明は、移動体の位置、速度、方位を計測するGPS複合航法装置に関し、特にGPS出力の異常検出及び異常発生時の処理を適切に行なうGPS複合航法装置に関する。
【背景技術】
【０００２】
従来、GPS受信機と慣性センサを統合して、移動体の位置、速度等を演算するGPS/INS統合装置はGPS受信機出力の位置（GPS位置）、速度（GPS速度）等が利用される。これらのGPS受信機出力はGPS受信機で生成されるGPS擬似距離観測量やGPSドップラー周波数観測量（以下、GPS観測量）やエフエメリス・データ等に基づいて演算され出力される。
【０００３】
信頼できるGPS航法出力を得るためにはGPS観測量の異常の有無を検出し、異常値を排除する必要がある。通常、GPS受信機でGPS受信信号の良否判定に基づいて、異常なGPS観測量を排除するようにしているが、完全に排除できないのが現状である。
【０００４】
従って、受信機外部でもGPS受信機出力の異常判定を行なう必要がある。従来の異常判定方法は特許文献１に記載されているように、観測された擬似距離とその推定値間の残差に基づく方法、また、特許文献２に記載のGPS位置と外部装置から得た位置との残差に基づく方法、更に、特許文献３に記載の測距コードから算定した擬似距離とキャリア位相から算定した擬似距離の残差に基づく方法などによって行われる。
【０００５】
特許文献２に記載の方法はGPS以外の外部支援装置を必要とするために高価になるため、適用範囲に制限を受ける。また、特許文献３に記載の方法は、例えば、マルチパスのように、片方により強く影響をより強く受ける場合の検出には有効である。しかし、電離層遅延誤差のように逆極性で影響を受ける場合や、同じ影響を受けるような誤差がある場合は残差による異常検出は効力を失う。
【０００６】
特許文献１の方法はRAIM（Receiver Autonomous Integrity Monitoring）技法に見られるように、よく利用される技法である。
【非特許文献１】Masato Kawai, Shigeki Hara, Osamu Arai,"A New Step Detector Algorithm For GPS/SBAS Receiver"ION GPS 2001,14th International Technical Meeting of the Satellite Division of the Institute of NavigationSeptember 11-14, 2001
【特許文献１】特開２００４−３０１７２５号公報
【特許文献２】米国特許第 ５９１７４４５ 号
【発明の開示】
【発明が解決しようとする課題】
【０００７】
しかしながら、前記のいずれの方法においても、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受信機出力は正常であるにも係わらず、異常と判定し、統合演算部出力から得たGPS観測量推定値やGPS位置推定値の誤差要因が解消されない限り、異常判定ループから抜け出せない従来の欠点は解消することができる。
【００１２】
また、本発明は、前記GPS異常検出部が、前記GPS受信機出力から得られるGPS受信信号のS/N、C/NoもしくはDOP値のいずれかを用いて、前記GPS受信機出力の信頼度を判定するGPS信頼度判定部をさらに備え、前記GPS信頼度判定部で非信頼と判定されたときは前記異常期間の計数を停止またはリセットすることを特徴とする。
【００１３】
この構成により、前記異常期間の計数値が前記閾値を超えた以後に、真に異常の観測量を正常と見なす確率を低減させることできる。
【００１４】
また、本発明は、前記GPS異常検出部における異常検知が、GPS擬似距離及び/或いはGPSドップラー周波数観測量の代わりに、前記GPS受信機出力の位置及び/或いは速度と、前記統合演算部出力の位置及び/或いは速度との差に基づいて異常を検知することを特徴とする。
【００１５】
また、本発明は、前記統合演算部が、少なくとも前記GPS受信機出力の位置と、少なくとも移動体の進行方向成分の加速度を計測する加速度センサと、少なくとも移動体の方位軸回転成分を計測する角速度センサ出力に基づいて、移動体の位置、速度及び方位を演算することを特徴とする。
【００１６】
また、本発明は、前記統合演算部が、少なくとも移動体の進行方向成分の速度を計測する速度センサと、少なくとも移動体の方位軸回転成分を計測する角速度センサ出力に基づいて、移動体の位置、速度及び方位を演算することを特徴とする。
【００１７】
また、本発明は、電源OFF直前の前記統合演算部出力を記憶する記憶部を設けて、電源ON時に前記記憶部出力を少なくとも位置、速度の初期値として用いることを特徴とする。この構成により、電源ON時のGPS受信機出力から前記初期値が得られない場合でも、前記記憶部出力から初期値を得ることができるので、前記統合演算部出力の早期の提供が可能となる。
【発明の効果】
【００１８】
本発明によれば、安価且つ最少構成の慣性センサを用いて、GPS受信機出力の異常を検出することが可能となり、しかも、GPS受信機出力が正常であるにも係わらず、GPS観測量の推定値の異常に起因して、異常と判定したとき、異常判定ループから抜け出せないという欠点が解消できる。
【００１９】
また、本発明は、GPS観測量残差による異常判定に先立って、信頼度指数によるGPS受信機出力の信頼度を判定するGPS信頼度判定部を追加することで、前記異常期間の計数後の、真に異常の観測量を正常と見なす確率を低減できる。
【発明を実施するための最良の形態】
【００２０】
（実施の形態１）
図１に本発明の実施の形態１に係るGPS複合航法装置の機能構成を示す。複数のGPS衛星からの測位信号をアンテナ１で受信し、GPS受信機２は、アンテナ１からの受信信号を受けて、衛星とアンテナ１間のGPS観測量を算出し、該GPS観測量に基づいてGPS位置及びGPS速度やGPS観測量、エフェメリス情報、GPS時刻などを算定し、GPS/INS統合演算部５に出力する。
【００２１】
一方、移動体の進行方向成分加速度を計測する加速度センサと移動体の垂直成分角速度を計測する角速度センサで構成される慣性センサ３と、移動体の進行方向速度を計測する速度センサ４等の出力がGPS/INS統合演算部５に送出される。
【００２２】
統合演算部５１は、GPS受信機２出力のGPS位置、慣性センサ３出力の加速度及び角速度に基づいて、下記の[数１]、[数２]、[数３]を積分し、測地座標系における統合位置（緯度、経度、高度）、移動体の進行方向速度、北基準の統合方位を算定する。また、移動体の進行方向速度および統合方位を用いて航法座標系における統合速度を算定する。
【００２３】
尚、下記の[数１]、[数２]、[数３]の積分演算に必要な統合位置の初期値はGPS受信機２出力のGPS位置を、また移動体の進行方向速度の初期値はGPS受信機２出力の速度を、また統合方位の初期値はGPS受信機２出力の水平速度より算定したGPS方位を用いることができる。尚、後述のように、誤差推定部５３で方位誤差を推定し、補正することにより、統合方位の初期値は必ずしも必要としない。
【００２４】
また、装置の電源OFF直前の統合位置、統合速度、統合方位を記憶しておき、電源ON時に統合位置、移動体の進行方向速度及び統合方位の初期値として使用するようにすることもできる。
移動体位置（緯度、経度、高度）のダイナミック方程式は[数１]、[数２]、[数３]で表せる。
【数１】
【数２】
【数３】
【数４】
ここで、φ、λ、hは、それぞれ、緯度、経度、高度、vxは移動体の進行方向速度、vN、vE、はそれぞれ、航法座標系における北方向速度、東方向速度である。また、wn、we、whは、それぞれ、誤差である。また、Ψは方位、θはピッチであるが、移動体のピッチ角が小さい場合は零で近似してもよい。ωe、re、eは、それぞれ、地球自転角速度、地球赤道半径、地球の離心率である。
また、移動体の進行方向速度及び方位のダイナミック方程式は、それぞれ、[数５]および[数６]で表す。
【数５】
ここで、axは加速度センサの観測値、waは該観測誤差である。
【数６】
ここで、ωzは角速度センサの観測値、wωは誤差である。
【００２５】
GPS異常検出部５２は、GPS受信機２出力の異常の有無を検出し、異常発生時において、異常判定ループから抜け出られない問題を解消する処理部である。
【００２６】
GPS異常検出部５２において、GPS受信機２出力から得られるエフェメリス情報（i衛星の位置及び速度）と、統合演算部５１出力の統合位置及び統合速度に基づいて、i衛星と移動体間のGPS観測量推定値(i)が計算される。
【００２７】
GPS観測量推定値(i)はGPS受信機２出力のGPS観測量(i)と比較され、GPS観測量残差(i)が計算される。該GPS観測量残差(i)が予め設定された閾値より大きいとき、GPS受信機２出力が異常と検知する。尚、GPS観測量残差(i)と閾値等しいときは異常とするか正常とするかはいずれでもよい。
【００２８】
GPS異常検出部５２で異常を検知したとき、残差異常カウンタ(i)はi(=0,1,---N)番目のGPS観測量(i)の異常期間を計数するために、計数を開始し、当該計数値が予め決められた閾値以下ではGPS受信機２出力を異常として扱い、計数値を超えた以後はGPS受信機２出力を正常として扱うようにする。
【００２９】
誤差推定部５３は、例えば、最小２乗法またはカルマン・フィルタを用いて、[数１]〜[数３]と[数５]〜[数６]のダイナミック方程式と、後述の[数８]の観測方程式に基づいて、[数７]の誤差を推定する。
【数７】
【００３０】
ここで、δφ、δλ、δhは、それぞれ、緯度誤差、経度誤差、高度誤差である。また、δvx、δΨは、それぞれ、移動体の進行方向速度誤差、方位誤差であり、δax、δωzはそれぞれ、加速度センサ出力の加速度バイアス誤差、角速度センサ出力の角速度バイアス誤差である。
【数８】
ここで、νは観測雑音である。観測ベクトルYは以下のとおりとする。
【数９】
【００３１】
また、観測マトリックスHは統合演算部５１の出力の及びGPS受信機２出力を用いて計算できる。
yi_ρは、i番目のGPS擬似距離観測量(i)と、該観測量推定値(i)との差である。yi_φは、i番目のGPSドップラー周波数観測量(i)と、該観測量推定値(i)との差である。yvxは速度センサ４の観測量と、統合演算部５１で推定された移動体の進行方向速度との差である。尚、速度センサ４の観測量の代わりにGPS受信機２出力のGPS速度で代用することも可能である。
【００３２】
ここで、i番目のGPS擬似距離推定値(i)ρi_est及びGPSドップラー周波数観測量推定値(i)fi_estは、それぞれ、[数１０]、[数１１]で計算できる。
【数１０】
【数１１】
【００３３】
ここで、pe_estは、それぞれ、ECEF（Earth-Centered, Earth-Fixed）座標系における移動体の統合位置であり、統合位置（緯度、経度、高度）より公知の方法で変換できる。pe_i_svはECEF座標系におけるi衛星の位置であり、GPSデータに含まれるエフェメリス情報より既知である。また、HeはECEF座標系における移動体位置から見た基準衛星及びi衛星の方向余弦である。
【００３４】
尚、[数１０]、[数１１]はi衛星と移動体間の観測モデルに限定するものではなく、i衛星および基準衛星と移動体間で構成する一重位相差観測モデルに相当する式で置換しても良い。
また、移動体の進行方向速度の推定値ve_estはECEF座標系における移動体の速度推定値であり、[数１２]で求まる。
【数１２】
ここで、Cgeは航法座標系からECEF座標系への変換マトリックスであり、統合位置を用いて求めることができる。また、θはピッチ角であるが、移動体のピッチ角が小さい場合は零で近似してもよい。
【００３５】
誤差推定部５３で推定された統合位置誤差、移動体進行速度誤差、統合方位誤差、慣性センサ誤差はそれぞれ、統合位置、移動体進行速度、統合速度、統合方位、慣性センサ３出力に対して、GPS受信機２出力の更新周期毎に補正される。
また、誤差推定部５３において、[数９]の観測ベクトルＹを[数１３]で置換して前述の諸誤差を推定することもできる。
【数１３】
ここで、ypは統合位置とGPS受信機２出力のGPS位置の差、yvは統合速度とGPS受信機２出力の速度の差、yvxは速度センサ４の観測値と移動体の統合速度の差である。
【００３６】
本発明は図２に示す構成でも適用できる。図２の構成と図１の構成との違いは、移動体の進行方位軸回転成分を計測する角速度センサ出力と移動体の進行方向成分を計測する速度センサ４出力を用いて統合演算部５１の処理を行なうようにしたことである。図２の構成では、[数５]が不要となり、直接、速度センサ４から得られた速度で代用できる。この場合、[数７]のδaxは削除または速度センサ誤差で置換すればよい。図２の構成においても、統合演算部５１の演算に必要な初期値の与え方は図１の構成で説明した方法と同じである。
【００３７】
図３に図１の構成のGPS/INS統合演算部５の処理フローを示している。該処理フローは図２の構成でも同じであるため、特に差異がある場合を除き説明を省略する。図３のステップ１０において、誤差推定部５３で推定された誤差の補正を行なう。
【００３８】
ステップ２０では、所定の周期で読み込まれたGPS受信機２出力、慣性センサ３出力、速度センサ４出力に基づいて、統合演算部５１の演算処理が行われる。ここで、所定の周期とは、GPS受信機２出力はGPS受信２出力の更新周期（通常、1sec〜0.02sec）であり、慣性センサ３出力はGPS受信機２出力の更新周期同等以上の周期である。尚、速度センサ４出力は図１の構成ではGPS受信機２出力の更新周期でよいが、図２の構成では慣性センサ３出力の読み込み周期と同じとする。
【００３９】
ステップ３０において、GPS受信機２出力の更新判定を行ない、更新タイミングであれば、GPS異常検出部５２の処理部であるステップ４０に至り、全てのチャンネル番号iについて、GPS異常検出の処理を行なう。そして、ステップ５０で、誤差推定部５３の演算処理が行なわれる。
【００４０】
尚、誤差を推定するための最小必要数の正常な観測数が得られない期間が、予め設定された期間を超える場合はステップ１０における統合推定値の誤差補正停止や、ステップ２０における統合推定値の時間更新停止などの適切な処理が行なわれるかもしれない。
【００４１】
図４は図３のステップ４０詳細処理フローである。図４のステップ４１で、GPS受信機２で観測可能な全てのチャンネルの内、判定すべきチャンネルi(=1.2.3.---)の設定を行なった後、ステップ４２において、チャンネル番号iのGPS観測量推定値(i)を算定する。
【００４２】
ステップ４３で、選定されたチャンネル番号i毎に、GPS観測量残差(i)による判定と残差異常カウンタ(i)による判定に基づいて、GPS観測量(i)の異常検出を行なう。
【００４３】
GPS観測量残差(i)の判定は[数１４]に基づいて判定を行なう。
【数１４】
【００４４】
ここで、ρi_mes、fi_estは、それぞれ、GPS擬似距離観測量(i)、GPSドップラー周波数観測量(i)である。
【００４５】
そして、GPS観測量(i)の異常の判定は [数１５]に示すように残差異常カウンタ(i)値が予め設定された閾値と比較することによって行なわれる。
【数１５】
【００４６】
GPS観測量残差(i)による判定の閾値と残差異常カウンタ(i)による判定の閾値はそれぞれ、独立に設定される。また、GPS観測量残差(i)による判定の閾値及び残差異常カウンタ(i)の各閾値はチャンネル番号i毎に独自に設定してもよい。また、[数１４]では、GPS擬似距離観測量残差(i)とGPSドップラー周波数観測量残差(i)の判定を独立に行なっているが、GPS擬似距離観測量残差(i)とGPSドップラー周波数観測量残差(i)のいずれか、または併用で行なっても良い。また、[数１４]及び[数１５]は等号を含める式でもよい。
【００４７】
前述のように、GPS観測量推定値(i)の推定誤差に起因して生じるGPS観測量(i)の異常は、推定誤差の原因が解消されない限り継続する。これに対して、通常、受信信号の劣化、マルチパス、サイクルスリップ等の要因で発生するGPS観測量(i)の異常は比較的短時間で正常に復帰する確率が高い。
従って、設定計数を適切な値に設定すれば、異常の観測量(i)を正常と見なす確率は実質的に問題なき程度に低減できる。
【００４８】
ステップ４３において、異常と判定されると、ステップ４４に至り、残差異常カウンタ(i)を１つカウントアップし、ステップ４５に至る。正常の場合は残差異常カウンタ(i)をゼロにリセットした後、ステップ４５に至る。
【００４９】
（実施の形態２）
通常、GPS観測量(i)の異常は比較的短時間で正常に復帰する確率が高いため、適切な閾値を設定しておけば、残差異常カウンタ(i)の設定期間経過後に、強制的に正常とする図４のステップ４３の判定方法でも、真に異常のGPS観測量(i)を正常と見なす確率は実質的に問題なき程度に低減できる。
【００５０】
しかし、低S/Nや、高DOPとなる衛星を使用するときなどはGPS観測量(i)の異常が短時間でない場合があるため、真に異常のGPS観測量(i)を正常と見なす確率はゼロではない。真に異常のGPS観測量(i)を正常と見なす確率を、更に改善するために、GPS信頼度判定部を設け、当該判定結果に応じて残差異常カウンタ(i)を制御するようにした実施を図５に示す。
【００５１】
図５と図４の構成の違いは、図４のステップ４２とステップ４３の間にステップ４３aとステップ４３bの処理を追加したことである。
【００５２】
ステップ４３aはGPS信頼度判定部であり、GPS受信機２出力の品質を示すパラメータを用いて信頼度指数を生成し、該信頼度指数と予め設定された閾値とを比較することによって、GPS受信機２出力から得られる観測量の信頼度を判定する。
【００５３】
この結果、非信頼の場合はステップ４３bで残差異常カウンタ(i)をゼロにリセットし、信頼の場合は残差異常カウンタ(i)のリセットは行なわない。ここで、信頼度指数はGPS受信機２出力から得られる受信信号のS/NまたはC/Noとするか、もしくはDOP値の逆比例値に基づいて決定する。
【００５４】
これによって、信頼できないGPS観測量(i)は残差異常カウンタ(i)をゼロにリセットするので、次のステップ４３において、異常と判定されたとき、異常の一定期間が経過した後に、強制的に正常としないため、真に異常であるGPS観測量(i)を正常とする確率を低減することができる。
【００５５】
（実施の形態３）
図４及び図５に示した構成において、GPS観測量残差(i)による異常判定の代わりに、移動体の位置及び/或いは速度と、統合演算部５１出力の統合位置及び/或いは統合速度との比較差、即ち、位置残差及び/或いは速度残差を用いて、GPS観測量残差(i)の異常判定を行なうことも可能である。
【００５６】
また、GPS観測量残差(i)と、位置残差及び/或いは速度残差とのOR結合またはAND結合により新たな残差信号を生成し、当該残差信号で図４のステップ３及び図５のステップ４に示すGPS観測量残差(i)の代用することも可能である。
【図面の簡単な説明】
【００５７】
【図１】本発明に係るGPS/INS統合GPS複合航法装置構成例１の説明図
【図２】本発明に係るGPS/INS統合GPS複合航法装置構成例２の説明図
【図３】本発明に係るGPS/INS統合演算部の処理フローについての説明図
【図４】本発明に係る異常検出判定実施例１の処理フローについての説明図
【図５】本発明に係る異常検出判定実施例２の処理フローについての説明図
【符号の説明】
【００５８】
１ アンテナ
２ GPS受信機
３ 慣性センサ
４ 速度センサ
５ GPS/INS統合演算部
５１ 統合演算部
５２ 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受信機出力の位置と、少なくとも移動体の進行方向成分の加速度を計測する加速度センサと、少なくとも移動体の方位軸回転成分を計測する角速度センサ出力に基づいて、移動体の位置、速度及び方位を演算することを特徴とするGPS複合航法装置。
【請求項５】
請求項１に記載のGPS複合航法装置において、
前記統合演算部は少なくとも移動体の進行方向成分の速度を計測する速度センサと、少なくとも移動体の方位軸回転成分を計測する角速度センサ出力に基づいて、移動体の位置、速度及び方位を演算することを特徴とするGPS複合航法装置。
【請求項６】
請求項１に記載のGPS複合航法装置において、
電源OFF直前の前記統合演算部出力を記憶する記憶部をさらに備え、該記憶部出力を統合演算部における演算の初期値として用いることを特徴とする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受信機出力の位置と、少なくとも移動体の進行方向成分の加速度を計測する加速度センサと、少なくとも移動体の方位軸回転成分を計測する角速度センサ出力に基づいて、移動体の位置、速度及び方位を演算することを特徴とするGPS複合航法装置。
【請求項５】
請求項１に記載のGPS複合航法装置において、
前記統合演算部は少なくとも移動体の進行方向成分の速度を計測する速度センサと、少なくとも移動体の方位軸回転成分を計測する角速度センサ出力に基づいて、移動体の位置、速度及び方位を演算することを特徴とするGPS複合航法装置。
【請求項６】
請求項１に記載のGPS複合航法装置において、
電源OFF直前の前記統合演算部出力を記憶する記憶部をさらに備え、該記憶部出力を統合演算部における演算の初期値として用いることを特徴とするGPS複合航法装置。
【図１】
【図２】
【図３】
【図４】
【図５】
【図２】
【図３】
【図４】
【図５】
【公開番号】特開２００８−２３２８６８（Ｐ２００８−２３２８６８Ａ）
【公開日】平成２０年１０月２日（２００８．１０．２）
【国際特許分類】
【出願番号】特願２００７−７３８２７（Ｐ２００７−７３８２７）
【出願日】平成１９年３月２２日（２００７．３．２２）
【出願人】（０００１６６２４７）古野電気株式会社 (441)
【Ｆターム（参考）】
【公開日】平成２０年１０月２日（２００８．１０．２）
【国際特許分類】
【出願日】平成１９年３月２２日（２００７．３．２２）
【出願人】（０００１６６２４７）古野電気株式会社 (441)
【Ｆターム（参考）】
[ Back to top ]