A laser radar-imu fusion positioning system and positioning method resistant to geometric perception degradation

Abstract

The application discloses a laser radar-IMU fusion positioning system and method against geometric perception degradation, and belongs to the technical field of laser radar measurement. The system comprises a preprocessing module, a geometric measurement correction module, an intensity information enhancement module, an iterative extended Kalman filtering module and a probabilistic planar voxel map construction module. The positioning method indicates the non-degradation direction of state optimization through a point cloud downsampling method based on "gradient flow" analysis, a geometric degradation detection method and a Tikhonov regularization technology; then a probabilistic adaptive voxel map is used to obtain more accurate geometric measurement and degradation detection results; when the above geometric measurement and complementary intensity measurement are available at the same time, a joint update of constraint iterative extended Kalman filtering is performed to complete state estimation. The method significantly improves the positioning accuracy and robustness in the geometric perception degradation scene, and promotes the application of mobile robots in complex challenge scenes such as tunnels, open terrains and caves.

Description

Technical Field

[0001] This invention relates to a lidar-IMU fusion positioning system and method resistant to geometric perception degradation, which belongs to the technical field of lidar measurement. Background Technology

[0002] LiDAR (Light Detection and Ranging) has become the dominant sensor in the current Simultaneous Localization and Mapping (SLAM) framework. Point cloud registration is the core component of the LiDAR-Inertial Odometry system. Feature extraction-based methods are classic algorithms. For example, LOAM[1] performs registration by extracting edge and planar features from the point cloud, while LEGO-LOAM[2] improves LOAM by introducing ground segmentation technology. However, pose estimation methods that rely solely on geometric feature extraction and matching are prone to failure in challenging environments or violent motion scenarios.

[0003] Integrating inertial measurement unit (IMU) data into lidar odometry can significantly improve the robustness and accuracy of positioning. LIO-SAM[3], as a mainstream optimization method, optimizes the system state by incorporating lidar odometry factors and IMU pre-integration factors[4] into the factor graph. However, such methods have the drawback of computational time consumption, so filtering-based methods are increasingly favored. FAST-LIO2[5] achieves industry-leading positioning accuracy and real-time performance by reconstructing the Kalman gain and using the original laser point cloud for registration. Similarly, Point-LIO[6] proposes a point-by-point processing LIO framework, which significantly improves the odometry output frequency and enhances robustness in violent motion. To obtain more accurate point cloud registration results, VoxelMAP[7] innovatively designed a probabilistic voxel map for environmental modeling. Although these methods improve robustness by integrating IMU data, lidar-inertial systems still experience drift or failure due to their reliance on lidar measurements with inherent geometric degradation characteristics.

[0004] LiDAR-based systems typically employ point cloud registration techniques to achieve accurate localization and dense environment reconstruction. However, these systems are not robust enough in degraded scenarios with structural self-similarity or sparse geometric features (such as long tunnels or feature-poor planar terrain), where the lack of informational orientation can lead to cumulative state estimation drift or complete localization failure.

[0005] To address these issues, extensive research has focused on systematically identifying informationless directions in state estimation and developing degradation-robust estimation frameworks. Current mitigation strategies primarily follow two paradigms: some methods address the problem by implementing constrained state estimation updates along the direction of geometric degradation, which involves detecting the direction of geometric feature degradation (such as unobservable translations in corridor structures) and imposing constraints on optimization variables. Other methods employ multi-source sensor fusion strategies to address the inherent geometric degradation problem in pure LiDAR systems by integrating complementary sensing modalities. However, most methods fail to synergistically integrate multiple degradation cancellation mechanisms, which limits positioning accuracy and robustness in certain degradation environments. Furthermore, redundant LiDAR points may generate invalid geometric residuals, potentially leading to drift in LiDAR-based real-time odometry.

[0006] To address the degradation problem of lidar-based odometry systems, various solutions have been proposed. Fusion of visual data with data from other sensors can enhance the robustness of SLAM systems, as shown in [8][9]

