基于傳感探測系統(tǒng)的多自治水下機(jī)器人編隊(duì)協(xié)調(diào)控制

龐師坤，王 健，易 宏，梁曉鋒

(上海交通大學(xué) 海洋智能裝備與系統(tǒng)教育部重點(diǎn)實(shí)驗(yàn)室；海洋工程國家重點(diǎn)實(shí)驗(yàn)室，上海 200240)

自治水下機(jī)器人(AUV)以其獨(dú)特的優(yōu)勢在海底偵查、探索等領(lǐng)域發(fā)揮著極其重要的作用.對(duì)于一些復(fù)雜任務(wù)，如大范圍協(xié)同偵查、對(duì)抗、打擊和水下信息組網(wǎng)[1]等，AUV協(xié)同編隊(duì)可以協(xié)作完成，而單個(gè)AUV卻由于自身的局限性而難以勝任.因此，越來越多的學(xué)者將研究重點(diǎn)聚焦于AUV協(xié)同編隊(duì)控制.編隊(duì)控制是指多個(gè)機(jī)器人在達(dá)到目的地的過程中保持某種隊(duì)形，同時(shí)又要適應(yīng)環(huán)境約束的控制技術(shù)[2].目前，編隊(duì)控制方法主要有基于行為法[3-4]、人工勢場法[5]、虛擬結(jié)構(gòu)法[6]、領(lǐng)航-跟隨法[7-13]、分布式控制法[14]等.

領(lǐng)航-跟隨法由于其可行性高、擴(kuò)展性強(qiáng)等特點(diǎn)，被眾多學(xué)者廣泛采用.Shojaei[7]基于視覺法利用領(lǐng)航-跟隨法的基本思想對(duì)AUV目標(biāo)追蹤進(jìn)行了深入研究，同時(shí)對(duì)水面欠驅(qū)動(dòng)無人艇編隊(duì)控制進(jìn)行了分析；Rout等[8]研究了在領(lǐng)航AUV和跟隨AUV之間存在間斷性信息傳輸條件下的編隊(duì)協(xié)調(diào)控制；Das等[9]對(duì)AUV編隊(duì)進(jìn)行了綜述性介紹，并在文獻(xiàn)[10]中對(duì)通信限制條件下的AUV領(lǐng)航-跟隨編隊(duì)進(jìn)行了研究；丁國華等[11]對(duì)多AUV主從式編隊(duì)及其避障控制方法進(jìn)行了研究，提出了一種基于反步控制法和人工勢場法的多AUV三維編隊(duì)與避障方法；朱大奇等[2]研究了基于領(lǐng)航位置信息的AUV三維編隊(duì)控制方法；向先波[12]研究了二階非完整性水下機(jī)器人路徑跟蹤控制，對(duì)基于路徑跟蹤的AUV編隊(duì)協(xié)調(diào)控制進(jìn)行了研究，并在后續(xù)工作中[13]以海底光(電)纜為編隊(duì)探測目標(biāo)，對(duì)AUV編隊(duì)在平面和三維空間內(nèi)協(xié)調(diào)控制進(jìn)行了探索.

由以上分析可以看出，在編隊(duì)中AUV個(gè)體之間經(jīng)常需要依靠水聲通信傳遞信息，然而該通信方式容易造成信息延遲、數(shù)據(jù)丟失等問題.如果采取傳感器探測的方式進(jìn)行編隊(duì)，那么跟隨AUV能夠?qū)崟r(shí)探測領(lǐng)航AUV的相對(duì)位姿信息等，編隊(duì)個(gè)體之間不存在信息交互，這種非通信式編隊(duì)可以有效避免上述問題.在傳感器探測條件下，本文采用領(lǐng)航-跟隨法對(duì)AUV編隊(duì)進(jìn)行研究，同時(shí)利用Lyapunov函數(shù)設(shè)計(jì)了反演控制器，提高了編隊(duì)系統(tǒng)的穩(wěn)定性和可靠性.

1 問題描述

