結合B樣條優化的UAV多區域路徑規劃融合算法

汪瀚洋，盧厚清，陳 亮，趙小康，楊 柳

(1.陸軍工程大學 野戰工程學院，南京 210007; 2.軍事交通學院 汽車士官學校，安徽 蚌埠 233011)

0 引言

無人機(UAV，unmanned aerial vehicle)作為新型智能化作戰力量，具有較低的應用成本的同時，也具有較好的機動性，其在戰場上被廣泛使用于偵察任務。在實際復雜戰場環境中，一般都有多個任務區域等待無人機去執行信息偵察任務。無人機需要規避戰場中的威脅區域(這些區域中存在敵防空火力等威脅)對各個目標區域進行遍歷偵察。如何快速規劃出保證無人機遍歷各個任務區域并安全返回原基地的最優路徑是亟待解決的問題，該情形可被視為類旅行商問題(TSP，travelling salesman problem)，其本質是無人機自主導航的需要。

當前國內外學者已提出了大量無人機路徑規劃算法。在主要文獻中提到的方法有：1)路圖法，如A*、D*算法;2)概率法;3)勢場法；4)智能優化算法，如蟻群算法等。需要指出的是，TSP問題因其本身的特性，目標點如果比較多，在進行窮舉求解時，會出現組合爆炸的問題，所以近似求解是常被采取的方法。蟻群算法是眾多智能優化算法中最經典的算法之一，其常被用于解決TSP問題，并取得了良好的計算效果。該算法具有良好的魯棒性和穩定性，同時在與其他算法融合方面也有良好的表現，在路徑規劃、自主導航等問題上有著廣泛應用。

A*算法是一種常見且應用廣泛的搜索尋徑算法，其特點在于對評價函數的定義上。在一般的有序路徑搜索中，該算法總是選擇綜合代價值最小的路徑節點作為擴展節點，優點是可實現路徑的快速規劃。文獻[1]提出了一種使用圓形鄰域節點擴展法的A*算法改進，提高了搜索效率，但是沒有解決生成路徑貼近或者斜穿障礙物的問題，給無人機的生存安全帶來隱患。文獻[2]提出了一種混合蟻群算法解決靜態環境下的機器人路徑規劃問題，避免了蟻群算法陷入局部最優，但是凸顯了蟻群算法收斂速度較慢的缺點。上述文章都體現出一個問題：即單獨使用蟻群算法或A*算法，都不能很好解決本文情況下的TSP問題。蟻群算法易出現局部最優和陷入遲滯，而A*算法會由于僅僅規劃兩兩目標之間的局部最優路徑，以及組合爆炸的問題，并不一定保證可以規劃出全局最優遍歷路徑。本文通過蟻群算法融合改進A*算法來解決無人機多目標區域遍歷路徑規劃的問題，并結合改進三次B樣條方法平滑規劃路徑。仿真結果表明,融合算法能有效解決問題,在算法規劃速度、路徑質量及安全度上有明顯改善,相比傳統算法有明顯優勢。

1 路徑規劃問題描述與環境建模

1.1 問題描述

無人機多目標區域遍歷問題本質是一個類TSP問題，如圖1所示，無人機從基地出發前往各個目標區域執行偵察任務。在存在威脅區的偵察任務環境中，無人機不僅需要規劃兩兩目標區域之間的路徑，還需要規劃到各個目的地的先后順序，在保證自身安全返回的同時力求全局路徑的最優化，以獲取更高的偵察收益。

圖1 問題描述

為方便描述無人機在偵察任務中規避威脅區域并遍歷各個任務區的問題，將該問題簡化描述如下：

1)無人機可看作一個可操縱質點；

2)執行偵察任務中，無人機保持穩定飛行高度及速度，環境為二維空間；[3]

3)基于柵格法建立地圖模型，全局靜態威脅區域范圍及任務區位置已知。無人機從給定起始基地位置出發，規避環境中的威脅區并規劃出一條遍歷所有偵察任務區域并最后返回基地的安全航路。

1.2 環境建模

