JP2003232845A - 移動体の方位および姿勢検出装置 - Google Patents

Abstract

(57)【要約】 【課題】 移動体に設けた複数のアンテナで、複数の測
位用衛星からの電波を受信し、アンテナの相対位置を求
めることにより、移動体の方位および姿勢角を検出する
装置において、キャリア位相の整数バイアスの決定を速
く、正確に行う。 【解決手段】 受信電波の中断から復帰する時、または
衛星の組み合わせが変更する時等、整数バイアスを求め
直す場合に、これまでの観測で得られた姿勢角および姿
勢角の誤差共分散を用いて、新たに、整数バイアスを決
定する。これにより、連続した高精度な方位および姿勢
角情報を得る。

Description

【発明の詳細な説明】

【０００１】

【発明の属する技術分野】この発明は、航空機や船舶等
の移動体の方位および姿勢を検出する装置に関するもの
である。

【０００２】

【従来の技術】従来、複数の測位用衛星からの電波を、
移動体の所定位置に配置された複数のアンテナで受信
し、キャリア信号の位相をそれぞれ観測し、複数のアン
テナのうち１つのアンテナを基準として他のアンテナの
相対位置を求めることによって、受信アンテナの相対位
置関係を求め、これによって移動体の方位および姿勢を
求める装置が用いられている。

【０００３】一方、レートジャイロ等の角速度を検出す
る角速度センサを用いて移動体の姿勢を測定するように
した装置も用いられている。

【０００４】前述の測位用衛星からの電波を用い、移動
体の方位および姿勢を求める装置では、キャリア位相の
整数バイアスを如何に高精度に決定するかという、重要
な技術要素が存在する。

【０００５】このような測位用衛星からの電波を用いた
装置では、移動体外部の障害物により、測位用衛星から
の電波が遮断され、キャリア位相を観測できなくなり、
方位および姿勢（以下、両方を含めて単に「姿勢角」と
いう。）を推定し続けることができなくなってしまう。
この場合、前述の角速度センサを用いた装置の情報をも
とに、姿勢角を推定していくことになる。

【０００６】しかし、移動体が測位用衛星からの電波を
受信できるようになると、再度測位用衛星からの電波を
もとにキャリア位相の整数バイアスの再決定の処理を始
めるのであるが、再決定開始までに時間がかかるため、
即時に有効な姿勢角を得ることができなかった。

【０００７】例えば、基線長が３ｍ、測位用衛星からの
電波中断時間が１分間の場合では、中断後の初期整数バ
イアス決定時間が、平均で約１分〜２分、最大で約１５
分もかかってしまう。

【０００８】この問題を解決するために、角速度センサ
により推定された姿勢角を利用し、電波復帰後に、即時
にキャリア位相の整数バイアスを決定する装置が考案さ
れている。

【０００９】この装置では、図６に示すようなフローに
もとづき、電波復帰後のキャリア位相の整数バイアスを
決定する。

【００１０】図６は、従来の装置における、電波復帰後
の整数バイアス決定のフローチャートである。

【００１１】図６に示すように、電波中断中に角速度セ
ンサにより推定し続けた姿勢角をもとに、基線ベクトル
を計算する。次に、基線ベクトルからfloating ambigui
tyを計算し、このfloating ambiguityを四捨五入等で、
その小数点以下を丸め込みし、整数化することにより、
整数バイアスの候補値を作成する。この整数バイアスの
候補値を検定し、問題なければ整数バイアスとして決定
する。この方法を用いると、前述の整数バイアス決定時
間が、平均で約８秒、最大で約３０秒に短縮することが
できる。

【００１２】

【発明が解決しようとする課題】しかし、このような従
来の姿勢角検出装置においては、以下に示す解決すべき
課題が存在した。

【００１３】すなわち、従来の姿勢角検出装置では、測
位用衛星からの電波が中断する時間が、条件により異な
るが１分〜５分程度以上になってしまうと、姿勢角の推
定精度が劣化してしまう。これは、主に、角速度センサ
における角速度センサ誤差の推定誤差に大きく影響され
る。このため、このような誤差を含んだ姿勢角推定値か
ら、計算した整数バイアス候補値は、真の整数バイアス
と一致しなくなる可能性がある。なお、この可能性は、
基線長（移動体に設置された複数のアンテナ間の距離）
が長くなるほど、すなわち基線ベクトルの大きさが大き
くなるほど、また中断時間が長くなるほど増加する。

【００１４】このように整数バイアス候補値の決定を誤
り、誤った姿勢角を出力する可能性がある。

【００１５】例えば、基線長が３ｍ、測位用衛星からの
電波中断時間が１分間での、算出された整数バイアスの
不正解率は、１％程度にもなってしまう。

【００１６】この発明の目的は、測位用衛星からの電波
中断後にも、即時に、正確な整数バイアスを算出し、こ
れをもとに、移動体の方位および姿勢を、高精度に且つ
速やかに検出する装置を構成することにある。

【００１７】

