JP4729197B2

JP4729197B2 - 物体の姿勢検出装置および整数バイアス再決定方法

Info

Publication number: JP4729197B2
Application number: JP2001167407A
Authority: JP
Inventors: 優福田; 定雄佐藤; 裕行戸田
Original assignee: Furuno Electric Co Ltd
Current assignee: Furuno Electric Co Ltd
Priority date: 2000-06-01
Filing date: 2001-06-01
Publication date: 2011-07-20
Anticipated expiration: 2021-06-01
Also published as: JP2002054946A

Description

【０００１】
【発明の属する技術分野】
この発明は航空機や船舶等の移動体や、海上浮揚構造体、超高層建築物のような姿勢が変化する物体の姿勢を検出する装置に関するものである。
【０００２】
【従来の技術】
従来、複数の測位用衛星からの電波を、物体の所定位置に配置された複数のアンテナで受信し、キャリア信号の位相をそれぞれ観測し、複数のアンテナのうち１つのアンテナを基準として他のアンテナの相対位置を求めることによって、受信アンテナの相対位置関係を求め、これによって移動体の３次元姿勢を計測するようにした姿勢検出装置（Attitude Determining System:ＡＤＳ）が研究開発されている。
【０００３】
一方、レートジャイロ等の角速度を検出する慣性センサユニット（以下、ＩＭＵ：Inertia Measurement Unitという。）を用いて移動体の姿勢を測定するようにした装置も用いられている。
【０００４】
【発明が解決しようとする課題】
従来のＡＤＳでは、移動体の所定位置に配置した複数のアンテナのうち１つを基準として他のアンテナの相対位置を求めるために、測位用衛星から送信された電波のキャリア位相について２重位相差または１重位相差を観測して２重位相差または１重位相差に含まれる整数バイアスを求めるが、従来の整数バイアス解法では統計確率的に誤りが生じ、正しい整数バイアスを見つける確率は例えば約８５〜９５％である。また、整数バイアスの決定時間は観測雑音の大きさ、衛星数、およびアンテナ基線長（複数のアンテナのうち１つの基準アンテナと他の相対位置を求めるべきアンテナとを結ぶ直線の長さ）に大きく依存する。通常の雑音レベルにおいて、基線長が１ｍ程度の場合には、平均３０秒から数分の時間を要する。また、雑音レベルが高い場合には、たとえば１０分程度の長時間をかけても整数バイアスを決定できないという場合もあった。
【０００５】
実用の環境下では、測位に用いる衛星の組合せを変更する場合や、雑音によってサイクルスリップ（キャリア再生回路の位相同期ループが追尾不能になることにより、キャリア位相をカウントするカウンタの整数分の値が狂う現象）が生じた場合等、高頻度で整数バイアスを再決定する必要がある。そのため、整数バイアスの決定時間が長いと、ユーザーへの姿勢出力が高頻度に、しかも許容し難い期間、中断してしまう。また、求められた整数バイアスの正解率が低いと、信頼できない姿勢情報を提供してしまう。これらがＡＤＳの実用化のネックとなっていた。
【０００６】
一方、ＩＭＵのみを用いた姿勢検出装置では、ｘ，ｙ，ｚの３軸成分の角速度を積分することによって三次元の姿勢を検出するものであるため、誤差の累積が問題となっていた。
【０００７】
そこで、米国特許5,349,531 では、従来のＡＤＳとＩＭＵによる姿勢検出システムとの統合化を図り、ＡＤＳによる姿勢出力が中断したときに、ＩＭＵの出力に基づく角で補間するようにしている。尚、以降では特に断らない限り、ＩＭＵの出力に基づく姿勢はローカル座標系基準の姿勢角を意味するものとし、ＩＭＵ姿勢角と呼ぶことにする。
【０００８】
また、米国特許4,754,280 の姿勢検出装置では、ＩＭＵとＧＰＳを統合することによりＩＭＵの誤差を推定し、高レート出力、かつ高精度な姿勢を提供することのみにとどまっている。
【０００９】
ＩＭＵ利用により移動体の姿勢検出を行う装置で、安価な普及タイプのＩＭＵを使用した場合には、バイアス、ドリフト、スケール・ファクタ等のセンサ誤差が大きく、しかも正確な誤差モデリングができないために、これらの誤差を完全に推定し、補正することができない。そのため、例えば姿勢角として約１°を許容誤差とする場合には、ＩＭＵ姿勢角によるデドレコ（推測航法）可能なバックアップ時間は約３０秒〜６０秒程度が限界となる。しかし、キャリア位相の整数バイアスのカウント中にサイクルスリップが生じた場合、このような短時間のうちに整数バイアスを再決定することは、従来の方法では困難であり、実用的ではなかった。
【００１０】
上記いずれの米国特許も、ＡＤＳにおけるキャリア位相差の整数バイアスを求めることに伴う上述の問題を解消するものではない。
【００１１】
この発明の目的は、ＩＭＵ姿勢角を利用して、より短時間に整数バイアスを確実に再決定するようにした物体の姿勢検出装置および整数バイアス再決定方法を提供することにある。
【００１２】
【課題を解決するための手段】
この発明は、キャリア位相差の整数バイアスを決定して、複数のアンテナの相対位置を測位している途中でサイクルスリップが生じたとき、ＩＭＵの姿勢角を利用して整数バイアスを直接決定することにより、サイクルスリップが頻繁に生じても、ユーザーに姿勢角を中断することなく出力できるようにする。
【００１３】
また、この発明は、ＩＭＵ姿勢角から整数バイアスを決定するために、ＩＭＵ姿勢角と、アンテナ座標系における基線ベクトルとからローカル座標系における基線ベクトルを求め、アンテナおよび測位用衛星の位置で決まる、アンテナから測位用衛星への視野方向ベクトル、観測による１重位相差または２重位相差、および前記ローカル座標系における基線ベクトルに基づいて決定する。
【００１４】
また、この発明は、物体上の、一直線上に並ばないそれぞれ異なった位置に固定配置された複数のアンテナで複数の測位用衛星からの電波をそれぞれ受信し、キャリア位相の１重位相差または２重位相差を観測し、該１重位相差または２重位相差の整数バイアスを初期化時において決定するステップと、物体に配置されたＩＭＵ姿勢角と、アンテナ座標系における基線ベクトルとからローカル座標系における基線ベクトルを求めるステップと、前記アンテナおよび測位用衛星の位置で決まるアンテナから測位用衛星への視野方向ベクトル、前記観測による１重位相差または２重位相差、および前記ローカル座標系における基線ベクトルに基づいて、前記１重位相差または２重位相差の整数バイアスを直接再決定するステップとを備える。
【００１５】
【発明の実施の形態】
まず、アンテナ座標系とローカル座標系について、および両者の座標変換について、図１を参照して説明する。
【００１６】
図１の（Ａ）はローカル座標系を表し、基準アンテナの位置を原点として例えばｘ軸を北、ｙ軸を東、ｚ軸を鉛直方向（下方）に採る。（ｘ ₁ ⁿ ，ｙ ₁ ⁿ ，ｚ ₁ ⁿ ）は第２アンテナの位置、（ｘ ₂ ⁿ ，ｙ ₂ ⁿ ，ｚ ₂ ⁿ ）は第３アンテナの位置、Ｌ ₁ ⁿ は第１アンテナから第２アンテナへの基線ベクトル（複数のアンテナのうち１つの基準アンテナと他の相対位置を求めるべきアンテナとを結ぶ直線の長さと方向を表すベクトル）、Ｌ ₂ ⁿ は第１アンテナから第３アンテナへの基線ベクトルである。尚、座標値および基線ベクトルの右肩の添字ｎはローカル座標系での座標値および基線ベクトルであることを表している。
【００１７】
図１の（Ｂ）はアンテナ座標系を表し、基準アンテナの位置を原点として例えばｘ軸を方位（機首または船首の方向）に採り、ｙ軸をｘ軸方向に直交する水平方向に採り、ｚ軸をｘ，ｙ軸にそれぞれ直交する方向に採る。（ｘ ₁ ^a ，ｙ ₁ ^a ，ｚ ₁ ^a ）は第２アンテナの位置、（ｘ ₂ ^a ，ｙ ₂ ^a ，ｚ ₂ ^a ）は第３アンテナの位置、Ｌ ₁ ^a は第１アンテナから第２アンテナへの基線ベクトル、Ｌ ₂ ^a は第１アンテナから第３アンテナへの基線ベクトルである。尚、座標値および基線ベクトルの右肩の添字ａはアンテナ座標系での座標値および基線ベクトルであることを表している。
【００１８】
上記アンテナ座標系からローカル座標系への座標変換の変換式は〔数１〕で示される。
【００１９】
【数１】

