CN117093004A - 一种基于分段贝塞尔曲线的自动驾驶车辆轨迹优化方法 - Google Patents

Abstract

本发明公开了一种基于分段贝塞尔曲线的自动驾驶车辆轨迹优化方法，包括：获取自车车辆位置及状态、决策粗轨迹、动静态障碍物信息、车道中心线信息；将自车车辆位置及状态、决策粗轨迹、动静态障碍物信息从笛卡尔坐标系转换到Frenet坐标系下，并填充到栅格地图中；以转换后的当前车辆位置为规划起点，基于填充了动静态障碍物信息和决策粗轨迹后的栅格地图进行分段时空三维凸空间的构建；采用分段贝塞尔曲线进行轨迹优化，将代价函数、约束条件转换成准二次规划问题数学表达式中各个矩阵的形式，进行最优轨迹求解。本发明借助于贝塞尔曲线的凸包特性和凸形图特性，对轨迹的高阶量进行约束并通过约束控制点在凸空间内来保证轨迹的安全性。

Description

一种基于分段贝塞尔曲线的自动驾驶车辆轨迹优化方法

技术领域

本发明属于自动驾驶车辆轨迹规划技术领域，具体指代一种基于分段贝塞尔曲线的自动驾驶车辆轨迹优化方法。

背景技术

近年来，自动驾驶一直是高校科研人员和企业研发工程师的研究热点，其技术架构主要分为感知、决策、规划、控制四个部分，其中规划算法部分的任务是为车辆找到一条遵循决策层结果且无碰撞、符合车辆运动学约束的最优轨迹，其规划得到轨迹是否足够安全、舒适且平滑，直接影响到车辆行驶安全性和乘员舒适性。

现有的基于优化的轨迹规划算法大多基于五次多项式对车辆横纵向轨迹进行拟合，然而五次多项式的表达能力有限，不适合时空联合规划下具有复杂构型时空障碍和动态约束的问题。此外，在目前关于多项式轨迹的研究中，约束仅仅在有限的采样点上进行检查，这种方案无法检测到样本点之间的碰撞，因此无法保证安全性和可行性。

发明内容

针对于上述现有技术的不足，本发明的目的在于提供一种基于分段贝塞尔曲线的自动驾驶车辆轨迹优化方法，其借助于贝塞尔曲线的凸包特性和凸形图特性，可以方便的对轨迹的高阶量进行约束并通过约束控制点在凸空间内来保证轨迹的安全性。

为达到上述目的，本发明采用的技术方案如下：

本发明的一种基于分段贝塞尔曲线的自动驾驶车辆轨迹优化方法，步骤如下：

1)获取自车车辆位置及状态、决策粗轨迹、动静态障碍物信息、车道中心线信息；

2)将步骤1)中获取到的自车车辆位置及状态、决策粗轨迹、动静态障碍物信息从笛卡尔坐标系转换到Frenet坐标系下，并填充到栅格地图中；

3)以转换后的当前车辆位置为规划起点，基于填充了动静态障碍物信息和决策粗轨迹后的栅格地图进行分段时空三维凸空间的构建；

4)基于步骤3)中构建的时空三维凸空间信息和步骤1)中获取的决策粗轨迹信息，采用分段贝塞尔曲线进行轨迹优化，将代价函数、约束条件转换成准二次规划问题数学表达式中各个矩阵的形式，并调用求解器库函数进行最优轨迹求解。

进一步地，所述步骤1)中自车车辆位置及状态包括：笛卡尔坐标系下车辆位置、横纵向速度、横纵向加速度、车辆横摆角、车辆转向角以及车辆参数；决策粗轨迹为自动驾驶车辆决策算法输出的车辆未来一段时间的决策轨迹，其由一系列的车辆离散状态点组成，每一个车辆离散状态点包含了对应时间戳的车辆位置及状态；动静态障碍物信息为自动驾驶车辆感知到的周围障碍物信息，其中静态障碍物为在规划周期内状态不会发生改变的障碍物(如路边静止的车辆)，动态障碍物为在规划周期内状态会随时间变化的障碍物(如自车周围不断运动的车辆、行人)；车道中心线信息为自动驾驶车辆通过高精地图或感知得到的车辆当前行驶车道以及附近车道的中心线信息，存储方式包括但不限于：一系列的车道中心线离散点、以样条曲线拟合的车道中心线函数表达式。

