ROS下基于EtherCAT的串聯(lián)機(jī)器人控制系統(tǒng)*

張 穎， 平雪良， 王晨學(xué)， 仇恒坦

(江南大學(xué) 機(jī)械工程學(xué)院 江蘇省食品先進(jìn)制造裝備技術(shù)重點(diǎn)實(shí)驗(yàn)室，江蘇 無錫 214122)

0 引 言

開源機(jī)器人操作系統(tǒng) ( robot operating system，ROS)[1]通過提供統(tǒng)一的軟件平臺，使用統(tǒng)一的方式來封裝機(jī)器人，使得代碼重用和模塊化設(shè)計(jì)變得更加簡便，可方便應(yīng)用在不同的機(jī)器人上，顯著提高了機(jī)器人控制系統(tǒng)應(yīng)用開發(fā)速度。基于以太網(wǎng)以其數(shù)據(jù)傳送能力強(qiáng)，技術(shù)成熟度高，開發(fā)成本低等優(yōu)點(diǎn)，受到工業(yè)控制現(xiàn)場總線領(lǐng)域的廣泛關(guān)注。在機(jī)器人領(lǐng)域中，EtherCAT[1]網(wǎng)絡(luò)已成為全球公認(rèn)的標(biāo)準(zhǔn)。

針對目前傳統(tǒng)機(jī)器人控制系統(tǒng)適用面窄，代碼復(fù)用率低、擴(kuò)展性差以及移植困難的問題，本文基于ROS并利用EtherCAT通信技術(shù)，設(shè)計(jì)了一種低成本、可移植、可擴(kuò)展、適用面廣的開源的工業(yè)串聯(lián)機(jī)械人控制系統(tǒng)。

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

控制系統(tǒng)硬件既要保證快速性、高集成性，又要保證兼容性、開放性。采用支持EtherCAT通信的伺服驅(qū)動器，工業(yè)機(jī)器人為典型的六軸串聯(lián)機(jī)器人，機(jī)械臂末端帶有法蘭，方便安裝不同類型的末端執(zhí)行器。硬件結(jié)構(gòu)摒棄了傳統(tǒng)的運(yùn)動控制器，直接將上位機(jī)與執(zhí)行機(jī)構(gòu)中的伺服驅(qū)動器以EtherCAT總線拓?fù)浣Y(jié)構(gòu)連接。

2 控制系統(tǒng)軟件設(shè)計(jì)

2.1 控制軟件總體設(shè)計(jì)

控制軟件框架基于ROS平臺提供的ROS_control[2]實(shí)現(xiàn)。上層使用moveit![3]實(shí)現(xiàn)運(yùn)動規(guī)劃。底層使用ROS提供的ros_control框架實(shí)現(xiàn)，并使用EtherCAT技術(shù)實(shí)現(xiàn)通信。控制系統(tǒng)整體框架如圖1所示。

用戶可通過moveit!提供的Rviz人機(jī)接口或者基于moveit!開發(fā)的擴(kuò)展功能發(fā)出運(yùn)動指令，controller_manager根據(jù)用戶的需求啟動相應(yīng)的controller，controller實(shí)現(xiàn)相應(yīng)的運(yùn)動控制算法并將計(jì)算結(jié)果通過由hardware_interface提供的接口傳遞到底層，實(shí)現(xiàn)對機(jī)器人的運(yùn)動控制。

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

2.2 底層控制實(shí)現(xiàn)

根據(jù)控制系統(tǒng)的需求，底層控制模塊需要實(shí)現(xiàn)如下功能：1)使用EtherCAT通信技術(shù)實(shí)現(xiàn)控制系統(tǒng)與機(jī)器人伺服驅(qū)動通信；2)實(shí)現(xiàn)機(jī)器人關(guān)節(jié)角度值到目標(biāo)位置的映射；3)為controller提供硬件層接口。加載運(yùn)動控制模塊后運(yùn)行的節(jié)點(diǎn)如圖2所示。

圖2 底層模塊

