面向復雜地形海洋勘探的Multi-AUV編隊協(xié)同控制策略

姜成林，徐會希

(1. 中國科學院沈陽自動化研究所 機器人學國家重點實驗室，遼寧 沈陽 110016；2. 中國科學院機器人與智能制造創(chuàng)新研究院，遼寧 沈陽 110169；3. 中國科學院大學，北京 100049)

0 引 言

近年來自主水下航行器（Autonomous Under-water Vehicle，AUV）在海洋資源勘探、羽流追蹤、海底地形測繪等方面都發(fā)揮著重要的作用[1]。但由于AUV 自身硬件的局限性和作業(yè)環(huán)境的特殊性，單個AUV 已經(jīng)無法滿足日益增加的快速大規(guī)模海洋探測任務的需求。相比之下Multi-AUV 系統(tǒng)不僅具有作業(yè)效率高、運行成本低等特點，還可以增加系統(tǒng)的冗余性和魯棒性。因此許多機構對海洋資源勘探的Multi-AUV 協(xié)同探測系統(tǒng)進行了研究，如美國的Ocean Infinity 公司[2]、日本的Zipangu 項目[3–4]、歐盟的MORPH 項目[5–6]以及XPRIZE 比賽中的SEA-KIT 項目[7]等。

Multi-AUV 系統(tǒng)的編隊控制是AUV 協(xié)同作業(yè)的關鍵技術。許多研究人員將無人車或無人機系統(tǒng)中使用的編隊控制方法應用到Multi-AUV 系統(tǒng)[8]。如通過將AUV 編隊控制分解為隊形形成、隊形保持、避障等一系列行為組合的基于行為法[9–10]；將AUV 編隊視為剛性結構，AUV 視為節(jié)點，參照虛擬結構幾何中心移動的虛擬結構法[11]；分別對目標及障礙物構建虛擬勢場，通過計算AUV 在勢場中受到的合力導出編隊控制律，從而實現(xiàn)編隊航行的人工勢場法[12–13]；以及通過指定編隊中1 臺AUV 作為領航者，其余AUV 作為跟隨者按期望距離進行跟隨從而實現(xiàn)編隊航行的領航-跟隨方法[14–16]。

但上述的方法均假設AUV 編隊處于同一水平面內(nèi)航行，并未考慮到AUV 實際的作業(yè)場景與運行環(huán)境。調(diào)查海域海底地形的起伏變化，甚至存在懸崖、海溝等復雜地形，為取得更好的勘探效果還需要維持AUV 距離海底在一定的高度，因此在執(zhí)行勘探任務時AUV 編隊可能不處于同一平面。

本文結合AUV 編隊實際的深海作業(yè)場景，將AUV 的編隊控制解耦到水平面和垂直面2 個方向分別進行控制。水平面內(nèi)采用基于領航跟隨和反饋線性化的方法，提出編隊控制律并使用李雅普諾夫穩(wěn)定性理論進行證明；垂直面內(nèi)對采集到的地形高度數(shù)據(jù)采用最小二乘法對海底地形高度進行擬合并估計的高度控制，并對可能存在的極端地形提出相應的避障策略。同時，針對AUV 編隊在作業(yè)過程中行駛的梳狀路徑提出編隊優(yōu)化策略。最后，采用“潛龍一號”AUV 作為研究對象，對所提出的協(xié)同控制策略進行仿真和驗證。

1 問題描述

Multi-AUV 與水面無人艇（Unmanned Surface Vehicle, USV）組成的異構海洋勘探作業(yè)系統(tǒng)應用場景如圖1 所示。由多臺AUV 組成的同構Multi-AUV 編隊在上方USV 搭載的超短基線（Ultra Short Baseline，USBL）提供的定位支持下，按預先規(guī)劃的作業(yè)路徑進行巡航，使用所搭載的傳感器或聲學設備對待探測區(qū)域進行掃描。為實現(xiàn)探測區(qū)域的全覆蓋掃描，需要保持編隊中各AUV 在水平方向上的相對位置。此外，為設備取得較好的探測效果，還需要維持AUV 的距底高度。考慮到海底地形的復雜變化，各AUV 將不再處于同一水平面內(nèi)，因此引入AUV 距海底的高度控制。

