JPH06265367A

JPH06265367A - 船舶の慣性航法装置

Info

Publication number: JPH06265367A
Application number: JP5245093A
Authority: JP
Inventors: Takashi Morimoto; 隆森本
Original assignee: YOKOGAWA NABITETSUKU KK
Current assignee: YOKOGAWA NABITETSUKU KK
Priority date: 1993-03-12
Filing date: 1993-03-12
Publication date: 1994-09-20

Abstract

(57)【要約】【目的】船舶の慣性航法装置における初期アライメン
トおよびキャリブレーションを高精度かつ短時間で行う
ことができるようにする。【構成】速度比較値が零である初期アライメントおよ
びキャリブレーション時に、時系列に得られた加速度測
定データからそのデータに含まれる動揺外乱加速度を推
定し、その推定値により加速度測定データを補正し、そ
れを積分してカルマンフィルタの入力とする。【効果】ジャイロ誤差およひ加速度計誤差による速度
誤差を短時間に抽出でき、初期姿勢および方位を高精度
かつ極めて短い時間内に決定できる。

Description

【発明の詳細な説明】

【０００１】

【産業上の利用分野】本発明は船舶の姿勢、方位、速度
および位置の検出に利用する。特に、これらの検出値を
出力するストラップダウン方式の慣性航法装置に関す
る。さらに詳しくは、このような慣性航法装置の初期姿
勢および方位の決定に関する。

【０００２】

【従来の技術】図２は従来の慣性航法装置の一例を示す
ブロック構成図である。この慣性航法装置は、ＩＭＵ部
１１と、入出力回路１２、加速度計誤差補正計算部２
１、ジャイロ誤差補正計算部２２、加速度座標変換計算
部２３、座標変換行列計算部２４、修正量計算部２５、
速度位置計算部２６、姿勢方位計算部２７、減算回路２
８、２９およびカルマンフィルタ部３０からなるコンピ
ュータ部と、入出力回路４１と、入出力データ表示装置
４２とを備える。カルマンフィルタ部３０は、出力誤差
推定計算部３１とＩＭＵ誤差推定計算部３２とを備え
る。

【０００３】ここで、この従来例の動作を説明する前
に、この装置で用いる座標について図３を参照して説明
する。慣性航法装置で用いられる座標には、船体座標と
航法座標（または航法計算座標）とがある。船体座標
は、ロール、ピッチおよびヨーの各軸成分により、（ｂ_x，ｂ_y，ｂ_z）と表される座標であり、船体中心を原点とする。これに
対して航法座標はｎ座標とも呼ばれ、北、東および高さ
方向の各成分により、（ｘ_N，ｙ_E，ｚ_D）と表される座標である。航法座標もまた船体中心を原点
とするが、その船上の点の座標は船の移動に伴って移動
する。ロール軸のまわりの回転角をロール角φ、ピッチ
軸まわりの回転角をピッチ角θ、船の向きを方位角ψと
いう。方位角ψは北向きのときψ＝０、東向きのときψ
＝９０°である。

【０００４】次に、従来例の各部の動作について、表
１、表２に示す記号を用いて説明する。

【表１】

【表２】

【０００５】ＩＭＵ部１１は加速度計とジャイロとによ
り構成され、加速度計は船の運動加速度を計測して加速
度ベクトルを出力し、ジャイロは船の運動角速度を計測
して回転角速度ベクトルを出力する。ストラップダウン
方式では、このＩＭＵ部１１が直接に船体基準軸に取り
つけられる。取り付け位置は可能な限り船体重心の近く
に設定される。

【０００６】加速度計の出力する加速度ベクトルは、ロ
ール軸、ピッチ軸およびヨー軸の各方向の加速度Ａ_bx、
Ａ_bxおよびＡ_bzを要素とする船体座標で表現され、船の
推進加速度ベクトル、加速度誤差ベクトルおよび動揺加
速度ベクトルに対して次の関係がある。

