狭窄道路的轨迹规划方法、装置、电子设备及存储介质与流程

本发明属于汽车轨迹规划，具体涉及到一种狭窄道路的轨迹规划方法、装置、电子设备及存储介质。

背景技术：

1、狭窄道路是自动驾驶中典型的复杂环境，尤其在乡间小道、露天矿山等非结构化场景中广范存在，在道路边界不规则且自由空间严格受限的场景下，如何生成一条安全、平稳的行驶轨迹，是自动驾驶轨迹规划面临的难题。

2、自动驾驶轨迹规划在处理结构化道路/非结构化道路方面存在诸多差异。在坐标系选择方面：非结构化场景中，不存在明显的道路引导线，因此基于frenet坐标系的结构化道路轨迹方法显然不适用，需要在笛卡尔坐标系下处理非结构化场景下轨迹规划问题。在规划算法方面：基于优化方法的轨迹规划算法在较高质量的原始引导线加持下，可以在结构化道路场景中取得了较好的效果；但非结构化道路通常没有引导线，这也意味着优化算法通常没有初解，造成后续规划问题无法求解；同时基于优化方法的轨迹规划严重依赖初解质量；当初解不可行时，往往会造成求解失败，这也要求给定的初始解必须是可行解；尤其当道路场景为非结构化狭窄道路时，初解的优劣直接影响是否能否求解成功。

3、因此，在非结构化场景的自动驾驶过程中，需要一种能规划出一条安全、平滑路径的轨迹规划方法。

技术实现思路

1、针对上述问题，本发明的主要目的在于设计一种面向非结构化狭窄道路的b样条轨迹规划方法、装置、电子设备及存储介质，首先对狭窄环境道路进行建模，然后使用混合a*的离散化搜索结果作为轨迹规划的初解，并采用基于b样条曲线的优化方法构造轨迹规划问题，解决狭窄道路行驶过程中不易生成安全、平稳行驶轨迹的技术问题。

2、为了实现上述目的本发明采用如下技术方案：

3、一种面向非结构化狭窄道路的b样条轨迹规划方法，该方法包括如下步骤：

4、步骤1：采用分段边界拟合的方式，针对狭窄道路环境进行建模，得到道路边线集合；

5、步骤2：使用混合a*算法搜索得到初始轨迹，并进行可行解计算，得到初始可行轨迹点的集合；

6、步骤3：将初始可行轨迹点作为控制点，构建b样条曲线，并得到每个轨迹点的状态；

7、步骤4：以车体后轴中心为基准，构建道路起点和终点的限制条件；

8、步骤5：建立目标函数；

9、步骤6：将限制条件和目标函数放入优化求解器求解，得到优化后的b样条曲线，在优化后的b样条曲线上采样，得到采样点组成的轨迹即为车体后轴中心的轨迹。

10、作为本发明的进一步描述，步骤1中，狭窄道路环境建模的步骤如下：

11、步骤1.1：若当前边界线段集合为空，则取边界点集p中前2个点pi(i＝0,1)，形成直线lj(j＝0)，并计算直线的朝向theta；

12、步骤1.2：取边界点集p中下一个点pi+1，求解pi+1与步骤1.1中直线lj末端点形成的直线朝向theta_new；

13、步骤1.3：若abs(theta-thetanew)＜theta_grid，则认为pi+1属于直线lj，可以用同一直线参数表示；否则，将以直线lj末端点及pi+1形成新的直线lj+1；

14、其中，theta_grid为设定的阈值；

15、步骤1.4：重复步骤1.2和步骤1.3，直至遍历完边界点集p，形成左边界直线集合bound_left、右边界直线集合bound_right，其中第i段边线的表达式为：aix+biy+ci＝0，i＝1...n；

16、作为本发明的进一步描述，将道路边线进行栅格化处理，每个栅格代表离散化的节点，各个节点间存在拓扑关联，所有栅格最终组成离散化地图，供步骤2中混合a*算法搜索使用；

17、步骤2中，混合a*算法中的代价函数为：

18、f(x)＝g(x)+h(x)

19、其中，g(x)表示起点到当前点的代价，h(x)表示当前点到终点的代价；

20、在h(x)较大或者较小时，对代价函数进行调整，即：

21、f(x)＝g(x)+nb*h(x)

22、其中，nb表示当前拓展节点附近的可行性区域节点数；

