0 引 言
目前，掘进机作为一种主要的掘进工具，在我国煤矿领域具有极其重要的作用和地位[1]。比起人工手动挖掘作业，悬臂式掘进机的应用大幅提高了井下煤矿开采的工作效率，降低煤矿井下工作成本的同时，改善了工人的工作环境。但由于煤矿井下工作环境复杂且具有较强的约束性，如巷道内部环境视野不开阔，光线投射不充足，温度不稳定等，掘进机的调动仍需大量工作人员进行协调指挥，导致掘进机工作效率过低，工作人员劳动强度过高，尤其是操作手的生命安全得不到高效保障[2]。“十二五”规划以来，国家明确提出深入井下无人开采技术的实现目标，井下掘进机无人化、自动化开采已经成为未来掘进机走向智能化开采道路的必然趋势[3-5]。
随着科学技术水平的不断提高，煤矿开采工作需要集智能化和自动化于一体的采掘装备，而掘进机在煤矿井巷内的自主调动与采掘技术正是当下研究热点[6-7]。传统的导线测量、贯通设计、平差计算等技术方案不能直接应用在狭长密闭的煤矿巷道环境建模问题上，且实施过程中难以保证建模精度，在掘进机自主调动与采掘方面得不到适用[8-9]。而随着即时定位与地图构建(SLAM)技术的兴起，激光SLAM技术得到迅速发展，相对视觉SLAM技术具有更广阔更灵活的应用空间和高效的应用性。清华大学研制了室外智能移动机器人试验平台THM R-V，实现了机器人导航及监控等功能[10]。上海交通大学研制了移动机构试验平台211AMCTB，针对凸型地形等各种典型障碍进行避障[11]。东南大学大幅改善了救援机器人的机械结构和控制，设计了一种扩展性强和鲁棒性好的操控系统结构[12]。国防科技大学对目前较常用的迭代最近点(ICP)方法进行了改进[13]，增大了ICP 点云匹配精度，提高了 ICP 算法匹配的效率。煤炭科学研究总院提出了基于粒子群优化的煤矿机器人FastSLAM算法研究，此算法过于依赖里程计[14]，目前，尚未在煤矿井下掘进机上运用SLAM技术。
激光SLAM技术可以使掘进机在狭长密闭的巷道环境中，通过运动模型结合数学统计估算掘进机位置，并通过激光雷达传感器的外部环境信息建立巷道环境地图，同时完成对掘进机在巷道内部掘进前方障碍物的检测。笔者考虑综掘工作面少人化、无人化的发展趋势，针对煤矿井下掘进机在巷道内部难定位、难定向的问题展开研究，提出了基于激光SLAM技术的煤矿掘进机井下巷道环境建模与障碍检测的方法。将激光SLAM技术应用到煤矿掘进机上，将狭长密闭的巷道环境建模问题转换为数理上的概率统计与数学模型预测问题，实现对巷道边缘形状的准确判断及巷道内部障碍物做出有效检测，为预测掘进方向和巷道形状提供保障。
1 巷道环境建模与障碍检测方法设计
很多关于SLAM的算法被应用在传统环境中，保证了在低成本传感器下可以获取到高精度的环境地图。目前主流环境建模方案分为3种：激光SLAM、视觉SLAM和多传感器融合的以视觉为主体的SLAM[15]。
激光雷达在井巷中可以全天候工作，不受巷道内部昏暗的视线环境和不定的温度影响，且防爆性能优于相机。从SLAM前端硬件选型问题出发，激光雷达在煤矿井巷中的适应性要更强一些。视觉SLAM依靠相机来完成一系列定位、轨迹规划和环境建模问题，但相机在巷道环境下受光照强度影响显著，在视野昏暗的巷道环境下，无法正常工作，提取不到关键帧。所以视觉和多传感器融合的以视觉为主体的SLAM在井下环境中的可靠性同样不如激光SLAM。
笔者研究了基于激光SLAM的煤矿掘进机井下巷道环境建模与障碍检测方法，研究方法系统设计框架如图1所示。
图1 掘进机井下巷道环境建模与障碍检测方法系统设计
Fig.1 System design of environmental modeling and obstacle
detection method for underground roadway of tunneling machine
2 系统坐标系与数学模型的建立
此次研究方案涉及2个运动学模型：煤矿巷道中掘进机运动学模型和激光雷达观测模型。
2.1 煤矿巷道中掘进机运动学模型
掘进机在移动过程中，需要在其上安装激光雷达对外部环境信息进行感知，从而完成对煤矿井巷的环境建模。然而安装着激光雷达的掘进机的位姿是随着时间的变化而变化，所以没有确定的方法对整体系统进行准确的表示。为解决这一问题，提出基于巷道环境建模的掘进机运动学模型这一概念。掘进机运动学模型主要用来表示掘进机和雷达在巷道内部的相对位置关系，激光雷达自身位置以及掘进机自身位置。
掘进机基于巷道环境建模如图2所示[16]，在时间坐标系中定义的掘进机运动学模型。其中，M点和P点分别为掘进机重心位置和激光雷达放置位置。设两点距离为L，掘进机行进距离为D。
X1OY1—全局方向坐标系；X2OY2—掘进机机体方向坐标系
图2 掘进机运动学模型
Fig.2 Kinematic model of roadheader
掘进机位置在全局坐标系下可表示为
(1)
激光雷达位置表述可以通过掘进机距离关系表述方式变换到全局坐标系下：
(2)
设掘进机相邻时刻掘进机掘进距离为Dk，设掘进机前进为掘进距离正方向，掘进机后退为掘进距离负方向。Xk、Yk分别为k时刻下掘进机在时间坐标下的横坐标和纵坐标。假设测得掘进机航向角为θk，相邻时刻掘进机航向角变化为Δθ，则掘进机位置矩阵可以表示为
(3)
2.2 基于巷道环境建模的激光雷达观测模型
激光雷达坐标系规定掘进机前进方向为雷达坐标系的x轴方向，顺时针旋转且与x轴垂直方向为激光雷达坐标系的y轴方向，旋转角度以顺时针为正方向，测量距离d是以雷达旋转中心为起始原点。激光雷达观测如图3所示。
图3 雷达观测
Fig.3 Radar observation
激光雷达对外部环境的信息感知观测值H包括距离信息s和角度信息a两部分，可表示为
(4)
激光雷达观测模型通过感知的距离信息，可以确定掘进机在掘进过程中的相对位置，随着掘进机的缓慢掘进[17]，雷达观测模型也会发生一定的变化。雷达坐标系下的任意激光点(xP,yP)映射到时间坐标系下可表示为(xi,yi)。
(5)
雷达的观测模型可表示为
3 基于巷道环境建模的激光SLAM算法
分析比较现有的主流激光SLAM算法，理论上确定HectorSLAM算法和Gmapping算法的适用性。并完成对Gmapping算法进行改进，以及HectorSLAM算法的参数优化。
3.1 算法分析
3.1.1 Cartographer算法分析
Cartographer算法将激光获取的当前帧与子地图进行环境匹配，规避了粒子滤波方法中对粒子大量消耗的问题[18]。核心函数见式(7)，主要构建大范围的栅格环境地图，在狭长密闭的长廊式的结构空间中，匹配效果不好。尤其是在没有回环检测的环境下，误差较大，不做采用。
(7)
式中：α为残差，是对激光观测值与算法预测值差值的描述；δ为激光观测值与算法预测值差值的方差函数值；Lδα为残差α的损失函数，具体表示为：当|α|≤δ时，算法将α作平方进行平方回环消除误差；其他情况下，算法将α直接带入系统进行线性回环消除误差。
3.1.2 Gmapping算法分析
Gmapping是基于粒子滤波的SLAM算法，主要适用于道路平坦的场景中以及长廊的环境下。因为具有粒子滤波的普遍特点，所以粒子数目越多，算法的准确性越高，环境建模效果越好[19]。对计算机内存消耗较大，在粒子滤波的重采样过程中，会降低建模精度。核心函数见式(8)，通过控制数据u1:t和观测数据z1:t来求解位姿和地图的联合概率分布。
(8)
式中：z1:t为t时刻观测数据；u1:t-1为t-1时刻的控制数据；P(m|x1:t,z1:t)为t时刻栅格地图m占用概率；P(x1:t|z1:t,u1:t-1)为在t时刻观测数据，t-1时刻的控制数据的前提下，t时刻下位姿x出现的概率。
3.1.3 HectorSLAM算法
HectorSLAM算法无需使用里程计信息，目前主要应用在无人机领域。对激光得到的点数据与当前地图进行最小误差优化所用的方法是采用高斯-牛顿法迭代求解最小误差，得到最优的位姿估计，见式(9)。在进行匹配时，采用了多分辨率的地图匹配方法，解决了在求解机器人位姿时陷入局部极小的问题。
x=argmin∑[1-M(Si(x))]2
(9)
式中：Si(x)表示将某一时刻第i个激光点转换到栅格地图坐标系下； 表示某一M(Si(x))时刻第i个激光点转换到栅格地图坐标系下，栅格被占用的概率，范围为0～1；x为对应时刻下的机器人位姿。
3.2 基于巷道环境建模的算法改进
3.2.1 自适应性Gmapping算法
Gmapping算法的使用需要激光数据和里程计数据，在煤矿复杂巷道内部，里程计数据信息经常失效，导致Gmapping算法无法在巷道内部直接使用，所以提出一种自适应性Gmapping算法，使用laser-scan-matcher功能包作为独立的里程计计算器供Gmapping算法使用，laser-scan-matcher功能包可以在没有其他传感器提供任何测距估计的情况下使用，既排除了其他干扰因素的影响，又可以作为独立的里程计算器来代替实际的里程计，解决了Gmapping算法无法在巷道内直接使用的问题。算法原理如图4所示。
图4 自适应性Gmapping算法原理
Fig.4 Principle of adaptive gmapping algorithm
3.2.2 优化HectorSLAM算法参数
在HectorSLAM算法中，角度阈值和距离阈值的参数取值对地图构建效果影响显著。其中，角度阈值为巷道环境建模过程中执行更新的阈值，掘进机平台以m为单位，当激光雷达距离上一次环境模型更新转动达到参数要求时执行模型更新；距离阈值同样是巷道环境建模过程中执行更新的阈值，掘进机平台以m为单位，当激光雷达直行距离和上一次环境模型更新比较达到参数要求后执行地图更新。已知角度阈值在小于0.4的范围内作用有限，所以对其初始值0.06不做修改，通过对距离阈值参数进行优化，确定当距离阈值处于0.05时，巷道环境建模效果达到最佳[20]。
4 改进的巷道环境建模算法试验
4.1 激光雷达
此次采用的是目前使用最多的低价国产激光雷达RPLidarA2雷达，包括激光头和驱动模块。内置电动机旋转机座，360°扫描范围，建图精度为厘米级。具体性能参数如下：
测距范围/m8扫描角度/(°)360单次测距时间/ms0.125测量频率/Hz8 000角度分辨率/(°)0.9
4.2 模拟巷道环境搭建
煤矿井下综掘环境包括：相对平整的工作地面以及昏暗的灯光环境，未知的障碍物。考虑煤矿井巷尺寸，结合实验室现有条件，采用楼道长廊模拟煤矿巷道狭长封闭的环境特点，如图5所示。
图5 狭长密闭试验长廊
Fig.5 Long and narrow closed experimental gallery
4.3 自适应性Gmapping算法巷道环境建模
采用自适应性Gmapping算法在试验长廊中进行建模试验，3次试验结果如图6所示。从图6可以看出，在煤矿井巷内部，自适应性Gmapping算法采用的软件功能包预算的里程计信息不准确，依旧不可以克服此算法依赖里程计进行建模的难题。
4.4 HectorSLAM算法巷道环境建模
设定 HectorSLAM算法程序中角度阈值为0.06，距离阈值分别为0.05、0.10、0.20。在试验长廊中对每次赋值进行2组试验，共计6次，试验结果如图7所示。图7a为优化前HectorSLAM算法建模结果，图7b和图7c为进行参数优化后的建模结果。由图7可知，在角度阈值不变时，随着距离阈值的增大，建模效果逐渐变差；相反，当距离阈值取值变小时，建模效果逐渐变好。从图7c可以看出，当距离阈值为0.05时，巷道建模更为精确，地图匹配逐渐准确，测量精度受雷达选型影响为厘米级。
综上，选择距离阈值为0.05，角度阈值为0.06时的HectorSLAM算法进行后续试验验证。
图6 自适应性Gmapping算法试验结果
Fig.6 Experimental results of adaptive Gmapping algorithm
图7 HectorSLAM算法试验结果
Fig.7 Experimental results of hectorSLAM algorithm
5 片帮巷道环境建模与障碍检测试验
5.1 片帮巷道环境搭建
煤矿井下发生片帮的巷道从环境建模角度出发，可以转换为对连续不规整边界异物线条特征的提取问题。能否准确提取巷道边缘异常线条特征直接影响到激光SLAM技术对巷道形状和掘进机掘进方向的预测。此次针对片帮巷道进行试验仿真的环境搭建如图8所示。
图8 片帮巷道试验环境
Fig.8 Experimental environment of spalling roadway
5.2 片帮巷道建模与障碍检测结果
采用优化后的HectorSLAM算法对片帮巷道试验环境进行建模，结果如图9所示。图9中黑色圆圈为此次模拟片帮巷道主体线条特征提取结果。受试验平台高度的影响，图中黑色点迹代表障碍物位置。利用RVIZ软件对图9中标出的物体A、B、C的长度进行建模测量，建模精读对照见表1。
从表1可知，受激光雷达选型影响，激光雷达建模误差控制在0.01～0.02 m的工程允许范围内，建模精度为厘米级。后续可以结合生成的增量式巷道栅格地图，根据巷道内部实际情况规避障碍物，为掘进机的掘进方向做进一步控制研究。
图9 片帮巷道试验建模结果
Fig.9 Experimental modeling result of spalling roadway
表1 建模精度对照
Table 1 Modeling accuracy comparison
项目实际测量距离/m建模测量距离/m误差/m长度A1.611.625 030.015 03长度B0.530.542 780.012 78长度C0.400.410 9980.010 998
6 结 论
提出基于激光SLAM技术的煤矿掘进机井下巷道环境建模与障碍检测的方法，结合现有建模算法并对其进行优化改进，将激光SLAM技术应用到煤矿掘进机上，获得如下结论：
1)建立了煤矿井下掘进机运动学模型与激光雷达观测模型，可以将狭长密闭的煤矿巷道环境建模问题转换为数理上的概率统计与数学模型预测问题。
2)对二维激光SLAM开源算法进行试验研究，结果表明HectorSLAM算法经参数优化后更适用于巷道环境建模以及障碍检测。
3)将激光SLAM技术运用到煤矿井下，生成的栅格地图模型可以对掘进机的行进、巷道形状起到很好的预测作用，尤其是片帮巷道环境，方法适用效果显著。
[1] 陈慎金,成 龙,王鹏江,等.基于掘进机位姿测量系统的自主标定方法误差分析[J].煤炭学报,2018,43(9):2647-2652.
CHEN Shenjin，CHENG Long，WANG Pengjiang，et al．Error analysis of autonomous calibration strategy based on roadheader attitude measurement system[J]．Journal of China Coal Society，2018，43( 9) :2647-2652．
[2] 刘 超,符世琛,成 龙,等.基于TSOA定位原理混合算法的掘进机位姿检测方法[J].煤炭学报,2019,44(4):1255-1264.
LIU Chao，FU Shichen，CHENG Long，et al．Pose detection method based on TSOA positioning principle for roadheader[J]．Journal of China Coal Society，2019,44(4)：1255-1264.
[3] 杨健健,唐至威,王晓林,等.煤巷掘进机纠偏路径随机约束环境建模研究[J].矿业科学学报,2019,4(1):58-65.
YANG Jianjian，TANG Zhiwei，WANG Xiaolin，et al．Study on stochastic constraint environment modeling of rectification path of coal roadway boring machine [J].Journal of Mining Science,2019,4(1):58-65.
[4] 毛清华,张旭辉,马宏伟,等.多传感器信息的悬臂式掘进机空间位姿监测系统研究[J].煤炭科学技术,2018,46(12):41-47.
MAO Qinghua,ZHANG Xuhui,MA Hongwei,et al.Study on spatial position and posture monitoring system of boom-type roadheader based on multi sensor information[J].Coal Science and Technology,2018,46(12):41-47.
[5] 杨仁树.我国煤矿岩巷安全高效掘进技术现状与展望[J].煤炭科学技术,2013,41(9):18-23.
YANG Renshu.Present status and outlook on safety and high efficient heading technology of mine rock roadway in China [J].Coal Science and Technology,2013,41(9):18-23.
[6] 贾文浩,陶云飞,符世琛,等.悬臂式掘进机位姿检测方法研究进展[J].煤炭科学技术,2016,44(S1):96-101.
JIA Wenhao，TAO Yunfei，FU ShiChen，et al．Research advances on position and attitude measuringmethods of boom-type roadheader[J].Coal Science and Technology,2016,44(S1):96-101.
[7] 张敏骏,臧富雨,吉晓冬,等.掘进机远程监控系统设计与位姿检测精度验证[J].煤炭科学技术,2018,46(12):48-53.
ZHANG Minjun,YAN Fuyu,JI Xiaodong,et al.Design of remote monitoring system for tunneling machine and verification of pose detection accuracy[J].Coal Science and Technology,2018,46(12):48-53.
[8] 杨文娟,张旭辉,马宏伟,等.悬臂式掘进机机身及截割头位姿视觉测量系统研究[J].煤炭科学技术,2019,47(6):50-57.
YANG Wenjuan,ZHANG Xuhui,MA Hongwei,et al.Research on position and posture measurement system of body and cutting head for boom-type roadheader based on machine vision[J]. Coal Science and Technology,2019,47(6):50-57.
[9] 王春林,王庆林,王玉琨.基于OpenGL的三维巷道漫游模型构建[J].煤炭科学技术,2009,37(9):75-78.
WANG Chunlin,WANG Qinglin,WANG Yuxi.Eatablishnment on roaming model of mine 3D roadway based on OpenGL[J].Coal Science and Technology,2009,37(9):75-78.
[10] 徐正飞．关节式移动机器人自主越障行为协调和基于行为的反应控制[D]．上海:上海交通大学，2001．
[11] 沈 慧．Robo Cup 救援机器人研究:操控系统设计与实现[D]．南京：东南大学，2010．
[12] 王 欣，张明明，于 晓，等.应用改进迭代最近点方法的点云数据配准[J]．光学精密工程，2012，20(9)：2068-2077．
WANG Xin,ZHANG Mingming,YU Xiao,et al.Application of point cloud data registration with improved iterative nearest point method [J].Optical Precision Engineering,2012,20(9):2068-2077.
[13] 黄开宏，杨兴锐，曾志文，等．基于 ROS 户外移动机器人软件系统构建[J]．机器人技术与应用，2013(4)：37-41．
HUANG Kaihong,YANG Xingrui,ZENG Zhiwen,et al.Based on ROS outdoor mobile robot software system construction [J].Robotics and Applications,2013(4):37-41.
[14] 许鹏程.基于粒子群优化的煤矿井下机器人FASTSLAM算法研究[D].北京：煤炭科学研究总院,2017.
[15] 张 玉,杨 维.矿井巷道综合衰减模型与无线传输误码率性能综合比较[J].煤炭学报,2011,36(9):1581-1586.
ZHANG Yu，YANG Wei．Comprehensive comparison of comprehensive attenuation model of mine roadway and wireless transmission BER performance[J].Journal of China Coal Society,2011,36(9):1581-1586.
[16] CHOSET H M,HUTCHINSON S,LYNCH K M,et al.Principles of robot motion:theory,algorithms,and implementation[M].Cambridge MA MIT press,2005.
[17] 常 皓,杨 巍.基于全向移动模型的Gmapping算法[J].计量与测试技术,2016,43(10):1-4.
CHANG Hao，YANG Wei．Gmapping based on omni-directional wheeled mobile robot[J].Metrology and Testing Techniques,2016,43(10):1-4.
[18] 黄 婷.基于激光雷达和深度相机的AGV自主定位方法研究[D].广州：华南理工大学,2018.
[19] 刘文之.基于激光雷达的SLAM和路径规划算法研究与实现[D].哈尔滨:哈尔滨工业大学,2018.
[20] 高文研,平雪良,贝旭颖,等.两种基于激光雷达的SLAM算法最优参数分析[J].传感器与微系统,2018,37(4):28-30，33.
GAO Wenyan,PING Xueliang,BEI Xuying,et al.Optimal parameter analysis of two Lithra-based SLAM algorithms [J].Sensors and Microsystems,2018,37(4):28-30，33.
