JPH11281300A - Inertial navigation system - Google Patents

Abstract

PROBLEM TO BE SOLVED: To reduce error in the estimated value of fixing error caused by distortion of hull by weighting an estimated angular speed obtained through an inertial navigation system and a main inertial navigation system mounted on a sailing body and setting the angular speed difference of error covariance of the observation disturbance vector of a Kalman filter larger than a standard value. SOLUTION: At the time of estimating a fixing error around Z axis through an inertial navigation system, an angular speed estimated based on X angular speed difference is weighted as compared with that based on Y angular speed difference obtained through a master INS 10 and a sailing body INS 20. X and Y angular speed differences of a matrix RE satisfy a relationship; RE (X angular speed) <RE (Y angular speed difference). At the time of estimating a fixing error through a Kalman filter, a plurality of parameters are selected and weighted as observation values. Extent of weighting of estimation is distributed to minimize the error caused by distortion of hull contained in the fixing error. Extent of each state vector is represented by Kalman gain from observation values. Subsequently, initial value of error covariance matrix, covariance matrix of system disturbance vector error, and the like, are set.

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. 1 and 2 show an 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] As shown in FIGS. 3 and 4, a sub-inertial navigation device 30 (hereinafter, referred to as a “slave IN”) that calculates the attitude and orientation of the hull launcher 3 in close proximity to the hull launcher 3.
S ". ) May be mounted.

[0006] A straight line is provided between the coordinate system set for the traveling body INS 20 (hereinafter referred to as “vehicle body coordinates”) and the coordinate system set for the master INS 10 (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.

[0005] In general, the roll, the pitch, and the direction are referred to as a posture and a direction. Further, the roll angle, the pitch angle, and the azimuth angle are referred to as an attitude angle and an azimuth angle, but here, they may be simply referred to as an attitude angle.

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

The INS 20 is initialized by the INS 20
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.

A method called a transfer alignment method is used as a method for quickly estimating the attitude and orientation of the INS 20. According to the transfer alignment method, the vehicle INS2 from the master INS10
0, and the mounting error φ is estimated using the signal from the master INS10.

[0010] More specifically, the mounting error φ is estimated by comparing the acceleration and angular velocity detected by the vehicle INS20 with the acceleration and angular velocity detected by the master INS10. Alternatively, the mounting error φ is estimated by comparing the speed and the attitude angle detected by the navigation vehicle INS20 with the speed and the attitude angle detected by the master INS10. The estimation error of the attachment error φ is performed by the marine vessel INS20 using the Kalman filter. Note that the operation may be performed by the slave INS 30 instead of the navigation body INS 20.

Hereinafter, a method of estimating the mounting error φ by the transfer alignment method will be specifically described. Due to the presence of the mounting error φ, the acceleration and angular velocity input to the accelerometer and gyro of the master INS10 and the vehicle IN
A deviation occurs between the acceleration and the angular velocity input to the accelerometer and the gyro in S20. Since the velocity and the attitude angle are calculated based on the acceleration and the angular velocity, these values are also different between the master INS 10 and the marine vessel INS 20.

The vehicle INS20 calculates a difference between the acceleration and the angular velocity detected by the accelerometer and the gyro of the master INS10 and the acceleration and the angular velocity detected by the accelerometer and the gyro of the vehicle INS20. Used for the observed value in the Kalman filter. Or
The speed difference and the posture difference are calculated from the acceleration and the angular velocity, and these differences are used as the observation values in the Kalman filter.

Here, the outline of the Kalman filter will be briefly described. It is assumed that the system can be described by a first order differential equation.

[0014]

## EQU1 ## d [x] / dt = [A] [x] + [C] [w]

The terms of this equation are as follows. [X]: State vector of estimated value [A]: System matrix [w]: System disturbance vector [C]: System disturbance coefficient matrix (in general, unit matrix is often used) This equation is calculated by the transition matrix method. When transformed into a discrete form,

[0016]

[X (t + Δt)] = [Φ] [x (t)] +
[G] [w]

The observation system is represented by the following equation.

[0018]

[Y] = [H] [x] + [v]

The terms of this equation are as follows. [Y]: Observation vector [H]: Observation matrix [v]: Observation disturbance vector If the system can be described by Equations (1) to (3), the estimated value [x] is expressed by the following equation: Required by

[0020]

P _E (t + Δt, t) = Φ · P _E (t, t) Φ
^T + G (Q _E / Δt) G ^T K (t + Δt) = P _E (t + Δt, t) H ^T [H · P _E
(T + Δt, t) H T + R E ] ^{-1 x (t + Δt, t} ) = Φ · x (t, t) x (t + Δt, t + Δt) = x (t + Δt, t) + K
(T + Δt) [y−H · x (t + Δt, t)] P _E (t + Δt, t + Δt) = [I−K (t + Δt) ·
H] P _E (t + Δt, t)

[0021]

Equation 5] ^{Φ = I + AΔt + Δt 2} /2 · A 2 G = {ΦΔt (I-Δt / 2 · A)} C

