基于改進(jìn)粒子群算法的三維路徑規(guī)劃

謝勇宏,孔月萍

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

0 引言

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

傳統(tǒng)算法求解三維路徑往往計(jì)算量大，過(guò)程復(fù)雜，而智能優(yōu)化算法通過(guò)規(guī)則可在一定時(shí)間內(nèi)得到較好的結(jié)果[3]。粒子群算法是一種具有收斂性快，實(shí)現(xiàn)思想簡(jiǎn)單等特點(diǎn)的群智能優(yōu)化算法，依靠粒子的速度和位置不斷更新來(lái)獲取最佳值[4]。迭代過(guò)程中，更新規(guī)則的限制，會(huì)產(chǎn)生不合理的搜索方向，使得粒子的位置滯留在某個(gè)狹義最優(yōu)解區(qū)域，不能到達(dá)整個(gè)可行解空間的最優(yōu)[5]。

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

現(xiàn)階段可供二次開(kāi)發(fā)的無(wú)人機(jī)已經(jīng)非常豐富，擁有較成熟的飛行控制硬件，但是面對(duì)的應(yīng)用場(chǎng)景中環(huán)境因素不一樣，運(yùn)動(dòng)規(guī)劃系統(tǒng)仍需完善。為此，通過(guò)無(wú)人機(jī)開(kāi)發(fā)系統(tǒng)提出了一種改進(jìn)融合牽引力粒子的三維路徑規(guī)劃方法。在仿真平臺(tái)中，通過(guò)三維障礙物坐標(biāo)信息構(gòu)造地形模型，驗(yàn)證方法的有效性。

1 系統(tǒng)結(jié)構(gòu)及飛行原理

1.1 系統(tǒng)結(jié)構(gòu)

無(wú)人機(jī)控制系統(tǒng)主要有前期輸入的飛行任務(wù)，運(yùn)動(dòng)規(guī)劃系統(tǒng)，對(duì)功能Agent傳輸?shù)目刂浦噶?、各種功能Agent，狀態(tài)檢測(cè)裝置。圖1為無(wú)人機(jī)控制系統(tǒng)體系結(jié)構(gòu)。

圖1 無(wú)人機(jī)控制系統(tǒng)體系結(jié)構(gòu)

1.2 三維路徑的飛行功能實(shí)現(xiàn)

實(shí)現(xiàn)無(wú)人機(jī)的飛行任務(wù)，需要在飛行控制管理系統(tǒng)中錄入預(yù)先設(shè)計(jì)的路徑規(guī)劃方法。管理人員根據(jù)需要選擇目標(biāo)位置點(diǎn)，無(wú)人機(jī)通過(guò)路徑跟蹤控制器按照路徑規(guī)劃方法優(yōu)化之后的目標(biāo)路徑進(jìn)行跟蹤飛行控制。

運(yùn)動(dòng)規(guī)劃系統(tǒng)屬于控制手柄端的后臺(tái)管理模塊。為了使得無(wú)人機(jī)接收控制手柄段的三維目標(biāo)路徑信息，采用MSDK與樹(shù)莓派端的OSDK架起通信鏈路，完成目標(biāo)路徑信息的交互。環(huán)境地形是進(jìn)行無(wú)人機(jī)飛行時(shí)考慮的必不可少的因素，以下將基于環(huán)境地形模型，對(duì)三維路徑規(guī)劃方法展開(kāi)分析設(shè)計(jì)，以求豐富運(yùn)動(dòng)規(guī)劃系統(tǒng)的知識(shí)模型。

2 環(huán)境地形模型

在對(duì)地形模型建模時(shí)，應(yīng)考慮切合實(shí)際，使得路徑規(guī)劃的效率更高。據(jù)此，基準(zhǔn)的地形模型[12]為：

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

(1)

其中：x表示橫坐標(biāo)，y表示縱坐標(biāo)，Z1表示區(qū)域的高，a、b、c、d、e、f、g為常數(shù)，來(lái)模擬地形項(xiàng)目中的高低分布。通過(guò)指數(shù)函數(shù)來(lái)布置外形數(shù)據(jù)點(diǎn)[13]。數(shù)學(xué)模型可以表示為：

(2)

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

3 三維路徑規(guī)劃方法

無(wú)人機(jī)飛行軌跡要考慮環(huán)境地形的情況，一般環(huán)境地形越復(fù)雜，無(wú)人機(jī)的飛行難度越大，路徑規(guī)劃的難度也增大。通過(guò)第2節(jié)建立環(huán)境地形，采用立體數(shù)字網(wǎng)的形式描述整個(gè)三維空間，構(gòu)造了三維軸線的網(wǎng)格數(shù)據(jù)。立體數(shù)字網(wǎng)的形式有利于簡(jiǎn)化環(huán)境地形的復(fù)雜性，即整個(gè)地形就是有序的三維點(diǎn)坐標(biāo)。障礙物采用外形邊緣的數(shù)據(jù)網(wǎng)格表示，存儲(chǔ)為一個(gè)數(shù)據(jù)集，定義在數(shù)據(jù)集閉區(qū)間內(nèi)的點(diǎn)坐標(biāo)，無(wú)人機(jī)的飛行路徑均需要避開(kāi)繞道。

