CN110286681A

CN110286681A - 一种变曲率弯道的动态自动驾驶换道轨迹规划方法

Info

Publication number: CN110286681A
Application number: CN201910603739.4A
Authority: CN
Inventors: 杨达; 熊明强; 吕蒙
Original assignee: Southwest Jiaotong University
Current assignee: Southwest Jiaotong University
Priority date: 2019-07-05
Filing date: 2019-07-05
Publication date: 2019-09-27
Anticipated expiration: 2039-07-05
Also published as: CN110286681B

Abstract

本发明公开了一种变曲率弯道的动态自动驾驶换道轨迹规划方法，包括如下步骤：步骤一、计算直道换道轨迹曲线；步骤二、计算弯道换道轨迹曲线；步骤三、采用防侧翻算法和避撞算法计算轨迹安全性约束；步骤四、确定满足步骤三所述的轨迹安全性约束的最优轨迹；步骤五、输出最终换道轨迹，作为自动驾驶车辆的换道轨迹，从而构建了一套完整的动态车辆换道轨迹规划模型。本发明采用了考虑舒适性与效率的最优轨迹算法来生成最优轨迹，再用安全性约束来调整最优轨迹，使决策出的最终换道轨迹在安全性前提下达到最优。因而本发明规划出的最优换道轨迹服务水平较高，能够保证乘客具有较高的舒适度体验与效率体验。

Description

一种变曲率弯道的动态自动驾驶换道轨迹规划方法

技术领域

本发明涉及一种变曲率弯道的动态自动驾驶换道轨迹规划方法。

背景技术

近年来，自动驾驶受到了世界范围内的巨大关注，其被认为是缓解交通拥堵，减少交通事故和环境污染的重要技术。本发明关注的是自动驾驶中的一项关键技术，换道轨迹规划技术。换道作为车辆的基本操作之一，对车辆的安全行驶起着重要的作用。在当前的交通事故中，有超过30％的道路事故是不合理的换道操作引起的。因此只有构建了完备的换道轨迹规划模型，才能最大程度上地减少自动驾驶交通事故的发生，使自动驾驶车辆的安全性得到保障。图1即为车辆换道轨迹示意图，换道车辆SV在执行换道过程中会受到目标车道后车LV和目标车道前车PV的影响。

目前对于弯道换道的研究还非常有限。在Ren et al.中提出了一种弯路上车辆换道轨迹规划方法，推导了换道时车辆在惯性坐标系的期望运动状态计算公式，根据车辆期望运动状态计算了期望横摆角，横摆角速度和横摆角加速度，设计了横摆角速度跟踪车辆弯道换道的方法。然而，该模型将弯道线形视为圆曲线，这与真实的道路曲线不符，而且该模型属于静态换道，没有考虑换道过程中周边车辆对于换道车辆的影响和反应时间。Luo等虽然在研究自动驾驶车辆直道动态换道轨迹规划时提到了在弯道情况下的应用，但该方法并没有详细说明如果将直接轨迹模型推广到弯道情况，也没有讨论内外侧车道的曲率差别对换道轨迹规划的影响和缓和曲线情况下的换道轨迹规划。所以当前的有关弯道换道轨迹规划的研究假设还十分简单，远远不能满足真实交通环境的需求。

