基于贝塞尔曲线的智能汽车避障轨迹规划方法研究*

曹昌盛，曹昊天

（1.国电南瑞南京控制系统有限公司，江苏 南京 210000；2.湖南大学 机械与运载工程学院，湖南 长沙 410082）

0 引言

近年来，智能汽车因其能够提高通行效率并减少交通事故而受到研究人员的广泛关注[1-2]。目前，百度、谷歌和特斯拉等公司在智能驾驶领域已经取得了一些研究成果，并开始进行路测，但由于技术的复杂性和法规的限制，智能汽车离真正的商业化应用还有一段距离[3]。智能汽车主要包括环境感知、行为决策、轨迹规划和运动控制四大模块，其中轨迹规划作为智能汽车的核心技术也涌现了大量研究成果[1,4]。局部轨迹规划是利用全局路径信息和车载传感器信息为智能汽车实时规划安全舒适的行驶轨迹（包括参考路径和速度）。

目前常见的局部轨迹规划算法有基于图搜索的方法、人工势场法、直接优化法和曲线插值法等。A*、Dijkstra 和IWD 算法是典型的基于图搜索的方法，通过将驾驶场景划分成网格形式，并用某些单元表示障碍物，然后在当前位置和目标位置之间搜索最优轨迹[5-7]。这类方法的计算代价会随着搜索空间的增加而急剧增加，因此，多用于封闭低速场景。人工势场法因其可方便地通过设置斥力函数来处理障碍物约束而被广泛应用于轨迹规划，但其采用梯度下降法确定行驶路径，易陷入局部最优解，进而导致路径的震荡[8]。为了克服这个缺点，曹昊天等提出了一种基于调和速度势的主动避障方法[9-10]。然而，基于势函数的方法只能生成无碰撞的路径，而不能保证路径的长度和曲率是最优的，这将可能影响到路径的跟随精度。Li 和Saravanakumar 等[11-13]在Frénet 坐标系使用直接优化法进行了局部轨迹规划，但他们使用一般的曲线生成轨迹很难在节点处保持连续曲率，这可能会影响驾驶的稳定性和舒适性。

综上分析，本文将利用贝塞尔曲线的曲率处处连续的优点并结合直接优化法，探讨一种基于四阶贝塞尔曲线的轨迹规划算法。路径规划时，将以路径的长度、曲率、连续性和车辆碰撞风险为代价函数，其中引入危险势场理论，描述车辆碰撞危险程度；速度规划时，将以行车效率和舒适性为规划目标进行速度规划，该方法可通过调整各子目标函数的权重来满足不同驾驶需求。

1 道路模型建立

为了轨迹规划时能适应直道和弯道情形，本文使用以车道中心线弧长为参数的三次多项式来描述车道线中心线，在Frénet 坐标系中描述道路上的任意点。Frénet 坐标系将道路中心线定义为基线，将横向偏移定义为与基线的垂直距离，允许车辆在与基线平行的道路上行驶。假设道路信息由感知系统准确获得，则车道线中心线可表示为：

式中，xc(s)和yc(s)分别为道路中心线在x和y方向的参数方程。Frénet 坐标系中，道路上任意一点与车道中心线的关系示意如图1 所示。

由图1 可知，道路上任意一点与道路中心线的关系为：

式中，d为路面上任意点到车道线中心线对应点切线的距离。由式（1）和式（2）可知路面上任意点的笛卡尔坐标(x,y)可映射为Frénet 坐标(s,d)。θ为车道线中心线航向角，由式（3）确定：

将式（3）代入式（2）可得：

车道中心线的航向角和曲率为：

2 轨迹规划算法

2.1 规划路径生成方法

2.1.1 贝塞尔曲线及其性质

如图2 所示，一条由两个端点P0和P4，3 个控制点所唯一确定的四阶贝塞尔曲线，通过调整中间3 个控制点的位置即可得到满足舒适性和安全性需求的路径。四阶贝塞尔曲线可表示为：

式中：Pi表示各控制点，为伯恩斯坦多项式，其表达式为：

贝塞尔曲线的主要性质有：

（1）贝塞尔曲线必经过P0和P4两端点，即：

（3）曲线在P0处的曲率为：

（4）贝塞尔曲线具有放射变换不变的特性。该性质结合性质（3）有助于生成一条曲率处处连续的路径。

2.1.2 贝塞尔曲线控制点推导

