本發(fā)明涉及無(wú)人水面艇的編隊(duì)控制領(lǐng)域，具體涉及一種基于領(lǐng)導(dǎo)跟隨結(jié)構(gòu)的無(wú)人水面艇編隊(duì)控制方法。

背景技術(shù)：

無(wú)人水面艇是一種具有在實(shí)際海洋環(huán)境下自主航行的能力，并能夠自主完成環(huán)境感知、目標(biāo)探測(cè)等任務(wù)的水面航行器。無(wú)人水面艇具有廣闊的應(yīng)用前景，可用于海洋資源的研究、勘探、開(kāi)采與運(yùn)輸，惡劣海況(如海上大風(fēng)、巨浪、熱帶風(fēng)暴等)的探測(cè)與預(yù)警，海洋地質(zhì)環(huán)境的勘探與監(jiān)控，海洋水文的觀測(cè)，以及海洋氣象研究等領(lǐng)域。

就目前的無(wú)人水面艇技術(shù)水平而言，單個(gè)無(wú)人水面艇在信息的獲取、處理及控制能力等方面是有限的。面對(duì)復(fù)雜的工作任務(wù)以及多變的海洋工作環(huán)境，單個(gè)無(wú)人水面艇的執(zhí)行能力可能顯得不足。利用多個(gè)無(wú)人水面艇組成的群體系統(tǒng)，將復(fù)雜的工作任務(wù)進(jìn)行分解成各個(gè)簡(jiǎn)單的子任務(wù)，多個(gè)無(wú)人水面艇并行地執(zhí)行各自的子任務(wù)，并通過(guò)各個(gè)無(wú)人水面艇的相互通信、協(xié)調(diào)、協(xié)作來(lái)提高整個(gè)工作的效率，可完成單個(gè)無(wú)人水面艇無(wú)法或難以完成的工作。受到自然界生物群體隊(duì)形的啟發(fā)，無(wú)人艇編隊(duì)控制是一個(gè)典型的協(xié)調(diào)運(yùn)動(dòng)控制問(wèn)題。多個(gè)無(wú)人水面艇編隊(duì)控制指通過(guò)設(shè)計(jì)合適的控制策略，使得多個(gè)無(wú)人水面艇組成的編隊(duì)系統(tǒng)的各個(gè)無(wú)人艇之間能夠保持一個(gè)期望的相對(duì)位置和姿態(tài)，并維持隊(duì)形的協(xié)同運(yùn)動(dòng)，完成特定的任務(wù)。無(wú)人水面艇編隊(duì)系統(tǒng)通常比單個(gè)無(wú)人水面艇系統(tǒng)具有優(yōu)越性，可以通過(guò)群體的編隊(duì)協(xié)同合作來(lái)彌補(bǔ)單個(gè)無(wú)人艇能力的不足，擴(kuò)大完成任務(wù)的能力范圍，完成單個(gè)無(wú)人水面艇難以完成的復(fù)雜任務(wù)。

本發(fā)明針對(duì)多個(gè)全驅(qū)動(dòng)無(wú)人水面艇的編隊(duì)控制問(wèn)題，提出基于領(lǐng)導(dǎo)者--跟隨者(leader-follower)的編隊(duì)控制方法解決任一跟隨者與其領(lǐng)導(dǎo)者防止碰撞和保持連接的問(wèn)題。根據(jù)領(lǐng)導(dǎo)者與跟隨者的安全距離和通訊連接范圍，設(shè)計(jì)跟隨者的位置輸出跟蹤誤差約束條件及跟蹤轉(zhuǎn)換函數(shù)，并構(gòu)造性地設(shè)計(jì)編隊(duì)控制器，確保任一無(wú)人水面艇的位置跟蹤誤差在任何時(shí)刻均不違反位置輸出跟蹤誤差約束條件，即保證了任一跟隨者與其領(lǐng)導(dǎo)者始終保持一定的安全距離并處于其領(lǐng)導(dǎo)者的通訊連接范圍內(nèi)，從而解決了編隊(duì)控制中任一跟隨者與其領(lǐng)導(dǎo)者防止碰撞和保持連接的問(wèn)題。

