Gyro drift estimation method for strapdown inertial navigation system based on heterogeneous dual-inertial navigation

Abstract

The application relates to a gyro drift estimation method of a strapdown inertial navigation system based on heterogeneous dual inertial navigation, which comprises the following steps: 1, subtracting the error state equation of the strapdown inertial navigation system from the error state equation of the rotating inertial navigation system to determine a system joint error equation; 2, determining joint error state variables; 3, establishing a joint state equation based on the determined joint error state variables; 4, taking the position error and the attitude array matching quantity between the dual inertial navigation as observation quantities, establishing an observation equation, constructing a matrix, and combining the latitude difference calculated by the strapdown inertial navigation system and the rotating inertial navigation system, the longitude calculated and the height calculated to establish an observation equation. The application can effectively estimate the gyro drift of the strapdown inertial navigation system under the condition of free disassembly based on the redundant configuration of the heterogeneous dual inertial navigation, can reduce the influence of the gyro drift on the navigation precision, and can realize the purpose of high-precision navigation.

Description

TECHNICAL FIELD

[0001] The present application belongs to the technical field of inertial navigation, and particularly relates to a gyro drift estimation method for a strapdown inertial navigation system based on heterogeneous dual inertial navigation. BACKGROUND

[0002] The navigation error of an inertial navigation system increases with time accumulation, and the inertial navigation system needs to be accurately calibrated before leaving the factory. Due to the influence of external environment and other factors, the device error of the inertial navigation system, especially the gyro drift, will change greatly after long-time use, resulting in a large navigation error and seriously affecting the accessibility of the navigation accuracy index. In order to improve the inertial navigation accuracy, it is necessary to estimate and compensate the gyro drift of the inertial navigation system.

[0003] The gyro drift estimation of the strapdown inertial navigation system usually adopts a two-position method, which utilizes the complete observability of the north gyro drift, places the inertial navigation system at two different positions, and estimates the gyro drift through Kalman filtering. This method needs to disassemble the inertial navigation system and move the position or adjust the heading of the carrier during use, which has poor practicability for the external environment which is inconvenient to disassemble and the marine field which has small maneuverability, and has certain limitations. SUMMARY

[0004] The present application proposes a gyro drift estimation method for the strapdown inertial navigation system based on heterogeneous dual inertial navigation, which can effectively estimate the gyro drift of the strapdown inertial navigation system, reduce the influence of the gyro drift on the navigation accuracy, realize high-precision navigation, avoid repeated disassembly of the strapdown inertial navigation system, reduce the cost of manpower and material resources, and has strong practicality.

[0005] The above object of the present application is realized by the following technical scheme

[0006] A gyro drift estimation method for a strapdown inertial navigation system based on heterogeneous dual inertial navigation, comprising the following steps:

[0007] Step 1, difference between the error state equation of the strapdown inertial navigation system and the error state equation of the rotating inertial navigation system is determined to determine the system joint error equation;

[0008] Step 2, the joint error state variable is determined;

[0009] Step 3, based on the determined joint error state variable, the joint state equation is established;

[0010] Step 4, the position error and the attitude matrix matching quantity between the dual inertial navigation systems are taken as the observation quantity to establish the observation equation, and the gyro drift of the strapdown inertial navigation system in the joint error state variable is estimated through Kalman filtering.

[0011] Moreover, step 1 is specifically:

[0012] i is the inertial coordinate system, n is the navigation coordinate system, b1 is the carrier coordinate system of the strapdown inertial navigation system, b2 is the platform coordinate system of the rotating inertial navigation system, b m is the carrier coordinate system of the rotating inertial navigation system;

[0013] The attitude error of the strapdown inertial navigation system The velocity error The latitude error δL1, the longitude error δλ1, and the height error δh1 and the attitude error of the rotating inertial navigation system The velocity error The difference of the latitude error δL2, the longitude error δλ2, and the height error δh2 δL 12 , δλ 12 , δh 12 As the combined error state, the difference between the error state equations of the two is obtained:

[0014]

[0015] In the formula, is the gyro output error of the strapdown inertial navigation system, which is modeled as the sum of gyro constant drift and white noise ; is the gyro output error of the rotating inertial navigation system, which is modeled as the sum of gyro constant drift and white noise ; is the accelerometer output error of the strapdown inertial navigation system, which is modeled as the sum of accelerometer constant bias and white noise ; is the accelerometer output error of the rotating inertial navigation system, which is modeled as the sum of accelerometer constant bias and white noise ; is the rotation angular velocity of the n system relative to the i system, is the difference of the rotation angular velocity error of the n system relative to the i system related to the latitude error and the velocity error, is the attitude matrix of the strapdown inertial navigation system, is the attitude matrix of the platform of the rotating inertial navigation system, f n is the specific force vector in the n system, is the earth rotation angular velocity, is the angular velocity of the n system relative to the e system, is the difference of the earth rotation angular velocity error related to the latitude error, is the difference of the angular velocity error of the n system relative to the e system related to the latitude error and the velocity error, v n is the velocity vector of the carrier in the n system, RM is the meridian radius of curvature, h is the height of the current position, v N is the northward velocity, v E is the eastward velocity, L is the local latitude, R N is the prime vertical radius of curvature, is the difference between the two northward velocity errors;

