基于有限時間和狀態(tài)觀測器的雙閉環(huán)AUV軌跡跟蹤控制研究

馬洪潮， 戴曉強(qiáng)， 曾慶軍， 夏 楠， 郭雨青

(1.徐州徐工汽車制造有限公司，江蘇 徐州 221000; 2.江蘇科技大學(xué)電子信息學(xué)院，江蘇 鎮(zhèn)江 212000)

0 引言

海洋蘊藏著豐富的石油、天然氣等發(fā)展經(jīng)濟(jì)所必須的不可再生能源。20世紀(jì)90年代以后，自主水下機(jī)器人(Autonomous Underwater Vehicle,AUV)應(yīng)用于海洋資源勘探、低強(qiáng)度軍事偵察等領(lǐng)域，由于AUV對軌跡和路徑的精確跟蹤的要求越來越高，對其研究也越來越多。對于AUV的軌跡運動控制問題，主要的困難在于絕大多數(shù)AUV屬于欠驅(qū)動類型，不滿足Brockett定理必要的條件，故無法通過持續(xù)的控制策略使AUV達(dá)到鎮(zhèn)定。另外，自主水下機(jī)器人在實際任務(wù)操作中會受到未知干擾、模型的不確定、輸入飽和、超調(diào)等因素的限制，使得AUV在進(jìn)行三維軌跡跟蹤時變得愈發(fā)困難。AUV的軌跡跟蹤控制主要為運動學(xué)控制和動力學(xué)控制。文獻(xiàn)[1]主要通過改進(jìn)積分視線法來設(shè)計運動學(xué)控制器，通過反步自適應(yīng)滑模方法來設(shè)計動力學(xué)控制器；文獻(xiàn)[2]主要通過引用生物啟發(fā)神經(jīng)動態(tài)模型對期望速度進(jìn)行濾波，利用積分滑模控制方法來設(shè)計動力學(xué)控制器，但不能有效抑制滑模固有的抖振問題。為了解決這些問題，文獻(xiàn)[3-4]提出了非線性擾動觀察滑模控制器，根據(jù)新的趨近律改進(jìn)滑模來抑制抖振，并提出一種基于模型預(yù)測控制的新型雙閉環(huán)三維軌跡跟蹤方法，利用新型趨近律和雙閉環(huán)控制器生成控制輸入來抑制滑模抖振；而文獻(xiàn)[5]考慮到外界環(huán)境干擾，使響應(yīng)速度的誤差收斂到零點上下。針對AUV模型的不確定性，文獻(xiàn)[6]提出了基于干擾觀測器的AUV深度自適應(yīng)終端滑模控制，解決對外部環(huán)境干擾和系統(tǒng)擾動難以控制的問題，但未考慮系統(tǒng)的快速收斂性及系統(tǒng)的穩(wěn)定性；文獻(xiàn)[7-8]分別通過自適應(yīng)神經(jīng)網(wǎng)絡(luò)和自適應(yīng)滑模控制方法來解決模型不確定問題和外界干擾，但未考慮神經(jīng)網(wǎng)絡(luò)收斂速度慢和學(xué)習(xí)時間較長的問題。針對輸入飽和性和模型的不確定問題，文獻(xiàn)[9]提出了基于三維自適應(yīng)軌跡跟蹤控制，但并未解決外部環(huán)境干擾下及系統(tǒng)不確定帶來的影響。

綜上所述，為提高雙閉環(huán)系統(tǒng)的收斂速度及克服系統(tǒng)的不確定性，本文引入有限時間的控制策略和降階擴(kuò)張狀態(tài)觀測器來設(shè)計一個高性能的軌跡跟蹤控制器；所設(shè)計的自適應(yīng)律的虛擬向?qū)В菧p弱了外界洋流干擾，克服了超調(diào)，確保了該控制器對位置量、姿態(tài)角的快速收斂以及補(bǔ)償混合不確定項。

1 控制問題描述

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

本文采用的AUV是自主研發(fā)的“探海Ⅱ型”AUV，其六自由度運動學(xué)和動力學(xué)方程如下。下文中未特別說明變量均視為標(biāo)量。

1) 運動學(xué)模型為

(1)

式中：(x,y,z)是AUV在慣性坐標(biāo)系的坐標(biāo)位置；φ,ψ,θ分別為俯仰角、艏向角、橫滾角；u,v,w,p，q,r在隨體坐標(biāo)系中分別表示縱向線速度、橫向線速度、垂向線速度、橫搖角速度、俯仰角速度、艏向角速度[10]。

2) 動力學(xué)模型為

(2)

(3)

綜合式(2)及式(3),AUV動力學(xué)方程可簡化表示為

(4)

其中

(5)

“探海Ⅱ型”AUV如圖1所示。

圖1 “探海Ⅱ型”AUVFig.1 “T-SEA Ⅱ” AUV

1.2 AUV 三維軌跡跟蹤誤差方程

