基于软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 结束语

与传统控制卡运动控制相比，本文提出的方法在机器人使用上更加简便，编程简单，灵活性更高。在实际应用过程中，可以很方便地把程序迁移到任何一台嵌入式工控机上，不受产商品牌间兼容性的影响，大大加快开发速度。针对不同构型的机器人，只需局部替换其中的运动模块即可，减小开发难度，提高效率。