【課題を解決するための手段】この発明は、１重位相差
または２重位相差を観測し、これらの位相差の整数バイ
アスを決定する手段と、整数バイアスの決定された位相
差から相対位置を求める手段と、移動体の姿勢角と該姿
勢角の誤差共分散とを求める手段と、整数バイアスを再
決定する場合に、姿勢角と姿勢角の誤差共分散とから、
１重位相差または２重位相差による、少なくとも一つの
整数バイアスの候補値を算出し、該候補値から整数バイ
アスを決定する手段とを備える。これにより、図７に示
すように、姿勢角、基線ベクトル、およびfloating amb
iguityのそれぞれで、姿勢角の推定値とともにその誤差
共分散とを算出し、これらをもとに複数の整数バイアス
候補値を算出し、検定することにより、一つの整数バイ
アス値を決定し、姿勢角を出力する。

【００１８】また、この発明は、１重位相差または２重
位相差を観測し、これらの位相差の整数バイアスを決定
する手段と、整数バイアスの決定された位相差から相対
位置を求める手段と、移動体の姿勢角と該姿勢角の誤差
共分散とを求め、姿勢角の誤差共分散を補正する手段
と、整数バイアスを再決定する場合に、姿勢角と姿勢角
の誤差共分散とから、１重位相差または２重位相差によ
る、少なくとも一つの整数バイアスの候補値を算出し、
該候補値から整数バイアスを決定する手段とを備える。
これにより、１重位相差または２重位相差から整数バイ
アスが決定できなくなった時点での、姿勢角と、所定の
補正項により補正された姿勢角の誤差共分散を用いて、
整数バイアスを再決定する。

【００１９】また、この発明は、整数バイアスを再決定
する場合に、姿勢角と姿勢角の誤差共分散とから整数バ
イアスを決定する手段と、１重位相差または２重位相差
から位相差の整数バイアスを決定する手段とを併用また
は、いずれかを選択して用いるようにすることで、移動
体の置かれている状況に応じて、整数バイアスを再決定
する。

【００２０】また、この発明は、姿勢角を求める手段
を、角速度センサと、該角速度センサの出力により、移
動体の姿勢角を算出する手段とから構成する。

【００２１】また、この発明は、姿勢角と姿勢角の誤差
共分散とを求める手段を、移動体の置かれている状態に
応じて仮定したモデルに基づいて予測する手段により構
成する。これにより、移動体の置かれている状況に応じ
て、最適な仮定モデルを適用して、整数バイアスを再決
定する。

【００２２】また、この発明は、floating ambiguityと
floating ambiguityの誤差共分散とを算出する手段を備
え、ＬＡＭＢＤＡ法を用いた決定手段を備えることによ
り、整数バイアスの推定値と推定誤差共分散とを算出
し、整数バイアスの候補値を複数出力する。

【００２３】

【発明の実施の形態】まず、アンテナ座標系とローカル
座標系について、および両者の座標変換について、図１
を参照して説明する。図１の（ａ）はローカル座標系を
表し、基準アンテナの位置を原点として、例えば、ｘ軸
を北、ｙ軸を東、ｚ軸を鉛直方向に採っている。

【００２４】図１の（ａ）において、（ｘ₁ ⁿ ，
ｙ₁ ⁿ ，ｚ₁ ⁿ ）は第２アンテナの位置、（ｘ₂ ⁿ ，ｙ
₂ ⁿ ，ｚ₂ ⁿ ）は第３アンテナの位置、Ｌ₁ ⁿ は第１ア
ンテナから第２アンテナへの基線ベクトル、Ｌ₂ ⁿ は第
１アンテナから第３アンテナへの基線ベクトルである。
なお、座標値と基線ベクトルの右肩の添字ｎはローカル
座標系での座標値および基線ベクトルであることを表
す。

【００２５】図１の（ｂ）はアンテナ座標系を表し、基
準アンテナの位置を原点として、例えば、ｘ軸を方位
（機首または船首の方向）に採り、ｙ軸をｘ軸方向に直
交する水平方向に採り、ｚ軸をｘ，ｙ軸のそれぞれに直
交する方向に採っている。

【００２６】図１の（ｂ）において、（ｘ₁ ^a ，
ｙ₁ ^a ，ｚ₁ ^a ）は第２アンテナの位置、（ｘ₂ ^a ，ｙ
₂ ^a ，ｚ₂ ^a ）は第３アンテナの位置、Ｌ₁ ^a は第１ア
ンテナから第２アンテナへの基線ベクトル、Ｌ₂ ^a は第
１アンテナから第３アンテナへの基線ベクトルである。
なお、座標値と基線ベクトルの右肩の添字ａはアンテナ
座標系での座標値および基線ベクトルであることを表
す。

【００２７】前記アンテナ座標系からローカル座標系へ
の座標変換の変換式は、（１）式で表される。

【００２８】

【数１】

【００２９】ここで、Ｌ₃ ⁿ はＬ₁ ⁿ とＬ₂ ⁿ との外積
である。また、Ｌ₃ ^a はＬ₁ ^a とＬ ₂ ^a との外積であ
る。

【００３０】ＧＰＳで計測されるローカル座標系に対す
るアンテナ座標系の姿勢角を、或る軸回転順序で定義さ
れたオイラー角（θ_r ，θ_p ，θ_y ）で表現すると、
（１）式における座標変換行列（オペレータ）Ｃ_a ⁿ は
次式で表される。

【００３１】

【数２】

【００３２】ここで、θ_r はロール角、θ_p はピッチ
角、θ_y はヨーイング角ということができる。なお、Ｇ
ＰＳによる基線ベクトルは、ローカル座標系で得られる
ので、姿勢角θｙは、真北基準の方位として得られる。
また、θ_r ，θ_p は水平面を基準とする姿勢角として得
られる。なお、以降、（θ_r ，θ_p ，θ_y ）をＧＰＳ姿
勢角と呼ぶ。

