JP6199679B2 - 姿勢検出装置及びこれを備えた移動体、並びに、姿勢検出方法 - Google Patents

Description

本発明は、物体に固定された複数のアンテナでＧＮＳＳ衛星（Global Navigation Satellite System）からのＧＮＳＳ信号を受信することにより前記物体の姿勢を検出する姿勢検出装置及びこれを備えた移動体、並びに、姿勢検出方法に関するものである。

船舶や飛行機などの移動体には、当該移動体の姿勢を検出するための姿勢検出装置が搭載される場合がある。この場合、姿勢検出装置は、例えば移動体に固定された複数のアンテナでＧＮＳＳ衛星からのＧＮＳＳ信号（例えば、ＧＰＳ衛星からのＧＰＳ信号）を受信し、搬送波位相をそれぞれ観測することにより、それらの搬送波位相の一重位相差又は二重位相差の整数値バイアスを算出する。

そして、姿勢検出装置は、算出された整数値バイアスと、各アンテナでＧＮＳＳ衛星から受信するＧＮＳＳ信号に基づいて算出された各アンテナの位置とに基づいて、基線ベクトル（基準アンテナに対する他のアンテナの位置ベクトル）を算出する。このようにして算出された基線ベクトルと、既知の基線ベクトルとに基づいて、移動体の姿勢が検出される。

下記特許文献１には、姿勢検出装置のような搬送波位相差を用いて測位を行う搬送波位相相対測位装置の一例として、複数のアンテナの相対的な位置関係より得られる既知情報を制約条件とすることにより、整数値バイアスのフロート解を高速かつ高精度に推定することができる技術が提案されている。

特開２００７−１０１４８４号公報

しかしながら、従来のように複数のアンテナでＧＮＳＳ衛星からＧＮＳＳ信号を受信して、それぞれのアンテナの位置を算出するような構成では、基線ベクトルの算出に時間がかかるとともに、各アンテナに固有の算出誤差が生じるため、基線ベクトルの算出精度が悪いという問題があった。

その結果、基線ベクトルに基づいて検出される移動体の姿勢についても、姿勢の検出に時間がかかるとともに、姿勢の検出精度が悪いという問題があった。このような問題は、姿勢検出装置が移動体に設けられた場合に限らず、他の物体に設けられた場合にも同様に生じる問題である。

本発明は、上記実情に鑑みてなされたものであり、姿勢の検出にかかる時間を短縮できる姿勢検出装置及びこれを備えた移動体、並びに、姿勢検出方法を提供することを目的とする。また、本発明は、姿勢の検出精度を向上することができる姿勢検出装置及びこれを備えた移動体、並びに、姿勢検出方法を提供することを目的とする。

本発明に係る姿勢検出装置は、物体に固定された複数のアンテナでＧＮＳＳ衛星からのＧＮＳＳ信号を受信することにより前記物体の姿勢を検出する姿勢検出装置であって、基準アンテナと、非基準アンテナと、基準アンテナ位置算出部と、速度ベクトル算出部と、暫定姿勢角算出部と、暫定基線ベクトル算出部と、非基準アンテナ位置算出部と、基線ベクトル算出部と、姿勢検出部とを備えている。前記基準アンテナは、前記ＧＮＳＳ信号を受信する。前記非基準アンテナは、前記基準アンテナに対しボディ座標系における所定の基線ベクトルを有する位置に設けられ、前記ＧＮＳＳ信号を受信する。前記基準アンテナ位置算出部は、前記ＧＮＳＳ信号に基づいて、前記基準アンテナの位置を算出する。前記速度ベクトル算出部は、前記ＧＮＳＳ信号に基づいて、前記基準アンテナの速度ベクトルを算出する。前記暫定姿勢角算出部は、前記速度ベクトルに基づいて、前記物体の暫定姿勢角を算出する。前記暫定基線ベクトル算出部は、前記暫定姿勢角を用いて前記ボディ座標系における所定の基線ベクトルをローカル座標系における基線ベクトルに座標変換し、暫定基線ベクトルを算出する。前記非基準アンテナ位置算出部は、前記基準アンテナの位置及び前記暫定基線ベクトルに基づいて、前記非基準アンテナの位置を算出する。前記基線ベクトル算出部は、前記基準アンテナの位置、前記非基準アンテナの位置及び前記ＧＮＳＳ信号に基づいて、ローカル座標系における基線ベクトルを算出する。前記姿勢検出部は、前記基線ベクトルに基づいて、前記物体の姿勢を検出する。

このような構成によれば、ＧＮＳＳ衛星から受信するＧＮＳＳ信号に基づいて基準アンテナの位置を算出するだけで、その基準アンテナの位置に基づいて非基準アンテナの位置を算出し、物体の姿勢を検出することができる。これにより、整数値バイアスを算出する際に推定する誤差の大きさが小さくなるため、整数値バイアスのサーチ範囲を狭く設定することができる。その結果、ＧＮＳＳ衛星から受信するＧＮＳＳ信号に基づいて複数のアンテナの位置を算出するような従来の構成と比較して、姿勢の検出にかかる時間を短縮できる。また、基準アンテナの位置を算出する際の算出誤差のみが姿勢の検出に影響を与えるため、複数のアンテナについての算出誤差が姿勢の検出に影響を与える従来の構成と比較して、姿勢の検出精度を向上することができる。

