基于時(shí)延補(bǔ)償?shù)腁UV領(lǐng)航跟隨編隊(duì)控制

馮之文，姚 堯，苗 艷，馮景祥

(江蘇自動(dòng)化研究所，江蘇 連云港 222006)

0 引 言

在過去的幾十年中，自主水下航行器(AUV)已被廣泛用于執(zhí)行各種水下任務(wù)，包括民用和軍事應(yīng)用，如深海檢查、海洋測繪。對于某些特定應(yīng)用，例如海洋采樣、測繪和海底測量，通過多個(gè)AUV合作執(zhí)行任務(wù)可以提高效率、增大服務(wù)區(qū)域并在發(fā)生故障時(shí)提供冗余，提高了系統(tǒng)的穩(wěn)定性。一般來說，有3種主要類型的編隊(duì)控制策略：即基于行為的策略[1-2]，虛擬結(jié)構(gòu)策略[3-4]，和領(lǐng)導(dǎo)者-跟隨者編隊(duì)策略[5]。每種策略都有自己的優(yōu)點(diǎn)和缺點(diǎn)[6]。在這些策略中，領(lǐng)導(dǎo)者-跟隨者編隊(duì)策略由于其簡單性和可擴(kuò)展性等優(yōu)點(diǎn)而被許多研究廣泛考慮。

在多AUV編隊(duì)系統(tǒng)中，AUV的通信能力有限以及環(huán)境的限制，水聲通信受到傳播速度的限制，不可避免地會(huì)產(chǎn)生較大的時(shí)延;水聲通信具有高噪聲、多途干擾、多普勒傳播等不穩(wěn)定特性，使通信具有較高的誤碼率;水聲通信所消耗的能量遠(yuǎn)高于陸上無線通信，造成水聲通信范圍遠(yuǎn)遠(yuǎn)變小。這類弱通信系統(tǒng)的主要特征有:不可忽略的通信時(shí)延、間歇性通信失效或中斷、有限通信范圍以及高誤碼率等。

信息延遲是弱通信系統(tǒng)必須面對的問題，對系統(tǒng)穩(wěn)定性的影響不能忽視。由于接收到的領(lǐng)航AUV信息存在時(shí)間上的滯后性，系統(tǒng)在未來時(shí)刻的狀態(tài)由當(dāng)前狀態(tài)和歷史狀態(tài)共同決定，系統(tǒng)的穩(wěn)定性受到時(shí)延大小的影響。當(dāng)個(gè)體間的信息傳輸延遲太大，超過某一臨界值時(shí)，多AUV系統(tǒng)無法形成穩(wěn)定的編隊(duì)隊(duì)形，使原來穩(wěn)定的編隊(duì)變?yōu)椴环€(wěn)定。文獻(xiàn)[7]中，作者基于一致性的思想研究了水聲通信帶來的通信延遲問題，提出的控制律能夠允許一定的通信時(shí)延，達(dá)到了編隊(duì)控制的目的。文獻(xiàn)[8]中，作者提出了具有通信約束的分布式水下航行器群編隊(duì)控制算法，當(dāng)航行器間的通信時(shí)延小于某個(gè)確定的上界時(shí)，算法是有效的。但是文中每個(gè)個(gè)體接收到的鄰居個(gè)體的信息狀態(tài)及其時(shí)間導(dǎo)數(shù)均存在一個(gè)固定的時(shí)延T，現(xiàn)實(shí)中個(gè)體與個(gè)體之間的通信時(shí)延是時(shí)變的，算法上具有一定的局限性。文獻(xiàn)[9]中，作者針對時(shí)延問題，提出了一種不同時(shí)變通信延遲的解決方法，也是將AUV的編隊(duì)時(shí)延問題轉(zhuǎn)化成一致性問題。

