基于改進粒子群算法的水下機器人路徑規劃研究

呂詩為，朱迎谷，盧倪斌，李忻陽，劉海瑞

（上海中車艾森迪海洋裝備有限公司，上海 201306）

0 引言

近年來，隨著國內外對海洋資源探索的不斷深入和對水下作業需求的增長，水下機器人技術呈現出蓬勃發展態勢。由于具有高度智能化和自動化的工作特點，水下機器人研究是未來深海探索及資源開發的新興方向［1］。

路徑規劃是機器人研究領域的關鍵技術［2］，對水下機器人來說，路徑規劃是其能夠實現自主作業的首要前提。在復雜的水下三維環境中，機器人不僅要識別障礙物并自主避障，還要考慮自身的能量限制、行動范圍和移動距離等其他約束，因此水下機器人的路徑規劃是典型的非確定性多項式歸約難題（nondeterministic polynomial hard problem，NP-Hard）［3］。

針對水下機器人路徑規劃問題的研究方法有很多［4］，比較典型的有人工勢場法［5］、啟發式搜索、智能優化算法等。人工勢場法通過建立水下機器人與障礙物之間的虛擬勢場，從而引導其規劃出一條可行路徑。王芳等［6］基于障礙物對柵格節點的不同影響，建立改進的人工勢場算法，在一定程度上克服了原算法易出現局部極小點的問題。馬小軒等［7］通過改進斥力場函數和設置子目標點的方式，進一步優化了算法的避障性能和目標不可達問題。車建濤等［8］利用啟發式搜索中的Dijkstra算法，采用多邊形擬合和分層多面體擬合的方法對障礙物建模，實現了二維和三維最優路徑的規劃。隨著遺傳算法（genetic algorithm，GA）［9］、蟻群算法（ant colony optimization，ACO）［10］等智能優化算法的廣泛研究，越來越多的學者也將其應用在水下機器人路徑尋優問題上。顧國昌等［11］提出了一種遺傳模擬退火算法，該方法基于區域分層模型，解決了大范圍海洋環境下的路徑規劃問題。劉雨青等［12］根據水下機器人的速度和受力情況建立了水下能耗模型，利用改進蟻群算法規劃初始路徑并用貝塞爾曲線改善路徑的平滑性，所提方法具備一定的工程意義。

實際上，機器人在復雜的水下環境中運動受能源、壓力、洋流等多方面條件制約。但在以往該領域的路徑規劃研究中，大多以避障和移動距離作為約束條件，但這些尚不能完全滿足實際工況。本文基于改進的粒子群算法，從減少水下機器人能量消耗的角度出發，在避障和距離最短的優化基礎上，將艏向角和縱傾角變化以及轉向節點間距分布的均勻性納入優化計算，并采用幾何造型方法將計算后的初始路徑進行平滑處理，從而實現三維環境下水下機器人的路徑尋優。

1 問題建模

1.1 環境模型建立

作業級水下機器人通常被用于水下作業平臺檢修、管道巡檢等，需要面臨復雜的水下環境。因此合理的環境地圖模型的建立可以幫助水下機器人更快地識別環境要素，是其完成路徑規劃的前提條件。

為了描述機器人水下作業環境，本文采用函數模擬法［13］建立地形模型。其中，模擬海底或湖泊水底的基準地形數學模型為

式中：x，y——水底平面上的橫、縱坐標；H1——水底地形起伏高度；a，b，c，d，e，f，g——常數，可以控制地形特征。

模擬水下障礙的數學模型為

式中：q——障礙山峰個數；xr，yr——障礙山峰最高點處的橫、縱坐標；hr——每個障礙山峰的最高值；xsr，ysr——控制山峰坡度的常數；H2——山峰表面任意一點的高度。

在路徑規劃過程中，判斷路徑是否與障礙干涉的依據是利用插值法計算路徑插值節點（xi，yi）的高程值Hi，并代入到式（1）、式（2）中進行對比。

1.2 優化目標設計

在路徑規劃中，優化目標能直接影響路徑的尋優效果。針對水下機器人的路徑規劃大多以距離最短作為主要優化目標。但是在實際工作環境中，機器人在水下的運動受多種因素制約，單一的距離最短并不能反映出路徑的好壞?？紤]水下機器人能量消耗的限制和路徑的光滑程度，本文在距離最短以及自主避障的基礎上，將機器人本體的運動姿態角度（即艏向角、縱傾角）變化以及路徑節點距離分布納入優化目標的約束中，以多個約束條件的均衡來綜合地體現路徑對機器人能量消耗的影響。

