露天矿区四轮转向车辆装载停靠路径规划算法

周 彬，王 闯，李一鸣，陈 闯，王俊华，王宏伟

（1.北京航空航天大学，北京 100191；2.北京踏歌智行科技有限公司，北京 100191；3.内蒙古电投能源股份有限公司，内蒙古 霍林郭勒 029200）

近年来，国内对矿产资源的需求日益增长，各矿区采掘规模逐步扩大，对运输车辆和驾驶员的数量需求与日俱增。运输作业作为矿区生产开采的重要环节，其过程中存在的安全、效率等方面问题为矿区的生产与发展带来了巨大挑战[1]。随着信息、通信技术的快速发展，人工智能正在向生产生活的各个方面渗透，无人驾驶技术应运而生。无人驾驶技术能够缓解复杂环境下人工驾驶风险高、效率低下等问题，因此在矿区得到了推广应用[2]。2020 年6月，踏歌智行通过了国家电投南露天煤矿无人驾驶项目的一期项目验收，并于2021 年7 月22 日完成安全员下车测试；2022 年6 月，踏歌智行在鄂尔多斯永顺煤矿的宽体车无人运输项目已实现7 × 24 h 多编组宽体车无安全员作业，且作业效率达到人工效率的80%以上。

无人驾驶运输车辆的作业区域一般可分为道路行驶区、装载区、卸载区等；其在道路行驶区的任务是沿固定运输道路进行路径跟踪，在装（卸）载区的任务则是根据装卸任务需要进行停靠装载与卸载。无人驾驶运输车辆重载运输任务示意图如图1。

图1 无人驾驶运输车辆重载运输任务示意图

图1 显示了无人驾驶运输车辆的重载运输过程，路径规划模块首先会分别生成装载区内的路径PA、道路行驶区内的路径AB、卸载区内的路径BQ。重载行驶时，车辆从P 点出发，沿PABQ 行驶，在Q点停靠；空载行驶时，车辆从Q 点出发，沿QBAP 行驶，在P 点停靠。

随着现场生产作业进行，无人驾驶运输车辆装（卸）载点的位置会根据下发任务实时更新，如装载点从P 更新至P1，卸载点从Q 更新至Q1。现有的规划算法每次执行任务时都需要重新规划路径P1A 和BQ1。此外，受限于车辆的最小转弯半径和装（卸）载区狭窄的运动范围，部分装卸任务需要在停靠点周围多次换挡泊车才能停靠到期望位姿。在此过程中，对无人驾驶车辆控制算法精度提出了较高要求，且耗费大量时间，影响了生产作业效率。

随着矿用机械设备的快速发展，当前出现了具备四轮转向能力的无驾驶室矿用卡车。结合矿区无人驾驶运输车辆无前后之分和四轮转向的特性，提出了一种高效的停靠路径规划算法。算法能够根据给定车辆的当前位置和朝向、目标停靠点位置和朝向，实时规划出1 条满足车辆最小转弯半径约束的停靠路径。

1 模型建立

1.1 四轮转向车辆特性

前轮转向车辆，即车辆在转弯过程中仅有前轮可以转向的车辆；根据驱动方式的不同，又可分为后轮驱动前轮转向和前轮驱动前轮转向2 种。一般货运车辆车采用的是后轮驱动前轮转向方式。四轮转向车辆，即车辆在转弯过程中，前轮和后轮各自能按照一定角度偏转的车辆。车辆在停靠路径上行驶时处于低速状态，此时将车辆的后轴中点作为参考点，可以认为4 个车轮无侧偏地绕同一瞬时圆心做圆周运动[3]。

四轮转向车辆的运动学模型如式（1）、式（2）、式（3）。式中：xr、yr、vr分别为车辆后轴中心的横坐标、纵坐标、速度；φ 为车头朝向与x 轴方向夹的角（即方位角）；δf为前轮朝向与车身方向的夹角；δr为后轮朝向与车身方向的夹角；L 为车辆的前后轴距。

车辆运动学模型如图2。

图2 车辆运动学模型

图2（a）为前轮转向车辆的运动学模型，图2（b）为四轮转向车辆的运动学模型，二者区别在于是否存在角δr。Q 为车辆运动的瞬时圆心；A 为车辆前轴中点；B 为后轮后轴中点；QB 为车辆运动的瞬时转弯半径。通过比较QB 长度可以得出，四轮转向车辆相比于前轮转向车辆能够拥有更小的转弯半径；即四轮转向车辆能够拥有更灵活的转向特性，能够更好地满足狭窄地段的停靠需求。

1.2 车身前后方对路径规划的影响

车身前后方对路径规划的影响如图3。

图3 车身前后方对路径规划的影响

在露天矿区，用于运输物料的作业车辆在行驶到装（卸）载区时，首先需要以特定的车头朝向停靠到指定位置。为表述方便，将车辆的位置坐标（x，y）和车头朝向角度φ 合称为车辆位姿。当普通的运输作业车辆在较小区域行驶时，由于停靠位姿对路径生成的限制，车辆可能需要多次换挡才能驶入指定位置（图3（a）），有时甚至无法得到1 条合适的路径（图3（b））。

