基于時(shí)空解耦的無(wú)人艇編隊(duì)路徑跟蹤控制*

廖 剛 徐海祥 余文曌 李文娟

(高性能船舶技術(shù)教育部重點(diǎn)實(shí)驗(yàn)室1) 武漢 430063) (武漢理工大學(xué)交通學(xué)院2) 武漢 430063) (江蘇科技大學(xué)海洋裝備研究院3) 鎮(zhèn)江 212003)

0 引 言

水面無(wú)人艇(unmanned surface vessel，USV)憑借著高速化、小型化、智能化等優(yōu)勢(shì)被廣泛應(yīng)用.目前，單水面無(wú)人艇取得較多的研究成果[1-4]，但隨著無(wú)人艇所承擔(dān)的任務(wù)越來(lái)越復(fù)雜，單艇作業(yè)不能滿足任務(wù)需求，多無(wú)人艇之間相互協(xié)作，才能更高效率和更高質(zhì)量地完成任務(wù).如用于環(huán)境監(jiān)測(cè)，溢油回收,油氣勘探人員搜救[5].在這些任務(wù)中，無(wú)人艇的期望運(yùn)動(dòng)根據(jù)要遵循的幾何路徑和沿路徑行駛的期望速度來(lái)表征[6].此外，無(wú)人艇編隊(duì)采用的控制方案中，“領(lǐng)導(dǎo)者-跟隨者”編隊(duì)結(jié)構(gòu)形式憑借著其簡(jiǎn)單性和可擴(kuò)展性，在實(shí)際工程運(yùn)用中較多.跟隨者通過(guò)與領(lǐng)導(dǎo)者相互感知和相互協(xié)同作用，與領(lǐng)導(dǎo)者保持相似的狀態(tài)，使多無(wú)人艇最終呈現(xiàn)設(shè)定的編隊(duì)隊(duì)形.

Ihle等[7]設(shè)計(jì)的控制目標(biāo)是使無(wú)人艇維持在編隊(duì)預(yù)定路徑中的期望位置，構(gòu)造獨(dú)立的參數(shù)變量，使無(wú)人艇形成編隊(duì).然而該方法在模型試驗(yàn)中，領(lǐng)航者是仿真數(shù)據(jù)，沒(méi)有實(shí)際運(yùn)動(dòng).文獻(xiàn)[6]設(shè)計(jì)了在未知海流情況下，使無(wú)人艇編隊(duì)沿著設(shè)定的直線以恒定航速航行的循跡控制.但是該方法沒(méi)有考慮曲線路徑的影響.徐國(guó)焯[8]設(shè)計(jì)出虛擬結(jié)構(gòu)法和領(lǐng)航者-跟隨者法相結(jié)合的編隊(duì)控制方法.在單UUV路徑跟蹤控制的基礎(chǔ)上，完成最終的多UUV編隊(duì)路徑跟蹤控制器設(shè)計(jì).Peng等[9]利用神經(jīng)網(wǎng)絡(luò)和動(dòng)態(tài)表面控制技術(shù)設(shè)計(jì)了魯棒自適應(yīng)控制器來(lái)解決欠驅(qū)動(dòng)自主地面車輛的編隊(duì)控制問(wèn)題.邊信黔等[10-11]針對(duì)多無(wú)人水下航行器的協(xié)調(diào)路徑跟蹤控制問(wèn)題，應(yīng)用分散控制策略將路徑跟蹤問(wèn)題和個(gè)體間的速度協(xié)調(diào)解耦，但是該控制方法不能直接運(yùn)用于欠驅(qū)無(wú)人艇.馬瑞龍[12]建立了領(lǐng)航者-跟隨者編隊(duì)控制模型，并采用運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)設(shè)計(jì)了控制器，實(shí)現(xiàn)編隊(duì)路徑跟蹤的目的，但是文中無(wú)人艇處于沒(méi)有風(fēng)浪流的理想環(huán)境中，沒(méi)有考慮無(wú)人艇實(shí)際航行時(shí)外界環(huán)境的干擾.齊小偉等[13]利用多智能體聚集模型，基于領(lǐng)航者-跟隨者的目標(biāo)跟蹤，實(shí)現(xiàn)了含有模型不確定性與未知海浪流干擾項(xiàng)的多無(wú)人艇協(xié)同編隊(duì)控制.由于神經(jīng)網(wǎng)絡(luò)需要計(jì)算出包括風(fēng)浪流和模型不確定項(xiàng)在內(nèi)的10個(gè)不確定項(xiàng)，對(duì)計(jì)算性能要求高，很難滿足控制在實(shí)時(shí)性方面的需求.

