薄　寧， 李相民， 代進進， 唐嘉鈺

(海軍航空大學(xué), 山東 煙臺　264001)

無人機(Unmanned Aerial Vehicle, UAV)由于其獨有的成本、機動性等優(yōu)勢,目前已被廣泛用于戰(zhàn)術(shù)壓制敵防空系統(tǒng)、偵察搜索、定位跟蹤、戰(zhàn)毀評估等枯燥、危險、惡劣的戰(zhàn)術(shù)任務(wù)[1]。靈活高效的自主航跡規(guī)劃是無人機系統(tǒng)生存與成功的核心基礎(chǔ)之一[2],其成為近年來理論研究的重難點問題之一。

UAV航跡規(guī)劃可分為路徑規(guī)劃和軌跡規(guī)劃。路徑規(guī)劃結(jié)果是與時間無關(guān)的靜態(tài)幾何路徑。軌跡規(guī)劃比路徑規(guī)劃更為細(xì)化,規(guī)劃結(jié)果是與時間相關(guān)的三維空間曲線。路徑規(guī)劃主要求解方法分為兩類：一類是確定型搜索算法,如D*算法、A*算法、Dijkstra算法[3]等;一類是隨機型搜索,如遺傳算法、粒子群算法等。但路徑規(guī)劃方法一般不考慮UAV動力學(xué)與運動學(xué)約束,結(jié)果較為粗略。軌跡規(guī)劃方法分為直接法與間接法兩種,直接法又可分為可行方向法、非線性規(guī)劃法等;間接法如牛頓法,最速下降法、共軛梯度法等[4]。軌跡規(guī)劃方法可以求得UAV運動控制的較優(yōu)解,但隨著避障、避碰、多機協(xié)同等約束的增多以及規(guī)劃地域范圍的擴大,其求解復(fù)雜度增大,實時性降低。例如,文獻(xiàn)[5]考慮UAV動力學(xué)運動學(xué)約束、避障約束,建立了UCAV武器投放軌跡規(guī)劃最優(yōu)控制問題框架,使用Gauss偽譜法求解,仿真結(jié)果表明,在使用較少插值點的情況下,計算時長仍在十幾秒左右。

針對上述問題,本文提出了一種路徑規(guī)劃與軌跡規(guī)劃相結(jié)合的多UAV實時航跡規(guī)劃層次結(jié)構(gòu)策略。在路徑規(guī)劃階段,考慮避障等約束,建立了路徑規(guī)劃問題模型,針對傳統(tǒng)稀疏A*算法中步長固定的限制,提出了一種連續(xù)可變步長的SAS算法,對問題進行了求解,得到由中間航路點連接而成的多條航路段;然后,針對任一中間航路段實時軌跡規(guī)劃過程,設(shè)計了基于DMPC思想的軌跡規(guī)劃問題求解框架,綜合考慮UAV運動學(xué)約束、避碰約束,建立了DMPC單步非線性規(guī)劃模型。仿真結(jié)果表明,該策略求解速度快,所得解較優(yōu),能夠滿足UAV在線實時航跡規(guī)劃需求。

1　問題分析與求解框架

由于UAV可以應(yīng)用于多種不同的戰(zhàn)場環(huán)境和戰(zhàn)術(shù)用途,在進行航跡規(guī)劃時,需要考慮的因素條件,例如優(yōu)化目標(biāo)、約束條件、協(xié)同要求、時限要求等各不相同,首先對所研究的問題背景進行分析和描述,然后選擇優(yōu)化問題所建立的模型和求解方法,并確定問題的整體求解框架。

1.1　問題分析

