自动驾驶车辆自适应换道轨迹规划方法与流程

本发明涉及一种自动驾驶车辆自适应换道轨迹规划方法。

背景技术：

近年来，自动驾驶技术受到了社会的广泛关注，被认为是缓解交通拥堵、减少交通事故和减少环境污染的重要技术。其中，车辆换道是车辆行驶的基本操作之一，也是造成交通事故的重要原因，根据中国公路交通安全管理局报告显示，60％以上的高速公路交通事故与车辆换道有关。由此，自动驾驶车辆换道轨迹是自动驾驶研究的重点，也是有待解决的技术难点。

现有自动驾驶车辆换道轨迹研究方法包括几何曲线法、搜索方法和模型预测控方法，其中以几何曲线为主。利用几何曲线规划车辆换道轨迹的方法中，根据表征换道轨迹的几何曲线的类型，细分为多项式轨迹、正弦函数、b样条函数和贝塞尔曲线等方法。nelson最早在提出多项式作为车辆换道轨迹曲线，利用连续曲率线段弥补圆弧线段在换道轨迹中不连续的问题。随后多项式方法以三次多项式和五次多项式为主，yang等人提出动态的车辆换道轨迹规划方法，将横纵坐标的三次多项式作为车辆换道轨迹。luo等人在研究了车联网环境下的动态车辆换道轨迹规划，采用了安全距离模型实现避撞，并构建了基于时间的多项式轨迹来表征换道轨迹。piazzi和norouzi等人首次用五次多项式曲线作为车辆的换道轨迹。bai等人研究了车辆协作环境下的车辆换道，将五次多项式作为车辆的换道轨迹。

除了多项式方法外，还有其他几何曲线算法用于规划车辆换道轨迹。chee和tomizuka评估了不同的轨迹规划方法，认为梯形加速度更能满足曲线的连续性，换道时间较短还能体现轨迹对横向加速度的约束。yang等人认为车辆在换道开始和结束时车辆转向角都应为0度，且车辆的横向加速度必须连续，因此提出了等速偏移和正弦函数叠加的轨迹规划模型。li等人以b样条曲线作为车辆换道的轨迹，保证曲线的连续性，且能实现避撞，但是在复杂环境下b样条轨迹规划方法计算速度较慢，因此并不适用于高速和复杂的换道环境。chen等人认为b样条曲线无法约束和控制车辆最大横向加速度，因此提出贝塞尔曲线作为车辆的换道轨迹，但是对于换道过程中的障碍物，贝塞尔曲线控制点的选取问题得不到解决。

此外，wolf和burdick首次提出了高速公路环境下的车辆换道的人工势场法，用势场来表征环境空间，通过求解势场中势能下降最快的方向来规划换道轨迹。li等人基于状态空间抽样的方法规划局部轨迹簇，生成满足安全和舒适的最优轨迹。不少研究者将模型预测控制方法应用到自动驾驶车辆轨迹规划研究中，schildbach和borrelli利用模型预测控制方法预测周围车辆的可能轨迹，对换道车辆的位置、方向、速度、加速度和安全距离等参数进行约束，用以产生满足舒适性和安全性的轨迹。nilsson等人首次在车辆换道轨迹研究中考虑车辆纵向和横向的安全间距，将车辆换道轨迹规划问题转变为解决纵向和横向运动的松散耦合的模型的预测控制问题。但是该方法的计算较复杂，对于复杂的换道环境，难以快速实时地反应和执行操作。

以上是当前的研究方法，其存在着一些不足之处。第一，模型对车辆换道环境的假设太强。上述研究中设定，换道过程中周围车辆的速度恒定不变，这与真实的驾驶环境不符。第二，当前的研究中换道车辆无法根据实时响应周围车辆状态的变化，即无法实时的调整车辆的速度和车辆间的距离，因而这些模型在真实交通环境中可能失效。

鉴于此，yang等人提出了动态换道轨迹规划模型，利用横纵坐标的多项式来表征换道轨迹，并在每个步长内，通过成本函数权衡换道过程中舒适性与效率从而得到最优轨迹，再通过调整车辆速度与舒适度权重参数，输出满足安全约束的动态最优换道轨迹。然而，该模型也存在一些不足之处。首先，该模型中只考虑了目标车道车辆对待换道车辆的影响，忽略了当前车道车辆对待换道车辆的影响，这与实际换道场景不符；第二，该模型只研究了换道车辆从当前车道至目标车道的横向位移过程，忽略了换道车辆在横向位移过程中的调整。第三，避撞模型不完善，该文献中的避撞模型只考虑车辆换道完成位置，换道车辆与周围车辆的安全间隙，忽略了车辆换道过程中的安全性。

