姚 鵬，王 琨

（中國(guó)海洋大學(xué)工程學(xué)院，青島 266100）

1 引 言

作為一種具備自主導(dǎo)航與規(guī)劃能力、可代替人類執(zhí)行水下作業(yè)任務(wù)的無(wú)人運(yùn)載平臺(tái)，自治式水下機(jī)器人（Autonomous Underwater Vehicle，AUV）代表了水下航行器的未來(lái)發(fā)展方向，已逐漸應(yīng)用于軍用或民用領(lǐng)域［1-2］?，F(xiàn)階段，隨著水下應(yīng)用領(lǐng)域與應(yīng)用需求的不斷擴(kuò)大、任務(wù)場(chǎng)景與任務(wù)要求的日益復(fù)雜，如何進(jìn)一步提升AUV的自主航行能力與環(huán)境適應(yīng)性，成為亟待解決的關(guān)鍵問(wèn)題。而結(jié)合近些年AUV領(lǐng)域的發(fā)展趨勢(shì)可知，路徑規(guī)劃技術(shù)是提升AUV自主性能的關(guān)鍵技術(shù)之一，因此受到越來(lái)越多的關(guān)注［3-4］。

AUV路徑規(guī)劃問(wèn)題［5］是指依據(jù)已知或?qū)崟r(shí)探測(cè)的環(huán)境信息，選定優(yōu)化目標(biāo)（如路徑長(zhǎng)度最短、能量消耗最少、或航行時(shí)間最少等），規(guī)劃一條從起點(diǎn)到終點(diǎn)的全局路徑，且可躲避各類障礙物或威脅，確保航行安全；而當(dāng)AUV沿全局路徑航行時(shí)，如果遭遇突發(fā)障礙或威脅，需在上述基礎(chǔ)上對(duì)局部路徑進(jìn)行重規(guī)劃?，F(xiàn)有的AUV路徑規(guī)劃方法主要借鑒了地面或空中機(jī)器人的相關(guān)方法，并結(jié)合水下環(huán)境特點(diǎn)與AUV性能約束而不斷深入。針對(duì)AUV路徑規(guī)劃問(wèn)題，國(guó)內(nèi)外學(xué)者從不同角度進(jìn)行了大量的研究，現(xiàn)有方法主要包括：路標(biāo)圖法如Voronoi圖、通視圖等［5-6］，空間分解法如A*、D*、Dijkstra算法［7-8］，隨機(jī)規(guī)劃法如快速擴(kuò)展隨機(jī)樹(shù)（Rapidly-exploring Random Tree，RRT）［9-10］，數(shù)學(xué)規(guī)劃法如模型預(yù)測(cè)控制、混合整數(shù)線性規(guī)劃［11-12］，基于勢(shì)場(chǎng)的方法如人工勢(shì)場(chǎng)法、流體擾動(dòng)動(dòng)態(tài)系統(tǒng)（Interfered Fluid Dynamical System，IFDS）［13-14］，導(dǎo)引法如幾何導(dǎo)引法，行為法如模糊選擇左轉(zhuǎn)/右轉(zhuǎn)等機(jī)動(dòng)行為，以及上述各種方法的組合。

空間分解法可處理航行區(qū)域內(nèi)的不規(guī)則障礙物，使用靈活方便，受到了國(guó)內(nèi)外學(xué)者的廣泛關(guān)注，它主要包括空間建模與優(yōu)化求解兩步：首先對(duì)規(guī)劃空間進(jìn)行處理，如構(gòu)建柵格地圖，從而將路徑規(guī)劃問(wèn)題建模為典型的優(yōu)化問(wèn)題，然后采用合適的優(yōu)化搜索算法（數(shù)學(xué)優(yōu)化方法（如動(dòng)態(tài)規(guī)劃法等）、智能優(yōu)化方法（如遺傳算法、粒子群優(yōu)化等）、啟發(fā)式方法（如A*、D*）等）尋找一系列連通的單元組成最優(yōu)路徑［7-8，15-18］。例如，Zeng Z等［16］首先將規(guī)劃空間離散化為一系列包含控制點(diǎn)的殼狀空間，然后采用量子粒子群優(yōu)化方法獲得最優(yōu)配置排序，以規(guī)劃AUV全局路徑。朱大奇等［17］引入柵格信度函數(shù)和環(huán)境信息來(lái)更新自組織神經(jīng)網(wǎng)絡(luò)中的獲勝神經(jīng)元權(quán)值，以引導(dǎo)AUV在向目標(biāo)航行的過(guò)程中安全避障，并且克服了傳統(tǒng)方法中的速度跳變?nèi)毕荨erguson D等［18］在D*算法基礎(chǔ)上引入線性插值，獲得連續(xù)平滑的AUV路徑。然而，現(xiàn)有的空間分解法仍具有路徑不平滑、優(yōu)化時(shí)間長(zhǎng)等缺陷，難以兼顧路徑質(zhì)量與計(jì)算效率，需進(jìn)一步從空間建模或優(yōu)化求解等角度進(jìn)行改進(jìn)。

