CN109324620A - 基于车道线平行偏移进行避障及超车的动态轨迹规划方法 - Google Patents

Abstract

本发明涉及一种基于车道线平行偏移进行避障及超车的动态轨迹规划方法，针对不同道路场景建立不同的状态空间，实现不同工况下轨迹规划；采用坐标转换并解耦的方式规划更加精确的后续实时轨迹。有益效果：本发明针对不同道路场景建立不同的状态空间、实现不同工况下的有效精确轨迹规划；采用坐标转换并解耦的方式，为后续轨迹规划方实现更加精确的实时轨迹规划；设置筛选规划轨迹目标函数及约束条件，提高轨迹规划的效率并满足规划轨迹具有安全性、舒适性和稳定性；设置保证路径一致性的评估条件，避免在车辆行驶中由于环境变化对规划路径产生不安全因素，及时发现规划路径的危险警示进行轨迹重规划或在无法选择最优路径时提示驾驶员介入操作。

Description

基于车道线平行偏移进行避障及超车的动态轨迹规划方法

技术领域

本发明属于无人驾驶汽车技术领域，尤其涉及一种基于车道线平行偏移进行避障及超车的动态轨迹规划方法。

背景技术

随着汽车保有量的快速增长，汽车带来的问题也日益明显，交通安全问题日益突出，交通事故发生的原因主要有天气、道路缺陷、汽车故障以及驾驶员操作不当等，无人驾驶技术的发展能够有效解决安全问题，弥补人为原因产生的交通事故。

无人驾驶车辆的轨迹规划方法能够在车路协同的基础上规划有效的行车轨迹，以实现车辆行驶过程中的避障及超车功能，从而大大减小因驾驶员的误判和误操作导致的交通事故。基于无人驾驶技术的车辆路径规划分为全局路径规划和局部路径规划，全局路径规划主要进行的是道路级别的规划，局部路径规划则是根据全局路径完成道路级别规划后，根据行驶过程中周边环境的动态变化实时进行局部轨迹规划。

现有技术中，将局部路径规划的参考轨迹进行平行偏移，使得平行偏移后的路径可以绕开障碍物，存在以下技术难点：1.由于道路中存在曲率，无人车行驶到平行偏移路径之间的平滑连接路径难以规划；2.由于无人车位姿存在不平行于路径切向量的情况，无人车行驶到平行偏移路径之间的平滑连接路径难以规划；3.在生成路径的基础上，难以进行平滑的速度规划及加速度规划，因此很难满足舒适性要求；4.当平行偏移路径跟随环境实时变化时，难以保证路径规划的一致性，不能满足稳定性要求；5.现有技术不适用于在结构化道路上的无人驾驶轨迹规划。

发明内容

本发明的目的在于克服上述技术的不足，而提供一种基于车道线平行偏移进行避障及超车的动态轨迹规划方法，能够实现在多种工况下实时生成跟车、换道、并道、避障、超车等决策命令的对应轨迹，避免因人为不及时的驾驶操作失误带来的交通事故。

本发明为实现上述目的，采用以下技术方案：一种基于车道线平行偏移进行避障及超车的动态轨迹规划方法，其特征是：针对不同道路场景建立不同的状态空间，实现不同工况下轨迹规划；采用坐标转换并解耦的方式为后续实时规划精确轨迹做准备，进而在不同工况下采用五次多项式实时规划平滑轨迹，具体步骤如下：

一、定义场景：

1)开阔环境中的固定路径；

2)仅带有道路边界线并无车道线的结构化道路；

3)具有多条并行车道的结构化道路；

二、根据场景分别选取一条固定路径为轨迹规划的参考路径：

针对场景1)选取该环境中的固定行驶路径为参考路径；

针对场景2)根据道路边界线定位道路中心线为参考路径；

针对场景3)选取保持某一车道行驶时的车道中心线为参考路径，当换道行驶时选取相应行驶的车道中心线为参考路径；

三、通过CARTESIAN到FRENET坐标系的变换方法将二维轨迹规划解耦为两个独立的一维轨迹规划，坐标转换至S、D轴方向，其中S轴定义为参考路径，S轴的法向为D轴；