在几何曲线法中，根据用以表征换道轨迹的几何曲线类型的不同，又细分为多项式轨迹，圆轨迹，正余弦函数轨迹，螺旋线轨迹，B样条曲线轨迹等方法，此外，有些学者通过设计换道过程的速度或加速度曲线来表征换道轨迹，例如正反梯形加速度曲线。多项式方法最早由Nelson提出，旨在构建曲率连续的(AGVs)自动引导车辆轨迹，其提出了极坐标多项式轨迹和笛卡尔坐标多项式并证明多项式方法能提高轨迹跟踪的准确性。Piazzi和Bianco提出的基于五次多项式的轨迹规划方法，运用多项式参数化曲线进行轨迹生成，轨迹规划曲线的平坦性保证了自动驾驶车辆轨迹生成的可执行性，通过调节参数可表征不同几何线性曲线轨迹，然而文中个别参数实际物理意义不明确，无法方便地应用于实时控制。Papadimitriou和Tomizuka采用五次多项式来表征车道变换轨迹，把障碍物简化处理为圆形并考虑了动态约束，但这个策略只能对换道开始时刻障碍物进行处理，而不能解决在换道期间出现的障碍物。Chu等研究了弯曲路段环境下的局部轨迹规划，使用不依赖时间的三次多项式曲线来表征换道轨迹，再通过轨迹计算曲率与转向角进而实现控制，并考虑了舒适性，效率和安全性来实现避障。Shim等提出了六次多项式轨迹规划方法，创新性地引入车辆航向角与转向角作为边界条件，以待定轨迹方程。并采用(MPC)模型预测控制方法来实现轨迹跟踪。除此之外，Chen等提出利用二次贝塞尔曲线进行路径规划，得到的换道轨迹具有连续的曲率半径，但该方法未涉及车辆的碰撞检测，同时未解决存在障碍物情况下贝赛尔曲线控制点的选取问题。之后Milam采用了三次B样条曲线进行换道轨迹生成，但车辆行为动态变化时无法合理地确定B样条曲线段数，并且生成的换道轨迹在车辆实际换道过程中其横向最大加速度不易控制。Ren等提出了一种基于梯形加速度曲线的规划方法，导出参考航偏角、航偏率和偏航角加速度，然后设计偏航率跟踪器，通过应用非模态滑动模式技术实现车道改变操纵但是只考虑了横向加速度满足正反梯形曲线，而无法实现纵向速度的实时调整。Chee和Tomizuka对比研究了四种不同的期望轨迹，圆轨迹、余弦函数轨迹、多项式轨迹、梯形加速度轨迹与两种轨迹跟踪算法并选择了梯形加速度轨迹作为虚拟期望轨迹，并给出滑模控制器算法从而提高系统的稳定性，然而他们孤立地研究了换道车辆，假设周围没有任何其他车辆对换道过程进行影响。Sledge and Marshek基于选定的标准比较了几种常用的候选车道变换轨迹，并转化为边界条件约束下的最优化问题来解决，把最大速度作为额外的鉴别指标，比较结果显示多项式曲线在两个最优轨迹函数中形式较为简化。Zhang等使用了正弦曲线、正反梯形加速度曲线、螺旋线来表达换道轨迹，用初始点、终止点状态以及其他约束条件(如碰撞规避)来确定参数选择的范围，最后根据优化目标确定参数的值并在中加入了考虑驾驶舒适性与效率的成本函数来进行轨迹优化，并使用时间依赖的三次多项式方程来表征换道几何曲线。

此外，Wolf和Burdick首次提出了高速公路环境下的车辆换道的人工势场法，用势场来表征环境空间，通过求解势场中势能下降最快的方向来规划换道轨迹。其势场函数考虑了车道线对车辆的吸引力、道路边缘对车辆的斥力作用、车辆障碍物对本车的斥力作用、当前道路期望行驶车速对当前行驶车速的引力作用。一方面根据环境信息决定是否换道超过缓慢的车辆障碍物，另一方面考虑到了车速依赖性和偏好。但可能陷入局部最小点，无法稳定地输出最优轨迹，并且在高速环境下由于搜索空间规模急剧增大，势场法效率会大幅度下降。同时存在一个很强的假设，认为换道车辆速度仅与车道限速有关，而不受周围车辆的影响，这与真实不符。但是当前的研究存在着一些不足之处。第一，假设在换道过程中周围车辆的速度都不发生变化，这与真实的驾驶环境不符。第二、当前的模型都没有考虑在换道过程中换道车辆根据周围车辆状态的变化进行实时响应，实时对速度进行动态调整，这样这些模型在真实交通环境中可能失效。第三，在安全性方面，上述研究认为只要换道完成时，换道车辆和目标车道车辆不发生碰撞即可，而不需要保持一个应对紧急事件发生时的安全间距，这种方法事实上是不能真正地保证换道的安全性的。

