基于EtherCAT總線的拖動示教機器人控制系統開發

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

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

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

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

針對中小噴涂企業的需求和拖拽示教軌跡存在的問題，本文提出了對示教采樣數據進行優化后再復現的處理方法，并結合“Linux+Xenomai”系統架構，在以高性能處理器AM4377為核心的硬件平臺上，開發了基于EtherCAT總線的拖拽示教噴涂機器人控制系統。

1 控制系統總體設計

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

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

圖1 控制系統總體架構Fig.1 Structure of control system

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

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

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

2 系統軟件設計

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

機器人再現噴涂時，系統將離散后的關節坐標發送給運動控制系統，利用運控系統的實時線程對關節坐標進行精插補，最后將插補數據通過EtherCAT總線輸出至各個伺服、I/O等從站設備，實現再現噴涂。

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

圖2 系統軟件模塊Fig.2 Module of system software

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

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

3 IPC上位機程序設計

IPC上位機軟件采用QT5.8.2開發。噴涂機器人控制系統上位機軟件主要的面向對象是噴涂機器人的操作人員，需要具有界面友好、操作簡單和實用等特點。根據企業拖拽示教噴涂生產流程，將機器人噴涂生產整個過程需要的功能進行分析和整合，采用模塊化的方法設計并完成了各個模塊的開發。

根據對采樣數據優化處理的要求，本文設計IPC上位機對示教數據處理的流程如圖4所示。

圖4 示教數據處理流程圖Fig.4 Teaching data processing flow chart

3.1 人機交互、通訊和系統管理模塊

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

3.2 示教采樣數據預處理模塊

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

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

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

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

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

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

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

3.3 粗插補模塊

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

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

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

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

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

噴涂再現時，上位機將插補段關節坐標的運控數據信息下發給下位機，由AM4377程序完成關節插補控制。

3.4 位姿同步模塊

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

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

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

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

4 AM4377端程序設計

運動控制程序及EtherCAT主站協議運行于TI公司的AM4377處理器，系統內核采用Linux+Xenomai雙核架構，控制器應用程序分為Linux程序和Xenomai程序兩部分。

4.1 AM4377軟件總體結構

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

圖6 AM4377程序架構Fig.6 AM4377 program architecture

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

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

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

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

4.2 Linux程序

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

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

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

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

指令處理模塊是AM4377端Linux程序最重要的模塊，主要負責處理接收到的指令。根據機器人的工作狀態，系統可以有不同控制狀態：空閑狀態、急停狀態、噴涂示教狀態、再現運動狀態、點動狀態、暫停狀態、系統錯誤狀態等。系統定義了一個用于描述系統狀態類型的結構體，該結構體中包含系統狀態相關的變量和函數指針。系統將不同的狀態封裝成對應的結構體，其中的函數指針在初始化時指向對應的狀態處理函數。

B線程會不停地去查詢任務鏈表是否為空，當任務鏈表不為空時，會按FIFO順序取出指令進行處理，根據指令信息改變當前系統狀態，并且執行該系統狀態的處理函數。

4.3 Xenomai程序

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

4.3.1 EtherCAT主站協議移植

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

4.3.2 XDDP協議

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

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

4.3.3 Xenomai的實時線程

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

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

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

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

伺服從站運行于CSP（周期性同步位置控制）模式，主站每1 ms向伺服從站發送EtherCAT數據幀，數據幀結構如下結構體，ec_pdo_entry_reg_t結構體類型由IgH定義。

EtherCAT應用層協議Coe中的CIA402協議規定0x6040為伺服的控制字，0x607a為伺服的目標位置字。本文將position數組與0x607a關聯，control數組與0x6040關聯。調用IgH提供的EC_WRITE_S32函數可實現對EtherCAT數據幀的寫操作，通過寫control數組可以用于使能伺服，通過寫position數組則可以驅動伺服電機運動到指定位置。

5 控制系統實現

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

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

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

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

圖9 優化后軌跡樣條的曲率Fig.9 Curvature of optimized trajectory spline

圖10 優化前后速度對比圖Fig.10 Speed comparison before and after optimization

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

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

目前該控制系統已完成樣機試制，并在佛山某機器人公司開展玻纖噴涂測試。根據機器人運行情況可知，系統運行穩定，機器人運動平穩，能夠對多種不同的產品進行噴涂作業，且噴涂質量達標。

6 結論

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