JP2756554B2 - 慣性装置 - Google Patents

Description

【発明の詳細な説明】

【０００１】

【産業上の利用分野】この発明は、航空機等に搭載さ
れ、姿勢角（ロール角とピッチ角）、方位角等を求める
慣性装置に関し、特に初期アライメントを行う際に、外
部より初期緯度の入力を不要にする技術に関する。

【０００２】

【従来の技術】従来の慣性装置は図３に示すように、ジ
ャイロスコープ（以下ジャイロと言う）１，加速度計２
及び慣性信号演算部３で構成される。この慣性装置が搭
載される機体が静止している状態において、ジャイロ１
が受感する角速度（地球自転角速度）及び加速度計２が
受感する加速度（重力加速度）を用いて、機体の姿勢角
（ロール角及びピッチ角）を表すのに必要な、機体に固
定された機体軸座標系（ｘ，ｙ，ｚ）を真下方向の鉛直
軸Ｚ及びそれに直角なＸ軸、Ｙ軸より成る局地水平座標
系（Ｘ，Ｙ，Ｚ）へ変換する座標変換行列（方向余弦行
列と言う）Ｃが求められる（初期アライメントと言
う）。

【０００３】図４に示すように、ｘ軸がＸ，Ｙ，Ｚの各
軸となす角をα₁ ，α₂ ，α₃ ；ｙ軸がＸ，Ｙ，Ｚの各
軸となす角をβ₁ ，β₂ ，β₃ ；ｚ軸がＸ，Ｙ，Ｚ軸と
なす角をγ₁ ，γ₂ ，γ₃ で表すと、座標変換行列Ｃは

【０００４】

【数１】 で与えられる。なお、図５に示す地球上の緯度λの現在
地点Ｐを座標の原点としたＮ(North),Ｅ(East)，Ｄ(Dow
n)の３軸より成る直交座標系をそのＤ軸の周りに方位角
ψ₂ だけ時計方向に回転させた座標系が前記局地水平座
標系（Ｘ，Ｙ，Ｚ）である（図６）。

【０００５】さて、図３に話しを戻すと、ジャイロ１で
は、機体が静止時に、入力される地球自転角速度ω_e よ
り機体軸座標系の角速度ベクトルω_b （ω_x ，ω_y ，ω
_z ）が検出されて、方向余弦行列計算部４へ入力され
る。方向余弦行列計算部４では、時刻ｎ＋１における方
向余弦行列Ｃ_n+1 は、時刻ｎの同行列Ｃ_n とジャイロ１
の出力ω_b （ω_x ，ω_y ，ω_z ）とを用いて、

【０００６】

【数２】 が演算される。こゝでＩは単位行列を示し、θ₀ ，Δθ
は θ₀ ＝（Δθ_x ² ＋Δθ_y ² ＋θ_z ² )^1/2 ……（３）

【０００７】

【数３】

【０００８】

【数４】 （２）式の同行列Ｃ_n+1 には一般に機体の動きによる角
速度の他に、地球自転角速度の影響が含まれているの
で、同行列Ｃ_n+1 に対して局地水平座標系の地球自転角
速度による分を補正して、同行列Ｃ_n+1 から地球自転角
速度成分を取り除く。即ち、 Ｃ_n+1 ′＝Ｃ_n+1 −Δθ_T ・Ｃ_n+1 ……（６） こゝで、

【０００９】

【数５】 ＴＱ_X ＝（ω_X ＾＋Ｋ₂ ・Ｖ_Y ）ΔＴ ； ＴＱ_Y ＝（ω_Y ＾＋Ｋ₂ Ｖ_X ）ΔＴ ； ＴＱ_Z ＝−ω_e ・sin λ・ΔＴ ； ……（８） と表される。Ｋ₂ はフィードバック・ゲイン，ΔＴはサ
ンプリング周期である。（６）式により同行列Ｃ_n+1 ′
が計算され、その後Ｃ_n+1 ′はＣ_n+1 に置き換えられ
る。（２）式のＣ_n ，Ｃ_n+1 はこの置き換えたものであ
る。（７）式のＴＱ _X ，ＴＱ_Y は、行列Ｃの地球自転角
速度に起因する補正量（トルキング量と言う）のＸ軸、
Ｙ軸方向の成分で、地球自転角速度成分の推定値ω
_X ＾，ω_Y ＾と、機体の速度Ｖ_X ，Ｖ_Y を用いて後述の
第１トルキング量計算部１０で演算される。またＴＱ_Z
はトルキング量のＺ軸方向成分で、地球自転角速度成分
ω_Z ＝−ω _e ・sin λ（λは緯度）を用いて後述の第２
トルキング量計算部１１で演算される。

