基于EtherCAT總線的拖動示教機器人控制系統(tǒng)開發(fā)

陳宇鵬 ，高偉強 ，劉建群 ，陳樂南 ，盧一光

（1.廣東工業(yè)大學機電工程學院，廣東 廣州 510006；2.廣東工業(yè)大學廣東省微納加工技術(shù)與裝備重點實驗室，廣東 廣州 510006）

工業(yè)機器人噴涂作業(yè)具有重復精度良好、生產(chǎn)效率高等優(yōu)點，并且能夠?qū)⒐と藦膼毫拥沫h(huán)境中解放出來，在工農(nóng)業(yè)各生產(chǎn)領域得到廣泛應用[1-3]。隨著技術(shù)的進步和市場競爭的發(fā)展，中小企業(yè)對機器人自動化噴涂技術(shù)的需求越來越大。不同于大型企業(yè)，中小型企業(yè)涂裝生產(chǎn)具有多品種、小批量、變化快等特點，并且存在嚴重缺乏機器人和自動化技術(shù)開發(fā)應用人才等問題。目前市面上的噴涂機器人控制大都采用離線示教或示教盒示教編程的方式獲取控制程序，由于機器人操作編程對作業(yè)人員素質(zhì)要求較高，需受過專業(yè)訓練，無疑給中小型涂裝企業(yè)的機器人應用推廣增加了難度。拖拽示教是由操作人員直接拖拽機器人末端噴槍工具的方式實現(xiàn)復雜軌跡的示教，噴涂效果所見即所得，無需編程，簡單易學，效率高，只需簡短培訓就可使原噴涂工人直接成為機器人的操作工人，解決了中小涂裝企業(yè)技術(shù)升級后機器人應用人才短缺的問題。

為提高機器人拖拽示教靈活性，常采用輔助示教技術(shù)實現(xiàn)示教。文獻[4]研究了一種通過變速換擋提高手把手示教的可操作性方法；文獻[5]通過機器人末端六維力傳感器采集的受力信息在位置控制策略下，實現(xiàn)了基于順應性控制的直接示教；文獻[6]發(fā)明了一種通過伺服電機助力提高手把手靈活性的方法。雖然采用示教輔助技術(shù)在一定程度上提高了機器人示教靈活性和操控性，但由于人工拖拽仍存在示教軌跡不平滑與速度不連續(xù)產(chǎn)生沖擊、振動等問題，影響復現(xiàn)時噴涂質(zhì)量和復現(xiàn)速度的提升。

針對中小噴涂企業(yè)的需求和拖拽示教軌跡存在的問題，本文提出了對示教采樣數(shù)據(jù)進行優(yōu)化后再復現(xiàn)的處理方法，并結(jié)合“Linux+Xenomai”系統(tǒng)架構(gòu)，在以高性能處理器AM4377為核心的硬件平臺上，開發(fā)了基于EtherCAT總線的拖拽示教噴涂機器人控制系統(tǒng)。

1 控制系統(tǒng)總體設計

EtherCAT總線是目前國際上主流的實時工控總線，因為其在開放性、實時性、可靠性、安全性、經(jīng)濟性和可擴展性等方面的優(yōu)勢，越來越多的控制系統(tǒng)采用EtherCAT總線[7]。AM437X是美國TI公司推出的基于ARM Cortex-A9內(nèi)核，專門用于在各類工業(yè)自動化設備中實現(xiàn)實時EtherCAT通信標準的處理器系列。本文將采用AM4377處理器為核心，開發(fā)機器人控制系統(tǒng)運控主站。

根據(jù)拖拽示教六關(guān)節(jié)機器人噴涂的工藝特點和要求，控制系統(tǒng)總體架構(gòu)如圖1所示。系統(tǒng)硬件分為三大部分：1）上位機采用工業(yè)計算機IPC，主要負責人機界面和示教采樣的數(shù)據(jù)處理和優(yōu)化等非實時任務；2）控制系統(tǒng)的控制層由ARM處理器AM4377承擔運動控制器任務，根據(jù)應用任務的實時性不同，AM4377運行一個Linux及Xenomai內(nèi)核，分別處理網(wǎng)絡通訊、指令處理、示教采樣、插補控制等弱實時和強實時運控任務；3）設備層是EtherCAT總線連接的各種伺服從站和I/O外設。

