EtherCAT伺服驅(qū)動(dòng)上位機(jī)指令容錯(cuò)機(jī)制的設(shè)計(jì)

文/譚賀元

0 引言

工業(yè)以太網(wǎng)早在21世紀(jì)前10年就開(kāi)始大規(guī)模應(yīng)用在數(shù)控加工、自動(dòng)化領(lǐng)域，典型應(yīng)用有Sercos II總線(xiàn)、Profi net總線(xiàn)、EtherCAT總線(xiàn)、Ethernet/IP總線(xiàn)等。其中，EtherCAT于2018～2019年間在自動(dòng)化領(lǐng)域得到廣泛應(yīng)用。但是工業(yè)環(huán)境復(fù)雜多變，而工業(yè)設(shè)備既要在充滿(mǎn)電磁干擾的環(huán)境下工作，又要維持穩(wěn)定運(yùn)轉(zhuǎn)，這給工業(yè)以太網(wǎng)的方案設(shè)計(jì)帶來(lái)不小的挑戰(zhàn)。此外，一些應(yīng)用場(chǎng)合除要考慮硬件成本外，還需考慮硬件方案的可靠性設(shè)計(jì)所帶來(lái)的試錯(cuò)成本。軟件設(shè)計(jì)成本低、驗(yàn)證周期較短、改動(dòng)方式靈活，甚至不受硬件供貨影響，因此工業(yè)以太網(wǎng)方案可先在軟件上進(jìn)行架構(gòu)設(shè)計(jì)和算法調(diào)整，以便快速滿(mǎn)足客戶(hù)需求。

在機(jī)器人、機(jī)械臂、金屬加工等設(shè)備中，工件需以特定角度移動(dòng)到指定的位置，加工設(shè)備需要根據(jù)工件的加工要求移動(dòng)到特定的位置，這對(duì)工件和加工設(shè)備移動(dòng)過(guò)程的定位精度有一定要求，因此伺服驅(qū)動(dòng)通常使用位置控制模式對(duì)電機(jī)進(jìn)行控制，這樣就需要上位機(jī)通過(guò)現(xiàn)場(chǎng)總線(xiàn)對(duì)伺服驅(qū)動(dòng)發(fā)送連續(xù)的位置指令，并且獲取實(shí)時(shí)的位置反饋，實(shí)現(xiàn)電機(jī)閉環(huán)控制。但在復(fù)雜工業(yè)現(xiàn)場(chǎng)，保證網(wǎng)絡(luò)通信數(shù)據(jù)幀的完整性是一大難點(diǎn)。雖然可以在物理層屏蔽和干擾過(guò)濾，通過(guò)鏈路層的重傳機(jī)制減少概率，但依舊無(wú)法避免數(shù)據(jù)幀的丟失。在數(shù)字系統(tǒng)中，上位機(jī)發(fā)送的指令是離散的，伺服驅(qū)動(dòng)接收的指令也是離散的，伺服驅(qū)動(dòng)通過(guò)計(jì)算兩次指令的差值獲取電機(jī)運(yùn)轉(zhuǎn)的增量值，這對(duì)上位機(jī)發(fā)出的指令順序有很高的要求，必須盡量做到順序正確和指令完整。譬如在規(guī)定的周期內(nèi)，伺服驅(qū)動(dòng)需要接收到5個(gè)指令，但中間丟失了3個(gè)，則第一和第四次指令的差值容易出現(xiàn)不合理的情況，導(dǎo)致指令突變，為跟隨突變的指令，伺服驅(qū)動(dòng)瞬間輸出較大電流，導(dǎo)致電機(jī)突然加速，帶來(lái)剛性沖擊，造成異響，甚至導(dǎo)致工件或者設(shè)備損壞。本文基于指令丟失背景下所導(dǎo)致的數(shù)據(jù)完整性缺失，設(shè)計(jì)了伺服驅(qū)動(dòng)與上位機(jī)通信的指令容錯(cuò)機(jī)制，以確保數(shù)據(jù)的連續(xù)性，減少數(shù)據(jù)丟失造成的負(fù)面影響。