而目前对于弯道换道的研究还非常有限。在Ren提出了一种弯路上车辆换道轨迹规划方法，推导了换道时车辆在惯性坐标系的期望运动状态计算公式，根据车辆期望运动状态计算了期望横摆角，横摆角速度和横摆角加速度，设计了横摆角速度跟踪车辆弯道换道的方法。然而，该模型将弯道线形视为圆曲线，这与真实的道路曲线不符，而且该模型属于静态换道，没有考虑换道过程中周边车辆对于换道车辆的影响和反应时间。Luo虽然在研究自动驾驶车辆直道动态换道轨迹规划时提到了在弯道情况下的应用，但该论文并没有详细说明如果将直接轨迹模型推广到弯道情况，也没有讨论内外侧车道的曲率差别对换道轨迹规划的影响和缓和曲线情况下的换道轨迹规划。所以当前的有关弯道换道轨迹规划的研究假设还十分简单，远远不能满足真实交通环境的需求。

发明内容

为了克服现有技术的上述缺点，本发明提出了一种变曲率弯道的动态自动驾驶换道轨迹规划方法，采用了不依赖时间的多项式轨迹方程来表征换道轨迹曲线，避免了对速度和加速度假设太强的问题，并引入了基于反应时间的避撞算法以及防侧翻算法来保障换道的安全性，然后在安全轨迹簇中通过实时调整速度来确定最优轨迹，最后通过轨迹生成模块输出一条实时动态的最终换道轨迹，作为自动驾驶车辆的换道轨迹，从而构建了一套完整的动态车辆换道轨迹规划模型。

本发明解决其技术问题所采用的技术方案是：一种变曲率弯道的动态自动驾驶换道轨迹规划方法，包括如下步骤：

步骤一、计算直道换道轨迹曲线；

步骤二、计算弯道换道轨迹曲线；

步骤三、采用防侧翻算法和避撞算法计算轨迹安全性约束；

步骤四、确定满足步骤三所述的轨迹安全性约束的最优轨迹；

步骤五、输出最终换道轨迹。

与现有技术相比，本发明的积极效果是：

1)弯道适应性：本发明对于各种固定曲率和变化曲率的弯道进行换道轨迹规划。

2)动态实时性：本发明中自动驾驶车辆在换道过程中能够对环境信息的实时变化做出动态响应，即通过对每次规划步长中生成的安全速度与最优轨迹进行实时调整来不断适应新的环境信息，并决定采用新的最优轨迹继续换道还是中止换道返回原车道。

3)安全性：本发明使用了基于反应时间的纵向避撞算法与基于最大侧向加速度的防侧翻算法，从而在纵向上即使在前车紧急制动的情景下依然可以保证不发生碰撞，在横向上不发生车辆侧翻。最大程度上保障了换道过程中自动驾驶车辆的安全性。

4)最优性：本发明采用了考虑舒适性与效率的最优轨迹算法来生成最优轨迹，再用安全性约束来调整最优轨迹，使决策出的最终换道轨迹在安全性前提下达到最优。因而本发明规划出的最优换道轨迹服务水平较高，能够保证乘客具有较高的舒适度体验与效率体验。

附图说明

本发明将通过例子并参照附图的方式说明，其中：

图1为换道过程示意图；

图2为车辆换道轨迹示意图；

图3为安全车距计算原理图；

图4为轨迹终点安全区间求解示意图；

图5为轨迹规划原理框架图；

图6为换道起点确定示意图；

图7为多步长轨迹生成示意图，其中：(a)为三次轨迹规划过程；(b)为三次轨迹规划后形成的一条完整的换道轨迹；

图8为换道轨迹示意图，其中：(a)为虚拟直道换道轨迹；(b)为弯道换道轨迹。

具体实施方式

一种变曲率弯道的动态自动驾驶换道轨迹规划方法，包括如下内容：

(1)最优轨迹算法