目前，國內(nèi)外雖然對編隊(duì)控制的研究策略取得了一定的成果，有部分學(xué)者對此類問題展開了研究，但都是在較為理想的條件下展開研究，比如時(shí)延不變而且已知，或者時(shí)延可變但有時(shí)延上限，且大部分研究主要是進(jìn)行一致性的算法設(shè)計(jì)，基于一些比較簡單的模型得到一致性收斂的條件。基于以上的分析，針對通信延遲下，領(lǐng)航跟隨編隊(duì)控制方法隊(duì)形穩(wěn)定性變差的問題，本文提出了一種基于時(shí)延補(bǔ)償?shù)木庩?duì)控制方案，采用反步法和李雅普諾夫直接法設(shè)計(jì)了領(lǐng)航跟隨編隊(duì)控制律，實(shí)現(xiàn)了理想通信情況下的編隊(duì)保持。接著引入通信時(shí)延，利用最小二乘法根據(jù)領(lǐng)航AUV的當(dāng)前和歷史狀態(tài)信息進(jìn)行狀態(tài)的擬合，根據(jù)時(shí)延誤差預(yù)測出當(dāng)前時(shí)刻的領(lǐng)航AUV的狀態(tài)信息，帶入到控制器中進(jìn)行擬合。從仿真結(jié)果可以看出，該方法簡單有效，可以減小編隊(duì)的誤差，提高編隊(duì)的穩(wěn)定性。

1 問題描述

1.1 AUV數(shù)學(xué)模型

本文只考慮AUV二維平面內(nèi)的運(yùn)動(dòng)，因此只考慮AUV水平面內(nèi)的數(shù)學(xué)模型，可分為運(yùn)動(dòng)學(xué)模型和動(dòng)力學(xué)模型，分別如下：

(1)

(2)

從式(2)中可以看到，對于側(cè)向速度vr而言，是沒有任何外部輸入的。

1.2 AUV領(lǐng)航跟隨編隊(duì)描述

考慮由n個(gè)AUV組成的系統(tǒng)，這些AUV將保持所需的編隊(duì)隊(duì)形，并且每個(gè)跟隨AUV都可以和領(lǐng)航AUV組成1個(gè)子系統(tǒng)。對于每一個(gè)子系統(tǒng)，當(dāng)領(lǐng)導(dǎo)者AUV巡航時(shí)，可以生成跟隨者的參考軌跡，以使其跟隨位置ηf相對于領(lǐng)導(dǎo)者位置ηl的距離為ρ，方向?yàn)棣?。我們提出了針對每個(gè)跟隨者AUV的跟隨編隊(duì)控制算法。令ηl=[xl,yl,ψl]T，表示為領(lǐng)航者的位置矢量。然后，領(lǐng)導(dǎo)者的運(yùn)動(dòng)學(xué)模型可以描述為：

(3)

式中：vl=[ul,vl,rl]T，為領(lǐng)航者AUV的速度矢量。

類似地，跟隨者AUV的運(yùn)動(dòng)學(xué)模型可以描述為：

(4)

如圖1所示，跟隨者AUV的期望位置和領(lǐng)航者AUV的幾何關(guān)系如圖1所示。

圖1 領(lǐng)航跟隨方法示意圖

由此幾何關(guān)系可以用領(lǐng)航者AUV的位置信息矢量表示跟隨者AUV的期望位置矢量信息如下：

(5)

只要控制跟隨船跟蹤并沿著參考船軌跡運(yùn)動(dòng)，便可以實(shí)現(xiàn)編隊(duì)航行。因此，基于領(lǐng)航-跟隨方法的編隊(duì)控制問題可以描述如下：在可以獲知領(lǐng)導(dǎo)船的位置、姿態(tài)信息的條件下，為跟隨船設(shè)計(jì)控制律，使得xf→xd,yf→yd,ψf→ψl。

1.3 控制律的設(shè)計(jì)

在這里首先假設(shè)領(lǐng)航者AUV與跟隨者AUV之間的通信是理想的，即領(lǐng)航者AUV與跟隨者AUV之間的通信不存在延時(shí)，在理想通信的情況下進(jìn)行控制律的設(shè)計(jì)[10]。

(1) 運(yùn)動(dòng)學(xué)控制律的設(shè)計(jì)

