基于EtherCAT的連續體機器人主從站設計

袁俊杰，魏任寒，何廣平，狄杰建，趙磊,張忠海，周林

(1.北方工業大學機械與材料工程學院，北京 100043；2.北京航天測控技術有限公司，北京 100041)

0 前言

隨著工業技術向自動化方向不斷地發展，工控任務變得日益復雜，工控任務中高質量、高精度等需求對控制系統提出了通信速度快、控制精度高、抗干擾能力強、實時性高等要求[1]。這使得傳統總線式的控制方式已經無法滿足其要求，而工業以太網技術以高實時性、高穩定性等特點促進了工業技術的進一步發展，其中在工控領域應用較為廣泛的工業以太網有EtherCAT、EtherNet/IP、Modbus、PowerLink、Profibus等[2]。

由德國Beckhoff公司提出的實時以太網EtherCAT(EtherNet Control Automation Technology)技術可以滿足日益復雜的工控任務的要求，它基于標準的以太網，使用特殊以太網數據幀類型(0x88a4)，采用一主多從的通信方式[3]，通信速率高達100 Mb/s，具有非常靈活的拓撲結構，因此目前EtherCAT在機器人領域應用得越來越廣泛。

近年來一類超冗余自由度的連續體機器人獲得較多研究[4]，該類型機器人通常由多個軟體關節組成，每個關節一般具備3個自由度，因此這類機器人需要同時對十幾個甚至幾十個電機進行同步控制。利用傳統的多軸運動控制卡或控制器成本較高，且對上位機要求較高，系統組成和布線均較為復雜。因此提出一種基于EtherCAT通信的連續體機器人主從站控制系統，該控制系統采用一般的PC機作為主站，由自主設計的控制板作為從站，主從站之間通過EtherCAT通信，具有高實時性、高靈活性、低延時性等特點。

1 總體方案設計

主從站之間采用實時以太網EtherCAT協議作為底層通信協議，基于現有的一種連續體機器人實驗平臺。連續體機器人共有4節柔性機械臂，每節柔性機械臂由3根呈120°均布的鋼絲繩進行拉動，而每根鋼絲繩由一個步進電機帶動來實現機械臂空間位姿的變化。圖1所示為主從站的總體結構，從站采用自主設計的STM32F407芯片+AX58100芯片的控制板，預留了6路步進電機接口和編碼器的接口以及AD采集接口等，能夠實現6路步進電機的驅動控制以及各種傳感器信號的采集。EtherCAT有很多種拓撲結構，包括總線形、星形和鏈形等等，這里采用了比較簡單、應用較為普遍、通信非常穩定帶有分支線的鏈形拓撲結構。

圖1 控制系統總體結構

EtherCAT主站硬件部分可使用通常的IPC工控機，使用工控機運行EtherCAT主站時，通常用一般的網卡作為硬件接口[5]。主站軟件部分使用開源的主站協議棧SOEM(Simple Open EtherCAT Master)，將開源的主站協議棧SOEM移植到開源的Linux操作系統上。由于Linux的非實時性，需要對其進行改造，在Linux操作系統上移植實時內核Xenomai，實現基于Linux的開源實時主站。

2 硬件設計

2.1 從站控制板硬件總體設計

如圖2所示，EtherCAT從站控制板主要由從站控制器芯片、微處理器芯片以及外圍電路組成。

從站控制器(ESC)芯片選用的是AX58100。AX58100是一款EtherCAT從站控制器芯片，集成了兩個支持100 Mb/s全雙工操作功能的高速以太網PHY，通過RJ45硬件接口與主站相連進行數據的傳輸和與其他的從站連接從而實現不同的拓撲結構，還支持CANopen(CoE)、TFTP(FoE)等標準EtherCAT協議，適用于工業自動化、運動控制等[6]。EtherCAT數據鏈路層的協議是固定的，這個協議已經被寫入從站控制器芯片AX58100中，主要功能是實現應用層通信協議，主要負責主從站之間的協議處理，能實現EtherCAT數據鏈路層的所有功能，是EtherCAT通信中關鍵的組成部分。

微處理器芯片選用的是基于Cortex-M4內核的STM32F407，主要負責通信的應用層部分和步進電機的驅動控制部分，并將采集的編碼器和傳感器數據通過從站控制器通過AX58100發送給主站。從站微處理器和從站控制器一起決定通信系統的性能和功能。

2.2 AX58100與STM32間的PDI接口

