基于ROS的機械臂運動規(guī)劃研究

屈力剛，高凱，邢宇飛,張丹雅

(沈陽航空航天大學機電工程學院，遼寧沈陽 110136)

0 前言

隨著機器人平臺與硬件設備日益豐富，機器人系統(tǒng)變得更加復雜，軟件的編寫難度也在不斷增加。當前機器人技術(shù)面臨的問題有：不同軟件之間難以互通互聯(lián)；硬件不斷豐富，軟件代碼復雜繁瑣；不同機器人之間軟件代碼不能復用和模塊化[1]。為解決以上問題，科學工作者研發(fā)了多種優(yōu)秀的軟件框架：ROS(機器人操作系統(tǒng))界面簡單易操作，適配多種機器人模型，采用統(tǒng)一的格式進行描述，可以控制多種型號機器人，系統(tǒng)設計結(jié)構(gòu)簡單，支持多種語言；OpenRTM機器人平臺是面向組件的開發(fā)，通過為各功能元素創(chuàng)建組件來構(gòu)建系統(tǒng)；Orocos是用于機器人控制的C語言庫，允許使用C語言編寫實時組件；其中ROS具有多語言支持、架構(gòu)精簡，集成度高、組件化工具包豐富、免費開源等多個優(yōu)點被廣泛使用[2-3]。

本文作者基于ROS實驗平臺，采用ABB irb-2400型號機器人為研究主體，進行正逆運動學分析。隨后將機器人的三維實體模型設置為URDF文件，采用Setup Assistant配置包，根據(jù)URDF模型文件完成機器人的配置和Rviz可視化，并對機器人進行笛卡爾空間規(guī)劃，最后進行避障仿真。

1 模型配置

ROS使用統(tǒng)一的機器人描述格式(Unified Robot Description Format，URDF)來構(gòu)建機器人模型，可以直接控制移動或者程序驅(qū)動機器人以及添加其他環(huán)境信息[4]。URDF文件采用XML標簽，以連桿(link)和關(guān)節(jié)(joint)為基本結(jié)構(gòu)，描述了機器人的各個部件及其位置關(guān)系。并且可以儲存機器人的基本物理屬性，以及機器人的運動學、動力學屬性。

URDF文件主要由連桿(link)和關(guān)節(jié)(joint)2個基本結(jié)構(gòu)組成。如圖1所示：連桿分別通過visual描述外觀參數(shù)，inertial描述慣性參數(shù)，以及collision描述碰撞屬性；關(guān)節(jié)的作用是連接2個連桿，稱為父連桿和子連桿。復雜的機械臂模型可以簡化為若干連桿，通過關(guān)節(jié)按照固定順序連接起來。每個連桿和關(guān)節(jié)的屬性都是固定的，因此，只要確定基座的坐標系就可以推出整個模型的坐標關(guān)系[5]。

圖1 URDF關(guān)節(jié)、連桿結(jié)構(gòu)

依據(jù)上述原理創(chuàng)建好ABB irb-2400型號機器人完整的URDF文件后，使用MoveIt Setup Assistant生成irb-2400機器人的MoveIt配置功能包，從而完成機器人運動規(guī)劃的初始配置、可視化和仿真等工作[6]。

隨后運行其中的launch文件，通過Rviz實時顯示機器人模型以及狀態(tài)，以便于直觀地配置和修改模型文件。機器人模型如圖2所示[7]。

圖2 機器人模型

2 運動學分析

2.1 正運動學求解

機器人的正向運動學分析是根據(jù)機械臂各關(guān)節(jié)類型，相鄰關(guān)節(jié)尺寸及運動量大小來確定末端執(zhí)行器的位置和位姿關(guān)系。運用D-H參數(shù)法對irb-2400型號機械臂進行正運動學求解，即通過坐標轉(zhuǎn)換把末端執(zhí)行器的位姿在坐標系里表示出來。該參數(shù)法由DENAVIT和HARTENBERG提出[8]，現(xiàn)已成為常用的機器人連桿建模方法[9]。根據(jù)D-H參數(shù)法建立的irb-2400 機器人的坐標系如圖3所示。

圖3 ABB irb-2400機器人連桿坐標系