进一步地，所述步骤2)中栅格地图的分辨率设置为0.2m。

进一步地，所述步骤2)中的笛卡尔坐标系转换到Frenet坐标系分为坐标点位置转换和运动状态转换；在进行坐标点位置转换时，将待转换点P向参考线(即车道中心线)进行投影，在确定待转换点P在Frenet坐标系下的纵向坐标s时，在参考线上寻找与待转换点P匹配的投影点M＝(x(s^*),y(s^*))，s^*为投影点对应的纵向坐标，使得线段PM长度最短，即：

其中，D(s)为线段PM的长度：

符合条件的投影点M能够在其切线方向与PM垂直，PM长度为|L^*|，投影点M位于参考轨迹s＝s^*处，则待转换点P在Frenet坐标系中的坐标值可确定为(s^*,l^*)；在计算s^*时，使用二分法获取距离较短的初始解s_init，在初始解s_init的微小邻域利用牛顿法迭代求解使得D′(s)＝0的精确解s＝s^*，迭代公式如下：

式中，为每次迭代的纵向坐标；

使用对s_init赋值并继续重复计算，直至/>取值收敛时将收敛值记为s^*；在计算l^*时，求解公式如下：

式中，l为Frenet坐标系下的横向坐标，(x_x,y_x)为待转换点在笛卡尔坐标系下的坐标，(x_r,y_r)为投影点在笛卡尔坐标系下的坐标，θ_r为投影点向量与笛卡尔坐标系x轴的夹角；

上述过程完成了坐标点从笛卡尔坐标系到Frenet坐标系的转换，对于车辆和动态障碍物，需进行运动状态(速度，加速度等)的转换，从笛卡尔坐标系向Frenet坐标系转换依据以下公式进行：

式中，s、分别为转换后纵向坐标对时间的0、1、2阶次导数，l、l′、l″分别为转换后横向坐标对纵向坐标的0、1、2阶次导数，v_x为车辆在笛卡尔坐标系下的车速，θ_x为待转换点向量与笛卡尔坐标系x轴的夹角，k_r、k_r′分别为投影点处参考线的曲率对时间的0、1阶次导数，a_x为车辆在笛卡尔坐标系下的加速度，k_x为当前车辆的转向曲率。

进一步地，所述步骤3)中分段时空三维凸空间的构建方法包含以下步骤：

31)获取决策粗轨迹中包含的所有状态点，并将规划起点和决策粗轨迹中的第一个状态点对应s、l、t的值作为第一个凸空间的上下界构建第一个凸空间，s为Frenet坐标系下的纵向坐标，l为Frenet坐标系下的横向坐标，t为对应时间戳；

32)对第一个凸空间进行栅格坐标检查和碰撞检查，如果栅格坐标超过设置的栅格地图大小且碰撞检查通过，则执行步骤33)；如果栅格坐标超过设置的栅格地图大小或碰撞检查不通过，则第一个凸空间构建失败，等待下一次规划循环；

33)对构建的第一个凸空间进行扩充，具体步骤为：依次对第一个凸空间的S、L、T轴上下限进行定步长的扩充，与动静态障碍物的边界产生重合即停止，停止扩充时第一个凸空间的S、L、T轴上下限的值作为第一个凸空间最终的S、L、T轴上下限；

34)获取扩充后的第一个凸空间内包含的最后一个决策粗轨迹的状态点，并将第一个凸空间的上限更新为第一个凸空间中包含的最后一个决策粗轨迹的状态点对应的S、L、T值，并以第一个凸空间内包含的最后一个决策粗轨迹的状态点和下一个状态点构建下一个凸空间；

