王馨蕊， 薛 梓， 黃 垚， 張福民

(1.天津大學(xué)，天津 300072；2.中國計量科學(xué)研究院，北京 100029)

1 引 言

隨著工業(yè)智能化和自動化的快速發(fā)展，多自由度非正交系統(tǒng)憑借其高效靈活性、高精度和高穩(wěn)定性等特點在工業(yè)領(lǐng)域得到了廣泛應(yīng)用。以工業(yè)機器人為典型代表，機器人的軌跡規(guī)劃可使其運動連續(xù)平滑，減少沖擊，很大程度上提高了機器人的穩(wěn)定性和精度，是機器人運動學(xué)和動力學(xué)的綜合應(yīng)用。目前常用的軌跡規(guī)劃方法主要分為在關(guān)節(jié)空間和在笛卡爾空間進行軌跡規(guī)劃。在笛卡爾空間中的軌跡規(guī)劃計算繁瑣，且在實際軌跡中存在奇異點；在關(guān)節(jié)空間中的軌跡規(guī)劃無機構(gòu)奇異性但避障性不好[1]。目前針對這些缺陷和不足，研究了各種解決方法，例如在關(guān)節(jié)空間內(nèi)，Chwa等[2]在樣條曲線規(guī)劃基礎(chǔ)上施加速度和力矩約束，可以使機械臂合理避障并有效提升速度；Saravanan等[3]針對B樣條曲線的不足，應(yīng)用NURBS曲線算法規(guī)劃機器人的運動軌跡來提升其精度和穩(wěn)定性；Xu X R等[4]針對規(guī)劃時速度和加速度曲線不連續(xù)的問題，采用了3-5-3樣條函數(shù)的分段軌跡規(guī)劃方法進行關(guān)節(jié)空間插補，簡化了規(guī)劃過程并提高了精確度。

本文研究的目標是完成對不同測量設(shè)備的統(tǒng)一溯源，保證其量值一致可比。用于測量多自由度非正交系統(tǒng)的設(shè)備種類繁多，例如激光跟蹤儀、計算機視覺測量設(shè)備等[5,6]，然而其各自的校準精度、連續(xù)性、速度等差異較大，使用時會造成用戶對同一系統(tǒng)采用不同設(shè)備測量時得到不同的位姿結(jié)果，無法準確判定、比較和評價。因此要規(guī)劃一個標準路徑，既能保證測量速度又能保證測量設(shè)備均能完整測得其空間曲線，并能反映出不同測量設(shè)備間的差異。

本文以工業(yè)機器人為研究對象，建立D-H參數(shù)模型，利用MATLAB Robotics toolbox對其運動學(xué)進行仿真驗證，并對三次和五次多項式插值法等多種不同運動軌跡進行仿真，觀察不同軌跡對設(shè)備的測量過程產(chǎn)生的影響；比較機器人在不同軌跡下的關(guān)節(jié)平順性，為后續(xù)進一步研究和規(guī)劃標準路徑，進行測量設(shè)備的溯源提供理論基礎(chǔ)。

2 工業(yè)機器人運動學(xué)分析

2.1 正向運動學(xué)

機器人的正向運動學(xué)是指給定各關(guān)節(jié)變量即角度值，通過計算公式，求解末端執(zhí)行器的位置和姿態(tài)，實現(xiàn)從關(guān)節(jié)空間到笛卡爾坐標系的轉(zhuǎn)換。首先建立機器人模型，本文采用最基本的D-H參數(shù)模型法[7]建立空間坐標系，如圖1所示，得到機器人的各連桿參數(shù)和關(guān)節(jié)變量值，見表1。其中αi為桿件扭角，ai為桿件長度，di為關(guān)節(jié)距離，θi為關(guān)節(jié)轉(zhuǎn)角。

根據(jù)相鄰連桿相對位置的4個齊次變換矩陣及其在基坐標系下的變換規(guī)則，可以得到相鄰兩連桿間的變換矩陣為：

(1)

