無人水下航行器近海底空間路徑規劃方法

嚴浙平，鄧超，趙玉飛，李本銀

(哈爾濱工程大學自動化學院，黑龍江哈爾濱150001)

近年來無人水下航行器 (unmanned underwater vehicle，UUV)作為海洋探測與開發的有力工具越來越受到人們的重視。UUV路徑規劃作為保障UUV安全、高效完成作業任務的重要功能，具有重要的現實意義。國內外眾多學者在這方面進行了大量研究，提出了很多有效規劃算法，取得了豐富的研究成果，如人工勢場、A*算法、可視圖法、快速擴展隨機數、Voronoi圖以及各種優化算法等獲得了廣泛的研究與應用［1-5］。然而，在一些特殊的偵查和探測任務中，UUV在回避障礙威脅的同時，還需要實現對地形的跟隨。在這種情況下，不能使用單純朝向目標的規劃方法，而要對地形威脅進行約束，使UUV在保證安全的情況下，實現近海底航行。

優化算法本身具有較強的尋優能力，而路徑規劃更多的是需要得到一個較優化的路徑，因此，將優化算法用于路徑規劃問題是研究的一個重要方面。作為仿生智能優化算法的一種，粒子群優化算法(particle swarm optimization，PSO)，具有概念簡單、參數調節方便、較強的尋優能力等特點，一經提出便受到了廣泛的關注［6-8］。本文考慮地形威脅約束，結合UUV自身性能，計算UUV近海底航行安全深度。將UUV近海底航行安全深度作為UUV航行深度，提出UUV躲避障礙和趨近目標代價函數，最終得到UUV近海底航行路徑規劃代價函數。提出一種改進的粒子群算法，將其用于路徑規劃代價函數尋優，以實現UUV近海底航行路徑規劃。

1 UUV近海底航行安全深度

由于海底地形起伏分布有著很大的差異，再加上UUV航行性能和航行速度的不同，在UUV穿越區域內不同方向，UUV安全航行深度有所不同。這就需要計算UUV當前點航行方向上的最大安全深度。假設UUV當前路徑點為P點，計算UUV路徑點P的安全深度，圍繞P點沿著UUV前進方向，向外擴展一定區域，如圖1所示，通過計算該區域的安全度，得到P的安全深度。首先將采樣計算區域網格化，然后計算網格點的均方差δ，利用均方差δ表達地形的起伏度，從而確定UUV的安全航行深度。

圖1 安全深度采樣計算區域Fig.1 Sampling calculation of safety depth area

則UUV最小離地高度為

UUV航行時受自身機動性能限制，需要滿足縱傾角與縱傾角速度約束，取縱傾角θ，最大縱傾角θmax，縱傾角速度q，最大縱傾角速度qmax則有

圖2所示，為縱傾角約束下最小離地高度示意圖，路徑點在采樣計算區域中，UUV縱傾角約束下UUV最小離地高度h2為使最大仰艏前行曲線在采樣計算區域中海底地形上方30 m的路徑點高度。

受水密性、材料、耐壓性能的影響，UUV具有一定的下潛深度約束Dmax，若P點垂面內海洋深度為Hp，則下潛深度約束下最小離地高度為

綜合以上約束，得到UUV近海底航行路徑點最小離地高度為

圖2 縱傾角約束下最小離地高度示意圖Fig.2 Pitch angle constraints under the minimum height

2 UUV路徑規劃方法

UUV規劃起始點坐標為P0(x0，y0，z0)，路徑點坐標為pi(xi，yi，zi)，其中i=1，2，…，n，終點坐標為pg(xg，yg，zg)。為簡化問題，規劃過程中，步長S在水平面內的投影為定值。這樣每一步搜索只需要尋找最優的前進角度ψ、θ即可。每步規劃中，首先建立UUV路徑規劃代價函數，然后利用粒子群算法在搜索空間尋找具有最優代價函數路徑點，從而得到下一個路徑點。

2.1 躲避障礙代價函數

海洋環境中存在海底山脈、礁巖、暗樁等固定障礙。如圖3所示，為實現對固定障礙的有效規避，需要建立UUV躲避障礙代價函數。

