基于EtherCAT的ROS全向移動導(dǎo)航系統(tǒng)

張 洪，邱曉天

（江南大學(xué)機械工程學(xué)院，江蘇無錫214122）

機器人操作系統(tǒng)（robot operating system，ROS）［1］采用分布式軟件架構(gòu)設(shè)計，十分有利于代碼的復(fù)用和移植。其內(nèi)部集成了豐富的驅(qū)動代碼和算法，可方便地應(yīng)用于不同類型的機器人，大大縮短機器人的研發(fā)周期。通信系統(tǒng)是機器人控制器的重要組成部分，其傳統(tǒng)總線存在協(xié)議不統(tǒng)一、應(yīng)用較為復(fù)雜和實時性較差等問題，大大制約了機器人的控制性能。德國倍福公司開發(fā)的EtherCAT（ether control for automation technology，用于控制自動化技術(shù)的以太網(wǎng)）［2］通信協(xié)議作為目前實時性最好、通信速度最快的工業(yè)以太網(wǎng)協(xié)議已廣泛應(yīng)用于高精度控制系統(tǒng)，成為全球公認的行業(yè)標準。近年來，開源操作系統(tǒng)的實時性能得到顯著提升 ，出現(xiàn)了如 Xenomai、RT Linux（real‐time Linux，實時Linux）、RTAI（real‐time application inter‐face，實時應(yīng)用接口）等實時改造方案［3］，因此機器人控制向著高精度、高實時性的方向發(fā)展。

針對目前移動機器人代碼復(fù)用率低以及應(yīng)用場景受限等問題，本文基于ROS平臺和EtherCAT通信技術(shù)，設(shè)計了一種開源性好、實時性好、可移植性強、成本低的全向移動導(dǎo)航系統(tǒng)。

1 全向移動導(dǎo)航系統(tǒng)的硬件組成

全向移動導(dǎo)航系統(tǒng)采用支持EtherCAT通信的步科伺服驅(qū)動器。全向移動平臺配置了4個伺服驅(qū)動器，舍棄了傳統(tǒng)的運動控制器，通過網(wǎng)線以EtherCAT總線線性串行的方式直接將上位機與伺服驅(qū)動器相連。通過采集激光雷達和里程計的數(shù)據(jù)實現(xiàn)系統(tǒng)自主導(dǎo)航，既保證了其高集成性又保證了開放性。

2 全向移動導(dǎo)航系統(tǒng)的軟件框架設(shè)計

筆者提出的全向移動導(dǎo)航系統(tǒng)的軟件部分主要由ROS平臺提供的導(dǎo)航功能框架和編寫相關(guān)功能包實現(xiàn)［4］。全向移動導(dǎo)航系統(tǒng)的軟件框架如圖1所示。整個導(dǎo)航系統(tǒng)運行在Xenomai實時內(nèi)核中，上層路徑規(guī)劃由ROS功能包集Navigation實現(xiàn)，底層采用EtherCAT技術(shù)進行通信，配置好相關(guān)文件后可驅(qū)動移動平臺。用戶可通過可視化工具rviz與機器人進行交互。controller_manager根據(jù)用戶發(fā)出的指令啟動相應(yīng)的controller，經(jīng)過相關(guān)的路徑規(guī)劃算法運算后，將結(jié)果發(fā)送至底層，最終實現(xiàn)對機器人的控制。

圖1 全向移動導(dǎo)航系統(tǒng)的軟件框架Fig.1 Software framework of omni‐directional mobile navigation system

3 全向移動導(dǎo)航系統(tǒng)的實現(xiàn)

全向移動導(dǎo)航系統(tǒng)的實現(xiàn)過程主要包括：底層控制設(shè)計；全向移動平臺運動學(xué)模型的建立；URDF（unified robot description format，統(tǒng)一的機器人描述格式）模型［5］的建立；里程計odometry［6］的建立；根據(jù)所用的傳感器，在導(dǎo)航功能框架下設(shè)計和配置全向移動平臺的運動控制器。

3.1 底層控制設(shè)計

根據(jù)實際的控制要求，全向移動平臺的底層控制須實現(xiàn)如下2個功能：

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

2）根據(jù)同步性控制要求選擇EtheCAT數(shù)據(jù)幀周期并實現(xiàn)IgH主站與ROS平臺的融合。

