移動体姿勢計測装置
【課題】従来に比べて移動体の姿勢の計算が簡易なものとし、また、移動体の姿勢の算出値の精度を向上させる。
【解決手段】移動体に設けられて衛星の信号を受信する基準アンテナ１１及びｎ個（１，２，３，…）のユーザアンテナ１２で受信した衛星の信号から前動体と前記衛星との位置関係を算出する。また、各アンテナで受信した衛星信号の搬送波位相を測定する。そして、この搬送波位相から一重位相差又は二重位相差を計算する。基準アンテナとユーザアンテナとの位置関係、移動体と衛星との位置関係、及び一重位相差又は二重位相差の関係に基づいて、一重位相差又は二重位相差に対する未知の整数値アンビギュイティ、移動体の未知の姿勢成分、及び一重位相差又は二重位相差についての初期値を算出する。非線形推定法を利用して初期値を収束させることにより、未知の整数値アンビギュイティ及び移動体の未知の姿勢成分を算出する。
【解決手段】移動体に設けられて衛星の信号を受信する基準アンテナ１１及びｎ個（１，２，３，…）のユーザアンテナ１２で受信した衛星の信号から前動体と前記衛星との位置関係を算出する。また、各アンテナで受信した衛星信号の搬送波位相を測定する。そして、この搬送波位相から一重位相差又は二重位相差を計算する。基準アンテナとユーザアンテナとの位置関係、移動体と衛星との位置関係、及び一重位相差又は二重位相差の関係に基づいて、一重位相差又は二重位相差に対する未知の整数値アンビギュイティ、移動体の未知の姿勢成分、及び一重位相差又は二重位相差についての初期値を算出する。非線形推定法を利用して初期値を収束させることにより、未知の整数値アンビギュイティ及び移動体の未知の姿勢成分を算出する。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ＧＰＳなどの衛星航法システムを用いて船舶などの移動体の姿勢を計測する移動体姿勢計測装置に関する。
【背景技術】
【０００２】
船舶などの移動体の姿勢は、方位ψ、ピッチθ、ロールφを使って表すことができる。移動体に固定された右手系直交座標(移動体座標系）を定義したとき、これらの量は次のように定義される。
【０００３】
方位とは、局所座標系のｘ軸から移動体座標系のｘ軸の局所座標系のｘ‐ｙ平面への射影へのなす角度である。
【０００４】
ピッチとは、移動体座標系のｘ軸の局所座標系のｘ‐ｙ平面への射影から移動体座標系のｘ軸へのなす角度である。
【０００５】
ロールとは、移動体座標系のｘ軸を中心とした、移動体座標系ｙ軸の局地座標系の水平面からの回転角である。
【０００６】
このような移動体の姿勢を検出する移動体姿勢検出装置としては、ＧＰＳ、ＧＬＯＮＡＳＳなどの衛星航法システムを用いたものが知られている。すなわち、この移動体姿勢検出装置は、複数のアンテナで受信されるＧＰＳ衛星などの信号から移動体の姿勢を検出するものである。
【０００７】
このように、移動体に設置された複数のアンテナで受信されるＧＰＳ衛星からの信号から姿勢情報を正確に算出する場合には、各アンテナ間の一重位相差または二重位相差を用いる。
【０００８】
図１に示すように、一重位相差とは、ある基線ｂを構成する基準アンテナ１０１及びユーザアンテナ１０２にそれぞれ接続された基準受信機１０３、ユーザ受信機１０４により測定された、同一の衛星１０５からの信号の搬送波位相の差分である。この差分と基準アンテナ１０１及びユーザアンテナ１０２により構成される基線ベクトルｂの間には行路差から次の関係式が成り立つ。
【数１】
……（１）
ここで、ｙは一重位相差、ｈは衛星１０５への単位方向ベクトル、ｂは基線ベクトル、ａは整数の値、Δｔは受信機１０３，１０４の位相の取得タイミングのズレを表す時計誤差、ｅは観測誤差である。添え字ｓは一重位相差であることを意味する。また、単位は波数で表している。式（１）は、一重位相差位相と行路差ｈsTｂの間には、ある波長λの整数倍とΔｔを足し合わせた分だけの差があることを示している。例えば、各アンテナで瞬間的な位相を観測したとき、その値の範囲は[-0.5:0.5]となる。
【０００９】
このとき、一重位相差の値は[-1.0:1.0]でしか表されないため、行路差ｈsTｂとの間には、ある波長λの整数倍とΔｔを足し合わせた分だけの差があることになる。この整数値は整数値アンビギュイティと呼ばれ、式（１）のａsにあたる。
【００１０】
また、二重位相差とは、ある基線における基準衛星の一重位相差と基準衛星以外の一重位相差との差分である。基準衛星ｉと衛星ｋの二重位相差と基線の関係は、
【数２】
……（２）
と表される。
【００１１】
ここで、
【数３】
……（３）
【数４】
……（４）
【数５】
……（５）
【数６】
……（６）
を意味する。ここで、添え字ｄは二重位相差に関する値であることを表している。このように二重位相差を計算することで、時計誤差を打ち消すことができる（なお、二重位相差については特許文献３を参照）。
【００１２】
次に、従来における移動体の姿勢の算出法について図２を参照して説明する。ここでは、移動体を船舶１１１とし、船舶１１１内にアンテナ１１２〜１１４を３つ配置し、二重位相差を用いて算出する場合を例に取り説明する。基準アンテナ１１２から各ユーザアンテナ１１３，１１４への基線ベクトルｂ１，ｂ２より、船舶１１１の姿勢は、
【００１３】
【数７】
……（７）
【数８】
……（８）
【数９】
……（９）
となる。ここで、ｇ{-}は、基線ベクトルから各姿勢情報を計算する関数である。
【００１４】
この関数ｇ{-}は具体的に次のように表される。ここでの添え字ｘ，ｙ，ｚは各基線の局地座標系における各ｘ，ｙ，ｚ軸の値であることを示す。
【００１５】
【数１０】
……（１０）
【数１１】
……（１１）
【数１２】
……（１２）
【００１６】
ここで、
【数１３】
……（１３）
である。また、arg(-)は複素数の偏角である。
【００１７】
基線ベクトルｂ１，ｂ２についての観測方程式は、
【数１４】
……（１４）
【数１５】
……（１５）
と表せる。ここで、ｙ(bi)は基線ｉで観測された位相差により構成されるベクトルである。ａ(bi)は基線ｉで観測された位相差に対応する整数値アンビギュイティにより構成されるベクトルである。Ｈ(bi)は基線ｉで観測された基線ベクトルと位相差の関係を表すベクトルｈにより構成される行列である。ｅ(bi)は基線ｉで観測される位相差に混入する観測誤差により構成されるベクトルである。
【特許文献１】特許第３５０２００７号公報
【特許文献２】特開２００６‐１０５６３５号公報
【特許文献３】特開２００２‐５４９４６号公報
【発明の開示】
【発明が解決しようとする課題】
【００１８】
しかしながら、前述の従来における移動体の姿勢の算出法では、基線ベクトルｂ１，ｂ２を求めるために、２つの連立方程式（１４）（１５）を別々に求める必要がある。このことから、前述の従来における移動体の姿勢の算出法では、整数値アンビギュイティベクトルａｄ(ｂ１)，ａｄ(ｂ２)をそれぞれ別々に求めており、よって、計算が煩雑であるという不具合があった。
【００１９】
また、全二重位相差の観測数に対して未知数は方位、ピッチ、ロールの３つであるが、基線ベクトルｂ１，ｂ２を求めることから未知数は６つとして求めている。このことから、基線ベクトルｂ１，ｂ２から算出された姿勢ベクトルの精度が劣化してしまうという不具合もあった。
【００２０】
そこで、本発明の目的は、従来に比べて移動体の姿勢の計算が簡易なものとし、また、移動体の姿勢の算出値の精度を向上させることである。
【課題を解決するための手段】
【００２１】
（１）本発明は、姿勢の計測対象となる移動体に設けられて衛星の信号を受信するｎ個（２，３，４，…）のアンテナと、前記各アンテナで受信した衛星の信号から前記移動体と前記衛星との位置関係を算出する位置関係算出手段と、前記各アンテナで受信した衛星信号の搬送波位相を測定する搬送波位相測定手段と、前記搬送波位相測定手段で測定した搬送波位相から一重位相差又は二重位相差を計算する位相差計算手段と、前記移動体の未知の姿勢成分の初期値を算出する初期値算出手段と、前記各アンテナの位置関係、前記移動体と前記衛星との位置関係、及び前記一重位相差又は二重位相差の関係に基づいて、非線形推定法を利用して前記初期値を収束させることにより、未知の整数値アンビギュイティ及び前記移動体の未知の姿勢成分を算出する姿勢等算出手段と、を備えている移動体姿勢計測装置である。
【００２２】
（２）前記初期値算出手段は、未知の前記整数値アンビギュイティのうちの幾つか又は全てに対する候補、それぞれの当該候補に対する前記未知の姿勢成分についての初期値を算出し、前記姿勢等算出手段は、前記整数値アンビギュイティ候補を既知として前記各候補の姿勢成分の初期値を前記非線形推定法により収束させて前記各候補の残りの未知な整数値アンビギュイティ及び前記各候補に対応する前記未知の姿勢成分を算出する算出手段と、前記候補の中から正しい候補を選出する選出手段と、を備えているようにしてもよい。
【００２３】
（３）前記姿勢等算出手段は、前記非線形推定法として非線形最小二乗法を用いるようにしてもよい。
【００２４】
（４）前記姿勢等算出手段で算出された整数値アンビギュイティ及び前記移動体の姿勢により前記一重位相差又は二重位相差における誤差推定量を求める誤差推定量算出手段と、前記誤差推定量を用いた評価指数値により、前記整数値アンビギュイティ及び前記移動体の姿勢の正しさを検定する検定手段と、をさらに備えているようにしてもよい。
【００２５】
（５）前記姿勢等算出手段で算出された整数値アンビギュイティ及び前記移動体の姿勢により前記一重位相差又は二重位相差における誤差推定量を求める誤差推定量算出手段をさらに備え、前記選出手段は、前記誤差推定値を用いた評価指数値により前記候補の中から正しい候補を選出するようにしてもよい。
【００２６】
（６）前記初期値算出手段は、前記姿勢成分のうち傾斜成分を測定し、当該傾斜成分により前記未知の姿勢の初期値を算出するようにしてもよい。
【００２７】
（７）前記姿勢等算出手段で算出された前記整数値アンビギュイティを保存する保存手段をさらに備え、前記姿勢等算出手段は、前記保存手段に保存されている前記整数値アンビギュイティのうち有効なものを既知なものとして利用して、未知の前記整数値アンビギュイティ及び前記移動体の未知の姿勢成分を算出するようにしてもよい。
【００２８】
（８）前記姿勢等算出手段で算出された前記姿勢の値を保存する保存手段をさらに備え、
前記初期値算出手段は、前記保存手段に保存されている前記姿勢の値により前記初期値の算出を行うようにしてもよい。
【００２９】
（９） （２）の発明において、前記初期値算出手段は、前記ｎ個のアンテナのうち２つのアンテナが構成する基線について観測される前記一重位相差又は二重位相差のうち２つ以上の位相差の前記整数値アンビギュイティに対して、当該２つ以上の位相差に対する当該整数値アンビギュイティを決定したとき、当該２つ以上の位相差及び前記衛星の配置の関係から推定される前記基線の基線長が実際の基線と近くなるものを前記候補として算出し、推定された当該基線を利用して残りの前記未知の整数値アンビギュイティに対する初期値と前記未知の姿勢成分に対する初期値とを求めるようにしてもよい。
【００３０】
（１０） （９）の発明において、前記初期値算出手段は、前記姿勢成分のうち傾斜成分を測定し、当該傾斜成分により前記２つ以上の位相差に対する整数値アンビギュイティを算出するようにしてもよい。
【発明の効果】
【００３１】
本発明によれば、整数値アンビギュイティベクトルをすべて同時に解くことができるので、従来に比べて移動体の姿勢の計算が簡易なものとなる。
【００３２】
また、整数値アンビギュイティベクトルを解く際に基線間の配置の関係を利用するので、得られた整数値アンビギュイティベクトルの確度が飛躍的に向上する。さらに、従来のように複数の基線ベクトルを求める必要はない等のため、移動体の姿勢の算出値の精度を向上させることができる。
【発明を実施するための最良の形態】
【００３３】
次に、本発明の実施形態について説明する。以下では、本発明の実施形態である移動体姿勢計測装置で使用されているアルゴリズムについて最初に説明し、次に、このアルゴリズムを使用した移動体姿勢計測装置の構成や動作について説明する。また、以下の例では、移動体が船舶であるものとして説明する。
【００３４】
［アルゴリズムについて］
最初に、本発明の実施形態である移動体姿勢計測装置で使用されているアルゴリズムについて詳細に説明する。
【００３５】
（１）ユーザアンテナ数を２とした場合
まず、図２に示すように、船舶１１１内に基準アンテナ１１２の他にユーザアンテナ１１３，１１４を２つ配置した場合を念頭において説明する。この場合の基線を基線ｂ１，ｂ２とする。
【００３６】
このアルゴリズムの特徴は、複数の線形連立方程式を方位ψ、ピッチθ、ロールφを用いて一つの非線形な連立方程式で表し、非線形推定法を用いて整数値アンビギュイティベクトルを算出しながら姿勢状態を算出することである。このアルゴリズムの詳細は以下のとおりである。
【００３７】
まず、船体座標系（移動体座標系）における各基線ベクトルをｂｖ１，ｂｖ２とする。これらのベクトルは既知である。基線ベクトルｂ１，ｂ２とｂｖ１，ｂｖ２との関係は、次の座標変換行列Ｕ（ψ，θ，φ）を用いて、
【００３８】
【数１６】
……（１６）
と表される。
【００３９】
ここで、座標変換行列Ｕ（ψ，θ，φ）は、次のような関数である。
【数１７】
……（１７）
二重位相差と移動体の姿勢との関係は次の方程式で表される。
【数１８】
……（１８）
【数１９】
……（１９）
【００４０】
そして、基線Ｌ１と基線Ｌ２の観測値を一つの式にまとめて、
【数２０】
……（２０）
と表す。ここで、
【数２１】
……（２１）
である。ここで、（２０）式を観測モデル式と呼び、（２１）式を状態変数ベクトルと呼ぶ。この状態変数ベクトルを用いて表された観測モデル式により、整数値アンビギュイティと移動体の姿勢とが算出される。
【００４１】
今、整数値アンビギュイティベクトルａは未知であるとする。このとき、ｘを推定するには、この観測方程式ｆ（ｘ）のＸに関して微分することで得られた行列、
【数２２】
……（２２）
を用いて、初期点ｘ０（ｘにハット）を適当な値に設定して、非線形推定として非線形最小二乗法を用いて、
【数２３】
……（２３）
を収束させることで求めることができる。
【００４２】
未知である整数値アンビギュイティベクトルａｋ（ａにハット）は、
【数２４】
……（２４）
として毎回を計算する。ここで、round(-)は、一番近い整数への丸め込みを意味する。
【００４３】
あるいは、ある回数ｌを設けて、
【数２５】
……（２５）
としても良い。
【００４４】
また、（２３）式においてのＱは重み行列であり、適当な値を設定するが、
【数２６】
……（２６）
もしくは、
【数２７】
……（２７）
として設定するのが普通である。ここで、Ｅ{-}は期待値、Ｉは単位行列を意味する。
【００４５】
以上のように、初期点を設定して非線形フィルタを収束させることで移動体の姿勢を算出し、また、この収束時に未知な整数値アンビギュイティベクトルを求めることができる。
収束時の残差は、
【００４６】
【数２８】
……（２８）
と求めることができ、その残差二乗和ｒ２は、
【数２９】
……（２９）
と求めることができる。
【００４７】
また、観測雑音の推定分散量は、観測モデル式（２０）における観測数がｍであり、状態変数ベクトルの要素数をｎ（ここでは３となる）とすれば、
【数３０】
……（３０）
として表すことができる。
【００４８】
このσ２（σにハット）と閾値σ２thとを比較し、算出された移動体の姿勢と整数値アンビギュイティベクトルとを検定することができる。
【００４９】
このような手段によると、従来の手段と比べて移動体の姿勢を解くための観測値が多くなるので推定された姿勢の精度が向上する。
【００５０】
また、基線間の配置の関係を利用するので、観測誤差の推定分散も向上し、得られた方位と整数値アンビギュイティが誤っている場合には値が大きくなることから、算出された姿勢の検定結果の確度が飛躍的に向上する。
【００５１】
（２）整数値アンビギュイティベクトルの値がわかっている場合
前述の例は整数値アンビギュイティベクトルの値がすべて不明である場合の例について説明したが、以下では、整数値アンビギュイティベクトルａのうちいくつかはすでに値が判明している場合について考える。
【００５２】
まず、（２０）式を整数値アンビギュイティベクトルがすでに定まっているものと、定まっていないものとに分けて、
【数３１】
……（３１）
と表す。ここで、添え字ｐは、整数値アンビギュイティベクトルが決まっているものを表し、添え字ｑは整数値アンビギュイティベクトルが決まっていないものを表す。
【００５３】
そして、状態ベクトルの初期点ｘ０（ｘにハット）を適当な値に設定して、
【数３２】
……（３２）
を収束させることで姿勢情報を算出する。
【００５４】
ここで、
【数３３】
……（３３）
である。
【００５５】
未知である整数値アンビギュイティベクトルａｑは、
【数３４】
……（３４）
として毎回算出する。
【００５６】
もしくは、ある回数ｌを設けて、
【数３５】
……（３５）
としても良い。
【００５７】
収束した移動体の方位に対して、観測誤差の推定分散σ２（σにハット）をもとめ、閾値σth２を比較することで、算出された移動体の姿勢と整数値アンビギュイティベクトルを検定することができる。
【００５８】
この場合、すべての整数値アンビギュイティベクトルを未知の値とした前述の場合に比べて、初期値ｘ０の値が適切な値でなく、誤った姿勢に収束した場合、固定された整数値アンビギュイティベクトル値の影響により推定分散量が大きくなることから、算出された姿勢ベクトルの検定の確度が向上する。
【００５９】
以上のことを利用して、何らかの方法によってａｐに対する候補及びそれぞれの候補に対する状態ベクトルの初期値ｘ０が与えられていたときに、正しい候補を選び出すことを考える。
【００６０】
ａｐに対する候補及びそれぞれの候補に対する状態ベクトルの初期値ｘ０の算出方法としては次の方法が考えられる。まず、ｐに関する式を、基線ｉに関する観測モデル式のうちの２つ以上の観測位相差から構成される観測モデル式を用いて次のように構成する。
【数３６】
……（３６）
【００６１】
このとき、基線は、
【数３７】
……（３７）
と推定できる。
【００６２】
そこで、基線ｉの基線長は既知であるということを利用して、
【数３８】
……（３８）
となるａｐを候補とする。
【００６３】
２つの観測位相差では、（３７）式を解くことはできないが、基線ｉのＺ成分をｂｚ（ｉ）と（ｂにハット）適当に与えて、
【数３９】
……（３９）
と、基線の水平方向のみを推定するようにすれば解くことができる。ここで、Ｈｘｙ（ｐ），ｈｚ（ｐ）はそれぞれＨｐの水平方向成分の行列、垂直方向成分の行列である。
【００６４】
このとき、
【数４０】
……（４０）
となるａｐを候補とする。ａｐに対する状態変数ベクトルは、推定された基線ベクトルの水平方向成分により計算される概略方位と、適当に与えたロール、ピッチとする。
【００６５】
以上のとおり、ｂｚ（ｉ）（ｂにハット）、ロール、ピッチは、傾斜計などの計測装置の測定値を基にすることで、状態変数ベクトルの精度を向上させることができ、ａｑが正しい値に収束しやすくすることができる。以上のようにして得られた各候補に対して、状態変数ベクトル（姿勢）、整数値アンビギュイティベクトル、観測誤差の推定分散量を計算して、推定分散量から適当な判断基準を用いて正しい候補を選び出す。適当な判断基準の例としては、一番小さい推定分散量と二番目に小さい推定分数量を用いて、その比をある闇値ｒｔｈと比較することで一番小さい推定分散量を持つ候補を検定できる。各候補においての推定分敵量は正しいものは小さク、誤っているものは大きいので、選び出される候補の検定の確度が向上する。
【００６６】
（３）一重位相差のモデル
以上の例では二重位相差を用いて移動体の姿勢を推定する場合について述べたが、二重位相差の代わりに一重位相差を用いることも可能である。
【００６７】
これは、二重位相差の観測モデル式（２０）と、（２１）式で表される状態変数ベクトルを、一重位相差の観測モデル式（４１）式と状態変数ベクトル（４２）式に代えて、前述の二重位相差の場合と同様に解けばよい。
【数４１】
……（４１）
【数４２】
……（４２）
ここでｃｂｉは基線ｉについて観測される一重位相差の要素数を持つ要素の値が１であるベクトル、Δｔｂｉは基線ｉについての時計誤差である。
【００６８】
（４）ロール、ピッチ測定可能のモデル
移動体の姿勢のうち、ピッチ若しくはロール又はその両者を、例えば傾斜計などの計測装置により求め、その得られた値よりピッチ若しくはロール又はその両者を定数で与え、方位のみを推定することも可能である。
【００６９】
いま、傾斜計などの計測装置より得られたピッチの値、ロールの値をθｃ，φｃとし、この値を用いて移動体の方位のみを推定することを考えれば、二重位相差においては、観測モデル式と状態変数ベクトルを、
【数４３】
……（４３）
【数４４】
……（４４）
として解けば良い。
【００７０】
一重位相差においては、
【数４５】
……（４５）
【数４６】
……（４６）
として解けばよい。
【００７１】
また、Δtｂ１若しくはΔtｂ２又はその両者の値が既知であるならば、その値を与えた観測モデル式を用いて解くことも可能である。
【００７２】
（５）ユーザアンテナ数をｎ個とした場合
次に、前述のようにユーザアンテナ数を２個には限定せず、図３に示すように一般的にｎ個（１，２，３，…）とする場合について考える。
【００７３】
移動体座標系における基準アンテナから各ユーザへの基線ベクトルをｂｖ１，ｂｖ２，…，ｂｖｎと表す。これらのベクトルは既知である。局地座標系における基準アンテナから各ユーザへの基線ベクトルｂ１，ｂ２，ｂ３は、
【数４７】
……（４７）
と表される。
【００７４】
よって、二重位相差と姿勢の関係は、
【数４８】
……（４８）
と表される。
【００７５】
これにより、観測モデルと状態変数ベクトルを、
【数４９】
……（４９）
【数５０】
……（５０）
として解けばよい。
【００７６】
一重位相差の場合は、
【数５１】
……（５１）
【数５２】
……（５２）
として解けばよい。
【００７７】
なお、基線が１つの場合でも、移動体のロール又はピッチに定数を与えたモデルを使えば同様に計算を行うことができる。
【００７８】
［移動体姿勢計測装置の構成・動作］
次に、上述のアルゴリズムを用いた移動体姿勢計測装置について説明する。
【００７９】
図４は、本実施形態の移動体姿勢計測装置１の構成を説明するブロック図である。移動体姿勢計測装置１は、衛星受信部２と、処理部３とからなる。
【００８０】
信号受信部２は、移動体姿勢計測装置１で姿勢を計測すべき船舶などの移動体に設けられ、それぞれ衛星の信号を受信する基準アンテナ１１と、複数個（ｎ個（１，２，３，…））のユーザアンテナ１２（第１ユーザアンテナ、第２ユーザアンテナ、…、第ｎユーザアンテナ）とを備えている。各アンテナ１１，１２は、衛星信号処理部１３にそれぞれ個別に用意された各信号受信機１４と接続されている。各信号受信機１４では、各アンテナ１１，１２で受信した信号から周知の手段により、移動体と衛星との位置関係、すなわち、衛星の方位角情報、仰角情報、並びに、各衛星から受信した各アンテナ１１，１２での衛星信号の搬送波位相の位相情報を算出し、この情報を処理部３に出力する。
【００８１】
処理部３は、信号受信部２から受信したから姿勢を算出する。
【００８２】
すなわち、概略姿勢等保存部２１は、移動体の概略姿勢の情報（また、存在するのであれば、その姿勢について定まっているいくつかの整数値アンビギュイティ）を記憶している。
【００８３】
計算部２６は各種計算を行う。まず、整数値アンビギュイティ計算部２２は、各衛星についての方位角情報、仰角情報、及び各アンテナ１１，１２での位相情報、並びに概略姿勢等保存部２１に記憶されている概略姿勢（また、存在するのであれば、その姿勢について定まっているいくつかの整数値アンビギュイティ）又は概略方位候補算出部２８において算出されたａｐに対する候補（前述の（３６）〜（４０）式に基づいて計算する）及びそれぞれの候補に対する状態ベクトルの初期値ｘ０（ｘにハット）が与えられていたとき、二重位相差の連立方程式（（２０）式と（２１）式）又は一重位相差の連立方程式（（５１）式と（５２）式）に基づいて二重位相差又は一重位相差を計算し、初期値（前述の初期点ｘ０（ｘにハット））に適当な値を設定して（例えば、前述の概略姿勢を初期値とする）、未知の整数値アンビギュイティを計算する。具体的には、（２４）式又は（２５）式により計算することができる。
【００８４】
また、姿勢推定部２３は、非線形推定法を用いて移動体の姿勢を推定する（例えば、前述の非線形最小二乗法を用いて（２３）式を収束させることで求める）。なお、姿勢成分はすべて計算で求めるのではなく、概略方位候補算出部２８、整数値アンビギュイティ計算部２２、姿勢推定部２３は、図５に示すように傾斜測定装置３０を備えた船体傾斜測定部２９を用いてロール、ピッチ又はその両者を測定し、その測定値を補助として用いるようにしてもよい。
【００８５】
また、収束判定部２４は、姿勢推定部２３で判定した移動体の姿勢の値が収束しているか否かを判定し、収束していないときは、整数値アンビギュイティ計算部２２、姿勢推定部２３による前述の演算を再度行う。
【００８６】
収束判定部２４で移動体の姿勢の値が収束していると判定したときは、分散推定値計算部２５で、移動体の姿勢の値の収束時の分散推定値を計算する（前述の（３０）式で計算できる）。
【００８７】
姿勢決定部２７は、分散推定値計算部２５で得られた分散推定値を用いて、得られた整数値アンビギュイティと移動体の姿勢の値とが正しいか否かを検定する。具体的には、前述のとおり、（３０）式のσ２（σにハット）と閾値σ２thとを比較することにより行う。また、複数のアンビギュイティの候補がある場合は、候補の中で１番小さい推定分散量と２番目に小さい推定分散量の比をある閾値ｒｔｈを用いて検定する。決定された整数値アンビギュイティと姿勢計測値は概略姿勢等保存部２１に保存され、次回の方位計算のとき又はアンビギュイティの決定のときに使用される。
【００８８】
以上説明した移動体姿勢計測装置１によれば、整数値アンビギュイティベクトルをすべて同時に解くことができるので、従来に比べて移動体の姿勢の計算が簡易なものとなる。
【００８９】
また、従来のように複数の基線ベクトルを求める必要はない等のため、移動体の姿勢の算出値の精度を向上させることができる。
【００９０】
さらに、算出値に対して計算される観測誤差の推定分散量の精度が向上し、算出された姿勢計測値と整数値アンビギュイティベクトルの検定の確度を向上させることができる。
【図面の簡単な説明】
【００９１】
【図１】一重位相差について説明する概念図である。
【図２】基準アンテナが２個の場合のアンテナ配置について説明する説明図である。
【図３】基準アンテナがｎ個の場合のアンテナ配置について説明する説明図である。
【図４】本発明の一実施の形態である移動体姿勢計測装置のブロック図である。
【図５】図５の移動体姿勢計測装置の変形例を示すブロック図である。
【符号の説明】
【００９２】
１ 移動体姿勢計測装置
１１ 基準アンテナ
１２ ユーザアンテナ
２２ 整数値アンビギュイティ計算部
２３ 姿勢推定部
２４ 収束判定部
２５ 姿勢決定部
【技術分野】
【０００１】
本発明は、ＧＰＳなどの衛星航法システムを用いて船舶などの移動体の姿勢を計測する移動体姿勢計測装置に関する。
【背景技術】
【０００２】
船舶などの移動体の姿勢は、方位ψ、ピッチθ、ロールφを使って表すことができる。移動体に固定された右手系直交座標(移動体座標系）を定義したとき、これらの量は次のように定義される。
【０００３】
方位とは、局所座標系のｘ軸から移動体座標系のｘ軸の局所座標系のｘ‐ｙ平面への射影へのなす角度である。
【０００４】
ピッチとは、移動体座標系のｘ軸の局所座標系のｘ‐ｙ平面への射影から移動体座標系のｘ軸へのなす角度である。
【０００５】
ロールとは、移動体座標系のｘ軸を中心とした、移動体座標系ｙ軸の局地座標系の水平面からの回転角である。
【０００６】
このような移動体の姿勢を検出する移動体姿勢検出装置としては、ＧＰＳ、ＧＬＯＮＡＳＳなどの衛星航法システムを用いたものが知られている。すなわち、この移動体姿勢検出装置は、複数のアンテナで受信されるＧＰＳ衛星などの信号から移動体の姿勢を検出するものである。
【０００７】
このように、移動体に設置された複数のアンテナで受信されるＧＰＳ衛星からの信号から姿勢情報を正確に算出する場合には、各アンテナ間の一重位相差または二重位相差を用いる。
【０００８】
図１に示すように、一重位相差とは、ある基線ｂを構成する基準アンテナ１０１及びユーザアンテナ１０２にそれぞれ接続された基準受信機１０３、ユーザ受信機１０４により測定された、同一の衛星１０５からの信号の搬送波位相の差分である。この差分と基準アンテナ１０１及びユーザアンテナ１０２により構成される基線ベクトルｂの間には行路差から次の関係式が成り立つ。
【数１】
……（１）
ここで、ｙは一重位相差、ｈは衛星１０５への単位方向ベクトル、ｂは基線ベクトル、ａは整数の値、Δｔは受信機１０３，１０４の位相の取得タイミングのズレを表す時計誤差、ｅは観測誤差である。添え字ｓは一重位相差であることを意味する。また、単位は波数で表している。式（１）は、一重位相差位相と行路差ｈsTｂの間には、ある波長λの整数倍とΔｔを足し合わせた分だけの差があることを示している。例えば、各アンテナで瞬間的な位相を観測したとき、その値の範囲は[-0.5:0.5]となる。
【０００９】
このとき、一重位相差の値は[-1.0:1.0]でしか表されないため、行路差ｈsTｂとの間には、ある波長λの整数倍とΔｔを足し合わせた分だけの差があることになる。この整数値は整数値アンビギュイティと呼ばれ、式（１）のａsにあたる。
【００１０】
また、二重位相差とは、ある基線における基準衛星の一重位相差と基準衛星以外の一重位相差との差分である。基準衛星ｉと衛星ｋの二重位相差と基線の関係は、
【数２】
……（２）
と表される。
【００１１】
ここで、
【数３】
……（３）
【数４】
……（４）
【数５】
……（５）
【数６】
……（６）
を意味する。ここで、添え字ｄは二重位相差に関する値であることを表している。このように二重位相差を計算することで、時計誤差を打ち消すことができる（なお、二重位相差については特許文献３を参照）。
【００１２】
次に、従来における移動体の姿勢の算出法について図２を参照して説明する。ここでは、移動体を船舶１１１とし、船舶１１１内にアンテナ１１２〜１１４を３つ配置し、二重位相差を用いて算出する場合を例に取り説明する。基準アンテナ１１２から各ユーザアンテナ１１３，１１４への基線ベクトルｂ１，ｂ２より、船舶１１１の姿勢は、
【００１３】
【数７】
……（７）
【数８】
……（８）
【数９】
……（９）
となる。ここで、ｇ{-}は、基線ベクトルから各姿勢情報を計算する関数である。
【００１４】
この関数ｇ{-}は具体的に次のように表される。ここでの添え字ｘ，ｙ，ｚは各基線の局地座標系における各ｘ，ｙ，ｚ軸の値であることを示す。
【００１５】
【数１０】
……（１０）
【数１１】
……（１１）
【数１２】
……（１２）
【００１６】
ここで、
【数１３】
……（１３）
である。また、arg(-)は複素数の偏角である。
【００１７】
基線ベクトルｂ１，ｂ２についての観測方程式は、
【数１４】
……（１４）
【数１５】
……（１５）
と表せる。ここで、ｙ(bi)は基線ｉで観測された位相差により構成されるベクトルである。ａ(bi)は基線ｉで観測された位相差に対応する整数値アンビギュイティにより構成されるベクトルである。Ｈ(bi)は基線ｉで観測された基線ベクトルと位相差の関係を表すベクトルｈにより構成される行列である。ｅ(bi)は基線ｉで観測される位相差に混入する観測誤差により構成されるベクトルである。
【特許文献１】特許第３５０２００７号公報
【特許文献２】特開２００６‐１０５６３５号公報
【特許文献３】特開２００２‐５４９４６号公報
【発明の開示】
【発明が解決しようとする課題】
【００１８】
しかしながら、前述の従来における移動体の姿勢の算出法では、基線ベクトルｂ１，ｂ２を求めるために、２つの連立方程式（１４）（１５）を別々に求める必要がある。このことから、前述の従来における移動体の姿勢の算出法では、整数値アンビギュイティベクトルａｄ(ｂ１)，ａｄ(ｂ２)をそれぞれ別々に求めており、よって、計算が煩雑であるという不具合があった。
【００１９】
また、全二重位相差の観測数に対して未知数は方位、ピッチ、ロールの３つであるが、基線ベクトルｂ１，ｂ２を求めることから未知数は６つとして求めている。このことから、基線ベクトルｂ１，ｂ２から算出された姿勢ベクトルの精度が劣化してしまうという不具合もあった。
【００２０】
そこで、本発明の目的は、従来に比べて移動体の姿勢の計算が簡易なものとし、また、移動体の姿勢の算出値の精度を向上させることである。
【課題を解決するための手段】
【００２１】
（１）本発明は、姿勢の計測対象となる移動体に設けられて衛星の信号を受信するｎ個（２，３，４，…）のアンテナと、前記各アンテナで受信した衛星の信号から前記移動体と前記衛星との位置関係を算出する位置関係算出手段と、前記各アンテナで受信した衛星信号の搬送波位相を測定する搬送波位相測定手段と、前記搬送波位相測定手段で測定した搬送波位相から一重位相差又は二重位相差を計算する位相差計算手段と、前記移動体の未知の姿勢成分の初期値を算出する初期値算出手段と、前記各アンテナの位置関係、前記移動体と前記衛星との位置関係、及び前記一重位相差又は二重位相差の関係に基づいて、非線形推定法を利用して前記初期値を収束させることにより、未知の整数値アンビギュイティ及び前記移動体の未知の姿勢成分を算出する姿勢等算出手段と、を備えている移動体姿勢計測装置である。
【００２２】
（２）前記初期値算出手段は、未知の前記整数値アンビギュイティのうちの幾つか又は全てに対する候補、それぞれの当該候補に対する前記未知の姿勢成分についての初期値を算出し、前記姿勢等算出手段は、前記整数値アンビギュイティ候補を既知として前記各候補の姿勢成分の初期値を前記非線形推定法により収束させて前記各候補の残りの未知な整数値アンビギュイティ及び前記各候補に対応する前記未知の姿勢成分を算出する算出手段と、前記候補の中から正しい候補を選出する選出手段と、を備えているようにしてもよい。
【００２３】
（３）前記姿勢等算出手段は、前記非線形推定法として非線形最小二乗法を用いるようにしてもよい。
【００２４】
（４）前記姿勢等算出手段で算出された整数値アンビギュイティ及び前記移動体の姿勢により前記一重位相差又は二重位相差における誤差推定量を求める誤差推定量算出手段と、前記誤差推定量を用いた評価指数値により、前記整数値アンビギュイティ及び前記移動体の姿勢の正しさを検定する検定手段と、をさらに備えているようにしてもよい。
【００２５】
（５）前記姿勢等算出手段で算出された整数値アンビギュイティ及び前記移動体の姿勢により前記一重位相差又は二重位相差における誤差推定量を求める誤差推定量算出手段をさらに備え、前記選出手段は、前記誤差推定値を用いた評価指数値により前記候補の中から正しい候補を選出するようにしてもよい。
【００２６】
（６）前記初期値算出手段は、前記姿勢成分のうち傾斜成分を測定し、当該傾斜成分により前記未知の姿勢の初期値を算出するようにしてもよい。
【００２７】
（７）前記姿勢等算出手段で算出された前記整数値アンビギュイティを保存する保存手段をさらに備え、前記姿勢等算出手段は、前記保存手段に保存されている前記整数値アンビギュイティのうち有効なものを既知なものとして利用して、未知の前記整数値アンビギュイティ及び前記移動体の未知の姿勢成分を算出するようにしてもよい。
【００２８】
（８）前記姿勢等算出手段で算出された前記姿勢の値を保存する保存手段をさらに備え、
前記初期値算出手段は、前記保存手段に保存されている前記姿勢の値により前記初期値の算出を行うようにしてもよい。
【００２９】
（９） （２）の発明において、前記初期値算出手段は、前記ｎ個のアンテナのうち２つのアンテナが構成する基線について観測される前記一重位相差又は二重位相差のうち２つ以上の位相差の前記整数値アンビギュイティに対して、当該２つ以上の位相差に対する当該整数値アンビギュイティを決定したとき、当該２つ以上の位相差及び前記衛星の配置の関係から推定される前記基線の基線長が実際の基線と近くなるものを前記候補として算出し、推定された当該基線を利用して残りの前記未知の整数値アンビギュイティに対する初期値と前記未知の姿勢成分に対する初期値とを求めるようにしてもよい。
【００３０】
（１０） （９）の発明において、前記初期値算出手段は、前記姿勢成分のうち傾斜成分を測定し、当該傾斜成分により前記２つ以上の位相差に対する整数値アンビギュイティを算出するようにしてもよい。
【発明の効果】
【００３１】
本発明によれば、整数値アンビギュイティベクトルをすべて同時に解くことができるので、従来に比べて移動体の姿勢の計算が簡易なものとなる。
【００３２】
また、整数値アンビギュイティベクトルを解く際に基線間の配置の関係を利用するので、得られた整数値アンビギュイティベクトルの確度が飛躍的に向上する。さらに、従来のように複数の基線ベクトルを求める必要はない等のため、移動体の姿勢の算出値の精度を向上させることができる。
【発明を実施するための最良の形態】
【００３３】
次に、本発明の実施形態について説明する。以下では、本発明の実施形態である移動体姿勢計測装置で使用されているアルゴリズムについて最初に説明し、次に、このアルゴリズムを使用した移動体姿勢計測装置の構成や動作について説明する。また、以下の例では、移動体が船舶であるものとして説明する。
【００３４】
［アルゴリズムについて］
最初に、本発明の実施形態である移動体姿勢計測装置で使用されているアルゴリズムについて詳細に説明する。
【００３５】
（１）ユーザアンテナ数を２とした場合
まず、図２に示すように、船舶１１１内に基準アンテナ１１２の他にユーザアンテナ１１３，１１４を２つ配置した場合を念頭において説明する。この場合の基線を基線ｂ１，ｂ２とする。
【００３６】
このアルゴリズムの特徴は、複数の線形連立方程式を方位ψ、ピッチθ、ロールφを用いて一つの非線形な連立方程式で表し、非線形推定法を用いて整数値アンビギュイティベクトルを算出しながら姿勢状態を算出することである。このアルゴリズムの詳細は以下のとおりである。
【００３７】
まず、船体座標系（移動体座標系）における各基線ベクトルをｂｖ１，ｂｖ２とする。これらのベクトルは既知である。基線ベクトルｂ１，ｂ２とｂｖ１，ｂｖ２との関係は、次の座標変換行列Ｕ（ψ，θ，φ）を用いて、
【００３８】
【数１６】
……（１６）
と表される。
【００３９】
ここで、座標変換行列Ｕ（ψ，θ，φ）は、次のような関数である。
【数１７】
……（１７）
二重位相差と移動体の姿勢との関係は次の方程式で表される。
【数１８】
……（１８）
【数１９】
……（１９）
【００４０】
そして、基線Ｌ１と基線Ｌ２の観測値を一つの式にまとめて、
【数２０】
……（２０）
と表す。ここで、
【数２１】
……（２１）
である。ここで、（２０）式を観測モデル式と呼び、（２１）式を状態変数ベクトルと呼ぶ。この状態変数ベクトルを用いて表された観測モデル式により、整数値アンビギュイティと移動体の姿勢とが算出される。
【００４１】
今、整数値アンビギュイティベクトルａは未知であるとする。このとき、ｘを推定するには、この観測方程式ｆ（ｘ）のＸに関して微分することで得られた行列、
【数２２】
……（２２）
を用いて、初期点ｘ０（ｘにハット）を適当な値に設定して、非線形推定として非線形最小二乗法を用いて、
【数２３】
……（２３）
を収束させることで求めることができる。
【００４２】
未知である整数値アンビギュイティベクトルａｋ（ａにハット）は、
【数２４】
……（２４）
として毎回を計算する。ここで、round(-)は、一番近い整数への丸め込みを意味する。
【００４３】
あるいは、ある回数ｌを設けて、
【数２５】
……（２５）
としても良い。
【００４４】
また、（２３）式においてのＱは重み行列であり、適当な値を設定するが、
【数２６】
……（２６）
もしくは、
【数２７】
……（２７）
として設定するのが普通である。ここで、Ｅ{-}は期待値、Ｉは単位行列を意味する。
【００４５】
以上のように、初期点を設定して非線形フィルタを収束させることで移動体の姿勢を算出し、また、この収束時に未知な整数値アンビギュイティベクトルを求めることができる。
収束時の残差は、
【００４６】
【数２８】
……（２８）
と求めることができ、その残差二乗和ｒ２は、
【数２９】
……（２９）
と求めることができる。
【００４７】
また、観測雑音の推定分散量は、観測モデル式（２０）における観測数がｍであり、状態変数ベクトルの要素数をｎ（ここでは３となる）とすれば、
【数３０】
……（３０）
として表すことができる。
【００４８】
このσ２（σにハット）と閾値σ２thとを比較し、算出された移動体の姿勢と整数値アンビギュイティベクトルとを検定することができる。
【００４９】
このような手段によると、従来の手段と比べて移動体の姿勢を解くための観測値が多くなるので推定された姿勢の精度が向上する。
【００５０】
また、基線間の配置の関係を利用するので、観測誤差の推定分散も向上し、得られた方位と整数値アンビギュイティが誤っている場合には値が大きくなることから、算出された姿勢の検定結果の確度が飛躍的に向上する。
【００５１】
（２）整数値アンビギュイティベクトルの値がわかっている場合
前述の例は整数値アンビギュイティベクトルの値がすべて不明である場合の例について説明したが、以下では、整数値アンビギュイティベクトルａのうちいくつかはすでに値が判明している場合について考える。
【００５２】
まず、（２０）式を整数値アンビギュイティベクトルがすでに定まっているものと、定まっていないものとに分けて、
【数３１】
……（３１）
と表す。ここで、添え字ｐは、整数値アンビギュイティベクトルが決まっているものを表し、添え字ｑは整数値アンビギュイティベクトルが決まっていないものを表す。
【００５３】
そして、状態ベクトルの初期点ｘ０（ｘにハット）を適当な値に設定して、
【数３２】
……（３２）
を収束させることで姿勢情報を算出する。
【００５４】
ここで、
【数３３】
……（３３）
である。
【００５５】
未知である整数値アンビギュイティベクトルａｑは、
【数３４】
……（３４）
として毎回算出する。
【００５６】
もしくは、ある回数ｌを設けて、
【数３５】
……（３５）
としても良い。
【００５７】
収束した移動体の方位に対して、観測誤差の推定分散σ２（σにハット）をもとめ、閾値σth２を比較することで、算出された移動体の姿勢と整数値アンビギュイティベクトルを検定することができる。
【００５８】
この場合、すべての整数値アンビギュイティベクトルを未知の値とした前述の場合に比べて、初期値ｘ０の値が適切な値でなく、誤った姿勢に収束した場合、固定された整数値アンビギュイティベクトル値の影響により推定分散量が大きくなることから、算出された姿勢ベクトルの検定の確度が向上する。
【００５９】
以上のことを利用して、何らかの方法によってａｐに対する候補及びそれぞれの候補に対する状態ベクトルの初期値ｘ０が与えられていたときに、正しい候補を選び出すことを考える。
【００６０】
ａｐに対する候補及びそれぞれの候補に対する状態ベクトルの初期値ｘ０の算出方法としては次の方法が考えられる。まず、ｐに関する式を、基線ｉに関する観測モデル式のうちの２つ以上の観測位相差から構成される観測モデル式を用いて次のように構成する。
【数３６】
……（３６）
【００６１】
このとき、基線は、
【数３７】
……（３７）
と推定できる。
【００６２】
そこで、基線ｉの基線長は既知であるということを利用して、
【数３８】
……（３８）
となるａｐを候補とする。
【００６３】
２つの観測位相差では、（３７）式を解くことはできないが、基線ｉのＺ成分をｂｚ（ｉ）と（ｂにハット）適当に与えて、
【数３９】
……（３９）
と、基線の水平方向のみを推定するようにすれば解くことができる。ここで、Ｈｘｙ（ｐ），ｈｚ（ｐ）はそれぞれＨｐの水平方向成分の行列、垂直方向成分の行列である。
【００６４】
このとき、
【数４０】
……（４０）
となるａｐを候補とする。ａｐに対する状態変数ベクトルは、推定された基線ベクトルの水平方向成分により計算される概略方位と、適当に与えたロール、ピッチとする。
【００６５】
以上のとおり、ｂｚ（ｉ）（ｂにハット）、ロール、ピッチは、傾斜計などの計測装置の測定値を基にすることで、状態変数ベクトルの精度を向上させることができ、ａｑが正しい値に収束しやすくすることができる。以上のようにして得られた各候補に対して、状態変数ベクトル（姿勢）、整数値アンビギュイティベクトル、観測誤差の推定分散量を計算して、推定分散量から適当な判断基準を用いて正しい候補を選び出す。適当な判断基準の例としては、一番小さい推定分散量と二番目に小さい推定分数量を用いて、その比をある闇値ｒｔｈと比較することで一番小さい推定分散量を持つ候補を検定できる。各候補においての推定分敵量は正しいものは小さク、誤っているものは大きいので、選び出される候補の検定の確度が向上する。
【００６６】
（３）一重位相差のモデル
以上の例では二重位相差を用いて移動体の姿勢を推定する場合について述べたが、二重位相差の代わりに一重位相差を用いることも可能である。
【００６７】
これは、二重位相差の観測モデル式（２０）と、（２１）式で表される状態変数ベクトルを、一重位相差の観測モデル式（４１）式と状態変数ベクトル（４２）式に代えて、前述の二重位相差の場合と同様に解けばよい。
【数４１】
……（４１）
【数４２】
……（４２）
ここでｃｂｉは基線ｉについて観測される一重位相差の要素数を持つ要素の値が１であるベクトル、Δｔｂｉは基線ｉについての時計誤差である。
【００６８】
（４）ロール、ピッチ測定可能のモデル
移動体の姿勢のうち、ピッチ若しくはロール又はその両者を、例えば傾斜計などの計測装置により求め、その得られた値よりピッチ若しくはロール又はその両者を定数で与え、方位のみを推定することも可能である。
【００６９】
いま、傾斜計などの計測装置より得られたピッチの値、ロールの値をθｃ，φｃとし、この値を用いて移動体の方位のみを推定することを考えれば、二重位相差においては、観測モデル式と状態変数ベクトルを、
【数４３】
……（４３）
【数４４】
……（４４）
として解けば良い。
【００７０】
一重位相差においては、
【数４５】
……（４５）
【数４６】
……（４６）
として解けばよい。
【００７１】
また、Δtｂ１若しくはΔtｂ２又はその両者の値が既知であるならば、その値を与えた観測モデル式を用いて解くことも可能である。
【００７２】
（５）ユーザアンテナ数をｎ個とした場合
次に、前述のようにユーザアンテナ数を２個には限定せず、図３に示すように一般的にｎ個（１，２，３，…）とする場合について考える。
【００７３】
移動体座標系における基準アンテナから各ユーザへの基線ベクトルをｂｖ１，ｂｖ２，…，ｂｖｎと表す。これらのベクトルは既知である。局地座標系における基準アンテナから各ユーザへの基線ベクトルｂ１，ｂ２，ｂ３は、
【数４７】
……（４７）
と表される。
【００７４】
よって、二重位相差と姿勢の関係は、
【数４８】
……（４８）
と表される。
【００７５】
これにより、観測モデルと状態変数ベクトルを、
【数４９】
……（４９）
【数５０】
……（５０）
として解けばよい。
【００７６】
一重位相差の場合は、
【数５１】
……（５１）
【数５２】
……（５２）
として解けばよい。
【００７７】
なお、基線が１つの場合でも、移動体のロール又はピッチに定数を与えたモデルを使えば同様に計算を行うことができる。
【００７８】
［移動体姿勢計測装置の構成・動作］
次に、上述のアルゴリズムを用いた移動体姿勢計測装置について説明する。
【００７９】
図４は、本実施形態の移動体姿勢計測装置１の構成を説明するブロック図である。移動体姿勢計測装置１は、衛星受信部２と、処理部３とからなる。
【００８０】
信号受信部２は、移動体姿勢計測装置１で姿勢を計測すべき船舶などの移動体に設けられ、それぞれ衛星の信号を受信する基準アンテナ１１と、複数個（ｎ個（１，２，３，…））のユーザアンテナ１２（第１ユーザアンテナ、第２ユーザアンテナ、…、第ｎユーザアンテナ）とを備えている。各アンテナ１１，１２は、衛星信号処理部１３にそれぞれ個別に用意された各信号受信機１４と接続されている。各信号受信機１４では、各アンテナ１１，１２で受信した信号から周知の手段により、移動体と衛星との位置関係、すなわち、衛星の方位角情報、仰角情報、並びに、各衛星から受信した各アンテナ１１，１２での衛星信号の搬送波位相の位相情報を算出し、この情報を処理部３に出力する。
【００８１】
処理部３は、信号受信部２から受信したから姿勢を算出する。
【００８２】
すなわち、概略姿勢等保存部２１は、移動体の概略姿勢の情報（また、存在するのであれば、その姿勢について定まっているいくつかの整数値アンビギュイティ）を記憶している。
【００８３】
計算部２６は各種計算を行う。まず、整数値アンビギュイティ計算部２２は、各衛星についての方位角情報、仰角情報、及び各アンテナ１１，１２での位相情報、並びに概略姿勢等保存部２１に記憶されている概略姿勢（また、存在するのであれば、その姿勢について定まっているいくつかの整数値アンビギュイティ）又は概略方位候補算出部２８において算出されたａｐに対する候補（前述の（３６）〜（４０）式に基づいて計算する）及びそれぞれの候補に対する状態ベクトルの初期値ｘ０（ｘにハット）が与えられていたとき、二重位相差の連立方程式（（２０）式と（２１）式）又は一重位相差の連立方程式（（５１）式と（５２）式）に基づいて二重位相差又は一重位相差を計算し、初期値（前述の初期点ｘ０（ｘにハット））に適当な値を設定して（例えば、前述の概略姿勢を初期値とする）、未知の整数値アンビギュイティを計算する。具体的には、（２４）式又は（２５）式により計算することができる。
【００８４】
また、姿勢推定部２３は、非線形推定法を用いて移動体の姿勢を推定する（例えば、前述の非線形最小二乗法を用いて（２３）式を収束させることで求める）。なお、姿勢成分はすべて計算で求めるのではなく、概略方位候補算出部２８、整数値アンビギュイティ計算部２２、姿勢推定部２３は、図５に示すように傾斜測定装置３０を備えた船体傾斜測定部２９を用いてロール、ピッチ又はその両者を測定し、その測定値を補助として用いるようにしてもよい。
【００８５】
また、収束判定部２４は、姿勢推定部２３で判定した移動体の姿勢の値が収束しているか否かを判定し、収束していないときは、整数値アンビギュイティ計算部２２、姿勢推定部２３による前述の演算を再度行う。
【００８６】
収束判定部２４で移動体の姿勢の値が収束していると判定したときは、分散推定値計算部２５で、移動体の姿勢の値の収束時の分散推定値を計算する（前述の（３０）式で計算できる）。
【００８７】
姿勢決定部２７は、分散推定値計算部２５で得られた分散推定値を用いて、得られた整数値アンビギュイティと移動体の姿勢の値とが正しいか否かを検定する。具体的には、前述のとおり、（３０）式のσ２（σにハット）と閾値σ２thとを比較することにより行う。また、複数のアンビギュイティの候補がある場合は、候補の中で１番小さい推定分散量と２番目に小さい推定分散量の比をある閾値ｒｔｈを用いて検定する。決定された整数値アンビギュイティと姿勢計測値は概略姿勢等保存部２１に保存され、次回の方位計算のとき又はアンビギュイティの決定のときに使用される。
【００８８】
以上説明した移動体姿勢計測装置１によれば、整数値アンビギュイティベクトルをすべて同時に解くことができるので、従来に比べて移動体の姿勢の計算が簡易なものとなる。
【００８９】
また、従来のように複数の基線ベクトルを求める必要はない等のため、移動体の姿勢の算出値の精度を向上させることができる。
【００９０】
さらに、算出値に対して計算される観測誤差の推定分散量の精度が向上し、算出された姿勢計測値と整数値アンビギュイティベクトルの検定の確度を向上させることができる。
【図面の簡単な説明】
【００９１】
【図１】一重位相差について説明する概念図である。
【図２】基準アンテナが２個の場合のアンテナ配置について説明する説明図である。
【図３】基準アンテナがｎ個の場合のアンテナ配置について説明する説明図である。
【図４】本発明の一実施の形態である移動体姿勢計測装置のブロック図である。
【図５】図５の移動体姿勢計測装置の変形例を示すブロック図である。
【符号の説明】
【００９２】
１ 移動体姿勢計測装置
１１ 基準アンテナ
１２ ユーザアンテナ
２２ 整数値アンビギュイティ計算部
２３ 姿勢推定部
２４ 収束判定部
２５ 姿勢決定部
【特許請求の範囲】
【請求項１】
姿勢の計測対象となる移動体に設けられて衛星の信号を受信するｎ個（２，３，４,…）のアンテナと、
前記各アンテナで受信した衛星の信号から前記移動体と前記衛星との位置関係を算出する位置関係算出手段と、
前記各アンテナで受信した衛星信号の搬送波位相を測定する搬送波位相測定手段と、
前記搬送波位相測定手段で測定した搬送波位相から一重位相差又は二重位相差を計算する位相差計算手段と、
前記移動体の未知の姿勢成分の初期値を算出する初期値算出手段と、
前記各アンテナの位置関係、前記移動体と前記衛星との位置関係、及び前記一重位相差又は二重位相差の関係に基づいて、非線形推定法を利用して前記初期値を収束させることにより、未知の整数値アンビギュイティ及び前記移動体の未知の姿勢成分を算出する姿勢等算出手段と、
を備えている移動体姿勢計測装置。
【請求項２】
前記初期値算出手段は、未知の前記整数値アンビギュイティのうちの幾つか又は全てに対する候補、それぞれの当該候補に対する前記未知の姿勢成分についての初期値を算出し、
前記姿勢等算出手段は、
前記整数値アンビギュイティ候補を既知として前記各候補の姿勢成分の初期値を前記非線形推定法により収束させて前記各候補の残りの未知な整数値アンビギュイティ及び前記各候補に対応する前記未知の姿勢成分を算出する算出手段と、
前記候補の中から正しい候補を選出する選出手段と、
を備えている、
請求項１に記載の移動体姿勢計測装置。
【請求項３】
前記姿勢等算出手段は、前記非線形推定法として非線形最小二乗法を用いる、請求項１又は２に記載の移動体姿勢計測装置。
【請求項４】
前記姿勢等算出手段で算出された整数値アンビギュイティ及び前記移動体の姿勢により前記一重位相差又は二重位相差における誤差推定量を求める誤差推定量算出手段と、
前記誤差推定量を用いた評価指数値により、前記整数値アンビギュイティ及び前記移動体の姿勢の正しさを検定する検定手段と、
をさらに備えている請求項１に記載の移動体姿勢計測装置。
【請求項５】
前記姿勢等算出手段で算出された整数値アンビギュイティ及び前記移動体の姿勢により前記一重位相差又は二重位相差における誤差推定量を求める誤差推定量算出手段をさらに備え、
前記選出手段は、前記誤差推定値を用いた評価指数値により前記候補の中から正しい候補を選出する、
請求項２に記載の移動体姿勢計測装置。
【請求項６】
前記初期値算出手段は、前記姿勢成分のうち傾斜成分を測定し、当該傾斜成分により前記未知の姿勢の初期値を算出する、請求項１又は２に記載の移動体姿勢計測装置。
【請求項７】
前記姿勢等算出手段で算出された前記整数値アンビギュイティを保存する保存手段をさらに備え、
前記姿勢等算出手段は、前記保存手段に保存されている前記整数値アンビギュイティのうち有効なものを既知なものとして利用して、未知の前記整数値アンビギュイティ及び前記移動体の未知の姿勢成分を算出する、
請求項１又は２に記載の移動体姿勢計測装置。
【請求項８】
前記姿勢等算出手段で算出された前記姿勢の値を保存する保存手段をさらに備え、
前記初期値算出手段は、前記保存手段に保存されている前記姿勢の値により前記初期値の算出を行う、
請求項１又は２に記載の移動体姿勢計測装置。
【請求項９】
前記初期値算出手段は、前記ｎ個のアンテナのうち２つのアンテナが構成する基線について観測される前記一重位相差又は二重位相差のうち２つ以上の位相差の前記整数値アンビギュイティに対して、当該２つ以上の位相差に対する当該整数値アンビギュイティを決定したとき、当該２つ以上の位相差及び前記衛星の配置の関係から推定される前記基線の基線長が実際の基線と近くなるものを前記候補として算出し、推定された当該基線を利用して残りの前記未知の整数値アンビギュイティに対する初期値と前記未知の姿勢成分に対する初期値とを求める、請求項２に記載の移動体姿勢計測装置。
【請求項１０】
前記初期値算出手段は、前記姿勢成分のうち傾斜成分を測定し、当該傾斜成分により前記２つ以上の位相差に対する整数値アンビギュイティを算出する、請求項９に記載の移動体姿勢計測装置。
【請求項１】
姿勢の計測対象となる移動体に設けられて衛星の信号を受信するｎ個（２，３，４,…）のアンテナと、
前記各アンテナで受信した衛星の信号から前記移動体と前記衛星との位置関係を算出する位置関係算出手段と、
前記各アンテナで受信した衛星信号の搬送波位相を測定する搬送波位相測定手段と、
前記搬送波位相測定手段で測定した搬送波位相から一重位相差又は二重位相差を計算する位相差計算手段と、
前記移動体の未知の姿勢成分の初期値を算出する初期値算出手段と、
前記各アンテナの位置関係、前記移動体と前記衛星との位置関係、及び前記一重位相差又は二重位相差の関係に基づいて、非線形推定法を利用して前記初期値を収束させることにより、未知の整数値アンビギュイティ及び前記移動体の未知の姿勢成分を算出する姿勢等算出手段と、
を備えている移動体姿勢計測装置。
【請求項２】
前記初期値算出手段は、未知の前記整数値アンビギュイティのうちの幾つか又は全てに対する候補、それぞれの当該候補に対する前記未知の姿勢成分についての初期値を算出し、
前記姿勢等算出手段は、
前記整数値アンビギュイティ候補を既知として前記各候補の姿勢成分の初期値を前記非線形推定法により収束させて前記各候補の残りの未知な整数値アンビギュイティ及び前記各候補に対応する前記未知の姿勢成分を算出する算出手段と、
前記候補の中から正しい候補を選出する選出手段と、
を備えている、
請求項１に記載の移動体姿勢計測装置。
【請求項３】
前記姿勢等算出手段は、前記非線形推定法として非線形最小二乗法を用いる、請求項１又は２に記載の移動体姿勢計測装置。
【請求項４】
前記姿勢等算出手段で算出された整数値アンビギュイティ及び前記移動体の姿勢により前記一重位相差又は二重位相差における誤差推定量を求める誤差推定量算出手段と、
前記誤差推定量を用いた評価指数値により、前記整数値アンビギュイティ及び前記移動体の姿勢の正しさを検定する検定手段と、
をさらに備えている請求項１に記載の移動体姿勢計測装置。
【請求項５】
前記姿勢等算出手段で算出された整数値アンビギュイティ及び前記移動体の姿勢により前記一重位相差又は二重位相差における誤差推定量を求める誤差推定量算出手段をさらに備え、
前記選出手段は、前記誤差推定値を用いた評価指数値により前記候補の中から正しい候補を選出する、
請求項２に記載の移動体姿勢計測装置。
【請求項６】
前記初期値算出手段は、前記姿勢成分のうち傾斜成分を測定し、当該傾斜成分により前記未知の姿勢の初期値を算出する、請求項１又は２に記載の移動体姿勢計測装置。
【請求項７】
前記姿勢等算出手段で算出された前記整数値アンビギュイティを保存する保存手段をさらに備え、
前記姿勢等算出手段は、前記保存手段に保存されている前記整数値アンビギュイティのうち有効なものを既知なものとして利用して、未知の前記整数値アンビギュイティ及び前記移動体の未知の姿勢成分を算出する、
請求項１又は２に記載の移動体姿勢計測装置。
【請求項８】
前記姿勢等算出手段で算出された前記姿勢の値を保存する保存手段をさらに備え、
前記初期値算出手段は、前記保存手段に保存されている前記姿勢の値により前記初期値の算出を行う、
請求項１又は２に記載の移動体姿勢計測装置。
【請求項９】
前記初期値算出手段は、前記ｎ個のアンテナのうち２つのアンテナが構成する基線について観測される前記一重位相差又は二重位相差のうち２つ以上の位相差の前記整数値アンビギュイティに対して、当該２つ以上の位相差に対する当該整数値アンビギュイティを決定したとき、当該２つ以上の位相差及び前記衛星の配置の関係から推定される前記基線の基線長が実際の基線と近くなるものを前記候補として算出し、推定された当該基線を利用して残りの前記未知の整数値アンビギュイティに対する初期値と前記未知の姿勢成分に対する初期値とを求める、請求項２に記載の移動体姿勢計測装置。
【請求項１０】
前記初期値算出手段は、前記姿勢成分のうち傾斜成分を測定し、当該傾斜成分により前記２つ以上の位相差に対する整数値アンビギュイティを算出する、請求項９に記載の移動体姿勢計測装置。
【図１】
【図２】
【図３】
【図４】
【図５】
【図２】
【図３】
【図４】
【図５】
【公開番号】特開２００８−６４５５５（Ｐ２００８−６４５５５Ａ）
【公開日】平成２０年３月２１日（２００８．３．２１）
【国際特許分類】
【出願番号】特願２００６−２４１６４９（Ｐ２００６−２４１６４９）
【出願日】平成１８年９月６日（２００６．９．６）
【出願人】（０００００４３３０）日本無線株式会社 (1,186)
【Ｆターム（参考）】
【公開日】平成２０年３月２１日（２００８．３．２１）
【国際特許分類】
【出願日】平成１８年９月６日（２００６．９．６）
【出願人】（０００００４３３０）日本無線株式会社 (1,186)
【Ｆターム（参考）】
[ Back to top ]