水下機(jī)器人-機(jī)械臂系統(tǒng)的滑模自抗擾控制

李小崗, 王紅都, 黎 明, 劉 鑫

(1. 中國海洋大學(xué) 工程學(xué)院, 山東 青島 266100; 2. 四川警學(xué)院 偵查系, 四川 瀘州 646000)

水下機(jī)器-機(jī)械臂系統(tǒng)(Underwater Vehicle-Manipulator System, UVMS)模型具有高度非線性、強(qiáng)耦合、水動力學(xué)參數(shù)變化等特點(diǎn), 且其在作業(yè)過程受到復(fù)雜海洋多源干擾的作用, 這些都會嚴(yán)重影響UVMS 的操作和定位精度等指標(biāo)[1]。因此, 有必要設(shè)計具有干擾抑制與補(bǔ)償能力的復(fù)合控制器, 以達(dá)到預(yù)期的抗擾等性能指標(biāo)要求。

目前, UVMS 已經(jīng)有了較為成熟的建模與控制方法[2-4]。Fossen[2]采用牛頓-歐拉公式建立了UVMS 的模型, Tarn[3]提出了一種新的基于歐拉-朗格朗日方程和牛頓-歐拉公式的模型建立公式。這兩種經(jīng)典的建模方法為基于模型的控制奠定了基礎(chǔ)。Mahesh[4]研究了UVMS 的協(xié)調(diào)控制, 將水下機(jī)器人和機(jī)械臂系統(tǒng)的相互影響進(jìn)行了系統(tǒng)性的闡述。隨著對控制精度要求的提高, 需要開發(fā)更加高效、精確的控制系統(tǒng), 在此基礎(chǔ)上許多學(xué)者提出了眾多新的控制方法[5]。Huang[6]結(jié)合干擾觀測器, 提出了基于模型的自適應(yīng)控制; 針對UVMS 的動態(tài)不確定性和時變外部擾動, Dai[7]將擴(kuò)張卡爾曼濾波器引入補(bǔ)償系統(tǒng), 將基于模型的力矩控制器與H∞魯棒補(bǔ)償跟蹤控制器結(jié)合設(shè)計了間接自適應(yīng)控制器; Londhe[8]將單輸入模糊控制器應(yīng)用于UVMS的軌跡跟蹤問題; Wang[9]采用離散延遲估計來處理未知的水動力干擾, 并針對UVMS 的閉環(huán)系統(tǒng)進(jìn)行了分析。Huang[10]將運(yùn)動學(xué)和動力學(xué)控制器相結(jié)合, 設(shè)計了基于動態(tài)模型的動力學(xué)控制器相結(jié)合, 設(shè)計了基于動態(tài)模型的自適應(yīng)控制器, 提高了UVMS 操縱軌跡的精確性。這些方法都能達(dá)到較好的控制效果, 但是需要模型精確已知或者模型過于復(fù)雜。

圖1 UVMS 的動態(tài)模型示意圖Fig. 1 Schematic of the dynamic model of UVMS

韓京清[11]借鑒PID 控制不依賴于模型的優(yōu)點(diǎn),提出了自抗擾控制技術(shù)(Active Disturbance Rejection Control, ADRC)。其核心思想是將系統(tǒng)的內(nèi)部不確定性與外部擾動集總為“廣義總擾動”, 引入了非線性擴(kuò)張狀態(tài)觀測器(Extended State Observer, ESO)對其進(jìn)行估計; 并且在外部信號輸入端加入跟蹤微分器(Tracker Differential, TD)對輸入信號安排過渡過程,使得自抗擾控制具有快速的響應(yīng)過程和良好的抗干擾能力[12-14]。但是其參數(shù)難以整定, 且非線性函數(shù)難以工業(yè)化的難點(diǎn)在很大程度上限制其工業(yè)應(yīng)用。為了解決以上難點(diǎn), Gao 等[15]用基于帶寬的思想提出了線性自抗擾控制器, 采用PD 控制器替換原來的非線性誤差反饋, 并將PD 控制器和線性擴(kuò)張狀態(tài)觀測器(Linear Extended State Observer, LESO)的參數(shù)與其帶寬聯(lián)系起來, 提出了結(jié)構(gòu)相對精簡的線性自抗擾控制器(Linear Active Disturbance Rejection Controller, LADRC), 該控制器極大減少了需要調(diào)節(jié)的參數(shù)。因此, LADRC 技術(shù)為具有多輸入多輸出的AUVMS 高精度控制設(shè)計帶來了可能。

