考虑车辆运动约束的最优避障轨迹规划算法*

2021-05-12宋学伟高振海

汽车工程 2021年4期

杨彬，宋学伟，高振海

（吉林大学，汽车仿真与控制国家重点实验室，长春 130022）

前言

对于无人驾驶车辆而言，在工作空间中寻找出一条从初始状态到目标状态的无碰撞轨迹是车辆自主行进的重要环节［1］。近年来，国内外专家学者针对轨迹规划问题进行了广泛研究［2-3］。然而，受到车辆状态、环境感知、定位技术的限制，很难找到一种计算效率高、实时性好、满足汽车运动约束的轨迹规划方法对车辆的可通行路径进行规划和预测。

在各类轨迹规划算法中，图解法［4-5］通常能够较好地解决复杂工作空间中的避障轨迹规划问题，但其生成的轨迹通常不满足车辆的运动约束，且计算效率受占位格密度和搜索空间维度的制约，很难用于车辆行驶过程的在线轨迹规划。采样法［6-7］在复杂的搜索空间中进行大量采样，能够快速寻找出工作空间中的可通行路径。然而，由于采样点不可能覆盖全部规划空间，采样法寻找最优或次优路径的能力较差；随机性强，相同搜索空间中可能会得到完全不同的结果。插值法［8-9］能够在可行性、舒适性、车辆动力学的约束下对路径进行描述，但容易忽视轨迹规划过程中交通环境变化的问题，应对动态的障碍物干扰的能力较差，常用于轨迹的平滑和优化环节中。

状态格（State Lattice）是一种基于图和采样的路径规划方法，在无人驾驶车辆的避障规划问题上得到了大量应用，并在DARPA 城市挑战赛中展现出较好的效果［10］。文献［11］中通过离线表格生成初始化轨迹，并在线根据行为规划层输出的目标位置对离线轨迹进行优化，但具有大分辨率和大分支因子的状态格集合会导致计算效率低下。文献［12］中引入了时空搜索图的概念，避免了动态环境下搜索空间引入时间维度后导致的状态格数量爆炸的情况。文献［13］中将搜索空间映射到低维度的变形空间中，并利用启发式搜索的方式寻找可通行路径，虽然对状态格进行了精简，但启发式搜索的计算复杂度仍然较高，并不能从根本上降低状态格算法的计算复杂度。文献［14］中将多分辨率状态格填充搜索空间，使状态格的分辨率完备，提升规划效果的最优性，并有效提升规划速度。该方法在复杂全局地图中的轨迹搜索问题中有较好的表现，但在局部动态轨迹规划问题上略显复杂。文献［15］中提出了基于衍生状态格的避障轨迹规划方法，考虑障碍物状态和状态格成本，但衍生状态格的选择较为单一，很难接近最优解，同时在规划过程中只考虑了道路曲率及轨迹长度，未能很好地考虑车辆的运动状态。

综合考虑多种轨迹规划方法的优缺点，本文结合图解法、采样法和插值法提出了一种考虑车辆运动约束的避障轨迹规划方法。在车辆的局部环境地图中衍生出车辆可安全通行的终端状态区域。并在衍生状态区域中对超越障碍物的期望位置进行采样，将复杂道路环境中的避障轨迹搜索问题转换为自车与衍生状态区域内离散终端状态点之间的轨迹规划和寻优问题。车辆当前位置与终端状态点集之间通过基于车辆动力学模型的Bézier 曲线规划器进行轨迹拟合，保证了局部轨迹是满足车辆运动特性的。针对车辆在轨迹跟随过程的操纵稳定性和行驶平顺性设计轨迹评价函数，并最终在避障轨迹簇中筛选出局部最优轨迹。通过与State Lattice 算法和MPC 算法进行对比，验证了该方法的操纵稳定性和平顺性。

1 车辆运动约束下的避障轨迹规划算法

状态格方法具有图解法和采样法的优势，能够应对动态环境的变化，同时在计算负载上远小于图解法。然而，状态格方法一般在车辆前进方向的一段结构化搜索空间中进行大范围、大间隔的均匀采样，导致规划得到路径很难得到最优或次优解，同时轨迹簇中存在许多冗余的无效解，产生大量不必要的运算，严重影响着算法的计算效率。同时，避障轨迹多以多项式或圆弧段进行拟合，很难根据搜索空间的环境变化和车辆运动变化对轨迹段进行局部调整。本文针对上述问题提出了如下改进和优化方案。

