黃宏俊 ，姜羽飛

(1.河南工程學(xué)院 工程訓(xùn)練中心，河南 鄭州 451191；2.鄭州理工職業(yè)學(xué)院 機(jī)電工程系，河南 鄭州441500)

工業(yè)機(jī)器人是科技進(jìn)步與發(fā)展的代表性成果，也是多學(xué)科、多領(lǐng)域的綜合產(chǎn)物。我國(guó)工業(yè)機(jī)器人的需求量約占全球的三分之一[1]。在工業(yè)機(jī)器人的研究中，最重要的是軌跡研究。早在1983年，Lin等[2]首次研究機(jī)器人在三次多項(xiàng)式下的軌跡優(yōu)化；1991年，Wang等[3]又研究出一種求解機(jī)械臂逆運(yùn)動(dòng)學(xué)問題的組合優(yōu)化方法；2015年，Gasparetto等[4]對(duì)機(jī)器人軌跡規(guī)劃算法給出了概述。

針對(duì)運(yùn)動(dòng)學(xué)軌跡的研究，各個(gè)領(lǐng)域都有學(xué)者在不斷探索。其中，關(guān)于工業(yè)機(jī)器人軌跡的研究一般分為兩類，一類是基于關(guān)節(jié)坐標(biāo)系來規(guī)劃工業(yè)機(jī)器人的軌跡，另一類則是在笛卡爾坐標(biāo)系下進(jìn)行工業(yè)機(jī)器人軌跡的規(guī)劃。唐越等[5]利用正向運(yùn)動(dòng)學(xué)對(duì)工業(yè)機(jī)器人進(jìn)行了關(guān)節(jié)空間軌跡規(guī)劃；王沐雨等[6]使用MATLAB Robotic工具箱對(duì)ABB IRB1600型工業(yè)機(jī)器人在關(guān)節(jié)坐標(biāo)系下的運(yùn)動(dòng)進(jìn)行了仿真研究；王曉明等[7]從逆向運(yùn)動(dòng)學(xué)方面對(duì)工業(yè)機(jī)器人進(jìn)行軌跡規(guī)劃并利用 MATLAB Robotic工具箱對(duì)機(jī)器人軌跡進(jìn)行了仿真；朱志偉等[8]對(duì)KUKA R1420型焊接機(jī)器人在關(guān)節(jié)空間中的焊接軌跡進(jìn)行了規(guī)劃；郭青陽[9]對(duì)KUKA焊接機(jī)器人定位誤差進(jìn)行分析并提出了補(bǔ)償方法。

目前，機(jī)器人軌跡研究已成為機(jī)器人研究的熱點(diǎn)，而類似于KUKA KR6型的六軸工業(yè)機(jī)器人卻在運(yùn)動(dòng)中存在軌跡誤差。因此，實(shí)際應(yīng)用中的軌跡往往與理論規(guī)劃的軌跡計(jì)算值不符，但利用MATLAB Robotic工具箱無法得到運(yùn)動(dòng)輸入與輸出變化?；谝陨蠁栴}，本研究利用Simscape Multibody對(duì)該類型工業(yè)機(jī)器人理想規(guī)劃軌跡進(jìn)行系統(tǒng)仿真，在可視化環(huán)境下檢測(cè)出仿真實(shí)際運(yùn)動(dòng)軌跡，對(duì)實(shí)際檢測(cè)軌跡與計(jì)算輸入的目標(biāo)軌跡之間的誤差規(guī)律進(jìn)行總結(jié)，并對(duì)生產(chǎn)提出了合理建議。

1 機(jī)器人運(yùn)動(dòng)學(xué)建模分析

對(duì)KUKA KR6型工業(yè)機(jī)器人進(jìn)行三維建模，模型如圖1所示。該機(jī)器人有6個(gè)關(guān)節(jié)，同時(shí)也有6個(gè)連桿，由此可以看出該機(jī)器人具有六自由度。