本文針對UVMS, 將復(fù)雜系統(tǒng)模型轉(zhuǎn)變?yōu)楹唵蔚姆e分串聯(lián)系統(tǒng), 利用LESO 可對包括建模誤差、耦合、內(nèi)外部干擾、海洋多源干擾在內(nèi)的集總干擾進(jìn)行估計并補(bǔ)償, 引入滑?？刂破?Sliding Mode Control, SMC)增加系統(tǒng)的魯棒性, 建立了滑模自抗擾控制器(Sliding Mode Active Disturbance Rejection Control, SM-ADRC), 考慮用簡單的控制結(jié)構(gòu)解決UVMS 的復(fù)雜控制問題, 最后仿真結(jié)果證明了該方法的有效性。首先建立了UVMS 的動力學(xué)模型, 將UVMS 分成6+n個子系統(tǒng), 針對每一個子系統(tǒng), 將內(nèi)部不確定性、外部干擾和其他子系統(tǒng)的耦合影響集總為總擾動, 設(shè)計LESO 進(jìn)行估計, 再利用估計誤差建立滑模面, 給出UVMS 的SM-ADRC 設(shè)計方法,并進(jìn)行 Lyapunov 穩(wěn)定性分析。在此基礎(chǔ)上得到UVMS 的SM-ADRC 設(shè)計方法, 并給出仿真結(jié)果, 與傳統(tǒng)LADRC 與SMC 的控制結(jié)果進(jìn)行對比分析。

1 UVMS 動力學(xué)分析

1.1 UVMS 的動力學(xué)模型

作者將整個UVMS 系統(tǒng)視為剛體, 根據(jù)Fossen[2]的建模方法建立如下UVMS 動力學(xué)模型:

式中向量q= [X,Y,Z,φ,θ,Ψ,θ1,θ2, …,θn]T由船體的位置[X,Y,Z]T, 航向角[φ,θ,Ψ]T, 機(jī)械臂關(guān)節(jié)角度向量 [θ1,θ2, …,θn]T(n為機(jī)械臂關(guān)節(jié)個數(shù), 本文選取n=3), 因此系統(tǒng)總自由度為水下機(jī)器人本體的6 個自由度和機(jī)械臂關(guān)節(jié)的自由度之和, 即6+n。斜對稱正定矩陣M(q)是包括附加質(zhì)量的系統(tǒng)質(zhì)量矩陣, 為,C(q,)表示包括附加質(zhì)量的科氏力和向心力部分[15],D(q,)表示水動力阻尼部分,G(q)表示船體的浮力與重力之間的總和作用,F(q,)表示系統(tǒng)中水下機(jī)器人與機(jī)械臂相對運(yùn)動間的耦合影響,τc是整個系統(tǒng)的控制輸入,τd為難以測量的未知擾動。根據(jù)UVMS的固有特征, 系統(tǒng)(1)中的系數(shù)矩陣有以下特性[16-19]:

式中f為建模誤差, 參數(shù)不確定性, 水動力阻尼影響和未知干擾等的總和, 即:

定義系統(tǒng)維數(shù)為m= 6+n, 因此系統(tǒng)模型(4)可展開為:

其中系數(shù)矩陣[aii]m×m(q)和[cii]m×m(q,q˙ ),y=q為系統(tǒng)輸出向量,u=τc為系統(tǒng)控制輸入向量,f為總擾動向量。

圖1 給出UVMS 的動態(tài)模型示意圖, 定義圖中坐標(biāo)軸Ie(X,Y,Z)為慣性坐標(biāo)系,Ib(xb,yb,zb)為機(jī)身坐標(biāo)系,Ii(xi,yi,zi),i=1, 2, …,n為機(jī)械臂關(guān)節(jié)自身坐標(biāo)系, 其中n為機(jī)械臂關(guān)節(jié)個數(shù), 本文只考慮機(jī)械臂關(guān)節(jié)繞Z軸旋轉(zhuǎn)。

1.2 水動力參數(shù)分析