一致性理論是指系統(tǒng)中不同個(gè)體的狀態(tài)值趨于一致或相等，目前已廣泛應(yīng)用于多智能體編隊(duì)等領(lǐng)域［19］。文獻(xiàn)［20］在傳統(tǒng)最小一致性理論的基礎(chǔ)上，首次提出了一種引入連接權(quán)重的分布式最小一致性算法，用于尋找離散化柵格空間內(nèi)的最短路徑，該方法創(chuàng)新性地從控制的角度對(duì)路徑規(guī)劃問(wèn)題進(jìn)行了闡述，經(jīng)證明總能獲得當(dāng)前空間精度下的最優(yōu)路徑。但上述方法僅考慮了路徑長(zhǎng)度指標(biāo)，在復(fù)雜海洋環(huán)境下往往需結(jié)合洋流場(chǎng)等因素建立能量消耗或航行時(shí)間等指標(biāo)，該類指標(biāo)更加合理，上述方法僅采用了傳統(tǒng)的柵格化空間策略，路徑平滑度較差，甚至不滿足機(jī)器人運(yùn)動(dòng)約束。因此本文在上述最小一致性算法的基礎(chǔ)上，引入多層鄰居?xùn)鸥癫呗?，并基于能量消耗與平滑度指標(biāo)確定各節(jié)點(diǎn)狀態(tài)值及連接權(quán)重，構(gòu)建節(jié)點(diǎn)圖，實(shí)現(xiàn)AUV全局路徑規(guī)劃。考慮到AUV沿全局路徑航行時(shí)可能遭遇突發(fā)威脅，還引入偏離度等指標(biāo)重新定義節(jié)點(diǎn)圖，以重規(guī)劃AUV局部路徑。

2 AUV路徑規(guī)劃問(wèn)題描述

通常來(lái)說(shuō)，依據(jù)已知的離線環(huán)境信息如海底地形障礙、洋流分布等，需首先離線規(guī)劃一條從起點(diǎn)到終點(diǎn)、滿足運(yùn)動(dòng)約束、可引導(dǎo)AUV安全躲避障礙物的全局最優(yōu)路徑。但當(dāng)AUV沿該條路徑航行時(shí)，其搭載的側(cè)掃聲吶或水下相機(jī)等可能會(huì)探測(cè)到一些突發(fā)威脅如海洋生物、未知地形等，此時(shí)需進(jìn)行局部路徑重規(guī)劃，引導(dǎo)AUV偏離初始路徑以避開(kāi)突發(fā)威脅，并迅速回到初始路徑。

AUV航行空間通常遠(yuǎn)遠(yuǎn)大于AUV自身尺寸，因此路徑規(guī)劃問(wèn)題僅考慮AUV質(zhì)點(diǎn)模型。本文將規(guī)劃空間離散化為二維柵格地圖，且每個(gè)柵格設(shè)定為自由狀態(tài)（f=0）或占據(jù)狀態(tài)（f=1），用來(lái)表示該柵格是否為障礙物/威脅空間，各柵格中心點(diǎn)可表示AUV的備選路徑點(diǎn)，路徑規(guī)劃問(wèn)題可簡(jiǎn)化為選取一系列的柵格并依次連接組成路徑。為降低計(jì)算復(fù)雜度，傳統(tǒng)方法僅將AUV所在柵格的周圍8個(gè)柵格（即第一層?xùn)鸥?，L=1）作為下一時(shí)刻可選位置，但會(huì)大大降低規(guī)劃路徑的質(zhì)量如平滑度等?？紤]到本文采用的最小一致性方法的計(jì)算量較少，本文進(jìn)一步擴(kuò)大了備選鄰居?xùn)鸥竦膶訑?shù)（L≥2），即下一時(shí)刻AUV不僅可移動(dòng)至周圍8個(gè)柵格，還可移動(dòng)至更遠(yuǎn)處的柵格，從而提高路徑質(zhì)量。如圖1所示，采用四層鄰居?xùn)鸥癫呗?，AUV可由位置a直接移動(dòng)到位置b，而傳統(tǒng)方法下AUV需經(jīng)過(guò)c、d、e等點(diǎn)才能到達(dá)d，后者的規(guī)劃質(zhì)量明顯低于前者。