[10]

[11] . However, these methods rely on additional sensor configurations, increasing system complexity.

[0007] For lidar-inertial odometry (LIO) systems, some methods have improved robustness through geometric degradation sensing optimization or laser intensity information enhancement techniques. Degradation detection and constraint construction are key steps in geometric degradation sensing optimization. Zhang et al.

[12] proposed a degradation factor and de-mapping method to constrain state optimization. Based on this, Turcan et al. proposed the X-ICP algorithm

[13] , which uses the correspondence between the source point cloud and the target point cloud to detect degradation and adds hard constraints in the degradation direction to reduce pose estimation error. However, inaccurate geometric calibration limits the effectiveness of degradation detection and constraint construction. In addition, in extreme degradation scenarios, constraints relying solely on geometric information are insufficient. Therefore, fusing laser intensity information

[14]

[15] has become a feasible solution, but these methods generally suffer from inaccurate environmental characterization and geometric measurement, which limits the quality of geometric degradation detection, thereby weakening the utility of intensity information and ultimately damaging the robustness and positioning accuracy of the system.

[0008] [1]Zhang J, Singh S. LOAM: Lidar odometry and mapping in real-time[C] / / Robotics: Science and systems. 2014, 2(9): 1-9.

[0009] [2]T. Shan and B. Englot, "LeGO-LOAM: Lightweight and Ground-Optimized Lidar Odometry and Mapping on Variable Terrain," 2018 IEEE / RSJInternational Conference on Intelligent Robots and Systems (IROS), Madrid,Spain, 2018, pp. 4758-4765.

[0010] [3]Shan T, Englot B, Meyers D, et al. Lio-sam: Tightly-coupled lidarinertial odometry via smoothing and mapping[C] / / 2020 IEEE / RSJ internationalconference on intelligent robots and systems (IROS). IEEE, 2020: 5135-5142.

[0011] [4]C. Forster, L. Carlone, F. Dellaert and D. Scaramuzza, "On-Manifold Preintegration for Real-Time Visual--Inertial Odometry," in IEEETransactions on Robotics, vol. 33, no. 1, pp. 1-21, Feb. 2017.

[0012] [5]Xu W, Cai Y, He D, et al. Fast-lio2: Fast direct lidar-inertialodometry[J]. IEEE Transactions on Robotics, 2022, 38(4): 2053-2073.

[0013] [6]He D, Xu W, Chen N, et al. Point‐LIO: Robust High‐Bandwidth LightDetection and Ranging Inertial Odometry[J]. Advanced Intelligent Systems,2023, 5(7): 2200459.

[0014] [7]C. Yuan, W. Xu, X. Liu, X. Hong and F. Zhang, "Efficient andProbabilistic Adaptive Voxel Mapping for Accurate Online LiDAR Odometry," inIEEE Robotics and Automation Letters, vol. 7, no. 3, pp. 8518-8525, July2022.

[0015] [8]Zheng C, Zhu Q, Xu W, et al. Fast-livo: Fast and tightly-coupledsparse-direct lidar-inertial-visual odometry[C] / / 2022 IEEE / RSJ internationalconference on intelligent robots and systems (IROS). IEEE, 2022: 4003-4009.

[0016] [9]Jia Y, Luo H, Zhao F, et al. Lvio-fusion: A self-adaptive multi-sensor fusion slam framework using actor-critic method[C] / / 2021 IEEE / RSJinternational conference on intelligent robots and systems (IROS). IEEE,2021: 286-293.

[0017]

[10] Lv J, Lang X, Xu J, et al. Continuous-time fixed-lag smoothingfor lidar-inertial-camera slam[J]. IEEE / ASME Transactions on Mechatronics,2023, 28(4): 2259-2270.

[0018]

