RDS-TR-5-82 ON THE CONTROL OF MECHANICAL MANIPULATORS C. S. G. NL J. Chung April 1982 CENTER FOR ROBOTICS AND INTEGRATED MANUFACTURING Robot Systems Division COLLEG E OF ENGINEER ING THE UNIVERSITY OF MICHIGAN ANN ARBOR, MICHIGAN 48109

TABLE OF CONTENTS 1. Introduction........................................................................................................... 2 2. Kinematics and Notation for Manipulators....................................... 3 3. Dynamics of Manipulators.................................................................................. 5 3.1. Lagrange-Euler Formulation.....................................6.. 6 3.2. Newton-Euler Formulation 8 4. Computed Torque Technique Based on the Newton-Euler Formulations.1............................................................................................. 1 5. Computer Simulation Results............................ 17 6. References................ 62

RSD-TR5-82 1 Abstract The dynamic performance of computer-controlled manipulators is directly linked to the formulation of the dynamic model of manipulators and its corresponding control law. Various formulations are available to describe the dynamic models of mechanical manipulators and most notable of these are the Lagrange-Euler and the Newton-Euler formulations. This report describes an efficient position plus derivative control in the Joint variable space for a PUMA= robot arm whose dynamic equations of motion are formulated by the Newton-Euler method. The recursive controller compensates the inertia loading, the nonlinear coupling reaction forces between joints and the gravity loading effects. Using a PDP 11/45 computer, the controller equations can be computed within 3 ms, which is sufficient for real-time control. Computer simulation of the performance of the control law is included for discussion. - PUMA Is a trademark of UnimatIon Inc.

2 RSD-TR5-82 1. Introduction A mechanical manipulator can be modeled as an open-loop articulated chain with several rigid bodies (links) connected in series by either revolute or prismatic joints. One end-of the chain is attached to a supporting base while the other end is free and attached with a tool (the end-effector) to manipulate objects or perform assembly tasks. The motion of the joints result in relative motion of the links. The purpose of manipulator control is to maintain the dynamical response of an electromechanical manipulator in accordance with some pre-specified system performance and desired goals. In general, the control problem consists of (I) obtaining dynamic models of the physical system and (ii) specifying corresponding control laws or strategies to achieve the desired system response and performance. This report deals with the second part of the control problem of computer-controlled manipulators, and In particular, the PUMA robot arm. A priori information needed for control is a set of differential equations describing the dynamic behavior of the manipulator. Though various approaches are available to formulate the robot ann dynamics such as the Lagrange-Euler [Uic65], the "Recursive-Lagrange" [Hol80], the Newton-Euller [LWP80], the Lagrange form of D'Alembert Principle [LLN82], and more recently the "Gibbs-Appell" [HoT80] formulation, two main approaches remain to be used by most researchers to systematically derive the dynamic model of the manipulator - the Lagrange-Euler and the NewtonEuler formulations. After obtaining the dynamic equations of motion of the manipulator, a suitable control law must be designed to compute the necessary feedback torques/forces to actuate the joints for every set point ( 6d,9;P0 ) in a pre-planned trajectory. Bejczy [Bej74] based on the Lagrangian formulation has shown that the dynamic equations of motion for a 6-jointed manipulator are highly nonlinear and consists of inertia loading, coupling reaction forces between joints and gravity loading effects. Hence, the control law must be designed to compensate all these nonRobot Systems Division Introduction

RSD-TRS-82 3 linear effects. A position plus derivative control based on the computed torque technique has been used previously to servo a Stanford arm [Mar73] whose dynamic equations of motion are formulated by the Lagrange-Euler approach. However, the dynamic equations of motion as formulated by the Lagrange-Euler method have been shown to be computationally inefficient [TML80,Pau72], and real-time control based on the 'complete' dynamic model has been found difficult to achieve if not Impossible [Pau72]. A simple control law in Joint space which compensates the inertia loading, the coupling reaction forces between joints and the gravity loading is shown through the "Equivalence Formulation" [TML80,LCT82] to have the same control effects as the one obtained by the computed torque technique. This control law Is based on the dynamic equations.of motion formulated by the Newton-Euler method. Computer simulation of the performance of the proposed control law for a PUMA robot arm on a VAX-1 1/780 computer shows the expected result. In the following sections, vectors are represented in boldface lower case alphabets while matrices are in boldface upper case alphabets. 2. Kinematics and Notation for Manipulators A mechanical manipulator consists of a sequence of rigid bodies, called links, connected by either revolute or prismatic joints. Each pair of joint-link constitutes one degree of freedom. Hence for an n degree-of-freedom manipulator, there are n pairs of joint-link with link 0 attached to a supporting base where an inertial coordinate frame is established. In order to describe the translational and rotational relationship between adjacent links, a Denavit-Hartenberg matrix representation for each link is used [DeH55] and shown in Figure 1. From Figure 1, an orthonormal coordinate frame system ( x1,yl,zl ) is assigned to the ith link, where the z1 axis passes through the axis of motion of joint i+1, and the xi axis is normal to the zl_1 axis, while the ye axis completes the right hand rule. With this orthonormal coordinate frame, link i is characterized by two parameters: a, the common normal Robot Systems Division Kinematics

4 RSD-TR5-82 distance between the z1i1 and zi axes, and M, the twist angle measured between the z,_1 and z1 axes in a plane perpendicular to a,; and joint i which connects link i1 to link i is characterized by a distance parameter di measured between the xit and xi axes and a revolute joint variable A1 which is between the normals and measured in a plane normal to the joint axis. If joint i is prismatic, then it is characterized by an angle parameter 65 and a joint variable di. With the coordinate frames established for adjacent links (link i and link i-I), one can relate the relationship between the adjacent coordinate frames (ith and _1 th frames) by performing the following four operations (see Figure 1): (a) Rotate an angle of 61 about the zl-1 axis ( the x-i and xi axes are aligned ). (b) Translate a distance of di along the z1-1 axis ( the xl,1 and xi axes are coincident ). (c) Translate a distance of al along the xi axis (the two origins are coincident ). (d) Rotate an angle of a/ about the xi axis. \ E zt0 ~~~~~~~~Joint I LS ri<nk i Y/ Joint j+ Y1 -I Joint i-1 Figure 1 Parameters of a Link Coordinate System Robot Systems Division Kinematics

