基于优化Fast-SLAM的煤矿水泵房巡检机器人研究

邱建都，吴 娟，金书奎 ，金 言，石军杰

(1.太原理工大学 机械工程学院，山西 太原 030024；2.矿山流体控制国家地方联合工程实验室，山西 太原 030024；3.上海交通大学 机械系统与振动国家重点实验室，上海 200240)

伴随着我国矿山工业智能化的不断推进以及机器人技术的进步与发展，煤矿水泵房巡检机器人已经逐渐成为我国在实现智慧矿山建设进程中一个迫切而必须探索和开拓的技术领域。

最初，人们通过将已知地图存储在巡检机器人中以实现自主定位和导航，近几年则主要利用在巡检环境上方架设轨道来实现机器人的定位导航[1]。然而这种方式限制了机器人只能对已知的地图进行导航或者只能按照预先铺设的轨道进行巡检，不能随着巡检环境的变化做出相应的变化。为了解决这种工作方式的局限性，同步定位与地图构建技术由此应运而生。SLAM技术最早于20世纪80年代被研究者提出，之后人们逐渐提出了卡尔曼滤波、扩展卡尔曼滤波、无迹卡尔曼滤波、粒子滤波、Fast-Slam、Gmapping等算法[2]，以上研究成果为我国煤矿水泵房巡检机器人激光SLAM算法的深入研究奠定了坚实的基础。

目前如何在煤矿水泵房内进行精确定位和建图是实现水泵房巡检机器人自主巡检所面临的重要问题之一。因此对激光SLAM算法进行了研究，通过利用激光雷达快速获取周围障碍物的信息，并对里程计进行标定、多传感器融合来减少定位误差，然后通过对Gmapping算法进行优化，将采样的区域限制在一个比较小的地方，可以用更少的粒子来覆盖机器人的概率分布，减少了重采样的次数，在满足机器人实时定位需求的同时也提高了建图精度。

1 SLAM的概率模型

基于概率模型的SLAM问题原理如图1所示。在该原理图中，mk代表外界环境地图；Uk代表k时刻里程计数据。Xk=(xk，yk，φk)代表移动机器人在k时刻下的位姿，其中(xk，yk)为移动机器人在环境地图上的二维坐标，φk为移动机器人自身相对于环境地图的转角坐标；Zk提供k时刻激光雷达观测到的数据。

图1 SLAM问题原理图

SLAM问题可以描述为：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为环境地图；u1：t为里程计数据；z1：t为激光雷达的观测数据；p(x1：t|u1：t，z1：t)为估计的机器人位姿；p(m|x1：t，z1：t)为已知机器人位姿和激光雷达观测数据情况下估计环境地图。

2 传感器数据预处理

2.1 惯性测量单元数据和编码器数据融合

在机器人移动时，车轮打滑或者颠簸等会造成编码器数据失真，从而影响到轮式里程计数据，并最终导致机器人建图效果不理想。而为了克服这些状况，采用EKF算法将轮式里程计数据与IMU进行了融合。

图2 EKF信息融合流程图

2.2 里程计标定

对于每组数据，可得：

令

则矫正矩阵：

X=(ATA)-1ATb

(4)

将矫正矩阵乘于里程计数据，使得里程计测得的机器人运动信息和激光雷达测得的机器人运动信息相对应。

3 机器人同步定位与地图构建

3.1 基于粒子滤波算法的自身定位

在将外部传感器获得的环境信息和机器人的运动信息转换为世界坐标系下后，即可获得当前状态的里程计数据和激光雷达数据，之后就可确定机器人处于世界坐标系中的位置即机器人的自身定位。采用粒子滤波算法去估计巡检机器人的位姿。其算法流程如图3所示。

图3 基于粒子滤波的定位流程图

粒子滤波算法的proposal分布即在机器人里程计运动学模型中进行采样，设t-1时刻第i个粒子的位姿xit-1=(xt-1，yt-1，φt-1)，t-1时刻的里程计数据ut=(Δx，Δy，Δφ)，t时刻第i个粒子的位姿xit=(xt，yt，φt)，噪声均值为0的高斯分布(白噪声)分别为(Nx，Ny，Nφ)，因此：

如果没有观测，单纯通过里程计去预测每个粒子的位姿，由于里程计中带有噪声，因此越传播，粒子扩散越大，最终所有粒子的分布呈现均匀分布。而权重即为机器人传感器的观测模型，权重表示机器人的proposal分布与实际分布的差，计算公式为：

bel(xt)=ηp(zt|xt)p(x|xt-1，ut)bel(xt-1)

(7)

因此权重为：

=ηp(zt|xt)

(9)