1.2.1 約束條件

1.2.1.1 路徑長度約束

路徑長度是路徑規劃算法效果的直觀體現，路徑主要由各節點之間的線段Lj接連組成，使這些線段組成的最終路徑最短是必不可少的優化方向。Lj具體可由如下公式表示：

式中：（xi，yi，zi）和（xi+1，yi+1，zi+1）分別表示一段路徑的前后兩個節點的坐標值。

1.2.1.2 節點距離約束

考慮路徑的順滑性，機器人轉向節點的分布要盡量做到距離平均，這樣一方面可使路徑呈現比較合理的曲線，另一方面也可以將轉彎角度的累積變化平均分散到每個節點，可避免出現某個轉彎角度過大的情況，符合減少能耗的初衷。

1.2.1.3 姿態角度約束

水下機器人運動姿態角度變化對路徑的光滑程度有著直接的影響，同時，角度變化大小也反映了推進器消耗能量的多少，若路徑中各節點處轉向角度過大則意味著推進器要消耗更多的能量。另外，轉向角度的突然變大或變小對機器人自身的控制設計而言也是不合理的，一般不希望水下機器人在航行過程中做急轉動作。

水下機器人的姿態角度由艏向角、縱傾角、橫滾角決定，考慮到橫滾運動對路徑形狀的影響不大，暫時對其忽略。這里用θi來表示路徑中i點處的艏向角，用φi來表示路徑中i點處的縱傾角。為了方便表示和計算，將路徑向xOy平面投影得到艏向角、向yOz平面投影得到縱傾角，二者的表達式為

式中：Pi，XY ——路徑中的節點在xOy平面的投影點；Pi，YZ——路徑中的i節點在yOz平面的投影點。

1.2.1.4 避障約束處理

在算法尋優過程中，若出現i節點組成的路徑與障礙物發生干涉的情況，則采用罰函數法處理，即在計算適應度函數時增加適當的懲罰項，以將此種情況在迭代計算中淘汰。

1.2.2 目標函數

針對以上幾種約束情況，為達到優化目標約束條件之間的均衡，實現對水下機器人路徑的尋優計算，本文采用線性加權的方式對各約束條件函數進行處理，得到最終的目標函數F表達式為

式中：λ1、λ2、λ3——相應約束的加權系數；P——與障礙干涉時的懲罰因子，通常為較大的常數；f1——路徑長度；f2——節點距離分布；f3——姿態角度變化。

路徑長度f1表示路徑從起點到終點的總距離，主要由路徑中若干個節點組成，結合式（3）其設計公式為

式中：α——路徑中的節點數量。

節點距離f2考慮了路徑中所有節點分布的均勻性，其設計公式為

式中：——路徑中各段長度的平均值。

對于姿態角度f3的計算，在式（4）、式（5）中Pi，XY和Pi，YZ分別表示路徑中的i節點在xOy平面和yOz平面的投影點，因此θi和φi是相應投影點與其前后兩個節點形成的兩個向量的夾角，故而得到姿態角度變化的計算公式為

2 改進粒子群算法

2.1 粒子群算法原理

粒子群算法（particle swarm optimization， PSO）［14］根據鳥群捕食行為，用一種無質量的粒子來模擬鳥群中的鳥，通過個體和群體之間的信息交流共享完成捕食（尋優）動作，是一種群智能優化算法。算法中，粒子僅有速度和位置兩個屬性。速度決定了粒子的搜索快慢，位置代表粒子當前相對于最優解的移動方向。在每一次迭代中，群體中的每個粒子都會找到自己當前的最優位置，并通過相互之間的比較決定出整個群體的最優位置，進而在后面的迭代中所有粒子都會根據自身的最優解和群體最優解調整自己的速度和位置。

假設整個種群中的粒子數量為N，搜索空間的維度為d，那么第m個粒子的位置可表示為Xm=（x1，m，x2，m，…，xd，m），第m個粒子的速度可以表示為Vm=（v1，m，v2，m，…，vd，m）， 其中m=1，2， …，N。在計算過程中，還要保存每個個體的最優解pbest以及群體最優解gbest。第m個粒子根據下面的公式更新自己的速度和位置：

