JP4563157B2 - 物体の方位および姿勢検出装置 - Google Patents

Description

この発明は、航空機や船舶等の移動体や、海上浮揚構造体、超高層建築物等の大型構造体など、姿勢が変化する物体の方位および姿勢を検出する装置に関するものである。

従来、複数の測位用衛星からの電波を移動体の所定位置に配置した複数のアンテナで受信し、そのキャリア位相を測定して、アンテナの相対測位を行うことによって、移動体の方位および姿勢を検出するようにした装置がコンパスとして用いられている（たとえば、非特許文献１参照）。

従来、このように複数のアンテナ間の相対測位を行うために、アンテナ間の配置関係が既知であることを利用している。そのため、複数のアンテナを既知の配置関係で取り付けるためのフレームを用い、このフレームにアンテナを取り付けたユニットを移動体の所定位置に、移動体に対して予め定めた関係で取り付けている。例えば上記フレームに３つのアンテナが正三角形をなすように取り付け、その３つのアンテナが成す平面が、移動体の姿勢が水平であるときに水平となるように、すなわちその３つのアンテナが成す平面と移動体の水平面とが平行になるように取り付けている。
"GPS電波"で真方位を検出 高精度でメンテナンスフリー！船舶用サテライトコンパスを開発、［２００４年１２月１日検索］インターネット<URL:http://www.furuno.co.jp/news/sateraito.html>

ところが、このような従来の物体の方位および姿勢検出装置では、複数のアンテナを一定間隔を保って取り付けるとともにそれらを一体化するために大きなフレームが必要となり、物体に対する取付方法の自由度が低いという問題があった。また、各アンテナを物体の所定位置に個別に取り付けると、上記フレームは不要であるが、物体に対する各アンテナの配置関係を事前に実測しておく必要がある。しかし船舶などの移動体の場合、各アンテナ間の距離をメジャーなどで正確に計測することは現実には非常に困難である。

物体の任意の位置に複数のアンテナを配置した場合に、リアルタイムキネマティック（ＲＴＫ）などの手法を用いることによって、各アンテナ間の配置関係を算出することは可能であるが、その算出までに１５分〜１時間程度の長時間を要する。また、この場合でも複数のアンテナを水平面上に設置しなければならないといった設置条件が生じてしまう。

そこで、この発明の目的は、上述の問題を解消して、フレームを用いて複数のアンテナをユニット化する必要がなく、複数のアンテナを水平面上に設置しなければならないといった設置条件を無くし、さらに通常の運用状態で短時間のうちに起動して物体の方位および姿勢を求められるようにした物体の方位および姿勢検出装置を提供することにある。

（１）この発明の物体の方位・姿勢検出装置は、一直線上に並ばず、且つ２つのアンテナの向く方向が非鉛直方向の関係で、物体上のそれぞれ異なる位置に配置した複数のアンテナと、
これら複数のアンテナで複数の測位用衛星からの電波をそれぞれ受信して、前記複数のアンテナのローカル座標系における基線ベクトルを測位するアンテナ相対測位手段と、
前記アンテナ相対測位手段により測位された前記複数のアンテナのローカル座標系における基線ベクトルに基づいて前記複数のアンテナが成す面の姿勢と前記物体の姿勢とのずれを物体座標系に対するアンテナの姿勢のオフセットとして算出し、基準とするアンテナに対する他のアンテナまでの距離である基線長を算出し、前記アンテナの姿勢のオフセット及び前記基線長をアンテナの配置関係として保存するアンテナ配置関係設定手段と、
前記アンテナ相対測位手段により求められた、前記基準とするアンテナから他のアンテナへのローカル座標系における基線ベクトルに基づいてアンテナ座標系からローカル座標系への座標変換オペレータを求め、アンテナ座標系からローカル座標系への座標変換オペレータと前記アンテナの配置関係とに基づいて、物体座標系からローカル座標系への座標変換オペレータを求め、当該物体座標系からローカル座標系への座標変換オペレータを基に、物体のローカル座標系に対する方位角または方位角と共に姿勢角を求める座標変換手段と、
を設けたことを特徴としている。

