基于改进A-Star算法的非平坦环境下路径规划方法

本发明涉及车辆路径智能规划，涉及基于改进a-star算法的非平坦环境下路径规划方法。

背景技术：

1、传统无人驾驶系统主要包括感知、决策、控制三大模块。作为决策模块重要组成部分之一的路径规划技术一直是该领域内的研究热点与难点。目前针对于路径规划的研究主要集中于结构化的二维道路环境，环境模型单一，无法切实体现出环境特征，从而使得算法在山地、丘陵、沟壑等崎岖非平坦环境下缺乏适应性，无法较好完成路径规划任务。

2、目前，在众多路径规划算法中，a-star算法因其特有的计算效率高、原理简单、规划路径距离短等优点，得到了广泛应用。针对非平坦地形下无人驾驶的轨迹规划，cn1156515157a公开一种基于凸优化的非平坦地形下无人车轨迹规划方法，首先，建立无人车的能耗模型，以确定无人车前往相邻节点所产生的能耗；其次，设计启发式函数并基于a-star搜索算法生成能耗最优初始路径；然后，经过修剪初始路径的路标点生成运动走廊，用以约束最终轨迹位置及运动学特性；最后，设计目标函数，施加多种约束，得到优化后的初始轨迹，进一步通过对生成分段贝塞尔轨迹所用时间进行重分配得到优化后的最终轨迹；

3、但在复杂环境下，使用传统a-star算法在dem(digitalelevation model，数字高程模型)地图中进行路径规划将会受到多重因素的制约，当地图分辨率过大时，规划速度较低，当地图分辨率过小时，路径质量较差。路径质量与规划速度二者之间存在不可调和的矛盾。

4、综上所述，现有技术对于车辆路径智能规划采用的a-star算法在非平坦环境下的规划速度与路径质量达不到预期。

技术实现思路

1、本发明的目的在于，提供基于改进a-star算法的非平坦环境下路径规划方法，能有效提高a-star算法在非平坦环境下的规划速度与路径质量。

2、为实现上述技术目的，达到上述技术效果，本发明是通过以下技术方案实现：

3、基于改进a-star算法的非平坦环境下路径规划方法，分离原始dem地图，建立子dem地图及地面陡峭度地图；确定路径起始点与目标点，并同时求取该两点在不同子dem地图中的映射；根据映射后的起点与终点分别在对应的子dem地图中采用a-star算法进行子路径规划；对子路径规划进行归一化转换计算，求取次优路径；对次优路径中的路径点进行缺值补全，得到最优路径，完成规划。

4、基于改进a-star算法的非平坦环境下路径规划方法，包括：

5、s1：分离原始数字高程模型dem地图m，分别建立分辨率为原始地图二分之一的两幅子dem地图m1、m2以及对应的陡峭度地图mσ1、mσ2；

6、s2：确定路径起始点ps(xs,ys)与目标点pg(xg,yg)，并同时求取该两点在不同子dem地图中的映射，其中子dem地图m1映射为pos(xos,yos)与pog(xog,yog)，子dem地图m2映射为pts(xts,yts)与ptg(xtg,ytg)；

7、s3：根据映射后的起点与终点分别在对应的子dem地图中采用a-star算法进行路径规划；

8、s4：对步骤s3中规划出的路径path1、path2进行转换计算，求取次优路径path；

9、s5：对次优路径path中的路径点进行缺值补全，得到最优路径path，规划结束。

10、进一步的，所述s1具体为：

11、定义如下映射关系：

12、

13、其中：原始dem地图m的分辨率为m×n，子dem地图m1的分辨率为子dem地图m2的分辨率为表示对a向上取整；

14、原始dem地图m向子dem地图m1的映射为：

15、

16、

17、具体的：

18、

19、其中：表示原始dem地图m向子dem地图m1的映射；(xk,yk)为原始dem地图m中节点pk的坐标；(xok,yok)为pk映射在子dem地图m1中的节点pok的坐标；为对节点坐标值分别向上取整；