35)对步骤34)中构建的下一个凸空间依次重复步骤32)-34)，直到所有决策粗轨迹状态点均被包含在所构建的决策粗轨迹中。

进一步地，所述步骤4)中构建n段贝塞尔曲线所构成的优化轨迹表示为：

式中，σ∈{s,l}表示贝塞尔曲线的维度，表示第j段贝塞尔曲线的第i个控制点；t₀,t₁,...,t_n为每一段贝塞尔曲线的起始点与终止点的时间维度坐标，为Bernstein多项式；α_j为第j段贝塞尔曲线的异步时域缩放因子，目的是为了将每个段轨迹的贝塞尔曲线定义在固定区间[0,1]内。

进一步地，所述步骤4)中采用轨迹的jerk值的平方对时间的积分作为优化的代价函数，其数学表达式为：

式中，J_j为第j段贝塞尔曲线的代价函数，分别为轨迹纵向、横向的jerk值，w_s和w_l分别为纵向与横向的控制权重。

进一步地，所述步骤4)中的约束条件归纳为三类：终端约束、连续性约束和碰撞约束与动态约束；终端约束指生成的轨迹应满足轨迹的起点状态和终点状态与决策粗轨迹的初始点状态与终止点状态/>相同，其中σ∈{s,l}，/>为t时刻位置坐标的k阶导数，其数学表达式如下：

连续性约束指所得到的优化轨迹的k阶导在每一个相邻的凸空间的连接点处都是连续的；第j个凸空间与第j+1个凸空间之间的连续性约束表示为：

碰撞约束与动态约束指算法中对车辆碰撞边界和动力学相关的约束，不同算法的约束的限制程度不同，需约束轨迹的位置、速度、加速度，即贝塞尔曲线的k＝0,1,2阶导数，约束表达为如下的线性不等式约束形式：

式中，为不等式约束的下界和上界，k表示需要约束量的导数的阶次。

进一步地，所述步骤4)中将轨迹优化问题转换成OOQP求解器的通用二次规划形式进行求解：

Ax＝b,d≤Cx≤f,l≤x≤u,

式中，Q为对称的正半定n×n矩阵；x∈Rⁿ为变量向量；A和C分别表示维度为m_a×n和m_c×n的矩阵，b,d,f,l,u为适当维度的向量。

本发明的有益效果：

1、本发明采用的多段曲线进行轨迹优化的方案对复杂的交通路况的适应性更好，且在一定程度上降低了轨迹优化问题的求解难度，降低了规划算法的计算复杂度。

2、本发明基于贝塞尔曲线进行轨迹优化，得益于贝塞尔曲线的凸形图特性，可以方便的对轨迹的高阶量进行表示并约束，相比于传统基于五次多项式的轨迹优化方法，本发明方法对于复杂构型时空障碍和动态约束的表达能力更强，在复杂道路工况下的鲁棒性更好，生成的轨迹更加符合实际。

3、本发明基于贝塞尔曲线进行轨迹优化，得益于贝塞尔曲线的凸包特性，本发明方法在轨迹优化时只要约束控制点在给定的凸空间内就能保证生成轨迹一定在凸空间内，进而确保了生成轨迹的安全性，相比于传统基于五次多项式的轨迹优化方法，本发明方法不仅在轨迹安全性上有提升，同时由于省去了传统方案中的轨迹检查模块，算法的计算效率也有所提升。

4、本发明中的分段时空三维凸空间构建方法，逻辑清晰，计算简单，对于算法的实时性有了较大提升，比传统凸空间构造方法更能发挥本发明中基于分段贝塞尔曲线的轨迹优化方法的优势。

附图说明

图1为本发明中基于分段贝塞尔曲线的轨迹优化方法流程图；

图2为本发明中分段时空三维凸空间构建方法流程图。

具体实施方式

为了便于本领域技术人员的理解，下面结合实施例与附图对本发明作进一步的说明，实施方式提及的内容并非对本发明的限定。

