A method to improve the robustness of laser odometry positioning

Abstract

The present invention discloses a method for improving the robustness of laser odometry positioning. The method comprises the following steps: calculating the curvature of a fitted plane of a neighborhood point set of each target point cloud and converting the curvature into a weight factor through exponential mapping; adaptively adjusting the weights of point-to-point residuals and point-to-surface residuals through the weight factor; constructing a curvature adaptive function; and significantly improving the accuracy of laser odometry positioning through a curvature adaptive residual fusion mechanism. The method also introduces a Hessian matrix eigenvalue analysis method to eliminate the reliance of traditional detection on heuristic thresholds. Based on rotation condition factors and translation condition factors, and according to the pose results of a visual-inertial odometry, a weighted fusion compensation method is used to alleviate the problem of decreased translation and rotation estimation performance of the laser odometry, thereby improving the positioning capability of the laser odometry, especially the pose estimation accuracy in scenes with sparse geometric features, such as long corridors.

Description

Technical Field

[0001] The present invention relates to autonomous driving and robot positioning and navigation technology, and in particular to a method for improving the positioning robustness of a laser odometer. Background Art

[0002] In recent years, breakthroughs in 3D simultaneous localization and mapping (SLAM) technology have revolutionized mobile robotics in environmental perception, autonomous navigation, and scene modeling. By integrating multimodal sensor data with advanced computational frameworks, modern SLAM systems have become key enabling technologies for scenarios such as industrial inspections, unmanned delivery, and complex terrain exploration. However, practical deployments still face significant challenges due to limitations in the inherent physical properties of sensors, the scarcity of environmental features, and significant dynamic interference.

[0003] LiDAR is widely used in SLAM technology due to its high measurement accuracy and its independence from lighting conditions. However, there are still some inherent limitations in practical applications. The iterative closest point algorithm used in traditional laser odometry usually relies on only a single type of residual term, such as point-to-point residual or point-to-plane residual. This design leads to two key defects: point-to-point residual is stable in unstructured scenes, but because it ignores geometric structure constraints, the positioning accuracy is significantly reduced in highly structured scenes; while point-to-plane residual can utilize planar features to improve the accuracy of structured environments, it is heavily dependent on the quality of the normal vector. Using only a single residual term will cause large deviations in the pose estimation of the laser odometry, especially in long corridor scenes, resulting in a significant decline in the positioning performance of the laser odometry.

[0004] Secondly, when determining the degradation of laser odometry positioning performance, existing methods often analyze the reliability of laser odometry based on the eigenvalues ​​of the Hessian matrix. However, this method has significant limitations: the eigenvalue thresholds are difficult to accurately measure. Currently, thresholds are generally set based on experience, which seriously affects the reliability and accuracy of detection. Existing solutions lack effective countermeasures to the poor positioning performance of laser odometry, especially those that fail to fully integrate sensors such as vision to form a collaborative optimization framework. This shortcoming directly restricts the positioning capabilities of laser odometry. Summary of the Invention

[0005] Purpose of the invention: The purpose of the present invention is to provide a method for improving the robustness of laser odometry positioning for real-time perception and dynamic compensation in long corridor scenes.

[0006] Technical solution: The method for improving the robustness of laser odometer positioning according to the present invention comprises the following steps:

[0007] (1) According to the covariance matrix of the neighborhood point set of the target point cloud, the curvature of the fitting plane of the neighborhood point set of the target point cloud is determined, and the curvature is converted into a weight factor through exponential mapping; based on the point-to-point residual from the source point cloud to the target point cloud, the point-to-surface residual from the source point cloud to the fitting plane of the neighborhood point set of the target point cloud and the weight factor, a curvature adaptive cost function is constructed, the curvature adaptive cost function is solved, and the incremental rotation matrix is ​​determined and incremental translation vector , get the translation estimation vector from the lidar coordinate system to the map coordinate system and the rotation estimation matrix ;

[0008] (2) The Jacobian matrix of the point-to-point residual from the source point cloud to the target point cloud and the Jacobian matrix of the point-to-surface residual from the source point cloud to the target point cloud neighborhood point set fitting plane are obtained to obtain the global Jacobian matrix, thereby approximating the Hessian matrix. According to the Hessian matrix, the conditional factors are determined, which include translation conditional factors and rotation conditional factors.