【００１０】姿勢角計算部６では、方向余弦行列Ｃを用
いて、機体のロール角φ，ピッチ角θと、ｘ軸がＸ軸と
なす角をＸＹ平面に投影した角ψ₁ とが、次式により計
算される。 φ＝tan ^-1（Ｃ₃₂／Ｃ₃₃） ……（９） θ＝tan ^-1｛−Ｃ₃₁／（１−Ｃ₃₁ ²)^1/2 ｝ ……（10） ψ₁ ＝tan ^-1（Ｃ₂₁／Ｃ₁₁） ……（11） なお、詳しい説明は省略するが、機体軸座標系（ｘ，
ｙ，ｚ）を局地水平座標系（Ｘ，Ｙ，Ｚ）に変換する座
標変換行列である方向余弦行列Ｃの要素を用いて求めた
（11）式の角ψ₁ は、図７に示したように、ｘ軸がＸ軸
となす角をＸＹ平面上に投影した角に等しい。

【００１１】加速度計２では、機械の静止時に、入力さ
れる重力加速度ｇにより機体軸座標系の加速度ベクトル
ａ_b （ａ_x ，ａ_y ，ａ_z ）が検出されて、座標変換部５
に入力される。座標変換部５では、入力された加速度ベ
クトルａ_b （ａ_x ，ａ_y ，ａ_z ）が方向余弦行列Ｃを用
いて、局地水平座標系の加速度ベクトルａ_L （ａ_X ，ａ
_Y ，ａ _Z ）に変換される。即ち、 ａ_L （ａ_X ，ａ_Y ，ａ_Z ）＝Ｃａ_b （ａ_x ，ａ_y ，ａ_z ） …… （12） 速度計算部８では、機体の加速度ａ_X ，ａ_Y を用いて、
次式により機体の局地水平座標系の速度Ｖ_X ，Ｖ_Y が計
算される。

【００１２】 Ｖ_X ＝Ｖ_X ′＋（ａ_X ・ΔＴ−Ｋ₁ Ｖ_X ′） …… （13） Ｖ_Y ＝Ｖ_Y ′＋（ａ_Y ・ΔＴ−Ｋ₁ Ｖ_Y ′） …… （14） こゝで、Ｖ_X ′，Ｖ_Y ′は１サンプリング前のＶ_X ，Ｖ
_Y の値であり、ΔＴはサンプリング周期、Ｋ₁ はフィー
ドバック・ゲインである。角速度推定部９では、機体の
速度Ｖ_X ，Ｖ_Y を用いて、初期アライメント実行時は、
機体は静止しているので水平面上の速度はゼロであり、
速度Ｖ_X ，Ｖ_Y がゼロとなるように、機体の局地水平座
標系の角速度の推定値ω_X ＾，ω_Y ＾が次式により計算
される。

【００１３】 ω_X ＾＝ω_X ＾′＋Ｋ₃ ・Ｖ_Y ・ΔＴ ……（15） ω_Y ＾＝ω_Y ＾′＋Ｋ₃ ・Ｖ_X ・ΔＴ ……（16） こゝで、ω_X ＾′，ω_Y ＾′は１サンプリング前のω_X
＾，ω_Y ＾の値であり、Ｋ₃ はフィードバック・ゲイン
である。このω_X ＾，ω_Y ＾は、図７に示すように、北
軸方向の地球自転角速度成分ω_N ＾のＸ，Ｙ成分（水平
成分）であり、これを用いて次の第１トルキング量計算
部１０で方向余弦行列Ｃに対する地球自転角速度に起因
する補正量（トルキング量）のＸ，Ｙ成分が計算され
る。また、トルキング量のＺ成分は第２トルキング量計
算部１１で計算される。

【００１４】第１トルキング量計算部１０では、機体の
速度Ｖ_X ，Ｖ_Y 及び角速度推定値ω _X ＾，ω_Y ＾を用い
て、方向余弦行列Ｃに対する地球自転角速度に起因する
補正量（トルキング量）のＸ，Ｙ成分であるＴＱ_X ，Ｔ
Ｑ_Y が次式により計算され、方向余弦行列計算部４へ入
力される。 ＴＱ_X ＝（ω_X ＾＋Ｋ₂ Ｖ_Y ）・ΔＴ ……（17） ＴＱ_Y ＝（ω_Y ＾＋Ｋ₂ Ｖ_X ）・ΔＴ ……（18） こゝでＫ₂ はフィードバック・ゲインである。