本発明の一実施形態に係る姿勢検出装置の構成例を示すブロック図である。 姿勢検出装置により姿勢を検出する際に用いられる座標系について説明するための図であり、ローカル座標系（ＮＥＤ座標系）を示している。 姿勢検出装置により姿勢を検出する際に用いられる座標系について説明するための図であり、ボディ座標系（移動体座標系）を示している。 ＧＰＳ受信機及び姿勢検出処理部の具体的構成を示す機能ブロック図である。 船舶の姿勢を検出する際の姿勢検出装置による処理の一例を示すフローチャートである。 姿勢検出装置の構成を概略的に示す機能ブロック図である。

図１は、本発明の一実施形態に係る姿勢検出装置１の構成例を示すブロック図である。本実施形態では、移動体の一例である船舶２（図示なし）に姿勢検出装置１が搭載された場合について説明する。ただし、本発明に係る姿勢検出装置１は、船舶２に限らず、飛行機などの船舶２以外の移動体に適用することもできるし、移動体以外の物体にも適用することができる。

姿勢検出装置１には、複数のアンテナ１１、ＧＰＳ（Global Positioning System）受信機１２、姿勢検出処理部１３が備えられている。複数のアンテナ１１は、それぞれ船舶２に固定されたＧＰＳアンテナからなり、ＧＮＳＳ衛星（測位用衛星）としてのＧＰＳ衛星３からの電波を複数のアンテナ１１で受信することにより、船舶２の姿勢を検出することができる。

この例では、３つのアンテナ１１が設けられているが、アンテナ１１の数は２つ以上であればよい。アンテナ１１が２つであれば、船舶２の姿勢として２つの姿勢角（例えばヨー角及びピッチ角）を検出することができ、アンテナ１１が３つ以上であれば、船舶２の姿勢として３つの姿勢角（例えばヨー角、ピッチ角及びロール角）を検出することができる。アンテナ１１が３つ以上の場合には、３つ以上のアンテナ１１が一直線上に並ばないように配置することが好ましい。また、各アンテナ１１は、ＧＰＳ衛星３ではなく、ＧＬＯＮＡＳＳ（Global Navigation Satellite System）衛星、ＧＡＬＩＬＥＯ衛星又は準天頂衛星などの他のＧＮＳＳ衛星からの電波を受信するものであってもよい。

ＧＰＳ衛星３からの電波は、コード及び航法メッセージを載せた搬送波からなる。搬送波としては、周波数が１５７５．４２ＭＨｚのＬ１帯、又は、周波数が１２２７．６ＭＨｚのＬ２帯が用いられる。各アンテナ１１で受信した電波は、各アンテナ１１に接続された各ＧＰＳ受信機１２において復調され、復調により得られたＧＮＳＳ信号としてのＧＰＳ信号に基づいて、搬送波位相、コード位相、ＧＰＳ衛星３の位置、アンテナ１１の位置及び速度などが算出される。各ＧＰＳ受信機１２により算出されたデータは、姿勢検出処理部１３に入力される。各ＧＰＳ受信機１２は、ＧＰＳ衛星３ではなく、ＧＬＯＮＡＳＳ衛星、ＧＡＬＩＬＥＯ衛星又は準天頂衛星などの他のＧＮＳＳ衛星からの電波を復調して、ＧＮＳＳ信号を得るものであってもよい。

姿勢検出処理部１３には、制御部１３１及び記憶部１３２が含まれる。制御部１３１は、ＣＰＵ（Central Processing Unit）又はＭＰＵ（Micro Processing Unit）により構成されており、各ＧＰＳ受信機１２から入力されるデータに基づいて、船舶２の姿勢を検出するための処理を行う。記憶部１３２は、例えばＲＡＭ（Random Access Memory）及びＲＯＭ（Read Only Memory）により構成されており、船舶２の姿勢の検出に必要なデータを記憶することができる。

図２Ａ及び図２Ｂは、姿勢検出装置１により姿勢を検出する際に用いられる座標系について説明するための図であり、図２Ａはローカル座標系（ＮＥＤ座標系）、図２Ｂはボディ座標系（移動体座標系）をそれぞれ示している。図２Ａ及び図２Ｂに示すように、ローカル座標系及びボディ座標系のいずれにおいても、複数のアンテナ１１に含まれる１つの基準アンテナ１１１が原点となる。

図２Ａにおいて、（ａ）は平面図、（ｂ）は側面図を示している。この図２Ａに示すように、ローカル座標系は、例えば北方向をＸ軸、東方向をＹ軸、Ｘ軸及びＹ軸に直交する鉛直方向をＺ軸とする座標系である。したがって、基準アンテナ１１１以外のアンテナ１１（非基準アンテナ１１２，１１３）は、船舶２の姿勢に応じて、図２Ａに示すようにＸ軸、Ｙ軸及びＺ軸の軸上に存在しない場合もあれば、いずれかの軸上に存在する場合もある。なお、図２Ａ（ａ）ではＺ軸を省略し、図２Ａ（ｂ）ではＸ軸及びＹ軸を省略して示している。