【数１】右辺第３項の動揺加速度ベクトルは初期アライメント中
に外乱加速度となり、姿勢決定時間および決定精度に大
きな影響を及ぼす。初期アライメントとは、加速度計出
力の水平成分を用いて初期の姿勢および方位角をできる
だけ正確に決定することをいう。この初期アライメント
のときに、ジャイロおよび加速度計のそれぞれの誤差の
一部についても補正する。これをキャリブレーションと
いう。

【０００７】ジャイロの出力する回転角速度ベクトルも
また船体座標で表現され、船体軸ロール角速度Ｗ_bx、船
体軸ピッチ角速度Ｗ_by、船体軸ヨー角速度Ｗ_bzを要素と
する。回転角速度ベクトルは旋回角速度ベクトル、ジャ
イロ誤差ベクトルおよび動揺角速度ベクトルに対して次
の関係がある。

【数２】

【０００８】加速度計およびジャイロの出力は、入出力
回路１２を介して、それぞれ加速度計誤差補正計算部２
１およびジャイロ誤差補正計算部２２に入力される。こ
れらの計算部２１、２２は、各誤差の一部を次の補正式
により補正する。

【数３】この二つの数式において、右辺第１項は船の推進加速度
ベクトルおよび旋回角度ベクトルであり、次の式で表さ
れる。

【数４】また、右辺第２項は船の動揺加速度ベクトルおよび角速
度ベクトルであり、次の式で表される。

【数５】そして、右辺第３項は加速度計誤差ベクトルおよびジャ
イロ誤差ベクトルであり、次の式で表される。

【数６】右辺最終項はＩＭＵ誤差推定計算部３２から出力された
加速度計誤差推定出力およびジャイロ誤差推定出力であ
り、次の式で表されるベクトルである。

【数７】

【０００９】加速度座標変換計算部２３は、加速度計誤
差補正計算部２１の出力を船体座標から航法座標である
ｎ座標へ次式により変換する。

【数８】

【００１０】座標変換行列計算部２４は、加速度座標変
換計算部２３での座標変換に使用する座標変換行列を次
式により計算する。

【数９】座標変換行列は次のように表される。

【数１０】また、数９の式において、ｄ／ｄｔは微分を表し、右辺
のオーバーラインで示した記号は船体座標のｎ座標に対
する相対回転角速度ベクトルの歪対称行列表現である。
相対回転角速度ベクトルは次の式により求められる。

【数１１】したがって、その歪対称行列表現は次のようになる。

【数１２】

【００１１】数１１の第１式において、右辺の最初の４
つの記号で表される項はジャイロの出力である。よく知
られているように、ジャイロは船の運動角速度（旋回角
速度＋動揺角速度）のみならず、絶対静止座標に対する
すべての角速度を計測する。具体的には、地球の自転角
速度、船が丸い地球の周辺を移動する移動角速度すなわ
ち緯度および経度の変化率、などについても計測する。
しかし、航法計算や船の姿勢および方位は地球に対する
運動と姿勢角および方位角とから表現されるので、ジャ
イロ出力から地球の自転角速度と移動角速度とを差し引
いて補正する必要がある。この補正が行われたものが数
１１の第１式である。この式において、最終項はジャイ
ロ誤差や加速度計誤差による姿勢角度誤差および方位角
誤差の修正量であり、次の式で表される。

【数１３】右辺は修正量計算部２５により計算される。また、減算
項は地球の自転角速度と船の移動角速度とに関する量で
あり、速度位置計算部２６により、次のように計算され
る。

【数１４】

【００１２】速度位置計算部２６はまた、数１４に示し
た式における速度および緯度経度について、加速度座標
変換計算部２３の出力に基づいて以下の計算により求め
る。

【数１５】船の場合には高度方向は関係ないが、水平方向と相関が
あるため、計算だけは次のように実行する。

【数１６】

【００１３】数１６の二つの式におけるそれぞれ最終項
は高度方向の誤差の水平方向への影響をできるだけ小さ
くするための高度方向誤差修正量であり、修正量計算部
２５により次のように計算される。