圖1 控制系統(tǒng)總體架構(gòu)Fig.1 Structure of control system

采用IPC作為上位機，可充分利用PC機豐富的軟硬件資源、高速的數(shù)據(jù)運算能力和友好的界面開發(fā)能力，滿足系統(tǒng)對人機界面、采樣示教位姿優(yōu)化處理等非實時任務需要。

控制層AM4377主站基礎架構(gòu)由應用程序、EtherCAT協(xié)議棧組成。因為EtherCAT周期性過程數(shù)據(jù)通信和插補運算的強實時需求，需要對主站Linux系統(tǒng)進行實時化改造。Linux的實時性改造通常有兩種方式：一種是直接修改內(nèi)核，對內(nèi)核進行大規(guī)模修改；另一種是在硬件平臺嵌入一個與Linux同時運行的小型實時內(nèi)核。Xenomai是針對Linux內(nèi)核進行強實時擴展的開源實時內(nèi)核[8-9]，被廣泛應用于工業(yè)控制，本文采用Linux+Xenomai作為主站系統(tǒng)內(nèi)核。為實現(xiàn)EtherCAT數(shù)據(jù)幀通訊，本文選用IgH EtherCAT Master協(xié)議棧，主站應用層程序通過調(diào)用IgH的API，實現(xiàn)從站節(jié)點的配置、郵箱數(shù)據(jù)和過程數(shù)據(jù)的收發(fā)任務。

AM4377主站硬件采用市場商品化成熟的AM437X系列最小核心板為基礎開發(fā)。該核心板提供了2個256 MB的DDR3內(nèi)存，1個4 GB的eMMC存儲，1個網(wǎng)卡單元。為了有效利用核心板提供的引腳資源，實現(xiàn)EtherCAT主站和外設控制等功能，利用該核心板提供外設總線接口自行開發(fā)拓展底板。底板由24 V電源供電，包括電源電路、TF卡單元、TTL電平調(diào)試串口、USB和458接口電路。由于核心板只提供1個網(wǎng)卡單元，為同時滿足EtherCAT通信和TCP/IP通信接口，在底板拓展了2路TLK105L網(wǎng)卡單元。AM4377主站具有硬件開發(fā)周期短，穩(wěn)定性和可靠性高、低成本等優(yōu)點。

2 系統(tǒng)軟件設計

機器人控制系統(tǒng)主要實現(xiàn)采樣示教和再現(xiàn)噴涂功能。采樣示教是指人工進行手把手示教噴涂時，控制系統(tǒng)對機器人6軸關(guān)節(jié)和2軸變位軸伺服電機的絕對位置編碼器數(shù)值以及噴涂系統(tǒng)I/O參數(shù)進行定時采樣，記錄得到8坐標的示教采樣數(shù)據(jù)。示教結(jié)束后需要對采樣數(shù)據(jù)進行處理：通過機器人機構(gòu)學正運算求出噴槍在笛卡爾空間運動的位置、姿態(tài)信息，并對這些位姿數(shù)據(jù)優(yōu)化擬合成樣條曲線，然后按照運控原理對噴涂過程的速度、加速度等運動參數(shù)進行優(yōu)化，再根據(jù)插補原理對樣條曲線進行離散化，經(jīng)過機器人逆變換得到優(yōu)化后的關(guān)節(jié)坐標。

機器人再現(xiàn)噴涂時，系統(tǒng)將離散后的關(guān)節(jié)坐標發(fā)送給運動控制系統(tǒng)，利用運控系統(tǒng)的實時線程對關(guān)節(jié)坐標進行精插補，最后將插補數(shù)據(jù)通過EtherCAT總線輸出至各個伺服、I/O等從站設備，實現(xiàn)再現(xiàn)噴涂。

