基于动态距离窗的交叉口CAV轨迹规划算法*

高志军，王江锋，陈 磊，董佳宽，罗冬宇，闫学东

（北京交通大学，综合交通运输大数据应用技术交通运输行业重点实验室，北京 100044）

前言

车路协同和自动驾驶技术在提高道路交通安全性和通行效率方面具有极其重要的应用价值，众多学者对此进行了大量研究［1-4］。作为城市道路网的节点和瓶颈点，交叉口一直制约着道路的通行能力。利用车路协同和自动驾驶技术提高交叉口通行效率、减少车辆延误，是目前的研究重点［5-7］。其中，借助车载单元（on board unit，OBU）与路侧单元（road side unit，RSU）通信获取交叉口信号灯信息，规划网联车（connected vehicle，CV）或智能网联车（connect⁃ed and autonomous vehicle，CAV）通过交叉口的轨迹是实现这一目标的重要手段。

CV 通过OBU 可实现与信号灯信息的共享，基于共享信息对驶入交叉口的CV 进行速度引导，使CV 高效并节能地通过交叉口。Yao 等［8］对CV 在驶入交叉口过程中的不同位置进行限速，使CV以较小的速度波动较快通过交叉口。Xia 等［9］利用干线多个交叉口的实时配时信息，设计了CV通过干线交叉口的速度引导算法，以减少油耗和排放。Qi 等［10］考虑了驾驶员在按照引导速度行驶过程会出现偏差，设计的速度引导算法具有一定的容错性，可适应驾驶员的行为。

随着自动驾驶技术的成熟，一些学者研究了适用于具备自动驾驶与网联功能的CAV 通过交叉口的轨迹规划算法。CAV 可实现车辆的自动化控制，使其完全按照规划轨迹行驶。CAV轨迹规划算法一般均基于延误、能耗或排放最小为目标进行设计。一些学者基于最小化油耗［11］、最小化延误和排放［12］、最小化加权延误和能耗［13］，分别提出了相应的CAV轨迹规划算法，并基于混合整数线性规划模型、双层规划模型、动态规划模型对提出的算法进行求解。此外，Li等［14］通过枚举交叉口所有可行配时方案，设计了滚动时域算法以确定与配时方案对应的CAV最优轨迹，提高了CAV 的通行效率。上述轨迹规划算法多只考虑通行效率，而交叉口的通行安全也是一个需要考虑的因素。Stebbins 等［15］提出了一种考虑交叉口安全约束的CAV轨迹规划算法。

现有轨迹规划算法或侧重于通行效率，或侧重于通行安全，较少考虑效率与安全的协同。针对此问题，本文中对驶入交叉口通信范围的CAV，兼顾其行驶的安全性与效率，引入动态距离窗（dynamic distance windows，DDW）的概念，设计了可实现CAV可控安全行驶下通行效率最优的交叉口轨迹规划算法。

1 轨迹规划问题描述

车路协同环境下，交叉口布设的RSU 可将实时信号灯信息发送给驶入交叉口通信范围CAV 的车载OBU，轨迹规划算法将信号灯信息和CAV 位置、速度等初始行驶状态参数作为输入，规划CAV 的行驶轨迹，实现CAV 不停车通过交叉口，在保证安全的情况下提升绿灯时间利用率和交叉口通行效率。图1 为典型有/无轨迹规划引导CAV 通过交叉口的轨迹示意图，图中vmax表示道路最大限速，tr表示绿灯启动损失时间。

图1 有/无轨迹规划的时空轨迹和速度轨迹

CAV 进入交叉口通信范围后，剩余绿灯时间内无法通过时，规划轨迹为先减速，再匀速，最后加速达到最大速度，在绿灯亮起时CAV 恰好以最大速度通过停车线，可以消除传统交通中的绿灯损失时间。而在传统交通中，车辆需要在停车线前停车，在绿灯亮起后，经过tr后加速通过交叉口。

2 基于DDW的CAV轨迹规划算法

