(57)【特許請求の範囲】
【請求項１】
移動体上に固定された複数のアンテナを備えるキャリア位相相対測位装置において、
前記複数のアンテナで受信した測位用信号のキャリアの位相差観測量を得る受信機と、
前記位相差観測量を入力とし、かつ前記複数のアンテナの相対的な位置関係より得られる既知情報を制約条件とするラグランジェ乗数法を用いて整数バイアスのフロート解を推定する整数バイアスフロート解推定部と、
前記整数バイアスフロート解推定部で推定された整数バイアスのフロート解から整数バイアス解を決定する整数バイアス決定部と、
前記整数バイアス決定部によって決定された整数バイアス解が正しいかどうかを検定する整数バイアス検定部と、を備えることを特徴とするキャリア位相相対測位装置。
【請求項２】
請求項１に記載のキャリア位相相対測位装置において、
移動体速度を検出する速度検出部と、
前記移動体速度から算出した擬似姿勢角を用いて基線ベクトルを推定する基線ベクトル初期値演算部をさらに備え、
前記整数バイアスフロート解推定部は、基線ベクトルと整数バイアスのフロート解を推定するものであり、前記基線ベクトル初期値演算部で推定した基線ベクトルを前記整数バイアスのフロート解推定処理時の基線ベクトルの初期値として与えることを特徴とするキャリア位相相対測位装置。
【請求項３】
請求項１に記載のキャリア位相相対測位装置において、
移動体の加速度及び角速度を計測する慣性センサと、
移動体速度を検出する速度検出部と、
前記慣性センサの観測量と前記移動体速度から基線ベクトルを推定する基線ベクトル初期値演算部をさらに備え、
前記整数バイアスフロート解推定部は、基線ベクトルと整数バイアスのフロート解を推定するものであり、前記基線ベクトル初期値演算部で推定した基線ベクトルを前記整数バイアスのフロート解推定処理時の基線ベクトルの初期値として与えることを特徴とするキャリア位相相対測位装置。
【請求項４】
請求項３に記載のキャリア位相相対測位装置において、
前記基線ベクトル初期値演算部は、前記慣性センサの観測量から移動体のピッチを算出するとともに前記速度検出部からの速度情報より方位を算出し、該ピッチ及び方位情報から基線ベクトルを推定することを特徴とするキャリア位相相対測位装置。
【請求項５】
請求項３に記載のキャリア位相相対測位装置において、
整数バイアス解の決定後に得られる情報を用いて、前記慣性センサの観測量誤差を補正する慣性センサ誤差補正部をさらに備え、
前記基線ベクトル初期値演算部は、整数バイアス解の再決定時に、前記慣性センサ誤差補正部で誤差補正された前記慣性センサの観測量を用いて初期値として与える基線ベクトルを推定することを特徴とするキャリア位相相対測位装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は移動体に固定された複数のＧＰＳアンテナを備えたキャリア位相相対測位装置に関する。
【背景技術】
【０００２】
キャリア位相差を用いて測位を行うキャリア位相相対測位装置として、例えば、移動体にリジットに固定された複数のＧＰＳアンテナと慣性センサユニットを用いて移動体の姿勢を検出する姿勢検出装置がある。この姿勢検出装置では、アンテナ対と複数の衛星間で生成されるキャリア位相差の観測量から整数バイアス解を求め、得られた整数バイアス解から基準アンテナに対する他のアンテナ（以下、スレーブアンテナとする。）の位置ベクトル、即ち、基線ベクトルを算出し、この基線ベクトルから移動体の姿勢を決定している。
【０００３】
このような姿勢算出装置等のキャリア位相差を用いて測位を行うキャリア位相相対測位装置において、整数バイアス解（Integer  Ambiguity）を高速にかつ正しく決定することは主要な技術課題である。
【０００４】
図６はこの整数バイアス解を得るための演算処理手順を説明する図である。整数バイアス解の決定は、図６に示すように整数バイアスのフロート解（Ambiguity float solution）の推定処理１０１、整数バイアス解の決定処理１０２、及び整数バイアス解の検定処理１０３の３つに大別できる。
【０００５】
整数バイアスのフロート解の推定処理１０１は測位用信号の位相差観測量から整数バイアスのフロート解を算出する処理であり、カルマンフィルタ等を用いて整数バイアスのフロート解を推定する（非特許文献１）。整数バイアス解の決定処理１０２は整数バイアスのフロート解を算出後、整数バイアスのフロート解の算出精度に応じて決定されるサーチ範囲に基づいて整数バイアス候補解を決定し、該整数バイアス候補解からｐ個の整数バイアス候補解セット（Ｎ１，Ｎ２，・・・，Ｎｋ）pを決定する。その後決定したｐ個の整数バイアス候補解セット（Ｎ１，Ｎ２，・・・，Ｎｋ）pから１個の真の整数バイアス解セットを絞り込み、整数バイアス解を決定する。整数バイアス解の決定処理１０２にはLAMBDA技法（非特許文献２）等の公知の技法が用いられる。整数バイアス解の検定処理１０３は基線長情報等の既知情報を用いて決定された整数バイアス解が正しいかどうかを検定する処理である。
【０００６】
一旦整数バイアス解が決まれば、整数バイアス解を既知としてカルマンフィルタ等を用いて基線ベクトルを算出することができる。この基線ベクトルから姿勢検出装置は移動体の姿勢を得ることができる。
【０００７】
ところで、整数バイアス解を高速にかつ正しく決定するための技法は種々提案されている。例えば、非特許文献３、４は整数バイアスのフロート解や整数バイアス解の検定に基線長等の制約(baseline length constrain)を利用した技法を提案している。
【０００８】
非特許文献３は、移動体外の既知位置に置かれたＧＰＳ基準局に対して  移動体に２個のＧＰＳ移動局が固定設置された構成において、２個の移動局アンテナ間の基線長を既知の制約条件として整数バイアス解を推定するものである。なお、非特許文献３は、ピッチ、heading等の制約条件の利用も提示しているが、此等の制約は整数バイアスが決定された後でないと適用できない。
【０００９】
非特許文献４は、整数バイアス候補解の決定において、３次元空間のサーチ範囲を低次元化することによって効率的に整数バイアス候補解を求めるために、また、整数バイアスの検定のために既知の基線長情報を利用する技法である。
【００１０】
これに対し、後述する本発明は、移動体上にリジットに固定された複数のアンテナから得られる位相差観測量を用いて、整数バイアスのフロート解、基線ベクトルを推定するものであり、整数バイアスのフロート解、基線ベクトルを推定する推定アルゴリズムに直接、基線長等の既知情報制約を組み込んで整数バイアスのフロート解或いは基線ベクトルを求める技法である。
【非特許文献１】Hwang ，Patrick Y．C，"Kinematic GPS For Differential Positioning Resolving Integer Ambiguities On The Fly"，Navigation，Vol-31，No．1
【非特許文献２】P．J．G． Teunissen，"A New Method for carrier Phase Ambiguity Estimation "， proc． IEEE "Position Location and Navigation Symposium PLANS 94"， Las Vegas，11-15 April， 1994， pp 562-573．
【非特許文献３】Shawn D． Weisenburger， "Effect of Constraints and Multiple Receivers for On-The-Fly Ambiguity Resolution"，UCGE Reports Number 20109，Department of Geomatics Engineering，University of Calgary，1997（http://www.geomatics.ucalgary.ca/Papers/Thesis/MEC/97.20109.SWeisenburger.pdf（２００５．１０．７時点））
【非特許文献４】Lu G．"Development of a GPS Multi-Antenna System for Attitude Determination"，UCGE Reports No． 20073， Department of Geomatics Engineering ，University of Calgary，1995
【非特許文献５】Kornfeld． R．P． Hansman， R．J． and Deyst， J．J．  "Single Antenna GPS Based Aircraft Attitude  Determination，" Proceedings of the Institute of  Navigation National Technical Meeting， Jan． 21-  23， 1998， Long Beach， CA， pp． 345-354．
【発明の開示】
【発明が解決しようとする課題】
【００１１】
しかしながら、前述の従来の技法を用いたとしても、整数バイアスのフロート解の収束に要する時間はＧＰＳの衛星数に大きく依存するため、使用できるＧＰＳ衛星数が最小必要数の場合には整数バイアスフロート解の推定処理１０１を終えるのに通常２０〜３０分以上かかってしまう。また、推定された整数バイアスのフロート解の精度も悪くなる。整数バイアスのフロート解の精度が劣化することにより、その後の整数バイアス解の決定処理１０２や整数バイアス解の検定処理１０３に要する時間も長くなり、誤った整数バイアス解が生じることにもなる。
【００１２】
さらに、整数バイアス解を決定するために要する時間や整数バイアス解の決定精度は、マルチパス観測誤差の影響も受け、時には整数バイアス解が求められないか誤った整数バイアス解を生じることもある。
【００１３】
特に、短い基線長が要求される装置においては、整数バイアス解の僅かな誤りでも位置、速度、姿勢等に許容できない誤差を与えるので、正しい整数バイアス解を速く決定することが極めて重要となる。
【００１４】
また、慣性センサを利用したＧＰＳ／ＩＮＳ統合化装置（ＭＡ−ＧＰＳ／ＩＮＳシステム）も提案されているが、上述の欠点は十分に克服できていない。特に、電源ON時の整数バイアス解の決定に関してはＧＰＳシステムの性能に依存するためＩＮＳを統合したメリットを十分に生かせていない。
【００１５】
本発明は前記課題に鑑みてなされたものであり、整数バイアスのフロート解の推定処理１０１において、整数バイアスのフロート解を高速且つ高精度に推定することを目的とする。また、同時に、本発明は整数バイアス解決定後に、決定された整数バイアス解を用いて基線ベクトルを高速且つ高精度に求めることを目的とする。
【課題を解決するための手段】
【００１６】
前記課題を解決するために、本発明は、移動体上に固定された複数のアンテナを備えるキャリア位相相対測位装置において、前記複数のアンテナで受信した測位用信号のキャリアの位相差観測量を得る受信機と、前記位相差観測量を入力とし、かつ前記複数のアンテナの相対的な位置関係より得られる既知情報を制約条件とするラグランジェ乗数法を用いて整数バイアスのフロート解を推定する整数バイアスフロート解推定部と、前記整数バイアスフロート解推定部で推定された整数バイアスのフロート解から整数バイアス解を決定する整数バイアス決定部と、前記整数バイアス決定部によって決定された整数バイアス解が正しいかどうかを検定する整数バイアス検定部とを備えることを特徴とする。
【００１７】
すなわち、整数バイアスフロート解推定部は、整数バイアスのフロート解の推定アルゴリズムに基線長等の既知情報制約条件を直接組み込み、整数バイアスのフロート解を推定する。既知情報制約付きの条件下で整数バイアスのフロート解を推定する具体的な技法としては、例えば、ラグランジェ乗数（Lagrange multiplier）や拡張カルマンフィルタによる方法が考えられる。
【００１８】
ここで、複数のアンテナの相対的な位置関係より得られる既知情報とは、複数のＧＰＳアンテナ１が固定されていることから得られる既知の情報をいい、例えば、ＧＰＳアンテナ間の基線長情報や、各ＧＰＳアンテナ間に形成される基線の内積、外積情報がある。
【００１９】
また、本発明は、基線ベクトル初期値演算部が基線ベクトルの初期値を推定し、推定した基線ベクトルを前記整数バイアスフロート解推定処理時の基線ベクトルの初期値として与えることを特徴とする。これにより、整数バイアスのフロート解の推定処理をより高精度かつ高速に行うことが可能になる。なお、基線ベクトルの初期値の推定技法としては、例えば、移動体速度から算出した擬似姿勢角を用いて基線ベクトルを推定する技法や慣性センサの観測量及び移動体速度を用いて基線ベクトルを推定する技法がある。
【００２０】
更に、本発明における既知情報制約による技法は、整数バイアスのフロート解の推定に適用できるのみならず、整数バイアス解が決定した後の基線ベクトルの算出においても同様に適用することができる。
【００２１】
すなわち、本発明は移動体上に固定された複数のアンテナを備えるキャリア位相相対測位装置において、前記複数のアンテナで受信した測位用信号から位相差観測量を得る受信機と、前記位相差観測量から整数バイアス解を算出する整数バイアス解算出部と、前記複数のアンテナの相対的な位置関係より得られる既知情報を制約条件として、前記位相差観測量及び前記整数バイアス解から基線ベクトルを算出する基線ベクトル算出部を備えることを特徴とする。
【００２２】
この基線ベクトル算出部は、整数バイアス解算出部で算出された整数バイアス解を既知量として、基線ベクトルの推定アルゴリズムに基線長等の既知情報制約条件を直接組み込み、基線ベクトルを算出する。既知情報制約付きの条件下で基線ベクトルを求める具体的な技法としては、例えば、ラグランジェ乗数（Lagrange multiplier）や拡張カルマンフィルタによる方法が考えられる。
【００２３】
また、本発明は、基線ベクトル初期値演算部が基線ベクトルの初期値を推定し、推定した基線ベクトルを前記基線ベクトル算出処理時の基線ベクトルの初期値として与えることを特徴とする。これにより、整数バイアスのフロート解の推定処理をより高精度かつ高速に行うことが可能になる。なお、基線ベクトルの初期値の推定技法としては、例えば、移動体速度から算出した擬似姿勢角を用いて基線ベクトルを推定する技法や慣性センサの観測量及び移動体速度を用いて基線ベクトルを推定する技法がある。
【００２４】
また、本発明は、慣性センサの観測量を基線ベクトルの初期値推定に用いる場合において該慣性センサ誤差を補正する手段をさらに備えることを特徴とする。すなわち、整数バイアス解の決定後に得られる情報を用いて前記慣性センサの観測量誤差を補正する慣性センサ誤差補正部をさらに備え、前記基線ベクトル初期値演算部は、整数バイアス解の再決定時に、前記慣性センサ誤差補正部で誤差補正された前記慣性センサの観測量を用いて基線ベクトルを推定することにより、初期値として与える基線ベクトルの値を得る。この構成により、整数バイアス解の再決定時には、より高精度かつ高速に整数バイアスのフロート解、及び基線ベクトルを求めることが可能になる。
【発明の効果】
【００２５】
本発明によれば、複数のアンテナの相対的な位置関係より得られる既知情報を制約条件として、前記位相差観測量から整数バイアスのフロート解を推定することにより、制約の条件のない従来の推定技法に比べて、より高精度且つ高速に整数バイアスのフロート解が推定できる。この結果、整数バイアス解も高精度かつ高速に決定できる。
【００２６】
また、本発明は基線ベクトル初期値演算部により基線ベクトルの初期値を正確に推定することができるため、基線ベクトルの初期値の精度が良い場合は直接、整数バイアスと見なせることができるような精度で整数バイアスのフロート解が決定できる。また、このような高精度で基線ベクトル初期値が得られない場合であったとしても、整数バイアスのフロート解の誤差共分散は従来の技法に比べて格段小さくできる。このため、整数バイアス解を決定のためのサーチ範囲を狭くすることができ、結果として、より高速且つ正確に整数バイアス解を求めることが可能になる。
【００２７】
更に、本発明における既知情報制約による技法は、整数バイアスのフロート解の推定に適用できるのみならず、整数バイアス解が決定した後の基線ベクトルの算出においても同様に適用することができる。
【００２８】
また、本発明による既知情報制約による技法は位相差観測量の観測雑音に起因する推定精度の劣化対策として寄与するのみならず、マルチパス雑音による精度の劣化対策としても効果が期待できる。
【発明を実施するための最良の形態】
【００２９】
（実施の形態１）
以下、本発明の実施の形態１によるキャリア位相相対測位装置について図面を参照しながら説明する。
【００３０】
図１は、本発明で使用する航法座標系と移動体座標系について説明する説明図である。
図１（ａ）は、航法座標系と移動体座標系について示す図である。図１に示すように、基準となる航法座標系は北、東方向をそれぞれＸ軸、Ｙ軸とする右手系の直交座標系（Geodetic座標系）である。また、移動体座標系は移動体の進行方向、右手方向をそれぞれＸ軸、Ｙ軸とする右手系の直交座標系であり、移動体が船舶の場合には船首方向をＸ軸、右舷方向をＹ軸として設定する。なお、両座標系の原点は必ずしも一致させる必要はないが、ここでは理解を助けるために一致させている。また、本発明の説明中、右肩添え字ｎは航法座標系を示し、右肩添え字ｂは、移動体座標系（Body座標系）を示す。
【００３１】
図１（ｂ）は、図１（ａ）の移動体座標系上にマウントされたＧＰＳアンテナ位置を示す図である。図１（ｂ）において、基準アンテナが移動体座標系の原点に設置され、スレーブアンテナが所定の位置に設置されている。スレーブアンテナの位置は複数の基線ベクトルの方向が異なるような既知の任意の位置に設定される。図１（ｂ）は２個のスレーブアンテナの例を示しているが、スレーブアンテナは１個以上であればよい。
【００３２】
図２は、本発明の実施の形態１によるキャリア位相相対測位装置の構成の一例を示す図であり、移動体上にリジットに固定された３個のＧＰＳアンテナを備えたキャリア位相相対測位装置の構成例を示している。
【００３３】
図２において、本発明の実施の形態１によるキャリア位相相対測位装置は、ＧＰＳアンテナ１と、ＧＰＳ受信機２と、演算処理部３ａとからなる。演算処理部３ａは、整数バイアスフロート解推定部１１と、整数バイアス決定部１２と、整数バイアス検定部１３と、基線ベクトル算出部１４と、ＧＰＳ姿勢演算部１５とから構成されている。
【００３４】
ＧＰＳアンテナ１は、移動体上にリジットに固定されたアンテナであり、各アンテナは既知の位置に置かれている。なお、移動体上に固定されたＧＰＳアンテナ１は必ずしも３個に限られず、少なくとも２個以上、また基線の内積、外積を既知情報として用いる場合には少なくとも３個以上であればよい。なお、本発明の実施形態では、ＧＰＳアンテナ１を例に挙げて説明するが、アンテナが受信する電波はＧＰＳ衛星から送信される測位用信号に限らず、ＧＬＯＮＡＳＳ衛星、ＧＡＬＩＬＥＯ衛星、或いは、準天頂衛星等の測位用衛星から送信される測位用信号のうち少なくとも１つ以上の信号を受信できるものであればよい。
【００３５】
ＧＰＳ受信機２は、複数の衛星からの測位用信号をＧＰＳアンテナ１経由で受信して位相差観測量を生成し、基準アンテナ位置及び衛星位置の情報（Ephemeris）と共に整数バイアスフロート解推定部１１に出力する。ここで、位相差観測量は１重位相差、または２重位相差であっても良い。
【００３６】
整数バイアスフロート解推定部１１は、位相差観測量と整数バイアスのフロート解とを関係づける観測方程式にＧＰＳアンテナ間の相対的な位置関係より得られる既知情報の制約を組み込んで整数バイアスのフロート解を推定する。なお、ＧＰＳアンテナ間の相対的な位置関係より得られる既知情報とは、複数のＧＰＳアンテナ１が固定されていることから得られる既知の情報をいい、例えば、基線長情報や、各ＧＰＳアンテナ間に形成される基線の内積、外積情報がある。また、整数バイアスフロート解推定部１１が既知情報制約付きの条件下で整数バイアスのフロート解を推定する具体的な技法としては、例えば、ラグランジェ乗数（Lagrange multiplier）や拡張カルマンフィルタによる方法が考えられる。なお、この詳細は後述する。
【００３７】
整数バイアス決定部１２は、公知のLAMBDA技法等を用いて整数バイアスフロート解推定部１１で推定された整数バイアスのフロート解からp個の整数バイアス候補解セットを求める。その後ｐ個の整数バイアスの整数バイアス候補解セットの中から１個の真の整数バイアス解セットを絞り込み、整数バイアス解を決定する。ところで、整数バイアス決定部１２における整数バイアス候補解のサーチ範囲は、前述の整数バイアスフロート解推定部１１によって得られる整数バイアスのフロート解の精度に依存する。そのため、整数バイアスのフロート解の高精度化を実現することにより、整数バイアス候補解のサーチ範囲を狭め、整数バイアスの決定処理の高速化も同時に実現することができる。また、整数バイアスフロート解推定部１１による整数バイアスのフロート解の推定精度によっては、直接整数バイアスの整数解が得られるため、整数バイアス決定部１２を省略し、直接、整数バイアスフロート解推定部１１から整数バイアス解の検定を行うことも不可能ではない。
【００３８】
整数バイアス検定部１３は、基線長検定や統計的検定等の公知の技法により整数バイアス決定部１２によって決定された整数バイアス解が正しいかどうかを検定するものである。
【００３９】
基線ベクトル算出部１４は、整数バイアス検定部１３をパスした整数バイアス解を既知量として、基線ベクトルに対して線形化した位相差観測方程式にＧＰＳアンテナ間の相対的な位置関係より得られる既知情報の制約を組み込んで基線ベクトルを算出する。この際、基線ベクトル算出部１４は、整数バイアス解を既知とした新たな位相差観測量を用いる。なお、既知情報制約付きの条件下で基線ベクトルを求める具体的な手法としては、上述と同様、ラグランジェ乗数や拡張カルマンフィルタ技法が適用できる。この詳細については後述する。
【００４０】
ＧＰＳ姿勢演算部１５は、基線ベクトル算出部１４で算出された基線ベクトルからTRIAD技法やQUEST技法等の公知の技法を用いて姿勢角を求めるものである。
【００４１】
整数バイアス解算出部１６は、位相差観測量から整数バイアス解を算出するブロックを図示したものであり、例えば、前述の整数バイアスフロート解推定部１１、整数バイアス決定部１２、及び整数バイアス検定部１３を構成要素とする。
【００４２】
次に、整数バイアスフロート解推定部１１が行う処理についてさらに詳細に説明する。
例えば、１重位相差観測モデル（single difference observation model ）における航法座標系のキャリア位相差観測量ΔΦｉｊは、次式〔数１〕で表わせる。ここで、右下添え字で示したi、ｊはそれぞれ、i番目のスレーブアンテナ、ｊ番目の衛星を示す。
【数１】
0005301762.tif 000002
【００４３】
なお、以下の説明では、説明の簡略化のため、ある１個の基線ベクトルについて考える。そのために基線ベクトルにつけた右下添え字iは省略する。もし、複数の基線ベクトルで考える場合はそれに対応する式を形成すれば良く、以下で説明する技法を同様に適用することができる。
【００４４】
さて、観測方程式〔数１〕を線形化してマトリクス表現すると次式〔数２〕のように表現することができる。なお、右下添え字で示したｔ（１，２，・・・，ｎ）は観測エポックを示す。
【数２】
0005301762.tif 000003
【００４５】
整数バイアスフロート解推定部１１は、観測方程式〔数２〕に基づいて基線ベクトル（ｘｔ，ｙｔ，ｚｔ）、整数バイアス解（ΔＮ１，ΔＮ２，・・・，ΔＮｋ）からなるステートＸｔをＧＰＳアンテナ間の相対的な位置関係より得られる既知情報の制約条件下で推定する。
【００４６】
ここで、〔数２〕の観測方程式に基づいてステートＸｔを推定するためには、観測数が推定される未知数と同等以上であることが要求される。１重位相差観測モデルの場合、１基線ベクトル当り得られる観測数はｎ×ｋであり、未知数は３×ｎ+ｋ個であるから、必要な観測エポックはｎ≧ｋ／ｋ−３でなければならない。従って、４個以上の衛星数（k=４）で４観測エポック以上の観測量が必要となる。 
【００４７】
そこで、必要な観測エポックにおいて形成された観測方程式を改めて次式〔数３〕のように表わす。
【数３】
0005301762.tif 000004
【００４８】
整数バイアスフロート解推定部１１は、この〔数３〕の観測方程式に基づいて、ラグランジェ乗数（Lagrange multiplier）や拡張カルマンフィルタを用いて、ＧＰＳアンテナ間の相対的な位置関係より得られる既知情報の制約条件の下、ステートＸを推定する。
【００４９】
以下に、このラグランジェ乗数（Lagrange multiplier）及び拡張カルマンフィルタを用いたステートＸの推定手法として、ＧＰＳアンテナ間の相対的な位置関係より得られる既知情報が基線長情報である場合を例にとって説明する。
【００５０】
先ず、ラグランジェ乗数を用いた技法について説明する。
（１）ラグランジェ乗数を用いた技法
ラグランジェ乗数の技法では、〔数３〕の観測方程式と〔数４〕の線形制約式を考える。
【数４】
0005301762.tif 000005
【００５１】
ここでは１基線ベクトルについて考えているから、Ｃは当該の基線長でありスカラーである。ラグランジェ乗数による制約条件下における最適問題は〔数３〕、〔数４〕より、次式〔数５〕の問題に変換できる。なお、μはラグランジェ乗数である。
【数５】
0005301762.tif 000006
【００５２】
この〔数５〕のステートＸの最適解は次式〔数６〕で得ることができる。なお、ステートＸの最適解をより正確且つ高速に推定するためには、基線ベクトルｖ（ｘ，ｙ，ｚ）の初期値をできるだけ正確に与えてやることが望ましい。
【数６】
0005301762.tif 000007
【００５３】
この時、ラグランジェ乗数μは〔数６〕を〔数４〕に代入することにより次式〔数７〕で求めることができる。
【数７】
0005301762.tif 000008
【００５４】
ところで、〔数６〕で求まるステートＸの最適解は線形の制約関数に対する解であるが、制約条件として基線長情報を用いる場合には、次式〔数８〕に示すように制約関数が非線形となる。
【数８】
0005301762.tif 000009
【００５５】
従って、〔数８〕をテーラーの次近似を用いて〔数９〕のように基線ベクトル推定値廻りで線形化し、反復計算でステートＸの最適解を求める必要がある。
【数９】
0005301762.tif 000010
【００５６】
この場合〔数４〕のＢは次式〔数１０〕から算出できる。
【数１０】
0005301762.tif 000011
【００５７】
なお、ここでは、基線長を制約条件として用いる場合について説明したが、基線長以外の既知情報を制約条件として用いる場合には、適用される既知量ＣとステートＸとの関係式を考えることによって同様に処理することができる。
【００５８】
次に、拡張カルマンフィルタを用いた技法について説明する。
（２）拡張フィルタを用いた技法
公知の拡張カルマンフィルタ（extended Kalman filter）を用いて既知情報制約付きで整数バイアスのフロート解及び基線ベクトルを推定することもできる。なお、拡張カルマンフィルタとは、推定したい状態量の初期値を与えておき、それと観測量の時間履歴を元に状態量の推定値を逐次計算していく手法である。この拡張カルマンフィルタは観測エポック毎の観測量に基づいて、順次、誤差共分散（error covariance）を更新しながらステートを推定できる利点があるので実用に便利である。
【００５９】
拡張カルマンフィルタによる技法においては、〔数２〕のｋ個の観測量に、ＧＰＳアンテナ間の相対的な位置関係より得られる既知情報を擬似観測量として追加することでラグランジェ乗数に等価な既知情報制約付きの推定ができる。この際、〔数２〕のステートＸｔの最適解をより正確且つ高速に推定するためには、基線ベクトルｖ（ｘ，ｙ，ｚ）の初期値をできるだけ正確に与えてやることが望ましい。
【００６０】
この〔数２〕のｋ個の観測量に擬似観測量として追加する擬似観測量ｙｃの観測モデルは次式〔数１１〕で定義することができる。
【数１１】
0005301762.tif 000012
【００６１】
本来νｃは零であるが、擬似観測量と実際の観測量とのウェートを調整するために平均値が零、分散Ｒｃの擬似観測雑音とすることもできる。また、Bは観測マトリクスであり、制約条件とする既知情報を基線長とするときは、前述と同様に、〔数１０〕で決まる観測マトリックスとなる。
【００６２】
このように、整数バイアスフロート解推定部１１は、（１）ラグランジェ乗数或いは（２）拡張カルマンフィルタを用いて、ＧＰＳアンテナ間の相対的な位置関係から得られる既知情報の制約の下で、整数バイアスのフロート解を推定することができる。なお、上述の例では、制約条件として基線長情報のみを用いるものについて説明したが、他の既知情報を制約条件とする場合も同様のことがいえる。もちろん制約条件として課す情報が増えれば増えるほど得られる結果は良くなる。
【００６３】
次に、基線ベクトル算出部１４が行う処理について詳細に説明する。
基線ベクトル算出部１４は、整数バイアス検定部１２で決定された整数バイアス解を既知量として観測方程式〔数２〕から基線ベクトルを求めるものである。具体的には、次の処理を行う。
【００６４】
先ず、整数バイアス検定部１２をパスした整数バイアス解を観測方程式〔数２〕の整数バイアス解（ΔＮ１，ΔＮ２，・・・，ΔＮｋ）に代入する。そして、基線ベクトル算出部１４は、整数バイアス解を既知とした新たな位相差観測量を用いてこの位相差観測方程式から基線ベクトルを求める。この際、推定アルゴリズムには、前述の整数バイアスフロート解推定部１１における処理と同様、ＧＰＳアンテナ間の相対的な位置関係より得られる既知情報が制約条件として直接組み込まれる。なお、当該の既知情報制約付きでステートを推定する具体的な技法としては、例えば、ラグランジェ乗数（Lagrange multiplier）や拡張カルマンフィルタを用いた技法があり、できるだけ正確な基線ベクトルｖ（ｘ，ｙ，ｚ）の初期値を与えることが望ましい。なお、これらの処理内容の詳細については、前述の整数バイアスフロート解推定部１１における処理で説明したものと同様であるため説明を省略する。
【００６５】
以上のように本発明の実施の形態１によるキャリア位相相対測位装置によれば、ＧＰＳアンテナ間の相対的な位置関係より得られる既知情報の制約条件の下で整数バイアスのフロート解の推定を行うことにより、高精度且つ高速に整数バイアスの決定を行うことが可能になる。また、本発明の実施の形態１によるキャリア位相相対測位装置によれば、ＧＰＳアンテナ間の相対的な位置関係より得られる既知情報の制約条件の下で基線ベクトルを求めることにより、高精度且つ高速に基線ベクトルを算出することが可能になる。
【００６６】
なお、本発明の実施の形態１では、整数バイアスフロート解推定部１１による整数バイアスのフロート解の推定処理と、基線ベクトル算出部１４による基線ベクトルの算出処理をともに制約条件を課して行うものについて説明したが、何れか一方の処理のみに制約条件を課して処理してもよい。
【００６７】
（実施の形態２）
前述のように、ラグランジェ乗数や拡張カルマンフィルタを用いて整数バイアスのフロート解や基線ベクトルを正確且つ高速に求めるためには、正確な基線ベクトルの初期値を与えることが望ましい。
【００６８】
そこで、本発明の実施の形態２では整数バイアスのフロート解や基線ベクトルを算出する際に、より正確な基線ベクトルの初期値を与えるための技法を提案する。
【００６９】
（１）移動体速度から算出した擬似姿勢角を用いて基線ベクトルを推定する技法
図３は、移動体速度から算出した擬似姿勢角を用いて基線ベクトルを推定する技法を説明するための図である。
【００７０】
図３において、キャリア位相相対測位装置は、ＧＰＳアンテナ１と、ＧＰＳ受信機２と、演算処理部３ｂとからなる。演算処理部３ｂは、整数バイアスフロート解推定部１１と、整数バイアス決定部１２と、整数バイアス検定部１３と、基線ベクトル算出部１４と、ＧＰＳ姿勢演算部１５と、速度検出部２１と、基線ベクトル初期値演算部２２とから構成されている。なお、本発明の実施の形態１によるキャリア位相相対測位装置と同様の構成要素については同じ符号を付し、ここでは説明を省略する。
【００７１】
速度検出部２１は、航法座標系における移動体速度を検出するものであり、移動体速度は、例えばＧＰＳアンテナ１で受信した側位用信号を用いて算出することが可能である。
【００７２】
基線ベクトル初期値演算部２２は、移動体速度から算出した擬似姿勢角を用いて基線ベクトルを推定し、整数バイアスフロート解推定部１１及び基線ベクトル算出部１４に最適な基線ベクトルの初期値を与えるものである。
【００７３】
基線ベクトル初期値演算部２２において、ある１個の基線ベクトル初期値は次式の関係に基づいて算定できる。
【数１２】
0005301762.tif 000013
【００７４】
ここで、３-２-１軸の回転順序（the３-２-１sequence）で定義された移動体座標系から航法座標系への回転マトリックス（rotation matrix）は次式で表わせる。なお、θｒ，θｐ，θｙはそれぞれ、ロール、ピッチ、ｙａｗである。
【数１３】
0005301762.tif 000014
【００７５】
移動体座標系における基線ベクトルは既知の位置に複数のＧＰＳアンテナ１を設置しているため既知情報である。そのため、θｒ，θｐ，θｙを何らかの方法により算出できれば、航法座標系における基線ベクトルを算出することができる。
【００７６】
ここでは、非特許文献５に開示されているように、θｒ，θｐ，θｙを速度検出部２１で検出された速度から次式〔数１４〕によって算定される擬似姿勢角（pseudo attitude）より求める。ここで、Ｌは速度を微分して求めた加速度と、重力（Gravity）から計算された上昇加速度（Lift acceleration ）であり、Ｐは速度とGravityから計算された水平基準ベクトルである。またＬ・ＰはＬ，Ｐの内積を示す。
【数１４】
0005301762.tif 000015
【００７７】
なお、θｙについては、ジャイロコンパス等の方位センサから情報を得ることも可能である。
【００７８】
（２）慣性センサの観測量及び移動体速度を用いて基線ベクトルを推定する技法
スレーブアンテナを移動体座標系Ｘｂ軸上に設置している場合には、当該基線ベクトルを求めるためのロール、ピッチを慣性センサの観測量から求めることができる。また、ｙａｗは方位検出部出力より求めることもできる。
【００７９】
図４は、慣性センサの観測量及び方位検出部出力を用いて基線ベクトルを推定する技法を説明するための図であり、前記実施の形態１によるキャリア位相相対測位装置にＩＮＳシステムを統合したものを示す。
【００８０】
図４において、キャリア位相相対測位装置は、ＧＰＳアンテナ１と、ＧＰＳ受信機２と、演算処理部３ｂと、ＩＭＵ４とからなる。演算処理部３ｂは、整数バイアスフロート解推定部１１と、整数バイアス決定部１２と、整数バイアス検定部１３と、基線ベクトル算出部１４と、ＧＰＳ姿勢演算部１５と、速度検出部３１と、基線ベクトル初期値演算部３２と、ＩＮＳ航法演算部３３と、ＩＮＳ航法誤差推定部３４とから構成されている。なお、本発明の実施の形態１によるキャリア位相相対測位装置と同様の構成要素については同じ符号を付し、ここでは説明を省略する。
【００８１】
ＩＭＵ４は角速度センサ及び加速度センサから構成される慣性センサであり、ＧＰＳアンテナ１と同様、移動体上にリジットに固定されている。図５は、図１（ａ）の移動体座標系上にマウントされた慣性センサ及びＧＰＳアンテナ位置を示す図である。図５に示すように、ＩＭＵ４は移動体座標系の直交する３軸にそれぞれマウントされた加速度センサと角速度センサから構成され、３軸の各々の軸に対して、各センサの感度軸（sensitive axis）が一致するように、加速度センサ及び角速度センサがそれぞれ１個づつマウントされている。これにより、移動体の前後方向、左右方向、及び上下方向の加速度及び角速度は、慣性センサから得られる観測量をそのまま使用することができる。もっとも、ＩＭＵ４で定義される直交３軸は移動体座標系からの回転量が既知であれば必ずしも一致させる必要はなく、慣性センサの観測量に対して軸回転のための演算処理を行うことにより、移動体の前後方向、左右方向、及び上下方向の加速度及び角速度を得ることができる。なお、慣性センサは、測定の対象となる情報或いは測定精度に応じて必要最小限のセンサが設けられていればよく、必ずしも３軸方向それぞれに加速度センサ及び角速度センサを設ける必要はない。
【００８２】
速度検出部３１は、航法座標系における移動体速度を検出するものであり、移動体速度は、例えばＧＰＳアンテナ１で受信した側位用信号を用いて算出することが可能である。
【００８３】
基線ベクトル初期値演算部３２は、整数バイアスフロート解推定部１１及び基線ベクトル算出部１４に最適な基線ベクトルの初期値を与えるものである。前述のように移動体座標系における基線ベクトルは既知であるため、基線ベクトル初期値演算部３２はθｒ，θｐ，θｙを算出することにより、〔数１２〕、〔数１３〕の関係式から航法座標系における基線ベクトルを求めることができる。なお、移動体の速度ベクトルが移動体座標系Ｘｂ軸と一致する場合、この移動体座標系における基線ベクトル（ｘｂ，ｙｂ，ｚｂ）Ｔは（ｘｂ，０，０）Ｔとすることができるため、〔数１２〕、〔数１３〕の関係式より、ピッチθＰ、方位θｙのみを用いて基線ベクトルを推定できる。
【００８４】
θｒ，θｐはＩＭＵ４からの加速度及び角速度の観測量を受け、直接或いはこれらの観測量に含まれる高周波雑音成分を除去するためのＬＰＦ処理を行った後、ＩＭＵ４観測量及び速度検出部３１出力を用いて後述の〔数１９〕から算出することができる。また、θｙは速度検出部３１出力を用いて後述の〔数２０〕から算出することができる。なお、詳細は後述する。
【００８５】
ＩＮＳ航法演算部３３はＩＭＵ４からの観測量、ＧＰＳ受信機からの位置、速度出力、ＧＰＳ姿勢演算部１４からの姿勢角等を受け、公知の方法で位置、速度、ロール、ピッチ、方位などの航法演算を行う。
【００８６】
ＩＮＳ誤差推定部３４は、慣性センサ誤差補正部に相当し、ＩＮＳ航法演算部３３から得られた位置、速度、姿勢角とＧＰＳ受信機やＧＰＳ姿勢演算部１５から得られたそれらの値との差に基づき、例えば、カルマンフィルタを用いてＩＭＵ４のセンサ誤差や航法誤差を推定し、これらの誤差を負帰還によって除去する。これにより、ＧＰＳの瞬断後の整数バイアス再決定（Ambiguity resolution）時に基線ベクトル初期値演算部２１は校正された慣性センサの観測量を用いてロール、ピッチの算出を算出することができ、より高精度な演算結果を得ることができる。
【００８７】
次に、基線ベクトル初期値演算部２１によるロールθｒ、ピッチθｐの算定処理について詳細に説明する。
【００８８】
地球に対する航法座標系の回転角速度及び慣性座標系に対する地球の回転角速度に関連する加速度は、重力（gravity）に比べ小さいため無視すると、移動体座標系における加速度センサ出力（specific force）Ａｂは次式〔数１５〕で近似できる。ここで、ｇは重力（gravity）であるから既知である。また、移動体座標系における速度成分は、移動体座標系のＸ軸を移動体の進行方向に設定しているため、ｖｂ（ｖｂｘ，０，０）と設定できる。
【数１５】
0005301762.tif 000016
【００８９】
また、３-２-１軸の回転順序（the３-２-１sequence）で定義された航法座標系から移動体座標系への回転マトリックス（rotation matrix）は次式〔数１６〕で表わせる。
【数１６】
0005301762.tif 000017
【００９０】
移動体が急発進や急停止のような高ダイナミックスな運動を行わない場合には運動加速度は無視することができる。もし、運動加速度が無視できない場合でも、前述のように加速度観測量に含まれる高周波雑音成分を除去するためのＬＰＦ処理を行うか、または、低周波数帯域で誤差の小さいＧＰＳ速度観測量の支援情報を用いることにより、信号処理で加速度観測量に含まれる運動加速度を低減することができる。そのため〔数１５〕は次式〔数１７〕のように近似できる。
【数１７】
0005301762.tif 000018
【００９１】
スレーブアンテナを移動体座標系Ｘｂ軸上に設置した場合、移動体座標系における速度ベクトルがｘ成分のみであること、及び航法座標系と移動体座標系における速度ベクトルの大きさが変化しないことから、ｖｂｘは次式〔数１８〕で求めることができる。
【数１８】
0005301762.tif 000019
【００９２】
従って、移動体のロールθｒ、ピッチθｐは、ＩＭＵ４から得られる観測量と速度検出部３１で得られる移動体のＸ軸方向に対する速度情報から次式〔数１９〕のように算出することができる。
【数１９】
0005301762.tif 000020
【００９３】
このように慣性センサから求めたロールθｒ、ピッチθｐは〔数１４〕で求めたロールθｒ、ピッチθｐの代用して使用することもできるが、より良い精度を期待するために併用することもできる。
【００９４】
また、方位θｙは、速度検出部３１出力を用いて次式で求めることができる。
【数２０】
0005301762.tif 000021
なお、θｙについてはジャイロコンパス等の方位センサから情報を得ることも可能である。
【００９５】
このようにして上記（１）、（２）の方法で求めたロールθｒ、ピッチθｐ、方位θｙの誤差は約数度以内である。この場合、１ｍの基線長のとき、基線ベクトルの初期値の誤差は数cm以内の十分な精度で決定できる。
【００９６】
なお、ここでは移動体座標系における移動体の速度Ｖｘｂを考慮してロールθｒを求めるものについて説明したが、移動体座標系における移動体の速度及び運動加速度を無視することによって、移動体の進行方向に対する速度情報なしで移動体のロールθｒを算出することもできる。
【００９７】
以上のように、本発明の実施の形態２によるキャリア位相相対測位装置によれば、整数バイアスフロート解推定部１１及び基線ベクトル算出部１４に対して基線ベクトル初期値演算部２１、３１から正確な基線ベクトルの初期値を与えることにより、整数バイアスフロート解推定部１１及び基線ベクトル算出部１４における演算処理をより正確且つ高速に行うことが可能になる。
【００９８】
なお、前述した実施形態１、２は最良の実施形態の一例であって、本発明の要旨を損なわない範囲で種々の変更が可能であり、本発明は前述した実施形態に限定されるものではない。
【００９９】
（実験結果）
以下、表１、表２に本発明のラグランジェ乗数による基線長制約付き整数バイアスフロート解と基線ベクトル推定特性のシミュレーション実験結果の一例を示す。
【０１００】
（１）整数バイアスフロート解推定部１１の性能評価実験
外部姿勢角支援データが得られると仮定したとき、スレーブアンテナを移動体座標系Ｘｂ軸上に設置した場合の基線ベクトルの初期値は、ピッチθＰ、方位θｙから前述の〔数１２〕より求めることができる。そこで、それぞれ３°、５°、１０°のピッチ／方位誤差を持つ姿勢角から算定した基線ベクトルの初期値に基づいて実験を行った。なお、使用衛星数は５個、アンテナ間距離（基線長）は０．５ｍとした。
【０１０１】
上記条件で、ラグランジェ乗数による基線長制約付技法を用いて整数バイアスのフロート解を求めた結果を表１に示す。
【表１】
0005301762.tif 000022
【０１０２】
表１に示すように、船の静止時はピッチ／方位誤差が３°（σ）、船の動揺時はピッチ／方位誤差が１０°（σ）以内であれば、衛星数５個で３０秒以内に整数バイアス解（整数Ambiguity）をほぼ１００％の確立で直接、整数バイアス解を推定することができた。
【０１０３】
また、静止時においてピッチ／方位誤差が５°、１０°の時には、それぞれ、８９％、７４％と決定率が低下するが、整数バイアス解の真値からの差はせいぜい±１〜±２の誤りであった。
【０１０４】
また、本実験では、船の静止時に比べて動揺時の整数バイアス解（整数Ambiguity）の決定率が高いことが確認され、実用面において大変好ましい結果が得られた。この理由は基線ベクトルの初期値が違っていても船の動揺により基線ベクトルの初期値と接近する、即ち、基線ベクトルの初期値と実際の基線ベクトル位置が一致するタイミングがあるためであると考えられる。
【０１０５】
（２）基線ベクトル算出部１４の性能評価実験
ラグランジェ乗数による基線長制約付技法を用いて基線ベクトルを求めた時の姿勢角換算誤差を表２に示す。なお、使用衛星数は５個、アンテナ間距離（基線長）は０．５ｍとした。
【表２】
0005301762.tif 000023
【０１０６】
表2から分かるように、ラグランジェ乗数による基線長制約付き基線ベクトル推定特性は制約無しに比較して、約３０％程度改善できる。また、マルチパスに対しても約１０％程度の改善が見られた。
【０１０７】
なお、ここではラグランジェ乗数を用いたシミュレーション実験結果を示したが、カルマンフィルタによる特性もほぼ同様の結果が得られた。
【図面の簡単な説明】
【０１０８】
【図１】本発明で使用する航法座標系と移動体座標系について説明するための説明図
【図２】本発明の実施の形態１によるキャリア位相相対測位装置の構成の一例を示すブロック図
【図３】本発明の移動体速度から算出した擬似姿勢角を用いて基線ベクトルを推定する技法を説明するための図
【図４】本発明の慣性センサの観測量及び方位検出部出力を用いて基線ベクトルを推定する技法を説明するための図
【図５】移動体座標系上にマウントされた慣性センサ及びＧＰＳアンテナ位置を示す図
【図６】整数バイアス解の決定処理手順を説明するための説明図
【符号の説明】
【０１０９】
１  ＧＰＳアンテナ
２  ＧＰＳ受信機
３ａ、３ｂ、３ｃ  演算処理部
４  ＩＭＵ
１１  整数バイアスフロート解推定部
１２  整数バイアス決定部
１３  整数バイアス検定部
１４  基線ベクトル算出部
１５  ＧＰＳ姿勢演算部
１６  整数バイアス解算出部
２１、３１  速度検出部
２２、３２  基線ベクトル初期値演算部
３３  ＩＮＳ航法演算部
３４  ＩＮＳ航法誤差推定部
１０１  整数バイアスのフロート解推定処理
１０２  整数バイアス解の決定処理
１０３  整数バイアス解の検定処理
【図１】
0005301762.tif 000024
【図２】
0005301762.tif 000025
【図３】
0005301762.tif 000026
【図４】
0005301762.tif 000027
【図５】
0005301762.tif 000028
【図６】
0005301762.tif 000029