（２）また、この発明の物体の方位・姿勢検出装置は、前記アンテナ配置関係設定手段が、前記アンテナの姿勢のオフセット及び前記基線長以外に、前記複数のアンテナのうち２つのアンテナが成す方位と前記物体の方位とのずれである方位のオフセットを計測し、前記アンテナの姿勢のオフセット、前記基線長及び前記方位のオフセットをアンテナの配置関係として保存することを特徴としている。

（３）また、この発明の物体の方位・姿勢検出装置は、前記複数のアンテナのうち２つのアンテナが成すアンテナ座標系に対する方位と物体の物体座標系に対する方位とが一致するようにアンテナが設置されて、前記方位のオフセットが０にされたことを特徴としている。

（４）前記アンテナ配置関係設定手段は、前記複数のアンテナのうち基準とするアンテナに対する他のアンテナの相対位置を例えばリアルタイムキネマティック法により求める。

（５）また、この発明の物体の方位・姿勢検出装置は、前記物体の姿勢が水平状態または準水平状態にあるとき、前記複数のアンテナのうち基準とするアンテナに対する他のアンテナの相対位置を求めるとともに、前記複数のアンテナが成す面の姿勢を算出し、該姿勢を前記アンテナの姿勢のオフセットとするアンテナ姿勢計測手段を備える。

（６）また、この発明の物体の方位・姿勢検出装置は、前記物体が水平状態を中心として動揺している状態で、前記複数のアンテナのうち基準とするアンテナに対する他のアンテナの相対位置を求めるとともに、前記複数のアンテナが成す面の姿勢を所定時間繰り返し算出し、該姿勢の平均値を前記アンテナの姿勢のオフセットとするアンテナ姿勢計測手段を備える。

（７）前記アンテナ姿勢計測手段は、前記複数のアンテナのうち基準とするアンテナに対する他のアンテナの相対位置を例えばリアルタイムキネマティック法により求める。

（８）前記アンテナ配置関係設定手段は、例えば前記アンテナの配置関係を算出した後、前記基準とするアンテナに対する他のアンテナまでの距離が既定範囲内であるか否かに基づいてその算出結果の妥当性を検証する。

（９）前記アンテナ配置関係設定手段は、例えば前記複数のアンテナの配置関係が既定範囲内であるか否かに基づいてその算出結果の妥当性を検証する。

（１）物体上のそれぞれ異なる位置に配置した複数のアンテナのうち基準とするアンテナに対する他のアンテナの相対位置を求めるとともに、複数のアンテナが成す面の姿勢と物体の姿勢とのずれを物体座標系に対するアンテナの姿勢のオフセットとして計測し、基準とするアンテナに対する他のアンテナまでの距離である基線長を計測し、アンテナの姿勢のオフセット及び基線長をアンテナの配置関係として保存するようにしたので、複数のアンテナを物体上のそれぞれ異なる位置に配置すればよく、従来のフレームを用いることによる制約がなくなる。また、複数のアンテナを水平面上に設置しなければならないといった設置条件も不要となる。さらに、このように一旦アンテナ配置関係を保存した後はアンテナ間の配置関係が既知であることを利用できるので、アンテナ配置関係を保存した後は、物体の方位および姿勢を短時間（例えば５分以内）で算出できることになる。

（２）物体上のそれぞれ異なる位置に配置した複数のアンテナのうち２つのアンテナが成す方位と物体の方位とのずれを方位のオフセットとして計測し、アンテナの姿勢のオフセット、基線長及び方位のオフセットをアンテナの配置関係として保存するようにしたので、複数のアンテナを物体上のそれぞれ異なる位置に配置すればよく、上記（１）の場合と同様に、フレームを用いることによる制約や、複数のアンテナを水平面上に設置しなければならないといった条件も不要となる。さらに、このように一旦アンテナ配置関係を保存した後はアンテナ間の配置関係が既知であることを利用できるので、アンテナ配置関係を保存した後は、物体の方位および姿勢を短時間で算出できることになる。

