CN111338335B - 一种结构化道路场景下的车辆局部轨迹规划方法 - Google Patents

Abstract

本发明提出的一种结构化道路场景下的车辆局部轨迹规划方法，首先通过建立一种基于车道参考线的参考坐标系，得到车辆当前时刻在该参考坐标系下的运动状态向量；接着分别生成不同速度区间的参考坐标系下的纵向采样轨迹集合和横向采样轨迹集合，纵向采样轨迹集合内的所有采样轨迹的曲线形式相同，横向采样轨迹集合内所有采样轨迹的曲线形式相同；然后将横纵向采样轨迹进行合成；随后将合成后的采样轨迹转换回笛卡尔坐标系下，并剔除不满足轨迹曲率和碰撞检测的采样轨迹；最后计算每一条采样轨迹的代价，其中代价最小的轨迹即为本发明所得到的最优轨迹。本发明通过构造高速和低速下的采样轨迹可提高采样有效率，对复杂约束条件适应性强。

Description

一种结构化道路场景下的车辆局部轨迹规划方法

技术领域

本发明属于智能车决策中的轨迹规划技术领域，特别涉及一种结构化道路场景下的车辆局部轨迹规划方法，用于在复杂动态交通场景下在线快速计算生成以自车起始点为起点的无碰撞轨迹。

背景技术

自动驾驶汽车局部轨迹规划问题根据应用场景的不同可以分为非结构化场景下的局部轨迹规划问题和结构化场景下的局部轨迹规划问题。结构化场景如高速公路、城市主干道等，其特点在于车道形状已知、环境动态性强、汽车行驶速度高、留给自动驾驶系统的响应时间较短。同时，在这种场景下，车辆局部轨迹规划的结果为一时空曲线，因此问题的求解需要同时考虑环境的静态约束条件和动态约束条件。针对这类场景，基于采样的局部轨迹规划方法由于其计算效率高，受环境约束条件影响小而取得了较好的效果。目前，这类基于采样的局部轨迹规划方法包括随机采样和确定性采样两种方式：

随机采样法包括PRM(Probabilistic Roadmap，随机路线图)和RRT(Rapidly-exploring Random Tree，快速搜索随机树)。这类方法通过随机采样的方式实现对环境的逐步随机探索，能够快速得到局部轨迹的同时还能避免陷入局部最优。然而由于其探索过程具备随机性，因此导致这种方法的规划结果难以预测，在实际工业应用中存在一定的不可控性。

确定性采样法通过生成一定数量的采样轨迹的方式来达到遍历车辆的所有运动可能性的目的，以此获得满足约束条件的相对最优解。这种方法一般通过忽略约束条件或采用参数化轨迹模型的方式来快速计算得到大量的采样轨迹，具有较高的规划效率。目前生成采样轨迹的方法主要分为两种：(1)给定车辆的控制输入(例如前轮转角，速度以及加速度等)，通过对车辆运动学方程进行积分运算得到采样轨迹。这类方法一般通过假定车速恒定，对车辆的前轮转角进行确定性采样得到不同的采样轨迹。然而由于这类方法没有考虑道路形状约束条件，因此容易导致产生大量的无效采样轨迹。(2)给定轨迹的终点，通过插值曲线得到采样轨迹。这类方法常采用的插值曲线包括Dubin曲线，Spline曲线以及五次多项式曲线等。其中，Dubin曲线曲率不连续；Spline曲线需要对曲率积分，计算量大；五次多项式曲线具有6个待定的采样值，采样空间维数较高。

综上所述，现有基于采样轨迹的局部轨迹规划方法虽然在结构化场景下得到了广泛的应用，但仍存在一定的弊端。

发明内容

本发明的目的是为了在结构化道路场景下快速计算生成无碰撞的轨迹，提出了一种结构化道路场景下的车辆局部轨迹规划方法，本发明具有计算效率高，对复杂约束条件适应性强的优点。

为了实现上述目的，本发明采用如下技术方案：

本发明提出的一种结构化道路场景下的车辆局部轨迹规划方法，其特征在于，包括以下步骤：

1)根据车辆当前所处车道的车道参考线信息，建立动态的参考坐标系；获取车辆当前时刻在笛卡尔坐标系下的运动状态向量，由车辆初始时刻在笛卡尔坐标系下的纵向和横向位置、速度和加速度组成；根据笛卡尔坐标系与参考坐标系之间的投影关系确定车辆在参考坐标系下的运动状态向量；

2)将车速划分为第一速度区间和第二速度区间，所述第一速度区间内的各速度均高于第二速度区间内各速度；若车辆当前时刻的速度属于第一速度区间，则执行步骤3)，若车辆当前时刻的速度属于第二速度区间，则执行步骤4)；

3)生成车辆在第一速度区间的采样轨迹集合Track_F1，该采样轨迹集合由车辆在参考坐标系下的纵向采样轨迹集合Track_{F1_s}和横向采样轨迹集合Track_{F1_r}合成转化获得，具体步骤如下：

3-1)生成车辆在第一速度区间的纵向采样轨迹集合Track_{F1_s}

车辆的纵向采样轨迹集合Track_{F1_s}由多条采样轨迹组成，设各采样轨迹的形式均为相同且关于时间的4次多项式；令各采样轨迹起点处的边界约束条件为车辆当前时刻在参考坐标系下的纵向位置、速度和加速度；对采样轨迹终点处的纵向速度，以及采样轨迹终点与起点之间的时间差进行均匀采样，同时取各采样轨迹终点处的加速度均为0，确定各采样轨迹终点处的边界约束条件；根据各采样轨迹的边界约束条件确定4次多项式的各系数，得到满足起点、终点边界约束条件的所有采样轨迹；将该所有采样轨迹中含有车速不属于[0，V_max]范围和该所有采样轨迹中含有加速度不属于[-a_dec，a_max]范围的采样轨迹剔除，由剩余的采样轨迹构成车辆的纵向采样轨迹集合Track_{F1_s}；V_max为车辆所在车道规定的最大限速，a_dec，a_max分别为车辆的最大制动减速度和最大加速度；

