Heterogeneous dual-redundancy strapdown inertial navigation reconstruction method and device, program and storage medium

Abstract

The invention discloses a heterogeneous dual-redundancy strapdown inertial navigation reconstruction method and device, a program and a storage medium, and belongs to the technical field of inertial navigation. According to the method, two sets of inertial systems with different precisions are adopted to form a coaxial common-base redundant architecture, fault detection is carried out through a method based on combination of optimal odd-even vectors and t inspection, and meanwhile, a fault positioning algorithm based on least square is adopted to accurately identify a fault sensor and the specific axial direction of the fault sensor. And after detecting that a fault occurs in a certain axial direction of the high-precision inertial system, the system fuses the converted low-precision inertial data with the residual normal high-precision data, so that the reliability of the system is ensured, and the navigation precision is maintained to the greatest extent by using redundant data.

Description

Technical Field

[0001] This invention relates to the field of inertial navigation technology, and in particular to a method, apparatus, program, and storage medium for reconstructing a heterogeneous dual-redundant strapdown inertial navigation system. Background Technology

[0002] Strapdown inertial navigation systems (SINS), as a core component of modern navigation technology, play an irreplaceable role in key fields such as aerospace, autonomous driving, and industrial precision control due to their unique advantages of complete autonomy, independence from external signals, strong anti-interference capabilities, and all-weather operation. Through the close integration of high-precision inertial sensors (such as gyroscopes and accelerometers) and navigation algorithms, this system can achieve precise positioning and attitude determination under complete autonomy, thus becoming the most reliable source of fundamental information in navigation systems.

[0003] However, in practical applications, SINS systems face several technical challenges: inertial sensors can develop drift errors on certain axes during long-term operation, and these errors accumulate over time, severely affecting navigation accuracy; environmental factors such as mechanical vibration and external shocks can lead to sensor performance degradation or even structural damage; and harsh operating conditions such as extreme temperature changes and electromagnetic interference can significantly affect system reliability. These factors can all cause failures on certain axes of the system, thereby affecting the stability and safety of the entire navigation system.

[0004] To improve the reliability of inertial navigation systems (INS), redundancy design techniques are currently the primary approach, which can be categorized into device-level redundancy and system-level redundancy. Device-level redundancy involves configuring multiple inertial measurement units (IMUs) within the system and employing voting algorithms (such as majority voting) or fault detection and isolation techniques to eliminate faulty sensors. While this method effectively enhances system fault tolerance, it suffers from complex assembly processes and high costs. System-level redundancy, on the other hand, uses two or more independent INS systems in a primary / backup architecture. When the primary system fails, the backup system takes over. This approach is relatively simple to implement and utilizes heterogeneous dual-redundancy systems—two systems with different levels of accuracy—which can improve system stability while reducing costs.

[0005] In terms of fault detection, existing technologies mostly employ methods based on the principle of equivalent space. However, this method has significant limitations in coaxial dual-inertial systems: on the one hand, it cannot accurately locate the specific sensor that has malfunctioned; on the other hand, its detection performance is heavily dependent on the initial noise parameter settings, and in actual operation, environmental changes or carrier movement can alter the noise characteristics, easily leading to false alarms or missed detections.

[0006] After fault detection, system reconfiguration is required. Existing fault reconfiguration methods for heterogeneous redundant inertial navigation systems often focus only on system reliability while neglecting the maintenance of navigation accuracy. When a fault is detected in the high-precision primary inertial system, it directly switches to the low-precision backup unit. While this approach ensures continuous system operation when only one axis of the high-precision inertial navigation system fails, the error accumulates rapidly when the low-precision inertial system operates independently, leading to a decrease in navigation accuracy. Furthermore, it fails to fully utilize the value of redundant data, resulting in a waste of system resources.

[0007] To address the issue of navigation accuracy degradation caused by a fault on a certain axis of the high-precision inertial navigation system when two inertial navigation systems with different accuracies are used, a reconstruction method, device, program, and storage medium for heterogeneous dual-redundant strapdown inertial navigation systems are proposed to improve system reliability and accuracy. Summary of the Invention

[0008] The purpose of this invention is to propose a reconstruction method, device, program and storage medium for heterogeneous dual-redundant strapdown inertial navigation systems, so as to overcome the shortcomings of the prior art and maintain navigation accuracy to the maximum extent by utilizing redundant data while ensuring system reliability.

[0009] This invention proposes a reconstruction method, device, program, and storage medium for heterogeneous dual-redundant strapdown inertial navigation systems, the core of which includes the following technical solutions:

[0010] A reconstruction method for heterogeneous dual-redundant strapdown inertial navigation systems includes the following steps:

[0011] Step 1: Place the high-precision inertial navigation system and the low-precision inertial navigation system in a coaxial, common-base manner, and initialize the observation matrix and decoupling matrix to obtain the real-time measurement values ​​of the sensors, including the raw data of the high-precision inertial navigation system and the raw data of the low-precision inertial navigation system.

[0012] Step 2: Based on the real-time sensor measurements and the decoupling matrix, perform residual calculation to obtain the residual vector.

[0013] Step 3: Based on the residual vector, perform fault detection. If a fault occurs, generate the fault detection result and fault axis identifier, and proceed to the next step; otherwise, directly use the high-precision inertial navigation raw data as the data to be solved and proceed to step 6.

