CN114815852B - 一种基于空间离散化的cacc车队轨迹规划方法 - Google Patents

Abstract

本发明公开了一种基于空间离散化的CACC车队轨迹规划方法，将CACC车队的轨迹规划问题简化为一个经典轨迹规划问题，即给定CACC车辆的初始状态(包括所有车队进入控制区域的时刻、位置、速度和加速度)，以及CACC车队前方距离最近的人工驾驶车辆的预测轨迹，为CACC车辆整体规划兼顾瓶颈效率和轨迹平滑性的轨迹。本发明在基于前车预测轨迹的情况下，以空间离散化的思路建模，在若干约束条件和目标函数下，通过F‑W算法得到了CACC车队的规划轨迹。本发明将CACC车队作为控制手段引入到交通流的管控中来，能够减小与前方人工驾驶车辆的空白间隙，同时平滑上游车辆的轨迹，以提升瓶颈通行效率。

Description

一种基于空间离散化的CACC车队轨迹规划方法

技术领域

本发明属于车辆轨迹规划技术领域，具体涉及一种基于空间离散化的CACC车队轨迹规划方法的设计。

背景技术

高速公路以其快速、安全、舒适等特点和优势，在现代综合交通运输体系中发挥着重要的作用，然而高速公路交通拥堵现象时有发生，特别是在节假日交通需求高峰时期，高速公路交通拥堵导致交通系统通行效率降低、出行时间增加、安全水平下降、环境污染加重等问题。当交通需求较高时，高速公路拥堵一般是从匝道口、隧道、车道数突变等瓶颈路段开始蔓延，车辆从瓶颈处开始累积排队，不断向上游蔓延，甚至导致整条高速公路及周边路网陷入瘫痪。高速公路瓶颈一般可分为移动瓶颈(如刚驶入密度较高的交通流中的低速行驶车辆)和固定瓶颈(出入口匝道、道路施工路段、事件发生路段、车道变窄路段、隧道口等)，其中固定瓶颈导致的高速公路交通拥堵将严重降低高速公路运行效率，提高瓶颈通行效率一直是高速公路管控的重点。然而应对瓶颈问题，至今仍无高效且可行的控制手段，而智能交通系统(Intelligent Transportation System，ITS)特别是车路协同技术的发展为解决此问题提供了新的思路。

智能网联车辆技术在减少交通事故、缓解交通拥堵、提高道路及车辆利用率方面具有巨大潜能，已成为目前智能交通领域的研究热点，也是目前公认的交通科技前沿发展方向。有研究机构预测，到2030年，较高等级的智能网联车辆将进入市场，到2045年，所有的车辆将拥有智能网联功能，而智慧高速是智能网联和车路协同技术的重点建设领域，对提高高速路行车安全和运输效率具有重要意义。其中，协同自适应巡航控制(CooperativeAdaptive Cruise Control，CACC)技术是目前自动驾驶研发工作所达到的最高层次，基于CACC的自动车编队运行(简称CACC车队)将是公路自动驾驶的主要模式之一。

在全网联的场景下，可以对CACC车队轨迹进行规划，进而控制CACC车辆轨迹，以达到减少瓶颈处交通振荡、减缓冲击波、提高瓶颈通行效率的目标。因此，在对人工驾驶的网联车进行轨迹预测的基础上对CACC车队的轨迹进行规划控制尤为重要。

发明内容

本发明的目的是为CACC车辆整体规划兼顾瓶颈效率和轨迹平滑性的轨迹，提出了一种基于空间离散化的CACC车队轨迹规划方法。

本发明的技术方案为：一种基于空间离散化的CACC车队轨迹规划方法，其特征在于，包括以下步骤：

S1、采用空间离散化的方式在空间轴上等间距取样，得到CACC车辆到达每个空间离散点的时间向量，并将其作为决策变量。

S2、根据决策变量建立轨迹规划模型的约束条件和目标函数，得到轨迹规划模型。

S3、采用F-W算法求解轨迹规划模型，得到CACC车队到达瓶颈点的规划轨迹。

进一步地，步骤S2中轨迹规划模型的约束条件包括动力学约束、与前车的安全车头时间距离约束、CACC车辆间的车头时间距离约束、初始状态约束和终止状态约束。

进一步地，动力学约束包括速度约束和加速度约束，速度约束为：

其中t_i(j)表示车辆i到达空间离散点j的时刻，v_max表示CACC车辆的最大速度，C表示CACC车队集合，P_i表示车辆i在进行轨迹规划时刻所在的空间离散点，N表示空间离散点总数，δ表示离散空间连续两点之间的距离。

