基于EtherCAT的六自由度自平衡平臺(tái)設(shè)計(jì)

賀順 溫秀蘭

摘要

為解決傳統(tǒng)自平衡平臺(tái)響應(yīng)速度慢、控制精度低的問(wèn)題，本文提出了基于EtherCAT總線的六自由度自平衡平臺(tái)設(shè)計(jì)方法，由6臺(tái)支持EtherCAT通信的伺服驅(qū)動(dòng)器帶動(dòng)6條電動(dòng)缸做伸縮運(yùn)動(dòng)，平臺(tái)的傾角數(shù)據(jù)由陀螺儀讀取并通過(guò)EtherCAT傳輸?shù)街髡荆⒗米钥箶_控制算法對(duì)平臺(tái)進(jìn)行精確控制.實(shí)驗(yàn)結(jié)果表明，所設(shè)計(jì)平臺(tái)具有低延時(shí)和較高的抗干擾能力等優(yōu)點(diǎn).關(guān)鍵詞

EtherCAT通信;Stewart平臺(tái);自抗擾控制

中圖分類號(hào) TP273;TP242

文獻(xiàn)標(biāo)志碼 A

0 引言

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

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

本文在搭建Stewart結(jié)構(gòu)的六自由度自平衡平臺(tái)基礎(chǔ)上，采用EtherCAT總線將上平臺(tái)安裝的陀螺儀數(shù)據(jù)實(shí)時(shí)讀入TwinCAT主站，采用自抗擾控制策略平臺(tái)進(jìn)行精確控制，并對(duì)實(shí)驗(yàn)平臺(tái)進(jìn)行實(shí)驗(yàn)驗(yàn)證，實(shí)驗(yàn)結(jié)果證明了Stewart平臺(tái)控制的有效性.

1 系統(tǒng)總體方案設(shè)計(jì)

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

2 EtherCAT通信設(shè)計(jì)

2.1 陀螺儀從站設(shè)計(jì)

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

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

由于陀螺儀只傳輸串口數(shù)據(jù)，所以本文利用LAN9252實(shí)現(xiàn)EtherCAT三層協(xié)議中的物理層和部分?jǐn)?shù)據(jù)鏈路層，利用STM32實(shí)現(xiàn)EtherCAT的應(yīng)用層協(xié)議和陀螺儀數(shù)據(jù)的讀寫(xiě)，兩者之間基于STM32的FSMC外設(shè)與LAN9252的HBI接口進(jìn)行通信，采用16位變址尋址方式，實(shí)現(xiàn)串口轉(zhuǎn)EtherCAT從站控制器電路.

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

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

2.2 TwinCAT主站程序設(shè)計(jì)

TwinCAT主站作為整個(gè)系統(tǒng)的控制核心，負(fù)責(zé)接收反饋并發(fā)送控制信號(hào).首先，TwinCAT掃描臺(tái)達(dá)伺服從站，新建NC軸，修改電機(jī)軸的速度、加速度和編碼器參數(shù)，修改DC同步周期為2 ms.利用NC軸可以對(duì)生成輪廓時(shí)具有加速度限制以及對(duì)速度和加速度的預(yù)控制，將跟隨誤差降到最小[9].激活主站后即可點(diǎn)動(dòng)測(cè)試伺服，伺服測(cè)試完畢后新建PLC工程，將Stewart平臺(tái)運(yùn)動(dòng)學(xué)算法使用ST結(jié)構(gòu)化語(yǔ)言寫(xiě)入程序塊，使用外部位置給定函數(shù)即可對(duì)平臺(tái)位姿進(jìn)行精確控制.TwinCAT掃描的I/O如圖5所示.

3 運(yùn)動(dòng)學(xué)分析

對(duì)Stewart平臺(tái)做自平衡控制，其運(yùn)動(dòng)學(xué)逆解也是其關(guān)鍵一步，圖6是其結(jié)構(gòu)簡(jiǎn)圖.