根據(jù)機器人示教、再現(xiàn)功能，以及各程序任務對系統(tǒng)實時性和運算能力的需求，系統(tǒng)軟件模塊如圖2所示。

圖2 系統(tǒng)軟件模塊Fig.2 Module of system software

圖2系統(tǒng)中，Linux程序和Xenomai程序并行運行。為保障EtherCAT周期性通訊等任務的實時性，Xenomai創(chuàng)建的線程優(yōu)先級最高，優(yōu)先對實時線程進行處理，并對系統(tǒng)資源進行占用。當Xenomai線程處理完任務后，再把系統(tǒng)資源的支配權(quán)轉(zhuǎn)交給Linux域中的線程。中斷管線任務分派如圖3所示。

圖3 中斷管線任務分派Fig.3 Interrupt pipeline task assignment

3 IPC上位機程序設計

IPC上位機軟件采用QT5.8.2開發(fā)。噴涂機器人控制系統(tǒng)上位機軟件主要的面向?qū)ο笫菄娡繖C器人的操作人員，需要具有界面友好、操作簡單和實用等特點。根據(jù)企業(yè)拖拽示教噴涂生產(chǎn)流程，將機器人噴涂生產(chǎn)整個過程需要的功能進行分析和整合，采用模塊化的方法設計并完成了各個模塊的開發(fā)。

根據(jù)對采樣數(shù)據(jù)優(yōu)化處理的要求，本文設計IPC上位機對示教數(shù)據(jù)處理的流程如圖4所示。

圖4 示教數(shù)據(jù)處理流程圖Fig.4 Teaching data processing flow chart

3.1 人機交互、通訊和系統(tǒng)管理模塊

上位機IPC端軟件的一個主要任務就是實現(xiàn)人機交互界面、系統(tǒng)設置管理和上/下位機通訊等非實時操作?？刂葡到y(tǒng)的所有參數(shù)，包括Ether-CAT從站參數(shù)、機器人關(guān)節(jié)連桿參數(shù)、機器人減速機參數(shù)、噴涂工藝參數(shù)等的設置、管理由上位機完成，并以數(shù)據(jù)庫表的形式存儲。上位機通過TCP/IP協(xié)議與下位機（AM4377）交互數(shù)據(jù)并發(fā)送各種操控指令。IPC端根據(jù)不同的運動控制和指令功能，封裝了一個運控庫函數(shù)接口，當操作者需要從上位機對系統(tǒng)下達指令時，IPC只需調(diào)用該庫的API函數(shù)，模塊根據(jù)指令類型對指令和數(shù)據(jù)打包成數(shù)據(jù)幀下發(fā)給下位機。

3.2 示教采樣數(shù)據(jù)預處理模塊

示教采樣數(shù)據(jù)預處理模塊的目的是通過示教數(shù)據(jù)構(gòu)造光順的噴槍位、姿曲線。首先，需要將所采樣的機器人關(guān)節(jié)坐標與噴槍位姿在笛卡爾坐標之間進行空間數(shù)據(jù)映射，這必須經(jīng)過機器人機構(gòu)學的正/逆運動矩陣變換，有大量的三角函數(shù)等浮點運算。這些運算沒有實時性要求，可以利用示教與再現(xiàn)之間的時間間隙在IPC完成。上位機程序?qū)κ窘虜?shù)據(jù)進行優(yōu)化時，首先通過高斯濾波對6關(guān)節(jié)坐標示教采樣數(shù)據(jù)進行低通濾波，降低數(shù)據(jù)噪聲。其次，對濾波后的示教數(shù)據(jù)進行機器人正運算，由關(guān)節(jié)坐標得到噴槍在笛卡爾空間下的位置離散數(shù)據(jù)以及對應的姿態(tài)離散數(shù)據(jù)。

位置數(shù)據(jù)描述的是噴槍相對于機器人基坐標系的坐標，姿態(tài)數(shù)據(jù)描述的是噴槍的噴軸矢量。對位置數(shù)據(jù)進行特征點的提取，去除冗余采樣點，用特征點來表示位置軌跡而不丟失軌跡自身的特征。

