JP5084303B2

JP5084303B2 - 移動体姿勢計測装置

Info

Publication number: JP5084303B2
Application number: JP2007054288A
Authority: JP
Inventors: 太郎柏柳; 幹男中村
Original assignee: Japan Radio Co Ltd
Current assignee: Japan Radio Co Ltd
Priority date: 2007-03-05
Filing date: 2007-03-05
Publication date: 2012-11-28
Anticipated expiration: 2027-03-05
Also published as: JP2008216062A

Description

本発明は、ＧＰＳなどの衛星航法システムを用いて船舶などの移動体の姿勢を計測する移動体姿勢計測装置に関する。

このような移動体の姿勢を計測するための背景技術について順を追って説明する。

（１）移動体における姿勢の定義
まず、２つの座標系、局所座標系と移動体座標系を次のように定義する。「局所座標系」は、移動体の位置において、ｘ-ｙ平面を水平面と平行させ、ｘ軸を真北に向けた右手系直交座標系である。「移動体座標系」は、移動体に固定された右手系直交座標系である。

このように定義したとき、船舶などの移動体の姿勢を示す、方位、ピッチ、ロールは、次のように表すことができる。

「方位」とは、局所座標系のｘ軸から移動体座標系のｘ軸の局所座標系のｘ-ｙ平面への射影へのなす角である。

「ピッチ」とは、移動体座標系のｘ軸の局所座標系のｘ-ｙ平面への射影からの移動体座標系のｘ軸へのなす角である。

「ロール」とは、移動体座標系のｘ軸を中心とした、移動体座標系のｙ軸の局所座標系の水平面からの回転角である。

（２）移動体姿勢計測装置の装置構成例
次に、前述のように表される方位、ピッチ、ロールからなる移動体の姿勢を計測する移動体姿勢計測装置の装置構成例を、図１、図２を参照して説明する。

この移動体姿勢計測装置１は、移動体の一例としての船舶Ｓに設置されている。移動体姿勢計測装置１は、アンテナ部２と処理部３とから構成される。

アンテナ部２は、アンテナα，β，γとからなり、アンテナαは基準アンテナとなる。このアンテナα，β，γは船舶Ｓ上に設置されているものとする。図１に示すように、移動体座標系のｘ軸はアンテナαとβがなす第１基線ｂ_１に一致するものとし、移動体座標系のｘ‐ｙ平面はアンテナα，β，γが作る平面にあるものとする。

処理部３は、３つの受信機４と姿勢算出処理機５とからなる。各受信機４は、それぞれ各アンテナα，β，γに接続されていて、各アンテナα，β，γで受信している衛星信号（ＧＰＳ信号など）を受信する。これにより、各受信機４では、受信している衛星信号の搬送波位相と、衛星への局所座標系での視線方向ベクトル（衛星と船舶Ｓとの位置関係を示す）とが得られる。ここでは、アンテナα，β，γ間の距離は比較的短いもの（例えば１メートル程度）とし、そのことから衛星に対する各アンテナα，β，γからの視線方向ベクトルはすべて同じであるものとする。姿勢算出処理機５は、各受信機４から得られた情報に基づいてアンテナα，β，γの姿勢を算出する演算を行う。

（３）一般的な姿勢算出法
移動体姿勢計測装置１では、衛星航法システムを利用して各アンテナα，β，γ間の基線ベクトルを算出すれば、その算出値から船舶Ｓの姿勢を算出することができる。

図１に示すように、アンテナαからβへのとの第１基線をｂ_１、アンテナαからγへの第２基線をｂ_２とすれば、この基線ｂ_１，ｂ_２から、船舶Ｓの姿勢、すなわち、方位、ピッチ、ロールは、それぞれ、
………（１）
と表すことができる。

ここで、
………（２）
である。なお、数１において、arg(・)は複素数の偏角を求める関数であり、添え字ｘ,ｙ,ｚは、ベクトルの座標系における各軸方向の成分を意味する。

（４）基線ベクトルの算出
前述のとおり、基線ベクトルを求めることにより、船舶Ｓの姿勢を算出することができる。ここで、基線ベクトルを求めるには、次のようにする。

