張 洪，黃 昭，張小勇，王通德

(1.江南大學(xué)機械工程學(xué)院，江蘇省食品先進制造裝備技術(shù)重點實驗室，江蘇無錫 214122；2.江蘇南瑞恒馳電氣裝備有限公司，江蘇無錫 214161)

0 引言

隨著智能制造的快速發(fā)展和機器人應(yīng)用范圍的擴大，機器人面臨的環(huán)境越來越復(fù)雜，對機器人的性能要求也越來越高。工業(yè)機器人通常由機械臂與固定底座構(gòu)成，在當(dāng)?shù)氐墓ぷ骺臻g內(nèi)執(zhí)行一系列任務(wù)[1]。智能、網(wǎng)絡(luò)化、高精度、高速正成為工業(yè)機器人未來的發(fā)展趨勢。網(wǎng)絡(luò)化控制實現(xiàn)了伺服驅(qū)動器、控制器和傳感器之間的相互通信，以及生產(chǎn)線上各機器人之間的信息交換，簡化了系統(tǒng)結(jié)構(gòu)，實現(xiàn)了協(xié)同操作。此外，多關(guān)節(jié)機器人的網(wǎng)絡(luò)化運動控制器是實現(xiàn)高精度、智能化控制的重要因素。然而，傳統(tǒng)機器人系統(tǒng)的開放性和可擴展性較差，大多數(shù)不具備網(wǎng)絡(luò)能力。其控制器通常具有特定的功能，特殊的封閉特性限制了系統(tǒng)的擴展和改進。傳統(tǒng)的機器人控制器軟件難以從一個系統(tǒng)轉(zhuǎn)移到另一個系統(tǒng)，且由于控制器的封閉結(jié)構(gòu)，難以根據(jù)需求進行擴展。

機器人開源操作系統(tǒng)(robot operating system,ROS)對于機器人的各種軟硬件進行了封裝，對于不同類型的機器人，通過ROS可以使用相同的方式表示，ROS為用戶提供模塊化的通信機制，用戶可以方便地替換系統(tǒng)內(nèi)的各種模塊，從而促使機器人的研發(fā)周期顯著縮減。實時工業(yè)以太網(wǎng)具有高精度、高實時性以及數(shù)據(jù)傳輸性能強等優(yōu)點，在高精度機器人控制系統(tǒng)中得到廣泛的關(guān)注與應(yīng)用[2]。

針對現(xiàn)有工業(yè)機器人研發(fā)周期長、實時性差、擴展性能低等問題，本文通過IgH主站與ROS融合，結(jié)合分布式時鐘同步技術(shù)，設(shè)計了一種實時性強、同步性高和性能穩(wěn)定的多關(guān)節(jié)機器人控制系統(tǒng)。

1 控制系統(tǒng)硬件組成

為了控制系統(tǒng)能夠?qū)崿F(xiàn)高效性和快速傳輸性，采用支持實時工業(yè)以太網(wǎng)EtherCAT的驅(qū)動器進行通訊，多關(guān)節(jié)機器人為典型的串聯(lián)機器人。上位機采用帶有Linux系統(tǒng)以及IgH主站的PC機，機器人控制系統(tǒng)采用一主多從的線性網(wǎng)絡(luò)拓撲結(jié)構(gòu)，通過網(wǎng)線將上位機和伺服驅(qū)動器連接。

2 控制系統(tǒng)軟件架構(gòu)設(shè)計

控制系統(tǒng)軟件架構(gòu)在帶有IgH主站和ROS的Linux系統(tǒng)下實現(xiàn)，軟件架構(gòu)如圖1所示。其中ROS主要提供Moveit!功能包集和ros_control框架，Moveit!作為上層通過插件進行運動規(guī)劃，可以和不同的運動規(guī)劃庫鏈接；ros_control作為下層提供控制器管理器、控制器、硬件資源接口以及硬件抽象層，實現(xiàn)中間控制環(huán)節(jié)；IgH主站通過EtherCAT總線與伺服驅(qū)動器進行通信。利用多線程通信，將IgH主站和ROS平臺融合，實現(xiàn)驅(qū)動器編碼器數(shù)據(jù)與多關(guān)節(jié)機器人關(guān)節(jié)角度的傳輸與轉(zhuǎn)換。

圖1 控制系統(tǒng)軟件框架

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