1 伺服控制系統(tǒng)的硬件平臺(tái)

伺服驅(qū)動(dòng)是一個(gè)以電機(jī)控制為主要對(duì)象的實(shí)時(shí)執(zhí)行機(jī)構(gòu)，需對(duì)電機(jī)控制所需的操作流程進(jìn)行快速響應(yīng)，通過(guò)總線(xiàn)接受上位機(jī)指令控制，因此總線(xiàn)通信對(duì)電機(jī)控制的同步行為的抖動(dòng)偏差需要控制在微秒級(jí)別。一個(gè)典型的伺服控制系統(tǒng)硬件平臺(tái)如圖1所示，主控芯片為T(mén)I C2000系列的數(shù)字信號(hào)處理芯片（DSP），型號(hào)為T(mén)MS320F28377S，現(xiàn)場(chǎng)總線(xiàn)通信硬件鏈路層芯片為ADI公司的FIDO5200，它和DSP之間采用16位并行總線(xiàn)連接。C2000系列DSP在電機(jī)控制領(lǐng)域應(yīng)用廣泛，其A/D轉(zhuǎn)換精度具有14bit采樣深度，在電流采樣精度上具有優(yōu)勢(shì)。ADI公司的FIDO5200結(jié)合了微控制器和門(mén)陣的特征，可滿(mǎn)足多協(xié)議的鏈路層芯片的需求，但是這種靈活性限制了并行接口的數(shù)據(jù)訪(fǎng)問(wèn)性能，需要通過(guò)握手的方式進(jìn)行鏈路層五路空間的間接訪(fǎng)問(wèn)，增加了主控芯片軟件的時(shí)間資源利用率。

圖1 伺服控制系統(tǒng)的硬件平臺(tái)

該硬件平臺(tái)在實(shí)際使用時(shí)發(fā)現(xiàn)，由于 C2000系列DSP在處理循環(huán)語(yǔ)句的時(shí)候需要用到判斷和長(zhǎng)跳轉(zhuǎn)指令，當(dāng)這兩條指令出現(xiàn)在中斷內(nèi)部時(shí)候，會(huì)消耗大量的時(shí)間，進(jìn)而影響中斷控制實(shí)時(shí)性?；趯?shí)時(shí)性考量，需通過(guò)以空間換時(shí)間的方式，快速執(zhí)行完總線(xiàn)的時(shí)序動(dòng)作，進(jìn)而給電機(jī)控制算法留出足夠的時(shí)間。

2 伺服通信的容錯(cuò)機(jī)制設(shè)計(jì)

數(shù)控系統(tǒng)的指令通過(guò)EtherCAT總線(xiàn)傳輸?shù)剿欧?qū)動(dòng)。一個(gè)通信周期內(nèi)的伺服驅(qū)動(dòng)的軟件執(zhí)行架構(gòu)抽象模型, 從站或者從節(jié)點(diǎn)的通信協(xié)議處理部分，通常分成實(shí)時(shí)和非實(shí)時(shí)部分（見(jiàn)圖2）。實(shí)時(shí)性的重點(diǎn)在于確定的時(shí)間點(diǎn)以及確定的行為，因此通信解析的實(shí)時(shí)部分通常放在固定時(shí)間點(diǎn)執(zhí)行的中斷函數(shù)中進(jìn)行，用來(lái)執(zhí)行實(shí)時(shí)要求較高的電機(jī)控制、數(shù)據(jù)丟失檢測(cè)、指令數(shù)據(jù)緩沖等功能。非實(shí)時(shí)狀態(tài)機(jī)部分則執(zhí)行優(yōu)先級(jí)較低的程序。

圖2 伺服驅(qū)動(dòng)軟件執(zhí)行架構(gòu)抽象模型

