Polar double-inertial navigation state monitoring method based on psi angle correction model

Abstract

The present application belongs to the technical field of inertial navigation, and discloses a polar double-inertial navigation state monitoring method based on a Psi angle correction model, which is suitable for state monitoring of a carrier equipped with multiple sets of inertial navigation systems with a rotation mechanism in a polar environment. The present application is aimed at the problem of limited state monitoring of an inertial navigation system under the condition of no external reference information during long navigation in the polar region. Based on a Psi angle error model, the model is defined in a calculation coordinate system, and the position error is decoupled, which is suitable for the characteristics of long navigation in the polar region. A horizontal calculation coordinate system under an earth ellipsoid model is used as a navigation coordinate system, the relative attitude, relative velocity and relative position between two sets of inertial navigation systems are used as constraint observations, and through correction of a velocity error model, the present application avoids the influence of inaccurate solution caused by differential of specific force under dynamic conditions on the state monitoring precision of devices. Strong tracking filtering based on residual normalization is used to monitor the gyro drift and accelerometer zero offset of the two sets of systems online. Further, the state of inertial devices is evaluated according to the monitored error parameters. The method of the present application is completely autonomous and does not depend on external reference information, which has important engineering significance.

Description

Technical Field

[0001] This invention belongs to the field of navigation technology and relates to a state monitoring method for inertial navigation systems. In particular, it relates to a polar dual-inertial navigation state monitoring method based on the Psi angle correction model, which is applicable to online state monitoring between two or more inertial navigation systems with dual-axis or tri-axis rotation mechanisms in polar regions. Background Technology

[0002] The polar regions are rich in natural resources and occupy an extremely important geographical location. To ensure the safe navigation of ships and other vessels in the polar regions, achieving precise positioning and navigation technology is a critical challenge that urgently needs to be overcome. Because meridians converge at the geographical poles, and geomagnetic lines concentrate near the poles, coupled with the complex polar environment and frequent geomagnetic storms and solar storms, many navigation methods commonly used in low-latitude regions are unsuitable for polar conditions. Inertial navigation systems (INS) can continuously provide navigation information to the vessel, thus becoming the primary navigation method under polar conditions. However, INS also face problems in the polar regions, such as error accumulation due to computational overflow and the lack of effective heading references. In the polar environment, component aging and harsh external conditions increase the likelihood of INS component failure. In these situations, online status monitoring of the INS is necessary to maintain its stability and reliability.

[0003] In conventional navigation applications, inertial navigation systems (INS) typically have accurate external reference information for observation. By combining this external reference information with the INS output information, the reliability of the current INS system can be determined. However, in polar environments, underwater environments, and GNSS-denied environments, the external reference information that INS can receive is extremely limited, restricting the application of state monitoring technology. Ships with polar navigation capabilities have long voyage times and high requirements for system reliability, and typically carry multiple INS systems with indexing mechanisms. By utilizing the redundant information from two INS systems, and using the relative attitude, relative velocity, and relative position between the two INS systems as constrained observations, a joint state Kalman filter can be constructed to achieve state monitoring of the INS system.

[0004] Furthermore, the traditional Phi angle error model is defined based on a real coordinate system, but the real coordinate system is usually unknown. In practice, a computational coordinate system is often used as an approximation, which leads to certain errors. In contrast, the Psi angle error model is defined in a computational coordinate system and is separate from position errors, making it more suitable for ships undertaking long-duration polar voyages. Additionally, when monitoring a ship's condition during its journey in extreme polar environments, using fixed thresholds based on conventional low-latitude operating conditions may cause the condition monitoring system to malfunction.

[0005] This invention addresses existing problems by proposing a polar dual-inertial navigation system (INS) state monitoring method based on the Psi angle correction model. This method is applicable to the state monitoring of carriers equipped with multiple INS systems featuring rotation mechanisms in polar environments. The invention utilizes the relative attitude, relative velocity, and relative position of the two INS systems in the transverse computational coordinate system as constraint observations, establishing a joint state Kalman filter for the dual INS systems based on the Psi angle correction model for real-time state monitoring. A strong tracking filter based on residual normalization is employed to monitor the gyroscope drift and accelerometer bias of both systems online. Furthermore, an adaptive threshold is constructed based on the estimated error parameters to monitor and diagnose the state of the inertial devices. This method is unaffected by the carrier's motion state and can achieve online state monitoring of the INS system under both static and dynamic base conditions, solving the problem of real-time online state monitoring of INS systems in polar environments where external reference information is lacking. By using an error correction model, the specific force term in the model is eliminated, improving the accuracy of state monitoring under dynamic conditions. The Psi angle error model is defined in the computational coordinate system and is decoupled from the position error, making it more suitable for the state monitoring of inertial navigation systems of ships undergoing long-endurance polar voyages. Summary of the Invention

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

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

[0008] A polar dual inertial navigation system state monitoring method based on 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 redundant inertial navigation systems as Inertial Navigation 1 and Inertial Navigation 2. The rotation order of both is dual-axis 16 sequence, and different rotation methods are used.

[0010] (2) Construct a horizontal Earth coordinate system and a horizontal computational coordinate system based on the Earth ellipsoid model;

[0011] Taking 0°N 90°E as the North Pole in the transverse Earth coordinate system, defined as the transverse North Pole, and 0°N 90°W as the South Pole in the transverse Earth coordinate system, defined as the transverse South Pole, the ellipse enclosed by the 0° meridian and the 180° meridian is the transverse equatorial plane. The semi-great ellipse formed by the transverse North Pole, transverse South Pole, and North Pole is taken as the 0° transverse meridian, and its plane is the transverse Prime Meridian. The transformation relationship between the Earth coordinate system e and the newly defined transverse Earth coordinate system e′ is explained. Represented as:

[0012]

