UP50工業(yè)機(jī)器人加工大直徑螺紋的運(yùn)動(dòng)學(xué)分析

張永貴，鄒 琰，黃中秋，程 兵

(蘭州理工大學(xué)機(jī)電工程學(xué)院，甘肅 蘭州 730050)

工業(yè)機(jī)器人是現(xiàn)代工廠實(shí)現(xiàn)生產(chǎn)自動(dòng)化、信息化的重要裝備，可用于焊接、噴涂和搬運(yùn)等作業(yè)。如果在工業(yè)機(jī)器人末端裝上相關(guān)驅(qū)動(dòng)裝置和刀具，即可代替機(jī)床的加工進(jìn)給運(yùn)動(dòng)，這將有助于解決傳統(tǒng)機(jī)床在施工現(xiàn)場加工大型零部件時(shí)存在的裝夾困難、運(yùn)輸成本高、耗費(fèi)時(shí)間長等問題。如大直徑螺紋[1]在施工現(xiàn)場損壞后，如運(yùn)用機(jī)器人可實(shí)現(xiàn)現(xiàn)場直接加工修復(fù)，那么以上的問題將會(huì)得到較好的解決。本文以機(jī)器人在不同進(jìn)給速度下，加工不同大直徑螺紋為實(shí)例，分析總結(jié)機(jī)器人各關(guān)節(jié)在關(guān)節(jié)空間的變化情況，以便為進(jìn)行關(guān)節(jié)空間螺旋軌跡規(guī)劃獲得更好的基礎(chǔ)條件。

由于現(xiàn)在大部分工業(yè)機(jī)器人采用的插補(bǔ)方式是直線插補(bǔ)和圓弧插補(bǔ)，很少采用螺旋插補(bǔ)，因此可以借鑒工業(yè)機(jī)器人在笛卡爾空間的軌跡規(guī)劃方法和數(shù)控機(jī)床螺旋插補(bǔ)原理，對螺旋線進(jìn)行插入節(jié)點(diǎn)分割，在保證一定精度要求的前提下，在兩個(gè)分割節(jié)點(diǎn)之間運(yùn)用機(jī)器人現(xiàn)有插補(bǔ)方式進(jìn)行插補(bǔ)，以達(dá)到運(yùn)用工業(yè)機(jī)器人進(jìn)行螺旋插補(bǔ)的目的。

1 工業(yè)機(jī)器人Motoman UP50模型的建立

1.1 機(jī)器人運(yùn)動(dòng)學(xué)正解

采用D-H法[2]建立機(jī)器人的運(yùn)動(dòng)學(xué)方程。定義機(jī)器人不同位姿時(shí)，坐標(biāo)系{i+1}與坐標(biāo)系{i}的變換矩陣為：

(1)

式中：zi為坐標(biāo)系{i}中的z軸；θi為機(jī)器人第i個(gè)關(guān)節(jié)角；βi，di，αi，ai為機(jī)器人零位時(shí)的連桿參數(shù)值。本文采用后置坐標(biāo)系方法[3]建立機(jī)器人坐標(biāo)系，如圖1所示，得到的Motoman UP50對應(yīng)連桿參數(shù)[4]見表1。

圖1 D-H坐標(biāo)系 表1 機(jī)器人連桿參數(shù)表

αi/(°)βi/(°)ai/mmdi/mmθi/(°)-90-901450θ118008700θ2-9001100θ39000-1025θ4-90000θ518000-175θ6

表中：αi為zi-1軸繞xi-1軸旋轉(zhuǎn)至zi軸時(shí)的扭角，正負(fù)通過右手法則判斷；βi為機(jī)器人在零位時(shí)各關(guān)節(jié)的關(guān)節(jié)角度；ai為zi-1軸與zi軸之間的公垂線長度，正向與xi一致；di為xi-1軸沿zi軸移動(dòng)至xi軸時(shí)的位移，正向與zi一致；θi為xi-1軸繞zi軸旋轉(zhuǎn)至xi時(shí)的轉(zhuǎn)角，正負(fù)通過右手法則判斷。

