一种自动驾驶交叉口车辆进入时刻与行驶轨迹控制方法

技术领域

本发明属于智能交通控制领域，涉及城市道路针对自动驾驶车辆通过交叉口的交通控制技术领域，更具体地说，涉及一种自动驾驶交叉口车辆进入时刻与行驶轨迹控制方法。

背景技术

自动驾驶技术最早在20世纪90年代在美国军事领域应用，经过多年发展，已成为智能交通发展的趋势，我国工信部公示了拟于2021年1月1日开始实施的《汽车驾驶自动化分级》推荐性国家标准报批稿。由此可见，我国对自动驾驶技术高度重视。自动驾驶技术的发展能较大程度改变交叉口的控制方式，现有交叉口多采用信号控制，许多研究表明，在自动驾驶环境下，车辆在无信号控制交叉口相互穿插通行的效率更高。为了确保无信号控制交叉口自动驾驶车辆的行车安全，研究学者提出多种控制方法，主要可分为基于冲突点的控制方法和基于交叉口空间离散化的控制方法两种。但无论哪种方法，研究主要集中在通过建立控制模型，确定车辆进入交叉口的时间和次序。目前，许多研究基于车辆在路段和交叉口内部匀速行驶的假设建立控制模型，如论文：Erasing Lane Changes From Roads: ADesign of Future Road Intersections(IEEE Transactions on IntelligentVehicles, vol. 3, no. 2, pp. 173-184, 2018)；Corridor level cooperativetrajectory optimization with connected and automated vehicles(TransportationResearch Part C: Emerging Technologies, 2019, vol.105: pp. 405-421)；Intersection Control and Delay Optimization for Autonomous Vehicles Flowsonly as Well as Mixed Flows with Ordinary Vehicles(Vehicles, 2020, 2(3):pp.523-541)；自动驾驶环境下交叉口车辆路径规划与最优控制模型(自动化学报,2020,46(09): pp. 1971-1985)等；专利：一种自动驾驶下交叉口通行权设置方法(CN201810822950.0)。现有研究鲜有针对车辆的行驶轨迹进行优化控制。

本发明提出一种自动驾驶交叉口车辆进入时刻与行驶轨迹控制方法，根据交叉口进出口道确定车辆在交叉口内部的行车轨迹并确定轨迹间的交点作为冲突点，根据轨迹上冲突点的时间及同一车道上前车进入交叉口的时间和车速调整后车行驶轨迹，在考虑行车安全的同时考虑通行效率，通过调整车速使得车辆尽可能在冲突点时间以较大速度到达冲突点，或者在有前车的情况下，后车尽可能以前车相等的车速，紧跟前车进入交叉口。车辆经过冲突点后更新冲突点时间，车辆尽快速通过交叉口能为其他车辆节省交叉口的时空资源。具体如何将车辆分类型考虑，根据冲突点时间或前车进入交叉口的时间调整车辆在路段和交叉口的行驶轨迹和进入时刻是本发明重点解决的问题。

经对现有技术的文献检索发现：现有交叉口交通控制方式主要可分为信号控制和无信号控制两方面，①针对车速引导的研究多集中在信号控制交叉口，一部分研究通过预测车辆到达时间，对信号配时进行调整，适应车辆到达；一部分研究在信号配时方案固定的条件下，通过车速诱导，使车辆尽可能在当前绿灯下通过，或减速行驶在下个周期绿灯通过，部分研究同时考虑信号配时优化和车速引导。②在自动驾驶环境下无信号控制交叉口研究中，为确保车辆在无信号控制的情况下安全通过交叉口，多侧重于车辆进入交叉口的次序和时间的优化，使得车辆相互穿插通行，研究中多考虑车辆始终匀速行驶，并未针对车速进行优化控制，获得车辆进入交叉口的行驶轨迹。

发明内容

本发明目的是为了克服上述技术存在的不足，提出一种自动驾驶交叉口车辆进入时刻与行驶轨迹控制方法。

技术方案：为解决上述技术问题，本发明的一种自动驾驶交叉口车辆进入时刻与行驶轨迹控制方法，包括如下步骤：