図２Ｂにおいて、（ａ）は平面図、（ｂ）は側面図を示している。この図２Ｂに示すように、ボディ座標系は、例えば船舶２の船首方向（飛行機であれば機首方向）をＸ軸、船舶２に平行な平面上でＸ軸に直交する方向をＹ軸、Ｘ軸及びＹ軸に直交する方向をＺ軸とする座標系である。したがって、基準アンテナ１１１以外のアンテナ１１（非基準アンテナ１１２，１１３）は、それらのアンテナ１１の位置に応じて、図２Ｂに示すようにＸ軸、Ｙ軸及びＺ軸のいずれかの軸上に存在する場合もあれば、いずれの軸上にも存在しない場合もある。なお、図２Ｂ（ａ）ではＺ軸を省略し、図２Ｂ（ｂ）ではＹ軸を省略して示している。

図３は、ＧＰＳ受信機１２及び姿勢検出処理部１３の具体的構成を示す機能ブロック図である。まず、ＧＰＳ受信機１２の具体的構成について説明する。基準アンテナに接続されたＧＰＳ受信機１２は、基準アンテナ位置算出部１２ａ、速度ベクトル算出部１２ｂ、衛星位置算出部１２ｃ及び位相算出部１２ｄとして機能する。非基準アンテナに接続されたＧＰＳ受信機１２は、位相算出部１２ｄとして機能する。ただし、非基準アンテナに接続されたＧＰＳ受信機１２も、基準アンテナに接続されたＧＰＳ受信機１２と同様に基準アンテナ位置算出部１２ａ、速度ベクトル算出部１２ｂ、衛星位置算出部１２ｃ及び位相算出部１２ｄとして機能してもよい。

基準アンテナ位置算出部１２ａは、ＧＰＳ衛星３から受信するＧＰＳ信号に基づいて、基準アンテナ１１１の位置を算出する。また、速度ベクトル算出部１２ｂは、ＧＰＳ衛星３から受信するＧＰＳ信号に基づいて、基準アンテナ１１１の速度ベクトルを算出する。基準アンテナ１１１の位置及び速度ベクトルの算出は、航法メッセージから得られる擬似距離や搬送波から得られるＡＤＲ（Accumulated Delta Range）などを用いて公知の演算により行うことができるため、詳細な説明を省略する。

基準アンテナ位置算出部１２ａは、ＧＰＳ衛星３から受信するＧＰＳ信号に基づいて衛星位置算出部１２ｃにより算出されるＧＰＳ衛星３の位置を用いて、基準アンテナ１１１の位置を算出する。各ＧＰＳ衛星３からの搬送波に載った航法メッセージには、各ＧＰＳ衛星３の軌道情報が含まれており、衛星位置算出部１２ｃは、当該軌道情報に基づいて各ＧＰＳ衛星３の位置を算出することができる。

位相算出部１２ｄは、各アンテナ１１においてＧＰＳ衛星３から受信するＧＰＳ信号に基づいて、搬送波位相及びコード位相を算出する。複数のＧＰＳ衛星３のうちｋ番衛星に対する観測量について、搬送波位相測定値をφ^（ｋ）、コード位相測定値をρ^（ｋ）、搬送波波長をλ、衛星・受信機間距離をｒ^（ｋ）、電離層遅延量をＩ^（ｋ）、対流圏遅延量をＴ^（ｋ）、搬送波周波数をｆ、受信機クロック誤差をδｔ、衛星クロック誤差をδｔ_ｓｖ、整数値バイアスをＮ^（ｋ）、その他のノイズをε_φ ^（ｋ）及びε_ρ ^（ｋ）とすると、搬送波位相測定値は、下記式（１）及び式（２）により表される。なお、式（１）により表されるφ_ｒ ^（ｋ）は、基準アンテナ１１１による搬送波位相測定値であり、式（２）により表されるφ_ｕ ^（ｋ）は、非基準アンテナ１１２，１１３による搬送波位相測定値である。

また、コード位相測定値は、下記式（３）及び式（４）により表される。なお、式（３）により表されるρ_ｒ ^（ｋ）は、基準アンテナ１１１によるコード位相測定値であり、式（４）により表されるρ_ｕ ^（ｋ）は、非基準アンテナ１１２，１１３によるコード位相測定値である。

次に、姿勢検出処理部１３の具体的構成について説明する。姿勢検出処理部１３の制御部１３１は、ＣＰＵ又はＭＰＵがプログラムを実行することにより、暫定姿勢角算出部１３１ａ、暫定基線ベクトル算出部１３１ｂ、非基準アンテナ位置算出部１３１ｃ、整数値バイアス算出部１３１ｄ、視線方向ベクトル算出部１３１ｅ、基線ベクトル算出部１３１ｆ、姿勢検出部１３１ｇ及び位相差算出部１３１ｈとして機能する。

位相差算出部１３１ｈは、各アンテナ１１においてＧＰＳ衛星３から受信するＧＰＳ信号に基づいて二重位相差を算出する。まず、位相差算出部１３１ｈは、基準アンテナ１１１による搬送波位相測定値φ_ｒ ^（ｋ）と非基準アンテナ１１２又は１１３による搬送波位相測定値φ_ｕ ^（ｋ）との差分から、下記式（５）に示すような搬送波位相測定値の一重位相差φ_ｕｒ ^（ｋ）を求める。