將表1的連桿參數(shù)代入式(1)，得到各連桿變換矩陣：

式中c1為cosθ1，s1為sinθ1，以此類推。

根據(jù)以上連桿變換，即可推導(dǎo)機(jī)器人末端坐標(biāo)系與基坐標(biāo)系之間的變換矩陣，如式(2)所示：

(2)

即：

(3)

式中：p為剛體在固定坐標(biāo)系中位置的齊次坐標(biāo)表示形式；n，o，a為剛體坐標(biāo)系的軸和固定坐標(biāo)系的軸之間的方向余弦，并用齊次坐標(biāo)形式的(4×1)列陣分別表示。式(3)即為UP50型機(jī)器人運(yùn)動(dòng)學(xué)的正解。

1.2 機(jī)器人運(yùn)動(dòng)學(xué)逆解

求解機(jī)器人運(yùn)動(dòng)學(xué)逆解的方法有很多，在此應(yīng)用代數(shù)法[5]來求解Motoman UP50工業(yè)機(jī)器人運(yùn)動(dòng)學(xué)逆解。

θ1=arctan[2(175ay-py,175ax-px)]

(4)

θ2=arctan[2(A2,B2)]-arctan{2[C2/ρ,

(5)

式(5)中正負(fù)號對應(yīng)于θ2的2個(gè)解，在求解具體工業(yè)機(jī)器人運(yùn)動(dòng)學(xué)逆解時(shí)，可通過數(shù)據(jù)驗(yàn)證確定正負(fù)號[6]。

θ3=arctan[2(110B3+1 025A3,110A3-1 025B3)]

(6)

θ4=arctan[2(B4,A4)]

(7)

θ5=arctan[2(-A4,175B5c4)]

(8)

θ6=arctan[2(B6,A6)]

(9)

2 普通螺旋線

設(shè)螺旋線的半徑r=50mm，螺距p=6mm(以下如不特殊說明，都是以mm為單位)，螺旋傾角為ψ0。假設(shè)螺旋線軸線與工件坐標(biāo)系{W}的z軸重合，且起始點(diǎn)從其x軸開始，螺旋線的空間坐標(biāo)原點(diǎn)為(xt，yt，zt)=(0，0，0)，則螺旋線在工件坐標(biāo)系{W}中的參數(shù)方程[7]如式(10)所示。

(10)

式中：φ為參數(shù)變量。

螺旋傾角ψ0可由式(11)得出：

(11)

φ從0到p變化一次，生成一個(gè)螺距的螺旋線。運(yùn)用MATLAB可顯示在基坐標(biāo)系{o}下的螺旋線圖形，如圖2所示。

圖2 螺旋線

3 傾斜螺旋線各節(jié)點(diǎn)的位姿

3.1 截取節(jié)點(diǎn)

螺旋線是機(jī)器人末端執(zhí)行器的運(yùn)動(dòng)路徑，如果要對螺旋線軌跡進(jìn)行規(guī)劃，就必須在螺旋線上截取一系列的路徑節(jié)點(diǎn)，而且這些節(jié)點(diǎn)的位姿需用齊次矩陣的形式表示出來。

采用折線擬合螺旋線的方法[8]，向xoy平面投影(如圖3所示)。設(shè)步進(jìn)角為αc，控制誤差為δmax(普通數(shù)控加工誤差可取0.01mm)[9]。

在xoy平面上：

即：

αc≤2·arccos(1-δmax/r)

(12)

根據(jù)步進(jìn)角選取節(jié)點(diǎn)，節(jié)點(diǎn)數(shù)為：

n=ceil(2·π/αc)

式中：ceil為MATLAB函數(shù)。向正無窮取整得到的n為158。

圖3 投影圖

3.2 節(jié)點(diǎn)姿態(tài)和位置

為了得到更精確的分析結(jié)果，取3個(gè)螺距的螺旋線進(jìn)行分析、研究。

3.2.1節(jié)點(diǎn)位置

由上可知，在螺旋線上截取的節(jié)點(diǎn)數(shù)為n，則螺旋線方程式(10)中變量φ的步進(jìn)為p/n。現(xiàn)假設(shè)φ從0每隔p/n取點(diǎn)，到3p為止，然后通過螺旋線方程式(10)可得各節(jié)點(diǎn)在工件坐標(biāo)系的位置坐標(biāo)(xW(i),yW(i),zW(i))。機(jī)器人末端坐標(biāo)系原點(diǎn)的齊次矩陣序列TW(:,:,i)如式(13)所示：

(13)

(14)

3.2.2節(jié)點(diǎn)姿態(tài)

假設(shè)在開始控制機(jī)器人時(shí)，機(jī)器人末端刀具上空間直角坐標(biāo)系3個(gè)坐標(biāo)軸的方向與參考坐標(biāo)系各坐標(biāo)軸的方向?qū)?yīng)一致。已知刀具旋轉(zhuǎn)軸線與關(guān)節(jié)6軸線夾角為90°，末端坐標(biāo)系yoz平面是砂輪所在平面，末端坐標(biāo)系的x軸方向是電主軸所在方向。具體變換如下：

