基于TwinCAT3的Stewart平台控制系统设计

上海大学智能制造及机器人重点实验室，德国倍福自动化有限公司 王进上海大学智能制造及机器人重点实验室 郭帅，聂松亮

基于TwinCAT3的Stewart平台控制系统设计

上海大学智能制造及机器人重点实验室，德国倍福自动化有限公司 王进上海大学智能制造及机器人重点实验室 郭帅，聂松亮

基于倍福TwinCAT3软件, 本文详细阐述了使用倍福PC控制技术完成Stewart平台的控制系统的设计流程。首先推导了六自由度并联平台的正、逆解算法。其次利用MATLAB/SIMULINK软件中的M函数生成正、逆解模块。再次，通过TwinCAT3的TE1400组件将SIMULINK代码生成器的目标配置为TwinCAT，生成可以集成到TwinCAT3环境的SIMULINK语言的TcCOM模型。最后在TwinCAT3软件中加载TcCOM模型，利用TwinCAT软件自带的运动控制功能块和NC轨迹生成器通过EtherCAT总线实时地对电机和平台进行控制。结果显示TwinCAT3软件与MATLAB结合使用给机器人控制系统设计带来的显著优越性。

Stewart平台；TwinCAT；SIMULINK；PC控制

1 引言

并联机器人刚度大，惯量低，精度高，可广泛应用于工业、航空、军事、娱乐等领域[1]。Stewart平台具有俯仰、横滚、偏航、升降、纵向平移和侧向平移六个自由度。由于其独特的结构，在工业领域，与机床技术相结合，相比传统机床具有刚度好、精度高、高速和高加速度等特点。在运动仿真领域，通过在活动平台上安置座舱，配合虚拟现实等技术手段可以开发各类运动模拟器，用于模拟飞机飞行、舰船航行以及车辆行驶。在测量测试领域，可以为多种设备提供模拟测试平台。在空间技术领域，可以研制开发空间飞行器对接机构。当前，六自由度并联机构动力学和运动学算法日渐成熟。但是怎么把仿真算法模型高效率地放在运动控制器运行是个新的难题。MATLAB具有强大的数学运算能力，同时SIMULINK标准库又提供了大量的图形化功能块，减少了工程人员建立机器人模型所需要的时间。但是MATLAB软件不具有实时控制自动化硬件的能力。而传统的PLC虽然可以控制I/O模块和伺服电机，但是缺少像MATLAB那样方便强大的矩阵运算能力和SIMULINK库中大量的算法功能块。

倍福TwinCAT3软件是基于Windows软硬件平台的实时控制软件[2]，它通过内部对Windows底层的更改，实现了对I/O模块和驱动器的实时控制。该软件采用Visual Studio的Shell，支持符合IEC61131的PLC语言及C++的混合编程，另外该软件通过TcCom Object功能可以导入MATLAB/SIMULINK生成的模块。该软件的PLC有丰富的运动控制功能库可用于轨迹规划和对伺服轴进行控制。同时采用倍福的百兆EtherCAT总线可以方便灵活实时地对伺服驱动器进行控制。

建模仿真技术与自动化技术相结合是机器人设计的趋势。本文以Stewart平台为例，基于Windows平台的TwinCAT3软件和MATLAB/SIMULINK软件完成了Stewart平台控制系统的研发，实现了运动学算法的编写和对平台的实时控制。

图1 平台实物图

Stewart运动平台（如图1所示）是由六个电动缸通过万向节铰链（或球铰链）将上、下两个平台连接而成。下平台固定在基础上，借助六个电动缸的伸缩运动，完成上平台在三维空间六个自由度（X，Y，Z，α，β，γ）的运动，从而模拟出各种空间运动姿态。六个电动缸采用倍福的基于EtheCAT总线的绝对值编码器伺服电机，可以通过网线直接连接到倍福的PC控制器。控制器采用倍福CX2020嵌入式控制器，CPU为赛扬1.4G单核处理器，内装Win7E系统可以方便地兼容其他Windows平台的软件。

2 运动学分析

并联机器人机构运动学分析包括正运动学分析和逆运动学分析两个方面。在求解前首先建立坐标系。

如图2所示，在下平台建立静止坐标系O-XYZ，其中X轴平行于铰点A4、A5所在直线。在上平台建立动坐标系o-xyz。上平台的运动可分解为X、Y、Z方向的平移，以及绕其坐标轴的旋转α，β，γ。下平台各铰点Ai的坐标为（XAi，YAi，ZAi），上平台各铰点ai的动坐标为（xai，yai，zai）静坐标为（Xai，Yai，Zai）与其对应的相连的电动缸长度为 (i=1，……，6)

图2 Stewart 平台坐标系定义

2.1 位置逆解