[0014] Step 4: Based on the fault detection results and fault axis identification, combined with sensor data from near-fault-free times, perform fault location and determine whether the fault point is a high-precision inertial navigation system. If so, generate a fault sensor identification and proceed to the next step; otherwise, directly use the original high-precision inertial navigation system data as the data to be solved and proceed to step 6.

[0015] Step 5: Based on the fault sensor identifier and combined with the real-time data from the low-precision inertial navigation system, perform system reconstruction to obtain the reconstructed sensor data.

[0016] Step 6: Perform inertial navigation calculations, obtain navigation results, and output them.

[0017] Further, the residual vector described in step 2 The specific calculation methods include:

[0018]

[0019] in, For decoupling matrix, The sensor measures real-time values. The sequence is a zero-mean Gaussian white noise sequence. This is the fault vector.

[0020] Furthermore, the fault detection method described in step 3 specifically includes:

[0021] For each sensor Calculate the detection function ;

[0022]

[0023] in, For sensors The residual, This represents the noise sensitivity weight.

[0024] From residuals Extraction Given a sample of 1, set the sample mean and variance, and construct the final test statistic. ;

[0025]

[0026] in, The residual sample mean. The residual sample variance For degrees of freedom of distributed.

[0027] Judgment detection function and final test statistic If none of the above values ​​exceed the first threshold, then the system is considered fault-free; otherwise, only the detection function is present. If the first threshold is exceeded, switch. Examine the sample window up to the post-fault data, reduce the sample size, and recalculate the detection function. If there is still no fault, it is determined to be fault-free; if the detection function... and final test statistic If all values ​​exceed the first threshold, it is considered a fault, the fault axis identifier is output, and step 4 is executed; if only the final test statistic is considered... If the first threshold is exceeded, it is determined to be a fault, the fault axis identifier is output and step 4 is executed.

[0028] Furthermore, the fault location described in step 4 specifically includes:

[0029] Calculate the predicted value for the current time based on sensor data from near the fault-free point. ;

[0030]

[0031] in, For the observation matrix, This is the weight matrix. For sensor data near the fault-free point, This is for transpose calculation.

[0032] Calculate the residual between the predicted value and the measured value at the current time. .

[0033] Calculate fault statistics .

[0034] Based on the relationship between the fault statistics of the high-precision inertial navigation system and the fault statistics of the low-precision inertial navigation system and the second threshold, fault location is performed; if the fault statistics of the high-precision inertial navigation system exceed the second threshold and the fault statistics of the low-precision inertial navigation system do not exceed the second threshold, the fault is located as a high-precision inertial navigation system fault, the fault sensor identifier is output, and step 5 is executed; otherwise, it is judged as a false alarm, the high-precision inertial navigation system data is used directly, and step 6 is executed.

[0035] Furthermore, the system reconstruction described in step 5 specifically includes:

[0036] The low-precision inertial navigation real-time data is converted to a high-precision inertial navigation coordinate system to obtain the converted sensor data.

[0037]

[0038]

[0039]

[0040]

[0041] in, The converted angular velocity, The converted ratio, This is the coordinate transformation matrix. For low-precision inertial navigation angular velocity, For low-precision inertial navigation acceleration, To compensate for acceleration due to lever arm effect, Let the lever arm vector be... Angular acceleration, This is the converted acceleration.

[0042] Furthermore, step 6, the inertial navigation calculation, specifically includes:

[0043] Calculate the attitude differential equation;

[0044]

[0045] in, The transformation matrix from the carrier system to the navigation system. This refers to the angular velocity output by the gyroscope. Let ω be the angular velocity of the navigation frame relative to the inertial frame.

[0046] Discretize the attitude differential equation to obtain Euler angles and complete the attitude update.

[0047] Calculate the velocity differential equation;

[0048]

[0049] in, The projection of the vehicle's velocity onto the navigation system. For accelerometer specific force, This is the acceleration due to gravity.

[0050] Discretize the velocity differential equation, recursively calculate the velocity increment, and complete the velocity update.

[0051] Calculate the position differential equation;

[0052]

[0053] in, These represent the latitude, longitude, and altitude vectors of the carrier in the geographic coordinate system. These are the radii of curvature along the Earth's meridian and the radii of curvature along the Earth's circumpolar region, respectively. These represent the speeds in the north, east, and sky directions, respectively.

[0054] Discretize the position differential equation to complete the position update.

[0055] A computer device includes a memory, a processor, and a computer program stored in the memory, wherein the processor executes the computer program to implement the steps of the method described above.

[0056] A computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the steps of the above-described method.

[0057] A computer program product includes computer instructions that, when executed by a processor, implement the steps of the method described above.

[0058] The beneficial effects of this invention are as follows: This invention employs two inertial systems with different accuracies to form a coaxial, shared-base redundant architecture. Fault detection is performed using a method combining optimal parity vectors and t-tests. Simultaneously, a least-squares-based fault location algorithm is used to accurately identify the faulty sensor and its specific axis. Upon detecting a fault in a certain axis of the high-precision inertial system, the system fuses the converted low-precision inertial data with the remaining normal high-precision data. This ensures system reliability while maximizing navigation accuracy using redundant data. Attached Figure Description

[0059] Figure 1 This is a schematic diagram of the present invention.

[0060] Figure 2 This is a flowchart of the present invention.

