RSD-TR-6-84 INCORPORATION OF PAYLOAD UNCERTAINTIES INTO ROBOT TRAJECTORY PLANNING1 Kang G. Shin and Neil D. McKay Department of Electrical Engineering and Computer Science The University of Michigan Ann Arbor, Michigan 48109-1109 May 1985 CENTER FOR RESEARCH ON INTEGRATED MANUFACTURING Robot Systems Division COLLEGE OF ENGINEERING THE UNIVERSITY OF MICHIGAN ANN ARBOR, MICHIGAN 48109-1109 'The work reported here is supported in part by the NSF Grant No. ECS.8409938 and the U.S. AFOSR Contract No. F49620-82-C-0089. Any opinions, findings, and conclusions or recommendations expressed in this publication are those of the authors and do not necessarily reflect the view of the funding agencies.

TABLE OF CONTENTS 1. INTRODUCTION......................................................... 2 2. CALCULATION OF DYNAMIC COEFFICIENT ERRORS......................... 6 3. CALCULATION OF TORQUE ERROR BOUNDS....................................... 10 4. NUMERICAL EXAMPLE......................................................... 19 5. CONCLUSIONS......................................................................................... 30 ii

RSD-TR-6-85 ABSTRACT A number of trajectory planning algorithms are available for determining a time history of the joint torques, positions, and velocities required to move a manipulator along a given geometric path in minimum time. These schemes require knowledge of the robot's dynamics, which in turn depend upon the characteristics of the payload which the robot is carrying. In practice, the dynamic properties of the payload will not be known exactly, so that the dynamics of the robot, and hence the required joint torques, must be calculated for a nominal set of payload characteristics. But since these trajectory planners generate nominal joint torques which are at the limits of the robot's capabilities, moving the robot along the desired geometric path at speeds calculated for the nominal payload may require torques which exceed the robot's capabilities. In this paper, bounds on joint torque uncertainties are derived in terms of payload uncertainties. This allows the trajectory planner to incorporate payload uncertainties into the trajectory planning process. Payload Uncertainties 1

RSD-TR-8-85 1. INTRODUCTION Various algorithms are available for performing trajectory planning for robots, i.e., generating a time history of desired positions, velocities, accelerations and torques [1, 5, 6]. These trajectory planners require knowledge of the robot's dynamics, which in turn depend upon the characteristics of the payload being carried. In practice, the exact characteristics of the payload will not be known; since the trajectory planners referenced above need to know the exact dynamics of the robot, the trajectory planning process must be carried out with dynamics which are calculated for a nominal payload. This practice can lead to difficulties. To see why this is the case, note that these trajectory planners generate nominal torques which are at the limits of the robot's capabilities for the given dynamics. Moving the robot along the desired path at speeds calculated for the nominal payload may therefore require torques which are beyond the robot's capabilities if the payload differs from the nominal one. If the robot's joints are controlled by independent servoes, as is usually the case, then attempting to make the robot move along the nominal trajectory will result in one or more joints "falling behind", so that the robot strays from the desired geometric path. In other words, the trajectory generated by the trajectory planner is realizable for the nominal payload, but not for the actual payload. There are a number of adaptive controllers which can compensate for the changes in load, provided that the plant (i.e. the robot joint drive) does not saturate [3, 2]. However if the plant saturates, as may happen if the actual and nominal payloads differ too much, then these controllers cannot possibly compensate for load changes. It is the objective of this paper to present an analysis of the torque errors caused by Payload Uncertainties 2

RSD-TR-6-85 payload changes, and incorporate the error information into the trajectory planning process so as to avoid saturation of the individual actuators. Changes in payload characteristics will be expressed as errors in the pseudo-inertia of the payload; the pseudo-inertia is a matrix containing the mass and first and second moments of the payload. It will be shown that bounds on the joint torque errors can be calculated in terms of the norm of the error in the pseudo-inertia of the payload, given the robot's kinematics. The general trajectory planning algorithm given in [6] can then be modified to handle uncertainties in the dynamics caused by the payload. If the actual and nominal payloads are described by the pseudo-inertias IA and IN respectively, then for a given positive real number E, the algorithm generates a trajectory which is realizable for all payloads IA for which IIIA - IN II < E. In order to determine errors in the torques, the dynamic equations of the robot are required. In tensor notation, the dynamic equations describing the behavior of a robot take the general form ui = J qj + Cq qjqk + Rjqq + g (1.1) where ui is the ith generalized force, qi is the ith generalized coordinate, J q is the inertia matrix, Cik is an array of Coriolis coefficients, defined by 1 (J,, aJnik J Cj -A + -i9qJ q 1 (1.2) 2 i.dqk elq clqaq The matrix Rq, is the viscous friction matrix, and g, is the gravitational force. The summation convention has been used here, so that all product terms in (1.1) are summed from 1 to N over repeated indices, where N is the number of degrees of freedom of the robot. The inertia matrix J.,, the Coriolis array C 0 and the gravitational 3 Payload Uncertainties

RSD-TR-6-85 loading vector g, are all functions of the position of the robot and the payload pseudoinertia. It will be assumed that the path which the robot is expected to follow has been given as a parameterized curve in joint space, i.e., the joint coordinates qI are given in terms of a single scalar X by the equations q = f '(X), 0 < X < X, (1.3) This allows all of the joint positions, velocities, and accelerations to be expressed in terms of the scalar parameter X and its time derivatives. Plugging these relations into the dynamic equations gives torque in terms of these quantities also. More specifically, we have = A H d2f d ui -'Ji P+ Jii + i C df ' d + (1.4) " dX " dX2 qskdX dX I dX gi (14) where P = X is the pseudo-velocity. The quantities Jij, Ci,,, and g, depend upon the masses and moments of inertia of the robot's links. The robot's payload is fixed to the last link, and hence must be regarded as part of the last link for purposes of calculating the dynamic coefficients. Therefore Jj,, Cjj, and g, will change as the characteristics of the payload vary. We may write Eq. (1.4) as ui = M,(X,^IN) + Qi(,IN)p2 + Ri(X)p + Si(XIN) (1.5) where the coefficients Mi, Q,, Ri, and Si are given by ay -Ja e(IN) dX P&yload Uncertainties 4