（３）物体上のそれぞれ異なる位置に配置した複数のアンテナのうち２つのアンテナが成す方位と物体の方位とが一致するようにアンテナを設置し、これらの複数のアンテナが成す面の姿勢と物体の姿勢とのずれを姿勢のオフセットとして計測または入力し、該姿勢のオフセットを前記アンテナの配置関係として保存するようにしたので、複数のアンテナを物体上のそれぞれ異なる位置に配置すればよく、上記（１）の場合と同様に、フレームを用いることによる制約や、複数のアンテナを水平面上に設置しなければならないといった条件も不要となる。さらに、このように一旦アンテナ配置関係を保存した後はアンテナ間の配置関係が既知であることを利用できるので、アンテナ配置関係を保存した後は、物体の方位および姿勢を短時間で算出できることになる。

（４）前記アンテナ配置関係設定手段による前記複数のアンテナのうち基準とするアンテナに対する他のアンテナの相対位置は、測量などで用いられるレーザ測量技術によってアンテナ間の距離や傾きを測定するという方法や、複数のアンテナを近接して設置した場合には、アンテナ間の距離をメジャー等で実測し、複数のアンテナにまたがるように板等を載せて、その板等の姿勢を姿勢センサ等で計測するという方法で求めることができるが、特にリアルタイムキネマティック法により求めることによって、相対測位に必要な時間だけ単に受信すれば、基準とするアンテナに対する他のアンテナの相対位置を自動的に求めることができ、利用者の取扱が容易となる。

（５）物体の姿勢が水平状態または準水平状態にある時、複数のアンテナが成す面の姿勢を算出し、その姿勢をアンテナの姿勢オフセットとして求めることにより、物体に対するアンテナの姿勢のオフセットを自動的に計測でき、傾斜計などの他のセンサを用いることなく簡便に利用できるようになる。

（６）物体が水平状態を中心として動揺している状態で、複数のアンテナの相対位置を求めて、複数のアンテナが成す面の姿勢を平均化してアンテナの姿勢オフセットを求めるようにしたことにより、物体に対するアンテナの姿勢のオフセットを自動的に計測でき、この場合も傾斜計などの他のセンサを用いることなく簡便に利用できるようになる。

（７）前記アンテナ姿勢計測手段による前記複数のアンテナのうち基準とするアンテナに対する他のアンテナの相対位置をリアルタイムキネマティック法により求めることによって、相対測位に必要な時間だけ単に受信すれば、基準とするアンテナに対する他のアンテナの相対位置を自動的に求めることができ、利用者の取扱が容易となる。

（８）前記アンテナ配置関係設定手段によりアンテナの配置関係を算出した後、基準とするアンテナに対する他のアンテナまでの距離が既定範囲内であるか否かに基づいてその算出結果の妥当性を検証するようにしたことにより、複数のアンテナの配置条件をアンテナ間の距離で既定した場合にその検証が容易となる。また、誤ってアンテナの配置関係が求められることや、誤った入力が検知でき、方位や姿勢の誤った出力が防止できる。

（９）前記アンテナ配置関係設定手段によりアンテナの配置関係を算出した後、基準とするアンテナに対する他のアンテナまでの距離が既定範囲内であるか否かによって上記妥当性の検証を行うことにより、複数のアンテナの配置関係でその配置を既定している場合に容易に検証可能となる。また、誤ってアンテナの配置関係が求められたり、誤って入力されたりすることが検知でき、方位や姿勢の誤った出力が防止できる。

この発明の実施形態に係る物体の方位および姿勢検出装置について各図を参照して説明する。
図１は船舶の方位および姿勢を検出する場合に、その船舶に対するアンテナの配置例を示している。この例では、船舶１に３つのアンテナＡ０，Ａ１，Ａ２をそれぞれ所定位置に配置している。これらのアンテナＡ０，Ａ１，Ａ２はそれぞれ複数のＧＰＳ衛星Ｓ１，Ｓ２，Ｓ３，・・・・Ｓｎからの電波を受信する。

