煤科总院郭宏涛：融合静态地图构建与多源信息感知的煤矿机器人重定位方法研究

2025年12月03日 16:15 智能矿山杂志责编：戚金荣作者：智能矿山杂志

摘要

针对煤矿地面环境中地形复杂、动态障碍物多变及GNSS信号不稳定等问题，提出1种基于多传感器融合与分层动态点云去除的鲁棒定位方法。采用多分辨率深度图像对比与改进ERASOR算法构建静态高精度地图，有效滤除动态干扰点云。在此基础上，通过扩展卡尔曼滤波融合自适应蒙特卡洛定位、IMU与GNSS传感器信息，构建环境自适应的定位与重定位框架。利用激光雷达匹配与GNSS状态监测机制实现机器人的异常运动检测与位姿恢复。试验结果表明，该方法在semindoor 中实现78.1%的动态点云去除率，并具备良好的重定位响应能力，适用于煤矿巡检及运输等应用场景。

文章来源：《智能矿山》2025年第11期“矿山机器人技术创新与实践特刊”

第一作者：郭宏涛，主要研究方向为煤矿机器人导航的相关技术。 E-mail：1947621749@qq.com

作者单位：煤炭科学研究总院有限公司；中煤科工机器人科技有限公司

引用格式：郭宏涛，王雷.融合静态地图构建与多源信息感知的煤矿机器人重定位方法研究[J].智能矿山，2025，6(11)：31-40.

点击文末左下角阅读原文，免费下载阅读pdf全文

关注微信公众号，了解更多矿山智能化建设进展

煤矿地面环境具有地形复杂、动态障碍物多变以及GNSS信号不稳定等特点，对机器人的精确定位提出了特殊要求。为实现煤矿地面机器人在巡检、运输等任务中的可靠定位，需要解决动态环境下的定位技术难题。近年来，基于激光雷达、视觉传感器和惯性测量单元的多源信息融合定位方法，为解决上述问题提供了可行的技术路径。其中，动态点云去除技术能够有效减少车辆、人员等动态物体的干扰影响。

目前动态点云去除主要分为在线处理和后处理2种技术路线。在线处理方法虽能实时更新环境地图，但在大范围场景下存在计算效率瓶颈。相比之下，后处理方法通过SLAM技术构建全局地图后，再通过时序比对实现动态点云滤除，能够提供更完整的环境表征。然而，针对煤矿地面特有的半结构化特征，仍需开发更具适应性的融合定位算法。

在煤矿地面环境中，机器人需要同时在复杂半结构化环境的高精度定位和开阔区域的全局位姿估计。自适应蒙特卡洛定位（AMCL）方法在结构化环境中表现优异，但在开阔场地易因特征缺失导致定位失效；AMCL在初始位姿未知或遭遇绑架问题时，传统的全局重采样策略收敛效率较低。

针对上述问题，笔者提出了一种融合后处理点云优化和多传感器数据融合的定位框架。采用改进的ERASOR算法实现动态点云的层级滤除，构建高精度静态地图。在此基础上，通过扩展卡尔曼滤波（EKF）算法融合多传感器数据，利用协方差矩阵实时评估各传感器数据可靠性，动态调整融合策略，并结合激光雷达扫描匹配与GNSS监测解决多场景下的重定位问题。

煤矿机器人定位系统主要框架

采用层次化架构解决动态环境下的建图以及机器人重定位问题。系统首先通过激光SLAM构建初始地图，经过地图数据转换等预处理后，采用两阶段动态点云去除策略：基于多分辨率深度图像对比的粗识别和改进区域占用描述子的精去除，最终生成静态环境地图。在定位阶段，通过激光雷达扫描匹配以及GNSS监测机制，在多场景为AMCL提供初始定位与重定位，通过EKF融合方法的自适应权重分配，利用场景传感器数据特征动态调整IMU、GNSS等传感器数据的融合权重，煤矿机器人定位系统组成框架如图1所示。

图1 煤矿机器人定位系统组成框架

（1）SLAM技术建图与地图数据处理

采取常规SLAM方法如SC-LIOSAM、fastlio获取环境的PCD点云地图与查询帧地图，并结合位姿数据对PCD文件增加VIEWPOINT关键字用于后续对比识别处理。

（2）动态点云初步识别

由于多分辨率深度图像对比识别方法识别动态点准确率较高，但识别数量占比较低，因此利用其特点进行初步处理。

