JP2896407B1 - 慣性航法装置 - Google Patents

Abstract

【要約】 【課題】 航走体の姿勢及び方位の初期化においてカル
マンフィルタを用いて取付誤差を推定するように構成さ
れた慣性航法装置（ＩＮＳ）において、船体の歪に起因
した誤差を考慮して取付誤差を推定すること目的をとす
る。 【解決手段】 航走体に搭載された慣性航法装置（航走
体ＩＮＳ）によって検出された角速度及び加速度と艦船
に搭載された慣性航法装置（マスタＩＮＳ）によって検
出された角速度及び加速度の差に基づいて、即ち、角速
度差及び加速度差をカルマンフィルタの観測値として取
付誤差を推定する。例えば、Ｘ軸及びＹ軸周りの取付誤
差の推定において、航走体ＩＮＳ及びマスタＩＮＳによ
って検出された角速度の差よりも加速度の差に基づいた
推定に重みを付ける。

Description

【発明の詳細な説明】

【０００１】

【発明の属する技術分野】本発明は艦船より発射される
航走体に搭載する慣性航法装置（ＩＮＳ）に関し、より
詳細には、トランスファアラインメント法を用いて初期
姿勢及び初期方位を算出（初期化）するように構成され
た慣性航法装置に関する。

【０００２】

【従来の技術】図１及び図２に艦船１に搭載された航走
体２の配置状態を示す。図示のように、航走体２は、典
型的には艦船１に装備された航走体発射装置３にセット
されている。艦船１には艦船１の姿勢及び方位を演算す
る主慣性航法装置１０（以下に「マスタＩＮＳ」と称す
る。）が搭載され、航走体２にも航走体２自身の姿勢及
び方位を演算する小型慣性航法装置２０（以下に「航走
体ＩＮＳ」と称する。）が搭載されている。

【０００３】尚、図３及び図４に示すように、航走体発
射装置３に近接して航走体発射装置３の姿勢及び方位を
演算する従慣性航法装置３０（以下に「スレーブＩＮ
Ｓ」と称する。）を搭載してもよい。

【０００４】航走体ＩＮＳ２０に設定した座標系（以下
に「航走体座標」と称する。）とマスタＩＮＳ１０に設
定した座標系（以下に「マスタ座標」と称する。）との
間には直線的偏差及び回転偏差が存在する。この回転偏
差を取付誤差又は取り付けミスアラインメントφと称す
る。取付誤差φは、Ｘ，Ｙ，Ｚ軸周りの取付誤差φ_X，
φ_Y ，φ_Z を含み、航走体２を艦船に機械的に装着する
際に生ずる誤差である。

【０００５】一般に、ロール、ピッチ及び方位は姿勢及
び方位と称されるが、ここでは随時、両者を纏めて単に
姿勢と称することがある。又、ロール角、ピッチ角及び
方位角は姿勢角及び方位角と称されるが、ここでは、両
者を纏めて単に姿勢角と称することがある。

【０００６】航走体２の慣性誘導は航走体２自身に搭載
された航走体ＩＮＳ２０によってなされるが、航走体２
を所定の軌道に沿って正確に誘導するためには航走体２
の発射前に、航走体２又は航走体ＩＮＳ２０の姿勢及び
方位を正確に求める必要がある。これを航走体ＩＮＳ２
０の初期化と称する。

【０００７】航走体ＩＮＳ２０の初期化は、航走体ＩＮ
Ｓ２０自身によって実行することはできない。航走体Ｉ
ＮＳ２０の初期化には数十分の時間を要するが、航走体
ＩＮＳ２０の電源は航走体発射直前に投入されるからで
ある。また、航走体ＩＮＳ２０に使用されるセンサは一
般に低価格且つ低精度だからである。

【０００８】一方、マスタＩＮＳ１０は常に運転状態に
あり、マスタＩＮＳ１０自身の姿勢及び方位は常に正確
に求められている。従って、航走体ＩＮＳ２０を初期化
するには、取付誤差又は取り付けミスアラインメントφ
が求められればよい。

【０００９】航走体ＩＮＳ２０の姿勢及び方位を高速に
推定する方式としてトランスファアラインメント法と呼
ばれる手法が使用されている。トランスファアラインメ
ント法によると、マスタＩＮＳ１０より航走体ＩＮＳ２
０へ信号が転送され、マスタＩＮＳ１０からの信号を使
用して取付誤差φが推定される。

【００１０】より詳細には、航走体ＩＮＳ２０によって
検出された加速度及び角速度とマスタＩＮＳ１０によっ
て検出された加速度及び角速度とを比較することによっ
て取付誤差φが推定される。又は、航走体ＩＮＳ２０に
よって検出された速度及び姿勢角とマスタＩＮＳ１０に
よって検出された速度及び姿勢角とを比較することによ
って取付誤差φが推定される。取付誤差φの推定演算
は、カルマンフィルタを使用し、航走体ＩＮＳ２０によ
ってなされる。尚、航走体ＩＮＳ２０の代わりにスレー
ブＩＮＳ３０によってなされてもよい。

【００１１】以下にトランスファアラインメント法によ
る取付誤差φの推定法について具体的に説明する。取付
誤差φが存在するために、マスタＩＮＳ１０の加速度計
及びジャイロに入力する加速度及び角速度と航走体ＩＮ
Ｓ２０の加速度計及びジャイロに入力する加速度及び角
速度との間に偏差が生じる。速度及び姿勢角は加速度及
び角速度に基づいて演算されるため、これらの値もマス
タＩＮＳ１０と航走体ＩＮＳ２０では異なる値となる。

【００１２】航走体ＩＮＳ２０は、マスタＩＮＳ１０の
加速度計及びジャイロによって検出された加速度及び角
速度と航走体ＩＮＳ２０の加速度計及びジャイロによっ
て検出された加速度及び角速度の差を演算し、これらの
差をカルマンフィルタにおける観測値に用いる。又は、
加速度及び角速度より速度差及び姿勢差を演算し、これ
らの差をカルマンフィルタにおける観測値に用いる。

【００１３】ここで、カルマンフィルタの概要を簡単に
説明する。システムは１次の微分方程式によって記述さ
れることができるものとする。

【００１４】

【数１】ｄ〔ｘ〕／ｄｔ＝〔Ａ〕〔ｘ〕＋〔Ｃ〕〔ｗ〕

