煤矿水泵房巡检机器人路径规划与跟踪算法的研究

2022-07-20金书奎寇子明

煤炭科学技术 2022年5期

金书奎，寇子明，吴娟

(1.太原理工大学机械工程学院,山西太原 030024；2.山西省矿山流体控制工程实验室，山西太原 030024；3.矿山流体控制国家地方联合工程实验室,山西太原 030024)

0 引言

煤矿井下主排水系统是煤炭生产中不可缺少的重要设备，其运行状况的好坏直接影响着矿山的正常生产和工人的生命安全[1]。为保证排水设备的正常运行，巡检人员要运用感官以及一些配套的检测仪器定期对其进行检查，该方式劳动强度大，主观因素多，巡检不到位，巡检结果数字化不便[2-3]，不符合数字矿山的发展方向。随着机器人技术的发展，采用机器人完成水泵房巡检工作成为了可能，水泵房巡检机器人，可以把水泵房巡检人员从日常简单、机械、重复性的工作中解放出来，便于人们集中精力处理更加重要和复杂的工作，对强化煤矿生产安全管理，真正实现减员增效、节能降耗具有重要意义[3]。巡检机器人导航包括定位、路径规划和路径跟踪三部分，是巡检机器人在水泵房内稳定安全行进的必要条件，没有可靠的导航保证，机器人将无法完成巡检任务。目前常用的导航方式有：电磁导航、视觉导航、GPS导航等[4]。电磁导航虽然能做到车体较精确定位，但种导航方式需要在机器人运行路线上铺设磁条和设定RFID标签，施工的工作量较大，不能预测前方道路情况，严重制约了移动机器人的行走速度；视觉导航在黑夜或光线不足的情况下效果较差，并且地面导航标志一旦破损或丢失会极大地影响机器人的导航定位性能；GPS导航有很好的灵活性，但是在煤矿井下水泵房内很难接收到GPS信号，即使接收到信号，也无法解决信号失真的问题[4]。

目前如何在煤矿井下水泵房内进行精确导航是实现水泵房巡检机器人自主巡检所面临的重要问题之一。对此，研究了基于检测点、行进点的最优路径规划算法和点对点的路径追踪算法。首先通过路径规划算法在建好的二维栅格地图中规划出一条经过所有检测点和行进点的无障碍路径，然后通过路径跟踪算法计算出巡检机器人沿该路径行走需要的实时线速度和角速度，最后将线速度和角速度发送给巡检机器人运动控制器，巡检机器人便可沿着规划好的路径行走，到达检测点后便可采集排水设备的运行数据。经仿真和实验结果表明使用该算法控制机器人巡检在导航精度上满足机器人巡检工作的要求。

1 二维栅格地图的构建

栅格地图是巡检机器人自主导航中常用的一种环境地图，目前主要通过激光雷达传感器与SLAM算法结合的方法建立二维栅格地图，该方法多在室内且环境状况未知的情况下使用，在实际建图过程中激光雷达传感器存在噪音，激光雷达数据存在运动畸变，使得建立的栅格地图与真实环境有较大的误差[5]，不利于巡检机器人的路径规划，因此采用人工测量与MATLAB中栅格地图生成算法结合建立二维栅格地图，具体步骤如下：

1)人工测量可得到水泵房的整体尺寸、排水设备的具体尺寸和排水设备在水泵房中的具体位置，根据这些信息绘制出水泵房二维平面图,如图1所示。

图1 水泵房二维平面图Fig.1 Two-dimensional plan pump house

2)根据水泵房二维平面图将水泵房内的障碍物和可通行区域以数字化的方式表示成一个44行34列的矩阵如图2所示。在矩阵元素中0代表可通行区域，1代表障碍物区域。

图2 水泵房二维平面图数字化表示Fig.2 Digital representation of two-dimensional plan of water pump house

3)通过调用MATLAB 机器人工具箱下“robotics.Binary Occupancy Grid”函数将二维平面图数字化矩阵生成二维栅格地图如图3所示，该地图横向有34个网格，纵向有44个网格，地图的分辨率为每米10个网格。

图3 二维占据栅格地图Fig.3 2D grid map

由于水泵房内各个设备的位置已知，且设备的位置是不经常发生改变的，因此人工测量与MATLAB中栅格地图生成算法结合建立水泵房的二维栅格地图的方法是可行的。

