Redundant rotary inertial navigation state monitoring method under Psi angle correction model

Abstract

The invention belongs to the field of navigation, and discloses a redundant rotary inertial navigation state monitoring method under a Psi angle correction model. According to the method, a speed error model is corrected, the situation that the device state monitoring precision is affected by inaccurate solution caused by specific force differential under the dynamic condition is avoided, the postures, the speeds and the positions of two sets of inertial navigation are projected to the same coordinate system, then the relative values of the postures, the speeds and the positions are used as constraint observation, and a joint error state Kalman filter based on a Psi angle correction model is established. And carrying out online monitoring on gyroscopic drift and accelerometer zero offset of the two sets of systems by adopting strong tracking filtering based on residual normalization. And further evaluating the state of the inertial device according to the monitored error parameters. The method is completely autonomous, does not need any external reference information, only needs two sets of inertial navigation information as input, adopts open-loop filtering in the whole process, does not affect the normal work of the system, and has important engineering significance.

Description

Technical Field

[0001] The present invention belongs to the technical field of navigation, and relates to an outfield state monitoring method for an inertial navigation system, in particular to a redundant rotating inertial navigation state monitoring method under a Psi angle correction model, which is applicable to the state monitoring between two or more inertial navigation systems. Background Art

[0002] The safe navigation of carriers such as ships depends on the navigation information provided by the inertial navigation system. During long-term navigation, it is particularly important to ensure the security and reliability of the output information of the inertial navigation system. The state monitoring technology of the inertial navigation system is one of the key technologies to ensure the long-term stable operation of the inertial navigation system. By implementing effective state monitoring technology, potential faults can be detected and located in a timely manner, and measures can be taken to correct them at an early stage, so as to ensure the continuity and accuracy of the inertial navigation system during the entire navigation process.

[0003] The state monitoring technology of the inertial navigation system generally uses accurate reference information from the outside world as the observation basis, fuses the external reference benchmark information with the information output by the inertial navigation, and evaluates the reliability of the inertial navigation system at the current moment. However, in an underwater environment or other GNSS-denied environments, the external reference information that can be received is extremely limited, which greatly restricts the application of state monitoring technology. For large ships with long navigation times, since their navigation time reaches several days or even dozens of days and the reliability requirements for the system are extremely high, multiple inertial navigation systems with a transposition mechanism are usually equipped. By using the redundant navigation information of multiple inertial navigation systems, the error parameters of inertial devices can be estimated online, and the state identification of abnormal inertial navigation information can be realized to monitor the operating state of the system.

[0004] The traditional Phi angle error model is defined based on the true coordinate system, but the true coordinate system is usually unknown and often needs to be approximated by the computational coordinate system in actual operation, which will lead to certain errors. In contrast, the Psi angle error model is defined in the computational coordinate system and decoupled from the position error, which means it can reduce the errors caused by the coordinate system approximation and is more suitable for application to ships with long endurance navigation. When monitoring the state during ship navigation, the specific force term in the traditional velocity error equation needs to be obtained through differentiation. However, in a dynamic environment, this method is prone to introducing specific force errors and affecting the accuracy of state monitoring. Using the Psi angle error model can not only avoid the errors introduced by differentiating the specific force term, but also improve the state monitoring accuracy under dynamic conditions, ensuring high monitoring reliability even under complex and variable navigation conditions. In addition, due to the operating characteristics of the long endurance navigation system in a complex environment, the fixed threshold setting in the traditional state monitoring method is no longer applicable. During long-term navigation, factors such as environmental impacts will increase the uncertainty of the system model, making it difficult for the preset fixed threshold to accurately monitor the system operating state, and thus prone to problems such as missed detection or false detection.

