EtherCAT姿態(tài)監(jiān)測從站的設(shè)計與實現(xiàn)*

陳樂南，高偉強，黃國靜，劉建群

(1.廣東工業(yè)大學 機電工程學院，廣州 510006；2.佛山市智昂科技有限公司，廣東 佛山 528225)

0 引言

姿態(tài)監(jiān)測技術(shù)在航空航天、汽車制造、醫(yī)療器械、工業(yè)機器人、智能機器人、交通設(shè)備等領(lǐng)域有著廣泛的應(yīng)用。MEMS(Micro-Electro-Mechanical-System)是采用微機械加工技術(shù)制造的新型傳感器[1]，可用于檢測角速度、加速度等物理量。處理器根據(jù)加速度和角加速度等姿態(tài)信息，融合相關(guān)姿態(tài)解算算法，即可求得運動物體的姿態(tài)角：橫滾角Roll、俯仰角Pitch、偏航角Yaw。

傳統(tǒng)的控制系統(tǒng)中，主控制器通過CAN、RS232、RS485等串行通訊總線與外設(shè)連接，獲得外設(shè)信息。這些傳統(tǒng)總線的傳輸速率、傳輸距離、數(shù)據(jù)量、實時性、兼容性、拓撲能力、穩(wěn)定性等方面均不如工業(yè)以太網(wǎng)。隨著通訊技術(shù)的發(fā)展，越來越多的控制器采用實時現(xiàn)場總線協(xié)議與設(shè)備層通訊。

EtherCAT是一種高速的開放式實時以太網(wǎng)總線，在連接伺服驅(qū)動器、傳感器等設(shè)備的控制系統(tǒng)中有著廣泛應(yīng)用。目前現(xiàn)有的EtherCAT從站模塊大多具備IO、AD、DA轉(zhuǎn)換功能，還未實現(xiàn)姿態(tài)監(jiān)測功能。將MEMS傳感器與EtherCAT總線相結(jié)合，是實現(xiàn)機器人等智能設(shè)備姿態(tài)實時監(jiān)測的可行方案。本文采用LAN9252、STM32、MEMS芯片，開發(fā)了一種低成本的EtherCAT姿態(tài)監(jiān)測從站，實現(xiàn)對物體姿態(tài)的實時采樣和監(jiān)測。

1 基于EtherCAT的姿態(tài)監(jiān)測系統(tǒng)

EtherCAT是德國BECKHOFF公司提出的開放式實時以太網(wǎng)，它具有高速、實時性、兼容性強等特點，支持幾乎所有拓撲結(jié)構(gòu)。EtherCAT系統(tǒng)采用主從通信結(jié)構(gòu)，主站發(fā)出下行電報數(shù)據(jù)幀遍歷所有的從站，每個從站在數(shù)據(jù)幀經(jīng)過時分析尋址到本站數(shù)據(jù)，并根據(jù)數(shù)據(jù)幀中的命令來提取數(shù)據(jù)或插入返回數(shù)據(jù)[2-3]。

如圖1所示，設(shè)備層的多種監(jiān)測和驅(qū)動設(shè)備通過EtherCAT總線連接在一起。以采樣示教噴涂機器人為例，機器人在對工件進行噴涂的過程中，需要沿著預(yù)設(shè)的軌跡移動，并且不斷改變噴槍的姿態(tài)，使噴漆能夠均勻覆蓋工件表面。將本文設(shè)計的EtherCAT姿態(tài)監(jiān)測從站固定于噴涂機器人執(zhí)行器末端，用于實時采樣監(jiān)測噴槍的姿態(tài)，同時將姿態(tài)信息作為輸入量經(jīng)EtherCAT總線反饋給EtherCAT主站。整個系統(tǒng)將構(gòu)成閉環(huán)控制系統(tǒng)，當噴槍的姿態(tài)與預(yù)設(shè)的姿態(tài)有偏差時，主站將控制伺服電機轉(zhuǎn)動，從而修正噴槍姿態(tài)。

圖1 EtherCAT系統(tǒng)

2 EtherCAT從站硬件配置

EtherCAT從站設(shè)備同時實現(xiàn)通信和應(yīng)用處理兩部分功能。主要由LAN9252從站控制芯片、STM32從站微處理和MEMS傳感器模塊等構(gòu)成。硬件設(shè)計部分采用模塊化的設(shè)計思想，從站整體結(jié)構(gòu)如圖2所示。

圖2 從站硬件結(jié)構(gòu)

2.1 LAN9252電路設(shè)計

LAN9252從站控制芯片是實現(xiàn)EtherCAT數(shù)據(jù)鏈路層協(xié)議的專用集成電路芯片，用于處理EtherCAT數(shù)據(jù)幀，可以支持絕大部分時序要求嚴格的功能。其具有4K字節(jié)的雙端口存儲器(DPRAM)、4個同步管理器(SyncManager)、3個現(xiàn)場總線存儲器管理單元(FMMU)、1個64位分布式時鐘和雙集成以太網(wǎng)PHY。主站與從站之間可以通過RJ45接口網(wǎng)線連接。