【数１７】修正量計算部２５はまた、水平方向速度誤差の修正量、
すなわち数１５の第１式における最終項についても計算
する。

【００１４】このような計算により、速度位置計算部２
６の出力には、数１５の第２式で表される速度ベクトル
と、同じく第３式および第４式で表される位置ベクトル
とが得られる。位置ベクトルは次のように表される。

【数１８】得られた速度ベクトルおよび位置ベクトルは、入出力回
路４１を介して入出力データ表示装置４２に出力され
る。

【００１５】なお、数１５では、次の式で表される重力
加速度ベクトルのｎ座標表現を用いた。

【数１９】また、数１４で用いた緯度方向および経度方向の地球半
径は、赤道面での地球半径ａを用いて次のように表され
る。

【数２０】さらに、数１５では次の式で行列表現される値を用い
た。

【数２１】

【００１６】一方、姿勢方位計算部２７は、座標変換行
列計算部２４の出力した座標変換行列から姿勢角（φ，
θ）と方位角ψとを計算し、入出力回路４１を介して入
出力データ表示装置４２に出力する。姿勢角および方位
角は、座標変換行列の要素を用いて、次のように計算さ
れる。

【数２２】

【００１７】速度位置計算部２６により得られた速度ベ
クトルおよび位置ベクトルは、コンピュータ部から出力
されるだけでなく、コンピュータ部内の減算回路２８、
２９に入力される。減算回路２８、２９にはさらに、入
出力回路４１を介してそれぞれ外部基準速度および外部
基準位置が入力される。減算回路２８、２９は、これら
を比較し、その差を出力誤差推定計算部３１およびＩＭ
Ｕ誤差推定計算部３２の双方に入力する。

【００１８】初期アライメント中は、次式で表される外
部基準速度および外部基準位置が入力される。

【数２３】また、初期アライメント中は、数１５の第１式が次式と
なる。

【数２４】

【００１９】数２４における水平方向速度誤差の修正量
は、修正量計算部７において、次の式により計算され
る。

【数２５】すなわち、数２５の第２式の右辺により修正量ベクトル
の二つの要素を求め、数１７により求めたもう一つの要
素を組み合わせて水平方向速度誤差の修正量を得る。

【００２０】数１４のＵ_λ、Ｕ_Λの式および数２５の第
２式において、

【数２６】であり、

【数２７】である。

【００２１】また、数２５第２式の係数は２行１４列の
行列で表される最適制御ゲインであり、次の評価関数を
最小にするように決められる。

【数２８】このＪを最小にするということは、区間〔０≦ｔ≦Ｔ〕
で使われる修正量と、そのときの誤差の累積とを最小に
することを意味する。すなわち、エネルギ最小化制御で
ある。

【００２２】数２７に示した二つの行列は、速度誤差、
姿勢角誤差、方位角誤差およびそれらの累積値の推定値
であり、出力誤差推定計算部３１から修正量計算部２５
に供給される。これらの二つの行列には次の関係があ
る。

【数２９】

【００２３】修正量計算部２５はまた、次式により姿勢
角誤差および方位角誤差の修正量を求める。

【数３０】右辺３番目の記号は数２６に示したものと同一であり、
２番目の記号は数２８の評価関数を最小にするように決
められた最適制御ゲインを表す。この場合の最適制御ゲ
インは３行１４列の行列で表される。

【００２４】カルマンフィルタ部３０は、出力誤差推定
計算部３１により速度誤差、位置誤差、姿勢角誤差、方
位角誤差を推定し、ＩＭＵ誤差推定計算部３２によりジ
ャイロ誤差、加速度誤差を推定する。この推定のための
モデルは次式で与えられる。

【数３１】

【００２５】カルマンフィルタ部３０はさらに、外部基
準速度誤差および潮流の速度について次のようにモデル
化して推定する。

【数３２】

【００２６】カルマンフィルタ部３０の動作は次式で与
えられる。

