JP2881225B1 - 慣性航法装置 - Google Patents

Abstract

【要約】 【課題】 航走体の姿勢及び方位の初期化においてカル
マンフィルタを用いて取付誤差を推定するように構成さ
れた慣性航法装置（ＩＮＳ）において、高精度にて遠心
加速度補正をすること目的をとする。 【解決手段】 航走体に搭載された慣性航法装置（航走
体ＩＮＳ）によって検出された角速度及び加速度と艦船
に搭載された慣性航法装置（マスタＩＮＳ）によって検
出された角速度及び加速度の差に基づいて、取付誤差を
推定する際に２つのＩＮＳの位置の差に起因して生ずる
遠心加速度誤差を補正する。この遠心加速度誤差の補正
においてαβフィルタを使用して角加速度の予測値を求
める。

Description

【発明の詳細な説明】

【０００１】

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

【０００２】

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

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

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

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

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

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

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

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

【００１０】取付誤差φが存在するために、マスタＩＮ
Ｓ１０の加速度計及びジャイロに入力する加速度及び角
速度と航走体ＩＮＳ２０の加速度計及びジャイロに入力
する加速度及び角速度との間に偏差が生じる。速度及び
姿勢角は加速度及び角速度に基づいて演算されるため、
これらの値もマスタＩＮＳ１０と航走体ＩＮＳ２０では
異なる値となる。

【００１１】艦船１が動揺すると、航走体ＩＮＳ２０及
びマスタＩＮＳ１０は回転運動をすることとなる。両者
間の相対的距離に起因して航走体ＩＮＳ２０及びマスタ
ＩＮＳ１０では加速度差が生ずる。これが遠心加速度誤
差である。トランスファアラインメント法による姿勢及
び方位の推定演算では、取付誤差φによって生ずる偏差
のみに基づいてなされるから、より正確な演算では遠心
加速度補正を行なう必要がある。速度は加速度より求め
るから、同様に遠心加速度補正をする必要がある。

【００１２】マスタＩＮＳ１０と航走体ＩＮＳ２０の間
の距離をＲとする。艦船には船体の動揺による角速度ω
が入力される。距離Ｒだけ離れた２点間の加速度差ΔＡ
は次の式のようにベクトルで表される。

【００１３】

【数１】ΔＡ＝（ｄω／ｄｔ）×Ｒ＋ω×（ω×Ｒ）

【００１４】ここで、ｄω／ｄｔは艦船に入力された角
加速度である。加速度差ΔＡ（ベクトル）の各成分は次
のようになる。

【００１５】

【数２】ΔＡ_X ＝−（ω_Y ² ＋ω_Z ² ）Ｒ_X ＋（−ｄω
_Z ／ｄｔ＋ω_X ω_Y ) Ｒ_Y＋（ｄω_Y ／ｄｔ＋ω_X ω_Z )
Ｒ_Z ΔＡ_Y ＝ (ｄω_Z ／ｄｔ＋ω_X ω_Y ）Ｒ_X −（ω_X ² ＋
ω_Z ² ）Ｒ_Y＋（−ｄω_X ／ｄｔ＋ω_Y ω_Z ) Ｒ_Z ΔＡ_Z ＝（−ｄω_Y ／ｄｔ＋ω_X ω_Z ) Ｒ_X＋（ｄω_X
／ｄｔ＋ω_Y ω_Z ) Ｒ_Y −（ω_X ² ＋ω_Y ² ）Ｒ_Z

【００１６】ω_X ，ω_Y ，ω_Z は角速度ωのＸ成分、Ｙ
成分及びＺ成分であり、Ｒ_X ，Ｒ_Y，Ｒ_Z は距離ＲのＸ
成分、Ｙ成分及びＺ成分である。遠心加速度の補正は、
サンプリング時間の速度変化という形で補正するため、
次式がサンプリング時間毎の補正値を表す補正式とな
る。

【００１７】