式中：Vm（t）——第m個粒子在第t次迭代時的速度；Xm（t）——粒子在第t次迭代時的位置；r1，r2——［0，1］范圍內的隨機數；c1，c2——認知系數及社會系數，二者代表了粒子向個體最優位置pm，best和群體最優位置gm，best逼近的趨勢，也稱加速項權重；w——慣性權重系數，代表了粒子的搜索性能，可體現粒子繼承先前速度的能力。

2.2 慣性權重的自適應改進

在粒子的速度更新公式（10）中，慣性權重系數w決定了粒子的搜索能力，w越大則對全局空間的搜索能力越強，w越小則對局部區域的搜索能力越強。因此，w的選取決定了算法的求解性能。在大多應用場景中，w的取值是隨著迭代次數線性變化的，而忽略了算法計算過程中的實時求解狀況。隨著迭代計算的進行，w也應該隨著求解情況做出適應性的調整。這里引入一種自適應慣性權重改進方法［15］，以動態計算每次迭代時的w，從而適應當前的求解情況。該方法的計算依據是群體粒子最優適應度與每個粒子最優適應度平均值的比較，從而指引粒子向更優方向前進。自適應慣性權重更新公式如下：

式中：fbest——粒子群體的最優適應度值；fm，best——第t次迭代時每個粒子的最優適應度。

2.3 B樣條曲線平滑處理

經過初始計算得到的路徑只是由若干轉向節點連接而成的分段折線，在轉彎拐點處還存在尖角，這并不符合依靠推進器驅動的水下機器人的實際運動路徑需求。因此還需要對該初始路徑進行平滑處理，使最終的路線是一條平滑連續的曲線［16］。

在幾何造型設計中，常用B 樣條曲線法對轉彎拐點進行平滑處理。根據曲線特征多邊形的頂點數，有二次、三次、四次等階次B 樣條曲線形式；又根據待處理折線中的節點區間間距是否均勻，還分為均勻和非均勻B 樣條曲線?？刂泣c數量與樣條曲線的數量決定了曲線的階次，本文采用三次均勻B 樣條曲線對路徑中的拐點進行平滑處理。該曲線的數學表達式為

式中：k——B樣條的次數；u——節點向量；n——樣條曲線的數量；l——B 樣條的序號；Nl，k（u）——k次B樣條的基函數；Pl——路徑中的控制點。

對于基函數的推導，一般遵循Cox-DeBoor［17-18］遞歸公式，這里不再贅述。經推導整理后的B樣條曲線公式矩陣形式為

2.4 路徑求解流程

改進后的粒子群算法（IPSO）求解路徑的流程如圖1所示，步驟如下：

圖1 改進粒子群算法路徑規劃流程Fig. 1 Flowchart of path planning using the improved particle swarm optimization algorithm

1） 初始化地形參數a，b，c，d，e，f以及粒子群算法基本參數N，c1，c2，w，T，設置目標起點和終點位置；

2） 所有粒子的位置及速度隨機初始化，以適當增強算法后續的計算效果；

3） 算法開始迭代，計算每一次迭代中全部粒子的速度Vm（t）和位置Xm（t）以及自適應慣性權重w（t）；

4） 計算當前迭代中每個粒子的適應度值fbest，并與粒子自身歷史最優值pbest和群體最優值gbest進行比較，進而得到當前最優適應度的粒子，即當前代數中的最優路徑；

5） 當前迭代中的粒子全部計算完畢后，更新每個粒子的最優位置以及群體最優值，得到當前全局最優粒子（即當前最優路徑），并開始下一次迭代，重復步驟3）和步驟4），直至迭代完畢；

6） 迭代計算結束，得到全局最優初始路徑（即最優粒子），再對該初始路徑進行三次均勻B樣條平滑處理；

7） 輸出得到最終路徑。

3 仿真實驗與分析

為了驗證本文所提方法的可行性與有效性，下面進行仿真實驗，并將本文方法（IPSO）與標準粒子群算法（PSO）和在以往水下機器人路徑規劃中常用的蟻群算法（ACO）進行對比。3 種算法的參數設置如表1 所示，其中γ，β，ρ，Q為ACO算法的控制參數。

表1 算法參數設置Tab.1 Parameter settings of the algorithms

在仿真實驗中，模擬場景為機器人入水后自主搜索水下作業目標。將水下環境的地形范圍設置為500 m×500 m×500 m，障礙物數量設置為7 個。3 種算法對比過程中起止點均設置為［75，450，440］和［400，50，30］。結合以上參數，將3種算法各運行20次，并統計分析20次計算后的路徑效果、求解精度以及收斂性等關鍵評價指標。

3.1 路徑可視化效果分析