圖1 異構海洋勘探系統(tǒng)應用場景Fig. 1 Application scenarios of heterogeneous oceanographic exploration system

如圖1 所示，建立北東坐標系O?XYZ，針對由AUV 組成的同構Multi-AUV 編隊，建立AUV 在水平面O?XY上運動學模型：

其中： [x(t),y(t),φ(t)]為AUV 在北東慣性坐標系下的位置信息與航向信息； [v(t),ω(t)]為AUV 在載體坐標系下的線速度和角速度。

同時根據(jù)AUV 實際的水下作業(yè)環(huán)境與需求，假設編隊中各AUV 均裝備超短基線、多普勒速度計程儀、電子羅盤、深高度計以及水聲通信機等，以保證AUV可以獲得自身的狀態(tài)信息的同時并保持整個異構系統(tǒng)之間的良好通信。

在以上的條件下設定Multi-AUV 編隊的控制目標，即在t≥t0時刻，編隊中AUV 滿足：

2 編隊協(xié)同控制

Multi-AUV 系統(tǒng)的協(xié)同編隊控制策略由基于領航跟隨和反饋線性化的編隊控制器、基于最小二乘估計的高度控制與避障策略以及路徑優(yōu)化編隊策略3 個部分組成。

2.1 編隊控制器

圖2 領航跟隨與反饋線性化編隊控制Fig. 2 Leader-follower and input-output feedback linearization formation control

結合AUV 在水下的實際作業(yè)應用場景，采用領航跟隨思想與反饋線性化的編隊控制方法。如圖2 所示，由3 臺AUV 組成的同構Multi-AUV 編隊，預先指定編隊中1 臺AUV 作為整個編隊的全局領航者，編隊中其余AUV 分解為領航-跟隨者子系統(tǒng)。引入領航者信息和跟隨者信息分別生成參考AUV 和離軸點P，離軸點P以參考AUV 為目標進行跟蹤，從而保持跟隨者與領航者之間的相對位置、以及偏航角和速度的一致，從而實現(xiàn)Multi-AUV 系統(tǒng)的編隊控制。

如圖2 所示，選取領航AUV 與跟隨AUV-3 子系統(tǒng)為例。領航和跟隨AUV 的位置和航向信息在北東慣性坐標系下分別表示為 [xl(t),yl(t),φl(t)] 與[xi(t),yi(t),φi(t)](i=3)，編隊跟隨的相對距離和角度則分別為l與θ(|θ|<π)。

以跟隨AUV 信息為參考，定義位于AUV 艏部，相對質(zhì)心偏移量為d的點為離軸點P。則離軸點P的位置信息和航向信息可以表示為：

則離軸點P與參考AUV 之間的位置和航向的誤差系統(tǒng)可以表示為：

將位置誤差式（5）代入式（7）中，位置誤差在載體坐標系下可以表示為：

對式（8）進行求導，跟蹤誤差在載體坐標系式（7）下可以表示為：

為使得誤差達到最小且系統(tǒng)達到穩(wěn)定，選用以下控制律 [vi,ωi]：

控制律 [vi,ωi]中 的k1和k2選取為正增益。將選取的控制律 [vi,ωi]代入到載體坐標系下的跟蹤誤差式（10）中，則可得以下跟蹤誤差：

選取李雅普諾夫函數(shù)：

對所選取的李雅普諾夫函數(shù)進行求導，并將(11)式代入式(12)中可得：

由式（12）和式（13）易知V≥0，且當且僅當滿足z1=z2=0 時V=0。因此可知編隊的位置誤差系統(tǒng)為漸進穩(wěn)定。