【００３３】前記、三つの姿勢角はそれぞれ次の関係か
ら求められる。

【００３４】

【数３】

【００３５】

【数４】

【００３６】

【数５】

【００３７】ここで、（１）式において、（Ｌ₁ ^a ，Ｌ
₂ ^a ，Ｌ₃ ^a ）は既知であるので、（Ｌ₁ ⁿ ，Ｌ₂ ⁿ ，
Ｌ₃ ⁿ ）をキャリア位相差観測量により求めると、Ｃ_a
ⁿ が求まる。Ｃ_a ⁿ が求まると、（３）〜（５）式で姿
勢角が求まる。したがって、姿勢角の計測精度は、基線
ベクトルＬ₁ ⁿ ，Ｌ₂ ⁿ の算出精度、すなわち
（ｘ₁ ⁿ ，ｙ₁ ⁿ ，ｚ₁ ⁿ ），（ｘ₂ ⁿ ，ｙ₂ ⁿ ，ｚ₂
ⁿ ）の算出精度によって決定される。そのため、通常は
擬似雑音コードによる測位ではなく、キャリア位相差を
観測して相対位置を高精度に計測する。その際、１重位
相差方式、２重位相差方式のいずれでも可能であるが、
以下２重位相差について述べる。

【００３８】２重位相差方式による観測方程式は次式で
与えられる。

【００３９】

【数６】

【００４０】ここで、 ｍ：衛星番号 ｋ：衛星数 ｊ：基準ベクトルの番号 λ：受信信号のキャリア波長 ▽Δρ_j ^m ：基準衛星ｓａｔ１と衛星ｓａｔｍ＋１（ｍ
＝１，２，３）に対する基線ベクトルＬ_j ⁿ の２重距離
差 ▽Δφ_j ^m ：同上に対する２重位相差観測量 ▽ΔＮ_j ^m ：同上に対する２重位相差の整数バイアス ▽Δｅ_j ^m ：同上に対する２重位相差の観測雑音 この２重位相差を図式化したものを図２に示す。図２に
おいて、各記号の意味は次の通りである。 Δρ_j ^m+1 ：衛星ｓａｔｍ＋１に対する基線ベクトルＬ
_j ⁿ の１重距離差 Δφ_j ^m+1 ：衛星ｓａｔｍ＋１に対する基線ベクトルＬ
_j ⁿ の１重位相差観測量 ΔＮ_j ^m+1 ：衛星ｓａｔｍ＋１に対する基線ベクトルＬ
_j ⁿ の１重位相差の整数バイアス Δｅ_j ^m+1 ：衛星ｓａｔｍ＋１に対する基線ベクトルＬ
_j ⁿ の１重位相差の観測雑音 また、１重位相差と２重位相差のそれぞれを表す、各変
数の関係は次の通りである。 ▽Δρ_j ^m ＝Δρ_j ^m+1 −Δρ_j ¹ ▽Δφ_j ^m ＝Δφ_j ^m+1 −Δφ_j ¹ ▽ΔＮ_j ^m ＝ΔＮ_j ^m+1 −ΔＮ_j ¹ ▽Δｅ_j ^m ＝Δｅ_j ^m+1 −Δｅ_j ¹ ２重位相差▽Δρ_j ^m は、方向余弦差行列と基線ベクト
ルとの積であるので、▽Δρ_j ^m ＝ΔＡ_j ^m Ｌ_j ⁿ （ｊ
＝１，２）で表される。ここで、ΔＡ_j ^m ，Ｌ_j ⁿ とも
に、（ｘ₁ ⁿ ，ｙ₁ ⁿ ，ｚ₁ ⁿ ）の関数である。したが
って、（６）式より、未知数は、Ｌ_j ⁿ すなわち（ｘ_j
ⁿ ，ｙ_j ⁿ ，ｚ_j ⁿ ）となる。いま、前もって何らかの
方法で整数バイアスを求めれば、未知数は（ｘ_j ⁿ ，ｙ
_j ⁿ ，ｚ_j ⁿ ）の３個であるので、少なくとも３個の観
測量▽Δφ_j ^m が得られると、（ｘ_j ⁿ ，ｙ_j ⁿ ，ｚ_j
ⁿ ）を求めることができる。

【００４１】いま、図１のように定義されたローカル座
標系とアンテナ座標系における任意の基線ベクトルＬの
座標ベクトルをそれぞれＸⁿ ，Ｘ^a とし、座標変換行列
（３×３）をＣ_a ⁿ とすると、これらの関係は、

【００４２】

【数７】

【００４３】したがって、Ｘⁿ の誤差共分散Ｑｘ＝Ｅ
［δｘ（δｘ）^T ］は、次式で表される。ここで、δｘ
はＸⁿ の誤差を表し、Ｅ［・］は平均操作を表し、
（・）^T は転置行列を表すものとする。

【００４４】

【数８】

【００４５】ここで、

【００４６】

【数９】

【００４７】基線ベクトル位置の誤差は前述のδｘであ
り、そのδｘを衛星視野方向に射影した誤差をδρとす
ると、δｘとδρとの関係は次式で表される。

【００４８】

