應(yīng)用于茶葉罐分揀的碼垛機(jī)器人最優(yōu)關(guān)節(jié)空間控制*

季曉明，文懷海，劉艷梨

(1. 江蘇安全技術(shù)職業(yè)學(xué)院電氣工程系，江蘇徐州，221011； 2. 大連理工大學(xué)機(jī)械工程學(xué)院，遼寧大連，116024；3. 南京航空航天大學(xué)機(jī)電學(xué)院，南京市，210016)

0 引言

罐盒分揀是茶葉自動(dòng)包裝技術(shù)中的重要一環(huán)。隨著智能制造、機(jī)器視覺、機(jī)器人等技術(shù)的發(fā)展，運(yùn)用碼垛機(jī)器人來(lái)提高自動(dòng)包裝生產(chǎn)線的作業(yè)效率越來(lái)越受到用戶的關(guān)注[1-2]。對(duì)于碼垛機(jī)器人的研究通常集中在本體設(shè)計(jì)、動(dòng)力學(xué)、控制系統(tǒng)設(shè)計(jì)與軌跡規(guī)劃幾個(gè)方面[3]，而本文研究?jī)?nèi)容為關(guān)節(jié)空間內(nèi)碼垛機(jī)器人的最優(yōu)軌跡跟蹤控制。機(jī)器人系統(tǒng)具有高非線性、多變量、高維度的特性，這給高精度控制器的設(shè)計(jì)帶來(lái)了很大的難度。再者，由系統(tǒng)摩擦力矩、關(guān)節(jié)負(fù)載、外界擾動(dòng)與未建模特性構(gòu)成的集總干擾力矩也會(huì)進(jìn)一步加劇控制器設(shè)計(jì)的難度。

對(duì)于上述問(wèn)題，國(guó)內(nèi)外學(xué)者已進(jìn)行了諸多探索。例如，Atalar-Ayyldz等[4]采用分?jǐn)?shù)階PID(Proportion Integration Differentiation，PID)控制策略來(lái)設(shè)計(jì)6自由度機(jī)器人的軌跡跟蹤控制器，在仿真環(huán)境下獲得了不錯(cuò)的控制效果。Nikdel等[5]設(shè)計(jì)了一種狀態(tài)增強(qiáng)自適應(yīng)反步法(Backstepping controller，BC)控制器來(lái)保證串聯(lián)型機(jī)器人在非線性與參數(shù)不確定性下的跟蹤性能，并成功應(yīng)用到了大型工業(yè)SCARA機(jī)器人的仿真中。針對(duì)系統(tǒng)中的集總干擾力矩，Homayounzade等[6]將BC和擾動(dòng)觀測(cè)器相結(jié)合有效補(bǔ)償與抑制了干擾項(xiàng)，提高了機(jī)器人軌跡跟蹤的精度。這些控制策略為本文提供了很好的借鑒。近年來(lái)，滑?？刂?Sliding mode control，SMC)技術(shù)頗受人們關(guān)注，它是一種常見的魯棒控制技術(shù)，被廣泛應(yīng)用于機(jī)器人控制系統(tǒng)設(shè)計(jì)中。不過(guò)傳統(tǒng)的SMC因其固有的滑模面結(jié)構(gòu)易引起控制信號(hào)的抖振，在一定程度上會(huì)削弱系統(tǒng)的控制性能[7]。因此，本文采用快速連續(xù)非奇異終端滑模面來(lái)改進(jìn)傳統(tǒng)SMC，提高機(jī)器人系統(tǒng)狀態(tài)量的收斂速度。另一方面，碼垛機(jī)器人軌跡跟蹤效果還依賴于參考軌跡的規(guī)劃。對(duì)于關(guān)節(jié)空間的軌跡規(guī)劃，本文先用螢火蟲算法(Glowworm swarm optimization algorithm，GSO)計(jì)算出碼垛機(jī)器人起止位形對(duì)應(yīng)的關(guān)節(jié)角，接著用五階多項(xiàng)式插補(bǔ)獲得關(guān)節(jié)角序列。為了保證算法的實(shí)時(shí)性，中間插值點(diǎn)不能過(guò)多，但這也會(huì)帶來(lái)關(guān)節(jié)不光滑的后果，因此引入NUBRS曲線進(jìn)行光滑處理，使得關(guān)節(jié)運(yùn)動(dòng)平滑柔順。最后，通過(guò)兩組仿真算例對(duì)本文所提方法的有效性進(jìn)行驗(yàn)證。

1 碼垛機(jī)器人設(shè)計(jì)與動(dòng)力學(xué)建模

1.1 系統(tǒng)設(shè)計(jì)

