基于EtherCAT的連續(xù)體機(jī)器人主從站設(shè)計(jì)

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

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

0 前言

隨著工業(yè)技術(shù)向自動(dòng)化方向不斷地發(fā)展，工控任務(wù)變得日益復(fù)雜，工控任務(wù)中高質(zhì)量、高精度等需求對控制系統(tǒng)提出了通信速度快、控制精度高、抗干擾能力強(qiáng)、實(shí)時(shí)性高等要求[1]。這使得傳統(tǒng)總線式的控制方式已經(jīng)無法滿足其要求，而工業(yè)以太網(wǎng)技術(shù)以高實(shí)時(shí)性、高穩(wěn)定性等特點(diǎn)促進(jìn)了工業(yè)技術(shù)的進(jìn)一步發(fā)展，其中在工控領(lǐng)域應(yīng)用較為廣泛的工業(yè)以太網(wǎng)有EtherCAT、EtherNet/IP、Modbus、PowerLink、Profibus等[2]。

由德國Beckhoff公司提出的實(shí)時(shí)以太網(wǎng)EtherCAT(EtherNet Control Automation Technology)技術(shù)可以滿足日益復(fù)雜的工控任務(wù)的要求，它基于標(biāo)準(zhǔn)的以太網(wǎng)，使用特殊以太網(wǎng)數(shù)據(jù)幀類型(0x88a4)，采用一主多從的通信方式[3]，通信速率高達(dá)100 Mb/s，具有非常靈活的拓?fù)浣Y(jié)構(gòu)，因此目前EtherCAT在機(jī)器人領(lǐng)域應(yīng)用得越來越廣泛。

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

1 總體方案設(shè)計(jì)

主從站之間采用實(shí)時(shí)以太網(wǎng)EtherCAT協(xié)議作為底層通信協(xié)議，基于現(xiàn)有的一種連續(xù)體機(jī)器人實(shí)驗(yàn)平臺(tái)。連續(xù)體機(jī)器人共有4節(jié)柔性機(jī)械臂，每節(jié)柔性機(jī)械臂由3根呈120°均布的鋼絲繩進(jìn)行拉動(dòng)，而每根鋼絲繩由一個(gè)步進(jìn)電機(jī)帶動(dòng)來實(shí)現(xiàn)機(jī)械臂空間位姿的變化。圖1所示為主從站的總體結(jié)構(gòu)，從站采用自主設(shè)計(jì)的STM32F407芯片+AX58100芯片的控制板，預(yù)留了6路步進(jìn)電機(jī)接口和編碼器的接口以及AD采集接口等，能夠?qū)崿F(xiàn)6路步進(jìn)電機(jī)的驅(qū)動(dòng)控制以及各種傳感器信號的采集。EtherCAT有很多種拓?fù)浣Y(jié)構(gòu)，包括總線形、星形和鏈形等等，這里采用了比較簡單、應(yīng)用較為普遍、通信非常穩(wěn)定帶有分支線的鏈形拓?fù)浣Y(jié)構(gòu)。

圖1 控制系統(tǒng)總體結(jié)構(gòu)

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

2 硬件設(shè)計(jì)

2.1 從站控制板硬件總體設(shè)計(jì)

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

從站控制器(ESC)芯片選用的是AX58100。AX58100是一款EtherCAT從站控制器芯片，集成了兩個(gè)支持100 Mb/s全雙工操作功能的高速以太網(wǎng)PHY，通過RJ45硬件接口與主站相連進(jìn)行數(shù)據(jù)的傳輸和與其他的從站連接從而實(shí)現(xiàn)不同的拓?fù)浣Y(jié)構(gòu)，還支持CANopen(CoE)、TFTP(FoE)等標(biāo)準(zhǔn)EtherCAT協(xié)議，適用于工業(yè)自動(dòng)化、運(yùn)動(dòng)控制等[6]。EtherCAT數(shù)據(jù)鏈路層的協(xié)議是固定的，這個(gè)協(xié)議已經(jīng)被寫入從站控制器芯片AX58100中，主要功能是實(shí)現(xiàn)應(yīng)用層通信協(xié)議，主要負(fù)責(zé)主從站之間的協(xié)議處理，能實(shí)現(xiàn)EtherCAT數(shù)據(jù)鏈路層的所有功能，是EtherCAT通信中關(guān)鍵的組成部分。