【００１５】この式の各項は次のようなものである。 〔ｘ〕：推定値の状態ベクトル 〔Ａ〕：システム行列 〔ｗ〕：システム外乱ベクトル 〔Ｃ〕：システム外乱係数行列（一般的に単位行列が使
われる場合が多い） この式を遷移行列法によって離散形に変形すると次のよ
うになる。

【００１６】

【数２】〔ｘ（ｔ＋Δｔ）〕＝〔Φ〕〔ｘ（ｔ）〕＋
〔Ｇ〕〔ｗ〕

【００１７】観測系は次の式によって表される。

【００１８】

【数３】〔ｙ〕＝〔Ｈ〕〔ｘ〕＋〔ｖ〕

【００１９】この式の各項は次のようなものである。 〔ｙ〕：観測ベクトル 〔Ｈ〕：観測行列 〔ｖ〕：観測外乱ベクトル システムが数１の式〜数３の式によって記述されること
ができる場合には、推定値〔ｘ〕は次の式によって求め
られる。

【００２０】

【数４】 Ｐ_E （ｔ＋Δｔ，ｔ）＝Φ・Ｐ_E （ｔ，ｔ）Φ^T ＋Ｇ
（Ｑ_E ／Δｔ）Ｇ^T Ｋ（ｔ＋Δｔ）＝Ｐ_E （ｔ＋Δｔ，ｔ）Ｈ^T 〔Ｈ・Ｐ_E
（ｔ＋Δｔ，ｔ）Ｈ^T＋Ｒ_E 〕^-1 ｘ（ｔ＋Δｔ，ｔ）＝Φ・ｘ（ｔ，ｔ） ｘ（ｔ＋Δｔ，ｔ＋Δｔ）＝ｘ（ｔ＋Δｔ，ｔ）＋Ｋ
（ｔ＋Δｔ）〔ｙ−Ｈ・ｘ（ｔ＋Δｔ，ｔ）〕 Ｐ_E （ｔ＋Δｔ，ｔ＋Δｔ）＝〔Ｉ−Ｋ（ｔ＋Δｔ）・
Ｈ〕Ｐ_E （ｔ＋Δｔ，ｔ）

【００２１】

【数５】 Φ＝Ｉ＋ＡΔｔ＋Δｔ² ／２・Ａ² Ｇ＝｛ΦΔｔ（Ｉ−Δｔ／２・Ａ）｝Ｃ

【００２２】Ｉは単位行列である。Ｐ_E は状態ベクトル
の誤差共分散行列、Ｑ_E はシステム外乱ベクトルの誤差
共分散行列、Ｒ_E は観測外乱ベクトルの誤差共分散行列
であり、それぞれ次のように表される。尚、右辺のＥは
期待値を表す。

【００２３】

【数６】 Ｐ_E ＝Ｅ〔ｘ，ｘ^T 〕 Ｑ_E ＝Ｅ〔ｗ，ｗ^T 〕 Ｒ_E ＝Ｅ〔ｖ，ｖ^T 〕

【００２４】尚、Ｐ_E ，Ｑ_E ，Ｒ_E については後に詳細
に説明する。Ｋはカルマンゲインであり、システム誤差
と観測ノイズとの統計量を比較し、システムと観測値の
どちらに重みを付けるかを決定する量である。

【００２５】数４の式の各項は次のような意味を有す
る。Ｐ_E （ｔ＋Δｔ，ｔ）は現時刻ｔでのシステム誤差
の統計量がΔｔ秒後にどの程度変化するのかを予想する
ものである。ｘ（ｔ＋Δｔ，ｔ）は、時刻ｔにおける時
刻ｔ＋Δｔの予測値である。ｘ（ｔ＋Δｔ，ｔ＋Δｔ）
は最適推定値であり、観測値と予測値との偏差とカルマ
ンゲインＫを用いて計算する。Ｐ_E （ｔ＋Δｔ，ｔ＋Δ
ｔ）は誤差共分散行列Ｐ _E の修正値である。

【００２６】次に、カルマンフィルタを用いて取付誤差
φの推定演算を行う。カルマンフィルタの観測値として
（１）マスタ座標系と航走体座標系の間の加速度差又は
角速度差を用いる場合と、（２）マスタ座標系と航走体
座標系の間の速度差又は姿勢差を使用する場合がある。

【００２７】（１）マスタ座標系と航走体座標系の間の
加速度差又は角速度差を観測値とする場合：取付誤差φ
は時間変化しない固定値であるため、次の式が成り立
つ。

【００２８】

【数７】〔ｄφ／ｄｔ〕＝０＋〔ｗ〕

【００２９】図５を参照して、マスタＩＮＳによって検
出された角速度又はマスタ座標系の角速度ω_X ，ω_Y と
航走体ＩＮＳによって検出された角速度又は航走体座標
系の角速度ω_X ’，ω_Y ’の間の関係を求める。尚、説
明の簡単化のため２次元の場合を考える。

【００３０】

【数８】 ω_X ’＝ω_X ・ｃｏｓφ_Z ＋ω_Y ・ｓｉｎφ_Z ω_Y ’＝−ω_X ・ｓｉｎφ_Z ＋ω_Y ・ｃｏｓφ_Z ω_X ，ω_Y ：マスタ座標系の角速度 ω_X ’，ω_Y ’：航走体座標系の角速度 φ_Z ：Ｚ軸周りの取付誤差

【００３１】Ｚ軸周りの取付誤差φ_Z が微小であるとす
ると、次の近似式が成り立つ。

【００３２】

【数９】 ｓｉｎφ_Z ≒φ_Z ｃｏｓφ_Z ≒１

【００３３】従って数８の式は次のようになる。

【００３４】

【数１０】 ω_X −ω_X ’＝−ω_Y ・φ_Z ω_Y −ω_Y ’＝ω_X ・φ_Z

【００３５】数１０の式より、Ｚ軸周りの取付誤差φ_Z
は、Ｘ軸周りの角速度差又はＹ軸周りの角速度差を観測
値として推定可能であることが判る。これを三次元に拡
張すると、次のように表される。

【００３６】

【数１１】ω−ω’＝φ×ω ω：マスタ座標系の角速度 ω’：航走体座標系の角速度

【００３７】加速度についても同様な関係式が得られ
る。結局、角速度差Δω及び加速度差Δａは次のように
表される。

【００３８】

