基于EtherCAT的ROS全向移動導航系統

張 洪，邱曉天

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

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

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

1 全向移動導航系統的硬件組成

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

2 全向移動導航系統的軟件框架設計

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

圖1 全向移動導航系統的軟件框架Fig.1 Software framework of omni‐directional mobile navigation system

3 全向移動導航系統的實現

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

3.1 底層控制設計

根據實際的控制要求，全向移動平臺的底層控制須實現如下2個功能：

1）采用EtherCAT通信技術實現伺服驅動器與上位機之間的通信；

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

3.1.1 EtherCAT通信的實現

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

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

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

表1 PDO配置Table 1 The configuration of PDO

3.1.2 IgH主站與ROS平臺的融合

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

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

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

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

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

經過測試發現，IgH主站任務周期過短容易引起系統抖動，引發伺服驅動器報警，任務周期過長則會增大系統延遲和響應時間，影響控制效果。因此，將IgH主站任務周期控制在100～200 μs。

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

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

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

3.3 URDF模型的建立

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

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

3.4 路徑規劃

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

4 全向移動導航系統的性能測試

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

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

4.1 電機同步性能的測試

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

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

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

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

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

4.2 導航性能測試

本文構建的全向移動導航系統用于全向移動平臺，實現其自主導航。為了測試其建圖定位和路徑規劃的效果，搭建試驗場地，進行全向移動平臺導航性能的測試。

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

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

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

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

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

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

4.3 導航性能對比

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

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

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

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

5 結 論

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