[0016] Let μ = [μ x , μ y , μ z ] T is the installation error angle of the SINS relative to the RINS, modeled as a random constant.

[0017] Moreover, in step 2, specifically:

[0018] The gyro constant drifts and the accelerometer constant biases and the installation error angle μ of the two systems are augmented as state variables, and the lower right corner of the numerical subscript is omitted, to obtain the joint error state variables as:

[0019]

[0020] In the formula, [φ E , φ N , φ U ] T represents the joint attitude error, [δv E , δv N , δv U ] T represents the joint velocity error, [δL, δλ, δh] T represents the joint position error, represents the gyro constant drift of the SINS, represents the accelerometer constant bias of the SINS, represents the gyro constant drift of the RINS, represents the accelerometer constant bias of the RINS.

[0021] Moreover, in step 3, the joint state equation is established as:

[0022]

[0023] In the formula, the system noise

[0024] The block matrix form of the system state matrix F(t) is as follows:

[0025]

[0026] In the formula, The non-zero elements in the matrix are:

[0027] f 2,7 =-ω ie sinL,

[0028] f 4,2 =-f U ,f 4,3 =f N , f 5,1 =f U ,f 5,3 =-f E , f 6,1 =-f N ,f 6,2 =f E , f 6,7 =-2v E ω ie sinL, f 9,6 =1;

[0029]

[0030] And in step 4, the established observation equation is as follows:

[0031] Let Z DCM =[Z x ,Z y ,Z z ] T , wherein:

[0032]

[0033] In the formula, M(3, 2) represents the element in the 3rd row and the 2nd column of the M matrix, represents the attitude matrix of the rotating inertial navigation system.

[0034] The functional relationship between Z DCM and the error quantity is as follows:

[0035]

[0036] Let L1, λ1 and h1 respectively represent the latitude, longitude and height calculated by the strapdown inertial navigation system, and let L2, λ2 and h2 respectively represent the latitude, longitude and height calculated by the rotating inertial navigation system, and the observation equation is as follows:

[0037] Z = HX + V

[0038] In the formula, the expression of observation Z and observation matrix H is respectively:

[0039]

[0040] In the formula, 0 3×3 Indicates a 3-order zero matrix, 0 3×12 Indicates a 3-row 12-column zero matrix, I 3×3 Indicates a three-order unit matrix.

[0041] Moreover, in step 4, the position error between the two inertial navigation systems in the observation should deduct the influence of the rod arm error caused by the installation position.

[0042] The advantages and positive effects of the present application are:

[0043] The present application is based on the redundant configuration of heterogeneous dual inertial navigation, fuses the navigation information of the two, can effectively estimate the gyro drift of the strapdown inertial navigation system under the condition of disassembly-free, reduces the influence of gyro drift on navigation accuracy, and realizes the purpose of high-precision navigation. Compared with the prior art, the present application avoids the repeated disassembly of the strapdown inertial navigation system, reduces the cost of manpower and material resources, has strong practical application, and has engineering use value. BRIEF DESCRIPTION OF DRAWINGS

[0044] Figure 1 is the flowchart of the strapdown inertial navigation system gyro drift estimation method based on heterogeneous dual inertial navigation provided by the embodiment of the present application. DETAILED DESCRIPTION

[0045] The structure of the present application will be further described below in combination with the drawings and through embodiments. It should be noted that the present embodiment is descriptive rather than limiting.

[0046] The present application is based on the characteristics that the long-time navigation accuracy of the rotating inertial navigation system is high, but the short-term speed and attitude stability is poor; the short-term speed and attitude stability of the strapdown inertial navigation is higher, but the long-time navigation accuracy is poor; the two are redundantly configured, can complement each other, and can realize the estimation of part of error parameters of the strapdown inertial navigation system through information fusion.

[0047] A strapdown inertial navigation system gyro drift estimation method based on heterogeneous dual inertial navigation, the method flowchart is as shown in Figure 1 The specific implementation is as follows:

[0048] Step 1, determine the system joint error equation