圖3中基坐標系即為x0y0z0構(gòu)成的坐標系，經(jīng)過坐標變換得到末端執(zhí)行器坐標系即為x6y6z6所構(gòu)成的坐標系。機器人連桿長度和關(guān)節(jié)轉(zhuǎn)角范圍的相關(guān)參數(shù)描述可以通過查閱URDF文件獲取。根據(jù)相關(guān)參數(shù)建立機械臂D-H參數(shù)，如表1所示。其中，i表示連桿，ai表示連桿長度，αi表示連桿扭曲角，di表示連桿偏置，θi表示變量關(guān)節(jié)角度[10]。

表1 ABB irb-2400機器人D-H參數(shù)

在建立各個關(guān)節(jié)的坐標系以后，根據(jù)D-H法則經(jīng)過2次旋轉(zhuǎn)和2次平移后確立相鄰兩連桿i-1和i之間的相對關(guān)系:

(1)

(2)

2.2 逆運動學求解

在實際情況中，大多數(shù)是已知機器人末端執(zhí)行器的目標位姿矩陣，求解各關(guān)節(jié)的角度變化，即求θ1、θ2、θ3、θ4、θ5、θ6的值。通過代數(shù)法對機器人進行逆運動學求解，ABB irb-2400機器人具有6個自由度，其運動學方程可寫成：

(3)

令矩陣方程(3)兩端元素2、4行對應相等得:

-sinθ1px+cosθ1py=d2

(4)

利用三角代換得：

(5)

式中正負號對應于θ1的2種可能解。選定其中一個解后，再令式(3)兩端元素1、4行和3、4行分別對應相等，得到2個方程，繼續(xù)使用三角代換可解出θ3。以此類推求出其余每個關(guān)節(jié)的解。

3 軌跡規(guī)劃

機器人在進行連續(xù)作業(yè)時，為保證末端執(zhí)行器按照既定軌跡進行運動，則需要在笛卡爾空間中進行軌跡規(guī)劃。笛卡爾空間軌跡規(guī)劃就是已知機器人末端的初始點位置和終止點位置，將軌跡劃分為有限個點，通過插補算法求解出中間點的位置，然后使用逆運動學求解出各中間點所對應的關(guān)節(jié)角度值。

3.1 直線軌跡規(guī)劃

對于直線軌跡規(guī)劃，已知機器人末端執(zhí)行器起始點Q0(x0,y0,z0)和終止點Q1(x1,y1,z1)，設插補次數(shù)為N，則中間插補點坐標為

(6)

在MoveIt中采用前面的直線軌跡規(guī)劃進行運算，完成軌跡點之間的插補，運動實驗如圖4所示，實體為機器人起始狀態(tài)，虛體為結(jié)束狀態(tài)，給定起始點和終止點坐標，運行程序。

圖4 直線軌跡規(guī)劃

3.2 圓弧軌跡規(guī)劃

機器人末端從Q1(x1,y1,z1)經(jīng)過第二點Q2(x2,y2,z2)到達終點Q3(x3,y3,z3)，此圓弧即為機械臂末端的運動軌跡。由Q1、Q2、Q3三點所構(gòu)成的平面方程為

Ax+By+Cz+D=0

(7)

取直線Q1Q2和Q2Q3垂直平分線，交點即為圓心O(x0,y0,z0)坐標，進而求得圓弧半徑R。

以圓心O為坐標原點，構(gòu)建新坐標系o-uvw。令Q1O連線方向為u軸方向，Q1Q2連線垂直方向為w軸方向，v軸方向通過u、w軸向量的叉乘得到[11]。則可建立起基座標系到新坐標系的變換矩陣為

(8)

設圓弧上有一點Qi在新坐標系下坐標為(ui,vi,wi),設Q0O和Q1O夾角為θ，則Q0坐標為(Rcosθ,Rsinθ,0),將插補結(jié)果得到坐標映射至基座標系上。設點Q0在基坐標系的坐標值為(xi,yi,zi),則有：

(9)

在MoveIt中對機器人進行圓弧軌跡規(guī)劃，運動軌跡如圖5所示。

圖5 圓弧軌跡規(guī)劃

3.3 自定義線性插補規(guī)劃

move_group是MoveIt中的核心節(jié)點，主要是將各功能包和插件集成起來為用戶提供動作指令和服務。自定義規(guī)劃算法的實現(xiàn)如圖6所示，將設計好的算法與MoveIt標準接口組成一個插件，添加注冊文件使得move_group能夠感知算法，之后通過yaml、launch文件調(diào)用算法，從而使機器人在運動規(guī)劃過程中采用自定義算法進行軌跡規(guī)劃。