由于AUV是欠驅(qū)動(dòng)的，因此在其橫向上沒有控制輸入。因此首先要設(shè)計(jì)控制速度uf和rf。定義跟隨者AUV與參考船的絕對誤差：

(6)

式中：ψd?Atan2(-ey,-ex)，并定義d?‖[ex,ey]T‖2，提出以下速度控制律，以使d和eψ收斂為零。

(7)

(8)

式中：ku,kr>0。

證明：應(yīng)用上述控制律，誤差動(dòng)態(tài)描述可以寫為：

(9)

又cosψd=ex/d,sinψd=ey/d，所以d和eψ對時(shí)間的導(dǎo)數(shù)可以寫成：

(10)

eψ收斂到0的證明：

(11)

(12)

因此，可以容易地得出結(jié)論：eψ有界并收斂到零，即eψ是漸近穩(wěn)定的。

d和vf的有界性：

-(d22/m22)vf-(m11/m22)×

{[-vf(taneψ-eψ/coseψ)-

kud/coseψ-al(·)-bl(·)taneψ]·

(13)

為了簡化計(jì)算，對式(13)進(jìn)行化簡，在這里定義：

(14)

取李雅普諾夫函數(shù)為：

(15)

其導(dǎo)數(shù)為：

[g2(·)+eψ]dvf+g3(·)vf

(16)

根據(jù)楊氏不等式定理可以得出下列不等式：

(17)

所以可以得到：

(18)

(19)

(20)

因此可以得出結(jié)論：對于所有的t∈[t0,T]，vf,d都是一致有界的?，F(xiàn)在考慮t∈[T,∞)的情況，根據(jù)eψ收斂到0的證明，隨著T→∞，eψ收斂到0，然后，通過選擇足夠大的T和足夠小的正常數(shù)ε,a,b，可以得出以下不等式：

(21)

然后，利用楊氏不等式，可以得出：

(22)

式(22)證明，對于t∈[T,∞)，vf和d都是有界的，結(jié)合之前證明的對于所有的t∈[t0,T]，vf,d有界，所以對于?t≥t0，Vf和d是一致有界的。d的收斂性證明參照文獻(xiàn)[11]的級(jí)聯(lián)理論進(jìn)行證明，不再進(jìn)行贅述。

(2) 動(dòng)力學(xué)控制律的設(shè)計(jì)

為了對動(dòng)力學(xué)控制律進(jìn)行設(shè)計(jì)，本文借助反演法將控制律擴(kuò)展到動(dòng)力學(xué)的層面上，將式(7)中的uf和rf作為虛擬的控制輸入ufd和rfd。

首先引入誤差變量：

(23)

式中：ufd和rfd為所需的速度控制輸入。

速度誤差的時(shí)間導(dǎo)數(shù)可推導(dǎo)為：

(24)

考慮由式(10)和式(24)描述的系統(tǒng)，應(yīng)用以下動(dòng)力學(xué)控制律：

(25)

式中：kτ1，kτ2為正數(shù)。

動(dòng)力學(xué)控制律穩(wěn)定性證明：

取李雅普諾夫函數(shù)：

(26)

其導(dǎo)數(shù)為：

(27)

根據(jù)楊氏不等式，可以獲得以下不等式：

(28)

然后，如果選擇速度控制增益，使得ku,kr>γ/2，則閉環(huán)系統(tǒng)漸近穩(wěn)定。

2 基于時(shí)延補(bǔ)償?shù)木庩?duì)控制

2.1 AUV通信方式

水下通信環(huán)境比陸地上要差得多，為了提高通信的穩(wěn)定性，目前的AUV領(lǐng)航跟隨編隊(duì)一般選取領(lǐng)航AUV廣播自身狀態(tài)信息的方式，將信息傳遞給跟隨AUV。如圖2所示，設(shè)#1為領(lǐng)航者AUV，#2～#n為跟隨者AUV，領(lǐng)航者AUV將自身的狀態(tài)信息廣播給跟隨者AUV，跟隨者AUV與領(lǐng)航者AUV、跟隨者AUV與跟隨者AUV之間均不進(jìn)行通信。

