Polar region double inertial navigation state monitoring method based on Psi angle correction model

Abstract

The invention belongs to the technical field of inertial navigation, discloses a polar region double inertial navigation state monitoring method based on a Psi angle correction model, and is suitable for state monitoring of a carrier equipped with multiple sets of inertial navigation systems with indexing mechanisms in a polar region environment. Aiming at the problem that the state monitoring is limited under the condition that an inertial navigation system has no external reference information during polar long-term navigation, based on a Psi angle error model, by utilizing the characteristic that the model is defined in a calculation coordinate system and is suitable for polar long-term navigation with position error decoupling, a transverse calculation coordinate system under an earth ellipsoid model is used as a navigation coordinate system; the relative attitude, the relative speed and the relative position between the two inertial navigation systems are used as constraint observation, and meanwhile, a speed error model is corrected, so that the situation that the device state monitoring precision is influenced by inaccurate solution caused by specific force differential under a dynamic condition is avoided. 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 independent, does not depend on external reference information, and has important engineering significance.

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 state of a polar dual inertial navigation system based on a Psi angle correction model, which is applicable to on-line state monitoring in the polar region between two or more inertial navigation systems with a two-axis or three-axis indexing mechanism. Background Art

[0002] The polar region is rich in natural resources and occupies an extremely important geographical location. In order to ensure the safe navigation of carriers such as ships in the polar region, achieving accurate positioning and navigation technology is a key challenge that needs to be overcome urgently. Since the meridians converge at the geographical poles and the geomagnetic lines are concentrated near the poles, coupled with the complex polar environment and frequent occurrence of magnetic storms and solar storms, many navigation methods commonly used in low-latitude regions are difficult to adapt to polar conditions. The inertial navigation system can continuously provide navigation information of the carrier, so it has become the main navigation means under polar conditions. However, the inertial navigation system also faces problems such as error accumulation caused by calculation overflow and lack of effective course reference in the polar region. In the polar environment, component aging and harsh external environment will increase the possibility of component failure of the inertial navigation system. In these cases, on-line state monitoring of the inertial navigation system is required to maintain the stability and reliability of the inertial navigation system.

[0003] In conventional navigation applications, the inertial navigation generally has accurate external reference information for observation. By combining the external reference benchmark information with the output information of the inertial navigation, the reliability of the current inertial navigation system can be judged. However, in the polar region environment, underwater environment, and GNSS denial environment, the external reference information that the inertial navigation can receive is extremely limited, and the application of state monitoring technology is restricted. Ships with polar navigation capabilities have a long navigation time and high requirements for system reliability, and usually carry multiple sets of inertial navigation systems with indexing mechanisms. By using the redundant information of the two inertial navigation systems and taking the relative attitude, relative velocity, and relative position between the two inertial navigation systems as constraint observations, a combined state Kalman filter can be constructed to realize the state monitoring of the inertial navigation system.

[0004] In addition, the traditional Phi angle error model is defined based on the true coordinate system, but the true coordinate system is usually unknown and is often approximated by the calculation coordinate system in actual operation, which will cause certain errors. In contrast, the Psi angle error model is defined in the calculation coordinate system and is separated from the position error, which makes it more suitable for ships sailing for a long time in the polar region. In addition, when the ship is in the process of state monitoring in the extreme polar environment, if the fixed threshold set based on the conventional working conditions in the middle and low latitudes is used for state monitoring, it may cause the state monitoring system to fail.

[0005] In view of the existing problems, the present invention proposes a polar dual-inertial navigation state monitoring method based on the Psi angle correction model, which is applicable to the state monitoring of carriers equipped with multiple sets of inertial navigation systems with indexing mechanisms in polar environments. The present invention uses the relative attitude, relative velocity, and relative position of two inertial navigation systems in the transverse calculation coordinate system as constraint observations, and establishes a joint state Kalman filter for the dual-inertial navigation system based on the Psi angle correction model to perform real-time state monitoring on the inertial navigation system. Strong tracking filtering based on residual normalization is used to online monitor the gyro drift and accelerometer zero bias of the two systems. Further, an adaptive threshold is constructed according to the estimated error parameters to monitor and diagnose the state of inertial devices. 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 in the polar environment when there is a lack of external reference information. By using the error correction model, the specific force term in the model is eliminated, improving the state monitoring accuracy under dynamic conditions; the Psi angle error model is defined in the calculation 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-term navigation in polar regions. Summary of the Invention

