一种自动驾驶车辆变道轨迹规划方法

技术领域

本发明涉及车辆变道轨迹规划技术领域，具体为一种自动驾驶车辆变道轨迹规划方法。

背景技术

自动驾驶技术是提升车辆主动安全的关键技术，在过去十几年里已经得到蓬勃发展。作为自动驾驶系统的核心模块，规划模块综合考虑传感器感知的信息，包括驾驶环境信息和车辆状态，然后生成一条安全、平滑、可行的轨迹，并输入控制模块。但在动态场景下，实现完全的自动驾驶仍面临许多挑战。

变道问题是最典型的动态场景，当自动驾驶车辆想要离开公路或在下一个十字路口左转时，车辆必须进行换道，此时自动驾驶车辆的轨迹更易受周围车辆的影响。针对变道场景，尽管人们提出了许多轨迹规划算法，但是生成良好的轨迹仍然是困难的，而且这些方法并不是在所有情况下都是有效和通用的。

变道场景下的轨迹规划问题是一个典型的时空耦合问题，由于障碍物的空间位置随时间动态变化，不仅要考虑空间维度，还要考虑时间维度。为了降低问题的复杂度，一些研究，提出了解耦的方法，其中包括路径-速度解耦(PV)和横向-纵向解耦(LL)。PV方法将一个三维时空规划问题，转化为一个二维路径规划和一维速度规划。路径规划只考虑静态障碍物，找到一条空间最优路径。速度规划沿着已经规划好的路径，通过规划车辆的速度来避免动态障碍物。PV适合静态场景和障碍物速度较小的变道场景。与路径-速度解耦(PV)对应的是横向-纵向解耦(LL)，LL将轨迹规划问题分解纵向和横向轨迹规划，LL分别考虑汽车横向轨迹和纵向轨迹随时间的变化，从而生成更加符合动态场景的轨迹。

文献[1]首先将当前帧和历史帧的特征信息输入神经网络模型，得到变道粗轨迹，然后使用二次规划(QP)做轨迹优化，得到更加精细的轨迹。该方法需要大量的数据训练网络模型，而且神经网络输出的粗轨迹缺乏必要的可解释性，很难满足车辆的动力学要求。

文献[2]采用多项式构建变道轨迹，通过对末状态多次采样得到候选轨迹簇，使用预先设计好的代价函数选择最优轨迹，最后在已有的轨迹上分配速度。该方法受限于末状态采样次数和多项式的形式，同时这种路径-速度解耦的方式，也难以得到全局最优解。

文献[3]也采样了路径-速度解耦的方式，与大多数不同的是，它首先进行纵向的速度规划，当前车辆在预设变道时间内的最优纵向速度序列。在已有速度规划的基础上将路径规划转化为一个二次规划问题(QP)。该方法将变道过程太过理想化，得到的变道过程速度序列很难保证满足之后能准确求出路径，也难以保证路径-速度两个子优化能得到全局解。

文献[4]该方法首先确定变道的开始点和终止点，以此得到变道汇入区域。通过对汇入区域的分析判断是否可以变道，但该方法并没有给出具体的轨迹变道算法，缺乏变道参考轨迹和变道优化轨迹的构建过程。

文献[5]包含变道过程中轨迹的生成和车辆的控制，但该方法只能采用预先规定的几种轨迹模式，通过调整参数来适应不同的路况。该方法实现简单，但由于轨迹模式是有限的，无法满足复杂路况，同时也很难根据场景信息来判断使用何种轨迹曲线。

通过研究上述若干方法，可以发现目前已知的变道方法存在很多不足。路径-速度解耦的方法，难以满足得到全局最优解的要求，路径-速度耦合的方法又存在计算量大，难以满足实时性要求高的场合，为此，我们提出一种实用性更高的自动驾驶车辆变道轨迹规划方法。

发明内容

本发明的目的在于提供一种自动驾驶车辆变道轨迹规划方法，解决了现有的问题。

为实现上述目的，本发明提供如下技术方案：一种自动驾驶车辆变道轨迹规划方法，包括以下步骤：

S1、启发阶段；

S11、目标车道Gap选择；

S12、汇入区域计算和静态车道边界约束；

S13、启发式参考轨迹生成；

S2、优化阶段；

S21、标称轨迹生成；

S22、约束条件添加：规划得到的变道轨迹应道考虑障车辆运动学约束、车道边界约束，障碍物约束；

S221、车辆运动学约束；

S222、车道边界约束；

S223、障碍物约束；

S23、代价函数建立；

S24、反向过程更新控制序列；

S25、正向过程更新状态序列；

S26、计算新的状态序列和控制序列对应的代价。

优选的，所述步骤S11具体包括以下步骤：

根据目标车道内车辆与当前主车的相对位置，将目标车道内的Gap分为三类：后方Gap、当前Gap、前方Gap，根据每个Gap的长度和Gap车辆与主车的相对速度、相对位置，选择一个最佳Gap，做为主车将要汇入的Gap。

优选的，所述步骤S12具体包括以下步骤：

计算主车变道的汇入开始点start和汇入结束点end，以及根据汇入区域进行车道边界约束设置；

S121、在start_s之前，仅受原车道边界约束；

