一种基于关键障碍物周围点集法和A*算法的无人机路径规划方法

本发明属于无人机路径规划，具体涉及一种基于关键障碍物周围点集法和a*算法的无人机路径规划方法。

背景技术：

1、作为科技创新的重要产业，无人机技术正处于井喷式发展的时期，由于技术日臻成熟，无人机的应用领域不断扩大，如军事侦查、农林生产、物流运输等。无人机的任务环境从最开始的空旷隔离空域，扩展到充满不规则障碍的复杂空域，面对各种各样的复杂环境，无人机的路径规划技术格外重要，好的飞行路径直接关系到无人机的运行效率、自身安全和对地面的安全影响。

2、现有的对无人机进行路径规划大都基于a*算法和障碍物周围点集法，传统的a*算法是一种静态路网中求解最短路径最有效的直接搜索方法，广泛地应用于各种类型搜索问题中，传统a*算法在二维空间中较为成熟，但在三维空间中由于维度的增加，容易出现运算速度慢，冗余节点过多等问题；有学者针对上述问题，提出动态加权以改变搜索邻域的方法对传统a*算法进行改进，从而减少了搜索节点，提高了搜索速度，同时多次使用高阶贝塞尔曲线对改进后的a*算法规划出来的路线进行平滑处理减少了转折点，但该方法在改变搜索邻域时舍弃了3个方向上的搜索，在复杂情况下会陷入局部最优，无法找到路径。障碍物周围点集法用来减少三维空间中不必要的搜索，该方法的灵感通过观察得来，得到的最短路径往往是对障碍物进行绕行，所以该方法只会去关注障碍物周围的点，而不去关注整个三维空间，以此对寻路空间进行缩减，但是生成的路径距离障碍物太近，无法保障无人机的安全，不适合无人机飞行。

技术实现思路

1、为了解决背景技术中存在的问题，本发明提供一种基于关键障碍物周围点集法和a*算法的无人机路径规划方法，通过设置安全距离改进关键障碍物周围点集法使最终获得的无人机路径中的可行点与障碍物保持安全距离，进而提高路径的安全性，保障无人机的安全；通过自适应加权策略改进+a*算法的估价函数，使算法能够更快完成路径的规划；通过冗余节点删除算法，优化路径中的冗余节点减少无人机的飞行时间，进而降低无人机的能量消耗，包括：

2、s1：构建三维无人机环境模型，获取无人机的起始点和目标点；

3、s2：根据无人机的起始点、目标点和障碍物信息利用基于关键障碍物周围点集法和a*算法计算得到无人机的目标路径；

4、s3：利用冗余节点删除算法删除无人机目标路径中的冗余节点得到无人机的最终路径。

5、进一步地，所述建立三维无人机环境模型包括：

6、s11：在三维地图中挑选合适的顶点作为坐标原点a，对该坐标原点a建立三维直角坐标系a-xyz，确定栅格地图的最大长度ab，最大宽度ad，最大高度ae，构造容纳三维地图的立方体abcd-efgh；

7、s12：对立方体abcd-efgh沿ab进行m等分，过等分点作平行于abef面的平面，得m个平面yi(i＝1,2,…,m)；

8、s13：对任一平面yi，沿ad进行n等分，沿ae进行h等分，此时yi便离散成n×h个平面栅格，即立方体abcd-efgh被离散成m×n×h个立体栅格，则无人机待扩展路径点表示为p(i,j,k),i＝1,2,…,m,j＝1,2,…,n,k＝1,2,…,h。

9、进一步地，所述利用基于关键障碍物周围点集法和a*算法计算得到无人机的目标路径包括：

10、s21：将障碍物所在的栅格确定为障碍物栅格，将与任意障碍物栅格距离小于预设安全距离的栅格确定为危险栅格，其余栅格确定为自由栅格，每个自由栅格的中点为无人机可扩展的可行点；

11、s22：将无人机的起始点s和目标点t相连构成线段st并根据障碍物信息确定关键障碍物co和非关键障碍物nco：

12、s23：遍历关键障碍物co确定si和ci，并利用关键障碍物周围点集法计算无人机的可行点集合v，其中，ci表示st经过第i个关键障碍物oi时线段st经过的所有栅格中距离起始点s最近的一个危险栅格；si表示线段st经过第i个关键障碍物oi时，线段st经过的所有栅格中距离ci最近的一个自由栅格；