[0013] Based on the horizontal latitude and longitude grid, the horizontal calculation coordinate system is defined, with the horizontal north direction pointing to the horizontal North Pole. The normal at the location points upwards to the celestial direction. Following the right-hand coordinate system definition, the horizontal east direction is defined. The transformation relationship between the horizontal calculation coordinate system c′ and the calculation coordinate system c is explained. Represented as:

[0014]

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

[0016] Determine the conversion relationships between β and longitude λ, latitude L, longitude λ′, and latitude L′:

[0017]

[0018] The transformation relationship between the horizontal calculation coordinate system and the horizontal platform coordinate system is expressed as:

[0019]

[0020] In the formula, p′ represents the horizontal platform coordinate system, I represents the identity matrix, and [ψ×] represents the antisymmetric matrix of the drift error angle in the horizontal calculation coordinate system;

[0021] The angle between the normal to the location of the carrier and the transverse equatorial plane is defined as transverse latitude, and the angle with the transverse prime meridian plane is defined as transverse longitude. The conversion relationship between latitude and longitude and transverse latitude and longitude in the Earth coordinate system is expressed as follows:

[0022]

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

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

[0025]

[0026] In the formula, b1 represents the volume coordinate system of inertial navigation system 1, b2 represents the volume coordinate system of inertial navigation system 2, c1′ represents the transverse calculation coordinate system of inertial navigation system 1, c2′ represents the transverse calculation coordinate system of inertial navigation system 2, p1 represents the platform coordinate system of inertial navigation system 1, p2 represents the platform coordinate system of inertial navigation system 2, and ψ1=[ψ E1 ψ N1 ψ U1 ] T ψ represents the drift error angle of inertial navigation system 1 in the horizontal calculation coordinate system. E1 ψN1 ψ U1 These represent the eastward, northward, and celestial drift error angles of Inertial Navigation System 1 in the horizontal calculation coordinate system, respectively. This represents the velocity error vector of inertial navigation system 1 after error correction in the horizontal calculation coordinate system. These represent the velocity errors of Inertial Navigation System 1 in the east, north, and sky directions, respectively, after error correction in the horizontal calculation coordinate system. This represents the position error of inertial navigation system 1 in the horizontal calculation coordinate system of inertial navigation system 1. This indicates the eastward error of inertial navigation system 1 in the horizontal calculation coordinate system of inertial navigation system 1. This represents the northward error of inertial navigation system 1 in the horizontal calculation coordinate system of inertial navigation system 1. This indicates the azimuth error of inertial navigation system 1 in the horizontal calculation coordinate system of inertial navigation system 1. This indicates the Earth's rotational angular velocity in the inertial navigation system 1 calculation coordinate system. This represents the transfer angular velocity in the horizontal calculation coordinate system of inertial navigation system 1. This represents the direction cosine matrix from the inertial navigation system 1 body coordinate system to the inertial navigation system 1 transverse platform coordinate system. This represents the gravity vector in the horizontal calculation coordinate system of the inertial navigation system 1. This represents the vehicle velocity output by inertial navigation system 1 in the horizontal calculation coordinate system of inertial navigation system 1, ψ2=[ψ E2 ψ N2 ψ U2 ] T ψ represents the drift error angle of inertial navigation system 2 in the horizontal calculation coordinate system. E2 ψ N2 ψ U2 These represent the drift error angles of inertial navigation system 2 in the east, north, and sky directions, respectively, in the horizontal calculation coordinate system of inertial navigation system 2. This represents the velocity error vector of inertial navigation system 2 after error correction in the horizontal calculation coordinate system. These represent the velocity errors of inertial navigation system 2 in the east, north, and sky directions, respectively, after error correction in the horizontal calculation coordinate system. This represents the position error of inertial navigation system 2 in the horizontal calculation coordinate system of inertial navigation system 2. This indicates the eastward error of inertial navigation system 2 in the horizontal calculation coordinate system of inertial navigation system 2. This represents the northward error of inertial navigation system 2 in the horizontal calculation coordinate system of inertial navigation system 2. This represents the azimuth error of inertial navigation system 2 in the horizontal calculation coordinate system of inertial navigation system 2. This represents the Earth's rotational angular velocity in the inertial navigation system's second horizontal coordinate system. This represents the transfer angular velocity in the inertial navigation system's second horizontal calculation coordinate system. This represents the direction cosine matrix from the inertial navigation system 2 body coordinate system to the inertial navigation system 2 transverse platform coordinate system. This represents the gravity vector in the inertial navigation system's second horizontal coordinate system. This indicates that the output of Inertial Navigation System 2 (INS2) represents the vehicle velocity in the INS2 horizontal calculation coordinate system. The error of the gyroscope component in inertial navigation system 1 is modeled as a constant drift. and gyroscope noise The sum of, among which, This indicates the x-axis gyroscope drift of inertial navigation system 1. This indicates the y-axis gyroscope drift of inertial navigation system 1. This indicates the z-axis gyroscope drift of inertial navigation system 1. The error of the accelerometer component of inertial navigation system 1 is modeled as a constant zero bias. and accelerometer noise The sum of, among which, This indicates that the x-axis accelerometer of inertial navigation system 1 has zero bias. This indicates that the y-axis accelerometer of inertial navigation system 1 has zero bias. This indicates that the z-axis accelerometer of inertial navigation system 1 has zero bias. The error of the gyroscope component in Inertial Navigation System 2 is modeled as a constant drift. and gyroscope noise The sum of, among which, This indicates the x-axis gyroscope drift of inertial navigation system 2. This indicates the y-axis gyroscope drift of inertial navigation system 2. This indicates the z-axis gyroscope drift of inertial navigation system 2. The error of the accelerometer component in inertial navigation system 2 is modeled as a constant zero bias. and accelerometer noise The sum of, among which, This indicates that the x-axis accelerometer of inertial navigation system 2 has zero bias. This indicates that the y-axis accelerometer of inertial navigation system 2 has zero bias. This indicates that the z-axis accelerometer of inertial navigation system 2 has zero bias;

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