pos_based_pos_traj_controller為底層運(yùn)動控制模塊，對上層運(yùn)動規(guī)劃結(jié)果進(jìn)行插值計(jì)算；follow_joint_trajectory/actions:提供moveit!輸出接口，傳遞使用moveit!接口運(yùn)動規(guī)劃的結(jié)果；position_command：負(fù)責(zé)接收位置控制的命令；/joint_states:讀取機(jī)器人當(dāng)前位置并發(fā)布在此主題上；/tf:實(shí)時輸出各連桿坐標(biāo)系的位姿信息；robot_HI為實(shí)現(xiàn)底層控功能的節(jié)點(diǎn)。

2.2.1 EtherCAT通信技術(shù)實(shí)現(xiàn)

主站控制所有從站數(shù)據(jù)的接收和發(fā)送，在一個通信周期中，主站發(fā)送以太網(wǎng)數(shù)據(jù)幀下行報(bào)文，從站在數(shù)據(jù)幀經(jīng)過節(jié)點(diǎn)時對相關(guān)數(shù)據(jù)進(jìn)行讀取或插入操作，全部從站處理完數(shù)據(jù)幀后，由系統(tǒng)末端從站將數(shù)據(jù)幀向前順序返回，并由首端從站作為響應(yīng)報(bào)文將其發(fā)送給主站。

目前IgH EtherCAT Master[4]對Linux環(huán)境下EtherCAT主站的實(shí)現(xiàn)功能比較完善，并提供相應(yīng)網(wǎng)卡的實(shí)時驅(qū)動。本文通信主站基于IgH實(shí)現(xiàn)ROS系統(tǒng)下主從站通信。主從站通信過程如圖3所示。

圖3 EtherCAT主站實(shí)現(xiàn)過程

EtherCAT在應(yīng)用層支持CANopen(CANopen over EtherCAT)[5]協(xié)議。通過COE實(shí)現(xiàn)周期性任務(wù)[6]。COE完全遵從CANopen的應(yīng)用層行規(guī)，伺服和運(yùn)動控制的應(yīng)用層行規(guī)為CiA402。本文伺服交流電機(jī)的控制模式為周期性同步位置模式。本文實(shí)現(xiàn)周期性任務(wù)所需配置的PDO以及對應(yīng)的對象字典如表1中所示。

表1 配置PDO

2.2.2 機(jī)器人關(guān)節(jié)與執(zhí)行機(jī)構(gòu)映射

控制系統(tǒng)控制機(jī)器人運(yùn)動最終以關(guān)節(jié)角度的形式進(jìn)行輸出，而機(jī)器人關(guān)節(jié)角度需轉(zhuǎn)換為目標(biāo)位置值發(fā)給相應(yīng)的從站實(shí)現(xiàn)運(yùn)動。同理，控制系統(tǒng)需獲取實(shí)物機(jī)器人當(dāng)前實(shí)際位置，驅(qū)動器中各個電機(jī)當(dāng)前位置需要轉(zhuǎn)換成機(jī)器人的關(guān)節(jié)角度值，反饋給控制系統(tǒng)。目標(biāo)位置值與機(jī)器人關(guān)節(jié)角度之間的關(guān)系為

式中a為發(fā)送到從站的目標(biāo)位置值；J為電機(jī)與執(zhí)行機(jī)構(gòu)之間的減速比；q為機(jī)器人關(guān)節(jié)需要轉(zhuǎn)過的角度；2n為電機(jī)旋轉(zhuǎn)一周實(shí)際位置讀數(shù)的增量;a0為機(jī)器人關(guān)節(jié)于運(yùn)動學(xué)零點(diǎn)時的實(shí)際位置值。同理，從驅(qū)動器中讀取的實(shí)際位置值可由此式反解出機(jī)器人當(dāng)前各關(guān)節(jié)角度值。

2.2.3 系統(tǒng)控制接口

