JP2011220791A - 慣性航法装置 - Google Patents

Abstract

【課題】方位角の初期化誤差を小さくする。
【解決手段】ジャイロ１１と、外部ＩＮＳ２０の検出角速度とジャイロ１１の検出角速度を用い、外部ＩＮＳ２０に対する取付けミスアライメントを推定する角速度マッチング部４１と、外部ＩＮＳ２０が出力するロール角、ピッチ角及び方位角とジャイロ１１の検出角速度を用い、外部ＩＮＳ２０に対する取付けミスアライメントを推定する角度マッチング部４２と、慣性航法装置の姿勢角を求める姿勢角演算部と、姿勢角の変動から求められる動揺レベルにより角速度マッチング４１で推定される取付けミスアライメント、角度マッチング部４２で推定される取付けミスアライメント及び取付けミスアライメントの推定値がゼロのうち、いずれかを選択して出力する選択部４３と、外部ＩＮＳ２０から入力される方位角を選択部４３の出力によって補正する補正演算部１４とを備える。
【選択図】図１

Description

この発明は自身で方位角を算出することができない慣性航法装置に関する。

この種の慣性航法装置では初期方位角の算出ができないため、基準となる外部慣性航法装置からの基準角（基準方位角）を使用し、慣性航法装置の初期化を行う必要がある。しかしながら、この初期化の際、一般に慣性航法装置と基準慣性航法装置との間には取付け誤差（取付けミスアライメント）が生じており、この取付けミスアライメントは慣性航法装置の初期化誤差となる。従って、慣性航法装置の初期化に際し、取付けミスアライメントを推定して補正することが従来より行われている。

取付けミスアライメントの推定にはいくつかの方法があるが、代表的な方式として以下の２種類がある。１つは慣性航法装置が検出する角速度と外部慣性航法装置が検出する角速度とを用い、両者間に発生する角速度差から取付けミスアライメントを推定する、いわゆる角速度マッチングを用いた推定方式である。

もう１つは慣性航法装置及び外部慣性航法装置が検出する角速度を積分した角度を用い、両者間に発生する角度差から取付けミスアライメントを推定する、いわゆる角度マッチングを用いた推定方式である。

図６は角速度マッチングを用いた取付けミスアライメントの推定方式を備える従来の慣性航法装置の構成を示したものであり、図７は角度マッチングを用いた取付けミスアライメントの推定方式を備える従来の慣性航法装置の構成を示したものである。これらはいずれも特許文献１に記載されているものであり、以下、両推定方式について簡単に説明する。

図６に示した慣性航法装置（ＩＮＳ）１０はジャイロ１１と角速度差演算部１２とカルマンフィルタ１３と補正演算部１４を具備している。基準となる外部慣性航法装置（外部ＩＮＳ）２０は入力角速度ωより自身に固有の入力軸Ｘ，Ｙ，Ｚ周りの角速度ωＸ，ωＹ，ωＺを検出し、検出した角速度ωＸ，ωＹ，ωＺを基に自身のロール角φ_Ｒ，ピッチ角θ_Ｒ及び方位角ψ_Ｒを演算する。

ＩＮＳ１０のジャイロ１１は入力角速度ωより自身に固有の入力軸ｘ，ｙ，ｚ周りの角速度ωｘ，ωｙ，ωｚを検出し、角速度差演算部１２及びカルマンフィルタ１３に出力する。角速度差演算部１２には外部ＩＮＳ２０から角速度ωＸ，ωＹ，ωＺが入力され、角速度差演算部１２はジャイロ１１が検出した角速度ωｘ，ωｙ，ωｚと、外部ＩＮＳ２０が検出した角速度ωＸ，ωＹ，ωＺとの角速度差
δωｘ＝ωｘ−ωＸ
δωｙ＝ωｙ−ωＹ
δωｚ＝ωｚ−ωＺ
を演算し、カルマンフィルタ１３に出力する。

