一种混合智能网联环境下匝道汇入区车辆协同控制的方法与流程

本发明属于智能交通，涉及一种混合智能网联环境下匝道汇入区车辆协同控制的方法。

背景技术：

1、匝道汇入区是高速公路、城市快速路以及具有主辅路合流结构道路的重要交通节点。在匝道车辆进入主线的过程中，匝道车辆需要在有限的汇入距离内识别主线车辆之间的可用间隙，并根据主线车辆的速度、位置和运行趋势调整自身速度和汇入轨迹。

2、由于主线车辆与匝道车辆之间通常存在速度差、位置差以及驾驶意图差异，匝道汇入区容易发生急加速、急减速、强制换道、抢行和追尾冲突。当匝道交通密度较高或匝道车辆排队长度较大时，匝道车辆难以及时获取满足安全距离要求的汇入间隙，容易引起匝道排队和主线交通波动。

3、在传统人工驾驶交通环境下，匝道车辆是否能够汇入主线主要由驾驶员依据视觉感知结果进行判断。驾驶员的反应时间、驾驶风格、注意力状态和风险偏好存在差异，导致汇入决策具有较强的不确定性。同时，车辆之间存在信息不对称，驾驶员难以准确获得相邻车辆的加速度和未来运行状态，进一步增加了匝道汇入过程中的安全风险。

4、随着车路协同和自动驾驶技术的发展，网联自动驾驶车辆具有车辆状态可获取、行驶轨迹可预测以及运动状态可调节的特点。通过对部分网联自动驾驶车辆实施速度控制，能够改变主线车辆之间的纵向间距，为匝道车辆创造汇入条件。

5、但是，网联自动驾驶车辆的渗透率在较长时期内难以达到百分之百，实际道路交通环境将长期处于人工驾驶车辆、普通网联车辆和网联自动驾驶车辆混合行驶的状态。现有针对全自动驾驶车辆交通流设计的协同控制方法难以直接应用于混合交通环境。

6、部分现有匝道汇入控制方法在主线最外侧车道不存在可控车辆时，直接控制其他车道内的网联自动驾驶车辆变道至主线最外侧车道，再通过减速形成汇入间隙。但是，车辆横向变道过程不仅与主线车流的平均速度相关，还与候选车辆自身的纵向速度、纵向加速度、剩余换道距离以及当前车道和目标车道前后车辆的位置、速度、加速度和车辆间距相关。仅采用主线车流平均速度计算单辆车辆的换道可用时间，不能准确反映候选车辆的实际换道条件。

7、部分横向变道优化模型直接将横向加速度平方项与换道时间倒数项进行加权求和，但横向加速度平方项和换道时间倒数项具有不同的物理量纲，直接相加所得目标函数缺少明确的物理意义。

8、此外，混合交通环境中的人工驾驶车辆和其他未被直接控制的车辆不受协同控制系统直接控制。若控制方法默认被控车辆后方的未受控车辆必然减速或必然保持在被控车辆之后，则难以保证被控车辆减速过程中后方车辆的跟驰安全。

9、在部分交通状态下，匝道车辆在协同控制开始前或者协同控制执行期间已经能够利用主线车流中的自然间隙完成汇入。若系统未先判断匝道车辆是否具备自然汇入条件而直接控制主线车辆变道或减速，将增加不必要的横向操作和纵向交通扰动。

10、因此，需要一种适用于混合智能网联交通环境的匝道汇入区车辆协同控制方法，在实施主线车辆横向变道和纵向减速控制之前，先判断匝道车辆是否已经具备自然汇入条件；在确需实施协同控制时，优先选取已经位于主线最外侧车道的网联自动驾驶车辆；在需要其他车道车辆变道时，综合考虑候选车辆及当前车道、目标车道前后车辆的运动状态，完成可复现的横向变道安全判定；在纵向控制期间，将后方未受控车辆作为实时安全约束对象，并根据交通状态变化滚动更新或者退出控制。

技术实现思路

1、有鉴于此，本发明的目的在于提供一种混合智能网联环境下匝道汇入区车辆协同控制的方法，用于解决现有技术中存在的以下问题：

2、现有控制方法未在实施协同控制前判断匝道车辆是否已经具备自然汇入条件，容易使主线车辆执行不必要的变道或减速操作；

