JP2021081203A - 航法計算装置、航法計算方法、及び、航法計算プログラム - Google Patents

Abstract

【課題】信頼性の向上が可能な航法計算装置、航法計算方法、及び、航法計算プログラムを提供すること。【解決手段】航法計算装置１は、第１受信機１１の第１搬送波位相観測値φ１と第２受信機１２の第２搬送波位相観測値φ２とに基づいて第１姿勢角Ａ１を算出する姿勢角算出部１３と、慣性センサ１４の検出結果に基づいて第２姿勢角Ａ２を算出する航法計算部１５と、姿勢角Ａ１，Ａ２を比較して搬送波位相観測値φ１，φ２が正常かを判定する第１判定部１６と、を備え、航法計算部１５は、第１判定部１６によって搬送波位相観測値φ１，φ２が正常であると判定された場合、搬送波位相観測値φ１，φ２の何れかを慣性航法の計算の補正に用い、姿勢角算出部１３は、第１姿勢角Ａ１を算出できない場合、搬送波位相観測値φ１，φ２のうち異なるタイミングで取得された複数の何れかの搬送波位相観測値に基づいて姿勢角Ａ１に代わる姿勢角Ａ３を算出する。【選択図】図１

Description

本発明は、航法計算装置、航法計算方法、及び、航法計算プログラムに関する。

慣性センサ（Ｉｎｅｒｔｉａｌ Ｍｅａｓｕｒｉｎｇ Ｕｎｉｔ；ＩＭＵ）によって検出される移動体の角速度及び加速度に基づいて当該移動体の位置、速度、及び、姿勢角等を算出する航法計算装置には、高い信頼性が求められている。しかしながら、航法計算装置は、慣性センサによる検出結果を積分しながら慣性航法の計算を行っているため、航法計算装置によって算出された移動体の位置、速度、及び、姿勢角等には、積分に伴うドリフト誤差が発生してしまうという問題があった。

このような問題に対する解決策は、例えば特許文献１に開示されている。特許文献１に開示された航法計算システムは、複数の受信アンテナと、姿勢角計算手段と、慣性センサと、慣性航法手段と、検定処理手段と、を備える。複数の受信アンテナは、それぞれ衛星測位システムからの信号を受信する。姿勢角計算手段は、複数の受信アンテナにより受信された信号を基に算出された複数の搬送波位相観測値に基づいて、第１の姿勢角を算出する。慣性航法手段は、慣性センサによる検出結果に基づいて慣性航法計算を行うことにより、第２の姿勢角を算出する。検定処理手段は、第１の姿勢角と第２の姿勢角とを比較することにより、第１の姿勢角の算出に用いられた搬送波位相観測値が正常であるか否かの判定を行う。ここで、慣性航法手段は、検知処理手段によって正常と判定された搬送波位相観測値を、慣性航法の計算の補正に用いる。それにより、積分に伴うドリフト誤差が抑制される。

その他、特許文献２にも、特許文献１と類似の構成が開示されている。また、特許文献３にも慣性航法計算に関する技術が開示されている。

特開２０１２−２０８０３３号公報 国際公開第２０１７／０９０３５９号 特開２０１２−２０７９１９号公報

しかしながら、関連技術の航法計算システムでは、障害物等の影響で衛星測位システムからの信号を受信することができずに搬送波位相観測値の算出において不具合が発生した場合、姿勢角計算手段が第１の姿勢角を算出することができない可能性がある。この場合、検定処理手段は、第１及び第２の姿勢角を比較することができないため、搬送波位相観測値に異常がある旨の判定を下すことができない。それにより、関連技術の航法計算システムは、実際には異常のある搬送波位相観測値を誤って慣性航法の計算の補正に用いてしまう可能性がある。そのため、関連技術の航法計算システムは、信頼性を低下させてしまうという課題があった。

本開示の目的は、上述した課題を解決する航法計算装置、航法計算方法、及び、航法計算プログラムを提供することにある。

一実施の形態によれば、航法計算装置は、複数の衛星機器からの信号を受信して第１搬送波位相観測値及び第１疑似距離観測値を算出する第１受信機と、前記複数の衛星機器からの信号を受信して第２搬送波位相観測値及び第２疑似距離観測値を算出する第２受信機と、前記第１受信機によって算出された第１搬送波位相観測値と、前記第２受信機によって算出された第２搬送波位相観測値と、に基づいて第１姿勢角を算出する姿勢角算出部と、自装置の加速度及び角速度を検出する慣性センサと、前記慣性センサによる検出結果を用いて慣性航法の計算を行うことにより、少なくとも第２姿勢角を算出する航法計算部と、前記第１及び前記第２姿勢角を比較することにより、前記第１及び前記第２搬送波位相観測値が正常であるか否かを判定する第１判定部と、を備え、前記航法計算部は、前記第１判定部によって前記第１及び前記第２搬送波位相観測値が正常であると判定された場合、前記第１及び前記第２搬送波位相観測値の何れかを、前記慣性航法の計算の補正に用いるように構成され、前記姿勢角算出部は、前記第１姿勢角を算出できない場合、前記第１及び前記第２搬送波位相観測値のうち異なるタイミングで取得された何れか一方の複数の搬送波位相観測値に基づいて、第３姿勢角を算出し、前記第１姿勢角として出力するように構成されている。

一実施の形態によれば、航法計算方法は、第１受信機を用いて、複数の衛星機器からの信号を受信して第１搬送波位相観測値及び第１疑似距離観測値を算出するステップと、第２受信機を用いて、前記複数の衛星機器からの信号を受信して第２搬送波位相観測値及び第２疑似距離観測値を算出するステップと、姿勢角算出部を用いて、前記第１受信機によって算出された第１搬送波位相観測値と、前記第２受信機によって算出された第２搬送波位相観測値と、に基づいて第１姿勢角を算出するステップと、慣性センサを用いて、自装置の加速度及び角速度を検出するステップと、航法計算部を用いて、前記慣性センサによる検出結果を用いて慣性航法の計算を行うことにより、少なくとも第２姿勢角を算出するステップと、第１判定部を用いて、前記第１及び前記第２姿勢角を比較することにより、前記第１及び前記第２搬送波位相観測値が正常であるか否かを判定するステップと、を備え、前記第２姿勢角を算出するステップでは、前記第１判定部によって前記第１及び前記第２搬送波位相観測値が正常であると判定された場合、前記第１及び前記第２搬送波位相観測値の何れかを、前記慣性航法の計算の補正に用いるように構成され、前記第１姿勢角を算出するステップでは、前記第１姿勢角を算出できない場合、前記第１及び前記第２搬送波位相観測値のうち異なるタイミングで取得された何れか一方の複数の搬送波位相観測値に基づいて、第３姿勢角を算出し、前記第１姿勢角として出力するように構成されている。