技術(shù)實(shí)現(xiàn)要素：

本發(fā)明的目的是針對(duì)上述現(xiàn)有技術(shù)的不足，提供了一種基于領(lǐng)導(dǎo)跟隨結(jié)構(gòu)的無(wú)人水面艇編隊(duì)控制方法，該方法針對(duì)無(wú)人水面艇編隊(duì)控制中的防止碰撞和保持連接問(wèn)題，將這兩個(gè)問(wèn)題轉(zhuǎn)化為位置跟蹤誤差的約束問(wèn)題，提出基于領(lǐng)導(dǎo)者-跟隨者結(jié)構(gòu)的編隊(duì)控制方法，設(shè)計(jì)編隊(duì)控制器使得任一無(wú)人水面艇的位置跟蹤誤差在任何時(shí)刻均不違反該約束條件，解決了防止碰撞和保持連接的問(wèn)題。

本發(fā)明的目的可以通過(guò)如下技術(shù)方案實(shí)現(xiàn)：

一種基于領(lǐng)導(dǎo)跟隨結(jié)構(gòu)的無(wú)人水面艇編隊(duì)控制方法，所述方法包括以下步驟：

步驟(1)、建立多個(gè)無(wú)人水面艇的動(dòng)態(tài)模型；

步驟(2)、依據(jù)領(lǐng)導(dǎo)者-跟隨者的編隊(duì)控制方式，根據(jù)領(lǐng)導(dǎo)者與跟隨者的安全距離和通訊連接范圍設(shè)計(jì)跟隨者的位置來(lái)輸出跟蹤誤差約束條件；

步驟(3)、設(shè)計(jì)跟蹤誤差轉(zhuǎn)換函數(shù)，將跟蹤誤差不等式約束轉(zhuǎn)換成等式約束；

步驟(4)、應(yīng)用動(dòng)態(tài)面控制技術(shù)設(shè)計(jì)虛擬控制器：結(jié)合動(dòng)態(tài)面控制技術(shù)與逐步后推控制器設(shè)計(jì)技術(shù)避免虛擬控制器的求導(dǎo)，從而避免控制器的輸入包含不可測(cè)的加速度信息；

步驟(5)、設(shè)計(jì)擾動(dòng)觀測(cè)器補(bǔ)償外界未知干擾：應(yīng)用擾動(dòng)觀測(cè)器的方法，設(shè)計(jì)擾動(dòng)觀測(cè)器對(duì)外界未知干擾進(jìn)行估計(jì)，并設(shè)計(jì)相應(yīng)的前饋擾動(dòng)補(bǔ)償控制器；

步驟(6)、應(yīng)用李雅普諾夫穩(wěn)定性理論設(shè)計(jì)編隊(duì)跟蹤控制器：設(shè)計(jì)編隊(duì)跟蹤控制器，應(yīng)用李雅普諾夫穩(wěn)定性理論嚴(yán)格證明閉環(huán)系統(tǒng)的穩(wěn)定性，并確保編隊(duì)控制中任一跟隨者與其領(lǐng)導(dǎo)者始終保持一定的安全距離并處于其領(lǐng)導(dǎo)者的通訊連接范圍內(nèi)。

進(jìn)一步地，步驟(1)中，所述多個(gè)無(wú)人水面艇的動(dòng)態(tài)模型為：

