煤矿消防灭火机器人系统设计

陈骋，陈秀田，朱明亮

（1.中煤科工集团沈阳研究院有限公司，辽宁 抚顺 113122；2.华晋焦煤有限责任公司,山西 吕梁 033000；3.煤矿安全技术国家重点实验室，辽宁 抚顺 113122）

0 引言

我国煤炭资源丰富，是世界上最大的煤炭生产和消费国。由于作业设备和工艺相对落后，煤矿井下事故发生率较高，煤矿安全形势严峻[1]。《全国安全生产专项整治三年行动计划》中明确指出，要全面推进煤矿智能化建设[2]，实现煤矿高危险现场少人化、无人化。这给煤矿机器人发展带来新的机遇和挑战[3]。煤矿井下作业环境恶劣且存在高危气体，消防灭火机器人不仅能够帮助救援人员率先探明灾情、预测火势、熟悉环境，还能代替救援者进入火灾现场援救受困人员，更能发挥灭火功效，高效完成抢险救灾工作，是煤矿井下防灭火治理及灾后救援的重要装备。

目前煤矿机器人研究在底盘设计及防爆处理等方面取得进展，机器人底盘环境适应性及越障能力显著提高，但针对煤矿井下复杂地形的机器人定位、路径规划[4]、火灾险情识别及精准灭火等方面的研究较少，相关技术仍需重点攻克。为减少煤炭生产环节消防安全隐患，提升煤矿防灭火及救援时效性，本文设计了煤矿消防灭火机器人系统，结合超宽带（Ultra Wide Band，UWB）和激光雷达技术，采用自适应蒙特卡洛定位（Adaptive Monte Carlo Localization，AMCL）算法实现机器人定位，采用优化A*算法实现机器人路径规划，采用模板匹配方法识别险情目标，并通过云台控制将消防弹投射至险情目标，为煤矿精准灭火提供技术支持。

1 系统总体设计

煤矿消防灭火机器人系统包括机器人硬件子系统、定位与路径规划子系统、目标识别与投掷子系统3 个部分，如图1 所示。机器人硬件子系统为机器人基础运动和信息采集提供保障。定位与路径规划子系统读取UWB 定位模块数据、激光雷达数据和里程计信息，实现机器人初始化定位、实时避障运动和路径规划等功能。目标识别与投掷子系统通过调用摄像头的视频流信息，采用模板匹配方法进行特定目标识别，并控制二轴云台运动和消防弹投掷。

图1 煤矿消防灭火机器人系统结构Fig.1 Structure of coal mine fire-fighting robot system

2 机器人硬件子系统

机器人底盘搭载基于STM32F427 芯片的主控板，采用4 个由M3508 减速电动机驱动的麦克纳姆轮作为动力组件，进行前行、横移、斜移、旋转及其组合方式的多种运动，实现机器人全向移动。

激光雷达是一种通过发射激光束来探测目标对象位置及速度等特征量的传感器[5-6]，本文选用RPLIDAR A3M1 型激光雷达。

机载计算设备选用Intel NUC8i7BEH 型工业计算机，运行机器人操作系统（Robot Operating System，ROS），实现图像处理和机器人运动控制等相关功能。

UWB 是一种无线载波通信技术，具有功耗低、抗干扰能力和穿透能力强、定位准确度较高等优点[7-8]。本文选用DJI-RM-V1 UWB 系统，其最高水平定位精度为5.5 cm，最高垂直定位精度为6.5 cm，位置更新频率为50 Hz。

3 定位与路径规划子系统

3.1 实时定位

3.1.1 定位原理

机器人开机后，首先通过UWB 定位模块确定机器人当前位置，在获取到的位置附近生成一系列正态分布的坐标点，再对这些坐标点进行筛选。针对每个坐标点，调用激光雷达扫描周边环境，得到二维点云数据；采用迭代最近点（Iterative Closest Point,ICP）算法对二维点云数据和预先存储的地图数据进行比较，根据误差最小原则确定机器人在该坐标点的姿态角及其置信度。对所有坐标点姿态角的置信度进行比较，将置信度最高的坐标点作为机器人的真实坐标，并将机器人在该坐标点的姿态角作为初始化时的真实姿态信息。

采用AMCL 算法[9]实现机器人实时定位。AMCL 算法是适用于二维平面内机器人运动位姿估计的算法，其流程如图2 所示。

图2 AMCL 算法流程Fig.2 Flow of the AMCL algorithm

AMCL 算法实现步骤：

（1）随机生成一批粒子并进行地图载入工作，根据惯性测量装置和里程计数据更新粒子的位姿信息。

（2）通过UWB 数据判断机器人位置，通过激光雷达数据判断机器人与周边障碍物的距离（根据灰度值判断是否为障碍物），对初始生成的粒子进行置信度更新。距离机器人越远的粒子，其置信度越低；与激光雷达所测得的周边障碍物姿态信息越不匹配的粒子，其置信度越低。

（3）根据粒子置信度高低赋予权值，置信度越高，权值越大。舍弃置信度低于阈值的粒子。