カルマンフィルタ１３はこれら角速度差δωｘ，δωｙ，δωｚとジャイロ１１より入力される角速度ωｘ，ωｙ，ωｚより、ＩＮＳ１０のＸ，Ｙ，Ｚ軸に対する取付けミスアライメントφＸ，φＹ，φＺを演算し、補正演算部１４に出力する。補正演算部１４はこれら取付けミスアライメントφＸ，φＹ，φＺにより、外部ＩＮＳ２０から入力される方位角ψ_Ｒを補正し、これにより補正された方位角ψが得られる。なお、取付けミスアライメントφＸ，φＹ，φＺを推定するカルマンフィルタ１３の演算詳細は特許文献１に記載されており、ここでは詳細説明を省略する。

次に、図７に示したＩＮＳ３０について説明する。ＩＮＳ３０はジャイロ１１と基準方向余弦行列演算部３１と方向余弦行列演算部３２と角度誤差演算部３３と回転角度演算部３４とカルマンフィルタ３５と補正演算部１４を具備している。

基準方向余弦行列演算部３１は外部ＩＮＳ２０より入力されるロール角φ_Ｒ，ピッチ角θ_Ｒ及び方位角ψ_Ｒを用いて基準方向余弦行列［Ｃ_Ｒ］を演算する。なお、演算詳細については特許文献１に記載されている通りであり、ここでは説明を省略する（以下、同様）。

方向余弦行列演算部３２にはジャイロ１１より角速度ωｘ，ωｙ，ωｚが入力され、所定の周期Ｔで基準方向余弦行列［Ｃ_Ｒ］で初期化した後、角速度ωｘ，ωｙ，ωｚを用いて方向余弦行列［Ｃ］を演算する。

角度誤差演算部３３は方向余弦行列演算部３２の前記初期化する直前の時点の方向余弦行列［Ｃ］と、同じ時点の基準方向余弦行列［Ｃ_Ｒ］とを周期Ｔでサンプリングする。ここで、外部ＩＮＳ２０の出力するφ_Ｒ，θ_Ｒ，ψ_Ｒに基づく入力軸Ｘ，Ｙ，Ｚ周りの回転角度をθＸ，θＹ，θＺとし、ジャイロ１１の出力する角速度ωｘ，ωｙ，ωｚに基づく入力軸ｘ，ｙ，ｚ周りの回転角度をθｘ，θｙ，θｚとし、前者に対する後者の偏差をδθｘ，δθｙ，δθｚとする。

δθｘ＝θｘ−θＸ
δθｙ＝θｙ−θＹ
δθｚ＝θｚ−θＺ
角度誤差演算部３３はこれらの偏差δθｘ，δθｙ，δθｚを演算してカルマンフィルタ３５に出力する。

回転角度演算部３４は方向余弦行列演算部３２の前記初期化と同じタイミングで自身の出力データをゼロにリセットした後、ジャイロ１１より入力される角速度ωｘ，ωｙ，ωｚを積分して回転角度θｘ，θｙ，θｚを演算し、カルマンフィルタ３５に出力する。

カルマンフィルタ３５は入力された偏差δθｘ，δθｙ，δθｚと回転角度θｘ，θｙ，θｚより、図６の場合と同様に取付けミスアライメントφＸ，φＹ，φＺの推定値を演算して補正演算部１４に出力する。補正演算部１４はこれら取付けミスアライメントφＸ，φＹ，φＺにより外部ＩＮＳ２０から入力される方位角ψ_Ｒを補正し、これにより補正された方位角ψが得られる。

特開平４−３７０７１２号公報

上述したように、方位角の取付けミスアライメントを推定すべく、従来においては角速度マッチングを用いた推定方式を備える慣性航法装置と、角度マッチングを用いた推定方式を備える慣性航法装置とがある。

しかしながら、角速度マッチングを用いた取付けミスアライメント推定方式は、慣性航法装置に印加される動揺レベル（角速度）が大きい場合には精度良く取付けミスアライメントを推定することができるものの、動揺レベルが小さい場合には精度が良くないといった問題がある。