四、生成S、D轴方向状态空间，即目标状态空间：

针对场景1)和场景2)

在D轴方向上生成目标状态空间(d_t,d_t’,d_t”)，

其中：d_t按照等距间隔取值[min:step:max]以对应不同平行偏移的距离，d_t’、d_t”代表速度与加速度，在场景1)及场景2)中无人驾驶车辆行驶到目标状态空间后，D轴上的速度和加速度均为0；

在S轴上根据车辆当前速度以及目标速度生成状态空间(s_t、s_t’、s_t”)，

其中s_t代表车辆原行驶路径与平行偏移路径的偏移距离；

s_t’代表车辆行驶到目标轨迹后的速度；

s_t”代表车辆行驶到目标轨迹后的加速度；车辆到达目标轨迹空间后保持目标车速匀速行驶，加速度取值为0；

针对场景3)

在D轴上生成目标状态空间(d_t,、d_t’、d_t”)，

其中d_t选取左右两条车道的中心线距离当前车辆行驶车道中心线的距离，取值表示为[left:0.0:right]，其中left为距左侧车道中心线的距离， right为距右侧车道中心线的距离；d_t’、d_t”代表速度与加速度，取值均为 0；

在S轴上的目标状态空间设定与场景1)和场景2)一致；

五、确定目标平行偏移轨迹：

针对不同场景下的每一个目标的配置，生成一条无人驾驶车辆的目标轨迹；所述目标轨迹分为两段，其中：无人驾驶车辆当前位置与平行偏移路径位置之间的平滑连接为第一段路径；达到目标轨迹后保持在该平行偏移路径的位置上行驶为第二段轨迹。

所述目标平行偏移轨迹的筛选目标路径的评估准则：

D轴向的评估准则：

设置筛选目标函数为J₁＝arg min C_d，C_d为D轴向上的损失函数；

其中损失函数为式中k_j1为D轴向的稳定性权重系数，为D轴向约束控制变量的变化率，k_t1为D 轴向的时间权重系数，k_d1为D轴向的距离误差权重系数，T为D轴向轨迹规划所需时间，d_d为D轴向目标轨迹与参考轨迹间的距离；

S轴向的评估准则：

设置筛选目标函数为J₂＝arg min C_s，C_s为S轴向上的损失函数；

其中损失函数为式中k_j2为S轴向的稳定性权重系数，为S轴向约束控制变量的变化率，k_t2为S 轴向的时间权重系数，k_d2为S轴向的距离误差权重系数，T为S轴向轨迹规划所需时间，d_s为S轴向目标轨迹与参考轨迹间的距离；

S轴向的速度约束：式中v_max为车辆设定最大限速；

S轴向的加速度约束：式中a_max为车辆设定最大加速度；

判断轨迹的曲率是否超过车辆的最大转向限制条件；

检测环境信息，判断规划后的轨迹是否会发生碰撞。

步骤五所述的确定目标平行偏移轨迹是否需要重新规划：评估准则为通过评估周围环境以及车辆状态，判断是否需要重新规划轨迹，若需要则从车辆当前状态重新规划，若不需要，则保留上一次规划的部分轨迹，继续进行后续的规划，完成车辆整体的轨迹规划，当***检测到车辆无法选择最优的重规划路径完成避障和超车时，***进入警示状态减速行驶并提示驾驶员介入操作；所述重新规划路径通过触发控制器进行重规划轨迹的评估，评估条件如下：

1)行驶过程中环境变化，检测该规划轨迹引起碰撞；

2)无法求解出步骤5的D轴向及S轴向的目标函数的最优解，导致控制器的损失函数过大，则放弃当前时刻规划的轨迹。

步骤五所述的目标轨迹优选地对于第一段轨迹，车辆的初始状态空间为(s₀、s₀’、s₀”)(d₀、d₀’、d₀”)，目标状态空间为(s_t、s_t’、0)(d_t、0、0)，采用五次多项式对换道轨迹拟合，以保证换道轨迹的平滑连续；对于D轴向，根据初始状态空间(d₀、d₀’、d₀”)以及目标状态空间(d_t、0、0)采用五次多项式规划，并通过设定的时间与目标状态空间的值求得五次多项式的系数；根据S轴向的初始状态空间和目标状态控制空间计算出对应的五次多项式系数，以此完成两个一维方向的目标轨迹的拟合；