EEPROM用于存儲LAN9252的相關(guān)設(shè)備信息，LAN9252在上電或復(fù)位后將通過IIC總線自動讀取EEPROM中的內(nèi)容并裝入LAN9252的相應(yīng)寄存器。

2.2 從站微處理器及外設(shè)電路設(shè)計

STM32從站微處理器用于處理應(yīng)用層事件，STM32與LAN9252之間采用FSMC接口進行數(shù)據(jù)交互[4]。

本文選用的MEMS傳感器型號為MPU9250，其內(nèi)部集成有3軸加速度計、3軸陀螺儀和3軸磁力計，擁有16位分辨率，自帶可編程數(shù)字濾波器。MPU9250自帶的數(shù)字運動處理器(DMP)，可以整合9軸傳感器數(shù)據(jù)，通過IIC總線向應(yīng)用端輸出完整的9軸融合演算數(shù)據(jù)，其姿態(tài)解算頻率可達200Hz，具有較高的精度和實時性。

STM32與MEMS之間采用IIC通訊，MEMS傳感器將采集到的加速度、角速度、磁場數(shù)據(jù)通過IIC總線傳遞給STM32，STM32經(jīng)過姿態(tài)解算得到物體的姿態(tài)角。STM32可以通過CAN、RS485、IIC、SPI總線與外設(shè)連接，從而實現(xiàn)對多種外設(shè)的控制。板子采用3.3V供電，最終設(shè)計的實物圖如圖3所示。

圖3 從站PBC電路板

3 XML設(shè)備描述文件

XML文件是設(shè)備描述文件，用于描述EtherCAT協(xié)議報文[5]，其燒錄在板子的EEPROM中，LAN9252上電后，會從EEPROM加載配置參數(shù)，完成初始化過程[6]。CANopen是基于CAN總線的應(yīng)用層協(xié)議，EtherCAT在應(yīng)用層支持CANopen協(xié)議[7]。本文采用周期性過程數(shù)據(jù)通訊，使用對象字典映射過程數(shù)據(jù)。姿態(tài)監(jiān)測過程中，從站傳遞給主站的EtherCAT數(shù)據(jù)幀中包括橫滾角Roll、俯仰角Pitch和偏航角Yaw數(shù)據(jù)，需要對XML文件進行修改。

首先對過程數(shù)據(jù)對象(PDO)進行數(shù)據(jù)分配，RxPdo為主站發(fā)送給從站的數(shù)據(jù)，所用的SM(SyncManager)通道為SM2，TxPdo為從站發(fā)送給主站的數(shù)據(jù)，所用的SM通道為SM3，姿態(tài)角數(shù)據(jù)則包含在TxPdo映射中。

本文主要修改了0x1A02、0x6020兩個索引的對象字典。首先在DT6020數(shù)據(jù)類型中，添加了pitch、roll、yaw這3個INT型數(shù)據(jù)，子索引分別對應(yīng)十進制的18、19、20。TxPdo的組成由映射關(guān)系0x1A02的值決定，0x1A02里映射的內(nèi)容為0x6020和0x1802的字典的內(nèi)容，如圖4所示。索引0x6020的子索引0x12、0x13、0x14則分別對應(yīng)pitch、roll、yaw數(shù)據(jù)。

圖4 IA02映射內(nèi)容

4 從站程序設(shè)計

從站STM32程序主要實現(xiàn)應(yīng)用層功能，包括狀態(tài)機處理、周期性過程數(shù)據(jù)的處理、非周期性郵箱數(shù)據(jù)的處理、姿態(tài)解算、外設(shè)驅(qū)動、讀取外設(shè)數(shù)據(jù)等[8]。

4.1 STM32程序結(jié)構(gòu)

STM32程序中首先完成硬件的初始化，EtherCAT功能函數(shù)的初始化，然后周期性判斷當前從站的運行模式[9]。EtherCAT從站程序結(jié)構(gòu)如圖5所示。

圖5 EtherCAT從站程序結(jié)構(gòu)

(1)硬件初始化部分用于初始化GPIO口、FSMC驅(qū)動、MPU9250九軸傳感器、IIC驅(qū)動、LED驅(qū)動、LAN9252設(shè)置等。

(2)EtherCAT初始化部分用于初始化EtherCAT功能模塊。包括初始化EtherCAT狀態(tài)機、郵箱、緊急報文、COE字典等。

(3)本文采用FreeRun自由運行模式，ECAT_Application()函數(shù)實現(xiàn)對應(yīng)用數(shù)據(jù)的處理，包括對MEMS采集到的加速度、角速度、磁場數(shù)據(jù)的讀取，并通過姿態(tài)解算算法得到物體的Roll、Pitch和Yaw姿態(tài)角。

