基于ROS的機(jī)械臂運(yùn)動(dòng)規(guī)劃研究

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

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

0 前言

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

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

1 模型配置

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

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

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

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

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

圖2 機(jī)器人模型

2 運(yùn)動(dòng)學(xué)分析

2.1 正運(yùn)動(dòng)學(xué)求解

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

圖3 ABB irb-2400機(jī)器人連桿坐標(biāo)系

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

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

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

(1)

(2)

2.2 逆運(yùn)動(dòng)學(xué)求解

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

(3)

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

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

(4)

利用三角代換得：

(5)

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

3 軌跡規(guī)劃

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

3.1 直線軌跡規(guī)劃

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

(6)

在MoveIt中采用前面的直線軌跡規(guī)劃進(jìn)行運(yùn)算，完成軌跡點(diǎn)之間的插補(bǔ)，運(yùn)動(dòng)實(shí)驗(yàn)如圖4所示，實(shí)體為機(jī)器人起始狀態(tài)，虛體為結(jié)束狀態(tài)，給定起始點(diǎn)和終止點(diǎn)坐標(biāo)，運(yùn)行程序。

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

3.2 圓弧軌跡規(guī)劃

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

Ax+By+Cz+D=0

(7)

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

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

(8)

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

(9)

在MoveIt中對機(jī)器人進(jìn)行圓弧軌跡規(guī)劃，運(yùn)動(dòng)軌跡如圖5所示。

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

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

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

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

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

Zi=(k×N+1)bi

(10)

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

將運(yùn)行過程中各軸插補(bǔ)點(diǎn)坐標(biāo)導(dǎo)入Excel表格，結(jié)果如圖7所示，各軸均保持線性運(yùn)動(dòng)，證明該算法的可行性。

圖7 線性插補(bǔ)過程各軸位置

4 避障規(guī)劃

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

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

圖8 RRT算法原理

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

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

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

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

圖10 機(jī)械臂到達(dá)終點(diǎn) 圖11 機(jī)械臂返回起點(diǎn)

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

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

5 結(jié)語

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