Redundant biaxial rotation inertial navigation state monitoring method based on error correction model

Abstract

The invention discloses a redundant biaxial rotation inertial navigation state monitoring method based on an error correction model, which is particularly suitable for a platform equipped with multiple sets of inertial navigation systems with indexing mechanisms. By correcting a speed error equation, the influence of a specific force item is eliminated, and the problem of inaccurate calculation of a speed error model in a dynamic environment is solved. Meanwhile, a combined state Kalman filter under a speed error correction model combining navigation information of the two inertial navigation systems is constructed, and the relative attitude, the relative speed and the relative position between the two systems are used as observation values. A residual error normalization strong tracking filtering technology is adopted, and gyroscopic drift and accelerometer zero offset of the two sets of systems are estimated and monitored on line. And designing a monitoring algorithm based on error parameters of online estimation, and evaluating the state of the inertial device. In the whole process, external reference information is not needed, and inertial device state monitoring can be realized only by depending on navigation data of the two inertial navigation systems.

Description

Technical Field

[0001] The present invention belongs to the technical field of navigation, and relates to a state monitoring method for a redundant biaxial rotary inertial navigation system, in particular to a state monitoring method for a redundant biaxial rotary inertial navigation based on an error correction model, which is applicable to state monitoring between two or more inertial navigation systems with biaxial or triaxial indexing mechanisms. Background Art

[0002] With the progress of modern navigation technology, the demand for long-endurance and highly reliable navigation systems is increasing day by day. The inertial navigation system has become a key component due to its independence and autonomy. However, the error information provided by the inertial navigation system may endanger navigation safety, so state monitoring is crucial.

[0003] Inertial navigation state monitoring relies on an external reference (such as GNSS) to evaluate the reliability of the inertial navigation system by comparing parameters such as speed and position. However, in an underwater or GNSS-denied environment, the lack of an external reference limits the effectiveness of such monitoring methods. Modern long-endurance vehicles are often equipped with multiple redundant inertial navigations with indexing mechanisms, such as biaxial rotary modulation inertial navigations. Fusing the redundant data of multiple inertial navigations can significantly improve the observability of the error state. By constructing an information fusion model and designing an effective state monitoring filtering algorithm, accurate identification of abnormal inertial navigation information can be achieved.

[0004] At the same time, in the traditional error model of the inertial navigation system, there is a specific force term in the velocity error equation. However, the specific force term in the navigation coordinate system cannot be directly measured and needs to be obtained by differentiation. In a dynamic environment, the specific force vector changes rapidly with the movement of the carrier, and inaccurate specific force calculation leads to an increase in the error of the system covariance matrix. Using the traditional velocity error model will cause problems of inconsistent Kalman filter estimation. At the same time, the acceleration-related error terms are directly coupled in the velocity error equation, and when the specific force estimation is inaccurate, it will seriously affect the estimation accuracy of the acceleration-related error terms. Traditional state monitoring techniques rely on preset fixed thresholds to evaluate the operating state of equipment. However, this method exposes many limitations in practical applications. Fixed thresholds are difficult to accurately capture the complex and dynamic response characteristics of equipment, ignore the inherent differences of equipment in different operating states, and the random interference of environmental noise and changes in operating conditions will have a significant impact on monitoring. Fixed thresholds cannot effectively distinguish these external factors from changes in the equipment's own state, easily causing misdiagnosis or missed diagnosis.

[0005] In view of the problem of inertial navigation state monitoring without external reference information at present, the present invention proposes a redundant dual-axis rotating inertial navigation state monitoring method based on an error correction model, which is applicable to platforms equipped with multiple inertial navigation systems with indexing mechanisms. Based on the navigation information of two inertial navigation systems operating in the dual-axis rotation modulation mode, the velocity error equation is corrected to avoid the presence of specific force terms in the error equation, solving the problem of inaccurate calculation of the velocity error model in a dynamic environment. A joint state Kalman filter for the dual-inertial navigation system under the velocity error correction model is constructed, and the geometric constraint relationships between the two inertial navigation systems, namely the relative attitude, relative velocity, and relative position, are used as observations. Strong tracking filtering based on residual normalization is used to online track and estimate the gyro drift and accelerometer zero bias of the two systems. Further, the states of the inertial devices are monitored and diagnosed according to the error parameters estimated online. This method is completely autonomous, does not require any external reference information, only requires the navigation information of two inertial navigation systems as input, and the entire state monitoring process uses open-loop filtering, and the algorithm does not affect the normal operation of the system. Summary of the Invention

[0006] The present invention proposes a redundant dual-axis rotating inertial navigation state monitoring method based on an error correction model, which corrects the velocity error equation, solves the problem of inaccurate error equation in a dynamic environment, and realizes autonomous state monitoring at the device level of the redundant dual-axis rotating inertial navigation system without any external reference information, having important engineering practical value.

[0007] To solve the above technical problems, the solution proposed by the present invention is as follows:

[0008] A redundant dual-axis rotating inertial navigation state monitoring method based on an error correction model, the method comprising the following steps:

[0009] (1) Set the indexing order of two dual-axis rotating inertial navigation systems, and define the two redundantly configured inertial navigation systems as Inertial Navigation 1 and Inertial Navigation 2 respectively, and their indexing orders are both dual-axis 16 orders, adopting different indexing methods;