3.1.1 EtherCAT通信的實現(xiàn)

在1個通信周期內(nèi)，主站發(fā)送EtherCAT數(shù)據(jù)幀給各個從站；數(shù)據(jù)幀抵達從站后，每個EtherCAT從站控制器（EtherCAT slave control，ESC）依據(jù)數(shù)據(jù)幀內(nèi)容判斷是否進行讀寫操作并將判斷后的數(shù)據(jù)寫入數(shù)據(jù)幀；全部從站處理完數(shù)據(jù)幀后由最后一個從站將處理后的數(shù)據(jù)幀依次按照順序返回主站。主站在整個EtherCAT網(wǎng)絡(luò)中起控制作用。本文采用Linux下功能較為完善的 IgH EtherCAT Master［7］作為主站。考慮到EtherCAT通信以及機器人控制系統(tǒng)的實時性要求，采用帶有Xenomai實時補丁的內(nèi)核，將IgH用戶層中的周期性任務(wù)移植到Xenomai的實時微內(nèi)核中。EtherCAT主從站通信過程如圖2所示。

圖2 EtherCAT主從站通信過程Fig.2 Communication process of master station and slave stations of EtherCAT

激活主站后，分別使用ec_read（）和ec_write（）函數(shù)進行過程數(shù)據(jù)的讀寫，再通過ecrt_master_send（）函數(shù)發(fā)送，實現(xiàn)主從站間的數(shù)據(jù)交換。伺服電機的周期性任務(wù)通過EtherCAT應(yīng)用層COE（CAN open over EtherCAT）協(xié)議實現(xiàn)，其完全遵循CAN open的應(yīng)用層行規(guī)。其中伺服和運動控制的專用應(yīng)用層行規(guī)為CiA402［8］。全向移動導(dǎo)航系統(tǒng)的電機控制模式為周期性同步速度模式。完成周期性任務(wù)所需的PDO（process data object，過程數(shù)據(jù)對象）配置如表1中所示。

表1 PDO配置Table 1 The configuration of PDO

3.1.2 IgH主站與ROS平臺的融合

IgH主站和ROS平臺均采用獨立的模塊化設(shè)計。因此，只需將IgH主站模塊改造成ROS下的驅(qū)動節(jié)點，通過ROS硬件資源接口層以及相關(guān)的硬件接口將所需的機器人速度等狀態(tài)參數(shù)下發(fā)至主站模塊，在主站的周期性任務(wù)中實現(xiàn)過程數(shù)據(jù)的交換，而無需修改IgH主站源碼。IgH主站與ROS平臺的融合過程如圖3所示。

圖3 IgH主站與ROS平臺的融合過程Fig.3 Fusion process of IgH master station and ROS plat‐form

為了取得較好的導(dǎo)航效果，須保證電機間的同步性。在ROS環(huán)境下編寫程序時應(yīng)盡可能選取較短的任務(wù)周期［9］，以減小系統(tǒng)的延遲和反應(yīng)時間，提升實時性。但限于步科伺服驅(qū)動器的性能，不能無限制地縮短任務(wù)周期，否則會造成系統(tǒng)的抖動過大，數(shù)據(jù)幀丟失，伺服驅(qū)動器報警出錯。

為選取合適的任務(wù)周期，進行幾組IgH主站任務(wù)周期與從站運行情況的對照試驗，試驗結(jié)果如表2所示。

表2 IgH主站任務(wù)周期與從站運行情況對照試驗的結(jié)果Table 2 The results of the control test of task cycle of IgH master station and the operation of slave station

經(jīng)過測試發(fā)現(xiàn)，IgH主站任務(wù)周期過短容易引起系統(tǒng)抖動，引發(fā)伺服驅(qū)動器報警，任務(wù)周期過長則會增大系統(tǒng)延遲和響應(yīng)時間，影響控制效果。因此，將IgH主站任務(wù)周期控制在100～200 μs。

3.2 全向移動平臺運動學(xué)模型和里程計模型的建立

全向移動導(dǎo)航系統(tǒng)的移動平臺采用麥克納姆輪的四輪全向驅(qū)動方式［10］。根據(jù)右手法則，設(shè)：車輪半徑為R；輪轂徑向與輥子軸線的夾角為α；移動平臺前進方向為Y方向，軸距為b；移動平臺橫移方向為X方向，軸距為a；1號車輪至4號車輪的角速度分別為w1、w2、w3、w4；移動平臺沿X、Y方向的線速度和繞Z軸轉(zhuǎn)動的角速度分別為vx、vy、w0。移動平臺速度與車輪角速度的轉(zhuǎn)換關(guān)系為：