圖2 AUV群體通信方式

領(lǐng)航者AUV廣播的狀態(tài)信息為(x,y,ψl,t),x,y為領(lǐng)航者AUV的位置信息，ψl為領(lǐng)航者的航向角信息，t為發(fā)送信息時(shí)的時(shí)間，即時(shí)間戳，當(dāng)通信理想時(shí)，按照1.3節(jié)所設(shè)計(jì)的控制律編隊(duì)就可以得到很好的保持，即：

(29)

但在實(shí)際應(yīng)用當(dāng)中，AUV之間的通信并不能被理想化，通信延時(shí)是必然存在的，領(lǐng)航者在t時(shí)刻廣播的狀態(tài)信息必然不能在t時(shí)刻被跟隨者AUV所接收，對于不同的跟隨者，存在著不同的通信時(shí)延τi，即跟隨者AUV會(huì)在t+τi時(shí)刻接收到領(lǐng)航者AUV在t時(shí)刻廣播的狀態(tài)信息，此時(shí)若不進(jìn)行補(bǔ)償，則跟隨者的控制律變?yōu)椋?/p>

(30)

延時(shí)系統(tǒng)框圖如圖3所示。

圖3 通信延時(shí)下的領(lǐng)航跟隨編隊(duì)控制系統(tǒng)框圖

在此控制律下，隊(duì)形的保持必然不如理想通信條件下的編隊(duì)效果，因?yàn)榭刂坡芍械臓顟B(tài)信息永遠(yuǎn)不是當(dāng)前時(shí)刻的領(lǐng)航者的狀態(tài)信息，因此，需要對當(dāng)前的情況采取一定的措施來改善通信延時(shí)對編隊(duì)隊(duì)形的影響。

2.2 AUV通信補(bǔ)償原理

在實(shí)際問題中，人們常常需要從1組觀測數(shù)據(jù)(xi,yi)，i=1,2,…,n中作出一種預(yù)測：下一個(gè)x對應(yīng)的y值是什么，即預(yù)測函數(shù)y=f(x)的表達(dá)式。從幾何上看，這個(gè)問題就是要由給定的數(shù)據(jù)點(diǎn)(xi,yi)，i=1,2,…,n去描繪曲線y=f(x)的圖像，即所謂的數(shù)據(jù)擬合問題。插值方法可以作為處理這種問題的一種數(shù)值方法，但不是很好的方法。因?yàn)楝F(xiàn)在所給數(shù)據(jù)本身就不一定可靠，而插值曲線要求嚴(yán)格通過所給的每一個(gè)數(shù)據(jù)點(diǎn)，這種限制會(huì)保留所給數(shù)據(jù)的誤差。此外，所給數(shù)據(jù)的數(shù)量通常很多，也會(huì)影響插值的效果。曲線擬合方法就是希望從這一大堆看上去雜亂無章的數(shù)據(jù)中找出規(guī)律來，設(shè)法構(gòu)造一條所謂的擬合曲線，反映所給數(shù)據(jù)點(diǎn)總的趨勢，最小二乘法就是這樣的一種擬合方法。

由于AUV的機(jī)動(dòng)性能不強(qiáng)，本文將基于最小二乘法提出補(bǔ)償時(shí)延的方法。最小二乘法是很經(jīng)典的參數(shù)估計(jì)方法，它計(jì)算快速簡單，通常用多項(xiàng)式擬合模型去逼近目標(biāo)軌跡，通過最小化誤差的平方和尋找數(shù)據(jù)的最佳函數(shù)匹配。利用最小二乘法可以簡便地求得未知的數(shù)據(jù)，并使得這些求得的數(shù)據(jù)與實(shí)際數(shù)據(jù)之間誤差的平方和為最小。