对于第二段轨迹，由于D轴方向的参数保持不变，仅进行S轴上的速度和加速度规划即可；所述两段轨迹规划后得到的(s,d)^T变换为(x,y)^T方向的坐标，完成一条完整轨迹的规划。

有益效果：与现有技术相比，本发明适用于当前大多数的道路环境场景，且针对不同道路场景建立不同的状态空间、实现不同工况下的有效精确轨迹规划；采用坐标转换并解耦的方式，为后续轨迹规划方法提供便利，同时可以实现更加精确的实时轨迹规划；设置筛选规划轨迹目标函数及约束条件，提高轨迹规划的效率并满足规划轨迹具有安全性、舒适性和稳定性；设置保证路径一致性的评估条件，避免在车辆行驶过程中由于环境变化对规划路径产生不安全因素，及时发现规划路径的危险警示进行轨迹重规划或在无法选择最优路径时提示驾驶员介入操作以保证无人驾驶车辆的安全。

附图说明

图1为本发明的控制结构框图；

图2为本发明的坐标变换图解；

图3为本发明的重规划轨迹评估策略图；

图4为本发明的轨迹规划纵向位移与换道时间关系图；

图5为本发明的轨迹规划横向位移与换道时间关系图。

具体实施方式

下面结合较佳实施例详细说明本发明的具体实施方式。

详见附图1、2，本实施例提供了一种基于车道线平行偏移进行避障及超车的动态轨迹规划方法，其特征是：针对不同道路场景建立不同的状态空间，实现不同工况下轨迹规划；采用坐标转换并解耦的方式为后续实时规划精确轨迹做准备，进而在不同工况下采用五次多项式实时规划平滑轨迹，具体步骤如下：

一、定义场景：

1)开阔环境中的固定路径；

2)仅带有道路边界线并无车道线的结构化道路；

3)具有多条并行车道的结构化道路；

二、根据场景分别选取一条固定路径为轨迹规划的参考路径：

针对场景1)选取该环境中的固定行驶路径为参考路径；

针对场景2)根据道路边界线定位道路中心线为参考路径；

针对场景3)选取保持某一车道行驶时的车道中心线为参考路径，当换道行驶时选取相应行驶的车道中心线为参考路径；

三、通过CARTESIAN到FRENET坐标系的变换方法将二维轨迹规划解耦为两个独立的一维轨迹规划，坐标转换至S、D轴方向，其中S轴定义为参考路径，S轴的法向为D轴；

四、生成S、D轴方向状态空间，即目标状态空间：

针对场景1)和场景2)

在D轴方向上生成目标状态空间(d_t,d_t’,d_t”)，

其中：d_t按照等距间隔取值[min:step:max]以对应不同平行偏移的距离，d_t’、d_t”代表速度与加速度，在场景1)及场景2)中无人驾驶车辆行驶到目标状态空间后，D轴上的速度和加速度均为0；

在S轴上根据车辆当前速度以及目标速度生成状态空间(s_t、s_t’、s_t”)，

其中s_t代表车辆原行驶路径与平行偏移路径的偏移距离；

s_t’代表车辆行驶到目标轨迹后的速度；

s_t”代表车辆行驶到目标轨迹后的加速度；车辆到达目标轨迹空间后保持目标车速匀速行驶，加速度取值为0；

针对场景3)

在D轴上生成目标状态空间(d_t,、d_t’、d_t”)，

其中d_t选取左右两条车道的中心线距离当前车辆行驶车道中心线的距离，取值表示为[left:0.0:right]，其中left为距左侧车道中心线的距离， right为距右侧车道中心线的距离；d_t’、d_t”代表速度与加速度，取值均为 0；

在S轴上的目标状态空间设定与场景1)和场景2)一致；

五、确定目标平行偏移轨迹：

