A combined navigation method fusing fault recovery and multi-task uncertainty estimation

Abstract

This invention relates to the field of ground vehicle navigation and positioning technology, and in particular to a combined navigation method that integrates fault recovery and multi-task uncertainty estimation. It addresses the technical shortcomings of existing IMU / ODO integrated navigation systems, where the traditional FDI method "only removes, does not replenish," forcing the system to degrade to a pure inertial navigation solution mode when ODO data is severely distorted due to slippage. This leads to navigation interruptions and a rapid decline in positioning accuracy. This invention provides a combined navigation method that integrates fault recovery and multi-task uncertainty estimation, possessing slip perception and intelligent recovery capabilities. It organically integrates three core steps: "detection-substitution-fusion." The steps are: data initialization, performing innovation-based ODO anomaly detection, generating multi-task pseudo-odometer substitution data, dynamically selecting the observation mode and constructing the measurement noise covariance matrix based on the innovation-based ODO anomaly detection results, using IEKF as the data fusion center, and measurement updates and time recursion.

Description

Technical Field

[0001] This invention relates to the field of ground vehicle navigation and positioning technology, and in particular to a combined navigation method that integrates fault recovery and multi-task uncertainty estimation. Background Technology

[0002] The fusion of inertial measurement unit (IMU) and wheel speed-odometer (ODO) is a fundamental and widely used core technology in ground vehicle navigation and positioning. In this fusion scheme, the IMU (or strapdown inertial navigation system, SINS) can output complete motion state parameters of the vehicle at high frequency, but its internal errors accumulate rapidly over time, causing positioning results to drift. On the other hand, the ODO can provide velocity measurement data without long-term drift, which can effectively suppress the error divergence problem of SINS. The two complement each other.

[0003] However, the reliability of this fusion scheme is highly dependent on the data quality of ODO. When vehicles travel on complex road surfaces such as wet, slippery, or bumpy surfaces, or perform high-dynamic operations such as sharp turns or emergency braking, the wheels are prone to slippage or spinning. This directly leads to severely distorted or even temporarily invalid "outlier" data output by ODO. Such abnormal data completely violates the basic assumption of mainstream fusion algorithms such as Kalman filters that measurement noise follows a Gaussian distribution, which may cause serious consequences such as navigation state contamination and filter divergence, significantly reducing positioning accuracy.

[0004] To address these issues, traditional Fault Detection and Isolation (FDI) methods, such as the chi-square test based on new information, are widely used to identify and remove contaminated ODO data online. However, these methods have a core flaw—a passive processing strategy of "removing but not replenishing." During periods of frequent ODO failures, the integrated navigation system is forced to degrade to a pure inertial navigation solution mode. For low-cost MEMS inertial sensors, their position errors accumulate rapidly over time in a quadratic or even cubic manner. Even a brief downgrade can lead to a rapid and significant decrease in positioning accuracy, making it difficult to meet the safety requirements of high-level autonomous driving. Summary of the Invention

[0005] To overcome the technical shortcomings of existing IMU / ODO integrated navigation systems, which, when faced with severe distortion of ODO data due to slippage, rely solely on the traditional FDI method ("only removing, not replenishing"), forcing the system to degrade to a pure inertial navigation solution mode, leading to navigation interruptions and a rapid decline in positioning accuracy, this invention provides an integrated navigation method that integrates fault recovery and multi-task uncertainty estimation. This method possesses slip perception and intelligent recovery capabilities, organically integrating the three core components of "detection-replacement-fusion," thus solving the problem of navigation interruptions and accuracy degradation caused by short-term ODO failures.

[0006] This invention provides a combined navigation method integrating fault recovery and multi-task uncertainty estimation, comprising the following steps:

[0007] Step S1, data initialization, has the following sub-steps:

[0008] Step S11: Define the state vector ,in Represents the current time step, state vector Defined in Lie groups The data includes the vehicle's attitude, speed, position, and the IMU's bias and extrinsic parameters.

[0009] Step S12: Real-time data acquisition angular velocity of the IMU at time t and acceleration ,as well as ODO measurement velocity value at time 1 ;

[0010] Step S13: Maintain historical sequence: Set a time window of fixed length N, store the IMU measurement data collected in step S12 within the time window, and construct a historical IMU data sequence as input. This is used as input for subsequent neural networks;

[0011] Step S2: Perform ODO anomaly detection based on innovation. Its sub-steps are as follows:

[0012] Step S21: Calculate the new information: Calculate the new information of the velocity observation. New information That is, the current measurement speed of ODO. The difference between the system predicted velocity and the angular velocity of the IMU, where the system predicted velocity is the angular velocity of the IMU. and acceleration It was obtained through inertial navigation calculations;

[0013] Step S22: Calculate the covariance: Based on the current state covariance matrix and the preset ODO measurement noise covariance matrix, calculate the innovation covariance matrix. ;

[0014] Step S23: Construct statistics: based on the new information covariance matrix Construct the normalized innovation squared statistic :

[0015] ,in It is new information transpose, It is the new information covariance matrix The inverse matrix;

[0016] Step S24, Decision Logic: Calculate the new squared statistic. Compared with the preset confidence threshold Comparison:

[0017] like If the state of ODO is determined to be normal, then jump to branch A in step S4;

[0018] like If the ODO state is determined to be abnormal, such as slippage or wild value, then proceed to step S3 to trigger the switching of observation mode;

[0019] Step S3: Generate multi-task pseudo-odometer replacement data;

[0020] First, a multi-task pseudo-odometry network (Multi-Task-Odometry-Net) is constructed: The multi-task pseudo-odometry network adopts a CNN-Transformer hybrid architecture. The input sequence is first processed by a one-dimensional convolutional CNN backbone network to extract local motion patterns, then sinusoidal position encoding is added, and then it is fed into a multi-layer Transformer encoder. Its self-attention mechanism is used to capture the long-term dependencies of the sequence. Finally, it is processed by two parallel prediction heads to achieve multi-task output. One prediction head is a velocity prediction head, and the other prediction head is an uncertainty prediction head.

[0021] Secondly, when step S2 determines that the state of ODO is abnormal, the trained multi-task pseudo-odometer network is activated.

[0022] Step S31, Network Input; Input data sequence maintained in step S13. The input is fed into a multi-task pseudo-odometry network, which extracts local and long-term motion features.

[0023] Step S32, Network Inference: The multi-task pseudo-odometry network outputs predicted velocity values ​​through two parallel prediction heads. and uncertain forecast values Among them, the speed prediction value It is based on the vehicle's three-dimensional pseudo-velocity derived from IMU sequence regression, i.e., generating multi-task pseudo-odometer surrogate data; uncertainty prediction value It is the logarithmic variance of the output speed prediction, used to quantify the confidence of the multi-task pseudo-odometer network in the current prediction;

[0024] Step S33: Jump to branch B in step S4;

[0025] Step S4: Based on the determination result of step S2, dynamically select the observation mode and construct the measurement noise covariance matrix. ;

[0026] Branch A is the normal observation mode: set the observation vector. For ODO measurements: Based on the nominal accuracy of ODO, the measurement noise covariance matrix is ​​set. The preset nominal noise covariance matrix ;

[0027] Branch B is the recovery observation mode: set the observation vector. The velocity prediction value generated in step S32 : ; Utilize the uncertainty prediction values ​​generated in step S32 The measurement noise covariance matrix is ​​restored through exponential mapping. :

[0028] ;

[0029] Step S5: Using the Invariant Extended Kalman Filter (IEKF) as the data fusion center, measurement updates and time recursion are performed. Its sub-steps are as follows:

[0030] Step S51, Measurement Update: Using the observation vector determined in step S4 and adaptive measurement noise covariance matrix For the state vector of IEKF After making corrections, the optimal posterior estimate is obtained;

[0031] Step S52, Time Advancement: Utilizing IMU velocity data ( , Integrating the data advances the system state to the next time step. It then updates the state covariance matrix, completing a full navigation solution cycle.

[0032] Preferably, a Gaussian negative log-likelihood loss function is used to simultaneously optimize the accuracy of velocity prediction and the rationality of uncertainty estimation, thereby completing the training of the multi-task pseudo-odometer network.

[0033] Preferably, in step S3, the multi-task pseudo-odometer network needs to be trained offline before it is put into use. The method for constructing training samples is as follows: 1) Input data: select historical IMU data sequences; 2) Training labels: select the high-precision vehicle attitude and speed output by the fusion calculation of IMU and odometer as described in step S5 when the odometer is in a non-slip state, that is, when the judgment result of step S2 is normal, as the ground truth labels; through supervised learning, the multi-task pseudo-odometer network learns the mapping relationship from historical IMU data sequences to vehicle motion state.

[0034] Step S4 enables the adaptive adjustment of the filter's trust weights for virtual measurements based on the confidence level of the multi-task pseudo-odometer network itself. In step S5, the invariant extended Kalman filter (IEKF) employs an adaptive switching mechanism.