控制模塊在硬件層接口通過hardware_interface與真實(shí)機(jī)器人進(jìn)行交互，運(yùn)動控制模塊由控制模塊管理器調(diào)度，載入和切換由不同算法實(shí)現(xiàn)的各個運(yùn)動控制算法模塊。控制模塊在管理器中的運(yùn)行根據(jù)時間和運(yùn)行狀態(tài)的不同可分為4個過程：init()，starting()，update()，stopping()。

本文底層運(yùn)動控制模塊根據(jù)上層運(yùn)動規(guī)劃的結(jié)果是否對加速度限制進(jìn)行3次或者5次樣條插值[7]，也可根據(jù)用戶需求實(shí)現(xiàn)B樣條曲線等插值算法[8]。最終將運(yùn)動指令解釋為系列機(jī)器人關(guān)節(jié)角度值與時間的函數(shù)。

2.3 Moveit!實(shí)現(xiàn)

上層的運(yùn)動規(guī)劃通過使用moveit!接口實(shí)現(xiàn)，moveit!是ROS系統(tǒng)下為機(jī)械臂運(yùn)動控制設(shè)計(jì)的功能包與工具的集合，通過使用moveit!可以快速實(shí)現(xiàn)機(jī)械臂的運(yùn)動學(xué)正逆解、運(yùn)動路徑規(guī)劃、碰撞檢測等功能，moveit!同時提供了RVIZ顯示界面，可以方便的查看機(jī)器人運(yùn)動規(guī)劃的路徑。

2.3.1 機(jī)器人運(yùn)動學(xué)模型建立

機(jī)器人數(shù)學(xué)模型的建立是對機(jī)器人進(jìn)行設(shè)計(jì)與控制的前提。ROS使用統(tǒng)一機(jī)器人描述格式 ( unified robot description format，URDF)[9]。URDF 基于XML語言，文件中將機(jī)器人抽象為通過各個關(guān)節(jié)連接而成的連桿結(jié)構(gòu)。URDF可以通過ROS可視化工具RVIZ顯示。

在SolidWorks 中構(gòu)建的機(jī)器人三維模型導(dǎo)出為STL 格式，將機(jī)器人連桿參數(shù)寫入U(xiǎn)RDF 文件中。基于URDF中的機(jī)器人模型屬性及當(dāng)前關(guān)節(jié)數(shù)值，ROS 將自動生成各關(guān)節(jié)相連的兩個連桿坐標(biāo)系間的轉(zhuǎn)換關(guān)系，坐標(biāo)系間的轉(zhuǎn)換關(guān)系可通過tf節(jié)點(diǎn)進(jìn)行廣播。機(jī)器人連桿坐標(biāo)系在RVIZ下的顯示效果如圖4所示。

圖4 機(jī)器人運(yùn)動學(xué)模型

表2中為URDF中表示機(jī)器人坐標(biāo)系之間的轉(zhuǎn)換關(guān)系的方式，由位置和姿態(tài)來表示，姿態(tài)使用繞自身旋轉(zhuǎn)的raw，pitch，yaw表示，最后3個參數(shù)表示旋轉(zhuǎn)軸和轉(zhuǎn)方向。

2.3.2 moveit!配置

表2 連桿坐標(biāo)系轉(zhuǎn)換參數(shù)

moveit!借助moveit_setup_assistant基于機(jī)器人URDF文件通過進(jìn)行初步配置，此外需對controller進(jìn)行配置，使用底層驅(qū)動提供的controller，moveit!借助moveit_simple_controller_manager插件實(shí)現(xiàn)與controller的連接。運(yùn)動規(guī)劃結(jié)果通過action傳遞到底層，在相應(yīng)controller進(jìn)行插值計(jì)算后將角度值轉(zhuǎn)換成目標(biāo)位置值傳遞到驅(qū)動器，從而實(shí)現(xiàn)控制執(zhí)行元件運(yùn)動到用戶定義的位置處。其中機(jī)器人正逆解通過OROCOS提供的KDL插件進(jìn)行求解。

2.3.3 運(yùn)動規(guī)劃

