張 洪，邱曉天

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

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

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

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

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

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

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

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

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

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

3.1 底層控制設(shè)計(jì)

根據(jù)實(shí)際的控制要求，全向移動(dòng)平臺(tái)的底層控制須實(shí)現(xiàn)如下2個(gè)功能：

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

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

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

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

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

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

表1 PDO配置Table 1 The configuration of PDO

3.1.2 IgH主站與ROS平臺(tái)的融合

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

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

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

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

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

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

3.2 全向移動(dòng)平臺(tái)運(yùn)動(dòng)學(xué)模型和里程計(jì)模型的建立

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

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

3.3 URDF模型的建立

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

圖4 機(jī)器人連桿坐標(biāo)系的布局Fig.4 Coordinate system layout of robot connecting rods

3.4 路徑規(guī)劃

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

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

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

圖5 全向移動(dòng)平臺(tái)Fig.5 Omni‐directional mobile platform

4.1 電機(jī)同步性能的測(cè)試

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

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

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

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

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

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

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

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

圖8 全向移動(dòng)平臺(tái)導(dǎo)航性能試驗(yàn)Fig.8 Navigation performance test of omni‐directional mo‐bile platform

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

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

為方便測(cè)量，先分別測(cè)出移動(dòng)平臺(tái)在X向和Y向的導(dǎo)航誤差，再計(jì)算出最終的導(dǎo)航誤差值。由表3可知，導(dǎo)航誤差均值為5.44 cm。考慮到麥克納姆輪輥?zhàn)拥幕疲?8］和移動(dòng)平臺(tái)設(shè)計(jì)及安裝的誤差，可見全向移動(dòng)平臺(tái)的導(dǎo)航精度較高，可滿足控制要求。

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

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

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

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

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

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

5 結(jié) 論

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