一方、角度マッチングを用いた取付けミスアライメント推定方式は、慣性航法装置に印加される動揺レベルが小さくても精度良く取付けミスアライメントを推定することができるが、印加される動揺レベルが大きい場合、角速度マッチングを用いた推定方式より誤差が大きくなるといった問題がある。

この発明の目的はこのような問題に鑑み、慣性航法装置に印加される動揺レベルの大小に拘らず、常に最適な取付けミスアライメントの推定を行えるようにし、よって方位角の初期化誤差を極めて小さくすることができるようにした慣性航法装置を提供することにある。

請求項１の発明によれば、基準となる外部慣性航法装置の出力を利用した方位角の初期化を必要とする慣性航法装置は、ジャイロと、外部慣性航法装置が検出する角速度とジャイロが検出する角速度とを用い、外部慣性航法装置に対する取付けミスアライメントを推定する角速度マッチング部と、外部慣性航法装置が出力するロール角、ピッチ角及び方位角とジャイロが検出する角速度とを用い、外部慣性航法装置に対する取付けミスアライメントを推定する角度マッチング部と、慣性航法装置の姿勢角を求める姿勢角演算部と、姿勢角の変動から求められる動揺レベルにより、角速度マッチング部で推定される取付けミスアライメント、角度マッチング部で推定される取付けミスアライメント及び取付けミスアライメントの推定値がゼロのうち、いずれかを選択して出力する選択部と、外部慣性航法装置から入力される方位角を選択部の出力によって補正する補正演算部とを具備する。

請求項２の発明では請求項１の発明において、角速度マッチング部は、外部慣性航法装置が検出する自身に固有の入力軸Ｘ，Ｙ，Ｚ周りの角速度ωＸ，ωＹ，ωＺと、ジャイロが検出する自身に固有の入力軸ｘ，ｙ，ｚ周りの角速度ωｘ，ωｙ，ωｚとの角速度差δωｘ，δωｙ，δωｚを演算する角速度差演算部と、角速度ωｘ，ωｙ，ωｚと角速度差δωｘ，δωｙ，δωｚとを用いて取付けミスアライメントφＸ，φＹ，φＺを推定するカルマンフィルタとよりなる。

請求項３の発明では請求項１の発明において、角度マッチング部は、外部慣性航法装置が出力するロール角φ_Ｒ，ピッチ角θ_Ｒ及び方位角ψ_Ｒを用いて基準方向余弦行列［Ｃ_Ｒ］を演算する基準方向余弦行列演算部と、所定の周期Ｔで基準方向余弦行列［Ｃ_Ｒ］で初期化した後、ジャイロが検出する自身に固有の入力軸ｘ，ｙ，ｚ周りの角速度ωｘ，ωｙ，ωｚを用いて方向余弦行列［Ｃ］を演算する方向余弦行列演算部と、方向余弦行列演算部の前記初期化する直前の時点の方向余弦行列［Ｃ］と、同じ時点の基準方向余弦行列［Ｃ_Ｒ］とを周期Ｔでサンプリングして、外部慣性航法装置の検出値に基づく外部慣性航法装置自身に固有の入力軸Ｘ，Ｙ，Ｚ周りの回転角度θＸ，θＹ，θＺに対するジャイロの検出値に基づく入力軸ｘ，ｙ，ｚ周りの回転角度θｘ，θｙ，θｚの偏差δθｘ，δθｙ，δθｚを演算する角度誤差演算部と、方向余弦行列演算部の前記初期化と同じタイミングで自身の出力データをゼロにリセットした後、角速度ωｘ，ωｙ，ωｚを積分して回転角度θｘ，θｙ，θｚを演算する回転角度演算部と、回転角度θｘ，θｙ，θｚと偏差δθｘ，δθｙ，δθｚとを用いて取付けミスアライメントφＸ，φＹ，φＺを推定するカルマンフィルタとよりなる。