自动驾驶车辆在换道时进行轨迹规划，每一规划步长会根据实时环境信息寻找一条对应的最优轨迹。本发明利用三次多项式曲线来模拟车辆换道过程中的行驶轨迹，三次多项式的线形和车辆换道曲线十分相近，而且三次多项式轨迹既具有曲率均匀变化的特点，又避免了高次多项式轨迹需要引入抽象参数来约束轨迹的麻烦，在此基础上，对直道换道轨迹进行改造成弯道换道轨迹，其表达式如下：

其中，a₀、a₁、a₂、a₃均为待确定的参数，x_n为纵向上车辆n的位置，y_n为横向上车辆n的位置。在换道过程中，车辆在每一个步长都会规划出一条新的换道轨迹，该轨迹以当前位置为起点，以目标车道中线上的某一位置为终点，在终点处车辆的运动方向与车道平行。本文中我们使用移动坐标系，将每一步长的起点位置依次定义为(0,0)点，终点位置定义为(x_f,y_f)，每一步长起点的车辆航向角为θ_i，终点的车辆航向角为0，则有：

y′_n(0)＝tanθ_i(2)

将以上车辆换道的起点坐标(0,0)和终点坐标(x_f,y_f)，以及式(2)和(3)代入轨迹方程(1)得到轨迹方程中各参数的表达式如下：

a₁＝tanθ_i(4)

a₀＝0(5)

将式(4)-(7)代入到轨迹方程(1)得到：

在式(8)中θ_i为规划步长起点的航向角，y_f为终点横向坐标，均为已知量。

在此基础上，进行弯道换道轨迹规划之前，首先假设道路是直道，然后使用直道换道轨迹规划方法规划出一条虚拟最优换道轨迹。所以，轨迹方程由换道轨迹终点纵向坐标x_f唯一确定。而在真实换道过程中，驾驶员在换道的过程中总是期望以更短的时间完成换道，而且尽量保证换道过程的舒适性最大。但是，这两个因素是相互矛盾的，驾驶员需要在换道过程中对这两个因素需进行权衡考虑。因此，本文构造了一个换道成本函数来表达驾驶员在换道中对效率和舒适性的考虑，在该成本函数中用侧向最大加速度代表舒适度，侧向最大加速度越大，说明该换道轨迹舒适度越差，由于换道轨迹的最大侧向加速度出现在轨迹终点，故轨迹终点侧向加速度即为侧向最大加速度，另一方面采用换道终点纵向坐标x_f代表效率，x_f越大，说明该换道轨迹的效率越低。为了使两个因素在一个数量级上进行评估，本文对它们进行归一化处理，具体表达式如下：

式中，J为成本函数期望值，指车辆侧翻对应的临界侧向加速度，指的是换道轨迹的最大纵向距离经验值，ω为需要标定的舒适性权重参数。

成本函数J在终点处的侧向加速度可由下式计算：

式中，K(·)为换道轨迹的曲率函数，u_n为车辆速度。计算公式如下：

其中，y′和y″分别为换道轨迹方程(8)的一阶和二阶导数，它们的表达式如下：

将式(12)和(13)代入到式(11)中得出曲率K关于x_n的函数(0≤x_n≤x_f)，其表达式如下：

随着x_n的变化，轨迹曲率先均匀变小至零，然后均匀变大，所以多项式换道曲线中曲率最大点出现在换道曲线的终点处。因而式(10)为：

再将式(15)代入到成本函数(9)中，得到成本函数J的最终表达式，如下：

从式子(16)可以看出，成本函数J是关于x_f的一元函数，J取到最小值时相应的x_f即为该步长最优轨迹终点的纵向位置

设点(x_n,y_n)为虚拟直道换道轨迹上的任一点，该点与原点的连线与x轴的夹角为α，如图8(a)，则有：

y_n＝x_ntanα(17)

然后根据相同坐标体系下在x_n点处弯道相对于x坐标轴的偏移值Δy_n，如图8(b)所示，得到弯道换道轨迹中相对应的横坐标

而，

结合方程17和方程18以及方程19，得到弯道换道轨迹方程：

