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

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

0 引言

隨著自動(dòng)化的普及與機(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ù)中一個(gè)普遍遇到的難題[1-2]。軌跡規(guī)劃的好壞關(guān)系到工業(yè)機(jī)器人控制系統(tǒng)的性能，在整個(gè)機(jī)器人控制系統(tǒng)設(shè)計(jì)中占比較大的比重。目前對于軌跡規(guī)劃的研究多是應(yīng)用機(jī)器人仿真平臺(tái)來進(jìn)行的，例如CARMEN、TeamBots等，這些機(jī)器人開發(fā)平臺(tái)各有特點(diǎn)，但是功能較為單一；一些機(jī)械制圖軟件例如Solodworks，同樣可以對工業(yè)機(jī)器人進(jìn)行仿真，但是缺乏算法；一些機(jī)器人廠家開發(fā)的商用軟件如KUKA公司的KUKASim，但是其兼容性很差。ROS是一款主要應(yīng)用于機(jī)器人開發(fā)的開源操作系統(tǒng)，具有多種編程語言兼容、免費(fèi)開源、跨平臺(tái)等優(yōu)點(diǎn)。

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

本文通過將工業(yè)機(jī)器人實(shí)體模型轉(zhuǎn)換為ROS中所使用的URDF格式文件，使用Moveit！工具添加在笛卡爾空間下的插值算法，生成軌跡規(guī)劃所需要的配置文件，引入加加速度參數(shù)，并對S形曲線軌跡規(guī)劃算法進(jìn)行優(yōu)化，將優(yōu)化后的S形曲線軌跡規(guī)劃算法植入ROS配置文件中，編寫launch文件建立與Move_group節(jié)點(diǎn)的通訊。最后在Rviz中進(jìn)行仿真實(shí)驗(yàn)，并運(yùn)用ROS工具觀察機(jī)器人關(guān)節(jié)角度的變化，可以看出機(jī)器人關(guān)節(jié)轉(zhuǎn)動(dòng)平滑，角度變化連續(xù)，能夠有效的降低關(guān)節(jié)運(yùn)動(dòng)過程中產(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ī)器人的每個(gè)組件，以URDF形式先描述機(jī)器人基座(在URDF中將基座看作一個(gè)固定的連桿)的名稱和類型、連接到基座的連桿，之后逐一說明連桿和關(guān)節(jié)的內(nèi)容。連桿link標(biāo)簽描述連桿的名稱、大小、重量和慣性等，在URDF中可以輸入CAD文件用于可視化界面的模型顯示，如STL和DAE。所以可以使用三維建模軟件建立各個(gè)連桿的三維模型，然后通過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é)運(yùn)動(dòng)的極限，例如給予關(guān)節(jié)的力，速度和角度等物理量的限制，還有很多標(biāo)簽用于描述機(jī)器人的一些其他特征，如顏色，材料等。

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

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

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

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

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

2.1 直線軌跡規(guī)劃

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

P1=X1i+Y1j+Z1k

(1)

P2=X2i+Y2j+Z2k

(2)

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

(3)

在ROS中，通過RViz實(shí)現(xiàn)直線插補(bǔ)仿真，具體過程如下：首先給定P1和P2，由P1運(yùn)動(dòng)到P2，機(jī)器人的軌跡動(dòng)態(tài)如圖3所示，然后采用笛卡爾空間的軌跡約束，通過逆解運(yùn)算得出6個(gè)關(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)為運(yùn)行時(shí)間，單位是s，縱坐標(biāo)為關(guān)節(jié)角度，采用弧度制。

圖3 直線軌跡運(yùn)動(dòng)動(dòng)態(tài)圖

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

2.2 圓弧軌跡規(guī)劃

對于圓弧插補(bǔ)問題，首先要轉(zhuǎn)化為二維平面問題，由不共線的三點(diǎn)A,B,C可以確定一個(gè)圓弧平面，其坐標(biāo)分別為P1(X1,Y1,Z1),P2(X2,Y2,Z2),P3(X3,Y3,Z3)，那么A,B,C三點(diǎn)可以唯一確定一個(gè)過此三點(diǎn)的平面，通過對AB,BC分別做垂直平分線，兩條垂直平分線相交于圓弧圓心處，可求得圓弧半徑R，進(jìn)而可以求出AB和BC兩段圓弧的圓心角φ1和φ2，接著構(gòu)建圓弧平面坐標(biāo)系，也就是將OR(X0,Y0,Z0)坐標(biāo)系原點(diǎn)與圓心重合，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軸的夾角為θ，由于運(yùn)動(dòng)是相對固定坐標(biāo)系，通過繞Z軸和X軸旋轉(zhuǎn)可得轉(zhuǎn)換矩陣：

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

(4)

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

在任意t時(shí)間內(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個(gè)插補(bǔ)點(diǎn)Pi的位置坐標(biāo)為：

(5)

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

(6)

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

圖6 圓弧軌跡運(yùn)動(dòng)動(dòng)態(tài)圖

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

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

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

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

(7)

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

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

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

(8)

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

(9)

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

(10)

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

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

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

(11)

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

(12)

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

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

4 結(jié)束語

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