I is a unit matrix. P _E is the error covariance matrix of the state vector, Q _E is the error covariance matrix of the system disturbance vector, and R _E is the error covariance matrix of the observed disturbance vector, and is expressed as follows. Note that E on the right side represents an expected value.

[0023]

P _E = E [x, x ^T ] Q _E = E [w, w ^T ] R _E = E [v, v ^T ]

Incidentally, P _E , Q _E , and R _E will be described later in detail. K is a Kalman gain, which is a quantity for comparing a statistic between the system error and the observation noise and determining whether to weight the system or the observation value.

Each term in the equation (4) has the following meaning.
You. P_E(T + Δt, t) is the system error at the current time t
How much the statistic of will change after Δt seconds
Things. x (t + Δt, t) is the time at time t
This is a predicted value of the time t + Δt. x (t + Δt, t + Δt)
Is the optimal estimate, the deviation between the observed and predicted values and the karma
This is calculated using the gain K. P_E(T + Δt, t + Δ
t) is the error covariance matrix P _EIs the corrected value of

Next, an estimation calculation of the mounting error φ is performed using a Kalman filter. (1) The case where the acceleration difference or the angular velocity difference between the master coordinate system and the vehicle body coordinate system is used as the observation value of the Kalman filter; and (2) The speed difference or attitude difference between the master coordinate system and the vehicle body coordinate system. May be used.

(1) When an acceleration difference or angular velocity difference between the master coordinate system and the vehicle coordinate system is used as an observation value: mounting error φ
Since is a fixed value that does not change over time, the following equation holds.

[0028]

[7] [dφ / dt] = 0 + [w]

Referring to FIG. 5, the angular velocity detected by the master INS or the angular velocity ω _X , ω _{Y of the} master coordinate system and the angular velocity detected by the navigation body INS or the angular velocity ω _X ′, Find the relationship between ω _Y '. Note that a two-dimensional case is considered for simplification of the description.

[0030]

[Equation 8] _{ω X '= ω X · cosφ} Z + ω Y · sinφ Z ω Y' = -ω X · sinφ Z + ω Y · cosφ Z ω X, ω Y: the master coordinate system angular velocity ω _X ', ω _Y ': Angular velocity of the vehicle body coordinate system φ _Z : mounting error around the Z axis

Assuming that the mounting error φ _Z around the Z axis is very small, the following approximate expression holds.

[0032]

[Equation 9] sinφ _Z ≒ φ _Z cos φ _Z ≒ 1

Therefore, the equation of Expression 8 is as follows.

[0034]

Ω _X −ω _X ′ = −ω _Y · φ _Z ω _Y −ω _Y ′ = ω _X · φ _Z

From the equation (10), the mounting error φ _Z around the Z axis is obtained.
It can be seen that can estimate the angular velocity difference around the X axis or the angular velocity difference around the Y axis as an observation value. When this is extended to three dimensions, it is expressed as follows.

[0036]

Ω−ω ′ = φ × ω ω: angular velocity in the master coordinate system ω ′: angular velocity in the vehicle coordinate system

A similar relational expression is obtained for the acceleration. After all, the angular velocity difference Δω and the acceleration difference Δa are expressed as follows.

[0038]

Δω = φ × ω + v Δa = φ × a + v Δω: Difference between the angular velocity of the master coordinate system and the angular velocity of the vehicle coordinate system Δa: The difference between the acceleration of the master coordinate system and the acceleration of the vehicle coordinate system ω: Angular velocity of master coordinate system a: Acceleration of master coordinate system φ: Mounting error v: Observation disturbance vector

This equation is expanded as follows.

[0040]

Equation 13] _{Δω X = φ Y ω Z -φ} Z ω Y Δω Y = φ Z ω X -φ X ω Z Δω Z = φ X ω Y -φ Y ω X Δa X = φ Y a Z -φ Z _{a Y Δa Y = φ Z a} X -φ X a Z Δa Z = φ X a Y -φ Y a X

From the equation (13), it can be seen that the mounting errors φ _X , φ _Y , φ _Z around the X, Y, Z axes can be estimated based on the following observations, respectively. Mounting error around X axis φ _X : YZ acceleration difference, YZ angular speed difference Mounting error around Y axis φ _Y : X, Z acceleration difference, X, Z angular speed difference Mounting error around Z axis, φ _Z : X・ Y acceleration difference, XY angular velocity difference