【００１５】第２トルキング量計算部１１では、外部信
号として図５の現在地点Ｐの緯度λ（北半球では正、南
半球では負とする）を入力することによって、次式によ
り前記トルキング量のＺ軸方向の成分ＴＱ_Z が計算され
る。 ＴＱ_Z ＝ω_Z ・ΔＴ ……（19) （１９）式のＺ軸方向の角速度ω_Z は、図５から明らか
なように、 ω_Z ＝ω_D ＝−ω_e ・sin λ ……（20) なお、図５におけるＮ軸、Ｅ軸方向の角速度ω_N ，ω_E
は、 ω_N ＝ω_e ・cos λ ……（21) ω_E ＝０ ……（22) で与えられる。

【００１６】図６より明らかなように、Ｘ軸、Ｙ軸方向
の角速度ω_X ，ω_Y はＸ軸の方位角ψ₂ を用いれば、 ω_X ＝ω_N ・cos ψ₂ ＝ω_e ・cos λ・cos ψ₂ ……（23) ω_Y ＝−ω_N sin ψ₂ ＝−ω_e ・cos λ・sin ψ₂ ……（24) と表すことができる。

【００１７】Ｘ軸方位角計算部１２では、機体のＸ軸、
Ｙ軸方向の角速度の推定値ω_X ＾，ω_Y ＾を用いて、Ｘ
軸の北方向となす角（方位角）ψ₂ が次式により計算さ
れる。 ψ₂ ＝tan ^-1( −ω_Y ＾／ω_X ＾） ……（25） 方位角計算部１３では、Ｘ軸の方位角ψ₂ と、姿勢角計
算部６で計算した、ｘ軸がＸ軸となす角をＸＹ平面上に
投影した角ψ₁ とを用いて、次式により機体の方位角
ψ、つまりｘ軸が北方向となす角（図７）が計算され、
外部に出力される。

【００１８】 ψ＝ψ₁ ＋ψ₂ ……（26） 初期アライメントにおいて、慣性信号演算部３で機体が
静止している状態でのロール角φ（９式）及びピッチ角
θ（１０式）と、方位角ψ（２６式）とが演算されて、
外部に出力される。初期アライメント実行時におけるこ
れらのデータは、やがて機体が移動を開始した場合のデ
ータの初期値とされるものである。

【００１９】

【発明が解決しようとする課題】従来の慣性装置では、
初期アライメントにおいて、方向余弦行列Ｃを計算する
のに、初期緯度λを外部より入力する必要があった。こ
のため外部の緯度計測装置とのインターフェースが必要
となり、慣性装置がそれだけ大型で高価になる問題があ
った。この発明の目的は、初期アライメントにおいて、
初期緯度の入力を不要として、装置の小型化、経済化を
図ろうとするものである。

【００２０】

【課題を解決するための手段】この発明では、以上述べ
た従来の慣性装置をベースに、前記角速度成分の推定値
ω_X ＾，ω_Y ＾を入力して、現在地点の緯度の推定値λ
＾＝±｜λ＾｜（＋符号は北半球、−符号は南半球に対
応する）を求め、それらの値を前記第２トルキング量計
算部に供給する緯度推定部と、その極性の異なる緯度の
推定値±｜λ＾｜を用いて、それぞれの場合の初期アラ
イメントが並行して開始されてから所定時間経過した時
点での前記機体の方位角ψ₊ （＋｜λ＾｜に対応）とψ
_- （−｜λ＾｜に対応）の時間に対する変化を比較し、
その変化が小さい方の方位角を真の方位角として、それ
に対応する緯度の推定値の極性を示す信号Ｓ₊ （＋｜λ
＾｜に対応）またはＳ- （−｜λ＾｜に対応）を前記緯
度推定部に供給して、それ以後、対応する一方の極性の
緯度の推定値のみを出力させる南緯／北緯判定部とが追
加される。

【００２１】

【実施例】この発明の実施例を図１に、図３と対応する
部分に同じ符号を付して示し、重複説明を省略する。こ
の発明では、初期緯度λを外部より入力する代わりに、
緯度推定部１４を設けて初期緯度λを推定し、そのデー
タを用いるようにしている。

【００２２】図６に示したように、地球自転角速度ω_e
の局地水平成分ω_N ＝ω_e ・cos λ（２１式）のＸ軸及
びＹ軸成分がω_X ，ω_Y であるので、緯度推定部１４で
は、局地水平成分ω_N の推定値ω_N ＾を機体静止時の角
速度、つまり地球自転角速度の推定値ω_X ＾，ω_Y ＾を
用いて次式により演算される（図７参照）。 ω_N ＾＝（ω_X ＾² ＋ω_Y ＾²)^1/2 ……（27） これより地球自転角速度ω_e のＤ軸方向成分ω_D （図５
参照）の推定値ω_D ＾ ω_D ＾＝±（ω_e ² −ω_N ＾²)^1/2 ……（28） が演算される。ω_D ＾の±符号は図５から分かるように
北半球では−，南半球では＋となる。