2.1 算法思路

轨迹规划算法一般将延误最小作为优化目标，理论上，CAV 在绿灯相位期间以道路最大限速通过交叉口时，延误最小。但在实际轨迹规划中，须考虑CAV行驶安全，行驶过程应满足必要安全约束条件。该约束条件可保证CAV在停车线前安全停车。

式中：t为当前时刻；tG为绿灯亮起时刻；aB为最大舒适减速度；v(t)和x(t)分别为t时刻CAV 的速度和位置；xI为交叉口停车线的位置。

在考虑式（1）安全约束的基础上，Stebbins 等［15］研究发现车辆通过交叉口的延误为

式中：d为车辆通过交叉口的延误；xG和vG分别为绿灯亮起时车辆的位置和速度；vmax为道路最大限速；aA为最大舒适加速度。

基于文献［15］中的研究，车辆通过交叉口过程中在绿灯起始时刻tG，存在一个特定的位置和对应的速度，分别定义为“目标位置”和“目标速度”，用x*和v*表示。当车辆在tG时刻以速度v*到达位置x*时，其延误最小。目标速度v*和目标位置x*分别为

基于式（1）～式（4），本文中研究车路协同环境下CAV 通过交叉口的轨迹规划算法，当CAV 驶入交叉口通信范围时，根据当前速度和位置、信号灯信息，按照设计的CAV 规划算法所确定的引导速度行驶，可实现延误最小。

考虑到CAV 驶入交叉口通信范围时刻的初始行驶状态具有随机性，以及道路限速、舒适加/减速度等约束条件，并非所有CAV 都可实现在tG时刻以目标速度v*到达目标位置x*。当CAV 驶入交叉口通信范围的初始位置与目标位置之间距离处于一定范围内，才可设计可行轨迹使CAV 实现此目标。本文中将此距离范围区间定义为“距离窗”。将CAV 可在tG时刻以目标速度v*到达目标位置x*称为“实现初始目标”，将CAV 初始位置与目标位置的距离称为“初始距离”。本文中基于距离窗的轨迹规划算法框架如图2所示。

图2 基于距离窗的轨迹规划算法框架

当CAV 进入交叉口通信范围时，根据初始行驶状态参数计算初始距离是否处于距离窗内。针对初始距离处于距离窗内和距离窗外的两种情况，分别设计轨迹规划算法。

2.2 动态距离窗（DDW）

CAV 驶入交叉口通信范围时，不同的初始行驶状态及不同的tG，对应可以实现初始目标的距离窗不同，因此称为“动态距离窗”（DDW）。图3 为vmax、aA、aB和xI取值固定，对应不同初始速度v0及不同tG可实现初始目标的DDW示意图。

图3 不同v0下tG不同时可以实现初始目标的DDW

图3 中，CAV 加/减速过程均取最大舒适加/减速度，tg0、tg1和tg2分别表示3 个临界时间。其中tg0表示由v0可达到v*所需的最小时间，如果tG

3个临界时间tg0、tg1和tg2的计算公式为

与3 个临界时间tg0、tg1和tg2对应，可实现初始目标DDW 上限和下限的具体取值，可由图3 中DDW示意图结合运动学公式进行计算。对于tg0tg2等情况，同理可得到实现初始目标的DDW。当tG>tg2时，可实现初始目标的DDW 上限为粗点划线表示的速度轨迹（前面一段被粗实线和粗虚线覆盖）与横纵坐标围成的面积。但是此时的距离窗下限值必须大于细点划线表示的速度轨迹（前面一段被细实线和细虚线覆盖）与横纵坐标围成的面积，否则车辆在减速过程会出现速度为0 的情况，即出现停车现象，这不符合一般的驾驶习惯。为了避免规划轨迹出现速度太小甚至接近于0 的情况，实际的轨迹规划算法中可设定最小速度的阈值。另外，当初始参数不同时，tg1可等于或大于tg2，如图4所示，但DDW的计算原理不变。