【数１０】

【００４９】ここで、ΔＡは２重位相差における衛星へ
の方向余弦差行列である。また、２重位相差の誤差をδ
（▽Δφ）、整数バイアスの誤差をδ（▽ΔＮ）とする
と、δρ，δ（▽Δφ）およびδ（▽ΔＮ）の間には、
次式の関係が成り立つ。

【００５０】

【数１１】

【００５１】したがって、基線ベクトル位置誤差と整数
バイアス誤差との関係は、（１０）、（１１）式より次
式で表される。

【００５２】

【数１２】

【００５３】そこで、２重位相差の誤差共分散をＱ▽Δ
φ、整数バイアスの誤差共分散をＱ▽ΔＮとすると、２
重位相差誤差と整数バイアス誤差とが無相関であるた
め、（１２）式より、次式が成り立つ。

【００５４】

【数１３】

【００５５】また、（８），（１３）式によって、姿勢
角誤差と整数バイアス誤差の関係は次式で表される。

【００５６】

【数１４】

【００５７】ここで、整数バイアスが決定している場合
には、整数バイアス誤差δ（▽ΔＮ）は０であるから、
基線ベクトル位置誤差δｘは、（１０），（１１）式よ
り次式で表される。

【００５８】

【数１５】

【００５９】また、（８）式より、基線ベクトルの誤差
共分散Ｑｘと姿勢角の誤差共分散Ｑθとの関係は次式で
表される。

【００６０】

【数１６】

【００６１】したがって、（１５），（１６）式より、
姿勢角の誤差共分散Ｑθは次式で表される。

【００６２】

【数１７】

【００６３】また、Ｈに逆行列Ｈ^-1が存在する場合に
は、（１７）式は次式と等しくなる。

【００６４】

【数１８】

【００６５】ここで、２重位相差の誤差共分散Ｑ▽Δφ
は、例えば、次に示す方法で予め見積もっておくことが
できる。

【００６６】装置を固定しておいて、基線ベクトルを実
測し、観測データ（２重位相差と方向余弦差行列）を記
録する。次に、実測した基線ベクトルと方向余弦差行列
から計算できる２重位相差を真値とみなし、その値と観
測した２重位相差の誤差の分散を計算する。この誤差の
分散を対角要素とする行列を、前述の２重位相差の誤差
共分散Ｑ▽Δφとする。

【００６７】したがって、整数バイアスが決定している
場合には、（３），（４），（５）式から姿勢角を、
（１７）式から姿勢角の誤差共分散をそれぞれ求めるこ
とができる。

【００６８】さらに、角速度センサと統合することで、
姿勢角をより高精度に求めることができる。したがっ
て、角速度センサを利用しない場合と比較して、より正
確な姿勢角と、より小さい誤差共分散を得ることができ
る。

【００６９】また、測位用衛星からの電波が建物等で遮
断されて、中断した場合（以下「ＧＰＳ中断」とい
う。）には、姿勢角とその誤差共分散を、（３），
（４），（５），（１７）式から求めることは出来なく
なるが、角速度センサから得られる角速度を用いて、ス
トラップダウン演算を行うことにより得られる。

【００７０】次に、姿勢角の誤差共分散は、ＧＰＳ中断
直前に（１７）式から求められていた姿勢角の誤差共分
散の対角要素に、ＧＰＳ中断の継続時間に所定の定数β
を掛けた値からなる補正項を加えることにより得られ
る。ここで、βは角速度センサ誤差と角速度センサオフ
セットバイアス推定誤差と更新周期等によって決定され
る設定値である。

【００７１】また、角速度センサを利用しなくても、Ｇ
ＰＳ中断が発生した場合に、例えば、次のような方法
で、姿勢角とその誤差共分散とを予測することができ
る。

【００７２】まず、ＧＰＳ中断直前に、（３），
（４），（５）式から求められていた姿勢角が、ＧＰＳ
中断直後にも変化していないと仮定して、その姿勢角を
姿勢角予測値とする。

【００７３】次に、ＧＰＳ中断直前に（１７）式から求
められていた姿勢角の誤差共分散の対角要素に、ＧＰＳ
中断の継続時間に所定の定数γを掛けた値を加えること
で、姿勢角の誤差共分散の予測値が得られる。ここで、
γは角速度の大きさと発生頻度等によって決定される設
定値である。このような補正を行うことにより、適当な
姿勢角の誤差共分散を得ることもできる。

【００７４】サイクルストリップ等の発生により、整数
バイアスを再決定する必要が生じた場合には、姿勢角か
ら求まるfloating ambiguityの標準偏差から、整数バイ
アス候補点を作成し、その候補点から最も確かであろう
候補点を選定して整数バイアスに決定する。これによ
り、姿勢角から求まるfloating ambiguityを丸め込みに
よって整数化した場合よりも、誤りを少なくして、整数
バイアスを決定することができる。

【００７５】なお、前述の説明は、アンテナが３個ある
場合であるが、アンテナが２個の場合、すなわち第１、
第２のアンテナのみの場合には、（１），（３），
（４），（５），（９）式を、それぞれ以下のように書
き換えることにより、同様に演算することができる。

【００７６】まず、アンテナ座標系からローカル座標系
への座標変換の変換式は、基線ベクトルが一つになるの
で、

【００７７】

【数１９】