請求項４の発明では請求項１の発明において、姿勢角演算部が求める姿勢角はロール角φ，ピッチ角θであり、選択部は、ロール角φ，ピッチ角θの変動より求めたロール軸周りの角速度ωφ及びピッチ軸周りの角速度ωθを動揺レベルとし、角速度ωφの判定基準をＤ_１，Ｄ_２，Ｄ_３（但し、Ｄ_１＜Ｄ_２＜Ｄ_３）とし、角速度ωθの判定基準をＥ_１，Ｅ_２，Ｅ_３（但し、Ｅ_１＜Ｅ_２＜Ｅ_３）とした時、Ｄ_２≦ωφ≦Ｄ_３かつＥ_２≦ωθ≦Ｅ_３の時、角速度マッチング部で推定される取付けミスアライメントを選択し、ωφ＜Ｄ_１，ωθ＜Ｅ_１，ωφ＞Ｄ_３及びωθ＞Ｅ_３の少なくとも一つを満足する時、ゼロを選択し、ωφ，ωθが上記以外の範囲の時、角度マッチング部で推定される取付けミスアライメントを選択する。

請求項５の発明では請求項４の発明において、選択部はロール角φの変動及びピッチ角θの変動をそれぞれローパスフィルタに通し、そのフィルタ後の信号に対し、Zero−down−cross法により周期、振幅を求めて角速度ωφ，ωθを算出する。

この発明によれば、慣性航法装置に印加される動揺レベルの大きさにより、角速度マッチングを用いて推定した取付けミスアライメント、角度マッチングを用いて推定した取付けミスアライメント及びゼロ（取付けミスアライメント推定なし）のうち、最適な取付けミスアライメントを自動的に選択し、その選択した取付けミスアライメントを用いて、外部慣性航法装置から入力される方位角の補正を行うものとなっており、よって動揺レベルの大小に拘らず、方位角の初期化誤差を極めて小さくすることが可能となる。

この発明による慣性航法装置の一実施例の構成を示すブロック図。 図１の選択部におけるロール角／ピッチ角の変動から角速度を求める方法を説明するための図。 図２における周期／振幅解析部の解析を説明するための図。 動揺レベルと取付けミスアライメントの推定方式との関係を示すテーブル。 シミュレーション結果を示す表。 従来の、角速度マッチングを用いた取付けミスアライメント推定方式を備える慣性航法装置の構成を示すブロック図。 従来の、角度マッチングを用いた取付けミスアライメント推定方式を備える慣性航法装置の構成を示すブロック図。

この発明の実施形態を図面を参照して実施例により説明する。

図１はこの発明による慣性航法装置の一実施例の機能構成を示したものであり、図６及び図７と対応する部分には同一符号を付し、その詳細な説明を省略する。

この例では慣性航法装置（ＩＮＳ）４０はジャイロ１１と角速度マッチング部４１と角度マッチング部４２と選択部４３と補正演算部１４を具備している。

角速度マッチング部４１は外部慣性航法装置（外部ＩＮＳ）２０が検出する角速度ωＸ，ωＹ，ωＺと、ジャイロ１１が検出する角速度ωｘ，ωｙ，ωｚとを用い、外部ＩＮＳ２０に対する取付けミスアライメントφＸ，φＹ，φＺを推定する。角速度マッチング部４１は図６に示した従来の構成と同様、角速度差演算部１２とカルマンフィルタ１３とを備えている。

角度マッチング部４２は外部ＩＮＳ２０が出力するロール角φ_Ｒ，ピッチ角θ_Ｒ及び方位角ψ_Ｒと、ジャイロ１１が検出する角速度ωｘ，ωｙ，ωｚとを用い、外部ＩＮＳ２０に対する取付けミスアライメントφＸ，φＹ，φＺを推定する。角度マッチング部４２は図７に示した従来の構成と同様、基準方向余弦行列演算部３１と方向余弦行列演算部３２と角度誤差演算部３３と回転角度演算部３４とカルマンフィルタ３５とを備えている。

角速度マッチング部４１で角速度マッチングを用いて推定された取付けミスアライメントφＸ，φＹ，φＺ及び角度マッチング部４２で角度マッチングを用いて推定された取付けミスアライメントφＸ，φＹ，φＺは共に選択部４３に入力される。