在研究過程中，首先利用D-H法對(duì)工業(yè)機(jī)器人關(guān)節(jié)進(jìn)行數(shù)學(xué)建模。根據(jù)機(jī)器人各關(guān)節(jié)與各連桿的關(guān)系，在笛卡爾空間作出相應(yīng)的坐標(biāo)系，以關(guān)節(jié)軸線為Z軸，指向末端操作器方向?yàn)閄軸，通過右手定則來判斷Y軸，可以得到空間中的坐標(biāo)系，由下至上分別對(duì)應(yīng)每個(gè)關(guān)節(jié)連桿，其中X0、Y0、Z0坐標(biāo)系為基準(zhǔn)坐標(biāo)系，如圖2 所示。

圖1 六軸機(jī)器人三維模型Fig.1 6-axis robot 3D model

圖2 機(jī)器人坐標(biāo)系Fig.2 Robot coordinate system

根據(jù)各個(gè)連桿關(guān)節(jié)的相互關(guān)系建立D-H參數(shù)表，如表1所示。其中，θj表示關(guān)節(jié)扭角，aj表示連桿長(zhǎng)度，αj表示相鄰連桿間扭轉(zhuǎn)角，dj表示關(guān)節(jié)偏距，以順時(shí)針旋轉(zhuǎn)方向?yàn)檎较颉?/p>

表1 D-H參數(shù)Tab.1 D-H parameters

通過D-H法可得到各個(gè)關(guān)節(jié)之間的位置關(guān)系，繼而可進(jìn)行正向運(yùn)動(dòng)學(xué)推導(dǎo)。相鄰兩連桿之間的齊次變換矩陣為

(1)

式中：c代表余弦函數(shù)cos(θ)；s代表正弦函數(shù)sin(θ)；R為旋轉(zhuǎn)齊次變換矩陣；T為平移齊次變換矩陣；n-1Tn表示連桿坐標(biāo)系之間的齊次矩陣。

將D-H參數(shù)代入公式(1)中，經(jīng)過6次迭代可推導(dǎo)出末端操作器位姿。末端操作器位姿矩陣如下：

(2)

在公式(2)中，各個(gè)元素可分別對(duì)應(yīng)結(jié)果(結(jié)果中省略乘號(hào))：

nx=sθ6[cθ4(sθ1-cθ1sθ2)-sθ4(cθ1sθ2sθ3+cθ1cθ2cθ3)+cθ1cθ2sθ3-cθ1cθ3sθ2]-cθ6{sθ5(cθ1cθ2sθ3-

cθ1cθ3sθ2)-cθ5[cθ4(cθ1sθ2sθ3+cθ1cθ2cθ3)+sθ4(sθ1-cθ1sθ2)]}，

ox=sθ6{sθ5(cθ1cθ2sθ3-cθ1cθ3sθ2)-cθ5[cθ4(cθ1sθ2sθ3+cθ1cθ2cθ3)+sθ4(sθ1-cθ1sθ2)]}+

cθ6[cθ4(sθ1-cθ1sθ2)-sθ4(cθ1sθ2sθ3+cθ1cθ2cθ3)+cθ1cθ2sθ3-cθ1cθ3sθ2]，

ax=-sθ5[cθ4(cθ1sθ2sθ3+cθ1cθ2cθ3)+sθ4(sθ1-cθ1sθ2)]-cθ5(cθ1cθ2sθ3-cθ1cθ3sθ2)，

px=500cθ1-115sθ5[cθ4(cθ1sθ2sθ3+cθ1cθ2cθ3)+sθ4(sθ1-cθ1sθ2)]}-

115cθ5(cθ1cθ2sθ3-cθ1cθ3sθ2)-346cθ1cθ2sθ3+346cθ1cθ2sθ3+180cθ1sθ2sθ3+180cθ1cθ2cθ3，

ny=sθ6[cθ4(cθ1+sθ1sθ2)+sθ4(cθ2cθ3sθ1+sθ1scθ2sθ3)-cθ2sθ1sθ3+cθ3sθ2sθ1] -

cθ6{cθ5[sθ4(cθ1+sθ1sθ2)-cθ4(cθ2cθ3sθ1+sθ1scθ2sθ3)]+sθ5(cθ2sθ1sθ3-cθ3sθ1sθ2)}，