[0010] (2) Define the body coordinate systems of the two inertial navigations. Among them, the body coordinate system b1 of Inertial Navigation 1 and the body coordinate system b2 of Inertial Navigation 2 are both defined as "right-front-up", and the navigation coordinate system n is the local geographical coordinate system, defined as "east-north-up". Using the attitude, velocity, and position-related information output by the two dual-axis rotating inertial navigation systems, establish a state space model under the velocity error correction model. The specific steps are as follows:

[0011] (2.1) Determine the system joint error equation:

[0012]

[0013] Among them,

[0014]

[0015] In the formula, represents the attitude error vector of inertial navigation 1, represents the eastward attitude error of inertial navigation 1, represents the northward attitude error of inertial navigation 1, represents the upward attitude error of inertial navigation 1, v n represents the velocity vector of the vehicle in the navigation coordinate system, represents the velocity error vector of inertial navigation 1 after error correction, represents the eastward velocity error of inertial navigation 1 after error correction, represents the northward velocity error of inertial navigation 1 after error correction, represents the upward velocity error of inertial navigation 1 after error correction, represents the position error of inertial navigation 1, δL1 represents the latitude error of inertial navigation 1, δλ1 represents the longitude error of inertial navigation 1, and δh1 represents the altitude error of inertial navigation 1, represents the angular velocity error of the navigation coordinate system relative to the inertial coordinate system related to the latitude error and velocity error of inertial navigation 1, represents the angular velocity error of the earth's rotation related to the latitude error of inertial navigation 1, represents the transfer angular velocity error related to the latitude error and velocity error of inertial navigation 1, represents the direction cosine matrix from the body coordinate system of inertial navigation 1 to the navigation coordinate system, represents the attitude error vector of inertial navigation 2, represents the eastward attitude error of inertial navigation 2, represents the northward attitude error of inertial navigation 2, represents the upward attitude error of inertial navigation 2, represents the velocity error vector of inertial navigation 2 after error correction, represents the eastward velocity error of inertial navigation 2 after error correction, represents the northward velocity error of inertial navigation 2 after error correction, represents the upward velocity error of inertial navigation 2 after error correction, represents the position error of inertial navigation 2, δL2 represents the latitude error of inertial navigation 2, δλ2 represents the longitude error of inertial navigation 2, and δh2 represents the altitude error of inertial navigation 2, represents the angular velocity error of the navigation coordinate system relative to the inertial coordinate system related to the latitude error and velocity error of inertial navigation 2, represents the angular velocity error of the earth's rotation related to the latitude error of inertial navigation 2, represents the transfer angular velocity error related to the latitude error and velocity error of inertial navigation 2, represents the direction cosine matrix from the body coordinate system of inertial navigation 2 to the navigation coordinate system, is the angular velocity of rotation of the navigation coordinate system relative to the inertial coordinate system, is the angular velocity vector of the Earth's rotation, is the angular velocity of rotation of the navigation coordinate system relative to the Earth coordinate system, g n represents the gravity vector at the location of the vehicle, respectively represent the velocities of the vehicle in the east, north, and up directions. L and h are the latitude and altitude at the location of the vehicle, R E and R N are respectively the radius of the prime vertical and the radius of the meridian at the location of the vehicle, represents the gyro component error of INS1, modeled as a constant drift and gyro noise is the sum of them, where, represents the x-axis gyro drift of INS1, represents the y-axis gyro drift of INS1, represents the z-axis gyro drift of INS1, represents the accelerometer component error of INS1, modeled as a constant zero bias and accelerometer noise is the sum of them, where, represents the x-axis accelerometer zero bias of INS1, represents the y-axis accelerometer zero bias of INS1, represents the z-axis accelerometer zero bias of INS1, represents the gyro component error of INS2, modeled as a constant drift and gyro noise is the sum of them, where, represents the x-axis gyro drift of INS2, represents the y-axis gyro drift of INS2, represents the z-axis gyro drift of INS2, represents the accelerometer component error of INS2, modeled as a constant zero bias and accelerometer noise is the sum of them, where, represents the x-axis accelerometer zero bias of INS2, represents the y-axis accelerometer zero bias of INS2, represents the z-axis accelerometer zero bias of INS2;

[0016] (2.2) Determine the combined state equation:

[0017]

[0018] where, F(t) represents the state transition matrix, obtained from the system combined error equation, and the state vector x(t) is expressed as:

[0019]

[0020] The noise distribution matrix G(t) and the noise matrix w(t) are expressed as:

[0021]

[0022] (2.3) Determine the state constraint observation equation:

[0023] Define the coordinate systems of inertial navigation 1 and inertial navigation 2 when the indexing mechanism is at the zero position as the b 10 system and the b 20 system. The attitude matrices and output by the two sets of two-axis rotating inertial navigations are expressed as:

[0024]

[0025] In the formula, is the direction cosine matrix from the body coordinate system of inertial navigation 1 to the navigation coordinate system when the indexing mechanism is at the zero position, 10 to the navigation coordinate system, and respectively represent the attitude matrices of the indexing mechanisms of inertial navigation 1 and inertial navigation 2 relative to their zero positions at time t, represents the attitude matrix between the b 20 system and the b 10 system;

[0026] Determine the difference expression of the attitude errors of the two sets of two-axis rotating inertial navigations as:

[0027]

