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

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

0 引言

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

路徑規(guī)劃是機(jī)器人研究領(lǐng)域的關(guān)鍵技術(shù)［2］，對水下機(jī)器人來說，路徑規(guī)劃是其能夠?qū)崿F(xiàn)自主作業(yè)的首要前提。在復(fù)雜的水下三維環(huán)境中，機(jī)器人不僅要識別障礙物并自主避障，還要考慮自身的能量限制、行動范圍和移動距離等其他約束，因此水下機(jī)器人的路徑規(guī)劃是典型的非確定性多項(xiàng)式歸約難題（nondeterministic polynomial hard problem，NP-Hard）［3］。

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

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

1 問題建模

1.1 環(huán)境模型建立

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

為了描述機(jī)器人水下作業(yè)環(huán)境，本文采用函數(shù)模擬法［13］建立地形模型。其中，模擬海底或湖泊水底的基準(zhǔn)地形數(shù)學(xué)模型為

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

模擬水下障礙的數(shù)學(xué)模型為

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

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

1.2 優(yōu)化目標(biāo)設(shè)計(jì)

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

1.2.1 約束條件

1.2.1.1 路徑長度約束

路徑長度是路徑規(guī)劃算法效果的直觀體現(xiàn)，路徑主要由各節(jié)點(diǎn)之間的線段Lj接連組成，使這些線段組成的最終路徑最短是必不可少的優(yōu)化方向。Lj具體可由如下公式表示：

式中：（xi，yi，zi）和（xi+1，yi+1，zi+1）分別表示一段路徑的前后兩個節(jié)點(diǎn)的坐標(biāo)值。

1.2.1.2 節(jié)點(diǎn)距離約束

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

1.2.1.3 姿態(tài)角度約束

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

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

式中：Pi，XY ——路徑中的節(jié)點(diǎn)在xOy平面的投影點(diǎn)；Pi，YZ——路徑中的i節(jié)點(diǎn)在yOz平面的投影點(diǎn)。

1.2.1.4 避障約束處理

在算法尋優(yōu)過程中，若出現(xiàn)i節(jié)點(diǎn)組成的路徑與障礙物發(fā)生干涉的情況，則采用罰函數(shù)法處理，即在計(jì)算適應(yīng)度函數(shù)時增加適當(dāng)?shù)膽土P項(xiàng)，以將此種情況在迭代計(jì)算中淘汰。

1.2.2 目標(biāo)函數(shù)

針對以上幾種約束情況，為達(dá)到優(yōu)化目標(biāo)約束條件之間的均衡，實(shí)現(xiàn)對水下機(jī)器人路徑的尋優(yōu)計(jì)算，本文采用線性加權(quán)的方式對各約束條件函數(shù)進(jìn)行處理，得到最終的目標(biāo)函數(shù)F表達(dá)式為

式中：λ1、λ2、λ3——相應(yīng)約束的加權(quán)系數(shù)；P——與障礙干涉時的懲罰因子，通常為較大的常數(shù)；f1——路徑長度；f2——節(jié)點(diǎn)距離分布；f3——姿態(tài)角度變化。

路徑長度f1表示路徑從起點(diǎn)到終點(diǎn)的總距離，主要由路徑中若干個節(jié)點(diǎn)組成，結(jié)合式（3）其設(shè)計(jì)公式為

式中：α——路徑中的節(jié)點(diǎn)數(shù)量。

節(jié)點(diǎn)距離f2考慮了路徑中所有節(jié)點(diǎn)分布的均勻性，其設(shè)計(jì)公式為

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

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

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

2.1 粒子群算法原理

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

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

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

2.2 慣性權(quán)重的自適應(yīng)改進(jìn)

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

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

2.3 B樣條曲線平滑處理

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

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

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

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

2.4 路徑求解流程

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

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

1） 初始化地形參數(shù)a，b，c，d，e，f以及粒子群算法基本參數(shù)N，c1，c2，w，T，設(shè)置目標(biāo)起點(diǎn)和終點(diǎn)位置；

2） 所有粒子的位置及速度隨機(jī)初始化，以適當(dāng)增強(qiáng)算法后續(xù)的計(jì)算效果；

3） 算法開始迭代，計(jì)算每一次迭代中全部粒子的速度Vm（t）和位置Xm（t）以及自適應(yīng)慣性權(quán)重w（t）；

4） 計(jì)算當(dāng)前迭代中每個粒子的適應(yīng)度值fbest，并與粒子自身歷史最優(yōu)值pbest和群體最優(yōu)值gbest進(jìn)行比較，進(jìn)而得到當(dāng)前最優(yōu)適應(yīng)度的粒子，即當(dāng)前代數(shù)中的最優(yōu)路徑；

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

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

7） 輸出得到最終路徑。

3 仿真實(shí)驗(yàn)與分析

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

表1 算法參數(shù)設(shè)置Tab.1 Parameter settings of the algorithms

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

3.1 路徑可視化效果分析

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

圖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 種算法的優(yōu)化計(jì)算對比過程中隨機(jī)選擇一次，其綜合能耗隨迭代次數(shù)的收斂曲線如圖4所示。此外，3種算法20次計(jì)算后得到的路徑總長度、節(jié)點(diǎn)距離差異以及姿態(tài)角度變化各自的平均值如表2 所示，這些參數(shù)也綜合影響著機(jī)器人在水下的能耗，數(shù)值越小代表搜索得到的路徑越良好，消耗的能量也就越少。

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

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

由圖4 不難看出，ACO 在迭代過程中過早地陷入了局部最優(yōu)，導(dǎo)致對問題解的搜索陷入停滯，而PSO和IPSO 還能繼續(xù)保持向最優(yōu)解的搜索趨勢。另外，IPSO在保持與原PSO收斂速度相當(dāng)?shù)那疤嵯拢谧顑?yōu)解的求解能力和求解質(zhì)量上有了更進(jìn)一步的提升。最終IPSO 規(guī)劃出的路徑所消耗的總體能量，相較于ACO的減少了56.6%，相較于PSO的減少了19.3%。

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

3.3 算法穩(wěn)定性分析

前文算例均默認(rèn)路徑的中間轉(zhuǎn)向節(jié)點(diǎn)為5 個，為了測試算法在更復(fù)雜計(jì)算情況下的表現(xiàn)，將路徑中間節(jié)點(diǎn)數(shù)量依次增加，并根據(jù)式（6）對比目標(biāo)函數(shù)的計(jì)算結(jié)果，如表3所列。從表中可以看出，隨著節(jié)點(diǎn)的增加，各個算法的適應(yīng)度求解數(shù)值都有不同程度的增長。其中ACO 的變化范圍最大，表明了求解問題越復(fù)雜，性能越差；PSO算法性能較為穩(wěn)定，沒有出現(xiàn)適應(yīng)度求解數(shù)值大幅度變化的情況；IPSO 的效果最好，相比ACO它能夠?qū)⑶蠼饨Y(jié)果浮動減少50%～60%，對比PSO其也能有10%左右程度的改善。顯然IPSO的求解結(jié)果優(yōu)于其他算法的，這也體現(xiàn)了該方法在多維度計(jì)算情況下較好的穩(wěn)定性。

表3 不同節(jié)點(diǎn)數(shù)量適應(yīng)度求解結(jié)果對比Tab. 3 Comparison of fitness solution results of different numbers of node

4 結(jié)束語

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