基于EtherCAT的六自由度自平衡平台设计

贺顺 温秀兰

摘要

为解决传统自平衡平台响应速度慢、控制精度低的问题，本文提出了基于EtherCAT总线的六自由度自平衡平台设计方法，由6台支持EtherCAT通信的伺服驱动器带动6条电动缸做伸缩运动，平台的倾角数据由陀螺仪读取并通过EtherCAT传输到主站，并利用自抗扰控制算法对平台进行精确控制.实验结果表明，所设计平台具有低延时和较高的抗干扰能力等优点.关键词

EtherCAT通信;Stewart平台;自抗扰控制

中图分类号 TP273;TP242

文献标志码 A

0 引言

自平衡平台在海运、灾区救援等领域广泛应用，其响应速度快慢及控制精度高低将直接影响财产和生命安全，因此多年来已有众多学者开展了相关研究.其中六自由度自平衡平台因具有自由度多、刚度高、精度高、承载能力强以及可模块化生产等优点被广泛使用.在六个自由度运动中，对船舶的行驶过程干扰最严重的是垂荡、横摇以及纵摇三种情况，其中又以横纵两个方向的影响最为严重[1].

文献[2]从平台的构思设计到理论分析、从机械建模到动力学分析、从控制系统回路仿真到最终控制算法的软件编程实现与验证，形成了一套完整的开发设计方案，但在控制系统仿真与后期设计实现过程中，未能进行位姿大闭环，整个系统仍是以开环控制为主，因而最终姿态误差较大.文献[3]分析了PID控制对系统的跟踪特性、响应性能等动态性能参数的影响.文献[4]为各单个通道分别设计了模糊PID控制器，其优点就是能够根据误差和误差的变化率实时地调节比例系数kp、积分系数ki和微分系数kd三个参数，使稳定平台的稳定精度得到进一步提高.现有的自平衡平台大多是基于脉冲控制或CAN总线实现通信，控制算法采用PID实现，存在响应速度慢、控制精度低等问题，难以满足目前对海运质量提出高要求的需要.Ether CAT以其数据传送能力强、技术成熟度高、开发成本低等优点，受到工业控制现场总线领域的广泛关注，已成为全球公认的标准，应用日趋广泛[5-6].

本文在搭建Stewart结构的六自由度自平衡平台基础上，采用EtherCAT總线将上平台安装的陀螺仪数据实时读入TwinCAT主站，采用自抗扰控制策略平台进行精确控制，并对实验平台进行实验验证，实验结果证明了Stewart平台控制的有效性.

1 系统总体方案设计

考虑到EtherCAT具备灵活的网络拓扑结构，系统配置简单，实施的成本低廉等特点，搭建的自平衡平台总体结构如图1所示.以倍福主站作为核心单元，运行TwinCAT3.1软件，作为实时工业以太网，EtherCAT 充分利用以太网全双工特性，采用主从站通信方式，主站通过EtherCAT网络连接6台伺服驱动器从站和1个陀螺仪传感器从站.驱动器通过带动电缸实现直线伸缩运动，进而改变平台的位姿，陀螺仪将采集到的姿态数据实时反馈给主站.

2 EtherCAT通信设计

2.1 陀螺仪从站设计

姿态闭环控制需要得到实时的反馈数据，所以需要设计一套支持EtherCAT协议的陀螺仪从站.陀螺仪从站由带串行接口的9轴传感器模块和LAN9252从站模块组成.陀螺仪采用广州阿路比电子科技有限公司生产的LPMS-UTTL2 9轴陀螺仪，该陀螺仪通过采集9轴传感器数据并对其进行融合滤波，TTL串口输出所需的欧拉角，串口波特率为921.6 kbit/s，数据采样率400 Hz.陀螺仪将处理好的数据输出给STM32.

STM32和陀螺仪模块之间采用串口DMA通信，通信协议采用LPBUS协议.LPBUS协议是基于工业标准的Modbus协议所设计的通信协议.这是LPMS设备默认的通信方式.LPMS设备具有数据流模式（Streaming Mode）和命令模式（Command Mode）两种.LPMS姿态传感器上电后，等初始化设备内部初始化完成后，就会直接进入数据流模式且默认频率为100 Hz.陀螺仪参数设置流程如图2所示.