PDI接口是過程數據接口的簡稱。AX58100具有兩種過程數據接口(PDI)，FSMC接口和SPI 接口，AX58100可通過這些接口連接到外部的傳統MCU以支持EtherCAT功能。SPI接口方式具有占用引腳少、傳輸速率高的特點，因此AX58100與微處理器STM32F407芯片接口采用SPI(串行數據總線)方式。如圖3所示：SPI總線工作時有4條信號線，通常包括1條主機輸出從機輸入信號線(MOSI)、1條主機輸入從機輸出信號線(MISO)、1條時鐘信號線(SCLK)、1條從機使能信號線(CS)[7]。將微處理器芯片STM32F407作為SPI通信中主設備并提供串行時鐘，而控制器芯片AX58100作為SPI通信從設備。

圖3 SPI接口電路

2.3 主從通信電路設計

如圖4所示：AX58100芯片與RJ45接口通過5個主要的引腳相連，P0_TXOP引腳為PHY0差分傳輸正信號，P0_TXON引腳為差動器傳輸的負信號。在以太網模式下，差分信號以MDI模式通過TXOP/TXON信號傳輸到對應的媒體；當高電平時為光纖模式，信號對應連接到光纖收發器的TX+/TX針腳。

P0_RXIP引腳為PHY0差分接收正信號，P0_RXIN為PHY0差分器接收到的負信號。在以太網模式下，在MDI模式下在RXIP/RXIN信號對上接收來自介質的差分數據；在光纖模式下，信號對應連接到光纖收發器的RX+/RX-針腳。

P0_ACT引腳是芯片引導模式設置，用于引導模式設置以決定PHY0的媒體模式，默認為低電平時為以太網模式、高電平時為光纖模式。

3 軟件設計

3.1 實時主站搭建

如圖5所示，EtherCAT主站使用的是開源的主站協議棧SOEM，運行在增加了實時內核Xenomai的Linux操作系統里。它通過在標準Linux內核空間中添加一個新的實時內核Xenomai-Linux并且優先級高于普通的Linux內核，實時任務由實時內核執行，系統在不需要執行實時任務的情況下允許運行普通的Linux程序或非實時性任務從而提高實時性。

Linux可被視為實時調度程序的空閑任務，當此空閑任務運行時，它將執行自己的調度程序并調度正常的Linux進程。由于實時內核具有更高的優先級，因此當實時任務準備好運行并立即執行實時任務時，通常的Linux進程將被搶占，從而保證系統的實時性。

SOEM主站協議棧的架構如圖6所示，它采用了分層設計方法，在軟件上提供了一個抽象層，將SOEM主站協議棧與具體的操作系統和硬件平臺分開，理論上使得SOEM主站協議棧可以移植到任意的操作系統和硬件平臺上。抽象層主要由OSAL(操作系統抽象層)和OSHW(網絡抽象層)兩個模塊組成，移植的主要內容就是在Linux的操作系統和硬件上重寫OSAL和OSHW[8]。

圖6 SOEM層級架構

3.2 主站軟件設計

主站主程序流程如圖7所示，進入主程序之后，首先是從站狀態監測線程ecatcheck()，主要負責發送從站狀態請求并且監測從站的異常狀態，定時檢測當前從站的在線狀態，及時發現從站是否掉線，掉線之后重新將從站置為運行狀態。然后由ec_init()函數負責初始化SOEM，將套接字綁定到指定的計算機網卡；ec_config_init()函數負責初始化從站并返回發現的從站數量；ec_config_map()函數負責配置PDO映射，將從站的輸入輸出映射到IOMap，并配置FMMU寄存器。IOMap是一個內存池，SOEM會將PDO的指針根據從站EEPROM內部參數來統計需要映射的輸入輸出長度，然后配置SM寄存器。主站向從站發出狀態改變請求，檢測到從站狀態已經轉換為運行狀態后，進入主循環發送周期性過程數據。

圖7 主站主程序流程

3.3 從站軟件總體設計

從站主程序流程如圖8所示。其中，主要有HW_Init()和Main_Init()兩個從站初始化函數。HW_Init()主要是初始化相關信號的中斷以及GPIO引腳和SPI接口等，Main_Init()則負責初始化EtherCAT協議的相關變量和對象詞典等。兩個初始化函數完成后進入到主循環函數MainLoop里實現周期性和非周期性數據交換，周期性的數據在PDI_Isr()函數中處理，非周期性事件在ECAT_Main里處理。同時判斷從站是否處于運行狀態，若處于運行狀態則循環下去，否則退出循環。

圖8 從站主程序流程

根據查詢到的寄存器值的不同，可以將通信模式設置為自由運行、分布時鐘和同步模式3種[9]。

自由運行模式是每個從站根據自己的時間中斷來處理來自主站的EtherCAT數據，和主站的運行周期以及其他從站的周期都沒有關系，主站發送數據幀的時間與從站處理數據幀的時間不同步[10]。