本文研究的問題背景是多UAV協(xié)同執(zhí)行任務(wù)中的航跡規(guī)劃問題。每架UAV上除掛載任務(wù)載荷外,還具有一定范圍內(nèi)的數(shù)據(jù)通信機制以進行相鄰UAV間的狀態(tài)信息通報。本文研究的內(nèi)容為:UAV在分別接收所分配的任務(wù)后,從當(dāng)前時刻點到任務(wù)點航線飛行中考慮避障、避碰、到達(dá)時間等多種約束的航跡規(guī)劃問題。航跡規(guī)劃考慮主要因素有:1)障礙物規(guī)避約束,如山川地形障礙、氣象禁飛區(qū)、敵方雷達(dá)等;2)任務(wù)執(zhí)行過程中的突發(fā)威脅,包括實現(xiàn)通過偵查情報已經(jīng)獲知的敵方預(yù)警雷達(dá)、防空系統(tǒng),以及在執(zhí)行任務(wù)過程中偵察發(fā)現(xiàn)或者接收的敵方威脅;3)己方UAV間的避碰以及UAV與有人機間的避碰問題;4)計算實時性要求。

1.2　求解框架

根據(jù)對于UAV編隊協(xié)同作戰(zhàn)航跡規(guī)劃問題的分析,本文設(shè)計了UAV層次化航跡規(guī)劃結(jié)構(gòu),每架UAV上運行的航跡規(guī)劃框架如圖1所示。

圖1　層次化航跡規(guī)劃結(jié)構(gòu)

方案采用完全分布式求解策略,每架UAV無須存儲全局戰(zhàn)場態(tài)勢,而是僅存儲任務(wù)執(zhí)行區(qū)域相關(guān)的戰(zhàn)場態(tài)勢信息。每架UAV接收任務(wù),負(fù)責(zé)為自身平臺進行航跡規(guī)劃。多UAV間的時間協(xié)同關(guān)系由任務(wù)管理單元進行規(guī)劃前解耦。第一步為路徑規(guī)劃過程,此階段根據(jù)指控節(jié)點提供的分配任務(wù)以及本機狀態(tài),態(tài)勢管理單元提供的各類障礙物,協(xié)同UAV任務(wù)完成情況等各種信息,將其轉(zhuǎn)換為路徑規(guī)劃問題模型,并使用連續(xù)可變步SAS算法進行求解,將生成的由多個中間航路點組成的航路規(guī)劃結(jié)果交至第二階段的航線存儲單元。同時,路徑規(guī)劃過程根據(jù)一定的觸發(fā)條件(例如任務(wù)改變、新的威脅區(qū)出現(xiàn)等)進行航跡重規(guī)劃判斷,若滿足條件,則進行航跡在線規(guī)劃以滿足態(tài)勢變化。經(jīng)過第一階段處理后,整個航跡被分解為多段小的航路段,再在每一航路段上運行第二階段的軌跡規(guī)劃過程。

第二個過程為軌跡規(guī)劃過程,軌跡規(guī)劃過程基于模型預(yù)測控制(Model Predictive Control, MPC)思想,在每個時域周期內(nèi),根據(jù)預(yù)先建立的UAV運動預(yù)測模型,推導(dǎo)出預(yù)測域內(nèi)的UAV多步狀態(tài),然后通過避碰管理單元輸出的本時刻避碰約束,采用軌跡規(guī)劃中的直接法,建立非線性規(guī)劃問題模型并進行求解,所得解為多步控制向量序列,取出第一個向量交由UAV控制器進行執(zhí)行。整個流程在一個步長周期內(nèi)運行一次,不斷循環(huán),從而構(gòu)成一個滾動在線規(guī)劃過程。

2　路徑規(guī)劃過程

路徑規(guī)劃過程部分綜合考慮地形、禁飛區(qū)、敵方威脅等情況,為UAV生成一條可飛航路并根據(jù)環(huán)境變化進行重規(guī)劃。本文不考慮UAV的飛行高度變化,假設(shè)所有UAV均在同一高度平面內(nèi)飛行。

2.1　環(huán)境建模

首先在UAV任務(wù)空間建立直角坐標(biāo)系,并對整個任務(wù)區(qū)域的戰(zhàn)場環(huán)境進行建模,將整個戰(zhàn)場劃分為若干正方形柵格,柵格的邊長依據(jù)式(1)確定[6]。

(1)

