基于势场引导的多车路径分层规划方法及装置

本发明属于智能网联汽车，具体涉及一种基于势场引导的多车路径分层规划方法及装置。

背景技术：

1、随着自动驾驶、智能交通及车路云协同技术的发展，多车辆在共享道路网络中的协同运动问题逐渐成为研究热点，在车路云协同自动驾驶场景中，现有基于冲突搜索（conflict-based search, cbs）的多车路径规划算法存在高层与低层之间通信机制单一、冲突判定不考虑车辆运动学约束、冲突检测计算冗余大、搜索起点质量差等结构性缺陷，导致在多车密集场景下搜索效率低下、规划耗时长、容易产生死锁或求解失败。

技术实现思路

1、有鉴于此，本发明提供了一种基于势场引导的多车路径分层规划方法及装置，旨在解决车路云协同自动驾驶场景中现有cbs算法存在的高层与低层通信机制单一、冲突判定不考虑车辆运动学约束、冲突检测计算冗余大、搜索起点质量差等问题，以提升多车密集场景下的搜索效率、减少规划耗时并避免死锁或求解失败情况。

2、根据本发明的第一方面，提供了一种基于势场引导的多车路径分层规划方法，应用于多车路径分层规划系统，该方法包括：

3、初始化环境地图，获取待规划车辆的起点与目标点，并执行优先级-势场混合热身，初始化势场通道，生成cbs根节点，并将根节点加入最小堆中；

4、从最小堆中筛选出优先级键值最小的高层节点，并对节点中的各车执行预剪枝，得到候选车辆对集合；

5、对候选车辆对集合中的每一车辆对进行轨迹扫描，根据扫描结果判断车辆间轨迹是否存在冲突；

6、针对判断结果中轨迹存在冲突的车辆分别进行判定，若判定冲突类型为硬冲突，则将硬冲突加入消解队列，以便基于双通道时空混合a*算法消解冲突并输出新的cbs子节点；

7、将新的cbs子节点加入最小堆，并从最小堆中筛选出优先级键值最小的高层节点；重复执行以上步骤，直至最小堆为空或已展开节点数达到预设上限，输出无冲突的多车的时空轨迹。

8、在第一方面的一些可实施方式中，执行优先级-势场混合热身，初始化势场通道，生成cbs根节点，包括：

9、按照车辆标识信息对所有待规划车辆进行升序排序；

10、按照排序后的顺序对位于序列前置位的车辆进行路径规划，并使后续待规划车辆依次通过预设的连续势场软约束通道感知序列前置位所有已完成规划的车辆的完整时空轨迹，从而主动进行路径规避；

11、待序列中所有车辆已完成规划后，生成规划结果，并将规划结果作为cbs算法的根节点。

12、在第一方面的一些可实施方式中，对节点中的各车执行预剪枝，得到候选车辆对集合：

13、提取节点中每一车辆的路径中所包含的车道标识集合；

14、检查车道拓扑图中车道标识集合之间是否存在长度≤预设值的车辆对，若存在，则将其纳入候选车辆对集合；若不存在，则判断节点已无冲突，直接输出堆中多车的时空轨迹。

15、在第一方面的一些可实施方式中，对候选车辆对集合中的每一车辆对进行轨迹扫描，根据扫描结果判断车辆间轨迹是否存在冲突，包括：

16、对于候选车辆对集合中的每一组车辆对，按照预设时间步长遍历所有采样时刻，并依次执行基于二分查找的时间插值运算和基于sat的矩形足迹碰撞检测；若经检测所有候选车辆对在全部时间步长内均未出现矩形重叠，则判定候选车辆对集合不存在冲突；若检测到矩形足迹重叠，则判定对应车辆对存在冲突。

17、在第一方面的一些可实施方式中，针对判断结果中轨迹存在冲突的车辆分别进行判定，包括：

18、基于双方的最小转弯半径和最大减速度计算最短逃逸时间下界，判断在冲突时间窗口内是否有一方自行避让，若是则标记为软冲突；若双方均无法自行避让，则标记为硬冲突。

19、在第一方面的一些可实施方式中，若判定冲突类型为硬冲突，则将硬冲突加入消解队列，以便基于双通道时空混合a*算法消解冲突并输出新的cbs子节点，包括：