【００２０】
ここでＬ ₃ ⁿ はＬ ₁ ⁿ とＬ ₂ ⁿ の外積である。また、Ｌ ₃ ^a はＬ ₁ ^a とＬ ₂ ^a の外積である。
ＧＰＳで計測されるローカル座標系に対するアンテナ座標系の姿勢角を、或る軸回転順序で定義されたオイラー角（θｒ，θｐ，θｙ）で表現すると、〔数１〕における座標変換オペレータＣ _a ⁿ は次式で表される。
【００２１】
【数２】

【００２２】
ここで、θｒはロール角、θｐはピッチ角、θｙはヨー角と言うことができる。尚、ＧＰＳによる基線ベクトルは、ローカル座標系で得られるので、姿勢角θｙは、真北基準の方位として得られる。また、θｒとθｐは水平面を基準とする姿勢角として得られる。尚、以降、（θｒ，θｐ，θｙ）をＧＰＳ姿勢角と呼ぶ。
【００２３】
上記３つの姿勢角はそれぞれ次の関係から求められる。
【００２４】
【数３】

【００２５】
【数４】

【００２６】
【数５】

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

【００２９】
ここで、
ｍ：衛星番号
ｋ：衛星数
ｊ：基線ベクトルの番号
λ：受信信号キャリア波長
δρ _j ^m ：基準衛星Ｓ¹ と衛星Ｓ^m+1 （ｍ＝１，２，３）に対する基線ベクトルＬ _j ⁿ の２重距離差
δφ _j ^m ：同上に対する２重位相差観測量
δＮ _j ^m ：同上に対する２重整数バイアス差
δｅ _j ^m ：同上に対する２重観測雑音差
この２重位相差を図式化したものを図２に示す。図中の記号の意味は次のとおりである。
【００３０】
Δρ _j ^m+1 ：衛星ｓ^m+1 に対する基線ベクトルＬ _j ⁿ の１重距離差
Δφ _j ^m+1 ：衛星ｓ^m+1 に対する基線ベクトルＬ _j ⁿ の１重位相観測量
ΔＮ _j ^m+1 ：衛星ｓ^m+1 に対する基線ベクトルＬ _j ⁿ の１重整数バイアス差
Δｅ _j ^m+1 は衛星ｓ^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 ^m とＬ _j ⁿ すなわち（ｘ _j ⁿ ，ｙ _j ⁿ ，ｚ _j ⁿ ）となる。いま、前もって何らかの方法で整数バイアスを求めれば、未知数は（ｘ _j ⁿ ，ｙ _j ⁿ ，ｚ _j ⁿ ）の３個であるので、少なくとも３個の観測量δφ _j ^m が得られると、すなわち衛星数が４以上であると、（ｘ _j ⁿ ，ｙ _j ⁿ ，ｚ _j ⁿ ）を求めることができる。
【００３３】
さて、以下に、ＩＭＵ姿勢角から整数バイアスを再決定できる根拠を示す。
いま、図１のように定義されたローカル座標系とアンテナ座標系における任意の基線ベクトルＬの座標ベクトルの座標位置を、それぞれ、マトリックスＸⁿ ，Ｘ^a で表記すれば、〔数１〕は次の式で表される。
【００３４】
【数７】
Ｘⁿ ＝Ｃ _a ⁿ Ｘ^a …(7)
したがって、Ｘⁿの誤差共分散σ_P ² ＝Ｅ[ δＸⁿ ( δＸⁿ ) ^T ] は次式で示される。
【００３５】
【数８】

