雙SCARA機(jī)器人運動模型仿真與控制系統(tǒng)

潘廣通，李慧玲，楊暢暢，姚天勝，王佳維

（湖北汽車工業(yè)學(xué)院 電氣與信息工程學(xué)院，湖北 十堰 442002）

相比單個機(jī)器人，雙機(jī)協(xié)作機(jī)器人對環(huán)境表現(xiàn)出更強(qiáng)的適應(yīng)能力，具有更強(qiáng)的靈活性和魯棒性，可以更加出色地完成作業(yè)任務(wù)［1］。經(jīng)過幾十年的迅速發(fā)展，雙機(jī)協(xié)作機(jī)器人的研究取得了較多成果。李猛等［2］對雙機(jī)協(xié)作空間進(jìn)行了求解與仿真分析，為雙機(jī)器人避讓問題提供了理論依據(jù)。李鵬飛等［3］對理想環(huán)境下雙機(jī)器人運動路徑進(jìn)行了規(guī)劃，為工業(yè)實際應(yīng)用提供了理論基礎(chǔ)。王文林等［4］對雙機(jī)器人系統(tǒng)協(xié)調(diào)搬運作業(yè)進(jìn)行了實驗分析，水平和垂直方向的運行精度均控制在±2.8 mm以內(nèi)。文中以2 臺三自由度SCARA 機(jī)器人為研究對象，針對雙機(jī)器人協(xié)作搬運時的干涉問題，建立運動學(xué)模型，制定雙機(jī)協(xié)調(diào)工作任務(wù)，開發(fā)上位機(jī)人機(jī)交互界面和雙機(jī)避讓程序，控制機(jī)器人運行。

1 方案設(shè)計及模型建立

1.1 方案設(shè)計

采用D-H 法建立雙機(jī)協(xié)作機(jī)器人運動學(xué)模型，通過MATLAB 編程求解雙機(jī)協(xié)作機(jī)器人的協(xié)調(diào)工作空間。以PLC 為下位機(jī)控制器，對SCARA機(jī)器人進(jìn)行邏輯控制，制定雙機(jī)協(xié)作機(jī)器人協(xié)調(diào)工作任務(wù)?；贑#開發(fā)的運動控制上位機(jī)根據(jù)協(xié)調(diào)工作空間選擇雙機(jī)協(xié)作機(jī)器人避讓程序，使雙機(jī)協(xié)作機(jī)器人在工作空間內(nèi)穩(wěn)定運行。PLC設(shè)備、上位機(jī)之間進(jìn)行雙向通信，上位機(jī)向PLC 發(fā)送命令，PLC反解程序?qū)⑽恢脭?shù)據(jù)轉(zhuǎn)換為脈沖，控制機(jī)器人運動，PLC 正解程序?qū)⒚}沖轉(zhuǎn)換為位置數(shù)據(jù)，通過上位機(jī)實時顯示。系統(tǒng)總體結(jié)構(gòu)如圖1所示。

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

1.2 運動學(xué)模型建立

為了對雙機(jī)協(xié)作時的干涉問題進(jìn)行分析，建立了雙機(jī)協(xié)作機(jī)器人運動學(xué)模型。對2 臺相同型號的SCARA 機(jī)器人運用D-H 法建立連桿坐標(biāo)系，以1 號機(jī)器人基坐標(biāo)系作為雙機(jī)器人系統(tǒng)的世界坐標(biāo)系。雙機(jī)協(xié)作機(jī)器人位姿約束方程［5］為

式中：T3A為1號機(jī)器人基坐標(biāo)A到末端的位姿變換矩陣；T3B為2號機(jī)器人基坐標(biāo)B到末端的位姿變換矩陣；T0為A到B的齊次變換矩陣。根據(jù)式（1）可確定每個插補(bǔ)點的約束方程。以1號機(jī)器人為例，建立連桿坐標(biāo)系如圖2所示，機(jī)器人D-H參數(shù)如表1所示，ai為第i個關(guān)節(jié)的連桿長度，αi為第i個關(guān)節(jié)的扭角，di為第i個關(guān)節(jié)的移動范圍，θi為第i個關(guān)節(jié)旋轉(zhuǎn)角度。

圖2 連桿坐標(biāo)系

表1 SCARA機(jī)器人D-H參數(shù)表

1.2.1 機(jī)器人運動學(xué)求解

正運動學(xué)問題是已知初始給定的各關(guān)節(jié)轉(zhuǎn)動角度及關(guān)節(jié)參數(shù)，求解末端坐標(biāo)系在基坐標(biāo)系下的位姿［6］。根據(jù)機(jī)器人學(xué)的連桿變換求解正運動學(xué)，將各連桿變換矩陣相乘，得到SCARA 機(jī)器人的末端變換矩陣：

式（2）描述了末端連桿坐標(biāo)系x3y3z3相對于基坐標(biāo)系x0y0z0的位姿，在給定D-H參數(shù)的情況下，通過正運動學(xué)求解，機(jī)器人工作空間范圍內(nèi)的任意位姿均可用末端變換矩陣表示。

