Adaptive Control of Manipulators Containing Closed Kinematic Loopsl Michael W. Walker Robotics Research Laboratory Department of Electrical Engineering and Computer Science The University of Michigan Ann Arbor, Michigan 48109 February 15, 1988'This work was supported in part by a TRW Faculty Assistantship Grant to the University of Michigan. i

Abstract This paper describes a new algorithm for the adaptive control of a robot manipulator which may contain closed kinematic loops. The algorithm identifies the mass properties of each link and the viscous friction coefficients for each joint of the manipulator. It is similar to the NewtonEuler inverse dynamics algorithm and hence, obtains its computational efficiency through the recursive nature of the algorithm. ii

Contents 1 Introduction 1 2 Preliminaries 2 2.1 The Data Structure.................................. 4 2.2 Manipulator Kinematics........................... 6 2.3 Kinematic Constraint Equations........................... 7 2.4 Mass Parameters........................... 8 3 The Equations of Motion 9 4 Adaptive Controller 11 5 Computational Algorithm 15 6 Conclusion 18 7 Appendix 19 iii

RSD-TR-02-88 1 Introduction The development of computationally efficient inverse dynamics algorithms for manipulators has enabled motion control systems to be designed which theoretically produce motions identical to the desired motions [1,2]. These algorithms are based upon a model which consist of a sequence of rigid bodies interconnected by rotational or translational joints. The mass properties of each link are assumed to be known exactly and nonlinearities such as link flexibility and gearing backlash do not exist. The performance of these controllers is limited to the extent that the actual manipulator exhibits these characteristics. However, even if the manipulator can be modeled perfectly one must still consider the effects of the unknown properties of an object being moved by a manipulator. For this reason and the fact that the model parameters of existing manipulators are not known exactly, several researchers have investigated the application of adaptive control methods. The approach of initial efforts have been to model the manipulator as a linear system and to apply existing adaptive control methods [3,4,5,6]. The major assumption is that good results can be achieved if the model parameters being identified are not changing rapidly. Stability has always been an issue in this approach and no proof has ever been given. Another approach to adaptive control of a manipulator has been to base the control on the full nonlinear model of the manipulator which is used by the inverse dynamics algorithms. The significant advantage of these approaches has been the ability of proving global stability. Craig, et al., was the first to use this approach [7]. Their approach is to drive the manipulator using an inverse dynamics model whose parameters are only estimates of the actual parameters. If the model matches the actual manipulator, the manipulator motion will exactly track the desired motion. Any deviation will be due to errors in the parameter estimates. The parameter estimates are continuously modified based upon the deviation of the manipulator from the desired trajectory. The method of parameter modification is chosen such that stability can be shown using a Lyapunov function. The only drawbacks of this method is the need for an initial guess of the unknown parameters which is near to the actual parameters, the need to invert a large matrix at each iteration of the algorithm, and the need to measure the joint acceleration. Of these three, the need for matrix inversion is the main problem. A more recent extension of these results [8] have shown that the need for matrix inversion and computing the joint accelerations can be eliminated by using a filtered form of the dynamic equations of motion in the derivation of the adaptation law. However, the computational complexity of the method is still relatively high. Another similar approach to adaptive control has been proposed by Slotine and Li [9]. A Lyapunov function is used to prove stability by showing that the output errors converge to a sliding surface, which in turn implies that the tracking errors converge to zero. The advantages over Craigs method is the elimination of the need for a close initial guess of the unknown parameters and the measurement of acceleration. The main problem is in the computational complexity of the algorithm. This is typical of controllers which are based upon the Lagrangian Adaptive Control 1