在EtherCAT周期性任務(wù)中讀取地址位0x606C.0的真實值，根據(jù)式（1）算得移動平臺的真實速度值，并對其瞬時速度進行積分，得出平臺的航跡即里程計odometry所需的數(shù)值。

3.3 URDF模型的建立

為抽象地描述機器人的外觀，便于人機交互，ROS應(yīng)用URDF來建立機器人模型。該格式是基于XML（extensible markup language，可擴展標記語言），將機器人各部件抽象成連桿，通過關(guān)節(jié)及其運動學(xué)關(guān)系將連桿相連接的。ROS提供的可視化工具rviz可顯示URDF模型，方便調(diào)試。ROS中的TF（transform，變換）樹能夠自動求解各個坐標系之間的轉(zhuǎn)換關(guān)系并廣播出來。機器人連桿坐標系的布局如圖4所示。

圖4 機器人連桿坐標系的布局Fig.4 Coordinate system layout of robot connecting rods

3.4 路徑規(guī)劃

在進行路徑規(guī)劃前須對導(dǎo)航功能包進行配置［11］，主要包括全局路徑規(guī)劃器和本地實時規(guī)劃器的設(shè)計和配置。針對在傳統(tǒng)A*算法計算下路徑冗余的問題［12］，根據(jù)全向移動平臺可橫移的特點，將該算法中的曼哈頓距離公式替換為歐幾里得公式，以加快路徑的生成。配置完相關(guān)功能包且成功定位加載地圖后，分別采用改進A*算法和動態(tài)窗口法［13］進行全局和局部路徑的規(guī)劃。

4 全向移動導(dǎo)航系統(tǒng)的性能測試

筆者搭建了一個完整的全向移動平臺用來測試全向移動導(dǎo)航系統(tǒng)的性能。其中，上位機采用一臺聯(lián)想Y480筆記本電腦，通過EtherCAT與4臺步科伺服電機通信，搭配上海思嵐科技有限公司生產(chǎn)的rplidar‐s1激光雷達進行建圖定位和導(dǎo)航。全向移動平臺如圖5所示。

圖5 全向移動平臺Fig.5 Omni‐directional mobile platform

4.1 電機同步性能的測試

電機間的同步性影響著全向移動平臺的建圖效果和導(dǎo)航精度：同步性越好，建圖效果越好，導(dǎo)航精度越高。試驗時，同時對4個電機設(shè)置相同的速度，在一定的時間內(nèi)讀取每個電機的真實角度值，以此判斷電機的同步性能。導(dǎo)航時，移動平臺不會一直維持在勻速狀態(tài)，因此進行了勻速狀態(tài)和變速狀態(tài)下電機同步性能的測試。根據(jù)室內(nèi)機器人的運動要求，設(shè)置移動平臺的速度極值為0.3 m/s，通過遙控手柄調(diào)控加速度。利用ROS中的ros_plot工具［14］，將電機輸出結(jié)果處理成直線圖，以直觀地反映電機間的同步誤差。勻速和變速狀態(tài)下電機同步性能的測試結(jié)果如圖6和圖7所示。

圖6 勻速狀態(tài)下電機同步性能測試結(jié)果Fig.6 Test results of motor synchronization performance un‐der constant speed

圖7 變速狀態(tài)下電機同步性能測試結(jié)果Fig.7 Test results of motor synchronization performance un‐der variable speed

伺服驅(qū)動器間采取線性串行方式相連，Ether‐CAT數(shù)據(jù)幀由1號電機依次傳遞至4號電機。由圖6可知，勻速狀態(tài)下兩電機之間輸出角度的誤差穩(wěn)定在0.05°左右。

由圖7（a）可知，在變速狀態(tài)下，4個電機的編碼器數(shù)值曲線幾乎重合。編碼器數(shù)值經(jīng)過處理后轉(zhuǎn)換成角度值輸出，由圖7（b）可知，在電機加上負載的情況下，兩電機之間輸出角度的最大誤差為0.13°，呈現(xiàn)較高的同步性，可滿足機器人的控制要求。