（1）车辆实际行驶过程中，最优终端状态点（如图1 中红色点所示）通常位于障碍物的侧方区域。因此调整状态点的搜索空间至理想终端状态点附近（State Lattice 采样方式如图1 所示，本文采样方式如图2 所示），将搜索空间压缩至障碍物位置和运动状态推算出的衍生状态区域（图2 中黑色框所示）。在初始状态点与终端状态点集之间以车辆动力学上可行的曲线进行拟合，减少轨迹的冗余计算，并使结果更加靠近最优解或次优解。

（2）相比于圆弧或多项式的曲线拟合方法，Bézier 曲线可通过调整轨迹控制点实现曲线的快速局部修改。依靠车辆起始状态点位置及终端状态点位置进行轨迹控制点的初步选择，后根据搜索空间的环境变化和车辆的运动特性进行轨迹局部适应性的调整及轨迹筛选，可大幅提升避障轨迹的灵活性和环境适应性。同时，Bézier 曲线规划器在进行曲线拟合时，只需提供起始点的位置和航向，通过改变中间控制点位置即可进行避障安全性、车辆运动约束等调整，间接减小了状态格的分支数量，降低了计算复杂度。

图1 状态格采样示意图

图2 本文算法采样示意图

本文的技术路线如图3 所示。以车辆开始执行避障操作的位姿作为初始状态点，车辆完成障碍物超越的期望位姿作为终端状态点。局部避障轨迹在初始状态点与终端状态点之间通过基于车辆运动模型的曲线规划器拟合后得到。在车辆的局部环境地图中，以障碍物的位姿为出发点衍生出车辆可安全通行的终端状态区域。在该区域内进行终端状态点的采样并组成离散终端状态点集。状态点集中的每一个终端状态点通过Bézier 曲线规划器的轨迹拟合和参数修正后得到一条对应的满足车辆运动约束的避障轨迹。最终组成一组车辆可安全平稳通行的避障轨迹簇。考虑车辆在跟随避障轨迹时的操纵稳定性和行驶平顺性，对轨迹簇中的轨迹进行客观评价，选择一条符合条件的最优轨迹作为最终的局部最优避障轨迹。

图3 本文算法技术路线

2 避障轨迹规划器设计

避障轨迹在保证能够安全通行的前提下，不仅需要考虑车辆的运动特性以保证轨迹可循，而且需要满足一定的曲率约束和连续性条件，使车辆在循迹过程保持较好的操纵稳定性和行驶平顺性。

2.1 车辆动力学模型

在车辆路径规划算法中考虑车辆运动约束，可以保证规划出的路径是车辆可实际跟随的，使车辆在自动驾驶的过程中能够更精准地跟随规划的路径。同时，在轨迹规划器中应用车辆动力学模型，对局部轨迹进行筛选，可控制无人驾驶车辆维持较小的侧向加速度和横摆角速度的变化幅度。

车辆在地面运动的动力学过程非常复杂，而无人驾驶车辆的规划层中引入过于复杂的模型是不必要的。因此在运动学假设下引入简化的车辆单轨模型［16］，如图4 所示。其中，［X，Y］为车辆在大地坐标系下的坐标；v为车辆质心的中心速度，φ为车辆的航向角；φ̇为车辆的横摆角速度。在速度控制上，本文考虑恒定的纵向速度，因此纵向加速度为0。

图4 车辆动力学模型

假设车辆完全按照规划轨迹段行驶，根据阿克曼转向规律，可将车辆转向控制量与轨迹段参数进行联系。

在车辆稳态转向和小角度轮胎侧偏角的假设下，对车辆的运动方程进行推导，由车轮转向引起的横摆角速度和侧向加速度如式（2）和式（3）所示。

通过式（2）和式（3）可由轨迹参数进行航迹点上的侧向加速度和横摆角速度的估算，用以在轨迹参数化的过程中进行轨迹的车辆运动约束和评价。

2.2 离散终端状态点集

