基于軟PLC的Delta機(jī)器人運(yùn)動(dòng)控制設(shè)計(jì)及實(shí)現(xiàn)

李忠浪，陳 忠

（華南理工大學(xué)機(jī)械與汽車工程學(xué)院，廣州 510640）

0 引言

Delta機(jī)器人是一種能夠?qū)崿F(xiàn)高速三自由度平動(dòng)的機(jī)構(gòu)。和串聯(lián)機(jī)器人不同，Delta機(jī)器人因剛度大、精度高、響應(yīng)快、末端件慣性小、動(dòng)作靈活、易實(shí)現(xiàn)高速等特點(diǎn)，被廣泛應(yīng)用于食品（巧克力、糖果、餅干、月餅等）、醫(yī)藥、電子元件等行業(yè)的高速分揀、裝箱操作[1]。運(yùn)動(dòng)控制技術(shù)作為機(jī)器人的大腦指揮，是機(jī)器人能穩(wěn)定運(yùn)行的關(guān)鍵技術(shù)，研究和發(fā)展機(jī)器人的運(yùn)動(dòng)控制技術(shù)對(duì)于推動(dòng)“中國制造2025”時(shí)代的來臨十分必要。

目前，常見的工業(yè)機(jī)器人的運(yùn)動(dòng)控制方式有兩種。一種是采用工控機(jī)和控制卡的方式，控制卡基于DSP+FPGA架構(gòu)，自帶PCI接口，具有比較強(qiáng)大的計(jì)算能力和邏輯處理能力，并且集成了連續(xù)插補(bǔ)、圓弧插補(bǔ)等功能，但是缺點(diǎn)明顯，不僅在軟件上二次開發(fā)復(fù)雜，在硬件上系統(tǒng)布線多，可靠性一般，遷移性差，兼容性差，而且不易和其他設(shè)備集成在一個(gè)控制系統(tǒng)，不利于機(jī)器人針對(duì)不同場(chǎng)景的快速開發(fā)和普及[2]。另一種方式是基于軟PLC的控制方式。軟PLC是使用個(gè)人計(jì)算機(jī)或嵌入式控制器作為硬件支撐平臺(tái)，利于軟件實(shí)現(xiàn)硬PLC的基本功能。和控制卡方式相比，軟PLC具有開放的體系結(jié)構(gòu)，可以在不同的硬件環(huán)境下使用；開發(fā)方便，可維護(hù)性強(qiáng)，利用軟件開發(fā)更為豐富的指令集并支持多種編程語言；不依賴于傳統(tǒng)廠商的技術(shù)壟斷，開放性強(qiáng)[3]。目前常用的開發(fā)平臺(tái)有3S公司的CodeSys，倍福公司的TwinCAT，固高公司的OtoStudio等。杭州電子科技大學(xué)何潔[4]對(duì)軟PLC運(yùn)動(dòng)控制系統(tǒng)可靠性進(jìn)行了研究分析；華中科技大學(xué)周明華[5]在Beremiz平臺(tái)上研究新一代數(shù)控系統(tǒng)原型；華南理工大學(xué)李迪團(tuán)隊(duì)[6]以自動(dòng)磨槽機(jī)為控制對(duì)象，驗(yàn)證了軟PLC運(yùn)動(dòng)控制算法。目前，對(duì)于軟PLC在Delta工業(yè)機(jī)器人的應(yīng)用開發(fā)還比較少。

本文從工廠實(shí)際需要出發(fā)，硬件上采用ARM+Windows+RunTime System的硬件控制方案，軟件開發(fā)平臺(tái)采用3S公司的CodeSys，編程語言采用結(jié)構(gòu)化文本和連續(xù)功能圖兩種方式，以CodeSys內(nèi)嵌的運(yùn)動(dòng)控制庫SoftMotionCNC為基礎(chǔ)，進(jìn)行二次開發(fā)，包括點(diǎn)到點(diǎn)插補(bǔ)運(yùn)動(dòng)、直線插補(bǔ)運(yùn)動(dòng)、圓弧插補(bǔ)運(yùn)動(dòng)等，并通過模塊之間的組合實(shí)現(xiàn)Delta機(jī)器人的連續(xù)運(yùn)動(dòng)。為了操作方便，開發(fā)出相對(duì)應(yīng)的運(yùn)動(dòng)控制指令、在可輸入界面進(jìn)行指令編程即可，克服了傳統(tǒng)控制卡控制復(fù)雜、拓展性差的缺點(diǎn)。最后，采用3-4-5多項(xiàng)式軌跡規(guī)劃出的門型路徑[7]，在Delta機(jī)器人上機(jī)實(shí)驗(yàn)，結(jié)果表明機(jī)器人能穩(wěn)定運(yùn)行并且各方向加速度與理論加速度一致，證明了機(jī)器人運(yùn)動(dòng)控制設(shè)計(jì)成功。

