基于改進(jìn)粒子群算法的水面無人艇全局路徑規(guī)劃*

林法君 李 焰

（1.海參軍事訓(xùn)練中心 北京 100036）（2.海軍工程大學(xué)兵器工程學(xué)院 武漢 430000）

1 引言

目前，隨著水面無人艇（USV）使用的日益增多，許多有前景的技術(shù)研究已經(jīng)擴(kuò)散開來［1～2］。USV具有協(xié)同合作，智能自主等特點(diǎn)，在編隊(duì)控制和智能避碰等領(lǐng)域已經(jīng)有了廣泛的研究［3～4］。USV可以執(zhí)行危險(xiǎn)任務(wù)，具有一定的自主能力，減少了對人員配備的需求。為了有效地完成實(shí)際任務(wù)，路徑規(guī)劃在USV的實(shí)際應(yīng)用和理論探索中都起到了良好的作用。

因此，一種安全高效的航路規(guī)劃技術(shù)對于USV在實(shí)際海洋環(huán)境中的應(yīng)用具有至關(guān)重要的作用。具體來說，USV需要路徑規(guī)劃方法來自主執(zhí)行海岸巡邏、救援等各項(xiàng)任務(wù)［5］。近年來在全局路徑規(guī)劃的研究上，啟發(fā)式算法被廣泛使用，啟發(fā)式算法在精確算法失效的情況下可以得到最優(yōu)解［6］。粒子群算法（PSO），蟻群算法（AC）和人工蜂群算法（ABC）等均屬于生物啟發(fā)式算法。粒子群算法具有操作簡單，設(shè)置參數(shù)少等易于實(shí)現(xiàn)的特點(diǎn)［7～8］。J.Kennedy，R.Eberhart建立了一個(gè)包含粒子群算法的框架來最小化路徑長度，但是在障礙復(fù)雜的環(huán)境下，傳統(tǒng)的粒子群算法在全局路徑規(guī)劃中還存在一些不足［9］，比如算法前期收斂慢、迭代后期個(gè)體最優(yōu)解易波動等問題，很大程度上影響了全局路徑規(guī)劃的效率和可靠性［10］。為了解決前期收斂速度慢的問題，張曉莉等［11］提出了動態(tài)速度權(quán)重法，較好地解決了前期收斂速度慢的問題。但是本文在研究驗(yàn)證過程中，發(fā)現(xiàn)動態(tài)速度權(quán)重法相對于原始的粒子群算法，更容易陷入局部最優(yōu)。為了解決上述存在的前期收斂速度慢，以及容易陷入局部最優(yōu)的問題，本文提出了一種改進(jìn)的速度權(quán)重法，在路徑尋優(yōu)過程中，同時(shí)運(yùn)行原始粒子群算法和改進(jìn)的速度權(quán)重法，然后根據(jù)權(quán)重對每個(gè)粒子以輪盤賭的方式?jīng)Q定粒子的迭代公式，最后得到全局路徑規(guī)劃的最優(yōu)路徑。

2 改進(jìn)粒子群算法

2.1 初始粒子群算法（PSO）

粒子群算法（Particle Swarm Optimization，PSO），也稱為鳥群覓食算法，是模仿鳥群覓食行為設(shè)計(jì)的一種尋優(yōu)算法，屬于進(jìn)化算法的一種，從一組隨機(jī)解出發(fā)，通過不斷地迭代尋找最優(yōu)解。在迭代的過程中，以某一適應(yīng)函數(shù)對粒子的位置進(jìn)行評價(jià)，每個(gè)粒子通過對比全局部最優(yōu)和個(gè)體最優(yōu)來更新自己的位置和速度的狀態(tài)。整個(gè)粒子群在迭代的過程中不斷向目的地靠攏，最后獲得最優(yōu)解［12］。

經(jīng)典的粒子群算法的數(shù)學(xué)模型為位置和速度更新模型，表達(dá)式為

對上式中的組成部分說明如下：vk(i，d)表示粒子群算法第k次迭代時(shí)，粒子i在維度d上的速度分量；qk(i，d)表示為粒子i在k次迭代中歷史最優(yōu)位置在維度d上的分量，即個(gè)體極值；xk(i，d)表示當(dāng)前粒子i在維度d上的位置值；gk(i，d)表示全部粒子在k次迭代中歷史最優(yōu)位置在維度d上的分量，即全局極值。式（1）描述的是粒子的速度更新方式，其中ω為慣性因子，反映了粒子沿原運(yùn)動方向的運(yùn)動趨勢；c1和c2是學(xué)習(xí)因子，c1反映的是當(dāng)前粒子向個(gè)體極值加速的趨勢，c2反映的是當(dāng)前粒子向全局極值加速的趨勢，r1和r2為［0，1］范圍內(nèi)的隨機(jī)數(shù)。式（2）描述的是粒子的位置更新方式，即下一時(shí)刻位置等于當(dāng)前時(shí)刻位置加上當(dāng)前時(shí)刻速度［13］。

