复杂环境下实时鲁棒3D激光雷达定位

复杂环境下实时鲁棒3D激光雷达定位

一、摘要

定位是机器人领域的重要研究方向。本篇文章里，我们提出了一种基于3D激光雷达的复杂环境下的定位方案。我们首先使用GPS和雷达建立一张点云地图，然后在匹配定位的时候从大地图中分割出一个小地图，这样能够减少匹配时间，同时提高鲁棒性。在定位阶段我们融合了多种传感器，以提高位姿输出频率和精度。我们在一个UGV上对融合定位算法进行了验证，即使在六个月以前建立的地图上做定位，仍然能取得10cm的定位精度以及很高的鲁棒性。该系统已经在一个环境复杂的工厂内持续运行了六个月，累计里程达到2000km。

附赠自动驾驶学习资料和量产经验：链接

二、主要算法

1、建立点云地图

建图三大要素：前端、约束和后端。

在这里前端选用的是loam，相比于icp或者ndt，它的匹配效率更高，能够使系统达到实时。约束包括闭环和gps，后端使用的是g2o。后端概率图的组成如下图所示

里程计运行过程中，输出位姿，在连续输出的位姿中选取关键帧，以关键帧作为概率图的节点，相邻关键帧之间的相对位姿以及闭环检测到的相对位姿作为边，最后把gps的位置作为先验边，就完成了概率图构建。

优化之后会得到修正的关键帧位姿，以此位姿为基础拼接对应的关键帧点云即可得到点云地图。

2、基于地图的匹配定位

地图定位其实就是当前帧点云和地图点云之间的匹配。此处采用的是ICP方法，因为点云匹配需要较好的初值，所以需要为每帧提供预测位姿。预测采用的是运动模型预测，即以上一帧和这一帧的匹配结果，给出下一帧的预测值。

同时，为了提高匹配效率和鲁棒性，不能直接和整张点云地图匹配，而应该和当前位置附近的点云匹配，所以每次匹配之前都根据预测位置，在大地图里分割出一个小地图用于匹配。

最后因为原始点云稠密度比较高，所以在匹配之前对小地图和当前帧点云都使用voxel_filter做一次滤波。

3、融合定位

在得到基于地图的匹配定位以后，为了提高输出频率和定位稳定性，需要和其他传感器进行融合，此处选用里程计。

在滤波算法上，由于EKF仅对当前同一时刻的信息进行融合，而此处ICP匹配具有延时，所以采用SSKF进行滤波。

三、实验

作者把该方法在kitti数据集上做了测试，精度指标如下表所示。

随后，作者自己采集数据进行了测试。结果如下图，其中(a)(b)©(d)(e)是轨迹，(f)是前几条轨迹的误差。

四、结论

作者提出了一种以3D激光雷达为核心的定位方案。首先使用GPS和雷达建立点云地图，然后以该地图为基础进行了匹配定位，最后以匹配结果为观测，加上里程计进行了融合定位。定位精度、稳定性均达到了较高的水平，并且在实际环境中经过了长时间定位测试的检验。