領(lǐng)航AUV廣播的自身狀態(tài)信息中包含(x,y,ψl,t)，假設(shè)領(lǐng)航AUV在tm時(shí)刻廣播了自身的狀態(tài)，系統(tǒng)中的某個(gè)跟隨AUV在tn時(shí)刻接收到了領(lǐng)航者AUV在tm時(shí)刻(在這里，AUV自身信息傳播時(shí)間忽略不計(jì))的信息，則該跟隨者AUV可以計(jì)算出時(shí)延τ，假設(shè)跟隨者AUV在tn時(shí)刻之前一共接收到n個(gè)領(lǐng)航者AUV的狀態(tài)信息，分別記為(x1,y1,ψl1,t1),(x2,y2,ψl2,t2),(x3,y3,ψl3,t3)...(xn,yn,ψln,tn)，本文的任務(wù)是根據(jù)這些歷史信息盡可能去預(yù)測領(lǐng)航者AUV在tm時(shí)刻的狀態(tài)信息(xm,ym,ψlm,tm)以及tm和下一通信時(shí)刻的中間狀態(tài)信息。

本文中的領(lǐng)航者狀態(tài)信息是一個(gè)四維數(shù)組時(shí)間序列，無法直接應(yīng)用最小二乘法預(yù)測領(lǐng)航AUV當(dāng)前時(shí)刻的狀態(tài)，因此需要對過去時(shí)刻的狀態(tài)信息進(jìn)行相應(yīng)的處理，本文將狀態(tài)信息(xi,yi,ψli,ti)轉(zhuǎn)化成3個(gè)二維狀態(tài)(xi,ti),(yi,ti),(ψli,ti)，然后運(yùn)用最小二乘法對歷史時(shí)刻的數(shù)據(jù)進(jìn)行擬合，可以得到3條關(guān)于自變量t的曲線方程如下：

(31)

根據(jù)擬合好的曲線對tm時(shí)刻的狀態(tài)信息進(jìn)行求解，將t=tm代入求解，得到xm,ym,ψlm，最終得到領(lǐng)航者經(jīng)過時(shí)延補(bǔ)償以后的狀態(tài)量(xm,ym,ψlm,tm)，在下一個(gè)領(lǐng)航者狀態(tài)信息到來之前，都將根據(jù)擬合好的狀態(tài)曲線對領(lǐng)航者的狀態(tài)信息進(jìn)行預(yù)測，整個(gè)過程如圖4所示。

圖4 通信延時(shí)補(bǔ)償下的領(lǐng)航跟隨編隊(duì)控制系統(tǒng)框圖

2.3 算法流程

一般來說，曲線的擬合要用到過去時(shí)刻的大量AUV狀態(tài)信息，但考慮到AUV在水中的機(jī)動(dòng)性較差，也就是說AUV的運(yùn)動(dòng)趨勢是緩慢變化的，因此在補(bǔ)償通信延時(shí)，對領(lǐng)航者AUV的狀態(tài)進(jìn)行預(yù)測的時(shí)候，無需將過去時(shí)刻的所有狀態(tài)信息考慮在內(nèi)，只需要選取當(dāng)前時(shí)刻之前的k組狀態(tài)信息來擬合領(lǐng)航者AUV的運(yùn)動(dòng)狀態(tài)，隨著跟隨AUV不斷接收領(lǐng)航者廣播的狀態(tài)信息，跟隨者AUV不斷滑動(dòng)采集當(dāng)前時(shí)刻以及當(dāng)前時(shí)刻之前的狀態(tài)信息，實(shí)現(xiàn)了對領(lǐng)航者AUV運(yùn)動(dòng)軌跡的動(dòng)態(tài)更新，這種做法可以提高補(bǔ)償?shù)目焖傩浴?/p>

具體算法如下：

(1) 領(lǐng)航者AUV按周期向跟隨AUV廣播自身的狀態(tài)信息；

(2) 跟隨者AUV根據(jù)接收到的領(lǐng)航者AUV的狀態(tài)信息計(jì)算出時(shí)延τ；

(3) 確定領(lǐng)航者AUV的前k個(gè)狀態(tài)信息，根據(jù)最小二乘法對公式(31)進(jìn)行擬合；

(5) 將預(yù)測得到的狀態(tài)信息代入到設(shè)計(jì)好的控制器式(30)當(dāng)中,得到下一時(shí)刻的狀態(tài)量；

