應用于茶葉罐分揀的碼垛機器人最優關節空間控制*

季曉明，文懷海，劉艷梨

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

0 引言

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

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

1 碼垛機器人設計與動力學建模

1.1 系統設計

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

1.2 機器人動力學建模

對于圖1所示生產線中的碼垛機器人，可采用拉格朗日方程推導出剛體動力學模型[8]

(1)

M——R6×6剛度矩陣；

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

G——R6×1重力項矩陣；

τ——R6×1的關節力矩；

τd——R6×1的包含了摩擦力矩、關節負載、外界擾動與未建模特性的集總干擾力矩。

為更好地展示機器人系統輸入輸出的關系，將式(1)改寫成

(2)

2 碼垛機器人最優運動控制

2.1 軌跡規劃

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

(3)

算法的具體過程如下。

1) 算法初始化。設GSO的種群數目為N，隨機產生i只螢火蟲的位置，即

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

(4)

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

qU、qL——關節空間的上限、下限；

t——迭代次數。

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

(5)

rs——最大感知半徑；

β——權重因子；

nt——控制參數；

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

(6)

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

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

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

(7)

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

(8)

式中：s——步長大小。

熒光素值的大小取決于目標函數值的大小，其更新公式為

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

(9)

式中：ρ——0～1的常數，表示熒光素揮發值；

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

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

通過上述GSO算法就可以獲得碼垛機器人的運動學逆解。為了使機器人在關節空間內運動光滑連續，還需對關節角序列做光滑處理。這里引入NURBS曲線結構[10]，其公式為

(10)

式中：Pi——控制頂點數；

ωi——權重因子，ωi≥0；

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

a、b——設定為0和1；

u——節點參數，滿足約束條件見式(11)。

(11)

2.2 控制器設計

當獲得關節角的參考軌跡后，將設計機器人的關節空間軌跡跟蹤控制器，該控制器設計思想為：不依賴機器人精確的動力學模型，僅根據系統輸入輸出數據來構造控制律[11]。

具體過程如下。

(12)

(13)

其中1<γ<2，0

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

求式(12)關于時間的一階導數，并聯立式(13)可得碼垛機器人的控制律為

(14)

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

2.3 穩定性證明

假設機器人系統中的集總干擾τd有界且可導，則在本文所設計控制器作用下能夠保證關節變量跟蹤誤差以及其一階導數在有限時間內收斂到某個區域[12]，即

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

(15)

(16)

選擇李雅普諾夫函數為

(17)

對式(17)求關于時間的一階導數有

(18)

聯立式(12)～式(14)，可得連續非奇異終端滑模面函數的一階導數為

(19)

聯立式(17)～式(19)可得李雅普諾夫函數的一階導數為

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

(21)

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

(22)

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

(23)

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

(24)

(25)

整理式(25)，有

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

(26)

進而，將滑模面變換成如下形式

(27)

由于|s|≤Δ，因此關節角跟蹤誤差的一階導數能在有限時間收斂到區域

(28)

本文所提方法的結構框架如圖2所示。

圖2 本文所提方法的結構框架Fig. 2 Structure frame of the proposed method

3 結果與分析

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

表1 碼垛機器人的DH參數Tab. 1 DH parameters of the palletizing robot

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

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

圖3 三種控制器下關節角1的跟蹤響應Fig. 3 Tracking response of joint 1 withthree controllers respectively

從圖3中可以看出，三種控制器均能幫助機器人關節角q1跟蹤上參考軌跡，但本文控制器的穩定時間小于0.5 s，且穩態誤差不大于2%，這說明本文控制器能在集總干擾力矩影響下提供相對穩定的輸出響應，從而證明了本文控制策略的優越性。

進而設計第二個仿真算例來模擬碼垛機器人對茶葉罐的分揀。設經視覺檢測模塊測量的紅色茶葉罐位形為[0.617,0.917,1.256]Tmm，[-40°,46°,99°]T，需要碼放的目標點位形為[0.090,-0.518,1.664]Tmm，[145°,92°,-160°]T。仿真中，先根據機器人末端執行器的起始位形用GSO算法計算出起始關節角，再經五階多項式插值法獲得關節角空間中的運動序列，并用NURBS曲線進行平滑過渡處理，最終生成機器人關節軌跡。

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

圖5給出了規劃后的機器人關節軌跡，可以看出在執行碼垛任務時各關節角變化較為平穩柔順。

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

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

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

圖7給出的各關節角控制力矩沒有明顯的抖振現象，這說明本文所提含有飽和函數的連續非奇異終端滑模面函數具有良好的抖振降低效果，進一步說明了所設計控制器的有效性。

圖5 軌跡規劃后的關節角序列Fig. 5 Joint angles response after trajectory planning

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

(a) 關節角1的響應

(b) 關節角2的響應

(c) 關節角3的響應

(d) 關節角4的響應

(e) 關節角5的響應

(f) 關節角6的響應圖6 本文控制器下關節角的跟蹤響應Fig. 6 Tracking response of joints with the proposed controller

圖7 關節力矩響應Fig. 7 Response of the torque

4 結論

針對茶葉罐分揀碼垛機器人的關節空間控制，本文提出了一種基于快速連續非奇異終端滑模策略的軌跡跟蹤控制方法。該方法根據碼垛機器人輸入輸出數據分別設計6個獨立的控制器，利用快速連續非奇異終端滑模面來提高系統狀態量的收斂速度，然后基于李雅普諾夫函數證明所設計的控制系統全局穩定，最后通過仿真算例對本文方法進行了驗證。

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

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

3) 結合GSO算法、五階多項式插補法與NURBS曲線能快速幫助碼垛機器人規劃出光滑柔順的關節運動軌跡，算法所需時間為0.37 s，實時性較高。