[0061] Figure 3 This is a schematic diagram of the placement of a heterogeneous dual-redundant inertial navigation system.

[0062] Figure 4 This is a fault detection diagram using the traditional optimal parity vector method.

[0063] Figure 5 This is a fault detection diagram based on the t-test.

[0064] Figure 6 A fault detection map that integrates the optimal parity vector and the t-test.

[0065] Figure 7 A comparison chart of position errors between low-precision inertial navigation compensation reconstruction and low-precision inertial navigation. Detailed Implementation

[0066] refer to Figure 1 and Figure 2 A reconstruction method for heterogeneous dual-redundant strapdown inertial navigation systems:

[0067] Step 1: Place the high-precision inertial navigation system and the low-precision inertial navigation system in a coaxial, common-base manner, and initialize the observation matrix and decoupling matrix to obtain the real-time measurement values ​​of the sensors, including the raw data of the high-precision inertial navigation system and the raw data of the low-precision inertial navigation system.

[0068] Step 2: Based on the real-time sensor measurements and the decoupling matrix, perform residual calculation to obtain the residual vector.

[0069] The residual vector The specific calculation methods include:

[0070]

[0071] in, For decoupling matrix, The sensor measures real-time values. The sequence is a zero-mean Gaussian white noise sequence. This is the fault vector.

[0072] Step 3: Based on the residual vector, perform fault detection. If a fault occurs, generate the fault detection result and fault axis identifier, and proceed to the next step; otherwise, directly use the high-precision inertial navigation raw data as the data to be solved and proceed to step 6.

[0073] The fault detection method specifically includes:

[0074] For each sensor Calculate the detection function ;

[0075]

[0076] in, For sensors The residual, This represents the noise sensitivity weight.

[0077] From residuals Extraction Given a sample of 1, set the sample mean and variance, and construct the final test statistic. ;

[0078]

[0079] in, The residual sample mean. The residual sample variance For degrees of freedom of distributed.

[0080] Judgment detection function and final test statistic If none of the above values ​​exceed the first threshold, then the system is considered fault-free; otherwise, only the detection function is present. If the first threshold is exceeded, switch. Examine the sample window up to the post-fault data, reduce the sample size, and recalculate the detection function. If there is still no fault, it is determined to be fault-free; if the detection function... and final test statistic If all values ​​exceed the first threshold, it is considered a fault, the fault axis identifier is output, and step 4 is executed; if only the final test statistic is considered... If the first threshold is exceeded, it is determined to be a fault, the fault axis identifier is output and step 4 is executed.

[0081] Step 4: Based on the fault detection results and fault axis identification, combined with sensor data from near-fault-free times, perform fault location and determine whether the fault point is a high-precision inertial navigation system. If so, generate a fault sensor identification and proceed to the next step; otherwise, directly use the original high-precision inertial navigation system data as the data to be solved and proceed to step 6.

[0082] The fault location specifically includes:

[0083] Calculate the predicted value for the current time based on sensor data from near the fault-free point. ;

[0084]

[0085] in, For the observation matrix, This is the weight matrix. For sensor data near the fault-free point, This is for transpose calculation.

[0086] Calculate the residual between the predicted value and the measured value at the current time. .

[0087] Calculate fault statistics .

[0088] Based on the relationship between the fault statistics of the high-precision inertial navigation system and the fault statistics of the low-precision inertial navigation system and the second threshold, fault location is performed; if the fault statistics of the high-precision inertial navigation system exceed the second threshold and the fault statistics of the low-precision inertial navigation system do not exceed the second threshold, the fault is located as a high-precision inertial navigation system fault, the fault sensor identifier is output, and step 5 is executed; otherwise, it is judged as a false alarm, the high-precision inertial navigation system data is used directly, and step 6 is executed.

[0089] Step 5: Based on the fault sensor identifier and combined with the real-time data from the low-precision inertial navigation system, perform system reconstruction to obtain the reconstructed sensor data.

[0090] The system reconstruction specifically includes:

[0091] The low-precision inertial navigation real-time data is converted to a high-precision inertial navigation coordinate system to obtain the converted sensor data.

[0092]

[0093]

[0094]

[0095]

[0096] in, The converted angular velocity, The converted ratio, This is the coordinate transformation matrix. For low-precision inertial navigation angular velocity, For low-precision inertial navigation acceleration, To compensate for acceleration due to lever arm effect, Let the lever arm vector be... Angular acceleration, This is the converted acceleration.

[0097] Step 6: Perform inertial navigation calculations, obtain navigation results, and output them.

[0098] The inertial navigation calculation specifically includes:

[0099] Calculate the attitude differential equation;

[0100]

[0101] in, The transformation matrix from the carrier system to the navigation system. This refers to the angular velocity output by the gyroscope. Let ω be the angular velocity of the navigation frame relative to the inertial frame.

[0102] Discretize the attitude differential equation to obtain Euler angles and complete the attitude update.

[0103] Calculate the velocity differential equation;

[0104]

[0105] in, The projection of the vehicle's velocity onto the navigation system. For accelerometer specific force, This is the acceleration due to gravity.

[0106] Discretize the velocity differential equation, recursively calculate the velocity increment, and complete the velocity update.

[0107] Calculate the position differential equation;

[0108]