2 两轮差速轮式移动机器人里程计模型

在轮式自主导航系统中，里程计是重要组成部分，从里程计的计算数据中可以获取机器人运动的位姿和速度等信息，这些信息对于实现机器人的定位和路径跟踪有重要作用[6]。

2.1 机器人坐标系和全局坐标系关系

把机器人建模成轮子上的一个刚体，运动在水平面上，在平面上机器人底盘总的自由度是3个。由于存在轮轴和小脚轮关节，还会有附加的自由度和灵活性。就机器人的底盘而言，把它看做刚体，忽略机器人和它的轮子间内在的关联和自由度。为了确定机器人在平面中的位置，建立全局坐标系和机器人坐标系之间的关系。

如图4所示，在平面上选择一点作为原点O，创建相互正交的X轴和Y轴，建立全局坐标系。为确定机器人的位置，在机器人底盘上选择一点C作为位置参考点。以点C为原点，创建相互正交的XR轴和YR轴，建立机器人坐标系。在全局坐标系上，机器人的位置由坐标x和y确定，机器人的姿态由机器人坐标系XR轴和全局坐标系X轴间夹角θ确定。机器人在全局坐标系中的位姿表示ξI=(x,y,θ)，通过正交旋转矩阵R(θ)，可把机器人坐标系的运动映射到全局坐标系下。

图4 全局坐标系和机器人坐标系Fig.4 World coordinate system and robot coordinate system

2.2 两轮差速轮式移动机器人里程计模型

巡检过程中地面可能存在渣石、沟坎和凸台，造成机器人的车轮与地面间出现相对滑动[7]，这使得里程计计算得到的位姿和机器人的真实位姿之间存在误差，由于煤矿井下水泵房的地面是平整和清洁的，车轮与地面间相对滑动可忽略，因此，假设机器人车轮在没有打滑的前提下，对其进行运动学分析。

机器人有2个主动轮，轮子直径为d，两轮轮间距为l，机器人左右两轮的运动速度分别为VL和VR，其中VL和VR可通过安装在电机上的光电编码器测量获得。巡检机器人在机器人坐标系下位置参考点C运动的线速度为v(t)，角速度为ω(t)为：

(1)

(2)

式中：VL、VR分别为左右车轮的线速度;v(t)、ω(t)分别为机器人位置参考点C的线速度和角速度。

则在全局坐标系下机器人在X轴和Y轴方向的线速度和绕Z轴旋转的角速度为：

(3)

在全局坐标系下机器人的位姿为：

(4)

式中：ξI为机器人在全局坐标系下的位姿矩阵。

在给定移动机器人线速度v(t)、角速度ω(t)和机器人坐标系相对于全局坐标系的变换矩阵的情况下，由式(4)可得到任意时刻机器人在全局坐标系中的位姿。

3 移动机器人路径规划

水泵房巡检机器人路径规划就是寻找一条机器人从起始点，中途经过所有选定的行进点和检测点，然后到达最终点的路径[8]。其中行进点是辅助机器人进行路径规划的坐标点，检测点是机器人停靠并对水泵房设备进行信息采集的坐标点。

3.1 水泵房巡检任务中各点坐标位置确定

如图5所示，点1和点6分别为水泵房巡检任务中的起始点和最终点，他们处于水泵房入口和出口的中间位置，此处障碍物较少，便于机器人通行；点2和点4是水泵房巡检任务中的检测点，检测点设置在排水设备的中间位置，方便机器人对排水设备中水泵、电机、联轴器、转轴进行拍照、红外测温和噪音检测；点3和点5是水泵房巡检任务中的行进点，行进点设置在水泵房走廊的中间位置，其作用是给路径规划算法增加约束，让该算法规划的全局路径位于障碍物较少的位置。由于水泵房内各排水设备的位置相对固定，所以点1～点6坐标位置可通过实地测量确定下来，机器人路径规划算法根据这些坐标点便可规划出一条合理的全局路径。

图5 带有行进点和巡检点的二维栅格地图Fig.5 2D grid maps with travel and patrol points

3.2 PRM路径规划算法