RSD-TR-6-85 QSi -J ( 2dfi df Qi Jij(IN) df X2 + Cik(IN) dx dX R; i df' i R dX and si - si(IN). The functional dependence of Jii, Ciik and gi on the payload IN has been shown explicitly. Then the dynamics of the system after the payload has been perturbed may be written u i = MA(X, IN + AIN)A + Q,(X, IN + IN) + Ri(X)/ + Si(X, IN + AIN). (1.6) In order to avoid excessive torque requirements, we wish to compute a set of velocities and accelerations p and p. such that if the nominal torques ui are given by (1.5), then the actual torques u' i given by (1.6) will be realizable, i.e., ui,'(X,) < u' < u.mX,p). (1.7) Formally, the Robust Trajectory Planning (RTP) problem may be stated as follows: Given a geometric path described as a parameterized curve, the torque limits u,' and ui"^ as functions of X and p, the dynamics of the robot when carrying the nominal payload IN, and a bound E on the norm of the difference between the pseudo-inertias of the actual and nominal payloads, determine the fastest trajectory (sequence of (X,j) pairs) such that the torques u' i given by (1.6) satisfy the constraints (1.7) for all points on the trajectory and for all payload errors AIN such that IIAINII < E. We will solve this problem by calculating the worst-case torque error, as a function of X, it, and ja, for a given payload error, and decreasing the torque limits by this amount when doing trajectory planning. 5 Payload Uncertainties

RSD-TR-6-85 The rest of the paper is organized as follows: in Section 2, torque errors are calculated in terms of changes to the payload pseudo-inertia. In Section 3, bounds on the joint torque errors are derived in terms of bounds on the norm of the pseudo-inertia error. Section 3 also discusses how these results can be incorporated into the trajectory planning process. Section 4 presents a numerical example, and the paper concludes with Section 5. 2. CALCULATION OF DYNAMIC COEFFICIENT ERRORS For a given path, we need to know the changes to the coefficients Mi, Q, Ri,, and,S in Eq. (1.5) which result from changes in the dynamics of the robot. In the sequel, changes in dynamics will be assumed to come from changes in payload characteristics. While changes in friction coefficients, and hence changes to Ri, also contribute to changes in required torques, such changes are independent of changes in payload characteristics, and for the sake of simplicity will not be dealt with here.1 Determining the change to M,, we have M,(IN) =Jj(IN) 4 (2.1) and M,(IN + AIN) = J, (IN + AIN) d (2.2) so that bM (IN,lIN) = (IN + AIN)- MA(IN) (2.3) tSuch changes are usually determined experimentally. Payload Uncertainties 6

RSD-TR-6-85 = {J,(IN + AIN)- J(IN)}7 d -6J,(IN^AIN) df IdX ), dX Differences between nominal and actual payload characteristics cause changes in the coefficients J,y, Ci, and gi in equation (1.1). Here we determine the relationship between changes in these coefficients and changes in payload characteristics. Changes in payload characteristics will result in changes to the pseudo-inertia tensor of the last joint of the robot, i.e., the pseudo-inertia tensor will have the value IN + AIN instead of IN. In order to obtain 6Mi, we consider how this affects the inertia matrix. The coefficients in the inertia matrix are given in [4] as J. (IN) = E j)Tr d (2.4) p=m ax(Sj) dq' 9q where Tp is the 4X4 homogeneous transformation matrix which transforms vectors given in the coordinate system associated with the p t link of the robot to world or base coordinates, and Ip is the pseudo-inertia of the pth link given in the p h link's coordinate frame. The pseudo-inertia is defined as the matrix [4] fz2dm fIy dm fzz dm fz dm f y dm fy2dm fyz dm fy dm I, =.(2.5) fz dm fyz dm fz2 dm f z dm fz dm fY dm z dm f dm The coordinates z, y, and z are expressed in the p h coordinate frame, and the integrals are all taken over the p th link. 7 Payload Uncertainties

RSD-TR-6-85 Introducing an error AIN into the pseudo-inertia of the last joint gives Jij (IN + AIN)= T I + Tr (IN + AIN) (2.6) P=mx(i) q aq a qi ' Subtracting (2.4) from (2.6) gives - - aIN. (2.7) b (IN,AI) = J, (A^I ) = TrAIN A N (2.7) Note that the error in the inertia matrix is linear in the pseudo-inertia error, and is independent of the nominal payload. To find 6M,, simply plug (2.7) into (2.3), giving 6M yjdfTr aqr &TNIN ). (2.8) dX aq i9qi Computation of the errors 6Q, follows the same pattern as the computation of 6Mi. The errors in the Coriolis terms can be determined in much the same way as the errors in the inertia matrix. From [4] we have N a aTp aT C0ik = TS -iP TT J (2.9) =mx (i,j.k) Iaq)qk q aq' The errors in the Coriolis terms due to errors in payload characteristics are therefore given by AC6sk = Tr N (N -aN, J | (2.10) The definition of Qi gives Payload Uncertainties 8

RSD-TR-6-85 Now we need to know the error in the gravitational terms. The gravitational forces g, are given by [4] 0q' GI q d X' N dTk } g A= -me GT i rk (2.12) + JE Tr aTN AIN?] d/i d kt ~ ~i=i,q {T i k ij aq)q' - ifX dNow we need to know the error in the gravitational terms. The gravitational forces gi are given by [4] N aT T 9i', -ink G __ r- (2.12) where 0 G= g is the gravitational force vector, mt is the mass of the kh link, g is the acceleration due to gravity, and rk= is the center of mass of the k tA link given in the coordinates of the k h frame. If we define wk mA r-, then we have N DTk g, -EGT. wk. (2.13) k== dq 9 Payload Uncertainties