【数３】ΔＶ_X ＝〔-(θ_Y (n)²+ θ_Z (n)²) Ｒ_X ＋｛-
(θ_Z (n)-θ_Z (o))+ θ_X (n) θ_Y(n) ｝Ｒ_Y ＋｛( θ_Y
(n)-θ_Y (o))+ θ_X (n) θ_Z (n) ｝Ｒ_Z 〕／Δｔ ΔＶ_Y ＝〔｛（θ_Z (n)-θ_Z (o))+ θ_X (n) θ_Y (n) ｝
Ｒ_X −( θ_X (n)²+ θ_Z(n)²) Ｒ_Y ＋｛-(θ_X (n)-θ
_X (o))+ θ_Y (n) θ_Z (n) ｝Ｒ_Z 〕／Δｔ ΔＶ_Z ＝〔｛-(θ_Y (n)-θ_Y (o))+ θ_X (n) θ_Z (n) ｝
Ｒ_X ＋｛( θ_X (n)-θ_X(o))+ θ_Y (n) θ_Z (n) ｝Ｒ_Y
−( θ_X (n)²+ θ_Y (n)²) Ｒ_Z 〕／Δｔ

【００１８】ここで、θ(n) は新サンプリングでの変化
角度（ジャイロ出力角）であり、θ(o) は前サンプリン
グでの変化角度（ジャイロ出力角）である。またΔｔは
サンプリング時間である。遠心加速度の補正は、上式の
値を航走体ＩＮＳ２０によって検出された加速度より減
算することによって行われる。

【００１９】

【発明が解決しようとする課題】数３の式の補正式に
は、サンプリング時間における角速度変化Δθ／Δｔを
計算する次のような、量子化項が含まれる。

【００２０】

【数４】｛θ(n) −θ(o) ｝／Δｔ＝Δθ／Δｔ

【００２１】この量子化項の演算にて、ジャイロ出力の
分解能に依存した量子化誤差が生ずる。この量子化誤差
は、相対距離Ｒに比例して大きな加速度補正誤差となる
ため、航走体ＩＮＳ２０のようにマスタＩＮＳ１０から
離れた位置に設置される慣性航法装置の場合、大きな問
題となる。

【００２２】角加速度の量子化誤差（ｄω／ｄｔ）err
は、新サンプリングと前サンプリングで１パルスの差が
あるとして、ジャイロの分解能Ｐ〔rad/pulse 〕及びサ
ンプリング時間Δｔとによって次の式で表される。

【００２３】

【数５】 （ｄω／ｄｔ）err ＝Ｐ／Δｔ／Δｔ [rad/s²]

【００２４】加速度補正誤差Ａerr は、航走体ＩＮＳ２
０とマスタＩＮＳ１０の間の相対距離Ｒとすれば、次の
式によって表される。

【００２５】

【数６】Ａerr ＝Ｐ・Ｒ／Δｔ／Δｔ [cm/s²]

【００２６】例えば、ジャイロの分解能Ｐ〔rad/pulse
〕、サンプリング時間Δｔ、相対距離Ｒを次のような
値とする。

【００２７】

【数７】Δｔ ＝ 10 [ms] Ｒ ＝ 100 [m] Ｐ ＝ 3.5 [ ″/pulse]

【００２８】この場合、ジャイロ量子化誤差に起因して
発生する加速度誤差を計算すると次のようになる。

【００２９】

【数８】Ａerr ＝ 1697[cm/s²] ＝ 1.73[G]

【００３０】この値は実際の加速度を打ち消してしまう
ほど大きい誤差である。

【００３１】本発明は、航走体ＩＮＳ２０とマスタＩＮ
Ｓ１０の間の相対距離Ｒが大きい場合でも、遠心加速度
補正を正確に行なう慣性航法装置を提供することを目的
とする。

【００３２】