[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] The noise distribution matrix G(t) and the noise matrix w(t) are expressed as:

[0032]

[0033] In the formula, 0 i×j Represents the zero matrix in row i and column j;

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

[0035] Define the body coordinate system as b when the inertial navigation system 1 and inertial navigation system 2 are in the zero position. 10 System and b 20 The coordinate system is b-frame, and the attitude matrices are output from two sets of dual-axis rotating inertial navigation systems. and Represented as:

[0036]

[0037]

[0038] In the formula, I 3×3 This represents a 3x3 identity matrix. The direction cosine matrix from the carrier coordinate system to the inertial navigation system 1 horizontal calculation coordinate system is given. For b1 series to b 10 The direction cosine matrix of the system, For b 10 The direction cosine matrix from system b to system b The direction cosine matrix from the carrier coordinate system to the inertial navigation system's second transverse coordinate system is calculated. Let c′2 be the direction cosine matrix from c1′2 to c′2. Let c1′ be the direction cosine matrix from c′2 to c′2. For b 20 The direction cosine matrix from system b to system b For b2 series to b 20 The direction cosine matrix of the system;

[0039] The expression for the difference in attitude error between the two sets of dual-axis rotating inertial navigation systems is determined as follows:

[0040]

[0041] Considering the lever arm, the velocity and position outputs of inertial navigation systems 1 and 2 are expressed as follows:

[0042]

[0043] In the formula, and These represent the actual velocities of the carrier in the c1′ and c2′ systems, respectively. and This represents the position information output by inertial navigation system 1 and inertial navigation system 2. and These represent the actual positions of the vector in the c1′ and c2′ systems, respectively. This represents the velocity difference between inertial navigation system 2 and inertial navigation system 1 caused by the outer arm between the two inertial navigation systems. This represents the position difference of inertial navigation system 2 relative to inertial navigation system 1 caused by the outer arm between the two inertial navigation systems.

[0044] Therefore, the difference between the velocity and position vectors of the two inertial navigation systems is expressed as:

[0045]

[0046] The observation equation is expressed as:

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

[0048] in,

[0049]

[0050] In the formula, z(t) represents the system's observation vector, H(t) represents the system's observation matrix, υ(t) is the noise vector of the corresponding observation, the subscript (1:2) indicates the first two elements of the corresponding vector, and H1 indicates... The first two rows of the skew-symmetric matrix, H2 represents The first two rows of the skew-symmetric matrix, H3 represents The first two rows of the matrix, I 2×2 Represents a 2x2 identity matrix;

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

[0052] A strong tracking filter based on residual normalization is used to track and estimate the error state. The one-step prediction of the filter covariance matrix is ​​expressed as:

[0053]

[0054] In the formula,

[0055]

[0056] And there are

[0057]

[0058] Where, λ k P is the fading factor. k / k-1 To predict the covariance matrix in one step, Φ k / k-1 P represents the state transition matrix in one step. k-1 Let G be the covariance matrix at time k-1. k-1 The process noise assignment matrix at time k-1, Q k-1 Let H be the system noise matrix at time k-1, tr(·) be the matrix trace operator, and H be the system noise matrix at time k-1. k Let R be the system observation matrix at time k. kLet l be the observation noise matrix at time k. k λ is a weakening factor. 0,k The fading factor is calculated at time k. Let k represent the residual covariance matrix at time k. Let represent the residual covariance matrix at time k-1, ρ be the forgetting factor (taken as 0.95 ≤ ρ ≤ 0.995), γ0 represent the innovation at time 0, and γ k Represents the information at time k, and η is the normalization parameter used to eliminate the problem of reduced response speed of error state estimation caused by information asymmetry due to differences in the residual values ​​themselves.

[0059] (5) Real-time status monitoring is performed based on the error parameters output by the filter;

[0060] When the inertial device is in an abnormal state, the corresponding gyroscope drift or accelerometer zero bias changes. Real-time health status monitoring of the inertial device is achieved by analyzing the output of the monitoring filter.

[0061] A data window is set to slide over time, with a length of N. At time k, the filter output information contained in the sliding window is... The statistical characteristics of the calculated data are as follows:

[0062]

[0063] In the formula, μ k and Let represent the mean and variance of the real-time estimated parameters of the filter within the sliding window at time k, respectively, and their dimensions are and . Same dimension;

[0064] Set a weighting coefficient α and iteratively calculate the mean of the historical sliding windows:

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

[0066] In the formula, Σ k The sliding window mean after iterations at time k is used to determine the two thresholds as follows:

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

[0068] T + =k2Σ k

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

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

[0071]

[0072] In the formula, T represents the i-th component of the filter's estimated output at time k+1. + (i) and T - (i) represents the i-th component of the desired real-time high threshold and low threshold, respectively. The monitoring threshold of each inertial device is updated in real time according to the sliding window. When all error parameter values ​​estimated by the filter are less than the low threshold, the output system is fault-free. When the i-th error parameter value estimated by the filter is greater than the low threshold but less than the high threshold, a fault warning is issued. When the i-th error parameter value estimated by the filter is greater than the high threshold, the output device i is faulty.