運(yùn)動規(guī)劃算法通常有兩個評價(jià)指標(biāo)；完備性和最優(yōu)性。完備性指在有限時間內(nèi)能解決所有有解問題，最優(yōu)性旨在找到最優(yōu)路徑。開源運(yùn)動規(guī)劃庫(open motion planning library,OMPL)運(yùn)動算法庫即可基于隨機(jī)采樣的運(yùn)動規(guī)劃算法庫，旨在最有時間內(nèi)找到有效路徑。moveit!集成了OMPL，其包含諸多先進(jìn)的算法庫，其中快速擴(kuò)張隨機(jī)樹(rapidly exploring random tree,RRT)[10]算法是一個概率完備的路徑規(guī)劃方法。在機(jī)器人自由工作空間有足夠多的采樣點(diǎn)且存在有效路徑，RRT算法能夠搜索到滿足要求的路徑。此外，基于隨機(jī)采樣的運(yùn)動規(guī)劃算法效率高，能滿足實(shí)時規(guī)劃的要求。本文的運(yùn)動規(guī)劃主要使用了RRT算法以及基于RRT擴(kuò)展的RRTconnect[11]算法。

高維空間內(nèi)的路徑搜索相比于低維空間更為復(fù)雜，RRT算法基本思想是在搜索空間中從機(jī)器人初始點(diǎn)開始樹狀搜索，在可行空間中隨機(jī)采樣并拓展枝葉，搜索樹覆蓋目標(biāo)點(diǎn)區(qū)域后即可停止生長。將有k個節(jié)點(diǎn)的搜索隨機(jī)樹稱為Tk，xinit為初始狀態(tài)，xgoal，為目標(biāo)狀態(tài)，xrand為在構(gòu)型空間中隨機(jī)選取的狀態(tài)點(diǎn)，通過不斷產(chǎn)生隨機(jī)點(diǎn)并擴(kuò)展RRT搜索樹枝葉的方法直到搜索到目標(biāo)位置。遍歷隨機(jī)樹Tk找到Tk上離xrand距離最近的葉子節(jié)點(diǎn)xnear，dist(xnear，xrand)代表構(gòu)型空間中兩個節(jié)點(diǎn)之間的尺度函數(shù)，表示兩個節(jié)點(diǎn)之間的距離。如果 dist(xnear,xrand)

為加快收斂速度，RRTconnect算法基于RRT進(jìn)行了改進(jìn)，規(guī)劃時首先初始化兩棵樹，兩棵樹在構(gòu)型空間向著對方交替擴(kuò)展，反復(fù)迭代，直到兩棵樹相遇，則成功找到一條從始態(tài)到終態(tài)的路徑。

3 實(shí) 驗(yàn)

啟動底層驅(qū)動節(jié)點(diǎn)和配置后的moveit!文件，通過ROS提供的ros_graph查看當(dāng)前運(yùn)行的節(jié)點(diǎn)的信息，節(jié)點(diǎn)圖直觀顯示出當(dāng)前運(yùn)行的節(jié)點(diǎn)，以及節(jié)點(diǎn)訂閱和發(fā)布的主題，如圖5所示。

圖5 控制系統(tǒng)節(jié)點(diǎn)

實(shí)驗(yàn)主要通過moveit!提供的move_group的接口實(shí)現(xiàn)，定義兩個點(diǎn)，機(jī)器人首先沿直線從目標(biāo)起始點(diǎn)運(yùn)動到目標(biāo)終點(diǎn)，再從目標(biāo)終點(diǎn)在關(guān)節(jié)空間下運(yùn)動到目標(biāo)起始點(diǎn)。機(jī)器人運(yùn)行實(shí)物以及仿真環(huán)境中軌跡如圖6所示。

圖6 機(jī)器人運(yùn)行狀態(tài)