一実施の形態によれば、航法計算プログラムは、第１受信機を用いて、複数の衛星機器からの信号を受信して第１搬送波位相観測値及び第１疑似距離観測値を算出する処理と、第２受信機を用いて、前記複数の衛星機器からの信号を受信して第２搬送波位相観測値及び第２疑似距離観測値を算出する処理と、姿勢角算出部を用いて、前記第１受信機によって算出された第１搬送波位相観測値と、前記第２受信機によって算出された第２搬送波位相観測値と、に基づいて第１姿勢角を算出する処理と、慣性センサを用いて、自装置の加速度及び角速度を検出する処理と、航法計算部を用いて、前記慣性センサによる検出結果を用いて慣性航法の計算を行うことにより、少なくとも第２姿勢角を算出する処理と、第１判定部を用いて、前記第１及び前記第２姿勢角を比較することにより、前記第１及び前記第２搬送波位相観測値が正常であるか否かを判定する処理と、をコンピュータに実行させ、さらに、前記第２姿勢角を算出する処理では、前記第１判定部によって前記第１及び前記第２搬送波位相観測値が正常であると判定された場合、前記第１及び前記第２搬送波位相観測値の何れかを、前記慣性航法の計算の補正に用いるように構成され、前記第１姿勢角を算出する処理では、前記第１姿勢角を算出できない場合、前記第１及び前記第２搬送波位相観測値のうち異なるタイミングで取得された何れか一方の複数の搬送波位相観測値に基づいて、第３姿勢角を算出し、前記第１姿勢角として出力するように構成されている。

前記一実施の形態によれば、信頼性を向上させることが可能な航法計算装置、航法計算方法、及び、航法計算プログラムを提供することができる。

実施の形態１に係る航法計算装置の構成例を示すブロック図である。 姿勢角Ａ１の算出に用いられた搬送波位相観測値φ１，φ２が正常であるか否かを判定する方法を説明するための図である。 車両（移動体）に搭載されたＧＰＳ受信機１１，１２による複数のＧＰＳ衛星機器からの信号の受信について説明するための図である。 所定周期のタイミングにおける、移動体の実際の位置と、ＧＰＳ受信結果に基づいて算出される移動体の位置（観測値）と、の関係を示す概念図である。 実施の形態１に係る航法計算装置による姿勢角Ａ３の算出方法を説明するための図である。 移動体の時速と、姿勢角Ａ１，Ａ３間の角度差と、の関係を示す図である。 姿勢角Ａ１，Ａ３間の角度差の正規分布を示す図である。 図１に示す航法計算装置の動作を示すフローチャートである。 実施の形態２に係る航法計算装置の構成例を示すブロック図である。 図９に示す航法計算装置の動作を示すフローチャートである。 図１に示す航法計算装置の変形例の動作を示すフローチャートである。

以下、図面を参照しつつ、実施の形態について説明する。なお、図面は簡略的なものであるから、この図面の記載を根拠として実施の形態の技術的範囲を狭く解釈してはならない。また、同一の要素には、同一の符号を付し、重複する説明は省略する。

以下の実施の形態においては便宜上その必要があるときは、複数のセクションまたは実施の形態に分割して説明する。ただし、特に明示した場合を除き、それらはお互いに無関係なものではなく、一方は他方の一部または全部の変形例、応用例、詳細説明、補足説明等の関係にある。また、以下の実施の形態において、要素の数等（個数、数値、量、範囲等を含む）に言及する場合、特に明示した場合および原理的に明らかに特定の数に限定される場合等を除き、その特定の数に限定されるものではなく、特定の数以上でも以下でもよい。

さらに、以下の実施の形態において、その構成要素（動作ステップ等も含む）は、特に明示した場合および原理的に明らかに必須であると考えられる場合等を除き、必ずしも必須のものではない。同様に、以下の実施の形態において、構成要素等の形状、位置関係等に言及するときは、特に明示した場合および原理的に明らかにそうでないと考えられる場合等を除き、実質的にその形状等に近似または類似するもの等を含むものとする。このことは、上記数等（個数、数値、量、範囲等を含む）についても同様である。

＜実施の形態１＞
図１は、実施の形態１に係る航法計算装置１の構成例を示すブロック図である。
図１に示す航法計算装置１は、例えば、車両、船舶、航空機、又は、ロケット等の移動体に搭載され、当該移動体の位置、速度、及び、姿勢角等を精度良く算出する装置である。以下、具体的に説明する。

図１に示すように、航法計算装置１は、ＧＰＳ受信機１１，１２と、姿勢角算出部（姿勢角算出回路）１３と、慣性センサ１４と、航法計算部（航法計算回路）１５と、第１判定部（第１判定回路）１６と、を備える。なお、ＧＰＳは、Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍの略である。以下では、航法計算装置１を自装置とも称す。

ＧＰＳ受信機１１は、少なくとも４つ以上のＧＰＳ衛星機器のそれぞれから搬送波による信号を受信し、それらの受信信号から、搬送波の位相を示す搬送波位相観測値φ１と、ＧＰＳ衛星機器から自装置までの疑似距離を示す疑似距離観測値ρ１と、を算出する。

ＧＰＳ受信機１２は、少なくとも４つ以上のＧＰＳ衛星機器のそれぞれから搬送波による信号を受信し、それらの受信信号から、搬送波の位相を示す搬送波位相観測値φ２と、ＧＰＳ衛星機器から自装置までの疑似距離を示す疑似距離観測値ρ２と、を算出する。

