JP3375720B2 - 船舶の慣性航法装置 - Google Patents
船舶の慣性航法装置Info
- Publication number
- JP3375720B2 JP3375720B2 JP6135494A JP6135494A JP3375720B2 JP 3375720 B2 JP3375720 B2 JP 3375720B2 JP 6135494 A JP6135494 A JP 6135494A JP 6135494 A JP6135494 A JP 6135494A JP 3375720 B2 JP3375720 B2 JP 3375720B2
- Authority
- JP
- Japan
- Prior art keywords
- error
- speed
- output
- calculation unit
- acceleration
- 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.)
- Expired - Fee Related
Links
Landscapes
- Navigation (AREA)
- Traffic Control Systems (AREA)
Description
【発明の詳細な説明】
【０００１】
【産業上の利用分野】本発明は船舶の姿勢、方位、速度
および位置の検出に利用する。特に、これらの検出値を
出力するストラップダウン方式の慣性航法装置に関す
る。さらに詳しくは、このような慣性航法装置の初期姿
勢および方位の決定に関する。 【０００２】 【従来の技術】図２は従来の船舶用慣性航法装置の一例
を示すブロック構成図である。この慣性航法装置は特願
平５−５２４５０（本願出願時未公開）に示されたもの
であり、ＩＭＵ部１１と、入出力回路１２と、加速度計
誤差補正計算部２１、ジャイロ誤差補正計算部２２、加
速度座標変換計算部２３、座標変換行列計算部２４、修
正量計算部２５、速度位置計算部２６、姿勢方位計算部
２７、減算回路２８、２９、カルマンフィルタ部３０、
動揺外乱加速度同定計算部３３、フィードフォワード補
正計算部３４および速度計算部３５からなるコンピュー
タ部と、入出力回路４１と、入出力データ表示装置４２
とを備える。カルマンフィルタ部３０は、出力誤差推定
計算部３１とＩＭＵ誤差推定計算部３２とを備える。 【０００３】ここで、この従来例の動作を説明する前
に、この装置で用いる座標について図３を参照して説明
する。船舶用の慣性航法装置で用いられる座標には、船
体座標と航法座標（または航法計算座標）とがある。船
体座標は、ロール、ピッチおよびヨーの各軸成分によ
り、 （ｂx ，ｂy ，ｂz ） と表される座標であり、船体中心を原点とする。これに
対して航法座標はｎ座標とも呼ばれ、北、東および高さ
方向の各成分により、 （ｘN ，ｙE ，ｚD ） と表される座標である。航法座標もまた船体中心を原点
とするが、その船上の点の座標は船の移動に伴って移動
する。ロール軸のまわりの回転角をロール角φ、ピッチ
軸まわりの回転角をピッチ角θ、船の向きを方位角ψと
いう。方位角ψは北向きのときψ＝０、東向きのときψ
＝９０°である。 【０００４】次に、従来例の各部の動作について表１に
示す記号を用いて説明する。 【表１】【０００５】ＩＭＵ部１１は加速度計とジャイロとによ
り構成され、加速度計は船の運動加速度を計測して加速
度ベクトルを出力し、ジャイロは船の運動角速度を計測
して回転角速度ベクトルを出力する。ストラップダウン
方式では、このＩＭＵ部１１が直接に船体基準軸に取り
つけられる。取り付け位置は可能な限り船体重心の近く
に設定される。 【０００６】加速度計の出力する加速度ベクトルは、ロ
ール軸、ピッチ軸およびヨー軸の各方向の加速度Ａbx、
ＡbxおよびＡbzを要素とする船体座標で表現され、船の
推進加速度ベクトル、加速度誤差ベクトルおよび動揺加
速度ベクトルのベクトルの和により表される。このうち
動揺加速度ベクトルは、初期アライメント中に外乱加速
度となり、姿勢決定時間および決定精度に大きな影響を
及ぼす。初期アライメントとは、加速度計出力の水平成
分を用いて初期の姿勢および方位角をできるだけ正確に
決定することをいう。この初期アライメントのときに、
ジャイロおよび加速度計のそれぞれの誤差の一部につい
ても補正する。これをキャリブレーションという。 【０００７】ジャイロの出力する回転角速度ベクトルも
また船体座標で表現され、船体軸ロール角速度Ｗbx、船
体軸ピッチ角速度Ｗby、船体軸ヨー角速度Ｗbzを要素と
する。回転角速度ベクトルは旋回角速度ベクトル、ジャ
イロ誤差ベクトルおよび動揺角速度ベクトルの和として
表される。 【０００８】加速度計およびジャイロの出力は、入出力
回路１２を介して、それぞれ加速度計誤差補正計算部２
１およびジャイロ誤差補正計算部２２に入力される。加
速度計誤差補正計算部２１は、カルマンフィルタ部３０
内のＩＭＵ誤差推定計算部３２からの加速度計誤差推定
出力により加速度ベクトルを補正し、補正出力を加速度
座標変換計算部２３に出力する。ジャイロ誤差補正計算
部２２も同様に、ＩＭＵ誤差推定計算部３２からのジャ
イロ誤差推定出力により回転角速度ベクトルを補正す
る。 【０００９】加速度計誤差補正計算部２１の出力は加速
度座標変換計算部２３に入力される。加速度座標変換計
算部２３は、加速度計誤差補正計算部２１の出力を船体
座標から航法座標であるｎ座標に変換する。この変換の
ための座標変換行列は、座標変換行列計算部２４におい
て、ジャイロ誤差補正計算部２２の出力したジャイロ誤
差補正出力と、速度位置計算部２６により求められた船
の速度と、修正量計算部２５で求めた姿勢角誤差および
方位角誤差の修正量とにより求められる。 【００１０】よく知られているように、ジャイロは船の
運動角速度（旋回角速度＋動揺角速度）のみならず、絶
対静止座標に対するすべての角速度を計測する。具体的
には、地球の自転角速度、船が丸い地球の周辺を移動す
る移動角速度すなわち緯度および経度の変化率、などに
ついても計測する。しかし、航法計算や船の姿勢および
方位は地球に対する運動と姿勢角および方位角とから表
現されるので、ジャイロ出力から地球の自転角速度と移
動角速度とを差し引いて補正する必要がある。このよう
な補正量を修正量計算部２５で求める。修正量計算部２
５はまた、水平方向速度および高度方向の誤差の修正量
についても求める。 【００１１】速度位置計算部２６は、航法座標における
加速度を積分してその船の速度および位置（緯度および
経度）を求める。船の場合には速度および位置に高度方
向の情報は直接には不要であるが、水平方向への影響が
あるため、修正量計算部２５により求めた高度方向誤差
修正量に基づいて、高さおよび高度方向の速度を求めて
おく。速度位置計算部２６の求めた速度ベクトルおよび
位置ベクトルは、入出力回路４１を介して入出力データ
表示装置４２に出力される。 【００１２】一方、姿勢方位計算部２７は、座標変換行
列計算部２４の出力した座標変換行列から姿勢角（φ，
θ）と方位角ψとを計算し、入出力回路４１を介して入
出力データ表示装置４２に出力する。 【００１３】速度位置計算部２６により得られた速度ベ
クトルおよび位置ベクトルは、コンピュータ部から出力
されるだけでなく、コンピュータ部内の減算回路２８、
２９に入力される。減算回路２８、２９にはさらに、入
出力回路４１を介してそれぞれ外部基準速度および外部
基準位置が入力される。減算回路２８、２９は、これら
を比較し、その差を出力誤差推定計算部３１およびＩＭ
Ｕ誤差推定計算部３２の双方に入力する。 【００１４】出力誤差推定計算部３１は、速度誤差、位
置誤差、姿勢角誤差および方位角誤差を推定し、推定値
を修正量計算部２５に出力する。ＩＭＵ誤差推定計算部
３２はジャイロ誤差および加速度誤差を推定し、それぞ
れ加速度計誤差補正計算部２１、ジャイロ誤差補正計算
部２２に出力する。 【００１５】初期アライメント時には、外部基準速度お
よび外部基準位置が零の状態で、ジャイロ誤差および加
速度計誤差による姿勢誤差および方位誤差を推定し、こ
れらの誤差の修正量を計算して座標変換行列計算部２４
に入力する。姿勢誤差および方位誤差を推定するために
は、当然、その原因となるジャイロ誤差および加速度計
誤差についても推定することになる。観測データとして
姿勢誤差および方位誤差が直接測定できるならこの推定
は簡単であるが、実際には速度データが観測データとな
り、ジャイロ誤差および加速度計誤差による速度誤差Δ
ＶN 、ΔＶE を正しく測定できるわけではなく、動揺外
乱としての動揺速度ＶdN、ＶdEが余計に測定されてしま
う。これらは速度誤差ΔＶN 、ΔＶE の信号に対してノ
イズ外乱となり、速度誤差ΔＶN 、ΔＶE 、姿勢角誤
差、方位角誤差、加速度計誤差およびジャイロ誤差のそ
れぞれの推定精度を低下させる要因とする。また、推定
完了までに要する時間（これを「整定時間」という）を
長くする要因ともなる。 【００１６】この整定時間を短縮するため、図２に示し
た従来例では、動揺外乱加速度同定計算部３３、フィー
ドフォワード補正計算部３４および速度計算部３５を備
える。動揺外乱加速度同定計算部３３は、初期アライメ
ント時に、加速度座標変換計算部２３からの時系列の出
力に基づいてその出力に含まれる動揺外乱加速度を推定
する。フィードフォワード補正計算部３４は、動揺外乱
加速度の推定値により加速度座標変換計算部２３の出力
を補正する。速度計算部３５は、フィードフォワード補
正計算部３４の出力を積分し、それを初期アライメント
時の速度誤差として出力誤差推定計算部３１およびＩＭ
Ｕ誤差推定計算部３２に出力する。 【００１７】 【発明が解決しようとする課題】上述した先の出願で
は、動揺外乱加速度を推定してフィードフォワード補正
を行うことで、動揺外乱の影響を短時間に除去すること
が可能となり、初期アライメントおよびキャリブレーシ
ョンの時間を短縮し、高精度に姿勢および方位を決定で
きるようになった。具体的には、それ以前に４時間要し
ていた初期アライメントおよびキャリブレーション時間
が、３０分以下と大幅に短縮された。 【００１８】しかし、（１）動揺外乱加速度同定計算部
の処理が複雑であり、計算時間をより短縮しようとする
と、処理性能の高い計算機が必要となって装置の価格が
高くなる、（２）正確な外乱フィードフォワードを設定
する必要があり、不正確だとフィードフォワード補正に
おいて満足な性能が得られない、（３）動揺外乱加速度
の同定精度が不十分であり、フィードフォワード補正す
ると信号波形が歪んでしまうことがあるという課題があ
った。 【００１９】本発明は、このような課題を解決し、高精
度で短時間に初期姿勢および初期方位角を決定すること
のできる慣性航法装置を提供することを目的とする。 【００２０】 【課題を解決するための手段】本発明の慣性航法装置
は、測定される速度データを 【数５】でモデル化し、評価関数 【数６】 を最小にするΘの推定値を 【数７】 により求め、 【数８】 により初期設定時の速度誤差を推定することを特徴とす
る。 【００２１】 【作用】初期アライメント中の加速度データの南北方向
成分および東西方向成分の積分値である速度データをバ
ッチシーケンシャル形の改良最小二乗法を用いてプリフ
ィルタ処理し、処理後の速度データをカルマンフィルタ
入力とする。これにより、簡単かつ精度よく速度誤差の
推定値を得ることができ、短時間で高精度に初期姿勢お
よび方位を決定できる。 【００２２】 【実施例】図１は本発明実施例の船舶用慣性航法装置を
示すブロック構成図である。この実施例装置は、上述し
た従来例と同様に、ＩＭＵ部１１と、入出力回路１２
と、加速度計誤差補正計算部２１、ジャイロ誤差補正計
算部２２、加速度座標変換計算部２３、座標変換行列計
算部２４、修正量計算部２５、速度位置計算部２６、姿
勢方位計算部２７、減算回路２８、２９、カルマンフィ
ルタ部３０、動揺外乱加速度同定計算部３３、フィード
フォワード補正計算部３４および速度計算部３５からな
るコンピュータ部と、入出力回路４１と、入出力データ
表示装置４２とを備える。カルマンフィルタ部３０は、
出力誤差推定計算部３１とＩＭＵ誤差推定計算部３２と
を備える。さらに本実施例では、コンピュータ部内に、
動揺外乱除去計算部４３を備える。 【００２３】図４ないし図７はこの実施例の動作フロー
を示す。この実施例において、動揺外乱除去計算部４３
以外の各部の動作は上述した従来例と同等である。これ
らの動作について簡単に説明した後に、動揺外乱除去計
算部４３の動作について詳しく説明する。 【００２４】ＩＭＵ部１１は船体基準軸に取り付けられ
た加速度計およびジャイロにより構成される。加速度計
の出力は入出力回路１２および加速度計誤差補正計算部
２１を介して加速度座標変換計算部２３に入力され、ジ
ャイロの出力は入出力回路１２およびジャイロ誤差補正
計算部２２を介して座標変換行列計算部２４に入力され
る。加速度座標変換計算部２３は、加速度計が出力した
船体座標における加速度を航法座標における加速度に変
換する。速度位置計算部２６はこの航法座標における加
速度を積分してその船の速度および位置を求める。座標
変換行列計算部２４はジャイロが出力した船の回転角速
度から船体座標と航法座標との間の変換規則を求める。
姿勢方位計算部２７は座標変換行列計算部２４の出力に
よりその船の姿勢および方位を求める。求められた速
度、位置、姿勢および方位は入出力回路４１を介して入
出力データ表示装置４２に出力される。 【００２５】入出力回路４１には外部から速度比較値お
よび位置比較値が入力され、速度比較値を減算回路２８
に、位置比較値を減算回路２９に供給する。減算回路２
８、２９は、それぞれ速度位置計算部２６により求めら
れた速度および位置と入出力回路４１からの速度比較値
および位置比較値との差を求め、カルマンフィルタ部３
０に供給する。カルマンフィルタ部３０内の出力誤差推
定計算部３１は減算回路２８、２９の出力に基づいて出
力誤差を推定して修正量計算部２５に出力し、修正量計
算部２５は速度位置計算部２６および座標変換行列計算
部２４のそれぞれの演算の修正量を求める。カルマンフ
ィルタ部３０内のＩＭＵ誤差推定計算部３２は減算回路
２８、２９の出力に基づいてＩＭＵ誤差を推定して加速
度計誤差補正計算部２１およびジャイロ補正誤差補正計
算部２２に出力し、加速度計誤差補正計算部２１および
ジャイロ補正誤差補正計算部２２は加速度計およびジャ
イロのそれぞれの出力誤差を補正する。 【００２６】動揺外乱除去計算部４３は、速度比較値が
零である初期アライメント中の速度誤差を推定してカル
マンフィルタ部３０内の出力誤差推定計算部３１および
ＩＭＵ誤差推定計算部３２にそれぞれ供給する。初期ア
ライメント中には、速度ベクトルの北方向成分ＶN およ
び東方向成分ＶE が、 【数９】 となる。この式において、ＶdN、ＶdEが各方向の動揺外
乱速度であり、ΔＶN (t) 、ΔＶE (t) はジャイロ誤
差、加速度計誤差、姿勢および方位誤差その他により発
生する速度誤差である。 【００２７】初期アライメントの目的は、何らかの方法
でＶdN、ＶdEを小さくし（理想的には零）、ΔＶN (t)
、ΔＶE (t) を抽出してカルマンフィルタ部３０の入
力とすることにより、ジャイロ誤差、加速度計誤差、姿
勢および方位誤差、速度誤差などの誤差を推定するもの
である。この初期アライメントで求めた推定値により誤
差を修正することにより、正確な姿勢および方位を求め
ることができる。各誤差の推定値の精度は、動揺外乱速
度ＶdN、ＶdEをどの程度除去できたかに大きく左右され
る。 【００２８】次に、改良形最小二乗法を用いて短い時間
で動揺外乱速度ＶｄＮ、ＶｄＥを除去してΔＶ
Ｎ （ｔ） 、ΔＶＥ （ｔ） を抽出することについ
て説明する。速度位置計算部２６により求められる北方
向および東方向の速度についての測定データｙ
Ｎ （ｔ） 、ｙＥ （ｔ） は、 【数１０】 と表される。ここで、δＶＮｒ、δＶＥｒは正規白色性
の観測ノイズである。これらの測定データｙＮ （ｔ）
、ｙＥ （ｔ） において、除去すべき動揺外乱速度
ＶｄＮ、ＶｄＥは周期約数１０秒程度の周期波形であ
り、信号ΔＶＮ （ｔ） 、ΔＶＥ （ｔ） は例えば
周期８４分（シューラー周期）の正弦波である。測定デ
ータｙＮ （ｔ） 、ｙＥ （ｔ） をまとめて次のよ
うにベクトル表現する。 【数１１】 【００２９】また、測定データとは別に、北方向および
東方向の速度を次の自己回帰モデルでモデル化する。 【数１２】 このモデルのパラメータΘを実際の測定データを用いて
推定することで、信号ΔＶN (t) 、ΔＶE (t) を近似す
ることができる。 【００３０】パラメータΘを最適推定するための評価関
数は、次のバッチ形データで表される。 【数１３】 【００３１】この評価関数を最小にする推定値は、 【数１４】 となる。この推定値を用いて、信号ΔＶN (t) 、ΔＶE
(t) の推定値が、 【数１５】 により求められる。この推定値をカルマンフィルタ部３
０への入力とする。 【００３２】図８はパラメータΘの推定動作を説明する
図である。現在の時刻がｔi のとき、ｔi-(N-1) からｔ
i までのそれぞれの時点における測定データからパラメ
ータΘの推定値を求める。 【００３３】以上の説明では速度位置計算部２６により
求めた速度データをプリフィルタ処理してカルマンフィ
ルタ部３０に入力する構成を示したが、加速度座標変換
計算部２３の出力する加速度データをプリフィルタ処理
してから積分し、これをカルマンフィルタ部３０の入力
とすることもできる。 【００３４】 【発明の効果】以上説明したように、本発明の船舶の慣
性航法装置は、初期アライメント中の速度データをバッ
チシーケンシャル形の改良最小二乗法を用いてプリフィ
ルタ処理し、処理後の速度データをカルマンフィルタ入
力とすることにより、簡単かつ精度よく速度誤差の推定
値を得ることができる。したがって、約１５分程度の短
時間で高精度に初期姿勢および方位を決定できる効果が
ある。 【００３５】また、同定が終了するまでの時間を節約す
ることができ、整定時間をより短縮することができるの
で、消費電力を節約できる効果がある。 【００３６】さらに、初期姿勢および初期方位をより高
精度かつ短時間に決定することができ、短時間での出航
が可能となり、船舶の運用コストを削減できる効果があ
る。
および位置の検出に利用する。特に、これらの検出値を
出力するストラップダウン方式の慣性航法装置に関す
る。さらに詳しくは、このような慣性航法装置の初期姿
勢および方位の決定に関する。 【０００２】 【従来の技術】図２は従来の船舶用慣性航法装置の一例
を示すブロック構成図である。この慣性航法装置は特願
平５−５２４５０（本願出願時未公開）に示されたもの
であり、ＩＭＵ部１１と、入出力回路１２と、加速度計
誤差補正計算部２１、ジャイロ誤差補正計算部２２、加
速度座標変換計算部２３、座標変換行列計算部２４、修
正量計算部２５、速度位置計算部２６、姿勢方位計算部
２７、減算回路２８、２９、カルマンフィルタ部３０、
動揺外乱加速度同定計算部３３、フィードフォワード補
正計算部３４および速度計算部３５からなるコンピュー
タ部と、入出力回路４１と、入出力データ表示装置４２
とを備える。カルマンフィルタ部３０は、出力誤差推定
計算部３１とＩＭＵ誤差推定計算部３２とを備える。 【０００３】ここで、この従来例の動作を説明する前
に、この装置で用いる座標について図３を参照して説明
する。船舶用の慣性航法装置で用いられる座標には、船
体座標と航法座標（または航法計算座標）とがある。船
体座標は、ロール、ピッチおよびヨーの各軸成分によ
り、 （ｂx ，ｂy ，ｂz ） と表される座標であり、船体中心を原点とする。これに
対して航法座標はｎ座標とも呼ばれ、北、東および高さ
方向の各成分により、 （ｘN ，ｙE ，ｚD ） と表される座標である。航法座標もまた船体中心を原点
とするが、その船上の点の座標は船の移動に伴って移動
する。ロール軸のまわりの回転角をロール角φ、ピッチ
軸まわりの回転角をピッチ角θ、船の向きを方位角ψと
いう。方位角ψは北向きのときψ＝０、東向きのときψ
＝９０°である。 【０００４】次に、従来例の各部の動作について表１に
示す記号を用いて説明する。 【表１】【０００５】ＩＭＵ部１１は加速度計とジャイロとによ
り構成され、加速度計は船の運動加速度を計測して加速
度ベクトルを出力し、ジャイロは船の運動角速度を計測
して回転角速度ベクトルを出力する。ストラップダウン
方式では、このＩＭＵ部１１が直接に船体基準軸に取り
つけられる。取り付け位置は可能な限り船体重心の近く
に設定される。 【０００６】加速度計の出力する加速度ベクトルは、ロ
ール軸、ピッチ軸およびヨー軸の各方向の加速度Ａbx、
ＡbxおよびＡbzを要素とする船体座標で表現され、船の
推進加速度ベクトル、加速度誤差ベクトルおよび動揺加
速度ベクトルのベクトルの和により表される。このうち
動揺加速度ベクトルは、初期アライメント中に外乱加速
度となり、姿勢決定時間および決定精度に大きな影響を
及ぼす。初期アライメントとは、加速度計出力の水平成
分を用いて初期の姿勢および方位角をできるだけ正確に
決定することをいう。この初期アライメントのときに、
ジャイロおよび加速度計のそれぞれの誤差の一部につい
ても補正する。これをキャリブレーションという。 【０００７】ジャイロの出力する回転角速度ベクトルも
また船体座標で表現され、船体軸ロール角速度Ｗbx、船
体軸ピッチ角速度Ｗby、船体軸ヨー角速度Ｗbzを要素と
する。回転角速度ベクトルは旋回角速度ベクトル、ジャ
イロ誤差ベクトルおよび動揺角速度ベクトルの和として
表される。 【０００８】加速度計およびジャイロの出力は、入出力
回路１２を介して、それぞれ加速度計誤差補正計算部２
１およびジャイロ誤差補正計算部２２に入力される。加
速度計誤差補正計算部２１は、カルマンフィルタ部３０
内のＩＭＵ誤差推定計算部３２からの加速度計誤差推定
出力により加速度ベクトルを補正し、補正出力を加速度
座標変換計算部２３に出力する。ジャイロ誤差補正計算
部２２も同様に、ＩＭＵ誤差推定計算部３２からのジャ
イロ誤差推定出力により回転角速度ベクトルを補正す
る。 【０００９】加速度計誤差補正計算部２１の出力は加速
度座標変換計算部２３に入力される。加速度座標変換計
算部２３は、加速度計誤差補正計算部２１の出力を船体
座標から航法座標であるｎ座標に変換する。この変換の
ための座標変換行列は、座標変換行列計算部２４におい
て、ジャイロ誤差補正計算部２２の出力したジャイロ誤
差補正出力と、速度位置計算部２６により求められた船
の速度と、修正量計算部２５で求めた姿勢角誤差および
方位角誤差の修正量とにより求められる。 【００１０】よく知られているように、ジャイロは船の
運動角速度（旋回角速度＋動揺角速度）のみならず、絶
対静止座標に対するすべての角速度を計測する。具体的
には、地球の自転角速度、船が丸い地球の周辺を移動す
る移動角速度すなわち緯度および経度の変化率、などに
ついても計測する。しかし、航法計算や船の姿勢および
方位は地球に対する運動と姿勢角および方位角とから表
現されるので、ジャイロ出力から地球の自転角速度と移
動角速度とを差し引いて補正する必要がある。このよう
な補正量を修正量計算部２５で求める。修正量計算部２
５はまた、水平方向速度および高度方向の誤差の修正量
についても求める。 【００１１】速度位置計算部２６は、航法座標における
加速度を積分してその船の速度および位置（緯度および
経度）を求める。船の場合には速度および位置に高度方
向の情報は直接には不要であるが、水平方向への影響が
あるため、修正量計算部２５により求めた高度方向誤差
修正量に基づいて、高さおよび高度方向の速度を求めて
おく。速度位置計算部２６の求めた速度ベクトルおよび
位置ベクトルは、入出力回路４１を介して入出力データ
表示装置４２に出力される。 【００１２】一方、姿勢方位計算部２７は、座標変換行
列計算部２４の出力した座標変換行列から姿勢角（φ，
θ）と方位角ψとを計算し、入出力回路４１を介して入
出力データ表示装置４２に出力する。 【００１３】速度位置計算部２６により得られた速度ベ
クトルおよび位置ベクトルは、コンピュータ部から出力
されるだけでなく、コンピュータ部内の減算回路２８、
２９に入力される。減算回路２８、２９にはさらに、入
出力回路４１を介してそれぞれ外部基準速度および外部
基準位置が入力される。減算回路２８、２９は、これら
を比較し、その差を出力誤差推定計算部３１およびＩＭ
Ｕ誤差推定計算部３２の双方に入力する。 【００１４】出力誤差推定計算部３１は、速度誤差、位
置誤差、姿勢角誤差および方位角誤差を推定し、推定値
を修正量計算部２５に出力する。ＩＭＵ誤差推定計算部
３２はジャイロ誤差および加速度誤差を推定し、それぞ
れ加速度計誤差補正計算部２１、ジャイロ誤差補正計算
部２２に出力する。 【００１５】初期アライメント時には、外部基準速度お
よび外部基準位置が零の状態で、ジャイロ誤差および加
速度計誤差による姿勢誤差および方位誤差を推定し、こ
れらの誤差の修正量を計算して座標変換行列計算部２４
に入力する。姿勢誤差および方位誤差を推定するために
は、当然、その原因となるジャイロ誤差および加速度計
誤差についても推定することになる。観測データとして
姿勢誤差および方位誤差が直接測定できるならこの推定
は簡単であるが、実際には速度データが観測データとな
り、ジャイロ誤差および加速度計誤差による速度誤差Δ
ＶN 、ΔＶE を正しく測定できるわけではなく、動揺外
乱としての動揺速度ＶdN、ＶdEが余計に測定されてしま
う。これらは速度誤差ΔＶN 、ΔＶE の信号に対してノ
イズ外乱となり、速度誤差ΔＶN 、ΔＶE 、姿勢角誤
差、方位角誤差、加速度計誤差およびジャイロ誤差のそ
れぞれの推定精度を低下させる要因とする。また、推定
完了までに要する時間（これを「整定時間」という）を
長くする要因ともなる。 【００１６】この整定時間を短縮するため、図２に示し
た従来例では、動揺外乱加速度同定計算部３３、フィー
ドフォワード補正計算部３４および速度計算部３５を備
える。動揺外乱加速度同定計算部３３は、初期アライメ
ント時に、加速度座標変換計算部２３からの時系列の出
力に基づいてその出力に含まれる動揺外乱加速度を推定
する。フィードフォワード補正計算部３４は、動揺外乱
加速度の推定値により加速度座標変換計算部２３の出力
を補正する。速度計算部３５は、フィードフォワード補
正計算部３４の出力を積分し、それを初期アライメント
時の速度誤差として出力誤差推定計算部３１およびＩＭ
Ｕ誤差推定計算部３２に出力する。 【００１７】 【発明が解決しようとする課題】上述した先の出願で
は、動揺外乱加速度を推定してフィードフォワード補正
を行うことで、動揺外乱の影響を短時間に除去すること
が可能となり、初期アライメントおよびキャリブレーシ
ョンの時間を短縮し、高精度に姿勢および方位を決定で
きるようになった。具体的には、それ以前に４時間要し
ていた初期アライメントおよびキャリブレーション時間
が、３０分以下と大幅に短縮された。 【００１８】しかし、（１）動揺外乱加速度同定計算部
の処理が複雑であり、計算時間をより短縮しようとする
と、処理性能の高い計算機が必要となって装置の価格が
高くなる、（２）正確な外乱フィードフォワードを設定
する必要があり、不正確だとフィードフォワード補正に
おいて満足な性能が得られない、（３）動揺外乱加速度
の同定精度が不十分であり、フィードフォワード補正す
ると信号波形が歪んでしまうことがあるという課題があ
った。 【００１９】本発明は、このような課題を解決し、高精
度で短時間に初期姿勢および初期方位角を決定すること
のできる慣性航法装置を提供することを目的とする。 【００２０】 【課題を解決するための手段】本発明の慣性航法装置
は、測定される速度データを 【数５】でモデル化し、評価関数 【数６】 を最小にするΘの推定値を 【数７】 により求め、 【数８】 により初期設定時の速度誤差を推定することを特徴とす
る。 【００２１】 【作用】初期アライメント中の加速度データの南北方向
成分および東西方向成分の積分値である速度データをバ
ッチシーケンシャル形の改良最小二乗法を用いてプリフ
ィルタ処理し、処理後の速度データをカルマンフィルタ
入力とする。これにより、簡単かつ精度よく速度誤差の
推定値を得ることができ、短時間で高精度に初期姿勢お
よび方位を決定できる。 【００２２】 【実施例】図１は本発明実施例の船舶用慣性航法装置を
示すブロック構成図である。この実施例装置は、上述し
た従来例と同様に、ＩＭＵ部１１と、入出力回路１２
と、加速度計誤差補正計算部２１、ジャイロ誤差補正計
算部２２、加速度座標変換計算部２３、座標変換行列計
算部２４、修正量計算部２５、速度位置計算部２６、姿
勢方位計算部２７、減算回路２８、２９、カルマンフィ
ルタ部３０、動揺外乱加速度同定計算部３３、フィード
フォワード補正計算部３４および速度計算部３５からな
るコンピュータ部と、入出力回路４１と、入出力データ
表示装置４２とを備える。カルマンフィルタ部３０は、
出力誤差推定計算部３１とＩＭＵ誤差推定計算部３２と
を備える。さらに本実施例では、コンピュータ部内に、
動揺外乱除去計算部４３を備える。 【００２３】図４ないし図７はこの実施例の動作フロー
を示す。この実施例において、動揺外乱除去計算部４３
以外の各部の動作は上述した従来例と同等である。これ
らの動作について簡単に説明した後に、動揺外乱除去計
算部４３の動作について詳しく説明する。 【００２４】ＩＭＵ部１１は船体基準軸に取り付けられ
た加速度計およびジャイロにより構成される。加速度計
の出力は入出力回路１２および加速度計誤差補正計算部
２１を介して加速度座標変換計算部２３に入力され、ジ
ャイロの出力は入出力回路１２およびジャイロ誤差補正
計算部２２を介して座標変換行列計算部２４に入力され
る。加速度座標変換計算部２３は、加速度計が出力した
船体座標における加速度を航法座標における加速度に変
換する。速度位置計算部２６はこの航法座標における加
速度を積分してその船の速度および位置を求める。座標
変換行列計算部２４はジャイロが出力した船の回転角速
度から船体座標と航法座標との間の変換規則を求める。
姿勢方位計算部２７は座標変換行列計算部２４の出力に
よりその船の姿勢および方位を求める。求められた速
度、位置、姿勢および方位は入出力回路４１を介して入
出力データ表示装置４２に出力される。 【００２５】入出力回路４１には外部から速度比較値お
よび位置比較値が入力され、速度比較値を減算回路２８
に、位置比較値を減算回路２９に供給する。減算回路２
８、２９は、それぞれ速度位置計算部２６により求めら
れた速度および位置と入出力回路４１からの速度比較値
および位置比較値との差を求め、カルマンフィルタ部３
０に供給する。カルマンフィルタ部３０内の出力誤差推
定計算部３１は減算回路２８、２９の出力に基づいて出
力誤差を推定して修正量計算部２５に出力し、修正量計
算部２５は速度位置計算部２６および座標変換行列計算
部２４のそれぞれの演算の修正量を求める。カルマンフ
ィルタ部３０内のＩＭＵ誤差推定計算部３２は減算回路
２８、２９の出力に基づいてＩＭＵ誤差を推定して加速
度計誤差補正計算部２１およびジャイロ補正誤差補正計
算部２２に出力し、加速度計誤差補正計算部２１および
ジャイロ補正誤差補正計算部２２は加速度計およびジャ
イロのそれぞれの出力誤差を補正する。 【００２６】動揺外乱除去計算部４３は、速度比較値が
零である初期アライメント中の速度誤差を推定してカル
マンフィルタ部３０内の出力誤差推定計算部３１および
ＩＭＵ誤差推定計算部３２にそれぞれ供給する。初期ア
ライメント中には、速度ベクトルの北方向成分ＶN およ
び東方向成分ＶE が、 【数９】 となる。この式において、ＶdN、ＶdEが各方向の動揺外
乱速度であり、ΔＶN (t) 、ΔＶE (t) はジャイロ誤
差、加速度計誤差、姿勢および方位誤差その他により発
生する速度誤差である。 【００２７】初期アライメントの目的は、何らかの方法
でＶdN、ＶdEを小さくし（理想的には零）、ΔＶN (t)
、ΔＶE (t) を抽出してカルマンフィルタ部３０の入
力とすることにより、ジャイロ誤差、加速度計誤差、姿
勢および方位誤差、速度誤差などの誤差を推定するもの
である。この初期アライメントで求めた推定値により誤
差を修正することにより、正確な姿勢および方位を求め
ることができる。各誤差の推定値の精度は、動揺外乱速
度ＶdN、ＶdEをどの程度除去できたかに大きく左右され
る。 【００２８】次に、改良形最小二乗法を用いて短い時間
で動揺外乱速度ＶｄＮ、ＶｄＥを除去してΔＶ
Ｎ （ｔ） 、ΔＶＥ （ｔ） を抽出することについ
て説明する。速度位置計算部２６により求められる北方
向および東方向の速度についての測定データｙ
Ｎ （ｔ） 、ｙＥ （ｔ） は、 【数１０】 と表される。ここで、δＶＮｒ、δＶＥｒは正規白色性
の観測ノイズである。これらの測定データｙＮ （ｔ）
、ｙＥ （ｔ） において、除去すべき動揺外乱速度
ＶｄＮ、ＶｄＥは周期約数１０秒程度の周期波形であ
り、信号ΔＶＮ （ｔ） 、ΔＶＥ （ｔ） は例えば
周期８４分（シューラー周期）の正弦波である。測定デ
ータｙＮ （ｔ） 、ｙＥ （ｔ） をまとめて次のよ
うにベクトル表現する。 【数１１】 【００２９】また、測定データとは別に、北方向および
東方向の速度を次の自己回帰モデルでモデル化する。 【数１２】 このモデルのパラメータΘを実際の測定データを用いて
推定することで、信号ΔＶN (t) 、ΔＶE (t) を近似す
ることができる。 【００３０】パラメータΘを最適推定するための評価関
数は、次のバッチ形データで表される。 【数１３】 【００３１】この評価関数を最小にする推定値は、 【数１４】 となる。この推定値を用いて、信号ΔＶN (t) 、ΔＶE
(t) の推定値が、 【数１５】 により求められる。この推定値をカルマンフィルタ部３
０への入力とする。 【００３２】図８はパラメータΘの推定動作を説明する
図である。現在の時刻がｔi のとき、ｔi-(N-1) からｔ
i までのそれぞれの時点における測定データからパラメ
ータΘの推定値を求める。 【００３３】以上の説明では速度位置計算部２６により
求めた速度データをプリフィルタ処理してカルマンフィ
ルタ部３０に入力する構成を示したが、加速度座標変換
計算部２３の出力する加速度データをプリフィルタ処理
してから積分し、これをカルマンフィルタ部３０の入力
とすることもできる。 【００３４】 【発明の効果】以上説明したように、本発明の船舶の慣
性航法装置は、初期アライメント中の速度データをバッ
チシーケンシャル形の改良最小二乗法を用いてプリフィ
ルタ処理し、処理後の速度データをカルマンフィルタ入
力とすることにより、簡単かつ精度よく速度誤差の推定
値を得ることができる。したがって、約１５分程度の短
時間で高精度に初期姿勢および方位を決定できる効果が
ある。 【００３５】また、同定が終了するまでの時間を節約す
ることができ、整定時間をより短縮することができるの
で、消費電力を節約できる効果がある。 【００３６】さらに、初期姿勢および初期方位をより高
精度かつ短時間に決定することができ、短時間での出航
が可能となり、船舶の運用コストを削減できる効果があ
る。
【図面の簡単な説明】
【図１】本発明実施例の慣性航法装置を示すブロック構
成図。 【図２】従来例の慣性航法装置を示すブロック構成図。 【図３】慣性航法装置で用いられる座標を示す図。 【図４】実施例装置の動作フローの一部を示す図。 【図５】実施例装置の動作フローの一部を示す図。 【図６】実施例装置の動作フローの一部を示す図。 【図７】実施例装置の動作フローの一部を示す図。 【図８】パラメータΘの推定動作を説明する図。 【符号の説明】 １１ ＩＭＵ部 １２、４１ 入出力回路 ２１ 加速度計誤差補正計算部 ２２ ジャイロ誤差補正計算部 ２３ 加速度座標変換計算部 ２４ 座標変換行列計算部 ２５ 修正量計算部 ２６ 速度位置計算部 ２７ 姿勢方位計算部 ２８、２９ 減算回路 ３０ カルマンフィルタ部 ３１ 出力誤差推定計算部 ３２ ＩＭＵ誤差推定計算部 ３３ 動揺外乱加速度同定計算部 ３４ フィードフォワード補正計算部 ３５ 速度計算部 ４２ 入出力データ表示装置 ４３ 動揺外乱除去計算部
成図。 【図２】従来例の慣性航法装置を示すブロック構成図。 【図３】慣性航法装置で用いられる座標を示す図。 【図４】実施例装置の動作フローの一部を示す図。 【図５】実施例装置の動作フローの一部を示す図。 【図６】実施例装置の動作フローの一部を示す図。 【図７】実施例装置の動作フローの一部を示す図。 【図８】パラメータΘの推定動作を説明する図。 【符号の説明】 １１ ＩＭＵ部 １２、４１ 入出力回路 ２１ 加速度計誤差補正計算部 ２２ ジャイロ誤差補正計算部 ２３ 加速度座標変換計算部 ２４ 座標変換行列計算部 ２５ 修正量計算部 ２６ 速度位置計算部 ２７ 姿勢方位計算部 ２８、２９ 減算回路 ３０ カルマンフィルタ部 ３１ 出力誤差推定計算部 ３２ ＩＭＵ誤差推定計算部 ３３ 動揺外乱加速度同定計算部 ３４ フィードフォワード補正計算部 ３５ 速度計算部 ４２ 入出力データ表示装置 ４３ 動揺外乱除去計算部
─────────────────────────────────────────────────────
フロントページの続き
(56)参考文献 特開 平５−340763（ＪＰ，Ａ)
特開 昭61−31920（ＪＰ，Ａ)
特開 平５−141981（ＪＰ，Ａ)
特開 平６−265367（ＪＰ，Ａ)
(58)調査した分野(Int.Cl.7，ＤＢ名)
G08G 3/00
G01C 21/16
Claims (1)
- (57)【特許請求の範囲】 【請求項１】 船体基準軸に取り付けられた加速度計お
よびジャイロからなるＩＭＵ部（１１）と、 上記加速度計が出力した船体座標における加速度を航法
座標における加速度に変換する第一の演算手段（２３）
と、 この航法座標における加速度を積分してその船の速度お
よび位置を求める第二の演算手段（２６）と、 上記ジャイロが出力した船の回転角速度から船体座標と
航法座標との間の変換規則を求める第三の演算手段（２
４）と、 この第三の演算手段の出力によりその船の姿勢および方
位を求める第四の演算手段（２７）と、 上記第二の演算手段により求められた速度および位置と
外部から入力された速度比較値および位置比較値との差
に基づいて上記第二および上記第三の演算手段のそれぞ
れの演算の修正量を求める第一の補正手段（３１、２
５）と、 上記第二の演算手段により求められた速度および位置と
外部から入力された速度比較値および位置比較値との差
に基づいて上記加速度計および上記ジャイロのそれぞれ
の出力誤差を補正する第二の補正手段（３２、２１、２
２）と、 速度比較値が零である初期設定時の速度誤差を推定して
上記第一および上記第二の補正手段にそれぞれ供給する
初期設定手段とを備えた船舶の慣性航法装置において、 この初期設定手段は、航法座標における加速度を積分し
た速度データを 【数１】でモデル化し、評価関数 【数２】 を最小にするΘの推定値を 【数３】 により求め、 【数４】 により初期設定時の速度誤差を推定する手段を含むこと
を特徴とする船舶の慣性航法装置。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP6135494A JP3375720B2 (ja) | 1994-03-30 | 1994-03-30 | 船舶の慣性航法装置 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP6135494A JP3375720B2 (ja) | 1994-03-30 | 1994-03-30 | 船舶の慣性航法装置 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JPH07270176A JPH07270176A (ja) | 1995-10-20 |
| JP3375720B2 true JP3375720B2 (ja) | 2003-02-10 |
Family
ID=13168737
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP6135494A Expired - Fee Related JP3375720B2 (ja) | 1994-03-30 | 1994-03-30 | 船舶の慣性航法装置 |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP3375720B2 (ja) |
Families Citing this family (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP3744641B2 (ja) * | 1997-03-25 | 2006-02-15 | 西日本旅客鉄道株式会社 | 事故検知装置 |
| JP4721324B2 (ja) * | 2005-01-17 | 2011-07-13 | 応用地質株式会社 | 加速度センサを用いる地盤等の変位モニタリング方法 |
| CN105278535B (zh) * | 2015-11-23 | 2018-02-13 | 上海海事大学 | 一种用于无动力设施拖带系统的智能转向协同控制方法 |
| CN112284419B (zh) * | 2020-10-19 | 2023-02-28 | 中国人民解放军空军工程大学 | 一种双轴旋转调制初始精对准方法 |
| CN116046027B (zh) * | 2023-03-31 | 2023-06-16 | 中国船舶集团有限公司第七〇七研究所 | 三轴旋转式惯导位置误差无源自主校准方法及系统 |
-
1994
- 1994-03-30 JP JP6135494A patent/JP3375720B2/ja not_active Expired - Fee Related
Also Published As
| Publication number | Publication date |
|---|---|
| JPH07270176A (ja) | 1995-10-20 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| JP4199553B2 (ja) | ハイブリッド航法装置 | |
| CN109916395B (zh) | 一种姿态自主冗余组合导航算法 | |
| US8560234B2 (en) | System and method of navigation based on state estimation using a stepped filter | |
| CN111551174A (zh) | 基于多传感器惯性导航系统的高动态车辆姿态计算方法及系统 | |
| CN111060097B (zh) | 一种提高位置误差估计精度的惯性/天文组合导航方法 | |
| US9534900B2 (en) | Inertial navigation sculling algorithm | |
| CN106370178B (zh) | 移动终端设备的姿态测量方法及装置 | |
| CN110887480A (zh) | 基于mems传感器的飞行姿态估计方法及系统 | |
| JP7025215B2 (ja) | 測位システム及び測位方法 | |
| JP2007232443A (ja) | 慣性航法装置およびその誤差補正方法 | |
| JPS5936208B2 (ja) | 航空機の慣性台を迅速に整列させる方法および装置 | |
| CN110988950A (zh) | 导航装置、航行辅助信息的生成方法及存储介质 | |
| CN112683269A (zh) | 一种附有运动加速度补偿的marg姿态计算方法 | |
| CN110440797A (zh) | 车辆姿态估计方法及系统 | |
| JP2014240266A (ja) | センサドリフト量推定装置及びプログラム | |
| CN111307114B (zh) | 基于运动参考单元的水面舰船水平姿态测量方法 | |
| JP3375720B2 (ja) | 船舶の慣性航法装置 | |
| CN109506674B (zh) | 一种加速度的校正方法及装置 | |
| JP2913122B2 (ja) | ストラップダウン型ジャイロ装置 | |
| JP3425689B2 (ja) | 慣性装置 | |
| RU2754396C1 (ru) | Адаптивный способ коррекции углов ориентации БИНС | |
| US6085149A (en) | Integrated inertial/VMS navigation solution | |
| JPH0949737A (ja) | 航法信号出力方法 | |
| JPH11118507A (ja) | 船舶の慣性航法装置 | |
| JPH06265367A (ja) | 船舶の慣性航法装置 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
| LAPS | Cancellation because of no payment of annual fees |