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

嚴(yán)浙平，鄧超，趙玉飛，李本銀

(哈爾濱工程大學(xué)自動(dòng)化學(xué)院，黑龍江哈爾濱150001)

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

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

1 UUV近海底航行安全深度

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

圖1 安全深度采樣計(jì)算區(qū)域Fig.1 Sampling calculation of safety depth area

則UUV最小離地高度為

UUV航行時(shí)受自身機(jī)動(dòng)性能限制，需要滿足縱傾角與縱傾角速度約束，取縱傾角θ，最大縱傾角θmax，縱傾角速度q，最大縱傾角速度qmax則有

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

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

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

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

2 UUV路徑規(guī)劃方法

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

2.1 躲避障礙代價(jià)函數(shù)

海洋環(huán)境中存在海底山脈、礁巖、暗樁等固定障礙。如圖3所示，為實(shí)現(xiàn)對固定障礙的有效規(guī)避，需要建立UUV躲避障礙代價(jià)函數(shù)。

圖3 UUV路徑規(guī)劃示意圖Fig.3 UUV path planning schematic

UUV在前行方向上距障礙物越遠(yuǎn)，代價(jià)函數(shù)越大，反之代價(jià)函數(shù)越小。如圖3所示，當(dāng)前點(diǎn)Pi在規(guī)劃下一點(diǎn)候選點(diǎn)方向上到障礙物的直線距離為，步長為S，從而躲避障礙代價(jià)函數(shù)為

式中，fmax是一較大數(shù)值，如果候選點(diǎn)遭遇障礙，則代價(jià)函數(shù)等于較大數(shù)值，UUV前行過程中始終尋找f1小的點(diǎn)，從而實(shí)現(xiàn)對固定障礙物的規(guī)避。

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

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

2.2 趨近目標(biāo)代價(jià)函數(shù)

為實(shí)現(xiàn)UUV朝向目標(biāo)運(yùn)動(dòng)，需要建立趨近目標(biāo)代價(jià)函數(shù)。如圖4所示。

圖4 趨近目標(biāo)示意圖Fig.4 Schematic of reaching the target

當(dāng)前點(diǎn)Pi與終點(diǎn)Pg連線的角度為(ψi，θi)，候選點(diǎn)的角度為，UUV朝向目標(biāo)點(diǎn)運(yùn)動(dòng)，即有(ψi，θi)與越相近時(shí)趨近目標(biāo)代價(jià)函數(shù)越大，(ψi，θi)與差值越大，趨近目標(biāo)代價(jià)函數(shù)越小。從而有趨近目標(biāo)代價(jià)函數(shù):

2.3 UUV路徑規(guī)劃總代價(jià)函數(shù)

UUV路徑規(guī)劃總代價(jià)函數(shù)為躲避障礙代價(jià)函數(shù)與趨近目標(biāo)代價(jià)函數(shù)的疊加。從而有

式中:ki為權(quán)重系數(shù)(ki＞0)。UUV在當(dāng)前點(diǎn)規(guī)劃路徑時(shí)，利用改進(jìn)的粒子群算法尋找代價(jià)函數(shù)小的點(diǎn)，將找到的點(diǎn)作為下一時(shí)刻UUV航行路徑。

3 改進(jìn)粒子群優(yōu)化算法

粒子群優(yōu)化算法概念和參數(shù)調(diào)節(jié)都比較簡單，易于實(shí)現(xiàn)，具有較強(qiáng)的尋優(yōu)能力。一經(jīng)提出便受到廣泛關(guān)注，許多專家學(xué)者對其進(jìn)行了深入的研究和分析，提出了很多改進(jìn)策略，以提高算法的性能。典型的改進(jìn)算法有 BPSO、LWPSO、EPSO、TVAC 等［9-12］。

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

式中:vid(k)表示第k次迭代中的第i個(gè)粒子第d維的進(jìn)化速度，ci(i=1，2)表示學(xué)習(xí)因子，ri(i=1，2)是［0，1］的隨機(jī)數(shù)，pid(k)表示第k次迭代后第i粒子歷史最優(yōu)位置第d維坐標(biāo);pgd(k)表示第k次迭代后種群最優(yōu)粒子第d維坐標(biāo)。

2)線性時(shí)變慣性權(quán)重粒子群優(yōu)化算法(linearly varying inertia weight particle swarm optimization，LWPSO):

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

3)指數(shù)時(shí)變慣性權(quán)重粒子群優(yōu)化算法(exponential inertia weight particle swarm optimization，EPSO):

在BPSO的基礎(chǔ)上采用式(12)隨著迭代的進(jìn)行慣性權(quán)重成指數(shù)下降。其中M為最大迭代次數(shù)，k是當(dāng)前迭代次數(shù)。

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

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

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

其中:

式中，k表示當(dāng)前迭代次數(shù)，k0表示采用參數(shù)組合2時(shí)的迭代次數(shù)，N為常系數(shù)。

4 算法性能分析

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

采用的測試函數(shù)為

1)sphere函數(shù):

2)Rotated hyper-ellipsoid函數(shù):

3)Rosenbrock函數(shù):

4)Rastrigin函數(shù):

5)Griewank函數(shù):

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

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

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

為驗(yàn)證算法在不同維數(shù)下的性能，對測試函數(shù)，維數(shù)從10維增加到100維，分別計(jì)算了4種算法作用下結(jié)果的均值和標(biāo)準(zhǔn)差。表2給出了Rosenbrock函數(shù)由10維增加到100維的部分實(shí)驗(yàn)數(shù)據(jù)。由表2可知，隨著測試函數(shù)維數(shù)的增加各搜索算法的搜索精度和穩(wěn)定性均存在不同程度的下降，而總體來看TVAC與NPSO算法的搜索精度和穩(wěn)定性要優(yōu)于其他算法。

圖5 平均適應(yīng)度變化曲線圖Fig.5 Curves of average fitness

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

5 UUV路徑規(guī)劃仿真

建立 UUV航行過程中近海底代價(jià)函數(shù)，將NPSO用于尋找代價(jià)函數(shù)小的點(diǎn)，將找到的點(diǎn)作為下一時(shí)刻UUV航行路徑。對所提方法進(jìn)行仿真驗(yàn)證。

圖6 UUV路徑規(guī)劃仿真圖Fig.6 UUV path planning simulation

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

6 結(jié)束語

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

［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三維海底路徑規(guī)劃［J］.中國工程科學(xué)，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］武善杰，鄭征，蔡開元.基于行為協(xié)同和虛擬目標(biāo)相結(jié)合的無人機(jī)實(shí)時(shí)航路規(guī)劃［J］.控制理論與應(yīng)用，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］朱毅，張濤，宋靖雁.非完整移動(dòng)機(jī)器人的人工勢場法路徑規(guī)劃［J］.控制理論與應(yīng)用，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］張頂學(xué)，關(guān)治洪，劉新芝.一種動(dòng)態(tài)改變慣性權(quán)重的自適應(yīng)粒子群算法［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.