如圖6所示，在下平臺(tái)建立靜止坐標(biāo)系O-XYZ，其中X軸平行于鉸點(diǎn)A4、A5 所在直線.在上平臺(tái)建立動(dòng)坐標(biāo)系o-xyz，上平臺(tái)的運(yùn)動(dòng)可分解為X，Y，Z方向的平移，以及繞其坐標(biāo)軸X，Y，Z的旋轉(zhuǎn).下平臺(tái)各鉸點(diǎn)Ai的坐標(biāo)為（XAi，YAi，ZAi），上平臺(tái)各鉸點(diǎn)ai的動(dòng)坐標(biāo)為（xai，yai，zai），靜坐標(biāo)為（Xai，Yai，Zai），與其對(duì)應(yīng)相連的電動(dòng)缸長(zhǎng)度為li（i=l，…，6）.

動(dòng)坐標(biāo)系相對(duì)于靜坐標(biāo)系的位置用矢量P描述：

P=xyzT.（1）

設(shè)下平臺(tái)定坐標(biāo)系為{A}，上平臺(tái)動(dòng)坐標(biāo)系為{a}，經(jīng)過(guò)推算后動(dòng)平臺(tái)的各點(diǎn)坐標(biāo)相對(duì)于靜平臺(tái)的各點(diǎn)坐標(biāo)的方向轉(zhuǎn)換矩陣表示如下

5 實(shí)驗(yàn)測(cè)試結(jié)果

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

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

6 結(jié)語(yǔ)

本文將Stewart結(jié)構(gòu)的并聯(lián)機(jī)器人應(yīng)用于自平衡平臺(tái)，分析了Stewart結(jié)構(gòu)的運(yùn)動(dòng)學(xué)逆解，設(shè)計(jì)了基于LAN9252的陀螺儀從站，并基于TwinCAT主站編寫(xiě)了自抗擾控制算法，對(duì)航運(yùn)影響最大的橫搖和縱搖方向進(jìn)行自平衡控制，最后對(duì)控制性能進(jìn)行了分析.實(shí)驗(yàn)結(jié)果表明搭建的基于EtherCAT的Stewart自平衡平臺(tái)方法可行，自抗擾控制的精度較高，易于在海運(yùn)、災(zāi)區(qū)救援等領(lǐng)域推廣應(yīng)用.

參考文獻(xiàn)

References

[1]

劉學(xué)功，宋振海，張宏凱.船舶推進(jìn)中兩種動(dòng)態(tài)平衡的研究[J].艦船科學(xué)技術(shù)，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結(jié)構(gòu)的六自由度并聯(lián)穩(wěn)定平臺(tái)技術(shù)研究[D].濟(jì)南：山東大學(xué)，2018

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

[3] 隋毅.基于并聯(lián)機(jī)構(gòu)的海上穩(wěn)定平臺(tái)的機(jī)液聯(lián)合仿真研究[D].青島：中國(guó)海洋大學(xué)，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] 劉義德.基于并聯(lián)機(jī)構(gòu)的穩(wěn)定平臺(tái)建模與控制[D].哈爾濱：哈爾濱工業(yè)大學(xué)，2009

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

[5] 張穎，平雪良，王晨學(xué)，等.ROS下基于EtherCAT的串聯(lián)機(jī)器人控制系統(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] 董海濤，朱國(guó)慶，黃麗宇.基于LAN9252的伺服驅(qū)動(dòng)EtherCAT通信接口實(shí)現(xiàn)[J].組合機(jī)床與自動(dòng)化加工技術(shù)，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ī)器人虛擬控制系統(tǒng)設(shè)計(jì)[J].制造技術(shù)與機(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] 王莉，郭偉.分?jǐn)?shù)階PID控制在網(wǎng)絡(luò)控制系統(tǒng)中的應(yīng)用[J].南京信息工程大學(xué)學(xué)報(bào)（自然科學(xué)版），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

資助項(xiàng)目 國(guó)家自然科學(xué)基金（51675259）;江蘇省研究生創(chuàng)新基金（SJCX18_0581）

作者簡(jiǎn)介賀順，男，碩士生，研究領(lǐng)域?yàn)闄C(jī)器人控制技術(shù).1162883527@qq.com

溫秀蘭（通信作者），女，博士，教授，主要研究方向?yàn)榫苡?jì)量理論與方法、機(jī)器人控制標(biāo)定技術(shù)、智能計(jì)算及其應(yīng)用等.zdhxwxl@njit.edu.cn