13、s24：以起始点s为起始搜索扩展点，并选取某个扩展点为当前扩展点，计算起始点s到当前扩展点之间的距离和当前扩展点到目标点t之间的距离；

14、s25：根据起始点s到当前扩展点之间的距离、当前扩展点到目标点t之间的距离、当前扩展点的邻居栅格和障碍物信息构建a*算法的估价函数；

15、s26：根据a*算法的估价函数和无人机的可行点集合v利用a*算法计算得到无人机的目标路径。

16、进一步地，所述关键障碍物co和非关键障碍物nco包括：

17、

18、其中，o是障碍物栅格集合；oi代表属于第i个障碍物的所有障碍物栅格集合；表示当oi和之间有交集则返回1，否则返回0。

19、进一步地，无人机的可行点集合v的计算步骤包括：

20、s231：将si放入tempset集合；

21、s232：将tempset集合中的第一个元素定义为参考点当中的栅格与关键障碍物周围的危险栅格发生交集时，将放入sps集合；并将中的自由栅格放入tempset集合；其中，表示参考点的邻居栅格集合；

22、s233：将从tempset集合中去除；

23、s234：判断tempset集合是否为空集，当tempset不为空集时，循环执行步骤s232～s234；当tempset为空集时，输出最终无人机的可行点集合v；

24、

25、其中，表示自由栅格集合，nst代表在直线st上的栅格集合。

26、进一步地，所述a*算法的估价函数包括：

27、f(pn)＝(2-p)g(pn)+ph(pn)

28、

29、其中，dis(pn,s)为当前扩展点pn到起始点s之间的距离；dis(pn,t)表示当前扩展点pn到目标点t之间的距离；nb(pn)表示当前扩展点pn的邻居栅格集合，o表示障碍物栅格集合；g(pn)表示起始点s到当前扩展点pn的实际距离；h(pn)表示当前扩展点pn和目标点t的之间的欧氏距离；f(pn)为改进后的代价函数。

30、进一步地，无人机的目标路径的计算步骤具体包括：

31、s261：初始化a*算法的openlist列表和closelist列表；

32、s262：将起始点s放入openlist列表利用a*算法的估价函数计算openlist列表中估价函数值最小的点作为当前点，并将当前点输入到closelist列表；

33、s263：若当前点为目标点，则规划结束，将closelist列表的节点依次组成无人机的目标路径；

34、s264：判断openlist列表中是否存在当前点的相邻节点，若存在，则将openlist列表中g(n)值最低的当前点的相邻节点作为新的当前点并根据a*算法的估价函数计算新的当前节点的估价函数值；其中，g(n)表示起始点到当前点的实际距离；

35、若不存在，遍历当前点的所有相邻节点，并将在无人机的可行点集合v中当前点的相邻节点移入openlist列表；

36、s265：重复执行步骤s261-s265。

37、进一步地，所述利用冗余节点删除算法删除无人机目标路径中的冗余节点包括：

38、s31：将无人机目标路径中的起点作为节点pcur，终点作为节点pnext；

39、s32：判断pcur和pnext的连线是否经过障碍物栅格或危险栅格；若经过，则将终点的前一节点作为新的pnext；其中，所述危险栅格包括：与任意障碍物栅格的距离小于预设安全距离的栅格；

40、若不经过，则删除节点pcur和节点pnext之间的中间节点；

41、s33：判断节点pnext是否等于节点pcur，若等于，则输出无人机的最终路径；若不等于，则重复s31～s33。

42、本发明至少具有以下有益效果

43、本发明通过创建三维无人机环境模型时考虑无人机的安全距离从而通过设定阈值的方式定义三维无人机环境模型中的危险栅格对关键障碍物周围点集法进行改进使生成的路径更加的安全可靠，通过改进后的关键障碍物周围点集法生成无人机的可行点集合v、起始点到当前扩展点之间的距离、当前扩展点到目标点之间的距离涉及自适应权重更新的a*算法的估价函数，使得无人机目标路径节点的选取更加合理，遍历节点的数量更少，进一步提升运算速度；通过冗余节点删除算法删除无人机目标路径中的冗余节点，可以删除大量冗余路径节点，减少路径的折点数，减少无人机的飞行时间，进而降低无人机的能量消耗，提高无人机的搜索效率。

44、说明书附图

45、图1为本发明的方法流程图；

46、图2为本发明三维无人机环境模型的示意图；

47、图3为本发明和传统无人机路径规划算法的仿真对比图；

48、图4为本发明和传统无人机路径规划算法的仿真对比图。