通過RVIZ提供的rqt_plot查看機(jī)器人運(yùn)行狀態(tài)，如圖7(a)為某一時刻監(jiān)控機(jī)器人各關(guān)節(jié)的位置與時間曲線，圖7(b)為機(jī)器人各關(guān)節(jié)速度與實(shí)踐的曲線。rqt_plot能夠顯示機(jī)器人運(yùn)行狀態(tài)的大致曲線，當(dāng)數(shù)據(jù)比較多的時，需通過rosbag將數(shù)據(jù)記錄，可在MATLAB中導(dǎo)入進(jìn)行數(shù)據(jù)分析處理。通過監(jiān)控界面可看出機(jī)器人在運(yùn)動過程中，位置和速度連續(xù)平滑，機(jī)器人能夠完成上層規(guī)劃的運(yùn)動，該控制系統(tǒng)能夠完成運(yùn)動控制需求。

圖7 rqt_plot監(jiān)控機(jī)器人狀態(tài)

4 結(jié) 論

實(shí)驗(yàn)結(jié)果表明:基于ROS搭建的控制系統(tǒng)能夠滿足工業(yè)機(jī)器人控制需求。基于ROS 的工業(yè)六自由度串聯(lián)機(jī)器人控制系統(tǒng)摒棄了傳統(tǒng)的運(yùn)動控制器，使用先進(jìn)的現(xiàn)場總線EtherCAT技術(shù)，實(shí)現(xiàn)了ROS系統(tǒng)下對六自由度串聯(lián)工業(yè)機(jī)器人有效控制。該控制系統(tǒng)具有開源、低成本、高性能、可擴(kuò)展等特點(diǎn)，滿足工業(yè)機(jī)器人基本控制需求。由于ros_control框架實(shí)現(xiàn)不具備硬實(shí)時，后續(xù)研究可以基于硬實(shí)時系統(tǒng)進(jìn)行框架搭建，由底層運(yùn)送控制硬實(shí)時，實(shí)現(xiàn)對機(jī)器人更高的控制要求。

[1] 余秋蕾,張崇峰,陳 萌,等. 基于EtherCAT總線的人形機(jī)器人控制系統(tǒng)設(shè)計(jì)[J].載人航天,2016(1):16-22.

[2] Edwards S,Lewis C.ROS-industrial:Applying the robot operating system(ROS)to industrial applications[C]∥IEEE Int’l Confe-rence on Robotics and Automation,ECHORD Workshop,2012.

[3] Chitta S,ucan I A S,Cousins S.MoveIt![ROS Topics][J].IEEE Robotics and Automation Magazine,2012,19(1):18-19.

[4] 張少勛.基于嵌入式數(shù)控平臺的EtherCAT主站實(shí)現(xiàn)[D].廣州：華南理工大學(xué),2013.

[5] 夏 勇,葉曉東,趙江海.基于CANopen協(xié)議的數(shù)字伺服控制[J].電氣自動化,2014(1):57-59，81.

[6] 劉 鑫,閔華松,陳友東,等.基于EtherCAT的工業(yè)機(jī)器人控制器設(shè)計(jì)[J].計(jì)算機(jī)工程,2012(11):290-293.

[7] 劉汝佳,王 芳,張 強(qiáng),等.基于ROS的機(jī)械臂軌跡規(guī)劃研究[J].導(dǎo)航定位與授時,2016(6):82-88.

[8] 王 憲,王 偉,張方生,等.基于嵌入式系統(tǒng)的機(jī)器人關(guān)節(jié)空間軌跡規(guī)劃[J].傳感器與微系統(tǒng),2011，30 (10):19-21，25.

[9] 曹正萬,平雪良,陳盛龍,等.基于ROS的機(jī)器人模型構(gòu)建方法研究[J].組合機(jī)床與自動化加工技術(shù),2015(8):51-54.

[10] 錢 偉.基于ROS的移動操作機(jī)械臂底層規(guī)劃及運(yùn)動仿真[D].哈爾濱：哈爾濱工業(yè)大學(xué),2015.

[11] 劉園園.基于RRTConnect算法的雙履帶起重機(jī)路徑規(guī)劃研究[D].大連：大連理工大學(xué),2012.