【００７８】ここで、ｄは基線ベクトルＬ₁ ^a の長さで
ある。

【００７９】また、アンテナが２個になることにより、
ロール角θ_r はなくなり、ピッチ角θ_p 、ヨーイング角
θ_y はそれぞれ、次式で表される。

【００８０】

【数２０】

【００８１】

【数２１】

【００８２】また、誤差分散行列Ｑｘ，Ｑθ，単位方向
余弦差行列Ｈは、次式で表される。

【００８３】

【数２２】

【００８４】次に、姿勢角からfloating ambiguityの演
算は以下のようにして求められる。

【００８５】アンテナ座標系における基線ベクトルＬ_j
^a （ｊ＝１，２）の位置（ｘ_j ^a ，ｙ_j ^a ，ｚ_j ^a ）は
既知であるから、角速度センサから得られた姿勢角を用
いて、（２），（７）式より、ローカル座標系における
基線ベクトルＬ_j ⁿ （ｊ＝１，２）の位置（ｘ_j ⁿ ，ｙ
_j ⁿ ，ｚ_j ⁿ ）を求める。

【００８６】この（ｘ_j ⁿ ，ｙ_j ⁿ ，ｚ_j ⁿ ）を用い
て、次式より各衛星における１重距離差Δρ_j ^m を逆算
する。

【００８７】

【数２３】

【００８８】ここで、Ａ_j ^m は基線ベクトルＬ_j ⁿ のｍ
衛星への視野方向ベクトルであるから、ローカル座標系
における各衛星位置（ｘ_m ⁿ ，ｙ_m ⁿ ，ｚ_m ⁿ ）と（ｘ
_j ⁿ ，ｙ_j ⁿ ，ｚ_j ⁿ ）より決定できる。なお、ｍは使
用衛星番号、ｊは基線ベクトルの番号である。

【００８９】１重位相差の観測方程式は次式で表される
から、１重位相差Δφ_j ^m を観測量とすると、ｍ衛星に
対する１重位相差のfloating ambiguityΔｎ_j ^m が、次
式から計算できる。

【００９０】

【数２４】

【００９１】また、１重位相差のfloating ambiguityの
誤差共分散ＱΔｎは、１重位相差の誤差共分散ＱΔφと
すると、（１４）式から、Ｑ▽ΔＮをＱΔｎに、Ｑ▽Δ
φをＱΔφに、ΔＡをＡに置き換えた次式により、姿勢
角の誤差共分散Ｑθとの関係が導かれる。

【００９２】

【数２５】

【００９３】ここで、姿勢角の誤差共分散Ｑθは、前述
の（１７）式を用いて計算できるため、（２５）式によ
り、floating ambiguityの誤差共分散ＱΔｎを算出する
ことができる。

【００９４】また、floating ambiguityの標準偏差σΔ
ｎは、floating ambiguityの誤差共分散ＱΔｎの対角要
素の１／２乗として求めることができる。

【００９５】このように、floating ambiguityΔｎとfl
oating ambiguityの誤差共分散ＱΔｎとが求まることに
より、floating ambiguityΔｎに、この標準偏差σΔｎ
を加えた値と、floating ambiguityΔｎからこの標準偏
差σΔｎを引いた値との間にある整数を、１重位相差の
整数バイアス候補点として算出することができる。

【００９６】なお、１重位相差のfloating ambiguityΔ
ｎとその誤差共分散ＱΔｎとを用いて、整数バイアスを
決定する方法として、前述の方法とは別に、ＬＡＭＢＤ
Ａ法を用いる方法もある。

【００９７】ＬＡＭＢＤＡ法では、次式を用いて、整数
バイアス候補点を算出する。

【００９８】

【数２６】

【００９９】ここで、ΔＮは整数バイアス候補点、Δｎ
はfloating ambiguity 、ＱΔｎはfloating ambiguit
y の誤差共分散、χ² はノルムの閾値である。

【０１００】すなわち、floating ambiguity Δｎと、
floating ambiguity の誤差共分散ＱΔｎとを用い、整
数バイアス候補点ΔＮを代入し、この演算値がノルムの
閾値χ² 以下であった場合に、整数バイアス候補点ΔＮ
が真であるとみなし、その結果を算出する。ここで、ノ
ルムの閾値χ² は、実機ではサーチボリュームにより設
定する。

【０１０１】なお、２重位相差方式の場合には、前述の
（６）式より、２重位相差floatingambiguity▽Δｎを
用いて計算できる。したがって、１重位相差方式、２重
位相差方式のいずれにも適用できる。

【０１０２】次に、移動体の姿勢角検出装置の全体の構
成を、図３および図４を参照して説明する。

【０１０３】図３は装置のブロック図である。ｓａｔ
１，ｓａｔ２，・・・，ｓａｔＮはＧＰＳ測位用衛星で
あり、複数のアンテナＡＮＴ１，ＡＮＴ２を備えるＧＰ
Ｓアンテナはこれら複数の測位用衛星からの電波を受信
し、中間周波信号に変換するとともに増幅する。ＧＰＳ
受信機はそれぞれのアンテナで受信した信号から、それ
ぞれのアンテナの位置およびキャリア位相差を演算し、
各衛星位置情報とともに、姿勢演算処理部へ所定の間隔
で出力する。測位により求められるアンテナ位置や衛星
のエフェメリス情報等はＧＰＳの座標系に従っているの
で、ＧＰＳ受信機または姿勢演算処理部のいずれかで、
ローカル座標系に変換する。以下の説明では、ＧＰＳ受
信機で変換するものとする。