However, the ship does not have a large acceleration motion, and the acceleration difference is dominated by the effect of gravitational acceleration. That is, when the mounting errors φ _X and φ _Y exist around the X and Y axes, and a gravity component appears on the other axis, an acceleration difference occurs. However, the Z acceleration difference is approximately 0 if the mounting errors φ _X and φ _Y are small angles. After all, the mounting errors φ _X , φ _Y , φ _Z around the X, Y, Z axes can be estimated based on the observed values as shown in Table 1 below.

[0043]

Table 1 Mounting error around X axis φ _X : Y acceleration difference, YZ angular velocity difference Mounting error around Y axis φ _Y : X acceleration difference, X / Z angular velocity difference Mounting error around Z axis φ _Z : X・ Y angular velocity difference

(2) When the speed difference and the attitude difference between the master coordinate system and the vehicle body coordinate system (here, the attitude difference includes the azimuth angle difference) are used as observation values: Body I
The posture difference Δψ and the speed difference Δv of the NS 20 are obtained.

[0045]

Equation 14] dΔψ / dt = - [omega '×] [Delta] [phi] + [omega' ×] φ dΔv / dt = -C ^L [a '×] [Delta] [phi] + C ^L [a' ×]
φ ω ′: angular velocity of the vehicle body coordinate system a ′: acceleration of the vehicle body coordinate system, C ^L : direction cosine matrix of the master INS

The matrices [ω'x] and [a'x] are expressed as follows.

[0047]

(Equation 15)

Therefore, when the equation of Expression 14 is expressed in a matrix form, the following is obtained.

[0049]

(Equation 16)

When this equation is compared with the equation (1), the following relation is obtained.

[0051]

[Equation 17]

The observed values are the speed difference y (Δv) and the attitude difference y (Δv
ψ). Therefore, it is expressed in the form of the equation (3).

[0053]

(Equation 18)

Here, I ₃ is a 3 × 3 unit matrix, and 0 ₃ is 3
It is a × 3 0 matrix. When this equation is compared with the equation (3), the following relationship is obtained.

[0055]

[Equation 19]

From the equation (16), it can be seen that the mounting errors φ _X , φ _Y , φ _Z around the X _, Y _, Z axes can be estimated based on the following observations, respectively. Mounting error about the X axis phi _X: V _Y - V _Z speed difference, mounting error of about the pitch angle and direction difference Y axis phi _Y: V _{X-V Z} speed difference, mounting error of about the roll angle, the misorientation Z axis φ _Z: V _X · V _Y speed difference, the roll angle and the pitch angle difference

However, the ship does not have a large acceleration motion, and the speed difference is dominated by the influence of the acceleration due to the gravity component. That is, when the mounting errors φ _X and φ _Y exist around the X _and Y axes _, and a gravity component appears on the other axis, a speed difference occurs. However, V _Z speed difference is by mounting error phi _X, a phi _Y and small angle, approximately zero. As a result, the mounting errors φ _X , φ _Y , φ _Z around the X _, Y, Z axes can be estimated based on the following observation values.

[0058]

[Table 2] Mounting error around X axis φ _X : V _Y speed difference, pitch angle / azimuth difference Mounting error around Y axis φ _Y : V _X speed difference, roll angle / azimuth difference Mounting error around Z axis φ _Z : Roll angle / pitch angle difference By the way, the speed difference that can be used as an observation value is
ΔV _n (North-South speed difference), ΔV _e (East-West speed difference), ΔV _d
(Vertical speed difference). When (ΔV _X , ΔV _Y , ΔV _Z ) is subjected to coordinate transformation by a coordinate transformation matrix ( ^CL ), (ΔV _n , ΔV
_e , ΔV _d ).

[0059]

[0008] In addition to the fixed mounting error φ between the master INS 10 and the marine vessel INS 20, the hull distortion δ (variation component of the mounting error φ) caused by the motion of the ship is caused. There are deviations. Now, suppose that the motion of the ship is expressed as follows.

[0060]

R = R _o · sin (ω _R · t + α _R ) P = P _O · sin (ω _P · t + α _P ) A = A _O · sin (ω _A · t + α _A ) R: Roll angle P: Pitch corner a: azimuth R _O: amplitude of the roll angle P _O: the pitch angle amplitude a _O: amplitude of azimuth _{ω R: = 2π / T R} (T R: upset period of roll) ω _P: = 2π / T _P (T _P: upset period of _{pitch) ω a: = 2π / T} a (T a: orientation of upset period) alpha _R: roll upset phase alpha _P: upset phase alpha _a pitch: orientation upset phase

Further, it is assumed that the distortion δ of the hull is expressed as follows.

[0062]

[Number 21] _{δ R = δ RO · sin (} ω R · t + β R) δ P = δ PO · sin (ω P · t + β P) δ A = δ AO · sin (ω A · t + β A) δ R: roll strain [delta] _P: pitch strain [delta] _a: azimuth strain [delta] _RO: amplitude of roll distortion [delta] _PO: amplitude of pitch distortion [delta] _AO: azimuth strain amplitude _{ω R: = 2π / T R} (T R: period of roll distortion) omega _{P: = 2π / T P (} T P: pitch period of _{strain) ω a: = 2π / T} a (T a: orientation strain cycle) beta _R: roll distortion phase beta _P: pitch distortion phase beta _a: Azimuth distortion phase

