基于EtherCAT的六自由度自平衡平臺設計

賀順 溫秀蘭

摘要

為解決傳統(tǒng)自平衡平臺響應速度慢、控制精度低的問題，本文提出了基于EtherCAT總線的六自由度自平衡平臺設計方法，由6臺支持EtherCAT通信的伺服驅動器帶動6條電動缸做伸縮運動，平臺的傾角數據由陀螺儀讀取并通過EtherCAT傳輸到主站，并利用自抗擾控制算法對平臺進行精確控制.實驗結果表明，所設計平臺具有低延時和較高的抗干擾能力等優(yōu)點.關鍵詞

EtherCAT通信;Stewart平臺;自抗擾控制

中圖分類號 TP273;TP242

文獻標志碼 A

0 引言

自平衡平臺在海運、災區(qū)救援等領域廣泛應用，其響應速度快慢及控制精度高低將直接影響財產和生命安全，因此多年來已有眾多學者開展了相關研究.其中六自由度自平衡平臺因具有自由度多、剛度高、精度高、承載能力強以及可模塊化生產等優(yōu)點被廣泛使用.在六個自由度運動中，對船舶的行駛過程干擾最嚴重的是垂蕩、橫搖以及縱搖三種情況，其中又以橫縱兩個方向的影響最為嚴重[1].

文獻[2]從平臺的構思設計到理論分析、從機械建模到動力學分析、從控制系統(tǒng)回路仿真到最終控制算法的軟件編程實現與驗證，形成了一套完整的開發(fā)設計方案，但在控制系統(tǒng)仿真與后期設計實現過程中，未能進行位姿大閉環(huán)，整個系統(tǒng)仍是以開環(huán)控制為主，因而最終姿態(tài)誤差較大.文獻[3]分析了PID控制對系統(tǒng)的跟蹤特性、響應性能等動態(tài)性能參數的影響.文獻[4]為各單個通道分別設計了模糊PID控制器，其優(yōu)點就是能夠根據誤差和誤差的變化率實時地調節(jié)比例系數kp、積分系數ki和微分系數kd三個參數，使穩(wěn)定平臺的穩(wěn)定精度得到進一步提高.現有的自平衡平臺大多是基于脈沖控制或CAN總線實現通信，控制算法采用PID實現，存在響應速度慢、控制精度低等問題，難以滿足目前對海運質量提出高要求的需要.Ether CAT以其數據傳送能力強、技術成熟度高、開發(fā)成本低等優(yōu)點，受到工業(yè)控制現場總線領域的廣泛關注，已成為全球公認的標準，應用日趨廣泛[5-6].

本文在搭建Stewart結構的六自由度自平衡平臺基礎上，采用EtherCAT總線將上平臺安裝的陀螺儀數據實時讀入TwinCAT主站，采用自抗擾控制策略平臺進行精確控制，并對實驗平臺進行實驗驗證，實驗結果證明了Stewart平臺控制的有效性.

1 系統(tǒng)總體方案設計

考慮到EtherCAT具備靈活的網絡拓撲結構，系統(tǒng)配置簡單，實施的成本低廉等特點，搭建的自平衡平臺總體結構如圖1所示.以倍福主站作為核心單元，運行TwinCAT3.1軟件，作為實時工業(yè)以太網，EtherCAT 充分利用以太網全雙工特性，采用主從站通信方式，主站通過EtherCAT網絡連接6臺伺服驅動器從站和1個陀螺儀傳感器從站.驅動器通過帶動電缸實現直線伸縮運動，進而改變平臺的位姿，陀螺儀將采集到的姿態(tài)數據實時反饋給主站.

2 EtherCAT通信設計

2.1 陀螺儀從站設計

姿態(tài)閉環(huán)控制需要得到實時的反饋數據，所以需要設計一套支持EtherCAT協議的陀螺儀從站.陀螺儀從站由帶串行接口的9軸傳感器模塊和LAN9252從站模塊組成.陀螺儀采用廣州阿路比電子科技有限公司生產的LPMS-UTTL2 9軸陀螺儀，該陀螺儀通過采集9軸傳感器數據并對其進行融合濾波，TTL串口輸出所需的歐拉角，串口波特率為921.6 kbit/s，數據采樣率400 Hz.陀螺儀將處理好的數據輸出給STM32.

