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

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

0 引 言

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

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

1 任務調度與系統(tǒng)結構

1.1 任務調度

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

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

(1)

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

類似地非周期任務時間描述為

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

(2)

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

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

(3)

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

1.2 系統(tǒng)結構

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

圖1 系統(tǒng)結構

2 被控對象建模

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

圖2 桌面機械臂

(4)

(7)

(10)

設末端齊次變換矩陣為

(11)

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

(12)

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

3 控制結構設計

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

圖3 控制結構

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

(13)

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

e=qd-q

(14)

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

(15)

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

(16)

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

(17)

(18)

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

(19)

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

4 實驗仿真與結果分析

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

void main(void)

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

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

hdStartchedule(gCallbackHandle);/*啟動當前任務調度*/

hdChackScheduler(flag);/*檢測任務標志位*/

mdlUpdate(gCallbackHandle);/*更新任務調度*/

hdReStart_Schedule(gCallbackHandle);/ *重啟任務調度*/}

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

圖4 末端跟蹤軌跡

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

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

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

5 結束語

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