Feedback Stabilization and Tracking of Constrained Robots N. Harris McClamroch Department of Aerospace Engineering Department of Electrical Engineering and Computer Science The University of Michigan Ann Arbor, Michigan 48109 and Danwei Wang Program in Computer, Information and Control Engineering The University of Michigan Ann Arbor, Michigan 48109 September, 1986 Abstract Mathematical models for constrained robot dynamics, incorporating the effects of contact forces required to maintain satisfaction of the constraints, are used to develop explicit conditions for stabilization and tracking using feedback. The control structure allows feedback of displacements, velocities and the contact forces. Global conditions for tracking based on a modified computed torque controller and local conditions for feedback stabilization using a linear controller are presented. The framework is also used to investigate the closed loop properties if there are force disturbances, dynamics in the contact force feedback loops or uncertainty in the constraint functions. This work was partially supported by the Center for Research on Integrated Manufacturing (CRIM) at the University of Michigon.

RSD-TR-26586 TABLE OF CONTENTS 1. Introduction and Motivation...................................................................1 2. Formulation of Constrained Dynamic Equations.................................... 3 3. Transformation of Constrained Dynamic Equations............................... 5 4. Global Tracking using Nonlinear Feedback............................................ 8 5. Local Stabilization using Linear Feedback.............................................. 11 6. Consequences of Model Imperfections..................................................... 14 7. An Example....................................................... 20 8. Conclusions........................................ 23 9. References....................................................... 25

