一种用于自动驾驶避障路径规划的空间安全走廊生成方法及系统与流程

摘要: 本技术涉及自动驾驶,特别是涉及一种用于自动驾驶避障路径规划的空间安全走廊生成方法及系统。背景技术:1、自动驾驶车辆的避障路径规划需要根据道路边界和障碍物分布,确...
一种用于自动驾驶避障路径规划的空间安全走廊生成方法及系统与流程

本技术涉及自动驾驶,特别是涉及一种用于自动驾驶避障路径规划的空间安全走廊生成方法及系统。

背景技术:

1、自动驾驶车辆的避障路径规划需要根据道路边界和障碍物分布,确定满足安全通行要求的行驶路径。空间安全走廊用于描述路径附近允许车辆通行的区域,并为路径平滑或优化提供空间边界约束,是连接环境信息与路径优化的重要环节。在露天矿区等非结构化道路环境中,道路宽度、边界形态及障碍物分布具有较大差异,车辆需要根据局部环境及时确定可通行范围,以适应绕行、会车及弯道行驶等作业需求。

2、现有空间安全走廊生成方法通常以初始路径上的离散点为基础,通过逐点扩展矩形等局部区域,并结合道路边界和障碍物的碰撞检测结果确定各区域的范围,再将这些区域用于约束后续路径优化。然而,相邻路径点附近的待检测区域往往存在重叠,逐点扩展过程中容易对相同或相近的环境区域重复执行碰撞检测和边界判断,计算开销随路径点数量及扩展次数增加而增大。在边界曲折、道路狭窄或障碍物分布复杂的场景下,局部区域的扩展还需要频繁判断其是否满足安全通行条件,进一步增加安全边界确定的耗时,影响后续路径优化及在线规划的响应速度。

3、因此,在自动驾驶避障路径规划的空间安全走廊生成过程中,局部区域扩展存在重复碰撞检测、安全边界确定效率较低以及复杂道路场景下在线生成实时性不足,成为亟需解决的问题。

技术实现思路

1、本技术提供一种用于自动驾驶避障路径规划的空间安全走廊生成方法及系统,旨在解决现有技术在自动驾驶避障路径规划的空间安全走廊生成过程中,局部区域扩展存在重复碰撞检测、安全边界确定效率较低以及复杂道路场景下在线生成实时性不足的问题。

2、第一方面,一种用于自动驾驶避障路径规划的空间安全走廊生成方法,所述方法包括:

3、根据道路边界和静态障碍物信息,得到障碍物距离地图;

4、根据待优化路径,构建覆盖待查询路径点所在区域的基础方格集合;

5、根据所述障碍物距离地图中各基础方格角点所在地图栅格单元的四个角点距离值进行双线性插值,得到各基础方格角点的障碍物距离;

6、根据所述障碍物距离和安全距离阈值,确定各基础方格的角点碰撞信息,并根据所述角点碰撞信息及角点与基础方格之间的对应关系,得到方格有效信息;

7、根据方格索引和角点索引,将所述角点碰撞信息与对应基础方格的方格有效信息关联存储,得到预存碰撞数据,其中,相邻基础方格的同一空间角点共用角点碰撞记录,所述预存碰撞数据供同一规划周期内不同待查询路径点共用;

8、根据待查询路径点在所述基础方格集合中的位置,按照预设方向顺序进行矩形区域扩展,并按所述方格索引和角点索引读取新增扩展区域对应的所述预存碰撞数据,判断所述新增扩展区域的有效性,得到所述待查询路径点对应的安全边界;

9、根据所述安全边界,得到所述待查询路径点对应的矩形安全走廊及用于约束所述待优化路径的安全边界约束。

10、上述方案中,可选地,所述根据道路边界和静态障碍物信息,得到障碍物距离地图,包括:

11、根据所述道路边界和静态障碍物信息进行栅格化处理,得到包含障碍物占用信息的二维栅格地图;

12、根据所述二维栅格地图,沿第一索引方向进行一维欧式距离转换,得到第一距离转换结果;

13、根据所述第一距离转换结果,沿与所述第一索引方向正交的第二索引方向进行距离转换,得到各栅格至最近障碍物的二维距离转换结果;

14、根据所述二维距离转换结果和栅格分辨率,得到用于查询空间位置与最近障碍物之间距离的所述障碍物距离地图。