oy=sθ6{cθ5[sθ4(cθ1+sθ1sθ2)-cθ4(cθ2cθ3sθ1+sθ1scθ2sθ3)]+sθ5(cθ2sθ1sθ3-cθ3sθ1sθ2)}-

cθ6[cθ4(cθ1+sθ1sθ2)+sθ4(cθ2cθ3sθ1+sθ1sθ2sθ3)-cθ2sθ1sθ3+cθ3sθ1sθ2]，

αy=sθ5[sθ4(cθ1+sθ1sθ2)-cθ4(cθ2cθ3sθ1+sθ1sθ2sθ3)]-cθ5(cθ2sθ1sθ3-cθ3sθ1sθ2)，

py=115sθ5[sθ4(cθ1+sθ1sθ2)-cθ4(cθ2cθ3sθ1+sθ1sθ2sθθ3)]+500cθ2sθ1-

115cθ5(cθ2sθ1sθ3-cθ3sθ1sθ2)+180cθ2cθ3sθ1-346cθ2sθ1sθ3+346cθ3sθ1sθ2+180sθ1sθ2sθ3，

nz=cθ6[sθ5(cθ3sθ2-sθ2sθ3)+cθ4cθ5(cθ3sθ2+sθ2sθ3)]-sθ6[sθ4(cθ3sθ2+sθ2θ3)+cθ3sθ2-sθ2sθ3]，

oz=-sθ6[sθ5(cθ3sθ2-sθ2sθ3)+cθ4cθ5(cθ3sθ2+sθ2sθ3)]-cθ6[sθ4(cθ3sθ2+sθ2sθ3)+cθ3sθ2-sθ2sθ3]，

az=cθ5(cθ3sθ2-sθ2sθ3)-cθ4sθ5(cθ3sθ2+sθ2sθ3)，

pz=500sθ2+526cθ3sθ2-166sθ2sθ3+115cθ5(cθ3sθ2-sθ2sθ3)-115cθ4sθ5(cθ3sθ2+sθ2sθ3)+200。

2 運(yùn)動(dòng)軌跡規(guī)劃

2.1 設(shè)定軌跡起始點(diǎn)與軌跡目標(biāo)點(diǎn)

關(guān)節(jié)空間軌跡規(guī)劃的第一步是設(shè)定起始點(diǎn)與目標(biāo)點(diǎn)。軌跡規(guī)劃過程中關(guān)注的往往是末端操作器的運(yùn)動(dòng)軌跡，即末端操作器運(yùn)動(dòng)狀態(tài)的改變。如果在關(guān)節(jié)空間中進(jìn)行軌跡規(guī)劃，首先要設(shè)定末端操作器的起始點(diǎn)姿態(tài)與目標(biāo)點(diǎn)姿態(tài)，利用逆運(yùn)動(dòng)學(xué)方程解出每個(gè)關(guān)節(jié)的運(yùn)動(dòng)角度，從而將末端操作器在笛卡爾空間中的姿態(tài)變化問題轉(zhuǎn)化為關(guān)節(jié)空間中各個(gè)關(guān)節(jié)角度隨規(guī)定時(shí)間變化的問題。基于三角讀法表示出末端操作器在初始狀態(tài)下與目標(biāo)狀態(tài)下的位姿，其中參數(shù)a、b、c表示以ZXZ方式整合的歐拉角，參數(shù)x、y、z表示在笛卡爾坐標(biāo)系方向的位移，具體如表2所示。

表2 末端操作器初始狀態(tài)下與目標(biāo)狀態(tài)下的位姿Tab.2 End manipulator posture in initial and target states

2.2 軌跡規(guī)劃方程

對(duì)六自由度工業(yè)機(jī)器人進(jìn)行數(shù)學(xué)建模后，要求工業(yè)機(jī)器人末端操作器由一個(gè)初始位置點(diǎn)移動(dòng)到目標(biāo)位置點(diǎn)，各關(guān)節(jié)運(yùn)動(dòng)速度要求平滑。