另一方面，粒子群算法對(duì)于處理散點(diǎn)數(shù)據(jù)具有優(yōu)越性。在實(shí)際應(yīng)用中，采用GPS定位當(dāng)前位置并判斷與環(huán)境坐標(biāo)是否沖突，沖突即遭遇障礙物，無(wú)人機(jī)在路徑追蹤時(shí)需要避開(kāi)當(dāng)前位置，重新規(guī)劃路線。在路徑規(guī)劃上可根據(jù)多個(gè)點(diǎn)坐標(biāo)組成一個(gè)數(shù)據(jù)組，首尾點(diǎn)坐標(biāo)為起始點(diǎn)與終止點(diǎn)。通過(guò)粒子尋優(yōu)，數(shù)據(jù)組的中間段的點(diǎn)坐標(biāo)分別分布在合適位置，通過(guò)曲線擬合方式可呈現(xiàn)規(guī)劃的結(jié)果路徑。

4 改進(jìn)粒子群算法的設(shè)計(jì)

標(biāo)準(zhǔn)粒子群算法(PSO)的思想是通過(guò)隨機(jī)產(chǎn)生給定數(shù)量的粒子群，依靠粒子本身記憶的最好位置及粒子群體之間的信息交流對(duì)全區(qū)域?qū)?yōu)。伴隨著迭代次數(shù)的增加，粒子極易陷入某些低谷，無(wú)法跳躍局限繼續(xù)搜索，使得最終獲得解非最優(yōu)。針對(duì)粒子群算法的缺陷主要做如下改進(jìn)。

4.1 個(gè)體編碼及自適應(yīng)初始化

在三維路徑規(guī)劃中，起始點(diǎn)到達(dá)目標(biāo)點(diǎn)呈現(xiàn)一條線，需要對(duì)障礙點(diǎn)進(jìn)行避讓?zhuān)€段中的彎道部位存在一個(gè)頂點(diǎn)。因此，對(duì)粒子引用染色體編碼形式，每個(gè)粒子由一個(gè)數(shù)組組成，在數(shù)組中包含多個(gè)途徑點(diǎn)，再和起始點(diǎn)和目標(biāo)點(diǎn)組成的連線表示規(guī)劃的路徑。途徑點(diǎn)的個(gè)數(shù)過(guò)多將會(huì)使得計(jì)算復(fù)雜，過(guò)少不利于尋優(yōu)路徑。并且在建造項(xiàng)目中無(wú)人機(jī)只涉及起始點(diǎn)位置及目標(biāo)點(diǎn)位置之間的合理規(guī)劃，在初始粒子時(shí)隨機(jī)產(chǎn)生的粒子次序?qū)Q定是否存在反復(fù)路徑。因此，粒子群的初始位置對(duì)算法尋優(yōu)具有較大影響，考慮障礙區(qū)域大小以及起始點(diǎn)和目標(biāo)點(diǎn)的距離，提出自適應(yīng)個(gè)體初始化機(jī)制。主要做法如下：通過(guò)計(jì)算起始點(diǎn)與目標(biāo)點(diǎn)的歐式距離：

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

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

(3)

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

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

(4)

4.2 三維路徑值的計(jì)算方法

在迭代過(guò)程中需要衡量前后迭代的粒子質(zhì)量，才能促使粒子在全區(qū)域內(nèi)不斷逼近優(yōu)秀位置。智能建造中的三維路徑規(guī)劃中主要考慮的指標(biāo)適應(yīng)度是最短的路徑。為了使得規(guī)劃后的路徑更平滑，無(wú)人機(jī)飛行轉(zhuǎn)彎時(shí)有足夠的角度，不至于太急促。為此，采用了三條樣條平滑方法進(jìn)行插值，在對(duì)插值進(jìn)行差分處理，通過(guò)計(jì)算三次樣條得到的散點(diǎn)的路徑長(zhǎng)度[14]。適應(yīng)度值公式如下：

fitness=

(5)

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

4.3 線性遞減的慣性權(quán)重

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

(6)

其中：ωmax為慣性權(quán)重最大值，ωmin慣性權(quán)重最小值，itermax為最大迭代次數(shù)。

4.4 牽引力操作

