基于EtherCAT总线的拖动示教机器人控制系统开发

陈宇鹏 ，高伟强 ，刘建群 ，陈乐南 ，卢一光

（1.广东工业大学机电工程学院，广东 广州 510006；2.广东工业大学广东省微纳加工技术与装备重点实验室，广东 广州 510006）

工业机器人喷涂作业具有重复精度良好、生产效率高等优点，并且能够将工人从恶劣的环境中解放出来，在工农业各生产领域得到广泛应用[1-3]。随着技术的进步和市场竞争的发展，中小企业对机器人自动化喷涂技术的需求越来越大。不同于大型企业，中小型企业涂装生产具有多品种、小批量、变化快等特点，并且存在严重缺乏机器人和自动化技术开发应用人才等问题。目前市面上的喷涂机器人控制大都采用离线示教或示教盒示教编程的方式获取控制程序，由于机器人操作编程对作业人员素质要求较高，需受过专业训练，无疑给中小型涂装企业的机器人应用推广增加了难度。拖拽示教是由操作人员直接拖拽机器人末端喷枪工具的方式实现复杂轨迹的示教，喷涂效果所见即所得，无需编程，简单易学，效率高，只需简短培训就可使原喷涂工人直接成为机器人的操作工人，解决了中小涂装企业技术升级后机器人应用人才短缺的问题。

为提高机器人拖拽示教灵活性，常采用辅助示教技术实现示教。文献[4]研究了一种通过变速换挡提高手把手示教的可操作性方法；文献[5]通过机器人末端六维力传感器采集的受力信息在位置控制策略下，实现了基于顺应性控制的直接示教；文献[6]发明了一种通过伺服电机助力提高手把手灵活性的方法。虽然采用示教辅助技术在一定程度上提高了机器人示教灵活性和操控性，但由于人工拖拽仍存在示教轨迹不平滑与速度不连续产生冲击、振动等问题，影响复现时喷涂质量和复现速度的提升。

针对中小喷涂企业的需求和拖拽示教轨迹存在的问题，本文提出了对示教采样数据进行优化后再复现的处理方法，并结合“Linux+Xenomai”系统架构，在以高性能处理器AM4377为核心的硬件平台上，开发了基于EtherCAT总线的拖拽示教喷涂机器人控制系统。

1 控制系统总体设计

EtherCAT总线是目前国际上主流的实时工控总线，因为其在开放性、实时性、可靠性、安全性、经济性和可扩展性等方面的优势，越来越多的控制系统采用EtherCAT总线[7]。AM437X是美国TI公司推出的基于ARM Cortex-A9内核，专门用于在各类工业自动化设备中实现实时EtherCAT通信标准的处理器系列。本文将采用AM4377处理器为核心，开发机器人控制系统运控主站。

根据拖拽示教六关节机器人喷涂的工艺特点和要求，控制系统总体架构如图1所示。系统硬件分为三大部分：1）上位机采用工业计算机IPC，主要负责人机界面和示教采样的数据处理和优化等非实时任务；2）控制系统的控制层由ARM处理器AM4377承担运动控制器任务，根据应用任务的实时性不同，AM4377运行一个Linux及Xenomai内核，分别处理网络通讯、指令处理、示教采样、插补控制等弱实时和强实时运控任务；3）设备层是EtherCAT总线连接的各种伺服从站和I/O外设。

图1 控制系统总体架构Fig.1 Structure of control system

采用IPC作为上位机，可充分利用PC机丰富的软硬件资源、高速的数据运算能力和友好的界面开发能力，满足系统对人机界面、采样示教位姿优化处理等非实时任务需要。