20、原始dem地图m向子dem地图m2的映射为：

21、

22、

23、具体的：

24、

25、其中：表示原始dem地图m向子dem地图m2的映射；(xk,yk)为原始dem地图m中节点pk的坐标；(xtk,ytk)为pk映射在子dem地图m2中的节点ptk的坐标；为对节点坐标值分别向上取整；

26、子dem地图m1向原始dem地图m的映射为：

27、

28、

29、具体的：

30、

31、

32、

33、其中：表示子dem地图m1向原始dem地图m的映射；(xok,yok)为子dem地图m1中节点pok的坐标；{(xk,yk)}为子dem地图m1中的节点pok映射在原始dem地图m中的节点集合{pk}对应的坐标集合；

34、子dem地图m2向原始dem地图m的映射为：

35、

36、

37、具体的：

38、

39、

40、

41、其中：表示子dem地图m2向原始dem地图m的映射；(xtk,ytk)为子dem地图m2中节点ptk的坐标；{(xk,yk)}为子dem地图m2中的节点ptk映射在原始dem地图m中的节点集合{pk}对应的坐标集合；

42、通过上述定义上述映射关系后，原始dem地图m、子dem地图m1、子dem地图m2中的任意节点坐标均可通过相关转换关系得到对应的子dem地图或原始dem地图中的坐标；

43、子dem地图m1中节点pok对应的高程值为：

44、

45、对应的陡峭度为：

46、

47、子dem地图m2中节点ptk对应的高程值为：

48、

49、对应的陡峭度为：

50、

51、其中：sumh(a)表示a中所有节点在原始dem地图m中对应高程值之和；size(a)表示a中节点个数；singlehi(a)表示a中第i个节点在原始dem地图m中对应的高程值(注：i∈[1,size(a)])；

52、通过上述转换计算，可通过原始dem地图m分离出分辨率约为原始地图二分之一的两幅子dem地图m1、m2以及对应的陡峭度地图mσ1、mσ2。

53、进一步的，所述s2具体为：

54、确定原始dem地图m中的路径起始点ps(xs,ys)与路径目标点pg(xg,yg)，通过步骤s1中所述转换关系可分别求得子dem地图m1中的路径起始点pos(xos,yos)与路径目标点pog(xog,yog)，以及子dem地图m2中的路径起始点pts(xts,yts)与路径目标点ptg(xtg,ytg)。

55、进一步的，所述s3具体为：

56、在子dem地图m1中通过a-star算法规划出路径起始点pos(xos,yos)至路径目标点pog(xog,yog)的子路径path1；在子dem地图m2中通过a-star算法规划出路径起始点pts(xts,yts)至路径目标点ptg(xtg,ytg)的子路径path2。

57、具体的：

58、a-star算法的启发函数为f(n)＝g(n)+h(n)

59、其中：f(n)为路径总代价，g(n)为起点至当前节点的实际代价，h(n)为当前节点到终点的预估代价；

60、其中：

61、

62、h(n)＝d(n,g)(1+e3σn)

63、其中：d(n-1,n)为节点n-1到节点n的欧几里得距离；θn为节点n的坡度值，θmax为一定条件下允许智能车辆安全通过的最大坡度值；δhn为节点n-1到节点n的阶跃高程值，δhmax为一定条件下允许智能车辆安全通过的最大阶跃高程值；σn为节点n的陡峭度，σmax为一定条件下允许智能车辆安全通过的最大陡峭度；e1、e2、e3分别为坡度代价、阶跃高程代价、陡峭度代价的权重，三者之和为1。

64、其中：

65、

66、δhn＝|hn-hn-1|

67、

68、具体的：

69、

70、

71、考虑智能车辆运动过程中的安全性与平顺性，在路径搜索过程中，若