1 控制系統(tǒng)方案設(shè)計(jì)

Delta機(jī)器人控制系統(tǒng)采用一主多從的控制模式，主站工控機(jī)采用ARM+Window控制方案，從站采用帶EtherCAT接口的清能德創(chuàng)CoolDriveRC4伺服驅(qū)動(dòng)器，主站與從站之間采用工業(yè)以太網(wǎng)EtherCAT進(jìn)行數(shù)據(jù)通訊。軟件開發(fā)平臺(tái)在PC端采用CodeSys編程環(huán)境，編程語言采用結(jié)構(gòu)化文本和連續(xù)功能圖兩種方式，編譯通過后固化到工控機(jī)上，直接發(fā)送指令控制機(jī)器人的運(yùn)動(dòng)。機(jī)器人控制系統(tǒng)模型如圖1所示。

圖1 機(jī)器人控制系統(tǒng)模型圖

SoftMotionCNC基礎(chǔ)庫中，主要用到的功能塊有插補(bǔ)控制類和正逆運(yùn)動(dòng)學(xué)變換類，其中插補(bǔ)控制類常用的有SMC_CheckVelocitics、 SMC_Smoothpath、 SMC_Interpolator、SMC_ControlAxisByPos，數(shù)組有SMC_PSINFO，結(jié)構(gòu)體有SMC_OUTQUEUE，Delta機(jī)器人正逆運(yùn)動(dòng)學(xué)變換模塊有SMC_TRAFOF_Tripod、SMC_TRAFO_Tripod。熟悉這些功能塊的作用后，按照一定的規(guī)則組合出想要的運(yùn)動(dòng)控制模塊。

2 運(yùn)動(dòng)控制功能塊設(shè)計(jì)

2.1 點(diǎn)到點(diǎn)的運(yùn)動(dòng)控制功能塊設(shè)計(jì)

機(jī)器人點(diǎn)到點(diǎn)運(yùn)動(dòng)控制是控制機(jī)器人的末端從起始點(diǎn)到給定點(diǎn)，只需考慮時(shí)間最短，不考慮中間的軌跡形狀。根據(jù)功能塊間的相互聯(lián)系，Delta機(jī)器人點(diǎn)到點(diǎn)運(yùn)動(dòng)控制功能塊設(shè)計(jì)流程如圖2所示，點(diǎn)到點(diǎn)運(yùn)動(dòng)控制封裝圖如圖3所示。

圖2 點(diǎn)到點(diǎn)運(yùn)動(dòng)控制流程圖

圖3 點(diǎn)到點(diǎn)運(yùn)動(dòng)控制封裝圖

2.2 圓弧插補(bǔ)運(yùn)動(dòng)控制功能塊設(shè)計(jì)

機(jī)器人圓弧插補(bǔ)運(yùn)動(dòng)控制是控制機(jī)器人的末端從起始點(diǎn)，經(jīng)過中間點(diǎn)，到達(dá)目的點(diǎn)，點(diǎn)與點(diǎn)之間的軌跡通過圓弧插補(bǔ)得到。圓弧插補(bǔ)功能塊流程圖如圖4所示，圓弧插補(bǔ)運(yùn)動(dòng)控制封裝圖如圖5所示。

圖4 圓弧插補(bǔ)運(yùn)動(dòng)控制流程圖

圖5 圓弧插補(bǔ)運(yùn)動(dòng)控制封裝圖

2.3 直線插補(bǔ)運(yùn)動(dòng)控制功能塊設(shè)計(jì)