其中，ui表示第i個(gè)無(wú)人水面艇的縱向速度，vi表示第i個(gè)無(wú)人水面艇的橫蕩速度，ri表示第i個(gè)無(wú)人水面艇的轉(zhuǎn)向角速度，ψi(t)為第i個(gè)無(wú)人水面艇的航向角，i＝1、2、3……n，表示ψi(t)的導(dǎo)數(shù)，表示第i個(gè)無(wú)人水面艇在x軸方向的速度，表示第i個(gè)無(wú)人水面艇在y軸方向的速度，mi表示無(wú)人水面艇的質(zhì)量矩陣，表示第i個(gè)無(wú)人水面艇在u，v，r方向上的加速度構(gòu)成的向量，c(vi)表示科氏力矩陣，vi＝[ui,vi,ri]^t，d(vi)表示阻尼矩陣，τi＝[τui,τvi,τri]^t，τui表示第i個(gè)無(wú)人水面艇縱向的推力，τvi表示第i個(gè)無(wú)人水面艇橫蕩方向的推力，τri表示第i個(gè)無(wú)人水面艇轉(zhuǎn)向的力矩，τwi＝[τwui,τwvi,τwri]^t，τwui表示第i個(gè)無(wú)人水面艇在縱向方向受到的外部時(shí)變擾動(dòng)，τwvi表示第i個(gè)無(wú)人水面艇在橫蕩方向受到的外部時(shí)變擾動(dòng)，τwri表示第i個(gè)無(wú)人水面艇在轉(zhuǎn)向角方向受到的外部時(shí)變擾動(dòng)。

進(jìn)一步地，所述步驟(2)的具體過(guò)程為：將多個(gè)無(wú)人水面艇依次編號(hào)為：1、2、3……n，任一無(wú)人水面艇i作為跟隨者與它的領(lǐng)導(dǎo)者無(wú)人水面艇i-1的可視距離為夾角為在整個(gè)運(yùn)動(dòng)控制過(guò)程中滿(mǎn)足如下約束條件：di,col<di(t)<di,con，以使領(lǐng)導(dǎo)者與跟隨者保持安全距離同時(shí)又在通訊連接范圍內(nèi)，其中i＝1、2、3……n，當(dāng)i＝0時(shí)，表示給定的第一艘無(wú)人水面艇的參考軌跡，(xi(t)，yi(t))為第i個(gè)無(wú)人水面艇的位置，(xi-1(t)，yi-1(t))為第i-1個(gè)無(wú)人水面艇的位置，di,col>0為無(wú)人水面艇之間防止碰撞的最小安全距離，di,con為無(wú)人水面艇之間保持通訊連接的最大允許距離，且di,con>di,col>0，引入任一無(wú)人水面艇i與其領(lǐng)導(dǎo)者無(wú)人水面艇i-1之間的期望距離di,des，并定義任一無(wú)人水面艇i與它的領(lǐng)導(dǎo)者無(wú)人水面艇i-1的的跟蹤誤差edi(t)＝di(t)-di,des、角度誤差eψi(t)＝ψi-1(t)-ψi(t)，其中di,con>di,des>di,col>0，ψi-1(t)為第i-1個(gè)無(wú)人水面艇的航向角，ψi(t)為第i個(gè)無(wú)人水面艇的航向角，將跟隨者與其領(lǐng)導(dǎo)者之間的距離約束轉(zhuǎn)換為跟蹤誤差約束：di,col-di,des<edi(t)<di,con-di,des。

進(jìn)一步地，步驟(2)中，所述跟蹤誤差約束條件設(shè)計(jì)如下：

其中，edi(t)表示任一無(wú)人水面艇i與它的領(lǐng)導(dǎo)者無(wú)人水面艇i-1的跟蹤誤差，eψi(t)表示任一無(wú)人水面艇i與它的領(lǐng)導(dǎo)者無(wú)人水面艇i-1的角度誤差，edi(t)表示edi(t)的下界性能函數(shù)，表示edi(t)的上界性能函數(shù)，eψi(t)表示eψi(t)的下界性能函數(shù)，表示eψi(t)的上界性能函數(shù)，di,des表示任一無(wú)人水面艇i與其領(lǐng)導(dǎo)者無(wú)人水面艇i-1之間的期望距離，di,col表示無(wú)人水面艇之間防止碰撞的最小安全距離，edi,∞表示性能函數(shù)edi(t)的穩(wěn)態(tài)值，κdi表示性能函數(shù)edi(t)的收斂速度，di,con表示無(wú)人水面艇之間保持通訊連接的最大允許距離，表示性能函數(shù)的穩(wěn)態(tài)值，eψi,0表示性能函數(shù)eψi(t)的初始值，eψi,∞表示性能函數(shù)eψi(t)的穩(wěn)定值，κψi表示性能函數(shù)eψi(t)的收斂速度。