作為從站，伺服驅(qū)動(dòng)對(duì)指令的處理包括數(shù)據(jù)包接收、指令同步解析、指令映射三個(gè)部分。

2.1 數(shù)據(jù)正確性檢測(cè)機(jī)制

（1）數(shù)據(jù)包丟失檢測(cè)機(jī)制

在現(xiàn)場(chǎng)總線(xiàn)中，數(shù)據(jù)包的接收呈現(xiàn)出周期特征，一般是一個(gè)通信周期接收一次。若是其中某個(gè)通信周期丟失了指令，將造成從站指令跳變，影響加工效果，甚至造成機(jī)械故障。因此需要在數(shù)據(jù)包接收的部分加入數(shù)據(jù)丟失檢測(cè)機(jī)制。由于數(shù)據(jù)包呈現(xiàn)出周期特征，從站內(nèi)部可根據(jù)通信周期設(shè)置脈沖信號(hào)，每個(gè)通信周期檢測(cè)一次，在兩個(gè)脈沖信號(hào)之間檢測(cè)數(shù)據(jù)接收的情況。

本文使用信號(hào)響應(yīng)機(jī)制，對(duì)數(shù)據(jù)包的接收進(jìn)行檢測(cè)。有關(guān)符號(hào)表示如下：C表示通信周期更新的脈沖信號(hào)，由從站自身發(fā)出，即使通信中斷也不受影響，通過(guò)同步信號(hào)調(diào)整發(fā)出的時(shí)間；D表示數(shù)據(jù)包到達(dá)的脈沖信號(hào)；R表示數(shù)據(jù)丟失檢測(cè)脈沖；Rcnt表示數(shù)據(jù)丟失計(jì)數(shù)器。

檢測(cè)機(jī)制為當(dāng)C出現(xiàn)時(shí)檢查R是否為0，然后令R=1，若R不為0則讓 Rcnt加1；當(dāng)D出現(xiàn)時(shí)，令R = 0。R作為信號(hào)量實(shí)現(xiàn)數(shù)據(jù)包接收和通信周期更新兩個(gè)線(xiàn)程的握手，握手失敗則代表數(shù)據(jù)丟失。

（2）周期數(shù)據(jù)指令校驗(yàn)機(jī)制

數(shù)據(jù)包接收完成后，要進(jìn)行數(shù)據(jù)的校驗(yàn)，因此周期數(shù)據(jù)指令校驗(yàn)機(jī)制在指令同步解析的部分執(zhí)行。常見(jiàn)的校驗(yàn)方法有累加和校驗(yàn)（check sum）、循環(huán)冗余校驗(yàn)等。累加和校驗(yàn)就是把數(shù)組分成Nf段，Nf為大于0的整數(shù)，逐段相加，得出校驗(yàn)和。循環(huán)冗余校驗(yàn)是在累加和校驗(yàn)的基礎(chǔ)上，使用特定的變量作為除數(shù)，對(duì)每段數(shù)據(jù)進(jìn)行模2除法求余，然后逐級(jí)計(jì)算，得出最后校驗(yàn)結(jié)果。最后，將從站計(jì)算的校驗(yàn)和或者校驗(yàn)結(jié)果與主站下發(fā)的校驗(yàn)和或校驗(yàn)結(jié)果進(jìn)行比較。這些校驗(yàn)機(jī)制都需要用到循環(huán)計(jì)算，而受限于成本，從站主控芯片在實(shí)時(shí)中斷的運(yùn)算資源有限，不適合為通信進(jìn)行大量的運(yùn)算。