1. Introduction and Motivation Current industrial robots are characterized by a wide diversity of physical design configurations. However, the basic task capabilities of most current robots are quite limited. The most common tasks involve so-called "pick and place" operations, which characterize the vast majority of current robot applications. If robot technology is to have a more wide ranging impact on industrial practice it is essential that the task capabilities of robots be substantially expanded. A robot can be viewed as a physical mechanism for performing work; the mechanism is often defined as a connection of articulated links constructed so that the end of the last link (which may include a gripper holding an object or a tool holder containing a tool) is the location at which the work is performed. It is natural to focus on the so-called end effector of the robot and to define particular robot tasks in terms of desired motions of the end effector; this is the common view in dealing with "pick and place" operations. However, there are many industrial tasks (most of which cannot be automated using current robots) that are defined in a fundamentally different way. In particular, there are numerous tasks which cannot be defined solely in terms of motion of the end effector. Of specific interest in this paper are tasks which are characterized by physical contact between the end effector and a constraint surface. A long list of such tasks can be given, including scribing, writing, deburring, grinding and others [ 1 -7]. There have been numerous research publications which have dealt with such applications. Although the primary focus of such research has not often been on the role of constraints in defining the tasks, research on compliant control [8- 111 and force feedback control [1i2- 191 are closely -1

related. Several formal control design approaches have been proposed and there have been descriptions of related robot experiments. However, there has been no theoretical framework established which can serve as a basis for the study of robot performance of constrained tasks. It is the premise of this paper that there is a need for a carefully developed theoretical framework for investigation into application of robots to such tasks. Furthermore, it is our premise that such a theoretical framework should explicitly incorporate the effects of contact forces required to maintain satisfaction of the constraints. Specifically, we present a dynamic model of a robot, incorporating constraint effects; the form of the model follows from classical results in dynamics [20-211 and has recently been recognized as the proper theoretical model for constrained robot problems [22-27]. This model is used in this paper to develop theoretical conditions for closed loop stabilization and tracking using a class of feedback controllers. The constraints are assumed to be nonlinear and the development is based on a canonical coordinate transformation for which the constraints are expressed in a simple form. Global tracking conditions are developed for the case of nonlinear dynamics using a modification of the computed torque method. Local stabilization conditions are also developed using a linear controller. The results are specialized to the case of linear constraints, for simplicity. Such stabilization conditions are new and serve to provide a theoretical basis for the use of force feedback in constrained systems such as has been suggested in [ 6]. We further investigate the properties of the closed loop using the proposed controller structure; we show that "high gain displacement feedback loops" reduce the steady state displacement regulation error for -2

constant force disturbances and uncertainty in the constraint function. We also show that "high gain contact force feedback loops" reduce the steady state contact force regulation error for constant force disturbances and uncertainty in the constraint function. Further, closed loop stabilization is shown not to be affected by a certain class of dynamics in the contact force feedback loops. 2. Formulation of Constrained Dynamic Equations Our development is based on a Lagrangian formulation of robot dynamics where we assume the existence of a symmetric and positive definite matrix valued inertia function M: Rn -- Rnxn a scalar valued potential function V: Rn -- R1 and a vector valued constraint function 0: Rn - Rm such that the equations of motion of the robot are defined in terms of the Lagrangian function L(q,q) = 0.5 i' M(q) qI - V(q) and the constraint function 0(q) as d a L(q,) a L(q,l) = J'(q) + u (1) dt aqC aq O(q) = 0 (2) For simplicity in our development, we do not take into account kinematic relations between robot coordinates and constraint coordinates; thus q is a vector of generalized displacements, q is their time derivative, \ is a vector of generalized contact forces (multipliers) associated with the constraints and u is a generalized force which depends on a control input, -3

disturbance inputs or dissipative effects to be included in the model. For simplicity throughout all functions are assumed to be sufficiently differentiable and J(q) = a 0(a) (3) aq Thus the equations of motion for the constrained robot are given by M(q) ql + F(q,l) = J'(q) A + u (4) O(q) = 0 (5) where F(q,l) = d [M(q)] l _ 0.5 a b' M(a) a + V(q) (6) dt <q aq Note that if O(q) = 0 is identically satisfied then also J(q) q O0. Thus, the constraint manifold S in R2n defined by S = ((qPl): O(q) = 0, J(q) 1 = 0) (7) is fundamental in the development. In particular, if conditions given in [25-26] are satisfied, then there is a unique solution of (4)-(5), denoted by q(t) satisfying (q(t),l(t)) c S, for each (q(O),ql(O)) e S. That is, S is an invariant manifold. Thus the constraints on the robot can be viewed as restricting the dynamics to the manifold S only rather than to the space R2n. It is this distinction that is critical in forming the constrained robot dynamics; this is also the fundamental source of difficulty in analysis and control of the robot dynamics. We again emphasize the important role of the constraints in the constrained dynamics, especially as relates to the stabilization problem. -4

In particular, it is easy to see that a closed loop system that is asymptotically stable if constraints are ignored may, in fact, not be asymptotically stable if the constraints are imposed. To make this point clear, a simple example is provided by the linear system l+ + < +2+2x1 =0 X2 + X + 2 + X2 =0 which is asymptotically stable, but if the constraint xl + x2 = 0 is imposed it can be shown that the resulting constrained linear system xl + l + x2 + 2 x1 = X2+ + X X22+ X2 x +X2 =0 is not asymptotically stable; its responses are oscillatory. 3. Transformation of Constrained Dynamic Equations In order to carry out our subsequent development we assume that the constraint function satisfies Assumption i. There is an open set V Rn-m and a function 2 ~ V - IR such that 0((q2), q2) - O for a// q2 e (8) Let O(q) = 0; assumption 1 holds in some neighborhood of q if rank J(q) = m, according to the implicit function theorem, although a reordering of the variables may be required. -5

Let Assumption 1 hold with V = Rn-m. Consider the nonlinear canonical transformation X: Rn -- Rn defined by Xl qlq - 0(q2) x = = X(q) = (9) x2 q2 which is differentiable and has a differentiable inverse transformation Q: Rn -- Rn such that q xli + 0(x2) q= = O(x) = (1 O0) The differential equations (4), when expressed in terms of the variables x are easily obtained. In particular, let Im a O(x2) ax2 T(x) = a Q(x) = (1) n-m be the nonsingular Jacoblan of the Inverse transformation. By abuse of notation we often write T(x2) or T(q) in place of T(x). Thus the differential equation (4) is transformed to T'(x) M(Q(x)) T(x) x + T'(x) {F(Q(x),T(x)k) + M(Q(x)) T(x) x) = T'(x) u + T'(x) J'(Q(x)) N (12) and the constraint equation (5) is transformed to x1 = 0. (13) -6

For simplicity in the notation define M(x) = T'(x) M(Q(x))T(x) (14) F(x,x) = T'(x) (F(Q(x),T(x)x) + M(Q(x)) T(x) dx} (15) and introduce the partitioning of the identity matrix In= [E' 1E'2] (16) where El is an mxn matrix and E2 is an (n-m)xn matrix. The equations (12) and (13) can be written in reduced form as El M(x2) E'2 x2 + El F(x2,X2) = E1T'(x2) u + E T'(x2) J'(x2) h (17) E2 M(x2) E'2 X2 + E2 F(x2,12) = E2 T'(x2) u (18) x -=0 (19) where we note that in (18), E2 T'(x2) J'(x2) = 0 follows from Assumption 1. The notation F(x2,x2) denotes F(x,k) evaluated at x = (O,x'2), = (O,5(2), etc. An important special case occurs if the constraints are linear; it can then be shown that the above development can be carried out explicitly in terms of a singular value decomposition. We make Assumption 2 Assume that (q) - J q where J is a constant mxn matrix with rank of Jbe ig m. Then it follows that there is a singular value decomposition of J such that J - U 2 V' (20) where U and V are mxm and nxn orthogonal matrices respectively, and 2 = [D 0], D = diag [d,...,dm] (21) with di > O, i = I,...,m being the singular values of J. -7

Now the previous development holds with Q(x) = Vx, X(q) = V'q, and T(x) = V. (22) The development is considerably simplified in this case; consequently specific results are subsequently presented for the case of linear constraints. 4. Global Tracking using Nonlinear Feedback In this section, we consider a general tracking problem where vector functions qd:R1 - Rn, d:R1 - Rm, are given such that q = qd(t) satisfies constraint equation (5) identically. Our objective is to determine a nonlinear feedback controller to solve the following tracking problem. A feedback control u (depending on the tracking functions qd' d) id Ad and the feedback functions q, q and M) is to be selected so that for all (q(O),i(O)) e S it follows that the closed loop responses satisfy q(t) - qd(t) as t e o N(t) -N d(t) as t -e For simplicity in the presentation we consider a version of the computed torque controller [12,28], modified to accomodate the use of feedback of the contact force. Based on equations (1 7)-(19) a modification of the computed torque controller, which takes into account the constraints and the tracking contact force objective, is characterized by -8

T(x2) u M(x2) E'2 2d + F(x2, x2) T'(x2) J'(x2) Nd + M(x2) E'2 Gv (X2d - x2) + M(X2) E'2 Gd (X2d - X2) + E'1 Gf E1 T (x2) J'(x2) (N - Nd) (23) where Gv and Gd are (n-m)x(n-m) constant feedback gain matrices and Gf is an mxm constant feedback gain matrix. The closed loop equations are E1 M(x2)E'2{e2+ + Gv e2 +Gde2 = (I + Gf)El T'(X2) J'(x2) (N - Nd) (24) E2 M(x2) E'2 (2 + Gv 2 + Gd e2 = (25) el = 0 (26) where e2 = x2 - E2 X(qd), el = X1. Conditions on the gain matrices so that the tracking problem is solved are readily obtained from equations (24)-(26). First, we write the controller equation (23) in terms of the original coordinates. The controller expression is given by u = M(q) T(q) T(qd)-1 q + F(q, (q) (q) + M(q) {T(q) T(q)- Ilq - T(q)d)- T(qd) T(qd)- ld} + M(q) T(q) E'2 G- E2 (T(qd) d -T(q)-1 q} + M(q) T(q) E'2 Gd E2 (X(qd) - X(q)) + T'(q)- I E'1 Gf E T'(q) J'(q) (N - Nd) (27) The following result is obtained. -9

Theorem I. Suppose that Assumption i is satisfied with V - R7-m The closed loop system defined by eqcations (4)-(5/) and (27) is globall# asymptotically stable in the sense that q(t - qd (t) as t -+ A t)- Ad(tas t -V for any (q(O), q(O)) e S if G6and 6d are symmetric andopositive definite an& Gf is symmetric and nonnegative definite. We now consider the simpler case where the constraints are linear. In such case the feedback controller (27) can be written as u = M(q) qd + F(q,q) - J' d + M(q) V E'2 GE2 V' (d ) + M(q) V E'2 Gd E2 V' (qd - q) + V E'1 Gt E V' J'(? - Nd) (28) where Gv and Gd are (n-m)x(n-m) constant feedback gain matrices and Gf is an mxm constant feedback gain matrix. Conditions so that the closed loop system solves the tracking problem are obtained as follows. Corollary 2 Suppose that Assumoption 2 is satisfied The closed loop system defrWed by eMuwtions (4)-(5) and (28) is globally symptoticall stable in the sense that q(t) - qdft as t - A t( AYt) as t - ~v for any (q(O), q(O)) c S if Gv and Gdare symmetric and positive definite and Gf is symmetric andnonnegative definite. It is important to note that the dependence of the controller on the contact force in expressions (27) and (28) is crucial; in particular, the -10

tracking contact force term -J'(q) Ad cannot be replaced by the feedback contact force term -J'(q) A because the closed loop system in such case is ill-posed. Such an incorrect approach has, in fact, been proposed in the literature. One proper form for introducing feedback of the contact force is through the controller expressions given by equations (27) or (28). Note that the closed loop can be stabilized even if Gf = O; feedback of the contact force is not required for stabilization. But, as we subsequently indicate, there are advantages in using feedback of the contact force. The control relations (27) and (28) suggest that feedback of q and q is required, but it should be kept in mind that those relations can be expressed in terms of feedback of q2 and q2 only, using the constraints. A schematic diagram of the closed loop system, indicating the control structure (28), is shown in Figure 1, for the case where the constraints are linear. The viewpoint of the controller as a computed torque controller, modified to conform to the constraints, and depending on feedback of q, q and A is clarified from that figure. 5. Local Stabilization using Linear Feedback In this section, we consider a regulation problem where constant vectors qd e Rn, Ad e Rm, ud e Rr are given such that q = qd, A = Ad, u = ud satisfy equations (4-5) thereby defining an equilibrium condition. Our objective Is to determine a feedback controller to solve the following regulation problem. For simplicity in the presentation we consider a linear controller. A linear feedback control u (depending on the regulation vectors -11

Ud, qd, d', and the feedback functions q, q and A) is to be selected so that there is a neighborhood N of (qd,O) in R2n such that for all (q(O),q(O)) e SA\N it follows that the closed loop responses satisfy q(t) -, qd as t -, A(t) A d as t —, Assuming that the constraint function is nonlinear, consider the linear feedback controller, based on the reduced equations (17)-(19), given by u = ud + T'(x2d)-l{ - E'2 Gv x2 + E'2 Gd E2 (x2d -x2) + E' Gf El T'(X2d)) J'(x2d) (N - Nd) } (29) where Gv and Gd are (n-m)x(n-m) constant feedback gain matrices and Gf is an mxm constant feedback gain matrix. The linearized closed loop equations are E1 IM(X2d) E' 2 + EI K(X2d) E'2 e2 = (I + Gf) E T'(2d) J'(x2d) (A - A d) (30) E2 M(x2d) E'2 2 + G e2 + Gd + E2K(x2d) E'2 )e2 = 0 (31) el = 0 (32) where K(x2d) = a F(x.)) a x X=(O,X2d), X = (0,0) The conditions on the gain matrices so that the regulation problem is solved are readily obtained from equations (30)-(32). First, we write the controller equation (29) in terms of the original coordinates. The controller expression is given by -12

u = ud - T'(qd) - E'2 Gy E2 T(qd)-f1 + T'(qd)) 1 E'2 Gd E2 T(qd)- (qd - q) + T(qd)- E Gf E I T(qd) J'(q) (A - N) (33) The following result is obtained. Theorem 3 Suppose that Assumption / is satisfied in some neighborhood f qd.Te closed loop system defined by equations (42-(5) and (29) is locally asymptotically stable in the sense that there is a neighborhood N of (qd, O) such that q( t)- qd as t - for any (q(O), (0)) e 5A / N f GVand Gdare symmetric andpositive definIte such that a// 2(n-m) zeros of de t {2 r'(qd ) / M(qd s2 + K(q )/ r(qd) EJ f G s Gd./ have negative real parts and G6 is symmetric and nonnegative definite where K(qd) T'(q -) {d T(q) F(q, J / dq =q~ q=O We now consider the simpler case where the constraints are linear. In such case the linear feedback controller (33) can be written as u d - V E2GvE2V' + VE'2GdE2 V'(qd q) +V E'1 Gf E I V J' ( - )(34) where Gv and Gd are (n-m)x(n-m) constant feedback gain matrices and Gf is an mxm constant feedback gain matrix. Conditions so that the closed loop -13

system solves the regulation problem are obtained as follows. Corollary 4 Suppose that Assumption 2 is satisfied The closed IooF, system defined by equations (4)-(5) and (74) Is locally asymptotica/Ip stab/le in the sense that there is a neighborhoodN of (q, 0) such that q(t)~ qd a5st,.t.) -- d as t -~ for any (q(O), (O)) S 5 N if Gv and Gd are symmetric and positive definite such that all 2(n-m) zeros of det [E2 V'[M(qfd.) +'qd ) l /E' +Gy s +.)I. ha& MegatiVe re&a3 artq and St is symmetric andnonlregative definite 31nd d q I q=qd q=O Again we mention that the control relations (33) and (34) have been written in terms of feedback of q and & but those relations can also be expressed in terms of feedback of q2 and q2 only using the constraints. A schematic diagram of the closed loop system, indicating the control structure (34), is shown in Figure 2, for the case where the constraints are linear. The controller consists of a constant bias input plus linear terms proportional to position error, velocity error and contact force error, chosen consistent with the constraints. 6. Consequences of Model Imperfections We have developed conditions which indicate how feedback controllers can be developed so that the closed loop is asymptotically -14

stable. In this case stability is defined as robustness to changes in the initial data that are consistent with satisfaction of the imposed constraints. As has been demonstrated, feedback can be used to improve the closed loop properties with respect to such uncertainties. But feedback can be expected to play a further important role in possibly reducing the effects of disturbances and model imperfections. In this section we briefly consider the implications of using feedback in the case that there are external force disturbances, additional dynamics in the contact force feedback loops, and uncertainties in the constraints. To avoid unnecessary complications, our developments are based on the linearized equations, assuming that the constraints are linear, i.e. that Assumption 2 holds. Thus our conclusions are approximations only valid near an equilibrium. Nevertheless, the qualitative features of the conclusions are of substantial importance as they suggest the general implications of feedback. Our conclusions about the effects of model uncertainties are limited; but the indicated linearized equations could form the basis for a more detailed study. Effects of Force Disturbances. Suppose there is an external force disturbance so that the constrained system is described by M(q) q + F(q,{I) = u + J' A + f (35) Jq=O (36) where f represents the n-vector force disturbance. Recall that q = qd A = Ad, u = ud define a constrained equilibrium, corresponding to f = 0. Assume that the controller is given by (34). Then, following the development indicated previously, the linearized equations for the closed -15

loop defined by (35)-(36) and (34) are given by E1 IM(x2d) E'2 e2 + E K(x2d) E'2 e2 (I + Gf) E V' J' ( - d) + El V'f (37) E2 M(x2d) E'2 e2 + Gv e2 + [ Gd + E2 K(x2d) E'2 )e2 =E2 V' f (38) e1 =O. (39) The effects of the force disturbance, at least locally, are characterized by equations (37)-(39). Suppose that the feedback gain matrices satisfy the conditions of Corollary 4. If f is a constant disturbance then there are steady state position error and contact force error such that, at least locally, the closed loop responses satisfy q - qd V E'2 [Gd + E2 V' K(qd) V E'2] E2 V' f as t (40) A - Nd - (E1V' J')- [I M + Gf]-1 ([E V K(qd) V E'2 [Gd +E2V' K(qd)VE'2] 1 E2-E) V'f ast —-. (41) Thus, the steady state displacement error does not depend on the force feedback gain matrix Gf and is inversely proportional to the displacement feedback gain matrix Gd. The steady state contact force error does depend on the force feedback gain matrix Gf and Is Inversely proportional to it. Thus, "high gain" in the displacement feedback loops results in improved -16

steady state displacement accuracy for additive force disturbances. And "high gain" in the force feedback loops results in improved steady state contact force accuracy for additive force disturbances. Effects of Dynamics in Force Feedback Loops. Suppose that there are dynamics in the force feedback loops such as might be due to force sensor dynamics. We make a simple assumption about the nature of these dynamics so results are easily obtained; we do not examine the effects of dynamics in the displacement and velocity feedback loops although that might also be of importance. Recall that q = qd, = Ad, u = ud define a constrained equilibrium if there are no sensor dynamics. Assume that the controller is given by u= Ud -VE'2GvE2V'i +VE2GdE2V'(qd -q) + V E'l Gfz z=-iz + El V'J'((A-A d) (42) where z represents the m vector state of the force feedback loops. This assumes first order feedback dynamics as a consequence of measurement of the vector force E1V'J' A normal to the constraints. For simplicity we take ji as a positive scalar. Then, following the development Indicated previously, the linearized equations for the closed loop given by equations (4)-(5) and (42) are given by -17

E M(x2d) E'e + E K(X2d) E' e2 = G z + E V J' ( - A d (43) E2 M(X2d) E2 e2 + Gv e2 + Gd + E2 K(X2d) E'2 e2 = 0 (44) z=-z+pEl V'J'(N-xd) (45) e =O. (46) The effects of the dynamics in the force feedback loops, at least locally, are characterized by equations (43)-(46). Suppose that the feedback gain matrices satisfy the conditions of Corollary 4. It is easy to show that if j > 0 equations (43)-(46) are locally asymptotically stable in the sense that for initial data near the equilibrium q(t)4qd ast oo \(t) Ad as t That is, the dynamics in the force feedback loops do not destabilize the closed loop system so long as the feedback dynamics are of the assumed simple form and the gains satisfy the conditions of Corollary 4. Note that the assumption that the force feedback gain Gf is nonnegative definite is critical here. Effects of Constraint Uncertainties. Suppose that there are uncertainties in the constraint function; it is of interest to determine the effect of such uncertainties on the previous results. Although there are various -18

assumptions that could be made, for simplicity we assume that the constraint is linear and given by Jq=A (47) where A represents the constant m vector of constraint uncertainty. Recall that q = qd, A = d, u = ud define a constrained equilibrium, corresponding to A = 0. Assume that the controller Is given by (34). Then, following the development indicated previously, the linearized equations for the closed loop defined by equations (4), (34), (47) are given by El M(X2d) E'2 e2 + E K(X2d) E2 e2 =(I + f)E V J' ( d - E1K(x2d) E (EVVJ' A (48) E2 M(x2d) E'2 e2 + Gv e2 + Gd + E2 K(x2d) E'2 e2 = - E2 K(x2d) E' 1 (E1 V J')-1 (49) el = (E1 V'J')-. (50) The effects of the constraint uncertainty, at least locally, are characterized by equations (48)-(50). Suppose that the feedback gain matrices satisfy the conditions of Corollary 4. Then there are steady state position error and contact force -19

error such that, at least locally, the closed loop responses satisfy q -qd - V E'2 [Gd +E2 V K(qd) V E'2]E2V'K(qd) V E' (EI VJ')-I as t -4 (51) A - Ad -V (E1 V' J)-1 [ + Gf]- E I V' K(qd) V E'2 [Gd + E V' K(qd) V E'2 V']- 1 E2 V' K(qd)V E' I - E V'K(qd)V E', (E1 V'J')A ast-.oo (52) Again we see that the steady state displacement error does not depend on the force feedback gain matrix Gf and is inversely proportional to the displacement feedback gain matrix Gd. The steady state contact force error does depend on the force feedback gain matrix Gf and is Inversely proportional to It. Thus, "high gain" In the displacement feedback loops results in improved steady state displacement accuracy for uncertainty in the constraint function. And "high gain" in the force feedback loops results in improved steady state contact force accuracy for uncertainty in the constraint function. 7. An Example Consider a simple example of a planar Cartesian manipulator constrained so that the end effector follows an elliptic arc. We take the equations of motion to be given by x= =ul +2 a2 x (53) = u2 + 2 y \ (54) -20

with scalar constraint function given by a2 2 + y2 1 =O. (55) where a2 > 0. The control objective is to choose a feedback controller so that the closed loop stably tracks the given functions Xd, Yd' hd which are assumed to satisfy the constraint equation (55), with xd> O. Our approach is based on the developments in Sections 3, 4, and 5. In the previous notation, let q' = (x,y) and let x'= (xl,x2) be defined by x = X(q) x - a- 1 -y2)-1/2 Y (56) Thus T(x) = I a l -y (_y2)-1/2 0 1 (57) The open loop nonlinear equations, corresponding to equations (17)-(19), are - a x2 ( 1-x22)- 1/2 x2 - a1 ( 122)-3/2 2 = U1 + 2 a (1-x22)-1/2 N (58) [+ a-1 (1-x22)-1 x22 x2 + a-2 (1-x22)-2 x2x22 - a1 (1-x2)-1/2 x2u1 + u2 (59) Following equation (23), the nonlinear controller is given by -21

U1 a 2 (1-x 2) X2d a' (1-x22)-3 x22 -2 a ( -x22)1/2 Ad - a-1x2 ( -x22) [ gv(2d 2) + gd(x2d - x2) ] + 9f 2 a ( -x22) 1/2 (A - Xd) (60) U2 X= 2d -2 X2 Ad + gv(x2d - x2) + gd(X2d - X2) + gf 2 X2 (N - Nd) (61) so that the closed loop equations, corresponding to equations (24)-(26), are - a-1x2 (1-x22)-1/2 [ e2 + gv e2 + gd e2] = (1 + gf) 2a(-x22 )12 ( - ) (62) e2 + e2 + gd e2 = 0 (63) e1 = 0. (64) Thus if gv > 0, gd > 0, gf > O, it follows that, globally, x - xd, y -Yd as t- N -- Nd as t- c. Also, the linear feedback controller given by the general form (29) is ul = -2 a (l-x2d2)1/2 d + gf 2 a (I-x2d2)1/2 (N - d) (65) U2 = -2 X2d Ad -v X2 + gd(X2d - X2) + 2 gf X2d (A - Ad) (66) The resulting linearized closed loop equations are given by -a 1X2d -x2d2) 1/2 e2-( +gf)2a( -x2d2) 1 (h- d) (67) [1 + a-I (1-X2d2)- 1x2d2] e2 + gv e2 + d e2 (68) e = O. (69) -22

Thus, if g, > O gd > 0, gf > O, it follows that, locally, x -Xd, y -e Yd as t, N -, Nd as t -4. The control formulas given here have been written in terms of feedback of y, y and N only. Equivalent control formulas can be written in terms of feedback of x, x and X only, or complete feedback of x, xj, y, y and N. The effects of model imperfections could be examined for this example, as described in Section 6. The computations are direct and hence are omitted here. 8. Conclusions Conditions for stabilization of a closed loop constrained robot have been developed using mathematical models which explicitly include the constraint functions. Global stabilization conditions have been developed using a nonlinear controller which is based on a modification of the computed torque method. Local stabilization conditions have also been developed using a linear controller. We have also investigated the properties of closed loop systems using the proposed controller structure; we have shown that "high gain displacement feedback loops" reduce the steady state displacememt regulation error for constant force disturbances and uncertainty in the constraint function. We also have shown that "high gain contact force feedback loops" reduce the steady state contact force regulation error for constant force disturbances and uncertainty in the constraint function. -23

Further, closed loop stabilization has been shown not to be affected by a certain class of dynamics in the contact force feedback loops. The suggested approach to investigation of applications of robots to tasks defined by constraints is new. We have shown that, although the underlying mathematical issues are complicated, a formal mathematical approach is tractable. Our emphasis here has been on feedback stabilization. But, as indicated in [23,25] we believe that this approach provides a theoretical basis for the Investigation of a variety of problems that involve the use of force feedback in constrained robot systems. -24

9. References [1] Asada, H. and N. Goldfine, "Optimal Compliance Design for Grinding Robot Tool Holders," Proceedings of IEEE Conference on Robotics and Automation, St. Louis, 1985, 316-322. [2] Asada, H. and N. Goldfine, "Compliance Analysis and Tool Holder Design for Grinding with Robots," Proceedings of American Control Conference, Boston, 1985, 65-68. [3] Gustafsson, L., "Deburring with Industrial Robots," SME Deburring and Surface Conditioning Conference, 1983. [4] Hogan, N. and S. R. Moore, "Part Referenced Manipulation-A Strategy Applied to Robotic Drilling," Proceedings of ASME Winter Annual Meeting, Boston, 1983, 183-191. [5] Plant, G. and G. Hirzinger, "Controlling a Robot's Motion Speed by a Force-Torque Sensor for Deburring Problems," Proceedings of IFAC/IFIP Symposium on Information Control Problems in Manufacturing Technology, 1982, 97-102. [6] Stepien, T. M., et. al., "Control of Tool/Workplece Contact Force with Application to Robotic Deburring," Proceedings of IEEE Conference on Robotics and Automation, St. Louis, 1985. [7] Starr, G. P., "Edge Following with a Puma 560 Manipulator using VAL-II," Proceedings of IEEE Conference on Robotics and Automation, San Francisco, 1986, 379-383. [81 Hogan, N., "Impedance Control: An Approach to Manipulation: Part I-Theory; Part II-lmplementation; Part Ill-Applications," ASME J. Dynamic Systems, Measurement and Control, 107, 1985. [9] Kazerooni, H. et. al., "Robust Compliant Motion for Manipulators; Part I: The Fundamental Concepts of Compliant Motion; Part ii: Design Methods," Proceedings of IEEE Journal of Robotics and Automation, Vol. RA-2, June, 1986, 83-92. -25

[10] Paul, R. P. and B. Shimano, "Compliance and Control," Proceedings of Joint Automatic Control Conference, 1976, 694-699. [11] Salisbury, J. K., "Active Stiffness Control of a Manipulator in Cartesian Coordinates," Proceedings of IEEE Conference on Decision and Control, Albuquerque, 1980, 95-100. [12] Craig, J. J., Introduction to Robotics: Mechanics and Control, Addison-Wesley, 1986. [131 Hirzinger, G., "Force Feedback Problems in Robotics," lASTED Symposium on Modelling, Identification and Control, Davos, 1982. [14] Hirzinger, G., "Direct Digital Robot Control using a Force Torque Sensor," Proceedings of IFAC Symposium on Real Time Digital Control Applications, 1983, 243-255. [151 Mason, M. T., "Compliance and Force Control for Computer-Controlled Manipulators," IEEE Trans. on Systems, Man and Cybernetics, Vol. SMC- 11, 1981. [161 Raibert, M. H. and J. J. Craig, "Hybrid Position/Force Control of Manipulators," ASME J. of Dynamic Systems, Measurement and Control, 102, 1981, 126-133. [17] West, H. and H. Asada, "A Method for the Design of Hybrid Position/Force Controllers for Manipulators Constrained by Contact with the Environment," Proceedings of IEEE Conference on Robotics and Automation, St. Louis, 1985. [181 Whitney, D., "Force Feedback Control of Manipulator Fine Motions," ASME J. Dynamic Systems, Measurement and Control, 1971, 91-97. [19] Whitney, D. E., "Historical Perspective and State of the Art in Robot Force Control," Proceedings of IEEE Conference on Robotics and Automation, St. Louis, 1985, 262-268. [20] Arnold, V. I., Mathematical Methods of Classical Mechanics," Springer-Verlag, 1978. -26

[21] Goldstein, H., Classical Mechanics, Addison-Wesley, 1950. [22] Hemami, H. and B. F. Wyman, "Modelling and Control of Constrained Dynamic Systems with Application to Biped Locomotion in the Frontal Plane," IEEE Trans. on Automatic Control, Vol. AC-24, 1979, 526-535 [23] Huang, H. P., "Constrained Manipulators and Contact Force Control of Contour Following Problems," Ph.D. dissertation, Department of Electrical Engineering and Computer Science, The University of Michigan, Ann Arbor, July, 1986. [241 Kankaanranta, R. and H. N. Koivo, "A Model for Constrained Motion of a Serial Link Manipulator," Proceedings of IEEE Conference on Robotics and Automation, San Francisco, 1986. [251 McClamroch, N. H., "Singular Systems of Differential Equations as Dynamic Models for Constrained Robot Systems," Proceedings of IEEE Conference on Robotics and Automation, San Francisco, 1986. [261 McClamroch, N. H. and H. P. Huang, "Dynamics of a Closed Chain Manipulator," Proceedings of American Control Conference, Boston, 1985. [271 Yoshikawa, T., "Dynamic Hybrid Position/Force Control of Robot Manipulators-Description of Hand Constraints and Calculation of Joint Driving Forces," Proceedings of IEEE Conference on Robotics and Automation, San Francisco, 1986, 1393-1398. [28] Luh, J. Y. S., "Conventional Controller Design for Industrial Robots - a Tutorial," IEEE Trans. on Systems, Man and Cybernetics, Vol. SMC- 11, 1983, 298-316. -27

UNIVERSITY OF MICHIGAN 3 9015 03465 8644 4 qd | Y~I~+)E2,SiVEE' i| E V 4 4'id - >1 mH -4 RobD! J', Figure 1: Closed Loop with Modifie d Computed Torque Controller dU Ud Robot t VE'l G~ El V' |' 5,g;,V i., Figure 2: Closed Loop with Linear Controller -28