煤矿履带式定向钻机路径规划算法

毛清华， 姚丽杰， 薛旭升

（1.西安科技大学 机械工程学院，陕西 西安 710054；2.陕西省矿山机电装备智能检测与控制重点实验室，陕西 西安 710054）

0 引言

煤炭是我国的主体能源，在能源生产和消费结构中一直占据主体地位，定向钻机作为煤矿井下钻孔施工的主要技术装备，在矿井瓦斯灾害、水害等灾害防治中发挥着巨大作用[1-2]。目前，履带式定向钻机在井下行驶和施工主要依靠操作人员遥控实现，工作环境恶劣，人员劳动强度大，智能化程度较低。2020年八部委联合发布《关于加快煤矿智能化发展的指导意见》，提出了煤矿钻探自动化、智能化和远程控制的发展方向[3]。自主行驶是智能化钻探的基础，对于提高钻探效率和钻机智能化程度，减轻人员劳动强度，推动煤矿安全、高效、智能生产具有重要意义。

路径规划是实现履带式定向钻机自主行驶的重要环节，包括全局路径规划和局部路径规划。全局路径规划是根据已知环境信息，从起点出发，找到一条可行路径到达目标点；局部路径规划是由传感器实时采集环境信息进行动态路径规划。履带式定向钻机依靠操纵台行驶，且体积较大，应用于中大型煤矿或巷道，大多情况下行驶于无障碍环境，但行驶过程中可能会遇到行人等动态障碍物。因此，实现定向钻机自主行走还需着重考虑避障的准确性[4-5]。目前常用的全局路径规划算法包括Dijkstra算法[6-7]、蚁群算法[8-9]、A*算法[10]等，局部路径规划算法有人工势场法[11-12]、时间弹性带（Time Elastic Band，TEB）算法[13]等。A*算法作为一种有效的静态路网路径规划算法，简单易实现，但存在拐点较多、转角大、路径贴近障碍物等缺点。许多学者对其进行了改进。张伟民等[14]对A*算法增加扩展节点，设置距离阈值，并采用五次B样条曲线拟合得到煤矿救援机器人的优化路径。赵久强等[15]以移动机器人行驶时间作为代价，依据障碍物信息调整A*算法的启发函数，使用Floyd算法进一步优化，提升了路径平滑度。薛光辉等[16]提出以障碍物大小为基准设定栅格尺寸、机器人占据多个栅格的狭长空间地图建模方法，利用障碍物碰撞检测函数改进A*算法，并完成了狭长空间的复杂环境路径规划。金辉等[17]利用拓扑图将燃油消耗与最优速度对应，将A*算法的启发函数改进为当前节点到终点的最优经济燃油消耗，得到最优速度规划，提升了燃油经济性，减少了交叉路口通过时间。Tian Hao等[18]提出一种核辐射环境下的改进A*算法，在启发函数中加入地形和移动速度等因素，将地形对运动的影响分为可接近和不可接近，其中可接近部分将地形的影响量化为速度，可求出核辐射复杂环境中的最短路径。Li Dongcheng等[19]优化了A*算法搜索策略、步长和成本函数，减少了搜索时间，在Q学习的探索机制中增加了动态探索因子，实现了无人机的全局-局部混合路径规划。除A*算法外，王文飞等[20]基于电势能原理改进动态窗口法的扩展轨迹评价函数，提升了路径规划能力和实时性。封硕等[21]将改进的双粒子群算法与路径规划模型相结合，在复杂路段寻找最优路径，提高了路径规划成功率，缩短了路径长度。Wang Yinchu等[22]提出一种基于深度强化学习的机器人路径规划方法。Wang Fang等[23]在遗传算法选择过程中引入自适应随机检验方法来增强初始多样性，提高了算法的全局搜索能力和收敛速率，并采用Clothoid曲线改善了路径曲线的连续性。

上述文献针对全局或局部路径规划算法进行了优化改进。当环境发生变化时，算法需要从当前节点重新规划到目标节点的路径，耗时较长。部分文献在路径规划阶段未考虑机器人的尺寸等，易与障碍物发生碰撞。另外，传统A*算法规划的路径存在拐点，不利于定向钻机控制，而局部路径规划算法因未利用全局信息，易出现局部最优解。针对上述问题，提出一种改进A*算法和动态窗口法（Dynamic Window Approach，DWA）融合的履带式定向钻机路径规划方法。首先，在传统A*算法中加入安全距离约束，保证路径的安全性，针对启发函数设计自适应权重系数，提高全局路径的搜索效率；其次，剔除路径中的冗余节点，采用分段三次Hermite插值对路径进行二次平滑处理，便于履带式定向钻机的跟踪控制；最后，以改进A*算法规划全局路径，引导DWA完成局部路径规划，实现定向钻机在煤矿井下巷道中的避障功能。

