基于ROS的6自由度機械臂運動軌跡規劃*

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

(東華大學機械工程學院，上海 201620)

0 引言

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

本文創建工業機器人實體模型并轉換為ROS中所使用的URDF格式文件，利用Moveit!提供的C++、Python接口實現了對笛卡爾空間連續軌跡直線與圓弧插補，并在Rviz中進行了仿真。在平面、空間進行路徑規劃，驗證了RRTConnect算法具有更高的搜索速度，完成了復雜環境下無碰撞路徑規劃，對工業機器人在實際工作中保障運動安全具有一定意義。

1 ROS仿真平臺搭建

ROS是Willow Garage公司2010年發布的開源機器人操作系統，它采用分布式架構，通過各功能獨立的節點(Node)傳遞消息實現任務的分層次運行，具有支持多語言、免費且開源、代碼易拓展等優點[3]。基于此，本文在Ubuntu18.04上利用ROS-Melodic搭建6自由度機械臂運動規劃仿真平臺。

1.1 URDF文件獲取

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

本文研究對象為新松6自由度工業機器人如圖1所示，在SolidWorks建立機器人模型，其中每個連桿設定相應的coordinate參考坐標系和axis旋轉軸，通過ROS官方的SW2URDF插件經順序定義各關節、父子連桿、限位參數、參考坐標系及旋轉軸后生成URDF文件。

圖1 新松工業機器人

1.2 Moveit!運動配置

ROS機械臂控制系統主要依賴Moveit!架構及其插件，Moveit!集成了運動學求解、路徑規劃、碰撞檢測三大核心功能，其系統結構如圖2所示。通過運動學庫選擇KDL、TRAC-IK、IK-FAST求解器完成正逆運動學的計算；開放運動規劃庫OMPL通過隨機采樣，使機械臂能夠避開障礙物，避免自身干涉完成路徑規劃。

圖2 Moveit!控制系統架構

參數服務器加載urdf、config和srdf機器人配置信息，控制系統核心Move Group類向機器人控制器接口傳遞Joint Trajectory Action關節運動數據信息，同時傳感器不斷反饋JointState關節信息，以便Move Group調用運動規劃算法求正逆運動學解和生成運動軌跡。

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

圖3 配置流程

2 機械臂路徑規劃

多自由度機械臂末端到達空間一點時，各關節存在多解，為避免在運動過程中與環境發生碰撞，需對其進行路徑規劃。基于快速擴展隨機樹(RRT / rapidly exploring random tree)的路徑規劃算法[6]，通過對狀態空間隨機采樣，能在復雜約束下快速搜索高維空間，把搜索導向空白區域，從而尋找到從起始點到目標點的規劃路徑。

2.1 RRT與RRTConnect算法原理

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

圖4 RRT原理圖

RRT每次搜索只從初始點生長快速擴展隨機樹來搜索狀態空間，與RRT相比基于雙向擴展平衡的連結型雙樹RRTConnect算法[8]在目標點區域建立第二棵樹與第一棵樹同時進行擴展如圖4所示，直到擴展失敗或與第一棵樹相連，整個算法結束。

2.2 算法測試

在平面路徑規劃測試中，本文利用MATLAB編程，輸入像素尺寸為500×500的地圖，定義起始點source=[10,10]，目標點goal=[490,490]，迭代步長Stepsize為20，最大迭代次數MaxAttempts為10 000次，使用RRT與RRTConnect算法搜索出無碰撞路徑，測試結果如表1所示。

表1 測試結果對比

在空間路徑規劃測試中，為提高路徑規劃效率，本文在ROS中的OMPL運動規劃庫，選用RRTConnect算法替代RRT算法進行路徑規劃，在Rviz中添加MotionPlanning插件，通過導入Blender生成的.dae文件添加障礙物，同時設定機器人運動起始點home與目標點goal，運行C++源文件執行機器人無碰撞路徑規劃如圖5所示，RRTConnect算法與RRT算法規劃分別耗時0.083 s和0.035 s。

(a) 起始點位置 (b) 目標點位置 (c) 路徑規劃過程圖5 無碰撞路徑規劃

平面與空間路徑規劃的仿真結果表明，與RRT相比RRTConnect通過朝向對方快速擴展，這種帶有啟發性的擴展使樹的擴展更加明確，能顯著提高搜索速度、搜索效率，有利于機械臂在多約束復雜環境下盡快完成路徑規劃，實現避障。

3 笛卡爾空間機械臂軌跡規劃

機械臂在進行連續軌跡作業時，要保證末端軌跡的準確定義[9]，使末端執行器避開障礙物，必須在笛卡爾空間進行軌跡規劃，并把規劃的路徑劃分為有限個過渡點，通過逆運動學求解，實時轉換為每個關節的角度值。本文主要研究新松SR7CL工業機器人在笛卡爾空間中的直線軌跡規劃和圓弧軌跡規劃[10]。

3.1 空間直線軌跡規劃

(1)

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

圖6 笛卡爾空間直線規劃

3.2 空間圓弧軌跡規劃

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

在笛卡爾空間圓弧軌跡規劃中，為計算簡便，在圓弧所在平面新建一個以圓弧中心為原點的坐標系，把圓弧上任意兩個始末點弧長計算轉化為兩點對應圓心角增量的計算，計算出圓弧的各插補點在新坐標系中的值，通過運動學關系將這些值映射到參考坐標系中。

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

由三點確定過圓弧的平面方程為：

(2)

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

(2)以P0為原點建立新坐標系B并進行坐標變換:

圖7 笛卡爾空間坐標變換示意圖

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

(3)

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

(4)

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

v=u×w

(5)

由A坐標系到B坐標系的位姿變換矩陣為：

(6)

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

(7)

笛卡爾空間圓弧規劃把對圓弧長度的插補轉換為對圓弧所對應圓心角θ的插補，生成B坐標系下圓弧上一系列點由式(7)映射為基坐標系下一系列路徑點，在ROS中利用Moveit!提供的Python接口編程，實現了笛卡爾空間圓弧規劃如圖8所示。

圖8 笛卡爾空間圓弧規劃

4 結論

本文基于ROS搭建了機械臂仿真平臺，創建URDF文件完成了6自由度機械臂的建模。為保證機械臂實際作業中避免與環境碰撞，在平面、空間分別進行算法測試，驗證了RRTConnect算法具有更高的搜索速度，完成了復雜環境下無碰撞路徑規劃，保證了機械臂運動安全。利用Moveit!進行笛卡爾空間下直線與圓弧軌跡規劃，實現了插補運算，保證了對連續軌跡的精確定義。