0 引言

对煤矿井下巷道环境和设备运行状态进行安全巡检，是煤矿企业安全生产的重要保障。传统煤矿井下的巡检分为2种：一是人工巡检，由于效率低、强度大、危险性高而逐渐被淘汰；二是视频监控，存在覆盖面积小、投入成本高、维护效率低等问题[1]。随着机器人技术的快速发展，人们希望危险性、重复性的巡检工作能由机器人替代自己完成，尤其是近年来，在全世界引发了应用于煤矿巡检领域的特种机器人的研究热潮[2-3]。定位导航技术是煤矿井下巡检机器人一个非常重要的研究课题，也是实现真正智能化和完全自主移动的关键所在，它的核心是要解决关于机器人“在哪”和“要去哪”的问题[4-5]，解决该问题的关键点在于如何实现巡检机器人的自主定位。

早期的巡检机器人通过将已知地图存储在机器中来实现自主定位和导航，但并不是所有的离线地图都与实时环境信息能保持高度的一致性，因此，需要根据实际情况来解决这种离线式工作的局限性问题。为此，SLAM(Simultaneous Localization and Mapping，同步定位与地图构建)技术应运而生。SLAM是解决煤矿井下无GPS环境下移动机器人自主定位的最佳手段。SLAM最早于20世纪80年代被提出，但不能准确地通过数学形式表达，之后人们逐渐提出了基于卡尔曼滤波的SLAM算法并引入概率论研究方法[6]。20世纪80年代末，R.Smith等[7]明确地通过概率随机估计方式来表述SLAM问题，认为机器人在定位过程中会不可避免产生误差的累积，于是提出采用状态向量及其协方差矩阵来表示机器人位置和路标位置，称为随机地图或扩展卡尔曼滤波。21世纪初期，SLAM的相关研究得到了迅速发展，主要集中在降低算法的运算量方面。J.E.Guivant等[8-9]提出了基于扩展卡尔曼滤波的改进算法，能够有效地降低计算复杂度。M.Montemerlo等[10]提出的Fast-SLAM算法使用粒子滤波算法，其优点是可以通过控制粒子数量来调整计算量，同时能够处理不确定数据之间的关联问题[11]。

国内对于SLAM的研究主要是通过激光雷达建立栅格地图，采用多种定位与建图的不同组合进行SLAM算法改进。徐曙[12]提出了一种基于A*算法的逆向D*算法。厉茂海等[13]提出了以Rao-Blackwellized粒子滤波为基础，结合融合自适应算法的粒子重采样算法。张文玲[14]利用自适应的SLAM算法更好地实现了移动机器人的自身定位和环境地图建立。朱磊等[15]在未知环境SLAM 中使用了人工鱼群算法来更新预测粒子，使预期粒子更接近机器人的实际姿态。罗元等[16]将RBPF(Rao-Blackweuized Particle Filter)和退火参数优化混合提议分布相结合，实现室内机器人的SLAM。以上研究成果为研究煤矿井下巡检机器人的同步定位与地图构建方法提供了理论依据。

在此基础上，本文研究了基于Fast-SLAM的巡检机器人同步定位与地图构建方法，通过激光雷达获取实时环境信息，并进行多特征匹配来达到定位和减小定位误差的目的，同时构建出机器人周围的环境地图。该方法是针对巡检机器人在煤矿井下无GPS情况下实现自主定位的最佳解决方案。在机器人自主导航过程中，无论是局部实时避障还是全局规划，都需要精确地知道巡检机器人的当前位置。定位是完成导航任务首先要解决的问题，同时也是其他高级导航任务的基础，精确的定位是提高巡检机器人性能的关键。

1 煤矿井下巡检机器人控制系统设计

1.1 系统总体架构

煤矿井下巡检机器人控制系统是以轮式移动机器人为本体结构、以ROS(Robot Operating System)开源机器人系统为总体架构，采用上下位机二级式分布共同搭建的，上下位机之间采用WiFi通信。整个系统由轮式机器人主体、车载计算机、双目相机、激光雷达、电动机驱动器、直流电动机、红外传感器、超声传感器、惯导元件、云台摄像机等硬件构成，总体架构如图1所示。轮式机器人主体包含2个驱动轮和2个同步轮，电动机驱动器与车载计算机通过串口通信。双目相机用来获取图像信息，以进行机器人位姿状态估计，通过USB与车载计算机通信。激光雷达能够实时获取机器人与周围环境的距离测量值，从而实现地图构建，通过TCP/IP与车载计算机相连。云台摄像机负责监测区域的实时监控。