[0009] (3) According to the conditional factors and their entropy values, the dynamic anomaly score of the corresponding conditional factors is determined, the maximum inter-class variance is calculated to determine the threshold of the abnormal dynamic anomaly score, and the degradation of the translation estimation performance and the rotation estimation performance of the laser odometry are detected;

[0010] (4) According to the results of the translation estimation performance degradation and the rotation estimation performance degradation of the laser odometry, based on the rotation condition factor and / or the translation condition factor, according to the pose result of the visual inertial odometry, the translation and / or rotation estimation of the laser odometry after compensation is determined, and the current pose estimation matrix estimated by the laser odometry is obtained.

[0011] Furthermore, in step (1), the covariance matrix of the neighborhood point set of the target point cloud is

[0012] ;

[0013] in, is the i-th target point cloud The covariance matrix of the neighborhood point set, is the i-th target point cloud The number of neighboring points, represents the coordinate vector of the kth point in the neighborhood, Represents the mean coordinate vector of the neighborhood point set;

[0014] The i-th target point cloud Curvature of the plane fitted by the neighborhood point set for

[0015] ;

[0016] ;

[0017] in, is the covariance matrix Perform eigenvalue decomposition and obtain the eigenvalues.

[0018] Furthermore, in step (1), the weight factor for

[0019] ;

[0020] in, is the curvature sensitivity parameter, .

[0021] Furthermore, in step (1), the point-to-point residual from the i-th source point cloud to the i-th target point cloud is for

[0022] ;

[0023] The point-to-surface residual of the plane fitted from the source point cloud to the neighborhood point set of the target point cloud for

[0024] ;

[0025] in, is the i-th target point cloud The unit normal vector of the plane fitted by the neighborhood point set, represents the incremental rotation matrix to be solved, represents the incremental translation vector to be solved; is the i-th target point cloud The three-dimensional coordinate vector of is the i-th source point cloud The three-dimensional coordinate vector of

[0026] Curvature adaptive cost function for

[0027] ;

[0028] Where N is the number of correspondences between the source point cloud and the target point cloud.

[0029] Furthermore, in step (2), the conditional factors are determined according to the Hessian matrix, as follows:

[0030] According to the rotation and translation relationship, the Hessian matrix is ​​divided into four sub-matrices, which are expressed as follows: is a submatrix that only contains information related to rotation and is used to analyze the problem of rotation estimation performance degradation; is a submatrix that only contains translation-related information and is used to analyze the problem of translation estimation performance degradation;

[0031] Approximate solution using power iteration method Maximum eigenvalue , using the inverse power iteration method to approximate the solution Minimum eigenvalue , calculate the rotation condition factor ;

[0032] Similarly, calculate the translation condition factor ,in, and Respectively The maximum and minimum eigenvalues ​​of .

[0033] Furthermore, in step (3), the dynamic anomaly score of the translation condition factor for

[0034] ;

[0035] ;

[0036] ;

[0037] ;

[0038] in, is the abnormal score of the translation condition factor; is the entropy enhancement coefficient, ; Entropy of the translation condition factor; is the average path length of the translation condition factor; represents the normalization factor; is the length of the translation condition factor in the binary tree; z is the number of binary trees; The translation condition factor value solved by the power iteration method; is the data density of the translation condition factor in each part within the window.

[0039] Furthermore, in step (3), when the latest translation condition factor dynamic anomaly score in the window When , the translation condition factor is abnormal and the translation estimation performance of the laser odometry is degraded.

[0040] Furthermore, in step (4), the translation estimation of the laser odometry after compensation is for

[0041] ;

[0042] in, Represents the translation estimate vector from the lidar coordinate system obtained by the visual inertial odometry to the map coordinate system, Represents the estimated translation vector from the compensated lidar coordinate system to the map coordinate system.

[0043] Furthermore, in step (4), the rotation estimation matrix from the laser radar coordinate system to the map coordinate system after compensation is Using quaternions express, for

[0044] ;

[0045] in, Represents the rotation estimation matrix from the lidar coordinate system obtained by the visual inertial odometry to the map coordinate system The quaternion of the rotation.

[0046] Furthermore, in step (4), if only the translation condition factor is abnormal, the rotation matrix from the lidar coordinate system to the map coordinate system is estimated And the translation estimation vector from the compensated lidar coordinate system to the map coordinate system , then the pose estimation matrix of the laser odometry is Re-expressed as: ;

