基于正态分布变换(NDT)的实时三维SLAM方法

01简介

矿井救援机器人在执行灾后探测救援任务时，巷道空间的准确描述和自定位是一个关键问题。在复杂的巷道环境中，特别是灾害发生后，三维同时定位与地图构建(SLAM)是一项有效但时间紧迫且极具挑战性的任务。本文提出了新的基于正态分布变换(NDT)的实时三维SLAM方法，该方法利用姿态图优化和回环检测来进一步提高mapping的一致性。我们创新地提取巷道中的地板和墙壁作为平面节点来构建地标约束，此外还通过无损检测应用激光雷达里程计中的姿态节点。

配备多传感器的煤矿救援机器人(CMRR)可以检查事故现场并重建与灾区的通信。声音、图像、温度、气体浓度等数据也可以传输到指挥中心，以便制定救援策略。但是，煤矿巷道环境复杂，场景不可预测，现有的机器人定位方法有限。煤矿灾害导致先验的路标和地图不再可靠，GPS也无法到达现场，在灾后电力中断的情况下，其依赖基站的无线定位方法也无法使用。基于航位推算的机器人姿态估计由于在粗糙地形上、特别是在旋转情况下的累积误差，无法提供精确的长期估计。目前，大多数煤矿救援机器人（CMRR）都是基于视频进行远程操作和定位，但这限制了机器人的运动范围和安全性。

本文利用里程计和回环检测的方法，建立了轻量级、有效的环路检测方法，以构建全局一致的地图。在公共数据集上对该方法进行了评估，并在矿井下进行了现场试验。结果表明，该算法具有较低的计算复杂度和漂移，可为煤矿救援机器人（CMRR）提供姿态估计和环境描述，实现煤矿救援任务中的遥控辅助和自动导航。

02方法

图1 煤矿井下典型环境(a)主巷；(b)采煤工作面附近半构造环境区域；(c)混乱场景

我们的工作是基于无损检测方法来构建激光雷达里程计。与寻找点、线、面等相关特征的ICP及其变体不同，无损检测使用范围扫描来描述环境模型。将空间划分为单元，并分配正态分布来表示测量空间的概率模型。数据扫描中的点与模型扫描中的一系列分布相匹配。所提出的算法框架如图2所示。

图2 系统概述，显示所有步骤执行的预滤波器，激光雷达里程计，约束，并与姿态图形优化线程闭环

为了处理一个数据集中的多个模型，在迭代RANSAC之前，利用点云表面的法线方向进行特征滤波。RANSAC用于检测平面的目标点云仅限于法线方向距离垂直法线或水平法线在一定阈值范围内的目标点云。如果滤波后有足够的点，则进一步使用迭代RANSAC进行平面检测。在真实环境中，特别是结构化矿井中，我们要检测的平面地标通常是侧壁或地面，通常是相互垂直的。特征滤波可以减少RANSAC中处理的点，加快平面地标的提取速度。图3给出了使用上述方法对平面点分段的直观解释。

图3利用迭代RANSAC对平面点进行扫描分割(a)将蓝、绿、青色点建模为平面特征，并将不同颜色的地基准点建模为平面特征，红色的点被特征过滤器过滤(b)扫描的原始点云

03实验及验证

初始测试采用在SLAM社区用于算法评估的KITTI odometry基准。图4描绘了基准测试使用的平台和数据集中的典型场景。该平台包含Velodyne HDL-64E 3D激光扫描仪和OXTS RT3003惯性和GPS导航系统等。现实世界的场景包括有建筑物的城市环境、有周围植被的乡村环境和有开放场景的高速公路。由于KITTI测程基准已经纠正了激光雷达数据收集中的点畸变，因此在实验中使用了原始数据集。该算法在CPU: i7-3555LE (2.5 GHz, 4核)和8G内存的IPC上运行。

图 4 (a) KITTI基准使用的平台(b)真实场景及其对应的点云。

图5是基于无损检测的图SLAM的建图结果。可以看到，点云是一致的，高建图质量。得益于环路检测和图形优化，该地图与真实场景极为吻合。地图在保证地图精度和细节的同时节省了点云的存储空间

图5（a)(b)表示序列00上构建的点云图(b)(c)不同点的高程(c)城市场景中一个十字路口的近景

从图6的轨迹可以看出，在序列00、05和07中，LeGO-LOAM在准确性和一致性方面表现最好。这些序列都在居民区和城市道路中，包含了许多环境中的线和平面特征。在所有序列上，该算法在与地面真值的一致性和符合性方面都优于ICP-graph-SLAM和NDT-Odometry方法。与NDT-Odometry相比，得益于环路检测和优化过程，该方法的姿态估计不会沿路径发散。

图6 NDT-graph-SLAM（蓝色），NDT-Odometry（绿色），ICP-graph-SLAM（黑色），（a)有环00序列，(b)无环01序列，(c)有环05序列，(d)有环07序列

在之前的工作中，设计了多驱动履带式行走机构，以保证CMRR在处理复杂地形时的高机动性。图8为新一代CMRR系统，一个具有多个感知传感器的防爆移动平台。该系统配备了独立的传感器套件，包括3D扫描激光雷达(rslidar-16,12V/9W)、电机驱动旋转2D激光雷达(UTM-30LX, 12V/8.4W)、IMU (Xsens Mti-G-710，在套件中)和RGB-D摄像头(Xtion Pro Live，由USB2.0接口供电)。所有传感器通过电源和网线的物理隔离连接到移动平台。该套件采用阻燃抗静电材料制成，满足煤矿应用要求。在这项工作中，只使用3D激光雷达进行SLAM工作。

图8 新CMRR系统

在充分模拟地下矿山环境的中国矿业大学瓦斯爆炸实验室进行了现场试验。图9的俯视图是通过我们的SLAM算法生成的。导线从A点开始，继续到B、C、d点，然后机器人从C、B、E点返回，在A点形成一个小环路。机器人穿过F、G点，在h点停止。有四个连接点(A、B、F、G)，只有从A点到C点的区域被照亮。构成了一条长约200米，宽约2.5米的狭长的巷道，从C到D，有轻微的曲率，没有照明。F-G部分为变宽巷道，其中一条巷道宽度4.75 m，长度15 m；另一个宽2.5米，长55米。从G到H区域为向下倾斜的巷道。图10是图13中从B到C、A点、F到G、G点分别生成的四张局部地图和照片。