对于某些特殊的无人驾驶运输车辆（如取消了驾驶室的无人驾驶车辆）来讲，其在停靠点处执行装卸任务时，车头朝向与停靠点处期望的车头朝向相同或相反并不影响其作业动作和作业效率。因此，算法将车辆考虑为没有前、后方之分的车辆，规划车辆的停靠路径时，不必区分车辆正向驶入还是逆向驶入停靠点。此方式在图3（a）和图3（b）的情形下，规划得到的路径如图3（c）。

1.3 算法适用场景与特点

规划算法基于3 条假设：①将车辆在道路上的行驶视为二维平面内的运动，该条假设使算法目标输出的形式是二维平面内的路径点的集合；②将车辆考虑为4 个轮可同时转向的车辆，在此条件下，车辆的转弯半径相比于前轮转向车辆更小；③无视车身的前后方向，该条假设使得算法在设计时无需考虑车辆是正向驶入停靠点还是逆向驶入停靠点。

在露天矿区，由于装（卸）载点的位置随着工程进度一直更新，因此无人驾驶运输车辆每次都需要以新的目标位姿进行停靠路径规划。露天矿区的作业车辆体积巨大、行进缓慢（一般不超过30 km/h）；露天矿区的地势复杂，随着车辆位置更新，车辆可行驶区域可能会变小。以上都为无人驾驶车辆的停靠路径规划算法提出了较高的要求。

提出的算法具有以下特点：①可以根据行驶时任意时刻的当前位姿与停靠目标位姿，规划得到不同的停靠路径，即停靠路径生成具有实时性；②停靠路径搜索、后端路径优化时始终考虑道路边界对行驶路径的限制，保证了行车的安全性；③可根据实际需求定义代价函数，算法会以此代价函数对搜索得到的多个备选路径进行筛选，选择1 条使得代价函数值最低的路径，即路径筛选具有最优性；④候选路径经过优化后，输出的路径满足车辆行驶过程中对路径的曲率要求，保证了行车的平稳性。

2 算法实现

算法根据车辆的当前位姿信息和目标位姿信息，规划出1 条满足运动学约束的停靠路径。算法由前端的停靠路径搜索和后端的路径优化组成：前端使用Hybrid A＊算法得到1 条连接起、终点并与边界无碰撞的路径；后端的路径优化部分则在前端生成路径的基础上，考虑变量的上下界数值约束、运动学曲率约束等，通过数值优化方式得到最终的停靠路径[4-9]。算法实现流程图如图4。

图4 算法实现流程图

2.1 停靠路径搜索

传统A＊算法首先将地图进行栅格化，使用栅格中心作为遍历的节点。将起、终点直线距离定义为启发函数，在启发函数的引导下扩展节点。将节点深度定义为代价函数，保留代价函数值最小的节点。算法最后输出1 条可以避开障碍物的全局最优路径。该算法仅仅保证了起点与终点的连通性，并不保证车辆实际可行。Hybrid A＊算法则考虑了车辆的运动学约束，并在传统A＊算法上改进。

2.1.1 节点扩展方式

传统A＊算法在进行节点扩展时只访问栅格化地图中每个栅格的中心点。而Hybrid A＊算法在扩展节点时考虑了车辆的行驶特性，因此引入了Reeds－Shepp 曲线的基本概念，根据车辆当前的位置和朝向状态，把向下一位置和朝向的转变过程限制为6种基本动作之一：①直行前进（S＋）；②直行后退（S－）；③左转前进（L＋）；④左转后退（L－）；⑤右转前进（R＋）；⑥右转后退（R－）。由于研究假设车辆无前后之分，因此将“L＋”、“R＋”、“S＋”中的“＋”定义为当前行驶方向与初始行驶方向的角度差值小于90°，将“L－”、“R－”、“S－”中的“－”定义为当前行驶方向与初始行驶方向的角度差值大于90°。算法节点扩展方式如图5。

图5 算法节点扩展方式

2.1.2 启发函数设置

Hybrid A＊算法将启发函数分为2 种，无障碍物的车辆非完整性约束启发函数和有障碍物的完整性约束启发函数。第1 种启发函数的值为当前点（x，y，θ）使用Reeds－Shepp 曲线连接目标点（xg，yg，θg）得到最优曲线的代价函数值（代价函数见式（3）），该启发函数能够修剪搜索树的分支，保证规划路径方向与目标方向一致；第2 种启发函数的值是使用Dijkstra算法计算目标点到起始点的最近距离，该启发函数能引导搜索树在生成过程中减少对U 型障碍区域的探索。Hybrid A＊算法最后取上述2 个启发函数中的最大值估计当前节点到目标节点的代价。

理论上，只要存在连接起点位姿和终点位姿的无碰撞路径，就一定存在连接起点位姿和终点位姿的无碰撞的Reeds－Shepp 曲线。然而，假设每次搜索路径都使用Reeds－Shepp 曲线，则搜索树的规模将快速增加，这将造成极大的计算量。因此在扩展节点时，令第1 种启发函数在当前节点离目标点直线距离大于80 m 时的值为0，来减少启发函数带来的计算量。