特征點判斷示意圖如圖5所示，計算空間矢量Si，Si+1之間的夾角αi+1，并與給定的閾值α0作比較。假如αi+1小于等于閾值α0，則認為三點處于同一空間直線上，此時舍去中間點Pi+1。假如αi+1大于閾值α0，則保留中間點Pi+1為特征點。

圖5 特征點判斷示意圖Fig.5 Judgment of feature points

根據(jù)上述方法可以得到一系列示教特征點，每個特征點都有空間坐標和姿態(tài)值。本文通過3次B樣條將上述特征點擬合得到噴槍運動的空間位置曲線C(u)和姿態(tài)曲線q(t)，其中q(t)采用單位四元數(shù)法描述噴槍噴軸矢量的姿態(tài)變化：

式中：u，t為曲線參數(shù)；i，j，k分別為坐標單位向量。

通過文獻[10]的方法可以根據(jù)特征點的空間坐標數(shù)據(jù)確定噴槍位置曲線C(u)；通過文獻[11]的方法可以根據(jù)姿態(tài)數(shù)據(jù)確定基于單位四元數(shù)姿態(tài)表述的噴槍姿態(tài)曲線q(t)。

3.3 粗插補模塊

位置曲線C(u)曲率變化較為復雜，粗插補模塊對曲線C(u)進行運動加減速分段，針對各分段進行連續(xù)區(qū)域的S曲線運動速度規(guī)劃，再根據(jù)時間分割插補原理將C(u)曲線離散。

位置曲線C(u)通過尋找曲率極值點來完成運動加減速分段。曲線C(u)的曲率極值點序列為{C(?0),C(?1),…,C(?n)}。根據(jù)速度的兩個約束條件[12]，得到插補速度約束公式：

式中：ρj為曲率半徑；δmax為最大插補弓高誤差；T0為插補周期；amax為噴槍運動允許最大加速度；vm為系統(tǒng)給定插補速度；vj_max為第j個曲率極值點允許的最大插補速度。

根據(jù)式（2）計算得到所有曲率極值點的最大允許插補速度序列{v0,v1,…,vn}。

通過曲率極值點將曲線C(u)分成若干段速度連續(xù)規(guī)劃曲線段，對每段曲線的加減速控制采用3次S型速度曲線規(guī)劃[13]。減速控制時需提前得知減速點，但正向插補時減速點難以預測，本文采用文獻[14]的減速點預測方法進行每段曲線的減速點計算。該方法將每段曲線的終點作為起點，起點作為終點，進行反向插補，原分段曲線的減速過程變?yōu)榉聪蚯€的加速過程，原減速點即為反向曲線的加速結(jié)束點。完成每段曲線的連續(xù)速度規(guī)劃后，根據(jù)時間分割插補原理對位置曲線離散成插補段。離散插補段的姿態(tài)信息則根據(jù)位姿同步模塊求得。然后對各個插補段位置姿態(tài)數(shù)據(jù)進行機器人機構(gòu)學逆變換，將插補段由笛卡爾坐標映射到關(guān)節(jié)坐標。

噴涂再現(xiàn)時，上位機將插補段關(guān)節(jié)坐標的運控數(shù)據(jù)信息下發(fā)給下位機，由AM4377程序完成關(guān)節(jié)插補控制。

3.4 位姿同步模塊

再現(xiàn)時噴槍的姿態(tài)必須與工人示教的姿態(tài)在空間上基本保持一致。由于噴槍空間位置是擬合后再插補，若直接對噴槍姿態(tài)曲線q(t)進行姿態(tài)插補，則會導致再現(xiàn)時噴槍姿態(tài)與工人示教的姿態(tài)在空間上不一致，噴涂效果可能會出現(xiàn)嚴重偏差。為解決這一問題，可以建立位置曲線C(u)參數(shù)u與姿態(tài)曲線q(t)參數(shù)t直接的映射關(guān)系。

