王震， 桑宏強(qiáng)， 孫秀軍，2，3*

(1. 天津工業(yè)大學(xué)機(jī)械工程學(xué)院， 天津 300387; 2. 中國(guó)海洋大學(xué)物理海洋教育部重點(diǎn)實(shí)驗(yàn)室， 青島 266100; 3. 青島海洋科學(xué)與技術(shù)國(guó)家實(shí)驗(yàn)室海洋動(dòng)力過(guò)程與氣候功能實(shí)驗(yàn)室， 青島 266237)

波浪滑翔器(wave glider， WG)作為一種新型的海洋探測(cè)平臺(tái)[1]，通過(guò)波浪能產(chǎn)生向前推力，同時(shí)攜帶太陽(yáng)能電池板，為長(zhǎng)期的海洋觀測(cè)任務(wù)提供了保障。波浪滑翔器具有續(xù)航時(shí)間長(zhǎng)、控制容易等優(yōu)點(diǎn)。然而單臺(tái)波浪滑翔器觀測(cè)區(qū)域有限，無(wú)法實(shí)現(xiàn)大范圍的海洋數(shù)據(jù)采集，因此需要多臺(tái)波浪滑翔器組成編隊(duì)來(lái)完成更加復(fù)雜的任務(wù)。波浪滑翔器編隊(duì)如何防止內(nèi)部的碰撞，又能安全躲避障礙物，最終順利地完成一定區(qū)域內(nèi)的觀測(cè)任務(wù)，顯得尤為重要。

為了實(shí)現(xiàn)期望的編隊(duì)行為[2-3]，學(xué)者們提出了多種思路，按照編隊(duì)控制思想可分為領(lǐng)航跟隨者法[4]、虛擬結(jié)構(gòu)法、基于一致性算法、基于行為法[5]、基于勢(shì)函數(shù)法[6-7]和圖論法[8]等。Cui等[9]應(yīng)用參考路徑和虛擬水下航行器的思想，同時(shí)通過(guò)反步法來(lái)控制編隊(duì)隊(duì)形。潘無(wú)為等[10]通過(guò)結(jié)合人工勢(shì)場(chǎng)法和虛擬結(jié)構(gòu)法，實(shí)現(xiàn)了智能水下機(jī)器人的編隊(duì)控制。Choi等[11]則應(yīng)用自適應(yīng)技術(shù)通過(guò)對(duì)領(lǐng)航者的速度進(jìn)行估計(jì)，然后利用位置信息完成編隊(duì)控制。而針對(duì)避障問(wèn)題，常見(jiàn)的避障控制算法包括遺傳算法、動(dòng)態(tài)窗口法、人工勢(shì)場(chǎng)法等。許多研究人員采用人工勢(shì)場(chǎng)法來(lái)實(shí)現(xiàn)避障控制，因?yàn)樵摲椒ň哂杏?jì)算簡(jiǎn)單、避障安全性高、實(shí)時(shí)性強(qiáng)等優(yōu)點(diǎn)。韓君[12]通過(guò)將北斗定位信息和全球定位系統(tǒng)(global positioning system，GPS)定位信息相融合，同時(shí)結(jié)合人工勢(shì)場(chǎng)法，實(shí)現(xiàn)了無(wú)人船的高精度自動(dòng)化智能航行。Wang等[13]提出了一種基于人工勢(shì)場(chǎng)算法的波浪滑翔器避障方法，通過(guò)引入一個(gè)新的斥力函數(shù)解決了局部最小域問(wèn)題，實(shí)現(xiàn)了波浪滑翔器避障，但未考慮編隊(duì)問(wèn)題。張強(qiáng)等[14]通過(guò)結(jié)合蟻群算法與人工勢(shì)場(chǎng)法，解決了自主機(jī)器人的路徑規(guī)劃問(wèn)題。Zhang等[15]通過(guò)增設(shè)虛擬目標(biāo)點(diǎn)的方法，解決了局部最小域問(wèn)題，實(shí)現(xiàn)了無(wú)人機(jī)的編隊(duì)避障控制。

