蔡文郁，楊俊雷，官靖凱

(杭州電子科技大學(xué)電子信息學(xué)院，浙江 杭州 310018)

自主水下航行器(Autonomous Underwater Vehicle，AUV)具有自治性強(qiáng)、機(jī)動性高等特點(diǎn)，在海洋觀測、海上救援、管道檢測等領(lǐng)域具有廣泛的應(yīng)用前景。面對更加復(fù)雜的水下任務(wù)，多AUV集群系統(tǒng)憑借著良好的冗余性、協(xié)調(diào)性和高效性，為海洋探索任務(wù)提供了新的解決思路。AUV編隊(duì)技術(shù)作為多AUV集群系統(tǒng)的關(guān)鍵技術(shù)之一，編隊(duì)方案如圖1所示，目前已經(jīng)受到了各國研究者們的廣泛關(guān)注。海洋環(huán)境中存在諸多不確定因素，包括海流、海浪、水下通信約束等外部因素的影響，對編隊(duì)控制而言是一個很大的挑戰(zhàn)，AUV集群編隊(duì)控制系統(tǒng)的設(shè)計(jì)有著嚴(yán)格的要求，必須兼具良好的穩(wěn)定性和魯棒性。

圖1 集群編隊(duì)方案

目前國內(nèi)外常用的編隊(duì)控制方法大致可以分為虛擬結(jié)構(gòu)方法[2－5](Virtual Structure Method)、基于行為的方法[6－8](Behavior-Based Method)、人工勢場方法[9－11](Artificial Potential Field Method)和領(lǐng)導(dǎo)跟隨者方法[12－18](Leader-Follower Method)。由于領(lǐng)導(dǎo)跟隨者方法具有良好的網(wǎng)絡(luò)拓?fù)浣Y(jié)構(gòu)特性，近年來很多研究者都針對領(lǐng)導(dǎo)跟隨者方法進(jìn)行了改進(jìn):Yan等人[12]基于圖論和虛擬領(lǐng)導(dǎo)者設(shè)計(jì)了新的編隊(duì)結(jié)構(gòu)，這種特殊結(jié)構(gòu)將AUV之間的連接固定為通過引線和鄰域AUV形成的三角形，因此增加了結(jié)構(gòu)的剛度和位置精度，增加了編隊(duì)結(jié)構(gòu)的穩(wěn)定性。李娟等人[13]提出了一種結(jié)合水下機(jī)器人路徑規(guī)劃和編隊(duì)控制的控制器，在控制器中采用滑?？刂品椒▽?shí)現(xiàn)AUV路徑跟蹤控制，將位置、姿態(tài)和速度跟蹤轉(zhuǎn)換為虛擬速度控制，使AUV達(dá)到預(yù)期位置。Panagou和Kumar[14]提出了一種在已知障礙物環(huán)境中，在通信約束情況下的機(jī)器人協(xié)同運(yùn)動方法，不需要機(jī)器人之間的數(shù)據(jù)交互，而是基于本地信息分散地執(zhí)行各自任務(wù)。Stover等人[15]提出了一種策略，在隊(duì)形保持的過程中跟隨者的位置不是相對于領(lǐng)導(dǎo)者位置嚴(yán)格固定的，而是以領(lǐng)導(dǎo)者參考系為中心的等距離圓弧上適當(dāng)?shù)淖兓i等人[16]提出了一種基于停滯規(guī)則的領(lǐng)導(dǎo)者跟隨者混合控制方案，一旦領(lǐng)導(dǎo)者到達(dá)理想的陣型，跟隨者會匯聚到領(lǐng)導(dǎo)者為中心的凸多面體中，當(dāng)所需的領(lǐng)導(dǎo)者任務(wù)失敗時，領(lǐng)導(dǎo)者會形成一個固定隊(duì)形，且擁有相同的速度和方向，因此跟隨者以與領(lǐng)導(dǎo)者相同的速度和方向收斂。由于多AUV系統(tǒng)與神經(jīng)網(wǎng)絡(luò)具有相似的特性，一些研究人員也試著將神經(jīng)網(wǎng)絡(luò)與編隊(duì)控制相結(jié)合，取得了良好的效果。例如Li和Zhu[17]提出了一種自適應(yīng)自組織圖(Self-Organizing Maps，SOM)神經(jīng)網(wǎng)絡(luò)方法，用于水下機(jī)器人集群的分布式編隊(duì)控制，包括對網(wǎng)絡(luò)初始神經(jīng)權(quán)重的特殊定義、獲勝者的選擇規(guī)則、工作量平衡定律、權(quán)重的更新方法以及編隊(duì)跟蹤策略。每臺AUV控制器僅僅使用自身信息和相鄰AUV的有限信息，而沒有明確指定領(lǐng)導(dǎo)者和跟隨者，所有AUV均被視為領(lǐng)導(dǎo)者或追隨者，因此它們具有更好的適應(yīng)性和容錯能力。針對需要躲避障礙物的復(fù)雜編隊(duì)控制場景，Ding和Zhu等人[18]采用生物啟發(fā)神經(jīng)網(wǎng)絡(luò)模型實(shí)現(xiàn)了AUV的三維編隊(duì)控制與避障。神經(jīng)網(wǎng)絡(luò)中的每個神經(jīng)元代表先導(dǎo)AUV的一個步驟，領(lǐng)導(dǎo)者AUV通信線路由神經(jīng)網(wǎng)絡(luò)中神經(jīng)元的輸出活動值確定，最終領(lǐng)導(dǎo)者AUV的導(dǎo)航轉(zhuǎn)化為神經(jīng)網(wǎng)絡(luò)中神經(jīng)元輸出的活動值。

