EtherCAT同步模式数据丢失的伺服指令预测算法

徐建明，周 培

(浙江工业大学 信息工程学院，浙江 杭州 310023)

网络化运动控制系统已成为当前工业界的发展趋势和研究热点。随着工业对以太网伺服电机驱动器需求的增加，目前伺服系统[1]已广泛加入了总线技术，因此现在的交流伺服已经向着网络化、智能化[2]等方向发展。当前以太网技术[3]已经广泛应用于伺服运动控制系统中，现在的以太网技术无论是在传输速度上还是数据帧容量上都远远优于传统的现场总线[4]技术。如今国际上已有多种实时工业以太网协议[5]：如Modbus/TCP，TC-net，EtherNet/IP，Profinet，EPA，CC-Link IE和EtherCAT[6]等。从传输速度以及数据帧的容量方面来看，EtherCAT系统可以在30 μs内刷新1 000个I/O点的数据，在300 μs内可传输完一条多达1 486个字节的协议字节的数据帧，这就相当于12 000个数字量输入或输出，控制100个输入输出均为16个字节的伺服轴只需要200 μs[7]，这使得EtherCAT成为了工业以太网的首选。另外，EtherCAT网络完全符合以太网标准，并具有较好的开放性[8]。因此，将EtherCAT应用于伺服控制系统，能较大地提高运动控制系统的传输实时性和传输精度。工业技术的不断进步给工厂的制造方式带来了巨大的改变，北京国际自动化展以及上海工业博览会展示了很多成熟产品及应用，这很大程度上增进了各个工业强国对EtherCAT自动化产品的交流，也引起了国内工业界对EtherCAT以太网总线技术的极大兴趣。近几年，国内也自主研发了大量基于EtherCAT的数控系统[9]和移动机器人[10]等自动化产品。因此，深入研究EtherCAT网络通信技术并将其引进伺服控制系统，构建新一代的分布式网络伺服控制系统[11]具有重要的研究意义和实用价值[12]。

在网络伺服控制系统中的DC同步模式下，主站需要以非常精准的时间发送过程数据。由于受到主站处理能力的制约，过程数据不能精准送出和快速传输，从站在处理复杂应用程序时执行时间过长和主从站之间的时钟不同步等原因，系统都会不定期地丢失数据包。另外，EtherCAT没有重发机制，因此当数据帧丢失之后，主站不会再重发一次。针对此类数据包丢失的问题，采用PI控制器以补偿时间戳在量化过程中引入的误差累积[13-15]，但是这种方法需要通过大量实验确定时钟工作期间最佳的PI参数。Giorgi等[16]使用了很多不同的滤波器来获得更好的同步精度，但线性编程和Kalman滤波器等滤波算法的计算量很大，并且无法保证收敛时间；黄文君等[17]提出了一种频率补偿方法，该方法可以将收敛时间最小化为一个同步周期，但这种方法可能会导致精度略有下降；Chen等[18]通过定期收集参考时钟数据来计算主时钟和参考时钟比值的具体值，主时钟根据具体值更改周期时间来调整其时钟速度。目前，对于数据的补偿还没有快速有效的办法，NC轴发送的指令数据如果发生丢失，不仅会导致伺服电机系统的控制品质下降，而且会导致电机的不可靠性运行。笔者针对EtherCAT从站指令数据丢失[19]的问题，提出一种基于拉格朗日N次多项式的预测算法来预测缺失的当前目标位置值。首先，构建经过该丢失点之前N个目标历史数据值的N-1项多项式；其次，通过该函数曲线外推出第N+1个数值作为当前目标位置值；最后，应用层函数将该数据值通过SPI通信发送给驱动芯片，进而驱动电机转动至目标位置。针对数据帧丢失情况使用自主设计的步进[20]伺服驱动器和TwinCAT上位机来验证该方法的实用性，结果表明该方法能准确预测出缺失位置点的目标位置值。

1 基于EtherCAT的伺服网络控制系统

