陳世健，陳宏，丁賀，范智藝，鞏偉杰

(深圳大學 機電與控制工程學院，廣東 深圳 518060)

0 引言

現(xiàn)今，自主式水下航行器(AUV)是對神秘海洋探索的重要工具。多AUV系統(tǒng)在水下進行數(shù)據(jù)采樣、環(huán)境成像、編隊控制和合作圍捕等海洋應用領(lǐng)域展現(xiàn)廣闊的前景，而水下編隊控制是多AUV協(xié)調(diào)控制策略[1-2]最基礎(chǔ)和最重要的研究課題。美國海軍研究生院用多AUV協(xié)調(diào)對水下雷區(qū)探測清除[3]；劉瑞軒[4]從任務(wù)分配和編隊控制探討多AUV的協(xié)調(diào)控制；許彥營[5]對多AUV的協(xié)作提出分布式投標方案。

本文主要研究多AUV基于一致性算法和虛擬領(lǐng)航結(jié)構(gòu)的編隊控制，采用一致性算法[6-7]解決位置和姿態(tài)的一致性問題，以虛擬領(lǐng)航結(jié)構(gòu)實現(xiàn)預設(shè)隊形。首先，AUV收集拓撲結(jié)構(gòu)中虛擬領(lǐng)航者信息，得到各類誤差值，用一致性算法保持其狀態(tài)一致；其次，以虛擬領(lǐng)航者位置計算各AUV期望位置，設(shè)計時間限定的跟蹤控制律，確保各AUV限時內(nèi)成隊。本文使用5個AUV的通信拓撲結(jié)構(gòu)進行編隊仿真，驗證本文控制策略的可行性。

1 多AUV系統(tǒng)的一致性算法l-φ

在多機器人系統(tǒng)中，將單體AUV的動態(tài)概述為

(1)

簡化式(1)，得式(2)：

(2)

2 多AUV編隊控制系統(tǒng)的設(shè)計

建立xwOwyw世界坐標系、虛擬領(lǐng)航者本體xaOya坐標系如圖1所示。

圖1 編隊控制

(3)

圖2 軌跡跟蹤控制

圖2中：NC為非線性控制系統(tǒng)；∑i為AUV(i)運動學系統(tǒng)；Km為速度角速度限制器，避免過大的線速度和角速度，導致漂移現(xiàn)象。參考系統(tǒng)∑d的運動學方程為

可將機器人AUV(i)在xwOwyw坐標系下的位姿誤差定義為

(4)

該式表示機器人AUV(i)從xaOaya坐標系到xwOwyw坐標系的轉(zhuǎn)換，Te是轉(zhuǎn)換矩陣。對式(4)進行求導，得到誤差的動力學方程如下：

(5)

3 軌跡跟蹤控制器設(shè)計

此系統(tǒng)的特征:假設(shè)初始態(tài)為x(0)≠0，有狀態(tài)x(t)，系統(tǒng)于有限時間tx內(nèi)，讓x(t)=0。此外，得tx=-ln[ξ2/(ξ2-ξ1x0sgn(x))]/ξ1。

AUV軌跡跟蹤控制器設(shè)計的步驟如下：

1)針對式(4)的偏航角誤差狀態(tài)θei，設(shè)計其一階系統(tǒng)限定時間約束的控制器：

(6)

2)對位置誤差狀態(tài)(xei，yei)構(gòu)成的系統(tǒng)進行位置控制器設(shè)計。

借由該引理，可令vi的控制率表示成

(7)

其中：k3、k4>0；0 <β1<1；β2=2β1/(1+β1)。由此可得，系統(tǒng)的閉環(huán)形式為

(8)

因此新得到的式(8)，能在有限的時間內(nèi)，使得xei→ 0，yei→ 0。

4 數(shù)值仿真

假設(shè)某海域有5個AUV，用矩陣A表示AUV間通信網(wǎng)絡(luò)拓撲圖，第1 ～n-1行為跟隨者的相互間通信狀況，n行為虛擬領(lǐng)航者，無需主動去與其他AUV通信。鄰接權(quán)重K的設(shè)值與A有關(guān)，若能與領(lǐng)航者通信，則較大權(quán)重在與領(lǐng)航者之間；若與領(lǐng)航者失聯(lián)，則就近搜索其他AUV獲取位置信息，并加大相對于該AUV位置的權(quán)重。

4.1 定點編隊

設(shè)定仿真步長 ΔT=0.02 s，仿真時間5 s。預設(shè)在4 s內(nèi)，5個AUV的預定狀態(tài)達到一致，形成了預想的編隊形狀。

設(shè)跟隨者AUV在xwOwyw坐標系下的初始位姿分別為 (70,70,-π/6)、(200,160,2π/3)、(200,300,π/4)、(450,400,-π/4)，虛擬領(lǐng)航者AUV的初始位姿為 (550,300,π/2)、初始角速度為(π/6)rad/s、線速度為0.5 m/s。仿真開始后，各AUV跟蹤軌跡路線如圖3所示，空心圓為各AUV的初始位置，大圓點為虛擬領(lǐng)航者，灰色為跟隨者，淺灰色為各AUV的軌跡。

圖3 定點編隊路線圖

圖4 定點編隊位置誤差圖

圖5 定點編隊偏航角誤差圖

4.2 橫排編隊

設(shè)定仿真步長 ΔT=0.02 s，仿真時間5 s。預設(shè)在5 s內(nèi)，使得5個AUV的預定狀態(tài)達到一致，形成了預想的編隊形狀。

設(shè)各AUV的初始值，4個跟隨者AUV在xwOwyw坐標系下的初始位姿分別為 (40,50,-π/6)、(50,120,2π/3)、(300,130,π/4)、(500,60,-π/4)，限速5 m/s。虛擬領(lǐng)航者AUV的初始位姿為 (250,100,π/2)、初始角速度為0 rad/s和線速度0.5 m/s，中間段做正弦移動，再恢復直線。仿真開始后，各AUV的跟蹤軌跡路線如圖6所示，空心圓為各AUV的初始位置，大圓點為虛擬領(lǐng)航者，灰色為跟隨者，淺灰色為各AUV的軌跡。

圖6 橫排編隊路線圖

由圖7-圖8知，隊形在2 ～ 3 s間完成編隊，4個AUV通過網(wǎng)絡(luò)拓撲結(jié)構(gòu)獲取位置信息，與以虛擬領(lǐng)航者為參考的幾何位置及角度保持一致。同時，中間隊形變換未造成太大影響。由此可知在控制律調(diào)解下，AUV(i)在有限時間內(nèi)跟蹤上預設(shè)期望軌跡，完成橫排隊形移動控制。

圖7 橫排編隊位置誤差圖

圖8 橫排編隊偏航角誤差圖

5 結(jié)語

本文針對二維平面內(nèi)多AUV控制系統(tǒng)的編隊問題進行研究,選用漸近一致性算法和虛擬領(lǐng)航結(jié)構(gòu)法來解決問題，完成了定點編隊及橫排編隊控制，驗證了控制策略的可行性。將本課題研究的控制器應用到多AUV系統(tǒng)在三維的6自由度編隊是今后的研究重點。