一种非结构化场景下的交叉路口多车协同通行方法及系统与流程

1.本发明涉及交叉路口多车协同通行控制技术领域，特别是关于一种非结构化场景下的交叉路口多车协同通行方法及系统。


背景技术：

2.无信号灯的交叉路口是存在多种潜在碰撞风险的复杂场景。当多辆智能车同时通过交叉路口时，如何在保证安全的前提下有效提高全局通行效率，是多智能车协同规划领域的一个极具挑战性的问题。在城市结构化道路场景下，通过设计aim scheme（英文全称为“autonomous intersection management scheme”；中文全称为“交叉路口智能管理方案”），对交叉路口附近的cavs（英文全称为“connected and autonomous vehicles”；中文全称为“智能网联汽车”）进行统筹轨迹规划是提高交叉路口多车通行安全性和通行效率的有效手段。目前，针对非结构化交叉路口场景的研究比较少见。在智能露天矿山等场景，存在许多道路十分宽阔的非结构化交叉路口。当前，仍缺少用于解决非结构化交叉路口的多车协同规划问题的系统解决方案。
3.目前对aim scheme研究的主要成果有：1）基于规则的方案：基于“预定”原则或者先到先走的规则，每次只有一辆车通过路口，也就是说，这种方法在同一时刻只允许一辆车行驶在路口区域，可以保障安全，但是限制了交叉路口的通行效率。
4.2）基于优化的方案：建立代价函数和碰撞约束，求解最优控制问题以得出最优规划。这种方法将交叉路口视作连续的自由空间，扩展了每辆车的通行区域，通过数值计算的方法求出兼顾所有车辆通行效率的全局最优解，旨在最大程度上提高整体的通行效率。但是对所有车辆完全没有路线和优先顺序方面的约束，车辆容易做出超车，变换到对面车道等“轻浮”的机动，导致通行太过于混乱，而且只能采用数值方法求解，求解时间过长。


技术实现要素：

