基于改進粒子群算法的三維路徑規劃

謝勇宏,孔月萍

(西安建筑科技大學 信息與控制工程學院，西安 710055)

0 引言

伴隨著科技手段的進步，在眾多領域逐漸實現無人化，正在改變那些依靠人進行笨重且重復的工作。例如采用無人機方式在建造的建筑群中進行安全作業巡查，可提高工作效率，保證整個項目作業的安全系數[1]。無人機硬件系統具有較好的控制性能，例如四旋翼無人機等[2]。而設計合理高效的三維路徑方法，將有助于無人機實現自主執行固定的飛行任務。

傳統算法求解三維路徑往往計算量大，過程復雜，而智能優化算法通過規則可在一定時間內得到較好的結果[3]。粒子群算法是一種具有收斂性快，實現思想簡單等特點的群智能優化算法，依靠粒子的速度和位置不斷更新來獲取最佳值[4]。迭代過程中，更新規則的限制，會產生不合理的搜索方向，使得粒子的位置滯留在某個狹義最優解區域，不能到達整個可行解空間的最優[5]。

一般來說，求解的最優值與粒子初始位置有關，粒子群的初始化質量對于算法的收斂性有較大影響[6]。文獻[7]采用混沌序列初始化粒子群，使得初始粒子質量更好，并且證明多種群機制下粒子分布更均勻，有益于提高搜索性能。為了增強粒子在搜索中的搜索能力，避免陷入局部最優。文獻[8]證明通過引入牽引操作，有效幫助算法擴大搜索范圍，提高求解性能。文獻[9]借助遺傳交叉變異策略提升粒子群的多樣性，在全區域中能產生高質量的解；幫助減少適應度的拐彎次數，對路徑簡單化以提升求解效率。另外，粒子搜索過程易跌入局部低谷，這需要借助更有效的跳躍能力來產生足夠大的步伐[10]。文獻[11]考慮搜索過程中粒子的速度及群體的離散度會有變化，使得慣性權重自適應變化，避免過早收斂。在位置更新中通過自然選擇方式保持粒子群的多樣性，加快算法的收斂速度。

現階段可供二次開發的無人機已經非常豐富，擁有較成熟的飛行控制硬件，但是面對的應用場景中環境因素不一樣，運動規劃系統仍需完善。為此，通過無人機開發系統提出了一種改進融合牽引力粒子的三維路徑規劃方法。在仿真平臺中，通過三維障礙物坐標信息構造地形模型，驗證方法的有效性。

1 系統結構及飛行原理

1.1 系統結構

無人機控制系統主要有前期輸入的飛行任務，運動規劃系統，對功能Agent傳輸的控制指令、各種功能Agent，狀態檢測裝置。圖1為無人機控制系統體系結構。

圖1 無人機控制系統體系結構

1.2 三維路徑的飛行功能實現

實現無人機的飛行任務，需要在飛行控制管理系統中錄入預先設計的路徑規劃方法。管理人員根據需要選擇目標位置點，無人機通過路徑跟蹤控制器按照路徑規劃方法優化之后的目標路徑進行跟蹤飛行控制。

運動規劃系統屬于控制手柄端的后臺管理模塊。為了使得無人機接收控制手柄段的三維目標路徑信息，采用MSDK與樹莓派端的OSDK架起通信鏈路，完成目標路徑信息的交互。環境地形是進行無人機飛行時考慮的必不可少的因素，以下將基于環境地形模型，對三維路徑規劃方法展開分析設計，以求豐富運動規劃系統的知識模型。

2 環境地形模型

在對地形模型建模時，應考慮切合實際，使得路徑規劃的效率更高。據此，基準的地形模型[12]為：

Z1(x,y)=sin(y+a)+

(1)

其中：x表示橫坐標，y表示縱坐標，Z1表示區域的高，a、b、c、d、e、f、g為常數，來模擬地形項目中的高低分布。通過指數函數來布置外形數據點[13]。數學模型可以表示為：

(2)

式中，(xi,yi)表示第i個樓層的中心坐標位置；hi是一個高度的控制參數；xsi和ysi則分別是第i個樓層在x軸和y軸方向的衰減量，控制坡度；N表示山峰總的個數。

3 三維路徑規劃方法

無人機飛行軌跡要考慮環境地形的情況，一般環境地形越復雜，無人機的飛行難度越大，路徑規劃的難度也增大。通過第2節建立環境地形，采用立體數字網的形式描述整個三維空間，構造了三維軸線的網格數據。立體數字網的形式有利于簡化環境地形的復雜性，即整個地形就是有序的三維點坐標。障礙物采用外形邊緣的數據網格表示，存儲為一個數據集，定義在數據集閉區間內的點坐標，無人機的飛行路徑均需要避開繞道。

另一方面，粒子群算法對于處理散點數據具有優越性。在實際應用中，采用GPS定位當前位置并判斷與環境坐標是否沖突，沖突即遭遇障礙物，無人機在路徑追蹤時需要避開當前位置，重新規劃路線。在路徑規劃上可根據多個點坐標組成一個數據組，首尾點坐標為起始點與終止點。通過粒子尋優，數據組的中間段的點坐標分別分布在合適位置，通過曲線擬合方式可呈現規劃的結果路徑。

