基于ROS的6自由度機(jī)械臂運(yùn)動(dòng)軌跡規(guī)劃*

朱 萌，孟 婥，張 豪，孫以澤

(東華大學(xué)機(jī)械工程學(xué)院，上海 201620)

0 引言

隨著傳統(tǒng)制造方式不斷向智能制造轉(zhuǎn)型，機(jī)器人應(yīng)用領(lǐng)域不斷拓展，對(duì)機(jī)器人控制系統(tǒng)的要求越來(lái)越高[1]。然而，工業(yè)機(jī)器人開(kāi)發(fā)過(guò)程面臨許多問(wèn)題。機(jī)器人廠商大多開(kāi)發(fā)自身的軟件接口，可移植性差；復(fù)雜的建模及對(duì)大量冗余代碼的編寫(xiě)嚴(yán)重影響開(kāi)發(fā)者的工作效率[2]。開(kāi)源機(jī)器人操作系統(tǒng)ROS (Robot Operating System)具有跨平臺(tái)、多語(yǔ)言支持、代碼復(fù)用等優(yōu)點(diǎn)[3]，能很好地解決這些問(wèn)題。

本文創(chuàng)建工業(yè)機(jī)器人實(shí)體模型并轉(zhuǎn)換為ROS中所使用的URDF格式文件，利用Moveit!提供的C++、Python接口實(shí)現(xiàn)了對(duì)笛卡爾空間連續(xù)軌跡直線與圓弧插補(bǔ)，并在Rviz中進(jìn)行了仿真。在平面、空間進(jìn)行路徑規(guī)劃，驗(yàn)證了RRTConnect算法具有更高的搜索速度，完成了復(fù)雜環(huán)境下無(wú)碰撞路徑規(guī)劃，對(duì)工業(yè)機(jī)器人在實(shí)際工作中保障運(yùn)動(dòng)安全具有一定意義。

1 ROS仿真平臺(tái)搭建

ROS是Willow Garage公司2010年發(fā)布的開(kāi)源機(jī)器人操作系統(tǒng)，它采用分布式架構(gòu)，通過(guò)各功能獨(dú)立的節(jié)點(diǎn)(Node)傳遞消息實(shí)現(xiàn)任務(wù)的分層次運(yùn)行，具有支持多語(yǔ)言、免費(fèi)且開(kāi)源、代碼易拓展等優(yōu)點(diǎn)[3]。基于此，本文在Ubuntu18.04上利用ROS-Melodic搭建6自由度機(jī)械臂運(yùn)動(dòng)規(guī)劃仿真平臺(tái)。

1.1 URDF文件獲取

URDF (universal robotic description format) 是統(tǒng)一描述機(jī)器人仿真模型設(shè)定的XML語(yǔ)言[4]，其主要定義了link和joint，包括限位信息limits，碰撞信息collision，慣性信息inertial，可視化信息visual及l(fā)ink間的父子從屬關(guān)系信息。

本文研究對(duì)象為新松6自由度工業(yè)機(jī)器人如圖1所示，在SolidWorks建立機(jī)器人模型，其中每個(gè)連桿設(shè)定相應(yīng)的coordinate參考坐標(biāo)系和axis旋轉(zhuǎn)軸，通過(guò)ROS官方的SW2URDF插件經(jīng)順序定義各關(guān)節(jié)、父子連桿、限位參數(shù)、參考坐標(biāo)系及旋轉(zhuǎn)軸后生成URDF文件。

圖1 新松工業(yè)機(jī)器人

1.2 Moveit!運(yùn)動(dòng)配置

ROS機(jī)械臂控制系統(tǒng)主要依賴(lài)Moveit!架構(gòu)及其插件，Moveit!集成了運(yùn)動(dòng)學(xué)求解、路徑規(guī)劃、碰撞檢測(cè)三大核心功能，其系統(tǒng)結(jié)構(gòu)如圖2所示。通過(guò)運(yùn)動(dòng)學(xué)庫(kù)選擇KDL、TRAC-IK、IK-FAST求解器完成正逆運(yùn)動(dòng)學(xué)的計(jì)算；開(kāi)放運(yùn)動(dòng)規(guī)劃庫(kù)OMPL通過(guò)隨機(jī)采樣，使機(jī)械臂能夠避開(kāi)障礙物，避免自身干涉完成路徑規(guī)劃。

圖2 Moveit!控制系統(tǒng)架構(gòu)

參數(shù)服務(wù)器加載urdf、config和srdf機(jī)器人配置信息，控制系統(tǒng)核心Move Group類(lèi)向機(jī)器人控制器接口傳遞Joint Trajectory Action關(guān)節(jié)運(yùn)動(dòng)數(shù)據(jù)信息，同時(shí)傳感器不斷反饋JointState關(guān)節(jié)信息，以便Move Group調(diào)用運(yùn)動(dòng)規(guī)劃算法求正逆運(yùn)動(dòng)學(xué)解和生成運(yùn)動(dòng)軌跡。