The difference between the angular velocities detected by the master INS 10 and the vehicle INS 20 is caused by the mounting errors φ _X , φ _Y , and φ _Z around the X, Y, and Z axes. Consider only φ _Z. Also, orientation upset A and azimuth strain [delta] _A of the micro-phase alpha _A upset and orientation distortion of azimuth, beta _A is assumed to be 0. The following equation is obtained from the equation (12).

[0064]

(Equation 22)_X= −δ_RO・ Ω_R・ Cos (ω_R・ T)
−φ_Z・ P_O・ Ω_P・ Cos (ω _P・ T) ・ cosR Δω_Y= −δ_PO・ Ω_P・ Cos (ω_P・ T) ・ cosR
+ Φ_Z・ R_O・ Ω_R・ Cos (ω_R・ T)

This is a value actually observed. In the Kalman filter, since the distortion is not modeled, the estimation is performed by judging that the observed angular velocity difference is caused only by the mounting error φ. The Kalman filter model formula is as follows.

[0066]

[Number 23] _{Δω X = -φ Z · P O} · ω P · cos (ω P
・ T) ・ cosR Δω _Y = + φ _Z・ R _O・ ω _R・ cos (ω _R・ t)

Roll and pitch fluctuations and distortion angular frequency ω
_R, omega _P equals (ω _R = ω _P), the roll angle R is set to be small (cosR = 1), a mounting error about the Z axis φ
The estimated value of _Z is estimated as follows using the X angular velocity difference and the Y angular velocity difference as observation values.

[0068]

２４ _Z (estimated value) = φ _Z (true value) + δ _RO / _PO φ _Z (estimated value) = φ _Z (true value) −δ _PO / R _O

Although the above discussion has considered the mounting error φ _Z around the Z axis, it can be similarly obtained when the mounting errors φ _X and φ _Y around the X and Y axes are considered.

[0070]

２５ _X (estimated value) = φ _X (true value) + δ _PO / A _O φ _X (estimated value) = φ _X (true value) −δ _AO / P _O

[0071]

## EQU26 ## φ _Y (estimated value) = φ _Y (true value) −δ _RO / A _O φ _Y (estimated value) = φ _Y (true value) + δ _AO / R _O

The equations (24) to (26) estimate the mounting error φ using the angular velocity difference and the acceleration difference as observation values, but the same applies to the case where the velocity difference and the attitude angle difference are used as observation values (details are omitted). Equation is obtained.

As described above, in the conventional transfer alignment method, the mounting errors φ _X and φ _X around the X, Y and Z axes
_Y, estimate of phi _Z includes due to the distortion δ of the hull error (the second term of the expression on the right of the formula to a few 26 number 24).

In view of the foregoing, the present invention provides an inertial navigation system for estimating the mounting error and the attitude and orientation of the hull by the transfer alignment method, in which the error of the estimated value of the mounting error φ caused by the distortion δ of the hull is reduced. It is intended to reduce it.

[0075]

According to the present invention, there is provided an inertial navigation apparatus configured to estimate a mounting error in initialization of a posture and an azimuth of a vehicle mounted on a vehicle using a Kalman filter. In estimating the mounting error around the X and Y axes, the estimation based on the difference in acceleration is weighted more than the difference in angular velocity obtained by the inertial navigation device and the main inertial navigation device mounted on the navigation body. , The error covariance matrix R of the observed disturbance vector of the Kalman filter
The angular velocity difference of _E is set to a value larger than the standard value.

According to the present invention, in the inertial navigation system,
In the estimation of mounting error about the Z axis, the placed weights to estimate based on the X angular difference than Y angular difference determined by the two inertial navigation system, the Y velocity difference and X angular difference of the matrix R _E The value is set to be R _E (X angular velocity difference) < _RE (Y angular velocity difference).

According to the present invention, in the inertial navigation device configured to estimate the mounting error in the initialization of the attitude and azimuth of the navigation body equipped on the navigation body using the Kalman filter, the X-axis and the Y-axis are provided. In estimating the surrounding mounting error, weights are given to the estimation based on the speed difference rather than the roll angle difference, the pitch angle difference, and the azimuth angle difference obtained by the inertial navigation device and the main inertial navigation device mounted on the navigation body. Place the roll angle difference of the error covariance matrix R _E of the observation disturbance vector of the Kalman filter, characterized by being set to a value greater than the standard value pitch angle difference and azimuth difference.