船舶１の船首方位（方位角）、姿勢（ロール角，ピッチ角）をそれぞれ検出するためには、これら３つのアンテナＡ０，Ａ１，Ａ２は次の条件を満たすように設置する必要がある。

（１）３つのアンテナＡ０，Ａ１，Ａ２を一直線上に配置しない。

（２）３つのアンテナＡ０，Ａ１，Ａ２のうち２つのアンテナが向く方向を鉛直方向にしない。

なお、ここでアンテナＡ０を基準アンテナとし、この基準アンテナＡ０から他のアンテナＡ１へのベクトルの水平成分の方向は船首方向に一致させることが望ましい。一致していない場合には、このＡ０→Ａ１ベクトルの船首方位からのずれをアンテナの方位オフセットとして入力する。
上記条件させ満足すれば３つのアンテナＡ０，Ａ１，Ａ２をどのような関係で配置してもよい。但し、３つのアンテナのうち２つのアンテナが成す方位と船体の方位とが一致するようにアンテナを配置すれば、上記アンテナの方位オフセットは０である。

図２は、上記３つのアンテナＡ０，Ａ１，Ａ２の相対位置を船体に対するアンテナの配置関係に基づいて船体の方位角および姿勢角を表す座標に変換する際に用いる３つの座標系について示している。ここで（Ａ）はローカル座標系、（Ｂ）はアンテナ座標系、（Ｃ）は船体座標系である。

ローカル座標系は（Ａ）に示すように、基準アンテナ（第１のアンテナ）Ａ０の位置を原点として北方向をｘ軸にとり、東方向をｙ軸にとり、鉛直方向をｚ軸にとる。

アンテナ座標系は（Ｂ）に示すように、基準アンテナＡ０の位置を原点として、基準アンテナＡ０から第２のアンテナＡ１への方向をｘ軸にとり、このｘ軸と第３のアンテナＡ２を含む平面上でｘ軸に直交する方向をｙ軸にとり、ｘ軸，ｙ軸にそれぞれ直交する方向をｚ軸にとる。

船体座標系は（Ｃ）に示すように、基準アンテナＡ０の位置を原点として、船首方向をｘ軸にとり、船体に平行な平面上でｘ軸に直交する方向をｙ軸にとり、ｘ軸，ｙ軸にそれぞれ直交する方向をｚ軸にとる。

図３は装置のブロック図である。ここでＧＰＳアンテナ１０は既に示した３つのアンテナＡ０，Ａ１，Ａ２からなる。ＧＰＳ受信機２０はアンテナＡ０，Ａ１，Ａ２からの信号を入力して、それぞれのアンテナ位置、キャリア位相、および衛星位置情報を演算し、方位・姿勢角演算処理部３０へ出力する。

方位・姿勢角演算処理部３０において、演算処理フラグ３１は、この装置の利用者の入力操作によって切り替えられる状態を記憶するものである。すなわちアンテナＡ０，Ａ１，Ａ２の設置後、最初に一度だけ行う処理として、後述するようにアンテナの配置関係設定処理を行い、それ以降は基線ベクトル演算を行う。この演算処理フラグ３１はアンテナの配置関係演算を行う状態（初期モード）であるか、基線ベクトル演算を行う状態（通常モード）であるかを表すものである。

位相差演算部３２は、ＧＰＳ受信機２０によって求められた各アンテナＡ０，Ａ１，Ａ２位置での各衛星についての受信信号の基準アンテナＡ０に対するＡ１，Ａ２でのキャリア位相の位相差を求める。衛星計画部３３は、ＧＰＳ受信機２０が捕捉した複数の衛星のうちどの衛星を用いて基線ベクトル演算を行うかを決定する。

配置関係演算部３４は、上記演算処理フラグ３１が上記初期モード（アンテナの配置関係演算を行うべき状態）である時、船体に対するアンテナの配置関係を求め、それを配置関係保存部３５に保存する。