图4 tg1≥tg2时可实现初始目标的DDW

2.3 算法设计

2.3.1 初始距离处于DDW内

当初始距离处于DDW 内时，可实现初始目标。轨迹规划算法根据车辆的当前行驶状态，在满足所有约束条件的情况下，设计3 个阶段的速度轨迹，即减速-匀速-加速、减速-匀速-减速、加速-匀速-减速、加速-匀速-加速中的一种以实现初始目标，如图5 所示。图中t0表示轨迹规划开始时刻，t1和t2分别为加速度切换时刻。在一些特殊情况下的初始行驶状态，只需1 个阶段或2 个阶段即可实现初始目标。

图5 3阶段速度轨迹

基于CAV 初始行驶状态及约束条件，结合图3所示DDW 示意图，可确定能实现初始目标的唯一速度轨迹，如表1所示。

表1 中，vuni表示3 阶段中匀速阶段的CAV 速度，，其他变量关系如下：

表1 初始距离处于DDW内的CAV 3阶段轨迹规划

表1 中，第1 列表示规划轨迹的速度变化模式；第2 列是不同速度轨迹阶段间的过渡时刻，t1和t2分别表示第1 和第2 阶段、第2 和第3 阶段之间的过渡时刻；第3 列为需要满足的约束条件；第4 列为各速度轨迹阶段的加速度，a1、a2和a3分别表示第1、第2和第3阶段的加速度。

根据表1 进行轨迹规划的计算时，当某一阶段的持续时间为0 时，3 阶段就退化为2 阶段轨迹。若计算得到t1= 0，此时vuni=v0，则是3 阶段中第1 阶段的持续时间为0 的特例，即t1= 0，此时的速度模式为匀速-加速或匀速-减速；若计算得到t2=tG，此时vuni=v*，则是3 阶段中第3 阶段的持续时间为0 时的特例，此时的速度模式为减速-匀速或加速-匀速；若计算得到t1=t2，则是3阶段中第2阶段的持续时间为0 时的特例，此时的速度模式为减速-加速或加速-减速。当规划轨迹只由匀减速、匀加速或匀速中的1 个速度轨迹变化模式即可实现初始目标时，3阶段就退化为1阶段轨迹。

1 阶段规划轨迹与2 阶段和3 阶段的判断方式不同，在初始时刻，首先通过距离约束判断初始行驶状态是否满足1 阶段规划。若=x*-x0，则满足1阶段规划，然后根据v0和v*的大小关系确定加速度：

若初始行驶状态不满足1 阶段规划轨迹时，则通过比较与x*-x0的大小，在3阶段规划的两个速度模式中选择其一。若>x*-x0，则选择减速-匀速-加速/减速的模式进行计算；若

2.3.2 初始距离处于DDW外

当初始距离处于距离窗之外时，则无法实现初始目标。由式（2）可知，tG时刻CAV 速度越大，同时越接近交叉口，则车辆延误越小。结合初始距离处于DDW 内的轨迹规划算法思路，初始距离处于DDW外的轨迹规划算法原理如图6和图7所示。

图6 初始距离处于DDW外的两类轨迹规划

图7 初始距离处于距离窗外的CAV轨迹规划

图6中，粗虚线和细虚线（前面一段被粗实线和细实线覆盖）分别表示初始行驶状态对应的DDW 上限和下限，粗实线和细实线分别表示初始距离大于DDW 上限和小于DDW 下限两种情况的轨迹规划，uG和vG分别表示tG时刻上述两种情况的速度。tN′和tN分别表示DDW 上限的轨迹规划和实际轨迹转折时刻，tM′和tM分别表示DDW 下限的轨迹规划和实际轨迹转折时刻。