同步模式指的是同步管理器(Sync Manager)的同步，同步模式的觸發方式是通過SM事件，每當EtherCAT數據幀在到達對應的從站時，會觸發一個SM事件，當從站接收到這個事件時，會進入對應的中斷服務函數來處理對應的數據。由于同步模式是根據數據幀到達特定從站的時間觸發SM事件信號進行同步，那么數據幀到達每一個從站的時間必然是不同的，當控制系統很龐大的時候，每個從站接收到數據幀的時間就會相差很大，越在后邊的從站接收到數據幀的時間越晚，它的同步效果就越差。

DC(Distributed Clock)Mode是一種高精度的同步時間模式，主站選取第一個從站的時鐘作為參考時鐘，主站及其他的從站都同步于此參考時鐘，DC模式下從站會在事件到來前完成數據幀的接收和運算，使得主從的同步性大大增強。

3.4 從站狀態機設計

EtherCAT狀態機模塊負責控制EtherCAT狀態機在以下幾種狀態間轉換，如圖9所示。一般的從站狀態分別是引導狀態(bootstrap)、初始化狀態(Init)、預運行狀態(Pre-Operational)、安全運行狀態(Safe-Operational)和運行狀態(Operational)，引導狀態一般不用設置，狀態轉換順序一般為初始化→預運行→安全運行→運行，狀態逐級轉換，不能跳躍[11]。

圖9 從站狀態轉換

圖10所示為狀態機的轉換流程，負責狀態機模塊的主要函數為ECAT_Main()，被主函數循環調用，從站狀態轉換一般由主站發起，主站發出狀態請求并將請求的狀態寫入到AL控制寄存器里面，從站接收到主站的狀態請求后，讀取AL控制寄存器里的值，根據不同的狀態轉換，檢查SM寄存器的配置[12]。對于PreOP狀態，檢查SM0和SM1的配置；對于SafeOP狀態，檢查應用層輸入和輸出數據映射；對于OP狀態，則檢查SM0-SM3。如果SM配置正確，則將新狀態寫入狀態機實際狀態位并調用轉換函數進行轉換，并將結果寫入AL狀態寄存器，等待主站讀取。若SM配置不正確或轉換不正確，則設置錯誤指示位并調用錯誤處理函數處理，將狀態未能轉換的原因寫入AL代碼寄存器里，主站讀取錯誤指示后清除錯誤指示位。

圖10 從站狀態請求與響應

3.5 周期性數據通信

從站周期性過程數據的處理流程如圖11所示：STM32F407與AX58100之間通過中斷方式通信，每當有SM事件時，AX58100向STM32F407觸發一個中斷信號，STM32F407響應這個中斷信號后，在Sync0_Isr()中斷服務函數中讀取AL事件寄存器的值，更新過程數據輸出映射，若是SM事件則更新過程數據，并從AX58100的SM2緩存區復制數據至STM32F407的緩沖區，然后執行主要的應用控制程序；STM32再將編碼器和傳感器的反饋數據寫入SM3緩存區(即更新過程數據輸入映射)，然后等待下一個周期被主站讀取。這樣即過程數據處理完成，否則退出中斷服務函數。

圖11 Sync0_Isr()中斷服務函數流程

3.6 PID算法模塊

由于計算機控制是一種采樣控制，它只能根據采樣值與期望值的偏差計算控制量，而不能像模擬控制那樣連續輸出控制量，進行連續控制。由于這一特點，公式里的積分項與微分項不能直接代入使用，必須進行離散化處理[13]。離散化處理的方法為：以T作為采樣周期，作為采樣序號，則離散采樣時間對應著連續時間，用矩形法數值積分近似代替積分，用一階后向差分近似代替微分。可作如下近似變換：

(1)

式(1)中，為了表示方便，將類似于e(kT)簡化成ek等。

將式(1)代入一般的PID表達式，得到離散的PID表達式為

(2)

或

(3)

其中：k為采樣序號，k=0，1，2，…；uk為第k次采樣時刻的計算機輸出值；ek為第k次采樣時刻輸入的偏差值；ek-1為第k-1次采樣時刻輸入的偏差值；Ki為積分系數，Ki=Kp·T/Ti；Kd為微分系數，Kd=Kp·Td/T；

如果采樣周期足夠小，則式(2)或式(3)的近似計算可以獲得相對來說比較精確的結果，使得離散控制過程與連續過程相對接近。

這種算法的缺點是：采樣的輸出量和以前的任何狀態都有關聯，計算時要對ek進行累加，工作量大；同時控制系統輸出對應實際位置，假如系統出現問題，uk則會大幅變化，進而會引起機構的劇烈變化[14]。

增量式PID控制算法則可以避免這種現象發生。增量式PID指的是控制器的輸出只是增量Δuk。增量式PID控制算法可以通過式(2)推導出。由式(2)可以得到控制器的第k-1個采樣時刻的輸出值為

(4)