According to the present invention, in the inertial navigation system,
In estimating the mounting error around the Z axis, the above two inertial navigation
Angle difference than pitch angle difference obtained by the method device
Is weighted, and the matrix R_ERoll angle
Difference and pitch angle difference_E (Roll angle difference) <R_E(Pitch angle difference). Furthermore, the mounting error around the Z axis
In the estimation, the inertial navigation device and the
Based on the pitch angle difference determined by the selected main inertial navigation system.
And the mounting error φ_ZPitch angle difference than estimating
System disturbance of the Kalman filter
Kuttle's error covariance matrix Q_EMounting error φ _ZAnd pitch angle
The difference, Q_E(Mounting error φ_Z) <Q_E(Pitch angle difference).

[0079]

DESCRIPTION OF THE PREFERRED EMBODIMENTS As shown in Tables 1 and 2 above, when estimating a mounting error φ by a Kalman filter, a plurality of parameters can be selected as observation values. In the present invention, these parameters are weighted. According to the present invention, the estimation from observation values is weighted, and to what extent the estimation is weighted is distributed to which observation value, whereby the error caused by the hull distortion δ included in the mounting error φ is reduced. Minimize.

A parameter related to the estimation weight is the Kalman gain K. The Kalman gain K indicates to what extent which state vector is estimated from which observation value. The Kalman gain K is equal to the above-described error covariance matrix P _E , Q
_{It depends} on _E and RE. Therefore, the error covariance matrices P _E , Q _E , and R _E shown in Expression 6 may be set as design values.
More specifically, as design values, the initial value P _EI of the state vector error covariance matrix P _E , the system disturbance vector error covariance matrix Q _E , and the observed disturbance vector error covariance matrix R
Set the value of _E.

Referring to FIG. 6, Kalman gain K and matrix P
_EI, Q _E, the relationship of R _E will be described. FIG. 6 is a graph showing a response curve of the Kalman gain K. As an example, assuming that the X angular velocity difference Δω _X is the observed value, the mounting error φ _Z around the Z axis
Is described. 6, the vertical axis represents the Kalman gain K (φ _Z , Δω _X ) [min / (rad /
sec)] and indicates how much the mounting error φ _Z is estimated when the observed value Δω _X is observed. The horizontal axis is time [sec].

As shown in the figure, generally, the initial time (time t ₁ )
Of the Kalman gain K is large, then decreases, and becomes constant in a steady state (time t ₂ ).

(A) Error covariance matrix P _{E of} state vector
Initial value P _EI : From this initial value P _EI , the initial Kalman gain K of the corresponding state vector can be determined. The initial Kalman gain K increases as the value of the element of the matrix P _EI increases.

(B) Error covariance matrix Q _{E of} system disturbance vector: The Kalman gain K of the corresponding state vector can be determined by the matrix Q _E. The Kalman gain K in the steady state increases as the value of the element of the matrix Q _E increases.

[0085] (c) the error covariance matrix of the observation disturbance vector R _E: to determine the overall value of the Kalman gain K is a matrix R _E. For example, the solid curve larger elements of the matrix R _E, the dashed curve small elements of the matrix R _E is. Matrix R
_E represents the precision of the corresponding observation. As the value of the elements of the matrix R _E is large, observations indicate that contain many errors. Therefore, when the value of the elements of the matrix R _E is large, to avoid using the observed value.

[0086] Here, the matrix R _E determined by the accuracy of the observed value will be referred to as the standard value. Observed value Δω, Δ
The accuracy of a, Δψ, and Δv is determined by the performance of the sensor and the like, but includes errors due to the hull distortion δ. This error δ
The value that does not include is the standard value.

When the mounting error φ _Z is estimated using the angular velocity difference and the acceleration difference as observation values, six parameters Δω _X , Δω
ω _Y , Δω _Z , Δa _X , Δa _Y , and Δa _Z can be used as observation values. Accordingly, six Kalman gains K (φ _Z , Δω _X ), K (φ _Z , Δω _Y ), and K
_{(Φ Z, Δω Z),} K (φ Z, Δa X), K (φ Z, Δ
a _Y ) and K (φ _Z , Δa _Z ).

By setting the elements of the matrices P _EI , R _E , and Q _E , the magnitude relation between the six Kalman gains K is determined. That is, by designing the matrices P _EI , R _E , and Q _E , it is possible to determine which state vector is estimated from which observation value and to what extent.

First, a description will be given of a case where the mounting error φ is estimated using the acceleration difference and the angular velocity difference between the vehicle body coordinate system and the master coordinate system as observation values, and then the speed difference and attitude between the vehicle body coordinate system and the master coordinate system. A case in which the mounting error φ is estimated using the angle difference as an observation value will be described.

(1) In the case of estimating the mounting error φ using the acceleration difference and the angular velocity difference as observation values: The state vector [x] and the observation vector [y] of the estimated value of the equation (3) are expressed as follows. .

