JPH11148827A

JPH11148827A - 方位姿勢基準装置

Info

Publication number: JPH11148827A
Application number: JP9315238A
Authority: JP
Inventors: Toru Maeda; 前田　　徹; Mikio Morohoshi; 幹雄諸星
Original assignee: Tokimec Inc
Current assignee: Tokimec Inc
Priority date: 1997-11-17
Filing date: 1997-11-17
Publication date: 1999-06-02
Anticipated expiration: 2017-11-17
Also published as: JP3919911B2

Abstract

(57)【要約】【課題】ストラップダウン型の方位姿勢基準装置におい
てアラインメントを迅速化し、インフライトでも実施可
能にすることを目的とする。【解決手段】座標変換マトリックス（ＣＴＭ）の誤差
修正において、姿勢角誤差修正と方位角誤差修正を分離
し、姿勢角誤差修正を行ってから方位角誤差修正を行
う。先ず姿勢角誤差の推定値が所定の確度にて得られる
ようになったら、この姿勢角誤差の推定値によって座標
変換マトリックス（ＣＴＭ）の誤差修正をし、それが終
了した後に、方位角の設定値によって座標変換マトリッ
クス（ＣＴＭ）を誤差修正する。

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

【特許請求の範囲】

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