直線插補(bǔ)運(yùn)動(dòng)控制是控制機(jī)器人末端從起點(diǎn)直線運(yùn)動(dòng)到目的點(diǎn)，嚴(yán)格遵守直線準(zhǔn)則。直線插補(bǔ)功能塊流程圖如圖6所示，直線插補(bǔ)運(yùn)動(dòng)控制封裝圖如圖7所示。

圖6 直線插補(bǔ)運(yùn)動(dòng)控制流程圖

圖7 直線插補(bǔ)運(yùn)動(dòng)控制封裝圖

2.4 連續(xù)運(yùn)動(dòng)控制功能塊設(shè)計(jì)

連續(xù)運(yùn)動(dòng)控制是控制機(jī)器人末端連續(xù)運(yùn)動(dòng)一段或多段軌跡到達(dá)目的點(diǎn)的運(yùn)動(dòng)控制模式，由點(diǎn)到點(diǎn)、圓弧插補(bǔ)、直線插補(bǔ)控制方式相互組合完成，并借助指令控制模塊關(guān)聯(lián)功能塊，對(duì)編程進(jìn)行簡(jiǎn)化。例如本文設(shè)計(jì)中，用PTP（）函數(shù)實(shí)現(xiàn)點(diǎn)對(duì)點(diǎn)功能，用LIN（）函數(shù)實(shí)現(xiàn)直線插補(bǔ)功能，用Circ（）實(shí)現(xiàn)圓弧插補(bǔ)功能。實(shí)驗(yàn)部分用了3個(gè)指令組合實(shí)現(xiàn)了門型軌跡的拾取動(dòng)作。路徑圖如圖8所示，程序設(shè)計(jì)如圖9所示。

圖8 路徑規(guī)劃示意圖

圖9 程序設(shè)計(jì)示意圖

其中，函數(shù)內(nèi)每個(gè)定義點(diǎn)包含的信息有點(diǎn)的坐標(biāo)、對(duì)應(yīng)的角速度、角加減速度。

3 實(shí)驗(yàn)驗(yàn)證

本文選取實(shí)驗(yàn)室自主搭建的Delta并聯(lián)機(jī)器人樣機(jī)為實(shí)驗(yàn)對(duì)象，編寫運(yùn)動(dòng)控制程序?qū)崿F(xiàn)門型軌跡拾取動(dòng)作，并利用LMS動(dòng)態(tài)測(cè)試儀和BK型加速度傳感器，測(cè)出機(jī)器人末端的加速度信號(hào)。測(cè)試方法參考喬正宇[8]。實(shí)驗(yàn)平臺(tái)如圖10所示，硬件型號(hào)如表1所示。

測(cè)試軌跡采用規(guī)劃好的3-4-5次多項(xiàng)式：

圖10 實(shí)驗(yàn)平臺(tái)

式中：amax=5.773 5·S/T2，據(jù)實(shí)際場(chǎng)景，設(shè)S=0.8m，amax=2m/s2。

表1 實(shí)驗(yàn)硬件型號(hào)

計(jì)算出8個(gè)代表點(diǎn)的信息，進(jìn)行運(yùn)動(dòng)編程。將加速度傳感器測(cè)試得到的X、Z坐標(biāo)軸方向的加速度和理論加速度進(jìn)行對(duì)比，如圖11、圖12所示。

圖11 軌跡平面X方向加速度

圖12 軌跡平面Z方向加速度

實(shí)驗(yàn)結(jié)果表明，機(jī)器人能穩(wěn)定運(yùn)行并且各方向加速度與理論加速度一致，證明了機(jī)器人運(yùn)動(dòng)控制設(shè)計(jì)成功。

4 結(jié)束語

與傳統(tǒng)控制卡運(yùn)動(dòng)控制相比，本文提出的方法在機(jī)器人使用上更加簡(jiǎn)便，編程簡(jiǎn)單，靈活性更高。在實(shí)際應(yīng)用過程中，可以很方便地把程序遷移到任何一臺(tái)嵌入式工控機(jī)上，不受產(chǎn)商品牌間兼容性的影響，大大加快開發(fā)速度。針對(duì)不同構(gòu)型的機(jī)器人，只需局部替換其中的運(yùn)動(dòng)模塊即可，減小開發(fā)難度，提高效率。