基于軟PLC的Delta機器人運動控制設計及實現

李忠浪，陳 忠

（華南理工大學機械與汽車工程學院，廣州 510640）

0 引言

Delta機器人是一種能夠實現高速三自由度平動的機構。和串聯機器人不同，Delta機器人因剛度大、精度高、響應快、末端件慣性小、動作靈活、易實現高速等特點，被廣泛應用于食品（巧克力、糖果、餅干、月餅等）、醫藥、電子元件等行業的高速分揀、裝箱操作[1]。運動控制技術作為機器人的大腦指揮，是機器人能穩定運行的關鍵技術，研究和發展機器人的運動控制技術對于推動“中國制造2025”時代的來臨十分必要。

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

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

1 控制系統方案設計

Delta機器人控制系統采用一主多從的控制模式，主站工控機采用ARM+Window控制方案，從站采用帶EtherCAT接口的清能德創CoolDriveRC4伺服驅動器，主站與從站之間采用工業以太網EtherCAT進行數據通訊。軟件開發平臺在PC端采用CodeSys編程環境，編程語言采用結構化文本和連續功能圖兩種方式，編譯通過后固化到工控機上，直接發送指令控制機器人的運動。機器人控制系統模型如圖1所示。

圖1 機器人控制系統模型圖

SoftMotionCNC基礎庫中，主要用到的功能塊有插補控制類和正逆運動學變換類，其中插補控制類常用的有SMC_CheckVelocitics、 SMC_Smoothpath、 SMC_Interpolator、SMC_ControlAxisByPos，數組有SMC_PSINFO，結構體有SMC_OUTQUEUE，Delta機器人正逆運動學變換模塊有SMC_TRAFOF_Tripod、SMC_TRAFO_Tripod。熟悉這些功能塊的作用后，按照一定的規則組合出想要的運動控制模塊。

2 運動控制功能塊設計

2.1 點到點的運動控制功能塊設計

機器人點到點運動控制是控制機器人的末端從起始點到給定點，只需考慮時間最短，不考慮中間的軌跡形狀。根據功能塊間的相互聯系，Delta機器人點到點運動控制功能塊設計流程如圖2所示，點到點運動控制封裝圖如圖3所示。

圖2 點到點運動控制流程圖

圖3 點到點運動控制封裝圖

2.2 圓弧插補運動控制功能塊設計

機器人圓弧插補運動控制是控制機器人的末端從起始點，經過中間點，到達目的點，點與點之間的軌跡通過圓弧插補得到。圓弧插補功能塊流程圖如圖4所示，圓弧插補運動控制封裝圖如圖5所示。

圖4 圓弧插補運動控制流程圖

圖5 圓弧插補運動控制封裝圖

2.3 直線插補運動控制功能塊設計

直線插補運動控制是控制機器人末端從起點直線運動到目的點，嚴格遵守直線準則。直線插補功能塊流程圖如圖6所示，直線插補運動控制封裝圖如圖7所示。

圖6 直線插補運動控制流程圖

圖7 直線插補運動控制封裝圖

2.4 連續運動控制功能塊設計

連續運動控制是控制機器人末端連續運動一段或多段軌跡到達目的點的運動控制模式，由點到點、圓弧插補、直線插補控制方式相互組合完成，并借助指令控制模塊關聯功能塊，對編程進行簡化。例如本文設計中，用PTP（）函數實現點對點功能，用LIN（）函數實現直線插補功能，用Circ（）實現圓弧插補功能。實驗部分用了3個指令組合實現了門型軌跡的拾取動作。路徑圖如圖8所示，程序設計如圖9所示。

圖8 路徑規劃示意圖

圖9 程序設計示意圖

其中，函數內每個定義點包含的信息有點的坐標、對應的角速度、角加減速度。

3 實驗驗證

本文選取實驗室自主搭建的Delta并聯機器人樣機為實驗對象，編寫運動控制程序實現門型軌跡拾取動作，并利用LMS動態測試儀和BK型加速度傳感器，測出機器人末端的加速度信號。測試方法參考喬正宇[8]。實驗平臺如圖10所示，硬件型號如表1所示。

測試軌跡采用規劃好的3-4-5次多項式：

圖10 實驗平臺

式中：amax=5.773 5·S/T2，據實際場景，設S=0.8m，amax=2m/s2。

表1 實驗硬件型號

計算出8個代表點的信息，進行運動編程。將加速度傳感器測試得到的X、Z坐標軸方向的加速度和理論加速度進行對比，如圖11、圖12所示。

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

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

實驗結果表明，機器人能穩定運行并且各方向加速度與理論加速度一致，證明了機器人運動控制設計成功。

4 結束語

與傳統控制卡運動控制相比，本文提出的方法在機器人使用上更加簡便，編程簡單，靈活性更高。在實際應用過程中，可以很方便地把程序遷移到任何一臺嵌入式工控機上，不受產商品牌間兼容性的影響，大大加快開發速度。針對不同構型的機器人，只需局部替換其中的運動模塊即可，減小開發難度，提高效率。