[发明专利]一种无人机路径规划方法

摘要

本发明公开一种无人机路径规划方法，所述方法包括如下步骤：第一步，初始化任务地图，标注起点Start(xstart,ystart)、终点Goal(xgoal,ygoal)和障碍物的坐标，计算出所述无人机的最小安全转向半径R；第二步，设定起点为节点Node[0]和终点为节点Node[-1]，查找其它节点坐标，以结构体建立二叉树，第三步，以深度优先左值遍历方法读取二叉树，依次记录节点顺序，将相邻节点间的连线及采用了Dijkstra算法生成的路径存入矩阵Path[]之中。与现有技术相比，提出了一种节点具体标定方法，通过在两点连线上任意位置作垂线，增加了路径的多样性；采用非进化的计算方法，节省了计算时间和存储空间，增加了算法的稳定性。

主权项

1.一种无人机路径规划方法，其特征在于，所述方法包括如下步骤：第一步，初始化任务地图，标注起点Start(x_start,y_start)、终点Goal(x_goal,y_goal)和障碍物的坐标，计算出所述无人机的最小安全转向半径R；第二步，设定起点为节点Node[0]和终点为节点Node[-1]，查找其它节点坐标，以结构体建立二叉树，所述结构体包括变量：节点在原属线上的位置double Position、节点坐标int X,Y、障碍物高度double Height、左子节点node*Child_Left、右子节点node*Child_Right、左父节点node*Father_Left、右父节点node*Father_Right，其中所述查找其它节点坐标，包括如下步骤：（1）对于任意Node[i]，i=-1、0、1…i，令其与Node[i-1]之间进行连线，所述连线为所述原属线；（2）判断所述Node[i]与所述Node[i-1]之间的连线距离，若小于6R并且连线被所述障碍物遮挡，可判定为进入了死角，返回上一层并重新选取Node[i]节点；若大于6R并且连线被障碍物遮挡，设点K(x_k,y_k)在连线(Node[i-1],Node[i])上，令K(x_k,y_k)从Node[i-1]沿该连线步进移动至Node[i]，形成数组K[Len]；对于K[Len]上每个处于障碍物区域中的点K[i]，向连线两侧作垂线H[i]，记录其距离障碍物边缘的距离，若该点障碍物延续到地图M[Width][Height]边缘则该侧均不予记录；（3）在数组H[i]的两侧之中分别选出K[i]到障碍物边缘的距离绝对值最大的一个值，分别记为H_MAX1和H_MAX2，其对应的点记为K(x_MAX1,y_MAX1)和K(x_MAX2,y_MAX2)；在点K(x_MAX1,y_MAX1)和K(x_MAX2,y_MAX2)上均作高度Height＝H_MAX的垂线，选取K(x_MAX1,y_MAX1)和K(x_MAX2,y_MAX2)两点垂线最短的一个为Node[i+1]；判断Node[i+1]到Node[i]和Node[i-1]之间的连线是否有障碍物，若有，则Node[i+1]替换为另外一个点，继续判断Node[i+1]到Node[i]和Node[i-1]之间的连线是否有障碍物，若有，则令Height以为单位步进（若不足1则以1为单位步进），继续判断Node[i+1]到Node[i]和Node[i-1]之间的连线是否有障碍物，直至：（3-A）：若Node[i+1]到Node[i]和Node[i-1]之间的连线均无障碍物遮挡；将Height、K(x_MAX,y_MAX)计入Node[i+1]；返回上一级；（3-B）若Height逐步增大使得K(x_MAX,y_MAX)进入障碍物区域，则选择另外一个点重复步骤（3），若仍未能找到与Node[i]和Node[i-1]之间无障碍物阻挡的点Node[i+1]，进行步骤（3-C）；（3-C）若两侧均无可以直接连通Node[i]和Node[i-1]的点，则搜索一个点Node[i]使之满足其与Node[i]和Node[i-1]的连线中，通过的障碍物区域总和最小。并令重复次数加一：Count＝Count+1；若重复次数Count满足：Count≥Level_Max-Level+1，（Level是该节点所在的二叉树层数，Level_Max是当前构造的二叉树最大层数），则强制使用Dijkstra算法，以Node[i]为起点，Node[i-1]为终点进行路径规划；若生成了可行路径，则记录路径轨迹，并进行步骤（4）。若未能生成可行路径，则判定进入了死角。返回步骤（1）并重新选取Node[i]节点。（4）检查节点Node[i]与左侧父节点Node[i-2]之间的连线上有无障碍物。方法同检查左侧的步骤。（5）重复上述步骤，完成二叉树建立；第三步，以深度优先左值遍历方法读取二叉树，依次记录节点顺序，将相邻节点间的连线及采用了Dijkstra算法生成的路径存入矩阵Path[]之中。