Extended Kalman filtering method based on dynamic adaptive residual error enhancement

Abstract

The invention discloses an extended Kalman filtering method based on dynamic self-adaption residual error enhancement, and relates to the field of optimization algorithm error accumulation methods, and the extended Kalman filtering method based on dynamic self-adaption residual error enhancement significantly improves the sensing precision and reliability of a system through cooperative processing of multi-source heterogeneous sensing information, and improves the accuracy and reliability of the system. A sliding window covariance estimator is introduced, time-varying statistical characteristics of IMU angular velocity noise and GPR echo noise are identified in real time, a dual-threshold residual monitoring mechanism is designed, Jacobian matrix update frequency is dynamically adjusted to balance linearization errors and calculation loads, a robust correction term based on a Mahalanobis distance is established, and the robustness of the algorithm is improved. And abnormal observation interference caused by intermittent failure of the sensor is effectively inhibited.

Description

Technical Field

[0001] This application belongs to the field of error accumulation methods for optimization algorithms, and more specifically, it relates to a dynamic adaptive residual enhancement extended Kalman filter method. Background Technology

[0002] Sensor fusion technology significantly improves the perception accuracy and reliability of the system by collaboratively processing multi-source heterogeneous sensor information. It has become the core technology support for environmental modeling and autonomous navigation of ground-penetrating radar robots. Most existing fusion algorithms adopt the extended Kalman filter framework with fixed parameters.

[0003] In complex terrain detection scenarios, traditional single-sensor systems face significant limitations. Although ground-penetrating radar has the ability to penetrate the ground surface, its reflected signals are easily affected by the non-uniformity of the medium and electromagnetic interference. GPS positioning suffers from signal attenuation defects in obstructed environments and is difficult to effectively cope with changes in dynamic noise statistical characteristics and non-Gaussian residual interference, resulting in theoretical bottlenecks in multi-sensor collaborative optimization.

[0004] Therefore, those skilled in the art have proposed a dynamic adaptive residual enhancement extended Kalman filter algorithm, which achieves a breakthrough in perception performance by constructing a three-level optimization architecture. This algorithm reduces the mean square error of location estimation by 17% compared to the traditional EKF, while increasing the confidence level of underground target identification to over 8%. Summary of the Invention

[0005] To address the shortcomings of existing technologies, this invention provides a dynamic adaptive residual enhancement extended Kalman filter method, which achieves a breakthrough in sensing performance by constructing a three-level optimization architecture: (1) Introducing a sliding window covariance estimator to identify the time-varying statistical characteristics of IMU angular velocity noise and GPR echo noise in real time; (2) Designing a dual-threshold residual monitoring mechanism to dynamically adjust the Jacobian matrix update frequency to balance linearization error and computational load; (3) Establishing a robust correction term based on Mahalanobis distance to effectively suppress abnormal observation interference caused by intermittent sensor failure.

[0006] To achieve the above objectives, the present invention is implemented through the following technical solution: a ground-penetrating radar robot platform with modular design is built based on a dynamic adaptive residual enhancement extended Kalman filter method. It integrates multiple sensors to achieve accurate detection and stable operation. A two-dimensional lidar is mounted on the front of the vehicle body for SLAM mapping and obstacle perception.

[0007] The RTK module and IMU module work together to obtain the reference position and attitude information of the unmanned vehicle.

[0008] The GPR is used for underground target detection. Its antenna frequency range is 200~1000MHz. The unmanned vehicle is equipped with a tracked mobile chassis and has excellent terrain adaptability. It can operate stably in unstructured environments such as loose gravel and muddy roads.

[0009] Preferably, the positioning and positioning conversion of the equipment: In the odometry coordinate system Odom, assume the robot's position vector is P = (X, Y, θ). T The robot's linear velocity is v, and its angular velocity is w, then

[0010] Since the robot ultimately needs coordinates in the global coordinate system, while the information obtained by the sensors is based on the robot's coordinate system, a coordinate transformation is required to obtain the coordinates (X, Y, θ) in the global coordinate system. The robot's transformation matrix T is... .

[0011] Preferably, the IMU module includes a gyroscope and an accelerometer, which can obtain the robot's velocity, acceleration, and angular velocity. Integrating the acceleration and angular velocity information yields the position information for the next moment, allowing for correction of the position coordinates and thus achieving optimization. Let v be the velocity obtained by the IMU at time t. t angular velocity is w t The acceleration is a t Furthermore, the acceleration and angular velocity are affected by the bias B and the noise N.

