JPH11118507A - 船舶の慣性航法装置 - Google Patents
船舶の慣性航法装置Info
- Publication number
- JPH11118507A JPH11118507A JP27759097A JP27759097A JPH11118507A JP H11118507 A JPH11118507 A JP H11118507A JP 27759097 A JP27759097 A JP 27759097A JP 27759097 A JP27759097 A JP 27759097A JP H11118507 A JPH11118507 A JP H11118507A
- Authority
- JP
- Japan
- Prior art keywords
- error
- accelerometer
- gyro
- acceleration
- output
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
Landscapes
- Gyroscopes (AREA)
- Navigation (AREA)
Abstract
(57)【要約】
【課題】 慣性航法装置の加速度計およびジャイロの誤
差補正では、船体の運動パターンによっては誤差補正を
行なうことにより誤差を増長させる場合がある。 【解決手段】 船体の運動パターンを判定し、それに応
じて加速度計およびジャイロの誤差補正を禁止する。
差補正では、船体の運動パターンによっては誤差補正を
行なうことにより誤差を増長させる場合がある。 【解決手段】 船体の運動パターンを判定し、それに応
じて加速度計およびジャイロの誤差補正を禁止する。
Description
【０００１】
【発明の属する技術分野】本発明は船舶の姿勢、方位、
速度および位置の検出に利用する。特に、これらの検出
値を出力するストラップダウン方式の慣性航法装置に関
する。さらに詳しくは、このような慣性航法装置の加速
度計およびジャイロの誤差補正技術に関する。
速度および位置の検出に利用する。特に、これらの検出
値を出力するストラップダウン方式の慣性航法装置に関
する。さらに詳しくは、このような慣性航法装置の加速
度計およびジャイロの誤差補正技術に関する。
【０００２】
【従来の技術】従来の慣性航法装置を図６を参照して説
明する。図６は従来の慣性航法装置の一例を示すブロッ
ク構成図である。この慣性航法装置は、ＩＭＵ部１１
と、入出力回路１２、加速度計誤差補正計算部２１、ジ
ャイロ誤差補正計算部２２、加速度座標変換計算部２
３、座標変換行列計算部２４、修正量計算部２５、速度
位置計算部２６、姿勢方位計算部２７、減算回路２８、
２９およびカルマンフィルタ部３０からなるコンピュー
タ部と、入出力回路４１と、入出力データ表示装置４２
とを備える。カルマンフィルタ部３０は、出力誤差推定
計算部３１とＩＭＵ誤差推定計算部３２とを備える。
明する。図６は従来の慣性航法装置の一例を示すブロッ
ク構成図である。この慣性航法装置は、ＩＭＵ部１１
と、入出力回路１２、加速度計誤差補正計算部２１、ジ
ャイロ誤差補正計算部２２、加速度座標変換計算部２
３、座標変換行列計算部２４、修正量計算部２５、速度
位置計算部２６、姿勢方位計算部２７、減算回路２８、
２９およびカルマンフィルタ部３０からなるコンピュー
タ部と、入出力回路４１と、入出力データ表示装置４２
とを備える。カルマンフィルタ部３０は、出力誤差推定
計算部３１とＩＭＵ誤差推定計算部３２とを備える。
【０００３】ここで、この従来例の動作を説明する前
に、この装置で用いる座標について図７を参照して説明
する。図７は慣性航法装置で用いられる座標を示す図で
ある。慣性航法装置で用いられる座標には、船体座標と
航法座標（または航法計算座標）とがある。船体座標
は、ロール、ピッチおよびヨーの各軸成分により、 （ｂx ，ｂy ，ｂz ） と表される座標であり、船体中心を原点とする。これに
対して航法座標はｎ座標とも呼ばれ、北、東および高さ
方向の各成分により、 （ｘN ，ｙE ，ｚD ） と表される座標である。航法座標もまた船体中心を原点
とするが、その船上の点の座標は船の移動に伴って移動
する。ロール軸のまわりの回転角をロール角φ、ピッチ
軸まわりの回転角をピッチ角θ、船の向きを方位角ψと
いう。方位角ψは北向きのときψ＝０、東向きのときψ
＝９０°である。
に、この装置で用いる座標について図７を参照して説明
する。図７は慣性航法装置で用いられる座標を示す図で
ある。慣性航法装置で用いられる座標には、船体座標と
航法座標（または航法計算座標）とがある。船体座標
は、ロール、ピッチおよびヨーの各軸成分により、 （ｂx ，ｂy ，ｂz ） と表される座標であり、船体中心を原点とする。これに
対して航法座標はｎ座標とも呼ばれ、北、東および高さ
方向の各成分により、 （ｘN ，ｙE ，ｚD ） と表される座標である。航法座標もまた船体中心を原点
とするが、その船上の点の座標は船の移動に伴って移動
する。ロール軸のまわりの回転角をロール角φ、ピッチ
軸まわりの回転角をピッチ角θ、船の向きを方位角ψと
いう。方位角ψは北向きのときψ＝０、東向きのときψ
＝９０°である。
【０００４】次に、従来例の各部の動作について、表
１、表２に示す記号を用いて説明する。
１、表２に示す記号を用いて説明する。
【０００５】
【表１】
【０００６】
【表２】 ＩＭＵ部１１は加速度計とジャイロとにより構成され、
加速度計は船の運動加速度を計測して加速度ベクトルを
出力し、ジャイロは船の運動角速度を計測して回転角速
度ベクトルを出力する。ストラップダウン方式では、こ
のＩＭＵ部１１が直接に船体基準軸に取りつけられる。
取り付け位置は可能な限り船体重心の近くに設定され
る。
加速度計は船の運動加速度を計測して加速度ベクトルを
出力し、ジャイロは船の運動角速度を計測して回転角速
度ベクトルを出力する。ストラップダウン方式では、こ
のＩＭＵ部１１が直接に船体基準軸に取りつけられる。
取り付け位置は可能な限り船体重心の近くに設定され
る。
【０００７】加速度計の出力する加速度ベクトルは、ロ
ール軸、ピッチ軸およびヨー軸の各方向の加速度Ａbx、
ＡbxおよびＡbzを要素とする船体座標で表現され、船の
推進加速度ベクトル、加速度誤差ベクトルおよび動揺加
速度ベクトルに対して次の関係がある。
ール軸、ピッチ軸およびヨー軸の各方向の加速度Ａbx、
ＡbxおよびＡbzを要素とする船体座標で表現され、船の
推進加速度ベクトル、加速度誤差ベクトルおよび動揺加
速度ベクトルに対して次の関係がある。
【０００８】
【数１】 右辺第３項の動揺加速度ベクトルは初期アライメント中
に外乱加速度となり、姿勢決定時間および決定精度に大
きな影響を及ぼす。初期アライメントとは、加速度計出
力の水平成分を用いて初期の姿勢および方位角をできる
だけ正確に決定することをいう。この初期アライメント
のときに、ジャイロおよび加速度計のそれぞれの誤差の
一部についても補正する。これをキャリブレーションと
いう。
に外乱加速度となり、姿勢決定時間および決定精度に大
きな影響を及ぼす。初期アライメントとは、加速度計出
力の水平成分を用いて初期の姿勢および方位角をできる
だけ正確に決定することをいう。この初期アライメント
のときに、ジャイロおよび加速度計のそれぞれの誤差の
一部についても補正する。これをキャリブレーションと
いう。
【０００９】ジャイロの出力する回転角速度ベクトルも
また船体座標で表現され、船体軸ロール角速度Ｗbx、船
体軸ピッチ角速度Ｗby、船体軸ヨー角速度Ｗbzを要素と
する。回転角速度ベクトルは旋回角速度ベクトル、ジャ
イロ誤差ベクトルおよび動揺角速度ベクトルに対して次
の関係がある。
また船体座標で表現され、船体軸ロール角速度Ｗbx、船
体軸ピッチ角速度Ｗby、船体軸ヨー角速度Ｗbzを要素と
する。回転角速度ベクトルは旋回角速度ベクトル、ジャ
イロ誤差ベクトルおよび動揺角速度ベクトルに対して次
の関係がある。
【００１０】
【数２】 加速度計およびジャイロの出力は、入出力回路１２を介
して、それぞれ加速度計誤差補正計算部２１およびジャ
イロ誤差補正計算部２２に入力される。これらの計算部
２１、２２は、各誤差の一部を次の補正式により補正す
る。
して、それぞれ加速度計誤差補正計算部２１およびジャ
イロ誤差補正計算部２２に入力される。これらの計算部
２１、２２は、各誤差の一部を次の補正式により補正す
る。
【００１１】
【数３】 この二つの数式において、右辺第１項は船の推進加速度
ベクトルおよび旋回角度ベクトルであり、次の式で表さ
れる。
ベクトルおよび旋回角度ベクトルであり、次の式で表さ
れる。
【００１２】
【数４】 また、右辺第２項は船の動揺加速度ベクトルおよび角速
度ベクトルであり、次の式で表される。
度ベクトルであり、次の式で表される。
【００１３】
【数５】 そして、右辺第３項は加速度計誤差ベクトルおよびジャ
イロ誤差ベクトルであり、次の式で表される。
イロ誤差ベクトルであり、次の式で表される。
【００１４】
【数６】 右辺最終項はＩＭＵ誤差推定計算部３２から出力された
加速度計誤差推定出力およびジャイロ誤差推定出力であ
り、次の式で表されるベクトルである。
加速度計誤差推定出力およびジャイロ誤差推定出力であ
り、次の式で表されるベクトルである。
【００１５】
【数７】 加速度座標変換計算部２３は、加速度計誤差補正計算部
２１の出力を船体座標から航法座標であるｎ座標へ次式
により変換する。
２１の出力を船体座標から航法座標であるｎ座標へ次式
により変換する。
【００１６】
【数８】 座標変換行列計算部２４は、加速度座標変換計算部２３
での座標変換に使用する座標変換行列を次式により計算
する。
での座標変換に使用する座標変換行列を次式により計算
する。
【００１７】
【数９】 座標変換行列は次のように表される。
【００１８】
【数１０】 また、数９の式において、ｄ／ｄｔは微分を表し、右辺
のオーバーラインで示した記号は船体座標のｎ座標に対
する相対回転角速度ベクトルの歪対称行列表現である。
相対回転角速度ベクトルは次の式により求められる。
のオーバーラインで示した記号は船体座標のｎ座標に対
する相対回転角速度ベクトルの歪対称行列表現である。
相対回転角速度ベクトルは次の式により求められる。
【００１９】
【数１１】 したがって、その歪対称行列表現は次のようになる。
【００２０】
【数１２】 数１１の第１式において、右辺の最初の４つの記号で表
される項はジャイロの出力である。よく知られているよ
うに、ジャイロは船の運動角速度（旋回角速度＋動揺角
速度）のみならず、絶対静止座標に対するすべての角速
度を計測する。具体的には、地球の自転角速度、船が丸
い地球の周辺を移動する移動角速度すなわち緯度および
経度の変化率、などについても計測する。しかし、航法
計算や船の姿勢および方位は地球に対する運動と姿勢角
および方位角とから表現されるので、ジャイロ出力から
地球の自転角速度と移動角速度とを差し引いて補正する
必要がある。この補正が行われたものが数１１の第１式
である。この式において、最終項はジャイロ誤差や加速
度計誤差による姿勢角度誤差および方位角誤差の修正量
であり、次の式で表される。
される項はジャイロの出力である。よく知られているよ
うに、ジャイロは船の運動角速度（旋回角速度＋動揺角
速度）のみならず、絶対静止座標に対するすべての角速
度を計測する。具体的には、地球の自転角速度、船が丸
い地球の周辺を移動する移動角速度すなわち緯度および
経度の変化率、などについても計測する。しかし、航法
計算や船の姿勢および方位は地球に対する運動と姿勢角
および方位角とから表現されるので、ジャイロ出力から
地球の自転角速度と移動角速度とを差し引いて補正する
必要がある。この補正が行われたものが数１１の第１式
である。この式において、最終項はジャイロ誤差や加速
度計誤差による姿勢角度誤差および方位角誤差の修正量
であり、次の式で表される。
【００２１】
【数１３】 右辺は修正量計算部２５により計算される。また、減算
項は地球の自転角速度と船の移動角速度とに関する量で
あり、速度位置計算部２６により、次のように計算され
る。
項は地球の自転角速度と船の移動角速度とに関する量で
あり、速度位置計算部２６により、次のように計算され
る。
【００２２】
【数１４】 速度位置計算部２６はまた、数１４に示した式における
速度および緯度経度について、加速度座標変換計算部２
３の出力に基づいて以下の計算により求める。
速度および緯度経度について、加速度座標変換計算部２
３の出力に基づいて以下の計算により求める。
【００２３】
【数１５】 船の場合には高度方向は関係ないが、水平方向と相関が
あるため、計算だけは次のように実行する。
あるため、計算だけは次のように実行する。
【００２４】
【数１６】 数１６の二つの式におけるそれぞれ最終項は高度方向の
誤差の水平方向への影響をできるだけ小さくするための
高度方向誤差修正量であり、修正量計算部２５により次
のように計算される。
誤差の水平方向への影響をできるだけ小さくするための
高度方向誤差修正量であり、修正量計算部２５により次
のように計算される。
【００２５】
【数１７】 修正量計算部２５はまた、水平方向速度誤差の修正量、
すなわち数１５の第１式における最終項についても計算
する。
すなわち数１５の第１式における最終項についても計算
する。
【００２６】このような計算により、速度位置計算部２
６の出力には、数１５の第２式で表される速度ベクトル
と、同じく第３式および第４式で表される位置ベクトル
とが得られる。位置ベクトルは次のように表される。
６の出力には、数１５の第２式で表される速度ベクトル
と、同じく第３式および第４式で表される位置ベクトル
とが得られる。位置ベクトルは次のように表される。
【００２７】
【数１８】 得られた速度ベクトルおよび位置ベクトルは、入出力回
路４１を介して入出力データ表示装置４２に出力され
る。
路４１を介して入出力データ表示装置４２に出力され
る。
【００２８】なお、数１５では、次の式で表される重力
加速度ベクトルのｎ座標表現を用いた。
加速度ベクトルのｎ座標表現を用いた。
【００２９】
【数１９】 また、数１４で用いた緯度方向および経度方向の地球半
径は、赤道面での地球半径ａを用いて次のように表され
る。
径は、赤道面での地球半径ａを用いて次のように表され
る。
【００３０】
【数２０】 さらに、数１５では次の式で行列表現される値を用い
た。
た。
【００３１】
【数２１】 一方、姿勢方位計算部２７は、座標変換行列計算部２４
の出力した座標変換行列から姿勢角（φ，θ）と方位角
ψとを計算し、入出力回路４１を介して入出力データ表
示装置４２に出力する。姿勢角および方位角は、座標変
換行列の要素を用いて、次のように計算される。
の出力した座標変換行列から姿勢角（φ，θ）と方位角
ψとを計算し、入出力回路４１を介して入出力データ表
示装置４２に出力する。姿勢角および方位角は、座標変
換行列の要素を用いて、次のように計算される。
【００３２】
【数２２】 速度位置計算部２６により得られた速度ベクトルおよび
位置ベクトルは、コンピュータ部から出力されるだけで
なく、コンピュータ部内の減算回路２８、２９に入力さ
れる。減算回路２８、２９にはさらに、入出力回路４１
を介してそれぞれ外部基準速度および外部基準位置が入
力される。減算回路２８、２９は、これらを比較し、そ
の差を出力誤差推定計算部３１およびＩＭＵ誤差推定計
算部３２の双方に入力する。
位置ベクトルは、コンピュータ部から出力されるだけで
なく、コンピュータ部内の減算回路２８、２９に入力さ
れる。減算回路２８、２９にはさらに、入出力回路４１
を介してそれぞれ外部基準速度および外部基準位置が入
力される。減算回路２８、２９は、これらを比較し、そ
の差を出力誤差推定計算部３１およびＩＭＵ誤差推定計
算部３２の双方に入力する。
【００３３】初期アライメント中は、次式で表される外
部基準速度および外部基準位置が入力される。
部基準速度および外部基準位置が入力される。
【００３４】
【数２３】 また、初期アライメント中は、数１５の第１式が次式と
なる。
なる。
【００３５】
【数２４】 数２４における水平方向速度誤差の修正量は、修正量計
算部２５において、次の式により計算される。
算部２５において、次の式により計算される。
【００３６】
【数２５】 すなわち、数２５の第２式の右辺により修正量ベクトル
の二つの要素を求め、数１７により求めたもう一つの要
素を組み合わせて水平方向速度誤差の修正量を得る。
の二つの要素を求め、数１７により求めたもう一つの要
素を組み合わせて水平方向速度誤差の修正量を得る。
【００３７】数１４のＵの式および数２５の第２式にお
いて、
いて、
【００３８】
【数２６】 であり、
【００３９】
【数２７】 である。
【００４０】また、数２５第２式の係数は２行１４列の
行列で表される最適制御ゲインであり、次の評価関数を
最小にするように決められる。
行列で表される最適制御ゲインであり、次の評価関数を
最小にするように決められる。
【００４１】
【数２８】 このＪを最小にするということは、区間〔０≦ｔ≦Ｔ〕
で使われる修正量と、そのときの誤差の累積とを最小に
することを意味する。すなわち、エネルギ最小化制御で
ある。
で使われる修正量と、そのときの誤差の累積とを最小に
することを意味する。すなわち、エネルギ最小化制御で
ある。
【００４２】数２７に示した二つの行列は、速度誤差、
姿勢角誤差、方位角誤差およびそれらの累積値の推定値
であり、出力誤差推定計算部３１から修正量計算部２５
に供給される。これらの二つの行列には次の関係があ
る。
姿勢角誤差、方位角誤差およびそれらの累積値の推定値
であり、出力誤差推定計算部３１から修正量計算部２５
に供給される。これらの二つの行列には次の関係があ
る。
【００４３】
【数２９】 修正量計算部２５はまた、次式により姿勢角誤差および
方位角誤差の修正量を求める。
方位角誤差の修正量を求める。
【００４４】
【数３０】 右辺３番目の記号は数２６に示したものと同一であり、
２番目の記号は数２８の評価関数を最小にするように決
められた最適制御ゲインを表す。この場合の最適制御ゲ
インは３行１４列の行列で表される。
２番目の記号は数２８の評価関数を最小にするように決
められた最適制御ゲインを表す。この場合の最適制御ゲ
インは３行１４列の行列で表される。
【００４５】カルマンフィルタ部３０は、出力誤差推定
計算部３１により速度誤差、位置誤差、姿勢角誤差、方
位角誤差を推定し、ＩＭＵ誤差推定計算部３２によりジ
ャイロ誤差、加速度誤差を推定する。この推定のための
モデルは次式で与えられる。
計算部３１により速度誤差、位置誤差、姿勢角誤差、方
位角誤差を推定し、ＩＭＵ誤差推定計算部３２によりジ
ャイロ誤差、加速度誤差を推定する。この推定のための
モデルは次式で与えられる。
【００４６】
【数３１】 カルマンフィルタ部３０はさらに、外部基準速度誤差お
よび潮流の速度について次のようにモデル化して推定す
る。
よび潮流の速度について次のようにモデル化して推定す
る。
【００４７】
【数３２】 カルマンフィルタ部３０の動作は次式で与えられる。
【００４８】
【数３３】 カルマンフィルタ部３０に入力される観測データは、初
期アライメントと航行中のアライメント時とで異なる。
また、航行中のアライメント時でも、外部基準速度観測
時、外部基準位置観測時、あるいは外部基準速度観測と
外部基準位置観測との併用時で異なってくる。
期アライメントと航行中のアライメント時とで異なる。
また、航行中のアライメント時でも、外部基準速度観測
時、外部基準位置観測時、あるいは外部基準速度観測と
外部基準位置観測との併用時で異なってくる。
【００４９】外部基準速度観測時の入力観測データは次
のようなる。
のようなる。
【００５０】
【数３４】 したがって、次式となる。
【００５１】
【数３５】 外部基準位置観測時には次のようになる。
【００５２】
【数３６】 したがって、次式となる。
【００５３】
【数３７】 初期アライメント時には観測データが速度データのみと
なるので、外部基準位置観測は行わない。
なるので、外部基準位置観測は行わない。
【００５４】外部基準速度観測と外部基準位置観測との
併用時の入力観測データは次のようになる。
併用時の入力観測データは次のようになる。
【００５５】
【数３８】 初期アライメント時には、この併用観測は行わない。す
なわち、初期アライメント時は速度観測のみとなる。
なわち、初期アライメント時は速度観測のみとなる。
【００５６】
【発明が解決しようとする課題】このような従来の慣性
航法装置では、カルマンフィルタ部３０のＩＭＵ誤差推
定計算部３２は、常時、ＩＭＵ部１１のジャイロおよび
加速度計の改善動作を行っている。しかし、船体の揺れ
が少なく、ジャイロおよび加速度計の改善動作を必要と
しない場合にも無条件に改善動作が行われることから、
本来の目的に反して逆にジャイロおよび加速度計の誤差
を増長させることがある。
航法装置では、カルマンフィルタ部３０のＩＭＵ誤差推
定計算部３２は、常時、ＩＭＵ部１１のジャイロおよび
加速度計の改善動作を行っている。しかし、船体の揺れ
が少なく、ジャイロおよび加速度計の改善動作を必要と
しない場合にも無条件に改善動作が行われることから、
本来の目的に反して逆にジャイロおよび加速度計の誤差
を増長させることがある。
【００５７】図８は従来の慣性航法装置におけるＩＭＵ
誤差の改善効果を示す図である。図８に示すように、第
１回目の改善後に行われた第２回目の改善では、改善を
行わないときの加速度計の誤差を上回る誤差が生じてい
る。さらに、第３回目の改善後に行われた第４回目の改
善では、加速度計の誤差が第３回目の改善後の誤差より
も増長されていることがわかる。
誤差の改善効果を示す図である。図８に示すように、第
１回目の改善後に行われた第２回目の改善では、改善を
行わないときの加速度計の誤差を上回る誤差が生じてい
る。さらに、第３回目の改善後に行われた第４回目の改
善では、加速度計の誤差が第３回目の改善後の誤差より
も増長されていることがわかる。
【００５８】この主な原因としては測定ノイズが挙げら
れる。例えば船体が直進中であれば加速度計およびジャ
イロの誤差は改善を行う必要はないレベルである。仮
に、この測定ノイズによる誤差は真値に対してプラス方
向およびマイナス方向に周期的に変動しているとする。
このときに、ＩＭＵ誤差推定計算部３２がこの誤差の改
善を行おうとすれば、例えばプラス方向に偏った誤差な
らばその誤差をマイナス方向に戻すように改善が行われ
る。しかし、改善が行われた時点で誤差がすでにマイナ
ス方向に変動していればマイナス方向の誤差は２倍に増
長される。
れる。例えば船体が直進中であれば加速度計およびジャ
イロの誤差は改善を行う必要はないレベルである。仮
に、この測定ノイズによる誤差は真値に対してプラス方
向およびマイナス方向に周期的に変動しているとする。
このときに、ＩＭＵ誤差推定計算部３２がこの誤差の改
善を行おうとすれば、例えばプラス方向に偏った誤差な
らばその誤差をマイナス方向に戻すように改善が行われ
る。しかし、改善が行われた時点で誤差がすでにマイナ
ス方向に変動していればマイナス方向の誤差は２倍に増
長される。
【００５９】本発明は、このような背景に行われたもの
であって、船体の運行パターンによらず加速度計および
ジャイロの誤差を低減させることができる船舶の慣性航
法装置を提供することを目的とする。
であって、船体の運行パターンによらず加速度計および
ジャイロの誤差を低減させることができる船舶の慣性航
法装置を提供することを目的とする。
【００６０】
【課題を解決するための手段】本発明は、船体の運行パ
ターンによっては、加速度計およびジャイロの誤差推定
を禁止することを最も主要な特徴とする。これにより、
不必要な誤差の改善が行われず、測定ノイズなどによる
誤差の増長を回避することができる。
ターンによっては、加速度計およびジャイロの誤差推定
を禁止することを最も主要な特徴とする。これにより、
不必要な誤差の改善が行われず、測定ノイズなどによる
誤差の増長を回避することができる。
【００６１】すなわち、本発明は、船体基準軸に取り付
けられた加速度計およびジャイロからなるＩＭＵ部（１
１）と、この加速度計が出力した船体座標における加速
度を航法座標における加速度に変換する第一の演算手段
（２３）と、この航法座標における加速度を積分してそ
の船の速度および位置を求める第二の演算手段（２６）
とを備えた船舶の慣性航法装置である。本発明の特徴と
するところは、前記第一の演算手段から出力される加速
度と前記第二の演算手段から出力される速度および位置
とにしたがってその船の運動パターンを推定する手段
と、この運動パターンの前記加速度計および前記ジャイ
ロの誤差に及ぼす影響度を演算する手段とを備えるとこ
ろにある。
けられた加速度計およびジャイロからなるＩＭＵ部（１
１）と、この加速度計が出力した船体座標における加速
度を航法座標における加速度に変換する第一の演算手段
（２３）と、この航法座標における加速度を積分してそ
の船の速度および位置を求める第二の演算手段（２６）
とを備えた船舶の慣性航法装置である。本発明の特徴と
するところは、前記第一の演算手段から出力される加速
度と前記第二の演算手段から出力される速度および位置
とにしたがってその船の運動パターンを推定する手段
と、この運動パターンの前記加速度計および前記ジャイ
ロの誤差に及ぼす影響度を演算する手段とを備えるとこ
ろにある。
【００６２】さらに詳細には、本発明は、船体基準軸に
取り付けられた加速度計およびジャイロからなるＩＭＵ
部（１１）と、この加速度計が出力した船体座標におけ
る加速度を航法座標における加速度に変換する第一の演
算手段（２３）と、この航法座標における加速度を積分
してその船の速度および位置を求める第二の演算手段
（２６）と、前記ジャイロが出力した船の回転角速度か
ら船体座標と航法座標との間の変換規則を求める第三の
演算手段（２４）と、この第三の演算手段の出力により
その船の姿勢および方位を求める第四の演算手段（２
７）と、前記第二の演算手段により求められた速度およ
び位置と外部から入力された速度比較値および位置比較
値との差に基づいて前記第二および前記第三の演算手段
のそれぞれの演算の修正量を求める第一の補正手段（３
１、２５）と、前記第二の演算手段により求められた速
度および位置と外部から入力された速度比較値および位
置比較値との差に基づいて前記加速度計および前記ジャ
イロのそれぞれの出力誤差を補正する第二の補正手段
（３２、２１、２２）とを備えた船舶の慣性航法装置で
ある。
取り付けられた加速度計およびジャイロからなるＩＭＵ
部（１１）と、この加速度計が出力した船体座標におけ
る加速度を航法座標における加速度に変換する第一の演
算手段（２３）と、この航法座標における加速度を積分
してその船の速度および位置を求める第二の演算手段
（２６）と、前記ジャイロが出力した船の回転角速度か
ら船体座標と航法座標との間の変換規則を求める第三の
演算手段（２４）と、この第三の演算手段の出力により
その船の姿勢および方位を求める第四の演算手段（２
７）と、前記第二の演算手段により求められた速度およ
び位置と外部から入力された速度比較値および位置比較
値との差に基づいて前記第二および前記第三の演算手段
のそれぞれの演算の修正量を求める第一の補正手段（３
１、２５）と、前記第二の演算手段により求められた速
度および位置と外部から入力された速度比較値および位
置比較値との差に基づいて前記加速度計および前記ジャ
イロのそれぞれの出力誤差を補正する第二の補正手段
（３２、２１、２２）とを備えた船舶の慣性航法装置で
ある。
【００６３】ここで、本発明の特徴とするところは、前
記第一の演算手段から出力される加速度と前記第二の演
算手段から出力される速度および位置とにしたがってそ
の船の運動パターンを推定する手段と、この運動パター
ンの前記加速度計および前記ジャイロの誤差に及ぼす影
響度を演算する手段とを備えるところにある。
記第一の演算手段から出力される加速度と前記第二の演
算手段から出力される速度および位置とにしたがってそ
の船の運動パターンを推定する手段と、この運動パター
ンの前記加速度計および前記ジャイロの誤差に及ぼす影
響度を演算する手段とを備えるところにある。
【００６４】このとき、前記影響度が所定値未満のとき
には、前記第二の補正手段の補正を禁止する手段を備え
ることが望ましい。
には、前記第二の補正手段の補正を禁止する手段を備え
ることが望ましい。
【００６５】
【発明の実施の形態】発明の実施の形態を図１を参照し
て説明する。図１は本発明実施例の慣性航法装置のブロ
ック構成図である。
て説明する。図１は本発明実施例の慣性航法装置のブロ
ック構成図である。
【００６６】本発明は、船体基準軸に取り付けられた加
速度計およびジャイロからなるＩＭＵ部１１と、この加
速度計が出力した船体座標における加速度を航法座標に
おける加速度に変換する第一の演算手段である加速度座
標変換計算部２３と、この航法座標における加速度を積
分してその船の速度および位置を求める第二の演算手段
である速度位置計算部２６と、前記ジャイロが出力した
船の回転角速度から船体座標と航法座標との間の変換規
則を求める第三の演算手段である座標変換行列計算部２
４と、この座標変換行列計算部２４の出力によりその船
の姿勢および方位を求める第四の演算手段である姿勢方
位計算部２７と、速度位置計算部２６により求められた
速度および位置と外部から入力された速度比較値および
位置比較値との差に基づいて速度位置計算部２６および
座標変換行列計算部２４のそれぞれの演算の修正量を求
める第一の補正手段である出力誤差推定計算部３１およ
び修正量計算部２５と、速度位置計算部２６により求め
られた速度および位置と外部から入力された速度比較値
および位置比較値との差に基づいて前記加速度計および
前記ジャイロのそれぞれの出力誤差を補正する第二の補
正手段であるＩＭＵ誤差推定計算部３２、加速度計誤差
補正計算部２１、ジャイロ誤差補正計算部２２とを備え
た船舶の慣性航法装置である。
速度計およびジャイロからなるＩＭＵ部１１と、この加
速度計が出力した船体座標における加速度を航法座標に
おける加速度に変換する第一の演算手段である加速度座
標変換計算部２３と、この航法座標における加速度を積
分してその船の速度および位置を求める第二の演算手段
である速度位置計算部２６と、前記ジャイロが出力した
船の回転角速度から船体座標と航法座標との間の変換規
則を求める第三の演算手段である座標変換行列計算部２
４と、この座標変換行列計算部２４の出力によりその船
の姿勢および方位を求める第四の演算手段である姿勢方
位計算部２７と、速度位置計算部２６により求められた
速度および位置と外部から入力された速度比較値および
位置比較値との差に基づいて速度位置計算部２６および
座標変換行列計算部２４のそれぞれの演算の修正量を求
める第一の補正手段である出力誤差推定計算部３１およ
び修正量計算部２５と、速度位置計算部２６により求め
られた速度および位置と外部から入力された速度比較値
および位置比較値との差に基づいて前記加速度計および
前記ジャイロのそれぞれの出力誤差を補正する第二の補
正手段であるＩＭＵ誤差推定計算部３２、加速度計誤差
補正計算部２１、ジャイロ誤差補正計算部２２とを備え
た船舶の慣性航法装置である。
【００６７】ここで、本発明の特徴とするところは、加
速度座標変換計算部２３から出力される加速度と速度位
置計算部２６から出力される速度および位置とにしたが
ってその船の運動パターンを推定し、この運動パターン
の前記加速度計および前記ジャイロの誤差に及ぼす影響
度を演算し、この影響度が所定値未満のときには、ＩＭ
Ｕ誤差推定計算部３２の補正を禁止する手段であるＩＭ
Ｕ性能改善判定部４３とを備えたところにある。
速度座標変換計算部２３から出力される加速度と速度位
置計算部２６から出力される速度および位置とにしたが
ってその船の運動パターンを推定し、この運動パターン
の前記加速度計および前記ジャイロの誤差に及ぼす影響
度を演算し、この影響度が所定値未満のときには、ＩＭ
Ｕ誤差推定計算部３２の補正を禁止する手段であるＩＭ
Ｕ性能改善判定部４３とを備えたところにある。
【００６８】
【実施例】本発明実施例を説明する。加速度計誤差ベク
トルΔＡb 、ジャイロ誤差ベクトルΔＷb の出力への影
響は船体の運行パターンによって大きくなる。したがっ
て、運行パターンを判定することができればＩＭＵ部１
１の誤差改善が不必要な場合にはこれを禁止することが
できる。
トルΔＡb 、ジャイロ誤差ベクトルΔＷb の出力への影
響は船体の運行パターンによって大きくなる。したがっ
て、運行パターンを判定することができればＩＭＵ部１
１の誤差改善が不必要な場合にはこれを禁止することが
できる。
【００６９】図２は船体の運動例を示す図である。図３
は船体運動および船種に依存する改善効果を示す図であ
る。図２に示すように船体が運動したときに、図３に示
すように、ＩＭＵ部１１のジャイロ誤差および加速度計
誤差に対する影響が生じる。図２に示すパターンＡは直
進状態であり、図３に示すように良好レベルの範囲内で
の影響が生じる。図２に示すパターンＢは半旋回状態で
あり、図３に示すように、良好レベルを超える影響が生
じる。図２に示すパターンＣは１回旋回状態であり、図
３に示すように、良好レベルを大きく超える影響が生じ
る。図２に示すパターンＤは相互逆１回旋回であり、図
３に示すように、良好レベルをきわめて大きく超える影
響が生じる。
は船体運動および船種に依存する改善効果を示す図であ
る。図２に示すように船体が運動したときに、図３に示
すように、ＩＭＵ部１１のジャイロ誤差および加速度計
誤差に対する影響が生じる。図２に示すパターンＡは直
進状態であり、図３に示すように良好レベルの範囲内で
の影響が生じる。図２に示すパターンＢは半旋回状態で
あり、図３に示すように、良好レベルを超える影響が生
じる。図２に示すパターンＣは１回旋回状態であり、図
３に示すように、良好レベルを大きく超える影響が生じ
る。図２に示すパターンＤは相互逆１回旋回であり、図
３に示すように、良好レベルをきわめて大きく超える影
響が生じる。
【００７０】すなわち、図３に示すように、各パターン
Ａ〜ＤにおけるＩＭＵ誤差推定計算部３２による改善効
果は、パターンＤが最も大きく、続いて、パターンＣ、
Ｂ、Ａの順に小さくなる。特に、パターンＡについては
改善を必要としない。
Ａ〜ＤにおけるＩＭＵ誤差推定計算部３２による改善効
果は、パターンＤが最も大きく、続いて、パターンＣ、
Ｂ、Ａの順に小さくなる。特に、パターンＡについては
改善を必要としない。
【００７１】したがって、ＩＭＵ性能改善判定部４３
は、図３に示した影響度を計算し、この影響度が良好レ
ベルを超えているときにはＩＭＵ誤差推定計算部３２に
よるジャイロ誤差および加速度計誤差の改善を行うこと
により、効果のある改善を行うことができる。逆に、こ
の良好度が良好レベルを超えていないときにはＩＭＵ誤
差推定計算部３２によるジャイロ誤差および加速度計誤
差の改善を禁止することにより、測定ノイズなどによる
改善後の誤差拡大を回避することができる。
は、図３に示した影響度を計算し、この影響度が良好レ
ベルを超えているときにはＩＭＵ誤差推定計算部３２に
よるジャイロ誤差および加速度計誤差の改善を行うこと
により、効果のある改善を行うことができる。逆に、こ
の良好度が良好レベルを超えていないときにはＩＭＵ誤
差推定計算部３２によるジャイロ誤差および加速度計誤
差の改善を禁止することにより、測定ノイズなどによる
改善後の誤差拡大を回避することができる。
【００７２】本発明実施例の動作を図４を参照して説明
する。図４は本発明実施例の動作を示すフローチャート
である。ＩＭＵ誤差推定計算部３２では、カルマンフィ
ルタにより各種誤差が推定される。このカルマンフィル
タは、
する。図４は本発明実施例の動作を示すフローチャート
である。ＩＭＵ誤差推定計算部３２では、カルマンフィ
ルタにより各種誤差が推定される。このカルマンフィル
タは、
【００７３】
【数３９】 の状態方程式により表される。本発明ではこの状態方程
式のＦ行列に着目し、運行パターンを判定する。
式のＦ行列に着目し、運行パターンを判定する。
【００７４】Ｆ行列は、加速度ベクトルＡb 、回転角速
度ベクトル〔外１〕、運動速度Ｖn、位置Ｐn により定
まる。ここで、Ｆ行列とＣ行列（座標変換行列）よりＭ
（ｔ）を常時計算する（Ｓ２、Ｓ３、Ｓ４）。初期値と
してはＭ＝０を入力する（Ｓ１）。
度ベクトル〔外１〕、運動速度Ｖn、位置Ｐn により定
まる。ここで、Ｆ行列とＣ行列（座標変換行列）よりＭ
（ｔ）を常時計算する（Ｓ２、Ｓ３、Ｓ４）。初期値と
してはＭ＝０を入力する（Ｓ１）。
【００７５】
【数４０】 このＭ（ｔ）が判定値Ｍｅ以上のときだけＩＭＵ誤差推
定計算部３２の推定計算を行う（Ｓ５）。すなわち、 Ｍ（ｔ）≧Ｍｅ のときにフラグＩを Ｉ＝１ とする（Ｓ６、Ｓ７）。ここで、 Ｉ＝０ であれば（Ｓ８）、ＩＭＵ誤差推定計算を禁止し、 Ｉ＝１ であれば（Ｓ８）、ＩＭＵ誤差推定計算を実行する（Ｓ
９、Ｓ１０）。このＩＭＵ誤差推定計算結果はそれぞれ
加速度計誤差補正計算部２１およびジャイロ誤差補正計
算部２２に与えられる（Ｓ１１）。
定計算部３２の推定計算を行う（Ｓ５）。すなわち、 Ｍ（ｔ）≧Ｍｅ のときにフラグＩを Ｉ＝１ とする（Ｓ６、Ｓ７）。ここで、 Ｉ＝０ であれば（Ｓ８）、ＩＭＵ誤差推定計算を禁止し、 Ｉ＝１ であれば（Ｓ８）、ＩＭＵ誤差推定計算を実行する（Ｓ
９、Ｓ１０）。このＩＭＵ誤差推定計算結果はそれぞれ
加速度計誤差補正計算部２１およびジャイロ誤差補正計
算部２２に与えられる（Ｓ１１）。
【００７６】本発明実施例の効果を図５を参照して説明
する。図５は本発明実施例の効果を示す図である。これ
により、パターンＢよりＣ、ＣよりＤという順で推定精
度は向上し、ジャイロ、加速度計の性能改善度も向上す
る。図５に示すように、改善効果は Ｍ（ｔ）≧Ｍｅ 時の推定と補正を繰り返す毎に向上する。すなわち、前
回実行時のＭを記憶しておき、それよりも大きいＭのと
きだけ推定を行う。
する。図５は本発明実施例の効果を示す図である。これ
により、パターンＢよりＣ、ＣよりＤという順で推定精
度は向上し、ジャイロ、加速度計の性能改善度も向上す
る。図５に示すように、改善効果は Ｍ（ｔ）≧Ｍｅ 時の推定と補正を繰り返す毎に向上する。すなわち、前
回実行時のＭを記憶しておき、それよりも大きいＭのと
きだけ推定を行う。
【００７７】
【発明の効果】以上説明したように、本発明によれば、
船体の運行パターンによらず加速度計およびジャイロの
誤差を低減させることができる。
船体の運行パターンによらず加速度計およびジャイロの
誤差を低減させることができる。
【図１】本発明実施例の慣性航法装置のブロック構成
図。
図。
【図２】船体の運動例を示す図。
【図３】船体運動および船種に依存する改善効果を示す
図。
図。
【図４】本発明実施例の動作を示すフローチャート。
【図５】本発明実施例の効果を示す図。
【図６】従来の慣性航法装置の一例を示すブロック構成
図。
図。
【図７】慣性航法装置で用いられる座標を示す図。
【図８】従来の慣性航法装置におけるＩＭＵ誤差の改善
効果を示す図。
効果を示す図。
１１ ＩＭＵ部 １２、４１ 入出力回路 ２１ 加速度計誤差補正計算部 ２２ ジャイロ誤差補正計算部 ２３ 加速度座標変換計算部 ２４ 座標変換行列計算部 ２５ 修正量計算部 ２６ 速度位置計算部 ２７ 姿勢方位計算部 ２８、２９ 減算回路 ３０ カルマンフィルタ部 ３１ 出力誤差推定計算部 ３２ ＩＭＵ誤差推定計算部 ３３ 動揺外乱加速度同定計算部 ３４ フィードフォワード補正計算部 ３５ 速度計算部 ４２ 入出力データ表示装置 ４３ ＩＭＵ性能改善判定部
Claims (3)
- 【請求項１】 船体基準軸に取り付けられた加速度計お
よびジャイロからなるＩＭＵ部（１１）と、 この加速度計が出力した船体座標における加速度を航法
座標における加速度に変換する第一の演算手段（２３）
と、 この航法座標における加速度を積分してその船の速度お
よび位置を求める第二の演算手段（２６）とを備えた船
舶の慣性航法装置において、 前記第一の演算手段から出力される加速度と前記第二の
演算手段から出力される速度および位置とにしたがって
その船の運動パターンを推定する手段と、 この運動パターンの前記加速度計および前記ジャイロの
誤差に及ぼす影響度を演算する手段とを備えたことを特
徴とする船舶の慣性航法装置。 - 【請求項２】 船体基準軸に取り付けられた加速度計お
よびジャイロからなるＩＭＵ部（１１）と、 この加速度計が出力した船体座標における加速度を航法
座標における加速度に変換する第一の演算手段（２３）
と、 この航法座標における加速度を積分してその船の速度お
よび位置を求める第二の演算手段（２６）と、 前記ジャイロが出力した船の回転角速度から船体座標と
航法座標との間の変換規則を求める第三の演算手段（２
４）と、 この第三の演算手段の出力によりその船の姿勢および方
位を求める第四の演算手段（２７）と、 前記第二の演算手段により求められた速度および位置と
外部から入力された速度比較値および位置比較値との差
に基づいて前記第二および前記第三の演算手段のそれぞ
れの演算の修正量を求める第一の補正手段（３１、２
５）と、 前記第二の演算手段により求められた速度および位置と
外部から入力された速度比較値および位置比較値との差
に基づいて前記加速度計および前記ジャイロのそれぞれ
の出力誤差を補正する第二の補正手段（３２、２１、２
２）とを備えた船舶の慣性航法装置において、 前記第一の演算手段から出力される加速度と前記第二の
演算手段から出力される速度および位置とにしたがって
その船の運動パターンを推定する手段と、 この運動パターンの前記加速度計および前記ジャイロの
誤差に及ぼす影響度を演算する手段とを備えたことを特
徴とする船舶の慣性航法装置。 - 【請求項３】 前記影響度が所定値未満のときには、前
記第二の補正手段の補正を禁止する手段を備えた請求項
２記載の船舶の慣性航法装置。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP27759097A JPH11118507A (ja) | 1997-10-09 | 1997-10-09 | 船舶の慣性航法装置 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP27759097A JPH11118507A (ja) | 1997-10-09 | 1997-10-09 | 船舶の慣性航法装置 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| JPH11118507A true JPH11118507A (ja) | 1999-04-30 |
Family
ID=17585583
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP27759097A Pending JPH11118507A (ja) | 1997-10-09 | 1997-10-09 | 船舶の慣性航法装置 |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JPH11118507A (ja) |
Cited By (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US6778924B2 (en) * | 2001-11-06 | 2004-08-17 | Honeywell International Inc. | Self-calibrating inertial measurement system method and apparatus |
| US7474197B2 (en) | 2004-03-26 | 2009-01-06 | Samsung Electronics Co., Ltd. | Audio generating method and apparatus based on motion |
| JP2009276242A (ja) * | 2008-05-15 | 2009-11-26 | Fujitsu Ltd | 角速度検出装置及び角速度検出方法 |
| CN103954282A (zh) * | 2014-03-04 | 2014-07-30 | 哈尔滨工程大学 | 基于加速度计输出增量的捷联惯性导航方法 |
| CN113092822A (zh) * | 2021-04-15 | 2021-07-09 | 中国人民解放军国防科技大学 | 一种基于惯组的激光多普勒测速仪的在线标定方法和装置 |
| CN113624256A (zh) * | 2021-08-06 | 2021-11-09 | 中国人民解放军63691部队 | 船载天线前馈陀螺在线性能分析方法及系统 |
-
1997
- 1997-10-09 JP JP27759097A patent/JPH11118507A/ja active Pending
Cited By (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US6778924B2 (en) * | 2001-11-06 | 2004-08-17 | Honeywell International Inc. | Self-calibrating inertial measurement system method and apparatus |
| US6968281B2 (en) | 2001-11-06 | 2005-11-22 | Honeywell International, Inc. | Method for calibrating an inertial measurement unit |
| US7474197B2 (en) | 2004-03-26 | 2009-01-06 | Samsung Electronics Co., Ltd. | Audio generating method and apparatus based on motion |
| JP2009276242A (ja) * | 2008-05-15 | 2009-11-26 | Fujitsu Ltd | 角速度検出装置及び角速度検出方法 |
| CN103954282A (zh) * | 2014-03-04 | 2014-07-30 | 哈尔滨工程大学 | 基于加速度计输出增量的捷联惯性导航方法 |
| CN113092822A (zh) * | 2021-04-15 | 2021-07-09 | 中国人民解放军国防科技大学 | 一种基于惯组的激光多普勒测速仪的在线标定方法和装置 |
| CN113092822B (zh) * | 2021-04-15 | 2023-11-10 | 中国人民解放军国防科技大学 | 一种基于惯组的激光多普勒测速仪的在线标定方法和装置 |
| CN113624256A (zh) * | 2021-08-06 | 2021-11-09 | 中国人民解放军63691部队 | 船载天线前馈陀螺在线性能分析方法及系统 |
| CN113624256B (zh) * | 2021-08-06 | 2023-08-25 | 中国人民解放军63691部队 | 船载天线前馈陀螺在线性能分析方法及系统 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN108827310B (zh) | 一种船用星敏感器辅助陀螺仪在线标定方法 | |
| JP4199553B2 (ja) | ハイブリッド航法装置 | |
| US8326561B2 (en) | Dynamic motion control | |
| KR19980042029A (ko) | 스트랩다운 관성 항법 시스템에서의 스컬링 보정을 위한 방법 및 장치 | |
| CN112798021B (zh) | 基于激光多普勒测速仪的惯导系统行进间初始对准方法 | |
| JP2007232443A (ja) | 慣性航法装置およびその誤差補正方法 | |
| JP5022747B2 (ja) | 移動体の姿勢及び方位検出装置 | |
| US11947364B2 (en) | Vessel anti-rolling control apparatus and anti-rolling control method | |
| RU2749152C1 (ru) | Адаптивный корректор углов ориентации для БИНС | |
| CN108627152A (zh) | 一种微型无人机基于多传感器数据融合的导航方法 | |
| JPH11118507A (ja) | 船舶の慣性航法装置 | |
| JPH0926328A (ja) | 位置標定装置 | |
| JP2012202749A (ja) | 方位測定装置 | |
| RU2754396C1 (ru) | Адаптивный способ коррекции углов ориентации БИНС | |
| JP3375720B2 (ja) | 船舶の慣性航法装置 | |
| JP3425689B2 (ja) | 慣性装置 | |
| JP2913122B2 (ja) | ストラップダウン型ジャイロ装置 | |
| JPH11248456A (ja) | ３軸姿勢検出装置 | |
| CN110057383B (zh) | 一种auv推位导航系统杆臂误差标校方法 | |
| JP2007155365A (ja) | 方位センサの補正係数演算装置及び演算プログラム | |
| JPH0821740A (ja) | 船舶用慣性航法装置 | |
| JPS63302317A (ja) | 移動体の位置速度測定装置 | |
| JPH0989585A (ja) | 慣性航法装置 | |
| JP3919911B2 (ja) | 方位姿勢基準装置 | |
| RU2790076C1 (ru) | Способ коррекции углов ориентации БИНС на скользящем интервале |