STM32和陀螺儀模塊之間采用串口DMA通信，通信協議采用LPBUS協議.LPBUS協議是基于工業(yè)標準的Modbus協議所設計的通信協議.這是LPMS設備默認的通信方式.LPMS設備具有數據流模式（Streaming Mode）和命令模式（Command Mode）兩種.LPMS姿態(tài)傳感器上電后，等初始化設備內部初始化完成后，就會直接進入數據流模式且默認頻率為100 Hz.陀螺儀參數設置流程如圖2所示.

由于陀螺儀只傳輸串口數據，所以本文利用LAN9252實現EtherCAT三層協議中的物理層和部分數據鏈路層，利用STM32實現EtherCAT的應用層協議和陀螺儀數據的讀寫，兩者之間基于STM32的FSMC外設與LAN9252的HBI接口進行通信，采用16位變址尋址方式，實現串口轉EtherCAT從站控制器電路.

LAN9252集成高性能100 Mbit/s以太網收發(fā)器，作為EtherCAT從控制器，它支持3個FMMU，支持4個SyncManager，支持64位分布式時鐘，有4KBDPRAM.它有8/16位主機總線接口，支持變址寄存器或復用總線，支持SPI/QSPI，其數字I/O模式可以優(yōu)化系統(tǒng)成本，第3個端口可實現靈活的網絡配置[7].STM32F407VET6是意法半導體公司基于ARMCortexM4內核的32位微控制器，有著1 MB片內Flash和192 kB片內RAM，并包含UART、ADC和FSMC等多種外設.

EtherCAT從站板應用層采用CANopen over EtherCAT協議，不需要支持DC同步，可以將信號統(tǒng)一交給TwinCAT主站，由主站對EtherCAT報文進行分析與后續(xù)操作[8].EtherCAT從站的硬件連接如圖3所示，其實物如圖4所示.

2.2 TwinCAT主站程序設計

TwinCAT主站作為整個系統(tǒng)的控制核心，負責接收反饋并發(fā)送控制信號.首先，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 實驗測試結果

控制系統(tǒng)由模擬海浪的三自由度平臺和控制自平衡的Stewart平臺構成，伺服驅動器和TwinCAT工控機置于三自由度平臺內部，陀螺儀固定在上平臺，陀螺儀的X軸與上平臺的X軸對齊，平臺整體實物如圖8所示，電氣連接實物如圖9所示.

當三自由度平臺搖擺幅度設定為3.2°，周期為4 s時，繞α軸和β軸的搖擺曲線如圖10所示.不同kp，kd情況下α軸和β軸的控制曲線分別如圖11和圖12所示.由實驗結果可見，當kp=4 000、kd=28時，控制效果最好.ADRC與傳統(tǒng)PID波形對比分別如圖13和圖14所示.由實驗結果可見，ADRC控制的波形對于位置跟蹤性能較傳統(tǒng)PID性能好，尤其是α角控制曲線效果更好，對負載擾動有更好的抑制作用.

6 結語

本文將Stewart結構的并聯機器人應用于自平衡平臺，分析了Stewart結構的運動學逆解，設計了基于LAN9252的陀螺儀從站，并基于TwinCAT主站編寫了自抗擾控制算法，對航運影響最大的橫搖和縱搖方向進行自平衡控制，最后對控制性能進行了分析.實驗結果表明搭建的基于EtherCAT的Stewart自平衡平臺方法可行，自抗擾控制的精度較高，易于在海運、災區(qū)救援等領域推廣應用.

參考文獻

References

[1]

劉學功，宋振海，張宏凱.船舶推進中兩種動態(tài)平衡的研究[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結構的六自由度并聯穩(wěn)定平臺技術研究[D].濟南：山東大學，2018

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

[3] 隋毅.基于并聯機構的海上穩(wěn)定平臺的機液聯合仿真研究[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] 劉義德.基于并聯機構的穩(wěn)定平臺建模與控制[D].哈爾濱：哈爾濱工業(yè)大學，2009

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

[5] 張穎，平雪良，王晨學，等.ROS下基于EtherCAT的串聯機器人控制系統(tǒng)[J].傳感器與微系統(tǒng)，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的機器人虛擬控制系統(tǒng)設計[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控制在網絡控制系統(tǒng)中的應用[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）;江蘇省研究生創(chuàng)新基金（SJCX18_0581）

作者簡介賀順，男，碩士生，研究領域為機器人控制技術.1162883527@qq.com

溫秀蘭（通信作者），女，博士，教授，主要研究方向為精密計量理論與方法、機器人控制標定技術、智能計算及其應用等.zdhxwxl@njit.edu.cn