JPH07270176A - Inertial navigation apparatus of ship - Google Patents

Abstract

PURPOSE:To operate an estimated value of a speed error with high accuracy by treating speed data in the middle of an initial alignment through a pre-filter process with using an improved method of least squares and using the processed speed data as an input to a Karman filter. CONSTITUTION:During an initial alignment, a component VN in the north direction and a component VE in the east direction of a speed vector are respectively VN(t)=VdN (t)+DELTAVN (t) and VE(t)=VdE(t)+DELTAVE (t). In the expressions, VdN and VdE are speeds of vibrations and outer disturbances in the respective directions, DELTAVN (t) and DELTAVE (t) are speed errors brought about due to an error of a gyro, an error of an accelerometer and errors in attitude and direction, etc. A removing/calculating part, 43 for removing/calculating vibrations and outer disturbances removes the speeds VdN and VdE in a short time with the use of an improved method of least squares, thereby to extract DELTAVN(t) and DELTAVE (t) and feed to an estimating/calculating part, 31 for estimating/calculating an output error and an estimating/calculating part 32 for estimating/calculating an IMU error in a Karman filter part 30.

Description

Detailed Description of the Invention

[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]

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.

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 °.

Next, the operation of each part of the conventional example will be described using the symbols shown in Table 1.

[Table 1]

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.

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.

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.

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.

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.

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.

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.

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.

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.

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.

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”).

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]

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.

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.

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]

SUMMARY OF THE INVENTION The inertial navigation system of the present invention collects measured velocity data.

[Equation 5] Modeled with, the evaluation function

[Equation 6] An estimate of Θ that minimizes

[Equation 7] Determined by

[Equation 8] Is characterized by estimating the speed error at the initial setting.

[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]

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.

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.

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.

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.

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

[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. .

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.

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

[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.

[Equation 11]

Separately from the measured data, the velocities in the north and east directions are modeled by the following autoregressive model.

[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.

The evaluation function for optimally estimating the parameter Θ is represented by the following batch type data.

[Equation 13]

The estimated value that minimizes this evaluation function is

[Equation 14] Becomes Using this estimated value, the signals ΔV _N (t) and ΔV _E
The estimated value of (t) is

[Equation 15] Required by. This estimated value is used as the Kalman filter unit 3
Input to 0.

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 .

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]

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.

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.

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.

[Brief description of drawings]

FIG. 1 is a block configuration diagram showing an inertial navigation device according to an embodiment of the present invention.

FIG. 2 is a block diagram showing a conventional inertial navigation system.

FIG. 3 is a diagram showing coordinates used in an inertial navigation system.

FIG. 4 is a diagram showing a part of an operation flow of the embodiment apparatus.

FIG. 5 is a diagram showing a part of an operation flow of the embodiment apparatus.

FIG. 6 is a diagram showing a part of an operation flow of the embodiment apparatus.

FIG. 7 is a diagram showing a part of the operation flow of the embodiment apparatus.

FIG. 8 is a diagram illustrating an estimation operation of a parameter Θ.

[Explanation of symbols]

 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)

[Claims] 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