JP2001153680A - 慣性装置 - Google Patents

Abstract

(57)【要約】 【課題】 慣性装置の初期アライメント計算時において
ジャイロ誤差にもとづく方位角誤差の増加を抑圧する。 【解決手段】 初期アライメント計算部３２に方位角補
正演算部２４を設け、初期アライメント計算中に、計算
した方位角Ψ_C を方位角補正演算部２４に入力して、そ
の出力を姿勢基準行列計算部３に帰還補正して、方位角
Ψ_C がゼロに収斂するようにする。

Description

【発明の詳細な説明】

【０００１】

【発明の属する技術分野】この発明は、ジャイロ、加速
度計の信号を用いて角速度、加速度、姿勢角、方位角、
速度、位置などを計算し、その際に、姿勢角、方位角の
初期値を設定するための初期アライメント計算部を備え
た慣性装置に関する。

【０００２】

【従来の技術】図２に従来のストラップダウン型慣性装
置の基本的機能構成を示す。この種の装置は例えば特開
平８−２１７４０号公報に詳細に述べられているので以
下に簡単に述べる。ジャイロ１から慣性装置内の直交座
標軸（航空機などに慣性装置が搭載された場合機体の進
行方向がｘ軸、右方向がｙ軸）回りの角速度（機軸角速
度）ω_x ，ω_y ，ω_z と加速度計２からの機軸加速度ａ
_x ，ａ_y ，ａ_z が演算処理部３１に入力され、演算処理
部３１内の姿勢基準行列計算部３において機軸角速度ω
_x ，ω_y ，ω_z を積分して姿勢基準行列が計算され、そ
の姿勢基準行列を用いて機軸加速度ａ_x ，ａ_y ，ａ_z が
加速度座標変換部４で基準座標軸加速度Ａ_X ，Ａ_Y に座
標変換され、更に速度計算部６で積分されて速度Ｖ_X ，
Ｖ_Y に変換される。この速度Ｖ_X ，Ｖ_Y が位置行列計算
部７で積分され、その位置行列を用いて位置計算部８で
位置が計算される。

【０００３】また姿勢角、方位角計算部５において姿勢
角（ロール角：φ、ピッチ角：θ）、方位角（Ψ）が姿
勢基準行列Ｃから通常下式により計算される。 φ＝ｔａｎ^-1（Ｃ_3,2 ／Ｃ_3,3 ） θ＝ｓｉｎ^-1（−Ｃ_3,1 ） Ψ_C ＝ｔａｎ^-1（Ｃ_2,1 ／Ｃ_1,1 ） Ψ＝Ψ_C ＋α Ｃ：姿勢基準行列（３×３行列） Ｃ_i,j ：Ｃのｉ，ｊ要素 α：航法計算座標軸の方位であって位置行列から座標軸
方位計算部９で計算される。

【０００４】位置行列と速度Ｖ_X ，Ｖ_Y とから座標軸回
転レートを計算部１０で計算し、この計算結果により姿
勢基準行列計算部３に対する補正を行う。またこの座標
軸回転レートとＶ_X ，Ｖ_Y とから座標軸回転により生じ
るコリオリ加速度を計算部１１で計算し、これにより速
度計算部６に対する補正を行う。その際、速度、位置行
列、姿勢基準行列演算には、それぞれの初期値を設定す
る必要があり、位置データは何らかの方法により外部か
ら入力され設定されるが、姿勢角、方位角計算の基とな
る姿勢基準行列、速度はジャイロ、加速度計が計測する
地球自転角速度、地球重力データに基づいて計算された
結果が設定される。この計算を通常初期アライメント計
算と呼んでいる。

