应用于履带机器人的激光雷达点云建图方法

姜晶营 路铭 王子钰 李钰泽 边亮亮

摘要：针对构建点云地图存在的位姿漂移误差问题，本文提出了一种基于因子图优化的点云地图构建的方法，利用回环约束与GNSS（global navigation satellite system）约束消除了建图的漂移误差。通过履带机器人平台在实际园区场景中进行了数据采集和实验验证。实验结果表明：该方法利用因子约束对历史位姿轨迹进行优化可进一步提高点云地图的局部结构精度与一致性，为履带机器人提供可靠的定位与导航数据。

关键词：因子图优化;履带机器人;点云地图

引言

随着科技技术的发展，无人驾驶技术广泛应用于各行各业。履带机器人以其优越的机动性能而得到发展，可应用于结构化与非结构化的道路上，可爬坡、越障，具有较好的环境适应性。当前履带机器人是无人驾驶技术发展的重要载体，通过两个技术的结合，可执行特殊场景下的任务。履带机器人实现巡逻侦查的关键技术则是定位与地图构建技术，利用定位与建图技术可为无人驾驶载体提供先验的地图信息，这在高精定位与导航中都发挥着重要作用。其中基于激光雷达的定位与建图技术受环境影响小，具有高可靠性而得到了充分的发展。

为了实现低成本的履带机器人的高精度激光SLAM（simultaneous localization and mapping）地图构建，本文设计了一种基于因子图优化的多传感器融合定位与地图构建方法，算法框架如图1所示。首先是传感器的标定与数据预处理，消除物理安装所带来的误差及提供平滑的点云数据;然后是基于NDT（normal distributions transform）算法的点云地图构建;最后是利用因子图对历史位姿轨迹进行优化，消除构建地图的漂移误差。通过本文方法可降低构建点云地图的重投影误差，提高点云地图的一致性。

1 基于因子图优化的点云建图方法

1.1点云地图构建

在多传感器融合的方法中，需要各将各个传感器基于自身的坐标进行标定，消除传感器物理安装上的误差。在履带机器人运行的过程中，激光雷达点云会产生畸变，其原因是激光雷达点云的采集不是瞬时获得的。为此采用IMU（inertial measurement unit）测量值对激光雷达点云进行去畸变，为点云地图的构建提供一个平滑的点云数据。激光雷达点云数据量大，对去畸变后的点云进行体素网格滤波进行降采样处理，在滤除噪点的同时极大的降低了计算量，降采样的频率设为0.9m。最后将降采样的点云数据通过NDT匹配算法进行累积建图。

1.2 因子图优化

因子图是一种基于非线性最小二乘优化的概率图，是SLAM中后端优化的主流技术之一。因子图一般可用来实现即插即用的多源传感器观测数据的输入融合，并且这一特点极大的降低了构建地图时的计算量。因子图在定位与建图技术问题中可以定义为当前位姿与历史位姿的约束问题。在因子图中，本文方法加入了GNSS约束和回环约束，如图2所示。

回环约束

在大场景建图的过程中，长时间的增量式构建点云地图存在位姿漂移误差的问题，而回环约束则是解决该问题的主要方法之一。回环约束是实时点云位姿与历史点云位姿的约束关系，利用该约束关系可消除建图过程中产生的累积误差。在本方法由文献的回环检测所启发，Wan等人虽然融合了几何特征与强度特征的ISC（Intensity Scan Context）回环检测，但是其强度只取了一个面元中的最大值，这会使得其丢失了其它点的强度信息。本文方法对面元的平均强度进行编码，可增强特征描述子的可靠性。在接收到实时雷达点云时，对激光雷达点云划分为面元，进行上下文扫描，将激光雷达的几何特征和强度特征编码到二维图像上。在全局地图构建的过程中，始终保持对历史轨迹的维护，在进行回环约束检测时，对实时的点云进行特征描述子检测，再将实时点云的描述子与候选点云的描述子进行匹配。

GNSS约束

当接收到GNSS的测量值时，首先利用通用横墨卡托格网系统（Universal Transverse Mercator Grid System，UTM）将GNSS测量值转换到激光雷达坐标系下的位姿;然后通过计算当前GNSS的位置残差。当GNSS测量值小于经验值时，将GNSS约束添加到因子图中，对当前位姿进行约束，消除建图的位姿漂移误差。

