基于ROS的工業(yè)機(jī)器人軌跡規(guī)劃和仿真

李 浩，鄭智貞，黃順舟，楊富偉，劉澤宇

(1.中北大學(xué) 機(jī)械工程學(xué)院，太原 030051；2.上海航天設(shè)備制造總廠，上海 200245)

0 引言

隨著自動化的普及與機(jī)器人技術(shù)的快速發(fā)展，以及企業(yè)對于生產(chǎn)效率的需求，給工業(yè)機(jī)器人的發(fā)展提供了良好環(huán)境，工業(yè)機(jī)器人應(yīng)用較為廣泛。由于工業(yè)機(jī)器人連桿結(jié)構(gòu)的復(fù)雜性和多自由度的工作空間，使得高精度的軌跡規(guī)劃成為了工業(yè)機(jī)器人技術(shù)中一個普遍遇到的難題[1-2]。軌跡規(guī)劃的好壞關(guān)系到工業(yè)機(jī)器人控制系統(tǒng)的性能，在整個機(jī)器人控制系統(tǒng)設(shè)計中占比較大的比重。目前對于軌跡規(guī)劃的研究多是應(yīng)用機(jī)器人仿真平臺來進(jìn)行的，例如CARMEN、TeamBots等，這些機(jī)器人開發(fā)平臺各有特點，但是功能較為單一；一些機(jī)械制圖軟件例如Solodworks，同樣可以對工業(yè)機(jī)器人進(jìn)行仿真，但是缺乏算法；一些機(jī)器人廠家開發(fā)的商用軟件如KUKA公司的KUKASim，但是其兼容性很差。ROS是一款主要應(yīng)用于機(jī)器人開發(fā)的開源操作系統(tǒng)，具有多種編程語言兼容、免費開源、跨平臺等優(yōu)點。

工業(yè)機(jī)器人軌跡規(guī)劃可以使所要控制的機(jī)器人完成在笛卡爾坐標(biāo)系和關(guān)節(jié)運動坐標(biāo)系的運動軌跡，在笛卡爾空間中規(guī)劃機(jī)器人末端執(zhí)行器運行軌跡，在關(guān)節(jié)空間坐標(biāo)系中得出關(guān)節(jié)運動參數(shù)關(guān)于時間的函數(shù)。文獻(xiàn)[3]用D-H參數(shù)法建立串聯(lián)機(jī)器人模型，運用五次插值算法在ROS中實現(xiàn)了仿真，但對加加速度這個對關(guān)節(jié)柔性沖擊影響較大的參數(shù)沒有進(jìn)行分析；文獻(xiàn)[4]運用改進(jìn)的D-H算法和高階多項式差值算法進(jìn)行6軸工業(yè)機(jī)器人軌跡規(guī)劃算法研究，并在Matlab中進(jìn)行了仿真實驗，但是仿真過程沒有實際機(jī)器人模型且Matlab提供了算法API，無需進(jìn)行底層算法編碼工作；文獻(xiàn)[5]提出了一種在ROS中構(gòu)建模型的方法，包括外界三維模型導(dǎo)入及URDF文件編寫。

本文通過將工業(yè)機(jī)器人實體模型轉(zhuǎn)換為ROS中所使用的URDF格式文件，使用Moveit！工具添加在笛卡爾空間下的插值算法，生成軌跡規(guī)劃所需要的配置文件，引入加加速度參數(shù)，并對S形曲線軌跡規(guī)劃算法進(jìn)行優(yōu)化，將優(yōu)化后的S形曲線軌跡規(guī)劃算法植入ROS配置文件中，編寫launch文件建立與Move_group節(jié)點的通訊。最后在Rviz中進(jìn)行仿真實驗，并運用ROS工具觀察機(jī)器人關(guān)節(jié)角度的變化，可以看出機(jī)器人關(guān)節(jié)轉(zhuǎn)動平滑，角度變化連續(xù)，能夠有效的降低關(guān)節(jié)運動過程中產(chǎn)生的柔性沖擊。

