基于EtherCAT的连续体机器人主从站设计

袁俊杰，魏任寒，何广平，狄杰建，赵磊,张忠海，周林

(1.北方工业大学机械与材料工程学院，北京 100043；2.北京航天测控技术有限公司，北京 100041)

0 前言

随着工业技术向自动化方向不断地发展，工控任务变得日益复杂，工控任务中高质量、高精度等需求对控制系统提出了通信速度快、控制精度高、抗干扰能力强、实时性高等要求[1]。这使得传统总线式的控制方式已经无法满足其要求，而工业以太网技术以高实时性、高稳定性等特点促进了工业技术的进一步发展，其中在工控领域应用较为广泛的工业以太网有EtherCAT、EtherNet/IP、Modbus、PowerLink、Profibus等[2]。

由德国Beckhoff公司提出的实时以太网EtherCAT(EtherNet Control Automation Technology)技术可以满足日益复杂的工控任务的要求，它基于标准的以太网，使用特殊以太网数据帧类型(0x88a4)，采用一主多从的通信方式[3]，通信速率高达100 Mb/s，具有非常灵活的拓扑结构，因此目前EtherCAT在机器人领域应用得越来越广泛。

近年来一类超冗余自由度的连续体机器人获得较多研究[4]，该类型机器人通常由多个软体关节组成，每个关节一般具备3个自由度，因此这类机器人需要同时对十几个甚至几十个电机进行同步控制。利用传统的多轴运动控制卡或控制器成本较高，且对上位机要求较高，系统组成和布线均较为复杂。因此提出一种基于EtherCAT通信的连续体机器人主从站控制系统，该控制系统采用一般的PC机作为主站，由自主设计的控制板作为从站，主从站之间通过EtherCAT通信，具有高实时性、高灵活性、低延时性等特点。

1 总体方案设计

主从站之间采用实时以太网EtherCAT协议作为底层通信协议，基于现有的一种连续体机器人实验平台。连续体机器人共有4节柔性机械臂，每节柔性机械臂由3根呈120°均布的钢丝绳进行拉动，而每根钢丝绳由一个步进电机带动来实现机械臂空间位姿的变化。图1所示为主从站的总体结构，从站采用自主设计的STM32F407芯片+AX58100芯片的控制板，预留了6路步进电机接口和编码器的接口以及AD采集接口等，能够实现6路步进电机的驱动控制以及各种传感器信号的采集。EtherCAT有很多种拓扑结构，包括总线形、星形和链形等等，这里采用了比较简单、应用较为普遍、通信非常稳定带有分支线的链形拓扑结构。

图1 控制系统总体结构

EtherCAT主站硬件部分可使用通常的IPC工控机，使用工控机运行EtherCAT主站时，通常用一般的网卡作为硬件接口[5]。主站软件部分使用开源的主站协议栈SOEM(Simple Open EtherCAT Master)，将开源的主站协议栈SOEM移植到开源的Linux操作系统上。由于Linux的非实时性，需要对其进行改造，在Linux操作系统上移植实时内核Xenomai，实现基于Linux的开源实时主站。

2 硬件设计

2.1 从站控制板硬件总体设计

如图2所示，EtherCAT从站控制板主要由从站控制器芯片、微处理器芯片以及外围电路组成。

从站控制器(ESC)芯片选用的是AX58100。AX58100是一款EtherCAT从站控制器芯片，集成了两个支持100 Mb/s全双工操作功能的高速以太网PHY，通过RJ45硬件接口与主站相连进行数据的传输和与其他的从站连接从而实现不同的拓扑结构，还支持CANopen(CoE)、TFTP(FoE)等标准EtherCAT协议，适用于工业自动化、运动控制等[6]。EtherCAT数据链路层的协议是固定的，这个协议已经被写入从站控制器芯片AX58100中，主要功能是实现应用层通信协议，主要负责主从站之间的协议处理，能实现EtherCAT数据链路层的所有功能，是EtherCAT通信中关键的组成部分。

微处理器芯片选用的是基于Cortex-M4内核的STM32F407，主要负责通信的应用层部分和步进电机的驱动控制部分，并将采集的编码器和传感器数据通过从站控制器通过AX58100发送给主站。从站微处理器和从站控制器一起决定通信系统的性能和功能。

2.2 AX58100与STM32间的PDI接口

