常用路径规划方法与轨迹规划函数对比

自动驾驶汽车的路径规划算法最早源于机器人的路径规划研究，但是就工况而言却比机器人的路径规划复杂得多，自动驾驶车辆需要考虑车速、道路的附着情况、车辆最小转弯半径、外界天气、行驶环境等因素。

本文将为大家介绍四种常用的路径规划算法，分别是搜索算法、随机采样、曲线插值和人工势场法，以及六种常用的轨迹规划函数。

一.路径规划方法

路径规划首先需要建立规划模型，利用状态空间法描述规划模型是建立非线性优化模型的关键。图搜索算法可以很好地解决该问题，其基本思想是将车辆的初始位姿和目标位姿映射到一个状态空间，然后将状态空间离散化，并将其构成一个图，随后从图中搜索满足约束条件的最优轨迹。目前主流的方法主要包括Voronoi图、栅格地图与代价地图、Lattice状态图、驾驶通道图等。为了兼顾实时性与障碍物约束空间处理能力，一般采用Lattice和通道图方法生成安全轨迹。

随机采样方法是另一类被广泛采用的方法，基本思想是在构型空间中随机采样，并筛选出满足性能需求的最优采样点，具备概率完备性，但其最大的缺点是舒适性较差，且计算效率随着障碍物数量的增长而下降。最常用的方法包括概率路标算法（Probabilistic Road Map，PRM）以及快速搜索随机树算法（Rapidly-exploring Random Trees，RRT）。

为了降低路径规划问题求解难度，确定性采样方法得到了广泛的应用，具体包括多项式参数化模型和样条曲线、螺旋线、回旋曲线、贝塞尔曲线等变种参数化曲线方法。基于多项式参数化模型的规划方法的设计思想是根据车辆的初始状态和目标状态对变道轨迹进行规划，使车辆在指定的时间到达相邻车道。试图在用函数f(x,y,t)描述的函数族类中寻找一条轨迹，能充分描述车辆从起始位置过渡到目标位置整个过程的动态特性。随着多项式次数的变大，曲线的拟合效果越好，但次数的增多也会导致参数求解的运算量指数增长，通常选用五次多项式进行变道轨迹的规划。在x方向、y方向分别选用五次多项式构造变道轨迹的曲线簇，如下式所示。

在道路结构的约束下，由五次多项式规划的曲线无论是在纵向上还是在侧向上都能达到期望的位置，车辆能在规定的变道时间内平顺的完成变道，且轨迹曲线的曲率在起始点和终了点都能达到零的期望值。但是，基于多项式的轨迹规划方法也存在变道时间和终了点必须预先已知的局限，对多项式中参数的确定需要有较充分的条件，对纵向车速变化的情况和实际车辆变道过程中终了点并不唯一的机动性和自适应性较差。

基于贝塞尔曲线的路径规划方法通过控制点的选取来改变曲线的形状，通常定义n阶贝塞尔曲线由n+1个控制点组成，表达式为：

式中，Pi、t分别是控制点i的坐标值与时间参数，Bi(t)是Bernstein多项式，具体为：

三次贝塞尔曲线的参数方程可表示为：

因其线条光滑且曲率值小的特点而被广泛地应用于轨迹曲线规划中。 

此外，还有基于人工势场法的轨迹规划方法，其基本思想是假设行驶目标点对车辆产生引力，而障碍物对车辆产生斥力，控制车辆沿势场中“势峰”间的“势谷”前进。其中，引力与车辆到行驶目标点的距离成正比，斥力与车辆到障碍物的距离成反比。通过求解车辆所受引力和斥力的合力作为车辆的合外力来控制车辆的行驶速度和运动方向。该方法具有易于数学表达、反应速度快、易于实现算法与环境形成闭环控制等优点，但它在求解过程中极易出现局部最优解而导致产生死锁现象。

各种方法的对比分析如下表所示。

二.轨迹函数选择

