一种多层次数据融合的SLAM定位算法

A Multi-level Data Fusion Localization Algorithm for SLAM

针对2D激光SLAM(同步定位和地图构建)机器人导航中激光点云匹配计算量大、轨迹闭合效果差、位姿累积误差大、以及各环节传感器观测数据利用不充分等问题,提出一种基于多层次传感器数据融合的实时定位与建图方法——Multilevel-SLAM.首先,在数据预处理方面,利用IMU(惯性测量单元)数据预积分结果为激光点云配准提供坐标转换依据.对激光点云进行特征采样,降低点云匹配计算量.其次,通过无迹卡尔曼滤波算法融合IMU、LiDAR(激光雷达)观测量得到机器人位姿,来提高闭环检测效果.最后,将激光点云配准约束、闭环约束、IMU预积分约束加入到SLAM算法的后端优化中,对全局地图位姿节点估计提供约束配准,实现多层次的数据融合.在实验中利用LiDAR-IMU公开数据集对Karto-SLAM、Cartographer和Multilevel-SLAM算法进行性能测试对比.Multilevel-SLAM算法的定位精度始终保持在5 cm以内,而对比方法则存在不同程度的定位偏移.实验结果表明,在没有显著增加计算量的前提下,Multilevel-SLAM算法有效提高了闭环处的轨迹闭合效果,具有更低的定位误差.

There exist some problems in 2D laser SLAM(simultaneous localization and mapping)based robot navigation,such as the large calculation amount in laser point cloud matching,the poor effect of trajectory closure,the large cumulative error of pose,and the insufficient use of data observed by sensors in each link.To solve these problems,a multi-level sensor data fusion method for real-time positioning and mapping is proposed,named Multilevel-SLAM.Firstly,coordinates are transformed based on the preintegration results of IMU(inertial measurement unit)data in data pre-processing,to register the laser point cloud.Features of laser point cloud are sampled to reduce the computation of point cloud matching.Secondly,the robot pose is obtained by combining IMU and LiDAR observations with unscented Kalman filter algorithm,to improve the loop closure detection effect.Finally,laser point cloud registration constraints,closed-loop constraints and IMU pre-integration constraints are added to back-end optimization of SLAM,to provide constraint registration for the pose node estimation in global map and to realize multi-level data fusion.In the experiment,performances of Karto-SLAM,Cartographer,and Multilevel-SLAM algorithms are tested and compared on LiDAR-IMU open datasets.The positioning accuracy is consistently kept within 5 cm by Multilevel-SLAM algorithm,while there are positioning deviations to different degrees by the contrast methods.Experimental results show that Multilevel-SLAM algorithm effectively improves the effect of trajectory closure at the closing point of the loop,and has a lower positioning error without any significant increment of computation.