本研究采用五次多項(xiàng)式插值對(duì)機(jī)器人運(yùn)動(dòng)過程中的參數(shù)進(jìn)行設(shè)置。在關(guān)節(jié)空間的運(yùn)動(dòng)規(guī)劃中，已知機(jī)器人的初始點(diǎn)末端位姿與目標(biāo)點(diǎn)末端位姿，計(jì)算初始末端位置對(duì)應(yīng)角度θ0與目標(biāo)末端位置對(duì)應(yīng)角度θf，再設(shè)定當(dāng)時(shí)間為0～tf時(shí)，應(yīng)用逆向運(yùn)動(dòng)學(xué)求得其余關(guān)節(jié)的角度，規(guī)劃出關(guān)節(jié)運(yùn)動(dòng)軌跡。同時(shí)，為使關(guān)節(jié)運(yùn)動(dòng)軌跡連續(xù)光滑，關(guān)節(jié)變量采用五次多項(xiàng)式進(jìn)行插值，軌跡規(guī)劃條件如下：

(3)

式中：θ(t)為關(guān)節(jié)角度變化函數(shù)。

基于初始條件式(3)，對(duì)運(yùn)動(dòng)初始位姿對(duì)應(yīng)的關(guān)節(jié)角度與運(yùn)動(dòng)目標(biāo)位置的關(guān)節(jié)角度軌跡進(jìn)行插值，設(shè)定一個(gè)以時(shí)間t為變量的五次多項(xiàng)式：

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

(4)

基于式(4)，可以繼續(xù)求導(dǎo)得到關(guān)節(jié)的速度與加速度方程：

(5)

基于初始條件式(3)，由式(4)和式(5)可推導(dǎo)出五次方程組特定矩陣形式：

(6)

根據(jù)式(6)，代入各個(gè)關(guān)節(jié)初始角度值和運(yùn)動(dòng)時(shí)間內(nèi)的角度值。設(shè)定運(yùn)動(dòng)時(shí)間為20 s，可得到規(guī)劃關(guān)節(jié)方程組：

(7)

圖3 MATLAB中的機(jī)器人模型Fig.3 Robot model in MATLAB

2.3 運(yùn)動(dòng)軌跡插值及圖像

已知運(yùn)動(dòng)軌跡起始點(diǎn)、目標(biāo)點(diǎn)與運(yùn)動(dòng)軌跡方程，本研究采用Peter Corke提供的MATLAB Robotic開源工具箱進(jìn)行數(shù)值運(yùn)算，得到相應(yīng)的軌跡規(guī)劃參數(shù)。

根據(jù)機(jī)器人數(shù)學(xué)模型及D-H參數(shù)，可以在機(jī)器人工具箱中建立對(duì)應(yīng)的機(jī)器人模型，如圖3所示。

建立模型后,在關(guān)節(jié)空間下還需要確定已知初始點(diǎn)與目標(biāo)點(diǎn)的空間角。根據(jù)表2可以得到工業(yè)機(jī)器人末端位姿,由于歐拉角旋轉(zhuǎn)按照ZXZ序列方式進(jìn)行，故在Robotic工具箱中使用內(nèi)置函數(shù)A=eul2r(a,b,c)，其中a、b、c為表2中的旋轉(zhuǎn)角度，并將轉(zhuǎn)換后的矩陣賦于變量A中，x、y、z為末端位姿原點(diǎn)坐標(biāo)。得到末端操作器在初始點(diǎn)與目標(biāo)點(diǎn)的位姿后，通過機(jī)器人逆向運(yùn)動(dòng)學(xué)可計(jì)算出各個(gè)關(guān)節(jié)的不同角度值，利用Robotic工具箱中自帶的求逆運(yùn)動(dòng)學(xué)解函數(shù)B=ikine(A)得到一組解且賦于變量B中，在該解中可得到初始關(guān)節(jié)角度。同理，可得末端關(guān)節(jié)角度。

由已知條件可得，初始各關(guān)節(jié)角度分別為θ1=π、θ2=π/2、θ3=-π/4、θ4=π、θ5=-π/2、θ6=0，目標(biāo)點(diǎn)各個(gè)關(guān)節(jié)角度為θ1=π/3、θ2=π/3、θ3=0、θ4=0、θ5=-π/3、θ6=π/2。

