面向水質(zhì)采樣的繩驅(qū)動空中機械臂抗干擾控制

丁 力 姚 勇 巢 淵 王堯堯 吳洪濤

(1.江蘇理工學院機械工程學院， 常州 213001; 2.南京航空航天大學機電學院， 南京 210016)

0 引言

水質(zhì)采樣是了解水環(huán)境的途徑，也是水樣調(diào)查和監(jiān)控的關(guān)鍵[1]。當前水質(zhì)采樣主要依賴人工，作業(yè)效率不高且費時耗力。雖然也存在一些機械采樣設備，但大都只能采集從排污口排入到湖泊或河流中的水樣，由于污水被稀釋，故樣本數(shù)據(jù)的可靠性和精確性無法保障。繩驅(qū)動空中機械臂可在三維空間中自由飛行，具有工作空間大、靈活性高、結(jié)構(gòu)輕巧的特點[2-3]，可以有效解決排污管口水質(zhì)采樣問題。然而，繩驅(qū)動空中機械臂是一個典型的非線性系統(tǒng)，表現(xiàn)出極其復雜的動力學特性，很難保證其作業(yè)時的位姿穩(wěn)定性。同時，驅(qū)動繩索的柔性變形以及外界干擾又會進一步加劇系統(tǒng)的非線性因素。

近年來，國內(nèi)外學者們對空中機械臂控制技術(shù)進行了研究，主要控制器有積分反步法控制[4]、神經(jīng)網(wǎng)絡模糊控制[5]、H∞魯棒控制[6]、滑?？刂?Sliding mode control，SMC)[7]等。其中，SMC因其魯棒性好、結(jié)構(gòu)簡單、對參數(shù)不敏感等優(yōu)點，廣泛應用于機電系統(tǒng)控制。由于SMC結(jié)構(gòu)中含有切換函數(shù)，會產(chǎn)生抖振現(xiàn)象。為此，MA等[8]在滑模面中加入高階非線性函數(shù)，提出了終端滑?？刂?Terminal SMC，TSMC)，有效削弱了抖振，但也帶來不連續(xù)和奇異性問題。進而，YI等[9]提出了快速連續(xù)非奇異終端滑模控制策略(Fast nonsingular TSMC，F(xiàn)NTSMC)，解決了奇異性問題，加快了系統(tǒng)狀態(tài)量的收斂速度。文獻[10-11]采用此類控制器解決繩驅(qū)動工業(yè)機械臂和壓電定位系統(tǒng)的控制問題。另外，繩驅(qū)動機械臂存在的由內(nèi)部不確定性和外部擾動構(gòu)成的集總干擾往往會影響關(guān)節(jié)角的穩(wěn)態(tài)性能，從而降低系統(tǒng)整體的控制品質(zhì)。文獻[12-13]研究表明，擾動觀測器能有效估計與補償集總干擾，提高系統(tǒng)的抗干擾能力。在擾動觀測器中，線性擴張狀態(tài)觀測器(Linear extended state observer，LESO)具有能耗小、易于工程實現(xiàn)的特點，被成功嵌入到反步法控制[14]、自適應控制[15]、PD(Proportion differentiation，PD)控制[16]等結(jié)構(gòu)中。

本文擬結(jié)合LESO和FNTSMC的優(yōu)勢，提出一種新型抗干擾控制策略以解決繩驅(qū)動空中機械臂的關(guān)節(jié)空間軌跡跟蹤控制問題，旨在提高其在排污管口水質(zhì)采樣時的操控品質(zhì)。設計機械臂的三維樣機，闡述機械臂的工作原理，推導機械臂的動力學模型，明確系統(tǒng)輸入輸出關(guān)系，進而根據(jù)不基于模型的思想設計各關(guān)節(jié)的抗干擾控制器，實現(xiàn)繩驅(qū)動機械臂的汲水動作。最后，通過可視化仿真和地面試驗對本文所提控制器的有效性進行驗證。

1 系統(tǒng)描述

1.1 結(jié)構(gòu)設計

