JP3919911B2

JP3919911B2 - 方位姿勢基準装置

Info

Publication number: JP3919911B2
Application number: JP31523897A
Authority: JP
Inventors: 前田　　徹; 幹雄諸星
Original assignee: Tokyo Keiki Inc
Current assignee: Tokyo Keiki Inc
Priority date: 1997-11-17
Filing date: 1997-11-17
Publication date: 2007-05-30
Anticipated expiration: 2017-11-17
Also published as: JPH11148827A

Description

【０００１】
【発明の属する技術分野】
本発明は船舶、航空機、自動車等の航行体に装備し、航行体の方位及び姿勢角（ロール角及びピッチ角）を検出するためのストラップダウン型の方位姿勢基準装置に関し、より詳細には、アラインメントモードにおいてカルマンフィルタによって誤差を推定するように構成された方位姿勢基準装置に関する。
【０００２】
【従来の技術】
航空機には方位及び姿勢角を検出するための方位姿勢基準装置又はＡＨＲＳ（Attitude Heading Reference System ）が搭載されている。ＡＨＲＳによって得られた方位及び姿勢信号等はオートパイロット装置に供給され、それによってオートパイロット装置は航空機を自動操縦する。こうして、ＡＨＲＳ及びオートパイロット装置はパイロットの感覚を補って安全な飛行を可能にする。
【０００３】
方位姿勢基準装置には、プラットホーム型とストラップダウン型が知られている。プラットホーム型は、ジャイロ等のセンサをプラットホームに取り付け、このプラットホームを水平に且つ南北方向に指向させる機構を有する。ストラップダウン型は、ジャイロ等のセンサを航行体に直接取り付け、機械的なプラットホームや方位ジンバルを備えない。コンピュータ上にて架空のプラットホームを想定し、この架空のプラットホームを水平に且つ南北方向に指向させるように構成されている。ストラップダウン型の方位姿勢基準装置ではこの架空のプラットホームを制御するために座標変換マトリックスが使用される。
【０００４】
ストラップダウン型の方位姿勢基準装置では、方位及び姿勢角の初期値を求める初期化又はアラインメントという作業がなされる。これは航行体又は方位姿勢基準装置の方位及び姿勢角の初期値を求めることであるが、基本的には方位及び姿勢誤差及びジャイロ誤差等を予め求め、その補正を演算することである。この初期化又はアラインメントは通常航行体の出発前になされるが、後に説明するように航行中に適宜行われてもよい。従って方位姿勢基準装置は先ず、アラインメントモードにて作動され、アラインメントが終了した後に方位及び姿勢角等が出力される。
【０００５】
図３は従来の方位姿勢基準装置の構成の概略を示すブロック図である。航空機、船舶等の航行体には、Ｘジャイロ１Ａ、Ｙジャイロ１Ｂ及びＺジャイロ１Ｃが取り付けられ、これらの３つのジャイロは航行体の主要３軸、即ちＸＹＺ軸方向の入力軸線を有するように配置される。Ｘジャイロ１Ａ、Ｙジャイロ１Ｂ及びＺジャイロ１ＣはそれぞれＸ軸周りの回転角速度、Ｙ軸周りの回転角速度及びＺ軸周りの回転角速度を検出する。これらのジャイロは例えば、リングレーザジャイロ、光ファイバジャイロ等の非回転式のジャイロよりなる。
【０００６】
航行体には、更にＸ加速度計２Ａ、Ｙ加速度計２Ｂ及びＺ加速度計２Ｃが取り付けられ、これらの３つの加速度計は航行体の主要３軸、即ちＸＹＺ軸方向の入力軸線を有するように配置されている。Ｘ加速度計２Ａ、Ｙ加速度計２Ｂ及びＺ加速度計２Ｃはそれぞれ水平面に対するＸ軸、Ｙ軸及びＺ軸の傾斜角度及び運動加速度を検出する。尚、航行体には、更に航行体の主要軸線、例えば、船首軸線、機体軸線に沿って速度センサ３が取り付けられる。
【０００７】
方位姿勢基準装置は信号変換部４と信号演算部５と信号出力部６とを有する。信号変換部４はＸジャイロ１Ａ、Ｙジャイロ１Ｂ及びＺジャイロ１Ｃの出力信号ＸＧ、ＹＧ、ＺＧとＸ加速度計２Ａ、Ｙ加速度計２Ｂ及びＺ加速度計２Ｃの出力信号ＸＡ、ＹＡ、ＺＡを入力する。
【０００８】
これらの信号は信号変換部４を介して信号演算部５に供給される。信号演算部５は信号変換部４から供給されたこれらの信号と速度センサ３から供給された（基準）速度信号Ｒ_EW、Ｒ_SNを入力して、ＸＹＺ方向の角速度、ロール角、ピッチ角、方位角、緯度及び経度等を演算する。これらの値は信号出力部６を経由して外部へ供給される。
【０００９】
図４を参照してカルマンフィルタを用いた方位姿勢基準装置のアラインメントを説明する。この例は、本願出願人と同一の出願人によって平成９年６月２３日付けにて出願された特願平９−１６５７９４号に開示されたものであり、詳細は同出願を参照されたい。図４は方位姿勢基準装置の信号演算部５の構成例を示す。信号演算部５は図示のように、ＣＴＭ演算部（座標変換マトリックス演算部）２１、加速度水平成分演算部２２、速度水平成分演算部２３及び方位姿勢誤差演算部２４を有する。
【００１０】
ＣＴＭ演算部（座標変換マトリックス演算部）２１は信号変換部４より供給されたＸ、Ｙ、Ｚジャイロ信号ＸＧ、ＹＧ、ＺＧを入力し、航行体座標を局地水平面上の局地水平座標系に変換するための座標変換マトリックス（ＣＴＭ）を演算する。この座標変換マトリックスは方位姿勢誤差演算部２４からの修正トルク信号によって時々刻々誤差修正される。
【００１１】
ＣＴＭ演算部２１は座標変換マトリックスによって局地水平座標系の角速度ω₁、ω₂、ω₃を演算し、それを時々刻々積分する。それによって方位角及び姿勢角が求められる。一方、Ｘ、Ｙ、Ｚジャイロ信号ＸＧ、ＹＧ、ＺＧはＸレート、Ｙレート及びＺレートとして信号出力部６に供給される。
【００１２】
加速度水平成分演算部２２は信号変換部４より供給されたＸ、Ｙ、Ｚ加速度計信号ＸＡ、ＹＡ、ＺＡと、ＣＴＭ演算部２１より供給されたＣＴＭ信号、即ち、座標変換マトリックスを入力して、東西方向の加速度の水平成分Ａ_EWと南北方向の加速度の水平成分Ａ_SNを演算する。
【００１３】
速度水平成分演算部２３は加速度水平成分演算部２２より供給され東西方向及び南北方向の加速度の水平成分Ａ_EW、Ａ_SNを入力し、それを積分して東西方向及び南北方向の速度の水平成分Ｖ_EW、Ｖ_SNを演算する。速度の水平成分信号Ｖ_EW、Ｖ_SNは、方位姿勢誤差演算部２４に供給される。
【００１４】
方位姿勢誤差演算部２４は、速度の水平成分信号Ｖ_EW、Ｖ_SNと速度センサ３から供給された（基準）速度信号Ｒ_EW、Ｒ_SNを入力して両者の偏差δ_EW、δ_SNを演算し、この偏差δ_EW、δ_SNに基づいて方位及び姿勢角の誤差を推定演算する。この方位及び姿勢角の誤差の推定値は修正トルク信号としてＣＴＭ演算部２１の供給される。方位姿勢誤差演算部２４におけるこの推定演算はカルマンフィルタを用いてなされる。
【００１５】
ここで方位姿勢誤差演算部２４の主要機能であるカルマンフィルタの概要を簡単に説明する。システムは１次の微分方程式によって記述されることができるものとする。
【００１６】
【数１】
ｄ〔ｘ〕／ｄｔ＝〔Ａ〕〔ｘ〕＋〔η〕
【００１７】
この式の各項は次のようなものである。
〔ｘ〕：最適推定値の状態ベクトル（ｎ次ベクトル）
〔Ａ〕：システム行列（ｎ×ｎ行列）
〔η〕：システム外乱ベクトル（ｎ次のベクトル）
この式を遷移行列法によって離散形に変形すると次のようになる。
【００１８】
【数２】
〔ｘ（ｔ＋Δｔ）〕＝〔Φ〕〔ｘ（ｔ）〕＋〔Ｇ〕〔η〕
【００１９】
尚、Φ及びＧは次の式によって表される。
【００２０】
【数３】
Φ＝Ｉ＋ＡΔｔ＋（Δｔ²／２）・Ａ²
Ｇ＝ΦΔｔ〔Ｉ−（Δｔ／２）・Ａ〕
【００２１】
Ｉは単位行列である。観測系は次の式によって表される。
【００２２】
【数４】
〔ｙ〕＝〔Ｈ〕〔ｘ〕＋〔ｖ〕
【００２３】
この式の各項は次のようなものである。
〔ｙ〕：観測値の状態ベクトル（ｍ次ベクトル）
〔Ｈ〕：観測行列（ｍ×ｎ行列）
〔ｖ〕：観測外乱ベクトル（ｎ×ｎ行列）
システムが数１の式〜数４の式によって記述されることができる場合には、システムの状態ベクトル、即ち、最適推定値ｘ（ｔ＋Δｔ，ｔ＋Δｔ）は次の式によって求められる。
【００２４】
【数５】
Ｐ（ｔ＋Δｔ，ｔ）＝Φ・Ｐ（ｔ，ｔ）Φ^T＋Ｇ（Ｑ／Δｔ）Ｇ^T
Ｋ（ｔ＋Δｔ，ｔ）＝Ｐ（ｔ＋Δｔ，ｔ）Ｈ^T〔Ｈ・Ｐ（ｔ＋Δｔ，ｔ）Ｈ^T＋Ｒ〕^-1
ｘ（ｔ＋Δｔ，ｔ）＝Φ・ｘ（ｔ，ｔ）
ｘ（ｔ＋Δｔ，ｔ＋Δｔ）＝ｘ（ｔ＋Δｔ，ｔ）＋Ｋ（ｔ＋Δｔ，ｔ）〔ｙ−Ｈ・ｘ（ｔ＋Δｔ，ｔ）〕
Ｐ（ｔ＋Δｔ，ｔ＋Δｔ）＝〔Ｉ−Ｋ（ｔ＋Δｔ，ｔ）・Ｈ〕Ｐ（ｔ＋Δｔ，ｔ）
【００２５】
Ｋはカルマンゲイン、Ｐはシステムの状態ベクトルの誤差共分散行列、Ｑはシステム外乱ベクトルの誤差共分散行列、Ｒは観測外乱ベクトルの誤差共分散行列であり、それぞれ次のように表される。尚、右辺のＥは期待値を表す。
【００２６】
【数６】
Ｐ＝Ｅ〔ｘ，ｘ^T〕
Ｑ＝Ｅ〔η，η^T〕
Ｒ＝Ｅ〔ｖ，ｖ^T〕
【００２７】
数４の式の各項は次のような意味を有する。Ｐ（ｔ，ｔ）は現時刻ｔにおける誤差共分散行列、Ｐ（ｔ＋Δｔ，ｔ）は現時刻ｔにて予測した時刻ｔ＋Δｔの誤差共分散行列の予測値であり、現時刻ｔにおけるシステム誤差の統計量がΔｔ後に如何なる値に変化するのかを予想した期待値である。
【００２８】
Ｋ（ｔ＋Δｔ，ｔ）は現時刻ｔにて時刻ｔ＋Δｔにおけるシステム誤差を推定するためのカルマンゲインである。ｘ（ｔ＋Δｔ，ｔ）は現時刻ｔにて予測した時刻ｔ＋Δｔにおけるシステム誤差の最適予測値であり、現時刻ｔの誤差ｘ（ｔ，ｔ）に係数Φを乗じて得られる。
【００２９】
ｘ（ｔ＋Δｔ，ｔ＋Δｔ）は時刻ｔ＋Δｔにて推定した時刻ｔ＋Δｔのシステム誤差の最適推定値である。この最適推定値は、現時刻ｔにおけるシステム誤差の予測値ｘ（ｔ＋Δｔ，ｔ）を、観測値と最適予測値の偏差〔ｙ−Ｈ・ｘ（ｔ＋Δｔ，ｔ）〕にカルマンゲインＫを乗じた値で修正することによって求められる。最適推定値によってシステム誤差が修正される。
【００３０】
Ｐ（ｔ＋Δｔ，ｔ＋Δｔ）は時刻ｔ＋Δｔにて推定した時刻ｔ＋Δｔの誤差共分散行列の最適推定値である。この推定値は、誤差共分散行列の予測値Ｐ（ｔ＋Δｔ，ｔ）をカルマンゲインＫを用いて修正することによって得られる。
【００３１】
カルマンフィルタによって位置及び姿勢角を推定する場合に、観測値として複数のパラメータを選択することができる。これらのパラメータに対して重み付けをすることができる。推定の重み付けに寄与するのはカルマンゲインＫである。カルマンゲインＫは、どの観測値からどの状態ベクトルをどの程度に推定するかを表す。
【００３２】
カルマンゲインＫは上述の誤差共分散行列Ｐ，Ｑ，Ｒによって変化する。従って、設計値として、状態ベクトルの誤差共分散行列Ｐの初期値Ｐ_I、システム外乱ベクトルの誤差共分散行列Ｑ、観測外乱ベクトルの誤差共分散行列Ｒの値を設定すればよい。
【００３３】
図５は方位姿勢誤差演算部２４の構成例を示す。本例の方位姿勢誤差演算部２４は、数５の第１式の演算によって状態ベクトルの誤差共分散行列Ｐ（ｔ＋Δｔ，ｔ）を求める誤差共分散行列演算部３０１と数５の第２式の演算によってカルマンゲインＫ（ｔ＋Δｔ，ｔ）を求めるカルマンゲイン演算部３０２と数５の第３式の演算によってシステム誤差の最適予測値ｘ（ｔ＋Δｔ，ｔ）を求める最適予測値演算部３０３と数５の第５式の演算によって誤差共分散行列の推定値Ｐ（ｔ＋Δｔ，ｔ＋Δｔ）を求める誤差共分散行列修正演算部３０４と数５の第４式の演算によってシステム誤差の最適推定値ｘ（ｔ＋Δｔ，ｔ＋Δｔ）を求める最適推定値演算部３０５とを有する。
【００３４】
最適推定値演算部３０５によって求められたシステム誤差の最適推定値ｘ（ｔ＋Δｔ，ｔ＋Δｔ）は修正トルク信号として座標変換マトリックス演算部２１に供給される。それによって座標変換マトリックス演算部２１は座標変換マトリックスの誤差修正を行う。その結果は誤差共分散行列演算部３０１及び誤差共分散行列修正演算部３０４の出力として現れる。最適予測値演算部３０３は、誤差共分散行列演算部３０１及び誤差共分散行列修正演算部３０４の出力を入力して、それを状態変数に影響させるための演算を行う。
【００３５】
【発明が解決しようとする課題】
ストラップダウン方式の方位姿勢基準装置では、航行体座標系又は方位姿勢基準装置座標系を局地水平座標系に変換するための座標変換マトリックス（ＣＴＭ）を使用する。従って、方位及び姿勢角に関する情報は座標変換マトリックス（ＣＴＭ）に取り込まれ、方位及び姿勢角（ロール角及びピッチ角）はマトリックス化されている。これは各座標系の間における変数変換には必ず所定の定義されたロジックが存在し、このロジックを無視すると変数変換の際に予期しない誤差が発生する結果となる。
【００３６】
図６を参照して説明する。例えば、航行体座標系から局地水平座標系への変換が、方位角⇒ピッチ角⇒ロール角の順に定義付けられている場合、局地水平座標系から航行体座標系への逆変換は、ロール角⇒ピッチ角⇒方位角の順にしなければ元の航行体座標系に戻らない。以下に簡単な例を挙げて説明する。航行体座標系から局地水平座標系への座標変換をピッチ角β⇒ロール角αの順に行い、局地水平座標系から航行体座標系への座標変換をピッチ角β⇒ロール角αの順に行うと、得られた航行体座標系は元の航行体座標系に戻らず、角度γの方位角誤差が生ずる。
【００３７】
図６Ａにて、先ず航行体座標系をピッチ軸Ｐ−Ｐ周りに回転角度βだけ回転する。それによってロール軸Ｒ−Ｒは傾斜してＯ−Ｒ’となる。次に、この座標系を、傾斜したロール軸Ｏ−Ｒ’周りに回転角度αだけ回転する。それによってピッチ軸Ｐ−Ｐは傾斜してＯ−Ｐ’となり局地水平座標系Ｐ’−Ｏ−Ｒ’が得られる。
【００３８】
次に、この局地水平座標系を、傾斜したピッチ軸Ｏ−Ｐ’周りに回転角度βだけ反対方向に回転すると、破線にて示すようにロール軸はＯ−Ｒ”となる。この座標系をこのロール軸Ｏ−Ｒ”周りに回転角度αだけ反対方向に回転すると、航行体座標系が得られる。しかしながらこの航行体座標系にて、ロール軸及びピッチ軸は元の平面上に戻っているが、ロール軸Ｏ−Ｒ”は元のロール軸Ｏ−Ｒに一致しない。
【００３９】
図６Ｂに示すように、座標変換前のロール軸Ｏ−Ｒと座標変換を２度行った後のロール軸Ｏ−Ｒ”との間の角度γは次の式によって求められる。
【００４０】
【数７】
ｔａｎγ＝ｔａｎα・ｓｉｎβ
γ＝ｔａｎ^-1（ｔａｎα・ｓｉｎβ）
【００４１】
このような方位誤差γは座標変換マトリックス（ＣＴＭ）を使用するストラップダウン型の方位姿勢基準装置のアラインメントにおいて、時々刻々ＣＴＭ修正する場合には必ず生ずる誤差である。即ち、時々刻々ＣＴＭ修正を行うと、座標変換の定義を無視することになり、当然、無理に座標軸が水平になるような演算をすれば方位誤差が生ずる欠点がある。
【００４２】
カルマンフィルタを用いたアラインメントは、起立系及び指北系を適用したアラインメントに比べて高速且つ未知変数の推定及び修正が可能である長所を有する。しかしながら、姿勢角誤差と方位角誤差を同時に推定し、座標変換マトリックス（ＣＴＭ）の姿勢角修正及び方位角修正を行う場合、方位角誤差の推定には姿勢角誤差の推定に比べて、長時間を要する欠点がある。
【００４３】
また、カルマンフィルタを用いたアラインメントにて時々刻々ＣＴＭ修正を行う場合、方位角修正を行わずに姿勢角修正だけを行う状態が起きる。これは座標変換において方位軸周りの誤差修正を行わずにピッチ軸周り及びロール軸周りの誤差修正だけを行うこととなり、座標変換誤差を生ずる。
【００４４】
本発明は斯かる点に鑑み、座標変換マトリックス（ＣＴＭ）を使用するストラップダウン型の方位姿勢基準装置において、座標変換誤差が生ずることがないアラインメントを迅速に実行することができるようにすることを目的とする。
【００４５】
本発明は斯かる点に鑑み、座標変換マトリックス（ＣＴＭ）を使用するストラップダウン型の方位姿勢基準装置において、カルマンフィルタを用いたアラインメントを実行する場合、座標変換誤差が生ずることがなく迅速にアラインメント実行することができるようにすることを目的とする。
【００４６】
【課題を解決するための手段】
本発明によると、方位姿勢基準装置は、航行体の主要３軸に沿った入力軸線を有し航行体に取り付けられた３個のジャイロと３個の加速度計および速度センサからの出力を信号演算に用いて方位姿勢角を出力するストラップダウン型の方位姿勢基準装置において、３個のジャイロの出力を入力して座標変換マトリックスを演算するＣＴＭ演算部と、ＣＴＭ演算部から出力された座標変換マトリックスと３個の加速度計の出力を入力して加速度の水平成分を求める加速度水平成分演算部と、加速度水平成分演算部の出力を入力し速度の水平成分を求める速度水平成分演算部と、速度水平成分演算部の出力と速度センサの出力を入力し比較することで姿勢誤差を推定する姿勢誤差演算部と、方位角の設定値を出力する方位設定部と、上記姿勢誤差演算部によって推定された姿勢誤差が予め定められた値に基づく確度にて推定されてから上記姿勢誤差の推定値によって座標変換の定義に従って上記座標変換マトリックスの姿勢誤差修正し、上記座標変換マトリックスの姿勢誤差修正がなされてから上記方位角の設定値によって上記座標変換マトリックスの方位誤差修正するように、姿勢誤差演算部の上記確度推定の判定出力に基づいて姿勢誤差演算部の姿勢誤差出力と方位設定部の方位誤差出力のタイミングを制御するための修正信号制御部とを有するように構成されている。姿勢誤差演算部はカルマンフィルタによって姿勢誤差を推定演算する。
【００４７】
本発明によると、方位姿勢基準装置において、姿勢誤差演算部の出力と方位設定部の出力のタイミングを制御するための修正信号制御部が設けられている。
【００４８】
本発明によると、ストラップダウン型方位姿勢基準装置のアラインメント方法は、航行体の主要３軸に沿った入力軸線を有し航行体に取り付けられた３個のジャイロと３個の加速度計および速度センサからの出力を信号演算に用いて方位姿勢角を出力するストラップダウン型の方位姿勢基準装置のアライメント方法において、３個のジャイロの出力を入力してＣＴＭ演算部により座標変換マトリックスを演算するステップと、上記ＣＴＭ演算部から出力された座標変換マトリックスと３個の加速度計の出力を入力して加速度水平成分演算部により加速度の水平成分を求めるステップと、上記加速度水平成分演算部の出力を入力し速度水平成分演算部により速度の水平成分を求めるステップと、上記速度水平成分演算部の出力と速度センサの出力を入力し比較することで姿勢誤差演算部により姿勢誤差を推定するステップと、方位角の設定値を方位設定部により出力するステップと、上記姿勢誤差演算部によって推定された姿勢誤差が予め定められた値に基づく確度にて推定されてから上記姿勢誤差の推定値によって座標変換の定義に従って上記座標変換マトリックスの姿勢誤差修正し、上記座標変換マトリックスの姿勢誤差修正がなされてから上記方位角の設定値によって上記座標変換マトリックスの方位誤差修正するように、上記姿勢誤差演算部の上記確度推定の判定出力に基づいて修正信号制御部により上記姿勢誤差演算部の姿勢誤差出力と上記方位設定部の方位誤差出力のタイミングを制御するステップとを含むことを特徴とする。更に、このアラインメント方法は航行体の航行中に実行されることを特徴とする。
【００４９】
こうして本発明によると、ストラップダウン型方位姿勢基準装置のアラインメントにおいて、時々刻々ＣＴＭ修正を行う場合、略完全に推定した姿勢角誤差と方位角の設定値を使用するから、３軸の座標変換を定義に従って行うことが可能となり、座標変換誤差が生じない。
【００５０】
【発明の実施の形態】
図面を参照して説明する前に本発明の概念を説明する。本発明によるアラインメントは次のような内容及びステップを含む。
（１）先ず姿勢角誤差の推定のみを行う。姿勢角誤差の推定は、好ましくはカルマンフィルタを用いて行う。従来のように方位及び姿勢角誤差の両者を推定する場合に比べて、方位角推定を行わないから、姿勢角誤差の推定速度が向上し、短時間の推定が可能となる。また、状態変数マトリックスの次数を下げることができるから、推定演算の高速化及び処理演算の負荷を軽減することができる。
（２）姿勢角誤差の推定値が所定の確度にて得られるようになったと判断されると、この姿勢角誤差の推定値によって座標変換の定義に従って座標変換マトリックス（ＣＴＭ）の姿勢角誤差修正をする。即ち、姿勢角誤差が所定の確度によって推定されるまでは、座標変換マトリックス（ＣＴＭ）の誤差修正は行わない。その理由は、発明が解決しようとする課題の欄にて説明したように、姿勢角誤差の推定値が正確でない場合に、それによって方位誤差γが増大し、方位誤差γに姿勢角誤差が累積的に蓄積されるメカニズムを除去するためである。
【００５１】
（３）姿勢角誤差の推定値による座標変換マトリックス（ＣＴＭ）の誤差修正が終了した後に、方位角の設定値によって座標変換マトリックス（ＣＴＭ）を誤差修正する。方位角の設定値は、手動又は自動によって設定される。方位角の設定値を使用することによって、座標変換マトリックス（ＣＴＭ）の方位誤差修正が迅速化される。
（４）こうして、アラインメントが短時間にて終了する。尚、座標変換マトリックス（ＣＴＭ）の内容は、ＸＹＺジャイロの出力信号であるＸレート、Ｙレート及びＺレートによって刻々更新されており、航行体の運動によって誤差が生ずることはない。本例のアラインメントは航行体の出発前に実行してもよいが、短時間にて終了するため航行中に実行してもよい。例えば、本例のアラインメントを航空機に適用する場合、飛行前のアラインメントばかりでなく飛行中のアラインメント、即ち、インフライトアラインメントとして使用可能である。
【００５２】
図１に本発明による方位姿勢基準装置の信号演算部５のアラインメント演算ブロックの詳細を示す。本例の信号演算部５は図示のように、ＣＴＭ演算部（座標変換マトリックス演算部）２１、加速度水平成分演算部２２、速度水平成分演算部２３、姿勢誤差演算部２５、方位設定部２６、修正信号制御部２７及び２つのスイッチ部２８、２９を有する。
【００５３】
本例の信号演算部５のアラインメント演算ブロックは図４に示した従来の信号演算部５のアラインメント演算ブロックと比較して、方位姿勢誤差演算部２４の代わりに姿勢誤差演算部２５、方位設定部２６、修正信号制御部２７及び２つのスイッチ部２８、２９が設けられている点が異なり、それ以外の部分、即ち、ＣＴＭ演算部（座標変換マトリックス演算部）２１、加速度水平成分演算部２２及び速度水平成分演算部２３は同様であってよい。
【００５４】
姿勢誤差演算部２５は速度水平成分演算部２３より供給された速度の水平成分信号Ｖ_EW、Ｖ_SNと速度センサ３から供給された（基準）速度信号Ｒ_EW、Ｒ_SNを入力して両者の偏差δ_EW、δ_SNを演算し、この偏差δ_EW、δ_SNに基づいて姿勢角の誤差を推定演算する。即ち、この偏差δ_EW、δ_SNをシステム誤差としてカルマンフィルタを用いて姿勢角誤差の最適推定値を演算する。
【００５５】
本例の姿勢誤差演算部２５はカルマンフィルタを用いて姿勢誤差の最適推定値を演算するが、方位誤差の推定演算しない。方位角の推定値は方位設定部２６より出力される。方位設定部２６は予め設定された方位角を保持している。修正信号制御部２７は方位修正トルク信号と姿勢修正トルク信号の出力タイミングを制御する。
【００５６】
上述のように、姿勢誤差が所定の確度にて推定されるようになったと判断されると姿勢誤差演算部２５から修正信号制御部２７へ判断信号が出力される。修正信号制御部２７は姿勢誤差演算部２５からの判断信号を入力すると、第１のスイッチ２８に作動信号を供給する。それによってスイッチ２８は開位置に変化し、姿勢誤差演算部２５より出力された姿勢誤差の推定値は姿勢修正トルク信号として座標変換マトリックス演算部２１に供給される。
【００５７】
座標変換マトリックス演算部２１にて座標変換マトリックスの姿勢誤差修正が終了したと判断されると姿勢誤差演算部２５から修正信号制御部２７へ終了信号が出力される。修正信号制御部２７は姿勢誤差演算部２５からの終了信号を入力すると、第２のスイッチ２９に作動信号を供給する。それによってスイッチ２９は開位置に変化し、方位設定部２６より方位角の設定値が方位修正トルク信号として座標変換マトリックス演算部２１に供給される。
【００５８】
尚、図示の例にて、第１及び第２のスイッチ２８、２９は姿勢誤差演算部２５及び方位設定部２６とは別個に設けられているが、第１のスイッチ２８を姿勢誤差演算部２５に組み込み、第２のスイッチ２９を方位設定部２６に組み込んでもよい。
【００５９】
図２を参照して本例による姿勢誤差演算部２５の構成例を説明する。本例の姿勢誤差演算部２５は図５に示した従来の方位姿勢誤差演算部２４の構成と比較して、修正トルク信号出力制御部３０６が設けられている点が異なり、それ以外の構成は同様であってよい。
【００６０】
最適推定値演算部３０５はカルマンフィルタによって姿勢角の誤差の最適推定値を演算するが、この姿勢角の誤差の最適推定値が高い確度にて得られるまでは出力しない。修正トルク信号出力制御部３０６は、姿勢角の誤差の最適推定値が確度が高い推定値であると判定すると、判定信号を生成し、修正信号制御部２７に供給する。
【００６１】
確度が高い推定値であるか否かは、例えば、最適推定値演算部３０５における推定演算の開始より経過した時間に基づいて判定してもよく、又は、カルマンフィルタ内の誤差共分散行列の値に基づいて判定してもよい。確度が高い推定値が得られてから姿勢角修正する理由は上述のように、逐次的に姿勢角誤差を修正することによって方位角誤差が累積的に生成するメカニズムを回避するためである。
【００６２】
こうして、姿勢角誤差の推定値が高い確度で得られてから、この姿勢角誤差の推定値によって座標変換マトリックスの姿勢角修正を行う。修正トルク信号出力制御部３０６は、この姿勢角修正がなされたと判断すると、姿勢角修正終了信号を生成し、それを修正信号制御部２７に供給する。
【００６３】
こうして座標変換マトリックスの姿勢角誤差修正と方位角誤差修正を分離し、更に方位角誤差修正には方位角の設定値を使用することによってアラインメントの迅速化が達成される。
【００６４】
以上本発明の実施の形態について詳細に説明したが、本発明はこれらの例に限定されることなく特許請求の範囲に記載された発明の範囲にて様々な変更等が可能であることは当業者にとって理解されよう。
【００６５】
【発明の効果】
本発明によると、座標変換マトリックスの姿勢角誤差修正と方位角誤差修正を分離し、更に方位角誤差修正には方位角の設定値を使用することによってアラインメントを短時間にて実行することができる利点がある。
【００６６】
本発明によると、確度の高い姿勢角誤差の推定値と方位角の設定値を使用して３軸のアラインメントを行うから、３軸の座標変換を定義に従って行うことができ、座標変換誤差が生じない利点がある。
【００６７】
本発明によると、アラインメントを短時間にて実行することができるから、航行体の出発時ばかりでなく航行中にもアラインメントを実施することができる利点がある。
【００６８】
本発明によると、簡単な構成によってストラップダウン式の方位姿勢基準装置のアラインメントを短時間化することができる利点がある。
【図面の簡単な説明】
【図１】本発明による方位姿勢基準装置の信号演算部の構成例を示す図である。
【図２】本発明による方位姿勢基準装置の信号演算部の姿勢誤差演算部の構成例を示す図である。
【図３】従来の方位姿勢基準装置の構成例を示す図である。
【図４】従来の方位姿勢基準装置の信号演算部の例を説明するための説明図である。
【図５】従来の方位姿勢誤差演算部の信号演算部の方位姿勢誤差演算部の構成例を示す図である。
【図６】座標変換マトリックスに起因した方位誤差発生のメカニズムを説明するための説明図である。
【符号の説明】
１Ａ，１Ｂ，１Ｃジャイロ、２Ａ，２Ｂ，２Ｃ加速度計、３速度計、４信号入力部、５信号演算部、６信号出力部、２１ＣＴＭ演算部（座標変換マトリックス演算部、２２加速度水平成分演算部、２３速度水平成分演算部、２５姿勢誤差演算部、２６方位設定部、２７修正信号制御部、２８，２９スイッチ部、