2.1.3 代价函数设置

代价函数一方面用于计算启发函数值，另一方面用于对生成的多个路径进行代价计算以便进行筛选。采用的代价函数公式如下：

式中：为表示当前路径；J（C）为路径C 的代价值；m 为基本动作数量；wi为路径C 的第i 个基本动作的惩罚权重；Si为第i 个基本动作的路径长度。

对车辆不同的基本动作设置了不同的惩罚权重，当车辆转弯或当前行驶方向与第1 个基本动作行驶方向相反时，该基本动作的惩罚权重较大。设置6 种基本动作的惩罚权重为：S+的惩罚权重为0；L+的惩罚权重为10；R+的惩罚权重为10；S-的惩罚权重为5；L-的惩罚权重为15；R-的惩罚权重为15。

2.2 后端路径优化

由于Hybrid A＊算法采用离散栅格地图生成车辆停靠路径，而栅格地图使得车辆停靠路径曲率变化率过大，导致下层控制算法难以跟随，因此需要在后端对停靠路径进行进一步优化。考虑到对算法的实时性要求，优化过程采用序列二次优化（SQP），路径优化模型的目标函数如下：

式中：G1、G2、G3分别为平滑度代价、长度代价、与参考路径之间的偏差量代价。

式中：n 为路径点的个数；xi、yi为优化后第i 个路径点的横、纵坐标；xi,ref、yi,ref为优化后第i 个路径点在原参考路径上相对应路径点的横、纵坐标。

该优化模型的约束条件如下：

式中：ei,x,lower、ei,x,upper为优化后第i 个路径点横坐标相较于原参考路径对应点横坐标与x 轴方向相反或相同的最大允许偏差的绝对值；ei,y,lower、ei,y,upper为优化后第i 个路径点纵坐标相较于原参考路径对应点纵坐标与y 轴方向相反或相同的最大允许偏差的绝对值；ri为曲率的松弛变量；ki为当前点的曲率；ΔS为相邻路径点之间的路径长度。

式（7）、式（8）表示路径点在笛卡尔坐标系下的横坐标范围约束、纵坐标范围约束；式（1）、式（10）表示关于曲率的松弛变量约束、路径曲率约束。

在二次优化算法的框架下求解路径优化问题，要求约束条件必须是线性的。因此，需要将上述非线性约束式（10）线性化表示，采用泰勒公式展开保留一阶项的方式进行线性化。将路径点的坐标用X（x,y）表示，将式（10）不等号左边的平方和记为F（X），在Xref点处进行泰勒展开：

线性化时仅保留一阶项：

而F（X）=Xref在 处的值为：

将式（3）、式（4）代入约束条件式（10），得到：

式（5）即可替换约束条件（10），作为二次优化模型的近似的曲率约束条件。

3 测试验证

仿真测试使用的CPU 是Intel（R）Core（TM）i5－10210U，测试环境为Ubuntu20.04，边界信息来自某矿区真实地图数据。仿真测试分为2 部分：①验证了无视车身前后时，算法生成的路径比传统算法得到的路径长度更短；②分验证了算法对4 种停靠场景下生成的路径进行后端优化的功能。

3.1 路径生成测试

算法前端生成路径效果如图6。

图6 算法前端生成路径效果

图6 中以矩形框表示车辆，其内部箭头指示了车辆在初始位置A 处的航向角所决定的车身前方，矩形框外部的箭头指示了车辆当前的行进方向。传统方法下，车辆从A 处位姿变换到B 处位姿会采取路径APB；将车辆考虑为无前后之分的四轮转向车辆时使用本算法，车辆会采取路径AQB。

图6（a）中，路径AQB 无视车辆的前后之分，直接驶入B 处，比路径APB 长度缩短了26.5%。图6（b）中，路径AQB 无视车辆的前后之分，一次切换倒挡后驶入B 处，比路径APB 长度缩短了9.2%。

3.2 后端优化测试

后端优化测试的4 种场景分别为：直接驶入场景、一次倒挡停靠场景、平行停靠场景、狭窄区域停靠场景。车辆在初始路径上行驶到A 点时，根据当前位姿和目标点B 的停靠位姿，解算得到优化前后的路径。

绘制上述4 种场景下优化前路径、优化后路径的曲率变化曲线（图略）；整理优化前后的曲率变化范围、路径长度可以看出，4 种场景下，后端优化后的曲线曲率绝对值最大值均比优化前更小，即路径更平滑。4 种场景路径曲率绝对值最大值对比见表1。

表1 4 种场景路径曲率绝对值最大值对比

可以看出，4 种场景下，后端优化后的曲线曲率绝对值最大值均比优化前更小，即路径更平滑。

4 结语

以露天矿区四轮转向车辆为研究对象，提出了无人驾驶车辆的1 种停靠路径规划算法。算法能够根据车辆当前的位姿信息、目标位姿信息，经过停靠路径搜索和后端路径优化，规划得到可行路径。最后，通过露天矿区实际作业现场的4 种典型场景对算法进行了验证，证明此算法可为露天矿区无人驾驶四轮转向车辆提供一种更为高效的停泊路径规划方案。