[0005] In view of the existing problems, the present invention proposes a redundant inertial navigation state monitoring method under the Psi angle correction model, which is applicable to the state monitoring of inertial navigation systems equipped with multiple sets of inertial navigation systems with indexing mechanisms. Using the relative attitude, relative velocity, and relative position between inertial navigation systems as constraint observations, a joint state Kalman filter for a dual-inertial navigation system based on the Psi angle error correction model is established to perform real-time state monitoring on the error state of the inertial navigation system. Further, the state of the inertial device is detected and diagnosed according to the estimation result of the error state. This method is not affected by the motion state of the carrier and can realize online state monitoring of the inertial navigation system under both static and dynamic base conditions, solving the problem of real-time online state monitoring of the inertial navigation system when there is a lack of external reference information. By means of the error correction model, the specific force term in the model is eliminated, improving the state monitoring accuracy under dynamic conditions. At the same time, the Psi angle error model is defined in the computational coordinate system and decoupled from the position error, making it more suitable for the state monitoring of the inertial navigation system of ships with long endurance navigation. Summary of the Invention

[0006] The present invention proposes a method for monitoring the state of redundant rotating inertial navigation under the Psi angle correction model, which corrects the velocity error equation, eliminates the specific force term, and solves the problem of inaccurate error equation in the dynamic environment; by using the advantages of the Psi angle error model defined in the computational coordinate system and decoupled from the position error, a joint state space model of dual inertial navigation is constructed in the computational coordinate system, and the real-time state monitoring of the inertial navigation system is carried out based on the output results of the residual normalized strong tracking filter. The present invention can achieve autonomous state monitoring at the device level of the redundant dual-axis rotating inertial navigation system without any external reference information, and has important engineering practical value.

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

[0008] A method for monitoring the state of redundant rotating inertial navigation under the Psi angle correction model, the method comprising the following steps:

[0009] (1) Set the rotation order of two sets of dual-axis rotating inertial navigation systems, define the two sets of inertial navigation systems with redundant configuration as inertial navigation 1 and inertial navigation 2 respectively, and their rotation orders are both dual-axis 16 orders, adopting different rotation methods;

[0010] (2) Construct the error models of the two sets of inertial navigation systems;

[0011] Using the attitude, velocity, and position-related information output by the two sets of inertial navigation systems, establish a joint state Kalman filter based on the Psi angle error correction model, and the specific steps are as follows:

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

[0013]

[0014] Wherein,

[0015]