并联机构的运动学逆解是从工作坐标反求关节坐标，有唯一解。在平台的实际应用中，通过路径规划算法每个周期给定的是平台的六个空间姿态参数X，Y，Z，α，β，γ。此过程的实质是在平台运动中，输入给定的位姿，控制伺服电机使电动缸到达所需长度。

我们以1号电动缸为例,（Xa1，Ya1，Za1）和（XA1，YA1，ZA1）分别是上、下铰点在静止坐标系中的坐标，（xa1，ya1，za1）为上铰点在运动坐标系中的坐标，（X，Y，Z）为上平台中心在静止坐标系中的坐标，其关系可如下表达

其中T是旋转变换矩阵，T=c×b×a。其中c表示绕z轴旋转γ，b表示绕y轴旋转β，a表示绕x轴旋转α[3]，分别如下所示：

旋转变换矩阵T=

将矩阵T代入公式（1），即可求出（Xa1, Ya1, Za1），从而可以求出电动缸的长度L1为：

同理可得L2…L6。[4]

2.2 位置正解

已知机构输入件的位置，求解机构输出件的位置和姿态称为机构的位置正解。不同于串联机器人，六自由度运动平台的正解非常复杂，并且存在多解。其求解方法大致可分为代数解析法[5、6]和数值法[7、8]。解析法的研究集中在寻找全部解，而数值法的研究注重提高求解效率以满足实时性的要求，本文采用数值法求正解。六自由度平台的逆解求解过程上一节中已经推导，为了简化正解的求解过程，本文使用牛顿迭代法。利用逆解算法迭代电动缸杆长采用逐次逼近数值法来求位置正解。由于本平台电机采用绝对值多圈编码器，所以上电后电机编码器的位置反馈可以转化为电动缸的杆长，再经过位置正解求出平台的空间姿态。图3是正解算法的流程图。

图3 运动学正解算法流程图

3 软件设计

3.1 MATLAB编程和生成TcCOM Object

首先利用MATLAB强大的矩阵运算和数学处理能力，把上文叙述的正逆解算法，采用M函数做成函数功能块，在SIMULINK中调用这个功能块并添加输入输出接口，如图4所示。

图4 在MATLAB/SIMULINK中编写正、逆解功能块

倍福TwinCAT3软件的TE1400组件是一个SIMULINK代码编译器，可以把SIMULINK中执行的图形化功能块生成可以实时运行的C或者C++的代码。在一台安装有MATLAB的电脑上安装TE1400后把SIMULINK代码生成器的目标配置为TwinCAT，就可以把SIMULINK中的正逆解工程文件生成可以集成到TwinCAT3环境的具有输入输出变量的SIMULINK语言的TcCOM模型。

3.2 TwinCAT软件配置和编程

首先在TwinCAT软件的TcCOM Object中调用上文中生成的正逆解功能块。可以看到加载的Lpt-backward和Lpt-forward功能块的图形与变量都与SIMULINK中一致。如图5所示。

图5 在TwinCAT中导入正、逆解功能块

由于Stewart平台具有笛卡尔坐标系的六个自由度并且需要控制六个关节电机。在倍福的TwinCAT软件的NC中首先添加A1到A6六个关节坐标系轴并链接到驱动器和PLC，再添加X、Y、Z、A、B、C（X、Y、Z、α、β、γ）六个笛卡尔坐标系轴链接到PLC。对于正解将六个电动缸的伺服电机A1到A6的位置作为输入变量链接到正解功能块，功能块输出X、Y、Z、A、B、C的值作为平台在笛卡尔坐标系中的位置。而逆解功能块将给定的X、Y、Z、A、B、C位姿作为输入变量，输出变量则作为电动缸的给定位置通过外部给定功能块发给NC再发给驱动器。

在PLC程序中，直接对笛卡尔坐标系中的X、Y、Z、A、B、C六个虚拟轴进行控制。调用TwinCAT软件的运动控制库中的点动、定位、凸轮等功能块，可以方便地让机器人移动到某一位姿或者进行连续循环运动。其原理是触发功能块后TwinCAT软件的NC运算核会自动按照样条曲线规划出运动路径，并以2ms的周期间隔计算出下一目标位置，我们将这一位置发送给机器人运行模块，模块会实时地逆解出电机的角度，再通过外部给定发送给伺服驱动器。通过NC运算核和机器人运算核，伺服驱动器每2ms会收到一个位置数据，并让电机运转到这一位置。由于NC的路径规划功能强大，这些位置数据的一阶导数和二阶导数都是连续的，从而保证了伺服驱动器的连续稳定运行，如图6所示。

图6 系统框图