步骤1：采集交叉口信息作为控制模型的输入条件，包括交叉口的类型、大小、各方向进口道和出口道的车道数、车道宽度；采集车辆信息，包括当前时刻车辆所在位置到交叉口停车线的距离s

步骤2：在交叉口建立直角坐标系，确定车辆通过进口道和出口道的位置点坐标，根据进口道位置点和出口道位置点，建立方程刻画车辆在交叉口内部的行车路径，路径间的交点为冲突点，通过路径方程确定各冲突点的坐标，根据距离公式计算每条轨迹上进口道位置点到冲突点的距离。

步骤3：设置速度控制区域，针对每辆车计算速度控制区域的长度，车辆进入速度控制区域后不再变换车道，在速度控制区域内调整车速；

步骤4：将车辆分为有前车和无前车两类，车辆为每条进口车道第一辆车时，建立控制模型，在满足冲突点时间制约地条件下，车辆以尽可能大的速度到达冲突点，确定车辆进入交叉口的时间及行驶轨迹，当车辆通过冲突点后更新冲突点的时间；

步骤5：车辆在进口车道上有前车时，前车和冲突点对车辆均有制约，为了确保行车安全，兼顾效率，后车尽可能以前车进入交叉口的时间到达交叉口，且车速不能大于前车车速，建立控制模型，确定车辆进入交叉口的时间及行驶轨迹，当车辆通过冲突点后更新冲突点的时间；

所述步骤3包括如下步骤：

步骤31：车辆在各自的速度控制区域内调整速度，不换道行驶；速度控制线外车辆匀速行驶速度为V

式中：V

所述步骤4，包括如下步骤：

步骤41：根据同一车道上车辆是否有前车，将车辆分为两类，第一类，同一车道上没有前车，即车辆为当前车道第一辆车时，只有冲突点的时间制约车辆，当车辆到达冲突点的时刻大于等于冲突点的时刻时，车辆能顺利通过冲突点，否则，车辆不能顺利通过冲突点，需要根据冲突点的时间调整车速或者停车等待，调整速度在冲突点时间通过冲突点，且通过速度尽可能大；第二类，同一车道上有前车时，需要同时考虑前车和冲突点的约束，前车进入交叉口的时刻和速度通过计算求得，调整车辆的速度，使得到达停车线的时刻尽可能等于前车进入交叉口的时刻，到达停车线的速度等于前车进入交叉口是速度，当前车约束车辆不能直接通过交叉口，需要停车等待时，车辆尽可能早到停车线停车等待。

步骤42：对于无前车的情况，即车辆为当前车道的第一辆车，只有冲突点制约车辆进入交叉口，根据冲突点的时间调整车速，再计算车辆通过交叉口停车线的时间和速度，为后车控制提供依据；

式中：表示各方向进口道第一辆车的集合，

当车辆通过每个冲突点后，更新冲突点的时间，车辆n只能在

式中：

当

式中：d

步骤43：

式中：v

当

车辆从停车线以

式中：

步骤44：当

情况1：如图5所示，调整速度后，到达冲突点的时刻等于冲突点时刻

式中：T

以速度波动最小为最佳方案，如公式(16)约束：

最终得到距离与时间的函数和速度与时间的函数如公式(17)-(18)所示：

式中：

情况2：当车辆进入速度控制区域，通过速度调整，到达冲突点的车速可能达不到V

式中：T

以车辆到达冲突点的速度最大为目标，满足公式(24)约束：

最终得到距离与时间的函数和速度与时间的函数如公式(25)-(26)所示：

步骤45：根据车辆到达冲突点的速度v

所述步骤5，包括如下步骤：

步骤51：对于有前车的情况，前车对后车制约，由于车辆在速度控制区域不换道，后车不可能越过前车提前进入交叉口，因此首先考虑前车对后车的制约；

当

(1) 当

(2) 当

(3) 当

车辆从当前位置开始以V

式中： T

在速度

式中，α、β表示权重系数；

得到距离与时间的函数和速度与时间的函数如公式(38)-(39)所示：

当

步骤52：当

情况3：车辆n-1与车辆n的转向相同时，在交叉口内部的行驶轨迹相同，制约车辆n和车辆n-1的冲突点相同，车辆n-1不受冲突点时刻的制约，则第n辆车同样不受冲突点时刻制约，能紧跟前车通过交叉口；