上記配置関係演算部３４と配置関係保存部３５とがこの発明に係るアンテナ配置関係設定手段に相当する。アンテナの配置関係としては、次の３つの情報を保存する。

（ａ）基準アンテナと他の２つのアンテナ間のそれぞれの基線長
（ｂ）３つアンテナのうち基準アンテナと他の１つのアンテナとの成す方位と物体の方位とのずれであるアンテナ方位オフセット
（ｃ）３つのアンテナが成す面の姿勢と物体の姿勢とのずれであるアンテナの姿勢オフセット
後述するように、配置関係演算部３４によって基準アンテナに対する他のアンテナの配置関係を求め、配置関係保存部３５によって、基準アンテナに対する他のアンテナの配置関係を一旦保存した後は、配置関係演算部の役割は終了するので、配置関係演算部３４はその他の装置とは別の装置として構成しておく。すなわち、配置関係演算部３４以外は所謂サテライトコンパスとして構成しておき、そのサテライトコンパスが用いるアンテナＡ０，Ａ１，Ａ２を適宜配置して、配置関係演算部３４が基準アンテナＡ０に対する他のアンテナＡ１，Ａ２の配置関係を求めた後は、サテライトコンパスから配置関係演算部３４を取り外して運用する。

基線ベクトル演算部３６は、演算処理フラグ３１が通常モード（基線ベクトル演算を行うべき状態）である時、位相差演算部３２で求められたキャリア位相の位相差と衛星情報（軌道情報と時刻）および受信点の位置を基に、基準アンテナＡ０から第２のアンテナＡ１方向への基線ベクトルと、基準アンテナＡ０から第３のアンテナＡ２方向への基線ベクトルをそれぞれ求める。

方位・姿勢角演算部３７は後述する方法により、配置関係保存部３５に保存された、船体に対するアンテナの配置関係と、基線ベクトル演算部３６で求められた基線ベクトルとに基づいて船体の方位角と姿勢角を求め、これらを出力する。

図４・図５は上記演算処理フラグが「初期モード」である時の処理手順を示すフローチャートである。３つのアンテナを船体に設置した後、図４に示すように、まずアンテナの方位オフセットを計測し、これを保存する（Ｓ１００）。これは、基準アンテナＡ０から第２のアンテナＡ１方向への方位が船首方位からどれだけずれているか、すなわち船体上の船首方向とアンテナＡ０−Ａ１間の直線とのなす方位角である。なお、図２の（Ｃ）に示した例は方位オフセットが０の例である。

続いてリアルタイムキネマティック法によりアンテナの相対測位を行い、アンテナＡ０−Ａ１間の基線長、アンテナＡ０−Ａ２間の基線長、およびアンテナの姿勢オフセットを求め、これらを保存する（Ｓ１１０）。その後、上記演算処理フラグ３１を「通常モード」にする。

上記アンテナの方位オフセットを求める他の方法は次のとおりである。
（ａ）他の方位センサ（ジャイロコンパスや磁気センサなど）を用い、方位センサが出力する方位と２つのアンテナが成す方位との差から方位オフセットを求める。
（ｂ）２つのアンテナが成す方位と物体の方位が一致するようにアンテナを設置し、方位オフセットを０とする。もしくは、２つのアンテナが成す方位と物体の方位とのずれが分かり易い値（例えば９０°）となるようにアンテナを設置し、その値（例えば９０°）を方位オフセットとする。
（ｃ）船体の図面などを用い、２つのアンテナが成す方向と船首方向とのずれから方位のずれを計算し、これを方位オフセットとする。

図５は図４のステップＳ１１０の処理手順を示すフローチャートである。まず、ＲＴＫ測位によって、アンテナＡ０，Ａ１の組と、Ａ０，Ａ２の組について、アンテナ間でのキャリア位相の２重差を観測し、その整数バイアスをそれぞれ決定して、Ａ０−Ａ１間の基線ベクトルおよびＡ０−Ａ２間の基線ベクトルをそれぞれ算出する（Ｓ１１１）。この基線ベクトルを求めるためには、通常１５分〜１時間程度の時間を要する。