1.1 AUV運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)模型

假設(shè)AUV是左右對(duì)稱的，并且慣性矩陣、附加質(zhì)量矩陣、水動(dòng)力阻尼矩陣均是對(duì)角矩陣，忽略其垂蕩、橫滾和俯仰運(yùn)動(dòng)，在無外界干擾時(shí)的運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)方程為[6]

(1)

(2)

式中：(x,y)為AUV在大地坐標(biāo)系下的位置；φ為大地坐標(biāo)系下的艏向角；u、v和r分別為隨體坐標(biāo)系下的縱向速度、橫向速度和艏向角轉(zhuǎn)動(dòng)的角速度；mi(i=1,2,3)為包含附加質(zhì)量在內(nèi)的慣性質(zhì)量；di(i=1,2,3)為橫向、縱向和艏向的水動(dòng)力系數(shù)；τu和τr分別為縱向力和偏航力矩的控制信號(hào).

圖1 領(lǐng)航-跟隨編隊(duì)構(gòu)型Fig.1 Leader-follower formation structure

1.2 領(lǐng)航-跟隨編隊(duì)控制

本文中領(lǐng)航AUV用L表示，跟隨AUV用F表示.在笛卡爾坐標(biāo)系內(nèi)，領(lǐng)航AUV與跟隨AUV的編隊(duì)幾何構(gòu)型如圖1所示.圖中：(XL,YL,φL)為領(lǐng)航AUV的位置和艏向角；(XF,YF,φF)為跟隨AUV的位置和艏向角；(lLF,λLF)為領(lǐng)航者和跟隨者之間的相對(duì)位置和視線角度；lx和ly分別為跟隨AUV相對(duì)于領(lǐng)航AUV在x和y方向上的投影.編隊(duì)定義為

FLF=[lLFλLF]T

(3)

lLF∈R>0，λLF∈[0,π]

理想AUV編隊(duì)定義為

(4)

(5)

(6)

綜合式(1)～(5)，可以得到AUV編隊(duì)的運(yùn)動(dòng)學(xué)模型：

(7)

x和y方向上的位置誤差為

(8)

聯(lián)立式(6)～(8)，得到編隊(duì)系統(tǒng)的誤差模型：

(9)

因此，控制任務(wù)是為跟隨AUV設(shè)計(jì)控制器，使得系統(tǒng)誤差趨近于零，得到理想的編隊(duì)狀態(tài)，即

(10)

Δ∈(-π/2,π/2)

式中：當(dāng)Δ=0時(shí)，編隊(duì)控制中跟隨AUV的艏向始終與領(lǐng)航AUV保持一致.

1.3 視覺傳感器分析

本文編隊(duì)中AUV均裝備了前向傳感探測系統(tǒng)，每個(gè)傳感探測系統(tǒng)都假設(shè)有傳感探測限制.圖2中所示扇形的半徑Drange和角度2α定義了傳感探測區(qū)域.因此，每個(gè)AUV可以測量到在傳感探測區(qū)域內(nèi)的物體.當(dāng)且僅當(dāng)跟隨AUV能夠探測到領(lǐng)航AUV時(shí)，圖2中的2艘AUV可以組成編隊(duì)隊(duì)形.

圖2 傳感探測區(qū)域Fig.2 Sensor measuring region

為方便模擬，本文做如下假設(shè)：

(1)編隊(duì)狀態(tài)向量和領(lǐng)航AUV的方向能夠被傳感探測傳感器準(zhǔn)確地探測到，速度則根據(jù)探測到的時(shí)間導(dǎo)數(shù)進(jìn)行計(jì)算.

(3)每艘AUV的速度和方向只有自己可知，互相之間沒有干擾和阻礙.

2 編隊(duì)控制器設(shè)計(jì)

圖3 跟隨AUV控制策略Fig.3 Control strategy of follower AUVs

2.1 運(yùn)動(dòng)學(xué)控制器

首先定義轉(zhuǎn)換矩陣