圖4 關(guān)節(jié)運(yùn)動(dòng)軌跡曲線Fig.4 Joint trajectory curve

最終在已知初始位置關(guān)節(jié)角度與末端位置關(guān)節(jié)角度下，可通過函數(shù)[q,qd,qdd]=jtarj(C,D,step)進(jìn)行計(jì)算,其中C、D分別為初始各關(guān)節(jié)角度與目標(biāo)位置各關(guān)節(jié)角度,step表示時(shí)間步數(shù)，q表示關(guān)節(jié)角度，qd表示關(guān)節(jié)角速度，qdd表示關(guān)節(jié)角加速度。MATLAB在進(jìn)行軌跡運(yùn)算時(shí)，只能用離散變量來代替連續(xù)變量，step越大，表示運(yùn)算中間點(diǎn)越多、迭代次數(shù)越多，同時(shí)軌跡方程曲線也就越光滑。

在確定的時(shí)間內(nèi)進(jìn)行仿真，時(shí)間步數(shù)選擇200，圖4為關(guān)節(jié)運(yùn)動(dòng)軌跡曲線，圖5為關(guān)節(jié)角速度在規(guī)定時(shí)間內(nèi)的變化曲線，圖6為關(guān)節(jié)角加速度的運(yùn)動(dòng)變化曲線。

圖5 關(guān)節(jié)角速度曲線Fig.5 Speed image of each join

圖6 關(guān)節(jié)加速度曲線Fig.6 Joint acceleration curve

3 運(yùn)動(dòng)軌跡仿真與誤差分析

3.1 模型可視化仿真

利用SolidWorks軟件建立三維模型后，將三維模型導(dǎo)入Simulink軟件，在Simulink軟件中自動(dòng)生成Multibody環(huán)境，在仿真模型中加入輸入/輸出模塊，具體如圖7所示。

圖7 Simscape Multibody環(huán)境下的系統(tǒng)仿真模型Fig.7 System simulation model under Simscape Multibody

輸入?yún)?shù)為6個(gè)關(guān)節(jié)位置角度隨時(shí)間變化的運(yùn)動(dòng)軌跡方程(7)，運(yùn)動(dòng)仿真時(shí)間設(shè)定為20 s，由于設(shè)定運(yùn)動(dòng)步數(shù)為200，可推導(dǎo)出每隔0.1 s對(duì)運(yùn)動(dòng)曲線進(jìn)行中間點(diǎn)插值，將已規(guī)劃好的5次方軌跡最終導(dǎo)入模型中，可經(jīng)過Simulink仿真觀測(cè)三維模型的運(yùn)動(dòng)軌跡。

圖8為由MTALAB Robotic工具箱計(jì)算的末端操作器在笛卡爾空間內(nèi)的運(yùn)動(dòng)軌跡，該軌跡在笛卡爾空間內(nèi)表示為一條直線。圖9為目標(biāo)位置下的Simscape Multibody模型窗口。經(jīng)Simscape Multibody可視化仿真后，模型基本符合運(yùn)動(dòng)軌跡。

圖8 笛卡爾空間仿真運(yùn)動(dòng)軌跡Fig.8 Cartesian space trajectory

圖9 Simscape Multibody模型窗口Fig.9 Simscape Multibody model window

3.2 運(yùn)動(dòng)軌跡誤差分析

圖10為規(guī)劃軌跡與仿真軌跡的誤差帶，關(guān)節(jié)曲線代表理論計(jì)算規(guī)劃的軌跡與利用Simscape Multibody模塊仿真出的軌跡。

圖11為軌跡誤差的變化曲線。由圖11可知，軌跡誤差隨時(shí)間的變化呈拋物線，每個(gè)軌跡從起始位置到目標(biāo)位置附近時(shí)，軌跡運(yùn)動(dòng)的誤差非常小，說明關(guān)節(jié)擾動(dòng)量很小，在5～15 s擾動(dòng)量最大，而在起始與終止的時(shí)間段，誤差逐漸減少。這個(gè)過程中的關(guān)節(jié)運(yùn)動(dòng)誤差見表3。

