方向和時間約束下的三維無人機航跡規劃

俞瑞，沈亮

（1.四川大學計算機學院，成都 610065；2.江西洪都航空工業集團有限責任公司，南昌 330000）

0 引言

無人機航跡規劃，指的是無人機在滿足性能、地形環境以及任務約束的條件下，尋找到從起始點到目標點的可行航跡[1-2]。由于戰場敵對環境的復雜，以及無人機自身性能存在約束，為無人機規劃航跡能夠更好地適應環境，避開威脅，提高無人機的安全性，減少航程，降低油耗，減少機動操作，保證預定任務的完成。

目前的路徑規劃用到的啟發式算法有A*算法、遺傳算法、粒子群算法、蟻群算法等[3]，其中A*算法更為簡單高效。傳統的A*算法[4]是在規劃環境網格化的基礎上根據設定的代價函數尋找最小代價的路徑，稀疏A*算法[5]是在傳統A*算法的基礎上，在擴展節點時，通過考察各種約束條件，有效地縮減搜索空間和縮短搜索時間，能夠快速收斂得到所求路徑。

根據任務的特殊性，存在針對無人機在飛行方向和時間上的要求，但傳統規劃算法通常并不考慮。針對此問題，文獻[6]在起點/終點附近設置高代價區域，但該方法并不能保證無人機一定以規定角度飛入或者飛出，只能控制在一定范圍內，且會造成可行解空間的損失；文獻[7]加入引導點的概念，但若設置不當也會產生副作用；文獻[8]通過調整速度來滿足時間維度的方法，但并未考慮速度的調整仍然需要時間，過于理想化，不符合實際情況。本文針對方向和時間約束的三維無人機航跡規劃問題，提出一種基于改進A*算法的有效解決此問題的航跡規劃方法，能使規劃出的航跡更符合實際情況且較優。

1 環境構建

1.1 三維環境構建

在三維空間中進行無人機航跡規劃，建立笛卡爾坐標系，設(x,y,z)為空間中節點的坐標，節點n的坐標為(xn,yn,zn)，其中xn、yn為橫向和縱向坐標，zn為該點的地形高度，則該搜索空間可表示為：

整個三維空間柵格化，每次擴展需要搜索相鄰的26 個節點，如圖1 所示，因此需要根據無人機自身性能約束以及對地形等障礙判斷作為限制條件來縮小搜索空間的范圍。

圖1 三維空間搜索擴展原理

1.2 無人機性能約束

受無人機硬件設施的性能限制，其自身性能存在一定的約束，滿足自身性能條件的航跡規劃也是為了保證安全。

（1）最小步長約束

要求無人機航跡段長度不小于某一個值，該值是無人機在改變姿態時必須直飛的一個距離，也是每次擴展節點的最小距離lmin。在三維網格空間中，無人機在相鄰的頂點之間飛行，故可以頂點間最小航行距離劃分。設無人機有N 段航跡ln，滿足:

（2）最大航程約束

由于無人機只能攜帶一定數量的燃油，因此航跡段之和應不超過最大航程的值ln，即：

（3）最大偏航角約束

無人機的飛行高度變化是有限制的，設允許的最大偏航角為θmax，相鄰節點的高度分別為zn和zn-1，則滿足：

（4）最大飛行高度和最小離地高度約束

受制于無人機自身性能，需要無人機在不超過某一個高度的范圍內飛行，同樣，飛行高度過低的話，無人機容易與地面發生碰撞，因此設定最大飛行高度zmax和最小離地高度zmin為一定值，確保無人機安全，設節點高度為zn，則滿足：

1.3 地形障礙和雷達威脅

真實環境中無人機需要對其航行空間中的地形障礙進行躲避，因此本文設定節點值c表示該點是否可行。山體或障礙所在的節點其c值為1，表示其為不可到達的點，值為0 則表示可到達的點。

預警雷達在戰場中應用廣泛，其威脅程度與雷達的最小半徑和最大半徑的距離有關，小于最小半徑的范圍屬于禁飛區，無人機是不能穿過的，否則被發現則此次任務失敗，大于最大半徑的范圍是雷達檢測不到的，屬于安全區，而在兩個半徑之間的范圍，雷達有一定的概率探測到無人機，這個概率與無人機距離威脅中心的遠近有關。雷達威脅度Tr表示為：