為幫助開(kāi)發(fā)者快速配置Moveit!，ROS提供了具有用戶(hù)界面的設(shè)置助手Moveit!SetupAssistant[5]，它可以幫助開(kāi)發(fā)者定制機(jī)器人運(yùn)動(dòng)規(guī)劃框架的初始配置，簡(jiǎn)化了設(shè)計(jì)編譯工作。這些配置包括自碰撞矩陣、虛擬關(guān)節(jié)列表、規(guī)劃組定義、機(jī)械臂位姿、末端執(zhí)行器及被動(dòng)關(guān)節(jié)列表[4]。依次完成上述配置，其流程如圖3所示。

圖3 配置流程

2 機(jī)械臂路徑規(guī)劃

多自由度機(jī)械臂末端到達(dá)空間一點(diǎn)時(shí)，各關(guān)節(jié)存在多解，為避免在運(yùn)動(dòng)過(guò)程中與環(huán)境發(fā)生碰撞，需對(duì)其進(jìn)行路徑規(guī)劃?；诳焖贁U(kuò)展隨機(jī)樹(shù)(RRT / rapidly exploring random tree)的路徑規(guī)劃算法[6]，通過(guò)對(duì)狀態(tài)空間隨機(jī)采樣，能在復(fù)雜約束下快速搜索高維空間，把搜索導(dǎo)向空白區(qū)域，從而尋找到從起始點(diǎn)到目標(biāo)點(diǎn)的規(guī)劃路徑。

2.1 RRT與RRTConnect算法原理

RRT算法從初始位置開(kāi)始，以樹(shù)的形式進(jìn)行探索[7]如圖4所示，通過(guò)在空間中隨機(jī)采樣對(duì)路徑進(jìn)行擴(kuò)展，直到“樹(shù)枝”覆蓋目標(biāo)區(qū)域，在探索出的狀態(tài)空間中選取最短路徑作為結(jié)果輸出。規(guī)劃流程為：①將起始點(diǎn)作為隨機(jī)樹(shù)的根節(jié)點(diǎn)qinit，并初始化根節(jié)點(diǎn)qinit；②在狀態(tài)空間隨機(jī)采樣一點(diǎn)qrandom；③在已生成隨機(jī)樹(shù)各節(jié)點(diǎn)中尋找距離qrandom最近的點(diǎn)qnearest；④qnearest向qrandom方向擴(kuò)展一個(gè)搜索步長(zhǎng)εq，生長(zhǎng)一個(gè)新節(jié)點(diǎn)qnew；⑤判斷新節(jié)點(diǎn)qnew是否與障礙物發(fā)生碰撞，若發(fā)生碰撞則放棄本次生長(zhǎng)，反之將qnew添加到隨機(jī)樹(shù)中，重復(fù)以上步驟直至搜索到目標(biāo)位置qgoal。

圖4 RRT原理圖

RRT每次搜索只從初始點(diǎn)生長(zhǎng)快速擴(kuò)展隨機(jī)樹(shù)來(lái)搜索狀態(tài)空間，與RRT相比基于雙向擴(kuò)展平衡的連結(jié)型雙樹(shù)RRTConnect算法[8]在目標(biāo)點(diǎn)區(qū)域建立第二棵樹(shù)與第一棵樹(shù)同時(shí)進(jìn)行擴(kuò)展如圖4所示，直到擴(kuò)展失敗或與第一棵樹(shù)相連，整個(gè)算法結(jié)束。

2.2 算法測(cè)試

在平面路徑規(guī)劃測(cè)試中，本文利用MATLAB編程，輸入像素尺寸為500×500的地圖，定義起始點(diǎn)source=[10,10]，目標(biāo)點(diǎn)goal=[490,490]，迭代步長(zhǎng)Stepsize為20，最大迭代次數(shù)MaxAttempts為10 000次，使用RRT與RRTConnect算法搜索出無(wú)碰撞路徑，測(cè)試結(jié)果如表1所示。

表1 測(cè)試結(jié)果對(duì)比

在空間路徑規(guī)劃測(cè)試中，為提高路徑規(guī)劃效率，本文在ROS中的OMPL運(yùn)動(dòng)規(guī)劃庫(kù)，選用RRTConnect算法替代RRT算法進(jìn)行路徑規(guī)劃，在Rviz中添加MotionPlanning插件，通過(guò)導(dǎo)入Blender生成的.dae文件添加障礙物，同時(shí)設(shè)定機(jī)器人運(yùn)動(dòng)起始點(diǎn)home與目標(biāo)點(diǎn)goal，運(yùn)行C++源文件執(zhí)行機(jī)器人無(wú)碰撞路徑規(guī)劃如圖5所示，RRTConnect算法與RRT算法規(guī)劃分別耗時(shí)0.083 s和0.035 s。

(a) 起始點(diǎn)位置 (b) 目標(biāo)點(diǎn)位置 (c) 路徑規(guī)劃過(guò)程圖5 無(wú)碰撞路徑規(guī)劃

平面與空間路徑規(guī)劃的仿真結(jié)果表明，與RRT相比RRTConnect通過(guò)朝向?qū)Ψ娇焖贁U(kuò)展，這種帶有啟發(fā)性的擴(kuò)展使樹(shù)的擴(kuò)展更加明確，能顯著提高搜索速度、搜索效率，有利于機(jī)械臂在多約束復(fù)雜環(huán)境下盡快完成路徑規(guī)劃，實(shí)現(xiàn)避障。