本文基于時(shí)空解耦策略，采用領(lǐng)航者-跟隨者模式解決路徑變化時(shí)的編隊(duì)路徑跟蹤控制問(wèn)題.建立了在不同路徑下的編隊(duì)隊(duì)形數(shù)學(xué)模型.將編隊(duì)路徑跟蹤控制問(wèn)題分解為以下兩個(gè)子問(wèn)題：每艘無(wú)人艇上設(shè)計(jì)路徑跟蹤控制器，保證在空間上運(yùn)動(dòng)收斂到各自的期望路徑；設(shè)計(jì)速度協(xié)調(diào)控制器，調(diào)整每艘無(wú)人艇的運(yùn)動(dòng)速度，在時(shí)間上實(shí)現(xiàn)多無(wú)人艇間的協(xié)調(diào)，保證無(wú)人艇的編隊(duì)隊(duì)形.仿真實(shí)驗(yàn)中，在多路徑點(diǎn)過(guò)渡過(guò)程考慮編隊(duì)隊(duì)形相對(duì)首向和速度的變化，使過(guò)渡過(guò)程更加平滑.

1 問(wèn)題描述

1.1 無(wú)人艇數(shù)學(xué)模型

對(duì)于水面無(wú)人艇而言，本文主要考慮水平面三自由度運(yùn)動(dòng).采用大地坐標(biāo)系XOY和隨艇坐標(biāo)系xoy建立無(wú)人艇運(yùn)動(dòng)和動(dòng)力學(xué)模型，見(jiàn)圖1.

圖1 無(wú)人艇運(yùn)動(dòng)坐標(biāo)系

在參考坐標(biāo)系下，無(wú)人艇的運(yùn)動(dòng)模型可以描述為[14]

(1)

(2)

τ=Buup

(3)

式中：η=[x,y,ψ]T為無(wú)人艇在固定坐標(biāo)系下的位置及首向；υ=[u,v,r]T為無(wú)人艇在隨艇坐標(biāo)系下的縱蕩、橫蕩、首搖速度；M為慣性矩陣；C(υ)為科氏力矩陣；D(υ)為水動(dòng)力阻尼矩陣；并且假設(shè)滿足M=MT，C(υ)=-CT(υ)，D(υ)>0，即無(wú)人艇是左右舷對(duì)稱結(jié)構(gòu)；J(ψ)為坐標(biāo)系轉(zhuǎn)換矩陣，表示為

υr為無(wú)人艇在隨艇坐標(biāo)系下相對(duì)于流速υc的相對(duì)速度，υc和υ的關(guān)系為[15]

(4)

其中：tp為推力減額系數(shù)；Yδ和Nδ為舵的水動(dòng)力參數(shù)；Tp和δ分別為推進(jìn)器推力和舵角.

考慮到推進(jìn)器限制，控制輸出作如下限制：

b為無(wú)人艇所遭受的外界環(huán)境力，主要由風(fēng)、浪、流組成.

1.2 編隊(duì)隊(duì)形數(shù)學(xué)模型

無(wú)人艇編隊(duì)路徑跟蹤過(guò)程中，除整體有機(jī)地進(jìn)行路徑跟蹤外，編隊(duì)還包括：①編隊(duì)隊(duì)形的形成問(wèn)題，即如何使得多無(wú)人艇從某種初始是狀態(tài)下形成符合任務(wù)需要的有規(guī)律行的有機(jī)整體；②編隊(duì)行進(jìn)中如何保證隊(duì)形的穩(wěn)定不變問(wèn)題.

多艘無(wú)人艇采用領(lǐng)導(dǎo)者-跟隨者的編隊(duì)模式，其中一艘為領(lǐng)航艇，其余為跟隨艇.第i艘跟隨艇與領(lǐng)航艇之間相對(duì)關(guān)系為

di=[dxi,dyi,dψi]T

(6)

式中：dxi和dyi分別為跟隨艇i在領(lǐng)航艇坐標(biāo)系中的縱向位置和橫向相對(duì)位置；dψi為相對(duì)首向角，根據(jù)設(shè)定的路徑不同有所差別.

1) 直線路徑 在直線路徑下，通常跟隨艇i的首向角與領(lǐng)航艇的保持一致，即

dψi=0