姿勢角算出部１３は、所謂ＧＰＳコンパスであり、ＧＰＳ受信機１１により算出された搬送波位相観測値φ１と、ＧＰＳ受信機１２により算出された搬送波位相観測値φ２と、に基づいて自装置（換言すると、自装置を搭載した移動体）の姿勢角Ａ１を算出する。

慣性センサ（Ｉｎｅｒｔｉａｌ Ｍｅａｓｕｒｉｎｇ Ｕｎｉｔ；ＩＭＵ）１４は、例えば、自装置の角速度を検出するジャイロと、自装置の加速度を検出する加速度計と、を備える。航法計算部１５は、慣性センサ１４による検出結果に基づいて慣性航法の計算を行うことにより、自装置の位置、速度、及び、姿勢角等を算出する。以下、航法計算部１５により算出された姿勢角を、姿勢角算出部１３により算出された姿勢角Ａ１と区別するため、姿勢角Ａ２と称す。航法計算部１５による慣性航法の計算方式には、例えば、ストラップダウン方式が用いられる。なお、慣性センサ１４と、航法計算部１５と、によって、慣性航法装置（Ｉｎｅｒｔｉａｌ Ｎａｖｉｇａｔｉｏｎ Ｓｙｓｔｅｍ；ＩＮＳ）が構成される。

第１判定部１６は、姿勢角算出部１３により算出された姿勢角Ａ１と、航法計算部１５により算出された姿勢角Ａ２と、を比較することにより、姿勢角Ａ１の算出に用いられた搬送波位相観測値φ１，φ２が正常であるか否かの判定を行う。

図２は、姿勢角Ａ１の算出に用いられた搬送波位相観測値φ１，φ２が正常であるか否かを判定する方法を説明するための図である。図２を参照すると、姿勢角Ａ１，Ａ２間の差分が許容範囲内の場合、姿勢角Ａ１の算出に用いられる搬送波位相観測値φ１，φ２が何れも正常であると判定する（図２の左図）。それに対し、姿勢角Ａ１，Ａ２間の差分が許容範囲外の場合、第１判定部１６は、姿勢角Ａ１の算出に用いられる搬送波位相観測値φ１，φ２の何れかに異常があると判定する（図２の右図）。

ここで、航法計算部１５は、慣性センサ１４による検出結果を積分しながら慣性航法の計算を行うことにより、自装置の位置、速度、及び、姿勢角等を算出している。そのため、航法計算部１５により算出された自装置の位置、速度、及び姿勢角等には、積分に伴うドリフト誤差が発生してしまう。

そこで、航法計算部１５は、第１判定部１６によって姿勢角Ａ１の算出に用いられた搬送波位相観測値φ１，φ２が正常と判定された場合、搬送波位相観測値φ１（又はφ２）及びそれに対応する疑似距離観測値ρ１（又はρ２）を慣性航法の計算の補正に用いる。それにより、航法計算部１５は、積分に伴うドリフト誤差を補正して、自装置の位置、速度、及び、姿勢角等を精度良く算出することができる。なお、補正の方式には、疎結合方式や密結合方式を含む任意の方式が用いられる。

他方、航法計算部１５は、第１判定部１６によって姿勢角Ａ１の算出に用いられた搬送波位相観測値φ１，φ２に異常があると判定された場合、搬送波位相観測値φ１，φ２及びそれらに対応する疑似距離観測値ρ１，ρ２を、慣性航法の計算の補正に用いない。

さらに、ＧＰＳ受信機１１，１２の何れかが、障害物等の影響によりＧＰＳ衛星機器からの信号を受信することができずに、搬送波位相観測値及び疑似距離観測値の算出において不具合を生じさせる場合がある。

図３は、車両（移動体）に搭載されたＧＰＳ受信機１１，１２による複数のＧＰＳ衛星機器からの信号の受信について説明するための図である。

図３の例では、ＧＰＳ受信機１２が４つのＧＰＳ衛星機器の全てからの信号を受信できているに対し、ＧＰＳ受信機１１は、４つのＧＰＳ衛星機器のうち一つのＧＰＳ衛星機器からの信号を受信できていない。そのため、ＧＰＳ受信機１２が搬送波位相観測値φ２及び疑似距離観測値ρ２を正しく算出することができるのに対し、ＧＰＳ受信機１１は、搬送波位相観測値φ１及び疑似距離観測値ρ１の算出に不具合（算出しない場合を含む）を生じさせてしまう。

この場合、姿勢角算出部１３は、ＧＰＳ受信機１１の算出結果に不具合があるため、姿勢角Ａ１を算出することができない。そのため、第１判定部１６は、姿勢角Ａ１，Ａ２の差分が許容範囲内であるか否かを判定することができない。つまり、第１判定部１６は、搬送波位相観測値φ１に異常がある旨の判定を下すことができない。そのため、航法計算部１５は、実際には異常のある搬送波位相観測値φ１及びそれに対応する疑似距離観測値ρ１を誤って慣性航法の計算の補正に用いてしまう可能性がある。

図４は、所定周期のタイミングにおける、移動体の実際の位置と、ＧＰＳ受信結果に基づいて算出される移動体の位置（観測値）と、の関係を示す概念図である。

図４に示すように、タイミングｎ（ｎは整数），ｎ＋２では、移動体の実際の位置と、ＧＰＳ受信結果に基づいて算出される移動体の位置と、が実質的に同一となっている。それに対し、タイミングｎ＋１では、移動体の実際の位置と、ＧＰＳ受信結果に基づいて算出される移動体の位置と、が互いに異なっている。また、移動体の実際の姿勢角と、ＧＰＳ受信結果に基づいて算出される移動体の姿勢角（Ａ１）と、も互いに異なっている。このとき、第１判定部１６は、姿勢角Ａ１と姿勢角Ａ２との間の誤差が許容範囲外になるため、姿勢角Ａ１の算出に用いられた搬送波位相観測値φ１，φ２の何れかに異常があると判定する。そのため、航法計算部１５は、タイミングｎ＋１のＧＰＳ受信結果に基づいて算出された搬送波位相観測値φ１，φ２及び疑似距離観測値ρ１，ρ２を慣性航法の計算の補正に用いない。しかしながら、タイミングｎ＋３では、不具合の影響でＧＰＳ受信結果に基づく姿勢角Ａ１の算出が行われていない。この場合、第１判定部１６は、搬送波位相観測値φ１，φ２の何れかに異常がある旨の判定を下すことができない。そのため、航法計算部１５は、異常のある搬送波位相観測値φ１又はφ２、及び、それに対応する疑似距離観測値ρ１又はρ２を、誤って慣性航法の計算の補正に用いてしまう可能性がある。