ここで、基準アンテナ１１１と非基準アンテナ１１２又は１１３とで、観測量を測定するためのクロックを共通で使用している場合には、受信機クロック誤差δｔ_ｒとδｔ_ｕが共通項となるため、下記式（６）により搬送波位相測定値の一重位相差φ_ｕｒ ^（ｋ）を求めることができる。

また、位相差算出部１３１ｈは、基準アンテナ１１１によるコード位相測定値ρ_ｒ ^（ｋ）と非基準アンテナ１１２又は１１３によるコード位相測定値ρ_ｕ ^（ｋ）との差分から、下記式（７）によりコード位相測定値の一重位相差ρ_ｕｒ ^（ｋ）を求める。

ここで、基準アンテナ１１１と非基準アンテナ１１２又は１１３とで、観測量を測定するためのクロックを共通で使用している場合には、受信機クロック誤差δｔ_ｒとδｔ_ｕが共通項となるため、下記式（８）によりコード位相測定値の一重位相差ρ_ｕｒ ^（ｋ）を求めることができる。

さらに、位相差算出部１３１ｈは、下記式（９）により、ｋ番衛星とｌ番衛星との間における搬送波位相測定値の二重位相差φ_ｕｒ ^{（ｋ，ｌ）}を求める。

また、位相差算出部１３１ｈは、下記式（１０）により、ｋ番衛星とｌ番衛星との間におけるコード位相測定値の二重位相差ρ_ｕｒ ^{（ｋ，ｌ）}を求める。

暫定姿勢角算出部１３１ａは、速度ベクトル算出部１２ｂにより算出された基準アンテナ１１１の速度ベクトルに基づいて、船舶２の暫定的な姿勢角（暫定姿勢角）を算出する。具体的には、速度ベクトルを（Ｖ_ｅ，Ｖ_ｎ，Ｖ_ｄ）とし、暫定姿勢角を（θ_ｙ，θ_ｐ，θ_ｒ）とした場合に、暫定姿勢角算出部１３１ａは、暫定的なヨー角θ_ｙを下記式（１１）、暫定的なピッチ角θ_ｐを下記式（１２）、暫定的なロール角θ_ｒを下記式（１３）により算出することができる。

なお、速度ベクトル算出部１２ｂにより算出される基準アンテナ１１１の速度ベクトルに基づいて船舶２の暫定姿勢角を算出する場合、速度ベクトルの一方向と船舶２の船首方向とが、一致していることが好ましい。これは、船舶２が船首方向に対して交差する方向に滑りながら進行していることにより、速度ベクトルに基づいて算出される暫定姿勢角と、実際の姿勢角との誤差が大きくなることを避けるためである。

暫定基線ベクトル算出部１３１ｂは、暫定姿勢角算出部１３１ａにより算出された船舶２の暫定姿勢角を用いて既知の基線ベクトルを変換することにより、暫定的な基線ベクトル（暫定基線ベクトル）を算出する。具体的には、ボディ座標系における既知の基線ベクトルをＢ^Ｂｏｄｙ、ローカル座標系における暫定基線ベクトルをＢ^{Ｌｏｃａｌ}とした場合に、暫定基線ベクトル算出部１３１ｂは、ローカル座標系における暫定基線ベクトルＢ^{Ｌｏｃａｌ}を下記式（１４）により算出することができる。

なお、ボディ座標系における既知の基線ベクトルＢ^Ｂｏｄｙは、船舶２に固定された各アンテナ１１間の相対的な距離及び方向に基づいて予め測定され、記憶部１３２に記憶されている。暫定基線ベクトル算出部１３１ｂは、記憶部１３２に記憶されている既知の基線ベクトルＢ^Ｂｏｄｙを読み出し、暫定基線ベクトルＢ^{Ｌｏｃａｌ}を算出する。

非基準アンテナ位置算出部１３１ｃは、基準アンテナ位置算出部１２ａにより算出された基準アンテナ１１１の位置、及び、暫定基線ベクトル算出部１３１ｂにより算出された暫定基線ベクトルＢ^{Ｌｏｃａｌ}に基づいて、非基準アンテナ１１２，１１３の位置を算出する。具体的には、ローカル座標系における基準アンテナ１１１の位置をＰＯＳ_ｒｅｆ、ローカル座標系における非基準アンテナ１１２，１１３の位置をＰＯＳ_ｕｓｅｒとした場合に、非基準アンテナ位置算出部１３１ｃは、非基準アンテナ１１２，１１３の位置ＰＯＳ_ｕｓｅｒを下記式（１５）により算出することができる。

整数値バイアス算出部１３１ｄは、位相差算出部１３１ｈにより算出された二重位相差の整数値バイアスを算出する。ここで、上述の式（９）及び式（１０）によれば、二重位相差の整数値バイアスＮ_ｕｒ ^{（ｋ，ｌ）}を下記式（１６）で表すことができる。整数値バイアス算出部１３１ｄは、下記式（１６）においてノイズε_ｕｒ ^{（ｋ，ｌ）}が取り得る値を見積り、その範囲内で整数値バイアスＮ_ｕｒ ^{（ｋ，ｌ）}の正解値をサーチする。したがって、ノイズε_ｕｒ ^{（ｋ，ｌ）}が小さいほど、整数値バイアスＮ_ｕｒ ^{（ｋ，ｌ）}の算出に要する時間は短くなる。