(7)

2) 曲線路徑 在曲線路徑下，由于存在曲率半徑，則跟隨艇i的首向角與領(lǐng)航艇首向角之間的關(guān)系為

dψi=2arcsin (dxi/2R)

(8)

式中:R為領(lǐng)航艇所在圓弧路徑的曲率半徑.

3) 一般路徑 對(duì)于平面一般路徑，可以用含標(biāo)量θ的參數(shù)化表示為ξ(θ).以|dxi|為半徑作圓，得到與路徑的交點(diǎn)，該點(diǎn)的首向角為切線與OX軸平行線的夾角，也是跟隨艇i的首向角.因此，跟隨艇i的首向角與領(lǐng)航艇首向角之間的關(guān)系：

dψi=ψi-ψl

(9)

三種路徑下編隊(duì)相對(duì)關(guān)系見(jiàn)圖2.

圖2 不同路徑下編隊(duì)相對(duì)關(guān)系

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

2.1 編隊(duì)偏差

當(dāng)無(wú)人艇偏離設(shè)定路徑時(shí)，需要驅(qū)使其回到期望位置，見(jiàn)圖3.由圖3可知，對(duì)于第i艘跟隨艇，偏差計(jì)算表達(dá)式為

(10)

式中：[xei,yei,ψei]T為跟隨艇i在自身艇坐標(biāo)系下的期望位置與實(shí)際位置的偏差；[xdi,ydi,ψdi]T為跟隨艇i在大地坐標(biāo)系下的期望位置，可以通過(guò)領(lǐng)航艇位置和設(shè)定編隊(duì)隊(duì)形計(jì)算得到；[xi,yi,ψi]T為跟隨艇i在大地坐標(biāo)系下的實(shí)際位置.

圖3 編隊(duì)偏差圖

由圖3可知，跟隨艇i回到預(yù)定路徑上的預(yù)期首向角為

ψdi=ψri+ψi

(11)

2.2 路徑跟蹤控制器

路徑跟蹤控制器主要保證各無(wú)人艇在空間上運(yùn)動(dòng)收斂到各自的期望路徑.

根據(jù)式(10)，跟隨艇i位置偏差可表示為

(12)

對(duì)其微分，則

式中：udi,vdi分別為跟隨艇i的期望縱向和橫向速度.

該子系統(tǒng)的輸入為ui和vi，選取該子系統(tǒng)虛擬控制量為

αui=k1xei+udicosψei-vdisinψei

αvi=k2yei+udisinψei+vdicosψei

(14)

式中：k1,k2>0.

代入式(14)，則式(13)中的ui和vi可以分別被替換成αui和αvi，則式(13)可以改寫為

(15)

構(gòu)造第一個(gè)Lyapunov函數(shù)為

(16)

明顯V1≥0，對(duì)V1微分可得

xei(-k1xei+yeiri)+yei(-k2yei-xeiri)=

(17)

由此可知，路徑跟蹤子系統(tǒng)是漸進(jìn)穩(wěn)定的，即

(18)

2.3 速度協(xié)調(diào)控制器

當(dāng)無(wú)人艇運(yùn)動(dòng)預(yù)期位置后，需要保證多無(wú)人艇首向和速度達(dá)到一致并保持穩(wěn)定.設(shè)計(jì)速度協(xié)調(diào)控制器，保證在設(shè)定隊(duì)形下達(dá)到一致，實(shí)現(xiàn)多無(wú)人艇間的協(xié)調(diào)，而不影響空間域上的路徑跟蹤性能.

定義投影向量h，首向偏差z1，速度偏差z2為[16]

h=[0,0,1]T

(19)

z1=ψi-ψdi=hTηi-ψdi

(20)

z2=[z2.1,z2,2,z2,3]T=υi-αi

(21)

式中：αi為虛擬控制量，

αi=[αui,αvi,αri]T

(22)

構(gòu)造第二個(gè)Lyapunov函數(shù)為

(23)

對(duì)V2微分可得

式中：k3>0，并且選取了首向虛擬控制量：

(25)

根據(jù)動(dòng)力學(xué)模型(2)和(4)：

因此，式可以改寫成

(27)

因此設(shè)計(jì)控制器如下.

rMSTJT(ψi)υc-(C(υ)+D(υ))JT(ψi)υc-k4z2

(28)

其中，環(huán)境適應(yīng)性參數(shù)為

(29)

最終式(24)可轉(zhuǎn)化為