因此，本文另辟蹊徑，對(duì)校驗(yàn)方式進(jìn)行簡(jiǎn)化，為主站生成一套校驗(yàn)數(shù)組，該數(shù)組的總長(zhǎng)度為所發(fā)送數(shù)據(jù)的Nc倍，其中Nc≥8。由于從站主控芯片的地址總線(xiàn)為16位，數(shù)組的基本元素為16位，因此將Nc值定為16。該校驗(yàn)數(shù)組在通信建立過(guò)程中發(fā)往從站，具體操作為周期數(shù)據(jù)傳輸前主從站各自將校驗(yàn)數(shù)組元素序號(hào)清零，周期數(shù)據(jù)傳輸時(shí)主站將數(shù)組元素逐一發(fā)送至從站，循環(huán)進(jìn)行，從站接收到周期數(shù)據(jù)之后將主站發(fā)送的校驗(yàn)數(shù)組元素和從站的校驗(yàn)數(shù)組元素進(jìn)行對(duì)比，達(dá)到校驗(yàn)的效果。此法節(jié)省從站運(yùn)算時(shí)間，保證從站電機(jī)控制的實(shí)時(shí)性。

2.2 指令丟失或出錯(cuò)后的容錯(cuò)處理

一般情況下，為了滿(mǎn)足數(shù)控系統(tǒng)主站和從站之間的同步，從站在執(zhí)行架構(gòu)的指令的映射部分把數(shù)控系統(tǒng)下發(fā)的指令轉(zhuǎn)化為電機(jī)控制所需的數(shù)據(jù)。指令丟失和容錯(cuò)具有如下特征：

（1）破壞性。

電機(jī)控制所需的電流和電壓是連續(xù)的，伺服電流和電壓的方式是離散的，需要通過(guò)D/A轉(zhuǎn)換，轉(zhuǎn)換過(guò)程中的數(shù)字量根據(jù)數(shù)控系統(tǒng)周期下發(fā)的指令求出，出錯(cuò)或丟失的指令可能會(huì)使驅(qū)動(dòng)內(nèi)部穩(wěn)定的數(shù)學(xué)模型變得不穩(wěn)定，進(jìn)而導(dǎo)致加工效果變差，甚至床身異響，最后機(jī)械故障。

（2）滯后性。

由于數(shù)控系統(tǒng)下發(fā)的指令在指令的映射中完成，而數(shù)控系統(tǒng)下發(fā)數(shù)據(jù)的正確性在指令同步解析中確認(rèn)，因此作為一個(gè)事件，指令丟失或出錯(cuò)存在天然的滯后性。

數(shù)據(jù)丟失后，為了應(yīng)對(duì)指令的滯后性，減少錯(cuò)誤指令帶來(lái)的破壞，需要根據(jù)過(guò)往的指令預(yù)測(cè)出當(dāng)前周期的指令。從站記錄過(guò)往周期的數(shù)控系統(tǒng)指令,過(guò)往指令記錄所使用的緩沖呈現(xiàn)出先進(jìn)先出（FIFO）的隊(duì)列式數(shù)據(jù)結(jié)構(gòu)，一般使用數(shù)組記錄過(guò)往周期的指令來(lái)減少運(yùn)算時(shí)間，提高代碼執(zhí)行效率。

2.3 指令數(shù)組隊(duì)列棧設(shè)計(jì)

（1）確定數(shù)組的長(zhǎng)度

數(shù)組的長(zhǎng)度和指令預(yù)測(cè)的準(zhǔn)確性、預(yù)測(cè)計(jì)算的時(shí)間正向相關(guān)。在計(jì)算機(jī)中通?？梢允褂米笥乙莆坏姆绞酱娉顺ㄖ噶?，因此數(shù)組長(zhǎng)度即數(shù)組元素個(gè)數(shù)Nl的其中一個(gè)特征滿(mǎn)足以下條件：

其中Nl、L為非零整數(shù)，Nl的范圍和預(yù)測(cè)算法相關(guān)。

（2）數(shù)組的使用方式