ただし、上記のように二重位相差の整数値バイアスを算出するような構成に限らず、位相差算出部１３１ｈで一重位相差を算出し、整数値バイアス算出部１３１ｄで一重位相差の整数値バイアスを算出するような構成であってもよい。この場合、上述の式（６）及び式（８）に基づいて、一重位相差の整数値バイアスＮ_ｕｒ ^（ｋ）を下記式（１７）で表すことができる。

視線方向ベクトル算出部１３１ｅは、衛星位置算出部１２ｃにより算出された各ＧＰＳ衛星３の位置、基準アンテナ位置算出部１２ａにより算出された基準アンテナ１１１の位置、及び、非基準アンテナ位置算出部１３１ｃにより算出された非基準アンテナ１１２，１１３の位置に基づいて、各アンテナ１１から各ＧＰＳ衛星３への視線方向ベクトルを算出する。すなわち、視線方向ベクトル算出部１３１ｅは、各ＧＰＳ衛星３の位置と基準アンテナ１１１の位置とに基づいて、基準アンテナ１１１から各ＧＰＳ衛星３への視線方向ベクトルを算出することができるとともに、各ＧＰＳ衛星３の位置と非基準アンテナ１１２，１１３の位置とに基づいて、非基準アンテナ１１２，１１３から各ＧＰＳ衛星３への視線方向ベクトルを算出することができる。

基線ベクトル算出部１３１ｆは、視線方向ベクトル算出部１３１ｅにより算出された視線方向ベクトル、及び、整数値バイアス算出部１３１ｄにより算出された整数値バイアスに基づいて、ローカル座標系における基線ベクトルを算出する。ここで、ノイズが十分に小さい場合には、整数値バイアスＮ_ｕｒ ^{（ｋ，ｌ）}を算出すると、下記式（１８）により衛星・受信機間距離ｒ_ｕｒ ^{（ｋ，ｌ）}の二重差を求めることができる。

このとき、衛星・受信機間距離の二重差をＲ_ｄ、視線方向ベクトルの二重差をＨ_ｄ、基線ベクトルをＢ_ｄとすると、下記式（１９）が成り立つため、基線ベクトル算出部１３１ｆは、下記式（２０）により基線ベクトルＢ_ｄを算出することができる。なお、一重位相差の整数値バイアスＮ_ｕｒ ^（ｋ）を用いて基線ベクトルを算出する場合にも、同様の関係が成り立つ。

このように、基準アンテナ１１１の位置に基づいて算出された非基準アンテナ１１２，１１３の位置を用いて視線方向ベクトルが算出されるため、上記式（２０）における視線方向ベクトルの二重差Ｈ_ｄの算出に際しては、基準アンテナ１１１の位置を算出する際の算出誤差しか重畳されない。したがって、基準アンテナ１１１及び非基準アンテナ１１２，１１３の双方に起因する誤差が重畳される従来技術と比較して、基線ベクトルＢ_ｄの算出精度が向上する。

姿勢検出部１３１ｇは、基線ベクトル算出部１３１ｆにより算出されたローカル座標系における基線ベクトルに基づいて、船舶２の姿勢を検出する。具体的には、アンテナ１１が３つ以上の場合であって、基線ベクトル算出部１３１ｆにより２つの基線ベクトルを算出することができる場合には、第１基線ベクトルを（Ｂ１_ｅ，Ｂ１_ｎ，Ｂ１_ｄ）、第２基線ベクトルを（Ｂ２_ｅ，Ｂ２_ｎ，Ｂ２_ｄ）、姿勢角を（θ_ｙ，θ_ｐ，θ_ｒ）とすると、下記式（２１）〜式（２３）により、ヨー角θ_ｙ、ピッチ角θ_ｐ及びロール角θ_ｒを算出することができる。

また、アンテナ１１が２つの場合であって、基線ベクトル算出部１３１ｆにより１つの基線ベクトルのみ算出できる場合には、下記式（２４）及び式（２５）により、ヨー角θ_ｙ及びピッチ角θ_ｐを算出することができる。

ただし、上記式（２１）〜式（２５）は、船舶２の水平な平面上に全てのアンテナ１１が設置されている場合の計算式であり、例えば全てのアンテナ１１が船舶２の水平な平面上にない場合には、当該平面と基線ベクトルとの角度を測定して、上記式（２１）〜式（２５）による計算結果を補正する必要がある。

なお、本実施形態では、基準アンテナ１１１が設置されている位置（点）の速度ベクトルを用いて、上記式（１１）〜式（１３）により船舶２の暫定姿勢角を算出するため、各アンテナ１１を座標軸上に配置する必要はない。すなわち、各アンテナ１１が座標軸上に配置されていなくても、基線ベクトルと座標軸とのずれが、暫定姿勢角の算出に影響を与えることはない。ただし、上記式（２１）〜式（２５）により船舶２の実際の姿勢角を算出する際には、基線ベクトルと座標軸とのずれを予め測定し、その測定結果を考慮して姿勢角を算出する必要がある。