由上述分析可知，雖然大多數(shù)改進(jìn)的人工勢(shì)場(chǎng)法解決了局部最小域問(wèn)題，但現(xiàn)有的研究主要集中在機(jī)器人個(gè)體的控制上，未考慮編隊(duì)內(nèi)部的碰撞及編隊(duì)的保持問(wèn)題，同時(shí)研究對(duì)象大多具有一定驅(qū)動(dòng)能力的自主水下機(jī)器人，針對(duì)波浪滑翔器這種欠驅(qū)動(dòng)無(wú)人船的研究較少。針對(duì)上述問(wèn)題，現(xiàn)通過(guò)結(jié)合領(lǐng)航跟隨者法與人工勢(shì)場(chǎng)法，將波浪滑翔器編隊(duì)視為多體系統(tǒng)，利用人工勢(shì)場(chǎng)法實(shí)現(xiàn)編隊(duì)的避障以及編隊(duì)內(nèi)部的防撞與保持，為波浪滑翔器編隊(duì)的應(yīng)用提供理論研究基礎(chǔ)。

1 波浪滑翔器編隊(duì)策略

1.1 波浪滑翔器動(dòng)力學(xué)建模

準(zhǔn)確的波浪滑翔器運(yùn)動(dòng)學(xué)模型，有利于分析和控制波浪滑翔器編隊(duì)和避障控制系統(tǒng)。由于海洋環(huán)境多變，所以波浪滑翔器的運(yùn)動(dòng)情況較為復(fù)雜。為了保證編隊(duì)避障方法研究的可靠性，對(duì)波浪滑翔器運(yùn)動(dòng)模型做出如下的簡(jiǎn)化。

(1)因?yàn)樗聽(tīng)恳龣C(jī)在水下足夠重，無(wú)論是浮體船隨波浪上升階段還是下降階段，臍帶纜始終處于拉緊狀態(tài)。因此，在將臍帶纜視為剛體的前提下，推導(dǎo)了波浪滑翔器的運(yùn)動(dòng)參數(shù)。

(2)當(dāng)考慮波浪滑翔器的整體運(yùn)動(dòng)時(shí)，水面浮體船和水下?tīng)恳龣C(jī)的形狀忽略不計(jì)。

(3)波浪滑翔器的質(zhì)量分布均勻，且原點(diǎn)位于重心。

所研究的波浪滑翔器主要是在海平面上運(yùn)動(dòng)，只考慮水面浮體船的橫蕩、縱蕩、垂蕩和艏搖4個(gè)自由度就可以模擬出波浪滑翔器的運(yùn)動(dòng)狀態(tài)。因此基于大地坐標(biāo)系(E)和船體坐標(biāo)系(F)(圖1)，建立波浪滑翔器運(yùn)動(dòng)學(xué)模型為

(1)

(2)

圖1 坐標(biāo)系建立Fig.1 Establishment of coordinate system

1.2 領(lǐng)航跟隨者編隊(duì)策略

采用領(lǐng)航跟隨者方法控制波浪滑翔器的編隊(duì)。領(lǐng)航跟隨者編隊(duì)策略中需要設(shè)定一臺(tái)波浪滑翔器為領(lǐng)航者，其余波浪滑翔器作為跟隨者，該編隊(duì)策略下的整體編隊(duì)行動(dòng)都是基于領(lǐng)航波浪滑翔器的行動(dòng)軌跡，領(lǐng)航跟隨者編隊(duì)策略示意圖如圖2所示。

圖2 領(lǐng)航跟隨者編隊(duì)示意圖Fig.2 The formation schematic of the leader and followers

1.3 間距勢(shì)能場(chǎng)編隊(duì)保持策略

由多個(gè)波浪滑翔器組成的集群網(wǎng)絡(luò)，可以通過(guò)編隊(duì)協(xié)同控制共同完成單臺(tái)波浪滑翔器無(wú)法完成的復(fù)雜海洋任務(wù)，同時(shí)降低任務(wù)對(duì)單臺(tái)設(shè)備性能的要求及波浪滑翔器的制造成本。波浪滑翔器的協(xié)同控制主要體現(xiàn)在對(duì)多臺(tái)波浪滑翔器進(jìn)行整體編隊(duì)和安全避障。

