基于混合A*算法的无人驾驶矿用卡车路径优化研究

邓穆坤，刘 勇，黄佳德，罗 羽

（株洲中车时代电气股份有限公司，湖南 株洲 412001）

0 引言

无人驾驶矿用卡车（简称“矿卡”）能够将驾驶员从繁琐的车辆驾驶操作中解放出来，对提高车辆的安全性和矿区的运输效率有着重要作用［1］。矿区的非结构化道路在短时间内可行驶区域相对比较固定，且矿区是封闭场景，环境相对静态，干扰因素少，“装、运、卸”典型作业过程线路相对固定，为矿卡无人驾驶的实现提供了有利条件。路径规划是无人驾驶矿卡实现行驶的第一步。适合矿区非结构化场景的路径规划算法主要有基于图搜索和采样的方法，基于图搜索的算法相对基于采样的方法求解更稳定，能够得到最优解［2］。目前，基于图搜索的路径规划算法有State Lattice算法［3-4］、Dijkstra算法［5］、A*算法［6］、D*算法［7］和混合A*算法［8］等，其中State Lattice和Dijkstra算法相较于具有启发函数的A*类算法更加耗时。A*类算法中，显然混合A*算法考虑了车辆运动学，求解出的路径较其他方法更为平滑，更加符合矿区封闭场景的规划要求；但还是容易产生突然的转向或者尖角，不符合车辆的运动学规律，也容易给人带来不舒适的乘车体验甚至伤害［9］，因此需要后期通过基于插值、特定曲线或优化的方法对路径进行平滑，来得到一条符合车辆运动学、舒适的行驶轨迹［10］。混合A*算法采用RS（reeds-shepp）曲线［11］进行终点拟合，而RS曲线曲率存在阶跃式的突变，不适合车辆的控制跟踪。为此，很多学者提出了路径后处理方法来平滑路径，如共轭梯度算法［8］、梯度下降方法［12］，这些方法虽然可以在一定程度上平滑路径，但是存在内缩、折线等算法退化问题［13］；且车辆控制需要密集的路径点，点数增加时此种方法计算耗时较大。对此，本文在传统混合A*算法基础上进行改进，首先用曲率连续的RS（continuous curvature reeds-shepp，CCRS）曲线［14］代替曲率不连续的传统RS曲线；同时，结合矿区使用场景进行筛选排序，以提高路径初始解的质量，并在初始解的基础上，采用分段样条曲线和二次规划相结合的方法对初始解节点拓展段进行平滑优化；得到最终优化拟合样条曲线方程后，与CCRS曲线进行拼接，即可生成起点到终点的路径。

1 CCRS曲线及其筛选

基于网格化搜索的传统混合A*算法无法精确地搜索到终点位置，须采用RS曲线拟合终点位置。对RS曲线路径，采用圆弧、直线等元素相结合方式，拟合起始点到终点的最短路径，这一方法没有考虑求解的路径在不同元素结合处的曲率突变。为此，本文采用CCRS曲线，其在圆弧前后添加回旋曲线进行曲率过渡。另外，由于RS曲线路径组合形式多种多样，因此需要结合其规律进行筛选，以符合矿区无人驾驶矿卡的路径规划要求。

1.1 CCRS曲线

为解决RS曲线曲率突变问题，本文通过在圆弧前后增加回旋曲线来实现RS曲线曲率的连续。

1.1.1 基于自行车模型的状态空间表示

为便于后续对CCRS曲线进行状态描述，首先对基于自行车模型的状态空间表示进行介绍。车辆在低速情况下，可用自行车模型表示其运动状态。其在平面中的状态可表示为(x，y，θ，k)，其中(x，y)为车辆后轴中心位置，θ为车辆的横摆角，k为车辆当前位置的曲率，车辆运动模型可表示为［15］：

式中：̇——车辆状态关于路径弧长s的导数；σ——曲率的变化率；d——车辆行驶方向，d=［-1，1］，d=1表示前进，d=-1表示后退。

除路径尖点处曲率变化率为无穷大之外，曲率变化率σ应在最大曲率变化率范围之内：

1.1.2 回旋曲线曲率过渡

回旋曲线［16］曲率随弧长呈线性变化。在RS曲线圆弧段的前后加入回旋曲线，可改善圆弧曲率阶跃不连续的情况。回旋曲线弧长s处的曲率为

如图1所示，假设车辆的初始状态qs为(0，0，0，0)，首先通过曲率变化率为σmax的回旋曲线连接过渡到中间状态qi，然后通过一段圆弧过渡到qj，最后通过曲率变化率为-σmax的回旋曲线过渡到目标状态qg。