针对不同场景下的每一个目标的配置，生成一条无人驾驶车辆的目标轨迹；所述目标轨迹分为两段，其中：无人驾驶车辆当前位置与平行偏移路径位置之间的平滑连接为第一段路径；达到目标轨迹后保持在该平行偏移路径的位置上行驶为第二段轨迹。

所述目标平行偏移轨迹的筛选目标路径的评估准则：

D轴向的评估准则：

设置筛选目标函数为J₁＝arg min C_d，C_d为D轴向上的损失函数；

其中损失函数为式中k_j1为D轴向的稳定性权重系数，为D轴向约束控制变量的变化率，k_t1为D 轴向的时间权重系数，k_d1为D轴向的距离误差权重系数，T为D轴向轨迹规划所需时间，d_d为D轴向目标轨迹与参考轨迹间的距离；

S轴向的评估准则：

设置筛选目标函数为J₂＝arg min C_s，C_s为S轴向上的损失函数；

其中损失函数为式中k_j2为S轴向的稳定性权重系数，为S轴向约束控制变量的变化率，k_t2为S 轴向的时间权重系数，k_d2为S轴向的距离误差权重系数，T为S轴向轨迹规划所需时间，d_s为S轴向目标轨迹与参考轨迹间的距离；

S轴向的速度约束：式中v_max为车辆设定最大限速；

S轴向的加速度约束：式中a_max为车辆设定最大加速度；

判断轨迹的曲率是否超过车辆的最大转向限制条件；

检测环境信息，判断规划后的轨迹是否会发生碰撞。

步骤五所述的确定目标平行偏移轨迹是否需要重新规划：评估准则为通过评估周围环境以及车辆状态，判断是否需要重新规划轨迹，若需要则从车辆当前状态重新规划，若不需要，则保留上一次规划的部分轨迹，继续进行后续的规划，完成车辆整体的轨迹规划，当***检测到车辆无法选择最优的重规划路径完成避障和超车时，***进入警示状态减速行驶并提示驾驶员介入操作；所述重新规划路径通过触发控制器进行重规划轨迹的评估，评估条件如下：

1)行驶过程中环境变化，检测该规划轨迹引起碰撞；

2)无法求解出步骤5的D轴向及S轴向的目标函数的最优解，导致控制器的损失函数过大，则放弃当前时刻规划的轨迹。

步骤五所述的目标轨迹优选地对于第一段轨迹，车辆的初始状态空间为(s₀、s₀’、s₀”)(d₀、d₀’、d₀”)，目标状态空间为(s_t、s_t’、0)(d_t、0、0)，采用五次多项式对换道轨迹拟合，以保证换道轨迹的平滑连续；对于D轴向，根据初始状态空间(d₀、d₀’、d₀”)以及目标状态空间(d_t、0、0)采用五次多项式规划，并通过设定的时间与目标状态空间的值求得五次多项式的系数；根据S轴向的初始状态空间和目标状态控制空间计算出对应的五次多项式系数，以此完成两个一维方向的目标轨迹的拟合；

对于第二段轨迹，由于D轴方向的参数保持不变，仅进行S轴上的速度和加速度规划即可；所述两段轨迹规划后得到的(s,d)^T变换为(x,y)^T方向的坐标，完成一条完整轨迹的规划。

实施例

本发明坐标转换和解耦是为了后续轨迹规划在两个轴向上更加便利，为后续的控制过程也提供了极大的便利因此能够更加准确高效的完成轨迹规划，而实际的轨迹规划是采用五次多项式进行。

步骤1，场景定义。根据道路场景选取对应参考路径，本发明涉及三种应用场景，分别是：1)开阔环境中的固定路径；2)没有车道线，仅带有道路边界线的结构化道路；3)具有多条并行车道的结构化道路。

根据以上三个场景分别选取一条固定路径为轨迹规划参考路径，针对场景1)选取该环境中的固定行驶路径为参考路径；针对场景2)根据道路边界线定位道路中心线为参考路径；针对场景3)选取保持某一车道行驶时的车道中心线为参考路径，当换道行驶时选取相应行驶的车道中心线为参考路径。