控制系統(tǒng)實現(xiàn)內(nèi)容主要包括底層驅(qū)動的實現(xiàn)，ROS平臺和IgH主站的融合。

3.1 底層驅(qū)動控制實現(xiàn)

基于ROS和IgH主站的多關(guān)節(jié)機器人控制系統(tǒng)底層驅(qū)動需要實現(xiàn)以下功能：

(1)通過EtherCAT通信技術(shù)實現(xiàn)伺服驅(qū)動器與上位機的通信；

(2)通過分布式時鐘(distributed clock,DC)控制各從站任務(wù)同步執(zhí)行；

(3)通過硬件交互層為控制器提供硬件抽象層接口，并完成機器人關(guān)節(jié)角度到電機目標(biāo)位置的映射。

3.1.1 EtherCAT通信實現(xiàn)

EtherCAT數(shù)據(jù)通過以太網(wǎng)數(shù)據(jù)幀進行傳輸，數(shù)據(jù)幀由主站發(fā)起控制各驅(qū)動器數(shù)據(jù)的接收和發(fā)送，由于以太網(wǎng)設(shè)備具有獨立處理雙向傳輸?shù)墓δ躘3]，在一個運行周期內(nèi)，各驅(qū)動器可以對主站發(fā)送的下行報文進行直接處理，并從報文中讀取或?qū)懭胂嚓P(guān)用戶數(shù)據(jù)，然后將報文數(shù)據(jù)傳輸給下一個驅(qū)動器，當(dāng)所有驅(qū)動器完成數(shù)據(jù)幀的處理后，由最后一個驅(qū)動器從后向前依次發(fā)回報文至主站[4]。工業(yè)機器人通信系統(tǒng)要求能夠在規(guī)定時間內(nèi)完成數(shù)據(jù)的收發(fā)，如各軸的狀態(tài)反饋信息，要求數(shù)據(jù)精確且通信延遲抖動低。標(biāo)準(zhǔn)的Linux內(nèi)核只滿足軟實時的要求，由于機器人控制系統(tǒng)對實時性要求較高，所以采用帶有Xenomai實時補丁的內(nèi)核，并將IgH用戶層周期性執(zhí)行任務(wù)移植到Xenomai實時內(nèi)核中[5]，通過創(chuàng)建實時任務(wù)的線程，設(shè)定調(diào)度優(yōu)先級，完成Xenomai周期任務(wù)，主站和伺服驅(qū)動器通信過程如圖2所示。

圖2 EtherCAT通信過程

主站和上位機實現(xiàn)通信后，過程數(shù)據(jù)的讀寫通過EtherCAT應(yīng)用程序來實現(xiàn)，PDO通過“數(shù)據(jù)域指針+地址偏移量”的方式直接讀寫[6]；再將數(shù)據(jù)域的所有報文插入到主站的報文序列；最后將所有報文發(fā)送到傳輸序列，從而實現(xiàn)主站和伺服驅(qū)動器的數(shù)據(jù)交互。從站的周期性數(shù)據(jù)根據(jù)CiA402去配置，應(yīng)用層采用CoE協(xié)議[7]。本文驅(qū)動器的控制模式為周期性同步位置模式(CSP)。所需配置的PDO以及對象字典如表1所示。

表1 PDO配置

3.1.2 分布式時鐘實現(xiàn)

多關(guān)節(jié)機器人控制系統(tǒng)在實際應(yīng)用中存在多軸聯(lián)動的情況，以執(zhí)行相對復(fù)雜的曲線運動，若關(guān)節(jié)不同步，會產(chǎn)生誤差，因此各從站需要使用相同的系統(tǒng)時間，使從站同步執(zhí)行任務(wù)，可以通過分布式時鐘產(chǎn)生同步的輸出信號，同步主從站之間的時鐘，從而實現(xiàn)多軸同步運動。由圖3所示，以n個從站節(jié)點說明各從站時鐘同步過程，包括傳輸延遲計算、時鐘偏移補償和時鐘漂移補償[8]，其中選擇從站1為參考時鐘。

下行報文到達每個從站后，從站寄存器會記錄到達時刻，假設(shè)報文由主站傳輸至參考時鐘從站的時刻記為T1，到達從站n的時刻為T2(n)，則有以下關(guān)系：

T2(n)-T1=Tdelay(n)+Toffset(n)

(1)

式中：Tdelay(n)為傳輸延時；Toffset(n)為初始偏移量。

整理式(1)可得：