式中：si= sinθi；ci= cosθi；i=1,2,3,…，6。

圖1 六自由度工業(yè)機器人D-H參數(shù)模型空間坐標系Fig.1 D-H parameter model space coordinate system of six-degree-of-freedom industrial robot

表1 六自由度工業(yè)機器人D-H參數(shù)表Tab.1 D-H parameters of six-degree-of-freedom industrial robot

將D-H參數(shù)代入可以得到各相鄰關(guān)節(jié)間的變換矩陣。根據(jù)坐標變換理論，將6個齊次變換矩陣相乘，可以得到機器人末端執(zhí)行器相對于基坐標系的運動學(xué)矩陣：

(2)

其中：

nx=c1[c23(c4c5c6-s4s6)-s23s5c6]+s1(s4c5c6+c4s6);

ny=s1[c23(c4c5c6-s4s6)-s23s5c6]-c1(s4c5c6+c4s6);

nz=-s23(c4c5c6-s4s6)-c23s5c6;

ox=-c1[c23(c4c5s6+s4c6)+s23s5s6]+s1(c4c6-s4c5c6);

oy=s1[c23(c4c5s6+s4c6)-s23s5s6]-c1(c4c6-s4c5s6);

oz=s23(c4c5s6+s4c6)+c23s5s6;

ax=-c1(c23c4s5+s23c5)-s1s4s5;

ay=-s1(c23c4s5+s23c5)+c1s4s5;

az=s23c4s5-c23c5;

px=-c1[c23(c4s5d6-a3)+s23(c5d6+d4)-a1-a2c2]-

s1s4s5d6;

py=-s1[c23(c4s5d6-a3)+s23(c5d6+d4)-a1-2a2c2]+

c1s4s5d6;

pz=s23(c4s5d6-a3)-c23(c5d6+d4)-a2s2。

式中：s23= sin (θ2+θ3)；c23= cos (θ2+θ3)。

2.2 逆向運動學(xué)

機器人的逆向運動學(xué)是指給定機器人末端執(zhí)行器的期望位姿，求解關(guān)節(jié)變量使得末端執(zhí)行器達到期望位姿，將解算的關(guān)節(jié)變量傳遞給控制器，進而驅(qū)動各個關(guān)節(jié)旋轉(zhuǎn)角度使機器人末端到達預(yù)期位置和姿態(tài)。因此逆向運動學(xué)的解算是機器人軌跡規(guī)劃問題的重要部分。

但機器人逆向運動學(xué)的解具有不唯一性，六自由度關(guān)節(jié)機器人逆解一般分為8種情況，需遵循各關(guān)節(jié)運動范圍限制和運動距離最小原則，選擇最佳關(guān)節(jié)角度[9]。

3 軌跡規(guī)劃

軌跡指機器人在運動過程中各個關(guān)節(jié)的位置、速度和加速度的變化聯(lián)系起來形成的一條曲線。軌跡規(guī)劃指根據(jù)客戶需求、工作任務(wù)和機器人性能來設(shè)定機器人末端執(zhí)行器的期望運動軌跡，保證機械臂末端位置、速度和加速度連續(xù)、軌跡平滑，避免過渡處出現(xiàn)較大振動和沖擊[10]。對工業(yè)機器人進行軌跡規(guī)劃時要考慮到各方面因素，包括機器人各部分結(jié)構(gòu)和零部件間的摩擦，要保證運動過程的平穩(wěn)性[11～13]。

軌跡規(guī)劃與時間函數(shù)有關(guān)，關(guān)節(jié)空間規(guī)劃是已知各關(guān)節(jié)變量的時間函數(shù)，利用其連續(xù)的一階、二階導(dǎo)數(shù)描述空間軌跡，計算速率快但避障性不好；而笛卡爾空間規(guī)劃是將機械臂末端的位置、速度和加速度用時間函數(shù)表示，通過逆向運動學(xué)解算各關(guān)節(jié)角度值，得到機器人末端的空間軌跡，軌跡直觀易觀察，但逆運動學(xué)解算計算量大，且可能存在奇異點。

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