图7 所示为初始距离处于DDW 外的CAV 轨迹规划算法原理。图中曲线表示CAV接近交叉口停车线过程中保证安全的位置-速度约束，即在停车线前任何位置以aB减速都可以安全停车。曲线及其下方区域为安全速度区域，图中表示位置的轴为其横轴。点划线表示以aB减速行驶，最后在停车线前停下的过程中的时间-速度线，图中表示时间的轴为其横轴。图中粗/细实线和粗/细虚线含义与图6一致。

在图7（a）中所示的初始距离小于DDW 下限的规划轨迹中，在t

在图7（b）中所示的初始距离大于DDW 上限的规划轨迹中，在tN′前的一段时间内，CAV 按照初始行驶状态对应DDW上限的速度轨迹行驶。当t=tN′时，CAV轨迹与DDW上限的轨迹开始不同。因为初始距离大于DDW 上限，如果按照DDW 上限的速度轨迹行驶，即若在t=tN′时开始减速，则在tG时车辆所到达的位置和速度在位置-速度曲线之下，此时的延误不是最小。因此，在tN′之后的一段时间内，CAV 应该仍按原来的加速度行驶。直到若当t=tN时，CAV开始进入到交叉口前安全约束范围内，则开始以aB减速行驶，在tG之后，则以aA加速行驶。

3 仿真验证

3.1 仿真案例描述

图8 为一条安装有信号灯的单向单车道道路，交叉口RSU 通信范围为200 m，即CAV 驶入交叉口上游200 m 范围时，即可根据收到的信号灯信息进行轨迹规划。道路限速设为vmax=50 km/h，信号灯绿灯时长为30 s，红灯时长为30 s。最大舒适加/减速度设置为aA=1.5 m/s2和aB=2 m/s2。根据设定的参数可以确定，交叉口上游的安全约束范围为48.2 m（图8 中右侧阴影区域），轨迹规划中初始目标的目标速度v*=7.9 m/s，目标位置为交叉口停车线上游15.6 m（图8 中的竖线）。将交叉口上游200 m 作为坐标轴原点，则x*=184.4 m。图8 中左侧阴影区域表示当CAV 驶入通信范围时，当前初始行驶状态对应的DDW。

图8 单向单车道交叉口示意图

仿真中车辆跟驰模型采用IDM 模型［16］，其中最小安全间距设置为2 m，车头时距设置为2 s。仿真中时间步长设定为0.1 s，车辆启动延误时间tr=2 s，车辆长度设置为4 m。仿真中0-30 s为红灯，31-60 s为绿灯（为简化场景，没有添加黄灯时间），仿真时长为1 个信号周期。仿真场景为一队CAV 驶入交叉口，头车执行轨迹规划算法，头车之后的CAV 跟随前车行驶。仿真中头车的初始位置为0 m，相邻CAV的初始间距相同。

3.2 结果分析

（1）仿真场景1：所有CAV 初始速度v0=40 km/h，初始车间距为35 m，规划轨迹为减速-匀速-加速。

根据初始参数可确定本场景中DDW 上限为405.3 m，下限为51.9 m。由于初始距离为184.4 m，处于距离窗内，因此本场景中第1 辆CAV 可实现初始目标。

为进行对比分析，选择无轨迹规划作为对比对象。有、无轨迹规划的第1辆CAV的行驶轨迹如图9所示。对于图9 中有轨迹规划而言，第1 辆CAV 在红灯期间以较小的速度行驶，然后在绿灯亮起时到达目标位置，在通过交叉口的过程中没有停车，且在仿真结束时，车辆行驶的距离更远。对于无轨迹规划而言，第1辆CAV 在红灯期间到达交叉口，需停车等待较长时间，当绿灯启亮时，经过一定的启动延误时间后通过交叉口。

图9 仿真场景1头车的时空轨迹

仿真期间，对于有轨迹规划和无轨迹规划引导的第1 辆CAV 的行驶速度如图10 所示。对于有轨迹规划而言，第1 辆CAV 只经历了减速-匀速-加速-匀速4 个阶段。其中，前30 s 的速度变化为轨迹规划算法中的减速-匀速-加速的3 阶段速度模式。而对于无轨迹规划而言，第1辆CAV经历了加速-匀速-减速-停车-加速-匀速共6个阶段。