[0049] i is the inertial coordinate system, n is the navigation coordinate system, b1 is the strapdown inertial navigation system carrier coordinate system, b2 is the rotating inertial navigation system platform coordinate system, b m is the rotating inertial navigation system carrier coordinate system.

[0050] The attitude error of the strapdown inertial navigation system The velocity error The latitude error δL1, the longitude error δλ1, and the height error δh1 and the attitude error of the rotating inertial navigation system The velocity error The difference of the latitude error δL2, the longitude error δλ2, and the height error δh2 δL 12 , δλ 12 , δh 12 As the combined error state, the difference between the error state equations of the two is obtained as the combined error state equation:

[0051]

[0052] In the formula, is the gyro output error of the strapdown inertial navigation system, which is modeled as the sum of gyro constant drift and white noise ; is the gyro output error of the rotating inertial navigation system, which is modeled as the sum of gyro constant drift and white noise ; is the accelerometer output error of the strapdown inertial navigation system, which is modeled as the sum of accelerometer constant bias and white noise ; is the accelerometer output error of the rotating inertial navigation system, which is modeled as the sum of accelerometer constant bias and white noise ; is the rotation angular velocity of the n system relative to the i system, is the difference of the rotation angular velocity error of the n system relative to the i system related to the latitude error and the velocity error, is the attitude matrix of the strapdown inertial navigation system, is the attitude matrix of the rotating inertial navigation system platform, f n is the specific force vector in the n system, is the earth rotation angular velocity, is the angular velocity of the n system relative to the e system, is the difference of the earth rotation angular velocity error related to the latitude error, is the difference of the angular velocity error of the n system relative to the e system related to the latitude error and the velocity error, v n is the velocity vector of the carrier in the n system, RM is the meridian radius of curvature, h is the height of the current position, v N is the northward velocity, v E is the eastward velocity, L is the local latitude, R N is the prime vertical radius of curvature, is the difference between the two northward velocity errors.

[0053] Let μ = [μ x , μ y , μ z ] T is the installation error angle of the SINS relative to the RINS, modeled as a random constant.

[0054] Step 2, determine the joint error state variable

[0055] The gyro constant drift and the accelerometer constant bias, installation error angle μ of the two systems are expanded into state variables, and the right lower corner of the numerical subscript is omitted, to obtain the joint error state variable:

[0056]

[0057] In the formula, [φ E , φ N , φ U ] T represents the joint attitude error, [δv E , δv N , δv U ] T represents the joint velocity error, [δL, δλ, δh] T represents the joint position error, represents the gyro constant drift of the SINS, represents the accelerometer constant bias of the SINS, represents the gyro constant drift of the RINS, represents the accelerometer constant bias of the RINS.

[0058] Step 3, establish the joint state equation

[0059]

[0060] In the formula, the system noise

[0061] The block matrix form of the system state matrix F(t) is as follows:

[0062]

[0063] In the formula, The non-zero elements in are:

[0064] f 4,2 = -f U , f 4,3 = f N , f 5,1 = f U , f 5,3 = -f E , f 6,1 = -f N , f 6,2 = f E , f 6,7 = -2v E ω ie sinL, f 9,6 = 1.

[0065]

[0066] Step 4, taking the position error and the attitude matrix matching quantity between the two inertial navigation systems as the observation quantity, the observation equation is established:

[0067] The matrix is constructed Let Z DCM = [Z x , Z y , Z z ] T , wherein

[0068]

[0069] In the formula, M(3, 2) represents the element in the 3rd row and the 2nd column of the M matrix, represents the attitude matrix of the rotating inertial navigation system.

[0070] The functional relationship between Z DCM and the error quantity is

[0071]

[0072] Let L1, λ1 and h1 respectively represent the latitude, longitude and height calculated by the strapdown inertial navigation system, and L2, λ2 and h2 respectively represent the latitude, longitude and height calculated by the rotating inertial navigation system, and the observation equation is established as:

[0073] Z = HX + V

[0074] In the formula, the expressions of the observation quantity Z and the observation matrix H are respectively:

[0075]

[0076] wherein 0 3×3 denotes a 3x3 zero matrix, 0 3×12 denotes a 3x12 zero matrix, I 3×3 denotes a 3x3 identity matrix.

[0077] The position error between the two inertial navigation systems in the above observation should deduct the influence of the lever arm error caused by the installation position.

[0078] The above only describes the preferred embodiments of the present application and is not used to limit the present application. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present application should be included in the protection scope of the present application.

Claims