其中η是直道换道轨迹经改造后的任意一点与原点的连线同x轴的夹角。

(2)安全性算法

安全性模块包括两个部分：防侧翻算法和避撞算法，防侧翻算法用以确定不发生侧翻轨迹簇的左边界，避撞算法用以确定轨迹终点安全区间，而在轨迹决策中则通过调整速度和舒适性权重参数来使最优轨迹满足安全性约束条件。

车辆在曲线运动时加速度可分解为侧向加速度a_s与轴向加速度a_f，其中侧向加速度过大会导致车辆侧翻，使换道安全性无法得到保障，因而需要利用一个防侧翻算法计算临界侧翻情景下的最大侧向加速度所对应的最优轨迹，作为最优轨迹簇的临界值。安全轨迹的纵向位移x_f均应大于该侧翻临界轨迹的纵向位移车辆模型见图2所示，其中v_n(t)为车辆SV在t时刻的纵向分速度，θ为车辆的航向角，也就是车辆运动方向与x坐标轴的夹角。R为车辆质心的运动的曲率半径。

首先计算车辆以最大侧向加速度与当前车速行驶时所能采用的最大轨迹曲率K(x_f)为其中u_n(t)为车辆SV在t时刻的总速度。同时又可以由换道轨迹的终点坐标(x_f,y_f)确定，公式如下：

故可以由来计算侧翻临界轨迹的终点纵向坐标x_f。令Q＝1/x_f，上式变形为，

由于在整个换道过程中θ_i取值范围在(0,π/2)：

1)如果故上式展开为，

考虑到该关于Q的二次函数的Δ>0，故由二次函数求根公式得出：

取其正根，则防侧翻安全区间为其中为：

2)同理有：

从而得到了理论上换道车辆不侧翻条件下的最小换道轨迹纵向位移只要换道轨迹的纵向位移大于则不会发生侧翻。

为了避免碰撞，当SV到达目标车道的时候，SV应该与目标车道的前车PV和后车LV保持安全距离，根据Gipps安全距离模型，这个SV在目标车道安全距离应该是其中表示车辆LV在SV换道完成时的纵向位置，表示PV车在SV换道完成时纵向位置，l_n-1表示车辆PV的车长，l_n+1表示车辆PV的车长。

图3中，车辆PV在t-τ时刻开始制动并在t_F时刻停止，车辆SV经过一个反应时间τ后在t时刻开始制动。假设道路是直道路，当SV紧急制动时，为了不与车辆PV发生碰撞，车辆SV至少应与车辆PV保持的纵向上安全距离记为S_ps，可通过下式计算：

式中，为车辆SV在t-τ时刻的纵向速度，表示车辆PV在t-τ时刻的切向速度，b_n为车辆SV的制动最大减速度，b_n-1为车辆PV的制动最大减速度。

类似的，可通过下式计算得到在直道情况下车辆LV至少应与车辆SV保持的安全距离s_ls：

式中，表示t-τ时刻车辆LV的切向速度，b_n+1为车辆LV的制动最大减速度。

在弯道中安全距离是公式(27)和(28)中的直线安全距离根据曲率变化积分得到的弯道距离，设换道起始时刻t-τ，换道车辆的纵向位置为x_n，则目标车道前车的横坐标为x_n+s_ps，故车辆SV应与车辆PV保持的弯道安全距离为：

式中，f(x)是目标车道中心线的曲线函数，如不能拟合出目标车道中心线的曲线函数，可由地图信息获得目标车道中心线每一点的位置。

车辆LV应与车辆SV保持的弯道安全距离为：

公式(28)和(29)中可由图3总速度与纵向分速度的固有三角关系计算得到：

进一步计算得到弯道换道轨迹终点，可通过下式计算得到：

t_a表示车辆SV从t-τ时刻以u_n(t)行驶至换道终点所需要的时间，计算公式如下：