1 履带式定向钻机路径规划方案

1.1 环境建模

栅格地图用于建立二维静态或动态运行环境，是路径规划中常用的环境地图模型。将煤矿井下环境简化为栅格平面，以栅格为单位记录煤矿环境信息，任意一个栅格与现实环境存在一一对应关系。栅格被赋予状态后共同组成环境地图模型，栅格划分越精细，算法运行占用空间越大。

在数学上以可通行区域为0、障碍物区域为1的元素M（i，j）（（i，j）为栅格点坐标）构成的矩阵表示栅格地图，如图1所示。白色栅格表示煤矿巷道可通行区域；黑色栅格表示煤层或障碍物，定向钻机无法通行。

图1 栅格地图模型Fig.1 Grid map model

1.2 A*算法路径规划

结合广度优先Dijkstra和深度优先贪心算法的A*算法是常见的路径搜索算法。A*算法的启发函数为

式中：f(n)为从起点经过当前节点n到目标节点的总代价；g(n)为从起点到当前节点n消耗的代价；h(n)为从当前节点n到目标节点的估计代价，包括曼哈顿距离h1(n)、欧几里得距离h2(n)和切比雪夫距离h3(n)。

式中：(xn,yn)为当前节点坐标；(xgoal,ygoal)为目标节点坐标。

1.3 A*算法搜索策略

A*算法在路径搜索阶段一般采用四邻域或八邻域的搜索策略，如图2所示。四邻域搜索是对当前节点东南西北4个方向进行扩展，扩展邻域集合可表示为{(xn±1,yn),(xn,yn±1)}，相邻方向夹角为90°。八邻域是在四邻域扩展的基础上增加4个方向，为当前节点的8个方向，扩展邻域集合为{(xn±1,yn),(xn,yn±1),(xn±1,yn±1)}，相邻方向夹角为45°。

图2 四邻域和八邻域搜索Fig.2 Four neighborhood and eight neighborhood search

二维平面中，欧几里得距离是两点之间的直线距离；曼哈顿距离是两点沿着网格线的总距离；切比雪夫距离是两点在各维度上坐标差值的最大绝对值，没有考虑路径走向。相比之下，曼哈顿距离计算简单，适用于四邻域情况，更符合煤矿环境的距离表达要求。因此，选用四邻域作为A*算法的搜索策略，以曼哈顿距离计算节点间的估计代价等，将安全距离约束引入传统A*算法，进行启发函数优化和路径平滑。

2 基于改进A*算法的路径规划

2.1 安全扩展策略

栅格地图中可能存在路径中的某一节点邻域是障碍物的情况。因寻求最优路径，规划的路径可能会贴近障碍物。定向钻机作为一种特殊移动体，若将其视为质点，很可能会出现规划的路径穿越障碍物的情况，如图3所示。这将增大定向钻机行驶过程中的碰撞风险。因此，在考虑路径最短之外，应考虑定向钻机自身的尺寸。本文引入安全扩展策略，为定向钻机和障碍物之间保留安全行驶距离。

图3 A*算法规划路径穿越障碍物Fig.3 Planned path of A* algorithm through obstacles

为避免规划路径受钻机机身尺寸影响，将栅格中心点视为钻机中心点，在定向钻机和巷道壁、障碍物之间添加距离函数约束，建立安全扩展策略（图4）。

图4 路径规划安全扩展策略Fig.4 Safety extension strategy of path planning

1） 遍历整体煤矿栅格地图，以父节点为中心构建正方形栅格区域。

2） 检测构建的正方形栅格区域是否存在煤层或障碍物节点。

3） 若该父节点邻域内存在障碍物节点，则将该节点的代价设为无穷大，将其标定为危险节点，不作为扩展节点。

4） 若该父节点邻域内不存在障碍物节点，则对其不做处理。

如果在路径搜索过程中判断危险节点，会延长路径规划时间。因此在程序运行前，先对栅格地图进行预处理，遍历栅格地图中的可通行区域，将其存储在矩阵中。

2.2 启发函数优化