PDI接口是过程数据接口的简称。AX58100具有两种过程数据接口(PDI)，FSMC接口和SPI 接口，AX58100可通过这些接口连接到外部的传统MCU以支持EtherCAT功能。SPI接口方式具有占用引脚少、传输速率高的特点，因此AX58100与微处理器STM32F407芯片接口采用SPI(串行数据总线)方式。如图3所示：SPI总线工作时有4条信号线，通常包括1条主机输出从机输入信号线(MOSI)、1条主机输入从机输出信号线(MISO)、1条时钟信号线(SCLK)、1条从机使能信号线(CS)[7]。将微处理器芯片STM32F407作为SPI通信中主设备并提供串行时钟，而控制器芯片AX58100作为SPI通信从设备。

图3 SPI接口电路

2.3 主从通信电路设计

如图4所示：AX58100芯片与RJ45接口通过5个主要的引脚相连，P0_TXOP引脚为PHY0差分传输正信号，P0_TXON引脚为差动器传输的负信号。在以太网模式下，差分信号以MDI模式通过TXOP/TXON信号传输到对应的媒体；当高电平时为光纤模式，信号对应连接到光纤收发器的TX+/TX针脚。

P0_RXIP引脚为PHY0差分接收正信号，P0_RXIN为PHY0差分器接收到的负信号。在以太网模式下，在MDI模式下在RXIP/RXIN信号对上接收来自介质的差分数据；在光纤模式下，信号对应连接到光纤收发器的RX+/RX-针脚。

P0_ACT引脚是芯片引导模式设置，用于引导模式设置以决定PHY0的媒体模式，默认为低电平时为以太网模式、高电平时为光纤模式。

3 软件设计

3.1 实时主站搭建

如图5所示，EtherCAT主站使用的是开源的主站协议栈SOEM，运行在增加了实时内核Xenomai的Linux操作系统里。它通过在标准Linux内核空间中添加一个新的实时内核Xenomai-Linux并且优先级高于普通的Linux内核，实时任务由实时内核执行，系统在不需要执行实时任务的情况下允许运行普通的Linux程序或非实时性任务从而提高实时性。

Linux可被视为实时调度程序的空闲任务，当此空闲任务运行时，它将执行自己的调度程序并调度正常的Linux进程。由于实时内核具有更高的优先级，因此当实时任务准备好运行并立即执行实时任务时，通常的Linux进程将被抢占，从而保证系统的实时性。

SOEM主站协议栈的架构如图6所示，它采用了分层设计方法，在软件上提供了一个抽象层，将SOEM主站协议栈与具体的操作系统和硬件平台分开，理论上使得SOEM主站协议栈可以移植到任意的操作系统和硬件平台上。抽象层主要由OSAL(操作系统抽象层)和OSHW(网络抽象层)两个模块组成，移植的主要内容就是在Linux的操作系统和硬件上重写OSAL和OSHW[8]。

图6 SOEM层级架构

3.2 主站软件设计

主站主程序流程如图7所示，进入主程序之后，首先是从站状态监测线程ecatcheck()，主要负责发送从站状态请求并且监测从站的异常状态，定时检测当前从站的在线状态，及时发现从站是否掉线，掉线之后重新将从站置为运行状态。然后由ec_init()函数负责初始化SOEM，将套接字绑定到指定的计算机网卡；ec_config_init()函数负责初始化从站并返回发现的从站数量；ec_config_map()函数负责配置PDO映射，将从站的输入输出映射到IOMap，并配置FMMU寄存器。IOMap是一个内存池，SOEM会将PDO的指针根据从站EEPROM内部参数来统计需要映射的输入输出长度，然后配置SM寄存器。主站向从站发出状态改变请求，检测到从站状态已经转换为运行状态后，进入主循环发送周期性过程数据。

图7 主站主程序流程

3.3 从站软件总体设计

从站主程序流程如图8所示。其中，主要有HW_Init()和Main_Init()两个从站初始化函数。HW_Init()主要是初始化相关信号的中断以及GPIO引脚和SPI接口等，Main_Init()则负责初始化EtherCAT协议的相关变量和对象词典等。两个初始化函数完成后进入到主循环函数MainLoop里实现周期性和非周期性数据交换，周期性的数据在PDI_Isr()函数中处理，非周期性事件在ECAT_Main里处理。同时判断从站是否处于运行状态，若处于运行状态则循环下去，否则退出循环。

图8 从站主程序流程

根据查询到的寄存器值的不同，可以将通信模式设置为自由运行、分布时钟和同步模式3种[9]。

自由运行模式是每个从站根据自己的时间中断来处理来自主站的EtherCAT数据，和主站的运行周期以及其他从站的周期都没有关系，主站发送数据帧的时间与从站处理数据帧的时间不同步[10]。