[0047] If only the rotation condition factor is abnormal, the translation vector from the lidar coordinate system to the map coordinate system is estimated And the rotation estimation matrix from the compensated lidar coordinate system to the map coordinate system , then the pose estimation matrix of the laser odometry is Re-expressed as ;

[0048] If both the translation condition factor and the rotation condition factor are abnormal, the rotation matrix from the compensated lidar coordinate system to the map coordinate system is estimated and the translation estimate vector from the lidar coordinate system to the map coordinate system , then the pose estimation matrix of the laser odometry is Re-expressed as: .

[0049] Beneficial effects: Compared with the prior art, the present invention has the following significant advantages: 1. Based on the rotation condition factor and the translation condition factor, the present invention alleviates the estimation performance degradation problem and the rotation estimation performance degradation problem of the laser odometry to a certain extent through a weighted fusion compensation method according to the posture results of the visual inertial odometry, thereby improving the positioning capability of the laser odometry; 2. The present invention significantly improves the accuracy of laser odometry positioning through a curvature adaptive residual fusion mechanism; improves the Hessian matrix eigenvalue analysis method, eliminates the traditional detection dependence on the heuristic threshold; integrates the visual inertial odometry data to compensate for the rotation estimation performance degradation and the translation estimation performance degradation in real time, thereby further improving the positioning accuracy of the laser odometry. BRIEF DESCRIPTION OF THE DRAWINGS

[0050] Figure 1 This is a flow chart of the improved iterative closest point algorithm of the present invention;

[0051] Figure 2 A flowchart of detecting and compensating for degradation in the rotation estimation / translation estimation performance of a laser odometry according to the present invention;

[0052] Figure 3 is a schematic diagram of the average rotation error;

[0053] Figure 4 Schematic diagram of the average translation error. DETAILED DESCRIPTION

[0054] The present invention will be further described below with reference to the accompanying drawings.

[0055] The method for improving the robustness of laser odometer positioning according to the present invention comprises the following steps:

[0056] (1) Determine the pose estimation matrix of the laser odometry.

[0057] (11) Offline calibrate the external parameters of the lidar and IMU, and the lidar and vision sensor to obtain the external parameter matrix from IMU to lidar and the external parameter matrix from vision sensor to lidar.

[0058] (12) Use the offline calibrated IMU to laser radar external parameter matrix to convert the relative pose change between frames measured by the IMU to the radar coordinate system, and then use the pose estimation matrix of the laser odometer of the previous frame to obtain the initial pose estimation matrix of the laser odometer in the current frame. , Represents the initial rotation estimate matrix from the lidar coordinate system to the map coordinate system, Represents the initial translation estimate vector from the lidar coordinate system to the map coordinate system.

[0059] (13) Perform motion distortion correction on the source point cloud of the current frame laser radar scan, and use the initial pose estimation matrix The corrected source point cloud is transformed into the map coordinate system, and then the corresponding target point cloud is found through nearest neighbor search.

[0060] (14) Dynamically set the search radius according to the spatial distribution characteristics of the target point cloud, and count each target point cloud Number of neighboring points Calculate the distance between each target point cloud and the lidar sensor , dynamically set the search radius based on the distance between the target point cloud and the lidar , represents the basic search radius, Represents the distance coefficient.

[0061] (15) For each target point cloud The covariance matrix of the neighborhood point set composed of adjacent points is calculated: , represents the coordinate vector of the kth point in the neighborhood, Represents the mean coordinate vector of the neighborhood point set; perform eigenvalue decomposition on the covariance matrix to obtain ( ), then the target point cloud The curvature of the fitted plane of the neighborhood point set is expressed as .

[0062] (16) Calculate the weight factor: Use the exponential mapping to transform the target point cloud The curvature of the plane fitted by the neighborhood point set Convert to weight factor : , is the curvature sensitivity parameter, controlling the weight Follow Sensitivity to change.

[0063] (17) Point-to-point residual represents the source point After transformation and target point The Euclidean distance between: ; Point-to-surface residuals represent source points After transformation and target point The perpendicular distance of the fitting plane of the neighborhood point set: ,in is the target point The unit normal vector of the plane fitted by the neighborhood point set, Indicates the source point The three-dimensional coordinate vector of Indicates the target point The three-dimensional coordinate vector of represents the incremental rotation matrix to be solved, Represents the incremental translation vector to be solved.

[0064] (18) The curvature adaptive cost function is constructed by the point-to-point residual, point-to-surface residual and weight factor: , where N represents the number of corresponding relationships between the source point cloud and the target point cloud, .