關(guān)節(jié)空間軌跡規(guī)劃是利用關(guān)節(jié)變量的時間函數(shù)描述機器人的運動軌跡。對空間中已知點的坐標采用逆運動學(xué)解算出關(guān)節(jié)角，在關(guān)節(jié)空間中采用適當?shù)乃惴〝M合出光滑的插值函數(shù)，使機器人末端能夠依次通過起始點到目標點間的所有路徑點，保證各關(guān)節(jié)在每段路徑的運動時間相同，使各關(guān)節(jié)同時達到目標點，機器人才能按計劃軌跡到達預(yù)期位姿。通常采用多項式插補和拋物線線性擬合方法進行規(guī)劃[14]。

3.1.1 三次多項式插值

設(shè)三次多項式的位移-時間函數(shù)方程為：

θ(t)=a0+a1t+a2t2+a3t3

(3)

4個約束條件為：起始點和終止點關(guān)節(jié)角度已知，且速度均為0。

(4)

(5)

可以解得三次多項式的系數(shù)為：

(6)

但三次多項式插值法具有局限性，在多個路徑點間進行連續(xù)關(guān)節(jié)軌跡規(guī)劃時,會出現(xiàn)加速度變化不平滑的問題，因此選擇五次多項式來保持路徑點的加速度平滑連續(xù)。

3.1.2 五次多項式插值

設(shè)機器人運動過程中關(guān)節(jié)角度是關(guān)于時間的函數(shù)為五次多項式：

θ(t)=a0+a1t+a2t2+a3t3+a4t4+a5t5

其中對起始點和終止點的角度、速度和加速度均有要求，需滿足約束條件有：

(7)

(8)

根據(jù)約束條件，可以求解出五次多項式的6個系數(shù)為：

(9)

利用五次多項式插值擬合得到的軌跡曲線、速度曲線、加速度曲線平滑連續(xù)，但軌跡曲線的曲率無規(guī)則變化，控制機器人運動變得困難。

3.1.3 拋物線過渡的線性插值

線性插值能夠?qū)崿F(xiàn)控制軌跡曲率有規(guī)則變化，但直接線性插值會導(dǎo)致起始點和終止點的關(guān)節(jié)速度不連續(xù)，因此需在每個路徑點附近用一段拋物線進行擬合，使整個運動過程平穩(wěn)，速度連續(xù)，加速度不發(fā)生突變，其原理如圖2所示。

圖2 拋物線過渡的插值原理圖Fig.2 Interpolation principle diagram of parabolic transition

在過渡區(qū)域(t0,tb)內(nèi)，所有路徑都經(jīng)過中點H且對稱，且中點速度等于直線段速度。根據(jù)確定的加速度值可以解出：

為保證tb有解，需保證加速度足夠大，滿足：

擬合區(qū)域加速度越大，拋物線長度越小，直線段長度越大；當速度增大到一定程度時，拋物線長度減小為0，路徑變?yōu)橐粭l直線[10]。

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

笛卡爾空間軌跡規(guī)劃是根據(jù)已知的末端執(zhí)行器位姿矩陣，利用逆運動學(xué)求解關(guān)節(jié)角度。主要方法有直線插補法和圓弧插補法[15]。

3.2.1 直線插補

直線插補是已知機器人末端的起始點和終止點位姿，求解各路徑點位姿。以圖3為例，已知起始點坐標和終止點坐標，機器人以速度v沿直線運動，插補時間間隔為t?？梢酝ㄟ^計算直線軌跡長度，時間間隔t內(nèi)機器人運動距離，插補總次數(shù)和機器人沿各坐標軸的運動增量，得到各插補點坐標值為：

(10)

再將坐標值轉(zhuǎn)化為關(guān)節(jié)變量，利用關(guān)節(jié)空間中的插補方法進行軌跡規(guī)劃。