笔者主要研究的是基于EtherCAT技术的伺服运动控制系统控制指令数据丢失的问题。如图1所示，基于EtherCAT的伺服运动控制系统分为控制层、通信层和执行层。控制层采用PC机和TwinCAT组态软件作为系统的主站，其功能主要是负责系统的控制运算和与从站的数据通信，由主站发送EtherCAT发送数据帧到从站控制器ESC来读写其内部存储区的数据，以完成数据通信，数据通信方式包括邮箱数据通信和过程数据通信，在过程数据通信过程中，主站基本使用逻辑读、写或交换等命令对从站设备的数据进行处理；邮箱数据通信时通过读写从站的通道管理器管理的缓冲区地址，然后通过判断返回的数据帧命令的WKC来判断读写是否成功。TwinCAT3主站扫描从站，配置从站的基本参数，然后开始运动路径的规划和速度规划。主站将会在每个DC同步周期向从站控制器发送控制字、目标位置和目标速度等控制命令。以LAN9252芯片作为EtherCAT从站控制器来组成通信层，其主要功能是通过解析运动控制指令下发给执行层，也就是伺服驱动器，从站MCU将在数据交互的函数中把目标位置和目标速度等指令数据通过SPI通信发送给TMC5160驱动芯片，进而驱动电机旋转。EtherCAT从站主要包括通信层和执行层，从站单元的核心就是从站控制器ESC，从站控制器内部直接固化了数据链路层和物理层协议。当主站下发的数据帧经过ESC时，完全由硬件来完成对数据帧的读写，然后通过内部存储来完成主站和从站之间的数据交互[21]。当主站下发的报文经过ESC时，ESC会从中提取主站发送给对应从站的指令数据，然后将其保存在内存中，等待从站MCU的读取，同时将从站发送给主站的数据从内存写入到对应的子报文中。

图1 基于EtherCAT的伺服网络控制系统整体结构图Fig.1 The overall structure diagram of the servo network control system based on EtherCAT

从站MCU主要负责建立EtherCAT通信对应用层进行控制，然后通过读写函数定时对ESC内部存储区[22]进行数据的读写。针对TwinCAT3发送控制命令数据发生数据丢失的情况，笔者在EtherCAT伺服从站的运动控制函数中对主站下发丢失位置点通过拉格朗日多项式来预测目标位置值。当主站下发的数据缺失后，ESC内部存储区没有得到数据更新，这将造成从站MCU没有得到下发的控制命令和数据，因此应用层函数从内存查询到的过程数据还是上一个周期的数据，故需要用历史接收到的数据值预测出当前目标数据值，并保存到本地内存作为当前的目标位置，然后再通过应用层函数将目标位置通过SPI通信发送给驱动芯片，进而驱动电机转动至目标位置。

2 DC同步丢帧分析

分布时钟(DC)可以使所有的EtherCAT设备使用相同的系统时间，从而控制各个设备的任务能够同步执行。从站设备可以根据同步系统产生同步中断，用于中断控制或触发数字量输入/输出。DC模式将取第一个从机作为参考时钟，主站及从站同步于参考时钟，从站在PDI中断(数据收/发事件中断)发生时从接收到的过程数据帧中复制出数据，然后等待SYNC信号到达后完成数据的交换和运算。同步于SYNC事件的本地周期如图2所示。

图2 同步于SYNC事件的本地周期Fig.2 Synchronous to the local period of the SYNC event

这种DC模式利用SM Event事件信号的触发完成了前期从数据帧中提取对应从站的数据然后复制到微处理器内存中，所以当Sync0 Event同步信号触发后，只需要从内存中读取数据并完成计算。

在这种模式下会出现一种同步丢帧的情况，当数据帧还没到达从站或者从站还没来得及将接收到的过程数据复制到内存中时，从站的Sync0 Event同步事件的信号就已经触发了，而此时从站却从管理器上获取不到数据帧中所对应的数据，从站就会判定数据帧丢失。如果从站驱动器在通信中连续丢失值大于所设定最大丢失值，就会给主站一个0x1a的报警信号并要求切换状态到Safe OP。SM事件中断计数器处理流程如图3所示，当SM Event事件信号触发后，先对SM同步中断计数器清零，再判断数据丢失值是否大于0以决定是否将丢失计数器作减1运算，以此保证驱动芯片不会因电机运转中偶然的数据帧丢失导致通信中断。同步中断计数器处理流程如图4所示，当Sync0 Event同步信号触发后，先对SM中断计数器进行判断，如果大于1表明其在同步中断之前并没有进入SM事件中断函数中接收主站下发的数据帧，因此对数据丢失计数器的值作加运算。

图3 SM事件中断和同步中断计数器处理流程图Fig.3 SM event interrupt and synchronous interrupt counter processing flowchart

图4 同步中断中计数器处理流程图Fig.4 Flow chart of counter processing in synchronous interrupt