15、上述方案中,可选地,所述角点碰撞信息和方格有效信息通过以下步骤得到:

16、根据基础方格中待查询角点的空间位置和所述障碍物距离地图的栅格分辨率,确定所述待查询角点所在的地图栅格单元,得到所述地图栅格单元的四个角点位置及对应距离值;

17、根据所述待查询角点相对于所述地图栅格单元的位置,得到两个坐标方向上的归一化偏移量;

18、根据所述归一化偏移量和所述四个角点对应的距离值进行双线性插值,得到所述待查询角点的障碍物距离;

19、根据所述待查询角点的障碍物距离与所述安全距离阈值的比较结果,得到各基础方格的角点碰撞信息;

20、根据所述角点碰撞信息及角点与基础方格之间的对应关系,进行基础方格的有效性判定,得到所述方格有效信息。

21、上述方案中,可选地,所述根据待优化路径,构建覆盖待查询路径点所在区域的基础方格集合,包括:

22、根据车辆当前位置与所述待优化路径之间的位置关系,确定所述待优化路径上距离车辆当前位置最近的路径点,得到方格构建起点;

23、根据所述方格构建起点和预设方格尺寸,沿全局坐标系两个坐标轴的正、负方向进行方格扩展,得到覆盖待查询路径点所在区域的基础方格;

24、根据各基础方格的空间位置及其四个角点的位置,建立基础方格与角点之间的位置对应关系,得到所述基础方格集合。

25、上述方案中,可选地,所述预存碰撞数据通过以下步骤得到:

26、根据所述基础方格集合中基础方格与角点之间的位置对应关系,分别设置方格索引和角点索引,得到方格与角点之间的索引对应关系;

27、根据所述角点索引和角点碰撞信息建立角点信息表,得到按角点索引查询的角点碰撞记录;

28、根据所述方格索引、对应的角点索引和方格有效信息建立方格信息表,得到按方格索引查询的方格有效记录;

29、根据所述索引对应关系,将所述角点碰撞记录和方格有效记录关联,得到所述预存碰撞数据。

30、上述方案中,可选地,所述安全边界通过以下步骤得到:

31、根据所述待查询路径点的位置及基础方格的空间范围,确定对应的查询起始方格,得到包含所述待查询路径点的初始矩形区域;

32、根据所述初始矩形区域,按照逆时针顺序依次确定扩展方向,得到每次扩展对应的待检测区域;

33、根据所述待检测区域覆盖的基础方格及对应角点,查询所述预存碰撞数据,得到所述待检测区域的有效性判断结果;

34、根据所述有效性判断结果,在所述待检测区域有效时将其纳入当前矩形区域,在所述待检测区域无效时保留当前矩形区域,得到更新后的矩形区域;

35、根据所述更新后的矩形区域继续进行扩展及有效性判断,在全局坐标系两个坐标轴的正、负方向均不能继续有效扩展时,得到最终矩形区域,并将所述最终矩形区域的边界作为所述安全边界。

36、上述方案中,可选地,所述根据所述安全边界,得到所述待查询路径点对应的矩形安全走廊及用于约束所述待优化路径的安全边界约束,包括:

37、根据所述安全边界在全局坐标系中的位置,得到所述待查询路径点对应的第一坐标下限、第一坐标上限、第二坐标下限及第二坐标上限;

38、根据所述第一坐标下限、第一坐标上限、第二坐标下限及第二坐标上限,确定四条直线边界围成的矩形区域,得到所述矩形安全走廊;

39、根据所述待优化路径中对应路径点的坐标变量和所述第一坐标下限、第一坐标上限、第二坐标下限及第二坐标上限,分别构建两个坐标方向上的上下限不等式,得到所述安全边界约束。

40、上述方案中,可选地,所述方法还包括:

41、根据所述待优化路径中的各路径点,分别生成对应的矩形安全走廊及安全边界约束,得到与各路径点一一对应的安全边界约束集合;

42、根据所述待优化路径拟合参考曲线,确定各路径点对应的参考位置,得到参考位置集合;

43、根据所述待优化路径的相邻路径点距离、各路径点相对于所述参考位置集合中对应参考位置的偏移以及连续路径点的平滑程度,分别得到路径长度成本、参考偏移成本及路径平滑度成本;

44、根据所述路径长度成本、参考偏移成本及路径平滑度成本进行加权组合,得到路径优化目标函数;

