，*，

(1.安徽理工大學(xué) 機械工程學(xué)院，安徽 淮南 232001；2.南通鵬飛鑄造有限公司，江蘇 南通 226623)

為了提高鑄造過程中組芯工藝的效率和精確性，在傳統(tǒng)工藝流程中引進組芯機器人替代人工在復(fù)雜工作環(huán)境下完成鑄造過程中的組芯工序已成為市場形勢所趨[1].目前，機器人在組芯工序中的應(yīng)用僅局限于對砂芯的浸涂，研究中的六自由度機器人則能實現(xiàn)將砂芯準確裝入砂箱，該工序不僅要求機器人工作中末端執(zhí)行器達到目標位置，也對執(zhí)行器的姿態(tài)精度提出較高要求.機器人逆運動學(xué)的求解問題是研究機器人運動控制和軌跡規(guī)劃的基礎(chǔ)，通過已知工具坐標系相對工作臺坐標系的期望位置和姿態(tài)，求解機器人到達預(yù)期位姿的關(guān)節(jié)變量.該問題的求解過程存在方程參數(shù)多、解的非線性和強耦合性等問題，一直是國內(nèi)外研究的難點.通常情況下，機器人的運動學(xué)建模都會采用D-H矩陣參數(shù)法，但是該方法存在局部坐標系過多、幾何意義不明顯等缺點.因此，尋求一種高效、簡單的數(shù)學(xué)工具來進行機器人運動學(xué)建模成為當(dāng)前機器人領(lǐng)域熱點之一.

旋量法[2]是用一組對偶矢量的螺旋來表示物體運動的角速度和線速度.相比于傳統(tǒng)D-H矩陣的參數(shù)法，旋量法的優(yōu)勢在于從整體上描述剛體的運動，避免局部坐標系描述時所造成的奇異性；對剛體運動進行幾何描述，可以簡化機構(gòu)的分析；具有明顯的幾何意義優(yōu)點，使用指數(shù)積進行逆解求解時，可以明確多解的條件與個數(shù).

為求解機器人運動學(xué)逆解問題，首先要解決一般機器人設(shè)計中遇到的逆解子問題，然后設(shè)法將整個運動學(xué)逆問題分解成若干個解為已知的子問題.其中最著名的是Paden-Kahan子問題，建立于Kahan[3]的著作中.呂世增[4]等采用吳方法對一般機器人運動學(xué)方程進行消元，實現(xiàn)了計算機機械化求解.李盛前[5]等在旋量理論的基礎(chǔ)上引入sylvester結(jié)式進行逆運動學(xué)求解，并在數(shù)學(xué)符號化運算軟件Maple中實現(xiàn)了逆解算法過程.李悅[6]等利用旋量理論對RRRP機器人進行運動學(xué)建模并且獲得機器人逆解算法.錢東海[7]等利用Paden-Kahan子問題簡化旋量理論求解六軸機器人逆解問題過程，得到精確的機器人逆解.Akad Nankl[8]利用旋量理論模擬了機器人在空間中的運動情況.Dinesh[9]等利用常規(guī)消元法對6R機械臂進行逆運動學(xué)求解.李君[10]等利用旋量理論對StanFord臂進行運動學(xué)建模，并得到了Satnford臂的雅各比矩陣.

研究基于旋量理論基礎(chǔ)對六自由度組芯機器人進行逆運動學(xué)分析，結(jié)合Paden-Kahan子問題算法對組芯機器人的各關(guān)節(jié)轉(zhuǎn)動的角度進行求解.

1 旋量理論

在歐氏空間中，質(zhì)點的瞬心位置可相對于慣性坐標系來描述.根據(jù)chasles定理[11]，任何剛體的運動都可以轉(zhuǎn)化為繞某一軸的轉(zhuǎn)動加上平行于該軸的移動來實現(xiàn)，即為旋量運動.

假設(shè)剛體坐標系B的原點相對于坐標系A(chǔ)的位置矢量為PAB∈R3,姿態(tài)矢量RAB∈SO(3)(SO(3) 是三維旋轉(zhuǎn)群)，則系統(tǒng)由(RAB,PAB)確定，用齊次坐標可以表示為：

(1)

根據(jù)旋量理論，剛體的變換可以表示為運動旋量的指數(shù)，其表達式為：

(2)

在剛體上的物體坐標系T相對于慣性坐標系S的位姿變換用gst(θ)表示，以gst(0) 表示剛體相對于慣性坐標系的起始位姿，那么，相對于慣性坐標系的最終位姿為：

(3)

因此，對于一個運動旋量來說，指數(shù)變換反映的是剛體的相對運動，運動旋量的指數(shù)可以理解為描述剛體由起始到最終位形的變換.

2 Paden-Kahan子問題

2.1 子問題1

