基于桌面機械臂的快速任務調度控制系統設計*

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

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

0 引 言

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

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

1 任務調度與系統結構

1.1 任務調度

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

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

(1)

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

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

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

(2)

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

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

(3)

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

1.2 系統結構

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

圖1 系統結構

2 被控對象建模

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

圖2 桌面機械臂

(4)

(7)

(10)

設末端齊次變換矩陣為

(11)

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

(12)

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

3 控制結構設計

控制結構選取多通道控制結構，如圖3所示。每個獨立的控制對象有著獨立且與之對應的控制器。在每個控制干路中采用與多回環反饋相結合的方式形成獨立的控制回環。子控制器參數可以獨立設置，在任務執行上提高了控制效率，降低了控制模塊之間的耦合。每個控制回環的執行與否經由任務調度控制器依據優先級進行調度處理，確保控制系統的每個子控制單元順利被調度，完成控制任務。

圖3 控制結構

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

(13)

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

e=qd-q

(14)

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

(15)

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

(16)

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

(17)

(18)

聯立得式(17)有

(19)

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

4 實驗仿真與結果分析

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

void main(void)

{Create_Scheduler();/*創建任務調度并分配子模塊優先級*/

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

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

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

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

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

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

圖4 末端跟蹤軌跡

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

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

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

5 結束語

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