1.2 系统工作原理

系统工作原理：以轮式机器人为主体，以高性能计算机作为控制核心，以直流电动机、编码器、霍尔传感器作为运动控制系统，以红外热像仪、可见光摄像机、气体传感器等作为远程监测系统，以超声、红外传感器作为应急避障系统，以多线激光雷达和双目相机结合而成的多传感器为环境感知系统。其中，激光雷达负责周围环境的地图构建，双目相机负责自身状态估计，与激光雷达所建地图相结合，实现机器人实时定位、构建地图的功能，并根据地图环境信息和机器人位姿信息进行行为决策。其工作原理如图2所示。

图1 煤矿井下巡检机器人控制系统总体架构
Fig.1 Overall architecture of coal mine underground inspection robot control system

图2 煤矿井下巡检机器人控制系统工作原理
Fig.2 Working principle of coal mine underground inspection robot control system

1.3 系统软件实现

系统软件功能如图3所示。根据不同的功能要求将机器人控制系统划分为4个模块：环境感知模块、地图构建模块、行为决策模块、运动控制模块。其中，环境感知模块的功能是感知机器人周围的环境信息，并基于传感器数据为环境建立可靠和详细的描述；地图构建模块的任务是利用激光雷达采集到的实时距离信息，结合SLAM算法和基于粒子滤波的自适应蒙特卡罗(Adaptive Monte Carlo Localization，AMCL)算法构建环境栅格地图和定位；行为决策模块的任务是利用所有输入的原始数据计算得出行为层面的决策；运动控制模块的设计目标在于让机器人尽可能地按照行为决策规划的动作序列运动。

图3 煤矿井下巡检机器人控制系统软件功能
Fig.3 Software function of coal mine underground inspection robot control system

2 巡检机器人同步定位与地图构建

SLAM可以描述如下：机器人在未知环境中开始移动，在移动过程中利用内部传感器数据进行自身状态估计，并结合外部传感器感知到的外部环境信息进行自身定位，同时在自身定位的基础上逐步构建增量式地图，实现机器人的定位和导航等功能[17-18]。该过程从数学意义上来说是非线性滤波过程，因此，求解该问题可看作是最优贝叶斯滤波。其中SLAM问题可以归纳为在给定传感器数据情况下，同时估计机器人位姿和构建环境地图的问题，可以用数学表达式表示为

p(x1:t,m|u1:t,z1:t)=

p(x1:t|u1:t,z1:t)p(m|x1:t,u1:t,z1:t)=

p(x1:t|u1:t,z1:t)p(m|x1:t,z1:t)

(1)

式中：x1:t为机器人位姿；m为地图数据；u为里程计的预测值；z1:t为激光雷达的观测值；p(x1:t|u1:t,z1:t)为估计机器人位姿；p(m|x1:t,z1:t)为给定机器人位姿和传感器观测数据。

2.1 位姿状态估计

巡检机器人的定位就是确定其在运动环境中的世界坐标系中的坐标。目前应用较多的定位方式是概率定位法，它是基于概率地图、用概率方法来表示不确定性的定位方法[19]。概率定位的原理模型如图4所示，其中ut为t时刻机器人的预测值，zt为t时刻机器人观测值，xt为t时刻机器人位姿信息，图中白色的位姿信息为未知量，灰色代表已知量。

图4 煤矿巡检机器人概率定位原理模型
Fig.4 Probabilistic localization principle model of coal mine inspection robot

由贝叶斯公式和贝叶斯估计可得t时刻机器人的位姿估计为

p(x1:t|u1:t,z1:t)=

ηp(zt|x1:t,u1:t,z1:t-1)p(x1:t|z1:t-1,u1:t)=

ηp(zt|xt)p(xt|xt-1,ut)p(x1:t-1|z1:t-1,u1:t-1)