[0006] The present invention proposes a polar dual-inertial navigation state monitoring method based on the Psi angle correction model, which is not affected by the absolute error of the inertial navigation system and can realize autonomous state monitoring at the redundant dual-axis rotating inertial device level in polar regions, having important engineering practical value.

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

[0008] A polar dual-inertial navigation state monitoring method based on the Psi angle correction model, the method comprising the following steps:

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

[0010] (2) Construct a transverse earth coordinate system and a transverse calculation coordinate system based on the earth ellipsoid model;

[0011] Taking the point at 0° north latitude and 90° east longitude as the north pole in the transverse earth coordinate system, defined as the transverse north pole, the point at 0° north latitude and 90° west longitude as the south pole in the transverse earth coordinate system, defined as the transverse south pole, the elliptical surface surrounded by the 0° meridian and the 180° meridian as the transverse equatorial plane, taking the half major ellipse composed of the transverse north pole, the transverse south pole and the north pole as the 0° transverse meridian, and the plane where it is located as the transverse prime meridian, the conversion relationship is expressed as:

[0012]

[0013] Define a transverse calculation coordinate system based on the transverse and longitudinal grid. The transverse north-south direction points to the transverse north pole, the normal at the location points upward as the sky direction, and the transverse east direction is defined according to the right-hand coordinate system. The conversion relationship between the transverse calculation coordinate system c' and the calculation coordinate system c is Expressed as:

[0014]

[0015] In the formula, β represents the rotation angle between the calculation coordinate system and the transverse calculation coordinate system;

[0016] Determine the conversion relationship between β and longitude λ, latitude L, transverse longitude λ', and transverse latitude L':

[0017]

[0018] Express the conversion relationship between the transverse calculation coordinate system and the transverse platform coordinate system as:

[0019]

[0020] In the formula, the p' system represents the transverse platform coordinate system, I represents the identity matrix, and [ψ×] represents the skew-symmetric matrix of the drift error angle in the transverse calculation coordinate system;

[0021] Define the angle between the normal at the location of the carrier and the transverse equatorial plane as the transverse latitude, and the angle with the transverse prime meridian plane as the transverse longitude. Express the conversion relationship between the longitude and latitude defined in the earth coordinate system and the transverse longitude and latitude as:

[0022]

[0023] (3) Use the attitude, velocity, and position-related information output by two sets of inertial navigation systems to construct a joint state Kalman filter. The specific steps are as follows:

[0024] (3.1) Determine the system joint error equation:

[0025]