RSD-TR-02-88 formulation of the manipulator dynamics. In both of these adaptive control methods, there is the practical issue of computational complexity. For this reason, neither have been applied to manipulators with more than two or three degrees of freedom. In addition, both have been formulated for serial link manipulators although many manipulators contain closed kinematic loops. This paper presents an efficient solution to the problem of the adaptive control of a manipulator containing closed kinematic loops and whose mass and friction parameters are unknown. It adopts the basic method of Slotine and Li and addresses the issues of generality and computational efficiency. Generality is obtained by allowing the manipulator to contain closed kinematic loops. Computational efficiency is obtained by basing the control on an efficient inverse dynamics model of the manipulator. The body of the paper begins in the next section with a presentation of the notation used in the paper. In addition, a new method of defining the kinematics of manipulators containing closed kinematic loops is presented. The method is a simple extension of the D-H notation [10] which is commonly used for serial link manipulators and naturally leads to the use of a binary tree data structure for storing the manipulator model. Finally, the form of the kinematic constraint equations which are characteristic of manipulators containing closed kinematic loops is presented. The next section presents a new inverse dynamics algorithm for manipulators containing closed kinematic loops. In this algorithm only inertial and gravitational forces are computed. This improves the computational efficiency over existing methods [11] by eliminating the need for the computation of constraint forces. The next section presents the adaptive controller. The use of the inverse dynamics model of the manipulator in the formulation of the control directly leads to an efficient algorithm for its implementation. Two important concepts which are introduced are spatial feedback vectors and scaler feedback functions. The character of the control is dictated by the definition of these vectors and functions. The next section presents the details of the implementation of the controller and the final section draws some conclusions and suggest some areas for further investigation. 2 Preliminaries This section presents some preliminary material which supports the development of the results in the following sections. First, the data structure used in the controller algorithm is described. Next, the kinematic notation is presented followed by a section on the constraint equations imposed by the closed kinematic loops. Finally, the method used to model the mass distribution of the rigid links of the manipulator is presented. Adaptive Control 2

RSD-TR-02-88 link i coordinates link ^ cordinate.s I, I f 9 Figure 2: An Example Link, Numbered i Adaptive Control 3

RSD-TI-02-88 2.1 The Data Structure In the implementation of the controller, several quantities associated with each link are needed, such as: the link velocity, acceleration, and inertial forces and moments. This section presents the data structure used to store this information. The form of this data structure is important since the design of the control algorithm is directly linked to this data structure. An example manipulator, illustrated in Figure 1, is used as bases for discussion. In general, the number of rigid links in the manipulator is m + 1. For the example in Figure 1, m+ 1 = 8. The base link is numbered 0 and all other links are numbered in an arbitrary order from 1 to m. It is apparent that a graph data structure could be used to store the information. Each record or item in the data structure would be associated with a particular link and all of the information needed about that link would be stored in the associated data record. The problem with using this type of data structure is that it does not lead to a particularly simple or efficient algorithm for the controller. A much better data structure is a binary tree data structure. The remainder of this section develops the method of obtaining this type of data structure. We begin by selecting a set of joints such that if the associated links were disconnected at these joints there would be a unique sequence of links connecting any given link to the base link. An example of two joints which accomplish this are indicated in Figure 1 by the large arrows. The resulting structure is a tree structure and we can now establish relationships between different links in terms of their descendants and predecessors. For example, the descendants of link 1 are links 2 and 3. The predecessors of. The immediate descendant of link 1 is link 2 and the immediate immediatpredecessor of link 2 is link 1. We also note that some links may have more than one immediate descendant. For example, link 0 has three: 1, 6, and 4. The fact that a link could have an arbitrary number of immediate descendants cause practical problems in the implementation of the controller. To get around this problem we introduce the concept of connector links. An example of a link with four joints is illustrated in Figure 2. The joint connecting the immediate predecessor to the link is assigned the same number as the link. One of the other three joints is used to locate the link coordinates. This determines the D-H kinematic parameters as, axi, d,, and Oi as illustrated in the Figure 2. To locate the remaining two joints with respect to link i coordinates we insert fictitious links, j and k, called connector links. They are called connector links because they connect another sequence of links to the tree structure. First is link j which is located relative to link i coordinates. The D-H kinematic parameters [10], aj, aj, dj, and 6j are used to locate this link j coordinates with respect to link i coordinates. Next is link k which is located relative to link j coordinates. Again the D-H kinematic parameters, ak, ah, d4, and Ok are used to locate this link k coordinates with respect to link j coordinates. Note that all of the kinematic parameters, a, a d, and 0, associated with connector links are constant and that for both the connector and successor links the position and orientation of the descendant with respect to the current link is described using homogeneous transforms parameterized by the D-H kinematic parameters a, a, d and 9. The real links are Adaptive Control 4

