張振國 陳彥杰 占巍巍 何炳蔚林立雄王耀南

(1.福州大學(xué)機(jī)械工程與自動(dòng)化學(xué)院,福建福州 350108;2.湖南大學(xué)電氣與信息工程學(xué)院,湖南 長沙 410082;3.機(jī)器人視覺感知與控制技術(shù)國家工程實(shí)驗(yàn)室,湖南長沙 410082)

1 引言

近些年來,隨著導(dǎo)航、控制技術(shù)的不斷發(fā)展,飛行機(jī)器人技術(shù)的相關(guān)研究日益成熟,廣泛用于航拍勘測、農(nóng)藥噴灑、目標(biāo)跟蹤等領(lǐng)域[1].隨著機(jī)器人應(yīng)用領(lǐng)域的擴(kuò)展,人們更加期望飛行機(jī)器人有更多的用途,便在飛行機(jī)器人上加上主動(dòng)作業(yè)裝置,稱之為作業(yè)型飛行機(jī)器人系統(tǒng)[2].文獻(xiàn)[3]采用具有7自由度機(jī)械臂的作業(yè)型飛行機(jī)器人,能夠靈活完成抓取和裝配作業(yè).文獻(xiàn)[4-5]在作業(yè)型飛行機(jī)器人系統(tǒng)上增加了視覺伺服控制,可以完成自主的抓取任務(wù).文獻(xiàn)[6]采用并聯(lián)作業(yè)型飛行機(jī)器人系統(tǒng),可以更好的仿生工作.文獻(xiàn)[7]使用作業(yè)型飛行機(jī)器人的機(jī)械臂末端和物體接觸,以代替力傳感器完成接觸力測量工作.

在作業(yè)型飛行機(jī)器人抓取物體任務(wù)過程中,穩(wěn)定的控制系統(tǒng)和末端的高精度控制是整個(gè)抓取過程中的關(guān)鍵部分.文獻(xiàn)[8]采用滑?？刂品椒ㄟM(jìn)行軌跡跟蹤,但是使用容易使系統(tǒng)產(chǎn)生抖振現(xiàn)象.文獻(xiàn)[9]采用混合視覺伺服控制增加自主性并且提高控制精度.針對(duì)由作業(yè)型飛行機(jī)器人產(chǎn)生的重心偏移以及外界擾動(dòng)等現(xiàn)象,文獻(xiàn)[10]采用了一種外加機(jī)構(gòu)來抵消產(chǎn)生的重心偏移,但是外加機(jī)構(gòu)不僅使作業(yè)型飛行機(jī)器人系統(tǒng)更加復(fù)雜,也增加了控制的難度.文獻(xiàn)[11]采用了重心補(bǔ)償控制器,使用做小二乘法估計(jì)抓取物體的質(zhì)量,然后使用二階低通濾波器進(jìn)行濾波,可以實(shí)時(shí)補(bǔ)償重心偏移.但是系統(tǒng)重心坐標(biāo)估計(jì)和狀態(tài)測量時(shí)存在噪聲和時(shí)延,導(dǎo)致估計(jì)效果并不好.文獻(xiàn)[12-14]采用魯棒H∞方法進(jìn)行控制,使用L2增益方法來補(bǔ)償外界擾動(dòng),文獻(xiàn)[15]采用基于區(qū)間矩陣的魯棒控制方法,針對(duì)在無人機(jī)飛行過程中的轉(zhuǎn)動(dòng)慣量變化進(jìn)行控制,但是由于魯棒控制的保守性,只能抓取較輕的物體.

相對(duì)于自由飛行狀態(tài)下的協(xié)調(diào)運(yùn)動(dòng)控制,關(guān)于抓取過程中的運(yùn)動(dòng)控制并不多見.控制方法面臨的主要困難在于如何保證機(jī)械臂與物體接觸時(shí)的系統(tǒng)平穩(wěn)性和安全性問題,文獻(xiàn)[7]采用力位混合控制的方法,使用飛行機(jī)器人和單自由度機(jī)械臂與墻體接觸,通過調(diào)整作業(yè)型飛行機(jī)器人的姿態(tài)來達(dá)到期望力的需求.但是應(yīng)用場景僅存在于靜態(tài)接觸的情況下,對(duì)于動(dòng)態(tài)抓取過程還是會(huì)存在較大的擾動(dòng).文獻(xiàn)[16]采用模型參考自適應(yīng)和增益調(diào)度方法控制機(jī)械臂完成抓取操作,但是其都只是在二維平面下通過控制方法方面去補(bǔ)償由抓取產(chǎn)生的巨大擾動(dòng),并未在動(dòng)力學(xué)建模過程中體現(xiàn)動(dòng)態(tài)抓取的過程.