【課題を解決するための手段】本発明によると、航行体
に装備された航走体の姿勢及び方位の初期化における取
付誤差の推定をカルマンフィルタを用いて行うように構
成された慣性航法装置において、該慣性航法装置及び上
記航行体に装着された主慣性航法装置によって求められ
た角加速度の差を演算する際に遠心加速度補正を行い、
その補正に使用する角加速度の算出にαβフィルタを用
いることにより、ジャイロセンサの量子化誤差の影響を
低減することを特徴とする。

【００３３】

【発明の実施の形態】図１を参照して本発明による慣性
航法装置における遠心加速度補正値ΔＶの演算装置及び
演算手順を示す。図示のように演算装置は角度積算部２
２と角速度予測演算部２３と角加速度予測演算部２４と
遠心加速度補正部２５とを含む。本発明によると、ジャ
イロ出力の量子化誤差の演算において、精度向上のため
にαβフィルタを用いる。即ち、角速度予測値及び角加
速度予測値をαβフィルタを用いて演算する。

【００３４】慣性航法装置に装備されたジャイロセンサ
２１によって角速度が検出される。角速度は、サンプリ
ング時間Δｔ当たりの変化角度θとして検出される。こ
の変化角度θは、角度積算部２２に供給され積算され
る。

【００３５】

【数９】θｍ１＝Σθ

【００３６】角度積算部２２によって得られた積算角度
θｍ１は角速度予測演算部２３に供給される。角速度予
測演算部２３はαβフィルタを用いて角速度予測値ωｐ
１を得る。角速度予測値ωｐ１は次の式によって表され
る。

【００３７】

【数１０】θs1(n) ＝ θp1(n) ＋ α１｛θm1(n)
−θp1(n) ｝ ωs1(n) ＝ ωp1(n) ＋ β１｛θm1(n) −θp1(n)
｝／Δｔ θp1(n+1) ＝ θs1(n) ＋ ωs1 (n)・Δｔ ωp1(n+1) ＝ ωs1(n)

【００３８】ここに各項は次のような意味である。 θp1(n) ：時刻ｎ−１の時に予測される時刻ｎでの予測
角度である。 ωp1(n) ：時刻ｎ−１の時に予測される時刻ｎでの予測
角速度である。 θs1(n) ：平滑化した時刻ｎでの平滑化角度である。 ωs1(n) ：平滑化した時刻ｎでの平滑化角速度である。 θm1(n) ：測定角度（ジャイロ出力の積算角）である。 Δｔ：サンプリング時間。 α１，β１：定数。

【００３９】これは次のような意味を有する。平滑化値
は、予測値に対して、予測値と測定値の間の偏差を適当
な重み付けして加えたものである。その重み係数がα、
βである。α、βは、平滑化値を求める際に、測定値の
影響をどの程度与えるかを決めるための係数である。

【００４０】予測角度θp1(n) は、平滑化角度θs1(n)
に対して、平滑化角速度から得られた平滑化変化角度を
加えたものである。予測角速度ωp1(n) は平滑化角速度
ωs1(n) に等しい。

【００４１】次に、角速度予測値ωp1を角加速度予測演
算部２４に入力し、再度αβフィルタを用いて角加速度
予測値（ｄω／ｄｔ）p2を得る。尚、１段目のフィルタ
の予測値ωp1(n) を２段目のフィルタの測定値ωm2(n)
として用いるから、ωm2(n)＝ωp1(n) である。

【００４２】

【数１１】 ωs2(n) ＝ ωp2(n) ＋α２｛ωm2(n) −ωp2(n) ｝ （ｄω／ｄｔ）s2(n) ＝（ｄω／ｄｔ）p2(n) ＋β２
｛ωm2(n) −ωp2(n)｝／Δｔ ωp2(n+1) ＝ ωp2(n) ＋（ｄω／ｄｔ）s2 (n)・Δｔ （ｄω／ｄｔ）p2(n+1) ＝（ｄω／ｄｔ）p2(n)