（3）改进ERASOR算法精细动态点云去除

ERASOR算法本身具有很高的去除率，但是对静态点云有较多的误删情况，结合步骤（2）的识别结果动态调整判定条件，增加综合性能指标。

（4）激光雷达扫描与AMCL联合定位

结合静态地图栅格，利用AMCL与EKF融合多传感器完成基于地图的定位，并通过激光雷达扫描匹配方法，解决丰富特征场景下的机器人“绑架”与重定位问题。

（5）开阔场景下GNSS辅助重定位与AMCL联合定位

根据GNSS信号质量评估结果与基于里程计和AMCL的位置检测机器人异常移动，结合EKF融合多传感器，解决机器人“绑架”与重定位问题。

基于分层动态点云去除的先验地图构建

动态点云去除部分主要分为基于多分辨率深度图像对比识别的粗标记和基于粗标记的改进ERASOR算法2部分。

2.1 基于多分辨率深度图像对比的动态初步标记

（1）利用激光雷达扫描数据及其对应的位姿，通过SLAM技术构建初步的点云地图。然后将该点云地图与激光雷达扫描数据投影至二维深度图像，即以矩阵形式表达点云分布，其中像素值由点到传感器的距离填充，作为深度信息。在高分辨率下，对深度点云地图与深度扫描图像进行像素级对比，若深度差值超过预设阈值，则初步标记对应点为动态点，并赋予强度值1，否则标记为0，形成初始的动态与静态点云分割，基于多分辨率深度图像的动态点云初步标记算法如图2所示。

图2 基于多分辨率深度图像的动态点云初步标记算法

（2）基于多分辨率深度图像的理论特性，系统从高分辨率逐步过渡至低分辨率；并利用不同分辨率下深度图像呈现的点云细节差异，重新评估被标记为动态的点云。通过降低分辨率，扩大像素覆盖范围，缓解高分辨率下因边界歧义或位姿误差导致的静态点误删问题，恢复部分被错误去除的静态点。为提高处理效率，采用批量处理方式，将多帧扫描数据同步与地图对比，增强动态点识别的鲁棒性，最终生成包含初步动态点标记的先验地图。该方法依托多分辨率对比与批处理的双重理论优势，确保静态地图构建的精度与完备性。

2.2 基于标记的自适应ERASOR算法优化方法

（1）基于标记方法生成的点云地图与激光雷达扫描数据，首先通过定义感兴趣区域筛选可能包含动态物体的点云范围，以降低计算复杂度。

（2）采用Scan Context的栅格划分方式，将VOI内的点云数据分配至空间单元，形成规则的径向和方位网格。针对每个栅格，引入伪占用率描述点云的空间占用状态，具体通过计算地面以上点云的高度差量化单元内的垂直分布特性。

（3）利用查询帧点云与地图点云的伪占用率比率，与基于标记结果的高度分布特征自适应调整的阈值进行比较。若比率小于该阈值，则判定该栅格为潜在动态区域，反映动态物体与地面接触导致的占用差异。在识别出的潜在动态区域内，应用区域式地面平面拟合方法（R-GPF），通过最低高度种子点估计局部地面平面，提取静态地面点并将其回退至地图，去除平面以上的动态点，基于自适应ERASOR算法的动态点云去除算法如图3所示。R-GPF的单元化操作适应非平面地面环境，提升静态点保留精度。

图3 基于自适应ERASOR算法的动态点云去除算法

（4）输出去除了动态痕迹的干净静态地图，为机器人定位与导航任务提供可靠支持。

基于激光雷达与GNSS的多环境鲁棒定位与重定位方法

为提高机器人在具有结构化与开阔区域的异构环境中的定位鲁棒性，提出了1种集成多策略的混合定位框架。该框架融合了2种针对特定环境挑战且互补的重定位策略，应对不同场景下的定位问题。

（1）针对具有丰富几何特征但GNSS信号缺失或退化的结构化场景（如室内廊道），采用基于激光雷达扫描匹配的全局重定位策略，以应对“机器人绑架”等全局定位问题。

（2）在相对开阔、激光雷达特征稀疏或退化的场景中，且GNSS信号稳定的情况下，采用GNSS辅助的定位监测与快速重定位策略，纠正大规模的定位漂移并恢复系统失效后的定位能力。

上述双策略架构依托第二节构建的高精度静态地图作为统一的先验地图，并通过共享的扩展卡尔曼滤波（EKF）后端进行多传感器数据融合，确保机器人运动轨迹的连续性与平滑性。