图1 CCRS曲线Fig.1 CCRS curve

qi可通过式（4）进行计算：

式中：Cf，Sf——菲涅尔函数；kmax——车辆可达到的最大曲率；t——车辆左右拐，t=［-1，1］，t=-1表示右拐，t=1表示左拐。

从qi经过一段圆弧到达qj，圆弧的圆心可由式（5）进行计算：

qj可由式（6）进行计算：

式中：β——qs和qg点车辆横摆角的变化值；βmin——回旋曲线起终点横摆角的变化值，βmin=k2maxσ-1max。

同理，可以通过曲率变化率为-σmax的回旋曲线到达目标点qg。

用改进后曲率连续的圆弧替代传统RS曲线中的圆弧，可改善传统混合A*算法终点拟合曲率不连续的问题。图2示出采用传统RS曲线和采用CCRS曲线规划的掉头路径曲率对比。可以看出，采用CCRS曲线，掉头路径曲率是梯形且连续；而采用RS曲线，掉头路径曲率为矩形但不连续。相比之下，CCRS曲线曲率更优，更利于车辆的行驶跟随。

图2 RS曲线曲率改进前后对比Fig.2 Comparison of RS curvature improvement

1.2 结合矿区场景分层筛选排序

CCRS曲线求解起点到终点的最优路径，组合形式多种多样，需要结合矿区使用场景进行分层筛选排序才能得到合适的解。

1.2.1 分层排序代价设计

如图3所示，CCRS曲线路径分层排序主要分为线形层、叉乘层、路径总长度层、CCRS路径平均曲率层、CCRS后退最大曲率层和CCRS后退段长度6层。通过对CCRS曲线的多种元素组合路径进行研究，发现其中的C|SC（C为圆弧，|为尖点，S为直线）形式的路径最符合矿区入铲和卸载的需求，其后退段通过一定的筛选排序可以将更为平滑的解作为优先路径，以降低入铲和卸载的路径跟随难度。对于任意起点和终点qs（xs，ys，θs，ks）、qg（xg，yg，θg，kg）的CCRS路径，各层代价计算公式为

式中：Jcross——起点与终点方向线的垂直距离；

Jsum_len——路径总长度；Javg_cur——CCRS路径段平均曲率；Jback_max_cur——CCRS后退段最大曲率；Jback_len——CCRS后退段长度；round——四舍五入取整函数；S0——路径起始点累加距离、SA*——A*搜索段路径长度；Sccrs——CCRS曲线总长度；Sb——CCRS曲线后退段长度。

1.2.2 分层排序流程

在离终点一定距离时启动CCRS拟合，得到一定数量的解。按C|SC、CS|C、其余线形路径的优先顺序将所有解分为3个一级子层；每个一级子层中，利用式（7）中的Jcross值对路径进行升序排序，以保证拟合起始点在终点方向线附近优先级最高，Jcross值相等的路径属于同一层，其被分为若干个二级子层；在每一个二级子层中，利用路径总长度Jsum_len进行排序，分为若干个三级子层；在三级子层中，利用CCRS路径平均曲率Javg_cur进行排序，得到四级子层；四级子层中，利用CCRS倒车段路径最大曲率进行排序和分层，得到五级子层；最后，利用倒车路径长度对五级子层中的路径进行升序排序，分层筛选流程如图3所示。

图3 分层筛选流程Fig.3 Flowchart of hierarchical screening

6层排序后，从最小代价路径开始进行碰撞检测，直至得到最优的无碰撞路径并将其作为最终路径。

2 二次规划样条曲线路径平滑

利用CCRS和分层筛选得到路径初始解后，CCRS曲线段由于本身曲率连续而可不再进行平滑处理，A*搜索段的路径点存在曲率不连续、包含不必要的转弯、路点间距较大且不均匀等问题。为此，本文利用数值优化方法对其做进一步的平滑与插值，具体步骤如下：

（1）对混合A*拓展得到的路径节点均匀分段，每一段优化拟合一个五次多项式，分段结合处保证三阶导数连续。

（2）利用基于二分法的可行域隧道，构建所有混合A*节点的前后左右可移动距离矩形约束。优化过程中，每一个路径节点将受到矩形框约束。

（3）基于分段结果和可行域隧道，构建二次规划模型；求解二次规划模型之后，可得到待求的每一分段五次多项式的系数。

（4）基于得到的分段优化拟合的五次多项式，可得到任意点距的路径点；与CCRS路径进行拼接，即可得到从起点到终点的离散路径点，以供实际车辆控制使用。

2.1 路径分段