启发函数的选择决定A*算法的搜索效率。大多情况下定向钻机行驶于无障碍的巷道，因此，提高全局路径的搜索效率非常有必要。在栅格地图中，当前节点和目标节点的估计代价始终比实际值低。若启发函数的估计代价h(n)=0，则A*算法会退化为Dijkstra算法。Dijkstra算法会在起点和目标节点之间找到最优路径，但其搜索范围变大，耗费大量时间。

传统A*算法中，g(n)与h(n)的权重比为1∶1。为h(n)增加权重系数w(n)，若w(n)=2，则g(n)与h(n)的权重比变为1∶2，使得启发函数偏向于估计代价h(n)，从而提高路径搜索效率。基于该方法，A*算法启发函数可表示为

定向钻机在路径规划过程中需兼顾搜索速度和最优路径，w(n)需根据当前节点位置调整。h(n)较大时，为了尽快完成目标节点扩展，w(n)应变大，从而加快搜索速度；h(n)较小时，倾向于搜索最优路径，w(n)应变小。为此，基于h(n)设计自适应权重系数，同时将父节点的影响引入启发函数中，减少A*算法遍历节点数量。优化的启发函数为

式中：D为起点到目标节点的欧几里得距离；(xstart，ystart)为起点坐标；H(q)为当前节点的父节点q到目标节点的曼哈顿距离。

通过对A*算法的启发函数进行自适应权重优化，调整定向钻机路径规划过程中起点到当前节点的实际代价与当前节点到目标节点的估计代价在评价函数中的权重占比，随着定向钻机逐渐靠近目标节点，h(n)的权重系数逐渐趋近于1，估计代价逐渐趋近于0。

2.3 路径平滑策略

定向钻机在搜索出起点到目标节点之间的路径后，开始向目标节点移动。规划出的路径由不同斜率的折线段组成，但因采用四邻域扩展策略，规划路径仍存在转折次数多的问题，导致钻机转向困难，降低了钻机行驶效率，需对路径进行平滑处理。

2.3.1 冗余节点剔除方法

在栅格地图中，定义S为路径上的一点，在S点后存在若干路径节点。剔除冗余节点的基本思想：在生成的路径列表中，依次连接起点和列表中的各节点，如果起点和节点n的连线不经过障碍物，且起点和节点n+1的连线经过障碍物，则剔除起点和节点n之间的若干点，最终提取保留的路径S→n。在剔除全部冗余节点后，按顺序连接列表中的节点，生成优化后的路径。冗余节点剔除方法如图5所示，其中红色线表示等距采样点，用于判断两点之间的路径是否安全无碰撞。

剔除冗余节点步骤如下：

1） 基于前文改进后的A*算法规划出一条全局最优路径，保存路径节点等信息。

2） 将上述全局最优路径上的节点记为集合{p1,p2,···,pN}，p1为路径起点，pN为路径目标节点，N为节点总数。

3） 从起点开始，遍历后续每个节点，分别将当前节点与后面节点相连作为备选路径，对备选路径进行碰撞检测。若在当前节点的连接路径发生碰撞，则保留前一节点，反之保留当前节点。

4） 若当前路径与地图中最近障碍物的距离大于设置的安全距离，则保留该路径，并删除中间节点；若小于安全距离则不做任何操作，保留原来的路径，直至到达目标节点。

2.3.2 规划路径二次平滑

在煤矿环境下，改进A*算法可以快速规划出一条安全路径，剔除冗余节点后的路径可减少路径拐点数量，但仍存在转折点，需对路径进行二次平滑。

Hermite插值在区间生成插值节点，构造出一条光滑的曲线，可以对路径进行平滑处理，保证路径的连续光滑性。但直接使用Hermite插值得到的多项式次数较高，因此采用分段三次Hermite插值函数P对剔除冗余节点后的路径进行二次平滑处理。

式中：Jn,In为插值基函数；Yn为路径节点n处的函数值。

3 改进A*算法与DWA融合算法

3.1 DWA路径规划算法

3.1.1 履带式定向钻机运动模型

履带式定向钻机与两轮差速驱动/四轮驱动均存在类似的非全向约束，通过控制两侧履带的相对速度实现转向，通过线速度、角速度描述其运动。履带式定向钻机运动模型如图6所示，v(t)，ω(t)分别为t时刻定向钻机在巷道坐标系xoy下的线速度与角速度，θ(t)为t时刻定向钻机姿态角。在采样周期∆t内，定向钻机可视为做匀速直线运动。

