基于TwinCAT3的一种开源机器人控制系统设计

黄睿, 张新*, 朱华炳, 张涛, 曾亿山

(1.安徽理工大学机械工程学院, 淮南 232001; 2.合肥工业大学机械工程学院, 合肥 230031)

当前,机器人已成为数字化经济时代最标志性的智能工具,推动了自动化技术、集成化技术的快速发展[1]。如工业机器人、健康医疗机器人等不同类型的机器人被广泛应用于各行业中,机器人技术已然是新时代科学技术发展的重要方向。

控制系统是机器人的核心,其运行状况的好坏与系统密不可分。目前市场上主流的工业机器人所配套的控制系统是封闭式结构[2],外部人员只能在其固有的软件平台上进行二次开发。即使不乏有学者基于ROS(robot operating system)[3]、嵌入式[4]等系统开发机器人,但ROS开发周期长、嵌入式系统开发难度大,借助其他系统平台[5-6]也难以在短期内开发出高性能与高实时通信的机器人控制系统。

不少研究人员开始采用EtherCAT(ethernet for control automation technology)技术[7-8],旨在利用实时以太网通信开发机器人控制系统。Ahn等[9]通过搭建双通道的EtherCAT主站,成功研发出具有高实时性和高稳定性的33自由度仿人机器人控制系统。Zhao等[10]基于EtherCAT通信网络,设计的双足轮机器人可以实现高频力控制和良好的反向驱动能力。周惠兴等[11]将Beckhoff嵌入式PC计算机(personal computer, PC)作为上位机,以EtherCAT为通信基础,研究出的机器人控制系统可实时、准确完成瓷砖铺贴。周楷文等[12]基于PC控制,以 ARM嵌入式处理器(advanced RISC machine, ARM)为系统核心搭建了EtherCAT主站,结合MATLAB软件平台设计出的协作机器人控制系统可实现高实时性的数据反馈,但产品的升级换代会因设备的不兼容而影响二次开发。邓剑楠等[13]基于ROS和EtherCAT研制了一种智能抹平机器人的多轴控制系统,其机器人控制的实时性可取代人工作业,但ROS开发难度较大,延长了开发周期。这些基于EtherCAT开发的控制系统使得机器人具有不错地稳定性和实时性,但目前并没有见到一种易开发的,同时兼顾移植性、拓展性和实时性的开放式机器人控制系统。

为促进、完善开放式机器人控制系统的发展[14],设计一种跨平台的开源机器人控制系统架构。以倍福TwinCAT3为控制系统的软件开发平台基础,综合PyCharm、MATLAB、CoppeliaSim等软件平台,以EtherCAT与ADS (automation device specification)通信为核心,采用一主多从的工作模式。同时结合视觉和运动控制,借助数字孪生技术,建立虚拟模型与机器人本体之间的联动[15],并通过离线仿真和在线控制实验分析,以期验证该系统架构设计的可行性。该跨平台开源机器人控制系统架构的研究推动了开源机器人控制系统的进步,为未来机器人技术的发展和应用具有深刻的指导和参考价值。

1 机器人控制系统架构设计

以实验室自研的五自由度轻型工业机器人为例(图1),设计了一种基于TwinCAT3 (the windows control and automation technology)的开源机器人控制系统架构,包括软件和硬件。

图1 五自由度机器人Fig.1 Five-degree-of-freedom robot

在软件方面,选择开源算法和软件平台开发机器人的视觉检测与运动控制,如PyCharm、MATLAB、TwinCAT3等,可以缩短开发周期,降低成本。其中,以嵌入TwinCAT3的Visual Studio 2017为核心开发环境,使用Python、C/C++、ST等语言编写程序,同时基于EtherCAT实时工业以太网总线技术和ADS通信协议进行数据传输,满足系统实时控制的需求。在硬件方面,工业相机选择大恒图像的水星二代系列,该系列相机具有可二次开发的软件开发包SDK,图像数据传输稳定、快速;运动控制器选择德国Beckhoff公司的CX系列,该系列控制器基于PC控制,凭借PC强大的计算能力,在最大限度控制255个轴时仍能高效作业;关节模组选择泰科的RJSIIZ系列,该系列模组集成化程度高,一体化结构极大地简化了机器人本体,而且支持EtherCAT通信协议;伺服驱动器亦选择支持EtherCAT通信协议的松下A6SE系列,保证了整体系统通信的协同性与兼容性。

如图2 所示,系统采用模块化设计的思想,主要划分为视觉模块、运动控制模块、算法集成与仿真控制模块,分别负责机器人的视觉检测、运动控制、相关算法集成与人机交互界面的设计。其中,算法集成与仿真控制模块于PC端开发,包括各开源软件平台,具有集成视觉与运动控制算法、仿真机器人运动和设计上位机人机交互界面的功能;视觉模块包括工业相机和PC,具有图像采集和处理的功能。运动控制模块关键是EtherCAT主站和从站(节点或设备)的搭建,PC与倍福控制器为主站,从站设备包括RJSIIZ14/17关节模组和松下伺服驱动器,该模块具有发送运动控制指令和接收视觉模块与各从站设备传感器数据信息的功能。当五自由度机器人运动时,机器人CoppeliaSim数字孪生模型与实物虚实交互,同时人机交互界面实时显示机器人各关节位姿信息。以这种系统架构的设计方案实现机器人控制,可以随时对机器人运动状态进行监控并调整。