在作業(yè)型飛行機(jī)器人動(dòng)態(tài)抓取物體的過程中,主要存在的擾動(dòng)為與物體碰撞產(chǎn)生的瞬時(shí)接觸力和物體運(yùn)動(dòng)與地面產(chǎn)生的摩擦力,通過以上描述,并且針對(duì)滑翔動(dòng)態(tài)抓取未知質(zhì)量物體過程中的動(dòng)力學(xué)建模問題和產(chǎn)生的瞬時(shí)擾動(dòng)問題,本文主要采用了以下方法進(jìn)行設(shè)計(jì):

1) 針對(duì)動(dòng)態(tài)滑翔抓取過程中的動(dòng)力學(xué)建模問題,本文基于四旋翼飛行機(jī)器人和二自由度機(jī)械臂,建立包含摩擦力和瞬時(shí)接觸力的動(dòng)力學(xué)模型,提高了建模和抓取的精度.并采取機(jī)械手和飛行機(jī)器人整合式控制策略,能夠完成滑翔抓取作業(yè)任務(wù).

2) 針對(duì)抓取過程中產(chǎn)生的擾動(dòng)以及摩擦力和接觸力補(bǔ)償問題,本文以所建立的作業(yè)型飛行機(jī)器人系統(tǒng)動(dòng)力學(xué)為基礎(chǔ)設(shè)計(jì)了魯棒自適應(yīng)控制器.其中,為消除機(jī)械臂運(yùn)動(dòng)和轉(zhuǎn)動(dòng)變量實(shí)時(shí)變化對(duì)動(dòng)力學(xué)系統(tǒng)產(chǎn)生的微小擾動(dòng),采用基于區(qū)間矩陣的魯棒控制方法.但是考慮到魯棒控制對(duì)于大擾動(dòng)的保守性問題[17],在抓取物體過程中產(chǎn)生的接觸力和摩擦力等較大擾動(dòng),設(shè)計(jì)了自適應(yīng)控制方法.

本文組織結(jié)構(gòu)安排如下:第2部分描述系統(tǒng)對(duì)象,建立滑翔抓取物體時(shí)的動(dòng)力學(xué)模型;第3部分使用魯棒自適應(yīng)方法對(duì)所建立動(dòng)力學(xué)模型進(jìn)行控制,并且使用李雅普諾夫函數(shù)證明其穩(wěn)定性;第4 部分在MATLAB/Simulink下進(jìn)行仿真對(duì)比,仿真結(jié)果表明了控制律的有效性.

2 作業(yè)型飛行機(jī)器人系統(tǒng)與滑翔抓取模型

2.1 系統(tǒng)模型

定義1如圖1所示,系統(tǒng)的坐標(biāo)系建立如下:W:{Ow,Xw,Yw,Zw}表示慣性坐標(biāo)系,是固定在地面上的坐標(biāo)系;B:{OB,XB,YB,ZB}表示機(jī)體坐標(biāo)系,坐標(biāo)系原點(diǎn)在無人機(jī)的幾何中心,ZB軸垂直于飛行平臺(tái)向下;E:{OE,XE,YE,ZE}表示機(jī)械手末端坐標(biāo)系,坐標(biāo)系ZB軸方向與機(jī)械手末端關(guān)節(jié)平行.

圖1 作業(yè)型飛行機(jī)器人結(jié)構(gòu)示意圖Fig.1 Structure of the unmanned aerial manipulator

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

作業(yè)型飛行機(jī)器人系統(tǒng)的動(dòng)力學(xué)模型由飛行平臺(tái)和機(jī)械臂組成,需要考慮二者之間的耦合作用和機(jī)械臂運(yùn)動(dòng)對(duì)飛行平臺(tái)產(chǎn)生的重心偏移影響.本文主要研究的是作業(yè)型飛行機(jī)器人系統(tǒng)滑翔抓取物體時(shí)的狀態(tài),而主要產(chǎn)生的影響是重心偏移和由抓取對(duì)飛行平臺(tái)產(chǎn)生的力.