参照图1-图2所示，本发明的一种基于分段贝塞尔曲线的自动驾驶车辆轨迹优化方法，步骤如下：

1)获取自车车辆位置及状态、决策粗轨迹、动静态障碍物信息、车道中心线信息；

自车车辆位置及状态包括：笛卡尔坐标系下车辆位置、横纵向速度、横纵向加速度、车辆横摆角、车辆转向角以及车辆参数；决策粗轨迹为自动驾驶车辆决策算法输出的车辆未来一段时间的决策轨迹，其由一系列的车辆离散状态点组成，每一个车辆离散状态点包含了对应时间戳的车辆位置及状态；动静态障碍物信息为自动驾驶车辆感知到的周围障碍物信息，其中静态障碍物为在规划周期内状态不会发生改变的障碍物(如路边静止的车辆)，动态障碍物为在规划周期内状态会随时间变化的障碍物(如自车周围不断运动的车辆、行人)；车道中心线信息为自动驾驶车辆通过高精地图或感知得到的车辆当前行驶车道以及附近车道的中心线信息，存储方式包括但不限于：一系列的车道中心线离散点、以样条曲线拟合的车道中心线函数表达式。

2)将步骤1)中获取到的自车车辆位置及状态、决策粗轨迹、动静态障碍物信息从笛卡尔坐标系转换到Frenet坐标系下，并填充到栅格地图中；

其中，栅格地图的分辨率设置为0.2m。

笛卡尔坐标系转换到Frenet坐标系分为坐标点位置转换和运动状态转换；在进行坐标点位置转换时，将待转换点P向参考线(即车道中心线)进行投影，在确定待转换点P在Frenet坐标系下的纵向坐标s时，在参考线上寻找与待转换点P匹配的投影点M＝(x(s^*),y(s^*))，s^*为投影点对应的纵向坐标，使得线段PM长度最短，即：

其中，D(s)为线段PM的长度：

符合条件的投影点M能够在其切线方向与PM垂直，PM长度为|L^*|，投影点M位于参考轨迹s＝s^*处，则待转换点P在Frenet坐标系中的坐标值可确定为(s^*,l^*)；在计算s^*时，使用二分法获取距离较短的初始解s_init，在初始解s_init的微小邻域利用牛顿法迭代求解使得D′(s)＝0的精确解s＝s^*，迭代公式如下：

式中，为每次迭代的纵向坐标；

使用对s_init赋值并继续重复计算，直至/>取值收敛时将收敛值记为s^*；在计算l^*时，求解公式如下：

式中，l为Frenet坐标系下的横向坐标，(x_x,y_x)为待转换点在笛卡尔坐标系下的坐标，(x_r,y_r)为投影点在笛卡尔坐标系下的坐标，θ_r为投影点向量与笛卡尔坐标系x轴的夹角；

上述过程完成了坐标点从笛卡尔坐标系到Frenet坐标系的转换，对于车辆和动态障碍物，需进行运动状态(速度，加速度等)的转换，从笛卡尔坐标系向Frenet坐标系转换依据以下公式进行：

式中，s、分别为转换后纵向坐标对时间的0、1、2阶次导数，l、l′、l″分别为转换后横向坐标对纵向坐标的0、1、2阶次导数，v_x为车辆在笛卡尔坐标系下的车速，θ_x为待转换点向量与笛卡尔坐标系x轴的夹角，k_r、k_r′分别为投影点处参考线的曲率对时间的0、1阶次导数，a_x为车辆在笛卡尔坐标系下的加速度，k_x为当前车辆的转向曲率。

3)以转换后的当前车辆位置为规划起点，基于填充了动静态障碍物信息和决策粗轨迹后的栅格地图进行分段时空三维凸空间的构建；

分段时空三维凸空间的构建方法包含以下步骤：

31)获取决策粗轨迹中包含的所有状态点，并将规划起点和决策粗轨迹中的第一个状态点对应s、l、t的值作为第一个凸空间的上下界构建第一个凸空间，s为Frenet坐标系下的纵向坐标，l为Frenet坐标系下的横向坐标，t为对应时间戳；