4.2 姿態(tài)解算程序

通過MPU9250采集到的是3軸角速度、3軸加速度和3軸磁感應(yīng)強度數(shù)據(jù)。STM32通過IIC通信接口讀取MPU9250的相關(guān)寄存器，即可得到9軸姿態(tài)數(shù)據(jù)。利用得到的9軸原始數(shù)據(jù)，進行姿態(tài)融合解算，就能求出運動物體的姿態(tài)角：Roll、Pitch和Yaw。

MPU9250自帶了數(shù)字運動處理器DMP，并且官方提供了一個嵌入式運動處理庫MPL。結(jié)合DMP，移植MPL軟件庫到STM32程序中，可以將傳感器的9軸原始數(shù)據(jù)，直接轉(zhuǎn)換成四元數(shù)輸出[10]。利用四元數(shù)，即可求出姿態(tài)角。相較于卡爾曼濾波、互補濾波等姿態(tài)解算方法，本方案中STM32不用參與姿態(tài)解算過程，從而有更多的時間去處理其他任務(wù)，提高了系統(tǒng)的實時性。

本文采用了MPU9250傳感器自身的坐標系，繞X軸旋轉(zhuǎn)的為橫滾角Roll，繞Y軸旋轉(zhuǎn)的為俯仰角Pitch，繞Z軸旋轉(zhuǎn)的為偏航角Yaw。姿態(tài)角的正負滿足右手螺旋法則。MPU9250自身坐標系如圖6所示。

圖6 MPU9250自身坐標系

5 從站測試

本文采用裝有TwinCAT 3.0軟件的PC機對EtherCAT從站進行測試，并傳遞實驗數(shù)據(jù)。同時，使用Qt開發(fā)的“三維姿態(tài)監(jiān)測軟件”與TwinCAT進行ADS(自動化設(shè)備規(guī)范)通訊[11]，從而實現(xiàn)數(shù)據(jù)交互。

5.1 建立通訊

在TwinCAT中新建工程，掃描得到從站設(shè)備。在PLC程序中添加3個變量，分別對應(yīng)從站的橫滾角Roll、俯仰角Pitch和偏航角Yaw，然后將PLC程序中定義的變量與上傳的姿態(tài)角數(shù)據(jù)進行鏈接。“三維姿態(tài)監(jiān)測軟件”通過ADS通訊協(xié)議獲取到變量值，從而將Roll、Pitch和Yaw姿態(tài)角變量曲線在圖表欄里顯示出來。圖7為Roll、Pitch和Yaw姿態(tài)角的實時曲線圖，右下角為實時姿態(tài)角度數(shù)。

圖7 姿態(tài)角實時曲線

5.2 姿態(tài)監(jiān)測測試

實驗數(shù)據(jù)傳輸過程中將監(jiān)測到的姿態(tài)數(shù)據(jù)放大了100倍，TwinCAT的通信周期設(shè)為5ms，從站的DMP輸出速率設(shè)置為100Hz。通過TwinCAT或者“三維姿態(tài)監(jiān)測軟件”可以觀察變量的實時值。

姿態(tài)角的監(jiān)測精度，主要取決于傳感器自身的精度和算法性能。將從站放置于水平面上，作為初始位置。由于電路板的焊接、安裝等原因，MPU9250芯片并沒有處于完全水平狀態(tài)，并且會受到一些干擾，此時Roll、Pitch、Yaw的值分別為81、96、-259，稱之為初始值。測量值均放大了100倍，后續(xù)測量的角度值減去初始值才是實際角度值。利用橫滾角Roll、俯仰角Pitch、偏航角Yaw分別測量30°、45°、60°角，采樣數(shù)據(jù)點為300個，即3s內(nèi)的測量值，監(jiān)測結(jié)果如表1所示，可知測量的誤差控制在±1°內(nèi)。

表1 姿態(tài)角精度測試

6 結(jié)論

本文基于EtherCAT從站提出了一種姿態(tài)監(jiān)測方案，詳述了該方案的軟硬件實現(xiàn)過程。利用EtherCAT總線的高速高實時以及支持拓撲連接等特性，實現(xiàn)對多物體的姿態(tài)監(jiān)測。最后，通過TwinCAT軟件與EtherCAT從站構(gòu)成了一個完整的測試系統(tǒng)。經(jīng)實驗驗證，采用低成本的MEMS姿態(tài)傳感器，本文研究的EtherCAT姿態(tài)監(jiān)測從站的監(jiān)測誤差控制在±1°內(nèi)，滿足噴涂機器人、平衡車運行、無人機飛行等一般工業(yè)、生活場景的姿態(tài)角度實時采樣、監(jiān)測的需求。