Method for monitoring external field state of redundant biaxial rotation inertial navigation system

Abstract

The invention discloses a method for monitoring the external field state of a redundant double-shaft rotary inertial navigation system aiming at the problem that the online monitoring of the inertial navigation system is limited when no external reference information exists, and the method is suitable for a plurality of platforms with indexing mechanism inertial navigation systems. According to the method, a joint error state Kalman filter is established through joint transposition of the double inertial navigation systems, and the geometric constraint relation between the two inertial navigation systems, namely the relative attitude, the speed and the position, is used for observation. A strong tracking filter based on residual normalization is adopted to estimate and monitor gyroscopic drift and accelerometer zero offset of the two systems. And calculating and updating a threshold value in real time according to an error parameter monitored on line to evaluate the state of the device. The method is completely autonomous, does not need any external reference information, only needs information of two sets of inertial navigation systems as input, adopts open-loop filtering in a state monitoring process, and does not affect normal work of the system by an algorithm.

Description

Technical Field

[0001] The present invention belongs to the technical field of navigation, and relates to a method for monitoring the state of an inertial navigation system, in particular to a method for monitoring the external field state of a redundant two-axis rotary inertial navigation system, which is applicable to the state monitoring of two or more inertial navigation systems with two-axis or three-axis indexing mechanisms. Background Art

[0002] With the development of the shipping industry, ship navigation technology is developing towards long endurance and high reliability. Among them, the inertial navigation system has become an indispensable part of modern ship navigation technology due to its independence. If the inertial navigation system provides incorrect information, it will seriously affect the navigation safety of the vehicle. Therefore, the inertial navigation state monitoring technology has become an indispensable part of navigation research.

[0003] The external field state monitoring technology generally relies on the outside world to provide basic reference information. By combining the outside reference benchmarks such as speed, position and other information with the inertial navigation output information to judge the reliability of the current inertial navigation system is the research focus of the inertial navigation system in recent years. However, the external field state monitoring has requirements for the motion state of the vehicle, the outside reference information, etc. For some situations lacking outside reference information, such as underwater environment, GNSS denied environment, etc., the use of the external field state monitoring technology will be severely restricted.

[0004] Existing vehicles with long endurance motion usually carry multiple inertial navigation systems with indexing mechanisms, such as shipborne redundant two-axis rotary modulation inertial navigation systems. Research in recent years has shown that fusing the redundant information of multiple inertial navigation systems can effectively improve the observability of some error states of the inertial navigation system. By building an information fusion model of multiple inertial navigation systems and designing a reasonable state monitoring filter algorithm, the state recognition of abnormal inertial navigation information can be achieved. In addition, traditional state monitoring methods often rely on expert experience or historical data analysis to set fixed thresholds, which are easily affected by the dynamic characteristics of the equipment, environmental noise and working condition changes. When the operating environment of the inertial navigation system changes greatly, the fixed threshold may lead to a significant increase in the risk of false alarms or missed detections.

[0005] In view of the problem of inertial navigation state monitoring without external reference information at present, the present invention proposes an external field state monitoring method for a redundant two-axis rotary inertial navigation system, which is applicable to platforms equipped with multiple sets of inertial navigation systems with indexing mechanisms. Based on the navigation information of two inertial navigation systems operating in the two-axis rotation modulation mode, a combined state Kalman filter for the two inertial navigation systems is constructed, and the geometric constraint relationships between the two inertial navigation systems, namely relative attitude, relative velocity and relative position, are used as observations. The gyro drift and accelerometer zero bias of the two systems are estimated online by using strong tracking filtering based on residual normalization. Furthermore, the state monitoring threshold is adaptively constructed based on the error parameters estimated online to realize the online state monitoring of the redundant inertial navigation system. 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 whole 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 an external field state monitoring method for a redundant two-axis rotary inertial navigation system, which is not affected by the absolute error of the reference inertial navigation system and can realize the autonomous state monitoring at the device level of the redundant two-axis rotary inertial navigation system, and has important engineering practical value.

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