局部路径规划就是生成一条引导智能汽车从初始状态到达目标状态的路径。车辆初始状态X0=(x0,y0,φ0,κ0)和目标状态XT=(xT,yT,φT)都包含位置、航向角及曲率信息。根据贝塞尔曲线性（1）可知，曲线两个端点P0和P4分别为车辆初始位置和目标位置。为简化计 算，将车辆 初始状 态旋转 平移至X0=(0,0,0,κ0)。如图2 所示，在X′OY′坐标系中，车辆初始位置即P0处的航向角为0，由性质（2）可得控制点P1的坐标表达式为：

式中，a为P0和P1两点间的距离。

此外，当已知P0与P1的坐标时，由性质（3）可进一步求得控制点P2的坐标为：

且由贝塞尔曲线仿射变换不变性可知，旋转平移变换并不会改变曲线特性，结合性质（2）可得控制点P4和P3的坐标表达式：

式中，c为P3和P4间的距离。

由上述分析可知，四阶贝塞尔曲线的未知参数仅剩a、b和c，通过设置合理的目标函数，求解对应的优化问题可得到最优路径。

2.2 规划路径优化机制

为了适应不同形状道路，令：

式中，s0和sp分别为规划时刻的初始距离和规划距离。路径规划时，以路径质量（路径的长度、曲率和连续性）和行车安全为目标。

（1）路径的质量指标：包括路径的长度、曲率和连续性，其数学描述如下：

①路径长度：

式中，S为伪路径长度，即Frénet 坐标系下规划路径的长度。

②路径曲率平方和：

③路径的连续性：

式中，θ、θpre分别为当前路径和上次规划路径的航向角。

（2）行车安全指标：综合考虑道路边界、静止障碍物和动态障碍物的影响、构建危险势场函数，用以评估车辆的碰撞风险。总势函数可表示为：

式中，n代表障碍物的数量，Ko是与障碍物相关的危险系数，B是车道宽度，Kl、Kr是分别与道路左右边缘相关的危险系数，Di为：

式中，(so,i,do,i)是第i个障碍物车辆在Frénet 坐标系中的坐标，E1、E2分别为尺度因子。在Frénet 坐标 系中(50,0)处有一障碍物时，碰撞风险的分布情况如图3 所示。

图3 在Frénet 坐标系中车辆碰撞风险分布情况

将长度为sp的规划路径等分成长度为∆s的Np段，可得具有Np+1 个参考点的规划路径，其碰撞风险代价J4为：

最终，规划路径时的总代价函数可表示为：

式中，wi(i=1,2,3,4)分别为路径的长度、平滑性、连续性和安全性代价函数的权重。因此，路径规划优化问题可由如下方程描述：

采用序列二次规划算法（Sequential Quadratic Programming，SQP）求解带约束的非线性优化问题（23）。SQP 通过构建近似的二次模型和迭代优化来寻找最优解，广泛应用于许多领域的工程和科学问题中；在每次迭代中，SQP 会计算目标函数的梯度和约束条件的梯度，并构建求解一个二次模型获得下一个迭代点；这个过程不断重复，直到满足停止准则为止。此外，为了确保车辆行驶稳定性，车辆转弯半径应大于最小转弯半径rmin。

2.3 速度规划

式中，t∈[0,1]为参变量，∈B(i=0,1,…,4)为贝塞尔曲线的端点和控制点的坐标。=s-s0，s，s0分别为行驶路程和规划时初始距离。

进一步计算V随行驶路程的变化率A和加速度随路程的变化率Z分别为：

速度规划时，车辆的初始状态用车辆速度、加速度、加速度变化率、规划时刻初始距离和规划距离表示，即(v0,a0,z0,s0,sp)。选择合适的贝塞尔曲线控制点横坐标(i=1,2,3) 确保式（25）和（26）分母不为0，由贝塞尔曲线性质可得V(0)==v0，==0，==sp。初始加速度和加速度变化率为：

考虑到通行效率和舒适性，速度规划的目标是使每个参考点的速度最大，同时使规划距离内的平均速度和加速度变化率最小。显然，此为多目标规划问题，一般不存在唯一最优解。但在决策时，可为每个目标函数设定一个目标值，然后根据驾驶员驾驶需求确定优先级，对目标函数值进行选择。此时，该问题可进一步转化为式（29）所示的极大极小问题[14]，即：

其中，gi,i=1,2,3 分别为子目标函数ϑi的目标值，q={qi},i=1,2,3 分别代表每个目标函数的权重系数，并且，

且满足以下约束：