[0091]

[Equation 27]

The system matrix [A] becomes zero according to the equations (1) and (7).

[0093]

[Equation 28]

The observation matrix [H] is as follows from the equations (3) and (12).

[0095]

(Equation 29)

The initial value P of the error covariance matrix of the state vector
_EI, error covariance matrix R _E of the error covariance matrix Q _E and the observation disturbance vector of the system disturbance vector, respectively as follows.

[0097]

[Equation 30]

[0098]

(Equation 31)

[0099]

(Equation 32)

[0100] Here, lowercase p, q, r are each matrix P _EI, Q _E, shows the that the diagonal elements of R _E.

As shown in Table 1, the mounting errors φ _X , φ _Y , and φ _Z around X, Y, and Z can be estimated based on two or more types of observation values. Which put weight on extrapolation from the observed values are determined by the error covariance matrix R _E of the observed disturbance vector of the design value of the Kalman filter. The matrix R _E represents the precision of the observations, and among the elements of the matrix R _E ,
The estimation is weighted for parameters set to small values.

(1-A) Mounting error φ _X around the X and Y axes
φ _Y : As shown in Table 1, the mounting error φ around the X and Y axes
_X, phi _Y is possible to estimate the acceleration difference or the angular velocity difference as an observed value. Compared to the angular velocity difference, the acceleration difference is the hull distortion δ
Is small, the mounting errors φ _X and φ _Y around the X and Y axes are weighted by estimation from the acceleration difference. In this example, that to weight the estimate of the acceleration difference is determined by the error covariance matrix R _E of the observation disturbance vector. Since matrix R _E is representative of the accuracy of the observations, to placing the weight to less observed value of this value.

[0103] That is, among the elements of the matrix R _E shown in the numerical formula 32, the angular velocity difference rΔω _X, rΔω _Y, rΔω acceleration difference value of _{Z rΔa X, rΔa Y,} is greater than the value of rΔa _Z. Although the two cannot be directly compared due to their different dimensions, the values of the angular velocity differences rΔω _X , rΔω _Y , and rΔω _Z are set to be larger than their standard values. Thereby, the acceleration difference Δa
The estimation from _X , Δa _Y and Δa _Z is weighted.

(1-B) Mounting error φ _Z around Z axis: Table 1
As shown in the above, the mounting error φ _Z around the Z axis can be estimated using the XY angular velocity difference as an observation value. However, when the attitude angle includes an error due to the distortion δ of the hull, the estimated value of the mounting error φ _Z around the Z axis also includes an error. The mounting error φ _Z that estimates the X angular velocity difference and the Y angular velocity difference as observed values includes the errors δ _RO / P _O and δ _PO / R _O as shown in Expression 24.

If the weighting of the estimation from the X angular velocity difference and the Y angular velocity difference is equal, the mounting error φ by the Kalman filter
The estimated value of _Z is expressed by the following empirical formula (empirical formula).

[0106]

[Equation 33]

In general, in the distortion δ of a ship, the distortion δ _P of the hull around the pitch axis is considerably larger than the distortion δ _R of the hull around the roll axis. Therefore, when the Y angular velocity difference, which is greatly affected by the hull distortion δ, is used as the observation value, the error included in the estimated value of the mounting error φ _Z increases. Therefore the following relation holds with respect to the error contained in the estimated value of the mounting error phi _Z.

[0108]

ERR (φ _Z , Δω _X ) <ERR (φ _Z , Δ
ω _X + Δω _Y ) <ERR (φ _Z , Δω _Y )

Here, each symbol has the following meaning. ERR (φ _Z , Δω _X ): Error included in mounting error φ _Z estimated only from X angular velocity difference ERR (φ _Z , Δω _X + Δω _Y ): Error included in mounting error φ _Z estimated from XY angular velocity difference Error ERR (φ _Z , Δω _Y ): Error included in mounting error φ _Z estimated only from Y angular velocity difference

As described above, the mounting error φ _Z around the Z axis is estimated using the X angular velocity difference, which is less affected by the hull distortion δ, than the Y angular velocity difference, which is more influenced by the hull distortion δ, as the observed value. That to weight the estimates from X angular difference is determined by the error covariance matrix R _E of the observation disturbance vector. Since matrix R _E is representative of the accuracy of the observations, to placing the weight to less observed value of this value.

[0111] That is, among the elements of the matrix R _E shown in the numerical formula 32, the value of X the difference in angular velocity Aruderutaomega _X smaller than Y angular difference Aruderutaomega _Y. Thereby, the estimation from the X angular velocity difference is weighted.

[0112]

[Expression 35] rΔω _X <rΔω _Y

Between the two design values rΔω _X , rΔω _Y ,
For example, the following ratio is set.

[0114]

RΔω _X : rΔω _Y = 1: 100