以上のように、本実施形態では、基準アンテナ１１１の速度ベクトルに基づいて船舶２の暫定姿勢角を算出し、当該暫定姿勢角を用いて既知の基線ベクトルを変換することにより算出される暫定基線ベクトルに基づいて、非基準アンテナ１１２，１１３の位置が算出される。そして、算出された非基準アンテナ１１２，１１３の位置と、ＧＰＳ衛星３から受信するＧＰＳ信号に基づいて算出される基準アンテナ１１１の位置とを用いて、ローカル座標系における基線ベクトルが算出され、当該基線ベクトルに基づいて船舶２の姿勢が検出される。

したがって、ＧＰＳ衛星３から受信するＧＰＳ信号に基づいて基準アンテナ１１１の位置を算出するだけで、その基準アンテナ１１１の位置に基づいて非基準アンテナ１１２，１１３の位置を算出し、船舶２の姿勢を検出することができる。これにより、整数値バイアスを算出する際に推定する誤差の大きさが小さくなるため、整数値バイアスのサーチ範囲を狭く設定することができる。その結果、ＧＰＳ衛星３から受信するＧＰＳ信号に基づいて複数のアンテナ１１の位置を算出するような従来の構成と比較して、姿勢の検出にかかる時間を短縮できる。

また、上述の通り、視線方向ベクトルの二重差Ｈ_ｄの算出に際しては、基準アンテナ１１１の位置を算出する際の算出誤差しか重畳されないため、基線ベクトルＢ_ｄの算出精度が向上する。すなわち、基準アンテナ１１１の位置を算出する際の算出誤差のみが姿勢の検出に影響を与えるため、複数のアンテナ１１についての算出誤差が姿勢の検出に影響を与える従来の構成と比較して、姿勢の検出精度を向上することができる。

図４は、船舶２の姿勢を検出する際の姿勢検出装置１による処理の一例を示すフローチャートである。船舶２の姿勢を検出する際には、まず、基準アンテナ位置算出部１２ａが、ＧＰＳ衛星３から受信するＧＰＳ信号に基づいて、基準アンテナ１１１の位置を算出する（ステップＳ１０１：基準アンテナ位置算出ステップ）。また、速度ベクトル算出部１２ｂにより、基準アンテナ１１１の速度ベクトルが算出される（ステップＳ１０２：速度ベクトル算出ステップ）。

その後、暫定姿勢角算出部１３１ａが、速度ベクトル算出部１２ｂにより算出された基準アンテナ１１１の速度ベクトルに基づいて、上記式（１１）〜式（１３）により船舶２の暫定姿勢角を算出する（ステップＳ１０３：暫定姿勢角算出ステップ）。そして、算出された船舶２の暫定姿勢角を用いて、暫定基線ベクトル算出部１３１ｂが、上記式（１４）により既知の基線ベクトルＢ^Ｂｏｄｙを変換することで、暫定基線ベクトルＢ^{Ｌｏｃａｌ}が算出される（ステップＳ１０４：暫定基線ベクトル算出ステップ）。

このようにして算出された暫定基線ベクトルＢ^{Ｌｏｃａｌ}及び基準アンテナ１１１の位置ＰＯＳ_ｒｅｆに基づいて、非基準アンテナ位置算出部１３１ｃは、上記式（１５）により非基準アンテナ１１２，１１３の位置ＰＯＳ_ｕｓｅｒを算出する（ステップＳ１０５：非基準アンテナ位置算出ステップ）。

一方で、位相差算出部１３１ｈは、各アンテナ１１においてＧＰＳ衛星３から受信するＧＰＳ信号に基づいて、一重位相差又は二重位相差を算出する（ステップＳ１０６：位相差算出ステップ）。このとき、位相差算出部１３１ｈは、上記式（１）〜式（８）により一重位相差を算出し、又は、上記式（１）〜式（１０）により二重位相差を算出する。

そして、整数値バイアス算出部１３１ｄが一重位相差又は二重位相差の整数値バイアスを算出する（ステップＳ１０７：整数値バイアス算出ステップ）。二重位相差の整数値バイアスＮ_ｕｒ ^{（ｋ，ｌ）}は、上記式（１６）により算出され、一重位相差の整数値バイアスＮ_ｕｒ ^（ｋ）は、上記式（１７）により算出される。また、ＧＰＳ衛星３の位置、基準アンテナ１１１の位置ＰＯＳ_ｒｅｆ、及び、非基準アンテナ１１２，１１３の位置ＰＯＳ_ｕｓｅｒに基づいて、視線方向ベクトル算出部１３１ｅにより、各アンテナ１１から各ＧＰＳ衛星３への視線方向ベクトルが算出される（ステップＳ１０８：視線方向ベクトル算出ステップ）。

その後、算出された視線方向ベクトル、及び、算出された整数値バイアスに基づいて、上記式（２０）により、基線ベクトル算出部１３１ｆがローカル座標系における基線ベクトルＢ_ｄを算出する（ステップＳ１０９：基線ベクトル算出ステップ）。そして、算出された基線ベクトルＢ_ｄに基づいて、姿勢検出部１３１ｇが上記式（２１）〜式（２３）、又は、上記式（２４）及び式（２５）を用いて演算を行うことにより、船舶２の姿勢が検出される（ステップＳ１１０：姿勢検出ステップ）。