【００３６】
ここで、σ _p ²,σθ²はそれぞれアンテナ基線ベクトル位置と姿勢角の共分散誤差マトリックスであり、次式で表される。また、ＨはＸⁿ と姿勢角との感度マトリックスである。
【００３７】
【数９】

【００３８】
基線ベクトル位置の誤差の標準偏差σ_P と衛星視野方向に射影した値δρとの関係は次式で表される。
【００３９】
【数１０】
δρ＝δＡσ_p …(10)
ここでδＡは、〔数６〕の説明中で述べたδＡ _j ^m （２重位相差における衛星への方向余弦マトリクス差）のサフィックスを省略したものである。
いま、δρの標準偏差をσ_Nとすると、上記〔数８〕と〔数１０〕とによって、姿勢角誤差と整数バイアスとの誤差との関係は次式で表される。
【００４０】
【数１１】

【００４１】
今、仮に、各辺が約５０ｃｍの三角形の各頂点に配置されたアンテナの架台を、ロール角±２０°、ロール角変動周期６０秒、ピッチ角±１５°、ピッチ角変動周期７０秒、ヨー角±３０°、ヨー角変動周期１８０秒で振揺させたとき、そして衛星の方位を３６０°、仰角を０〜８０°の範囲でそれぞれ変化させたとき、ＩＭＵ姿勢角の誤差が標準偏差で数度以内であれば、衛星視野方向換算の距離誤差は、衛星から送信される電波のキャリア波長の１／４（５ｃｍ程度）以下となる。
【００４２】
このことは、姿勢角誤差は通常数度以内の精度であるから、上記の条件でＩＭＵの出力に基づいて求めた姿勢角からキャリア位相差のアンビギュイティを丸め込みによって整数化しても、キャリア位相差の整数バイアスを誤りなく決定できることを意味する。
【００４３】
次に、ＩＭＵ姿勢角より整数バイアスを再決定する方法を具体的に記述する。
アンテナ座標系における基線ベクトルＬ_j ^a（ｊ＝１，２）の位置Ｘ^a （ｘ_j ^a，ｙ_j ^a，ｚ_j ^a）は既知であるから、ＩＭＵ姿勢角を用いて〔数２〕、〔数７〕より、ローカル座標系における基線ベクトルＬ_j ⁿ （ｊ＝１，２）の位置Ｘⁿ（ｘ_j ⁿ，ｙ_j ⁿ，ｚ_j ⁿ）を求める。
【００４４】
この（ｘ _j ⁿ ，ｙ _j ⁿ ，ｚ _j ⁿ ）を用いて、次式より各衛星における１重距離差Δρ _j ^m を逆算する。
【００４５】
【数１２】
Δρ _j ^m ＝Ａ _j ^m Ｘ _j ⁿ （ｍ＝１，２，３，４，ｊ＝１，２） …(12)
ここで、Ａ _j ^m は基線ベクトルＬ _j ⁿ のｍ衛星への視野方向ベクトルであるから、ローカル座標系における各衛星位置とアンテナの絶対位置より決定できる。尚、ｍは使用衛星番号、ｊは基線ベクトルの番号である。
【００４６】
１重位相差の観測方程式は次式で表されるから、１重位相差Δφ _j ^m を観測量とすると、ｍ衛星に対する１重位相差の実数アンビギュイティΔＮ _j ^m が計算できる。
【００４７】
【数１３】
λΔＮ _j ^m ＝Δρ _j ^m −λΔφ _j ^m ＋Δｅ _j ^m …(13)
（ｍ＝１，２，３，４，ｊ＝１，２）
前述の例示のようにＩＭＵ姿勢角誤差の衛星視野方向への距離換算誤差は、通常、キャリア波長の１／４（５ｃｍ程度）以下となるので、このΔＮ _j ^m を丸め込みによって、１重位相差の整数バイアス（１重整数バイアス差）が決定できる。
【００４８】
尚、２重位相差方式の場合には、前述の〔数６〕より２重位相差実数アンビギュイティδＮ _j ^m で計算できる。したがって、１重位相差方式、２重位相差方式のいずれにも適用できる。
【００４９】
以上のようにして、ＩＭＵ姿勢角を用いて整数バイアスを決定することができる。
【００５０】
尚、以上に述べた例では、３つのアンテナを用い、そのうちの１つを基準アンテナとし、他の２つのアンテナとの間の組で１重位相差または２重位相差を求めるようにしたが、２つのアンテナのみを設け、その２つのアンテナ間で１重位相差または２重位相差を求めるようにしてもよい。例えば、２つのアンテナ間の基線を船舶の船首尾線上に配置すれば、その２つのアンテナの相対位置から、船首方位（θｙ）および、ピッチ角（θｐ）を検出することができる。また、２つのアンテナ間の基線を右舷−左舷方向に配置すれば、その２つのアンテナの相対位置から、船首方位（θｙ）とロール角（θｒ）を検出することができる。
【００５１】
但し、前者ではロール角（θｒ）を検出できず、後者ではピッチ角（θｐ）を検出できない。３次元の姿勢が求まらなければ、〔数１〕および〔数２〕より、この場合は、ＩＭＵ出力から得られる相対姿勢角を完全にＧＰＳ姿勢角で初期化できないために、ＩＭＵ姿勢角から整数バイアスを求めることができない。そこで、本発明の方法を適用するためには、後述するように、２つのアンテナだけでは検出できない姿勢角を検出するための不足分の姿勢を計測可能な加速度センサ、または傾斜計を併用すればよい。
【００５２】
また、４つ以上のアンテナを用いて、そのうちの１つを基準アンテナとし、他の３つのアンテナとの間の組で１重位相差または２重位相差をそれぞれ求めることによって、各アンテナ間の相対位置を求めるようにしてもよい。
【００５３】
次に、物体の姿勢検出装置の全体の構成を図３および図４を参照して説明する。
図３は装置のブロック図である。ここでｓ１，ｓ２・・・ｓｋはＧＰＳ測位衛星であり、ＧＰＳアンテナはこれらの複数の測位用衛星からの電波を受信し、中間周波信号に変換するとともに増幅する。ＧＰＳ受信機はそれぞれのアンテナで受信した信号から、それぞれのアンテナ位置およびキャリア位相差を演算し、各衛星位置情報と共に姿勢演算処理部へ所定（数十ミリ秒〜数秒）の間隔で送出する。測位により求められるアンテナ位置や衛星のエフェメリス情報等はＧＰＳの座標系に従っているので、ＧＰＳ受信機部または姿勢演算処理部のいずれかでローカル座標系に変換する。以下の説明ではＧＰＳ受信機部で変換するものとする。
【００５４】
姿勢演算処理部における衛星計画部は、衛星のエフェメリス情報とアンテナ位置とから、使用する衛星を選定し、その衛星位置や選定された衛星の衛星情報を位相差演算部へ与える。この位相差演算部は、衛星計画部から受け取った情報とＧＰＳ受信機より受け取ったキャリア位相差信号から１重（または２重）位相差の観測量を算出し、整数バイアス決定処理部へ送出する。整数バイアス決定部では、該当衛星に対する１重（または２重）位相差の整数バイアスを決定し、その妥当性を確認した後に整数バイアス情報をＧＰＳ姿勢演算部へ与える。ＧＰＳ姿勢演算部では、整数バイアスと位相差演算部より送出されたキャリア位相差観測量とを用いて、公知の方法によって、ローカル座標系における基線ベクトルを算出する。これと既知のアンテナ座標系における基線ベクトルより、〔数１〕〜〔数５〕によって姿勢角（ＧＰＳ姿勢角）を算出する。
【００５５】
図３において、ＩＭＵは直交３軸回りの角速度を検出するレートジャイロと、それらの出力を増幅するＸ軸増幅部、Ｙ軸増幅部、Ｚ軸増幅部を備えていて、姿勢演算処理部のＩＭＵ姿勢演算部に対して各軸回りの角速度を与える。具体的には、地球の赤道平面と春分点子午面との交線をｘ軸、これに直交する東向きをｙ軸とする右手系の慣性座標軸基準に対するＩＭＵ座標軸の角速度ω ^s _isを与える。いま、必要なのは、ローカル座標系に対するＩＭＵ座標軸の角速度ω ^s _nsである。ω ^s _isとω ^s _nsとの関係は、ω ^s _ns＝ω ^s _is−ω ^s _inで表される。ここで、ω ^s _inは慣性座標軸系基準に対するローカル座標軸の角速度をＩＭＵ座標軸系で観測した角速度である。船舶のような低速で移動する物体の場合ではω ^s _is＞＞ω ^s _inが成立するから、ω ^s _inは無視できる。航空機のような高速移動する物体では、移動体の位置および周知の方法によりω ^s _nsを計算する。
【００５６】
ＩＭＵ姿勢演算部では、周知のオイラー方程式またはGilmore のアルゴリズムによって姿勢角を算出する。尚、ＩＭＵ姿勢角の初期値はＧＰＳ姿勢角を用いて初期化するものとする。
【００５７】
ＩＭＵ／ＧＰＳ統合化処理演算部では、ＩＭＵ姿勢角とＧＰＳ姿勢角とを統合化して、最終的にロール，ピッチ，ヨーの各姿勢角を出力する。
【００５８】
図４は、図３に示した姿勢演算処理部における再初期化判定部およびＩＭＵ／ＧＰＳ統合化処理演算部等での処理手順を示すフローチャートである。まず測位に用いる衛星の選択、および選択した衛星からの電波を受信して求めた観測データを取り込む（ｎ１）。初期状態で未だ整数バイアスを決定していなければ、先ず各アンテナについて基線ベクトルを推定する。（実数解を求める。）この推定には例えばカルマン・フィルタまたは最小２乗法を用いる。続いて、公知の方法により実数解を整数化することによって整数バイアスを決定する（ｎ２→ｎ３→ｎ５→ｎ６→ｎ５・・・）。
【００５９】
その後、χ² 検定および基線長が実際の値に一致するか否か等によって、整数バイアス推定の妥当性の検定を行う（ｎ７）。もし、検定結果が妥当でなければ、再び整数バイアスの決定処理を行う（ｎ８→ｎ５・・・）。検定結果が妥当とみなされれば、求められた整数バイアスを既知として基線ベクトルを求め、各アンテナの相対位置からＧＰＳ姿勢角を算出する（ｎ９）。続いて、このＧＰＳ姿勢角を算定した同時刻におけるＩＭＵの３軸回りの角速度からＩＭＵ姿勢角を演算する（ｎ１０）。
【００６０】
尚、前述のようにＩＭＵ姿勢角の初期値はＧＰＳ姿勢角を用いて初期化されるので、ＩＭＵ姿勢角はＧＰＳ姿勢角と同じローカル座標系の姿勢角に一致したものとなる。
【００６１】
その後、ＧＰＳ姿勢角とＩＭＵ姿勢角との統合化処理を行う。ここでは、ＧＰＳダウン時の処理や、ＧＰＳ姿勢角とＩＭＵ姿勢角との差からＩＭＵの誤差補正を行う（ｎ１０→ｎ１１）。
【００６２】
続いて、姿勢演算処理部における再初期化判定部で、サイクルスリップが生じたか否かを判定し、サイクルスリップが生じていなければ、求められた姿勢角を出力する（ｎ１２→ｎ１３→ｎ１５）。
【００６３】
サイクルスリップが生じたことを検出すれば、上述した方法により、ＩＭＵ姿勢角データより整数バイアスを直接決定し、その整数バイアスで基線ベクトルを定める（ｎ１３→ｎ１４→ｎ４）。このステップｎ４の基線ベクトル推定とステップｎ３の基線ベクトル推定との違いは、例えばカルマン・フィルタを用いる場合、ステップｎ３の基線ベクトル推定でのステート数はｘ，ｙ，ｚの３個とｍ個のアンビギュイティであるが、ステップｎ４の基線ベクトル推定でのステート数はｘ，ｙ，ｚの３個のみである。従って、このステップｎ４の基線ベクトル推定の処理負荷は小さく、極短時間に基線ベクトルを推定できる。
以降、既に述べた整数バイアスの検定以降の処理を行う（ｎ７→ｎ８→・・・）。
【００６４】
尚、通常はステップｎ１４での整数バイアスの決定に誤りがないので、ステップｎ７，ｎ８の処理を省略して、ステップｎ４からステップｎ９へ進むようにしてもよい。
【００６５】
以上のようにして、サイクルスリップ、衛星選択の組合せの変更等によって再初期化が必要となった場合に、ＩＭＵ姿勢角データを基にして整数バイアスを直接決定するようにしたため、整数バイアスの決定に長時間を必要としない。そのため、物体の姿勢角を実質上中断することなく出力でき、高精度な姿勢角情報を連続的にユーザに提供できるようになる。
【００６６】
次に、２つのアンテナを用いた物体の姿勢検出装置の構成を図５を参照して説明する。
図３に示した例では、３つのアンテナを用いてＧＰＳ姿勢角を求めたが、この図５に示す例では、３つの姿勢角θｒ，θｐ，θｙのうち、２つ（θｙ、θｒまたはθｐのいずれか）の姿勢角を２つのアンテナを用いて求め、２アンテナ方式では算出できないθｐ、またはθｒのいずれかの姿勢角を加速度センサを用いて求める。
【００６７】
すなわち、図５において、ＧＰＳ受信機部には２つのＧＰＳアンテナでＧＰＳ衛星からの電波を受信し、キャリア位相を観測する。このＧＰＳ受信機部または姿勢演算処理部でθｙ、θｒまたはθｐのいずれかの姿勢角を求める。ＩＭＵには、レートジャイロの他に１つ以上の加速度センサを備えていて、この加速度センサの検出結果から上記２アンテナ方式では算出できないθｐまたはθｒのいずれかの姿勢角を求める。
【００６８】
初期化時における整数バイアスの決定後にサイクルスリップが生じれば、図３および図４に示した場合と同様の処理によって整数バイアスの再決定を行う。但し、この実施形態ではアンテナが２本しかなく、整数バイアスの再決定の際に、上記２アンテナ方式では算出できないθｐまたはθｒのいずれかの姿勢角を求めるために、ＩＭＵ／ＧＰＳ統合化処理演算部は、加速度センサの出力により求めた姿勢角を利用する。図中のスイッチで示す記号は、この整数バイアスの再決定時にのみオンする、という概念を示している。
【００６９】
尚、図５に示した例では、加速度センサの出力に基づいて姿勢角の１つを求めるようにしたが、その他に、傾斜計を用いてもよい。
【００７０】
【発明の効果】
この発明によれば、ＩＭＵを用いて求めた物体の姿勢角を利用して、複数のアンテナの相対測位に用いるキャリア位相差の整数バイアスを直接的に決定するようにしたため、使用衛星の組み合わせが変更される場合やサイクルスリップの発生時にも、短時間のうちに整数バイアスを再決定することができる。その結果、ユーザーに対してほぼ連続して姿勢情報を出力できるようになる。また、整数バイアスの再決定時の正解率が高まるので、信頼性の高い姿勢情報を出力できるようになる。
【図面の簡単な説明】
【図１】ローカル座標系とアンテナ座標系におけるアンテナ配置の例を示す図
【図２】基線ベクトルと１重位相差等との関係を示す図
【図３】物体の姿勢角検出装置の構成を示すブロック図
【図４】図３におけるＩＭＵ／ＧＰＳ統合化処理演算部等での処理手順を示すフローチャート
【図５】物体の姿勢角検出装置の他の構成を示すブロック図