20、若判定冲突类型为硬冲突，则针对冲突双方分别创建对应的新的cbs子节点，对新的cbs子节点添加cbs离散时空硬约束，并调用低层双通道时空混合a*算法，以连续势场软约束作为全局启发函数、以离散时空硬约束作为搜索边界，进行路径重规划，若重规划成功则输出新的cbs子节点；其中，新的cbs子节点继承父节点的路径及连续势场软约束信息。

21、在第一方面的一些可实施方式中，该方法还包括：

22、若重规划失败则舍弃当前新的cbs子节点，重新创建下一新的cbs子节点并对新的cbs子节点添加新的cbs离散时空硬约束，调用低层双通道时空混合a*算法进行路径重规划直至已展开节点数达到预设上限。

23、在第一方面的一些可实施方式中，该方法还包括：

24、若判断冲突类型为软冲突，则由连续势场软约束引导其自然消解。

25、在第一方面的一些可实施方式中，所有cbs节点间的信息流动模式为：高层节点同时向低层子节点传递连续势场软约束信息与离散时空硬约束信息；其中，连续势场软约束信息可在多个cbs子节点之间继承和动态更新。

26、根据本发明的第二方面，提供了一种基于势场引导的多车路径分层规划装置，应用于多车路径分层规划系统，该装置包括：

27、第一处理模块，用于初始化环境地图，获取待规划车辆的起点与目标点，并执行优先级-势场混合热身，初始化势场通道，生成cbs根节点，并将根节点加入最小堆中；

28、第二处理模块，用于从最小堆中筛选出优先级键值最小的高层节点，并对节点中的各车执行预剪枝，得到候选车辆对集合；

29、第三处理模块，用于对候选车辆对集合中的每一车辆对进行轨迹扫描，根据扫描结果判断车辆间轨迹是否存在冲突；

30、第四处理模块，用于针对判断结果中轨迹存在冲突的车辆分别进行判定，若判定冲突类型为硬冲突，则将硬冲突加入消解队列，以便基于双通道时空混合a*算法消解冲突并输出新的cbs子节点；

31、第五处理模块，用于将新的cbs子节点加入最小堆，并从最小堆中筛选出优先级键值最小的高层节点；重复执行以上步骤，直至最小堆为空或已展开节点数达到预设上限，输出无冲突的多车的时空轨迹。

32、在本发明中，通过建立软硬混合双通道约束架构，辅以优先级-势场混合热身、预剪枝及冲突分级剪枝，有效克服了现有cbs算法存在的高层与低层通信机制单一、冲突判定不考虑车辆运动学约束、冲突检测计算冗余大、搜索起点质量差等问题，提升了多车密集场景下的搜索效率、减少规划耗时并避免死锁或求解失败情况。

33、以下将结合附图及具体实施例对本发明做进一步详细说明。

1.一种基于势场引导的多车路径分层规划方法，应用于多车路径分层规划系统，其特征在于，所述方法包括：

2.根据权利要求1所述的方法，其特征在于，所述执行优先级-势场混合热身，初始化势场通道，生成cbs根节点，包括：

3.根据权利要求1所述的方法，其特征在于，所述对所述节点中的各车执行预剪枝，得到候选车辆对集合：

4.根据权利要求1所述的方法，其特征在于，所述对所述候选车辆对集合中的每一车辆对进行轨迹扫描，根据扫描结果判断车辆间轨迹是否存在冲突，包括：

5.根据权利要求1所述的方法，其特征在于，所述针对判断结果中轨迹存在冲突的车辆分别进行判定，包括：

6.根据权利要求1或5中任一项所述的方法，其特征在于，所述若判定冲突类型为硬冲突，则将硬冲突加入消解队列，以便基于双通道时空混合a*算法消解冲突并输出新的cbs子节点，包括：

7.根据权利要求6所述的方法，其特征在于，所述方法还包括：

8.根据权利要求5所述的方法，其特征在于，所述方法还包括：

9.根据权利要求1-5或7-8中任一项所述的方法，其特征在于，所有cbs节点间的信息流动模式为：高层节点同时向低层子节点传递连续势场软约束信息与离散时空硬约束信息；其中，连续势场软约束信息可在多个cbs子节点之间继承和动态更新。

10.一种基于势场引导的多车路径分层规划装置，应用于多车路径分层规划系统，其特征在于，所述装置包括：