【数１２】 Δω＝φ×ω＋ｖ Δａ＝φ×ａ＋ｖ Δω：マスタ座標系の角速度と航走体座標系の角速度の
差 Δａ：マスタ座標系の加速度と航走体座標系の加速度の
差 ω：マスタ座標系の角速度 ａ：マスタ座標系の加速度 φ：取付誤差 ｖ：観測外乱ベクトル

【００３９】この式を展開すると次のようになる。

【００４０】

【数１３】 Δω_X ＝φ_Y ω_Z −φ_Z ω_Y Δω_Y ＝φ_Z ω_X −φ_X ω_Z Δω_Z ＝φ_X ω_Y −φ_Y ω_X Δａ_X ＝φ_Y ａ_Z −φ_Z ａ_Y Δａ_Y ＝φ_Z ａ_X −φ_X ａ_Z Δａ_Z ＝φ_X ａ_Y −φ_Y ａ_X

【００４１】数１３の式より、Ｘ，Ｙ，Ｚ軸周りの取付
誤差φ_X ，φ_Y ，φ_Z は、それぞれ以下の観測値に基づ
いて推定可能であることが判る。 Ｘ軸周りの取付誤差φ_X ：Ｙ・Ｚ加速度差、Ｙ・Ｚ角速
度差 Ｙ軸周りの取付誤差φ_Y ：Ｘ・Ｚ加速度差、Ｘ・Ｚ角速
度差 Ｚ軸周りの取付誤差φ_Z ：Ｘ・Ｙ加速度差、Ｘ・Ｙ角速
度差

【００４２】しかし、艦船には大きな加速度運動がな
く、加速度差は重力加速度の影響が支配的である。つま
り、Ｘ，Ｙ軸周りに取付誤差φ_X ，φ_Y が存在し、重力
成分が他軸に現れる場合に、加速度差が発生する。ただ
し、Ｚ加速度差は取付誤差φ_X，φ_Y を微小角とすれ
ば、近似的に０である。結局Ｘ，Ｙ，Ｚ軸周りの取付誤
差φ_X ，φ_Y ，φ_Z は次の表１のような観測値に基づい
て推定可能である。

【００４３】

【表１】 Ｘ軸周りの取付誤差φ_X ：Ｙ加速度差、Ｙ・Ｚ角速度差 Ｙ軸周りの取付誤差φ_Y ：Ｘ加速度差、Ｘ・Ｚ角速度差 Ｚ軸周りの取付誤差φ_Z ：Ｘ・Ｙ角速度差

【００４４】（２）マスタ座標系と航走体座標系の間の
速度差と姿勢差（ここで姿勢差は方位角差を含む。）を
観測値とする場合：先ず、マスタＩＮＳ１０と航走体Ｉ
ＮＳ２０の姿勢差Δψ及び速度差Δｖを求める。

【００４５】

【数１４】 ｄΔψ／ｄｔ＝−〔ω’×〕Δψ＋〔ω’×〕φ ｄΔｖ／ｄｔ＝−Ｃ^L 〔ａ’×〕Δψ＋Ｃ^L 〔ａ’×〕
φ ω’：航走体座標系の角速度 ａ’：航走体座標系の加速度、 Ｃ^L ：マスタＩＮＳの方向余弦行列

【００４６】行列〔ω’×〕及び〔ａ’×〕は次のよう
に表される。

【００４７】

【数１５】

【００４８】従って、数１４の式を行列形式によって表
すと、次のようになる。

【００４９】

【数１６】

【００５０】この式を数１の式と比較すると次の関係が
得られる。

【００５１】

【数１７】

【００５２】観測値は速度差ｙ（Δｖ）と姿勢差ｙ（Δ
ψ）である。従って、数３の式の形に表される。

【００５３】

【数１８】

【００５４】ここでＩ₃ は３×３の単位行列、０₃ は３
×３の０行列である。この式を数３の式と比較すると次
の関係が得られる。

【００５５】

【数１９】

【００５６】数１６の式より、Ｘ_, Ｙ_, Ｚ軸周りの取付
誤差φ_X ，φ_Y ，φ_Z は、それぞれ次のような観測値に
基づいて推定可能であることが判る。 Ｘ軸周りの取付誤差φ_X ：Ｖ_Y ・Ｖ_Z 速度差、ピッチ角
・方位差 Ｙ軸周りの取付誤差φ_Y ：Ｖ_X ・Ｖ_Z 速度差、ロール角
・方位差 Ｚ軸周りの取付誤差φ_Z ：Ｖ_X ・Ｖ_Y 速度差、ロール角
・ピッチ角差

【００５７】しかし、艦船には大きな加速度運動がな
く、速度差は重力成分による加速度の影響が支配的であ
る。つまり、Ｘ_, Ｙ軸周りに取付誤差φ_X ，φ_Y が存在
し、重力成分が他軸に現れる場合に、速度差が発生す
る。ただし、Ｖ_Z 速度差は取付誤差φ_X ，φ_Y を微小角
とすれは、近似的に０である。結局、Ｘ_, Ｙ，Ｚ軸周り
の取付誤差φ_X ，φ_Y ，φ_Z は次のような観測値に基づ
いて推定可能である。

【００５８】

【表２】 Ｘ軸周りの取付誤差φ_X ：Ｖ_Y 速度差、ピッチ角・方位差 Ｙ軸周りの取付誤差φ_Y ：Ｖ_X 速度差、ロール角・方位差 Ｚ軸周りの取付誤差φ_Z ：ロール角・ピッチ角差 ところで、観測値として用いることができる速度差は、
ΔＶ_n （南北速度差）、ΔＶ_e （東西速度差）、ΔＶ_d
（鉛直速度差）である。（ΔＶ_X ，ΔＶ_Y ，ΔＶ_Z ）を
座標変換行列（Ｃ^L ）で座標変換すると（ΔＶ_n ，ΔＶ
_e ，ΔＶ_d ）になる。

【００５９】

【発明が解決しようとする課題】マスタＩＮＳ１０と航
走体ＩＮＳ２０の間には固定的な取付誤差φの他に、艦
船の動揺によって生ずる船体の歪δ（取付誤差φの変動
成分）に起因した偏差が存在する。今、艦船の動揺が次
のように表されるものとする。

【００６０】