まず、受信機４から得られた搬送波位相から一重位相差、二重位相差を求める。ここで、「一重位相差」とは、基準アンテナ（アンテナα）及びユーザアンテナ（アンテナβ，γ）における、同一衛星からの信号の搬送波位相の差分であり、「二重位相差」とは、基準衛星に対する一重位相差と他の衛星の一重位相差の差分である。

そして、このような一重位相差又は二重位相差と衛星への視線方向ベクトルとの関係から、最小二乗法をもちいて基線ベクトルを算出することができる。

（５）基線と一重位相差又は二重位相差との関係
図３を参照して、一重位相差とは基準アンテナ１０１及びユーザアンテナ１０２における、同一衛星１０５からの信号の搬送波位相の差分である。この差分と基準アンテナ１０１（その信号は基準受信機１０３で受信する）及びユーザアンテナ１０２（その信号は基準受信機１０４で受信する）により構成される基線ベクトルの間には行路差から、
………（３）
が成り立つ。なお、ｙは位相差、ｈは衛星への単位方向ベクトル、ｂは基線ベクトル、ａは整数の値、Δｔは受信機の位相差の取得タイミングのズレを表す時計誤差、ｅは観測誤差、であり、これらのｙ，ｂ，ａ，Δｔ，ｅの単位は波数とする。また、添え字ｓは一重位相差についての値であることを意味する。

この式は、一重位相差と行路差ｈ^Ｔ _ｓｂとの間には、ある波長の整数倍とΔｔとを足し合わせた分だけの差があることを示している。例えば、各アンテナで瞬時位相を観測したとき、その値の範囲は［‐０．５：０．５］となる。このとき、一重位相差の値は［‐１．０：１．０］でしか表されないため、行路差ｈ^Ｔ _ｓｂとの間には、ある波長の整数倍とΔｔとを足し合わせた分だけの差があることになる。この整数値は整数値アンビギュイティと呼ばれ、（３）式中のａ_ｓにあたる。

また、二重位相差とは、ある基線における基準衛星の一重位相差と基準衛星以外の一重位相差の差分である。基準衛星ｉと衛星ｋとにおける二重位相差と基線との関係は、
………（４）
と表される。

ここで、
………（５）
を意味する。なお、添え字ｄは二重位相差に関する値であることを表している。二重位相差を用いる利点は、時計誤差を打ち消すことができることである。

（６）基線の算出
複数の衛星を受信すれば、基線ベクトルｂに関する連立方程式を立てることができ、この連立方程式を最小二乗法により解けば基線ベクトルｂを算出できる。

一重位相差の場合、連立方程式は、

………（６）
と表される。

ここで、
………（７）
である。

整数値アンビギュイティが既知であるとすれば、基線と時計誤差は、最小二乗法を用いて、
………（８）
と推定できる。

ここで、Ｑは重み行列であり、通常、
………（９）
と設定するのが普通である。ここでσ^２は規格化係数である。

二重位相差の場合も同様に解け、連立方程式は、
………（１０）
と表され、整数値アンビギュイティが既知であるとすれば、基線と時計誤差の推定値ｂ（ｂにハット）は、
………（１１）
と求めることができる。

整数値アンビギュイティは、周知のLAMBDA法や、特願2006-241649号（本出願時において未出願公開）の明細書中に示される手法を用いることで解くことができる。

（７）アンテナ配置関係拘束による姿勢の計算
前述の手法とは異なる次の手法を用いても船舶Ｓの姿勢を算出することができる。これは各アンテナの配置が既知であることを利用した姿勢の計測法で、前述の手法に比べて姿勢の測定精度が良いという利点がある。

移動体座標系における各基線ベクトルをｂ_ｖ１，ｂ_ｖ２と表す。これらのベクトルは既知であり、前述の局所座標系における各基線ベクトルｂ_１，ｂ_２は、
………（１２）
の関係がある。

ここで、Ｕ（ψ，θ，φ）は、移動体座標系から局所座標系への変換行列で、
………（１３）
と表される。

ここで、一重位相差を用いて姿勢を算出することを考える。このとき、一重位相差と船舶Ｓの姿勢との関係は、各基線ベクトルｂ_１，ｂ_２について、
………（１４）
と表される。

各基線ベクトルｂ_１，ｂ_２についての連立方程式を一つにまとめれば、
………（１５）
と表される。ここで、
………（１６）
である。上記の（１５）式を観測モデル式と呼び、(１６)式を状態変数ベクトルと呼ぶ。