技术实现要素：

为了克服现有技术的上述缺点，本发明提出了一种自动驾驶车辆自适应换道轨迹规划方法，实现换道过程中自动驾驶车辆对交通环境变化的自动调整，提高换道效率和安全性。首先在原有的换道轨迹前增加一段纵向换道轨迹，让车辆在纵向轨迹上调整自身速度和与目标车道间的相对位置，然后再进行横向换道移动，保证当前车辆换道的安全性和效率性；第二，该模型中增加了换道返回轨迹。考虑了车辆在换道过程中当换道条件不符合安全约束时，规划车辆安全返回当前车道的轨迹；第三，该模型引入新的车辆换道避撞算法，根据自动驾驶车辆车身姿态变化对换道过程中安全的影响，进而提出了车辆冲突区域的概念，保证车辆完成换道以后的安全性约束。

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

步骤一、判断换道轨迹规划起点的横坐标是否小于等于第一冲突区域结束时换道车辆的横坐标：若是，则进入步骤二；若否，则进入步骤三；

步骤二、利用最优轨迹算法规划出最优换道轨迹，然后判断最优换道轨迹是否同时满足第一安全约束条件和第二安全约束条件：若是，则车辆执行最优换道轨迹到下一规划步长；若否，则利用避撞算法调整最优换道轨迹的换道时间和加速度生成安全最优轨迹：若能生成安全最优轨迹，则车辆执行安全最优轨迹换道到下一规划步长，若无法生成安全最优轨迹，则判断车辆是否发生横向位移：

(1)若否，则利用避撞算法调整换道时间和加速度，若仍然无法生成安全最优轨迹，则在当前车道继续调整车辆速度和相对距离，等待下一次换道机会；

(2)若是，则利用避撞算法调整换道时间和加速度，若仍然无法生成安全最优轨迹，则根据最优轨迹算法规划出的返回轨迹，返回至当前车道继续调整车辆速度和相对距离，等待下一次换道机会；

步骤三、判断车辆是否完成换道：若是，则进入车辆跟驰操作；若否，则利用最优轨迹算法规划出最优换道轨迹，然后判断最优换道轨迹是否满足第二安全约束条件：若是，则车辆执行最优换道轨迹到下一规划步长；若否，则利用避撞算法调整最优换道轨迹的换道时间和加速度生成安全最优轨迹：若能生成安全最优轨迹，则车辆执行安全最优轨迹换道到下一规划步长，若无法生成安全最优轨迹，则判断当前步长的横坐标是否小于换道车辆完全离开当前车道时的横坐标：

(1)若是，则利用避撞算法调整换道时间和加速度，若仍然无法生成安全最优轨迹，则根据规划出的返回轨迹，返回至当前车道继续调整车辆速度和相对距离，等待下一次换道机会；

(2)若否，则利用避撞算法调整换道时间和加速度，若仍然无法生成安全最优轨迹，则车辆在当前位置继续调整速度和相对距离，并不断向目标车道靠近，直至完成换道。

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

1)效率性：本发明充分考虑到自动驾驶车辆自身行驶状态和周边车辆行驶状态，从而增加了自动驾驶车辆换道前的纵向行驶准备过程，然后待换道车辆再进行横向移动。该方法实现了自动驾驶车辆换道过程的完整性，保证了车辆的成功换道，同时纵向行驶准备过程还可以很好的连接换道决策和执行过程，进而提高了车辆换道过程的效率。

2)动态实时性：本发明中自动驾驶车辆在换道准备和换道过程中能够对周边场景的实时变化做出动态响应。在换道准备过程中对自动驾驶车辆自身速度和位置根据周边环境变化而实时调整，当换道过程的轨迹规划中，出现换道条件不符合安全约束时，还规划了车辆安全返回当前车道的轨迹。