同步模式指的是同步管理器(Sync Manager)的同步，同步模式的触发方式是通过SM事件，每当EtherCAT数据帧在到达对应的从站时，会触发一个SM事件，当从站接收到这个事件时，会进入对应的中断服务函数来处理对应的数据。由于同步模式是根据数据帧到达特定从站的时间触发SM事件信号进行同步，那么数据帧到达每一个从站的时间必然是不同的，当控制系统很庞大的时候，每个从站接收到数据帧的时间就会相差很大，越在后边的从站接收到数据帧的时间越晚，它的同步效果就越差。

DC(Distributed Clock)Mode是一种高精度的同步时间模式，主站选取第一个从站的时钟作为参考时钟，主站及其他的从站都同步于此参考时钟，DC模式下从站会在事件到来前完成数据帧的接收和运算，使得主从的同步性大大增强。

3.4 从站状态机设计

EtherCAT状态机模块负责控制EtherCAT状态机在以下几种状态间转换，如图9所示。一般的从站状态分别是引导状态(bootstrap)、初始化状态(Init)、预运行状态(Pre-Operational)、安全运行状态(Safe-Operational)和运行状态(Operational)，引导状态一般不用设置，状态转换顺序一般为初始化→预运行→安全运行→运行，状态逐级转换，不能跳跃[11]。

图9 从站状态转换

图10所示为状态机的转换流程，负责状态机模块的主要函数为ECAT_Main()，被主函数循环调用，从站状态转换一般由主站发起，主站发出状态请求并将请求的状态写入到AL控制寄存器里面，从站接收到主站的状态请求后，读取AL控制寄存器里的值，根据不同的状态转换，检查SM寄存器的配置[12]。对于PreOP状态，检查SM0和SM1的配置；对于SafeOP状态，检查应用层输入和输出数据映射；对于OP状态，则检查SM0-SM3。如果SM配置正确，则将新状态写入状态机实际状态位并调用转换函数进行转换，并将结果写入AL状态寄存器，等待主站读取。若SM配置不正确或转换不正确，则设置错误指示位并调用错误处理函数处理，将状态未能转换的原因写入AL代码寄存器里，主站读取错误指示后清除错误指示位。

图10 从站状态请求与响应

3.5 周期性数据通信

从站周期性过程数据的处理流程如图11所示：STM32F407与AX58100之间通过中断方式通信，每当有SM事件时，AX58100向STM32F407触发一个中断信号，STM32F407响应这个中断信号后，在Sync0_Isr()中断服务函数中读取AL事件寄存器的值，更新过程数据输出映射，若是SM事件则更新过程数据，并从AX58100的SM2缓存区复制数据至STM32F407的缓冲区，然后执行主要的应用控制程序；STM32再将编码器和传感器的反馈数据写入SM3缓存区(即更新过程数据输入映射)，然后等待下一个周期被主站读取。这样即过程数据处理完成，否则退出中断服务函数。

图11 Sync0_Isr()中断服务函数流程

3.6 PID算法模块

由于计算机控制是一种采样控制，它只能根据采样值与期望值的偏差计算控制量，而不能像模拟控制那样连续输出控制量，进行连续控制。由于这一特点，公式里的积分项与微分项不能直接代入使用，必须进行离散化处理[13]。离散化处理的方法为：以T作为采样周期，作为采样序号，则离散采样时间对应着连续时间，用矩形法数值积分近似代替积分，用一阶后向差分近似代替微分。可作如下近似变换：

(1)

式(1)中，为了表示方便，将类似于e(kT)简化成ek等。

将式(1)代入一般的PID表达式，得到离散的PID表达式为

(2)

或

(3)

其中：k为采样序号，k=0，1，2，…；uk为第k次采样时刻的计算机输出值；ek为第k次采样时刻输入的偏差值；ek-1为第k-1次采样时刻输入的偏差值；Ki为积分系数，Ki=Kp·T/Ti；Kd为微分系数，Kd=Kp·Td/T；

如果采样周期足够小，则式(2)或式(3)的近似计算可以获得相对来说比较精确的结果，使得离散控制过程与连续过程相对接近。

这种算法的缺点是：采样的输出量和以前的任何状态都有关联，计算时要对ek进行累加，工作量大；同时控制系统输出对应实际位置，假如系统出现问题，uk则会大幅变化，进而会引起机构的剧烈变化[14]。

增量式PID控制算法则可以避免这种现象发生。增量式PID指的是控制器的输出只是增量Δuk。增量式PID控制算法可以通过式(2)推导出。由式(2)可以得到控制器的第k-1个采样时刻的输出值为

(4)

将式(2)与式(4)相减并整理，就可以得到增量式PID控制算法公式为