【数３３】

【００２７】カルマンフィルタ部３０に入力される観測
データは、初期アライメントと航行中のアライメント時
とで異なる。また、航行中のアライメント時でも、外部
基準速度観測時、外部基準位置観測時、あるいは外部基
準速度観測と外部基準位置観測との併用時で異なってく
る。

【００２８】外部基準速度観測時の入力観測データは次
のようなる。

【数３４】したがって、次式となる。

【数３５】

【００２９】外部基準位置観測時には次のようになる。

【数３６】したがって、次式となる。

【数３７】初期アライメント時には観測データが速度データのみと
なるので、外部基準位置観測は行わない。

【００３０】外部基準速度観測と外部基準位置観測との
併用時の入力観測データは次のようになる。

【数３８】初期アライメント時には、この併用観測は行わない。す
なわち、初期アライメント時は速度観測のみとなる。

【００３１】

【発明が解決しようとする課題】このような従来のスト
ラップダウン方式慣性航法装置では、初期アライメント
およびキャリブレーション時に、カルマンフィルタ入力
となる観測データが速度データ（Ｖ_N，Ｖ_E）のみとな
り、しかも外部基準速度Ｖ_Nr、Ｖ_Erは零である。船は停
止しているので

【数３９】であり、動揺速度Ｖ_dN、Ｖ_dEがあるだけである。すなわ
ち、

【数４０】となる。

【００３２】ここで、初期アライメントおよびキャリブ
レーションの目的は、ジャイロ誤差および加速度計誤差
による姿勢誤差および方位誤差を推定し、これらの誤差
の修正量を計算して座標変換行列計算部の入力とするこ
とにある。

【００３３】姿勢誤差および方位誤差を推定するために
は、当然、その原因となるジャイロ誤差および加速度計
誤差についても推定することになる。観測データとして
姿勢誤差および方位誤差が直接測定できるならこの推定
は簡単であるが、実際には、数４０に示したように速度
データが観測データとなる。すなわち、ジャイロ誤差お
よび加速度計誤差による速度誤差ΔＶ_N、ΔＶ_Eを正し
く測定できるわけではなく、動揺外乱としての動揺速度
Ｖ_dN、Ｖ_dEが余計に測定されてしまう。これらは速度誤
差ΔＶ_N、ΔＶ_Eの信号に対してノイズ外乱となり、速
度誤差ΔＶ_N、ΔＶ_E、姿勢角誤差、方位角誤差、加速
度計誤差およびジャイロ誤差のそれぞれの推定精度を低
下させる要因とする。また、推定完了までに要する時間
（これを「整定時間」という）を長くする要因ともな
る。

【００３４】この整定時間について説明すると、従来の
ストラップダウン方式慣性航法装置では、動揺外乱の影
響をできるだけ小さく抑えるために約４時間を費やして
いる。これは、船が緊急時に出航するときに大きな問題
となる。

【００３５】ここで、姿勢角誤差および方位角誤差と整
定時間との関係について説明する。

【００３６】整定時間後の姿勢角誤差および方位角誤差
は次式で表される。

【数４１】ここで、

【数４２】であり、

【数４３】である。

【００３７】例えばε_{N s.s}およびε_{E s.s}の精度仕様
を最大０．０２°、ε_{D s.s}の精度仕様を０．０７°と
する。この仕様を満足する値を数４１から逆算すると、

【数４４】となるようなローパスフィルタを設ける必要がある。

【００３８】ここで、ローパスフルタの前段における外
乱その他の振幅と周期を次のように設定する。

【数４５】ここに示した数値は船で推定される簡易化したモデルの
場合のデータである。

【００３９】したがって、上述した仕様を満足するに
は、１ｇを３×１０^-4ｇまでおとすローパスフィルタ
と、１°／ｓｅｃを０．０１°／ｈまでおとすローパス
フィルタとを設定する必要がある。ここで、Ｇ（ｓ）を
次のような一次遅れ系のローパスフィルタとする。