避障轨迹可通过初始状态点和终端状态点的参数化拟合后得到。初始状态点位于车辆开始执行避障操作处，终端状态点则位于障碍物与行驶环境边界之间，方向与路径点处的道路曲率相关。障碍物与行驶环境边界之间的可通行区域称为衍生状态区域，在衍生状态区域中完成采样的多个可行的终端状态点组成离散终端状态点集。衍生状态区域将终端状态点的搜索空间压缩为与障碍物推演位置和车辆可行驶区域相关的局部区域中，如图5 所示。由于衍生状态区域的生成考虑了自车与障碍物和道路边界的安全距离，几乎可以保证自车行驶至衍生状态区域内的终端状态点的过程中不会与障碍物发生碰撞，因此避障轨迹在初始状态点与离散终端状态点集之间进行拟合。

图5 大地坐标系下衍生状态区域示意图

终端状态点在衍生状态区域中进行均匀采样。衍生状态区域位于障碍物矩形块和可行驶区域边界之间，其边界与障碍物的侧向间距（D′obs）、与可行驶区域边界的侧向间距（D′lane）均须大于车辆行驶的临界安全车距［17］。大地坐标系下衍生状态区域示意图如图5 所示。为简化轨迹搜索和求解的难度，将大地坐标系下衍生状态区域、障碍物与车辆的相对位置转换至Frenét框架下：

式中[Dlane，Dobs，L]为Frenét框架下的相对位置关系。Frenét 框架下，衍生状态区域的纵向位置位于障碍物运动方向的侧向，受交通规则的限制，在障碍物左侧可以得到至多1个衍生状态区域，如图6所示。若侧向间距不满足侧向安全距离的约束，则排除掉该衍生状态区域，认为当前环境无法满足避障需求，此时不会进行避障轨迹的规划。

本文的研究重点在于避障轨迹的生成，在此假设障碍物的运动状态可预测，且t时间后的障碍物位置[xobs_t，yobs_t]和航向φt可知。假设车辆的行驶区域为车道宽度为d的双车道结构化道路，在检测到前方路径上存在障碍物后，开始进行障碍物规避行为，且驶过L距离后完成轨迹偏移并到达衍生状态区域。衍生状态区域的位置会随着障碍物的移动而发生变化，但相对于障碍物的航向保持平行，与车辆前进方向保持一致，并满足安全车距的约束。

图6 Frenét框架下衍生状态区域示意图

在Frenét 框架下，衍生状态区域中心的纵向位置ystate由障碍物完成轨迹偏移时的行驶距离L确定，范围在［0，Ldom］之间；侧向衍生状态区域由障碍物完成轨迹偏移的侧向位置xobst与侧向安全车距确定。得到衍生状态区域的坐标范围：

在衍生状态区域内，假设车辆的瞬时转向半径与道路曲率半径相同，终端状态点的航向可由该位置的道路切线方向近似计算得到。假设道路轨迹以函数Y=f(X)表示，可得到终端状态点的航向为

通过起始避障位置和终端状态点拟合出的轨迹不仅应满足车辆的约束，还应有较小的侧向加速度、侧向急动度、横摆角速度以保证车辆在转向过程中保持良好的操纵稳定性和行驶平顺性。定义成本函数如下：

式中：κ*max为标准归一化的路径段的最大曲率；为标准归一化的最大侧向急动度；为标准归一化的最大横摆角速度；ωκ、ωJ和ωφ为权重系数。成本函数越小则表征路径段更满足车辆的运动性质，车辆能够更为平稳快速地对路径段进行跟踪。

在轨迹规划起点和终端状态点之间的轨迹拟合的过程中，为保证路径段对于车辆运动特性而言是可实现的，需要对路径段的曲率和横摆角速度进行严格约束，因此在成本函数中曲率和横摆角速度具有更大的权重。在轨迹簇的筛选过程中，可能需要对驾驶员需求进行车辆的操控稳定性和行驶平顺性的个性化调节，因此在这段过程中，成本函数中侧向急动度和横摆角速度具有更大的权重。

2.3 轨迹参数化

以通过自车的当前位姿和衍生状态区域内的位姿，利用Bézier 曲线可规划出无穷多条行驶轨迹，需要在其中唯一确定一条满足车辆运动约束且综合评价最佳的曲线。

对于实际的无人驾驶车辆系统来说，轨迹规划需要保证其规划出来的轨迹满足运动学约束和执行机构约束。以多项式进行轨迹规划时，曲线拟合规律不好把握，较难进行车辆运动状态的约束，在部分情况下多项式拟合无法得到解析解。通过Bézier 曲线进行拟合则能够较好地解决上述问题，不仅在拟合区间上所有的连续函数都可以用多项式来逼近，且一致收敛；同时，Bézier 曲线在拟合过程中，可以通过很少的控制点，去生成复杂的平滑曲线。