船舶Ｓの姿勢の算出は、状態変数ベクトルを推定することにより行うことができる。いま、整数値アンビギュイティベクトルａは既知であるとすれば、状態変数ベクトルは、初期点ｘ_０として適当な値に設定して、非線形推定として非線形最小二乗法を用いて、
………（１７）
を収束させることで、推定できる。

ここで、
………（１８）
である。

二重位相差を用いて姿勢を算出する場合は、観測モデル式、状態変数ベクトルを、
………（１９）
………（２０）
とすることで同様に求めることができる。
特許第３５０２００７号公報

前述のアンテナ配置関係拘束による船舶Ｓの姿勢の計算は、一般的な姿勢の計算の手法に比べて精度が良いという利点がある。

しかしながら、アンテナ配置関係拘束による姿勢の計算手法によっても、その計算の精度を十分に高めようとすると、移動体座標系におけるアンテナの配置を正確に求める必要がある。

よって、アーム等で各アンテナ間の配置関係を正確に固定するか、あるいは、各アンテナ間の配置関係を正確に測定しなければならず、大変な手間暇を要してしまうという不具合があった。

そこで、本発明の目的は、手間暇をかけることなく、移動体の姿勢の計算の精度を向上させることができる移動体姿勢計測装置を提供することである。

（１）本発明は、移動体に設置されて衛星の信号を受信するｎ個（ｎは３以上の整数）のアンテナと、前記各アンテナで受信した衛星の信号から当該アンテナと前記衛星との位置関係を算出する第１の位置関係算出手段と、前記各アンテナで受信した衛星信号の搬送波位相を測定する搬送波位相測定手段と、前記搬送波位相測定手段で測定した搬送波位相から一重位相差又は二重位相差を計算する位相差計算手段と、前記各アンテナの移動体座標系における相対的位置関係を記憶する記憶手段と、前記第１の位置関係算出手段で算出した前記アンテナと前記衛星との位置関係と前記一重位相差又は二重位相差とを用いて、前記記憶手段に記憶している各アンテナの相対的位置関係を拘束条件として前記移動体の姿勢を算出する姿勢算出手段と、前記第１の位置関係算出手段で算出した前記アンテナと前記衛星との位置関係と前記一重位相差又は二重位相差とを用いて、前記各アンテナの局所座標系における相対的位置関係を算出する第２の位置関係算出手段と、前記姿勢算出手段で算出した移動体の姿勢を用いて、前記第２の位置関係算出手段で算出した前記各アンテナの局所座標系における相対的位置関係を移動体座標系における相対的位置関係に変換する座標系変換手段と、前記座標系変換手段で移動体座標系に変換後の前記各アンテナの移動体座標系における相対的位置関係について誤差を補正する補正手段と、前記記憶手段に記憶されている各アンテナの移動体座標系における相対的位置関係を前記補正手段で補正後の各アンテナの相対的位置関係により更新する更新手段と、を備えている移動体姿勢計測装置である。

（２）この場合に、前記移動体の姿勢のうちロール及びピッチを計測する傾斜計をさらに備え、前記座標系変換手段は、前記姿勢算出手段で算出した移動体の姿勢のうちの方位を用い、又、前記姿勢算出手段で算出した移動体の姿勢のうちのロール及びピッチに代えて前記計測手段で計測したロール及びピッチを用いて、前記位置関係算出手段で算出した前記各アンテナの相対的位置関係を移動体座標系における相対的位置関係に変換する、ようにしてもよい。

（１）の発明によれば、記憶手段に初期値として記憶されているのが各アンテナの配置関係の設計値であって誤差を含んでいても、位置関係算出手段により各アンテナの局所座標系における相対的位置関係を求め、これを座標系変換手段で移動体座標系に変換し、補正手段で補正することにより、各アンテナの移動体座標系における相対的位置関係を初期値に比べてより正確に求めることできる。よって、この求めた各アンテナの相対的位置関係により記憶手段に記憶されているアンテナの配置関係を更新すれば、次回の移動体の姿勢の計測はより正確なものとなる。よって、以上の処理を繰り返すことにより記憶手段に記憶されている各アンテナの移動体座標系における相対的位置関係はさらに正確なものとなり、これにより計測される移動体の姿勢もさらに正確なものとなる。従って、各アンテナ間の配置関係を正確に測定するような大変な手間暇をかけることなく、移動体の姿勢の計算の精度を向上させることができる。