【数４６】ｓ＝ｊωを代入してゲイン｜Ｇ（ｊω）｜を計算する
と、

【数４７】となる。ここで、Ｔ_cω≫１のようにＴ_cを設定する
と、

【数４８】となる。ω＝ω_dN＝ω_dE＝ｆ_dE＝０．６ｒａｄ／ｓｅｃ
とすると、

【数４９】となる。ここで、Ｔ_c＝５５５５ｓｅｃとすると、

【数５０】となる。振幅が１ｇの外乱は、この時定数のフィルタを
通ると、１／３３３３、すなわち３×１０^-4ｇとなり、
数４４の第１式および第２式を満足する。しかし、フィ
ルタの時定数が５５５５秒であり、整定するのに約６時
間もかかることになる。

【００４０】ここでは仮定したデータと単純な一次のロ
ーパスフィルタを用いた場合について説明したが、実用
的には、より短い時間で初期アライメントおよびキャリ
ブレーションを行うことができる。しかし、それでも約
４時間程度の時間が必要となる。

【００４１】本発明は、このような課題を解決し、初期
アライメントおよびキャリブレーションを高精度かつ短
時間に行うことのできる船舶の慣性航法装置を提供する
ことを目的とする。

【００４２】

【課題を解決するための手段】本発明の船舶の慣性航法
装置は、船体基準軸に取り付けられた加速度計およびジ
ャイロからなるＩＭＵ部と、加速度計が出力した船体座
標における加速度を航法座標における加速度に変換する
第一の演算手段と、この航法座標における加速度を積分
してその船の速度および位置を求める第二の演算手段
と、ジャイロが出力した船の回転角速度から船体座標と
航法座標との間の変換規則を求める第三の演算手段と、
この第三の演算手段の出力によりその船の姿勢および方
位を求める第四の演算手段と、第二の演算手段により求
められた速度および位置と外部から入力された速度比較
値および位置比較値との差に基づいて第二および第三の
演算手段のそれぞれの演算の修正量を求める第一の補正
手段と、第二の演算手段により求められた速度および位
置と外部から入力された速度比較値および位置比較値と
の差に基づいて加速度計およびジャイロのそれぞれの出
力誤差を補正する第二の補正手段とを備えた船舶の慣性
航法装置において、速度比較値が零である初期設定時
に、第一の演算手段からの時系列の出力に基づいてその
出力に含まれる動揺外乱加速度を推定する第五の演算手
段と、この動揺外乱加速度の推定値により第一の演算手
段の出力を補正する第六の演算手段と、この第六の演算
手段の出力を積分し、初期設定時の速度誤差として第一
および第二の補正手段に供給する第七の演算手段とを備
えたことを特徴とする。

【００４３】

【作用】初期アライメントおよびキャリブレーションを
行うとき、加速度計の出力から航法座標上の加速度（北
向き成分Ａ_Nおよび東向き成分Ａ_E）を求め、時系列に
得られる加速度の測定データから動揺外乱加速度モデル
の係数パラメータを推定し、この係数パラメータを用い
てその測定データに含まれる動揺外乱加速度（北向き成
分Ａ_dNおよび東向き成分Ａ_dE）を推定する。この動揺外
乱加速度の推定値により測定データをフィードフォワー
ド補正する。これにより動揺外乱加速度成分を実質的に
含まない加速度データが得られる。これを積分し、カル
マンフィルタの入力とする。

【００４４】このようにして、初期アライメントおよび
キャリブレーション時に、ＳＮ比の良好な速度データが
カルマンフィルタの入力となり、ジャイロ誤差および加
速度計誤差による速度誤差（北向き成分ΔＶ_N、東向き
成分ΔＶ_E）を正確かつ短時間に抽出でき、初期姿勢お
よび方位を高い精度かつ極めて短い時間で決定すること
ができる。

【００４５】