3、现有横向变道安全判定未充分考虑候选车辆、当前车道前后车辆和目标车道前后车辆的实际运动状态；

4、现有横向变道目标函数中的不同目标项存在物理量纲不一致的问题；

5、现有纵向间隙形成过程依赖后方人工驾驶车辆或其他未受控车辆配合，难以保证控制过程的跟驰安全；

6、交通状态发生变化后，现有方法缺少重新判断自然汇入条件以及中止不必要控制的滚动退出机制。

7、为达到上述目的，本发明提供如下技术方案：

8、一种混合智能网联环境下匝道汇入区车辆协同控制的方法，包括以下步骤：

9、s1：获取交通状态数据

10、通过设置于匝道汇入区的路侧感知设备、路侧单元和车辆通信设备，获取主线车辆和匝道车辆的交通状态数据。

11、所述交通状态数据包括主线车辆的位置、纵向速度、纵向加速度、所在车道和车辆长度，匝道车辆的位置、纵向速度、纵向加速度和车辆长度，以及主线车流平均纵向速度、匝道车流平均纵向速度、匝道交通密度、匝道实时排队长度和网联自动驾驶车辆渗透率。

12、所述网联自动驾驶车辆为网联自动驾驶车辆（connected and autonomousvehicle，cav）。

13、建立道路纵向坐标轴，将主线车辆行驶方向确定为纵向正方向，将车辆

j在时刻

t的纵向位置、纵向速度和观测纵向加速度分别记为：、、；

14、其中，下标

z表示道路纵向方向；下标

j表示对应车辆；观测纵向加速度为具有正负方向的车辆纵向加速度，车辆沿行驶方向加速时取正值，车辆沿行驶方向减速时取负值。

15、当车辆通信数据中包含纵向加速度时，直接读取车辆通信数据中的纵向加速度。当车辆通信数据中不包含纵向加速度时，根据连续采集的纵向速度计算观测纵向加速度：

16、

17、其中，为交通状态数据采样周期。

18、将主线道路沿车辆行驶方向依次划分为车辆准备区（vehicle preparationarea，vpa）、车辆控制区（vehicle control area，vca）和车辆汇入区（vehicle mergingarea，vma）。

19、车辆准备区vpa位于车辆控制区vca的上游，用于进行候选被控cav选取和横向变道安全判定。

20、车辆控制区vca位于车辆准备区vpa和车辆汇入区vma之间，用于控制被控cav完成纵向速度优化。

21、车辆汇入区vma位于匝道与主线的实际合流区域，用于使匝道车辆和主线车辆依次通过。

22、s2：判断匝道车辆是否具备自然汇入条件

23、当匝道车辆检测器检测到待汇入匝道车辆通过时，将检测时刻记为，首先预测待汇入匝道车辆到达汇入点时，主线最外侧车道内是否存在满足自然汇入条件的自然汇入间隙。

24、所述主线最外侧车道为与匝道相邻且用于接收匝道车辆汇入的主线车道。

25、将待汇入匝道车辆记为匝道车辆，获取匝道车辆在检测时刻至汇入点的纵向距离、纵向速度和观测纵向加速度。

26、当且时，根据下式计算匝道车辆到达汇入点的预测行驶时间：

27、当且时，确定当前不能形成有效的自然汇入预测结果，并按照预设控制周期继续获取匝道车辆的更新状态；其中，为预设最小有效匝道车辆速度；为预设加速度判定阈值，用于判断车辆纵向加速度是否接近零。

28、当满足：时，根据下式求解匝道车辆到达汇入点的预测行驶时间：

29、

30、所述预测行驶时间的候选解为：

31、

32、将满足纵向运动方程的最小正根确定为预测行驶时间。

33、当根号内数值小于零、纵向运动方程不存在正根或者根据预测运动状态确定匝道车辆在停止前不能到达汇入点时，确定当前不能形成有效的自然汇入预测结果，在当前控制周期内不进入被控车辆选取步骤，并按照预设控制周期继续获取匝道车辆的位置、纵向速度和观测纵向加速度；当根据更新后的车辆状态能够求得匝道车辆到达汇入点的正预测行驶时间时，重新预测自然汇入间隙；仅当能够形成有效的自然汇入预测结果且不存在满足自然汇入条件的自然汇入间隙时，进入被控车辆选取步骤。