本文以領(lǐng)導(dǎo)者跟隨者的編隊(duì)方法為基礎(chǔ)，根據(jù)實(shí)際領(lǐng)導(dǎo)者跟隨者編隊(duì)模型，借鑒反步控制法的思想提出了一種基于幾何數(shù)學(xué)模型的新型控制策略。而且，利用李雅普諾夫理論證明了在該控制策略下，AUV集群系統(tǒng)能夠達(dá)到穩(wěn)定狀態(tài)，最后在MATLAB平臺上進(jìn)行了仿真驗(yàn)證。

1 編隊(duì)模型

本文采用了基于領(lǐng)導(dǎo)者跟隨者的編隊(duì)模型，通過領(lǐng)導(dǎo)者AUV的位置、姿態(tài)、速度等信息，建立虛擬領(lǐng)導(dǎo)者，跟隨者AUV動態(tài)地調(diào)節(jié)自身的狀態(tài)，完成路徑的跟隨。假設(shè)多AUV系統(tǒng)模型由N臺AUV組成，為了簡單起見，假設(shè)它們處在恒定深度的平面運(yùn)動，第i個AUV的狀態(tài)可以用下式描述:

式中:ηi＝[x i，y i，ψi]T表示在全局坐標(biāo)系下第i個AUV的位置和姿態(tài)信息，v i＝[u i，v i，r i]T表示第i個AUV在自身所在載體坐標(biāo)系下的速度矢量。根據(jù)坐標(biāo)系轉(zhuǎn)換矩陣可知從全局坐標(biāo)系到AUV載體坐標(biāo)系的轉(zhuǎn)換矩陣為:

在Leader-Follower集群模型中，跟隨者通過領(lǐng)導(dǎo)者的位置、姿態(tài)動態(tài)的調(diào)整自身的位置姿態(tài)。由于跟隨者的軌跡是隨著領(lǐng)導(dǎo)者運(yùn)動而生成的，本文引入了虛擬領(lǐng)導(dǎo)者的概念，虛擬領(lǐng)導(dǎo)者是以領(lǐng)導(dǎo)者AUV為參考點(diǎn)，按照幾何關(guān)系生成其位置，因此虛擬領(lǐng)導(dǎo)者本質(zhì)上可以認(rèn)為是跟隨者AUV的理想位置。在笛卡爾坐標(biāo)系中的多AUV系統(tǒng)運(yùn)動模型如圖2所示。

圖2 多AUV編隊(duì)模型