PRM是一种基于图搜索的算法，它将连续空间转换成离散空间，再利用A*等搜索算法在路线图上寻找路径[9]，在图6中PRM算法运行过程分为6个步骤，前5个步骤为离线学习阶段，第6个步骤为查询阶段。离线学习阶段主要内容是在给定地图的自由空间内生成N个随机点，在每个随机点的周围选取邻域点，把随机点和它的所有邻域点相连，并去掉穿过障碍物区域的连线，由此得到一个无向路径网络图G=(V,E)，其中其中V为节点集，E为所有可能的两点之间的路径集。查询阶段主要内容是根据学习阶段构造的无向网络路径图，采用某种搜索算法，如A*算法，广度优先搜索算法等，在起点和终点之间搜索出无碰撞的路径。

图6 PRM算法具体流程示意Fig.6 PRM algorithm specific flow

4 移动机器人路径跟踪

机器人路径跟踪是在平面坐标系下要求机器人从某一点出发，按照某种控制规律到达路径规划算法生成的最优的路径上，并沿着该路径运动到目标点[10]。水泵房一定程度上属于时变环境，在已规划的路径上，某段时间内可能存在障碍物，即便是最优的路径，也很难完全考虑到机器人规划路径中的各种障碍物情况[11]，因此机器人在进行路径跟踪时还需通过传感器探测前方障碍物，并及时避障。

4.1 Pure Pursuit路径跟踪算法

图7中G(gx,gy)是下一个要跟踪的目标点，它位于已经规划好的全局路径上，现在需要控制巡检机器人的位置参考点C经过该目标点，参考点C的当前位置为(cx,cy)，ld表示巡检机器人当前位置到目标点的距离，α表示目前车身和目标路点的夹角。根据两点之间的距离公式可得到：

图7 Pure Pursuit路径跟踪算法简化模型Fig.7 Simplified model of Pure Pursuit path tracking algorithm

(5)

式中：gx和gy为目标点的横坐标和纵坐标；cx和cy为机器人当前位置的横坐标和纵坐标；ld为巡检机器人当前位置到目标点的距离。

移动过程中控制机器人的线速度大小与机器人距离目标的远近ld成正比得到：

(6)

式中：VL、VR分别为左右车轮的线速度;v(t)为机器人质心移动的线速度;ld为机器人位置参考点C距目标点的距离;kv为ld与v(t)的比值。

根据正弦定理可以推导出如下转换式：

(7)

(8)

(9)

式中：ld为机器人位置参考点C距目标点的距离；α为机器人从当前位置运动到目标点需要转过的角度；R为机器人运动轨迹的圆弧半径。

通过上式可得到机器人需要转动的角度为：

(10)

式中：α为机器人从当前位置运动到目标点需要转过的角度；ld为机器人位置参考点C距目标点的距离；R为机器人运动轨迹的圆弧半径。

移动过程中控制机器人的角速度大小与机器人需要转动的角度α成正比得到：

ω(t)=khα

(11)

式中：ω(t)为机器人旋转角速度；kh为角速度ω(t)与角度α的比值。

4.2 机器人避障算法

通过激光雷达传感器实现机器人的避障，由激光测距的原理可知，在激光雷达旋转一周的过程中，激光发射头会等时间间隔发射激光(每束激光的间隔角度相等)，假定激光雷达旋转一周后，激光发射头总共发射了360束激光，则每束激光的间隔角度为1°，这样就可以划分所有的激光束，使之对应机器人前后左右的方位，如图8所示，定义1～3激光束扫描的位置为机器人右方，4～18激光束扫描的位置为机器人前方，19～21激光束扫描的位置为机器人的左方。

图8 激光束对应的机器人的方位Fig.8 Orientation of robot corresponding to laser beam

在机器人右方、前方和左方的每束激光测得的距离d中选取最小值，分别为y、q和z，避障算法流程如图9所示。

图9 避障算法流程Fig.9 Flow of avoidance algorithm

5 仿真试验

5.1 PRM路径规划算法仿真

在3.1节中已经通过人工测量的方式得到水泵房巡检任务中起始点、行进点、检测点和最终点总共6个点的坐标位置，此处主要在MATLAB中根据点1～点6的坐标位置结合PRM路径规划算法进行路径规划仿真，由于PRM路径规划算法执行一次只能规划出两个坐标点间的路径，要规划出一条经过6个坐标点的路径，至少需要执行5次PRM路径规划算法，执行完5次PRM路径规划算法后生成的路径如图10所示。