基于柵格地圖的環境建模具有建模復雜性低、適應能力強、易于實現與存儲等優點。柵格地圖將三維空間降維為二維平面，并將整個平面劃為大小相等的各個柵格。整個二維平面由柵格取值為二進制0-1矩陣所構成，利用0-1矩陣進行判斷柵格是否可通行。矩陣中數值為0代表無人機可以正常通行，反之數值為1則為威脅區并不可通行[4]。具體對應關系如圖2所示。

圖2 柵格設置

在實際環境中，威脅區會呈現出不同的形狀，在柵格地圖中構建威脅區時，如按照威脅區的原形狀進行建立，如圓形、多邊形甚至不規則形等，將會導致障礙物邊緣出現失真[5]，從而導致搜索效率和路徑安全性的下降。

在現有研究中，柵格地圖大多使用矩形外包原形狀以避免失真情況的發生。因此，本文中的威脅區統一使用“外包”進行處理，即分別將原有規則或不規則形狀統一進行柵格填充，當威脅區不滿足一個柵格大小，將其填充為一個柵格。圖3中分別為處理前后的威脅區形狀，這樣只要保證規劃路徑不進入填充后的威脅區，即可保證規劃路徑安全性。

圖3 威脅區形變

無人機在柵格環境中運動，圖4顯示了無人機的運動情況。各箭頭所指方向為無人機初始可擴展方向，即初始8鄰域節點擴展。現在假設無人機在二維平面區域S內移動，設S的左底角為原點O，以水平方向為x軸，垂直方向為y軸，建立平面直角坐標系xOy，如圖5所示。對于該25個柵格集合，其中任一柵格都有確定坐標和對應的序號，則無人機在柵格地圖中的地圖坐標和序號位置對應關系如下:

圖4 運動環境

圖5 柵格坐標

(1)

其中:mod表示取余運算;ceil表示求整運算;nx、ny表示每行每列的柵格數;xi、yi表示柵格i的中心點橫坐標和縱坐標[6]。

2 算法改進及融合

2.1 蟻群算法

螞蟻在運動中,會在其所經過的路徑上留下一種被稱為信息素的物質,這種物質是各螞蟻之間進行信息交流的載體。螞蟻在運動中會感知到這種物質，并循跡此物質進行運動,同時爬行過程中同樣釋放信息素。某一路徑上的信息素濃度越高,螞蟻將以更高的概率選擇此路徑進行運動,使得該路徑上的信息素濃度繼續增高,至此便出現一種信息正反饋現象。某路徑上運動過的螞蟻越多,后來者選擇該路徑進行運動的可能性就越大[7]。

蟻群算法的基本原理，即基于螞蟻根據路徑上同類釋放的信息素濃度進行路徑選擇，距離短的路徑在理論上會更多地被選擇，經過迭代和避免局部優化后，可得到全局最優路徑。蟻群算法可以有效求解TSP問題，在本任務環境中，算法數學模型如下：

(2)

其中:αk表示待偵查區域的集合，α表示信息度啟發因子，β為啟發函數的重要程度因子,ηij=1/dij是從目標區i到目標區j的啟發式因子。

在所有螞蟻選擇完路徑后，路徑上的信息素濃度更新為：

(3)

(4)

其中:Q為信息素常量，表示1次循環中，螞蟻k所釋放的信息素總量。Pathk為螞蟻k的路徑集合,Lk表示第k只螞蟻本次循環中途徑的路程總長[8]。

所有螞蟻完成一次路徑選擇后，根據路徑上的信息素濃度記錄本次最佳路徑，更新路徑信息量。經過指定次數循環后，輸出最終路徑的優化結果。

2.2 A*算法

A*算法是一種啟發式搜索算法，其常被用來解決無人機路徑規劃問題。其是在Dijkstra算法的基礎上引入評價函數機制，將路徑搜索代價進行綜合考慮，能有效解決全局靜態環境下最短路徑的搜索問題。

A*算法通過評價函數來確定到目標節點搜索方向，路徑由當前節點到目標節點的評價函數f(n)的最小值來確定。傳統A*算法在進行路徑規劃時會設置兩個列表，一個是Open list表，其用來保存準備搜索的節點，另一個是Closed list表，用來存放已經被搜索到的截至目前最小路徑搜索代價的點。探索過程中，先從Open list中找到路徑搜索代價最小的點設為當前節點，將其放入Closed list，然后對其進行擴展搜索，將擴展搜索后得到的節點更新到Open list中，再從Open list選取搜索代價最小的點設為當前節點，重復過程，直到找到目標點。

