Quaternion Based Inverse Kinematics for Industrial Robot Manipulators with Euler Wrist

Transcription

1 Quatenion Based Invese Kinematics fo Industial Robot Manipulatos with Eule Wist Yavuz Aydın Electonics and Compute Education Kocaeli Univesity Umuttepe Kocaeli Tukey Seda Kucuk Electonics and Compute Education Kocaeli Univesity Umuttepe Kocaeli Tukey Abstact The majo complications of invese kinematics poblem fo seial obot manipulatos ae singulaities and nonlineaities In this context it is impotant to fomulize the invese poblem in a compact closed fom In this pape we pesent closed fom solutions of the 6-DOF industial obot manipulatos with Eule wist using dual quatenions The successive scew displacements in dual quatenions educe sine/cosine-type nonlineaities esulting in a vey compact fomulation The RS R and S obot manipulatos ae consideed as the examples of the employed method I ITRODUCTIO The invese kinematics equations of a obot manipulato basically map the Catesian space to the joint space The invese kinematics poblem is difficult to solve since the mapping between the joint space and Catesian space is non-linea and involves complicated tanscendental equations with multiple solutions The dual quatenions povide a possible way to educe sine and cosine-type nonlineaities as in [] thus offe compact fomulation of the invese kinematics A dual-quatenion can povide both otation and tanslation with a compact tansfomation vecto While the oientation of a body is epesented nine elements in homogenous tansfomations dual quatenions educe the numbe of elements to fou As a esult a consideable advantage in tems of computational obustness as well as stoage efficiency is achieved while dealing with the kinematics of obot chains as in [] Since Ref [3] s intoduction of quatenions they have been used in many applications such as classical and quantum mechanics aeospace geometic analysis and obotics While Ref [4] pesented advantages of quatenions and matices as otational opeatos the fist application of the fome in the kinematics was consideed by Ref [5] Late geneal popeties of quatenions as otational opeatos wee studied by Ref [6] who also pesented quatenion fomulation of moving geometic objects Ref [7] used quatenions fo computing the Jacobians fo obot kinematics and dynamics Ref [] compaed quatenions with homogenous tansfoms in tems of computational efficiency Ref [8] used quatenions fo the solution of diect and invese kinematics of a 6-DOF obot manipulato Ref [9] used quatenions fo kinematic contol of a edundant space manipulato mounted on a fee-floating space-caft Ref [0] pesented a new technique fo the obots calibation based on the quatenion-vecto pais In this pape the invese kinematics solutions of 6-DOF industial obot manipulatos classified by Ref [] ae solved in closed fom by using dual quatenions Each obot manipulato has an Eule wist with thee axes intesecting at a common point Desciption of obot configuations quatenion-vecto pai methodology and invese kinematics poblem statement ae pesented successively Finally quatenion-based invese kinematics solutions of the RS R and S type obot manipulatos ae pesented as examples II DESCRIPTIO OF ROBOT COFIGURATIOS Ref [] used a two-lette code to classify a obot configuation The fist lette chaacteizes the fist joint and the fist joint s elationship to the second joint The second lette identifies the thid joint and thid joint s association to the second joint The code lettes and thei meanings ae: S is slide C is otay paallel to slide is otay pependicula to otay and R is otay pependicula to otay o otay paallel to otay The combination of these otay and pismatic joints is SS SC S CS CC CR S R RC R RR RS SR C and C Table gives the joint stuctues of sixteen fundamental obot manipulatos (P R F S T and M epesent pismatic evolute fist second and thid joints and manipulato espectively) Table The joint stuctues of sixteen fundamental obot manipulatos M S S S C C C R R R R S C S C S C R S R C R S R C F P P P R R R R R R R R R R P P R S P P R P P P R R R P R P R R R R T P R R P R R P R R R R R P R R P III QUATERIO-VECTOR PAIRS METHODOLOGY A quatenion q is the sum of scala (s) and thee dimensional vecto (v) Anthe wods it is a quadinomial expession with a eal angle θ and an axis of otation n = ix + jy + kz whee i j and k ae imaginay numbes It may be expessed as a quaduple q = (θ x y z) o as a /06/$ IEEE 58