45、根据当前迭代路径的连续路径点及车辆最小转弯半径,构建曲率约束并进行一阶线性化,得到线性化曲率约束,其中,初始迭代路径为所述待优化路径;

46、根据所述路径优化目标函数、安全边界约束集合和线性化曲率约束进行序列二次规划求解,并随路径更新重新确定线性化曲率约束,得到各路径点满足其对应安全边界约束的平滑路径。

47、上述方案中,可选地,所述待优化路径通过以下步骤获得:

48、根据参考路径建立路径坐标系,并根据车辆速度和道路宽度进行纵向及横向采样,得到分层路径节点;

49、根据相邻层路径节点之间的连接关系进行曲线插值,并根据所述障碍物距离地图对路径节点及插值曲线进行安全距离筛选,得到由有效节点和有效图边构成的道路图;

50、根据所述有效图边的横向偏移和障碍物距离确定图边代价,并沿所述道路图逐层递推,得到各有效节点的最小累积代价和前驱节点;

51、根据末层有效节点的最小累积代价确定目标节点,并沿所述前驱节点回溯,得到作为所述待优化路径的无碰撞粗糙路径。

52、第二方面,一种用于自动驾驶避障路径规划的空间安全走廊生成系统,所述系统包括:

53、距离地图生成模块,用于根据道路边界和静态障碍物信息,得到障碍物距离地图;

54、基础方格构建模块,用于根据待优化路径,构建覆盖待查询路径点所在区域的基础方格集合;

55、碰撞信息确定模块,用于根据所述障碍物距离地图中各基础方格角点所在地图栅格单元的四个角点距离值进行双线性插值,得到各基础方格角点的障碍物距离,并根据所述障碍物距离和安全距离阈值,确定各基础方格的角点碰撞信息,并根据所述角点碰撞信息及角点与基础方格之间的对应关系,得到方格有效信息;

56、碰撞数据存储模块,用于根据方格索引和角点索引,将所述角点碰撞信息与对应基础方格的方格有效信息关联存储,得到预存碰撞数据,其中,相邻基础方格的同一空间角点共用角点碰撞记录,所述预存碰撞数据供同一规划周期内不同待查询路径点共用;

57、安全边界确定模块,用于根据待查询路径点在所述基础方格集合中的位置,按照预设方向顺序进行矩形区域扩展,并按所述方格索引和角点索引读取新增扩展区域对应的所述预存碰撞数据,判断所述新增扩展区域的有效性,得到所述待查询路径点对应的安全边界;

58、安全走廊生成模块,用于根据所述安全边界,得到所述待查询路径点对应的矩形安全走廊及用于约束所述待优化路径的安全边界约束。

59、相比现有技术,本技术至少具有以下有益效果:

60、本技术基于对现有技术问题的进一步分析和研究,认识到在自动驾驶避障路径规划的空间安全走廊生成过程中,局部区域扩展存在重复碰撞检测、安全边界确定效率较低以及复杂道路场景下在线生成实时性不足的问题,通过将道路边界和静态障碍物信息转化为障碍物距离地图,为安全距离判断提供统一的距离查询依据,并根据待优化路径构建覆盖待查询路径点所在区域的基础方格集合,使碰撞信息的确定与后续安全边界查询采用相对应的空间划分;根据地图栅格单元的四个角点距离值进行双线性插值,得到基础方格角点的障碍物距离,并与安全距离阈值比较确定角点碰撞信息,再利用角点与基础方格之间的对应关系得到方格有效信息,并将两类信息按方格索引和角点索引关联存储,使相邻基础方格的同一空间角点共用角点碰撞记录,使后续矩形区域扩展能够在同一规划周期内通过查询预存碰撞数据复用已有判断结果,减少不同路径点查询及多次区域扩展过程中对相同空间位置的重复距离查询和碰撞计算;在此基础上,按照预设方向顺序进行矩形区域扩展,并依据扩展区域的有效性确定安全边界,使安全边界的生成与道路边界、静态障碍物及安全距离阈值所对应的判定结果保持一致,进而形成与待查询路径点对应的矩形安全走廊及安全边界约束,将环境避障要求转化为能够用于约束待优化路径的空间边界,从而降低安全走廊生成过程中重复碰撞检测的计算开销,提高安全边界查询和走廊生成效率,并简化后续路径优化中安全约束的构建过程。