[0012]

[0013]

[0014] In the formula: It is the transformation matrix for the IMU from the world coordinate system to the IMU coordinate system; This is the gravity vector.

[0015] The robot's movement is inferred through the IMU; the robot is... Rotation of time ,speed ,Location .

[0016] Preferably, the latitude and longitude coordinates obtained from GPS belong to the WSG-84 coordinate system. This needs to be converted from the WSG-84 coordinate system to a spatial rectangular coordinate system, and then Gaussian projected onto the spatial rectangular coordinate system to obtain the commonly used geodetic coordinate system, which is necessary for proper positioning and navigation. A system of equations is established using the three-dimensional Cartesian coordinates of three or more common points, and seven parameters are solved to obtain the coordinate transformation formula. In the three-dimensional Cartesian coordinate system, assuming there is a common point A, based on its position coordinate relationship in the two coordinate systems, the transformation equation can be obtained, where β is the value of the parameter being calculated; X... i These are the coordinates in the GPS coordinate system; X j These are the coordinate values ​​in a rectangular coordinate system; A i It is a transformation matrix from a spatial rectangular coordinate system to a GPS coordinate system. By transforming the coordinates of known points on each satellite into Cartesian coordinates, we can obtain n transformation equations. By solving these n equations simultaneously, we can find multiple parameter values. By using the least squares principle to linearly fit multiple parameter values, the parameter can be solved. The optimal solution can be found by using the transformation equation to obtain the coordinates in the spatial rectangular coordinate system. Then, perform a Gaussian projection on these coordinates to obtain the coordinates in the geodetic coordinate system. .

[0017] Preferably, in the traditional extended Kalman filter framework, the process noise covariance matrix Q and the observation noise covariance matrix... This value is typically set to a fixed value. However, this assumption has significant drawbacks in practical applications. Firstly, there's the time-varying nature of the noise; the sensor's noise characteristics change dynamically with the environment. For example, RTK signals exhibit relatively low observation noise in open environments. While diagonal elements have lower values, noise increases significantly in scenarios with poor signal strength. Odom exhibits relatively stable noise on flat surfaces, but its process noise Q changes significantly in bumpy or slippery conditions, requiring dynamic adjustment. Another issue is sensitivity to anomalies. RTK signals may produce outliers due to occlusion or interference; directly using these outliers for state correction can lead to divergent state estimates. However, traditional EKF lacks robust mechanisms for handling outlier data, making it difficult to address this problem.

[0018] To address the aforementioned issues, this project proposes an adaptive noise covariance strategy based on sensor data consistency. Specifically, this is achieved by dynamically adjusting the process noise covariance matrix Q and the observation noise covariance matrix. .

[0019] High-frequency data from Odom and IMU should satisfy kinematic consistency within a short time window. Define angular velocity residuals. IMU angular velocity Angular velocity with Odom The difference, similarly, the linear velocity residual.

[0020] The mean of the residuals is calculated using a sliding window. With variance Real-time update of the angular velocity noise term in the process noise covariance matrix Q and angular velocity noise term : To achieve adaptive adjustment of process noise covariance, this method employs a sliding window statistical mechanism for real-time analysis of sensor residuals. The time window length is defined as N, and at each time step k, the angular velocity and linear velocity residuals within the window are statistically calculated. in, This is a smoothing coefficient used to balance historical and current noise estimates. Similarly, the linear velocity noise term... Odom linear velocity Integral results with IMU acceleration The residuals are dynamically adjusted. Finally, the dynamic process noise covariance matrix is:

[0021] The reliability of RTK signals can be determined by the signal-to-noise ratio (SNR) output by the receiver or the positioning status. A SNR threshold is defined. When SNR < When the RTK observation noise is determined to be increased, the adjustment should be made. Position and heading angle noise terms:

[0022] in, This is the noise amplification factor, reflecting the increased observation uncertainty when signal quality deteriorates.

[0023] Preferably, in the correction phase of the Extended Kalman Filter (EKF), the observation residuals are the core basis for judging the quality of sensor data. Traditional EKF directly uses the observation residuals to update the state, but does not consider the impact of outlier data on the system. To address this, this method proposes a chi-square test based on Mahalanobis distance, which quantifies the rationality of the residuals through statistical properties, thereby enabling proactive identification and isolation of outlier data.