加速度约束为：

其中a_min表示CACC车辆的最小加速度，a_max表示CACC车辆的最大加速度。

进一步地，与前车的安全车头时间距离约束为：

t_i(j)-t_p(j)≥h_si∈C,j＝P_i,…,N-1

其中t_i(j)表示车辆i到达空间离散点j的时刻，t_P(j)表示CACC车队前方人工驾驶车辆到达空间离散点j的时刻，h_s表示CACC车辆与前方人工驾驶车辆的安全车头时距，C表示CACC车队集合，P_i表示车辆i在进行轨迹规划时刻所在的空间离散点，N表示空间离散点总数。

进一步地，CACC车辆间的车头时间距离约束为：

t_i(j)-t_i-1(j)>h_mi∈C,j＝P_i,…,N-1

其中t_i(j)表示车辆i到达空间离散点j的时刻，h_m表示CACC车辆与CACC车辆之间的安全车头时距，C表示CACC车队集合，P_i表示车辆i在进行轨迹规划时刻所在的空间离散点，N表示空间离散点总数。

进一步地，初始状态约束为：

t_i(P_i)＝T_i,v_i(P_i)＝V_i,i∈C

其中t_i(P_i)表示车辆i到达空间离散点P_i的时刻，P_i表示车辆i在进行轨迹规划时刻所在的空间离散点，T_i表示车辆i的轨迹，v_i(P_i)表示车辆i在空间离散点P_i的速度，V_i表示车辆i在轨迹规划时刻的速度，C表示CACC车队集合。

进一步地，终止状态约束为：

v_end-μ≤v_i(N)≤v_end+μ,i∈C

其中v_end表示预测的前方人工驾驶车辆到达瓶颈点时的速度，μ表示速度范围调控阈值，v_i(N)表示表示车辆i在最后一个空间离散点的速度，N表示空间离散点总数，C表示CACC车队集合。

进一步地，步骤S2中轨迹规划模型的目标函数为：

Min A＝αT+βG+γS

其中A表示目标函数，α,β,γ均为权重系数，T表示CACC车队到达瓶颈点S_B的时间，t_i(j)表示车辆i到达空间离散点j的时刻，n表示CACC车队中的车辆总数，N表示空间离散点总数，C表示CACC车队集合，G表示CACC车队头车与前方人工驾驶车辆的空白间隙，t_P(j)表示CACC车队前方人工驾驶车辆到达空间离散点j的时刻，P_i表示车辆i在进行轨迹规划时刻所在的空间离散点，S表示CACC车辆的加速度平方和，a_i(j)表示车辆i在空间离散点j的加速度。

进一步地，步骤S3包括以下分步骤：

S31、将轨迹规划模型转化为线性规划子问题，并初始化设置线性规划子问题的解为t₀，迭代次数k＝0，迭代结束误差阈值为c，线性规划子问题为：

s.t.d_k∈Ω

其中A(t_k)表示第k次迭代的目标函数，T表示转置，t_k表示第k次迭代线性规划子问题的解，d_k表示第k次迭代的约束条件，Ω为约束条件集合。

S32、更新迭代步长γ：

S33、更新线性规划子问题的解和迭代次数：

t_k+1←t_k+γ(d_k-t_k)

k←k+1

S34、判断是否满足终止条件‖t_k+1-t_k‖<c，若是则得到CACC车队到达瓶颈点的规划轨迹，否则返回步骤S31。

本发明的有益效果是：

(1)本发明基于全网联场景及已知前车的预测轨迹进行建模，在建模时考虑了对CACC车队车辆整体轨迹平滑以及终止状态等优化目标，得到的CACC规划轨迹满足平滑性要求。

(2)本发明将CACC车队作为控制手段引入到交通流的管控中来，能够减小与前方人工驾驶车辆的空白间隙，平滑上游车辆的轨迹，提高瓶颈点的通行效率。

附图说明

图1所示为本发明实施例提供的一种基于空间离散化的CACC车队轨迹规划方法流程图。

图2所示为本发明实施例提供的轨迹规划空间离散化决策变量示意图。

图3所示为本发明实施例提供的空白间隙最小化示意图。

具体实施方式

现在将参考附图来详细描述本发明的示例性实施方式。应当理解，附图中示出和描述的实施方式仅仅是示例性的，意在阐释本发明的原理和精神，而并非限制本发明的范围。