其次證明AUV 之間航向角誤差eφ(t)有界，由式（11）可知航向角誤差eφ(t)的導數(shù)為：

可知式（15）漸進穩(wěn)定，對于 δ(z2,eφ)有：

當t→∞時， (1/d)k2z2→0，且考慮到AUV 運行的實際情況，角速度存在約束 ‖ωl‖≤K，因此可知δ(z2,eφ)有界。結合以上分析可知航向角的系統(tǒng)誤差eφ(t)有界。至此可證該編隊控制器李雅普諾夫穩(wěn)定。

2.2 高度控制與避障策略

不同于Multi-AUV 系統(tǒng)的圍捕與追擊任務，需要對動態(tài)障礙物進行規(guī)避，海洋探測任務中的避障任務主要針對靜態(tài)的地形避障。與路徑規(guī)劃中的繞行避障不同的是，為實現(xiàn)探測區(qū)域的全覆蓋掃描要保證AUV水平之間的相對位置，需要對極端地形進行跨越式避障。因此引入AUV 在垂直方向上的高度控制與避障策略。

考慮到實際的探測需求與作業(yè)環(huán)境，AUV 通常需要搭載側(cè)掃聲吶等聲學設備對海底指定區(qū)域進行全覆蓋掃描，為獲得更佳的探測效果，需要保持AUV 的距底高度。將動態(tài)滑動采樣和最小二乘法引入到AUV 的高度控制中。

最小二乘法是經(jīng)典的參數(shù)估計方法，具有計算簡單、運算速度快等特點。選取采用滑動采樣獲得的高度數(shù)據(jù)，使用基于最小二乘擬合的多項式模型逼近實際的海底地形高度，并對下一時刻海底地形高度進行預測。

如圖3 所示，h(·)為通過高度計測得的海底地形高度數(shù)據(jù)；t和d分別為高度計的采樣周期與高度控制的決策周期；h?(·)為通過最小二乘擬合的多項式對海底地形預測的高度。選用當前時刻及鄰近的m個高度采樣點h(nd·t),···,h((nd+m)·t)來擬合用于 表達地形的多項式：

圖3 動態(tài)滑動采樣與最小二乘擬合Fig. 3 Dynamic sliding sampling and least square fitting

式（17）可以表示為矩陣形式T·A=H，通過最小二乘法可求得系數(shù)矩陣A=[a0,···,ap]T。則預測的下一時刻海底地形的高度可以表示為：

同時設定AUV 期望的距底高度為H，因此AUV在垂直方向上的輸出Height為：

其中采樣周期t、決策周期d、用于數(shù)據(jù)擬合的采樣點個數(shù)m以及多項式階次p可結合實際進行調(diào)整。高度控制的輸出Height作為AUV 在垂直運動底層控制的輸入，底層采用PID 控制實現(xiàn)AUV 在垂直方向上運動的控制。

此外海底地形起伏變化復雜，當AUV 遇到地形高度落差極大的極端海底地形如海底懸崖、海溝等，則需要AUV 越過障礙，為此提出相應的避障策略。

如圖4 所示，當AUV 前方出現(xiàn)海底懸崖時，設定AUV 避障的安全距離為Ra與Rb，Rd表示由高度計或前視聲吶測得的AUV 與前方障礙物之間的距離，Rr(Rr>Rb>Ra)為前視聲吶或高度計的探測距離，則有如下的避障策略：

圖4 避障策略Fig. 4 Obstacle-avoidance strategy

當AUV 與障礙物之間的距離Rd大于安全距離Rb時，AUV 保持定高巡航狀態(tài)，當AUV 進入安全范圍參數(shù)Rb之內(nèi)，AUV 將全力提升高度越過障礙物。同時 當 距離Rd處于Ra與Rb之間時，AUV 巡航速度vl將隨距離Rd減小而線性減??；當距離Rd小于安全距離Ra時，巡航速度將下降至0。當編隊中某一AUV 觸發(fā)避障策略時，編隊中所有其他AUV 都會降低巡航速度保持速度的一致，維持編隊隊形的恒定。