（２）の発明によれば、設計上ではアンテナを水平面に一致するように配置したが実際には水平面と誤差が生じているときには、衛星を使用して測定した移動体のピッチ、ロールにもずれが生じてしまうので、当該ピッチ、ロールを傾斜計で測定したピッチ、ロールの値に置き換えることで、さらに移動体の姿勢の計算の精度を向上させることができる。

以下、本発明の実施の形態について図面を参照して説明する。

図１、図２は、本実施の形態の移動体姿勢計測装置の説明図である。この移動体姿勢計測装置１は、移動体の一例としての船舶Ｓに設置されている。移動体姿勢計測装置１は、アンテナ部２と処理部３とから構成される。

アンテナ部２はアンテナα，β，γとからなり、アンテナαは基準アンテナとなる。このアンテナα，β，γは船舶Ｓ上に設置されているものとする。図１に示すように、移動体座標系のｘ軸はアンテナαとβがなす第１基線ｂ_１に一致するものし、移動体座標系のｘ‐ｙ平面はアンテナα，β，γが作る平面に一致するものとする。

処理部３は、３つの受信機４と姿勢算出処理機５とからなる。各受信機４は、それぞれ各アンテナα，β，γに接続されていて、各アンテナα，β，γで受信している衛星信号（ＧＰＳ信号など）を受信する。これにより、各受信機４では、受信している衛星信号の搬送波位相と、衛星への局所座標系での視線方向ベクトル（衛星と船舶Ｓとの位置関係を示す）とが得られる。また、姿勢算出処理機５は、各受信機４から得られた情報に基づいてアンテナα，β，γの姿勢を算出する演算を行う。

次に、姿勢算出処理機５が実行する処理について説明する。図４は、姿勢算出処理機５が実行する処理を説明する機能ブロック図である。姿勢算出処理機５が実行する処理は、マイクロコンピュータが所定のプログラムに基づいて実行する。

まず、位相差計算部１１は、各受信機４から得られる受信中の衛星信号の搬送波位相から一重位相差又は二重位相差を計算する。この具体的な計算手法については前述のとおりである。

記憶部１２は、アンテナα，β，γの移動体座標系における相対的位置関係を予め記憶している。このアンテナα，β，γの相対的位置関係のデータとしては、アンテナα，β，γの配置関係の設計値を初期値として予め記憶している。

姿勢算出部１３は、衛星への局地座標系での視線方向ベクトルと、位相差計算部１１で求めた一重位相差又は二重位相差とを用いて、記憶部１２に記憶している各アンテナα，β，γの移動体座標系における相対的位置関係から船体Ｓの姿勢を算出する。この姿勢の算出は、前述したアンテナ配置関係拘束による姿勢の計算により求めることができる。すなわち、状態変数ベクトル（（１６）式）を推定することにより求めることができる。この求めた船体Ｓの姿勢（方位、ピッチ、ロール）をψ（ψにハット）、θ（θにハット）、φ（φにハット）と表すことにする。ここで求めた船体Ｓの姿勢は表示装置（図示せず）に表示され、ユーザに報知される。

位置関係算出部１４は、衛星への局所座標系での視線方向ベクトルと、位相差計算部１１で求めた一重位相差又は二重位相差とを用いて、各アンテナα，β，γのうちの基準アンテナとなるアンテナαから見た各アンテナβ，γの局所座標系における相対的位置関係（基線ベクトル）を算出する。算出された相対的位置関係を基線ベクトルｂ_ｌ（ｂにハット）と表す。ここでｂの添え字のｌは各基線に割り当てられた番号を表す。図５は、この場合の基線ベクトルｂ_ｌ，ｂ_２（いずれもｂにハット）を図示するものである。

座標系変換部１５は、姿勢算出部１３で算出した船体Ｓの姿勢を用いて、位置関係算出部１４で算出した局所座標系における各アンテナα，β，γの相対的位置関係（基線ベクトルｂ_ｌ，ｂ_２）を移動体座標系における相対的位置関係に変換する。（２１）式は、このときの移動体座標系への変換処理を示すもので、ｂ_ｖｌ（ｂにハット）は変換後の移動体座標系を示し、添え字のｌは各基線に割り当てられた番号を表す。また、Ｕ^−１は、（１３）式の逆行列である。
………（２１）