3-2)生成车辆在第一速度区间的横向采样轨迹集合Track_{F1_r}

车辆的横向采样轨迹集合Track_{F1_r}由多条采样轨迹组成，设各采样轨迹的形式均为相同且以时间作为变量的5次多项式；令各采样轨迹起点处的边界约束条件为车辆当前时刻在参考坐标系下的横向位置、速度和加速度；对采样轨迹终点处的横向位置，以及采样轨迹终点与起点之间的时间差进行均匀采样，同时取各采样轨迹终点处的速度和加速度均为0，确定各目标终点的采样轨迹终点约束条件；根据各采样轨迹的边界约束条件确定5次多项式的各系数，得到满足起点、终点边界约束条件的所有采样轨迹；将该所有采样轨迹中含有横向位置不属于[R_min，R_max]范围的采样轨迹剔除，由剩余的采样轨迹构成车辆的横向采样轨迹集合Track_{F1_r}，R_min，R_max分别表示当前车道外侧边界与内侧边界在参考坐标系下的横坐标值；

3-3)从步骤3-1)生成的纵向采样轨迹集合Track_{F1_s}和步骤3-2)生成的横向采样轨迹集合Track_{F1_r}中选取对应的纵向采样轨迹和横向采样轨迹进行合成，得到车辆在第一速度区间的采样轨迹集合Track_F1，执行步骤5)；

4)生成车辆在第二速度区间的采样轨迹集合Track_F2，该采样轨迹集合由车辆在参考坐标系下的纵向采样轨迹集合Track_{F2_s}和车辆的横向采样轨迹集合Track_{F2_r}组成

4-1)生成车辆在第二速度区间的纵向采样轨迹集合Track_{F2_s}

车辆的纵向采样轨迹集合Track_{F2_s}由多条采样轨迹组成，设各采样轨迹的形式均为相同且关于时间的3次多项式；令各采样轨迹起点处的边界约束条件为车辆当前时刻在参考坐标系下的纵向位置、速度和加速度；对采样轨迹终点处的纵向位置，以及采样轨迹终点与起点之间的时间差进行均匀采样，确定各采样轨迹终点处的边界约束条件；根据各采样轨迹的边界约束条件确定3次多项式的各系数，得到满足起点、终点边界约束条件的所有采样轨迹；将该所有采样轨迹中含有车速不属于[0，V_max]范围和该所有采样轨迹中含有加速度不属于[-a_dec，a_max]范围的采样轨迹剔除，由剩余的采样轨迹构成车辆的纵向采样轨迹集合Track_{F2_s}；

4-2)生成车辆在第二速度区间的横向采样轨迹集合Track_{F2_r}

车辆的横向采样轨迹集合Track_{F2_r}由多条采样轨迹组成，设各采样轨迹均为相同且关于参考坐标系纵坐标的5次多项式；令各采样轨迹起点处的边界约束条件为车辆当前时刻在参考坐标系下的横向位置以及其横向位移相对于参考坐标系纵轴的一阶导数和二阶导数，对采样轨迹终点处的横向位置，以及采样轨迹终点所对应的纵向位置进行均匀采样，同时取采样轨迹终点处的横向位移相对于参考坐标系纵轴的一阶导数和二阶导数均为0，确定各采样轨迹终点处的边界约束条件；根据各采样轨迹的边界约束条件确定5次多项式的各系数，得到满足起点、终点边界约束条件的所有采样轨迹；将所有采样轨迹中含有横向位置不属于[R_min，R_max]范围内的采样轨迹剔除，由剩余的采样轨迹构成车辆的横向采样轨迹集合Track_{F2_r}；

4-3)将第二速度区间下车辆的纵向采样轨迹集合Track_{F2_s}与横向采样轨迹集合Track_{F2_r}中具有相同纵向终点位置的采样轨迹进行合成，并将合成后的采样轨迹转换为与第一速度区间下的合成轨迹相同的形式，得到车辆在第二速度区间的采样轨迹集合Track_F2，执行步骤5)；

5)将步骤3-3)或步骤4-3)得到的采样轨迹集合中的采样轨迹转换到笛卡尔坐标系下，得到笛卡尔坐标系下的采样轨迹集合。

6)对步骤5)所得到的各采样轨迹进行曲率检测和碰撞检测，将满足无碰撞约束条件的所有采样轨迹作为车辆可行轨迹集合；

7)设步骤6)得到的车辆可行采样轨迹集合中各采样轨迹在参考坐标系下的轨迹代价函数为轨迹纵向加速度波动量、轨迹横向加速度波动量、轨迹终点纵向位置与期望纵向位置的偏差、轨迹终点纵向速度与期望速度的偏差以及轨迹终点横向位置与期望横向位置的偏差之和，将代价最小的车辆可行采样轨迹作为最优轨迹。

本发明的特点及有益效果：

由于在较低车速下，忽略车辆横纵向运动之间的耦合关系容易导致大部分采样轨迹曲率过大而产生计算浪费，而在中高车速下，车辆横纵向运动之间的耦合关系可以相对忽略，因此本发明通过分别构造高速和低速下的采样轨迹以提高采样有效率(有效采样轨迹占所有采样轨迹的比例)，避免生成大量无效采样轨迹造成计算浪费。

进一步地，由于基于给定终点约束的采样方法采用的插值曲线均存在一定弊端。本方法同样采用了多项式函数作为插值曲线，但为了避免五次多项式函数插值曲线存在的采样空间维数过高的问题，本方法采用了结合四次多项式和五次多项式的高速轨迹、结合三次多项式和五次多项式的低速轨迹，同时通过限制轨迹终点处的横纵向加减速和横向运动来进一步减少采样空间维数，同时符合结构化道路下的驾驶目标。

此外，本发明通过构建考虑了车辆局部驾驶目标(如目标速度和目标位置)的代价函数，能够适应于不同驾驶行为。

综上，本发明能够极大提高局部轨迹规划问题的求解速度，同时能够避免由于交通环境造成的复杂约束条件对于求解局部轨迹规划问题的影响，应用范围更广。

附图说明