[0035] The technical solution provided by this invention has the following technical effects compared with the prior art:

[0036] First, it solves the fundamental flaw of the traditional FDI method that "only removes, does not replenish," and can provide high-quality alternative velocity observations when ODO fails.

[0037] II. Ensuring Continuity and Robustness: It achieves a smooth switch and recovery from real measurement to virtual measurement, effectively solving the problem of navigation interruption and accuracy reduction caused by short-term ODO failure, and significantly improving the robustness and continuity of the system under complex working conditions.

[0038] III. Intelligent Integration: The multi-task pseudo-odometer network proposed in this invention can output speed prediction and quantitative assessment of the uncertainty of the prediction in an integrated manner.

[0039] IV. Adaptive Weighting: The uncertainty of network prediction is directly used to dynamically construct the measurement noise covariance matrix of IEKF, which enables the filter to intelligently and sample-by-sample (time-by-time) adjust the confidence weight of virtual measurement according to the network's own confidence. Attached Figure Description

[0040] The accompanying drawings, which are incorporated in and form part of this specification, illustrate embodiments consistent with the invention and, together with the description, serve to explain the principles of the invention.

[0041] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, for those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0042] Figure 1 This is a general block diagram of a combined navigation method that integrates fault recovery and multi-task uncertainty estimation according to a certain embodiment of the present invention;

[0043] Figure 2 This is a schematic diagram of the structure of the multi-task pseudo-odometer network in a certain embodiment of the present invention. Detailed Implementation

[0044] To better understand the above-mentioned objectives, features, and advantages of the present invention, the solutions of the present invention will be further described below. It should be noted that, unless otherwise specified, the embodiments of the present invention and the features thereof can be combined with each other.

[0045] In this description, it should be noted that the terms "first" and "second" are used for descriptive purposes only and should not be construed as indicating or implying relative importance. It should also be noted that, unless otherwise expressly specified and limited, the terms "installation," "connection," and "joint" should be interpreted broadly. For example, they can refer to fixed connections, detachable connections, or integral connections; they can refer to mechanical connections or electrical connections; they can refer to direct connections or indirect connections through an intermediate medium; and they can refer to the internal connection between two components. Those skilled in the art can understand the specific meaning of the above terms according to the specific circumstances.

[0046] Many specific details are set forth in the following description in order to provide a full understanding of the invention, but the invention may also be practiced in other ways different from those described herein; obviously, the embodiments in the specification are only some embodiments of the invention, and not all embodiments.

[0047] The specific embodiments of the present invention will now be described in detail with reference to the accompanying drawings.

[0048] In one embodiment, such as Figure 1 As shown, a combined navigation method integrating fault recovery and multi-task uncertainty estimation is disclosed, the steps of which are as follows:

[0049] Step S1, data initialization, has the following sub-steps:

[0050] Step S11, Definition System state vector at time t ,in Represents the current time step, the system state vector. Defined in Lie groups superior, This includes the vehicle's attitude, speed, position, and the IMU's bias and extrinsic parameters;

[0051] Step S12: Real-time data acquisition angular velocity of the IMU at time t and acceleration ,as well as ODO measurement velocity value at time 1 This is the raw reading of ODO;

[0052] Step S13: Maintain historical sequence: Set a time window of fixed length N, store the IMU measurement data collected in step S12 within the time window, and construct a historical IMU data sequence as input. This is used as input to the subsequent neural network, and it contains data from the past N frames.

[0053] Step S2: Perform ODO anomaly detection based on innovation. Its sub-steps are as follows:

[0054] Step S21: Calculate the new information: Calculate the new information of the velocity observation. New information That is, the current measurement speed of ODO. The difference between the system predicted velocity and the angular velocity of the IMU, where the system predicted velocity is the angular velocity of the IMU. and acceleration It was obtained through inertial navigation calculations;

[0055] Step S22: Calculate the covariance: Based on the current state covariance matrix and the preset ODO measurement noise covariance matrix, calculate the innovation covariance matrix. It is used to standardize information;

[0056] Step S23: Construct statistics: based on the new information covariance matrix Construct the normalized innovation squared statistic This serves as the basis for fault detection decisions:

[0057] ,in It is new information transpose, It is the new information covariance matrix The inverse matrix;

[0058] Step S24, Decision Logic: Calculate the new squared statistic. Compared with the preset confidence threshold Comparison, preset confidence threshold This is a constant; values ​​exceeding this are considered abnormal.

