Real-Time 6D Lidar SLAM in Large Scale Natural Terrains for UGV

Simultaneous Localization And Mapping (SLAM) plays a more and more important role in the environment perception system of Unmanned Ground Vehicle (UGV), most SLAM technologies used to be applied indoor or in urban scenarios, we present a real-time 6D SLAM approach suitable for large scale natural terrain with the help of an Inertial Measurement Unit(IMU) and two 3D Lidars. Besides dividing the entire map into many submaps which consists of large numbers of tree structure based voxels, we use probabilistic methods to represent the possibility of one voxel being occupied/null. A Sparse Pose Adjustment (SPA) method has been used to solve 6D global pose optimization with some relative poses as pose constraints and relative motions computed from IMU data as kinetics constraints. A place recognition method integrated a method named Rotation Histogram Matching (RHM) and a Branch and Bound Search (BBS) based Iterative Closest Points (ICP) algorithm is applied to realize a real-time loop closure detection. We complete global pose optimization with the help of Ceres. Experimental results obtained from a real large scale natural environment shows an effective reduction for Lidar odometry pose accumulative error and a good performance for 3D mapping.