作者：德国倍福自动化有限公司 王进，王建成，孔惠增

摘要：目前国内的平行四边形机器人主要用于码垛，其结构主要有混联式丝杠机构和双平行四边形旋转轴式机构，本文通过对这两种四边形机构的运动学分析，推导出四边形机器人的正逆解算法。再利用倍福TwinCAT软件的实时运算核运行正逆解算法，把轨迹规划功能块产生的笛卡尔坐标系位置值实时转化成关节坐标系位置值并利用外部给定功能通过EtherCAT总线实时地对电机和机器人进行控制。本文还通过对码垛机器人末端轨迹的分析利用TwinCAT软件的NCI插补功能开发了轨迹规划抓放功能块。结果显示通过对两种机器人结构的分析以及在TwinCAT平台的软件编程实现了码垛机器人的控制。

关键词: 码垛机器人；TwinCAT；NCI插补；抓放功能；PC控制

Abstract: At present, there are two types of mechanism of the domestic palletizing robots, i.e., the mixed gear and rack mechanism and the double parallel quadrilateral rotating axis mechanism. Through the kinematic analysis of the two palletizing mechanisms, this paper derives the positive and inverse algorithm of the palletizing robot. TwinCAT is implemented to solve the inverse algorithm in real time. Then the Descartes coordinate system generated by the trajectory planning function block is transformed into the position value of the joint coordinate system. At the same time, the motor and the robot are controlled by the EtherCAT bus by the motion control function block. By analyzing the terminal trajectory of palletizing robot, a trajectory planning and grasping function block is developed based on the NCI function of TwinCAT software. By analyzing the structure of the two types of robots and the software and hardware development of the TwinCAT platform,results show that the control of palletizing robot is realized.

Key words: Robot; TwinCAT; Pick and place; PC Control
1　引言

平行四边形机器人主要用于码垛行业，码垛机器人主要将包装袋或者箱体等按照一定次序码到托盘上，以便实现物料的搬运、装卸、存储、运输等物流活动。具有工作效率高，可靠性好，柔性好的特点，在粮油食品饲料化工等行业得到广泛应用。

图1 双平行四边形旋转轴式机器人

图2 混联式丝杠码垛机器人实物图

混联式丝杠码垛机器人是一种四自由度混联机构，它结构简单，价格低廉，性能优越，广泛应用于工业搬运和物流生产线，但受限于丝杠或齿条传动机构对负载的限制，难以用于重载机器人。双平行四边形旋转轴式机构主要用于重载搬运机器人，其特点是重复精度高、运行速度快、负载能力大、可靠性高等特点。但是相对于轻载机器人由于负载自重比其在本体设计和高动态性能实现方面难度较大^[1]。

本文对这两种机构进行了简单分析得到这两种机构的正逆解的算法，重点对如何在TwinCAT中实现机器人的运动控制设计和运动轨迹规划进行了论述，并在实际应用中通过监测该机器人运行的位置、速度以及加速度曲线对机器人的性能进行分析。

2 机构运动学分析

2.1 混联式丝杠机构分析

图3 混联式丝杠码垛机器人几何示意图

混联式丝杠机器人有四个关节运动：底座旋转（绕Z轴）、水平运动（X轴方向）、垂直上下运动（Z轴上下）和末端旋转运动。底座电机和末端旋转电机通过减速机驱动底座和机械手末端旋转。水平丝杠电机和垂直丝杠电机通过同步带轮带动水平丝杠导轨和垂直丝杠导轨运动。其导轨上的滑块带动前大臂和后大臂沿各自导轨做水平方向运动和垂直方向运动。

如图3所示，现设定一个固定坐标系O-xy，各关节坐标为A（0，zA）、C（xc，0）、B（xB，zB）、D（xD，zD）、E(xE，zE)、F（xF，zF），根据机构分析^[2]当满足杆长条件：ad=bc时，可大大简化主传动机构的运动学求解。这将使计算量减小，便于机器人运动控制设计。这时机械手的位置正解为：

对机构做逆运动学分析，可得逆解公式如下，其中θ为码垛机器人在搬运过程中箱子的角度。

2.2　双平行四边形旋转轴式机器人机构分析

图4 双平行四边形旋转轴式码垛机器人几何示意图

双平行四边形机构如图4所示，由两组平行四边形组成。其中红色所示的主平行四边形确定末端点位置，绿色的辅助平行四边形使末端执行器与水平面保持一个固定的姿态。驱动主平行四边形的电机被对称安装在转座上，其分别标为轴2、轴3，对应关节角度为θ2、 3 ；底座由一个电机驱动，记为轴1，对应关节角度为θ1，前三个关节确定末端点的位置。在手爪处加装一个电机，转角记为θ4，用以对所操作工件进行旋转。根据机构几何关系，可得末端执行器位置正解如下：^[1]

（3）式中x、y、z、θz为机器人末端在世界坐标系中的坐标和角度；

θ1、θ2、θ3、θ4、θ5 为机器人各关节角度。

由正解方程，可得逆解方程如下：

（5）

（6）

（7）θ4=θ2-θ1

（8）θ5=π-θ3

3　软件设计

3.1　PLC编程

根据上文的运动学分析可以在TwinCAT中写出机器人的正逆解函数功能块，图5是以混联式丝杠机构为例写出的正逆解函数。

图5 混联式丝杠机构正逆解函数