本文設(shè)計(jì)了一條碼垛機(jī)器人生產(chǎn)線來(lái)提高不同顏色茶葉罐的分揀效率，如圖1所示。整條生產(chǎn)線的工作原理為：深淺兩種顏色的茶葉罐倒入下料筒中，在輸送帶A上經(jīng)視覺檢測(cè)，深色茶葉罐落入落料盒A中，淺色茶葉罐由碼垛機(jī)器人搬運(yùn)至輸送帶B上，落入落料盒B中。

1.2 機(jī)器人動(dòng)力學(xué)建模

對(duì)于圖1所示生產(chǎn)線中的碼垛機(jī)器人，可采用拉格朗日方程推導(dǎo)出剛體動(dòng)力學(xué)模型[8]

(1)

M——R6×6剛度矩陣；

C——R6×6包含科氏力與離心力的矩陣；

G——R6×1重力項(xiàng)矩陣；

τ——R6×1的關(guān)節(jié)力矩；

τd——R6×1的包含了摩擦力矩、關(guān)節(jié)負(fù)載、外界擾動(dòng)與未建模特性的集總干擾力矩。

為更好地展示機(jī)器人系統(tǒng)輸入輸出的關(guān)系，將式(1)改寫成

(2)

2 碼垛機(jī)器人最優(yōu)運(yùn)動(dòng)控制

2.1 軌跡規(guī)劃

假設(shè)碼垛機(jī)器人末端執(zhí)行器的實(shí)際位形為(Rd,Pd)，目標(biāo)位形為(Rf,Pf)，那么軌跡規(guī)劃的本質(zhì)就是引入GSO算法使得下列的目標(biāo)函數(shù)最小[9]。

(3)

算法的具體過(guò)程如下。

1) 算法初始化。設(shè)GSO的種群數(shù)目為N，隨機(jī)產(chǎn)生i只螢火蟲的位置，即

qi(t)=qL+rand(0,1)·(qU-qL)

(4)

式中： rand(·)——0～1之間的隨機(jī)函數(shù)；

qU、qL——關(guān)節(jié)空間的上限、下限；

t——迭代次數(shù)。

2) 更新決策閾。螢火蟲群體間的信息交互能力是通過(guò)決策閾大小來(lái)決定的，決策閾的更新公式為

(5)

rs——最大感知半徑；

β——權(quán)重因子；

nt——控制參數(shù)；

Ni(t)——第i只螢火蟲在第t次迭代時(shí)鄰域集合，滿足約束條件見式(6)。

(6)

式中：gj(t)——鄰域內(nèi)第j只螢火蟲位置；

li(t)、lj(t)——兩只螢火蟲的熒光素值。

3) 更新螢火蟲位置與熒光素值。比較當(dāng)前螢火蟲和鄰域螢火蟲之間的熒光素值大小，種群選擇朝熒光素值較大的螢火蟲方向移動(dòng)，移動(dòng)的概率為

(7)

第i只螢火蟲的位置更新公式為

(8)

式中：s——步長(zhǎng)大小。

熒光素值的大小取決于目標(biāo)函數(shù)值的大小，其更新公式為

li(t)=(1-ρ)li(t-1)+γf(gi(t))

(9)

式中：ρ——0～1的常數(shù)，表示熒光素?fù)]發(fā)值；

γ——熒光素值的更新速度。

4) 終止條件。在鄰域集合內(nèi)，螢火蟲i遇到熒光素值更高的螢火蟲j時(shí)，若兩者距離小于感知半徑，則前者以概率Pij(t)選擇鄰域個(gè)體并其靠攏。接著，螢火蟲i更新位置與熒光素值，直到算法迭代次數(shù)等于最大值Tmax，算法終止。

通過(guò)上述GSO算法就可以獲得碼垛機(jī)器人的運(yùn)動(dòng)學(xué)逆解。為了使機(jī)器人在關(guān)節(jié)空間內(nèi)運(yùn)動(dòng)光滑連續(xù)，還需對(duì)關(guān)節(jié)角序列做光滑處理。這里引入NURBS曲線結(jié)構(gòu)[10]，其公式為

(10)

式中：Pi——控制頂點(diǎn)數(shù)；

ωi——權(quán)重因子，ωi≥0；

Ni,k(u)——k-1次的B樣條基函數(shù)；

a、b——設(shè)定為0和1；

u——節(jié)點(diǎn)參數(shù)，滿足約束條件見式(11)。

(11)

2.2 控制器設(shè)計(jì)

