JPH11281391A - Inertial navigation system - Google Patents

Abstract

PROBLEM TO BE SOLVED: To enable accurate correction of centrifugal acceleration by reducing influence of quantization error of a gyro sensor by using an αβ filter for angular acceleration calculation to be used for correction. SOLUTION: An angular velocity is detected by a gyro sensor 21 installed in an inertial navigation system, and is detected as a change angle θ per sampling time Δt, which angle is supplied to an angle integrating part 22 and integrated. An integrated angle θm1 obtained by the angle integrating part 22 is supplied to an angular velocity estimation operating part 23, which obtains an angular velocity estimation value ωp1 by using an αβ filter. The angular velocity estimation value ωp1 is inputted in an angular acceleration estimation operating part 24, and an angular acceleration estimation value (dω/dt)p2 is obtained by using again the αβ filter. By using the angular acceleration estimation value (dω/dt)p2 , the centrifugal acceleration is corrected in a centrifugal acceleration correcting part 25.

Description

DETAILED DESCRIPTION OF THE INVENTION

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention relates to an inertial navigation system (INS) mounted on a marine vehicle fired from a ship, and more particularly, to calculating an initial attitude and an initial azimuth using a transfer alignment method (initialization). ).

[0002]

2. Description of the Related Art FIGS. 2 and 3 show the arrangement of a hull 2 mounted on a ship 1. FIG. As shown, the vehicle 2 is set on a vehicle launching device 3 typically mounted on the ship 1. The ship 1 is equipped with a main inertial navigation device 10 (hereinafter referred to as "master INS") for calculating the attitude and direction of the ship 1, and the ship 2 also calculates the attitude and direction of the ship 2 itself. And a small inertial navigation device 20 (hereinafter referred to as a “running vehicle INS”).

[0003] A straight line is provided between the coordinate system set for the traveling body INS20 (hereinafter referred to as "vehicle body coordinates") and the coordinate system set for the master INS10 (hereinafter referred to as "master coordinates"). There is a target deviation and a rotational deviation. This rotation deviation is referred to as a mounting error or mounting misalignment φ. The mounting error φ is the mounting error φ _X around the X, Y, and Z axes,
The error includes φ _Y and φ _Z and is an error that occurs when the aircraft 2 is mechanically mounted on a ship.

[0004] In general, the roll, the pitch, and the orientation are referred to as an attitude and an orientation. Further, the roll angle, the pitch angle, and the azimuth angle are referred to as an attitude angle and an azimuth angle.

[0005] The inertial guidance of the navigation body 2 is performed by the navigation body INS20 mounted on the navigation body 2 itself.
In order to accurately guide the ship along a predetermined trajectory,
It is necessary to accurately determine the attitude and the azimuth of the vehicle 2 or the vehicle INS 20 before launching the vehicle. This is the airframe INS2
This is referred to as zero initialization.

[0006] Initialization of the traveling vehicle INS20 is performed by using the traveling vehicle IN.
It cannot be executed by S20 itself. Aircraft I
This is because the initialization of the NS 20 requires several tens of minutes, but the power of the navigation body INS 20 is turned on immediately before the launch of the navigation body. Also, the sensors used for the vehicle INS20 are generally inexpensive and have low accuracy.

On the other hand, the master INS 10 is always in a driving state, and the attitude and orientation of the master INS 10 itself are always accurately obtained. Therefore, in order to initialize the airframe INS20, the mounting error or the mounting misalignment φ
Should be required.

[0008] As a method for quickly estimating the attitude and orientation of the INS 20, a method called a transfer alignment method is used. According to the transfer alignment method, the vehicle INS2 from the master INS10
The signal is transferred to 0, whereby the mounting error φ is estimated and calculated.

More specifically, the mounting error φ is estimated by comparing the acceleration and the angular velocity detected by the INS 20 with the acceleration and the angular velocity detected by the master INS 10. Or, INS20
Speed and attitude angle detected by the master INS10
The mounting error φ is estimated by comparing the speed and the posture angle detected by the above. The estimation error of the attachment error φ is performed by the marine vessel INS20 using the Kalman filter.