【実施例】図１は本発明実施例の慣性航法装置を示すブ
ロック構成図である。この装置は、船体基準軸に取り付
けられた加速度計およびジャイロからなるＩＭＵ部１１
を備え、その出力が入出力回路１２を介してコンピュー
タ部に供給される。コンピュータ部は、ＩＭＵ部１１の
出力に基づいて船舶の速度、位置、姿勢および方位を求
め、入出力回路４１を介して入出力データ表示装置４２
に表示する。また、外部から入力された速度比較値およ
び位置比較値が入出力回路４１を介してコンピュータ部
に入力される。

【００４６】コンピュータ部には、加速度計が出力した
船体座標における加速度を航法座標における加速度に変
換する第一の演算手段として加速度座標変換計算部２３
を備え、この航法座標における加速度を積分してその船
の速度および位置を求める第二の演算手段として速度位
置計算部２６を備え、ジャイロが出力した船の回転角速
度から船体座標と航法座標との間の変換規則を求める第
三の演算手段として座標変換行列計算部２４を備え、こ
の座標変換行列計算部２４の出力によりその船の姿勢お
よび方位を求める第四の演算手段として姿勢方位計算部
２７を備え、速度位置計算部２６により求められた速度
および位置と外部から入力された速度比較値および位置
比較値との差に基づいて速度位置計算部２６および座標
変換行列計算部２４のそれぞれの演算の修正量を求める
第一の補正手段として出力誤差推定計算部３１および修
正量計算部２５を備え、速度位置計算部２６により求め
られた速度および位置と外部から入力された速度比較値
および位置比較値との差に基づいて加速度計およびジャ
イロのそれぞれの出力誤差を補正する第二の補正手段と
してＩＭＵ誤差推定計算部３２、加速度計誤差補正計算
部２１およびジャイロ誤差補正計算部２２を備える。ま
た、コンピュータ部には、速度位置計算部２６により求
められた速度および位置と外部から入力された速度比較
値および位置比較値との差を求めるため、それぞれ減算
回路２８、２９を備える。出力誤差推定計算部３１およ
びＩＭＵ誤差推定計算部３２はカルマンフィルタ部３０
を構成する。

【００４７】ここで本実施例の特徴とするところは、コ
ンピュータ部にさらに、速度比較値が零である初期設定
時に、加速度座標変換計算部２３からの時系列の出力に
基づいてその出力に含まれる動揺外乱加速度を推定する
第五の演算手段として動揺外乱加速度同定計算部３３を
備え、この動揺外乱加速度の推定値により加速度座標変
換計算部２３の出力を補正する第六の演算手段としてフ
ィードフォワード補正計算部３４を備え、このフィード
フォワード補正計算部３４の出力を積分し、その積分値
を初期設定時の速度誤差として出力誤差推定計算部３１
およびＩＭＵ誤差推定計算部３２にそれぞれ供給する第
七の演算手段として速度計算部３５を備えたことにあ
る。

【００４８】本実施例の動作について、図２に示した従
来例と異なる動作のみを説明する。本実施例が従来例と
異なるのは初期アライメントおよびキャリブレーション
中の動作のみであり、動揺外乱加速度同定計算部３３、
フィードフォワード補正計算部３４および速度計算部３
５はこのときだけ動作する。

【００４９】図２に示した従来例では、数４０に示した
ように、ジャイロおよび加速度計の誤差による速度誤差
ΔＶ_N、ΔＶ_E（北向き成分、東向き成分）を観測デー
タとしての速度Ｖ_N、Ｖ_Eから取り出しているため、動
揺外乱による動揺速度Ｖ_dN、Ｖ_dE分だけＳＮ比が低下し
てしまう。

【００５０】速度は、次式に示すように、加速度計によ
り得られた加速度ベクトルを船体座標から航法座標に座
標変換して積分することにより求められる。

【数５１】初期アライメント中なので、数５１における第二式に
は、船の移動速度によるコリオリの項は含まれない。

【００５１】数５１の第二式から、観測データとしての
速度Ｖ_N、Ｖ_Eが次のように求められる。