34、将匝道车辆的预测汇入时刻记为：：

35、采用恒加速度短时预测模型计算车辆

j在预测汇入时刻的预测纵向位置和预测纵向速度：

36、

37、

38、其中，车辆

j为匝道车辆或主线最外侧车道内的车辆；上标表示预测值。

39、根据各车辆在预测汇入时刻的预测纵向位置，确定在主线最外侧车道内位于匝道车辆前方的前方相邻车辆

f和位于匝道车辆后方的后方相邻车辆

b。

40、根据下式计算匝道车辆与前方相邻车辆

f之间的预测纵向净间距：

41、

42、根据下式计算后方相邻车辆

b与匝道车辆之间的预测纵向净间距：

43、

44、其中，、和分别为前方相邻车辆

f、匝道车辆和后方相邻车辆

b的车辆长度。

45、对于由跟驰车辆和位于所述跟驰车辆前方的车辆组成的车辆对，根据下式计算跟驰车辆相对于前方车辆的安全纵向间距：

46、

47、其中，为预设最小静止间距；为预设安全车头时距；为预设安全制动减速度幅值。

48、当同时满足以下条件时，确定前方相邻车辆

f与后方相邻车辆

b之间存在满足自然汇入条件的自然汇入间隙：

49、

50、

51、

52、

53、其中，为预设最大允许汇入速度差。

54、第一项条件为匝道车辆相对于前方相邻车辆的前向安全距离约束；第二项条件为后方相邻车辆相对于匝道车辆的后向安全距离约束；第三项条件和第四项条件共同构成汇入速度差约束。

55、当存在满足所述自然汇入条件的自然汇入间隙时，不选取被控cav，不对主线车辆实施横向变道控制和纵向减速控制，匝道车辆利用所述自然汇入间隙汇入主线最外侧车道。

56、当不存在满足所述自然汇入条件的自然汇入间隙时，进入被控车辆选取步骤。

57、s3：确定被控车辆数量和被控车辆选取范围

58、获取匝道实时排队长度、主线上单个被控cav能够提供的最大汇入空隙以及被控车辆最大允许数量，所述最大汇入空隙为在车辆控制区vca的纵向长度、被控cav最大允许纵向减速度、被控cav最小允许纵向速度、被控cav与前方车辆的纵向安全距离约束以及被控cav后方未受控车辆跟驰安全约束下，单个被控cav能够形成的最大动态汇入间隙；

59、主线上单个被控cav能够提供的最大汇入空隙通过离线标定或在线优化确定。在进行在线优化时，根据下式确定：

60、

61、其中，纵向减速度幅值和纵向优化结束时刻应同时满足被控cav加速度约束、被控cav速度约束、加速度变化率约束、被控cav与前方车辆的纵向安全距离约束、被控cav后方未受控车辆跟驰安全约束以及动态汇入间隙约束。

62、当采用离线标定方式时，根据车辆控制区vca的纵向长度、道路设计速度、允许纵向减速度范围和预设安全距离参数预先计算并存储所述最大汇入空隙。

63、当满足：时，确定被控车辆数量：

64、当满足：时，根据下式确定被控车辆数量：；

65、其中，为被控车辆数量；表示向上取整运算。

66、获取主线车流平均纵向速度、匝道车流平均纵向速度以及匝道车辆检测器至汇入点的纵向距离，根据下式计算被控车辆选取范围：

67、

68、其中，为预设最小有效匝道车流平均纵向速度，匝道车辆按照匝道车流平均纵向速度行驶至汇入点所需的时间为：

69、主线车辆在所述时间内按照主线车流平均纵向速度行驶的距离为被控车辆选取范围。

70、在被控车辆选取范围内，按照距离汇入点由近至远的顺序选取cav。

71、优先从已经位于主线最外侧车道并满足纵向控制可行性约束的cav中选取被控cav。

72、所述纵向控制可行性约束包括被控cav速度约束、被控cav加速度约束、被控cav与前方车辆的碰撞风险约束以及被控cav后方未受控车辆跟驰安全约束。

73、当主线最外侧车道内满足纵向控制可行性约束的cav数量达到被控车辆数量时，不再从其他车道选取车辆。