式（6）中，d為無人機到威脅中心的歐氏距離，Rmax為該威脅所影響的最大范圍半徑，Rmin為一定會受到威脅的區域半徑，在Rmin內的區域則為禁飛區，該區域內無人機不可達，在Rmax和Rmin內的區域屬于威脅區，該區域內無人機存在著一定概率的威脅。

2 改進A＊ 算法航跡規劃

2.1 基本思想

A*算法是一種啟發式搜索算法，傳統的A*算法是在規劃環境網格化的基礎上根據設定的估價函數尋找最小代價的航跡，能夠有效地引導無人機飛向目標點。稀疏A*算法是在傳統A*算法的基礎上，在擴展節點時，通過考察各種約束條件，有效地縮減搜索空間和縮短搜索時間，能夠快速收斂得到所求航跡。構建代價函數[9-10]：

式（7）中，n表示當前所在的節點，f(n)為從初始點經由節點n到達目標點所估計的總代價函數，g(n)是從起點到節點n的實際代價，h(n)是從節點n到目標點的估計代價。

2.2 改進A★算法航跡規劃

標準A*算法規劃的航跡存在不足：節點只能反映無人機的位置信息，無法體現無人機的姿態信息；僅通過比較各相鄰擴展節點的代價大小來判斷航跡點是否可行，未考慮到雷達探測概率；冗余點和拐點多，航跡不平滑；無法滿足方向和時間上的約束條件。

因此對傳統的A*算法進行改進，對航跡點的擴展搜索方式、代價函數的設定、新航跡點的可行性判定、航跡優化以及運算時間等進行優化改進。

（1）變權值評估

在搜索過程中，g(n)和h(n)對評估的影響是不同的，應當選擇加權評估方法，從而根據實際情況更改各代價值的比重，保證路徑的合理性，即：

其中，wg和wh分別為實際代價g(n)和估計代價h(n)對應的權值，且滿足wg+wh=1。

（2）引入加速度代價

為了滿足任務要求中對于到達目標點時間的要求，引入加速度代價，以確定到達時間為準，在每一次擴展搜索節點時，通過更改無人機的加速度值來調節其飛行速度，從而達到按時到達目標點的任務要求。節點的加速度值的計算方法：設定當前點作勻變速直線運動到擴展點后，再勻速運動到目標點，運動總時間為預計到達時間，假設當前點的速度為v0，到飛行當前點的所花時間t0，當前點到擴展點的距離為d、所花時間為t1，擴展點的加速度為a，擴展點到目標點的估計距離為s、所花時間為t2，預計到達時間為T，根據方程組（9）求解得出擴展點的加速度，取其絕對值為該點的加速度代價a(n)。

（3）引入威脅代價

為了保障無人機執行任務的安全，避免被敵方雷達發現，在實際代價函數中引入威脅度的評估。根據雷達威脅模型，節點的威脅代價取值w(n)為該點的雷達威脅度Tr。改進A*算法的代價函數f(n)為：

其中，g(n)為初始節點到該點的實際代價，h(n)為該點到目標節點的估計代價，a(n)為該點的加速度代價，w(n)為該點的威脅代價，并針對各項參數的單位以及數量級上的差異進行歸一化的處理，w1、w2、w3、w4為對應的權重系數。

針對各項參數的單位以及數量級上的差異進行如下歸一化的處理：

其中，n表示當前航跡節點，i表示取擴展節點集合中的一個擴展節點，L(n)i為從初始節點到擴展點i的航跡段之和，L(n)為從初始節點到所有擴展點的航跡段之和的集合，D(n)i為擴展點i到目標節點的距離，D(n)為所有擴展節點到目標節點的距離的集合，A(n)i為擴展點i的加速度值，A(n)為所有擴展點的加速度值的集合，W(n)i為擴展點i的威脅值，W(n)為所有擴展點的威脅值的集合，max 表示取集合中的最大值。