2.2 速度權(quán)重法（PPSO）

封碩等［14］指出標(biāo)準(zhǔn)的粒子群算法中的粒子位置，可能會因?yàn)楹笃诘^程中速度過大，個(gè)體極值偏離全局極值，于是提出了對速度進(jìn)行加權(quán)處理的方法，并給出了新的位置更新公式：

其中p為速度權(quán)重，權(quán)重p的確定如下式：

(xp，yp，…)表示在k次迭代中，當(dāng)前粒子的個(gè)體極值，(xg，yg，…)表示為當(dāng)前時(shí)刻粒子群的全局極值坐標(biāo)點(diǎn)，表示前一時(shí)刻粒子群的全局極值坐標(biāo)點(diǎn)。

本研究在驗(yàn)證的過程中對上述式子中未能給出的隱含信息進(jìn)行了補(bǔ)充。比如上述式子在第一次迭代過程中缺少前一時(shí)刻的粒子狀態(tài)信息，以及前一時(shí)刻與當(dāng)前時(shí)刻全局極值坐標(biāo)點(diǎn)相同的情形如何處理，從而導(dǎo)致仿真實(shí)驗(yàn)中出現(xiàn)“變量未定義”、“出現(xiàn)分母為零”之類的報(bào)錯。因此，本文對上述速度權(quán)重法做了一定的補(bǔ)充：

2.3 動態(tài)速度權(quán)重法（DPSO）

在本研究仿真實(shí)驗(yàn)的過程中，發(fā)現(xiàn)速度權(quán)重法的確大大加快了前期的收斂速度，但是在多次的仿真實(shí)驗(yàn)中，該方法更容易陷入局部最優(yōu)，使得找到最優(yōu)路徑的不確定性大了很多。

為了既加快算法的收斂速度，又提高尋找最優(yōu)解的可靠性，本文采用輪盤賭的方法結(jié)合了速度權(quán)重的粒子群算法和初始的粒子群算法。在算法的設(shè)計(jì)中加入了兩輪嵌套判斷語句對位置的更新公式做出了引導(dǎo)。

在每次計(jì)算p值得時(shí)候，引入一個(gè)0～1之間的隨機(jī)數(shù)o，令

pi根據(jù)當(dāng)前粒子i的狀態(tài)計(jì)算得到，li表示為歸一化后的輪盤指針，由輪盤指針的落點(diǎn)決定位置的迭代公式。當(dāng)li＜o(jì)時(shí)，執(zhí)行原始粒子群算法的位置迭代公式，li≥o時(shí)，執(zhí)行速度加權(quán)粒子群算法的位置迭代公式。

2.4 算法測試

為了比較原始粒子群算法（PSO）、速度加權(quán)粒子群算法（PPSO）和兩者結(jié)合的動態(tài)粒子群算法（DPSO）的表現(xiàn)，本文選用了Sphere函數(shù)作為適應(yīng)度函數(shù)做仿真實(shí)驗(yàn)。實(shí)驗(yàn)首先用三個(gè)粒子群算法求解適度函數(shù)的最小值，然后對三組實(shí)驗(yàn)做直觀上的比較和求解速度以及精度上的比較來評比其表現(xiàn)。

適度函數(shù)的計(jì)算如下：

(x1，x2，…，xn)為粒子的點(diǎn)坐標(biāo)，Sphere函數(shù)是一個(gè)單峰函數(shù)，在坐標(biāo)（0，0，…，0）處取得最小值0。算法的粒子數(shù)量定為50個(gè)，迭代次數(shù)為50次。用三種方法對其進(jìn)行實(shí)驗(yàn)，仿真繪圖結(jié)果如圖1所示：

圖1 Sphere函數(shù)尋優(yōu)實(shí)驗(yàn)

提取圖中三種算法初次進(jìn)入收斂階段的迭代代數(shù)k和最優(yōu)值q記于表1中。

表1 收斂階段的測試數(shù)據(jù)

對以上圖形的直觀對比和表格數(shù)據(jù)的具體對比作出分析如下。