進(jìn)一步地，步驟(3)中設(shè)計(jì)的誤差跟蹤轉(zhuǎn)換函數(shù)為：

其中，zji表示第i個(gè)無(wú)人水面艇的轉(zhuǎn)換誤差，γji表示第i個(gè)無(wú)人水面艇性能函數(shù)的上界除以下界，表示自然底數(shù)的zji次冪，表示自然底數(shù)的-zji次冪，表示第i個(gè)無(wú)人水面艇性能函數(shù)的下界除以上界，當(dāng)且僅當(dāng)zji＝0時(shí)，tji(zji,γji)＝0；將跟蹤誤差不等式約束轉(zhuǎn)換成如下等式約束：

其中：

得到如下轉(zhuǎn)換誤差：

進(jìn)一步地，步驟(4)中，引入動(dòng)態(tài)面控制技術(shù)并設(shè)計(jì)虛擬控制器的一階濾波器為：

其中，αfi＝[αf1i,αf2i,αf3i]^t為濾波虛擬控制輸入向量，αf1i表示α1i的濾波虛擬控制，αf2i表示α2i的濾波虛擬控制，αf3i表示α3i的濾波虛擬控制，表示αfi的導(dǎo)數(shù)，αi＝[α1i,α2i,α3i]^t為虛擬控制輸入向量，α1i表示縱向速度ui的虛擬控制器，α2i表示橫蕩速度vi的虛擬控制器，α3i表示轉(zhuǎn)向角速度ri的虛擬控制器，μi＝diag[μ1,μ2,μ3]為濾波器時(shí)間常數(shù)矩陣，μ1表示設(shè)計(jì)濾波虛擬控制αf1i的濾波器時(shí)間常數(shù)，μ2表示設(shè)計(jì)濾波虛擬控制αf2i的濾波器時(shí)間常數(shù)，μ3表示設(shè)計(jì)濾波虛擬控制αf3i的濾波器時(shí)間常數(shù)，i∈1、2、3……n,j＝d、ψ；αfi(0)表示濾波虛擬控制αfi的初始值，αi(0)表示虛擬控制器αi的初始值，虛擬控制器αi設(shè)計(jì)如下：

其中，ψi(t)為第i個(gè)無(wú)人水面艇的航向角，表示第i-1個(gè)無(wú)人水面艇的航向角的導(dǎo)數(shù)，為第i個(gè)無(wú)人水面艇和第i-1個(gè)無(wú)人水面艇之間的視角，kzdi>0表示虛擬控制器α1i的設(shè)計(jì)參數(shù)，zdi表示第i個(gè)無(wú)人水面艇的距離轉(zhuǎn)換誤差，i∈1、2、3……n,j＝d、ψ；表示第i-1個(gè)無(wú)人水面艇在x軸方向的速度，表示第i-1個(gè)無(wú)人水面艇在y軸方向的速度，kzψi>0表示虛擬控制器α3i的設(shè)計(jì)參數(shù)，zψi表示第i個(gè)無(wú)人水面艇的角度轉(zhuǎn)換誤差。

進(jìn)一步地，步驟(5)中的擾動(dòng)觀測(cè)器設(shè)計(jì)如下：