[0028] In the formula, represents the direction cosine matrix from the body coordinate system of inertial navigation 1 at the initial moment to the body coordinate system at time t, represents the direction cosine matrix from the navigation coordinate system to the body coordinate system of inertial navigation 2 at time t. Considering the lever arm, determine the velocity and position outputs of inertial navigation 1 and inertial navigation 2 as:

[0029]

[0030] In the formula, and respectively represent the velocity information in the navigation coordinate system output by inertial navigation 1 and inertial navigation 2, represents the position information output by inertial navigation 1, represents the position information output by inertial navigation 2, r n represents the true position of the carrier in the navigation coordinate system, represents the velocity difference of inertial navigation 2 relative to inertial navigation 1 caused by the external lever arm between the two sets of inertial navigations, l r12It represents the position difference of INS2 relative to INS1 caused by the external lever arm between the two sets of INSs;

[0031] Therefore, the speed error and the difference of the position vectors of the two sets of INS systems are expressed as:

[0032]

[0033] The observation equation is expressed as:

[0034] z(t) = H(t)x(t) + υ(t),

[0035] where z(t) represents the observation vector, and H(t) represents the observation transfer matrix, which are respectively expressed as:

[0036]

[0037]

[0038] In the formula, I 3×3 represents a 3×3 identity matrix, and I 2×2 represents a 2×2 identity matrix, and υ(t) is the noise vector corresponding to the observed quantity;

[0039] (3) Establish an adaptive error parameter estimation filter;

[0040] Adopt a strong tracking filter based on residual normalization to track and estimate the error state, and express the one-step prediction of the filter covariance matrix as:

[0041]

[0042] In the formula,

[0043]

[0044] And there is

[0045]

[0046] where, λ k is the fading factor, P k / k-1 is the one-step prediction covariance matrix, Φ k / k-1 represents the state one-step transfer matrix, P k-1 is the covariance matrix at the (k - 1)th moment, G k-1 is the process noise allocation matrix at the (k - 1)th moment, Q k-1 is the system noise matrix at the (k - 1)th moment, tr(·) is the matrix trace operator, H k is the system observation matrix at the kth moment, R k is the observation noise matrix at the kth moment, l k is the weakening factor, λ0,k The fading factor calculated at time k Denotes the residual covariance matrix at time k Denotes the residual covariance matrix at time k - 1, ρ is the forgetting factor, taking 0.95 ≤ ρ ≤ 0.995, γ0 represents the innovation at time 0, γ k Denotes the innovation at time k, η is the normalization parameter, used to eliminate the problem that the information asymmetry caused by the difference in the values of the residuals themselves leads to a decrease in the response speed of the error state estimation

[0047] (4) Based on the error parameters output by the filter, perform real - time state monitoring

[0048] When the inertial device is in an abnormal state, the corresponding gyro drift or accelerometer zero - bias changes, and the real - time health state monitoring of the inertial device is realized by analyzing the output of the monitoring filter

[0049] Set a sliding data window over time, with a length of N. The filter output information included in the sliding window at time k is Calculate the data statistical characteristics as follows

[0050]

[0051] In the formula, μ k and respectively represent the mean and variance of the real - time estimation parameters of the filter within the sliding window at time k, and the dimensions of both are the same as the dimensions

[0052] Set a weighting coefficient α to perform iterative calculation on the mean of the historical sliding window

[0053] Σ k = α·μ k +(1 - α)Σ k-1

[0054] In the formula, Σ k represents the mean of the sliding window after iteration at time k, and determine the upper and lower thresholds as follows

[0055] T - = Σ k + k1σ k

[0056] T + = k2Σ k

[0057] In the formula, k1, k2 are parameters to be adjusted, and k1 ≥ 1, k2 > 1, T + is the upper threshold, T - is the lower threshold

[0058] The criteria for establishing the health status monitoring are as follows:

[0059]

[0060] Wherein, represents the i-th component of the filter estimated output at the (k + 1)-th moment, T + (i) and T - (i) respectively represent the i-th components of the real-time high threshold and low threshold to be obtained. The monitoring thresholds of each inertial device are updated in real time according to the sliding window. When all the error parameter values estimated by the filter are less than the low threshold, it is output that the system has no fault; when the i-th error parameter value estimated by the filter is greater than the low threshold and less than the high threshold, a fault warning is given; when the i-th error parameter value estimated by the filter is greater than the high threshold, it is output that device i has a fault.

[0061] Based on the joint indexing method given in step (1), make inertial navigation 1 and inertial navigation 2 in the normal navigation state, and construct a dual-inertial navigation state space model through step (2) and step (3); based on the error parameters output by the filter, the state monitoring of the inertial navigation system can be realized through step (4).

[0062] Furthermore, in step (1), inertial navigation 1 and inertial navigation 2 rotate at different times according to the same indexing order, that is, the two inertial navigations rotate in an asynchronous manner according to the same rotation scheme.

[0063] Furthermore, in step (1), inertial navigation 1 and inertial navigation 2 adopt different indexing orders and rotate synchronously.

[0064] Furthermore, the relative attitude of inertial navigation 1 and inertial navigation 2 when they are in the zero position in step (2) is determined after the two sets of inertial navigations are installed and aligned by their respective systems, or determined with the help of an external attitude reference.