其中,D為網(wǎng)格邊長,δN為UAV導(dǎo)航系統(tǒng)誤差,δM為數(shù)字地圖誤差,δA為其他誤差。K為兼顧魯棒性和算法運算速度的系數(shù)。

文獻(xiàn)[6]提供了航路規(guī)劃時,地形高程、威脅危險、戰(zhàn)略回避三種威脅區(qū)的單元格代價計算方法,本文為簡化計算,將地形障礙、禁飛區(qū)、威脅區(qū)等統(tǒng)一處理為禁飛區(qū),規(guī)劃航路不得穿越標(biāo)識為禁飛區(qū)的單元格,定義單元格代價為

(2)

其中,(xi,yj)為單元格坐標(biāo),C(xi,yj)為單元格(xi,yj)的代價值。

2.2　問題建模

航路規(guī)劃的一般問題模型為尋找一條從起始點到終點的航路,使得經(jīng)過航路的代價最小。由于本文中將安全區(qū)單元格代價定義1,威脅區(qū)單元格代價定義為無窮大,則航路總代價最小等效為航路路徑在滿足避障的條件下路徑總長度最小,則問題可描述為

(3)

其中,Np為UAV規(guī)劃的航路點數(shù)量,Lineseg(j+1,j)表示第j個航路點與其下一航路點連接而成的線段,Intersect()為相交性判斷函數(shù),其值為0表示線段(j+1,j)與單元格cellk不相交。

2.3　改進的SAS求解算法

A*算法是一種經(jīng)典的啟發(fā)式搜索算法,被廣泛應(yīng)用于路徑搜索問題中。隨著研究的不斷深入,其得到了不斷完善和改進。如SAS算法[7]、動態(tài)A*算法(Dynamic Sparse A* Search, DSAS)[8]等。A*搜索通過在每一次迭代中選擇搜索空間中啟發(fā)函數(shù)值最小的節(jié)點進行擴展。其代價函數(shù)形式為

f(x)=a·g(x)+b·h(x)

(4)

其中,g(x)表示從起始位置到當(dāng)前位置x的真實代價;h(x)表示從當(dāng)前位置到目標(biāo)點的預(yù)計代價;a,b為真實代價與預(yù)計代價的權(quán)值。

傳統(tǒng)的SAS算法需要事先選定固定步長,算法的效率一定程度受步長選取效果影響。當(dāng)選取步長較小時,可求得較為精確的解,但是搜索迭代次數(shù)變大,搜索效率降低。若步長較大,遇到障礙物較密的情形時,有可能發(fā)生繞路,特殊情況下可能導(dǎo)致規(guī)劃失敗。文獻(xiàn)[9]對算法進行了改進,其思想是在絕對安全區(qū)域使用大步長搜索,在“緊迫環(huán)境”下使用小步長搜索,可以在搜索速度與解的質(zhì)量之間進行折衷,但其通過使用緊迫度閾值判斷的方法,由大步長直接跳轉(zhuǎn)至小步長,仍不夠靈活,且尚未對大步長與小步長兩種情況下搜索子扇形區(qū)的數(shù)量進行討論。本文借鑒文獻(xiàn)[9]變步長基本思想,設(shè)計了連續(xù)可變步長和搜索區(qū)域聯(lián)合調(diào)整方法,使變步長方案更為合理。

首先對子扇形區(qū)數(shù)量進行確定。假設(shè)UAV速度大小不變,使用下式確定搜索扇面角:

(5)

其中,L為步長,v為UAV速度大小,ωmax為UAV最大角速度,ρ1為調(diào)節(jié)比例系數(shù)。則步長為L時,進行一步搜索擴展的子扇形區(qū)數(shù)量為

(6)

其中,Δθ為子扇形區(qū)扇面角。根據(jù)公式(6)可以得出,假設(shè)擴展搜索的子扇形區(qū)扇面角不變,則待擴展的子扇形區(qū)數(shù)量與步長成正比。

然后確定搜索步長。定義最大步長與最小步長的關(guān)系為

Lmax=Lmin+mmax·ΔL

