基于改進粒子群算法的水面無人艇全局路徑規劃*

林法君 李 焰

（1.海參軍事訓練中心 北京 100036）（2.海軍工程大學兵器工程學院 武漢 430000）

1 引言

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

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

2 改進粒子群算法

2.1 初始粒子群算法（PSO）

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

經典的粒子群算法的數學模型為位置和速度更新模型，表達式為

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

2.2 速度權重法（PPSO）

封碩等［14］指出標準的粒子群算法中的粒子位置，可能會因為后期迭代過程中速度過大，個體極值偏離全局極值，于是提出了對速度進行加權處理的方法，并給出了新的位置更新公式：

其中p為速度權重，權重p的確定如下式：

(xp，yp，…)表示在k次迭代中，當前粒子的個體極值，(xg，yg，…)表示為當前時刻粒子群的全局極值坐標點，表示前一時刻粒子群的全局極值坐標點。

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

2.3 動態速度權重法（DPSO）

在本研究仿真實驗的過程中，發現速度權重法的確大大加快了前期的收斂速度，但是在多次的仿真實驗中，該方法更容易陷入局部最優，使得找到最優路徑的不確定性大了很多。

為了既加快算法的收斂速度，又提高尋找最優解的可靠性，本文采用輪盤賭的方法結合了速度權重的粒子群算法和初始的粒子群算法。在算法的設計中加入了兩輪嵌套判斷語句對位置的更新公式做出了引導。

在每次計算p值得時候，引入一個0～1之間的隨機數o，令

pi根據當前粒子i的狀態計算得到，li表示為歸一化后的輪盤指針，由輪盤指針的落點決定位置的迭代公式。當li＜o時，執行原始粒子群算法的位置迭代公式，li≥o時，執行速度加權粒子群算法的位置迭代公式。

2.4 算法測試

為了比較原始粒子群算法（PSO）、速度加權粒子群算法（PPSO）和兩者結合的動態粒子群算法（DPSO）的表現，本文選用了Sphere函數作為適應度函數做仿真實驗。實驗首先用三個粒子群算法求解適度函數的最小值，然后對三組實驗做直觀上的比較和求解速度以及精度上的比較來評比其表現。

適度函數的計算如下：

(x1，x2，…，xn)為粒子的點坐標，Sphere函數是一個單峰函數，在坐標（0，0，…，0）處取得最小值0。算法的粒子數量定為50個，迭代次數為50次。用三種方法對其進行實驗，仿真繪圖結果如圖1所示：

圖1 Sphere函數尋優實驗

提取圖中三種算法初次進入收斂階段的迭代代數k和最優值q記于表1中。

表1 收斂階段的測試數據

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

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

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

具體原因分析如下所示。

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

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

3 障礙水域環境模型建立及路徑規劃

3.1 水域環境模型的建立

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

圖2 環境水域模型

3.2 路徑規劃

本文在該環境模型的基礎上進行仿真實驗的過程中，發現在不開闊區域，尤其是被障礙物夾雜的區域，步長過長容易導致“粒子”突然陷入障礙物中，這既不符合船體運動的實際情況，也違背了避障的初衷。考慮到船體在障礙物群中行駛緩慢的特點以及避免船體碰撞障礙物的情形，本研究對粒子群算法做出了兩部分的調整。

1）減小了粒子的運動步長；

2）對運動到障礙矩形框內的粒子進行了姿態矯正處理。

調整操作如下：

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

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

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

圖3 粒子的姿態矯正

算法的適應度函數設為f：

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

M用來表示粒子目前點位與目的地的距離估計，M的計算如下：

xi和yi表示粒子i的當前位置坐標，分別取目標點橫縱軸坐標與粒子橫縱軸坐標的距離，用距離的和表示實際距離估計值，取權重為2，是為了加快粒子向目標點靠攏。

4 仿真結果與分析

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

1）設定USV的起點為（1，1），目的地為（20，20）；

2）粒子的規模為10，迭代次數設為100。粒子的學習因子c1=0.3，c2=0.7，慣性因子ω=0.5。

3）粒子群算法的適度函數為f。

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

圖4 PSO算法路徑規劃仿真結果

圖5 PPSO算法路徑規劃仿真結果

圖6 DPSO算法路徑規劃仿真結果

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

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

表2 三種算法的測試數據

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

實驗數據結果表明，動態權重的粒子群算法在路徑規劃模型中的應用，有效地提高了尋找路徑的成功率，能在一定程度上縮短路徑長度。

5 結語

本文在建立了柵格化的障礙區海域模型的基礎上，針對路徑規劃的問題，提出了動態權重的粒子群算法DPSO。結合水面無人艇實際的運動情況，為了避免碰撞到障礙物，減小了粒子的運動步長，并對跳入障礙區的粒子進行了姿態矯正處理。

在構建的環境海域模型中對初始粒子群算法PSO、速度權重粒子群算法PPSO和動態粒子群算法DPSO進行仿真實驗，結果表明，DPSO算法吸收了收斂速度快的優點，而且提高了尋優成功率，縮短了行駛路徑長度，減小了USV的轉彎幅度，突出了DPSO算法在水面無人艇路徑尋優上的有效性。