如圖1是重心不在飛行平臺(tái)幾何中心的示意圖,假設(shè)重心偏移量rG=[xGyGzG]是作業(yè)型飛行機(jī)器人系統(tǒng)的重心在飛行平臺(tái)坐標(biāo)系{B}中的向量,目前動(dòng)力學(xué)建模的主要方法有牛頓-歐拉方程法和拉格朗日動(dòng)力學(xué)方程法,若使用拉格朗日方程法進(jìn)行計(jì)算,需要對(duì)系統(tǒng)各部分的動(dòng)能和勢能進(jìn)行計(jì)算,計(jì)算過程較為復(fù)雜.因此,本文使用牛頓-歐拉方法進(jìn)行動(dòng)力學(xué)建模[18]:

其中:F是運(yùn)動(dòng)產(chǎn)生的外力總和,M是外部轉(zhuǎn)矩,m表示飛行平臺(tái)和機(jī)械臂的總質(zhì)量,ξ=[x y z]T表示機(jī)體坐標(biāo)系{B}的原點(diǎn)OB在慣性坐標(biāo)系{W}中的位置矢量,η=[φ θ ψ]T是飛行平臺(tái)姿態(tài)的方位向量,φ是繞XB軸的翻滾角,θ是繞YB的俯仰角,ψ是繞ZB的偏航角.Ω==[p q r]T為飛行平臺(tái)的角速度矢量,B=m=m[u v w]T.

四旋翼飛行機(jī)器人的1號(hào)電機(jī)和3號(hào)電機(jī)順時(shí)針旋轉(zhuǎn),2號(hào)和4號(hào)電機(jī)逆時(shí)針旋轉(zhuǎn),以此來提供向上的升力和電機(jī)旋轉(zhuǎn)方向相反的作用力,電機(jī)升力fi和反作用力gi的計(jì)算式為

其中:i=1,2,3,4,ωi表示第i個(gè)電機(jī)的電機(jī)轉(zhuǎn)速;b和k都是與空氣密度、阻力系數(shù)等有關(guān)的常數(shù);FB和MB是由作業(yè)型飛行機(jī)器人進(jìn)行抓取時(shí)對(duì)基座產(chǎn)生的力和力矩,可以得出受外力F和外部力矩M為

其中d是電機(jī)力臂長度.

聯(lián)立式(1)-(3)可以得出作業(yè)型飛行機(jī)器人系統(tǒng)的動(dòng)力學(xué)方程為

其中:

如圖1所示的二自由度機(jī)械臂系統(tǒng)傳遞矩陣為[19]

其中:c1=cosq1,c2=cosq2,s1=sinq1,s2=sinq2,qi(i=1,2)為機(jī)械臂第i關(guān)節(jié)的角位移,li(i=1,2)為機(jī)械臂連桿i的有效長度.

二自由度機(jī)械臂系統(tǒng)的動(dòng)力學(xué)方程為

其中:mi(i=1,2)為機(jī)械臂i的質(zhì)量,Ii(i=1,2)為機(jī)械臂i相對(duì)于旋轉(zhuǎn)中心Z軸的轉(zhuǎn)動(dòng)慣量,

2.3 滑翔抓取動(dòng)力學(xué)分析

在作業(yè)型飛行機(jī)器人系統(tǒng)滑翔抓取物體過程中,機(jī)械手主要受到的力為機(jī)械手末端與物體瞬時(shí)接觸時(shí)的接觸力和物體與地面之間的摩擦力,如圖2所示.接觸力由動(dòng)量定理和沖量定理所求[20],設(shè)m0為飛行平臺(tái)質(zhì)量,m3為末端機(jī)械手質(zhì)量.

圖2 抓取物體受力示意圖Fig.2 Schematic diagram of grasping objects

整合式(5)(7),得到如下整體動(dòng)力學(xué)模型:

為作業(yè)型飛行機(jī)器人建模過程中的擾動(dòng)項(xiàng),Fe和Fs分別為末端機(jī)械手瞬時(shí)抓取時(shí)受的接觸力和摩擦力,IRE為傳遞到各個(gè)關(guān)節(jié)的雅可比矩陣.由式(6)計(jì)算得

其中:cφd=cosφd,sφd=sinφd,cθd=cosθd,sθd=sinθd,cψd=cosψd,sψd=sinψd.

虛擬向量由位置環(huán)控制器輸出產(chǎn)生,通過這一向量可以計(jì)算出飛行平臺(tái)總升力、期望翻滾角和期望俯仰角,如下式:

物體瞬時(shí)接觸時(shí)間極短,整個(gè)過程滿足動(dòng)量守恒,對(duì)式(8)在碰撞時(shí)間內(nèi)進(jìn)行積分得

其中:t0為碰撞時(shí)刻,t為碰撞時(shí)間.由于碰撞時(shí)間很短,可以認(rèn)為在這一時(shí)刻僅有廣義速度和廣義加速度發(fā)生突變,因此式(12)可以近似寫為

其中Pe為順時(shí)接觸產(chǎn)生的沖量.同理,接觸過程中物體產(chǎn)生的沖量可以寫為

3 魯棒自適應(yīng)控制器的設(shè)計(jì)

本小節(jié)的主要內(nèi)容是建立誤差動(dòng)態(tài)模型和設(shè)計(jì)內(nèi)外環(huán)魯棒自適應(yīng)控制器.將作業(yè)型飛行機(jī)器人系統(tǒng)解耦為內(nèi)環(huán)角度系統(tǒng)和外環(huán)位置系統(tǒng)計(jì)算期望翻滾角和期望俯仰角,然后計(jì)算系統(tǒng)的誤差動(dòng)態(tài)模型.對(duì)于系統(tǒng)的不確定參數(shù),采用區(qū)間距陣的方法進(jìn)行控制[15].對(duì)于系統(tǒng)產(chǎn)生的非線性擾動(dòng),采用自適應(yīng)的方法進(jìn)行補(bǔ)償[21],所提出的控制系統(tǒng)如圖3所示.

圖3 魯棒自適應(yīng)控制器總體結(jié)構(gòu)圖Fig.3 Structure diagram of the robust adaptive controller

3.1 誤差動(dòng)態(tài)模型

令χd=[xdydzdφdθdψdq1dq2d]T,定義穩(wěn)定的參考模型如下:

定義誤差跟蹤向量:

聯(lián)立式(19)-(21)得

3.2 魯棒自適應(yīng)控制器設(shè)計(jì)

引理1若區(qū)間矩陣A ∈[Am,AM],則A可以描述為[15,22]

ei為n×n維單位矩陣的第i個(gè)列向量;Σ為可變向量,且對(duì)于任意的Σ有ΣTΣ≤1.

引理2設(shè)X和Y為合適維度的實(shí)矩陣,對(duì)于任意給定的正常數(shù),有下列不等式成立[15,23]:

引理3對(duì)于合適的矩陣G ＜0,Z ＜0,XY Z-1YT＜0,可等價(jià)于下列矩陣[15,24]:

定理1針對(duì)誤差動(dòng)態(tài)模型(23),設(shè)計(jì)魯棒自適應(yīng)控制器如下:

4 仿真對(duì)比及分析

為了驗(yàn)證本文提出的魯棒自適應(yīng)控制方法的有效性,以作業(yè)型飛行機(jī)器人作為研究對(duì)象,研究在摩擦力和接觸力的影響下滑翔抓取物體時(shí)的控制跟蹤效果.利用MATLAB/Simulink搭建作業(yè)型飛行機(jī)器人模型,并進(jìn)行軌跡跟蹤仿真,其系統(tǒng)參數(shù)如表1所示.

表1 系統(tǒng)參數(shù)Table 1 System parameters

為了驗(yàn)證本文所提出的魯棒自適應(yīng)控制方法的有效性,設(shè)定在摩擦力和接觸力存在的條件下抓取0.5 kg物體,且設(shè)置接觸碰撞時(shí)間較短,為0.02 s.在模擬過程中考慮轉(zhuǎn)動(dòng)慣量所用時(shí)間不斷變化,同時(shí)考慮了外界的擾動(dòng)對(duì)飛行平臺(tái)產(chǎn)生的影響,仿真結(jié)果如圖4-6所示.

圖4 位置控制效果圖Fig.4 Effect of position control method

圖6 機(jī)械臂控制效果圖Fig.6 Effect of mechanical arm control method