图1为本发明实施例所提供的车辆局部轨迹规划流程示意图；

图2为本发明实施例所提供的基于车道的参考坐标系示意图；

图3为本发明实施例所提供的车辆运动状态投影到参考坐标系的示意图；

图4为本发明实施例所提供的横纵向轨迹合成示意图；

图5为本发明实施例所提供的采样轨迹坐标转换示意图；

图6为本发明实施例所提供的车辆碰撞检测范围示意图。

具体实施方式

为使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步的详细说明。应当理解，此处所描述的具体实施方式仅仅用以解释本发明，并不限定本发明的保护范围。

为了更好地理解本发明，以下详细阐述一个本发明一种结构化道路场景下的车辆轨迹规划方法的应用实例。

如图1所示，本实施例所提供的一种结构化道路场景下的车辆轨迹规划方法主要分为以下部分：首先通过建立一种基于车道参考线的参考坐标系，得到车辆当前时刻在该参考坐标系下的运动状态向量。接着分别生成参考坐标系下的纵向采样轨迹集合和横向采样轨迹集合，纵向采样轨迹集合内的所有采样轨迹的曲线形式相同，横向采样轨迹集合内所有采样轨迹的曲线形式相同。然后将横纵向采样轨迹进行合成。再然后将合成后的采样轨迹转换回笛卡尔坐标系下，并剔除其中不满足轨迹曲率和碰撞检测的采样轨迹。最后计算每一条采样轨迹的代价，其中代价最小的轨迹即为本发明所得到的最优轨迹。本方法实施例选用但不限于双车道场景，具体步骤如下：

步骤1)根据车辆当前所处车道的车道参考线信息(可以是与车道中心线平行的任意线段，在结构化道路场景下，车道参考线信息为已知条件，本实施例中取车道外侧边界线作为车道参考线)，建立参考坐标系O_sr-sr(本实施例中参考坐标系采用Frenet坐标系)，参考坐标系O_sr-sr的原点为车道参考线起点；获取车辆当前时刻在笛卡尔坐标系O-xy下的运动状态向量，由车辆当前时刻在笛卡尔坐标系下的纵向和横向位置、速度和加速度组成，根据笛卡尔坐标系与参考坐标系之间的投影关系确定车辆在参考坐标系下的运动状态向量。

参见图2，其中，参考坐标系的s轴与车道参考线重合，r轴与车道参考线垂直。设X(s)，Y(s)以及

分别表示车道参考线上与车道参考线起点(即参考坐标系原点)之间的弧长为s的点在笛卡尔坐标系下的横坐标、纵坐标以及车道参考线在该点处的切线方向与笛卡尔坐标系x轴的夹角。在给定车道参考线路径点的前提下，参考坐标系对应的X(s)，Y(s)以及

均可以通过曲线插值的方式计算得到。

参见图3，记车辆当前时刻在笛卡尔坐标系O-xy下的运动状态向量为

分别表示当前时刻车辆在笛卡尔坐标系下的横向位置、速度和加速度，y0，

分别为当前时刻车辆在笛卡尔坐标系下的纵向位置、速度和加速度，其中，符号

和符号

分别表示变量对于时间的一阶导数和二阶导数。计车辆当前时刻在参考坐标系下的运动状态向量为

s₀为车辆当前时刻距离车道参考线上距离最近的点P₀与车道参考线起点(即参考坐标系原点)之间的弧长，

分别为车辆当前时刻相对于参考坐标系的纵向速度和纵向加速度；r₀为车辆当前时刻与上述点P₀之间的距离，

分别为相对于参考坐标系的横向速度和横向加速度；计算公式分别如下：

式中，X(s₀)，Y(s₀)分别表示车辆当前时刻距离车道参考线上最近的点P₀在笛卡尔坐标系中的横坐标和纵坐标；

表示车道参考线在上述点P₀处的切线方向与笛卡尔坐标系x轴的夹角。

步骤2)根据速度大小将车速划分为第一速度区间和第二速度区间，所述第一速度区间内的各速度均高于第二速度区间内各速度，本实施例中第一速度区间为[0，5)，第二速度区间为[5，+∞)；若车辆当前时刻的速度属于第一速度区间，则执行步骤3)，若车辆当前时刻的速度属于第二速度区间，则执行步骤4)。

在较低车速下，忽略车辆横纵向运动之间的耦合关系容易导致大部分采样轨迹曲率过大而产生计算浪费；而中高车速下，车辆横纵向运动之间的耦合关系可以相对忽略，因此可以通过根据速度区间分别规划横纵向独立规划的方式简化计算量。

步骤3)生成车辆在第一速度区间的采样轨迹集合Track_F1，该采样轨迹集合由车辆在参考坐标系下的纵向采样轨迹集合Track_{F1_s}和横向采样轨迹集合Track_{F1_r}合成转化获得，如图4所示，具体步骤如下：

步骤3-1)生成车辆在第一速度区间的纵向采样轨迹集合Track_{F1_s}

车辆的纵向采样轨迹集合Track_{F1_s}由多条采样轨迹组成，设各采样轨迹的形式均为相同且关于时间的4次多项式；令各采样轨迹起点处的边界约束条件为车辆当前时刻在参考坐标系下的纵向位置、速度和加速度；对采样轨迹终点处的纵向速度，以及采样轨迹终点与起点之间的时间差进行均匀采样，同时取各采样轨迹终点处的加速度均为0，确定各采样轨迹终点处的边界约束条件；根据各采样轨迹的边界约束条件确定4次多项式的各系数，得到满足起点、终点边界约束条件的所有采样轨迹；将该所有采样轨迹中含有车速不属于[0，v_max]范围和含有加速度不属于[-a_dec，a_max]范围的采样轨迹剔除，由剩余的采样轨迹构成车辆的纵向采样轨迹集合Track_{F1_s}；v_max为车辆所在车道规定的最大限速，a_dec，a_max分别为车辆的最大制动减速度和最大加速度。具体步骤如下：

3-1-1)确定车辆纵向采样轨迹集合Track_{F1_s}中各采样轨迹的函数形式