（4）重采样阶段，根据粒子权值对粒子进行复制，复制份数与权值大小成正比。采用KL 散度（Kullback-Leibler Divergense，KLD）算法确定粒子数量。KLD 算法是一种自适应的粒子数量确定算法，可使粒子数量估计值与后验值之间的偏差不超过设定界限，即新生成粒子的数量随迭代次数增加而逐渐减少。粒子之间的距离不超过规定距离0.05 m时，认为定位成功，输出机器人当前位置。

在实际应用中，可能出现机器人运动过程中定位丢失的情况。为解决该问题，采用ROS 中的SnapMapICP 包进行机器人重定位[10-11]。

3.1.2 定位测试

首先获取UWB 信号，确定机器人初始位置信息，使用激光雷达实时扫描得到点云数据，并与该位置地图数据进行匹配。初始化阶段，机器人姿态角的预测精度达3～5°，耗时1～2 s。在运动过程中，通过UWB 模块判断机器人大致位置，根据激光雷达数据进行精确定位修正，测试结果如图3 所示。图3（b）中十字坐标位置为机器人中心位置，与障碍物（黄色）之间的距离以二维云图方式体现，实测定位精度达5～10 cm。

图3 机器人实时定位测试结果Fig.3 Test results of robot real-time positioning

3.2 路径规划

3.2.1 A*算法原理

A*算法作为一种求解静态网络最短路径最有效的直接搜索算法，综合了深度优先算法和广度优先算法的特性，被广泛应用于静态场景导航中[12-13]。该算法在深度优先策略的基础上增加了一个启发函数，使得搜索方向具有目标性。但是当选取的启发因子不适合当前规划任务时，会导致路径规划失败。针对该问题，对A*算法进行优化，加入寻找新目标点的容忍距离参数。加入该参数后，若路径规划失败，则在目标位置附近寻找另一个可以到达的位置替换当前目标位置，重新规划一条全局路径[14]。

3.2.2 路径规划测试

机器人路径规划测试结果如图4 所示，图4（a）中右上角红色为机器人位置，绿色曲线为通过优化A*算法规划的路径，绿色曲线终点为目标灭火点。

图4 机器人路径规划测试结果Fig.4 Test results of robot path planning

在试验场地内设置不同着火点进行测试，实测数据见表1。可看出优化A*算法用时短，规划的路径合理。

表1 路径规划指标Table 1 Path planning indexes

4 目标识别与投掷子系统

4.1 目标识别

通过先验知识生成着火点特征图像库，将特征图像作为特征匹配环节的模板图像。读取摄像头采集的视频流，并对模板图像和视频帧进行归一化处理。采用滑动窗口方式对模板图像（分辨率为128×128）和视频帧（分辨率为1 920×1 080）进行比较，计算相关系数R。相关系数R反映模板图像与待检验图像的相关程度，当模板图像与视频帧中某区域完全一致时，R=1。设置相关系数阈值，将相关系数大于阈值且相关系数值最大的区域确定为着火点目标。

获取着火点中心位置，将云台当前位置与着火点中心位置的偏差值作为给定信息传输到云台控制模块。

在分辨率为640×480 的较复杂场景下，在距离机器人3 m 范围内，着火点目标识别测试结果见表2。可看出机器人对动态多着火点的虚警率在10%以内，识别率达90%，满足应用要求。

表2 着火点目标识别测试结果Table 2 Test results of ignition points detection

4.2 云台控制

云台控制的目的是准确、快速地跟踪着火点位置，使消防弹能够投射至目标区域。采用速度、位置双闭环的串级控制器实现云台俯仰轴和偏航轴角度控制。以偏航轴角度控制为例，串级控制流程如图5所示。

图5 云台串级控制流程Fig.5 Cascade control process of the pan-tilt-zoom（PTZ）

通过速度、位置双闭环PID 调节[15]，使得云台响应曲线较平滑且准确地跟随给定曲线。在实际测试中，即使目标识别模块给出的控制量波动较大，云台的实际响应曲线也较平滑，且具有一定的超前预测效果[16]。通过整定串级控制器的PID 参数，可弥补目标识别频率略小于云台控制频率的不足，抵消零阶保持效果[17]，实现对险情目标的精确处理。

5 结论

（1）结合UWB、激光雷达技术，采用ICP 算法实现机器人定位初始化；在实时定位阶段，结合惯性测量装置和里程计数据，采用AMCL 算法实现机器人定位，实测定位精度达5～10 cm。

（2）在A*算法中，当选取的启发因子不适合当前规划任务时，会导致路径规划失败。针对该问题，对A*算法进行优化，加入寻找新目标点的容忍距离参数。加入该参数后，若路径规划失败，则在目标位置附近寻找另一个可以到达的位置替换当前目标位置，重新规划一条全局路径。测试结果表明，采用优化A*算法进行路径规划用时短，规划的路径合理。

（3）通过先验知识生成着火点特征图像库，将特征图像作为特征匹配环节的模板图像，采用模板匹配方法识别险情目标。测试结果表明，机器人对动态多着火点的虚警率在10%以内，识别率达90%，满足应用要求。

（4）采用速度、位置双闭环的串级控制器实现云台俯仰轴和偏航轴角度控制，能够准确、快速地跟踪着火点位置，使消防弹投射至目标区域。