(2)

式中η为权重。

由式(2)可知，将对t时刻估计机器人位姿转换为一个t-1时刻机器人路径增量估计问题。其中，p(x1:t-1|z1:t-1,u1:t-1)用粒子群表示，每个粒子用运动模型p(xt|xt-1,ut)进行传播，对于传播之后的粒子，用观测模型进行权重η(前一时刻的预测值与当前时刻的观测值)计算，并且根据估计的位姿构建地图。

2.2 栅格地图构建

通过对机器人位姿状态的估计，将构建地图的问题转换为在给定机器人的位姿和激光雷达的观测值时估计出最可能的地图极大似然估计的问题，此问题用数学描述可以表示为

m*=arg maxp(m|x1:t,z1:t)

(3)

栅格地图中的栅格是一个二元随机变量，只能取2个值：占用(Occupied)或者空闲(Free)。地图中的每一个栅格都是独立的，因此，估计环境的地图只需对每一个独立的栅格进行估计即可。地图估计问题的数学表达式为

p(m|x1:t,z1:t)=∏p(mi|x1:t,z1:t)

(4)

式中：p(mi)=1表示被占用；p(mi)=0表示空闲。

结合条件贝叶斯公式，单个栅格的地图估计可以表示为

p(mi|x1:t,z1:t)=

(5)

由于栅格地图描述的是当前时刻环境信息的占用或空闲状态，所以，t时刻2种状态的比值可以表示为

(6)

令对式(6)进行化简，可得

l(mi|x1:t,z1:t)=

l(mi|xt,zt)+l(mi|x1:t-1,z1:t-1)-l(mi)

(7)

式中：l(mi|xt,zt)为激光雷达的逆观测模型；l(mi|x1:t-1,z1:t-1)为栅格在t-1时刻的状态；l(mi)为栅格的先验值，此值对所有栅格相同，可忽略。

由此可见，t时刻的栅格地图估计问题可转换成t时刻激光雷达的逆观测模型和t-1时刻栅格地图的叠加问题。

3 巡检机器人重定位

为了使机器人能够利用传感器将自己实际位置信息表示在已构建的栅格地图上，需要进行重定位。本文利用AMCL算法进行重定位，该算法在机器人定位领域应用广泛。假设机器人在t时刻的位姿是xt(x,y,θ)(θ为航向角)，激光雷达传感器从开始到t时刻为止的所有观测值为z1:t={z1,z2,…,zt}，里程计从开始到t时刻所获得运动信息的预测值为u1:t={u1,u2,…,ut}，则其当前t时刻的贝叶斯后验概率为

bel(xt)=p(xt|z1:t,u1:t)

(8)

贝叶斯滤波过程分为预测和更新。在预测阶段通过机器人运动模型p(xt|xt-1,ut-1)、t-1时刻的贝叶斯后验概率bel(xt)和里程计获得的运动信息ut计算出的t时刻机器人当前位置预测概率分布为

(9)

通过传感器观测模型校正p(zt|xt)、当前t时刻预测概率分布和归一化常数ηt，计算更新后t时刻的机器人位置概率为

(10)

根据更新后t时刻的机器人位置概率可得到t时刻机器人当前位置真实概率分布为

bel(xt)=ηtp(zt|xt)·

(11)

然后通过计算得到t时刻当前位置概率，通过粒子滤波器生成N个粒子来估计位置。在AMCL中使用样品代替粒子，其工作过程称为样品权重重采样(Sampling Importance Weighting Resampling，SIR)过程。已知t时刻的机器人位置预测传播模型则有t-1时刻的新概率分布(粒子分布)为

(12)

式中为t-1时刻第i个粒子的权重。

根据里程计获得的运动信息ut和t-1时刻概率分布，预测t时刻的概率分布(粒子分布)为

(13)

对于某一粒子而言，由t时刻机器人当前位置预测概率分布和真实概率分布bel(xt)可得出t时刻第i个粒子的权重(地图的匹配度)为

(14)

根据权重进行重采样，以的概率接受如果满足则权重清零，对粒子进行重新分布。在重采样过程中，使用样本和权重来创建n个样本集合，可得到新的样本集合为

(15)