【０００５】初期アライメント計算は初期アライメント
計算部３２で行うがその従来の方法を図３に示す。この
方法は、慣性装置に組込まれ航空機等の機体に取付けら
れた加速度計が計測した地球重力データを正しく設定さ
れた姿勢基準行列を用いて基準座標に座標変換すれば鉛
直方向成分のみの加速度データになるべきものが、計算
開始時に予め設定した姿勢基準行列の値に誤差がある場
合、加速度計信号を座標変換した後の基準座標における
加速度データは、鉛直成分の他に水平成分Ａ_X ，Ａ_Y も
算出される。そのため、座標変換後の加速度データを積
分して速度を計算すると姿勢基準行列に誤差がある場合
水平方向に速度が発生することになる。この現象は、計
算開始時に設定された姿勢基準行列の値に誤差がない場
合でも、ジャイロによって計測される地球自転角速度デ
ータにより姿勢基準行列が更新され、姿勢基準行列の誤
差となり同様に水平方向速度が発生する。

【０００６】この水平方向速度Ｖ_X ，Ｖ_Y を図３に示す
ように計算部２１で帰還利得を掛けて速度フィードバッ
クデータＦＡ_X ，ＦＡ_Y を作り、これをＡ_X ，Ａ_Y に帰
還減算し、また計算部２２でＶ_X ，Ｖ_Y に帰還利得を掛
けて、姿勢角フィードバックデータＦω_X ，Ｆω_Y を作
る。更にＶ_X ，Ｖ_Y を計算部２３で帰還利得を掛けて積
分（１／Ｓは積分記号を表わす）することにより推定角
速度Ｅω_X ，Ｅω_Y を求め、このＥω_X ，Ｅω_Y を姿勢
角フィードバックデータＦω_X ，Ｆω_Y と加算し、これ
らの加算結果Ω_X ，Ω_Y にて姿勢基準行列計算部３内の
姿勢基準行列を補正して、Ａ_X ，Ａ_Y が小さくなるよう
にする。地球自転角速度鉛直軸成分計算部１０ａにその
慣性装置の位置における緯度が入力されて、地球自転角
速度鉛直軸成分Ω_Z （＝ωＥ_Z ）が計算され、これも姿
勢基準行列計算部３に入力される。

【０００７】このように慣性装置に組込まれた航空機な
どの機体が静止した状態で、基準座標系の水平方向の加
速度成分Ａ_X ，Ａ_Y を処理して姿勢基準行列計算部３、
速度計算部６にフィードバックすることを繰返すことに
より姿勢基準行列の水平に対する誤差を減少させて正し
い姿勢基準行列が計算されることとなる。その結果を初
期値として、機体移動中において慣性装置は姿勢基準行
列を求め、正しい姿勢角が計算されることになる。

【０００８】一方、図３中に示すフィードバック処理の
計算部２３でＫ₃ の掛かるフィードバック項Ｅω_X ，Ｅ
ω_Y は定常的には地球自転角速度の基準座標系における
水平成分ω_X ，ω_Y の推定値を示し、この２つの値を使
用して基準座標系Ｘ軸の北からの方位αを次式により算
出することができる。（図４参照） α＝ｔａｎ^-1（−Ｅω_Y ／Ｅω_X ） …（１） ちなみに、北基準局地水平座標軸Ｎ，Ｅ，Ｄ（北、東、
鉛直下方向）と、速度計算などを行う基準座標系でＸ軸
が任意の方位にある局地水平座標軸Ｘ，Ｙ，Ｚ（Ｘ，Ｙ
は局地水平面内、Ｚ軸は鉛直下方向）との関係は例えば
図４Ａに示すようになる。計算部２３で計算されたフィ
ードバック項Ｅω_X ，Ｅω_Y は先に延べたように定常的
には地球自転角速度の基準座標系における水平成分
ω_X ，ω_Y の推定値を示し、これらは図４Ｂに示すよう
に、地球自転角速度北方向成分ωＥ_Nに対し、ω_X ＝ω
Ｅ_N ×ｃｏｓα、ω_Y ＝−ωＥ_N ×ｓｉｎαなる関係に
あり、Ｅω_X ＝ω_X ，Ｅω_Y ＝ω_Y となった状態ではα
は式（１）により求まる。

