波浪滑翔器編隊避障方法

王震， 桑宏強， 孫秀軍，2，3*

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

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

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

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

1 波浪滑翔器編隊策略

1.1 波浪滑翔器動力學建模

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

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

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

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

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

(1)

(2)

圖1 坐標系建立Fig.1 Establishment of coordinate system

1.2 領航跟隨者編隊策略

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

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

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

由多個波浪滑翔器組成的集群網絡，可以通過編隊協同控制共同完成單臺波浪滑翔器無法完成的復雜海洋任務，同時降低任務對單臺設備性能的要求及波浪滑翔器的制造成本。波浪滑翔器的協同控制主要體現在對多臺波浪滑翔器進行整體編隊和安全避障。

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

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

(3)

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

當相鄰波浪滑翔器間距rijd0時，間距勢能場會呈現吸引勢場，使得相鄰波浪滑翔器互相吸引回到相應的位置，保證編隊隊形的穩定性。三角形波浪滑翔器編隊示意圖如圖3所示。

間距勢能場產生的作用力為它的負梯度，可表示為

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

(4)

2 波浪滑翔器避障策略

2.1 人工勢場法

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

人工勢場法通過勢場函數來表達波浪滑翔器、目標點以及障礙物之間的約束關系，勢場函數的負梯度即為相應的受力表達式。傳統人工勢場法中引力勢場的數學模型和斥力勢場的數學模型分別表示為

(5)

(6)

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

2.2 局部最小域問題

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

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

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

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

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

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

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

(7)

切點的坐標可以表示為

(8)

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

(9)

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

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

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

4 仿真與結果分析

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

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

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

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

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

圖9為波浪滑翔器編隊中各設備的速度圖，可以看出各設備的速度保持穩定，所以編隊保持較為穩定。

圖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 有無間距勢能場編隊仿真對比

波浪滑翔器編隊的控制主要是用來調節相鄰波浪滑翔器之間的距離。圖10用間距勢能場實現波浪滑翔器的編隊控制，圖11的編隊避障算法不包括間距勢能場的控制，所以兩個波浪滑翔器的運動軌跡有重合的部分，表明兩設備發生了碰撞。圖12給出了無間距勢能場的作用下相鄰波浪滑翔器之間的距離，可以看出隨著編隊朝向目標點運動，受障礙物排斥勢場的影響，設備間距越來越小，在距離障礙物最近的時候WG1和WG2發生了碰撞，圖中的紅色實線表明兩個波浪滑翔器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 結論

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

(1)采用領航跟隨法與人工勢場法結合的思想，實現了波浪滑翔器編隊。通過間距勢能場產生的力使各個波浪滑翔器連接成一個構型，同時控制設備的間距，避免編隊內部發生碰撞的同時實現了編隊隊形的保持。

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