図６は、このときの変換後の移動体座標系を示すもので、ｂ_ｖｌ，ｂ_ｖ２（それぞれのｂにハット）は、変換後の移動体座標系を示す基線ベクトルである。「観測された位置」とあるのは、移動体座標系に変換後の基線ベクトルｂ_ｖｌ，ｂ_ｖ２が示すアンテナαから見たアンテナβ，γそれぞれの位置である。「設計上の位置」とあるのは、記憶部１２が記憶しているアンテナαから見たアンテナβ，γそれぞれの位置である。

補正部１６は、座標系変換部１５で移動体座標系に変換後の各アンテナα，β，γの相対的位置関係を示す基線ベクトルｂ_ｖｌ，ｂ_ｖ２をフィルタリングして当該各アンテナα，β，γの相対的位置関係に含まれる誤差を補正する。

この場合のフィルタリングの例としては、
………（２２）
のように、ｂ_ｖ（ｌ）（ｂにハット）の一定時間の平均を求めるか（ここでＡｖ(-)は、平均を求める操作を意味する）、あるいは、
………（２３）
のように、得られた各アンテナα，β，γの相対的位置関係を、記憶部１２に記憶されている各アンテナα，β，γの相対的位置関係と係数ｑとによりローパスフィルタリングする方法が挙げられる。このフィルタリングすることでより正確な各アンテナα，β，γの相対的位置関係を示す基線ベクトルｂ^Ｆ _ｖ（ｌ）を算出することができる。

図７は、この場合の基線ベクトルｂ^Ｆ _ｖ（ｌ）を示す説明図である。ここで、「フィルタリング後の位置」とは、補正部１６でフィルタリング後の各アンテナα，β，γの相対的位置関係を示すものである。また、想像線で示されている位置は、記憶部１２が記憶しているアンテナαから見たアンテナβ，γそれぞれの位置である「設計上の位置」である。

更新部１７は、補正部１６で補正後の各アンテナα，β，γの移動体座標系における相対的位置関係のデータで、記憶部１２に記憶されているアンテナα，β，γの移動体座標系における相対的位置関係を、
………（２４）
と更新する。ここで、「Ａ←Ｂ」はＡにＢを代入することを意味する。

以上説明したように、記憶部１２に初期値として記憶されているのがアンテナα，β，γの配置関係の設計値であって誤差を含んでいても、位置関係算出部１４によりアンテナα，β，γの相対的位置関係を求め、これを座標系変換部１５で移動体座標系に変換し、補正部１６で補正することにより、アンテナα，β，γの相対的位置関係を初期値に比べてより正確に求めることできる。よって、この求めたアンテナα，β，γの相対的位置関係により記憶部１２に記憶されているのがアンテナα，β，γの配置関係を更新すれば、次回の船舶Ｓの姿勢の計測はより正確なものとなる。よって、以上の処理を繰り返すことにより記憶部１２に記憶されているアンテナα，β，γの相対的位置関係はさらに正確なものとなり、これにより計測される船舶Ｓの姿勢もさらに正確なものとなる。

従って、各アンテナα，β，γ間の移動体座標系における配置関係を正確に測定するような大変な手間暇をかけることなく、船舶Ｓの姿勢の計算の精度を向上させることができる。

なお、求めた移動体の姿勢の種類によっては、前述のフィルタリング後の移動体座標系における各アンテナα，β，γ間の相対位置関係が作る移動体座標系における姿勢は、設計値による各アンテナ間の相対位置関係が作る移動体座標系における姿勢と異なる場合があるので、両者の姿勢のすべて又は一部の成分を一致させるように式（２４）の代わりに、
………（２５）
のように、前述のフィルタリング後の移動体座標系における各アンテナα，β，γ間の相対位置を適切な変換行列Tを用いて適切な値に修正した相対位置関係を移動体座標系に対するアンテナα，β，γ間の配置関係の設計値として記憶部１2の値を更新してもよい。

また、図８に示すように、移動体姿勢計測装置１に船舶Ｓの姿勢のうち、少なくともロール、ピッチを計測する傾斜計２１を設けてもよい。

