ROBOT ANALYSIS AND CONTROL H. ASADA and J.-J. E. SLOTINE Massachusetts Institute of Technology A Wiley-interscience Publication JOHN WILEY AND SONS New York Chichester Brisbane Toronto Singapore a ‘ ao“ eee Chapter 2 KINEMATICS I: GEOMETRY Manipulator kinematics is «study ofthe geomet of ehipulator a ‘he performance of speifie tasks Is sehleved through the mov rotons, Since wat of the manipulator acm linkages, Kinerase» fendame tool in manipsator desig 9 contac. Ue thi chapter, ‘he mathematical tale required to desribe arm linkage motion ate developed. Abo, he fundamental equations that govern Kiematie Weave are derived and the sstion of hove uations is dsewsed 2.1, Mathematical Preliminary 2.1.1 Position and Orlent of « Rigid Body “The ato inkage of « manipulator can be modeled asa system of tigid bodies. The locaton ofeach singe righ body is completly dreeribed by ie postion aud eietaio. ‘The postion ean be repreeated by the coordinates of an arbitrary pit feed with tespect tothe rg body. Let Onzys bea cord © bean abitrary ‘he rg body i “i : here tg it 13 X 1 column yer te frame fied to the ground and Bet point Fred 10 the sgid body, a shown in Figure sated with reference to the coordinate fame O-rs2 by | Then the position of16 Kinematic I: Geometey Pheer Bt: Post an ‘To represent the orientation of the rieid body, the coordinate ance zy, yyamd 3 are tached to the rigid body at shown ia te figate, These aes form another coordinate f Omry4%5, which mover with the rig Yody. The orientation of the right Body i the represented by the directions ofthese coordinate axes, Lat n, tand be unit vectors pointing the ditetons ofthe coordinate ate, x,y, and 3, respectively. The components of exch unit sector are dirsetion sonar of rach coordinate ax projected into the five cord (O-ays, For convenience, te combine the three vectors together and write them sing the 3X S matte R Rained] aa ‘The matrisR completely describes the oin!ation ofthe iid body with zference tothe Fed frame Omsys, Note that the colsma vectors of matels Rate orthogonal to each stimo a amo snd further have nit eet Ina laa Bet {where [al designates the Buctiian norm of @ veetor a Such a matrix in which all che 21 Mathematical Preliminary 17 column vector Ive unit length and are orthogonal to each other, efersed to as an rthonorne tea Dolnt fa space, as shown in Figure 2.2. We represent the ‘oondinates of point P with reference tothe fixed fare O=rys by -() ‘The pelion af pont P can be abo represented with reference tothe coordinate fame fixed 0 Me ied body, Om eyes, by () : “The superscript #indieates th ‘the vector ib defined with reference tothe body coordinate frame, Lat ut now find the relatonship between the two cordate syste. This relationship Aefines the coordinate transformation between the ied frame and the body coordinate frame ‘The postion and exentation of the rg body, which are represented by the 5X 1 Yelor My18 Kinematics i Geometry ! and the 8 X 8 matt Rin the previous selion, are now wed (o obtain the conedinste i tramaormation. AS shown in Figute 22, the point P 0}, Aand B. This is represented mathematically by sn be reached through points : OF TOs OR AT OP where OFT x and TO my, Note that the sectors TAT AB" and Pave ral tothe sit vectors 3d, Thus, we can fei the above expression as and , rexpetively, apd that thelr lengths ae prem by» xeaytent eta a sce rom (2-2) nd (2-6), a xen Re eo) Equation (2-0) provides he desied coordinate transformation from the body coor the Fixed coordinates x, Note tht this coordinate (ransortation is gven in terms of xy and R,, which represent the postion and erentation ofthe sgh body, ot of the Body caordnste frame relative tothe fed fame spoce RT of matic R, Let ws premltiply both sider of equation (2.0) by het Rig RTs Ta! Prom (23) sod (2+), the mates product RTR om the right-hand side become nix one ay 10 Rr=| va ety [=| 0 Va Be Tb oon 21 Mathematical Preliminary 19 Flew 28; Exagh aha Rlag + RT em Equation (212) repreeens the coordinate transformation from the ised cardinals to the ody coordinate, that ithe inverse of the orginal transformation (20) Thus, the inverse luansformation is simply oblained by using the trampore of the matrix RL Alko, ax equation (241) sows the ed mats tee ofan orthonormal ita i simply ven by the tans rien Example 21 coincides with the L045 ‘Axis 44,00 the other band, coincides with ane & Let ws find the vector np and the mats R As shown in Figure 2, the ovigin of coordinate lrame O's ‘oviin ofthe ied feame O-rys. The angle beeen axes x andy ix dented by, that represent the position and ot tion of frame Oey relative lo frame O=ry, and ‘then obtain the coordinate transformation fm O34 0 O—rye. Since the orglas of the two coordinate frames eon, potion vector tera. Toe oblain (he rotation matric R, lt ws find the thee anit vetors mt, and b, composing R. As2 Kinomation : Geometry shows in Figure 22, tbe components of each recor are its dection cosines with respect to Onays. Therctore, “The coordinate transformation is then obtained by substituting the matic FE and xy =O into fone are thu given by ‘quation (2.0), The componeuts of the transformation exp yer esine, + coe Let us verity the above resus, Figate 24 shows 4 two-dimensional viow of the tw teordinate frames, ‘The point Pie the projection of point Ponto the zy plane. Pols and Ci the oeetin of point B ont the x axis. From this figure, the sbore equations ofthe coordinate B ae projections of the point PY onlo axes x and a, cespectvely, and po ‘wansfrmation can be interpreted flows. We have 1-04 00-37 = UF cee £200 — PPD sin AP 'B ‘Which agrees withthe frst equation of (2:45). The athe equations can be derived inthe same way, 44 21 Mathematial Preliminary 24 o = Example 2.2 Fin in the previous figate, Lat Q be the punt inthe sy plane whore coordinates with sefervace to 25 shows the same (wo dimensonal coordinate frames and arbitrary int 2 ae ‘Onzy ate the came as the coordinate of point PI with tespoet to Omry4. Naraly OG, m A and TT , = PA, as shownin Figue reached by rotating point Qabout the origin O by an ange, The problem i ta show that point P From the figure, 7% =O and LP"OA = £90Q,, Thereore, £QOP = 49,0. and point PU is oblained by the rotation @, of point @ aboat the orgla O. This icussion yes another interpretation of equation (216) and matrix R. If we regard w aud sn (2:15) ab the coordinates of point Q in Omzy, then (218) provides the coordinates of the point P* oblained by rotating point @ about the origin by an angle 6, In the three Aimensions space shown in Figure 2, this rotation f about the = axis. Consequently. she matric Rsteociatd with equation (2.15) represents he ation about the ais and therfore fs called the rotation mari asa Jn eummary, the rotation matt har three distinct physial meanings: It can represent (0) the orientation of the coordinate fame Oz) relative 10 Omzps, where the folome vector represents th dteton covet of tach ate of O22 bolted into the Omzys frame,22 Kinematic Js Geometry (2) the coordinate transformation fom O'-r. coordinates Lo O-2y: coordinates, Ia} the rotation of» veetor inthe O29 coordinate frame ‘The above thee propositions ate equivalent in the sense tht, axing aty one ofthe thre, can derive the other two, While the examples diseased above te special two-dimensional cose, the equivalence of tbe three proportions olde in the general Creediensonal ase ‘This equvseace wil often be exploited in the folowing sections 2.2.3, Boler Angles Jn the previous sections, we waed the 3 x 3 melee R to represent the cientation of & ‘igi body or a cordinate frame attached (othe body. The elements of the mattis, however ave no independent, The mattix has nine elements a tal, whieh se all subject te the orthogonality condi ‘outitons, only thier of the nine clements are independent. In other words, the matric (2-2) andthe wit length condlons (21). Since there ae sic af these ioe i redundant, In this sttion, a representation ofthe rigid ery representation of event [Consider the three rotations of frame O-zys shown in Figure 2-6. Fis, the coordinate frame i rotated about shes aris by an angle ¢ (Figure 2-6]. Secondly, he new coordinate fname O-2¢ is sotated about the 2 axe by an angle # (Figure 2.68). Faly, the newest 1 is then rotated about the 2 asin by an angle The resultant coordinate ame Onsyty i shown in Figure 26¢ . The thre angles, 4, € 204 determine the ed to a8 Ener angles, ‘The Euler titra rigtaton of he cordinate frame oniuely, and ave ct angles are independent, fn that each of them aa Fors ven Aotermined a follows, Let lie Ox? in Figur (| be the intersection Between the 25-9, plane fered to a8 the Fine of node, The angle ie defined 1 the angle from the lve of odes to the 4, asi in Figate (c), while the angle @ isthe angle tray ointation of coordinate frame O22, the Euler eagles ean bo snd the -y plue, This iterection it ‘tom the z axis to the line of nodes, The angle & om the other aad, is defined asthe angle2 Kinematics Is Geometry from he sas (othe 5y=" 2" axis, All the 8 sense, Ths les are measured in 3 rig the three anges can be defined for an a ry arentaton of coordinate fame Oma, relative to he xed frame Omzys. Note, however, thal the Kuler anges are not unique. ‘The set of angles (6 +, 6) giv he same orientation a6 (0, 8 9, a Inter disused in Sesion 232. ‘The Euler angles are independent variables which determine the etientation of a coordinate Ieame uniquely. Let us Gnd the totation is fix R. that represents the thees courcatve rotations aesocited with the Euler anges. Consider he cordinate transformation sssocated with the rotation ¢, Coordinates x'= [2/2] are transformed to coordinates x= [eal by the 3 x 3 rotation matric RG), which i defined as x=Rion! cos sing 0 o cap 0 e-16) oo4 Ryo Sinialy, the coordinate transformation trom xt [eye tox! asocated with the retton #5 Gen by He RAO" where Rl)=| 0 cond sind 0 ind oatd Fally, or the rotation, we have vaRyn! 21 Mathematical Preliminary 2% esp tne 0 Rav] sing coy 0 eon oot Combining the tree coordinate Uransfrmations yes Xe RIIRAOR AY? e190) Let ws replace the above matrix produc by Rigo] = RPI RAD RAG (e-20) “The matric R(¢, 6, provides the coordinate transformation frm x 10%. Ae a eel of the cquivaleney between the cordinat transformation matrix andthe rotation matrix dicasend in Section 21.2, the matrix RG, 6, repre ncrdinate frame O-1,y2,. Ako, the column vector of Re, @,@) represent the dltetion eslne of the coordinate axes 2y yp and 5, with reference tothe Omzy® fre. the rotation from coordinate Frame O92 ta Another independent st of anges, consisting of ral ple, and yeu; is widely wsed in gid ody ovieetation, ‘The rll angle @, teprecen 8 totation aboot = robotic 1 deer sn while the pte and yave angle represent consecutive rotations about corresponding y and 4 aces, Using the notations of Figote 246, the rotation matric asoiated to rolbpitch-yaw is RUIR ARAB) 2.1.4. Homogencous Transformations In this section, we develop 9 sel method for representing coordinate tansfrmstins in compact form. ‘Lt us recall the coordinate transformation given by equation (20 sta! ean26 Kinematicn I: Geometry ‘The first term of tbe righthand side sepreseats the tratsational transformation, while the second term represents the rotational wansfomation. ‘The golf this section isto deve a ‘mole renesentaton of the coordinate tansornation tn which beth the tran onal ad totatons transformation are given by single mari, To tig end, let of define the 4 361 te and the 4 A mati ‘The orginal vectors x and x! are augmented by adding 4 "1" asthe foutth element 30 thatthe result is 4 4 3¢ 1 vector, Alo, the rotation matrix R it atended fo a 4X 4 matt by ‘combining it with the 8% 1 potion vecice xy, with thee O' and a 1 in the fourth tow. Equation (2-2) ean then be written as Xaaxt == hati, een) Note that the 4 X 4 matt A represents both the portion and oveettion of the feame O-eyyyy The (wo terms onthe ighshand side of equation (221) ate reduced to the single 2.1 Mathematical Preliminary 27 term in equation (223). The cordinate transformation given by equation (223) refereed to a the homogeneous transformation “The compactness of the homogeneous transformation ie particularly adantagous whe swe represent conecative transformations, Let O's,jz, be another cotdinate fears, 2 shoo x Figne 27, and abe the ordinates of ol Pith sefeence to OP=r2¢- Then Bay eRit i225) where xh and RU are the 3X 1 veetor and 3 X 3 mals asernted with he coordinate from x® to x8, Sebattuing (2.25) ino (221), we etal learstrra wang 4 Rah + RR a0) nd side of equation (2-26). As the transformation is ‘There ae now thtce term ia the rgb repeated, the numberof terms Inthe righthand side Igcreaees. In gnetal, n canseative coordinate Uransformations lead to a yeth order polynomial consisting of (#1) nom omnogencous terms, ‘The horogencos transformation which wees the 4 X 4 mari, on the other hand, provides a compact form that represents any consecutive transformation with 428 Kinematics I Geometry slog erm. Consider consecutive transformation from frame m back to frame Lat A‘! be the 4 x 4 matri asocated with the homogeseous tesnsormstion from frame Fto frame 521 than position warton CF ram mi tenant Xin fare 0 by XP ADAL = ATES ‘Thus the consecutive teeaformations are compactly decribed by a angle er, ‘The 4 x 4 matrices bave te other properties equivalent to those diseweed eater for Folation matrices. A 4X 4 mate represeats tbe position and orientation of coordinate fame 1 represents tbe tanstion and roteion of the coordinate frame, Thus, the aglvalence property for rotation matrices also elds for 44 matrices, in which both Leapalations and rotations ae fvolted In this examplo, we derive an expression of the inverse of the 4X 4 matrix A , vxing the coresponding inter coordinate teenformation ‘The inverse coordinate ansermtion given from (223) by a= AK (2-28) From equation (212), the same inverse transformation can be expreted in the 9% 3 matrix form aba Rig + RT Let ws now convert the above expression into the 4X 4 malt frm, and determine the matri AY Comparing equation (2-20) with equation (2-2), we find that ng of {221 s replaced by RTs tn (2:20), while Ris simply replaced by RT. Applying the same conversion atin tution (2.29, we obtain 22 Kinomatle Modeling of Manipulator Arms 29 (2-20) | at RT | Ry 1 1 ‘The above result cam alo be proved by cbecking that both products ALA and At Indeed egul tothe identity mattis Asa 2.2, Kinematic Modeling of Manipulator Arms 2.2.1. Open Kinematic Chaine ‘The mathematical tools that we developed in the previous section ave now applied to ‘he Lineratle modeling of manipulator arms. In pattie We wSe the Romogenrons ‘anformation to describe the postion and orientation of each ink member involved in 4 ‘msnipultor arm, A mi Figure ator arm is baseally series of rigid bodies io kinematie strut i bes. Sacha linkage with a 24 shows s manipstor arm modeled as ell Kinkage of