RSD-TR-02-88 Figure 3: Binary Tree Structure called successor links to distinguish them from the imaginary connector links. Therefore, each link can have up to two immediate descendants. One would be a connector link and the other would be a successor link. Only one of each type is allowed. Hence, with the introduction of the connector links we have converted a general tree data structure into a binary tree data structure. For a given link, the first joint encountered when moving in the direction of the base is numbered the same as the given link. Thus, all of the joints except those where the kinematic loops have been broken have been assigned a number. There are m of these. Assuming there are r independent loops the remaining r joints located at the points where the kinematic loops have been broken are numbered from m + 1 to m + r making a total of m + r joints. Finally, a fictitious successor link called a terminating link is associated with these joints and are given the same number as the associated joint. The purpose of the terminating links are to allocate a item in the data structure to store all of the information concerning the associated joints, for example, their position and the viscous friction coefficients. The D-H kinematic parameters for the terminating links are chosen so that the loop closure equations can be written in terms of homogeneous transforms. This will be described in more detail in the section concerning the constraint equations. If link i is a successor link then the associated joint is either translational or rotational. In either case the joint position is denoted by qi. If the joint is translational then qi = di. If the joint is rotational then qi = 8i. Finally, connector links are numbered starting at m + r + 1 on up to the total number of links, both imaginary and real, contained in the manipulator. Adaptive Control 5

RSD-TR-02-88 The resulting data structure for the example manipulator is illustrated in Figure 3. The convention we use is that the items in the data structure associated with connector links are drawn to the left of its predecessor and the items associated with successor links are drawn to the right. 2.2 Manipulator Kinematics jFrom Figure 3 one can see that the position of each link can be determined with respect to base by starting at the base and successively computing each link position while moving out from the base. Let qi denote the number of the immediate predecessor of link i. If the homogeneous transform of link qi coordinates with respect to link 0 coordinates, T0, is known then the homogeneous transform of link i coordinates with respect to the base is To and can be computing using the following equation. o'- T0*TS where T. is a function of the ai, di, ai, 8i. c(0i) -s(Oi)c(ai) s(0i)s(ai) aic(i) i s(e0i) c(Oi)c(ai) -c(Oi)s(ai) ais(i) 0 S(ai) C) () d 0 0 0 1 where c() and a() stand for the sine and cosine functions. Note that the same equation applies if link i is a connector link or a successor link. This paper uses the spatial notation which was popularized by Featherstone, [12,13]. With this notation transformation matrices are 6 x 6 matrices. The spatial transformation matrix from link i to link /i is: A 0 [ p )K()A A' where 0 is the 3 x 3 null matrix, A and p are the upper left 3 x 3 submatrix and upper right 3 x 1 submatrix of the homogeneous transformation matrix Ti, respectively. The 3 x 3 matrix K() is a skew symmetric matrix such that K(a)b = a x b for any 3 x 1 vectors a and b. As with homogeneous transforms, the spatial transformation from link i coordinates to link 0 coordinates can be computed given that of its immediate predecessor by multiplying the transforms together. xo = x0x^ (1) Similarly, the spatial velocity and acceleration of link i can be determined given those of its Adaptive Control 6