そこで、姿勢角算出部１３は、搬送波位相観測値φ１，φ２の一方の算出の不具合により姿勢角Ａ１を算出できない場合、搬送波位相観測値φ１，φ２の他方を異なるタイミングで複数取得し、それらの複数の観測値に基づいて姿勢角Ａ３を算出する。具体的には、図５に示すように、姿勢角算出部１３は、移動体の移動に伴って変化する搬送波位相観測値φ１又はφ２の変位（緯度経度、及び、高度のそれぞれの変位）から、姿勢角Ａ３を算出する。

図３の例では、姿勢角算出部１３が、搬送波位相観測値φ１の算出の不具合により姿勢角Ａ１を算出することができないため、搬送波位相観測値φ２を異なるタイミングで複数（例えば２個）取得し、それらの複数の観測値に基づいて姿勢角Ａ３を算出する。この姿勢角Ａ３は、姿勢角Ａ１に相当する。

なお、姿勢角Ａ３の算出精度を向上させるには、航法計算装置１を搭載した移動体が移動中である必要がある。図６は、移動体の時速と、姿勢角Ａ１，Ａ３間の角度差と、の関係を示す図である。また、図７は、姿勢角Ａ１，Ａ３間の角度差の正規分布を示す図である。なお、図６及び図７は、航法計算装置１を搭載した車両（移動体）が１時間走行している期間中に、１秒毎に１つのサンプルを取得することによって得られた結果である。図６及び図７を参照すると、移動体の時速が０ｋｍ付近では姿勢角Ａ１，Ａ３間の角度差が大きいが、移動体の時速が１ｋｍ程度でもあれば姿勢角Ａ１，Ａ３間の角度差は急激に小さくなっていることが分かる。

この姿勢角Ａ３は、姿勢角Ａ１の代わりに用いられる。即ち、第１判定部１６は、姿勢角算出部１３により算出された姿勢角Ａ３と、航法計算部１５により算出された姿勢角Ａ２と、を比較することにより、姿勢角Ａ３の算出に用いられた搬送波位相観測値（図３の例ではφ２）が正常であるか否かを判定する。

その後、航法計算部１５は、第１判定部１６によって姿勢角Ａ３の算出に用いられた搬送波位相観測値が正常であると判定された場合、当該搬送波位相観測値及びそれに対応する疑似距離観測値を慣性航法の計算の補正に用いる。図３の例では、航法計算部１５は、搬送波位相観測値φ２及びそれに対応する疑似距離観測値ρ２を慣性航法の計算の補正に用いる。それにより、航法計算部１５は、積分に伴うドリフト誤差を補正して、自装置の位置、速度、及び、姿勢角等を精度良く算出することができる。

他方、航法計算部１５は、第１判定部１６によって姿勢角Ａ３の算出に用いられた搬送波位相観測値に異常があると判定された場合、当該搬送波位相観測値及びそれに対応する疑似距離観測値を、慣性航法の計算の補正に用いない。

このように、航法計算装置１では、通常は、姿勢角算出部１３が、ＧＰＳ受信機１１，１２のそれぞれによって算出された搬送波位相観測値に基づいて、姿勢角Ａ１を算出している。さらに、姿勢角算出部１３は、ＧＰＳ受信機１１，１２の一方の搬送波位相観測値の算出に不具合がある場合でも、他方のＧＰＳ受信機によって異なるタイミングで算出された複数の搬送波位相観測値に基づいて、姿勢角Ａ１に相当する姿勢角Ａ３を算出する。この場合、航法計算装置１では、第１判定部１６が、姿勢角算出部１３によって算出された姿勢角Ａ３と、航法計算部１５によって算出された姿勢角Ａ２と、を比較することで、姿勢角Ａ３の算出に用いられた搬送波位相観測値が正常であるか否かの判定を行う。それにより、航法計算装置１は、姿勢角Ａ１の未算出による第１判定部１６の判定処理の未実施を防ぐことができるため、異常のある搬送波位相観測値及び疑似距離観測値を誤って慣性航法の計算の補正に用いるのを防ぐことができる。その結果、航法計算装置１は、自装置の位置、速度、及び、姿勢角等の算出精度を向上させることができる。換言すると、航法計算装置１は、信頼性を向上させることができる。

（フローチャート）
続いて、図８に示すフローチャートを用いて、航法計算装置１の動作について説明する。図８は、航法計算装置１の動作を示すフローチャートである。

まず、各ＧＰＳ受信機１１，１２は、少なくとも４つ以上の複数のＧＰＳ衛星機器のそれぞれから搬送波による信号を受信する（ステップＳ１０１）。

その後、ＧＰＳ受信機１１は、複数のＧＰＳ生成機器のそれぞれから受信した信号に基づいて、搬送波位相観測値φ１及び疑似距離観測値ρ１を算出する（ステップＳ１０２）。また、ＧＰＳ受信機１２は、複数のＧＰＳ生成機器のそれぞれから受信した信号に基づいて、搬送波位相観測値φ２及び疑似距離観測値ρ２を算出する（ステップＳ１０２）。

その後、姿勢角算出部１３は、ＧＰＳ受信機１１，１２のそれぞれによって算出された搬送波位相観測値φ１，φ２に基づいて、自装置の姿勢角Ａ１を算出する（ステップＳ１０３）。

姿勢角算出部１３によって姿勢角Ａ１が算出された場合（ステップＳ１０４のＹＥＳ）、第１判定部１６は、当該姿勢角Ａ１と、慣性センサ１４による検出結果を用いて航法計算部１５によって算出された姿勢角Ａ２と、の比較を行う（ステップＳ１０５）。