從收斂速度上來看PSO算法在經(jīng)過13次迭代進(jìn)入收斂階段，PPSO算法在經(jīng)過7次迭代后進(jìn)入收斂階段，DPSO算法在經(jīng)過8次迭代后進(jìn)入收斂階段，就收斂速度而言，PPSO?DPSO?PSO。

從逼近最小值的情況來看，PSO基本完成了最小值的尋找，DPSO以更高的精度完成了最小值的尋找，PPSO陷入了局部最優(yōu)，沒有找到最小值，就尋求最小值而言DSO?PSO?PPSO。

具體原因分析如下所示。

PPSO中，在經(jīng)過幾輪迭代后，當(dāng)前時(shí)刻的全局極值與前一時(shí)刻的全局極值差距會縮小，pi值越來越小，使得迭代的步進(jìn)值減小，因此當(dāng)粒子陷入局部最優(yōu)時(shí)，也很難再“跑”出來。DPSO中，前面的幾輪迭代中，pi比較大，輪盤賭法更大可能地選中了PPSO的位置更新公式，因此也吸收了PPSO前期收斂速度快的特點(diǎn)，隨之pi值減小，輪盤賭法更傾向于PSO的位置更新公式，因此后期的收斂逼近效果要優(yōu)于PPSO，同時(shí)，在大量粒子的更新中，后期仍有一部分粒子使用了PPSO的位置更新公式，向外探索新的位置導(dǎo)致可能碰到更優(yōu)解，因此DPSO的尋優(yōu)效果反而比PSO更高。

在多次的重復(fù)仿真試驗(yàn)后，以上分析的總體趨勢并沒有偏差，表現(xiàn)出了較強(qiáng)的一致性。因此在適度函數(shù)為Sphere的情況下，DPSO算法的總體表現(xiàn)呈現(xiàn)了較強(qiáng)的有效性。

3 障礙水域環(huán)境模型建立及路徑規(guī)劃

3.1 水域環(huán)境模型的建立

為了驗(yàn)證水面無人艇在復(fù)雜環(huán)境水域的航行路徑規(guī)劃，本文設(shè)計(jì)了20×20的模擬海域，并在海域中投放了11個(gè)大小各異的障礙物，為了保證船體的航行安全，本研究對障礙物進(jìn)行了邊緣膨脹的處理，將每個(gè)障礙物膨脹為矩形，并保證對應(yīng)矩形的每條邊都與障礙物保持了一定的安全距離。同時(shí)，為了限制無人艇駛出模擬海域，本模型在限制海域的周邊補(bǔ)上了障礙圍欄。具體的環(huán)境水域模型如圖2所示。

圖2 環(huán)境水域模型

3.2 路徑規(guī)劃

本文在該環(huán)境模型的基礎(chǔ)上進(jìn)行仿真實(shí)驗(yàn)的過程中，發(fā)現(xiàn)在不開闊區(qū)域，尤其是被障礙物夾雜的區(qū)域，步長過長容易導(dǎo)致“粒子”突然陷入障礙物中，這既不符合船體運(yùn)動的實(shí)際情況，也違背了避障的初衷?？紤]到船體在障礙物群中行駛緩慢的特點(diǎn)以及避免船體碰撞障礙物的情形，本研究對粒子群算法做出了兩部分的調(diào)整。

1）減小了粒子的運(yùn)動步長；

2）對運(yùn)動到障礙矩形框內(nèi)的粒子進(jìn)行了姿態(tài)矯正處理。

調(diào)整操作如下：

由于障礙物邊界的寬度設(shè)定為0.5，為了避免粒子跳入障礙物中心，本文設(shè)定粒子的運(yùn)動步長固定為0.5。

本文研究是基于兩維度上的USV運(yùn)動，因此設(shè)置了兩個(gè)方向上的運(yùn)動速度，vx和vy分別表示環(huán)境模型中橫軸方向和縱軸方向的運(yùn)動速度，vT表示兩者的合速度，vx'和vy'表示調(diào)整后的速度大小，調(diào)整后的速度與原迭代得到的速度方向一致，但調(diào)整后的合速度大小固定為0.5。

如圖3所示，粒子的原位置為A，經(jīng)過下一輪迭代計(jì)算得到新的粒子位置為B，通過判斷粒子落在了圖1中灰色的障礙區(qū)域，則對粒子進(jìn)行位置矯正處理，將位置修正到障礙區(qū)域的邊緣C處，則粒子的移動由A→B矯正為A→C。

圖3 粒子的姿態(tài)矯正

算法的適應(yīng)度函數(shù)設(shè)為f：