2 实验结果与分析

实验使用履带机器人作為实验平台。平台设备主要有线控底盘，联适导航R60S-U GNSS接收机，速腾16线激光雷达与HWT905 IMU等传感器。程序运行在工控机配置为Inteli7-9750 CPU，8GB内存，环境为Linux系统下的机器人操作系统（ROS，Robot Operating System）。在实验验证中，我们在校园真实场景中作了分析验证。

2.1数据采集与建图效果

在校园场景中采集了数据进行建图效果分析。实际测试场景如图3（a）所示，构建的三维点云地图如图3（b）所示。点云地图去除地面点，测试场景主要为非结构化道路，建筑物与树木较多。在采集数据的过程中，通过遥控控制履带机器人的行进，测试道路是平坦的园区道路，轨迹长度为0.86km，履带机器人的平均运行速度为1.2m/s，激光雷达帧数为7132。

2.2定位精度分析

如图4所示，针对不同算法的位姿轨迹进行分析，并以GNSS测量值为真值进行对比。图4（b）是图4（a）中方框的局部显示图。在图4（a）中，NDT算法与其他算法相比，其轨迹具有明显的漂移，其原因是没有回环约束的优化，累积的漂移误差无法消除;而LeGO-LOAM算法[4]加入回环约束优化位姿轨迹，在一定程度上消除了位姿漂移误差，但在局部轨迹处与GNSS真值还存在一定的误差，如图4（b）所示。本方法通过添加回环约束与GNSS约束，极大的消除了位姿漂移误差，位姿轨迹与GNSS真值轨迹基本一致，均优于NDT算法与LeGO-LOAM算法。

在量化分析时采用了绝对轨迹误差（ATE，absolute trajectory error）进行分析，如表1所示，NDT算法达到了31.22，而本方法的均方根误差（RMSE，root mean square error）为0.87，优于LeGO-LOAM的1.57。

3 结论

针对园区场景下构建点云地图存在的位姿漂移误差的问题，提出了一种基于因子图优化的NDT点云地图构建方法，应用于园区场景中的位姿估计与地图构建。在开源算法NDT的基础上加入回环约束与GNSS约束，消除了长时间累积建图过程中产生的位姿漂移误差。为了验证所提出算法的性能，与LeGO-LOAM等经典算法进行了对比实验。实验结果表明所提出的方法有效的提高了点云地图的局部结构精度与全局一致性。在下一步的工作中，需要研究多传感器数据融合算法优化，进一步提高算法的实时性与精度。

参考文献：

[1]林俊钦，韩宝玲，罗庆生，赵嘉珩，葛卓.基于NDT匹配和改进回环检测的SLAM研究[J].光学技术，2018，44（02）：152-157.

[2]付光耀，周海，卞春江，陳宇.基于闭环检测的三维点云地图创建[J].传感器与微系统，2020，39（9）：4.

[3]Wang H，Wang C，Xie L . Intensity Scan Context：Coding Intensity and Geometry Relations for Loop Closure Detection[J].  2020：2095-2101.

[4]Shan T，Englot B. Lego-loam：Lightweight and ground-optimized lidar odometry and mapping on variable terrain.32 IEEE/RSJ International Conference on Intelligent Robots and Systems （IROS） 2018;4758-4765.

作者简介：

通讯作者：路铭（1981.10.6），男，满族，博士/研究生，北京联合大学应用科技学院 讲师，高频开关电源拓扑结构及其控制策略、无人驾驶车辆电气系统架构优化。

姜晶营（1998.01.13），女，汉族，河北，硕士研究生，北京联合大学应用科技学院，研究方向为无人驾驶技术职业教育。

王子钰（2000.07.12），女，满族，辽宁，本科生，北京联合大学机器人学院。

李钰泽（1999.05.05），男，汉族，河南，本科生，北京联合大学机器人学院。

边亮亮（1999.05.04），男，汉族，甘肃，本科生，北京联合大学应用科技学院。

基金项目：202101北京联合大学“启明星”大学生科技创新项目。