（4）計算新的起點和終點

為了滿足無人機以預定角度出發和到達的端點方向約束，分別沿起點和終點的朝向角反方向計算出新的起點和終點。設起點坐標位置(x0,y0,z0)以及其朝向角分別為轉彎角α1和俯仰角β1，輸入終點的坐標位置(xg,yg,zg)以及其朝向角分別為轉彎角α2和俯仰角β2，以最小步長lmin為單位長度，沿起點朝向角方向計算出新的起點(x1,y1,z1)，沿終點朝向角反方向計算出新的終點(xg-1,yg-1,zg-1)，方程組（12）-（13）為計算方法。

（5）雙向搜索對比

將起點和終點的坐標位置和對應朝向角對調后輸入，重新進行航跡規劃，得到另一條規劃航跡；對比所得兩條航跡，選其中總航跡代價值較小的一條作為最終航跡輸出。

因此，本文提出的無人機航跡規劃流程如圖2所示。

3 仿真結果與分析

3.1 規劃區域

設定三維環境大小為 100 × 100 × 50，即MAX_X、MAX_Y、MAX_Z分別為100、100、50，建立一個飛行環境，取某區域的真實地形導入，每單位長度為100m，無人機的起點坐標（10,10,10），終點坐標（90,80,12），初始速度為100m/s，預計到達時間為150s，代價函數中w1、w2、w3、w4分別取值0.2、0.3、0.3、0.2。該實驗在CPU 為Intel Core i7-4500U，RAM 為7.89GB 的計算機上實現，軟件是MATLAB R2016a。規劃區域已有雷達參數如表1 所示，無人機性能約束如圖2 所示。

圖2 無人機航跡規劃流程

表1 雷達參數

表2 無人機性能約束

3.2 仿真對比

為了驗證本文所提出后的方法的有效性，在上述規劃的三維空間中進行航跡規劃仿真實驗。

（1）改進算法與稀疏A*算法比較

稀疏A*算法實驗中，在起點和終點附近設置高代價區，代價函數為實際到達時間與預計到達時間的差值，其余約束條件不變，假設無人機勻速運動，仿真結果如圖3。

本文所提的改進A*算法航跡規劃方法仿真結果如圖4。

由圖3 和圖4 可以看出，本文所提方法在起點和終點的朝向更滿足預設條件。

圖3

圖4

統計兩種方法的端點角度和實際到達時間與實驗約束條件對比，結果如表3 所示。結果顯示，本文所提出的方法更加滿足端點角度約束且到達時間更接近預計到達時間。

表3 2 種算法航跡規劃結果比較

（2）雙向搜索結果對比

對調起點和終點進行雙向搜索實驗，仿真結果如圖5 所示，得到的兩條航跡（a）和（b）的總航跡代價值分別為77.95、82.87，因此從兩條航跡中選出總航跡代價值較小的一條航跡（a）作為最終航跡輸出，即為圖4所示航跡。從該對比圖中也可以看出，雙向搜索后選出的航跡在總航跡代價值較小的前提下，規劃出的航跡較平滑。

圖5

3.3 分析

本文提出的方向和時間約束下的改進A*算法三維無人機航跡規劃方法，能夠較為精確地滿足任務中對于起始和到達時方向以及飛行時間的約束，使無人機以特定角度和在預定時間到達終點，并且規劃出的較優航跡。

4 結語

實際戰場中環境是復雜的，任務要求是多樣的，針對真實戰場的復雜環境，在構建三維環境模型以及滿足無人機性能約束的基礎上，提出了一種能夠滿足方向和時間約束的基于A*算法的無人機航跡規劃方法。根據朝向角計算出新的起點和終點，更滿足對方向上的約束；改進稀疏A*算法，采用變權值代價函數并引入加速度代價和威脅代價，滿足時間約束條件，且提高了航跡的安全性；雙向搜索，選出較優航跡輸出。仿真結果表明，該方法能夠解決無人機在方向和時間約束下的三維航跡規劃問題，并能在滿足無人機性能限制的條件下有效地規避地形障礙和雷達威脅，提高作戰安全性。