2 相关模块设计

2.1 算法集成与仿真控制模块

算法集成与仿真控制模块包括上位机软件和下位机软件,如图3所示。

TwinCAT3为倍福自动化控制软件;Visual Studio 2017为核心开发环境;MATLAB为计算机软件;CoppeliaSim为机器人仿真软件;Qt/Pyqt5为人机交互界面开发软件;PyCharm为Python语言开发的集成开发环境图3 算法集成与仿真控制模块Fig.3 Algorithm integration and simulation control module

TwinCAT3是五自由度机器人算法程序设计和开发的核心编程环境,上位机软件集成的视觉和运动控制模块算法在该下位机软件系统中交互数据。视觉方面,PyCharm集成视觉目标检测算法,通过Pyqt5封装并设计视觉检测人机交互界面,如图4所示;运动控制方面,MATLAB集成机器人的运动学控制和轨迹规划算法(如多项式插值轨迹规划等),CoppeliaSim完成对机器人数字孪生模型仿真运动,并联合Qt设计机器人人机交互界面,如图5所示。

图4 目标检测人机交互界面Fig.4 Object detection human-computer interface

2.2 视觉模块

视觉模块以TwinCAT3软件系统为数据传输媒介与运动控制模块进行数据交互,借助该软件系统中的ADS模块将视觉图像数据输送至EtherCAT主站。

2.2.1 ADS通信

ADS是TwinCAT3软件系统中各模块之间通信的消息路由,其采用的工作模式为服务器/客户端(server/client);是一种访问ADS设备的接口控制通信协议[16]。协议规定ADS设备的唯一标识为用于确定设备硬件的ADS网络ID和用于确定软件服务的端口,任何ADS通信的请求必须指定ID和端口,才能找到对应的服务器。TwinCAT ADS模块数据访问的方法有两种,一种是访问TwinCAT PLC变量的地址,变量地址由分组索引和偏移地址组成,这种方法先访问变量地址,再访问变量;另一种是直接访问PLC变量名,TwinCAT PLC程序中的任意变量都有唯一的句柄,该方法需要先获取变量句柄,变量读写完成后,释放句柄。

2.2.2 基于ADS的视觉模块设计

为提高机器人运动精度,减少如抓取、焊接任务时的定位误差,视觉模块采用开源深度学习框架Pytorch搭建视觉检测算法的网络模型,利用强大的GPU硬件快速、精准识别出目标2D坐标,通过立体校正与匹配获取目标3D位姿,最后封装至Pyqt5中。该模块基于ADS通信,调用倍福提供的TcAdsDll库构建通信功能,以直接访问变量名的方式,建立上位机软件(Pyqt5)控制接口的变量与下位机软件(TwinCAT3)PLC变量之间一一对应的关系,完成图像数据的传输。具体涉及的变量接口及通信流程如图6所示。

首先,PC端完成Pyqt5与TwinCAT3 PLC通信地址和端口的配对;其次,打开ADS通信,将Pyqt5封装的算法输出接口变量名与PLC变量名完成对应变量链接;最后,同步传输视觉图像数据。视觉图像数据传输主要分为两部分,第一部分是将图像数据输送至Pyqt5人机交互界面中视觉检测算法输入变量的控制接口,算法处理后将数据结果存储在输出变量的控制接口,随后由客户端(Pyqt5)向服务器(TwinCAT3 PLC)发送请求;第二部分是服务器接受请求,打开通信端口,获取对应PLC变量句柄,从客户端中同步读取图像数据结果并写数据到PLC变量中,以便后续运动控制模块接收该图像数据信息,最后释放句柄关闭端口,等待下一次请求。

2.3 运动控制模块

运动控制模块是五自由度机器人控制系统的关键部分,为实现对机器人的快速、实时控制,该模块所选的控制器、关节模组、伺服驱动器均支持EtherCAT通信协议。

2.3.1 EtherCAT通信

在倍福基于PC和EtherCAT的控制技术中, EtherCAT通信是核心技术[17]。是一种用于过程数据的优化协议技术,解决了传统专用交换机分配以太网包存在严重时间延迟的问题。该技术无需接受以太网数据包,可以直接解码数据包并复制数据到每个从站设备,而后从站设备接收并处理数据。系统数据传输时间延迟只有几纳秒,加快和便捷了主站控制器与其他外部从站设备之间的通信,足以满足控制系统高实时性的需求而不受时间延迟的限制。

2.3.2 基于EtherCAT的运动控制模块设计

在机器人运动时,各从站设备的运动控制指令来自倍福控制器,机器人到达指定位置并完成特定任务所需的位姿通过各关节模组、伺服驱动电机输出的电压电流信号转换而来,而这些数据信号的传输由EtherCAT技术支撑。