其中，eαi＝αfi-αi，αfi＝[αf1i,αf2i,αf3i]^t為濾波虛擬控制輸入向量，αi＝[α1i,α2i,α3i]^t為虛擬控制輸入向量，z2i＝vi-αfi＝[z21i,z22i,z23i]^t，vi＝[ui,vi,ri]^t，z21i表示縱向速度ui與濾波虛擬控制αf1i的差，z22i表示橫蕩速度vi與濾波虛擬控制αf2i的差，z23i表示轉(zhuǎn)向角速度ri與濾波虛擬控制αf3i的差，kd1i＝diag[kd11i,kd12i,kd13i]為對(duì)角矩陣，kd11i表示第一個(gè)擾動(dòng)觀測(cè)器的設(shè)計(jì)參數(shù)，kd12i表示第二個(gè)擾動(dòng)觀測(cè)器的設(shè)計(jì)參數(shù)，kd13i表示第三個(gè)擾動(dòng)觀測(cè)器的設(shè)計(jì)參數(shù)，mi表示無(wú)人水面艇的質(zhì)量矩陣，c(vi)表示科氏力矩陣，d(vi)表示阻尼矩陣，τi＝[τui,τvi,τri]^t，τui表示第i個(gè)無(wú)人水面艇縱向的推力，τvi表示第i個(gè)無(wú)人水面艇橫蕩方向的推力，τri表示第i個(gè)無(wú)人水面艇轉(zhuǎn)向的力矩，μi＝diag[μ1,μ2,μ3]為濾波器時(shí)間常數(shù)矩陣，μ1表示濾波虛擬控制αf1i的濾波器時(shí)間常數(shù)，μ2表示濾波虛擬控制αf2i的濾波器時(shí)間常數(shù)，μ3表示濾波虛擬控制αf3i的濾波器時(shí)間常數(shù)，ξ1i＝[ξ11i,ξ12i,ξ13i]^t是擾動(dòng)觀測(cè)器的狀態(tài)變量，ξ11i表示第一個(gè)擾動(dòng)觀測(cè)器的狀態(tài)變量，ξ12i表示第二個(gè)擾動(dòng)觀測(cè)器的狀態(tài)變量，ξ13i表示第三個(gè)擾動(dòng)觀測(cè)器的狀態(tài)變量，為外界未知擾動(dòng)的估計(jì)值。

進(jìn)一步地，步驟(6)中設(shè)計(jì)的編隊(duì)跟蹤控制器如下：

其中，k2i＝diag[k21i,k22i,k23i]為對(duì)角矩陣，k21i表示編隊(duì)控制器τ1的設(shè)計(jì)參數(shù)，k22i表示編隊(duì)控制器τ2的設(shè)計(jì)參數(shù)，k23i表示編隊(duì)控制器τ3的設(shè)計(jì)參數(shù)。

本發(fā)明與現(xiàn)有技術(shù)相比，具有如下優(yōu)點(diǎn)和有益效果：

1、本發(fā)明通過(guò)擾動(dòng)觀測(cè)器來(lái)補(bǔ)償風(fēng)浪流等外界未知干擾，使得本設(shè)計(jì)方法對(duì)外界干擾具有較強(qiáng)的魯棒性。

2、本發(fā)明采用動(dòng)態(tài)面控制技術(shù)設(shè)計(jì)跟蹤編隊(duì)控制器，避免使用領(lǐng)導(dǎo)者的加速度，提高了設(shè)計(jì)方案的實(shí)用性。

3、本發(fā)明采用的控制器設(shè)計(jì)方案，使得任一無(wú)人水面艇的位置跟蹤誤差在任何時(shí)刻均不違反位置輸出跟蹤誤差約束條件，確保了任一跟隨者與其領(lǐng)導(dǎo)者始終保持一定的安全距離并處于其領(lǐng)導(dǎo)者的通訊連接范圍內(nèi)，從而解決了編隊(duì)控制中任一跟隨者與其領(lǐng)導(dǎo)者防止碰撞和保持連接的問(wèn)題。

附圖說(shuō)明

圖1為本發(fā)明實(shí)施例多個(gè)無(wú)人水面艇的領(lǐng)導(dǎo)者-跟隨者編隊(duì)結(jié)構(gòu)示意圖。

圖2為本發(fā)明實(shí)施例多個(gè)無(wú)人水面艇編隊(duì)控制系統(tǒng)結(jié)構(gòu)框圖。

圖3為本發(fā)明實(shí)施例一組無(wú)人水面艇與其領(lǐng)導(dǎo)者的可視距離變化示意圖。

圖4為本發(fā)明實(shí)施例一組無(wú)人水面艇的航向角跟蹤誤差eψi(t)的示意圖。

圖5為本發(fā)明實(shí)施例一組無(wú)人水面艇編隊(duì)控制的位置輸出軌跡示意圖。

圖6為本發(fā)明實(shí)施例無(wú)人水面艇縱向的推力τui的示意圖。