根據(jù)文獻[10-11]，曲線C(u)和q(t)的節(jié)點矢量都是通過規(guī)范積累弦長化方法構(gòu)造的。所以曲線C(u)的節(jié)點矢量U與曲線q(t)的節(jié)點矢量Γ具有一一對應的關(guān)系，利用這一關(guān)系可以給出參數(shù)u和參數(shù)t的線性映射關(guān)系。

首先判斷參數(shù)u所在的節(jié)點矢量區(qū)間[uj,uj+1]，通過曲線C(u)的節(jié)點矢量U和q(t)的節(jié)點矢量Γ的對應關(guān)系，得到參數(shù)t所在區(qū)間[tj,tj+1]。然后建立參數(shù)u，t的線性關(guān)系：

對于點Pi的參數(shù)ui，通過式（3）可以得到該點姿態(tài)在曲線q(t)上的參數(shù)ti。將ti代入姿態(tài)曲線q(t)的表達式，得到該點的單位四元數(shù)姿態(tài)表述。結(jié)合旋轉(zhuǎn)矩陣和四元數(shù)轉(zhuǎn)換公式[15]，可以得到姿態(tài)旋轉(zhuǎn)矩陣Ri，再結(jié)合位置空間坐標Pi，得到噴槍在該點處的變換矩陣，這樣就實現(xiàn)了位置和姿態(tài)的同步。對變換矩陣進行機器人逆運動學計算可以得到機器人6關(guān)節(jié)坐標數(shù)據(jù)。

4 AM4377端程序設計

運動控制程序及EtherCAT主站協(xié)議運行于TI公司的AM4377處理器，系統(tǒng)內(nèi)核采用Linux+Xenomai雙核架構(gòu)，控制器應用程序分為Linux程序和Xenomai程序兩部分。

4.1 AM4377軟件總體結(jié)構(gòu)

根據(jù)Linux和Xenomai各自的任務特點，AM3477程序由4個主要線程構(gòu)成。AM3477程序架構(gòu)如圖6所示。

圖6 AM4377程序架構(gòu)Fig.6 AM4377 program architecture

線程A為通訊線程，通過TCP/IP協(xié)議與IPC通訊，接收上位機發(fā)來的指令數(shù)據(jù)包，并將數(shù)據(jù)包進行解析。該線程會對不同類型的指令分別注冊對應的回調(diào)函數(shù)，并按時間順序添加到相應任務鏈表中等待傳遞給Xenomai。

線程B為Linux內(nèi)核的主線程，其按照任務鏈表的FIFO順序，依次取出任務模塊，并根據(jù)任務的類型，調(diào)用相應的回調(diào)函數(shù)進行任務處理。如果該任務是機器人再現(xiàn)運動，會將關(guān)節(jié)插補段信息通過XDDP通訊傳遞給Xenomai實時核，為后續(xù)的關(guān)節(jié)精插補運算服務。

線程C中設計了一個“系統(tǒng)運行參數(shù)”結(jié)構(gòu)體，負責存儲系統(tǒng)運行的關(guān)鍵數(shù)據(jù)和插補等緩存的位置指針，并定義該結(jié)構(gòu)體的全局變量。線程C將會獲取線程B傳來的數(shù)據(jù)，響應Linux的讀、寫、點位運動、插補運動等命令。對于插補運動指令，線程C會把插補相關(guān)信息寫入插補信息變量中。

線程D為Xenomai的主運算線程，該線程通過一個定時中斷負責關(guān)節(jié)坐標精插補運算和EtherCAT主站通信，因此該線程是一個實時線程。當插補信息變量中存在未執(zhí)行的插補數(shù)據(jù)時，線程D會按時間順序提取出數(shù)據(jù)，然后調(diào)用關(guān)節(jié)插補算法進行處理。最后調(diào)用IgH提供的API接口將數(shù)據(jù)通過EtherCAT總線下發(fā)給Ether-CAT伺服從站。

4.2 Linux程序