設(shè)ξ為一個零節(jié)距的單位運動螺旋，p、q∈R3是空間兩點，點P繞軸旋轉(zhuǎn)θ角到達q點，如圖1所示.假定r是軸ξ上的一點，定義r與q之間的矢量為u=(p-r)，r與q之間的矢量為v=(q-r)，u′和v′分別為u，v在垂直于軸ξ平面上的投影.文獻[11]給出了該問題的求解公式：

θ=atan(ωT(u′×v′),u′Tv′),

(4)

2.2 子問題2

設(shè)ξ1和ξ2為兩個零節(jié)距、軸線相交的單位運動螺旋，p、q∈R3是兩點，點p先繞軸ξ1旋轉(zhuǎn)θ1角到達c點，然后繞軸ξ2旋轉(zhuǎn)θ2角到達q點，如圖2所示.若兩個軸線平行，則該問題退化為子問題1，且滿足θ1+θ2=θ的任意θ1，θ2都是正確解.如果兩個軸線不平行，設(shè)c滿足下式：

(5)

圖1 繞單軸的螺旋運動圖2 繞兩軸的螺旋運動

定義向量u=(p-r),v=(q-r),z=(c-r),則得到：

(6)

因為‖u‖2=‖z‖2=‖v‖2，且ω1,ω2,ω3是線性獨立的，所以得到下式：

z=αω1+βω2+γ(ω1×ω2),

(7)

(8)

(9)

(10)

(11)

式(11)存在0,1或2個實根的情況.在有根的情況下，將α,β,γ值代入即可求得z和c，然后利用子問題1可以求得θ1和θ2的值.

3 組芯機器人逆運動學(xué)求解算法

3.1 組芯機器人運動學(xué)建模

六自由度組芯機器人機械結(jié)構(gòu)如圖3所示，各連桿坐標系結(jié)構(gòu)簡圖如圖4所示.此類機器人各關(guān)節(jié)通過轉(zhuǎn)動副連接.前3個關(guān)節(jié)確定手腕參考點位置，后3個關(guān)節(jié)確定手腕的姿態(tài).與D-H矩陣參數(shù)法不同，旋量法選取的坐標系是世界坐標系.設(shè)各連桿旋轉(zhuǎn)軸ξi的單位矢量為ωi(I=1,2,…,6) ，關(guān)節(jié)軸轉(zhuǎn)動的角度為θi，連桿i的坐標原點為θi.將各關(guān)節(jié)的運動加以組合，即得運動學(xué)正解映射為：

(12)

式(12)為機器人運動學(xué)正解的指數(shù)積公式.

圖3 六自由度組芯機器人圖4 組芯機器人結(jié)構(gòu)簡圖

3.2 組芯機器人逆運動學(xué)求解

根據(jù)Pieper準則：相鄰關(guān)節(jié)的軸線相交于一點，或3個軸線相互平行，則對于任意給定的末端工具的位置和姿態(tài)都有運動學(xué)逆解.組芯機器人屬于六自由度關(guān)節(jié)轉(zhuǎn)動機器人，其4,5,6三軸的軸線均交于同一點，滿足Pieper準則，所以該機器人存在封閉解.

給定機器人的末端工具坐標系T，機器人的基座慣性坐標系S，則機器人各個軸轉(zhuǎn)動角度為0時，基礎(chǔ)坐標系與工具坐標系的轉(zhuǎn)換為：

(13)

(1)θ1的求解.根據(jù)chalse定理：如果一點位于旋轉(zhuǎn)軸上，無論該點繞旋轉(zhuǎn)軸旋轉(zhuǎn)多少角度，該點的坐標不變，即：

(14)

(15)

將式(15)右乘qw，根據(jù)上述定理可得：

(abc)T,

(16)

將式(16)展開得：

L(c2s1s3+c3s1s2)-D(c2c3s1-s1s2s3)+m=a,

(17)

D(c1c2c3-c1s2s3)-L(c1c2s3+c1c3s2)+n=b,

(18)

D(c2s3+c3s2)+L(c2c3-s2s3)+k=c,

(19)

m=-s1[s2(l1+l2)-d1(c2-1)]-c2s1[Ls3-d1(c3-1)]-s1s2[d1s3+L(c3-1)]

n=c1[s2(l1+l2)-d1(c2-1)]+c1c2[Ls3-d1(c3-1)]+c1s2[d1s3+L(c3-1)]

k=-(l1+l2)(c2-1)-c2[d1s3+L(c3-1)]-d1s2+s2[Ls3-d1(c3-1)]

式中，ci(i=1∶6)是cosθi的縮寫，si是sin(θi)的縮寫.

式(17)乘以c1加上式(18)乘以s1得到下式：

acosθ1+bsinθ1=0,

解上述方程，可得：

(20)

(2)θ2和θ3的求解.組芯機器人的2軸和3軸是兩個平行的關(guān)節(jié)，qn繞軸2旋轉(zhuǎn)θ2后到達點c，再繞軸3旋轉(zhuǎn)θ3角度后到達qw點，取qn=[qnxqnyqnz]T根據(jù)子問題2的方法，可得：