【０００９】こうして求まった基準座標系Ｘ軸の方位α
と、姿勢基準行列から計算される基準座標系Ｘ軸に対す
る慣性装置ｘ軸の方位Ψ_C を加算することによりｘ軸の
北からの方位角Ψが次式により計算される。 Ψ＝Ψ_C ＋α …（２） 基準座標軸Ｘ，Ｙ，Ｚと、慣性装置内の直交座標軸（一
般にジャイロ、加速度計が取付けられる軸であり、慣性
装置が搭載された機体の進行方向、右方向、下方向と一
致する）ｘ，ｙ，ｚとは図４Ｃに示す関係とする。姿勢
角、方位角計算部５では、ｘ軸のＸ，Ｙ面間におけるＸ
軸に対する方向Ψ_C が計算される。よって慣性装置のｘ
軸の北に対する方位角は式（２）により求まる。

【００１０】

【発明が解決しようとする課題】従来の慣性装置は、加
速度計出力を基準座標系に座標変換して求めた水平方向
速度データを用いて姿勢角誤差の修正、地球自転角速度
の推定を行い、推定された地球自転角速度の水平方向成
分Ｅω_X ，Ｅω_Y を用いて基準座標系Ｘ軸の北の方から
の方位角αを計算し、更に基準座標系に対する慣性装置
の方位角Ψ_C を加えて北に対する慣性装置の方位角Ψを
算出している。

【００１１】この方法では、鉛直軸に対しては初期アラ
イメント計算実施地点の緯度データを外部から入力して
地球自転角速度の鉛直軸成分の補正を行っているため、
機体が静止していれば、慣性装置の方位角Ψ_C は一定の
値（通常はゼロ）にとどまっているが、ジャイロに誤差
があると誤った角速度を計測し機体が回転しているのと
等価な状態となり、慣性装置の方位角Ψ_C が変化してし
まう。このΨ_C の変化に対し、地球自転角速度の推定計
算においてαがΨ_C の変化分を補うように変化するが完
全に補いきれず方位角の誤差が発生することになる。

【００１２】所で計算部２１〜２３における帰還利得Ｋ
₁ 〜Ｋ₃ の各値を小さくして外乱の影響を受け難くして
いるため、初期アライメント計算に、航空機の場合は３
分程度、ロケットの場合は１時間程度も時間を掛けてい
る。このため、ジャイロに誤差があると、姿勢基準行列
計算部３での積分動作のため、誤差が初期アライメント
計算中積分されて増加する、このため、図５Ａに示すよ
うに初期アライメント計算中に方位角誤差が時間と共に
増加し、特に初期アライメント計算が比較的長い場合は
方位角誤差が大きなものとなり慣性装置としての誤差も
大きなものとなった。

【００１３】この発明の目的は、初期アライメント計算
中に発生する方位角誤差を補正し、精度良く方位角を計
算することができる慣性装置を提供することにある。

【００１４】

【課題を解決するための手段】この発明によれば姿勢基
準行列に基づき計算した方位角Ψ_C 、この値が小さくな
るように姿勢基準行列計算部に帰還する方位角補正演算
部を、初期アライメント計算部に設ける。つまりこの発
明は、慣性装置の初期アライメント計算が、慣性装置が
搭載された機体が移動しない（機体の動揺等はあっても
オフセット的に方位角が変化しない）という条件下で行
われることを利用し、初期アライメント計算で計算され
た方位角Ψ_C は振動状に変動しても時間とともに増加
（または減少）するものではないので、このΨ_C の値が
ゼロになるように補正量を計算し、地球自転角速度の鉛
直軸成分と合わせて補正することにより、ジャイロ誤差
によりΨ_C に誤差が発生することをふせぎ、機体の正し
い方位角Ψを計算することを可能とする。

【００１５】

【発明の実施の形態】この発明の要部は慣性装置中の初
期アライメント計算部３２にあり、その実施例を図３と
対する部分に同一符号を付けて示す。この実施例によれ
ば、初期アライメント計算部３２に方位角補正演算部２
４を設け、初期アライメント計算中に、姿勢角、方位角
計算部５で計算された方位角Ψ_C を方位角補正演算部２
４に入力し、その方位角補正演算部２４の出力を姿勢基
準行列計算部３に帰還して方位角Ψ_C の値が小さくなる
ように、つまりΨ_C がゼロに収斂するようにする。