[0024] During the correction phase, the observed residuals are calculated. Mahalanobis distance:

[0025] If d(k) exceeds the chi-square distribution threshold If the current RTK data is deemed abnormal, the following mechanism will be triggered: When an anomaly in RTK data is detected, the system triggers the following two-level processing mechanism to suppress error correction and maintain positioning accuracy: triggering correction suppression, freezing Kalman gain updates, and relying solely on the prediction state.

[0026] Multi-sensor compensation is used to compensate for the linear velocity drift of Odom by integrating IMU acceleration. At the same time, the attitude angle change rate is constrained by a short-time kinematic model to avoid heading angle divergence.

[0027] This invention discloses a dynamic adaptive residual enhancement extended Kalman filter method, which has the following beneficial effects: This dynamic adaptive residual-enhanced extended Kalman filter method significantly improves the system's sensing accuracy and reliability by collaboratively processing multi-source heterogeneous sensing information. It introduces a sliding window covariance estimator to identify the time-varying statistical characteristics of IMU angular velocity noise and GPR echo noise in real time. A dual-threshold residual monitoring mechanism is designed to dynamically adjust the Jacobian matrix update frequency to balance linearization error and computational load. Furthermore, a robust correction term based on Mahalanobis distance is established to effectively suppress abnormal observation interference caused by intermittent sensor failures. Attached Figure Description

[0028] In the attached diagram: Figure 1 This is a schematic diagram of the method flow of an embodiment of this application. Detailed Implementation

[0029] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention are described clearly and completely. Obviously, the described embodiments are only some embodiments of the present invention, not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0030] This invention discloses a dynamic adaptive residual enhancement extended Kalman filter method. A modular ground-penetrating radar robot platform was built, which integrates multiple sensors to achieve accurate detection and stable operation of predetermined targets. A two-dimensional lidar is mounted on the front of the vehicle for SLAM mapping and obstacle perception.

[0031] The RTK module and IMU module work together to obtain the reference position and attitude information of the unmanned vehicle.

[0032] The GPR is used to detect underground targets. Its antenna frequency range is 200~1000MHz. The unmanned vehicle is equipped with a tracked mobile chassis and has excellent terrain adaptability. It can operate stably in unstructured environments such as loose gravel and muddy roads.

[0033] In the odometry coordinate system Odom, assume the robot's position vector is P = (X, Y, θ). T Given a robot with linear velocity v and angular velocity w, perform vector positioning of the robot's position:

[0034] Since the robot ultimately needs coordinates in the global coordinate system and displacement coordinates within that system, and the information obtained by the sensors is based on the robot's position, the coordinate system of the sensor points changes as the robot moves. This change is not reflected as a movement of the robot's coordinates, but rather as a change in relative position within the coordinate system. Therefore, a coordinate transformation is needed to obtain the coordinates (X, Y, θ) in the global coordinate system. The robot's transformation matrix T is... .

[0035] The IMU module contains a gyroscope and an accelerometer, which can measure the robot's speed, acceleration, and angular velocity. By integrating information such as acceleration and angular velocity, the position information at the next moment can be obtained. Based on this information, the position coordinates at the next moment can be corrected, thereby achieving optimization.

[0036] Let the velocity v obtained by the IMU at time t be... t angular velocity is w t The acceleration is a t Furthermore, the acceleration and angular velocity are affected by the bias B and the noise N.

[0037]

[0038]

[0039] In the formula: It is the transformation matrix for the IMU from the world coordinate system to the IMU coordinate system; This is the gravity vector.

[0040] The robot's movement is inferred through the IMU; the robot is... Rotation of time ,speed ,Location They are respectively

[0041]

[0042]

[0043] GPS latitude and longitude coordinates are in WSG-84 coordinate system. They need to be converted to a spatial rectangular coordinate system, and then Gaussian projected onto the spatial rectangular coordinate system to obtain the commonly used geodetic coordinate system, which is essential for proper positioning and navigation. A system of equations is established using the three-dimensional Cartesian coordinates of three or more common points. Solving for seven parameters yields the coordinate transformation formula. In the three-dimensional Cartesian coordinate system, assuming a common point A exists, based on its positional coordinate relationship in the two coordinate systems, the transformation equation can be obtained as follows:

[0044] In the formula, β is the parameter value to be found; X i These are the coordinates in the GPS coordinate system; X j These are the coordinate values ​​in a rectangular coordinate system; A i It is a transformation matrix from a spatial rectangular coordinate system to a GPS coordinate system. From the transformation between the coordinates of known points on each satellite and spatial rectangular coordinates, we can obtain n transformation equations.

[0045] By solving the above n equations simultaneously, multiple parameter values ​​can be obtained. By using the least squares principle to linearly fit multiple parameter values, the parameter can be solved. The optimal solution can be found by using the transformation equation to obtain the coordinates in the spatial rectangular coordinate system. Then, perform a Gaussian projection on these coordinates to obtain the coordinates in the geodetic coordinate system. The conversion formula is as follows:

[0046]

[0047] The location information of RTK can then be represented as .

[0048] In the traditional extended Kalman filter framework, the process noise covariance matrix Q and the observation noise covariance matrix... This value is typically set to a fixed value. However, this assumption has significant drawbacks in practical applications. Firstly, there's the time-varying nature of the noise; the sensor's noise characteristics change dynamically with the environment. For example, RTK signals exhibit relatively low observation noise in open environments. While diagonal elements have lower values, noise increases significantly in scenarios with poor signal strength. Odom exhibits relatively stable noise on flat surfaces, but its process noise Q changes significantly in bumpy or slippery conditions, requiring dynamic adjustment. Another issue is sensitivity to anomalies. RTK signals may produce outliers due to occlusion or interference; directly using these outliers for state correction can lead to divergent state estimates. However, traditional EKF lacks robust mechanisms for handling outlier data, making it difficult to address this problem.

[0049] To address the aforementioned issues, this project proposes an adaptive noise covariance strategy based on sensor data consistency. Specifically, this is achieved by dynamically adjusting the process noise covariance matrix Q and the observation noise covariance matrix. .

[0050] High-frequency data from Odom and IMU should satisfy kinematic consistency within a short time window. Define angular velocity residuals. IMU angular velocity Angular velocity with Odom The difference, similarly, the linear velocity residual.

[0051]

[0052]

[0053] The mean of the residuals is calculated using a sliding window. With variance Real-time update of the angular velocity noise term in the process noise covariance matrix Q and angular velocity noise term :

[0054]

[0055] To achieve adaptive adjustment of process noise covariance, this method employs a sliding window statistical mechanism for real-time analysis of sensor residuals. The time window length is defined as N, and at each time step k, the angular velocity and linear velocity residuals within the window are statistically calculated. in, This is a smoothing coefficient used to balance historical and current noise estimates. Similarly, the linear velocity noise term... Odom linear velocity Integral results with IMU acceleration The residuals are dynamically adjusted. Finally, the dynamic process noise covariance matrix is:

[0056] The reliability of RTK signals can be determined by the signal-to-noise ratio (SNR) output by the receiver or the positioning status. A SNR threshold is defined. When SNR < When the RTK observation noise is determined to be increased, the adjustment should be made. Position and heading angle noise terms:

[0057] in, This is the noise amplification factor, reflecting the increased observation uncertainty when signal quality deteriorates.

[0058] In the correction phase of Extended Kalman Filter (EKF), the observation residuals are the core criterion for judging the quality of sensor data. Traditional EKF directly uses the observation residuals to update the state, but does not consider the impact of outlier data on the system. To address this, this method proposes a chi-square test based on Mahalanobis distance, which quantifies the rationality of the residuals through statistical properties, thereby enabling proactive identification and isolation of outlier data.

[0059] During the correction phase, the observed residuals are calculated. Mahalanobis distance:

[0060] If d(k) exceeds the chi-square distribution threshold If the current RTK data is deemed abnormal, the following mechanism will be triggered: When an anomaly in RTK data is detected, the system triggers the following two-stage processing mechanism to suppress error correction and maintain positioning accuracy: triggering correction suppression, freezing Kalman gain updates, and relying solely on the prediction state.

[0061] Multi-sensor compensation is used to compensate for Odom's linear velocity drift by integrating IMU acceleration. Simultaneously, a short-time kinematic model is used to constrain the rate of change of attitude angles, preventing heading angle divergence.

[0062] To evaluate the localization performance of the algorithm, the results were compared with those of various open-source algorithms. Under the same external environment, localization tests were conducted on the DARE-EKF algorithm, the EKF algorithm, and the single sensor fusion algorithm.