本文所设计的两种四边形机器人在笛卡尔坐标系中一共有四个自由度并且需要控制四个关节电机。在倍福的TwinCAT软件的NC中首先添加A1到A4四个关节坐标系轴并链接到驱动器和PLC，为了方便对笛卡尔坐标系轴操作再添加X、Y、Z、C四个虚拟轴并链接到PLC。对于正解是把四个伺服电机A1到A4的角度或者位置作为输入变量赋值给正解功能块，经过正解运算输出X、Y、Z、C的位置值作为机器人在笛卡尔坐标系中的位置。而逆解功能块将给定的X、Y、Z、C的位置和角度作为输入变量，通过逆解运算得到电机的角度值，通过外部给定功能块发给NC再发给伺服驱动器。^[3]

在PLC程序中，可以直接对笛卡尔坐标系中的X、Y 、Z 、C四个虚拟轴进行控制。调用TwinCAT软件的运动控制库中的PTP、NCI等运动控制功能，可以方便地让机器人移动到某一位姿或者进行连续循环运动。软件的具体编程步骤如图6所示的软件流程图。由于NC系统的路径规划功能强大，这些位置数据的一阶导数和二阶导数都是连续的从而保证了伺服驱动器的连续稳定运行。

图6 软件流程图

倍福的TwinCAT软件自带了HMI功能，界面可用于调试和用户操作，如图7所示。

图7 HMI界面

3.2　轨迹规划

NCI软件是倍福TwinCAT自带的插补运动控制软件，支持三轴插补以及五个辅助轴的运动，可以实现空间中的直线插补、圆弧插补、样条曲线等轨迹插补动作。码垛机器人有三个插补轴以及一个旋转轴，选择NCI做轨迹规划能满足码垛机器人的动作要求，同时减少了开发难度。

图8 NCI和G代码编程

四边形码垛机器人抓放动作通常是一个门型曲线，由三条平滑过渡的曲线组成，如图9所示。其在NCI中的G代码编程如图8所示，其中#setparamVertexSmoothing(R5)#是设定平滑过渡类型和过渡半径R5；

G01 Z=R2 F=R4 M80是从放置点运行到放置点上方，F=R4是指运行速度。Z=R2目标位置的Z轴坐标，所有R变量都可以在PLC中写入。M80是通过NCI的M函数来控制码垛机器人夹手关闭的变量；

G01 X=R10 Y=R11 Z=R12 Q1=R13 F=R14M8 1是从放置点上方运行到抓取点上方，X=R10Y=R11 Z=R12 Q1=R13是运行目标位置，F=R14是运行速度，M81是通过NCI的M函数来控制码垛机器人夹手打开的变量；

G01 Z=R22 F=R24 是从抓取点上方运行到抓取点。

所有的R参数都可以在PLC程序或者HMI中赋值。

图9 抓放曲线

4　运行和调试

本文以混联式丝杠四边形机器人为例进行运行测试，先移动到抓取位置（300,500,-300），再从当前位置走门型曲线运行到放置位置（0,-500,-400），其中Z轴最高提升到绝对高度0，首先可以在TwinCAT的HMI中进行示教和位置设置，如图10所示。

图10 示教点设置

然后可以在PLC程序中通过NCI的R变量读写功能把这些示教好的位置传递到NCI中，运行轨迹曲线如图11第一栏；X和Y以及Z轴的速度曲线如下图第二栏；X、Y、Z、A1、A2、A3轴的位置曲线如下图第三栏。

从TwinCAT自带的ScopeView示波器软件采集到的轨迹和速度位置数据可以看出，通过NCI编程可以直接对码垛机器人的笛卡尔坐标系轴进行操作，关节坐标系轴会按照逆解功能块所得到的角度值自动运行，实现了混联式丝杠四边形机器人的末端轨迹控制。对于双平行四边形旋转轴式机器人，按照所分析好的运动学算法，编写相应的正逆解功能块采用同样的方法也能实现相应的控制。

图11 运行效果图

5　结论

本文主要介绍了利用倍福TwinCAT软件编程实现四边形码垛机器人控制系统设计和实现的方法。首先介绍了两种常用码垛机器人运动学的正解和逆解算法，然后重点论述了对于本文的机器人正逆解算法怎么在TwinCAT软件中进行了编程、并给出了机器人控制的程序流程图，还介绍了一种利用倍福NCI插补软件实现机器人空间抓放轨迹的方法，最后在倍福TwinCAT软件中进行了PLC程序和操作界面编程，并结合硬件进行了实际运行和调试。运行结果表明本机器人软件设计可以对四边形码垛机器人进行笛卡尔坐标系轴点动操作、抓放轨迹操作等各种控制，也可以单独对关节坐标系轴操作。从而实现了在TwinCAT软件中对四边形码垛机器人的设计和控制，验证了运动学算法的正确性以及使用TwinCAT设计机器人的优越性，也为其他机器人模型在TwinCAT软件中的开发和程序编写提供了示范。

作者简介：

王进（1985-），男，河南驻马店人，现任德国倍福自动化有限公司运动控制产品经理，主要从事机器人和运动控制方面应用研发。

王建成（1975-），男，安徽黄山人，现任德国倍福自动化有限公司系统应用部经理。

孔惠增（1989-），男，河南南阳人，现任德国倍福自动化有限公司系统应用部应用工程师。

参考文献：

[1] 游玮. 双平行四边形高速重载搬运机器人动态设计与控制研究[D]. 哈尔滨工业大学, 2011.

[2] 张志强, 臧冀原. 混联码垛机器人运动学分析及仿真[J]. 机械设计, 2010, 11 (11) ：47 - 51.

[3] 王进, 郭帅. 基于TwinCAT3的Stewart平台控制系统设计[J]. 自动化博览，2015, 32 (9) ：82 - 85.

摘自《自动化博览》2018年9月刊