Multi-AUV 的編隊控制與高度控制如圖5 所示?？刂破鞯目刂颇繕嗽谟诰庩犞懈SAUV 的狀態(tài)(pi,vi,φi,hi)可以對以領航AUV 狀態(tài)信息生成的參考AUV 的狀態(tài)信息 (pr,vr,φr,hr)跟隨。

圖5 Multi-AUV 編隊與高度控制示意圖Fig. 5 Schematic of horizontal and perpendicular controller for the Multi-AUV

編隊中各AUV 的狀態(tài)信息可以通過自身搭載的設備進行獲取。通過USBL 和航跡推算可以獲得AUV 自身的位置信息，多普勒速度計程儀（DVL）獲取AUV 的航行速度，電子羅盤獲得AUV 航向角和角速度，而安裝在AUV 首部的高度計可以用于探測AUV據(jù)前方障礙物的距離以確定是否執(zhí)行避障進程。

2.3 路徑變換優(yōu)化策略

梳狀掃描路徑是AUV 海洋探測任務中最常見的作業(yè)路徑。如圖6 所示，折線為“潛龍一號”AUV在執(zhí)行探測作業(yè)時行駛的梳狀掃描路徑[17]。

圖6 梳狀搜索路徑Fig. 6 Lawnmower searching path

圖7 路徑變換優(yōu)化策略Fig. 7 Route switching optimized formation strategy

圖6所示的梳狀掃描路徑，當AUV 編隊完成當前一列掃描路徑變換到下一列掃描路徑的過程中存在路徑變換的問題。圖7 左側(cè)所示的折線路徑，AUV 編隊沿AB?BC?CD折線進行航行。由于編隊左右兩側(cè)的AUV 轉(zhuǎn)彎半徑的不同會導致AUV 編隊隊形的失效同時還會導致編隊中各AUV 的航行速度發(fā)生波動。

為消除路徑變換對AUV編隊造成的不良影響，采用圓弧替代折線對路徑進行優(yōu)化，如圖7 右側(cè)所示。圓弧路徑避免了編隊兩側(cè)AUV 轉(zhuǎn)彎半徑不同的問題，使得AUV 在路徑的變換過程中，不僅可以保證AUV 編隊中各AUV 航行路徑的相同還保證AUV 編隊航行速度的恒定。

雖然優(yōu)化策略消除了路徑變換對AUV 編隊造成的不良影響，但編隊中AUV 的相對位置發(fā)生了改變。為更加直觀展示編隊中AUV 之間關系的變化，引入圖論中有向圖的鄰接矩陣Ga與編隊參數(shù)矩陣Gp來表示3-AUV 編隊中各AUV 之間的跟隨關系和位置關系：

鄰接矩陣Ga中Ai表示編隊中第i(i∈[1,2,3])號AUV，元素gi j表示編隊AUV 之間的跟隨關系Ai→Aj(i,j∈[1,2,3])：

編隊參數(shù)矩陣Ga中l(wèi)與 θ(0?<|θ|<90?)分別表示隊形的期望距離與期望角度，其中A1為AUV 編隊的全局領航者，不跟編隊中任何其他AUV，因此矩陣Ga與矩陣Gp第1 行全為0。

如圖7 右側(cè)所示，當AUV 編隊采用路徑優(yōu)化的策略后，AUV 編隊可以平滑的完成掃描路徑的變換，完成變換后AUV 編隊依然可以保持編隊的外形和航向速度，可以無需進行調(diào)整繼續(xù)進行全覆蓋掃描。而編隊內(nèi)部位置關系的改變可以通過編隊的參數(shù)矩陣Gp進行表示：

采用這種優(yōu)化的路徑變換策略，可使得編隊在路徑變換的過程中各AUV 所航行的路程以及航行的速度保持相同，同時在完成路徑變換后不需要對編隊進行調(diào)整緩沖即可立即進行作業(yè)，可以避免因路徑變換造成不利影響。