3 笛卡爾空間機(jī)械臂軌跡規(guī)劃

機(jī)械臂在進(jìn)行連續(xù)軌跡作業(yè)時(shí)，要保證末端軌跡的準(zhǔn)確定義[9]，使末端執(zhí)行器避開(kāi)障礙物，必須在笛卡爾空間進(jìn)行軌跡規(guī)劃，并把規(guī)劃的路徑劃分為有限個(gè)過(guò)渡點(diǎn)，通過(guò)逆運(yùn)動(dòng)學(xué)求解，實(shí)時(shí)轉(zhuǎn)換為每個(gè)關(guān)節(jié)的角度值。本文主要研究新松SR7CL工業(yè)機(jī)器人在笛卡爾空間中的直線軌跡規(guī)劃和圓弧軌跡規(guī)劃[10]。

3.1 空間直線軌跡規(guī)劃

(1)

在ROS中利用Moveit!提供的Python接口，完成軌跡點(diǎn)之間的直線插補(bǔ)，如圖6所示。

圖6 笛卡爾空間直線規(guī)劃

3.2 空間圓弧軌跡規(guī)劃

設(shè)笛卡爾空間不共線的3點(diǎn)為P1(x1,y1,z1)、P2(x2,y2,z2)、P3(x3,y3,z3)，機(jī)械臂末端以圓弧從P1經(jīng)P2運(yùn)動(dòng)至P3，這三點(diǎn)所確定的圓弧就是機(jī)械臂末端的運(yùn)動(dòng)軌跡。

在笛卡爾空間圓弧軌跡規(guī)劃中，為計(jì)算簡(jiǎn)便，在圓弧所在平面新建一個(gè)以圓弧中心為原點(diǎn)的坐標(biāo)系，把圓弧上任意兩個(gè)始末點(diǎn)弧長(zhǎng)計(jì)算轉(zhuǎn)化為兩點(diǎn)對(duì)應(yīng)圓心角增量的計(jì)算，計(jì)算出圓弧的各插補(bǔ)點(diǎn)在新坐標(biāo)系中的值，通過(guò)運(yùn)動(dòng)學(xué)關(guān)系將這些值映射到參考坐標(biāo)系中。

(1)確定圓心P0(x0,y0,z0)及半徑r

由三點(diǎn)確定過(guò)圓弧的平面方程為：

(2)

易得過(guò)P1P2中點(diǎn)且與P1P2垂直的平面方程和過(guò)P2P3中點(diǎn)且與P2P3垂直的平面方程，與式(2)聯(lián)立，可求圓心P0(x0,y0,z0)以及半徑r。

(2)以P0為原點(diǎn)建立新坐標(biāo)系B并進(jìn)行坐標(biāo)變換:

圖7 笛卡爾空間坐標(biāo)變換示意圖

圖7為笛卡爾空間坐標(biāo)變換示意圖，U軸位于P0P1連線上，其單位向量為:

(3)

W軸方向與圓弧所在平面垂直，其單位向量為：

(4)

V軸由右手定則通過(guò)U、W軸確定，其單位向量為：

v=u×w

(5)

由A坐標(biāo)系到B坐標(biāo)系的位姿變換矩陣為：

(6)

設(shè)笛卡爾空間圓弧上一點(diǎn)為AP=(x,y,z)，∠PP0P1=θ，BP=(rcosθ,rsinθ,0)，則笛卡爾空間圓弧軌跡可通過(guò)坐標(biāo)變換關(guān)系轉(zhuǎn)化成平面圓弧，即:

(7)

笛卡爾空間圓弧規(guī)劃把對(duì)圓弧長(zhǎng)度的插補(bǔ)轉(zhuǎn)換為對(duì)圓弧所對(duì)應(yīng)圓心角θ的插補(bǔ)，生成B坐標(biāo)系下圓弧上一系列點(diǎn)由式(7)映射為基坐標(biāo)系下一系列路徑點(diǎn)，在ROS中利用Moveit!提供的Python接口編程，實(shí)現(xiàn)了笛卡爾空間圓弧規(guī)劃如圖8所示。

圖8 笛卡爾空間圓弧規(guī)劃

4 結(jié)論

本文基于ROS搭建了機(jī)械臂仿真平臺(tái)，創(chuàng)建URDF文件完成了6自由度機(jī)械臂的建模。為保證機(jī)械臂實(shí)際作業(yè)中避免與環(huán)境碰撞，在平面、空間分別進(jìn)行算法測(cè)試，驗(yàn)證了RRTConnect算法具有更高的搜索速度，完成了復(fù)雜環(huán)境下無(wú)碰撞路徑規(guī)劃，保證了機(jī)械臂運(yùn)動(dòng)安全。利用Moveit!進(jìn)行笛卡爾空間下直線與圓弧軌跡規(guī)劃，實(shí)現(xiàn)了插補(bǔ)運(yùn)算，保證了對(duì)連續(xù)軌跡的精確定義。