在第一速度区间内(即中高速条件下)，简化认为车辆的纵向运动与横向运动无关，且根据采样轨迹边界约束条件个数设各采样轨迹均为以时间作为变量的4次多项式，记为s₁(t)＝a₀+a₁t+a₂t²+a₃t³+a₄t⁴，a₀～a₄为4次多项式的系数；

令各采样轨迹起点处的边界约束条件为步骤1)确定的车辆投影到参考坐标系下的运动状态向量

中的纵向运动状态量，分别如下：

s₁(t)|_t＝0＝s₀ (6)

令各采样轨迹终点处的边界约束条件如下：

式中，T_i表示在范围[T_min，T_max]内以ΔT作为采样间隔(采样间隔ΔT的取值根据实际情况确定，ΔT越小采样轨迹越多，方法所需计算量就越大，同时规划结果会更好。下文中Δr，Δs和Δs的取值类似)进行均匀采样得到的采样值，其中T_min，T_max分别为车辆完成一次换道操作所需的最短、最长时间；

表示在范围[0，V_max]内以

作为采样间隔进行均匀采样得到的纵向速度采样值，其中V_max表示车辆当前所在车道的最大限速。式(10)表示本方法期望车辆尽量避免产生过多的速度波动量，因此在轨迹规划目标即轨迹终点边界条件的选取中忽略车辆的纵向加减速运动。同时，考虑到车辆在中高速下的纵向运动范围较大，因此在以上纵向轨迹的终点边界条件中不对轨迹的终点纵向位置

作硬性约束。

根据如下公式求取表示纵向采样轨迹的4次多项式中的各项系数：

从而得到第一速度区间下满足起点、终点边界约束条件的所有纵向采样轨迹。

3-1-2)剔除步骤3-1-1)确定的所有纵向采样轨迹中含有车速

不属于[0，V_max]范围和含有加速度

不属于[-a_dec，a_max]范围的采样轨迹，由剩余的采样轨迹构成车辆的纵向采样轨迹集合Track_{F1_s}。

步骤3-2)生成第一速度区间下车辆的横向采样轨迹集合Track_{F1_r}

车辆的横向采样轨迹集合Track_{F1_r}由多条采样轨迹组成，设各采样轨迹的形式均为相同且以时间作为变量的5次多项式；令各采样轨迹起点处的边界约束条件为车辆当前时刻在参考坐标系下的横向位置、速度和加速度；对采样轨迹终点处的横向位置，以及采样轨迹终点与起点之间的时间差进行均匀采样，同时取各采样轨迹终点处的速度和加速度均为0，确定各采样轨迹终点处的边界约束条件；根据各采样轨迹的边界约束条件确定5次多项式的各系数，得到满足起点、终点边界约束条件的所有采样轨迹；将所有采样轨迹中含有横向位置不属于[R_min，R_max]范围的采样轨迹剔除，由剩余的采样轨迹构成车辆的横向采样轨迹集合Track_{F1_r}；R_min，R_max分别表示当前车道外侧边界与内侧边界在参考坐标系下的横坐标值。具体步骤如下：

3-2-1)确定车辆横向采样轨迹集合Track_{F1_r}中各采样轨迹的函数形式

在第一速度区间内(即中高速条件下)，简化认为车辆的横向运动与纵向运动无关，且根据约束条件个数设各采样轨迹均为以时间作为变量的相同5次多项式，记为r₁(t)＝b₀+b₁t+b₂t²+b₃t³+b₄t⁴+b₅t⁵，b₀～b₅为5次多项式的系数；

令各采样轨迹起点处的边界约束条件为步骤1)确定的车辆投影到参考坐标系下的运动状态向量

中的横向运动状态量，分别如下：

r₁(t)|_t＝0＝r₀ (12)

令各采样轨迹终点处的边界约束条件如下：

式中，T_i取值与步骤3-1-1)相同；r_k表示在范围[R_min，R_max]内以Δr作为采样间隔进行均匀采样得到的横向位置采样值，其中R_min，R_max分别表示车辆当前所在车道的外侧边界与内侧边界在参考坐标系下的横坐标值；式(16)和式(17)表示轨迹规划算法期望车辆尽量避免产生相对于车辆当前所在车道的横向驾驶行为，因此在轨迹规划目标即轨迹终点约束的选取中忽略车辆的横向运动；

根据如下公式求取表示横向采样轨迹的5次多项式中的各系数：

从而得到第一速度区间下满足起点、终点约束的所有横向采样轨迹。

3-2-2)剔除步骤3-2-1)确定的所有横向采样轨迹中不满足车辆所在道路边界约束的横向采样轨迹(即横向采样轨迹上的各采样点均应满足车辆所在道路的边界约束)，由剩余的横向采样轨迹构成车辆在参考坐标系下第一速度区间的横向采样轨迹集合Track_{F1_r}。

步骤3-3)从步骤3-1)生成的纵向采样轨迹集合和步骤3-2)生成的横向采样轨迹集合中选取对应的纵向采样轨迹和横向采样轨迹进行合成，得到参考坐标系下的采样轨迹集合Track_F1，执行步骤5)。其中，轨迹终点与起点间的时间差T_i相同的纵向采样轨迹和横向采样轨迹之间通过两两组合得到参考坐标系下的合成轨迹，如下式所示：

步骤4)生成车辆在第二速度区间的采样轨迹集合Track_F2，该采样轨迹集合由车辆在参考坐标系下的纵向采样轨迹集合Track_{F2_s}和横向采样轨迹集合Track_{F2_r}合成转化获得，如图4所示，具体步骤如下：

步骤4-1)生成车辆在第二速度区间的纵向采样轨迹集合Track_{F2_s}

车辆的纵向采样轨迹集合Track_{F2_s}由多条采样轨迹组成，设各采样轨迹的形式均为相同且关于时间的3次多项式；令各采样轨迹起点处的边界约束条件为车辆当前时刻在参考坐标系下的纵向位置、速度和加速度；对采样轨迹终点处的纵向位置，以及采样轨迹终点与起点之间的时间差进行均匀采样，确定各采样轨迹终点处的边界约束条件；根据各采样轨迹的边界约束条件确定3次多项式的各系数，得到满足起点、终点边界约束条件的所有采样轨迹；将该所有采样轨迹中含有车速不属于[0，v_max]和含有加速度不属于[-a_dec，a_max]范围的采样轨迹剔除，由剩余的采样轨迹构成车辆的纵向采样轨迹集合Track_{F2_s}。具体步骤如下：