步骤2，通过CARTESIAN(笛卡尔)到FRENET坐标系的变换方法将二维轨迹规划解耦为两个独立的一维轨迹规划。定义参考路径为S轴，S轴的法向为D轴，进而将二维平面的坐标(x,y)变换至坐标(s,d)，从而将实际道路路径的几何结构由二维规划解耦变换为两个独立的一维规划。

步骤2中坐标变换方式如图2所示，坐标变换步骤如下：

步骤2.1：设a为任意变量，记：

a'＝d(a)/d(s)

a”＝d(a')/d(s)

步骤2.2：设分别为二维平面内一条曲线上一点的切向量、法向量，κ为该点的曲率，记：

步骤2.3：CARTESIAN坐标系下原点指向点(x,y)的向量为参考路径上离点(x,y)最近的点为(x_r,y_r)，原点指向点(x_r,y_r)的向量为

步骤2.4：点(x,y)所在轨迹上的切向量与CARTESIAN坐标系x轴夹角为θ_x，点(x_r,y_r)所在参考路径上的切向量与CARTESIAN坐标系x轴夹角为θ_r，两者的差值为Δθ＝θ_x-θ_r；

步骤2.5：d，d'，d”，s'，s”的变换公式如下：

if(y-y_r)cosθ_r-(x-x_r)sinθ_r>0,positive；otherwisenegetive

d'＝(1-κ_rd)tanΔθ

步骤3，生成轨迹规划目标状态空间。轨迹的生成分别对应三种不同的应用场景，针对场景1)和场景2)在D轴方向上生成目标状态空间(d_t,、 d_t’、d_t”)，其中d_t按照等距间隔取值[min:step:max]以对应不同平行偏移的距离，根据不同的道路宽度选取特定值，例如[-4.5:0.3:4.5]。 d_t’、d_t”代表速度与加速度，在步骤1定义的场景1)及场景2)无人驾驶车辆行驶到目标状态后，D轴上的速度和加速度均为0；S轴上根据车辆当前速度以及目标速度生成状态空间(s_t、s_t’、s_t”)，其中s_t代表车辆原行驶路径与平行偏移路径的偏移距离，s_t’代表车辆行驶到目标轨迹后的速度，s_t”代表车辆行驶到目标轨迹后的加速度，因车辆到达目标轨迹后保持目标车速匀速行驶,故加速度取值为0。

针对场景3)，D轴上生成目标状态空间(d_t、d_t’、d_t”)，其中d_t选取左右两条车道的中心线距离当前车辆行驶车道中心线的距离，取值表示为 [left:0.0:right]，其中left为距左侧车道中心线的距离，right 为距右侧车道中心线的距离，若车辆行驶在道路最左侧或最右侧车道，那么对应取值分别为[0.0,right]和[left,0.0]，d_t’、d_t”代表速度与加速度取值均为0。S轴上的状态空间设定与场景1)和场景2)一致。

步骤4，根据目标平行偏移规划轨迹生成新的轨迹。针对不同场景下的每一个目标的配置，生成一条无人驾驶车辆的目标轨迹。该轨迹分为两段，第一段为无人驾驶车辆当前位置与平行偏移路径位置之间的平滑连接路径；第二段为达到目标轨迹后保持在该平行偏移路径的位置上行驶的轨迹。

优选地对于第一段轨迹，车辆的初始状态空间为(s₀、s₀’s₀”)(d₀、d₀’、 d₀”)，目标状态空间为(s_t、s_t’、0)(d_t、0、0)，采用五次多项式对换道轨迹拟合，以保证换道轨迹的平滑连续；对于D轴向，根据初始状态空间 (d₀、d₀’、d₀”)以及目标状态空间(d_t、0、0)带入以下表达式：

d(t₀)＝α₀+α₁t₀+α₂t₀ ²+α₃t₀ ³+α₄t₀ ⁴+α₅t₀ ⁵

d(t_d)＝β₀+β₁t_d+β₂t_d ²+β₃t_d ³+β₄t_d ⁴+β₅t_d ⁵