[0073] Based on the joint rotation method given in step (1), inertial navigation system 1 and inertial navigation system 2 are put into normal navigation state. The dual inertial navigation system state space model under polar region is constructed through steps (2), (3) and (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 system 1 and inertial navigation system 2 rotate at different times according to the same rotation sequence, that is, the two inertial navigation systems rotate asynchronously according to the same rotation scheme.

[0075] Furthermore, in step (1), inertial navigation system 1 and inertial navigation system 2 adopt different rotation sequences and rotate synchronously.

[0076] Furthermore, in step (3) and The relative attitude between the inertial navigation system 1 and the inertial navigation system 2 and the carrier coordinate system is obtained by calibrating the rotation mechanism when it is in the zero position.

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

[0078] Furthermore, in step (3) The position is determined by the outputs of inertial navigation system 1 and inertial navigation system 2.

[0079] Furthermore, the method of the present invention is not only applicable to the case where both inertial navigation system 1 and inertial navigation system 2 are dual-axis rotation modulation inertial navigation systems, but also applicable to the case where both inertial navigation system 1 and inertial navigation system 2 are triaxial rotation modulation inertial navigation systems, where inertial navigation system 1 is a dual-axis rotation modulation inertial navigation system or a triaxial rotation modulation inertial navigation system and inertial navigation system 2 is a single-axis rotation modulation inertial navigation system, where inertial navigation system 1 is a single-axis rotation modulation inertial navigation system and inertial navigation system 2 is a dual-axis rotation modulation inertial navigation system or a triaxial rotation modulation inertial navigation system, and where multiple sets of dual-axis rotation modulation inertial navigation systems or multiple sets of triaxial rotation modulation inertial navigation systems are redundantly configured.

[0080] In summary, the advantages and positive effects of this invention are as follows: This invention achieves online monitoring of the inertial navigation system at the device level by asynchronously rotating two sets of dual-axis rotating inertial navigation systems and utilizing the redundant information of the two inertial navigation systems. The method proposed in this invention is completely autonomous, does not rely on any external reference information, is not limited by the usage environment, and can improve the accuracy of inertial device status monitoring on mobile platforms, which has important engineering practical significance. Attached Figure Description

[0081] Figure 1 This is a flowchart provided in an embodiment of the present invention. Detailed Implementation

[0082] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to embodiments. It should be understood that the specific embodiments described herein are merely illustrative and not intended to limit the invention.

[0083] In high-latitude polar regions, due to the rapid convergence of meridians, navigation schemes using traditional geographic coordinate systems as navigation coordinate systems suffer from significant errors. Furthermore, in the harsh natural and geographical environment of polar regions, inertial navigation systems (INS) lack reliable external reference information, making INS state monitoring methods relying solely on external assistance unsuitable for such environments. Additionally, the traditional Psi angle error model is defined in the true coordinate system, which is unknown. Approximating the true coordinate system with the calculated coordinate system introduces approximation errors. The specific force term in the traditional velocity error equation cannot be directly measured, leading to errors in specific force calculation under a moving base. These factors affect the accuracy of monitoring. To address these issues, this invention proposes a polar dual-INS state monitoring method based on the Psi angle correction model. The monitoring method, as described... Figure 1 As shown. The specific implementation method is as follows:

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

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

[0086] Taking 0°N 90°E as the North Pole in the transverse Earth coordinate system, defined as the transverse North Pole, and 0°N 90°W as the South Pole in the transverse Earth coordinate system, defined as the transverse South Pole, the ellipse enclosed by the 0° meridian and the 180° meridian is the transverse equatorial plane. The semi-great ellipse formed by the transverse North Pole, transverse South Pole, and North Pole is taken as the 0° transverse meridian, and its plane is the transverse Prime Meridian. The transformation relationship between the Earth coordinate system e and the newly defined transverse Earth coordinate system e′ is explained. Represented as:

[0087]

[0088] Based on the horizontal latitude and longitude grid, the horizontal calculation coordinate system is defined, with the horizontal north direction pointing to the horizontal North Pole. The normal at the location points upwards to the celestial direction. Following the right-hand coordinate system definition, the horizontal east direction is defined. The transformation relationship between the horizontal calculation coordinate system c′ and the calculation coordinate system c is explained. Represented as:

[0089]

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

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

[0092]

[0093] The transformation relationship between the horizontal calculation coordinate system and the horizontal platform coordinate system is expressed as:

[0094]

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

[0096] The angle between the normal to the location of the carrier and the transverse equatorial plane is defined as transverse latitude, and the angle with the transverse prime meridian plane is defined as transverse longitude. The conversion relationship between latitude and longitude and transverse latitude and longitude in the Earth coordinate system is expressed as follows:

[0097]

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

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

[0100]

[0101] In the formula, b1 represents the volume coordinate system of inertial navigation system 1, b2 represents the volume coordinate system of inertial navigation system 2, c1′ represents the transverse calculation coordinate system of inertial navigation system 1, c2′ represents the transverse calculation coordinate system of inertial navigation system 2, p1 represents the platform coordinate system of inertial navigation system 1, p2 represents the platform coordinate system of inertial navigation system 2, and ψ1=[ψ E1 ψ N1 ψ U1 ] T ψ represents the drift error angle of inertial navigation system 1 in the horizontal calculation coordinate system. E1 ψ N1 ψ U1 These represent the eastward, northward, and celestial drift error angles of Inertial Navigation System 1 in the horizontal calculation coordinate system, respectively. This represents the velocity error vector of inertial navigation system 1 after error correction in the horizontal calculation coordinate system. These represent the velocity errors of Inertial Navigation System 1 in the east, north, and sky directions, respectively, after error correction in the horizontal calculation coordinate system. This represents the position error of inertial navigation system 1 in the horizontal calculation coordinate system of inertial navigation system 1. This indicates the eastward error of inertial navigation system 1 in the horizontal calculation coordinate system of inertial navigation system 1. This represents the northward error of inertial navigation system 1 in the horizontal calculation coordinate system of inertial navigation system 1. This indicates the azimuth error of inertial navigation system 1 in the horizontal calculation coordinate system of inertial navigation system 1. This indicates the Earth's rotational angular velocity in the inertial navigation system 1 calculation coordinate system. This represents the transfer angular velocity in the horizontal calculation coordinate system of inertial navigation system 1. This represents the direction cosine matrix from the inertial navigation system 1 body coordinate system to the inertial navigation system 1 transverse platform coordinate system. This represents the gravity vector in the horizontal calculation coordinate system of the inertial navigation system 1. This represents the velocity of the carrier output by inertial navigation system 1 in the transverse computational coordinate system, ψ2=[ψ E2 ψ N2 ψ U2 ] T ψ represents the drift error angle of inertial navigation system 2 in the horizontal calculation coordinate system. E2 ψ N2 ψ U2 These represent the drift error angles of inertial navigation system 2 in the east, north, and sky directions, respectively, in the horizontal calculation coordinate system of inertial navigation system 2. This represents the velocity error vector of inertial navigation system 2 after error correction in the horizontal calculation coordinate system. These represent the velocity errors of inertial navigation system 2 in the east, north, and sky directions, respectively, after error correction in the horizontal calculation coordinate system. This represents the position error of inertial navigation system 2 in the horizontal calculation coordinate system of inertial navigation system 2. This indicates the eastward error of inertial navigation system 2 in the horizontal calculation coordinate system of inertial navigation system 2. This represents the northward error of inertial navigation system 2 in the horizontal calculation coordinate system of inertial navigation system 2. This represents the azimuth error of inertial navigation system 2 in the horizontal calculation coordinate system of inertial navigation system 2. This represents the Earth's rotational angular velocity in the inertial navigation system's second horizontal coordinate system. This represents the transfer angular velocity in the inertial navigation system's second horizontal calculation coordinate system. This represents the direction cosine matrix from the inertial navigation system 2 body coordinate system to the inertial navigation system 2 transverse platform coordinate system. This represents the gravity vector in the inertial navigation system's second horizontal coordinate system. This indicates that the output of Inertial Navigation System 2 (INS2) represents the vehicle velocity in the INS2 horizontal calculation coordinate system. The error of the gyroscope component in inertial navigation system 1 is modeled as a constant drift. and gyroscope noise The sum of, among which, This indicates the x-axis gyroscope drift of inertial navigation system 1. This indicates the y-axis gyroscope drift of inertial navigation system 1. This indicates the z-axis gyroscope drift of inertial navigation system 1. The error of the accelerometer component of inertial navigation system 1 is modeled as a constant zero bias. and accelerometer noise The sum of, among which, This indicates that the x-axis accelerometer of inertial navigation system 1 has zero bias. This indicates that the y-axis accelerometer of inertial navigation system 1 has zero bias. This indicates that the z-axis accelerometer of inertial navigation system 1 has zero bias. The error of the gyroscope component in Inertial Navigation System 2 is modeled as a constant drift. and gyroscope noise The sum of, among which, This indicates the x-axis gyroscope drift of inertial navigation system 2. This indicates the y-axis gyroscope drift of inertial navigation system 2. This indicates the z-axis gyroscope drift of inertial navigation system 2. The error of the accelerometer component in inertial navigation system 2 is modeled as a constant zero bias. and accelerometer noise The sum of, among which, This indicates that the x-axis accelerometer of inertial navigation system 2 has zero bias. This indicates that the y-axis accelerometer of inertial navigation system 2 has zero bias. This indicates that the z-axis accelerometer of inertial navigation system 2 has zero bias;

[0102] (3.2) Determine the joint state equations:

[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] In the formula, 0 i×j Represents the zero matrix in row i and column j;

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

[0110] Define the body coordinate system as b when the inertial navigation system 1 and inertial navigation system 2 are in the zero position. 10 System and b 20 The coordinate system is b-frame, and the attitude matrices are output from two sets of dual-axis rotating inertial navigation systems. and Represented as:

[0111]

[0112]

[0113] In the formula, I 3×3 This represents a 3x3 identity matrix. The direction cosine matrix from the carrier coordinate system to the inertial navigation system 1 horizontal calculation coordinate system is given. For b1 series to b 10 The direction cosine matrix of the system, For b 10 The direction cosine matrix from system b to system b The direction cosine matrix from the carrier coordinate system to the inertial navigation system's second transverse coordinate system is calculated. Let c′2 be the direction cosine matrix from c1′2 to c′2. Let c1′ be the direction cosine matrix from c′2 to c′2. For b 20 The direction cosine matrix from system b to system b For b2 series to b 20 The direction cosine matrix of the system;

[0114] The expression for the difference in attitude error between the two sets of dual-axis rotating inertial navigation systems is determined as follows:

[0115]

[0116] Considering the lever arm, the velocity and position outputs of inertial navigation systems 1 and 2 are expressed as follows:

[0117]

[0118] In the formula, and These represent the actual velocities of the carrier in the c1′ and c2′ systems, respectively. and This represents the position information output by inertial navigation system 1 and inertial navigation system 2. and These represent the actual positions of the vector in the c1′ and c2′ systems, respectively. This represents the velocity difference between inertial navigation system 2 and inertial navigation system 1 caused by the outer arm between the two inertial navigation systems. This represents the position difference of inertial navigation system 2 relative to inertial navigation system 1 caused by the outer arm between the two inertial navigation systems.

[0119] Therefore, the difference between the velocity and position vectors of the two inertial navigation systems is expressed as:

[0120]

[0121] The observation equation is expressed as:

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

[0123] in,

[0124]

[0125] In the formula, z(t) represents the system's observation vector, H(t) represents the system's observation matrix, υ(t) is the noise vector of the corresponding observation, the subscript (1:2) indicates the first two elements of the corresponding vector, and H1 indicates... The first two rows of the skew-symmetric matrix, H2 represents The first two rows of the skew-symmetric matrix, H3 represents The first two rows of the matrix, I 2×2 Represents a 2x2 identity matrix;

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

[0127] A strong tracking filter based on residual normalization is used to track and estimate the error state. The one-step prediction of the filter covariance matrix is ​​expressed as:

[0128]

[0129] In the formula,

[0130]

[0131] And there are

[0132]

[0133] Where, λ k P is the fading factor. k / k-1To predict the covariance matrix in one step, Φ k / k-1 P represents the state transition matrix in one step. k-1 Let G be the covariance matrix at time k-1. k-1 The process noise assignment matrix at time k-1, Q k-1 Let H be the system noise matrix at time k-1, tr(·) be the matrix trace operator, and H be the system noise matrix at time k-1. k Let R be the system observation matrix at time k. k Let l be the observation noise matrix at time k. k λ is a weakening factor. 0,k The fading factor is calculated at time k. Let k represent the residual covariance matrix at time k. Let represent the residual covariance matrix at time k-1, ρ be the forgetting factor (taken as 0.95 ≤ ρ ≤ 0.995), γ0 represent the innovation at time 0, and γ k Represents the information at time k, and η is the normalization parameter used to eliminate the problem of reduced response speed of error state estimation caused by information asymmetry due to differences in the residual values ​​themselves.

[0134] (5) Real-time status monitoring is performed based on the error parameters output by the filter;

[0135] When the inertial device is in an abnormal state, the corresponding gyroscope drift or accelerometer zero bias changes. Real-time health status monitoring of the inertial device is achieved by analyzing the output of the monitoring filter.

[0136] A data window is set to slide over time, with a length of N. At time k, the filter output information contained in the sliding window is... The statistical characteristics of the calculated data are as follows:

[0137]

[0138] In the formula, μ k and Let represent the mean and variance of the real-time estimated parameters of the filter within the sliding window at time k, respectively, and their dimensions are and . Same dimension;

[0139] Set a weighting coefficient α and iteratively calculate the mean of the historical sliding windows:

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

[0141] In the formula, Σ k The sliding window mean after iterations at time k is used to determine the two thresholds as follows:

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

[0143] T + =k2Σ k

[0144] In the formula, k1 and k2 are the parameters to be adjusted, and k1≥1 and k2>1, T + For a high threshold, T - Low threshold;

[0145] The guidelines for establishing health status monitoring are as follows:

[0146]

[0147] In the formula, T represents the i-th component of the filter's estimated output at time k+1. + (i) and T - (i) represents the i-th component of the desired real-time high threshold and low threshold, respectively. The monitoring threshold of each inertial device is updated in real time according to the sliding window. When all error parameter values ​​estimated by the filter are less than the low threshold, the output system is fault-free. When the i-th error parameter value estimated by the filter is greater than the low threshold but less than the high threshold, a fault warning is issued. When the i-th error parameter value estimated by the filter is greater than the high threshold, the output device i is faulty.

[0148] Based on the joint rotation method given in step (1), inertial navigation system 1 and inertial navigation system 2 are put into normal navigation state. The dual inertial navigation system state space model under polar region is constructed through steps (2), (3) and (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 system 1 and inertial navigation system 2 rotate at different times according to the same rotation sequence, that is, the two inertial navigation systems rotate asynchronously according to the same rotation scheme.

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

[0151] As an improvement, in step (3) and The relative attitude between the inertial navigation system 1 and the inertial navigation system 2 and the carrier coordinate system is obtained by calibrating the rotation mechanism when it is in the zero position.

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

[0153] As an improvement, in step (3) The position is determined by the outputs of inertial navigation system 1 and inertial navigation system 2.

[0154] As an improvement, the method of the present invention is not only applicable to the case where both inertial navigation system 1 and inertial navigation system 2 are dual-axis rotation modulation inertial navigation systems, but also applicable to the case where both inertial navigation system 1 and inertial navigation system 2 are triaxial rotation modulation inertial navigation systems, where inertial navigation system 1 is a dual-axis rotation modulation inertial navigation system or a triaxial rotation modulation inertial navigation system and inertial navigation system 2 is a single-axis rotation modulation inertial navigation system, where inertial navigation system 1 is a single-axis rotation modulation inertial navigation system and inertial navigation system 2 is a dual-axis rotation modulation inertial navigation system or a triaxial rotation modulation inertial navigation system, and where multiple sets of dual-axis rotation inertial navigation systems or multiple sets of triaxial rotation modulation inertial navigation systems are redundantly configured.

[0155] The above description is merely a preferred embodiment of the present invention and is not intended to limit the present invention. All technical solutions falling within the scope of the present invention's concept are protected by the present invention. Any improvements and modifications made without departing from the principles of the present invention should also be considered within the scope of protection of the present invention.

Claims

1. A polar double inertial navigation state monitoring method based on a Psi angle correction model, characterized in that, The method includes the following steps: (1) Set the rotation order of two sets of dual-axis rotating inertial navigation systems. Define two redundant inertial navigation systems as Inertial Navigation 1 and Inertial Navigation 2. Both of them have the dual-axis 16-order rotation and adopt different rotation methods. (2) Construct a horizontal Earth coordinate system and a horizontal computational coordinate system based on the Earth ellipsoid model; Taking 0°N 90°E as the North Pole in the transverse Earth coordinate system, defined as the transverse North Pole, and 0°N 90°W as the South Pole in the transverse Earth coordinate system, defined as the transverse South Pole, the ellipse enclosed by the 0° meridian and the 180° meridian is the transverse equatorial plane. Taking the semi-great ellipse formed by the transverse North Pole, transverse South Pole, and North Pole as the 0° transverse meridian, the plane containing this meridian is the transverse Prime Meridian. The Earth coordinate system... The system and the newly defined transverse Earth coordinate system Conversion relationships between systems Represented as: ， Based on the horizontal latitude and longitude grid, the horizontal calculation coordinate system is defined, with the horizontal north direction pointing to the horizontal North Pole. The normal at the location points upwards to the celestial direction. Following the right-hand coordinate system definition, the horizontal east direction is defined. Coordinate system and computational coordinate system Conversion relationships between systems Represented as: ， In the formula, This represents the rotation angle between the computational coordinate system and the horizontal computational coordinate system; Sure With longitude ,latitude Longitude Latitude and longitude The conversion relationship between them: ， ， The transformation relationship between the horizontal calculation coordinate system and the horizontal platform coordinate system is expressed as: ， In the formula, The system represents the horizontal platform coordinate system. Represents the identity matrix. The antisymmetric matrix representing the drift error angle in the horizontal computational coordinate system; The angle between the normal to the location of the carrier and the transverse equatorial plane is defined as transverse latitude, and the angle with the transverse prime meridian plane is defined as transverse longitude. The conversion relationship between latitude and longitude and transverse latitude and longitude in the Earth coordinate system is expressed as follows: ， ， (3) Using the attitude, velocity, and position information output by the two inertial navigation systems, a joint state Kalman filter is constructed. The specific steps are as follows: (3.1) Determine the joint error equation of the system: ， ， ， ， ， ， In the formula, The volume coordinate system representing inertial navigation system 1, The volume coordinate system representing inertial navigation system 2, The horizontal calculation coordinate system represents inertial navigation system 1. The horizontal calculation coordinate system represents inertial navigation system 2. Indicates the coordinate system of the inertial navigation system 1 platform. Indicates the coordinate system of the inertial navigation system 2 platform. This represents the drift error angle of inertial navigation system 1 in the horizontal calculation coordinate system. , , These represent the eastward, northward, and celestial drift error angles of Inertial Navigation System 1 in the horizontal calculation coordinate system, respectively. This represents the velocity error vector of inertial navigation system 1 after error correction in the horizontal calculation coordinate system. , , These represent the velocity errors of Inertial Navigation System 1 in the east, north, and sky directions, respectively, after error correction in the horizontal calculation coordinate system. This represents the position error of inertial navigation system 1 in the horizontal calculation coordinate system of inertial navigation system 1. This indicates the eastward error of inertial navigation system 1 in the horizontal calculation coordinate system of inertial navigation system 1. This represents the northward error of inertial navigation system 1 in the horizontal calculation coordinate system of inertial navigation system 1. This indicates the azimuth error of inertial navigation system 1 in the horizontal calculation coordinate system of inertial navigation system 1. This indicates the Earth's rotational angular velocity in the inertial navigation system 1 calculation coordinate system. This represents the transfer angular velocity in the horizontal calculation coordinate system of inertial navigation system 1. This represents the direction cosine matrix from the inertial navigation system 1 body coordinate system to the inertial navigation system 1 transverse platform coordinate system. This represents the gravity vector in the horizontal calculation coordinate system of the inertial navigation system 1. This indicates that the inertial navigation system 1 outputs the carrier velocity in the transverse computational coordinate system. This represents the drift error angle of inertial navigation system 2 in the horizontal calculation coordinate system. , , These represent the drift error angles of inertial navigation system 2 in the east, north, and sky directions, respectively, in the horizontal calculation coordinate system of inertial navigation system 2. This represents the velocity error vector of inertial navigation system 2 after error correction in the horizontal calculation coordinate system. , , These represent the velocity errors of inertial navigation system 2 in the east, north, and sky directions, respectively, after error correction in the horizontal calculation coordinate system. This represents the position error of inertial navigation system 2 in the horizontal calculation coordinate system of inertial navigation system 2. This indicates the eastward error of inertial navigation system 2 in the horizontal calculation coordinate system of inertial navigation system 2. This represents the northward error of inertial navigation system 2 in the horizontal calculation coordinate system of inertial navigation system 2. This represents the azimuth error of inertial navigation system 2 in the horizontal calculation coordinate system of inertial navigation system 2. This represents the Earth's rotational angular velocity in the inertial navigation system's second horizontal coordinate system. This represents the transfer angular velocity in the inertial navigation system's second horizontal calculation coordinate system. This represents the direction cosine matrix from the inertial navigation system 2 body coordinate system to the inertial navigation system 2 transverse platform coordinate system. This represents the gravity vector in the inertial navigation system's second horizontal coordinate system. This indicates that the output of Inertial Navigation System 2 (INS2) represents the vehicle velocity in the INS2 horizontal calculation coordinate system. The error of the gyroscope component in inertial navigation system 1 is modeled as a constant drift. and gyroscope noise The sum of, among which, This indicates the x-axis gyroscope drift of inertial navigation system 1. This indicates the y-axis gyroscope drift of inertial navigation system 1. This indicates the z-axis gyroscope drift of inertial navigation system 1. The error of the accelerometer component of inertial navigation system 1 is modeled as a constant zero bias. and accelerometer noise The sum of, among which, This indicates that the x-axis accelerometer of inertial navigation system 1 has zero bias. This indicates that the y-axis accelerometer of inertial navigation system 1 has zero bias. This indicates that the z-axis accelerometer of inertial navigation system 1 has zero bias. The error of the gyroscope component in Inertial Navigation System 2 is modeled as a constant drift. and gyroscope noise The sum of, among which, This indicates the x-axis gyroscope drift of inertial navigation system 2. This indicates the y-axis gyroscope drift of inertial navigation system 2. This indicates the z-axis gyroscope drift of inertial navigation system 2. The error of the accelerometer component in inertial navigation system 2 is modeled as a constant zero bias. and accelerometer noise The sum of, among which, This indicates that the x-axis accelerometer of inertial navigation system 2 has zero bias. This indicates that the y-axis accelerometer of inertial navigation system 2 has zero bias. This indicates that the z-axis accelerometer of inertial navigation system 2 has zero bias; (3.2) Determine the joint state equations: ， The state transition matrix can be derived from the error equation, and the state vector is... Represented as: ， noise distribution matrix and noise matrix Represented as: ， ， In the formula, Represents the zero matrix in row i and column j; (3.3) Determine the state constraint observation equations: Define the body coordinate system when the inertial navigation system 1 and inertial navigation system 2 are in the zero position as follows: System and The coordinate system is b-frame, and the attitude matrices are output from two sets of dual-axis rotating inertial navigation systems. and Represented as: ， ， In the formula, This represents a 3x3 identity matrix. The direction cosine matrix from the carrier coordinate system to the inertial navigation system 1 horizontal calculation coordinate system is given. for ties The direction cosine matrix of the system, for ties The direction cosine matrix of the system, The direction cosine matrix from the carrier coordinate system to the inertial navigation system's second transverse coordinate system is calculated. for ties The direction cosine matrix of the system, for ties The direction cosine matrix of the system, for ties The direction cosine matrix of the system, for ties The direction cosine matrix of the system; The expression for the difference in attitude error between the two sets of dual-axis rotating inertial navigation systems is determined as follows: ， Considering the lever arm, the velocity and position outputs of inertial navigation systems 1 and 2 are expressed as follows: ， ， In the formula, and They respectively represent the carrier in System and The actual speed under the system, and This represents the position information output by inertial navigation system 1 and inertial navigation system 2. and They respectively represent the carrier in System and The actual location of the system This represents the velocity difference between inertial navigation system 2 and inertial navigation system 1 caused by the outer arm between the two inertial navigation systems. This represents the position difference of inertial navigation system 2 relative to inertial navigation system 1 caused by the outer arm between the two inertial navigation systems. Therefore, the difference between the velocity and position vectors of the two inertial navigation systems is expressed as: ， ， The observation equation is expressed as: ， in, ， ， In the formula, Represents the observation vector of the system. Represents the system's observation matrix. For the noise vector corresponding to the observation, the subscript (1:2) indicates the first two elements of the corresponding vector. express The first two rows of the skew-symmetric matrix, express The first two rows of the skew-symmetric matrix, express The first two rows of the matrix, Represents a 2x2 identity matrix; (4) Establish an adaptive error parameter estimation filter; A strong tracking filter based on residual normalization is used to track and estimate the error state. The one-step prediction of the filter covariance matrix is ​​expressed as: ， In the formula, ， And there are ， ， ， ， in, As a gradually diminishing factor, To predict the covariance matrix in one step, This represents the state transition matrix in one step. Let be the covariance matrix at time k-1. Assign a matrix to the process noise at time k-1. Let k be the system noise matrix at time k-1. For the trace operator of a matrix, Let k be the system observation matrix at time k. Let k be the observation noise matrix at time k. As a weakening factor, The fading factor is calculated at time k. Let k represent the residual covariance matrix at time k. Let the residual covariance matrix at time k-1 be represented. Forgetting factor, take , This indicates the information at time 0. This represents the information at time k. The normalization parameter is used to eliminate the problem of reduced response speed of error state estimation caused by information asymmetry due to differences in the residual values ​​themselves. (5) Real-time status monitoring is performed based on the error parameters of the filter output; When the inertial device is in an abnormal state, the corresponding gyroscope drift or accelerometer zero bias changes. Real-time health status monitoring of the inertial device is achieved by analyzing the output of the monitoring filter. A data window is set to slide over time, with a length of N. At time k, the filter output information contained in the sliding window is... The statistical characteristics of the calculated data are as follows: ， ， In the formula, and Let represent the mean and variance of the real-time estimated parameters of the filter within the sliding window at time k, respectively, and their dimensions are and . Same dimension; Set weighting coefficients The mean of the historical sliding windows is calculated iteratively: ， In the formula, The sliding window mean after iterations at time k is used to determine the two thresholds as follows: ， ， In the formula, , The parameter to be adjusted is... , , For a high threshold, Low threshold; The guidelines for establishing health status monitoring are as follows: ， In the formula, express The filter estimates the output at time 1. One portion, and Represent the first and second halves of the real-time high and low thresholds, respectively. The system updates the monitoring thresholds of each inertial device in real time according to the sliding window. When all error parameter values ​​estimated by the filter are less than the low threshold, the output system is fault-free; when the filter estimates the first component, the second component is fault-free. When the value of the first error parameter is greater than the low threshold but less than the high threshold, a fault warning is issued; when the filter estimates the first error parameter... If any error parameter value is greater than the high threshold, the output device... A malfunction has occurred.

2. The polar dual inertial navigation system state monitoring method based on the Psi angle correction model as described in claim 1, characterized in that, In step (1), inertial navigation system 1 and inertial navigation system 2 rotate at different times according to the same rotation sequence, that is, the two inertial navigation systems rotate asynchronously according to the same rotation scheme.

3. The polar dual inertial navigation system state monitoring method based on the Psi angle correction model as described in claim 1, characterized in that, In step (1), inertial navigation system 1 and inertial navigation system 2 rotate in different rotation sequences and rotate synchronously.

4. The polar dual inertial navigation system state monitoring method based on the Psi angle correction model as described in claim 1, characterized in that, In step (3) and The relative attitude between the inertial navigation system 1 and the inertial navigation system 2 and the carrier coordinate system is obtained by calibrating the rotation mechanism when it is in the zero position.

5. The polar dual inertial navigation system state monitoring method based on the Psi angle correction model as described in claim 1, characterized in that, In step (3), the lever arm between inertial navigation system 1 and inertial navigation system 2 is calibrated and determined after the two sets of inertial navigation systems are installed.

6. The polar dual inertial navigation system state monitoring method based on the Psi angle correction model as described in claim 1, characterized in that, In step (3) The position is determined by the outputs of inertial navigation system 1 and inertial navigation system 2.

Images