4-1-1)确定车辆纵向采样轨迹集合Track_{F2_s}中各采样轨迹的函数形式

在第二速度区间内(即低速条件下)，根据纵向采样轨迹边界约束条件个数设各纵向采样轨迹均为以时间作为变量的3次多项式，记为s₂(t)＝c₀+c₁t+c₂t²+c₃t³，c₀～c₃为3次多项式的系数；

令各纵向采样轨迹起点处的边界约束条件为步骤1)确定的车辆投影到参考坐标系下的运动状态向量

中的纵向运动状态量，分别如下：

s₂(t)|_t＝0＝s₀ (20)

令各纵向采样轨迹终点处的边界约束条件如下：

式中，T_i取值与步骤3-1-1)相同；s_m表示在范围[s₀，s₀+D_slow]内以Δs作为采样间隔进行均匀采样得到的纵向位置采样值，其中D_slow表示车辆在第二速度区间下的最大采样距离，D_slow＝T_max·5，T_max为车辆完成一次换道操作所需的最长时间。

根据如下公式求取表示纵向采样轨迹的3次多项式中的各项系数：

从而得到第二速度区间下满足起点、终点边界约束条件的所有纵向采样轨迹。

4-1-2)剔除步骤4-1-1)确定的所有纵向采样轨迹中含有车速

不属于[0，V_max]范围和含有加速度

不属于[-a_dec，a_max]范围的采样轨迹，由剩余的采样轨迹构成车辆的纵向采样轨迹集合TrackF_{2_s}。

步骤4-2)生成车辆在第二速度区间的横向采样轨迹集合Track_{F2_r}

车辆的横向采样轨迹集合Track_{F2_r}由多条采样轨迹组成，设各采样轨迹的形式均为相同且以纵坐标s作为变量的5次多项式；令各采样轨迹起点处的边界约束条件为车辆当前时刻在参考坐标系下的横向位置以及其横向位移相对于参考坐标系纵轴的一阶导数和二阶导数；对采样轨迹终点处的横向位置，以及采样轨迹终点所对应的纵向位置进行均匀采样，同时取各采样轨迹终点处的横向位移相对于参考坐标系纵轴的一阶导数和二阶导数均为0，确定各采样轨迹终点处的边界约束条件；根据各采样轨迹的边界约束条件确定5次多项式的各系数，得到满足起点、终点边界约束条件的所有采样轨迹；将所有采样轨迹中含有横向位置不属于[R_min，R_max]范围内的采样轨迹剔除，由剩余的采样轨迹构成车辆的横向采样轨迹集合Track_{F2_r}。具体步骤如下：

4-2-1)确定车辆横向采样轨迹集合Track_{F2_r}中各采样轨迹的函数形式

在第二速度区间内(即低速条件下)，车辆的横向运动与纵向运动之间的耦合关系无法忽略，且根据约束条件个数设各采样轨迹均为以横向位置s作为变量的5次多项式，记为r₂(s)＝d₀+d₁s+d₂s²+d₃s³+d₄s⁴+d₅s⁵，d₀～d₅为5次多项式的系数；

令各采样轨迹起点处的边界约束条件为步骤1)确定的车辆投影到参考坐标系下的运动状态向量

中的横向运动状态量，分别如下：

式中符号(·)′和符号(·)″分别表示变量对于参考坐标系纵坐标s的一阶导数和二阶导数。

令各采样轨迹终点处的边界约束条件如下：

式中，s_m的取值与步骤4-1-1)相同；r_k的取值与步骤3-2-1)相同。式(29)和式(30)表示轨迹规划算法期望车辆尽量避免产生相对横向运动，因此在轨迹规划目标即轨迹终点约束的选取中忽略车辆的横向运动。

根据如下公式求取表示横向采样轨迹的5次多项式中的各系数：

从而得到第二速度区间下满足起点、终点边界约束条件的所有横向采样轨迹。

4-2-2)剔除步骤4-2-1)确定的所有横向采样轨迹中不满足车辆所在道路边界约束的横向采样轨迹，由剩余的横向采样轨迹构成车辆在参考坐标系下第二速度区间的横向采样轨迹集合Track_{F2_r}。

步骤4-3)从步骤4-1)生成的纵向采样轨迹集合和步骤4-2)生成的横向采样轨迹集合中选取对应的纵向采样轨迹和横向采样轨迹进行合成，并将合成后的采样轨迹转换为与第一速度区间下的合成轨迹相同的形式，得到参考坐标系下的采样轨迹集合Track_F2，执行步骤5)。其中，轨迹终点处的纵向位置s_m相同的纵向采样轨迹和横向采样轨迹之间通过两两组合得到参考坐标系下的合成轨迹，如下式所示：

通过下式将第二速度区间内(即低速条件下)的采样轨迹转换为与第一速度区间内(即中高速条件下)的采样轨迹具有相同的形式：

r₃(t)＝r₂(s₂(t)) (33)

步骤5)将步骤3)或步骤4)生成的参考坐标系下的横纵向采样轨迹集合通过坐标转换得到笛卡尔坐标系下的采样轨迹集合，如图5所示。

可以根据笛卡尔坐标系与参考坐标系之间的投影关系通过下式(36)-(41)转换为车辆在笛卡尔坐标系下的运动状态量

步骤6)在笛卡尔坐标系下对采样轨迹进行曲率和碰撞检测，将满足无碰撞约束条件的所有采样轨迹作为车辆可行轨迹集合。该步骤具体包括：

步骤6-1)对轨迹的曲率进行检测。根据参数曲线的定义，通过下式(42)计算采样轨迹的曲率κ：

考虑到车辆运动轨迹受运动学约束限制，因此需要对采样轨迹上的曲率进行限制，如下式(43)所示：