控制层AM4377主站基础架构由应用程序、EtherCAT协议栈组成。因为EtherCAT周期性过程数据通信和插补运算的强实时需求，需要对主站Linux系统进行实时化改造。Linux的实时性改造通常有两种方式：一种是直接修改内核，对内核进行大规模修改；另一种是在硬件平台嵌入一个与Linux同时运行的小型实时内核。Xenomai是针对Linux内核进行强实时扩展的开源实时内核[8-9]，被广泛应用于工业控制，本文采用Linux+Xenomai作为主站系统内核。为实现EtherCAT数据帧通讯，本文选用IgH EtherCAT Master协议栈，主站应用层程序通过调用IgH的API，实现从站节点的配置、邮箱数据和过程数据的收发任务。

AM4377主站硬件采用市场商品化成熟的AM437X系列最小核心板为基础开发。该核心板提供了2个256 MB的DDR3内存，1个4 GB的eMMC存储，1个网卡单元。为了有效利用核心板提供的引脚资源，实现EtherCAT主站和外设控制等功能，利用该核心板提供外设总线接口自行开发拓展底板。底板由24 V电源供电，包括电源电路、TF卡单元、TTL电平调试串口、USB和458接口电路。由于核心板只提供1个网卡单元，为同时满足EtherCAT通信和TCP/IP通信接口，在底板拓展了2路TLK105L网卡单元。AM4377主站具有硬件开发周期短，稳定性和可靠性高、低成本等优点。

2 系统软件设计

机器人控制系统主要实现采样示教和再现喷涂功能。采样示教是指人工进行手把手示教喷涂时，控制系统对机器人6轴关节和2轴变位轴伺服电机的绝对位置编码器数值以及喷涂系统I/O参数进行定时采样，记录得到8坐标的示教采样数据。示教结束后需要对采样数据进行处理：通过机器人机构学正运算求出喷枪在笛卡尔空间运动的位置、姿态信息，并对这些位姿数据优化拟合成样条曲线，然后按照运控原理对喷涂过程的速度、加速度等运动参数进行优化，再根据插补原理对样条曲线进行离散化，经过机器人逆变换得到优化后的关节坐标。

机器人再现喷涂时，系统将离散后的关节坐标发送给运动控制系统，利用运控系统的实时线程对关节坐标进行精插补，最后将插补数据通过EtherCAT总线输出至各个伺服、I/O等从站设备，实现再现喷涂。

根据机器人示教、再现功能，以及各程序任务对系统实时性和运算能力的需求，系统软件模块如图2所示。

图2 系统软件模块Fig.2 Module of system software

图2系统中，Linux程序和Xenomai程序并行运行。为保障EtherCAT周期性通讯等任务的实时性，Xenomai创建的线程优先级最高，优先对实时线程进行处理，并对系统资源进行占用。当Xenomai线程处理完任务后，再把系统资源的支配权转交给Linux域中的线程。中断管线任务分派如图3所示。

图3 中断管线任务分派Fig.3 Interrupt pipeline task assignment

3 IPC上位机程序设计

IPC上位机软件采用QT5.8.2开发。喷涂机器人控制系统上位机软件主要的面向对象是喷涂机器人的操作人员，需要具有界面友好、操作简单和实用等特点。根据企业拖拽示教喷涂生产流程，将机器人喷涂生产整个过程需要的功能进行分析和整合，采用模块化的方法设计并完成了各个模块的开发。

根据对采样数据优化处理的要求，本文设计IPC上位机对示教数据处理的流程如图4所示。

图4 示教数据处理流程图Fig.4 Teaching data processing flow chart

3.1 人机交互、通讯和系统管理模块

上位机IPC端软件的一个主要任务就是实现人机交互界面、系统设置管理和上/下位机通讯等非实时操作。控制系统的所有参数，包括Ether-CAT从站参数、机器人关节连杆参数、机器人减速机参数、喷涂工艺参数等的设置、管理由上位机完成，并以数据库表的形式存储。上位机通过TCP/IP协议与下位机（AM4377）交互数据并发送各种操控指令。IPC端根据不同的运动控制和指令功能，封装了一个运控库函数接口，当操作者需要从上位机对系统下达指令时，IPC只需调用该库的API函数，模块根据指令类型对指令和数据打包成数据帧下发给下位机。

3.2 示教采样数据预处理模块

