基于桌面機械臂的快速任務(wù)調(diào)度控制系統(tǒng)設(shè)計*

徐 亮， 劉滿祿2,， 張 靜2,， 張 華， 王 姮

(1.西南科技大學(xué) 信息工程學(xué)院，四川 綿陽 621010；2.中國科學(xué)技術(shù)大學(xué) 信息科學(xué)技術(shù)學(xué)院，安徽 合肥 230026)

0 引 言

目前多數(shù)機械臂[1,2]采用集成控制板卡或者控制柜進行控制的，在處理繁雜任務(wù)時往往需要人為示教和反復(fù)調(diào)整，且示教后的實時控制性能并不理想[3，4]。針對此問題，鄭天江提出了工控機內(nèi)核開發(fā)和運動控制板卡相結(jié)合方案，盡管在一定程度上提高了控制系統(tǒng)的實時性，但系統(tǒng)和應(yīng)用程序接口(application program interface,API)移植卻很不方便[5]。張艷瓊等人[6]提出了基于Web Service 的控制系統(tǒng)，雖然加快了上位機與硬件的數(shù)據(jù)交互，但對于多任務(wù)調(diào)度下的機器人姿態(tài)解算的實時性并不高。王帥華等人[7]提出了基于ARM實時搶占式的控制系統(tǒng)，在一定程度上提高了實時性，但在任務(wù)調(diào)度中卻未給出。楊杏等人[8]提出了基于Qt開發(fā)的控制系統(tǒng)，雖然開發(fā)和移植簡便，但并未考慮到機械臂在運動控制中需要實時進行繁雜的姿態(tài)計算。

本文提出了基于快速任務(wù)調(diào)度的控制系統(tǒng)。將系統(tǒng)分為三個部分進行設(shè)計，機械臂的API以及控制系統(tǒng)的任務(wù)調(diào)度均采用C++語言處理，經(jīng)由MATLAB外部語言接口調(diào)用并能夠?qū)崿F(xiàn)模塊的快速裝載。機械臂的姿態(tài)解算由機器人工具箱集成的專用函數(shù)塊快速處理，能夠?qū)崟r解算機械臂姿態(tài)，并對位姿進行記錄理，為系統(tǒng)實時操控提供了保障。

1 任務(wù)調(diào)度與系統(tǒng)結(jié)構(gòu)

1.1 任務(wù)調(diào)度

現(xiàn)有的以任務(wù)為導(dǎo)向的控制系統(tǒng)，一般為實時任務(wù)系統(tǒng)和非實時任務(wù)系統(tǒng)[9]。針對實時任務(wù)系統(tǒng)又分為周期性任務(wù)控制系統(tǒng)和非周期性控制任務(wù)系統(tǒng)。就非周期實時任務(wù)控制系統(tǒng)而言，通常采用事件的優(yōu)先級的順序進行任務(wù)調(diào)度處理[10，11]。系統(tǒng)中第i個執(zhí)行任務(wù)事件為Ei,對于系統(tǒng)任務(wù)集合描述為{E1,E2,…,En}，于是在周期性任務(wù)系統(tǒng)中時間描述為

TR={max(Cp1,Cp2,Cp3,…,Cpn),TPp,TDp}

(1)

式中Cpn為任務(wù)計算時間,TPp為任務(wù)周期，TDp為任務(wù)時間限制。

類似地非周期任務(wù)時間描述為

TUR={Capmax,Rap,TDap,η,Δtmin}

(2)

式中Capmax為最大任務(wù)計算時間；Rap為任務(wù)下發(fā)時間；TDap為任務(wù)限制；η為非周期任務(wù)達到率；Δtmin為任務(wù)最小時間間隔。

設(shè)任務(wù)調(diào)度最大干擾時間為Nt，則對于任意一個任務(wù)在周期系統(tǒng)下能被調(diào)度，則滿足

(3)