3 仿真分析

為驗證所提出的Multi-AUV 系統(tǒng)編隊控制與策略的有效性，采用“潛龍一號”AUV 作為研究對象（見圖8）。“潛龍一號”在尾部配置有4 臺矢量布置的推進器，前部分別配置有垂直方向和水平方向的槽道槳，首部裝備有高度計用作前方障礙物檢測，同時裝備有USBL、DVL、IMU、電子羅經(jīng)、深高度計、側(cè)掃聲吶等，滿足本文對AUV 的假設。仿真案例為由3 臺AUV 組成的Multi-AUV 編隊，在復雜海底環(huán)境中執(zhí)行全覆蓋探測掃描探測的任務。執(zhí)行任務的過程中，要保持AUV 編隊在執(zhí)行任務時的隊形，以及各AUV 的距底高度。采用Matlab 軟件對所提出的編隊與高度控制以及避障與路徑變換優(yōu)化策略進行數(shù)值仿真。

圖8 “潛龍一號”AUVFig. 8 Qian Long I AUV

3.1 協(xié)同編隊控制與策略仿真

設定AUV 作業(yè)航行時的速度為2 kn，小于AUV的最大續(xù)航速度3 kn；按順序設定AUV 的起始位置分別 為 (300,175)， (100,50) 與 (475,30)；初 始 航 向 分 別 為；編隊隊形為三角形，參數(shù)l與 θ分別為350 m與60°；編隊中AUV 的安全距離R為100 m，當AUV之間距離小于安全距離時，視為AUV 有碰撞危險；此外編隊中所有AUV 均處在USV 上USBL 的覆蓋范圍內(nèi)，同時Multi-AUV 編隊作業(yè)區(qū)域?qū)儆谛〕叨确秶虼瞬豢紤]洋流對AUV 的影響。

圖9 為Multi-AUV 編隊控制器的仿真過程，Multi-AUV 編隊中各AUV 可以迅速對參考AUV 狀態(tài)進行跟隨形成期望隊形并對隊形進行維持。但當領航AUV 進行轉(zhuǎn)向時，由于左右兩側(cè)跟隨AUV 轉(zhuǎn)彎半徑的不同導致兩側(cè)AUV 的速度和航向發(fā)生變化對隊形的維持產(chǎn)生不良影響。

圖9 Multi-AUV 編隊航行過程Fig. 9 Simulation result of Multi-AUV formation control

圖10和圖11 分別表示仿真過程中Multi-AUV 編隊位置誤差和編隊中各AUV 速度與航向的變化。

圖10 Multi-AUV 編隊位置誤差變化Fig. 10 Position error of the Multi-AUV formation

圖11 Multi-AUV 編隊中個體速度與航向Fig. 11 Speed and angle variation during the simulation

如圖10 與圖11 所示，開始階段AUV 可以迅速收斂到參考AUV 的位置并對生成的參考AUV 的速度和航行進行跟蹤。當領航AUV 進行轉(zhuǎn)向時，轉(zhuǎn)彎半徑不同導致兩側(cè)AUV 的速度和航向發(fā)生變化，隊形發(fā)生畸變，AUV 的位置誤差也相應增大；當領航AUV 完成轉(zhuǎn)向時，AUV 的速度和航向再次收斂到領航AUV 的狀態(tài)，位置誤差同樣需要調(diào)整時間重新收斂隊形；當領航AUV 轉(zhuǎn)彎半徑減小時，隊形將發(fā)生更嚴重的畸變。

圖12 Multi-AUV 編隊路徑變換優(yōu)化策略Fig. 12 Multi-AUV route switching optimized strategy