1. A strapdown inertial navigation system gyro drift estimation method based on heterogeneous dual inertial navigation, characterized in that: The method comprises the following steps: Step 1, determining a system joint error equation by subtracting an error state equation of a rotary inertial navigation system from an error state equation of a strapdown inertial navigation system; Step 2, determining joint error state variables; Step 3, establishing a joint state equation based on the determined joint error state variables; Step 4, taking a position error and an attitude matrix matching quantity between the two inertial navigation systems as observation quantities, establishing an observation equation, and estimating gyro drift of the strapdown inertial navigation system in the joint error state variables through Kalman filtering; Step 1 is specifically: Let i system be the inertial coordinate system, n system be the navigation coordinate system, b1 system be the strapdown inertial navigation system carrier coordinate system, b2 system be the rotating inertial navigation system platform coordinate system, b m system be the rotating inertial navigation system carrier coordinate system; attitude error of strapdown inertial navigation system speed error Latitude error δL1, longitude error δλ1, altitude error δh1, and attitude error of the rotating inertial navigation system speed error The difference between latitude error δL2, longitude error δλ2, and altitude error δh2 As a joint error state, the joint error state equation can be obtained by subtracting the error state equations of the two: wherein is the gyro output error of a strapdown inertial navigation system, modeled as a gyro constant drift and white noise ; is the gyro output error of a rotating inertial navigation system, modeled as a gyro constant drift and white noise ; is the accelerometer output error of a strapdown inertial navigation system, modeled as an accelerometer constant bias and white noise ; is the accelerometer output error of a rotating inertial navigation system, modeled as an accelerometer constant bias and white noise ; is the rotation angular velocity of the n-frame with respect to the i-frame, is the difference of the rotation angular velocity error of the n-frame with respect to the i-frame related to the latitude error and the velocity error, is the attitude matrix of a strapdown inertial navigation system, is the attitude matrix of a rotating inertial navigation system, n is the specific force vector in the n-frame, is the vector expression of the earth rotation angular velocity in the n-frame, is the angular velocity of the n-frame with respect to the e-frame, is the difference of the earth rotation angular velocity error related to the latitude error, is the difference of the angular velocity error of the n-frame with respect to the e-frame related to the latitude error and the velocity error, n is the velocity vector of the carrier in the n-frame, M is the meridian curvature radius, h is the height of the current position, N is the northward velocity, E is the eastward velocity, L is the local latitude, N is the prime vertical curvature radius, is the difference of the two northward velocity errors; Let μ = [μ x , μ y , μ z ] T be the misalignment angles of the SINS with respect to the RINS, modeled as random constants; Step 2 is specifically: The gyro constant drift and the accelerometer constant zero offset and the installation error angle μ of the two systems are expanded into state variables, and the numerical subscripts in the lower right corner are omitted, so that the joint error state variables are: wherein [φ E ,φ N ,φ U ] T denotes the joint attitude error, [δv E ,δv N ,δv U ] T denotes the joint velocity error, [δL,δλ,δh] T denotes the joint position error, denotes the gyro constant drift of the strapdown inertial navigation system, denotes the accelerometer constant bias of the strapdown inertial navigation system, denotes the gyro constant drift of the rotating inertial navigation system, denotes the accelerometer constant bias of the rotating inertial navigation system; In step 3, the established joint state equation is: In the formula, the system noise F(t) is a system state matrix; In step 4, the established observation equation is as follows: Let Z DCM = [Z x , Z y , Z z ] T , Wherein: denotes the attitude matrix of the rotating inertial navigation system; Z DCM The functional relationship between the error amount and the error is: Let L1, λ1 and h1 represent latitude, longitude and height calculated by the strapdown inertial navigation system respectively, and L2, λ2 and h2 represent latitude, longitude and height calculated by the rotary inertial navigation system respectively, and the observation equation is established as: Z = HX + V In the formula, the expressions of Z and H are respectively: where Z is the observation, H is the observation matrix, 0 3×3 denotes a 3x3 zero matrix, 0 3×12 denotes a 3x12 zero matrix, I 3×3 denotes a 3x3 identity matrix.

2. The method of claim 1, wherein the method further comprises: determining a first gyro bias of the first IMU based on the first IMU measurements; determining a second gyro bias of the second IMU based on the second IMU measurements; and determining the gyro bias of the IMU based on the first gyro bias, the second gyro bias, and the first and second IMU measurements. In step 3, the block matrix form of the system state matrix F(t) is as follows: In the formula, The non-zero elements are: f 4,2 = -f U , f 4,3 = f N , f 5,1 = -f U , f 5,3 = f E , f 6,1 = -f N , f 6,2 = f E , f 6,7 = -2v E ω ie sin L, f 9,6 = 1; 3. The method of claim 1, wherein: In step 4, the position error between the two inertial navigation systems in the observation quantity should deduct the influence of the arm error caused by the installation position.

Images