EtherCAT姿态监测从站的设计与实现*

陈乐南，高伟强，黄国静，刘建群

(1.广东工业大学 机电工程学院，广州 510006；2.佛山市智昂科技有限公司，广东 佛山 528225)

0 引言

姿态监测技术在航空航天、汽车制造、医疗器械、工业机器人、智能机器人、交通设备等领域有着广泛的应用。MEMS(Micro-Electro-Mechanical-System)是采用微机械加工技术制造的新型传感器[1]，可用于检测角速度、加速度等物理量。处理器根据加速度和角加速度等姿态信息，融合相关姿态解算算法，即可求得运动物体的姿态角：横滚角Roll、俯仰角Pitch、偏航角Yaw。

传统的控制系统中，主控制器通过CAN、RS232、RS485等串行通讯总线与外设连接，获得外设信息。这些传统总线的传输速率、传输距离、数据量、实时性、兼容性、拓扑能力、稳定性等方面均不如工业以太网。随着通讯技术的发展，越来越多的控制器采用实时现场总线协议与设备层通讯。

EtherCAT是一种高速的开放式实时以太网总线，在连接伺服驱动器、传感器等设备的控制系统中有着广泛应用。目前现有的EtherCAT从站模块大多具备IO、AD、DA转换功能，还未实现姿态监测功能。将MEMS传感器与EtherCAT总线相结合，是实现机器人等智能设备姿态实时监测的可行方案。本文采用LAN9252、STM32、MEMS芯片，开发了一种低成本的EtherCAT姿态监测从站，实现对物体姿态的实时采样和监测。

1 基于EtherCAT的姿态监测系统

EtherCAT是德国BECKHOFF公司提出的开放式实时以太网，它具有高速、实时性、兼容性强等特点，支持几乎所有拓扑结构。EtherCAT系统采用主从通信结构，主站发出下行电报数据帧遍历所有的从站，每个从站在数据帧经过时分析寻址到本站数据，并根据数据帧中的命令来提取数据或插入返回数据[2-3]。

如图1所示，设备层的多种监测和驱动设备通过EtherCAT总线连接在一起。以采样示教喷涂机器人为例，机器人在对工件进行喷涂的过程中，需要沿着预设的轨迹移动，并且不断改变喷枪的姿态，使喷漆能够均匀覆盖工件表面。将本文设计的EtherCAT姿态监测从站固定于喷涂机器人执行器末端，用于实时采样监测喷枪的姿态，同时将姿态信息作为输入量经EtherCAT总线反馈给EtherCAT主站。整个系统将构成闭环控制系统，当喷枪的姿态与预设的姿态有偏差时，主站将控制伺服电机转动，从而修正喷枪姿态。

图1 EtherCAT系统

2 EtherCAT从站硬件配置

EtherCAT从站设备同时实现通信和应用处理两部分功能。主要由LAN9252从站控制芯片、STM32从站微处理和MEMS传感器模块等构成。硬件设计部分采用模块化的设计思想，从站整体结构如图2所示。

图2 从站硬件结构

2.1 LAN9252电路设计

LAN9252从站控制芯片是实现EtherCAT数据链路层协议的专用集成电路芯片，用于处理EtherCAT数据帧，可以支持绝大部分时序要求严格的功能。其具有4K字节的双端口存储器(DPRAM)、4个同步管理器(SyncManager)、3个现场总线存储器管理单元(FMMU)、1个64位分布式时钟和双集成以太网PHY。主站与从站之间可以通过RJ45接口网线连接。

EEPROM用于存储LAN9252的相关设备信息，LAN9252在上电或复位后将通过IIC总线自动读取EEPROM中的内容并装入LAN9252的相应寄存器。

2.2 从站微处理器及外设电路设计

STM32从站微处理器用于处理应用层事件，STM32与LAN9252之间采用FSMC接口进行数据交互[4]。

本文选用的MEMS传感器型号为MPU9250，其内部集成有3轴加速度计、3轴陀螺仪和3轴磁力计，拥有16位分辨率，自带可编程数字滤波器。MPU9250自带的数字运动处理器(DMP)，可以整合9轴传感器数据，通过IIC总线向应用端输出完整的9轴融合演算数据，其姿态解算频率可达200Hz，具有较高的精度和实时性。

STM32与MEMS之间采用IIC通讯，MEMS传感器将采集到的加速度、角速度、磁场数据通过IIC总线传递给STM32，STM32经过姿态解算得到物体的姿态角。STM32可以通过CAN、RS485、IIC、SPI总线与外设连接，从而实现对多种外设的控制。板子采用3.3V供电，最终设计的实物图如图3所示。

图3 从站PBC电路板

3 XML设备描述文件

XML文件是设备描述文件，用于描述EtherCAT协议报文[5]，其烧录在板子的EEPROM中，LAN9252上电后，会从EEPROM加载配置参数，完成初始化过程[6]。CANopen是基于CAN总线的应用层协议，EtherCAT在应用层支持CANopen协议[7]。本文采用周期性过程数据通讯，使用对象字典映射过程数据。姿态监测过程中，从站传递给主站的EtherCAT数据帧中包括横滚角Roll、俯仰角Pitch和偏航角Yaw数据，需要对XML文件进行修改。