A*算法的評估函數為：

f(n)=g(n)+h(n)

(5)

其中:n為柵格中某節點；g(n)為起點到該點的最短路徑的代價函數；h(n)為該點達到目標點最短路徑代價函數。

在柵格地圖中，評價函數的選擇在一般情況下根據移動體可擴展的方向而定。如果移動體只能向4個方向擴展，這時曼哈頓距離是計算評價函數的最佳選擇；如果移動體可以向8個方向擴展，這時評價函數使用切比雪夫距離計算更加合適；如果移動體可以在任意方向移動，則評價函數選擇歐幾里得距離計算比較有優越性。具體如圖6所示。

圖6 評價函數計算

考慮到無人機實際運行軌跡的任意性[9]，本文中A*算法的評估函數中的h(n)采用歐氏距離為代價值進行計算。設當前節點坐標為(xn，yn),目標點坐標為(xg，yg)，歐幾里得距離表示兩坐標的最短距離，其公式為:

(6)

2.3 改進A*算法

A*算法的規劃路徑存在斜穿威脅區柵格頂點、斜穿兩個威脅區柵格相接點的現象，這是由于該路徑的擴展代價最低，且符合節點擴展規責造成的，容易導致路徑發生進入威脅區的風險，路徑如圖7所示。

圖7 非法路徑

針對傳該問題，本文對節點選擇規責進行重新定義：

圖8 節點分類

1)當威脅區位置在第Ⅰ類節點時。其中，當威脅區位置在節點2時，則不選擇節點1、3；威脅區位置在節點7時，則不選擇節點6、8。

2)當威脅區位置在第Ⅱ類節點時。其中，當威脅區位置在節點4時，則不選擇節點1、6；威脅區位置在節點5時，則不選擇子節點3、8。

經過初步試驗，上述優化路徑如圖9所示。改進后的路徑可以有效避免無人機進入威脅區。

圖9 優化路徑

2.4 蟻群-A*算法融合

蟻群算法在解決TSP問題時，是基于目標區的坐標來計算的，即此時各目標區之間的距離是已知的。在本文柵格地圖中，由于各任務區之間距離未確定、環境中存在威脅區不可通過等原因，蟻群算法在計算各任務區之間的距離時，需要反復迭代，且因為蟻群算法本身計算時間長、啟發效果差和易陷入局部最優等原因，會導致全局規劃路徑耗時較長[10]。

A*算法同樣不能很好地單獨解決TSP問題。A*算法通過找到任意兩個目標區之間的最短距離的方法搜索全局路徑,會導致結果不一定是全局范圍中的最優路徑。另外，不同于蟻群算法解決TSP問題屬于近似計算的范疇，A*算法屬于精確計算，當節點數很少時，A*算法進行路徑窮舉可以解決，但實際問題中，節點數往往很多。例如，對于一個僅有16個城市的旅行商問題，若采用A*算法進行窮舉來求解問題的最優解，可行解共有15!/2=653 837 184 000個[11]。在1993年，使用當時的工作站用窮舉法求解此問題需時92小時。即使現在計算機速度快，面對復雜的問題，效率仍然不夠。這就是所謂的“組合爆炸”，即所需要處理的數據因指數級的增長而變得難以處理。所以科學家逐步尋找近似算法或者啟發式算法，目的是在合理的時間范圍內找到可接受的最優解。

總之，蟻群算法的優勢在于解決TSP問題的全局路徑規劃，而A*算法可以很好地解決TSP問題中兩兩目標區之間的局部路徑規劃[12]。為了提高算法可行性和運行效率，本文考慮使用融合算法，即使用改進A*算法快速計算各任務區之間距離[13]，然后使用蟻群算法規劃全局路徑[14]。

3 基于改進三次B樣條法的路徑平滑處理

3.1 平滑策略

在柵格地圖中，規劃路徑由柵格中心點連線組成，這導致A*算法生成的路徑存在轉折拐點多的問題，客觀上給無人機運動增加了多余路徑長度，且不符合無人機的動力學原理(無人機存在最大轉彎角等)。本文選擇改進的三次B樣條方法對規劃路徑進行平滑處理，即先使用雙向插點法處理路徑，再使用三次B樣條法平滑路徑。