當(dāng)獲得關(guān)節(jié)角的參考軌跡后，將設(shè)計(jì)機(jī)器人的關(guān)節(jié)空間軌跡跟蹤控制器，該控制器設(shè)計(jì)思想為：不依賴機(jī)器人精確的動(dòng)力學(xué)模型，僅根據(jù)系統(tǒng)輸入輸出數(shù)據(jù)來(lái)構(gòu)造控制律[11]。

具體過(guò)程如下。

(12)

(13)

其中1<γ<2，0

式中：β、k1、k2——控制增益矩陣。

求式(12)關(guān)于時(shí)間的一階導(dǎo)數(shù)，并聯(lián)立式(13)可得碼垛機(jī)器人的控制律為

(14)

式(14)中控制律包括等效控制律τe和趨近律τr兩部分。其中，τe用于抑制控制器的抖振和減弱干擾力矩的影響，以保證系統(tǒng)的魯棒性；τr用于加快系統(tǒng)各狀態(tài)量的響應(yīng)速率，提高系統(tǒng)的被控性能。

2.3 穩(wěn)定性證明

假設(shè)機(jī)器人系統(tǒng)中的集總干擾τd有界且可導(dǎo)，則在本文所設(shè)計(jì)控制器作用下能夠保證關(guān)節(jié)變量跟蹤誤差以及其一階導(dǎo)數(shù)在有限時(shí)間內(nèi)收斂到某個(gè)區(qū)域[12]，即

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

(15)

(16)

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

(17)

對(duì)式(17)求關(guān)于時(shí)間的一階導(dǎo)數(shù)有

(18)

聯(lián)立式(12)～式(14)，可得連續(xù)非奇異終端滑模面函數(shù)的一階導(dǎo)數(shù)為

(19)

聯(lián)立式(17)～式(19)可得李雅普諾夫函數(shù)的一階導(dǎo)數(shù)為

式(20)包含兩種形式，即

(21)

引理：如果存在李雅普諾夫函數(shù)V不等式(22)，則其收斂到初始值V(0)所需的時(shí)間為

(22)

ts≤ω-1(1-ζ)-1ln(1+ωp-1V01-ζ)

(23)

其中ω、p>0且0<ζ<1。

(24)

(25)

整理式(25)，有

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

(26)

進(jìn)而，將滑模面變換成如下形式

(27)

由于|s|≤Δ，因此關(guān)節(jié)角跟蹤誤差的一階導(dǎo)數(shù)能在有限時(shí)間收斂到區(qū)域

(28)

本文所提方法的結(jié)構(gòu)框架如圖2所示。

圖2 本文所提方法的結(jié)構(gòu)框架Fig. 2 Structure frame of the proposed method

3 結(jié)果與分析

為驗(yàn)證本文所提方法的有效性，在Matlab/Simulink環(huán)境下進(jìn)行仿真模擬。其中，碼垛機(jī)器人的DH參數(shù)如表1所示，末端執(zhí)行器(夾爪，用數(shù)字7表示)的長(zhǎng)度為108 mm。

表1 碼垛機(jī)器人的DH參數(shù)Tab. 1 DH parameters of the palletizing robot

為了比較本文所提控制器的性能，引入SMC和BC進(jìn)行比較(這兩種控制器參數(shù)的定義可參見文獻(xiàn)[13-14]，與本文控制器一樣，它們也由GSO算法進(jìn)行參數(shù)整定)，三種控制器的參數(shù)如表2所示。以q1為例，設(shè)定其需要跟蹤的參考軌跡為q1r=114.6°，τd1=50 sint，仿真時(shí)間為5 s，其余初始條件均為0，結(jié)果如圖3所示。

表2 三種控制器的參數(shù)Tab. 2 Parameters of the three controllers

圖3 三種控制器下關(guān)節(jié)角1的跟蹤響應(yīng)Fig. 3 Tracking response of joint 1 withthree controllers respectively

從圖3中可以看出，三種控制器均能幫助機(jī)器人關(guān)節(jié)角q1跟蹤上參考軌跡，但本文控制器的穩(wěn)定時(shí)間小于0.5 s，且穩(wěn)態(tài)誤差不大于2%，這說(shuō)明本文控制器能在集總干擾力矩影響下提供相對(duì)穩(wěn)定的輸出響應(yīng)，從而證明了本文控制策略的優(yōu)越性。

進(jìn)而設(shè)計(jì)第二個(gè)仿真算例來(lái)模擬碼垛機(jī)器人對(duì)茶葉罐的分揀。設(shè)經(jīng)視覺檢測(cè)模塊測(cè)量的紅色茶葉罐位形為[0.617,0.917,1.256]Tmm，[-40°,46°,99°]T，需要碼放的目標(biāo)點(diǎn)位形為[0.090,-0.518,1.664]Tmm，[145°,92°,-160°]T。仿真中，先根據(jù)機(jī)器人末端執(zhí)行器的起始位形用GSO算法計(jì)算出起始關(guān)節(jié)角，再經(jīng)五階多項(xiàng)式插值法獲得關(guān)節(jié)角空間中的運(yùn)動(dòng)序列，并用NURBS曲線進(jìn)行平滑過(guò)渡處理，最終生成機(jī)器人關(guān)節(jié)軌跡。