32)对第一个凸空间进行栅格坐标检查和碰撞检查，如果栅格坐标超过设置的栅格地图大小且碰撞检查通过，则执行步骤33)；如果栅格坐标超过设置的栅格地图大小或碰撞检查不通过，则第一个凸空间构建失败，等待下一次规划循环；

33)对构建的第一个凸空间进行扩充，具体步骤为：依次对第一个凸空间的S、L、T轴上下限进行定步长的扩充，与动静态障碍物的边界产生重合即停止，停止扩充时第一个凸空间的S、L、T轴上下限的值作为第一个凸空间最终的S、L、T轴上下限；

34)获取扩充后的第一个凸空间内包含的最后一个决策粗轨迹的状态点，并将第一个凸空间的上限更新为第一个凸空间中包含的最后一个决策粗轨迹的状态点对应的S、L、T值，并以第一个凸空间内包含的最后一个决策粗轨迹的状态点和下一个状态点构建下一个凸空间；

35)对步骤34)中构建的下一个凸空间依次重复步骤32)-34)，直到所有决策粗轨迹状态点均被包含在所构建的决策粗轨迹中。

4)基于步骤3)中构建的时空三维凸空间信息和步骤1)中获取的决策粗轨迹信息，采用分段贝塞尔曲线进行轨迹优化，将代价函数、约束条件转换成准二次规划问题数学表达式中各个矩阵的形式，并调用求解器库函数进行最优轨迹求解；

其中，所述步骤4)中构建n段贝塞尔曲线所构成的优化轨迹表示为：

式中，σ∈{s,l}表示贝塞尔曲线的维度，表示第j段贝塞尔曲线的第i个控制点；t₀,t₁,...,t_n为每一段贝塞尔曲线的起始点与终止点的时间维度坐标，为Bernstein多项式；α_j为第j段贝塞尔曲线的异步时域缩放因子，目的是为了将每个段轨迹的贝塞尔曲线定义在固定区间[0,1]内。

采用轨迹的jerk值的平方对时间的积分作为优化的代价函数，其数学表达式为：

式中，J_j为第j段贝塞尔曲线的代价函数，分别为轨迹纵向、横向的jerk值，w_s和w_l分别为纵向与横向的控制权重。

约束条件归纳为三类：终端约束、连续性约束和碰撞约束与动态约束；终端约束指生成的轨迹应满足轨迹的起点状态和终点状态与决策粗轨迹的初始点状态与终止点状态/>相同，其中σ∈{s,l}，/>为t时刻位置坐标的k阶导数，其数学表达式如下：

连续性约束指所得到的优化轨迹的k阶导在每一个相邻的凸空间的连接点处都是连续的；第j个凸空间与第j+1个凸空间之间的连续性约束表示为：

碰撞约束与动态约束指算法中对车辆碰撞边界和动力学相关的约束，不同算法的约束的限制程度不同，需约束轨迹的位置、速度、加速度，即贝塞尔曲线的k＝0,1,2阶导数，约束表达为如下的线性不等式约束形式：

式中，为不等式约束的下界和上界，k表示需要约束量的导数的阶次。

将轨迹优化问题转换成OOQP求解器的通用二次规划形式进行求解：

Ax＝b,d≤Cx≤f,l≤x≤u,

式中，Q为对称的正半定n×n矩阵；x∈Rⁿ为变量向量；A和C分别表示维度为m_a×n和m_c×n的矩阵，b,d,f,l,u为适当维度的向量。

本发明具体应用途径很多，以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以作出若干改进，这些改进也应视为本发明的保护范围。

Claims (9)

1.一种基于分段贝塞尔曲线的自动驾驶车辆轨迹优化方法，其特征在于，步骤如下：

1)获取自车车辆位置及状态、决策粗轨迹、动静态障碍物信息、车道中心线信息；

2)将步骤1)中获取到的自车车辆位置及状态、决策粗轨迹、动静态障碍物信息从笛卡尔坐标系转换到Frenet坐标系下，并填充到栅格地图中；