(2) When the mounting error φ is estimated using the velocity difference and the attitude angle difference (roll angle difference, pitch angle difference and azimuth angle difference) as observation values: State vector [x] of the estimated value of equation (3) ,
The observation vector [y] is expressed as follows.

[0116]

(37)

The system matrix [A] is as follows from the equations (1) and (16).

[0118]

(38)

The observation matrix [H] is as follows from the equations (3) and (18).

[0120]

[Equation 39]

Initial value P of error covariance matrix of state vector
_EI, error covariance matrix R _E of the error covariance matrix Q _E and the observation disturbance vector of the system disturbance vector, respectively as follows.

[0122]

(Equation 40)

[0123]

[Equation 41]

[0124]

(Equation 42)

As shown in Table 2 above, the mounting errors φ _X , φ _Y , and φ _Z around X, Y, and Z can be estimated based on two or more types of observation values. Which of the observations is weighted for the estimation is determined by the Kalman filter design values P _EI , Q _E ,
Determined by the error covariance matrix R _E of the observed disturbance vector of R _E. The matrix R _E represents the precision of the observations and the matrix R
Among the elements of _E , estimation is weighted for parameters set to small values.

(2-A) Mounting error φ _X around X and Y axes,
φ _Y : Since the speed difference Δv is less affected by the strain δ than the attitude angle difference Δψ (roll angle difference, pitch angle difference and azimuth angle difference), the mounting errors φ _X and φ _Y around the X and Y axes are Speed difference Δv
Weight the estimate from.

[0127] That is, among the elements of the matrix R _E shown in the numerical formula 42, the attitude angle difference rΔψ _R, rΔψ _P, a value speed difference than Aruderutav _n of rΔψ _A, rΔv _e, decreasing the value of rΔv _d. Since the two have different dimensions, they cannot be directly compared, but the values of the attitude angle differences rΔψ _R , rΔψ _P , and rΔψ _A are made larger than their standard values. Thus, the speed difference Δ
v _n, Δv _e, weighted attached to estimate from Delta] v _d.

(2-B) Mounting error φ _Z around the Z axis: Table 2
As shown in the above, the mounting error φ _Z around the Z axis can be estimated using the roll difference and the pitch difference as observation values. But,
If the roll angle or the pitch angle includes an error due to the hull distortion δ, the estimated value of the mounting error φ _Z around the Z axis includes the error. As in the case where the angular velocity difference is estimated as the observed value, the estimated value of the mounting error φ _{Z using} the roll difference and the pitch difference as the observed values is the error δ _RO / P _O , δ _PO /
Including R _O.

When the weights estimated from the roll difference and the pitch difference are equal, the mounting error φ _Z by the Kalman filter is used.
Is also represented by an empirical formula (experimental formula) of Formula 33. Here, with respect to strain [delta] of the ship, the strain [delta] _P around the pitch axis than the strain [delta] _R of the hull about the roll axis is found to be quite large. Therefore, if a pitch difference that is greatly affected by the hull distortion δ is used as an observation value, an error increases. The following relational expression holds for the error included in the mounting error φ _Z.

[0130]

ERR (φ _Z , ΔR) <ERR (φ _Z , ΔR)
+ ΔP) <ERR (φ _Z , ΔP)

Here, each symbol has the following meaning. ERR (φ_Z, ΔR): Mounting estimated only from roll difference
Error φ_ZError ERR (φ_Z, ΔR + ΔP): Roll difference and pitch difference
Mounting error φ estimated from _ZError ERR (φ_Z, ΔP): Mounting estimated only from pitch difference
Error φ_ZError included in

As described above, the mounting error φ _Z around the Z-axis is estimated by using the roll difference as an observation value rather than the pitch difference which is greatly affected by the hull distortion δ. Specifically, the values of R _E, which represents the accuracy of observations of the Kalman filter is set as follows, giving greater weight to the estimate from the roll difference.

[0133]

[Equation 44] rΔψ _R <rΔψ _P

(2-C) Mounting error φ _Z around the Z-axis: Also, the estimation of the roll difference from the observed value can be weighted by the value of Q _E. The value of Q _E determines which estimation item is to be estimated more, and the larger the value is, the more the estimation item is estimated.

When estimating the speed difference, the roll difference, the pitch difference, and the azimuth difference as observation values, there are estimation items such as the speed difference, the roll difference, the pitch difference, and the azimuth difference in addition to the attachment error φ. Therefore, when the pitch difference is used as the observation value, the mounting error φ
Since the pitch difference due to addition to the hull of the distortion δ such a pitch difference due to _Z is abundant, to the design value, such as to estimate the pitch difference itself than mounting error phi _Z.

[0136]

[Equation 45] qφ _Z <qΔψ _P

