JPH07270176A - Inertial navigation apparatus of ship - Google Patents
Inertial navigation apparatus of shipInfo
- Publication number
- JPH07270176A JPH07270176A JP6135494A JP6135494A JPH07270176A JP H07270176 A JPH07270176 A JP H07270176A JP 6135494 A JP6135494 A JP 6135494A JP 6135494 A JP6135494 A JP 6135494A JP H07270176 A JPH07270176 A JP H07270176A
- Authority
- JP
- Japan
- Prior art keywords
- error
- speed
- calculation unit
- output
- 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.)
- Granted
Links
Landscapes
- Navigation (AREA)
- Traffic Control Systems (AREA)
Abstract
Description
【0001】[0001]
【産業上の利用分野】本発明は船舶の姿勢、方位、速度
および位置の検出に利用する。特に、これらの検出値を
出力するストラップダウン方式の慣性航法装置に関す
る。さらに詳しくは、このような慣性航法装置の初期姿
勢および方位の決定に関する。BACKGROUND OF THE INVENTION The present invention is used to detect the attitude, heading, speed and position of a ship. In particular, the present invention relates to a strapdown inertial navigation device that outputs these detection values. More particularly, it relates to determining the initial attitude and heading of such an inertial navigation system.
【0002】[0002]
【従来の技術】図2は従来の船舶用慣性航法装置の一例
を示すブロック構成図である。この慣性航法装置は特願
平5−52450(本願出願時未公開)に示されたもの
であり、IMU部11と、入出力回路12と、加速度計
誤差補正計算部21、ジャイロ誤差補正計算部22、加
速度座標変換計算部23、座標変換行列計算部24、修
正量計算部25、速度位置計算部26、姿勢方位計算部
27、減算回路28、29、カルマンフィルタ部30、
動揺外乱加速度同定計算部33、フィードフォワード補
正計算部34および速度計算部35からなるコンピュー
タ部と、入出力回路41と、入出力データ表示装置42
とを備える。カルマンフィルタ部30は、出力誤差推定
計算部31とIMU誤差推定計算部32とを備える。2. Description of the Related Art FIG. 2 is a block diagram showing an example of a conventional inertial navigation system for ships. This inertial navigation device is shown in Japanese Patent Application No. 5-52450 (not yet published at the time of filing of the present application), and includes an IMU unit 11, an input / output circuit 12, an accelerometer error correction calculation unit 21, and a gyro error correction calculation unit. 22, an acceleration coordinate conversion calculation unit 23, a coordinate conversion matrix calculation unit 24, a correction amount calculation unit 25, a velocity position calculation unit 26, a posture direction calculation unit 27, subtraction circuits 28 and 29, a Kalman filter unit 30,
A computer unit including a shake disturbance acceleration identification calculation unit 33, a feedforward correction calculation unit 34, and a velocity calculation unit 35, an input / output circuit 41, and an input / output data display device 42.
With. The Kalman filter unit 30 includes an output error estimation calculation unit 31 and an IMU error estimation calculation unit 32.
【0003】ここで、この従来例の動作を説明する前
に、この装置で用いる座標について図3を参照して説明
する。船舶用の慣性航法装置で用いられる座標には、船
体座標と航法座標(または航法計算座標)とがある。船
体座標は、ロール、ピッチおよびヨーの各軸成分によ
り、 (bx ,by ,bz ) と表される座標であり、船体中心を原点とする。これに
対して航法座標はn座標とも呼ばれ、北、東および高さ
方向の各成分により、 (xN ,yE ,zD ) と表される座標である。航法座標もまた船体中心を原点
とするが、その船上の点の座標は船の移動に伴って移動
する。ロール軸のまわりの回転角をロール角φ、ピッチ
軸まわりの回転角をピッチ角θ、船の向きを方位角ψと
いう。方位角ψは北向きのときψ=0、東向きのときψ
=90°である。Before describing the operation of this conventional example, the coordinates used in this apparatus will be described with reference to FIG. The coordinates used in the inertial navigation system for ships include hull coordinates and navigation coordinates (or navigation calculation coordinates). Hull coordinates, roll, by each axial component of the pitch and yaw, the coordinate represented as (b x, b y, b z), the hull center as the origin. On the other hand, the navigation coordinates are also called n coordinates, and are coordinates represented by (x N , y E , z D ) by the respective components in the north, east and height directions. The navigation coordinates also have the center of the hull as the origin, but the coordinates of points on the ship move as the ship moves. The rotation angle around the roll axis is called the roll angle φ, the rotation angle around the pitch axis is called the pitch angle θ, and the direction of the ship is called the azimuth angle ψ. Azimuth ψ is ψ = 0 when facing north, ψ when facing east
= 90 °.
【0004】次に、従来例の各部の動作について表1に
示す記号を用いて説明する。Next, the operation of each part of the conventional example will be described using the symbols shown in Table 1.
【表1】 [Table 1]
【0005】IMU部11は加速度計とジャイロとによ
り構成され、加速度計は船の運動加速度を計測して加速
度ベクトルを出力し、ジャイロは船の運動角速度を計測
して回転角速度ベクトルを出力する。ストラップダウン
方式では、このIMU部11が直接に船体基準軸に取り
つけられる。取り付け位置は可能な限り船体重心の近く
に設定される。The IMU unit 11 is composed of an accelerometer and a gyro. The accelerometer measures the motion acceleration of the ship and outputs an acceleration vector, and the gyro measures the motion angular velocity of the ship and outputs a rotational angular velocity vector. In the strapdown system, this IMU unit 11 is directly attached to the hull reference shaft. The mounting position is set as close to the center of gravity of the ship as possible.
【0006】加速度計の出力する加速度ベクトルは、ロ
ール軸、ピッチ軸およびヨー軸の各方向の加速度Abx、
AbxおよびAbzを要素とする船体座標で表現され、船の
推進加速度ベクトル、加速度誤差ベクトルおよび動揺加
速度ベクトルのベクトルの和により表される。このうち
動揺加速度ベクトルは、初期アライメント中に外乱加速
度となり、姿勢決定時間および決定精度に大きな影響を
及ぼす。初期アライメントとは、加速度計出力の水平成
分を用いて初期の姿勢および方位角をできるだけ正確に
決定することをいう。この初期アライメントのときに、
ジャイロおよび加速度計のそれぞれの誤差の一部につい
ても補正する。これをキャリブレーションという。The acceleration vector output from the accelerometer is the acceleration A bx in each direction of the roll axis, pitch axis and yaw axis.
It is represented by hull coordinates having A bx and A bz as elements, and is represented by the sum of the propulsion acceleration vector, the acceleration error vector, and the shaking acceleration vector of the ship. Of these, the wobbling acceleration vector becomes a disturbance acceleration during the initial alignment, and has a great influence on the posture determination time and the determination accuracy. Initial alignment refers to determining the initial attitude and azimuth as accurately as possible using the horizontal component of the accelerometer output. During this initial alignment,
Some of the errors in the gyro and accelerometer are also corrected. This is called calibration.
【0007】ジャイロの出力する回転角速度ベクトルも
また船体座標で表現され、船体軸ロール角速度Wbx、船
体軸ピッチ角速度Wby、船体軸ヨー角速度Wbzを要素と
する。回転角速度ベクトルは旋回角速度ベクトル、ジャ
イロ誤差ベクトルおよび動揺角速度ベクトルの和として
表される。The rotational angular velocity vector output from the gyro is also expressed in hull coordinates, and has hull axis roll angular velocity W bx , hull axial pitch angular velocity W by , and hull axial yaw angular velocity W bz as elements. The rotation angular velocity vector is represented as the sum of the turning angular velocity vector, the gyro error vector, and the fluctuation angular velocity vector.
【0008】加速度計およびジャイロの出力は、入出力
回路12を介して、それぞれ加速度計誤差補正計算部2
1およびジャイロ誤差補正計算部22に入力される。加
速度計誤差補正計算部21は、カルマンフィルタ部30
内のIMU誤差推定計算部32からの加速度計誤差推定
出力により加速度ベクトルを補正し、補正出力を加速度
座標変換計算部23に出力する。ジャイロ誤差補正計算
部22も同様に、IMU誤差推定計算部32からのジャ
イロ誤差推定出力により回転角速度ベクトルを補正す
る。The outputs of the accelerometer and the gyro are input / output circuit 12 to the accelerometer error correction calculation unit 2 respectively.
1 and the gyro error correction calculation unit 22. The accelerometer error correction calculation unit 21 includes a Kalman filter unit 30.
The acceleration vector is corrected by the accelerometer error estimation output from the IMU error estimation / calculation unit 32 in the above, and the corrected output is output to the acceleration coordinate conversion calculation unit 23. Similarly, the gyro error correction calculation unit 22 corrects the rotational angular velocity vector based on the gyro error estimation output from the IMU error estimation calculation unit 32.
【0009】加速度計誤差補正計算部21の出力は加速
度座標変換計算部23に入力される。加速度座標変換計
算部23は、加速度計誤差補正計算部21の出力を船体
座標から航法座標であるn座標に変換する。この変換の
ための座標変換行列は、座標変換行列計算部24におい
て、ジャイロ誤差補正計算部22の出力したジャイロ誤
差補正出力と、速度位置計算部26により求められた船
の速度と、修正量計算部25で求めた姿勢角誤差および
方位角誤差の修正量とにより求められる。The output of the accelerometer error correction calculator 21 is input to the acceleration coordinate conversion calculator 23. The acceleration coordinate conversion calculation unit 23 converts the output of the accelerometer error correction calculation unit 21 from ship coordinates to n coordinates which are navigation coordinates. The coordinate conversion matrix for this conversion is calculated in the coordinate conversion matrix calculation unit 24 by the gyro error correction output output from the gyro error correction calculation unit 22, the speed of the ship obtained by the speed position calculation unit 26, and the correction amount calculation. It is obtained by the attitude angle error and the correction amount of the azimuth angle error obtained by the unit 25.
【0010】よく知られているように、ジャイロは船の
運動角速度(旋回角速度+動揺角速度)のみならず、絶
対静止座標に対するすべての角速度を計測する。具体的
には、地球の自転角速度、船が丸い地球の周辺を移動す
る移動角速度すなわち緯度および経度の変化率、などに
ついても計測する。しかし、航法計算や船の姿勢および
方位は地球に対する運動と姿勢角および方位角とから表
現されるので、ジャイロ出力から地球の自転角速度と移
動角速度とを差し引いて補正する必要がある。このよう
な補正量を修正量計算部25で求める。修正量計算部2
5はまた、水平方向速度および高度方向の誤差の修正量
についても求める。As is well known, the gyro measures not only the angular velocity of the ship (turning angular velocity + swing angular velocity) but also all angular velocities with respect to absolute stationary coordinates. Specifically, the rotational angular velocity of the earth, the angular velocity of movement of the ship around the round earth, that is, the rate of change in latitude and longitude are also measured. However, since the navigation calculation and the attitude and azimuth of the ship are expressed by the motion with respect to the earth and the attitude angle and azimuth, it is necessary to correct the gyro output by subtracting the rotation angular velocity and the moving angular velocity of the earth. The correction amount calculator 25 calculates such a correction amount. Correction amount calculation unit 2
5 also determines the correction amount of the error in the horizontal speed and the altitude.
【0011】速度位置計算部26は、航法座標における
加速度を積分してその船の速度および位置(緯度および
経度)を求める。船の場合には速度および位置に高度方
向の情報は直接には不要であるが、水平方向への影響が
あるため、修正量計算部25により求めた高度方向誤差
修正量に基づいて、高さおよび高度方向の速度を求めて
おく。速度位置計算部26の求めた速度ベクトルおよび
位置ベクトルは、入出力回路41を介して入出力データ
表示装置42に出力される。The speed position calculation unit 26 integrates the acceleration in the navigation coordinates to obtain the speed and position (latitude and longitude) of the ship. In the case of a ship, it is not necessary to directly obtain information about the altitude in the speed and position, but since it has an influence in the horizontal direction, the height based on the altitude error correction amount obtained by the correction amount calculation unit 25 is used. And the speed in the altitude direction is obtained. The velocity vector and the position vector obtained by the velocity position calculator 26 are output to the input / output data display device 42 via the input / output circuit 41.
【0012】一方、姿勢方位計算部27は、座標変換行
列計算部24の出力した座標変換行列から姿勢角(φ,
θ)と方位角ψとを計算し、入出力回路41を介して入
出力データ表示装置42に出力する。On the other hand, the posture azimuth calculation unit 27 calculates the posture angle (φ, φ from the coordinate conversion matrix output from the coordinate conversion matrix calculation unit 24).
θ) and the azimuth angle ψ are calculated and output to the input / output data display device 42 via the input / output circuit 41.
【0013】速度位置計算部26により得られた速度ベ
クトルおよび位置ベクトルは、コンピュータ部から出力
されるだけでなく、コンピュータ部内の減算回路28、
29に入力される。減算回路28、29にはさらに、入
出力回路41を介してそれぞれ外部基準速度および外部
基準位置が入力される。減算回路28、29は、これら
を比較し、その差を出力誤差推定計算部31およびIM
U誤差推定計算部32の双方に入力する。The velocity vector and the position vector obtained by the velocity position calculation unit 26 are not only output from the computer unit but also a subtraction circuit 28 in the computer unit,
29 is input. The external reference speed and the external reference position are input to the subtraction circuits 28 and 29 via the input / output circuit 41, respectively. The subtraction circuits 28 and 29 compare these, and the difference is output error estimation calculation unit 31 and IM.
It is input to both of the U error estimation calculation units 32.
【0014】出力誤差推定計算部31は、速度誤差、位
置誤差、姿勢角誤差および方位角誤差を推定し、推定値
を修正量計算部25に出力する。IMU誤差推定計算部
32はジャイロ誤差および加速度誤差を推定し、それぞ
れ加速度計誤差補正計算部21、ジャイロ誤差補正計算
部22に出力する。The output error estimation / calculation unit 31 estimates the velocity error, the position error, the attitude angle error, and the azimuth angle error, and outputs the estimated value to the correction amount calculation unit 25. The IMU error estimation / calculation unit 32 estimates the gyro error and the acceleration error, and outputs them to the accelerometer error correction calculation unit 21 and the gyro error correction calculation unit 22, respectively.
【0015】初期アライメント時には、外部基準速度お
よび外部基準位置が零の状態で、ジャイロ誤差および加
速度計誤差による姿勢誤差および方位誤差を推定し、こ
れらの誤差の修正量を計算して座標変換行列計算部24
に入力する。姿勢誤差および方位誤差を推定するために
は、当然、その原因となるジャイロ誤差および加速度計
誤差についても推定することになる。観測データとして
姿勢誤差および方位誤差が直接測定できるならこの推定
は簡単であるが、実際には速度データが観測データとな
り、ジャイロ誤差および加速度計誤差による速度誤差Δ
VN 、ΔVE を正しく測定できるわけではなく、動揺外
乱としての動揺速度VdN、VdEが余計に測定されてしま
う。これらは速度誤差ΔVN 、ΔVE の信号に対してノ
イズ外乱となり、速度誤差ΔVN 、ΔVE 、姿勢角誤
差、方位角誤差、加速度計誤差およびジャイロ誤差のそ
れぞれの推定精度を低下させる要因とする。また、推定
完了までに要する時間(これを「整定時間」という)を
長くする要因ともなる。At the time of initial alignment, when the external reference velocity and the external reference position are zero, the attitude error and the azimuth error due to the gyro error and the accelerometer error are estimated, and the correction amount of these errors is calculated to calculate the coordinate transformation matrix. Part 24
To enter. In order to estimate the attitude error and the azimuth error, naturally, the gyro error and the accelerometer error that cause the error are also estimated. This estimation is easy if the attitude and orientation errors can be directly measured as observation data, but in reality the velocity data becomes the observation data, and the velocity error Δ due to gyro error and accelerometer error Δ
It is not possible to correctly measure V N and ΔV E, and the vibration velocities V dN and V dE as vibration disturbances are excessively measured. These become noise disturbance to the signal speed error [Delta] V N, [Delta] V E, a factor of lowering the speed error [Delta] V N, [Delta] V E, respectively of the estimated accuracy of the attitude angle error, the azimuth angle error, accelerometer errors and gyroscope errors To do. In addition, it becomes a factor of lengthening the time required to complete the estimation (this is referred to as “settling time”).
【0016】この整定時間を短縮するため、図2に示し
た従来例では、動揺外乱加速度同定計算部33、フィー
ドフォワード補正計算部34および速度計算部35を備
える。動揺外乱加速度同定計算部33は、初期アライメ
ント時に、加速度座標変換計算部23からの時系列の出
力に基づいてその出力に含まれる動揺外乱加速度を推定
する。フィードフォワード補正計算部34は、動揺外乱
加速度の推定値により加速度座標変換計算部23の出力
を補正する。速度計算部35は、フィードフォワード補
正計算部34の出力を積分し、それを初期アライメント
時の速度誤差として出力誤差推定計算部31およびIM
U誤差推定計算部32に出力する。In order to reduce the settling time, the conventional example shown in FIG. 2 is provided with a swing disturbance acceleration identification calculation section 33, a feedforward correction calculation section 34 and a speed calculation section 35. The shake disturbance acceleration identification calculation unit 33 estimates the shake disturbance acceleration included in the output based on the time-series output from the acceleration coordinate conversion calculation unit 23 during the initial alignment. The feedforward correction calculation unit 34 corrects the output of the acceleration coordinate conversion calculation unit 23 based on the estimated value of the shaking disturbance acceleration. The speed calculation unit 35 integrates the output of the feedforward correction calculation unit 34, and uses it as the speed error at the time of initial alignment, and the output error estimation calculation unit 31 and IM.
It is output to the U error estimation calculation unit 32.
【0017】[0017]
【発明が解決しようとする課題】上述した先の出願で
は、動揺外乱加速度を推定してフィードフォワード補正
を行うことで、動揺外乱の影響を短時間に除去すること
が可能となり、初期アライメントおよびキャリブレーシ
ョンの時間を短縮し、高精度に姿勢および方位を決定で
きるようになった。具体的には、それ以前に4時間要し
ていた初期アライメントおよびキャリブレーション時間
が、30分以下と大幅に短縮された。In the above-mentioned prior application, by estimating the sway disturbance acceleration and performing feedforward correction, it becomes possible to remove the influence of the sway disturbance in a short time, and the initial alignment and calibration are performed. It shortens the session time and can determine the attitude and orientation with high accuracy. Specifically, the initial alignment and calibration time, which required 4 hours before that, was greatly reduced to 30 minutes or less.
【0018】しかし、(1)動揺外乱加速度同定計算部
の処理が複雑であり、計算時間をより短縮しようとする
と、処理性能の高い計算機が必要となって装置の価格が
高くなる、(2)正確な外乱フィードフォワードを設定
する必要があり、不正確だとフィードフォワード補正に
おいて満足な性能が得られない、(3)動揺外乱加速度
の同定精度が不十分であり、フィードフォワード補正す
ると信号波形が歪んでしまうことがあるという課題があ
った。However, (1) the processing of the shake disturbance acceleration identification calculation section is complicated, and if the calculation time is further shortened, a computer with high processing performance is required and the cost of the apparatus becomes high. It is necessary to set accurate disturbance feedforward, and if it is inaccurate, satisfactory performance cannot be obtained in feedforward correction. (3) The identification accuracy of the sway disturbance acceleration is insufficient, and if feedforward correction is performed, the signal waveform becomes There was a problem that it could be distorted.
【0019】本発明は、このような課題を解決し、高精
度で短時間に初期姿勢および初期方位角を決定すること
のできる慣性航法装置を提供することを目的とする。It is an object of the present invention to solve the above problems and provide an inertial navigation system capable of determining the initial attitude and the initial azimuth with high accuracy and in a short time.
【0020】[0020]
【課題を解決するための手段】本発明の慣性航法装置
は、測定される速度データをSUMMARY OF THE INVENTION The inertial navigation system of the present invention collects measured velocity data.
【数5】 でモデル化し、評価関数[Equation 5] Modeled with, the evaluation function
【数6】 を最小にするΘの推定値を[Equation 6] An estimate of Θ that minimizes
【数7】 により求め、[Equation 7] Determined by
【数8】 により初期設定時の速度誤差を推定することを特徴とす
る。[Equation 8] Is characterized by estimating the speed error at the initial setting.
【0021】[0021]
【作用】初期アライメント中の加速度データの南北方向
成分および東西方向成分の積分値である速度データをバ
ッチシーケンシャル形の改良最小二乗法を用いてプリフ
ィルタ処理し、処理後の速度データをカルマンフィルタ
入力とする。これにより、簡単かつ精度よく速度誤差の
推定値を得ることができ、短時間で高精度に初期姿勢お
よび方位を決定できる。[Operation] The velocity data, which is the integrated value of the north-south component and the east-west component of the acceleration data during the initial alignment, is pre-filtered using the batch sequential type improved least squares method, and the velocity data after processing is input to the Kalman filter. To do. As a result, the estimated value of the velocity error can be obtained easily and accurately, and the initial attitude and azimuth can be determined with high accuracy in a short time.
【0022】[0022]
【実施例】図1は本発明実施例の船舶用慣性航法装置を
示すブロック構成図である。この実施例装置は、上述し
た従来例と同様に、IMU部11と、入出力回路12
と、加速度計誤差補正計算部21、ジャイロ誤差補正計
算部22、加速度座標変換計算部23、座標変換行列計
算部24、修正量計算部25、速度位置計算部26、姿
勢方位計算部27、減算回路28、29、カルマンフィ
ルタ部30、動揺外乱加速度同定計算部33、フィード
フォワード補正計算部34および速度計算部35からな
るコンピュータ部と、入出力回路41と、入出力データ
表示装置42とを備える。カルマンフィルタ部30は、
出力誤差推定計算部31とIMU誤差推定計算部32と
を備える。さらに本実施例では、コンピュータ部内に、
動揺外乱除去計算部43を備える。1 is a block diagram showing an inertial navigation system for a ship according to an embodiment of the present invention. The device of this embodiment has an IMU unit 11 and an input / output circuit 12 as in the above-described conventional example.
And an accelerometer error correction calculation unit 21, a gyro error correction calculation unit 22, an acceleration coordinate conversion calculation unit 23, a coordinate conversion matrix calculation unit 24, a correction amount calculation unit 25, a velocity position calculation unit 26, a posture azimuth calculation unit 27, and subtraction. A computer unit including circuits 28 and 29, a Kalman filter unit 30, a shake disturbance acceleration identification calculation unit 33, a feedforward correction calculation unit 34, and a velocity calculation unit 35, an input / output circuit 41, and an input / output data display device 42. The Kalman filter unit 30
An output error estimation calculation unit 31 and an IMU error estimation calculation unit 32 are provided. Further, in this embodiment, in the computer section,
The agitation disturbance removal calculation unit 43 is provided.
【0023】図4ないし図7はこの実施例の動作フロー
を示す。この実施例において、動揺外乱除去計算部43
以外の各部の動作は上述した従来例と同等である。これ
らの動作について簡単に説明した後に、動揺外乱除去計
算部43の動作について詳しく説明する。4 to 7 show the operation flow of this embodiment. In this embodiment, the agitation disturbance removal calculation unit 43
The operation of each unit other than is the same as that of the above-described conventional example. After briefly describing these operations, the operation of the fluctuation disturbance removal calculating unit 43 will be described in detail.
【0024】IMU部11は船体基準軸に取り付けられ
た加速度計およびジャイロにより構成される。加速度計
の出力は入出力回路12および加速度計誤差補正計算部
21を介して加速度座標変換計算部23に入力され、ジ
ャイロの出力は入出力回路12およびジャイロ誤差補正
計算部22を介して座標変換行列計算部24に入力され
る。加速度座標変換計算部23は、加速度計が出力した
船体座標における加速度を航法座標における加速度に変
換する。速度位置計算部26はこの航法座標における加
速度を積分してその船の速度および位置を求める。座標
変換行列計算部24はジャイロが出力した船の回転角速
度から船体座標と航法座標との間の変換規則を求める。
姿勢方位計算部27は座標変換行列計算部24の出力に
よりその船の姿勢および方位を求める。求められた速
度、位置、姿勢および方位は入出力回路41を介して入
出力データ表示装置42に出力される。The IMU unit 11 is composed of an accelerometer and a gyro attached to the hull reference axis. The output of the accelerometer is input to the acceleration coordinate conversion calculation section 23 via the input / output circuit 12 and the accelerometer error correction calculation section 21, and the gyro output is coordinate converted via the input / output circuit 12 and the gyro error correction calculation section 22. It is input to the matrix calculation unit 24. The acceleration coordinate conversion calculation unit 23 converts the acceleration in the hull coordinates output by the accelerometer into the acceleration in the navigation coordinates. The speed position calculation unit 26 integrates the acceleration in the navigation coordinates to obtain the speed and position of the ship. The coordinate conversion matrix calculation unit 24 obtains a conversion rule between the hull coordinates and the navigation coordinates from the rotational angular velocity of the ship output by the gyro.
The attitude and azimuth calculation unit 27 obtains the attitude and azimuth of the ship from the output of the coordinate conversion matrix calculation unit 24. The obtained speed, position, attitude and azimuth are output to the input / output data display device 42 via the input / output circuit 41.
【0025】入出力回路41には外部から速度比較値お
よび位置比較値が入力され、速度比較値を減算回路28
に、位置比較値を減算回路29に供給する。減算回路2
8、29は、それぞれ速度位置計算部26により求めら
れた速度および位置と入出力回路41からの速度比較値
および位置比較値との差を求め、カルマンフィルタ部3
0に供給する。カルマンフィルタ部30内の出力誤差推
定計算部31は減算回路28、29の出力に基づいて出
力誤差を推定して修正量計算部25に出力し、修正量計
算部25は速度位置計算部26および座標変換行列計算
部24のそれぞれの演算の修正量を求める。カルマンフ
ィルタ部30内のIMU誤差推定計算部32は減算回路
28、29の出力に基づいてIMU誤差を推定して加速
度計誤差補正計算部21およびジャイロ補正誤差補正計
算部22に出力し、加速度計誤差補正計算部21および
ジャイロ補正誤差補正計算部22は加速度計およびジャ
イロのそれぞれの出力誤差を補正する。The speed comparison value and the position comparison value are input to the input / output circuit 41 from the outside, and the speed comparison value is subtracted by the subtraction circuit 28.
Then, the position comparison value is supplied to the subtraction circuit 29. Subtraction circuit 2
Reference numerals 8 and 29 respectively obtain the difference between the speed and position obtained by the speed position calculation unit 26 and the speed comparison value and the position comparison value from the input / output circuit 41, and the Kalman filter unit 3
Supply to 0. The output error estimation calculation unit 31 in the Kalman filter unit 30 estimates the output error based on the outputs of the subtraction circuits 28 and 29 and outputs the output error to the correction amount calculation unit 25. The correction amount calculation unit 25 outputs the velocity position calculation unit 26 and the coordinates. The correction amount of each operation of the conversion matrix calculation unit 24 is calculated. The IMU error estimation calculation unit 32 in the Kalman filter unit 30 estimates the IMU error based on the outputs of the subtraction circuits 28 and 29 and outputs the IMU error to the accelerometer error correction calculation unit 21 and the gyro correction error correction calculation unit 22. The correction calculation unit 21 and the gyro correction error correction calculation unit 22 correct the output errors of the accelerometer and the gyro.
【0026】動揺外乱除去計算部43は、速度比較値が
零である初期アライメント中の速度誤差を推定してカル
マンフィルタ部30内の出力誤差推定計算部31および
IMU誤差推定計算部32にそれぞれ供給する。初期ア
ライメント中には、速度ベクトルの北方向成分VN およ
び東方向成分VE が、The shaking disturbance removal calculation unit 43 estimates the velocity error during the initial alignment with the velocity comparison value being zero and supplies it to the output error estimation calculation unit 31 and the IMU error estimation calculation unit 32 in the Kalman filter unit 30, respectively. . During the initial alignment, the north component V N and the east component V E of the velocity vector are
【数9】 となる。この式において、VdN、VdEが各方向の動揺外
乱速度であり、ΔVN (t) 、ΔVE (t) はジャイロ誤
差、加速度計誤差、姿勢および方位誤差その他により発
生する速度誤差である。[Equation 9] Becomes In this equation, V dN and V dE are fluctuation disturbance velocities in each direction, and ΔV N (t) and ΔV E (t) are velocity errors caused by gyro error, accelerometer error, attitude and heading error, and others. .
【0027】初期アライメントの目的は、何らかの方法
でVdN、VdEを小さくし(理想的には零)、ΔVN (t)
、ΔVE (t) を抽出してカルマンフィルタ部30の入
力とすることにより、ジャイロ誤差、加速度計誤差、姿
勢および方位誤差、速度誤差などの誤差を推定するもの
である。この初期アライメントで求めた推定値により誤
差を修正することにより、正確な姿勢および方位を求め
ることができる。各誤差の推定値の精度は、動揺外乱速
度VdN、VdEをどの程度除去できたかに大きく左右され
る。The purpose of the initial alignment is to reduce V dN and V dE (ideally to zero) by some method, and ΔV N (t)
, ΔV E (t) are extracted and input to the Kalman filter unit 30, so that errors such as a gyro error, an accelerometer error, a posture and azimuth error, and a velocity error are estimated. By correcting the error with the estimated value obtained by this initial alignment, it is possible to obtain an accurate posture and azimuth. The accuracy of the estimated value of each error largely depends on how much the fluctuation speeds V dN and V dE can be removed.
【0028】次に、改良形最小二乗法を用いて短い時間
で動揺外乱速度VdN、VdEを除去してΔVN (t) 、ΔV
E (t) を抽出することについて説明する。速度位置計算
部26により求められる北方向および東方向の速度につ
いての測定データyN (t) 、yE (t) は、Next, the fluctuation disturbance velocities V dN and V dE are removed in a short time by using the improved least squares method to obtain ΔV N (t) and ΔV.
Extracting E (t) is explained. The measured data y N (t) and y E (t) about the speed in the north direction and the speed in the east direction obtained by the speed position calculation unit 26 are
【数10】 と表される。ここで、δVNr、δVErは正規白色性の観
測ノイズである。これらの測定データyN (t) 、y
E (t) において、除去すべき動揺外乱速度VdN、VdEは
周期約数10年程度の周期波形であり、信号ΔVN (t)
、ΔVE (t) は例えば周期84分(シューラー周期)
の正弦波である。測定データyN (t) 、yE (t)をまと
めて次のようにベクトル表現する。[Equation 10] Is expressed as Here, δV Nr and δV Er are normal white observation noises. These measured data y N (t), y
At E (t), the disturbance disturbance velocities V dN and V dE to be removed are periodic waveforms with a period of about several tens of years, and the signal ΔV N (t)
, ΔV E (t) is, for example, a period of 84 minutes (Schuler period)
Is a sine wave of. The measurement data y N (t) and y E (t) are collectively expressed as a vector as follows.
【数11】 [Equation 11]
【0029】また、測定データとは別に、北方向および
東方向の速度を次の自己回帰モデルでモデル化する。Separately from the measured data, the velocities in the north and east directions are modeled by the following autoregressive model.
【数12】 このモデルのパラメータΘを実際の測定データを用いて
推定することで、信号ΔVN (t) 、ΔVE (t) を近似す
ることができる。[Equation 12] By estimating the parameter Θ of this model using actual measurement data, the signals ΔV N (t) and ΔV E (t) can be approximated.
【0030】パラメータΘを最適推定するための評価関
数は、次のバッチ形データで表される。The evaluation function for optimally estimating the parameter Θ is represented by the following batch type data.
【数13】 [Equation 13]
【0031】この評価関数を最小にする推定値は、The estimated value that minimizes this evaluation function is
【数14】 となる。この推定値を用いて、信号ΔVN (t) 、ΔVE
(t) の推定値が、[Equation 14] Becomes Using this estimated value, the signals ΔV N (t) and ΔV E
The estimated value of (t) is
【数15】 により求められる。この推定値をカルマンフィルタ部3
0への入力とする。[Equation 15] Required by. This estimated value is used as the Kalman filter unit 3
Input to 0.
【0032】図8はパラメータΘの推定動作を説明する
図である。現在の時刻がti のとき、ti-(N-1) からt
i までのそれぞれの時点における測定データからパラメ
ータΘの推定値を求める。FIG. 8 is a diagram for explaining the estimation operation of the parameter Θ. T i- (N-1) to t when the current time is t i
The estimated value of the parameter Θ is obtained from the measurement data at each time point up to i .
【0033】以上の説明では速度位置計算部26により
求めた速度データをプリフィルタ処理してカルマンフィ
ルタ部30に入力する構成を示したが、加速度座標変換
計算部23の出力する加速度データをプリフィルタ処理
してから積分し、これをカルマンフィルタ部30の入力
とすることもできる。In the above description, the speed data obtained by the speed position calculation unit 26 is prefiltered and input to the Kalman filter unit 30. However, the acceleration data output from the acceleration coordinate conversion calculation unit 23 is prefiltered. Then, it is also possible to perform integration and use this as the input of the Kalman filter unit 30.
【0034】[0034]
【発明の効果】以上説明したように、本発明の船舶の慣
性航法装置は、初期アライメント中の速度データをバッ
チシーケンシャル形の改良最小二乗法を用いてプリフィ
ルタ処理し、処理後の速度データをカルマンフィルタ入
力とすることにより、簡単かつ精度よく速度誤差の推定
値を得ることができる。したがって、約15分程度の短
時間で高精度に初期姿勢および方位を決定できる効果が
ある。As described above, in the inertial navigation system for a ship of the present invention, the velocity data during the initial alignment is pre-filtered by using the batch sequential type improved least squares method, and the velocity data after the processing is processed. By using the Kalman filter input, it is possible to easily and accurately obtain the estimated value of the velocity error. Therefore, there is an effect that the initial posture and azimuth can be determined with high accuracy in a short time of about 15 minutes.
【0035】また、同定が終了するまでの時間を節約す
ることができ、整定時間をより短縮することができるの
で、消費電力を節約できる効果がある。Further, the time until the identification is completed can be saved, and the settling time can be further shortened, so that the power consumption can be saved.
【0036】さらに、初期姿勢および初期方位をより高
精度かつ短時間に決定することができ、短時間での出航
が可能となり、船舶の運用コストを削減できる効果があ
る。Furthermore, the initial attitude and the initial bearing can be determined with higher accuracy and in a shorter time, and it is possible to depart in a short time, which has the effect of reducing the operating cost of the ship.
【図1】本発明実施例の慣性航法装置を示すブロック構
成図。FIG. 1 is a block configuration diagram showing an inertial navigation device according to an embodiment of the present invention.
【図2】従来例の慣性航法装置を示すブロック構成図。FIG. 2 is a block diagram showing a conventional inertial navigation system.
【図3】慣性航法装置で用いられる座標を示す図。FIG. 3 is a diagram showing coordinates used in an inertial navigation system.
【図4】実施例装置の動作フローの一部を示す図。FIG. 4 is a diagram showing a part of an operation flow of the embodiment apparatus.
【図5】実施例装置の動作フローの一部を示す図。FIG. 5 is a diagram showing a part of an operation flow of the embodiment apparatus.
【図6】実施例装置の動作フローの一部を示す図。FIG. 6 is a diagram showing a part of an operation flow of the embodiment apparatus.
【図7】実施例装置の動作フローの一部を示す図。FIG. 7 is a diagram showing a part of the operation flow of the embodiment apparatus.
【図8】パラメータΘの推定動作を説明する図。FIG. 8 is a diagram illustrating an estimation operation of a parameter Θ.
11 IMU部 12、41 入出力回路 21 加速度計誤差補正計算部 22 ジャイロ誤差補正計算部 23 加速度座標変換計算部 24 座標変換行列計算部 25 修正量計算部 26 速度位置計算部 27 姿勢方位計算部 28、29 減算回路 30 カルマンフィルタ部 31 出力誤差推定計算部 32 IMU誤差推定計算部 33 動揺外乱加速度同定計算部 34 フィードフォワード補正計算部 35 速度計算部 42 入出力データ表示装置 43 動揺外乱除去計算部 11 IMU unit 12, 41 Input / output circuit 21 Accelerometer error correction calculation unit 22 Gyro error correction calculation unit 23 Acceleration coordinate conversion calculation unit 24 Coordinate conversion matrix calculation unit 25 Correction amount calculation unit 26 Velocity position calculation unit 27 Attitude direction calculation unit 28 , 29 Subtraction circuit 30 Kalman filter section 31 Output error estimation calculation section 32 IMU error estimation calculation section 33 Swing disturbance acceleration identification calculation section 34 Feedforward correction calculation section 35 Velocity calculation section 42 Input / output data display device 43 Disturbance disturbance removal calculation section
Claims (1)
よびジャイロからなるIMU部(11)と、 上記加速度計が出力した船体座標における加速度を航法
座標における加速度に変換する第一の演算手段(23)
と、 この航法座標における加速度を積分してその船の速度お
よび位置を求める第二の演算手段(26)と、 上記ジャイロが出力した船の回転角速度から船体座標と
航法座標との間の変換規則を求める第三の演算手段(2
4)と、 この第三の演算手段の出力によりその船の姿勢および方
位を求める第四の演算手段(27)と、 上記第二の演算手段により求められた速度および位置と
外部から入力された速度比較値および位置比較値との差
に基づいて上記第二および上記第三の演算手段のそれぞ
れの演算の修正量を求める第一の補正手段(31、2
5)と、 上記第二の演算手段により求められた速度および位置と
外部から入力された速度比較値および位置比較値との差
に基づいて上記加速度計および上記ジャイロのそれぞれ
の出力誤差を補正する第二の補正手段(32、21、2
2)と、 速度比較値が零である初期設定時の速度誤差を推定して
上記第一および上記第二の補正手段にそれぞれ供給する
初期設定手段とを備えた船舶の慣性航法装置において、 この初期設定手段は、航法座標における加速度を積分し
た速度データを 【数1】 でモデル化し、評価関数 【数2】 を最小にするΘの推定値を 【数3】 により求め、 【数4】 により初期設定時の速度誤差を推定する手段を含むこと
を特徴とする船舶の慣性航法装置。1. An IMU unit (11) including an accelerometer and a gyro attached to a hull reference axis, and a first computing means (23) for converting the acceleration in the hull coordinates output by the accelerometer into the acceleration in the navigation coordinates. )
And a second calculation means (26) for integrating the acceleration in the navigation coordinates to obtain the speed and position of the ship, and a conversion rule between the hull coordinates and the navigation coordinates based on the rotational angular speed of the ship output by the gyro. The third calculation means (2
4), fourth computing means (27) for obtaining the attitude and bearing of the ship by the output of the third computing means, and the speed and position obtained by the second computing means and the external input. First correction means (31, 2) for obtaining a correction amount for each calculation of the second and third calculation means based on the difference between the speed comparison value and the position comparison value.
5) and the output errors of the accelerometer and the gyro are corrected based on the difference between the speed and position obtained by the second calculating means and the speed comparison value and the position comparison value input from the outside. Second correction means (32, 21, 2
2) and an initial setting means for estimating a speed error at the initial setting when the speed comparison value is zero and supplying the speed error to the first and second correcting means, respectively. The initial setting means uses the velocity data obtained by integrating the acceleration in the navigation coordinates as follows: Modeled with, the evaluation function The estimated value of Θ that minimizes It is calculated by An inertial navigation system for a ship, comprising means for estimating a speed error at the time of initial setting by
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP6135494A JP3375720B2 (en) | 1994-03-30 | 1994-03-30 | Ship inertial navigation system |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP6135494A JP3375720B2 (en) | 1994-03-30 | 1994-03-30 | Ship inertial navigation system |
Publications (2)
Publication Number | Publication Date |
---|---|
JPH07270176A true JPH07270176A (en) | 1995-10-20 |
JP3375720B2 JP3375720B2 (en) | 2003-02-10 |
Family
ID=13168737
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
JP6135494A Expired - Fee Related JP3375720B2 (en) | 1994-03-30 | 1994-03-30 | Ship inertial navigation system |
Country Status (1)
Country | Link |
---|---|
JP (1) | JP3375720B2 (en) |
Cited By (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JPH10271604A (en) * | 1997-03-25 | 1998-10-09 | West Japan Railway Co | Accident detecting equipment |
JP2006194822A (en) * | 2005-01-17 | 2006-07-27 | Oyo Corp | Displacement monitoring method of ground or the like using acceleration sensor |
CN105278535A (en) * | 2015-11-23 | 2016-01-27 | 上海海事大学 | Intelligent turning cooperative control method for unpowered facility towing system |
CN112284419A (en) * | 2020-10-19 | 2021-01-29 | 中国人民解放军空军工程大学 | Initial fine alignment method for biaxial rotation modulation |
CN116046027A (en) * | 2023-03-31 | 2023-05-02 | 中国船舶集团有限公司第七〇七研究所 | Passive autonomous calibration method and system for triaxial rotary inertial navigation position error |
-
1994
- 1994-03-30 JP JP6135494A patent/JP3375720B2/en not_active Expired - Fee Related
Cited By (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JPH10271604A (en) * | 1997-03-25 | 1998-10-09 | West Japan Railway Co | Accident detecting equipment |
JP2006194822A (en) * | 2005-01-17 | 2006-07-27 | Oyo Corp | Displacement monitoring method of ground or the like using acceleration sensor |
CN105278535A (en) * | 2015-11-23 | 2016-01-27 | 上海海事大学 | Intelligent turning cooperative control method for unpowered facility towing system |
CN105278535B (en) * | 2015-11-23 | 2018-02-13 | 上海海事大学 | A kind of automated steering cooperative control method for unpowered facility traction system |
CN112284419A (en) * | 2020-10-19 | 2021-01-29 | 中国人民解放军空军工程大学 | Initial fine alignment method for biaxial rotation modulation |
CN112284419B (en) * | 2020-10-19 | 2023-02-28 | 中国人民解放军空军工程大学 | Initial fine alignment method for biaxial rotation modulation |
CN116046027A (en) * | 2023-03-31 | 2023-05-02 | 中国船舶集团有限公司第七〇七研究所 | Passive autonomous calibration method and system for triaxial rotary inertial navigation position error |
Also Published As
Publication number | Publication date |
---|---|
JP3375720B2 (en) | 2003-02-10 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN100541135C (en) | Fiber-optic gyroscope strapdown inertial navigation system initial attitude based on Doppler is determined method | |
CN109916395B (en) | Gesture autonomous redundant combined navigation algorithm | |
JP4876204B2 (en) | Small attitude sensor | |
CN104596514B (en) | The Real-time Noisy Reducer and method of accelerometer and gyroscope | |
CN111551174A (en) | High-dynamic vehicle attitude calculation method and system based on multi-sensor inertial navigation system | |
CN110887480B (en) | Flight attitude estimation method and system based on MEMS sensor | |
EP1200802A1 (en) | Vibration compensation for sensors | |
WO2014010727A1 (en) | Device for estimating moving object travel direction and method for estimating travel direction | |
CN110715659A (en) | Zero-speed detection method, pedestrian inertial navigation method, device and storage medium | |
CN106370178B (en) | Attitude measurement method and device of mobile terminal equipment | |
JP2007232443A (en) | Inertia navigation system and its error correction method | |
CN111307114B (en) | Water surface ship horizontal attitude measurement method based on motion reference unit | |
JP2014240266A (en) | Sensor drift amount estimation device and program | |
Carratù et al. | IMU self-alignment in suspensions control system | |
JP3375720B2 (en) | Ship inertial navigation system | |
CN109674480B (en) | Human motion attitude calculation method based on improved complementary filtering | |
CN110030991B (en) | High-speed rotation angle movement measuring method for flyer integrating gyroscope and magnetometer | |
CN101118160A (en) | Low accuracy piezoelectric gyroscope zero-bias real-time estimation compensation process | |
JP3425689B2 (en) | Inertial device | |
US6085149A (en) | Integrated inertial/VMS navigation solution | |
JPH06265367A (en) | Inertial navigation apparatus of ship | |
JPH0949737A (en) | Navigation signal outputting method | |
CN109990776B (en) | Attitude measurement method and device | |
CN109489689B (en) | Satellite vector measurement error on-orbit estimation method based on alpha-beta filtering | |
JPH11118507A (en) | Inertial navigation system of marine vessel |
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 |