圖1 柵格空間下的AUV運(yùn)動(dòng)示意圖Fig.1 Illustration of AUV motion in grid-based space

3 基于最小一致性的AUV路徑規(guī)劃

3.1 最小一致性算法

首先，定義G=(V,E)為帶權(quán)重的無(wú)向連接圖，V={1,2,...,NV}表示各節(jié)點(diǎn)集合，E={(i,j)},i∈V,j∈V表示節(jié)點(diǎn)i和j連接，ai,j表示連接(i,j)的權(quán)重，令si和ui分別表示節(jié)點(diǎn)的狀態(tài)值和控制輸入，則節(jié)點(diǎn)狀態(tài)方程可定義為。傳統(tǒng)的最小一致性算法是指系統(tǒng)內(nèi)的各節(jié)點(diǎn)到達(dá)穩(wěn)定狀態(tài)，其中si(0)表示節(jié)點(diǎn)的初始狀態(tài)值，通常系統(tǒng)節(jié)點(diǎn)可分為L(zhǎng)eader節(jié)點(diǎn)V1和Follower節(jié)點(diǎn)V2，各節(jié)點(diǎn)的控制輸入可定義為：

其中：N(i)表示節(jié)點(diǎn)i的鄰居節(jié)點(diǎn)集合。通過(guò)更新各節(jié)點(diǎn)狀態(tài)，系統(tǒng)最終到達(dá)穩(wěn)定狀態(tài)。

然后，可改進(jìn)傳統(tǒng)的最小一致性算法，使其能夠適用于路徑規(guī)劃問(wèn)題。AUV規(guī)劃路徑通常由一系列路徑點(diǎn)組成，路徑指標(biāo)可定義為相鄰路徑點(diǎn)間的各路徑段指標(biāo)之和，因此可將所有的備選路徑點(diǎn)作為圖節(jié)點(diǎn)，當(dāng)前路徑點(diǎn)到終點(diǎn)的路徑指標(biāo)作為節(jié)點(diǎn)狀態(tài)值，將相鄰路徑點(diǎn)間的路徑段看作節(jié)點(diǎn)連接，路徑段指標(biāo)看作連接權(quán)重。各節(jié)點(diǎn)的控制輸入將不僅取決于鄰居節(jié)點(diǎn)狀態(tài)值，還取決于本節(jié)點(diǎn)與鄰居節(jié)點(diǎn)間的連接權(quán)重ai,j：

系統(tǒng)最終將到達(dá)如下穩(wěn)定狀態(tài)［20］：

3.2 基于能量最優(yōu)的AUV全局路徑規(guī)劃

3.2.1 全局路徑優(yōu)化指標(biāo)

眾所周知，AUV攜帶能量有限，沿路徑航行所需要的能量越少，路徑性能越好。本文主要采用能量指標(biāo)來(lái)衡量全局路徑的優(yōu)劣，為使得規(guī)劃路徑易于跟蹤，提升路徑可行性，引入平滑性指標(biāo)。AUV航行時(shí)必須滿足環(huán)境約束即躲避障礙物，在計(jì)算上述指標(biāo)之前，需首先判斷規(guī)劃路徑是否成功避障，如果不滿足環(huán)境約束，則直接引入無(wú)窮大的懲罰項(xiàng)。

假設(shè)全局路徑為X={P1,…,Pm,…,PM}，其中P1，PM分別表示AUV起點(diǎn)和終點(diǎn)，則能量指標(biāo)可定義如下［21］：

其中：k 表示AUV拖動(dòng)常量，Vr表示AUV相對(duì)于洋流的速度（相對(duì)速度），Va表示AUV相對(duì)于大地的速度（絕對(duì)速度），定義Vc為洋流速度，則三者的矢量關(guān)系為：

平滑性指標(biāo)可用全局平滑度來(lái)表示：

其中：λ1和λ2分別表示兩類指標(biāo)的權(quán)值?；谏鲜鼍C合指標(biāo)，AUV可在避開(kāi)障礙物的前提下，盡可能消耗較少的能量并平滑地從起點(diǎn)到達(dá)終點(diǎn)。

3.2.2 最小一致性算法用于AUV全局路徑規(guī)劃