本文采取加權(quán)平均值的方式對(duì)丟失或出錯(cuò)的指令進(jìn)行預(yù)測(cè)。取Nl= 4作為數(shù)組元素個(gè)數(shù)。由于指令丟失的破壞性在運(yùn)行狀態(tài)發(fā)生改變的時(shí)候體現(xiàn)得更為明顯，因此加權(quán)平均值的權(quán)重需應(yīng)對(duì)加速、減速、方向變化等情況。

加速過(guò)程通常發(fā)生在金屬切割完成后，或者刀具快速移動(dòng)之前，這種情況對(duì)工件影響不大，但減速和換向過(guò)程所導(dǎo)致的危害不容忽視。電機(jī)換向過(guò)程使用專(zhuān)用的補(bǔ)償算法，無(wú)需重復(fù)設(shè)計(jì)，因此重點(diǎn)考慮減速過(guò)程。數(shù)控系統(tǒng)對(duì)電機(jī)發(fā)送減速指令的過(guò)程中，指令的差值越來(lái)越小，因此預(yù)測(cè)的結(jié)果不易過(guò)大，需在最近周期設(shè)置較大的權(quán)重，在最遠(yuǎn)周期設(shè)置較小權(quán)重，如表1所示。

（3）數(shù)據(jù)記錄

在理想狀態(tài)下，可以使用一個(gè)長(zhǎng)度無(wú)限的數(shù)組記錄周期變化的數(shù)據(jù)，每增加一個(gè)周期，數(shù)組標(biāo)號(hào)加1，但這是無(wú)法實(shí)現(xiàn)的，實(shí)際中使用有限長(zhǎng)度的數(shù)組記錄，如長(zhǎng)度為5的數(shù)組FIFO_IN記錄方式如表2所示。

這樣可以把最近5個(gè)周期的數(shù)據(jù)記錄下來(lái)。每個(gè)周期被記錄的數(shù)據(jù)有自己的編號(hào)，保存于fi fo_index變量中，如周期7則fi fo_index = 7。

表1 指令數(shù)組計(jì)算的權(quán)重設(shè)計(jì)

表2 FlFO_lN數(shù)組記錄

（4）數(shù)據(jù)的使用

在數(shù)組FIFO_IN中存儲(chǔ)了最近5個(gè)周期的數(shù)據(jù)，然而這幾個(gè)數(shù)據(jù)的順序并不理想，比如從周期3到周期7的FIFO_IN數(shù)組中的內(nèi)容變化如表3所示。

表3 FlFO_lN數(shù)組實(shí)際記錄情況

如在周期7的時(shí)候，預(yù)期數(shù)組內(nèi)的數(shù)據(jù)按照從舊到新的排列順序如表4所示，而實(shí)際上卻有可能如表3所示。

表4 周期7FlFO_lN數(shù)組預(yù)期記錄情況

由此可見(jiàn)需要重新調(diào)整FIFO_IN的數(shù)據(jù)順序，將調(diào)整后的數(shù)據(jù)記錄在FIFO_OUT數(shù)組中，方便使用。

數(shù)據(jù)順序調(diào)整如果是通過(guò)硬件邏輯移位實(shí)現(xiàn)又快又方便，然而實(shí)際中的CPU無(wú)法做到高速而簡(jiǎn)便地實(shí)現(xiàn)數(shù)據(jù)的移位操作。

為實(shí)現(xiàn)從FIFO_IN取出數(shù)據(jù)并把它們按從舊到新的位置重構(gòu)于FIFO_OUT中，分別定義最新位置變量fi fo_head、最舊數(shù)據(jù)位置變量fi fo_tail。如前所述變量fi fo_index記錄周期號(hào)，指向的是實(shí)時(shí)周期位置，其值無(wú)限循環(huán)更新。其中fi fo_head = fi fo_index%FIFO_LENGTH; （求余運(yùn)算）由于fi fo_head是最新數(shù)據(jù)的位置，fi fo_tail是最舊數(shù)據(jù)的位置，最新到達(dá)最后一個(gè)位置（FIFO_IN[4]）之前，緊跟著最舊的數(shù)據(jù)，到達(dá)最后一個(gè)位置的時(shí)候，最舊數(shù)據(jù)在第一個(gè)位置（FIFO_IN[0]），因此fi fo_head和fi fo_tail的關(guān)系可表達(dá)如下：

