本发明涉及路径规划，尤其涉及一种全局与局部融合路径规划方法、装置及存储介质。
背景技术：
1、路径规划问题在无人驾驶技术中占据着重要地位。路径规划问题指在有障碍物的环境中，在满足距离、计算时间、通信延迟和能量消耗等优化条件的前提下，寻找从车辆初始位置到期望位置的最优路径。
2、目前，a*算法是常用的全局路径规划算法。全局路径规划通常是面向目的地，根据已知环境，要求找到连接起点和终点的完整路径。但是传统的方法存在实时性差和路径转折点较多的问题，在实际环境中难以避免出现未知障碍物，要仍能顺利到达终点，则需要涉及局部路径规划。对于局部路径规划，要想达到舒适度、驾驶性、安全性和能耗的综合，往往需要基于最优控制的方法以最优控制理论为基础，这类方法可以考虑车辆的运动学和动力学特性，但计算复杂度较高，初始解质量差时，收敛慢，难以实时应用于自动驾驶系统。基于人工智能的方法如遗传算法、粒子群优化算法、深度学习等，这类方法在处理复杂道路环境和动态交通条件方面具有较好的适应性，但需要大量的训练数据和计算资源。
技术实现思路
1、为了解决上述技术问题或者至少部分地解决上述技术问题，本发明提供一种全局与局部融合路径规划方法、装置及存储介质。
2、第一方面，本发明提供一种全局与局部融合路径规划方法，包括：
3、利用改进双向a*搜索算法获取全局起点和全局目标点之间的路点；其中，所述改进双向a*搜索算法的总估价函数为：
4、f(n)＝g(n)+h(n)+t(n)，其中，g(n)＝dist(n0，n)为全局起点到节点n之间的欧几里得距离估价函数，h(n)＝dist(n，nend)为节点n到全局目标点之间的欧几里得距离估价函数，t(n)为车辆航向角波动代价函数，a为系数，θn表示节点n处航向角，θn_pre表示节点n的父节点处航向角；
5、对所得到的路点采用贝塞尔曲线拟合进行优化平滑生成平滑的全局路径；
6、在全局路径的基础上进行局部指引轨迹和规划凸空间的生成，其中，进行局部指引轨迹生成时，截取当前车辆所在全局路径的局部路径并将其转换为frenet坐标系，将车辆和障碍物映射到该frenet坐标系上，并使用五次多项式曲线进行局部状态空间采样，设定不同采样点间的状态转移代价函数，对采样结果以动态规划的方式进行评估，筛选出采样点间的最优路径作为局部指引轨迹；在局部指引轨迹的基础上生成规划凸空间；
7、基于迭代lqr算法的轨迹规划算法将规划涉及到不等式约束转化为惩罚加入目标函数，并在规划凸空间中进行轨迹规划的优化。
8、更进一步的，所述改进双向a*搜索算法包括：
9、将地图栅格化得到栅格地图，栅格地图将连续的空间划分成离散的表示可通过或不可通过的单元格节点；于栅格地图上设置全局起点和全局目标点；
10、将全局起点设置为正向遍历的当前节点n1，并加入到第一open列表，从格栅地图搜索当前节点n1周围可到达的相邻节点x，将相邻节点x加入到第一open列表，将当前节点n1设置为新加入节点的父节点，将当前节点n1从第一open列表中删除，并加入到第一close列表；
11、将全局目标点设置为反向遍历的当前节点n2，并加入到第二open列表，从格栅地图搜索当前节点n2周围可到达的相邻节点y，将相邻节点y加入到第二open列表，将当前节点n2设置为新加入节点的父节点，将当前节点n2从第二open列表中删除，并加入到第二close列表；
12、重复执行：
13、遍历第一open列表，找到总估价函数最小的节点将其作为当前节点n1，将当前节点n1从第一open列表中删除加入第一close列表中；
14、检查更新后的当前节点n1周围可到达的相邻节点x，若找到的相邻节点x不在第一open列表中则加入第一open列表，并且将当前节点n1设置新加入节点的父节点，若找到的可到达的相邻节点x原先在第一open列表中，取相邻节点到当前节点n1的欧几里得距离，若相邻节点x到当前节点n1的欧几里得距离小于原值，则以当前节点n1为父节点，第一open列表更新后，更新总估价函数和相邻节点到当前节点n1的欧几里得距离；
15、遍历第二open列表，找到总估价函数最小的节点将其作为当前节点n2，将当前节点n2从第二open列表中删除加入第二close列表中；
16、检查更新后的当前节点n2周围可到达的相邻节点y，若找到的相邻节点y不在第二open列表中则加入第二open列表，并且将当前节点n1设置新加入节点的父节点，若可到达的相邻节点y原先在第一open列表中，取相邻节点到当前节点n2的欧几里得距离，若相邻节点y到当前节点n2的欧几里得距离小于原值，则以当前节点n2为父节点，第二open列表更新后，更新总估价函数和相邻节点到当前节点n2的欧几里得距离；
17、终止：
18、当第一open列表中包含第二close列表中的点或第二open列表中包含第一close列表中的点时，表示完整路径的路点找到，当第一open列表或第二open列表为空时，获取全局起点和全局目标点之间路径失败。
19、更进一步的，截取车辆前方全局路径的l1米，车辆后方全局路径的l2米两部分作为局部路径；若车辆后方全局路径不够l2米，则向前多截取一部分全局路径作为补偿，保证局部路径的长度保持固定长度，同样，若车辆前方的全局路径不足l1米，将向后多截取一部分全局路径作为补偿，保证局部路径的长度保持固定长度。
20、更进一步的，使用五次多项式进行局部状态空间采样，设定不同采样点间的状态转移代价函数，对采样结果以动态规划的方式进行评估，筛选出采样点间的最优路径作为局部指引轨迹包括：
21、设置规划起点；
22、在确定参考线和规划起点后，进行横纵向的状态点采样；
23、设置状态点之间进行转移动作和转移代价函数，其中转移动作采用五次多项式曲线去连接相邻纵向步长的采样点；
24、采用动态规划的方法计算出当前采样起始点至采样终点的最小代价，并将其所在五次多项式曲线视作当前离散空间内的最优路径，作为局部指引轨迹。
25、更进一步的，进行横纵向的状态点采样时，横向的采样步长为1m，纵向的采样步长为12m，横向采样步数为9，纵向采样步数为10。
26、更进一步的，两个相邻步长采样点之间的转移代价考虑曲线的平滑代价、与障碍物之间的距离代价、与参考线之间的距离代价，转移代价表示为：
27、fcost＝ω1σf'(si)2+ω2∑f”(si)2+ω3∑f”'(si)2+σg[d]+∑li2；
28、其中，ω1、ω2、ω2为相邻步长采样点之间一阶、二阶和三阶状态导数的平滑系数，g[d]是障碍物距离代价函数，取值方式为：
29、
30、d是车辆与障碍物的距离，d1是车辆与障碍物的安全距离，d2是车辆与障碍物的极限安全距离，li表示五次多项式曲线与参考线之间的距离。
31、更进一步的，动态规划的过程包括：
32、在进行动态规划前首先定义状态转移方程，设定v(s)表示状态s到终点的最优代价，q为对应的五次多项式曲线，则状态转移方程为：
33、v(s)＝min{cost(s,q,s')+v(s')}
34、这个方程表示，从状态s到采样终点的最优代价等于从状态s到所有相邻状态s'的代价与从状态s'到终点的最优代价之和的最小值；
35、初始化采样终点g的最优代价为零；
36、从采样终点g开始，逆序遍历所有采样点的状态s，对于每个状态s，对于所有相邻采样点的状态s'，计算状态转移方程，在计算过程中，确保五次多项式曲线满足速度、加速度和急跳度的约束；
37、递归计算所有中间采样点的状态的最优代价，直到计算出采样起点状态的最优代价；
38、从采样起点s开始，正向遍历所有采样点的状态s，根据状态转移方程选择最优的下一个状态s'＝argmin{cost(s,q,s')+v(s')}；
39、将s更新为s'，并将对应的五次多项式曲线l添加到最优路径中，重复这个过程，直到到达采样终点g；
40、最后，得到的局部指引轨迹包含一系列相邻状态之间的五次多项式曲线段，将这些曲线段连接起来，得到离散空间下，满足约束条件的局部指引轨迹。
41、更进一步的，基于迭代lqr算法的轨迹规划算法的主要结构包括外循环和内循环两部分，外循环使用松弛屏障函数将轨迹规划涉及到的不等式约束转换为惩罚添加到目标函数中，内循环使用迭代lqr来解决变换后的包含约束目标函数的优化问题。
42、第二方面，本发明提供一种实现全局与局部融合路径规划方法的装置，包括：至少一处理单元，所述处理单元通过总线单元连接存储单元，所述存储单元存储计算机程序，所述计算机程序被所述处理单元执行时，实现所述的全局与局部融合路径规划方法。
43、第三方面，本发明提供一种计算机可读存储介质，所述计算机可读存储介质存储计算机程序，所述计算机程序被处理器执行时，实现所述的全局与局部融合路径规划方法。
44、本发明实施例提供的上述技术方案与现有技术相比具有如下优点：
45、针对全局路径道路信息的实时不确定性，设计了一种考虑航向波动的改进双向a*图搜索算法，能够快速搜索出囊括当前道路信息的路点，然后采用贝塞尔曲线拟合进行优化平滑，生成新的平滑的全局路径。显著提高了路径生成的效率和准确性、缩短了路径长度，减少了总体的能量消耗。
46、针对局部规划中数值优化方法对于初始解质量的依赖问题，本技术基于frenet坐标系，使用状态空间采样进行空间离散化，使用动态规划的方法对采样轨迹的安全性、平滑度以及能量消耗等进行评估，筛选出采样点间的最优路径作为局部指引轨迹，并以此决策出具有凸特性的规划空间。该局部指引轨迹与凸特性规划空间将有效提高后续所设计轨迹求解器的规划效率。
47、针对传统数值优化算法计算效率较低和迭代lqr算法无法处理不等式约束的问题，本技术基于迭代lqr算法的轨迹规划算法将规划涉及到不等式约束转化为惩罚加入目标函数，并在规划凸空间中进行轨迹规划的优化。
1.一种全局与局部融合路径规划方法，其特征在于，包括：
2.根据权利要求1所述的全局与局部融合路径规划方法，其特征在于，所述改进双向a*搜索算法包括：
3.根据权利要求1所述的全局与局部融合路径规划方法，其特征在于，截取车辆前方全局路径的l1米，车辆后方全局路径的l2米两部分作为局部路径；若车辆后方全局路径不够l2米，则向前多截取一部分全局路径作为补偿，保证局部路径的长度保持固定长度，同样，若车辆前方的全局路径不足l1米，将向后多截取一部分全局路径作为补偿，保证局部路径的长度保持固定长度。
4.根据权利要求1所述的全局与局部融合路径规划方法，其特征在于，使用五次多项式进行局部状态空间采样，设定不同采样点间的状态转移代价函数，对采样结果以动态规划的方式进行评估，筛选出采样点间的最优路径作为局部指引轨迹包括：
5.根据权利要求4所述的全局与局部融合路径规划方法，其特征在于，进行横纵向的状态点采样时，横向的采样步长为1m，纵向的采样步长为12m，横向采样步数为9，纵向采样步数为10。
6.根据权利要求4所述的全局与局部融合路径规划方法，其特征在于，两个相邻步长采样点之间的转移代价考虑曲线的平滑代价、与障碍物之间的距离代价、与参考线之间的距离代价，转移代价表示为：
7.根据权利要求4所述的全局与局部融合路径规划方法，其特征在于，动态规划的过程包括：
8.根据权利要求1所述的全局与局部融合路径规划方法，其特征在于，基于迭代lqr算法的轨迹规划算法的主要结构包括外循环和内循环两部分，外循环使用松弛屏障函数将轨迹规划涉及到的不等式约束转换为惩罚添加到目标函数中，内循环使用迭代lqr来解决变换后的包含约束目标函数的优化问题；
9.一种实现全局与局部融合路径规划方法的装置，其特征在于，包括：至少一处理单元，所述处理单元通过总线单元连接存储单元，所述存储单元存储计算机程序，所述计算机程序被所述处理单元执行时，实现如权利要求1-8任一所述的全局与局部融合路径规划方法。
10.一种计算机可读存储介质，所述计算机可读存储介质存储计算机程序，其特征在于，所述计算机程序被处理器执行时，实现如权利要求1-8任一项所述的全局与局部融合路径规划方法。
