，*，

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

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

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

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

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

1 旋量理論

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

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

(1)

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

(2)

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

(3)

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

2 Paden-Kahan子問題

2.1 子問題1

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

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

(4)

2.2 子問題2

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

(5)

圖1 繞單軸的螺旋運(yùn)動圖2 繞兩軸的螺旋運(yùn)動

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

(6)

因?yàn)椤瑄‖2=‖z‖2=‖v‖2，且ω1,ω2,ω3是線性獨(dú)立的，所以得到下式：

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

(7)

(8)

(9)

(10)

(11)

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

3 組芯機(jī)器人逆運(yùn)動學(xué)求解算法

3.1 組芯機(jī)器人運(yùn)動學(xué)建模

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

(12)

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

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

3.2 組芯機(jī)器人逆運(yùn)動學(xué)求解

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

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

(13)

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

(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的求解.組芯機(jī)器人的2軸和3軸是兩個(gè)平行的關(guān)節(jié)，qn繞軸2旋轉(zhuǎn)θ2后到達(dá)點(diǎn)c，再繞軸3旋轉(zhuǎn)θ3角度后到達(dá)qw點(diǎn)，取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上的點(diǎn)qm=[0D+d4L]T

將qm點(diǎn)帶入式(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個(gè)軸的轉(zhuǎn)動角度代入式(12)可得：

(30)

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

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

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

(31)

4 逆解算法的實(shí)例驗(yàn)證

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

(1)隨機(jī)給定每個(gè)關(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ù)機(jī)器人正運(yùn)動學(xué)指數(shù)積公式計(jì)算末端執(zhí)行器的位姿.

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

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

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

5 結(jié)論

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

[1] 林祖盛.ABB機(jī)器人在鑄造組芯整體浸涂上的應(yīng)用[J].科技經(jīng)濟(jì)導(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機(jī)器人逆運(yùn)動學(xué)旋量方程求解[J].機(jī)械工程學(xué)報(bào)，2010，46(17)：35-41.

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

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

[7] 錢東海，王新峰,趙偉，等.基于旋量理論和Paden-Kahan子問題的6自由度機(jī)器人逆解算法[J].機(jī)械工程學(xué)報(bào)，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臂的運(yùn)動學(xué)分析[J].天津科技大學(xué)學(xué)報(bào)，2010,25(4):72-75.

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