すなわち、設計上ではアンテナα，β，γの各高さを水平面に一致するように配置した場合において、実際には各アンテナα，β，γが水平面からみて一律の高さとなるとは限らない。この場合に衛星の信号を用いて測定した船舶Ｓのピッチ、ロールは実際のピッチ、ロールとはずれてしまうことになる。

そこで、船舶Ｓに少なくとも船舶Ｓのピッチ、ロールを測定する傾斜計２１を設け、座標系変換部１５では、姿勢算出部１３で算出した方位と、（姿勢算出部１３で算出したピッチ、ロールに代えて）傾斜計２１で測定したピッチ、ロールとを船体Ｓの姿勢として用いて、位置関係算出部１４で算出した局所座標系における各アンテナα，β，γの相対的位置関係（基線ベクトルｂ_ｌ，ｂ_２）を移動体座標系における相対的位置関係に変換するようにする。

これにより、水平面からみたアンテナα，β，γの位置の誤差についてもアンテナの配置関係に加味することができ、より正しい姿勢を推定することができる。

なお、前述の各例では、船舶Ｓに設置されるアンテナをアンテナα，β，γの３本の例で説明したが、４本以上のアンテナを用いても良い。

本発明の一実施の形態である移動体姿勢計測装置を船体に配置した状態の概念図である。移動体姿勢計測装置の処理部の構成を説明するブロック図である。一重位相差の説明図である。姿勢算出処理機が実行する処理を説明する機能ブロック図である。位置関係算出部が実行する処理について説明する説明図である。座標系変換部が実行する処理について説明する説明図である。補正部が実行する処理について説明する説明図である。傾斜計を備えた移動体姿勢計測装置について説明する機能ブロック図である。

符号の説明

１移動体姿勢計測装置
４受信機
１１位相差計算部
１２記憶部
１３姿勢算出部
１４位置関係算出部
１５座標系変換部
１６補正部
１７更新部
２１傾斜計
α，β，γ アンテナ
Ｓ移動体

Claims

移動体に設置されて衛星の信号を受信するｎ個（ｎは３以上の整数）のアンテナと、
前記各アンテナで受信した衛星の信号から当該アンテナと前記衛星との位置関係を算出する第１の位置関係算出手段と、
前記各アンテナで受信した衛星信号の搬送波位相を測定する搬送波位相測定手段と、
前記搬送波位相測定手段で測定した搬送波位相から一重位相差又は二重位相差を計算する位相差計算手段と、
前記各アンテナの移動体座標系における相対的位置関係を記憶する記憶手段と、
前記第１の位置関係算出手段で算出した前記アンテナと前記衛星との位置関係と前記一重位相差又は二重位相差とを用いて、前記記憶手段に記憶している各アンテナの相対的位置関係を拘束条件として前記移動体の姿勢を算出する姿勢算出手段と、
前記第１の位置関係算出手段で算出した前記アンテナと前記衛星との位置関係と前記一重位相差又は二重位相差とを用いて、前記各アンテナの局所座標系における相対的位置関係を算出する第２の位置関係算出手段と、
前記姿勢算出手段で算出した移動体の姿勢を用いて、前記第２の位置関係算出手段で算出した前記各アンテナの局所座標系における相対的位置関係を移動体座標系における相対的位置関係に変換する座標系変換手段と、
前記座標系変換手段で移動体座標系に変換後の前記各アンテナの移動体座標系における相対的位置関係について誤差を補正する補正手段と、
前記記憶手段に記憶されている各アンテナの移動体座標系における相対的位置関係を前記補正手段で補正後の各アンテナの相対的位置関係により更新する更新手段と、
を備えている移動体姿勢計測装置。
前記移動体の姿勢のうちロール及びピッチを計測する傾斜計をさらに備え、
前記座標系変換手段は、前記姿勢算出手段で算出した移動体の姿勢のうちの方位を用い、又、前記姿勢算出手段で算出した移動体の姿勢のうちのロール及びピッチに代えて前記計測手段で計測したロール及びピッチを用いて、前記位置関係算出手段で算出した前記各アンテナの相対的位置関係を移動体座標系における相対的位置関係に変換する、
請求項１に記載の移動体姿勢計測装置。