選択部４３は印加される動揺レベルにより、角速度マッチング部４１で推定された取付けミスアライメントφＸ，φＹ，φＺ、角度マッチング部４２で推定された取付けミスアライメントφＸ，φＹ，φＺ及びゼロ（取付けミスアライメント推定なし：φＸ＝０，φＹ＝０，φＺ＝０）のうち、いずれかを選択して補正演算部１４に出力する。

補正演算部１４は外部ＩＮＳ２０から入力される方位角ψ_Ｒを選択部４３の出力によって補正し、これにより補正された方位角ψが得られる。

いずれの取付けミスアライメントφＸ，φＹ，φＺを補正に使用するかの選択基準となる動揺レベルはＩＮＳ４０が演算により求めたロール角φの変動及びピッチ角θの変動より算出される。ＩＮＳ４０は方向余弦行列演算部３２で計算した方向余弦行列[Ｃ]を使用して姿勢角であるロール角φとピッチ角θを姿勢角演算部４８で計算する。図２及び図３は選択部４３における動揺レベルの算出方法を示したものである。

ロール角φ及びピッチ角θはそれぞれローパスフィルタ４４に通され、フィルタ後の信号φ_ｆ，θ_ｆが中心点解析部４５及び周期／振幅解析部４６にそれぞれ入力される。中心点解析部４５は最小二乗法により各信号φ_ｆ，θ_ｆの中心点を見つけ、周期／振幅解析部４６はZero−down−cross法により各信号φ_ｆ，θ_ｆの周期／振幅を解析して求める。

図３は信号φ_ｆを例に、その様子を示したものであり、フィルタ後の信号φ_ｆがその平均値に対し、負→正にクロスする点から次のクロス点までを１つの波と仮定し、振幅Ａ_φ，周期Ｔ_φを求める。

このようにして、信号φ_ｆから求めた振幅Ａ_φ，周期Ｔ_φ及び信号θ_ｆから求めた振幅Ａ_θ，周期Ｔ_θは角速度計算部４７に入力され、角速度が算出される。

ロール軸周りの角速度ωφ及びピッチ軸周りの角速度ωθは下記となる。

ωφ＝２πＡ_φ／Ｔ_φ
ωθ＝２πＡ_θ／Ｔ_θ
選択部４３はこれら角速度ωφ，ωθを動揺レベルとし、この動揺レベルの大きさにより、角速度マッチング／角度マッチングによる取付けミスアライメントφＸ，φＹ，φＺ及びゼロ（取付けミスアライメント推定なし）のうち、いずれかを選択する。

図４は動揺レベルによりいずれの取付けミスアライメントを用いるかを示したものであり、基本的に動揺レベルが小さい場合は角度マッチングを選択し、大きい場合は角速度マッチングを選択する。なお、動揺レベルが極めて小さい場合は角速度マッチング／角度マッチング共に取付けミスアライメントの推定が良くないため、外部ＩＮＳ２０から入力される方位角ψ_Ｒをそのまま方位角ψとして使用する。

以下、図４における動揺レベル（ωφ，ωθ）の判定基準を決定するためにシミュレーションを行った結果について説明する。

シミュレーションの条件はＩＮＳ４０が船上で使用されることを想定し、下記とした。

・動揺レベル ロール軸周りの角速度ωφ：０〜４０°／ｓ
ピッチ軸周りの角速度ωθ：０〜４０°／ｓ
・取付けミスアライメント推定時間：４０ｓ
図５の表はシミュレーション結果を示したものであり、表より以下のことが言える。即ち、
・動揺がほとんどない（0.1°／ｓ未満）時はミスアライメントの推定が進まず、方位角誤差が大きい。

・動揺が小さい（0.1°／ｓ以上１°／ｓ未満）時は角速度マッチングより角度マッチングの方が方位角誤差が小さい。

・動揺が大きい（１°／ｓ以上３２°／ｓ以下）時は角度マッチングより角速度マッチングの方が方位角誤差が小さい。