本実施形態において、船舶２の姿勢は、図４のステップＳ１０７で整数値バイアスが初期決定される場合だけでなく、整数値バイアスが再決定される場合にも検出できる。したがって、いわゆるサイクルスリップが発生した場合などであっても、再決定された整数値バイアスに基づいて、船舶２の姿勢を短時間で検出することができるため、従来よりも高いアベイラビリティを確保することができる。

以上の実施形態では、視線方向ベクトル算出部１３１ｅにより算出された視線方向ベクトル、及び、整数値バイアス算出部１３１ｄにより算出された整数値バイアスに基づいて、ローカル座標系における基線ベクトルＢ_ｄが算出されるような構成について説明した。しかし、このような構成に限らず、基線ベクトル算出部１３１ｆは、基準アンテナ位置算出部１２ａにより算出された基準アンテナ１１１の位置、非基準アンテナ位置算出部１３１ｃにより算出された非基準アンテナ１１２，１１３の位置、及び、整数値バイアス算出部１３１ｄにより算出された整数値バイアスに基づいて、他の態様によりローカル座標系における基線ベクトルを算出するような構成であってもよい。

また、姿勢検出装置１の具体的構成は、上記実施形態のような構成に限られるものではない。例えば、ＧＰＳ受信機１２の機能の少なくとも一部が姿勢検出処理部１３に備えられていてもよいし、姿勢検出処理部１３の機能の少なくとも一部がＧＰＳ受信機１２に備えられていてもよい。すなわち、ＧＰＳ受信機１２の機能又は姿勢検出処理部１３の機能として、少なくとも基準アンテナ位置算出部１２ａ、速度ベクトル算出部１２ｂ、暫定姿勢角算出部１３１ａ、暫定基線ベクトル算出部１３１ｂ、非基準アンテナ位置算出部１３１ｃ、基線ベクトル算出部１３１ｆ及び姿勢検出部１３１ｇが姿勢検出装置１に備えられていればよい。

図５は、姿勢検出装置１の構成を概略的に示す機能ブロック図である。この図５に示すように、本発明に係る姿勢検出装置１は、基準アンテナ位置算出部１２ａと、速度ベクトル算出部１２ｂと、暫定姿勢角算出部１３１ａと、暫定基線ベクトル算出部１３１ｂと、非基準アンテナ位置算出部１３１ｃと、基線ベクトル算出部１３１ｆと、姿勢検出部１３１ｇとを備えている。

基準アンテナ位置算出部１２ａは、基準アンテナ１１１により受信したＧＮＳＳ信号に基づいて、基準アンテナ１１１の位置を算出する。速度ベクトル算出部１２ｂは、前記ＧＮＳＳ信号に基づいて、基準アンテナ１１１の速度ベクトルを算出する。暫定姿勢角算出部１３１ａは、前記速度ベクトルに基づいて、物体の暫定姿勢角を算出する。暫定基線ベクトル算出部１３１ｂは、前記暫定姿勢角を用いてボディ座標系における所定の基線ベクトルをローカル座標系における基線ベクトルに座標変換し、暫定基線ベクトルを算出する。非基準アンテナ位置算出部１３１ｃは、前記基準アンテナ１１１の位置及び前記暫定基線ベクトルに基づいて、非基準アンテナ１１２又は１１３の位置を算出する。基線ベクトル算出部１３１ｆは、前記基準アンテナ１１１の位置、前記非基準アンテナ１１２又は１１３の位置、基準アンテナ１１１により受信したＧＮＳＳ信号、及び基準アンテナ１１２又は１１３により受信したＧＮＳＳ信号に基づいて、ローカル座標系における基線ベクトルを算出する。姿勢検出部１３１ｇは、前記基線ベクトルに基づいて、前記物体の姿勢を検出する。

１ 姿勢検出装置
２ 船舶
３ ＧＰＳ衛星
１１ アンテナ
１２ ＧＰＳ受信機
１２ａ 基準アンテナ位置算出部
１２ｂ 速度ベクトル算出部
１２ｃ 衛星位置算出部
１２ｄ 位相算出部
１３ 姿勢検出処理部
１１１ 基準アンテナ
１１２ 非基準アンテナ
１１３ 非基準アンテナ
１３１ 制御部
１３１ａ 暫定姿勢角算出部
１３１ｂ 暫定基線ベクトル算出部
１３１ｃ 非基準アンテナ位置算出部
１３１ｄ 整数値バイアス算出部
１３１ｅ 視線方向ベクトル算出部
１３１ｆ 基線ベクトル算出部
１３１ｇ 姿勢検出部
１３１ｈ 位相差算出部
１３２ 記憶部

Claims (5)