RSD-TR5-82 5 These four operations may be expressed by a 4 x 4 homogeneous coordinate transformation matrix as: cos 1A - cos a, sin 1A sin a, sin 1/ a, cos 1, sin 1d cos al cos 1 -sin al cos id a, sin 6/ (2.1) T/_1 = O sin al cos a, di 0 0 0 1 [R/I P1- i (2.2) 0 1 The upper left 3x3 submatrix of T/1. is called the rotation matrix R/I1 while the upper right 3x1 vector is called the position vector Pi 1. One can view the rotation matrix R/_I as a transformation matrix which maps a vector r/ = (x, y, z )T expressed in the ith coordinate frame into the (i-l)th coordinate frame with both origins coincided at one point, and the position vector as the displacement vector of the origin of the ith coordinate frame from the origin of the (i-1 )th coordinate frame. The above kinematics relationship between adjacent links will be used in the following sections to derive the dynamic equations of motion and show the equivalence of the two controllers based on the two most popular arm dynamics formulations. 3. Dynamics of Manipulators The dynamic equations of motion for a PUMA robot arm can be obtained from known physical laws (Newtonian and Lagrangian mechanics) and physical measurements (link inertias and geometric parameters). The actual derivation is based on the Lagrangian/Newtonian formulation applied to open articulated chains represented in Denavit-Hartenberg matrix notation form. The equations of motion for a six-jointed manipulator have been derived previously by Bejczy[Bej74], Robot Systems Division Dynamics

8 RSD-TRS-82 Paul[Pau72] and Uicker[Uic65] using the Lagranglan generalized coordinates. The equations of motion derived from the Lagrangian and Newtonian formulations will be briefly presented here. 3.1. Lagrange-Euler Formulation Consider a position vector expressed in homogeneous coordinates, p = (x, y, z, 1)T, which points from the base coordinate system to a differential mass, dm, located in the lih link. p can be written as: p=T/r/, and r, =(x/,y,,z/,1)T (3.1) where r1 is the position of the differential mass dm represented in the Eth coordinate frame. The velocity of this differential mass with respect to the base coordinate frame ( an inertial frame ) is: _v= dt = aT_,r,; for i=1,2,...n (3.2) The associated kinetic energy dK, is ITr(vo(vo)T)dm which equals: 1~j Tro f,(r,)Tdm idK,=. 2 E Tr1 r/(r)Tcl. | (3.3) dK; = 2 t~trJ=lk=l When each link is integrated over its entire mass and the kinetic energies of all links are summed, we have:,K. = Afdm = >,j=TlkTr B-n I -aTOI j (3.4) where Jis de=lk=fined as: where Divis defined as: Robot Systems Division Lagrange-Euler Formulation

RSD-TR5-82 7 -IXx+lyyZZ + m +72 m1 rxrT#mi FZm, Tdm, 1xx'/YY+IZz 2 T7xmI 2 + m7;9zmi T7mI Jr= __ rXX+lylzz -2 - 7rzrXmm 2 +rz m rzm, rxm / rym/ r am ma The total potential energy of the arm is the sum of the potential energy of each link expressed in the base coordinate frame: PI. = P = -m/gTol/ (3.5) I=1 I=1 where?- is the position vector of the center of mass of link i expressed in the ith coordinate frame. g is the gravity row vector = (gx,gy,gz,O) and I g I = 9.8062m/ s2 Applying the Lagrange-Euler equations of motion to the Lagrangian function L = K.E. - P.E., we obtain the necessary generalized torque Tr for joint i to drive the ith link of the arm: r=d r 0 | 0_ = 1 6 E Tr, 20 T J o (3.6) e m m | d2Tf OTOM e T +X 2 Tr{, OJmk (m 0Tf - T; for i= 1,2,...,6 Because of its matrix structure, this formulation is appealing from a control viewpoint in that it gives a set of closed form differential equations as: D(d)6 + H(59,9) + G(O) = T (3.7) where: Robot Systems Division Lagrange-Euler Formulation

8 RSD-TR5882 D(O) = a 6x6 inertia acceleration matrix D/k= J Tr{"9- _j1 [L ]J; for ik=1,2,... (3.8) H({, '= = a 6xl nonlinear Coriolis and Centrifugal vector H/kmn =xJ=mJ)rmTr Oa1ka m/, l |; for k,m=1,2,...,6 (39) G(O) = a 6xl gravity loading vector of the links GI/ = - j -lmj ga- r/; for i=1,2,...,6 =O( 1, 62, ' ",Xs6) = (A,, *..., 6,)T a = (A, e, '", s)T' = (T,T2,T3,T4,65,re) external applied torques for the joints This form provides more insight to the coupling effects between Joints and to designing a control law that compensates all these nonlinear effects easily. Computationally, however, the Lagrangian formulation is extremely inefficient as compared with other formulations. 3.2. Newton-Euler Formulation The Newton-Euler equations of motion of a manipulator consist of a set of compact forward and backward recursive equations. The most significant of this formulation is the computation time of the applied torques could be reduced tremendously so that real-time open-loop control is possible. A brief derivation of the Robot Systems Division Newton-Euler Formulation

RSD-TR5-82 9 formulation based on [LWP80] is presented here for completeness. The forward recursive equations propagate linear velocity, linear acceleration, angular velocity, angular acceleration, total link forces and moments from the base to the end-effector of the manipulator. For manipulators having all the rotary Joints, these equations are: Forward Equations: i = 1, 2,..,n C; = R/-1 (i,-1 + z0O) (3.11) a, = R-1' [a,-1 + Zo;1 + ~,_ x ZOi] (3.12) a, = a x r/ + c/ x x r,] + R-1 a/_1 (3.13) al =al x I' + cl x [C x?rJ + a, (3.14) F: = mea4 (8.1 5) NI = Il al + ol x II G (3.16) The backward recursive equations of motion propagate, from the end-effector to the base of the manipulator, the forces and moments exerted on link i by link i-1. Backward Equations: i = n,n-1, ~ ~ ~,1 ft = R/+1fl+1 + F1 (3.17) n1 = R/+lnj+l + r1 x fJ+1 + (r, + JF) x F1 + Ni (3.18) r, =nf(R/-1 zo) (3.19) with the "usual" initial conditions of co = 0,ao = gzo, oo = 0, g = 9.8062m/s2, fn+l = external force exerted on the hand and nn+l = external moment exerted Robot Systems Division Newton-Euler Formulation

10 RSD-TR5-82 on the hand. where: R/-1 = the rotation matrix which transforms a vector from its representation in the i-1th coordinate system to its equivalent in the ith coordinate system. d P= the angular velocity of link i with respect to the ith coordinate system. a, = the angular acceleration of link i with respect to the ith coordinate system. ri = the position vector of the origin of the i-1 th frame with respect to the ith coordinate system. VI = the position vector of the center of mass of link i with respect to the ith coordinate system. a, = the linear acceleration vector of link i with respect to the ith coordinate system. al = the linear acceleration vector of the center of mass of link i with respect to the if6 coordinate system. If = the Inertia matrix about center of mass of link i with respect to the ith coordinate system. Fi = the total external force vector exerted on link i with respect to the ith coordinate system. Nl = the total external moment vector exerted on link i with respect to the ith coordinate system. fl = the force vector exerted on link i by link i-1 nl = the moment vector exerted on link i by link i-1. r1 = the applied torque exerted on link i Robot Systems Division Newton-Euler Formulation

RSD-TRS-82 11 4. Computed Torque Technique Based on the Newton-Euler Formulations Given the equations of motion of a manipulator as in Eqs. (3.7)-(3.1 0) (Lagrange-Euler formulation) or Eqs. (3.11)-(3.19) (Newton-Euler formulation), the control problem is to find appropriate feedback torques/forces to servo all the Joints of the manipulator in real-time to track a desired position trajectory as closely as possible. Several methods are available in accomplishing this task. Most notably of these are: (i) Resolved Motion Rate Control (RMRC)[Whi69], (ii) Cerebellar Model Articulation Controller (CMAC)[Alb75], (iii) Near-minimum-time control [KaB711], and (iv) Computed torque technique [Mar73, Pau72]. The RMRC is a technique for determining the Joint angle rates required to cause a manipulator end point (or tool) to move in the directions which are expressed in the hand or world coordinate system. In order to find the required i the inverse Jacobian matrix J()-l1 is required. One of the drawbacks of this method is the added computation load needed to find the inverse Jacobian matrix and the singularity problem associated with the matrix inversion. The CMAC is a table look-up control method which based on neuro-physiological theory. It computes control functions by referring to a table stored In the computer memory rather than by solution of analytic equations. For useful applications several problems such as memory size management and accuracy need to be solved. Due to the nonlinearity and complexity of the dynamical model of manipulator, a closed form solution of the optimal control is very difficult, if not impossible. Nearminimum-time control is based on the linearization of the equations of motion about the nominal trajectory and linear feedback and/or suboptimal control law are obtained analytically. This control method is still too complex to be useful for manipulators with four or more degree of freedom and furthermore it neglects the effect of unknown external loads. Robot $ystens Division Control Law

12 RSD-TR5-82 One of the basic control schemes is the computed torque technique [Pau72,Mar73] based on the Lagrange-Euler equations of motion. The computed torque technique assumes that one can accurately compute the counterparts of D(), H(O,, and G(O) in Eq. (3.7) to minimize their nonlinear effects and use a position plus derivative control to servo the joints [Pau72]. Thus the structure of the control law has the form of: D( = K;a)[i -,9 + Kp(ld -i ]) + Ha(n, ) + Ga(i) (4.1) where Ku is an nxn derivative feedback gain matrix. Kp Is an nxn position feedback gain matrix. n is the number of degree of freedom of a manipulator. Substituting 'r from Eq. (4.1) into Eq. (3.7), we have: D(0)a + H(ii; + HG(i) = D,(e) ['d + Kv(i - ) + Kp(td -)] (4.2) + Ha(, i) + G,(a) If Da(a), Ha(i, d, Ga() are equal to D(~), H(, 6), G(i) respectively, then Eq. (4.2) reduces to: D() [e + Kv + Kpe = 0 (4.3) where e = -d _ 6 and e = ~d _ Since D(O) is always non-singular, if the values of K, and Kp are chosen so that the characteristic roots of Eq. (4.3) have negative real parts, then the position error vector e approaches zero asymptotically. However, the computation of the joint torques from Eq. (4.1) is very inefficient if the dynamic model is based on the complete Lagrange-Euler equations of motion. Because of this reason, it is common to simplify Eq. (4.1) by neglecting the Robot Systems Division Control Law

R!SD-TR5-82 13 velocity-related coupling term Ha(~,d) and the off-diagonal elements of the acceleration-related matrix Da(C). That is, the structure of the control law has the form of: ' = d/ag[ Da(D) ][+ KC - + KK(( -) + GKp(-)] ) (4.4) In the next section we will show the effects of neglecting these terms when the controller (as in Eq. (4.4)) is based on the simplified Lagrange-Euler equations of motion. In order to utilize the complete equations of motion, an analogous control law derived from the computed torque based on the Newton-Euler equations of motion is proposed. The analogous control law can be obtained by substituting 'O in Eqs. (3.11 )-(3.1 9). _' +, Kvs"(~d - ) + [I:K d _ "- ) 8=1 s=1 or (4.5) d + Kses + E Kses s=1 s=1 where K's and K/s are the derivative and position feedback gains for joint i respectively and es = ids_ ds is the position error for joint s. The values of feedback gain matrices KL and Kp can be determined systematically as follow: If Kv is a symmetric and semi-positive definite matrix and Kp is a symmetric and positive definite matrix, and the rank of Kv I KK I* Kn-1 Kv =n,then the position error vector e approaches zero asymptotically. [LCT82,TML80O show the equivalence of the proposed recursive controller and the control law obtained by the computed torque technique based on the LagrangeEuler equations of motion. Since the recursive controller is based on the complete Robot Systems Division Control Law

14 RSD-TRG-82:~ dynamic equations, it is expected that the performance of this controller is always better than the controller based on the simplified Lagrange-Euler equations of motion. In the remaining of this section, the computational complexity of the proposed recursive control law based on the Newton-Euler equations of motion and the analogous control law obtained from the Lagrange-Euler equations of motion is tabulated. Also the feasibility of the real time control of a PUMA robot arm using the recursive controller is discussed. As a mean of comparing their computational complexity, their efficiency is determined based on the number of mathematical operations (multiplications and additions) in terms of the number of joints of the robot arm, n. The number of mathematical operations of some of the terms in both control laws may be slightly different from other papers [TML80,Hol80] due to the method of implementation of the control algorithms in programming. In this study, the homogeneous transformation matrices T/.. are computed first and then other relevant terms such as the velocity-related, the acceleration-related and the gravity loading terms in the Lagrange-Euler equations of motion are computed respectively. The number of mathematical operations of the control laws based on these two formulations are tabulated in Table 1 and Table 2. In general, for a six-jointed robot arm with rotary joints, the number of mathematical operations In the control law as in Eq. (4.1) based on the Lagrangian formulation is about 100 times more than that of the Newton-Euler formulation. Based on a PDP 1 1/45 computer and its manufacturer's specification sheet, an ADD (integer addition) instruction requires 300 ns and -a MUL (integer multiply) instruction requires 3.3/ s. If we assume that for each ADD and MUL instruction, we need to fetch data from the core memory and the memory cycle time is 450 ns, then the proposed recursive control law based on the Newton-Euler equations of motion Robot Systems Division Control Law

RSD-TR5-82 15 Controller based on Lagrange-Euler Multiplications Additions Ethetauations of Motion TI 32n(n-1 ) 24n(n-1) -mjg j 4n(n-7) n (51 n -45) -mj g -rj 2 TmJg 4 O -n(n-1 ) ~mlg a~, 2 r OT0 roT&1 128 65 OTr j A- o. J1l — 8n(n+1)(n+2) -n(n+1)(n+2) 8191 (NJ 2 k >ma T~s 8 To JkI -pe| 0 n(n-1)(n+l) T a=TO [ t T + l n2(n+1 )(n+2) 65 n2(n+l)(n+2) O'dj 816k m 80 3 2 '"[a~,ja~,,, J6 m=max(hjdk)L BtJ815k m -' J;d + Kve + Kpe 2n 4n - = Da(d Kve + Kpe) n2(n+2) n2(n+1) + Ha,(.,0) + Ga(6) 1Tot28 4 515 3 98 4 787 n3 Total n + 3 n g 3 3 3 6 Mathematical Operations + 2 + 82 + 640 2 + 131 3 3' 3 where n = number of degree-of-freedom of the robot arm Table 1 Breakdown of Mathematical Operations of the Controller Based on Lagrange-Euler Formulation Robot Systems Division Control Law

16 RSD-TR5-82 Controller based on Newton-Euler Multiplications Additions Ethetauations of Motion | GJ i | 9n | 7n i 1 5n 14n FI 8n 0 fl 9(n-1) 9n-6 N1 24n 18n no 21n-15 24n-16 td + Kien + Kpe 2n 4n Total 11 gn-24 107n-21 Mathematical Operations where n = number of degree-of-freedom of the robot arm Table 2 Breakdown of Mathematical Operations of the Controller Based on Newton-Euler Formulation Robot Systems Division Control Law

RSD-TR5-82 17 requires approximately 3 ms. to compute the necessary joints torques to servo all the joints of a PUMA robot arm for a trajectory set point. This certainly is quite acceptable for the time delay in the servo loop and thus allows one to perform realtime feedback control on a PUMA robot arm with all its dynamics taken into consideration. 5. Computer Simulation Results This section discusses the computer simulation result of the proposed recursive control law and compares it with that of the simplified Lagrange-Euler equation of motion. A computer simulation study to evaluate the performance of the above control laws for a PUMA robot arm was carried out on a VAX-1 1/780 computer. In this simulation, the six-jointed manipulator moves from an Initial joint angles 'lnitla = (00, 450~, 450~ 00, 00, 0OO)T to a final joint angles Iflna, = (900 -450, 1350, 900, 900, 900). The required time for this motion is 1 second. In this trajectory, the PUMA robot arm is fully stretched at 0.5 seconds. At this position, 0o.bs,.c = ( 450, 00, 900, 450, 450, 450), the torques due to the gravity have the maximum values and the absolute values of joint velocity of the arm also becomes the maximum. The accelerations are sharply changed from the maximum values to the minimum values or vice versa. The sampling time is chosen to be 0.01 second. The feedback gain matrices K, and Kp of the control law are kept constant for the whole motion execution to facilitate the comparison of both control laws. The elements of K and Kp are assigned according to the stability criterion as outlined in Eq. (4.3). The principal diagonal elements of Kp are assigned the value of 100 and the diagonal elements of K, to 2 v/Kp = 20. Again to simplify the comparison, all the non-diagonal elements of Kv and Kp are ztero which neglect the position and Robot Systems Division Computer Simulation

18 RSD-TR5-82 derivative error effects between Joints. With reference to the dynamic equations of motion as in Eqs. (3.11) - (3.19) and Eq. (3.6) the numerical values used in this simulation are: dl= 0.664 meter a2 = 0.432 meter d2 = 0O1495 meter d4 = 0.432 meter de =0.130 meter m = [ 227, 15.91, 6.82, 3.1 8, 0.91, 0.45] Kg daig 11 = [ 0.0071, 0.0267, 0.0267] Kg-meter2 daig 12 = [ 0.1000, 0.7300, 0.8025] Kg-meter2 daig 13 = [ 0.0222, 0.2160, 0.2245] Kg-meter2 daig 14 = [ 0.0020, 0.0010, 0.001 0] Kg-meter2 dalg 15 = 0.0030, 0.0030, 0.0004] Kg-meter2 daig l = [ 0.0050, 0.0050, 0.0003] Kg-meter2 rl =[ 0., -0.664, 0.]T meter r2 = [ 0.432, 0., 0.1 495]T meter r3 = [ 0., o., 0.]T meter r4 = [ O., -0.432, 0o]T meter Robot Systems Division Computer Simulation

RSD-TR5-82 19 r = [. 0., 0..T meter re,= [ 0., 0., 0.13]T meter 1 = [ 0., 0, 0.073]T meter r2 = [ -0.432, 0., O.]T meter 3 = 0., 0., 0. O.11T meter r4 = [ 0., 0, 0.1 T meter rF = [ -0., 0., 0.01 ]T meter re = [ 0., 0., -0.05]jT meter Figure 2 shows the flow-chart for the computer simulation program implementation. Figures 3-6 show the preplanned position, velocity and acceleration trajectory for joint 1. In this simulation each link moves 90 degrees from its original position. It is expected that velocity trajectory and acceleration trajectory for each joint are the same as joint 1 except joint 2 whose trajectories are reversed. Since the complete Lagrange-Euler equations of motion require long computational time, it is common to simplify the Lagrange-Euler equations of motion by neglecting the off-diagonal terms in the acceleration-related matrix and the Coriolis and centrifugal terms. Though the simplified Lagrange-Euler equations of motion has an advantage for computational time, the neglected terms becomes significant when the robot arm is moving at high speeds. In order to show the effects of neglecting the off-diagonal elements of the acceleration-related term and the velocity-related terms ( Coriolis and centrifugal ), the applied torques have been computed for the following cases: (a) the Lagrange-Euler equations of motion with the off-diagonal elements of the acceleration-related matrix set to zero, (b) the Lagrange-Euler equations of motion without the Coriolis and centrifugal terms and (c) the Robot Systems Division Cormputer Simulation

20 RSD-TR5-82 Lagrange-Euler equations of motion with both the off-diagonal elements in the acceleration-related matrix and the Coriolis and centrifugal terms set to zero. Figures 6-1 1 show the applied torques computed from the Newton-Euler equations of motion and from the case (a). Figures 12-17 show the applied torques from the Newton-Euler equations of motion and from the case (b). The applied torques from the Newton-Euler equations of motion and from the case (c) are shown in Figures 18-23. For this particular motion through a given trajectory ( fast movement ), the off-diagonal terms in the acceleration-related matrix are large and dominant. In joints 2,3,4 and 5, the differences between the applied torques computed from the recursive control law and the simplified control law are large and as a result It Is expected to have large position errors In joints 2,3,4 and 5. In Figures 24-41, the position errors between the two dynamic models, the Newton-Euler equations of motion and the simplified Lagrange-Euler equations of motion, are shown for each Joint for various loading conditions. The recursive controller based on the Newton-Euler equations of motion always shows better perfosrmance for various loading conditions and various trajectories. Although the position errors from the proposed control technique are slightly "oscillatory" about the desired position set points, they are always small. The simulation results are tabulated in Table 3. Since the manipulator is a highly nonlinear and complex system, further improvements in the performance of the control law can be done by using adaptive feedback gains. Our future work will focus on finding proper adaptive control strategies for industrial robots whose loads are varying within a task cycle time [LeC82]. Robot Systems Division Computer Simulation

RSD-TR5-82 21 Simplified model Complete model Various Trajectory Tracking Trajectory Tracking Loading Joint Max. Error Max. Error Max. Error Max. Error Conditions ( radian ) (mm ) ( radian ) ( mm ) 1 0.0194 1 9.40 0.0040 4.00 2 0.0494 49.40 0.0070 7.00 No Load 3 0.1882 94.10 0.0101 5.07 4 0.4698 70.47 0.0062 0.94 5 0.2278 34.17 0.0010 0.16 6 0.0726 10.89 0.0039 0.59 1 0.0239 23.90 0.0046 4.60 2 0.0748 74.80 0.0099 9.90 1/2 Max. Load 3 0.2435 121.75 0.0151 7.53 4 0.7565 113.47 0.0096 1.44 5 0.3056 45.84 0.0014 0.21 6 0.1605 24.08 0.0041 0.61 1 0.0261 26.10 0.0048 4.80 2 0.0954 95.54 0.0122 12.20 Max. Load 3 0.2812 140.60 0.0193 9.65 4 0.8752 131.28 0.01 22 1.83.5 0.3523 52.85 0.0016 2.33 6 0.2131 31.97 0.0053 0.79 Table 3 Comparison of Control Method based on Two Dynamic Models Robot Systems Division Computer Simulation

22 RSD-TR5-82 (1) Set i = 0 where i = ith sampling period. (2) Determine [d[i], 't[1] and f'9[i] from preplanned trajectory. (3) Compute position and velocity errors. e[i] = d[] _,a[/] elf]= Ci]- a~[ (4) Determine error signals with Kv, Kp feedback gain matrices. '9[i] + Kve[i]. Kpe[i] (6) Forward recursive equations in Newton-Euler formulation. (8) Backward recursive equations in Newton-Euler formulation to determine the applied torques rI[i]. (7) Compute coefficients of robot arm model using Lagrange-Euler formulation as in Eq. (3.6). (8) Integrate the dynamic equation of a PUMA arm derived from Lagrange-Euler formulation using the 4th order Runge-Kutta method. The outputs are 3a[i] and (9) Set i = i + 1 next sampling period. (10)l Is i = N? ( Total of N sampling periods). If yes, stop. Else go to step (2). Figure 2 Flow-Chart of Computer Simulation for the Proposed Controller Robot Systems Division Computer Simulation

RSD-TRS-82 28 t.57t - t.257 - Of * 91t25 ~-,.6283.t31t -,0000, 0 000.2000 4000.6000.8000 1.000 Time (s5ec)Figure 3 Position Trajectory ( Joint 1 ) Robot Systems Division Computer Simulation

24 RSD-TR5-82 31980 v3, 80 - 3.205 - on U 18807.1058 - 0.000.2000 o tooo.6000.28000 1.000 Ti me sec) Figure 4 Velocity Trajectory ( Joint I ) Robot Systems Division Computer Simulation

RSD-TR5-82 25 15.32 - U t 1 91. tgt I I U 17 3,06t - ~ - -3.06/ L.) U -9. t91t -r -t5.32 A,.000 o2000 000ooo. 6000. 8000,oo000 Time (sec) Figure 5 Acceleration Trajectory ( Joint 1 ) Robot Systems Division Computer Simulation

26 RSD-TR5-82 Jont t1 99.87 Tcompwt.e Yl\ n Ti (a) 58.10 i6,3't - r -L -25 st3 -67,19 -109.0 0.000.2000. OOo. 6000 68000. t. 000 Time (sec) Figure 6 TComplete vs. Case (a) Robot Systems Division Computer Simulation

RSD-TR5-82 27 Jo n~t t2 t17,82 Tcompjae -t7.28 -52.39 L -87.50 -t22.6 - -57.7 - 0.000. 2000. o oo.6000.8000 t.000 Ti me (sec) Figure 7 TComplete vs. Case (a) Robot Systems Division Computer Simulation

28 RSD-TR5-82 J' ent 4t3 -7Z tO.O* Toomplete Tcas.(a) -1tt.57 -2e2,03 H. -29.50 -36.,97 o000. 000.00 o000..6000.8000 t000 Time (sec) Figure 8 TComplete vs. Case (a) Robot Systems Division Computer Simulation

RSD-TR5-82 29 Jotnt #f.3177 Tcom.tt I~ HA/ / TcO (a) [,912 06t7 - I-, 0618 -,.883 - 0.000.2000.ooo.o6000.8000 1.000 Time (sec) Figure 9 Tcomplete vs. Case (a) Robot Systems Division Computer Simulation

30 RSD-TR5-82 Join M5. 1580. I -, w30t t * t* O 9;n TTowmp(a) L -.94509 - 5973 0.000 2 o000. o000.6000 800o0 1.000 Tlime (sec) Figure 10 TCompepte vs. Case (a) Robot Systems Division Computer Simulation

RSD-TR5-82 31 Jo tnt t6 6.565 Toomplato I.338 - ri Tam(a) CD -..' -2. 3t' 0.000.2000.9000.6000 18000 1.000 Ti me (sec) Figure 11 TCompete vs. Case (a) Robot Systems Division Computer Simulation

Joint #1 97.53 Tcomplets Tcn,, (b) 56.23 I t,3 -3 CL -26.36,.676 - -109.0 0o.0.2000,L+0o0.6000.8000 1.000 Time (sec) Figure 12 TCompiete Vs. Case (b) Robot Systems Division Computer Simulation

RSD-TR5-82 33 -3.O+ 1, Tcompete Teo, (b) -2 7. 3.. I -5l 63 r L -75.93 -100.2 -124. 5 0. 000.aooo, Ooo.6o000.8000 t.000 Time (sec) Figure 13 TComplete vs. Case (b) Robot Systems Division Computer Simulation

34 RSD-TR5-82 Jo tnt t3 -9959 - 1 ompLt Tcau ble -16.27 -22,59,L -28.90 F-35922 -'t 1.53.000 o2000.o000.6000.8000 t1000 Timne ~(sec) Figure 14 TComplete vs. Case (b) Robot Systems Division Computer Simulation

RSD-TR5-82 85 Jo tnt #t '.t56't J' [ rl ~ ~.To., (b) L..0t 80 0 S20X -.0881 -_ - 222 - o.000.2000 000.6000.8000 1. 000 Tlime (sec) Figure 15 TComplete vs. Case (b) Robot Systems Division Computer Simulation

36 RSD-TR6-82 Jo nt 4t5 -t8tf - - - 32't '* compaete r %..e 1111~~~~~~~~~~~~l Tm A(b)!-.6038 o.000.2000.000. 6000.8000 1.000 Time (sec) Figure 16 TComp/ete vs. Case (b) Robot Systems Division Computer Simulation

RSD-TR5-82 37 Jo tnt t6 7.672 Tc omp1 vt 5.202 CD. 21 733 Z-2.206 -'.676 -0o oo.000 2000.+000.6000.8000 11000 Time (sec) Figure 17 Tcomplete vs. Case (b) Robot Systems Division Computer Simulation

38 RSD'-TR5-82 Jo trn 41 tot.? t Tmpjetc 59, 55 -a, zIo - L '1~-2'1t 70 -66,83 -t10910 o.000 a2000., ooo, 6000,8000 1.000 T' me (sec) Figure 18 Tcomplete vs. Case (c) Robot Systems Division Computer Simulation

RSD-TR5-82 89 Jo rnt t2 23,69 ' ' Rt~3~~ s 1;gS 1 o * zc~~~~~~TOomplet. rI T. (Tc(c) i -,5tlF - <3 -t63.2-, 0,000.2000.o000.6000 8000 1t 000 T me (sec) Figure 19 TComplete vs. Case (c) Robot Systems Division Computer Simulation

40 RSD-TR5-82 Jo'nt t3 -8,36"' -t * TcOmpJt Troi.(c) -1t5 t 28 - i -22, t9 L -29.10 t -'s,93 - o0000 o 2000.t ooo.6000 o000 t.oo T me ( sec) Figure 20 Toomplete vs. Case (c) Robot Systems Division Computer Simulation

RSD-TR5-82 41 Jo nt tn ~ 31?77 - * Tcompsute E, tIt2 - n T /. T (m rI Tome (c). 069? - -0618 7 -. t883 - o0, 000.2000,o000.6000 8000 t.000 T i me sec ) Figure 21 Tcomplete vs. Case (c) Robot Systems Division Computer Simulation

42 RSD-TR5-82 Jo -tnt tt5 * 7'comi, tE.t5Z r ' ) 79 ~Figure 22 Tc~or~plete vs. Case (c),7437 0D000.ao000.4000.6000.8000 1.000 T im sec) Figure 22 Tcomplete vs. Case Cc) Robot Systems Division Computer Simulation

RSD-TR5-82 43 Jo lin 4t6 _.I 6. 565 ZI -.t1310 F-21363 -~, 595 -o0 000.o2000 o 00.6000,8000 t.000 Time (sec) Figure 23 TComplete vs. Case (c) Robot Systems Division Computer Simulation

44 RSD-TR5-82 Jo tnt 41.0194I I[I Complete Model * $Simvplifiesd Modet.0 26 U ~0 I.. Li -.0008 o -.0075 O.000 2000.1000. 6000.8000 1 ~ 000 Time (sec) Figure 24 No Load Robot Systems Division Computer Simulation

RSD-TR5-82 45 Jo int 42.0osgL4 n Complete Model SimpliTied Model.03'+9.0060 C c -.0089f,020~.29 0. 000 2000.4 000O.6000 8000 1t 000 T ime (sec) Figure 25 No Load Robot Systems Division Computer Simulation

46 RSD-TR5-82.15L~Lt JOtnt 4t3 II Complete Model * SimplifPiesd Model C ',.0858 LC Ld o -.t1197 0 000.2000. too0.6000 8000 1.000 Titne (sec) Figure 26 No Load Robot Systems Division Computer Simulation

RSD-TR5-82 47 Jo nt #It, *698 FI Complete Model Simrplifiied Model h,.3298 -,1898 n i \ Ld, 0 9 0 -:0901 - -. 230 -. 0. 000.2000.o00O.6000 o 8000 1t000 Time (sec) Figure 27 No Load Robot Systems Division Computer Simulation

48 RSD-TR5-82 Jotnt 45.2278 t I Complete Model * Siplijfiesd Model C t622 C 0 u -. t03tol -0 t oo t O 000.2000.'+000,6000,8000 lt. 000 Ti me (sec) Figure 28 No Load Robot Systems Division Computer Simulation

RSD-TR5-82 49 Jo tnt I6.0 726 ' ri Complete Model Simplified Model.0 I90 - o3 1025'tL 0 r-4 VI o -.0217 o.000.2000 oo000o.6000.8000 1.000 Tine (sec) Figure 29 No Load Robot Systems Division Computer Simulation

50 RSD-TRS-82 Jo nlt 1,0239 - II Complete Model Sirpli f isd Model,0155 - rnl LL[ - -"" CL C -. 0098 - -,0183.... 0.OO. 2000. tooO.6000., 8000.I o00 Ti me (sec) Figure 30 Half Load Robot Systems Division Computer Simulation

RSD-TR5-82 51 Jo tn 4t2.0I? 8 nr Complete Model Simplif ie d Model,058.8 -,0308 LLJ 0 Li 0 -.013l - -.0350 -O, 000 0 o000.OOOO 6o00 o8000 t o000 T me ( sec) Figure 31 Half Load Robot Systems Division Computer Simulation

82 RSD-TR5-82 Joatn 43 t998 rI Complete Model Simplif isd Mode l,022'L __j' Li r-.I l, f 35 O, 000,o000.000,6000,8000 1.000 Time Csec) Figure 32 Half Load Robot Systems Division Computer Simulation

RSD-TR5-82 5a Jo nt etL,7565 "II Complete Model Siemplified Model.5' t - 3271 (U L Figure 33 Half Load Robot Systems Division Computer Simulation

54 RSD-TR5-82 Jo nrt #5 3056 - Hl Complete Model SimpLifie d Modea.2200, t3- t a0 ~489,0't89 a oI_ <3 -, 0366 - I222 - 0.000 oo000.ao00 oo 00ooo 800 1t000 T in me (sec) Figure 34 Half Load Robot Systems Division Computer Simulation

RSD-TR5-82 65 Jotn' 46.t605 In Complete Modls * $~Simplified Model /",10t66 - Q -',0550 - t1089 o0.000.2000.tooo 0.6000 8000 t.o00 Ti me ( sec) Figure 35 Half Load Robot Systems Division Computer Simulation

56 RSD-TR5-82 Jo int tl rII Complete Model /,* Simplifie d Model I 016 (U.0073 L_ -.0019 Ir0 -,OttU - -.2011 - 0..000 000.a 000.6000.8000 1.000 Time (sec) Figure 36 Maximum Load Robot Systems Division Computer Simulation

RSD-TR5-82 57 Jo tnt t_.095'+ II Complete Model Simplified Model.0673 -C ID 1 -, 169 -1- 50' LL -~~T Ottt z 5ec ) 0.000.2000.tOOO -.6000.8000 1.000 Time (sec) Figure 37 Maximum Load Robot Systems Division Computer Simulation

58 RSD-TR5-82 Jotnt -3.230 2 Ii Complete Model * Simplified Model 1 t280 (U,0256 C.,, 789 r-I.000 000 o.000.8 000 1.00 Ti me (sec) Figure 38 Maximum Load Robot Systems Division Computer Simulation

RSD-TR6-82 59 Jotn~ #It.8752 8n2f Complete Model Simplified Model C.6280 C,380 L C aI O - 1139 - 000 2000 00 80 00 o. 000.2000.+0ooo.6000.8000 t. ooo Tlmne (sec) Figure 39 Maximum Load Robot Systems Division Computer Simulation

60 RSD-TR-82 Joint 45.3523 Hr Complete Model Simpli f iedModel,2487 ru m... m.0t16 C -, 1655 - 0.000 oo000.000 ooo 000.8000 1.000 Tme (seca) Figure 40 Maximum Load Robot Systems Division Computer Simulation

RSD-TR5-82 61 Jo Lnt 4t6.23t - I1 Complete Model Simplif ied Model,t't?07 C 7 L -I "' |o68t - -,0042 O,-.0767 - 0 t920 O - 100 o., 000.2000.o000,6000. eooo t.ooo Ti me (sec) Figure 41 Maximum Load Robot Systems Division Computer Simulation

62 RSD-TR5-82 6. References [Alb75] Albus, J. S., "A New Approach to Manipulator Control: The Cerebellar Model Articulation Controller (CMAC)," Transaction of the ASME, Journal of Dynamic Systems, Measurement and Control, Series G, Vol. 97, No. 3, September 1975, pp. 220-227. [BeJ74] Bejczy, A. K., "Robot Arm Dynamics and Control," Technical Memo 33 -669. Jet Propulsion Laboratory, February 1974. [DeH55] Denavit, J., Hartenberg R. S., "A Kinematic Notation for Lower-Pair Mechanisms Based on Matrices," Journal of Applied Mechanics, June 1955, pp.215-221. [Hol80 Hollerbach, J. M., "A Recursive Lagrangian Formulation of Manipulator Dynamics and a Comparative Study of Dynamics Formulation Complexity," IEEE Trans. on Systems, Man, and Cybernetics, Vol. SMC-1 0, No. 1 1, November 1980, pp. 730-736, [HoT80] Horowitz, R., Tomizuka M., "An Adaptive Control Scheme for Mechanical Manipulators- Compensation of Nonlinearity and Decoupling Control," Dynamic System and Control Division of the AoS.M.E., Winter Annual Meeting, Chicago, IIl., November 1980. [KaB71] Kahn, M. E., Roth, B., "The Near-Minimum-Time Control of Open-Loop Articulated Kinematic Chains," Transaction of the ASME, Journal of Dynamic Systems, Measurement, and Control, September 1971, pp 164-172. [LeC82] Lee, C. S. G.,Chung, M. J. "An Adaptive Control Strategy for ComputerBased Manipulators," Proceedings of the 21st Conference on Decision and Control Orlando, Fla., December 8-10, 1982. [Lew74] Lewis, R. — A., "Autonomous Manipulation on a Robot: Summary of Robot Systems Division References

RSD-TR5-82 63 Manipulator Software Functions," Technical Memo 33-679, Jet Propulsion Lab, March, 1974. [LLN82] Lee, C. S. G., Lee, B. H., Nigam, R., "An Efficient Formulation of Robot Arm Dynamics for Control and Computer Simulation," CRIM Technical Report RSD-TR-8-82, University of Michigan, August, 1982. [LWP80] Luh, J. Y. S., Walker M. W., and Paul R. P. C., "On-Line Computational Scheme for Mechanical Manipulators," Transaction of ASME, Journal of Dynamic Systems, Measurement, and Control, Vol. 120, June 1980 pp. 69-76. [Mar73] Markiewicz, B.R., "Analysis of the Computed Torque Drive Method and Comparison with Conventional Position Servo for a ComputerControlled Manipulator," Technical Memorandum 33-601, Jet Propulsion Laboratory, March 15, 1973. [OMV79] Orin, D. E., McGhee, R. B., and Vukobratovic, M., and Hartoch, G., "Kinematic and Kinetic Analysis of Open-Chain Linkages Utilizing Newton-Euler Methods," Mathematical Biosciences, Vol. 43, No.1/2, February 1979, pp 107-130. [Pau72] Paul, R. "Modeling, Trajectory Calculation, and Servoing of a Computer Controlled Arm," Stanford Artificial Intelligence Laboratory Memo AM177, November 1972. [Ple68] Pieper, D. L., "The Kinematics of Manipulators Under Computer Control," Computer Science Department, Stanford University, Artificial Intelligence Project Memo No. 72, October 1968. [Sym71] Symon, K. R., Mechanics, 2nd addition, Addison-Wesley, 1971. [TML80] Turney, J., Mudge, T. N., Lee, C. S. G., "Equivalence of Two Formulations for Robot Arm Dynamics," SEL Report 142, ECE Department, University Robot Systems Division References

UNIVE:ISITY OF MOICIA" 23 3 9015 03023 323 R-TR82 of Michigan, December 1980. [Whi69] Whitney, D. E., "Resolved Motion Rate Control of Manipulators and Human Prostheses," IEEE Transaction on Man-Machine Systems, Vol. MMS-1 0, no. 2, June 1969 pp 47-53 Robot Systems Division References