【０１０４】姿勢演算処理部における衛星計画部は、衛
星のエフェメリス情報とアンテナ位置とから、使用する
衛星を選択し、その衛星位置や選択された衛星の衛星情
報を位相差演算部へ与える。この位相差演算部は、衛星
計画部から受け取った情報とＧＰＳ受信機から受け取っ
たキャリア位相差信号から、１重（または２重）位相差
の観測量を算出し、整数バイアス決定処理部へ出力す
る。整数バイアス決定処理部では、該当衛星に対する１
重（または２重）位相差の整数バイアスを決定し、その
妥当性を確認した後に、整数バイアス情報をＧＰＳ姿勢
演算部へ与える。ＧＰＳ姿勢演算部では、整数バイアス
と、位相差演算部から出力されたキャリア位相差観測量
とを用いて、公知の方法により、ローカル座標系におけ
る基線ベクトルを算出する。これと、既知のアンテナ座
標系における基線ベクトルより、（１）〜（５）式を用
いて姿勢角を算出する。以下、ＧＰＳにより求めた姿勢
角を「ＧＰＳ姿勢角」という。

【０１０５】また、角速度センサ部は、直交３軸回りの
角速度を検出するレートジャイロと、それらの出力を増
幅するＸ軸増幅器、Ｙ軸増幅器、Ｚ軸増幅器を備えてお
り、姿勢角演算処理部の角速度姿勢演算部に対して、各
軸回りの角速度を与える。

【０１０６】具体的には、地球の赤道平面と春分点子午
面との交線をＸ軸、これに直交する東向きをＹ軸とする
右手系の慣性座標軸基準に対する角速度センサ座標軸の
角速度ω^s _isを与える。いま、必要なのは、ローカル座
標系に対する角速度センサ座標軸の角速度ω^s _nsであ
る。ω^s _isとω^s _nsとの関係は、 ω^s _ns＝ω^s _is−ω^s _in である。ここで、ω^s _inは慣性座標軸系基準に対するロ
ーカル座標系の角速度を、角速度センサ座標軸系で観測
した角速度である。船舶のような低速度移動体の場合で
は、ω^s _is＞＞ω^s _inが成立するから、ω^s _inは無視で
きる。航空機のような高速移動体では、移動体の位置お
よび周知の方法により、ω^s _nsを計算する。

【０１０７】角速度姿勢演算部では、周知のオイラー方
程式またはＧｉｌｍｏｒｅのアルゴリズムによって、姿
勢角を算出する。以下、この姿勢角を「角速度姿勢角」
という。

【０１０８】角速度／ＧＰＳ統合処理演算部では、角速
度姿勢角とＧＰＳ姿勢角とを統合化して、最終的にロー
ル、ピッチ、ヨーイングの各姿勢角を出力する。

【０１０９】図４は、図３に示した姿勢演算処理部にお
ける再初期化判定部および角速度／ＧＰＳ統合化処理演
算部等での処理手順を示すフローチャートである。

【０１１０】まず、測位用衛星からの電波を受信すると
（ｎ１）、測位に用いる衛星の選択、および選択した衛
星からの電波を受信して求めた観測データを取り込む
（ｎ２）。初期状態で整数バイアスが決定されていなけ
れば、まず各アンテナについて基線ベクトルを推定し、
floating ambiguityを求める。この推定には、例えばカ
ルマンフィルタまたは最小２乗法等を用いる。続いて、
公知の方法により、floating ambiguityから整数値を導
き出すことにより、整数バイアスを決定する（ｎ３→ｎ
５→ｎ６→ｎ７→ｎ６・・・）。

【０１１１】その後、χ² 検定および基線長が実際の値
に一致しているか否か等によって、整数バイアス推定の
妥当性を検定する（ｎ８）。もし、検定結果が妥当でな
ければ、再び整数バイアスの決定処理を行う（ｎ９→ｎ
６・・・）。検定結果が妥当とみなされれば、求められ
た整数バイアスを既知として、基線ベクトルを決定し、
これをもとに各アンテナの相対位置からＧＰＳ姿勢角を
算出する（ｎ１０）。続いて、このＧＰＳ姿勢角を算定
した同時刻における角速度センサの３軸回りの角速度か
ら角速度姿勢角を算出する（ｎ１１）。

【０１１２】その後、ＧＰＳ姿勢角と角速度姿勢角との
統合化処理を行い、統合化された姿勢角を出力する（ｎ
１２→ｎ１３）。すなわち、ＧＰＳ姿勢角を角速度姿勢
角のストラップダウンの初期値とすることによって、角
速度姿勢角をＧＰＳ姿勢角と同じローカル座標系の姿勢
角に一致させる。また、ＧＰＳ姿勢角と角速度姿勢角と
の差から角速度センサの誤差推定を行う。また、姿勢角
の誤差共分散を、（１７）式と（２５）式とを用いて求
める。

【０１１３】統合化処理の計算方法の例としては、姿勢
角をステートとするカルマンフィルタを用いて、姿勢角
と姿勢角の誤差共分散を計算する方法等がある。

