同步定位与地图构建（SLAM）是实现车辆自主定位和导航的关键技术。然而，地下矿山环境中照明条件差、粉尘干扰严重、空间结构复杂等特点，导致SLAM 的稳定性与精度面临严峻挑战。本文提出了一种激光雷达和惯性测量单元（IMU）数据融合的SLAM 方法。该方法利用区域生长的点云地面分割实现特征提取，通过IMU 预积分消除点云畸变，并在迭代卡尔曼滤波基础上引入自适应因子，提高位姿估计精度。针对矿山巷道的截面特征和拓扑结构，设计基于巷道几何特征的回环检测。实验表明该方法能有效应对狭窄隧道、不规则墙面等因素影响，与主流方法相比定位轨迹的均方根误差约降低30%，所建点云地图质量明显提升，有效地提高了SLAM 系统的性能。相关成果发表于矿冶期刊群《有色金属（矿山部分）》2025年第6期，题目：面向地下矿山的多传感器融合SLAM方法研究。作者：闫恺航，张元生，张迎琪，吕潇，刘鹏，朱铭，陈章铭。

随着矿山行业向智能化、自动化方向发展，实时三维建图与精准定位已成为实现地下矿山无人驾驶、自动巡检等任务的关键技术之一。同步定位与地图构建（Simultaneous Localization and Mapping， SLAM）通过融合激光雷达（LiDAR）以及惯性测量单元（Inertial Measurement Unit，IMU），可在未知环境中同时完成自身定位和环境地图构建，为地下矿山的无人驾驶设备、自动化监控和智能调度提供技术支撑。然而，地下矿山环境复杂多变，缺少光照和GPS 信号，存在大量的障碍物、行人及移动设备，给SLAM 系统的稳定性和准确性带来极大挑战。

现有的SLAM 方法可以分为视觉SLAM、激光SLAM、多传感器融合SLAM 等。其中视觉SLAM 使用摄像头采集图像数据，通过特征提取和匹配实现定位与建图。激光SLAM 基于激光雷达生成点云数据，通过点云配准实现建图和定位。多传感器融合SLAM 通过不同传感器数据耦合建图。由于地下矿山光照条件差，且有粉尘等因素的影响，视觉传感器可能无法正常工作，因此基于激光雷达和IMU 的SLAM 方法更符合地下矿山的实际需求。

LOAM（LiDAR Odometry and Mapping）是一种经典的激光SLAM 算法，通过将激光雷达里程计和建图任务分离，在小时间窗口内计算实时位姿， 并通过大时间窗口优化地图建图，但在缺少特征的环境中表现一般。SHAN 等提出的LeGO-LOAM（lightweight and ground-optimized LiDAR odometry and mapping）方法通过地面分割和点云聚类处理，降低了计算复杂度，但其对地面特征依赖较强，在无地面环境中精度下降。LIO-SAM（tightly-coupled LiDAR inertial odom etry via sm oothing and mapping）方法，通过回环检测、扫描匹配和IMU 预积分构建因子图实现激光雷达与IMU 的紧耦合优化，提升了建图质量，但在快速运动场景中定位精度会下降。FAST-LIO（fast direct LiDAR-inertial odometry）是XU 等提出的基于卡尔曼滤波的激光雷达-IMU 紧耦合系统，提高了定位精度和点云密度，但在动态环境下鲁棒性不足。Faster-LIO进一步优化FAST-LIO 的计算效率，它使用并行稀疏增量体素替代k-d 树，可以有效降低点云配准的时间开销，但其对特征稀疏场景的处理能力仍存在局限。

针对地下矿山结构复杂、动态干扰多、光照条件差等因素，本文提出融合点云聚类分割的激光雷-IMU 紧耦合SLAM 方法。该方法通过种子生长对激光雷达点云进行分割，去除冗余噪声并提取结构化特征点，提高点云处理效率。其次通过在里程计模块中加入自适应因子动态调整参数，提高定位稳定性。最后融合回环检测方法，减少累计误差，确保建图的全局一致性。