23、当nb较大时，表示当前节点处在开阔区域，此时，增加h(x)权重；当nb较小时，表示当前为狭窄空间，此时，需要更多的拓展节点；

24、基于上述算法，得到一系列节点组成的离散化序列，即初始可行解集合pci，即初始可行轨迹点的集合，其中，i＝1，2…n，n为初始轨迹点数目。

25、作为本发明的进一步描述，步骤3中，构建b样条曲线的方式包括以下步骤：

26、步骤3.1：将初始可行轨迹点pc作为控制点，构建b样条曲线，表示为：

27、

28、其中，u表示节点向量，pci表示第i个控制点，ni,k(u)表示在u时的i各控制点对应的k阶基函数系数；

29、步骤3.2：在b样条曲线上均匀采样，得到采样点集以及对应的累计长度lengthi，i＝1，2…num；每个状态包含当前点的x、y、theta。

30、作为本发明的进一步描述，步骤4中，起点和终点的限制条件构建方式包括如下步骤：

31、步骤4.1：计算左边界直线集合bound_left和右边界直线集合bound_right中每个线段在b样条曲线的投影，得到每条线段b样条长度s方向的投影范围start_s、end_s，即s值，并保存在直线段上；

32、步骤4.2：根据当前轨迹点的s值，取bound_left、bound_right位于s值前后的直线段；

33、步骤4.3：将车体后轴中心放置在每个轨迹点，得到行驶到轨迹点处的车体轮廓，并计算车体轮廓与步骤4.2中直线段的最小距离，作为该轨迹点的左右条件约束b_l、b_r；

34、步骤4.4：针对每个轨迹点均进行计算，得到所有待优化轨迹点的左右边界条件约束b_ls、b_rs；

35、步骤4.5：起点和终点的限制条件为：起点的限制条件为p0＝[x0,y0,theta0]，终点的限制条件为pend＝[xend,yend,thetaend]。

36、作为本发明的进一步描述，步骤5中，设置代价函数cost＝w1∫bs′2+w2∫bs″2+w3∫bs″′2；

37、其中，bs′为b样条曲线的一阶导数，bs″为b样条曲线的二阶导数，bs″′为b样条曲线的三阶导数，w1为b样条曲线一阶导数对应的权重系数，w2为b样条曲线二阶导数对应的权重系数，w3为b样条曲线三阶导数对应的权重系数。

38、作为本发明的进一步描述，步骤6中，将步骤4的限制条件和步骤5的代价函数代入求解器，得到优化后的b样条曲线函数

39、在优化后的b样条曲线上按照等间隔进行采样，得到一系列的位姿点i＝1，2…n，所有位姿点组成的轨迹即为车体行驶的轨迹。

40、一种用于狭窄道路的车辆轨迹规划装置，所述装置包括获取模块和生成模块；

41、所述的获取模块，用于获取狭窄道路的边界，以及环境信息；

42、所述的生成模块，用于得到初始轨迹，并构建b样条曲线、构建道路起点和终点的限制条件、建立目标函数，进而得到优化后的b样条曲线，并在优化后的b样条曲线上进行采样，得到车体行驶的轨迹。

43、一种电子设备，包括处理器、通信接口、存储器和通信总线，其中，所述处理器、所述通信接口和所述存储器通过所述通信总线完成相互间的通信，所述存储器，用于存储计算机程序；

44、所述处理器，用于通过运行所述存储器上所存储的所述计算机程序，执行上述轨迹规划方法的步骤。

45、一种计算机可读的存储介质，所述存储介质中存储有计算机程序，其中，所述计算机程序被处理器执行时实现上述轨迹规划方法的步骤。

46、相对于现有技术，本发明的技术效果为：

47、本发明提供了一种面向非结构化狭窄道路的b样条的轨迹规划方法、装置、电子设备及存储介质，使用多段线性方程描述狭窄道路的边界，避免逐点遍历道路边界带来的计算困难，提升计算效率，同时，使用混合a*搜索得到初始轨迹，并根据周边自由空间的大小，动态调整启发项权重，在保证最大程度搜索到可行性解的同时，提高算法收敛速度，另外，本发明以可行性解为输入，结合车辆运动学模型为约束，建立轨迹优化求解模型，得到安全、平滑的最优路径，有效的提高优化求解成功率及效率。