4 改進粒子群算法的設計

標準粒子群算法(PSO)的思想是通過隨機產生給定數量的粒子群，依靠粒子本身記憶的最好位置及粒子群體之間的信息交流對全區域尋優。伴隨著迭代次數的增加，粒子極易陷入某些低谷，無法跳躍局限繼續搜索，使得最終獲得解非最優。針對粒子群算法的缺陷主要做如下改進。

4.1 個體編碼及自適應初始化

在三維路徑規劃中，起始點到達目標點呈現一條線，需要對障礙點進行避讓，線段中的彎道部位存在一個頂點。因此，對粒子引用染色體編碼形式，每個粒子由一個數組組成，在數組中包含多個途徑點，再和起始點和目標點組成的連線表示規劃的路徑。途徑點的個數過多將會使得計算復雜，過少不利于尋優路徑。并且在建造項目中無人機只涉及起始點位置及目標點位置之間的合理規劃，在初始粒子時隨機產生的粒子次序將決定是否存在反復路徑。因此，粒子群的初始位置對算法尋優具有較大影響，考慮障礙區域大小以及起始點和目標點的距離，提出自適應個體初始化機制。主要做法如下：通過計算起始點與目標點的歐式距離：

d=sqrt((g(x)-s(x))2+

(g(y)-s(y))2+(g(z)-s(z))2)

(3)

其中：g(x,y,z)表示目標點位置，s(x,y,z)表示起始點位置。然后結合原有障礙點的個數進行確立粒子組成的位置數組。利用均勻插值方式隨機產生隨機的n個連續均勻分布的數組，然后對其進行升序排序，產生粒子的初始位置。公式如下：

X(t+1)=sort(unifrnd(start,goal,n))

(4)

4.2 三維路徑值的計算方法

在迭代過程中需要衡量前后迭代的粒子質量，才能促使粒子在全區域內不斷逼近優秀位置。智能建造中的三維路徑規劃中主要考慮的指標適應度是最短的路徑。為了使得規劃后的路徑更平滑，無人機飛行轉彎時有足夠的角度，不至于太急促。為此，采用了三條樣條平滑方法進行插值，在對插值進行差分處理，通過計算三次樣條得到的散點的路徑長度[14]。適應度值公式如下：

fitness=

(5)

其中：K表示插值數量，k表示插值的位次，對所有路徑段距離求和得到總的路徑長度，該值越小則路徑越短。

4.3 線性遞減的慣性權重

慣性權重w是粒子記憶前代速度的重要參數，在算法的前期，對于搜索的范圍較大，需要較大的慣性權值來提高大范圍搜索的性能，而在后期需要進行更精細的搜索。故采用線性遞減的方式調節權值，慣性權重將在最大和最小值之間線性變化，以平衡搜索能力[15]。同時，為了均衡慣性權重數值與粒子速度之間的量程差，引進路徑量程系數A，防止權重值發生發散，公式如下：

(6)

其中：ωmax為慣性權重最大值，ωmin慣性權重最小值，itermax為最大迭代次數。

4.4 牽引力操作

為解決粒子群算法在尋優過程易陷入局部區域無法跳躍谷底，導致最終獲取的非全局最優解的問題。對粒子的速度的更新方式引進加速度思想，使得粒子能夠獲得足夠的動力翻越谷底，增強全局的搜索能力，提高粒子尋優的收斂速度。加速度的公式如下：

(7)

其中：B為加速度量程系數，控制加速度量程。fiti(t+1)為第i粒子在第t次迭代周期的適應度值，best(t)表示為上一代粒子的最優適應度值，ave(t)表示粒子群第t次迭代周期的平均適應度值。分析可知，假如粒子的當前適應度值與前代粒子群的最優適應度值偏差越大，會使得粒子速度增大，搜索的跨度更廣，利于擺脫局部困擾。

在迭代中同時監察粒子的適應度在三次迭代中是否具有改善，若無改善則通過一定概率在進行牽引操作增大種群的多樣性，若有改善時則縮小進行牽引操作的概率，以此防止不同粒子在迭代中逐漸過于聚集，保持全局搜索。改進后的粒子群算法的粒子速度和位置更新分別為：

X(t+1)=X(t)+v(t+1)

(8)

其中：v表示速度值，t表示第t次迭代數，c1表示個體學習因子，c2表示群體學習因子，pbest表示粒子的個體最優，gbest表示粒子的種群最優，d表示粒子的第d維，rand(0,1)表示0～1之間的隨機數。

4.5 改進算法的操作流程

混合引力的粒子群算法(MGPSO,particle swarm optimization algorithm for mixed gravity)流程如圖2所示。

圖2 改進算法流程圖

步驟一：初始化粒子規模，最大迭代數、最大慣性權值和最小慣性權值，自身學習因子，群體學習因子。