(7)

其中,Lmax預(yù)定義的搜索最大步長,Lmin為最小步長,mmax為最大步長對應(yīng)的搜索子扇形區(qū)數(shù)。ΔL為單位變換步長。

從待擴展表中取出節(jié)點進行擴展時,首先使用最大步長進行擴展試探,然后根據(jù)穿越禁飛區(qū)的子扇區(qū)數(shù)量確定搜索步長:

(8)

其中,mf為使用最大步長試探時落入禁飛區(qū)的擴展節(jié)點數(shù),ρ2為步長調(diào)節(jié)系數(shù),滿足1≤ρ≤mmax。

其他步驟同傳統(tǒng)的SAS算法相同。改進的算法流程圖如圖2所示。

圖2　改進的變步長SAS算法流程圖

3　軌跡規(guī)劃過程

航跡規(guī)劃過程得到的結(jié)果,送至UAV軌跡規(guī)劃模塊的航跡存儲單元,每次從中取出一個中間航路點,作為目標(biāo)點,目標(biāo)狀態(tài)的速度、航向角可不做嚴(yán)格約束。以當(dāng)前時刻狀態(tài)(位置,速度,航向角)為初始狀態(tài),主要考慮UAV性能、協(xié)同避碰管理等約束,對UAV進行軌跡規(guī)劃。UAV到達(dá)中間航路點后,再從航跡存儲單元中取出下一航路點作為目標(biāo)點進行軌跡規(guī)劃過程,不斷進行,直至到達(dá)UAV航跡最后一個航路點。

文獻(xiàn)[11]采用分布式模型預(yù)測控制(Decentralized Model Predictive Control, DMPC)結(jié)構(gòu)進行UAV軌跡規(guī)劃控制。對UAV質(zhì)點運動學(xué)模型離散化后,以UAV速度大小、航向角速度大小為輸入,推導(dǎo)出了UAV位置預(yù)測模型;以能力-時間組合最優(yōu)為目標(biāo)函數(shù),綜合考慮無人機性能、協(xié)同避碰控制規(guī)則、避碰距離等約束條件,建立了MPC滾動優(yōu)化過程中的單步優(yōu)化問題模型;設(shè)計了UAV避碰管理單元,采用交互圖更新機制解決多碰撞管理問題了,形成了一套完整的基于MPC的UAV編隊協(xié)同軌跡規(guī)劃方案。本文直接采用其軌跡規(guī)劃方案。在每一采樣時刻,MPC通過求解一個有限時域優(yōu)化問題,得到系統(tǒng)當(dāng)前時刻的最優(yōu)控制序列,從而實現(xiàn)在整個控制時域內(nèi)的在線閉環(huán)控制。UAV協(xié)同軌跡規(guī)劃DMPC控制器滾動優(yōu)化工作過程如圖3所示。

圖3　UAV分布式協(xié)同軌跡規(guī)劃MPC工作過程

其中,本機狀態(tài)由本機傳感器獲取并傳送至MPC控制器,通過預(yù)測模型得到帶有控制量參數(shù)的預(yù)測狀態(tài),由此得到滾動在線優(yōu)化問題的一步,通過一定的算法對一步優(yōu)化問題進行求解后,得到關(guān)于UAV速度、角速度大小控制量的優(yōu)化結(jié)果,并將結(jié)果輸出至UAV動作控制單元。

4　仿真分析

采用計算機仿真驗證層次化航跡規(guī)劃方案的性能。對于路徑規(guī)劃過程,主要對比本文方法與其他方法的性能比較。對于軌跡規(guī)劃過程,首先通過本文路徑規(guī)劃方法得到多架UAV規(guī)劃航路,然后將航路作為軌跡規(guī)劃過程的輸入,驗證本文方案在多UAV協(xié)同軌跡規(guī)劃過程的可實現(xiàn)性及性能,進而總體驗證本文層次化航跡規(guī)劃方案的有效性。仿真程序運行計算機基本配置:Intel Core i3 3.1GHz、3G內(nèi)存。仿真平臺為MATLAB R2012b。