Toffset(n)=T2(n)-T1-Tdelay(n)

(2)

上行報文返回主站時，假設(shè)到達從站n的時刻為T3(n)，到達參考時鐘的時刻為T4，由于線纜傳輸報文延時均勻，且各從站處理和轉(zhuǎn)發(fā)報文時間一致，可得傳輸延時為

Tdelay={(T4-T1)-[T3(n)-T2(n)]}/2

(3)

根據(jù)式(2)和式(3)可得初始偏移Toffset：

Toffset=[T2(n)+T3(n)-(T1+T4)]/2

(4)

主站發(fā)送報文讀取相關(guān)寄存器得到傳輸延時和時鐘偏移的數(shù)值后，寫入對應(yīng)從站，由于不同時間讀寫寄存器，每個從站會與參考從站產(chǎn)生漂移，因此需要對時鐘漂移進行補償，從而實現(xiàn)時鐘同步。首先系統(tǒng)的本地時間Tsys_local(n)和參考時間要維持同步關(guān)系，可得本地系統(tǒng)時間為

Tsys_local(n)=Tlocal(n)-Toffset(n)

(5)

式中Tlocal(n)為每個從站的本地時間。

其次將參考時鐘的系統(tǒng)時間Tsys_ref寫入到其他從時鐘設(shè)備中，然后利用參考時鐘系統(tǒng)時間Tsys_ref和傳輸延時Tdelay(n)，得到時鐘漂移補償Δt：

Δt=Tlocal(n)-Toffset(n)-Tdelay(n)-Tsys_ref

(6)

根據(jù)時鐘漂移補償?shù)臄?shù)值，從站本地時鐘需要調(diào)整其運行速度，即在每個運行周期通過增加ΔT的值來補償時間偏差，ΔT的取值方法如下：

(7)

為保證多關(guān)節(jié)機器人控制系統(tǒng)的同步性，首先控制整個系統(tǒng)和伺服驅(qū)動器同時上電，避免通訊部分因非同步上電造成失同步故障。其次，通過初始化時鐘完成系統(tǒng)時間的同步，包括傳輸延時的測量、時鐘偏差和時鐘漂移的補償[9]。然后，通過初始化同步信號完成DC從站的同步，設(shè)置同步信號的周期時間，同步信號的周期時間和控制系統(tǒng)關(guān)節(jié)位置插補周期保持一致。最后控制寄存器，設(shè)置同步信號的開始時間，并激活同步信號。同步信號激活后，機器人控制系統(tǒng)在中斷服務(wù)程序中進行插補運算，然后將數(shù)據(jù)發(fā)給從站，從站接收到同步信號后，對數(shù)據(jù)進行處理，并轉(zhuǎn)發(fā)給主站，完成周期性數(shù)據(jù)交換。

3.1.3 硬件交互層

ros_control定義了機器人硬件接口，控制器通過硬件接口與機器人硬件連接，通過接口層自定義機器人并使用控制器管理器進行控制[10]。

使用硬件交互層與機器人硬件連接首先需要初始化，獲取機器人的關(guān)節(jié)名，并對關(guān)節(jié)的位置、速度、力矩等信息重新分配空間；然后連接并注冊機器人的狀態(tài)接口和位置接口[11]。完成初始化工作后，對機器人的信息進行讀取和寫入。通過API讀接口，主站可以在從站設(shè)備中獲取數(shù)據(jù)幀并處理報文，讀取各從站的狀態(tài)字、當(dāng)前位置以及當(dāng)前速度；通過API寫接口，主站將控制字、目標(biāo)位置寫入機器人系統(tǒng)，從而控制機器人運動。其中目標(biāo)位置是由關(guān)節(jié)角度轉(zhuǎn)化為電機脈沖，再通過中間變量轉(zhuǎn)發(fā)給從站實現(xiàn)運動，目標(biāo)位置與關(guān)節(jié)角度的轉(zhuǎn)換關(guān)系為

(8)

式中：W(n)為各從站的目標(biāo)位置(n=1,2,3,4,5,6)；θ(n)為每個關(guān)節(jié)的關(guān)節(jié)角度(n=1,2,3,4,5,6)；P為電機轉(zhuǎn)一圈指令脈沖數(shù)；R為減速比。

上位機讀取關(guān)節(jié)角度的反饋只需反解此式。

完成硬件交互后，在循環(huán)中，分別執(zhí)行讀取和發(fā)送任務(wù)，并周期性地更新控制器的狀態(tài)，保證實時任務(wù)的更迭。