・動揺が大きい（３２°／ｓを超える）時はミスアライメントの推定が進まず、方位角誤差が大きい。

以上より、図４における動揺レベルの判定基準を以下のように設定した。

・角速度ωφの判定基準Ｄ_１，Ｄ_２，Ｄ_３
Ｄ_１＝0.1°／ｓ , Ｄ_２＝1.0°／ｓ , Ｄ_３＝３２°／ｓ
・角速度ωθの判定基準Ｅ_１，Ｅ_２，Ｅ_３
Ｅ_１＝0.1°／ｓ , Ｅ_２＝1.0°／ｓ , Ｅ_３＝３２°／ｓ
なお、判定基準Ｄ_３，Ｅ_３は参考として設定したものであり、このような動揺レベルの極めて大きい状態では角度マッチング及び角速度マッチングの推定が良くないため、取付けミスアライメントの推定値はゼロとする。

動揺レベルの判定基準は上記のようにシミュレーションにより設定することができる。

選択部４３はこの例では設定された判定基準Ｄ_１，Ｄ_２，Ｄ_３（Ｄ_１＜Ｄ_２＜Ｄ_３）及びＥ_１，Ｅ_２，Ｅ_３（Ｅ_１＜Ｅ_２＜Ｅ_３）を用い、Ｄ_２≦ωφ≦Ｄ_３かつＥ_２≦ωθ≦Ｅ_３の時、角速度マッチング部４１で推定される取付けミスアライメントφＸ，φＹ，φＺを選択し、ωφ＜Ｄ_１，ωθ＜Ｅ_１，ωφ＞Ｄ_３及びωθ＞Ｅ_３の少なくとも一つを満足する時、φＸ＝φＹ＝φＺ＝０を選択する。そして、ωφ，ωθが上記以外の範囲の時、角度マッチング部４２で推定される取付けミスアライメントφＸ，φＹ，φＺを選択する。

例えば、ωφ＝1.0°／ｓ，ωθ＝0.5°／ｓの時は角度マッチングで推定された取付けミスアライメントφＸ，φＹ，φＺを使用し、方位角ψ_Ｒを補正する。また、ωφ＝3.0°／ｓ，ωθ＝10.0°／ｓの時には角速度マッチングで推定された取付けミスアライメントφＸ，φＹ，φＺを使用し、方位角ψ_Ｒを補正する。

以上説明したように、この例では印加される動揺レベルの大きさを判定基準をもとに判定し、角度マッチング／角速度マッチングといった推定方式で推定された取付けミスアライメントφＸ，φＹ，φＺあるいは取付けミスアライメント推定なし（ゼロ）の３つのうち、最適なものを選択して外部ＩＮＳから入力された方位角ψ_Ｒを補正するものとなっており、よって方位角ψの初期化誤差の最小化を図ることができ、真値に近い方位角ψを得ることができる。

Claims (5)

1. 基準となる外部慣性航法装置の出力を利用した方位角の初期化を必要とする慣性航法装置であって、
   ジャイロと、
   前記外部慣性航法装置が検出する角速度と、前記ジャイロが検出する角速度とを用い、前記外部慣性航法装置に対する取付けミスアライメントを推定する角速度マッチング部と、
   前記外部慣性航法装置が出力するロール角、ピッチ角及び方位角と前記ジャイロが検出する角速度とを用い、前記外部慣性航法装置に対する取付けミスアライメントを推定する角度マッチング部と、
   前記慣性航法装置の姿勢角を求める姿勢角演算部と、
   前記姿勢角の変動から求められる動揺レベルにより、前記角速度マッチング部で推定される取付けミスアライメント、前記角度マッチング部で推定される取付けミスアライメント及び取付けミスアライメントの推定値がゼロのうち、いずれかを選択して出力する選択部と、
   前記外部慣性航法装置から入力される方位角を前記選択部の出力によって補正する補正演算部と、
   を具備することを特徴とする慣性航法装置。