在本文開發(fā)的系統(tǒng)中，Linux程序主要負責與IPC上位機進行信息交互，并執(zhí)行噴涂機器人系統(tǒng)的非實時任務，如：接收上位機指令、參數(shù)設置、狀態(tài)修改等任務。根據(jù)功能，其總體包括兩個模塊：指令傳輸模塊和指令處理模塊。

Linux應用程序與IPC之間的數(shù)據(jù)交互通訊采用TCP/IP協(xié)議。根據(jù)操作指令（如：急停、回零指令、采樣指令、運動控制指令、I/O指令、狀態(tài)查詢）的不同，在IPC端封裝了一個命令函數(shù)庫接口。IPC上位機只需調(diào)用運動庫中的API規(guī)定的數(shù)據(jù)結(jié)構(gòu)發(fā)送數(shù)據(jù)即可實現(xiàn)相關(guān)運動和控制命令。圖7所示為IPC上位機API接口與Linux通訊的框架結(jié)構(gòu)。每次通訊都是IPC發(fā)出請求幀，下位機回應。

圖7 IPC上位機API接口與Linux通訊的框架結(jié)構(gòu)Fig.7 The framework of communication between API of IPC host computer and Linux

Linux與上位機交互的指令分為三類，分別是讀指令、寫指令和Cmd指令，指令傳輸模塊會對不同類型的指令分別處理。讀指令和寫指令主要用于上位機對控制器參數(shù)和狀態(tài)的查詢和設置。Cmd指令是上位機對機器人和外設發(fā)出的運控和操作指令。A線程解析指令后會將該指令按照時間順序添加到任務鏈表中，等待指令處理模塊進行處理。

指令處理模塊是AM4377端Linux程序最重要的模塊，主要負責處理接收到的指令。根據(jù)機器人的工作狀態(tài)，系統(tǒng)可以有不同控制狀態(tài)：空閑狀態(tài)、急停狀態(tài)、噴涂示教狀態(tài)、再現(xiàn)運動狀態(tài)、點動狀態(tài)、暫停狀態(tài)、系統(tǒng)錯誤狀態(tài)等。系統(tǒng)定義了一個用于描述系統(tǒng)狀態(tài)類型的結(jié)構(gòu)體，該結(jié)構(gòu)體中包含系統(tǒng)狀態(tài)相關(guān)的變量和函數(shù)指針。系統(tǒng)將不同的狀態(tài)封裝成對應的結(jié)構(gòu)體，其中的函數(shù)指針在初始化時指向?qū)臓顟B(tài)處理函數(shù)。

B線程會不停地去查詢?nèi)蝿真湵硎欠駷榭?，當任務鏈表不為空時，會按FIFO順序取出指令進行處理，根據(jù)指令信息改變當前系統(tǒng)狀態(tài)，并且執(zhí)行該系統(tǒng)狀態(tài)的處理函數(shù)。

4.3 Xenomai程序

Xenomai程序主要負責處理示教采樣、關(guān)節(jié)精插補和EtherCAT主站通訊功能等實時任務。

4.3.1 EtherCAT主站協(xié)議移植

為了在控制系統(tǒng)中實現(xiàn)EtherCAT主站協(xié)議棧，必須將EtherCAT主站協(xié)議移植到Xenomai內(nèi)核。目前開源的EtherCAT主站代碼主要有RTLAB開發(fā)的SOEM（simple open source EtherCAT master）和EtherLab開發(fā)的IgH EtherCAT Master。由于IgH對EtherCAT主站實現(xiàn)更加完整，支持的網(wǎng)卡類型眾多，并提供通用的網(wǎng)卡驅(qū)動程序，因此本文采用了移植IgH EtherCAT Master方案[16-17]。IgH提供了豐富的API接口函數(shù)，Xenomai應用程序模塊調(diào)用IgH提供的API接口實現(xiàn)與EtherCAT從站的數(shù)據(jù)通訊。

4.3.2 XDDP協(xié)議