步驟二：通過表達式(1)～(2)建立三維場景模型，并返回模型界面的三位數值，作為碰撞檢驗。

步驟三：通過表達式(3)～(4)進行初始化粒子種群，根據公式(4)計算粒子各自的適應度值并保存個體最優及群體最優；

步驟四：通過公式(5)～(7)進行更新粒子的速度及位置，并作碰撞檢驗，對碰撞位置進行約束調整。

步驟五：判斷是否達到最大迭代次數，是則結束循環輸出最優粒子；否則返回進入步驟三。

5 實驗仿真及分析

仿真實驗在Matlab2016a(Windows10系統，主機IntelCorei5-8300H，CPU@2.20GHz，8GBRAM)進行。對算法的基本設置如下：最大迭代次數iter_max=100，種群個數M=50，個體學習因子c1=2，種群學習因子c2=2；最大慣性權重ωmax=0.9，最小慣性權重ωmin=0.4，路徑量程系數A=0.1，加速度量程系數B=0.1初始位置startPos = [1,1,1];目標位置goalPos = [100,100,50]。

5.1 自適應初始化機制的檢驗

首先，檢驗所提的自適應初始化機制在改進算法中的有效性，采用隨機初始化種群的改進粒子群算法(IGPSO,improved gravitational particle swarm optimization)對照組，其他參數一致相同。圖3為算法對照組實驗的求解迭代圖，由圖中可以看出在初始位置。MGPSO算法在迭代前期的尋優值明顯好于IGPSO，說明自適應初始化種群更有針對性地幫助算法獲取初始點與目標點之間的合理路徑。同時觀察圖4(a)(b)的最終尋優路徑，可知在有限區域內，障礙物的數量較多且密集，場景復雜。初始粒子群的優劣直接影響算法在尋優時的有效性，而自適應初始化機制有效地提升初始粒子群的質量。

圖3 適應度

圖4 路徑規劃圖

同時，對比了IGPSO，MGPSO算法收斂的穩定性。障礙物的數量不變情況下，隨機產生障礙物，分別運行了10次并統計了10次尋優的平均最優值及其方差，結果如表1所示?？芍?，MGPSO算法求解穩定性更高。

表1 自適應初始化機制的有效性結果

5.2 與其他算法的比較

同時驗證改進算法相對于其他一些策略的算法優先性，采用了IPSO算法[8]、IHPSO算法[11]與MGPSO算法進行對比。

3種算法均采用了自適應初始化機制產生初始種群，實驗收斂迭代過程如圖5所示。從圖中可以看出3種算法都具有較好的收斂效果，同時對比圖3可知，采用自適應初始化機制在IPSO算法體現效果最明顯。IPSO算法前期的粒子群質量最優，但在迭代10次之后的搜索能力明顯減弱，即使后期仍有尋優余熱，效果卻不明顯。本文所提的優化算法雖然在前期粒子群質量均差于其余兩種算法，但是在保持尋優的耐力上更具有優勢，在20次迭代時基本完成全局最優解區域的定位，后續仍有尋優余熱。

圖5 3種算法的收斂迭代圖

分析可知主要是因為采用了線性遞減的慣性權重，使得初期粒子的速度較大，加強在全局區域的最優值搜索，隨著迭代周期的增加，權值逐漸減小，減弱大范圍的跳躍，同時加強局部的搜索，使得路徑值不斷逼近最優。同時采用牽引力操作添加了加速度作用，在搜索過程中擴大了步伐，使得粒子更易跳出局部最優。在圖6為3種算法的最終尋優路徑結果，可以較為直觀地看出，MGPSO算法明顯相對于其他兩個算法，在復雜的場景中跳出了局部最優并獲得更優路徑。

圖6 3種算法規劃路徑的結果圖

障礙物的數量不變情況下，隨機產生障礙物，通過三十次的重復實驗，統計3種算法求解的平均最優路徑值及其

方差，結果如表2所示，MGPSO算法的求解質量最優且穩定性能上也表現不錯。穩定性相對于IPSO較好69.75%，相對于IHPSO較好17.41%。

表2 3種算法的性能比較

6 結束語

為了使得無人機在執行任務中具有更完善的路徑規劃的知識模型，分析了無人機的系統結構及四旋翼無人的飛行原理。指出粒子群算法進行無人機的三維路徑規劃極易陷入局部最優，提出了一種混合慣性牽引力的三維路徑規劃方法。實驗仿真結果中表明，自適應初始機制對于幫助算法對于最優路徑的求解性能，在特定的算法搜索能力下，極大影響求解質量。通過引入線性慣性權重并且結合路徑的量程大小，動態地調節算法在全局搜索能力與局部搜索能力之間的平衡。增加了加速度方式，可幫助粒子群增大擺脫陷入局部最優的概率。改進的算法具有較好的搜索能力，也能保持較好的求解穩定性。

由于路徑的平滑度對尋優的質量具有較大的影響，因此，后續將結合算法與高效的路徑平滑度對智能建造中無人機路徑展開研究。