74、当主线最外侧车道内满足纵向控制可行性约束的cav数量少于被控车辆数量时，从与主线最外侧车道相邻的车道内选取候选被控cav，并对候选被控cav进行横向变道安全约束判定。

75、由此，仅在不存在自然汇入间隙且主线最外侧车道内满足纵向控制条件的cav数量不足时，才允许其他车道内的候选被控cav执行横向变道。

76、当被控车辆数量大于1时，按照距离汇入点由近至远的顺序对各被控cav进行编号，并按照所述编号顺序依次进行纵向速度优化。对于位于另一被控cav后方的被控cav，将其前方最近车辆作为前方相邻车辆；当所述前方相邻车辆为已经完成纵向速度优化求解的被控cav时，采用该前方相邻车辆的预测纵向位置、预测纵向速度和实际纵向加速度控制指令建立纵向安全距离约束。各被控cav分别形成动态汇入间隙，各动态汇入间隙按照距离汇入点由近至远的顺序，与待汇入匝道车辆按照预测到达汇入点的先后顺序进行对应。

77、s4：计算候选被控cav的换道可用时间

78、对于未位于主线最外侧车道的候选被控cav，将候选被控cav记为车辆

c。

79、获取车辆

c在候选车辆选取时刻至车辆准备区vpa下游边界的剩余纵向距离、纵向速度和观测纵向加速度。

80、所述剩余纵向距离表示车辆

c当前所在位置至车辆准备区vpa下游边界之间的实际纵向距离，不等同于车辆准备区vpa的总长度。仅在车辆

c位于车辆准备区vpa上游边界时，所述剩余纵向距离等于车辆准备区vpa的总长度。

81、当满足：且时，根据下式计算换道可用时间：

82、

83、当时，确定所述候选被控cav不满足换道可用时间约束；

84、当满足：时，根据下式计算换道可用时间：

85、

86、在根号内数值不小于零时，将满足车辆

c纵向运动方程的最小正值确定为换道可用时间。

87、当根号内数值小于零、所得结果不存在正值或者根据预测结果确定车辆

c在到达车辆准备区vpa下游边界前已经停止时，确定车辆

c不满足换道可用时间约束，不将车辆

c确定为被控cav。

88、s5：构建横向变道目标函数

89、将横向变道过程划分为横向加速阶段和横向减速阶段。

90、横向加速阶段和横向减速阶段的持续时间相等，横向加速度方向相反，使车辆

c在横向变道结束时的横向速度恢复为零。

91、以横向加速度幅值为决策变量，将车辆

c从当前车道中心移动至主线最外侧车道中心所需的横向移动距离记为。

92、设横向加速阶段的持续时间为。横向加速阶段的横向位移为：；

93、横向减速阶段的横向位移为：；

94、两个阶段的横向位移之和等于，由此得到横向变道执行时间：；

95、构建横向变道目标函数：

96、

97、其中，为横向变道目标函数；和为横向变道目标函数的权重系数，且满足：，

g为重力加速度。

98、目标函数中的第一项目标为横向加速度幅值与最大允许横向加速度之比的平方，属于无量纲量。目标函数中的第二项目标为横向变道执行时间与换道可用时间之比，也属于无量纲量。因此，两个目标项具有相同的物理量纲，能够进行加权求和。

99、横向变道执行时间应满足：；

100、将横向变道执行时间公式代入，得到横向加速度幅值下限：；

101、横向加速度幅值上限为：；因此，横向加速度约束为：；

102、s6：计算横向变道潜在碰撞概率

103、获取车辆

c所在当前车道的前方车辆和后方车辆，以及主线最外侧车道内与车辆

c对应位置的前方车辆和后方车辆。

104、在预设有效感知范围内存在候选被控cav所在当前车道的前方车辆时，将候选被控cav与所述当前车道的前方车辆组成第一相邻车辆对；

105、在所述预设有效感知范围内存在所述当前车道的后方车辆时，将所述当前车道的后方车辆与候选被控cav组成第二相邻车辆对；

106、在所述预设有效感知范围内存在所述主线最外侧车道的前方车辆时，将候选被控cav与所述主线最外侧车道的前方车辆组成第三相邻车辆对；

107、在所述预设有效感知范围内存在所述主线最外侧车道的后方车辆时，将所述主线最外侧车道的后方车辆与候选被控cav组成第四相邻车辆对；