3.2 ROS平臺和IgH主站的融合

ROS平臺和IgH主站是兩個獨立的架構(gòu)，都采用模塊化設(shè)計[12]。為實現(xiàn)ROS和IgH主站的數(shù)據(jù)交互，將IgH主站模塊封裝為ROS下的驅(qū)動節(jié)點。在相應(yīng)的節(jié)點中，將需要傳遞的數(shù)據(jù)保存在自定義消息中，以發(fā)布和訂閱話題的形式，將所需的機器人關(guān)節(jié)位置等狀態(tài)參數(shù)傳遞至主站，在主站的周期性任務(wù)中實現(xiàn)過程數(shù)據(jù)的交換。同時，通過多線程通信，允許多個節(jié)點同時存在，不同的功能由不同的線程執(zhí)行。ROS平臺和IgH主站的融合流程如圖4所示。

圖4 ROS平臺和IgH主站融合流程

多線程通信的頻率對系統(tǒng)的延遲、實時性以及穩(wěn)定性具有一定的影響，頻率越大，控制系統(tǒng)數(shù)據(jù)的傳輸速率越高，但頻率過大會造成系統(tǒng)的損壞。為選取合適的多線程通信頻率，展開實驗，觀察不同頻率下，系統(tǒng)的運行情況，實驗結(jié)果如表2所示。

由表2可知，多線程通信頻率過高，會引起控制系統(tǒng)的卡死，頻率過低，會增大系統(tǒng)的延遲，降低實時性，控制效果不佳。因此，將多線程通信頻率控制在1 000～1 250 Hz。

4 實驗

本系統(tǒng)運行在ROS平臺和IgH主站中，運行環(huán)境為Ubuntu14.04+Xenomai2.6.5 +IGH EtherCAT Master，上位機采用一臺華碩筆記本，采用一主多從的線性拓撲結(jié)構(gòu)將驅(qū)動器和多關(guān)節(jié)機器人連接，多關(guān)節(jié)機器人實驗平臺如圖5所示。

圖5 控制系統(tǒng)實驗平臺

4.1 系統(tǒng)實時性測試

在機器人的控制系統(tǒng)中，提高系統(tǒng)的實時性是關(guān)鍵問題，本文通過多線程通信，將系統(tǒng)任務(wù)劃分為實時域和非實時域。對本文設(shè)計的控制系統(tǒng)進行實時性能評估，將系統(tǒng)程序分別運行在Linux內(nèi)核和Xenomai實時內(nèi)核下，測量并記錄IgH主站運行周期時間，結(jié)果如圖6所示。由圖6(a)可知，在Linux內(nèi)核下，周期在0.2～6.5 ms波動；由圖6(b)可知，在Xenomai實時內(nèi)核下，周期波動基本穩(wěn)定在0.04 ms，表明在Xenomai內(nèi)核下，IgH主站的周期抖動性更小，EtherCAT的通信更加穩(wěn)定，實時性更強，保證了數(shù)據(jù)傳輸?shù)木_性。

通過實時性測試工具Latency，設(shè)置實時任務(wù)周期為1 ms，分別對系統(tǒng)在用戶、內(nèi)核以及定時器中斷模式下的任務(wù)調(diào)度延遲進行測試，測試結(jié)果如表3所示。由表3可知，用戶模式下，實時任務(wù)調(diào)度最大延遲為11.038 μs，占整個測試周期的0.11%；內(nèi)核模式下，實時任務(wù)調(diào)度最大延遲為8.672 μs，占整個測試周期的0.08%；定時器中斷模式下，實時任務(wù)調(diào)度最大延遲為5.691 μs，占整個測試周期的0.05%。由測試結(jié)果可知，系統(tǒng)任務(wù)調(diào)度延遲從用戶、內(nèi)核到定時器中斷模式依次降低，系統(tǒng)實時性依次增強，說明該系統(tǒng)具有良好的實時性。

(a)Linux內(nèi)核運行周期

(b)Xcnomai內(nèi)核運行周期圖6 主站周期分布

表3 系統(tǒng)任務(wù)調(diào)度延遲測試 μs

4.2 關(guān)節(jié)同步性測試