1)空間直角參考坐標(biāo)系固定不動(dòng)，使與機(jī)器人末端刀具固接的空間直角坐標(biāo)系繞空間直角參考坐標(biāo)系的y軸旋轉(zhuǎn)-π/2，這樣會(huì)讓機(jī)器人的末端坐標(biāo)系軸x與空間直角參考坐標(biāo)系的z軸重合，此時(shí)呈豎直向上的形狀。

2)將與機(jī)器人末端執(zhí)行器固接的直角坐標(biāo)系再繞空間直角參考坐標(biāo)系的z軸旋轉(zhuǎn)π，這樣會(huì)讓機(jī)器人的末端坐標(biāo)系z軸方向與空間直角參考坐標(biāo)系的x軸重合。

4)以上已經(jīng)假設(shè)過此空間螺旋線的傾角為ψ0，因此使機(jī)器人末端坐標(biāo)系繞空間螺旋軸k旋轉(zhuǎn)ψ0，最終可得到機(jī)器人末端坐標(biāo)系姿態(tài)矩陣Tr。

經(jīng)過以上4步變換后，可以得到與機(jī)器人末端執(zhí)行器固接的空間直角坐標(biāo)系姿態(tài)矩陣Tr。調(diào)用MATLAB函數(shù)angvec2tr()可得節(jié)點(diǎn)姿態(tài)矩陣Tr，具體表達(dá)式如式(15)所示：

Tr=angvec2tr(ψ0,k(i,:))·Rot(x,π/6)·Rot(z,π)·Rot(y,-π/2)

(15)

式中空間螺旋軸k是在空間螺旋線上隨著機(jī)器人末端位置改變而不斷變化的單位方向向量(機(jī)器人末端沿螺旋線上升過程中，出現(xiàn)在每個(gè)節(jié)點(diǎn)所處圓平面上的圓半徑方向，方向朝里)，k的數(shù)學(xué)計(jì)算公式在MATLAB中可用數(shù)學(xué)函數(shù)reshape()[10]得到。具體表達(dá)式如式(16)所示：

zeros(1,3n+1)],3n+1,3}

(16)

由此可知，k是一個(gè)(3n+1)×3階矩陣，矩陣的每一個(gè)行向量代表對應(yīng)節(jié)點(diǎn)的螺旋軸單位方向向量。

最后，在得到節(jié)點(diǎn)位置矩陣和姿態(tài)矩陣后，運(yùn)用MATLAB循環(huán)程序?qū)烧呷诤铣晒?jié)點(diǎn)位姿矩陣T(:,:,i)，矩陣T是4×4×(3n+1)階矩陣，即(3n+1)個(gè)節(jié)點(diǎn)在基坐標(biāo)系{o}的位姿矩陣序列，節(jié)點(diǎn)位置如圖4所示。

圖4 節(jié)點(diǎn)位置

4 驗(yàn)證機(jī)器人模型