(11)

然后將式(8)中的距離誤差到跟隨AUV的隨體坐標(biāo)系下：

Σ=ReφE

(12)

式中：Σ=[ε1ε2]T；E=[exey]T.由此可得

(13)

確定Lyapunov控制函數(shù)(CLF)：

ε1(uF-ξ1coseφ+ξ2sineφ)+

ε2(vF-ξ1sineφ-ξ2coseφ)

(14)

運(yùn)動(dòng)學(xué)控制律為

(15)

(16)

式中：k1,k2>0；k1,k2∈R.

式(14)可以寫成

(17)

由此可見，系統(tǒng)是漸近穩(wěn)定的.

定義縱向速度誤差

(18)

聯(lián)立式(15)和(16)，可得

k2(vF-ξ1sineφ-ξ2coseφ-ε1rF)-

(19)

另外，k1=d1/m1，k2=d2/m2，式(19)可以表示為

(ξ2sineφ-ξ1coseφ)rL-

(20)

定義CLF：

(21)

從而得到

(22)

式中：λ=ξ2sineφ-ξ1coseφ；ζ=ξ1sineφ+ξ2coseφ.

選擇虛擬控制律為

(23)

式中：

式(22)變?yōu)?/p>

(24)

式中：σ為任意有界無窮小常數(shù).

由此可見，系統(tǒng)(ve,eφ)是漸近穩(wěn)定的.

2.2 動(dòng)力學(xué)控制器

(1)定義縱向速度誤差

(25)

其時(shí)間導(dǎo)數(shù)為

(26)

定義CLF：

(27)

聯(lián)立式(27)和(2)，可得

(28)

控制律為

(29)

k6>0，k6∈R

式(28)變?yōu)?/p>

(30)

(2)定義角速度誤差

(31)

其時(shí)間導(dǎo)數(shù)為

(32)

定義CLF：

(33)

聯(lián)立式(33)和(2)，可得

(34)

控制律為

(35)

k7>0，k7∈R

則式(34)可表示為

(36)

由此可見，系統(tǒng)是漸進(jìn)穩(wěn)定的.

3 仿真分析

在仿真分析中，設(shè)定AUV傳感探測系統(tǒng)的探測范圍是2α∈[0,π]，由3艘具有相同參數(shù)的AUV組成編隊(duì)[15]，其中，1艘作為領(lǐng)航者，另外2艘作為跟隨者.領(lǐng)航AUV根據(jù)設(shè)定的軌跡航行，跟隨AUV則按照編隊(duì)控制器產(chǎn)生的軌跡航行，從而得到期望的編隊(duì)隊(duì)形.AUV參數(shù)：m1=1 116 kg，m2=2 133 kg，m3=2 133 kg，d1=25.5 kg/s，d2=138 kg/s，d3=138 kg/s.

算例1期望的控制模型參數(shù)：lLF1=5 m，λLF1=π/4，lLF2=5 m，λLF2=3π/4.初始位置分別為：領(lǐng)航AUV，[XL,YL]=[0,0]；跟隨AUV1，[XF1,YF1]=[0,-5] m；跟隨AUV2，[XF2,YF2]=[0,5] m.

領(lǐng)航AUV和跟隨AUV的初始速度分別為：uL=1 m/s，vL=0 m/s，rL=0.01 rad/s，速度和轉(zhuǎn)艏角速度保持不變；跟隨AUV1，[uF1,vF1,rF1]=[0,0,0]；跟隨AUV2，[uF2,vF2,rF2]=[0,0,0].

仿真結(jié)果如圖4～6所示.從圖4中可以看出領(lǐng)航AUV做勻速圓周運(yùn)動(dòng)且初始狀態(tài)時(shí)與跟隨AUV處于非編隊(duì)隊(duì)形，控制器會(huì)控制跟隨AUV調(diào)整相應(yīng)的運(yùn)動(dòng)軌跡，從而得到期望的編隊(duì)隊(duì)形.由圖5和6可知，在X和Y方向，2個(gè)跟隨AUV的位置誤差很快趨近于零；由于做圓周運(yùn)動(dòng)，領(lǐng)航AUV的艏向在均勻變化，在達(dá)到穩(wěn)定狀態(tài)后跟隨AUV的艏向則與其保持固定的誤差，大約在2° 左右.

