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

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

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

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

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

1.1 機器人運動學正解

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

(1)

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

圖1 D-H坐標系 表1 機器人連桿參數表

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

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

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

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

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

(2)

即：

(3)

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

1.2 機器人運動學逆解

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

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

(4)

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

(5)

式(5)中正負號對應于θ2的2個解，在求解具體工業(yè)機器人運動學逆解時，可通過數據驗證確定正負號[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 普通螺旋線

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

(10)

式中：φ為參數變量。

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

(11)

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

圖2 螺旋線

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

3.1 截取節(jié)點

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

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

在xoy平面上：

即：

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

(12)

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

n=ceil(2·π/αc)

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

圖3 投影圖

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

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

3.2.1節(jié)點位置

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

(13)

(14)

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

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

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

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

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

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

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

(15)

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

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

(16)

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

最后，在得到節(jié)點位置矩陣和姿態(tài)矩陣后，運用MATLAB循環(huán)程序將兩者融合成節(jié)點位姿矩陣T(:,:,i)，矩陣T是4×4×(3n+1)階矩陣，即(3n+1)個節(jié)點在基坐標系{o}的位姿矩陣序列，節(jié)點位置如圖4所示。

圖4 節(jié)點位置

4 驗證機器人模型

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

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

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

將q1代入式(3)中，得

將q2代入式(3)中，得

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

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

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

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

圖5 關節(jié)角

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

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

(17)

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

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

設機器人末端在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內元素的方法相同，本文只給出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]時的機器人雅克比，結果為：

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

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

圖6 關節(jié)角速度

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

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

(20)

其中：

(21)

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

引進矩陣層如下所示：

(22)

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

(23)

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

(24)

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

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

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

采用上述計算、仿真過程，得到以不同速度vz加工不同直徑螺紋的全部結果。例如：用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 關節(jié)角加速度

普通大直徑螺紋參數見表2。

表2 螺紋參數 mm

6 結論

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

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

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

參考文獻：

[1] 彭成勇，樓一珊，曹元平，等.鉆鋌螺紋連接疲勞失效研究[J].石油鉆探技術，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] 龔振邦，王勤愨. 機器人機械設計[M]. 北京：電子工業(yè)出版社,1995:44-111.

[4] 熊有倫. 機器人技術基礎[M]. 武漢：華中科技大學出版社,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]. 機械設計與制造,2014,11(3):123-126.

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

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

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

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