現(xiàn)有的多機(jī)器人編隊(duì)系統(tǒng)絕大多數(shù)是通過(guò)控制單體的各種局部行為，從而實(shí)現(xiàn)整體編隊(duì)系統(tǒng)的協(xié)同工作。在基于領(lǐng)航跟隨者編隊(duì)策略的基礎(chǔ)上，根據(jù)波浪滑翔器編隊(duì)的特殊海洋工作環(huán)境，考慮到各波浪滑翔器單體間信息異步傳輸?shù)膹?fù)雜性，以及各個(gè)參數(shù)獲取的不確定性等因素，引入人工勢(shì)場(chǎng)算法的思想，在波浪滑翔器編隊(duì)間建立間距勢(shì)能場(chǎng)，利用間距勢(shì)能場(chǎng)所產(chǎn)生的作用力使波浪滑翔器個(gè)體之間形成互聯(lián)，成為一個(gè)“虛擬的多波浪滑翔器系統(tǒng)”，最終實(shí)現(xiàn)編隊(duì)隊(duì)形的保持。

在相鄰兩波浪滑翔器間建立間距勢(shì)能場(chǎng)，來(lái)控制相鄰兩波浪滑翔器的間距，使它們保持在一定的距離位置上。定義間距勢(shì)能函數(shù)為

(3)

式(3)中：rij為相鄰波浪滑翔器之間的距離；d0為相鄰波浪滑翔器間的最小期望距離；d1為相鄰波浪滑翔器發(fā)生吸引作用還是排斥作用的臨界距離；kI為間距勢(shì)能場(chǎng)系數(shù)。

當(dāng)相鄰波浪滑翔器間距rijd0時(shí)，間距勢(shì)能場(chǎng)會(huì)呈現(xiàn)吸引勢(shì)場(chǎng)，使得相鄰波浪滑翔器互相吸引回到相應(yīng)的位置，保證編隊(duì)隊(duì)形的穩(wěn)定性。三角形波浪滑翔器編隊(duì)示意圖如圖3所示。

間距勢(shì)能場(chǎng)產(chǎn)生的作用力為它的負(fù)梯度，可表示為

圖3 三角形波浪滑翔器編隊(duì)Fig.3 Triangular formation of wave gliders

(4)

2 波浪滑翔器避障策略

2.1 人工勢(shì)場(chǎng)法

人工勢(shì)場(chǎng)法具有計(jì)算簡(jiǎn)單、實(shí)時(shí)性好等特點(diǎn)，采用人工勢(shì)場(chǎng)法實(shí)現(xiàn)波浪滑翔器的編隊(duì)控制，為了方便研究，障礙物都近似為圓形。

人工勢(shì)場(chǎng)法通過(guò)勢(shì)場(chǎng)函數(shù)來(lái)表達(dá)波浪滑翔器、目標(biāo)點(diǎn)以及障礙物之間的約束關(guān)系，勢(shì)場(chǎng)函數(shù)的負(fù)梯度即為相應(yīng)的受力表達(dá)式。傳統(tǒng)人工勢(shì)場(chǎng)法中引力勢(shì)場(chǎng)的數(shù)學(xué)模型和斥力勢(shì)場(chǎng)的數(shù)學(xué)模型分別表示為

(5)

(6)

式中：(x，y)為波浪滑翔器的位置坐標(biāo)；δ為吸引力系數(shù)；ρgoal為波浪滑翔器與目標(biāo)點(diǎn)之間的距離；η為排斥力系數(shù)；ρob為波浪滑翔器與障礙物之間的距離；ρ0為障礙物的影響距離，意味著只有波浪滑翔器在障礙物影響距離內(nèi)才會(huì)受到障礙物的排斥力。

2.2 局部最小域問(wèn)題

人工勢(shì)場(chǎng)法實(shí)現(xiàn)波浪滑翔器編隊(duì)避障的過(guò)程中通常存在局部最小域問(wèn)題，表現(xiàn)為引力和排斥力的合力為零。滿(mǎn)足以下任意條件則波浪滑翔器陷入局部最小域。

條件(1)：Uatt(x)+Urep(x)+UI(x)=0。

條件(2)：|xa-xb|≤αsab。