示教采样数据预处理模块的目的是通过示教数据构造光顺的喷枪位、姿曲线。首先，需要将所采样的机器人关节坐标与喷枪位姿在笛卡尔坐标之间进行空间数据映射，这必须经过机器人机构学的正/逆运动矩阵变换，有大量的三角函数等浮点运算。这些运算没有实时性要求，可以利用示教与再现之间的时间间隙在IPC完成。上位机程序对示教数据进行优化时，首先通过高斯滤波对6关节坐标示教采样数据进行低通滤波，降低数据噪声。其次，对滤波后的示教数据进行机器人正运算，由关节坐标得到喷枪在笛卡尔空间下的位置离散数据以及对应的姿态离散数据。

位置数据描述的是喷枪相对于机器人基坐标系的坐标，姿态数据描述的是喷枪的喷轴矢量。对位置数据进行特征点的提取，去除冗余采样点，用特征点来表示位置轨迹而不丢失轨迹自身的特征。

特征点判断示意图如图5所示，计算空间矢量Si，Si+1之间的夹角αi+1，并与给定的阈值α0作比较。假如αi+1小于等于阈值α0，则认为三点处于同一空间直线上，此时舍去中间点Pi+1。假如αi+1大于阈值α0，则保留中间点Pi+1为特征点。

图5 特征点判断示意图Fig.5 Judgment of feature points

根据上述方法可以得到一系列示教特征点，每个特征点都有空间坐标和姿态值。本文通过3次B样条将上述特征点拟合得到喷枪运动的空间位置曲线C(u)和姿态曲线q(t)，其中q(t)采用单位四元数法描述喷枪喷轴矢量的姿态变化：

式中：u，t为曲线参数；i，j，k分别为坐标单位向量。

通过文献[10]的方法可以根据特征点的空间坐标数据确定喷枪位置曲线C(u)；通过文献[11]的方法可以根据姿态数据确定基于单位四元数姿态表述的喷枪姿态曲线q(t)。

3.3 粗插补模块

位置曲线C(u)曲率变化较为复杂，粗插补模块对曲线C(u)进行运动加减速分段，针对各分段进行连续区域的S曲线运动速度规划，再根据时间分割插补原理将C(u)曲线离散。

位置曲线C(u)通过寻找曲率极值点来完成运动加减速分段。曲线C(u)的曲率极值点序列为{C(ϕ0),C(ϕ1),…,C(ϕn)}。根据速度的两个约束条件[12]，得到插补速度约束公式：

式中：ρj为曲率半径；δmax为最大插补弓高误差；T0为插补周期；amax为喷枪运动允许最大加速度；vm为系统给定插补速度；vj_max为第j个曲率极值点允许的最大插补速度。

根据式（2）计算得到所有曲率极值点的最大允许插补速度序列{v0,v1,…,vn}。

通过曲率极值点将曲线C(u)分成若干段速度连续规划曲线段，对每段曲线的加减速控制采用3次S型速度曲线规划[13]。减速控制时需提前得知减速点，但正向插补时减速点难以预测，本文采用文献[14]的减速点预测方法进行每段曲线的减速点计算。该方法将每段曲线的终点作为起点，起点作为终点，进行反向插补，原分段曲线的减速过程变为反向曲线的加速过程，原减速点即为反向曲线的加速结束点。完成每段曲线的连续速度规划后，根据时间分割插补原理对位置曲线离散成插补段。离散插补段的姿态信息则根据位姿同步模块求得。然后对各个插补段位置姿态数据进行机器人机构学逆变换，将插补段由笛卡尔坐标映射到关节坐标。

喷涂再现时，上位机将插补段关节坐标的运控数据信息下发给下位机，由AM4377程序完成关节插补控制。

3.4 位姿同步模块