The presence of the mounting error φ causes the master IN
A deviation occurs between the acceleration and the angular velocity input to the accelerometer and the gyro of S10 and the acceleration and angular velocity input to the accelerometer and the gyro of the marine vessel INS20. Since the speed and the attitude angle are calculated based on the acceleration and the angular speed,
These values are also different between the master INS 10 and the traveling vehicle INS 20.

When the ship 1 shakes, the marine vessel INS 20 and the master INS 10 rotate. Due to the relative distance between the two, there is an acceleration difference between the vehicle INS20 and the master INS10. This is the centrifugal acceleration error. In the calculation for estimating the attitude and orientation by the transfer alignment method, the calculation is performed based only on the deviation caused by the mounting error φ. Therefore, it is necessary to correct the centrifugal acceleration in a more accurate calculation. Since the speed is obtained from the acceleration, it is necessary to similarly correct the centrifugal acceleration.

The distance between the master INS 10 and the vehicle INS 20 is R. For ships, the angular velocity ω due to the motion of the hull
Is entered. Acceleration difference ΔA between two points separated by distance R
Is represented by a vector as in the following equation.

[0013]

ΔA = (dω / dt) × R + ω × (ω × R)

Here, dω / dt is the angular acceleration input to the ship. Each component of the acceleration difference ΔA (vector) is as follows.

[0015]

ΔA _X = − (ω _Y ² + ω _Z ² ) R _X + (− dω _Z / dt
_{+ Ω X ω Y) R Y} + (dω Y / dt + ω X ω Z) R Z ΔA Y = (dω Z / dt + ω X ω Y) R X - (ω X 2 +
^{_{ω Z 2) R Y + (}} - dω X / dt + ω Y ω Z) R Z ΔA Z = (- dω Y / dt + ω X ω Z) R X + (dω X
/ Dt + ω _Y ω _Z ) R _Y − (ω _X ² + ω _Y ² ) R _Z

Ω _X , ω _Y , ω _Z are the X components of the angular velocity ω, Y
And R _Z , R _X , R _Y , and R _Z are the X of the distance R
Component, Y component and Z component. Correction of centrifugal acceleration
To compensate in the form of a change in sampling time speed,
The following equation is a correction equation representing a correction value for each sampling time.

[0017]

ΔV _X = [− (θ _Y (n) ² + θ _Z (n) ² ) R _X + ｛−
(θ _Z (n) -θ _Z (o)) + θ _X (n) θ _Y (n)｝ R _Y + ｛(θ _Y
(n) -θ _Y (o)) + θ _X (n) θ _Z (n)｝ R _Z ] / Δt ΔV _Y = [｛(θ _Z (n) -θ _Z (o)) + θ _X (n ) θ _Y (n)｝
R _X − (θ _X (n) ² + θ _Z (n) ² ) R _Y + ｛− (θ _X (n) -θ
_X (o)) + θ _Y (n) θ _Z (n)｝ R _Z ] / Δt ΔV _Z = [｛-(θ _Y (n) -θ _Y (o)) + θ _X (n) θ _Z ( n)｝
R _X + ｛(θ _X (n) −θ _X (o)) + θ _Y (n) θ _Z (n)｝ R _Y
− (Θ _X (n) ² + θ _Y (n) ² ) R _Z ] / Δt

Here, θ (n) is the change angle (gyro output angle) in the new sampling, and θ (o) is the change angle (gyro output angle) in the previous sampling. Δt is a sampling time. The correction of the centrifugal acceleration is performed by subtracting the value of the above equation from the acceleration detected by the vehicle INS20.

[0019]

The correction equation of the equation (3) includes the following quantization term for calculating the angular velocity change Δθ / Δt during the sampling time.

[0020]

４θ (n) −θ (o)｝ / Δt = Δθ / Δt

The calculation of the quantization term causes a quantization error depending on the resolution of the gyro output. This quantization error becomes a large acceleration correction error in proportion to the relative distance R, and thus poses a serious problem in the case of an inertial navigation device installed at a position distant from the master INS 10, such as the navigation vehicle INS20.

Quantization error (dω / dt) err of angular acceleration
Is represented by the following equation by the gyro resolution P [rad / pulse] and the sampling time Δt, assuming that there is a difference of one pulse between the new sampling and the previous sampling.

[0023]

(Dω / dt) err = P / Δt / Δt [rad / s ² ]

The acceleration correction error Aerr is calculated by
If the relative distance R between 0 and the master INS 10 is represented by the following equation:

[0025]

Aerr = P · R / Δt / Δt [cm / s ² ]

For example, the gyro resolution P [rad / pulse
], The sampling time Δt and the relative distance R are set to the following values.

[0027]

[Mathematical formula 7] Δt = 10 [ms] R = 100 [m] P = 3.5 [″ / pulse]

In this case, the acceleration error generated due to the gyro quantization error is calculated as follows.

[0029]

[Equation 8] Aerr = 1697 [cm / s ² ] = 1.73 [G]

This value is an error large enough to cancel the actual acceleration.

According to the present invention, the vehicle INS20 and the master IN
It is an object of the present invention to provide an inertial navigation device that accurately corrects centrifugal acceleration even when the relative distance R between S10 is large.

[0032]

According to the present invention, there is provided an inertial navigation apparatus configured to estimate a mounting error in initializing a posture and an azimuth of a navigation body mounted on a navigation body using a Kalman filter. Performing centrifugal acceleration correction when calculating the difference in angular acceleration determined by the inertial navigation device and the main inertial navigation device mounted on the navigation body,
By using an αβ filter for calculating the angular acceleration used for the correction, the influence of the quantization error of the gyro sensor is reduced.

[0033]

DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS Referring to FIG. 1, there is shown a calculation device and a calculation procedure of a centrifugal acceleration correction value ΔV in an inertial navigation device according to the present invention. As shown in FIG.
2, an angular velocity prediction calculation unit 23, an angular acceleration prediction calculation unit 24, and a centrifugal acceleration correction unit 25. According to the present invention, an αβ filter is used to improve the accuracy in calculating the gyro output quantization error. That is, the predicted angular velocity and the predicted angular acceleration are calculated using the αβ filter.

The angular velocity is detected by a gyro sensor 21 provided in the inertial navigation device. The angular velocity is detected as a change angle θ per sampling time Δt. The change angle θ is supplied to the angle integrating unit 22 and integrated.

[0035]

Equation 9 θm1 = θθ

The integrated angle θm1 obtained by the angle integrating section 22 is supplied to an angular velocity prediction calculating section 23. The angular velocity prediction calculation unit 23 uses the αβ filter to calculate the predicted angular velocity ωp
Get 1. The angular velocity predicted value ωp1 is represented by the following equation.

[0037]

[Equation 10] θs1 (n) = θp1 (n) + α1 ｛θm1 (n)
−θp1 (n)｝ ωs1 (n) = ωp1 (n) + β1 ｛θm1 (n) −θp1 (n)
｝ / Δt θp1 (n + 1) = θs1 (n) + ωs1 (n) · Δt ωp1 (n + 1) = ωs1 (n)

Here, each term has the following meaning. θp1 (n): a predicted angle at time n predicted at time n-1. ωp1 (n): Predicted angular velocity at time n predicted at time n-1. θs1 (n): Smoothing angle at time n when smoothing was performed. ωs1 (n): Smoothing angular velocity at the time n when smoothing was performed. θm1 (n): measurement angle (integrated angle of gyro output). Δt: sampling time. α1, β1: constants.

This has the following meaning. The smoothed value is obtained by adding a deviation between the predicted value and the measured value to the predicted value with appropriate weighting. The weighting factor is α,
β. α and β are coefficients for determining the influence of the measured value when obtaining the smoothed value.

The predicted angle θp1 (n) is the smoothed angle θs1 (n)
Is obtained by adding the smoothing change angle obtained from the smoothing angular velocity. The predicted angular velocity ωp1 (n) is equal to the smoothed angular velocity ωs1 (n).

Next, the predicted angular velocity value ωp1 is input to the angular acceleration prediction calculation unit 24, and the predicted angular acceleration value (dω / dt) p2 is obtained again using the αβ filter. The predicted value ωp1 (n) of the first-stage filter is replaced with the measured value ωm2 (n) of the second-stage filter.
Ωm2 (n) = ωp1 (n).

[0042]

[Equation 11] ωs2 (n) = ωp2 (n) + α2) ωm2 (n) −
ωp2 (n)｝ (dω / dt) s2 (n) = (dω / dt) p2 (n) + β2
{Ωm2 (n) −ωp2 (n)} / Δtωp2 (n + 1) = ωp2 (n) + (dω / dt) s2 (n) Δt (dω / dt) p2 (n + 1) = (dω / Dt) p2 (n)