圖6 自定義規(guī)劃算法實現(xiàn)原理

線性插補是機器人在運動規(guī)劃過程中從起始點到終止點各軸均保持線性均速移動。對于線性插補規(guī)劃，已知起始點Q0(x0,y0,z0)和終止點Q1(x1,y1,z1)，設插補次數(shù)為N，各軸中心點初始坐標為bi，插補后坐標為Zi(i=1,…,6)，斜率為k，則中間各軸坐標滿足：

Zi=(k×N+1)bi

(10)

將算法插件接入MoveIt，運行ABB機械臂launch文件，打開Rviz界面后，即可看到運動規(guī)劃器已換成自定義算法。給定起始點和終止點，點擊運行，機器人即完成線性插補。

將運行過程中各軸插補點坐標導入Excel表格，結(jié)果如圖7所示，各軸均保持線性運動，證明該算法的可行性。

圖7 線性插補過程各軸位置

4 避障規(guī)劃

在很多應用場景下，機器人的工作環(huán)境中有其他物體，在機器人運動過程中這些物體可能成為其運動規(guī)劃的障礙物。為避免與物體發(fā)生碰撞，運動規(guī)劃需要考慮避障問題。MoveIt中的OMPL是包含許多運動規(guī)劃算法的開源采樣運動規(guī)劃庫并且支持避障規(guī)劃，文中采用其中的RRT-Connect算法對機器人進行避障仿真分析。

傳統(tǒng)RRT(Rapidly Exploring Random Tree)算法是一種快速搜索算法，從起始點開始，以樹狀分支的路徑形式快速擴展來填充空間的大部分區(qū)域，直到尋找到目標區(qū)域，原理如圖8所示[12]。RRT-Connect算法在RRT的基礎(chǔ)上進行改進，在目標點區(qū)域建立第二棵樹，2棵樹不斷朝向?qū)Ψ浇惶鏀U展，顯著提高了搜索速度和效率[13]。

圖8 RRT算法原理

通過在MATLAB編程，加載障礙物地圖，設定起始點[10,10]和目標點[490,490],迭代次數(shù)5 000，迭代步長為20，使用RRT和RRT-Connect算法搜索出到達目標點的無碰撞路徑，所用時間和路徑長度如圖9所示。

圖9 RRT算法與RRT-Connect算法結(jié)果對比

可見，RRT-Connect算法所用時間和規(guī)劃路徑長度均短，因此，為規(guī)劃提高效率，在仿真實驗中采用RRT-Connect算法進行路徑規(guī)劃。

在終端啟動配置的irb-2400機器人文件，進入Rviz界面，選擇RRT-Connect算法，在Motion Planning里將機器人末端運動軌跡顯示出來，以便于分析和觀察。令初始位置為起點位置，設定機器人的終點位置。另起一個終端，將編寫的Python程序運行，可以看到障礙物添加到環(huán)境中。將障礙物發(fā)布于環(huán)境，機器人在運動過程中會躲避障礙物到達終點。當機器人從終點返回到起點時，仍然會規(guī)劃一條避障路徑，運動軌跡如圖10、圖11所示。

圖10 機械臂到達終點 圖11 機械臂返回起點

Rqt_plot是ROS平臺的二維數(shù)值曲線繪制工具，可以將需要顯示的數(shù)據(jù)用平面坐標圖描繪。在終端輸入Rqt打開界面，輸入/joint_states/position，即可得到實時關(guān)節(jié)變化曲線。如圖12所示，在機器人進行避障規(guī)劃過程中，各關(guān)節(jié)平穩(wěn)運行。

圖12 關(guān)節(jié)變化曲線

5 結(jié)語

文中基于ROS平臺對機器人進行運動規(guī)劃研究，包括軌跡規(guī)劃及避障仿真實驗。通過各關(guān)節(jié)位置變化曲線可以看出：機器人運動過程軌跡平滑連續(xù)，穩(wěn)定性強。既可以按照既定直線和圓弧軌跡運動，又可以靈活進行避障，并且可以根據(jù)實際需求自定義運動規(guī)劃算法，對機器人的研究具有一定的參考價值。