例えば、姿勢角Ａ１，Ａ２間の差分（誤差）が許容範囲内である場合（ステップＳ１０６のＹＥＳ）、第１判定部１６は、姿勢角Ａ１の算出に用いられる搬送波位相観測値φ１，φ２が何れも正常であると判定する（ステップＳ１０７）。この場合、航法計算部１５は、姿勢角Ａ１の算出に用いられた搬送波位相観測値φ１（又はφ２）及びそれに対応する疑似距離観測値ρ１（又はρ２）を慣性航法の計算の補正に用いる（ステップＳ１０８）。それにより、航法計算部１５は、ドリフト誤差を補正して、自装置の位置、速度、及び、姿勢角等を精度良く算出することができる。

それに対し、姿勢角Ａ１，Ａ２間の差分（誤差）が許容範囲外である場合（ステップＳ１０６のＮＯ）、第１判定部１６は、姿勢角Ａ１の算出に用いられる搬送波位相観測値φ１，φ２の何れかに異常があると判定する（ステップＳ１０９）。この場合、航法計算部１５は、搬送波位相観測値φ１，φ２及びそれらに対応する疑似距離観測値ρ１，ρ２を慣性航法の計算の補正に用いない（ステップＳ１１０）。

さらに、姿勢角算出部１３による姿勢角Ａ１の算出処理において、搬送波位相観測値φ１，φ２の一方の算出の不具合により姿勢角Ａ１が算出されない場合がある（ステップＳ１０４のＮＯ）。この場合、姿勢角算出部１３は、搬送波位相観測値φ１，φ２の他方を異なるタイミングで複数取得し、それらの複数の観測値に基づいて、姿勢角Ａ１に相当する姿勢角Ａ３を算出する（ステップＳ１１１）。

姿勢角算出部１３によって姿勢角Ａ３が算出された場合（ステップＳ１１２のＹＥＳ）、第１判定部１６は、当該姿勢角Ａ３と、慣性センサ１４による検出結果を用いて航法計算部１５によって算出された姿勢角Ａ２と、の比較を行う（ステップＳ１０５）。その後のステップＳ１０６〜Ｓ１１０の処理は、既に説明した通りであるため、省略する。

なお、姿勢角算出部１３によって姿勢角Ａ３も算出されなかった場合（ステップＳ１１２のＮＯ）、航法計算部１５は、搬送波位相観測値φ１，φ２及びそれらに対応する疑似距離観測値ρ１，ρ２を慣性航法の計算の補正に用いない（ステップＳ１１３）。

このように、航法計算装置１では、通常は、姿勢角算出部１３が、ＧＰＳ受信機１１，１２のそれぞれによって算出された搬送波位相観測値に基づいて、姿勢角Ａ１を算出している。さらに、姿勢角算出部１３は、ＧＰＳ受信機１１，１２の一方の搬送波位相観測値の算出に不具合がある場合でも、他方のＧＰＳ受信機によって異なるタイミングで算出された複数の搬送波位相観測値に基づいて、姿勢角Ａ１に相当する姿勢角Ａ３を算出する。この場合、航法計算装置１では、第１判定部１６が、姿勢角算出部１３によって算出された姿勢角Ａ３と、航法計算部１５によって算出された姿勢角Ａ２と、を比較することで、姿勢角Ａ３の算出に用いられた搬送波位相観測値が正常であるか否かの判定を行う。それにより、航法計算装置１は、姿勢角Ａ１の未算出による第１判定部１６の判定処理の未実施を防ぐことができるため、異常のある搬送波位相観測値及び疑似距離観測値を誤って慣性航法の計算の補正に用いるのを防ぐことができる。その結果、航法計算装置１は、自装置の位置、速度、及び、姿勢角等の算出精度を向上させることができる。換言すると、航法計算装置１は、信頼性を向上させることができる。

＜実施の形態２＞
図９は、実施の形態２に係る航法計算装置２の構成例を示すブロック図である。
図９に示す航法計算装置２は、図１に示す航法計算装置１と比較して、第２判定部（第２判定回路）１７をさらに備える。航法計算装置２のその他の構成については、航法計算装置１の場合と同様であるため、その説明を省略する。

第２判定部１７は、第１判定部１６によって正常であると判定された何れかの搬送波位相観測値の単位時間当たりの変化量と、それに対応する疑似距離観測値の単位時間当たりの変化量と、の差分に基づいて、当該疑似距離観測値が正常であるか否かを判定する。

例えば、第２判定部１７は、第１判定部１６によって正常と判定された搬送波位相観測値φ１の単位時間当たりの変化量と、搬送波位相観測値φ１に対応する疑似距離観測値ρ１の単位時間当たりの変化量と、を比較する。

ここで、搬送波位相観測値φ１の単位時間当たりの変化量と、搬送波位相観測値φ１に対応する疑似距離観測値ρ１の単位時間当たりの変化量と、の差分が許容範囲内である場合、第２判定部１７は、疑似距離観測値ρ１が正常であると判定する。この場合、航法計算部１５は、搬送波位相観測値φ１及び疑似距離観測値ρ１を慣性航法の計算の補正に用いる。それにより、航法計算部１５は、ドリフト誤差を補正して、自装置の位置、速度、及び、姿勢角等を精度良く算出することができる。

他方、搬送波位相観測値φ１の単位時間当たりの変化量と、搬送波位相観測値φ１に対応する疑似距離観測値ρ１の単位時間当たりの変化量と、の差分が許容範囲外である場合、第２判定部１７は、疑似距離観測値ρ１に異常があると判定する。この場合、航法計算部１５は、搬送波位相観測値φ１及び疑似距離観測値ρ１を慣性航法の計算の補正に用いない。

なお、第２判定部１７は、第１判定部１６によって正常と判定された搬送波位相観測値φ２の単位時間当たりの変化量と、搬送波位相観測値φ２に対応する疑似距離観測値ρ２の単位時間当たりの変化量と、を比較しても良い。この場合、航法計算部１５は、第２判定部１７による判定結果に基づいて、搬送波位相観測値φ２及び疑似距離観測値ρ２を慣性航法の計算の補正に用いるか否かを決定する。

（フローチャート）
図１０は、航法計算装置２の動作を示すフローチャートである。
以下では、主に、図８に示すフローチャートと異なる内容について説明する。