因此，此时轨迹的设计为寻找满足上述边界条件的问题，根据边界条件求出式中α₀、α₁、α₂、α₃、α₄、α₅、β₁、β₂、β₃、β₄、β₅的五次多项式轨迹规划系数，t₀为规划初始时刻，t_d为轨迹规划结束时刻，轨迹规划初始时刻的位置已知，轨迹规划初末时刻换道轨迹的切线与车辆行驶方向一致。

本发明在轨迹规划的仿真场景中设定如下情景：车辆以25m/s的纵向速度开始进行避障及超车的换道轨迹规划，在16s的换道时间内从原行驶车道换至目标车道，在整个换道过程中车辆纵向速度保持不变，假设两个自车行驶车道与目标车道的中心轴线距离d_t为3.75m，选取车辆开始换道时所处的位置为坐标初始原点，由此确定车辆的初始状态和目标状态分别为：

S_ini＝(0,25,0,0,0,0)

S_fin＝(200,25,0,3.75,0)

(α₁,α₂,α₃,α₄,α₅)＝(0,0,0,0,25,0)

(β₁,β₂,β₃,β₄,β₅)＝(0.0005,-0.011,0.0536,0,0,0)

根据上述表达式与已知参数可求得五次多项式系数。同理，根据S轴向的初始状态空间和目标状态控制空间可计算出对应的五次多项式系数，以此完成两个一维方向的目标轨迹的拟合，如图4、图5分别为经过五次多项式规划后的纵向位移与换道时间以及横向位移与换道时间之间的关系，图中可以看出采用以上五次多项式的规划方法能够在规定的换道时间内完成换道并且纵横向位置能够达到期望位置，所规划的轨迹平滑满足***设计的要求。

对于第二段轨迹，由于D轴方向的参数保持不变，仅进行S轴上的速度和加速度规划即可。当上述两段轨迹规划后得到的(s,d)^T变换为(x,y)^T方向的坐标，完成一条完整轨迹的规划。

步骤5，筛选目标路径。根据上述步骤得出多条规划轨迹，需要根据当前周围环境并设定相应的评估准则筛选出满足车辆特性、安全性即舒适性要求且耗费最小的轨迹，以实现功能上的避障和超车功能。评估准则如下：

步骤5.1，D轴向的评估准则：

设置筛选目标函数为J₁＝arg min C_d，C_d为D轴向上的损失函数。

其中损失函数为式中k_j1为D轴向的稳定性权重系数，为D轴向约束控制变量的变化率，k_t1为D轴向的时间权重系数，k_d1为D轴向的距离误差权重系数，T为D轴向轨迹规划所需时间，d_d为D轴向目标轨迹与参考轨迹间的距离。

步骤5.2，S轴向的评估准则：

与D轴向的评估准则采用相似的方法，设置筛选目标函数为 J₂＝arg min C_s，C_s为S轴向上的损失函数。

其中损失函数为式中k_j2为S轴向的稳定性权重系数，为S轴向约束控制变量的变化率，k_t2为S轴向的时间权重系数，k_d2为S轴向的距离误差权重系数，T为S轴向轨迹规划所需时间，d_s为S轴向目标轨迹与参考轨迹间的距离。

S轴向的速度约束：式中v_max为车辆设定最大限速按实际场景中的道路法规设置。

S轴向的加速度约束：式中a_max为车辆设定最大加速度按车辆满足的舒适性要求设置，一般为1-2m/s²。

步骤5.3，判断轨迹的曲率是否超过车辆的最大转向限制条件。

步骤5.4，检测环境信息，判断规划后的轨迹是否会发生碰撞。

步骤6，保持路径一致性。随着无人驾驶车辆的移动，车辆周围的环境不断变化，由于控制器计算轨迹规划存在计算的时间延迟，实时规划的轨迹可能与车辆当前行驶的轨迹偏差较大，导致车辆在行驶过程中稳定性很差。

优选地需要制定评估准则，通过评估周围环境以及车辆状态判断是否需要重新规划轨迹，若需要则从车辆当前状态重新规划，若不需要，则保留上一次规划的部分轨迹，继续进行后续的规划，完成车辆整体的轨迹规划，当***检测到车辆无法选择最优的重规划路径完成避障和超车时，***进入警示状态减速行驶并提示驾驶员介入操作。