圖3 笛卡爾空間直線插補Fig.3 Linear interpolation of Cartesian space

3.2.2 圓弧插補

1) 平面圓弧插補

圖4為平面圓弧插補原理圖，假設(shè)機器人末端以速度v沿圓弧軌跡運動，t為插補時間間隔，可以求得由已知平面內(nèi)不共線的3點確定的圓弧半徑R；由總圓心角θi+1=θi+Δθ，時間間隔t內(nèi)角位移量和總插補步數(shù)N，由式(11)可推算出路徑中任意插補點坐標。

2) 空間圓弧插補

如圖5所示，已知不共線3點確定一半徑為R的圓弧，與坐標面相交于直線AB，BC,CA。

圖4 平面圓弧插補原理圖Fig.4 Plane arc interpolation schematic

(11)

設(shè)ZR軸與Z0軸夾角為α，XR軸與X0軸夾角為θ，圓心OR在基坐標系下的坐標為(XO，YO，ZO)，則圓弧坐標系向基坐標系過渡的轉(zhuǎn)換矩陣Tr為：

(12)

圖5 空間圓弧與基坐標系的關(guān)系Fig.5 Relationship between space arc and base coordinate system

基坐標系中的坐標值向圓弧坐標系轉(zhuǎn)換可由Tr的逆矩陣表示：

(13)

4 仿真驗證

本文基于MATLAB中的Robotics toolbox進行仿真，該工具箱涵蓋了很多機器人研究中的重要函數(shù)，為研究機器人的運動學(xué)、軌跡規(guī)劃等問題及圖形仿真實驗提供了支持[16]。

基于本文仿真可以驗證機器人運動學(xué)正解和逆解的正確性，確定軌跡規(guī)劃的較優(yōu)空間，比較多項式插值法的不同次數(shù)時軌跡、速度和加速度的差異。

4.1 機械臂運動學(xué)仿真

首先建立數(shù)學(xué)模型，運用Link函數(shù)結(jié)合D-H參數(shù)對各桿件建模；再運用SeriaLink函數(shù)根據(jù)各桿件之間的關(guān)系，對模型命名，運行程序可得到圖6模型。

圖6 工業(yè)機器人模型Fig.6 Industrial robot model

正向運動學(xué)仿真是利用工具箱中的fkine函數(shù)，

T=fkine(robot,[0 0 0 0 0 0]),

得到運動學(xué)矩陣T：

逆向運動學(xué)仿真需調(diào)用工具箱中的ikine函數(shù)，已知機器人末端位姿求解關(guān)節(jié)角度Q，Q=ikine(robot,T)，代入正向運動學(xué)位姿T可以求得機器人在該位姿下的六軸關(guān)節(jié)角度值Q：

Q=1.0×10-16[-0.893 3 0 0 -0.446 6 0 -0.446 6]

可以看出仿真結(jié)果與逆向運動學(xué)方程求解一致。

4.2 軌跡規(guī)劃仿真

4.2.1 機器人工具箱軌跡規(guī)劃函數(shù)仿真

在關(guān)節(jié)空間中,使用jtraj函數(shù)進行軌跡規(guī)劃，規(guī)定起始點和終止點的關(guān)節(jié)角度和軌跡點數(shù)，使用正向運動學(xué)函數(shù)，運行程序即可得到如圖7所示軌跡模型。

圖7 jtraj函數(shù)生成軌跡模型Fig.7 Trajectory model using jtraj function

在笛卡爾空間中,使用ctraj函數(shù)進行軌跡規(guī)劃，規(guī)定起始點和終止點位姿和步長，即可得到如圖8所示的直線軌跡；若先進行逆運動學(xué)求解出關(guān)節(jié)角度，再用jtraj函數(shù)進行軌跡規(guī)劃，可以得到圖10所示的曲線軌跡，機器人末端在X,Y,Z方向上的位置變化如圖9和圖11所示。

圖8 ctraj函數(shù)生成的直線軌跡Fig.8 Straight track using ctraj function