[11] Zhao S, Zhang H, Wang P, et al. Super odometry: Imu-centriclidar-visual-inertial estimator for challenging environments[C] / / 2021 IEEE / RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE,2021: 8729-8736.

[0019]

[12] Zhang J, Kaess M, Singh S. On degeneracy of optimization-basedstate estimation problems[C] / / 2016 IEEE international conference on roboticsand automation (ICRA). IEEE, 2016: 809-816.

[0020]

[13] Tuna T, Nubert J, Nava Y, et al. X-icp: Localizability-awarelidar registration for robust localization in extreme environments[J]. IEEETransactions on Robotics, 2023.

[0021]

[14] H. Li, B. Tian, ​​H. Shen and J. Lu, "An Intensity-Augmented LiDAR-Inertial SLAM for Solid-State LiDARs in Degenerated Environments," in IEEETransactions on Instrumentation and Measurement, vol. 71, pp. 1-10, 2022.

[0022]

[15] Y. Zhang et al., "RI-LIO: Reflectivity Image Assisted Tightly-Coupled LiDAR-Inertial Odometry," in IEEE Robotics and Automation Letters, vol. 8, no. 3, pp. 1802-1809, March 2023. Summary of the Invention

[0023] To address the problems of positioning failure and insufficient accuracy in existing lidar-inertial odometry systems in scenarios such as tunnels, long corridors, and open spaces, this invention proposes a lidar-inertial odometry system based on geometric-intensity complementary fusion and degradation constraint.

[0024] The technical solution adopted in this invention is: a lidar-IMU fusion positioning system that resists geometric perception degradation, the system including a preprocessing module, a geometric measurement correction module, an intensity information enhancement module, an iterative extended Kalman filter module, and a probabilistic planar voxel map construction module;

[0025] The preprocessing module processes the raw IMU measurement data and lidar scanning information based on prior state estimation, and generates a distortion-free point cloud through motion compensation.

[0026] The geometric measurement correction module uses the gradient flow entropy maximization criterion to select the LiDAR point cloud with the optimal information content for scanning-map registration, and then calculates the geometric residual, Jacobian matrix and detects the degradation direction. Based on this, it solves the Tikhonov regularization term to construct optimization constraints.

[0027] The intensity information enhancement module generates an intensity image along the direction of missing geometric information and extracts image features to construct a complementary photometric measurement.

[0028] The iterative extended Kalman filter module uses optimization constraints to jointly optimize geometric and photometric measurements in order to update the system state.

[0029] The probabilistic planar voxel map construction module uses a probabilistic adaptive voxel map to provide planar features, ensuring scan-map geometry measurement and degradation detection.

[0030] A localization method for a lidar-IMU fusion localization system resistant to geometric perception degradation, the method comprising the following steps:

[0031] S1, Based on the radar point cloud of the current frame IMU data between the previous frame point cloud and the current frame radar point cloud And the global geometric voxel map that the system has currently established. ; obtain State estimation results at time 1 and its covariance matrix ;

[0032] S2. Forward Propagation: Based on the IMU data between the previous frame point cloud and the current frame radar point cloud, perform the IMU forward propagation integration process to obtain the prior state estimation information of the current radar point cloud frame.

[0033] Backpropagation: Based on the IMU data between the previous frame point cloud and the current frame radar point cloud, perform the IMU backpropagation process to obtain the pose transformation relationship of all radar points in a frame point cloud up to the end of the frame scan. Transform all radar points in the frame point cloud to the end of the frame to obtain a distortion-free point cloud. ;

[0034] S3. Based on the spherical projection model, project the original point cloud into a panoramic lidar intensity image according to the radar scanning mode. ;

[0035] S4. Based on the entropy of the gradient flow, obtain the point cloud that provides information gain for frame-to-map matching from the distorted point cloud. ;

[0036] S5. Calculate the Jacobian matrix of the actual state relative to the error state.

[0037] Please complete the definitions of each parameter in the formula, and also complete the formulas marked below:

[0038] in: , , These represent the system state at time k and the k-th iteration, the system state error state at time k and the predicted system state at time k, respectively.

[0039] Update the covariance matrix of the current error state.

[0040] in: are the covariance matrices for predicting the system state at time k.

[0041] Using point clouds Based on the frame-to-map geometric measurement model, construct the geometric residuals and calculate their Jacobian matrix: ;

[0042] in: , These are the set measurement residuals and the corresponding Jacobian matrix, respectively.

[0043] Using the obtained Jacobian matrix, the geometric directions that degenerate during the state estimation process are detected. ;

[0044] in, The matrix consisting of the detected degradation directions is n. The matrix is ​​denoted by n, where n is the number of degenerate directions.

[0045] From the geometric direction of degradation Combined with the lidar intensity image in step S3 Generate image features with complementary observation properties. Next, calculate the intensity residuals of image features in adjacent lidar intensity images in the time series, and calculate the Jacobian matrix; ;

[0046] in, , These are the strength measurement residuals and the corresponding Jacobian matrix, respectively.

[0047] Combined geometric residuals and photometric residuals Construct the combined residual and Jacobian matrix ;in, , These are the combined measurement residuals and the corresponding Jacobian matrix, respectively; then, from the degenerate geometric direction... Construct an information correction matrix ;in, The degradation adjustment parameters are set manually; then the corrected Kalman gain is calculated.

[0048] Update the system state based on the corrected Kalman gain; determine whether the state estimation process has converged. If it has converged, exit the loop; otherwise, continue executing the loop process.

[0049] S6. Using the optimal system state estimate obtained in step 5, update the system state and covariance matrix, and output the odometer information.

[0050] S7. Update the probabilistic planar voxel map using the latest system status and the distortion-free point cloud.

[0051] Furthermore, in step S3, the panoramic lidar intensity image is processed to remove linear artifacts present in the image.

[0052] The beneficial effects of this invention: To address the problems of frequent positioning failures and insufficient positioning accuracy in existing lidar-inertial odometry systems in scenarios such as tunnels, long corridors, and open areas, this invention proposes a lidar-inertial odometry system based on geometric-intensity complementary fusion and degradation constraint restrictions. Unlike existing methods, the core innovation of this method lies in fusing a geometric measurement model with redundancy-degradation awareness with a complementary intensity image measurement model, and introducing geometric degradation constraints into the state estimation process within the Kalman filter framework. This mechanism ensures that the system state is always updated along the non-degradation direction, thereby significantly improving positioning accuracy and system robustness. Specifically, this paper proposes a novel geometric measurement model construction process, which uses a point cloud downsampling method based on gradient flow analysis, a geometric degradation detection method, and Tikhonov regularization to indicate the non-degenerate direction of state optimization. Secondly, the system adopts a probabilistic adaptive voxel map to obtain more accurate geometric measurement and degradation detection results. When the above geometric measurement and complementary strength measurement are available simultaneously, a joint update of constrained iterative extended Kalman filter (CIEKF) is performed to complete the state estimation.

[0053] Experimental results show that the present invention can significantly improve the positioning accuracy and robustness in geometrically degraded scenarios, and promote the application of mobile robots in complex and challenging scenarios such as tunnels, open terrain, and caves. Attached Figure Description

[0054] Figure 1 This is a flowchart of a lidar-IMU fusion localization method that resists geometric perception degradation.

[0055] Figure 2 This is a comparison of the mapping results of different methods on the RunwayS and IntersectionD sequences.

[0056] Figure 3 This is a comparison of different trajectories on the RunwayS sequence with the true values.

[0057] Figure 4 It is the result of comparing different trajectories with the true value on the IntersectionD sequence.

[0058] Figure 5 This is a comparison of the mapping results of the proposed method on all sequences in the Enwide dataset. Detailed Implementation