GSO算法參數(shù)設(shè)置如下：N=20，γ=0.6，ρ=0.4，s=0.03，β=0.1，nt=8。GSO算法的迭代曲線如圖4所示，算法所需時(shí)間為0.37 s，完全能夠滿足碼垛機(jī)器人的工作節(jié)拍。

圖5給出了規(guī)劃后的機(jī)器人關(guān)節(jié)軌跡，可以看出在執(zhí)行碼垛任務(wù)時(shí)各關(guān)節(jié)角變化較為平穩(wěn)柔順。

最后利用本文所設(shè)計(jì)控制器去跟蹤上述規(guī)劃好的關(guān)節(jié)軌跡。選擇均值為0、方差為0.01、頻率為20 Hz、增益為10的隨機(jī)噪聲來(lái)模擬集總干擾力矩。表3給出了碼垛機(jī)器人系統(tǒng)控制器參數(shù)。仿真結(jié)果如圖6所示。

圖4 GSO迭代曲線Fig. 4 Iteration curve of GSO

從圖6中可以看出，在本文控制器作用下機(jī)器人能夠較好地抑制住集總干擾力矩，使得各關(guān)節(jié)跟蹤上參考軌跡。進(jìn)而，以均方根誤差(Root Mean Square Error，簡(jiǎn)稱RMSE)[15]來(lái)量化關(guān)節(jié)空間內(nèi)的跟蹤誤差，各關(guān)節(jié)角的RMSE分別為0.025，0.031，0.087，0.103，0.154，0.160，這種量級(jí)誤差在實(shí)操中是可接受的。

圖7給出的各關(guān)節(jié)角控制力矩沒有明顯的抖振現(xiàn)象，這說(shuō)明本文所提含有飽和函數(shù)的連續(xù)非奇異終端滑模面函數(shù)具有良好的抖振降低效果，進(jìn)一步說(shuō)明了所設(shè)計(jì)控制器的有效性。

圖5 軌跡規(guī)劃后的關(guān)節(jié)角序列Fig. 5 Joint angles response after trajectory planning

表3 本文控制器參數(shù)Tab. 3 Parameters of the proposed controller

(a) 關(guān)節(jié)角1的響應(yīng)

(b) 關(guān)節(jié)角2的響應(yīng)

(c) 關(guān)節(jié)角3的響應(yīng)

(d) 關(guān)節(jié)角4的響應(yīng)

(e) 關(guān)節(jié)角5的響應(yīng)

(f) 關(guān)節(jié)角6的響應(yīng)圖6 本文控制器下關(guān)節(jié)角的跟蹤響應(yīng)Fig. 6 Tracking response of joints with the proposed controller

圖7 關(guān)節(jié)力矩響應(yīng)Fig. 7 Response of the torque

4 結(jié)論

針對(duì)茶葉罐分揀碼垛機(jī)器人的關(guān)節(jié)空間控制，本文提出了一種基于快速連續(xù)非奇異終端滑模策略的軌跡跟蹤控制方法。該方法根據(jù)碼垛機(jī)器人輸入輸出數(shù)據(jù)分別設(shè)計(jì)6個(gè)獨(dú)立的控制器，利用快速連續(xù)非奇異終端滑模面來(lái)提高系統(tǒng)狀態(tài)量的收斂速度，然后基于李雅普諾夫函數(shù)證明所設(shè)計(jì)的控制系統(tǒng)全局穩(wěn)定，最后通過(guò)仿真算例對(duì)本文方法進(jìn)行了驗(yàn)證。

1) 與SMC和BC相比，本文控制器跟蹤性能最好，并且穩(wěn)定時(shí)間小于0.5 s，穩(wěn)態(tài)誤差不大于2%。

2) 本文控制器能有效抑制集總干擾力矩影響和削弱控制抖振，能保證機(jī)器人在關(guān)節(jié)空間內(nèi)的高精度軌跡跟蹤控制。

3) 結(jié)合GSO算法、五階多項(xiàng)式插補(bǔ)法與NURBS曲線能快速幫助碼垛機(jī)器人規(guī)劃出光滑柔順的關(guān)節(jié)運(yùn)動(dòng)軌跡，算法所需時(shí)間為0.37 s，實(shí)時(shí)性較高。