通过这种工作方式，在重复样本权重重采样(SIR)过程的同时移动样本，重采样的目的是去除权重小的粒子，提高机器人位置估计的精确度。

4 Fast-SLAM优化算法

巡检机器人在实际运动过程中，运动模型与观测模型相差较大，似然概率与先验概率之间重叠较小[20-21]。基于粒子滤波的方法对模型进行直接近似，它不要求控制向量和观测值满足高斯分布假设，将粒子滤波方法应用到高维SLAM时，则需要较多的粒子才能较好地表示机器人位姿的后验概率分布。因此，基于粒子滤波的方法存在粒子群数量过大、粒子多样性退化和重要粒子丢失等问题。为克服传统SLAM算法的缺陷，Fast-SLAM优化算法把联合SLAM状态分成运动部分和条件地图部分，以缩小采样空间，机器人的位姿用不同权值的粒子表示，地图由独立的高斯分布解析表示，机器人位姿状态的递归估计采用粒子滤波方法，地图状态的递归估计采用扩展卡尔曼滤波方法。

4.1 基础粒子群较小数量的维持

由于每个粒子都包含自己对应的栅格地图，对于复杂环境而言，每个粒子都会占用较大内存空间。如果机器人的里程计误差较大，即proposal分布与实际分布相差较大，则需要较多的粒子才能表示机器人位姿的后验概率分布，会造成内存爆炸。所以，通过引入Scan-Matching方法，求得局部最优，将基础粒子群数量维持在一个较小的数值，提升proposal分布采样的位姿质量。粒子群的极大似然估计为

(16)

式中：p(zt|xt,m)为激光雷达观测值，极大似然估计代表粒子最优；为粒子群分布概率。

4.2 粒子耗散缓解

由于Fast-SLAM是基于粒子滤波的方法，所以，每进行一次重采样都有一定的随机性。随着采样次数的增加，粒子多样性会丧失，即最终的所有粒子都来自同一个粒子或少数的几个粒子的复制。在滤波过程中引入有效粒子数Neff，用来估计在重采样过程中当前粒子群对目标函数的近似程度，可具体表示为

(17)

式中wi为计算后的粒子权重。

当Neff较大时，说明差异性很小，不进行重采样；当Neff较小时，说明差异性很大，进行重采样。

4.3 proposal分布函数改进

考虑到在最近一帧的观测，把proposal分布限制在一个狭小的有效区域，激光雷达的匹配比里程计的测量精确，即激光雷达匹配的方差要比里程计模型的方差小。因此，可以把采样范围限制在一个比较小的区域，激光雷达的数值占主导，里程计数值当做常量，从而用更少的粒子覆盖机器人的概率分布。即机器人在t时刻proposal分布位姿概率为

p(xt|xt-1,ut,zt,m)=ηp(zt|xt,m)p(xt|xt-1,ut)

(18)

式中p(xt|xt-1,ut)为里程计预测值，当做常数，即proposal分布从里程计预测模型变换为激光雷达观测模型。

假设proposal分布服从高斯分布，结合Scan-Matching方法，通过极大似然估计求出局部极值，认为离高斯分布的均值最近。因此，在附近采样得到K个位姿，有

(19)

式中:xj为第j个粒子下机器人估计的位姿；△为一个小量，无确定值，可根据现场情况设定。

对这K个位姿进行打分，概率p(zt|xj,m)最大即为打分最高，并认为这K个位姿服从高斯分布，即可解得高斯分布的表达式为

(20)

(21)

Proposal分布变为由(μ,Σ)表示的高斯分布，粒子传播由从运动模型采样变为对高斯分布进行采样。因此，对权重的计算可化简为

w=p(zt|xt-1,ut,m)=

(22)

5 实验分析

利用如图5所示的煤矿井下轮式巡检机器人在无GPS且结构相似环境下，对基于激光雷达的同步定位与地图构建方法进行试验验证，试验场地为某模拟煤矿巷道，如图6所示。该机器人上配备有Velodyne VPL-16激光雷达、ZED STEREO CAMERA双目相机、可见光云台摄像机、惯性测量单元、里程计和NVIDIA jetson TX2机载计算机。操作系统为Ubuntu 16.04，程序代码在ROS Kinetic版本上运行，通过WiFi与另一台安装相同操作系统、使用环境相同的笔记本建立通信，远程控制机器人运动，并用Rviz显示所构建的地图。