φ1=arctan(-l2(qny-d1),-l2(l1-qnz)),

(21)

c,r23的夾角Δ1為：

(22)

則可求得θ2

θ2=φ1±Δ1,

(23)

同理

φ2=arctan(-l2(d2+d3),-l2l3),

則可求得θ3

θ3=φ2±Δ2,

(24)

(3)θ4和θ5的求解.根據(jù)得到的θ1,θ2和θ3值，由式(12)可得：

(25)

取在軸6上而不在軸4,5上的點qm=[0D+d4L]T

將qm點帶入式(25)中，得：

qmyqmz]T,

(26)

根據(jù)式(9),式(10)，式(11)可得：

(27)

根據(jù)式(4)得：

×c),cTv)=arctan((γ-L)qmx,qmy(qmy-D)-(L-qmz)(L-γ)),

(28)

(29)

(4)θ6的求解.將前面得到5個軸的轉(zhuǎn)動角度代入式(12)可得：

(30)

取不在軸6上的任意點(該點選取時盡量選擇較遠處點，u可近似等于u在ω垂直的平面投影) ，po=[ijk]T,將po點帶入式(30)中，得：

根據(jù)子問題1的方法可求θ6

arctan(i(L-poz)-pox(L-K),(D-j)(D-poy)+(L-K)(l-poz)+poxi).

(31)

4 逆解算法的實例驗證

組芯機器人的具體參數(shù)如下：l1=404 mm，l2=100 mm，l3=780 mm，d1=170 mm，d2+d3=760 mm，d4=125 mm，具體求解六自由度組芯機器人逆解過程如下：

(1)隨機給定每個關(guān)節(jié)的對應(yīng)轉(zhuǎn)動角度.

θ1=-32.45°,θ2=-32.20°,θ3=-1.15°,θ4=0°,θ5=-58.66°,θ6=-32.42°.

(2)根據(jù)機器人正運動學(xué)指數(shù)積公式計算末端執(zhí)行器的位姿.

(3)應(yīng)用研究所給出的求解逆解的方法，在Matlab軟件中編制逆運動學(xué)算法程序，將數(shù)據(jù)代入后共得到8組逆解，如表1所示.

表1 8組運動學(xué)逆解

將上述8組逆解代入機器人正運動學(xué)公式，得到的位姿解與設(shè)定解相對誤差在0.2%以內(nèi)，所以該逆解算法具有較高的精確度.

5 結(jié)論

基于旋量理論對六自由度組芯機器人進行運動學(xué)建模，針對組芯機器人結(jié)構(gòu)特殊性，利用Paden-Kahan子問題算法對逆解求解過程進行簡化，從而求得了機器人的位姿，并對結(jié)果進行了驗證.該逆運動學(xué)求解算法從整體上描述了機器人的運動，避免了傳統(tǒng)D-H參數(shù)方法求解過程中復(fù)雜的坐標轉(zhuǎn)換，簡化了運算過程，減少了控制系統(tǒng)運算的時間；同時描述了組芯機器人組芯作業(yè)時末端手爪的姿態(tài)和各軸的運動參數(shù)，為組芯機器人精確組芯的控制提供了理論依據(jù).

[1] 林祖盛.ABB機器人在鑄造組芯整體浸涂上的應(yīng)用[J].科技經(jīng)濟導(dǎo)刊，2016(33)：74-75.

[2] R S BALL.A treatise on the theory of screws[M].UK：Cambrige University Press,1900.

[3] K HUNT.H kinematic geometry of mechanisms[M].UK:Oxford University press,1978.

[4] 呂世增，張大衛(wèi),劉海年.基于吳方法的6R機器人逆運動學(xué)旋量方程求解[J].機械工程學(xué)報，2010，46(17)：35-41.

[5] 李盛前，謝小鵬.基于旋量理論和Sylvester結(jié)式法的6自由度機器人逆運動學(xué)求解分析[J].農(nóng)業(yè)工程學(xué)報，2015,31(20):48-54.

[6] 李悅，周利坤.基于旋量理論的RRRP機器人逆運動學(xué)分析與研究[J].機械科學(xué)與技術(shù)，2014，33(6):820-824.

[7] 錢東海，王新峰,趙偉，等.基于旋量理論和Paden-Kahan子問題的6自由度機器人逆解算法[J].機械工程學(xué)報，2009,45(9):72-77.

[8] F M DIMENTBERG.Determination of the motion of spatial mechansims (Russian)[M].Moscow:Akad Nank,1950.

[9] DINESH MANOCHA,F C JOHN.Efficient inverse kinematic for general 6R manipulator[J].IEEE Transactions on Robotics and Automation,1994,10(5):648-657.

[10] 李君.基于旋量理論的Stanford臂的運動學(xué)分析[J].天津科技大學(xué)學(xué)報，2010,25(4):72-75.

[11] 理查德.機器人操作的數(shù)學(xué)導(dǎo)論[M].北京:機械工業(yè)出版社，1994.