再现时喷枪的姿态必须与工人示教的姿态在空间上基本保持一致。由于喷枪空间位置是拟合后再插补，若直接对喷枪姿态曲线q(t)进行姿态插补，则会导致再现时喷枪姿态与工人示教的姿态在空间上不一致，喷涂效果可能会出现严重偏差。为解决这一问题，可以建立位置曲线C(u)参数u与姿态曲线q(t)参数t直接的映射关系。

根据文献[10-11]，曲线C(u)和q(t)的节点矢量都是通过规范积累弦长化方法构造的。所以曲线C(u)的节点矢量U与曲线q(t)的节点矢量Γ具有一一对应的关系，利用这一关系可以给出参数u和参数t的线性映射关系。

首先判断参数u所在的节点矢量区间[uj,uj+1]，通过曲线C(u)的节点矢量U和q(t)的节点矢量Γ的对应关系，得到参数t所在区间[tj,tj+1]。然后建立参数u，t的线性关系：

对于点Pi的参数ui，通过式（3）可以得到该点姿态在曲线q(t)上的参数ti。将ti代入姿态曲线q(t)的表达式，得到该点的单位四元数姿态表述。结合旋转矩阵和四元数转换公式[15]，可以得到姿态旋转矩阵Ri，再结合位置空间坐标Pi，得到喷枪在该点处的变换矩阵，这样就实现了位置和姿态的同步。对变换矩阵进行机器人逆运动学计算可以得到机器人6关节坐标数据。

4 AM4377端程序设计

运动控制程序及EtherCAT主站协议运行于TI公司的AM4377处理器，系统内核采用Linux+Xenomai双核架构，控制器应用程序分为Linux程序和Xenomai程序两部分。

4.1 AM4377软件总体结构

根据Linux和Xenomai各自的任务特点，AM3477程序由4个主要线程构成。AM3477程序架构如图6所示。

图6 AM4377程序架构Fig.6 AM4377 program architecture

线程A为通讯线程，通过TCP/IP协议与IPC通讯，接收上位机发来的指令数据包，并将数据包进行解析。该线程会对不同类型的指令分别注册对应的回调函数，并按时间顺序添加到相应任务链表中等待传递给Xenomai。

线程B为Linux内核的主线程，其按照任务链表的FIFO顺序，依次取出任务模块，并根据任务的类型，调用相应的回调函数进行任务处理。如果该任务是机器人再现运动，会将关节插补段信息通过XDDP通讯传递给Xenomai实时核，为后续的关节精插补运算服务。

线程C中设计了一个“系统运行参数”结构体，负责存储系统运行的关键数据和插补等缓存的位置指针，并定义该结构体的全局变量。线程C将会获取线程B传来的数据，响应Linux的读、写、点位运动、插补运动等命令。对于插补运动指令，线程C会把插补相关信息写入插补信息变量中。

线程D为Xenomai的主运算线程，该线程通过一个定时中断负责关节坐标精插补运算和EtherCAT主站通信，因此该线程是一个实时线程。当插补信息变量中存在未执行的插补数据时，线程D会按时间顺序提取出数据，然后调用关节插补算法进行处理。最后调用IgH提供的API接口将数据通过EtherCAT总线下发给Ether-CAT伺服从站。

4.2 Linux程序

在本文开发的系统中，Linux程序主要负责与IPC上位机进行信息交互，并执行喷涂机器人系统的非实时任务，如：接收上位机指令、参数设置、状态修改等任务。根据功能，其总体包括两个模块：指令传输模块和指令处理模块。

Linux应用程序与IPC之间的数据交互通讯采用TCP/IP协议。根据操作指令（如：急停、回零指令、采样指令、运动控制指令、I/O指令、状态查询）的不同，在IPC端封装了一个命令函数库接口。IPC上位机只需调用运动库中的API规定的数据结构发送数据即可实现相关运动和控制命令。图7所示为IPC上位机API接口与Linux通讯的框架结构。每次通讯都是IPC发出请求帧，下位机回应。

图7 IPC上位机API接口与Linux通讯的框架结构Fig.7 The framework of communication between API of IPC host computer and Linux

