基于改进D*算法的无人机三维路径规划方法及系统

本发明涉及无人机路径规划，尤其涉及一种基于改进d*算法的无人机三维路径规划方法及系统。

背景技术：

1、无人机(unmanned aerial vehicle，uav)使用传统d*算法进行三维路径自主规划时，会创建open和closed两个表来完成扩展节点和选取最优节点的任务。从目标点栅格开始，对目标点栅格周围的26个栅格节点进行搜索并存入open表，然后计算他们的代价值和代价估计值，选取最优的栅格节点(代价值最小的栅格节点)继续扩展，被扩展过的点从open表中删除，存入closed表中。重复上述操作，依次扩展节点，直到起始点从open表中删除，执行过程如图1所示。

2、图1中，当遍历到当前栅格节点n时，将节点n存入closed表，遍历该栅格周围的26个栅格并存入open表；经过代价函数计算，将代价值最小的栅格节点n1从open表中删除，存入closed表中。然后遍历节点n1周围的26个栅格节点，计算出最小的代价值节点n1-2从open表中删除，存入closed表中。之后继续向前遍历，直到起始点s存入closed表中。

3、d*算法在面对动态障碍物以及寻找安全路径方面一直有着良好表现，尤其与基于强化学习、深度学习的智能算法相比，d*算法不需要庞大的训练数据和漫长的训练时间，并且d*算法不必担心规划出的路径会受训练数据的影响，即过拟合问题。但传统d*算法在用作路径规划时也经常出现搜索效率低、拐点过多、路径复杂以及路径安全性不高等问题。文献[1][2]将全局地图环境分解为多个局部环境，以局部环境关键点为主，优化子节点的选取方式，舍弃无用节点；同时改进代价函数并引入平滑函数，所得路径拐点数有所减少,路径复杂性降低。文献[3]也提出一种基于沃罗诺伊路线图的方法将全局环境分解为多个局部环境，改进代价函数并引入平滑函数，在转角度数和转弯次数上有所优化。但这些方法一般运用在二维环境中，针对三维环境中遇到的问题，文献[4]在代价函数中加入障碍物威胁代价项，并采用b样条曲线拟合模型对路径进行了平滑处理，使d*算法规划出的路径与障碍物保持安全距离，但并未减少路径中的不必要拐点，路径依旧复杂。文献[5][[6]结合遗传算法提出一种不同的改进方案，根据特定策略从d*算法规划的初始航迹中提取特征节点生成特征点路径，并以优化后的特征点路径为基础生成初始种群，设计了合理的适应度函数。实验结果表明，改进后的d*算法规划航迹航程更短，威胁代价更小。但在方向约束下的d*算法会产生大量无用节点，算法时间复杂度过高。

4、以上方法在一定程度上改进了传统d*算法的搜索效率和规划路径。但在实际用作路径规划时依然存在一些弊端：(1)算法规划阶段遍历节点速度过慢，搜索速度慢；(2)算法计算量大，算法效率不高；(3)算法生成路径冗余节点多，路径复杂性高。

5、参考文献

6、[1]秦旭,黄晓华,马东明,翁亚华.基于改进d*算法的巡检机器人路径规划[j].组合机床与自动化加工术,2022,(06):10-13.

7、[2]史久根,李凯业.基于分层改进d～*算法的室内路径规划[j].计算机应用研究,2015,32(12):3609-3612.

8、[3]王帅军,胡立坤,王一飞.基于改进d*算法的室内移动机器人路径规划[j].计算机工程与设计,2020,41(04):1118-1124.

9、[4]朱蟋蟋,孙兵,朱大奇.基于改进d*算法的auv三维动态路径规划[j].控制工程,2021,28(04):736-743.

10、[5]周璐,李军华.具有方向约束的无人机改进d*算法航迹规划[j].计算机仿真,2018,35(09):46-51.

11、[6]赵娟.启发点引导d*算法扩展的无人机航迹规划策略[j].机械设计与制造,2020,(02):153-157.

技术实现思路

1、为此，本发明实施例提供了一种基于改进d*算法的无人机三维路径规划方法及系统，用于解决现有技术中无人机使用传统d*算法进行三维路径自主规划过程中，存在算法效率低和路径拐点多且复杂的问题。

2、为了解决上述问题，本发明实施例提供一种基于改进d*算法的无人机三维路径规划方法，所述方法包括：

3、s1：获取无人机当前环境地图信息，初始化地图信息，包括起始点、目标点以及障碍物；

4、s2：利用改进的d*算法，根据初始化后的地图信息进行路径优化，得到最优路径，具体包括：

5、s21：创建open表和closed表，将目标点存入open表中；

6、s22：判断open表是否为空，如果open表为空，则路径规划失败，如果open表不为空，遍历当前栅格周围栅格，计算代价函数，将代价值最小节点a从open表中删除，存入closed表中；

7、s23：判断节点a是否是起始点，若节点a是起始点，则执行步骤s24，若节点a不是起始点，则执行步骤s22；

8、s24：对所得路径进行优化处理，将目标点存入new_path中，并将目标点定义为第一个判断点；

9、s25：将判断点与其前面的路径节点b连线判断是否穿过障碍物，如果穿过障碍物，则将路径节点b后面的一个路径节点c更新为判断点，如果没有穿过障碍物，执行步骤s26；

10、s26：判断节点b是否是起始点，若节点b不是起始点，则执行步骤s25，若节点b是起始点，则路径优化完成，得到最优路径；

11、s3：根据最优路径对无人机的三维路径进行规划。

12、优选地，在遍历当前栅格周围栅格时，采用变步长的方式扩展节点，具体包括：

13、采用变步长的方式扩展节点，根据扩展节点栅格(xr,yr,zr)和目标点栅格(xt,yt,zt)的位置关系定义步长h，当扩展节点栅格未到目标点栅格时，为加快遍历速度，步长较大；当扩展节点栅格超过目标节点栅格，则选取较小的步长。

14、优选地，在x方向上，根据扩展节点栅格(xr,yr,zr)和目标点栅格(xt,yt,zt)的位置关系定义步长h的方法为：

15、当xr<xt时，步长h为xr与地图x轴最大值xmax的差值的二十分之一向上取整，即

16、

17、当xr>xt时，步长h为-1。

18、优选地，所述代价函数的计算公式为：

19、f(s)＝g(s)+h(s)

20、

21、其中，f(s)是当前所在节点到起始点的代价值，g(s)是当前所在节点到起始点的实际代价值，h(s)是当前所在节点到目标点的代价估计值，s′是当前所在节点s的拓展节点，succ(s)是拓展节点集，sgoal是目标节点；h(s′)是s′的代价估计值，c(s,s′)是两个相邻节点的代价值。

22、优选地，采用切比雪夫距离和曼哈顿距离的融合式作为代价函数中的代价值。

23、优选地，所述切比雪夫距离和曼哈顿距离的融合式为：

24、在三维环境下，两个点(x1,y1,z1)、(x2,y2,z2)之间的切比雪夫距离公式为：

25、dc＝max||x1-x2|,|y1-y2|,|z1-z2||

26、在三维环境下，两个点(x1,y1,z1)、(x2,y2,z2)之间的曼哈顿距离公式为：

27、dm＝|x1-x2|+|y1-y2|+|z1-z2|

28、将当前节点、目标点所在直线与当前节点、扩展节点所在直线的夹角定为α，并将sinα作为切比雪夫距离和曼哈顿距离的权重值，得到距离融合式如下：

29、d＝sinαdc+(1-sinα)dm

30、其中，d表示切比雪夫距离和曼哈顿距离的距离融合值。

31、优选地，在判断两点之间的连线是否穿过障碍物时，采用投影法将三维路径及三维障碍物地图投影到二维平面上，具体包括：

32、首先，在二维平面上，判定路径与障碍物是否有交点，若存在交点则比较该线段上各点的高度与障碍物的高度；当某段路径在二维平面上与障碍物有交点且路径段上存在某点高度小于障碍物高度，那么在三维空间内，该段路径穿过了障碍物。

33、本发明实施例还提供了一种基于改进d*算法的无人机三维路径规划系统，所述系统包括：

34、初始化模块，用于获取无人机当前环境地图信息，并初始化地图信息，包括起始点、目标点以及障碍物；

35、规划及优化模块，用于利用改进的d*算法，根据初始化后的地图信息进行路径优化，得到最优路径，具体包括：

36、表格创建单元，用于创建open表和closed表，将目标点存入open表中；

37、第一判断单元，用于判断open表是否为空，如果open表为空，则路径规划失败，如果open表不为空，遍历当前栅格周围栅格，计算代价函数，将代价值最小节点a从open表中删除，存入closed表中；

38、第二判断单元，用于判断节点a是否是起始点，若节点a是起始点，则执行判断点确定单元，若节点a不是起始点，则执行所述第一判断单元；

39、判断点确定单元，用于对所得路径进行优化处理，将目标点存入new_path中，并将目标点定义为第一个判断点；

40、第三判断单元，用于将判断点与其前面的路径节点b连线判断是否穿过障碍物，如果穿过障碍物，则将路径节点b后面的一个路径节点c更新为判断点，如果没有穿过障碍物，执行判断及输出单元；

41、判断及输出单元，用于判断节点b是否是起始点，若节点b不是起始点，则执行所述第三判断单元，若节点b是起始点，则路径优化完成，得到最优路径；

42、执行模块，用于根据最优路径对无人机的三维路径进行规划。

43、本发明实施例还提供了一种电子设备，所述电子设备包括处理器、存储器和总线系统，所述处理器和存储器通过该总线系统相连，所述存储器用于存储指令，所述处理器用于执行存储器存储的指令，以实现上述任意一项所述的基于改进d*算法的无人机三维路径规划方法。

44、本发明实施例还提供了一种计算机存储介质，所述计算机存储介质存储有计算机软件产品，所述计算机软件产品包括的若干指令，用以使得一台计算机设备执行上述任意一项所述的基于改进d*算法的无人机三维路径规划方法。

45、从以上技术方案可以看出，本发明申请具有以下优点：

46、(1)本发明采用变步长的方式扩展节点，提高了算法在规划阶段遍历节点的速度，算法的搜索效率增强；

47、(2)本发明使用切比雪夫距离和曼哈顿距离的融合式作为算法的代价函数，避免了欧式距离大量的开方计算，减少了算法计量，提高了算法效率，一定程度上增强了生成路径的精确性；

48、(3)本发明将三维路径和障碍物投影在二维平面上，降低了优化路径时的计算量，路径优化处理保存了关键节点，删除了冗余节点，生成路径的复杂性降低，更加符合无人机动力学和无人机飞行任务的要求。