由于陀螺仪只传输串口数据，所以本文利用LAN9252实现EtherCAT三层协议中的物理层和部分数据链路层，利用STM32实现EtherCAT的应用层协议和陀螺仪数据的读写，两者之间基于STM32的FSMC外设与LAN9252的HBI接口进行通信，采用16位变址寻址方式，实现串口转EtherCAT从站控制器电路.

LAN9252集成高性能100 Mbit/s以太网收发器，作为EtherCAT从控制器，它支持3个FMMU，支持4个SyncManager，支持64位分布式时钟，有4KBDPRAM.它有8/16位主机总线接口，支持变址寄存器或复用总线，支持SPI/QSPI，其数字I/O模式可以优化系统成本，第3个端口可实现灵活的网络配置[7].STM32F407VET6是意法半导体公司基于ARMCortexM4内核的32位微控制器，有着1 MB片内Flash和192 kB片内RAM，并包含UART、ADC和FSMC等多种外设.

EtherCAT从站板应用层采用CANopen over EtherCAT协议，不需要支持DC同步，可以将信号统一交给TwinCAT主站，由主站对EtherCAT报文进行分析与后续操作[8].EtherCAT从站的硬件连接如图3所示，其实物如图4所示.

2.2 TwinCAT主站程序设计

TwinCAT主站作为整个系统的控制核心，负责接收反馈并发送控制信号.首先，TwinCAT扫描台达伺服从站，新建NC轴，修改电机轴的速度、加速度和编码器参数，修改DC同步周期为2 ms.利用NC轴可以对生成轮廓时具有加速度限制以及对速度和加速度的预控制，将跟随误差降到最小[9].激活主站后即可点动测试伺服，伺服测试完毕后新建PLC工程，将Stewart平台运动学算法使用ST结构化语言写入程序块，使用外部位置给定函数即可对平台位姿进行精确控制.TwinCAT扫描的I/O如图5所示.

3 运动学分析

对Stewart平台做自平衡控制，其运动学逆解也是其关键一步，图6是其结构简图.

如图6所示，在下平台建立静止坐标系O-XYZ，其中X轴平行于铰点A4、A5 所在直线.在上平台建立动坐标系o-xyz，上平台的运动可分解为X，Y，Z方向的平移，以及绕其坐标轴X，Y，Z的旋转.下平台各铰点Ai的坐标为（XAi，YAi，ZAi），上平台各铰点ai的动坐标为（xai，yai，zai），静坐标为（Xai，Yai，Zai），与其对应相连的电动缸长度为li（i=l，…，6）.

动坐标系相对于静坐标系的位置用矢量P描述：

P=xyzT.（1）

设下平台定坐标系为{A}，上平台动坐标系为{a}，经过推算后动平台的各点坐标相对于静平台的各点坐标的方向转换矩阵表示如下

5 实验测试结果

控制系统由模拟海浪的三自由度平台和控制自平衡的Stewart平台构成，伺服驱动器和TwinCAT工控机置于三自由度平台内部，陀螺仪固定在上平台，陀螺仪的X轴与上平台的X轴对齐，平台整体实物如图8所示，电气连接实物如图9所示.

当三自由度平台摇摆幅度设定为3.2°，周期为4 s时，绕α轴和β轴的摇摆曲线如图10所示.不同kp，kd情况下α轴和β轴的控制曲线分别如图11和图12所示.由实验结果可见，当kp=4 000、kd=28时，控制效果最好.ADRC与传统PID波形对比分别如图13和图14所示.由实验结果可见，ADRC控制的波形对于位置跟踪性能较传统PID性能好，尤其是α角控制曲线效果更好，对负载扰动有更好的抑制作用.

6 结语

本文将Stewart结构的并联机器人应用于自平衡平台，分析了Stewart结构的运动学逆解，设计了基于LAN9252的陀螺仪从站，并基于TwinCAT主站编写了自抗扰控制算法，对航运影响最大的横摇和纵摇方向进行自平衡控制，最后对控制性能进行了分析.实验结果表明搭建的基于EtherCAT的Stewart自平衡平台方法可行，自抗扰控制的精度较高，易于在海运、灾区救援等领域推广应用.