圖2為AUV空間曲線軌跡跟蹤示意圖，其中：I,B,F分別為固定坐標(biāo)系、運動坐標(biāo)系以及Serret-Frenet坐標(biāo)系；AUV的重心與運動坐標(biāo)系原點O重合，F(xiàn)坐標(biāo)系原點為AUV期望軌跡中的任意一個虛擬參考點，相對于AUV的速度為Vp，原點P為水下機(jī)器人期望軌跡上的“虛擬向?qū)А薄?/p>

圖2 虛擬向?qū)к壽E示意圖Fig.2 Schematic diagram of virtual guide trajectory

利用Serret-Frenet坐標(biāo)系導(dǎo)出跟蹤誤差公式，根據(jù)三維軌跡跟蹤示意圖，將固定坐標(biāo)系I繞η軸旋轉(zhuǎn)ψ角度，再繞ζ軸旋轉(zhuǎn)θ角度，然后平移固定坐標(biāo)系中點E與運動坐標(biāo)系P相重合[12]。Serret-Frenet坐標(biāo)系框架相對于固定坐標(biāo)系I的ψ,θ旋轉(zhuǎn)的角度為

(6)

分別定義航向角γα和俯仰角γβ，即

(7)

考慮到AUV在水下實際運動中是受到未知環(huán)境影響的，AUV艏向角ψ和俯仰角θ可以表述為

(8)

綜上所述，AUV運動方程在坐標(biāo)系I下可以重新表示為

(9)

式中：U為AUV空間合成速度；ψω=ψ+β；θω=θ-α。

(10)

式中：ψe和θe分別表示坐標(biāo)系H的艏向角誤差和俯仰角誤差;r/cosθ+β和q+α分別表示水下機(jī)器人的艏向和俯仰角速度，具體可表示為

(11)

綜上所述，Serret-Frenet坐標(biāo)系下的軌跡跟蹤誤差模型可具體表示為

(12)

2 AUV控制系統(tǒng)設(shè)計

2.1 位置控制器設(shè)計

首先定義李雅普諾夫函數(shù)Ve1為

(13)

則對式(13)的系統(tǒng)求導(dǎo)可得

(14)

定義虛擬控制變量α1為

(15)

系數(shù)k1,k2,k3均為大于零的常數(shù)，則

(16)

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

(17)

則系統(tǒng)沿著式(14)的導(dǎo)數(shù)計算如下

(18)

定義虛擬控制輸入α2，設(shè)e2=θ-α1。

α2=-k2e2+1+ue1

(19)

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

(20)

則系統(tǒng)沿著式(14)的導(dǎo)數(shù)計算如下

(21)

定義e3=q-α2，對狀態(tài)量e3采用有限時間控制方法進(jìn)行設(shè)計，得出τq控制器表達(dá)式為

(22)

狀態(tài)量q的誤差導(dǎo)數(shù)可表示為

(23)

假設(shè)系統(tǒng)的平衡點為原點，Ω為系統(tǒng)狀態(tài)有限時間內(nèi)收斂區(qū)域，Ω={e3:|e3|≤[(k4+l)/K]1/a}，K>0,0

證明過程如下。

選擇李雅普諾夫函數(shù)

(24)

系統(tǒng)初始狀態(tài)在Ω外，即e3∈R-Ω，由于V(e3)<0,則存在t1>0,使得e3(t)∈bdΩ，b為待設(shè)計參數(shù)。

將式(21)、式(22)、式(23)代入式(20)得

(25)

根據(jù)有限時間控制器設(shè)計判定以及不等式(23)的證明，所設(shè)計的式(21)控制器是有限時間穩(wěn)定，使得系統(tǒng)的控制效果更好，抗擾動能力強(qiáng)。

2.2 姿態(tài)控制器設(shè)計

為解決外界多種環(huán)境干擾及輸入飽和的問題，引入降階擴(kuò)張狀態(tài)觀測器、動態(tài)面控制技術(shù)、積分滑模及輔助動態(tài)系統(tǒng)。

引入線性降階擴(kuò)張狀態(tài)觀測器(ESO)為

(26)

引入動態(tài)面的思想簡化控制器提高跟蹤響應(yīng)效率和高質(zhì)量的控制輸入。構(gòu)造的數(shù)學(xué)表達(dá)式為

(27)

定義誤差跟蹤速度為

(ueqere)T=(u-v1q-qedr-red)T

(28)

式中：ue為鎮(zhèn)定縱向速度誤差;qe為縱傾速度誤差；re為艏向速度誤差;qed和red為所設(shè)計的虛擬控制律。

設(shè)計用于鎮(zhèn)定縱向、俯仰、艏向速度的誤差積分滑模面為

(29)

式中，λu，λq，λr為控制器待設(shè)計參數(shù)。

對式(29)求導(dǎo)可得

(30)

令式(30)為零，可求得控制律為

(31)

然而，AUV本身存在參數(shù)攝動項，又在洋流的干擾下，等效控制的控制效果并不能收斂逼近；引入指數(shù)趨近律進(jìn)行補(bǔ)償，可以有效抑制滑模帶來的抖振問題，使AUV在大角度轉(zhuǎn)彎時會產(chǎn)生連續(xù)輸出信號，進(jìn)而優(yōu)化AUV的運動效果，進(jìn)一步提高系統(tǒng)的魯棒性。