Here, each term has the following meaning. ωp2 (n): Predicted angular velocity at time n predicted at time n-1. (Dω / dt) p2 (n): predicted angular acceleration at time n predicted at time n-1. ωs2 (n): Smoothing angular velocity at time n when smoothing was performed. (Dω / dt) s2 (n): Smoothed angular acceleration at time n when smoothing was performed. ωm2 (n): Measured angular velocity (predicted angular velocity of first-stage filter)
It is. Δt: sampling time. α2, β2: constants.

The centrifugal acceleration correction unit 25 corrects the centrifugal acceleration using the predicted angular acceleration value (dω / dt) p2. Each component of the centrifugal acceleration correction value ΔV for each sampling time Δt is as follows.

[0045]

ΔV _X = [− (θ _Y ² + θ _Z ² ) R _X + ｛−
(Dω / dt) p2 _Z · Δt ² + θ _X θ _Y ｝ R _Y + ｛(d
ω / dt) p2 _Y・ Δt ² + θ _X θ _Z ｝ R _Z ] / Δt ΔV _Y = [{(dω / dt) p 2 _Z・ Δt ² + θ _X θ _Y ｝
R _X − (θ _X ² + θ _Z ² ) R _Y + ｛− (dω / dt) p2
_X · Δt ² + θ _Y θ _Z ｝ R _Z ] / Δt ΔV _Z = [｛− (dω / dt) p2 _Y · Δt ² + θ
_X θ _Z ｝ R _X + ｛(dω / dt) p2 _X · Δt ² + θ _Y θ
_Z ｝ R _Y- (θ _X ² + θ _Y ² ) R _Z ] / Δt

The vehicle INS calculates the correction value ΔV (Δ
V _X, ΔV _Y, correcting the output signal of the accelerometer using a [Delta] V _Z).

Although the embodiments of the present invention have been described in detail, the present invention is not limited to these examples, and various modifications can be made within the scope of the invention described in the claims. It will be understood by those skilled in the art.

Although the description has been given by taking the navigation body INS as an example, the present invention is not limited to the navigation body INS alone, but is applied to an inertial navigation device in which the acceleration must be compared after correcting the centrifugal acceleration. it can.

[0049]

According to the present invention, there is an advantage that an acceleration error due to a gyro quantization error can be suppressed to about 200 times smaller than that of the prior art.

According to the present invention, the angular acceleration used for the centrifugal acceleration correction can be accurately obtained.
There is an advantage that the centrifugal acceleration can be accurately corrected even when the resolution of the gyro is poor or when the relative distance between the master INS and the traveling vehicle INS is large.

Therefore, according to the present invention, there is no restriction on the equipment of the traveling body INS, and there is an advantage that the initialization performance of the traveling body INS can be improved.

[Brief description of the drawings]

FIG. 1 is a diagram showing a calculation procedure for calculating centrifugal acceleration correction according to the present invention.

FIG. 2 is a diagram showing an arrangement state of an inertial navigation device mounted on a ship.

FIG. 3 is a diagram showing an arrangement state of a traveling body mounted on a ship.

[Explanation of symbols]

1 ship, 2 hull, 3 hull launcher, 1
0 Master INS, 20 Aircraft INS

 ────────────────────────────────────────────────── ─── Continuing on the front page (72) Inventor Shunkichi Takeuchi 4-11-28-301, Kamima, Setagaya-ku, Tokyo (72) Inventor Shinichi Nanki 2-16-46 Minami Kamata, Ota-ku, Tokyo Tokimec Co., Ltd. (72) Inventor Ryo Hitomi 2-16-46 Minami Kamata, Ota-ku, Tokyo Inside Tokimec Co., Ltd.

Claims (1)

[Claims] An inertial navigation device configured to estimate a mounting error in initialization of a posture and an orientation of a navigation device mounted on a navigation device by using a Kalman filter, wherein the inertial navigation device and the navigation device are provided. The centrifugal acceleration is corrected when calculating the difference between the angular accelerations obtained by the main inertial navigation system attached to the gyro, and the αβ filter is used to calculate the angular acceleration used for the correction. An inertial navigation device characterized by reducing the influence of the wind.