4.1　路徑規(guī)劃過程仿真

基本參數(shù)設(shè)置如下:

D=500m,ωmax=0.2rad/s,v=80m/s,ρ1=0.2,Δθ=π/24rad,Lmax=5000m,Lmin=1000m,ΔL=200m,ρ2=1。戰(zhàn)場區(qū)域為0km×100km的矩形區(qū)域,經(jīng)柵格化后,變?yōu)?×200的二維搜索空間,UAV起始點位于(10, 10),終點位于(195, 195)。本文分別對SAS算法使用三類步長方案進行仿真比較。第一類是固定步長,分別取步長為5000m(Lmax)、4000m、3000m、2000m、1000m(Lmin);第二類是文獻(xiàn)[9]的雙步長閾值切換方法;第三類是本文的改進SAS方法。共計7種步長選取方案,為便于行文,分別命名為方案1至方案7。其中,方案1、方案5、方案6、方案7的計算機仿真結(jié)果分別如圖4、圖5、圖6、圖7所示。

圖4　方案1路徑規(guī)劃結(jié)果

圖5　方案5路徑規(guī)劃結(jié)果

圖6　方案6路徑規(guī)劃結(jié)果

圖7　方案7路徑規(guī)劃結(jié)果

7種方案在運算時間、生成路徑航路點數(shù)、生成航跡長度、open表與close表節(jié)點數(shù)等更為具體的結(jié)果對比如表1所示。

由上述圖表對比分析可以得出,傳統(tǒng)固定步長的SAS算法大步長搜索時求解速度最快,航路點數(shù)最少,close表和open表存儲空間占用也最少,但是所得解在四種方案中最差。固定小步長優(yōu)缺點正好與之相反,求得解較優(yōu),但是計算時間、空間占用等均大幅增加。方案6使用的通過閾值來控制在大、小兩種步長間切換,經(jīng)試驗仿真,改進效果并不明顯,并且仿真中發(fā)現(xiàn),方案3對于閾值的選取要求較高,在閾值選取不精確的情況下,容易導(dǎo)致尋徑搜索失敗。本文的方案在計算時間、解質(zhì)量、存儲空間占用等性能上介于最大步長SAS算法和最小步長SAS算法之間,與方案3結(jié)果相近。但由于每步采用“試探-選擇”的方法確定步長,步長的大小根據(jù)試探結(jié)果在多個離散值上動態(tài)選擇,解決了SAS算法中確定性步長對于不同環(huán)境條件下求解質(zhì)量不穩(wěn)定的問題,對于不同條件的規(guī)劃環(huán)境具有非常好的適應(yīng)性,較之于固定步長SAS,其更適應(yīng)于環(huán)境動態(tài)變化或信息部分已知的路徑規(guī)劃。

表1　不同方案下的運行結(jié)果對比

4.2　軌跡規(guī)劃過程仿真

為驗證軌跡規(guī)劃過程的有效性,對多無人機協(xié)同軌跡規(guī)劃過程進行仿真,假定整個飛行過程中UAV速度保持不變?；緟?shù)設(shè)置如下:

ΔT=1s,N=20,Nc=10,dif=5000m,ds=500m,

為簡化計算,假設(shè)MPC的預(yù)測控制序列中僅第一項不為0,即MPC預(yù)測控制序列中角速度輸入序列形如(ω1,0,…,0)。假設(shè)UAV獲取自身狀態(tài)信息是完全準(zhǔn)確的,即仿真中UAV當(dāng)前時刻獲取的狀態(tài)等于MPC在上一時刻模型計算輸出的本時刻狀態(tài)。

4架UAV初始信息、終點信息、參數(shù)設(shè)置等如表2所示。

表2　UAV參數(shù)表

其中,第一航路點位置由路徑規(guī)劃階段計算得出。這里僅對4架UAV從初始狀態(tài)到第一航路點進行軌跡規(guī)劃仿真。障礙物設(shè)置、區(qū)域大小、柵格大小等初始條件與4.1節(jié)完全相同。4架UAV的路徑規(guī)劃階段得到結(jié)果如圖8所示。