圖3 UUV路徑規劃示意圖Fig.3 UUV path planning schematic

UUV在前行方向上距障礙物越遠，代價函數越大，反之代價函數越小。如圖3所示，當前點Pi在規劃下一點候選點方向上到障礙物的直線距離為，步長為S，從而躲避障礙代價函數為

式中，fmax是一較大數值，如果候選點遭遇障礙，則代價函數等于較大數值，UUV前行過程中始終尋找f1小的點，從而實現對固定障礙物的規避。

利用之前所述的UUV最小離地高度，可以計算出確定艏向角ψ時，下一路徑點處UUV最小離地高度，進而計算出該路徑點的縱傾角θ，即有

從而躲避障礙代價函數為自變量為ψ的一維代價函數，式(4)也可化為

2.2 趨近目標代價函數

為實現UUV朝向目標運動，需要建立趨近目標代價函數。如圖4所示。

圖4 趨近目標示意圖Fig.4 Schematic of reaching the target

當前點Pi與終點Pg連線的角度為(ψi，θi)，候選點的角度為，UUV朝向目標點運動，即有(ψi，θi)與越相近時趨近目標代價函數越大，(ψi，θi)與差值越大，趨近目標代價函數越小。從而有趨近目標代價函數:

2.3 UUV路徑規劃總代價函數

UUV路徑規劃總代價函數為躲避障礙代價函數與趨近目標代價函數的疊加。從而有

式中:ki為權重系數(ki＞0)。UUV在當前點規劃路徑時，利用改進的粒子群算法尋找代價函數小的點，將找到的點作為下一時刻UUV航行路徑。

3 改進粒子群優化算法

粒子群優化算法概念和參數調節都比較簡單，易于實現，具有較強的尋優能力。一經提出便受到廣泛關注，許多專家學者對其進行了深入的研究和分析，提出了很多改進策略，以提高算法的性能。典型的改進算法有 BPSO、LWPSO、EPSO、TVAC 等［9-12］。

1)基本粒子群優化算法(basic particle swarm optimization，BPSO):

式中:vid(k)表示第k次迭代中的第i個粒子第d維的進化速度，ci(i=1，2)表示學習因子，ri(i=1，2)是［0，1］的隨機數，pid(k)表示第k次迭代后第i粒子歷史最優位置第d維坐標;pgd(k)表示第k次迭代后種群最優粒子第d維坐標。

2)線性時變慣性權重粒子群優化算法(linearly varying inertia weight particle swarm optimization，LWPSO):

即在BPSO基礎上，采用式(11)的慣性權重。搜索初期慣性權重比較大，隨著搜索的進行，慣性權重線性下降，搜索后期慣性權重較小。其中ωmax表示最大慣性權重，ωmin表示最小慣性權重，M表示最大迭代次數。

3)指數時變慣性權重粒子群優化算法(exponential inertia weight particle swarm optimization，EPSO):

在BPSO的基礎上采用式(12)隨著迭代的進行慣性權重成指數下降。其中M為最大迭代次數，k是當前迭代次數。

4)具有時變加速因子的自組織粒子群優化算法(time-varying acceleration co-efficient，TVAC):

即在BPSO基礎上搜索初期c1＞c2提高搜索初期的全局搜索能力，搜索后期c1＜c2有利于粒子收斂于全局最優解。其中cmax表示最大學習因子，cmin表示最小學習因子。