因此，根据前文的Gipps安全距离，将各个参数代入该安全距离模型内，所以该安全距离是表示LV在t时刻的纵向位置。表示PV在t时刻的纵向位置。在移动坐标系内，SV的换道起点始终都得是(0,0),所以每个步长的换道起点坐标都得减去上一步长结束时的坐标终点，故真实的安全区间(见图4)为：

其中，表示SV在t时刻的纵向位置。

图4虚线车辆表示换道开始时刻各车辆的位置。只要换道轨迹终点落在该安全区间中，便不会与目标车道前后车发生碰撞，令则根据前边的计算，避撞轨迹终点的安全区间为

(3)最优参数调整

在最优轨迹算法中，轨迹寻优过程是基于当前的车辆速度以及舒适度权重参数的，当其变化时最优轨迹随之发生变化。而轨迹决策的核心思路就在于通过调整速度与舒适度权重参数，从而使输出的最优轨迹满足防侧翻算法和避撞算法所确定的轨迹安全性约束。即令最优轨迹的终点落在安全区间内。首先我们设定当前步长速度与当前步长舒适度权重参数ω的调整范围，设车辆的最大轴向加速度为a，则可由上一规划步长的速度确定当前步长速度的取值范围，即(代表SV最大减速度)，ω取值范围设为(0,1)。两个参数的初始值分别为和ω。ω是经过真实数据标定得到的舒适性权重参数。在轨迹决策算法中，每输入一组与ω，都需要重新计算与之对应的侧翻极限轨迹纵向坐标最优轨迹以及避撞轨迹终点安全区间。

轨迹规划模块的算法原理见图5，算法输入端为上一时间步长的车辆速度和标定得到的ω，计算出与之对应的侧翻极限轨迹纵向坐标最优轨迹以及避撞轨迹终点安全区间。然后进行第一层决策，比较侧翻极限轨迹纵向坐标与安全区间的关系，有如下三种情况：

1.当位于安全区间的左侧时，即则安全区间保持不变，仍为此时进行第二层决策，比较最优轨迹纵向坐标与安全区间的位置关系，也有如下三种情况：

(1)若大于安全区间的上界，则优先调整速度(减速)，保持当前舒适性权重参数，使最优轨迹向安全区间方向靠拢，直至最优轨迹成为安全区间上界轨迹，当调整速度无法达到目标时开始调整舒适性权重参数(变小)，再调整速度以移动最优轨迹至安全区间上界。此时的速度即为该步长规划输出的速度，而此时的轨迹即为输出的最优轨迹，该轨迹在安全性保证的同时，能够尽量靠近舒适性权重参数ω所确定的最优轨迹。

(2)若小于安全区间下界，则优先调整速度(加速)，保持当前舒适性权重参数，使最优轨迹向安全区间方向靠拢，直至最优轨迹成为安全区间下界轨迹，当调整速度无法达到目标时开始调整舒适性权重参数(变大)，再调整速度以移动最优轨迹至安全区间下界。此时的速度即为该步长规划输出的速度，而此时的轨迹即为输出的最优轨迹。

(3)若在安全区间内，则不需要调整速度和舒适性权重参数。

2.当位于安全区间内时，即此时安全区间改换为此时进行第二层决策，比较最优轨迹纵向坐标与安全区间的位置关系，有如下两种情况：

(1)若大于安全区间上界，则首先调整速度(减速)，保持当前舒适性权重参数，使最优轨迹向安全区间方向靠拢，直至最优轨迹成为安全区间上界轨迹，当调整速度无法达到目标时开始调整舒适性权重参数(变小)，之后再调整速度以移动最优轨迹至安全区间上界。此时的速度即为该步长规划输出的速度，而此时的轨迹即为输出的最优轨迹。

(2)若在安全区间中，则不需要调整速度和舒适性权重参数。

3.当位于安全区间右侧时，即则优先调整速度(减速)，保持当前舒适性权重参数，使最优轨迹向安全区间方向靠拢，直至最优轨迹成为安全区间上界轨迹，当调整速度无法达到目标时开始调整舒适性权重参数(变小)，再调整速度以移动最优轨迹至安全区间上界。此时的速度即为该步长规划输出的速度，而此时的轨迹即为输出的最优轨迹。经过如上的决策，便可以得到当前规划步长中期望的速度与最优轨迹。