[0065] Furthermore, the lever arm between inertial navigation 1 and inertial navigation 2 in step (2) is calibrated and determined after the two sets of inertial navigations are installed.

[0066] Furthermore, in step (2) and are determined by the angular position of the rotating frame output by the indexing mechanism.

[0067] Furthermore, the method of the present invention is not only applicable to the case where both inertial navigation 1 and inertial navigation 2 are dual-axis rotation modulation inertial navigations, but also applicable to the cases where both inertial navigation 1 and inertial navigation 2 are tri-axis rotation modulation inertial navigations, inertial navigation 1 is a dual-axis rotation modulation inertial navigation or a tri-axis rotation modulation inertial navigation, inertial navigation 2 is a single-axis rotation modulation inertial navigation, inertial navigation 1 is a single-axis rotation modulation inertial navigation, inertial navigation 2 is a dual-axis rotation modulation inertial navigation or a tri-axis rotation modulation inertial navigation, and the redundant configurations of multiple sets of dual-axis rotation inertial navigations and multiple sets of tri-axis rotation inertial navigations.

[0068] In summary, the advantages and positive effects of the present invention are as follows: The present invention realizes the online monitoring at the device level of the inertial navigation system by the asynchronous rotation of two sets of dual-axis rotating inertial navigation systems, and utilizes the redundant information of the two inertial navigation systems. The method proposed by the present invention is completely autonomous, does not rely on any external reference information, is not restricted by the usage environment, can improve the monitoring accuracy of the inertial device state on a mobile platform, and has important engineering practical significance. Description of the Drawings

[0069] Figure 1 It is a flowchart provided by an embodiment of the present invention. Detailed Embodiment

[0070] In order to make the objectives, technical solutions and advantages of the present invention clearer and more understandable, the present invention will be further described in detail below in conjunction with embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not used to limit the present invention.

[0071] The real-time monitoring of the inertial navigation state is crucial for ensuring the safe navigation of the carrier. The existing inertial navigation state monitoring schemes usually require external reference benchmark information. For some special application environments, such as underwater environments, GNSS denial environments, etc., the inertial navigation will not be able to monitor its own state. In addition, the traditional speed error model has a specific force term, and there will be a problem in dynamic environments that inaccurate specific force calculation leads to an increase in the system covariance matrix error, affecting the algorithm accuracy. To solve the above technical problems, the present invention proposes a redundant dual-axis rotating inertial navigation state monitoring method based on an error correction model, and the state monitoring method is as Figure 1 shown. The detailed implementation is as follows:

[0072] (1) Set the indexing order of the two sets of dual-axis rotating inertial navigation systems. Define the two redundantly configured inertial navigation systems as inertial navigation 1 and inertial navigation 2 respectively. The indexing order of both is the dual-axis 16 order, and different indexing methods are adopted;

[0073] (2) Define the body coordinate systems of the two inertial navigations. Among them, the body coordinate system b1 of inertial navigation 1 and the body coordinate system b2 of inertial navigation 2 are both defined as "right-front-up", and the navigation coordinate system n is the local geographic coordinate system, defined as "east-north-up". Using the attitude, velocity, and position-related information output by the two sets of dual-axis rotating inertial navigation systems, establish a state space model under the speed error correction model. The specific steps are as follows:

[0074] (2.1) Determine the system joint error equation:

[0075]

[0076] Among them,

[0077]

[0078]

[0079] Wherein, represents the attitude error vector of inertial navigation 1, represents the eastward attitude error of inertial navigation 1, represents the northward attitude error of inertial navigation 1, represents the upward attitude error of inertial navigation 1, v n represents the velocity vector of the vehicle in the navigation coordinate system, represents the velocity error vector of inertial navigation 1 after error correction, represents the eastward velocity error of inertial navigation 1 after error correction, represents the northward velocity error of inertial navigation 1 after error correction, represents the upward velocity error of inertial navigation 1 after error correction, represents the position error of inertial navigation 1, δL1 represents the latitude error of inertial navigation 1, δλ1 represents the longitude error of inertial navigation 1, and δh1 represents the altitude error of inertial navigation 1, represents the angular velocity error of the navigation coordinate system relative to the inertial coordinate system related to the latitude error and velocity error of inertial navigation 1, represents the angular velocity error of the Earth's rotation related to the latitude error of inertial navigation 1, represents the transfer angular velocity error related to the latitude error and velocity error of inertial navigation 1, represents the direction cosine matrix from the body coordinate system of inertial navigation 1 to the navigation coordinate system, represents the attitude error vector of inertial navigation 2, represents the eastward attitude error of inertial navigation 2, represents the northward attitude error of inertial navigation 2, represents the upward attitude error of inertial navigation 2, represents the velocity error vector of inertial navigation 2 after error correction, represents the eastward velocity error of inertial navigation 2 after error correction, represents the northward velocity error of inertial navigation 2 after error correction, represents the upward velocity error of inertial navigation 2 after error correction, represents the position error of inertial navigation 2, δL2 represents the latitude error of inertial navigation 2, δλ2 represents the longitude error of inertial navigation 2, and δh2 represents the altitude error of inertial navigation 2, represents the angular velocity error of the navigation coordinate system relative to the inertial coordinate system related to the latitude error and velocity error of inertial navigation 2, represents the angular velocity error of the Earth's rotation related to the latitude error of inertial navigation 2, represents the transfer angular velocity error related to the latitude error and velocity error of inertial navigation 2, represents the direction cosine matrix from the body coordinate system of inertial navigation 2 to the navigation coordinate system, is the angular velocity of rotation of the navigation coordinate system relative to the inertial coordinate system, is the angular velocity vector of the Earth's rotation, is the angular velocity of rotation of the navigation coordinate system relative to the Earth coordinate system, g n represents the gravity vector at the location of the vehicle, respectively represent the velocities of the vehicle in the east, north, and up directions. L and h are the latitude and altitude of the location of the vehicle, and R E and R N are respectively the radius of the prime vertical and the radius of the meridian at the location of the vehicle, represents the gyro assembly error of INS1, modeled as a constant drift and gyro noise the sum of, where, represents the x-axis gyro drift of INS1, represents the y-axis gyro drift of INS1, represents the z-axis gyro drift of INS1, represents the accelerometer assembly error of INS1, modeled as a constant zero bias and accelerometer noise the sum of, where, represents the x-axis accelerometer zero bias of INS1, represents the y-axis accelerometer zero bias of INS1, represents the z-axis accelerometer zero bias of INS1, represents the gyro assembly error of INS2, modeled as a constant drift and gyro noise the sum of, where, represents the x-axis gyro drift of INS2, represents the y-axis gyro drift of INS2, represents the z-axis gyro drift of INS2, represents the accelerometer assembly error of INS2, modeled as a constant zero bias and accelerometer noise the sum of, where, represents the x-axis accelerometer zero bias of INS2, represents the y-axis accelerometer zero bias of INS2, represents the z-axis accelerometer zero bias of INS2;

