一种基于动态轨迹规划的自动驾驶队列协同换道控制策略

本发明涉及自动驾驶队列控制领域，具体涉及到一种基于动态轨迹规划的自动驾驶队列协同换道控制策略。

背景技术：

1、随着智能交通系统(its)的快速发展和自动驾驶汽车的出现，自动驾驶车辆作为智能交通系统的典型应用之一，具备了环境感知和自主决策的智能特性。然而，单个车辆的智能化意义并不显著，多个车辆的智能化协同互联则具备更大的潜力。因此，目前的研究热点已从单车智能化转向以多车协同控制为核心的多车智能化。

2、当前队列协同控制研究主要集中在队列纵向一致性控制方面，对于单队列行驶中可能出现的车辆切入、切出等行为以及多队列之间的换道协同行为研究较少。另外，目前已有的换道轨迹研究也有部分可参考模型，如基于五次多项式的换道轨迹、贝尔塞曲线轨迹规划等，但这些模型的换道轨迹计算一般较为复杂或是经优化难以达到理想效果。为了提高道路运行效率和安全性，需建立基于智联网条件下的车辆横纵向协同控制的模型，以灵活面对自动驾驶队列行驶中可能出现的各种换道行为。

3、本发明针对双队列行驶中自由单车进入车辆队列的典型协同换道场景进行分析，开展网联自动驾驶车辆队列协同换道控制策略的研究，提出一种基于动态轨迹规划的自动驾驶队列协同换道控制策略。结合纵向队列协同和横向换道轨迹规划，设计了前车跟随式(pf)与前车-领航者跟随式(plf)相结合的通信拓扑结构，利用连续可导的正弦曲线动态跟踪车辆行驶轨迹，提高了换道行为的安全性，提升道路交通运行效率。

技术实现思路

1、本发明面向城市快速路提出了一种基于动态轨迹规划的自动驾驶队列协同换道控制策略，针对双队列行驶中自由单车进入车辆队列的典型协同换道场景，构建了包含车车通信、协同控制和决策规划的协同换道控制架构。此外，在纵向上构建了基于分布式控制的队列纵向协同控制模型，设计了车辆在不同换道阶段之间的控制律切换逻辑；在横向上建立了动态的队列协同换道决策规划与跟踪控制方法。本方法可以在保证自由单车成功换道的前提下，提高车辆换道效率，同时保持车辆队列行驶的稳定性，减少换道行为带来的速度波动等负面影响。

2、该方法的具体实现步骤如下：

3、步骤1，在非换道期间，基于四元素模型构建单个队列的纵向协同控制模型，采用plf通信拓扑结构，使队列中的单个车辆利用邻域车辆的信息做出控制决策，从而达到队列全局的协调目标，使车辆队列稳定行驶。步骤1的具体实现步骤如下：

4、步骤11车辆纵向动力学建模

5、队列纵向协同控制问题主要考虑车辆操纵稳定性，采用车辆-轮胎模型对单个车辆的纵向动力学进行建模。为了便于理论分析，采用反馈线性化技术对模型进行线性化处理，并采用前向欧拉公式队纵向动力学的三阶状态空间方程式进行离散化。

6、步骤12期望间距策略选择

7、采用恒定车头时距策略，设置车辆队列纵向控制目标，即当领航车处于稳态行驶状态时，队列中相邻两辆车的车头通过同一断面的时间间隔相同。

8、步骤2，自由单车通过车车通信向队列头车发出换道请求，队列头车基于队列中所有车辆和换道车辆状态信息判断从哪两辆车中间插入，并返回新的队列id信息给换道车辆。步骤2的具体实现步骤如下：

9、步骤21，基于步骤1纵向控制策略获得稳定行驶队列后，行驶中的自由单车向队列头车发出换道请求。

10、步骤22，队列头车进行插入位置的决策

11、基于距离最近原则选择一个初始插入位置。在此基础上进一步考虑稳定性影响，根据换道请求时刻换道车辆与目标队列各车辆速度差的不同来进一步确定插入位置：

12、当换道车辆sv与车辆i速度相同时，不需要考虑速度差的影响，选择最近的空隙插入。插入位置n0表达式如下所示:

13、

14、其中，psv(tp)和pi(tp)分别为tp时刻换道车辆sv和车辆i的纵向位置。

15、当换道车辆sv与车辆i有速度差时，若换道车辆sv的速度大于目标队列中车辆i的速度时，如果选择插在车辆i前方的某个位置，换道车辆sv以较小的加速度和较短的时间即可达到换道触发条件，且有利于队列恢复稳定，故此情况下插入位置选择范围为n0∈(1,i]；当换道车辆sv的速度小于队列中车辆i的速度时同理，表达式如下：

16、

17、其中，vsv(tp)和vi(tp)分别为tp时刻换道车辆sv和车辆i的速度。