本文方法的整体流程见图1，首先对输入的点云数据通过IMU 预积分的结果来校正点云畸变，随后通过特征提取对点云分割与聚类。预积分和点云分割结果通过自适应迭代卡尔曼滤波器进行耦合。回环检测用于识别重复区域，减少累积误差。回环校正的位姿通过里程计更新，并利用IMU 预积分与激光雷达数据融合进行实时位姿估计，最后通过增量k-d 树（ikd-Tree）实现地图更新与维护。

为从点云数据中提取初始种子点，本文首先对输入点云按高度进行升序排序，并计算点云低位面的平均高度，确定地面种子点的初步范围。将高度在加上阈值τ 以内的点作为地面种子点。随后采用奇异值分解（SVD）方法估计地面平面模型，并基于地面点的分布构建协方差矩阵C。

式中，N是地面种子点云中的点数；P_i 为点云三维坐标；μ 是点云质心。

使用SVD 对C进行特征分解，将其分解为包含协方差矩阵的特征向量U以及奇异值对角矩阵Σ，其对角线元素为协方差矩阵的特征值，计算平面方程为：

式中，n 为最小奇异值对应的特征向量。获得平面方程后，对地面和非地面点进行迭代分割。初始阶段使用提取的种子点P_s 估计平面模型的法向量和方程参数。在每次迭代中，计算每个点到平面的距离：

根据阈值τ 筛选距离小于τ 的点，更新地面点集P_s，并继续迭代直到达到最大迭代次数或满足收敛条件。

2.1.2 基于区域生长的点云聚类方法

为从分割出的点云中提取不同点云簇，采用区域生长算法对点云聚类。首先对点云进行法线估计， 使用k-d 树来加速最近邻搜索。之后通过协方差矩阵特征值计算点云的局部曲率，提取曲率小于预设阈值的点集。曲率较小的点通常代表较为平滑的区域，这些点作为初始的种子点用于后续的区域生长操作。曲率过滤的标准为：

式中，I_c 是满足条件的点的索引集合；c_t 为曲率阈值；θ_s 为平滑度阈值。

该方法以种子点为起点，递归搜索邻域内满足平滑度约束的点，符合上述条件的点则归为同一簇，直到整个点云被分割为若干个平滑的点云簇。

本文基于FAST-LIO2 框架提出改进的里程计方法。在原有算法的里程计框架上加入点云去畸变方法，通过IMU 预积分的结果对雷达点云进行校正。采用自适应迭代卡尔曼滤波器（Adaptive Iterative Kalman Filter）进行状态估计，并通过ikd- Tree 维护地图。

由于IMU 提供100 Hz 的高频数据，而激光雷达扫描频率通常为10 Hz，所以采用预积分方法对IMU 的数据进行处理，如图2 所示。IMU 预积分的基础公式由IMU 运动模型导出，IMU 提供的角速度ω 和加速度a 描述了机体在惯性系中的运动，其运动状态可用如下方程描述：

式中，p^w、v^w、R_b^w分别表示位移、速度、姿态，g^w 为重力加速度；[•]_×表示反对称矩阵。预积分方法通过离散积分近似上述连续模型，定义预积分量ΔR、Δv、Δp 以简化状态传播。

激光雷达在动态环境中采集点云时，由于载体运动的影响，会导致同一帧点云中不同点的采集时间不一致，产生运动畸变。为解决此问题，本文将采集到的点云变换至同一坐标系，利用IMU 所提供的运动状态对点云的每个点进行坐标补偿。点的真实坐标可通过如下公式估计：

式中，t₀ 是某一帧点云的采集开始时间；点P_i 是时间为t_i 时刻的点云，其观测坐标为p_i⁰ ；R(t_i) 和T(t_i)是车辆在t_i时刻的姿态和位置。

为提高车辆位姿的估计精度，本文提出自适应迭代卡尔曼滤波（Adaptive Iterative Kalman Filter， AIKF）的激光雷达里程计方法。AIKF 通过多次迭代优化状态更新，每次迭代都基于最新的状态估计重新计算残差和卡尔曼增益，根据系统的初始状态预测x₀，以及测量更新公式，更新状态x_k和协方差P_{k 。}对于过程噪声协方差和测量噪声协方差则使用自适应的方式进行动态调整。最后，重复上述过程， 直到残差||z_k||收敛或达到设定的迭代次数。