[0080] (2.2) Determine the combined state equation:

[0081]

[0082] where, F(t) represents the state transition matrix, obtained from the combined error equation of the system, and the state vector x(t) is expressed as:

[0083]

[0084] The noise distribution matrix G(t) and the noise matrix w(t) are expressed as:

[0085]

[0086] (2.3) Determine the state constraint observation equation:

[0087] Define the coordinate systems of inertial navigation 1 and inertial navigation 2 when the indexing mechanism is at the zero position as the b 10 system and the b 20 system. The attitude matrices output by the two sets of two-axis rotating inertial navigations are respectively and expressed as:

[0088]

[0089] where is the direction cosine matrix from the body coordinate system of inertial navigation 1 to the navigation coordinate system when the indexing mechanism is at the zero position, 10 and respectively represent the attitude matrices of the indexing mechanisms of inertial navigation 1 and inertial navigation 2 relative to their zero positions at time t, represents the attitude matrix between the b 20 system and the b 10 system;

[0090] Determine the difference expression of the attitude errors of the two sets of two-axis rotating inertial navigations as:

[0091]

[0092] where represents the direction cosine matrix from the body coordinate system of inertial navigation 1 at the initial moment to the body coordinate system at time t, represents the direction cosine matrix from the navigation coordinate system to the body coordinate system of inertial navigation 2 at time t. Considering the lever arm, the velocity and position outputs of inertial navigation 1 and inertial navigation 2 are expressed as:

[0093]

[0094] where and respectively represent the velocity information in the navigation coordinate system output by inertial navigation 1 and inertial navigation 2, represents the position information output by inertial navigation 1, represents the position information output by inertial navigation 2, r n represents the true position of the carrier in the navigation coordinate system, represents the velocity difference of inertial navigation 2 relative to inertial navigation 1 caused by the external lever arm between the two sets of inertial navigations, l r12 ​It represents the position difference of INS 2 relative to INS 1 caused by the external lever arm between the two sets of INSs;

[0095] Therefore, the speed error and the difference of the position vectors of the two sets of INS systems are expressed as:

[0096]

[0097] The observation equation is expressed as:

[0098] z(t) = H(t)x(t) + υ(t),

[0099] where z(t) represents the observation vector, and H(t) represents the observation transition matrix, which are respectively expressed as:

[0100]

[0101] In the formula, I 3×3 represents the 3×3 identity matrix, and I 2×2 represents the 2×2 identity matrix, and υ(t) is the noise vector corresponding to the observed quantity;

[0102] (3) Establish an adaptive error parameter estimation filter;

[0103] Adopt a strong tracking filter based on residual normalization to track and estimate the error state, and express the one-step prediction of the filter covariance matrix as:

[0104]

[0105] In the formula,

[0106]

[0107] And there is

[0108]

[0109] where, λ k is the fading factor, P k / k-1 is the one-step prediction covariance matrix, Φ k / k-1 represents the state one-step transition matrix, P k-1 is the covariance matrix at time k - 1, G k-1 is the process noise allocation matrix at time k - 1, Q k-1 is the system noise matrix at time k - 1, tr(·) is the matrix trace operator, H k is the system observation matrix at time k, R k is the observation noise matrix at time k, l k is the weakening factor, λ 0,k is the fading factor calculated at time k, Denote the residual covariance matrix at time k, Denote the residual covariance matrix at time k-1, ρ is the forgetting factor, taking 0.95 ≤ ρ ≤ 0.995, γ0 represents the innovation at time 0, γ k Denote the innovation at time k, η is the normalization parameter, used to eliminate the problem that the information asymmetry caused by the difference in the numerical value of the residual itself leads to a decrease in the response speed of the error state estimation;