圖7為本發(fā)明實(shí)施例無(wú)人水面艇橫蕩方向的推力τvi的示意圖。

圖8為本發(fā)明實(shí)施例無(wú)人水面艇轉(zhuǎn)向的力矩τri的示意圖。

具體實(shí)施方式

下面結(jié)合實(shí)施例及附圖對(duì)本發(fā)明作進(jìn)一步詳細(xì)的描述，但本發(fā)明的實(shí)施方式不限于此。

實(shí)施例：

本實(shí)施例提供了一種基于領(lǐng)導(dǎo)跟隨結(jié)構(gòu)的無(wú)人水面艇編隊(duì)控制方法，該方法針對(duì)無(wú)人水面艇編隊(duì)控制中的防止碰撞和保持連接問(wèn)題，將這兩個(gè)問(wèn)題轉(zhuǎn)化為位置跟蹤誤差的約束問(wèn)題，提出基于領(lǐng)導(dǎo)者-跟隨者結(jié)構(gòu)的編隊(duì)控制方法，多個(gè)無(wú)人水面艇的領(lǐng)導(dǎo)者-跟隨者編隊(duì)結(jié)構(gòu)示意圖如圖1所示，多個(gè)無(wú)人水面艇編隊(duì)控制系統(tǒng)結(jié)構(gòu)框圖如圖2所示，所述方法具體包括以下步驟：

步驟(1)、建立多個(gè)無(wú)人水面艇的動(dòng)態(tài)模型；

所述多個(gè)無(wú)人水面艇的動(dòng)態(tài)模型為：

其中，ui表示第i個(gè)無(wú)人水面艇的縱向速度，vi表示第i個(gè)無(wú)人水面艇的橫蕩速度，ri表示第i個(gè)無(wú)人水面艇的轉(zhuǎn)向角速度，ψi(t)為第i個(gè)無(wú)人水面艇的航向角，i＝1、2、3……n，表示ψi(t)的導(dǎo)數(shù)，表示第i個(gè)無(wú)人水面艇在x軸方向的速度，表示第i個(gè)無(wú)人水面艇在y軸方向的速度，表示第i個(gè)無(wú)人水面艇在u，v，r方向上的加速度構(gòu)成的向量，vi＝[ui,vi,ri]^t，τi＝[τui,τvi,τri]^t，τui表示第i個(gè)無(wú)人水面艇縱向的推力，τvi表示第i個(gè)無(wú)人水面艇橫蕩方向的推力，τri表示第i個(gè)無(wú)人水面艇轉(zhuǎn)向的力矩，τwi＝[τwui,τwvi,τwri]^t，τwui表示第i個(gè)無(wú)人水面艇在縱向方向受到的外部時(shí)變擾動(dòng)，τwvi表示第i個(gè)無(wú)人水面艇在橫蕩方向受到的外部時(shí)變擾動(dòng)，τwri表示第i個(gè)無(wú)人水面艇在轉(zhuǎn)向角方向受到的外部時(shí)變擾動(dòng)，mi表示無(wú)人水面艇的質(zhì)量矩陣，c(vi)表示科氏力矩陣，d(vi)表示阻尼矩陣，其中：

τwi＝[2+4sin(0.05t),-3+2cos(0.05t),2-5sin(0.05t)]^t

在本實(shí)施例中，選取5個(gè)(i＝1,2,3,4,5)相同的無(wú)人水面艇動(dòng)態(tài)模型，無(wú)人艇的系統(tǒng)參數(shù)分別為：

m11i＝25.8kg，m22i＝33.8kg，m23i＝m32i＝1.0948kg，

m33i＝2.76kg，c13(vi)＝-m22ivi-m23iri，

d22(vi)＝0.8612+36.2823*|vi|+0.805*|ri|，

d23(vi)＝-0.1079+0.845*|vi|+3.45*|ri|，

d32(vi)＝-0.1052-5.0437*|vi|-0.13*|ri|，

d33(vi)＝1.9-0.08*|vi|+0.75*|ri|，i＝1,2,3,4,5。

艇體長(zhǎng)度為li＝1.225m。