影響粒子群搜索性能的因素主要有自身慣性速度，自身歷史最優位置，以及群體歷史最優位置3種因素。為有效利用這些因素，本文提出一種改進的粒子群優化算法(novel particle swarm optimization，NPSO)，將群體中粒子編號，每次迭代，按粒子序號順序計算子粒子代價函數。粒子初始進化時，應具有較大的慣性權重，粒子更側重于自身經驗的搜索，以增強全局搜索能力。所有粒子均取參數組合1:較大的慣性權重ω=ωmax，較大的自身經驗學習因子c1=cmax，較小的群體經驗學習因子c2=cmin。如果粒子代價函數較上次迭代的代價函數更優，則繼續采用參數組合1。若粒子代價函數并不比上次迭代的歷史最優代價函數更優，則說明粒子接近較優點，此時，應采用較小的慣性權重，加強局部搜索，而且粒子進化側重于群體經驗，粒子向群體最優點運動。故從下個粒子進化開始采用參數組合2:較小的慣性權重ω=ωmin，較小的自身經驗學習因子c1=cmin，較大的群體經驗學習因子c2=cmax。如果粒子代價函數較上次迭代的代價函數更優，則采用參數組合1。如果粒子最優代價函數始終不變，則粒子可能進入局部最優點，應再次加大自身經驗的比重。因此參數組合2改為:較小的慣性權重ω=ωmin，學習因子隨著迭代次數的增加而變化，有

其中:

式中，k表示當前迭代次數，k0表示采用參數組合2時的迭代次數，N為常系數。

4 算法性能分析

為衡量算法的性能，采用不同的測試函數對算法進行測試，并將 NPSO與 BPSO、LWPSO、EPSO、TVAC 性能進行比較［9-12］。

采用的測試函數為

1)sphere函數:

2)Rotated hyper-ellipsoid函數:

3)Rosenbrock函數:

4)Rastrigin函數:

5)Griewank函數:

仿真時，對于 NPSO、BPSO、LWPSO、EPSO、TVAC取種群粒子個數為n=30。學習因子取c1=c2=2．0，自變量維數D=10，慣性因子ω設為 0．7，ωmax設為 0．9，ωmin設為0．4，cmax=2．5，cmin=0．5，N=10，最大迭代次數為 1 000。搜索空間xi∈[-100，100]，速度變量vi∈ [-100，100]，搜索過程中，如果粒子位置超出邊界，則此時邊界坐標設為粒子位置坐標，并將速度設為零。如果速度超出邊界，則將速度邊界值設為速度值。為了獲得更加準確的仿真結果，對每個函數的優化都重復100次，每次都迭代1 000次，對得到的100個結果求取平均數和標準差，仿真結果如表1所示。

表1 BPSO、LWPSO、EPSO、TVAC和NPSO對比結果Table 1 Comparison results of BPSO，LWPSO，EPSO，TVAC and NPSO

由表1可以看出，對于單模態函數，搜索精度和穩定性由低到高依次為 BPSO、LWPSO、EPSO、TVAC、NPSO，唯獨對于高度多模態 Rastrigin函數NPSO搜索效果并不理想。Rosenbrock函數f3和Griewank函數f5進化過程中的平均適應度變化如圖5所示。由圖5中可以看出，搜索速度由快到慢依次為 TVAC、NPSO、EPSO、LWPSO、BPSO。其中NPSO與TVAC的搜索速度相當。

為驗證算法在不同維數下的性能，對測試函數，維數從10維增加到100維，分別計算了4種算法作用下結果的均值和標準差。表2給出了Rosenbrock函數由10維增加到100維的部分實驗數據。由表2可知，隨著測試函數維數的增加各搜索算法的搜索精度和穩定性均存在不同程度的下降，而總體來看TVAC與NPSO算法的搜索精度和穩定性要優于其他算法。

圖5 平均適應度變化曲線圖Fig.5 Curves of average fitness

表2 Rosenbrock函數不同維數下BPSO和GHEPSO對比結果Table 2 Comparison results of BPSO and GHEPSO on Rosenbrock function under the different dimensions

5 UUV路徑規劃仿真

建立 UUV航行過程中近海底代價函數，將NPSO用于尋找代價函數小的點，將找到的點作為下一時刻UUV航行路徑。對所提方法進行仿真驗證。

圖6 UUV路徑規劃仿真圖Fig.6 UUV path planning simulation

選取海底三維地形圖的一部分{［0，14］，［0，17］，［0，-0.3］}km 作為規劃空間。取起始點為(2，2，-0.17)km，目標點為(11，15，-0.125)km。縱傾角約束為，艏向角約束為ψmax=60°，其中Δψ為相鄰2個路徑段之間艏向角的變化，仿真結果如圖6所示。由圖6可以看出，UUV可以很好地規避障礙威脅，在安全航行高度近海底航行，得到了有效地近海底航行路徑。

