基于改進粒子群算法的無人機三維航跡規劃

陳明強，李奇峰，馮樹娟，徐開俊

(中國民用航空飛行學院 飛行技術學院，四川 廣漢 618307)

0 引言

隨著航空技術與智能技術的不斷進步，無人機在各個領域都有著良好的應用前景。無人機航跡規劃是實現無人機自主飛行的關鍵一步，得到了世界的廣泛關注和研究，目前仍是研究熱點之一。在考慮各種環境威脅和自身性能下，如何在當前環境中規劃出一條最佳的飛行路徑，是一項非常重要并復雜的任務[1-2]。

無人機航跡規劃算法可以大致分為2類:傳統規劃算法和智能規劃算法。傳統規劃算法包括A*算法[3]、快速擴展隨機數算法[4]、人工勢場算法[5]和概率圖算法[6]等。智能規劃算法包括遺傳算法[7]、粒子群(Particle Swarm Optimization，PSO)算法、蟻群算法[8]、動態窗口算法[9]和灰狼優化算法[10]等。PSO算法[11]是一種仿生智能算法，基本原理是利用群體與個體之間的協作和信息共享，得到最優解，具有搜索速度快和收斂速度快的特點，但是具有容易產生早熟和陷入局部最優值的缺點。

劉科等[12]利用Dijkstra算法粗略求得最短路徑后使用PSO算法和最小二乘法擬合出最優路徑。該算法雖然規劃出了最優路徑，但是只能適用于威脅和空間較小的環境。Zeng等[13]提出了一種基于非齊次馬爾可夫鏈和差分進化的PSO優化算法，用于智能機器人在環境中遇到障礙物時的路徑規劃。通過利用非齊次馬爾可夫鏈將PSO速度更新方程從一種模式跳躍到另外一種模式，加快了收斂的速度，同時差分進化增強了粒子全局搜索的能力，但是計算非常復雜且規劃路徑不夠平滑。羅陽陽等[14]提出了一種使用突變算子更新粒子位置的方法。在PSO算法迭代的過程中，使用突變算子使得算法能快速收斂并擺脫局部最優值。朱佳瑩等[15]提出了PSO與蟻群算法相結合的路徑規劃算法，利用PSO算法優化蟻群算法初始信息素，再使用改進后的蟻群算法進行全局路徑規劃。2種算法結合后全局搜索能力得到了較大提升，并且迭代次數更少，但是算法拐點較多，導致規劃路徑多為折線，不利于運行在實際環境中。

本文針對PSO算法的缺點，提出了一種將PSO算法搜索空間由原始的笛卡爾坐標系改為球坐標系，使得PSO算法能直接與無人機自身性能相關聯。同時引入天牛須(Beetle Antennae Search，BAS)算法，克服傳統PSO算法易陷入早熟、收斂速度慢的特點。

1 目標函數

對于無人機航跡規劃問題，目標函數是用來評估規劃航跡的質量，多是由一系列的約束條件和優化條件組成，目標函數值越小時，得到的航跡質量越好。為了確定目標函數，必須要考慮所有可能影響規劃航跡質量的因素，例如：航跡距離、飛行高度、障礙物限制和航跡平滑度等[16]。假設航跡是由n個航跡點組成，(xi,yi,zi)表示第i個航跡點的位置。

1.1 航跡距離

航跡長度是判斷規劃航跡質量的重要指標，考慮無人機的續航能力，規劃航跡越短越有利于無人機的實際飛行，因此航跡成本函數為：

(1)

1.2 飛行高度

無人機飛行過程中受自身性能的影響和任務的需要，一般飛行高度限制在最低飛行高度和最高飛行高度之間，無人機航跡飛行高度成本函數定義為：

(2)

1.3 障礙物限制

無人機飛行時需要距離障礙物一定的距離，假設空間中存在圓柱體障礙物，障礙物數量為T，無人機距障礙物距離為d，障礙物半徑為R，無人機尺寸為S，為了提升飛行時的安全，設定碰撞危險區距離為D，障礙物模型如圖1所示。

圖1 障礙物模型Fig.1 Obstacle model

障礙物成本函數可以定義為：

(3)

(4)

1.4 航跡平滑度

規劃航跡應該盡量少的偏航和高度切換，需要保持航跡的平滑，li表示2個航跡點之間的距離，式(5)和式(6)分別表示偏轉角ai和俯仰角bi，航跡平滑度成本函數可以定義為式(7)：