[0109] in, These represent the latitude, longitude, and altitude vectors of the carrier in the geographic coordinate system. These are the radii of curvature along the Earth's meridian and the radii of curvature along the Earth's circumpolar region, respectively. These represent the speeds in the north, east, and sky directions, respectively.

[0110] Discretize the position differential equation to complete the position update.

[0111] Example 1

[0112] The two inertial navigation systems are placed in a coaxial, common-base configuration, as follows: Figure 3 As shown.

[0113] Fault detection is performed using a fault detection function that combines optimal parity vectors and t-tests to determine whether the inertial navigation system has malfunctioned.

[0114] Construct a decoupling matrix based on the sensor mounting matrix and calculate the odd and even residuals.

[0115] The equivalent space method constructs a decoupling matrix based on the installation matrix of redundant inertial navigation system sensors, separates the motion state of the carrier from the fault, obtains residuals that are independent of the system state, and constructs detection statistics based on the residuals to determine whether the system has a fault.

[0116] Assume the system has There are 1 sensor, and the output vector is... The true three-dimensional physical quantity is (e.g., acceleration or angular velocity), then:

[0117]

[0118]

[0119] in, The observation matrix is ​​determined by the sensor mounting method. This is the fault vector, which is 0 when there is no fault. The noise is measured and follows a zero-mean Gaussian distribution.

[0120] To improve the system's ability to detect sensor faults, a residual signal that is insensitive to normal conditions but sensitive to faults needs to be constructed. Therefore, a decoupling matrix is ​​introduced. And construct the following residual vector:

[0121]

[0122] Among them, the decoupling matrix The following two conditions must be met:

[0123] (1) This ensures that, under fault-free conditions, the residual does not depend on the actual physical quantity. ;

[0124] (2) This ensures that the residuals have a certain degree of orthogonality and numerical stability.

[0125] In this structure, residual Mainly composed of fault items and noise terms Composition. Through analysis of Statistical analysis allows for real-time monitoring of the system status.

[0126] To ensure the effectiveness of the decoupling matrix in fault detection within an asymmetric redundant inertial navigation system, a method based on singular value decomposition (SVD) is used to construct the decoupling matrix. Specifically, the sensor mounting matrix... Perform singular value decomposition and construct a decoupling matrix based on its singular value vectors. .

[0127] Fault detection here is based on the optimal parity vector method. The constraints of the optimal parity vector are:

[0128]

[0129] For the first The optimal parity vector for each sensor. For decoupling matrix, Decoupling matrix The coordinates of each linear combination in the row, and They are respectively identity matrix of order The The and the first column vectors, and The first Optimal decoupling vector for an inertial sensor For the The and the first Sensitivity to inertial sensor failure, This indicates sensitivity to noise.

[0130] Based on the constraints, the optimal decoupling vectors for each inertial sensor can be obtained as follows:

[0131]

[0132] Based on the optimal decoupling vector, the parity residuals of each sensor can be obtained as follows:

[0133]

[0134] in, For the first Parity residuals of an inertial sensor.

[0135] Using the odd-even residual vector, fault detection functions based on the optimal odd-even vector method and the t-test are constructed respectively.

[0136] The statistical characteristics of the residual vector of the i-th sensor under fault-free and fault-prone conditions are as follows:

[0137]

[0138]

[0139] in, Let Variance be the noise variance. , For the optimal parity vector elements, For matrix diagonal elements, This is the fault value.

[0140] After standardizing the residual vector, it is selected as the fault detection function for the optimal parity vector method:

[0141]

[0142] However, this method cannot adapt to changes in different environments. In actual operation, environmental changes or vehicle movement can alter noise characteristics, easily leading to false alarms or missed detections. Therefore, a fault detection function is constructed based on the t-test to eliminate the influence of noise.

[0143] When the sensor is fault-free, in the residual vector Take l samples consecutively from the sample, and let the mean of the residual samples be... The sample variance is Then its statistical distribution satisfies the following properties:

[0144]

[0145]

[0146] From the above formula, we can see that the sample mean Follows a pattern with a mean of zero and a variance of . The sample variance follows a normal distribution; the normalized sample variance follows a set of degrees of freedom of . The chi-square distribution.

[0147] Since both of the above formulas have uncertain parameters This statistic cannot be directly used for testing. To eliminate the uncertainty parameter, based on the definition of the t-distribution, the sample mean and sample variance are combined to construct the following statistic:

[0148]

[0149] Simplifying the equation and eliminating the unknown parameters yields the final test statistic:

[0150]

[0151] To simultaneously leverage the rapid fault response of the optimal parity vector method and the environmental adaptability of the t-test fault detection method, the two methods are combined for fault detection, and the system fault status is determined based on the normal distribution table and the t-distribution table.

[0152] If both the optimal parity vector and the fault detection function of the t-test exceed the threshold, or if the fault detection function of the t-test exceeds the threshold, the system is considered to have malfunctioned, and the specific sensor causing the malfunction is located. If the fault detection function of the optimal parity vector method exceeds the threshold, but the fault detection function of the t-test does not, the sample window of the t-test is switched to the data after the fault, and the sample size is reduced to increase the sensitivity to fault signals. Fault detection continues to be performed to prevent false alarms caused by environmental changes. If neither fault detection function exceeds the threshold, the system is considered not to have malfunctioned, and inertial navigation data is used to perform inertial navigation calculations.

