[发明专利]一种基于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来记录两点间的最短路径，通过此种方法找出始终节点间的最短路径。

主权项

1.一种基于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来记录两点间的最短路径，该最短路径就是无人机的最佳航线；采用floyd算法从初始点到中间节点的选择到目标节点，将初始点作为根节点，从根节点到目标节点进行遍历，组建成一个图，采用floyd方法进行最佳路径规划，包含变量节点个数vertexnum，权重weight，将每次算法计算得到的路径都存放path[]中，通过权重矩阵和路径矩阵的矩阵运算可以算出最佳路线，得到从根节点到目标节点的最短路径。