Claims

航行体の主要３軸に沿った入力軸線を有し航行体に取り付けられた３個のジャイロと３個の加速度計および速度センサからの出力を信号演算に用いて方位姿勢角を出力するストラップダウン型の方位姿勢基準装置において、
３個のジャイロの出力を入力して座標変換マトリックスを演算するＣＴＭ演算部と、
上記ＣＴＭ演算部から出力された座標変換マトリックスと３個の加速度計の出力を入力して加速度の水平成分を求める加速度水平成分演算部と、
上記加速度水平成分演算部の出力を入力し速度の水平成分を求める速度水平成分演算部と、
上記速度水平成分演算部の出力と速度センサの出力を入力し比較することで姿勢誤差を推定する姿勢誤差演算部と、
方位角の設定値を出力する方位設定部と、
上記姿勢誤差演算部によって推定された姿勢誤差が予め定められた値に基づく確度にて推定されてから上記姿勢誤差の推定値によって座標変換の定義に従って上記座標変換マトリックスの姿勢誤差修正し、上記座標変換マトリックスの姿勢誤差修正がなされてから上記方位角の設定値によって上記座標変換マトリックスの方位誤差修正するように、上記姿勢誤差演算部の上記確度推定の判定出力に基づいて上記姿勢誤差演算部の姿勢誤差出力と上記方位設定部の方位誤差出力のタイミングを制御するための修正信号制御部と、
を有するように構成された方位姿勢基準装置。
請求項１記載の方位姿勢基準装置において、
上記姿勢誤差演算部はカルマンフィルタによって上記姿勢誤差を推定演算することを特徴とする方位姿勢基準装置。
航行体の主要３軸に沿った入力軸線を有し航行体に取り付けられた３個のジャイロと３個の加速度計および速度センサからの出力を信号演算に用いて方位姿勢角を出力するストラップダウン型の方位姿勢基準装置のアライメント方法において、
３個のジャイロの出力を入力してＣＴＭ演算部により座標変換マトリックスを演算するステップと、
上記ＣＴＭ演算部から出力された座標変換マトリックスと３個の加速度計の出力を入力して加速度水平成分演算部により加速度の水平成分を求めるステップと、
上記加速度水平成分演算部の出力を入力し速度水平成分演算部により速度の水平成分を求めるステップと、
上記速度水平成分演算部の出力と速度センサの出力を入力し比較することで姿勢誤差演算部により姿勢誤差を推定するステップと、
方位角の設定値を方位設定部により出力するステップと、
上記姿勢誤差演算部によって推定された姿勢誤差が予め定められた値に基づく確度にて推定されてから上記姿勢誤差の推定値によって座標変換の定義に従って上記座標変換マトリックスの姿勢誤差修正し、上記座標変換マトリックスの姿勢誤差修正がなされてから上記方位角の設定値によって上記座標変換マトリックスの方位誤差修正するように、上記姿勢誤差演算部の上記確度推定の判定出力に基づいて修正信号制御部により上記姿勢誤差演算部の姿勢誤差出力と上記方位設定部の方位誤差出力のタイミングを制御するステップと、
を含むように構成された方位姿勢基準装置のアライメント方法。
航行体の航行中に実行されることを特徴とする請求項３記載のストラップダウン型方位姿勢基準装置のアライメント方法。