[0065] (19) Solve the curvature adaptive cost function.

[0066] Point-by-point residual linearization: , get the corresponding Jacobian matrix: ,but can be re-expressed as ,in, , , , .

[0067] Point-to-surface residual linearization , and get the corresponding Jacobian matrix: ,but can be re-expressed as , Then the original curvature adaptive cost function can be linearized and rewritten as: ; Solve to get .

[0068] The estimated rotation matrix from the new lidar coordinate system to the map coordinate system and the translation estimate vector , get the new laser odometry pose estimation matrix .

[0069] (2) Analysis of the rotation and translation constraint strength of the laser odometry.

[0070] (21) The Jacobian matrix of a single point cloud is obtained by combining the Jacobian matrix of the point-to-point residual and the Jacobian matrix of the point-to-surface residual of each source point cloud. , the global Jacobian matrix is ​​constructed by stacking all points .

[0071] (22) The Hessian matrix can be approximated based on the global Jacobian matrix , It can be divided into four sub-matrices according to rotation and translation, expressed as The method of splitting the Hessian matrix into four sub-matrices based on rotation and translation is described in T. Tuna, J. Nubert, Y. Nava, S. Khattak and M. Hutter, "X-ICP: Localizability-Aware LiDAR Registration for Robust Localization in Extreme Environments," in IEEE Transactions on Robotics, vol. 40, pp. 452-471, 2024, doi: 10.1109 / TRO.2023.3335691.

[0072] (23) Among them The submatrix contains only information related to rotation and is used to analyze the problem of rotation estimation performance degradation: ; Contains only translation-related information, used to analyze translation estimation performance degradation issues: .

[0073] (24) Use the power iteration method to approximate Maximum eigenvalue , choose an initial non-zero vector ,calculate ,get and normalized , the eigenvalue estimate for the first iteration ; ,get and normalized , the eigenvalue estimate of the second iteration Repeat the above process until the and , or to the maximum number of iterations m; approximate calculation .

[0074] (25) Use the inverse power iteration method to approximate Minimum eigenvalue . Choose an initial non-zero vector , add regularization term ,calculate ,get and normalized , the eigenvalue estimate for the first iteration ; ,get and normalized , the eigenvalue estimate of the second iteration Repeat the above process until the and , or the maximum number of iterations m is reached; approximate calculation .

[0075] (26) Calculate the rotation condition factor , in order to prevent Too small will cause overflow. , then directly order .

[0076] (27) Repeat steps (24) and (25) to calculate the translation condition factor , in order to prevent Too small will cause overflow, if , then directly order .

[0077] (3) Detection of degradation in laser odometry positioning performance.

[0078] (31) The laser odometry is run in a scene with rich geometric features to collect translation condition factors and rotation condition factors as a stable reference benchmark. When entering a long corridor scene, the anomalies of the translation condition factors and rotation condition factors are detected in real time.

[0079] (32) A sliding window is used to store n translation condition factors (the earliest data is removed when new data is added), and anomalies are detected using an improved isolation forest algorithm.

[0080] For n translation condition factors in the window, starting from the root node, randomly select a data point each time as the split value to divide the data into two parts. Repeat this process until there is only one data point left in each subinterval or the preset maximum tree depth is reached, and finally a binary tree is formed. Calculate the length of the translation condition factor in the binary tree .

[0081] Repeat the above process for each translation condition factor to construct z binary trees and calculate the average path length of each translation condition factor .

[0082] Calculate the anomaly score for each translation condition factor , Represents the normalization factor .

[0083] Divide the n translation condition factor data into v parts and calculate the data density in each part ; Get the entropy value of the translation condition factor .

[0084] Adopting a dynamic anomaly score based on entropy for each translation condition factor ( represents the entropy enhancement coefficient), The value of the translation condition factor for the power iteration solution.

[0085] The maximum inter-class variance is calculated using the Otsu algorithm to determine the threshold value of the abnormality of the dynamic anomaly score of the translation condition factor. , when the latest translation condition factor dynamic anomaly score in the window When , the translation condition factor is judged to be abnormal, and the translation estimation performance of the laser odometry is degraded.

[0086] (33) Repeat step (32) and use the Otsu algorithm to determine the threshold value of dynamic outlier anomaly ; When the latest rotation condition factor dynamic outlier in the window When , the rotation estimation performance of the laser odometry degrades.

[0087] (4) Vision-lidar cross-modal posture correction.

