JPH0731063B2 - Inertial device - Google Patents

Description

Detailed Description of the Invention

[0001]

BACKGROUND OF THE INVENTION 1. Field of the Invention The present invention inputs an attitude angle and azimuth signal from an external reference inertial navigation system (hereinafter referred to as an external INS) to deviate its own mounting angle from the external INS mounting angle. The present invention relates to an inertial navigation device (hereinafter referred to as INS) that calculates attachment misalignment), calculates the attitude angle and azimuth angle of the aircraft on which it is mounted based on the deviation data, and outputs the result to the outside.

[0002]

2. Description of the Related Art An INS 1 of this type is built in, for example, a navigation body (for example, a missile) mounted on an aircraft, and an external INS 2 is an INS of the aircraft.
The conventional INS1 will be described with reference to FIG. External INS
2 detects the rotational angular velocities ωX, ωY, ωZ around the input axes X, Y, Z unique to itself from the input angular velocity ω, and based on these detected values, the roll angle φ _R , pitch angle θ _R and Calculate the azimuth angle ψ _R. The roll angle and the pitch angle are collectively referred to as the posture angle. The angular velocities ωX, ωY, ω
Z is input to the angular velocity difference value calculation unit 3 of the INS ₁ , and the posture angles φ _R and θ _R and the azimuth angle ψ _R are input to the correction calculation unit 4 of the INS 1. A gyroscope (hereinafter, simply referred to as a gyro) 5 incorporated in the INS 1 has an input axis (also referred to as an input axis of the INS 1) peculiar to itself based on an input angular velocity ω.
The rotational angular velocities ωx, ωy, ωz around the respective axes are detected and supplied to the angular velocity difference value calculation unit 3 and the attachment misalignment estimation calculation unit (hereinafter referred to as Kalman filter) 6. The angular velocity difference value calculation unit 3 subtracts the angular velocity detected by the external INS 2 from the angular velocity detected by the gyro 5 to calculate deviation values δωx = ωx−ωX, δωy = ωy−ωY, δωz = ωz−ωZ (1) , To the Kalman filter 6.

The Kalman filter 6 uses these deviation values and the angular velocities ωx, ωy, ωz input from the gyro 5 to determine the mounting misalignment φX, φY, φZ for the X, Y, Z axes of the INS 1 as shown in FIG. The calculated value is supplied to the correction calculation unit 4. The correction calculator 4 calculates the roll angle φ, the pitch angle θ, and the azimuth angle ψ of the INS1 from these misalignment and the roll angle φ _R , the pitch angle θ _R , and the azimuth angle ψ _{R of the} external INS2, and outputs them to the outside. .

Next, the essential points of the known Kalman filter will be described. N rows 1 to be estimated in a system
Column data (state vector) [X] ([] represents a matrix. The same applies hereinafter) is modeled by a differential equation represented by d [X] / dt = [F] [X] + [U] (2) If the quantity [Z] obtained by multiplying [X] by some coefficient [H], that is, [Z] = [H] [X] + [V] (3) is observable in an actual system, the Kalman filter is The estimated data [Ξ] (n rows and 1 column) of the data [X] is calculated as follows. In addition, the above [H] (m
(N rows) is the observation matrix, [Z] (m rows and 1 column) is the observation vector, [U] (n rows and 1 column) is the system noise vector,
[F] (n rows and n columns) is the system matrix, and [V] (m rows and 1)
Column) is called the observation noise vector.

The calculation is performed discretely for each cycle Δt, and the estimated data [Ξ] and its covariance matrix [P] (n rows and n columns) are updated as follows. [Ξ] _k ^- = [Φ] _k-1 [Ξ] _k-1 ⁺ (4) (P) _k ^- = [Φ] _k-1 (P) _k-1 ⁺ [Φ] _k-1 ^T + [ Q] _k-1 (5) [Φ] is a state transition matrix (n rows and n columns), and [Φ] = exp
([F] Δt). [Q] (n rows and n columns) is the covariance matrix of [U]. The subscript k means the kth calculated value. When observation is performed, the Kalman gain (K) to (K) _k = (P) _k ^- [H _k] ^T ([H] _k (P) _k ^- (H) _k ^T + [R] _k) ^-1 (6), and the state vector estimator [Ξ] and its covariance [P] are corrected as follows. [Ξ] _k ⁺ = [Ξ] _k ^- + [K] _k ([Z] _k - [H] _k [Ξ] _k ^-) (7) (P) _k ⁺ = ([1] - [K] _k [H] _k ) [P] _k ^- ([1]-[K] _k [H] _k ) ^T + [K] _k [R] _k [K] _k ^T (8) Note that "-" in the index is Kalman. “+” Means a value before correction calculation by the gain, “+” means a value after correction calculation, and “T” means a transposed matrix. By repeating such a correction operation every cycle Δt, the value of the estimated data [Ξ] approaches the value of the actual state vector [X]. In this way, the estimated data [X] of [X] is obtained.