このように整数バイアスが決定できて一旦基線ベクトルが計算できれば、後は短時間周期で、変動する基線ベクトルの計算が可能となる。そこで、所定の設定回数だけ基線ベクトルの計算（Ｓ１１２）、基線長の計算（Ｓ１１３）および姿勢オフセットの計算（Ｓ１１４）を繰り返し行う。

ここで「基線長」とは、基線ベクトルの長さである。また「姿勢オフセット」とは、水平面に対するアンテナの姿勢（３つのアンテナが成す面の姿勢）である。

通常、船体の姿勢がまだ不明である状態で姿勢オフセットを算出しなければならない。そこで、船体が水平状態にあること、または船体が水平状態を中心として動揺していて、平均的には水平状態と見なせるものと仮定して上記姿勢オフセットを計算する。具体的には、船体の停泊中にアンテナの姿勢を求め、その姿勢の符号を反転したものを姿勢オフセットとする。

このようにして、予め設定した回数だけ基線ベクトル、基線長および姿勢オフセットを計算した後、基線長および姿勢オフセットの平均値を求める（Ｓ１１５→Ｓ１１６）。このように平均値を求めることによって、基線長および姿勢オフセットの算出精度を向上させる。

その後、基線長および姿勢オフセットの妥当性をチェックする（Ｓ１１７）。この妥当性のチェック（検証）は次のような事項について行う。

（１）計算によって求めた基線長が、予め規定したアンテナ間の距離範囲内にあるか否かを判定する。例えば３つのアンテナＡ０，Ａ１，Ａ２のそれぞれの間隔を０．５ｍ以上５ｍ以下の関係となるように設置することを、この装置の仕様として定めておけば、上記計算によって求めた基線長が０．５ｍ〜５ｍの範囲内であれば妥当であると見なし、この範囲外であれば基線長の演算にミスがあったものとして扱う。

（２）３つのアンテナＡ０，Ａ１，Ａ２が一直線上に配置されていないか否かを判定する。これは、３つのアンテナを一直線上に配置しないことを条件に定めたことを利用するものである。具体的には、アンテナＡ１から基線Ａ０−Ａ２への垂線の長さが予め設定した値（例えば０．５ｍ）以上であれば妥当と見なす。その他の方法として、２つの基線のなす角度が予め設定した値（例えば１０°）以上であれば妥当と見なす。

（３）３つのアンテナのうち２つのアンテナの配置関係が鉛直関係となっていないか否かを判定する。これはアンテナの配置条件を利用するものである。具体的には基線ベクトルの水平成分の長さが予め設定した値（例えば０．５ｍ）以上であれば妥当と見なす。他の方法として姿勢オフセットが予め設定した値（例えば±８５°）以内であれば妥当と見なす。

このような判定によって、基線長と姿勢オフセットの計算結果が妥当であるものと見なした場合には、その基線長と姿勢オフセットを保存する（Ｓ１１８→Ｓ１１９）。その他の場合にはエラーメッセージを表示し、ＲＴＫ法による基線ベクトルの計算をやり直す旨の表示を行う（Ｓ１２０）。

なお、アンテナの姿勢オフセットを求める他の方法は次のとおりである。
（ａ）他の姿勢センサを用いて船体の姿勢を検出し、アンテナの姿勢と船体の姿勢とのずれを求め、これをアンテナの姿勢オフセットとする。

（ｂ）船体の図面などを用い、３つのアンテナが成す面と船体が成す面とのずれから姿勢のずれを計算し、これをアンテナの姿勢オフセットとする。

図６・図７は上記演算処理フラグ３１が「通常モード」である時の処理手順を示すフローチャートである。まずＲＴＫ法によりアンテナ間の基線ベクトルを求める（Ｓ２００）。続いてアンテナ座標系からローカル座標系への座標変換オペレータＣ_a ⁿを求める（Ｓ２１０）。また、アンテナの姿勢オフセットを用いて、船体座標系からローカル座標系への座標変換オペレータＣ_b ⁿを求める（Ｓ２２０）。さらにこの変換オペレータＣ_b ⁿを用いて船体の方位角と姿勢角を算出する（Ｓ２３０）。