RSD-TR-02-88 immediate predecessor, qi. v_ + si if link i is a successor |v if linki is a connector ( i = { i + 8iq+i + Vi X 8ii if link i is a successor V it. if i iis a connector ( where si is the third column of X^0 if joint i is rotational or the sixth column of X0' if joint i is translational. Thus, one starts at the root of the binary tree data structure and works out along the branches using the above equations to successively compute the spatial transformation matrix, velocity and the acceleration of each link. 2.3 Kinematic Constraint Equations Since, in general, the manipulator is graph structured, there exist constraints in the relative position of each of the joints of the manipulator. These constraints can be represented by a set of equations of the following form: q = U(Q) (4) where Q is an n x 1 vector containing the positions of the joints where the actuators are located. That is, the manipulator has n degrees of freedom corresponding to the n actuators contained in the manipulator and for each Qi there is a qj such that Qi - qj. Given U(Q), Q, and Q, the velocity of all the joints can be determined. q = E(Q)Q (5) where E(Q)= a(Q) (6) The acceleration is given by: q = E(Q)Q + E(Q)Q (7) For a given manipulator these equations are usually fairly simple. Often the matrix E(Q) is constant. For example, it is simply the identity matrix for a serial link manipulator. However, for some manipulators these equations can become complex. For these manipulators we have found that the E-algebra is very convenient for evaluating the time derivatives of the joint positions [14]. Using this algebra one only has to program to solution to Equation 4, change the order of the algebra and automatically obtain the solution to equations 5 and 7. The equations of constraint are obtained from the loop closure equations. For the manipulator shown in Figure 1 there are two independent loops. The two loop closure equations are: Tl(q1)T"3 = T10T 1T (q4)TS(qs)T8(q8) (8) 0 1jL1 oi 1 4\4Jq~5j 5 \8 Adaptive Control 7

RSD-TR-02-88 and T1(q,)T2(q2)Tr12 = T o0T6 (q6)T7(q7)T9(q9) (9) O\Y- 0 1 2' 10 These two matrix equations consist a total of 32 scaler equations of which only six are independent. Thus, we can determine qg, q2, q4, q6, q8, and q9 as a function of the actuator positions q5 and q7. Using the above notation, q5 = Q1, q7 = Q2 and q3 = Q3. These three equations along with the six obtained from equations 8 and 9 give us the function U(Q) defined above. 2.4 Mass Parameters There are two types of parameters of the manipulator model whose values are estimated by the adaptive controller. One is the viscous friction coefficients of the joints and the other is the parameters associated with the distribution of mass in each of the links. There are ten quantities which specify the distribution of mass in each rigid link of the manipulator. These are the six unique components of the moment of inertia matrix, Ji, the center of mass vector times the total mass and the total mass, mi. These are all referred to link i coordinates and combined into a single 6 x 6 spatial moment of inertia matrix, Li. I _ -K() rmiI It is the ten unique components of this matrix we need to estimate for the adaptive control. The moment of inertia matrix, jLaxi 4Wc Jzzi Ji i L= JZi s Yi ALyzi LLzzi Lezyi lszzi 10 the center of mass vector times the mass, i = [2i Zyi R zkil and the mass, mi. For notational purposes the parameters are combined into a single 10 x 1 vector, mi. mi = [M7l M7i2 7i3 mi4 m5 7n6 mi7 miS mis milo]T = [.-xi wyi Izzi lyi Lyzi Izzi rai Syi rzi miJ In this way we can easily write;. as a linear function of these parameters. 10 iz = E Rkmik k=l Adaptive Control 8

RSD-TR-02-88 where the Rk are 6 x 6 matrices with one's where the associated parameter is located in Li and zero's elsewhere. For example, 0 0 0 1 0 0 0 0 0 0 1 0 0 0 0 0 0 1 000010 0 00000 0 0 0 0 0 0 In the development of the dynamic equations of motion in the following section all quantities are referred to the base coordinate system. To refer Ii to the base coordinates one premultiplies and postmultiplies by the link i spatial transformation matrix. Xi Xi, Ii = X iLX O The following is an important property of the spatial moment of inertia matrices. If y is a spatial vector and its derivative with respect to time exist then. d 1/2Tt(y'liy) = y'(Iiy + vi x ly) (10) where vi is the spatial velocity of the i - th link. This is easily shown using the definition of Ii and the fact that dX\o =vi x X dt 3 The Equations of Motion The recursive Newton-Euler form of the equations of motion is used to model the manipulator. This form of the equation is especially useful for solving the inverse dynamics problem. In this problem the actuator torques/forces are computed given the joint positions, velocities and accelerations. The approach used here is similar to that proposed by Luh and Zheng [11] except that the constraint forces are not needed. The method is different in that link inertial forces are used to determine the actuator forces instead of the actual forces on each link. In fact, the approach to modeling is more similar to Kane's [15] formulation than the Newton-Euler formulation. The algorithm is a three step process. 1. Given the desired positions, velocities, and accelerations of the independent variables, Q, calculate the corresponding positions, velocities, and accelerations of the joints, q, using equations, 4 through 7. Adaptive Control 9

RSD-TR-02-88 2. For each link, determine pi, the sum of the friction force and the projection of the sum of the inertial forces plus gravity loading on the axis of motion of joint j. 3. Determine the actuator forces, ji using the constraint equations and the pj computed in step 2. T = E(Q)Tp where T is an n x 1 vector of the actuator forces, and p is an (m + r) x 1 vector of the pj Step 2 is exactly the same procedure which would be used if the manipulator was a serial link or tree structure manipulator. In this case the pi would be identical to the actuator forces. The only difference is in the first step, wherein joint positions, velocities, and accelerations are determined which are consistent with the constraint equations, and the last step, wherein the constraint equations are again used in determining the actuator forces. Step 2 can be implemented in three steps. First, the position, velocity and acceleration of each link is computed using equations 1 through 3. This is done by starting at the base and working out along the branches of the tree. Next, for each link j we determine fj, the sum of the gravitational and inertial forces of link j and all of its descendants. This is done by starting at the tips of the branches of the tree and working back toward the base of the tree using the following equations. Let Fj denote the inertial force on link j. Then: Fj = Mj = Ijij + Vj x IjVj (11) where Mj = Ijij is the spatial momentum vector for the j-th link. Note that Fj for connector and terminating links are zero since they have no mass. Let j be the index of the current link. Let k and I be the index of the successor and connector links. The vector fj is used to denote the sum of the inertial and gravitational forces for link j and all of it's descendants. Then, fj = Fj - Ig + fk + fl (12) where g = [0 0 - 9.8 0 0 0]' is the spatial acceleration due to gravity vector. The last step is to determine pj, the force due to friction, tlj, plus the projection of fj onto the axis of motion of joint j. pj = 8f fj + (13) where r7i is the component of joint force due to friction. This recursive form of the dynamic equations of motion can be shown to be a valid formulation by using the principle of virtual work. We begin by defining some sets which facilitate the development. The set'i is the set of all joint numbers encountered while traversing the tree from link i to the base link. For the example manipulator in figure 1 (4 = {4, 10, 11}. The set Pij = {ilij E h} is the set consisting of i plus all of the indices of link i's descendants. For the example manipulator q1 = {1, 2,3,12,13}. Adaptive Control 10

RSD-TR-02-88 Now, let q be a vector of virtual joint velocities, i.e. any set of joint velocities which are consistent with the constraint equations. The virtual velocity of link i, vii, is written using the above notation as: 1i = E qj (14) jE4i Also, fj= E (F- ig) (15) iE Ij We wish to show that the work done by inertial, gravitational, and friction forces, is equal to the work done by the active forces. That is: m+r n dtj (i(Fi - ig) + qi) = dt E QkTk i=1 k=l Dividing by dt and successively using Equations 14, 30, and 15 on the left side of the above equation gives: m+r m+r Z (i(F- ig)+ g i) ) = [I (,j(Fi - Iig)) + nim] i=1 i=1 jeZi m-+r = E [ ( Pji(F - ig)) + j7j] j=l iE6i m+r = E (qjafj + qpj) j=l m+r m+r n = qjpj= E Qkejkpj j=1 j=1 k=1 n = EZQI.T k=l Therefore, the recursive set of equations given above constitute a valid representation of the equations of motion of a manipulator. 4 Adaptive Controller The basic approach used here is the same as presented by Slotine and Li. The differences in the control law and adaptation law are primarily a result of the use of the Newton-Euler formulation of the manipulator dynamics in the derivation. We begin by defining a class of control laws and then discuss an important property of this class. This class of control laws have the following two characteristics. Adaptive Control 11

RSD-TR-02-88 1. The feedback control law is implemented through the use of a set of spatial feedback control vectors, Fj, and functions, 7j, which are defined for each link of the manipulator. These can be any functions of the desired trajectory and the actual trajectory of the manipulator. 2. The rj are computed using the Fj and 77 as in the following recursive equations: fj = Fj,+k+ fl Pj = 8fj+ j T = E(Q)Tp where k and I are the immediate decendants of link j, p is an (m + r) x 1 vector of the Pj These controllers cover a fairly broad class which include classical PID controllers and inverse dynamic controllers. In this paper we show how the Fj and 7j can be chosen to implement an adaptive controller. The following is an important property of this class of control systems. m+r m+r E (~(F - Ijg) + qjij) = E (vF}j + F j q) (16) j=1 j=1 where the qj are any joint velocities which are consistent with the constraint equations and vj is the resulting link velocity. Basically, this is simply a restatement of the principle of virtual work. If the qj are the same as the actual joint velocities, 4j, then the term on the left is the rate at which energy is absorbed by the manipulator and the term on the right is the rate at which energy is delivered to the arm by the actuators. We now present the spatial feedback vectors and functions which implement the adaptive controller. First a reference trajectory, denoted by Q(t) is defined. It is generated from the desired trajectory and the actual trajectory by solving the following differential equation. Q = d- AQ, where Qd is the preplanned desired trajectory; the tracking error is Q, = Q - Qd; and A is a positive definite diagonal matrix. Rearranging the above equation gives: Q + AQ = Q (17) where Q = Q- Q. iFrom these we compute the corresponding joint velocities and accelerations: q = U(Q) Adaptive Control 12

RSD-TR-02-88 q = E(Q)Q q = E(Q)Q q = q-q q = E(Q)Q +E(Q)Q Note that E is a function of Q and Q. Next we compute the link velocities, vi and accelerations, i using Equations 2 and 3. Then we compute the link velocities and accelerations using q and q as joint velocites. i 0t + saii if link i is a successor V = 0 if link i is a connector i = v*; + siqi + Vi x aiqi if link i is a successor (19) Vi =: (19) ={ toi + +if link i is a connector Vi = vi-0Vi (20) The spatial feedback vectors and functions are: Fj = ijj + j x Ijj - Ijg -j Vj (21) j = dj4j (22) where 7j is a positive scaler and Ij and Ij are the initial and the current estimates of Ij, respectively. The equations for updating the parameter estimates are: mj = -(l/aj)wj (23) dj = -(l/lp) f, (24) where wj is a 10 x 1 vector whose k - th component is, Wjk = j'(Rkvj + Xj x Rkj 9 - Rkgj) (25) and vj, j, j, and gj are the vectors 0i, vj, 0j, and g referred to link j coordinates, respectively. Global convergence of the tracking can be shown by first realizing that Equation 17 is a linear differential equation in Q. and Q and that we have chosen the gain matrix A to be symmetric and positive definite. Thinking of this as a system with Q as the input and Q, as the output we know that this system is bibo stable. Therefore, if we can show that the input, Q, is bounded and Q - 0 as t - oo then we are assured that Q. -- 0 and that global tracking has been realized. To show that Q - 0 as t -- oo we use the following Lyapunov function: m+r V(t) = 1/2: (0' I, j + ahTmjnm + 3j) (26) j=1 Adaptive Control 13

RSD-TR-02-88 where the aj and /j are positive scalers, nj = mj - mj, and dj = d- dj. Since V(t) is bounded from below, by showing that V(t) < 0 we have shown that V(t) -> constant and therefore, V(t) -+ 0. First we consider the derivative of the first term on the right hand side of equation 26. iFrom Equation 10 we get: =' 0(10, + V3 x 1101) 1/2 (vjjj)= vj +v = vo(Mj - Ij(vj x Vj)) where Mj = d(j i) = Ij + Vj x Ijj + Ij(vj x v However, since j = vj - Vj, then Mj = Mj-Mj = Fj - (Ij,j + vj x Ijj + Ij(vj x vj)) where j = d(Ijp) Thereodt Therefore, m+r d E 1/2dt(.J ) j=1 and since vj = vj + Vj then, m+r = Z'j(Fj -(Ijj + 3 X j x I )) j=l m+r d E 1/2dt('Iji0j) j=1 m+r = E'j(Fj - (Ijj + vj x Ijvj)) j=l Using Equation 16 gives: m+r d E 1/2d(tIjij) = j=l m+r j= ( - (Ifj + 0 x 1j1 + Ijg)) j=l m+r + E qj(j - rij) j=1 Substituting in the equations for Fj, r/j and i, gives: m+r d E 1/2dt (I) j=1 m+r -= E (Iv + vj x ivj - Ig) j=l Adaptive Control 14

RSD-TR-02-88 m+r m+r - E jj3Irj3 + Z djjj j=l j=l m+r m+r = E miTwj - E - t j i j=1 j=1 m+r + ZE jqjj (27) j=i The derivative of the second term on the right hand of Equation 26 is: m+r d m+r m+r E 1/2aj-?(Tj) = E im-m a_- Mj j=1 j=1 j=l m+r _' "mT j (28) j=1 The derivative of the third term on the right hand of Equation 26 is: m+r d m+r m+r 1/2 E = E jdjj = - Z djq-j (29) j=1 j=l j= Therefore, adding Equations 27, 28, and 29 gives: m+r V(t) = - 7E3Zj'I~j < 0 j=1 jFrom the above, we now know that V(t) -* 0. It is easy to choose the J such that Ij > 0. Therefore, Q -+ 0 and hence, global stability of the tracking is assured. That is, Q -- 0. 5 Computational Algorithm The purpose of this section is to present the exact steps used in the implementation of the adaptive control algorithm. The controller is organized so that changing the control algorithm only involves the changing a two procedures within the controller program. As in most inverse dynamics type algorithms computational efficiency is improved by computing all velocities and accelerations referred to their own link coordinates. This change of coordinates will be denoted by underlining the variable in what follows. For example, Mi = (Xo)ti Adaptive Control 15

RSD-TR-02-88 The spatial vector si is referred to link qi coordinates since they are constant in that coordinate system. That is, i= (Xio)Yi Another method used to speed up the computations is to add the acceleration due to gravity to the base coordinate acceleration at the beginning of the computation. This avoids having to explicitly compute t the gravity loading for each as is implied by the previous equations. The following three procedures traverse, forward, and backward are used in the implementation of the controller. The procedure traverse implements a traversal of the binary tree data structure where all of the computed quantities are stored. This is a basic procedure which is used in the laboratory regardless of the particular control algorithm being implemented. The input to the procedure is a link number and the output is li which is equal to fi referred to link Xi coordinates. This ordering of the traversal of the tree directly corresponds to the ordering of the computations implied by the above equations. Each time a new node in the tree is visited on the trip down through the data structure the procedure forward is called. This procedure is used to update any state variable information associated with the control law. The input to this procedure is the current link number and there is no output returned from the procedure. Only data local to this procedure and the backward procedure is changed. On the trip back up through the data structure procedure traverse calls the procedure backward. The input to this procedure is the current link number and the output is the spatial feedback vector, F, and the scaler feedback function, 4i for the current link. The procedure traverse is initially called with i = 0, the number of the base link. PROCEDURE traverse(ili) begin 1. If i = O then set q = U(Q) q = E(Q)Q no = 0 else if link i is a successor link set _i = (x.)l(_. + ~,~) else if link i is a connector link set ~i = (xi.)' 2. Call forward(i). 3. If a connector link for link i exist and its number is k then call traverse(k,lk), else set lk = O. Adaptive Control 16

RSD-TR-02-88 4. If a successor link for link i exist and its number is j then call traverse(j,ij), else set ij =0. 5. Call backward(i I,i,). 6. Set: L = -+ik+ij ii = X Pi = ili+ri 7. If i = O then set: and output r to the manipulator. end; PROCEDURE forward(i) begin Tr =E(Q)Tp 1. If i = 0 then set Q Q q q q eo else iflik i is a s essr link seto else if link i is a successor link set = Qd-AQe = Qd-AQe = E(Q)Q = E(Q)Q + (Q)Q - q-q =o = =-9=0 = —g ii = (x=)' ( +_, i) i = (X )'( + x aig + a.gi) i= 2 = ~i-~. and compute wi using Equation 25, else if link i is a connector link set wi = (X^)' (x= i = (X,)' Si = ~i -i Adaptive Control 17

RSD-TR-02-88 end; PROCEDURE backward(itj,iT) begin 1. If link i is a successor link then set: Fi = iii+iiXLJ-7iJ?~t Li = di~ mi = -l/aiu i di = -I/Piq4i and update the parameter estimates, else if link i is a terminating link then set: Fi = 0 di = -1/pi qgqi and update the parameter estimates, else set i, = 0, i- = 0. end; 6 Conclusion An efficient algorithm for the adaptive control of a manipulator containing closed kinematic loops has been presented. The issues that have been addressed are computational efficiency and generality. Generality was obtained by formulating the algorithm for manipulators containing closed kinematic loops. Computational efficiency was obtained with the use of a new inverse dynamics model for manipulators with closed kinematic loops. This model achieves improved computational efficiency by eliminating the need to compute the constraint forces. Only inertial and gravitational forces are actually computed by the algorithm. The algorithm used in the implementation of the adaptive controller directly follows from the binary data structure used in modeling the manipulator. Three procedures were presented for its implementation. The first is the procedure traverse which is used in the traversal of the binary tree data structure. Each node in the data structure corresponds to a different link in the manipulator. As each node is visited the controller calls a second procedure called forward. This procedure is used to update any state information associated with the control law for the particular node being visited. On return steps of the traversal algorithm the controller Adaptive Control 18

RSD-TR-02-88 calls a third procedure called backward. The function of this procedure is to compute the spatial feedback vectors and functions which define the control law being implemented. After traversing the entire tree the computed torques are outputted to the manipulator to effect the control. It is difficult to quantify the computational complexity of the algorithm since a general closed form solution to the constraint equations does not exist. However, it is clear that for a serial link manipulator the number of computations is linear in the number links contained in the manipulator. Manipulator controllers should be designed to perform a variety of different classes of compliant motion task. An example of a class could be opening door. Every door is basically the same. Only the size, friction, and mass properties change. It might seem that the adaptive controller presented in this paper could be applied to the compliant motion control problem. However, the model used by this controller is special in that the number of degrees of freedom of the manipulator is the same as the number of actuators. This fact allows us to simplify the model and the formulation of the controller. However, there is little difference between the models presented in this paper and the model for a manipulator preforming a compliant motion task. The main difference is the number of degrees of freedom of the combined manipulator and task are now less than the number of actuators. Thus, we have an infinite number of choices of actuator forces which produce the same motion of the end-effector. As in the hybrid compliant motion control scheme one would need to control both force and position. In addition, compensating for modeling errors in the task kinematics is likely to play an important role in future adaptive compliant motion controllers. 7 Appendix The following identity is used: For any function, f(i, j) of the indices i and j: m+r m+r E E f(ij)= E f(ij) (30) i=1 jE'i j=l iE~$ References [1] J. Y. S. Luh, M. W. Walker, and R. P. Paul, "On-Line Computational Scheme for Mechanical Manipulators", Trans. AME Journal of Dynamic Systems, Measurement, and Control, vol. 12(2), pp. 69-76, 1980. [2] D.E. Orin, R.B. McGhee, M. Vukobratovic, and G. Hartoch, "Kinematic and Kinetic Analysis of Open-Chain Linkages Utilizing Newton-Euler Methods", Mathematical Biosciences, vol. 43 No. 1/2, February 1979. [3] A. J. Koivo and T. Guo, "Adaptive Linear Controller for Robotic Manipulators", IEEE Trans. on Auto. Contr., vol. AC-28, 1983. Adaptive Control 19

RSD-TR-02-88 [4] A. J. Koivo, Force-position-velocity control with self-tuning for robotic manipulators, In I.E.E.E. Int. Conf. Robotics and Automation, San Francisco, 1986. [5] G. Leininger, "Self-tuning Control of Manipulators", Int'l Symp. on Advanced Software in Robotics, 1983. [6] S. Dubowsky and D. T. Desforges, "The Application of Model-Reference Adaptive Control to Robotic Manipulators", J. Dyn. Syst. Meas. Contr., vol. 101, 1979. [7] J. J. Craig, P. Hsu, and S. S. Sastry, Adaptive control of mechanical manipulators, In IEEE Int'l Conf. on Robotics and Automation, San Francisco, CA, 1986. [8] P. Hsu, M. Bodson, S. Sastry, and B. Paden, Adaptive identification and control for manipulators without using joint accelerations, In IEEE Int'l Conf. on Robotics and Automation, Raleigh, North Carolina, 1987. [9] J. E. Slotine and W. Li, "On the Adaptive Control of Robot Manipulators", International Journal of Robotics Research, vol. 6(3), 1987. [10] J. Denavit and R. Hartenberg, "A Kinematic Notation for Lower-Pair Mechanisms Based on Matrices", AME Journal of Applied Mechanics,, pp. 215-221, June 1955. [11] J. Y. S. Luh and Y. F. Zheng, "Computation of Input Generalized Forces for Robotics with Closed Kinematic Chain Mechanisms", IEEE J. of Robotics and Automation, vol. 4, pp. 95-103, 1985. [12] R. Featherstone, "The Calculation of Robot Dynamics Using Articulated-Body Inertias", The Int. J. of Robotics Res., vol. 2, no. 1, pp. 13-30, Spring 1983. [13] R. Lathrop, Constrained (closed-loop) robot simulation by local constraint propagation, In I.E.E.E. International Conference on Robotics and Automation, San Francisco, CA, 1986. [14] M. W. Walker, Manipulator kinematics and the epsilon algebra, In I.E.E.E. International Conference on Robotics and Automation, Raleigh, North Carolina, 1987. [15] T.R. Kane and D.. A. Levinson, "The use of Kane's dynamic equations in robotics", The Int. J. of Robotics Res., vol. 2, no. 3, pp. 3-21, 1983. Adaptive Control 20