基于NDT图优化的矿井救援机器人3D SLAM教程

矿井救援机器人在灾后执行探测任务时，能否准确描述巷道空间并实现自我定位，是决定救援效率的关键。本文介绍一种基于正态分布变换（NDT）的实时三维SLAM方法，通过姿态图优化和回环检测构建全局一致的地图，适用于煤矿井下等复杂环境。

1. 背景与问题

煤矿救援机器人（CMRR）需携带多传感器（如激光雷达、IMU、摄像头）进入事故现场，重建环境并传回数据。但煤矿巷道环境复杂，灾害后原有地图和路标失效，GPS无法使用，无线定位基站也可能断电。传统基于航位推算的定位方法在粗糙地形上容易积累误差，尤其在旋转时更明显。目前大多数CMRR依赖视频远程操作，限制了运动范围和安全性。

核心挑战：在无GPS、无先验地图的矿井巷道中，实时、低漂移地完成三维同时定位与地图构建（SLAM）。

2. 方法概述

本文提出的SLAM框架基于NDT激光雷达里程计，并融合平面地标约束、回环检测和姿态图优化。系统流程如图2所示：

图2 系统概述：预滤波→激光雷达里程计→约束构建→回环检测→姿态图优化

• NDT激光雷达里程计：将空间划分为单元，用正态分布描述测量点的概率模型，通过匹配点云与分布模型估计位姿。
• 平面地标提取：利用迭代RANSAC提取巷道中的地板和侧壁作为平面节点，构建地标约束。
• 轻量回环检测：基于关键帧的外观相似度和距离判断回环，修正累积漂移。
• 姿态图优化：将里程计约束、平面约束和回环约束统一优化，提高地图一致性。

3. 关键技术详解

3.1 NDT激光雷达里程计

与ICP算法（寻找点线面特征）不同，NDT不依赖显式特征，而是用概率分布描述环境模型。步骤如下：

1. 将空间网格化，每个单元内计算点云的正态分布参数（均值和协方差）。
2. 新扫描中的点根据当前位姿变换到模型坐标系。
3. 计算每个点落在对应网格的概率，通过最大化总概率优化位姿。

小提示：NDT对初始位姿敏感，建议使用IMU或低漂移里程计提供初始估计。

3.2 平面地标提取与约束

在结构化矿井中，侧壁和地面是主要的平面地标，且相互垂直。提取流程：

• 特征滤波：利用点云法线方向，只保留法线垂直或水平（与地面/侧壁一致）的点云，减少计算量。
• 迭代RANSAC：在滤波后的点云中分段提取平面，输出平面参数（方程、范围）。
• 构建约束：将提取的平面作为节点加入姿态图，约束机器人与平面之间的相对位姿。

图3展示了平面点分割效果：

小提示：当巷道内灰尘或烟雾严重时，平面检测可能失败，可结合IMU重力方向预对准。

3.3 回环检测与图优化

回环检测用于消除长时间运行后的漂移。本文方法：

1. 对每个关键帧计算外观描述子（如基于NDT网格的直方图）。
2. 当机器人再次访问已知区域时，根据描述子相似度和空间距离匹配到历史关键帧。
3. 回环匹配成功后，添加相对位姿约束到姿态图中。
4. 利用图优化库（如g2o）同时优化所有节点和边，得到全局一致地图。

重点：回环检测必须结合外观和几何距离，避免误检导致地图扭曲。

4. 实验验证

4.1 KITTI公开数据集测试

使用KITTI odometry基准（城市、乡村、高速等场景）评估算法。平台配备Velodyne HDL-64E激光雷达和GPS真值。实验CPU为i7-3555LE (2.5GHz, 4核)，内存8G。

建图结果（图5）显示点云一致性高，细节清晰：

轨迹对比（图6）表明，本文方法（NDT-graph-SLAM）在所有序列上优于ICP-graph-SLAM和NDT-Odometry，尤其在闭环序列（00、05、07）中姿态不发散：

结论：NDT-graph-SLAM具有较低计算复杂度和漂移，适合实时应用。

4.2 真实矿井现场试验

试验在中国矿业大学瓦斯爆炸实验室进行，模拟地下矿山环境。机器人平台（图8）搭载3D激光雷达（rslidar-16）、旋转2D激光雷达、IMU、RGB-D摄像头，仅使用3D激光雷达进行SLAM。

图8 新一代CMRR系统

试验路径：A→B→C→D→C→B→E→A→F→G→H，总长约200米，包含弯曲、变宽、倾斜巷道。生成的地图俯视图（图10）显示全局一致，与真实场景吻合：

图10 现场地图俯视图（红色标记为固定反射标靶）

图11 局部地图与实际照片对比

5. 结论与启示

本文提出的3D激光SLAM算法，通过NDT构建里程计约束、迭代RANSAC提取平面地标、轻量回环检测和姿态图优化，显著提高了矿井环境下的建图一致性和定位鲁棒性。对撰写建造机器人SLAM综述的启示：

• 了解不同SLAM算法：特别是带闭环和图优化的算法，可拓展认知。
• 关注传感器融合：结合IMU、红外视觉等可提升退化环境精度。
• 实地测试评估：验证算法在实际应用场景的有效性。
• 解决特定环境问题：针对无GPS矿井，基于NDT的多传感器集成定位是可行方向。

常见问题（FAQ）

Q1：为什么选择NDT而不是ICP？

NDT不对点云做特征匹配（如点、线、面），而是用概率分布描述网格，对噪声和局部缺失更鲁棒，且计算效率较高。ICP依赖点对点匹配，在退化环境（如长直巷道）中容易发散。

Q2：回环检测如何保证正确性？

本文采用外观相似度（基于NDT网格的直方图）加上空间距离阈值双重判断。只有同时满足两个条件才添加回环约束，避免误检。实验中在KITTI和矿井现场均未出现错误闭环。

Q3：平面地标提取在非结构化环境中是否有效？

本文针对结构化矿井设计，地面和侧壁普遍存在。若环境为天然洞xue（无规则平面），则RANSAC提取平面可能失败，此时可去掉平面约束，仅依赖NDT里程计和回环。

Q4：算法的计算资源需求如何？

实验在2.5GHz四核CPU、8G内存上实时运行。点云预处理（降采样）和NDT网格化是主要计算瓶颈，采用多线程加速后满足实时性。