108、将实际形成的各相邻车辆对组成相邻车辆对集合；当任一方向不存在对应车辆或者对应车辆位于所述预设有效感知范围之外时，不建立该方向对应的相邻车辆对；当所述相邻车辆对集合为空集时，将所述潜在碰撞概率确定为0；

109、对于相邻车辆对集合中的任一相邻车辆对，将车辆确定为跟驰车辆，将车辆确定为前方车辆。

110、按照预设预测时间步长，在横向变道执行期间设置预测时刻，并将横向变道结束时刻设置为最后一个预测时刻：其中：；

111、对于相邻车辆对中的车辆

j，根据下式计算车辆

j在预测时刻的预测纵向速度：

112、

113、根据下式计算相邻车辆对在预测时刻的预测纵向净间距均值：

114、

115、其中，为跟驰车辆与前方车辆在候选车辆选取时刻的初始纵向净间距。

116、根据下式计算跟驰车辆相对于前方车辆在预测时刻的安全纵向间距：

117、

118、考虑车辆状态测量误差和预测模型误差，根据下式计算相邻车辆对的纵向净间距预测标准差：

119、

120、其中，为相邻车辆对的相对间距预测误差标准差；为相对速度预测误差标准差；为相对加速度预测误差标准差。

121、相对间距预测误差标准差根据下式确定：

122、

123、相对速度预测误差标准差根据下式确定：

124、

125、相对加速度预测误差标准差根据下式确定：

126、

127、其中，、和分别为车辆的位置、速度和加速度测量误差标准差；、和分别为车辆的位置、速度和加速度测量误差标准差。

128、各测量误差标准差通过路侧感知设备和车载感知设备的标定结果确定，或者根据预设滑动时间窗口内的预测值与实际测量值之间的均方根误差确定。

129、当计算得到的纵向净间距预测标准差小于预设最小标准差时，将其设置为：，其中，，用于避免碰撞风险概率计算过程中出现分母为零。

130、在预测纵向净间距服从均值为、标准差为的正态分布条件下，根据下式计算相邻车辆对在预测时刻的碰撞风险概率：

131、

132、其中，为标准正态分布累积分布函数。

133、将全部相邻车辆对和全部预测时刻对应的碰撞风险概率中的最大值确定为潜在碰撞概率：

134、

135、由于横向变道执行时间随横向加速度幅值变化，预测时间范围和预测步数随横向加速度幅值变化，因此潜在碰撞概率为横向加速度幅值的函数。

136、潜在碰撞概率约束为：其中，为预设安全阈值。

137、在以下约束条件下求解横向变道目标函数：

138、

139、

140、得到最优横向加速度幅值。

141、采用序列二次规划算法、内点法或者其他满足约束优化要求的数值优化算法求解所述横向变道目标函数。

142、当存在满足全部约束条件的最优横向加速度幅值时，将车辆

c确定为被控cav，并将最优横向加速度幅值、横向加速阶段持续时间、横向减速阶段持续时间以及对应的横向加速度方向发送至被控cav的底盘控制系统。

143、横向加速阶段持续时间和横向减速阶段持续时间均为：；

144、当不存在满足全部约束条件的横向加速度幅值时，不将车辆

c确定为被控cav，并按照距离汇入点由近至远的顺序对下一候选被控cav进行判定。

145、当全部候选被控cav均不满足横向变道安全约束时，系统不强制控制候选被控cav执行横向变道，继续监测主线和匝道交通状态，直至出现满足自然汇入条件的自然汇入间隙或者出现新的满足控制条件的候选被控cav。

146、对于已经位于主线最外侧车道的被控cav，不执行横向变道控制，控制所述被控cav保持当前车道并进入后续纵向速度优化阶段。

147、s7：滚动判断自然汇入条件

148、系统在被控cav开始横向变道之前、横向变道期间以及被控cav进入车辆控制区vca后，按照预设控制周期更新主线车辆和匝道车辆的交通状态，并重新预测待汇入匝道车辆是否具备自然汇入条件。

149、在被控cav开始横向变道之前，若检测到满足自然汇入条件的自然汇入间隙，则取消被控cav的横向变道控制和纵向减速控制。