3)安全性：本发明生成了基于反应时间的避撞算法来保证了车辆在换道准备和执行过程的安全性。避撞算法由安全条件和冲突区域两部分组成。即换道车辆紧急制动或前车紧急制动的情景下依然可以保证不发生碰撞；冲突区域利用几何特性确定，即车辆车身的轮廓线与周围车辆车身轮廓延长线相交区域，考虑轨迹在各个相交区域内的安全性，从而生成满足各个区域的安全约束。进而最大程度保障了车辆在换道过程中的安全性。

附图说明

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

图1为自动驾驶车辆换道场景示意图；

图2为安全距离示意图；

图3为车辆换道冲突区域示意图；

图4为车辆车身端点的位置坐标图；

图5为车辆自适应换道轨迹规划原理框架图；

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

具体实施方式

一种自动驾驶车辆自适应换道轨迹规划方法，以下对本发明内容描述如下：

一、轨迹规划模块

自动驾驶车辆换道决策模块主要包括最优轨迹算法、避撞算法和轨迹决策三部分。其中，最优轨迹算法用以计算给定舒适性权重参数条件下的最优轨迹；避撞算法确定车辆换道的安全约束，轨迹决策算法根据车辆换道所在的位置，引用安全约束判断车辆最优轨迹的安全性，并实现对最优换道轨迹的调整，使换道轨迹更适应于换道环境，最终生成安全的决策结果，确定车辆下一步长的驾驶行为。

(1)最优轨迹算法

车辆在每一个规划步长内，根据实时的环境信息，利用最优轨迹算法，规划出一条满足车辆换道效率和舒适性的最优轨迹。最优轨迹由车辆换道轨迹和返回轨迹组成，其中车辆返回轨迹作为车辆换道轨迹规划的安全机制，当发生紧急情况而不能执行换道时，车辆根据返回轨迹返回至原车道，等待下一次换道机会。

如图1所示，本算法建立的自适应换道轨迹由直线和曲线组成，车辆在面对苛刻的无法直接换道的环境时，先利用直线轨迹在当前车道行驶，以调整自身速度和与周围车辆的相对距离，创造出满足安全需要的换道条件，然后再利用曲线轨迹完成从当前车道至目标车道的横向换道过程。自适应轨迹规划模型中，其中直线部分是车辆沿着当前车道中心线匀变速行驶，如下式：

式中，x(t0)为车辆n换道的初始位置，v(t0)为车辆的初速度，a1为直线轨迹的加速度，tb为车辆n的直线行驶时间。

当车辆结束在直线轨迹上的速度和位置调整后，开始发生横向位移，向目标车道方向行驶。利用不依赖时间的三次多项式表征车辆发生横向位移的横向换道曲线轨迹，三次多项式能保证换道轨迹曲率的连续性，使车辆横纵向坐标结合为一体，保证了换道曲线的圆滑性，更符合真实的换道轨迹。如下式：

y(x)＝a0+a1x+a2x²+a3x³(2)

其中，a0、a1、a2、a3为待确定的参数，x为纵向上车辆n的位置，y为横向上车辆n的位置。车辆在换道过程中每一个步长都会规划出一条新的换道轨迹，该轨迹以当前位置为起点，以目标车道中线上的某一位置为终点，而终点处车辆的运动方向与车道平行。在该模型中使用移动坐标系，将当前规划步长的起点位置定义为(0,0)点，终点位置定义为(xf,yf)，当前规划步长起点的车辆航向角为θi，终点的车辆航向角为0，则有：

式(3)中θi为规划步长起点的航向角，yf为终点横向坐标，均为已知量。所以，轨迹方程由换道轨迹终点纵向坐标xf唯一确定。车辆n在曲线轨迹的纵向位移xf根据车辆的运动学原理和曲线弧长求解，车辆在发生横向位移的换道过程的弧长为：

式中，v(tb)为车辆开始发生横向位移时的速度，为曲线轨迹的切向加速，tc表示车辆横向换道的行驶时间。其中v(tb)的表达式为：

v(tb)＝v(t0)+a1tb(5)

使用矩形区域积分quad方法可计算弧长如下：

其中y′(x)为车辆换道轨迹的一阶倒数，可以由轨迹y(x)方程得到：

使车辆发生横向位移的曲线轨迹长和弧长的值相等，即(4)和(6)相等，求解出车辆n在横向换道过程中的纵向总位移xf。

