Laser point cloud inertial odometer method based on 4D millimeter wave radar compensation denoising
By using 4D millimeter-wave radar compensation and denoising methods, combined with multi-sensor fusion and adaptive ESKF filtering, the noise problem of lidar in degraded environments was solved, enabling precise positioning of robots in high-dynamic scenarios and improving positioning accuracy and robustness.
Patent Information
- Application Number
- CN202610006243.9
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2026-01-05
- Publication Date
- 2026-04-17
AI Technical Summary
In degraded environments such as rain, snow, and dust, lidar generates a large amount of noisy point clouds, leading to map distortion and reduced positioning performance. Existing filtering methods cannot adapt to complex spatial distributions, and inertial odometry accumulates errors in high-dynamic scenarios, resulting in inaccurate positioning.
A 4D millimeter-wave radar compensation and denoising method is adopted. Laser point clouds and 4D millimeter-wave point clouds are obtained through multi-sensor fusion. Neighborhood information is quickly obtained using K-ary trees. The odometry system status is propagated by the midpoint method. Adaptive ESKF filtering and intensity-weighted GICP matching are used to optimize the denoised point cloud and perform least squares cost function iterative solution to achieve accurate positioning in high dynamic environments.
Robust perception and precise localization of the robot were achieved in degraded environments, improving localization accuracy, ensuring the structuring of point clouds in high-noise scenarios, and enhancing the stability and localization accuracy of the inertial odometry.
Smart Images
Figure CN121876948A_ABST
Abstract
Description
Technical Field
[0001] This invention belongs to the field of mobile robot environmental perception and autonomous localization technology, specifically involving a laser point cloud inertial odometry method based on 4D millimeter-wave radar compensation and denoising. Background Technology
[0002] LiDAR is widely used in mobile robots and autonomous driving systems due to its high-precision 3D perception capabilities. However, in degraded environments such as rain, snow, and dust, the laser beam is strongly scattered by airborne particles, resulting in a large number of noisy point clouds. This leads to map distortion, geometric degradation, and inter-frame matching failure, ultimately causing a severe decline in positioning performance. Existing laser point cloud filtering methods can reduce noise to some extent, but they still have the following problems: fixed filtering parameters cannot adapt to complex spatial distributions; and the problem of erroneous deletion of environmental points is more prominent in high-noise density areas of dusty work scenarios. 4D millimeter-wave radar, due to its all-day effectiveness, can stably detect real targets in degraded scenarios, but its low resolution and significant clutter and multipath effects make it unsuitable as the sole sensor for high-precision map construction. In the field of inertial odometry, the Inertial Measurement Unit (IMU) can provide continuous motion prediction for slow-frequency laser updates due to its high-frequency and high-bandwidth characteristics. However, the traditional Error State Kalman Filter (ESKF) suffers from linearization error accumulation in high-dynamic scenarios, leading to covariance divergence.
[0003] Therefore, how to ensure that the robot can obtain accurate perception of the environmental point cloud in real time in degraded scenarios, dynamically remove particulate noise, and improve the accuracy of ESKF in the nonlinear IMU integration process so that it can adapt to high dynamic scenarios with large angular velocities and strong accelerations and long-term robot operation and localization processes in real time are the key issues in the current research. Summary of the Invention
[0004] In view of this, the present invention provides a laser point cloud inertial odometry method based on 4D millimeter-wave radar compensation and denoising, which realizes robust perception and accurate positioning of robots in degraded environments.
[0005] This invention provides a laser point cloud inertial odometry method based on 4D millimeter-wave radar compensation and denoising, specifically including the following steps:
[0006] Step 1: Calibrate multiple sensors, acquire laser point cloud and 4D millimeter wave point cloud of the surrounding environment of the work scene, record inertial measurement unit (IMU) data, and perform spatiotemporal alignment of laser point cloud and 4D millimeter wave point cloud.
[0007] Step 2: Acquire the laser point cloud of the current frame. Calculate the dynamic threshold based on the horizontal distance between the laser points and the sensor center. Use the distance at which the point cloud density decreases beyond the threshold as the distance threshold, and the median intensity of the laser point cloud in the current frame as the intensity threshold. Use millimeter-wave radar to identify and determine the real target area. Use a K-ary tree to quickly acquire the neighborhood information of the laser points. Retain laser points that simultaneously meet the following conditions: average neighborhood distance less than the dynamic threshold, horizontal distance not greater than the distance threshold, intensity not less than the intensity threshold, and belonging to the real target area identified by the millimeter-wave radar. This yields a structured, denoised point cloud. ;
[0008] Step 3: Denoise the point cloud The IMU data within the corresponding timestamp is taken as a frame of the odometer system. The system state of the odometer system is propagated using the midpoint method, and the covariance of the system state is propagated synchronously. Based on the state estimate of the previous time step... Covariance Obtain the state prediction for the next time step and the uncertainty of forecasts Propagate error state variables; then denoise the point cloud. Distortion removal;
[0009] Step 4: Calculate the denoised point cloud The mean and intensity-weighted covariance are used to calculate the residuals that match the GICP based on the state prediction at the next time step. The GICP residuals and the error state variables obtained in step 3 are used together as optimization variables to construct the least squares cost function. The least squares cost function is solved iteratively to obtain the optimal pose estimate.
[0010] Furthermore, the distance threshold is calculated as follows: the detection range of the lidar is divided into multiple shell intervals according to a set horizontal distance interval, and the point cloud height is limited according to the actual scene height; the point cloud density in each shell is calculated to obtain a discrete density sequence; spline fitting is performed on the discrete density sequence to obtain the first function of density with respect to distance, the derivative of the first function is obtained to obtain the distance at which the point cloud density decreases the fastest, and the distance at which the point cloud density decreases by a set multiple exceeds the normal attenuation rate is taken as the distance threshold.
[0011] Furthermore, the method for identifying and determining the real target area by millimeter-wave radar is as follows: the millimeter-wave point cloud is pre-processed by filtering, and high-confidence millimeter-wave point clouds are selected based on intensity and velocity; these high-confidence millimeter-wave point clouds are mapped to the IMU coordinate system and used as prior labels for the laser point cloud. The laser points representing the spatial location should be real environmental points that cannot be filtered out, which are the real target areas.
[0012] Furthermore, the method of using the midpoint method to propagate the system state of the odometry system is as follows: In the stationary state, the average value of the IMU angular velocity and acceleration is used as the initial bias of the IMU measurement; with the initial position of the IMU as the origin of the world coordinate system, and the previous moment as the prediction starting point, the rotation matrix and translation vector of the IMU relative to the world coordinate system at the next moment are calculated using the midpoint method based on the IMU kinematics.
[0013] Furthermore, the method for synchronously propagating the covariance of the system state is to use an adaptive error state Kalman filter to propagate the covariance of all error states, specifically:
[0014]
[0015] in, Let Jacobian matrix be the propagation matrix for error state. Let Jacobian matrix be the propagation matrix of the noise state. For the noise matrix, Let covariance be the value at the previous time step. This is for predicting the covariance of the system state at the next time step.
[0016] The Jacobian matrix for angular covariance propagation is:
[0017]
[0018] in, and These are Jacobian matrices. Jacobian blocks with respect to angle and Jacobian blocks with respect to angular velocity offset. This represents the negative rotation increment during IMU propagation. For noise variables in state propagation, It is the identity matrix. For time intervals, This refers to the angular error state quantity; and All are second-order compensation terms;
[0019] The angle noise matrix is:
[0020]
[0021] in, This refers to the angular noise block in the noise matrix. To correct the noise, For correction factor, As a regulating factor, The 2-norm of the angular velocity difference. For time intervals.
[0022] Furthermore, the K-nearest neighbor classification algorithm is used to calculate the local normal vector and covariance matrix of the denoised point cloud. The covariance matrix is calculated by searching the set of neighborhood points of the source point in a frame of laser point cloud. Specifically:
[0023]
[0024] in Let the local covariance of the source point be... All eigenvalues, To compensate for the post-covariance; For perceptual structure indicators. For structural threshold, These are weighting coefficients;
[0025] The intensity confidence function based on the covariance weighting of point cloud intensity is as follows:
[0026]
[0027] in These are weighting coefficients. For adjustment coefficients, This represents the laser detection intensity value at the current point. This represents the average intensity value of the laser point cloud in that frame. The optimized covariance, weighted by points, can be used for direct GICP registration.
[0028] Furthermore, the least squares cost function is:
[0029]
[0030] in, The optimization objective is weighted least squares cost. To obtain a laser point set with appropriate covariance information, For the matching residual of points, The weighted covariance of the corresponding points. The IMU propagation residual is the distance from the previous time step to the next time step.
[0031] Furthermore, the method for iteratively solving the least squares cost function to obtain the optimal environmental point cloud is as follows:
[0032] Based on the Gauss-Newton method, the Hessian matrix and observation vector are constructed using the covariance of the matched residuals and propagation residuals, and the Jacobian blocks of the residuals with respect to the state variables, to solve the normal equations. Iterate until the corrected state is reached. Convergence, Correction for And use ESKF to reset the Hessian matrix from the last iteration. The covariance of the system state at the next moment after the update is .
[0033] Beneficial effects:
[0034] This invention achieves robust environmental perception for the robot in degraded environments through multi-sensor fusion, and completes the corresponding precise positioning by combining optimized laser inertial odometry. The coupling denoising of lidar and 4D millimeter-wave radar ensures that structured environmental point clouds are preserved while filtering out laser noise. Intensity-weighted optimized GICP matching and ESKF with added second-order commutation terms ensure the stability of odometry pose estimation under high dynamics and sampling jitter. Compared with existing technologies, it has higher positioning accuracy on degraded datasets and is deployable in actual operation scenarios. Attached Figure Description
[0035] Figure 1 This is a flowchart illustrating the laser point cloud inertial odometry method based on 4D millimeter-wave radar compensation and denoising provided by the present invention.
[0036] Figure 2 This is a schematic diagram illustrating the process of coupling millimeter-wave radar and lidar denoising in the laser point cloud inertial odometry method based on 4D millimeter-wave radar compensation denoising provided by the present invention.
[0037] Figure 3 This is a schematic diagram illustrating the state propagation and update of the laser point cloud inertial odometry method based on 4D millimeter-wave radar compensation and denoising provided by the present invention. Detailed Implementation
[0038] The present invention will be described in detail below with reference to the accompanying drawings and embodiments.
[0039] The laser point cloud inertial odometry method based on 4D millimeter-wave radar compensation and denoising provided by this invention has the following process: Figure 1 As shown, the specific steps include:
[0040] Step 1: Acquire data acquisition results from multiple sensors in the operational scenario, and calibrate the 4D millimeter-wave radar, lidar, and inertial measurement unit (IMU); using the IMU coordinate system as the reference coordinate system, obtain the fixed coordinate transformation matrices between the 4D millimeter-wave radar and lidar, and between lidar and IMU; calibrate sensor clock drift based on the second pulse (PPS), and unify the time reference of all sensor data acquisitions using Unix timestamps to ensure that the millimeter-wave point cloud and the lidar point cloud correspond at the same physical moment; transform both the millimeter-wave point cloud and the lidar point cloud into the IMU coordinate system to obtain IMU data, achieving spatial dimension unification.
[0041] Step 2: Calculate the average Euclidean distance of the neighborhood of each laser point in the current frame's laser point cloud. Global mean and standard deviation A dynamic radius factor and spatial scale normalization strategy is adopted, based on the horizontal distance between the laser point and the center of the sensor. Adaptive calculation yields dynamic threshold To adapt to point cloud distributions in different spatial locations and avoid the limitations of fixed thresholds;
[0042] The detection range of the lidar is divided into multiple shell-like intervals according to a set horizontal distance, and the point cloud height is limited according to the actual scene height. The point cloud density within each shell is calculated to obtain a discrete density sequence. Spline fitting is performed on the discrete density sequence to obtain the first function of density with respect to distance. The derivative of the first function is used to obtain the distance at which the point cloud density decreases the fastest. The distance at which the point cloud density decreases by a set multiple exceeds the normal attenuation rate is used as the distance threshold. The median intensity of the laser point cloud in the current frame is used as the intensity threshold. ;
[0043] K-ary trees are used to quickly obtain neighborhood information of laser points, retaining those that simultaneously satisfy the condition that the average neighborhood distance is less than a dynamic threshold. Horizontal distance not greater than the distance threshold Strength not less than the strength threshold Furthermore, laser points belonging to the real target area identified by millimeter-wave radar are coupled and denoised to obtain a structured denoised point cloud. The coupling denoising process is as follows: Figure 2 As shown.
[0044] Specifically, the expression for coupling denoising is:
[0045]
[0046] in, To denoise point clouds, Radar point cloud; The average neighborhood distance of the point. For dynamic thresholds; The horizontal distance between the point and the lidar. Distance threshold; The intensity of the laser point, Intensity threshold; Preserve prior knowledge for millimeter waves; i is the point number.
[0047] The dynamic threshold ensures local density in the point cloud, conforming to the structure of the real environment. The distance threshold distinguishes between the natural decay of real-world point clouds and density abrupt changes caused by noise points, thus filtering out noise points that exceed a reasonable range. The intensity threshold utilizes the intensity difference between real-world points and noise points to assist in screening effective points and excluding low-intensity dust, rain, and snow noise points.
[0048] 4D millimeter-wave radar can stably detect real targets in degraded environments, but its resolution is low. Therefore, its core function is to provide high-confidence, non-removable priors for laser point clouds. The target detection results from the millimeter-wave radar are used to verify that the laser points are real environmental points, avoiding accidental deletion. Specifically, the millimeter-wave point cloud is first pre-processed with filtering. The advantages of millimeter-wave radar are used to comprehensively evaluate its intensity and velocity information, filtering out high-confidence millimeter-wave point clouds, thus determining the spatial location of the real target. These high-confidence millimeter-wave point clouds are then mapped to the IMU coordinate system and used as prior labels for the laser point cloud. The laser point representing that spatial location should be a real environmental point and cannot be filtered out.
[0049] This invention addresses the problem of traditional fixed-parameter filtering being unable to adapt to complex scenarios by using shell density fitting and dynamic thresholding; it compensates for the noise sensitivity of lidar by utilizing the all-day effectiveness of millimeter-wave radar, and improves prior reliability through both intensity and velocity information; it features a low false deletion rate: the introduction of high-confidence millimeter-wave priors avoids the problem of falsely deleting real environmental points in high-noise-density areas; and it ensures that the geometric structure of the environment is preserved in the denoised point cloud by filtering based on local neighborhood features and density attenuation patterns, providing high-quality data for subsequent GICP matching and localization.
[0050] Step 3: Denoise the point cloud Multiple sets of IMU data within their corresponding timestamps are used as a frame of the odometry system. The system state of the odometry system is propagated using the midpoint method based on IMU kinematics, and the covariance of the system state is propagated synchronously. The odometry system's state is estimated based on the previous moment's state. covariance Obtain the state prediction for the next time step and the uncertainty of forecasts Furthermore, the error state variables are propagated according to ESKF. The covariance matrix of the system state is mathematically strictly equivalent to the covariance matrix of the system error state.
[0051] Specifically, the system state of the odometry system is propagated using the midpoint method based on IMU kinematics. In a stationary state, the average of the IMU's angular velocity and acceleration is used as the initial bias for IMU measurements; the initial position of the IMU is taken as the origin of the world coordinate system, and a specific timestamp is used... That is, taking the previous moment as the prediction starting point, the IMU is calculated using the midpoint method based on the IMU kinematics. Timestamp, i.e., the prediction of the rotation matrix relative to the world coordinate system at the next moment. and translation vector prediction Wait for system status.
[0052] Furthermore, this invention propagates the covariance matrix of all error states using an adaptive error state Kalman filter (ESKF), specifically in the following propagation form:
[0053]
[0054] in, Let Jacobian matrix be the propagation matrix for error state. Let Jacobian matrix be the propagation matrix of the noise state. For the noise matrix, Let be the covariance matrix of the previous time step; if the previous time step is the system starting point, then initialize . Low noise; This is for predicting the covariance of the system state at the next time step.
[0055] A second-order compensation term was added to the angle error state propagation, therefore the Jacobian matrix for angle covariance propagation is:
[0056]
[0057] in, and These are Jacobian matrices. Jacobian blocks with respect to angle and Jacobian blocks with respect to angular velocity offset. This represents the negative rotation increment during IMU propagation. For noise variables in state propagation, It is the identity matrix. For time intervals, This is the state quantity of angular error. and These are all second-order compensation terms, which introduce additional noise into the propagation of the uncertainty of the rotation error state during propagation. Therefore, the angle noise matrix is corrected as follows:
[0058]
[0059] in, This refers to the angular noise block in the noise matrix. To correct the noise, For correction factor, As a regulating factor, The 2-norm of the angular velocity difference. For time intervals.
[0060] Furthermore, this invention uses the K-Nearest Neighbor (KNN) classification algorithm to calculate the local normal vector and covariance matrix of the denoised point cloud, searches for the set of neighboring points of the source point in a frame of laser point cloud, and calculates the initial covariance matrix. It describes the standard unbiased covariance estimate of the geometric distribution of a point in its neighborhood, and uses the SVD method to perform eigenvalue decomposition to obtain the eigenvalues of the covariance matrix. A structural degradation metric is used to introduce isotropic compensation to prevent singularities in the covariance matrix.
[0061]
[0062] in Let the local covariance of the source point be... for eigenvalues, To compensate for the post-covariance; For perceptual structure indicators. For structural threshold, These are the weighting coefficients. Based on this, the compensated covariance is weighted again, and the intensity confidence function, weighted by the point cloud intensity, is as follows:
[0063]
[0064] in These are weighting coefficients. For adjustment coefficients, This represents the laser detection intensity value at the current point. This represents the average intensity value of the laser point cloud in that frame. The optimized covariance, weighted by points, can be used for direct GICP registration.
[0065] Step 4: For denoising point clouds For each laser point, search for its nearest IMU state based on its timestamp; predict the state propagation based on the IMU corresponding to that timestamp. As a priori, the coordinates of the laser point are mapped from the lidar coordinate system at that moment to the state prediction. Aligned and denoised point cloud in the IMU coordinate system at the corresponding time point Using IMU data, we completed the noise reduction of the cloud. The distortion correction process.
[0066] Step 5: Calculate the denoised point cloud The mean and intensity-weighted covariance are used to calculate the residuals that match GICP based on the state prediction at the next time step. The GICP residuals and the error state variables obtained in step 3 are used together as optimization variables to construct the least squares cost function. The least squares cost function is then solved iteratively using the Gauss-Newton method to obtain the optimal environmental point cloud.
[0067] Specifically, the least squares cost function is as follows:
[0068]
[0069] in, The optimization objective is weighted least squares cost. To obtain a laser point set with appropriate covariance information, For the matching residual of points, The weighted covariance of the optimized points in step 3. To propagate residuals to the IMU.
[0070] The weighted covariance of the GICP residual and the point cloud acts on the rotation, translation, and velocity components of the state variables; the error state and error state covariance of the adaptive ESKF act on the rotation, translation, velocity, angular velocity bias, acceleration bias, and gravity components of the state variables.
[0071] Based on Gauss-Newton's method, we construct a function about the corrected state using the covariance of two types of residuals and the Jacobian block of the residuals with respect to the state variables. Hessian matrix With observation vector And solve the normal equation Iterate continuously until the correct state is reached. Convergence, Correction for And use ESKF to reset the Hessian matrix from the last iteration. Update covariance as .in This is the standard reset matrix for the ESKF process. The processes of propagation, distortion correction, correction, and updating described above are as follows: Figure 3 As shown.
[0072] Experiments have verified that the laser point cloud inertial odometry method based on 4D millimeter-wave radar compensation and denoising achieves an average absolute trajectory error of 0.386m in degraded scenarios. The method provided by this invention was deployed on an automated shovel and loader platform in an extreme dust environment, a scenario characterized by significant noise and dust, corresponding to ring-shaped low-intensity noise points in the laser radar detection point cloud. After applying 4D millimeter-wave radar compensation and denoising, a clean, structured point cloud was obtained, leading to an accurate map of the operational environment.
[0073] In summary, the above are merely preferred embodiments of the present invention and are not intended to limit the scope of protection of the present invention. 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 method for 4D millimeter-wave radar-based compensation denoising of laser point cloud inertial odometry, characterized in that, Specifically, the following steps are included: Step 1: Calibrate multiple sensors, acquire laser point cloud and 4D millimeter wave point cloud of the surrounding environment of the work scene, record inertial measurement unit (IMU) data, and perform spatiotemporal alignment of laser point cloud and 4D millimeter wave point cloud. Step 2: Acquire the laser point cloud of the current frame. Calculate the dynamic threshold based on the horizontal distance between the laser points and the sensor center. Use the distance at which the point cloud density decreases beyond the threshold as the distance threshold, and the median intensity of the laser point cloud in the current frame as the intensity threshold. Use millimeter-wave radar to identify and determine the real target area. Use a K-ary tree to quickly acquire the neighborhood information of the laser points. Retain laser points that simultaneously meet the following conditions: average neighborhood distance less than the dynamic threshold, horizontal distance not greater than the distance threshold, intensity not less than the intensity threshold, and belonging to the real target area identified by the millimeter-wave radar. This yields a structured, denoised point cloud. ; Step 3: Denoise the point cloud The IMU data within the corresponding timestamp is taken as a frame of the odometer system. The system state of the odometer system is propagated using the midpoint method, and the covariance of the system state is propagated synchronously. Based on the state estimate of the previous time step... Covariance Obtain the state prediction for the next time step and the uncertainty of forecasts Propagate error state variables; then denoise the point cloud. Distortion removal; Step 4: Calculate the denoised point cloud The mean and intensity-weighted covariance are used to calculate the residuals that match the GICP based on the state prediction at the next time step. The GICP residuals and the error state variables obtained in step 3 are used together as optimization variables to construct the least squares cost function. The least squares cost function is solved iteratively to obtain the optimal pose estimate.
2. The laser point cloud inertial odometry method according to claim 1, characterized in that, The distance threshold is calculated as follows: the detection range of the lidar is divided into multiple shell intervals according to a set horizontal distance interval, and the point cloud height is limited according to the actual scene height; the point cloud density in each shell is calculated to obtain a discrete density sequence; spline fitting is performed on the discrete density sequence to obtain the first function of density with respect to distance, the derivative of the first function is obtained to obtain the distance at which the point cloud density decreases the fastest, and the distance at which the point cloud density decreases by a set multiple exceeds the normal attenuation rate is taken as the distance threshold.
3. The laser point cloud inertial odometry method according to claim 1, characterized in that, The method for identifying and determining the real target area by millimeter-wave radar is as follows: the millimeter-wave point cloud is pre-processed by filtering, and high-confidence millimeter-wave point clouds are selected based on intensity and velocity; these high-confidence millimeter-wave point clouds are mapped to the IMU coordinate system and used as prior labels for the laser point cloud. The laser points representing the spatial location should be real environmental points that cannot be filtered out, which are the real target areas.
4. The laser point cloud inertial odometry method according to claim 1, characterized in that, The method of using the midpoint method to propagate the system state of the odometry system is as follows: In the stationary state, the average value of the IMU's angular velocity and acceleration is used as the initial bias of the IMU measurement; with the initial position of the IMU as the origin of the world coordinate system, and the previous moment as the prediction starting point, the rotation matrix and translation vector of the IMU relative to the world coordinate system at the next moment are calculated using the midpoint method based on the IMU kinematics.
5. The laser point cloud inertial odometry method according to claim 1, characterized in that, The method for synchronously propagating the covariance of the system state is to use an adaptive error state Kalman filter to propagate the covariance of all error states, specifically: ， in, Let Jacobian matrix be the propagation matrix for error state. Let Jacobian matrix be the propagation matrix of the noise state. For the noise matrix, Let covariance be the value at the previous time step. This is for predicting the covariance of the system state at the next time step. The Jacobian matrix for angular covariance propagation is: ， in, and These are Jacobian matrices. Jacobian blocks with respect to angle and Jacobian blocks with respect to angular velocity offset. This represents the negative rotation increment during IMU propagation. For noise variables in state propagation, It is the identity matrix. For time intervals, This refers to the angular error state quantity; and All are second-order compensation terms; The angle noise matrix is: ， in, This refers to the angular noise block in the noise matrix. To correct the noise, For correction factor, As a regulating factor, The 2-norm of the angular velocity difference. For time intervals.
6. The laser point cloud inertial odometry method according to claim 1, characterized in that, The K-nearest neighbor classification algorithm is used to calculate the local normal vector and covariance matrix of the denoised point cloud. The covariance matrix is calculated by searching the set of neighborhood points of the source point in a frame of laser point cloud. Specifically: ， in Let the local covariance of the source point be... All eigenvalues, To compensate for the post-covariance; For perceptual structure indicators. For structural threshold, These are weighting coefficients; The intensity confidence function based on the covariance weighting of point cloud intensity is as follows: ， in These are weighting coefficients. For adjustment coefficients, This represents the laser detection intensity value at the current point. This represents the average intensity value of the laser point cloud in that frame. The optimized covariance, weighted by points, can be used for direct GICP registration.
7. The laser point cloud inertial odometry method according to claim 1, characterized in that, The least squares cost function is: ， in, The optimization objective is weighted least squares cost. To obtain a laser point set with appropriate covariance information, For the matching residual of points, The weighted covariance of the corresponding points. The IMU propagation residual is the distance from the previous time step to the next time step.
8. The laser point cloud inertial odometry method according to claim 7, characterized in that, The method for iteratively solving the least squares cost function to obtain the optimal environmental point cloud is as follows: Based on the Gauss-Newton method, the Hessian matrix and observation vector are constructed using the covariance of the matched residuals and propagation residuals, and the Jacobian blocks of the residuals with respect to the state variables, to solve the normal equations. Iterate until the corrected state is reached. Convergence, Correction for And use ESKF to reset the Hessian matrix from the last iteration. The covariance of the system state at the next moment after the update is .