本发明实施例提供了一种基于空间离散化的CACC车队轨迹规划方法，如图1所示，包括以下步骤S1～S3：

S1、采用空间离散化的方式在空间轴上等间距取样，得到CACC车辆到达每个空间离散点的时间向量，并将其作为决策变量。

本发明实施例中，将CACC车队轨迹规划问题转化成决策变量为CACC车辆到达每个空间离散点的时间优化问题，如图2所示，以空间离散化的方式在空间轴上等间距取样获取决策变量t＝[t_i(1),t_i(2),t_i(3),…,t_i(N)]，其中t_i(N)表示车辆i到达最后一个空间离散点N的时刻，N表示空间离散点总数。

S2、根据决策变量建立轨迹规划模型的约束条件和目标函数，得到轨迹规划模型。

本发明实施例中，轨迹规划模型的约束条件包括动力学约束、与前车的安全车头时间距离约束、CACC车辆间的车头时间距离约束、初始状态约束和终止状态约束。

其中，动力学约束包括速度约束和加速度约束。

CACC车辆在行驶的过程中，为保证安全，其速度和加速度等运动指标不能超过一定阈值限制。因此本发明实施例采用的是空间离散化的思想，速度可以表示为离散空间连续两点的距离δ除以到达两点时的时刻之差，即速度v_i(j)和加速度a_i(j)可分别表示为：

其中v_i(j)表示车辆i在空间离散点j的速度，t_i(j)表示车辆i到达空间离散点j的时刻，a_i(j)表示车辆i在空间离散点j的加速度，C表示CACC车队集合，P_i表示车辆i在进行轨迹规划时刻所在的空间离散点，N表示空间离散点总数，δ表示离散空间连续两点之间的距离。

综合上述两式有以下公式：

则有速度约束：

加速度约束：

其中v_min表示CACC车辆的最小速度，v_max表示CACC车辆的最大速度，a_min表示CACC车辆的最小加速度，a_max表示CACC车辆的最大加速度，

上面的公式中，由于速度v_i(j)和加速度a_i(j)的分母包含决策变量，是非线性约束条件，会给模型求解带来巨大困难，因此，对于速度约束，将其转换为线性约束条件为：

对于加速度，则通过下列方法将其转换为线性约束条件，则有：

若定义

为加速度约束中间变量，有以下等式：

则加速度约束可以转化为以下约束：

上面的加速度约束公式虽然不能完全代表加速度的约束，但其是加速度约束的严格保证，如果变量满足上面的公式，则一定会满足上述公式的加速度约束，同时也能够较为接近加速度约束上下限。

为保证CACC车队的行驶安全，对于CACC车队头车，其与前方人工驾驶车辆的车头时间距离应该不小于自动驾驶车辆与前方人工驾驶车辆的安全车头时距h_s，即与前车的安全车头时间距离约束为：

t_i(j)-t_p(j)≥h_si∈C,j＝P_i,…,N-1

其中t_P(j)表示CACC车队前方人工驾驶车辆到达空间离散点j的时刻。

在为每辆CACC车辆规划轨迹时，还需考虑CACC车队内部各车辆间的行驶安全，即CACC车队中的CACC车辆需与其前方行驶的CACC车辆保持在安全车头时距h_m，则CACC车辆间的车头时间距离约束为：

t_i(j)-t_i-1(j)>h_mi∈C,j＝P_i,…,N-1

当CACC车队进入控制区域以后会向云控中心发送自身的位置、速度等状态信息，由于本发明实施例中假设CACC车队一旦全部进入控制区域，就立即进行轨迹规划，因此，此时的运动状态也应作为轨迹规划模型的输入。由于本发明实施例中不对加速度的变化做约束，只对速度进行约束，在此不约束加速度的初值。因此初始状态约束为：

t_i(P_i)＝T_i,v_i(P_i)＝V_i,i∈C

其中t_i(P_i)表示车辆i到达空间离散点P_i的时刻，T_i表示车辆i的轨迹，v_i(P_i)表示车辆i在空间离散点P_i的速度，V_i表示车辆i在轨迹规划时刻的速度。

当CACC车辆离开瓶颈点时，其速度必须要有一定的限制，如果远大于前车的行驶速度，虽然在轨迹规划的范围内不会有安全隐患，但如若到达瓶颈点下游路段之后，下游路段的平均速度较低，高速行驶的CACC车辆容易和前车发生追尾，降低行车安全性，因此对于CACC车队，则需令其出瓶颈点时的速度在一定的速度范围内。令v_end为预测的前方人工驾驶车辆到达瓶颈点时的速度，则允许的速度变化范围值为区间[v_end-μ,v_end+μ]，其中μ表示速度范围调控阈值。因此终止状态约束为：

