高俊东 刘雨佳 林忠文 余彬炀

摘 要：并联机器人具有高动态响应性、高刚度、高精度的优点，可以广泛应用在烟草企业异型烟分拣线上以解决效率和成本的问题。本文以Tripod并联机器人为控制对象，提出了一种基于倍福公司TwinCAT的电控系统解决方案，重点阐述了基于运动学模型的机器人算法的实现方式。

关键词：并联机器人;TwinCAT;运动学模型;电控系统

面对烟草行业亟待解决的效率和成本问题，具有高动态响应性、高刚度、高精度的并联机器人在烟草企业异型烟分拣线上得到广泛应用[1-2]。本文在Tripod并联机器人运动学分析的基础上[3-4]，对Tripod并联机器人的电控系统进行研究。对Tripod并联机器人的运动控制，可基于位置逆解算法对三个交流伺服电机进行插补控制[5-6]，且由于并联机构是非线性、高耦合的复杂机构，所以对电控系统的控制器、伺服驱动系统性能具有更高的要求。

1 硬件设计

电控系统的硬件设计，从控制器到I/O模块，再到伺服驱动系统都是通过EtherCAT总线[8]链接。

2 软件设计

倍福公司的TwinCAT3软件是电控系统的核心部分[7-8]。软件设计总体架构，包含三个模块：机器人算法模块（运动学位置正解和逆解）、NC运算模块（轨迹规划和伺服控制）、PLC模块（逻辑控制和运动控制的主控）。机器人算法模块由MTALAB Simulink调试完成后，通过TE1400组件生成。上位人机交互界面采用VB开发，通过TwinCAT3提供的ADS-OCX控件与TwinCAT3进行通讯。

Tripod并联机器人运动控制时，首先在NC轴配置中新建关节轴坐标系中的伺服实轴A1、A2、A3和笛卡尔坐标系中的三个虚轴x、y、z，把伺服实轴链接到伺服驱动器，把虚轴链接到PLC。伺服实轴的编码器位置反馈值链接到正解模块的输入，经过位置正解运算，就可得到末端TCP在笛卡尔空间中的当前位置，为运动控制提供当前坐标。

在轨迹控制过程中，主要是使用逆解模块。例如，在PLC程序中调用TwinCAT3运动控制库中的电子凸轮功能块，当功能块被激活后，基于给定的轨迹要求，NC运算核会规划出具体的运动路径，每隔2ms的时间间隔计算出下一路径点，把路径点的笛卡尔空间位置坐标                    链接到逆解模块的输入，实时计算相应的关节轴位置坐标                        ，并通过NC发送给伺服驱动器。基于NC运算核和机器人算法模块，每隔2ms，伺服驱动器就会收到一个位置数据，并控制电机运转到这一位置，当循环 次后，末端TCP就完成了给定轨迹。

2.1机器人算法模块设计

根据Tripod并联机器人的位置逆解和位置正解[3-4]，在MATLAB Simulink中使用MATLAB Function函数编写算法程序，为机器人本体的机械参数，（x，y，z）和（A1，A2，A3）为对应的TCP笛卡尔空间位置坐标和关节轴坐标，X0设置关节轴坐标系的原点偏置，X0确定关节轴坐标系的正负方向， 为错误标志。

假设TCP当前位置（x，y，z）=（50，150，785）、X0=401.7mm、X0=1，比较MATLAB和TwinCAT3中位置逆解结果：（A1，A2，A3）=（-12.86，-93.37，17.38），证明机器人算法模块的实现方式是正确的。在实际应用中，将（A1，A2，A3）与NC伺服实轴链接，（x，y，z）与NC虚轴链接，其余的输入输出与PLC相关变量链接，就可以将机器人算法模块进行灵活调用，实现Tripod并联机器人的运动控制。

2.2上位人机交互界面设计

本文使用VB进行上位人机交互界面设计，并通过ADS-OCX控件[8]与TwinCAT3进行通讯。

3 结论

本文提出了一种基于TwinCAT3的Tripod并联机器人电控系统的解决方案，搭建了硬件设计和软件设计框架，详细设计了机器人算法模块，并阐述了运动控制的实现方式。最后，根据人机交互需求，采用VB完成了上位交互界面的设计，为开发自主开放的机器人控制系统奠定了坚实基础。未来，将以本文为基础，对机器人视觉技术、动力学模型、相关控制策略[9-10]的综合应用进行深入研究，实现Tripod并联机器人的运动性能提升和应用范围扩展。

参考文献：

[1]任志刚. 工业机器人的发展现状及发展趋势[J]. 装备制造技术， 2015（3）：166-168.

[2]穆剑桥， 王智斌. 工业机器人的发展及现状综述[J]. 电子制作， 2015（18）：81-82.

[3]刘昆. 3-PSS并联机器人运动学分析及仿真[D]. 重庆大学， 2015.

[4]Jingjun Zhang. A Method for Obtaining Direct and Inverse Pose Solutions to Delta Parallel  Robot Based on ADAMS[C]. IEEE International Conference on Mechatronics and Automation， 2009：1332-1336.

[5]顾文昊， 肖武运， 张志强， 等. 基于Beckhoff平台Delta并联机构轨迹规划的实现[J]. 电脑知识与技术， 2014， 10（27）： 83-86.

[6]楊中山. 基于IPC的电动缸实验平台测控系统设计[D]. 重庆大学，  2014.

[7]王进， 郭帅， 聂松亮. 基于TwinCAT3的Stewart平台控制系统设计[J]. 自动化博览， 2015（9）：82-85.

[8]黎妞. 基于EtherCAT的伺服运动控制系统研究[D]. 武汉科技大学， 2012.

[9]毛洪国. 基于动力学模型的DELTA机器人运动控制研究[D]. 哈尔滨工业大学， 2014.

[10]郭超. 高速并联机器人及其控制系统研究[D]. 山东理工大学， 2014.