【００２３】（２０），（２１）式より −ω_D ／ω_N ＝tan λ ……（29） であるから、いま求めたω_N ＾，ω_D ＾を用いて、緯度
λの推定値λ＾ λ＾＝tan ^-1（−ω_D ＾／ω_N ＾） ……（30） が緯度推定部１４で演算され、第２トルキング量計算部
１１へ入力され、従来例で述べたようにトルキング量の
Ｚ成分ＴＱ_Z が演算される。

【００２４】（２８）式の地球自転角速度のＤ軸成分の
推定値ω_D ＾の±符号は現在地Ｐが南緯（緯度λが負）
か、北緯（緯度λが正）かによりそれぞれ正または負に
なるものであるので、初期アライメントにおいて、緯度
推定部１４は（２８）式の±符号を決めることができな
い。そこで、現在地Ｐが南半球にあり角速度ω_D ＾が
正の場合、つまりλ＾の極性が負のデータと、現在地
が北半球にあり、ω_D＾が負の場合、つまりλ＾の極性
が正のデータとが出力され、双方の場合の初期アライメ
ントが並行して行われる。

【００２５】図２に示すのは、このようにして初期アラ
イメントをｔ＝Ｔ₀ から行った場合に得られる緯度λ＾
及び方位角ψの時間に対する変化特性を示したものであ
り、図２の例ではＴ₁ 時間において、の方位角ψ（λ
＾＝＋｜λ＾｜）は一定値に収斂するのに対して、の
方位角ψ（λ＾＝−｜λ＾｜）は発散する傾向にある。
初期方位角ψは一定値でなければならないことから、南
緯／北緯判定部１５では、Ｔ₁ 時間の方位角ψの時間ｔ
に対する傾斜より、のλ＾＝＋｜λ＾｜が正しいもの
と判断して、緯度λ＾の極性が正であることを示すデー
タＳ₊ （負のときはＳ_- ）を緯度推定部１４へ入力し、
それ以後、緯度推定部１４ではλ＾＝＋｜λ＾｜のみを
地球自転角速度補正量計算部１１へ入力し、北半球での
初期アライメントのみが引き続き行われる。

【００２６】なお、λ＾＝＋｜λ＾｜と、λ＾＝−
｜λ＾｜とのそれぞれのアライメントにおける推定緯度
λ＾の時間に対する変化特性は図２Ａに示すように極性
以外は相等しいデータが得られる。両特性が極性を除い
て一致することは理論上証明できるが、その詳細は省略
する。

【００２７】

【発明の効果】以上述べたように、この発明では従来の
慣性装置に、緯度推定部１４と南緯／北緯判定部１５と
を追加して、初期アライメントに必要な現在地の緯度を
推定することができる。よって、従来用いていた外部よ
り緯度信号を入力するためのインターフェースが不要と
なり、それだけ装置の小型化、経済化が図られる。

【図面の簡単な説明】

【図１】この発明の実施例を示すブロック図。

【図２】図１の初期アライメントにおける推定緯度λ＾
と方位角ψの時間に対する変化特性を示す図。

【図３】従来の慣性装置のブロック図。

【図４】機体軸座標系（ｘ，ｙ，ｚ）の各軸と、局地水
平座標系（Ｘ，Ｙ，Ｚ）の各軸とのなす角α_i ，β_i ，
γ_i （ｉ＝１，２，３）を定義するための図。