姿勢角Ａ１又は姿勢角Ａ３の算出に用いられた搬送波位相観測値が正常と判定された場合（ステップＳ１０７）、第２判定部１７は、正常と判定された搬送波位相観測値に対応する疑似距離観測値が正常であるか否かの判定を行う。以下では、第２判定部１７が、正常と判定された搬送波位相観測値φ１に対応する疑似距離観測値ρ１が正常であるか否かの判定を行う場合を例に説明する。

具体的には、第２判定部１７は、正常と判定された搬送波位相観測値φ１の単位時間当たりの変化量と、疑似距離観測値ρ１の単位時間当たりの変化量と、の比較を行う（ステップＳ２０１）。

例えば、搬送波位相観測値φ１の単位時間当たりの変化量と、疑似距離観測値ρ１の単位時間当たりの変化量と、の差分が許容範囲内の場合（ステップＳ２０２のＹＥＳ）、第２判定部１７は、疑似距離観測値ρ１が正常であると判定する（ステップＳ２０３）。この場合、航法計算部１５は、搬送波位相観測値φ１、及び、それに対応する疑似距離観測値ρ１、を慣性航法の計算の補正に用いる（ステップＳ１０８）。それにより、航法計算部１５は、ドリフト誤差を補正して、自装置の位置、速度、及び、姿勢角等を精度良く算出することができる。

それに対し、搬送波位相観測値φ１の単位時間当たりの変化量と、疑似距離観測値ρ１の単位時間当たりの変化量と、の差分が許容範囲外の場合（ステップＳ２０２のＮＯ）、第２判定部１７は、疑似距離観測値ρ１に異常があると判定する（ステップＳ２０４）。この場合、航法計算部１５は、搬送波位相観測値φ１及びそれに対応する疑似距離観測値ρ１、を慣性航法の計算の補正に用いない（ステップＳ１１０）。

図１０に示すフローチャートのその他の処理については、基本的には図８に示すフローチャートの場合と同様であるため、その説明を省略する。

このように、航法計算装置２は、第２判定部１７を用いることにより、第１判定部１６によって正常と判定された搬送波位相観測値（例えばφ１）、に対応する疑似距離観測値（例えばρ２）が正常であるか否かの判定を行う。それにより、航法計算装置２は、自装置の位置、速度、及び、姿勢角等の算出精度をさらに向上させることができる。換言すると、航法計算装置１は、信頼性をさらに向上させることができる。

なお、本実施の形態では、第２判定部１７が、第１判定部１６によって正常と判定された搬送波位相観測値φ１に対応する疑似距離観測値ρ１が正常であるか否かの判定を行う場合を例に説明したが、これに限られない。第２判定部１７は、第１判定部１６によって正常と判定された搬送波位相観測値φ２、に対応する疑似距離観測値ρ２が正常であるか否かの判定を行ってもよい。

以上、図面を参照して、本開示の実施の形態について詳しく説明してきたが、具体的な構成は上述のものに限られることはなく、本開示の要旨を逸脱しない範囲内において様々な設計変更等が可能である。

上記実施の形態１，２では、姿勢角算出部１３が、姿勢角Ａ１を算出できない場合にのみ、搬送波位相観測値φ１，φ２の一方を異なるタイミングで複数取得して姿勢角Ａ３を算出する場合を例に説明したが、これに限られない。

例えば、航法計算装置１を搭載した車両が長期間トンネル内を走行している場合、その期間中、ＧＰＳ受信結果に基づく慣性航法の計算の補正が行われないため、姿勢角Ａ２にはドリフト誤差が蓄積され続けてしまう。この場合、大きなドリフト誤差を含む姿勢角Ａ２と、姿勢角Ａ１と、の間には許容範囲外の差分が生じてしまうため、たとえ車両がトンネルを抜け出したとしても、依然としてＧＰＳ受信結果に基づく慣性航法の計算の補正は行われなくなってしまう。

このような問題を解決するため、姿勢角算出部１３は、姿勢角Ａ１を算出できる場合でも、姿勢角Ａ１とともに姿勢角Ａ３を算出するようにしても良い。このとき、第１判定部１６は、姿勢角Ａ１，Ａ３の差分が所定閾値以下の場合、かつ、これら姿勢角Ａ１，Ａ３と、航法計算部１５によって算出された姿勢角Ａ２と、の差分が許容範囲外の場合には、慣性センサの検出結果に異常があるものと判定する。そして、航法計算部１５は、姿勢角Ａ１，Ａ３の差分が所定閾値以下の場合には、姿勢角Ａ１，Ａ３と姿勢角Ａ２との差分が許容範囲外の場合でも、姿勢角Ａ１，Ａ３の算出に用いられた搬送波位相観測値及び疑似距離観測値を慣性航法の計算の補正に用いる。それにより、航法計算部１５は、姿勢角Ａ２に大きなドリフト誤差が蓄積されている場合でも、そのドリフト誤差を補正して、自装置の位置、速度、及び、姿勢角等を精度良く算出することができる。以下、図１１のフローチャートを用いて詳細に説明する。

図１１は、航法計算装置１の変形例の動作を示すフローチャートである。
以下では、主に、図８に示すフローチャートと異なる内容について説明する。

姿勢角算出部１３は、ＧＰＳ受信機１１，１２による受信結果に基づいて、自装置の姿勢角Ａ１だけでなく姿勢角Ａ３を算出する（ステップＳ３０１）。姿勢角Ａ１，Ａ３の算出方法については、既に説明した通りであるため省略する。

例えば、姿勢角算出部１３によって姿勢角Ａ１，Ａ３が算出された場合（ステップＳ３０２のＸ）、第１判定部１６は、姿勢角Ａ１，Ａ３の差分が所定の閾値以下であるか否かを判定する（ステップＳ３０３）。姿勢角Ａ１，Ａ３の差分が所定の閾値以下である場合（ステップＳ３０３のＹＥＳ）、第１判定部１６は、姿勢角Ａ１，Ａ３の算出に用いられた搬送波位相観測値及び疑似距離観測値が正常であると判定する（ステップＳ１０７）。この場合、航法計算部１５は、姿勢角Ａ１，Ａ３と、姿勢角Ａ２と、の差分に関わらず（即ち、許容範囲外の場合でも）、姿勢角Ａ１，Ａ３の算出に用いられた搬送波位相観測値及び疑似距離観測値を慣性航法の計算の補正に用いる（ステップＳ１０８）。それにより、航法計算部１５は、姿勢角Ａ２に大きなドリフト誤差が蓄積されている場合でも、そのドリフト誤差を補正して、自装置の位置、速度、及び、姿勢角等を精度良く算出することができる。