RSD-TR-6-85 But wk is just the last column of the pseudo-inertia matrix I, so that s.i -- i OaQ' Ik. (2.14) As be k gives As before, introducing an error into the pseudo-inertia of the last link gives Si = -g, = —GT ATN IN 0. (2.15) 1 We may now calculate the error in u, by adding up the individual components, giving bui = 6M + 6Qi p2 + 6Si. (2.16) 3. CALCULATION OF TORQUE ERROR BOUNDS If the errors AIN were known exactly, then the torque errors could also be computed exactly. Of course, in practice AIN will not be known exactly. However, if bounds on the norm of AIN can be obtained, then we may find bounds on 6u,. To obtain these bounds, note that 6Mi, 6Qi, and 6Si are all functions of the pseudo-inertia error AIN; in fact, they are linear in AIN, so that 6u, is also linear in AIN. If we write 6u = Z(AIN), (3.1) then we wish to maximize or minimize the linear function Z with respect to AIN, subject to Payload Uncertainties 10

RSD-TR-6-85 IIAINII < E (3.2) where E is the bound on the pseudo-inertia error. At this point, some observations are in order. First, as we noted before, the errors in the 6u, depend linearly upon the pseudo-inertia error AIN. Second, note also that 6u, depends only on the kinematics of the robot and on the desired velocity and acceleration, not on the nominal dynamics. These facts are consequences of the fact that both kinetic and potential energy are linear in mass. To see this, consider the form of the Lagrangian, namely L - K - P, where K and P are the kinetic and potential energies of the robot. Any difference between the actual and nominal dynamics of the robot will cause a corresponding difference in the Lagrangian. If we think of the error in the pseudo-inertia as a piece of stray material stuck to the end effector, then both the kinetic and potential energies of this material, and therefore the Lagrangian, depend only upon the mass and shape of the stray material and upon the position and speed of the end effector. The Lagrangian is linear in the mass of the material; since Lagrange's differential equations are linear in L, the errors in the generalized forces can be separated from the forces resulting from the nominal mass of the manipulator. One consequence of this separability of forces is that the errors 6u, do not depend upon the nominal dynamics, as shown above. The implication of this is that much of the error analysis can proceed without regard to the nominal dynamics of the robot. The linearity of the bu, in the pseudo-inertia has some other practical consequences as well. Consider the maximization which must be performed in order to evaluate 6u;. This maximization requires that the space of 4X4 symmetric matrices with norm less than E be searched, which in general is a rather formidable problem. 11 Payload Uncertainties

RSD-TR-6-85 However, by choosing a particular class of matrix norms, the problem can be made quite simple; in fact, it can be transformed into a linear programming problem. To see how this transformation can be performed, consider the problem of maximizing the function Z in equation (3.1), namely Problem A: maximize Z(M) = -E - Mi (3 3) i I subject to IIMII < E and M=Mr. (34) Treatment of the minimization problem proceeds analogously to problem A. We will show that problem A transforms into a linear programming problem if the norm used to constrain the matrix M in (3.4) is chosen properly. This will be accomplished by eliminating some absolute values from the constraints. Z(M), the function to be maximized, is a linear function of M. It remains to be shown that the constraints can be made linear. Of course if the norm used in problem A is arbitrary, then in general the constraints will not be linear. However, there is a set of norms, all very easy to calculate, which will yield linear constraints. Consider the class of functions F: R4x4'R+ given by F (M) = max ai (M) where 4 4 Ui(M)- E Cijk IMjk I. j=ik = The matrix 1-norm and oo-norm, max I Mi I and E I Mi, I are all functions of this Payload Uncertainties 12

RSD-TR-6-85 form. We now show that, under suitable conditions, F is a norm. Lemma: If aQjk > 0 for all i, j and k, and if for every pair of indices (j,k) there is an i such that a ijk 7 O, then F (M) is a norm. Proof: In order to prove that F is a norm, we must show that 1)F(M) > 0 for all M, 2) F(M) = iff M = 0, 3) F (M) = - 1 I F(M) for all scalars - and all matrices M, and 4) F (X + Y) < F(X) + F (Y) for all matrices X and Y. Obviously (1) is true, since F is the maximum of a set of non-negative quantities. F(O) = 0, proving the "if" part of (2). To prove the "only. if" part, observe that if M is non-zero, then it has some non-zero element Myk. For this particular jk pair, there is some i such that ajk is non-zero, so that a7 > 0 for this i. Therefore F > 0. (3) is true, since 4 4 4 4 a i(AM) - S k IMp =Mk I'f S a I Mk = -I7lo(M) =lIk= j =ik = and hence F (yM) = max o (7M) = max I y l,(M)= I | max,i (M) = I- I F(M). i i s Finally, (X + Y)= Etaij IXik +.Yi, I j k < Ccai k IXik I + EC |jk IYik - (X)+ r(Y) so that 13 Payload Uncertainties