步驟(2)、依據(jù)領(lǐng)導(dǎo)者-跟隨者的編隊(duì)控制方式，根據(jù)領(lǐng)導(dǎo)者與跟隨者的安全距離和通訊連接范圍設(shè)計(jì)跟隨者的位置來(lái)輸出跟蹤誤差約束條件，任一無(wú)人水面艇i與它的領(lǐng)導(dǎo)者無(wú)人水面艇i-1的可視距離為夾角為在整個(gè)運(yùn)動(dòng)控制過(guò)程中滿(mǎn)足如下約束條件：di,col<di(t)<di,con，以使領(lǐng)導(dǎo)者與跟隨者保持安全距離同時(shí)又在通訊連接范圍內(nèi)，當(dāng)i＝0時(shí)，表示給定的第一艘無(wú)人水面艇的參考軌跡，(xi(t)，yi(t))為第i個(gè)無(wú)人水面艇的位置，(xi-1(t)，yi-1(t))為第i-1個(gè)無(wú)人水面艇的位置，di,col>0為無(wú)人水面艇之間防止碰撞的最小安全距離，di,con為無(wú)人水面艇之間保持通訊連接的最大允許距離，且di,con>di,col>0，引入任一無(wú)人水面艇i與其領(lǐng)導(dǎo)者無(wú)人水面艇i-1之間的期望距離di,des，并定義任一無(wú)人水面艇i與它的領(lǐng)導(dǎo)者無(wú)人水面艇i-1的的跟蹤誤差edi(t)＝di(t)-di,des、角度誤差eψi(t)＝ψi-1(t)-ψi(t)，其中di,con>di,des>di,col>0，ψi-1(t)為第i-1個(gè)無(wú)人水面艇的航向角，ψi(t)為第i個(gè)無(wú)人水面艇的航向角，將跟隨者與其領(lǐng)導(dǎo)者之間的距離約束轉(zhuǎn)換為跟蹤誤差約束：di,col-di,des<edi(t)<di,con-di,des，本實(shí)施例中，di,con＝6m，di,des＝5m，di,col＝4m，所期望的參考軌跡設(shè)計(jì)為：

當(dāng)t≤60s時(shí)，所期望的參考軌跡為直線運(yùn)動(dòng)：xd＝3t，yd＝ψd＝0；

當(dāng)t>60s時(shí)，所期望的參考軌跡為如下圓周運(yùn)動(dòng)：

xd＝180+60sin(0.05(t-60))，

yd＝60(1-cos(0.05(t-60)))，

ψd＝0.05(t-60)。

以ηi＝[xi(t),yi(t),ψi(t)]^t表示無(wú)人水面艇的位置(xi(t)，yi(t))和航向角ψi(t)，無(wú)人水面艇的初始位置及航向角分別選擇為η1(0)＝[0,5,0]^t，η2(0)＝[0,10,0]^t，η3(0)＝[0,15,0]^t，η4(0)＝[0,20,0]^t，η5(0)＝[0,25,0]^t，初始速度選擇為vi(0)＝[0,0,0]^t，i＝1,2,3,4,5；

任一跟隨的無(wú)人水面艇i與它的領(lǐng)導(dǎo)者無(wú)人水面艇i-1的跟蹤誤差約束條件設(shè)計(jì)如下：

其中，edi(t)表示任一無(wú)人水面艇i與它的領(lǐng)導(dǎo)者無(wú)人水面艇i-1的跟蹤誤差，eψi(t)表示任一無(wú)人水面艇i與它的領(lǐng)導(dǎo)者無(wú)人水面艇i-1的角度誤差，本實(shí)施例中

步驟(3)、設(shè)計(jì)跟蹤誤差轉(zhuǎn)換函數(shù)，經(jīng)過(guò)轉(zhuǎn)換函數(shù)變換后，得到如下誤差方程：

任一跟隨的無(wú)人水面艇i與它的領(lǐng)導(dǎo)者無(wú)人水面艇i-1的可視距離變化示意圖如圖3所示，任一無(wú)人水面艇的航向角跟蹤誤差eψi(t)如圖4所示。