具体的には次の手順で船体の方位角と姿勢角を算出する。
まず、アンテナ座標系からローカル座標系への座標変換オペレータC_a ⁿを次の関係で求める。

ただし、

尚、Ｌ₁ ⁿ，Ｌ₂ ⁿは、図３の基線ベクトル演算部で算出した基線ベクトルにあたる。Ｌ₁ ^a，Ｌ₂ ^aは、図３の配置関係演算部で算出した基線長より求まる。

次に、姿勢オフセット（配置関係演算部の算出結果）を用いて、船体座標系からローカル座標系への座標変換オペレータC_b ⁿを求める。

ただし、φ_oはロール角オフセット、θ_oはピッチ角オフセットである。

尚、方位オフセットが存在する場合には、以下の式によって船体座標系からローカル座標系への座標変換オペレータC_b ⁿを求める。

ただし、φ_oはロール角オフセット、θ_oはピッチ角オフセット、ψ_oは方位角オフセットである。

方位オフセットは、既に述べたように、第１アンテナから第２アンテナへのベクトルの水平成分の方向と船首方向とを一致させていない場合に、ユーザーによって手入力される値である。この方位オフセットの設定は、例えばこの装置で求めた方位角と他の方位センサで求めた方位角との差を求め、その値を入力することで行う。または、船首尾線とアンテナＡ０−Ａ１基線の水平面投影線との成す角度を計測し、その値を入力することで行う。

最後に、座標変換オペレータC_b ⁿからロール角φ、ピッチ角θ、方位角ψをそれぞれ次の式で算出する。

図７は図６のステップＳ２００の処理内容を示すフローチャートである。まず電源投入直後などで捕捉し始めた衛星や、新たに視野内に入って捕捉し始めた衛星について、そのキャリア位相の整数バイアスがまだ決定されていなければ、その決定を行うために、複数の整数バイアス候補点を作成する（Ｓ２０１→Ｓ２０２）。この時、２重位相差を求める２つのアンテナの配置関係は既に求まっているので、その配置関係を利用する。このアンテナの配置関係は基線長および／またはアンテナの姿勢オフセット（船体が水平状態であるときのアンテナの姿勢）とする。これにより候補点の数を初めから削減でき、演算処理量が大幅に削減できる。

続いて各整数バイアス候補点の検定を行う（Ｓ２０３）。この検定の際にも、アンテナの配置関係を利用する。すなわち、基線長および／またはアンテナの姿勢が、既に求めた基線長および／またはアンテナの姿勢角に対して所定範囲内に収まる１つの候補点を最終的に整数バイアスとして決定する。

このように整数バイアスを決定した後は、その整数バイアスを含めたキャリア位相の二重位相差から、基準アンテナＡ０に対する残る２つのアンテナ間の基線ベクトルをそれぞれ求める（Ｓ２０４）。

この発明の実施形態に係る物体の方位および姿勢検出装置の船舶に適用した例を示す図である。 方位および姿勢検出に用いる３つの座標系を示す図である。 同装置の構成を示すブロック図である。 物体にアンテナを設置した後に最初に行う初期処理の手順を示すフローチャートである。 図４のステップＳ１１０の処理手順を示すフローチャートである。 初期処理終了後の通常処理の手順を示すフローチャートである。 図６のステップＳ２００の処理手順を示すフローチャートである。

符号の説明

１−船舶（物体）
Ａ０、Ａ１，Ａ２−アンテナ
Ｓ１〜Ｓｎ−ＧＰＳ衛星（測位用衛星）
１０−ＧＰＳアンテナ
２０−ＧＰＳ受信機
３０−方位・姿勢角演算処理部

Claims (9)

