(57)【要約】【課題】姿勢の検出にかかる時間を短縮できる姿勢検出装置及びこれを備えた移動体、並びに、姿勢検出方法を提供する。【解決手段】暫定姿勢角算出部１３１ａが、基準アンテナの速度ベクトルに基づいて物体の暫定姿勢角を算出する。当該暫定姿勢角を用いて、暫定基線ベクトル算出部１３１ｂがボディ座標系における所定の基線ベクトルをローカル座標系における基線ベクトルに座標変換し、暫定基線ベクトルを算出する。当該暫定基線ベクトルを用いて、非基準アンテナ位置算出部１３１ｃが非基準アンテナの位置を算出する。算出された非基準アンテナの位置と、ＧＮＳＳ衛星から受信するＧＮＳＳ信号に基づいて算出される基準アンテナの位置とを用いて、基線ベクトル算出部１３１ｆがローカル座標系における基線ベクトルを算出し、当該基線ベクトルに基づいて、姿勢検出部１３１ｇが物体の姿勢を検出する。【選択図】  図５
2015068714.tif 000002
【特許請求の範囲】
【請求項１】
物体に固定された複数のアンテナでＧＮＳＳ衛星からのＧＮＳＳ信号を受信することにより前記物体の姿勢を検出する姿勢検出装置であって、
前記ＧＮＳＳ信号を受信する基準アンテナと、
前記基準アンテナに対しボディ座標系における所定の基線ベクトルを有する位置に設けられ、前記ＧＮＳＳ信号を受信する非基準アンテナと、
前記ＧＮＳＳ信号に基づいて、前記基準アンテナの位置を算出する基準アンテナ位置算出部と、
前記ＧＮＳＳ信号に基づいて、前記基準アンテナの速度ベクトルを算出する速度ベクトル算出部と、
前記速度ベクトルに基づいて、前記物体の暫定姿勢角を算出する暫定姿勢角算出部と、
前記暫定姿勢角を用いて前記ボディ座標系における所定の基線ベクトルをローカル座標系における基線ベクトルに座標変換し、暫定基線ベクトルを算出する暫定基線ベクトル算出部と、
前記基準アンテナの位置及び前記暫定基線ベクトルに基づいて、前記非基準アンテナの位置を算出する非基準アンテナ位置算出部と、
前記基準アンテナの位置、前記非基準アンテナの位置及び前記ＧＮＳＳ信号に基づいて、ローカル座標系における基線ベクトルを算出する基線ベクトル算出部と、
前記基線ベクトルに基づいて、前記物体の姿勢を検出する姿勢検出部とを備えたことを特徴とする姿勢検出装置。
【請求項２】
前記基準アンテナ及び前記非基準アンテナがそれぞれ受信したＧＮＳＳ信号に基づいて、搬送波位相の位相差を算出する位相差算出部と、
前記位相差の整数値バイアスを算出する整数値バイアス算出部とをさらに備え、
前記基線ベクトル算出部は、前記基準アンテナの位置、前記非基準アンテナの位置及び前記整数値バイアスに基づいて、ローカル座標系における基線ベクトルを算出することを特徴とする請求項１に記載の姿勢検出装置。
【請求項３】
前記ＧＮＳＳ信号に基づいて、当該ＧＮＳＳ信号を送信したＧＮＳＳ衛星の位置を算出する衛星位置算出部と、
前記ＧＮＳＳ衛星の位置、前記基準アンテナの位置及び前記非基準アンテナの位置に基づいて、前記基準アンテナ又は前記非基準アンテナから前記ＧＮＳＳ衛星への視線方向ベクトルをそれぞれ算出する視線方向ベクトル算出部とをさらに備え、
前記基線ベクトル算出部は、前記視線方向ベクトル、前記非基準アンテナの位置及び前記整数値バイアスに基づいて、ローカル座標系における基線ベクトルを算出することを特徴とする請求項２に記載の姿勢検出装置。
【請求項４】
複数のアンテナが固定され、前記複数のアンテナでＧＮＳＳ衛星からのＧＮＳＳ信号を受信することができる移動体であって、
前記ＧＮＳＳ信号を受信する基準アンテナと、
前記基準アンテナに対しボディ座標系における所定の基線ベクトルを有する位置に設けられ、前記ＧＮＳＳ信号を受信する非基準アンテナと、
前記ＧＮＳＳ信号に基づいて、前記基準アンテナの位置を算出する基準アンテナ位置算出部と、
前記ＧＮＳＳ信号に基づいて、前記基準アンテナの速度ベクトルを算出する速度ベクトル算出部と、
前記速度ベクトルに基づいて、前記移動体の暫定姿勢角を算出する暫定姿勢角算出部と、
前記暫定姿勢角を用いて前記ボディ座標系における所定の基線ベクトルをローカル座標系における基線ベクトルに座標変換し、暫定基線ベクトルを算出する暫定基線ベクトル算出部と、
前記基準アンテナの位置及び前記暫定基線ベクトルに基づいて、前記非基準アンテナの位置を算出する非基準アンテナ位置算出部と、
前記基準アンテナの位置、前記非基準アンテナの位置及び前記ＧＮＳＳ信号に基づいて、ローカル座標系における基線ベクトルを算出する基線ベクトル算出部と、
前記基線ベクトルに基づいて、前記移動体の姿勢を検出する姿勢検出部とを備えたことを特徴とする移動体。
【請求項５】
物体に固定された基準アンテナ、及び、前記基準アンテナに対しボディ座標系における所定の基線ベクトルを有する位置に設けられた非基準アンテナで、ＧＮＳＳ衛星からのＧＮＳＳ信号を受信することにより前記物体の姿勢を検出する姿勢検出方法であって、
前記基準アンテナで受信した前記ＧＮＳＳ信号に基づいて、前記基準アンテナの位置を算出する基準アンテナ位置算出ステップと、
前記ＧＮＳＳ信号に基づいて、前記基準アンテナの速度ベクトルを算出する速度ベクトル算出ステップと、
前記速度ベクトルに基づいて、前記物体の暫定姿勢角を算出する暫定姿勢角算出ステップと、
前記暫定姿勢角を用いて前記ボディ座標系における所定の基線ベクトルをローカル座標系における基線ベクトルに座標変換し、暫定基線ベクトルを算出する暫定基線ベクトル算出ステップと、
前記基準アンテナの位置及び前記暫定基線ベクトルに基づいて、前記非基準アンテナの位置を算出する非基準アンテナ位置算出ステップと、
前記基準アンテナの位置、前記非基準アンテナの位置及び前記ＧＮＳＳ信号に基づいて、ローカル座標系における基線ベクトルを算出する基線ベクトル算出ステップと、
前記基線ベクトルに基づいて、前記物体の姿勢を検出する姿勢検出ステップとを含むことを特徴とする姿勢検出方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、物体に固定された複数のアンテナでＧＮＳＳ衛星（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）などを用いて公知の演算により行うことができるため、詳細な説明を省略する。
【００２２】
基準アンテナ位置算出部１２ａは、ＧＰＳ衛星３から受信するＧＰＳ信号に基づいて衛星位置算出部１２ｃにより算出されるＧＰＳ衛星３の位置を用いて、基準アンテナ１１１の位置を算出する。各ＧＰＳ衛星３からの搬送波に載った航法メッセージには、各ＧＰＳ衛星３の軌道情報が含まれており、衛星位置算出部１２ｃは、当該軌道情報に基づいて各ＧＰＳ衛星３の位置を算出することができる。
【００２３】
位相算出部１２ｄは、各アンテナ１１においてＧＰＳ衛星３から受信するＧＰＳ信号に基づいて、搬送波位相及びコード位相を算出する。複数のＧＰＳ衛星３のうちｋ番衛星に対する観測量について、搬送波位相測定値をφ（ｋ）、コード位相測定値をρ（ｋ）、搬送波波長をλ、衛星・受信機間距離をｒ（ｋ）、電離層遅延量をＩ（ｋ）、対流圏遅延量をＴ（ｋ）、搬送波周波数をｆ、受信機クロック誤差をδｔ、衛星クロック誤差をδｔｓｖ、整数値バイアスをＮ（ｋ）、その他のノイズをεφ（ｋ）及びερ（ｋ）とすると、搬送波位相測定値は、下記式（１）及び式（２）により表される。なお、式（１）により表されるφｒ（ｋ）は、基準アンテナ１１１による搬送波位相測定値であり、式（２）により表されるφｕ（ｋ）は、非基準アンテナ１１２，１１３による搬送波位相測定値である。
【数１】
2015068714.tif 000003
【００２４】
また、コード位相測定値は、下記式（３）及び式（４）により表される。なお、式（３）により表されるρｒ（ｋ）は、基準アンテナ１１１によるコード位相測定値であり、式（４）により表されるρｕ（ｋ）は、非基準アンテナ１１２，１１３によるコード位相測定値である。
【数２】
2015068714.tif 000004
【００２５】
次に、姿勢検出処理部１３の具体的構成について説明する。姿勢検出処理部１３の制御部１３１は、ＣＰＵ又はＭＰＵがプログラムを実行することにより、暫定姿勢角算出部１３１ａ、暫定基線ベクトル算出部１３１ｂ、非基準アンテナ位置算出部１３１ｃ、整数値バイアス算出部１３１ｄ、視線方向ベクトル算出部１３１ｅ、基線ベクトル算出部１３１ｆ、姿勢検出部１３１ｇ及び位相差算出部１３１ｈとして機能する。
【００２６】
位相差算出部１３１ｈは、各アンテナ１１においてＧＰＳ衛星３から受信するＧＰＳ信号に基づいて二重位相差を算出する。まず、位相差算出部１３１ｈは、基準アンテナ１１１による搬送波位相測定値φｒ（ｋ）と非基準アンテナ１１２又は１１３による搬送波位相測定値φｕ（ｋ）との差分から、下記式（５）に示すような搬送波位相測定値の一重位相差φｕｒ（ｋ）を求める。
【数３】
2015068714.tif 000005
【００２７】
ここで、基準アンテナ１１１と非基準アンテナ１１２又は１１３とで、観測量を測定するためのクロックを共通で使用している場合には、受信機クロック誤差δｔｒとδｔｕが共通項となるため、下記式（６）により搬送波位相測定値の一重位相差φｕｒ（ｋ）を求めることができる。
【数４】
2015068714.tif 000006
【００２８】
また、位相差算出部１３１ｈは、基準アンテナ１１１によるコード位相測定値ρｒ（ｋ）と非基準アンテナ１１２又は１１３によるコード位相測定値ρｕ（ｋ）との差分から、下記式（７）によりコード位相測定値の一重位相差ρｕｒ（ｋ）を求める。
【数５】
2015068714.tif 000007
【００２９】
ここで、基準アンテナ１１１と非基準アンテナ１１２又は１１３とで、観測量を測定するためのクロックを共通で使用している場合には、受信機クロック誤差δｔｒとδｔｕが共通項となるため、下記式（８）によりコード位相測定値の一重位相差ρｕｒ（ｋ）を求めることができる。
【数６】
2015068714.tif 000008
【００３０】
さらに、位相差算出部１３１ｈは、下記式（９）により、ｋ番衛星とｌ番衛星との間における搬送波位相測定値の二重位相差φｕｒ（ｋ，ｌ）を求める。
【数７】
2015068714.tif 000009
【００３１】
また、位相差算出部１３１ｈは、下記式（１０）により、ｋ番衛星とｌ番衛星との間におけるコード位相測定値の二重位相差ρｕｒ（ｋ，ｌ）を求める。
【数８】
2015068714.tif 000010
【００３２】
暫定姿勢角算出部１３１ａは、速度ベクトル算出部１２ｂにより算出された基準アンテナ１１１の速度ベクトルに基づいて、船舶２の暫定的な姿勢角（暫定姿勢角）を算出する。具体的には、速度ベクトルを（Ｖｅ，Ｖｎ，Ｖｄ）とし、暫定姿勢角を（θｙ，θｐ，θｒ）とした場合に、暫定姿勢角算出部１３１ａは、暫定的なヨー角θｙを下記式（１１）、暫定的なピッチ角θｐを下記式（１２）、暫定的なロール角θｒを下記式（１３）により算出することができる。
【数９】
2015068714.tif 000011
【００３３】
なお、速度ベクトル算出部１２ｂにより算出される基準アンテナ１１１の速度ベクトルに基づいて船舶２の暫定姿勢角を算出する場合、速度ベクトルの一方向と船舶２の船首方向とが、一致していることが好ましい。これは、船舶２が船首方向に対して交差する方向に滑りながら進行していることにより、速度ベクトルに基づいて算出される暫定姿勢角と、実際の姿勢角との誤差が大きくなることを避けるためである。
【００３４】
暫定基線ベクトル算出部１３１ｂは、暫定姿勢角算出部１３１ａにより算出された船舶２の暫定姿勢角を用いて既知の基線ベクトルを変換することにより、暫定的な基線ベクトル（暫定基線ベクトル）を算出する。具体的には、ボディ座標系における既知の基線ベクトルをＢＢｏｄｙ、ローカル座標系における暫定基線ベクトルをＢＬｏｃａｌとした場合に、暫定基線ベクトル算出部１３１ｂは、ローカル座標系における暫定基線ベクトルＢＬｏｃａｌを下記式（１４）により算出することができる。
【数１０】
2015068714.tif 000012
【００３５】
なお、ボディ座標系における既知の基線ベクトルＢＢｏｄｙは、船舶２に固定された各アンテナ１１間の相対的な距離及び方向に基づいて予め測定され、記憶部１３２に記憶されている。暫定基線ベクトル算出部１３１ｂは、記憶部１３２に記憶されている既知の基線ベクトルＢＢｏｄｙを読み出し、暫定基線ベクトルＢＬｏｃａｌを算出する。
【００３６】
非基準アンテナ位置算出部１３１ｃは、基準アンテナ位置算出部１２ａにより算出された基準アンテナ１１１の位置、及び、暫定基線ベクトル算出部１３１ｂにより算出された暫定基線ベクトルＢＬｏｃａｌに基づいて、非基準アンテナ１１２，１１３の位置を算出する。具体的には、ローカル座標系における基準アンテナ１１１の位置をＰＯＳｒｅｆ、ローカル座標系における非基準アンテナ１１２，１１３の位置をＰＯＳｕｓｅｒとした場合に、非基準アンテナ位置算出部１３１ｃは、非基準アンテナ１１２，１１３の位置ＰＯＳｕｓｅｒを下記式（１５）により算出することができる。
【数１１】
2015068714.tif 000013
【００３７】
整数値バイアス算出部１３１ｄは、位相差算出部１３１ｈにより算出された二重位相差の整数値バイアスを算出する。ここで、上述の式（９）及び式（１０）によれば、二重位相差の整数値バイアスＮｕｒ（ｋ，ｌ）を下記式（１６）で表すことができる。整数値バイアス算出部１３１ｄは、下記式（１６）においてノイズεｕｒ（ｋ，ｌ）が取り得る値を見積り、その範囲内で整数値バイアスＮｕｒ（ｋ，ｌ）の正解値をサーチする。したがって、ノイズεｕｒ（ｋ，ｌ）が小さいほど、整数値バイアスＮｕｒ（ｋ，ｌ）の算出に要する時間は短くなる。
【数１２】
2015068714.tif 000014
【００３８】
ただし、上記のように二重位相差の整数値バイアスを算出するような構成に限らず、位相差算出部１３１ｈで一重位相差を算出し、整数値バイアス算出部１３１ｄで一重位相差の整数値バイアスを算出するような構成であってもよい。この場合、上述の式（６）及び式（８）に基づいて、一重位相差の整数値バイアスＮｕｒ（ｋ）を下記式（１７）で表すことができる。
【数１３】
2015068714.tif 000015
【００３９】
視線方向ベクトル算出部１３１ｅは、衛星位置算出部１２ｃにより算出された各ＧＰＳ衛星３の位置、基準アンテナ位置算出部１２ａにより算出された基準アンテナ１１１の位置、及び、非基準アンテナ位置算出部１３１ｃにより算出された非基準アンテナ１１２，１１３の位置に基づいて、各アンテナ１１から各ＧＰＳ衛星３への視線方向ベクトルを算出する。すなわち、視線方向ベクトル算出部１３１ｅは、各ＧＰＳ衛星３の位置と基準アンテナ１１１の位置とに基づいて、基準アンテナ１１１から各ＧＰＳ衛星３への視線方向ベクトルを算出することができるとともに、各ＧＰＳ衛星３の位置と非基準アンテナ１１２，１１３の位置とに基づいて、非基準アンテナ１１２，１１３から各ＧＰＳ衛星３への視線方向ベクトルを算出することができる。
【００４０】
基線ベクトル算出部１３１ｆは、視線方向ベクトル算出部１３１ｅにより算出された視線方向ベクトル、及び、整数値バイアス算出部１３１ｄにより算出された整数値バイアスに基づいて、ローカル座標系における基線ベクトルを算出する。ここで、ノイズが十分に小さい場合には、整数値バイアスＮｕｒ（ｋ，ｌ）を算出すると、下記式（１８）により衛星・受信機間距離ｒｕｒ（ｋ，ｌ）の二重差を求めることができる。
【数１４】
2015068714.tif 000016
【００４１】
このとき、衛星・受信機間距離の二重差をＲｄ、視線方向ベクトルの二重差をＨｄ、基線ベクトルをＢｄとすると、下記式（１９）が成り立つため、基線ベクトル算出部１３１ｆは、下記式（２０）により基線ベクトルＢｄを算出することができる。なお、一重位相差の整数値バイアスＮｕｒ（ｋ）を用いて基線ベクトルを算出する場合にも、同様の関係が成り立つ。
【数１５】
2015068714.tif 000017
【００４２】
このように、基準アンテナ１１１の位置に基づいて算出された非基準アンテナ１１２，１１３の位置を用いて視線方向ベクトルが算出されるため、上記式（２０）における視線方向ベクトルの二重差Ｈｄの算出に際しては、基準アンテナ１１１の位置を算出する際の算出誤差しか重畳されない。したがって、基準アンテナ１１１及び非基準アンテナ１１２，１１３の双方に起因する誤差が重畳される従来技術と比較して、基線ベクトルＢｄの算出精度が向上する。
【００４３】
姿勢検出部１３１ｇは、基線ベクトル算出部１３１ｆにより算出されたローカル座標系における基線ベクトルに基づいて、船舶２の姿勢を検出する。具体的には、アンテナ１１が３つ以上の場合であって、基線ベクトル算出部１３１ｆにより２つの基線ベクトルを算出することができる場合には、第１基線ベクトルを（Ｂ１ｅ，Ｂ１ｎ，Ｂ１ｄ）、第２基線ベクトルを（Ｂ２ｅ，Ｂ２ｎ，Ｂ２ｄ）、姿勢角を（θｙ，θｐ，θｒ）とすると、下記式（２１）〜式（２３）により、ヨー角θｙ、ピッチ角θｐ及びロール角θｒを算出することができる。
【数１６】
2015068714.tif 000018
【００４４】
また、アンテナ１１が２つの場合であって、基線ベクトル算出部１３１ｆにより１つの基線ベクトルのみ算出できる場合には、下記式（２４）及び式（２５）により、ヨー角θｙ及びピッチ角θｐを算出することができる。
【数１７】
2015068714.tif 000019
【００４５】
ただし、上記式（２１）〜式（２５）は、船舶２の水平な平面上に全てのアンテナ１１が設置されている場合の計算式であり、例えば全てのアンテナ１１が船舶２の水平な平面上にない場合には、当該平面と基線ベクトルとの角度を測定して、上記式（２１）〜式（２５）による計算結果を補正する必要がある。
【００４６】
なお、本実施形態では、基準アンテナ１１１が設置されている位置（点）の速度ベクトルを用いて、上記式（１１）〜式（１３）により船舶２の暫定姿勢角を算出するため、各アンテナ１１を座標軸上に配置する必要はない。すなわち、各アンテナ１１が座標軸上に配置されていなくても、基線ベクトルと座標軸とのずれが、暫定姿勢角の算出に影響を与えることはない。ただし、上記式（２１）〜式（２５）により船舶２の実際の姿勢角を算出する際には、基線ベクトルと座標軸とのずれを予め測定し、その測定結果を考慮して姿勢角を算出する必要がある。
【００４７】
以上のように、本実施形態では、基準アンテナ１１１の速度ベクトルに基づいて船舶２の暫定姿勢角を算出し、当該暫定姿勢角を用いて既知の基線ベクトルを変換することにより算出される暫定基線ベクトルに基づいて、非基準アンテナ１１２，１１３の位置が算出される。そして、算出された非基準アンテナ１１２，１１３の位置と、ＧＰＳ衛星３から受信するＧＰＳ信号に基づいて算出される基準アンテナ１１１の位置とを用いて、ローカル座標系における基線ベクトルが算出され、当該基線ベクトルに基づいて船舶２の姿勢が検出される。
【００４８】
したがって、ＧＰＳ衛星３から受信するＧＰＳ信号に基づいて基準アンテナ１１１の位置を算出するだけで、その基準アンテナ１１１の位置に基づいて非基準アンテナ１１２，１１３の位置を算出し、船舶２の姿勢を検出することができる。これにより、整数値バイアスを算出する際に推定する誤差の大きさが小さくなるため、整数値バイアスのサーチ範囲を狭く設定することができる。その結果、ＧＰＳ衛星３から受信するＧＰＳ信号に基づいて複数のアンテナ１１の位置を算出するような従来の構成と比較して、姿勢の検出にかかる時間を短縮できる。
【００４９】
また、上述の通り、視線方向ベクトルの二重差Ｈｄの算出に際しては、基準アンテナ１１１の位置を算出する際の算出誤差しか重畳されないため、基線ベクトルＢｄの算出精度が向上する。すなわち、基準アンテナ１１１の位置を算出する際の算出誤差のみが姿勢の検出に影響を与えるため、複数のアンテナ１１についての算出誤差が姿勢の検出に影響を与える従来の構成と比較して、姿勢の検出精度を向上することができる。
【００５０】
図４は、船舶２の姿勢を検出する際の姿勢検出装置１による処理の一例を示すフローチャートである。船舶２の姿勢を検出する際には、まず、基準アンテナ位置算出部１２ａが、ＧＰＳ衛星３から受信するＧＰＳ信号に基づいて、基準アンテナ１１１の位置を算出する（ステップＳ１０１：基準アンテナ位置算出ステップ）。また、速度ベクトル算出部１２ｂにより、基準アンテナ１１１の速度ベクトルが算出される（ステップＳ１０２：速度ベクトル算出ステップ）。
【００５１】
その後、暫定姿勢角算出部１３１ａが、速度ベクトル算出部１２ｂにより算出された基準アンテナ１１１の速度ベクトルに基づいて、上記式（１１）〜式（１３）により船舶２の暫定姿勢角を算出する（ステップＳ１０３：暫定姿勢角算出ステップ）。そして、算出された船舶２の暫定姿勢角を用いて、暫定基線ベクトル算出部１３１ｂが、上記式（１４）により既知の基線ベクトルＢＢｏｄｙを変換することで、暫定基線ベクトルＢＬｏｃａｌが算出される（ステップＳ１０４：暫定基線ベクトル算出ステップ）。
【００５２】
このようにして算出された暫定基線ベクトルＢＬｏｃａｌ及び基準アンテナ１１１の位置ＰＯＳｒｅｆに基づいて、非基準アンテナ位置算出部１３１ｃは、上記式（１５）により非基準アンテナ１１２，１１３の位置ＰＯＳｕｓｅｒを算出する（ステップＳ１０５：非基準アンテナ位置算出ステップ）。
【００５３】
一方で、位相差算出部１３１ｈは、各アンテナ１１においてＧＰＳ衛星３から受信するＧＰＳ信号に基づいて、一重位相差又は二重位相差を算出する（ステップＳ１０６：位相差算出ステップ）。このとき、位相差算出部１３１ｈは、上記式（１）〜式（８）により一重位相差を算出し、又は、上記式（１）〜式（１０）により二重位相差を算出する。
【００５４】
そして、整数値バイアス算出部１３１ｄが一重位相差又は二重位相差の整数値バイアスを算出する（ステップＳ１０７：整数値バイアス算出ステップ）。二重位相差の整数値バイアスＮｕｒ（ｋ，ｌ）は、上記式（１６）により算出され、一重位相差の整数値バイアスＮｕｒ（ｋ）は、上記式（１７）により算出される。また、ＧＰＳ衛星３の位置、基準アンテナ１１１の位置ＰＯＳｒｅｆ、及び、非基準アンテナ１１２，１１３の位置ＰＯＳｕｓｅｒに基づいて、視線方向ベクトル算出部１３１ｅにより、各アンテナ１１から各ＧＰＳ衛星３への視線方向ベクトルが算出される（ステップＳ１０８：視線方向ベクトル算出ステップ）。
【００５５】
その後、算出された視線方向ベクトル、及び、算出された整数値バイアスに基づいて、上記式（２０）により、基線ベクトル算出部１３１ｆがローカル座標系における基線ベクトルＢｄを算出する（ステップＳ１０９：基線ベクトル算出ステップ）。そして、算出された基線ベクトルＢｄに基づいて、姿勢検出部１３１ｇが上記式（２１）〜式（２３）、又は、上記式（２４）及び式（２５）を用いて演算を行うことにより、船舶２の姿勢が検出される（ステップＳ１１０：姿勢検出ステップ）。
【００５６】
本実施形態において、船舶２の姿勢は、図４のステップＳ１０７で整数値バイアスが初期決定される場合だけでなく、整数値バイアスが再決定される場合にも検出できる。したがって、いわゆるサイクルスリップが発生した場合などであっても、再決定された整数値バイアスに基づいて、船舶２の姿勢を短時間で検出することができるため、従来よりも高いアベイラビリティを確保することができる。
【００５７】
以上の実施形態では、視線方向ベクトル算出部１３１ｅにより算出された視線方向ベクトル、及び、整数値バイアス算出部１３１ｄにより算出された整数値バイアスに基づいて、ローカル座標系における基線ベクトルＢｄが算出されるような構成について説明した。しかし、このような構成に限らず、基線ベクトル算出部１３１ｆは、基準アンテナ位置算出部１２ａにより算出された基準アンテナ１１１の位置、非基準アンテナ位置算出部１３１ｃにより算出された非基準アンテナ１１２，１１３の位置、及び、整数値バイアス算出部１３１ｄにより算出された整数値バイアスに基づいて、他の態様によりローカル座標系における基線ベクトルを算出するような構成であってもよい。
【００５８】
また、姿勢検出装置１の具体的構成は、上記実施形態のような構成に限られるものではない。例えば、ＧＰＳ受信機１２の機能の少なくとも一部が姿勢検出処理部１３に備えられていてもよいし、姿勢検出処理部１３の機能の少なくとも一部がＧＰＳ受信機１２に備えられていてもよい。すなわち、ＧＰＳ受信機１２の機能又は姿勢検出処理部１３の機能として、少なくとも基準アンテナ位置算出部１２ａ、速度ベクトル算出部１２ｂ、暫定姿勢角算出部１３１ａ、暫定基線ベクトル算出部１３１ｂ、非基準アンテナ位置算出部１３１ｃ、基線ベクトル算出部１３１ｆ及び姿勢検出部１３１ｇが姿勢検出装置１に備えられていればよい。
【００５９】
図５は、姿勢検出装置１の構成を概略的に示す機能ブロック図である。この図５に示すように、本発明に係る姿勢検出装置１は、基準アンテナ位置算出部１２ａと、速度ベクトル算出部１２ｂと、暫定姿勢角算出部１３１ａと、暫定基線ベクトル算出部１３１ｂと、非基準アンテナ位置算出部１３１ｃと、基線ベクトル算出部１３１ｆと、姿勢検出部１３１ｇとを備えている。
【００６０】
基準アンテナ位置算出部１２ａは、基準アンテナ１１１により受信したＧＮＳＳ信号に基づいて、基準アンテナ１１１の位置を算出する。速度ベクトル算出部１２ｂは、前記ＧＮＳＳ信号に基づいて、基準アンテナ１１１の速度ベクトルを算出する。暫定姿勢角算出部１３１ａは、前記速度ベクトルに基づいて、物体の暫定姿勢角を算出する。暫定基線ベクトル算出部１３１ｂは、前記暫定姿勢角を用いてボディ座標系における所定の基線ベクトルをローカル座標系における基線ベクトルに座標変換し、暫定基線ベクトルを算出する。非基準アンテナ位置算出部１３１ｃは、前記基準アンテナ１１１の位置及び前記暫定基線ベクトルに基づいて、非基準アンテナ１１２又は１１３の位置を算出する。基線ベクトル算出部１３１ｆは、前記基準アンテナ１１１の位置、前記非基準アンテナ１１２又は１１３の位置、基準アンテナ１１１により受信したＧＮＳＳ信号、及び基準アンテナ１１２又は１１３により受信したＧＮＳＳ信号に基づいて、ローカル座標系における基線ベクトルを算出する。姿勢検出部１３１ｇは、前記基線ベクトルに基づいて、前記物体の姿勢を検出する。
【符号の説明】
【００６１】
１    姿勢検出装置
２    船舶
３    ＧＰＳ衛星
１１    アンテナ
１２    ＧＰＳ受信機
１２ａ  基準アンテナ位置算出部
１２ｂ  速度ベクトル算出部
１２ｃ  衛星位置算出部
１２ｄ  位相算出部
１３    姿勢検出処理部
１１１    基準アンテナ
１１２    非基準アンテナ
１１３    非基準アンテナ
１３１    制御部
１３１ａ  暫定姿勢角算出部
１３１ｂ  暫定基線ベクトル算出部
１３１ｃ  非基準アンテナ位置算出部
１３１ｄ  整数値バイアス算出部
１３１ｅ  視線方向ベクトル算出部
１３１ｆ  基線ベクトル算出部
１３１ｇ  姿勢検出部
１３１ｈ  位相差算出部
１３２    記憶部
【図１】
2015068714.tif 000020
【図２Ａ】
2015068714.tif 000021
【図２Ｂ】
2015068714.tif 000022
【図３】
2015068714.tif 000023
【図４】
2015068714.tif 000024
【図５】
2015068714.tif 000025