【００４３】ここに各項は次のような意味である。 ωp2(n) ：時刻ｎ−１の時に予測される時刻ｎでの予測
角速度である。 （ｄω／ｄｔ）p2(n) ：時刻ｎ−１の時に予測される時
刻ｎでの予測角加速度である。 ωs2(n) ：平滑化した時刻ｎでの平滑化角速度である。 （ｄω／ｄｔ）s2(n) ：平滑化した時刻ｎでの平滑化角
加速度である。 ωm2(n) ：測定角速度（１段目フィルタの予測角速度）
である。 Δｔ：サンプリング時間。 α２，β２：定数。

【００４４】この角加速度予測値（ｄω／ｄｔ）p2を用
いて遠心加速度補正部２５において遠心加速度の補正を
行なう。サンプリング時間Δｔ毎の遠心加速度の補正値
ΔＶの各成分は次のようになる。

【００４５】

【数１２】ΔＶ_X ＝〔−（θ_Y ² ＋θ_Z ²)Ｒ_X ＋｛−
（ｄω／ｄｔ）p2_Z ・Δｔ²＋θ_X θ_Y ｝Ｒ_Y ＋｛（ｄ
ω／ｄｔ）p2_Y ・Δｔ² ＋θ_X θ_Z ｝Ｒ_Z 〕／Δｔ ΔＶ_Y ＝〔｛（ｄω／ｄｔ）p2_Z ・Δｔ² ＋θ_X θ_Y ｝
Ｒ_X−（θ_X ² ＋θ_Z ²) Ｒ_Y ＋｛−（ｄω／ｄｔ）p2
_X ・Δｔ² ＋θ_Y θ_Z ｝Ｒ_Z〕／Δｔ ΔＶ_Z ＝〔｛−（ｄω／ｄｔ）p2_Y ・Δｔ² ＋θ
_X θ_Z ｝Ｒ_X＋｛（ｄω／ｄｔ）p2_X ・Δｔ² ＋θ_Y θ
_Z ｝Ｒ _Y−（θ_X ² ＋θ_Y ²)Ｒ_Z 〕／Δｔ

【００４６】航走体ＩＮＳは、この補正値ΔＶ（Δ
Ｖ_X ，ΔＶ_Y ，ΔＶ_Z ）を用いて加速度計の出力信号を
補正する。

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

【００４８】なお、航走体ＩＮＳを例として説明した
が、本発明は航走体ＩＮＳのみに限定された技術ではな
く、遠心加速度を補正して加速度を比較しなければなら
ない慣性航法装置に適用できる。

【００４９】

【発明の効果】本発明によると、ジャイロの量子化誤差
による加速度誤差を、従来技術の約２００分の１に抑え
ることが可能であるという利点がある。

【００５０】また、本発明によると、遠心加速度補正に
用いる角加速度を正確に求めることが可能になるため、
ジャイロの分解能が悪い場合及びマスタＩＮＳと航走体
ＩＮＳとの相対距離が大きい場合でも、遠心加速度補正
を精度良く行なうことが可能であるという利点がある。

【００５１】従って、本発明によると、航走体ＩＮＳの
装備上の制約がなく、更に、航走体ＩＮＳの初期化性能
を向上させることができるという利点がある。

【図面の簡単な説明】

【図１】本発明による遠心加速度補正を演算するため演
算手順を示す図である。

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

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

【符号の説明】

１ 艦船、 ２ 航走体、 ３ 航走体発射装置、 １
０ マスタＩＮＳ、２０ 航走体ＩＮＳ

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

Claims (1)

(57)【特許請求の範囲】
1. 【請求項１】 航行体に装備された航走体の姿勢及び方
   位の初期化における取付誤差の推定をカルマンフィルタ
   を用いて行うように構成された慣性航法装置において、 該慣性航法装置及び上記航行体に装着された主慣性航法
   装置によって求められた角加速度の差を演算する際に遠
   心加速度補正を行い、その補正に使用する角加速度の算
   出にαβフィルタを用いることにより、ジャイロセンサ
   の量子化誤差の影響を低減することを特徴とする慣性航
   法装置。