【数５２】

【００５２】初期アライメント中であることから、

【数５３】とすると、

【数５４】となる。数５４において速度Ｖ_N、Ｖ_Eが観測データで
あるが、本実施例では、動揺速度Ｖ_dN、Ｖ_dEの値を可能
な限り小さくするため、航法座標における動揺加速度ベ
クトルを推定し、フィードフォワード補正する。この推
定は動揺外乱加速度同定計算部３３で行われる。

【００５３】動揺加速度ベクトルは、

【数５５】で表され、

【数５６】である。ここで、初期アライメントおよびキャリブレー
ション中は、ＩＭＵ部１１、入出力回路１２、座標変換
行列計算部２４、加速度座標変換計算部２３、動揺外乱
加速度同定計算部３３の順序で処理が行われ、外乱加速
度同定中はこのサイクルで動作し続ける。この間は

【数５７】となり、数５６は次式のように簡略化される。

【数５８】

【００５４】数５８より、加速度Ａ_N、Ａ_Eは、

【数５９】となる。

【００５５】加速度誤差ΔＡ_N、ΔＡ_Eの大きさは１０
^-5ｇ〜１０^-4ｇ程度であり、動揺加速度Ａ_dN、Ａ_dNの大
きさは０．１ｇ〜１ｇ程度である。したがって、

【数６０】となる。これらの加速度成分が動揺外乱加速度同定計算
部３３の入力となる。

【００５６】動揺外乱加速度同定計算部３３では、加速
度Ａ_N、Ａ_Eの測定データにより動揺加速度Ａ_dN、Ａ_dN
を推定する。

【００５７】この推定について説明する。今、加速度Ａ
_N、Ａ_Eの測定データを次のように定義する。

【数６１】そして、この測定データを次式のようなＡＲＭＡ（自己
回帰移動平均）モデルで近似する。

【数６２】この式の示す意味は、現在の二つの測定データ、すなわ
ち北向き成分のデータと東向き成分のデータとが、現在
からｎステップ前までの北向き成分のデータの重み付け
和と、東向き成分のデータの重み付け和とで表現できる
ことを示している。このときの重み係数は未知の定数で
あり、動揺外乱加速度同定計算部３３では、この重み係
数を時系列の測定データから推定する。

【００５８】ここで、数６２を次のように書き換える。

【数６３】

【００５９】数６３では、求めるべき２ｎ個の未知パラ
メータ（重み係数）に対して、式が二つしかない。そこ
で、式を増やすため、Ｒ＝Ｒ₁、Ｒ₂、…Ｒ_mとして各
々に対して数６３を適用し、

【数６４】とする。これをまとめて次のように書き換える。

【数６５】数６５の重み係数ベクトルを推定すれば、その推定値を
用いて加速度についての測定データの推定値を求めるこ
とができ、動揺加速度Ａ_dN、Ａ_dNの推定値を求めること
ができる。

【００６０】推定の方法には最小２乗法、カルマンフィ
ルタ法、最尤推定法などがあるが、ここではバッチ処理
タイプの最小２乗法を用いた場合について説明する。こ
の場合には、重み係数ベクトルの推定値が、

【数６６】により求められる。左辺の記号は、時刻ｔ_R1〜ｔ_Rm間の
測定データにより求めた推定値を表す。この推定値によ
り、動揺加速度Ａ_dN、Ａ_dNのそれぞれの推定値が、

【数６７】で近似的に推定される。この推定値がフィードフォワー
ド補正計算部３４の入力となる。

【００６１】フィードフォワード補正計算部３４は、次
式の計算により、入力された推定値で水平方向の加速度
をフィードフォワード補正する。

【数６８】補正された加速度データは速度計算部３５に入力され
る。

【００６２】速度計算部３５は、補正された加速度デー
タを次の式により計算し、カルマンフィルタ部３０の入
力を求める。

【数６９】

【００６３】ここで、推定が完全に行われれば、

【数７０】となり、