根据权重对粒子群进行重采样，去除低权值的粒子，复制高权值的粒子，得到需要的真实状态xt。而这些重采样之后的粒子就代表了真实状态的概率分布，即可获得机器人当前位置与姿态的准确估计。

3.2 覆盖栅格建图算法

当通过粒子滤波算法来估计得出机器人从运动到某一时刻t的位姿估计X1：t之后，便能够建立全局环境地图mt。其数学描述为以已知机器人位姿和激光雷达观测到的数据为基础估计环境地图：

地图估计问题表示为：

p(m|x1：t，z1：t)=∏p(mi|x1：t，z1：t)

(11)

mi是一个二元随机变量，结合条件贝叶斯公式：

p(mi|x1：t，z1：t)

同理对于mi，两者之比为：

对于P(x)，定义对应的log-Odd项：

等式两边取log得：

l(mi|x1：t，z1：t)

=l(mi|xt，zt)+l(mi|x1：t-1，z1：t-1)-l(mi)

(16)

式中，l(mi|xt，zt)表示激光雷达得逆观测模型；l(mi|x1：t-1，z1：t-1)表示栅格mi在t-1时刻得状态，l(mi)表示栅格mi的先验值，该值对所有的栅格相同。

4 Fast-SLAM算法现有问题及其优化

4.1 Fast-SLAM现有问题

Fast-SLAM算法以粒子滤波算法为基础，在给定传感器数据的情况下用粒子去估计机器人的位姿和地图，其将对机器人的轨迹x1：t的估计转换为一个增量估计的问题，并根据估计的位姿构建地图。Fast-SLAM算法存在以下几个问题：

1)粒子耗散问题。为了使粒子群分布近似后验概率分布，需要对粒子群进行多次重采样，这就导致粒子的多样性减少，最终所有粒子都由一个粒子或者几个复制得到，这对于建图是致命的。

2)维数灾难。当空间的维数增大时，所需要的粒子数也随之增大，由于每一个粒子都包含自己的栅格地图，当环境大时、里程计精度不高时、预测与实际分布相差很大时都会造成内存保证。

3)proposal分布。当里程计精度较差时，即proposal分布很广，需要很多的粒子才能将机器人的后验概率表达清楚。

4.2 Fast-SLAM算法优化

Gmapping算法是目前应用最为广泛的2D激光SLAM算法，它在较小的环境中能实现较好的建图效果，即能较好的应用与煤矿水泵房环境。但Gmapping算法在Fast-SLAM算法的基础上只对粒子滤波的前两个问题进行了优化。因此在Gmapping算法的基础上，对第三个问题进行研究和优化，提高机器人的定位和建图精度。

为了减少粒子数，采用激光雷达匹配的方法对proposal分布进行优化，将其限制在一个比较小的范围，可以用更少的粒子覆盖机器人的概率分布，如图4所示，从而达到减少采样次数来保持粒子多样性。

图4 proposal分布

机器人在t时刻的proposal分布位姿概率为：

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

(17)

式中，p(zt|xt，m)在自己的区域(L(i))占主导地位，此时p(xt|xt-1，ut)的值不再重要，令其为常数，因此：

p(xt|xt-1，ut，zt，m)=ηp(zt|xt，m)xt∈L(i)

(18)

proposal分布从里程计观测模型变成了激光雷达观测模型，激光雷达观测模型的方差较小，假设其服从高斯分布：

式中，xj为第j个粒子下机器人估计的位姿，Δ为一个微小量，对这k个位姿进行打分p(zt|xj，m)，并认为这k个位姿服从高斯分布，即可求得高斯分布表达式为：

Proposal分布变为高斯分布N(μ，Σ)，分差较小，即可以用更少的粒子覆盖机器人的概率分布。

因此对权重的计算可以化简为：

5 水泵房巡检机器人平台搭建和实验

5.1 平台搭建和系统总体架构

搭建以轮式底盘为车体结构的水泵房巡检机器人，车体内部的单片机与工控机通过RS232串口通信，小车选用的工控机为凌华无风扇工控机，它采用英特尔酷睿i5系列处理器，8G内存，32G硬盘，装有ROS机器人操作系统。机器人由轮式机器人本体、工控机、激光雷达、惯导元件等硬件构成，如图5所示。

图5 水泵房巡检机器人

其中激光雷达无接触远距离测量周围环境的障碍物信息，为了保证地图构建的质量，小车使用深圳玩智商科技有限公司研发的YDLIDAE TG50激光雷达，雷达参数见表1。

表1 YDLIDAE TG50技术参数