為將最小一致性算法用于解決路徑規(guī)劃問(wèn)題，首先，需將AUV航行空間構(gòu)建為對(duì)應(yīng)的節(jié)點(diǎn)圖。假設(shè)規(guī)劃空間已離散化為N×N個(gè)柵格，從中可選取未被障礙物占據(jù)的柵格點(diǎn)作為圖節(jié)點(diǎn)V，進(jìn)而根據(jù)備選鄰居?xùn)鸥竦膶訑?shù)來(lái)確定鄰居節(jié)點(diǎn)，構(gòu)建節(jié)點(diǎn)圖。將任意節(jié)點(diǎn)i的狀態(tài)值si定義為按式（7）計(jì)算的從當(dāng)前節(jié)點(diǎn)i 到終點(diǎn)的路徑指標(biāo)值，以終點(diǎn)PM為L(zhǎng)eader節(jié)點(diǎn)且其初始狀態(tài)值為0，以其他節(jié)點(diǎn)為Follower節(jié)點(diǎn)且初始狀態(tài)值設(shè)置為大于0的任意值，相鄰節(jié)點(diǎn)間的連接權(quán)重ai,j可定義為：

其中：Xi→j表示節(jié)點(diǎn)i和j 之間的路徑段，能量代價(jià)和平滑性代價(jià)分別由式（4）和（6）計(jì)算獲得。

然后，各節(jié)點(diǎn)采用改進(jìn)的最小一致性算法，即利用式（2）迭代計(jì)算控制輸入值ui并更新?tīng)顟B(tài)值，最終系統(tǒng)到達(dá)穩(wěn)定狀態(tài)即式（3）。需注意的是，不管節(jié)點(diǎn)初始狀態(tài)取何值（但需大于0），經(jīng)過(guò)有限步迭代更新后系統(tǒng)總能到達(dá)穩(wěn)定狀態(tài)，因此該算法在不同場(chǎng)景下均具有較好的適應(yīng)性與較高的計(jì)算效率。在穩(wěn)定狀態(tài)下，起點(diǎn)的穩(wěn)定狀態(tài)值即表示全局路徑的最優(yōu)指標(biāo)值，因此從起點(diǎn)開(kāi)始，依次尋找父節(jié)點(diǎn)（即從鄰居節(jié)點(diǎn)集合中選取具有最小的節(jié)點(diǎn)）直至到達(dá)終點(diǎn)，即可獲得全局最優(yōu)路徑。

3.3 基于最小偏差的AUV局部路徑重規(guī)劃

AUV沿著規(guī)劃的全局路徑航行時(shí)，可能會(huì)遭遇突發(fā)障礙或威脅，因此還需進(jìn)行局部路徑重規(guī)劃，使AUV偏離初始路徑以避開(kāi)突發(fā)威脅，并迅速回到初始路徑。在確保航行安全的前提下，本文把AUV偏離初始路徑的程度作為主要指標(biāo)，并考慮平滑性等指標(biāo)。

平滑性指標(biāo)定義為：

因此路徑指標(biāo)為：

其中：λ3和λ4分別表示兩類指標(biāo)的權(quán)值。基于上述指標(biāo)，AUV可在避開(kāi)障礙的前提下盡可能靠近初始路徑航行。

然后，采用最小一致性算法獲得最優(yōu)局部路徑。節(jié)點(diǎn)圖的構(gòu)造方式類似于章節(jié)3.2.2，但需將節(jié)點(diǎn)狀態(tài)值si定義為按式（13）計(jì)算的從當(dāng)前節(jié)點(diǎn)i 到終點(diǎn)的指標(biāo)值，而相鄰節(jié)點(diǎn)間的連接權(quán)重ai,j可定義為：

其中：Yi→j表示節(jié)點(diǎn)i和j 之間的路徑段，偏離度代價(jià)和平滑性代價(jià)分別由式（9）和（12）計(jì)算獲得。最終起點(diǎn)的穩(wěn)定狀態(tài)值即表示最優(yōu)路徑指標(biāo)值，可通過(guò)迭代尋找父節(jié)點(diǎn)，最終獲得局部路徑。

此外，為減少系統(tǒng)迭代收斂到穩(wěn)定狀態(tài)所需的時(shí)間，可將全局路徑點(diǎn)集合對(duì)應(yīng)的節(jié)點(diǎn)的初始狀態(tài)值均設(shè)置為0，并且當(dāng)起點(diǎn)的狀態(tài)值到達(dá)穩(wěn)定時(shí)即可結(jié)束迭代過(guò)程。

4 仿真結(jié)果及分析

本文通過(guò)仿真實(shí)驗(yàn)來(lái)驗(yàn)證最小一致性算法用于解決路徑規(guī)劃問(wèn)題的有效性。設(shè)定規(guī)劃空間為1000m×1000m，柵格數(shù)為100×100，AUV絕對(duì)速率|Va|=2m/s，洋流場(chǎng)速率恒定，鄰居?xùn)鸥駥訑?shù)L=3，AUV起點(diǎn)為（0,0），終點(diǎn)為（1000,1000），AUV拖動(dòng)常量k=1000kg。