Bézier 曲线的计算成本随阶数的增加而增加，为提升避障轨迹的计算效率，选用可生成连续曲率轨迹的最小阶数曲线：3 阶Bézier 曲线［18］进行起始点至终点之间的轨迹拟合。由平面上4 个控制点P0（x0，y0）、P1（x1，y1）、P2（x2，y2）、P3（x3，y3）可唯一确定1 条3 阶Bézier 曲线。Bézier 曲线的参数化表达式为

式中：0 ≤t≤1；Bi，3(t)为Bernstein基函数。

曲线上任意点的切矢量和2阶导矢量为［19］

由曲率公式可得到轨迹端点的曲率为

为使避障轨迹与参考轨迹能够平顺连接，仅保证轨迹之间的连续性是不够的，还需要尽量保证拟合轨迹段与直线路段之间的连接处保持切矢量的一致和曲率的连续。直线路段上，任意位置的曲线切矢量和曲率均为0。同样的，在拟合轨迹段与直线路段之间的连接处，拟合曲线的起点和终端处的切矢量和曲率需趋近于0：

拟合轨迹段需要满足其与直线路段的连接处的切矢量为0。由式（4）～式（6）可得到式（11）所示的控制点间关系。当y0=y1且y2=y3，即控制点y1、y2分别在y0、y3所在的直线路段方向上时，能够保证规划路径段在起点、终点处的切矢量连续。

在满足切矢量连续的前提下，将式（4）～式（7）带入式（8）中可得到：

可以看出，3阶Bézier 曲线规划器拟合的轨迹段不能保证起点和终点处曲率为0。相应的，在车辆循迹过程中，3阶Bézier 曲线在起始端和终端将会产生不连续的侧向加速度和急动度变化，即曲线起始端和终端处的车辆侧向加速度和急动度不为0。且|和|越大，起点和终点的曲率越趋近于0。由于控制点之间存在约束，在确定了控制点P0和P3的位置后，通过对e1、e2进行合理的赋值，即可唯一确定一条拟合轨迹。

2.3.1 曲率连续约束

在进行避障轨迹规划时，希望整条路径上的航迹点在避免碰撞的同时，都能够平稳满足曲率连续性和最大曲率约束，使曲线在端点处与车辆的航向一致保证曲率矢量的连续。

为保证曲率矢量连续，需要保持控制点P1、P2分别选择在车辆当前位置和规划终点位置的航向上。3 阶Bézier 曲线控制点及控制参数如图7 所示，其中P0为初始状态点位置，P3为终端状态点位置，为保证规划的3 阶Bézier 曲线在原始路径的连接处保持1阶几何连续，选取P1、P2时需要保证为车辆的航向为该位置处的轨迹切线方向。e1、e2分别为P1、P2在纵向、侧向距P0、P3的比例系数，即可通过确定e1、e2的大小和P0、P3的位置得到P1、P2的位置：

图7 Bézier曲线控制点及相关参数示意图

通过改变e1、e2可直接对曲线进行局部调整和控制。以固定e2值时e1的变化对曲线的影响为例进行说明，如图8 所示。随着e1的逐渐变大，轨迹段初始区域处的曲率将产生更加平滑的过渡效果，而轨迹终端区域则未受到明显影响。因此在状态格生成的过程中，不需要在起始控制点和终端控制点之间额外增加更多的分支。通过对e1、e2的控制和调整对传统状态格的分支进行代替，以减小算法的计算成本。

图8 通过改变控制点位置可实现曲线的局部调整

2.3.2 车辆运动学约束

受车辆转向半径的约束，须保证拟合的曲线上任意点曲率不小于车辆当前速度下的最小转向半径，在Bézier曲线上的任意点须满足：

2.3.3 车辆动力学约束

为防止车辆在转向过程中发生侧滑或侧倾，提升避让路径的舒适性，须选择适当的参数对侧向加速度、横摆角速度和侧向急动度进行约束［20］。

其中，车辆侧向加速度和横摆角速度由车辆动力学模型估算得到，而侧向急动度通过轨迹段上航迹点之间的侧向加速度变化率计算获得。

3 仿真试验与结果分析