图6 定向钻机简化模型Fig.6 Simplified model of directional drilling rig

定向钻机位姿为

式中(x(t),y(t))为t时刻定向钻机位置。

3.1.2 速度采样

实际应用中要根据定向钻机性能和环境对速度矢量空间[v,w]采样进行约束。

定向钻机自身最大、最小速度约束为

式中：vmin,vmax分别为定向钻机最小线速度和最大线速度；ωmin,ωmax分别为定向钻机最小角速度和最大角速度。

定向钻机最大、最小加速度约束为

式中：vc,ωc分别为定向钻机当前时刻的线速度和角速度；分别为定向钻机当前时刻的最大线减速度和最大角减速度；分别为定向钻机当前时刻的最大线加速度和最大角加速度。

定向钻机制动距离约束为

式中d(v,ω)为v,ω下定向钻机与障碍物的最小距离。

对式（9）—式（11）所示的 3个约束求交集，得到定向钻机采样速度Vr=Vs∩Vd∩Va。

3.1.3 评价函数

DWA的评价函数综合考虑定向钻机与目标节点的角度偏差、线速度、模拟轨迹末端与最近障碍物距离。对上述3个量进行归一化处理，并分别赋予权重后相加，得到轨迹评价函数：

式中：σ（·）为归一化函数；A(v,ω)为方位角评价函数，用于评价钻机轨迹末端朝向与目标节点之间的角度差；L(v,ω)为定向钻机当前线速度评价函数；α，β，γ为各项权重。

3.2 融合算法

A*算法是基于先验地图的路径规划算法，只能应用于环境信息全部已知的情况，若在A*算法规划好的路径上出现障碍物，定向钻机无法躲避，需引入局部避障算法。DWA依靠外部传感器实时检测环境信息进行局部路径规划，在采样周期∆t内的速度空间[v,ω]中对速度进行采样，通过评价函数评价运动轨迹，选取对应[v,ω]下的最优轨迹完成运动，但缺少指引点，存在局部最优解。

将改进A*算法与DWA融合：以改进A*算法规划全局路径，指引DWA进行局部路径规划，并以最优轨迹控制钻机向目标节点移动，当激光雷达或其他外部传感器检测到未知障碍时，利用DWA实现避障。融合算法流程如图7所示。

图7 改进A*算法与DWA融合算法流程Fig.7 Fusion algorithm flow of improved A* algorithm and dynamic window approach (DWA)

4 仿真及结果分析

为验证改进A*算法和融合算法的有效性，针对履带式定向钻机不同工况环境建立栅格地图，在12th Gen Intel（R） Core（TM） i5-12500H 2.50 GHz计算机上，采用Matlab 2022b软件对改进A*算法、改进A*算法与DWA融合算法进行仿真验证。

4.1 基于20×20栅格的改进A*算法仿真

在Matlab中建立20×20的巷道栅格地图，对改进A*算法的安全扩展策略、启发函数优化策略和路径平滑策略进行验证。设栅格地图中x，y为横纵坐标，单位栅格长度为1 m，黑色栅格代表煤层、障碍物，白色栅格代表可通行区域。

4.1.1 引入安全扩展策略前后路径规划对比

在20×20的煤矿巷道栅格地图中，对传统A*算法和引入安全扩展策略后的算法进行仿真，结果如图8所示。可看出A*算法引入安全扩展策略后，规划出的路径远离障碍物，路径长度增加，但保证了定向钻机与巷道壁、障碍物的安全距离，避免了因路径贴近障碍物而发生碰撞。

4.1.2 启发函数优化前后路径规划对比

在20×20的煤矿巷道栅格地图中，对优化启发函数前后的A*算法进行仿真，结果如图9所示。可看出采用自适应权重优化方法优化启发函数后，A*算法减少了路径搜索节点，搜索时间为0.024 7 s，较传统A*算法的0.038 8 s减少36.3%，搜索效率显著提高。

图9 A*算法启发函数优化前后20×20 栅格地图内钻机路径规划Fig.9 Drilling rig path planning in 20×20 grid before and after optimizing heuristic function in A* algorithm

4.1.3 路径平滑前后对比

在20×20的煤矿巷道栅格地图中，剔除冗余节点前后的路径规划结果如图10所示。可看出剔除冗余节点后，路径长度为19.571 m，较剔除冗余节点前的23 m减小14.9%，且减少了不必要的转弯，规划出的路径更为平滑。