【００１６】方位角補正演算部２４では例えば図１中に
示すように入力されたΨ_C に対し、比例演算部２４ａで
帰還利得Ｋ₄ を乗算しその出力を姿勢基準行列計算部３
に帰還入力するだけでもよく、更に積分演算部２４ｂで
Ψ_C に帰還利得Ｋ₅ を掛けて積分した結果をも姿勢基準
行列計算部３に帰還してもよい。この方位角補正演算部
２４の出力Ｆω_Z の帰還は姿勢基準行列計算部３に直接
行ってもよく、図１中に示すように、地球自転角速度鉛
直軸成分計算部１０ａの出力ωＥ_Z と加算して姿勢基準
行列計算部３に帰還してもよい。

【００１７】この構成により、Ｋ₄ Ψ_C の帰還作用によ
り、ジャイロ誤差に基づく増大していく方位角誤差は微
小レベルに抑えられ、例えば図５Ｂに示すように、方位
角誤差は初期アライメント計算中増加しないようにする
ことができる。また帰還利得Ｋ₄ を単なる利得とするこ
となく、適当な時定数をもたせることにより、機体の動
揺により方位角Ψ_C が振動状に変動しても、その平均な
量が抑圧され、かつジャイロ誤差により増大していく方
位誤差が微小レベルに抑えられるように、フィルタ効果
を持たすこともできる。

【００１８】更に積分演算部２４ｂも設ける場合は、図
５Ｂ中における消去できない微小誤差を積分して、この
誤差がゼロになるように動作させることもできる。上述
においては初期アライメント計算部３２に基準座標速度
Ｖ_X ，Ｖ_Y を入力として初期アライメント計算を行った
が、Ｖ_X ，Ｖ_Y を積分した位置データを初期アライメン
ト計算部３２に入力して、初期アライメント計算を行う
こともできる。要は基準座標軸加速度Ａ_X ，Ａ_Y にもと
づく信号により初期アライメント計算を行えばよい。

【００１９】

【発明の効果】以上述べたようにこの発明によれば、慣
性装置の初期アライメント計算中に、加速度計などの基
準軸に対する方位角計算値Ψ_C がゼロになるように帰還
制御が、方位角補正演算部２４により姿勢基準行列計算
部３に対して行われるため、ジャイロ誤差による時間と
ともに増大する初期アライメント計算の方位角誤差を微
小レベルに抑えることができる。

【図面の簡単な説明】

【図１】この発明の実施例の要部である初期アライメン
ト計算部を示すブロック図。

【図２】ストラップダウン型慣性装置の基本的構成を示
すブロック図。

【図３】従来の慣性装置の初期アライメント計算部を示
すブロック図。

【図４】Ａは基準座標軸の定義を示す図、Ｂは基準座標
軸における地球自転角速度成分を示す図、Ｃは基準座標
に対する慣性装置姿勢角、方位角の定義を示す図であ
る。

【図５】Ａは従来の慣性装置における初期アライメント
計算時の方位角誤差の変化状態を示す図、Ｂはこの発明
による慣性装置における初期アライメント計算時の方位
角誤差の変化状態を示す図である。

Claims (1)

【特許請求の範囲】
1. 【請求項１】 ジャイロの出力から姿勢基準行列計算部
   で姿勢基準行列を計算し、その姿勢基準行列により方位
   角、姿勢角を計算し、また加速度計の出力を姿勢基準行
   列により基準座標軸加速度に変換し、その基準座標軸加
   速度から得られた信号を初期アライメント計算部に入力
   し、その初期アライメント計算部の出力により上記姿勢
   基準行列計算部の姿勢基準行列の値を帰還補正して、姿
   勢基準行列の初期値を決定する慣性装置において、 上記姿勢基準行列に基づき計算した方位角を、この値が
   小さくなるように上記姿勢基準行列計算部に帰還する方
   位角補正演算部が上記初期アライメント計算部に設けら
   れていることを特徴とする慣性装置。