在相同坐标系下，使用横纵坐标的三次多项式表征车辆返回轨迹，车辆在规划起点的位置坐标为(x0,y0)，同理，将每一步长的起点位置定义为(0,0)点，终点位置为(xf,-y0)，即用-y0替换换道轨迹中的yf，带入式(3)中，得到车辆返回轨迹的表达式，

式(3)的换道轨迹和式(8)的返回轨迹由xf唯一确定，而由式(4)(5)(6)可知，xf与车辆初始速度、加速度和换道时间有关，为求解横向换道的纵向换道总位移xf，只需确定车辆换道加速度和换道时间。在真实换道过程中，驾驶员总是希望以最快的速度完成换道，并且保证乘坐的舒适性，但是这两个因素之间是相互矛盾的，自动驾驶车辆在换道时需要对这两个因素需进行权衡考虑。因此，该模型中构造了一个自适应换道成本函数来表征车辆在换道中的效率和舒适性。成本函数中使用直线和曲线两个过程中的最大加速度代表舒适度，加速度越大则该自适应换道轨迹的舒适度越差，另一方面采用换道所需要的总时间代表效率，其中tf＝tb+tc，总时间越长则该最优换道轨迹的效率越低。为了使两个因素在一个数量级上进行评估，对它们进行归一化处理。建立的最优化轨迹模型具体表达式如下：

式中，a2为车辆横向换道的加速度，沿换道轨迹切线方向将其分解为切向加速度和法向加速度a2的大小由切向加速度和法向加速度的大小决定，其中大小保持不变，因此法向加速度越大，车辆n的总加速度a2越大，当车辆法向加速度最大，车辆的总加速度a2最大：

式(10)中，a2max为车辆横向位移的最大加速度，为车辆的最大法向加速度。

其中，换道曲线轨迹的切向加速度大小不变，车辆匀变速行驶，根据车辆n在起点时刻t和换道完成时刻tf的速度，求解得到如下式所示：

式(10)中，车辆法向加速度与车辆速度和轨迹曲率有关，

式中，k为换道轨迹的曲率函数，计算公式如下：

将y′(xi)和y″(xi)的表达式，带入到轨迹的曲率函数k的表达中，得到曲率k关于xi的函数(0≤xi≤xf，yf≥0)，其表达式如下：

根据车辆曲率的变化情况，随着xi的变化，轨迹曲率先均匀变小至零，然后均匀变大，因此车辆n在换道开始和换道完成时车辆的轨迹曲率半径最大。换道过程中车辆n的速度v(t)均匀变化，在换道起点或换道完成点速度最大，因此车辆n在换道起点或换道完成点处法向加速度最大，即在横向换道过程中，总加速度a2在换道起点或换道完成点处最大。

(2)避撞算法

避撞算法从车辆换道安全性和效率两个方面入手，车辆的横向和纵向位置关系确定车辆安全约束的适用范围，保障车辆换道安全性的同时又不影响车辆换道的效率。

1.安全条件

自动驾驶领域对安全模型进行了较多的研究，其中gipps模型是较为经典的车辆跟驰模型，考虑了车辆与前车的关系和驾驶员自身的特性，进而引入车辆换道的行驶特性对gipps模型进行改进，生成适用于自动驾驶车辆换道特性的安全条件。

车辆换道过程中受到周围车辆实时的影响，为了保证换道过程的安全性，需要实时检测周围车辆的行驶状态，并对周围车辆的行驶状态进行预测。设车辆n在换道过程中对周围车辆的驾驶行为不造成产生影响，周围车辆以当前状态继续行驶。

上式中，j分别表示车辆n-1(即当前车道前车)、车辆n-2(即目标车道前车)、车辆n+2(即目标车道后车)，xj(t0)和vj分别表示当前状态各个车辆的纵向位置坐标和速度，xj(t)表示t时刻车辆j的纵向位置坐标。

gipps模型是基于安全行驶距离来建立的，它考虑了车辆的安全行驶距离约束和最大加速度的约束。gipps理论假设当前车遇到紧急状况而猛然减速时，后车也紧急刹车，而不发生碰撞。传统的gipps模型未考虑各个车辆的车长情况，认为车辆以当前速度匀速行驶。对此，本模型将车辆的车型考虑在内，建立适用于车辆换道的关于车辆质心的安全模型。图2中计算出车辆n和车辆n-1的紧急刹车距离，如下所示：