【数２０】 Ｒ＝Ｒ_O ・ｓｉｎ（ω_R ・ｔ＋α_R ） Ｐ＝Ｐ_O ・ｓｉｎ（ω_P ・ｔ＋α_P ） Ａ＝Ａ_O ・ｓｉｎ（ω_A ・ｔ＋α_A ） Ｒ：ロール角 Ｐ：ピッチ角 Ａ：方位角 Ｒ_O ：ロール角の振幅 Ｐ_O ：ピッチ角の振幅 Ａ_O ：方位角の振幅 ω_R ：＝２π／Ｔ_R （Ｔ_R ：ロールの動揺周期） ω_P ：＝２π／Ｔ_P （Ｔ_P ：ピッチの動揺周期） ω_A ：＝２π／Ｔ_A （Ｔ_A ：方位の動揺周期） α_R ：ロールの動揺の位相 α_P ：ピッチの動揺の位相 α_A ：方位の動揺の位相

【００６１】また、船体の歪δが次のように表されるも
のとする。

【００６２】

【数２１】 δ_R ＝δ_RO・ｓｉｎ（ω_R ・ｔ＋β_R ） δ_P ＝δ_PO・ｓｉｎ（ω_P ・ｔ＋β_P ） δ_A ＝δ_AO・ｓｉｎ（ω_A ・ｔ＋β_A ） δ_R ：ロール歪 δ_P ：ピッチ歪 δ_A ：方位歪 δ_RO：ロール歪の振幅 δ_PO：ピッチ歪の振幅 δ_AO：方位歪の振幅 ω_R ：＝２π／Ｔ_R （Ｔ_R ：ロール歪の周期） ω_P ：＝２π／Ｔ_P （Ｔ_P ：ピッチ歪の周期） ω_A ：＝２π／Ｔ_A （Ｔ_A ：方位歪の周期） β_R ：ロール歪の位相 β_P ：ピッチ歪の位相 β_A ：方位歪の位相

【００６３】マスタＩＮＳ１０と航走体ＩＮＳ２０によ
って検出される角速度の差は、Ｘ，Ｙ，Ｚ軸周りの取付
誤差φ_X ，φ_Y ，φ_Z に起因するが、先ず、Ｚ軸周りの
取付誤差φ_Z のみを考える。また、方位の動揺Ａ及び方
位歪δ_A は微小とし、方位の動揺及び方位歪の位相
α_A ，β_A が０であるとする。数１２の式より次の式が
得られる。

【００６４】

【数２２】 Δω_X ＝−δ_RO・ω_R ・ｃｏｓ（ω_R ・ｔ）−φ_Z ・Ｐ
_O ・ω_P ・ｃｏｓ（ω _P ・ｔ）・ｃｏｓＲ Δω_Y ＝−δ_PO・ω_P ・ｃｏｓ（ω_P ・ｔ）・ｃｏｓＲ
＋φ_Z ・Ｒ_O ・ω_R ・ｃｏｓ（ω_R ・ｔ）

【００６５】これは実際に観測される値である。カルマ
ンフィルタでは、歪をモデル化していないため、観測さ
れる角速度差は取付誤差φのみによって生じたものと判
断して推定を行う。カルマンフィルタのモデル式では次
のようになる。

【００６６】

【数２３】 Δω_X ＝−φ_Z ・Ｐ_O ・ω_P ・ｃｏｓ（ω_P ・ｔ）・ｃ
ｏｓＲ Δω_Y ＝＋φ_Z ・Ｒ_O ・ω_R ・ｃｏｓ（ω_R ・ｔ）

【００６７】ロール及びピッチの動揺と歪の角振動数ω
_R ，ω_P が等しく（ω_R ＝ω_P ）、ロール角Ｒは小さい
（ｃｏｓＲ＝１）ものとすると、Ｚ軸周りの取付誤差φ
_Z の推定値は、Ｘ角速度差及びＹ角速度差をそれぞれ観
測値として次のように推定される。

【００６８】

【数２４】 φ_Z （推定値）＝φ_Z （真値）＋δ_RO／Ｐ_O φ_Z （推定値）＝φ_Z （真値）−δ_PO／Ｒ_O

【００６９】以上の議論はＺ軸周りの取付誤差φ_Z につ
いて考えたが、Ｘ，Ｙ軸周りの取付誤差φ_X ，φ_Y を考
慮した場合も同様に求められる。

【００７０】

【数２５】 φ_X （推定値）＝φ_X （真値）＋δ_PO／Ａ_O φ_X （推定値）＝φ_X （真値）−δ_AO／Ｐ_O

【００７１】

【数２６】 φ_Y （推定値）＝φ_Y （真値）−δ_RO／Ａ_O φ_Y （推定値）＝φ_Y （真値）＋δ_AO／Ｒ_O

【００７２】数２４の式〜数２６の式は角速度差及び加
速度差を観測値として取付誤差φを推定したが、速度差
及び姿勢角差を観測値とした場合（詳細は省略）も、同
様な式が得られる。

【００７３】以上のように、従来のトランスファアライ
ンメント法では、Ｘ，Ｙ，Ｚ軸周りの取付誤差φ_X ，φ
_Y ，φ_Z の推定値は、船体の歪みδに起因した誤差（数
２４の式〜数２６の式の右辺の第２項）を含む。

【００７４】本発明は、斯かる点に鑑み、トランスファ
アラインメント法により取付誤差及び航走体の姿勢及び
方位を推定する慣性航法装置において、船体の歪δによ
って生ずる取付誤差φの推定値の誤差を低減することを
目的とするものである。

【００７５】

【課題を解決するための手段】本発明によると、航行体
に装備された航走体の姿勢及び方位の初期化における取
付誤差の推定をカルマンフィルタを用いて行うように構
成された慣性航法装置において、Ｘ軸及びＹ軸周りの取
付誤差の推定において、該慣性航法装置及び上記航行体
に装着された主慣性航法装置によって求められた角速度
の差よりも加速度の差に基づいた推定に重みをおき、カ
ルマンフィルタの観測外乱ベクトルの誤差共分散行列Ｒ
_E の角速度差を標準値より大きな値に設定したことを特
徴とする。

【００７６】本発明によると、慣性航法装置において、
Ｚ軸周りの取付誤差の推定において、上記２つの慣性航
法装置によって求められたＹ角速度差よりもＸ角速度差
に基づいた推定に重みをおき、上記行列Ｒ_E のＹ角速度
差とＸ角速度差の値を、 Ｒ_E （Ｘ角速度差）＜Ｒ_E （Ｙ角速度差） とすることを特徴とする。