(5)

(6)

(7)

1.5 總目標函數

無人機在執行任務的過程中需要考慮自身性能限制與地形性質，因此對無人機俯仰角、偏轉角和飛行高度進行約束。以上約束條件參數均與成本函數參數有關，因此將飛行高度約束加入式(2)中，偏轉角和俯仰角約束在SPSO算法中對粒子更新時已經做出了約束，所以無需對其設置額外的約束條件。

根據以上成本函數，可以構建無人機規劃航跡的目標函數，由于無人機執行任務種類較多，并且每種任務所要求的航跡均不相同，所以定義wi，i=(1,2,3,4)分別為每種成本函數的系數，wi之間的關系如式(8)所示，根據任務的需求可以動態地調整權重，因此目標函數如式(9)所示：

(8)

F=w1Fl+w2Fa+w3Fo+w4Fs。

(9)

本文目標函數參數設定為：最高飛行高度hmax為300 m；最低飛行高度hmin為20 m；無人機尺寸S為2 m；危險區距離D為20 m；w1為0.4；w2，w3和w4均為0.2。

2 改進PSO算法設計

2.1 SPSO算法

PSO算法是由Kennedy和Russell提出，是基于鳥群和魚群聚集行為的啟發式優化算法。PSO算法具有認知和社會一致性，根據這2個特性，可以使得PSO中的粒子根據自己的經驗和種群的經驗自行搜索求解，從而擺脫了原始的進化算子。與其他的啟發式算法相比，PSO算法對初始條件和目標函數的變化并不敏感，能夠在適應多種環境的同時使用少量的參數，在較短的時間內獲得具有收斂的全局解。

為了滿足無人機飛行的實際條件，在規劃無人機航跡的過程中應當考慮無人機的性能參數。在傳統的PSO坐標下，通常需要根據無人機最大偏轉角和俯仰角在目標函數設置約束條件。而SPSO將粒子的搜索空間由笛卡爾坐標系轉換到球坐標系，在球坐標系(ρ,θ,φ)下，ρ表示航跡段的距離，θ表示俯仰角，φ表示偏轉角，PSO的搜索空間可以與無人機偏轉角和俯仰角之間產生相互關系，不僅增加了規劃航跡的安全性也對無人機偏轉角和俯仰角進行了約束。

在SPSO下，粒子i對應D維向量的一條航跡，每個向量代表了從一個航跡點到另一個航跡點的移動，粒子位置Ωi表示為Ωi=(Ωi1,Ωi2,…,ΩiD)，其中Ωij表示為(ρij,θij,φij)，速度ΔΩi可以表示為ΔΩi=(ΔΩi1,ΔΩi2,…,ΔΩiD),其中ΔΩij表示為(Δρij,Δθij,Δφij)。

SPSO算法獲取到粒子在球坐標下的坐標點后，需要根據目標函數來判定粒子質量，獲得粒子個體最優位置Pb和全局最佳位置Gb。因此，需要將粒子位置Ωi映射到笛卡爾坐標系下粒子位置Xi，Xi=(Xi1,Xi2,…,XiD),其中Xij表示為(xij,yij,zij)，粒子在笛卡爾坐標系下的更新方式為：

(10)

(11)

(12)

SPSO迭代過程中位置和速度更新公式為：

(13)

(14)

式中，w為慣性權重；C1和C2分別表示個體學習因子和群體學習因子；r1和r2均為0～1的均勻隨機數。

2.2 BAS算法

BAS算法[17]是一種生物啟發式優化算法，與PSO算法都是基于不斷更新迭代獲得最優解，但BAS算法是基于個體求解而非群體。BAS算法的優點在于算法復雜度低，只需要少量參數就能求解最優解[18]。

BAS算法是根據天牛的覓食行為而建立，將食物位置定義為最優解位置，食物的氣味在空間中每個位置的濃度不同，將氣味濃度定義為目標函數，當天牛的2根觸須獲取食物氣味濃度存在差別時，其會隨著氣味濃度高的一側移動，直至目標點，這樣就可以建立起數學模型。

對于多維優化問題，首先需要確定天牛2根觸須的坐標和觸須之間的距離，其基本模型如圖2所示，x表示天牛位置，xl表示左須坐標，xr表示右須坐標，d表示兩須之間的距離。算法基本步驟如下：