v_end-μ≤v_i(N)≤v_end+μ,i∈C

其中v_i(N)表示表示车辆i在最后一个空间离散点的速度。

本发明实施例中进行轨迹规划的核心目标是通行能力、轨迹平滑度以及与前车的空白间隙Gap。对于通行能力来说，主要是要求规划的CACC车队能够尽快地驶离瓶颈点，进入瓶颈点下游路段。而轨迹平滑度与通行能力也并非是独立的关系，因为在交通流比较稠密的状态下，轨迹越平滑，则意味着交通流越稳定，可间接提高瓶颈处的通行能力。

瓶颈的通行能力由CACC车队到达瓶颈点S_B的时刻决定。在保证与前车轨迹安全间隔的情况下，CACC车辆越早到达瓶颈点S_B，意味着通行时间越短，车辆通过量越大。此外，在保证安全的前提下，到达瓶颈的速度也应越快越好，因此有：

其中T表示CACC车队到达瓶颈点S_B的时间，n表示CACC车队中的车辆总数。

与单个CACC车辆的速度协调相比，用CACC车队进行速度协调的优点是可以同时通过头车和尾车进行轨迹控制，达到更多的控制目标。因此，本发明实施例考虑最小化车队头车与前车的间隙之和。如图3所示，即阴影部分的面积最小化：

其中G表示CACC车队头车与前方人工驾驶车辆的空白间隙。

在瓶颈处，因交通流较为稠密，CACC车辆与人工驾驶车辆交互频繁，保证规划轨迹的平滑性实则是降低速度的变化幅度，减少急加减速和怠速的情况，进而降低通行能力下降的概率。同时，轨迹的平滑度定义为加速度的L2范数，即计算加速度的平方和S最小化：

因此，整个轨迹规划模型目标函数可以写成以下表达式：

Min A＝αT+βG+γS

其中A表示目标函数，α,β,γ均为权重系数。

S3、采用F-W算法求解轨迹规划模型，得到CACC车队到达瓶颈点的规划轨迹。

通过约束转换，轨迹优化模型的约束条件全部转化成线性的。在目标函数中，通行能力和与前车的空白间隙Gap这两个目标函数是线性的，然而轨迹平滑度这个目标函数是非线性的。在目标函数是线性的条件下，因约束条件全为线性，整个优化模型就是一个典型的线性规划问题，可以直接调用现成的求解器求解。而在目标函数是非线性的条件下，较难得到其最优解，可以使用常见的非线性迭代算法来得到模型的较优解，本发明实施例采用交通领域中的F-W算法来求解上述目标函数为非线性的优化问题。

步骤S3包括以下分步骤S31～S34：

S31、将轨迹规划模型转化为线性规划子问题，并初始化设置线性规划子问题的解为t₀，迭代次数k＝0，迭代结束误差阈值为c，线性规划子问题为：

s.t.d_k∈Ω

其中A(t_k)表示第k次迭代的目标函数，T表示转置，t_k表示第k次迭代线性规划子问题的解，d_k表示第k次迭代的约束条件，Ω为约束条件集合。

S32、更新迭代步长γ：

S33、更新线性规划子问题的解和迭代次数：

t_k+1←t_k+γ(d_k-t_k)

k←k+1

S34、判断是否满足终止条件‖t_k+1-t_k‖<c，若是则得到CACC车队到达瓶颈点的规划轨迹，否则返回步骤S31。

如果还有其它优化目标，如轨迹的简易度(简易度表示车辆在连续位置上速度变化幅度)等非线性目标函数，也可使用上述F-W算法进行迭代求解。

本发明在基于前车预测轨迹的情况下，以空间离散化的思路建模，在若干约束条件和目标函数下，通过F-W算法得到了CACC车队的规划轨迹。本发明将CACC车队作为控制手段引入到交通流的管控中来，能够减小与前方人工驾驶车辆的空白间隙，同时平滑上游车辆的轨迹，以提升瓶颈通行效率。

本领域的普通技术人员将会意识到，这里所述的实施例是为了帮助读者理解本发明的原理，应被理解为本发明的保护范围并不局限于这样的特别陈述和实施例。本领域的普通技术人员可以根据本发明公开的这些技术启示做出各种不脱离本发明实质的其它各种具体变形和组合，这些变形和组合仍然在本发明的保护范围内。