圖4 圓周運(yùn)動(dòng)時(shí)AUV編隊(duì)軌跡Fig.4 AUV trajectory of circling motion

圖5 圓周運(yùn)動(dòng)時(shí)跟隨AUV1誤差Fig.5 Errors of follower AUV1 of circling motion

圖6 圓周運(yùn)動(dòng)時(shí)跟隨AUV2誤差Fig.6 Errors of follower AUV2 of circling motion

算例2期望的控制模型參數(shù)：lLF1=3 m，λLF1=π/3，lLF2=6 m，λLF2=2π/3.初始位置分別為：領(lǐng)航AUV，[XL,YL]=[0,0]；跟隨AUV1，[XF1,YF1]=[-6,-6] m；跟隨AUV2，[XF2,YF2]=[-6,6] m.

領(lǐng)航AUV初始速度：uL=1 m/s，vL=0 m/s，rL=0 rad/s，速度始終保持不變；跟隨AUV1，[uF1,vF1,rF1]=[0,0,0]；跟隨AUV2，[uF2,vF2,rF2]=[0,0,0].

仿真結(jié)果如圖7～9所示.由圖7可知，當(dāng)領(lǐng)航AUV做勻速直線運(yùn)動(dòng)、2個(gè)跟隨AUV與領(lǐng)航AUV的距離不同且初始位置處于領(lǐng)航AUV異側(cè)時(shí)，同樣可以按照設(shè)定的編隊(duì)條件得到期望的編隊(duì)隊(duì)形.由圖8和9可以看出，雖然在初始的隊(duì)形形成階段，跟隨AUV位置和艏向均有較大變化，但2個(gè)跟隨AUV的位置誤差和艏向誤差均在較短時(shí)間內(nèi)收斂于0，并最終完成編隊(duì)，與理論值相符.

圖7 直線運(yùn)動(dòng)時(shí)AUV編隊(duì)軌跡Fig.7 AUV trajectory of rectilinear motion

圖8 直線運(yùn)動(dòng)時(shí)跟隨AUV1誤差Fig.8 Errors of follower AUV1 of rectilinear motion

圖9 直線運(yùn)動(dòng)時(shí)跟隨AUV2 誤差Fig.9 Errors of follower AUV2 of rectilinear motion

4 結(jié)語

編隊(duì)中AUV之間依靠水聲通信容易造成數(shù)據(jù)丟失和通信延遲等問題，而基于傳感器探測的編隊(duì)方式，AUV個(gè)體之間不需要通信，因此可以避免上述問題.本文在傳感器探測條件下，采用領(lǐng)航-跟隨法對(duì)AUV編隊(duì)進(jìn)行了研究.同時(shí)，利用Lyapunov函數(shù)設(shè)計(jì)了編隊(duì)控制器，并證明了系統(tǒng)穩(wěn)定性.隨后將該控制器應(yīng)用到具體AUV模型上，對(duì)領(lǐng)航AUV做圓周運(yùn)動(dòng)和直線運(yùn)動(dòng)軌跡進(jìn)行仿真計(jì)算.結(jié)果表明，跟隨AUV能夠與領(lǐng)航AUV保持期望的位姿關(guān)系，得到期望的編隊(duì)隊(duì)形，驗(yàn)證了方法的有效性和實(shí)用性.本文所設(shè)計(jì)的控制器比較適用于近距離攝像和聲納探測系統(tǒng)，對(duì)于較遠(yuǎn)距離聲納探測，根據(jù)實(shí)際工作環(huán)境則需要考慮外部擾動(dòng)及通信延遲等因素影響，作者將繼續(xù)對(duì)此進(jìn)行研究.