1. 一直線上に並ばず、且つ２つのアンテナの向く方向が非鉛直方向の関係で、物体上のそれぞれ異なる位置に配置した複数のアンテナと、
   これら複数のアンテナで複数の測位用衛星からの電波をそれぞれ受信して、前記複数のアンテナのローカル座標系における基線ベクトルを測位するアンテナ相対測位手段と、
   前記アンテナ相対測位手段により測位された前記複数のアンテナのローカル座標系における基線ベクトルに基づいて前記複数のアンテナが成す面の姿勢と前記物体の姿勢とのずれを物体座標系に対するアンテナの姿勢のオフセットとして算出し、基準とするアンテナに対する他のアンテナまでの距離である基線長を算出し、前記アンテナの姿勢のオフセット及び前記基線長をアンテナの配置関係として保存するアンテナ配置関係設定手段と、
   前記アンテナ相対測位手段により求められた、前記基準とするアンテナから他のアンテナへのローカル座標系における基線ベクトルに基づいてアンテナ座標系からローカル座標系への座標変換オペレータを求め、アンテナ座標系からローカル座標系への座標変換オペレータと前記アンテナの配置関係とに基づいて、物体座標系からローカル座標系への座標変換オペレータを求め、当該物体座標系からローカル座標系への座標変換オペレータを基に、物体のローカル座標系に対する方位角または方位角と共に姿勢角を求める座標変換手段と、
   を設けた物体の方位および姿勢検出装置。
2. 前記アンテナ配置関係設定手段は、前記アンテナの姿勢のオフセット及び前記基線長以外に、前記複数のアンテナのうち２つのアンテナが成す方位と前記物体の方位とのずれである方位のオフセットを計測し、前記アンテナの姿勢のオフセット、前記基線長及び前記方位のオフセットをアンテナの配置関係として保存する、請求項１に記載の物体の方位および姿勢検出装置。
3. 前記複数のアンテナのうち２つのアンテナが成すアンテナ座標系に対する方位と物体の物体座標系に対する方位とが一致するようにアンテナが設置されて、前記方位のオフセットが０にされた、請求項１に記載の物体の方位および姿勢検出装置。
4. 前記アンテナ配置関係設定手段は、前記複数のアンテナのうち基準とするアンテナに対する他のアンテナのローカル座標系における基線ベクトルをリアルタイムキネマティック法により求めるものである請求項１、２または３に記載の物体の方位および姿勢検出装置。
5. 前記物体の姿勢が水平状態または準水平状態にあるとき、前記複数のアンテナのうち基準とするアンテナに対する他のアンテナのローカル座標系における基線ベクトルを求めるとともに、前記複数のアンテナが成す面の姿勢を算出し、該姿勢を前記アンテナの姿勢のオフセットとするアンテナ姿勢計測手段を設けた請求項１、２または３に記載の物体の方位および姿勢検出装置。
6. 前記物体が水平状態を中心として動揺している状態で、前記複数のアンテナのうち基準とするアンテナに対する他のアンテナのローカル座標系における基線ベクトルを求めるとともに、前記複数のアンテナが成す面の姿勢を所定時間繰り返し算出し、該姿勢の平均値を前記アンテナの物体座標系に対する姿勢のオフセットとするアンテナ姿勢計測手段を設けた請求項１、２または３に記載の物体の方位および姿勢検出装置。
7. 前記アンテナ姿勢計測手段は、前記複数のアンテナのうち基準とするアンテナに対する他のアンテナのローカル座標系における基線ベクトルをリアルタイムキネマティック法により求めるものである請求項５または６に記載の物体の方位および姿勢検出装置。
8. 前記アンテナ配置関係設定手段は、前記基線長が既定範囲内であるか否かに基づいて該基線長を求めた演算結果の妥当性を検証する手段を含む、請求項４に記載の物体の方位および姿勢検出装置。
9. 前記アンテナ配置関係設定手段は、前記複数のアンテナのアンテナ座標系に対する配置関係を算出した後、前記複数のアンテナの配置関係が既定範囲内であるか否かに基づいて前記姿勢のオフセットを求めた演算結果の妥当性を検証する手段を含む、請求項４に記載の物体の方位および姿勢検出装置。

Images