Xenomai提供XDDP協(xié)議幫助實現(xiàn)Xenomai中的RT（RealTime實時）線程與Linux域中NRT（NotRealTime非實時）線程間的通訊，并且提供了消息管道——基于RTDM的XDDP協(xié)議實現(xiàn)。

在Xenomai中的C線程通過XDDP協(xié)議負責與Linux進行數(shù)據(jù)通訊。線程C中設計了一個“系統(tǒng)運行參數(shù)”結(jié)構(gòu)體，負責存儲系統(tǒng)運行中的數(shù)據(jù)和插補等緩存的位置指針。線程C將會獲取線程B傳來的預處理數(shù)據(jù)，響應Linux的讀、寫、點位運動、插補運動等命令。對于激活主站、復位主站、獲取從站信息等簡單的控制指令，線程C將會直接調(diào)用IgH的API進行主站操作。對于精插補運動指令，線程C會把插補相關(guān)信息寫入“系統(tǒng)運行參數(shù)”結(jié)構(gòu)體的插補信息變量中，交給實時線程D處理。

4.3.3 Xenomai的實時線程

線程D為Xenomai的主運算線程，且為實時線程。使用Xenomai提供的rt_task_create（）函數(shù)創(chuàng)建實時線程，通過rt_task_start（）函數(shù)啟動線程，并設置其采用最高優(yōu)先級（99）。

示教采樣時，線程D調(diào)用IgH的API對機器人6個關(guān)節(jié)軸和2個變位軸的EtherCAT伺服電機絕對位置編碼器數(shù)值進行定時采樣，采樣周期為10 ms，然后通過XDDP協(xié)議將采樣數(shù)據(jù)傳送給B線程。

關(guān)節(jié)精插補運算在線程D中進行，系統(tǒng)采用時間分割插補算法進行關(guān)節(jié)精插補運算。除了插補運算外，線程D根據(jù)軸的運行狀態(tài)（點位運動狀態(tài)、插補運動狀態(tài)、空狀態(tài)等）進行EtherCAT主站控制、狀態(tài)查詢等運算。如果是插補運動狀態(tài)，則根據(jù)“系統(tǒng)運行參數(shù)”變量中的插補信息進行插補運算，計算各關(guān)節(jié)坐標插補增量，然后寫入插補緩存（插補結(jié)果以絕對位置模式保存）。并且線程D將插補結(jié)果寫到EtherCAT過程數(shù)據(jù)域中。

線程D另一個重要任務是處理EtherCAT主從站之間的同步通訊。本文主站與從站之間采用DC通訊模式，實現(xiàn)EtherCAT的DC同步機制，通訊周期為1 ms。Xenomai的線程D負責調(diào)用IgH的API與EtherCAT從站進行通訊，周期需與DC周期相同，因此將線程D的運行周期設置為1 ms。

伺服從站運行于CSP（周期性同步位置控制）模式，主站每1 ms向伺服從站發(fā)送EtherCAT數(shù)據(jù)幀，數(shù)據(jù)幀結(jié)構(gòu)如下結(jié)構(gòu)體，ec_pdo_entry_reg_t結(jié)構(gòu)體類型由IgH定義。

EtherCAT應用層協(xié)議Coe中的CIA402協(xié)議規(guī)定0x6040為伺服的控制字，0x607a為伺服的目標位置字。本文將position數(shù)組與0x607a關(guān)聯(lián)，control數(shù)組與0x6040關(guān)聯(lián)。調(diào)用IgH提供的EC_WRITE_S32函數(shù)可實現(xiàn)對EtherCAT數(shù)據(jù)幀的寫操作，通過寫control數(shù)組可以用于使能伺服，通過寫position數(shù)組則可以驅(qū)動伺服電機運動到指定位置。

5 控制系統(tǒng)實現(xiàn)

針對拖拽示教機器人功能需求進行軟硬件系統(tǒng)設計，實現(xiàn)了控制系統(tǒng)的開發(fā)。工人在進行拖拽示教噴涂時，控制器以10 ms為周期對機器人6個關(guān)節(jié)軸和2個變位軸的伺服電機絕對位置編碼器數(shù)值，以及噴涂系統(tǒng)I/O狀態(tài)進行采樣，并且上傳至上位機保存為關(guān)節(jié)采樣示教文件。

