矿用机器人局部路径优化算法研究

武 达，王然风，付 翔，梁 毅

(太原理工大学，山西 太原 030024)

科学技术的发展使得煤矿搜救机器人的研究越来越深入。机器人代替人工探测井下复杂的环境，完成搜索和救援任务，减少了劳力，大大提高了工作效率。但是煤矿井下复杂，当发生矿难时，为了使机器人能够达到指定地点展开作业，要求机器人必须具备一定的避障和路径规划能力[1，2]。

目前有关煤矿井下机器人路径规划的研究可归纳为以下几类。文献[3]提出了基于梯度-坐标轮换法的煤矿搜救机器人最优路径规划算法，虽然可以规划出从起点到目标点的最佳路径，但是没有考虑到机器人需要转弯的问题，容易与障碍物发生摩擦[4]。文献[5]提出了蚁群算法优化的Dijkstra算法煤矿机器人路径规划方法，虽然可以在满足收敛速度条件和安全距离的前提下找到最优路径，但是Dijkstra算法遍历节点多，导致算法计算量大，效率低。文献[6]提出了改进人工势场法来完成局部机器人避障，来达到局部路径规划的目的。文献7通过改进A*算法来使得机器人寻求最优路径，方法是改进了传统A*算法的评价函数。

为了解决上述问题，本文针对传统的A*算法进行改进，设计了基于JPS-A*的算法，该方法主要是针对传统的A*算法在寻找路径的过程中对大量无用的节点进行计算浪费内存导致算法效率低的问题进行了优化，改进后的算法删除了不必要的节点，对筛选出的跳点直接进行操作，大大提高了算法寻找最优路径的效率，达到了局部路径优化的目的[7，8]。

1 矿用机器人控制器总体方案设计

在整个矿用机器人的设计当中，控制器是核心，机器人的所有功能都是基于控制器展开的，本文设计的矿用机器人具有障碍物检测、自动避障导航等功能，根据上述所说，本套控制器的硬件部分包括：电源模块、微控制器模块、障碍物检测模块、电机驱动模块、CAN通信模块等。由于采用的是模块化编程，所以各功能相对独立，方便二次开发与改造升级。

2 控制系统硬件和软件设计

2.1 微控制器选择

主控芯片是由TI公司推出的TMS320C28x系列32位浮点DSP处理器，型号是TMS320f28335，它的工作频率最高是150MHz，内置有256k×16bit的闪存和34k×16bit的SRAM，带有USB、CAN、串口等丰富的外设，具有价格低廉，运算速度块、开法周期短等优点。

2.2 障碍物检测模块

2.2.1 超声波传感器

本文选用的超声波传感器型号是HC-SR04，它的检测范围是2～400cm，精度控制在3cm左右。它的基本工作原理是：采用IO口TRIG触发测距，给最少是10μs的高电平信号，然后模块会自动发送8个频率为40kHz的方波，然后检测是否有信号返回，一旦系统检测有信号返回，ECHO引脚就会输出一个高电平，然后这个信号会返回给主控制器，高电平的持续时间是超声波从发射到返回的时间，把这个时间记为Δt，然后可以算出机器人距离障碍物的距离X。计算公式如下：

X=c×Δt/2

(1)

式中，c为声波速度，取331.4m/s(空气温度为0℃)。

2.2.2 红外光电传感器

红外光电传感器又称为光电开关，电信号输入后转换为光信号输出，照射到障碍物后反射回来，接收器通过接收到的红外线的强弱和有无来进行探测。本文选择的光电开关型号是E18-D80NK，它的测量距离是3～80cm，具有测量距离远、抗干扰信号能力强、易于安装等优点。

这个红外开关的工作原理是：在正常状态下，光电开关连接的IO口输出为高电平，一旦检测到障碍物时间，即输出低电平，将相关IO口设置为下降沿触发中断，一旦发生中断，信号便会传到主控制器，通过程序判断障碍物的方向，从而采取相应的措施进行避障。

2.3 外界环境信息采集模块

