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

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

0 引言

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

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

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

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

(1)

簡(jiǎn)化式(1)，得式(2)：

(2)

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

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

圖1 編隊(duì)控制

(3)

圖2 軌跡跟蹤控制

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

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

(4)

該式表示機(jī)器人AUV(i)從xaOaya坐標(biāo)系到xwOwyw坐標(biāo)系的轉(zhuǎn)換，Te是轉(zhuǎn)換矩陣。對(duì)式(4)進(jìn)行求導(dǎo)，得到誤差的動(dòng)力學(xué)方程如下：

(5)

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

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

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

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

(6)

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

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

(7)

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

(8)

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

4 數(shù)值仿真

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

4.1 定點(diǎn)編隊(duì)

設(shè)定仿真步長(zhǎng) ΔT=0.02 s，仿真時(shí)間5 s。預(yù)設(shè)在4 s內(nèi)，5個(gè)AUV的預(yù)定狀態(tài)達(dá)到一致，形成了預(yù)想的編隊(duì)形狀。

設(shè)跟隨者AUV在xwOwyw坐標(biāo)系下的初始位姿分別為 (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的初始位置，大圓點(diǎn)為虛擬領(lǐng)航者，灰色為跟隨者，淺灰色為各AUV的軌跡。

圖3 定點(diǎn)編隊(duì)路線圖

圖4 定點(diǎn)編隊(duì)位置誤差圖

圖5 定點(diǎn)編隊(duì)偏航角誤差圖

4.2 橫排編隊(duì)

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

設(shè)各AUV的初始值，4個(gè)跟隨者AUV在xwOwyw坐標(biāo)系下的初始位姿分別為 (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，中間段做正弦移動(dòng)，再恢復(fù)直線。仿真開始后，各AUV的跟蹤軌跡路線如圖6所示，空心圓為各AUV的初始位置，大圓點(diǎn)為虛擬領(lǐng)航者，灰色為跟隨者，淺灰色為各AUV的軌跡。

圖6 橫排編隊(duì)路線圖

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

圖7 橫排編隊(duì)位置誤差圖

圖8 橫排編隊(duì)偏航角誤差圖

5 結(jié)語

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