如图4所示，按每n个点一段对路径进行分段，段与段之间有一个重复点；每一段利用五次多项式进行拟合，在路径点数不能均匀分段的情况下，将剩余路径点均匀分配到各段中：

图4 路径分段示意Fig.4 Schematic diagram of path segmentation

式中：N——总路径点数；Nnum_spline——路径被分割的段数；Rreminder——相除的余数；k——余数的顺序值，k∈[0，Rreminder）；Oorder——路径分段，将第k个余数分配到第Oorder个分段，第Oorder个段点数由n变为(n+1)，Oorder∈[0，Nnum_spline)。

2.2 可行域隧道构建

优化过程中，每一个路径点都是数值优化模型的约束点。将拟合点约束在原始路径点方向上一定大小的矩形框中，矩形框大小通过基于二分法的碰撞检测进行确定，具体流程如图5所示。

图 5二分法可行域隧道构建流程Fig.5 Process of feasible tunnel building based on bisection method

图5中，Pre_Max_boundary为预设的往前后左右（f、b、l、r）任一方向无碰撞的最大距离，Collisionless_Max_boundary为最终得到的某一方向上可移动的无碰撞最大距离，delta为收敛阈值，IsCollsion（（Up_boundary+Down_boundary）/2）为判断路径点移动((Up_boundary+Down_boundary)/2)距离后是否与障碍物碰撞的函数。重复上述流程，得到单个路径点前后左右矩形约束，所有路径点矩形约束构成可行域隧道。

2.3 数值优化模型

路径按点数分段之后，每一段按五次多项式进行拟合。为了便于对路径进行优化，将拟合路径点（x'，y'）表示为路径累加距离s的参数方程［17］：

式中：a0~a5和b0~b5为待求系数。

在路径分段和可行域隧道的基础上，建立数值优化二次规划模型。该模型可表示为

式中：ω——稀疏参数，为防止过拟合的正则化项；H——目标函数实对称系数矩阵；LB——自变量上界矩阵；UB——自变量下界矩阵；AI，bI——等式系数矩阵；Aε，bε——不等式系数矩阵；x——待求多项式系数。

目标函数为分段优化样条曲线的三阶导数平方的积分之和。目标函数最小化，可近似认为是曲线曲率变化率最小化：

二次规划约束主要包括（1）可行域隧道对拟合点的约束；（2）各分段函数相接处函数值、一阶导数、二阶导数及三阶导数的相等约束；（3）首末端点位置、曲率和切向的约束。

可行域隧道构建后，每一个路径点对相应的拟合点进行约束，即拟合点不得超出路径点前后左右的约束距离。如图6所示，假设路径点pr(x，y)相应的拟合点为pf(x'，y')，以路径点方向θ为F轴方向，建立车辆坐标系，分别求出路径点和拟合点到F、L轴的投影lpf、lpr、fpf和fpr，通过投影值，即可建立对拟合点的约束。

图6 车辆坐标投影示意Fig.6 Schematic diagram of vehicle coordinate projection

假设二分法得到的前后左右约束距离为[Sfront，Sback，Sleft，Sright]，则相应的拟合点约束可表示为

式（19）~式（20）中，lpf、lpr、fpf、fpr的计算如下：

任意两分段函数相接处的约束用式（27）表示，其中i、j代表前后相接的两个五次多项式曲线。

假设原始路径首末端点为p0(x0，y0)、pn(xn，yn)，则拟合路径首末端点位置约束如下：

为保证车辆启动和停止时前轮回正，需要使首末端点曲率为0。依据参数方程曲率计算公式，可令首末端点的二阶导数为0，以保证曲率为0，具体如下：

首末端点的切向约束主要用于保证路径符合车辆起点方向和终点期望方向。此时，需要保证首末端点的拟合曲线一阶导数在L轴的投影为0，且首末端点方向向量与其一阶导向量的内积大于0。

模型构建后，可通过OSQP求解器［18］快速求解，得到待求的分段样条曲线系数。

3 试验验证

本节通过仿真和实车试验，验证本文提出的无人驾驶矿卡混合A*路径优化方法的有效性和实用性。首先，采集实际矿区的地图；基于Linux搭建了运行平台，采用C++编写算法，对所提出方法与传统混合A*算法［10］进行了仿真对比实验，以验证算法的优越性。然后，进行实车路径规划试验，进一步验证本文所提方法的可靠性。试验相关参数见表1。

表1 试验相关参数Tab.1 Related experimental parameters

3.1 仿真对比实验

如图7所示，在1 580 m×1 115 m的地图中，对规划路径的平滑优化时间和曲率进行对比实验。

图7 实验矿区地图Fig.7 Map of the experimental mining area

本文所提分段样条曲线数值优化法与传统混合A*离散点梯度下降法［10］的路径平滑时间和路径规划总耗时对比试验结果数据如表2所示。可以看出，本文方法相较于传统混合A*路径离散点平滑算法计算速度平均提高10倍，从而大大提高了路径生成的效率，使算法具备更好的实时性。从总耗时上看，本文方法相较于传统方法速度更快。虽然采用CCRS和分层筛选后，混合A*路径初始解搜索时间有所增加，但得益于平滑耗时的减少，路径总耗时优势明显。

表2 路径规划耗时数据对比Tab.2 Comparison of path planning time-consuming data

此外，还对路径曲率进行了对比，如图8所示。在采集矿区地图上，以同样的起点和终点，利用传统混合A*方法与本文方法规划路径。从图8（a）中可以看出，本文方法规划的路径结合了基于二分法构造的可行域隧道，路径更平直。从图8（b）中可以看出，采用本文方法得到的路径曲率相较于传统混合A*算法的更为平顺，尤其是起点和终点附近，平滑效果更为明显，且起点和终点曲率均为0，更利于车辆的行驶跟随。综上实验结果可以看出，本文方法相较于传统混合A*算法在平滑耗时和路径曲率上更具优越性。

图8 路径结果对比Fig.8 Comparison of path results

3.2 实车试验

为了进一步验证所提出方法的可靠性，本文结合矿区排队、卸载和入铲这3种场景，对本文所提方法与传统混合A*算法进行实车对比试验。本试验所使用的车辆为改制的930E型矿用自卸车，其配备有激光雷达、毫米波雷达以及惯导等感知和定位设备。图9为测试的实际矿车运输场景。

图9 试验场景Fig.9 Test scenarios

图10（a）示出基于矿区地图所规划的一条从卸载点前往装载区排队点的路径。可以看出，采用本文方法和采用传统混合A*方法所规划的路径并不完全重合，存在细微偏差，主要是由于本文方法利用二分法构造了可行域隧道，给了优化更大的空间。从图10（b）可以看出，采用本文方法，曲率更为平滑，不存在曲率超限情况；而采用传统混合A*方法，曲率存在更为明显的波动。前往排队点的路径有1 620 m长，更平滑的曲率可以降低车辆高速行驶时的控制难度，从而提高了车辆运行效率。

图10 前往排队点路径效果对比Fig.10 Comparison of paths to the queuing point

如图11（a）所示，规划一条从装载排队点前往装载点的路径。可以看出，采用本文方法，利用结合分层筛选排序的CCRS曲线得到的路径前进段和后退段均更为平直；而采用传统混合A*算法，前进段存在S形路径，加大了控制跟随难度。由图11（b）可以看出，采用本文方法得到的路径曲率更为平滑；而采用传统混合A*方法，曲率存在更为明显的波动，且前进后退切换处尖点存在曲率突变，这样会导致车辆难以控制跟随。本文方法利用CCRS曲线，从初始解开始就避免了这种曲率不连续的情况，大大提高了求解路径的可靠性。

图11 前往装载点路径效果对比Fig.11 Comparison of paths to the loading point

规划一条从卸载排队点前往卸载点的路径。从图12（a）中可以看出，本文方法和传统混合A*算法所规划的路径形状存在较大区别，其按先掉头然后接近直线的后退路径方式进行卸载，前进段掉头符合矿区右侧卸载的操作规程，后退段接近直线可以降低靠挡墙进行卸载的控制难度，提高了车辆卸载的成功率。从图12（b）中可以看出，本文方法曲率没有大幅度正负的切换，后退曲率也很小，比传统混合A*方法更利于车辆行驶跟随。

图12 前往卸载点路径效果对比Fig.12 Comparison of paths to the unloading point

4 结语

本文对矿区非结构化道路无人驾驶矿卡的混合A*路径规划方法进行了优化研究，提出了一种路径平滑优化方法。首先，其利用CCRS曲线代替传统RS曲线，解决了曲率突变问题；之后，结合矿区使用场景进行分层筛选，得到适合场景的初始路径解；最后，利用结合数值优化的五次样条曲线对路径进行平滑和插值处理，效率较离散点梯度下降法更高。仿真对比实验和实车试验结果表明：采用本文所提出的方法能显著提高对矿区场景的适用性（较传统混合A*算法而言），同时保证了最终生成路径的安全性、平滑性以及矿卡的可行驶性。

为保证求解效率，本文在路径平滑模型中没有显式地约束路径曲率和曲率变化率，而这将是下一步研究的方向。