κ≤κ_max (43)

式(43)中，κ_max表示车辆运动轨迹可达到的最大曲率，取车辆最小转弯半径的倒数，剔除κ＞κ_max的采样轨迹。

步骤6-2)对满足κ≤κ_max约束的各采样轨迹进行碰撞检测，将满足无碰撞约束条件的所有采样轨迹作为车辆可行轨迹集合。

将各车辆分别用长L_car、宽W_car的矩形目标框表征，并分别构建用以3行、2列排布的六个圆形碰撞圈，各碰撞圈的半径均取车辆矩形目标框长度的六分之一，每列中3个碰撞圈至车辆矩形目标框对应侧长边的距离均为目标框宽度的四分之一，如图6所示。对于各碰撞圈的圆心位置通过下式(57)-(62)计算得到：

式中，X₀和Y₀表示车辆形心在笛卡尔坐标系下的横、纵坐标；θ_car表示车辆的偏航角，即车辆的纵轴与x轴方向的夹角；W_car和L_car分别表示车辆矩形目标框的宽和长；其中，周车的X₀，Y₀，θ_car，W_car和L_car均通过自车上装载的传感器采集相应数据后经环境感知算法处理得到(为公知技术)；自车的X₀和Y₀由步骤5)得到的笛卡尔坐标系下的采样轨迹给出；自车的偏航角θ_car通过下式计算得到：

式中，

表示上一时刻所对应的自车偏航角。

因此，选择采样轨迹应满足如下式(51)所示的无碰撞约束条件：

上式中，

和

分别表示t时刻下用以表示自车的第i个碰撞圈的圆心坐标，

为该碰撞圈的半径；

和

表示t时刻下用以表示自车周围第n辆车的第j个碰撞圈的圆心坐标，

为该碰撞圈的半径；D_min表示自车与他车之间的最小距离(D_min取值越大越，本方法所取得的规划结果越安全(离周车越远)，但同时也导致更为保守)；Δt为轨迹离散化采样的时间间隔；T表示轨迹终点与起点间的时间差。

将满足无碰撞约束条件的所有采样轨迹作为可行轨迹集合。

步骤7)从步骤6)确定的可行轨迹集中选择最优轨迹。该步骤具体包括：

步骤7-1)按照下式计算可行轨迹集中各采样轨迹的代价：

上式中，

为可行轨迹集中采样轨迹

的代价函数，为轨迹纵向加速度

波动量、轨迹横向加速度

波动量、轨迹终点纵向位置与期望纵向位置的偏差、轨迹终点纵向速度与期望速度的偏差以及轨迹终点横向位置与期望横向位置的偏差之和；k_lon，k_lat，k_s，k_v和k_r分别表示采样轨迹各惩罚项的比例系数；s_p，

和r_p分别表示采样轨迹终点处的纵向位置，纵向速度和横向位置，角标p取1或2，p＝1时对应第一速度区间，p＝2时对应第二速度区间；r^*表示车辆期望达到的横向位置，在结构化道路下可以取r^*＝R_center，R_center表示车道中心线在参考坐标系下的横坐标；s^*和

分别表示车辆期望达到的纵向位置和纵向速度，在跟车时，其值可以通过下式计算：

式中，T表示采样轨迹终点与起点间的时间差；s_f(T)表示T时刻时对前车在参考坐标系下的纵坐标的预测值；

表示T时刻时对前车在参考坐标系下的纵向速度的预测值；τ为安全跟车车头时距，根据一般交通规定取τ＝2～3s。

在准备停车时，s^*和

可以通过下式计算：

式中，s_stop表示停车点在参考坐标系下的纵坐标；L_ego表示自车矩形目标框的长度。

在自由行驶时(无前车且不准备停车)，s^*和

可以通过下式计算：

s^*＝s_p (56)

步骤7-2)根据式52)计算得到的各轨迹代价，对所有可行轨迹进行排序，选其中的代价最小的轨迹作为最优轨迹。

Claims (8)

1.一种结构化道路场景下的车辆局部轨迹规划方法，其特征在于，包括以下步骤：

1)根据车辆当前所处车道的车道参考线信息，建立动态的参考坐标系；获取车辆当前时刻在笛卡尔坐标系下的运动状态向量，由车辆初始时刻在笛卡尔坐标系下的纵向和横向位置、速度和加速度组成；根据笛卡尔坐标系与参考坐标系之间的投影关系确定车辆在参考坐标系下的运动状态向量；

2)将车速划分为第一速度区间和第二速度区间，所述第一速度区间内的各速度均高于第二速度区间内各速度；若车辆当前时刻的速度属于第一速度区间，则执行步骤3)，若车辆当前时刻的速度属于第二速度区间，则执行步骤4)；

3)生成车辆在第一速度区间的采样轨迹集合Track_F1，该采样轨迹集合由车辆在参考坐标系下的纵向采样轨迹集合Track_{F1_s}和横向采样轨迹集合Track_{F1_r}合成转化获得，具体步骤如下：

3-1)生成车辆在第一速度区间的纵向采样轨迹集合Track_{F1_s}

车辆的纵向采样轨迹集合Track_{F1_s}由多条采样轨迹组成，设各采样轨迹的形式均为相同且关于时间的4次多项式；令各采样轨迹起点处的边界约束条件为车辆当前时刻在参考坐标系下的纵向位置、速度和加速度；对采样轨迹终点处的纵向速度，以及采样轨迹终点与起点之间的时间差进行均匀采样，同时取各采样轨迹终点处的加速度均为0，确定各采样轨迹终点处的边界约束条件；根据各采样轨迹的边界约束条件确定4次多项式的各系数，得到满足起点、终点边界约束条件的所有采样轨迹；将该所有采样轨迹中含有车速不属于[0，V_max]范围和该所有采样轨迹中含有加速度不属于[-a_dec，a_max]范围的采样轨迹剔除，由剩余的采样轨迹构成车辆的纵向采样轨迹集合Track_{F1_s}；V_max为车辆所在车道规定的最大限速，a_dec，a_max分别为车辆的最大制动减速度和最大加速度；