微處理器芯片選用的是基于Cortex-M4內(nèi)核的STM32F407，主要負(fù)責(zé)通信的應(yīng)用層部分和步進(jìn)電機(jī)的驅(qū)動(dòng)控制部分，并將采集的編碼器和傳感器數(shù)據(jù)通過從站控制器通過AX58100發(fā)送給主站。從站微處理器和從站控制器一起決定通信系統(tǒng)的性能和功能。

2.2 AX58100與STM32間的PDI接口

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

圖3 SPI接口電路

2.3 主從通信電路設(shè)計(jì)

如圖4所示：AX58100芯片與RJ45接口通過5個(gè)主要的引腳相連，P0_TXOP引腳為PHY0差分傳輸正信號，P0_TXON引腳為差動(dòng)器傳輸?shù)呢?fù)信號。在以太網(wǎng)模式下，差分信號以MDI模式通過TXOP/TXON信號傳輸?shù)綄?yīng)的媒體；當(dāng)高電平時(shí)為光纖模式，信號對應(yīng)連接到光纖收發(fā)器的TX+/TX針腳。

P0_RXIP引腳為PHY0差分接收正信號，P0_RXIN為PHY0差分器接收到的負(fù)信號。在以太網(wǎng)模式下，在MDI模式下在RXIP/RXIN信號對上接收來自介質(zhì)的差分?jǐn)?shù)據(jù)；在光纖模式下，信號對應(yīng)連接到光纖收發(fā)器的RX+/RX-針腳。

P0_ACT引腳是芯片引導(dǎo)模式設(shè)置，用于引導(dǎo)模式設(shè)置以決定PHY0的媒體模式，默認(rèn)為低電平時(shí)為以太網(wǎng)模式、高電平時(shí)為光纖模式。

3 軟件設(shè)計(jì)

3.1 實(shí)時(shí)主站搭建

如圖5所示，EtherCAT主站使用的是開源的主站協(xié)議棧SOEM，運(yùn)行在增加了實(shí)時(shí)內(nèi)核Xenomai的Linux操作系統(tǒng)里。它通過在標(biāo)準(zhǔn)Linux內(nèi)核空間中添加一個(gè)新的實(shí)時(shí)內(nèi)核Xenomai-Linux并且優(yōu)先級高于普通的Linux內(nèi)核，實(shí)時(shí)任務(wù)由實(shí)時(shí)內(nèi)核執(zhí)行，系統(tǒng)在不需要執(zhí)行實(shí)時(shí)任務(wù)的情況下允許運(yùn)行普通的Linux程序或非實(shí)時(shí)性任務(wù)從而提高實(shí)時(shí)性。

Linux可被視為實(shí)時(shí)調(diào)度程序的空閑任務(wù)，當(dāng)此空閑任務(wù)運(yùn)行時(shí)，它將執(zhí)行自己的調(diào)度程序并調(diào)度正常的Linux進(jìn)程。由于實(shí)時(shí)內(nèi)核具有更高的優(yōu)先級，因此當(dāng)實(shí)時(shí)任務(wù)準(zhǔn)備好運(yùn)行并立即執(zhí)行實(shí)時(shí)任務(wù)時(shí)，通常的Linux進(jìn)程將被搶占，從而保證系統(tǒng)的實(shí)時(shí)性。

SOEM主站協(xié)議棧的架構(gòu)如圖6所示，它采用了分層設(shè)計(jì)方法，在軟件上提供了一個(gè)抽象層，將SOEM主站協(xié)議棧與具體的操作系統(tǒng)和硬件平臺(tái)分開，理論上使得SOEM主站協(xié)議?？梢砸浦驳饺我獾牟僮飨到y(tǒng)和硬件平臺(tái)上。抽象層主要由OSAL(操作系統(tǒng)抽象層)和OSHW(網(wǎng)絡(luò)抽象層)兩個(gè)模塊組成，移植的主要內(nèi)容就是在Linux的操作系統(tǒng)和硬件上重寫OSAL和OSHW[8]。