RSD-TR-6-85 F (X + Y) = max ( (X + Y) < max a (X) + r (Y) < max oi (X)} + max ( (Y) = F (X) + F(Y) 3 Problem A with this class of norms becomes Problem B: maximize Z(M)- = Ei Mi (3 5) 4 4 subject to max E c aik I M, | < E and Mjk = MMk. This problem obviously is equivalent to Problem C: maximize Z(M) = E- j Mii (3 6). i 4 4 subject to aijk | Mijk < E and Mk = Mk. j =lk =1 Problem C may be transformed into a standard linear programming problem by making the substitutions Mii = Pi - Nij and I Mi I = P;i + Nij, where P;i and Ni, are non-negative real numbers. To prove that this substitution gives the correct result, first eliminate the symmetry constraint, giving Payload Uncertainties 14

RSD-TR-6-85 4 4 Problem C': maximize W(M)= S E B jkMik i =1t =i 4 4 subject to S S a '* i Mik I< E j=l =i where a; --- j —k j=k j ij= al jjk + aO k. j3k and = bi + i-ii Then we have the following theorem: Theorem: Let problem D be defined as: 4 4 maximize Z(P,N)= S ik (Pik - Ni) j=1k =i 4 4 subject to E a' i (Pik + Ntk) < E and Pii > 0, Nii ~ 0 i =1k =j Then the optimal W from problem C' is equal to the optimal Z from problem D. Proof: Let M' be a solution of problem C', and let W =' W(M ). If we make the substitutions 15 Payload Uncertainties

RSD-TR-6-85 MOA M, > 0 k lo0 M;t < 0 and fo M, > 0 Nik ---- M, N- = Mik Mik < 0 then we have Mi = Pij - Niy, and Mi, | = Pi + Ni,. Making these substitutions in problem C' gives 4 4 W' =W(M*)-= — E Y9 ik(Prk -Nik)-=Z(P,N) 4 4 E ac' ii (Pip + Nk) < E and Pii > O, Nij > 0. i=lk =i The conditions for problem D are satisfied, so we must have W' < Z' where Z' is the optimal value of Z obtained from problem D. Likewise, let PO and NO be an optimal solution to problem D. Then for every pair of indices (j,k) we have ' jk > 0, 9 ik = 0, or jk i < 0. If jk > 0, then we must have Ny = 0. Otherwise, we could substitute Py + N j for Pjk and 0 for Nj.; these new values still satisfy the required constraints, but increase the objective function, contradicting the fact that (P', N) is optimal. Similarly, if f jk < 0, then we must have P j = 0. If f jk = 0, then we may take PA = Ny = 0, since this leaves the constraints satisfied and has no effect on the objective function. Therefore we always have either PA = 0 or NAj = 0. Taking Mi = Pj - Nij, it follows that Payload Uncertainties 16