3)以转换后的当前车辆位置为规划起点，基于填充了动静态障碍物信息和决策粗轨迹后的栅格地图进行分段时空三维凸空间的构建；

4)基于步骤3)中构建的时空三维凸空间信息和步骤1)中获取的决策粗轨迹信息，采用分段贝塞尔曲线进行轨迹优化，将代价函数、约束条件转换成准二次规划问题数学表达式中各个矩阵的形式，并调用求解器库函数进行最优轨迹求解。

2.根据权利要求1所述的基于分段贝塞尔曲线的自动驾驶车辆轨迹优化方法，其特征在于，所述步骤1)中自车车辆位置及状态包括：笛卡尔坐标系下车辆位置、横纵向速度、横纵向加速度、车辆横摆角、车辆转向角以及车辆参数；决策粗轨迹为自动驾驶车辆决策算法输出的车辆未来一段时间的决策轨迹，其由一系列的车辆离散状态点组成，每一个车辆离散状态点包含了对应时间戳的车辆位置及状态；动静态障碍物信息为自动驾驶车辆感知到的周围障碍物信息，其中静态障碍物为在规划周期内状态不会发生改变的障碍物，动态障碍物为在规划周期内状态会随时间变化的障碍物；车道中心线信息为自动驾驶车辆通过高精地图或感知得到的车辆当前行驶车道以及附近车道的中心线信息。

3.根据权利要求1所述的基于分段贝塞尔曲线的自动驾驶车辆轨迹优化方法，其特征在于，所述步骤2)中栅格地图的分辨率设置为0.2m。

4.根据权利要求1所述的基于分段贝塞尔曲线的自动驾驶车辆轨迹优化方法，其特征在于，所述步骤2)中的笛卡尔坐标系转换到Frenet坐标系分为坐标点位置转换和运动状态转换；在进行坐标点位置转换时，将待转换点P向参考线进行投影，在确定待转换点P在Frenet坐标系下的纵向坐标s时，在参考线上寻找与待转换点P匹配的投影点M＝(x(s^*),y(s^*))，s^*为投影点对应的纵向坐标，使得线段PM长度最短，即：

其中，D(s)为线段PM的长度：

符合条件的投影点M能够在其切线方向与PM垂直，PM长度为|L^*|，投影点M位于参考轨迹s＝s^*处，则待转换点P在Frenet坐标系中的坐标值可确定为(s^*,l^*)；在计算s^*时，使用二分法获取距离较短的初始解s_init，在初始解s_init的微小邻域利用牛顿法迭代求解使得D′(s)＝0的精确解s＝s^*，迭代公式如下：

式中，为每次迭代的纵向坐标；

使用对s_init赋值并继续重复计算，直至/>取值收敛时将收敛值记为s^*；在计算l^*时，求解公式如下：

式中，l为Frenet坐标系下的横向坐标，(x_x,y_x)为待转换点在笛卡尔坐标系下的坐标，(x_r,y_r)为投影点在笛卡尔坐标系下的坐标，θ_r为投影点向量与笛卡尔坐标系x轴的夹角；

上述过程完成了坐标点从笛卡尔坐标系到Frenet坐标系的转换，对于车辆和动态障碍物，需进行运动状态的转换，从笛卡尔坐标系向Frenet坐标系转换依据以下公式进行：

式中，s、分别为转换后纵向坐标对时间的0、1、2阶次导数，l、l′、l″分别为转换后横向坐标对纵向坐标的0、1、2阶次导数，v_x为车辆在笛卡尔坐标系下的车速，θ_x为待转换点向量与笛卡尔坐标系x轴的夹角，k_r、k_r′分别为投影点处参考线的曲率对时间的0、1阶次导数，a_x为车辆在笛卡尔坐标系下的加速度，k_x为当前车辆的转向曲率。

5.根据权利要求4所述的基于分段贝塞尔曲线的自动驾驶车辆轨迹优化方法，其特征在于，所述步骤3)中分段时空三维凸空间的构建方法包含以下步骤：