In the conventional INS, [X] = [x ₁₁ x ₂₁ x ₃₁ ] ^T = [φXφYφZ] ^T (9), and the system model corresponding to the equation (2) is d [φXφYφZ] ^T / dt =

[0] [φXφYφZ] ^T + [uXuYuZ] ^T (10) Further, by using the rotational angular velocities ωx, ωy, ωz of the element h _ij of the observation matrix [H] of the formula (3), [h ₁₁ h ₁₂ h ₁₃ ] = [0, −ωz, ωy]; [h ₂₁ h ₂₂ h ₂₃ ] = [ωz, 0, −ωx]; [h ₃₁ h ₃₂ h ₃₃ ] = [− ωy, ωx, 0] (11), and the observation vector [Z] is [Z] = [z ₁₁ z ₂₁ z ₃₁ ] ^T = [δωxδωyδωz] ^T (12), the estimated values of the mounting misalignments φX, φY, and φZ are obtained by the calculation by the Kalman filter described above. Note that δωx, δωy, and δωz in the equation (12) are (1)
It is shown by the formula.

[0007]

In the conventional INS1,
Angular velocity signals ωx, ωy, ω detected by own gyro 5
z is directly compared with the reference angular velocity signals ωX, ωY, ωZ of the external INS2 to obtain deviation values δωx, δωy, δωz, and mounting misalignments φX, φY, φ are obtained from these deviation values.
Since Z is estimated, the angular velocity signals ωx, ωy, ωz
Alternatively, if there is a lot of noise superimposed on the reference angular velocity signals ωX, ωY, ωZ, the mounting misalignment φX, φY, φZ will occur.
However, there is a drawback that the accuracy of calculation of is decreased and the error of the attitude angles φ and θ; An object of the present invention is to solve these conventional drawbacks, reduce the influence of noise of the angular velocity signal, and improve the accuracy of the attitude angles φ and θ; the azimuth angle ψ.

[0008]

According to the present invention, a reference direction cosine matrix calculation unit, a gyroscope, a direction cosine matrix calculation unit, an angle error calculation unit, a rotation angle calculation unit, a Kalman filter, and a correction calculation unit. The present invention relates to an inertial device including: The reference direction cosine matrix calculation unit inputs the roll angle φ _R , the pitch angle θ _R, and the azimuth angle ψ _R of the airframe from an external INS serving as a reference, calculates the reference direction cosine matrix [C _R ], and calculates It is supplied to the direction cosine matrix calculator and the angle error calculator.

The gyroscope is unique to itself,
Rotational angular velocity ω around input axes x, y, z orthogonal to each other
x, ωy, ωz are measured and supplied to the direction cosine matrix calculation unit and the rotation angle calculation unit. The direction cosine matrix calculation unit initializes the reference direction cosine matrix [C _R ] at a predetermined cycle T, and then the rotational angular velocities ωx, ω.
The direction cosine matrix [C] is calculated using y and ωz and supplied to the angle error calculator.

The angle error calculation unit samples the direction cosine matrix [C] at the time immediately before the initialization of the direction cosine matrix calculation unit and the reference direction cosine matrix at the same time at the cycle T. And reference rotation angles θX, θY, θ
Rotation angle θ with respect to Z (rotation angle around the input axis X, Y, Z of the external INS based on the measurement value of the external INS)
Deviation δθ of x, θy, θz (rotation angle around the input axis x, y, z based on the measurement value of the gyroscope)
x, δθy, δθz are calculated and supplied to the Kalman filter.