[0153] The fault termination time is determined by the optimal parity vector method and the t-test. When the statistic falls back to the normal range, the system determines that the fault has terminated and switches back to the high-precision inertial navigation mode to continue operation.

[0154] The specific axis of the sensor failure was obtained using the method described above. However, since it involves comparing two inertial navigation data points, it is impossible to pinpoint the specific sensor that failed. Therefore, a least-squares-based fault location method was used to locate the specific sensor that failed.

[0155] Due to the continuity of the carrier's angular velocity and acceleration, the gyroscope output will not change significantly within a sufficiently short time interval. Therefore, based on the fault-free proximity data output by the sensor, the predicted value at the fault-free moment is obtained using the weighted least squares method:

[0156] The linear observation model is as follows:

[0157]

[0158] in, For sensor observation vectors, For the observation matrix, The state to be estimated. To observe noise.

[0159] The optimal least squares estimate is obtained as follows:

[0160]

[0161] in, It is a weight matrix, take .

[0162] This invention uses high-precision and low-precision inertial navigation data from fault-free times to predict the system's output value at potential fault moments. Since the noise variance may change in a dynamic environment, a sliding window estimation method is used for real-time updates. .

[0163] To quantify the degree of deviation from the fault, the residual between the measured value and the predicted value is calculated at the moment the fault is detected:

[0164]

[0165] residual of faulty sensor It will deviate significantly from the normal range, and the source of the fault can be located by comparing the weighted residuals.

[0166] Calculate the least squares residual fault statistics based on the residuals:

[0167]

[0168] The fault threshold can be obtained by consulting the chi-square table. Then, determine whether the fault statistics of both inertial navigation systems exceed the threshold. If the fault detection count of the high-precision inertial navigation system is greater than the threshold, and the fault detection count of the low-precision inertial navigation system is less than the threshold, it indicates that a fault has occurred on the corresponding axis of the high-precision inertial navigation system, and system reconstruction continues. If the fault detection counts of both the high-precision and low-precision inertial navigation systems are greater than or less than the threshold, it indicates that a false alarm has occurred, and the system has not experienced a fault; in this case, inertial navigation calculation is performed using the high-precision inertial navigation data. If the fault detection count of the low-precision inertial navigation system is greater than the threshold, and the fault detection count of the high-precision inertial navigation system is less than the threshold, it indicates that a fault has occurred on the corresponding axis of the low-precision inertial navigation system, but this does not affect the operation of the inertial navigation system; in this case, inertial navigation calculation is performed using the high-precision inertial navigation data.

[0169] The low-precision inertial navigation data is converted to a high-precision inertial navigation data coordinate system to reconstruct the system. Using the specific axis identifiers of the faults in the high-precision inertial navigation system, the fault information is compensated using low-precision inertial navigation information, and then fused together with the fault-free high-precision inertial navigation data for calculation.

[0170] Assume that the two inertial navigation systems have achieved time synchronization and completed online calibration and measurement coefficient error compensation under fault-free conditions. Simultaneously, the installation error between the two inertial measurement units (IMUs) has been acquired, allowing the output of the low-precision IMU to be converted to the high-precision IMU coordinate system. The angular velocity and acceleration after installation error and lever arm compensation are as follows:

[0171]

[0172]

[0173]

[0174]

[0175] in, The converted angular velocity, The converted ratio, This is the coordinate transformation matrix. For low-precision inertial navigation angular velocity, For low-precision inertial navigation acceleration, To compensate for acceleration due to lever arm effect, Let the lever arm vector be... Angular acceleration, This is the converted acceleration.

[0176] Using gyroscope and acceleration data, a mechanical arrangement for the strapdown inertial navigation system (INS) is established, and INS calculations are completed. If the high-precision INS is functioning correctly, the gyroscope and acceleration data output by the high-precision INS are used directly for calculations. If the high-precision INS malfunctions, the reconstructed data from the low-precision INS after compensation is used for calculations.

[0177] The attitude of the inertial navigation system is updated using the northeast-central coordinate system as the navigation coordinate system.

[0178] The goal of attitude update is to solve for the transformation matrix from the vehicle coordinate system (b-frame) to the navigation coordinate system (n-frame). The attitude differential equation is:

[0179]

[0180] in, This refers to the angular velocity output by the gyroscope. This represents the angular velocity of the navigation frame relative to the inertial frame (i-frame).

[0181] Since it is difficult to directly solve the attitude differential equation, the chain multiplication principle is adopted.

[0182]

[0183] in, and They represent the previous time step, respectively. and The pose matrix at time step, By the carrier during the sampling period The rotation within is determined. The rotation of the navigation system within period T is determined by the rotation caused by the Earth's rotation and the motion of the carrier.

[0184] Using the angular increment output from the gyroscope for two-sample cone error compensation, the equivalent rotation vector is calculated. :

[0185]

[0186] in, and This represents the angular increment between two equally spaced samples within the period. It can then be obtained through the response transformation relationship:

[0187]

[0188] in, , for The opposition to the formation.

[0189] Due to the navigation update cycle The change is very small, so it is approximately a constant. The calculation formula is:

[0190]

[0191] Earth's rotational angular velocity components:

[0192]

[0193] Navigation rotation angular velocity:

[0194]

[0195] in, This is the Earth's rotational angular rate. and These are latitude and altitude, respectively.

