6R的反向运动学的强实时算法
?----------------------- Page 1-----------------------
6R的反向运动学的强实时算法
机器人
AStrong Real Time Algorithm for Inverse Kinematics of 6R
Robots
Huashan Liu，Shiqiang Zhu，Jianbo Wu
The State Key Lab of Fluid Power Transmission and Control，Zhejiang University,Hangzhou，China，
watson683@1 63．com
Abstract motion control，especially in the online control systems．
Different from the traditional method，we propose a Inverse transformation method，as a traditional algorithm，
novel algorithm based on vector dot product and is widely applied in inverse kinematics computations．It
orthogonal matrix for inverse kinematics of 6R robots， needs to repeat the computations of inverse matrix SO
which features in strong real time performance and no many times that makes the solving process complex and
extraneous roots in the solving process．By using the time—consuming．Tsai and Morgan proposed an inverse
character that the rotation sub—matrix is an orthogonal kinematics algorithm invoking vector dot product for
matrix，the complex solution procedures of inverse matrix general 6R robot，which helped to simplify the solving
are avoided．Then the kinematics equations containing 6 process【3】．Motivated by it，we propose a strong real
unknown joint angle variables can be reconstructed to 1 0 time algorithm for inverse kinematics of Pieper Criterion
pure algebraic equations by vector dot product． based 6R robots by using the properties of vector dot
Particularly,aiming to improve the real time performance product and orthogonal matrix．
further,we optimized the linear combinations of the
related equations for the 5th and 6th joint angle variables．
2 Inverse Kinematics
Also，we utilize MAPLE in symbolic operations to
increase the accuracy of solutions．Finally,experiments The kinematics of 6R robots with revolute
equations
a 6R robot are show
on implemented，which that，the joints can be described as：
proposed algorithm can solve the equations in an average
time of only 9．936 gs，and makes a good performance in 。瓦=
。五1t 2五3五4瓦5疋=[：彳]
the practical strong real time motion control situation．
nx ox dx
(1)
Keywords 0 a nF oy nv
6R robot；Inverse kinematics，Real time control； =l O 0 小 nz 0z d。
0 0 O 成乃乜●
Vector dot product；Orthogonai matrix 1●●●●●●●●●●，j
Where R is the 3x3 rotation matrix，including three 3x 1
vectors以，0 and a，which respectively denote the normal
1 Introduction
vector，sliding vector，and approach vector；P is the 3 xl
Nowadays，SO many robots based on Pieper Criterion position vector．
(three consecutive joint axes are parallel or intersect at a And the 4x4 homogeneous transformation matrix is：
common point)are being used in different kinds of
q--Siri s，q qq
industrial occasions[1】．In solving the problem of inverse
c2，
kinematics for Criterion based 6R robots，we can
Pieper —z=[o々]=l丢cqirj一{_：尹l
find 8 closed—form solutions of each joint angle value by l 0 0 0 1 l
analytical method given the position and orientation Where I—sing
matrix of the end．effector relative to the base and the
q—cos包
values of D—H parameters【2】．The real—time performance
O"i—sina．
of the solving process directly affects the results on
t—COS口，
f=1，2…，6
Project(No．2008C2 1 1 06)supported by the Zhejia
ng Provincial
Science and Technology Foundation of China
295
----------------------- Page 2-----------------------
2 2 2
3 Some Useful Properties ，I 1^1 Ip：1 2一，，2|，；{2，，2，。I
=2【风·(月；^)+‘R风+P，)·(盯P，)+(只砖见
For v H。1 vector“H H。n orthogonal matrixM and
+R．^+^)·(刷P3)+(置t也凡+墨月a^ (15)
exits：
constsntP，there ujways
+R、“；P—tR：P2、+(R2R，R，R-h+RIR，R～n
Pl (pⅣ)一p(Ⅳ·p)=p(v·“) +且R3P。+R2B+n)·(爿P．)J
P2 Ⅳ一=Ⅳ7
In addition，take Eq 4for example，…an obtain
P3 Mu一ⅣM’·v
P4 fMu)·(My、=Ⅳ·v q 2RZ‘一。(∥』)2=‘(啊月s‘月·。叫月z12r‘1)㈨。
=z·(^j譬厨砰月j砰z)
We c日n find that the rolary matrix R肌d R．am all
orlhogonalmatrices，Sothey sagsfythe proper【ies above The transformations and simplificagons of the other
equations aMthe same as above
4 Reconstruction ofEquations
5 Calculation Example
Orderx=【1 0 o一，=【01 on卜【0 01】7，from Eql，
we c…constmct l 0 nonlinear vector equatiolls 5．1 S)mbolie Operations
containing 610im angle variables by Vector dot product： Take QJ-1 6R robot for example as shown in Fig I
p'p=p11 (3) Thelastthree consecutive㈣oint es ofQJ-l integer at a
…on soit satisfiesthe
point PieperCrderian
P，2 p·x (4)
F。2 p·y (5)
pⅧp (6)6
o，～…Rzj (7)
口，=a．y=RZ·Y (8)
m一㈤Rz： 口)
ox 2 o*x…ny (10)
ox=o．y=母-Y (11)
o，=olz=时-： (12)
Andfrom Eqs 1 and 2 we ca”get：
，=Rj{置【R(且(B凡+P，)+只)+^卜n}+^(L3)
R2RRR置月。也 (14)
Then transform Eq 13“follows：
P=(置R2月，R{月，)慨增，A+巧坷1P。憎，。凡1马。见
Fig I：Q^MRtopoi
+月i‘《1坷墨1，。—。乓1巧1月，1巧。矸A)
Where wc can easily find that E月2置或置is atl TileD—H parameters ofOJ-l…hownin Table
orthogonal matrix qllerefore，using propeaies P4，tile Tablel：D—H paramelers ofQJ一1 6R robot
Eq 3 can be simplified as+
JOInt dhm] d【mm] 口r。]l d【。】
j，2一p 2一}，：1 2，，’～【，。f—Iv，2^2 ．90 1口。
=帆+^，n+月，只。p4+Rl|^。5月)P， 0 ＆
+E巧’町《n坩，茸巧啊1月。。P)． 巩
饥—。碍。P，十月s1目凡憎5‘巧‘巧1P] 吼
一月5只1碍‘月，。p2+R；‘月。‘R，’月z‘矸1P1) 90 B
Further we can get 吼
----------------------- Page 3-----------------------
In order to avoid the accumulative errors and improve 6 Experimental Results
the simplification efficiency，we involve the computing
software MAPLE in the The position and orientation matrix of the end．effector
symbolic operations．
According the symbolic preprocessing by MAPLE， relative to the base point is given as：
equations 3-12 can be transformed tO： 0．1021-0．7796 0．6 179 -45．2334]
o， l-0．4479 0．5 1 87 o．7283 —78．3466 l
P J2一q2一碣2一吒2一q2一以2
。
l-0．8883-0．351 I-0．2962．331．3244 I
=2[a2(a3c3一以s3)+q(一d,s2】+呸乞+qc23) (17)
一4(以c23+a2s2+呜s23)】 l o o o l
j
以2Cl(a36"23+呸巳+q—d4s23) (18) According the algorithm proposed above and D-H
p，=sl(码c23+吒cj+aI—a,s23)(19) parameters of QJ·1 6R robot shown in Table 1，we get 8
solutions as shown in Table 2，and the attitude simulation
见=一以c23一qs23一a：s2+碣 (20)
of each solution in Fig．2，which is seen from YOZ plane
q=(ctc23cj+sis4)s5+(cls23)c5(21) in the base coordinates．
ay=(^c23c4一CtS4)s5+(JlS23)g (22)
Table 2：8 solutions of inverse kinematics
az=一S23C4S5+C23c5(23)
No． 0【o】 NO． 0【o】
q=s6(qJ23％一C]C230c5一SIS4C5)+CAqq3■一JlC4)(24)
0l=240．0000 0l=240．0000
Dv=J6(而S23长一S1C23c4c5+ct$4c5)-I-c6(slc23‘+c1 c4)(25)
02=一198．5530 02=-198．5530
呸=86(J23c,c5+c23s5)一C6S23& (26) 03=-195．0331 03=-195．0331
l 2
Where％一sin(q+目，) 04=一169．7603 吼=10．2397
ci——cos(p+p，) 岛=-74．1525 05=-74．1525
i，／=1，2…，6 06=-15．6571 06=164．3429
5．2 Solving Steps 9l=240．0000 01=240．0000
and 2 solutions 凸=30．2475 02=30．2475
Step 1：By Eqs．18 19，we get of05
and 4 solutions 岛=45．1839 03=45．1839
Step 2：By Eqs．17，18 20，we get of03， 3 4
2 ones for each 0卜Then，4 solutions of 02 by Eqs．18 and 0A=-73．6080 04=106．3920
05=169．7318 良=一169．7318
20，onefor each03；
Step 3：When solving 04 by Eqs．21，22
and 23，we 06=一125．1254 06=54．8746
should judge whether the robot is singular or not．If so 0l=60．0000 0l=60．0000
(i…e s520)，the 4th and the 6th joint axes are overlapped， 02=一185．0548 ＆=-185．0548
then the value of 04 can be chosen arbitrarily； 03=一189．8492 岛=一189．8492
5 6
Step 4：Motivated by【4】，we get 8 solutions of 05 by 04=11．6163 04=-168．3837
optimizing the linear combination ofEqs．21-23 as below： 良=121．8652 05=一121．8652
墨=(CtC23C4+一s4)ax+(‘c23c4一CIS4)口，+(一$23G)G(27) 巩=吒4．6749 06=155．3251
e5=(qs23)q+(墨s23)口y+(cj3)呸 (28) 0l=60．0000 0l=60．0000
02=50．0000 良=50．0000
Then，色can be solved by包=atan2(q，s5)．
03=40．0000 03=40．0000
Step 5：With the same method used in Step 4．from 7 8
04=30．0000 04=一150．0000
Eqs．24·26，we can obtain
05=20．0000 岛=屯O．0000
％2(。c·s23s5--CTC．23，C4C5--JiS4Cs’≮+(5-sz，Js—s-c2，C4C5(29) 06=10．0000 魄=一170．0000
+ClS4Cs)Oy+(823c4c5+c23J5)oz
c6=(CIC2354一S,c4)0x+(^乞3＆+clc4)q+(一s23s4)c6(30) In Table 2，the solution range of each joint angle 0i is：
Then，06 can be solved by见=atan2(c6，&)． 900≤0l<2700， -2700<岛<90。，
Until now，we have got the complete 8 closed—form —2700<03<900， 一1 80。<04 S1 800，
solutions ofeach joint angle for QJ一1 6R robot． 一1800<05 S1800， 一1800<06 51800．
297．
----------------------- Page 4-----------------------
嗲压一 荸声一 _童敷攀
。咪～ 。：～：长 疹孰≮渡i
l ： l 。蝼
靠
Fig 3：ActualmotionconlroI|oftrafoctorie&
J‘。哆 qIn the exp目…ls we sampled 600—1 200 discrete
pointsfromthe circles by equal dividingthe central a1ⅦIe，
then got inverse kinematics solutions by the proposed
algorithm in vc++compiling environment based on the
PC platf一：InterPentiumDual 2GHzCPU 2GBRAM
WindowsXP SP2 With 5000times oflnveTsg kinematics
、斗 。一洋 calculations ofthe discrete points…got averagetime
ofjust 9 936,us at atimein solvingtheinverse kinematics
oporto pointwith an acgurficy of0 000lo
矿＼
。瓜：一l， 一!E 7 Conclusion
一
x／■爵～ By using s0Ine properties of vector dot product and
；7下’一 8 I
orthogonal matrix，a sfrong real time algorithm for
Fi91．AltitudeMmulonon oftⅢ啪e【‘口 inverse kinematics ofPinper Criterion based 6R robotis
indicate that it caⅡwork
proposed Experiments
Consideringthe cagingpolice ofmechanism andthe efficientlyinthe acmal orthne robotmotion controI
acmal needforworkspace ofQJ一1 6R robot wc cTcatg a
fonherhoundwiththeworkspace range of目。： References
800<0j<260。， 一170。‘岛<80。，
【l】Sicili卸o，B，Khatib，O Springer Handbook of
一2600‘岛180。， I 800‘以<1 800，
Robotics
SpingerBerlinHeidelberg，2008
一140。‘巩<140。， 一180。(06<180。
【2】2 Tolani，D，Goswami，A，Badler,N Real-time
Notingthat，there am only nvo solutions(No 7 and 8)
lnverse kinematics techniques for anthropomorphic
sarisfying restrictions above Therefore，in actual robot
limbs GraphicalModels，Vol 62，pp 353-388，2000
motion control we should mak…opt um choice firom
【3】Tsai，L。Morgan A Solving the kinematics ofthe
these two solutions on the basis of somc strategies of
most general six-and five-degree-o晰eedom
optimi丑tion，such as minimum angular displacement
m矾ipuintom by continuation methods JournaI of
method，minimum energymethod，et al
MechanBms．Transmissions．and Automation lⅡ
To tesofy the real time performance and the
D#ign，Vol 107(2)，PP 1 89-200，1985
efficiency in practical motion control of the proposed
14】EIgazzar,S Efficient kinematic transfo肌atinns for
algorithm，a sample eoaⅡol of some circletrajectoriesin
me PUMA 560 robot IEEE JonrnaI of Robotics
XOY口la他dthe bose—fonztes…ade镐s‰帅
and Automation，Vol RA·l(3)，PP 142-l 51。1985
inFig 3，withintheworkingtime ofI 6sfor each ci rcle