俞瑞，沈亮

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

0 引言

無人機(jī)航跡規(guī)劃，指的是無人機(jī)在滿足性能、地形環(huán)境以及任務(wù)約束的條件下，尋找到從起始點(diǎn)到目標(biāo)點(diǎn)的可行航跡[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è)定的代價(jià)函數(shù)尋找最小代價(jià)的路徑，稀疏A*算法[5]是在傳統(tǒng)A*算法的基礎(chǔ)上，在擴(kuò)展節(jié)點(diǎn)時(shí)，通過考察各種約束條件，有效地縮減搜索空間和縮短搜索時(shí)間，能夠快速收斂得到所求路徑。

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

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

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

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

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

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

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

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

（1）最小步長約束

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

（2）最大航程約束

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

（3）最大偏航角約束

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

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

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

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

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

預(yù)警雷達(dá)在戰(zhàn)場中應(yīng)用廣泛，其威脅程度與雷達(dá)的最小半徑和最大半徑的距離有關(guān)，小于最小半徑的范圍屬于禁飛區(qū)，無人機(jī)是不能穿過的，否則被發(fā)現(xiàn)則此次任務(wù)失敗，大于最大半徑的范圍是雷達(dá)檢測不到的，屬于安全區(qū)，而在兩個(gè)半徑之間的范圍，雷達(dá)有一定的概率探測到無人機(jī)，這個(gè)概率與無人機(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è)定的估價(jià)函數(shù)尋找最小代價(jià)的航跡，能夠有效地引導(dǎo)無人機(jī)飛向目標(biāo)點(diǎn)。稀疏A*算法是在傳統(tǒng)A*算法的基礎(chǔ)上，在擴(kuò)展節(jié)點(diǎn)時(shí)，通過考察各種約束條件，有效地縮減搜索空間和縮短搜索時(shí)間，能夠快速收斂得到所求航跡。構(gòu)建代價(jià)函數(shù)[9-10]：

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

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

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

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

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

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

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

（2）引入加速度代價(jià)

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

（3）引入威脅代價(jià)

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

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

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

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

（4）計(jì)算新的起點(diǎn)和終點(diǎn)

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

（5）雙向搜索對比

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

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

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

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

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

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

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

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

3.2 仿真對比

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

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

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

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

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

圖3

圖4

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

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

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

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

圖5

3.3 分析

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

4 結(jié)語

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