機(jī)器人的逆運動學(xué)問題是根據(jù)已知機(jī)械臂末端的位姿，反推各個關(guān)節(jié)到達(dá)期望位姿時的角度值［7］。求解機(jī)器人逆運動學(xué)的方法主要有幾何法、逆變換法和解析法，文中采用逆變換法，其計算速度快，便于實時控制。機(jī)器人末端相鄰關(guān)節(jié)位姿的逆矩陣間關(guān)系為

式（5）中θ2、θ3即SCARA 機(jī)器人的運動學(xué)逆解，均在機(jī)器人工作空間范圍內(nèi)，控制各關(guān)節(jié)角與逆解相同，可到達(dá)期望位姿。

1.2.2 雙機(jī)器人關(guān)節(jié)模型與協(xié)調(diào)工作空間

機(jī)器人的大臂長21.5 cm，小臂長21.5 cm，升降臂變化范圍為0～30 cm，大臂旋轉(zhuǎn)范圍為-180°～0°，小臂旋轉(zhuǎn)范圍為-135°～135°，2個機(jī)器人間相距78 cm。MATLAB 中雙機(jī)器人關(guān)節(jié)模型根據(jù)SCARA 機(jī)器人實際參數(shù)建立，其仿真效果與實際工況理論上一致，模型如圖3所示。

圖3 雙SCARA關(guān)節(jié)模型

工作空間指機(jī)器人末端執(zhí)行器能夠到達(dá)的工作范圍［8］。文中采用蒙特卡羅法求取雙SCARA 機(jī)器人的協(xié)作空間，在Robotics toolbox工具箱中運用蒙特卡羅法求取雙機(jī)器人工作空間。根據(jù)正運動學(xué)方程解出機(jī)器人末端坐標(biāo)（px，py，pz）,用Rand 函數(shù)產(chǎn)生關(guān)節(jié)角度變量隨機(jī)值：

式中：θimin為第i個關(guān)節(jié)的最小轉(zhuǎn)動角度；θimax為第i個關(guān)節(jié)的最大轉(zhuǎn)動角度；N為生成的隨機(jī)點的數(shù)量，取值100 000。利用MATLAB軟件，用打點的方式畫出1～2號機(jī)器人的工作空間，如圖4所示。重疊部分為雙機(jī)協(xié)調(diào)工作空間，由圖4可得雙機(jī)器人協(xié)作空間為x∈［35,43］，y∈［-18,12］，z∈［0,30］。

圖4 雙機(jī)器人工作空間

2 雙機(jī)器人系統(tǒng)控制設(shè)計

2.1 控制系統(tǒng)方案設(shè)計

控制系統(tǒng)主要由SCARA機(jī)器人、西門子PLC、末端氣動吸取裝置及上位機(jī)組成。PLC 負(fù)責(zé)控制機(jī)器人的動作運行，上位機(jī)負(fù)責(zé)實現(xiàn)雙機(jī)器人自動避讓功能和實時顯示機(jī)器人運動過程中的位置信息。在雙機(jī)器人協(xié)作搬運物料過程中，要求1號機(jī)器人末端氣動裝置夾持物料至協(xié)調(diào)工作空間內(nèi)指定位置，放下物料后返回，2 號機(jī)器人到上述指定位置取料。采用2臺實驗室三自由度SCARA 機(jī)器人構(gòu)成協(xié)作平臺，完成協(xié)調(diào)搬運作業(yè)。

整個控制系統(tǒng)分為2 級，第1 級為上位機(jī)與雙機(jī)器人控制器間的通信與控制，第2 級為2 個PLC控制器對各自機(jī)器人本體的控制。將2 臺PLC 通過以太網(wǎng)與計算機(jī)相連，基于C#編寫的上位機(jī)通過S7協(xié)議與PLC建立通信，采集解析PLC數(shù)據(jù)，在上位機(jī)界面顯示，通過上位機(jī)向PLC 發(fā)送命令，PLC反解程序?qū)⑵矫嫖恢脭?shù)據(jù)轉(zhuǎn)換為脈沖，通過驅(qū)動器發(fā)送給步進(jìn)電機(jī)，步進(jìn)電機(jī)將脈沖信號轉(zhuǎn)換為角度信號，控制機(jī)械臂各軸運動。

2.2 PLC程序設(shè)計

根據(jù)實際工況，在博圖軟件運動控制向?qū)е薪M態(tài)速度參數(shù)，通過設(shè)置合理的加減速參數(shù)和急停時間參數(shù)，使設(shè)備平穩(wěn)運行。由于步進(jìn)電機(jī)通過脈沖轉(zhuǎn)換為步距角，控制機(jī)器人動作，因此選擇脈沖作為度量單位，機(jī)器人步進(jìn)電機(jī)初始步距角為1.8°，為保證機(jī)器人的運行精度，設(shè)定關(guān)節(jié)3驅(qū)動器8細(xì)分，結(jié)合機(jī)器人自身傳動比，修改后關(guān)節(jié)3 步進(jìn)電機(jī)步距角為0.028°；根據(jù)逆運動學(xué)求解時多移動小關(guān)節(jié)少移動大關(guān)節(jié)原則，設(shè)定關(guān)節(jié)2 驅(qū)動器16 細(xì)分，修改后關(guān)節(jié)2 步進(jìn)電機(jī)步距角為0.014°。經(jīng)運動學(xué)正解計算，機(jī)器人末端水平X方向運行精度為±0.065 mm，水平Y(jié)方向運行精度為±0.25 mm，運行精度高，運動可靠。雙機(jī)器人系統(tǒng)各關(guān)節(jié)運行參數(shù)，如表2 所示。PLC 程序采用模塊化編程，程序由1個主程序和若干個子程序組成，通過主程序和子程序的互相配合，使2個機(jī)器人按照設(shè)定路徑循環(huán)運動［9］。功能子程序包括軸初始化程序、建立零點程序、手動控制程序、自動控制程序、反解程序和正解程序。圖5為雙機(jī)器人協(xié)調(diào)運動實物圖。