某復(fù)雜場(chǎng)景包含多個(gè)隨機(jī)生成的任意形狀的障礙物，洋流場(chǎng)為正弦分布且速率恒定，圖2表示采用最小一致性算法（MC）以及IFDS、RRT的規(guī)劃結(jié)果，三種方法均能獲得安全避障的全局路徑。RRT方法規(guī)劃的路徑不平滑，甚至難以跟蹤，可行性大打折扣；IFDS方法規(guī)劃的路徑雖然較平滑，但路徑走向未沿洋流方向，消耗能量較多；而MC方法規(guī)劃的路徑在一定程度上順流航行，消耗能量較少，且通過(guò)引入多層鄰居?xùn)鸥?，使得路徑較平滑。經(jīng)統(tǒng)計(jì)，基于MC、IFDS、RRT三種方法的能量消耗分別為4098kJ、5669kJ、5065kJ，說(shuō)明MC方法可獲得能量最優(yōu)的全局路徑。此外，考慮到RRT等方法的隨機(jī)特性，本文分別在50種場(chǎng)景下進(jìn)行了仿真對(duì)比，經(jīng)統(tǒng)計(jì)MC方法每次都能獲得最優(yōu)的全局路徑，具有較好的環(huán)境適應(yīng)性與魯棒性。

圖2 采用不同方法的AUV路徑規(guī)劃結(jié)果Fig.2 AUV path planning results by different methods

圖3給出了選取不同的鄰居?xùn)鸥駥訑?shù)（L=1,2,3）時(shí)的規(guī)劃結(jié)果，其中L=1表示傳統(tǒng)的柵格法。顯然，通過(guò)增加鄰居?xùn)鸥駥訑?shù)，可以大大提高規(guī)劃路徑的平滑度，減少AUV能量消耗（分別為4389kJ、4150kJ、4098kJ）。但隨著柵格層數(shù)增多，算法計(jì)算量將大大增加，而規(guī)劃性能的提升有限，因此需選取兼顧計(jì)算量與優(yōu)化性能的層數(shù)（本文選取L=3）。

圖3 采用不同L的AUV路徑規(guī)劃結(jié)果Fig.3 AUV path planning results with different L

圖4 基于最小一致性的 AUV路徑重規(guī)劃Fig.4 AUV path re-planning by minimum consensus

在上述已知場(chǎng)景基礎(chǔ)上，假設(shè)還存在一個(gè)未知的突發(fā)圓形威脅，如圖4所示。當(dāng)AUV沿初始路徑航行到（500,440）時(shí)才探測(cè)到該威脅，此時(shí)再次采取MC方法進(jìn)行局部路徑重規(guī)劃。仿真結(jié)果表明，AUV會(huì)偏離初始路徑即離線規(guī)劃路徑，以躲避該突發(fā)威脅，在避開(kāi)威脅后AUV迅速回到初始路徑，重規(guī)劃路徑的偏差程度較小、平滑度較好。

5 結(jié) 論

本文針對(duì)海洋環(huán)境下的AUV路徑規(guī)劃問(wèn)題，采取改進(jìn)的最小一致性算法，分別用于規(guī)劃基于能量最優(yōu)的全局路徑以及基于最小偏差的局部路徑，并通過(guò)仿真驗(yàn)證了本文方法在全局能量消耗、路徑平滑度、局部偏離程度等方面的優(yōu)勢(shì)。主要結(jié)論如下：

（1）改進(jìn)傳統(tǒng)的最小一致性算法，使得各節(jié)點(diǎn)的控制輸入不僅取決于鄰居節(jié)點(diǎn)的狀態(tài)值，還取決于相鄰節(jié)點(diǎn)間的連接權(quán)重。

（2）根據(jù)離散化的柵格空間構(gòu)建節(jié)點(diǎn)圖，基于能量消耗與平滑度指標(biāo)等確定各節(jié)點(diǎn)的狀態(tài)值及節(jié)點(diǎn)間的連接權(quán)重，進(jìn)而將最小一致性理論用于求解AUV全局路徑規(guī)劃問(wèn)題。

（3）基于局部路徑偏離度與平滑度指標(biāo)等，重新定義各節(jié)點(diǎn)的狀態(tài)值及節(jié)點(diǎn)間的連接權(quán)重，利用最小一致性算法進(jìn)行局部路徑重規(guī)劃。