3.1 基于扫描特征匹配的全局重定位

基于先验地图的机器人定位系统采用AMCL算法框架，通过粒子滤波实现激光雷达观测数据与先验地图的概率匹配。AMCL算法通过重要性采样和重采样机制迭代更新位姿估计，但在实际应用中面临2个关键性挑战。

（1）在初始位姿未知或遭遇“绑架”问题时，采用全局随机初始化粒子分布策略，会导致算法收敛速度显著降低。

（2）在环境特征稀疏或具有高度对称性的场景下，粒子滤波容易陷入局部最优解，严重影响定位精度。

针对上述问题，引入全局位姿探测器模块的创新解决方案。该模块首先通过ORB特征提取算法从激光雷达点云数据中提取显著特征，随后采用RANSAC算法实现特征匹配与位姿估计的鲁棒性计算。为量化评估候选位姿的可靠性，引入F1评分机制对每个可能位置的匹配度进行精确度量，最终选择最优匹配结果作为AMCL算法的初始粒子分布，提升算法的收敛效率。

为进一步优化位姿估计的连续性和运动平滑性，系统采用扩展卡尔曼滤波EKF框架实现多传感器数据融合。系统以惯性测量单元的角速度和加速度测量作为状态预测输入，同时将AMCL输出的全局一致位姿估计作为观测更新项。通过动态调整过程噪声和观测噪声的协方差矩阵，实现传感器贡献度的自适应平衡，达到轨迹平滑优化的目的。在系统运行过程中，通过持续迭代更新直至位姿估计达到稳定收敛状态，最终输出具有高精度特性的定位结果。

3.2 GNSS辅助的状态监测与快速重定位

在开阔环境中，GNSS 能提供稳定的全局位姿信息，为定位系统提供初始参考。采用EKF融合 IMU 与轮式里程计数据，实现机器人状态的连续预测。EKF 利用系统运动模型对状态进行先验估计，并结合预测误差与观测误差的协方差计算卡尔曼增益，以更新状态估计。

基于已知环境地图的AMCL被用于提供高精度的位姿观测。AMCL 通过粒子滤波方法匹配当前激光扫描与预先构建的地图，输出位姿估计及其协方差，用于对 EKF 状态的校正。

GNSS 不直接参与滤波器的状态更新，在系统初始化阶段用于提供初始全局位姿。GNSS 被用于运行过程中的状态监测。通过综合分析轮式里程计推算的位姿变化、AMCL 输出的协方差及 GNSS 信号质量，实现异常状态检测。当检测到机器人发生异常移动或定位失效时，系统触发重定位机制，利用 GNSS 当前的位姿估计重新初始化 AMCL 粒子集，以恢复系统定位能力。

通过将 IMU 与轮式里程计的短时精度、AMCL 的地图匹配能力以及 GNSS 的全局参考特性有机结合，可实现对机器人位姿的鲁棒估计，并具备重定位能力，多场景定位算法流程如图4所示。

图4 多场景定位算法流程

煤矿机器人定位系统测试与结果分析

4.1 动态点云去除与定位测试环境及方案

利用支持livox avia激光雷达的Fastlio去完成初步的建图，并修改代码使其能够保存VIEWPOINT关键字的PCD文件用于动态点云的处理算法获取先验地图。接下来将干净的点云地图进行二维栅格化处理，用于后续AMCL重定位及EKF融合多传感器，动态点云的处理需要SLAM算法获取先验点云地图与激光雷达扫描如图5所示。

图5 激光雷达单帧扫描

使用蒙迪欧轿数据采集车辆如图6所示，搭载livox avia激光雷达采集沈阳浑南大道工厂附近周边环境的数据，并结合gazebo仿真环境进行试验验证。为定量评估动态点云去除方法的性能，使用semindoor数据集提供的真值点云作为基准。

图6 数据采集车辆

4.2 动态点云去除效果验证

人工标注的原始点云数据semindoor场景动态点云去除如图7所示，其中白色部分代表人工标记的动态物体产生的点云，动态点云在原始地图中引入了明显的干扰，影响了环境特征的提取及定位精度。使用此方法处理后的结果如图7a。由于算法在处理前对地图进行了体素化和下采样，整体点云的密度低于真值点云。

图7 semindoor场景动态点云去除