圖8　4架UAV航路規(guī)劃結(jié)果

其協(xié)同軌跡規(guī)劃結(jié)果如圖9所示。

使用航向角變化規(guī)則進行分布式自主協(xié)同軌跡規(guī)劃,UAV速度大小保持不變。由圖9中可以看出,4架UAV可以有效進行分布式協(xié)同軌跡規(guī)劃, 4架UAV間距始終保持在大于安全間距。

5　結(jié)束語

本文采用路徑規(guī)劃與軌跡規(guī)劃相結(jié)合的層次化航跡規(guī)劃方法,將UAV執(zhí)行任務(wù)中,航線飛行階段的航跡規(guī)劃過程,眾多的避障約束和UAV協(xié)同約束等主要非線性約束部分交由路徑規(guī)劃部分處理,均衡路徑規(guī)劃過程和軌跡規(guī)劃過程的計算負(fù)擔(dān)。對于路徑規(guī)劃過程使用可變步長機制,改進了傳統(tǒng)SAS算法固定步長,難以滿足動態(tài)環(huán)境變化的不足。而軌跡規(guī)劃過程又可以為UAV在中間航路段過程提供精細(xì)的運動控制,而由于此時規(guī)劃地域范圍小,使得軌跡規(guī)劃計算量大為減輕,從而實現(xiàn)在能夠快速得到可行解的情況下實現(xiàn)對UAV的較優(yōu)控制。仿真結(jié)果表明,該方案能夠在動態(tài)環(huán)境條件下得到較優(yōu)解,算法實時性較好。

圖9　4架UAV協(xié)同軌跡規(guī)劃仿真結(jié)果

參考文獻(xiàn):

[1]沈林成,牛軼峰,朱華勇. 多無人機自主協(xié)同控制理論與方法[M]. 北京: 國防工業(yè)出版社, 2013.

[2]M.Valenti, T.Schouwenaars, Y.Kuwata,E.Feron, J. How. Implementation of a Manned Vehicle-UAV Mission System[C].AAIA Guidance, Navigation and Control Conference, Providence, RI, August 2004,AIAA2004.

[3]Jungert E, Holmes P D. A Knowledge-based Approach to the Shortest Path Problem in a Digtized Map[R]. IEEE Workshop on, Visual Languages, 1998.

[4]王維平,劉娟. 無人飛行器航跡規(guī)劃方法綜述[J]. 飛行力學(xué),2010, 28(2): 6-10.

[5]張煜,張萬鵬,陳璟，等. 基于Gauss偽譜法的UCAV對地攻擊武器投放軌跡規(guī)劃[J]. 航空學(xué)報, 2011, 32(97): 1240-1250.

[6]謝曉方,孫濤,歐陽中輝. 反艦導(dǎo)彈航路規(guī)劃技術(shù)[M]. 北京: 國防工業(yè)出版社, 2010.

[7]SZCZERBA ROBERT J. Robust algorithm for realtime route planning[J]. IEEE Transaction on Aerospace and Electronic Systems(S0018-9251), 2000, 36(3): 869-878.

[8]Guo Jianming, Liu Liang. A study of improvement of D* algorithms for mobile robot path planning in partial unknown environments[J]. Kybernetes, 2010, 39(6):935-945.

[9]黃文剛,張怡,姜文毅，等. 變步長稀疏A*算法的無人機航路規(guī)劃[J]. 計算機工程與應(yīng)用,2012, 48(29):206-209.

[10] 魏瑞軒,呂明海,茹常劍,等. 基于 DE-DMPC的 UAV編隊重構(gòu)防碰撞控制 [J]. 系統(tǒng)工程與電子技術(shù),2014, 36(12):2473-2478.

[11] 李相民,薄寧,代進進. 基于模型預(yù)測控制的多無人機避碰航跡規(guī)劃研究[J]. 西北工業(yè)大學(xué)學(xué)報,2017,35(3): 513-521.