圖2 天牛模型Fig.2 Beetle model

① 根據式(15)給出的歸一化隨機向量確定天牛個體的朝向：

(15)

式中，k代表待優化問題維數。

② 更新第t次迭代天牛左右須位置：

(16)

③ 獲取到的天牛左右須位置后，計算目標函數值，分別用F(xl)和F(xr)表示左須和右須的目標函數值，根據天牛兩須探測到的濃度差異決定下一步移動方向。

④ 迭代更新天牛位置：

xt+1=xt+δtbsign(F(xr)-F(xl))，

(17)

式中，δt為第t次迭代時天牛向下一個方向移動的步長；sign為符號函數。

⑤ 更新步長：

δt+1=ηδδt，

(18)

式中，ηδ表示步長衰減系數。

⑥ 判斷是否可以終止迭代，如不滿足，則回到步驟③，直至滿足迭代終止條件，跳出迭代。

2.3 SPSO與BAS算法相結合

PSO算法著重于群體對單個粒子的影響，所以具有很強的全局尋優能力，但是也很容易陷入局部最優解，當粒子均趨向于全局最優解時，可能失去粒子的多樣性。而BAS算法是只考慮個體，不考慮群體的影響，因此本文將PSO算法與BAS算法相結合，提出一種基于BAS算法的SPSO優化算法，在每一次迭代的過程中，利用BAS算法對環境空間進行判斷，有利于跳出局部最優解，使得規劃路徑更加合理。

改進的算法將每個粒子也看作一個天牛，其整體步驟和PSO算法相似，在對粒子位置和速度進行更新的過程中加入BAS算法，改進后算法粒子的速度和位置更新公式為：

C3r3δtbsign(f(xr)-f(xl))，

(19)

(20)

改進后的算法，根據PSO算法的速度影響因子和BAS算法步長和觸須的思想得到新的位置，算法的具體流程為：

① 初始化PSO參數，設置粒子維度D,學習因子c1，c2和c3，慣性權重W，以及天牛須兩須距離d0。

② 隨機化粒子初始位置和速度，并且計算適應度函數，使用初始化的位置作為個體最優解Pb，得到全局最優解Gb。

③ 按照式(16)計算天牛左右須位置，并且計算左右須位置的適應度函數，利用式(19)和式(20)更新粒子位置和速度。

④ 計算更新后粒子的適應度函數，更新粒子個體最優值、全局最優值和天牛須步長。

⑤ 判斷是否滿足終止條件，如滿足，則輸出最優值；否則，回到步驟③，繼續迭代。

3 仿真分析

3.1 實驗環境設置

為了驗證改進算法的有效性，在Matlab環境下對無人機路徑規劃進行仿真建模。設計了2種仿真環境：一種環境中只有圓柱形障礙物，對應于地形較為平坦的城市環境；另一種環境是在第1種環境中加上了真實的地形數據，對應于地形較復雜的山區[19]。真實地形數據根據數字高程地圖(Digital Elevation Model，DEM)獲得，本文DEM數據由ALOS PALSAR數據庫得到，根據DEM得到高程點，將其均勻地離散化在1 000×1 000的坐標空間中。

DEM數據選擇31°04′56″N～31°11′37″N，103°50′58″E～103°57′47″E，離散在坐標空間后用x軸表示東經，x∈[0,1 000]，y軸表示北緯，y∈[0,1 000]，z軸表示高度，為每一個坐標點的真實高度。在環境1中設置起點為(330,40,50)，終點為(780,800,200)。環境2中設置起點為(1,1,50)，終點為(800,900,200)。

3.2 算法參數設置

SPSO算法參數設置為：最大迭代次數為300，粒子維數為10，慣性權重為1，自我認知和社會認知均為1.5。BAS算法參數設置為：天牛兩須寬度為1，初始步長為0.1，天牛須認知為1.2，步長衰減系數為0.95。

3.3 仿真分析

本文分別采用標準PSO，SPSO和天牛須球坐標粒子群(BSPSO)三種算法對無人機航跡規劃進行仿真驗證，得到的仿真結果如表1和圖3～圖5所示。表1中所示數據是在環境1和環境2下分別運行10次所得到的仿真結果，其中數據為求得的平均值。圖3是在仿真環境1中規劃的航跡圖，圖4是在仿真環境2中規劃的航跡圖，圖5是目標函數變化曲線圖。