圖6 SOEM層級架構(gòu)

3.2 主站軟件設(shè)計(jì)

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

圖7 主站主程序流程

3.3 從站軟件總體設(shè)計(jì)

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

圖8 從站主程序流程

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

自由運(yùn)行模式是每個(gè)從站根據(jù)自己的時(shí)間中斷來處理來自主站的EtherCAT數(shù)據(jù)，和主站的運(yùn)行周期以及其他從站的周期都沒有關(guān)系，主站發(fā)送數(shù)據(jù)幀的時(shí)間與從站處理數(shù)據(jù)幀的時(shí)間不同步[10]。

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

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

3.4 從站狀態(tài)機(jī)設(shè)計(jì)

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

圖9 從站狀態(tài)轉(zhuǎn)換

圖10所示為狀態(tài)機(jī)的轉(zhuǎn)換流程，負(fù)責(zé)狀態(tài)機(jī)模塊的主要函數(shù)為ECAT_Main()，被主函數(shù)循環(huán)調(diào)用，從站狀態(tài)轉(zhuǎn)換一般由主站發(fā)起，主站發(fā)出狀態(tài)請求并將請求的狀態(tài)寫入到AL控制寄存器里面，從站接收到主站的狀態(tài)請求后，讀取AL控制寄存器里的值，根據(jù)不同的狀態(tài)轉(zhuǎn)換，檢查SM寄存器的配置[12]。對于PreOP狀態(tài)，檢查SM0和SM1的配置；對于SafeOP狀態(tài)，檢查應(yīng)用層輸入和輸出數(shù)據(jù)映射；對于OP狀態(tài)，則檢查SM0-SM3。如果SM配置正確，則將新狀態(tài)寫入狀態(tài)機(jī)實(shí)際狀態(tài)位并調(diào)用轉(zhuǎn)換函數(shù)進(jìn)行轉(zhuǎn)換，并將結(jié)果寫入AL狀態(tài)寄存器，等待主站讀取。若SM配置不正確或轉(zhuǎn)換不正確，則設(shè)置錯(cuò)誤指示位并調(diào)用錯(cuò)誤處理函數(shù)處理，將狀態(tài)未能轉(zhuǎn)換的原因?qū)懭階L代碼寄存器里，主站讀取錯(cuò)誤指示后清除錯(cuò)誤指示位。

圖10 從站狀態(tài)請求與響應(yīng)

3.5 周期性數(shù)據(jù)通信

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

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

3.6 PID算法模塊

由于計(jì)算機(jī)控制是一種采樣控制，它只能根據(jù)采樣值與期望值的偏差計(jì)算控制量，而不能像模擬控制那樣連續(xù)輸出控制量，進(jìn)行連續(xù)控制。由于這一特點(diǎn)，公式里的積分項(xiàng)與微分項(xiàng)不能直接代入使用，必須進(jìn)行離散化處理[13]。離散化處理的方法為：以T作為采樣周期，作為采樣序號，則離散采樣時(shí)間對應(yīng)著連續(xù)時(shí)間，用矩形法數(shù)值積分近似代替積分，用一階后向差分近似代替微分?？勺魅缦陆谱儞Q：

(1)

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

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

(2)

或

(3)

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

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

這種算法的缺點(diǎn)是：采樣的輸出量和以前的任何狀態(tài)都有關(guān)聯(lián)，計(jì)算時(shí)要對ek進(jìn)行累加，工作量大；同時(shí)控制系統(tǒng)輸出對應(yīng)實(shí)際位置，假如系統(tǒng)出現(xiàn)問題，uk則會(huì)大幅變化，進(jìn)而會(huì)引起機(jī)構(gòu)的劇烈變化[14]。

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

(4)

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

(5)

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

PID算法程序如下：

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