[0110] (4) Based on the error parameters output by the filter, perform state monitoring in real time;

[0111] When the state of the inertial device is abnormal, the corresponding gyro drift or accelerometer zero bias changes, and the real-time health state monitoring of the inertial device is realized by analyzing the output of the monitoring filter;

[0112] Set a sliding data window over time, its length is N, and the filter output information included in the sliding window at time k is Calculate the data statistical characteristics as follows:

[0113]

[0114] In the formula, μ k and respectively represent the mean and variance of the real-time estimation parameters of the filter within the sliding window at time k, and the dimensions of both are the same as the dimension;

[0115] Set the weighting coefficient α, and perform iterative calculation on the mean of the historical sliding window:

[0116] Σ k = α·μ k +(1-α)Σ k-1

[0117] In the formula, Σ k represents the mean of the sliding window after iteration at time k, and determine the upper and lower thresholds as follows:

[0118] T - = Σ k + k1σ k

[0119] T + = k2Σ k

[0120] In the formula, k1, k2 are parameters to be adjusted, and k1 ≥ 1, k2 > 1, T + is the upper threshold, T - is the lower threshold;

[0121] Establish the health state monitoring criterion as:

[0122]

[0123] In the formula, represents the i-th component of the estimated output of the filter at the (k + 1)-th moment, T + (i) and T - (i) respectively represent the i-th components of the required real-time high threshold and low threshold. According to the sliding window, the monitoring thresholds of each inertial device are updated in real time. When all the error parameter values estimated by the filter are less than the low threshold, it is output that the system has no fault; when the i-th error parameter value estimated by the filter is greater than the low threshold and less than the high threshold, a fault warning is given; when the i-th error parameter value estimated by the filter is greater than the high threshold, it is output that device i has a fault.

[0124] Based on the joint indexing method given in step (1), make inertial navigation 1 and inertial navigation 2 in the normal navigation state, and construct a dual-inertial navigation state space model through step (2) and step (3); based on the error parameters output by the filter, the state monitoring of the inertial navigation system can be realized through step (4).

[0125] As an improvement, in step (1), inertial navigation 1 and inertial navigation 2 rotate at different times according to the same indexing order, that is, the two inertial navigations rotate in an asynchronous manner according to the same rotation scheme.

[0126] As an improvement, in step (1), inertial navigation 1 and inertial navigation 2 adopt different indexing orders and rotate synchronously.

[0127] As an improvement, in step (2), the relative attitude of inertial navigation 1 and inertial navigation 2 when they are at zero position is determined after the two sets of inertial navigations are installed and aligned by their respective systems, or determined with the help of an external attitude reference.

[0128] As an improvement, the lever arm between inertial navigation 1 and inertial navigation 2 in step (2) is calibrated and determined after the two sets of inertial navigations are installed.

[0129] As an improvement, in step (2) and are determined by the angular position of the rotating frame output by the indexing mechanism.

[0130] As an improvement, the method of the present invention is not only applicable to the case where both inertial navigation 1 and inertial navigation 2 are two-axis rotation modulation inertial navigations, but also applicable to the cases where both inertial navigation 1 and inertial navigation 2 are three-axis rotation modulation inertial navigations, inertial navigation 1 is a two-axis rotation modulation inertial navigation or a three-axis rotation modulation inertial navigation, inertial navigation 2 is a single-axis rotation modulation inertial navigation, inertial navigation 1 is a single-axis rotation modulation inertial navigation, inertial navigation 2 is a two-axis rotation modulation inertial navigation or a three-axis rotation modulation inertial navigation, and the redundant configurations of multiple sets of two-axis rotation inertial navigations and multiple sets of three-axis rotation inertial navigations.

[0131] The method of the present invention has no requirements for the motion state of the carrier, and online state monitoring can be realized whether the carrier is in a moored state or a moving state; there are no requirements for the environment where the carrier is located, and it is applicable in both underwater environments and GNSS-denied environments.

[0132] The above are only the preferred embodiments of the present invention and are not intended to limit the present invention. All technical solutions within the concept of the present invention fall within the protection scope of the present invention. Several improvements and refinements made without departing from the principle of the present invention should also be regarded as within the protection scope of the present invention.

Claims