【００７７】本発明によると、航行体に装備された航走
体の姿勢及び方位の初期化における取付誤差の推定をカ
ルマンフィルタを用いて行うように構成された慣性航法
装置において、Ｘ軸及びＹ軸周りの取付誤差の推定にお
いて、該慣性航法装置及び上記航行体に装着された主慣
性航法装置によって求められたロール角差、ピッチ角
差、方位角差よりも速度差に基づいた推定に重みをお
き、カルマンフィルタの観測外乱ベクトルの誤差共分散
行列Ｒ_E のロール角差、ピッチ角差及び方位角差を標準
値より大きな値に設定したことを特徴とする。

【００７８】本発明によると、慣性航法装置において、
Ｚ軸周りの取付誤差の推定において、上記２つの慣性航
法装置によって求められたピッチ角差よりもロール角差
に基づいた推定に重みをおき、上記行列Ｒ_E のロール角
差及びピッチ角差を、 Ｒ_E（ロール角差）＜Ｒ_E （ピ
ッチ角差） とすることを特徴とする。更に、Ｚ軸周りの取付誤差の
推定において、該慣性航法装置及び上記航行体に装着さ
れた主慣性航法装置によって求められたピッチ角差に基
づいて取付誤差φ_Z を推定するよりもピッチ角差そのも
のを推定するようにカルマンフィルタのシステム外乱ベ
クトルの誤差共分散行列Ｑ_E の取付誤差φ _Z とピッチ角
差を、 Ｑ_E （取付誤差φ_Z ）＜Ｑ_E （ピッチ角差） とすることを特徴とする。

【００７９】

【発明の実施の形態】上述の表１及び表２に示したよう
に、カルマンフィルタによって取付誤差φを推定する場
合に、観測値として複数のパラメータを選択することが
できる。本発明ではこれらのパラメータに対して重み付
けをする。本発明によると、観測値からの推定に重み付
けをし、どの観測値に対してどの程度推定の重み付けを
するかを配分し、それによって取付誤差φに含まれる船
体の歪みδに起因した誤差を最小化する。

【００８０】推定の重み付けに関係するパラメータはカ
ルマンゲインＫである。カルマンゲインＫは、どの観測
値からどの状態ベクトルをどの程度に推定するかを表
す。カルマンゲインＫは上述の誤差共分散行列Ｐ_E ，Ｑ
_E ，Ｒ_E によって変化する。従って数６の式に示した誤
差共分散行列Ｐ_E ，Ｑ_E ，Ｒ_E を設計値とすればよい。
より詳細には、設計値として、状態ベクトルの誤差共分
散行列Ｐ_E の初期値Ｐ_EI、システム外乱ベクトルの誤差
共分散行列Ｑ_E 、観測外乱ベクトルの誤差共分散行列Ｒ
_E の値を設定する。

【００８１】図６を参照してカルマンゲインＫと行列Ｐ
_EI，Ｑ_E ，Ｒ_E の関係を説明する。図６は、カルマンゲ
インＫの応答曲線を表すグラフである。例として、Ｘ角
速度差Δω_X を観測値として、Ｚ軸周りの取付誤差φ_Z
を推定する場合について説明する。図６にて、縦軸はカ
ルマンゲインＫ（φ_Z ，Δω_X ）〔ｍｉｎ／（ｒａｄ／
ｓｅｃ）〕であり、観測値Δω_X が観測された時に、取
付誤差φ_Z をどの程度推定するかを表す。横軸は時間
〔ｓｅｃ〕である。

【００８２】図示のように、一般に、初期（時間ｔ₁ ）
のカルマンゲインＫは大きく、その後減少し、定常状態
（時間ｔ₂ ）では一定となる。

【００８３】（ａ）状態ベクトルの誤差共分散行列Ｐ_E
の初期値Ｐ_EI：この初期値Ｐ_EIによって、対応する状態
ベクトルの初期のカルマンゲインＫを決定することがで
きる。行列Ｐ_EIの要素の値が大きいほど初期のカルマン
ゲインＫが大きくなる。

【００８４】（ｂ）システム外乱ベクトルの誤差共分散
行列Ｑ_E ：行列Ｑ_E によって、対応する状態ベクトルの
カルマンゲインＫを決定することができる。行列Ｑ_E の
要素の値が大きいほど定常状態におけるカルマンゲイン
Ｋが大きくなる。

【００８５】（ｃ）観測外乱ベクトルの誤差共分散行列
Ｒ_E ：カルマンゲインＫの全体的な値を決定するのが行
列Ｒ_E である。例えば、実線の曲線は行列Ｒ_E の要素が
大きく、破線の曲線は行列Ｒ_E の要素が小さい。行列Ｒ
_E は、対応する観測値の精度を表す。行列Ｒ_E の要素の
値が大きいほど、観測値は多くの誤差を含むことを示
す。従って行列Ｒ_E の要素の値が大きい場合には、その
観測値を用いないようにする。

【００８６】ここで、観測値の精度によって決定した行
列Ｒ_E を標準値と称することとする。観測値のΔω、Δ
ａ、Δψ及びΔｖの精度はセンサ等の性能によって決ま
るが、船体の歪みδに起因した誤差を含む。この誤差δ
が含まれない値が標準値である。

【００８７】角速度差及び加速度差を観測値として取付
誤差φ_Z を推定する場合、６個のパラメータΔω_X ，Δ
ω_Y ，Δω_Z ，Δａ_X ，Δａ_Y ，Δａ_Z を観測値とする
ことができる。従って、それに対応して６個のカルマン
ゲインＫ（φ_Z ，Δω_X ），Ｋ（φ_Z ，Δω_Y ），Ｋ
（φ_Z ，Δω_Z ），Ｋ（φ_Z ，Δａ_X ），Ｋ（φ_Z ，Δ
ａ_Y ），Ｋ（φ_Z ，Δａ_Z ）が存在する。

【００８８】各行列Ｐ_EI，Ｒ_E ，Ｑ_E の要素を設定する
ことによって、６個のカルマンゲインＫの大小関係が決
まる。即ち、行列Ｐ_EI，Ｒ_E ，Ｑ_E を設計することによ
って、どの観測値から、どの状態ベクトルを、どの程度
推定するかを決定することができる。