重新調(diào)整FIFO_IN的數(shù)據(jù)并把它們放在FIFO_OUT里，分兩部分完成。首先是數(shù)據(jù)的選取，操作方法如下：

在FIFO_IN里，選 取[fi fo_tail]到FIFO_LENG TH的數(shù)據(jù)

在FIFO_OUT里，選取[0]到[fi fo_head]的數(shù)據(jù)

其次是數(shù)據(jù)的放置，操作方法如下

對(duì) 于[fi fo_tail]到[FIFO_LENGTH - 1]的 數(shù) 據(jù)，要從FIFO_OUT的[0]開(kāi)始，但是數(shù)據(jù)的獲取是從FIFO_IN的[fi fo_tail]開(kāi)始，[0]到[fi fo_tail]的距離剛好是i - fi fo_tail , 若是把這兩個(gè)距離對(duì)應(yīng)起來(lái)，則表達(dá)如下：

對(duì) 于[fi fo_tail]到[FIFO_LENGTH - 1]的 數(shù) 據(jù)，要從FIFO_OUT的[fi fo_tail]開(kāi)始，但是數(shù)據(jù)的獲取是從FIFO_IN的[0]開(kāi)始，[i]到[fi fo_head]的距離是FIFO_LENGTH - fi fo_tail ,因此fi fo_head這部分?jǐn)?shù)據(jù)的放置從[FIFO_LENGTH - fi fo_tail + i]開(kāi)始，表達(dá)如下：

最后，F(xiàn)IFO_OUT存儲(chǔ)了最近FIFO_LENGTH周期的數(shù)據(jù)。

這種方法減少了循環(huán)的次數(shù)，減少了單個(gè)元素的空間和復(fù)雜性，使代碼易于維護(hù)，易于故障的排查，減少了長(zhǎng)跳轉(zhuǎn)指令的使用，無(wú)需使用優(yōu)化，保證程序內(nèi)部時(shí)序的可控以及主控芯片內(nèi)部空間的安全。

3 應(yīng)用效果測(cè)試

使用指令容錯(cuò)之前輸出W線(xiàn)測(cè)得電壓有±6V波動(dòng)，伺服驅(qū)動(dòng)顯示電流為2.7A，如圖3（a）所示；使用指令容錯(cuò)之后輸出W線(xiàn)測(cè)得電壓，其波動(dòng)消減到±2V以?xún)?nèi)，伺服顯示電流為0.7A，如圖3（b）所示。

圖3 使用指令容錯(cuò)機(jī)制前后測(cè)試結(jié)果

4 結(jié)語(yǔ)

使用了指令容錯(cuò)機(jī)制之后，電機(jī)的抖動(dòng)顯著減少，伺服輸出電流和電壓的峰值也顯著降低，異響消除，解決了時(shí)間緊迫情況下面臨的問(wèn)題。但如上文中所述，通信協(xié)議的可靠性和實(shí)用性是通過(guò)長(zhǎng)期的不同場(chǎng)合的大量測(cè)試和優(yōu)化而提升，因此使用指令容錯(cuò)機(jī)制這種方式只能在時(shí)間較少、對(duì)伺服控制精度要求較低的情況下使用。若在使用EtherCAT總線(xiàn)的時(shí)候，要同時(shí)保證伺服的性能和可靠性，只能花時(shí)間深入了解EtherCAT通信的機(jī)理和器件以及鏈路層的實(shí)現(xiàn)方式，并且與上位機(jī)軟件開(kāi)發(fā)人員深度配合，這樣才能獲得可靠、高效、高性能的EtherCAT應(yīng)用。