上式中，分别为车辆n-1和车辆n的刹车距离，xn-1(t)、x(t)分别表示车辆n-1和车辆n在t时刻的纵向位置坐标，vn-1(t)、v(t)分别表示车辆n-1和车辆n在t时刻的速度，bn-1、b分别表示车辆n-1和车辆n的最大减速度，a为车辆n的加速度，τ为后车的反应时间。

当车辆n-1紧急制动时，车辆间刚好不发生碰撞的间隙为为车辆的最小安全间隙。同理可计算得到车辆n与车辆n-2的最小安全间隙

上式中，ln-1，ln-2分别表示车辆n-1、车辆n-2的车身长度，xn-2(t)为车辆n-2在t时刻的纵向位置坐标，vn-2(t)表示车辆n-2在t时刻的速度，bn-2表示车辆n-2的最大减速度。

同理，利用改进后的新安全换道模型计算车辆n和车辆n+2之间的最小安全间隙车辆n在系统反应时间τ后执行紧急刹车操作，车辆n+2检测到车辆n紧急制动，经过系统反应时间τ后执行紧急刹车，由此得到车辆n和车辆n+2的最小安全间隙

上式中，ln+2为车辆n+2的车身长度，xn+2(t)表示车辆n+2在t时刻的纵向位置坐标，vn+2(t)表示车辆n+2在t时刻的速度，bn+2表示车辆n+2的最大减速度。

车辆换道的安全性与车辆速度也有关，根据改进的gipps安全模型计算车辆的安全速度。由式(20)、(21)和(22)中的最小安全间隙和推算出车辆n的安全速度范围。车辆n受到车辆n-1影响的最大安全速度为：

同理，根据车辆n和车辆n-2、车辆n-2的最小距离和推算出车辆n分别在车辆n-2和车辆n+2影响下的最大安全速度和最小安全速度

2.冲突区域确定

模型以向左换道为例，根据车辆车身的几何特性，以及换道车辆在换道过程中的姿态变化，从横向和纵向分析换道车辆与环境车辆的位置变化，确定车辆n与周围车辆的冲突区域。如图3所示，车辆n在当前车道行驶时，车辆n的车身与车辆n-1的车身横向重叠，车辆间存在冲突，当前步长车辆n车身与车辆n-1车身相切时，那么车辆行驶至步长结束时，车辆间的车身不再相交，即车辆间不会因为纵向距离小而发生碰撞，此处，将车辆换道的这段相交区域定义为车辆n与车辆n-1的冲突区域g1。同理，当将车辆n车身与目标车道车辆车身延长线相交的位置，到车辆完成换道的范围定义为车辆n与目标车道车辆的冲突范围g2。

换道过程中车辆间的几何关系如图3所示，利用长方形表示车辆车身，以车辆n换道开始时车辆质心的位置为原心建立坐标系。当车辆n质心的坐标为(x,y)时，根据车辆n的车身长度和航向角，确定车辆车身p点的坐标为(x+l/2cos(θ),y+l/2sin(θ))，再根据车辆n的车身宽度和航向角，分别计算出p1、p2、p3和p4端点的位置坐标，如表1所示。

表1车辆n车身端点坐标

以向左换道为例，如图4所示。计算车辆n与目标车道车辆冲突区域g2左端点位置和时间，g2开始位置为车辆n的左前端点p1与目标车道车辆车身的相切位置，此时，p1的横坐标与目标车道车辆车身的右轮廓线的横坐标相等。设g2冲突开始时车辆质心的位置坐标为(x1,y1)，航向角为θ1。此时，车辆左前端点p1的坐标为(x1+l/2cos(θ1)-w/2sin(θ1),y1+l/2sin(θ1)+w/2cos(θ1))。此时车辆左前端点p1的横向坐标满足：

其中，l为车辆车身的长，w为车辆车身的宽，w为车道宽。由此计算出y1，然后根据车辆换道轨迹式(3)反推出车辆的横坐标x1。然后再把轨迹的规划起点x和x1带入到公式(4)和公式(6)反解出车辆n与目标车道车辆冲突开始时的时刻t1。

同理，求解车辆n与车辆n-1的冲突区域g1右端点位置，g1的结束位置为车辆n的右后端点p3与车辆n-1的车身相切位置，此时，p3的横坐标与车辆n-1车身的左轮廓线的横坐标相等。设g1冲突结束时车辆n的位置坐标为(x2,y2)，航向角为θ2，此时p3的坐标(x2-l/2cos(θ2)+w/2sin(θ2),y2-l/2sin(θ2)-w/2cos(θ2))，因此：