150、在被控cav已经开始横向变道之后，若检测到满足自然汇入条件的自然汇入间隙，则控制被控cav按照已经确定的安全横向变道轨迹完成当前横向变道，不控制被控cav在横向变道中途反向变道，并取消被控cav完成横向变道后用于扩大汇入间隙的纵向减速控制。

151、在被控cav执行纵向减速控制期间，若检测到满足自然汇入条件的自然汇入间隙，则根据下式限定被控cav纵向加速度控制指令的变化率：

152、

153、其中，为被控cav在时刻

t的纵向加速度控制指令；为预设最大允许加速度变化率。

154、在满足纵向加速度控制指令变化率约束的条件下，逐步减小被控cav的纵向减速度幅值，使被控cav平稳恢复正常行驶状态。

155、s8：构建纵向速度优化模型

156、当持续不存在满足自然汇入条件的自然汇入间隙时，控制被控cav在车辆控制区vca内完成纵向速度优化。

157、将被控cav进入车辆控制区vca并开始纵向速度优化的时刻记为，将被控cav离开车辆控制区vca并进入车辆汇入区vma的时刻记为。

158、获取时刻对应的匝道交通密度和匝道临界密度。

159、匝道临界密度根据匝道历史交通状态数据预先标定。具体地，对匝道历史交通密度和匝道历史排队长度进行时间同步，确定匝道排队长度由稳定状态转变为连续增长状态时对应的匝道交通密度，并将所述匝道交通密度确定为匝道临界密度。所述匝道临界密度按照预设更新周期根据新增交通状态数据重新标定。

160、当满足：时，采用工况一，控制被控cav在车辆控制区vca内全程持续减速。

161、当满足：时，采用工况二，控制被控cav先减速后匀速行驶。

162、构建自适应多目标优化模型：

163、其中，为纵向速度优化目标函数；为匝道汇入区统计范围内cav数量占车辆总数量的比例；为归一化潜在冲突指标；为纵向优化基准时间；和为自适应权重系数。

164、纵向优化基准时间根据下式计算：

165、

166、其中，为车辆控制区vca的纵向长度，为被控cav在时刻的纵向速度，为预设最小有效被控cav纵向速度。

167、将纵向优化预测期间内需要进行安全判断的车辆对组成纵向车辆对集合：；

168、所述纵向车辆对集合包括被控cav与其前方车辆组成的车辆对、被控cav后方未受控车辆与被控cav组成的车辆对，以及多个被控cav之间构成的相邻车辆对。

169、对于纵向车辆对集合中的车辆对(

p,

l)，根据各车辆预测纵向位置计算预测纵向净间距：

170、

171、其中，和分别为跟驰车辆

p和前方车辆

l的车辆长度。

172、对于未被直接控制的车辆

j，按照下式预测纵向位置和纵向速度：

173、

174、

175、以非负的纵向减速度幅值为被控cav的纵向速度优化变量，被控cav对应的实际纵向加速度为：；

176、在工况一下，被控cav在纵向优化期间的预测纵向位置和预测纵向速度为：

177、

178、

179、在工况二下，将被控cav减速完成的时刻记为，将减速持续时间记为：

180、

181、当：时，被控cav的预测纵向位置和预测纵向速度为：

182、

183、

184、当：时，被控cav的预测纵向位置为：

185、

186、被控cav的预测纵向速度为：

187、

188、其中：

189、

190、对于纵向车辆对集合中的车辆对，根据下式计算原始潜在冲突车辆对数量：

191、

192、其中，为示性函数，括号内条件成立时取值为1，括号内条件不成立时取值为0。

193、根据下式计算归一化潜在冲突指标：

194、

195、其中，为纵向车辆对集合中的车辆对数量。

196、由此得到：

197、纵向优化时长通过纵向优化基准时间进行归一化：

198、因此，纵向速度优化目标函数中的归一化潜在冲突指标和归一化纵向优化时长均为无量纲量。

199、自适应权重系数满足：

200、

201、按照以下顺序确定自适应权重系数。

202、当满足：或者时，确定：；此时优先降低潜在交通冲突。

203、当不满足前述条件，并且满足：或者中的至少一个条件时，确定：

204、；此时优先缩短纵向优化时长。

205、当既不满足第一组条件，也不满足第二组条件时，确定：。