3-2)生成车辆在第一速度区间的横向采样轨迹集合Track_{F1_r}

车辆的横向采样轨迹集合Track_{F1_r}由多条采样轨迹组成，设各采样轨迹的形式均为相同且以时间作为变量的5次多项式；令各采样轨迹起点处的边界约束条件为车辆当前时刻在参考坐标系下的横向位置、速度和加速度；对采样轨迹终点处的横向位置，以及采样轨迹终点与起点之间的时间差进行均匀采样，同时取各采样轨迹终点处的速度和加速度均为0，确定各目标终点的采样轨迹终点约束条件；根据各采样轨迹的边界约束条件确定5次多项式的各系数，得到满足起点、终点边界约束条件的所有采样轨迹；将该所有采样轨迹中含有横向位置不属于[R_min，R_max]范围的采样轨迹剔除，由剩余的采样轨迹构成车辆的横向采样轨迹集合Track_{F1_r}，R_min，R_max分别表示当前车道外侧边界与内侧边界在参考坐标系下的横坐标值；

3-3)从步骤3-1)生成的纵向采样轨迹集合Track_{F1_s}和步骤3-2)生成的横向采样轨迹集合Track_{F1_r}中选取对应的纵向采样轨迹和横向采样轨迹进行合成，得到车辆在第一速度区间的采样轨迹集合Track_F1，执行步骤5)；

4)生成车辆在第二速度区间的采样轨迹集合Track_F2，该采样轨迹集合由车辆在参考坐标系下的纵向采样轨迹集合Track_{F2_s}和车辆的横向采样轨迹集合Track_{F2_r}组成

4-1)生成车辆在第二速度区间的纵向采样轨迹集合Track_{F2_s}

车辆的纵向采样轨迹集合Track_{F2_s}由多条采样轨迹组成，设各采样轨迹的形式均为相同且关于时间的3次多项式；令各采样轨迹起点处的边界约束条件为车辆当前时刻在参考坐标系下的纵向位置、速度和加速度；对采样轨迹终点处的纵向位置，以及采样轨迹终点与起点之间的时间差进行均匀采样，确定各采样轨迹终点处的边界约束条件；根据各采样轨迹的边界约束条件确定3次多项式的各系数，得到满足起点、终点边界约束条件的所有采样轨迹；将该所有采样轨迹中含有车速不属于[0，V_max]范围和该所有采样轨迹中含有加速度不属于[-a_dec，a_max]范围的采样轨迹剔除，由剩余的采样轨迹构成车辆的纵向采样轨迹集合Track_{F2_s}；

4-2)生成车辆在第二速度区间的横向采样轨迹集合Track_{F2_r}

车辆的横向采样轨迹集合Track_{F2_r}由多条采样轨迹组成，设各采样轨迹均为相同且关于参考坐标系纵坐标的5次多项式；令各采样轨迹起点处的边界约束条件为车辆当前时刻在参考坐标系下的横向位置以及其横向位移相对于参考坐标系纵轴的一阶导数和二阶导数，对采样轨迹终点处的横向位置，以及采样轨迹终点所对应的纵向位置进行均匀采样，同时取采样轨迹终点处的横向位移相对于参考坐标系纵轴的一阶导数和二阶导数均为0，确定各采样轨迹终点处的边界约束条件；根据各采样轨迹的边界约束条件确定5次多项式的各系数，得到满足起点、终点边界约束条件的所有采样轨迹；将所有采样轨迹中含有横向位置不属于[R_min，R_max]范围内的采样轨迹剔除，由剩余的采样轨迹构成车辆的横向采样轨迹集合Track_{F2_r}；

4-3)将第二速度区间下车辆的纵向采样轨迹集合Track_{F2_s}与横向采样轨迹集合Track_{F2_r}中具有相同纵向终点位置的采样轨迹进行合成，并将合成后的采样轨迹转换为与第一速度区间下的合成轨迹相同的形式，得到车辆在第二速度区间的采样轨迹集合Track_F2，执行步骤5)；

5)将步骤3-3)或步骤4-3)得到的采样轨迹集合中的采样轨迹转换到笛卡尔坐标系下，得到笛卡尔坐标系下的采样轨迹集合。

6)对步骤5)所得到的各采样轨迹进行曲率检测和碰撞检测，将满足无碰撞约束条件的所有采样轨迹作为车辆可行轨迹集合；

7)设步骤6)得到的车辆可行采样轨迹集合中各采样轨迹在参考坐标系下的轨迹代价函数为轨迹纵向加速度波动量、轨迹横向加速度波动量、轨迹终点纵向位置与期望纵向位置的偏差、轨迹终点纵向速度与期望速度的偏差以及轨迹终点横向位置与期望横向位置的偏差之和，将代价最小的车辆可行采样轨迹作为最优轨迹。

2.根据权利要求1所述的车辆局部轨迹规划方法，其特征在于，令步骤3-1)中，纵向采样轨迹集合Track_{F1_s}内的各采样轨迹均为s₁(t)＝a₀+a₁t+a₂t²+a₃t³+a₄t⁴，系数a₀～a₄按照以下公式计算：

式中，T_i表示在范围[T_min，T_max]内进行等间隔均匀采样得到的时间采样值，其中T_min，T_max分别为车辆完成一次换道操作所需的最短、最长时间；s₀为车辆当前时刻距离车道参考线上距离最近的点与车道参考线起点之间的弧长；符号

和符号

分别表示变量对于时间的一阶导数和二阶导数，

分别为车辆当前时刻相对于参考坐标系的纵向速度和纵向加速度；

表示在范围[0，V_max]内进行等间隔均匀采样得到的纵向速度采样值。

3.根据权利要求1所述的车辆局部轨迹规划方法，其特征在于，令步骤3-2)中，横向采样轨迹集合Track_{F1_r}内的各采样轨迹均为r₁(t)＝b₀+b₁t+b₂t²+b₃t³+b₄t⁴+b₅t⁵，系数b₀～b₅按照以下公式计算：