【００８９】まず航走体座標系とマスタ座標系の加速度
差及び角速度差を観測値として取付誤差φを推定する場
合について説明し、次に航走体座標系とマスタ座標系の
速度差及び姿勢角差を観測値として取付誤差φを推定す
る場合について説明する。

【００９０】（１）加速度差及び角速度差を観測値とし
て取付誤差φを推定する場合：数３の式の推定値の状態
ベクトル〔ｘ〕、観測ベクトル〔ｙ〕は次のように表さ
れる。

【００９１】

【数２７】

【００９２】数１の式及び数７の式よりシステム行列
〔Ａ〕はゼロとなる。

【００９３】

【数２８】

【００９４】数３の式及び数１２の式より観測行列
〔Ｈ〕は次のようになる。

【００９５】

【数２９】

【００９６】状態ベクトルの誤差共分散行列の初期値Ｐ
_EI、システム外乱ベクトルの誤差共分散行列Ｑ_E 及び観
測外乱ベクトルの誤差共分散行列Ｒ_E はそれぞれ次のよ
うになる。

【００９７】

【数３０】

【００９８】

【数３１】

【００９９】

【数３２】

【０１００】ここに、小文字ｐ，ｑ，ｒは、それぞれ行
列Ｐ_EI，Ｑ_E ，Ｒ_E の対角要素であることをを示す。

【０１０１】上述の表１に示したように、Ｘ，Ｙ，Ｚ周
りの取付誤差φ_X ，φ_Y ，φ_Z は２種類以上の観測値に
基づいて推定が可能である。どの観測値からの推定に重
みをおくかは、カルマンフィルタの設計値のうち観測外
乱ベクトルの誤差共分散行列Ｒ_E によって決定する。行
列Ｒ_E は観測値の精度を表し、行列Ｒ_E の要素のうち、
小さな値に設定したパラメータに対して推定に重みが付
けられる。

【０１０２】（１−Ａ）Ｘ，Ｙ軸周りの取付誤差φ_X ，
φ_Y ：表１に示したように、Ｘ，Ｙ軸周りの取付誤差φ
_X ，φ_Y は加速度差又は角速度差を観測値として推定が
可能である。角速度差と比べて、加速度差は船体の歪δ
による影響が小さいため、Ｘ，Ｙ軸周りの取付誤差
φ_X ，φ_Y は加速度差からの推定に重みをおく。本例で
は、加速度差からの推定に重みを付けることを、観測外
乱ベクトルの誤差共分散行列Ｒ_E によって決定する。行
列Ｒ_E は観測値の精度を表すため、この値の小さい観測
値に重みをおくことにする。

【０１０３】即ち、数３２の式に示す行列Ｒ_E の要素の
うち、角速度差ｒΔω_X ，ｒΔω_Y，ｒΔω_Z の値を加
速度差ｒΔａ_X ，ｒΔａ_Y ，ｒΔａ_Z の値より大きくす
る。両者は次元が異なるため直接比較することはできな
いが、角速度差ｒΔω_X ，ｒΔω_Y ，ｒΔω_Z の値をそ
の標準値より大きくする。それによって、加速度差Δａ
_X ，Δａ_Y ，Δａ_Z からの推定に重みが付けられる。

【０１０４】（１−Ｂ）Ｚ軸周りの取付誤差φ_Z ：表１
に示したように、Ｚ軸周りの取付誤差φ_Z はＸ・Ｙ角速
度差を観測値として推定が可能である。しかし、姿勢角
が船体の歪δに起因した誤差を含むと、Ｚ軸周りの取付
誤差φ_Z の推定値も誤差を含む。Ｘ角速度差及びＹ角速
度差を観測値として推定した取付誤差φ_Z は、数２４の
式に示したように、誤差δ_RO／Ｐ_O，δ_PO／Ｒ_O を含
む。

【０１０５】Ｘ角速度差とＹ角速度差からの推定の重み
付けが等しければ、カルマンフィルタによる取付誤差φ
_Z の推定値は、次のような経験式（実験式）によって表
される。

【０１０６】

【数３３】

【０１０７】一般に、艦船の歪δにおいて、ロール軸周
りの船体の歪δ_R よりもピッチ軸周りの船体の歪δ_P が
かなり大きい。従って、船体の歪δの影響が大きいＹ角
速度差を観測値として用いると取付誤差φ_Z の推定値に
含まれる誤差が大きくなる。従って取付誤差φ_Z の推定
値に含まれる誤差に関して次の関係式が成り立つ。

【０１０８】

【数３４】ＥＲＲ（φ_Z ，Δω_X ）＜ＥＲＲ（φ_Z ，Δ
ω_X ＋Δω_Y ）＜ＥＲＲ（φ_Z ，Δω_Y ）

【０１０９】ここに、各記号は次のような意味である。 ＥＲＲ（φ_Z ，Δω_X ）：Ｘ角速度差のみから推定した
取付誤差φ_Z に含まれる誤差 ＥＲＲ（φ_Z ，Δω_X ＋Δω_Y ）：Ｘ・Ｙ角速度差から
推定した取付誤差φ_Zに含まれる誤差 ＥＲＲ（φ_Z ，Δω_Y ）：Ｙ角速度差のみから推定した
取付誤差φ_Z に含まれる誤差

【０１１０】以上より、船体の歪δの影響が大きいＹ角
速度差よりも船体の歪δの影響が小さいＸ角速度差を観
測値としてＺ軸周りの取付誤差φ_Z を推定する。Ｘ角速
度差からの推定に重みを付けることを、観測外乱ベクト
ルの誤差共分散行列Ｒ_E によって決定する。行列Ｒ_E は
観測値の精度を表すため、この値の小さい観測値に重み
をおくことにする。

【０１１１】即ち、数３２の式に示す行列Ｒ_E の要素の
うち、Ｘ角速度差ｒΔω_X の値をＹ角速度差ｒΔω_Y よ
り小さくする。それによって、Ｘ角速度差からの推定に
重みが付けられる。

【０１１２】

【数３５】ｒΔω_X ＜ｒΔω_Y

【０１１３】２つの設計値ｒΔω_X ，ｒΔω_Y の間に、
例えば、次のような比率を設定する。

【０１１４】

【数３６】ｒΔω_X ：ｒΔω_Y ＝１：１００