轨迹函数决定了车辆能否顺畅、快速、舒适、安全地运行。常见的车辆换道轨迹有基于变道轨迹的等速偏移模型、圆弧换道模型，基于期望侧向加速度的换道模型，余弦函数换道模型，余弦函数，双曲正切函数加权换道模型和等速偏移正弦函数换道模型。轨迹函数可根据轨迹路径曲率变化是否连续、起点和终点曲率是否为零、轨迹函数的灵活性进行选择。

等速偏移模型和圆弧换道模型是对实际变道轨迹进行了简化，等速偏移模型如图1所示，其由三条直线段构成，在轨迹起点C0、终点C3处曲率为零，但轨迹的曲率在C1、C2会发生突变，在实际行车中无法实现。

图1 等速偏移模型

圆弧换道模型是在等速偏移模型的基础上，采用圆弧作为起始段、终了段，两端圆弧的曲率半径均为R，如图2所示。同样，由于在圆弧端点C0、C1、C2、C3处轨迹曲率不连续，使得车辆在实际变道行驶中无法完全实现该轨迹。此外，由于圆弧换道模型是多阶非线性曲线，计算麻烦，如果车辆要调整换道过程，比较困难。

图2 圆弧换道模型

基于期望侧向加速度的换道模型是基于换道车辆的侧向加速度变化规律为线性变化、最大侧向加速度不超过一定值而提出的，该方法认为车辆在直线道路上进行变道行驶时，侧向加速度的形状由两个大小相等的正反梯形组成，梯形的高为侧向加速度的最大值，梯形腰的斜率为侧向加速度率，如图3所示。通过对期望侧向加速度进行两次积分得到理想变道轨迹。基于侧向加速度的变道轨迹能够很好的满足变道过程中曲率连续变化的要求，且在轨迹起点、终点曲率为零，但该模型是分段函数，存在动态调整比较困难的局限性。

图3 基于梯形加速度的换道模型

余弦函数换道模型由于其计算简便、平滑性较好，是目前被广泛采用的轨迹之一，其轨迹如图4所示。假设两车道的车道中心线距离为d，换道过程产生的纵向位移为L，余弦函数换道模型轨迹函数为：

对上式求导得：

则车辆换道模型的轨迹曲率K为：

虽然余弦函数换轨迹的曲率连续变化，但最大曲率出现在路径起点x=0和终点处x=L处，最大曲率为

此时侧向加速度最大，不满足换道模型轨迹中起点和终点处曲率均应为0的要求。

图4 余弦函数换道模型

余弦函数和双曲正切函数加权换道模型是余弦轨迹函数ycos(x)和双曲正切换道函数ytanh(x)加权的轨迹，如图5中虚线和实线分别表示余弦和双曲正切函数换道轨迹函数。该模型通过引入纵向拉伸系数和加权系数，使得在轨迹起点和终点处的曲率值接近于0，但仍不为0。

图5 余弦函数和双曲正切函数加权换道模型

等速偏移正弦函数换道模型是将等速偏移轨迹函数与正弦函数叠加，具备了等速偏移轨迹侧向加速度恒为0的优点与正弦函数换道轨迹平滑性优异的特点。该模型初始函数为：

为了满足换道轨迹的一些要求，上式可改进为：

式中，d为两车道的车道中心线的距离，L为换道过程产生的沿车道方向的纵向位移。

等速偏移正弦函数轨迹如图6所示，只要整个换道行程 L ≥ Lmin ，就能满足车辆在公路的加速度和加速度变化率的限制条件，其中

u为车辆沿车道的纵向速度，amax为车辆允许的最大安全侧向加速度，Jmax为允许的车辆最大侧向冲击度。通过对y进行两次求导，可计算出轨迹曲率，当x=0或x=L时，曲率K=0，此时曲率半径无穷大，车辆沿直线行驶。该模型既满足了换道轨迹路径曲率连续变化、不发生突变的要求，又实现了在换道路径起点和终点处曲率均为0，使得车辆在换道起始时刻、结束时刻的运动方向与车道线保持平行。

图6 等速偏移正弦函数加权换道模型