[0008] An external field state monitoring method for a redundant two-axis rotary inertial navigation system, the method comprising the following steps:

[0009] (1) Set the indexing order of two two-axis rotary inertial navigation systems, and define the two redundantly configured inertial navigation systems 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;

[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 two-axis rotary inertial navigation systems, establish a state space model. The specific steps are as follows:

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

[0012]

[0013] Among them,

[0014]

[0015] In the formula, represents the attitude error vector of Inertial Navigation 1, Indicates the eastward attitude error of INS 1, Indicates the northward attitude error of INS 1, Indicates the upward attitude error of INS 1, v n Represents the true velocity of the vehicle in the navigation coordinate system, Indicates the velocity error vector of INS 1, δL1 represents the latitude error of INS 1, δλ1 represents the longitude error of INS 1, and δh1 represents the altitude error of INS 1, Indicates the eastward velocity error of INS 1, Indicates the northward velocity error of INS 1, Indicates the upward velocity error of INS 1, Indicates the angular velocity error of the navigation coordinate system relative to the inertial coordinate system related to the latitude error and velocity error of INS 1, Indicates the angular velocity error of the Earth's rotation related to the latitude error of INS 1, Indicates the transfer angular velocity error related to the latitude error and velocity error of INS 1, Indicates the direction cosine matrix from the body coordinate system of INS 1 to the navigation coordinate system, Indicates the attitude error vector of INS 2, Indicates the eastward attitude error of INS 2, Indicates the northward attitude error of INS 2, Indicates the upward attitude error of INS 2, Indicates the velocity error vector of INS 2, δL2 represents the latitude error of INS 2, δλ2 represents the longitude error of INS 2, and δh2 represents the altitude error of INS 2, Indicates the eastward velocity error of INS 2, Indicates the northward velocity error of INS 2, Indicates the upward velocity error of INS 2, Indicates the angular velocity error of the navigation coordinate system relative to the inertial coordinate system related to the latitude error and velocity error of INS 2, Indicates the angular velocity error of the Earth's rotation related to the latitude error of INS 2, Indicates the transfer angular velocity error related to the latitude error and velocity error of INS 2, Indicates the direction cosine matrix from the body coordinate system of INS 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 rotational angular velocity of the navigation coordinate system relative to the Earth coordinate system, f n Is the projection of the specific force in the navigation coordinate system, v E Is the eastward velocity of the vehicle, L and h are the latitude and altitude of the vehicle's location, R E And R NThey are the radius of the prime vertical circle and the radius of the meridian circle at the location of the carrier, respectively. It represents the gyro component error of INS1, modeled as a constant drift and gyro noise 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, It represents the accelerometer component error of INS1, modeled as a constant zero bias and accelerometer noise 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, It represents the gyro component error of INS2, modeled as a constant drift and gyro noise 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, It represents the accelerometer component error of INS2, modeled as a constant zero bias and accelerometer noise 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 joint state equation:

[0017]

[0018] Among them,

[0019]

[0020]

[0021]

[0022] In the formula, F(t) represents the system state matrix, 0 i×j represents the zero matrix of the i-th row and j-th column, v N , v U represent the northward and upward velocities of the carrier, λ represents the longitude of the carrier's location, ω ieDenote the angular velocity of the Earth's rotation as f E and f N and f U respectively represent the projections of the specific force in the east, north, and up directions;

[0023] The state vector x(t) is expressed as:

[0024]

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

[0026]

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

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

[0029]

[0030] wherein, is the direction cosine matrix from the inertial navigation 1 coordinate system s 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 s 20 system and the s 10 system;

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

[0032]

[0033] Considering the lever arm, determine the velocity and position outputs of inertial navigation 1 and inertial navigation 2 as:

[0034]

[0035] wherein, and respectively represent the velocity information in the navigation coordinate system output by inertial navigation 1 and inertial navigation 2, and v n represents the true velocity of the vehicle in the navigation coordinate system, represents the difference between the velocity output by inertial navigation 1 in the navigation coordinate system and the true velocity of the vehicle, It represents the difference between the velocity output by the inertial navigation 2 in the navigation coordinate system and the true velocity of the carrier. It represents the position information output by the inertial navigation 1. It represents the position information output by the inertial navigation 2, r n It represents the true position of the carrier in the navigation coordinate system. It represents the difference between the position output by the inertial navigation 1 in the navigation coordinate system and the true position of the carrier. It represents the difference between the position output by the inertial navigation 2 in the navigation coordinate system and the true position of the carrier. It represents the velocity difference of the inertial navigation 2 relative to the inertial navigation 1 caused by the external lever arm between the two inertial navigations, l r12 It represents the position difference of the inertial navigation 2 relative to the inertial navigation 1 caused by the external lever arm between the two inertial navigations;

[0036] Therefore, the differences in the velocity error and position error of the two inertial navigation systems are expressed as:

[0037]

[0038] The observation equation is expressed as:

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

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

[0041]

[0042] In the formula, 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 observed quantity;

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

[0044] 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:

[0045]

[0046] In the formula,

[0047]

[0048] And there is

[0049]

[0050]

[0051] Among them, λ 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 value of the residual itself leads to a decrease in the response speed of the error state estimation;

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

[0053] 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;

[0054] Set a data window that slides with 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:

[0055]

[0056] 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 those of ;

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

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

[0059] 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:

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

[0061] T + = k2Σ k

[0062] 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;

[0063] The health status monitoring criterion is established as:

[0064]

[0065] wherein, 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 real-time high threshold and the low threshold to be obtained. 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.

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

[0067] Furthermore, in the step (1), the inertial navigation 1 and the 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.

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

[0069] Furthermore, the relative attitude of the inertial navigation 1 and the inertial navigation 2 when they are at zero position in the step (2) is determined after the two sets of inertial navigations are aligned respectively after installation, or determined with the help of an external attitude reference.

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

[0071] Further, in the step (2), is determined by the angular position of the rotating frame output by the indexing mechanism.

[0072] Further, 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 rotationally modulated inertial navigations, but also applicable to the cases where both inertial navigation 1 and inertial navigation 2 are three-axis rotationally modulated inertial navigations, inertial navigation 1 is a two-axis rotationally modulated inertial navigation or a three-axis rotationally modulated inertial navigation, inertial navigation 2 is a single-axis rotationally modulated inertial navigation, and redundant configurations of multiple sets of two-axis rotating inertial navigations and multiple sets of three-axis rotating inertial navigations.

[0073] In summary, the advantages and positive effects of the present invention are as follows: By asynchronous rotation of two sets of two-axis rotating inertial navigations, the present invention realizes online monitoring at the device level of the inertial navigation system by using 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 use environment, can improve the monitoring accuracy of the states of inertial devices on mobile platforms, and has important engineering practical significance. Description of the Drawings

[0074] Figure 1 is the flowchart of the method provided by the embodiment of the present invention;

[0075] In the figure, and respectively represent the attitude matrices output by inertial navigation 1 and inertial navigation 2, and respectively represent the speeds in the navigation system output by inertial navigation 1 and inertial navigation 2, and respectively represent the positions output by inertial navigation 1 and inertial navigation 2. Detailed Embodiments

[0076] 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 with reference to the 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.

[0077] The existing field state monitoring solutions for inertial navigation systems have requirements for the motion state of the carrier and external reference information. However, for environments without external reference information such as underwater environments, the current general solutions cannot meet the requirements. In view of this problem, based on the current situation that multiple sets of inertial navigation systems are generally installed on the current carrier, the present invention proposes a method for monitoring the field state of a redundant two-axis rotating inertial navigation system, and the method is as Figure 1 shown. The detailed embodiments are as follows:

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

[0079] A method for monitoring the external field state of a redundant two-axis rotary inertial navigation system, the method comprising the following steps:

[0080] (1) Set the indexing order of two sets of two-axis rotary inertial navigation systems. Define the two sets of 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;

[0081] (2) Define the body coordinate systems of the two sets of inertial navigation systems. 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 rotary inertial navigation systems to establish a state space model. The specific steps are as follows:

[0082] (2.1) Determine the system combined error equation:

[0083]

[0084] Among them,

[0085]

[0086] 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 true velocity of the carrier in the navigation coordinate system, represents the velocity error vector 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 height error of Inertial Navigation 1, represents the eastward velocity error of Inertial Navigation 1, represents the northward velocity error of Inertial Navigation 1, represents the upward velocity 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, denotes the velocity error vector of INS 2, δL2 denotes the latitude error of INS 2, δλ2 denotes the longitude error of INS 2, and δh2 denotes the altitude error of INS 2. denotes the eastward velocity error of INS 2. denotes the northward velocity error of INS 2. denotes the upward velocity error of INS 2. denotes the angular velocity error of the navigation coordinate system relative to the inertial coordinate system related to the latitude error and velocity error of INS 2. denotes the angular velocity error of the Earth's rotation related to the latitude error of INS 2. denotes the transfer angular velocity error related to the latitude error and velocity error of INS 2. denotes the direction cosine matrix from the body coordinate system of INS 2 to the navigation coordinate system. is the rotation 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 rotation angular velocity of the navigation coordinate system relative to the Earth coordinate system, f n is the projection of the specific force in the navigation coordinate system, v E is the eastward velocity of the carrier, L and h are the latitude and altitude of the carrier's location, R E and R N are respectively the radius of the prime vertical and the radius of the meridian at the carrier's location. denotes the gyro assembly error of INS 1, modeled as a constant drift and gyro noise the sum of which, where, denotes the x-axis gyro drift of INS 1. denotes the y-axis gyro drift of INS 1. denotes the z-axis gyro drift of INS 1. denotes the accelerometer assembly error of INS 1, modeled as a constant zero bias and accelerometer noise the sum of which, where, denotes the x-axis accelerometer zero bias of INS 1. denotes the y-axis accelerometer zero bias of INS 1. denotes the z-axis accelerometer zero bias of INS 1. denotes the gyro assembly error of INS 2, modeled as a constant drift and gyro noise the sum of which, where, denotes the x-axis gyro drift of INS 2. denotes the y-axis gyro drift of INS 2. denotes the z-axis gyro drift of INS 2. denotes the accelerometer assembly error of INS 2, modeled as a constant zero bias and accelerometer noise and, where represents the zero bias of the x-axis accelerometer of inertial navigation 2, represents the zero bias of the y-axis accelerometer of inertial navigation 2, represents the zero bias of the z-axis accelerometer of inertial navigation 2;

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

[0088]

[0089] where

[0090]

[0091]

[0092]

[0093] In the formula, F(t) represents the system state matrix, 0 i×j represents the zero matrix of the i-th row and j-th column, v N , v U represent the northward and upward velocities of the carrier, λ represents the longitude of the carrier's location, ω ie represents the angular velocity of the Earth's rotation, f E , f N , f U respectively represent the projections of specific force in the eastward, northward, and upward directions;

[0094] The state vector x(t) is expressed as:

[0095]

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

[0097]

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

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

[0100]

[0101] In the formula, is the coordinate system s of inertial navigation 1 when the indexing mechanism is in the zero position10 The direction cosine matrix to the navigation coordinate system and respectively represent the attitude matrices of the indexing mechanisms of INS 1 and INS 2 relative to their zero positions at time t represents s 20 system and s 10 The attitude matrix between systems;

[0102] The difference expression for determining the attitude errors of the two sets of two-axis rotating INSs is:

[0103]

[0104] Taking into account the lever arm, the velocity and position outputs of INS 1 and INS 2 are expressed as:

[0105]

[0106] In the formula, and respectively represent the velocity information in the navigation coordinate system output by INS 1 and INS 2, v n represents the true velocity of the carrier in the navigation coordinate system represents the difference between the velocity output by INS 1 in the navigation coordinate system and the true velocity of the carrier represents the difference between the velocity output by INS 2 in the navigation coordinate system and the true velocity of the carrier represents the position information output by INS 1 represents the position information output by INS 2, r n represents the true position of the carrier in the navigation coordinate system represents the difference between the position output by INS 1 in the navigation coordinate system and the true position of the carrier represents the difference between the position output by INS 2 in the navigation coordinate system and the true position of the carrier represents the velocity difference of INS 2 relative to INS 1 caused by the external lever arm between the two sets of INSs, l r12 represents the position difference of INS 2 relative to INS 1 caused by the external lever arm between the two sets of INSs;

[0107] Therefore, the differences in the velocity error and position error of the two sets of INS systems are expressed as:

[0108]

[0109] The observation equation is expressed as:

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

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

[0112]

[0113] Wherein, 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;

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

[0115] Use 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:

[0116]

[0117] Wherein,

[0118]

[0119] And there is

[0120]

[0121]

[0122] Among them, λ 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, and γ k represents the innovation at time k, and η is the normalization parameter, which is used to eliminate the problem that the response speed of the error state estimation is reduced due to the information asymmetry caused by the difference in the numerical value of the residual itself;

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

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

[0125] Set a data window that slides with 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:

[0126]

[0127] In the formula, μ 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 ;

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

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

[0130] In the formula, Σ k represents the sliding window mean after iteration at time k. Determine the upper and lower thresholds as follows:

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

[0132] T + =k2Σ k

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

[0134] Establish the health status monitoring criterion as:

[0135]

[0136] In the formula, represents the i-th component of the estimated output of the filter at time k + 1, T + (i) and T -(i) respectively represents the i-th component 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.

[0137] Based on the joint indexing method given in the above 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 the above step (2) and the above step (3); based on the error parameters output by the filter, the state monitoring of the inertial navigation system can be realized through the above step (4).

[0138] In the above 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.

[0139] In the above step (1), Inertial Navigation 1 and Inertial Navigation 2 adopt different indexing orders and rotate synchronously.

[0140] The relative attitude when Inertial Navigation 1 and Inertial Navigation 2 are in the zero position in the above step (2) It is determined after the two sets of inertial navigations are aligned respectively after installation, or determined with the help of an external attitude reference.

[0141] The lever arm between Inertial Navigation 1 and Inertial Navigation 2 in the above step (2) is calibrated and determined after the two sets of inertial navigations are installed.

[0142] In the above step (2) and is determined by the angular position of the rotating frame output by the indexing mechanism.

[0143] 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 rotationally modulated inertial navigations, but also applicable to the cases where both Inertial Navigation 1 and Inertial Navigation 2 are tri-axis rotationally modulated inertial navigations, Inertial Navigation 1 is a dual-axis rotationally modulated inertial navigation or a tri-axis rotationally modulated inertial navigation, Inertial Navigation 2 is a single-axis rotationally modulated inertial navigation, and multiple sets of dual-axis rotating inertial navigations and multiple sets of tri-axis rotating inertial navigations are redundantly configured.

[0144] The method of the present invention has no requirements for the motion state of the carrier, and 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 an underwater environment and a GNSS-denied environment.

[0145] The above are only the preferred embodiments of the present invention, and are not intended to limit the present invention. All technical solutions falling within the idea of the present invention belong to 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 the protection scope of the present invention.

Claims

1. A method for monitoring the external field state of a redundant biaxial rotating inertial navigation system, characterized in that The method includes the following steps: (1) Set the indexing order of two sets of two-axis rotating 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 rotating inertial navigation systems to establish a state space 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 true velocity of the vehicle in the navigation coordinate system, represents the velocity error vector 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 eastward velocity error of inertial navigation 1, represents the northward velocity error of inertial navigation 1, represents the upward velocity 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, δ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 eastward velocity error of inertial navigation 2, represents the northward velocity error of inertial navigation 2, represents the upward velocity 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 Earth's rotation angular velocity vector, is the rotational angular velocity of the navigation coordinate system relative to the Earth coordinate system, f n is the projection of the specific force in the navigation coordinate system, v E The eastward velocity of the carrier, L and h are the latitude and altitude of the carrier's location, R E and R N are the radius of the prime vertical and the radius of the meridian at the carrier's location, respectively, represents the gyro component error of Inertial Navigation 1, modeled as a constant drift and gyro noise The sum of, 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 component error of Inertial Navigation 1, modeled as a constant zero bias and accelerometer noise The sum of, 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 component error of Inertial Navigation 2, modeled as a constant drift and gyro noise The sum of, 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 component error of Inertial Navigation 2, modeled as a constant zero bias and accelerometer noise The sum of, 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, In the formula, F(t) represents the system state matrix, 0 i×j represents the zero matrix of the i-th row and j-th column, v N and v U represent the northward and upward velocities of the vehicle, λ represents the longitude of the location where the vehicle is located, ω ie represents the angular velocity of the Earth's rotation, f E and f N and f U respectively represent the projections of the specific force in the eastward, northward, and upward directions; 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 in the zero position as the s 10 system and the s 20 system, the vehicle coordinate system is the b system, and 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 inertial navigation 1 coordinate system s 10 to the navigation coordinate system when the indexing mechanism is at the zero position, 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 s 10 coordinate systems; Determine the difference expression of the attitude errors of the two sets of two-axis rotating inertial navigations: Considering the lever arm, determine the velocity and position outputs of Inertial Navigation 1 and Inertial Navigation 2 as: In the formula, and respectively represent the velocity information in the navigation coordinate system output by inertial navigation 1 and inertial navigation 2, and v n represents the true velocity of the vehicle in the navigation coordinate system, represents the difference between the velocity output by inertial navigation 1 in the navigation coordinate system and the true velocity of the vehicle, represents the difference between the velocity output by inertial navigation 2 in the navigation coordinate system and the true velocity of the vehicle, represents the position information output by inertial navigation 1, represents the position information output by inertial navigation 2, and r n represents the true position of the vehicle in the navigation coordinate system, and δr1 n represents the difference between the position output by inertial navigation 1 in the navigation coordinate system and the true position of the vehicle, represents the difference between the position output by inertial navigation 2 in the navigation coordinate system and the true position of the vehicle, 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 navigation, and l r12 represents the position difference of inertial navigation 2 relative to inertial navigation 1 caused by the external lever arm between the two sets of inertial navigation; Therefore, express the differences in the velocity errors and position errors of the two sets of inertial navigation systems 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 transfer 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 observed quantity; (3) Establish an adaptive error parameter estimation filter; Use 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: 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 decrease 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 achieved 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 represent the mean and variance of the real-time estimated parameters of the filter within the sliding window at time k, respectively. The dimensions of both are the same as those of ; Set a weighting coefficient α and perform iterative calculation on the mean value of the historical sliding window: Σ k = α·μ k + (1 - α)Σ k-1 where, Σ k represents the moving 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 where 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: 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 required real-time high threshold and low threshold. 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.

2. The method for monitoring the external field state of a redundant two-axis rotary inertial navigation system 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. The method for monitoring the external field state of a redundant two-axis rotary inertial navigation system according to claim 1, wherein In step (1), Inertial Navigation 1 and Inertial Navigation 2 adopt different indexing orders and rotate synchronously.

4. A method for monitoring the external field state of a redundant biaxial rotary inertial navigation system 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 method for monitoring the external field state of a redundant two-axis rotating inertial navigation system 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 two sets of inertial navigations are installed.

6. The method for monitoring the external field state of a redundant two-axis rotary inertial navigation system according to claim 1, wherein In the said step (2) and is determined by the angular position of the rotating frame output by the indexing mechanism.

7. A method for monitoring the external field state of a redundant two-axis rotary inertial navigation system 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 rotationally modulated inertial navigations, but also applicable to the cases where both Inertial Navigation 1 and Inertial Navigation 2 are three-axis rotationally modulated inertial navigations, Inertial Navigation 1 is a two-axis rotationally modulated inertial navigation or a three-axis rotationally modulated inertial navigation, Inertial Navigation 2 is a single-axis rotationally modulated inertial navigation, and the redundant configuration of multiple sets of two-axis rotating inertial navigations and multiple sets of three-axis rotating inertial navigations.

Images