姿勢標定装置、姿勢標定方法および姿勢標定プログラム
【課題】姿勢角の標定処理において、少なくとも１本の基線ベクトルに対する二重位相差の観測値が得られる場合に姿勢角の誤差の拡大を抑えることを可能にする。
【解決手段】二重位相差算出部１２０は複数のＧＰＳ受信機９５２が観測した搬送波位相に基づいて二重位相差の観測値を算出する。また、二重位相差推定部１３０は姿勢角の標定値に基づいて二重位相差の推定値を算出する。次に、二重位相差残差計算部１４０は二重位相差の観測値と二重位相差の推定値との差分を算出する。そして、カルマンフィルタ１５０は二重位相差の観測値と推定値との差分に基づいて補正量を算出し、姿勢角計算部１１０はジャイロ９５３が測定したレートを積分して算出した姿勢角の計算値を補正量で補正して姿勢角の標定値を算出する。
【解決手段】二重位相差算出部１２０は複数のＧＰＳ受信機９５２が観測した搬送波位相に基づいて二重位相差の観測値を算出する。また、二重位相差推定部１３０は姿勢角の標定値に基づいて二重位相差の推定値を算出する。次に、二重位相差残差計算部１４０は二重位相差の観測値と二重位相差の推定値との差分を算出する。そして、カルマンフィルタ１５０は二重位相差の観測値と推定値との差分に基づいて補正量を算出し、姿勢角計算部１１０はジャイロ９５３が測定したレートを積分して算出した姿勢角の計算値を補正量で補正して姿勢角の標定値を算出する。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）とジャイロセンサ（以下、ジャイロとする）を用いた姿勢標定処理における姿勢標定装置、姿勢標定方法および姿勢標定プログラムに関するものである。
【背景技術】
【０００２】
姿勢標定技術にはＧＰＳが用いられ、ＧＰＳだけ（非カップリング）を用いて姿勢標定するもの、ＧＰＳとジャイロをカップリングして姿勢標定するものがある。
ＧＰＳを用いる従来の姿勢標定では、同時刻の搬送波位相について二重位相差を観測する必要があり、ＧＰＳ受信機間の受信タイミングの同期を取るために、全てのＧＰＳ受信機を単一の発信器で駆動する、いわゆる専用受信機が使われている。
また、ＧＰＳだけ（非カップリング）を用いた従来の姿勢標定では、エポック（観測周期）毎に搬送波位相の整数値バイアスを探索して二重位相差を求めている。そして、二重位相差で特定される各ＧＰＳ受信機の相対位置に基づいて２本以上の基線ベクトルを算出し、２本以上の基線ベクトルに基づいて姿勢角を算出している。
また、ＧＰＳとジャイロをカップリングした従来の姿勢標定では、ジャイロの測定値に基づく姿勢角とＧＰＳ受信機の観測値に基づく姿勢角との差分に基づいてジャイロの測定値に基づく姿勢角を補正（観測更新）して姿勢角を標定している。そして、ＧＰＳ受信機の観測値に基づく姿勢角の計算値を算出するため、上記のＧＰＳだけ（非カップリング）を用いた従来の姿勢標定と同じく、二重位相差を算出して２本以上の基線ベクトルを算出している。
上記従来の姿勢標定では、１本の基線ベクトルで方位角と仰角とが定まり、２本の基線ベクトルで形成される平面に対して回転角が定まることを利用して姿勢角（回転角［ロール］、仰角［ピッチ］、方位角［ヨー］）を算出している。
【特許文献１】特開平１１−９４５７３号公報
【発明の開示】
【発明が解決しようとする課題】
【０００３】
しかし、ＧＰＳを用いる従来の姿勢標定は、専用受信機を使用するため用途が限定され、ハードウェアの構成も特殊となることから姿勢標定装置は必然的に高価なものとなる。
そして、ＧＰＳだけ（非カップリング）を用いた従来の姿勢標定では、２本以上の基線ベクトルに対して二重位相差の観測値が得られていない場合、姿勢角を得ることが出来ない。つまり、３機以上のＧＰＳ受信機について搬送波位相を観測し二重位相差の観測値が得られていない場合、２本以上の基線ベクトルを算出できず姿勢角を得ることが出来ない。
さらに、エポック毎に搬送波位相の整数値バイアスを探索する必要があるため、計算負荷が高く、前回のエポックまでに算出した姿勢角を利用して不適切な観測値を棄却することが出来ない。
また、ＧＰＳとジャイロをカップリングした従来の姿勢標定では、カップリングに姿勢角が利用されているため、２本以上の基線ベクトルに対して二重位相差が観測されてなければ観測更新が出来ずに姿勢角の標定値の精度が落ちる。
つまり、３機以上のＧＰＳ受信機が同時にＧＰＳ観測を行う必要があるためＧＰＳ観測のアベイラビリティが低下する可能性が高くなる。そこで、姿勢角の標定値の精度を落とさないためには安定性の高いジャイロが必要となり姿勢標定装置が比較的高価なものとなる。
また、オイラー角（局所座標系［例えば、ジャイロの測定値が属する座標系］での姿勢角）には特異点（ピッチが９０°）が存在し、特異点ではロールとヨーとを区別して算出することができない。このため、３つ以上のＧＰＳ受信機が同時にＧＰＳ観測が行える場合でも、特異点においては観測更新が正常に出来ず、姿勢角の標定値の精度が落ちる。
【０００４】
本発明は、例えば、少なくとも１本の基線ベクトルに対する二重位相差の観測値が得られる場合に姿勢角の誤差の拡大を抑えることを可能にすることを目的とする。また、例えば、ＧＰＳ観測のアベイラビリティを向上することを目的とする。また、例えば、安定性の低い低価格なジャイロを使用した構成を可能にすることを目的とする。
【課題を解決するための手段】
【０００５】
本発明の姿勢標定装置は、ジャイロの測定値を用いて計算した姿勢角の計算値をＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）受信機の観測値に基づいて補正して姿勢角の標定値を算出する姿勢標定装置であり、前記ジャイロの測定値に基づき中央処理装置を用いて姿勢角の計算値を算出する姿勢角計算部と、複数のＧＰＳ受信機が観測した搬送波位相を入力し中央処理装置を用いて複数のＧＰＳ受信機における二重位相差の観測値を算出する二重位相差算出部と、算出した姿勢角の標定値に基づき中央処理装置を用いて複数のＧＰＳ受信機における二重位相差の推定値を算出する二重位相差推定部と、前記二重位相差算出部が算出した複数のＧＰＳ受信機における二重位相差の観測値と前記二重位相差推定部が算出した複数のＧＰＳ受信機における二重位相差の推定値との差分を中央処理装置を用いて算出する二重位相差残差計算部と、前記二重位相差残差計算部が算出した複数のＧＰＳ受信機における二重位相差の観測値と推定値との差分に基づき前記姿勢角計算部が算出した姿勢角の計算値に対する補正量を中央処理装置を用いて算出する補正量算出部と、前記補正量算出部が算出した補正量に基づき前記姿勢角計算部が算出した姿勢角の計算値を中央処理装置を用いて補正して姿勢角の標定値を算出する姿勢角標定部とを備えたことを特徴とする。
【発明の効果】
【０００６】
本発明によれば、オイラー姿勢角ではなくで二重位相差を利用してＧＰＳとジャイロとをカップリング、つまり、観測更新することにより、例えば、少なくとも１本の基線ベクトルに対する二重位相差の観測値を得られている場合に姿勢角の誤差の拡大を抑えることが可能である。また、例えば、ＧＰＳ観測のアベイラビリティを向上することができる。また、例えば、安定性の低い低価格なジャイロを使用した構成を可能にすることができる。
【発明を実施するための最良の形態】
【０００７】
二重位相差を利用してＧＰＳとジャイロとをカップリングする姿勢標定装置、姿勢標定方法および姿勢標定プログラムについて以下に説明する。
特に、複数のＧＰＳ受信機における二重位相差の観測値とジャイロの測定値に基づく二重位相差の推定値との差分に基づいて、ジャイロの測定値に基づく姿勢角の計算値を補正し姿勢角を標定する姿勢標定装置、姿勢標定方法および姿勢標定プログラムについて説明する。
【０００８】
実施の形態１．
図１は、実施の形態１における姿勢標定システム１００の構成の概要図である。
１つのプラットフォーム９５４上にＧＰＳアンテナ９５１とＧＰＳ受信機９５２とジャイロ９５３とを配置し、一緒に動くようにする。このとき、複数のＧＰＳアンテナ９５１間の相対位置関係は拘束されており、変化しない。
姿勢標定装置１０１はＧＰＳアンテナ９５１とＧＰＳ受信機９５２とジャイロ９５３とが配置された姿勢標定対象（特に、移動体）の姿勢角（ロール［回転角］、ピッチ［仰角］、ヨー［方位角］）を標定する。
ここで、複数のＧＰＳアンテナ９５１間の相対位置関係が保てるのであれば、必ずしもプラットフォーム９５４を設ける必要はない。
また、ＧＰＳアンテナ９５１間の基線ベクトルにより形成される座標系の座標軸とジャイロ９５３の測定値が属する座標系の座標軸とを平行に位置付けるため、ジャイロ９５３の設置姿勢のアライメントを物理的に実施するか、座標軸のずれをソフトウェア的に算出して補正する処理を実装する。以下、姿勢標定装置１０１が、ジャイロ９５３の座標軸のずれをソフトウェア的に算出して補正する処理を実装する形態について説明する。ここで、基線ベクトルとは、２点間の距離と方向を示すベクトルであり、例えば、図１における主局から従局（１）へのベクトルや主局から従局（２）へのベクトルのことである。
図１では、姿勢標定システム１００は、ＧＰＳアンテナ９５１およびＧＰＳ受信機９５２をプラットフォーム９５４に３台備えているが、備える台数は２台でも４台以上でも構わない。３台のＧＰＳ受信機９５２のうち最低２台がＧＰＳアンテナ９５１を介してＧＰＳ衛星から測位信号である搬送波を受信し観測値を算出（ＧＰＳ観測）できれば、１つの基線ベクトルが定まり、姿勢標定装置１０１は姿勢角を観測更新し標定することができる。
また、ジャイロ９５３は直交する３軸それぞれの方向について角速度を測定する３軸ジャイロである。
なお、姿勢標定装置１０１は、ジャイロ９５３が出力した角速度を積分して姿勢角の算出を行う。以下、ジャイロ９５３が出力する角速度に対してソフトウェア的に補正量を算出してレベリングし、レベリングした角速度を姿勢角の算出に使用する姿勢標定装置１０１について説明する。
【０００９】
図２は、実施の形態１における姿勢標定システム１００のハードウェア構成図である。
姿勢標定システム１００は、複数のＧＰＳアンテナ９５１と各ＧＰＳアンテナ９５１に接続するＧＰＳ受信機９５２と、３軸方向（ロール、ピッチ、ヨー）の角速度を測定するジャイロ９５３と、姿勢標定装置１０１とを備える。
各ＧＰＳ受信機９５２は、それぞれが接続するＧＰＳアンテナ９５１がＧＰＳ衛星から受信した測位信号（Ｌ１、Ｌ２、Ｌ５など）の搬送波に基づいて搬送波位相、衛星軌道情報、ＧＰＳ単独測位結果などの観測値を出力する。
ジャイロ９５３は３軸方向（ロール、ピッチ、ヨー）の角速度（以下、レートという）を測定し出力する。
【００１０】
姿勢標定装置１０１は、プログラムを実行するＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ：中央処理装置）９１１を備えている。ＣＰＵ９１１はバスを介して記憶装置９２０と接続され、記憶装置９２０はＲＡＭ、ＲＯＭ、磁気ディスク装置などの記憶機器で構成される。
記憶装置９２０には、オペレーティングシステム（ＯＳ）９２１、プログラム群９２３、ファイル群９２４が記憶されている。プログラム群９２３は、ＣＰＵ９１１、ＯＳ９２１により実行される。
上記プログラム群９２３には、実施の形態の説明において「〜部」、「カルマンフィルタ」、「アンテナセパレーション」として説明する機能を実行するプログラムが記憶されている。プログラムは、ＣＰＵ９１１により読み出され実行される。
ファイル群９２４には、実施の形態の説明において、「〜情報」として説明するデータ、「〜部」として説明する機能を実行した際の判定結果・演算結果を示すデータ、「〜部」として説明する機能を実行するプログラム間で受け渡しするデータなどが「〜ファイル」として記憶されている。例えば、標定した姿勢角、ジャイロ９５３が出力したレート、ＧＰＳ受信機９５２が出力した搬送波位相などの情報が「〜ファイル」として記憶される。
また、実施の形態の説明において、フローチャートや構成図の矢印の部分は主としてデータの入出力を示し、そのデータの入出力のためにデータは、記憶装置９２０、あるいは、信号線やその他の伝送媒体により伝送される。
また、実施の形態の説明において「〜部」として説明するものは、ＲＯＭに記憶されたファームウェアで実現されていても構わない。或いは、ソフトウェアのみ、或いは、ハードウェアのみ、或いは、ソフトウェアとハードウェアとの組み合わせ、さらには、ファームウェアとの組み合わせで実施されても構わない。
【００１１】
図３は、実施の形態１における姿勢標定装置１０１の機能構成図である。
実施の形態１における姿勢標定装置１０１が備える姿勢標定装置１０１の機能構成を図３に基づいて以下に説明する。
初期値取得部２１０は姿勢角の初期値を取得する。
アンテナセパレーション２３０は各ＧＰＳアンテナ９５１の幾何学的配置（機体座標系の基線ベクトルｂＢＯＤＹ）を算出する。
方向余弦行列計算部２２０は姿勢角に基づいて機体座標系の基線ベクトルｂＢＯＤＹを標定座標系の基線ベクトルｂＮＥＤに変換する方向余弦行列ＣＢＮを計算する。以下、局所座標系（姿勢標定装置１０１が標定する姿勢角の座標系）をＮＥＤ（Ｎｏｒｔｈ Ｅａｓｔ Ｄｏｗｎ）座標系として説明する。
デザイン行列計算部２４０は、ＧＰＳ受信機９５２が出力する衛星軌道情報を入力し、衛星軌道情報により定まる方向余弦基底ベクトルｅ（以下、ＬＯＳ［Ｌｉｎｅ Ｏｆ Ｓｉｇｈｔ］ベクトルｅとする）に基づくデザイン行列Ａを計算する。方向余弦基底ベクトルｅはＧＰＳ受信機９５２からＧＰＳ衛星への基底となるベクトルを示す。
二重位相差推定部１３０はＮＥＤ座標系の基線ベクトルｂＮＥＤとデザイン行列Ａとに基づいて二重位相差の推定値Ａｂλを算出する。
二重位相差算出部１２０は各ＧＰＳ受信機９５２が出力したそれぞれの搬送波位相φに基づいて二重位相差の観測値Ｗλを算出する。
二重位相差残差計算部１４０は二重位相差の推定値Ａｂλと二重位相差の観測値Ｗλとの差分ｄＺλを計算する。
カルマンフィルタ１５０（補正量算出部の一例）は、状態量（例えば、姿勢角やジャイロセンサ測定誤差の推定値）のダイナミクスをモデル化した状態方程式、観測量（例えば、二重位相差の観測値）と状態量との関係を定式化した観測方程式の両方程式に基づいて状態量の誤差推定を行い状態量の推定精度を改善するフィードバックフィルタであり、差分ｄＺλに基づいてジャイロ９５３のレート誤差補正量・ジャイロ９５３とＧＰＳアンテナ９５１との軸ずれ補正量・姿勢角誤差補正量などの補正量を算出する。
レート補正部２５０（姿勢角標定部の一例）はジャイロ９５３が出力したレートをレート誤差補正量に基づいて補正する。
姿勢角計算部１１０（姿勢角標定部の一例）は、ジャイロ９５３が出力しレート補正部２５０が補正したレートで前回の姿勢角の標定値または姿勢角の初期値を積分し、軸ずれ補正量、姿勢角誤差補正量で補正して姿勢角の標定値を算出する。
【００１２】
図４は、実施の形態１における姿勢標定システム１００の姿勢標定処理を示すフローチャートである。
実施の形態１における姿勢標定システム１００が実行する姿勢標定処理の流れ（姿勢標定方法）について図４に基づいて以下に説明する。
なお、姿勢標定処理はコンピュータに実行させることができる。姿勢標定処理をコンピュータに実行させるプログラムが姿勢標定プログラムである。
【００１３】
姿勢標定装置１０１は、ＧＰＳ受信機９５２が出力した搬送波位相φに基づいて二重位相差の観測値Ｗλを算出する（Ｓ１０１〜Ｓ１０２：二重位相差観測値算出処理）と共に、ジャイロ９５３が測定したレートに基づいて二重位相差の推定値Ａｂλを算出し（Ｓ１０３〜Ｓ１０８：二重位相差推定値算出処理）、二重位相差の観測値Ｗλと二重位相差の推定値Ａｂλとの差分ｄＺλに基づく補正量で姿勢角を補正し姿勢角の標定値を算出する（Ｓ１０９〜Ｓ１１１：姿勢角標定値算出処理）。
【００１４】
ここで、姿勢標定処理における二重位相差の観測値と推定値との関係について説明する。
まず、二重位相差について説明する。
１機の同じＧＰＳ衛星に対する２台のＧＰＳ受信機の観測値の差を受信機間の一重差といい、２機のＧＰＳ衛星に対するＧＰＳ受信機の観測値の差を衛星間の一重差という。そして、２つの受信機間の一重差同士の差、または２つの衛星間の一重差同士の差を二重差といい、二重位相差は位相の二重差のことである。
【００１５】
図５は、基線ベクトルｂとＬＯＳベクトルｅとＧＰＳアンテナからＧＰＳ衛星までの距離ρとの関係図である。
図５において、「ａ」，「ｂ」はＧＰＳアンテナを示し、「ｉ」，「ｊ」はＧＰＳ衛星を示す。そして、「ｂｂａ」はＧＰＳアンテナａからＧＰＳアンテナｂへの基線ベクトルを示す。また、「ｅａｉ」はＧＰＳアンテナａからＧＰＳ衛星ｉへのＬＯＳベクトルを示し、「ｅａｊ」はＧＰＳアンテナａからＧＰＳ衛星ｊへのＬＯＳベクトルを示す。また、「ρａｉ」はＧＰＳアンテナａからＧＰＳ衛星ｉまでの距離を示し、「ρｂｉ」はＧＰＳアンテナｂからＧＰＳ衛星ｉまでの距離を示し、「ρａｊ」はＧＰＳアンテナａからＧＰＳ衛星ｊまでの距離を示し、「ρｂｊ」はＧＰＳアンテナｂからＧＰＳ衛星ｊまでの距離を示す。
【００１６】
図５において、距離の二重差「ρａｂｊｉ」はＧＰＳアンテナからＧＰＳ衛星までの距離ρを用いて以下の［式１］で表わされる。
【００１７】
ρａｂｊｉ＝（ρｂｊ−ρａｊ）−（ρｂｉ−ρａｉ） ［式１］
【００１８】
また、ＧＰＳアンテナからＧＰＳ衛星までの距離ρはＧＰＳアンテナ−ＧＰＳ衛星間の搬送波の数に搬送波の波長λを乗算した値で表わされ、搬送波の数はＧＰＳ受信機が観測可能なドップラー（搬送波位相の時間変化率）の積算値である搬送波位相φと、整数値バイアスと呼ばれる搬送波の数の整数値Ｎとの合計値で表わされる。つまり、ＧＰＳアンテナからＧＰＳ衛星までの距離ρは以下の［式２］で表わされる。
【００１９】
ρ＝λ（φ＋Ｎ） ［式２］
【００２０】
ここで、［式２］を用いて［式１］を表わすと以下の［式３］になる。
【００２１】
ρａｂｊｉ＝λ（φａｂｊｉ−Ｎａｂｊｉ） ［式３］
【００２２】
そして、［式３］の「λ（φａｂｊｉ−Ｎａｂｊｉ）」が距離の二重差の観測値に対応し、「φａｂｊｉ−Ｎａｂｊｉ」が二重位相差の観測値に対応する。
【００２３】
また、図５において、距離の二重差「ρａｂｊｉ」は基線ベクトルｂとＬＯＳベクトルｅとを用いて以下の［式４］で表わされる。
【００２４】
ρａｂｊｉ＝−（ｅａｊ−ｅａｉ）×ｂｂａ ［式４］
【００２５】
［式４］において、基線ベクトルｂｂａは姿勢角に応じて機体座標系からＮＥＤ座標系に変換して算出される計算値であり、［式４］の「−（ｅａｊ−ｅａｉ）×ｂｂａ」が距離の二重差の推定値に対応し、「（−（ｅａｊ−ｅａｉ）×ｂｂａ）／λ」が二重位相差の推定値に対応する。
【００２６】
また、基線ベクトルｂは４つのＧＰＳ衛星の位置関係に基づいて特定されるため、ＧＰＳ衛星を「ｉ」と「ｊ」の２つから「ｉ」・「ｊ」・「ｋ」・「ｌ」の４つに拡張すると上記の［式３］と［式４］とを用いて以下の［関係式５］、［関係式６］が成り立つ。
【００２７】
【数１】
【００２８】
Ｗ＝Ａｂ ［関係式６］
【００２９】
そして、姿勢標定装置１０１は、上記［関係式５］、［関係式６］において二重位相差の観測値に対応する「Ｗ／λ（以下、二重位相差の観測値Ｗλとする）」を算出する（Ｓ１０１〜Ｓ１０２：二重位相差観測値算出処理）と共に、二重位相差の推定値に対応する「Ａｂ／λ（以下、二重位相差の推定値Ａｂλとする）」を算出し（Ｓ１０３〜Ｓ１０８：二重位相差推定値算出処理）、ＷλとＡｂλとの差分ｄＺλに基づく補正量で姿勢角を補正し姿勢角の標定値を算出する（Ｓ１０９〜Ｓ１１１：姿勢角標定値算出処理）。
【００３０】
各処理の詳細を以下に説明する。
二重位相差の観測値Ｗλを算出する二重位相差観測値算出処理（Ｓ１０１〜Ｓ１０２）について説明する。
＜二重位相差観測値算出処理＞
まず、各ＧＰＳ受信機９５２は、それぞれが接続するＧＰＳアンテナ９５１を介して、４機のＧＰＳ衛星から搬送波を受信し、各ＧＰＳ衛星について搬送波位相φと衛星軌道情報とを観測し出力する（Ｓ１０１：搬送波位相観測処理）。
そして、二重位相差算出部１２０は、ＧＰＳ受信機９５２が出力した搬送波位相φに基づいて二重位相差の観測値Ｗλを算出する。このとき、二重位相差算出部１２０は上記［関係式５］におけるＷを整数値バイアスＮを省いて「Ｗ＝λφ」で表わし搬送波の波長λで除算してＷλを算出する（Ｓ１０２：二重位相差算出処理）
【００３１】
次に、二重位相差の推定値Ａｂλを算出する二重位相差推定値算出処理（Ｓ１０３〜Ｓ１０８）について説明する。
＜二重位相差推定値算出処理＞
デザイン行列計算部２４０は、各ＧＰＳ受信機９５２が出力した衛星軌道情報で特定される４機のＧＰＳ衛星の位置関係に基づいて各ＬＯＳベクトルｅを算出し、上記［関係式５］に示すデザイン行列Ａを計算する（Ｓ１０３：基底ベクトル行列計算処理）。
また、初期値取得部２１０は任意の方法により姿勢角の初期値を取得する。初期値取得部２１０は、例えば、Ｈａｔｃｈ法により整数値バイアスＮを算出して上記［関係式５］を満たす基線ベクトルｂを２つ算出し、２つの基線ベクトルｂに基づいて姿勢角の初期値を算出する。また、ジャイロ９５３が測定したレートと重力方向との関係式に基づいて算出したロール（回転角）およびピッチ（仰角）と、コンパスを有してコンパスが測定したヨー（方位角）とを姿勢角の初期値に用いてもよい。また、キーボードなどの入力装置を有してユーザに姿勢角の初期値を指定させてもよい（Ｓ１０４：姿勢角初期値取得処理）。
次に、姿勢角計算部１１０は、初期値取得部２１０が取得した姿勢角の初期値をジャイロ９５３が出力したレートで積分して当該時点における姿勢角を算出する（Ｓ１０５：姿勢角計算処理）。
次に、方向余弦行列計算部２２０は、姿勢角計算部１１０が算出した姿勢角に基づいて、機体座標系の基線ベクトルｂＢＯＤＹをＮＥＤ座標系の基線ベクトルｂＮＥＤに変換する方向余弦行列ＣＢＮを計算する（Ｓ１０６：座標変換行列計算処理）。
次に、二重位相差推定部１３０は、方向余弦行列計算部２２０が計算した方向余弦行列ＣＢＮを用いて局所座標系の基線ベクトルｂＢＯＤＹを変換しＮＥＤ座標系の基線ベクトルｂＮＥＤを算出する。このとき、アンテナセパレーション２３０は、あらかじめ、各ＧＰＳアンテナ９５１の幾何学的配置に基づいて局所座標系の基線ベクトルｂＢＯＤＹを算出し記憶機器（例えば、記憶装置９２０）に記憶しておくものとする（Ｓ１０７：基線ベクトル座標系変換処理）。
次に、二重位相差推定部１３０は、デザイン行列計算部２４０が計算したデザイン行列Ａと二重位相差推定部１３０が算出したＮＥＤ座標系の基線ベクトルｂＮＥＤとを乗じた値を搬送波の波長λで除算して二重位相差の推定値Ａｂλを算出する（Ｓ１０８：二重位相差推定処理）。
【００３２】
次に、二重位相差の観測値Ｗλと二重位相差の推定値Ａｂλとの差分ｄＺλに基づく補正量で姿勢角を補正し姿勢角の標定値を算出する姿勢角標定値算出処理（Ｓ１０９〜Ｓ１１１）について説明する。
【００３３】
姿勢角標定値算出処理（Ｓ１０９〜Ｓ１１１）は、姿勢角標定対象に搭載された複数のＧＰＳ受信機９５２による搬送波位相観測値の二重位相差（二重位相差の観測値）とジャイロ９５３のレート出力を積分することで得られる幾何学的な二重位相差の予測値（二重位相差の推定値）との残差（差分）により、ジャイロ９５３が出力するレートや姿勢角計算値及びＧＰＳアンテナアレイとジャイロ９５３のアライメント誤差等の補正量を計算する処理である。
姿勢角を使用して観測更新をする従来の姿勢標定方法では、アレイを構成するＧＰＳアンテナ９５１間の２本以上の基線ベクトルに対して二重位相差が同時に観測できる必要がある。一方、実施の形態１で説明する二重位相差を使用して観測更新する姿勢標定方法では、アレイを構成する複数のＧＰＳアンテナ９５１間の少なくとも１本の基線に対して二重位相差が観測できていればよい。このため、観測更新を頻繁に行え、安定性の低い低価格なジャイロ９５３を使用して姿勢角を精度良く標定することが可能である。また、ＧＰＳ受信機９５２の観測周期が、ジャイロ誤差が拡大する時間に対し十分短ければ、二重位相差の観測残差は±０．５λ（波長）に収まり二重位相差の観測残差は小数値に現れるため、以下に説明するように、整数値バイアスを求めることなく観測更新を行うことができる。さらに、２本以上の基線ベクトルについて処理する場合でも、基線ベクトル間でタイミングを取る必要はない。つまり、２本の基線ベクトルに対する二重位相差の算出の周期を同一にする必要はなく、ＧＰＳ受信機９５２の選択の自由度が大きくなる。
【００３４】
＜姿勢角標定値算出処理＞
まず、二重位相差残差計算部１４０は、二重位相差算出部１２０が算出した二重位相差の観測値Ｗλと二重位相差推定部１３０が算出した二重位相差の推定値Ａｂλとの差分ｄＺλを算出する。このとき、差分ｄＺλの整数値はＧＰＳ受信機の観測周期が、ジャイロ誤差が拡大する時間に対し十分短ければ波長λを超えないため、二重位相差の観測残差は、二重位相差残差計算部１４０は差分ｄＺλの小数値を二重位相差の差分ｄＺλとする（Ｓ１０９：二重位相差残差計算処理）。
次に、カルマンフィルタ１５０は、二重位相差残差計算部１４０が算出した差分ｄＺλを使用して状態方程式と観測方程式により、ジャイロ９５３のレート誤差補正量、ジャイロ９５３とＧＰＳアンテナ９５１との軸ずれ補正量、姿勢角誤差補正量などの補正量を算出する（Ｓ１１０：補正量算出処理）。
そして、姿勢角計算部１１０は、前回の姿勢角の標定値（初回は初期値取得部２１０が取得した姿勢角の初期値）をジャイロ９５３が出力したレートで積分し、積分して算出した当該時点の姿勢角を姿勢角誤差補正量に基づいて補正し姿勢角の標定値を計算する。このとき、レート補正部２５０は、ジャイロ９５３が出力したレートをレート誤差補正量で補正する。また、姿勢角計算部１１０は、補正されたレートを積分して積分値（３軸方向それぞれの変動角度）を軸ずれ補正量で補正する。そして、姿勢角計算部１１０は、補正した積分値を前回の姿勢角の標定値に加算して当該時点の姿勢角を計算し、当該時点の姿勢角を姿勢角誤差補正量に基づいて補正して姿勢角の標定値を計算する（Ｓ１１１：姿勢角標定処理）。
【００３５】
姿勢標定システム１００は、姿勢角標定処理（Ｓ１１１）で姿勢角を標定後、姿勢角の標定値を使用して座標変換行列計算処理（Ｓ１０６）から処理を繰り返し、当該時点における姿勢角を標定する。
【００３６】
上記二重位相差算出処理（Ｓ１０２）において、二重位相差算出部１２０は、搬送波を受信したタイミングを複数のＧＰＳ受信機９５２間でソフトウェア的に同期させて、二重位相差の観測値Ｗλを算出する。
【００３７】
このとき、複数のＧＰＳ受信機９５２がＧＰＳ衛星から測位信号の搬送波を受信したタイミングを精度よく求め、二重位相差算出部１２０が、例えば、主局のＧＰＳ受信機９５２の搬送波位相の観測値を従局のＧＰＳ受信機９５２の受信時刻に合わせて補間し、タイミングの同期を図っている。これにより、基本的には、どんな汎用のＧＰＳ受信機９５２でも姿勢標定システム１００に使用することが出来、低価格な姿勢標定システム１００を構成することができる。また、機能、性能（受信周波数、観測値の出力周期など）が異なる複数のＧＰＳ受信機９５２を組み合わせて姿勢標定システム１００に使用することができる。
【００３８】
また、二重位相差の観測値Ｗλを算出する方法には、１周波の搬送波を利用する方法と複数の周波（例えば、２周波）の搬送波を利用する方法とがある。
【００３９】
２周波受信機は、現在は高価であるが、Ｌ５等の新周波が公開された後は価格が低下すると思われる。そこで、２周波受信機を使用した姿勢標定システム１００を想定すると、２周波の搬送波位相の観測値について線形結合を取ったワイドレーンの搬送波位相を用いて二重位相差の観測値を算出することができる。そして、ワイドレーンでの搬送波の波長は単周波（１周波）の波長に比べて長く、このため、ワイドレーンの搬送波位相を用いることで引き込み可能な残差（観測値と推定値との二重位相差の差分）の範囲が広がる。例えば、Ｌ１の１周波の波長が約１９ｃｍ（センチメートル）であるに対し、Ｌ１とＬ５との２周波を線形結合したワイドレーンでの波長は約７５ｃｍになる。そして、観測値と推定値との距離の二重差の差分が±０．５λ（波長）に収まると仮定すると、１周波（Ｌ１）では距離の二重差の差分が±９．５ｃｍを超えると二重位相差の差分が小数値に収まらないのに対し、２周波（Ｌ１とＬ５）では距離の二重差の差分が±３７．５ｃｍを超えなければ二重位相差の差分が小数値に収まる。また、上記二重位相差残差計算処理（Ｓ１０９）において、二重位相差残差計算部１４０は差分ｄＺλの小数値を二重位相差の差分として処理するため、二重位相差の差分が小数値に収まらなければ標定する姿勢角の精度が落ちる。つまり、引き込み可能な残差の範囲が広がるほど、上記二重位相差残差計算処理（Ｓ１０９）の整合性が高まり、精度の高い姿勢角を標定することが可能になる。
【００４０】
以下に、１周波を利用する方法と複数の周波を利用する方法とのそれぞれについて、二重位相差算出部１２０と二重位相差算出処理（Ｓ１０２）とを説明する。
【００４１】
図６は、実施の形態１における１周波を利用する二重位相差算出部１２０の構成図である。
１周波を利用する二重位相差算出部１２０は以下の機能要素を備える。
受信タイミング算出部１２１は各ＧＰＳ受信機９５２が搬送波を受信した受信時刻ｔｒｖを算出する。
搬送波位相補間計算部１２２は補間計算を行い複数のＧＰＳ受信機９５２について同じ受信時刻における搬送波位相φＸを算出する。
二重位相差ベクトル計算部１２３は複数のＧＰＳ受信機９５２の同じ受信時刻における搬送波位相φについて二重位相差の観測値Ｗλを計算する。
【００４２】
図７は、実施の形態１における１周波を利用する二重位相差算出処理（Ｓ１０２）を示すフローチャートである。
１周波を利用する二重位相差算出処理（Ｓ１０２）を図７に基づいて以下に説明する。
このとき、上記搬送波位相観測処理（Ｓ１０１）において、各ＧＰＳ受信機９５２が１周波（例えば、Ｌ１）の搬送波を受信して搬送波位相φＸ（φａ、φｂ、φｃ）を出力したものとする。
【００４３】
＜二重位相差算出処理（Ｓ１０２）＞
まず、受信タイミング算出部１２１は、各ＧＰＳ受信機９５２から搬送波の観測時刻ｔｒｖｏを取得し、最小自乗法などにより各ＧＰＳ受信機９５２のクロックバイアスｔｒｖＢｉａｓを計算する。そして、搬送波の観測時刻ｔｒｖｏからクロックバイアスｔｒｖＢｉａｓを減算して搬送波の受信時刻ｔｒｖを算出する（Ｓ２０１：受信タイミング算出処理）。
次に、搬送波位相補間計算部１２２は、各ＧＰＳ受信機９５２が算出した搬送波位相φＸと受信タイミング算出部１２１が算出した各ＧＰＳ受信機９５２の搬送波の受信時刻ｔｒｖＸ（ｔｒｖａ，ｔｒｖｂ，ｔｒｖｃ）とに基づいて補間計算を行い同じ受信時刻における搬送波位相φＸ（ｔ）を算出する。補間計算は線形補間、一次補間、二次補間など任意の補間計算で構わない。例えば、二次補間計算の場合、搬送波位相補間計算部１２２は、図８に示す受信時刻の関係において、各ＧＰＳ受信機９５２が出力した搬送波位相φａ、φｂ、φｃを受信時刻ｋで同期した搬送波位相φａ（ｋ）、φｂ（ｋ）、φｃ（ｋ）を以下の［式７］を計算して算出する（Ｓ２０２：搬送波位相補間計算処理）。
【００４４】
【数２】
【００４５】
そして、二重位相差ベクトル計算部１２３は、搬送波位相補間計算部１２２が算出した搬送波位相φＸ（ｔ）に基づいて二重位相差の観測値Ｗλ（ベクトル）を算出する。このとき、二重位相差ベクトル計算部１２３は上記［関係式５］におけるＷを整数値バイアスＮを省いて「Ｗ＝λφ」で表わし搬送波の波長λで除算してＷλを算出する。例えば、ＧＰＳ衛星ｉとＧＰＳ衛星ｊそれぞれについて、主局の搬送波位相φａｉ、φａｊと従局１の搬送波位相φｂｉ、φｂｊとの二重位相差の観測値φａｂｊｉ（スカラー）を算出する場合、二重位相差ベクトル計算部１２３は以下の［式８］を計算する。そして、４機のＧＰＳ衛星についての二重位相差の観測値Ｗλ（ベクトル）を算出する（Ｓ２０３：二重位相差観測値計算処理）。
【００４６】
φａｂｊｉ＝φｂｊ−φａｊ−（φｂｉ−φａｉ） ［式８］
【００４７】
図９は、実施の形態１における２周波を利用する二重位相差算出部１２０の構成図である。
２周波を利用する二重位相差算出部１２０は、図９に基づいて説明した１周波を利用する二重位相差算出部１２０の機能要素に加えて線形結合計算部１２４を備える。
線形結合計算部１２４は各ＧＰＳ受信機９５２が出力した複数の周波の搬送波位相φＸＹを線形計算して線形結合後の搬送波位相φＸを算出する。
【００４８】
図１０は、実施の形態１における複数の周波を利用する二重位相差算出処理（Ｓ１０２）を示すフローチャートである。
複数の周波を利用する二重位相差算出処理（Ｓ１０２）を図１０に基づいて以下に説明する。
このとき、上記搬送波位相観測処理（Ｓ１０１）において、各ＧＰＳ受信機９５２が２周波（例えば、Ｌ１とＬ５）の搬送波を受信して搬送波位相φＸＹ（φａ１、φａ５、φｂ１、φｂ５、φｃ１、φｃ５）を出力したものとする（φａ１，φｂ１，φｃ１：第１の搬送波位相の一例）（φａ５，φｂ５，φｃ５：第２の搬送波位相の一例）。
【００４９】
＜二重位相差算出処理（Ｓ１０２）＞
まず、受信タイミング算出部１２１は、各ＧＰＳ受信機９５２から各周波それぞれの搬送波の観測時刻ｔｒｖｏを取得し、最小自乗法により各ＧＰＳ受信機９５２のクロックバイアスｔｒｖＢｉａｓを計算する。そして、搬送波の観測時刻ｔｒｖｏからクロックバイアスｔｒｖＢｉａｓを減算して搬送波の受信時刻ｔｒｖを算出する（Ｓ２０１：受信タイミング算出処理）。
次に、線形結合計算部１２４は各ＧＰＳ受信機９５２が出力した各周波の搬送波位相φＸＹを線形計算して線形結合後の搬送波位相φＸを算出する（Ｓ３０２：線形結合計算処理）。
以下、二重位相差算出部１２０は、Ｓ３０３とＳ３０４とをそれぞれ前記Ｓ２０２とＳ２０３と同様に実行する。
【００５０】
図６、図９には３台のＧＰＳ受信機９５２を示しているが、ＧＰＳ受信機９５２は３台以上あればよく、台数を増して構わない。
また、受信タイミング算出部１２１、搬送波位相補間計算部１２２、二重位相差ベクトル計算部１２３および線形結合計算部１２４をＧＰＳ受信機９５２に対応して複数示しているが、１つずつで構わない。
また、基線ベクトル間で同期して二重位相差の観測値を算出する必要はなく、例えば、図６と図９とに３つ示している搬送波位相補間計算部１２２の真ん中の１つは処理しなくても構わない。
【００５１】
上記実施の形態１で説明した姿勢標定装置１０１は、ＧＰＳアンテナ９５１間の２つの基線ベクトルを求めて算出した姿勢角の初期値をジャイロ９５３のレート出力により更新（積分）し、時間の経過とともに拡大する各種誤差を、ＧＰＳ観測に基づく二重位相差を利用して観測更新することで、精度よく姿勢角を出力する装置である。
特に、図３に（１）と（２）で示す、二重位相差算出部１２０と二重位相差残差計算部１４０とカルマンフィルタ１５０とが特徴点である。
【００５２】
上記実施の形態１で説明した姿勢標定装置１０１は、ＧＰＳとジャイロをカップリングすることにより、以下のメリットを有する。
まず、ＧＰＳ衛星可視時は、観測更新によりジャイロ誤差の高精度な補正を行うことができ、姿勢角の高精度化が可能である。
また、ＧＰＳ衛星が不可視の時でもジャイロが出力するレートで積分することで姿勢角の出力が可能である。
また、ＧＰＳ受信機の観測周期に比べ、一般的にジャイロの出力周期は早いため、高周波で姿勢角を出力することが可能である。
【００５３】
上記実施の形態１で説明した姿勢標定装置１０１は、単独測位等により高精度に求まる受信時刻を用いて搬送波位相観測値の補間計算を行い、ＧＰＳ受信機間の同期をソフトウェア的に取っている。これにより、上記実施の形態１で説明した姿勢標定装置は以下のメリットを有する。
基本的には、どんな汎用受信機（ＧＰＳ受信機）でも使用することが出来、低価格な姿勢標定システム１００を構成することができる。
また、機能、性能（受信周波数、観測値の出力周期など）の異なる複数のＧＰＳ受信機を組み合わせることが可能となる。そこで、例えば、姿勢標定システム１００は測位システムの構成に最適なＧＰＳ受信機を測位システムと兼用して姿勢角を標定することができる。
【００５４】
上記実施の形態１で説明した姿勢標定装置１０１は、従来の姿勢標定方法で利用されたオイラー姿勢角ではなく、二重位相差を利用して観測更新することにより、以下のメリットを有する。
まず、ＧＰＳのみを利用した従来の姿勢角の決定には同時刻に２本以上の基線ベクトルに対する二重差位相差の観測値を得ることが必要であるのに対し、少なくとも１本の基線ベクトルに対する二重差位相差の観測値が得られていれば、観測更新が行え、誤差の拡大を抑えることが可能である。
つまり、ＧＰＳ受信機の観測値に基づく観測更新のアベイラビリティが向上することから、安定性の低い低価格なジャイロを使用した構成が可能である。
また、オイラー角にはその特質上、特異点（ピッチ９０°）が存在し、観測更新にオイラー角を利用する従来の姿勢標定方法が特異点において適切な観測更新を実施できないのに対し、観測更新に二重位相差を利用する上記実施の形態１の姿勢標定装置１０１では特異点において適切な観測更新を実施できないということは一切ない。
また、二重位相差の整数値バイアスを正常に求めて姿勢角の絶対量（姿勢角の初期値）を決定した後は、ジャイロのレート出力を積分することで姿勢角を時間更新することが可能である。
また、ＧＰＳ受信機の観測周期が、ジャイロ誤差が拡大する時間に対し十分短ければ、二重位相差の観測残差は±０．５λ（波長）に収まることから、整数値バイアスを求めることなく観測更新を行うことができる。
つまり、二重位相差の観測残差が特定の閾値（例えば、±０．２λ）を超えた場合、何らかの原因によりＧＰＳ受信機が不適切な観測値を出力した、または、ジャイロが不適切なレートを出力したと判断することができ、これらのデータを適切に棄却することができる。
また、現在の高価な２周波受信機または将来は低価格になることが予想されるＬ５等の新周波対応受信機を使用すれば、２周波の線形結合によりワイドレーンを利用して観測残差を算出することができる。これにより、二重位相差の残差を引き込める領域（例えば、±０．５λ）の範囲を広げることが出来る。
また、ＧＰＳとジャイロとをカップリングするときに利用する観測値がオイラー姿勢角である場合に比べ、二重位相差のノイズモデル（カルマンフィルタで使用する状態方程式や観測方程式）のほうが把握しやすく、推定精度が向上する。
【００５５】
上記実施の形態１の説明におけるＧＰＳは、Ｇａｌｉｌｅｏ、ＧＬＯＮＡＳＳ（ＧＬｏｂａｌ ＮＡｖｉｇａｔｉｏｎ Ｓａｔｅｌｌｉｔｅ Ｓｙｓｔｅｍ）などのその他のＧＮＳＳ（Ｇｌｏｂａｌ Ｎａｖｉｇａｔｉｏｎ Ｓａｔｅｌｌｉｔｅ Ｓｙｓｔｅｍ）であっても構わない。
【図面の簡単な説明】
【００５６】
【図１】実施の形態１における姿勢標定システム１００の構成の概要図。
【図２】実施の形態１における姿勢標定システム１００のハードウェア構成図。
【図３】実施の形態１における姿勢標定装置１０１の機能構成図。
【図４】実施の形態１における姿勢標定システム１００の姿勢標定処理を示すフローチャート。
【図５】基線ベクトルｂとＬＯＳベクトルｅとＧＰＳアンテナからＧＰＳ衛星までの距離ρとの関係図。
【図６】実施の形態１における１周波を利用する二重位相差算出部１２０の構成図。
【図７】実施の形態１における１周波を利用する二重位相差算出処理（Ｓ１０２）を示すフローチャート。
【図８】主局と従局とにおける搬送波受信時刻の関係図。
【図９】実施の形態１における２周波を利用する二重位相差算出部１２０の構成図。
【図１０】実施の形態１における複数の周波を利用する二重位相差算出処理（Ｓ１０２）を示すフローチャート。
【符号の説明】
【００５７】
１００ 姿勢標定システム、１０１ 姿勢標定装置、１１０ 姿勢角計算部、１２０ 二重位相差算出部、１２１ 受信タイミング算出部、１２２ 搬送波位相補間計算部、１２３ 二重位相差ベクトル計算部、１２４ 線形結合計算部、１３０ 二重位相差推定部、１４０ 二重位相差残差計算部、１５０ カルマンフィルタ、２１０ 初期値取得部、２２０ 方向余弦行列計算部、２３０ アンテナセパレーション、２４０ デザイン行列計算部、２５０ レート補正部、９１１ ＣＰＵ、９２０ 記憶装置、９２１ ＯＳ、９２３ プログラム群、９２４ ファイル群、９５１ ＧＰＳアンテナ、９５２ ＧＰＳ受信機、９５３ ジャイロ、９５４ プラットフォーム。
【技術分野】
【０００１】
本発明は、ＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）とジャイロセンサ（以下、ジャイロとする）を用いた姿勢標定処理における姿勢標定装置、姿勢標定方法および姿勢標定プログラムに関するものである。
【背景技術】
【０００２】
姿勢標定技術にはＧＰＳが用いられ、ＧＰＳだけ（非カップリング）を用いて姿勢標定するもの、ＧＰＳとジャイロをカップリングして姿勢標定するものがある。
ＧＰＳを用いる従来の姿勢標定では、同時刻の搬送波位相について二重位相差を観測する必要があり、ＧＰＳ受信機間の受信タイミングの同期を取るために、全てのＧＰＳ受信機を単一の発信器で駆動する、いわゆる専用受信機が使われている。
また、ＧＰＳだけ（非カップリング）を用いた従来の姿勢標定では、エポック（観測周期）毎に搬送波位相の整数値バイアスを探索して二重位相差を求めている。そして、二重位相差で特定される各ＧＰＳ受信機の相対位置に基づいて２本以上の基線ベクトルを算出し、２本以上の基線ベクトルに基づいて姿勢角を算出している。
また、ＧＰＳとジャイロをカップリングした従来の姿勢標定では、ジャイロの測定値に基づく姿勢角とＧＰＳ受信機の観測値に基づく姿勢角との差分に基づいてジャイロの測定値に基づく姿勢角を補正（観測更新）して姿勢角を標定している。そして、ＧＰＳ受信機の観測値に基づく姿勢角の計算値を算出するため、上記のＧＰＳだけ（非カップリング）を用いた従来の姿勢標定と同じく、二重位相差を算出して２本以上の基線ベクトルを算出している。
上記従来の姿勢標定では、１本の基線ベクトルで方位角と仰角とが定まり、２本の基線ベクトルで形成される平面に対して回転角が定まることを利用して姿勢角（回転角［ロール］、仰角［ピッチ］、方位角［ヨー］）を算出している。
【特許文献１】特開平１１−９４５７３号公報
【発明の開示】
【発明が解決しようとする課題】
【０００３】
しかし、ＧＰＳを用いる従来の姿勢標定は、専用受信機を使用するため用途が限定され、ハードウェアの構成も特殊となることから姿勢標定装置は必然的に高価なものとなる。
そして、ＧＰＳだけ（非カップリング）を用いた従来の姿勢標定では、２本以上の基線ベクトルに対して二重位相差の観測値が得られていない場合、姿勢角を得ることが出来ない。つまり、３機以上のＧＰＳ受信機について搬送波位相を観測し二重位相差の観測値が得られていない場合、２本以上の基線ベクトルを算出できず姿勢角を得ることが出来ない。
さらに、エポック毎に搬送波位相の整数値バイアスを探索する必要があるため、計算負荷が高く、前回のエポックまでに算出した姿勢角を利用して不適切な観測値を棄却することが出来ない。
また、ＧＰＳとジャイロをカップリングした従来の姿勢標定では、カップリングに姿勢角が利用されているため、２本以上の基線ベクトルに対して二重位相差が観測されてなければ観測更新が出来ずに姿勢角の標定値の精度が落ちる。
つまり、３機以上のＧＰＳ受信機が同時にＧＰＳ観測を行う必要があるためＧＰＳ観測のアベイラビリティが低下する可能性が高くなる。そこで、姿勢角の標定値の精度を落とさないためには安定性の高いジャイロが必要となり姿勢標定装置が比較的高価なものとなる。
また、オイラー角（局所座標系［例えば、ジャイロの測定値が属する座標系］での姿勢角）には特異点（ピッチが９０°）が存在し、特異点ではロールとヨーとを区別して算出することができない。このため、３つ以上のＧＰＳ受信機が同時にＧＰＳ観測が行える場合でも、特異点においては観測更新が正常に出来ず、姿勢角の標定値の精度が落ちる。
【０００４】
本発明は、例えば、少なくとも１本の基線ベクトルに対する二重位相差の観測値が得られる場合に姿勢角の誤差の拡大を抑えることを可能にすることを目的とする。また、例えば、ＧＰＳ観測のアベイラビリティを向上することを目的とする。また、例えば、安定性の低い低価格なジャイロを使用した構成を可能にすることを目的とする。
【課題を解決するための手段】
【０００５】
本発明の姿勢標定装置は、ジャイロの測定値を用いて計算した姿勢角の計算値をＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）受信機の観測値に基づいて補正して姿勢角の標定値を算出する姿勢標定装置であり、前記ジャイロの測定値に基づき中央処理装置を用いて姿勢角の計算値を算出する姿勢角計算部と、複数のＧＰＳ受信機が観測した搬送波位相を入力し中央処理装置を用いて複数のＧＰＳ受信機における二重位相差の観測値を算出する二重位相差算出部と、算出した姿勢角の標定値に基づき中央処理装置を用いて複数のＧＰＳ受信機における二重位相差の推定値を算出する二重位相差推定部と、前記二重位相差算出部が算出した複数のＧＰＳ受信機における二重位相差の観測値と前記二重位相差推定部が算出した複数のＧＰＳ受信機における二重位相差の推定値との差分を中央処理装置を用いて算出する二重位相差残差計算部と、前記二重位相差残差計算部が算出した複数のＧＰＳ受信機における二重位相差の観測値と推定値との差分に基づき前記姿勢角計算部が算出した姿勢角の計算値に対する補正量を中央処理装置を用いて算出する補正量算出部と、前記補正量算出部が算出した補正量に基づき前記姿勢角計算部が算出した姿勢角の計算値を中央処理装置を用いて補正して姿勢角の標定値を算出する姿勢角標定部とを備えたことを特徴とする。
【発明の効果】
【０００６】
本発明によれば、オイラー姿勢角ではなくで二重位相差を利用してＧＰＳとジャイロとをカップリング、つまり、観測更新することにより、例えば、少なくとも１本の基線ベクトルに対する二重位相差の観測値を得られている場合に姿勢角の誤差の拡大を抑えることが可能である。また、例えば、ＧＰＳ観測のアベイラビリティを向上することができる。また、例えば、安定性の低い低価格なジャイロを使用した構成を可能にすることができる。
【発明を実施するための最良の形態】
【０００７】
二重位相差を利用してＧＰＳとジャイロとをカップリングする姿勢標定装置、姿勢標定方法および姿勢標定プログラムについて以下に説明する。
特に、複数のＧＰＳ受信機における二重位相差の観測値とジャイロの測定値に基づく二重位相差の推定値との差分に基づいて、ジャイロの測定値に基づく姿勢角の計算値を補正し姿勢角を標定する姿勢標定装置、姿勢標定方法および姿勢標定プログラムについて説明する。
【０００８】
実施の形態１．
図１は、実施の形態１における姿勢標定システム１００の構成の概要図である。
１つのプラットフォーム９５４上にＧＰＳアンテナ９５１とＧＰＳ受信機９５２とジャイロ９５３とを配置し、一緒に動くようにする。このとき、複数のＧＰＳアンテナ９５１間の相対位置関係は拘束されており、変化しない。
姿勢標定装置１０１はＧＰＳアンテナ９５１とＧＰＳ受信機９５２とジャイロ９５３とが配置された姿勢標定対象（特に、移動体）の姿勢角（ロール［回転角］、ピッチ［仰角］、ヨー［方位角］）を標定する。
ここで、複数のＧＰＳアンテナ９５１間の相対位置関係が保てるのであれば、必ずしもプラットフォーム９５４を設ける必要はない。
また、ＧＰＳアンテナ９５１間の基線ベクトルにより形成される座標系の座標軸とジャイロ９５３の測定値が属する座標系の座標軸とを平行に位置付けるため、ジャイロ９５３の設置姿勢のアライメントを物理的に実施するか、座標軸のずれをソフトウェア的に算出して補正する処理を実装する。以下、姿勢標定装置１０１が、ジャイロ９５３の座標軸のずれをソフトウェア的に算出して補正する処理を実装する形態について説明する。ここで、基線ベクトルとは、２点間の距離と方向を示すベクトルであり、例えば、図１における主局から従局（１）へのベクトルや主局から従局（２）へのベクトルのことである。
図１では、姿勢標定システム１００は、ＧＰＳアンテナ９５１およびＧＰＳ受信機９５２をプラットフォーム９５４に３台備えているが、備える台数は２台でも４台以上でも構わない。３台のＧＰＳ受信機９５２のうち最低２台がＧＰＳアンテナ９５１を介してＧＰＳ衛星から測位信号である搬送波を受信し観測値を算出（ＧＰＳ観測）できれば、１つの基線ベクトルが定まり、姿勢標定装置１０１は姿勢角を観測更新し標定することができる。
また、ジャイロ９５３は直交する３軸それぞれの方向について角速度を測定する３軸ジャイロである。
なお、姿勢標定装置１０１は、ジャイロ９５３が出力した角速度を積分して姿勢角の算出を行う。以下、ジャイロ９５３が出力する角速度に対してソフトウェア的に補正量を算出してレベリングし、レベリングした角速度を姿勢角の算出に使用する姿勢標定装置１０１について説明する。
【０００９】
図２は、実施の形態１における姿勢標定システム１００のハードウェア構成図である。
姿勢標定システム１００は、複数のＧＰＳアンテナ９５１と各ＧＰＳアンテナ９５１に接続するＧＰＳ受信機９５２と、３軸方向（ロール、ピッチ、ヨー）の角速度を測定するジャイロ９５３と、姿勢標定装置１０１とを備える。
各ＧＰＳ受信機９５２は、それぞれが接続するＧＰＳアンテナ９５１がＧＰＳ衛星から受信した測位信号（Ｌ１、Ｌ２、Ｌ５など）の搬送波に基づいて搬送波位相、衛星軌道情報、ＧＰＳ単独測位結果などの観測値を出力する。
ジャイロ９５３は３軸方向（ロール、ピッチ、ヨー）の角速度（以下、レートという）を測定し出力する。
【００１０】
姿勢標定装置１０１は、プログラムを実行するＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ：中央処理装置）９１１を備えている。ＣＰＵ９１１はバスを介して記憶装置９２０と接続され、記憶装置９２０はＲＡＭ、ＲＯＭ、磁気ディスク装置などの記憶機器で構成される。
記憶装置９２０には、オペレーティングシステム（ＯＳ）９２１、プログラム群９２３、ファイル群９２４が記憶されている。プログラム群９２３は、ＣＰＵ９１１、ＯＳ９２１により実行される。
上記プログラム群９２３には、実施の形態の説明において「〜部」、「カルマンフィルタ」、「アンテナセパレーション」として説明する機能を実行するプログラムが記憶されている。プログラムは、ＣＰＵ９１１により読み出され実行される。
ファイル群９２４には、実施の形態の説明において、「〜情報」として説明するデータ、「〜部」として説明する機能を実行した際の判定結果・演算結果を示すデータ、「〜部」として説明する機能を実行するプログラム間で受け渡しするデータなどが「〜ファイル」として記憶されている。例えば、標定した姿勢角、ジャイロ９５３が出力したレート、ＧＰＳ受信機９５２が出力した搬送波位相などの情報が「〜ファイル」として記憶される。
また、実施の形態の説明において、フローチャートや構成図の矢印の部分は主としてデータの入出力を示し、そのデータの入出力のためにデータは、記憶装置９２０、あるいは、信号線やその他の伝送媒体により伝送される。
また、実施の形態の説明において「〜部」として説明するものは、ＲＯＭに記憶されたファームウェアで実現されていても構わない。或いは、ソフトウェアのみ、或いは、ハードウェアのみ、或いは、ソフトウェアとハードウェアとの組み合わせ、さらには、ファームウェアとの組み合わせで実施されても構わない。
【００１１】
図３は、実施の形態１における姿勢標定装置１０１の機能構成図である。
実施の形態１における姿勢標定装置１０１が備える姿勢標定装置１０１の機能構成を図３に基づいて以下に説明する。
初期値取得部２１０は姿勢角の初期値を取得する。
アンテナセパレーション２３０は各ＧＰＳアンテナ９５１の幾何学的配置（機体座標系の基線ベクトルｂＢＯＤＹ）を算出する。
方向余弦行列計算部２２０は姿勢角に基づいて機体座標系の基線ベクトルｂＢＯＤＹを標定座標系の基線ベクトルｂＮＥＤに変換する方向余弦行列ＣＢＮを計算する。以下、局所座標系（姿勢標定装置１０１が標定する姿勢角の座標系）をＮＥＤ（Ｎｏｒｔｈ Ｅａｓｔ Ｄｏｗｎ）座標系として説明する。
デザイン行列計算部２４０は、ＧＰＳ受信機９５２が出力する衛星軌道情報を入力し、衛星軌道情報により定まる方向余弦基底ベクトルｅ（以下、ＬＯＳ［Ｌｉｎｅ Ｏｆ Ｓｉｇｈｔ］ベクトルｅとする）に基づくデザイン行列Ａを計算する。方向余弦基底ベクトルｅはＧＰＳ受信機９５２からＧＰＳ衛星への基底となるベクトルを示す。
二重位相差推定部１３０はＮＥＤ座標系の基線ベクトルｂＮＥＤとデザイン行列Ａとに基づいて二重位相差の推定値Ａｂλを算出する。
二重位相差算出部１２０は各ＧＰＳ受信機９５２が出力したそれぞれの搬送波位相φに基づいて二重位相差の観測値Ｗλを算出する。
二重位相差残差計算部１４０は二重位相差の推定値Ａｂλと二重位相差の観測値Ｗλとの差分ｄＺλを計算する。
カルマンフィルタ１５０（補正量算出部の一例）は、状態量（例えば、姿勢角やジャイロセンサ測定誤差の推定値）のダイナミクスをモデル化した状態方程式、観測量（例えば、二重位相差の観測値）と状態量との関係を定式化した観測方程式の両方程式に基づいて状態量の誤差推定を行い状態量の推定精度を改善するフィードバックフィルタであり、差分ｄＺλに基づいてジャイロ９５３のレート誤差補正量・ジャイロ９５３とＧＰＳアンテナ９５１との軸ずれ補正量・姿勢角誤差補正量などの補正量を算出する。
レート補正部２５０（姿勢角標定部の一例）はジャイロ９５３が出力したレートをレート誤差補正量に基づいて補正する。
姿勢角計算部１１０（姿勢角標定部の一例）は、ジャイロ９５３が出力しレート補正部２５０が補正したレートで前回の姿勢角の標定値または姿勢角の初期値を積分し、軸ずれ補正量、姿勢角誤差補正量で補正して姿勢角の標定値を算出する。
【００１２】
図４は、実施の形態１における姿勢標定システム１００の姿勢標定処理を示すフローチャートである。
実施の形態１における姿勢標定システム１００が実行する姿勢標定処理の流れ（姿勢標定方法）について図４に基づいて以下に説明する。
なお、姿勢標定処理はコンピュータに実行させることができる。姿勢標定処理をコンピュータに実行させるプログラムが姿勢標定プログラムである。
【００１３】
姿勢標定装置１０１は、ＧＰＳ受信機９５２が出力した搬送波位相φに基づいて二重位相差の観測値Ｗλを算出する（Ｓ１０１〜Ｓ１０２：二重位相差観測値算出処理）と共に、ジャイロ９５３が測定したレートに基づいて二重位相差の推定値Ａｂλを算出し（Ｓ１０３〜Ｓ１０８：二重位相差推定値算出処理）、二重位相差の観測値Ｗλと二重位相差の推定値Ａｂλとの差分ｄＺλに基づく補正量で姿勢角を補正し姿勢角の標定値を算出する（Ｓ１０９〜Ｓ１１１：姿勢角標定値算出処理）。
【００１４】
ここで、姿勢標定処理における二重位相差の観測値と推定値との関係について説明する。
まず、二重位相差について説明する。
１機の同じＧＰＳ衛星に対する２台のＧＰＳ受信機の観測値の差を受信機間の一重差といい、２機のＧＰＳ衛星に対するＧＰＳ受信機の観測値の差を衛星間の一重差という。そして、２つの受信機間の一重差同士の差、または２つの衛星間の一重差同士の差を二重差といい、二重位相差は位相の二重差のことである。
【００１５】
図５は、基線ベクトルｂとＬＯＳベクトルｅとＧＰＳアンテナからＧＰＳ衛星までの距離ρとの関係図である。
図５において、「ａ」，「ｂ」はＧＰＳアンテナを示し、「ｉ」，「ｊ」はＧＰＳ衛星を示す。そして、「ｂｂａ」はＧＰＳアンテナａからＧＰＳアンテナｂへの基線ベクトルを示す。また、「ｅａｉ」はＧＰＳアンテナａからＧＰＳ衛星ｉへのＬＯＳベクトルを示し、「ｅａｊ」はＧＰＳアンテナａからＧＰＳ衛星ｊへのＬＯＳベクトルを示す。また、「ρａｉ」はＧＰＳアンテナａからＧＰＳ衛星ｉまでの距離を示し、「ρｂｉ」はＧＰＳアンテナｂからＧＰＳ衛星ｉまでの距離を示し、「ρａｊ」はＧＰＳアンテナａからＧＰＳ衛星ｊまでの距離を示し、「ρｂｊ」はＧＰＳアンテナｂからＧＰＳ衛星ｊまでの距離を示す。
【００１６】
図５において、距離の二重差「ρａｂｊｉ」はＧＰＳアンテナからＧＰＳ衛星までの距離ρを用いて以下の［式１］で表わされる。
【００１７】
ρａｂｊｉ＝（ρｂｊ−ρａｊ）−（ρｂｉ−ρａｉ） ［式１］
【００１８】
また、ＧＰＳアンテナからＧＰＳ衛星までの距離ρはＧＰＳアンテナ−ＧＰＳ衛星間の搬送波の数に搬送波の波長λを乗算した値で表わされ、搬送波の数はＧＰＳ受信機が観測可能なドップラー（搬送波位相の時間変化率）の積算値である搬送波位相φと、整数値バイアスと呼ばれる搬送波の数の整数値Ｎとの合計値で表わされる。つまり、ＧＰＳアンテナからＧＰＳ衛星までの距離ρは以下の［式２］で表わされる。
【００１９】
ρ＝λ（φ＋Ｎ） ［式２］
【００２０】
ここで、［式２］を用いて［式１］を表わすと以下の［式３］になる。
【００２１】
ρａｂｊｉ＝λ（φａｂｊｉ−Ｎａｂｊｉ） ［式３］
【００２２】
そして、［式３］の「λ（φａｂｊｉ−Ｎａｂｊｉ）」が距離の二重差の観測値に対応し、「φａｂｊｉ−Ｎａｂｊｉ」が二重位相差の観測値に対応する。
【００２３】
また、図５において、距離の二重差「ρａｂｊｉ」は基線ベクトルｂとＬＯＳベクトルｅとを用いて以下の［式４］で表わされる。
【００２４】
ρａｂｊｉ＝−（ｅａｊ−ｅａｉ）×ｂｂａ ［式４］
【００２５】
［式４］において、基線ベクトルｂｂａは姿勢角に応じて機体座標系からＮＥＤ座標系に変換して算出される計算値であり、［式４］の「−（ｅａｊ−ｅａｉ）×ｂｂａ」が距離の二重差の推定値に対応し、「（−（ｅａｊ−ｅａｉ）×ｂｂａ）／λ」が二重位相差の推定値に対応する。
【００２６】
また、基線ベクトルｂは４つのＧＰＳ衛星の位置関係に基づいて特定されるため、ＧＰＳ衛星を「ｉ」と「ｊ」の２つから「ｉ」・「ｊ」・「ｋ」・「ｌ」の４つに拡張すると上記の［式３］と［式４］とを用いて以下の［関係式５］、［関係式６］が成り立つ。
【００２７】
【数１】
【００２８】
Ｗ＝Ａｂ ［関係式６］
【００２９】
そして、姿勢標定装置１０１は、上記［関係式５］、［関係式６］において二重位相差の観測値に対応する「Ｗ／λ（以下、二重位相差の観測値Ｗλとする）」を算出する（Ｓ１０１〜Ｓ１０２：二重位相差観測値算出処理）と共に、二重位相差の推定値に対応する「Ａｂ／λ（以下、二重位相差の推定値Ａｂλとする）」を算出し（Ｓ１０３〜Ｓ１０８：二重位相差推定値算出処理）、ＷλとＡｂλとの差分ｄＺλに基づく補正量で姿勢角を補正し姿勢角の標定値を算出する（Ｓ１０９〜Ｓ１１１：姿勢角標定値算出処理）。
【００３０】
各処理の詳細を以下に説明する。
二重位相差の観測値Ｗλを算出する二重位相差観測値算出処理（Ｓ１０１〜Ｓ１０２）について説明する。
＜二重位相差観測値算出処理＞
まず、各ＧＰＳ受信機９５２は、それぞれが接続するＧＰＳアンテナ９５１を介して、４機のＧＰＳ衛星から搬送波を受信し、各ＧＰＳ衛星について搬送波位相φと衛星軌道情報とを観測し出力する（Ｓ１０１：搬送波位相観測処理）。
そして、二重位相差算出部１２０は、ＧＰＳ受信機９５２が出力した搬送波位相φに基づいて二重位相差の観測値Ｗλを算出する。このとき、二重位相差算出部１２０は上記［関係式５］におけるＷを整数値バイアスＮを省いて「Ｗ＝λφ」で表わし搬送波の波長λで除算してＷλを算出する（Ｓ１０２：二重位相差算出処理）
【００３１】
次に、二重位相差の推定値Ａｂλを算出する二重位相差推定値算出処理（Ｓ１０３〜Ｓ１０８）について説明する。
＜二重位相差推定値算出処理＞
デザイン行列計算部２４０は、各ＧＰＳ受信機９５２が出力した衛星軌道情報で特定される４機のＧＰＳ衛星の位置関係に基づいて各ＬＯＳベクトルｅを算出し、上記［関係式５］に示すデザイン行列Ａを計算する（Ｓ１０３：基底ベクトル行列計算処理）。
また、初期値取得部２１０は任意の方法により姿勢角の初期値を取得する。初期値取得部２１０は、例えば、Ｈａｔｃｈ法により整数値バイアスＮを算出して上記［関係式５］を満たす基線ベクトルｂを２つ算出し、２つの基線ベクトルｂに基づいて姿勢角の初期値を算出する。また、ジャイロ９５３が測定したレートと重力方向との関係式に基づいて算出したロール（回転角）およびピッチ（仰角）と、コンパスを有してコンパスが測定したヨー（方位角）とを姿勢角の初期値に用いてもよい。また、キーボードなどの入力装置を有してユーザに姿勢角の初期値を指定させてもよい（Ｓ１０４：姿勢角初期値取得処理）。
次に、姿勢角計算部１１０は、初期値取得部２１０が取得した姿勢角の初期値をジャイロ９５３が出力したレートで積分して当該時点における姿勢角を算出する（Ｓ１０５：姿勢角計算処理）。
次に、方向余弦行列計算部２２０は、姿勢角計算部１１０が算出した姿勢角に基づいて、機体座標系の基線ベクトルｂＢＯＤＹをＮＥＤ座標系の基線ベクトルｂＮＥＤに変換する方向余弦行列ＣＢＮを計算する（Ｓ１０６：座標変換行列計算処理）。
次に、二重位相差推定部１３０は、方向余弦行列計算部２２０が計算した方向余弦行列ＣＢＮを用いて局所座標系の基線ベクトルｂＢＯＤＹを変換しＮＥＤ座標系の基線ベクトルｂＮＥＤを算出する。このとき、アンテナセパレーション２３０は、あらかじめ、各ＧＰＳアンテナ９５１の幾何学的配置に基づいて局所座標系の基線ベクトルｂＢＯＤＹを算出し記憶機器（例えば、記憶装置９２０）に記憶しておくものとする（Ｓ１０７：基線ベクトル座標系変換処理）。
次に、二重位相差推定部１３０は、デザイン行列計算部２４０が計算したデザイン行列Ａと二重位相差推定部１３０が算出したＮＥＤ座標系の基線ベクトルｂＮＥＤとを乗じた値を搬送波の波長λで除算して二重位相差の推定値Ａｂλを算出する（Ｓ１０８：二重位相差推定処理）。
【００３２】
次に、二重位相差の観測値Ｗλと二重位相差の推定値Ａｂλとの差分ｄＺλに基づく補正量で姿勢角を補正し姿勢角の標定値を算出する姿勢角標定値算出処理（Ｓ１０９〜Ｓ１１１）について説明する。
【００３３】
姿勢角標定値算出処理（Ｓ１０９〜Ｓ１１１）は、姿勢角標定対象に搭載された複数のＧＰＳ受信機９５２による搬送波位相観測値の二重位相差（二重位相差の観測値）とジャイロ９５３のレート出力を積分することで得られる幾何学的な二重位相差の予測値（二重位相差の推定値）との残差（差分）により、ジャイロ９５３が出力するレートや姿勢角計算値及びＧＰＳアンテナアレイとジャイロ９５３のアライメント誤差等の補正量を計算する処理である。
姿勢角を使用して観測更新をする従来の姿勢標定方法では、アレイを構成するＧＰＳアンテナ９５１間の２本以上の基線ベクトルに対して二重位相差が同時に観測できる必要がある。一方、実施の形態１で説明する二重位相差を使用して観測更新する姿勢標定方法では、アレイを構成する複数のＧＰＳアンテナ９５１間の少なくとも１本の基線に対して二重位相差が観測できていればよい。このため、観測更新を頻繁に行え、安定性の低い低価格なジャイロ９５３を使用して姿勢角を精度良く標定することが可能である。また、ＧＰＳ受信機９５２の観測周期が、ジャイロ誤差が拡大する時間に対し十分短ければ、二重位相差の観測残差は±０．５λ（波長）に収まり二重位相差の観測残差は小数値に現れるため、以下に説明するように、整数値バイアスを求めることなく観測更新を行うことができる。さらに、２本以上の基線ベクトルについて処理する場合でも、基線ベクトル間でタイミングを取る必要はない。つまり、２本の基線ベクトルに対する二重位相差の算出の周期を同一にする必要はなく、ＧＰＳ受信機９５２の選択の自由度が大きくなる。
【００３４】
＜姿勢角標定値算出処理＞
まず、二重位相差残差計算部１４０は、二重位相差算出部１２０が算出した二重位相差の観測値Ｗλと二重位相差推定部１３０が算出した二重位相差の推定値Ａｂλとの差分ｄＺλを算出する。このとき、差分ｄＺλの整数値はＧＰＳ受信機の観測周期が、ジャイロ誤差が拡大する時間に対し十分短ければ波長λを超えないため、二重位相差の観測残差は、二重位相差残差計算部１４０は差分ｄＺλの小数値を二重位相差の差分ｄＺλとする（Ｓ１０９：二重位相差残差計算処理）。
次に、カルマンフィルタ１５０は、二重位相差残差計算部１４０が算出した差分ｄＺλを使用して状態方程式と観測方程式により、ジャイロ９５３のレート誤差補正量、ジャイロ９５３とＧＰＳアンテナ９５１との軸ずれ補正量、姿勢角誤差補正量などの補正量を算出する（Ｓ１１０：補正量算出処理）。
そして、姿勢角計算部１１０は、前回の姿勢角の標定値（初回は初期値取得部２１０が取得した姿勢角の初期値）をジャイロ９５３が出力したレートで積分し、積分して算出した当該時点の姿勢角を姿勢角誤差補正量に基づいて補正し姿勢角の標定値を計算する。このとき、レート補正部２５０は、ジャイロ９５３が出力したレートをレート誤差補正量で補正する。また、姿勢角計算部１１０は、補正されたレートを積分して積分値（３軸方向それぞれの変動角度）を軸ずれ補正量で補正する。そして、姿勢角計算部１１０は、補正した積分値を前回の姿勢角の標定値に加算して当該時点の姿勢角を計算し、当該時点の姿勢角を姿勢角誤差補正量に基づいて補正して姿勢角の標定値を計算する（Ｓ１１１：姿勢角標定処理）。
【００３５】
姿勢標定システム１００は、姿勢角標定処理（Ｓ１１１）で姿勢角を標定後、姿勢角の標定値を使用して座標変換行列計算処理（Ｓ１０６）から処理を繰り返し、当該時点における姿勢角を標定する。
【００３６】
上記二重位相差算出処理（Ｓ１０２）において、二重位相差算出部１２０は、搬送波を受信したタイミングを複数のＧＰＳ受信機９５２間でソフトウェア的に同期させて、二重位相差の観測値Ｗλを算出する。
【００３７】
このとき、複数のＧＰＳ受信機９５２がＧＰＳ衛星から測位信号の搬送波を受信したタイミングを精度よく求め、二重位相差算出部１２０が、例えば、主局のＧＰＳ受信機９５２の搬送波位相の観測値を従局のＧＰＳ受信機９５２の受信時刻に合わせて補間し、タイミングの同期を図っている。これにより、基本的には、どんな汎用のＧＰＳ受信機９５２でも姿勢標定システム１００に使用することが出来、低価格な姿勢標定システム１００を構成することができる。また、機能、性能（受信周波数、観測値の出力周期など）が異なる複数のＧＰＳ受信機９５２を組み合わせて姿勢標定システム１００に使用することができる。
【００３８】
また、二重位相差の観測値Ｗλを算出する方法には、１周波の搬送波を利用する方法と複数の周波（例えば、２周波）の搬送波を利用する方法とがある。
【００３９】
２周波受信機は、現在は高価であるが、Ｌ５等の新周波が公開された後は価格が低下すると思われる。そこで、２周波受信機を使用した姿勢標定システム１００を想定すると、２周波の搬送波位相の観測値について線形結合を取ったワイドレーンの搬送波位相を用いて二重位相差の観測値を算出することができる。そして、ワイドレーンでの搬送波の波長は単周波（１周波）の波長に比べて長く、このため、ワイドレーンの搬送波位相を用いることで引き込み可能な残差（観測値と推定値との二重位相差の差分）の範囲が広がる。例えば、Ｌ１の１周波の波長が約１９ｃｍ（センチメートル）であるに対し、Ｌ１とＬ５との２周波を線形結合したワイドレーンでの波長は約７５ｃｍになる。そして、観測値と推定値との距離の二重差の差分が±０．５λ（波長）に収まると仮定すると、１周波（Ｌ１）では距離の二重差の差分が±９．５ｃｍを超えると二重位相差の差分が小数値に収まらないのに対し、２周波（Ｌ１とＬ５）では距離の二重差の差分が±３７．５ｃｍを超えなければ二重位相差の差分が小数値に収まる。また、上記二重位相差残差計算処理（Ｓ１０９）において、二重位相差残差計算部１４０は差分ｄＺλの小数値を二重位相差の差分として処理するため、二重位相差の差分が小数値に収まらなければ標定する姿勢角の精度が落ちる。つまり、引き込み可能な残差の範囲が広がるほど、上記二重位相差残差計算処理（Ｓ１０９）の整合性が高まり、精度の高い姿勢角を標定することが可能になる。
【００４０】
以下に、１周波を利用する方法と複数の周波を利用する方法とのそれぞれについて、二重位相差算出部１２０と二重位相差算出処理（Ｓ１０２）とを説明する。
【００４１】
図６は、実施の形態１における１周波を利用する二重位相差算出部１２０の構成図である。
１周波を利用する二重位相差算出部１２０は以下の機能要素を備える。
受信タイミング算出部１２１は各ＧＰＳ受信機９５２が搬送波を受信した受信時刻ｔｒｖを算出する。
搬送波位相補間計算部１２２は補間計算を行い複数のＧＰＳ受信機９５２について同じ受信時刻における搬送波位相φＸを算出する。
二重位相差ベクトル計算部１２３は複数のＧＰＳ受信機９５２の同じ受信時刻における搬送波位相φについて二重位相差の観測値Ｗλを計算する。
【００４２】
図７は、実施の形態１における１周波を利用する二重位相差算出処理（Ｓ１０２）を示すフローチャートである。
１周波を利用する二重位相差算出処理（Ｓ１０２）を図７に基づいて以下に説明する。
このとき、上記搬送波位相観測処理（Ｓ１０１）において、各ＧＰＳ受信機９５２が１周波（例えば、Ｌ１）の搬送波を受信して搬送波位相φＸ（φａ、φｂ、φｃ）を出力したものとする。
【００４３】
＜二重位相差算出処理（Ｓ１０２）＞
まず、受信タイミング算出部１２１は、各ＧＰＳ受信機９５２から搬送波の観測時刻ｔｒｖｏを取得し、最小自乗法などにより各ＧＰＳ受信機９５２のクロックバイアスｔｒｖＢｉａｓを計算する。そして、搬送波の観測時刻ｔｒｖｏからクロックバイアスｔｒｖＢｉａｓを減算して搬送波の受信時刻ｔｒｖを算出する（Ｓ２０１：受信タイミング算出処理）。
次に、搬送波位相補間計算部１２２は、各ＧＰＳ受信機９５２が算出した搬送波位相φＸと受信タイミング算出部１２１が算出した各ＧＰＳ受信機９５２の搬送波の受信時刻ｔｒｖＸ（ｔｒｖａ，ｔｒｖｂ，ｔｒｖｃ）とに基づいて補間計算を行い同じ受信時刻における搬送波位相φＸ（ｔ）を算出する。補間計算は線形補間、一次補間、二次補間など任意の補間計算で構わない。例えば、二次補間計算の場合、搬送波位相補間計算部１２２は、図８に示す受信時刻の関係において、各ＧＰＳ受信機９５２が出力した搬送波位相φａ、φｂ、φｃを受信時刻ｋで同期した搬送波位相φａ（ｋ）、φｂ（ｋ）、φｃ（ｋ）を以下の［式７］を計算して算出する（Ｓ２０２：搬送波位相補間計算処理）。
【００４４】
【数２】
【００４５】
そして、二重位相差ベクトル計算部１２３は、搬送波位相補間計算部１２２が算出した搬送波位相φＸ（ｔ）に基づいて二重位相差の観測値Ｗλ（ベクトル）を算出する。このとき、二重位相差ベクトル計算部１２３は上記［関係式５］におけるＷを整数値バイアスＮを省いて「Ｗ＝λφ」で表わし搬送波の波長λで除算してＷλを算出する。例えば、ＧＰＳ衛星ｉとＧＰＳ衛星ｊそれぞれについて、主局の搬送波位相φａｉ、φａｊと従局１の搬送波位相φｂｉ、φｂｊとの二重位相差の観測値φａｂｊｉ（スカラー）を算出する場合、二重位相差ベクトル計算部１２３は以下の［式８］を計算する。そして、４機のＧＰＳ衛星についての二重位相差の観測値Ｗλ（ベクトル）を算出する（Ｓ２０３：二重位相差観測値計算処理）。
【００４６】
φａｂｊｉ＝φｂｊ−φａｊ−（φｂｉ−φａｉ） ［式８］
【００４７】
図９は、実施の形態１における２周波を利用する二重位相差算出部１２０の構成図である。
２周波を利用する二重位相差算出部１２０は、図９に基づいて説明した１周波を利用する二重位相差算出部１２０の機能要素に加えて線形結合計算部１２４を備える。
線形結合計算部１２４は各ＧＰＳ受信機９５２が出力した複数の周波の搬送波位相φＸＹを線形計算して線形結合後の搬送波位相φＸを算出する。
【００４８】
図１０は、実施の形態１における複数の周波を利用する二重位相差算出処理（Ｓ１０２）を示すフローチャートである。
複数の周波を利用する二重位相差算出処理（Ｓ１０２）を図１０に基づいて以下に説明する。
このとき、上記搬送波位相観測処理（Ｓ１０１）において、各ＧＰＳ受信機９５２が２周波（例えば、Ｌ１とＬ５）の搬送波を受信して搬送波位相φＸＹ（φａ１、φａ５、φｂ１、φｂ５、φｃ１、φｃ５）を出力したものとする（φａ１，φｂ１，φｃ１：第１の搬送波位相の一例）（φａ５，φｂ５，φｃ５：第２の搬送波位相の一例）。
【００４９】
＜二重位相差算出処理（Ｓ１０２）＞
まず、受信タイミング算出部１２１は、各ＧＰＳ受信機９５２から各周波それぞれの搬送波の観測時刻ｔｒｖｏを取得し、最小自乗法により各ＧＰＳ受信機９５２のクロックバイアスｔｒｖＢｉａｓを計算する。そして、搬送波の観測時刻ｔｒｖｏからクロックバイアスｔｒｖＢｉａｓを減算して搬送波の受信時刻ｔｒｖを算出する（Ｓ２０１：受信タイミング算出処理）。
次に、線形結合計算部１２４は各ＧＰＳ受信機９５２が出力した各周波の搬送波位相φＸＹを線形計算して線形結合後の搬送波位相φＸを算出する（Ｓ３０２：線形結合計算処理）。
以下、二重位相差算出部１２０は、Ｓ３０３とＳ３０４とをそれぞれ前記Ｓ２０２とＳ２０３と同様に実行する。
【００５０】
図６、図９には３台のＧＰＳ受信機９５２を示しているが、ＧＰＳ受信機９５２は３台以上あればよく、台数を増して構わない。
また、受信タイミング算出部１２１、搬送波位相補間計算部１２２、二重位相差ベクトル計算部１２３および線形結合計算部１２４をＧＰＳ受信機９５２に対応して複数示しているが、１つずつで構わない。
また、基線ベクトル間で同期して二重位相差の観測値を算出する必要はなく、例えば、図６と図９とに３つ示している搬送波位相補間計算部１２２の真ん中の１つは処理しなくても構わない。
【００５１】
上記実施の形態１で説明した姿勢標定装置１０１は、ＧＰＳアンテナ９５１間の２つの基線ベクトルを求めて算出した姿勢角の初期値をジャイロ９５３のレート出力により更新（積分）し、時間の経過とともに拡大する各種誤差を、ＧＰＳ観測に基づく二重位相差を利用して観測更新することで、精度よく姿勢角を出力する装置である。
特に、図３に（１）と（２）で示す、二重位相差算出部１２０と二重位相差残差計算部１４０とカルマンフィルタ１５０とが特徴点である。
【００５２】
上記実施の形態１で説明した姿勢標定装置１０１は、ＧＰＳとジャイロをカップリングすることにより、以下のメリットを有する。
まず、ＧＰＳ衛星可視時は、観測更新によりジャイロ誤差の高精度な補正を行うことができ、姿勢角の高精度化が可能である。
また、ＧＰＳ衛星が不可視の時でもジャイロが出力するレートで積分することで姿勢角の出力が可能である。
また、ＧＰＳ受信機の観測周期に比べ、一般的にジャイロの出力周期は早いため、高周波で姿勢角を出力することが可能である。
【００５３】
上記実施の形態１で説明した姿勢標定装置１０１は、単独測位等により高精度に求まる受信時刻を用いて搬送波位相観測値の補間計算を行い、ＧＰＳ受信機間の同期をソフトウェア的に取っている。これにより、上記実施の形態１で説明した姿勢標定装置は以下のメリットを有する。
基本的には、どんな汎用受信機（ＧＰＳ受信機）でも使用することが出来、低価格な姿勢標定システム１００を構成することができる。
また、機能、性能（受信周波数、観測値の出力周期など）の異なる複数のＧＰＳ受信機を組み合わせることが可能となる。そこで、例えば、姿勢標定システム１００は測位システムの構成に最適なＧＰＳ受信機を測位システムと兼用して姿勢角を標定することができる。
【００５４】
上記実施の形態１で説明した姿勢標定装置１０１は、従来の姿勢標定方法で利用されたオイラー姿勢角ではなく、二重位相差を利用して観測更新することにより、以下のメリットを有する。
まず、ＧＰＳのみを利用した従来の姿勢角の決定には同時刻に２本以上の基線ベクトルに対する二重差位相差の観測値を得ることが必要であるのに対し、少なくとも１本の基線ベクトルに対する二重差位相差の観測値が得られていれば、観測更新が行え、誤差の拡大を抑えることが可能である。
つまり、ＧＰＳ受信機の観測値に基づく観測更新のアベイラビリティが向上することから、安定性の低い低価格なジャイロを使用した構成が可能である。
また、オイラー角にはその特質上、特異点（ピッチ９０°）が存在し、観測更新にオイラー角を利用する従来の姿勢標定方法が特異点において適切な観測更新を実施できないのに対し、観測更新に二重位相差を利用する上記実施の形態１の姿勢標定装置１０１では特異点において適切な観測更新を実施できないということは一切ない。
また、二重位相差の整数値バイアスを正常に求めて姿勢角の絶対量（姿勢角の初期値）を決定した後は、ジャイロのレート出力を積分することで姿勢角を時間更新することが可能である。
また、ＧＰＳ受信機の観測周期が、ジャイロ誤差が拡大する時間に対し十分短ければ、二重位相差の観測残差は±０．５λ（波長）に収まることから、整数値バイアスを求めることなく観測更新を行うことができる。
つまり、二重位相差の観測残差が特定の閾値（例えば、±０．２λ）を超えた場合、何らかの原因によりＧＰＳ受信機が不適切な観測値を出力した、または、ジャイロが不適切なレートを出力したと判断することができ、これらのデータを適切に棄却することができる。
また、現在の高価な２周波受信機または将来は低価格になることが予想されるＬ５等の新周波対応受信機を使用すれば、２周波の線形結合によりワイドレーンを利用して観測残差を算出することができる。これにより、二重位相差の残差を引き込める領域（例えば、±０．５λ）の範囲を広げることが出来る。
また、ＧＰＳとジャイロとをカップリングするときに利用する観測値がオイラー姿勢角である場合に比べ、二重位相差のノイズモデル（カルマンフィルタで使用する状態方程式や観測方程式）のほうが把握しやすく、推定精度が向上する。
【００５５】
上記実施の形態１の説明におけるＧＰＳは、Ｇａｌｉｌｅｏ、ＧＬＯＮＡＳＳ（ＧＬｏｂａｌ ＮＡｖｉｇａｔｉｏｎ Ｓａｔｅｌｌｉｔｅ Ｓｙｓｔｅｍ）などのその他のＧＮＳＳ（Ｇｌｏｂａｌ Ｎａｖｉｇａｔｉｏｎ Ｓａｔｅｌｌｉｔｅ Ｓｙｓｔｅｍ）であっても構わない。
【図面の簡単な説明】
【００５６】
【図１】実施の形態１における姿勢標定システム１００の構成の概要図。
【図２】実施の形態１における姿勢標定システム１００のハードウェア構成図。
【図３】実施の形態１における姿勢標定装置１０１の機能構成図。
【図４】実施の形態１における姿勢標定システム１００の姿勢標定処理を示すフローチャート。
【図５】基線ベクトルｂとＬＯＳベクトルｅとＧＰＳアンテナからＧＰＳ衛星までの距離ρとの関係図。
【図６】実施の形態１における１周波を利用する二重位相差算出部１２０の構成図。
【図７】実施の形態１における１周波を利用する二重位相差算出処理（Ｓ１０２）を示すフローチャート。
【図８】主局と従局とにおける搬送波受信時刻の関係図。
【図９】実施の形態１における２周波を利用する二重位相差算出部１２０の構成図。
【図１０】実施の形態１における複数の周波を利用する二重位相差算出処理（Ｓ１０２）を示すフローチャート。
【符号の説明】
【００５７】
１００ 姿勢標定システム、１０１ 姿勢標定装置、１１０ 姿勢角計算部、１２０ 二重位相差算出部、１２１ 受信タイミング算出部、１２２ 搬送波位相補間計算部、１２３ 二重位相差ベクトル計算部、１２４ 線形結合計算部、１３０ 二重位相差推定部、１４０ 二重位相差残差計算部、１５０ カルマンフィルタ、２１０ 初期値取得部、２２０ 方向余弦行列計算部、２３０ アンテナセパレーション、２４０ デザイン行列計算部、２５０ レート補正部、９１１ ＣＰＵ、９２０ 記憶装置、９２１ ＯＳ、９２３ プログラム群、９２４ ファイル群、９５１ ＧＰＳアンテナ、９５２ ＧＰＳ受信機、９５３ ジャイロ、９５４ プラットフォーム。
【特許請求の範囲】
【請求項１】
ジャイロの測定値を用いて計算した姿勢角の計算値をＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）受信機の観測値に基づいて補正して姿勢角の標定値を算出する姿勢標定装置であり、
前記ジャイロの測定値に基づき中央処理装置を用いて姿勢角の計算値を算出する姿勢角計算部と、
複数のＧＰＳ受信機が観測した搬送波位相を入力し中央処理装置を用いて複数のＧＰＳ受信機における二重位相差の観測値を算出する二重位相差算出部と、
算出した姿勢角の標定値に基づき中央処理装置を用いて複数のＧＰＳ受信機における二重位相差の推定値を算出する二重位相差推定部と、
前記二重位相差算出部が算出した複数のＧＰＳ受信機における二重位相差の観測値と前記二重位相差推定部が算出した複数のＧＰＳ受信機における二重位相差の推定値との差分を中央処理装置を用いて算出する二重位相差残差計算部と、
前記二重位相差残差計算部が算出した複数のＧＰＳ受信機における二重位相差の観測値と推定値との差分に基づき前記姿勢角計算部が算出した姿勢角の計算値に対する補正量を中央処理装置を用いて算出する補正量算出部と、
前記補正量算出部が算出した補正量に基づき前記姿勢角計算部が算出した姿勢角の計算値を中央処理装置を用いて補正して姿勢角の標定値を算出する姿勢角標定部と
を備えたことを特徴とする姿勢標定装置。
【請求項２】
前記二重位相差算出部は、
各ＧＰＳ受信機が観測した搬送波位相と各ＧＰＳ受信機における搬送波の受信時刻とに基づいて補間計算を行い同時刻における各ＧＰＳ受信機の搬送波位相を算出し、算出した各ＧＰＳ受信機の搬送波位相に基づいて複数のＧＰＳ受信機における二重位相差の観測値を算出する
ことを特徴とする請求項１記載の姿勢標定装置。
【請求項３】
前記二重位相差算出部は、
複数のＧＰＳ受信機がそれぞれ観測した第１の搬送波位相と第２の搬送波位相とを入力し第１の搬送波位相と第２の搬送波位相とを線形結合計算して各ＧＰＳ受信機の線形結合した搬送波位相を算出し線形結合した搬送波位相に基づいて複数のＧＰＳ受信機における二重位相差の観測値を算出する
ことを特徴とする請求項１〜請求項２いずれかに記載の姿勢標定装置。
【請求項４】
前記二重位相差推定部は、
前記姿勢標定装置が標定した姿勢角の標定値に基づいて複数のＧＰＳ受信機における基線ベクトルを算出し、算出した基線ベクトルと搬送波を送信したＧＰＳ衛星に対するＧＰＳ受信機の方向余弦基底ベクトルとに基づいて複数のＧＰＳ受信機における二重位相差の推定値を算出する
ことを特徴とする請求項１〜請求項３いずれかに記載の姿勢標定装置。
【請求項５】
前記二重位相差残差計算部は、
前記二重位相差算出部が算出した二重位相差の観測値と前記二重位相差推定部が算出した二重位相差の推定値との差を算出し、算出した差の小数値を複数のＧＰＳ受信機における二重位相差の観測値と推定値との差分とする
ことを特徴とする請求項１〜請求項４いずれかに記載の姿勢標定装置。
【請求項６】
ジャイロの測定値を用いて計算した姿勢角の計算値をＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）受信機の観測値に基づいて補正して姿勢角の標定値を算出する姿勢標定装置の姿勢標定方法であり、
姿勢角計算部がジャイロの測定値に基づき中央処理装置を用いて姿勢角の計算値を算出する姿勢角計算処理と、
二重位相差算出部が各ＧＰＳ受信機の観測した搬送波位相を入力し中央処理装置を用いて複数のＧＰＳ受信機における二重位相差の観測値を算出する二重位相差算出処理と、
二重位相差推定部が前記姿勢標定装置の標定した姿勢角の標定値に基づき中央処理装置を用いて複数のＧＰＳ受信機における二重位相差の推定値を算出する二重位相差推定処理と、
二重位相差残差計算部が前記二重位相差算出部の算出した複数のＧＰＳ受信機における二重位相差の観測値と前記二重位相差推定部の算出した複数のＧＰＳ受信機における二重位相差の推定値との差分を中央処理装置を用いて算出する二重位相差残差計算処理と、
補正量算出部が前記二重位相差残差計算部の算出した複数のＧＰＳ受信機における二重位相差の観測値と推定値との差分に基づき前記姿勢角計算部の算出した姿勢角の計算値に対する補正量を中央処理装置を用いて算出する補正量算出処理と、
姿勢角標定部が前記補正量算出部の算出した補正量に基づき前記姿勢角計算部の算出した姿勢角の計算値を中央処理装置を用いて補正して姿勢角の標定値を算出する姿勢角標定処理と
を実行することを特徴とする姿勢標定方法。
【請求項７】
請求項６記載の姿勢標定方法をコンピュータに実行させることを特徴とする姿勢標定プログラム。
【請求項１】
ジャイロの測定値を用いて計算した姿勢角の計算値をＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）受信機の観測値に基づいて補正して姿勢角の標定値を算出する姿勢標定装置であり、
前記ジャイロの測定値に基づき中央処理装置を用いて姿勢角の計算値を算出する姿勢角計算部と、
複数のＧＰＳ受信機が観測した搬送波位相を入力し中央処理装置を用いて複数のＧＰＳ受信機における二重位相差の観測値を算出する二重位相差算出部と、
算出した姿勢角の標定値に基づき中央処理装置を用いて複数のＧＰＳ受信機における二重位相差の推定値を算出する二重位相差推定部と、
前記二重位相差算出部が算出した複数のＧＰＳ受信機における二重位相差の観測値と前記二重位相差推定部が算出した複数のＧＰＳ受信機における二重位相差の推定値との差分を中央処理装置を用いて算出する二重位相差残差計算部と、
前記二重位相差残差計算部が算出した複数のＧＰＳ受信機における二重位相差の観測値と推定値との差分に基づき前記姿勢角計算部が算出した姿勢角の計算値に対する補正量を中央処理装置を用いて算出する補正量算出部と、
前記補正量算出部が算出した補正量に基づき前記姿勢角計算部が算出した姿勢角の計算値を中央処理装置を用いて補正して姿勢角の標定値を算出する姿勢角標定部と
を備えたことを特徴とする姿勢標定装置。
【請求項２】
前記二重位相差算出部は、
各ＧＰＳ受信機が観測した搬送波位相と各ＧＰＳ受信機における搬送波の受信時刻とに基づいて補間計算を行い同時刻における各ＧＰＳ受信機の搬送波位相を算出し、算出した各ＧＰＳ受信機の搬送波位相に基づいて複数のＧＰＳ受信機における二重位相差の観測値を算出する
ことを特徴とする請求項１記載の姿勢標定装置。
【請求項３】
前記二重位相差算出部は、
複数のＧＰＳ受信機がそれぞれ観測した第１の搬送波位相と第２の搬送波位相とを入力し第１の搬送波位相と第２の搬送波位相とを線形結合計算して各ＧＰＳ受信機の線形結合した搬送波位相を算出し線形結合した搬送波位相に基づいて複数のＧＰＳ受信機における二重位相差の観測値を算出する
ことを特徴とする請求項１〜請求項２いずれかに記載の姿勢標定装置。
【請求項４】
前記二重位相差推定部は、
前記姿勢標定装置が標定した姿勢角の標定値に基づいて複数のＧＰＳ受信機における基線ベクトルを算出し、算出した基線ベクトルと搬送波を送信したＧＰＳ衛星に対するＧＰＳ受信機の方向余弦基底ベクトルとに基づいて複数のＧＰＳ受信機における二重位相差の推定値を算出する
ことを特徴とする請求項１〜請求項３いずれかに記載の姿勢標定装置。
【請求項５】
前記二重位相差残差計算部は、
前記二重位相差算出部が算出した二重位相差の観測値と前記二重位相差推定部が算出した二重位相差の推定値との差を算出し、算出した差の小数値を複数のＧＰＳ受信機における二重位相差の観測値と推定値との差分とする
ことを特徴とする請求項１〜請求項４いずれかに記載の姿勢標定装置。
【請求項６】
ジャイロの測定値を用いて計算した姿勢角の計算値をＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）受信機の観測値に基づいて補正して姿勢角の標定値を算出する姿勢標定装置の姿勢標定方法であり、
姿勢角計算部がジャイロの測定値に基づき中央処理装置を用いて姿勢角の計算値を算出する姿勢角計算処理と、
二重位相差算出部が各ＧＰＳ受信機の観測した搬送波位相を入力し中央処理装置を用いて複数のＧＰＳ受信機における二重位相差の観測値を算出する二重位相差算出処理と、
二重位相差推定部が前記姿勢標定装置の標定した姿勢角の標定値に基づき中央処理装置を用いて複数のＧＰＳ受信機における二重位相差の推定値を算出する二重位相差推定処理と、
二重位相差残差計算部が前記二重位相差算出部の算出した複数のＧＰＳ受信機における二重位相差の観測値と前記二重位相差推定部の算出した複数のＧＰＳ受信機における二重位相差の推定値との差分を中央処理装置を用いて算出する二重位相差残差計算処理と、
補正量算出部が前記二重位相差残差計算部の算出した複数のＧＰＳ受信機における二重位相差の観測値と推定値との差分に基づき前記姿勢角計算部の算出した姿勢角の計算値に対する補正量を中央処理装置を用いて算出する補正量算出処理と、
姿勢角標定部が前記補正量算出部の算出した補正量に基づき前記姿勢角計算部の算出した姿勢角の計算値を中央処理装置を用いて補正して姿勢角の標定値を算出する姿勢角標定処理と
を実行することを特徴とする姿勢標定方法。
【請求項７】
請求項６記載の姿勢標定方法をコンピュータに実行させることを特徴とする姿勢標定プログラム。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【公開番号】特開２００７−１６３３３５（Ｐ２００７−１６３３３５Ａ）
【公開日】平成１９年６月２８日（２００７．６．２８）
【国際特許分類】
【出願番号】特願２００５−３６１３６９（Ｐ２００５−３６１３６９）
【出願日】平成１７年１２月１５日（２００５．１２．１５）
【出願人】（０００００６０１３）三菱電機株式会社 (33,312)
【Ｆターム（参考）】
【公開日】平成１９年６月２８日（２００７．６．２８）
【国際特許分類】
【出願日】平成１７年１２月１５日（２００５．１２．１５）
【出願人】（０００００６０１３）三菱電機株式会社 (33,312)
【Ｆターム（参考）】
[ Back to top ]