Claims

物体上の、一直線上に並ばないそれぞれ異なった位置に固定配置された少なくとも３つのアンテナと、これらのアンテナで複数の測位用衛星からの電波をそれぞれ受信し、これらのアンテナのうち１つの基準アンテナに対する他のアンテナの相対位置を、キャリア位相の１重位相差または２重位相差を観測するとともに、これらの位相差の整数バイアスを決定することによって求め、該アンテナの相対位置から前記物体の姿勢角を求める手段を備えた装置において、
物体に配置された慣性センサユニットと、該慣性センサユニットの出力により物体の姿勢角を求める手段と、
前記アンテナの相対位置から求められた前記物体の姿勢角で、前記慣性センサユニットの出力により求められる姿勢角を初期化する手段と、
前記慣性センサユニットの出力により求められた姿勢角から前記整数バイアスを直接再決定する手段とを設けた物体の姿勢検出装置。
物体上の、離れた位置に固定配置された２つのアンテナと、これらのアンテナで複数の測位用衛星からの電波をそれぞれ受信し、前記２つのアンテナのうち一方の基準アンテナに対する他方のアンテナの相対位置を、キャリア位相の１重位相差または２重位相差を観測するとともに、これらの位相差の整数バイアスを決定することによって求め、該アンテナの相対位置から前記物体の姿勢角を検出する装置において、
物体に配置された慣性センサユニットと、該慣性センサユニットの出力により物体の姿勢角を求める手段と、
前記アンテナの相対位置から求められた前記物体の姿勢角で、前記慣性センサユニットの出力により求められる姿勢角を初期化する手段と、
前記慣性センサユニットの出力により求められた姿勢角から前記整数バイアスを直接再決定する手段とを設けた物体の姿勢検出装置。
前記整数バイアスを再決定する手段は、
前記慣性センサユニットの出力により求められた姿勢角と、アンテナ座標系における基線ベクトルとからローカル座標系における基線ベクトルを求め、前記アンテナおよび測位用衛星の位置で決まるアンテナから測位用衛星への視野方向ベクトル、前記観測による１重位相差または２重位相差、および前記ローカル座標系における基線ベクトルに基づいて再決定する請求項１または２に記載の物体の姿勢検出装置。
物体上のそれぞれ異なった位置に固定配置された複数のアンテナと、これらのアンテナで複数の測位用衛星からの電波をそれぞれ受信し、前記複数のアンテナの相対位置を、前記アンテナを用いた前記測位用衛星の観測によるキャリア位相差の整数バイアスを決定することによって求め、該アンテナの相対位置から前記物体の姿勢角を検出する装置において、
物体に配置された慣性センサユニットと、該慣性センサユニットの出力により物体の姿勢角を求める手段と、
前記アンテナの相対位置から求められた前記物体の姿勢角で、前記慣性センサユニットの出力により求められる姿勢角を初期化する手段と、
前記慣性センサユニットの出力により求められた姿勢角から前記整数バイアスを直接再決定する手段とを設けた物体の姿勢検出装置。
物体上の、一直線上に並ばないそれぞれ異なった位置に固定配置された複数のアンテナで複数の測位用衛星からの電波をそれぞれ受信し、キャリア位相の１重位相差または２重位相差を観測し、該１重位相差または２重位相差の整数バイアスを初期化時において決定するステップと、
前記アンテナの相対位置から前記物体の姿勢角を求めるステップと、
前記アンテナの相対位置から求められた前記物体の姿勢角で、物体に配置された慣性センサユニットの出力により求められる姿勢角を初期化するステップと、
前記慣性センサユニットの出力により求められた姿勢角と、アンテナ座標系における基線ベクトルとからローカル座標系における基線ベクトルを求めるステップと、
前記アンテナおよび測位用衛星の位置で決まるアンテナから測位用衛星への視野方向ベクトル、前記観測による１重位相差または２重位相差、および前記ローカル座標系における基線ベクトルに基づいて、前記１重位相差または２重位相差の整数バイアスを直接再決定するステップとを備えた、物体の姿勢検出装置における整数バイアス再決定方法。