图10 PRM路径规划算法生成的5段路径Fig.10 5 paths generated by path planning algorithm

由图10中可看出，PRM路径规划算法总共生成了5段路径，这5段路径连接在一起组成了由起始点(点1)到最终点(点6)的全局路径，在这条全局路径上点2和点4是巡检机器人对排水设备进行信息采集的坐标点，点3和点5是辅助PRM算法进行路径规划的坐标点，他们能够把PRM算法规划的路径约束在道路中间的位置，这样在满足路径较短的前提下，又可让规划的路径尽量避开道路边缘的障碍物。选取坐标点4到坐标点5之间的路径规划(路径4)进行重点介绍，在进行路径规划前，首先根据机器人的尺寸对水泵房二维栅格地图进行膨胀，以防止机器人在进行路径跟踪时与转角处的障碍物发生碰撞，然后将膨胀后的地图给PRM路径规划算法，并设置规划路径的起点(1.2,2.4)，终点(2.7,3.3)，PRM路径规划算法生成的随机点数目为800个，PRM路径规划算法的仿真结果如图11所示。在图11中蓝色的点是PRM路径规划算法生成的随机点，灰色的区域为随机点之间的连线，绿色的线为PRM算法生成的点4～点5的路径，在图12中可看到该路径是由一系列坐标点连线组成，这些坐标点在接下来的路径跟踪中有重要作用。

图11 PRM路径规划算法仿真结果Fig.11 PRM path planning algorithm simulation results

图12 PRM路径规划算法生成的路径Fig.12 Path generated by path planning algorithm

5.2 PurePursuit路径跟踪算法仿真

在上一节中，已经得到了一条由点1～点6的全局路径，这条全局路径上点2和点4是巡检机器人对排水设备进行信息采集的坐标点，在这两点处机器人要调整好位姿停止一段时间对排水设备进行信息采集，若直接通过Pure Pursuit算法控制机器人跟踪点1～点6这条全局路径，则在经过点2和点4时机器人无法停留，因此需要先将这条全局路径分割为3段，分割点即为点2和点4，然后运行3次Pure Pursuit算法分别跟踪路径1(点1～点2)，路径2和路径3(点2～点4)，路径4和路径5(点4～点6)，运行3次Pure Pursuit算法后机器人运动轨迹如图13所示。

图13 机器人的运动轨迹Fig.13 Trajectory of robot

由图13看出轨迹1、轨迹2和轨迹3与PRM路径规划算法生成的路径基本重合。机器人从起始点(点1)跟踪路径1到达排水设备1的检测点(点2)，机器人在此处停留一段时间采集排水设备1的数据，然后跟踪路径2和路径3到达排水设备2的检测点(点4)，机器人在此处停留一段时间采集排水设备2的数据，最后跟踪路径4和路径5到达最终点(点6)。重点介绍Pure Pursuit算法控制移动机器人跟踪路径4和路径5(点4～点6)。在进行路径跟踪前要在MATLAB中生成一个两轮差速机器人模型，设置机器人的直径为0.2 m，该机器人可通过里程计得到任意时刻全局坐标系下的位姿，该位姿可以判断机器人是否到达了目标点；再把点4～点6的规划路径存储到Pure Pursuit路径跟踪算法中，点4到点6的规划路径由一些坐标点连线组成，这些坐标点是由PRM算法生成的，他们是Pure Pursuit算法需要跟踪的目标点；设置机器人进行路径跟踪时的线速度0.1 m/s，最大角速度1 rad/s，距离阈值为0.01 m，执行Pure Pursuit算法得到的仿真结果如图14所示。

图14 Pure Pursuit路径跟踪算法仿真结果Fig.14 Simulation results of Pure Pursuit path tracking algorithm

