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

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

0 引言

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

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

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

1 系統(tǒng)描述

1.1 結(jié)構(gòu)設(shè)計(jì)

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

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

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

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

1.2 動(dòng)力學(xué)模型

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

(1)

(2)

(3)

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

Im——電機(jī)慣性

Dm——電機(jī)阻尼

τ——電機(jī)輸出力矩

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

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

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

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

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

G(q)——重力項(xiàng)τd——擾動(dòng)項(xiàng)

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

(4)

(5)

式中H——包含未建模特性及外部擾動(dòng)集總干擾

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

2.1 LESO設(shè)計(jì)

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

(6)

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

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

(7)

其中

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

(8)

其中

設(shè)計(jì)式(8)的線(xiàn)性擴(kuò)張狀態(tài)觀測(cè)器為

(9)

L——觀測(cè)器增益

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

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

(10)

2.2 FNTSMC設(shè)計(jì)

(11)

對(duì)式(11)求關(guān)于時(shí)間的導(dǎo)數(shù)，可得

(12)

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

(13)

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

(14)

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

進(jìn)而，可推導(dǎo)出基于趨近律的切換控制力矩為

(15)

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

τ1=τ1_eq+τ1_sw

(16)

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

τ2=τ2_eq+τ2_sw

(17)

3 穩(wěn)定性分析

3.1 觀測(cè)器收斂性證明

定義線(xiàn)性擴(kuò)張狀態(tài)觀測(cè)器的估計(jì)誤差為

(18)

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

(19)

其中

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

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

(20)

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

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

(21)

當(dāng)t>0時(shí)，式(21)的m∞范數(shù)滿(mǎn)足

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

(22)

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

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

(23)

很明顯，根據(jù)假設(shè)1，觀測(cè)器的估計(jì)誤差穩(wěn)定。進(jì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é)角速度跟蹤誤差能在有限時(shí)間內(nèi)收斂，即

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

(26)

(27)

其中

Δ1=|3-H1|/k11

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

選擇一個(gè)標(biāo)準(zhǔn)的李雅普諾夫函數(shù)

(28)

對(duì)式(28)求關(guān)于時(shí)間的導(dǎo)數(shù)可得

(29)

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

(30)

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

(31)

(32)

定理2[21]：當(dāng)李雅普諾夫函數(shù)V1滿(mǎn)足

(33)

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

(34)

其中V0是V1的初始條件。

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

(35)

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

3.3 跟蹤誤差收斂性證明

將式(11)改寫(xiě)成

(36)

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

(37)

4 仿真算例

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

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

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

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

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

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

圖5 繩驅(qū)動(dòng)空中機(jī)械臂的可視化運(yùn)動(dòng)Fig.5 Visual motion of cable-driven aerial manipulator

5 試驗(yàn)驗(yàn)證

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

圖6 地面測(cè)試平臺(tái)Fig.6 Ground test platform1.目標(biāo)機(jī) 2.宿主機(jī) 3.吊艙 4.繩驅(qū)動(dòng)機(jī)械臂 5.水源 6.水樣

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

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

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

6 結(jié)論

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

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

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