As shown in the first equation of the equation (22), when the mounting error φ _Z around the Z axis is estimated using the roll difference or the X angular velocity difference as an observed value, the error δ _RO / _PO is included. However, since the strain δ _RO around the roll axis is very small, it becomes as follows.

[0138]

[Equation 46] φ _Z (estimated value) ≒ φ _Z (true value)

Therefore, according to the present invention, it is possible to accurately determine the mounting error φ. In the above discussion, the case where the angular velocity difference or the acceleration difference is used as the observed value and the case where the speed difference or the attitude angle difference is used as the observed value have been described. However, a combination of these parameters may be used as the observed value.

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 navigational vehicle INS mounted on a ship has been described above as an example, the present invention relates to any inertial navigation device configured to perform initialization using a transfer alignment method. It is also applicable to inertial navigation systems. For example, underwater vehicles, unmanned aerial vehicles,
The present invention is applicable to a vehicle such as an unmanned submarine.

Further, the present invention is not limited to the inertial navigation device mounted on the navigation vehicle, but may be any inertial navigation device configured to perform initialization using the transfer alignment method. The present invention can also be applied to an inertial navigation device mounted on a launch vehicle launching device such as a flying vehicle launch device.

[0143]

According to the present invention, when initialization is performed using the transfer alignment method, it is possible to accurately estimate the mounting error φ even if the hull distortion δ exists,
There is an advantage that the initialization performance of the vehicle can be improved.

According to the present invention, when initialization is performed using the transfer alignment method, the mounting error φ can be accurately estimated even when the hull distortion δ is present. Even when the hull distortion is relatively large, there is an advantage that the navigation body can be operated.

[Brief description of the drawings]

FIG. 1 is a diagram illustrating an arrangement state of an inertial navigation device mounted on a ship and a cruising vehicle.

FIG. 2 is a view showing an arrangement state of a traveling body mounted on a ship.

FIG. 3 is a diagram showing an arrangement state of an inertial navigation device mounted on a ship and a cruising vehicle.

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

FIG. 5 is a diagram illustrating a relationship between a detected angular velocity of a master INS and a detected angular velocity of a marine vessel INS.

FIG. 6 is an explanatory diagram for explaining a response of a Kalman gain.

[Explanation of symbols]

1 ship, 2 hull, 3 hull launch tube, 10
Master INS, 20 Aircraft INS, 30 Slave 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 (7)

[Claims] 1. An inertial navigation device 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, wherein the mounting around an X axis and a Y axis is performed. In the estimation of the error, the estimation based on the difference in acceleration is weighted rather than the difference in angular velocity obtained by the inertial navigation device and the main inertial navigation device mounted on the navigation body, and the error in the observed disturbance vector of the Kalman filter is shared. that the difference in angular velocity of the covariance matrix R _E with the set standard value to a larger value the inertial navigation system according to claim. 2. The inertial navigation device according to claim 1, wherein
In the estimation of mounting error about the Z axis, the placed weights to estimate based on the X angular difference than Y angular difference determined by the two inertial navigation system, the Y velocity difference and X angular difference of the matrix R _E An inertial navigation apparatus characterized in that a value is set as _RE (X angular velocity difference) < _RE (Y angular velocity difference). 3. An inertial navigation device configured to estimate a mounting error in initializing a posture and an orientation of a navigation body mounted on a navigation body using a Kalman filter, wherein the mounting around the X axis and the Y axis is performed. In estimating the error, the Kalman filter weights the estimation based on the speed difference rather than the roll angle difference, pitch angle difference, and azimuth angle difference obtained by the inertial navigation device and the main inertial navigation device mounted on the navigation body. observation roll angle difference error covariance matrix R _E of the disturbance vector, inertial navigation system, characterized in that set to a value greater than the standard value pitch angle difference and azimuth difference. 4. The inertial navigation device according to claim 3, wherein
In the estimation of mounting error about the Z axis, the placed weights to estimate based on the roll angle difference than the pitch angle difference determined by the two inertial navigation system, a roll angle difference and the pitch angle difference of the matrix R _E , R _E (roll angle difference) <R _E (pitch angle difference). 5. The inertial navigation device according to claim 3, wherein the estimation of the mounting error about the Z axis is based on a pitch angle difference obtained by the inertial navigation device and a main inertial navigation device mounted on the navigation body. the pitch angle difference and mounting error phi _Z of the error covariance matrix Q _E system disturbance vector of the Kalman filter, to estimate the pitch angle difference itself than to estimate the mounting error phi _Z Te Q _E (mounting error phi _Z) <inertial navigation system, characterized in that the Q _E (pitch angle difference). 6. The inertial navigation device according to claim 1, wherein the inertial navigation device is mounted near the navigation device itself or a launching device of the navigation device. And inertial navigation equipment. 7. The inertial navigation device according to claim 1, wherein a fixed device is provided on the navigation body in place of the navigation body. .