一種無人機路徑規劃方法
【專利摘要】本發明公開一種無人機路徑規劃方法，所述方法包括如下步驟：第一步，初始化任務地圖，標注起點Start(xstart,ystart)、終點Goal(xgoal,ygoal)和障礙物的坐標，計算出所述無人機的最小安全轉向半徑R；第二步，設定起點為節點Node[0]和終點為節點Node[-1]，查找其它節點坐標，以結構體建立二叉樹，第三步，以深度優先左值遍歷方法讀取二叉樹，依次記錄節點順序，將相鄰節點間的連線及采用了Dijkstra算法生成的路徑存入矩陣Path[]之中。與現有技術相比，提出了一種節點具體標定方法，通過在兩點連線上任意位置作垂線，增加了路徑的多樣性；采用非進化的計算方法，節省了計算時間和存儲空間，增加了算法的穩定性。
【專利說明】 一種無人機路徑規劃方法
【技術領域】
[0001]本發明屬于無人機【技術領域】，尤其涉及一種無人機路徑規劃方法。
【背景技術】
[0002]隨著信息化技術的發展，無人機(UAV)將逐漸的在軍事領域和民用領域中取得更加廣泛的應用。無人機應用中的關鍵技術之一是路徑規劃。路徑規劃(Path Planning)的目的是在給定區間上，找出一條連接起點和終點的，代價最小的路徑。
[0003]路徑規劃是一個NP-Complete問題，截至目前，仍未有固定的最優算法。主流的路徑規劃算法有:Dijkstra、A*、進化算法(Genetic Algorithm)、人工勢場法(ArtificialPotential Field)等。但這些算法的計算復雜度較高，其計算量隨著地圖的增大呈指數上升。
[0004]在2001 年，Cem Hocaoglu 和 Arthur C.Sanderson 在《Planning Multiple Pathswith Evolutionary Speciation))中提出了一種計算復雜度與地圖大小無關的路徑規劃H0CA0GLU算法，H0CA0GLU算法計算復雜度只與地圖的障礙物數量相關，地圖越復雜，路徑需要的節點就越多，H0CA0GLU算法的搜索空間就越大，從而導致算法計算代價的提升，而若是對于一種極端情況——無障礙物的地圖，H0CA0GLU算法的計算復雜度為常量，H0CA0GLU算法避免了大多數路徑規劃算法在大地圖上運算量過大的問題。
[0005]H0CA0GLU算法盡管上述多種優點，但依然存在固有的缺陷。H0CA0GLU算法并未給出確立節點的具體方法，而是采用進化算法進行搜索；同時，H0CA0GLU算法為了減小進化算法的搜索空間，僅在兩點連線的中點上作垂線，這降低了路徑的多樣性，經常會導致算法搜索不到可行路徑。
【發明內容】
[0006]本發明的目的是提供一種無人機路徑規劃方法，以解決現有技術中利用H0CA0GLU算法計算路徑時，存在的無法具體確定節點以及降低路徑多樣性進而經常無法搜到可行路徑的問題。
[0007]為實現本發明的目的，本發明提供了一種無人機路徑規劃方法，所述方法包括如下步驟:
[0008]第一步,初始化任務地圖,標注起點Start (xstart, ystart)、終點Goal (xgoal, ygoal)和障礙物的坐標，計算出所述無人機的最小安全轉向半徑R ；
[0009]第二步，設定起點為節點Node [O]和終點為節點Node [_1]，查找其它節點坐標，以結構體建立二叉樹，所述結構體包括變量:節點在原屬線上的位置double Position、節點坐標 int X, Y、障礙物高度 double Height、左子節點 node*Child_Left、
[0010]右子節點node*Child_Right、左父節點 node*Father_Left、右父節點node*Father_Right,
[0011]其中所述查找其它節點坐標，包括如下步驟:[0012](I)對于任意Node[i]，i=_l、0、l…i,令其與Node [1-1]之間進行連線,所述連線為所述原屬線；
[0013](2)判斷所述Node[i]與所述Node[i_l]之間的連線距離，
[0014]若小于6R并且連線被所述障礙物遮擋，可判定為進入了死角，返回上一層并重新選取Node[i]節點；
[0015]若大于6R并且連線被障礙物遮擋，設點K(xk, yk)在連線(Node[i_l]，Node[i])上,令K(xk，yk)從Node [1-1]沿該連線步進移動至Node [i],形成數組K [Len];對于K [Len]上每個處于障礙物區域中的點K[i]，向連線兩側作垂線H[i]，記錄其距離障礙物邊緣的距離，若該點障礙物延續到地圖M[Width] [Height]邊緣則該側均不予記錄；
[0016](3)在數組H[i]的兩側之中分別選出K[i]到障礙物邊緣的距離絕對值最大的一個值，分別記為Hmajq和Hmax2,其對應的點記為K (xMxl, ymxi)和K (X
MAX2) ΥμΑΧ2);在
點 K(xMxl, Y1AX1)和 K(Xmax2, Y1AX2)上均作聞度 Height — Hmax 的垂線，選取!((Xmaxi, Y1Axi)和K (xmx2, yMX2)兩點垂線最短的一個為Node [i+Ι]；判斷Node [i+Ι]到Node[i]和No d e [ 1-1 ]之間的連線是否有障礙物，若有，則No d e [ i+1 ]替換為另外一個點，繼續判HiNode [i+Ι]到Node[i] Node [1-1]之間的連線是否有障礙物，若有，則令Height以
【權利要求】
1.一種無人機路徑規劃方法，其特征在于，所述方法包括如下步驟: 第一步，初始化任務地圖，標注起點Start (Xstart, ystart)、終點Goal (xgoal, ygoal)和障礙物的坐標，計算出所述無人機的最小安全轉向半徑R ； 第二步，設定起點為節點Node [O]和終點為節點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=_l、0、l…i，令其與Node[i_l]之間進行連線,所述連線為所述原屬線； (2)判斷所述Node[i]與所述Node[i_l]之間的連線距離， 若小于6R并且連線被所述障礙物遮擋，可判定為進入了死角，返回上一層并重新選取Node[i]節點； 若大于6R并且連線被障礙物遮擋，設點K(xk，yk)在連線(Node[1-1]，Node[i])上，令K (xk, yk)從Node [1-1]沿該連線步進移動至Node [i],形成數組K [Len];對于K[Len]上每個處于障礙物區域中的點K[i]，向連線兩側作垂線H[i]，記錄其距離障礙物邊緣的距離，若該點障礙物延續到地圖M[Width] [Height]邊緣則該側均不予記錄； (3)在數組H[i]的兩側之中分別選出K[i]到障礙物邊緣的距離絕對值最大的一個值，分力I」記為Hmaxi和Hmax2，其對應的點記為K(xMxl，yMxl)和K(xMX2, Ymax2);在點K(Xmaxi, yMxl)和K (xmax2，yMX2)上均作聞度 Height — Hmax 的垂線，選取 K (Xmaxi, yMxl)和 K (xMX2, Ymax2)兩點垂線最短的一個為Node [i+Ι];判斷Node[i+l]到Node [i] Node [1-1]之間的連線是否有障礙物，若有，則Node [i+1]替換為另外一個點，繼續判斷Node [i+1]到Node [i] Node [1-1]之間的連線是否有障礙物，若有，則令
2.根據權利要求1所述的無人機路徑規劃方法，其特征在于，進行路徑平滑處理，更新所述路徑矩陣，所述進行路徑平滑處理的步驟具體包括:對于每個節點Node [i]，設點A、B，A、B點從Node[i]開始，沿路徑分別向兩側移動，在A、B點上分別向內角方向作高為R的垂線，當二者的垂線末端相交時停止，記交點為0，以O為圓心，R為半徑作圓，取弧AB代替A-Node [i]-B 折線，并更新 Path[]。
3.根據權利要求1或2所述的無人機路徑規劃方法，其特征在于，所述障礙物包括敵方雷達警戒區和高度超過無人機飛行高度的地 形凸起。在地圖上將敵方雷達警戒區和高度超過UAV飛行高度的地形凸起標注為障礙物。
【文檔編號】G01C21/20GK103697896SQ201410012975
【公開日】2014年4月2日 申請日期:2014年1月13日 優先權日:2014年1月13日
【發明者】焦李成, 馬文萍, 居陽, 馬晶晶, 王爽, 侯彪, 李陽陽 申請人:西安電子科技大學