The rotation angle calculation unit resets its output data to zero at the same timing as the initialization of the direction cosine matrix calculation unit, and then the rotation angular velocities ωx, ω.
The rotation angles θx, θy and θz are calculated by integrating y and ωz and supplied to the Kalman filter. The Kalman filter includes the rotation angles θx, θy, θz.
And the rotation angle deviations δθx, δθy, δθz, the mounting misalignment φ with respect to the external INS.
X, φY, φZ are calculated and supplied to the correction calculation unit.

The correction calculation unit is configured to use the roll as a reference.
Angle φ_R, Pitch angle θ_RAnd azimuth angle ψ _RAnd the installation
Using the alignments φX, φY, φZ,
Calculation of angle φ, pitch angle θ and azimuth angle ψ and output to the outside
To do.

[0013]

DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENT An embodiment of the present invention will be described with reference to FIG. In FIG. 1, parts corresponding to those in FIG. 4 are designated by the same reference numerals, and redundant description will be omitted. The direction cosine matrix calculator 11 calculates the direction cosine matrix [C] using the rotational angular velocities ωx, ωy, ωz input from the gyro 5 and supplies the calculated angle cosine to the angle error calculator 12.

The direction cosine matrix is a matrix representing the relationship between two orthogonal coordinate systems. For example, N (North), E (Eas)
t), D (Down), and the orthogonal coordinate axes x, y, and z, the angles formed by the orthogonal coordinate axes are α ₁ , α
₂ , α ₃ ; β ₁ , β ₂ , β ₃ ; γ ₁ , γ ₂ , γ ₃ , if the element at the i-th row and j-th column of the direction cosine matrix [C] is c _ij , then [c ₁₁ c ₁₂ c ₁₃ ] = [cos α ₁ , cos β ₁ , cos γ ₁ ]; [c ₂₁ c ₂₂ c ₂₃ ] = [cos α ₂ , cos β ₂ , cos γ ₂ ]; [c ₃₁ c ₃₂ c ₃₃ ] = [cos α ₂ , cos β ₃ , Cos γ ₃ ] (13).

Of the direction cosine matrix [C] and the angular velocity matrix [Ω]
The relation is represented by a differential equation of d [C] / dt = [C] [Ω] (14) as is well known. Where the angular velocity matrix [Ω] is
The element at row i and column j is ω _ijThen, [ω₁₁ω₁₂ω₁₃] = [0, -ωz, ωy], [ω_{twenty one}ω_{twenty two}ω_{twenty three}] = [Ωz, 0, −ωx], [ω₃₁ω₃₂ω₃₃] = [− Ωy, ωx, 0] (15)

The direction cosine matrix calculator 11 calculates the direction cosine matrix [C] _{n + 1} by the following equation. [C] _{n + 1} = [C] _n ([1] + [Ω] ΔT + (1/2) [Ω] ² ΔT ² + (1/6) [Ω] ³ ΔT ³ + ...) (16) Above In the equation, ΔT is a calculation cycle, and [1] is a unit matrix. The subscript n means the calculated value of the nth time.

The rotation angle calculation unit 13 has a cycle T shown in FIG. 3B.
After resetting the output value to zero at the timing of the clock CK ₂ of, the angular velocities ωx, ω input from the gyro 5
Using y and ωz, the rotation angles θx, θy, and θz
Is calculated and supplied to the Kalman filter 6 (FIG. 3D). θx = ∫ωxdt, θy = ∫ωydt, θz = ∫ωzdt (17) The reference direction cosine matrix calculation unit 14 uses the reference attitude angles φ _R , θ _R, and azimuth angle ψ _R , which are input from the external INS2, according to the following equation. The reference direction cosine matrix [C _R ] is calculated and supplied to the angle error calculation unit 12.

[0018] _{c R11 = cosθ R cosψ R,} c R12 = -cosφ R sinψ R + sinφ R sinθ
_R cos ψ _R , c _R13 = sin φ _R sin ψ _R + cos φ _R sin θ _{R
cosψ R, c R21 = cosθ R} sinψ R, c R22 = cosφ R cosψ R + sinφ R sinθ R
sin ψ _R , c _R23 = −sin Φ _R cos ψ _R + cos Φ _R sin θ
_{R sinψ R, c R31 = -sinθ} R, c R32 = sinφ R cosθ R, in _{c R33 = cosφ R cosθ R (} 18) the above equation represents the element at row i and column j of the [C _R] in c _Rij .