(30)

式中：k4=diag(k4,1,k4,2,k4,3)>0.

由此可知，速度協(xié)調(diào)子系統(tǒng)是漸進(jìn)穩(wěn)定的，即

(31)

2.4 穩(wěn)定性分析

考慮到控制器包含路徑跟蹤和速度協(xié)調(diào)兩個(gè)子系統(tǒng)，定義起始偏差

(32)

定理1對(duì)于光滑的參考路徑，設(shè)計(jì)路徑跟蹤控制律(14)，并結(jié)合首向控制律(25)，速度協(xié)調(diào)控制律(28)和環(huán)境適應(yīng)律(29)，可以保證關(guān)于偏差的系統(tǒng)是漸進(jìn)穩(wěn)定的.

證明在路徑跟蹤控制器中定義了虛擬控制量αui和αvi，在速度協(xié)調(diào)控制器中定義了虛擬控制量αri，αri可改寫成為αri=-k3ψei+rdi.則

(33)

式中：rdi為跟隨艇i的期望首向速度.

(34)

根據(jù)式(31)，該子系統(tǒng)是漸進(jìn)穩(wěn)定的.

然后考慮路徑跟蹤子系統(tǒng)，定義包含誤差子系統(tǒng)ε的Lyapunov函數(shù)：

(35)

根據(jù)式(18)，該子系統(tǒng)是漸進(jìn)穩(wěn)定的.

3 仿真分析

為驗(yàn)證編隊(duì)路徑跟蹤控制效果，以三艘無(wú)人艇作為仿真實(shí)驗(yàn)平臺(tái)，其中一艘為領(lǐng)航者，另外兩艘為跟隨者.仿真所用無(wú)人艇模型采用Cybership-II，其主要參數(shù)和水動(dòng)力導(dǎo)數(shù)在文獻(xiàn)[14]中有詳細(xì)介紹.

外界環(huán)境力參數(shù)見(jiàn)表1，緩變環(huán)境力可以通過(guò)經(jīng)驗(yàn)公式[17]計(jì)算得到.期望路徑通過(guò)路徑點(diǎn)給出，兩線段之間通過(guò)圓弧過(guò)渡，期望速度等參數(shù)見(jiàn)表2.表3為三艘無(wú)人艇的初始點(diǎn)位置及編隊(duì)的相對(duì)位置關(guān)系.控制器參數(shù)見(jiàn)表4.仿真中推進(jìn)器上下限制值為

umax=[2N,10°]T,umin=-[2N,10°]T

在圓弧階段，不僅跟隨艇i的首向角與領(lǐng)航艇的首向角滿足式(8)，而且跟隨艇的圓弧切向(縱向)期望速度與領(lǐng)航艇的切向期望速度滿足：

(36)

表2 預(yù)設(shè)路徑、相應(yīng)的轉(zhuǎn)彎半徑、期望速度參數(shù)

表3 初始位置和編隊(duì)相對(duì)關(guān)系

表4 控制器參數(shù)

圖4 多無(wú)人艇運(yùn)動(dòng)軌跡及同時(shí)刻的相對(duì)位置

圖5 編隊(duì)隊(duì)形相對(duì)位置

仿真結(jié)果見(jiàn)圖4～8.圖4為領(lǐng)航艇和跟隨艇運(yùn)動(dòng)軌跡，以及同時(shí)刻的相對(duì)位置圖.由圖4可知，領(lǐng)航艇在根據(jù)預(yù)設(shè)點(diǎn)形成的軌跡上航行，跟隨艇按照設(shè)定的編隊(duì)隊(duì)形航行.圖5為領(lǐng)航艇和跟隨艇的相對(duì)位置.在開(kāi)始階段和轉(zhuǎn)向階段跟隨艇需要調(diào)整自身位置達(dá)到設(shè)定值，之后縱向和橫向相對(duì)位置收斂到設(shè)定值.圖6為各艇首向角隨時(shí)間變化曲線.開(kāi)始階段，為了沿著設(shè)定路徑航行，首向角將會(huì)被調(diào)整.之后跟隨艇的首向角將與領(lǐng)航艇保持一致.在領(lǐng)航艇轉(zhuǎn)向時(shí)，跟隨者的首向角也會(huì)改變.轉(zhuǎn)向后，跟隨艇的首向角將被調(diào)整與領(lǐng)航艇保持一致.由于存在外界環(huán)境力，無(wú)人艇的首向不是沿著路徑的切線方向，但其航向角與路徑的切線方向保持一致.圖7為無(wú)人艇縱向和橫向速度.領(lǐng)航艇轉(zhuǎn)向時(shí)，位于領(lǐng)航艇轉(zhuǎn)向內(nèi)側(cè)的跟隨艇(第1次轉(zhuǎn)向時(shí)的1號(hào)跟隨艇，第2次轉(zhuǎn)向時(shí)的2號(hào)跟隨艇)的圓弧半徑比領(lǐng)航艇的小，因此,速度比領(lǐng)航艇速度?。晃挥陬I(lǐng)航艇轉(zhuǎn)向外側(cè)的跟隨艇(第1次轉(zhuǎn)向時(shí)的2號(hào)跟隨艇，第2次轉(zhuǎn)向時(shí)的1號(hào)跟隨艇)的圓弧半徑比領(lǐng)航艇的大，其速度比領(lǐng)航艇速度大.螺旋槳推力和舵角見(jiàn)圖8，推力和舵角在設(shè)定的限制值內(nèi).為了適應(yīng)外界環(huán)境力，舵角不為零.