4.2 導(dǎo)航性能測試

本文構(gòu)建的全向移動導(dǎo)航系統(tǒng)用于全向移動平臺，實現(xiàn)其自主導(dǎo)航。為了測試其建圖定位和路徑規(guī)劃的效果，搭建試驗場地，進行全向移動平臺導(dǎo)航性能的測試。

使用rplidar‐s1激光雷達、Gmapping功能包［15］和輪式里程計構(gòu)建柵格地圖［16］，然后利用move_base功能包和可視化工具rviz進行導(dǎo)航。導(dǎo)航性能試驗如圖8所示。

圖8 全向移動平臺導(dǎo)航性能試驗Fig.8 Navigation performance test of omni‐directional mo‐bile platform

圖8（a）和圖8（b）為全向移動平臺的建圖位置；圖8（c）和圖8（d）為建立的柵格地圖，其中白色部分為可通行區(qū)域，黑色部分為障礙物［17］，前者示出了移動平臺的初始位置，目標點為G，后者示出了導(dǎo)航目標位置。試驗中，移動平臺經(jīng)過路徑上的A、B點時姿態(tài)變化明顯，到達目標位置后的目標姿態(tài)如圖8（c）箭頭方向所示。可見，全向移動平臺能夠進行全局路徑規(guī)劃和局部路徑規(guī)劃，導(dǎo)航過程中實現(xiàn)了動態(tài)避障，最終到達目標位置。

為檢驗EtherCAT通信良好的實時性和電機間的同步性，設(shè)置同樣的導(dǎo)航目標位置，進行多次試驗，記錄每次移動平臺實際到達位置與目標位置間的誤差，如表3所示。

為方便測量，先分別測出移動平臺在X向和Y向的導(dǎo)航誤差，再計算出最終的導(dǎo)航誤差值。由表3可知，導(dǎo)航誤差均值為5.44 cm。考慮到麥克納姆輪輥子的滑移［18］和移動平臺設(shè)計及安裝的誤差，可見全向移動平臺的導(dǎo)航精度較高，可滿足控制要求。

表3 基于EtherCAT的全向移動平臺的導(dǎo)航誤差Table 3 Navigation errorsofomni‐directionalmobile platform based on EtherCAT

4.3 導(dǎo)航性能對比

為檢驗筆者搭建的全向移動導(dǎo)航系統(tǒng)的性能優(yōu)勢，進行基于EtherCAT和基于RS‐232總線的全向移動平臺的自主導(dǎo)航對比試驗。使用相同的移動平臺和4.2節(jié)所述測試方法，基于RS‐232總線的全向移動平臺的導(dǎo)航誤差如表4所示。

表4 基于RS-232總線的全向移動平臺的導(dǎo)航誤差Table 4 Navigation errorsofomni‐directionalmobile platform based on RS‐232 bus

由表4可知，導(dǎo)航誤差均值為8.51 cm。可見，基于基于EtherCAT的全向移動導(dǎo)航系統(tǒng)具有更高的導(dǎo)航精度。

在系統(tǒng)硬件方面，由于RS‐232接口在總線上只允許連接1個從站［19］，需要額外的串口擴展卡才能滿足使用要求，而EtherCAT主站理論上可連接65 536個從站且不需要額外的擴展硬件。此外RS‐232接口的有效傳輸距離為15 m，遠短于EtherCAT總線的有效傳輸距離。

5 結(jié) 論

針對傳統(tǒng)機器人控制系統(tǒng)存在的實時性差、定位精度低的問題，筆者搭建了基于EtherCAT的ROS全向移動導(dǎo)航系統(tǒng)。其采用性能優(yōu)越的EtherCAT通信技術(shù)實現(xiàn)PC（personal computer，個人計算機）與伺服驅(qū)動器之間的通信，舍棄了傳統(tǒng)的運動控制器，精簡了控制系統(tǒng)，提高了電機間的同步性。ROS的使用有利于代碼的復(fù)用和移植，使整個控制系統(tǒng)具有開源性和可擴展性，且響應(yīng)迅速、成本低。試驗結(jié)果表明該導(dǎo)航系統(tǒng)可有效實現(xiàn)定位建圖和自主導(dǎo)航等功能，且具有較高的導(dǎo)航精度。