1. A redundant two-axis rotating inertial navigation state monitoring method based on an error correction model, characterized in that, The method includes the following steps: (1) Set the indexing order of two sets of two-axis rotation inertial navigation systems. Define the two inertial navigation systems with redundant configuration as Inertial Navigation 1 and Inertial Navigation 2 respectively. The indexing order of both is the two-axis 16 order, and different indexing methods are adopted. (2) Define the body coordinate systems of the two inertial navigations. Among them, the body coordinate system b1 of Inertial Navigation 1 and the body coordinate system b2 of Inertial Navigation 2 are both defined as "right-front-up", and the navigation coordinate system n is the local geographical coordinate system, defined as "east-north-up". Use the attitude, velocity, and position-related information output by the two sets of two-axis rotation inertial navigation systems to establish a state space model under the velocity error correction model. The specific steps are as follows: (2.1) Determine the system combined error equation: Where, In the formula, represents the attitude error vector of inertial navigation 1, represents the eastward attitude error of inertial navigation 1, represents the northward attitude error of inertial navigation 1, represents the upward attitude error of inertial navigation 1, v n represents the velocity vector of the carrier in the navigation coordinate system, represents the velocity error vector of inertial navigation 1 after error correction, represents the eastward velocity error of inertial navigation 1 after error correction, represents the northward velocity error of inertial navigation 1 after error correction, represents the upward velocity error of inertial navigation 1 after error correction, represents the position error of inertial navigation 1, δL1 represents the latitude error of inertial navigation 1, δλ1 represents the longitude error of inertial navigation 1, and δh1 represents the altitude error of inertial navigation 1, represents the angular velocity error of the navigation coordinate system relative to the inertial coordinate system related to the latitude error and velocity error of inertial navigation 1, represents the angular velocity error of the earth's rotation related to the latitude error of inertial navigation 1, represents the transfer angular velocity error related to the latitude error and velocity error of inertial navigation 1, represents the direction cosine matrix from the body coordinate system of inertial navigation 1 to the navigation coordinate system, represents the attitude error vector of inertial navigation 2, represents the eastward attitude error of inertial navigation 2, represents the northward attitude error of inertial navigation 2, represents the upward attitude error of inertial navigation 2, represents the velocity error vector of inertial navigation 2 after error correction, represents the eastward velocity error of inertial navigation 2 after error correction, represents the northward velocity error of inertial navigation 2 after error correction, represents the upward velocity error of inertial navigation 2 after error correction, represents the position error of inertial navigation 2, δL2 represents the latitude error of inertial navigation 2, δλ2 represents the longitude error of inertial navigation 2, and δh2 represents the altitude error of inertial navigation 2, represents the angular velocity error of the navigation coordinate system relative to the inertial coordinate system related to the latitude error and velocity error of inertial navigation 2, represents the angular velocity error of the earth's rotation related to the latitude error of inertial navigation 2, represents the transfer angular velocity error related to the latitude error and velocity error of inertial navigation 2, represents the direction cosine matrix from the body coordinate system of inertial navigation 2 to the navigation coordinate system, is the rotational angular velocity of the navigation coordinate system relative to the inertial coordinate system, is the angular velocity vector of the earth's rotation, is the angular velocity of the navigation coordinate system relative to the Earth coordinate system, g n represents the gravity vector at the location of the vehicle, respectively represent the velocities of the vehicle in the east, north, and up directions. L and h are the latitude and altitude of the vehicle's location, R E and R N are respectively the radius of the prime vertical and the radius of the meridian at the location of the vehicle, represents the gyro assembly error of inertial navigation 1, modeled as a constant drift and gyro noise is the sum of them, where, represents the x-axis gyro drift of inertial navigation 1, represents the y-axis gyro drift of inertial navigation 1, represents the z-axis gyro drift of inertial navigation 1, represents the accelerometer assembly error of inertial navigation 1, modeled as a constant zero bias and accelerometer noise is the sum of them, where, represents the x-axis accelerometer zero bias of inertial navigation 1, represents the y-axis accelerometer zero bias of inertial navigation 1, represents the z-axis accelerometer zero bias of inertial navigation 1, represents the gyro assembly error of inertial navigation 2, modeled as a constant drift and gyro noise is the sum of them, where, represents the x-axis gyro drift of inertial navigation 2, represents the y-axis gyro drift of inertial navigation 2, represents the z-axis gyro drift of inertial navigation 2, represents the accelerometer assembly error of inertial navigation 2, modeled as a constant zero bias and accelerometer noise is the sum of them, where, represents the x-axis accelerometer zero bias of inertial navigation 2, represents the y-axis accelerometer zero bias of inertial navigation 2, represents the z-axis accelerometer zero bias of inertial navigation 2; (2.2) Determine the combined state equation: Where, F(t) represents the state transition matrix, obtained from the system combined error equation. The state vector x(t) is expressed as: The noise distribution matrix G(t) and the noise matrix w(t) are expressed as: (2.3) Determine the state constraint observation equation: Define the coordinate systems of inertial navigation 1 and inertial navigation 2 when the indexing mechanism is at the zero position as the b 10 system and the b 20 system. The attitude matrices and output by the two sets of two-axis rotating inertial navigations are expressed as: In the formula, is the direction cosine matrix from the body coordinate system b of the inertial navigation 1 when the indexing mechanism is at the zero position 10 to the navigation coordinate system, and respectively represent the attitude matrices of the indexing mechanisms of inertial navigation 1 and inertial navigation 2 relative to their zero positions at time t, represents 20 the attitude matrix between the b 10 coordinate systems; Determine the difference expression of the attitude errors of the two sets of two-axis rotation inertial navigations: In the formula, represents the direction cosine matrix from the body coordinate system at the initial moment of inertial navigation 1 to the body coordinate system at time t. represents the direction cosine matrix from the navigation coordinate system to the body coordinate system of inertial navigation 2 at time t. Considering the lever arm, the velocity and position outputs of inertial navigation 1 and inertial navigation 2 are determined and expressed as: In the formula, and respectively represent the velocity information in the navigation coordinate system output by INS 1 and INS 2, represents the position information output by INS 1, represents the position information output by INS 2, r n represents the true position of the vehicle in the navigation coordinate system, represents the velocity difference of INS 2 relative to INS 1 caused by the external lever arm between the two INSs, l r12 represents the position difference of INS 2 relative to INS 1 caused by the external lever arm between the two INSs; Therefore, the difference of the velocity errors and position vectors of the two inertial navigation systems is expressed as: The observation equation is expressed as: z(t) = H(t)x(t) + υ(t), Where z(t) represents the observation vector, and H(t) represents the observation transition matrix, which are respectively expressed as: where, I 3×3 represents a 3×3 identity matrix, I 2×2 represents a 2×2 identity matrix, and υ(t) is the noise vector corresponding to the observation quantity; (3) Establish an adaptive error parameter estimation filter; Adopt a strong tracking filter based on residual normalization to track and estimate the error state. Express the one-step prediction of the filter covariance matrix as: In the formula, And there is where, λ k is the fading factor, P k / k-1 is the one-step prediction covariance matrix, Φ k / k-1 represents the state one-step transition matrix, P k-1 is the covariance matrix at time k - 1, G k-1 is the process noise allocation matrix at time k - 1, Q k-1 is the system noise matrix at time k - 1, tr(·) is the matrix trace operator, H k is the system observation matrix at time k, R k is the observation noise matrix at time k, l k is the weakening factor, λ 0,k is the fading factor calculated at time k, represents the residual covariance matrix at time k, represents the residual covariance matrix at time k - 1, ρ is the forgetting factor, taking 0.95 ≤ ρ ≤ 0.995, γ0 represents the innovation at time 0, γ k represents the innovation at time k, η is the normalization parameter, used to eliminate the problem that the information asymmetry caused by the difference in the numerical values of the residuals themselves leads to a reduction in the response speed of the error state estimation; (4) Based on the error parameters output by the filter, perform real-time state monitoring; When the state of the inertial device is abnormal, the corresponding gyro drift or accelerometer zero bias changes. Real-time health state monitoring of the inertial device is realized by analyzing the output of the monitoring filter; Set a data window that slides over time, with a length of N. The filter output information contained in the sliding window at time k is Calculate the data statistical characteristics as follows: where, μ k and respectively represent the mean and variance of the real-time estimated parameters of the filter within the sliding window at time k, and the dimensions of both are the same as those of with the same dimension; Set the weighting coefficient α and perform iterative calculation on the mean value of the historical sliding window: Σ k = α·μ k + (1 - α)Σ k-1 where, Σ k represents the sliding window mean value after iteration at the k-th moment, and the upper and lower thresholds are determined as follows: T - = Σ k + k1σ k T + = k2Σ k Wherein, k1 and k2 are parameters to be adjusted, and k1≥1, k2>1, T + is the high threshold, and T - is the low threshold; Establish a health state monitoring criterion as: In the formula, represents the i-th component of the filter estimated output at the (k + 1)-th moment, T + (i) and T - (i) respectively represent the i-th components of the required real-time high threshold and low threshold. According to the sliding window, the monitoring thresholds of each inertial device are updated in real time. When all the error parameter values estimated by the filter are less than the low threshold, it is output that the system has no fault; when the i-th error parameter value estimated by the filter is greater than the low threshold and less than the high threshold, a fault warning is given; when the i-th error parameter value estimated by the filter is greater than the high threshold, it is output that device i has a fault.