本文設計的面向水質(zhì)采樣的繩驅(qū)動空中機械臂虛擬樣機如圖1所示，主要由四旋翼飛行器、二自由度繩驅(qū)動機械臂及吊艙組成。其中，四旋翼飛行器上裝有飛行控制器、電機、槳葉及電調(diào)等航空電子設備，吊艙用于放置鋰電池、水泵、水箱與驅(qū)動電機。所設計樣機的工作原理為：先控制四旋翼飛行器懸停至排污管口附近，再操控繩驅(qū)動機械臂將末端執(zhí)行器伸入管道口內(nèi)汲水采樣。需要指出的是，本文工作僅研究繩驅(qū)動機械臂的運動控制，而不涉及旋翼飛行器的動態(tài)性能。

圖1 水質(zhì)采樣空中機械臂樣機Fig.1 Prototype of aerial manipulator for water samples

與現(xiàn)有空中機械臂相比，本文設計的機械臂采用繩驅(qū)動技術(shù)，如圖2所示，將直流減速電機放置在基座處，下方電機通過驅(qū)動輪上纏繞的兩根繩索拉動大臂關(guān)節(jié)1正反轉(zhuǎn)，上方電機通過主動輪、導向輪及張緊輪將力矩傳遞到小臂關(guān)節(jié)2處，最終實現(xiàn)末端執(zhí)行器的汲水運動。這種設計可有效減輕臂身質(zhì)量和關(guān)節(jié)慣量，提高末端負載自重比，同時兼具結(jié)構(gòu)輕巧、減振吸振的優(yōu)點。

圖2 繩驅(qū)動機械臂三維模型Fig.2 3D model of cable-driven manipulator1.直流減速電機 2.主動輪 3.大臂 4.導向輪 5.吸水管 6.小臂 7.編碼器 8.關(guān)節(jié)輪2 9.末端執(zhí)行器 10.關(guān)節(jié)2 11.張緊輪 12.驅(qū)動繩索 13.關(guān)節(jié)輪1 14.關(guān)節(jié)1

1.2 動力學模型

在繩驅(qū)動空中機械臂中，電機到關(guān)節(jié)的動力靠柔繩來傳遞，故可將柔繩的柔性效應集中到關(guān)節(jié)處，即等效成柔性關(guān)節(jié)。根據(jù)課題組前期研究[17]，計及關(guān)節(jié)柔性的n(n=2)自由度繩驅(qū)動機械臂的動力學模型可描述為

(1)

(2)

(3)

式中θ——電機轉(zhuǎn)子角位移

Im——電機慣性

Dm——電機阻尼

τ——電機輸出力矩

q——機械臂關(guān)節(jié)角位移

τs——機械臂關(guān)節(jié)輸出力矩

Ks——機械臂關(guān)節(jié)剛度

Ds——機械臂關(guān)節(jié)阻尼

M(q)——關(guān)節(jié)慣性

G(q)——重力項τd——擾動項

將式(3)代入式(1)中，可得

(4)

(5)

式中H——包含未建模特性及外部擾動集總干擾

2 抗干擾控制器設計

2.1 LESO設計

本文控制器設計只依賴被控對象輸入、輸出數(shù)據(jù)。式(5)實際上是一個二階系統(tǒng)，故以關(guān)節(jié)角q1來推演本文控制器的設計過程，即

(6)

式中y1——系統(tǒng)輸出

將式(6)改寫成狀態(tài)空間方程

(7)

其中

引入一個擴張狀態(tài)量x3來表征集總干擾，則式(7)轉(zhuǎn)換為

(8)

其中

設計式(8)的線性擴張狀態(tài)觀測器為

(9)

L——觀測器增益

ω0——觀測器帶寬——系統(tǒng)輸出

ξi(i=1,2,3)為λ(s)=s3+ζ1s2+ζ2s+ζ3特征根[18]，滿足

(10)

2.2 FNTSMC設計

(11)

對式(11)求關(guān)于時間的導數(shù)，可得

(12)

將式(6)代入式(12)可得系統(tǒng)等效控制力矩為

(13)

同時，為了保證系統(tǒng)狀態(tài)能在有限時間內(nèi)收斂并抑制抖振，選取快速終端滑模趨近律為

(14)

式中k11、k12——控制器參數(shù)

進而，可推導出基于趨近律的切換控制力矩為

(15)

聯(lián)立式(14)、(15)可得關(guān)節(jié)角q1的抗干擾控制器為

τ1=τ1_eq+τ1_sw

(16)

同理，設計關(guān)節(jié)角q2的抗干擾控制器為

τ2=τ2_eq+τ2_sw

(17)