如图7所示,机器人运动控制模块以TwinCAT3和倍福控制器为EtherCAT主站控制器,PC端集成有MATLAB在Robotics Toolbox中开发的运动控制算法、Qt设计的机器人人机交互界面,以及CoppeliaSim数字孪生技术实时显示机器人运动轨迹(位移、速度和加速度等数据变化曲线)的仿真控制。

TCP/IP为网络通信协议;RemoteAPI为CoppeliaSim软件API框架的远程通信接口函数;Robotics Toolbox为机器人工具箱;C++ module为C++模块图7 运动控制模块通信流程Fig.7 Motion control module communication process

首先,基于TCP/IP协议,借助Remote API函数分别建立CoppeliaSim与Qt、MATLAB之间的通信,通过TwinCAT3 C++ Module调用MATLAB封装的运动控制算法;其次,完成TwinCAT3与倍福控制器之间的Ethernet连接;最后,基于EtherCAT协议建立倍福控制器与机器人本体中各从站设备之间的通信。当机器人进行如关节空间、笛卡尔空间的轨迹规划时,轨迹规划算法生成各关节所有插值点数据,由EtherCAT主站一边发送该控制指令(各关节值)至CoppeliaSim中控制机器人仿真模型运动,同时运动仿真模型的位姿信息实时反馈至Qt人机交互界面;另一边控制指令经过数据处理后由EtherCAT发送给各从站设备,控制机器人本体按规划的轨迹进行运动。此外,各关节模组中的编码器等传感器接收、处理机器人与外界的数据信息并反馈给EtherCAT主站,等待主站控制器的下一次指令。

3 离线仿真与在线控制

五自由度机器人控制系统按功能模式分为离线仿真和在线控制。视觉在系统模式切换中,始终为机器人提供目标3D位姿,辅助其精准定位。如图8所示,绿色虚线框为离线仿真模式,视觉仅为机器人仿真提供目标位姿,完成机器人轨迹规划;紫色虚线框为在线控制模式,视觉提供的目标位姿需在TwinCAT3设计的视觉伺服控制器中进行处理,获取机器人末端与目标之间的误差位姿,完成误差位姿变量与调用MATLAB运动控制算法的C++ Module、TwinCAT PLC之间的变量链接,等待机器人运动的控制指令。

在机器人系统控制中,离线仿真主要包括轨迹规划和仿真,视觉提供目标位姿、MATLAB进行轨迹规划和CoppeliaSim完成机器人模型仿真,筛选出安全、高效的运动轨迹策略;在线控制主要包括仿真和通信,选择合适的离线运动策略,由主站向伺服驱动器、各关节模组发出控制指令,驱动机器人本体沿期望的轨迹路线运动,同时接收各从站设备反馈的位移(d为直线滑台的水平位移)、角度(θ1、θ2、θ3、θ4为4个旋转关节的旋转角度)。调用CoppeliaSim仿真,实时显示机器人的运动状态,通过视觉伺服控制器不断补偿机器人末端位姿与目标之间的位姿误差,最终协助机器人到达指定位置。

4 实验验证

为了验证本开源机器人控制系统设计的可行性和有效性,在图1所示的五自由度机械臂上进行实验并讨论。

图9为实现的机器人离线仿真。通过视觉给定的目标位姿,采用笛卡尔空间直线轨迹规划生成各关节所有插值点的数据,借助CoppeliaSim仿真控制机器人模型各关节运动到目标位置。分别记录了机器人数字孪生模型在不同时刻末端的位移和速度曲线变化,可以明显地观察到机器人末端的运动轨迹为一条直线,同时位移和速度曲线没有发生突变和离散,验证了在离线仿真模式下,机器人轨迹规划的正确性。

图10为实现的机器人在线控制。根据离线仿真的轨迹规划策略,在线控制机器人样机按规划的轨迹做直线运动实验,根据视觉与从站设备反馈的数据信息实时调整机器人各关节角度θ(或d)。分别记录机器人实物在不同时刻的运动轨迹曲线,观察到机器人实物与CoppeliaSim仿真中机器人数字孪生模型(图9)各关节的位姿一致,验证了机器人在线控制的可行性,同时验证了机器人控制系统的高实时性能。

T为机器人运动的不同时刻图10 在线控制Fig.10 Online control

5 结论

针对主流工业机器人为封闭式控制结构的问题,设计一种基于TwinCAT3的开源机器人控制系统架构,既能拓展控制系统的功能,又能支持二次开发。通过上述系统架构的设计,得出以下结论。

(1)系统采用开源可扩展架构,包含视觉、运动控制和算法集成与仿真控制模块,以ADS、EtherCAT通信技术传输数据。

(2)视觉模块借助开源深度学习算法视觉算法实现精准定位;运动控制模块建立了一主多从工作模式;算法集成与仿真控制模块集成视觉、运动控制等智能算法,设计了目标检测和机器人人机交互界面。

(3)系统结合数字孪生技术,通过离线与在线控制完成了仿真模型与实物之间虚实交互的联动,优化运动策略,提高控制精度。验证了该系统架构的可拓展性好且实时性强。