图15为Pure Pursuit路径跟踪算法控制机器人跟踪路径4和路径5时各个时刻的姿态角、角速度和线速度的变化，在区间1内机器人处于起始点，位姿(x,y,θ)=(1.2,2.4,0)，规划路径姿态角(规划路径与全局坐标系X轴间夹角)大于机器人的姿态角(机器人坐标系的X轴与全局坐标系的X轴之间的夹角)，因此Pure Pursuit路径跟踪算法给出的机器人旋转角速度大于0，机器人逆时针方向旋转，机器人的姿态角增加，在区间2内机器人的姿态角与规划路径姿态角相等，Pure Pursuit路径跟踪算法给出的机器人旋转角速度等于0，线速度为0.1 m/s保持不变，机器人将沿着规划路径移动。在区间3内由于规划路径的姿态角进一步增大，因此Pure Pursuit路径跟踪算法给出的机器人旋转角速度大于0，机器人逆时针方向旋转，机器人的姿态角也增大。在区间4内机器人的姿态角与规划路径姿态角相等，Pure Pursuit路径跟踪算法给出的机器人旋转角速度等于0，线速度仍保持不变，机器人沿着规划路径移动。同理在区间5内也发生了规划路径姿态角的改变，同时Pure Pursuit路径跟踪算法也给出了机器人不同的旋转角速度，使机器人的姿态角最终与规划路径的姿态角相等。在区间6内机器人到达规划路径的最终点，此时线速度和角速度都为0，机器人的最终的姿态角仍和规划路径姿态角相等。

图15 Pure Pursuit算法跟踪路径过程分析Fig.15 Pure Pursuit Algorithm tracking path process analysis

5.3 避障算法仿真

机器人避障是指机器人在进行路径跟踪时，如果激光雷达检测到机器人前方存在障碍物，就运行避障算法控制机器人绕开该障碍物，然后继续跟踪原始路径。本节通过Pure Pursuit路径跟踪算法跟踪路径2和路径3，并在路径2上设置一个障碍物用来进行避障算法的仿真，仿真结果如图16所示。

图16 避障算法仿真结果Fig.16 Simulation results of obstacle avoidance algorithm

图17为机器人在避障过程中各个时刻的机器人的姿态角、机器人前方、右方和左方距障碍物距离的变化，设置距离阈值为0.1 m，打开激光雷达传感器，通过分析激光雷达传感器的数据可得到机器各个方位距障碍物的距离，当该距离大于距离阈值时，机器人处于路径跟踪阶段，此时路径跟踪算法控制机器人沿原始路径移动，当机器人前方或左方距障碍物的距离小于距离阈值时，机器人处于避障阶段，开始执行避障算法并停止执行路径跟踪算法，由于机器人右方距障碍物的距离明显大于前方和左方，机器人右转(顺时针旋转)，机器人的姿态角减小了，机器人前进一段距离后左转，检测到机器人各个方位距障碍物的距离已经大于距离阈值，说明机器人已经绕过障碍物，机器人回到路径跟踪阶段，停止避障算法并继续执行路径跟踪算法。

图17 避障算法过程分析Fig.17 Process analysis of avoidance algorithm

5.4 试验验证

对巡检机器人导航至某一坐标点时机器人的实际位姿与测量位姿间的误差进行分析，将MATLAB软件通过无线串口与巡检机器人联机，在MATLAB中首先运行PRM算法规划出一条经过点1～点6的全局路径，然后运行Pure Pursuit路径跟踪算法，控制真实的巡检机器人在水泵房环境中按照PRM算法规划的路径运动依次经过点1～点6，并避开路径跟踪过程中出现的障碍物，如图18所示，红色箭头为巡检机器人进行路径跟踪时的运动方向。

图18 机器人路径跟踪运动方向Fig.18 Robot path tracking motion direction

测量机器人运动到在每个坐标点处的位姿，测量结果见表1和表2。

在实际导航中，巡检机器人始终沿着规划的路径行走，根据表1和表2的测量数据通过式(12)和式(13)计算得到巡检机器人在导航过程中各坐标点的位置误差和角度误差，见表3。

表1 各个坐标点的实际位置与测量位置Table 1 Actual position and measurement position of each coordinate point m

表2 各个坐标点的实际偏转角与测量偏转角 Table 2 Actual deflection angle of each coordinate point and measured deflection angle rad

(12)

(13)

由表3可得该巡检机器人在导航过程中位置平均误差为4 cm，姿态角度平均误差为0.095 rad，在机器人即将离开巡检点并移动到下个巡检点的时候，有时位置误差超过 4 cm 且角度误差大于0.1 rad，这是由于移动机器人在加速或减速过程中自重引起的惯性顿挫所造成，在所有的测试中，巡检移动机器人都没有碰到规划路径上的障碍物。结果表明，该巡检机器人能够水泵房中进行巡检工作且满足实际工况需求。