首先对过程数据对象(PDO)进行数据分配，RxPdo为主站发送给从站的数据，所用的SM(SyncManager)通道为SM2，TxPdo为从站发送给主站的数据，所用的SM通道为SM3，姿态角数据则包含在TxPdo映射中。

本文主要修改了0x1A02、0x6020两个索引的对象字典。首先在DT6020数据类型中，添加了pitch、roll、yaw这3个INT型数据，子索引分别对应十进制的18、19、20。TxPdo的组成由映射关系0x1A02的值决定，0x1A02里映射的内容为0x6020和0x1802的字典的内容，如图4所示。索引0x6020的子索引0x12、0x13、0x14则分别对应pitch、roll、yaw数据。

图4 IA02映射内容

4 从站程序设计

从站STM32程序主要实现应用层功能，包括状态机处理、周期性过程数据的处理、非周期性邮箱数据的处理、姿态解算、外设驱动、读取外设数据等[8]。

4.1 STM32程序结构

STM32程序中首先完成硬件的初始化，EtherCAT功能函数的初始化，然后周期性判断当前从站的运行模式[9]。EtherCAT从站程序结构如图5所示。

图5 EtherCAT从站程序结构

(1)硬件初始化部分用于初始化GPIO口、FSMC驱动、MPU9250九轴传感器、IIC驱动、LED驱动、LAN9252设置等。

(2)EtherCAT初始化部分用于初始化EtherCAT功能模块。包括初始化EtherCAT状态机、邮箱、紧急报文、COE字典等。

(3)本文采用FreeRun自由运行模式，ECAT_Application()函数实现对应用数据的处理，包括对MEMS采集到的加速度、角速度、磁场数据的读取，并通过姿态解算算法得到物体的Roll、Pitch和Yaw姿态角。

4.2 姿态解算程序

通过MPU9250采集到的是3轴角速度、3轴加速度和3轴磁感应强度数据。STM32通过IIC通信接口读取MPU9250的相关寄存器，即可得到9轴姿态数据。利用得到的9轴原始数据，进行姿态融合解算，就能求出运动物体的姿态角：Roll、Pitch和Yaw。

MPU9250自带了数字运动处理器DMP，并且官方提供了一个嵌入式运动处理库MPL。结合DMP，移植MPL软件库到STM32程序中，可以将传感器的9轴原始数据，直接转换成四元数输出[10]。利用四元数，即可求出姿态角。相较于卡尔曼滤波、互补滤波等姿态解算方法，本方案中STM32不用参与姿态解算过程，从而有更多的时间去处理其他任务，提高了系统的实时性。

本文采用了MPU9250传感器自身的坐标系，绕X轴旋转的为横滚角Roll，绕Y轴旋转的为俯仰角Pitch，绕Z轴旋转的为偏航角Yaw。姿态角的正负满足右手螺旋法则。MPU9250自身坐标系如图6所示。

图6 MPU9250自身坐标系

5 从站测试

本文采用装有TwinCAT 3.0软件的PC机对EtherCAT从站进行测试，并传递实验数据。同时，使用Qt开发的“三维姿态监测软件”与TwinCAT进行ADS(自动化设备规范)通讯[11]，从而实现数据交互。

5.1 建立通讯

在TwinCAT中新建工程，扫描得到从站设备。在PLC程序中添加3个变量，分别对应从站的横滚角Roll、俯仰角Pitch和偏航角Yaw，然后将PLC程序中定义的变量与上传的姿态角数据进行链接。“三维姿态监测软件”通过ADS通讯协议获取到变量值，从而将Roll、Pitch和Yaw姿态角变量曲线在图表栏里显示出来。图7为Roll、Pitch和Yaw姿态角的实时曲线图，右下角为实时姿态角度数。

图7 姿态角实时曲线

5.2 姿态监测测试

实验数据传输过程中将监测到的姿态数据放大了100倍，TwinCAT的通信周期设为5ms，从站的DMP输出速率设置为100Hz。通过TwinCAT或者“三维姿态监测软件”可以观察变量的实时值。

姿态角的监测精度，主要取决于传感器自身的精度和算法性能。将从站放置于水平面上，作为初始位置。由于电路板的焊接、安装等原因，MPU9250芯片并没有处于完全水平状态，并且会受到一些干扰，此时Roll、Pitch、Yaw的值分别为81、96、-259，称之为初始值。测量值均放大了100倍，后续测量的角度值减去初始值才是实际角度值。利用横滚角Roll、俯仰角Pitch、偏航角Yaw分别测量30°、45°、60°角，采样数据点为300个，即3s内的测量值，监测结果如表1所示，可知测量的误差控制在±1°内。

表1 姿态角精度测试

6 结论

本文基于EtherCAT从站提出了一种姿态监测方案，详述了该方案的软硬件实现过程。利用EtherCAT总线的高速高实时以及支持拓扑连接等特性，实现对多物体的姿态监测。最后，通过TwinCAT软件与EtherCAT从站构成了一个完整的测试系统。经实验验证，采用低成本的MEMS姿态传感器，本文研究的EtherCAT姿态监测从站的监测误差控制在±1°内，满足喷涂机器人、平衡车运行、无人机飞行等一般工业、生活场景的姿态角度实时采样、监测的需求。