此外，在以上算法中，在速度和舒适性权重系数的取值范围内仍然无法使最优轨迹出现在防侧翻安全区域中，则判定为当前交通环境不适合向目标车道换道，于是触发换道中止进程，将原车道指定为新的目标车道，从而重新规划返回原车道的换道轨迹。

二、轨迹输出

经过轨迹规划模块的计算可以得到当前规划步长终点的期望车速以及当前步长的期望舒适性权重参数，从而得到该步长的运动轨迹，最终获得当前步长结束时车辆新的位置和航向角。而轨迹生成模块中，上一步长中规划出的车辆位置与航向角将作为下一步长的初始状态进入下一次轨迹规划中。经过多次迭代后，最终能够生成一条完整的车辆换道轨迹。本文认为在每一步长中车辆执行匀变速曲线运动，接下来对具体过程进行介绍。

(1)换道起点确定

换道的第一步是找到一个合适的换道起点。图6显示了确定变道起始点的过程。设SV在点1处产生换道意图时的速度是为了确保安全，SV应评估是否符合安全性约束，如果满足安全要求，则SV直接规划一个最优轨迹，在这种情况下，点1为变道起点，SV可以利用图6中沿轨迹l₁的速度直接改变车道。如果不安全，应继续纵向移动，同时调整其速度。应用安全性算法，可以推导出经过时间步长的安全换道起始速度，SV将尝试在一个时间步长后达到预期的换道起始速度。如果施加最大加速度也不能达到预期速度，则需要根据之前的速度模型采取更多步长来调整车辆速度以满足换道要求。假设AV在第m点满足安全要求，则换道起点位置计算如下:

(2)换道执行过程

在这个过程，SV会以如下的轨迹方程进行单个步长的执行，并且在每个步长之后会计算新的位置和新的航向角，单个步长的换道轨迹曲线的长度如下所示：

其中，是每个步长结束时的车辆速度。

同时，假如SV可以到达下一个步长的换道起点，这个步长的轨迹曲线长度也可计算如下：

对公式(19)进行求导，我们可以得到航向角的正切值：

其中θ_e是当前步长结束后下一步长新的航线角。

所以，θ_e可以从如下方程得到：

至此，就可以得到当前步长结束时的车辆状态，也就获得了下一步长开始的状态。经过多次迭代后，每一个步长的局部轨迹首尾相接最终到达目标车道中线，形成了一个完整的换道轨迹。图7为某一辆车在换道过程中的轨迹动态调整示意图，经过图7(a)中五次轨迹规划后，形成了图7(b)中一条完整的换道轨迹。

Claims

1.一种变曲率弯道的动态自动驾驶换道轨迹规划方法，其特征在于：包括如下步骤：

步骤一、计算直道换道轨迹曲线；

步骤二、计算弯道换道轨迹曲线；

步骤三、采用防侧翻算法和避撞算法计算轨迹安全性约束；

步骤五、输出最终换道轨迹。

2.根据权利要求1所述的一种变曲率弯道的动态自动驾驶换道轨迹规划方法，其特征在于：按如下公式计算弯道换道轨迹方程：

式中，表示弯道换道轨迹中相对应的横坐标，(x_n,y_n)为虚拟直道换道轨迹上的任一点，该点与原点的连线与x轴的夹角为α，θ_i为规划步长起点的航向角，x_f，y_f分别表示换道轨迹终点纵向坐标和横向坐标，η表示弯道换道轨迹上任意一点与坐标系原点的连线与x轴之间的夹角。

3.根据权利要求1所述一种变曲率弯道的动态自动驾驶换道轨迹规划方法，其特征在于：步骤三所述计算轨迹安全性约束的方法为：

(1)采用防侧翻算法计算不发生侧翻轨迹簇的左边界：

1)当时，按如下公式计算防侧翻轨迹簇的左边界

