面向铲装作业场景的自动驾驶矿车路径规划方法研究

张纳川， 王亚飞，2， 章翼辰， 王炜杰， 梅贵周

（1. 上海交通大学 机械与动力工程学院，上海 200240；2. 上海交通大学 汽车动力与智能控制国家工程研究中心，上海 200240；3. 安徽海博智能科技有限公司，安徽，芜湖 241200）

为提高矿区生产作业效率、降低矿区运营成本、满足矿业智能化产业升级需求，各类无人驾驶矿车正被广泛地应用于露天矿区“智慧矿山”的建设中。其中，矿车自动化铲装作业是提高智能化开采效率的重要环节之一。铲装作业需要挖机与矿车协调配合，作业能否高效安全地进行主要取决于停车位置的准确性、复杂环境下的车辆绕障性能以及多车序列化作业的连续度等。面对矿区复杂多变的行车环境以及较高的实时性与安全性要求，如何在短时间内完成安全系数较高的路径规划便成为了铲装作业场景下构建矿车自动驾驶系统的重要课题。

路径规划是自动驾驶架构中的重要功能层级，旨在通过分析获取到的环境信息为车辆构建连接起点位置与终点位置的序列点或曲线，常见的路径规划方法包括基于图搜索的Dijkstra算法［1］、A*算法［2］，基于采样的RRT算法［3］，基于虚拟力的人工势场法［4］，基于仿生学的蚁群算法［5］，基于Frenet框架的OTG算法［6］等。然而在实际应用中，上述大多数规划方法输出的路径并未考虑车辆运动学约束，需要经过后续处理才能进一步使用。

针对这一问题，DOLGOV等［7］提出了一种考虑车辆行进最大曲率约束的混合A*算法，该算法能在有障碍物的低速环境下生成符合车辆运动学约束的路径。然而，该算法的缺点也较为明显，即计算时间较长，实时性较差，且生成的路径中存在较多不必要的倒车与转弯，易产生安全隐患。一直以来，不同作者都在针对混合A*算法的局限性进行优化与补充，KURZER［8］提出的改进算法改进了搜索与剪枝方法，并为代价函数增添了转向、倒车等附加成本，但规划时间依然较长。SEDIGHI等［9］提出的引导式算法通过预先快速构建出无约束路径上的关键点作为方向引导，从而辅助混合A*算法进行路径规划，明显降低了规划时长，但在复杂场景下因未对关键点进行筛选而导致输出路径不够平滑。SHENG Weitian等［10］提出的针对泊车场景的改进算法与引导式算法思路类似，即先通过传统A*算法构建无约束路径，再提取路径上障碍物较为密集的狭窄区域进行局部混合A*路径规划，最后使用全局混合A*路径规划进行连通，提高了路径的安全性，但在复杂场景下对运算效率的提升有限。TU Kangbin等［11］提出的改进算法限制了部分场景下搜索时的反向扩展，同时将车辆朝向角引入启发信息中，提高了运算速度，但需要针对不同场景调试具体参数且未解决局部最优问题。ZHANG Songyi等［12］提出的改进算法优化了子节点扩展方法，同时基于曲率、曲率变化、曲线长度、倒车惩罚等设置了代价函数加权系数，生成路径的平滑性得到提高，但并未显著提升运算效率。

综上所述，多数改进算法不能同时兼顾对计算效率和路径质量的提升，也难以解决大型地图中面对连续U形障碍物的复杂场景时车辆陷入局部最优的问题。针对混合A*算法的固有问题以及现有改进算法在矿区铲装作业场景下的不足，本文提出了一种基于引导式变步长混合A*算法的动态路径规划方法，通过建立并提取维诺图中的关键点作为方向引导以避免车辆在U形障碍物处陷入局部最优，同时引入变步长搜索降低规划时长，有效地提高了运算效率及安全性，并通过开展矿区实车场景试验验证了该算法的可用性。

1 混合A*算法结构

混合A*算法是基于传统A*算法的改进，以KURZER提出的算法为例，其相比于传统A*算法的改进，一方面在于搜索路径时考虑了车辆运动学约束，即根据拓展步长、车辆最小转弯半径、前轮转角以及前进后退约束拓展子节点，如图2所示。

图2 混合A*算法子节点扩展

另一方面在于代价函数的重新设置，设当前节点n(xn，yn)，起点s(xs，ys)，终点e(xe，ye)，则计算节点代价值的公式为：

式中：ReedsSheppCost为在忽略地图障碍物分布的情况下当前节点到终点的Reeds-Shepp曲线长度；AStarCost为在真实情况下当前节点到终点的传统A*算法代价值。由于节点搜索的顺序与代价值大小的启发高度相关，通过上述改动，混合A*算法在搜索时兼顾了对障碍物限制与车辆运动约束的考虑，最终能够输出符合车辆运动学的避障路径。

