[发明专利]一种基于floyd算法的无人机路径规划方法

说明书

本发明公开了一种基于floyd算法的无人机路径规划方法，基于floyd的动态路径规划，从图的带权邻接矩阵A＝[a(i,j)]n×n开始，递归地进行n次更新，即由矩阵D(0)＝A，按一个公式，构造出矩阵D(1)；又用同样的公式由D(1)构造出D(2)；……；最后又用同样的公式由D(n‑1)构造出矩阵D(n)，矩阵D(n)的i行j列元素便是i号顶点到j号顶点的最短路径长度，称D(n)为图的距离矩阵，同时还可引入一个后继节点矩阵path来记录两点间的最短路径，通过此种方法找出始终节点间的最短路径。

技术领域

本发明涉及一种无人机路径规划方法，尤其涉及一种基于floyd算法的无人机路径规划方法。

背景技术

随着科技的发展，无人机被广泛地应用到了各个领域，如军事、农业、建筑行业等。无人机包含了多种技术，如协同控制，编队等。繁重的任务、变化的多机协同战术应用，有效组织与科学管理无人机完成指定的任务迫在眉睫。基于此，路径动态规划与无人机而言是一项举足轻重的技术，必不可少。无人机路径的动态规划既可以提高效率又能保证安全性，是无人机的一项关键技术。

无人机在飞行过程中，可能会有无数条路径可以到达目标节点，怎么选择最佳路径到达目标节点是动态路径规划的目的。在规划过程中需要考虑障碍物，障碍物越是密集，则需要的中间节点也就越多，计算复杂度就会越高。寻找最佳飞行航线，确定该航线上对载荷设备的控制策略是无人机路径规划的目的，从而使无人机在飞行安全确保的前提下，发挥载荷设备的作用，完成各项任务。经过动态规划，无人机可以从根节点通过最短路径到达终端节点，节省了时间和资源。

就目前而言，我国对于无人机的路径动态规划探索还处于初级阶段，很多技术尚未成熟。目前也存在一些路径规划算法，如dijistra算法和HOCAOGLU算法等，但是存在计算复杂度高，效率低等问题，。

发明内容

有鉴于此，本发明的目的是提供一种基于floyd算法的无人机路径规划方法，该算法不仅中间节点选取方法简单而且效率高。

本发明通过以下技术手段解决上述技术问题：

一种基于floyd算法的无人机路径规划方法，其特征在于，包括如下步骤：

第一步，确定坐标，基于地球中心坐标系，标注出障碍物节点以及初始点和目标节点的位置，分别设为Z(x,y),S(x,y),G(x,y)，从而构成任务地图的初始形态；

建立地球中心坐标系，采用地图工具标注出初始点S(x,y),目标节点G(x,y),并标注出障碍物节点Z(x,y),建立起三个节点的坐标，为图的组成提供基础节点；搭建结构体，所述结构体包括LinkNode和Node，其中结构体变量包括：邻接的结点在数组中的编号vex，指向下一个节点next，节点所存数据data，列表头节点head，并以此来建立图；

第二步，选择中间节点，以无人机飞行方向的转折点及航向改变点作为中间节点，改变航向即是无法进行两点之间直线飞行时，遇到障碍物时会发生方向的改变，将每一个方向的改变点作为一个中间节点，结合上述障碍物以及初始点和目标节点构成图；

初始化初始点S(x,y)航向角θ₀，比较点i处的航向角θ_i与前一航向角θ_i-1，判断是否相等，若相等，则i点不能选择为中间节点，若不相等则i就是其中一个中间节点；

第三步，最佳路径选择，从图的带权邻接矩阵A＝[a(i,j)]n×n开始，递归地进行n次更新，即由矩阵D(0)＝A，按一个公式，构造出矩阵D(1)；又用同样的公式由D(1)构造出D(2)；……；最后又用同样的公式由D(n-1)构造出矩阵D(n)，矩阵D(n)的i行j列元素便是i号顶点到j号顶点的最短路径长度，称D(n)为图的距离矩阵，同时引入一个后继节点矩阵path来记录两点间的最短路径，该最短路径就是无人机的最佳航线；