[0059] The present invention will be specifically described below through embodiments. It should be noted that the following embodiments are only used to further illustrate the present invention, but are not limited thereto, unless otherwise stated.

[0060] Example 1

[0061] Figure 1 This paper presents the proposed geometry-intensity fusion lidar inertial odometry (GIF-LIO) framework, which aims to improve the robustness and positioning accuracy of the system in geometrically degraded environments. The system has five core modules: (1) Preprocessing module: Based on prior state estimation, it processes the original IMU measurement data and lidar scanning information, and generates distortion-free point clouds through motion compensation; (2) Information refinement module: It uses the gradient flow entropy maximization criterion to screen the lidar point cloud with the optimal information content for scan-map registration, and then calculates the geometric residual, Jacobian matrix and detects the degradation direction. On this basis, it solves the Tikhonov regularization term to construct optimization constraints; (3) Information enhancement module: It generates intensity images along the direction of missing geometric information and extracts image features to construct complementary photometric measurements; (4) Iterative extended Kalman filter module: It uses optimization constraints to jointly optimize geometric measurements and photometric measurements to update the system state; (5) Probabilistic voxel mapping module: It uses probabilistic adaptive voxel maps to provide planar features to ensure accurate scan-map geometric measurements and degradation detection.

[0062] The detailed process for system state estimation is as follows:

[0063] S1, Input the radar point cloud of the current frame. IMU data between the previous frame point cloud and the current frame radar point cloud Input the currently established global geometry voxel map of the system. Get State estimation results at time 1 and its covariance matrix :

[0064] S2. Forward Propagation: Based on the IMU data between the previous frame point cloud and the current frame radar point cloud, perform the IMU forward propagation integration process to obtain the prior state estimation information of the current radar point cloud frame. .

[0065] Backpropagation: Based on the IMU data between the previous frame point cloud and the current frame radar point cloud, perform the IMU backpropagation process to obtain the pose transformation relationship of all radar points in a frame point cloud up to the end of the frame scan. Transform all radar points in the frame point cloud to the end of the frame to obtain a distortion-free point cloud. .

[0066] S3. Based on the spherical projection model, project the original point cloud into a panoramic lidar intensity image according to the radar scanning mode. The obtained spherical projection model is processed to remove linear artifacts in the image and improve image quality.

[0067] S4. Based on the entropy of the gradient flow, obtain the portion of the point cloud from the distortion-free point cloud that matches the information gain of the map. .

[0068] S5. Begin executing the iterative state update process:

[0069] Calculate the Jacobian matrix of the actual state with respect to the error state. ;

[0070] , These represent the system state at time k and the k-th iteration, the system state error state at time k and the predicted system state at time k, respectively.

[0071] Update the covariance matrix of the current error state. ;in, The covariance matrix for predicting the system state when k is given. This is achieved using the obtained point cloud. Based on the frame-to-map (point-to-plane) geometric measurement model, construct the geometric residuals and calculate their Jacobian matrix. ,in: , These are the set measurement residuals and the corresponding Jacobian matrix, respectively.

[0072] Based on the obtained Jacobian matrix, the geometric directions that degenerate during the state estimation process are detected. .in, The matrix consisting of the detected degradation directions is n. The matrix is ​​denoted by n, where n is the number of degenerate directions.

[0073] The detected degraded geometric directions are reflected in the panoramic lidar intensity image. In this process, image features with complementary observation properties are generated. By combining image features, the intensity residuals of image features in adjacent lidar intensity images in time series are calculated, and the Jacobian matrix is ​​calculated. ,in, , These are the strength measurement residuals and the corresponding Jacobian matrix, respectively.

[0074] Combine geometric and photometric residuals to construct combined residuals and Jacobian matrices. , ;in, , These are the combined measurement residuals and the corresponding Jacobian matrix, respectively.

[0075] Then, based on the detected geometric direction of degradation Construct an information correction matrix ;in, These are manually set degradation adjustment parameters.