2. 請求項１記載の慣性航法装置において、
   前記角速度マッチング部は、
   前記外部慣性航法装置が検出する自身に固有の入力軸Ｘ，Ｙ，Ｚ周りの角速度ωＸ，ωＹ，ωＺと、前記ジャイロが検出する自身に固有の入力軸ｘ，ｙ，ｚ周りの角速度ωｘ，ωｙ，ωｚとの角速度差δωｘ，δωｙ，δωｚを演算する角速度差演算部と、
   前記角速度ωｘ，ωｙ，ωｚと前記角速度差δωｘ，δωｙ，δωｚとを用いて前記取付けミスアライメントφＸ，φＹ，φＺを推定するカルマンフィルタと、
   よりなることを特徴とする慣性航法装置。
3. 請求項１記載の慣性航法装置において、
   前記角度マッチング部は、
   前記外部慣性航法装置が出力するロール角φ_Ｒ，ピッチ角θ_Ｒ及び方位角ψ_Ｒを用いて基準方向余弦行列［Ｃ_Ｒ］を演算する基準方向余弦行列演算部と、
   所定の周期Ｔで前記基準方向余弦行列［Ｃ_Ｒ］で初期化した後、前記ジャイロが検出する自身に固有の入力軸ｘ，ｙ，ｚ周りの角速度ωｘ，ωｙ，ωｚを用いて方向余弦行列［Ｃ］を演算する方向余弦行列演算部と、
   前記方向余弦行列演算部の前記初期化する直前の時点の前記方向余弦行列［Ｃ］と、同じ時点の前記基準方向余弦行列［Ｃ_Ｒ］とを前記周期Ｔでサンプリングして、前記外部慣性航法装置の検出値に基づく外部慣性航法装置自身に固有の入力軸Ｘ，Ｙ，Ｚ周りの回転角度θＸ，θＹ，θＺに対する前記ジャイロの検出値に基づく前記入力軸ｘ，ｙ，ｚ周りの回転角度θｘ，θｙ，θｚの偏差δθｘ，δθｙ，δθｚを演算する角度誤差演算部と、
   前記方向余弦行列演算部の前記初期化と同じタイミングで自身の出力データをゼロにリセットした後、前記角速度ωｘ，ωｙ，ωｚを積分して回転角度θｘ，θｙ，θｚを演算する回転角度演算部と、
   前記回転角度θｘ，θｙ，θｚと前記偏差δθｘ，δθｙ，δθｚとを用いて前記取付けミスアライメントφＸ，φＹ，φＺを推定するカルマンフィルタと、
   よりなることを特徴とする慣性航法装置。
4. 請求項１記載の慣性航法装置において、
   前記姿勢角演算部が求める姿勢角はロール角φ，ピッチ角θであり、
   前記選択部は、前記ロール角φ，前記ピッチ角θの変動より求めたロール軸周りの角速度ωφ及びピッチ軸周りの角速度ωθを前記動揺レベルとし、
   角速度ωφの判定基準をＤ_１，Ｄ_２，Ｄ_３（但し、Ｄ_１＜Ｄ_２＜Ｄ_３）とし、角速度ωθの判定基準をＥ_１，Ｅ_２，Ｅ_３（但し、Ｅ_１＜Ｅ_２＜Ｅ_３）とした時、
   Ｄ_２≦ωφ≦Ｄ_３かつＥ_２≦ωθ≦Ｅ_３の時、前記角速度マッチング部で推定される取付けミスアライメントを選択し、
   ωφ＜Ｄ_１，ωθ＜Ｅ_１，ωφ＞Ｄ_３及びωθ＞Ｅ_３の少なくとも一つを満足する時、ゼロを選択し、
   ωφ，ωθが上記以外の範囲の時、前記角度マッチング部で推定される取付けミスアライメントを選択することを特徴とする慣性航法装置。
5. 請求項４記載の慣性航法装置において、
   前記選択部は前記ロール角φの変動及び前記ピッチ角θの変動をそれぞれローパスフィルタに通し、そのフィルタ後の信号に対し、Zero−down−cross法により周期、振幅を求めて前記角速度ωφ，ωθを算出することを特徴とする慣性航法装置。

Images