31)获取决策粗轨迹中包含的所有状态点，并将规划起点和决策粗轨迹中的第一个状态点对应s、l、t的值作为第一个凸空间的上下界构建第一个凸空间，s为Frenet坐标系下的纵向坐标，l为Frenet坐标系下的横向坐标，t为对应时间戳；

32)对第一个凸空间进行栅格坐标检查和碰撞检查，如果栅格坐标超过设置的栅格地图大小且碰撞检查通过，则执行步骤33)；如果栅格坐标超过设置的栅格地图大小或碰撞检查不通过，则第一个凸空间构建失败，等待下一次规划循环；

33)对构建的第一个凸空间进行扩充，具体步骤为：依次对第一个凸空间的S、L、T轴上下限进行定步长的扩充，与动静态障碍物的边界产生重合即停止，停止扩充时第一个凸空间的S、L、T轴上下限的值作为第一个凸空间最终的S、L、T轴上下限；

34)获取扩充后的第一个凸空间内包含的最后一个决策粗轨迹的状态点，并将第一个凸空间的上限更新为第一个凸空间中包含的最后一个决策粗轨迹的状态点对应的S、L、T值，并以第一个凸空间内包含的最后一个决策粗轨迹的状态点和下一个状态点构建下一个凸空间；

35)对步骤34)中构建的下一个凸空间依次重复步骤32)-34)，直到所有决策粗轨迹状态点均被包含在所构建的决策粗轨迹中。

6.根据权利要求5所述的基于分段贝塞尔曲线的自动驾驶车辆轨迹优化方法，其特征在于，所述步骤4)中构建n段贝塞尔曲线所构成的优化轨迹表示为：

式中，σ∈{s,l}表示贝塞尔曲线的维度，表示第j段贝塞尔曲线的第i个控制点；t₀,t₁,...,t_n为每一段贝塞尔曲线的起始点与终止点的时间维度坐标，/>为Bernstein多项式；α_j为第j段贝塞尔曲线的异步时域缩放因子。

7.根据权利要求5所述的基于分段贝塞尔曲线的自动驾驶车辆轨迹优化方法，其特征在于，所述步骤4)中采用轨迹的jerk值的平方对时间的积分作为优化的代价函数，其数学表达式为：

式中，J_j为第j段贝塞尔曲线的代价函数，分别为轨迹纵向、横向的jerk值，w_s和w_l分别为纵向与横向的控制权重。

8.根据权利要求5所述的基于分段贝塞尔曲线的自动驾驶车辆轨迹优化方法，其特征在于，所述步骤4)中的约束条件归纳为三类：终端约束、连续性约束和碰撞约束与动态约束；终端约束指生成的轨迹应满足轨迹的起点状态和终点状态与决策粗轨迹的初始点状态与终止点状态/>相同，其中σ∈{s,l}，/>为t时刻位置坐标的k阶导数，其数学表达式如下：

连续性约束指所得到的优化轨迹的k阶导在每一个相邻的凸空间的连接点处都是连续的；第j个凸空间与第j+1个凸空间之间的连续性约束表示为：

碰撞约束与动态约束指算法中对车辆碰撞边界和动力学相关的约束，不同算法的约束的限制程度不同，需约束轨迹的位置、速度、加速度，即贝塞尔曲线的k＝0,1,2阶导数，约束表达为如下的线性不等式约束形式：

式中，为不等式约束的下界和上界，k表示需要约束量的导数的阶次。

9.根据权利要求5所述的基于分段贝塞尔曲线的自动驾驶车辆轨迹优化方法，其特征在于，所述步骤4)中将轨迹优化问题转换成OOQP求解器的通用二次规划形式进行求解：

Ax＝b,d≤Cx≤f,l≤x≤u,

式中，Q为对称的正半定n×n矩阵；x∈Rⁿ为变量向量；A和C分别表示维度为m_a×n和m_c×n的矩阵，b,d,f,l,u为适当维度的向量。