Linux与上位机交互的指令分为三类，分别是读指令、写指令和Cmd指令，指令传输模块会对不同类型的指令分别处理。读指令和写指令主要用于上位机对控制器参数和状态的查询和设置。Cmd指令是上位机对机器人和外设发出的运控和操作指令。A线程解析指令后会将该指令按照时间顺序添加到任务链表中，等待指令处理模块进行处理。

指令处理模块是AM4377端Linux程序最重要的模块，主要负责处理接收到的指令。根据机器人的工作状态，系统可以有不同控制状态：空闲状态、急停状态、喷涂示教状态、再现运动状态、点动状态、暂停状态、系统错误状态等。系统定义了一个用于描述系统状态类型的结构体，该结构体中包含系统状态相关的变量和函数指针。系统将不同的状态封装成对应的结构体，其中的函数指针在初始化时指向对应的状态处理函数。

B线程会不停地去查询任务链表是否为空，当任务链表不为空时，会按FIFO顺序取出指令进行处理，根据指令信息改变当前系统状态，并且执行该系统状态的处理函数。

4.3 Xenomai程序

Xenomai程序主要负责处理示教采样、关节精插补和EtherCAT主站通讯功能等实时任务。

4.3.1 EtherCAT主站协议移植

为了在控制系统中实现EtherCAT主站协议栈，必须将EtherCAT主站协议移植到Xenomai内核。目前开源的EtherCAT主站代码主要有RTLAB开发的SOEM（simple open source EtherCAT master）和EtherLab开发的IgH EtherCAT Master。由于IgH对EtherCAT主站实现更加完整，支持的网卡类型众多，并提供通用的网卡驱动程序，因此本文采用了移植IgH EtherCAT Master方案[16-17]。IgH提供了丰富的API接口函数，Xenomai应用程序模块调用IgH提供的API接口实现与EtherCAT从站的数据通讯。

4.3.2 XDDP协议

Xenomai提供XDDP协议帮助实现Xenomai中的RT（RealTime实时）线程与Linux域中NRT（NotRealTime非实时）线程间的通讯，并且提供了消息管道——基于RTDM的XDDP协议实现。

在Xenomai中的C线程通过XDDP协议负责与Linux进行数据通讯。线程C中设计了一个“系统运行参数”结构体，负责存储系统运行中的数据和插补等缓存的位置指针。线程C将会获取线程B传来的预处理数据，响应Linux的读、写、点位运动、插补运动等命令。对于激活主站、复位主站、获取从站信息等简单的控制指令，线程C将会直接调用IgH的API进行主站操作。对于精插补运动指令，线程C会把插补相关信息写入“系统运行参数”结构体的插补信息变量中，交给实时线程D处理。

4.3.3 Xenomai的实时线程

线程D为Xenomai的主运算线程，且为实时线程。使用Xenomai提供的rt_task_create（）函数创建实时线程，通过rt_task_start（）函数启动线程，并设置其采用最高优先级（99）。

示教采样时，线程D调用IgH的API对机器人6个关节轴和2个变位轴的EtherCAT伺服电机绝对位置编码器数值进行定时采样，采样周期为10 ms，然后通过XDDP协议将采样数据传送给B线程。

关节精插补运算在线程D中进行，系统采用时间分割插补算法进行关节精插补运算。除了插补运算外，线程D根据轴的运行状态（点位运动状态、插补运动状态、空状态等）进行EtherCAT主站控制、状态查询等运算。如果是插补运动状态，则根据“系统运行参数”变量中的插补信息进行插补运算，计算各关节坐标插补增量，然后写入插补缓存（插补结果以绝对位置模式保存）。并且线程D将插补结果写到EtherCAT过程数据域中。

线程D另一个重要任务是处理EtherCAT主从站之间的同步通讯。本文主站与从站之间采用DC通讯模式，实现EtherCAT的DC同步机制，通讯周期为1 ms。Xenomai的线程D负责调用IgH的API与EtherCAT从站进行通讯，周期需与DC周期相同，因此将线程D的运行周期设置为1 ms。