[0063] In an indoor environment, a rectangular path was set, and several fixed points were selected on the path as observation points. These points were marked with letters A to H in the diagram. The changes in the relative positions between the points and the robot were measured to ensure the accuracy of the calculations.

[0064] Manually control the robot to circle along the set route, and record whether the robot's position output by the current program is correct when it passes the observation point.

[0065] The embodiments described above are merely preferred embodiments of this application, and while the descriptions are specific and detailed, they should not be construed as limiting the scope of this application. It should be noted that those skilled in the art can make various modifications, improvements, and substitutions without departing from the concept of this application, and these all fall within the protection scope of this application.

Claims

1. A dynamic adaptive residual enhancement extended Kalman filter method integrating multiple sensors, characterized in that: Equipped with a 2D LiDAR, it performs SLAM mapping and obstacle perception; The RTK module and IMU module work together to obtain reference position and attitude information; GPR is used for underground target detection.

2. The extended Kalman filter method based on dynamic adaptive residual enhancement as described in claim 1, characterized in that: Equipped with an odometer, and assuming the robot's position vector under Odom, the coordinates of the points of the device in the Odom coordinate system are transformed to obtain the coordinates of the device in the global coordinate system.

3. The extended Kalman filter method based on dynamic adaptive residual enhancement as described in claim 1, characterized in that: The IMU module contains a gyroscope and an accelerometer to measure the device's speed V. t acceleration a t and angular velocity w t进行 The measurement integrates the acceleration and angular velocity information to obtain the device's position information at the next moment, and then corrects the device's position coordinates at the next moment.

4. The extended Kalman filter method based on dynamic adaptive residual enhancement as described in claim 3, characterized in that: The motion of the device is inferred using the IMU module, and the rotation angle R of the device at time t+Δt is defined. t++△t Speed ​​V t++△t Location P t++△t .

5. The extended Kalman filter method based on dynamic adaptive residual enhancement as described in claim 1, characterized in that: The device coordinates obtained from GPS are converted into spatial rectangular coordinates. The device coordinates are then subjected to Gaussian projection into the spatial rectangular coordinate system to obtain the commonly used geodetic coordinate system, which is then provided for navigation.

6. The extended Kalman filter method based on dynamic adaptive residual enhancement as described in claim 1, characterized in that: A system of equations is established using the three-dimensional Cartesian coordinates of three or more common points. Seven parameters are solved to obtain the coordinate transformation formula. In the three-dimensional Cartesian coordinate system, assuming there is a common point A, the transformation equation is obtained based on the position coordinate relationship between the two coordinate systems.

7. The extended Kalman filter method based on dynamic adaptive residual enhancement as described in claim 6, characterized in that: The transformation between the coordinates of known points on each satellite and spatial rectangular coordinates yields n transformation equations. By solving these n equations simultaneously, multiple parameter values ​​β can be obtained. The optimal solution for parameter β can then be obtained by linearly fitting these multiple parameter values ​​using the least squares principle.

8. The extended Kalman filter method based on dynamic adaptive residual enhancement as described in claim 7, characterized in that: The coordinates in the spatial rectangular coordinate system are obtained by using the transformation equation of the IMU module, and then the coordinates are Gaussian projected to obtain the coordinates of the device in the geodetic coordinate system.

9. The extended Kalman filter method based on dynamic adaptive residual enhancement as described in claim 1, characterized in that: The process noise covariance matrix Q and the observation noise covariance matrix are dynamically adjusted. The mean and variance of the residuals are statistically analyzed through a sliding window, and the angular velocity noise term and angular velocity noise term in the process noise covariance matrix are updated in real time.

10. The extended Kalman filter method based on dynamic adaptive residual enhancement as described in claim 9, characterized in that: The high-frequency data from Odom and IMU should satisfy kinematic consistency within a short time window, and the angular velocity residual e is defined. w For the IMU angular velocity w Imu With Odom angular velocity w odom The difference is calculated by using a sliding window to statistically determine the mean μe and variance θ of the residuals. 2 V Real-time update of the angular velocity noise term θ in the process noise covariance matrix Q 2 w and angular velocity noise term θ 2 v Define the time window length as N, and at each time k, perform statistical calculations on the angular velocity residuals and linear velocity residuals within the window.

Images