[0088] According to the pose estimation matrix of the visual inertial odometry in the visual sensor coordinate system , and then use the external parameter matrix of the visual sensor to the lidar sensor through offline calibration , the pose estimation matrix of the visual inertial odometry is expressed in the lidar coordinate system: , Represents the rotation estimation matrix from the lidar coordinate system obtained by the visual inertial odometry to the map coordinate system, Represents the translation estimate vector from the lidar coordinate system obtained by the visual inertial odometry to the map coordinate system.

[0089] When the translation condition factor is detected to be abnormal, it indicates that the translation estimation performance of the laser odometry in the current frame has degraded. Compensate for the degradation of translation estimation performance of laser odometry, , Represents the estimated translation vector from the compensated lidar coordinate system to the map coordinate system.

[0090] Estimated rotation matrix from the lidar coordinate system to the map coordinate system and the translation estimation vector from the compensated lidar coordinate system to the map coordinate system , then the pose estimation matrix of the laser odometry is Re-expressed as: .

[0091] When the rotation condition factor is detected to be abnormal, it indicates that the rotation estimation performance of the laser odometry in the current frame has degraded. Compensate for the degradation of the laser odometry's rotation estimation performance, expressed as a quaternion: , It is the rotation estimation matrix from the laser radar coordinate system to the map coordinate system after compensation The quaternion of corresponds to the quaternion of the rotation estimate matrix, corresponds to The quaternion of the rotation estimate matrix; (a unit quaternion ( ) can uniquely represent a rotation estimation matrix ) .

[0092] Estimate the translation vector from the lidar coordinate system to the map coordinate system And the rotation estimation matrix from the lidar coordinate system to the map coordinate after compensation , then the pose estimation matrix of the laser odometry is Re-expressed as: .

[0093] If the rotation estimation performance and translation estimation performance of the laser odometry in the current frame are both degraded, the rotation estimation matrix from the laser radar coordinate system to the map coordinate system after compensation is calculated. And the translation estimation vector from the lidar coordinate system to the map coordinate after compensation , then the pose estimation matrix of the laser odometry is Re-expressed as: .

[0094] To verify the effectiveness and rationality of the proposed method, we conducted a simulation experiment on Ubuntu 20.04 LTS, ROS1 Noetic, and Gazebo to verify the robustness of the proposed positioning method. The experimental scenario was a circular corridor approximately 90 meters long, using a mid360 lidar (with built-in IMU) and a T265 binocular camera (with built-in IMU).

Claims

1. A method for improving the robustness of laser odometry positioning, characterized in that: The following steps are involved: (1) According to the covariance matrix of the neighborhood point set of the target point cloud, the curvature of the fitting plane of the neighborhood point set of the target point cloud is determined, and the curvature is converted into a weight factor through exponential mapping; Based on the point-to-point residual from the source point cloud to the target point cloud, the point-to-surface residual from the source point cloud to the target point cloud neighborhood point set fitting plane and the weight factor, a curvature adaptive cost function is constructed, the curvature adaptive cost function is solved, and the incremental rotation matrix is ​​determined. and incremental translation vector , get the translation estimation vector from the lidar coordinate system to the map coordinate system and the rotation estimation matrix ; (2) The Jacobian matrix of the point-to-point residual from the source point cloud to the target point cloud and the Jacobian matrix of the point-to-surface residual from the source point cloud to the target point cloud neighborhood point set fitting plane are obtained to obtain the global Jacobian matrix, thereby approximating the Hessian matrix. According to the Hessian matrix, the conditional factors are determined, which include translation conditional factors and rotation conditional factors. (3) According to the conditional factors and their entropy values, the dynamic anomaly score of the corresponding conditional factors is determined, the maximum inter-class variance is calculated to determine the threshold of the abnormal dynamic anomaly score, and the degradation of the translation estimation performance and the rotation estimation performance of the laser odometry are detected; (4) According to the results of the translation estimation performance degradation and rotation estimation performance degradation of the laser odometry, based on the rotation condition factor and / or translation condition factor, according to the pose result of the visual inertial odometry, the translation and / or rotation estimation of the laser odometry after compensation is determined, and the new pose estimation matrix estimated by the laser odometry is obtained. .