程序在EtherCAT_CheckTimer()函数中设置了定时1 ms的喂狗程序，每隔1 ms从站都会检查丢失数据帧计数器。当丢失数据帧计数器值超过所设置的最大丢失值后，将SM中断序列有效标志位置0，然后Mainloop()函数中的检查错误函数向主站发送报警信号0x1a，并切换状态。

3 基于拉格朗日多项式预测DC伺服指令

EtherCAT从站以确定的时间周期向ESC读取过程数据objTargetPosition和objTargetVelocity等轴变量，如果没有发生数据帧丢失，直接将轴变量赋值给本地变量i32PositionTargetValue和i32VelocityTargetValue。若数据帧丢失，先对缺失目标位置点用拉格朗日多项式构造出经过历史目标位置值的趋势线，然后预测目标位置再进行赋值。运动控制程序的数据预测流程如图5所示，在进入图5周期性同步位置模式CSP模式后，首先判断SM事件中断计数器是否大于1，当该计数器大于1时，表明在本次DC中断的过程中发生了数据帧丢失的情况，则通过拉格朗日多项式预测丢失目标位置，然后对其进行赋值和处理。如果在运行过程中出现数据持续丢失的不可靠状态，数据帧丢失计数器的值会增加到所设置的最大值，从站向主站发送报警信号并中断网络以保证电机的安全稳定运行。

图5 数据预测流程Fig.5 Data estimate process

所提出的基于拉格朗日插值法定义为设有n+1个互不相同的节点x0，x1，…，xn处的y0，y1，…，yn(即该函数经过(x0，y0)，(x1，y1)，…，(xn，yn))，则存在唯一的多项式，即

yk=Pn(xk)

(1)

设Fn是关于点(x，y)的角标的集合，Fn={0，1，2，3，…，n}，作n+1个多项式Pj(x)，j∈Fn。对于任意的k∈Fn，都有Pk(x)，Dk={i|i≠k，i∈Fn}，使得

(2)

式(2)为n次多项式，且满足∀m∈Dk，Pk(xm)=0并且Pk(xk)=1，最后得到拉格朗日插值多项式f(x)，即

(3)

若从站运行在DC同步模式出现数据帧丢失，将会导致电机不能平稳运行。图6为在电机运行中连续出现数据丢失的一部分脉冲曲线，Z1，Z2，Z3，Z4点的数据值丢失后一直保持着上一个周期的数据值，但是上位机是按照运动规划的目标位置下发的，因此Z5点接收到的目标位置为上位机按照正常运动规划下发的位置值，这就会导致接收到的位置值发生跳变，而Z5点位置值的跳变会导致电机的速度发生跳变，电机在运行状态下会发生抖动，产生不安全动作。

图6 位置值连续丢失曲线Fig.6 Position value continuous loss curve

图7中的*号标记曲线是通过拉格朗日多项式对每个丢失位置点预测后得到的曲线。从站应用函数通过SM事件同步中断计数器的值来判断是否发生数据帧丢失，如果没有数据帧丢失，就直接对主站下发的命令数据进行赋值。否则，通过将前6个历史数据值代入式(3)，求出一个五次多项式，然后将x=x6代入，解得目标位置。

图7 位置值丢失预测曲线Fig.7 Position value loss fitting compensation curve

如图7所示，选取丢失位置点(x6，y6)的前6个已知的数据{(x0，y0)，(x1，y1)，(x2，y2)，(x3，y3)，(x4，y4)，(x5，y5)}形成一组新的点列，作出一条五次多项式函数使其经过这组点列，然后将丢失位置x6代入函数中求出缺失的目标位置值。同理，之后的丢失点在每次预测运算之前，首先都需要对即将预测的目标位置的前6个历史位置值进行保存，然后通过构造的趋势线外推预测丢失位置点的数据值，最后应用层函数将该数据值通过SPI通信发送给驱动芯片，进而驱动电机转动至目标位置。

4 实验与结果分析

笔者在实验中采用倍福TwinCAT 3.0作为主站，从站将采用自主设计的基于STM32F407VET6和LAN9252的EtherCAT伺服驱动器。驱动器采用48 V开关电源供电，配套增量式编码器在TOYO的丝杆滑台模组上面进行实验。基于EtherCAT的伺服驱动控制系统实物图如图8所示。

图8 EtherCAT伺服驱动控制装置实物图Fig.8 Physical image of EtherCAT servo drive control device