The element c of the above c _Rij and the direction cosine matrix [C]
_The change characteristic of _ij with respect to time is shown in FIG. 3C. c _ij is initialized to a value equal to c _Rij at clock CK ₂ . The angle error calculation unit 12 has a direction cosine matrix [C] at a time point immediately before the next initialization after a lapse of about T time from the time when the direction cosine matrix calculation unit 11 is initialized, and a reference direction cosine matrix [C] at the same time point. C _R ] is sampled at the timing of the clock signal CK _{1 having} the cycle T of FIG. 3A, and the direction cosine error matrix [δC] is calculated by the following equation.

[ΔC] = [C _R ] ^T [C] (19) When the equation (19) is expressed by the elements of each matrix, δc ₁₁ = c _R11 c ₁₁ + c _R21 c ₂₁ + c _R31 c ₃₁ , δc ₁₂ = c _R11 c ₁₂ + c _R21 c ₂₂ + c _R31 c ₃₂ , δc ₁₃ = c _R11 c ₁₃ + c _R21 c ₂₃ + c _R31 c ₃₃ , δc ₂₁ = c _R12 c ₁₁ + c _R22 c ₂₁ + c _R32 c ₃₁ , δc ₂₂ = c _R12 c ₁₂ + c _R22 c ₂₂ + c _R32 c ₃₂ , δc ₂₃ = c _R12 c ₁₃ + c _R22 c ₂₃ + c _R32 c ₃₃ , δc ₃₁ = c _R13 c ₁₁ + c _R23 c ₂₁ + c _R33 c ₃₁ , δc ₃₂ = c _R13 c ₁₂ + c _R23 c ₂₂ + c _R33 c ₃₂ , δc ₃₃ = c _R13 c ₁₃ + c _R23 c ₂₃ + c _R33 c ₃₃ , (20) Now, based on φ _R , θ _R , and ψ _R output from the external INS2 (reference direction cosine matrix [ C _R] corresponding to) the reference input shaft X, Y, the rotation angle around the Z and .theta.X, [theta] Y, and .theta.Z, the rotational angular velocity output from the gyro 5 x, .omega.y, and (corresponding to the direction cosine matrix [C]) based on ωz x, y, θx rotation angle about the z-axis, [theta] y, and [theta] z, the latter deviation of the former δθx, δθy, and Derutashitaz. That is, δθx = θx−θX, δθy = θy−θY, δθz = θz−θZ (21) The angle error computing unit 12 calculates these angle deviations δθ from the following equation.
x, δθy, δθz are calculated and supplied to the Kalman filter 6.

Δθx = (δc ₃₂ −δc ₂₃ ) / 2, δθy = (δc ₁₃ −δc ₃₁ ) / 2, δθz = (δc ₂₁ −δc ₁₂ ) / 2 (22) The Kalman filter 6 is represented by the formula (22). Rotation angle error δθ
x, δθy, δθz and the rotation angles θx, θ of the equation (17).
Conventionally, y and θz are sampled at the timing of the clock CK _{1 in} FIG. 3A, and the respective data are used instead of the angular velocity errors δωx, δωy, and δωz and the angular velocity ωx, ωy, and ωz described in the conventional example of FIG. Mounting misalignment φX, φ by the same calculation as explained in the example
The estimated values of Y and φZ are obtained and supplied to the correction calculation unit 4.

Now, the correct direction cosine matrix of INS1 [C
₀ ] is defined as follows. [C _O ] = [C _R ] [ΔC] (23) In the above equation, [C _R ] is a reference direction cosine matrix, and [ΔC] is [first line] = [1, −φZ, φY]; Second row] = [φZ, 1, −φX]; [Third row] = [− φY, φX, 1] (24) The correction calculator 4 calculates the roll angle φ, the pitch angle θ, and the azimuth angle ψ from the element c _oij of [C _o ] by the following equations and outputs them to the outside.