18、步骤3，换道车辆sv和插入位置后方车辆根据新的队列id信息调整速度，直至满足换道触发条件。并在车辆换道期间，车辆队列采用前车跟随式(pf)与前车-领航者跟随式(plf)相结合的通信拓扑结构。步骤3的具体实现步骤如下：

19、步骤31，换道车辆sv接收到来自目标队列头车返回的队列id信息后，根据队列纵向协同控制策略，根据新的队列id返回的控制量进行速度调整，以达到换道触发条件。同时，目标队列插入位置的后方车辆也根据新的队列id信息调整自身车速，以满足换道触发条件。

20、步骤32，考虑目标队列插入位置前后车与换道车辆sv相对速度大小的不同，设置不同的换道触发条件。

21、首先，考虑一个最小安全距离，其值为换道安全系数λ与车长l的乘积；在最小安全距离基础上，设置一个动态的换道安全间距，以保证在换道结束时刻换道车辆sv不会和前车或后车发生碰撞；

22、其次，换道车辆sv与即将插入位置的前、后车距离需达到换道触发条件c，来触发换道。换道触发条件c具体表达如下：

23、

24、其中，pfv，psv，prv分别代表换道触发时刻插入位置前车、换道车辆、插入位置后车的位置，vsv，vsv，vrv代表换道触发时刻插入位置前车、换道车辆、插入位置后车的速度，td为平均换道时间估计值，λ为协同换道安全系数。

25、步骤33，换道期间通信拓扑结构

26、在换道车辆未发出换道请求前，原队列和目标队列采用plf来保持队列稳定性；在换道车辆发出换道请求后到换道完成前即协同换道期间，原队列离开位置前方车辆及插入位置前方车辆仍保持plf的通信拓扑结构，由于换道期间，原队列离开位置及目标队列插入位置间距大小的不确定，原队列离开位置后方车辆及插入位置后方车辆转变为pf的通信拓扑结构；在换道完成后，原队列和目标队列继续采用plf的通信拓扑结构。

27、步骤4，基于步骤2的选择位置决策以及步骤3的触发换道条件、机制，实施换道车辆的轨迹规划以及mpc换道跟踪控制实施方案。步骤4的具体实现步骤如下：

28、步骤41，合理规划换道车辆的横纵向轨迹

29、首先，利用基于正弦曲线的动态换道轨迹规划方法，将纵向协同控制算法与正弦函数曲线相结合，通过纵向协同控制模型来更新换道过程中换道车辆sv的纵向加速度：

30、asv(t)＝usv(t-t)

31、其中，asv(t)是换道车辆sv在t时刻的加速度，usv(t-t)是t-t时刻的纵向控制量，下标sv表示换道车辆。

32、其次，构建基于正弦曲线的换道轨迹规划函数。通过纵向控制算法实时更新换道车辆的速度，然后用动态更新的速度实时生成换道纵向位移，再转移到正弦函数中，实现动态的轨迹规划。由于纵向上需要根据队列控制目标来调整，所以换道轨迹规划函数可表示为纵向位移的函数：

33、

34、

35、其中，yr(xr(t))为参考换道轨迹函数，r下标代表参考轨迹相关量，t0，tend分别为换道初始时刻和结束时刻，是换道车辆在换道初始时刻的纵向位移，为换道车辆在换道初始时刻的横向位移，δxsv(t)是t时刻规划的纵向换道位移，w为车道宽度，ay是换道车辆的横向加速度。

36、步骤42，基于mpc进行换道轨迹跟踪控制

37、首先，计算得到轨迹曲线的曲率，并由此计算出换道车辆的期望航向角和期望前轮转角δfr；

38、定义和分别为误差状态向量和误差控制向量，x(k)和xr(k)分别为状态向量和期望状态向量，u(k)和ur(k)分别为控制向量和期望控制向量，得到状态空间方程如下：

39、

40、

41、

42、其次，在轨迹跟踪过程中，为了保证实际轨迹与期望轨迹的误差和控制量误差尽可能地小，设计目标函数以确保车辆能够快速平稳地跟踪参考轨迹。mpc的最小成本函数和约束条件如下：

43、

44、约束条件如下：

45、

46、

47、其中，q和r是权重矩阵：

48、

49、

50、其中，nu和nx分别为控制量和状态量的个数，i为单位矩阵。u(k+i|k)是预测时间步长k+i时的控制变量，np和nc分别是预测时域和控制时域，δu(k)是控制时域nc中控制步长k的系统控制变量增量，umin和umax是控制变量u的最小值和最大值，δumin和δumax为控制变量增量δu的最小值和最大值。因此，在mpc框架下，将该问题转化为一个标准的二次型规划(quadratic programming，qp)问题。

51、步骤5，完成换道后，队列纵向需要协同控制来达到稳定，即在无外界干扰情况下，基于换道车辆sv和头车的信息，进行速度和加速度协同，在达到渐进稳定的过程中，动态计算横向跟踪误差，直至横向跟踪误差为0，达到队列稳定，换道结束。