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

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

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

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

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

1 波浪滑翔器編隊策略

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

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

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

(2)當(dāng)考慮波浪滑翔器的整體運(yùn)動時，水面浮體船和水下牽引機(jī)的形狀忽略不計。

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

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

(1)

(2)

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

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

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

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

1.3 間距勢能場編隊保持策略

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

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

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

(3)

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

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

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

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

(4)

2 波浪滑翔器避障策略

2.1 人工勢場法

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

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

(5)

(6)

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

2.2 局部最小域問題

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

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

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

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

針對上述局部最小域問題，通過引入一個沿障礙物切線方向的引力Uatt2來幫助波浪滑翔器編隊逃出局部最小域，如圖4所示。

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

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

(7)

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

(8)

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

(9)

3 波浪滑翔器編隊避障策略

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

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

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

4.1 波浪滑翔器三角形編隊避障仿真

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

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

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

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

圖9為波浪滑翔器編隊中各設(shè)備的速度圖，可以看出各設(shè)備的速度保持穩(wěn)定，所以編隊保持較為穩(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 有無間距勢能場編隊仿真對比

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

圖10 有間距勢能場的編隊軌跡Fig.10 Formation trajectories with spaced potential field

圖11 無間距勢能場的編隊軌跡Fig.11 Formation trajectories without spaced potential field

圖12 無間勢能場下相鄰波浪滑翔器間距Fig.12 The distances among adjacent wave gliders without spaced potential field

5 結(jié)論

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

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

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