3 穩(wěn)定性分析

3.1 觀測器收斂性證明

定義線性擴張狀態(tài)觀測器的估計誤差為

(18)

將式(9)代入到式(7)中，可得

(19)

其中

根據(jù)式(19)可知，Ae具有3個不同的特征根，故必然存在一個正定矩陣T，使得

Ae=Tdiag(-λ1,-λ2,-λ3)T-1

(20)

將式(20)寫成指數(shù)形式，有

eAet=Tdiag(e-λ1t,e-λ2t,e-λ3t)T-1

(21)

當t>0時，式(21)的m∞范數(shù)滿足

‖eAet‖m∞≤Ωhbe-λ1t

(22)

式中Ω——權(quán)重函數(shù)

則式(19)中估計誤差的解為

(23)

很明顯，根據(jù)假設1，觀測器的估計誤差穩(wěn)定。進而，根據(jù)m∞范數(shù)的性質(zhì)可得

(24)

(25)

3.2 滑模面收斂性證明

定理1[20]：以關(guān)節(jié)1為例，在式(16)的作用下，二階系統(tǒng)式(6)的關(guān)節(jié)角跟蹤誤差及關(guān)節(jié)角速度跟蹤誤差能在有限時間內(nèi)收斂，即

|e1|≤2Δ=2min(Δ1,Δ2)

(26)

(27)

其中

Δ1=|3-H1|/k11

Δ2=(|3-H1|/k12)1/p1

選擇一個標準的李雅普諾夫函數(shù)

(28)

對式(28)求關(guān)于時間的導數(shù)可得

(29)

根據(jù)式(11)～(16)，可得

(30)

式(30)具有2種形式，即

(31)

(32)

定理2[21]：當李雅普諾夫函數(shù)V1滿足

(33)

其中，ξ1>0，ρ1>0。此時，穩(wěn)定時間為

(34)

其中V0是V1的初始條件。

結(jié)合定理1、2，可知快速連續(xù)非奇異終端滑模面(式(11))能在有限時間內(nèi)收斂，即

(35)

因此，有‖s1‖≤Δ=min(Δ1,Δ2)，即滑模面有界且收斂。

3.3 跟蹤誤差收斂性證明

將式(11)改寫成

(36)

當‖s1‖≤Δ時，關(guān)節(jié)角速度跟蹤誤差能在有限時間內(nèi)收斂

(37)

4 仿真算例

建立繩驅(qū)動空中機械臂的虛擬樣機，并導入Simscape環(huán)境中進行可視化仿真分析。為測試本文控制器的性能，引入文獻[22]提出的線性自抗擾控制(Linear active disturbance rejection control，LADRC)和文獻[23]提出的TSMC與之比較。選擇2個關(guān)節(jié)角參考軌跡為方波信號，q1與q2的初始條件分別為0°和82.6°(在Simscape中測量而來)，其他條件為0。同時添加均值為0、方差為0.01的高斯噪聲信號作為干擾力矩，仿真時間持續(xù)10 s。3種控制器參數(shù)如表1所示。

表1 3種控制器參數(shù)Tab.1 Control parameters of three controllers

3種控制下的關(guān)節(jié)角響應如圖3所示，可以明顯看出本文控制器的響應速度、跟蹤精度和魯棒性優(yōu)于LADRC和TSMC。圖4為3種控制器的輸出力矩響應，可以看出由于本文控制器和LADRC結(jié)構(gòu)中的LESO能有效估計出集總干擾，故它們的控制力矩相對TSMC較為平坦，同時本文控制力矩也沒有明顯的抖振現(xiàn)象。上述結(jié)果均表明了本文控制器具有較好的瞬態(tài)性能和穩(wěn)態(tài)性能，同時也驗證了線性擴張狀態(tài)觀測器與快速連續(xù)非奇異終端滑模控制結(jié)合的合理性與有效性。

圖3 仿真中關(guān)節(jié)角位移響應曲線Fig.3 Response of joint position in simulation

圖4 仿真中關(guān)節(jié)力矩響應曲線Fig.4 Response of joint torques in simulation

圖5給出了繩驅(qū)動空中機械臂在Simscape中的可視化運動過程，從圖中可以清晰地觀察兩個關(guān)節(jié)角的變化情況。在實際的水質(zhì)采樣中，先操控旋翼飛行器飛至目標點附近懸停，然后控制機械臂汲水作業(yè)。本文沒有考慮飛行器的動力學特性，而是將其與機械臂的耦合效應當作系統(tǒng)的外部擾動，并用線性擴張狀態(tài)觀測器來補償。