條件(1)成立表示波浪滑翔器的合勢(shì)能為零，其中，Uatt為目標(biāo)點(diǎn)的引力勢(shì)能，Urep為障礙物的排斥勢(shì)能，UI為相鄰波浪滑翔器的間距勢(shì)能。條件(2)成立表示波浪滑翔器移動(dòng)了較長(zhǎng)距離，但是位移很小，|xa-xb|表示兩點(diǎn)之間的位移大小，sab表示兩點(diǎn)間的路程，α是一個(gè)很小的正數(shù)。

針對(duì)上述局部最小域問(wèn)題，通過(guò)引入一個(gè)沿障礙物切線方向的引力Uatt2來(lái)幫助波浪滑翔器編隊(duì)逃出局部最小域，如圖4所示。

根據(jù)幾何知識(shí)，從波浪滑翔器的位置(x，y)到圓心為(xo，yo)半徑為r的障礙物可做一條切線，切線的斜率可以表示為

圖4 切線方向的新輸入引力Fig.4 The new input attracted force along tangent direction

(7)

切點(diǎn)的坐標(biāo)可以表示為

(8)

最終，沿切線方向的新輸入引力Uatt2的大小可以表示為

(9)

3 波浪滑翔器編隊(duì)避障策略

浪滑翔器編隊(duì)的控制體系主要分為3個(gè)層次，分別是任務(wù)協(xié)調(diào)層、任務(wù)執(zhí)行層和編隊(duì)避障層，如圖5所示。任務(wù)協(xié)調(diào)層的主要任務(wù)，即所有波浪滑翔器控制系統(tǒng)初始化，主要包括編隊(duì)隊(duì)形信息初始化操作、編隊(duì)任務(wù)分配初始化、設(shè)備搭載傳感器初始化操作。任務(wù)執(zhí)行層根據(jù)分配好的任務(wù)需求同時(shí)參考搭載傳感器獲得的環(huán)境信息，按照既定的虛擬領(lǐng)航者編隊(duì)方式行進(jìn)同時(shí)通過(guò)間距勢(shì)能場(chǎng)實(shí)現(xiàn)編隊(duì)隊(duì)形保持。編隊(duì)避障層在波浪滑翔器編隊(duì)行進(jìn)的過(guò)程中，根據(jù)改進(jìn)人工勢(shì)場(chǎng)法對(duì)障礙物進(jìn)行躲避，保證波浪滑翔器編隊(duì)的安全。

圖5 波浪滑翔器編隊(duì)控制體系Fig.5 Control architecture of wave glider formation

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

4.1 波浪滑翔器三角形編隊(duì)避障仿真

運(yùn)用所提出的新輸入引力的方法給出具有局部最小域下的編隊(duì)避障仿真。假設(shè)由3臺(tái)波浪滑翔器組成三角形的編隊(duì)隊(duì)形，要求波浪滑翔器編隊(duì)在保持隊(duì)形的情況下，自主地躲避障礙物抵達(dá)目標(biāo)位置。區(qū)域內(nèi)有一個(gè)半徑為50 m的圓形障礙物，圓心位于(230，230)m處。相鄰兩波浪滑翔器之間的期望距離范圍為16～18 m。波浪滑翔器的初始位置分別為WG1(50，50)m，WG2(50，30)m和WG3(30，50)m，目標(biāo)點(diǎn)位置為(400，400)m。波浪滑翔器編隊(duì)避障仿真圖如圖6所示。編隊(duì)在(80，80)m處陷入局部最小域，此時(shí)引入沿切線方向的新輸入引力幫助編隊(duì)逃離局部最小域。

圖7為波浪滑翔器和障礙物之間的距離，可以看出隨著波浪滑翔器朝向目標(biāo)點(diǎn)運(yùn)動(dòng)，距離障礙物越來(lái)越近并在75 s時(shí)距離障礙物最近，但始終與障礙物保持著安全距離，沒(méi)有發(fā)生碰撞。