1 機(jī)器人模型建立

本文以ABB-IRB-4400機(jī)器人為研究對象，該機(jī)器人是6自由度串聯(lián)工業(yè)機(jī)器人。由于ROS中采用的是機(jī)器人描述格式URDF(Unified Robot Description Format)來構(gòu)建機(jī)器人，URDF使用XML標(biāo)簽來描述機(jī)器人的每個組件，以URDF形式先描述機(jī)器人基座(在URDF中將基座看作一個固定的連桿)的名稱和類型、連接到基座的連桿，之后逐一說明連桿和關(guān)節(jié)的內(nèi)容。連桿link標(biāo)簽描述連桿的名稱、大小、重量和慣性等，在URDF中可以輸入CAD文件用于可視化界面的模型顯示，如STL和DAE。所以可以使用三維建模軟件建立各個連桿的三維模型，然后通過link標(biāo)簽將模型導(dǎo)入，joint標(biāo)簽描述連桿之間的關(guān)節(jié)，包括關(guān)節(jié)的名稱、類型、并且定義連接兩桿的父子關(guān)系[6-7]。limit標(biāo)簽還可以設(shè)置關(guān)節(jié)運動的極限，例如給予關(guān)節(jié)的力，速度和角度等物理量的限制，還有很多標(biāo)簽用于描述機(jī)器人的一些其他特征，如顏色，材料等。

在完成URDF文件的編寫后，還可以通過RViz3D可視化工具來驗證文件的正確與否。如圖1所示，RViz中顯示的機(jī)器人模型正確。圖2是用PROE建立的各個連桿的模型。

圖1 RViz顯示機(jī)器人

圖2 機(jī)器人連桿模型圖

2 笛卡爾空間軌跡規(guī)劃

Moveit！是ROS中用于軌跡規(guī)劃功能非常強(qiáng)大的工具，它具有非常多的功能，包括用于運動規(guī)劃的快速逆運動學(xué)分析、運動學(xué)算法、動力學(xué)、運動規(guī)劃和控制器。Moveit！中的核心節(jié)點Move_group可以使用ROS動作和服務(wù)與用戶交換命令，可以從URDF文件和Moveit！配置文件中接受機(jī)器人信息，并且能夠通過ROS話題和動作提供機(jī)器人的狀態(tài)與控制，還可以提供環(huán)境信息[8]。

2.1 直線軌跡規(guī)劃

機(jī)器人末端從初始點P1(X1,Y1,Z1)運動到P2(X2,Y2,Z2)，其運動軌跡為一條直線的情況。在笛卡爾空間的基坐標(biāo)系下，用矢量方程表示出P1和P2：

P1=X1i+Y1j+Z1k

(1)

P2=X2i+Y2j+Z2k

(2)

用d表示這兩點的直線距離，那么在機(jī)器人末端以速度矢量v從P1運動到P2這段時間T內(nèi)，任意t時刻，機(jī)器人末端的位置矢量Pt(Xt,Yt,Zt)可以計算出：

(3)

在ROS中，通過RViz實現(xiàn)直線插補(bǔ)仿真，具體過程如下：首先給定P1和P2，由P1運動到P2，機(jī)器人的軌跡動態(tài)如圖3所示，然后采用笛卡爾空間的軌跡約束，通過逆解運算得出6個關(guān)節(jié)的變化過程，通過rqt_plot插件可以繪制出關(guān)節(jié)變化曲線。如圖4所示，定義連接基座與連桿1的關(guān)節(jié)為1，依此類推，定義關(guān)節(jié)2～6，關(guān)節(jié)2對應(yīng)圖中曲線2，關(guān)節(jié)3對應(yīng)圖中曲線3，后文如沒有特別指出，則對應(yīng)關(guān)系不變，圖中橫坐標(biāo)為運行時間，單位是s，縱坐標(biāo)為關(guān)節(jié)角度，采用弧度制。