Φ = tan ⁻¹ (c ₀₃₂ / c ₀₃₃ ) (25) θ = sin ⁻¹ (−c ₀₃₁ ) (26) ψ = tan ⁻¹ (c ₀₂₁ / c ₀₁₁ ) (27) In FIG. Although the direction cosine matrix calculation unit 14 is built in the INS 1, the present invention is not limited to this case and may be provided outside the INS 1.

[0024]

As described above, according to the present invention, the angular velocity signal is not directly used for calculating the mounting misalignment of the inertial device, but the angular data obtained by integrating the angular velocity signal is used. The angular velocity noise component is averaged, and the mounting misalignment can be accurately obtained. Therefore, the influence of noise on the posture angles φ and θ azimuth angle ψ is reduced, and the accuracy can be greatly improved.

Further, in the conventional apparatus that calculates using the angular velocity, it is necessary to perform the Kalman filter process for the misalignment calculation at a cycle sufficiently fast with respect to the change in the angular velocity, but in the apparatus according to the present invention, the change in the angular velocity. Since the rotation angle that is integrated and changes more gently is used, the Kalman filter processing for misalignment calculation may be performed at a slow cycle, and the load on the computer can be reduced.

Since the conventional device requires an external angular velocity signal, the external inertial navigation device used as a reference needs a strapdown type inertial navigation device which can also supply an angular velocity signal. As long as it can supply attitude angle and azimuth signals, it is convenient to use a platform-type inertial navigation system in addition to the strapdown type.

[Brief description of drawings]

FIG. 1 is a block diagram showing an embodiment of the present invention.

FIG. 2 is a diagram for explaining a direction cosine matrix defined between two orthogonal coordinate systems sharing an origin.

FIG. 3 is a waveform diagram of a main part of FIG.

FIG. 4 is a block diagram of a conventional INS.

FIG. 5 shows angular velocity input axes X, Y, Z of the external INS2 and I
Angular velocity input axes x, y, z of NS1 and external I of INS1
Mounting misalignment φX, φ based on NS2
The figure which shows Y and (phi) Z.

Claims (1)

[Claims] 1. A reference direction cosine matrix calculation unit and a gyro
A co-op, a direction cosine matrix calculator, an angle error calculator,
Rotation angle calculator, Kalman filter, correction calculator
An inertial device comprising: a reference direction cosine matrix
The calculation unit uses the standard external INS to roll the aircraft.
φ _R, Pitch angle θ_RAnd azimuth angle ψ_REnter the reference method
Cosine matrix [C_R] To calculate the direction cosine matrix
Section and the angle error calculation section,
Gyroscopes have their own unique, orthogonal inputs.
Rotational angular velocities ωx, ωy, ωz around the force axes x, y, z
Measure and measure the direction cosine matrix and the rotation angle
And the direction cosine matrix calculation unit,
The reference direction cosine matrix [C_R] To initialize
Then, the rotational angular velocity ωx, ωy, ωz is used to
Calculate the cosine matrix [C] and supply it to the angle error calculator.
And the angle error calculator calculates the direction cosine.
The direction cosine at the time immediately before the initialization of the matrix calculation unit
Matrix [C] and the reference direction cosine matrix at the same time
The reference rotation angles θX and θ are sampled at the cycle T.
Y, θZ (based on the measured value of the external INS, the external IN
Rotation of S around the input axes X, Y, Z)
Angles θx, θy, θz (measurement values of the gyroscope
Rotation angle about the input axis x, y, z, based on
The deviations δθx, δθy, δθz are calculated to obtain the Kalman
The rotation angle calculation unit is supplied to the filter.
Is the same timing as the initialization of the direction cosine matrix calculation unit.
After resetting its output data to zero with
The rotation angle θx, ωy, ωz is integrated to obtain the rotation angle θx,
Calculates θy and θz and supplies them to the Kalman filter
The Kalman filter has the rotation angle θ.
x, θy, θz and the rotation angle deviations δθx, δθy,
Using δθz, installation error to the external INS
Alignment φX, φY, φZ are calculated and the correction
Is supplied to the calculation unit, and the correction calculation unit
Becomes the roll angle φ_R, Pitch angle θ_RAnd azimuth angle ψ
_RAnd the mounting misalignment φX, φY, φZ
The roll angle φ, pitch angle θ and azimuth angle ψ
Characterized by being calculated and output to the outside,
Inertial device.