Claims (1)

1.一种基于空间离散化的CACC车队轨迹规划方法，其特征在于，包括以下步骤：

S1、采用空间离散化的方式在空间轴上等间距取样，得到CACC车辆到达每个空间离散点的时间向量，并将其作为决策变量；

S2、根据决策变量建立轨迹规划模型的约束条件和目标函数，得到轨迹规划模型；

S3、采用F-W算法求解轨迹规划模型，得到CACC车队到达瓶颈点的规划轨迹；

所述步骤S2中轨迹规划模型的约束条件包括动力学约束、与前车的安全车头时间距离约束、CACC车辆间的车头时间距离约束、初始状态约束和终止状态约束；

所述动力学约束包括速度约束和加速度约束，所述速度约束为：

其中t_i(j)表示车辆i到达空间离散点j的时刻，v_max表示CACC车辆的最大速度，C表示CACC车队集合，P_i表示车辆i在进行轨迹规划时刻所在的空间离散点，N表示空间离散点总数，δ表示离散空间连续两点之间的距离；

所述加速度约束为：

其中a_min表示CACC车辆的最小加速度，a_max表示CACC车辆的最大加速度；

所述与前车的安全车头时间距离约束为：

t_i(j)-t_p(j)≥h_s i∈C，j＝P_i，...，N-1

其中t_i(j)表示车辆i到达空间离散点j的时刻，t_P(j)表示CACC车队前方人工驾驶车辆到达空间离散点j的时刻，h_s表示CACC车辆与前方人工驾驶车辆的安全车头时距，C表示CACC车队集合，P_i表示车辆i在进行轨迹规划时刻所在的空间离散点，N表示空间离散点总数；

所述CACC车辆间的车头时间距离约束为：

t_i(j)-t_i-1(j)＞h_m i∈C，j＝P_i，...，N-1

其中t_i(j)表示车辆i到达空间离散点j的时刻，h_m表示CACC车辆与CACC车辆之间的安全车头时距，C表示CACC车队集合，P_i表示车辆i在进行轨迹规划时刻所在的空间离散点，N表示空间离散点总数；

所述初始状态约束为：

t_i(P_i)＝T_i，v_i(P_i)＝V_i，i∈C

其中t_i(P_i)表示车辆i到达空间离散点P_i的时刻，P_i表示车辆i在进行轨迹规划时刻所在的空间离散点，T_i表示车辆i的轨迹，v_i(P_i)表示车辆i在空间离散点P_i的速度，V_i表示车辆i在轨迹规划时刻的速度，C表示CACC车队集合；

所述终止状态约束为：

v_end-μ≤v_i(N)≤v_end+μ，i∈C

其中v_end表示预测的前方人工驾驶车辆到达瓶颈点时的速度，μ表示速度范围调控阈值，v_i(N)表示车辆i在最后一个空间离散点的速度，N表示空间离散点总数，C表示CACC车队集合；

所述步骤S2中轨迹规划模型的目标函数为：

Min A＝αT+βG+γS

其中A表示目标函数，α，β，γ均为权重系数，T表示CACC车队到达瓶颈点S_B的时间，t_i(j)表示车辆i到达空间离散点j的时刻，n表示CACC车队中的车辆总数，N表示空间离散点总数，C表示CACC车队集合，G表示CACC车队头车与前方人工驾驶车辆的空白间隙，t_P(j)表示CACC车队前方人工驾驶车辆到达空间离散点j的时刻，P_i表示车辆i在进行轨迹规划时刻所在的空间离散点，S表示CACC车辆的加速度平方和，a_i(j)表示车辆i在空间离散点j的加速度；

所述步骤S3包括以下分步骤：

S31、将轨迹规划模型转化为线性规划子问题，并初始化设置线性规划子问题的解为t₀，迭代次数k＝0，迭代结束误差阈值为c，所述线性规划子问题为：

s.t.d_k∈Ω

其中A(t_k)表示第k次迭代的目标函数，T表示转置，t_k表示第k次迭代线性规划子问题的解，d_k表示第k次迭代的约束条件，Ω为约束条件集合；

S32、更新迭代步长γ：

S33、更新线性规划子问题的解和迭代次数：

t_k+1←t_k+γ(d_k-t_k)

k←k+1

S34、判断是否满足终止条件||t_k+1-t_k||＜c，若是则得到CACC车队到达瓶颈点的规划轨迹，否则返回步骤S31。

Images