2)当时，按如下公式计算防侧翻轨迹簇的左边界

式中，Q＝1/x_f，为车辆侧翻对应的临界侧向加速度，u_n(t)为车辆SV在t时刻的总速度；

(2)采用避撞算法计算轨迹终点安全区间：

1)按如下公式计算车辆SV至少应与车辆PV保持的纵向上安全距离S_ps：

式中，为车辆SV在t-τ时刻的纵向速度，表示车辆PV在t-τ时刻的切向速度，b_n为车辆SV的制动最大减速度，b_n-1为车辆PV的制动最大减速度；

2)按如下公式计算在直道情况下车辆LV至少应与车辆SV保持的安全距离s_ls：

式中，表示t-τ时刻车辆LV的切向速度，b_n+1为车辆LV的制动最大减速度，

3)按如下公式计算车辆SV应与车辆PV保持的弯道安全距离s_f：

式中，f(x)是目标车道中心线的曲线函数；

4)按如下公式计算车辆LV应与车辆SV保持的弯道安全距离s_r：

5)按如下公式计算弯道换道轨迹终点：

其中，表示车辆LV在换道完成时的纵向位置，表示PV车在换道完成时的纵向位置，t_a表示车辆SV从t-τ时刻以u_n行驶至换道终点所需要的时间；

6)得到避撞轨迹终点的安全区间为其中l_n表示SV的车身长度，l_n-1表示PV的车身长度。

4.根据权利要求3所述的一种变曲率弯道的动态自动驾驶换道轨迹规划方法，其特征在于：步骤四所述确定满足轨迹安全性约束的最优轨迹的方法为：

(1)根据上一时间步长的车辆速度和标定得到的ω，计算出与之对应的侧翻极限轨迹纵向坐标最优轨迹终点的纵向位置以及避撞轨迹终点安全区间；

(2)判断侧翻极限轨迹纵向坐标与安全区间的关系：

1)当位于安全区间的左侧时，安全区间保持不变，进一步判断与安全区间的位置关系：

a)若大于安全区间的上界，则先减速，保持当前舒适性权重参数，使最优轨迹向安全区间方向靠拢，直至最优轨迹成为安全区间上界轨迹，当减速无法达到目标时，则减小舒适性权重参数，再调整速度以移动最优轨迹至安全区间上界；

b)若小于安全区间下界，则先加速，保持当前舒适性权重参数，使最优轨迹向安全区间方向靠拢，直至最优轨迹成为安全区间下界轨迹，当加速无法达到目标时，则增大舒适性权重参数，再调整速度以移动最优轨迹至安全区间下界；

c)若在安全区间内，保持速度和舒适性权重参数不变；

2)当位于安全区间内时，则将安全区间改换为然后判断与安全区间的位置关系：

a)若大于安全区间上界，则先减速，保持当前舒适性权重参数，使最优轨迹向安全区间方向靠拢，直至最优轨迹成为安全区间上界轨迹，当减速无法达到目标时，则先减小舒适性权重参数，然后再调整速度以移动最优轨迹至安全区间上界；

b)若在安全区间中，保持速度和舒适性权重参数不变；

3)当位于安全区间右侧时，则先减速，保持当前舒适性权重参数，使最优轨迹向安全区间方向靠拢，直至最优轨迹成为安全区间上界轨迹，当减速无法达到目标时则先减小舒适性权重参数，再调整速度以移动最优轨迹至安全区间上界。

5.根据权利要求4所述的一种变曲率弯道的动态自动驾驶换道轨迹规划方法，其特征在于：步骤五所述输出最终换道轨迹的方法为：

(1)按如下公式计算换道起点的位置：

式中，表示SV在第m点产生换道意图时的位置，表示SV在第1点产生换道意图时的位置，表示SV在点k处产生换道意图时的速度；

(2)按如下公式计算单个步长的换道轨迹曲线的长度L：

式中，是每个步长结束时的车辆速度；

(3)按如下公式计算航向角的正切值：

其中θ_e是当前步长结束后下一步长新的航线角。