李忠浪,陳 忠
(華南理工大學機械與汽車工程學院,廣州 510640)
Delta機器人是一種能夠?qū)崿F(xiàn)高速三自由度平動的機構(gòu)。和串聯(lián)機器人不同,Delta機器人因剛度大、精度高、響應快、末端件慣性小、動作靈活、易實現(xiàn)高速等特點,被廣泛應用于食品(巧克力、糖果、餅干、月餅等)、醫(yī)藥、電子元件等行業(yè)的高速分揀、裝箱操作[1]。運動控制技術(shù)作為機器人的大腦指揮,是機器人能穩(wěn)定運行的關鍵技術(shù),研究和發(fā)展機器人的運動控制技術(shù)對于推動“中國制造2025”時代的來臨十分必要。
目前,常見的工業(yè)機器人的運動控制方式有兩種。一種是采用工控機和控制卡的方式,控制卡基于DSP+FPGA架構(gòu),自帶PCI接口,具有比較強大的計算能力和邏輯處理能力,并且集成了連續(xù)插補、圓弧插補等功能,但是缺點明顯,不僅在軟件上二次開發(fā)復雜,在硬件上系統(tǒng)布線多,可靠性一般,遷移性差,兼容性差,而且不易和其他設備集成在一個控制系統(tǒng),不利于機器人針對不同場景的快速開發(fā)和普及[2]。另一種方式是基于軟PLC的控制方式。軟PLC是使用個人計算機或嵌入式控制器作為硬件支撐平臺,利于軟件實現(xiàn)硬PLC的基本功能。和控制卡方式相比,軟PLC具有開放的體系結(jié)構(gòu),可以在不同的硬件環(huán)境下使用;開發(fā)方便,可維護性強,利用軟件開發(fā)更為豐富的指令集并支持多種編程語言;不依賴于傳統(tǒng)廠商的技術(shù)壟斷,開放性強[3]。目前常用的開發(fā)平臺有3S公司的CodeSys,倍福公司的TwinCAT,固高公司的OtoStudio等。杭州電子科技大學何潔[4]對軟PLC運動控制系統(tǒng)可靠性進行了研究分析;華中科技大學周明華[5]在Beremiz平臺上研究新一代數(shù)控系統(tǒng)原型;華南理工大學李迪團隊[6]以自動磨槽機為控制對象,驗證了軟PLC運動控制算法。目前,對于軟PLC在Delta工業(yè)機器人的應用開發(fā)還比較少。
本文從工廠實際需要出發(fā),硬件上采用ARM+Windows+RunTime System的硬件控制方案,軟件開發(fā)平臺采用3S公司的CodeSys,編程語言采用結(jié)構(gòu)化文本和連續(xù)功能圖兩種方式,以CodeSys內(nèi)嵌的運動控制庫SoftMotionCNC為基礎,進行二次開發(fā),包括點到點插補運動、直線插補運動、圓弧插補運動等,并通過模塊之間的組合實現(xiàn)Delta機器人的連續(xù)運動。為了操作方便,開發(fā)出相對應的運動控制指令、在可輸入界面進行指令編程即可,克服了傳統(tǒng)控制卡控制復雜、拓展性差的缺點。最后,采用3-4-5多項式軌跡規(guī)劃出的門型路徑[7],在Delta機器人上機實驗,結(jié)果表明機器人能穩(wěn)定運行并且各方向加速度與理論加速度一致,證明了機器人運動控制設計成功。
Delta機器人控制系統(tǒng)采用一主多從的控制模式,主站工控機采用ARM+Window控制方案,從站采用帶EtherCAT接口的清能德創(chuàng)CoolDriveRC4伺服驅(qū)動器,主站與從站之間采用工業(yè)以太網(wǎng)EtherCAT進行數(shù)據(jù)通訊。軟件開發(fā)平臺在PC端采用CodeSys編程環(huán)境,編程語言采用結(jié)構(gòu)化文本和連續(xù)功能圖兩種方式,編譯通過后固化到工控機上,直接發(fā)送指令控制機器人的運動。機器人控制系統(tǒng)模型如圖1所示。
圖1 機器人控制系統(tǒng)模型圖
SoftMotionCNC基礎庫中,主要用到的功能塊有插補控制類和正逆運動學變換類,其中插補控制類常用的有SMC_CheckVelocitics、 SMC_Smoothpath、 SMC_Interpolator、SMC_ControlAxisByPos,數(shù)組有SMC_PSINFO,結(jié)構(gòu)體有SMC_OUTQUEUE,Delta機器人正逆運動學變換模塊有SMC_TRAFOF_Tripod、SMC_TRAFO_Tripod。熟悉這些功能塊的作用后,按照一定的規(guī)則組合出想要的運動控制模塊。
機器人點到點運動控制是控制機器人的末端從起始點到給定點,只需考慮時間最短,不考慮中間的軌跡形狀。根據(jù)功能塊間的相互聯(lián)系,Delta機器人點到點運動控制功能塊設計流程如圖2所示,點到點運動控制封裝圖如圖3所示。
圖2 點到點運動控制流程圖
圖3 點到點運動控制封裝圖
機器人圓弧插補運動控制是控制機器人的末端從起始點,經(jīng)過中間點,到達目的點,點與點之間的軌跡通過圓弧插補得到。圓弧插補功能塊流程圖如圖4所示,圓弧插補運動控制封裝圖如圖5所示。
圖4 圓弧插補運動控制流程圖
圖5 圓弧插補運動控制封裝圖
直線插補運動控制是控制機器人末端從起點直線運動到目的點,嚴格遵守直線準則。直線插補功能塊流程圖如圖6所示,直線插補運動控制封裝圖如圖7所示。
圖6 直線插補運動控制流程圖
圖7 直線插補運動控制封裝圖
連續(xù)運動控制是控制機器人末端連續(xù)運動一段或多段軌跡到達目的點的運動控制模式,由點到點、圓弧插補、直線插補控制方式相互組合完成,并借助指令控制模塊關聯(lián)功能塊,對編程進行簡化。例如本文設計中,用PTP()函數(shù)實現(xiàn)點對點功能,用LIN()函數(shù)實現(xiàn)直線插補功能,用Circ()實現(xiàn)圓弧插補功能。實驗部分用了3個指令組合實現(xiàn)了門型軌跡的拾取動作。路徑圖如圖8所示,程序設計如圖9所示。
圖8 路徑規(guī)劃示意圖
圖9 程序設計示意圖
其中,函數(shù)內(nèi)每個定義點包含的信息有點的坐標、對應的角速度、角加減速度。
本文選取實驗室自主搭建的Delta并聯(lián)機器人樣機為實驗對象,編寫運動控制程序?qū)崿F(xiàn)門型軌跡拾取動作,并利用LMS動態(tài)測試儀和BK型加速度傳感器,測出機器人末端的加速度信號。測試方法參考喬正宇[8]。實驗平臺如圖10所示,硬件型號如表1所示。
測試軌跡采用規(guī)劃好的3-4-5次多項式:
圖10 實驗平臺
式中:amax=5.773 5·S/T2,據(jù)實際場景,設S=0.8m,amax=2m/s2。
表1 實驗硬件型號
計算出8個代表點的信息,進行運動編程。將加速度傳感器測試得到的X、Z坐標軸方向的加速度和理論加速度進行對比,如圖11、圖12所示。
圖11 軌跡平面X方向加速度
圖12 軌跡平面Z方向加速度
實驗結(jié)果表明,機器人能穩(wěn)定運行并且各方向加速度與理論加速度一致,證明了機器人運動控制設計成功。
與傳統(tǒng)控制卡運動控制相比,本文提出的方法在機器人使用上更加簡便,編程簡單,靈活性更高。在實際應用過程中,可以很方便地把程序遷移到任何一臺嵌入式工控機上,不受產(chǎn)商品牌間兼容性的影響,大大加快開發(fā)速度。針對不同構(gòu)型的機器人,只需局部替換其中的運動模塊即可,減小開發(fā)難度,提高效率。