因?yàn)楸緦?shí)驗(yàn)取3個(gè)螺距的螺旋線進(jìn)行分析、研究，所以總節(jié)點(diǎn)數(shù)為3×158+1=475。

取第一個(gè)節(jié)點(diǎn)來驗(yàn)證機(jī)器人模型正、逆解準(zhǔn)確性，并確定唯一逆解。由以上MATLAB程序可得前兩個(gè)點(diǎn)的位姿序列如下：

求節(jié)點(diǎn)1在關(guān)節(jié)空間對應(yīng)的位移向量。在式(5)的計(jì)算中，分別取負(fù)號、正號，可得：

將q1代入式(3)中，得

將q2代入式(3)中，得

經(jīng)過比較發(fā)現(xiàn)：取正號時(shí)，求解的位姿與實(shí)際位姿相差非常大；而取負(fù)號時(shí)，誤差則非常小。綜上可知：在求解θ2的式(5)中，取負(fù)號滿足計(jì)算要求。

5 螺旋線各節(jié)點(diǎn)的位移、速度、加速度向量圖像

5.1 各節(jié)點(diǎn)關(guān)節(jié)空間的位移向量

對運(yùn)動(dòng)學(xué)逆解表達(dá)式(4)～(9)進(jìn)行編程，可得475個(gè)節(jié)點(diǎn)在關(guān)節(jié)空間的位移向量。通過MATLAB可分別顯示6個(gè)關(guān)節(jié)在各節(jié)點(diǎn)關(guān)節(jié)角的連線，使6個(gè)關(guān)節(jié)在關(guān)節(jié)空間的關(guān)節(jié)角變化更直觀，如圖5所示。

圖5 關(guān)節(jié)角

5.2 各節(jié)點(diǎn)關(guān)節(jié)空間的速度向量

關(guān)節(jié)速度求解公式，如式(17)所示：

(17)

式中：J(q)為機(jī)器人雅克比矩陣，是6×n的偏導(dǎo)數(shù)矩陣。

工業(yè)機(jī)器人末端在工作空間的速度表示為：

設(shè)機(jī)器人末端在z軸方向?yàn)楹闼伲?/p>

vz=0.2mm/s

(18)

則第i個(gè)節(jié)點(diǎn)的其余速度分量可表示為：

vx(i)=wz(i)·r·sin[(i-1)·αc]

vy(i)=wz(i)·r·cos[(i-1)·αc]

用矢量積法計(jì)算Motoman UP50工業(yè)機(jī)器人的速度雅克比矩陣，并用特殊點(diǎn)驗(yàn)證雅克比矩陣的準(zhǔn)確性。

計(jì)算J內(nèi)元素的方法相同，本文只給出J1各元素：

J=[J1J2J3J4J5J6]

J1=[j11j21j31j41j51j61]

(19)

j11=-145s1-870s1s2-110s1s(2-3)-1 025s1c(2-3)-175s1c(2-3)c5-175c1s4s5-175s1s(2-3)c4s5,j21=145c1+870c1s2+110c1s(2-3)+1 025c1c(2-3)+175c1c(2-3)c5-175s1s4s5+175c1s(2-3)c4s5,j31=0,j41=0,j51=0,j61=1。

為了驗(yàn)證得到的速度雅克比的準(zhǔn)確性，計(jì)算q=[0 0 0 0 0 π/2 0]時(shí)的機(jī)器人雅克比，結(jié)果為：

如J的第一列為[0 1 170×dθ10 0 0 dθ1]，則表示在位姿q=[0 0 0 0 π/2 0]時(shí)，關(guān)節(jié)1的微分轉(zhuǎn)動(dòng)dθ1反映到末端在基坐標(biāo)系下的微分運(yùn)動(dòng)為[0 0 0 0 π/2 0]，由此可知速度雅克比是準(zhǔn)確的。

已知速度雅克比矩陣、操作空間的機(jī)器人末端速度，可獲得475個(gè)節(jié)點(diǎn)在關(guān)節(jié)空間的速度向量。通過MATLAB可分別顯示6個(gè)關(guān)節(jié)在各節(jié)點(diǎn)角速度的連線，使6個(gè)關(guān)節(jié)在關(guān)節(jié)空間的角速度變化更加直觀。