图5 煤矿巡检机器人
Fig.5 Coal mine inspection robot

图6 模拟煤矿巷道
Fig.6 Simulated coal mine roadway

为探究AMCL算法和Fast-SLAM优化算法的有效性，分别采用Fast-SLAM传统算法和Fast-SLAM优化算法进行同步定位和地图构建的对比试验。传统算法构建的栅格地图效果如图7所示，优化算法构建的栅格地图效果如图8所示。从图7、图8可知：传统算法在模拟巷道等结构相似环境构建的栅格地图出现了失真、变形，而优化算法不仅能准确估计机器人的位姿，而且能构建出完整的栅格地图，同时地图精度也明显得到提升。实验结果表明，结合AMCL算法和Fast-SLAM优化算法对结构相似环境(如走廊、巷道)构建的栅格地图具备自适应性强和定位精度高等优点。

图7 传统算法构建的栅格地图效果
Fig.7 Raster map effect constructed by traditional algorithm

图8 优化算法构建的栅格地图效果
Fig.8 Raster map effect constructed by optimal algorithm

6 结论

(1)设计了基于ROS的煤矿井下巡检机器人系统总体框架，并根据各传感器工作原理及特点，构建了系统软件模块，控制系统通过采用二级式分布结构，有效建立上下位机之间的通信，实现了实时数据交互和远程视频监控。

(2)采用Fast-SLAM算法将联合SLAM状态分成运动部分和条件地图部分，以缩小采样空间，机器人的位姿用不同权值的粒子表示，地图由独立的高斯分布解析表示，机器人位姿状态的递归估计采用粒子滤波方法，地图状态的递归估计采用扩展卡尔曼滤波方法。

(3)采用基于粒子滤波的AMCL算法建立激光雷达观测模型和里程计预测模型，通过对巡检机器人的概率定位问题用数学方式进行表达，分析得出当前时刻机器人真实概率分布模型，去除权重(地图的匹配度)小的粒子，提高了机器人位置估计的精确度。

(4)对Fast-SLAM算法进行优化，提出维持基础粒子群较小数量的方法，提升proposal分布采样的位姿质量；引入了自适应重采样的方法，缓解了粒子耗散的问题；改进proposal分布函数的方法，将粒子传播由里程计预测的运动学模型转换为精确度更高的激光雷达观测高斯分布模型进行采样，极大程度上减少了重采样粒子的数量。

(5)研究了激光SLAM的同步定位与地图构建方法，实现了巡检机器人定位和地图构建。实验结果表明，在结构相似环境下，采用自适应蒙特卡罗定位算法提高了位姿估计的自适应性，改进Fast-SLAM算法减少了粒子数量和缓解了粒子耗散，提高了地图构建的精确性和控制系统的鲁棒性。

参考文献：(References)

[1] 姜俊英,周展,曹现刚,等.煤矿巷道悬线巡检机器人结构设计及仿真[J].工矿自动化,2018,44(5):76-81.

JIANG Junying，ZHOU Zhan，CAO Xiangang，et al.Structure design of suspension line inspection robot in coal mine roadway and its simulation[J].Industry and Mine Automation,2018,44(5):76-81.

[2] 汤怀量,雷泽勇,雷林,等.巡检与核应急机器人研究现状分析[J].机械工程师,2018(2)：20-23.

TANG Huailiang,LEI Zeyong,LEI Lin,et al.Research status of inspection and nuclear emergency response robot[J].Mechanical Engineer,2018(2)：20-23.

[3] 杨林,马宏伟,王川伟,等.校园巡检机器人智能导航与控制[J].西安科技大学学报,2018,38(6):1013-1020.

YANG Lin,MA Hongwei,WANG Chuanwei,et al.Intelligent navigation and control of campus inspection robot[J].Journal of Xi'an University of Science and Technology,2018,38(6):1013-1020.

[4] LEONARD J J,DURRANT-WHYTE H F.Mobile robot localization by tracking geometric beacons[J].IEEE Transactions on Robotics and Automation,1991,7(3):376-382.