情况4：车辆n-1与车辆n的转向不相同时，在交叉口内部的行驶轨迹不相同，制约车辆n和车辆n-1的冲突点不相同，需进一步判断车辆n能否不停车进入交叉口；

本发明有益效果：

很多自动驾驶环境下交叉口控制的研究中，在所有车辆匀速行驶的假设前提下，以交叉口通行效率最大或延误最低为目标，优化车辆进入次序和进入时间，并未考虑车辆的行驶轨迹控制，但是车辆行驶轨迹会影响车辆到达交叉口的时间，本发明考虑在自动驾驶环境下，交叉口无需信号灯控制，车辆进入交叉口服从先到先服务的通行规则，车辆在交叉口内部相互穿插通行，根据轨迹上冲突点的时刻和当前车辆所在位置、速度，对车辆进入交叉口前的轨迹进行优化控制，确保车辆在速度控制区域内以最佳轨迹行驶、最佳进入时刻安全通过交叉口。本发明在确保车辆通行安全的同时，通过优化车速，尽量以最大速度进入交叉口，能减少在交叉口内行驶的时间，可以留更多的时间给其他车辆，能确保通行效率。

附图说明

图1 为本发明方法的流程图；

图2 为交叉口冲突点示意图；

图3 为速度控制线示意图；

图4 为s-t图；

图5 为步骤4中情况1对应v-t图；

图6 为步骤4中情况2对应v-t图；

图7 为步骤5对应v-t图；

图8 为交叉口东进口方向车辆位置及交叉口内部行驶路径示意图。

具体实施方式

结合附图1～8和实施例，对本发明技术方案详细说明如下：

实施例1

步骤1：采集交叉口信息作为控制模型的输入条件，包括交叉口的类型、大小、各方向进口道和出口道的车道数、车道宽度；采集车辆信息，包括当前时刻车辆所在位置到交叉口停车线的距离s

步骤2：在交叉口建立直角坐标系，确定车辆通过进口道和出口道的位置点坐标，根据进口道位置点和出口道位置点，建立方程刻画车辆在交叉口内部的行车路径，路径间的交点为冲突点，通过路径方程确定各冲突点的坐标，根据距离公式计算每条轨迹上进口道位置点到冲突点的距离；

步骤3：设置速度控制区域，针对每辆车计算速度控制区域的长度，车辆进入速度控制区域后不再变换车道，在速度控制区域内调整车速；

步骤4：将车辆分为有前车和无前车两类，车辆为每条进口车道第一辆车时，建立控制模型，在满足冲突点时间制约地条件下，车辆以尽可能大的速度到达冲突点，确定车辆进入交叉口的时间及行驶轨迹，当车辆通过冲突点后更新冲突点的时间；

步骤5：车辆在进口车道上有前车时，前车和冲突点对车辆均有制约，为了确保行车安全，兼顾效率，后车尽可能以前车进入交叉口的时间到达交叉口，且车速不能大于前车车速，建立控制模型，确定车辆进入交叉口的时间及行驶轨迹，当车辆通过冲突点后更新冲突点的时间。

所述步骤3包括如下步骤：

步骤31：车辆在各自的速度控制区域内调整速度，不换道行驶；速度控制线外车辆匀速行驶速度为V

式中：V

根据步骤1，实施例以双向两车道的平面十字交叉口为例，各方向进口道和出口道的车道数均为2，车道宽度均为3m；采集到当前时刻2辆车的状态数据进行实施例计算：采集到第一辆车距离停车线的长度为s1=60.50m，在东方向最内侧进口车道左转，在交叉口内部在轨迹r1上行驶；第二辆车距离停车线的长度为s2=80.50m，在南方向最外侧进口车道直行，在交叉口内部在轨迹r2上行驶，行驶速度V

步骤1中根据车辆当前所在位置到停车线的距离大小对车辆进入交叉口的次序进行排序，车辆在行驶过程中由于调整车速，不同车辆到交叉口停车线的距离大小可能发生变化，但已经排序好的次序不变。