圖2 中（a）、（b）、（c）分別展示了3 種算法路徑規劃三維立體圖，圖3 展示了3 種算法對應路徑的俯視圖，三角符號表示路徑中的轉向節點。從圖中可以看出，3 種算法相較之下，ACO 的路徑效果最差，具體表現為距離過長、角度變化太大以及轉向節點間距分布不合理，與理想效果還有一定差距。IPSO 與PSO 兩者從路徑距離與下降過程的平緩性來說都比較好，但IPSO顯示了更強的避障性能以及更優的路線選擇。

圖2 不同算法路徑三維立體圖對比Fig.2 Comparison of 3D views of different algorithm paths

圖3 不同算法路徑俯視圖對比Fig. 3 Comparison of top view of different algorithm paths

3.2 能耗分析及求解精度對比

在3 種算法的優化計算對比過程中隨機選擇一次，其綜合能耗隨迭代次數的收斂曲線如圖4所示。此外，3種算法20次計算后得到的路徑總長度、節點距離差異以及姿態角度變化各自的平均值如表2 所示，這些參數也綜合影響著機器人在水下的能耗，數值越小代表搜索得到的路徑越良好，消耗的能量也就越少。

表2 求解精度對比Tab. 2 Comparison of solution accuracy

圖4 算法收斂過程對比Fig. 4 Comparison of algorithm convergence processes

由圖4 不難看出，ACO 在迭代過程中過早地陷入了局部最優，導致對問題解的搜索陷入停滯，而PSO和IPSO 還能繼續保持向最優解的搜索趨勢。另外，IPSO在保持與原PSO收斂速度相當的前提下，在最優解的求解能力和求解質量上有了更進一步的提升。最終IPSO 規劃出的路徑所消耗的總體能量，相較于ACO的減少了56.6%，相較于PSO的減少了19.3%。

表2列出了各個算法在路徑總長度、轉向節點分布均勻性及姿態變化方面的目標函數差異，分別由式（7）～式（9）計算得到。其中在路徑長度上，IPSO的長度適應度值為736.157 2 m，為三者中最小，分別減少了23.5%和1.9%，說明其能夠找到更短的路徑；在節點分布方面，ACO 的適應度值為346.375 3 m，遠大于PSO 及IPSO，表明其轉向節點分布不均勻，效果最差，PSO較之有所改善，但IPSO 的數值最小，比前兩者分別下降了94.8%和50.3%，表明效果最好，節點分布更均勻；姿態角度方面，適應度值越小則表明轉向角度變化得越小，PSO 的適應度值為239.683 3°，比ACO 的628.377 8°已大幅改善，但IPSO 更小的適應度值185.841 3°顯然更具優勢。

3.3 算法穩定性分析

前文算例均默認路徑的中間轉向節點為5 個，為了測試算法在更復雜計算情況下的表現，將路徑中間節點數量依次增加，并根據式（6）對比目標函數的計算結果，如表3所列。從表中可以看出，隨著節點的增加，各個算法的適應度求解數值都有不同程度的增長。其中ACO 的變化范圍最大，表明了求解問題越復雜，性能越差；PSO算法性能較為穩定，沒有出現適應度求解數值大幅度變化的情況；IPSO 的效果最好，相比ACO它能夠將求解結果浮動減少50%～60%，對比PSO其也能有10%左右程度的改善。顯然IPSO的求解結果優于其他算法的，這也體現了該方法在多維度計算情況下較好的穩定性。

表3 不同節點數量適應度求解結果對比Tab. 3 Comparison of fitness solution results of different numbers of node

4 結束語

針對水下機器人的路徑規劃問題，本文基于函數模擬法構建了水下環境，從能耗的角度分析設計了多種約束條件下的優化目標，并對粒子群算法中的慣性權重引入自適應改進方法，對算法求解的初始路徑采用B樣條方法進行平滑處理。仿真實驗結果顯示，本文所提基于改進粒子群算法，在穩定性等方面表現良好，其中在總體能耗表現上相較于標準的粒子群算法降低了56.6%，相較于傳統蟻群算法降低了19.3%。這表明其能合理有效地輔助水下機器人規劃路徑，已達到了減少機器人能量消耗的目標，進而驗證了本文方法的有效性和準確性。本文的研究適用于對靜態環境進行路徑規劃，而對于實時的動態環境情況還需要更多考量。后續的研究將考慮水下動態障礙物對路徑的影響，從而向著工程化進一步邁進。