如圖4-6所示,本文設(shè)計(jì)的控制器可以使作業(yè)型飛行機(jī)器人位置和姿態(tài)以及機(jī)械手轉(zhuǎn)速各分量在較小的波動(dòng)下追蹤到目標(biāo)軌跡.之后使作業(yè)型飛行機(jī)器人在較小的穩(wěn)態(tài)誤差下運(yùn)動(dòng).為體現(xiàn)所提出方法在抓取控制方向性能的優(yōu)越性,本文與文獻(xiàn)[12]魯棒控制方法進(jìn)行對(duì)比.由圖7可得,在完全相同的環(huán)境下,所提出方法的輸出力矩的平滑性以及輸出力矩的穩(wěn)定時(shí)間均有明顯的優(yōu)勢,這是因?yàn)樵谧ト≥^大質(zhì)量的物體時(shí),僅有魯棒控制的情況下會(huì)有很大的保守性.而充分利用魯棒控制和自適應(yīng)控制的優(yōu)點(diǎn),在由抓取較大物體產(chǎn)生的擾動(dòng)進(jìn)行自適應(yīng)控制,可以有效地減輕飛行平臺(tái)總拉力的抖動(dòng).

圖7 系統(tǒng)拉力對(duì)比效果圖Fig.7 Effect drawing of system tension comparison

在此基礎(chǔ)上,給予飛行平臺(tái)轉(zhuǎn)動(dòng)慣量一定的擾動(dòng),并在5 s時(shí)加一個(gè)3 N的瞬時(shí)擾動(dòng),比較兩種方法,相應(yīng)的位置跟蹤誤差和角度跟蹤誤差如圖8-9所示.

由圖8-9可以看出,在設(shè)定環(huán)境下兩種方法有效的跟蹤目標(biāo)軌跡,產(chǎn)生較小的誤差.從圖8可以看出在X,Y方向,采用文獻(xiàn)[12]方法的跟蹤誤差小于0.02 m,在Z方向,跟蹤誤差小于0.01 m.本文所提出方法在X,Y方向的跟蹤誤差均小于0.01 m,在Z方向的跟蹤誤差小于0.005 m.這是由于在轉(zhuǎn)動(dòng)慣量變化的情況下本文使用了區(qū)間矩陣控制來進(jìn)行補(bǔ)償控制,從而達(dá)到減小誤差的效果.在5 s 時(shí)加一個(gè)3 N的瞬時(shí)擾動(dòng),所提出方法產(chǎn)生更小的波動(dòng),說明自適應(yīng)控制在對(duì)于較大擾動(dòng)有更好的控制效果.從圖9可以看出,本文所提出方法的角度跟蹤誤差也具有較好的控制效果.

圖8 作業(yè)型飛行機(jī)器人位置誤差Fig.8 Contrast position error of aerial manipulator robot

圖9 作業(yè)型飛行機(jī)器人姿態(tài)誤差Fig.9 Contrast attitude error of aerial manipulator robot

為了進(jìn)一步驗(yàn)證本文提出方法的有效性,在此基礎(chǔ)上針對(duì)作業(yè)型飛行機(jī)器人從自由飛行狀態(tài)到接觸抓取狀態(tài)進(jìn)一步實(shí)驗(yàn),相應(yīng)的在抓取方向上的位置跟蹤誤差如圖10所示.

圖10 未抓取至抓取狀態(tài)x,z軸位置誤差Fig.10 Position of x and z axis not grasped to grasp state

由圖10可以看出,在5 s之前作業(yè)型飛行機(jī)器人處于自由飛行狀態(tài),此時(shí)跟蹤誤差小于0.005 m,在5 s～20 s之間處于抓取飛行狀態(tài),此時(shí)的跟蹤誤差在-0.01 m～0.01 m之間,雖然誤差抖動(dòng)有所變化,但是仍然限制在可接受的范圍內(nèi).進(jìn)一步說明了本文所提出方法具有較好的控制效果.

5 結(jié)論

面向作業(yè)型飛行機(jī)器人滑翔抓取作業(yè)任務(wù),本文根據(jù)由四旋翼無人機(jī)和2自由度機(jī)械臂組成的作業(yè)型飛行機(jī)器人系統(tǒng)建立了滑翔抓取物體過程中的動(dòng)力學(xué)模型,并設(shè)計(jì)了抗擾動(dòng)的魯棒自適應(yīng)控制器,不僅能有限減弱機(jī)械臂擾動(dòng)和轉(zhuǎn)動(dòng)慣量變化對(duì)作業(yè)型飛行機(jī)器人系統(tǒng)產(chǎn)生的影響,同時(shí)可以盡可能的減小輸出力矩的抖動(dòng),提高作業(yè)型飛行機(jī)器人系統(tǒng)的飛行穩(wěn)定性.最后通過仿真對(duì)比,充分證明了所提出控制方法的穩(wěn)定性和優(yōu)越性.