通过对比图中算法识别并标记为白色的动态点云与真值标注的动态点云，识别出动态点云区域与真值标注高度一致。经过与真值数据的定量比较，笔者的方法成功去除了78.1%的动态点云，同时静态点云保留率达83.4%。动态点云去除效果提升了静态地图的准确性，为后续的机器人定位和导航任务提供了更加可靠的地图数据支持。

车辆采集的浑南大道动态点云去除结果如图8所示，选取前1 000帧作为试验样本。场景包括高架桥、车辆、路边的楼房、路灯等，环境相对复杂。经算法处理后，观察到系统对多数动态点云（包括车辆、行人等移动物体产生的鬼影）实现了有效识别并完成去除。算法的单帧平均处理时间为220 ms。本研究采用后处理方式对实时性要求较低，但针对大范围场景的点云处理需求，可通过多帧点云累积和批量处理策略进一步提升处理效率。

图8 车辆采集的浑南大道动态点云去除结果

4.3 异常状态检测与重定位系统测试

为了方便模拟机器人的“绑架”场景，测试重定位方法的效果，采用gazebo仿真煤矿地面场景存在的相对封闭和开放环境。场景内包括传送带、煤堆、移动的车辆、室内运动的行人、非平坦的地面等物体，gazebo煤矿地面仿真场景如图9所示。

图9 gazebo煤矿地面仿真场景

fastlio建图结果如图10所示，建立环境3维地图，图中绿色圆圈部分为场景中的车辆和行人移动在最终建图中留下的鬼影，直接栅格化会导致地图出现大量不属于环境本身的特征。

图10 fastlio建图结果

仿真建图动态点云去除如图11所示，观察对比图中红色部分、fastlio建图结果与仿真场景，地图中动态物体所遗留的鬼影点云基本去除。部分静态物体误识别为动态是由于fastlio本身缺少回环检测，点云地图误差累计，此方法在识别判定对比查询帧点云与先验地图时被影响从而误识别。将动态点云去除后的干净地图二维栅格化，仿真场景二维栅格化地图如图12所示，用于多传感器融合定位。

图 11 仿真建图动态点云去除

图 12 仿真场景二维栅格化地图

（1）激光雷达扫描匹配下的重定位验证

将二维栅格化的地图用于AMCL定位，在gazebo中强制平移机器人后，触发重定位流程，激光雷达扫描匹配重定位如图13所示，图13a为机器人在当前未知位姿下采集的真实激光雷达扫描图像，显示出清晰的结构边界，有利于几何特征的提取；图13b为在地图基础上、针对候选位姿生成的仿真激光扫描结果，作为匹配参照；图13c为将真实扫描数据根据当前估计位姿变换至地图坐标系下的投影，用以直观反映估计姿态与地图结构之间的几何一致性。

图 13 激光雷达扫描匹配重定位

图13d—图13f像进一步展示了从候选生成到最终位姿确定的全过程。图13d为真实扫描与仿真扫描的重叠图，其中红色点表示实际扫描点在估计位姿下变换后的坐标位置，点云之间具备较高的空间一致性，反映了较好的匹配质量；图13e展示了候选区域（红色边界）与采样位姿点（绿色点）的空间分布，候选区域由地图特征显著性信息生成，采样点则覆盖该区域内可能的位姿解。系统采用F1分数对各候选位姿进行匹配度量与排序，选取评分最优者作为AMCL 粒子滤波的初始化中心，实现重定位；图13f为最终估计位姿在全局地图上的可视化结果，红色标记点与环境结构边界保持高度对齐，表明估计结果具有较高的精度与稳定性。机器人实际位置如图14所示。

图 14 机器人实际位置

为量化系统的重定位性能，激光雷达重定位量化试验位置如图15所示，分别在A、B、C3处各进行10次重定位试验，激光雷达重定位量化试验误差结果如图16所示。结果表明：每处试验中平均有2～3次出现较大定位误差，依靠机器人自身移动引发的AMCL粒子重新分布，粒子权重能够逐步收敛至真实位置，最终实现可靠的重定位。

图15 激光雷达重定位量化试验位置

图16 激光雷达重定位量化试验绝对误差结果

（2）GNSS与AMCL位置监测重定位

为了验证系统在开阔空间机器人异常运动状态下的重定位能力，在Gazebo仿真环境中设置机器人初始位姿，并开启“绑架”检测模块。该模块实时监测以下2类指标：基于轮式里程计推算的位姿与AMCL当前位姿之间的欧式距离；AMCL输出的协方差指标。当检测到机器人位姿变化超过设定阈值（设为 7.0 m），或AMCL 协方差超过设定阈值（1.0），并且GNS信号质量满足设定标准时，系统将触发重定位流程。