[0059] like If the state of ODO is determined to be normal, then jump to branch A in step S4;

[0060] like If the ODO state is determined to be abnormal, such as slippage or wild value, then proceed to step S3 to trigger the switching of observation mode;

[0061] Step S3: Generate multi-task pseudo-odometer replacement data;

[0062] First, a multi-task pseudo-odometry network (Multi-Task-Odometry-Net) is constructed: the multi-task pseudo-odometry network adopts a CNN-Transformer hybrid architecture, see details. Figure 2 The input sequence is first processed by a one-dimensional convolutional CNN backbone network to extract local motion patterns, then sinusoidal position encoding is added, and then it is fed into a multi-layer Transformer encoder. Its self-attention mechanism is used to capture the long-term dependencies of the sequence. Finally, it is processed by two parallel prediction heads to achieve multi-task output. One prediction head is a velocity prediction head, and the other prediction head is an uncertainty prediction head.

[0063] Secondly, when step S2 determines that the state of ODO is abnormal, the trained multi-task pseudo-odometer network is activated.

[0064] Step S31, Network Input; Input data sequence maintained in step S13. The input is fed into a multi-task pseudo-odometry network, which extracts local and long-term motion features.

[0065] Step S32, Network Inference: The multi-task pseudo-odometry network outputs predicted velocity values ​​through two parallel prediction heads. and uncertain forecast values Among them, the speed prediction value It is based on the vehicle's three-dimensional pseudo-velocity derived from IMU sequence regression, i.e., generating multi-task pseudo-odometer replacement data to replace failed ODOs; uncertainty prediction values. It is the logarithmic variance of the output speed prediction, used to quantify the confidence of the multi-task pseudo-odometer network in the current prediction;

[0066] Step S33: Jump to branch B in step S4;

[0067] Step S4: Based on the determination result of step S2, dynamically select the observation mode and construct the measurement noise covariance matrix. ;

[0068] Branch A is the normal observation mode: set the observation vector. For ODO measurements: That is, the system uses ODO measurements. Update; based on ODO's nominal accuracy, set the measurement noise covariance matrix. The preset nominal noise covariance matrix Nominal noise covariance matrix It is a fixed value;

[0069] Branch B is the recovery observation mode: set the observation vector. The velocity prediction value generated in step S32 : That is, the measured value of system isolation ODO. It then invokes the multi-task pseudo-odometer network and utilizes the uncertainty prediction values ​​generated in step S32. The measurement noise covariance matrix is ​​restored through exponential mapping. :

[0070] ;

[0071] Step S5: Using the Invariant Extended Kalman Filter (IEKF) as the data fusion center, measurement updates and time recursion are performed. Its sub-steps are as follows:

[0072] Step S51, Measurement Update: Using the observation vector determined in step S4 and adaptive measurement noise covariance matrix For the state vector of IEKF After making corrections, the optimal posterior estimate is obtained;

[0073] Step S52, Time Advancement: Utilizing IMU velocity data ( , Integrating the data advances the system state to the next time step. It then updates the state covariance matrix, completing a full navigation solution cycle.

[0074] Based on the above embodiments, in a preferred embodiment, a Gaussian negative log-likelihood loss function is used to simultaneously optimize the accuracy of velocity prediction and the rationality of uncertainty estimation, thereby completing the training of the multi-task pseudo-odometer network.

[0075] Based on the above embodiments, in a preferred embodiment, the multi-task pseudo-odometer network needs to be trained offline before it is put into use. The method for constructing training samples is as follows: 1) Input data: Select historical IMU data sequences; 2) Training labels: Select the high-precision vehicle attitude and speed output by the IMU and odometer fusion calculation in step S5 when the odometer is in a non-slip state, that is, when the result of step S2 is normal, as the ground truth labels; through supervised learning, the multi-task pseudo-odometer network learns the mapping relationship from historical IMU data sequences to vehicle motion state.

[0076] The above description is merely a specific embodiment of the present invention, enabling those skilled in the art to understand or implement the present invention. Although detailed descriptions have been provided with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some or all of the technical features therein; and these modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments, and all should be covered by the protection scope of the claims.

Claims