[0076] Based on the above joint residual, Jacobian matrix, and information correction matrix Calculate the corrected Kalman gain K is the corrected Kalman gain.

[0077] Update the system state based on the corrected Kalman gain; determine whether the state estimation process has converged. If it has converged, exit the loop; otherwise, continue executing the loop process.

[0078] S6. Using the optimal system state estimate obtained in step S5, update the system state and covariance matrix, and output the odometer information.

[0079] S7. Update the probabilistic planar voxel map using the latest system status and the distortion-free point cloud.

[0080] To evaluate the performance of this method, the following cutting-edge open-source LiDAR inertial odometry (LIO) systems were selected for quantitative and qualitative comparative experiments: Fast-LIO2, Point-LIO, VoxelMap, and Coin-LIO. Quantitative evaluation of positioning accuracy used the root mean square error of absolute translation (RMSE) as the metric; a lower RMSE value indicates higher positioning accuracy. Qualitative evaluation of map quality used the mean map entropy (MME) as an information-theoretic metric to measure local consistency. The point cloud was color-coded using the MME value (lower MME values ​​indicate higher consistency).

[0081] Experiments were conducted on the Enwide dataset. This dataset was acquired using a 128-line Ouster LiDAR and reliable intensity images can be generated via the official driver. All experiments were performed in Robot Operating System (ROS) on an Intel i9-13900K processor (5.80GHz) with 32GB of RAM. Given the similarity between the baseline method and the algorithmic framework of this study, identical parameters were kept identical, and other parameters adopted the original settings from the baseline method.

[0082] Example 2

[0083] The core objective of this study is to improve localization and mapping performance in geometrically degraded environments. Therefore, we selected the highly challenging Enwide dataset for validation: 1) This dataset was collected in typical geometrically degraded environments (such as narrow corridors); 2) All sequences contain large-scale geometrically degraded segments and are accompanied by reliable laser intensity information. To verify the effectiveness of the Information Optimization Module (IR), an ablation experiment was conducted: the module was removed, and only the probabilistic voxel map was retained for geometric measurements (denoted as ours-w / o-IR).

[0084] Table 1 shows the positioning accuracy results and Figure 3 and Figure 4 The trajectory comparison shows that: 1) Fast-LIO2, Point-LIO, and VoxelMap fail in extreme degradation scenarios such as tunnels and runways due to the lack of degradation suppression mechanisms; 2) Coin-LIO, which integrates laser intensity information, and our method successfully completed all sequences; 3) Compared with the version without information optimization module (ours-w / o-IR), the positioning accuracy of the complete system is significantly improved, especially in the Runway and Intersection sequences where the error reduction is most significant; 4) The confidence of geometric measurement points provided by the probabilistic voxel map effectively improves the reliability of pose estimation.

[0085] Figure 2 The locations of the RunwayS and IntersectionD sequences are shown respectively. Figure 1 Consistency visualization results Figure 5 The mapping results for the Enwide sequence are presented. Comparative analysis shows that this system exhibits higher performance in geometrically degraded scenarios (especially long, straight corridors). Figure 1 Consistency; the information optimization module effectively improves local performance under extreme degradation environments. Figure 1 To the point of being responsive.

[0086] Table 1. Experimental results of the absolute error RMSE on the Enwide dataset.

[0087]

[0088] Experimental results show that the present invention can significantly improve the positioning accuracy and robustness in geometrically degraded scenarios, and promote the application of mobile robots in complex and challenging scenarios such as tunnels, open terrain, and caves.

[0089] The above embodiments are only used to illustrate the present invention. Any equivalent transformations and improvements made on the basis of the technical solutions of the present invention should not be excluded from the protection scope of the present invention.

Claims