RSD-TR-6-85 Mi |= P i} + Ni'. Making these substitutions in problem D, 4 4 Z' -Z(PN') =I S ff jMjk < W' i =k =i E Sa' i, IMij I -< E i1k =ji Therefore W' < Z' < W', proving the theorem. Now that torque error bounds can be obtained from pseudo-inertia errors, these results must be incorporated into the trajectory planning process. Direct use of the results derived above in the trajectory planner described in [51 is not easy, since this trajectory planner requires that we solve (1.5) for p in terms of X, p, and ui. This solution is required because the trajectory planner must convert torque ranges into p ranges. When errors are introduced, we have u' (X,P) < MA + Qi + Rij + Ss + min l ME { + 6Q, 2 + 6S < Mi p+ Qi R + + Ri + + mla {M M + 6Qi2 + 6S ) < u.ma(X,). For given X and p, these inequalities determine a range of values of p. However, the worst-case values of 6M,, 6Qi, and 6S, depend upon A, which in turn depends upon the values of 6Mi, 6Q;, and 6S,, so finding the allowable range of values of p explicitly is 17 Payload Uncertainties

RSD-TR-6-85 difficult. Of course these equations can be solved numerically. For example, to find the maximum allowable value of p, the equation M,+QP2+Rip+ Si + maIx {M + + i 6, \ = u.u m(X,p) ||AlN|[<E 1 can be solved by bisection for p. A simpler solution to the problem is to use one of the trajectory planners described in [6] or [7]. These trajectory planners only need to have available a test function which determines whether or not a given (X,p,p) triple requires excessive torque; in effect, they automatically perform the numerical search for the allowable values of p. But such a function is easily constructed, since for a given (X,p,I) we can easily minimize or maximize bu; in (2.16), and see if u, + 6um1 exceeds u m~X,^) or u, + bumrn falls below U mXn(X,). In particular, the following algorithm checks to see if a particular (X,p,p) triple meets all the torque constraints: for each joint i do begin compute uN= M,(X)p + Q,(X)p2 + R (X)p + Si(X) compute bun = max M(X,(JAIN)+ IN + (XAN)2 + S(X,AIN) i llcm e IIJ<EI. computeSM = mrin {tMi(X,A~IN)p + JQ,(XAIN)p2 + 6S(X,AIN)} c pJAIN l\<E if uN + bumin < umin(X,p), then return REJECT if u,N + uimax > umX(X,p), then return REJECT end return ACCEPT. Payload Uncertainties 18

RSD-TR-6-85 It should be noted that this function is called for each (X,.o) pair; it does not, for example, reject a (X,#,p) triple based on an error which is computed for all positions or all velocities. As a consequence, speed is sacrificed only when absolutely necessary to guarantee that the trajectory will be realizable for all payloads within the allowable range. 4. NUMERICAL EXAMPLE As an example, we will apply the methods of the previous section to the first three joints of the Bendix PACS robot arm. This arm is cylindrical in configuration and is driven by DC servos. Its dynamics and actuator characteristics are described in Table 1. The trajectory planner used is described in [6]. The kinematics of the PACS arm are quite simple. If the coordinate frames of the base and hand are as shown in Figure 1, then the coordinate transform T3 is easily shown to be cos 0 -sin 9 -rsin 0 sin e 0 cos rcos.1) T3= 0 -1 0 (4.1) 0 0 1 The partial derivatives of T3 are 0 0 0 0 -sin 9 0 -cos 9 -rcos 9 aT3 0 0 0 0 aT3 cos 0 -sin 9 -rsin (42) ~Qz == 0 0 0 1 ~00 0 0 0 O 0000 0 0 0 0 19 Payload Uncertainties

RSD-TR-6-85 0 00 -sin 0 rT3 000 cos (4.3) 0 0 o0 0o 000 0 There are two non-zero second partials; they are -os 0 sin 0 rsin [0 0 00 -cos 0 82T3 -sin 0 -cos O -rcos 9 a2T3 0 0 -sin U ao2 | 0 0 0 0 | @ 10 0 ~0 0 (4.4) ~4 - o o o o = ~ ) 0 0 0 O 000 0 We are now in a position to compute the 6Mi. If we let H = AIN and define N N m Tj [ N H9q dq' then we have d q' M - mij d X Computing the mij, 00 0 0 i9T i9T TT OTr aT T 0 0 0 m - T H |T and O- H = -- 0 H4 0 I & z Gz J az 9z 00 H440 0 0 0 Taking the trace of this gives mr = H44. mze --- m Tr T- H T Payload Uncertainties 20

RSD-TR-6-86 0 0 0 O -sinO cos$ 0 0 dT3 0T3T 0 0 0 0 0 0 00 d z a = H14 H24 H34 H44 -cos9 -sin 0 0 0 0 0 0 -rcos0 -r sin 0 0 The trace of this matrix is easily found to be zero. mzr — m = Tr -3 H dO3r 00 0 0 0 0 0 0 TT a9T T 0 0 0 0 0 0 00 z a~ H14 4 H24 H34 H44 0 0 00 0 0 0 0 -sin0 cos080 0 The trace of this matrix also is zero. mt = Tr ( H -O -sin 0 -cost -rcose H11 H12 H13 H14 -sinO cosO 0 0 a aTT3T = cost 0 -sinO -rsinO H12 H22 H23 H24 0 0 0 0 adO aH 0 0 0 0 H13 H23 H33 H34 -cos0 -sinO 0 0 0 0 0 0 H 14 H24 H34 H44J -rcos -rsinG 0 0 The diagonal entries of this matrix are cll = H1sin20 + 2H13sinmcos9 + 2Hl4r sinOcosO + H33cos20 + 2H34r cos92 + H44r 2cos20 22 = H11cos2O - 2H,,3sincosG - 2H4r sinmcosO + H33sin28 + 2H4r sin2G + H4r 2sin28 e33 e 44 = 0. 21 Payload Uncertainties

RSD-TR —8-85 Adding these up to get the trace, mo =Tr a~-d3 J = H1 + H33+ 2rH34 + r2H4 mr ---- r, Tr - rH. {aT3 aT37T The diagonal entries of -8 H- are 90 Or e = H14sin2o + H34sinOcosO + H44r sinkcos0 22 H14cos20 - H34sinOcosU - H44r sincosl C33 = C44 = 0. Adding these up gives m,. - H14. { aT3 9T3r mrr = Trl -3 H l 0 0 0 -sin 11 Hl H1 HH13 H14 0 0 0 0 T3 aT3T O 000 cos9 H12 H22 H23 H24 0 0 00 3 3 Or Or = lo 0 00 0 H1 H23 H33 H34 0 0 00 10 0 0 0 H14 H24 H34 H44 -sinO cosO 0 0 H44sin2U -H44sincosO 0 0 -H4,sincos 1 H44cos25 0 0 0 00 ' L 0 0 00 The trace of this is mr,, H44. Payload Uncertainties 22

RSD-TR-6-85 We can now find the 6Mi. We have 6MZ H= dz 6Mo = (Hll + H33 + 2rH34 + r2H)- + H1 dX M= dl dr /Mr = H14 dX + H44 dX (d2T T DT If we define cijk by ci,- = Tr | -i.ak H - aN, then we have I. q) Cajqk 90q' J d2f df dfk Qj = mq d K 7 + c~ik ~ d d Qi idX2 sCk' dX dX We now compute the cijk. Only six cases need to be considered, since all but two of the second partials of T3 are zero. zoe —Tr 1 23H dT3 -cosO 0 sinG r sint H1l H12 H13 H14 0 0 00 a2T3 aT3 -sin8 0 -cosO -r cos8 H12 H22 H23 H24 0 0 0 0 02H az 0 0 0 0 H13 H23 H33 H34 000 0 0 0 0 0 H1 24H3 H4 HO 0 1 0 The trace of this matrix is zero. 02T3 0Ts The diagonal terms of -2 H- are 23 Payload Uncertainties

RSD-TR-6-85 e 11 = H1sinOcos0 - H13sin2U - H14r sin20 + H13cos20 - H3sinOcost - H34r sinkcosO + H14r cos20 - H34r sinOcos9 - H44r2sincosO e22 -Hllsinkcost - H13cos2 - H14r cos2U + H13sin20 + H33sinlcos9 + H34r sinkcosf + H14r sin2l + H34r sinkcosO + H44r 2sincosO C33 - 44 =0 The trace is zero. a2T3 aT3} Cr = Tr| (d2TsH- j. a2T3 aT3 The diagonal terms of -2 H are a02 Dr e 11 H4sin0cosO - H34sin2O - H44r sin2O e 22 = -HlsinOcosO - H34cos28 - H44r cos20 C33 C44 0. The trace therefore is c,e -= -H34 - H44r. aI d2T, r dT 0 0 0-costl H H 12 H13 H14 0 000 D2T3 dT3 0 0 -sini H 2 H22 H23 H24 0 0 0 0 d adr z 0 0O0 H13 H23 H33 H34 0 000o O 0 0 O H14 H,4 H,4 H 0 1 0 Payload Uncertainties 24

RSD-TR-6-85 The trace of this matrix is zero. 2TT3 3 T3 The diagonal terms of - H- are Od or 99 e = H14sinOcosO + H34cos29 + H44r cos2O e 22 -H4lsintcosO + H34sin28 + H4r sin20 C33- 44 = 0. The trace is cr- = H34 + H44r. I 02T3 aTs]3 2T3s aT3 The diagonal terms of a 3 H a are QO Dr Or e 1 =- H44sinOcosO = 22 - -H44sinfcos9 C33 = — 44 -0. The trace is zero. Calculating the 6Qi, 6Q = d2z 25 - H -d X2 25 Payload Uncertainties

RSD-TR-6-85 6Qe = (H,1 + H33 + 2rH34 + r2H) d2 + H d + 2(H34 + rH d d d\ rf+ HdX24 d\ d\ 2. d0 dur [d,} Qr H, dX 2 + 4 d Z2 -(H3 + rH4 dX The bSi, the gravitational error coefficients, are easily found to be 6S, = H44g and 6SS =- S, =- 0, where g is the acceleration due to gravity. We will use the norm 4 4 1|HII- E ES i gHij I i =li =1 where aij > O. This makes the problem of finding the error bounds very simple. It is easily seen that if the functional to be maximized is Z -EEjHj i i then the maximum over H for igHII < E occurs when all the Hii are zero except for those Hi for which |-/ is a maximum; this number times E is also the maximum aii value of Z. If we use J1 2 i-j =ti - 1/2 i-ji then the resulting bounds on the I 6u; I are I d X d j2 - - lI IP,1I < znc r +28 Payload Uncertainties 26

RSD-TR-6-85 16ue < max dd A + - t, d-X + d 2 | d5~ d29 2 dr dO 2 I AdX dXa dX 2r d + 2- dx +2 d X bur 1 max + | dX d2 d | {dr dd2r 2, d2r 1 2 I TP + x- P _ r P IA NII~ dX The joint torques that can be applied to the PACS arm are limited by saturation of the drive motors, which gives a constant torque or force limit for each joint. In addition, there are limits on the voltages which can be applied to the motors, so we need to know how the errors in the joint torques translate into errors in the motor voltages. It will be assumed that the back-EMF constant, winding resistance, and voltage source resistance are known exactly, though this is not necessary. Since for a given speed voltage is a linear function of torque, i.e., V, = A, u, + B,, the change in voltage will be 6Vi = A, 6u,. These changes in voltage can then be added to the nominal voltage and tested against the motor voltage limits in much the same way that the torques are checked against the motor torque saturation limits. The perturbation to the nominal dynamics of the manipulator will be caused by placing a cube with edges of length L and uniform mass density p in the gripper of the robot, with its center of mass coincident with the origin of the end effector coordinate 27 Payload Uncertainties

RSD-TR-8-85 system. The pseudo-inertia of this cube is pL5 0 0 0 0 pLS 0 0 12 A13 0o o - pL6 0 12 o 0 pL3 The norm of this "error" is I1AI311 - p L3+ I pL The maximum torque error for a given range of pseudo-inertia errors occurs when the error bound E is precisely equal to the norm of the actual pseudo-inertia error. Therefore the most stringent test of the results of the previous section is to use a tight error bound, i.e., E = 1AI311 This has been done for a cube with sides of 5 centimeters and densities of 0, 6, 12, 18, 24, and 30 grams/cc. The path traversed is a straight line from the (Cartesian) point (0.7,0.7,0.1) to (0.4,-0.4,0.4). For comparison, the true optimal solution has been calculated, using the actual dynamics (including the effects of the cube in the gripper). The results are summarized in Tables 2 through 6. Table 2 gives traversal times for the true optimal solution and for the case in which errors are included. The "percent difference" column gives the percentage by which the true optimal traversal time is exceeded. Tables 3 and 4 give minimum and maximum voltages, respectively. The actual and nominal values are both computed for the "nominal" trajectory, i.e. the Payload Uncertainties 28

RSD-TR-6-85 trajectory which is calculated with errors included. The actual voltages are those required to move the robot with the cube in the gripper, while the nominal values are those which are required without the cube (i.e. with the nominal payload.) The minimum and maximum voltages available are -40 and 40 volts, and it is easily seen that these limits are not exceeded for any joint or for either payload. Tables 5 and 6 give the minimum and maximum torques or forces for each joint. The torque or force limits are given at the head of the column for the appropriate joint; again, the limits are not exceeded. The phase plane ( X vs. A ) plot and motor voltage vs. time plot for the zerodensity case are shown in Figures 2a and 2b. Since the error is zero in this case, the results are exact. For a density of 12 grams/cc., the optimal and nominal (i.e. with errors included) phase plane plots are shown in Figure 3a. Figure 3b gives joint positions vs. time; z and r are in meters, 9 in radians. Figures 3c through 3e give nominal and actual motor voltages required to drive the robot along the nominal trajectory for the z, 9, and r joints respectively. Figures 3f through 3h give the nominal and actual torques. (The nominal torques/voltages are those which would be required if the actual payload were identical to the nominal payload. The actual torques are the torques required to keep the robot with the perturbed payload on the nominal trajectory.) Figures 4a through 4h show the same plots for a density of 24 grams/cc. It was noted above that none of the joint torque or voltage constraints was violated. However, the minimum voltage for the r joint at one point meets the lower voltage limit. This indicates that the trajectory which is generated when payload errors are included is indeed the fastest possible trajectory for the given range of possible payloads; for this particular point, the worst-case payload happens to have the same 29 Payload Uncertainties

RSD-TR-6-85 characteristics as the actual payload. A larger payload would have resulted in violation of a voltage constraint. Another point to consider is the relationship between the nominal and optimal phase trajectories. It is expected that the nominal phase trajectory will be lower than the optimal trajectory; a nominal trajectory which was higher than the optimal one would lead to a contradiction of the optimality of the optimal trajectory. Also, the difference between the optimal and nominal trajectories increases as the payload error bound increases. This would be expected, since the nominal trajectory must accommodate all payloads within a given range; as the range of payloads increases, the worstcase errors also increase, resulting in more restrictive limits on the nominal torques, and hence slower trajectory traversal times. 5. CONCLUSIONS A method for including payload inertia errors in the manipulator trajectory planning process has been presented. Errors in the payload inertia are characterized by bounds on the norm of the difference between the actual and nominal pseudo-inertias of the payload. Given such a bound, it has been shown that a trajectory can be constructed which meets all torque and force constraints for all actual payloads, provided that the norm of the difference of the pseudo-inertias of the actual and nominal payloads differs by less than the given error bound. This technique was applied to the Bendix PACS robot for a number of different payloads, and the resulting trajectories were shown not to violate any joint torque or motor voltage constraints. In the worst case, in which the actual payload mass differs from the nominal mass by approximately one third of the robot's rated maximum load, the traversal time was less than twenty percent over the optimal value. Payload Uncertainties 30

RSD-TR-6-85 REFERENCES [1] J. E. Bobrow, S. Dubowsky, and J. S. Gibson, "On the Optimal Control of Robotic Manipulators with Actuator Constraints," Proceedings of the 1983 Automatic Control Conference, pp. 782-787, June 1983. [2] S. Dubowsky and D. T. DesForges, "The Application of Model-Referenced Adaptive Control to Robotic Manipulators," ASME Journal of Dynamic Systema, Measurement, and Control, vol. 101, pp. 193-200, September 1979. [3] A. J. Koivo and T. H. Guo, "Adaptive Linear Controller for Robotic Manipulators," IEEE Transactions on Automatic Control, vol. AC-28, no. 2, pp. 162-170, February 1983. [41 R. P. C.-Paul, Robot manipulators: Mathematics, programming, and control. Cambridge, Mass.: MIT Press, 1981. [5] K. G. Shin and N. D. McKay, "Minimum-Time Control of a Robotic Manipulator with Geometric Path Constraints," IEEE Transactions on Automatic Control, vol. AC-30, no. 6, June 1985. [6] K. G. Shin and N. D. McKay, "Robot Path Planning Using Dynamic Programming," Proceedings of the 2,rd CDC, Dec. 1984. [7] K. G. Shin and N. D. McKay, "Minimum Time Trajectory Planning with General Torque Constraints", In preparation. 31 Payload Uncertainties

RSD-TR-6-85 Parameter Description Value rt Saturation torque of 0 motor 2.0 Nt.-M. r/t Saturation torque of r motor 0.05 Nt.-M. r/_a_ Saturation torque of z motor 2.0 Nt.-M. Vmina Lower voltage limit for 0 joint -40 v. Vrmin Lower voltage limit for r joint -40 v. Vzmin Lower voltage limit for z joint -40 v. Ve Upper voltage limit for 0 joint 40 v. Vrmx Upper voltage limit for r joint 40 v. VzmX Upper voltage limit for z joint 40 v. kg Gear ratio for 8 drive 0.01176 k? Gear ratio for r drive 0.00318 Meters/radian kz Gear ratio for z drive 0.00318 Meters/radian ks Motor constant for 9 joint 0.0397 Nt.-M./amp k~ Motor constant for r joint 0.79557 X 10-3 Nt.-M./amp kz Motor constant for z joint 0.0397 Nt.-M./amp R Motor and power supply resistance, 0 joint 1 f R. Motor and power supply resistance, r joint 1 n RZ Motor and power supply resistance, z joint 1 fl ke Friction coefficient of 0 joint 8.0 Kg./sec. kr Friction coefficient of r joint 4.0 Kg./sec. k ____Friction coefficient of z joint 1.0 Kg./sec. Mt Mass of r joint 10.0 Kg. Mz Mass of z joint 40.0 Kg. Jt Moment of inertia around 0 axis 12.3183 Kg.-M. 2 K Moment of inertia offset term 3.0 Kg.-M. Table 1. Dynamic coefficients and actuator characteristics for PACS arm Payload Uncertainties 32

RSD-TR-6-85 Density Time (Seconds) percent difference Nominal Optimal___ 0 1.789 1.789 0 6 1.934 1.844 4.9% 12 2.076 1.898 9.4% 18 2.213 1.950 13.5% 24 2.340 2.002 16.9% 30 2.459 2.054 19.7% Table 2. Traversal times for nominal and optimal trajectories Minimum Voltages Density z joint | 0. joint r joint Nominal Actual Nominal Actual Nominal Actual 0 29.86 29.88 -39.93 -39.93 -40.00 -40.00 6 30.34 30.91 -38.26 -38.52 -37.94 -40.00 12 30.54 31.67 -33.05 -33.51 -36.12 -39.98 18 30.67 32.39 -24.92 -25.37 -34.47 -39.99 24 30.78 33.07 -19.94 -20.38 -33.01 -40.00 30 30.86 33.73 -16.80 -17.18 -31.70 -39.96 Table 3. Minimum required voltages for nominal and actual payloads. Maximum Voltages Density z joint 0 joint r joint Nominal Actual Nominal Actual Nominal Actual 0 37.64 37.64 39.86 39.86 40.00 40.00 6 37.40 38.03 37.14 37.73 32.62 32.60 12 36.79 38.04 17.64 18.25 27.01 27.48 18 35.78 37.61 10.21 11.21 23.79 24.94 24 35.12 37.54 7.30 7.89 21.39 23.41 30 34.69 37.69 5.39 5.98 19.51 22.39 Table 4. Maximum required voltages for nominal and actual payloads. 33 Payload Uncertainties

RSD-TR-8-85 Minimum Torques/forces... Density z joint (Newtons) 0 joint (Newton-Meters) r joint (Newtons) Limit = -629 Nt. Limit = -170 Nt.-M. Limit = -15.7 Nt. Nominal Actual Nominal Actual Nominal Actual 0 333.52 333.52 -112.29 -112.29 -9.99 -9.99 6 335.91 342.20 -104.78 -105.71 -9.47 -9.99 12 363.02 376.62 -87.70 -89.26 -9.02 -9.99 18 373.03 394.00 -62.91 -64.43 -8.61 -9.99 24 377.84 406.17 -48.15 -49.55 -8.24 -9.99 30 380.11 415.72 -39.14 -40.43 -7.92 -9.98 Table 5. Minimum required torques/forces for nominal and actual payloads. Maximum Torques/forces Density z joint (Newtons) O joint (Newton-Meters) r joint (Newtons) Limit = 629 Nt. Limit - 170 Nt.-M. Limit = 15.7 Nt. Nominal Actual Nominal Actual Nominal Actual 0 421.31 421.31 161.72 161.72 10.03 10.03 6 418.68 426.52 150.39 152.36 8.15 8.18 12 414.50 430.04 78.43 80.48 6.76 6.89 18 406.68 429.55 51.39 53.41 5.96 6.25 24 402.17 432.31 38.18 40.19 5.36 5.87 30 400.42 437.96 30.20 32.20 4.89 5.61 Table 6. Maximum required torques/forces for nominal and actual payloads. Payload Uncertainties 34

RSD-TR-6-85 18 SI Figure 1. Kinematics of the Bendix PACS robot 35 Payload Uncertainties ~'i~ ~ 2

RSD-TR-6-85 8.000 6.000 2.000 / 4.000 2.000 4.000 6.000 Figure 2a. Phase plane plot for zero errors. Payload Uncertainties 38

RSD-TR-6-85 0 motor voltage ---—. - r motor voltage z motor voltage - V - v -20t!'''''~-s~-' -'w~. i:.. II II * -20Figure 2b. Voltae s t o 37 Payload Unvertfinties *

RSD-TR-6-85 Optimal ---- ---- With errors E';\ 8.000 // \ 6.000 / V 2.000 2.CO0 4.000 6.000 Figure 3a. Phae plane plots for density 12.0 g./cc. Payload Uncertainties 38

RSD-TR-6-85. joint position ---r joint position z joint position. —.-..-... qdP ~~*. *.. t.. I,, II ~ - ' "" -I " — t 0.500 1.000.1.500 2.000 -0.500 Figure 3b. Jot position s. time fo density 12.0 g./c. Fiur39 3b. Joint psitio v. time forUncerta desity inti.0./cc. 39 Payload Uncertainties

RSD-TR-6-88 Actual - -- Nominal V *i /i 36 /.. 32t 0 I. 0.500 1.000 1.500 2.000 Figure 3c. Nominal and actual motor voltages for z joint, density 12.0 g./cc. Payload Uncertainties 40 Payload Uncertainties 40

RSD-TR-6-85 Actual -r-.-.-.... Nominal V! / A t 0.5001.000 loo 2.000 -20 Figure 3d Nominal and actual motor voltages for 0 joint, density 12.0 g./cc. 41 Payload Uncertainties

RSD-TR-6-85 Actual -- -- Nominal V - t 0.500 1.000 15 2. 20 \, Figure 3e. Nominal and actual motor voltages for r joint, density 12.0 g./cc. Payload Uncertainties 42

RSD-TR-6-85 Actual ------ -- Nominal F 420 / -..,. 0... 00 1...000 1.500 2.oo 380 X. 1 Figure 3f Nominal and actual joint forces for z joint, density 12.0 g./cc. 43 Payload Uncertainties

RSD-TR-6-85 Actual Nominal T I a t 0.500 1.000.500 2.000 -50 Figure 3g. Nominal and actual joint torques for 9 joint, density 12.0 g./cc. Payload Uncertainties 44

RSD-TR-6-85 Actual ---- Nominal - F 5.000 _[ I Ct 0.500 1.000oo S2. -5.000 i' Figure 3b. Nominal and actual joint forces for r joint, density 12.0 g./cc. 45 Payload Uncertainties

RSD-TR-6-85 Optimal -- -. With errors 8 %w 8.000 /. 6.000 4.000 / 2.000 2.000 4.000 6.000 Figure 4a Phase plane plots for density 24.0 g./cc. Payload Uncertainties 46

RSD-TR-6-85,i joint position.... r joint position q z joint position -......0.500.,..o~~~~Ie ~.. 0.500 1.000 1. 2.000 -0.500 me'. Ftt Figure 4b. Joint position vs. time for density 24.0 g./cc. 47 Payload Uncertainties

RSD-TR-6-85 Actual ---- Nominal V./I. -/i\/ 32I, |,,,. to' /I t 0.500 1.000 1.500 2.000 Figure 4c. Nominal and actual motor voltages for z joint, density 24.0 g./cc. Payload Uncertainties 48

RSD-TR-6-85 Actual -.-.-. —.. Nominal V 0.5 00 1.000 1. 2.000t -10 Figure 4d. Nominal and actual motor voltages for 0 joint, density 24.0 g./cc. 49 Payload Uncertainties

RSD-TR-6-85 Actual ------- Nominal V 0.500 1.000 l 12.000 i /~ A" -20 -.Nmialad ctalmto vltgsor ontdn-.it2 Figure 4e. Nominal and actual motor voltages for r joint, density 24.0 g./cc. Payload Uncertainties 60

RSD-TR-8-85 Actual - - * F Nominal 420i i! l - I 380 0.500 1.000 1.500 2.000 Figure 4f. Nominal and actual joit forces for z joint, density 24.0 g./cc. 51 Payload Uncertainties

RSD-TR-6-85 Actual -..-.-.-.Nominal T t -20 -40 Figure 4g. Nominal and actual joint torques for 9 joint, density 24.0 g./cc. Payload Uncertainties 52

UNIVERSITY OF MICHIGAN RSD-T.R —8. 5IIlillllliIIii iNEI Il 3 9015 03525 0524 Actual - -.Nominal F 0.500 1.000 o0 2.000 1 -5.000 / Fiure 4h. Nominal and actual joint forces for r joint, density 24.0 /cc Figure 3h. Nominal ad actual joint fores aor r joint, density 24.0 n./cc. 53 Payload Uncertainties