S122、在start_s和end_s之间，打开到目标车道的边界；

S123、在end_s之后，仅受目标车道边界约束。

优选的，所述步骤S13具体包括以下步骤：

在start_s之前，主车沿原车道行驶，在start_s和end_s之前，主车开始变道。

优选的，所述步骤S21具体包括以下步骤：

S211、将不同时刻的状态变量离散化表示:S＝{s

S212、将不同时刻的控制变量离散化表述:U＝{u

根据车辆动力学模型，可以得到车辆的状态方程:

计算得到第一次迭代的标称轨迹:

s

s

优选的，所述步骤S221具体包括以下步骤：

为满足车辆的运动控制要求，规定:

g

g

g

g

其中a

与现有技术相比，本发明的有益效果如下：

本发明的启发式轨迹生成，具体包括变道Gap的选择，和正弦变道曲线生成，可以在极短时间内生成参考轨迹，为后续优化提供依据；

本发明的优化阶段采用屏障函数将带约束的优化问题，转化为无约束优化问题，从而可以使用无约束iLQR算法求解；

本发明提出屏障函数可以调节参数以满足不同类型约束，具有极高的普适性；

本发明的车道边界约束构建，本发明将主车与车道边界的投影距离作为约束条件，而不是直接构建车道边界函数，可以适用任意曲率的车道；

本发明提出的方法不同与任何一种解耦的轨迹规划方法，而是直接对控制量进行优化，采用路径和速度耦合的方式，求解得到满足动态场景要求的轨迹，更容易得到全局最优解；

本发明控制量为加速度和横摆角速度，符合车辆的运动学模型，在笛卡尔坐标系下的优化过程，也更容易得到满足车辆运动学约束的可行解。

附图说明

图1为本发明变道流程示意图；

图2为本发明Gap选择示意图；

图3为本发明汇入车道示意图；

图4为本发明车道边界约束示意图；

图5为本发明简化椭圆车辆示意图；

图6为本发明简化圆形车辆示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。

如图1所示，一种自动驾驶车辆变道轨迹规划方法，包括以下步骤：

S1、启发阶段；

S11、目标车道Gap选择：

如图2所示，根据目标车道内车辆与当前主车的相对位置，将目标车道内的Gap分为三类：后方Gap、当前Gap、前方Gap，根据每个Gap的长度和Gap车辆与主车的相对速度、相对位置，选择一个最佳Gap，做为主车将要汇入的Gap；

S12、汇入区域计算和静态车道边界约束：

如图3所示，计算主车变道的汇入开始点start(start_s，start_l)和汇入结束点end(end_s，end_l)，以及根据汇入区域(start_s，end_s)进行车道边界约束设置；

S121、在start_s之前，仅受原车道边界约束；

S122、在start_s和end_s之间，打开到目标车道的边界；

S123、在end_s之后，仅受目标车道边界约束；

S13、启发式参考轨迹生成：

在start_s之前，主车沿原车道行驶，在start_s和end_s之前，主车开始变道；

预设变道的启发式粗轨迹为正弦曲线

S2、优化阶段；

S21、标称轨迹生成：

S211、将不同时刻的状态变量离散化表示:S＝{s0,s1,s2,...,sN}，其中s

S212、将不同时刻的控制变量离散化表述:U＝{u

根据车辆动力学模型，可以得到车辆的状态方程:

计算得到第一次迭代的标称轨迹:

s

s

S22、约束条件添加：规划得到的变道轨迹应道考虑障车辆运动学约束、车道边界约束，障碍物约束；

S221、车辆运动学约束：

为满足车辆的运动控制要求，规定:

g

g

g

g

其中ak,ωk为tk时刻的加速度和横摆角速度，a

S222、车道边界约束：

如图4所示，根据在启发阶段创建的静态车道边界，建立车道边界约束，在任意t

S223、障碍物约束：

将障碍车辆简化为椭圆形，如图5，将主车简化为两个圆形，如图6。

在任意tk时刻，障碍车辆中心坐标

/>

其中

本发明建立屏障函数，将带约束的优化问题，转化为无约束问题，从而使用无约束的iLQR进行迭代求解。

屏障函数：

通过调整参数k

S23、代价函数建立：

tk时刻的代价函数:

L

Cost(s

S24、反向过程更新控制序列：

在已有标称轨迹的基础上，采用无约束得iLQR反向动态规划更新控制序列；

S25、正向过程更新状态序列：正向过程更新状态序列，根据初始状态和新的控制序列，更新状态序列，得到新的标称轨迹；

s

s

S26、计算新的状态序列和控制序列对应的代价J

背景技术的参考文献:

[1]基于无人驾驶的车辆自动变道方法、装置、设备及介质:202211128210.X[P]2022.10.18

[2]一种多车复杂交通环境下的智能客车动态变道轨迹规划方法:202210548395.3[P]2022.05.20

[3]一种变道行车轨迹规划方法、装置、设备和介质:202210386283.2[P]2022.07.08

[4]用于自动驾驶车辆的变道方法:202111639588.1[P]2022.03.15

[5]一种车辆的变道控制系统、控制方法:202010237235.8[P]2021.10.22。