RSD-TR-8-82 AN EFFICIENT FORMULATION OF ROBOT ARM DYNAMICS FOR CONTROL ANALYSIS AND MANIPULATOR DESIGN C. S. G. Lee B. H. Lee R. Nlgam Department of Electrical and Computer Engineering The University of Michigan Ann Arbor, Michigan 48107 April 1982 CENTER FOR ROBOTICS AND INTEGRATED MANUFACTURING Robot Systems Division COLLEGE OF ENGINEERING THE UNIVERSITY OF MICHIGAN ANN ARBOR, MICHIGAN 48109

TABLE OF CONTENTS 1. Introduction......................................................................................... 3 2. The Generalized d'Alembert Equations of Motion.............................. 5 3. Applications to Obtaining Simplified Dynamic Model........................ 14 4. Equations of Motion for a Three-link PUMA Robot........................... 17 5. Conclusion........................................................................................... 32 6. References............................................................................................ 32 * @

RSD-TR-8-82 ABSTRACT This paper presents the development of the generalized d'Alembert equations of motion for application to robot manipulators with rotary joints. These equations, when applied to a robot arm, result in an efficient and explicit set of closed form second order nonlinear differential equations with vector cross product terms. They give well "structured" equations of motion suitable for state-space control analysis. The interaction and coupling reaction forces/torques between the neighboring joints of a manipulator can be easily identified as coming from the translational and rotational effects of the links. With this information, either a simplified dynamic model can be developed or an appropriate controller can be designed to compensate the nonlinear effects. Application to obtaining simplified dynamic model is discussed together with the computational complexities of the dynamic coefficients in the generalized d'Alembert equations of motion. The dynamic equations of the first three links of a PUMA robot are worked out to illustrate the method. This work was supported in part by the National Science Foundation Grant ECS-8106954 and the AFOSR Grant F49620-82C-0089. Any opinions, findings, and conclusions or recommendations expressed in this article are those of the authors and do not necessarily reflect the views of the funding agencies. Generalized d'Alembert Equations 2

RSD-TR-8-82 1. Introduction A priori information needed for manipulator control analysis is a set of closed form differential equations describing the dynamic behavior of the manipulator. Various approaches are available to formulate the robot arm dynamics, such as Lagrange-Euler (L-E) [1-5], Newton-Euler (N-E) [7-12], and recursive Lagrangian (R-L) [13], though mainly two approaches are used by most researchers - the Lagrange-Euler and the Newton-Euler formulations. The L-E equations of motion maintain a well structured form, but computationally it is very difficult to utilize for real time control purposes unless the equations of motion are simplified. The N-E formulation results in a very efficient set' of recursive equations, but they are very difficult to use for deriving state-space control laws. This paper presents the generalized d'Alembert (G-D) equations of motion which give well "structured" equations suitable for control analysis. In addition to having faster computation time for computing the dynamic coefficients than the L-E equations of motion, the G-D equations of motion explicitly indicate the contributions of the translational and rotational effects of the links. Such information is useful for designing a controller in state-space or in obtaining an appropriate approximate model of the manipulator which simplifies the design of the controller. Assuming rigid body motion, the Lagrange-Euler equations of motion, excluding the gear friction and backlash, are a set of second order coupled nonlinear differential equations. In general, the necessary generalized torque r; for joint i to drive the ish link of the robot arm can be written as [1-5], Tr Jki }- t (1) - e ua =lr for i= 1.2, These equations can be expressed in matrix form explicitly, C,, + C,,,OkO,, + CG,= r,; or i = 1, 2,.-.,n (2) k=1 k1'm=1 where c= - r3 JT d a;Td k = fo i,.1,2,, (3) =mar(it) e8t Eqai Generalized d'Alembert Equations 3