具体过程如下，首先基于IMU 数据对系统状态进行预测：

式中，x_i+1表示系统在i+1时刻的预测状态，包含位置、速度、IMU 零偏、外参等变量；(表示状态的增量运算；Δt表示两次状态预测之间的时间间隔， 用于将连续时间模型转化为离散时间模型；f(x_i ,u_i, w_i）为系统的状态转移函数，x_i为当前状态，u_i为IMU 测量的角速度和加速度，w_i为系统的过程噪声。状态更新过程可表示为：

式中，F_xk是状态转移模型的雅可比矩阵；Q_k是过程噪声协方差。

对于自适应参数的调整，这里引入一个过程噪声变化因子γ 来动态调整Q_k 。

式中，Q₀是初始过程噪声协方差；γ则基于IMU 测量的变化率动态调整；Δu_i 表示IMU 输入的角速度和加速度的变化率；α 是预设的调节系数。

在测量更新中，计算点云与地图的残差：

随后计算系统的卡尔曼增益：

式中，R_k 是测量噪声协方差，根据点云与地图的匹配误差动态调整，具体的调整策略如下；R₀ 是初始测量噪声协方差；β是调节系数； mean（z_j²） 是当前点云残差的均值。

整个过程的状态更新以及协方差更新为：

地图的更新与维护以ikd-Tree 的形式存储，用于高效管理点云。ikd-Tree 支持动态插入、删除和局部重平衡。新的点云通过估计的位姿投影到全局坐标系后插入k-d 树。在插入时，使用局部分辨率对点云进行下采样，避免地图冗余。如果当前地图超过阈值，会从地图中移除这些点以限制地图的大小，并定期对k-d 树进行局部子树的重建与平衡，以确保查询和更新操作的效率。

在激光- 惯性SLAM 中，尽管激光雷达能提供高精度的几何测量，IMU 可以补充运动估计，但系统在长期运行过程中仍不可避免地存在累积误差。回环检测通过识别机器人重访的场景特征，为系统提供全局几何一致性约束，有效减少由雷达配准误差、IMU 积分漂移等因素导致的累积误差。

矿山巷道的断面形状与尺寸通常与采矿需求及地质条件有关，一般为拱形、梯形或者是椭圆形。因此利用激光雷达的点云曲率来分析提取巷道截面的几何特征，通过巷道高度、宽度的连续变化规律构建特征向量。同时，利用巷道的拓扑结构特征通过空间分布编码成点云描述子。

为提高点云描述子的检索与匹配效率，采用哈希表存储描述子。哈希键的计算公式为：

式中，L_e为巷道关键点的边长参数，为法向投影向量。对于关键帧F中的描述子集合D，计算匹配分数：

式中，H[key(d)]表示与描述子d具有相同哈希 键的候选描述子集合，δ()为描述子匹配函数。选取 分数最高的前十个关键帧作为候选帧，用于后续的匹配。

对于通过点云特征匹配获得的描述子对，采用奇异值分解方法计算两帧点云之间的相对变换。结合平面重叠度参数，判断回环检测的有效性。具体实现步骤如下：

对于两个匹配的点云特征描述子Ω_a 和Ω_b，其特征点分别为(p₁,p₂,p₃)_a和(p₁,p₂,p₃)_b 。通过下面公式计算描述子的质心：

式中，q_a 和 q_b 是两个特征平面的质心。对矩阵C 进行奇异值分解，得到旋转矩阵U和平移向量V，通过平面重叠率对描述子的匹配情况进行验证。关键帧间旋转和平移公式为：

随后检查相邻平面是否重叠，如果满足以下条件，则认为两个平面重叠：

式中，θ_n和 θ_d 是法向量和点到平面距离的阈值。平面重叠率通过如下公式计算：

式中，N_o和N_t分别表示重叠平面数和总平面数。如果当前帧和历史帧的重叠率超过预设阈值时，则认为检测到回环，即可将当前位姿的更新状态传递给里程计。

本文通过轨迹分析的方式对定位精度进行评估，所使用的数据为卡内基梅隆大学机器人研究所在地下、洞穴等复杂环境下创建的SubT-MARS 数据集。为了更好的评估本文方法的定位精度，本文将与LIO-SAM 和FAST-LIO2 方法的轨迹进行对比分析。图3 为个方法的整体轨迹对比，可以看到FAST-LIO2 由于缺少回环模块，后期轨迹出现明显漂移。图4 为转弯处x-y-z 方向轨迹对比，可以看到，本文方法在z 轴方向的漂移明显小于其他方法。图5 为转弯处各方法的RPY 对比图，可以看到LIO-SAM 虽然有回环检测，但在剧烈工况时俯仰角出现较大波动，存在一定的姿态估计误差。相比之下，本文方法能够更好地贴合真实轨迹。

表1、表2 为绝对轨迹误差（Absolute Trajectory Error，ATE）和相对位姿误差（Relative Pose Error，RPE）的对比结果。在ATE 评估中，最大误差降低至0.26 m、均方根误差降低至0.08 m，分别较其他算法改进了约60% 和46%；在RPE 评估中，最大误差0.15 m、均方根误差0.021 m，较次优的FAST-LIO2 分别改进了28% 和32%。同时本文算法在误差平方和指标上的显著降低也表明其具有更好的累积误差控制能力。

建图效果实验在模拟巷道中进行，巷道整体呈拱形，长宽高分别为67、4.9、4.5 m，图6 为巷道平面图。巷道中段有一个Y 字形岔道，岔道尽头放置有碎石料堆和粗石，如图7（a）、（b）所示。实验车辆为一台2 m³ 地下铲运机，如图7（c）所示。车辆配备导远6 轴IMU 和Velodyne16 线激光雷达，其中激光雷达和IMU 通过结构件刚性连接安装在驾驶室上方，相关参数如表3、表4 所示。传感器安装位置见图7（d）。

为了验证聚类分割方法的效果，本文分别在KITTI 数据集和地下模拟巷道进行验证。KITTI数据集采用00 序列，包含城市、道路及复杂动态场景的多模态数据（激光雷达点云频率10 Hz，分辨率64 线），可与地下模拟巷道进行对比验证。如图8 所示，本文方法将场景中的地面点云标记为白色，并通过聚类方法对非地面目标进行了场景要素划分，以不同颜色进行标记，从图8 可以看出，激光雷达线束越高，聚类分割效果越好。

本文通过在巷道 Y 形路口折返的方式对回环检测效果进行评估，车辆由主路开进岔道，之后再返回主路。图9（a）为回环检测方法的特征提取结果，其中白色点云为注册的点云地图，红色点表示当前提取的特征点。可以观察到，算法准确提取出了巷道场景中的主要几何结构。图9（b）为点云的特征匹配结果，绿色点云表示关键帧发生匹配的点云区域，黄色点为特征匹配点对。可以看出，在墙面、拐角等区域，算法成功建立了稳定的特征匹配关系。

LIO-SAM 的建图结果如图10 所示。方法整体建图准确，但在岔道区域，点云存在一定的模糊现象， 边缘特征不够清晰，同时点云较为稀疏，无法还原场景细节。FAST-LIO2 相比LIO-SAM 在边缘清晰度上有所改善，如图11 所示，但在局部放大图中仍可看出，点云噪声较多，特别是在拐角处的特征提取略显不足。本文方法的建图效果如图12 所示，在岔道结构局部放大图中表现出色，点云的边缘特征保持得更为锐利和准确，碎石堆等细节清晰可见。在整体环境的重建中，本方法很好地保持了场景的几何结构特征。

本文提出的多传感器融合SLAM 方法通过将聚类分割技术与SLAM 框架相结合，有效提升了系统在地下矿山环境下的定位与建图精度，增强了特征提取的可靠性。实验结果表明，本文方法的平均误差约为0.073 m、均方根误差为0.082 m，比表现较好的FAST-LIO2 方法改进30% 左右，特别是在地下矿山巷道这种结构化的场景中，能够更好地利用环境的结构特征，实现更为准确的地图重建。但所提方法在动态环境下的建图效果有待提高，后续研究将聚焦于提升算法在动态环境下的适应性以及优化系统的计算效率。本文的实验结果均来自公开数据集与模拟巷道数据，实际应用效果还需要在实际矿山巷道中进一步验证。