206、在被控cav加速度约束、被控cav速度约束、加速度变化率约束、被控cav与其前方相邻车辆的纵向安全距离约束、被控cav后方未受控车辆跟驰安全约束以及动态汇入间隙不小于预设最小安全汇入间隙的约束下，求解纵向速度优化目标函数，得到最优纵向减速度幅值：其中，最优纵向减速度幅值为非负值，对应的实际纵向加速度控制指令为：；

207、被控cav加速度约束为：；其中，为预设最大允许纵向减速度幅值。

208、被控cav速度约束为：；其中，和分别为预设最小纵向速度和预设最大纵向速度。

209、加速度变化率约束为：；

210、将被控cav记为车辆

c，将位于被控cav前方且与被控cav处于主线最外侧车道的最近车辆记为前方相邻车辆。

211、在纵向优化期间，根据下式计算被控cav的预测纵向速度：

212、

213、根据下式计算前方相邻车辆的预测纵向速度：

214、

215、根据下式计算被控cav与前方相邻车辆之间的预测纵向净间距：

216、

217、根据下式计算被控cav相对于前方相邻车辆的安全纵向间距：

218、；

219、被控cav与前方相邻车辆的纵向安全距离约束为：

220、。

221、当被控cav前方在预设有效感知范围内不存在车辆时，不设置所述纵向安全距离约束。

222、动态汇入间隙约束用于保证被控cav在时刻仍位于汇入点上游，并使被控cav与汇入点之间保留动态安全距离。所述动态汇入间隙约束为：

223、

224、其中，为根据被控cav在时刻至时刻的纵向行驶距离、被控cav在时刻至汇入点的纵向距离以及时刻对应的动态安全距离计算得到的动态汇入间隙，为预设最小安全汇入间隙。

225、当满足所述动态汇入间隙约束时，被控cav在时刻至汇入点的剩余纵向距离不小于所述动态安全距离与所述预设最小安全汇入间隙之和。

226、s9：建立后方未受控车辆跟驰安全约束

227、将位于被控cav后方且与被控cav处于同一主线最外侧车道的人工驾驶车辆或其他未被直接控制的车辆记为后方未受控车辆

u。

228、系统不向后方未受控车辆

u发送控制指令，也不将后方未受控车辆

u必然减速作为被控cav纵向减速控制的前提。

229、获取后方未受控车辆

u在时刻的纵向位置、纵向速度和观测纵向加速度。

230、根据下式计算后方未受控车辆

u与被控cav之间在纵向优化期间的预测纵向净间距：

231、

232、其中，为后方未受控车辆

u与被控cav在时刻的初始纵向净间距；为后方未受控车辆

u在时刻的纵向速度；为后方未受控车辆

u在时刻的观测纵向加速度。

233、根据下式计算后方未受控车辆

u相对于被控cav的安全纵向间距：

234、

235、后方未受控车辆跟驰安全约束为：；其中：；

236、系统按照预设控制周期更新后方未受控车辆的位置、纵向速度和纵向加速度，并重新求解被控cav的纵向减速度幅值。

237、当后方未受控车辆加速、未及时减速或者预测纵向净间距趋于小于安全纵向间距时，系统降低被控cav的纵向减速度幅值。

238、当不存在满足后方未受控车辆跟驰安全约束的纵向减速度幅值时，取消利用当前被控cav形成动态汇入间隙，并重新选取其他被控cav或者继续监测自然汇入间隙。

239、因此，后方未受控车辆仅作为交通状态感知对象和安全约束对象，系统不以其服从控制指令或者必然保持在被控cav之后为前提。

240、s10：计算动态汇入间隙

241、将被控cav在时刻至汇入点的纵向距离记为：；

242、所述纵向距离表示被控cav当前位置至汇入点之间沿道路纵向方向的剩余距离，不表示被控cav的纵向位置坐标。

243、在工况一下，根据下式计算被控cav从时刻至时刻的纵向行驶距离：

244、

245、根据下式计算工况一下的动态汇入间隙：

246、

247、其中，仅表示工况一下被控cav从时刻至时刻的纵向行驶距离。

248、在工况二下，将被控cav减速完成的时刻记为。

249、根据下式计算被控cav从时刻至时刻的减速行驶距离：

250、

251、根据下式计算被控cav在时刻的纵向速度：

252、

253、根据下式计算被控cav从时刻至时刻的匀速行驶距离：