表2 各關(guān)節(jié)參數(shù)

圖5 雙機(jī)器人協(xié)調(diào)運動

2.3 上位機(jī)功能設(shè)計

上位機(jī)開啟2 個線程，分別與2 臺PLC 建立通信。上位機(jī)作為客戶端，PLC 作為服務(wù)端，服務(wù)端先在內(nèi)核中創(chuàng)建socket結(jié)構(gòu)體，使當(dāng)前進(jìn)程與網(wǎng)卡建立聯(lián)系，然后綁定IP地址和端口號，主動監(jiān)聽該端口，處理監(jiān)聽狀態(tài)?？蛻舳讼蚍?wù)端發(fā)送報文，進(jìn)行連接請求，等待服務(wù)端回復(fù)報文，接受請求，建立客戶端連接，客戶端確認(rèn)連接建立后，完成握手。實現(xiàn)客戶端對服務(wù)端位置數(shù)據(jù)的讀寫，程序運行完成后關(guān)閉socket，將資源釋放。上位機(jī)連接PLC設(shè)備后，需要定時循環(huán)讀取PLC數(shù)據(jù)。為保證數(shù)據(jù)實時有效，在上位機(jī)與下位機(jī)通信成功后，開啟1 個線程不斷地讀取下位機(jī)數(shù)據(jù)。為解決上位機(jī)讀寫操作時的資源共享問題，在上位機(jī)讀寫程序中創(chuàng)建線程鎖，確保寫操作程序運行時不會被讀取線程中斷，避免上位機(jī)崩潰，直到該代碼塊運行完成后，釋放該線程鎖［11］。

上位機(jī)界面如圖6 所示，從功能上可劃分為4個部分：初始化操作，包括連接PLC，設(shè)置IP 地址、端口號、顯示當(dāng)前連接狀態(tài)等；實時顯示，可以實時顯示機(jī)器人的位置信息；參數(shù)修改，可以改變機(jī)器人各關(guān)節(jié)的位移值和速度值；運動控制，能夠控制機(jī)器人自動運行、點動運行、回歸原點等操作。

圖6 上位機(jī)界面

2.4 實驗驗證

將對2臺PLC進(jìn)行數(shù)據(jù)采集的程序封裝成2個類，在主窗體類中創(chuàng)建2 個數(shù)據(jù)采集類的對象，在上位機(jī)中實現(xiàn)2個PLC的數(shù)據(jù)交互，將MATLAB中求得的雙機(jī)協(xié)調(diào)工作空間作為雙機(jī)協(xié)作機(jī)器人的干涉空間。通過運動學(xué)正解，在上位機(jī)中獲得機(jī)器人末端執(zhí)行器的位置，運行時雙機(jī)器人末端x、y坐標(biāo)變化曲線見圖7。由圖7 可看出，在75 s 時，2 個機(jī)器人的末端坐標(biāo)分別為（35，-18）和（48，-20），1號機(jī)器人的末端先于2 號機(jī)器人末端進(jìn)入干涉空間，此時，上位機(jī)執(zhí)行軸暫停命令，使2號機(jī)器人停止當(dāng)前運動并減速停止；在115 s時，2個機(jī)器人的末端坐標(biāo)分別為（35，-18）和（48，-20），1號機(jī)器人離開干涉空間，上位機(jī)執(zhí)行軸啟動命令，使2 號機(jī)器人繼續(xù)運行。同理，當(dāng)2號機(jī)器人先進(jìn)入干涉空間時，暫停1號機(jī)器人運動。圖7中雙SCARA機(jī)器人移動軌跡變化平緩，可在干涉區(qū)間內(nèi)進(jìn)行避讓，沒有發(fā)生碰撞，說明控制系統(tǒng)設(shè)計合理，運行可靠。

圖7 雙機(jī)器人末端x、y坐標(biāo)變化曲線

3 結(jié)論

建立了雙SCARA 機(jī)器人運動學(xué)模型，推導(dǎo)其正逆解，運用蒙特卡羅法求出了雙機(jī)器人協(xié)作空間。編寫了PLC 控制程序，開發(fā)上位機(jī)控制軟件，根據(jù)干涉空間范圍設(shè)計了雙機(jī)器人避讓策略，實現(xiàn)了雙機(jī)器人的有效避讓，運行可靠。