{

float iError = 0,iIncpid = 0; //當(dāng)前誤差

iError = TargetVal-NextPoint;//增量計(jì)算

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

iError = 0; //|e|< 0.5,不做調(diào)整

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

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

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

sPID.LastError = iError;

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

}

4 主從站測試與分析

4.1 主站任務(wù)調(diào)度測試

在Xenomai-Linux實(shí)時(shí)內(nèi)核模式下，運(yùn)行測試程序latency，設(shè)定周期為100 μs，測試時(shí)長1 h，測試控制系統(tǒng)主站的任務(wù)調(diào)度延時(shí)性。測試結(jié)果如表1所示：最大的調(diào)度延時(shí)為27.367 μs，最小的調(diào)度延時(shí)為0.015 μs，平均調(diào)度延時(shí)為1 μs左右，能夠滿足EtherCAT主站實(shí)時(shí)性要求。

表1 主站調(diào)度延時(shí) 單位：μs

4.2 主從站實(shí)時(shí)性測試

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

圖12 wireshark抓取EtherCAT報(bào)文

圖13 系統(tǒng)發(fā)送周期統(tǒng)計(jì)

5 實(shí)驗(yàn)驗(yàn)證

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

圖14 連續(xù)體機(jī)器人 圖15 運(yùn)動(dòng)實(shí)驗(yàn)

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

圖15僅展示第一節(jié)機(jī)械臂的運(yùn)動(dòng)，圖15(a)、圖15(b)分別對應(yīng)0、6 s時(shí)運(yùn)動(dòng)狀態(tài)。由運(yùn)動(dòng)實(shí)驗(yàn)曲線(圖16)可以得到：機(jī)械臂在整個(gè)運(yùn)動(dòng)過程中的運(yùn)動(dòng)較為平穩(wěn)，在按照設(shè)定的軌跡運(yùn)動(dòng)時(shí)運(yùn)動(dòng)精度較高，基本實(shí)現(xiàn)了設(shè)想的功能，使得該實(shí)驗(yàn)成功實(shí)現(xiàn)。

6 結(jié)論

對于連續(xù)體機(jī)器人在控制時(shí)實(shí)時(shí)性低、穩(wěn)定性差的問題，提出一種基于EtherCAT的連續(xù)體機(jī)器人主從站控制系統(tǒng)，得出以下結(jié)論。

(1)在開源的Linux操作系統(tǒng)內(nèi)核里增加了一個(gè)實(shí)時(shí)內(nèi)核，再基于此操作系統(tǒng)移植開源主站協(xié)議棧SOEM，實(shí)現(xiàn)了具備高實(shí)時(shí)性、低延時(shí)的開源控制系統(tǒng)主站。

(2)設(shè)計(jì)了從站控制器AX58100+從站微處理器STM32F407的從站控制板，AX58100與STM32F407之間通過SPI接口通信，主站能夠通過網(wǎng)口對從站控制板發(fā)送控制指令，從站接收來自主站的控制指令和數(shù)據(jù)進(jìn)行相應(yīng)的數(shù)據(jù)分析和處理，對電機(jī)運(yùn)動(dòng)進(jìn)行協(xié)調(diào)，從而實(shí)現(xiàn)對機(jī)械臂的控制，從站還能采集相應(yīng)的編碼器與傳感器數(shù)據(jù)發(fā)送給主站。

(3)對主從站進(jìn)行任務(wù)調(diào)度延時(shí)測試以及實(shí)時(shí)性測試，結(jié)果表明：主從站通信具有較低的延時(shí)以及高實(shí)時(shí)性，同時(shí)通信的抖動(dòng)比較低，表明通信的穩(wěn)定性較好。

(4)根據(jù)設(shè)計(jì)的PID算法對連續(xù)體機(jī)器人進(jìn)行運(yùn)動(dòng)控制實(shí)驗(yàn)驗(yàn)證，實(shí)驗(yàn)結(jié)果表明：該主從站對多個(gè)電機(jī)的協(xié)同控制效果較好，能實(shí)現(xiàn)預(yù)期的運(yùn)動(dòng)。