l為粒子走過的路徑長度：

M用來表示粒子目前點(diǎn)位與目的地的距離估計(jì)，M的計(jì)算如下：

xi和yi表示粒子i的當(dāng)前位置坐標(biāo)，分別取目標(biāo)點(diǎn)橫縱軸坐標(biāo)與粒子橫縱軸坐標(biāo)的距離，用距離的和表示實(shí)際距離估計(jì)值，取權(quán)重為2，是為了加快粒子向目標(biāo)點(diǎn)靠攏。

4 仿真結(jié)果與分析

在建立了圖2的環(huán)境水域模型后，分別對初始粒子群算法（PSO）、速度權(quán)重粒子群算法（PPSO）和動態(tài)權(quán)重粒子群算法（DPSO）三種算法進(jìn)行仿真實(shí)驗(yàn)。本研究使用Python語言編輯腳本進(jìn)行仿真實(shí)驗(yàn)，算法中的具體參數(shù)設(shè)置如下：

1）設(shè)定USV的起點(diǎn)為（1，1），目的地為（20，20）；

2）粒子的規(guī)模為10，迭代次數(shù)設(shè)為100。粒子的學(xué)習(xí)因子c1=0.3，c2=0.7，慣性因子ω=0.5。

3）粒子群算法的適度函數(shù)為f。

三種算法運(yùn)算后得出的水面無人艇路徑規(guī)劃仿真如圖4～6所示。

圖4 PSO算法路徑規(guī)劃仿真結(jié)果

圖5 PPSO算法路徑規(guī)劃仿真結(jié)果

圖6 DPSO算法路徑規(guī)劃仿真結(jié)果

三種算法均有效地尋得了可行路徑，方向和路徑大體一致，但在細(xì)節(jié)處的表現(xiàn)略有偏差。在中間的開闊無障礙地帶，三種算法表現(xiàn)基本一致，但是在障礙物夾縫處，三者的表現(xiàn)出現(xiàn)了明顯的差異。在障礙物夾縫處，PSO算法和PPSO算法出現(xiàn)了明顯的直角轉(zhuǎn)彎，這是由于大量的粒子跳入障礙區(qū)之后被矯正所導(dǎo)致，水面船體的機(jī)動制動能力不如陸地載具，直角轉(zhuǎn)彎并不適合水面無人艇的航行，而DPSO算法得到的路徑在夾縫處的航行轉(zhuǎn)角均為鈍角，有利于USV航行的實(shí)現(xiàn)。

接下來分別采用PSO、PPSO、DPSO算法仿真50次。為了比較三種算法的性能，從尋找路徑的成功次數(shù)、進(jìn)入收斂階段的平均迭代代數(shù)以及平均路徑長度（僅計(jì)算尋找路徑成功的迭代次數(shù)和路徑長度）三個(gè)方面進(jìn)行對比。測試的數(shù)據(jù)如表2。

表2 三種算法的測試數(shù)據(jù)

由表2可知，DPSO無論在尋找路徑的成功率上表現(xiàn)出較強(qiáng)的優(yōu)勢，從平均路徑長度來看，較其他兩種算法略微有所縮短，而收斂速度略慢于PPSO，但是差距不大，與前面的理論分析結(jié)果一致。

實(shí)驗(yàn)數(shù)據(jù)結(jié)果表明，動態(tài)權(quán)重的粒子群算法在路徑規(guī)劃模型中的應(yīng)用，有效地提高了尋找路徑的成功率，能在一定程度上縮短路徑長度。

5 結(jié)語

本文在建立了柵格化的障礙區(qū)海域模型的基礎(chǔ)上，針對路徑規(guī)劃的問題，提出了動態(tài)權(quán)重的粒子群算法DPSO。結(jié)合水面無人艇實(shí)際的運(yùn)動情況，為了避免碰撞到障礙物，減小了粒子的運(yùn)動步長，并對跳入障礙區(qū)的粒子進(jìn)行了姿態(tài)矯正處理。

在構(gòu)建的環(huán)境海域模型中對初始粒子群算法PSO、速度權(quán)重粒子群算法PPSO和動態(tài)粒子群算法DPSO進(jìn)行仿真實(shí)驗(yàn)，結(jié)果表明，DPSO算法吸收了收斂速度快的優(yōu)點(diǎn)，而且提高了尋優(yōu)成功率，縮短了行駛路徑長度，減小了USV的轉(zhuǎn)彎幅度，突出了DPSO算法在水面無人艇路徑尋優(yōu)上的有效性。