另外还可以通过高级语言或者第三方软件编辑预定轨迹，按照2ms的时间间隔发送给机器人运行程序，让机器人按照预定轨迹完成一连串动作。在调试阶段有时候需要不通过机器人正逆解而直接对A1到A6电机进行单独操作，所以在PLC程序中还增加了对实轴电机的单独操作功能。倍福的TwinCAT软件可以通过ADS通信和C#开发的HMI软件进行数据交换，HMI界面如图7所示。

图7 HMI界面

3.3 硬件平台

如图8拓扑结构所示，在工控机的网口通过网线直接连接倍福输入输出I/O模块以及六个伺服驱动器AX5101。输入模块EL1008用作启停按钮和限位开关，输出模块el2008用作报警等输出。

图8 硬件链接拓扑结构图

倍福的EtherCAT总线是一种高性能的以太网总线，与100个伺服轴的通讯只需100μs 。在此期间，可以向所有轴提供给定位置和速度以及控制字，并接收它们的实际位置和速度以及状态字。分布式时钟技术保证了这些轴之间的同步时间偏差小于1μs 。从硬件上满足了Stewart平台的控制要求。

4 仿真和测试

为了验证算法和编程的正确性，首先将正逆解算法在MATLAB软件中进行编程，对于正解算法和逆解算法都能得到与输入变量对应的输出变量。然后将算法移植到TwinCAT软件中，利用TwinCAT软件的可以不带硬件运行的功能进行测试。最后把TwinCAT软件的轴变量与硬件电机驱动器相链接，运行后可以看到Stewart运动平台可以按照预定的轨迹进行正常运行，运行效果如图9所示。蓝色为给定X轴位置，红色为X轴实际位置。

图9 运行效果图

5 结语

本文主要介绍了利用TwinCAT3和MATLAB软件设计Stewart运动平台的方法。首先研究了运动平台的正解和逆解算法，然后对于本文的正逆解算法在MATLAB软件中进行了编程和仿真并利用TE1400组件导入到TwinCAT3，最后在倍福TwinCAT3软件平台进行了接口和操作界面编程并在基于PC和EtherCAT总线的硬件平台进行了实际运行。运行结果表明本设计可以对Stewart平台进行定位操作、按预置轨迹操作、反馈位姿信息等各种操作。从而验证了运动学算法的正确性以及使用TwinCAT3与MATLAB软件编写程序的优越性，提高了机器人设计中仿真和软件编程的效率。

[1] 黄真, 孔令富, 方跃法. 并联机器人机构学理论及控制[M]. 北京：机械工业出版社, 1997: 46 - 64.

[2] http: //www. beckhoff. com. cn/english/[EB/CD].

[3] 刘极峰. 机器人技术基础[M]. 北京: 高等教育出版社, 2006: 63 - 67.

[4] 杨永立. 六自由度运动平台的仿真研究[J]. 工程机械文摘, 2009, 1.

[5] 梁崇高, 荣辉. 一种Stewart平台机械手位移正解[J]. 机械工程学报, 1991, 27 (2): 26 - 30.

[6] 赵铁石, 黄真. 一种新型四自由度并联平台就够及其位置分析[J]. 机械科学与技术, 2000, 19 (6): 927 - 929.

[7] 刘安心, 杨廷利. 求一般6 - SPS并联机器人结构的全部位置正解[J]. 机械科学与技术, 1996, 15 (4): 543 - 546.

[8] 赵新华, 彭商贤. 一种分析并联机器人位置正解的高效算法[J]. 天津大学学报. 2000, 33 (2): 134 - 137.

Design of Control System for Stewart Platform based on Beckhoff Twincat3

Based on BECKHOFF software in TwinCAT3, this paper describes the process of control system design using BECKHOFF PC technology for Stewart platform in detail. First, the direct and inverse solution algorithms of the 6-degrees parallel connection platform are developed. Second, the direct and inverse solution modules are generated through the M functions in MATLAB/ SIMULINK. Third, the target of SIMULINK code generator is configured to TwinCAT via TE1400 in TwinCAT3 and the TcCOM model in SIMULINK language can be integrated to TwinCAT3 environment. Finally, the TcCOM model is loaded into TwinCAT3. Using TwinCAT motion control function blocks and NC trajectory generator, the motors and platform can be controlled in real time via EtherCAT bus, which realizes the desired functions. Results show that Twinca3 combined with MATLAB improves the efficiency for control system design.

Stewart platform; TwinCAT; SIMULINK; PC control

王进（1985-），男，河南驻马店人，上海大学在职研究生，现任德国倍福自动化工程师，主要从事机器人应用研究。

郭帅（1970-），男，安徽蚌埠人，博士，副研究员，现就职于德国倍福自动化有限公司，主要研究方向为机器人、张力控制。

聂松亮（1989-），男，硕士研究生，现就读于上海大学。