【０１１５】（２）速度差と姿勢角差（ロール角差、ピ
ッチ角差及び方位角差）を観測値として取付誤差φを推
定する場合：数３の式の推定値の状態ベクトル〔ｘ〕、
観測ベクトル〔ｙ〕は次のように表される。

【０１１６】

【数３７】

【０１１７】数１の式及び数１６の式よりシステム行列
〔Ａ〕は次のようになる。

【０１１８】

【数３８】

【０１１９】数３の式及び数１８の式より観測行列
〔Ｈ〕は次のようになる。

【０１２０】

【数３９】

【０１２１】状態ベクトルの誤差共分散行列の初期値Ｐ
_EI、システム外乱ベクトルの誤差共分散行列Ｑ_E 及び観
測外乱ベクトルの誤差共分散行列Ｒ_E はそれぞれ次のよ
うになる。

【０１２２】

【数４０】

【０１２３】

【数４１】

【０１２４】

【数４２】

【０１２５】上述の表２に示したように、Ｘ，Ｙ，Ｚ周
りの取付誤差φ_X ，φ_Y ，φ_Z は２種類以上の観測値に
基づいて推定が可能である。どの観測値からの推定に重
みをおくかは、カルマンフィルタの設計値Ｐ_EI，Ｑ_E ，
Ｒ_E のうち観測外乱ベクトルの誤差共分散行列Ｒ_E によ
って決定する。行列Ｒ_E は観測値の精度を表し、行列Ｒ
_E の要素のうち、小さな値に設定したパラメータに対し
て推定に重みが付けられる。

【０１２６】（２−Ａ）Ｘ，Ｙ軸周りの取付誤差φ_X ，
φ_Y ：姿勢角差Δψ（ロール角差、ピッチ角差及び方位
角差）と比べて、速度差Δｖは歪δによる影響が少ない
から、Ｘ，Ｙ軸周りの取付誤差φ_X ，φ_Y は速度差Δｖ
からの推定に重みを付ける。

【０１２７】即ち、数４２の式に示す行列Ｒ_E の要素の
うち、姿勢角差ｒΔψ_R ，ｒΔψ_P，ｒΔψ_A の値より
速度差ｒΔｖ_n ，ｒΔｖ_e ，ｒΔｖ_d の値を小さくす
る。両者は次元が異なるため直接比較することはできな
いが、姿勢角差ｒΔψ_R ，ｒΔψ_P ，ｒΔψ_A の値をそ
の標準値より大きくする。それによって、速度差Δ
ｖ_n，Δｖ_e ，Δｖ_d からの推定に重みが付けられる。

【０１２８】（２−Ｂ）Ｚ軸周りの取付誤差φ_Z ：表２
に示したように、Ｚ軸周りの取付誤差φ_Z はロール差及
びピッチ差を観測値として推定が可能である。しかし、
ロール角又はピッチ角に船体の歪δに起因した誤差が含
まれると、Ｚ軸周りの取付誤差φ_Z の推定値は誤差を含
む。角速度差を観測値として推定した場合と同様に、ロ
ール差及びピッチ差を観測値とした取付誤差φ_Z の推定
値は数２２の式に示したように誤差δ_RO／Ｐ_O ，δ_PO／
Ｒ_Oを含む。

【０１２９】ロール差とピッチ差からの推定の重み付け
が等しい場合に、カルマンフィルタによる取付誤差φ_Z
の推定値の場合も、数３３の式の経験式（実験式）によ
って表される。ここで、艦船の歪δに関して、ロール軸
周りの船体の歪δ_R よりもピッチ軸周りの歪δ_P がかな
り大きいことが判明した。従って、船体の歪δの影響が
大きいピッチ差を観測値として用いると誤差が大きくな
る。取付誤差φ_Z に含まれる誤差に関して次の関係式が
成り立つ。

【０１３０】

【数４３】ＥＲＲ（φ_Z ，ΔＲ）＜ＥＲＲ（φ_Z ，ΔＲ
＋ΔＰ）＜ＥＲＲ（φ_Z ，ΔＰ）

【０１３１】ここに、各記号は次のような意味である。 ＥＲＲ（φ_Z ，ΔＲ）：ロール差のみから推定した取付
誤差φ_Z に含まれる誤差 ＥＲＲ（φ_Z ，ΔＲ＋ΔＰ）：ロール差及びピッチ差か
ら推定した取付誤差φ _Z に含まれる誤差 ＥＲＲ（φ_Z ，ΔＰ）：ピッチ差のみから推定した取付
誤差φ_Z に含まれる誤差

【０１３２】以上より、船体の歪δの影響が大きいピッ
チ差よりもロール差を観測値としてＺ軸周りの取付誤差
φ_Z を推定する。具体的には、カルマンフィルタの観測
値の精度を表すＲ_E の値を次のように設定し、ロール差
からの推定に重みを大きくつける。

【０１３３】

【数４４】ｒΔψ_R ＜ｒΔψ_P

【０１３４】（２−Ｃ）Ｚ軸周りの取付誤差φ_Z ：ま
た、Ｑ_E の値によってもロール差の観測値からの推定に
重みをつけることができる。Ｑ_E の値は、どの推定項目
をより推定するかを決定するものであり、大きいほどそ
の推定項目を推定するようになる。

【０１３５】速度差及びロール差、ピッチ差、方位差を
観測値として推定する場合、取付誤差φの他に速度差と
ロール差、ピッチ差、方位差という推定項目が存在す
る。従って、ピッチ差を観測値とする場合、取付誤差φ
_Z に起因したピッチ差の他に船体の歪δ等に起因するピ
ッチ差が多く含まれるから、取付誤差φ_Z よりもピッチ
差そのものを推定するような設計値にする。

【０１３６】

【数４５】ｑφ_Z ＜ｑΔψ_P

【０１３７】数２２の式の第１式に示したように、ロー
ル差又はＸ角速度差を観測値としてＺ軸周りの取付誤差
φ_Z を推定した場合に、誤差δ_RO／Ｐ_O を含む。しかし
ながら、ロール軸周りの歪δ_ROは微小であるため、次の
ようになる。

【０１３８】

【数４６】φ_Z （推定値）≒φ_Z （真値）

【０１３９】従って、本発明により取付誤差φを精度良
く求めることが可能となる。以上の議論にて、角速度差
又は加速度差を観測値とする場合及び速度差又は姿勢角
差を観測値とする場合を説明したが、これらのパラメー
タを組み合わせたものを観測値としてもよい。