矿用机器人要进行躲避障碍物，进行路径规划，首先要了解周围的信息，本文采用普通消费级视频摄像头来得到周围环境的图像，它的图像分辨率是640×480，视频每秒传输的帧数为30，通过CAN通讯接口传递到机器人CPU进行处理，使得机器人知道起始点、障碍物的位置以及终点。

3 路径规划算法设计

3.1 A*寻路算法

A*算法是静态网络中求解最短路径的直接搜索算法，它将Dijkstra算法和BFS算法的搜索策略结合在一起，在保证寻得路径最优的前提下，采用启发式搜索。A*算法通过评估函数来确定路径搜索方向，它的评估函数表示为：

f(m)=g(m)+h(m)

(2)

式中，f(m)表示从起始点到达目的点的评价函数；g(m)表示从起始点到达目的点的实际代价；h(m)表示从起始点到达目标点的估计代价。本文启发式函数h(m)采用欧几里得来测量两点的估计代价：

对于评估函数f(m)，当h(m)=0时，原式退化成Dijkstra算法，在搜索路径时，需要计算大量的节点；当g(m)=0时，原式变成计算从起始点到目标点的BFS算法，虽然计算速度块，但容易进入死胡同，不便于得到最优解。A*算法朝着目的地点的方向进行搜索，对搜索方向上的点进行计算，当达到某一个点时，该点周围的点会被加入到OpenList中，A*算法会选择具有最小估价值的点作为下一个拓展节点，同时该点会被加入ClosedList，然后不断循环这个过程，直到将目的地点加入OpenList。

但是传统的A*算法存在着问题，虽然可以寻找到最优路径，然而寻路过程中，产生了大量不必操作的节点，算法要对这些节点进行不断的访问和计算，当起始点到目的地的路径过于长时，A*算法会呈现出计算量大、大量占用计算机内存、效率低下等问题。

3.2 跳点搜索算法

跳点搜索算法(jump point search，JPS)是一种搜索跳点的策略，跳点就是在搜索过程中产生的一些节点，这些节点可以直接跳跃。寻路过程节点的扩展过程如图1所示，X是起点，Y是终点，从X到Y的路径有：X→b2→b3→Y，X→d2→d3→Y，X→b1→a2→a3→a4→b4→Y，X→c2→c3→Y，在A*算法寻路的过程中，算法会对路径中经过的节点进行评估，选取代价值最小的节点。由以上给出的路径，发现除了节点c2和c3，对其它节点的评估是没有必要的，由图1明显得出路径X→c2→c3→Y永远是最短的。跳点搜索算法就是滤除掉路径上不必要的节点，而只对某点节点进行评估，而这些被评估的节点被称为跳点。图1中的X和Y节点是两个跳点，这两个跳点中间的节点将不会被扩展，这也就大大降低了在寻路过程中对不必要节点的评估，减少了计算量和内存消耗，增加了寻找最优路径的效率。

图1 寻路过程节点的扩展过程

3.2.1 跳点的筛选

对A*算法的改进主要是针对跳点的筛选，结合参考文献[9]提出的几条修剪不必要节点的规则，本文根据现场条件做出了修改，对以下两种情况做出讨论。

1)节点周围不存在障碍物。水平方向扩展如图2所示，S(x)是X的父节点，Z是图上的任意节点，要规划一条从S(x)到Z的路径，对所有情况进行分析可知，一种是经过X向右扩展到达Z节点，另一种是不经过X节点，进而由S(x)向上下扩展到达Z节点。明显可以看出，在所有路径中经过X节点的路径是最短的，但是A*算法经常访问S(x)→X周围的节点，计算量大且没有意义，所以要筛选掉图中的灰色栅格进而提高算法的运算速度。由图2中看出，在达到目标节点Z之前，S(x)→X扩展方向上的邻节点都可以由S(x)直接到达，若是经过X再达到这些邻节点，路径就会变得复杂，所以这些节点被认为是扩展过程中的不必要节点。因此本文给出在如图2所示情况下的筛选节点条件：