3.2 雙向插點法

雙向插點法核心思想是在Floyd算法的基礎上，運用雙向優化的理念，刪除冗余節點，設置安全距離，以最大程度減少路徑拐點。在優化中，設置安全距離，通過比較威脅區到路徑的垂直距離與設置的安全距離的關系來判斷路徑的安全性。如圖10，設路徑節點S坐標為(xs,ys)，T坐標為(xt,yt)。威脅區幾何中點O坐標(xo,yo)；設柵格邊長為d；距離OA為O到線段ST之間的垂線距離，記為d2；線段OB為O到線段ST縱向距離，記為l；夾角α為線段ST與水平方向夾角；記外接圓半徑為r。

圖10 安全距離設置

則有：

(7)

設置安全距離為D(D為設置的以O為起點的線段長度，D>r),以保證處理后的路徑不進入威脅區，即將D與d2進行比較：如d2≤D，則該路徑不可被選擇。如d2>D,則該路徑可被選擇[15]。

雙向插點法具體步驟如圖11所示。

圖11 雙向插值法

步驟1：對路徑中同一線段上的冗余點進行刪除，只保持路徑轉折前的起點S、下一個目標點T。S→N1→N2→N3→N4→N5→N7→T刪除冗余點后的路徑為S→N2→N3→N4→T。

步驟2：從路徑起始點S起算，在保留的Ni、Nj之間每步取一節點，步長記為q，判斷取得節點與上一節點之間是否存在威脅區域，并通過公式(7)計算d2，判斷d2與D的大小；如果威脅區域在安全距離之外，則選擇該節點為路徑節點；否則不予選擇。如圖所示，處理之后的路徑為S→N6→T。

步驟3：從目標點T方向出發，反方向進行取點判斷，如圖所示，處理后得到S→N8→T。

3.3 三次B樣條曲線優化

B樣條曲線常用于路徑規劃的平滑，其具有針對局部路徑修改的特點，并使用逼近多邊形的方法從而獲得曲線優化。結合前述雙向插點法，B樣條曲線優化的路徑會更為平滑[17]。B樣條曲線實際是貝塞爾曲線的一種特例，與貝塞爾曲線相比，B樣條曲線在保留貝塞爾曲線全部優點的同時，又克服了其缺乏局部性質、連續性較差等缺點，是路徑平滑常用的數學工具。其核心思想是通過控制點對路徑軌跡進行n次B樣條曲線處理。

廣義的樣條函數定義為：

給定一組平面上頂點(xi,yi)(i=0,1,…,n)，并設在區間[a,b]上的Δ：a=x0

1)在每個小區間[xi-1,xi](i=1,2,…,n)內，S(x)是具有K階或K階以上連續函數。

2)在xi(i=1,2,…,n-1)處成立。

則對于三次樣條函數，現在假設在區間[a,b]上給定一個分割Δ：a=x0

1)在每個小區間[xi-1,xi](i=1,2,…,n)內，S(x)是三次多項式函數。

2)在xi(i=1,2,…,n-1)處成立：

S(k)(xi-0)=S(k)(xi-0),k=0,1,2

(8)

即小區間上的三次多項式函數，在拼接點處xi具有二階連續拼接。

3)滿足條件yi=S(xi)，i=0,1,…,n

下面對B樣條曲線進行定義，給定m+n+1個平面或空間頂點(即控制點)Pi(i=0,1,…,m+n),稱為n次參數曲線段：

(9)

其頂點Pi所組成的多邊形稱為B樣條曲線的特征多邊形。其中，基函數Gi,n(t)定義為：

(10)

其中:t∈[0,1],i=0,1,…,n。

取n=3，則有三次B樣條曲線的基函數如下：

(11)

將公式(11)代入式(9)中可得：

P(x)=

(12)

將公式(12)以矩陣形式表示，則可得三次B樣條曲線段P0，3(t)為：

(13)

Pi頂點位置定義為：

(14)

(15)

利用上述公式可得滿足三次B樣條的控制點及優化曲線。平滑結果如圖12所示，圖中虛線即為處理后的實際軌跡。

圖12 三次B樣條曲線優化

4 算法流程

本文融合算法結構如圖13，步驟如下。

