2 14 Robot ann dynamics A14 = a, COS O4 + d6 COS e4 Sin 8, A,, = sin 8, cos e5 cos 8, - cos e4 sin 8, Azz = -sin O4 cos 8, sin 8, - cos 8, cos 8, A23 = sin O4 sin 8, A24 = a4 sin O4 + d, sin O4 sin 8, A3, = sin 8, cos e6 A32 = -sin 8, sin e6 A,, = -COS 8, A34 = -d6 COS 8, A4, = 0 A4* = 0 A,, = 0 A- = 1 In this example the end effector is shown in a position for which 8, = 90", 8, = 90" and O6 = 0. The complete transformation from the (~2)~ axes to the (xyz), set of axes is 3[A16($)6 ($10 = 0[A16(d)6 = o[A13 or, in general terms, (8.30) It is seen by putting x6 = 0, y6 = 0 and z6 = 0 that (ry,rz) is the location of the origin of the (Vz)6 axes. If we put x6 = I with y6 = o and z6 = o we have x, - rx = n, Yo - yv = nv z,, - r, = n, Therefore (n,n,n,) are the direction cosines of the x6 axis. Similarly the components of (s) are the direction cosines of the y6 axis and the components of (a) are the direction cosines of the z6 axis. These directions are referred to as normal, sliding and approach respectively, the sliding axis being the gripping direction. 8.3.13 THE INVERSE KINEMATIC PROBLEM For the simpler cases the inverse case can be solved by geometric means, see equations (8.1) to (8.3); that is, the joint variables may be expressed directly in terms ofthe co-ordinates and ori- Kinematics of a robot ann 2 15 entation of the end effector. For more complicated cases approximate techniques may be used. An iterative method which is found to converge satisfactorily is first to locate the end effec- tor by a trial and error approach to the first three joint variables followed by a similar method on the last three variables for the orientation of the end effector. Further adjustments of the position of the arm will be necessary because moving the end effector will alter the refer- ence point. This adjustment will then have a small effect upon the orientation. Small variations of the joint variables can be expressed in terms of small variations of the co-ordinates. For example, if (p) is a function of (e) then or ae, ae, ae, (8.3 1) where [D] is the matrix of partial differential coefficients which are dependent on position. It is referred to by some authors as the Jacobian. The partial differential coefficients can be obtained by differentiation of the respective A matrices. The matrix, if not singular, can be inverted to give (W = [DI-' @PI0 (8.32) since (Ae) = (e)n+l - (01, (e)n+l = (e), + [DI,' (~~10 (8.33) Repeated use enables the joint positions to be evaluated. In general since (PI0 = ,[AI, * . n-I[Aln (@In then where qi is one of the variables, that is 0 or d. for a revolute joint or of di for a prismatic/sliding joint. It should be noted that in this context (p),, is constant and that i-,[A]i is a finction of Bi For the general case ;-,[AI; = SO, CB;Ca, -CBiSai aisei Sai Cui 0 0 1 ;-,[AI; = 1 cei -SeiCai SB,Sa, aiCei SO, CB;Ca, -CBiSai aisei 0 Sai Cui 0 0 0 1 so differentiating with respect to Oi 2 16 Robot arm dynamics 1 1 -SO, -CB,Ca, CB,Sa, -a,SB, CB, -SB,Ca, SB,Sa, a,CB, 0 0 0 0 0 0 0 0 CB, -S0,Ca, SB,Sa, a,CB, SO, C0,Ca, -CB,Sa, a,SB, a 30, ,-i[AI, = - Note that the right hand side of equation (8.34) may be written Ca, dl 0 0 0 1 or in symbol form a 80, ,-i[AI, = [Ql ,-i[Al, - where In a similar manner (8.34) (8.35) (8.36) where 0000 [I::::] 0001 8.3.14 The basis for determining the joint velocities given the motion of a particular point has already been established in section 8.3.13 where the matrix [D] was discussed. If we con- sider the variations to take place in time At and then make At + 0 then, by definition of velocity, (do = [Dl(e) (8.37) LINEAR AND ANGULAR VELOCITY OF A LINK so the joint velocities can found by inversion <e> = [DI-'(P)o (8.38) Kinematics of a robot ann 2 17 Refemng to Fig. 8.20 we can also write ($10 = o[Aln(d)n Thus (8.39) d dt - ($10 = 0[21n(~)n where ($)n is constant. Consider the product of two A matrices [ ::;’ (:,,I [ ‘1 (,,;I = [ [RI;;], 1 [RlI(u:, + (41 ] It is readily seen that for any number of multiplications the top left submatrix will be the product of all the rotation matrices and the top right submatrix is a func- tion of [R] and (u) submatrices. So in general the product of A matrices is of the form [RI (r) [A1 = LO) 1 1 We have already shown that the column matrix (r) is the position of the origin of the final set of axes and the three columns of [R] are the direction cosines of these axes. So (p)o = [R](p), + (r) and the position of a point relative to the base axes is (ph - (r) = (AP) = [RI(p)n Differentiating with respect to time gives the relative velocity (AP) = [RI(P)~ (8.40) Fig. 8.20 2 18 Robot arm dynamics so it is seen that [i] is related to the angular velocity of the nth link. Now (Ab) is the veloc- ity of P relative to 0, referred to the fixed base axes. We can find the components referred to the (xyz), axes by premultiplying (AP) by [RI-’ = [RIT thus (8.4 1) We know that [RIT[R] = [I] so by differentiating with respect to time we have [R],T[i] + [iIT[R] = [O] and as the second term is the transpose of the first it follows that [R] [R] is a skew symmetric matrix. This matrix will have the form (Ab), = [RI~(A~) = [RI~[RI(P)~ where (a,ra,a,)T is the angular velocity vector of the nth link. Now i=n [Rln = GI [Rli so 101; = [RlT,[Rln = [RIT, 2 [Rll[R12 * [Ql[Rli [Rlnqi where (8.42) 0 -1 0 [.I=[ 01 for the 3 X 3 rotation matrices. Each term in the above series is equivalent to the change in [a]’ as we progress from link to link. 8.3.15 LINEAR AND ANGULAR ACCELERATION The second differentiation can be found by simply reapplying the rules developed for the first differentiation with respect to time. We see that since (b)o = 0[4n(b)n (8.43) In order to see the operation let us look at a two-dimensional case and with n = 2, as shown in Fig. 8.21, for which theA matrices are functions of 8’ and 8, so that (810 = 0[4l I[42(b)2 where Kinematics of a robot arm 2 19 Fig. 8.21 and the A matrix for the two-dimensional case (for which a = 0) may be written I lo 0 1 cos 8 -sin 8 a cos 8 [A] = sin 8 cos 8 a sin 8 Therefore (P>o = [Qlo[Ali i[Al2 9i<P>2 + o[Ali[Qli[Al2 e2(P)2 and (P>o = [QloZ[Ali i[Alz ef(P12 + [QIo[~li[QI~[~l~ eie2(P>2 + [Qlo[Ali iW12 4i(P)2 + [Qlo[~li[Qli[~lt eie2(P>2 + o[Ali[Ql~[Al2 e,’(P>2 + o[~li[Qli[~l2 GI($), If we require the origin of the (x Y)~ axes to follow a specified path then for each point on the path, (x Y)~, the corresponding values of 8, and O2 can be found. Also if the velocities and accelerations of the point are prescribed the derivatives of the angles can be calculated using the above equations. Once the values of e,, 8, and their derivatives are known any linear or angular velocity and linear or angular acceleration can be found. The above scheme can in principle readily be extended to the three-dimensional case and any number of links can be considered. The general form of the equations is i=n (PI0 = !i i-i[Ali(P)n = [Wn(P)n (8.44) where [un is defined by the above equation (8.44). 220 Robot arm dynamics The velocity is where [Uln,i is a fimction of the A matrices and hence of the joint variables, that is [Uln.i= ,[All . . [Qli-~[Ali . . * ,[AI, (8.46) For the acceleration I= I where (8.47) (8.48) We have shown previously that in general so [R] (r) [AI=[ ] Since, for constant (p),,, we have tb) = [jl<i>n then we have (PI = [RI(P)~ + ('1 and thus the acceleration of P relative to the origin of the (vz), axes, (Ap),, = (p) - (P) = [RI(P)~* If we now refer the components of this vector to the (vz), axes we have [~l'(~ii)n = [RI~[~I(P)~. [0IX = [R]'[d] Now so [&IX = [R]'[R] + [RIT[k] [R]'[k] = [&I" - [ri]'[ri] and therefore Kinematics of a robot am 22 1 Also [RIT[il = [ilT([~l[RIT) [il = ([~lT~~l)T([~lT~~l) = [o]xT[o]x = -[o]"[o]" Finally [RI~(AP)~ = [RI~[RI(P)~ = {[&Ix + [oIX[~Ix)(p)n = [bI"(p)n + [oI"[~I"(p)n (8.49) where (p), is of constant magnitude. This result is seen to agree with that obtained from direct vector analysis as shown in the next section. 8.3.16 DIRECT VECTOR ANALYSIS It is possible to derive expressions for the velocity and acceleration of each link by vector analysis. The computation in this case uses only 3 x 1 and 3 x 3 matrices rather than the full 4 X 4 A matrices used in the last section. Referring to Fig. 8.22 we see that the position vector of the origin of the (xyz), axes is r, = r,-, + d, + 0, (8.50) Fig. 822 222 Robot arm dynamics and The velocity of 0 is dr, dri-l dt dt + - (d, + a;) + 0, x (dj + ai) at Vi=-=- (8.51) (8.52) We require our reference axes to be fixed to the ith link in order that when the moment of inertia of the ith link is introduced it shall be constant. For the second term on the right the partial differentiation means that only changes in magnitude as seen from the (qz), axes are to be considered. For a revolute joint where both d and u are constant in magnitude this term will be zero. For a prismatic joint the term will be Aiki-,. To simplify the appearance of the subsequent equations we again use ui = dj + ai (8.53) and write vi-, for d(r,-,)/(dt) so equation (8.52) becomes au, at v. = v. + - + 0; x ui 1 1-1 For a revolute joint the second term on the right hand side is zero. The acceleration of 0; is (8.54) (8.55) For a revolute joint the second and fourth terms on the right hand side are zero whilst for a prismatic joint the third term is zero. Both of these equations may be expressed in matrix form assuming that the base vectors for all terms are the unit vectors of the (qz), axes. Thus we may write aL, aO, au, at2 at at u, = tZ-1 + - + - x u, + 0, x - + 0, x (0, x u,) (8.56) a at (VI, = (a, + - (41 + Lo]: (41 and a2 a a at at at (a), = (a),-, + 7 W, + ( - [a]: ) W, + [a]: - (u), + [o~:[ol:(u), (8.57) where a [a]: = [R]:[R], and -[a]: = [R]T[d], + [R]f[k], at also [RI, = [RI~ . * [QI[RI, * * [~~nii and [k], = [R], . . . [Q][RIj . . . [R],9, + C C [R], . [Q][R], . . . [Q][R], . [R],eiij i=n j=n ,=I j=l Kinetics of a robot am 223 8.3.17 TRAJECTORY PLANNING AND CONTROL In a practical pick and place type of operation an object is to be moved from point A to point B and, for example, has to avoid an obstacle C, as indicated in Fig. 8.23. The problem is often tackled by planning for the end effector to move from A to B so as to arrive with low speed at B and then to align the gripper. The exact path is not important apart from the three specified points, so there are an infinite number of possible paths that the arm can fol- low. The many factors which affect the choice of path are outside the scope of this book as we are concerned only with the pure dynamics of the problem. One technique used is to consider the path to be constructed from short segments passing through a number of prescribed points. Usually a polynomial of third or fourth order is cho- sen to represent the path between the specified points. Another powexfbl method is to use position sensors to give feedback to an automatic control system. These control systems are frequently digital, which makes adaptive control easier. Fig. 8.23 8.4 Kinetics of a robot arm Our next task is to determine the forces and couples associated with the prescribed motion. In the practical case it is not always possible to generate the required forces so the motion which ensues from given forces may need to be calculated. A dynamical model may be used for the prediction of performance or for forming part of a real-time control algorithm. We shall use Lagrange’s equation in conjunction with homogeneous transformation matrices and the Newtoe-Euler approach using a vector algebra method. It should be noted that both Lagrange and Newton-Euler could be associated with either the homogeneous matrix formulation or vector algebra. 8.4.1 LAGRANGE’S EQUATIONS Here we only generate one equation for each degree of freedom of the system and the basic formulation only needs expressions for velocities and not for acceleration. However, differ- entiation of the velocity- and position-dependent functions is required. [...]... torque about the zo axis acting on link 2 and F3 is the force acting on link 3 along the Z, axis The kinetic energy (see Fig 8.26) is T = - m,(a6)’ 2 { l For qi = 8, + 124 + m3[(d3 - b),g + ,I ai] + 13% aT = [rn2a2+ 1, + I~ + m,[(d3 - b12]i2 8% - -.- dt (aT) d ae, = [rn, + I* + I, + m3(d3 - b)2~82 2m3(d3 - b)i3& + Fig 8.26 Discussion example 23 1 ar - av - -0 , - = O and Q = C 2 (302 a02 so the equation... the ‘inertia forces’ equals zero For virtual displacement 65 = 60,’ (6d3 = 0) C260, - 126 ,60, - m2a6,a60, - m3[(d3- b)8, + 2a,0,](d3 - b)68, - 1 ~ 6 ~= 0 , ~ o and now with 65 = ad,, (68, = 0) F,M, + m3[(d3- b)e: - d3]6d3= o Thus C, - [I, + r3 + m2a2 + m3 (d3 - b)’16, Fig 8.27 - 2m,ci,B,(d3 - b) = o 232 Robot arm dynamics and F, + m3[(4 - b)G2 - d3] = 0 Let us now return to the Lagrange method but... follows that Xmx2 = 0, Emy2= 0 and Ern2 = 8.8kgm The term Xmz = 20x0.2 = 4 The inertia matrix for link 2 is [J12 = I 0 0 () 0 0 0 8.8 4 similarly for link 3 0 0 4.9 -3 :I 4 20 Ej -3 10 Robotics data sheet 233 All other terms are zero Thus 15.38, + 5.00,d3 = C, In this problem the geometry is particularly simple so that the 4x4 matrix methods can readily be compared with other methods It is left as an... 0[i]3 = QA,A2A3i), + A,QA,A,i), + A,A,P!3Li, As 4, = 0 the first term is zero and by direct multiplication the other two terms are I 0 -0.866 -0.500 0 I 0 0 0 0 0 0.5 -0.866 -0.779 0 0 1 230 Robot arm dynamics A,A2PA3d3 = 0 0 0 i o 0 0 0 0 0.866 0.500 0 0 0 0 1 The velocity of E is given by summing the last columns of the preceding two matrices XEO =0 YE0 = 0 4 ~ 2 0 8 6 6 ~ 3= 3 3 9 8 d ~ + 2, =...224 Robot arm dynamics Thus the kinetic energy of link n will be (8.58) where R is the number of point masses used to represent the rigid body For an exact representation R 2 3 The total kinetic energy will be just... by reversing the order of s u m mation This requires adjusting the limits The form given below can be justified by expansion The term max(ij,k) means the highest value of i,j, or k (8.65) 226 Robot ann dynamics where (8.66) (8.67) 8.4.2 NEWTON-EULER METHOD This method will involve all internal forces between each link as indicated on a free-body diagram, as shown in Fig 8.24 Expressions for the accelerations... sheet given at the end of the chapter we see that as a is the distance between successive z axes all the a dimensions are zero The distance between successive x axes is d and hence d , = 0 228 Robot ann dynamics (4 Fig 8.25 (a) and (b) Now a is the rotation of one z axis relative to the previous z axis so aI = -90" and a, = 90" Special care is needed to ensure that the signs are correct The table is as . 1) where [D] is the matrix of partial differential coefficients which are dependent on position. It is referred to by some authors as the Jacobian. The partial differential coefficients. 9i<P>2 + o[Ali[Qli[Al2 e2(P)2 and (P>o = [QloZ[Ali i[Alz ef(P12 + [QIo[~li[QI~[~l~ eie2(P>2 + [Qlo[Ali iW12 4i(P)2 + [Qlo[~li[Qli[~lt eie2(P>2 + o[Ali[Ql~[Al2 e,’(P>2. Fig. 8.26) is T = - m,(a6)’ + 124 + m3[(d3 - b),g + ai] + 13% 2 l{ .,I For qi = 8, aT = [rn2a2 + 1, + I~ + m,[(d3 - b12]i2 8% - = [rn, + I* + I, +