由式(3)可知對于任務(wù)調(diào)度系統(tǒng)若要保持任務(wù)能快速被調(diào)用，就必須壓縮任務(wù)計算執(zhí)行時間，因此采用多模塊并行處理，運用任務(wù)調(diào)度的控制方式是十分必要的。

1.2 系統(tǒng)結(jié)構(gòu)

系統(tǒng)設(shè)計分為控制系統(tǒng)與任務(wù)調(diào)度、機械臂的API和機械臂位姿解算3部分。當(dāng)控制信號給定時，由控制器結(jié)合姿態(tài)結(jié)算信息按照優(yōu)先級進行任務(wù)調(diào)度分配，任務(wù)調(diào)度模塊通過機械臂的API實現(xiàn)控制任務(wù)的快速調(diào)度與執(zhí)行。在控制器執(zhí)行的同時，姿態(tài)解算模塊對機械臂進行實時位姿信息獲取并快速解析送至控制器，確保任務(wù)調(diào)度高效、有序進行。系統(tǒng)整體結(jié)構(gòu)如圖1所示。

圖1 系統(tǒng)結(jié)構(gòu)

2 被控對象建模

Geomagic Touch多自由度桌面機械臂作為控制對象，如圖2所示。

圖2 桌面機械臂

(4)

(7)

(10)

設(shè)末端齊次變換矩陣為

(11)

得末端位置坐標(biāo)(px，py，pz)，求導(dǎo)有速度表達式滿足

(12)

根據(jù)速度表達式，代入?yún)?shù)計算出速度雅克比矩陣，即可建立關(guān)節(jié)角速度跟末端姿態(tài)的關(guān)系，其中，實時的姿態(tài)解算由MATLAB下的機器人工具箱完成[12，13]。

3 控制結(jié)構(gòu)設(shè)計

控制結(jié)構(gòu)選取多通道控制結(jié)構(gòu)，如圖3所示。每個獨立的控制對象有著獨立且與之對應(yīng)的控制器。在每個控制干路中采用與多回環(huán)反饋相結(jié)合的方式形成獨立的控制回環(huán)。子控制器參數(shù)可以獨立設(shè)置，在任務(wù)執(zhí)行上提高了控制效率，降低了控制模塊之間的耦合。每個控制回環(huán)的執(zhí)行與否經(jīng)由任務(wù)調(diào)度控制器依據(jù)優(yōu)先級進行調(diào)度處理，確保控制系統(tǒng)的每個子控制單元順利被調(diào)度，完成控制任務(wù)。

圖3 控制結(jié)構(gòu)

對于N個關(guān)節(jié)的機械臂動力學(xué)方程為

(13)

式中M(q)為正定慣性矩陣；C(q)為離心和哥氏力項；G(q)為重力項。設(shè)期望關(guān)節(jié)角度為qd，跟蹤誤差為e，有

e=qd-q

(14)

采用PD控制器，設(shè)比例控制增益與微分控制增益分別為Kp和Kd；控制率為

(15)

設(shè)期望重力為G(q)d有

(16)

由于桌面機械臂末端在水平面運行，于是實際重力項和期望重力項之差近似為零。期望重向關(guān)節(jié)與水平面接觸，由式(16)整理得

(17)

(18)

聯(lián)立得式(17)有

(19)

由式(21)知控制系統(tǒng)輸出收斂。

4 實驗仿真與結(jié)果分析

實驗平臺為SensAble科技公司的Geomagic Touch多自由度桌面機械臂設(shè)備[14，15]。該機械臂前3個關(guān)節(jié)有電機，后2個關(guān)節(jié)只有位置電位器。實驗過程中，機械臂末端處于近似光滑的桌子平面。給定位置信號后，使得末端軌跡進行跟蹤，并根據(jù)跟蹤情況對系統(tǒng)性能進行分析。系統(tǒng)的任務(wù)調(diào)度函數(shù)由C++語言編寫并封裝，經(jīng)由MATLAB的C編譯器編譯后生成后綴為.mexw的可調(diào)用文件。系統(tǒng)運行時，首先啟動各個模塊，并依據(jù)優(yōu)先級進行任務(wù)調(diào)度。在調(diào)度過程中，當(dāng)檢測到各個子模塊的標(biāo)志參數(shù)滿足一定條件時，對任務(wù)執(zhí)行的優(yōu)先級順序進行修改，使得控制任務(wù)順利完成，確保系統(tǒng)運行的快速性和實時性。任務(wù)調(diào)度的主函數(shù)部分核心代碼如下：

