| Simultaneous Localization and Mapping(SLAM)is a prerequisite for autonomous robots to achieve downstream tasks such as feedback control and path planning,and is one of the core technologies of mobile robots.Light Detection and Ranging(Li DAR)has high measurement accuracy and strong anti-interference ability,Li DAR-SLAM has higher accuracy and better robustness.Therefore,Li DAR-SLAM is more popular in mobile robotics applications.In recent years,many excellent works have emerged in Li DAR-SLAM and gradually started to be applied in industry.Li DAR-SLAM systems mostly adopt the scheme of front-end odometry to estimate the initial pose and back-end to optimize the initial state.In contrast to Visual SLAM methods,most Li DAR-SLAM systems lack batch optimization.In this paper,we propose a full Li DAR-SLAM system based on explicit geometric features,which improves the line and plane feature extraction method for point clouds in the front end,and introduces a batch state optimization method based on line and plane primitives in the backend.The main contributions of this paper are as follows:(1)We improve the feature extraction method for point clouds,and propose a clusteringbased explicit line and plane feature extraction method.For plane features,an agglomerative hierarchical clustering(AHC)method is used to achieve plane segmentation of the point cloud.The explicit plane segmentation method exploits the ordered structure of the point cloud and stores the point cloud in a graph data structure.After that,the nodes located in the same plane in the figure are merged by AHC,which achieves fast and efficient point cloud plane segmentation.A line feature detection method based on Breadth First Search(BFS)is proposed for extracting line features.We first calculate the local curvature value of the point cloud and screen the high curvature points as candidate line features.After that,a BFS is used to traverse the candidate line features,which achieves the clustering extraction of line features by merging the neighboring candidate features that share the same line.(2)Based on the explicit line and plane features,we implement a Li DAR-SLAM system.In the front-end,the initial pose estimation of the Li DAR motion is achieved by aligning point cloud feature and feature submap.In the back-end,a local state optimization method based on line and plane primitives is proposed.The method fuses line and plane factors based on a factor graph,and achieves joint batch optimization of the pose,line,and plane parameters by minimizing line to line and plane to plane distance.In the back-end module,we also design a loop closure detection method based on Euclidean distance and achieve global state optimization by adding loop factors to the factor graph.(3)In this paper,the proposed Li DAR-SLAM system is experimented and evaluated on the public dataset and the self-collected dataset,respectively.In the public dataset experiments,the absolute trajectory error is used to metric the difference between the estimated trajectory and ground truth.We not only compare our method with other methods,but also conduct ablation experiments.The experimental results show that the method proposed is effective.To evaluate the performance of the system in other environments,we conduct self-collected dataset experiments using end-to-end relative positional error to metric the trajectory error.The experimental results show that the SLAM system proposed in this paper can achieve high accuracy localization and map construction in other scenarios as well.Finally,the efficiency of the main modules of the system is tested on the public dataset and the self-collected dataset respectively.The experimental results show that the system meets the real-time requirements of SLAM. |