選取如下趨近律

(32)

考慮到系統(tǒng)飽和對控制性能有較大影響，設(shè)計輔助動態(tài)系統(tǒng)為

(33)

式中:gu=1/m11,gq=1/m44,gr=1/m55;bi>0(i=u,q,r)；χi(i=u,q,r)為輔助動態(tài)系統(tǒng)的狀態(tài)變量。

考慮到執(zhí)行器的飽和問題及趨近律，則實際控制律可表示為

(34)

通過構(gòu)造合理的李雅普諾夫函數(shù)來證明該控制器的穩(wěn)定性，預(yù)選函數(shù)為

(35)

結(jié)合式(34)對式(35)求導(dǎo)得

(36)

進(jìn)一步可簡化為

。

(37)

由楊氏不等式得

(38)

式中，χi=τi，i=u,q,r。

3 仿真實驗

為了驗證控制算法的有效性和可行性，基于“探海II型”AUV數(shù)學(xué)模型進(jìn)行仿真，對AUV的三維軌跡同步跟蹤和鎮(zhèn)定控制問題進(jìn)行數(shù)值仿真。

圖3所示為AUV三維軌跡。圖3中，本文算法在拐點以及軌跡誤差明顯好于自適應(yīng)滑模算法。

圖3 AUV三維軌跡Fig.3 Three dimensional trajectory of AUV

圖4、圖5所示為AUV推力對比及深度控制對比情況。由圖4、圖5可知，在加入隨機(jī)干擾以及在模型不確定性下，自適應(yīng)滑模算法超調(diào)較大且易抖動，在受到較大干擾下，需要較長時間控制到穩(wěn)定的期望狀態(tài)；在有限時間力控制和降階ESO滑模控制下超調(diào)較小，能夠在短時間內(nèi)調(diào)整至期望值并保持穩(wěn)定。

圖4 AUV推力對比圖Fig.4 Comparison of AUV thrust

圖5 AUV深度控制對比圖Fig.5 Comparison of AUV depth control

圖6所示為AUV姿態(tài)角對比情況。由圖6可知，在利用自適應(yīng)滑模算法進(jìn)行姿態(tài)角控制時，收斂速度慢且姿態(tài)抖動較為嚴(yán)重；而在有限時間及降階ESO滑模控制器的姿態(tài)控制下，響應(yīng)速度較快且較好地抑制本身固有的抖振，能夠快速平穩(wěn)回到期望狀態(tài)。

圖6 AUV姿態(tài)角對比圖Fig.6 Comparison of AUV attitude angle

4 湖泊試驗

AUV湖泊試驗于2021年7月在某湖完成，湖水平均深6 m，湖水流速3 kn(1 kn≈1.852 km)左右，下潛深度為1 m。設(shè)置使命為六方形，起點GPS經(jīng)緯度坐標(biāo)為(120.559 228°，31.892 370°)。使命任務(wù)的時長為14 min。圖7為上位機(jī)使命規(guī)劃圖，圖8～10分別為AUV湖試的三維軌跡、姿態(tài)角、深度的變化。

由圖8～10可知，在設(shè)定使命后，AUV能夠迅速到達(dá)期望的軌跡、姿態(tài)、深度及航向；因AUV搭載的捷聯(lián)慣導(dǎo)，所記錄的轉(zhuǎn)向角到達(dá)最高360°后歸零，故圖9在400 s左右艏向角會從零開始，屬正常現(xiàn)象。在AUV進(jìn)行轉(zhuǎn)向時，因?qū)嶋H水流速度發(fā)生較大變化，圖10的深度控制曲線會出現(xiàn)輕微波動，但實際控制的平均誤差在±0.1 m以內(nèi)，航向角誤差在3°內(nèi)，達(dá)到指定的控制目標(biāo)，能夠應(yīng)用于工程項目。

圖7 上位機(jī)使命規(guī)劃Fig.7 Mission planning of host computer

圖8 AUV三維軌跡跟蹤曲線

圖9 AUV姿態(tài)角控制曲線Fig.9 AUV attitude angle control curve

圖10 AUV深度控制曲線Fig.10 AUV depth control curve

5 結(jié)束語

本文考慮在未知環(huán)境下擾動和常規(guī)滑模控制器容易出現(xiàn)控制超調(diào)、收斂速度慢的情況，針對三維軌跡跟蹤控制問題提出了有限時間控制以及降階ESO動態(tài)積分滑模控制。引入動態(tài)輔助系統(tǒng)及具有自適應(yīng)律的虛擬向?qū)А７抡娼Y(jié)果表明，該控制器能較好地結(jié)合降階ESO和有限時間控制的優(yōu)點，較好地抑制了抖振、減弱了外界干擾，同時具有控制器響應(yīng)快、魯棒性好的優(yōu)點，更是克服了在拐點容易出現(xiàn)控制器超調(diào)的問題。