void main(void)

{Create_Scheduler();/*創(chuàng)建任務(wù)調(diào)度并分配子模塊優(yōu)先級*/

Enable_Device(hHD);/*初始化硬件設(shè)備*/

hdStartchedule(gCallbackHandle);/*啟動當(dāng)前任務(wù)調(diào)度*/

hdChackScheduler(flag);/*檢測任務(wù)標(biāo)志位*/

mdlUpdate(gCallbackHandle);/*更新任務(wù)調(diào)度*/

hdReStart_Schedule(gCallbackHandle);/ *重啟任務(wù)調(diào)度*/}

在信道給定正弦信號下，末端軌跡在桌面跟蹤情況如圖4所示。

圖4 末端跟蹤軌跡

由圖4可以看出，桌面機械臂的末端在Y軸和X軸方向分別在375 ms和492 ms內(nèi)完成期望運動軌跡跟蹤，且首次跟蹤時間均控制在0.5 s以內(nèi)，表明系統(tǒng)的動態(tài)響應(yīng)性能良好。在500 ms以后，Y方向和X軸方向跟蹤較為平滑，均未出現(xiàn)跟蹤上的時延滯后和與跟蹤不足問題，表明系統(tǒng)的穩(wěn)定性能良好。

在時間順序上，Y軸與X軸方向首次完成軌跡跟蹤時間并不同步，這是由于執(zhí)行Y方向的跟蹤任務(wù)調(diào)度先于X軸方向跟蹤任務(wù)的調(diào)度。此外，從Y方向的軌跡跟蹤可以看出，從信號輸入到完成首次跟蹤時間為375 ms，這是由于系統(tǒng)要初始化各個模塊并進行任務(wù)調(diào)度的耗時。從Y軸方向的任務(wù)完成到調(diào)度X軸方向的任務(wù)執(zhí)行亦即二者完成首次跟蹤消耗時間之差，即完成二次任務(wù)調(diào)度時間117 ms，控制在0.12 s以內(nèi)，從任務(wù)調(diào)度到任務(wù)執(zhí)行完成較信號周期時間較為短暫。這表明系統(tǒng)的實時性能良好，也為機械臂末端實時跟蹤提供了保障。

此外機械臂末端運行的桌面存一定的摩擦，機械臂的關(guān)節(jié)的機械摩擦等外界因素也給控制系統(tǒng)帶來了一定的非線性干擾。盡管如此，設(shè)計的系統(tǒng)對桌面機械臂仍實現(xiàn)了較好任務(wù)調(diào)度實時控制，表明設(shè)計方法的有效性。

5 結(jié)束語

實驗證明，所設(shè)計的快速任務(wù)調(diào)度控制系統(tǒng)，實時性能良好。整體系統(tǒng)初始化至第一次任務(wù)調(diào)度執(zhí)行耗時小，任務(wù)調(diào)度切換完成迅速，系統(tǒng)動態(tài)特性良好，滿足控制要求。從實驗結(jié)果看，Y方向和X方向的軌跡均實現(xiàn)了較為平滑的穩(wěn)定的跟蹤，系統(tǒng)穩(wěn)定、可靠。因此，設(shè)計的控制方法是可行的，另外該系統(tǒng)結(jié)構(gòu)簡單，為同類工程實現(xiàn)提供了參考。下一步的研究工作，應(yīng)優(yōu)化機械臂的姿態(tài)解算控制器，使之能夠快速完成更為復(fù)雜的機械臂的姿態(tài)解算，進一步優(yōu)化整體系統(tǒng)的實時性，提高系統(tǒng)實用價值。