圖10 規(guī)劃軌跡與仿真軌跡的誤差帶Fig.10 Error graph of planned and simulated trajectory

圖11 軌跡誤差變化曲線Fig.11 Histogram of trajectory errors

表3 關(guān)節(jié)運(yùn)動(dòng)誤差Tab.3 Joint motion errors

3.3 實(shí)測(cè)誤差分析

仿真模型為理想狀態(tài)下機(jī)器人的運(yùn)動(dòng)軌跡，而實(shí)際工作環(huán)境下機(jī)器人的運(yùn)動(dòng)軌跡與仿真模型的運(yùn)動(dòng)軌跡存在誤差，基于KUKA工作臺(tái)對(duì)機(jī)器人運(yùn)動(dòng)時(shí)的軌跡點(diǎn)進(jìn)行取樣，利用示教器進(jìn)行編程，使得實(shí)際工作環(huán)境下的機(jī)器人在關(guān)節(jié)空間下進(jìn)行PTP運(yùn)動(dòng)。

圖12是各個(gè)關(guān)節(jié)實(shí)際誤差的4次方擬合曲線。經(jīng)過實(shí)測(cè)可得出，機(jī)器人在關(guān)節(jié)空間下運(yùn)動(dòng)時(shí)始終存在誤差。在機(jī)器人進(jìn)行PTP運(yùn)動(dòng)時(shí)，由于機(jī)器人在標(biāo)定過程中關(guān)注于末端點(diǎn)位置，于是產(chǎn)生一定的標(biāo)定誤差，又因?yàn)檫\(yùn)動(dòng)軌跡中的誤差先增加后又通過自帶算法補(bǔ)償，故使得后半段運(yùn)動(dòng)過程中的誤差減少。誤差值與仿真誤差不同，但誤差規(guī)律基本相同。

圖12 各關(guān)節(jié)實(shí)際誤差的4次方擬合曲線Fig.12 The quadruplicate fitting curve of the actual error of each joint

4 結(jié)語

本研究以KUKA KR6機(jī)器人為原型，對(duì)機(jī)器人進(jìn)行關(guān)節(jié)軌跡規(guī)劃，利用MATLAB Robotic開源工具箱計(jì)算規(guī)劃數(shù)值，最終在Simscape Multibody中進(jìn)行仿真。在仿真結(jié)果中可以實(shí)時(shí)看到模型在計(jì)算軌跡中的動(dòng)態(tài)變化，并可得到實(shí)際軌跡與計(jì)算輸入軌跡的誤差，即在運(yùn)動(dòng)到中間點(diǎn)附近時(shí)軌跡誤差值最大，而初始點(diǎn)與末端點(diǎn)附近的誤差值最小。在實(shí)際運(yùn)動(dòng)過程中，誤差隨時(shí)間變化而變化，在運(yùn)動(dòng)到中間時(shí)間點(diǎn)時(shí)，誤差值最大。

在實(shí)際應(yīng)用中，機(jī)器人在由起始點(diǎn)到終止點(diǎn)的運(yùn)動(dòng)過程中，存在標(biāo)定誤差、零點(diǎn)誤差、機(jī)械公差、減速器誤差等一系列誤差。為保證機(jī)器人運(yùn)動(dòng)軌跡精確，對(duì)于短距離運(yùn)動(dòng)而言，這些軌跡誤差很小、可適當(dāng)忽略，而對(duì)于長(zhǎng)距離且需要高精度軌跡的情況，有兩種方式可減少誤差，一種是改善算法、增加標(biāo)定點(diǎn)以實(shí)時(shí)追蹤運(yùn)動(dòng)軌跡，另一種是在編程過程中增加中間點(diǎn)來進(jìn)行過渡，使運(yùn)動(dòng)軌跡更加清晰，這樣可以最大限度地減少運(yùn)動(dòng)過程中的誤差、保證運(yùn)動(dòng)軌跡的精確。