【０１１４】前記ＧＰＳ姿勢角はローカル座標系を基準
とするアンテナ座標軸の姿勢角であり、角速度姿勢角は
ローカル座標系を基準とする角速度センサ座標軸の姿勢
角である。このアンテナ座標軸と角速度センサの座標軸
とが正確に一致するように、アンテナと角速度センサと
を移動体に装着するのが望ましいが、現実には困難であ
るため、座標軸回転の演算処理によって、この座標軸の
ずれ分を補正する。

【０１１５】また、角速度センサの誤差推定では、角速
度姿勢角とＧＰＳ姿勢角との差の情報を用いて、角速度
センサ誤差であるバイアス、ドリフト誤差等を推定し、
これらを角速度姿勢角から除去する。

【０１１６】次に、姿勢角演算処理部における再初期化
判定部で、サイクルストリップが生じたか否かを判定し
（ｎ１３→ｎ１４）、サイクルストリップが生じたこと
を検出すれば、前記のように算出された姿勢角と姿勢角
誤差共分散とを用いて、整数バイアスを決定し、基線ベ
クトルを推定する（ｎ１５→ｎ１６→ｎ１７→ｎ１８→
ｎ４）。

【０１１７】再初期化信号を受け、姿勢角および姿勢角
誤差共分散から整数バイアスを決定する手順を示したフ
ローチャートを図５に示す。

【０１１８】再初期化信号を受信すると、姿勢角および
姿勢角誤差共分散から整数バイアスの決定を開始する
（ＳＴＡＲＴ）。ここで、整数バイアス候補点があれ
ば、これらを検定により絞り込む（ｎ５１→ｎ５５）。
一方、整数バイアス候補点が無ければ、次に示す方法で
候補点を作成し、検定により絞り込む。

【０１１９】前述のように、姿勢角が分かれば、基線ベ
クトルを算出することができるため、これをもとに、fl
oating ambiguityを計算する（ｎ５２）。このfloating
ambiguity、この誤差共分散、およびこの標準偏差をも
とに整数バイアス候補点を少なくとも一つ作成し、前述
のような各検定を行う（ｎ５３→ｎ５４）。検定によ
り、候補点が一つに絞られれば、これを整数バイアスと
して決定し（ｎ５６→ｎ５７）、候補点が一つに絞り込
まれるまで、再度、整数バイアスの作成および検定を行
う（ｎ５６→ｎ５１・・・）。

【０１２０】ところで、図４における、前述の再初期化
を行う過程において、ＧＰＳ測位用衛星からの電波を受
信した場合には、姿勢角を利用して整数バイアスを決定
するが、ＧＰＳ測位用衛星からの電波が受信できなけれ
ば、受信中断直前のＧＰＳ姿勢角を角速度姿勢角のスト
ラップダウンの初期値にすることにより、姿勢角を算出
し、出力する（ｎ１６→ｎ１１→ｎ１２→ｎ１３）。姿
勢角の誤差共分散については、必要に応じて前述の補正
を行い利用する。

【０１２１】また、ＧＰＳ測位用衛星からの電波を受信
して、姿勢角データを利用した場合には、利用しない場
合に比較して速く整数バイアスを決定できるが、ＧＰＳ
中断時間が長くなる（例えば１５分以上等）と、姿勢角
の誤差共分散が大きくなる。この誤差共分散を利用する
と、利用しない場合よりも、整数バイアスの決定に時間
を要してしまう現象が生じる。また、このように整数バ
イアスを求めると、誤った整数バイアスを決定してしま
う可能性が増大する。このため、再初期化後の整数バイ
アスの決定に、姿勢角および姿勢角の誤差共分散を用い
るかどうかを選択し、整数バイアスを決定する（ｎ１
７）。

【０１２２】この選択方法としては、ＧＰＳ中断時間を
判断基準とし、所定時間以上ＧＰＳ測位用衛星からの電
波を受信できなかった場合には、姿勢角等を利用せず、
所定時間内であれば、姿勢角等を利用する方法がある。
また、姿勢角の誤差共分散に閾値を設け、閾値以下では
姿勢角等を利用し、閾値より大きければ利用しない方法
もある。また、姿勢角等を利用するフローと姿勢角等を
利用しないフローの両方で整数バイアスを決定し、いず
れか速い方を適用する方法や、姿勢角等を利用するフロ
ーと姿勢角等を利用しないフローとを交互に行う方法も
ある。

【０１２３】姿勢角および姿勢角の誤差共分散を用いる
ことにより、前述の従来技術で示した条件で、整数バイ
アスを決定した場合に、整数バイアスの間違いは無くな
った。

【０１２４】以上のように、ＧＰＳ中断後の復帰時な
ど、再初期化が必要となった場合に、姿勢角と姿勢角の
誤差共分散とを利用する等して、整数バイアスを、速
く、正確に決定することができる。

【０１２５】また、姿勢角の誤差共分散が大きくなった
場合においても、姿勢角等を用いなかったり、適当な補
正項を加えて演算することにより、整数バイアスを、速
く、正確に決定することができる。

【０１２６】また、状況に応じて、姿勢角および姿勢角
の誤差共分散を用いないことにより、速く、正確に、整
数バイアスを決定することができる。

【０１２７】

【発明の効果】この発明によれば、姿勢角および姿勢角
の誤差共分散を利用することにより、アンテナ相対位置
を測定する際の１重位相差または２重位相差の整数バイ
アスを、ＧＰＳ中断後に再決定する場合に、速く、正確
に決定することができる。