[0026] 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 transverse calculation coordinate system of inertial navigation 1, c2' represents the transverse 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 in the transverse calculation coordinate system of inertial navigation 1, ψ E1 、ψ​N1 , ψ U1 respectively represent the drift error angles of INS1 in the east, north, and up directions in the INS1 horizontal calculation coordinate system, represents the velocity error vector of INS1 after error correction in the INS1 horizontal calculation coordinate system, respectively represent the velocity errors of INS1 in the east, north, and up directions after error correction in the INS1 horizontal calculation coordinate system, represents the position error of INS1 in the INS1 horizontal calculation coordinate system, represents the eastward error of INS1 in the INS1 horizontal calculation coordinate system, represents the northward error of INS1 in the INS1 horizontal calculation coordinate system, represents the upward error of INS1 in the INS1 horizontal calculation coordinate system, represents the angular velocity of the Earth's rotation in the INS1 calculation coordinate system, represents the transfer angular velocity in the INS1 horizontal calculation coordinate system, represents the direction cosine matrix from the INS1 body coordinate system to the INS1 horizontal platform coordinate system, represents the gravity vector in the INS1 horizontal calculation coordinate system, represents the vehicle velocity of the INS1 output in the INS1 horizontal calculation coordinate system, ψ2 = [ψ E2 ψ N2 ψ U2 T represents the drift error angle of INS2 in the INS2 horizontal calculation coordinate system, ψ E2 , ψ N2 , ψ U2 respectively represent the drift error angles of INS2 in the east, north, and up directions in the INS2 horizontal calculation coordinate system, represents the velocity error vector of INS2 after error correction in the INS2 horizontal calculation coordinate system, respectively represent the velocity errors of INS2 in the east, north, and up directions after error correction in the INS2 horizontal calculation coordinate system, represents the position error of INS2 in the INS2 horizontal calculation coordinate system, represents the eastward error of INS2 in the INS2 horizontal calculation coordinate system, represents the northward error of INS2 in the INS2 horizontal calculation coordinate system, represents the upward error of INS2 in the INS2 horizontal calculation coordinate system, represents the angular velocity of the Earth's rotation in the INS2 horizontal calculation coordinate system, represents the transfer angular velocity in the INS2 horizontal calculation coordinate system, represents the direction cosine matrix from the INS2 body coordinate system to the INS2 horizontal platform coordinate system, represents the gravity vector in the INS2 horizontal calculation coordinate system, ​Denotes the vehicle velocity of the INS2 output in the INS2 horizontal calculation coordinate system. Denotes the gyro component error of INS1, modeled as a constant drift and gyro noise The sum of, where Denotes the x-axis gyro drift of INS1, Denotes the y-axis gyro drift of INS1, Denotes the z-axis gyro drift of INS1, Denotes the accelerometer component error of INS1, modeled as a constant zero bias and accelerometer noise The sum of, where Denotes the x-axis accelerometer zero bias of INS1, Denotes the y-axis accelerometer zero bias of INS1, Denotes the z-axis accelerometer zero bias of INS1, Denotes the gyro component error of INS2, modeled as a constant drift and gyro noise The sum of, where Denotes the x-axis gyro drift of INS2, Denotes the y-axis gyro drift of INS2, Denotes the z-axis gyro drift of INS2, Denotes the accelerometer component error of INS2, modeled as a constant zero bias and accelerometer noise The sum of, where Denotes the x-axis accelerometer zero bias of INS2, Denotes the y-axis accelerometer zero bias of INS2, Denotes the z-axis accelerometer zero bias of INS2;

[0027] (3.2) Determine the joint state equation:

[0028]

[0029] F(t) is the state transition matrix, determined by the error equation, and the state vector x(t) is expressed as:

[0030]

[0031] Express the noise distribution matrix G(t) and the noise matrix w(t) as:

[0032]

[0033] In the formula, 0 i×j Denotes the zero matrix of the i-th row and j-th column;

[0034] (3.3) Determine the state constraint observation equation:

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

[0036]

[0037]

[0038] In the formula, I 3×3 represents the 3-row and 3-column identity matrix, is the direction cosine matrix from the body coordinate system to the inertial navigation 1 transverse calculation coordinate system, is the direction cosine matrix from the b1 system to the b 10 system, is the direction cosine matrix from the b 10 system to the b system, is the direction cosine matrix from the body coordinate system to the inertial navigation 2 transverse calculation coordinate system, is the direction cosine matrix from the c′2 system to the c1′ system, is the direction cosine matrix from the c1′ system to the c′2 system, is the direction cosine matrix from the b 20 system to the b system, is the direction cosine matrix from the b2 system to the b 20 system;

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

[0040]

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

[0042]

[0043] 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, It represents the position difference of INS 2 relative to INS 1 caused by the external lever arm between the two sets of INSs;

[0044] Therefore, the differences in velocity and position vectors between the two sets of INS systems are expressed as:

[0045]

[0046] The observation equation is expressed as:

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

[0048] where,

[0049]

[0050] 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 matrix, I 2×2 represents the 2×2 identity matrix;

[0051] (4) Establish an adaptive error parameter estimation filter;

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

[0053]

[0054] In the formula,

[0055]

[0056] And there is

[0057]

[0058] 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 kis 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 of information asymmetry caused by the difference in the numerical value of the residual itself, resulting in a decrease in the response speed of the error state estimation;

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

[0060] When the inertial device state 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;

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

[0062]

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

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

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

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

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

[0068] T + = k2Σ k

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

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

[0071]

[0072] 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 low threshold to be obtained. The monitoring thresholds of each inertial device are updated in real time according to the sliding window. When all the error parameter values estimated by the filter are less than the low threshold, it is output that the system has no fault; when the i-th error parameter value estimated by the filter is greater than the low threshold and less than the high threshold, a fault warning is given; when the i-th error parameter value estimated by the filter is greater than the high threshold, it is output that device i has a fault.

[0073] Based on the joint rotation mode 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 under the polar region through step (2), step (3) and step (4); based on the error parameters output by the filter, the state monitoring of the inertial navigation system can be realized through step (5).

[0074] Furthermore, in step (1), Inertial Navigation 1 and Inertial Navigation 2 rotate at different times according to the same rotation sequence, that is, the two inertial navigations rotate in an asynchronous manner according to the same rotation scheme.

[0075] Furthermore, in step (1), Inertial Navigation 1 and Inertial Navigation 2 adopt different rotation sequences and rotate synchronously.

[0076] Furthermore, in step (3), and are obtained by calibrating the relative postures between the body coordinate systems of Inertial Navigation 1 and Inertial Navigation 2 and the carrier coordinate system when the calibration rotation mechanism is at the zero position.

[0077] Furthermore, the lever arm between Inertial Navigation 1 and Inertial Navigation 2 is calibrated and determined after the two sets of inertial navigations are installed.

[0078] Furthermore, in step (3), is determined by the positions output by Inertial Navigation 1 and Inertial Navigation 2.

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

[0080] In summary, the advantages and positive effects of the present invention are as follows: Through the asynchronous rotation of two sets of dual-axis rotation inertial navigations, the present invention uses the redundant information of the two inertial navigation systems to realize the online monitoring at the device level of the inertial navigation system. 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 inertial device states on a mobile platform, and has important engineering practical significance. Brief Description of the Drawings

[0081] Figure 1 It is a flow chart provided by an embodiment of the present invention. Detailed Embodiment

[0082] 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.

[0083] In the high-latitude polar regions, due to the rapid convergence of the meridians, there are large errors in the navigation scheme using the traditional geographical coordinate system as the navigation coordinate system. Moreover, in the harsh natural geographical environment of the polar regions, the inertial navigation system lacks reliable external reference information, and the inertial navigation system state monitoring method relying only on external auxiliary conditions is not applicable to the harsh environment of the polar regions. In addition, the traditional Phi angle error model is defined in the true coordinate system, but the true coordinate system is unknown. Using the calculated coordinate system approximated as the true coordinate system has an approximation error, and the specific force term in the traditional velocity error equation cannot be directly measured, and there is an error in the specific force calculation under a moving base. These factors will affect the accuracy of monitoring. To address these problems, the present invention proposes a polar dual-inertial navigation state monitoring method based on the Psi angle correction model. The monitoring method is as Figure 1 shown. The specific implementation is as follows:

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

[0085] (2) Construct a transverse Earth coordinate system and a transverse calculation coordinate system based on the Earth ellipsoid model;

[0086] Taking the point at 0° north latitude and 90° east longitude as the north pole in the transverse Earth coordinate system, defined as the transverse north pole, and the point at 0° north latitude and 90° west longitude as the south pole in the transverse Earth coordinate system, defined as the transverse south pole, the elliptical surface enclosed by the 0° meridian and the 180° meridian is the transverse equatorial plane. Taking the half major ellipse composed of the transverse north pole, the transverse south pole, and the north pole as the 0° transverse meridian, and the plane where it is located as the transverse prime meridian, the conversion relationship between the Earth coordinate system e and the newly defined transverse Earth coordinate system e′ is expressed as:

[0087]

[0088] Based on the transverse latitude and longitude grid, define the transverse calculation coordinate system. The transverse north direction points to the transverse north pole, the normal direction at the location points upward as the sky direction, and define the transverse east direction according to the right - hand coordinate system. The conversion relationship between the transverse calculation coordinate system c′ and the calculation coordinate system c is expressed as:

[0089]

[0090] In the formula, β represents the rotation angle between the calculation coordinate system and the transverse calculation coordinate system;

[0091] Determine the conversion relationship between β and longitude λ, latitude L, transverse longitude λ′, and transverse latitude L′:

[0092]

[0093] Express the conversion relationship between the transverse calculation coordinate system and the transverse platform coordinate system as:

[0094]

[0095] In the formula, the p′ system represents the transverse platform coordinate system, I represents the identity matrix, and [ψ×] represents the skew - symmetric matrix of the drift error angle in the transverse calculation coordinate system;

[0096] Define the angle between the normal direction at the location of the carrier and the transverse equatorial plane as the transverse latitude, and the angle between the normal direction at the location of the carrier and the transverse prime meridian plane as the transverse longitude. Express the conversion relationship between the longitude and latitude defined in the Earth coordinate system and the transverse longitude and latitude as:

[0097]

[0098] (3) Using the attitude, velocity, and position - related information output by two inertial navigation systems, construct a joint - state Kalman filter. The specific steps are as follows:

[0099] (3.1) Determine the system joint - error equation:

[0100]

[0101] 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 transverse calculation coordinate system of inertial navigation 1, c2′ represents the transverse 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 in the transverse calculation coordinate system of inertial navigation 1, ψ E1 、ψ N1 、ψ U1 respectively represent the eastward, northward, and upward drift error angles of inertial navigation 1 in the transverse calculation coordinate system of inertial navigation 1, represents the velocity error vector of inertial navigation 1 after error correction in the transverse calculation coordinate system of inertial navigation 1, respectively represent the velocity errors of inertial navigation 1 in the eastward, northward, and upward directions after error correction in the transverse calculation coordinate system of inertial navigation 1, represents the position error of inertial navigation 1 in the transverse calculation coordinate system of inertial navigation 1, represents the eastward error of inertial navigation 1 in the transverse calculation coordinate system of inertial navigation 1, represents the northward error of inertial navigation 1 in the transverse calculation coordinate system of inertial navigation 1, represents the upward error of inertial navigation 1 in the transverse calculation coordinate system of inertial navigation 1, represents the angular velocity of the Earth's rotation in the calculation coordinate system of inertial navigation 1, represents the transfer angular velocity in the transverse calculation coordinate system of inertial navigation 1, represents the direction cosine matrix from the body coordinate system of inertial navigation 1 to the transverse platform coordinate system of inertial navigation 1, represents the gravity vector in the transverse calculation coordinate system of inertial navigation 1, represents the vehicle velocity of the output of inertial navigation 1 in the transverse calculation coordinate system, ψ2 = [ψ E2 ψ N2 ψ U2 T represents the drift error angle of inertial navigation 2 in the transverse calculation coordinate system of inertial navigation 2, ψ E2 、ψ N2 、ψ U2 respectively represent the eastward, northward, and upward drift error angles of inertial navigation 2 in the transverse calculation coordinate system of inertial navigation 2, represents the velocity error vector of inertial navigation 2 after error correction in the transverse calculation coordinate system of inertial navigation 2, respectively represent the velocity errors of inertial navigation 2 in the eastward, northward, and upward directions after error correction in the transverse calculation coordinate system of inertial navigation 2, represents the position error of inertial navigation 2 in the transverse calculation coordinate system of inertial navigation 2, represents the eastward error of inertial navigation 2 in the transverse calculation coordinate system of inertial navigation 2, ​​Denotes the northward error of INS2 in the INS2 transverse calculation coordinate system, Denotes the vertical error of INS2 in the INS2 transverse calculation coordinate system, Denotes the earth's angular velocity of rotation in the INS2 transverse calculation coordinate system, Denotes the transfer angular velocity in the INS2 transverse calculation coordinate system, Denotes the direction cosine matrix from the INS2 body coordinate system to the INS2 transverse platform coordinate system, Denotes the gravity vector in the INS2 transverse calculation coordinate system, Denotes the vehicle velocity of the INS2 output in the INS2 transverse calculation coordinate system, Denotes the gyro component error of INS1, modeled as a constant drift and gyro noise The sum of which, where, Denotes the gyro drift of the x-axis of INS1, Denotes the gyro drift of the y-axis of INS1, Denotes the gyro drift of the z-axis of INS1, Denotes the accelerometer component error of INS1, modeled as a constant zero bias and accelerometer noise The sum of which, where, Denotes the zero bias of the x-axis accelerometer of INS1, Denotes the zero bias of the y-axis accelerometer of INS1, Denotes the zero bias of the z-axis accelerometer of INS1, Denotes the gyro component error of INS2, modeled as a constant drift and gyro noise The sum of which, where, Denotes the gyro drift of the x-axis of INS2, Denotes the gyro drift of the y-axis of INS2, Denotes the gyro drift of the z-axis of INS2, Denotes the accelerometer component error of INS2, modeled as a constant zero bias and accelerometer noise The sum of which, where, Denotes the zero bias of the x-axis accelerometer of INS2, Denotes the zero bias of the y-axis accelerometer of INS2, Denotes the zero bias of the z-axis accelerometer of INS2;

[0102] (3.2) Determine the combined state equation:

[0103]

[0104] F(t) is the state transition matrix, determined by the error equation, and the state vector x(t) is expressed as:

[0105]

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

[0107]

[0108] where 0 i×j represents the zero matrix of the i-th row and j-th column;

[0109] (3.3) Determine the state constraint observation equation:

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

[0111]

[0112]

[0113] where I 3×3 represents the 3×3 identity matrix, is the direction cosine matrix from the carrier coordinate system to the transverse 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 b 10 system to the b system, is the direction cosine matrix from the carrier coordinate system to the transverse calculation coordinate system of inertial navigation 2, is the direction cosine matrix from the c′2 system to the c1′ system, is the direction cosine matrix from the c1′ system to the c′2 system, is the direction cosine matrix from the b 20 system to the b system, is the direction cosine matrix from the b2 system to the b 20 system;

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

[0115]

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

[0117]

[0118] where and respectively represent the true velocities of the vehicle in the c1' system and the c2' system, and represent the position information output by INS1 and INS2, and respectively represent the true positions of the vehicle in the c1' system and the c2' system, represents the velocity difference of INS2 relative to INS1 caused by the external lever arm between the two INSs, represents the position difference of INS2 relative to INS1 caused by the external lever arm between the two INSs;

[0119] Therefore, the differences in the velocities and position vectors of the two INS systems are expressed as:

[0120]

[0121] The observation equation is expressed as:

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

[0123] where,

[0124]

[0125] 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 the first two rows of the skew-symmetric matrix of the first two rows of the matrix of 2×2 represents the 2×2 identity matrix;

[0126] (4) Establish an adaptive error parameter estimation filter;

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

[0128]

[0129] In the formula,

[0130]

[0131] and there is

[0132]

[0133] where, λ k is the fading factor, P k / k-1is the one-step prediction covariance matrix, Φ k / k-1 represents the one-step state 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 of information asymmetry caused by the difference in the numerical value of the residual itself, resulting in a decrease in the response speed of the error state estimation;

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

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

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

[0137]

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

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

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

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

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

[0143] T + = k2Σ k

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

[0145] The health state monitoring criterion is established as:

[0146]

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

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

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

[0150] As an improvement, in step (1), inertial navigation 1 and inertial navigation 2 adopt different rotation sequences and rotate synchronously.

[0151] As an improvement, in step (3), and are obtained by calibrating the relative attitudes between the body coordinate systems of inertial navigation 1 and inertial navigation 2 and the vehicle coordinate system when the calibration rotation mechanism is at the zero position.

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

[0153] As an improvement, in step (3), Determined by the positions output by inertial navigation 1 and inertial navigation 2.

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

[0155] 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 concept 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 polar dual-inertial navigation state monitoring method based on 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 a transverse Earth coordinate system and a transverse calculation coordinate system based on the Earth ellipsoid model; Taking the point at 0° north latitude and 90° east longitude as the north pole in the transverse Earth coordinate system, defined as the transverse north pole, and the point at 0° north latitude and 90° west longitude as the south pole in the transverse Earth coordinate system, defined as the transverse south pole. The elliptical plane enclosed by the 0° and 180° meridians is the transverse equatorial plane. Taking the half major ellipse formed by the transverse north pole, transverse south pole, and the north pole as the 0° transverse meridian, and the plane where it is located as the transverse prime meridian. The conversion relationship between the Earth coordinate system e and the newly defined transverse Earth coordinate system e' is expressed as: Define a transverse calculation coordinate system based on the transverse and longitudinal grid. The transverse north direction points to the transverse north pole, the normal direction at the location is upward as the celestial direction, and the transverse east direction is defined according to the right-hand coordinate system. The conversion relationship between the transverse calculation coordinate system c' and the calculation coordinate system c is expressed as: In the formula, β represents the rotation angle between the calculation coordinate system and the transverse calculation coordinate system; Determine the conversion relationship between β and longitude λ, latitude L, transverse longitude λ′, and transverse latitude L′: Express the conversion relationship between the transverse calculation coordinate system and the transverse platform coordinate system as: In the formula, the p′ system represents the transverse platform coordinate system, I represents the identity matrix, and [ψ×] represents the skew-symmetric matrix of the drift error angle in the transverse calculation coordinate system; Define the angle between the normal line at the location of the carrier and the transverse equatorial plane as the transverse latitude, and the angle with the transverse prime meridian plane as the transverse longitude. Express the conversion relationship between the longitude and latitude defined in the Earth coordinate system and the transverse longitude and latitude as: (3) Utilize the attitude, velocity, and position-related information output by the two sets of inertial navigation systems to construct a joint state Kalman filter. The specific steps are as follows: (3.1) Determine the system joint error equation: 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 transverse calculation coordinate system of inertial navigation 1, c2′ represents the transverse 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 in the transverse calculation coordinate system of inertial navigation 1, ψ E1 、ψ N1 、ψ U1 respectively represent the eastward, northward, and upward drift error angles of inertial navigation 1 in the transverse calculation coordinate system of inertial navigation 1, represents the velocity error vector of inertial navigation 1 after error correction in the transverse calculation coordinate system of inertial navigation 1, respectively represent the velocity errors of inertial navigation 1 in the eastward, northward, and upward directions after error correction in the transverse calculation coordinate system of inertial navigation 1, represents the position error of inertial navigation 1 in the transverse calculation coordinate system of inertial navigation 1, represents the eastward error of inertial navigation 1 in the transverse calculation coordinate system of inertial navigation 1, represents the northward error of inertial navigation 1 in the transverse calculation coordinate system of inertial navigation 1, represents the upward error of inertial navigation 1 in the transverse calculation coordinate system of inertial navigation 1, represents the angular velocity of the earth's rotation in the calculation coordinate system of inertial navigation 1, represents the transfer angular velocity in the transverse calculation coordinate system of inertial navigation 1, represents the direction cosine matrix from the body coordinate system of inertial navigation 1 to the transverse platform coordinate system of inertial navigation 1, represents the gravity vector in the transverse calculation coordinate system of inertial navigation 1, represents the vehicle speed of the output of inertial navigation 1 in the transverse calculation coordinate system, ψ2 = [ψ E2 ψ N2 ψ U2 T represents the drift error angle of inertial navigation 2 in the transverse calculation coordinate system of inertial navigation 2, ψ E2 、ψ N2 、ψ U2 respectively represent the eastward, northward, and upward drift error angles of inertial navigation 2 in the transverse calculation coordinate system of inertial navigation 2, represents the velocity error vector of inertial navigation 2 after error correction in the transverse calculation coordinate system of inertial navigation 2, respectively represent the velocity errors of inertial navigation 2 in the eastward, northward, and upward directions after error correction in the transverse calculation coordinate system of inertial navigation 2, represents the position error of inertial navigation 2 in the transverse calculation coordinate system of inertial navigation 2, represents the eastward error of inertial navigation 2 in the transverse calculation coordinate system of inertial navigation 2, represents the northward error of inertial navigation 2 in the transverse calculation coordinate system of inertial navigation 2,​​ represents the celestial error of INS2 in the INS2 transverse calculation coordinate system, represents the earth's angular velocity of rotation in the INS2 transverse calculation coordinate system, represents the transfer angular velocity in the INS2 transverse calculation coordinate system, represents the direction cosine matrix from the INS2 body coordinate system to the INS2 transverse platform coordinate system, represents the gravity vector in the INS2 transverse calculation coordinate system, represents the vehicle velocity of the INS2 output in the INS2 transverse calculation coordinate system, represents the gyro component error of INS1, modeled as a constant drift and gyro noise the sum of which, where, represents the gyro drift of the x-axis of INS1, represents the gyro drift of the y-axis of INS1, represents the gyro drift of the z-axis of INS1, represents the accelerometer component error of INS1, modeled as a constant zero bias and accelerometer noise the sum of which, where, represents the zero bias of the x-axis accelerometer of INS1, represents the zero bias of the y-axis accelerometer of INS1, represents the zero bias of the z-axis accelerometer of INS1, represents the gyro component error of INS2, modeled as a constant drift and gyro noise the sum of which, where, represents the gyro drift of the x-axis of INS2, represents the gyro drift of the y-axis of INS2, represents the gyro drift of the z-axis of INS2, represents the accelerometer component error of INS2, modeled as a constant zero bias and accelerometer noise the sum of which, where, represents the zero bias of the x-axis accelerometer of INS2, represents the zero bias of the y-axis accelerometer of INS2, represents the zero bias of the z-axis accelerometer of INS2; (3.2) Determine the joint state equation: F(t) is the state transition matrix, which can be derived from the error equation. The state vector x(t) is expressed as: Express the noise distribution matrix G(t) and the noise matrix w(t) as: In the formula, 0 i×j represents a zero matrix of the i-th row and j-th column; (3.3) Determine the state constraint observation equation: Define the body coordinate system when the inertial navigation 1 and inertial navigation 2 transfer 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 inertial navigation 1 transverse calculation coordinate system, is the direction cosine matrix from the b1 system to the b 10 system, is the direction cosine matrix from the b 10 system to the b system, is the direction cosine matrix from the vehicle coordinate system to the inertial navigation 2 transverse calculation coordinate system, is the direction cosine matrix from the c′2 system to the c1′ system, is the direction cosine matrix from the c1′ system to the c′2 system, is the direction cosine matrix from the b 20 system to the b 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 as: 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 INS 1 and INS 2, and respectively represent the true positions of the carrier in the c1' system and the c2' system, represents the velocity difference of INS 2 relative to INS 1 caused by the external lever arm between the two sets of INSs, represents the position difference of INS 2 relative to INS 1 caused by the external lever arm between the two sets of INSs; Therefore, the difference between the velocity and position vectors of the two sets of 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; (4) Establish an adaptive error parameter estimation filter; Adopt a strong tracking filter based on residual normalization to track and estimate the error state. Express the one-step prediction of the filter covariance matrix as: In the formula, And there is where, λ k is the fading factor, P k / k-1 is the one-step prediction covariance matrix, Φ k / k-1 represents the state one-step transition matrix, P k-1 is the covariance matrix at time k-1, G k-1 is the process noise allocation matrix at time k-1, Q k-1 is the system noise matrix at time k-1, tr(·) is the matrix trace operator, H k is the system observation matrix at time k, R k is the observation noise matrix at time k, l k is the weakening factor, λ 0,k is the fading factor calculated at time k, represents the residual covariance matrix at time k, represents the residual covariance matrix at time k-1, ρ is the forgetting factor, taking 0.95 ≤ ρ ≤ 0.995, γ0 represents the innovation at time 0, γ k represents the innovation at time k, η is the normalization parameter, 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; (5) 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 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 the dimension; 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 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 a health state monitoring criterion as: In the formula, represents the i-th component of the filter estimated output at the (k + 1)-th moment, T + (i) and T - (i) respectively represent the i-th components of the required real-time high threshold and low threshold. According to the sliding window, the monitoring thresholds of each inertial device are updated in real time. When all the error parameter values estimated by the filter are less than the low threshold, it is output that the system has no fault; when the i-th error parameter value estimated by the filter is greater than the low threshold and less than the high threshold, a fault warning is given; when the i-th error parameter value estimated by the filter is greater than the high threshold, it is output that device i has a fault.

2. The polar double inertial navigation state monitoring method based on the Psi angle correction model according to claim 1, characterized in that 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 polar double inertial navigation state monitoring method based on the Psi angle correction model according to claim 1, wherein In step (1), Inertial Navigation 1 and Inertial Navigation 2 adopt different indexing orders and rotate synchronously.

4. The polar dual-inertial navigation state monitoring method based on the Psi angle correction model according to claim 1, wherein In the said step (3) and obtained by the relative postures 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 at the zero position.

5. The polar dual-inertial navigation state monitoring method based on the Psi angle correction model according to claim 1, wherein In step (3), the lever arm between Inertial Navigation 1 and Inertial Navigation 2 is calibrated and determined after the installation of the two sets of inertial navigations.

6. The polar dual inertial navigation state monitoring method based on the Psi angle correction model according to claim 1, characterized in that In the said step (3) Determined by the positions output by inertial navigation 1 and inertial navigation 2.

7. The polar dual-inertial navigation state monitoring method based on the Psi angle correction model according to claim 1, wherein The method of the present invention is applicable not only to the case where both inertial navigation 1 and inertial navigation 2 are two-axis rotationally modulated inertial navigations, but also 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, inertial navigation 1 is a single-axis rotationally modulated inertial navigation, inertial navigation 2 is a two-axis rotationally modulated inertial navigation or a three-axis rotationally modulated inertial navigation, and the redundant configurations of multiple sets of two-axis rotationally inertial navigations and multiple sets of three-axis rotationally inertial navigations.

Images