【０１４０】以上本発明の実施の形態について詳細に説
明したが、本発明はこれらの例に限定されることなく特
許請求の範囲に記載された発明の範囲にて様々な変更等
が可能であることは当業者にとって理解されよう。

【０１４１】以上にて艦船に搭載された航走体ＩＮＳを
例として説明したが、本発明は、トランスファアライン
メント法を用いて初期化を行うように構成された慣性航
法装置であればどのような慣性航法装置にも適用可能で
ある。例えば、艦船に搭載された水中航走体、無人機、
無人潜水艇等の航走体に適用可能である。

【０１４２】更に、本発明は、航走体に搭載された慣性
航法装置ばかりでなく、トランスファアラインメント法
を用いて初期化を行うように構成された慣性航法装置で
あればよく、艦船に搭載された航走体発射装置等の航走
体の発射装置に搭載された慣性航法装置にも適用可能で
ある。

【０１４３】

【発明の効果】本発明によると、トランスファアライン
メント法を用いて初期化を行う場合に、船体の歪δが存
在しても取付誤差φの推定を精度良く行うことができ、
航走体の初期化性能を向上することができる利点があ
る。

【０１４４】本発明によると、トランスファアラインメ
ント法を用いて初期化を行う場合に、船体の歪δが存在
しても取付誤差φの推定を精度良く行うことができるか
ら、荒天航行時のように船体歪みが比較的大きい場合に
も、航走体の運用が可能となる利点がある。

【図面の簡単な説明】

【図１】艦船及び航走体に搭載された慣性航法装置の配
置状態を示す図である。

【図２】艦船に搭載された航走体の配置状態を示す図で
ある。

【図３】艦船及び航走体に搭載された慣性航法装置の配
置状態を示す図である。

【図４】艦船に搭載された航走体の配置状態を示す図で
ある。

【図５】マスタＩＮＳの検出角速度と航走体ＩＮＳの検
出角速度の間の関係を示す図である。

【図６】カルマンゲインの応答を説明するための説明図
である。

【符号の説明】

１ 艦船、 ２ 航走体、 ３ 航走体発射管、 １０
マスタＩＮＳ、 ２０ 航走体ＩＮＳ、 ３０ スレ
ーブＩＮＳ

───────────────────────────────────────────────────── フロントページの続き (72)発明者 南木 真一 東京都大田区南蒲田２丁目16番46号 株 式会社トキメック内 (72)発明者 人見 亮 東京都大田区南蒲田２丁目16番46号 株 式会社トキメック内 (56)参考文献 特開 平９−89585（ＪＰ，Ａ) 特開 平６−18276（ＪＰ，Ａ) 特開 平６−11354（ＪＰ，Ａ) 特公 平７−31063（ＪＰ，Ｂ２) (58)調査した分野(Int.Cl.⁶，ＤＢ名) G01C 21/16 B64C 13/18 F41G 7/36 F42B 15/01

Claims (7)

(57)【特許請求の範囲】
1. 【請求項１】 航行体に装備された航走体の姿勢及び方
   位の初期化における取付誤差の推定をカルマンフィルタ
   を用いて行うように構成された慣性航法装置において、 Ｘ軸及びＹ軸周りの取付誤差の推定において、該慣性航
   法装置及び上記航行体に装着された主慣性航法装置によ
   って求められた角速度の差よりも加速度の差に基づいた
   推定に重みをおき、カルマンフィルタの観測外乱ベクト
   ルの誤差共分散行列Ｒ_E の角速度差を標準値より大きな
   値に設定したことを特徴とする慣性航法装置。
2. 【請求項２】 請求項１記載の慣性航法装置において、
   Ｚ軸周りの取付誤差の推定において、上記２つの慣性航
   法装置によって求められたＹ角速度差よりもＸ角速度差
   に基づいた推定に重みをおき、上記行列Ｒ_E のＹ角速度
   差とＸ角速度差の値を、 Ｒ_E （Ｘ角速度差）＜Ｒ_E （Ｙ角速度差） とすることを特徴とする慣性航法装置。
3. 【請求項３】 航行体に装備された航走体の姿勢及び方
   位の初期化における取付誤差の推定をカルマンフィルタ
   を用いて行うように構成された慣性航法装置において、 Ｘ軸及びＹ軸周りの取付誤差の推定において、該慣性航
   法装置及び上記航行体に装着された主慣性航法装置によ
   って求められたロール角差、ピッチ角差、方位角差より
   も速度差に基づいた推定に重みをおき、カルマンフィル
   タの観測外乱ベクトルの誤差共分散行列Ｒ_E のロール角
   差、ピッチ角差及び方位角差を標準値より大きな値に設
   定したことを特徴とする慣性航法装置。
4. 【請求項４】 請求項３記載の慣性航法装置において、
   Ｚ軸周りの取付誤差の推定において、上記２つの慣性航
   法装置によって求められたピッチ角差よりもロール角差
   に基づいた推定に重みをおき、上記行列Ｒ_E のロール角
   差及びピッチ角差を、 Ｒ_E （ロール角差）＜Ｒ_E （ピ
   ッチ角差） とすることを特徴とする慣性航法装置。
5. 【請求項５】 請求項３記載の慣性航法装置において、 Ｚ軸周りの取付誤差の推定において、該慣性航法装置及
   び上記航行体に装着された主慣性航法装置によって求め
   られたピッチ角差に基づいて取付誤差φ_Z を推定するよ
   りもピッチ角差そのものを推定するようにカルマンフィ
   ルタのシステム外乱ベクトルの誤差共分散行列Ｑ_E の取
   付誤差φ_Z とピッチ角差を、 Ｑ_E （取付誤差φ_Z ）＜Ｑ_E （ピッチ角差） とすることを特徴とする慣性航法装置。
6. 【請求項６】 請求項１〜５のいずれか１項記載の慣性
   航法装置において、該慣性航法装置は上記航走体自身又
   は上記航走体の発射装置の近傍に装着されていることを
   特徴とする慣性航法装置。
7. 【請求項７】 請求項１〜６のいずれか１項記載の慣性
   航法装置において、上記航走体の代わりに上記航行体に
   固定的な装置が装備されていることを特徴とする慣性航
   法装置。