【０１２８】また、この発明によれば、姿勢角および姿
勢角の誤差共分散に所定の補正を行うことにより、いろ
いろな状況においても、速く、正確に、整数バイアスを
決定することができる。

【０１２９】また、この発明によれば、ＧＰＳ中断時間
等を基準に、姿勢角および姿勢角の誤差共分散を利用し
ないフローを用いることにより、状況に応じて、違うフ
ローを用い、速く、正確に整数バイアスを決定すること
ができる。

【０１３０】また、この発明によれば、姿勢角と姿勢角
の誤差共分散とを求める手段として、角速度センサを用
いたり、移動体の状況に応じて仮定したモデルを用いる
ことにより、いろいろな状況においても、速く、正確
に、整数バイアスを決定することができる。

【図面の簡単な説明】

【図１】ローカル座標系とアンテナ座標系におけるアン
テナ配置の例を示す図

【図２】基線ベクトルと１重位相差等との関係を示す図

【図３】移動体の姿勢角検出装置の構成を示すブロック
図

【図４】図３における角速度／ＧＰＳ統合化処理演算部
等での処理手順を示すフローチャート

【図５】図４における姿勢角等から整数バイアスを決定
する手順を示すフローチャート

【図６】従来の姿勢角から整数バイアスを決定する手順
の概要を示すフローチャート

【図７】本発明による姿勢角から整数バイアスを決定す
る手順の概要を示すフローチャート

Claims (7)

【特許請求の範囲】
1. 【請求項１】 移動体上の、少なくとも二つのアンテナ
   と、該アンテナで複数の測位用衛星からの電波をそれぞ
   れ受信し、前記複数のアンテナのうち一つの基準アンテ
   ナに対する他のアンテナの相対位置を求め、該アンテナ
   の相対位置から前記移動体の方位および姿勢を検出す
   る、移動体の方位および姿勢検出装置において、 １重位相差または２重位相差を観測し、これらの位相差
   の整数バイアスを決定する手段と、 該整数バイアスの決定された前記位相差より前記相対位
   置を求める手段と、 前記移動体の姿勢角と該姿勢角の誤差共分散とを求める
   手段と、 前記整数バイアスを再決定する場合に、前記姿勢角と姿
   勢角の誤差共分散とから、前記測位用衛星毎の前記１重
   位相差、または二つの前記測位用衛星の組み合わせ毎の
   前記２重位相差による、少なくとも一つの整数バイアス
   の候補値を算出し、該候補値から前記整数バイアスを決
   定する整数バイアス再決定手段とを備えた移動体の方位
   および姿勢検出装置。
2. 【請求項２】 移動体上の、少なくとも二つのアンテナ
   と、該アンテナで複数の測位用衛星からの電波をそれぞ
   れ受信し、前記複数のアンテナのうち一つの基準アンテ
   ナに対する他のアンテナの相対位置を求め、該アンテナ
   の相対位置から前記移動体の方位および姿勢を検出す
   る、移動体の方位および姿勢検出装置において、 １重位相差または２重位相差を観測するとともに、これ
   らの位相差の整数バイアスを決定する手段と、 該整数バイアスの決定された前記位相差より前記相対位
   置を求める手段と、 前記移動体の姿勢角と該姿勢角の誤差共分散とを求め、
   該姿勢角の誤差共分散を補正する手段と、 前記整数バイアスを再決定する場合に、前記姿勢角と前
   記補正された姿勢角の誤差共分散とから、前記測位用衛
   星毎の前記１重位相差、または二つの前記測位用衛星の
   組み合わせ毎の前記２重位相差による、少なくとも一つ
   の整数バイアスの候補値を算出し、該候補値から前記整
   数バイアスを決定する整数バイアス再決定手段とを備え
   た移動体の方位および姿勢検出装置。
3. 【請求項３】 前記整数バイアスを再決定する場合に、
   前記姿勢角と姿勢角の誤差共分散とから前記整数バイア
   スを決定する手段と、前記１重位相差または２重位相差
   から位相差の整数バイアスを決定する手段とを併用また
   は、いずれかを選択して用いるようにした請求項１また
   は請求項２に記載の移動体の方位および姿勢検出装置。
4. 【請求項４】 前記姿勢角を求める手段を、角速度セン
   サと、該角速度センサの出力により、移動体の姿勢角を
   算出する手段とから構成した、請求項１〜３のいずれか
   に記載の移動体の方位および姿勢検出装置。
5. 【請求項５】 前記姿勢角と姿勢角の誤差共分散とを求
   める手段を、前記移動体の状態により仮定したモデルに
   基づいて姿勢角と姿勢角の誤差共分散とを予測する手段
   で構成した、請求項１〜３のいずれかに記載の移動体の
   方位および姿勢検出装置。
6. 【請求項６】 前記整数バイアス再決定手段は、floati
   ng ambiguityと該floating ambiguityの誤差共分散とを
   算出する手段を備える請求項１〜５のいずれかに記載の
   移動体の方位および姿勢検出装置。
7. 【請求項７】 前記整数バイアス再決定手段は、ＬＡＭ
   ＢＤＡ法により前記整数バイアスの候補値を決定する手
   段を備える請求項１〜６のいずれかに記載の移動体の方
   位および姿勢検出装置。