圖12為水平面內(nèi)Multi-AUV 編隊路徑優(yōu)化策略的仿真對比結果，圖12（a）和圖12（b）分別為未采用優(yōu)化策略以及采用優(yōu)化策略后AUV 編隊的軌跡。采用優(yōu)化策略的AUV 編隊航行軌跡明顯更加平滑，且在完成路徑變換后無需對編隊位置和狀態(tài)進行調(diào)整即可繼續(xù)進行掃描作業(yè)，相比之下未采用優(yōu)化策略的AUV 編隊在進入下一列掃描路徑后需要對AUV 編隊進行調(diào)整才可繼續(xù)進行全覆蓋掃描作業(yè)。

如圖12 仿真結果俯視圖所示，AUV 編隊在執(zhí)行路徑變換策略的過程中軌跡存在交叉，因此在仿真過程中對編隊各AUV 之間的距離進行監(jiān)測。如圖13 所示，在執(zhí)行路徑變換時AUV 之間的最小距離大于所設定的安全距離R，因此編隊中AUV 無碰撞風險。

圖13 Multi-AUV 編隊中各AUV 間距離變化Fig. 13 Distance between each AUV in the formation

3.2 高度控制與避障策略仿真

對于AUV 的高度控制，設定AUV 距離海底的期望高度H為50 m；用于地形擬合的采樣點個數(shù) 與多項式階次p分別選取為10 與2；避障策略的安全距離Ra與Rb分別選取為100 m 與50 m。值得注意的是，AUV 作業(yè)巡航的速度設定為2 kn，AUV 首部的槽道槳處于失效狀態(tài)，當且僅當AUV 觸發(fā)避障策略，航行速度降至低速時槽道槳才產(chǎn)生效果。同時，AUV 尾部裝備的4 個矢量布置的推進器可以維持AUV 在一定的俯仰角度下進行巡航。圖14 與圖15 分別為AUV 在垂直方向上的高度控制與避障策略的仿真結果與誤差。

圖14 中線1 為實際地形；線2 和線3 分別表示基于提出的最小二乘估計與未采用估計的被動高度控制的仿真結果?？梢钥闯觯瑑烧呔梢詫Φ匦芜M行有效的跟蹤。圖15 為2 種方法對地形進行定高跟蹤時的誤差變化，其中基于最小二乘估計方法的高度誤差的標準差為2.59 m 小于不采用估計的被動跟蹤方法的3.06 m。所提出的高度控制方法可以有效的進行定高航行。

圖14 AUV 高度控制仿真Fig. 14 Simulation result of AUV height control

圖15 AUV 定高航行誤差變化Fig. 15 Height error of AUV with fixed height

圖16左側(cè)為三維視圖下Multi-AUV 編隊執(zhí)行復雜地形海洋探測任務仿真結果，右側(cè)為仿真結果的俯視圖。圖中線1 表示領航AUV 的航行軌跡，線2 與線3 為跟隨AUV 的航行軌跡。通過仿真結果可以看出Multi-AUV 編隊執(zhí)行任務時，在水平方向上可以有效維持編隊的幾何構型，在垂直方向上可以獨立的維持AUV 的距底高度，并實現(xiàn)探測區(qū)域的全覆蓋。

圖16 Multi-AUV 編隊海洋勘探任務過程Fig. 16 Simulation result of Multi-AUV oceanographic exploration in three dimensions and top view

從以上的仿真實驗結果可以看出，本文提出的Multi-AUV 協(xié)同編隊控制方法和策略可以有效實現(xiàn)Multi-AUV 系統(tǒng)的編隊航行和高度控制，滿足復雜地形海洋勘探任務要求。

4 結 語

本文針對現(xiàn)在Multi-AUV 編隊執(zhí)行復雜海底地形勘探任務的實際需求與問題，提出了基于領航-跟隨思想和反饋線性化的系統(tǒng)編隊控制方法、基于滑動采樣與最小二乘估計的高度控制以及避障與路徑優(yōu)化策略，最后建立3-AUV 編隊作業(yè)場景，通過計算機仿真驗證了本文提出的協(xié)同編隊控制與策略在執(zhí)行海底復雜地形探測任務中的有效性。