(5)

由式(5)可以看出，如果控制系统采用的采样周期T，确定了A、B、C之后，只需要使用3次测量的偏差值，即可由式(5)求出控制量。增量式PID算法与位置式PID控制算法式(2)相比，计算量小了很多，因此得到了广泛的应用。

PID算法程序如下：

float IncPIDCalc(int NextPoint,float TargetVal)//期望值

{

float iError = 0,iIncpid = 0; //当前误差

iError = TargetVal-NextPoint;//增量计算

if((iError<0.5f)&&(iError>-0.5f))

iError = 0; //|e|< 0.5,不做调整

iIncpid=(sPID.Proportion * iError)//E[k]项

-(sPID.Integral * sPID.LastError)//E[k-1]项

+(sPID.Derivative * sPID.PrevError);//E[k-2]项sPID.PrevError= sPID.LastError;//存储误差

sPID.LastError = iError;

return(iIncpid); //返回增量值

}

4 主从站测试与分析

4.1 主站任务调度测试

在Xenomai-Linux实时内核模式下，运行测试程序latency，设定周期为100 μs，测试时长1 h，测试控制系统主站的任务调度延时性。测试结果如表1所示：最大的调度延时为27.367 μs，最小的调度延时为0.015 μs，平均调度延时为1 μs左右，能够满足EtherCAT主站实时性要求。

表1 主站调度延时 单位：μs

4.2 主从站实时性测试

对搭建的整个主从站进行实时性的测试，在使用Xenomai实时内核的Linux操作系统上，用SOEM主站对从站不停地发送控制指令(如图12所示)，通过wireshark软件抓取EtherCAT报文，从抓取的报文中再筛选出由主站发出控制指令的报文，两个报文的时间差就是主站的发送周期，时间差与平均发送周期的差即为系统的通信抖动。随机连续选取了3 000组数据，得到如图13所示的结果，主站的发送周期为1 055 μs左右，而系统的通信抖动为上下浮动10 μs左右。由统计数据结果可知系统具有较高的实时性并且系统的通信稳定性较为良好。

图12 wireshark抓取EtherCAT报文

图13 系统发送周期统计

5 实验验证

为验证该主从站的控制效果和控制精度，现对其进行实验验证。实验是在现有的连续体机器人平台上进行的，该实验平台由四阶柔性机械臂组成，机械臂的主体是弹性较好的弹簧，由3根呈120°均布的钢丝绳进行拉动，而每根钢丝绳由一个步进电机带动来实现机械臂的弯曲，并实现机械臂在不同方向的运动。从站控制板使用基于Cortex-M4处理器为核心的STM32F407芯片和ASIX公司的AX58100芯片进行通信、数据处理、位置确定和算法实现等。连续体机器人实验平台如图14所示。

图14 连续体机器人 图15 运动实验

通过PID控制算法和对应的数据计算，让机械臂按照预先设定的运动方式进行运动。通过同时控制多个电机进行协同运动(其中电机最高转速为1.8 mm/s)，以中间支撑轴为原点，可以得到最终状态下机械臂弯曲了约45°，钢丝绳缩短约5 cm。具体运动实验过程如图15所示。

图15仅展示第一节机械臂的运动，图15(a)、图15(b)分别对应0、6 s时运动状态。由运动实验曲线(图16)可以得到：机械臂在整个运动过程中的运动较为平稳，在按照设定的轨迹运动时运动精度较高，基本实现了设想的功能，使得该实验成功实现。

6 结论

对于连续体机器人在控制时实时性低、稳定性差的问题，提出一种基于EtherCAT的连续体机器人主从站控制系统，得出以下结论。

(1)在开源的Linux操作系统内核里增加了一个实时内核，再基于此操作系统移植开源主站协议栈SOEM，实现了具备高实时性、低延时的开源控制系统主站。

(2)设计了从站控制器AX58100+从站微处理器STM32F407的从站控制板，AX58100与STM32F407之间通过SPI接口通信，主站能够通过网口对从站控制板发送控制指令，从站接收来自主站的控制指令和数据进行相应的数据分析和处理，对电机运动进行协调，从而实现对机械臂的控制，从站还能采集相应的编码器与传感器数据发送给主站。

(3)对主从站进行任务调度延时测试以及实时性测试，结果表明：主从站通信具有较低的延时以及高实时性，同时通信的抖动比较低，表明通信的稳定性较好。

(4)根据设计的PID算法对连续体机器人进行运动控制实验验证，实验结果表明：该主从站对多个电机的协同控制效果较好，能实现预期的运动。