5.本发明的目的在于提供一种非结构化场景下的交叉路口多车协同通行方法及系统来克服或至少减轻现有技术的上述缺陷中的至少一个。
6.为实现上述目的，本发明提供一种非结构化场景下的交叉路口多车协同通行方法，其包括：步骤1，从非结构化场景下的交叉路口的中心开始，由内至外设置协同区域、接近区域和分流区域；步骤2，在所述接近区域设置虚拟道路中心线，取所述虚拟道路中心线与所述协同区域外边界线的交点作为转向点，从所述转向点开始，在所述虚拟道路中心线上间隔选取分流目标点；步骤3，确定每一区域内部以及不同区域之间的车辆优先级；
步骤4，按照所述车辆优先级，依次对所述分流区域内车辆进行分流轨迹规划，使所述车辆在进入所述协同区域前行驶到所述虚拟道路中心线上；步骤5，将进入所述协同区域内车辆作为等待协同轨迹规划车辆，按照所述车辆优先级进行协同轨迹规划。
7.进一步地，所述步骤5具体包括：步骤51，根据所述协同轨迹规划车辆的未来机动，确定其离开所述交叉路口所通过的路口，以该所通过的路口的所有所述转向点作为可选择的协同轨迹规划终点，获得无碰撞的最优轨迹；步骤52，根据所述协同轨迹规划车辆的未来机动，将所述接近区域内满足预设要求的车辆构建成为碰撞集，判断所述碰撞集中的车辆与所述最优轨迹之间的空间重合点的数量是否为0，如果是，则选择该最优轨迹作为所述协同轨迹；反之，则进入步骤53；步骤53，判断所述协同轨迹规划车辆以车辆速度最大值匀速行驶的情况下是否在所述空间重合点处也发生时间上的重合，如果是，则放弃该最优轨迹，反之，界定为未发生碰撞，则按照如下规则确定所述协同轨迹：原则一，若存在至少两条所述最优轨迹未发生碰撞时，优先选择耗时较短的所述最优轨迹作为所述协同轨迹；原则二，所有所述最优轨迹均发生碰撞时，则选择所述空间重合点的数量最小的所述最优轨迹作为协同轨迹，并重新进行避免碰撞的速度规划。
8.进一步地，所述步骤52中的所述预设要求包括如下需要同时满足的三点要求：第一要求，或，且车辆位于所述协同轨迹规划车辆的左侧；其中，表示所述协同区域内的车辆集合，表示所述接近区域内位于所述虚拟道路中心线上的车辆集合；第二要求，车辆与所述协同轨迹规划车辆存在交叉碰撞、汇入碰撞或追尾碰撞三种碰撞情形之一；第三要求，当所述协同轨迹规划车辆到达开始所述转向点时，而车辆还未驶出所述协同区域。
9.进一步地，所述步骤53中的“速度规划”的方法具体包括：步骤531，建立最优控制模型（1）：
式（1）中，为代价函数，、权重，为第个时间步长，为第个时间步长，、分别为将时间离散化后待协同轨迹规划的车辆协同轨迹规划车辆的加速度、速度；式（2）为车辆的运动学约束，、分别为笛卡尔坐标系下协同轨迹规划车辆的位置坐标、朝向角、加速度；式（3）为车辆边值约束，、、、分别为大地坐标系下协同轨迹规划车辆的刚进入协同区域时的初始位置、驶出协同区域时的最终位置、进入协同区域的起点和终点的位置坐标和朝向角；式（4）是对车辆速度和加速度的最大限制，为车辆加速度最大值；式（5）是车辆的碰撞约束，、分别为大地坐标系下第个空间重合点的坐标、协同轨迹规划车辆在碰撞集中车辆到达第个空间重合点的时刻位置的坐标，为空间重合点的总数，、分别为车辆间的横向和纵向的安全距离。
10.进一步地，所述步骤2中的“在所述接近区域设置虚拟道路中心线”的方法具体包括：步骤21，将接近区域内的两侧道路边界朝相向的方向平移预设距离，然后进行平滑处理，获得两条虚拟道路中心线；步骤22，在已获得的两条虚拟道路中心线的基础上，再采用所述步骤31相同的平移方法，得到其它的虚拟道路中心线。
11.进一步地，所述步骤3确定的每一区域内部的车辆优先级为：在同一区域中的车辆，每辆车辆的优先级与该车辆到所述中心之间的距离成反比；所述步骤3确定的不同区域之间的车辆优先级为：所述协同区域内优先级最低的车辆在不同区域的优先级高于所述接近区域内优先级最高的车辆，所述接近区域
内优先级最低的车辆在不同区域的优先级高于所述分流区域内优先级最高的车辆。
12.本发明还提供一种非结构化场景下的交叉路口多车协同通行系统，其包括：分区单元，其用于从非结构化场景下的交叉路口的中心开始，由内至外设置协同区域、接近区域和分流区域；优先级确定单元，其用于确定每一区域内部以及不同区域之间的车辆优先级；虚拟道路中心线设置单元，其用于在所述接近区域设置虚拟道路中心线，取所述虚拟道路中心线与所述协同区域外边界线的交点作为转向点，从所述转向点开始，在所述虚拟道路中心线上间隔选取分流目标点；分流规划单元，其用于按照所述车辆优先级，依次对所述分流区域内车辆进行分流轨迹规划，使所述车辆在进入所述协同区域前行驶到所述虚拟道路中心线上；协同规划单元，其用于将进入所述协同区域内车辆作为等待协同轨迹规划车辆，按照所述车辆优先级进行协同轨迹规划。
13.进一步地，所述协同规划单元具体包括：最优轨迹获取子单元，其用于所述协同轨迹规划车辆的未来机动，确定其离开所述交叉路口所通过的路口，以该所通过的路口的所有所述转向点作为可选择的协同轨迹规划终点，获得无碰撞的最优轨迹；协同轨迹规划子单元，其用于所述协同轨迹规划车辆的未来机动，将所述接近区域内满足预设要求的车辆构建成为碰撞集，判断所述碰撞集中的车辆与所述最优轨迹之间的空间重合点的数量是否为0，如果是，则选择该最优轨迹作为所述协同轨迹；反之，则判断所述协同轨迹规划车辆以车辆速度最大值匀速行驶的情况下是否在所述空间重合点处也发生时间上的重合，如果是，则放弃该最优轨迹，反之，界定为未发生碰撞，则按照如下规则确定所述协同轨迹：原则一，若存在至少两条所述最优轨迹未发生碰撞时，优先选择耗时较短的所述最优轨迹作为所述协同轨迹；原则二，所有所述最优轨迹均发生碰撞时，则选择所述空间重合点的数量最小的所述最优轨迹作为协同轨迹，并重新进行避免碰撞的速度规划；其中，所述预设要求包括如下需要同时满足的三点要求：第一要求，或，且车辆位于所述协同轨迹规划车辆的左侧；其中，表示所述协同区域内的车辆集合，表示所述接近区域内位于所述虚拟道路中心线上的车辆集合；第二要求，车辆与所述协同轨迹规划车辆存在交叉碰撞、汇入碰撞或追尾碰撞三种碰撞情形之一；第三要求，当所述协同轨迹规划车辆到达开始所述转向点时，而车辆还未驶出所述协同区域。
14.进一步地，所述协同轨迹规划子单元通过最优控制模型（1）进行速度规划：地，所述协同轨迹规划子单元通过最优控制模型（1）进行速度规划：式（1）中，为代价函数，、权重，为第个时间步长，为第个时间步长，、分别为将时间离散化后待协同轨迹规划的车辆协同轨迹规划车辆的加速度、速度；式（2）为车辆的运动学约束，、分别为笛卡尔坐标系下协同轨迹规划车辆的位置坐标、朝向角、加速度；式（3）为车辆边值约束，、、、分别为大地坐标系下协同轨迹规划车辆的刚进入协同区域时的初始位置、驶出协同区域时的最终位置、进入协同区域的起点和终点的位置坐标和朝向角；式（4）是对车辆速度和加速度的最大限制，为车辆加速度最大值；式（5）是车辆的碰撞约束，、分别为大地坐标系下第个空间重合点的坐标、协同轨迹规划车辆在碰撞集中车辆到达第个空间重合点的时刻位置的坐标，为空间重合点的总数，、分别为车辆间的横向和纵向的安全距离。
15.进一步地，所述虚拟道路中心线设置单元的方法具体包括：首先，将接近区域内的两侧道路边界朝相向的方向平移预设距离，然后进行平滑处理，获得两条虚拟道路中心线，在已获得的两条虚拟道路中心线的基础上，再采用相同的平移方法，得到其它的虚拟道路中心线。
16.本发明由于采取以上技术方案，其具有以下优点：1、本发明针对矿区无信号灯的宽阔非结构化交叉路口，利用道路边界生成虚拟道路中心线，对交叉路口附近的车辆进行优先级规定，在接近交叉路口时为其设定目标车道，并进行预先分流处理，通过混合a*算法为不同机动的车辆设计既定路径，最终将车辆之间的距离、通过路口用时、加减速的舒适性等性能指标综合设置成代价函数，对优先级低的辆车进行速度规划达到避免碰撞同时又能提高交叉路口的整体通行效率的目的。
17.2、本发明将非结构化路口进行结构化处理，在提高路口通行效率的同时充分保证了安全性和矿区工作的有序进行。
18.3、本发明的部分优化-部分规则的方法，比纯优化算法计算量小，比纯规则算法通行效率更高。
附图说明
19.图1为本发明实施例提供的aim系统的结构示意图。
20.图2为本发明实施例提供的非结构化场景下的交叉路口多车协同通行的原理图。
21.图3为本发明实施例提供的车辆优先级的关系示意图。
22.图4为本发明实施例提供的碰撞关系分析示意图。
23.具体实施方式
24.在附图中，使用相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面结合附图对本发明的实施例进行详细说明。
25.本文涉及下列术语，为便于理解，对其含义说明如下。本领域技术人员应当理解，下列术语也可能有其它名称，但在不脱离其含义的情形下，其它任何名称都应当被认为与本文所列术语一致。
26.布置aim系统：在交叉路口附近的aim系统及其通信模式。在交叉路口布置一个路侧单元，它能与智能车进行v2i通信（中文全称为“智能车与道路设施之间的通信”，v2i的英文全称为“vehicle-to-infrastructure”）。当然智能车之间也能进行v2v通信（中文全称为“车辆之间的通信”，v2v的英文全称为“vehicle-to-vehicle”）。当智能车进入交叉路口附近后会向路侧单元发送自身坐标以及参与协同规划的申请。路侧单元会根据一定规则为智能车赋予优先级顺序，并按此顺序为智能车规划轨迹。对于每一辆智能车，路侧单元都将做出两次规划。第一次规划是分流轨迹规划，使得原本位置散乱的汽车在进入交叉路口前可以行驶到期望路径上，这样会使得通行更具秩序性，也可以减轻后续协同规划的计算负担。第二次规划是协同轨迹规划，为智能车规划出通过路口的具体轨迹。
27.如图1所示，本发明实施例提供的非结构化场景下的交叉路口多车协同通行方法包括：步骤1，从非结构化场景下的交叉路口的中心开始，由内至外设置协同区域、接近区域和分流区域。
28.其中，交叉路口的中心可以但不限于选取交叉路口的几何中心，“内”相对于“外”，更靠近中心。
29.在一个实施例中，如图2所示，协同区域、接近区域和分流区域的形状可以设置为图中示出的圆形，根据该交叉路口的大小和形状，从小到大依次选取第一预设半径、第二预设半径和第三预设半径，生成三个圆圈，设置协同区域、接近区域和分流区域，其中：协同区域为第一预设半径对应的外边界线包围的内部圆形区域，在本实施例中
可以理解为刚好与交叉路口四个类直角边界相交的圆所包含的区域。
30.接近区域为第一预设半径对应的外边界线外部、且位于第二预设半径对应的外边界线包围的内部所形成的第一圆环区域。第一圆环区域的半径，即第一预设半径与第二预设半径之差主要由交叉路口大小和形状确定，通常可以选择100m左右。
31.分流区域为第二预设半径对应的外边界线外部、且位于第三预设半径对应的外边界线包围的内部所形成的第二圆环区域。第二圆环区域的半径，即第三预设半径与第二预设半径之差主要由交叉路口大小和形状确定，通常可以选择50m以上。
32.当然，也可以根据需要选择方形或其他几何形状的区域，在此不再一一列举。
33.步骤2，在所述接近区域设置虚拟道路中心线，取所述虚拟道路中心线与所述协同区域外边界线的交点作为转向点，从所述转向点开始，在所述虚拟道路中心线上间隔选取分流目标点。
34.至此，完成对交叉路口以及周边区域地图的预处理。虚拟道路中心线、转向点和分流目标点均被加载到高精度地图中，方便智能车加以利用。
35.在一个实施例中，“在接近区域设置虚拟道路中心线”的方法可以包括：如图2所示，本实施例中，图2中展示的是道路宽度约为四车道宽的场景。将接近区域内的两侧道路边界分别相向，即往该道路中心线平移预设距离，该预设距离可以但不限于设置为，其中，为安全距离，为矿卡车辆宽度。优选地，两侧道路边界相向平移后加以平滑处理，最后形成第一虚拟道路中心线，如图2中的虚线所示。
36.进一步地，在已形成的两条第一虚拟道路中心线的基础上，再采用上述类似的平移方法，继续相向平移，得到另外的两条第二虚拟道路中心线。对交汇于这个路口的四条道路都进行相同的处理后可以得到如图2所示的虚拟道路中心线，在更宽阔的道路环境下可能存在更多数量的虚拟道路中心线。需要说明的是，虚拟道路中心线的条数可以根据非结构化道路的宽度与智能体车的宽度之间的关系确定，在非结构化道路的宽度允许的条件下，尽可能多地创建虚拟道路中心线，以增加通过时的机动性，充分利用道路流通能力。
37.将多条虚拟道路中心线形成的集合描述为集合，对应为虚拟道路中心线的索引，表示第条虚拟道路中心线，为虚拟道路中心线的条数。
38.将上的多个分流目标点形成的集合描述为集合，对应为第条虚拟道路中心线上的分流目标点的索引，表示第条虚拟道路中心线上的第个分流目标点，为第条虚拟道路中心线上的分流目标点的个数。
39.还可以采用现有的其他方式在接近区域设置虚拟道路中心线，在此不再详述。
40.在一个实施例中，“取所述虚拟道路中心线与协同区域外边界线的交点作为转向点”的方法具体包括：取虚拟道路中心线与协同区域的外边界线的交点，如图2中点和点所示，转向点为第一虚拟道路中心线与协同区域外边界线的交点，转向点为第二虚拟道路
中心线与协同区域外边界线的交点。
41.在一个实施例中，转向点作为转向时的协同轨迹规划起点或终点。比如：车辆进入协同区域前所行驶虚拟车道中心线与协同区域的外边界线的交点即为该辆车的协同轨迹规划起点。而协同轨迹规划终点的确定，则是根据车辆要执行的未来机动（左转、右转或直行），确定车辆离开交叉路口所通过的路口，所通过的路口的接近区域存在多条行驶虚拟车道中心线，由此有多个转向点可以作为终点进行选择，最终的协同轨迹规划终点是在协同规划阶段确定得到。这部分的详细内容将在后文详细展开描述。
42.在一个实施例中，“在所述虚拟道路中心线上间隔选取分流目标点”的方法可以包括：分流目标点如图2中黑色方形块所示，为路径搜索方法提供协同轨迹规划终点。需要说明的是，间隔的相邻的分流目标点之间的距离不小于矿卡的长度与纵向安全距离之和。
43.步骤3，确定每一区域内部以及不同区域之间的车辆优先级。
44.例如，如图3所示，以交叉路口的中心为起点，建立一根表征车辆优先级的轴，并将车辆投影到轴上，可以以每一车辆到所述中心之间的距离来确定车辆优先级。
45.协同区域内的车辆集合表示第一车辆集合，其中，表示协同区域内第辆车，下文简称为“车辆”。的数值与车辆所对应的的优先级呈反比，也就是说，的数值越大，车辆的优先级越低。
46.接近区域内位于虚拟道路中心线上的车辆集合表示第二车辆集合，其中，表示接近区域内第辆车辆，下文简称为“车辆”。的数值与车辆所对应的的优先级呈反比，也就是说，的数值越大，车辆的优先级越低。
47.接近区域内未处于虚拟道路中心线上的车辆以及分流区域中的车辆集合表示为第三车辆集合，其中，表示接近区域内第辆车，下文简称为“车辆”。的数值与车辆所对应的的优先级呈反比，也就是说，的数值越大，车辆的优先级越低。
48.在另一个实施例中，将每一区域内部的车辆优先级确定为：在同一区域中的车辆，每辆车辆的优先级与该车辆到所述中心之间的距离成反比。
49.将不同区域之间的车辆优先级确定为：所述协同区域内优先级最低的车辆在不同区域的优先级高于所述接近区域内优先级最高的车辆，所述接近区域内优先级最低的车辆在不同区域的优先级高于所述分流区域内优先级最高的车辆。
50.步骤4，按照所述车辆优先级，依次对所述分流区域内车辆进行分流轨迹规
划，使所述车辆在进入所述协同区域前行驶到所述虚拟道路中心线上。
51.在一个实施例中，分流区域中的车辆，由于矿区环境的非结构性特定以及在之前道路上行驶时可能存在的局部绕障行为，其行驶位置是随意散乱的。为是的车辆的位置更加有秩序，减轻中心协同规划器的计算负担，在车辆驶入接近区域之前有必要使其开始逐渐驶向本发明期望的虚拟车道，该虚拟车道即为步骤2中的以“虚拟道路中心线”为象征的车道。路侧规划单元会按照如上实施例中的车辆优先级对第三车辆集合中的车辆依次进行如下规划：步骤41，根据车辆当前所处的位置，从虚拟道路中心线集中就近选择目标车道线，由此也得到对应的分流目标点集合。
52.步骤42，为非结构化场景设立速度上限和速度下限，选择集合中靠近协同区域的分流目标点为协同轨迹规划终点，以作为协同轨迹规划终点时刻的边界速度，以自身坐标为起点进行路径规划。本实施例采用的是混合a*算法，生成了图2中的黑色细线轨迹。当然，也可以采用现有的基于图搜索的算法如迪杰斯特拉、快速搜索随机树等进行路径规划。
53.步骤43，碰撞检测：当检测到与前车分流轨迹存在时空上的重合点时，选择集合中的下一个分流目标点，再次进行规划，直到生成无碰撞的路径，以使车辆行驶到虚拟车道中心线上，至此规划成功。
54.步骤44，由于不希望交叉路口附近发生超车动作，因此每次规划成功时的终点以及它之前的分流目标点都不能再作为下辆车的终点。那么，从分流目标点集合中剔除规划成功的车辆所选定的分流目标点，在该规划成功的车辆到达所选定的目标分流点并往前行驶一段安全距离后，该分流目标点对后续车辆开放，即将重新被添加到集合中，供后续车辆规划时遍历选择。
55.步骤45，对于遍历集合也未能找到合适的分流目标点的车辆，将按照原方向以速度低速行驶，等待新的分流目标点的开放。
56.通过步骤4提供的规划方法能够使得杂乱的智能车辆遵循一定规则行驶，为之后的协同区域规划减轻计算负担，并使得车辆运行具有秩序性。
57.步骤5，到达分流目标点的车辆也就是步骤2中第二车辆集合中的车辆，这些车辆在下文他处也称为“协同轨迹规划车辆”，将沿着虚拟道路中心线继续往前行驶，此时其进入协同区域的起点为其所在虚拟道路中心线的转向点，按照车辆优先级，进行协同区域内的协同轨迹规划。
58.为了提高灵活性，为左转、右转和直行这三种未来机动动规划协同轨迹时都有多种终点可以选择，可供选择的终点数量和对应车道上虚拟道路中心线的条数相等（如图2中的举例所示）。对于协同轨迹规划车辆，经过步骤4的分流后，速度已经达到路口的最大通行
速度。碰撞集中的车辆的车辆优先级都高于协同轨迹规划车辆，所以，车辆的协同轨迹均已规划完成，也就是说，车辆在任意时间点的位置都是已知的。
59.步骤5具体包括：步骤51，根据协同轨迹规划车辆的未来机动，确定其离开所述交叉路口所通过的路口，以该所通过的路口的所有所述转向点作为可选择的协同轨迹规划终点，获得无碰撞的最优轨迹；步骤52，根据协同轨迹规划车辆的未来机动，将所述接近区域内满足预设要求的车辆构建成为碰撞集，判断所述碰撞集中的车辆与所述最优轨迹之间的空间重合点的数量是否为0，如果是，则选择该最优轨迹作为所述协同轨迹；反之，则进入步骤53；步骤53，判断协同轨迹规划车辆以车辆速度最大值匀速行驶的情况下是否在所述空间重合点处也发生时间上的重合，如果是，则放弃该最优轨迹，反之，界定为未发生碰撞，则按照如下规则确定所述协同轨迹：原则一，若存在至少两条所述最优轨迹未发生碰撞时，优先选择耗时较短的所述最优轨迹作为所述协同轨迹。
60.原则二，所有所述最优轨迹均发生碰撞时，则选择所述空间重合点的数量最小的所述最优轨迹作为协同轨迹，并重新进行避免碰撞的速度规划。
61.以图2中标号为
①
的左转车辆为例，车辆
①
即将进入协同区域，为协同轨迹规划车辆。本实施例为协同轨迹规划车辆提供的协同轨迹规划的方法具体包括：步骤51，根据可选择的终点数量规划无碰撞下的最优轨迹。如图2所示，根据第一终点α和第二终点β，以车辆最大速度保持匀速，规划出车辆
①
无碰撞下的第一最优轨迹和第二最优轨迹，完成第一最优轨迹和第二最优轨迹的时间分别为。
62.步骤52，对可能发生碰撞的点，即第一最优轨迹和第二最优轨迹与其碰撞集中车辆轨迹在空间上的重合点，进行碰撞检测。优先选择不发生碰撞的轨迹作为最终轨迹，有多条轨迹未发生碰撞时，优先选择耗时较短的轨迹。
63.例如，在图2中，记车辆
①
为，其碰撞集中有两辆车
②
和
③
，分别记为和。暂不考虑时间，、分别与第一最优轨迹和第二最优轨迹产生空间重合点ⅰ、ⅱ、ⅲ。ⅰ在第一最优轨迹上，ⅱ、ⅲ在第二最优轨迹上。由于目前、和经过空间重合点的时间都是已知的，因此容易进行碰撞检测。经过检测，若第一最优轨迹和第二最优轨迹中只有一条未发生碰撞，则未发生碰撞的最优轨迹作为的协同轨迹；若有第一最优轨迹和第二最优轨迹均未发生碰撞，则选取对应完成时间中较小者所对应的轨迹作为的协同轨迹。
64.步骤53，在两条轨迹都发生碰撞时，则选择空间重合点较少的一条重新进行速度规划以避免碰撞。
65.在另一个实施例中，步骤5还包括：步骤54，如果协同区域内已经规划好轨迹并正在按照规划轨迹行驶的车辆发生故障，向路侧单元和周围一定范围内的车辆发送自己的定位信息。
66.具体地，已经规划好轨迹的车辆会将故障车辆视为临时障碍物，启用避让临时障碍物程序，用内置的在线规划器规划避障轨迹。路侧单元会将故障车辆作为静态障碍物加入地图中。路侧单元对还未规划好轨迹的车辆在规划时也会考虑新增的静态障碍物。
67.对所有第二车辆集合中的车辆按照优先级顺序重复此过程即可得到当前时刻准备通过路口的所有智能车的协同轨迹规划。
68.现有的协同轨迹办法可分为三类：1、基于预约订阅机制，路口每个时间段都只有一辆车通过；2、基于结构化道路上的行车规则或者交通信号灯；3、基于求解最优控制问题的全局车辆最优轨迹规划。前两者只能应用于结构化道路场景，最后一种计算时间很长、行车杂乱无章。而本实施例提供出的协同轨迹规划办法保留较大机动性以适应非结构化场景提高整体同行效率，同时基于一定规则性减小计算量使得通过不至于太过杂乱无章。
69.在一个实施例中，路侧单元与车辆进行信息交互，可以得知每辆车的未来机动，从而判断是否存在潜在的碰撞关系，交叉路口的车辆碰撞存在以下几种碰撞情形：第一碰撞情形，交叉碰撞，如图4中的a在有交点的路径上行驶的两辆车辆之间发生的碰撞。
70.第二碰撞情形，汇入碰撞，如图4中的b所示，汇入同一车道的车辆之间发生的碰撞。
71.第三碰撞情形，追尾碰撞，如图4中的c，在同一车道上行进的车辆在分离之前或者序列前进的途中发生前后车之间的碰撞。
72.对于第三车辆集合中的车辆，由于还未完成分流处理，也未进行轨迹规划，暂时无须考虑以上三种碰撞情况。
73.对于第一车辆集合中的车辆，已经完成好了轨迹规划并正在按照该轨迹行驶，也无须再进行碰撞分析。
74.对于第二车辆集合中的车辆，即协同轨迹规划车辆，必须考虑以上三种碰撞情形，对于第二车辆集合中的车辆，步骤52中的所述预设要求包括如下需要同时满足的三点要求：第一要求，或，且车辆位于所述协同轨迹规划车辆的左侧；其中，表示所述协同区域内的车辆集合，表示所述接近区域内位于所述虚拟道路中心线上的车辆集合；第二要求，车辆与所述协同轨迹规划车辆存在交叉碰撞、汇入碰撞或追尾碰撞三种碰撞情形之一；第三要求，当所述协同轨迹规划车辆到达开始所述转向点时，而车辆还未驶出所述协同区域。
75.在一个实施例中，如图2中的第一最优轨迹和第二最优轨迹上分别有ⅰ一个空间重合点和ⅱ、ⅲ空间重合点，则选取第一最优轨迹所对应的路径重新进行速度规划得到新的可以避免碰撞的第三最优轨迹，步骤53中的“速度规划”的方法具体包括：步骤531，建立最优控制模型（1）：
式（1）中，为代价函数，、权重，例如：取5，取1；为第个时间步长，为第个时间步长，、分别为离散后协同轨迹规划车辆的加速度、速度；式（2）为车辆的运动学约束，、分别为笛卡尔坐标系下协同轨迹规划车辆的位置坐标、朝向角、加速度；式（3）为车辆边值约束，、、、分别为大地坐标系下协同轨迹规划车辆的刚进入协同区域时的初始位置、驶出协同区域时的最终位置、进入协同区域的起点和终点的位置坐标和朝向角；式（4）是对车辆速度和加速度的最大限制，为车辆速度最大值，为车辆加速度最大值；式（5）是车辆的碰撞约束，、分别为大地坐标系下第个空间重合点的坐标、协同轨迹规划车辆在碰撞集中车辆到达第个空间重合点的时刻位置的坐标，为空间重合点的总数，、分别为车辆间的横向和纵向的安全距离。
76.由上述检测结果可知，若协同轨迹规划车辆一直以行驶，在空间重合点处会发生碰撞，碰撞的时间是已知的即为碰撞集中的车辆（优先级更高，轨迹已经确定）到达第个名义碰撞点的时间，因此只需保证时刻，被规划的车辆与既定路径上的所有n个空间重合点在x和y方向都大于安全距离和，即可有效避免碰撞。用内点算法数值或其他求解最优控制问题的计算方法（比如龙格-库塔迭代法）均可，求解此最优控制问题，即可得到上述实施例中车辆
①
的有效轨迹。
77.需要说明的是，对于速度规划，除了可以采用非线性规划问题建模，用数值方法迭代求解，还可以采用整数规划等其他方式建模求解，在此不再一一展开说明。
78.本发明实施例还提供一种非结构化场景下的交叉路口多车协同通行系统，其包括分区单元、优先级确定单元、虚拟道路中心线设置单元、分流规划单元和协同规划单元，其中：分区单元用于从非结构化场景下的交叉路口的中心开始，由内至外设置协同区域、接近区域和分流区域。
79.优先级确定单元其用于确定每一区域内部以及不同区域之间的车辆优先级。
80.虚拟道路中心线设置单元其用于在所述接近区域设置虚拟道路中心线，取所述虚拟道路中心线与所述协同区域外边界线的交点作为转向点，从所述转向点开始，在所述虚拟道路中心线上间隔选取分流目标点。
81.分流规划单元用于用于按照所述车辆优先级，依次对所述分流区域内车辆进行分流轨迹规划，使所述车辆在进入所述协同区域前行驶到所述虚拟道路中心线上。
82.协同规划单元用于将进入所述协同区域内车辆作为等待协同轨迹规划车辆，按照所述车辆优先级进行协同轨迹规划。
83.在一个实施例中，所述协同规划单元具体包括最优轨迹获取子单元和协同轨迹规划子单元，其中：最优轨迹获取子单元用于根据进入所述协同区域内车辆的未来机动，确定其离开所述交叉路口所通过的路口，以该所通过的路口的所有所述转向点作为可选择的协同轨迹规划终点，获得无碰撞的最优轨迹。
84.协同轨迹规划子单元，其用于所述协同轨迹规划车辆的未来机动，将所述接近区域内满足预设要求的车辆构建成为碰撞集，判断所述碰撞集中的车辆与所述最优轨迹之间的空间重合点的数量是否为0，如果是，则选择该最优轨迹作为所述协同轨迹；反之，则判断所述协同轨迹规划车辆以车辆速度最大值匀速行驶的情况下是否在所述空间重合点处也发生时间上的重合，如果是，则放弃该最优轨迹，反之，界定为未发生碰撞，则按照如下规则确定所述协同轨迹：原则一，若存在至少两条所述最优轨迹未发生碰撞时，优先选择耗时较短的所述最优轨迹作为所述协同轨迹；原则二，所有所述最优轨迹均发生碰撞时，则选择所述空间重合点的数量最小的所述最优轨迹作为协同轨迹，并重新进行避免碰撞的速度规划；其中，所述预设要求包括如下需要同时满足的三点要求：第一要求，或，且车辆位于所述协同轨迹规划车辆的左侧；其中，表示所述协同区域内的车辆集合，表示所述接近区域内位于所述虚拟道路中心线上的车辆集合；第二要求，车辆与所述协同轨迹规划车辆存在交叉碰撞、汇入碰撞或追尾碰撞三
种碰撞情形之一；第三要求，当所述协同轨迹规划车辆到达开始所述转向点时，而车辆还未驶出所述协同区域。
85.在一个实施例中，所述协同轨迹规划子单元速度规划方法已经在上文已经说明，在此不再赘述。
86.本发明根据设计的aim系统能对所有接近非机构化无信号灯交叉路口的智能车进行协同轨迹规划。用路侧单元先对车辆进行分流处理，再进行协同规划，计算量较少，同时有效提高通行效率。
87.上述实施例中，多次使用混合a*算法进行路径规划，在生成路径的时候也可以采用其他常规的路径生成办法比如a*算法、rrt算法等。
88.在对本发明方法进行的五十次实验中，每次实验设定的协同车辆数量为24辆，规划时间的长短主要取决于路径规划无法避免碰撞而进一步采用速度规划进行避免碰撞的车的数量。五十次实验中每辆车发生速度规划的概率为29.2%至45.8%。仅仅进行混合a*路径规划便可实现避免碰撞和有效通行的车辆平均规划时间为62.67毫秒。每次进行速度规划的平均时间为24.25毫秒。因此本发明方法对每辆车的平均规划时间为62.67+(29.2%～45.8%)
×
24.25=62.67+(7.08～11.10)=69.75～73.77毫秒不等。
89.对比其他类似的对交叉路口的协同规划办法，如湖南大学李柏采用的连续空间最优控制问题数值求解规划方法中，在24辆车的实验场景下，规划的时间是3.3858秒到3.4572秒不等，平均每辆车的规划时间在141毫秒到144毫秒之间不等。
90.因此本发明在计算时间方面约有50%左右的提升而且规划出来的整体行车路径有更具秩序化的特点。
91.最后需要指出的是：以上实施例仅用以说明本发明的技术方案，而非对其限制。本领域的普通技术人员应当理解：可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。