假設(shè)UVMS 的構(gòu)成為圓柱體, 則其在水下所受的黏性曳力動力學(xué)模型為[20]:

式中vr為被控對象與洋流的相對速度,ρ為水密度(海水密度為1.02～1.07),Ds為線性表面摩擦系數(shù),Dsvr即為線性表面摩擦力。A(α)為被示性曲面前部的投影橫截面積,α為沖角。ο ()為階數(shù)大于等于三階的高階相, 通常可忽略。Cd(Rn,α)為曳引系數(shù), 由雷諾數(shù)Rn決定。而雷諾系數(shù)可以由以下公式求得:

式中d是垂直于流體速度vb方向的物體的特征長度,μ為流體黏性系數(shù)。

升力垂直于流動方向。對于水下運(yùn)動物體, 它們可以被建模為[20]:

式中Cl為升力系數(shù), 由雷諾系數(shù)與沖角決定。表1給出了圓柱體雷諾數(shù)函數(shù)的升力系數(shù)與曳力系數(shù)。

表1 UVMS 的升曳系數(shù)Tab. 1 Drag force and lift force of the UVMS

當(dāng)剛體在重力作用下浸入流體中時, 必須考慮兩個主要的力: 重力和浮力。后者是唯一的流體靜力學(xué)效應(yīng), 它不是機(jī)身和液體之間相對運(yùn)動的函數(shù)。

定義如下向量:

為重力加速度向量,V為機(jī)身的體積,m為質(zhì)量。則物體浮重為W=, 所受浮力為B=。作用在機(jī)身的重心=[xG,yG,zG]的重力在機(jī)身坐標(biāo)系可表示為:

式中Rt為轉(zhuǎn)移矩陣, 作用于浮心=[xB,yB,zB]的浮力在機(jī)身坐標(biāo)系可表示為:

因此在機(jī)身坐標(biāo)系內(nèi)的重力和浮力作用下的力/力矩矢量可表示為:

由于假設(shè)重力作用在剛體上是一致的, 所以質(zhì)心和重心是重合的。

用歐拉角可以將gRB表示為:

式中s(.)和c(.)表示正弦與余弦函數(shù)。

當(dāng)一個剛體在流體中運(yùn)動時, 必須考慮圍繞著它的流體的附加慣性, 也就是由于流體的運(yùn)動而加速的慣性。這種效應(yīng)在工業(yè)機(jī)器人中可以忽略, 因?yàn)榭諝獾拿芏缺纫苿訖C(jī)械系統(tǒng)的密度輕得多。然而在水下時水下機(jī)器人的密度相比, 水的密度ρ≈1 000 kg/m3。在0 度時凈水密度為1 002.68 kg/m3; 含鹽量為3.5%的海水密度為1 028.48 kg/m3。

當(dāng)物體具有一定速度地通過流體時, 在物體的支撐下, 周圍的流體也會加速。因此, 會產(chǎn)生方向相反的力施加在物體上, 即附加質(zhì)量力, 它可以由為一個6×6 的矩陣表示。由于附加質(zhì)量是物體表面幾何形狀的函數(shù), 所以對于剛體的6×6 慣性矩陣, 其性質(zhì)各不相同。例如慣性矩陣不一定是正定的。

以沿xb方向的線性加速度所產(chǎn)生的水動力為例,定義:

同樣地, 可定義所有與6 個力/力矩分量[X,Y, Z,K,M,N]T和6 個線性/角加速度相關(guān)的其余元素, 這些元素可以被分組到附加的質(zhì)量矩陣MA中。對于完全浸入水中的物體則有MA>0, 并且對于理想液體(無雜物), 物體移速較慢且無海浪或洋流時有[21]:

附加質(zhì)量也增加了科氏力和向心力, 可以用矩陣表示為:

物體完全浸入水中, 運(yùn)動速度較低, 且具有水下航行器常見的3 個對稱面, 則可以考慮以下矩陣MA和CA的結(jié)構(gòu):

其中[u,v,w]與[p,q,r]分別對應(yīng)船體的位置[X,Y,Z]與航向角[φ,θ,Ψ]方向上的速度。

對于質(zhì)量為m, 長度為L, 截面半徑為r的圓柱形剛體, 其附加質(zhì)量系數(shù)可選取為:

在[20]中可以找到關(guān)于圓柱在流體中運(yùn)動的附加質(zhì)量效應(yīng)的詳細(xì)的理論和實(shí)驗(yàn)討論, 它表明附加質(zhì)量矩陣是狀態(tài)相關(guān)的, 它的系數(shù)是圓柱運(yùn)動距離的函數(shù)。

流體的黏度也會對物體產(chǎn)生耗散阻力和升力,即為式(1)中的D（q,q˙）q˙ 項(xiàng)。一種常見的簡化方法是只考慮線性和二次阻尼項(xiàng), 并將這些項(xiàng)組合成一個矩陣D（q,q˙ ）, 并有:

這個矩陣的系數(shù)也被認(rèn)為是常數(shù)。對于完全浸入水中的物體, 可以做以下進(jìn)一步的假設(shè)

需要注意的是當(dāng)假定阻尼矩陣為對角結(jié)構(gòu)時就意味著忽略耦合耗散項(xiàng)。

2 滑模自抗擾控制器

系統(tǒng)(7)是一個高維不確定系統(tǒng)。為了方便進(jìn)行控制設(shè)計, 根據(jù)其具體維數(shù)將UVMS 分成m個子系統(tǒng), 針對每一個子系統(tǒng), 將建模誤差, 外部干擾和其他子系統(tǒng)的耦合影響等集總為總擾動, 設(shè)計LESO進(jìn)行估計, 再利用估計誤差建立滑模面, 構(gòu)造滑?？刂破?。在此基礎(chǔ)上得到針對整個 UVMS 的SM-ADRC 設(shè)計方法。

2.1 線性擴(kuò)張觀測器

以系統(tǒng)(7)中的第一個子系統(tǒng)為例:

式中為ξ1子系統(tǒng)(25)的廣義總擾動, 包含了內(nèi)部的不確定性和外部未知干擾, 假設(shè)其未知但有界。則系統(tǒng)(26)可重新寫作:

現(xiàn)將總擾動ξ1擴(kuò)展為系統(tǒng)的狀態(tài)變量x3= ξ1,同時假設(shè)其微分有界, 并定義=w, 則可以列出所對應(yīng)的二階系統(tǒng)的狀態(tài)空間表達(dá)式:

基于狀態(tài)方程(29)可以建立如下觀測器:

式中l(wèi)1,l2,l3為觀測器增益, 加入擴(kuò)張狀態(tài)量的觀測器(31)稱為LESO。若觀測器增益選為:

可使得s3+β1s2+β2s+β3滿足Hurwitz 條件。而且可以通過改變β來調(diào)整特征根的位置[13], 如將其配置于s3+ β1s2+ β2s+ β3= (s+ ω1o)3, 則有:

式中ω1o為第一個子系統(tǒng)的LESO 的觀測器帶寬。

2.2 LESO 收斂性及估計誤差分析

將(31)中觀測器增益取為(33)中對應(yīng)值, 在做拉氏變換, 可以求得狀態(tài)變量的估計值z1,z2,z3的傳遞函數(shù):

定義觀測器跟蹤誤差為:

因此可以解出誤差的傳遞函數(shù):

一般性地將式(37)中輸入輸出Y1(s),U1(s)取為幅值為K的階躍信號即Y1(s)=K/s,U1(s)=K/s來對系統(tǒng)的穩(wěn)態(tài)進(jìn)行分析, 則可求得穩(wěn)態(tài)誤差為:

再考慮LESO 的估計誤差的動態(tài)特性。由(30)和(31)可得LESO 對系統(tǒng)(9)的跟蹤誤差方程:

假設(shè)作用于此系統(tǒng)的擾動有界, 定義式(39)的狀態(tài)轉(zhuǎn)移為[22]:

其中，e= [e1,e2,e3]T,η= [η1,η2,η3]T,Λ= diag[,1]T。將(41)代入(39)可得:

通過2.1 節(jié)的配置已使得Ao的特征值均具有負(fù)實(shí)部:

因此由式(42)不難得出當(dāng)ω1o增大時總擾動項(xiàng)ξ1的影響會隨之減小, 且LESO 的跟蹤誤差逐漸減小。

2.3 子系統(tǒng)的滑模變結(jié)構(gòu)控制器