多關(guān)節(jié)機器人在執(zhí)行較復(fù)雜的任務(wù)時，關(guān)節(jié)間的同步性影響著機器人軌跡規(guī)劃的精度：同步性越好，精度越高。為驗證關(guān)節(jié)間的同步性，對各關(guān)節(jié)設(shè)置相同的目標(biāo)位置，觀察機器人從初始位置到目標(biāo)位置的軌跡變化，根據(jù)各關(guān)節(jié)軌跡變化的一致度，判斷關(guān)節(jié)的同步性能。在實際應(yīng)用中，機器人不會一直處于無負載狀態(tài)，因此，進行了無負載狀態(tài)和有負載狀態(tài)下關(guān)節(jié)同步性能的測試。利用基于Qt的應(yīng)用程序PlotJuggler，記錄關(guān)節(jié)的軌跡變化，直觀反映關(guān)節(jié)間的同步誤差。無負載和有負載狀態(tài)下關(guān)節(jié)同步性的測試結(jié)果如圖7和圖8所示。

(a)關(guān)節(jié)原始測量角度

(b)編碼器輸出數(shù)值圖7 無負載關(guān)節(jié)同步性能測試

(a)關(guān)節(jié)原始測量角度

(b)編碼器輸出數(shù)值圖8 有負載關(guān)節(jié)同步性能測試

由圖7(a)可知，無負載狀態(tài)下，在關(guān)節(jié)剛接收到控制指令運動后，兩關(guān)節(jié)的輸出角度誤差在0.001 25 rad左右，短暫時間后，各關(guān)節(jié)軌跡曲線重合。由圖7(b)可知，將關(guān)節(jié)角度處理成電機的編碼器數(shù)值輸出，電機的編碼器數(shù)值曲線重合。

由圖8(a)可知，有負載狀態(tài)下，各關(guān)節(jié)軌跡未重合，關(guān)節(jié)間的輸出角度誤差穩(wěn)定在0.001 rad。將關(guān)節(jié)角度處理成編碼器數(shù)值后，由圖8(b)可知，由于誤差極小，編碼器數(shù)值曲線重合。由測試結(jié)果分析可知，無負載和有負載狀態(tài)下，關(guān)節(jié)間均呈現(xiàn)出較高的同步性，滿足工業(yè)機器人的控制要求。

4.3 軌跡規(guī)劃誤差測試

完成控制系統(tǒng)的實時性和同步性測試后，需要對多關(guān)節(jié)機器人的軌跡規(guī)劃誤差進行測試，設(shè)置機器人各關(guān)節(jié)角度的期望值，采用改進的快速擴張隨機樹算法進行運動規(guī)劃，測試結(jié)果如圖9所示。

圖9 關(guān)節(jié)軌跡誤差

由圖9可知，各關(guān)節(jié)的期望值為[0.2,0.1,-0.1,-0.15],運行結(jié)果為[0.199 993,0.099 977 2,-0.100 054,-0.150 023]，誤差較小。為驗證控制系統(tǒng)的穩(wěn)定性，設(shè)置60個不同的位姿，得出每個關(guān)節(jié)軌跡誤差的平均值，測試結(jié)果如表4所示。

表4 關(guān)節(jié)軌跡誤差測試

由表4可知，關(guān)節(jié)1到關(guān)節(jié)4的平均軌跡誤差值分別為1.2×10-4rad、1.7×10-4rad、3.0×10-5rad和2.0×10-5rad，軌跡規(guī)劃精度較高，滿足控制要求。

5 結(jié)論

本文針對現(xiàn)有工業(yè)機器人控制系統(tǒng)存在的實時性差、同步性低以及軌跡規(guī)劃精度低等問題，設(shè)計了基于ROS和IgH主站的多關(guān)節(jié)機器人控制系統(tǒng)，采用多線程通信，將IgH主站、Moveit!以及ros_control融合，選取合適的任務(wù)控制周期，在IgH主站下采用分布式時鐘，使該機器人得到有效控制。通過對系統(tǒng)的實時性、同步性以及軌跡規(guī)劃誤差進行測試：該機器人控制系統(tǒng)運行周期波動穩(wěn)定在0.04 ms；關(guān)節(jié)間的輸出角度誤差穩(wěn)定在0.001 rad；關(guān)節(jié)1到關(guān)節(jié)4的平均軌跡誤差值為1.2×10-4rad、1.7×10-4rad、3.0×10-5rad和2.0×10-5rad。表明該多關(guān)節(jié)機器人控制系統(tǒng)具有良好的實時性、同步性以及可靠性。