圖6 各艇首向角變化圖

圖7 各艇縱向和橫向速度變化圖

圖8 各艇推力和舵角變化圖

為驗(yàn)證考慮圓弧過(guò)渡并且滿足圓弧設(shè)定要求的編隊(duì)路徑跟蹤控制方法有效性，在相同的控制器參數(shù)下，與未考慮圓弧過(guò)渡的結(jié)果進(jìn)行對(duì)比.圖9為兩種情況下無(wú)人艇軌跡圖.由圖9可知，在轉(zhuǎn)向過(guò)程時(shí)，在未考慮圓弧過(guò)渡時(shí)跟隨艇出現(xiàn)轉(zhuǎn)圈的現(xiàn)象(如放大圖所示)，主要原因?yàn)楦S艇在領(lǐng)航艇坐標(biāo)系中相對(duì)位置超過(guò)了設(shè)定位置，需要跟隨者向領(lǐng)航者的后面運(yùn)動(dòng)，同時(shí)向后面運(yùn)動(dòng)導(dǎo)致橫向相對(duì)位置變大.兩種情況下的編隊(duì)相對(duì)位置見(jiàn)圖10a)，考慮圓弧過(guò)渡的情況軌跡收斂速度更快且與設(shè)定值之間沒(méi)有偏差.圖10b)為兩種情況下無(wú)人艇航向角變化情況.兩種情況都能使跟隨艇首向角與領(lǐng)航艇保持一致，但考慮圓弧過(guò)渡的情況首向角收斂速度更快.因此考慮圓弧過(guò)渡的情況軌跡更加平滑，編隊(duì)效果更好.

圖9 兩種情況下編隊(duì)軌跡對(duì)比圖

圖10 兩種情況下編隊(duì)相對(duì)位置和無(wú)人艇首向角對(duì)比圖

4 結(jié) 束 語(yǔ)

文中提出一種多點(diǎn)路徑的編隊(duì)路徑跟蹤控制方法.考慮了不同路徑中的編隊(duì)隊(duì)形數(shù)學(xué)模型之間的偏差.基于時(shí)空解耦策略把編隊(duì)路徑跟蹤控制分解為路徑跟蹤和編隊(duì)形成兩個(gè)子問(wèn)題.在每艘無(wú)人艇上設(shè)計(jì)路徑跟蹤控制器，使跟蹤誤差漸近收斂到零形成編隊(duì)隊(duì)形，即空間上滿足無(wú)人艇在期望路徑上運(yùn)動(dòng).同時(shí)設(shè)計(jì)速度協(xié)調(diào)控制器，來(lái)調(diào)整自身的運(yùn)動(dòng)速度，最終達(dá)到期望的速度，在時(shí)間上實(shí)現(xiàn)多無(wú)人艇間的協(xié)調(diào)，保持設(shè)定的隊(duì)形.采用三艘無(wú)人艇進(jìn)行仿真實(shí)驗(yàn)，在多路徑點(diǎn)過(guò)渡過(guò)程中，考慮編隊(duì)隊(duì)形中相對(duì)首向和速度的變化，其結(jié)果比未考慮路徑的過(guò)渡過(guò)程更加平滑.然而控制方法中忽略路徑跟蹤過(guò)程可能出現(xiàn)障礙物，將在后續(xù)進(jìn)行深入研究.