他方、姿勢角Ａ１，Ａ３の差分が所定の閾値を超える場合（ステップＳ３０３のＮＯ）、第１判定部１６は、姿勢角Ａ１，Ａ３のうち姿勢角Ａ２との差分が許容範囲内の姿勢角を抽出する（ステップＳ１０５→ステップＳ１０６のＹＥＳ→ステップＳ１０７）。航法計算部１５は、第１判定部１６によって抽出された姿勢角の算出に用いられた搬送波位相観測値及び疑似距離観測値を慣性航法の計算の補正に用いる（ステップＳ１０８）。

なお、ステップＳ３０２において、姿勢角Ａ１，Ａ３の何れか一方のみが算出された場合（ステップＳ３０２のＹ）、第１判定部１６は、姿勢角算出部１３によって算出された姿勢角（Ａ１又はＡ３）と、姿勢角Ａ２と、の比較を行う（ステップＳ１０５）。その後のステップＳ１０６〜Ｓ１１０の処理は、実施の形態１で既に説明した通りであるため、省略する。

また、ステップＳ３０２において、姿勢角Ａ１，Ａ３の何れも算出されなかった場合（ステップＳ３０２のＺ）、航法計算部１５は、搬送波位相観測値φ１，φ２及びそれらに対応する疑似距離観測値ρ１，ρ２を慣性航法の計算の補正に用いない（ステップＳ１１３）。

図１１では、航法計算装置１に設けられた姿勢角算出部１３が姿勢角Ａ１，Ａ３を共に算出する場合を例に説明したが、これに限られない。当然ながら、航法計算装置２に設けられた姿勢角算出部１３が姿勢角Ａ１，Ａ３を共に算出するように構成されても良い。

また、上記実施の形態１，２では、ＧＰＳ衛星機器から受信した信号に基づいて姿勢角Ａ１，Ａ３が算出される場合を例に説明したが、これに限られない。例えば、移動体の速度に応じた周波数のパルスとして出力される車速パルスに基づいて姿勢角Ａ１，Ａ３が算出されてもよい。

さらに、上記実施の形態１，２では、本開示をハードウェアの構成として説明したが、本開示は、これに限定されるものではない。本開示は、航法計算装置１，２による計算処理を、ＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）にコンピュータプログラムを実行させることにより実現することが可能である。

また、上述したプログラムは、様々なタイプの非一時的なコンピュータ可読媒体（ｎｏｎ−ｔｒａｎｓｉｔｏｒｙ ｃｏｍｐｕｔｅｒ ｒｅａｄａｂｌｅ ｍｅｄｉｕｍ）を用いて格納され、コンピュータに供給することができる。非一時的なコンピュータ可読媒体は、様々なタイプの実体のある記録媒体（ｔａｎｇｉｂｌｅ ｓｔｏｒａｇｅ ｍｅｄｉｕｍ）を含む。非一時的なコンピュータ可読媒体は、例えば、磁気記録媒体、光磁気記録媒体、ＣＤ−ＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）、ＣＤ−Ｒ、ＣＤ−Ｒ／Ｗ、半導体メモリを含む。磁気記録媒体は、例えば、フレキシブルディスク、磁気テープ、ハードディスクドライブなどである。光磁気記録媒体は、例えば光磁気ディスクなどである。半導体メモリは、例えば、マスクＲＯＭ、ＰＲＯＭ（Ｐｒｏｇｒａｍｍａｂｌｅ ＲＯＭ）、ＥＰＲＯＭ（Ｅｒａｓａｂｌｅ ＰＲＯＭ）、フラッシュＲＯＭ、ＲＡＭ（Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）などである。また、プログラムは、様々なタイプの一時的なコンピュータ可読媒体（ｔｒａｎｓｉｔｏｒｙ ｃｏｍｐｕｔｅｒ ｒｅａｄａｂｌｅ ｍｅｄｉｕｍ）によってコンピュータに供給されてもよい。一時的なコンピュータ可読媒体の例は、電気信号、光信号、及び電磁波を含む。一時的なコンピュータ可読媒体は、電線及び光ファイバ等の有線通信路、又は無線通信路を介して、プログラムをコンピュータに供給できる。

＜関連技術と上記実施の形態１，２に係る航法計算装置との差異＞
特許文献２に開示された異常値判定装置は、異常値判定装置を搭載した車両が移動中にのみ単体のＧＰＳ受信機により受信した情報に基づいて姿勢角を算出している。それに対し、上記実施の形態１，２に係る航法計算装置は、当該航法計算装置を搭載した移動体が移動中の場合だけでなく停止中の場合でも、複数のＧＰＳ受信機により受信した信号に基づいて姿勢角を算出することができる。

１ 航法計算装置
２ 航法計算装置
１１ ＧＰＳ受信機
１２ ＧＰＳ受信機
１３ 姿勢角算出部
１４ 慣性センサ
１５ 航法計算部
１６ 第１判定部
１７ 第２判定部

Claims (8)

1. 複数の衛星機器からの信号を受信して第１搬送波位相観測値及び第１疑似距離観測値を算出する第１受信機と、
   前記複数の衛星機器からの信号を受信して第２搬送波位相観測値及び第２疑似距離観測値を算出する第２受信機と、
   前記第１受信機によって算出された第１搬送波位相観測値と、前記第２受信機によって算出された第２搬送波位相観測値と、に基づいて第１姿勢角を算出する姿勢角算出部と、
   自装置の加速度及び角速度を検出する慣性センサと、
   前記慣性センサによる検出結果を用いて慣性航法の計算を行うことにより、少なくとも第２姿勢角を算出する航法計算部と、
   前記第１及び前記第２姿勢角を比較することにより、前記第１及び前記第２搬送波位相観測値が正常であるか否かを判定する第１判定部と、
   を備え、
   前記航法計算部は、前記第１判定部によって前記第１及び前記第２搬送波位相観測値が正常であると判定された場合、前記第１及び前記第２搬送波位相観測値の何れかを、前記慣性航法の計算の補正に用いるように構成され、
   前記姿勢角算出部は、前記第１姿勢角を算出できない場合、前記第１及び前記第２搬送波位相観測値のうち異なるタイミングで取得された何れか一方の複数の搬送波位相観測値に基づいて、第３姿勢角を算出し、前記第１姿勢角として出力するように構成されている、
   航法計算装置。