伺服从站运行于CSP（周期性同步位置控制）模式，主站每1 ms向伺服从站发送EtherCAT数据帧，数据帧结构如下结构体，ec_pdo_entry_reg_t结构体类型由IgH定义。

EtherCAT应用层协议Coe中的CIA402协议规定0x6040为伺服的控制字，0x607a为伺服的目标位置字。本文将position数组与0x607a关联，control数组与0x6040关联。调用IgH提供的EC_WRITE_S32函数可实现对EtherCAT数据帧的写操作，通过写control数组可以用于使能伺服，通过写position数组则可以驱动伺服电机运动到指定位置。

5 控制系统实现

针对拖拽示教机器人功能需求进行软硬件系统设计，实现了控制系统的开发。工人在进行拖拽示教喷涂时，控制器以10 ms为周期对机器人6个关节轴和2个变位轴的伺服电机绝对位置编码器数值，以及喷涂系统I/O状态进行采样，并且上传至上位机保存为关节采样示教文件。

上位机程序对示教文件进行示教采样数据预处理、粗插补、位姿同步等模块处理后，得到格式相同的新示教文件。图8a、图8b所示分别为机器人玻纤喷涂浴缸时新、旧示教文件喷枪位姿仿真图，图中轨迹上的短线条段代表机器人喷枪的姿态向量。优化后该段示教轨迹更加光滑，姿态变化连续，与原示教轨迹对比姿态基本同步，同时优化后示教点减少调整为349个，相比于原始示教点数量870减少了50%，效率得到调整提高。

图8 新旧示教文件轨迹位姿仿真图Fig.8 Simulation diagram of track pose of new and old teaching documents

图9所示为优化后轨迹样条的曲率变化曲线。从图9可知优化后喷涂轨迹曲率平缓，只出现有限个不大的曲率峰值。图10所示为优化前后喷枪移动速度的曲线对比图。从图10a可见，优化前喷枪速度变化剧烈、随机，若提速复现喷涂，机器人运动将出现剧烈的冲击和振动。优化后喷枪速度如图10b所示，整个运动速度按S型加减速连续平滑变化，可将粗插补最大速度提升到500 mm/s，有效提高了整体运行速度。同时，机器人没有产生冲击和振动。

图9 优化后轨迹样条的曲率Fig.9 Curvature of optimized trajectory spline

图10 优化前后速度对比图Fig.10 Speed comparison before and after optimization

再现喷涂时，上位机程序读取新示教文件，然后下发至下位机。下位机再对10 ms关节空间数据进行基于三次多项式的精插补运算，计算出每1 ms时电机的位置，即将349个点密化为3 480个点。图11为精插补过程中每一步插补所花费的时间分析图，线程D每1 ms进行一个点的精插补运算，插补运算最大耗时不超过7 μs，远远低于线程D的运行周期1 ms，可以满足周期性任务计算。

图11 精插补耗时Fig.11 Fine interpolation time

目前该控制系统已完成样机试制，并在佛山某机器人公司开展玻纤喷涂测试。根据机器人运行情况可知，系统运行稳定，机器人运动平稳，能够对多种不同的产品进行喷涂作业，且喷涂质量达标。

6 结论

拖动示教喷涂机器人具有示教喷涂效果所见即所得、无需编程、易操作等优点，是多品种、中小批生产型涂装企业解决机器人自动化喷涂的有效解决方案。本文采用IPC+AM4377上/下位机开发了基于EtherCAT总线的拖动示教六关节机器人控制系统，实现了机器人拖拽示教喷涂采样—采样数据优化处理—再现喷涂的功能要求，解决了示教数据直接用于再现喷涂时存在轨迹不平滑与速度抖动的问题，同时由于采用了EtherCAT总线控制，系统具有软硬件开发周期短、性能稳定、易维护、可扩展和低成本等优点。通过样机在企业的运行测试，该喷涂机器人控制系统能够平稳运行，取得了较好的喷涂效果，达到了设计要求。