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

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

0 引言

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

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

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

1 機器人模型建立

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

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

圖1 RViz顯示機器人

圖2 機器人連桿模型圖

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

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

2.1 直線軌跡規(guī)劃

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

P1=X1i+Y1j+Z1k

(1)

P2=X2i+Y2j+Z2k

(2)

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

(3)

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

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

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

2.2 圓弧軌跡規(guī)劃

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

圖5 圓弧坐標系建立

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

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

(4)

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

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

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

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

可得第i個插補點Pi的位置坐標為：

(5)

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

(6)

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

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

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

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

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

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

(7)

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

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

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

(8)

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

(9)

再次進行積分運算可以得到速度的表達式如下：

(10)

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

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

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

(11)

綜合上述公式，可以得出插補點的表達式為：

(12)

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

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

4 結束語

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