1. 物体に固定された複数のアンテナでＧＮＳＳ衛星からのＧＮＳＳ信号を受信することにより前記物体の姿勢を検出する姿勢検出装置であって、
   前記ＧＮＳＳ信号を受信する基準アンテナと、
   前記基準アンテナに対しボディ座標系における所定の基線ベクトルを有する位置に設けられ、前記ＧＮＳＳ信号を受信する非基準アンテナと、
   前記ＧＮＳＳ信号に基づいて、前記基準アンテナの位置を算出する基準アンテナ位置算出部と、
   前記ＧＮＳＳ信号に基づいて、前記基準アンテナの速度ベクトルを算出する速度ベクトル算出部と、
   前記速度ベクトルに基づいて、前記物体の暫定姿勢角を算出する暫定姿勢角算出部と、
   前記暫定姿勢角を用いて前記ボディ座標系における所定の基線ベクトルをローカル座標系における基線ベクトルに座標変換し、暫定基線ベクトルを算出する暫定基線ベクトル算出部と、
   前記基準アンテナの位置及び前記暫定基線ベクトルに基づいて、前記非基準アンテナの位置を算出する非基準アンテナ位置算出部と、
   前記基準アンテナの位置、前記非基準アンテナの位置及び前記ＧＮＳＳ信号に基づいて、ローカル座標系における基線ベクトルを算出する基線ベクトル算出部と、
   前記基線ベクトルに基づいて、前記物体の姿勢を検出する姿勢検出部とを備えたことを特徴とする姿勢検出装置。
2. 前記基準アンテナ及び前記非基準アンテナがそれぞれ受信したＧＮＳＳ信号に基づいて、搬送波位相の位相差を算出する位相差算出部と、
   前記位相差の整数値バイアスを算出する整数値バイアス算出部とをさらに備え、
   前記基線ベクトル算出部は、前記基準アンテナの位置、前記非基準アンテナの位置及び前記整数値バイアスに基づいて、ローカル座標系における基線ベクトルを算出することを特徴とする請求項１に記載の姿勢検出装置。
3. 前記ＧＮＳＳ信号に基づいて、当該ＧＮＳＳ信号を送信したＧＮＳＳ衛星の位置を算出する衛星位置算出部と、
   前記ＧＮＳＳ衛星の位置、前記基準アンテナの位置及び前記非基準アンテナの位置に基づいて、前記基準アンテナ又は前記非基準アンテナから前記ＧＮＳＳ衛星への視線方向ベクトルをそれぞれ算出する視線方向ベクトル算出部とをさらに備え、
   前記基線ベクトル算出部は、前記視線方向ベクトル、前記非基準アンテナの位置及び前記整数値バイアスに基づいて、ローカル座標系における基線ベクトルを算出することを特徴とする請求項２に記載の姿勢検出装置。
4. 複数のアンテナが固定され、前記複数のアンテナでＧＮＳＳ衛星からのＧＮＳＳ信号を受信することができる移動体であって、
   前記ＧＮＳＳ信号を受信する基準アンテナと、
   前記基準アンテナに対しボディ座標系における所定の基線ベクトルを有する位置に設けられ、前記ＧＮＳＳ信号を受信する非基準アンテナと、
   前記ＧＮＳＳ信号に基づいて、前記基準アンテナの位置を算出する基準アンテナ位置算出部と、
   前記ＧＮＳＳ信号に基づいて、前記基準アンテナの速度ベクトルを算出する速度ベクトル算出部と、
   前記速度ベクトルに基づいて、前記移動体の暫定姿勢角を算出する暫定姿勢角算出部と、
   前記暫定姿勢角を用いて前記ボディ座標系における所定の基線ベクトルをローカル座標系における基線ベクトルに座標変換し、暫定基線ベクトルを算出する暫定基線ベクトル算出部と、
   前記基準アンテナの位置及び前記暫定基線ベクトルに基づいて、前記非基準アンテナの位置を算出する非基準アンテナ位置算出部と、
   前記基準アンテナの位置、前記非基準アンテナの位置及び前記ＧＮＳＳ信号に基づいて、ローカル座標系における基線ベクトルを算出する基線ベクトル算出部と、
   前記基線ベクトルに基づいて、前記移動体の姿勢を検出する姿勢検出部とを備えたことを特徴とする移動体。
5. 物体に固定された基準アンテナ、及び、前記基準アンテナに対しボディ座標系における所定の基線ベクトルを有する位置に設けられた非基準アンテナで、ＧＮＳＳ衛星からのＧＮＳＳ信号を受信することにより前記物体の姿勢を検出する姿勢検出方法であって、
   前記基準アンテナで受信した前記ＧＮＳＳ信号に基づいて、前記基準アンテナの位置を算出する基準アンテナ位置算出ステップと、
   前記ＧＮＳＳ信号に基づいて、前記基準アンテナの速度ベクトルを算出する速度ベクトル算出ステップと、
   前記速度ベクトルに基づいて、前記物体の暫定姿勢角を算出する暫定姿勢角算出ステップと、
   前記暫定姿勢角を用いて前記ボディ座標系における所定の基線ベクトルをローカル座標系における基線ベクトルに座標変換し、暫定基線ベクトルを算出する暫定基線ベクトル算出ステップと、
   前記基準アンテナの位置及び前記暫定基線ベクトルに基づいて、前記非基準アンテナの位置を算出する非基準アンテナ位置算出ステップと、
   前記基準アンテナの位置、前記非基準アンテナの位置及び前記ＧＮＳＳ信号に基づいて、ローカル座標系における基線ベクトルを算出する基線ベクトル算出ステップと、
   前記基線ベクトルに基づいて、前記物体の姿勢を検出する姿勢検出ステップとを含むことを特徴とする姿勢検出方法。

Images