式中，v0、a0和j0分别为主车在规划时刻的速度、加速度和加速度变化率。Vmax,i是参考点处的最大允许速度，为了防止车辆发生侧滑，最大速度应满足：

其中，μ是道路附着系数，ay_usr是决策者期望的最大侧向加速度，Κ是路径曲率。

3 轨迹规划算法验证

为了验证轨迹规划方法的有效性，设计了针对良好天气条件下，智能车辆在低风险交通环境和结构化道路上，包括直线和曲线道路上的动态避障场景。轨迹规划算法在安装MATLAB R2018b 的计算机硬件平台上编程实现，计算机操作系统为64 位Windows 10，CPU 为Intel-i76700K @4.0 GHz，内存32 GB；然后将轨迹规划结果导入到专业车辆动力学软件CarSim®中实现路径跟随和速度跟踪。轨迹规划方法仿真验证的整体框架如图4 所示，仿真参数设置如表1 所示。

表1 轨迹规划算法参数设置

图4 轨迹规划框架

3.1 直道连续避障轨迹规划仿真

如图5(a)所示，假设下车道中心线的y坐标为0。主车起始位置的坐标为(0,0)，初始速度为25 m/s，静态障碍物中心的Frénet 坐标为(250,2)，动态障碍物中心初始位置的Frénet 坐标为(50,-2)，并以恒定速度10 m/s 沿车道中心线行驶。单次规划路径的长度为60 m，车辆每1 s 重新规划一次，最大允许侧向加速度设为6 m/s2，主车最大速度为25 m/s。

图5 直道避障轨迹规划结果

实验结果如图5 所示，由图5(a)可知，所规划的路径可以保证主车成功避开动态和静态障碍车且可被很好地跟随。同时由图5(b)可看出，整个避障过程，加速度和加速度变化率变化光滑平缓。为了验证车辆在避障过程中的行驶稳定性，将轨迹规划结果导入到CarSim®中进行跟随仿真实验，由图5(c)可知，跟随过程主车方向盘转角和车轮控制力矩最大绝对值分别为36°和328 N·m左右，且保持平缓。由图5(d)可知，跟随过程中纵向和侧向加速度的最大绝对值分别为0.27 m/s2和4.0 m/s2左右，且变化平缓光滑，说明主车运动平稳、舒适。综上，本文轨迹规划方法在直道动态避障时具有良好的效果。

3.2 弯道连续轨迹规划仿真

如图6(a)所示，假设下车道中心线起止点y坐标为0。主车起始位置坐标为(0,0)，初始速度为15 m/s，静态障碍物中心的Frénet 坐标为(200,2)，动态障碍物中心初始位置的Frénet 坐标为(50,-2)，并以恒定速度10 m/s 沿车道中心线行驶，单次规划路径的长度为60 m，车辆每行驶1 s 重新规划一次，主车最大允许侧向加速度为5 m/s2，最大允许速度为20 m/s。

图6 弯道避障轨迹规划仿真结果

实验结果如图6 所示。由图6(a)可知，所规划的路径可以保证主车成功的避开动态和静态障碍车。同时由图6(a)的局部放大图可知规划路径可以被很好地跟随。由图6(b)可看出加速度和加速度变化率在整个行驶过程中光滑平缓。为了验证车辆在避障过程中的行驶稳定性，将轨迹规划结果导入到CarSim®中进行跟随仿真实验，由图6(c)可知，跟随过程主车方向盘转角和车轮控制力矩最大绝对值分别为40°和700 N·m 左右，且总体保持平缓。由图6(d)可知，跟随过程中主车纵向和侧向加速度的最大绝对值分别为1.0 m/s2和3.2 m/s2左右，且总体变化平缓光滑，说明主车运动平稳。

4 结论

本文针对一般结构化道路环境下的智能车动态避障工况，提出了一种基于四阶贝塞尔曲线的避障轨迹规划方法；基于Frénet 坐标，构建有关路径长度、曲率、连续性和行车风险的综合指标，采用SQP 方法求解非线性优化问题获得最优路径；基于通行效率和驾驶舒适性指标，采用多目标优化方法获得最优速度。仿真结果表明：在保证车辆稳定行驶和安全避障的前提下，提出的轨迹规划方法还能尽可能满足通行效率和舒适性的不同需求。此外，本研究也尚存不足，目前仅局限于低风险交通环境下的动态避障工况，后续研究中将着重解决复杂交通流场景下的最优决策和规划问题。