為解決粒子群算法在尋優(yōu)過(guò)程易陷入局部區(qū)域無(wú)法跳躍谷底，導(dǎo)致最終獲取的非全局最優(yōu)解的問(wèn)題。對(duì)粒子的速度的更新方式引進(jìn)加速度思想，使得粒子能夠獲得足夠的動(dòng)力翻越谷底，增強(qiáng)全局的搜索能力，提高粒子尋優(yōu)的收斂速度。加速度的公式如下：

(7)

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

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

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

(8)

其中：v表示速度值，t表示第t次迭代數(shù)，c1表示個(gè)體學(xué)習(xí)因子，c2表示群體學(xué)習(xí)因子，pbest表示粒子的個(gè)體最優(yōu)，gbest表示粒子的種群最優(yōu)，d表示粒子的第d維，rand(0,1)表示0～1之間的隨機(jī)數(shù)。

4.5 改進(jìn)算法的操作流程

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

圖2 改進(jìn)算法流程圖

步驟一：初始化粒子規(guī)模，最大迭代數(shù)、最大慣性權(quán)值和最小慣性權(quán)值，自身學(xué)習(xí)因子，群體學(xué)習(xí)因子。

步驟二：通過(guò)表達(dá)式(1)～(2)建立三維場(chǎng)景模型，并返回模型界面的三位數(shù)值，作為碰撞檢驗(yàn)。

步驟三：通過(guò)表達(dá)式(3)～(4)進(jìn)行初始化粒子種群，根據(jù)公式(4)計(jì)算粒子各自的適應(yīng)度值并保存?zhèn)€體最優(yōu)及群體最優(yōu)；

步驟四：通過(guò)公式(5)～(7)進(jìn)行更新粒子的速度及位置，并作碰撞檢驗(yàn)，對(duì)碰撞位置進(jìn)行約束調(diào)整。

步驟五：判斷是否達(dá)到最大迭代次數(shù)，是則結(jié)束循環(huán)輸出最優(yōu)粒子；否則返回進(jìn)入步驟三。

5 實(shí)驗(yàn)仿真及分析

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

5.1 自適應(yīng)初始化機(jī)制的檢驗(yàn)

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

圖3 適應(yīng)度

圖4 路徑規(guī)劃圖

同時(shí)，對(duì)比了IGPSO，MGPSO算法收斂的穩(wěn)定性。障礙物的數(shù)量不變情況下，隨機(jī)產(chǎn)生障礙物，分別運(yùn)行了10次并統(tǒng)計(jì)了10次尋優(yōu)的平均最優(yōu)值及其方差，結(jié)果如表1所示?？芍?，MGPSO算法求解穩(wěn)定性更高。

表1 自適應(yīng)初始化機(jī)制的有效性結(jié)果

5.2 與其他算法的比較

同時(shí)驗(yàn)證改進(jìn)算法相對(duì)于其他一些策略的算法優(yōu)先性，采用了IPSO算法[8]、IHPSO算法[11]與MGPSO算法進(jìn)行對(duì)比。

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

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

分析可知主要是因?yàn)椴捎昧司€性遞減的慣性權(quán)重，使得初期粒子的速度較大，加強(qiáng)在全局區(qū)域的最優(yōu)值搜索，隨著迭代周期的增加，權(quán)值逐漸減小，減弱大范圍的跳躍，同時(shí)加強(qiáng)局部的搜索，使得路徑值不斷逼近最優(yōu)。同時(shí)采用牽引力操作添加了加速度作用，在搜索過(guò)程中擴(kuò)大了步伐，使得粒子更易跳出局部最優(yōu)。在圖6為3種算法的最終尋優(yōu)路徑結(jié)果，可以較為直觀地看出，MGPSO算法明顯相對(duì)于其他兩個(gè)算法，在復(fù)雜的場(chǎng)景中跳出了局部最優(yōu)并獲得更優(yōu)路徑。

圖6 3種算法規(guī)劃路徑的結(jié)果圖

障礙物的數(shù)量不變情況下，隨機(jī)產(chǎn)生障礙物，通過(guò)三十次的重復(fù)實(shí)驗(yàn)，統(tǒng)計(jì)3種算法求解的平均最優(yōu)路徑值及其

方差，結(jié)果如表2所示，MGPSO算法的求解質(zhì)量最優(yōu)且穩(wěn)定性能上也表現(xiàn)不錯(cuò)。穩(wěn)定性相對(duì)于IPSO較好69.75%，相對(duì)于IHPSO較好17.41%。

表2 3種算法的性能比較

6 結(jié)束語(yǔ)

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

由于路徑的平滑度對(duì)尋優(yōu)的質(zhì)量具有較大的影響，因此，后續(xù)將結(jié)合算法與高效的路徑平滑度對(duì)智能建造中無(wú)人機(jī)路徑展開(kāi)研究。