6 結束語

為實現UUV近海底航行，綜合考慮地形約束以及UUV自身性能約束，提出了UUV近海底航行安全深度計算方法，同時，將基本粒子群算法進行了改進，然后利用幾個典型的測試函數對NPSO的性能進行了分析。結果表明，對于單模態函數NPSO在搜索精度、穩定性和搜索速度上均優于BPSO、LWPSO、EPSO、TVAC，對于多模態函數 NPSO性能與BPSO相當。在UUV近海底空間路徑規劃過程中，結合UUV近海底安全航行高度建立UUV近海底路徑規劃代價函數，利用粒子群算法對代價函數尋優，找到較優的路徑點。仿真結果顯示，UUV可以在近海底安全航行高度上實現對地形威脅和障礙威脅的規避，并朝向目標點航行，最終得到安全有效的近海底航行路徑。

［1］LIU Liqiang，DAI Yuntao.3D space path planning of complex environmental underwater vehicle［C］//International Joint Conference on Computational Sciences and Optimization.Sanya，China，2009:204-209.

［2］郝燕玲，張京娟.基于遺傳算法的AUV三維海底路徑規劃［J］.中國工程科學，2003，5(11):56-60.HAO Yanling，ZHANG Jingjuan.AUV path planning in 3D seabed environment using genetic algorithm［J］.Engineering Science，2003，5(11):56-60.

［3］NATHAN E B.Three-dimensional route planner usingA*algorithm application to autonomous underwater vehicles［R］.Baton Rouge:Louisiana State University，2008.

［4］武善杰，鄭征，蔡開元.基于行為協同和虛擬目標相結合的無人機實時航路規劃［J］.控制理論與應用，2011，28(1):131-136.WU Shanjie，ZHENG Zheng，CAI Kaiyuan.Real-time path planning for unmanned aerial vehicles using behavior coordination and virtual goal［J］.Control Theory and Applications，2011，28(1):131-136.

［5］朱毅，張濤，宋靖雁.非完整移動機器人的人工勢場法路徑規劃［J］.控制理論與應用，2010，24(2):154-161.ZHU Yi，ZHANG Tao，SONG Jingyan.Path planning for nonholonomic mobile robots using artificial potentical field method［J］.Control Theory and Application，2010，24(2):154-161.

［6］MONDAL D，CHAKRABARTI A，SENGUPTA A.Optimal placement and parameter setting of SVC and TCSC using PSO to mitigate small signal stability problem［J］.International Journal of Electrical Power and Energy Systems，2012，42(1):334-340.

［7］ISHAQUE K，SALAM Z，AMJAD M，et al.An improved particle swarm optimization(PSO)-based MPPT for PV with reduced steady-state oscillation［J］.IEEE Transactions on Power Electronics，2012，27(8):3627-3638.

［8］MOHAMMADI-IVATLOO B，RABIEE A，SOROUDI A，et al.Iteration PSO with time varying acceleration coefficients for solving non-convex economic dispatch problems［J］.International Journal of Electrical Power ＆ Energy Systems，2012，42(1):508-516.

［9］張頂學，關治洪，劉新芝.一種動態改變慣性權重的自適應粒子群算法［J］.控制與決策，2008，23(11):1253-1257.ZHANG Dingxue，GUAN Zhihong，LIU Xinzhi.Adaptive particle swarm optimization algorithm with dynamically changing inertia weight［J］.Control and Decision，2008，23(11):1253-1257.

［10］RATNAWEERA A，HALGAMUGE S K，WATSON H C.Self-organizing hierarchical particle swarm optimizer with time-varying acceleration coefficients［J］.IEEE Transactions on Evolutionary Computation，2004，8(3):240-255.

［11］SHI Y，EBERHART R.A modified particle swarm optimizer［C］//Evolutionary Computation Proceedings.Anchorage，USA，1998:69-73.

［12］CHEN G，HUANG X，JIA J，et al.Natural exponential inertia weight strategy in particle swarm optimization［C］//Intelligent Control and Automation.Dalian，China，2006:3672-3675.