圖2中深色的AUV是AUV實(shí)際所在位置，淺色的AUV是編隊(duì)模型中虛擬領(lǐng)導(dǎo)者的位置。假設(shè)編隊(duì)模型設(shè)置為圖中所示的正三角形，控制律的目標(biāo)就是讓跟隨者AUV運(yùn)動到理想位置，并能保持住隊(duì)形和姿態(tài)跟隨領(lǐng)導(dǎo)者AUV繼續(xù)運(yùn)動。本文用ηL＝[x L，y L，ψL]T和ηF＝[x F，y F，ψF]T分別表示領(lǐng)導(dǎo)者AUV和跟隨者AUV的坐標(biāo)以及速度矢量。從圖2可以看出，通過領(lǐng)導(dǎo)者的狀態(tài)可以計(jì)算出跟隨者AUV的理想坐標(biāo):

式中:l LF和分別是虛擬領(lǐng)導(dǎo)者與領(lǐng)導(dǎo)者AUV之間的距離與角度?？紤]虛擬領(lǐng)導(dǎo)者位置以及AUV的真實(shí)所在位置，全局誤差ErrF＝表示AUV的實(shí)際位置與理想位置的誤差，其中表示AUV理想位置與AUV實(shí)際位置在全局坐標(biāo)系下X軸的誤差，表示的是AUV理想位置與AUV實(shí)際位置在全局坐標(biāo)系下Y軸的誤差表示AUV理想位置與實(shí)際AUV的航向角誤差。

全局坐標(biāo)系下的AUV誤差模型為:

假設(shè)在整個AUV系統(tǒng)運(yùn)動過程中，領(lǐng)導(dǎo)者AUV遵循以下約束:

式中:umax和rmax分別是領(lǐng)導(dǎo)者AUV的最大線速度和角速度。

接下來分析跟隨者AUV和它所對應(yīng)的虛擬領(lǐng)導(dǎo)者之間關(guān)系，根據(jù)上述公式以及坐標(biāo)系轉(zhuǎn)換矩陣將坐標(biāo)原點(diǎn)固定在虛擬領(lǐng)導(dǎo)者AUV，并建立載體坐標(biāo)系，從而可得AUV載體坐標(biāo)系誤差模型:

式中:x e表示虛擬領(lǐng)導(dǎo)者AUV與AUV實(shí)際位置在載體坐標(biāo)系下的X軸誤差，y e表示載體坐標(biāo)系Y軸上的誤差，ψe表示虛擬領(lǐng)導(dǎo)者與AUV實(shí)際位置的航向角誤差，ψe∈(－2π，2π)。

定義轉(zhuǎn)換成矩陣為:

通過執(zhí)行上述變換，跟隨者的位置將從全局坐標(biāo)轉(zhuǎn)換為載體坐標(biāo)。在載體坐標(biāo)系中，對誤差求微分可得領(lǐng)導(dǎo)者和跟隨者之間動態(tài)誤差的微分模型:

因此，本文的目標(biāo)是設(shè)計(jì)合適的控制律減小AUV實(shí)際位置與虛擬領(lǐng)導(dǎo)者之間的誤差，如下式所示:

2 控制策略

本文研究的運(yùn)動學(xué)控制器模型如圖3所示，將虛擬領(lǐng)導(dǎo)者的狀態(tài)與自身的位置和姿態(tài)信息輸入到運(yùn)動學(xué)控制器模型可計(jì)算出當(dāng)前時刻的理想速度與角速度，同時更新自身位姿，最終形成優(yōu)化的閉環(huán)控制。

圖3 控制器模型

為了實(shí)現(xiàn)AUV編隊(duì)的全局軌跡跟蹤控制，定義了如下數(shù)學(xué)變換:

式中:u1為引入的虛擬變量，將式(10)代入微分誤差方程式(8)，可得:

式中:矩陣ε表示為:ε＝[ε1，ε2，ε3]T。f i和g i為函數(shù)的集合，如式(13)和式(14):

根據(jù)上式(11)可知，該控制器系統(tǒng)可以看成是一個通過積分環(huán)節(jié)串聯(lián)的非線性系統(tǒng)，因此可利用反步控制法思想。根據(jù)李雅普諾夫定理可知，如果可以證明和有界且u L>0，則存在控制律使得上式所描述的誤差系統(tǒng)漸進(jìn)穩(wěn)定。本文先選取式(11)中的第1個子系統(tǒng)如下:

為該子系統(tǒng)設(shè)計(jì)一個李雅普諾夫函數(shù)，如式(16)所示:

可以選取如下的控制律:

式中:M＝sinε3/k3，k1、k2、k3都是大于0的常數(shù)，則可得到如下公式:

選擇如下的控制律:

代入式(19)可得到:

負(fù)定，因此在如式(20)所示控制律的作用下，如式(11)所示的系統(tǒng)能夠獲得漸進(jìn)穩(wěn)定，由式(10)可知，如式(8)所示的系統(tǒng)與如式(11)所示的系統(tǒng)能夠一樣獲得漸進(jìn)穩(wěn)定。

綜上所述，本文提出的AUV集群編隊(duì)系統(tǒng)的控制律定義如下:

也可以表示為:

式中:u1為虛擬控制輸入，如式(24)所示，k1，k2，k3，k4都為常數(shù)。

3 仿真結(jié)果

為了驗(yàn)證本文控制方法的可行性，利用MATLAB R2016a軟件平臺對一個領(lǐng)導(dǎo)者AUV、兩個跟隨者AUV的情況進(jìn)行編隊(duì)仿真。三臺AUV在坐標(biāo)系原點(diǎn)周圍隨機(jī)部署，假設(shè)有海流等環(huán)境因素的干擾，選取[－0.15，0.15]的白噪聲強(qiáng)度。仿真主要參數(shù)如表1所示，主要是本文控制律中的四個參量取值。

表1 仿真參數(shù)

多AUV的編隊(duì)仿真結(jié)果如圖4所示，圖4(a)是理想的編隊(duì)軌跡，圖4(b)是在本文提出控制策略的編隊(duì)軌跡。圖中實(shí)線軌跡是領(lǐng)導(dǎo)者AUV的軌跡，虛線軌跡是兩臺跟隨者AUV的軌跡。

由圖4所示的AUV仿真軌跡圖可以看出，在本文設(shè)計(jì)的控制器作用下，AUV編隊(duì)軌跡與理想狀態(tài)接近，編隊(duì)效果良好。兩臺跟隨者AUV的速度、角速度如圖5所示，其中點(diǎn)線與點(diǎn)劃線曲線為需要達(dá)到的理想線速度曲線和理想角速度曲線，虛線與實(shí)線曲線依次為線速度變化曲線與角速度變化曲線。跟隨者誤差定義為跟隨者的實(shí)際位置與期望位置差值的絕對值。兩臺跟隨者AUV的誤差變化曲線圖如圖6所示，實(shí)線，虛線，點(diǎn)劃線曲線依次表示X軸、Y軸和航向角的誤差曲線。

圖4 仿真軌跡圖

圖5 跟隨者AUV線速度與角速度

圖6 跟隨者AUV的誤差曲線

從圖5和圖6的仿真結(jié)果圖中可以看出兩臺AUV的線速度和角速度隨著時間變化逐漸趨于穩(wěn)定。除了在編隊(duì)初始時刻的位置誤差較大，隨著編隊(duì)過程的推進(jìn)，跟隨誤差也漸漸趨近零，驗(yàn)證了本文提出控制律能夠保證良好的編隊(duì)隊(duì)形。

4 結(jié)語

針對水下環(huán)境的特點(diǎn)與實(shí)際編隊(duì)控制需求，本文提出了一種新型的主從架構(gòu)下多水下機(jī)器人編隊(duì)控制算法。對于AUV的非線性系統(tǒng)，采用反步法的思想與李雅普諾夫理論設(shè)計(jì)了新型的控制律函數(shù)，并證明了其收斂性。仿真結(jié)果表明本文提出的編隊(duì)控制策略能夠滿足AUV編隊(duì)的需要。后期研究將水聲通信的延時約束引入到控制律的設(shè)計(jì)中，并利用真實(shí)AUV集群系統(tǒng)進(jìn)行算法驗(yàn)證。