六自由度組芯機器人逆運動學研究

，*，

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

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

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

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

研究基于旋量理論基礎對六自由度組芯機器人進行逆運動學分析，結合Paden-Kahan子問題算法對組芯機器人的各關節轉動的角度進行求解.

1 旋量理論

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

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

(1)

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

(2)

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

(3)

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

2 Paden-Kahan子問題

2.1 子問題1

設ξ為一個零節距的單位運動螺旋，p、q∈R3是空間兩點，點P繞軸旋轉θ角到達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

設ξ1和ξ2為兩個零節距、軸線相交的單位運動螺旋，p、q∈R3是兩點，點p先繞軸ξ1旋轉θ1角到達c點，然后繞軸ξ2旋轉θ2角到達q點，如圖2所示.若兩個軸線平行，則該問題退化為子問題1，且滿足θ1+θ2=θ的任意θ1，θ2都是正確解.如果兩個軸線不平行，設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 組芯機器人逆運動學求解算法

3.1 組芯機器人運動學建模

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

(12)

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

圖3 六自由度組芯機器人圖4 組芯機器人結構簡圖

3.2 組芯機器人逆運動學求解

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

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

(13)

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

(14)

(15)

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

(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軸是兩個平行的關節，qn繞軸2旋轉θ2后到達點c，再繞軸3旋轉θ3角度后到達qw點，取qn=[qnxqnyqnz]T根據子問題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的求解.根據得到的θ1,θ2和θ3值，由式(12)可得：

(25)

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

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

qmyqmz]T,

(26)

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

(27)

根據式(4)得：

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

(28)

(29)

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

(30)

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

根據子問題1的方法可求θ6

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

(31)

4 逆解算法的實例驗證

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

(1)隨機給定每個關節的對應轉動角度.

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

(2)根據機器人正運動學指數積公式計算末端執行器的位姿.

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

表1 8組運動學逆解

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

5 結論

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

[1] 林祖盛.ABB機器人在鑄造組芯整體浸涂上的應用[J].科技經濟導刊，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] 呂世增，張大衛,劉海年.基于吳方法的6R機器人逆運動學旋量方程求解[J].機械工程學報，2010，46(17)：35-41.

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

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

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

[11] 理查德.機器人操作的數學導論[M].北京:機械工業出版社，1994.