圖6 關(guān)節(jié)角速度

5.3 各節(jié)點(diǎn)關(guān)節(jié)空間加速度向量

機(jī)器人末端加速度方程為：

(20)

其中：

(21)

式中：Hmn為雅克比矩陣J的第m行n列元素Jmn對q的偏導(dǎo)數(shù)，為6×1矩陣。

引進(jìn)矩陣層如下所示：

(22)

稱H為二階系數(shù)矩陣，并規(guī)定

(23)

結(jié)合式(20)～(23)有：

(24)

根據(jù)式(24)可算得Hmn，最后可得二階系數(shù)矩陣H。

對式(24)編程，求得475個(gè)節(jié)點(diǎn)在關(guān)節(jié)空間的加速度向量。通過MATLAB可分別顯示6個(gè)關(guān)節(jié)在各節(jié)點(diǎn)角加速度的連線，使6個(gè)關(guān)節(jié)在關(guān)節(jié)空間的角加速度變化更加直觀，如圖7所示。

5.4 不同加工速度加工不同直徑螺紋

采用上述計(jì)算、仿真過程，得到以不同速度vz加工不同直徑螺紋的全部結(jié)果。例如：用vz=0.1mm/s的速度分別加工直徑64mm～280mm的螺紋，其他亦是如此。

vz分別取0.1mm/s、0.2mm/s、0.4mm/s、0.5mm/s、0.8mm/s、2mm/s、3mm/s、4mm/s、5mm/s。

圖7 關(guān)節(jié)角加速度

普通大直徑螺紋參數(shù)見表2。

表2 螺紋參數(shù) mm

6 結(jié)論

1)相同的速度(vz)下，隨著加工螺紋直徑的增大，各關(guān)節(jié)位移變化幅度增大，速度變化幅度增大，加速度變化幅度減小。

2)相同加工直徑下，隨著加工速度的增大，各關(guān)節(jié)速度增大，加速度也增大。

3)機(jī)器人有針對地修復(fù)加工大直徑螺紋，可大大提高其加工精度和效率。

參考文獻(xiàn)：

[1] 彭成勇，樓一珊，曹元平，等.鉆鋌螺紋連接疲勞失效研究[J].石油鉆探技術(shù)，2006，34(6)：20-22.

[2] HAYATI S A. Robot arm geometric link parameter estimation[C]//Decision and Control. [S.l.]:IEEE,1983:1477-1483.

[3] 龔振邦，王勤愨. 機(jī)器人機(jī)械設(shè)計(jì)[M]. 北京：電子工業(yè)出版社,1995:44-111.

[4] 熊有倫. 機(jī)器人技術(shù)基礎(chǔ)[M]. 武漢：華中科技大學(xué)出版社,1996:15-30,94-115.

[5] MAYER G E. Differential kinematic control equations for simple manipulators[J]. IEEE Translation on Systems,Man,and Cybernetics,1981,11(6):456-460.

[6] 張永貴，劉晨榮，劉鵬.工業(yè)機(jī)器人運(yùn)動(dòng)學(xué)逆向建模[J]. 機(jī)械設(shè)計(jì)與制造,2014,11(3):123-126.

[7] 羅良玲，劉旭波.基于時(shí)間分割法的圓柱螺旋線直接插補(bǔ)算法[J].南昌大學(xué)學(xué)報(bào)(工科版),2001,23(4):57-59.

[8] 黃建. 數(shù)控加工空間運(yùn)動(dòng)平滑路徑的規(guī)劃[D]. 揚(yáng)州：揚(yáng)州大學(xué)，2013：17.

[9] 陳蕭. 大口徑螺紋加工的分析和實(shí)現(xiàn)[J].南通職業(yè)大學(xué)學(xué)報(bào)，2009，23(2)：98-101.

[10] 陳富久. 基于Robotics Toolbox的工業(yè)機(jī)器人螺旋軌跡研究[D]. 蘭州：蘭州理工大學(xué)，2016：39-42.