[0196] Then the rotation vector Calculate using a method similar to the previous step. .

[0197] To improve computational efficiency and numerical stability, quaternions are used for updating, which allows for the calculation of new values. After performing quaternion-optimal normalization, the updated pose matrix is ​​obtained as follows:

[0198]

[0199] From the elements of the attitude matrix Solving Euler angles: Pitch angle Roll angle heading angle .

[0200] The goal of velocity updates in inertial navigation systems is to determine the velocity of the vehicle within the navigation system.

[0201] The velocity differential equation is as follows:

[0202]

[0203] In the formula, This represents the projection of the vehicle's velocity onto the three coordinate axes of the geographic coordinate system. The specific force measured by the accelerometer. This is the acceleration due to gravity.

[0204] After discretization, it can be rewritten in the following recurrence form:

[0205]

[0206]

[0207]

[0208]

[0209] and These represent the velocity increment of the navigation system relative to its velocity and the velocity increment of harmful acceleration, respectively, within the time period.

[0210] The goal of position update for an inertial navigation system is to determine the latitude, longitude, and altitude of the vehicle.

[0211] The differential equation for position calculation is as follows:

[0212]

[0213] In the formula, , and The vector represents the latitude, longitude, and altitude of the carrier in a geographic coordinate system. Let be the radius of curvature along the Earth's meridian. Let be the radius of curvature along the Earth's geoid.

[0214] After discretization, we can obtain:

[0215]

[0216] in, , .

[0217] Example (II)

[0218] In this embodiment, two inertial navigation systems (INS) with different calibers are placed coaxially on a common base. First, it is determined whether a system fault has occurred and the specific axis of the fault. Fault detection is performed using a method combining optimal parity vectors and t-tests. Once a fault is detected, a fault location method based on least squares is used to pinpoint the specific sensor causing the fault, achieving precise fault location and providing a foundation for subsequent system reconstruction. Then, if the high-precision INS fails, the fault axis information is replaced and compensated using converted low-precision INS axis data, reconstructing the system and improving navigation accuracy while ensuring system reliability. The specific implementation flowchart is as follows: Figure 2 As shown.

[0219] Main processing steps:

[0220] The first step is to place the two inertial navigation systems on a coaxial, shared base. The sensor mounting matrix is ​​as follows:

[0221]

[0222] The second step is to introduce the decoupling matrix. The motion state and fault of the carrier are separated to obtain a residual vector that is independent of the system state.

[0223] Under this installation matrix, based on the method for calculating the optimal parity vector, the optimal decoupling vector for each sensor can be calculated as follows:

[0224]

[0225]

[0226]

[0227]

[0228]

[0229]

[0230] The residual vector is obtained by calculating the optimal decoupling vector and the increments of angular velocity and acceleration output from the inertial navigation system sensors:

[0231]

[0232] The third step is to construct a fault detection function using the optimal parity vector method and the t-test.

[0233] Using the residual vector, a fault detection function based on the optimal parity vector method is constructed:

[0234]

[0235] in, Let Variance be the noise variance. , For the optimal parity vector elements, For matrix The diagonal elements.

[0236] Select a residual vector sample of length l, and construct a fault detection function for t-test using the mean and variance of the residual vector:

[0237]

[0238] The fourth step is to determine whether the fault detection function exceeds the threshold.

[0239] The threshold can be obtained by consulting the chi-square distribution table and the t-distribution table. If both the optimal parity vector method and the fault detection function of the t-test exceed the threshold, or if the fault detection function of the t-test exceeds the threshold, the system is considered to have failed, and fault location is performed to pinpoint the specific sensor that failed. If the fault detection function of the optimal parity vector method exceeds the threshold, but the fault detection function of the t-test does not, the sample window of the t-test is switched to the data after the fault, the sample size is reduced, and the sensitivity to fault signals is increased, and fault detection continues. If neither fault detection function exceeds the threshold, the system has not failed, and navigation continues using high-precision inertial navigation data.

[0240] The fault termination time is determined by the optimal parity vector method and the t-test. When the statistic falls back to the normal range, the system determines that the fault has terminated and switches back to the high-precision inertial navigation mode to continue operation.

[0241] The fifth step is to use a least-squares-based fault location method to locate the specific sensor causing the fault.

[0242] Using the outputs of the gyroscope and accelerometer near the fault-free moment, the optimal estimated solution is obtained by weighted least squares:

[0243]

[0244] in, It is a weight matrix, take .

[0245] Since the noise variance may change in a dynamic environment, a sliding window estimation method is used for real-time updates. At the moment the fault is detected, calculate the residual between the measured value and the predicted value:

[0246]

[0247] Calculate the least squares residual fault statistics based on the residuals:

[0248]

[0249] Determine whether the fault statistics of the two inertial navigation systems are greater than the threshold. If the fault detection count of the high-precision inertial navigation system is greater than the threshold and the fault detection count of the low-precision inertial navigation system is less than the threshold, it indicates that a fault has occurred on the corresponding axis of the high-precision inertial navigation system, and the following steps are performed for system reconstruction. If the fault detection counts of both the high-precision and low-precision inertial navigation systems are greater than the threshold or both are less than the threshold, or if the fault detection count of the low-precision inertial navigation system is greater than the threshold and the fault detection count of the high-precision inertial navigation system is less than the threshold, continue to use the output data of the high-precision inertial navigation system for navigation.