(6) 到達(dá)領(lǐng)航者AUV廣播周期，返回(1)。

3 仿真實(shí)驗(yàn)

(1) 理想通信情況下的AUV領(lǐng)航跟隨控制

由圖5可以看出，在通信理想且連續(xù)的情況下，3個(gè)AUV在本文所設(shè)計(jì)的控制律中達(dá)到了較好的編隊(duì)效果，圖6和圖7表明了領(lǐng)航者AUV和跟隨者AUV之間的距離和相對方位角達(dá)到了仿真時(shí)所設(shè)置的期望值，驗(yàn)證了控制律的有效性。

圖5 理想通信下AUV領(lǐng)航跟隨編隊(duì)路徑圖

圖6 理想通信下領(lǐng)航跟隨AUV之間的距離

圖7 理想通信下領(lǐng)航跟隨AUV之間的相對方位角

圖8 通信延時(shí)下領(lǐng)航跟隨AUV路徑圖

(2) 通信延遲下的領(lǐng)航跟隨AUV編隊(duì)控制

圖8顯示了通信延時(shí)下的領(lǐng)航跟隨編隊(duì)，由于通信延時(shí)和通信間隔的存在，相比于理想通信條件下，編隊(duì)效果較差。根據(jù)圖9可以看出，由于通信時(shí)延和通信間隔的存在，每隔10 s控制律才會(huì)做1次相應(yīng)的調(diào)整，跟隨者AUV與領(lǐng)航者AUV之間的相對距離已經(jīng)不能維持在本文所設(shè)置的理想距離，并且領(lǐng)航者所做的是圓周運(yùn)動(dòng)，所以跟隨者AUV與領(lǐng)航者AUV之間的距離在20 m上下浮動(dòng)。由圖10可以看出，跟隨者AUV與領(lǐng)航者AUV的相對方位角也因?yàn)檠訒r(shí)和通信間隔的存在無法達(dá)到較好的跟蹤效果。

圖9 通信延時(shí)下領(lǐng)航跟隨AUV之間的距離

圖10 通信延時(shí)下領(lǐng)航跟隨AUV的相對方位角

(3) 時(shí)延補(bǔ)償下的AUV編隊(duì)控制

圖11顯示了通信延時(shí)情況下引入延時(shí)補(bǔ)償和預(yù)測的編隊(duì)效果，和理想通信條件相比，收斂速度略微有點(diǎn)差距但相比于通信延時(shí)情況，隊(duì)形得到了很大的改善。從圖12和圖13可以看出，領(lǐng)航者AUV與跟隨者AUV到了我們所期望的距離和方位角，達(dá)到了很好的跟蹤效果。

圖11 時(shí)延補(bǔ)償下領(lǐng)航跟隨AUV路徑圖

圖12 時(shí)延補(bǔ)償下領(lǐng)航跟隨AUV之間的距離

圖13 時(shí)延下領(lǐng)航跟隨AUV之間的相對方位角

4 結(jié)束語

本文對AUV領(lǐng)航跟隨編隊(duì)控制中的通信延時(shí)問題提出了一種延時(shí)補(bǔ)償?shù)姆椒?，領(lǐng)航AUV將自身狀態(tài)傳遞給跟隨AUV，跟隨AUV根據(jù)領(lǐng)航AUV發(fā)送的當(dāng)前狀態(tài)信息與歷史狀態(tài)信息進(jìn)行預(yù)測，并根據(jù)時(shí)延的大小預(yù)測出領(lǐng)航AUV當(dāng)前時(shí)刻的狀態(tài)信息，用于補(bǔ)償領(lǐng)航跟隨AUV之間的通信延時(shí)，減小系統(tǒng)的編隊(duì)誤差。通過仿真可以看出本文所提方法的有效性，減小了編隊(duì)誤差并提高了編隊(duì)穩(wěn)定性。但本文在設(shè)計(jì)的過程中，沒有考慮到控制器的約束問題，結(jié)合控制約束問題進(jìn)行延時(shí)補(bǔ)償將是下一步工作。