圖9 直線軌跡機器人末端位置變化圖Fig.9 Linear path robot end position change diagram

圖10 逆向運動學(xué)和jtraj函數(shù)規(guī)劃軌跡Fig.10 Trajectory planning using reverse kinematics and jtraj function

圖11 曲線軌跡機器人末端位置變化圖Fig.11 Curve path robot end position change diagram

在機器人工具箱中利用兩種函數(shù)進行軌跡規(guī)劃，可以看出在笛卡爾空間中可以較好觀察機器人的實時規(guī)劃動態(tài)，但計算量較大且機構(gòu)具有奇異性；而在關(guān)節(jié)空間中可以控制關(guān)節(jié)速度和加速度，并對關(guān)節(jié)力矩進行約束，隨時調(diào)整軌跡，可有效避免機器人冗余度和機構(gòu)奇異性問題。因此本文設(shè)計標準路徑時選擇在關(guān)節(jié)空間中進行規(guī)劃。

4.2.2 多項式插值法軌跡仿真

設(shè)定起始點和終止點的角度、速度和時間等參數(shù)，解算出三次多項式和五次多項式，編程繪制軌跡。圖12所示為三次多項式和五次多項式插值法所得軌跡規(guī)劃。由圖12可以看出：三次多項式插值法所得軌跡規(guī)劃雖然位置曲線連續(xù)平滑，但速度和加速度曲線明顯不平滑，且在路徑點處加速度會發(fā)生突變，對機器人運動產(chǎn)生沖擊，損壞電機和減速器；采取同樣的時間參數(shù)對五次多項式軌跡規(guī)劃進行仿真，其速度和加速度曲線的平滑性和穩(wěn)定性明顯優(yōu)于三次多項式插值法。

圖12 多項式插值法軌跡規(guī)劃Fig.12 Trajectory planning by polynomial interpolation

以機器人關(guān)節(jié)1～6為例，設(shè)起始點關(guān)節(jié)角度為Q1=[-π/12π/3π/60-π/6-π/12]，終止點關(guān)節(jié)角度為Q2=[π/4-π4π/12π-π/33π/4],分別采用三次多項式插值法和五次多項式插值法進行軌跡規(guī)劃，得到的位置、速度、加速度曲線如圖13～圖15所示。

圖13 多項式軌跡規(guī)劃的位置曲線Fig.13 Position curve of polynomial trajectory planning

圖14 多項式軌跡規(guī)劃的速度曲線Fig.14 Speed curve of polynomial trajectory planning

圖15 多項式軌跡規(guī)劃的加速度曲線Fig.15 Acceleration curve of polynomial trajectory planning

由圖可以看出：在多項式插值法中多項式次數(shù)越高，各關(guān)節(jié)的位置、速度和加速度曲線越平滑連續(xù)，可以有效避免激光跟蹤儀的丟光擋光等問題，也能夠增加視覺測量設(shè)備測量的實時性。

5 總結(jié)與展望

本文以工業(yè)機器人為研究對象，建立D-H坐標系和參數(shù)模型，推算了機器人兩連桿間的位姿變換矩陣和正向、逆向運動方程，使用MATLAB Robotics Toolbox進行了仿真驗證；分析研究了在關(guān)節(jié)空間中的三次和五次多項式插值法，以及在笛卡爾空間中的直線插補和圓弧插補，并在機器人工具箱中對不同軌跡規(guī)劃進行仿真，模擬機器人各關(guān)節(jié)在運動過程中的變化，總結(jié)不同軌跡對測量設(shè)備產(chǎn)生的影響，得出在設(shè)計標準路徑時選擇在關(guān)節(jié)空間中采用高次多項式插值法的初步結(jié)論。后續(xù)還需進一步研究在關(guān)節(jié)空間中優(yōu)于高次多項式插值法的軌跡規(guī)劃方法，保證測量設(shè)備的連續(xù)性和實時性，為進行測量設(shè)備的溯源技術(shù)研究提供理論基礎(chǔ)。