【数７１】となる。このとき、速度についての観測データｙ₁、ｙ
₂は、

【数７２】となり、速度誤差が、ジャイロ誤差と加速度計誤差とに
よるものだけとなる。ここで、

【数７３】である。

【００６４】したがって、カルマンフィルタ部３０で
は、動揺加速度Ａ_dN、Ａ_dEにまったく影響を受けずに加
速度計誤差およびジャイロ誤差、そして姿勢誤差、方位
誤差、および速度誤差を推定することができる。

【００６５】図４ないし図８に本実施例の動作フローを
示し、図９に方位角誤差整定の時間応答を本実施例と従
来例とについて示す。図９から、初期アライメントおよ
びキャリブレーション時間が従来の４時間から３０分以
下と大幅に短縮され、アライメントおよびキャリブレー
ションの精度も大幅に改善されることがわかる。

【００６６】

【発明の効果】以上説明したように、本発明の船舶の慣
性航法装置は、動揺外乱加速度を推定してフィードフォ
ワード補正を行うことにより、動揺外乱の影響を短時間
に除去することができる。このため、初期アライメント
およびキャリブレーションの時間を短縮でき、高精度に
姿勢および方位を決定できる効果がある。

【図面の簡単な説明】

【図１】本発明実施例の慣性航法装置を示すブロック構
成図。

【図２】従来の船舶用慣性航法装置の例を示すブロック
構成図。

【図３】慣性航法装置で用いられる座標を示す図。

【図４】実施例装置の動作フローの一部を示す図。

【図５】実施例装置の動作フローの一部を示す図。

【図６】実施例装置の動作フローの一部を示す図。

【図７】実施例装置の動作フローの一部を示す図。

【図８】実施例装置の動作フローの一部を示す図。

【図９】方位角誤差整定の時間応答を示す図。

【符号の説明】

１１ＩＭＵ部１２、４１入出力回路２１加速度計誤差補正計算部２２ジャイロ誤差補正計算部２３加速度座標変換計算部２４座標変換行列計算部２５修正量計算部２６速度位置計算部２７姿勢方位計算部２８、２９減算回路３０カルマンフィルタ部３１出力誤差推定計算部３２ＩＭＵ誤差推定計算部３３動揺外乱加速度同定計算部３４フィードフォワード補正計算部３５速度計算部４２入出力データ表示装置

Claims

【特許請求の範囲】

【請求項１】船体基準軸に取り付けられた加速度計お
よびジャイロからなるＩＭＵ部（１１）と、上記加速度計が出力した船体座標における加速度を航法
座標における加速度に変換する第一の演算手段（２３）
と、この航法座標における加速度を積分してその船の速度お
よび位置を求める第二の演算手段（２６）と、上記ジャイロが出力した船の回転角速度から船体座標と
航法座標との間の変換規則を求める第三の演算手段（２
４）と、この第三の演算手段の出力によりその船の姿勢および方
位を求める第四の演算手段（２７）と、上記第二の演算手段により求められた速度および位置と
外部から入力された速度比較値および位置比較値との差
に基づいて上記第二および上記第三の演算手段のそれぞ
れの演算の修正量を求める第一の補正手段（３１、２
５）と、上記第二の演算手段により求められた速度および位置と
外部から入力された速度比較値および位置比較値との差
に基づいて上記加速度計および上記ジャイロのそれぞれ
の出力誤差を補正する第二の補正手段（３２、２１、２
２）とを備えた船舶の慣性航法装置において、速度比較値が零である初期設定時に、上記第一の演算手
段からの時系列の出力に基づいてその出力に含まれる動
揺外乱加速度を推定する第五の演算手段（３３）と、この動揺外乱加速度の推定値により上記第一の演算手段
の出力を補正する第六の演算手段（３４）と、この第六の演算手段の出力を積分し、初期設定時の速度
誤差として上記第一および上記第二の補正手段にそれぞ
れ供給する第七の演算手段（３５）とを備えたことを特
徴とする船舶の慣性航法装置。