引理1[23]對于V:[0,∞)∈R不等式:

同樣以UVMS 的第一個子系統(tǒng)為例, 并基于上節(jié)所述的總擾動項(xiàng)有:

定義系統(tǒng)期望輸出為y1d, 且假設(shè)y1d有界。LESO 估計誤差為:-y1d, 定義滑模面為:

式中c>0。

基于LESO(31)可建立如下滑?？刂破?

針對系統(tǒng)(47), 選擇自抗擾滑?？刂破?48)、(49)中的參數(shù)k>1/2, 則其閉環(huán)系統(tǒng)的跟蹤誤差將收斂至某鄰域, 且其收斂速度與LESO 的參數(shù)ωo和k有關(guān)。

證明:

引入Lyapunov 函數(shù):

由引理1 可解得:

由于V(t) =s2>0, 所以當(dāng)t→∞有:

可以看到當(dāng)LESO 對擾動的跟蹤誤差逼近于0,k足夠大時:

因此當(dāng)k>1/2 時跟蹤誤差e? 逐漸收斂到鄰域D= ｛≤ φ｝且其收斂速度取決于LESO 增益ωo和滑?？刂破髟鲆鎘。

由以上可知系統(tǒng)的穩(wěn)定性與收斂速度由滑模參數(shù)k和觀測器參數(shù)ωo決定。其余子系統(tǒng)的控制器及其穩(wěn)定性可由同樣的方法建立與驗(yàn)證。

2.4 微分跟蹤器設(shè)計

考慮當(dāng)輸入信號為非光滑信號時, 避免出現(xiàn)數(shù)字計算中的高頻振蕩, 和為狀態(tài)變量的提取安排過渡過程, 采用積分鏈?zhǔn)轿⒎制魈崛≥斎胄盘柕奈⒎?通過每層的積分環(huán)節(jié)對擾動進(jìn)行抑制。該有限時間收斂二階階微分器(TD)為:

式中v0(t)為輸入信號, 狀態(tài)量x1在加速度的限制≤r之下, 能快速的跟蹤輸入信號v0(t), 且r越大,其跟蹤速度越快, 同樣當(dāng)x1充分的接近v0(t)時, 另一狀態(tài)量x2(t) =x˙1(t)可作為輸入信號的近似微分。

2.5 UVMS 的SM-ADRC 設(shè)計

綜上, 完整的滑模自抗擾控制結(jié)構(gòu)圖由圖2 給出,圖中yd1與yd2分別為參考輸入的跟蹤信號與其微分信號。其他8 個子系統(tǒng)可以以同樣的方法建立控制器, 針對AUVMS 的完整SM-ADRC 的結(jié)構(gòu)圖如圖3 所示。

圖2 SM-ADRC 結(jié)構(gòu)圖Fig. 2 Structure of SM-ADRC

圖3 UVMS 的SM-ADRC 結(jié)構(gòu)圖Fig. 3 Frame of SM-ADRC for UVMS

3 仿真結(jié)果與分析

基于UVMS 的工作特點(diǎn), 在本節(jié)對帶有兩連桿(連接關(guān)節(jié)n=3)的UVMS 的航行軌跡, 機(jī)械臂旋轉(zhuǎn)軌跡進(jìn)行跟蹤實(shí)驗(yàn), 測驗(yàn)控制器對UVMS 的航向角切換頻率要求。并且將實(shí)驗(yàn)結(jié)果與傳統(tǒng)滑?？刂? 自抗擾控制進(jìn)行對比。

在仿真實(shí)驗(yàn)中, 9 個子系統(tǒng)的LESO 的參數(shù)分別為ωo=[150, 80, 135, 420, 556, 894, 240, 779, 368]。微分跟蹤器參數(shù)r均在集合[20, 30]取值 , 滑模控制器中參數(shù)c在集合[2, 3]中取值, 參數(shù)k=[45, 36, 22, 44,59, 71, 46, 59, 88] 對應(yīng)每個子系統(tǒng)。定義UVMS 的初始位置為原點(diǎn), 即[X, Y, Z] =[0, 0, 0]。為了驗(yàn)證控制器對位置的快速變化的跟蹤能力, 將UVMS 的機(jī)械臂關(guān)節(jié)角度在0 到5 秒內(nèi)期望角度變化軌跡設(shè)定為:

并同時考慮UVMS 的位置快速變化時的跟蹤效果和航向角變化狀態(tài)。為了使仿真效果盡可能的接近水下環(huán)境, 作者將外部干擾設(shè)計為一階高斯-馬爾科夫模型來模擬洋流的運(yùn)動學(xué)特征。其表達(dá)式如下:

式中vc為設(shè)定的洋流速度,αc和βc為洋流的運(yùn)動方向,εc、εα和εβ為較小的正實(shí)數(shù),ωv、ωα和ωβ分別為不同頻率的高斯白噪聲。

在圖4 和圖5 中可以看到, 滑模自抗擾控制器可以很好地避免傳統(tǒng)滑?？刂频亩墩瘳F(xiàn)象, 實(shí)現(xiàn)更精確的跟蹤軌跡, 并且在位置陡然變化時能較傳統(tǒng)線性自抗擾控制器和滑模控制器更快的跟蹤位置變化,有更平穩(wěn)快速的響應(yīng)過程, 圖5 中[Ex,Ey,Ez]分別指代對UVMS 的位置[X,Y,Z]的跟蹤誤差。在圖6 中可以看到3 個控制器都能跟蹤機(jī)械臂的擺動角度軌跡進(jìn)行但在圖7 中, [Eθ1,Eθ2,Eθ3]分別對應(yīng)在各個控制器的控制下關(guān)節(jié)角的跟蹤誤差, 從中可以看出, 機(jī)械臂關(guān)節(jié)角度軌跡跟蹤結(jié)果顯示滑模自抗擾控制器能更快更精準(zhǔn)地跟蹤機(jī)械臂擺動的軌跡, 在不同工況(機(jī)械臂不同的擺動頻率)下均能達(dá)到非常良好的軌跡跟蹤效果, 跟蹤誤差小于LADRC 與SMC, 顯示了SM-ADRC 優(yōu)良的魯棒性。為了公平地驗(yàn)證所設(shè)計的控制的控制效果, 在仿真中3 個控制器的所產(chǎn)生的控制輸入在圖8 給出, 控制輸入的消耗在一定程度上可以反映控制對能量的消耗, 如電機(jī)的耗電量。在圖8 中可以看到與傳統(tǒng)滑?？刂婆cLADRC 相比, 要達(dá)到圖 4～圖7 的控制效果, 文中所設(shè)計的SM-ADRC所許需的控制輸入量是最少的, 這說明了SM-ADRC 可以依靠更小的能量消耗而達(dá)到更好的控制效果, 具有更高的工業(yè)化的能力。

圖4 UVMS 位置變化軌跡Fig. 4 Trajectory of UVMS’s position

圖5 UVMS 位置跟蹤誤差Fig. 5 Error in the UVMS’s position tracking

圖6 機(jī)械臂關(guān)節(jié)角度軌跡跟蹤結(jié)果Fig. 6 Angular trajectory tracking of manipulators

圖7 UVMS 機(jī)械臂期望關(guān)節(jié)角軌跡的跟蹤誤差Fig. 7 Error in tracking the UVMS manipulator angular Trajectory

圖8 不同控制器的控制輸出對比Fig. 8 Comparison of the control outputs of three controllers

4 結(jié)論

作者提出的滑模自抗擾控制器具有良好的響應(yīng)過程, 并通過線性擴(kuò)張觀測器對擾動的實(shí)時估計,很大程度上消除了傳統(tǒng)滑?？刂破鞯亩墩瘳F(xiàn)象。通過滑?？刂破鞔?zhèn)鹘y(tǒng)線性自抗擾控制器中的PD控制器, 在系統(tǒng)輸入信號快速切換時能保持整個系統(tǒng)的穩(wěn)定, 并且控制過程中的超調(diào)現(xiàn)象大幅度減小,此外, 作者所設(shè)計的控制器與傳統(tǒng)滑模控制器和線性自抗擾控制器相比能用更少的控制量達(dá)到更好的控制效果, 這在一定程度上保證了所設(shè)計的控制器的可進(jìn)行實(shí)際工業(yè)化。最后通過仿真實(shí)驗(yàn)驗(yàn)證了提出的控制器有效性。