一种无人机路径规划方法

1.一种无人机路径规划方法，其特征在于，所述方法包括如下步骤：
第一步，初始化任务地图，标注起点Start(xstart,ystart)、终点Goal(xgoal,ygoal)和障碍物的坐标，计算出所述无人机的最小安全转向半径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(xk,yk)在连线(Node[i-1],Node[i])上，令K(xk,yk)从Node[i-1]沿该连线步进移动至Node[i]，形成数组K[Len]；对于K[Len]上每个处于障碍物区域中的点K[i]，向连线两侧作垂线H[i]，记录其距离障碍物边缘的距离，若该点障碍物延续到地图M[Width][Height]边缘则该侧均不予记录；
（3）在数组H[i]的两侧之中分别选出K[i]到障碍物边缘的距离绝对值最大的一个值，分别记为HMAX1和HMAX2，其对应的点记为K(xMAX1,yMAX1)和K(xMAX2,yMAX2)；在点K(xMAX1,yMAX1)和K(xMAX2,yMAX2)上均作高度Height＝HMAX的垂线，选取K(xMAX1,yMAX1)和K(xMAX2,yMAX2)两点垂线最短的一个为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(xMAX,yMAX)计入Node[i+1]；返回上一级；
（3-B）若Height逐步增大使得K(xMAX,yMAX)进入障碍物区域，则选择另外一个点重复步骤（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[]之中。
2.根据权利要求1所述的无人机路径规划方法，其特征在于，进行路径平滑处理，更新所述路径矩阵，所述进行路径平滑处理的步骤具体包括：对于每个节点Node[i]，设点A、B，A、B点从Node[i]开始，沿路径分别向两侧移动，在A、B点上分别向内角方向作高为R的垂线，当二者的垂线末端相交时停止，记交点为O，以O为圆心，R为半径作圆，取弧AB代替A-Node[i]-B折线，并更新Path[]。
3.根据权利要求1或2所述的无人机路径规划方法，其特征在于，所述障碍物包括敌方雷达警戒区和高度超过无人机飞行高度的地形凸起。在地图上将敌方雷达警戒区和高度超过UAV飞行高度的地形凸起标注为障碍物。

暂无