2. The method for improving the robustness of laser odometry positioning according to claim 1, characterized in that: In step (1), the covariance matrix of the neighborhood point set of the target point cloud is ； in, is the i-th target point cloud The covariance matrix of the neighborhood point set, is the i-th target point cloud The number of neighboring points, represents the coordinate vector of the kth point in the neighborhood, Represents the mean coordinate vector of the neighborhood point set; The i-th target point cloud Curvature of the plane fitted by the neighborhood point set for ； ； in, is the covariance matrix Perform eigenvalue decomposition and obtain the eigenvalues.

3. The method for improving the robustness of laser odometry positioning according to claim 2, characterized in that: In step (1), the weight factor for ； in, is the curvature sensitivity parameter, .

4. The method for improving the robustness of laser odometry positioning according to claim 3, characterized in that: In step (1), the point-to-point residual from the i-th source point cloud to the i-th target point cloud is for ； The point-to-surface residual of the plane fitted from the source point cloud to the neighborhood point set of the target point cloud for ； in, is the i-th target point cloud The unit normal vector of the plane fitted by the neighborhood point set, represents the incremental rotation matrix to be solved, represents the incremental translation vector to be solved; is the i-th target point cloud The three-dimensional coordinate vector of is the i-th source point cloud The three-dimensional coordinate vector of Curvature adaptive cost function for ； Where N is the number of correspondences between the source point cloud and the target point cloud.

5. The method for improving the robustness of laser odometry positioning according to claim 4, characterized in that: In step (2), the conditional factors are determined according to the Hessian matrix, as follows: According to the rotation and translation relationship, the Hessian matrix is ​​divided into four sub-matrices, which are expressed as follows: is a submatrix that only contains information related to rotation and is used to analyze the problem of rotation estimation performance degradation; is a submatrix that only contains translation-related information and is used to analyze the problem of translation estimation performance degradation; Approximate solution using power iteration method Maximum eigenvalue , using the inverse power iteration method to approximate the solution Minimum eigenvalue , calculate the rotation condition factor ; Similarly, calculate the translation condition factor ,in, and Respectively The maximum and minimum eigenvalues ​​of .

6. The method for improving the robustness of laser odometry positioning according to claim 5, characterized in that: In step (3), the dynamic anomaly score of the translation condition factor for ； ； ； ； in, is the abnormal score of the translation condition factor; is the entropy enhancement coefficient, ; Entropy of the translation condition factor; is the average path length of the translation condition factor; represents the normalization factor; is the length of the translation condition factor in the binary tree; z is the number of binary trees; The translation condition factor value solved by the power iteration method; is the data density of the translation condition factor in each part within the window.

7. The method for improving the robustness of laser odometry positioning according to claim 6, characterized in that: In step (3), When the latest translation condition factor dynamic anomaly score in the window hour, is the threshold of the abnormal dynamic anomaly score of the translation condition factor. If the translation condition factor is abnormal, the translation estimation performance of the laser odometry will deteriorate.

8. The method for improving the robustness of laser odometry positioning according to claim 7, characterized in that: In step (4), the translation estimation of the laser odometry after compensation is for ； in, Represents the translation estimate vector from the current frame lidar coordinate system to the map coordinate system obtained by the visual inertial odometry; Represents the estimated translation vector from the compensated lidar coordinate system to the map coordinate system.

9. The method for improving the robustness of laser odometry positioning according to claim 8, characterized in that: In step (4), the rotation estimation matrix from the lidar coordinate system to the map coordinate system after compensation is Using quaternions express, for ； in, Represents the rotation estimation matrix from the current frame lidar coordinate system to the map coordinate system obtained by the visual inertial odometry The quaternion of the rotation; express The quaternion of the rotation.

10. The method for improving the robustness of laser odometry positioning according to claim 9, characterized in that: In step (4), if only the translation condition factor is abnormal, the rotation matrix from the lidar coordinate system to the map coordinate system is estimated And the translation estimation vector from the compensated lidar coordinate system to the map coordinate system , then the pose estimation matrix of the laser odometry is Re-expressed as: ; If only the rotation condition factor is abnormal, the translation vector from the lidar coordinate system to the map coordinate system is estimated And the rotation estimation matrix from the lidar coordinate system to the map coordinate system after compensation , then the pose estimation matrix of the laser odometry is Re-expressed as: ; If both the translation condition factor and the rotation condition factor are abnormal, the rotation matrix from the compensated lidar coordinate system to the map coordinate system is estimated And the translation estimation vector from the lidar coordinate system to the map coordinate system after compensation , then the pose estimation matrix of the laser odometry is Re-expressed as: .

Images