基于ROS的工業機器人軌跡規劃和仿真

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

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

0 引言

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

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

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

1 機器人模型建立

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

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

圖1 RViz顯示機器人

圖2 機器人連桿模型圖

2 笛卡爾空間軌跡規劃

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

2.1 直線軌跡規劃

機器人末端從初始點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內，任意t時刻，機器人末端的位置矢量Pt(Xt,Yt,Zt)可以計算出：

(3)

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

圖3 直線軌跡運動動態圖

圖4 關節角度變化曲線

2.2 圓弧軌跡規劃

對于圓弧插補問題，首先要轉化為二維平面問題，由不共線的三點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至基礎坐標系的轉換矩陣。若ZR軸與基礎坐標系ZO軸的夾角為α，XR軸與基礎坐標系XO軸的夾角為θ，由于運動是相對固定坐標系，通過繞Z軸和X軸旋轉可得轉換矩陣：

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

(4)

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

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

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

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

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

(5)

通過轉換矩陣運算可得圓弧位置插補為：

(6)

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

圖6 圓弧軌跡運動動態圖

圖7 關節角度變化曲線

3 關節空間軌跡規劃

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

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

(7)

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

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

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

(8)

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

(9)

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

(10)

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

圖9 優化后的變化曲線

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

(11)

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

(12)

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

圖10 關節空間軌跡規劃

4 結束語

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