仿真中所有通过交叉口车辆的时空轨迹如图11 所示。由图可知，有轨迹规划时，共有11 辆车可以不停车通过交叉口，且速度波动较小。对于无轨迹规划而言，只有9 辆通过交叉口，并且会出现停车现象，速度波动较大。

图10 仿真场景1的头车速度

图11 仿真场景1通过交叉口车辆的时刻轨迹

图12 为有、无轨迹规划两种情况的仿真中所有车辆平均速度变化。对于无轨迹规划而言，车辆平均速度波动较大；而对于有轨迹规划而言，车辆平均速度波动较小，且最小平均速度明显大于无轨迹规划时的最小平均速度。

图12 仿真场景1的车辆平均速度

（2）仿真场景2：所有CAV 初始速度v0=22 km/h，初始车间距为15 m，规划轨迹为匀速-加速。

根据初始参数可确定本场景中的DDW 上限为387.6 m，下限为30.3 m。由于初始距离为184.4 m，处于DDW 内，因此本场景中的第1辆CAV 可以实现初始目标。仿真期间，对于有、无轨迹规划引导的第1辆CAV的行驶速度如图13所示。

（3）仿真场景3：所有CAV的初始速度v0=10 km/h，初始车间距为8 m，规划轨迹为加速-匀速-加速。

根据初始参数可以确定本场景中的距离窗上限为366.7 m，下限为22.9 m。由于初始距离为184.4 m，处于DDW 内，因此本场景中的第1辆CAV 可以实现初始目标。仿真期间，对于有、无轨迹规划引导的第1辆CAV的行驶速度如图14所示。

图13 仿真场景2的头车速度

图14 仿真场景3的头车速度

（4）各仿真场景中的定量统计指标

各仿真场景中仿真结果的定量指标统计如表2所示。

表2 3个场景中有/无轨迹规划的定量指标比较

由表2 可以看出，与无轨迹规划相比，3 个仿真场景中有轨迹规划的车辆平均延误减少了12.4%～16.9%，平均速度提高了11.9%～13.6%。对于无轨迹规划而言，绿灯期间可以通过9 辆车，其中4～5 辆需要停车，总停车时间为26.3～38.6 s。而对有轨迹规划而言，可以通过11 辆，增加了22.2%，停车现象完全消失。仿真结束时，有轨迹规划的头车比无轨迹规划头车多行驶64.5～65.7 m，与无轨迹规划尾车相同序号的有轨迹规划车辆多行驶41.7～42.8 m。上述结果说明轨迹规划算法可以有效提高交叉口的通行效率，增加交叉口的通行能力。另外，相比于无轨迹规划，有轨迹规划的速度方差降低了42.8%～70.9%，说明有轨迹规划的车辆速度波动更小，有助于平滑交通流，保证乘员舒适性和行车安全性。

4 结论

引入动态距离窗概念设计了一种CAV 通过交叉口的轨迹规划算法。根据CAV 驶入交叉口通信范围时的初始行驶状态参数和信号灯信息、最大舒适加/减速度和道路限速等约束条件确定DDW 的范围，针对CAV 初始位置与停车线上游特定位置间的距离处于DDW 范围之内和之外两种情况，分别设计了相应的轨迹规划算法，使得不同初始行驶状态下CAV 都可实现延误最小，在保证安全的同时使CAV高效通过交叉口。仿真结果表明，算法可有效提高CAV 行驶速度，减少延误，提高交叉口通行能力，同时有助于减小CAV 通过交叉口过程中的速度波动。所提出的轨迹规划算法具有形式简易、可获得解析解和求解速度快等特点，对于未来车联网和自动驾驶中的车辆轨迹规划具有一定的参考价值。本研究主要针对单车进行轨迹规划，在下一步的工作中，将重点研究多车轨迹的协同规划。