圖5 繩驅(qū)動空中機械臂的可視化運動Fig.5 Visual motion of cable-driven aerial manipulator

5 試驗驗證

搭建的繩驅(qū)動空中機械臂地面測試平臺如圖6所示。試驗過程中，在宿主機的Simulink環(huán)境中設計控制器，編譯后下載至目標機中，目標機根據(jù)PCI6225e板卡采集關(guān)節(jié)位置信號，再由與期望關(guān)節(jié)位置的誤差計算出所需的控制力矩信號，再將力矩信號經(jīng)PCI6225e板卡傳送至吊艙內(nèi)的驅(qū)動電機，實現(xiàn)關(guān)節(jié)空間的閉環(huán)控制。另外，驅(qū)動電機選用RoboMaster M2006型直流減速電機，額定轉(zhuǎn)速為500 r/min，額定力矩為1 N·m；驅(qū)動器型號為RoboMaster C610；關(guān)節(jié)處安裝的編碼器型號為AMT102-V，測量精度為0.044°。引入Cycloidal曲線[24]來設計2個關(guān)節(jié)的參考軌跡，q1的變化為0°～40°(持續(xù)5 s)、40°～0°(持續(xù)5 s)、0°～40°(持續(xù)5 s)；q2的變化為0°～50°(持續(xù)5 s)、50°～0°(持續(xù)5 s)、0°～50°(持續(xù)5 s)；汲水時間持續(xù)5 s；總時間截取20 s；采樣頻率為1 000 Hz。

圖6 地面測試平臺Fig.6 Ground test platform1.目標機 2.宿主機 3.吊艙 4.繩驅(qū)動機械臂 5.水源 6.水樣

將本文控制器與LADRC、TSMC作比較，結(jié)果如圖7～9所示。從圖7可以看出，在3種控制器的作用下，q1與q2都能較好地跟蹤參考軌跡，但很明顯本文控制器的跟蹤精度最高。圖8給出了3種控制器下兩關(guān)節(jié)角的跟蹤誤差，若以絕對最大誤差emax和均方根誤差RMSE作為評價指標[25]，則對于q1，本文控制器的emax分別比LADRC和TSMC減小了20.06%和25.53%，RMSE分別減小了21.03%和35.07%；對于q2，本文控制器的emax分別比LADRC和TSMC減小了14.24%和19.04%，RMSE分別小了59.87%和91.32%。最后，圖9給出3種控制力矩的響應曲線，可以看出本文控制器相對平坦，尤其是在10～15 s汲水期間最為平穩(wěn)；以q2為例，當關(guān)節(jié)角誤差約為1°時(8～10 s)，本文控制器的邊界層厚度增大，使得狀態(tài)量能以更快的趨近速度收斂至滑模面，同時有效削弱抖振現(xiàn)象，而此階段其他兩種控制器的輸出力矩抖動較大，LADRC的控制力矩還發(fā)生了激變(-0.052 N·m)，只能靠激增力矩將系統(tǒng)狀態(tài)量拉回至目標值。因此，本文控制器的性能優(yōu)于LADRC和TSMC。

圖8 3種控制器關(guān)節(jié)角跟蹤誤差曲線Fig.8 Joint tracking errors curves of three controllers

圖9 3種控制器輸出力矩曲線Fig.9 Comparison of joint torques curves of three controllers

6 結(jié)論

(1)設計了水質(zhì)采樣繩驅(qū)動空中機械臂機械結(jié)構(gòu)，并將繩驅(qū)動的柔性效應等效到關(guān)節(jié)處，建立了繩驅(qū)動空中機械臂的動力學模型。仿真結(jié)果也證明了模型的有效性。

(2)可視化仿真展示了繩驅(qū)動空中機械臂的運動過程，表明了本文控制器比LADRC和TSMC具有更好的瞬態(tài)性能和穩(wěn)態(tài)性能。

(3)地面汲水試驗結(jié)果表明本文控制器的emax和RMSE指標都小于LADRC和TSMC，具有較高的跟蹤精度、較快的響應速度和較強抗干擾能力，也能較好地削弱控制抖振。