2 ICM 006 IEEE 3d Intenational Confeence on Mechatonics scala and a vecto q = (θ u) whee u= x y z In this pape it is denoted as q = [ s v] o q [cos( θ / ) sin( θ / ) < k k k > ] () = x y z 3 whee s R v R and θ and k a otation angle and unit axis espectively Fo a vecto oiented an angle θ about the vecto k thee is a quatenion q = [cos( θ / ) sin( θ / ) < k k k > ] = [ s < x y z > ] x that epesents the oientation This is equivalent to the otation matix given as follows: y y z xy sz xz + sy R = xy + sz x z yz sx () xz sy yz + sx x y If R is equated to a 3x3 otational matix such as 3 3 and since q is unit magnitude ( s + x + y + z = ) then the otation matix R can be mapped to a quatenion q = [ s < x y z > ] as follows: z (3) s = (4) 3 3 x = (5) 4s 3 3 y = (6) 4s z = (7) 4s Although unit quatenions ae vey suitable to expess the oientation of a igid body they do not contain any infomation about its position in the 3D space The way to epesent both otation and tanslation in a single tansfomation vecto is to use dual quatenions The point vecto tansfomation using dual quatenions Q fo evolute joints can be denoted as Q( q p) = ([cos( θ / ) sin( θ / ) < k x k y k z > ] p p p > (8) < x y z whee the unit quatenion q epesents appopiate otation and the vecto p =< px py pz > encodes coesponding tanslational displacement In the case of pismatic joints the displacement is epesented by a quatenion-vecto pai whee [ < > ] epesents unit identity quatenion Quatenion multiplication is vital to combining the otations Let q = [ s v ] and q = [ s v] denote two unit quatenions In this case multiplication pocess is shown as q q = s s v v s v + s v + v ] (0) [ v whee ( ) ( ) and ( ) ae dot poduct coss poduct and quatenion multiplication espectively In the same manne the quatenion multiplication of two point vecto tansfomations is denoted as = ) ( ) = Q Q ( q p q p q q q p q + p () whee q p q = p + s( v p) + v ( v p) A unit quatenion invese equies only negating its vecto pat ie q = [ s v] = [ s v] () Finally an equivalent expession fo the invese of a quatenion-vecto pais can be witten as Q = ( q q * p * q) (3) whee q * p * q = p + [ s( v ( p)) + v ( v ( p))] IV IVERSE KIEMATICS PROBLEM STATEMET To solve the invese kinematics poblem the tansfomation quatenion is defined as [ R T ] ([ w < a b c > ] < p p p (4) w w = x y z whee ( R w Tw ) epesents the known oientation and tanslation of the obot end-effecto with espect to the base Let Q i ( i 6) denotes kinematics tansfomations descibing the spatial elationships between successive coodinate fames along the manipulato linkages such as Q = ( q p) Q = ( q p) Q 6 = ( q6 p6) The quatenion vecto poducts M i and the quatenion vecto pais ae defined as j+ M = Q Q Q 6 (5) i i i+ j+ = Q j j (6) whee i 6 and j 5 espectively ote that = [ R w Tw ] In ode to extact joint vaiables as functions of s v p p p and fixed link paametes appopiate x y z M i and j+ M = M = M 6 = 6 tems ae equated such as Q ( q p) ([ < > ] < p p p (9) = x y z 58

3 Y Aydin S Kucuk Quatenion Based Invese Kinematics fo Industial Robot Manipulatos with Eule Wist V EXAMPLES In this section the invese kinematics of RS R and S obot manipulatos ae solved in closed fom using quatenion-vecto pais In ode to deive the kinematics tansfomations a fixed coodinate fame is attached to the base of the obot manipulatos The z-axis of the coodinate fame is assigned fo the otation axis of fist joint The position and the oientation vectos of all othe joints ae assigned in tems of this efeence coodinate fame The invese kinematics solution pocedues fo the obot manipulatos ae given in detail to illustate the powe of the quatenion algeba Some tanscendental equations used in the solutions of invese kinematics poblems ae given in Appendix A A Invese Kinematics fo RS Robot Manipulato The igid body model and coodinate fame attachments of the RS obot manipulato with Eule wist ae given in Fig wheeθ θ θ4 θ5 θ6 and d 3 ae the joint vaiables fo evolute and pismatic joints espectively and also l and l denote the link lengths θ θ l l d 3 Q = ([ c sk] < l 00 (4) Q 3 = ([ 0] < 00 d3 (5) s4k] (6) Q 5 = ([ c5 s5 (7) Q 6 = ([ c6 s6k] (8) The quatenion vecto poducts ae M 6 = Q 6 M 6 = ([ c6 s6k] (9) M ) (30) 5 Q5M 6 5 = ([ c5c6 < s5s6 s5c6 c5s6 > ] < 000 > M ) (3) 4 Q4M 5 4 = ([ M 4 < M 4 M 43 M 44 > ] < 000 > whee M 4 = c5c(4 + 6) M 4 = s5s(4 6) M 43 = s5c(4 6) and M 44 = c5s(6+ 4) Y Z h 3 Q3M 4 M 3 = ([ M 3 < M 3 M 33 M 34 > ] < 00 d 3 (3) X θ 4 θ 6 θ 5 whee M 3 = M 4 M 3 = M 4 M 33 = M 43 and 34 M 44 Figue Rigid body model and coodinate fame attachments of the RS obot The kinematics tansfomations defining the spatial elationships between successive linkages can be expessed as follows: Q = ([ c sk] < lc ls h (7) Q = ([ c sk] < lc ls 0 (8) Q 3 = ([ 0] < 00 d 3 (9) s4k] (0) Q 5 = ([ c5 s5 () Q 6 = ([ c6 s6k] () Fo eason of compactness θ i / sin( θ i / ) cos( θ i / ) sin( θ i ) and cos( θ i ) ae epesented as θ i s i c i s i c i espectively In this case the invese tansfomations ae Q = ([ c sk ] < l0 h (3) QM 3 M = ([ M < M M 3 M 4 > ] < lc ls d 3 (33) whee M = c5c 4 6 M = s5s 4+ 6 M 3 = s5c 4+ 6 and M 4 = c5s 4 6 QM M = ([ M < M M 3 M 4 > ] < M 5 M 6 h d 3 (34) whee M = cc5c sm 4 M = s5s M 3 = s5c M 4 = cm 4 + sc 5c M 5 = lc+ + lc and M 6 = ls+ + ls The quatenion vecto pais ae ([ w < a b c > ] < p p p ) (35) = x y z > = Q = ([ < 3 4 > ] < 5 6 p z h > ) (36) whee = wc + cs = ac + bs 3 = bc as 4 = cc ws 5 = p x c l + p y s and 583

4 ICM 006 IEEE 3d Intenational Confeence on Mechatonics 6 p yc p x s = 3 = Q 3 = ([ 3 < > ] < p z h > whee = c + c s c ) w( s ) 3 ( s 3 c s 3 33 c 3 s 34 = c 4 s 35 = p xc+ l + p y s+ lc = + = 36 = p y c+ p x s+ + ls ) (37) 44 4 θ 6 actan actan (44) 4 43 B Invese Kinematics fo S Robot Manipulato The igid body model and coodinate fame attachments of the S obot manipulato with Eule wist ae given in Fig wheeθ θ θ4 θ5 θ6 and d 3 ae the joint vaiables fo evolute and pismatic joints espectively d 3 θ 4 θ 6 θ 5 4 = Q3 3 4 = ([ 4 < > ] < > ) (38) θ l whee = c + c s c ) w( s ) 4 ( s 4 c s 3 43 c3 s 44 = c 4 s 45 = p xc+ l + p y s+ lc = + = + and 47 = d3 h + p z 46 = p y c p x s+ + ls X l θ Z h z 0 The evolute joint vaiables θ θ and pismatic joint vaiable d3 can be detemined by equating the tems M M M 3 to and 3 espectively a px + py l l whee a = l l θ = actan ( ± a ) (39) ( ) + p p p ls x x y θ = actan ± actan (40) py ls d 3 = h (4) p z s = + c = + tan( θ and 43 +θ 4 6 ) = 4 tan( θ 4 θ 6 ) = equations can be deived fom equating the tems M 4 to 4 whee c 5c(4 6) = 4 s 5s(4 6) = 4 s 5c(4 6) = and c 5s(6+ 4) = 44 In this case the oientation angles of the Eule wist can be detemined as follows: θ 5 = actan ± (4) 44 4 θ 4 actan + actan (43) 4 43 Figue Rigid body model and coodinate fame attachments of the S obot The kinematics tansfomations ae Q = ([ c sk ] < ls lc h (45) Q = ([ c s < ls 0 lc (46) Q 3 = ([ 0] < 00 d3 (47) s4k] (48) Q 5 = ([ c5 s5 (49) Q 6 = ([ c6 s6k] (50) The invese tansfomations ae Q = ([ c sk ] < 0 l h (5) Q = ([ c sk] < 00 l (5) Q 3 = ([ 0] < 00 d3 (53) s4k] (54) Q 5 = ([ c5 s5 (55) Q 6 = ([ c6 s6k] (56) Y 584

5 Y Aydin S Kucuk Quatenion Based Invese Kinematics fo Industial Robot Manipulatos with Eule Wist The joint vaiables θ d 3 θ θ 5 θ 4 and θ 6 can be detemined by equating the tems M 3 M M 4 to 3 and 4 espectively The tems M i and i ae obtained the same as in example A ± + p p p l x x y θ = actan + actan (57) py l d ( p xc + p y s ) + ( p z h ) = (58) 3 l p z h p z h θ = actan ± (59) d 3 + l d 3 + l θ 5 = actan± (60) θ 4 = actan actan (6) θ 6 = actan actan (6) 4 43 whee 4 = c 3s 4 = c + 4s 43 = 3c + s 44 = 4c s = wc + cs = ac + bs 3 = bc as and 4 = cc ws C Invese Kinematics fo R Robot Manipulato The igid body model and coodinate fame attachments of the 6R R obot manipulato with Eule wist ae given in Fig 3 θ θ θ 3 Q 3 = ([ c3 s3 < l3s3 0 l3c3 (66) s4k] (67) Q 5 = ([ c5 s5 (68) Q 6 = ([ c6 s6k] (69) The invese tansfomations fo each joint ae Q = ([ c sk ] < l0 h (70) Q = ([ c sk] < l00 (7) Q 3 = ([ c3 s3 < 00 l3 (7) s4k] (73) Q 5 = ([ c5 s5 (74) Q 6 = ([ c6 s6k] (75) The evolute joint vaiables θ 3 θ θ θ 5 θ 4 and θ 6 can be detemined by equating the tems M 3 M M M 4 to 3 and 4 espectively h pz h pz θ 3 = actan ± (76) l3 l3 b θ = actan ( ± b ) (77) ( ) ( ) px + py l l3s3 l whee b = l l l s 3 3 Y Z l X l θ 4 θ 6 Figue 3 Rigid body model and coodinate fame attachments of the R obot The kinematics tansfomations ae Q = ([ c sk ] < lc ls h (63) Q = ([ c sk] < lc ls0 (64) Q = ([ c sk] < lc ls0 (65) l 3 θ 5 y x x y c θ = actan ( p p ) ± actan( p + p c ) (78) l + l3 s3 ll3 s3 px py l whee c = l θ 5 = actan ± (79) 44 4 θ 4 actan + actan 4 43 (80) 44 4 θ 6 actan actan 4 43 (8) 585

6 ICM 006 IEEE 3d Intenational Confeence on Mechatonics whee 4 = 3c3 + 33s3 4 = 3c 34s3 43 = 33c3 3s3 44 = 34c3 3s3 3 = 4s + c 3 = c + 3s 33 = 3c s 34 = 4c s = wc + cs = ac + bs 3 = bc as and 4 = cc ws VI COCLUSIO This pape has attempted to illustate the easy applications of quatenions to the obot kinematics Theefoe the invese kinematics solutions of 6-DOF industial obot manipulatos with Eule wist ae solved analytically using quatenion-vecto pais The fundamental popeties of quatenion vecto-pais have been outlined It has also been shown that fo obot manipulatos quatenion vecto-pais ae compact and efficient mathematical tool to epesent otation and tanslation simultaneously REFERECES [l] JF Bitó Gy Eőss JK Ta A new method fo solving kinematic tasks fo obots Engng Applic Atif Intell Vol 4 o 6 99 pp [] JFundaR H Taylo RPPaul On homogeneous tansoms quatenions and computational efficiency IEEETansRobot Automat vol 6 June 990 pp [3] W R Hamilton Elements of Quatenions Vol I and II ewyok Chelsea 869 [4] E Salamin Application of quatenions to computa-tion with otations Tech AI Lab Stanfod Univ 979 [5] A P Kotelnikov Scew calculus and some of its applications to geomety and mechanics 895 [6] EPevin J A Webb Quatenions in vision and obotics Dept Comput Sci Camegie-Mellon Uinv Tech Rep CMU-CS [7] YL Gu J Luh Dual-numbe tansfomation and its application to obotics IEEE J Robot Automat Vol pp [8] J H Kim V R Kuma Kinematics of obot manipulatos via line tansfomations J Robot Syst Vol 7 o pp [9] F Caccavale B Siciliano Quatenion-based kinematic contol of edundant spacecaft/ manipulato systems In Poceedings of the 00 IEEE intenational Confeence on Robotics and Automation pp [0] M A P Rueda A L Laa J C F Maineo J D Uecho and JLG Sanchez Manipulato kinematic eo model in a calibation pocess though quatenion-vecto pais In poceedings of the 00 IEEE intenational Confeence on Robotics and Automation pp [] VMilenkovic BHuang Kinematics of majo obot linkages Robotics Intenational of SME Vol Apil 983 pp 6-3 APPEDIX A Table Some tigonometic equations and solutions used in invese kinematics Equations a sin θ + bcosθ = c a sin θ + bcosθ = 0 cosθ = a and θ = b Solutions θ = Atan ( a b) m Atan ( a + b c c) θ = Atan ( b a) o θ = Atan ( b a) sin θ = A tan ( b a ) cos θ = a θ = A tan m a a sin θ = a θ = A tan a m a 586