同样，利用(27)计算此时车辆的横坐标y2，然后再根据轨迹反推，最后得到车辆冲突g1结束时的位置坐标(x2,y2)，以及车辆n与当前车道前车n-1冲突结束时的时刻t2。

车辆n完全离开当前车道时，车辆n车身上的p3与当前车道的轮廓线相切，同理，应用相同的方法可计算出车辆n完全离开当前车道时车辆质心的位置坐标和时间。设车辆n完全离开当前车道时的位置坐标为(x3,y3)，航向角为θ3。因此存在以下关系：

同理，得到车辆完全离开目标车道时的位置坐标为(x3,y3)，以及车辆n完全离开当前车道时的时间t3。

由以上安全模型和冲突区域的信息，可计算出以车辆n质心为参考系的换道轨迹的安全约束。在冲突区域g1内的换道轨迹部分，即车辆横向坐标y(t)在(0,y2)，而此时车辆换道时刻t在(t0,t2)范围内，那么车辆的换道轨迹的位置坐标必须满足此时车辆换道速度的约束为模型中将车辆n在冲突区域g1内的位置和速度约束设为安全约束1。同理，计算车辆n在冲突区域g2的安全约束。在冲突区域g2内的车辆换道轨迹部分，即车辆横向坐标y(t)在(y1,w)，而此刻车辆换道时间t在(t1,tf)范围内，车辆的换道轨迹的位置坐标x(t)满足且此刻车辆换道速度v(t)满足在该模型中，将车辆在冲突区域g2内的位置和速度约束设为安全约束2。

(3)轨迹决策

自动驾驶车辆在最优轨迹算法中规划出满足效率和舒适性的最优换道轨迹，但无法确定该轨迹的安全性，由此建立车辆换道轨迹决策模型，保障车辆换道的安全性，提升轨迹规划模型的自适用性。换道轨迹决策的核心是根据换道车辆当前位置和周围车辆的行驶状态，判断最优轨迹是否满足安全约束，并利用避撞算法调整最优轨迹的换道时间和加速度，生成安全最优轨迹，最终生成安全的换道轨迹决策结果，以提高模型的自适应性。

车辆n在换道过程中受到当前车道前车和目标车道车辆的共同影响，但是随着车辆横向位置的变化，车辆n-1对车辆n换道的影响变小，当车辆n离开冲突区域g1后，在横向上，车辆n不再与车辆n-1发生冲突，车辆n-1不再对车辆换道产生影响。车辆换道轨迹的当前规划起点在不同的区间时，最优轨迹的安全约束要求不同，且最终生成的轨迹决策结果也不同。具体的决策框架如图5所示。

1.若换道轨迹规划起点的横坐标y(t)满足y(t)≤y2，即x(t)在g1区间内。车辆换道受到车辆n-1和目标车道车辆的共同影响，此时车辆规划的换道轨迹必须同时满足安全条件1和安全条件2。在换道轨迹决策算法中，避撞算法首先判断最优轨迹的安全性，若最优轨迹满足安全约束，车辆执行换道轨迹到下一规划步长；若最优轨迹不满足安全约束，为了提高模型的适应性，车辆根据避撞算法调整最优轨迹的换道时间和加速度，生成安全最优轨迹。如果调整后，生成安全最优轨迹，车辆执行换道至下一步长，如果无法生成安全最优轨迹，则根据车辆当前步长所在位置存在以下两种情况。

(1)若当前规划步长的横坐标满足y(t)＝0时，车辆n未发生横向位移，在当前车道上沿直线行驶。如果调整后仍然无法生成安全最优轨迹，继续在当前车道继续调整车辆速度和相对距离，等待下一次换道机会。

(2)若当前规划步长的横坐标满足y(t)>0时，车辆n已经发生横向位移，不断向目标车道靠近。如果调整后仍然无法生成安全最优轨迹，车辆n根据规划出的返回轨迹，返回至当前车道继续调整车辆的速度和相对距离，等待下一次换道机会。