触发控制器重规划轨迹的评估条件如下：

1)行驶过程中环境变化，检测该规划轨迹引起碰撞；

2)无法求解出步骤5.1和5.2目标函数的最优解，导致控制器的损失函数过大，则放弃当前时刻规划的轨迹

本发明采用车道线平行偏移的方法，通过实时的动态轨迹规划并制定适用不同场景的轨迹评估准则确定车辆规划的轨迹，在保证车辆安全、稳定和舒适的前提下实现车辆的避障以及超车。如图1所示为本发明的整体***控制框架，本发明通过针对不同的场景定义车辆参考路径使得该方法适用于多种道路情况，为保证车辆轨迹规划的精确和便利，采用坐标转换的方法，从笛卡尔坐标系转换至FRENET坐标系，实现两个坐标维度的解耦。根据确定不同场景下的参考路径，生成轨迹规划的目标状态空间，根据偏移的目标轨迹通过五次多项式规划目标轨迹共由两段组成。由于环境场景的不断变化，轨迹规划后生成轨迹在实际行驶过程中可能会出现与它车碰撞或超出车辆自身动力学特性的轨迹，因此本发明制定相应的评估准则，筛选生成的目标轨迹，检测规划轨迹是否需要重规划，若需要则返回上面步骤进行轨迹的重新规划，若不需要则按当前轨迹行驶，以保障车辆超车和避障过程中的安全性。

本发明解决了现有技术中由于道路存在的曲率导致车辆与平行偏移路径之间的平滑连接难以实现的问题以及由于车辆位姿不平行于路径切向量情况下车辆到平行偏移路径平滑连接路径难以规划的问题。与此同时，保证了在路径平滑的基础上实现平滑的速度规划以及加速度规划满足舒适性的要求，并在随车行驶的环境逐渐变化过程中保证路径规划的一致性满足稳定性的要求。在本发明设计的应用场景中可适用于多种场景下的轨迹规划包括结构化道路的无人驾驶轨迹规划。

在本发明的描述中，术语“中心”、“纵向”、“横向”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明保护范围的限制。

Claims (4)

1.一种基于车道线平行偏移进行避障及超车的动态轨迹规划方法，其特征是：针对不同道路场景建立不同的状态空间，实现不同工况下轨迹规划；采用坐标转换并解耦的方式为后续实时规划精确轨迹做准备，进而在不同工况下采用五次多项式实时规划平滑轨迹，具体步骤如下：

一、定义场景：

1)开阔环境中的固定路径；

2)仅带有道路边界线并无车道线的结构化道路；

3)具有多条并行车道的结构化道路；

二、根据场景分别选取一条固定路径为轨迹规划的参考路径：

针对场景1)选取该环境中的固定行驶路径为参考路径；

针对场景2)根据道路边界线定位道路中心线为参考路径；

针对场景3)选取保持某一车道行驶时的车道中心线为参考路径，当换道行驶时选取相应行驶的车道中心线为参考路径；

三、通过CARTESIAN到FRENET坐标系的变换方法将二维轨迹规划解耦为两个独立的一维轨迹规划，坐标转换至S、D轴方向，

其中S轴定义为参考路径，S轴的法向为D轴；

四、生成S、D轴方向状态空间，即目标状态空间：

针对场景1)和场景2)

在D轴方向上生成目标状态空间(d_t,d_t’,d_t”)，

其中：d_t按照等距间隔取值[min:step:max]以对应不同平行偏移的距离，d_t’、d_t”代表速度与加速度，在场景1)及场景2)中无人驾驶车辆行驶到目标状态空间后，D轴上的速度和加速度均为0；

在S轴上根据车辆当前速度以及目标速度生成状态空间(s_t、s_t’、s_t”)，

其中s_t代表车辆原行驶路径与平行偏移路径的偏移距离；

s_t’代表车辆行驶到目标轨迹后的速度；

s_t”代表车辆行驶到目标轨迹后的加速度；车辆到达目标轨迹空间后保持目标车速匀速行驶，加速度取值为0；