圖3 直線軌跡運動動態(tài)圖

圖4 關(guān)節(jié)角度變化曲線

2.2 圓弧軌跡規(guī)劃

對于圓弧插補(bǔ)問題，首先要轉(zhuǎn)化為二維平面問題，由不共線的三點A,B,C可以確定一個圓弧平面，其坐標(biāo)分別為P1(X1,Y1,Z1),P2(X2,Y2,Z2),P3(X3,Y3,Z3)，那么A,B,C三點可以唯一確定一個過此三點的平面，通過對AB,BC分別做垂直平分線，兩條垂直平分線相交于圓弧圓心處，可求得圓弧半徑R，進(jìn)而可以求出AB和BC兩段圓弧的圓心角φ1和φ2，接著構(gòu)建圓弧平面坐標(biāo)系，也就是將OR(X0,Y0,Z0)坐標(biāo)系原點與圓心重合，ORXRYR平面為圓弧所在平面，且保持ZR為外法線方向，如圖5所示[2]。

圖5 圓弧坐標(biāo)系建立

令TR表示由圓弧坐標(biāo)ORXRYRZR至基礎(chǔ)坐標(biāo)系的轉(zhuǎn)換矩陣。若ZR軸與基礎(chǔ)坐標(biāo)系ZO軸的夾角為α，XR軸與基礎(chǔ)坐標(biāo)系XO軸的夾角為θ，由于運動是相對固定坐標(biāo)系，通過繞Z軸和X軸旋轉(zhuǎn)可得轉(zhuǎn)換矩陣：

TR=T(XOR,YOR,ZOR)L(Z,θ)L(X,α)=

(4)

其中，XOR,YOR,ZOR是圓弧坐標(biāo)系原點在基座標(biāo)系下的坐標(biāo)，L表示繞坐標(biāo)軸旋轉(zhuǎn)一定的角度。

在任意t時間內(nèi)，基座標(biāo)系下的坐標(biāo)值為POt(XOt,YOt,ZOt)，圓弧坐標(biāo)系下的坐標(biāo)為PRt(XRt,YRt,ZRt)。

則在其插補(bǔ)周期內(nèi)對應(yīng)的圓心角為：Δφ=vt/R。

圓弧插補(bǔ)的總步數(shù)：N=φ1+φ2/Δφ+1。

可得第i個插補(bǔ)點Pi的位置坐標(biāo)為：

(5)

通過轉(zhuǎn)換矩陣運算可得圓弧位置插補(bǔ)為：

(6)

依照關(guān)節(jié)空間軌跡規(guī)劃的方法，結(jié)合末端執(zhí)行器空間的姿態(tài)歐拉角，可求得機(jī)器人末端執(zhí)行器在各個插補(bǔ)點的位姿角。在ROS中實現(xiàn)的圓弧插補(bǔ)仿真如圖6所示，通過rqt_plot插件繪制出的關(guān)節(jié)角度變化曲線如圖7所示，圓弧軌跡規(guī)劃中，6個關(guān)節(jié)角度均出現(xiàn)變化，從圖中可以看出，關(guān)節(jié)變化平滑，萬向節(jié)無鎖死。

圖6 圓弧軌跡運動動態(tài)圖

圖7 關(guān)節(jié)角度變化曲線

3 關(guān)節(jié)空間軌跡規(guī)劃

工業(yè)機(jī)器人在運動過程中關(guān)節(jié)運動參數(shù)的變化對運動路徑精度影響很大，通常在關(guān)節(jié)伺服電機(jī)進(jìn)行啟動或停止的加減速期間易產(chǎn)生突變，故引入加加速度參數(shù)，通過對S形軌跡規(guī)劃方法優(yōu)化，得到關(guān)節(jié)速度、加速度和加加速度的表達(dá)式，來完成對關(guān)節(jié)空間軌跡規(guī)劃插值計算，保證運動軌跡的平滑性，避免了運動過程中產(chǎn)生的柔性沖擊。