机器人“绑架”检测与GPS重定位界面如图17所示，在模拟机器人被外力“绑架”后，轮式里程计推算的位姿与 AMCL 估计结果出现显著偏差（8.18 m），系统识别该异常状态后，根据当前 GNSS 提供的全局位姿重新初始化AMCL 粒子集，实现快速重定位。日志信息显示系统成功检测到位姿异常，并在满足重定位触发条件后，依据 GNSS 发布新的 AMCL 初始位姿。

图17 机器人“绑架”检测与GPS重定位界面

为量化GPS的重定位性能，GPS重定位量化试验如图18所示，分别在A、B、C三处进行重定位试验，每处重复试验10次，GPS重定位量化试验误差结果如图19所示。结果显示，A点由于处于相对开阔的环境中，无明显遮挡，因此在X轴和Y轴方向上的绝对误差均较小，稳定性良好；B点与C点因邻近墙体或障碍物，Y轴误差显著高于X轴，表明该方向的信号受遮挡影响更为明显。

图18 GPS重定位量化试验

图19 GPS重定位量化试验误差结果

尽管少量试验中存在较大误差，但整体误差水平仍处于可接受范围。通过后续机器人移动带来的AMCL粒子云的再分布与收敛，系统最终均能完成有效重定位，验证了该方法在不同场景下的稳健性与适应性。

总结

（1）采用分层的多分辨率深度图像对比的粗识别与基于改进ERASOR算法的精细去除，在可评估数据集上达到78.1%的动态点去除率与83.4%的静态点保留率，可生成干净的静态地图用于定位和规划。

（2）多源融合定位方案通过激光雷达点云与先验地图的特征匹配，结合GNSS定位数据，实现了自适应蒙特卡洛定位（AMCL）的自动初始化，并有效解决了机器人“绑架”"与重定位问题，实际测试表明，该方案在封闭室内环境和开阔室外场景均表现出稳定的定位性能。

（3）当前系统在室内外场景切换时仍存在流畅性问题，传感器融合权重主要依赖扩展卡尔曼滤波（EKF）的协方差矩阵自动调整。后续研究需要重点优化定位场景切换机制以及基于环境特征（煤矿地面的室内外、露天矿等）的动态权重分配策略。

编辑丨李莎

审核丨赵瑞

煤炭科学研究总院期刊出版公司拥有科技期刊21种。其中，SCI收录1种，Ei收录5种、CSCD收录6种、Scopus收录8种、中文核心期刊9种、中国科技核心期刊11种、中国科技期刊卓越行动计划入选期刊4种，是煤炭行业最重要的科技窗口与学术交流阵地，也是行业最大最权威的期刊集群。

期刊简介

《智能矿山》（月刊，CN 10-1709/TN，ISSN 2096-9139）是由中国煤炭科工集团有限公司主管、煤炭科学研究总院有限公司主办的聚焦矿山智能化领域产学研用新进展的综合性技术刊物。

主编：王国法院士

刊载栏目：企业/团队/人物专访政策解读视角·观点智能示范矿井对话革新·改造学术园地、专题报道等。

投稿网址：www.chinamai.org.cn（期刊中心-作者投稿）

👉 征稿函详见链接： 征稿┃《智能矿山》面向广大读者征稿，欢迎投稿

期刊成果：创刊5年来，策划出版了“中国煤科煤矿智能化成果”“陕煤集团智能化建设成果”“聚焦煤炭工业‘十四五’高质量发展”等特刊/专题30多期。主办“煤矿智能化重大进展发布会”“煤炭清洁高效利用先进成果发布会”“《智能矿山》理事、特约编辑年会暨智能化建设论坛”“智能矿山零距离”“矿山智能化建设运维与技术创新高新研修班”等活动20余次。组建了理事会、特约编辑团队、卓越人物等千余人产学研用高端协同办刊团队，打造了“刊-网-号-群-库”全覆盖的1+N全媒体传播平台，全方位发布矿山智能化领域新技术、新产品、新经验。

👉 具体详见链接：《智能矿山》创刊5周年回顾

联系人：李编辑 010-87986441

邮发代号：82-476

👉 期刊订阅详见链接：欢迎订阅┃《智能矿山》杂志2026年订阅开始了！

往期荐读