三维正态分布函数（正态分布图像变换）

01简介

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

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

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

02方法

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

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

图2 系统概览，显示预过滤器、激光雷达里程计、约束和带有姿态图优化线程的闭环执行的所有步骤

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

图3 使用迭代RANSAC 扫描和分割平面点(a) 将蓝色、绿色和青色点建模为平面特征，将不同颜色的地面基准点建模为平面特征。红点经过特征过滤器过滤（b）扫描的原始点云

03实验与验证

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

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

图5是基于无损测试的图SLAM建图结果。可以看出，点云一致，建图质量较高。由于环路检测和图形优化，地图与真实场景紧密匹配。地图在保证地图精度和细节的同时，节省点云存储空间。

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

从图6 的轨迹可以看出，LeGO-LOAM 在序列00、05 和07 中的准确性和一致性方面表现最好。这些序列位于住宅区和城市道路中，包含许多在环境。在所有序列中，该算法在一致性和符合地面实况方面优于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.75m，长15m；另一个宽2.5 m，长55 m。从G到H的区域是一条向下倾斜的隧道。图10是图13中B点到C点、A点、F点到G点、G点生成的四张局部地图和照片。

图10 NDT-SLAM 生成的地图的顶视图，包括多个扫描姿势。机器人从A点出发，遵循A-b-c-b-d-c-b-e-A-f-g-h的顺序。在A、C、D、F、G的交叉点处设置固定反射标记

图11 现场测试生成的地图及其实际位置

04结论

本文提出了一种具有闭环和图优化功能的三维激光SLAM算法。通过使用3D-NDT构建激光雷达测距约束和迭代RANSAC提取地标平面，提高了建图的一致性和定位的鲁棒性。同时，通过检测外观相似度和关键帧之间的距离来实现轻量级闭环，并利用多线程加速来提高计算速度以满足实时应用。

本文对于撰写构建机器人SLAM 应用的综述具有以下意义：

（1）了解不同的SLAM算法：通过研究不同的SLAM算法，特别是具有闭环和图优化功能的算法，可以深入了解SLAM技术的发展和应用。

（2）注重传感器融合：考虑多传感器融合，例如结合IMU和红外视觉传感器，可以提高退化环境下的精度性能。

（3）现场测试和评估：通过实际环境中的测试和评估，可以验证SLAM算法在特定应用场景下的有效性和适用性。

（4）解决特定环境下的问题：针对特定环境下定位方法有限的问题，可探索基于无损检测的多传感器集成定位方法，提高精准定位能力。

责任编辑：彭静