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

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

(上海交通大學 海洋智能裝備與系統(tǒng)教育部重點實驗室；海洋工程國家重點實驗室，上海 200240)

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

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

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

1 問題描述

1.1 AUV運動學和動力學模型

假設AUV是左右對稱的，并且慣性矩陣、附加質量矩陣、水動力阻尼矩陣均是對角矩陣，忽略其垂蕩、橫滾和俯仰運動，在無外界干擾時的運動學和動力學方程為[6]

(1)

(2)

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

圖1 領航-跟隨編隊構型Fig.1 Leader-follower formation structure

1.2 領航-跟隨編隊控制

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

FLF=[lLFλLF]T

(3)

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

理想AUV編隊定義為

(4)

(5)

(6)

綜合式(1)～(5)，可以得到AUV編隊的運動學模型：

(7)

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

(8)

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

(9)

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

(10)

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

式中：當Δ=0時，編隊控制中跟隨AUV的艏向始終與領航AUV保持一致.

1.3 視覺傳感器分析

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

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

為方便模擬，本文做如下假設：

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

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

2 編隊控制器設計

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

2.1 運動學控制器

首先定義轉換矩陣

(11)

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

Σ=ReφE

(12)

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

(13)

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

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

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

(14)

運動學控制律為

(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 動力學控制器

(1)定義縱向速度誤差

(25)

其時間導數(shù)為

(26)

定義CLF：

(27)

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

(28)

控制律為

(29)

k6>0，k6∈R

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

(30)

(2)定義角速度誤差

(31)

其時間導數(shù)為

(32)

定義CLF：

(33)

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

(34)

控制律為

(35)

k7>0，k7∈R

則式(34)可表示為

(36)

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

3 仿真分析

在仿真分析中，設定AUV傳感探測系統(tǒng)的探測范圍是2α∈[0,π]，由3艘具有相同參數(shù)的AUV組成編隊[15]，其中，1艘作為領航者，另外2艘作為跟隨者.領航AUV根據(jù)設定的軌跡航行，跟隨AUV則按照編隊控制器產(chǎn)生的軌跡航行，從而得到期望的編隊隊形.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.初始位置分別為：領航AUV，[XL,YL]=[0,0]；跟隨AUV1，[XF1,YF1]=[0,-5] m；跟隨AUV2，[XF2,YF2]=[0,5] m.

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

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

圖4 圓周運動時AUV編隊軌跡Fig.4 AUV trajectory of circling motion

圖5 圓周運動時跟隨AUV1誤差Fig.5 Errors of follower AUV1 of circling motion

圖6 圓周運動時跟隨AUV2誤差Fig.6 Errors of follower AUV2 of circling motion

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

領航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].

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

圖7 直線運動時AUV編隊軌跡Fig.7 AUV trajectory of rectilinear motion

圖8 直線運動時跟隨AUV1誤差Fig.8 Errors of follower AUV1 of rectilinear motion

圖9 直線運動時跟隨AUV2 誤差Fig.9 Errors of follower AUV2 of rectilinear motion

4 結語

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