UP50工業(yè)機器人加工大直徑螺紋的運動學(xué)分析

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

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

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

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

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

1.1 機器人運動學(xué)正解

采用D-H法[2]建立機器人的運動學(xué)方程。定義機器人不同位姿時，坐標系{i+1}與坐標系{i}的變換矩陣為：

(1)

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

圖1 D-H坐標系 表1 機器人連桿參數(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軸時的扭角，正負通過右手法則判斷；βi為機器人在零位時各關(guān)節(jié)的關(guān)節(jié)角度；ai為zi-1軸與zi軸之間的公垂線長度，正向與xi一致；di為xi-1軸沿zi軸移動至xi軸時的位移，正向與zi一致；θi為xi-1軸繞zi軸旋轉(zhuǎn)至xi時的轉(zhuǎn)角，正負通過右手法則判斷。

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

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

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

(2)

即：

(3)

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

1.2 機器人運動學(xué)逆解

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

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

(4)

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

(5)

式(5)中正負號對應(yīng)于θ2的2個解，在求解具體工業(yè)機器人運動學(xué)逆解時，可通過數(shù)據(jù)驗證確定正負號[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è)螺旋線軸線與工件坐標系{W}的z軸重合，且起始點從其x軸開始，螺旋線的空間坐標原點為(xt，yt，zt)=(0，0，0)，則螺旋線在工件坐標系{W}中的參數(shù)方程[7]如式(10)所示。

(10)

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

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

(11)

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

圖2 螺旋線

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

3.1 截取節(jié)點

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

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

在xoy平面上：

即：

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

(12)

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

n=ceil(2·π/αc)

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

圖3 投影圖

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

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

3.2.1節(jié)點位置

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

(13)

(14)

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

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

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

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

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

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

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

(15)

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

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

(16)

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

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

圖4 節(jié)點位置

4 驗證機器人模型

因為本實驗取3個螺距的螺旋線進行分析、研究，所以總節(jié)點數(shù)為3×158+1=475。

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

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

將q1代入式(3)中，得

將q2代入式(3)中，得

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

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

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

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

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

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

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

(17)

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

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

設(shè)機器人末端在z軸方向為恒速：

vz=0.2mm/s

(18)

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

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

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

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

計算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。

為了驗證得到的速度雅克比的準確性，計算q=[0 0 0 0 0 π/2 0]時的機器人雅克比，結(jié)果為：

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

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

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

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

機器人末端加速度方程為：

(20)

其中：

(21)

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

引進矩陣層如下所示：

(22)

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

(23)

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

(24)

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

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

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

采用上述計算、仿真過程，得到以不同速度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)機器人有針對地修復(fù)加工大直徑螺紋，可大大提高其加工精度和效率。

參考文獻：

[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] 龔振邦，王勤愨. 機器人機械設(shè)計[M]. 北京：電子工業(yè)出版社,1995:44-111.

[4] 熊有倫. 機器人技術(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è)機器人運動學(xué)逆向建模[J]. 機械設(shè)計與制造,2014,11(3):123-126.

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

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

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

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