[5] DURRANTWHYTE.Where am I? a tutorial on mobile vehicle localization[J].Industrial Robot,1994,21(2):11-16.

[6] CADENA C,CARLONE L,CARRILLO H,et al.Past,present,and future of simultaneous localization and mapping:toward the robust-perception age[J].IEEE Transactions on Robotics,2016,32(6):1309-1332.

[7] SMITH R,SELF M,CHEESEMAN P.Estimating uncertain spatial relationships in robotics[J].Machine Intelligence &Pattern Recognition,2013,5(5):435-461.

[8] GUIVANT J E,NEBOT E M.Optimization of the simultaneous localization and map-building algorithm for real-time implementation[J].IEEE Transactions on Robotics &Automation,2002,17(3):242-257.

[9] GUIVANT J E,NEBOT E M.Solving computational and memory requirements of feature-based simultaneous localization and mapping algorithms[J].IEEE Transactions on Robotics &Automation,2003,19(4):749-755.

[10] MONTEMARLO M.FastSLAM:a factored solution to the simultaneous localization and mapping problem[C]//Proceedings of the AAAI National Conference on Artificial Intelligence,Edmonton,2002:593-598.

[11] NIETO J,GUIVANT J,NEBOT E,et al.Real time data association for FastSLAM[C]//IEEE International Conference on Robotics and Automation,2003:412-418.

[12] 徐曙.基于SLAM的移动机器人导航系统研究[D].武汉：华中科技大学，2014.

[13] 厉茂海,洪炳熔,罗荣华.用改进的Rao Blackwellized粒子滤波器实现移动机器人同时定位和地图创建[J].吉林大学学报,2007,37(2):401-406.

LI Maohai,HONG Bingrong,LUO Ronghua.Improved Rao Blackwellized particle filters for mobile robot simultaneous localization and mapping[J].Journal of Jilin University,2007,37(2):401-406.

[14] 张文玲.移动机器人同时定位与地图创建自适应算法研究[D].合肥：中国科学技术大学,2009.

[15] 朱磊,樊继壮,赵杰,等.改进粒子滤波器的移动机器人同步定位与地图构建方法[J].重庆大学学报,2014,37(4):39-45.

ZHU Lei,FAN Jizhuang,ZHAO Jie,et al.A method for mobile robot SLAM based on modified particle filter[J].Journal of Chongqing University,2014,37(4):39-45.

[16] 罗元,苏琴,张毅,等.基于优化RBPF的同时定位与地图构建[J].华中科技大学学报(自然科学版),2016,44(5):30-34.

LUO Yuan,SU Qin,ZHANG Yi,et al.Simultaneous localization and mapping implementation based on optimized RBPF[J].Journal of Huazhong University of Science and Technology (Natural Science Edition),2016,44(5):30-34.

[17] 任祥华.激光雷达室内SLAM方法[D].哈尔滨：哈尔滨工程大学,2018.

[18] 武二永,项志宇,沈敏一,等.大规模环境下基于激光雷达的机器人SLAM算法[J].浙江大学学报(工学版),2007,41(12):1982-1986.

WU Eryong,XIANG Zhiyu,SHEN Minyi,et al.Robot SLAM algorithm based on laser range finder for large scale environment[J].Journal of Zhejiang University (Engineering Edition),2007,41(12):1982-1986.

[19] 赵希宇.救援机器人的同时定位建图与导航研究[D].沈阳：沈阳工业大学,2018.

[20] 王依人,邓国庆,刘勇,等.基于激光雷达传感器的RBPF-SLAM系统优化设计[J].传感器与微系统,2017,36(9):77-80.

WANG Yiren,DENG Guoqing,LIU Yong,et al.Optimal design of RBPF-SLAM system based on LIDAR sensor[J].Transducer and Microsystem Technologies,2017,36(9):77-80.

[21] 张亚楠,孙丰财,史旭华.一种改进的RBPF激光SLAM算法[J].无线通信技术,2017(4):16-20.

ZHANG Yanan,SUN Fengcai,SHI Xuhua.An improved RBPF laser SLAM algorithm[J].Wireless Communication Technology,2017(4):16-20.