l({S(x)，…，Z|X})≤l({S(x)，X，Z})

(4)

函数中的l(x)表示路径的长度，{S(x)，…，Z|X}表示从S(x)为起始点、Z为目标点且不经过X的路径，{S(x)，X，Z}表示这条路径经过X到达目标点Z。同理给出在对角线方向扩展跳点的筛选条件：

l({S(x)，…，Z|X})

(5)

斜对角线方向扩展如图3所示，筛选掉不必要节点后，当路径扩展到X节点时，X周围的相邻节点定义为X的自然邻节点。

图2 水平方向扩展

图3 斜对角线方向扩展

2)节点周围存在障碍物。节点周围存在障碍物时如图4所示，给出筛选条件：①Z不是X的自然邻节点；②l({S(x)，…，Z|X})>l({S(x)，X，Z})满足筛选条件的X的邻节点a3和a1定义为强制邻节点，在路径扩展过程中它们都将被视为跳点。

图4 节点周围存在障碍物

3.2.2 A*算法的改进

改进A*算法的路径寻路过程如图5所示，在满足节点周围存在障碍物筛选条件下，删除不必要节点后，然后再执行A*算法，进而得到最优路径。

图5 改进后的A*算法寻路图

在地图栅格中，S表示起始节点，Z表示目标节点，首先将S节点添加到OpenList，然后沿着水平方向和竖直方向扩展，直到遇到障碍物或者地图边缘，然后沿着对角线扩展到下一节点，然后继续沿着水平和竖直方向扩展，不断重复上述过程，直到在水平方向发现一个强制节点，这时候将强制节点添加到OpenList，继续扩展，直到发现下一个强制节点，重复上述过程，直到发现目标点，停止迭代，将目标点加入到OpenList，得到最终路径[10-12]。

4 仿真分析与实验验证

4.1 环境搭建

为了更好的验证改进后的A*算法的合理性，使用MATLAB进行路径的仿真分析测试。由实际仿真知，当栅格地图较小时，改进后的A*算法和传统的A*算法差别不是特别明显，当栅格地图为60×60以及更大尺寸时，改进后的A*算法的搜索速度得到了较大程度的提升。

4.2 MATLAB仿真结果与分析

在15×15的栅格地图进行的仿真实验如图6和图7所示，其中的黑色部分表示障碍物，蓝色代表起始点，紫色代表目标点，灰色的栅格表示算法在寻路过程中曾添加到Openlist的点。由图6、图7看出改进后的A*算法生成的路径和传统的A*算法基本一致，但是改进后的A*算法寻路过程中操作的节点数大大减少。其他尺寸的栅格地图仿真分析结果见表1。

图6 传统A*算法

图7 改进后的A*算法

由表1得出，改进后的A*算法较传统的A*算法在搜索时间和扩展节点数量大大减少，从而提高了搜索路径的效率，降低了对CPU的内存消耗，满足了井下机器人躲避障碍物应用的条件。

表1 优化A*算法与传统A*算法数据对比

4.3 实验验证

将改进后的A*算法应用到基于TMS320C28x系列32位浮点DSP处理器的移动机器人上，通过机器人挂载的传感器来获取外界环境信息，传感器的位置如图8所示。

图8 试验样机

为了验证改进后的A*算法的时效性，预先在楼道设计好机器人的起始点以及目标点，然后机器人在已设定好的路线上进行路径规划试验，机器人在不同路径下的寻路长度以及寻路时间见表2，实验结果表明结合JPS算法改进的A*算法较传统的A*算法寻路时间大大缩短，因而计算速度更快，寻路效率更高。

表2 不同路径下2种算法寻路时间及路径长度对比

5 结 语

矿用机器人在井下开展作业要进行局部路径规划，本文结合了JPS算法的优势，改进了传统的A*算法遍历节点多、计算量大、时间久等问题，仿真表明改进后的算法在满足躲避障碍物的前提下能够大大提高寻求最优路径的效率，可以满足实际需求，且优化后的效果明显，对矿用机器人的发展有指导意义。