轮式机器人通过两轮差速驱动，为了对里程计的误差进行补偿，巡检机器人的角速度由IMU测量获得，采用GY85惯性测量模组来测量机器人运动的实时角速度，惯性测量模组参数见表2。水泵房巡检机器人的平台框架如图6所示。

表2 GY85惯性测量模组参数

图6 水泵房巡检机器人平台框架

5.2 里程计标定实验

为了验证里程计标定后的数据和原始数据之间的效果，使用移动机器人试验平台沿着室内试验场地进行了试验。里程计标定的程序代码在ROS Kinetic版本上运行，订阅/odom、/scan和TF三个话题，其中/odom话题为里程计位姿，/scan话题为激光雷达的数据，TF为各个关节之间的坐标转换关系，通过PI-ICP算法计算激光雷达预测的位姿。在输入两组数据后构建最小二乘需要的超定方程组Ax=b，求解并返回得到的矫正矩阵：

在获得矫正矩阵x后，乘于里程计的位姿，便可以获得较里程计更为准确的矫正位姿。在ROS中使用rviz可视化插件实时显示原始里程计位姿和激光雷达估计位姿，在控制巡检机器人小车在室内试验场地走一段距离后获得修正矩阵：

将小车行走获得的矫正矩阵乘里程计数据即为矫正后的里程计标定位姿，其效果图在matlab中显示，如图7所示。

图7 里程计标定效果图

由图7可知，随着机器人行驶距离的增大，里程计中的位移分量和激光雷达估计位姿中的位移分量几乎相等，但姿态相差越来越大。由于激光雷达有环境的观测信息作为后验分布，所以激光里程计较轮式里程计更为精确。图中经矫正后的里程计标定位姿更靠近激光里程计位姿，所以经过里程计标定后能保证巡检机器人具有更高的定位精度。

5.3 惯性测量单元数据和编码器数据融合

为了分析惯性测量单元数据和轮式里程计数据融合后的实际效果，利用图5的机器人平台进行实验，记录实际位姿和里程计位姿并进行对比。

由于机器人的零部件在加工、安装、尺寸测量等过程中使机器人存在系统误差，所以测试机器人在EKF算法融合里程计和IMU数据后行驶1m、3m和5m距离中偏离直线的大小和角度，测试试验结果见表3。

机器人的定位误差计算公式如下：

由表3数据分析可知，机器人向前行驶的距离越长，偏离直线的大小和角度也会随之增大越大。但定位误差Er< 0.1，满足水泵房定位和建图的要求。

表3 直线行驶测测试结果

转弯试验共分为转过π/4、π/2、3π/4、π共4种情况进行。记录实际转角和里程计转角的误差，测试试验结果见表4。

表4 转弯试验测试结果

机器人的控制精度计算公式如下：

在机器人建图过程中，轮胎会与地面发生摩擦，并且摩擦力会随着机器人速度的增加而增加，所以机器人实际转动的角度会比指令发送的角度小，但融合里程计和IMU数据之后，机器人的控制精度ω<2°/rad，保证其定位轨迹更加贴近理想轨迹。

5.4 Fast-SLAM的优化试验

利用如图5所示的机器人巡检小车在经过上述里程计标定和融合里程计和IMU数据之后，模拟井下水泵房内无GPS的情况，分别采用传统的Gmapping建图算法和Fast-SLAM优化后的Gmapping建图算法进行同步定位与地图构建的对比试验。建图程序代码在工控机上运行，操作系统为Ubuntu 16.04，使用VNC进行远程控制机器人运动，并用Rviz可视化插件显示建图效果。

使用传统的Gmapping算法在室内环境构建的栅格地图能基本描绘出试验场地的地图，障碍物探测准确，但出现了失真、变形，而经过里程计标定、融合惯性元件、优化后的给Gmapping算法较之前建图边界各位平滑，异常点减少，且直线度更好。同时，激光雷达建图仍存在一些不足，受雷达高度的影响只能检测同一高度的障碍物，速度过快会引起地图失真、变形、断裂等问题。

6 结 语

通过对激光雷达的同步定位与建图进行研究，分析了SLAM概率模型，对里程计进行标定并融合惯性元件IMU，实现巡检机器人更为精确的定位。然后对Gmapping算法进行优化以缩小采样空间，用更少的粒子表示机器人的后验概率，通过减少重采样粒子的数量来提高建图精度。仿真和实验结果表明：经过里程计标定、融合惯性元件、优化算法后的巡检机器人能够在水泵房中进行同步定位和建图且满足实际工况需求。下一步研究工作的一个重点任务是加入视觉闭环检测，提取关键帧并判断闭环检测数据来帮助系统有效消除累计误差，使该算法构建的二维栅格地图能够拥有更高的精度和鲁棒性。