步驟(4)、應(yīng)用動(dòng)態(tài)面控制技術(shù)設(shè)計(jì)虛擬控制器，引入動(dòng)態(tài)面控制技術(shù)并設(shè)計(jì)虛擬控制器的一階濾波器為：

其中，αfi＝[αf1i,αf2i,αf3i]^t為濾波虛擬控制輸入向量，αi＝[α1i,α2i,α3i]^t為虛擬控制輸入向量，本實(shí)施例中的濾波器時(shí)間常數(shù)矩陣設(shè)計(jì)為μi＝diag[0.1,0.1,0.1]，虛擬控制器αi設(shè)計(jì)如下：

其中，為第i個(gè)無(wú)人水面艇和第i-1個(gè)無(wú)人水面艇之間的視角，kzdi＝kzψi＝4，zdi表示第i個(gè)無(wú)人水面艇的距離轉(zhuǎn)換誤差，表示第i-1個(gè)無(wú)人水面艇在x軸方向的速度，表示第i-1個(gè)無(wú)人水面艇在y軸方向的速度，zψi表示第i個(gè)無(wú)人水面艇的角度轉(zhuǎn)換誤差。

步驟(5)、設(shè)計(jì)擾動(dòng)觀測(cè)器補(bǔ)償外界未知干擾，擾動(dòng)觀測(cè)器設(shè)計(jì)如下：

其中，eαi＝αfi-αi，αfi＝[αf1i,αf2i,αf3i]^t為濾波虛擬控制輸入向量，αi＝[α1i,α2i,α3i]^t為虛擬控制輸入向量，z2i＝vi-αfi＝[z21i,z22i,z23i]^t，vi＝[ui,vi,ri]^t，z21i表示縱向速度ui與濾波虛擬控制αf1i的差，z22i表示橫蕩速度vi與濾波虛擬控制αf2i的差，z23i表示轉(zhuǎn)向角速度ri與濾波虛擬控制αf3i的差，kd1i＝diag[6,6,6]為對(duì)角矩陣，τi＝[τui,τvi,τri]^t，τui表示第i個(gè)無(wú)人水面艇縱向的推力，τui的示意圖如圖6所示，τvi表示第i個(gè)無(wú)人水面艇橫蕩方向的推力，τvi的示意圖如圖7所示，τri表示第i個(gè)無(wú)人水面艇轉(zhuǎn)向的力矩，τri的示意圖如圖8所示，ξ1i＝[ξ11i,ξ12i,ξ13i]^t是擾動(dòng)觀測(cè)器的狀態(tài)變量，觀測(cè)器的狀態(tài)初始值為ξ1i(0)＝[0.5,0.5,0.5]^t，為外界未知擾動(dòng)的估計(jì)值。

步驟(6)、應(yīng)用李雅普諾夫穩(wěn)定性理論設(shè)計(jì)編隊(duì)跟蹤控制器：設(shè)計(jì)編隊(duì)跟蹤控制器，應(yīng)用李雅普諾夫穩(wěn)定性理論嚴(yán)格證明閉環(huán)系統(tǒng)的穩(wěn)定性，并確保編隊(duì)控制中任一跟隨者與其領(lǐng)導(dǎo)者始終保持一定的安全距離并處于其領(lǐng)導(dǎo)者的通訊連接范圍內(nèi)，設(shè)計(jì)的編隊(duì)跟蹤控制器如下：

其中，k2i＝diag[6,6,6]，一組無(wú)人水面艇編隊(duì)控制的位置輸出軌跡示意圖如圖5所示。

以上所述，僅為本發(fā)明專(zhuān)利較佳的實(shí)施例，但本發(fā)明專(zhuān)利的保護(hù)范圍并不局限于此，任何熟悉本技術(shù)領(lǐng)域的技術(shù)人員在本發(fā)明專(zhuān)利所公開(kāi)的范圍內(nèi)，根據(jù)本發(fā)明專(zhuān)利的技術(shù)方案及其發(fā)明專(zhuān)利構(gòu)思加以等同替換或改變，都屬于本發(fā)明專(zhuān)利的保護(hù)范圍。