【図５】緯度λの現在地点Ｐにおける地球自転角速度ω
_e と、その北軸及び垂直軸方向成分ω_N ，ω_D とを示す
図。

【図６】地球自転角速度の北方向成分ω_N とその局地水
平座標系のＸ軸、Ｙ軸方向成分ω_X ，ω_Y とを示す図。

【図７】機体の方位角ψと、Ｘ軸が北方向となす角ψ₂
と、ｘ軸がＸ軸となす角をＸＹ平面上に投影した角ψ₁
との間の関係を示す図。

Claims (1)

(57)【特許請求の範囲】
1. 【請求項１】 機体が静止時に、地球自転角速度ω_e を
   検出して、機体軸座標系（ｘ，ｙ，ｚ）の角速度ベクト
   ルω_b （ω_x ，ω_y ，ω_z ）を出力するジャイロと、 機体が静止時に、重力加速度を検出して、前記機体軸座
   標系の加速度ベクトルａ_b （ａ_x ，ａ_y ，ａ_z ）を出力
   する加速度計と、 機体の静止状態において、前記角速度ベクトルω_b と地
   球自転角速度に起因する補正量（トルキング量）とを入
   力して、前記機体軸座標系（ｘ，ｙ，ｚ）を局地水平座
   標系（Ｘ，Ｙ，Ｚ）に変換する座標変換行列（方向余弦
   行列と言う）Ｃを演算する（初期アライメントと言う）
   方向余弦行列計算部と、 その方向余弦行列Ｃを入力して、機体の静止状態でのロ
   ール角φ，ピッチ角θ及びｘ軸がＸ軸となす角をＸＹ平
   面上に投影した角ψ₁ を演算する姿勢角計算部と、 前記加速度ベクトルａ_b を入力して、前記方向余弦行列
   Ｃを用いて、加速度成分ａ_X ，ａ_Y に変換する座標変換
   部と、 その加速度成分ａ_X ，ａ_Y を入力し、積分して機体の速
   度成分Ｖ_X ，Ｖ_Y を演算する速度計算部と、 その機体の速度成分Ｖ_X ，Ｖ_Y を入力して、Ｖ_X ＝０，
   Ｖ_Y ＝０となるように角速度成分の推定値ω_X＾，ω_Y＾
   を演算する角速度推定部と、 その角速度成分の推定値ω_X ＾，ω_Y ＾と前記機体の速
   度成分Ｖ_X ，Ｖ_Y とを入力して、前記方向余弦行列Ｃに
   対する補正量（トルキング量）のＸ軸，Ｙ軸成分Ｔ
   Ｑ_X ，ＴＱ_Y を演算する第１トルキング量計算部と、 現在地点の緯度情報を入力して、前記トルキング量のＺ
   軸成分ＴＱ_Z を演算する第２トルキング量計算部と、 前記角速度成分の推定値ω_X ＾，ω_Y ＾を入力して、局
   地水平座標系のＸ軸が北方向となす角（方位角）ψ₂ を
   演算するＸ軸方位角計算部と、 そのＸ軸方位角ψ₂ と前記ｘ軸がＸ軸となす角のＸＹ平
   面への投影角ψ₁ とを入力して、機体の方位角（ｘ軸が
   北方向となす角）ψを演算する方位角計算部とを具備す
   る慣性装置において、 前記角速度成分の推定値ω_X ＾，ω_Y ＾を入力して、現
   在地点の緯度の推定値λ＾＝±｜λ＾｜（＋符号は北半
   球、−符号は南半球に対応する）を求め、それらの値を
   前記第２トルキング量計算部に供給する緯度推定部と、 その極性の異なる２つの緯度の推定値±｜λ＾｜を用い
   てそれぞれの場合の初期アライメントが並行して開始さ
   れてから所定時間経過した時点での前記機体の方位角ψ
   ₊ （＋｜λ＾｜に対応）とψ_- （−｜λ＾｜に対応）の
   時間に対する変化を比較し、その変化が小さい方の方位
   角を真の方位角として、それに対応する緯度の推定値の
   極性を示す信号Ｓ₊ （＋｜λ＾｜に対応）またはＳ-
   （−｜λ＾｜に対応）を前記緯度推定部に供給して、そ
   れ以後、対応する一方の極性の緯度の推定値のみを出力
   させる南緯／北緯判定部とを付加したことを特徴とす
   る、 慣性装置。