1. A lidar-IMU fusion positioning system resistant to geometric perception degradation, characterized in that: The system includes a preprocessing module, a geometric measurement correction module, an intensity information enhancement module, an iterative extended Kalman filter module, and a probabilistic planar voxel map construction module; The preprocessing module processes the raw IMU measurement data and lidar scanning information based on prior state estimation, and generates a distortion-free point cloud through motion compensation. The geometric measurement correction module uses the gradient flow entropy maximization criterion to select the LiDAR point cloud with the optimal information content for scanning-map registration, and then calculates the geometric residual, Jacobian matrix and detects the degradation direction. Based on this, it solves the Tikhonov regularization term to construct optimization constraints. The intensity information enhancement module generates an intensity image along the direction of missing geometric information and extracts image features to construct a complementary photometric measurement. The iterative extended Kalman filter module uses optimization constraints to jointly optimize geometric and photometric measurements in order to update the system state. The probabilistic planar voxel map construction module uses a probabilistic adaptive voxel map to provide planar features, ensuring scan-map geometry measurement and degradation detection.

2. The positioning method of the lidar-IMU fusion positioning system with resistance to geometric perception degradation as described in claim 1, characterized in that, The method includes the following steps: S1, Based on the radar point cloud of the current frame IMU data between the previous frame point cloud and the current frame radar point cloud And the global geometric voxel map that the system has currently established. ; obtain State estimation results at time 1 and its covariance matrix ; S2. Forward Propagation: Based on the IMU data between the previous frame point cloud and the current frame radar point cloud, perform the IMU forward propagation integration process to obtain the prior state estimation information of the current radar point cloud frame. ; Backpropagation: Based on the IMU data between the previous frame point cloud and the current frame radar point cloud, perform the IMU backpropagation process to obtain the pose transformation relationship of all radar points in a frame point cloud up to the end of the frame scan. Transform all radar points in the frame point cloud to the end of the frame to obtain a distortion-free point cloud. ; S3. Based on the spherical projection model, project the original point cloud into a panoramic lidar intensity image according to the radar scanning mode. ; S4. Based on the entropy of the gradient flow, obtain the point cloud that provides information gain for frame-to-map matching from the distorted point cloud. ; S5. Calculate the Jacobian matrix of the actual state relative to the error state. ; Please complete the definitions of each parameter in the formula, and also complete the formulas marked below: in: , These represent the system state at time k and the k-th iteration, the system state error state at time k and the predicted system state at time k, respectively. Update the covariance matrix of the current error state. ; in: The covariance matrix for predicting the system state when k is k; Using point clouds Based on the frame-to-map geometric measurement model, construct the geometric residuals and calculate their Jacobian matrix: ; in: , These are the set measurement residuals and the corresponding Jacobian matrix, respectively. Using the obtained Jacobian matrix, the geometric directions that degenerate during the state estimation process are detected. ; in, The matrix consisting of the detected degradation directions is n. The matrix, where n is the number of degenerate directions; From the geometric direction of degradation Combined with the lidar intensity image in step S3 Generate image features with complementary observation properties. Next, calculate the intensity residuals of image features in adjacent lidar intensity images in the time series, and calculate the Jacobian matrix; ; in, , These are the residuals from the strength measurement and the corresponding Jacobian matrix, respectively. Combined geometric residuals and photometric residuals Construct the combined residual and Jacobian matrix ;in, , These are the combined measurement residuals and the corresponding Jacobian matrix, respectively. Then from the geometric direction of degradation Construct an information correction matrix ;in, Degradation adjustment parameters set manually; Then calculate the corrected Kalman gain. ; Update the system state based on the corrected Kalman gain; determine whether the state estimation process has converged. If it has converged, exit the loop; otherwise, continue executing the loop process. S6. Using the optimal system state estimate obtained in step 5, update the system state and covariance matrix, and output the odometer information. S7. Update the probabilistic planar voxel map using the latest system status and the distortion-free point cloud.

3. The positioning method according to claim 2, characterized in that, In step S3, the intensity image of the panoramic lidar is processed to remove linear artifacts present in the image.

Images