2.若换道轨迹规划起点的横坐标y(t)满足y(t)>y2，即x(t)不在g1区间内。首先确定车辆换道是否完成，当y(t)＝w时，车辆完成换道操作，进入车辆跟驰操作；否则，对换道轨迹进行决策，继续换道操作。若最优轨迹满足安全约束，车辆执行换道到下一规划步长，否则利用避撞算法调整最优轨迹，使生成安全最优轨迹，若生成安全最优轨迹，车辆执行换道轨迹到下一规划步长；若无法生成安全最优轨迹，则根据车辆当前步长所在位置存在以下两种情况。

(1)若当前步长的横坐标y(t)满足y2<y(t)<y3，车辆n-1不影响车辆n换道过程，但车辆n车身未完全离开当前车道。在调整后仍然无法生成安全最优轨迹时，车辆n根据规划出的返回轨迹，返回至原车道，继续调整车辆速度和相对距离，等待下一次换道机会。

(2)若当前步长的横坐标y(t)满足y3<y(t)<w，车辆n车身已经完全离开当前车道，换道已经接近完成。若在调整后仍无法生成安全最优轨迹，车辆n在当前位置继续调整速度和相对距离，并不断向目标车道靠近，直至完成换道。此过程可能对目标车道车辆造成影响。

二、换道轨迹生成模块

经过车辆换道轨迹决策模块的计算，可以得到当前规划步长的期望换道时间和期望加速度，从而得到该步长的运动轨迹，最终获得当前步长结束时的新位置、新速度和新航向角。而轨迹生成模块中，上一步长中规划出的车辆位置与航向角，将作为下一步长的初始状态进入下一次的轨迹规划中。模型认为车辆在每一步长中都是匀变速运动，在多次迭代之后，最终生成一条完整的车辆换道轨迹。

设定车辆n在当前步长的坐标为(x(t),y(t))，车辆的速度为v(t)，车辆的航向角θ(t)，根据车辆的换道轨迹可以确定当前规划步长结束时，也就是下一个规划步长车辆位置坐标和速度。

v(t+τ)＝v(t)+τa(29)

x(t+τ)＝x(t)+xi(30)

上式中，xi为车辆在当前规划步长的纵向位移，x(t)为车辆下一个步长的纵向位置坐标，a为车辆加速度，当轨迹起点步长在直线轨迹上时a＝a1，当步长在曲线轨迹上时可以直接根据最优轨迹的加速度计算出车辆在当前步长结束时的速度v(t+τ)。车辆在当前步长的纵向位移xi的求解与车辆所在位置有关，具体分成了两种情况，进行求解。

(1)当θ(t)＝0在当前规划步长内不发生横向位移时，车辆n在当前车道上继续行驶。此时，车辆换道轨迹由直线和曲线组成，当前规划步长的轨迹为直线。根据最优轨迹算法计算出直线轨迹的加速度a1，那么xi为：

(2)当θ(t)>0时，车辆n发生横向换道，此时车辆的换道轨迹为三次多项式曲线。由最优轨迹算法计算出曲线换道轨迹的总位移xf、切向加速度再利用曲线积分公式计算仿真步长内车辆行驶的弧长li，以及根据运动学原理计算出仿真步长内车辆的行驶弧长li。

令公式(32)和公式(33)相等，计算出单位步长内车辆行驶的纵向位移xi，由式(30)得到下一个步长的纵向坐标x(t+τ)，再将x(t)带入轨迹方程(3)中，计算得到下一个步长的横向坐标y(t+τ)，从而确定下一个步长的位置(x(t+τ),y(t+τ))。接下来计算步长结束时车辆的航向角，对于换道轨迹上的任意一点车辆的航向角θ(t)，它和轨迹方程的一阶导函数有如下关系式：

将x＝xi代入到式(34)中可以得到当前步长结束时车辆的航向角如下：

至此，就可以得到当前步长结束时的车辆状态，步长结束的坐标(x(t+τ),y(t+τ))，车辆的速度为v(t+τ)，车辆的航向角θ(t+τ)，也就获得了下一步长开始的状态。经过多次迭代后，每一个步长的局部轨迹首尾相接，最终使换道轨迹到达目标车道的中心线，形成一个完整的自适应换道轨迹。车辆在换道过程中轨迹的自适应调整过程如图6所示，图中横纵坐标的单位均为米。由于车辆在每个规划步长内的速度不相同，因此各个步长的横向和纵向位移也不相同。换道车辆在经过图6(a)中五次轨迹规划的迭代以后，形成了如图6(b)所示的一条完整的自适应换道轨迹。