首先将设备描述XML文件放置到TwinCAT规定路径的文件夹内，然后扫描从站进行XML烧写，等烧写完成后，再次扫描设备从站，就可以看到设备和设备描述文件中设置的参数列表。扫描到从站之后，会自动新建一个TwinCAT NC轴。在使能伺服电机轴之前，先对NC的编码器(Enc)、驱动器(Drive)和PID(Ctrl)控制器等进行设置参数，设置轴参数如表1所示，设置完成后点击“Download”按钮生效新设置的参数。

表1 NC轴参数Table 1 NC axis parameters

在DC模式下，主站设置10 ms的分布时钟周期给伺服电机轴上使能，然后对工作模式写入十进制数8进入CSP模式。设定以起始位置为0，10 mm/s的速度，30 mm/s2的加速度运动到目标位置10 mm后再运动到起始位置进行往返运动为实验目标。图9是调试页面，当Status的Ready标记为True，然后上位机开始模式设置成Reversing Sequence、目标位置1设置10 mm、目标位置2设置0 mm和目标速度设置10 mm/s后，点击Start，电机就会跟随主站下发的位置值转动。

图9 TwinCAT主站调试界面Fig.9 TwinCAT master station debugging interface

当主从站时钟不同步、分布时钟周期短或从站长时间执行复杂程序时，从站接收的数据帧会连续丢失，并向主站发送报警信号。如图10所示，在实验中设置最大缺失值为8，当丢失计数器的值没有超过8时，主站正常运行。一旦丢失计时器的值等于8时，如图11所示，主站就会收到从站发来的0x1a报警信号，并将状态切换到了Safe OP。

图10 计数器限制值和SM事件丢失计数值Fig.10 Counter limit value and SM event lost count value

图11 TwinCAT主站收到报警信号Fig.11 TwinCAT master station receives an alarm signal

主站会在每个NC周期和伺服驱动器交换数据，目标位置、目标速度、控制字和状态字都会在每个NC周期进行更新。本次用目标下发的位置值来验正预测补偿之后位置曲线的平滑度，目标位置的波形可以通过TwinCAT软件的示波器插件显示出来，结果如图12所示。

图12 目标位置波形图Fig.12 Target position waveform

当NC轴将指令下发到驱动器上，驱动器在输出数据映射前执行其他接口函数的应用程序，主站设置分布时钟为2 ms。从站以2 ms为固定周期处理和运算主站下发的指令数据，从站在复制输出数据前执行其他接口函数的程序，导致在SYNC同步事件发生时从站还未完成接收主站发送的指令数据，因此出现传输过程中数据帧的丢失，丢失数据点的数据值将会保持上一个周期的值，导致驱动器在实际运行中所接收到的位置曲线如图13所示。对这种情况采用拉格朗日多项式来预测丢失点的目标位置，经过对缺失位置点预测补偿后的位置曲线如图14所示。

图13 丢失数据后的位置波形图Fig.13 Position waveform after data loss

图14 拉格朗日多项式预测补偿后的位置波形图Fig.14 Position waveform after Lagrangian polynomial prediction and compensation

图15为电机在往返运动的过程中位置命令值与编码器实际返回值的偏差值，从图15中可以看出：在数据丢失点的位置，跟踪误差值会出现跳跃值。图16为采用拉格朗日多项式预测算法后的跟踪误差曲线，预测补偿后偏差值不会出现跳变值，而且曲线较光滑。从图14，16中可以看出：应用文中所给数据预测算法是有效的，能够很好地实现对丢失数据的预测。

图15 丢失数据后的跟踪误差Fig.15 Tracking error after lost data

图16 拉格朗日多项式预测补偿后的的跟踪误差Fig.16 Tracking error predicted after Lagrange polynomial

5 结 论

首先，分析了运动控制中出现以太网数据帧丢失情况的原因；其次，针对数据丢失的问题提出一种基于拉格朗日N次多项式的预测算法来预测缺失位置点的数据值，EtherCAT从站通过SM事件同步中断计数器是否大于1来判断该位置是否为丢失位置点，若该点为丢失位置点，则通过该点前N个历史位置值建立一个经过这N个点的N-1阶拉格朗日多项式，再通过曲线外推预测出丢失的当前目标位置；最后，应用层函数将目标位置通过SPI通信发送给驱动芯片，进而驱动电机转动至目标位置。通过实验测试，该方法能准确预测出缺失位置点的目标位置值。