2 引导式关键点

由于露天矿山地理环境的复杂性以及铲装作业堆放土石的作业特点，矿车的行驶区域常常会出现包括U形在内的各种形状的连续障碍物，常规混合A*算法在规划绕开U形障碍物的行车路径时花费时间较长，为尽可能提高该过程的计算效率，本文设计了引导式关键点用于指导路径规划方向，具体流程如下。

2.1 建立维诺图

维诺图又称泰森多边形或狄利克雷镶嵌，指在给定平面及生成点的情况下，通过连接两相邻生成点直线的垂直平分线构成的数个连续多边形对平面空间进行分割后得到的图形。每个多边形内均有唯一的生成点，且多边形内任意一点到该区域生成点的距离小于到其他区域生成点的距离。

在路径规划中，通常将单个连续障碍物作为生成点构建维诺图［13］，主要思路为由栅格地图中的障碍物边缘向外不断扩展，计算每个栅格对应最近障碍物的坐标及距离并划分维诺图边缘，最后对维诺图进行剪枝，在保留连通区域的前提下，尽可能地将两栅格宽度的边缘精简为单栅格宽度。该构建方法耗时较短，通过对新加入或被删除的障碍物重复上述流程，可以在保证实时性的情况下完成维诺图的更新，适用于动态规划。如图3所示，黑色区域为障碍物，红色区域即为生成的维诺图边缘。

图3 栅格地图（左）与维诺图（右）

2.2 提取关键点

设置关键点的目的在于给予混合A*路径规划大致的避开障碍物的方向引导，以进一步提升后续规划效率。首先输入本次路径规划的起点与终点，寻找距离起点与终点距离最近的维诺图边缘上的点作为路网起点与路网终点，通过Dijkstra算法得到一条从路网起点到路网终点的位于维诺图边缘上的路径；对路径点进行进一步筛选，由于此前进行过剪枝，所以可通过栅格点连通情况提取关键点，统计路径点四向在维诺图边缘上的栅格点数量，如大于等于3则可认为该栅格点为路网连通区域，即维诺图多边形顶点。这些顶点到周围障碍物的距离相等，是该区域避开障碍物相对安全的通行点，因此，将其作为路径规划的引导式关键点。如图4所示，绿色栅格点即为提取到的引导式关键点。

图4 关键点提取方法

2.3 根据关键点规划路径

接下来便是完成起点-关键点-终点的混合A*路径规划。在障碍物连续复杂且并非规律几何体的情况下，此前提取的关键点存在较多冗余，在部分连通区域尤其集中，因此需要进行筛选整合。在目标为关键点的规划过程中，改为使用以关键点为中心的九宫格作为终点区域，在搜索过程中将子节点拓展至终点区域时便视为完成当前规划，并将此时车辆的朝向角与坐标作为下一次规划的起点姿态。该方法既能为中间点设定符合车辆当前运动状态的朝向，又能在关键点相对集中时将其整合到同一区域，避免了车辆不必要的倒车与转弯，提高了路径质量。如图5所示，绿色栅格及其周围的蓝色栅格的集合构成了每次路径规划的终点区域。

图5 根据关键点规划路径

3 自适应变步长

为在进一步提高运算效率的同时保证安全性，在计算代价值时引入了自适应变步长搜索，具体流程如下。

3.1 变步长搜索规则

混合A*算法在计算代价值时会引入传统A*算法的代价值，因此可通过加快传统A*算法代价的计算过程来加速整体规划流程。预先将搜索步长设定为1～5的整值，在扩展子节点时以当前节点为中心，扩展周围到中心欧式距离小于当前步长的栅格。由于规划过程中关键点之间的方位关系十分明显，反向搜索往往会得到代价值更大的子节点，所以在搜索时不考虑从当前节点到终点反方向上的栅格以降低搜索成本。

为保证安全性，在接近障碍物的区域应缩短步长，进行更精细的搜索；在远离障碍物的区域应加大步长，进行更快速的搜索。具体方法是在每次扩展子节点时，通过此前构建的维诺图获取最近的障碍物距离，并将当前搜索步长设置为小于该距离的最大预设整值，如图6所示，黄色为当前节点，紫色为终点，黑色为障碍物，蓝色为搜索范围。

图6 不同位置节点的搜索范围

3.2 代价函数改进