2. The redundant biaxial rotation inertial navigation state monitoring method based on an error correction model according to claim 1, wherein In step (1), Inertial Navigation 1 and Inertial Navigation 2 rotate at different times according to the same indexing order, that is, the two inertial navigations rotate in an asynchronous manner according to the same rotation scheme.

3. A redundant two-axis rotating inertial navigation state monitoring method based on an error correction model according to claim 1, characterized in that, In step (1), Inertial Navigation 1 and Inertial Navigation 2 adopt different indexing orders and rotate synchronously.

4. A redundancy dual-axis rotating inertial navigation state monitoring method based on an error correction model according to claim 1, characterized in that The relative attitude when the inertial navigation 1 and the inertial navigation 2 are at zero position in the step (2) It is determined after the alignment of the two sets of inertial navigation systems respectively after the installation of the two sets of inertial navigation systems, or determined with the help of an external attitude reference.

5. A redundancy dual-axis rotating inertial navigation state monitoring method based on an error correction model according to claim 1, characterized in that, In step (2), the lever arm between Inertial Navigation 1 and Inertial Navigation 2 is calibrated and determined after the installation of the two sets of inertial navigations.

6. The redundant two-axis rotating inertial navigation state monitoring method based on an error correction model according to claim 1, wherein, In the said step (2) and determine the angular position of the rotating frame output by the indexing mechanism.

7. A redundancy dual-axis rotating inertial navigation state monitoring method based on an error correction model according to claim 1, characterized in that The method of the present invention is not only applicable to the case where both Inertial Navigation 1 and Inertial Navigation 2 are two-axis rotation modulation inertial navigations, but also applicable to the cases where both Inertial Navigation 1 and Inertial Navigation 2 are three-axis rotation modulation inertial navigations, Inertial Navigation 1 is a two-axis rotation modulation inertial navigation or a three-axis rotation modulation inertial navigation, Inertial Navigation 2 is a single-axis rotation modulation inertial navigation, Inertial Navigation 1 is a single-axis rotation modulation inertial navigation, Inertial Navigation 2 is a two-axis rotation modulation inertial navigation or a three-axis rotation modulation inertial navigation, and the cases of redundant configuration of multiple sets of two-axis rotation inertial navigations and multiple sets of three-axis rotation inertial navigations.

Images