本节中的结果是在对比仿真试验中获得的。在高速公路驾驶和城市驾驶两种试验场景下（如图9所示），通过比较本文提出方法与State Lattice 算法和MPC 算法的避障效果，对避障算法规划出的轨迹进行对比评价［21］。

图9 仿真场景

测试场景1 为双车道高速公路场景，测试车辆以80 km/h的速度行驶并接近单一障碍物，测试车辆须及时进行障碍物规避动作；测试场景2 为双车道城市驾驶场景，测试车辆以50 km/h 的速度行驶，在测试车辆的当前车道和相邻车道上均有障碍物阻碍车辆的行驶，要求测试车辆能够同时避让多个障碍物。仿真结果如图10～图15 所示，分别显示了不同算法在行进距离上的侧向位移、侧向加速度和侧向急动度的变化规律。

图10～图12 为测试场景1 中不同规划路径方法的仿真结果对比，包括3 种路径规划方法生成的避障轨迹、轨迹段上侧向加速度和侧向急动度的变化规律。单一障碍物下，3 种规划方法生成的轨迹虽较为接近，但也存在可辨别的差异。MPC 算法在完成避障后有返回原行驶路线的趋势，State Lattice 算法与本文算法类似，在完成避障后保持航向在当前车道方向上。本文算法较另两种方法更靠近障碍物，但避障过程更为平缓。3 种方法在避障过程中产生的侧向冲击则清晰可辨，本文中提出的避障轨迹规划器生成的路径较其他方法具有更低的侧向加速度，侧向急动度也控制在更为稳定且更小的水平。

在侧向急动度的表现上，State Lattice 算法不仅会在避障起始和终止位置出现突变，而且具有较大的侧向急动度峰值；MPC 算法在避障开始时刻会出现急动度小幅的震荡，但随后避障过程的表现良好，急动度变化平滑且保持在更小的阈值内；相比而言，本文算法具有更好侧向急动度变化趋势，避障过程的侧向冲击变化平稳，且峰值仅在避障轨迹中段出现，在其余避障轨迹段均维持在较小的变化范围内。

如前文所述，由于3 阶Bézier 曲线为2 阶几何连续，至多能保证曲线段间的曲率连续性，无法处理曲线3 阶导数连续的情况。而在轨迹段上航迹点处侧向加速度和急动度的估算中，分别存在曲线3 阶导数和4 阶导数的计算环节。因此在轨迹段的起始端和终端无法保证侧向加速度和侧向急动度连续变化。虽然本文算法生成的路径在起始点处的侧向加速度和急动度不为0，但初始值较小。其在变化规律上更为平滑和稳定，且始终保持在更小的侧向加速度范围内，以更小的侧向冲击实现低侧向加速度，整体表现更为稳定和平滑。

图10 场景1（高速公路驾驶）规划路径仿真对比

图11 场景1侧向加速度对比结果

图12 场景1侧向急动度对比结果

图13～图15中针对测试场景2，进行了不同规划路径方法的仿真结果对比。3 种方法之间存在明显的差异，本文避障轨迹规划器生成的轨迹在应对多障碍物的情况下，同样有更好的表现。State Lattice算法受曲率段之间的道路对齐约束，规避多障碍物时的轨迹路段之间过渡生涩，路段中的侧向冲击变化剧烈，存在较大的侧向冲击峰值。MPC 算法则有更好的表现，轨迹更为平顺，侧向冲击变化平稳。然而MPC 算法同样会在避障行为的开始阶段出现急动度的震荡。本文算法生成的避障轨迹过渡平滑，在侧向冲击的表现上也有更好的表现，侧向加速度、急动度均保持在较小的水平上，且变化平稳。同时，本文算法在完成多障碍的规避后，能够较好地返回初始轨迹线，更适合具有全局参考轨迹的无人驾驶车辆局部避障轨迹规划问题。

在仿真过程中，虽然早两段避障曲线的连接处不可避免地出现了侧向急动度突变的现象，但急动度在两段避障轨迹段的连接处数据跳动极小，不足0.7 m/s3，对车辆的操稳性产生的影响十分有限，因此不需要更高阶次的Bézier 曲线规划器进行轨迹拟合。同时，急动度在避障起始和终点均接近0，避障路径与初始路径的连接性能有较好的保证。本文算法不仅具备处理复杂场景的能力，同时能够保证规划方案有良好的寻优能力。