步骤2中，在交叉口建立直角坐标系，东方向最内侧进口车道至南方向最内侧出口车道左转轨迹为r1、南方向最外侧进口道至北方向最外侧出口车道直行轨迹为r2，计算轨迹r1和r2的冲突点p1的坐标(x1, y1)；根据直角坐标系和车道宽度，得到轨迹r1的曲线方程和r2的直线方程分别为：

通过联立公式(1)和(2)即可求得冲突点p1的坐标为(4.5，0.487)。

类似的，可以求出所有轨迹相交产生的冲突点的坐标值。

对于直行轨迹，根据两点间坐标公式计算冲突点到位置点的距离；对于转弯轨迹，根据弧长公式计算冲突点的距离，以轨迹r1进入交叉口的位置点q1与冲突点p1的距离L

类似的，可以求出所有位置点到轨迹上冲突点的距离。

步骤3中，根据部分城市规定，城市道路最高时速为70公里，70km/h=19.45m/s。在本实施例中设置所有车辆在速度控制范围外路段的行驶速度V

实施例2

所述步骤4，包括如下步骤：

步骤41：根据同一车道上车辆是否有前车，将车辆分为两类，第一类，同一车道上没有前车，即车辆为当前车道第一辆车时，只有冲突点的时间制约车辆，当车辆到达冲突点的时刻大于等于冲突点的时刻时，车辆能顺利通过冲突点，否则，车辆不能顺利通过冲突点，需要根据冲突点的时间调整车速或者停车等待；第二类，同一车道上有前车时，在控制车辆行驶轨迹时需要同时考虑前车和冲突点的约束；

步骤42：对于无前车的情况，即车辆为当前车道的第一辆车，只有冲突点制约车辆进入交叉口，根据冲突点的时间调整车速，再计算车辆通过交叉口停车线的时间和速度，为后车控制提供依据；

式中：

当车辆通过每个冲突点后，更新冲突点的时间，车辆n只能在

式中：

当

式中：d

步骤43：

式中：v

当

车辆从停车线以

式中：

步骤44：当

情况1：调整速度后，到达冲突点的时刻等于冲突点时刻

(14)

式中：T

以速度波动最小为最佳方案，如公式(16)约束：

最终得到距离与时间的函数和速度与时间的函数如公式(17)-(18)所示：

式中：

情况2：当车辆进入速度控制区域，通过速度调整，到达冲突点的车速可能达不到V

式中：T

以车辆到达冲突点的速度最大为目标，满足公式(24)约束：

最终得到距离与时间的函数和速度与时间的函数如公式(25)-(26)所示：

步骤45：根据车辆到达冲突点的速度v

根据实施例1计算的速度控制线到停车线的距离为L

根据步骤43的公式(6)，计算车辆1进入速度控制区域后以a

根据步骤44中情况1计算行驶轨迹方案，设置加速度a_的取值范围均为[0m/s

(20)

求得结果为：T

得到距离与时间的函数和速度与时间的函数如公式(25)-(26)所示：

根据步骤45，车辆到达冲突点的速度v

实施例3

所述步骤5，包括如下步骤：

步骤51：对于有前车的情况，前车对后车制约，由于车辆在速度控制区域不换道，后车不可能越过前车提前进入交叉口，因此首先考虑前车对后车的制约；

当

(1) 当

(2) 当

(3) 当

车辆从当前位置开始以V

式中： T

在速度

式中，α、β表示权重系数；

得到距离与时间的函数和速度与时间的函数如公式(38)-(39)所示：

当

步骤52：当

情况3：车辆n-1与车辆n的转向相同时，在交叉口内部的行驶轨迹相同，制约车辆n和车辆n-1的冲突点相同，车辆n-1不受冲突点时刻的制约，则第n辆车同样不受冲突点时刻制约，能紧跟前车通过交叉口；

情况4：车辆n-1与车辆n的转向不相同时，在交叉口内部的行驶轨迹不相同，制约车辆n和车辆n-1的冲突点不相同，需进一步判断车辆n能否不停车进入交叉口；

车辆2和车辆1在同一进口车道，根据实施例2计算得，车辆1以

得到距离与时间的函数和速度与时间的函数如公式(38)-(39)所示：

本发明的实施方式并不受所述实施例的限制，其他的任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。