圖13 算法流程

步驟1:初始化柵格地圖，并設定n個目標區域。

步驟2:基于改進A*算法進行各目標區之間路徑規劃，得到任意兩個目標區之間的最短距離。

步驟3:初始化蟻群算法各參數。令時間t=0和循環次數Nc=0，設置最大循環次數Nmax，將m個螞蟻置于n個目標區上，令有向圖上每條邊(i，j)的初始信息素τij(t)=c，其中c表示常數，且初始時刻Δτij(0)=0。

步驟4:循環次數Nc=Nc+1。

步驟5:螞蟻禁忌表索引號k=1。

步驟6:螞蟻個體根據狀態轉移概率公式(2)計算的概率選擇下一個目標區j并前進，j∈ak。

步驟7:修改禁忌表指針，即選擇之后螞蟻移動到新的目標區，并把該目標區移動該螞蟻個體的禁忌表中[18]。

步驟8:所有螞蟻完成移動，記錄本次最佳路線。

步驟9:根據公式(3)和公(4)更新每條路徑上的信息素。

步驟10:若滿足結束條件，即如果循環次數Nc≥G，則循環結束并得到最佳遍歷路徑；否則清空禁忌表轉到步驟4。

步驟11:使用三次B樣條與插點法相結合的方法進行路徑平滑，最后輸出最佳路徑。

5 仿真實驗與分析

為驗證算法的性能，擬在MATLAB2018b環境中進行仿真實驗。仿真選擇基本蟻群算法(ACO)、蟻群-A*算法(ACO-A*)、蟻群-A*改進算法(ACO-IA*)和本文融合算法(ACO-MA*)。首先建立30×30 km的柵格地圖，并在地圖中設立20個目標區域進行實驗，目標區域如圖中淺色區域所示。參數初始化設置如下：螞蟻數量m=30,目標區域數量n=20，最大迭代次數200,信息素啟發因子α=1，啟發函數重要度因子β=5，ρ=0.3，Q=50，d=1 km，D=0.8 km，q=0.1 km。仿真結果如圖14所示。

圖14 實驗對比情況

從表1中可以看出，基本蟻群算法(ACO)、蟻群-A*算法(ACO-A*)、蟻群-A*改進算法(ACO-IA*)和本文融合算法(ACO-MA*)都能順利獲得路徑。但對比而言，ACO算法處理時間最長，且路徑轉角和路徑靠近威脅區次數最多，在實際應用中，無人機將消耗更多燃料，且無人機生存面臨更大威脅。ACO-A*算法相比ACO算法路徑長度增加了1 km，但轉角次數減少了12次，占比19.3%。處理時間顯著減少了24.33秒，占比92.7%。路徑靠近威脅區次數減少了2次，占比22.2%。ACO-IA*算法生成的路徑相比ACO-A*算法生成的路徑，路徑長度、轉角次數和處理時間幾乎相同，但有效避免了路徑靠近威脅區，顯著增加了無人機路徑安全性[19]。而ACO-MA*算法相比ACO算法路徑長度減少了6.13 km，占比4.2%。轉角次數減少了43次，占比69.4%。處理時間減少了24.3秒，占比92.9%，同時有效保證了路徑的安全性。且ACO-MA*算法相比于ACO-A*算法和ACO-IA*算法，除了處理時間有略微增加，各項數據指標均有明顯優化。考慮到本地圖僅是30×30 km的柵格地圖，如果地圖區域更大，融合算法的性能將更有顯著提升。如圖13所示，融合算法相比其他算法也更有良好的收斂性。

表1 實驗數據對比

圖15 收斂性對比情況

6 結束語

本文針對無人機多目標區域遍歷偵察問題，提出一種高效的遍歷路徑規劃融合算法，對無人機多任務區遍歷路徑進行快速規劃。針對傳統的蟻群算法和A*算法在解決TSP路徑規劃問題上均有缺陷的情況,通過兩種算法的改進與融合，使新算法相比于單個或其他融合算法有明顯優勢,證明了該算法的有效性和實用性。預計本文的研究成果將幫助智能無人機或決策者在復雜戰場環境中快速和高效地作出決策，提高無人機生存能力和偵察效益，更好地發揮無人作戰力量的優勢[20]。