上位機程序?qū)κ窘涛募M行示教采樣數(shù)據(jù)預處理、粗插補、位姿同步等模塊處理后，得到格式相同的新示教文件。圖8a、圖8b所示分別為機器人玻纖噴涂浴缸時新、舊示教文件噴槍位姿仿真圖，圖中軌跡上的短線條段代表機器人噴槍的姿態(tài)向量。優(yōu)化后該段示教軌跡更加光滑，姿態(tài)變化連續(xù)，與原示教軌跡對比姿態(tài)基本同步，同時優(yōu)化后示教點減少調(diào)整為349個，相比于原始示教點數(shù)量870減少了50%，效率得到調(diào)整提高。

圖8 新舊示教文件軌跡位姿仿真圖Fig.8 Simulation diagram of track pose of new and old teaching documents

圖9所示為優(yōu)化后軌跡樣條的曲率變化曲線。從圖9可知優(yōu)化后噴涂軌跡曲率平緩，只出現(xiàn)有限個不大的曲率峰值。圖10所示為優(yōu)化前后噴槍移動速度的曲線對比圖。從圖10a可見，優(yōu)化前噴槍速度變化劇烈、隨機，若提速復現(xiàn)噴涂，機器人運動將出現(xiàn)劇烈的沖擊和振動。優(yōu)化后噴槍速度如圖10b所示，整個運動速度按S型加減速連續(xù)平滑變化，可將粗插補最大速度提升到500 mm/s，有效提高了整體運行速度。同時，機器人沒有產(chǎn)生沖擊和振動。

圖9 優(yōu)化后軌跡樣條的曲率Fig.9 Curvature of optimized trajectory spline

圖10 優(yōu)化前后速度對比圖Fig.10 Speed comparison before and after optimization

再現(xiàn)噴涂時，上位機程序讀取新示教文件，然后下發(fā)至下位機。下位機再對10 ms關(guān)節(jié)空間數(shù)據(jù)進行基于三次多項式的精插補運算，計算出每1 ms時電機的位置，即將349個點密化為3 480個點。圖11為精插補過程中每一步插補所花費的時間分析圖，線程D每1 ms進行一個點的精插補運算，插補運算最大耗時不超過7 μs，遠遠低于線程D的運行周期1 ms，可以滿足周期性任務計算。

圖11 精插補耗時Fig.11 Fine interpolation time

目前該控制系統(tǒng)已完成樣機試制，并在佛山某機器人公司開展玻纖噴涂測試。根據(jù)機器人運行情況可知，系統(tǒng)運行穩(wěn)定，機器人運動平穩(wěn)，能夠?qū)Χ喾N不同的產(chǎn)品進行噴涂作業(yè)，且噴涂質(zhì)量達標。

6 結(jié)論

拖動示教噴涂機器人具有示教噴涂效果所見即所得、無需編程、易操作等優(yōu)點，是多品種、中小批生產(chǎn)型涂裝企業(yè)解決機器人自動化噴涂的有效解決方案。本文采用IPC+AM4377上/下位機開發(fā)了基于EtherCAT總線的拖動示教六關(guān)節(jié)機器人控制系統(tǒng)，實現(xiàn)了機器人拖拽示教噴涂采樣—采樣數(shù)據(jù)優(yōu)化處理—再現(xiàn)噴涂的功能要求，解決了示教數(shù)據(jù)直接用于再現(xiàn)噴涂時存在軌跡不平滑與速度抖動的問題，同時由于采用了EtherCAT總線控制，系統(tǒng)具有軟硬件開發(fā)周期短、性能穩(wěn)定、易維護、可擴展和低成本等優(yōu)點。通過樣機在企業(yè)的運行測試，該噴涂機器人控制系統(tǒng)能夠平穩(wěn)運行，取得了較好的噴涂效果，達到了設計要求。