[0250] The sixth step is to convert the low-precision inertial navigation data to a high-precision inertial navigation coordinate system, replace the high-precision inertial navigation fault axis data, and combine it with the remaining normal high-precision data to complete the system reconstruction.

[0251]

[0252]

[0253]

[0254]

[0255] in, The converted angular velocity, The converted ratio, This is the coordinate transformation matrix. For low-precision inertial navigation angular velocity, For low-precision inertial navigation acceleration, To compensate for acceleration due to lever arm effect, Let the lever arm vector be... Angular acceleration, This is the converted acceleration.

[0256] The sixth step is to use the outputs of the gyroscope and accelerometer in the inertial navigation system to perform inertial navigation calculations.

[0257] Based on the outputs of the gyroscope and accelerometer, attitude, velocity, and measurement are updated to obtain the attitude, velocity, and position calculated by the inertial navigation system.

[0258] The experimental conditions and results of this invention are as follows:

[0259] A semi-physical simulation was conducted using an on-vehicle experiment. The high-precision fiber optic IMU employed the HY / LFL-2B optical inertial navigation system, while the low-precision MEMS IMU utilized the outputs of the gyroscope and accelerometer from the MTI-670 attitude reference system. The reference standard was the CGI-430 centimeter-level integrated navigation system. The fiber optic IMU gyroscope zero bias was... accelerometer zero bias is MEMS IMU gyroscope zero bias is accelerometer zero bias is gyroscope noise density for The accelerometer noise density is The two inertial navigation systems are configured with coaxial, shared base redundancy and an output frequency of 100Hz.

[0260] To demonstrate the effectiveness of the method proposed in this invention, the faults injected into the high-precision inertial gyroscope were manually added. Common faults can generally be divided into abrupt faults and gradually changing faults, so these two types of faults were simulated during the addition process. The fault addition table is shown in Table 1.

[0261]

[0262] To compare and evaluate the performance of fault detection algorithms, the proposed fault detection method was compared and analyzed with the traditional optimal parity vector method and the t-test-based detection method. Figures 4 to 6 The results of fault detection based on the optimal parity vector method, the t-test method, and a combination of the optimal parity vector and t-test methods are presented respectively. The sample length for the t-test is set to 10, and the sample length for fault detection using the optimal parity vector method is set to 6.

[0263] Experimental results show that the traditional optimal parity vector method has good real-time fault detection performance: abrupt faults can be detected instantly upon injection, and slowly changing faults can be identified 0.04 seconds after occurrence. However, because this method sets the noise variance to a fixed constant during initialization, it cannot adapt to the dynamic changes in environmental noise during actual operation, resulting in false alarms in the 0–5 second and 55–56 second intervals. In contrast, the t-test-based detection method effectively suppresses the impact of environmental uncertainty by eliminating uncertain environmental noise, and no false alarms occurred due to environmental changes in the experiment; however, due to the window smoothing effect, its fault detection has a certain lag, with detection delays of 0.08 seconds, 0.3 seconds, and 0.08 seconds for the three faults, respectively. The fault detection method proposed in this invention combines the advantages of the two traditional methods, maintaining high detection sensitivity while eliminating environmental uncertainty. Experimental results show that the method does not produce false alarms due to noise changes, and the detection delay for the three faults is only 0.05 seconds, 0.16 seconds and 0.05 seconds, which is less than the delay time of the t-test method. This indicates that the fusion algorithm has a fast fault identification speed while ensuring environmental adaptability.

[0264] After fault detection, a least-squares-based fault location method was used to identify the specific faulty sensor. In three fault injection experiments, the fault detection statistics for the fiber optic inertial navigation system (Fiber Optic IMU) were 13.107, 109.412, and 403.929, respectively, while the corresponding values ​​for the MEMS IMU were 1.4725, 0.0194, and 0.0028. By consulting the chi-square distribution table, a threshold value of 6.635 was selected, corresponding to a significance level of 0.01. The comparison shows that in all fault scenarios, the detection value of the fiber optic IMU exceeded this threshold, while the detection value of the MEMS IMU was below the threshold. Therefore, the faulty sensor can be identified as the fiber optic inertial navigation system, verifying the effectiveness of the fault location method.

[0265] When a fault occurs on a certain axis of a high-precision inertial navigation system (INS), traditional methods use a low-precision INS to continue navigation. In this invention, the low-precision INS data is converted to a high-precision INS coordinate system, replacing the faulty axis data. This data is then combined with the remaining normal high-precision data to reconstruct the system and continue navigation. Therefore, the error map after the low-precision INS compensation and reconstruction is compared with the error map of the low-precision INS navigation result. Figure 7 The figure shows a comparison of positional errors. As can be seen from the figure, after system reconstruction, the accuracy of the navigation results is significantly improved compared to using low-precision inertial navigation entirely.

