JPH10176932A - 慣性航法装置 - Google Patents
慣性航法装置Info
- Publication number
- JPH10176932A JPH10176932A JP10628797A JP10628797A JPH10176932A JP H10176932 A JPH10176932 A JP H10176932A JP 10628797 A JP10628797 A JP 10628797A JP 10628797 A JP10628797 A JP 10628797A JP H10176932 A JPH10176932 A JP H10176932A
- Authority
- JP
- Japan
- Prior art keywords
- initial
- azimuth
- model
- inertial navigation
- calculation
- 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
- 230000033001 locomotion Effects 0.000 claims abstract description 21
- 239000011159 matrix material Substances 0.000 claims description 4
- 230000014509 gene expression Effects 0.000 claims description 3
- 238000010586 diagram Methods 0.000 description 25
- 230000003044 adaptive effect Effects 0.000 description 17
- 230000003068 static effect Effects 0.000 description 10
- 230000006641 stabilisation Effects 0.000 description 9
- 238000011105 stabilization Methods 0.000 description 9
- 230000008030 elimination Effects 0.000 description 5
- 238000003379 elimination reaction Methods 0.000 description 5
- 238000005259 measurement Methods 0.000 description 4
- 230000001133 acceleration Effects 0.000 description 3
- 230000000694 effects Effects 0.000 description 2
- 238000000034 method Methods 0.000 description 2
- 206010044565 Tremor Diseases 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 230000010485 coping Effects 0.000 description 1
- 238000011156 evaluation Methods 0.000 description 1
- 230000001788 irregular Effects 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
Landscapes
- Navigation (AREA)
Abstract
(57)【要約】
【課題】 慣性航法装置の初期設定時に、海象状況およ
び船体特性が急激に変化すると、これを補正することが
できず、初期設定が速やかに行えない。 【解決手段】 あらかじめ複数の海象状況および船体特
性を想定して補正演算をモデル化し、実際の海象状況お
よび船体特性を測定してこのモデルの中から最適なもの
を選択し、補正を行う。
び船体特性が急激に変化すると、これを補正することが
できず、初期設定が速やかに行えない。 【解決手段】 あらかじめ複数の海象状況および船体特
性を想定して補正演算をモデル化し、実際の海象状況お
よび船体特性を測定してこのモデルの中から最適なもの
を選択し、補正を行う。
Description
【０００１】
【発明の属する技術分野】本発明は船舶に搭載して利用
する慣性航法装置に関する。本発明は自動操縦装置に利
用するに適する。本発明は船舶用として開発されたもの
であるが、航空機その他に広く用いることができる。
する慣性航法装置に関する。本発明は自動操縦装置に利
用するに適する。本発明は船舶用として開発されたもの
であるが、航空機その他に広く用いることができる。
【０００２】
【従来の技術】慣性航法装置は船舶および航空機その他
に利用されているが、ここでは船舶に搭載されているも
のについて説明する。慣性航法装置はジャイロおよび加
速度計を備え、このジャイロの有する保針性および加速
度計の測定値を基準として船体の速度、方位、加速度、
位置などの航法データを得るものである。
に利用されているが、ここでは船舶に搭載されているも
のについて説明する。慣性航法装置はジャイロおよび加
速度計を備え、このジャイロの有する保針性および加速
度計の測定値を基準として船体の速度、方位、加速度、
位置などの航法データを得るものである。
【０００３】図１６は従来例の慣性航法装置のブロック
構成図であり、特開平７−２７０１７６に開示されてい
る。慣性航法装置を高い精度で使用するためには、正確
な方位および位置が明らかな地点において初期アライメ
ントおよび初期キャリブレーション（以下、初期設定と
いう）を行う必要がある。
構成図であり、特開平７−２７０１７６に開示されてい
る。慣性航法装置を高い精度で使用するためには、正確
な方位および位置が明らかな地点において初期アライメ
ントおよび初期キャリブレーション（以下、初期設定と
いう）を行う必要がある。
【０００４】この初期設定を行うためには、船体を陸上
に設置し、微動だにしない状態で行うことが理想的であ
る。しかし、大型船舶については、これを初期設定のつ
ど陸上に設置することは不可能である。したがって、図
１６に示す動揺外乱除去計算部８により、停泊中の船体
の微動を理論的な手法により除去する。
に設置し、微動だにしない状態で行うことが理想的であ
る。しかし、大型船舶については、これを初期設定のつ
ど陸上に設置することは不可能である。したがって、図
１６に示す動揺外乱除去計算部８により、停泊中の船体
の微動を理論的な手法により除去する。
【０００５】そのままの測定データ（北方向、東方向の
速度データ）をｙとし、この自己回帰モデルをｙm とす
る。ｙm は、
速度データ）をｙとし、この自己回帰モデルをｙm とす
る。ｙm は、
【０００６】
【数３】 Ｌはモデルの次数であり、ａiN(i＝0 〜L)、ａjE(j＝0
〜L)は未知係数パラメータである。この未知パラメータ
ベクトルΘは次の評価関数を最小にするように決められ
る。
〜L)は未知係数パラメータである。この未知パラメータ
ベクトルΘは次の評価関数を最小にするように決められ
る。
【０００７】
【数４】 そして、信号〔外２〕は、
【０００８】
【数５】 で与えられる。ここで、
【０００９】
【数６】 となる。
【００１０】
【外２】 なお、本明細書における各記号の定義は表１に示すとお
りである。
りである。
【００１１】
【表１】
【００１２】
【発明が解決しようとする課題】図１７〜図１９は従来
例の静定カーブを示す図である。横軸に時間をとり、縦
軸に方位角誤差をとる。上式（８）の信号〔外２〕を用
いた初期設定を実行中に、海象状態がＣ１からＣ２に急
変したり、固定次数Ｌのモデルを用いた固定初期アライ
メントおよびキャブレーション機構を有する慣性航法装
置を別の船体特性Ｓ２（例えばＳ１は大型船、Ｓ２は小
型船）の船に装備したとき、初期設定の方位角誤差など
の性能および精度は図１７〜図１９に示すように静定値
および静定時間ともに大きく劣化する。
例の静定カーブを示す図である。横軸に時間をとり、縦
軸に方位角誤差をとる。上式（８）の信号〔外２〕を用
いた初期設定を実行中に、海象状態がＣ１からＣ２に急
変したり、固定次数Ｌのモデルを用いた固定初期アライ
メントおよびキャブレーション機構を有する慣性航法装
置を別の船体特性Ｓ２（例えばＳ１は大型船、Ｓ２は小
型船）の船に装備したとき、初期設定の方位角誤差など
の性能および精度は図１７〜図１９に示すように静定値
および静定時間ともに大きく劣化する。
【００１３】Ｔs1は当初の状態のときの静定時間であ
り、δψs1は静定誤差（静定値）である。図１８は海象
状態がＣ１からＣ２に変化し、図１９は船体特性がＳ１
からＳ２に変化したとき（航法装置を別の船体に移し変
えた場合など）の静定カーブであり、静定時間Ｔs2、Ｔ
s3、静定誤差δψs2、δψs3などは図１７に示す当初の
状態から大きくオーバーし、性能および精度が悪くなっ
ていることが確認される。
り、δψs1は静定誤差（静定値）である。図１８は海象
状態がＣ１からＣ２に変化し、図１９は船体特性がＳ１
からＳ２に変化したとき（航法装置を別の船体に移し変
えた場合など）の静定カーブであり、静定時間Ｔs2、Ｔ
s3、静定誤差δψs2、δψs3などは図１７に示す当初の
状態から大きくオーバーし、性能および精度が悪くなっ
ていることが確認される。
【００１４】本発明は、このような背景に行われたもの
であって、海象状態や船体特性の変化を自動的に判断し
常に最適なモデルを選択決定することができる慣性航法
装置を提供することを目的とする。本発明は、海象状態
や船体特性にかかわらず常に高い精度および性能を得る
ことができる慣性航法装置を提供することを目的とす
る。本発明は、海象状態や船体特性にかかわらず初期設
定を高い精度で短時間に行うことができる慣性航法装置
を提供することを目的とする。本発明は、効率的に安全
な運行を行うことができる慣性航法装置を提供すること
を目的とする。
であって、海象状態や船体特性の変化を自動的に判断し
常に最適なモデルを選択決定することができる慣性航法
装置を提供することを目的とする。本発明は、海象状態
や船体特性にかかわらず常に高い精度および性能を得る
ことができる慣性航法装置を提供することを目的とす
る。本発明は、海象状態や船体特性にかかわらず初期設
定を高い精度で短時間に行うことができる慣性航法装置
を提供することを目的とする。本発明は、効率的に安全
な運行を行うことができる慣性航法装置を提供すること
を目的とする。
【００１５】
【課題を解決するための手段】本発明は、外乱除去用の
演算に用いるモデルを海象状態および船体特性に基づき
あらかじめ複数用意し、実際の船体の動きにしたがって
このモデルの中から最適なモデルを選択することによ
り、幅広くさまざまな海象状態および船体特性に対応し
高い精度により初期設定を行うことを最も主要な特徴と
する。
演算に用いるモデルを海象状態および船体特性に基づき
あらかじめ複数用意し、実際の船体の動きにしたがって
このモデルの中から最適なモデルを選択することによ
り、幅広くさまざまな海象状態および船体特性に対応し
高い精度により初期設定を行うことを最も主要な特徴と
する。
【００１６】すなわち、本発明は慣性航法装置であっ
て、装置の使用に先立って移動体の初期姿勢および初期
方位角を設定する手段と、この設定された初期姿勢およ
び初期方位角にしたがってその移動体の現在姿勢および
現在方位角を出力する手段とを備え、前記設定する手段
は、前記移動体の微動による初期姿勢および初期方位角
の変化を補正演算により除去する手段を備えた慣性航法
装置である。
て、装置の使用に先立って移動体の初期姿勢および初期
方位角を設定する手段と、この設定された初期姿勢およ
び初期方位角にしたがってその移動体の現在姿勢および
現在方位角を出力する手段とを備え、前記設定する手段
は、前記移動体の微動による初期姿勢および初期方位角
の変化を補正演算により除去する手段を備えた慣性航法
装置である。
【００１７】ここで、本発明の特徴とするところは、前
記補正演算をあらかじめ海象状況および船体特性の異な
る複数のモデルについてモデル化し、この複数のモデル
についてそれぞれ演算式を蓄積する手段を備え、前記微
動による初期姿勢および初期方位角の変化を取込み前記
蓄積する手段に蓄積された複数のモデルから最適なモデ
ルを自動的に選択する手段を備えたところにある。
記補正演算をあらかじめ海象状況および船体特性の異な
る複数のモデルについてモデル化し、この複数のモデル
についてそれぞれ演算式を蓄積する手段を備え、前記微
動による初期姿勢および初期方位角の変化を取込み前記
蓄積する手段に蓄積された複数のモデルから最適なモデ
ルを自動的に選択する手段を備えたところにある。
【００１８】前記複数のモデルをｙm i (t )、(i＝1 〜
n)とするとき、この複数のモデルｙm i (t )は、
n)とするとき、この複数のモデルｙm i (t )は、
【００１９】
【数７】 により与えられることが望ましい。ただし、Ｈｉ（ｔ）
は２行２（ｉ＋１）列の行列、Θi は２（ｉ＋１）行１
列の未知パラメータベクトル。
は２行２（ｉ＋１）列の行列、Θi は２（ｉ＋１）行１
列の未知パラメータベクトル。
【００２０】また、前記選択する手段は、モデル選定規
範をＡＩＣとし、
範をＡＩＣとし、
【００２１】
【数８】 について、このＡＩＣの値を最小にする〔外３〕および
次数ｉを含むモデルを選択することが望ましい。
次数ｉを含むモデルを選択することが望ましい。
【００２２】
【外３】 前記自動的に選択する手段は、前記補正演算により除去
する手段を動揺の大きい荒天時には無効にする手段を含
む構成とすることもできる。
する手段を動揺の大きい荒天時には無効にする手段を含
む構成とすることもできる。
【００２３】すなわち、荒天時に激しい動揺が発生する
海象下では、前述したモデル化を行うことが不可能な場
合がある。このような場合には、ジャイロの速度および
位置出力とＧＰＳの速度および位置出力とを比較し、そ
の差をカルマンフィルタ入力とすることにより短時間で
高い精度の初期設定を実現することができる。
海象下では、前述したモデル化を行うことが不可能な場
合がある。このような場合には、ジャイロの速度および
位置出力とＧＰＳの速度および位置出力とを比較し、そ
の差をカルマンフィルタ入力とすることにより短時間で
高い精度の初期設定を実現することができる。
【００２４】
【００２５】
（第一実施例）本発明第一実施例の構成を図１および図
２を参照して説明する。図１は本発明第一実施例装置の
ブロック構成図である。図２は適応型動揺外乱除去計算
部のブロック構成図である。
２を参照して説明する。図１は本発明第一実施例装置の
ブロック構成図である。図２は適応型動揺外乱除去計算
部のブロック構成図である。
【００２６】本発明は、慣性航法装置であって、装置の
使用に先立って船体１の初期姿勢および初期方位角を設
定する手段としてのカルマンフィルタ部１７と、この設
定された初期姿勢および初期方位角にしたがって船体１
の現在姿勢および現在方位角を出力する手段としての速
度位置計算部５および姿勢方位計算部１３とを備え、カ
ルマンフィルタ部１７は、船体１の微動による初期姿勢
および初期方位角の変化を補正演算により除去する手段
としての適応型動揺外乱除去計算部１８の外乱除去計算
部８２を備えた慣性航法装置である。
使用に先立って船体１の初期姿勢および初期方位角を設
定する手段としてのカルマンフィルタ部１７と、この設
定された初期姿勢および初期方位角にしたがって船体１
の現在姿勢および現在方位角を出力する手段としての速
度位置計算部５および姿勢方位計算部１３とを備え、カ
ルマンフィルタ部１７は、船体１の微動による初期姿勢
および初期方位角の変化を補正演算により除去する手段
としての適応型動揺外乱除去計算部１８の外乱除去計算
部８２を備えた慣性航法装置である。
【００２７】ここで、本発明の特徴とするところは、前
記補正演算をあらかじめ海象状況および船体特性の異な
る複数のモデルについてモデル化し、この複数のモデル
についてそれぞれ演算式を蓄積する手段としてのモデル
選定部８１を備え、前記微動による初期姿勢および初期
方位角の変化を取込みモデル選定部８１に蓄積された複
数のモデルから最適なモデルを自動的に選択する手段と
しての状態変化判定部８３およびモデル選定規範部８０
を備えたところにある。
記補正演算をあらかじめ海象状況および船体特性の異な
る複数のモデルについてモデル化し、この複数のモデル
についてそれぞれ演算式を蓄積する手段としてのモデル
選定部８１を備え、前記微動による初期姿勢および初期
方位角の変化を取込みモデル選定部８１に蓄積された複
数のモデルから最適なモデルを自動的に選択する手段と
しての状態変化判定部８３およびモデル選定規範部８０
を備えたところにある。
【００２８】図１の適応型動揺外乱除去計算部１８が本
発明の特徴である。この適応型動揺外乱除去計算部１８
について図３〜図５を参照して説明する。図３〜図５は
本発明第一実施例の適応型動揺外乱除去計算部１８の動
作を示すフローチャートである。図２のモデル選定規範
部８０では以下の計算を行い適切なモデルの次数ｉを選
定する。
発明の特徴である。この適応型動揺外乱除去計算部１８
について図３〜図５を参照して説明する。図３〜図５は
本発明第一実施例の適応型動揺外乱除去計算部１８の動
作を示すフローチャートである。図２のモデル選定規範
部８０では以下の計算を行い適切なモデルの次数ｉを選
定する。
【００２９】この適切なモデルの選定は次のＡＩＣの値
が最も小さいものを採用する。測定ベクトルをｙ（ｔ）
とし、モデルｙm1(t) 、ｙm2(t) 、〜ｙmn(t) は次のｎ
個のモデルがモデル選定部８１で用意される。
が最も小さいものを採用する。測定ベクトルをｙ（ｔ）
とし、モデルｙm1(t) 、ｙm2(t) 、〜ｙmn(t) は次のｎ
個のモデルがモデル選定部８１で用意される。
【００３０】
【数９】 Ｈｉ（ｔ）は２行２（ｉ＋１）列の行列でΘi は、２
（ｉ＋１）行１列の未知パラメータベクトルである。
（ｉ＋１）行１列の未知パラメータベクトルである。
【００３１】
【数１０】 ここで、〔外３〕はＡＩＣを最も小さくするモデルのΘ
で、ｉはそのときの次数でｉ番目のモデル指標でもあ
る。
で、ｉはそのときの次数でｉ番目のモデル指標でもあ
る。
【００３２】Ｆ（Θ）は対数尤度関数で次式で与えられ
る。
る。
【００３３】
【数１１】 ここで、ｆ（ｙ（ｔj ）｜Θ）はｙの条件付確率密度関
数であり、Ｌｎｆ（ｙ（ｔj ）｜Θ）はｆの自然対数で
ある。ここで、ｙは次のような平均値〔外４〕と分散σ
y 2 を有する正規分布であると仮定する（この仮定は妥
当である）。
数であり、Ｌｎｆ（ｙ（ｔj ）｜Θ）はｆの自然対数で
ある。ここで、ｙは次のような平均値〔外４〕と分散σ
y 2 を有する正規分布であると仮定する（この仮定は妥
当である）。
【００３４】図３に示すように、まず、最初のデータ測
定バッチ区間Ｔ0 秒間での測定データ列を ｛ｙ（ｔ1 ）、ｙ（ｔ2 ）、…、ｙ（ｔj ）、…、ｙ
（ｔL ）｝ とするとき、平均値〔外４〕および分散〔外５〕を計算
する（Ｓ１）。
定バッチ区間Ｔ0 秒間での測定データ列を ｛ｙ（ｔ1 ）、ｙ（ｔ2 ）、…、ｙ（ｔj ）、…、ｙ
（ｔL ）｝ とするとき、平均値〔外４〕および分散〔外５〕を計算
する（Ｓ１）。
【００３５】
【数１２】
【００３６】
【外４】
【００３７】
【外５】 すると、ｆ（ｙ（ｔj ）｜Θ）はモデルｙm を用いて次
式で与えられる。 ｆ（ｙ（ｔj ）｜Θ） ＝〔１／〔（２π）2 ｄｅｔＶｙ〕1/2 〕ｅｘｐ 〔（−１／２）（ｙm （ｔj ）−Ｍｙ）T Ｖｙ-1（ｙm −Ｍｙ）〕 （１９） ＝〔１／〔（２π）2 ｄｅｔＶｙ〕1/2 〕ｅｘｐ 〔（−１／２）（Ｈ（ｔj ）Θ−Ｍｙ）T Ｖｙ-1（Ｈ（ｔj ）Θ−Ｍｙ）〕 （２０） （１５）式の〔外３〕は、Ｆ（Θ）を最大にする（最大
対数尤度）Θとして求まる。つまり、（１６）式の偏微
分
式で与えられる。 ｆ（ｙ（ｔj ）｜Θ） ＝〔１／〔（２π）2 ｄｅｔＶｙ〕1/2 〕ｅｘｐ 〔（−１／２）（ｙm （ｔj ）−Ｍｙ）T Ｖｙ-1（ｙm −Ｍｙ）〕 （１９） ＝〔１／〔（２π）2 ｄｅｔＶｙ〕1/2 〕ｅｘｐ 〔（−１／２）（Ｈ（ｔj ）Θ−Ｍｙ）T Ｖｙ-1（Ｈ（ｔj ）Θ−Ｍｙ）〕 （２０） （１５）式の〔外３〕は、Ｆ（Θ）を最大にする（最大
対数尤度）Θとして求まる。つまり、（１６）式の偏微
分
【００３８】
【数１３】 より求まる（Ｓ３）。（２０）式より、 Ｌｎｆ（ｙ（ｔj ）｜Θ） ＝−Ｌｎ〔（２π）2 ｄｅｔＶｙ〕1/2 − （１／２）（Ｈ（ｔj ）Θ−Ｍｙ）T Ｖｙ-1（Ｈ（ｔj ）Θ−Ｍｙ）（２２） が得られる。
【００３９】（２２）式をΘで偏微分した結果のΘに関
する方程式を解いて〔外３〕が得られる（Ｓ２）。この
〔外３〕を（９）式〜（１２）式の各モデルについて求
めて（１５）式のＡＩＣを最小にするモデルを選択決定
すればよい（Ｓ３、Ｓ４）。図４はｉ番目のモデルが決
定された場合である（Ｓ５）。
する方程式を解いて〔外３〕が得られる（Ｓ２）。この
〔外３〕を（９）式〜（１２）式の各モデルについて求
めて（１５）式のＡＩＣを最小にするモデルを選択決定
すればよい（Ｓ３、Ｓ４）。図４はｉ番目のモデルが決
定された場合である（Ｓ５）。
【００４０】このモデル決定（あるいは次数決定）は最
初のデータ取得バッチ区間のみの１回限りであり、各バ
ッチ区間で〔外３〕および〔外２〕を求める計算が行わ
れるが（Ｓ６）、海象状態あるいは船体特性が変化しな
い限り新たなモデル決定は行われない。
初のデータ取得バッチ区間のみの１回限りであり、各バ
ッチ区間で〔外３〕および〔外２〕を求める計算が行わ
れるが（Ｓ６）、海象状態あるいは船体特性が変化しな
い限り新たなモデル決定は行われない。
【００４１】上記方法で、ｉ番目のモデルが決定される
と従来と同じ方法で外乱除去計算部で信号〔外２〕が得
られる。
と従来と同じ方法で外乱除去計算部で信号〔外２〕が得
られる。
【００４２】この〔外２〕と生の測定データを用いてε
ｙを求め、その分散Ｖεを計算し、分散Ｖεからそのト
レースＴｒ〔Ｖε〕を求め、モデルと真の状態のずれの
判定閾値Ｌεと比較する。
ｙを求め、その分散Ｖεを計算し、分散Ｖεからそのト
レースＴｒ〔Ｖε〕を求め、モデルと真の状態のずれの
判定閾値Ｌεと比較する。
【００４３】閾値より大きければ（Ｓ７）、海象状態あ
るいは船体特性が変化したと判断し（Ｓ８）、このとき
の海象状態あるいは船体特性に見合ったモデルを再選定
する。
るいは船体特性が変化したと判断し（Ｓ８）、このとき
の海象状態あるいは船体特性に見合ったモデルを再選定
する。
【００４４】小さければ変化無しと判断し（Ｓ８）、
〔外２〕を出力する。このようにして、いかなる海象状
態変化および船体特性変化にも対処し、常時最適な性能
と精度を有する初期設定を実現することができる。
〔外２〕を出力する。このようにして、いかなる海象状
態変化および船体特性変化にも対処し、常時最適な性能
と精度を有する初期設定を実現することができる。
【００４５】図６〜図８は本発明第一実施例の静定カー
ブを示す図である。横軸に時間をとり、縦軸に方位角誤
差をとる。初期設定を実行中に、海象状態がＣ１からＣ
２に急変しても、慣性航法装置を別の船体特性Ｓ２（例
えばＳ１は大型船、Ｓ２は小型船）の船に装備しても、
初期設定の方位角誤差などの性能および精度は図６〜図
８に示すように静定値および静定時間ともにほとんど一
定である。
ブを示す図である。横軸に時間をとり、縦軸に方位角誤
差をとる。初期設定を実行中に、海象状態がＣ１からＣ
２に急変しても、慣性航法装置を別の船体特性Ｓ２（例
えばＳ１は大型船、Ｓ２は小型船）の船に装備しても、
初期設定の方位角誤差などの性能および精度は図６〜図
８に示すように静定値および静定時間ともにほとんど一
定である。
【００４６】Ｔs1は当初の状態のときの静定時間であ
り、δψs1は静定誤差（静定値）である。図７は海象状
態がＣ１からＣ２に変化し、図８は船体特性がＳ１から
Ｓ２に変化したときの静定カーブであり、静定時間
Ｔs2、Ｔs3、静定誤差δψs2、δψs3などは図６に示す
当初の状態と比較してもほとんど変化がなく、本発明の
効果が顕著に現れている。
り、δψs1は静定誤差（静定値）である。図７は海象状
態がＣ１からＣ２に変化し、図８は船体特性がＳ１から
Ｓ２に変化したときの静定カーブであり、静定時間
Ｔs2、Ｔs3、静定誤差δψs2、δψs3などは図６に示す
当初の状態と比較してもほとんど変化がなく、本発明の
効果が顕著に現れている。
【００４７】（第二実施例）本発明第二実施例を図９な
いし図１５を参照して説明する。図９は本発明第二実施
例の慣性航法装置のブロック構成図である。図１０は本
発明第二実施例の適応型動揺外乱除去計算部１８のブロ
ック構成図である。本発明第二実施例では、本発明第一
実施例で説明したモデル化が行えないくらいの激しい動
揺が発生する海象下において、適用型動揺外乱除去計算
部１８の状態変化判定部８３は、適応型動揺外乱除去計
算部１８の機能をスイッチ２０の操作により慣性航法装
置から切り離すことを特徴とする。
いし図１５を参照して説明する。図９は本発明第二実施
例の慣性航法装置のブロック構成図である。図１０は本
発明第二実施例の適応型動揺外乱除去計算部１８のブロ
ック構成図である。本発明第二実施例では、本発明第一
実施例で説明したモデル化が行えないくらいの激しい動
揺が発生する海象下において、適用型動揺外乱除去計算
部１８の状態変化判定部８３は、適応型動揺外乱除去計
算部１８の機能をスイッチ２０の操作により慣性航法装
置から切り離すことを特徴とする。
【００４８】すなわち、本発明第一実施例で示した適応
型動揺外乱除去計算部を設けた慣性航法装置は、さまざ
まな海象状態および船体特性に対応することができる優
れた装置であるが、荒天時などの激しい動揺の下にさら
される環境においては、初期設定の精度および初期設定
の静定時間などの観点から満足することができない場合
がある。その主な理由は前述したモデル化が行えないと
ころにある。
型動揺外乱除去計算部を設けた慣性航法装置は、さまざ
まな海象状態および船体特性に対応することができる優
れた装置であるが、荒天時などの激しい動揺の下にさら
される環境においては、初期設定の精度および初期設定
の静定時間などの観点から満足することができない場合
がある。その主な理由は前述したモデル化が行えないと
ころにある。
【００４９】図１に示すカルマンフィルタ部１７では、
【００５０】
【数１４】 となり、また、適応型動揺外乱除去計算部１８では、
【００５１】
【数１５】 となることから、カルマンフィルタ部１７の能力として
は、
は、
【００５２】
【数１６】 となる。
【００５３】これによれば、荒天時には、各推定誤差値
が大きくなり、各誤差推定値は静定しないことがわか
る。この状態を図１１および図１２に示す。図１１は通
常の動揺環境下での初期設定の精度および静定時間を示
す図である。図１２は荒天時の動揺環境下での初期設定
の精度および静定時間を示す図である。
が大きくなり、各誤差推定値は静定しないことがわか
る。この状態を図１１および図１２に示す。図１１は通
常の動揺環境下での初期設定の精度および静定時間を示
す図である。図１２は荒天時の動揺環境下での初期設定
の精度および静定時間を示す図である。
【００５４】この主な理由としては、〔数１５〕に示す
ように、荒天時には高次成分Ｖｈが含まれるため、適応
型動揺外乱除去計算部１８が変化に追従しきれないため
である。すなわち、高次成分Ｖｈは高次であるためにモ
デル化しきれない。
ように、荒天時には高次成分Ｖｈが含まれるため、適応
型動揺外乱除去計算部１８が変化に追従しきれないため
である。すなわち、高次成分Ｖｈは高次であるためにモ
デル化しきれない。
【００５５】このような場合には、ジャイロとともに船
体１に設けられているＧＰＳからの速度および位置信号
とジャイロからの速度および位置信号とを比較し、その
差をカルマンフィルタ部１７の入力とすることにより、
荒天時の激しい動揺下でも短時間で高精度な初期設定を
行うことができる。
体１に設けられているＧＰＳからの速度および位置信号
とジャイロからの速度および位置信号とを比較し、その
差をカルマンフィルタ部１７の入力とすることにより、
荒天時の激しい動揺下でも短時間で高精度な初期設定を
行うことができる。
【００５６】本発明第二実施例の動作を図１３を参照し
て説明する。図１３は本発明第二実施例の動作を示すフ
ローチャートである。まず、ジャイロデータが姿勢方位
計算部１３に入力される（Ｓ１１）。加速度計からの加
速度データが入力され、速度および位置が計算される
（Ｓ１２）。続いて、ＧＰＳから速度Ｖnrおよび位置Ｐ
nrデータがそれぞれ入力される（Ｓ１３）。ここで、Ｇ
ＰＳの速度Ｖnrおよび位置Ｐnrデータとジャイロの速度
Ｖn および位置Ｐn データとの差ｙ1 およびｙ2をそれ
ぞれ計算する。
て説明する。図１３は本発明第二実施例の動作を示すフ
ローチャートである。まず、ジャイロデータが姿勢方位
計算部１３に入力される（Ｓ１１）。加速度計からの加
速度データが入力され、速度および位置が計算される
（Ｓ１２）。続いて、ＧＰＳから速度Ｖnrおよび位置Ｐ
nrデータがそれぞれ入力される（Ｓ１３）。ここで、Ｇ
ＰＳの速度Ｖnrおよび位置Ｐnrデータとジャイロの速度
Ｖn および位置Ｐn データとの差ｙ1 およびｙ2をそれ
ぞれ計算する。
【００５７】この差ｙ1 およびｙ2 をカルマンフィルタ
部１７に入力することにより（Ｓ１４）、速度および位
置誤差と姿勢および方位誤差の修正量を計算することが
できる（Ｓ１５）。以下に、カルマンフィルタ部１７に
おける計算を示す。なお、カルマンフィルタ部１７の構
造は、本発明第一実施例とは若干異なっており、測定デ
ータに位置データを追加している。
部１７に入力することにより（Ｓ１４）、速度および位
置誤差と姿勢および方位誤差の修正量を計算することが
できる（Ｓ１５）。以下に、カルマンフィルタ部１７に
おける計算を示す。なお、カルマンフィルタ部１７の構
造は、本発明第一実施例とは若干異なっており、測定デ
ータに位置データを追加している。
【００５８】
【数１７】 ここで、δＶnrとδＰnrはＧＰＳの速度および位置の不
規則ノイズである。式（２６）〜式（３１）より、カル
マンフィルタ入力であるｙ1 およびｙ2 には動揺による
速度外乱値Ｖd および位置外乱値Ｐd は混入しないこと
がわかる。よって、ΔＶn およびΔＷの推定には船体動
揺外乱（Ｖd 、Ｐd ）は影響しないことがわかる。
規則ノイズである。式（２６）〜式（３１）より、カル
マンフィルタ入力であるｙ1 およびｙ2 には動揺による
速度外乱値Ｖd および位置外乱値Ｐd は混入しないこと
がわかる。よって、ΔＶn およびΔＷの推定には船体動
揺外乱（Ｖd 、Ｐd ）は影響しないことがわかる。
【００５９】ここで、ΔＶ、ΔＰは加速度計誤差ΔＡ、
ジャイロ誤差ΔＷによる速度誤差、位置誤差である。カ
ルマンゲインは加速度計不規則誤差ΔＡr 、ジャイロ不
規則誤差ΔＷr の影響を最小にするように決定される。
ジャイロ誤差ΔＷによる速度誤差、位置誤差である。カ
ルマンゲインは加速度計不規則誤差ΔＡr 、ジャイロ不
規則誤差ΔＷr の影響を最小にするように決定される。
【００６０】図１４は本発明第四実施例の原理を説明す
るための図であるが、本発明第四実施例の原理をさらに
視覚的に説明すると図１４に示すとおりである。図１５
は本発明第四実施例による初期設定特性を示す図である
が、これにより、図１５に示すように荒天時の動揺環境
下においても初期設定を短時間に高い精度で行うことが
できる。
るための図であるが、本発明第四実施例の原理をさらに
視覚的に説明すると図１４に示すとおりである。図１５
は本発明第四実施例による初期設定特性を示す図である
が、これにより、図１５に示すように荒天時の動揺環境
下においても初期設定を短時間に高い精度で行うことが
できる。
【００６１】
【発明の効果】以上説明したように、本発明によれば、
海象状態や船体特性の変化を自動的に判断し常に最適な
モデルを選択決定することができる。また、海象状態や
船体特性にかかわらず常に高い精度および性能を得るこ
とができる。さらに、海象状態や船体特性にかかわらず
初期設定を高い精度で短時間に行うことができる。これ
により、効率的に安全な運行を行うことができる。
海象状態や船体特性の変化を自動的に判断し常に最適な
モデルを選択決定することができる。また、海象状態や
船体特性にかかわらず常に高い精度および性能を得るこ
とができる。さらに、海象状態や船体特性にかかわらず
初期設定を高い精度で短時間に行うことができる。これ
により、効率的に安全な運行を行うことができる。
【図１】本発明第一実施例装置のブロック構成図。
【図２】適応型動揺外乱除去計算部のブロック構成図。
【図３】本発明第一実施例の適応型動揺外乱除去計算部
の動作を示すフローチャート。
の動作を示すフローチャート。
【図４】本発明第一実施例の適応型動揺外乱除去計算部
の動作を示すフローチャート。
の動作を示すフローチャート。
【図５】本発明第一実施例の適応型動揺外乱除去計算部
の動作を示すフローチャート。
の動作を示すフローチャート。
【図６】本発明第一実施例の静定カーブを示す図。
【図７】本発明第一実施例の静定カーブを示す図。
【図８】本発明第一実施例の静定カーブを示す図。
【図９】本発明第二実施例の慣性航法装置のブロック構
成図。
成図。
【図１０】本発明第二実施例の適応型動揺外乱除去計算
部のブロック構成図。
部のブロック構成図。
【図１１】通常の動揺環境下での初期設定の精度および
静定時間を示す図。
静定時間を示す図。
【図１２】荒天時の動揺環境下での初期設定の精度およ
び静定時間を示す図。
び静定時間を示す図。
【図１３】本発明第二実施例の動作を示すフローチャー
ト。
ト。
【図１４】本発明第四実施例の原理を説明するための
図。
図。
【図１５】本発明第四実施例による初期設定特性を示す
図。
図。
【図１６】従来例の慣性航法装置のブロック構成図。
【図１７】従来例の静定カーブを示す図。
【図１８】従来例の静定カーブを示す図。
【図１９】従来例の静定カーブを示す図。
【符号の説明】 １ 船体 ２、６ 入出力部 ３ 加速度計誤差補正計算部 ４ 加速度座標変換計算部 ５ 速度位置計算部 ７ 入出力データ表示装置 ８ 動揺外乱除去計算部 ９、１０ 加算器 １１ ジャイロ誤差補正計算部 １２ 座標変換行列計算部 １３ 姿勢方位計算部 １４ 修正量計算部 １５ 出力誤差推定計算部 １６ ＩＭＵ誤差推定計算部 １７ カルマンフィルタ部 １８ 適応型動揺外乱除去計算部 ２０ スイッチ ８０ モデル選定規範部 ８１ モデル選定部 ８２ 外乱除去計算部 ８３ 状態変化判定部
Claims (4)
- 【請求項１】 装置の使用に先立って移動体の初期姿勢
および初期方位角を設定する手段と、この設定された初
期姿勢および初期方位角にしたがってその移動体の現在
姿勢および現在方位角を出力する手段とを備え、 前記設定する手段は、その移動体の微動による初期姿勢
および初期方位角の変化を補正演算により除去する手段
を備えた慣性航法装置において、 前記補正演算をあらかじめ海象状況および船体特性の異
なる複数のモデルについてモデル化し、この複数のモデ
ルについてそれぞれ演算式を蓄積する手段を備え、 前記微動による初期姿勢および初期方位角の変化を取込
み前記蓄積する手段に蓄積された複数のモデルから最適
なモデルを自動的に選択する手段を備えたことを特徴と
する慣性航法装置。 - 【請求項２】 前記複数のモデルをｙm i (t )、(i＝1
〜n)とするとき、この複数のモデルｙm i (t )は、 【数１】 により与えられる請求項１記載の慣性航法装置。ただ
し、Ｈｉ（ｔ）は２行２（ｉ＋１）列の行列、Θi は２
（ｉ＋１）行１列の未知パラメータベクトルである。 - 【請求項３】 前記選択する手段は、モデル選定規範を
ＡＩＣとし、 【数２】 について、このＡＩＣの値を最小にする〔外１〕および
次数ｉを含むモデルを選択する請求項２記載の慣性航法
装置。 【外１】 - 【請求項４】 前記自動的に選択する手段は、前記補正
演算により除去する手段を動揺の大きい荒天時には無効
にする手段を含む請求項１記載の慣性航法装置。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP10628797A JPH10176932A (ja) | 1996-10-14 | 1997-04-23 | 慣性航法装置 |
Applications Claiming Priority (3)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP8-271017 | 1996-10-14 | ||
| JP27101796 | 1996-10-14 | ||
| JP10628797A JPH10176932A (ja) | 1996-10-14 | 1997-04-23 | 慣性航法装置 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| JPH10176932A true JPH10176932A (ja) | 1998-06-30 |
Family
ID=26446405
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP10628797A Pending JPH10176932A (ja) | 1996-10-14 | 1997-04-23 | 慣性航法装置 |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JPH10176932A (ja) |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2011174771A (ja) * | 2010-02-24 | 2011-09-08 | Clarion Co Ltd | 位置推定装置および位置推定方法 |
| JP2012193965A (ja) * | 2011-03-15 | 2012-10-11 | Mitsubishi Electric Corp | 位置標定装置、位置標定装置の位置標定方法および位置標定プログラム |
-
1997
- 1997-04-23 JP JP10628797A patent/JPH10176932A/ja active Pending
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2011174771A (ja) * | 2010-02-24 | 2011-09-08 | Clarion Co Ltd | 位置推定装置および位置推定方法 |
| JP2012193965A (ja) * | 2011-03-15 | 2012-10-11 | Mitsubishi Electric Corp | 位置標定装置、位置標定装置の位置標定方法および位置標定プログラム |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109443379B (zh) | 一种深海潜航器的sins/dvl水下抗晃动对准方法 | |
| CN106123921B (zh) | 动态干扰条件下捷联惯导系统的纬度未知自对准方法 | |
| Fossen et al. | Nonlinear output feedback control of dynamically positioned ships using vectorial observer backstepping | |
| US20220404152A1 (en) | Motion constraint-aided underwater integrated navigation method employing improved sage-husa adaptive filtering | |
| RU2348903C1 (ru) | Способ определения навигационных параметров бесплатформенной инерциальной навигационной системой | |
| CN106643723B (zh) | 一种无人艇安全航行船位推算方法 | |
| US11947364B2 (en) | Vessel anti-rolling control apparatus and anti-rolling control method | |
| CN102252677A (zh) | 一种基于时间序列分析的变比例自适应联邦滤波方法 | |
| CN113608534A (zh) | 一种无人艇跟踪控制方法及系统 | |
| CN114111771B (zh) | 一种双轴稳定平台的动态姿态测量方法 | |
| CN120403663B (zh) | 一种水下航行器多源信息融合精确导航方法及系统 | |
| Davari et al. | Multirate adaptive Kalman filter for marine integrated navigation system | |
| CN113790724B (zh) | 一种基于速度阻尼的惯性/多普勒组合导航方法及系统 | |
| Dukan et al. | Integration filter for APS, DVL, IMU and pressure gauge for underwater vehicles | |
| RU2749152C1 (ru) | Адаптивный корректор углов ориентации для БИНС | |
| JP2007232443A (ja) | 慣性航法装置およびその誤差補正方法 | |
| CN115096298B (zh) | 基于自适应梯度下降法的旋翼飞行器组合导航方法及系统 | |
| CN103109159A (zh) | 用于对位置测量装置中的漂移进行补偿的方法 | |
| CN115014222A (zh) | 一种基于宽带卡尔曼滤波器的船体变形测量装置及其测量方法 | |
| CN118816891B (zh) | 一种非完整性约束辅助的sins/eml组合导航方法、程序、设备及存储介质 | |
| CN119469094A (zh) | 一种光纤罗经和磁罗经一体化校准方法 | |
| CN117553828B (zh) | 一种水上动基座惯性导航系统初始粗对准方法 | |
| CN112629540B (zh) | 一种基于载体姿态信息的升沉测量方法 | |
| CN117470233A (zh) | 一种优化的地球椭球模型下横坐标系组合导航方法 | |
| Gryazin et al. | Development of a MEMS-based electronic inclinometer free from the effect of translational accelerations |