72、θn＞θmax、δhn＞δhmax、σn＞σmax，则g(n)＝∞

73、通过上述改进启发函数后的a-star算法可分别在子dem地图m1与与子dem地图m2规划出子路径path1与子路径path2。

74、进一步的，所述s4具体为：

75、通过s1中建立原始dem地图m、子dem地图m1、子dem地图m2对应的转换关系后，当子dem地图m1中的节点pok与子dem地图m2的子节点ptk存在空间交集时可惟一映射一个原始dem地图中的节点pk，即

76、

77、通过上述转换关系对s3中所得到的子路径path1与子路径path2进行归一化转换计算，可得到次优路径path；

78、具体的：

79、

80、进一步的，所述s5具体为：

81、通过上述s4可得到一条次优路径path＝{p1,p2,p3…pi…pm+1}，对于该次优路径，遍历所有路径点，计算两相邻路径点坐标距离l(pi,pi+1)，根据此距离对次优路径进行缺值补全；

82、具体的：

83、

84、若l(pi,pi+1)＝1或则

85、若l(pi,pi+1)≠1且则在原始dem地图中，以pi为起点，pi+1为终点，使用a-star算法进行路径规划，规划出的路径为

86、进一步的，所述步骤s5中a-star算法的启发函数为f(n)＝g(n)+h(n)

87、其中：f(n)为路径总代价，g(n)为起点至当前节点的实际代价，h(n)为当前节点到终点的预估代价。

88、

89、

90、其中：d(n-1,n)为节点n-1到节点n的欧几里得距离；θn为节点n的坡度值，θmax为一定条件下允许智能车辆安全通过的最大坡度值；δhn为节点n-1到节点n的阶跃高程值，δhmax为一定条件下允许智能车辆安全通过的最大阶跃高程值；为陡峭度常量，恒等于0，σmax为一定条件下允许智能车辆安全通过的最大陡峭度；e1、e2、e3分别为坡度代价、阶跃高程代价、陡峭度代价的权重，三者之和为1。

91、其中：

92、

93、δhn＝|hn-hn-1|

94、

95、具体的：

96、

97、

98、考虑智能车辆运动过程中的安全性与平顺性，在路径搜索过程中，若：

99、θn＞θmax、δhn＞δhmax，则g(n)＝∞

100、通过上述对次优路径缺值补齐后，可得到最优路径path

101、

102、路径规划结束。

103、本发明的有益效果为：

104、本发明公开基于改进a-star算法的非平坦环境下路径规划方法，通过分离原始dem地图，在保留原始环境地图信息的前提下，将原始dem地图分离为两幅子dem地图的同时，建立了子dem地图相对应的地面陡峭度地图，随后在两幅子dem地图中分别规划，在原始dem地图中进行路径点归一化转换计算，最后对路径点进行缺值补全，规划出最优路径；

105、本发明公开基于改进a-star算法的非平坦环境下路径规划方法，与传统直接降低地图分辨率的方法相比，本发明方法通过分辨率错位降低的思想，极大程度保留了原始地图信息，避免了局部环境特征的丢失。

106、本发明公开基于改进a-star算法的非平坦环境下路径规划方法，通过子地图分别规划，原始地图路径归一化，在实现算法速度提升的同时兼顾路径质量，解决了原始算法的局限性。

107、本发明公开基于改进a-star算法的非平坦环境下路径规划方法，在路径规划过程中，将地形特征与车辆参数作为对路径代价的衡量指标之一，因此，适用范围更广，可移植性更好。

108、本发明公开基于改进a-star算法的非平坦环境下路径规划方法，相较于原始a-star路径规划算法，本发明改进a-star算法的路径规划方法规划速度提升了一个数量级，规划的同时考量了地面的崎岖程度与车辆有关性能参数，因此规划出的路径也更为平坦，更适合智能车辆运动。

109、当然，实施本发明的任一产品并不一定需要同时达到以上所述的所有优点。