RSD-TR-8-82 jCan=.s,k ad; for i,k,m =1,2,,n (4) i —,..(ltk,) 90t08, MSi C.= VI jm(, }i; for i=1,2,,n (5) and To' is a 4x4 homogeneous link transformation matrix which relates the spatial relationship between the skh and the base coordinate frames, J, is the inertial matrix of link i about the,tj coordinate frame, i, is the position of the center of mass of link i with respect to the sjA coordinate system, s is the gravity row vector - (gsgygzo) and jg-=9.8062m/s2, the superscript T on vectors and matrices indicates the transpose operation, and n is the number of degrees-offreedom of the robot arm. The above dynamic equations of motion are highly nonlinear and consist of inertia loading, coupling Coriolis and centrifugal reaction forces between joints, and gravity loading effects. Furthermore, these interaction torques/forces depend on the manipulator's physical parameters, instantaneous joint configuration, and the load it is carrying. To improve the speed of computation, simplified sets of equations have been proposed by many investigators [2-4, 6]. In general. these "approximate" models simplify the underlying physics by neglecting second order terms such as the Coriolis and centrifugal reaction terms. However. at high arm speeds the neglected terms become significant, making the accurate position control of the robot arm more difficult [16]. As an alternative to deriving more efficient equations of motion, NeuwtonEuler equations of motion were developed [7-121. The derivation was based mainly on the "moving coordinate systems" and d'Alembert's principle. The resulting Newton-Euler equations of motion, excluding the gear friction and backlash. are a set of compact forward and backward recursive equations. This set of recursive equations can be applied to the robot links sequentially. The forward recursion propagates kinematics information (such as angular velocities, angular accelerations, linear accelerations, and linear acceleration about the center of mass of each link) from the base reference frame (inertial frame) to the end-effector. The backward recursion propagates the forces exerted on each link from the end-effector of the manipulator to the base reference frame and the applied joint torques are computed from these forces. Because of the nature of the formulation and the method of systematically computing the joint torques, computations are much simpler. The most significant of this formulation is that the computation time of the applied torques is found linearly proportional to the number of joints of the robot arm and independent of the robot arm Generalized d'Alembert Equations

RSD-TR-8-82 configuration. This enables the implementation of simple real-time control algorithm for a robot arm in the joint-variable space. The inefficiency of the equations of motion as formulated by the L-E method comes mainly from the 4x4 homogeneous matrices describing the kinematic chain [14-15], while the efficiency of the N-E formulation can be seen from the vector formulation and its recursive nature. Turney et al. [14] explicitly verified that one can obtain the L-E equations of motion from the N-E equations, while Silver [15] showed the equivalence of the -rE and the N-E equations of motion through tensor analysis. To further improve the computation time of the Lagrangian formulation, Hollerbach [13] exploited the recursive nature of the Lagrangian formulations. However, the recursive equations destroy the "structure" of the dynamic model which is quite useful in providing insight for the controller design. For state-space control analysis, one would like to obtain an explicit set of closed form differential equations that describe the dynamic behavior of a manipulator and the interaction and coupling reaction forces in the equations can be easily identified so that an appropriate controller can be designed to compensate their effects. To obtain such an efficient set of equations of motion, Huston [12] employed Kane's dynamical equations to develop an algorithmic approach for deriving the equations of motion suitable for computer implementation. Another approach of obtaining an efficient set of closed form equations of motion is based on the generalized d'Alembert principle. Chace [17] successfully used the technique for multi-freedom mechanical parts. This paper extends Chace's work to develop the generalized d'Alembert equations of motion (G-D) for mechanical manipulators with rotary joints and shows the equations of motion explicitly in vector-matrix form. The method uses Euler transformation matrices ( or rotation matrices ) and relative position vectors between joints to increase the computational efficiency. The G-D equations of motion of a three-link PUMNA1 robot arm are then worked out showing the simplicity of the method. For a rather "loose" comparison, the computation of the applied joint torques from the G-D equations of motion is of order O(n3), while the L-E equations of motion is of order O(n4), and the N-E equations of motion is of order O(n), where n is the number of degrees of freedom of the robot arm. 2. The Generalized d'Alembert Equations of Motion Computationally, the bE equations of motion are inefficient due to the 4x4 homogeneous matrix manipulations, while the efficiency of the N-E formulation can be seen from the vector formulation and its recursive nature. In order to obtain an efficient set of closed form equations of motion, one can PUMA1 is a trademark of Unimation Inc. Generalized d'Alembert Equations 5

RSD-TR-8-82 utilize the relative position vector and rotation matrix representation to describe each link's kinematics information, obtain the kinetic and potential energies of the robot arm to form the Lagrangian function and apply the Lagrange-Euler formulation to obtain the equations of motion. With reference to Figure 1, assuming that the links of the robot arm are rigid bodies, the angular velocity w, of link 8 with respect to the base coordinate frame can be expressed as a sum of the relative angular velocities from the lower joints, w.e I~= 6j~rl(6) j=1 where sj_1 is the axis of rotation of joint j with reference to the-base coordinate frame. Premultiplying the above angular velocity by the rotation matrix RO changes its reference to the 8dt link coordinate frame, Rw, O BiR sP, (7) i=1 Let r, be the position vector to the center of mass of link 8 from the base coordinate frame. This position vector can be expressed as, -r = E pi* + c- = pol +' (8) j=1 where pi' is the position vector from the origin of link — 1 coordinate frame to the origin of link j coordinate frame with respect to the base coordinate system, P,-i is the position vector from the base coordinate frame to the origin of link 1- coordinate frame, and c, is the position vector of the center of mass of link 8 from the (5l1)th coordinate frame with reference to the base coordinate frame. Using Eqs. 6-8, the linear velocity of link 8, v,, with respect to the base coordinate frame can be computed as a sum of the linear velocities from the lower links, V.= 1 eis_.1 xpkt + x-. (9) k —j= I=I The kinetic energy of link 8 (1 < 8 < n) with mass m, can be expressed as the summation of the kinetic energies due to the translational and rotational effects at its center of mass [18]: 6 Generalized d'Alembert Equations

RSD-TR-8-82 Joint i Link i Link i+ Zi-nate System Figure 1 Vector Definition for the GDELink i + s G a i Link i-17 Yi-1 Pi xi q Yi _i-1 pi Base Coordinate System /iue1Vco eii~o o h - qain [eeaie 'lmbr qain

RSD-TR-8-82 K =- (K9),r + (K)o = m v + (ROw I, (Rw,) (10) where I, is the inertia tensor matrix of link a about its center of mass expressed in the 81h coordinate system. For ease of discussion and derivation, the equations of motion due to the translational;- rotational, and gravitational effects of the links will be considered and treated separately. Applying the Lagrange-Euler formulation to the above translational kinetic energy of link 8 with respect to the generalized coordinate a, (8 > i), we have d (K,)tra a(K.)tra d- &" N" di [ J 6To ae(11) a *v S d Os at v, m,, + mev, - - msm vw where 06, =FI X (p,+ P-l,+ '''+ P t, -i+ e. =e -,I X (,-p.-1_; > i (12) Using the following identities, a Zv aJ, and o. _N (13) dt Oi; O d9i a0i Oe Eq. 11 becomes d [ (K,)t,, ] O(KI,)tr. i;..|, X (,_ Pi)} (14) dt O9i 00 Summing all the links from i to n gives us the reaction torques due to the translational effect of all the links, 8 Generalized d'.lembert Equations

RSD-TR-8-82 d o '(KE.),,,, __ 4d 1.(K.)tr,, = _(K8)tr* 1 dt L -ao, ] T t J 8~( ]=- -.1,X(15) ----. ' s,-I.x (Fs-ps-0 where, the acceleration of link s is - { EU | Ojsj-l X Pkt + [ oslj x E# = 1 [i -- X P + { IjI X Pt(" Oj.i Xj.i X Pk (1) + { fIz9J X 'E + { e(O#jIJ X | iziI X EJ} 0(16) I~l It 9)-i + E? - X C ' {' I X Pk' lJ-l X Z + I-1 X CE k=2 p=2 q= l p=2 ' — l Next. the kinetic energy due to the rotational effect of link 8 is: 2 12 (Ks)rot (=- 2 ( 1R,~W, I )= I { v 6,RosR.l} T4 ~ 0,R~zi; (17) where I, is the inertia matrix about the center of mass of link 8 with respect to the base coordinate frame. Since a(K)ro, = [R i] Is R ~il,; i (18) a" (R Ozj —) = R Ozj~- x Ric; i>j (19) (90 {R.0z,1) = R - X Rsl; i 2 and d- (R,01) = Y{ R O,_, dt: = ROs., x (9R r5l (20) Generalized d'Alembert Equations 9

R.SDTR- 8-82 then the time derivative of Eq. 18 is -~ { ~ t, d '[ -t - x R ]R TJ.[ I,|;., ro] T. — ] T Ii d + [R ] I. jR, s-j] + [R Isl] T i R,_ X R tRt] Next using Eq. 19, we can find the partial derivative of (K.)rot with respect to 1 ol(21) the generalized coordinate 0i (8 _> O, 0 i ~ + Rvtrs - R |siI, x R ^t) I# sjl- j Ok (22), -'1 ~ [R:OIC] T 1 a~a O~i-1X a~O —I he reaction torques due can findt the rotational effects of all the links, dthe generaliz 86 o d> 80 J 0, =f- X(-R#) T-I ' ae /j[l- + (Ris"I) 4 jR~ si- X Z kRsk-Ji(23) Subtracting Eq. 22 from Eq. 21 and summing all the links from i t-o n gives us +{R%,;_ X E{t~lt'~'-'J I, oiaRys_; i 1,2,,n The potential energy of the robot arm equals to the sum of the potential energies of each link, P.E.= -3 P. (24) where P, is the potential energy of link s and is found to be 10 Generalized d'Alembert Equations wher Pis te ptenial nery o lin 8 P. E. nd o 9

RSD-TR-8-82 p, = - m * m, — =-g ' m,(pl- + pi,' + *. + (25) where g = (g,,gy,gz)T and Isl - 9.8062m/A2. Applying the Lagrange-Euler formulation to the potential energy of link 8 with respect to the generalized coordinate 6i (8 > i), we have d a(PO) (P.) (Po) O(Pi1 + Ps + * + ) d-, _, _, CI O*, dt I ao, iJ- 86, - - S =mg 80, z(26) a( r- Ps-J IgF m M a'= g s X ( rP-JPg). where pi is not a function of 0,. Summing all the links from i to n gives us the reaction torques due to the gravity effects of all the links, aP.E.) I a(P.E.) P, X - (27) d —7 oe, - oe,.~ ~E M X- - o, } dt 89, CThe summation of Eqs. 15, 23, and 27 is equal to the generalized applied torque exerted at joint i to drive link i, r d a(KE.)ravs d(K.E.)tre n + r d a (K.E.),roI a(K.E.),ro a(P.E.) Tdl 8 dt 8a6 dJ d+ a - ~ [ m{[8j/1;i I= X P ) +I X I, + j [i4(Ro=eei. ) T ed] [ (:}' xs((_1 Xquan Pk )p_} + clI X O iJ X FjjJ i*-1lM xas t- I Gnaplie d metEutos1

R.SD-TR-8-82 + [mI( E..Zp x (28) p+ x ( up~iJ XI,o_ Jx E]:= )iXYmi( *-pi)J}; for i=1,2,, (for i 1.{', * z + H,_l x j, ) + Hx.t(,) + C ( -p29 ) j-1 D= Dj (+ DJa~ I E ( I(R~s I-, x) T k~)}[= {R ) T (R )} + I|m'i- X (I X ( IP pli)}]; } t I a~,~, x e,a~,_~ I| 1e, ~, I 1) 12 s.-In i d' 'EJem-perl) for i 1,2, 'i,n The above equation can be re-written in a more "structured" form as Hfor i = 2, n) 1 Gejer Halize d+ AlmbE) + q oi nsr, where, for i 1,2,. ~ ~,n, (30) +M mzs, _ '-Ix Ir.-*,_E);. I x fl - P. i<_

RSD-TR-8-82 ( j | r X Pk}].(5FI X( 1 (31) p==2 1 — I +: me1 =='P I 1 I I) + IN I jI X~ | pi P 1 ) (sFI X |( re- Pxi) ] H,'rot(O,) = ~ IIRsoFI IT4 IOiRU lzO X | kRe z.l-j I(2 (32) + {R S/ X | fP-' Zl) IJ, Z I)0 l Gi -- g - [ 'i- X Emji Fi - p-) ] ( ) The dynamic coefficients D,i and G, are functions of both the joint variables and inertial parameters of the manipulator, while the H,t"" and HrO are functions of the joint variables, the joint velocities and inertial parameters of the manipulator. These coefficients have the following physical interpretations: (1) The elements of the D,, matrix are related to the link inertias of the manipulator. Eq. 30 reveals the acceleration effects of joint j acting on joint i where the driving torque r, acts. The first term of Eq. 30 indicates the inertial effects of moving link j on joint i due to the rotational motion of link j, and vice versa. If i = j, it is the effective inertias felt at joint i due to the rotational motion of link i; while if i ' j, it is the pseudo products of inertia of link j felt at joint i due to the rotational motion of link j. The second term has the same physical meaning except it is due to the translational motion of link j acting on joint i. (2) The H,''(6e,e) is related to the velocities of the joint variables. Eq. 31 represents the combined centrifugal and Coriolis reaction torques felt at joint i due to the velocities of joints p and q resulted from the translational Generalized d'Alembert Equations 13

RSD-TR-8-82 motion of links p and q. The first and third terms of Eq. 31 constitute the centrifugal and Coriolis reaction forces from all the links below link s and link 8 respectively in the kinematic chain due to the translational motion of the links. If p -- q, then it represents the centrifugal reaction forces felt at joint i. If p y q, then it indicates the Coriolis forces acting on joint i. The second and fourth terms of Eq. 31 indicate the Coriolis reaction forces contributed from the links below link 8 and link 8 respectively due to the translational motion of the links. (3) The Hrol(6e,e) is also related to the velocities of the joint variables. Similar to the Hi'9"(S,O), Eq. 32 reveals the combined centrifugal and Coriolis reaction torques felt at joint i due to the velocities of joints p and q resulted from the rotational motion of links p and q. The first term of Eq. 32 indicates purely the Coriolis reaction forces of joints p and q acting on joint i due to the rotational motion of the links. The second term is the combined centrifugal and Coriolis reaction forces acting on joint i. If p = q, then it indicates the centrifugal reaction forces felt at joint i, while if p vq, then it represents the Coriolis forces acting on joint * due to the rotational motion of the links. (4) The coefficient C; represents the gravity effects acting on joint i from the links above joint i Since these coefficients are used quite often in designing a feedback controller for the manipulator, it would be useful to evaluate the computational complexities of these coefficients in Eqs. 30-33. An example of using these coefficients in designing a feedback controller is the nonlinear decoupled control [19]. At first sight, Eqs. 30-33 seem to require a large amount of computations. However, most of the cross product terms can be computed very fast. As an indication of their computational complexities, a block diagram explicitly showing the procedure in calculating these coefficients for every set point in the trajectory in terms of multiplication and addition operations is shown in Figure 2. Table 1 summaries the computational complexities of the L-E, N-E, and G-D equations of motion in terms of required mathematical operations per trajectory set point. 3. Applications to Obtaining Simplified Dynamic Model The main objective of developing the G-D equations of motion (Eqs. 29-33) is to facilitate the design of suitable controller for the manipulator in state-space or in obtaining approximate dynamic model which simplifies the design of the controller. Similar to the L-E equations of motion ( Eqs. 2-5 ), the G-D equations of motion are explicitly expressed in vector-matrix form and all the interaction and coupling reaction forces that are present in a manipulator can be easily identified in Eqs. 30-33. Furthermore the elements in the D, matrix, the H14ar, the Hi', and the CG vectors can be clearly identified as coming from the 14 Generalized d'Alembert Equations

RSD-TR-8-82 (lin + 36)M,(15n + lS)A,(28n)5S (6.)A,(7n)S (3)S; 1 1 1 0/O zP02 { j i i }i xc - I=- II A A 3 l'; II X t33 2-12 i + 5 n 2 + 3 ) M ( -rn 3 +-2 n 2 + 1 2 n ) M | q 3 Z - | $11 p=2 1 n) A + 1 + 5) A + n) S (32 + 3n) S (7n2 n + 42) M 2 3 2 3 2 2 2 2 2 ~ 4n2 + 11 n) A (n3 + I 2n2 + 5n) A H( ln. HP.3 A (2n + n + 332 ) A (20n +ln-3 ) A (n ) A n2 + +9) S 2 3) S 2 2 indicates multiplication operation indicates addition operation indicates memory storage requirement indicates output from block n Figure 2 Computational Procedure for D~, H-, an-d G, Generalized d'Alembert Equations 15

R.SD-TR-8-82 Approach Lagrange-Euler Newton-Euler Generalized d'Alembert 128n4 + 512 3 13n3 + 1052 Multiplications 132n 6 2 739 2 160 268. + Ln + +n |+ n+69 3 3 3 98 4 + 781 3 4 3 + 44n2 -n + -+ n44 Additions 3 6 1 1 1 n- 4 + 559 2 245 146 + - +. 4 n +,.n + 45 3 O 3 Kinematics 4x4 Homogeneous Rotation Matrices Rotation Matrices Representation Matrices and Position Vector=; and Position Vectors Equations of Closed-form Recursive Closed-form Motion Differential Equations Equations Differential Equations where n = number of degrees-of-freedom of the robot arm Table 1 Comparison of Robot Arm Dynamics Computational Complexities 16 Generalized d'Alembert Equations

RSD-TR-8-82 translational and the rotational motion of the links. This greatly aids the construction of simplified dynamic model for control purpose. For example, for a PUMA or T32 robot arm, the elements of the Di matrix come from the translational and rotational effects of the links. These effects depend on the joint variables and the inertial parameters of the manipulator. For the first three joints (81,62,03), because of their usually long link length for maximum reach and long distance traveled between the initial position and final position, the effects of translational motion will dominate the rotational motion. In contrast to the first three joints, the rotational effects will dominate for the last three joints. Hence, one can simplify the computation of the Dij matrix by considering only the translational effects for the first three joints and the rotational effects for the last three joints. Similarly, one can evaluate the contribution of H;'" and H1rot and eliminate their computations if they are insignificant. The resulting simplified model retains all the major interaction and coupling reaction forces at a reduced computation time and greatly aids the design of an appropriate control law for controlling the robot arm. Current industrial robots are designed kinematically to reach any given point in their specific workspace without considering the efficiency of the dynamics and control strategies. Since physical link parameters play an important role in determinating its sphere of influence and the magnitude of the interaction and coupling reaction forces between joints, it is important to choose these parameter values properly so that the dynamic coefficients used in the controller can be computed rapidly to obtain the necessary applied torques to the joint actuators. Eqs. 30-33 can be used extensively to verify the effects of the links' length, the link inertia, and location of the center of mass on the magnitude of the interaction and coupling reaction forces between joints. Based on Eqs. 30-33, one can design a robot arm with a simplified dynamic model for control purpose. 4. Equations of Motion for a Three-link PUMA Robot Consider the first three links of a PUMA robot arm shown in Figure 3. We would like to derive the generalized d'Alembert equations of motion for it. m;, i = 1,2,3, represent its respect link mass, and the link inertia tensor matrices are [I,-o I, o0 i= 1,2,3. 0o o0 li T32 is a trademark of Cincinnati Malcron Generalized d'Alembert Equations 17

ZO ~( 82 xo - d2 d4 Y3 44 d6 X 3 3 Y5 2/o I 431.8 mm 149.09 mm I -225 to +45 Y6 4 0 -9 0 430 mmY +17 [5!! I 0 lOO to +100 x4 X5.J.~ ~ ~ ~ ~ ~ ~~g( PUMA Robot Link Coordinate Parameters 1GeJoint neaiz, di Range 90 go-90 0 o 0 -160 to +1o60! 2 0 0 t 431.8 mm [ 149.09 mm i -225 to +45 ' 3 J go 90 - 20.32mmt 0 -45 to +225 i 4 0 1 -90 i 0 433.07 mm -110 to +170! 5 O go 0 0 -1 00 to +1 00 I 18 Generalized d'Alembert Equations

RSD-TR-8-82 The homogeneous transformation matrices, A-',1 i = 1,2,3, are found to be C, o -Si C 21 - 2 ~ C2 C3 o S3 a 3C3 0I S C1 0 1 C2 0 aS2 S3 0 -C3 a3S3 A1r 0 A C 0 3 a C -1 0 -1 0 [-0 0 l 0 0 0 1 0 0 0 1 o 0 0 1 Various rotation matrices used in deriving the equations of motion are C1C2 -C1S2 -51 Ro2 =R ol R S C2 -s1S2 cj -S -C2 ~ C1C23 -S1 C1S23 Ro3 Ro *R2=[ SIC23 C1 S1S23 023 0 C23 C23 0 S23 R R2- S23 0 -C23 0 1 0 where c = cos,, Si = sin 6,, Cj = cos (6, + 6,), Sii = sin (6, + 6j). The physical parameters of the manipulator such as pi', c,,;, and p; are 1~1 1C CC PS Oa2C2 2C -C 1 P2a1 Ro I. e ----o'R --- 1S- C, 1.1 0 L a2S2 1P2 O -a3S23 P3t0 Generalized d'A4lembert Equations 19

RSD-TR-8-82 ro]0'* ~C1C2 P2j Po -P = [O P2-1+P2 = la2 SC2 j = [P2,-2S2 P-2 P3 - P2 + P3' -- C25 + a30C23 hy [ C, C2 + a3CiCcj [P3,] -a2s2 - a3&s3 P3z [ris1 [r2 r3] [Ci c2] C~ rl= r2 r3 Ce l, 2 C2yz 3 C3y The following equivalences are used in the rest of the equations that follow. P2 P2, P3 =P2 + P3 -, = = l, -2 tF.= P-I + c, i-= 1, 2, 3. The joint axes of motion with respect to the base coordinate frame are. zo[L] Ro j[-SI o -sI, o C1ls23] [ -S2 S23 20=O [n1], R~zo = - -2], =R:O - C [ 20 Generalized d'Alembert Equations

RSD-TR-882 Rl= [O, R201~ = [ R30L, = I] Ras2 =, =2~ =, R32 = Using the D,j equation in Eq. 30, we derive the elements of the D(O) matrix. D3s3 = (R3z2) TI3(R3~2) + m3( 2 X (3 - P2) 2 X (3 X(-P2)} oo -sI, C3s -sr [, 13[1 + m3 C X /"y] I x Iy = 3yy + m3{ c32 + (S1,3y + Ca,)3) D..2 = (R2z) rlT2(R2l) + (R3)OslR) TI3(R~ O,) + m2( x (r2 -P) P) (, X (i2 - PI)} + m3al X (R3-pl) (1 X(i3-Pl)} [0] I2[0] + ] I3[I] + i XC ] X 2j C X,| Generalized d'Alembert Equations 21

RSD-TR-8-82 +,|C3 X x | C~ i X _3 y, = 12:: + 13g + m +( C:+(q CZ 2y + CI322) + M 32z + (Sri + C, 3)2) D23= (R3031)T3R3052) + m34s2 X (3 - P2)* si X (F3 - pl)) _~ I~ +,,~x, - x D'i [o [r ] - ] c31 [1 r l TOj LOJ 10 J [3z LJ [' r3z.J 3yw + m3{ 3zT3z + (S3y + Cl~3z) (S1'3y + Clr3z)) D13 = (R3Zo)rI3 (R3~s2) +,3(2 X (a3 - P2) (so X i3 - Po) 01S - Cc 0 [S Cc,23,] [k [5f-CC3, ]r3, -2 L - ~,7,7' [ 'g - m3(- ClC3z3y + S5137r33) D,_ =o(R20,o)T2(R2)O, +R3, so)T1o3(R3O, 0 )+m2(siX.-p)) (so x (,2-Po)) 22 Generalized d'Alembert Equatioas

RSD-TR-8-82 + m3{sI X (F3- Pl) {S0 X (F3'-Po)) IT~-s,~l 0~S23Cr3, ]F [r2j [ Ct3Z 1 [3? '2[jJ2 + [~] T O 3[] + m2 [s'3' J + m3| S13 [ r3sj -OJ LJ L~3J o l~JS ~ I -S1c31- s C173 - m2(- C13r'2y3- + S1T3zU2) + m3(- C1r3zr3y + S133r3s) D11 = (R so)TI,(R Oo) + (R230o)rI2(R200) + (R3~o)TI3(R3so) + mlz X (I,- po)} {o X (, —Po)} + m4s{o x ('2-po) {50 X ('2-Po)} + Im3 x (F3-oP}. ({o X (F3-PO) +ml!y;+ + 2) + 2 t m3(2 + 32) = lyy + S2212z + C22yy + + 23I + m2 + 2) + m2Cg + +r + m33y + r32 To derive the components of Hit'To, 0) and H,''(O, 0), we use Eqs. 31 and 32 respectively, Generalized d'Alembert Equations 23

RSD-TR-8-82 H'= m3 LSo x (S;So X P1 + (6l10 + 621s ) X X ((IO + 62s1) X P2) +(eiso x 62si) XP2] 2 X 3 - p2 + m3 (ISo + 821s + 63s2) x {(ISo + 62s1 + 3s2) Xi3 + {(Olso X 62s1) + (lso + 6251) X 3s2) X 3 2 X (3 -P2) S102 -S62 - b2iP C....821 I >~21 P II - 2 P2 CIC3Z m3 C1602 X C162 X JP 2 + -S81612 X P2;1I I 5 Z3 I 61 01 [ J3 1P2ZJ I O P2 z;-SIc3 - C1c3 - Sl(82 + 63)1 - S1(02 + 63)1 1 I - C1.182 + m3 C(0(2 + 83) x C1 (82 + 03) X IC13 + - S18182 L L C L 1 i - s3. [~,, [o, ] [ L S 'C1 j3 + i 2C 3 X C3 71 S1c ~3- C1C3 24 Generalized d'Alembert Equations

RSD-TR-8-82 where -I C 3y ' 2C13s)32 * S a = - (p. + - )s8X _ (Cip2 + C1P2, + 51CSllc3 + - (251C73, + 2S1Cl13,)82e3 + (2C1J3x)8183 After some simple manipulations, we obtain 4 = r CI34 - P2. - 31+s) + SIF3(- - P2; -F3 I + ( -P2 C173zP2'5 + (SI73y+ Ci73z)PA}22+( S13p Cc13s)(clp2* CiP2)} 9192] From Eq. 32, we have Hat= (R~)TI3 01Rso x (02R3Oa1 + 03R~ 2) + 82R Oa, X 03R~ 3 I3g 3 23 3+3 (2 2 X ( o + +2R OI R)) I4 e1R (e + e2R O +!3R3 32 Generalized d'Alembert Equations 25

RSD-TR- 8-82 T o ]? ~S23J1- 0 C- I S230131 =[I1] O X l2 + 031 + 1 X 12 +0;3 13 02 +'03 **C2301 0 C2381 C23;1 - - 3ssC23s23.I + 13zzC23S23 -C2 J3( 3-3 )8 Hence, H3 = H "3 + H3Ot = C2,3S23(J13=- 3z) - 7CFm3C31P2s + c3s) - 3SlC3(P2y + c3 + { 1 pSf33 + cG33) - SIc3zP2 c, C lC3P2 0S2s + ((Sl3, + C,13s,)(C1P2 - CIP2,) 0162 Similarly, using Eqs. 31 and 32, we obtain + m3lzo X (1i50 X Pl) + ((l00 + 0251) X ((oiso + 2S1) X P2)) +(1so X 2s1) X P21 (1 X 3 - Pi)) + 2[(I + 0212s) x ((2so + 251) X E) + (10o X 0251) X 1 1 X ( -P1)) 26 Generalized d'Alembert Equations

RSD-TR-8-82 + mn[(0 +;251 + 0352) X ((Io + 02S1 + 03S2) X C3 + (eso x 2S1)+ (eo + 02s11)x 3s2} X Si x (F3 - - SI C102p; - C2822P2 - 2;S'102 P - o:p2 m3 - S2p222 - S1 C10822 + 2C1 012P2 - I8 P2; SI3.... ~. ~ S Mh - 0 82 P~y~ fiC18223 '2P 2 C~82 - * 2;,S 3- C?3S 22822 - Cj02 2p2 + C10102P2- C10102z2 L - s 81 +c2, - 2C2e2. - 25s,3.|02- r, ~+ m. - s - sC03 C,23 +- 2G~,j.102 - 0?s2 - S1c2/2J- C13022 + C20201027 - C1C2010 +73 - S(2 + C..182 + SG C1(82 + ) + 2G33(2 + s 3, - c,|l,3 12-c3(2 + 83)2 - CG213(2 + 83)2 + C31101(02 + 83) - C(3t3A(2 + [83) S - m2[( C- Sc 2c2 + (S1, + Cc32')(CI, - Cc,%)0,e2] + C -2 2 r - SC123733Z812 - s~..e~+~3:-~,3(e +~3~+,2 + +3e3)-C~ c, 3~,(~ + 3) + (S1P2,3g + C1P2,r3= - S iP2yr3 - CiP2 r32 + Sic37r3 + C1c3r3s - S1i.33 - C3 )2 + (S1c3r3l, + C133, 33- Sl(3r3)2 - C(33r/3z)03 + (CSp23 + P - CSIP23 CP2yr3s Generalized d'Alembert Equations 27

RSD-TR8-82 + CSf33r73 + Csls - CSS1srg- C+L CC33s)e12 + 2(S, 3r3., + Cl13r3, - S 3yr32 - C +(CiSI 7F3, + (C - 3 ss, - cS s3y - Csc3s3s) 3j H'~t= -(R~s1 ) TI2(1R2~o X 82R2si1) (R3 Os1){ SIR3~O0 x- (2R~ +s + 3R~) + 02R3~sl X 3R3~s2 3 3 * *2 O ~O, T T Ozl X (elR3~zo + R2R ~sl + O ~} '82R z 83R C- A0a2 1 S201 - S2;, -= [~ '21 S28 82 + + 0 [O X - C.2 12 - 201 + C23S23(I3z,- I3s,)12 [ 0 2 1 e2 {SiC2(I2y - '2:) + C2323(I3zz - 13Sz)} H=m2 [ e o X (0]so X P1)] ~ X (2 - PO) + m3[0 X ( x P + (10 + 2s1) x ((lso + 2s1) x p 28 Generalized d'Alembert Equations

RSD-TR-8-82 + m2[(010 + 6251) X ((ISO + 6251) X C2} + (e150 X 211) X (2 So X (F2 - Po) + 7[( 1 + 621 + (5 X o +;1S5 + 2) + ) X (r3 o } 1) SI S1 2 C12;- 2 - 2S 10;162p - 2 sp- [a - t~ i Ci6?2, - C?6?3(a - 2s) 3se x 62 -(22-o += m. s si2p 2 - S 1C1e22P2s + 2Clela2 M- ~[ -SI C 822c2 - C822-c2s - 2S]c2.,12 _? 2 s12 B - SCl(2 + 03)23y - C12(12 + 63)2cs, - 2SI, 331(2 + 03) C-3, _ + ms3 - S(02 + 63)2gc3, S CI(12 + 93)'2c3, + 2CG13 B1(2 + 03) - 3go r Generalized d'Alembert Equations 29

RSD-TR-8-82 +-2( SI2F e 2 + Cle-... + 2>(5f2c?2r + C(3c r ~)0] + m3 [p'r p'r s+ c r - r)01 + (SI CIp2;r3 + C2P2sr3y - S12P2Yrs - Si CIp2A3s + S1C1 '3;3, + C1 2c3;r3 c-.' ~' ' S1 C'3 3s 3s).2 + (Si Cl'3r3FI, + C12?3r3,x - S12c393s - SI C1 C3r'3,)832 + SI1P2zr3g + C1P2zr3 + S+13'3yt + C1'37'3s)0182 + 2(Cl3;37 + Clc3s;3y- S1c3;Ss - SiC2i3sr3s)8283 + 2(51c3zty+ C+ C3zi3s)818 where A,B. and C need not to be computed. HI'ro= (R0so X 01,Ro)T I, (01R ~o) + (R20~o)rI2 (0111~25 X 02R20~,) + R2~so x (,aR2~s + 02~112 I12 (lR20o + 82R2~s1) + (R3)T 13 (R39so X (82R351 + 01~s52) + 3R~) 3+ 2 X 03R3~0s +R~.so RXO 3 2 13(0113950 + 0(21~1o3 + + o + 03R ) = |- c2 S 121 +Sl2 X - C- C2C01 300 Gz d'lme02 J02 30 Generalized d'Alembert Equations

RSD-TR-882. T - S231 e- 2301 -S,23 -s ',.o_1. IS23,,.., I+ 0jX [;] x 0 + +03I + o x X02 + 03 1302 + 0 C231 C|2301 = 2(S2C212z - S2C212W)0102 + 2(S23C2313ss - S23CG3I3s)$102 + 2(S23C2313ss - 23C23I3z)0183 To derive the elements of the gravity vector G(O), we use Eq. 33, - Sx,,-1 of3C3zJ = m3gSl'3g + m3gCl'c3 =-~ m2gC~2A + iCl) + m3F 3Sj + 3+ M3C1) s' X / - 0+ X >'1 Gi= — 0P m I)+m2 + 3 =i m2l2l + mJ2 + mrl 0 + m2 r2 + m3 r3LuJ mlr + m"r2 + m3r3 cG = - g * X / m"rl" + m22 + t! r|- [l ' | I X I mll + m2'2- + m3"'~ = o mlGea ml riz2+ m3t3 ] L9J F mlrI+ u r2z+ mt r3z Thus the equations of motion for the first three links of a PUMA robot arm are: Generalized d'Alembert Equations 31

RSD-TR-8-82 Li DI3J [] [ I [r] HIrI GI rI, D12 D22 D1 1 H3fra 2o,, C1 12:- D12 D l2 ~23 +.2 HH2 H + C r3 D1S D25 D03 Be H3 H3'' [ CS3 5. Conclusion Three different formulations for robot arm dynamics have been presented and discussed. The L-E equations of motion can be put in a well structured form, but computationally it is very difficult to utilize for real time control purposes unless the equations of motion are simplified. The N-E formulation results in a very efficient set of recursive equations, but they are very difficult to use for deriving advanced control laws. The G-D equations of motion give well "structured" equations at the expense of higher computations. In addition to having faster computation time than the IrE equations of motion, the G-D equations of motion explicitly indicate the contributions of the translational and rotational effects of the links. Such information is useful for control analysis in obtaining an appropriate approximate model of the manipulator. To briefly summarize the results, an user is able to choose between a formulation which is highly structured but computationally inefficient ( L-E ), a formulation which has efficient computations at the expense of the "structure" of the equations of motion ( N-E ), and a formulation which retains the "structure" of the problem with only a moderate computing penalty ( G-D ) 6. References 1. J. J. Uicker, "On the Dynamic Analysis of Spatial Linkages using 4X4 Matrices," Ph. D. dissertation, Northwestern University, August 1965. 2. A. K. Bejczy,"Robot Arm Dynamics and Control," Technical Memo 33-669, Jet Propulsion Laboratory, February 1974. 3. R. P. Paul, "Modeling, Trajectory Calculation and Servoing of a Computer Controlled Arm," Stanford Artificial Intelligence Laboratory, A.I. Memo 177, Sep. 1972. 4. R. P. Paul, Robot Manipulators: Mathematics, Programming and Control, MIT Press, 1981, pp. 157-195. 32 Generalized d'Alembert Equations

RSD-TR-8-82 5. C. S. G. Lee, "Robot Arm Kinematics, Dynamics, and Control", Computer, Vol. 15, No. 12, December 1982, pp 62-80. 6. A. K. Bejczy and R. P. Paul, "Simplified Robot Arm Dynamics for Control," Proceedings of the 20th IEEE Conference on Decision and Control, San Diego, Dec. 1981, pp 261-262. 7. W. AM. Armstrong, "Recursive Solution to the Equations of Motion of an N-link Manipulator," Proceedings of the 5th World Congress, Theory of Machines, Mechanisms, Vol. 2, July 1979, pp 1343-1346. 8. D. E. Orin, R. B. McGhee, M. Vukobratovic, and G. Hartoch, "Kinematic and Kinetic Analysis of Open-Chain Linkages Utilizing Newton-Euler Methods," Math. Biosc., Vol. 43, 1979, pp 107-130. 9. J. Y. S. Luh, M. W. Walker, and R. P. C. Paul, "On-Line Computational Scheme for Mechanical Manipulators," Transactions of ASME, Journal of Dynamic Systems, Measurement, and Control, Vol. 120, June 1980, pp. 69 -76. 10. R. L. Huston, C. E. Passerello, and M. W. Harlow, "Dynamics of Multirigid-Body Systems," J. Appl. Mech., Vol. 45, 1978, pp 889-894. 11. M. W. Walker and D. E. Orin, "Efficient Dynamic Computer Simulation of Robotic Mechanisms," Transactions of ASME, Journal of Dynamic Systems, Measurement, and Control, Vol. 104, Sept. 1982, pp. 205-211. 12. R. L. Huston and F. A. Kelly, "The Development of Equations of Motion of Single-Arm Robots," IEEE Transactions on Systems, Man, and Cybernetics, Vol. SMC-12, No. 3, May/June 1982, pp 259-266. 13. J. M. Hollerbach, "A Recursive Lagrangian Formulation of Manipulator Dynamics and a Comparative Study of Dynamics Formulation Complexity," IEEE Trans. on Systems, Man, and Cybernetics, Vol. SMC-10, No. 11, Nov. 1980, pp. 730-736. 14. J. L. Turney, T. N. Mudge, and C. S. G. Lee, "Connection Between Formulations of Robot Arm Dynamics with Applications to Simulation and Control," CRIM Technical Report No. RSD-TR-4-82, the University of Michigan, April 1982. Generalized d'Alembert Equations 33

RSD-TR-8-82 15. W. M. Silver, "On the Equivalence of the Lagrangian and Newton-Euler Dynamics for Manipulators," The International Journal of Robotics Research, Vol. 1, No. 2, 1982, MIT Press, pp 60-70. 16. M. J. Chung, "Adaptive Control Strategies for Computer-Controlled Manipulators," Ph. D. Dissertation, the Computer, Information, and Control Engineering Program, the University of Michigan, Ann Arbor, August 1983. 17. M. A. Chace and Y. O. Bayazitoglu, "Development and Application of a Generalized d'Alembert Force for Multifreedom Mechanical Systems," Transactions of ASME, Journal of Engineering for Industry, Series B, Vol. 93, Feb. 1971, pp 317-327. 18. S. H. Crandall, D. C. Karnopp, E. F. Kurtz, Jr., and D. C. PridmoreBrown, Dynamics of Mechanical and Electromechanical Systems, McGraw-Hill, 1968, pp 177. 19. E. Freund, "Fast Nonlinear Control with Arbitrary Pole Placement for Industrial Robots and Manipulators," the Int. J. of Robotics Research, Vol.l, No.l, Spring 1982, pp.65-78. 34 Generalized d'Alembert Equations

UNIVERSITY OF MICHIGAN 3 9015 03023 0299