传统A*算法代价函数f(n)=g(n)+h(n)中的g(n)为起点到节点的曼哈顿距离，该方法会导致加长搜索步长扩展时距离当前节点更远的子节点代价大于距离更近的子节点代价，使算法更倾向于将距离较近的子节点作为下一次的扩展节点，导致搜索范围始终在近处，降低了搜索效率。

为解决这一问题，引入了多步长A*搜索中的权重系数w(n)［14］，设当前计算代价的子节点n(xn，yn)，父节点c(xc，yc)，本次扩展的所有字节点k(xk，yk)，则权重系数计算表达式为：

加入权重系数w(n)后的代价函数为：

该权重系数基于子节点到当前节点的相对距离赋权g(n)，使距离更远的子节点g(n)值更小，从而被更加优先扩展，充分发挥了加长搜索步长时范围更广、速度更快的优势。

综上所述，本文提出的引导式变步长混合A*算法的流程如图7所示。

图7 引导式变步长混合A*算法流程

4 实车试验

为验证算法的可用性，在某矿区一处场地进行了实车试验。如图8所示，白色为可通行区域，黑色为山体与石堆，该处场地大小约为788 m×789 m，具有露天矿区障碍物连续复杂且非规律几何体的特点，在场地东西方向各存在一较大的U形障碍物，适于验证算法的绕障情况。

图8 试验场地及对应的栅格地图

依照此前的算法流程，首先通过建立维诺图来提取引导式关键点，如图9所示，红色区域为维诺图边缘，绿色栅格点为引导式关键点，由于障碍物本身形状不规律，可以看到生成的维诺图边缘比较复杂，引导式关键点在部分复杂区域明显集中。

图9 试验场地维诺图

将起点设置在左侧U形障碍物内部且令车辆面朝障碍物方向（x：289.633，y：528.732，heading：2.49°），将终点设置在右侧U形障碍物内部（x：516.231，y：251.173，heading： 195.38°），在ROS操作系统及AMD Ryzen 7 5800H处理器的环境下开展路径规划。与此同时，设置KURZER提出的改进混合A*算法及SEDIGHI等提出的引导式混合A*算法作为对照组进行试验。

试验结果如图10～12和表1所示，这里选择了规划时长、到障碍物平均距离、路径曲率变动次数3种评价指标，前者决定计算效率，后两者决定路径质量。其中，规划时长指算法从输入到输出的总计算时间，该时间过长会使车辆在规划完成前出现原地等待现象，导致自动铲装作业效率降低；到障碍物平均距离指路径点到周围障碍物最小距离的平均值，该值过小表明车辆距离障碍物过近，导致路径安全性降低；路径曲率变动指单次转弯朝向角变化超过20 °或一次前进倒车转换，次数过多会造成车辆轮胎磨损严重且控制误差提升。

图1 露天矿区铲装作业

表1 试验结果对比

图10 Karl Kurzer算法试验结果

在计算效率方面，KURZER的算法由于缺乏引导与变步长加速，加之地图本身较大，其规划时长接近19 min，在实际作业中会导致矿车队列等待，降低工作效率。SEDIGHI的算法计算效率明显提升，但由于将部分多余的引导式关键点纳入规划且无变步长，计算速度相较于本文提出算法略低。本文提出的引导式变步长混合A*算法相比SEDIGHI的算法运算效率提升近68%，显著加强了算法的实时性。

在路径质量方面，能够明显看出KURZER的算法输出路径到障碍物的距离过近，安全性较低。SEDIGHI的算法输出路径由于复杂场景关键点较多且未进行筛选整合，因而产生了很多不必要的倒车及转弯，增大了车辆轮胎的磨损率与车辆误差。本文提出的引导式变步长混合A*算法相比SEDIGHI的算法到障碍物平均距离增加了11%，路径曲率变动次数减少了45%，兼顾了安全性与路径平滑，具有更好的路径质量。

综上所述，本文提出的引导式变步长混合A*算法在实时性与安全性上均有更好的表现，适用于铲装作业场景自动驾驶矿车路径规划。

图11 ShilanSedighi算法试验结果

图12 本文提出的算法试验结果

5 结论

针对露天矿区铲装作业场景复杂多变、路径规划安全性与实时性要求高的特点，本文提出了一种基于引导式变步长混合A*算法的矿车路径规划方法，在混合A*算法的基础上引入了引导式关键点与自适应变步长的改进策略，解决了矿车在路径规划过程中计算时间久、路径质量差的问题，并通过在实车试验中与其他改进混合A*算法的对比验证了该算法的实时性与安全性优势。在后续研究中，可基于矿区场景中的多车协同铲装作业模式进行算法优化，避免同时段工作车辆路径冲突。此外，还可以基于部分矿车的铰接结构设计配套的控制方法，进一步提升算法工程应用价值。