S形軌跡規(guī)劃方法中關(guān)節(jié)運動的角速度變化可以描述為：

(7)

其速度和加速度曲線如圖8所示，可以看出，加速度的變化呈梯字型，所以導(dǎo)致其導(dǎo)數(shù)加加速度在拐點處存在突變，在運動過程中造成柔性沖擊。針對關(guān)節(jié)運動過程中加加速度的非連續(xù)性，可以采用三角函數(shù)代替梯形函數(shù)對其進(jìn)行擬合優(yōu)化。

圖8 速度加速度變化曲線圖

在運動過程中，速度和加速度連續(xù)變化的同時，采用正弦函數(shù)對加速度曲線進(jìn)行擬合，從而消除加加速度在變化過程中產(chǎn)生的突變，可以有效的避免由于突變產(chǎn)生的振動和沖擊，從而避免影響機(jī)器人運行軌跡的精度。用A表示加速度的峰值，w為角頻率，T為運動總時長，Ta表示關(guān)節(jié)運動中加速過程的時間，得出加加速度表達(dá)式為：

(8)

通過積分運算可以得到加速度的正弦函數(shù)表達(dá)式：

(9)

再次進(jìn)行積分運算可以得到速度的表達(dá)式如下：

(10)

通過對優(yōu)化后的速度，加速度，加加速度的表達(dá)式曲線繪制，可以看出在加速或減速的Ta期間，關(guān)節(jié)速度和加速度變化連續(xù)平滑，通過具有光滑性的三角函數(shù)擬合，加加速度的變化相對之前表現(xiàn)為緩慢遞增或者遞減，變化更加均勻連續(xù)。

圖9 優(yōu)化后的變化曲線

設(shè)關(guān)節(jié)運動在t=0和t=T時刻角度值分別為θ0和θ1，且速度和加速度均為0，結(jié)合伺服電機(jī)加速度幅值A(chǔ)，可以得出Ta為：

(11)

綜合上述公式，可以得出插補(bǔ)點的表達(dá)式為：

(12)

在ROS中實現(xiàn)關(guān)節(jié)空間軌跡規(guī)劃的具體過程：通過Moveit！生成相關(guān)配置文件后，此時通過RViz仿真器可以接受運動指令，控制機(jī)器人運動并對其關(guān)節(jié)位置信息進(jìn)行反饋。接著使用Python語言編譯插值算法，使其作為一個單獨的action文件啟動，并且建立于RViz節(jié)點的通信。運行此action文件后，RViz便可接受節(jié)點信息并作出響應(yīng)。關(guān)節(jié)空間軌跡規(guī)劃ROS仿真如圖10所示。

圖10 關(guān)節(jié)空間軌跡規(guī)劃

4 結(jié)束語

本文通過ROS機(jī)器人平臺對六軸工業(yè)機(jī)器人進(jìn)行路徑規(guī)劃算法研究和仿真實驗，完成了在笛卡爾空間的直線軌跡和圓弧軌跡的模型構(gòu)建，并用RVIZ進(jìn)行了仿真；引入了加加速度參數(shù)，通過優(yōu)化的S型軌跡規(guī)劃方法得到的關(guān)節(jié)角速度、加速度和加加速度光滑連續(xù)，減少了關(guān)節(jié)運動過程中的柔性沖擊，保證在關(guān)節(jié)運動的平穩(wěn)性及運動軌跡的平滑性，并且對機(jī)器人末端軌跡進(jìn)行了仿真，驗證了該算法的正確性。對ROS機(jī)械臂開發(fā)和工業(yè)機(jī)器人路徑規(guī)劃研究都有一定的指導(dǎo)意義。