式中，T_i表示在范围[T_min，T_max]内进行等间隔均匀采样得到的时间采样值，其中T_min，T_max分别为车辆完成一次换道操作所需的最短、最长时间；r₀为车辆当前时刻与点P₀之间的距离，P₀为车辆当前时刻距离车道参考线上距离最近的点；符号

和符号

分别表示变量对于时间的一阶导数和二阶导数，

分别为相对于参考坐标系的横向速度和横向加速度；r_k表示在范围[R_min，R_max]内进行等间隔均匀采样得到的横向位置采样值，R_min，R_max分别表示车辆当前所在车道的外侧边界与内侧边界在参考坐标系下的横坐标值。

4.根据权利要求1所述的车辆局部轨迹规划方法，其特征在于，令步骤4-1)中，纵向采样轨迹集合Track_{F2_s}内的各采样轨迹均为s₂(t)＝c₀+c₁t+c₂t²+c₃t³，系数c₀～c₃按照以下公式计算：

式中，T_i表示在范围[T_min，T_max]内进行等间隔均匀采样得到的时间采样值，其中T_min，T_max分别为车辆完成一次换道操作所需的最短、最长时间；s₀为车辆当前时刻距离车道参考线上距离最近的点与车道参考线起点之间的弧长；符号

和符号

分别表示变量对于时间的一阶导数和二阶导数，

分别为车辆当前时刻相对于参考坐标系的纵向速度和纵向加速度；s_m表示在范围[s₀，s₀+D_slow]内进行等间隔均匀采样得到的纵向位置采样值，D_slow表示车辆在第二速度区间下的最大采样距离，D_slow＝T_max·5。

5.根据权利要求1所述的车辆局部轨迹规划方法，其特征在于，令步骤4-2)中，横向采样轨迹集合Track_{F2_r}内的各采样轨迹均为r₂(s)＝d₀+d₁s+d₂s²+d₃s³+d₄s⁴+d₅s⁵，系数d₀～d₅按照以下公式计算：

式中：

符号(·)′和符号(·)″分别表示变量对于参考坐标系纵坐标s的一阶导数和二阶导数，

按照以下公式计算：

其中，符号

和符号

分别表示变量对于时间的一阶导数和二阶导数；s₀为车辆当前时刻距离车道参考线上距离最近的点P₀与车道参考线起点之间的弧长，

分别为车辆当前时刻相对于参考坐标系的纵向速度和纵向加速度；r₀为车辆当前时刻与点P₀之间的距离，

分别为相对于参考坐标系的横向速度和横向加速度；

r_k表示在范围[R_min，R_max]内进行等间隔均匀采样得到的横向位置采样值，R_min，R_max分别表示车辆当前所在车道的外侧边界与内侧边界在参考坐标系下的横坐标值；

s_m表示在范围[s₀，s₀+D_slow]内进行等间隔均匀采样得到的纵向位置采样值，D_slow表示车辆在第二速度区间下的最大采样距离，D_slow＝T_max·5，T_max为车辆完成一次换道操作所需的最长时间。

6.根据权利要求1所述的车辆局部轨迹规划方法，其特征在于，步骤6)中所述车辆可行轨迹集合内的各采样轨迹的曲率均小于等于车辆运动轨迹可达到的最大曲率；各采样轨迹的曲率k按照以下公式计算：

式中，符号

和符号

分别表示变量对于时间的一阶导数和二阶导数；

分别为所述车辆可行轨迹集合内的各采样轨迹在笛卡尔坐标系下对应的车辆横向速度、纵向速度、横向加速度和纵向加速度；

对满足上述曲率条件的各采样轨迹进行如下碰撞检测：

将各车辆分别用长L_car、宽W_car的矩形目标框表征，并分别构建用以3行、2列排布的六个圆形碰撞圈，各碰撞圈的半径均取车辆矩形目标框长度的六分之一，每列中3个碰撞圈至车辆矩形目标框对应侧长边的距离均为目标框宽度的四分之一，各碰撞圈的圆心位置通过下式计算得到：

式中，X₀和Y₀表示车辆形心在笛卡尔坐标系下的横、纵坐标；θ_car表示车辆的偏航角；自车的偏航角θ_car通过下式计算得到：

式中，

表示自车上一时刻的偏航角；

令车辆可行轨迹集合内的采样轨迹均满足如下无碰撞约束条件：

式中，

和

分别表示t时刻自车的第i个碰撞圈的圆心坐标，

为自车第i个碰撞圈的半径；

和

表示t时刻自车周围第n辆车的第j个碰撞圈的圆心坐标，

为该第j个碰撞圈的半径；D_min表示自车与他车之间的最小距离；Δt为轨迹离散化采样的时间间隔；T表示轨迹终点与起点间的时间差。

7.根据权利要求1所述的车辆局部轨迹规划方法，其特征在于，步骤7)中，令轨迹代价函数的计算公式如下：

式中，

表示采样轨迹

的代价函数；

k_lon，k_lat，k_s，k_v和k_r分别表示采样轨迹各惩罚项的比例系数；

s_p，

和r_p分别表示采样轨迹终点处的纵向位置，纵向速度和横向位置，角标p取1或2，p＝1时对应第一速度区间，p＝2时对应第二速度区间；

r^*表示车辆期望达到的横向位置，在结构化道路下取r^*＝R_center，R_center表示车道中心线在参考坐标系下的横坐标；

s^*和

分别表示车辆期望达到的纵向位置和纵向速度，在跟车时，其值通过下式计算：

其中，T表示采样轨迹终点与起点间的时间差；s_f(T)表示T时刻时对前车在参考坐标系下的纵坐标的预测值；

表示T时刻时对前车在参考坐标系下的纵向速度的预测值；τ为安全跟车车头时距；

在准备停车时，s^*和

分别通过下式计算：

式中，s_stop表示停车点在参考坐标系下的纵坐标；L_ego表示自车矩形目标框的长度；

在自由行驶时，s^*和

通过下式计算：

s^*＝s_p

8.根据权利要求1～7中任意一项所述的车辆局部轨迹规划方法，其特征在于，所述参考坐标系选用Frenet坐标系。

Images