圖8為波浪滑翔器編隊(duì)內(nèi)部?jī)蓛稍O(shè)備之間的距離，可以看出由于受間距勢(shì)能場(chǎng)和障礙物排斥勢(shì)場(chǎng)的共同影響，相鄰波浪滑翔器之間的距離越來(lái)越小，直到75 s時(shí)，因?yàn)槭艿秸系K物的排斥勢(shì)場(chǎng)影響最大，此時(shí)相鄰波浪滑翔器之間的間距最小。當(dāng)波浪滑翔器編隊(duì)逐漸遠(yuǎn)離障礙物后，障礙物的排斥勢(shì)場(chǎng)影響逐漸變小，此時(shí)波浪滑翔器受間距勢(shì)能場(chǎng)的影響，相鄰設(shè)備間的間距逐漸恢復(fù)到期望范圍內(nèi)。

圖6 波浪滑翔器編隊(duì)運(yùn)動(dòng)軌跡Fig.6 Motion trajectories of the wave glider formation

圖9為波浪滑翔器編隊(duì)中各設(shè)備的速度圖，可以看出各設(shè)備的速度保持穩(wěn)定，所以編隊(duì)保持較為穩(wěn)定。

圖7 波浪滑翔器和障礙物之間的距離Fig.7 The distances of the wave glider and the obstacle

圖8 相鄰波浪滑翔器之間的距離Fig.8 The distances among adjacent wave gliders

圖9 波浪滑翔器速度Fig.9 The velocities of wave gliders

4.2 有無(wú)間距勢(shì)能場(chǎng)編隊(duì)仿真對(duì)比

波浪滑翔器編隊(duì)的控制主要是用來(lái)調(diào)節(jié)相鄰波浪滑翔器之間的距離。圖10用間距勢(shì)能場(chǎng)實(shí)現(xiàn)波浪滑翔器的編隊(duì)控制，圖11的編隊(duì)避障算法不包括間距勢(shì)能場(chǎng)的控制，所以?xún)蓚€(gè)波浪滑翔器的運(yùn)動(dòng)軌跡有重合的部分，表明兩設(shè)備發(fā)生了碰撞。圖12給出了無(wú)間距勢(shì)能場(chǎng)的作用下相鄰波浪滑翔器之間的距離，可以看出隨著編隊(duì)朝向目標(biāo)點(diǎn)運(yùn)動(dòng)，受障礙物排斥勢(shì)場(chǎng)的影響，設(shè)備間距越來(lái)越小，在距離障礙物最近的時(shí)候WG1和WG2發(fā)生了碰撞，圖中的紅色實(shí)線表明兩個(gè)波浪滑翔器WG1和WG2之間的距離為零。

圖10 有間距勢(shì)能場(chǎng)的編隊(duì)軌跡Fig.10 Formation trajectories with spaced potential field

圖11 無(wú)間距勢(shì)能場(chǎng)的編隊(duì)軌跡Fig.11 Formation trajectories without spaced potential field

圖12 無(wú)間勢(shì)能場(chǎng)下相鄰波浪滑翔器間距Fig.12 The distances among adjacent wave gliders without spaced potential field

5 結(jié)論

針對(duì)欠驅(qū)動(dòng)波浪滑翔器的編隊(duì)避障問(wèn)題，提出了一種基于領(lǐng)航跟隨者法和人工勢(shì)場(chǎng)法的多波浪滑翔器編隊(duì)避障方法。通過(guò)MATLAB仿真實(shí)驗(yàn)，得出如下結(jié)論。

(1)采用領(lǐng)航跟隨法與人工勢(shì)場(chǎng)法結(jié)合的思想，實(shí)現(xiàn)了波浪滑翔器編隊(duì)。通過(guò)間距勢(shì)能場(chǎng)產(chǎn)生的力使各個(gè)波浪滑翔器連接成一個(gè)構(gòu)型，同時(shí)控制設(shè)備的間距，避免編隊(duì)內(nèi)部發(fā)生碰撞的同時(shí)實(shí)現(xiàn)了編隊(duì)隊(duì)形的保持。

(2)采用人工勢(shì)場(chǎng)法實(shí)現(xiàn)波浪滑翔器編隊(duì)的避障，針對(duì)人工勢(shì)場(chǎng)法的局部最小域問(wèn)題，通過(guò)引入沿障礙物切線方向的引力，幫助波浪滑翔器打破平衡狀態(tài)，從而逃脫局部最小域。通過(guò)仿真驗(yàn)證了所提波浪滑翔器編隊(duì)避障方法的有效性。