[0266] The above description is merely a preferred embodiment of the present invention and is not intended to limit the invention. Various modifications and variations can be made to the present invention by those skilled in the art. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. A reconstruction method for a heterogeneous dual-redundant strapdown inertial navigation system, characterized in that, Includes the following steps: Step 1: Place the high-precision inertial navigation system and the low-precision inertial navigation system in a coaxial, common-base manner, and initialize the observation matrix and decoupling matrix to obtain the real-time measurement values ​​of the sensors, including the raw data of the high-precision inertial navigation system and the raw data of the low-precision inertial navigation system; Step 2: Based on the real-time sensor measurements and the decoupling matrix, perform residual calculation to obtain the residual vector; Step 3: Based on the residual vector, perform fault detection. If a fault occurs, generate the fault detection result and fault axis identifier, and proceed to the next step; otherwise, directly use the high-precision inertial navigation raw data as the data to be solved and proceed to step 6. Step 4: Based on the fault detection results and fault axis identification, combined with sensor data from near-fault-free times, perform fault location and determine whether the fault point is a high-precision inertial navigation system. If so, generate a fault sensor identification and proceed to the next step; otherwise, directly use the original high-precision inertial navigation system data as the data to be solved and proceed to step 6. Step 5: Based on the fault sensor identifier and combined with the real-time data from the low-precision inertial navigation system, perform system reconstruction to obtain the reconstructed sensor data; Step 6: Perform inertial navigation calculations, obtain navigation results, and output them.

2. The reconstruction method for a heterogeneous dual-redundant strapdown inertial navigation system according to claim 1, characterized in that, The residual vector in step 2 The specific calculation methods include: in, For decoupling matrix, The sensor measures real-time values. The sequence is a zero-mean Gaussian white noise sequence. This is the fault vector.

3. The reconstruction method for a heterogeneous dual-redundant strapdown inertial navigation system according to claim 2, characterized in that, The fault detection method described in step 3 specifically includes: For each sensor Calculate the detection function ; in, For sensors The residual, For noise sensitivity weights; From residuals Extraction Given a sample of 1, set the sample mean and variance, and construct the final test statistic. ; in, The residual sample mean. The residual sample variance For degrees of freedom of distributed; Judgment detection function and final test statistic If none of the above values ​​exceed the first threshold, then the system is considered fault-free; otherwise, only the detection function is present. If the first threshold is exceeded, switch. Examine the sample window up to the post-fault data, reduce the sample size, and recalculate the detection function. If there is still no fault, it is determined to be fault-free; if the detection function... and final test statistic If all values ​​exceed the first threshold, it is considered a fault, the fault axis identifier is output, and step 4 is executed; if only the final test statistic is considered... If the first threshold is exceeded, it is determined to be a fault, the fault axis identifier is output and step 4 is executed.

4. The reconstruction method for a heterogeneous dual-redundant strapdown inertial navigation system according to claim 3, characterized in that, Step 4, the fault location, specifically includes: Calculate the predicted value for the current time based on sensor data from near the fault-free point. ; in, For the observation matrix, This is the weight matrix. For sensor data near the fault-free point, Calculation for transpose; Calculate the residual between the predicted value and the measured value at the current time. ; Calculate fault statistics ; Based on the relationship between the fault statistics of the high-precision inertial navigation system and the fault statistics of the low-precision inertial navigation system and the second threshold, fault location is performed; if the fault statistics of the high-precision inertial navigation system exceed the second threshold and the fault statistics of the low-precision inertial navigation system do not exceed the second threshold, the fault is located as a high-precision inertial navigation system fault, the fault sensor identifier is output, and step 5 is executed; otherwise, it is judged as a false alarm, the high-precision inertial navigation system data is used directly, and step 6 is executed.

5. The reconstruction method for a heterogeneous dual-redundant strapdown inertial navigation system according to claim 4, characterized in that, Step 5, the system reconfiguration, specifically includes: The low-precision inertial navigation real-time data is converted to a high-precision inertial navigation coordinate system to obtain the converted sensor data. in, The converted angular velocity, The converted ratio, This is the coordinate transformation matrix. For low-precision inertial navigation angular velocity, For low-precision inertial navigation acceleration, To compensate for acceleration due to lever arm effect, Let the lever arm vector be... Angular acceleration, This is the converted acceleration.

6. The reconstruction method for a heterogeneous dual-redundant strapdown inertial navigation system according to claim 5, characterized in that, Step 6, the inertial navigation calculation, specifically includes: Calculate the attitude differential equation; in, The transformation matrix from the carrier system to the navigation system. This refers to the angular velocity output by the gyroscope. This is the angular velocity of the navigation frame relative to the inertial frame; Discretize the attitude differential equation to obtain Euler angles and complete the attitude update; Calculate the velocity differential equation; in, The projection of the vehicle's velocity onto the navigation system. For accelerometer specific force, It is the acceleration due to gravity; Discretize the velocity differential equation, recursively calculate the velocity increment, and complete the velocity update. Calculate the position differential equation; in, These represent the latitude, longitude, and altitude vectors of the carrier in the geographic coordinate system. These are the radii of curvature along the Earth's meridian and the radii of curvature along the Earth's circumpolar region, respectively. These are the north, east, and celestial velocities, respectively. Discretize the position differential equation to complete the position update.

7. A computer device comprising a memory, a processor, and a computer program stored in the memory, characterized in that: The processor executes the computer program to implement the steps of the method according to any one of claims 1 to 6.

8. A computer-readable storage medium having a computer program stored thereon, characterized in that: When executed by a processor, the computer program implements the steps of the method according to any one of claims 1 to 6.

9. A computer program product comprising computer instructions, characterized in that: When executed by a processor, the computer instructions implement the steps of the method according to any one of claims 1 to 6.

Images