1. A combined navigation method fusing fault recovery with multi-task uncertainty estimation, characterized in that, The steps are as follows: Step S1, data initialization, has the following sub-steps: Step S11: Define the state vector ,in Represents the current time step, state vector Defined in Lie groups The data includes the vehicle's attitude, speed, position, and the IMU's bias and extrinsic parameters. Step S12, Real-time data acquisition angular velocity of the IMU at time t and acceleration ,as well as ODO measurement velocity value at time ; Step S13: Set a time window of fixed length N, store the IMU measurement data collected in step S12 within the time window, and construct a historical IMU data sequence as input. This is used as input for subsequent neural networks; Step S2: Perform ODO anomaly detection based on innovation. Its sub-steps are as follows: Step S21: Calculate the new information from velocity observations New information That is, the current measurement speed of ODO. The difference between the system predicted velocity and the angular velocity of the IMU, where the system predicted velocity is the angular velocity of the IMU. and acceleration It was obtained through inertial navigation calculations; Step S22: Calculate the innovation covariance matrix based on the current state covariance matrix and the preset ODO measurement noise covariance matrix. ; Step S23: Based on the new information covariance matrix Construct the normalized innovation squared statistic : ,in It is new information transpose, It is the new information covariance matrix The inverse matrix; Step S24: Calculate the new squared statistic. Compared with the preset confidence threshold Comparison: like If the state of ODO is determined to be normal, then jump to branch A in step S4; like If the state of ODO is determined to be abnormal, then proceed to step S3. Step S3: Generate multi-task pseudo-odometer replacement data; First, a multi-task pseudo-odometry network is constructed: The multi-task pseudo-odometry network adopts a CNN-Transformer hybrid architecture. The input sequence is first processed by a one-dimensional convolutional CNN backbone network to extract local motion patterns, then sinusoidal position encoding is added, and then it is fed into a multi-layer Transformer encoder. Its self-attention mechanism is used to capture the long-term dependencies of the sequence. Finally, it is processed by two parallel prediction heads to achieve multi-task output. One prediction head is a velocity prediction head, and the other prediction head is an uncertainty prediction head. Secondly, when step S2 determines that the state of ODO is abnormal, the trained multi-task pseudo-odometer network is activated. Step S31: The input data sequence maintained in step S13 The input is fed into a multi-task pseudo-odometry network, which extracts local and long-term motion features. Step S32: The multi-task pseudo-odometry network outputs predicted velocity values ​​through two parallel prediction heads. and uncertain predicted values Among them, the speed prediction value It is based on the vehicle's three-dimensional pseudo-velocity derived from IMU sequence regression, i.e., generating multi-task pseudo-odometer surrogate data; uncertainty prediction value It is the logarithmic variance of the output speed prediction, used to quantify the confidence of the multi-task pseudo-odometer network in the current prediction; Step S33: Jump to branch B in step S4; Step S4: Based on the determination result of step S2, dynamically select the observation mode and construct the measurement noise covariance matrix. ; Branch A is the normal observation mode: set the observation vector. For ODO measurements: Based on the nominal accuracy of ODO, the measurement noise covariance matrix is ​​set. The preset nominal noise covariance matrix ; Branch B is the recovery observation mode: set the observation vector. The velocity prediction value generated in step S32 : ; Using the uncertainty prediction value generated in step S32 The measurement noise covariance matrix is ​​restored through exponential mapping. : ； Step S5: Perform Invariant Extended Kalman Filter (IEKF) measurement update and time recursion. Its sub-steps are as follows: Step S51: Using the observation vector determined in step S4 and adaptive measurement noise covariance matrix The state vector of the invariant extended Kalman filter (IEKF) After making corrections, the optimal posterior estimate is obtained; Step S52: Utilize the measurement data from the IMU ( , Integrating the data advances the system state to the next time step. It then updates the state covariance matrix, completing a full navigation solution cycle.

2. The combined navigation method integrating fault recovery and multi-task uncertainty estimation according to claim 1, characterized in that, In step S3, a Gaussian negative log-likelihood loss function is used to optimize both the accuracy of velocity prediction and the rationality of uncertainty estimation, thereby completing the training of the multi-task pseudo-odometer network.

3. The combined navigation method integrating fault recovery and multi-task uncertainty estimation according to claim 1, characterized in that, In step S3, the multi-task pseudo-odometer network needs to be trained offline before it is put into use. The training sample construction method is as follows: 1) Input data: select historical IMU data sequence; 2) Training label: select the vehicle attitude and speed output by the IMU and odometer fusion calculation in step S5 when the odometer is in a non-slip state, that is, when the judgment result of step S2 is normal, as the ground value label; through supervised learning, the multi-task pseudo-odometer network learns the mapping relationship from historical IMU data sequence to vehicle motion state.

Images