張 穎， 平雪良， 王晨學， 仇恒坦

(江南大學 機械工程學院 江蘇省食品先進制造裝備技術重點實驗室，江蘇 無錫 214122)

0 引 言

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

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

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

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

2 控制系統(tǒng)軟件設計

2.1 控制軟件總體設計

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

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

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

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

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

圖2 底層模塊

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

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

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

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

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

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

表1 配置PDO

2.2.2 機器人關節(jié)與執(zhí)行機構映射

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

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

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

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

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

2.3 Moveit!實現(xiàn)

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

2.3.1 機器人運動學模型建立

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

在SolidWorks 中構建的機器人三維模型導出為STL 格式，將機器人連桿參數(shù)寫入URDF 文件中?；赨RDF中的機器人模型屬性及當前關節(jié)數(shù)值，ROS 將自動生成各關節(jié)相連的兩個連桿坐標系間的轉換關系，坐標系間的轉換關系可通過tf節(jié)點進行廣播。機器人連桿坐標系在RVIZ下的顯示效果如圖4所示。

圖4 機器人運動學模型

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

2.3.2 moveit!配置

表2 連桿坐標系轉換參數(shù)

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

2.3.3 運動規(guī)劃

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

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

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

3 實 驗

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

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

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

圖6 機器人運行狀態(tài)

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

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

4 結 論

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

[1] 余秋蕾,張崇峰,陳 萌,等. 基于EtherCAT總線的人形機器人控制系統(tǒng)設計[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主站實現(xiàn)[D].廣州：華南理工大學,2013.

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

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

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

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

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

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

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