方向和時間約束下的三維無人機(jī)航跡規(guī)劃

俞瑞，沈亮

（1.四川大學(xué)計算機(jī)學(xué)院，成都 610065；2.江西洪都航空工業(yè)集團(tuán)有限責(zé)任公司，南昌 330000）

0 引言

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

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

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

1 環(huán)境構(gòu)建

1.1 三維環(huán)境構(gòu)建

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

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

圖1 三維空間搜索擴(kuò)展原理

1.2 無人機(jī)性能約束

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

（1）最小步長約束

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

（2）最大航程約束

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

（3）最大偏航角約束

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

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

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

1.3 地形障礙和雷達(dá)威脅

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

預(yù)警雷達(dá)在戰(zhàn)場中應(yīng)用廣泛，其威脅程度與雷達(dá)的最小半徑和最大半徑的距離有關(guān)，小于最小半徑的范圍屬于禁飛區(qū)，無人機(jī)是不能穿過的，否則被發(fā)現(xiàn)則此次任務(wù)失敗，大于最大半徑的范圍是雷達(dá)檢測不到的，屬于安全區(qū)，而在兩個半徑之間的范圍，雷達(dá)有一定的概率探測到無人機(jī)，這個概率與無人機(jī)距離威脅中心的遠(yuǎn)近有關(guān)。雷達(dá)威脅度Tr表示為：

式（6）中，d為無人機(jī)到威脅中心的歐氏距離，Rmax為該威脅所影響的最大范圍半徑，Rmin為一定會受到威脅的區(qū)域半徑，在Rmin內(nèi)的區(qū)域則為禁飛區(qū)，該區(qū)域內(nèi)無人機(jī)不可達(dá)，在Rmax和Rmin內(nèi)的區(qū)域?qū)儆谕{區(qū)，該區(qū)域內(nèi)無人機(jī)存在著一定概率的威脅。

2 改進(jìn)A＊ 算法航跡規(guī)劃

2.1 基本思想

A*算法是一種啟發(fā)式搜索算法，傳統(tǒng)的A*算法是在規(guī)劃環(huán)境網(wǎng)格化的基礎(chǔ)上根據(jù)設(shè)定的估價函數(shù)尋找最小代價的航跡，能夠有效地引導(dǎo)無人機(jī)飛向目標(biāo)點。稀疏A*算法是在傳統(tǒng)A*算法的基礎(chǔ)上，在擴(kuò)展節(jié)點時，通過考察各種約束條件，有效地縮減搜索空間和縮短搜索時間，能夠快速收斂得到所求航跡。構(gòu)建代價函數(shù)[9-10]：

式（7）中，n表示當(dāng)前所在的節(jié)點，f(n)為從初始點經(jīng)由節(jié)點n到達(dá)目標(biāo)點所估計的總代價函數(shù)，g(n)是從起點到節(jié)點n的實際代價，h(n)是從節(jié)點n到目標(biāo)點的估計代價。

2.2 改進(jìn)A★算法航跡規(guī)劃

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

因此對傳統(tǒng)的A*算法進(jìn)行改進(jìn)，對航跡點的擴(kuò)展搜索方式、代價函數(shù)的設(shè)定、新航跡點的可行性判定、航跡優(yōu)化以及運算時間等進(jìn)行優(yōu)化改進(jìn)。

（1）變權(quán)值評估

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

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

（2）引入加速度代價

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

（3）引入威脅代價

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

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

針對各項參數(shù)的單位以及數(shù)量級上的差異進(jìn)行如下歸一化的處理：

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

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

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

（5）雙向搜索對比

將起點和終點的坐標(biāo)位置和對應(yīng)朝向角對調(diào)后輸入，重新進(jìn)行航跡規(guī)劃，得到另一條規(guī)劃航跡；對比所得兩條航跡，選其中總航跡代價值較小的一條作為最終航跡輸出。

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

3 仿真結(jié)果與分析

3.1 規(guī)劃區(qū)域

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

圖2 無人機(jī)航跡規(guī)劃流程

表1 雷達(dá)參數(shù)

表2 無人機(jī)性能約束

3.2 仿真對比

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

（1）改進(jìn)算法與稀疏A*算法比較

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

本文所提的改進(jìn)A*算法航跡規(guī)劃方法仿真結(jié)果如圖4。

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

圖3

圖4

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

表3 2 種算法航跡規(guī)劃結(jié)果比較

（2）雙向搜索結(jié)果對比

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

圖5

3.3 分析

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

4 結(jié)語

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