2. 前記第１判定部は、前記第１姿勢角と前記第２姿勢角との差分が許容範囲内である場合に前記第１及び前記第２搬送波位相観測値が正常であると判定し、前記第１姿勢角と前記第２姿勢角との差分が許容範囲外である場合に前記第１及び前記第２搬送波位相観測値に異常があると判定する、
   請求項１に記載の航法計算装置。
3. 前記航法計算部は、前記第１判定部によって前記第１及び前記第２搬送波位相観測値に異常があると判定された場合、前記第１及び前記第２搬送波位相観測値を、前記慣性航法の計算の補正に用いないように構成されている、
   請求項１又は２に記載の航法計算装置。
4. 前記第１判定部によって正常であると判定された前記第１及び前記第２搬送波位相観測値の一方の単位時間当たりの変化量と、前記第１及び前記第２疑似距離観測値の一方の単位時間当たりの変化量と、の差分に基づいて、前記第１及び前記第２疑似距離観測値の前記一方が正常であるか否かを判定する第２判定部をさらに備え、
   前記航法計算部は、前記第１判定部によって前記第１及び前記第２搬送波位相観測値が正常であると判定され、かつ、前記第２判定部によって前記第１及び前記第２疑似距離観測値の前記一方が正常であると判定された場合、前記第１及び前記第２搬送波位相観測値の前記一方と、前記第１及び前記第２疑似距離観測値の前記一方と、を前記慣性航法の計算の補正に用いるように構成されている、
   請求項１〜３の何れか一項に記載の航法計算装置。
5. 前記航法計算部は、前記第１判定部によって前記第１及び前記第２搬送波位相観測値に異常があると判定された場合、又は、前記第２判定部によって前記第１及び前記第２疑似距離観測値の前記一方に異常があると判定された場合、前記第１及び前記第２搬送波位相観測値と、前記第１及び前記第２疑似距離観測値と、を前記慣性航法の計算の補正に用いないように構成されている、
   請求項４に記載の航法計算装置。
6. 前記姿勢角算出部は、前記第１姿勢角を算出できる場合でも、前記第１及び前記第２搬送波位相観測値のうち異なるタイミングで取得された何れか一方の複数の搬送波位相観測値に基づいて、前記第３姿勢角を算出するように構成され、
   前記航法計算部は、前記第１姿勢角及び前記第３姿勢角の差分が所定の閾値以下の場合、前記第１及び前記第３姿勢角と、前記第２姿勢角と、のそれぞれの差分が許容範囲内であるか否かに関わらず、前記第１及び前記第２搬送波位相観測値の何れかを、前記慣性航法の計算の補正に用いるように構成されている、
   請求項１〜５の何れか一項に記載の航法計算装置。
7. 第１受信機を用いて、複数の衛星機器からの信号を受信して第１搬送波位相観測値及び第１疑似距離観測値を算出するステップと、
   第２受信機を用いて、前記複数の衛星機器からの信号を受信して第２搬送波位相観測値及び第２疑似距離観測値を算出するステップと、
   姿勢角算出部を用いて、前記第１受信機によって算出された第１搬送波位相観測値と、前記第２受信機によって算出された第２搬送波位相観測値と、に基づいて第１姿勢角を算出するステップと、
   慣性センサを用いて、自装置の加速度及び角速度を検出するステップと、
   航法計算部を用いて、前記慣性センサによる検出結果を用いて慣性航法の計算を行うことにより、少なくとも第２姿勢角を算出するステップと、
   第１判定部を用いて、前記第１及び前記第２姿勢角を比較することにより、前記第１及び前記第２搬送波位相観測値が正常であるか否かを判定するステップと、
   を備え、
   前記第２姿勢角を算出するステップでは、前記第１判定部によって前記第１及び前記第２搬送波位相観測値が正常であると判定された場合、前記第１及び前記第２搬送波位相観測値の何れかを、前記慣性航法の計算の補正に用いるように構成され、
   前記第１姿勢角を算出するステップでは、前記第１姿勢角を算出できない場合、前記第１及び前記第２搬送波位相観測値のうち異なるタイミングで取得された何れか一方の複数の搬送波位相観測値に基づいて、第３姿勢角を算出し、前記第１姿勢角として出力するように構成されている、
   航法計算方法。
8. 第１受信機を用いて、複数の衛星機器からの信号を受信して第１搬送波位相観測値及び第１疑似距離観測値を算出する処理と、
   第２受信機を用いて、前記複数の衛星機器からの信号を受信して第２搬送波位相観測値及び第２疑似距離観測値を算出する処理と、
   姿勢角算出部を用いて、前記第１受信機によって算出された第１搬送波位相観測値と、前記第２受信機によって算出された第２搬送波位相観測値と、に基づいて第１姿勢角を算出する処理と、
   慣性センサを用いて、自装置の加速度及び角速度を検出する処理と、
   航法計算部を用いて、前記慣性センサによる検出結果を用いて慣性航法の計算を行うことにより、少なくとも第２姿勢角を算出する処理と、
   第１判定部を用いて、前記第１及び前記第２姿勢角を比較することにより、前記第１及び前記第２搬送波位相観測値が正常であるか否かを判定する処理と、
   をコンピュータに実行させ、さらに、
   前記第２姿勢角を算出する処理では、前記第１判定部によって前記第１及び前記第２搬送波位相観測値が正常であると判定された場合、前記第１及び前記第２搬送波位相観測値の何れかを、前記慣性航法の計算の補正に用いるように構成され、
   前記第１姿勢角を算出する処理では、前記第１姿勢角を算出できない場合、前記第１及び前記第２搬送波位相観測値のうち異なるタイミングで取得された何れか一方の複数の搬送波位相観測値に基づいて、第３姿勢角を算出し、前記第１姿勢角として出力するように構成されている、
   航法計算プログラム。

Images