表1 3種算法在2種環境中的仿真結果對比Tab.1 Comparison of simulation results of three algorithms in two environments

圖3 仿真環境1規劃航跡Fig.3 The planned trajectory in simulation environment 1

圖4 仿真環境2規劃航跡Fig.4 The planned trajectory in simulation environment 2

圖5 目標函數曲線變化Fig.5 Change of the objective function curve

環境1為不帶地形數據的簡單環境，根據圖3和表1可知，BSPSO規劃的航跡長度明顯低于PSO和SPSO，其航跡長度相比PSO和SPSO縮短了11.43%和3.5%。航跡平滑度BSPSO相比PSO和SPSO下降了48.57%和11.36%。飛行高度和障礙物限制上，3種算法的差距均不大，在加權后對應的總目標函數值均可忽略不計。在障礙物限制上PSO優于SPSO和BSPSO，這是由于BSPSO規劃出的最優路線需要穿過障礙物，會進入障礙物危險區，而PSO規劃的航跡選擇繞過障礙物，增加了航跡長度。結合所有因數，加權總目標函數值BSPSO比PSO和SPSO分別降低了14.15%和3.5%，BSPSO規劃航跡為最優航跡。

環境2為帶地形數據的復雜山區環境，根據圖4和表1可知，BSPSO規劃的航跡長度明顯低于PSO和SPSO，其航跡長度相比PSO和SPSO縮短了10.77%和1.59%。航跡平滑度BSPSO相比PSO和SPSO下降了28.15%和4.83%，這表明BSPSO算法規劃的航跡更加平滑，偏轉和俯仰的次數更少或者變化角度越少。由于山區地形復雜，無人機機動次數越少，不僅有利于節約能耗也有利于飛行安全。在障礙物限制上PSO明顯優于SPSO和BSPSO，還是因為BSPSO在確保相對安全的情況下穿過障礙物，犧牲了部分障礙物限制，降低航跡長度。結合所有因數，加權總目標函數值BSPSO比PSO和SPSO分別降低了13.44%和2.81%。因此，BSPSO算法規劃航跡在復雜地形中可以與地形的變化保持一致，滿足了山區等復雜地形的航跡規劃要求并且規劃航跡最優。

根據圖5可知，3種算法的總目標函數隨著迭代次數的增加都在減小，但是PSO和SPSO算法目標函數值均大于BSPSO算法目標函數值，顯然PSO和SPSO算法規劃的航跡不是最優航跡，均陷入了局部最優值。而改進后的算法具有跳出局部最優值的能力，是由于PSO算法更新后，不是直接進入下一輪的搜索，而是利用BAS算法的更新規則，使粒子不再盲目地按照當前速度向發現的全局最優點聚攏，從而增加了對全體極值的搜索，使得局部搜索能力得到增強。因此驗證BAS算法能夠幫助PSO算法跳出局部最優值，證明了改進后算法的有效性。

本文通過空間復雜度和時間復雜度分析改進后算法的復雜度。改進后的算法通過BAS算法更新了粒子，其只是將原有粒子替換，并未占用新的運算空間，因此空間復雜度并未增加。由3種算法在仿真環境2下多次運行后得到迭代總耗時的平均值可知，SPSO耗時最短為165 s，PSO耗時最長為186 s，這是由于SPSO能對無人機性能條件直接約束，減少了計算量，而BSPSO在SPSO的基礎上引入了BAS算法，增加了一定的計算量，總耗時為178 s，但仍比PSO算法少。

4 結束語

本文考慮了無人機飛行航跡長度、飛行高度約束、障礙物約束和航跡平滑度約束，建立了無人機航跡規劃模型。通過提出了一種SPSO算法結合BAS算法對航跡規劃模型求解，將傳統PSO的搜索空間從笛卡爾坐標系轉換到球坐標系，利用球坐標系直接與無人機飛行航向角和俯仰角產生約束，同時結合BAS算法有助于幫助PSO算法跳出局部最優解，獲得最優航跡。通過Matlab仿真將改進算法與SPSO和傳統PSO算法進行仿真測試，測試結果表明改進后的算法具有更強的全局搜索能力，并且規劃出的航跡更短、更平滑，提高了無人機規劃航跡的質量。