[0016] In the formula, b1 represents the body coordinate system of inertial navigation 1, b2 represents the body coordinate system of inertial navigation 2, c1 represents the computational coordinate system of inertial navigation 1, c2 represents the computational coordinate system of inertial navigation 2, p1 represents the platform coordinate system of inertial navigation 1, p2 represents the platform coordinate system of inertial navigation 2, ψ1 = [ψ E1 ψ N1 ψ U1 T represents the drift error angle of inertial navigation 1, ψ E1 , ψ N1 , ψ U1 respectively represent the eastward, northward, and upward drift error angles of inertial navigation 1, represents the velocity error vector of inertial navigation 1 after error correction in the computational coordinate system of inertial navigation 1, ​respectively represent the velocity errors of INS 1 in the east, north, and up directions after error correction, represent the position error of INS 1, represent the east error of INS 1, represent the north error of INS 1, represent the up error of INS 1, represent the earth's angular velocity of rotation in the calculation coordinate system of INS 1, represent the transfer angular velocity in the calculation coordinate system of INS 1, represent the direction cosine matrix from the body coordinate system of INS 1 to the platform coordinate system of INS 1, represent the gravity vector in the calculation coordinate system of INS 1, represent the vehicle velocity output by INS 1, ψ2 = [ψ E2 ψ N2 ψ U2 T represent the drift error angle of INS 2, ψ E2 、ψ N2 、ψ U2 respectively represent the drift error angles of INS 2 in the east, north, and up directions, represent the velocity error vector of INS 2 after error correction in the platform coordinate system respectively represent the velocity errors of INS 2 in the east, north, and up directions after error correction, represent the position error of INS 2, represent the east error of INS 2, represent the north error of INS 2, represent the up error of INS 2, represent the earth's angular velocity of rotation in the calculation coordinate system of INS 2, represent the transfer angular velocity in the calculation coordinate system of INS 2, represent the direction cosine matrix from the body coordinate system of INS 2 to the platform coordinate system of INS 2, respectively represent the gravity vector in the calculation coordinate system of INS 2, represent the vehicle velocity output by INS 2, represent the gyro component error of INS 1, modeled as the sum of a constant drift and gyro noise where, represent the x-axis gyro drift of INS 1, represent the y-axis gyro drift of INS 1, represent the z-axis gyro drift of INS 1, represent the accelerometer component error of INS 1, modeled as the sum of a constant zero bias and accelerometer noise where, ​Represents the zero bias of the x-axis accelerometer of inertial navigation 1, Represents the zero bias of the y-axis accelerometer of inertial navigation 1, Represents the zero bias of the z-axis accelerometer 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;

[0017] (2.2) Determine the joint state equation:

[0018]

[0019] Among them, the state vector x(t) is expressed as:

[0020]

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

[0022]

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

[0024] Define the coordinate systems when the indexing mechanisms of inertial navigation 1 and inertial navigation 2 are in the zero position as the b 10 system and the b 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:

[0025]

[0026] In the formula, I 3×3 Represents a 3-row and 3-column identity matrix, Is the direction cosine matrix from the vehicle coordinate system to the calculation coordinate system of inertial navigation 1, Is the direction cosine matrix from the b1 system to the b 10 system, is the direction cosine matrix from the vehicle coordinate system to the inertial navigation 2 calculation coordinate system, is the direction cosine matrix from the c2 system to the c1 system, is the direction cosine matrix from the c1 system to the c2 system, is the direction cosine matrix from the b2 system to the b 20 system;

[0027] The difference expression for determining the attitude errors of two sets of two-axis rotating inertial navigations is:

[0028]

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

[0030]

[0031] In the formula, and respectively represent the true velocities of the vehicle in the c1 system and the c2 system, and represent the position information output by inertial navigation 1 and inertial navigation 2, and respectively represent the true positions of the vehicle in the c1 system and the c2 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, 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 navigations;

[0032] Therefore, the differences of the velocity and position vectors of the two sets of inertial navigation systems are expressed as:

[0033]

[0034] The observation equation is expressed as:

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

[0036] where,

[0037]

[0038] In the formula, z(t) represents the observation vector of the system, H(t) represents the observation matrix of the system, υ(t) is the noise vector of the corresponding observed quantity, the subscript (1:2) represents the first two elements of the corresponding vector, and H1 represents the first two rows of the skew-symmetric matrix of the first two rows of the skew-symmetric matrix of the first two rows of the matrix, I 2×2 represents the 2×2 identity matrix;

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

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

[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 transition matrix, P k-1 is the covariance matrix at time k-1, G k-1 is the noise distribution 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, which is 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;

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

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

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

[0050]

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

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

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

[0054] where Σ k represents the mean of the sliding window after iteration at time k, and determine the high and low thresholds as follows:

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

[0056] T + = k2Σ k

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

[0058] Establish the health status monitoring criterion as:

[0059]

[0060] where represents the i-th component of the filter estimated output at time k + 1, T + (i) and T - (i) respectively represent the i-th components of the real-time high and low thresholds 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.

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

[0062] Furthermore, the indexing sequence in step (1) is only an example of the preferred solution for two inertial navigation systems equipped with a biaxial indexing mechanism. For combined indexing strategies with different rotation modulation sequences, they are also covered by the protection scope of the present invention.

[0063] Furthermore, in step (2) and obtained by calibrating the relative postures between the body coordinate systems of inertial navigation 1 and inertial navigation 2 and the vehicle coordinate system when the indexing mechanism is at the zero position.

[0064] Furthermore, the lever arm parameters between inertial navigation 1 and inertial navigation 2 in step (2) are obtained by calibration after the installation of the two inertial navigation devices is completed.

[0065] Furthermore, in step (2) determined by the positions output by inertial navigation 1 and inertial navigation 2.

[0066] Furthermore, the method of the present invention is not only applicable to the case where both inertial navigation 1 and inertial navigation 2 are biaxial rotation modulation inertial navigations, but also applicable to the cases where both inertial navigation 1 and inertial navigation 2 are triaxial rotation modulation inertial navigations, inertial navigation 1 is a biaxial rotation modulation inertial navigation or a triaxial rotation modulation inertial navigation, inertial navigation 2 is a uniaxial rotation modulation inertial navigation, inertial navigation 1 is a uniaxial rotation modulation inertial navigation, inertial navigation 2 is a biaxial rotation modulation inertial navigation or a triaxial rotation modulation inertial navigation, and redundant configurations of multiple sets of biaxial rotation inertial navigations and multiple sets of triaxial rotation inertial navigations.

[0067] In summary, the advantages and positive effects of the present invention are as follows: The present invention does not rely on external reference information and realizes device-level fault diagnosis of the inertial navigation system by using the redundant information of two inertial navigation systems. The present invention is not restricted by the usage environment and solves the problem of inaccurate error equations in dynamic environments through the Psi angle error correction model, which can improve the accuracy of state monitoring on mobile platforms and has important engineering practical significance. Description of the Drawings

[0068] Figure 1 is the flowchart of the method provided by the embodiment of the present invention. Detailed Embodiments

[0069] In order to make the objectives, technical solutions and advantages of the present invention clearer, the present invention will be further described in detail below with reference to 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.

[0070] The existing field state monitoring scheme for inertial navigation systems relies on accurate external reference information. However, for environments without external reference information such as underwater environments, and situations where the vehicle needs to navigate for a long time, the existing schemes cannot meet the requirements. To address this issue, in view of the current situation that current vehicles generally carry multiple sets of inertial navigation systems, the present invention proposes a redundant rotating inertial navigation state monitoring method under a Psi angle correction model. The state monitoring method is as follows Figure 1 as shown. The specific implementation is as follows:

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

[0072] A redundant rotating inertial navigation state monitoring method under a Psi angle correction model, the method comprising the following steps:

[0073] (1) Set the rotation order of two sets of two-axis rotating inertial navigation systems. Define the two sets of inertial navigation systems with redundant configuration as Inertial Navigation 1 and Inertial Navigation 2 respectively. The rotation order of both is the two-axis 16 order, and different rotation methods are used;

[0074] (2) Construct the error models of the two sets of inertial navigation systems;

[0075] Using the attitude, velocity, and position-related information output by the two sets of inertial navigation systems, establish a joint state Kalman filter based on the Psi angle error correction model. The specific steps are as follows:

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

[0077]

[0078] where,

[0079]

[0080] In the formula, b1 represents the body coordinate system of Inertial Navigation 1, b2 represents the body coordinate system of Inertial Navigation 2, c1 represents the calculation coordinate system of Inertial Navigation 1, c2 represents the calculation coordinate system of Inertial Navigation 2, p1 represents the platform coordinate system of Inertial Navigation 1, p2 represents the platform coordinate system of Inertial Navigation 2, ψ1 = [ψ E1 ψ N1 ψ U1 T represents the drift error angle of Inertial Navigation 1, and ψ E1 , ψ N1 , ψ U1 respectively represent the drift error angles of Inertial Navigation 1 in the east, north, and sky directions, represents the velocity error vector of Inertial Navigation 1 after error correction in the calculation coordinate system of Inertial Navigation 1, respectively represent the velocity errors of Inertial Navigation 1 in the east, north, and sky directions after error correction, represents the position error of Inertial Navigation 1,​ represents the eastward error of inertial navigation 1, represents the northward error of inertial navigation 1, represents the upward error of inertial navigation 1, represents the earth's angular velocity of rotation in the calculation coordinate system of inertial navigation 1, represents the transfer angular velocity in the calculation coordinate system of inertial navigation 1, represents the direction cosine matrix from the body coordinate system of inertial navigation 1 to the platform coordinate system of inertial navigation 1, represents the gravity vector in the calculation coordinate system of inertial navigation 1, represents the vehicle velocity output by inertial navigation 1, ψ2 = [ψ E2 ψ N2 ψ U2 T represents the drift error angle of inertial navigation 2, ψ E2 、ψ N2 、ψ U2 respectively represent the drift error angles of inertial navigation 2 in the eastward, northward, and upward directions, represents the velocity error vector of inertial navigation 2 after error correction in the platform coordinate system, respectively represent the velocity errors of inertial navigation 2 in the eastward, northward, and upward directions after error correction, represents the position error of inertial navigation 2, represents the eastward error of inertial navigation 2, represents the northward error of inertial navigation 2, represents the upward error of inertial navigation 2, represents the earth's angular velocity of rotation in the calculation coordinate system of inertial navigation 2, represents the transfer angular velocity in the calculation coordinate system of inertial navigation 2, represents the direction cosine matrix from the body coordinate system of inertial navigation 2 to the platform coordinate system of inertial navigation 2, respectively represent the gravity vector in the calculation coordinate system of inertial navigation 2, represents the vehicle velocity output by inertial navigation 2, represents the gyro component error of inertial navigation 1, modeled as the sum of a constant drift and gyro noise 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 the sum of a constant zero bias and accelerometer noise where, represents the x-axis accelerometer zero bias of inertial navigation 1, represents the y-axis accelerometer zero bias of inertial navigation 1, ​Denote the zero bias of the z-axis accelerometer of inertial navigation 1, denote the gyro component error of inertial navigation 2, modeled as a constant drift and gyro noise the sum of which, where, denote the x-axis gyro drift of inertial navigation 2, denote the y-axis gyro drift of inertial navigation 2, denote the z-axis gyro drift of inertial navigation 2, denote the accelerometer component error of inertial navigation 2, modeled as a constant zero bias and accelerometer noise the sum of which, where, denote the x-axis accelerometer zero bias of inertial navigation 2, denote the y-axis accelerometer zero bias of inertial navigation 2, denote the z-axis accelerometer zero bias of inertial navigation 2;

[0081] (2.2) Determine the joint state equation:

[0082]

[0083] where, represent the state vector x(t) as:

[0084]

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

[0086]

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

[0088] Define the coordinate systems when the transposition mechanisms of inertial navigation 1 and inertial navigation 2 are at the zero position as the b 10 system and the b 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 represented as:

[0089]

[0090]

[0091] In the formula, I 3×3 represents the 3-row and 3-column identity matrix, is the direction cosine matrix from the vehicle coordinate system to the calculation coordinate system of inertial navigation 1, is the direction cosine matrix from the b1 system to the b 10 system, is the direction cosine matrix from the vehicle coordinate system to the calculation coordinate system of inertial navigation 2, is the direction cosine matrix from the c2 system to the c1 system, is the direction cosine matrix from the c1 system to the c2 system, is the direction cosine matrix from the b2 system to the b 20 system;

[0092] The difference expression for determining the attitude errors of two sets of two-axis rotating inertial navigation is:

[0093]

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

[0095]

[0096] In the formula, and respectively represent the true velocities of the carrier in the c1 system and the c2 system, and represent the position information output by inertial navigation 1 and inertial navigation 2, and respectively represent the true positions of the carrier in the c1 system and the c2 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 navigation, 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;

[0097] Therefore, the differences of the velocity and position vectors of the two sets of inertial navigation systems are expressed as:

[0098]

[0099] The observation equation is expressed as:

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

[0101] where,

[0102]

[0103] In the formula, z(t) represents the observation vector of the system, H(t) represents the observation matrix of the system, υ(t) is the noise vector corresponding to the observed quantity, the subscript (1:2) represents the first two elements of the corresponding vector, and H1 represents the first two rows of the skew-symmetric matrix of H2 represents the first two rows of the skew-symmetric matrix of H3 represents the first two rows of the 2×2 matrix, and I

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

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

[0106]

[0107] In the formula,

[0108]

[0109] And there is

[0110]

[0111] 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 transfer matrix, P k-1 is the covariance matrix at time k-1, G k-1 is the noise distribution 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, which is 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;

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

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

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

[0115]

[0116] wherein, μ 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 ;

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

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

[0119] wherein, Σ k represents the mean of the sliding window after iteration at time k, and determine the following high and low thresholds:

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

[0121] T + =k2Σ k

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

[0123] Establish the health status monitoring criterion as:

[0124]

[0125] wherein, represents the i-th component of the estimated output of the filter at time k + 1, T + (i) and T - (i) respectively represent the i-th components of the real-time high threshold and 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.

[0126] 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).

[0127] As an improvement, the indexing sequence in step (1) is only an example of the preferred solution for two inertial navigation systems equipped with a biaxial indexing mechanism. For combined indexing strategies using different rotation modulation sequences, they are also included in the protection scope of the present invention.

[0128] As an improvement, in step (2) and is obtained by calibrating the relative postures between the body coordinate systems of INS 1 and INS 2 and the vehicle coordinate system when the indexing mechanism is at the zero position.

[0129] As an improvement, the lever arm parameters between INS 1 and INS 2 in step (2) are obtained by calibration after the installation of the two inertial navigation devices is completed.

[0130] As an improvement, in step (2) is determined by the positions output by INS 1 and INS 2.

[0131] As an improvement, the method of the present invention is not only applicable to the case where both INS 1 and INS 2 are biaxial rotation modulation inertial navigations. It is also applicable to the cases where both INS 1 and INS 2 are triaxial rotation modulation inertial navigations, INS 1 is a biaxial rotation modulation inertial navigation or a triaxial rotation modulation inertial navigation, INS 2 is a uniaxial rotation modulation inertial navigation, INS 1 is a uniaxial rotation modulation inertial navigation, INS 2 is a biaxial rotation modulation inertial navigation or a triaxial rotation modulation inertial navigation, and the redundant configurations of multiple sets of biaxial rotation inertial navigations and multiple sets of triaxial rotation inertial navigations.

[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 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 within the protection scope of the present invention.

Claims

1. A redundant rotation inertial navigation state monitoring method under the Psi angle correction model, 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) Construct the error models of the two inertial navigation systems; Using the attitude, velocity, and position-related information output by the two inertial navigation systems, establish a joint state Kalman filter based on the Psi angle error correction model. The specific steps are as follows: (2.1) Determine the system joint error equation: Where, Where, b1 represents the body coordinate system of INS1, b2 represents the body coordinate system of INS2, c1 represents the calculation coordinate system of INS1, c2 represents the calculation coordinate system of INS2, p1 represents the platform coordinate system of INS1, p2 represents the platform coordinate system of INS2, ψ1 = [ψ E1 ψ N1 ψ U1 T represents the drift error angle of INS1, and ψ E1 , ψ N1 , ψ U1 represent the eastward, northward, and upward drift error angles of INS1 respectively. represents the velocity error vector of INS1 after error correction in the calculation coordinate system of INS1. represent the velocity errors of INS1 in the eastward, northward, and upward directions after error correction respectively. represents the position error of INS1. represents the eastward error of INS1. represents the northward error of INS1. represents the upward error of INS1. represents the earth's angular velocity of rotation in the calculation coordinate system of INS1. represents the transfer angular velocity in the calculation coordinate system of INS1. represents the direction cosine matrix from the body coordinate system of INS1 to the platform coordinate system of INS1. represents the gravity vector in the calculation coordinate system of INS1. represents the vehicle velocity output by INS1, ψ2 = [ψ E2 ψ N2 ψ U2 T represents the drift error angle of INS2, and ψ E2 , ψ N2 , ψ U2 represent the eastward, northward, and upward drift error angles of INS2 respectively. represents the velocity error vector of INS2 after error correction in the platform coordinate system. represent the velocity errors of INS2 in the eastward, northward, and upward directions after error correction respectively. represents the position error of INS2. represents the eastward error of INS2. represents the northward error of INS2. represents the upward error of INS2. represents the earth's angular velocity of rotation in the calculation coordinate system of INS2. represents the transfer angular velocity in the calculation coordinate system of INS2. represents the direction cosine matrix from the body coordinate system of INS2 to the platform coordinate system of INS2. ​​respectively represent the gravity vector in the calculation coordinate system of INS2, represent the vehicle speed output by INS2, represent the gyro component error of INS1, modeled as a constant drift and gyro noise The sum of, where, represent the x-axis gyro drift of INS1, represent the y-axis gyro drift of INS1, represent the z-axis gyro drift of INS1, represent the accelerometer component error of INS1, modeled as a constant zero bias and accelerometer noise The sum of, where, represent the x-axis accelerometer zero bias of INS1, represent the y-axis accelerometer zero bias of INS1, represent the z-axis accelerometer zero bias of INS1, represent the gyro component error of INS2, modeled as a constant drift and gyro noise The sum of, where, represent the x-axis gyro drift of INS2, represent the y-axis gyro drift of INS2, represent the z-axis gyro drift of INS2, represent the accelerometer component error of INS2, modeled as a constant zero bias and accelerometer noise The sum of, where, represent the x-axis accelerometer zero bias of INS2, represent the y-axis accelerometer zero bias of INS2, represent the z-axis accelerometer zero bias of INS2; (2.2) Determine the joint state equation: Where, 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 system when the inertial navigation 1 and inertial navigation 2 indexing mechanisms are in zero position as b 10 Department and b 20 The carrier coordinate system is b system, and the attitude matrix output by two sets of dual-axis rotation inertial navigation is and It is expressed as: Where, I 3×3 represents a 3×3 identity matrix, is the direction cosine matrix from the vehicle coordinate system to the calculation coordinate system of inertial navigation 1, is the direction cosine matrix from the b1 system to the b 10 system, is the direction cosine matrix from the vehicle coordinate system to the calculation coordinate system of inertial navigation 2, is the direction cosine matrix from the c2 system to the c1 system, is the direction cosine matrix from the c1 system to the c2 system, is the direction cosine matrix from the b2 system to the b 20 system; 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: In the formula, and respectively represent the true velocities of the carrier in the c1 system and the c2 system, and represent the position information output by inertial navigation 1 and inertial navigation 2, and respectively represent the true positions of the carrier in the c1 system and the c2 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, 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 navigations; Therefore, the difference between the velocity 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, where \(z(t)\) represents the observation vector of the system, \(H(t)\) represents the observation matrix of the system, \(\upsilon(t)\) is the noise vector corresponding to the observed quantity, the subscript \((1:2)\) represents the first two elements of the corresponding vector, \(H_1\) represents the first two rows of the skew-symmetric matrix of the first two rows of the skew-symmetric matrix of the first two rows of the matrix, \(I\) 2×2 represents the \(2\times2\) identity matrix; (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 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 noise distribution 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, which is 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; (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 statistical characteristics of the data 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 ; 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 time k, 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 the 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. 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 redundant rotational inertial navigation state monitoring method under the Psi angle correction model according to claim 1, characterized in that In step (1), the indexing order of Inertial Navigation 1 and Inertial Navigation 2 adopts a combined indexing strategy with different rotation modulation orders.

3. The redundant rotation inertial navigation state monitoring method under the Psi angle correction model according to claim 1, characterized in that In the said step (2) and obtained by the relative attitudes between the inertial navigation 1 body coordinate system and the inertial navigation 2 body coordinate system and the carrier coordinate system when the calibration indexing mechanism is in the zero position.

4. The redundant rotation inertial navigation state monitoring method under the Psi angle correction model according to claim 1, characterized in that, In step (2), the lever arm parameters between Inertial Navigation 1 and Inertial Navigation 2 are obtained through calibration after the installation of the two inertial navigation devices is completed.

5. The redundant rotation inertial navigation state monitoring method under the Psi angle correction model according to claim 1, characterized in that In the said step (2) Determined by the positions output by inertial navigation 1 and inertial navigation 2.

6. The redundant rotational inertial navigation state monitoring method under the Psi angle correction model according to claim 1, wherein 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 rotating inertial navigations and multiple sets of three-axis rotating inertial navigations.

Images