參考文献

References

[1]

刘学功，宋振海，张宏凯.船舶推进中两种动态平衡的研究[J].舰船科学技术，2010，32（4）：32-34，108

LIU Xuegong，SONG Zhenhai，ZHANG Hongkai.Research on two kinds of dynamic balance in ship propulsion[J].Ship Science and Technology，2010，32（4）：32-34，108

[2] 高建峰.基于Stewart结构的六自由度并联稳定平台技术研究[D].济南：山东大学，2018

GAO Jianfeng.Research on 6-DOF parallel stable platform technology based on Stewart structure[D].Jinan：Shandong University，2018

[3] 隋毅.基于并联机构的海上稳定平台的机液联合仿真研究[D].青岛：中国海洋大学，2015

SUI Yi.Research on machine-hydraulic joint simulation of offshore stable platform based on parallel mechanism[D].Qingdao：Ocean University of China，2015

[4] 刘义德.基于并联机构的稳定平台建模与控制[D].哈尔滨：哈尔滨工业大学，2009

LIU Yide.Modeling and control of stable platform based on parallel mechanisms[D].Harbin：Harbin Institute of Technology，2009

[5] 张颖，平雪良，王晨学，等.ROS下基于EtherCAT的串联机器人控制系统[J].传感器与微系统，2018，37（3）：106-109

ZHANG Ying，PING Xueliang，WANG Chenxue，et al.EtherCAT-based serial robot control system under ROS[J].Transducer and Microsystem Technologies，2018，37（3）：106-109

[6] 董海涛，朱国庆，黄丽宇.基于LAN9252的伺服驱动EtherCAT通信接口实现[J].组合机床与自动化加工技术，2019（4）：95-98

DONG Haitao，ZHU Guoqing，HUANG Liyu.Servo-driven EtherCAT communication interface implementation based on LAN9252[J].Modular Machine Tool & Automatic Manufacturing Technique，2019（4）：95-98

[7] Microchip Technology Inc.LAN9252 datasheet[EB/OL].[2019-10-28].http：∥www.eeboard.com/wp-content/uploads/downloads/2017/12/LAN9252-Datasheet.pdf

[8] Kang C，Pang Y，Ma C，et al.Design of EtherCAT slave module[C]∥IEEE International Conference on Mechatronics and Automation，2011：1600-1604

[9] 張炜岸，吴世林.基于TwinCAT的机器人虚拟控制系统设计[J].制造技术与机床，2019，683（5）：63-66

ZHANG Weian，WU Shilin.Design of TwinCAT-based robot virtual control system[J].Manufacturing Technology & Machine Tool，2019，683（5）：63-66

[10] 王莉，郭伟.分数阶PID控制在网络控制系统中的应用[J].南京信息工程大学学报（自然科学版），2009，1（4）：309-313

WANG Li，GUO Wei.Application of fractional order PID control in networked control system[J].Journal of Nanjing University of Information Science & Technology（Natural Science Edition），2009，1（4）：309-313

Six degree-of-freedom self-balancing platform design based on EtherCAT

HE Shun1 WEN Xiulan1

1

School of Automation，Nanjing Institute of Technology，Nanjing 211167

Abstract In order to solve the problem of slow response and low control precision for the traditional self-balancing platform，a six degree-of-freedom self-balancing platform is designed based on EtherCAT bus.Six servo drivers that support EtherCAT communication drive six electric cylinders to stretch contraction.The inclination data of the platform is read by the gyroscope and transmitted to the main station through EtherCAT.And the platform is accurately controlled by using the active disturbance rejection control （ADRC） algorithm.Experimental results show that the designed platform has the advantages of low latency and high anti-interference ability.

Key words EtherCAT communication;Stewart platform;active disturbance rejection control （ADRC）

收稿日期 2019-11-21

资助项目 国家自然科学基金（51675259）;江苏省研究生创新基金（SJCX18_0581）

作者简介贺顺，男，硕士生，研究领域为机器人控制技术.1162883527@qq.com

温秀兰（通信作者），女，博士，教授，主要研究方向为精密计量理论与方法、机器人控制标定技术、智能计算及其应用等.zdhxwxl@njit.edu.cn