图10 剔除冗余节点前后20×20 栅格地图内定向钻机路径规划Fig.10 Drilling rig path planning in 20×20 grid before and after deleting redundant nodes

采用分段三次Hermite插值方法对剔除冗余节点后的路径进行二次平滑，结果如图11所示。可看出二次平滑后的路径基本平滑，更符合定向钻机的运动特性。

图11 二次平滑处理后定向钻机规划路径Fig.11 Drilling rig path planning after quadratic smoothing

4.2 基于50×50栅格的改进A*算法仿真

考虑煤矿井下巷道实际环境，履带式定向钻机主要行驶于井下辅运巷、回风巷等，因此根据巷道环境对辅运巷（直行工况）、由辅运巷进入辅运联络巷（转弯工况）和由辅运巷到达回风巷（转巷工况）3种工况建立栅格地图，地图大小为50×50，单位栅格长度为1 m。分别采用Dijkstra算法、传统A*算法和改进A*算法在不同栅格地图中进行路径规划，仿真结果如图12所示，不同算法性能对比见表1。

表1 改进 A*算法与其他路径规划算法性能对比Table 1 Performance comparison between improved A* algorithm and other path planning algorithms

图12 50 × 50栅格地图内不同算法路径规划结果Fig.12 Path planning result of different algorithms in 50 × 50 grid map

由图12和表1可看出，改进A*算法的路径搜索时间分别较Dijkstra算法和传统A*算法平均减少88.5%和63.2%，且在路径长度和转折点上有不同程度的优化，对转弯和转巷工况的优化效果较明显。改进A*算法规划出的路径可与巷道壁及障碍物保持必要的安全距离，保证了定向钻机在巷道中行驶的安全性。同时，改进A*算法通过剔除路径中的冗余节点和路径平滑处理，提高了规划路径的质量。

4.3 基于50×50栅格的融合算法仿真

在前文建立的50×50栅格地图中进行改进A*算法与DWA融合算法仿真验证。在改进A*算法规划路径上设置障碍物，验证融合算法的避障效果，并将本文算法与PRM算法和RRT*算法进行对比分析。仿真结果如图13所示。4种算法的性能对比见表2。

表2 融合算法与其他路径规划算法性能对比Table 2 Performance comparison between the fusion algorithm and other path planning algorithms

图13 50×50栅格地图内改进A*算法与DWA融合算法路径规划结果Fig.13 Path planning results of fusion algorithm of the improved A* algorithm and DWA in 50 × 50 grid map

从图13和表2可看出，在相同的工况环境下，因PRM和RRT*为基于概率采样的路径规划算法，若采样次数过少，PRM算法路径规划可能失败，RRT*算法路径规划效果较差；融合算法规划路径长度较改进A*算法稍有增加，但平滑性和路径长度优于PRM算法和RRT*算法规划路径，更便于定向钻机行驶；融合算法与未知障碍物保持了安全距离，可在全局路径最优的基础上，使定向钻机安全绕过未知障碍物并到达目标节点。

5 结论

1） 在传统A*算法中引入安全距离约束，并对启发函数进行自适应权重优化。仿真结果表明，经上述优化后，A*算法规划出的路径可保证定向钻机与巷道壁、障碍物之间保持安全距离，且路径搜索效率高，路径搜索时间较传统A*算法减少了36.3%。

2） 对基于四邻域的A*算法规划出的路径剔除冗余节点，之后采用分段三次Hermite插值方法进行二次路径优化。仿真结果表明，经路径平滑后，规划路径拐点及转折点减少，路径长度较平滑前减小14.9%，定向钻机运行效率得以提高。

3） 对钻机在直行、转弯、转巷3种工况下的路径规划进行仿真，结果表明，改进A*算法的路径搜索时间分别较Dijkstra算法和传统A*算法平均减少88.5%和63.2%。

4） 将改进A*算法和DWA融合，在保证全局最优的前提下，使得定向钻机可动态规划出最优路径，避开未知障碍物。仿真结果表明，在不同工况下，改进A*算法规划路径与未知障碍物的距离为0，融合算法规划路径与未知障碍物的最小距离为1.257 m；与PRM算法和RRT*算法相比，融合算法规划路径长度分别平均减小5.5%和2.9%，路径平滑性更好。

5） 目前主要通过仿真分析验证了路径规划算法的有效性，未涉及履带式定向钻机的控制，下一步重点研究将提出的路径规划算法融入控制算法，并在煤矿井下进行试验验证。