针对场景3)

在D轴上生成目标状态空间(d_t,、d_t’、d_t”)，

其中d_t选取左右两条车道的中心线距离当前车辆行驶车道中心线的距离，取值表示为[left:0.0:right]，其中left为距左侧车道中心线的距离，right为距右侧车道中心线的距离；d_t’、d_t”代表速度与加速度，取值均为0；

在S轴上的目标状态空间设定与场景1)和场景2)一致；

五、确定目标平行偏移轨迹：

针对不同场景下的每一个目标的配置，生成一条无人驾驶车辆的目标轨迹；所述目标轨迹分为两段，其中：无人驾驶车辆当前位置与平行偏移路径位置之间的平滑连接为第一段路径；达到目标轨迹后保持在该平行偏移路径的位置上行驶为第二段轨迹。

2.根据权利要求1所述的基于车道线平行偏移进行避障及超车的动态轨迹规划方法，其特征是：所述目标平行偏移轨迹的筛选目标路径的评估准则：

D轴向的评估准则：

设置筛选目标函数为J₁＝argmin C_d，C_d为D轴向上的损失函数；

其中损失函数为式中k_j1为D轴向的稳定性权重系数，为D轴向约束控制变量的变化率，k_t1为D轴向的时间权重系数，k_d1为D轴向的距离误差权重系数，T为D轴向轨迹规划所需时间，d_d为D轴向目标轨迹与参考轨迹间的距离；

S轴向的评估准则：

设置筛选目标函数为J₂＝argmin C_s，C_s为S轴向上的损失函数；

其中损失函数为式中k_j2为S轴向的稳定性权重系数，为S轴向约束控制变量的变化率，k_t2为S轴向的时间权重系数，k_d2为S轴向的距离误差权重系数，T为S轴向轨迹规划所需时间，d_s为S轴向目标轨迹与参考轨迹间的距离；

S轴向的速度约束：式中v_max为车辆设定最大限速；

S轴向的加速度约束：式中a_max为车辆设定最大加速度；

判断轨迹的曲率是否超过车辆的最大转向限制条件；

检测环境信息，判断规划后的轨迹是否会发生碰撞。

3.根据权利要求1所述的基于车道线平行偏移进行避障及超车的动态轨迹规划方法，其特征是：步骤五所述的确定目标平行偏移轨迹是否需要重新规划：评估准则为通过评估周围环境以及车辆状态，判断是否需要重新规划轨迹，若需要则从车辆当前状态重新规划，若不需要，则保留上一次规划的部分轨迹，继续进行后续的规划，完成车辆整体的轨迹规划，当***检测到车辆无法选择最优的重规划路径完成避障和超车时，***进入警示状态减速行驶并提示驾驶员介入操作；所述重新规划路径通过触发控制器进行重规划轨迹的评估，评估条件如下：

1)行驶过程中环境变化，检测该规划轨迹引起碰撞；

2)无法求解出步骤5的D轴向及S轴向的目标函数的最优解，导致控制器的损失函数过大，则放弃当前时刻规划的轨迹。

4.根据权利要求1所述的基于车道线平行偏移进行避障及超车的动态轨迹规划方法，其特征是：步骤五所述的目标轨迹优选地对于第一段轨迹，车辆的初始状态空间为(s₀、s₀’、s₀”)(d₀、d₀’、d₀”)，目标状态空间为(s_t、s_t’、0)(d_t、0、0)，采用五次多项式对换道轨迹拟合，以保证换道轨迹的平滑连续；对于D轴向，根据初始状态空间(d₀、d₀’、d₀”)以及目标状态空间(d_t、0、0)采用五次多项式规划，并通过设定的时间与目标状态空间的值求得五次多项式的系数；根据S轴向的初始状态空间和目标状态控制空间计算出对应的五次多项式系数，以此完成两个一维方向的目标轨迹的拟合；

对于第二段轨迹，由于D轴方向的参数保持不变，仅进行S轴上的速度和加速度规划即可；所述两段轨迹规划后得到的(s,d)^T变换为(x,y)^T方向的坐标，完成一条完整轨迹的规划。