254、

255、根据下式计算工况二下的动态汇入间隙：

256、

257、其中，仅表示工况二下被控cav的减速行驶距离；仅表示工况二下被控cav的匀速行驶距离。

258、时刻对应的动态安全距离为：

259、

260、前述安全纵向间距用于判断跟驰车辆与其前方车辆之间是否存在纵向碰撞风险，其计算同时考虑预设最小静止间距、安全车头时距和前后车辆之间的相对制动距离；本步骤中的动态安全距离用于限定被控cav在时刻与汇入点之间应保留的最小位置裕度。

261、所述动态安全距离不替代车辆对之间的安全纵向间距。在纵向速度优化过程中，被控cav应同时满足与其前方车辆之间的纵向安全距离约束、后方未受控车辆跟驰安全约束以及由所述动态安全距离确定的动态汇入间隙约束。

262、所得动态汇入间隙满足：

263、

264、其中，为预设最小安全汇入间隙。

265、被控cav构成动态汇入间隙的后边界。位于被控cav前方的主线车辆按照其原有行驶状态继续向下游行驶，匝道车辆在被控cav前方进入主线最外侧车道。

266、当匝道实时排队长度大于单个被控cav能够提供的最大汇入空隙时，多个被控cav按照距离汇入点由近至远的顺序分别形成多个动态汇入间隙，使匝道车辆分组汇入。

267、s11：车辆汇入和控制恢复

268、控制被控cav和待汇入匝道车辆在车辆汇入区vma内依次通过，使匝道车辆在被控cav前方汇入主线最外侧车道。

269、当匝道车辆完成汇入并驶离车辆汇入区vma后，终止对被控cav的纵向减速控制。

270、被控cav按照加速度变化率约束逐步恢复至主线期望行驶速度。

271、当匝道车辆尚未完成汇入或者主线交通状态发生变化时，继续执行自然汇入条件判断、后方未受控车辆跟驰安全约束更新和纵向速度优化滚动求解。

272、本发明的有益效果在于：

273、（1）在选取被控cav并实施横向变道或纵向减速之前，先预测匝道车辆到达汇入点时的自然汇入间隙。当匝道车辆已经具备自然汇入条件时，不对主线车辆实施额外控制，降低了不必要的横向变道和纵向减速操作。优先从已经位于主线最外侧车道的cav中选取被控车辆。仅在主线最外侧车道内满足纵向控制条件的cav数量不足时，才从相邻车道选取候选被控cav并进行横向变道安全判定，降低了额外横向操作产生的风险。

274、（2）根据候选被控cav自身的剩余换道距离、纵向速度和观测纵向加速度计算换道可用时间，不再采用主线车流平均速度代替单辆车辆的实际运动状态。在横向变道安全判定中，同时考虑候选被控cav、当前车道前后车辆和目标车道前后车辆的位置、纵向速度、纵向加速度、纵向间距以及车辆状态预测误差，能够按照明确的公式计算潜在碰撞概率。本发明的横向变道目标函数采用归一化横向加速度和归一化换道执行时间作为目标项，两个目标项均为无量纲量，消除了不同物理量纲直接相加的问题。本发明的纵向速度优化目标函数采用归一化潜在冲突指标和归一化纵向优化时长作为目标项，使纵向优化目标函数中的两个目标项均为无量纲量。

275、（3）本发明不假定后方人工驾驶车辆或其他未受控车辆服从系统控制，而是将后方未受控车辆作为实时安全约束对象，根据后方车辆的实际加速、减速状态滚动更新被控cav的纵向控制量。本发明在控制过程中持续重新判断自然汇入条件。当自然汇入条件形成后，根据被控cav所处控制阶段取消尚未开始的控制、完成已经开始的安全换道或者平稳退出纵向减速控制，避免继续执行已经失去必要性的控制操作。能够适应不同cav渗透率、不同匝道交通密度和不同主线交通状态，在保证安全性的同时降低对混合交通流的干扰。

276、本发明的其他优点、目标和特征在某种程度上将在随后的说明书中进行阐述，并且在某种程度上，基于对下文的考察研究对本领域技术人员而言将是显而易见的，或者可以从本发明的实践中得到教导。本发明的目标和其他优点可以通过下面的说明书来实现和获得。