基于TwinCAT的机器人虚拟控制系统设计

张炜岸 吴世林

(武汉纺织大学，湖北武汉430200)

近年来，随着《中国制造2025》行动纲领的提出，中国工业正有条不紊的向目标迈进。而如今，工业机器人不仅仅活跃在加工制造、食品包装等行业，也开始逐步被引入大学校园，成为学业学习的一部分。由于工业机器人技术的普及，且工业机器人的种类繁多，开发一种具有低成本、鲁棒性、高兼容性的机器人控制系统迫在眉睫。

其中以六轴的串联臂机器人应用最为广泛，而六轴工业机器人市场早已被“四大家族”分割[1］，整套纯进口的工业机器人价格是十分高昂的。因此，如果能够选用国产的驱控系统，便能大大减少预算和额外开支。除开国产驱动器选择性较多外，作为整个机器人控制系统的核心——控制器的选择则是重中之重。通过TwinCAT3可以将随手可见的个人计算机“改造”成一台性能精良的机器人控制器。

TwinCAT3是德国倍福公司发布的一款基于Windows操作系统的软件，同时兼容32位系统和64位系统。TwinCAT3将非实时的Windows系统转换为一个多核的实时控制系统，满足工业机器人对实时性、稳定性、安全性的要求。除了支持IEC 61131-3国际工业控制编程标准的低级编程语言之外，还支持例如C＋＋等高级语言的混合编写，为后续开发复杂的算法提供了富裕的空间与方法。

TwinCAT NC轴是通过现场总线与驱动器和测量系统循环交换数据来仿真的控制模块，由于个人计算机的处理能力十分强大，因此可以轻松地对多个运动轴进行定位和调试，通过优秀的定位算法，对生成轮廓时具有加加速度限制以及对速度和加速度的预控制，可以将跟随误差降到最小。

本文通过载有TwinCAT3的个人计算机作为机器人控制器，CPU为酷睿i5多核处理器，内装Windows 7 64位系统用以兼容Windows平台的其他软件。

1 运动学分析

六轴工业机器人通常指的是六关节串联机器人(图1)，根据右手定则，在轴一到轴六建立六个坐标系，其中坐标系{B}和坐标系{O}重合。

1.1 位置正解

串联机器人的运动学正解是已知当前各轴的角度求末端工作坐标。

根据连杆变换矩阵公式:

展开得到:

将这些连杆变换矩阵连乘:

色差仪，CR-400，日本Konica Minolta公司；质构仪，TA.XT Plus，英国SMS公司；手持式糖度计，PAL-1，日本ATAGO；游标卡尺，上海恒量量具有限公司；电热恒温鼓风干燥箱，DHG-9075A，上海齐欣科学仪器有限公司；蔬菜分辨器，台湾天烨集团有限公司。

随后经过计算便可得到坐标系{T}相对于坐标系{O}的变换矩阵[2］:

再分别计算矩阵中各元素的值即可。值得一提的是，矩阵中px、py及pz便是坐标系{T}相对于坐标系{B}位置参数。

因为运动学正解计算相对简单，完全可以用st语言进行编写，直接运行在TwinCAT PLC中，因此，可以把正解算法通过TwinCAT3的PLC模块封装成一个单独的函数功能块，从而方便用来计算机器人末端的坐标。

1.2 位置逆解

即已知点坐标，反求相应各轴的角度。

对于串联六轴机器人，求逆解的方式存在多种，在这里采用代数解法进行求解.但是由于求解过程的复杂程度高于正解，并且存在多解的情况。因此，需要根据实际情况对多解进行筛选从而选择最为合理的逆解角度，比如六轴进行耦合之后的工业机器人各轴运动范围是受限制的，因此首先要舍弃超出各轴运动范围的解。

而由于运动学逆解与正解不同，通常不会直接用来使用，而是通过用来实现机器人的轨迹规划简介使用，例如直线插补、圆弧插补等功能[3］。

直线插补是基于笛卡尔空间的轨迹规划，需要反复求解逆运动学方程来求解下个中间点的姿态[4］，如下给出算法基本逻辑:

已知笛卡尔空间中两点A、B，插补次数为N，则有:

那么对于每个周期坐标系XYZ各轴插补增量有:

对于该规划的直线上任意一点i的坐标是:

在进行逆解时，有时候在排除了超过运动范围的解后还会出现不止一个解，这时，可以综合计算轴的移动距离，从而选择转动量最少的一组解，或者也可以遵循“少转大轴，多转小轴的”的原则，来对多解进行筛选。这样，在每一次逆解后，就可以确保能得到一组符合要求的解值。

当然，为了保证整个架构清晰、方便后续开发调用，需要把逆解算法单独用C＋＋封装成一个模块。

图2是插补的的基本算法流程图。

2 软件设计

2.1 用C＋＋编写逆解算法，生成TcCOM对象

运动学逆解计算复杂，过程中需要进行多次反三角函数计算，如果和正解一样用st语言编写会显得比较麻烦，还会使TwinCAT PLC系统资源被过多占用，而刚好TwinCAT3同时又支持C＋＋编程，因此最好的方法便是通过C＋＋来编写逆解算法，然后通过PLC来调用。

TC1300是TwinCAT3的一个C＋＋组件，它可以将C＋＋模块运行在实时环境下。我们首先通过“新建”一个C＋＋模块并命名“InverseKinematic1”，随后选择“TwinCAT Module Class with Cyclic IO”模板，这个模板是一个标准模板，作用是用于建立C＋＋模块的IO数据接口。随后点击项目下的“TMC File”文件夹里面的“InverseKinematic1.tmc”文件，如图3所示。

在这里需要添加输入量和输出量，输入变量需要添加逆解需要的数据变量和使能信号，输出变量添加六轴的角度坐标。在建立完需要链接的变量后，右键工程名称，点击“TwinCAT TMC Code Generator”对新建的变量进行代码生成，这样一来，我们在编写程序时，才可以调用这些IO变量。

与编写C＋＋一般程序不同的是，用于TwinCAT3的C＋＋代码需要写在特定的位置。对于需要实时循环的代码我们放在一个特定方法“CycleUpdate()”内，如图4所示。

在编写程序时，首先要注意的是IO变量的调用需要通过访问成员的方式来调用，比如”m_Inputs.px”、”m_Outputs.theta1”。其次，因为逆解的运算涉及到一些函数方法的使用，比如“sin()”、“atant2()”等，而在 Twin-CAT3的C＋＋模块中，并不能通过一般包含头文件“math.h”的方法访问函数，因为TwinCAT3将“math.h”头文件更名为“fpu87.h”，其中所有的函数都需要在结尾添加“_”，比如“sin_()”、“atant2_()”。 最后，Windows自带的API并不能直接运行在内核模式中，如果需要，可以使用TwinCAT提供的SDK来实现。

在编写完代码后，还需要做的便是利用TcCOM技术将算法实例化生成TcCOM对象，使逆解算法生成相应的变量接口，再依次与PLC程序里相应的变量进行链接。这样，C＋＋中的变量便能访问PLC中的变量数值，从而进行实时数据交换。

2.2 PLC编写主体程序，HMI编写界面

对于串联的六轴机器人，需要控制6台电动机，而各轴最终的运动都是通过TwinCAT NC轴来完成，因此我们先要建立6个NC虚拟轴用来对应机器人的6个轴，在PLC程序中，要建立相应的轴变量“axis_ref”来与NC轴一一链接，这样就可以监测轴的位置状态和运行状态。在用PLC封装的正解功能块里，我们将六轴的绝对位置作为输入，那么输出的则是坐标系{6}在笛卡尔坐标系的位置参数，如图5所示。程序编写完后，再通过HMI插件对界面进行绘制。

对于六轴机器人最基础的功能便是示教，点对点等功能，在TwinCAT平台上，实现这些功能可以通过调用运动功能库中的Jog和MC_MoveRelative功能实现，由于轴运动是基于TwinCAT NC轴上的，因此，对于各轴运动的精度和速度规划是由NC模块内部自动完成，以每2 ms的周期计算下一次的位置，因此可以十分精准和平滑地运动到所需要的位置。而在轴运动的同时，由于正解功能块的输入为各轴的绝对位置，我们同时也设定PLC的运行周期以2 ms的周期运行，因此保证了正解功能块计算的准确性和实时性。如图7、8截取了通过示教点动造成的随机角度所对应的笛卡尔坐标系的坐标和PTP功能中轴1的仿真结果。

3 结语

本文主要介绍了通过TwinCAT3和C＋＋设计了一个基于DH模型的六轴串联机器人的运动控制系统，在硬件部分，可以任意选用总线的驱动器，仅需要将相关的ESI通信文件放入相关文件夹中，然后将NC虚拟轴与IO实体轴一一链接，便能实现通过仿真到实体运行的结果，因此TwinCAT3平台使得本系统的兼容性得到了极大拓展。在软件部分，首先通过DH建模实现了正逆解的推导和算法编写，之后通过TwinCAT3组件TC1300将逆解算法用C＋＋封装，之后通过接口与TwinCAT3软件的PLC平台进行数据交互。最后在PLC平台上编写并封装了正解、点对点、直线插补等基本功能块，并通过仿真验证了算法的正确性和实时性。因此，基于TwinCAT3软件平台的开发不仅提高了软件编写的效率，同时减少了机器人开发的成本与周期。