[發明專利]一種無人機路徑規劃方法

在審

• 申請號: 201410012975.6
• 申請日: 2014-01-13
• 公開（公告）號: CN103697896A
• 公開（公告）日: 2014-04-02
• 發明（設計）人: 焦李成;馬文萍;居陽;馬晶晶;王爽;侯彪;李陽陽
• 申請（專利權）人: 西安電子科技大學
• 主分類號: G01C21/20
• 分類號: G01C21/20
• 代理公司: 天津市三利專利商標代理有限公司 12107
• 代理人: 閆俊芬

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[]之中。