將式(2)與式(4)相減并整理，就可以得到增量式PID控制算法公式為

(5)

由式(5)可以看出，如果控制系統采用的采樣周期T，確定了A、B、C之后，只需要使用3次測量的偏差值，即可由式(5)求出控制量。增量式PID算法與位置式PID控制算法式(2)相比，計算量小了很多，因此得到了廣泛的應用。

PID算法程序如下：

float IncPIDCalc(int NextPoint,float TargetVal)//期望值

{

float iError = 0,iIncpid = 0; //當前誤差

iError = TargetVal-NextPoint;//增量計算

if((iError<0.5f)&&(iError>-0.5f))

iError = 0; //|e|< 0.5,不做調整

iIncpid=(sPID.Proportion * iError)//E[k]項

-(sPID.Integral * sPID.LastError)//E[k-1]項

+(sPID.Derivative * sPID.PrevError);//E[k-2]項sPID.PrevError= sPID.LastError;//存儲誤差

sPID.LastError = iError;

return(iIncpid); //返回增量值

}

4 主從站測試與分析

4.1 主站任務調度測試

在Xenomai-Linux實時內核模式下，運行測試程序latency，設定周期為100 μs，測試時長1 h，測試控制系統主站的任務調度延時性。測試結果如表1所示：最大的調度延時為27.367 μs，最小的調度延時為0.015 μs，平均調度延時為1 μs左右，能夠滿足EtherCAT主站實時性要求。

表1 主站調度延時 單位：μs

4.2 主從站實時性測試

對搭建的整個主從站進行實時性的測試，在使用Xenomai實時內核的Linux操作系統上，用SOEM主站對從站不停地發送控制指令(如圖12所示)，通過wireshark軟件抓取EtherCAT報文，從抓取的報文中再篩選出由主站發出控制指令的報文，兩個報文的時間差就是主站的發送周期，時間差與平均發送周期的差即為系統的通信抖動。隨機連續選取了3 000組數據，得到如圖13所示的結果，主站的發送周期為1 055 μs左右，而系統的通信抖動為上下浮動10 μs左右。由統計數據結果可知系統具有較高的實時性并且系統的通信穩定性較為良好。

圖12 wireshark抓取EtherCAT報文

圖13 系統發送周期統計

5 實驗驗證

為驗證該主從站的控制效果和控制精度，現對其進行實驗驗證。實驗是在現有的連續體機器人平臺上進行的，該實驗平臺由四階柔性機械臂組成，機械臂的主體是彈性較好的彈簧，由3根呈120°均布的鋼絲繩進行拉動，而每根鋼絲繩由一個步進電機帶動來實現機械臂的彎曲，并實現機械臂在不同方向的運動。從站控制板使用基于Cortex-M4處理器為核心的STM32F407芯片和ASIX公司的AX58100芯片進行通信、數據處理、位置確定和算法實現等。連續體機器人實驗平臺如圖14所示。

圖14 連續體機器人 圖15 運動實驗

通過PID控制算法和對應的數據計算，讓機械臂按照預先設定的運動方式進行運動。通過同時控制多個電機進行協同運動(其中電機最高轉速為1.8 mm/s)，以中間支撐軸為原點，可以得到最終狀態下機械臂彎曲了約45°，鋼絲繩縮短約5 cm。具體運動實驗過程如圖15所示。

圖15僅展示第一節機械臂的運動，圖15(a)、圖15(b)分別對應0、6 s時運動狀態。由運動實驗曲線(圖16)可以得到：機械臂在整個運動過程中的運動較為平穩，在按照設定的軌跡運動時運動精度較高，基本實現了設想的功能，使得該實驗成功實現。

6 結論

對于連續體機器人在控制時實時性低、穩定性差的問題，提出一種基于EtherCAT的連續體機器人主從站控制系統，得出以下結論。

(1)在開源的Linux操作系統內核里增加了一個實時內核，再基于此操作系統移植開源主站協議棧SOEM，實現了具備高實時性、低延時的開源控制系統主站。

(2)設計了從站控制器AX58100+從站微處理器STM32F407的從站控制板，AX58100與STM32F407之間通過SPI接口通信，主站能夠通過網口對從站控制板發送控制指令，從站接收來自主站的控制指令和數據進行相應的數據分析和處理，對電機運動進行協調，從而實現對機械臂的控制，從站還能采集相應的編碼器與傳感器數據發送給主站。

(3)對主從站進行任務調度延時測試以及實時性測試，結果表明：主從站通信具有較低的延時以及高實時性，同時通信的抖動比較低，表明通信的穩定性較好。

(4)根據設計的PID算法對連續體機器人進行運動控制實驗驗證，實驗結果表明：該主從站對多個電機的協同控制效果較好，能實現預期的運動。