Force/Motion Control of Constrained Manipulators Without Velocity ...

Force/Motion Control of Constrained Manipulators Without Velocity Measurements Antonio Loria

Elena Panteley

Center for Control Engineering and Computation (CCEC) Department of Electrical and Computer Engineering University of California Santa Barbara, CA 93106-9560, USA [email protected] [email protected]

Abstract

In this note we deal with the problem of force/tracking control of constrained manipulators without velocity measurements interacting with an in nitely sti environment. We use a reduced order model for which the coordinates space is restricted to a subset where the constraints Jacobian is guaranteed to be non singular. In contrast to other similar approaches, we do not assume that the generalized trajectories belong to the subset all the time thus, our contribution is to prove that if the trajectories start within the subset , they remain in it. Furthermore, we prove uniform asymptotic stability considering only position and force measurements. Keywords: Output feedback, tracking control, holonomic constraints.

1 Introduction Starting with [22, 13, 10] the interaction with an in nitely sti environment is modeled by holonomic (algebraic) constraints imposed to the manipulator's motion. This kind of motion is often referred to in the literature as constrained robot motion. Unfortunately, the constraints equation is singular, hence in order to cope with this diculty, various techniques for deriving so-called reduced order models have been proposed in the literature based on the projection of the dynamic robot equations onto a submanifold described by the algebraic equation of constraints. Concerning the force/position control, several schemes have been proposed for stabilization of constrained manipulators [13, 5, 15]; the position/force regulation problem was considered in [20], where the Lyapunov's direct method was used to guarantee local stability. Nevertheless, the above cited solutions for constrained manipulators present two major disadvantages: the rst is the need of velocity measurements which are often contaminated with noise [3]. The second is that it is assumed that the constraints equation is globally solvable. It is supposed either that the constraint Jacobian is non singular in the whole state space ([13, 4, 21, 5, 8]) or that the actual trajectory is contained in the neighbourhood of the desired set-point ([20, 1]). However, it should be noted that these assumptions are rather restrictive, for instance the rst may be not ful lled even in very simple cases as that of a planar two-link revolute-joint manipulator whose endpoint is constrained to a plane. To the best of our knowledge the force/position control problem without velocity measurements was rst treated by [9] and [16]; Huang and Tseng studied the (open loop) observer design for 

This work was supported in part by the INTAS France-Russia collaboration project and by CONACyT, Mexico.

1

constrained robots. Later in [16] the authors proposed a nonlinear observer and proved for the rst time, asymptotic stability of the closed loop system considering an in nitely sti environment. Some other results in this direction are [12, 8, 6]. In [12] we proposed the rst force/position controller without velocity measurements and with uncertain gravity knowledge considering an elastic environment. Under the assumption that the constraints Jacobian never degenerates, the authors of [8] proposed a \semi-global" adaptive force/motion controller for manipulators under holonomic constraints. In this paper we consider that the end-tool is interacting with an in nitely sti environment. We use the reduced order model introduced in [15] for which the coordinates space is restricted to a subset where the constraints Jacobian is guaranteed to be non singular. In contrast to the other similar approaches mentioned above, we do not assume that the generalized trajectories belong to the subset all the time thus, our contribution is to prove that if the trajectories start within the subset , they remain in it. Furthermore, we prove uniform asymptotic stability considering only position and force measurements using standard Lyapunov stability theory. Notation. In this paper we use k
k for the Euclidean norm of vectors and the induced L2 norm of matrices. The largest and smallest eigenvalues of a matrix K are denoted by kM and km respectively, where K and its eigenvalues may have a subindex.

2 Dynamic model 2.1 Preliminaries

We consider in this note the standard model of a rigid revolute joint robot manipulator

D(q)q + C (q; q_)q_ + g(q) = u + f

(1)

where D(q) = D>(q) > 0 is the robot inertia matrix, g(q) is the gravitational forces vector, C (q; q_)q_ represents the Coriolis and centrifugal forces, u 2 IRn are the applied torques and f 2 IRn is the reaction forces vector. In order to introduce our notation, we recall that there exist some positive constants kc , dm and dM such that 8 q 2 IRn , dm I  D(q)  dM I and kC (q; q_)k  kc kq_k. We assume that the manipulator's end-eector interacts with an in nitely sti environment hence, that its motion is constrained to a smooth (n ? m){dimensional submanifold , de ned by

(q) = 0

(2)

where the function  : IRn ! IRm is at least twice continuously dierentiable, and m is the number of holonomic constraints. Assumption A1 below, concerns the solvability of the constraints equation (2) and will be used in the sequel in order to derive a reduced order model of constrained motion.

A1 We assume that there exists an operating region  IRn de ned as =4 1  2, where 1 is a convex subset of IRn?m , 2 is an open subset of IRm . We also assume the existence of a function k : 1 ! IRm twice continuously dierentiable, such that (q1 ; k(q1 )) = 0 for all q1 2 1 . Under these conditions, the vector q2 can be uniquely de ned by the vector q1 such that q2 = k(q1 ) for all q1 2 1 .

Remark 1 (Jacobian structure) Notice that under this assumption the matrix J (q) =

4 4 @(q)=@q can be partitioned as J (q) = [J1 (q); J2 (q)], where J1 (q) = @(q)=@q1 , J2 (q) = @(q)=@q2 m  m and the Jacobian matrix J2 (q) 2 IR never degenerates in the set . Necessary and sucient

conditions for global solvability have been given in [16] laying on results of Sandberg [19]. 2

Thus, without loss of generality we can assume that for all q 2 there exist positive constants 1 ; 2 ; 3 such that, for all i = 1; 2; : : : ; n, 0 < 1  kJ2 (q)k  kJ (q)k  2 (3)

@J (q )

< :

@qi

3

Remark 2 1 In words, Assumption A1 guarantees the solvability of (2) only in the set , which is equivalent to assume that the Jacobian J2 (q) is not singular only if q 2 . This is the main dierence with other similar reduced order models used in the literature where it is supposed that J2 (q) is full rank in the entire state space. Considering that the end-eector motion is constrained to the submanifold , the above de nition of the set allows a parameterization of the generalized coordinates vector in which only n ? m independent coordinates need to be controlled [13, 15, 14]. Thus, without loss of generality we can choose q1 and q2 as independent and dependent coordinates respectively and we can express the generalized velocities vector q_ in terms of the independent velocities as where

q_ = H (q)q_1 

(4) 

In?m H (q) = (5) ? ? J2 1(q)J1 (q) : Similarly to the parameterization of the vector q_, the generalized reaction forces f which do not deliver power on admissible velocities i.e., q_>f = 0, can be parameterized by the vector of Lagrange multipliers  2 IRm as f = J >(q): (6)

2.2 Reduced order model and its properties

In this section we present the reduced order model introduced in [15] and we stress some properties which are fundamental for further analysis. First, using equations (4) { (6) in (1) we obtain D (q)q1 +? C (q; q_)q_1 + g (q) =
H >(q)u  = Z (q) C(q; q_)q_1 + g(q) ? u where we have de ned 4 > D (q) = H (q)D(q)H (q) (7) 4 C(q; q_) = D(q)H_ (q) + C (q; q_)H (q) 4 > C (q; q_) = H (q)C (q; q_) 4 > g (q) = H (q)g(q)  ?1 4 Z (q) = J (q)D?1 (q)J >(q) J (q)D?1 (q) Next, similarly to [13] we introduce a decoupled control scheme which allows to control generalized positions and constraint forces separately, thus consider the control input u of the form u = H + >(q)ua ? J >(q)ub

Notice that the inequality kJ2 (q)k  kJ (q)k in (3) is not obvious. Interested readers are referred to [7], ch. X, sec. 6,7. 1

3

where ua 2 IRn?m ; ub 2 IRm and H + (q) = H >(q)H (q) ?1 H >(q) which under Assumption A1, exists and is bounded for all q 2 . Then the decoupled reduced order dynamic model for robot manipulators under holonomic constraints can be rewritten in the form ([15]) D (q)q1 +C (q; q_)q_1 + g (q) = ua (8)  (9)  = Z (q) C (q; q_)q_1 + g(q) ? H + >(q)ua + ub ?




Equation (8) de nes the dynamics of the system in the position direction i.e., the dynamics of the independent coordinates, which are to be controlled. Equation (9) de nes the dynamics in the force direction i.e., of the force exerted by the manipulator's end-tool when interacting with its environment. Due to the decoupled nature of this reduced order model we are able to design two separate position and force control laws ua and ub respectively.

2.2.1 Properties P1 For all q 2 IRn, the matrix D(q) = D>(q)  dm I > 0. Moreover, for all q 2 there exists 0 < dM < 1 such that D (q)  dM I . Moreover for a suitable choice of C (q; q_) we have that for all q 2 IRn the matrix D_  (q) ? 2C (q; q_) is skew-symmetric. It is worth remarking the existence of dM < 1 only for all q 2 . This stems from the assumption that the Jacobian J2 (q) is nonsingular only in this subset. See (5) and (7).

P2 For all q 2 the norm of the matrix H (q) satis es the inequality 1  kH (q)k  1 + 2 = 1 moreover there exist a constant 4 such that

kH + (q)k  4

P3 For all x 2 IRn?m the matrix C(q; q_1 )x satis es the following C (q; H (q)q_1 )x = C(q; H (q)x)q_1 moreover for all q 2 there exists a kc > 0 such that kC (q; H (q)q_1 )k  kc kq_1k

For a proof of Property P1 see [17]; for a proof of P2 and P3 see [11].

3 Problem statement and its solution 3.1 Motivation

In the previous section we have shown that the dynamic model in the position direction, that is, eq. (8) possesses similar properties than the model of unconstrained manipulators provided Assumption A1 is satis ed. Hence, algorithms used to control unconstrained manipulators can also be applied in presence of holonomic constraints, provided one can ensure that the trajectories never leave the set where contact is possible and the reduced order model is well de ned. In order to overcome this diculty, most of reduced order models considered in the literature rely on the (implicit or explicit) assumption that, either = IRn or that the manipulator remains in a neighbourhood of the desired trajectory during the whole transient. However, it must be noted that assuming that

= IRn is restrictive and unrealistic. In order to illustrate this idea, and the type of result we seek for, consider the simple example of the planar 2-link manipulator shown in Figure 1. 4

q2 111111111111111111 000000000000000000 l1 11111111 00000000 000000000000000000 111111111111111111 00000000 11111111 l2 000000000000000000 111111111111111111 q1 00000000 11111111 000000000000000000 111111111111111111 000000000011 11111111

Figure 1: Two-link planar constrained manipulator For this manipulator the constraints equation is given by l1 sin(q1 ) ? l2 sin(q2 ? q1 ) = 0: (10) Let us assume that l2 < l1 and that the manipulator cannot go into the surface, in this case it is easy to see that contact of the end-tool with the surface is only possible if either 

or



 l 2 q1 2 IR : 0 < q1 < arcsin l 1 







q1 2 IR :  ? arcsin ll2 < q1 <  : 1

(11) (12)

This clearly shows that the set cannot coincide with IR2 . On the other hand, in order to satisfy Assumption A1, must be de ned in a way such that the dependent coordinate be uniquely de ned as a twice continuously dierentiable function of the independent coordinate. For thisexample let  4 us choose q1 to be the independent coordinate; then from (10) q2 = k(q1 ) = q1 +arcsin ll21 sin(q1) . Notice that k(q1 ) is C 2 only if (11) or (12) hold thus, for the reduced order model (8) { (9) to be well de ned, 1 must be chosen according to (11) or (12) depending on the initial condition q1 (t0 ). To strengthen the justi cation of this choice notice further that for this example, (with q1 as independent coordinate) ! 1 H (q) = l1 cos(q1 ) + 1 ; cos(q2 ?q1 )

 

then from the de nition of k(q1 ) it follows that q_ = H (q)q_1 grows unboundedly if q1 =  ?arcsin ll12   or q1 = arcsin ll21 , hence the reduced order model (8) { (9) is not valid at these points. For these reasons, before exploring the stability properties of the closed loop system of the constrained manipulator (1) { (2), with a control algorithm commonly used for unconstrained manipulators, one should be concerned by the permanence of the solutions in the set . This simple example illustrates our motivation to consider the following important problem.

3.2 Problem statement

4 De ne I = [0; 1), assume that only position and force measurements are available, then design a smooth control law u = u(t; q; f ) such that  q(t) 2 8 t  t0; to 2 I 4  tlim q ~ ( t ) = lim [q(t) ? qd (t)] = 0; !1 t!1  lim f~(t) =4 lim [f (t) ? fd(t)] = 0; t!1

t!1

where fd (t) and qd (t) stand for the desired values of force and trajectory respectively, which are supposed to satisfy the following restrictions: 5

A2 The desired trajectory qd(t) 2 C 2 and maxt2I fkqd (t)k; kq_d (t)k; kqd(t)kg  Bd. A3 The desired trajectory qd (t) 2 for all t  0. Furthermore, it belongs to the interior of (i.e. 

) with some distance from the boundary of the set (i.e. @ ) inf kqd1 (t) ? qb1 k > ; 1 qb 2@ 1

(13)

where  is a positive constant whose choice may depend on the initial conditions and the robot parameters.

A4 For all desired trajectories qd(t) 2 the constraint equation (qd (t)) = 0 is satis ed. Furthermore, the desired contact force satis es fd (t) = J > (qd (t))d (t) for all t  0.

3.3 Main result

Proposition 3 Consider the model (8) { (9) in closed loop with the control law ua = D (q)qd1 + C (q; q_d )q_d1 + g (q) ? kpq~1 ? Kd # ub = Z (q)H +> (q)D (q)qd1 + d ? kf ~  q_c = ?A(qc + B q~1) # = (qc + B q~1 )

(14) (15) (16)

4 4 where A, B , and Kd are diagonal positive de nite, kp ; kf > 0 and ~ =  ? d . De ne x(t) = col[~q1 (t); q~_ 1 (t); #(t)], then there always exist some constants 0 < 5 < 4 <  with  as in A3, such that for any initial condition (t0 ; x0 ) 2 (I  x) where o 4 n 1 _1

x = q~ ; q~ ; # 2 IRn?m : kq~1 k; kq~_ 1 k; k#k < 5

(17)

the independent coordinates q1 (t) remain in the set 1 for all t  t0 . Moreover there always exist matrices A and B suciently large to ensure that the closed loop system (8) { (9), (14){ (16) is uniformly asymptotically stable.  It is important to remark that it is not necessary to assume that the end-tool is always in contact with the surface. Putting aside the problem of impact, we can consider the problem of not loosing contact under the assumption that initially, the manipulator interacted with its environment. This can be easily proven as done in [21] by showing that the manipulator always exerts some force against its environment hence, assuming that constraints are unilateral and that the manipulator can only push the surface, we need to prove that fi > 0 for all i = 1; : : : ; m. From equation (33) and the boundedness of q and q_, there exists kZ > 0 such that kf~k = kf ? fdk  kZk ++kf1d k : f Now, let kld be a constant such that fdi > kld for all i  m. Notice that one can always choose kf such that kf~k < kld . Then we obtain that fi  ?kf~k + fdi > ?kld + kld that is, fi > 0 as required. After the original submission of this paper, we have noticed an increasing interest on the problem of control of manipulators during their complete task: free-motion, impact, constrained motion. Even though it is beyond the scope of this paper to deal in full detail with this problem, it is worth pointing out the importance of guaranteeing that the trajectories does not escape the domain where the Jacobian's rank does not degenerate and henceforth where the reduced order model is valid. 6

To be more precise, let us recall the stability criterion established in [2] for the problem of the robot control problem during its complete task. In that reference the authors divide the time span of the complete task into successive intervals corresponding to the constrained, unconstrained and impact phases: IR+ = 0 [ I0 [ 1 [ 2 [


where the 's are transition phases and the I 's, impact phases. Based on this assumption the authors introduce the concept of weak stability which roughly speaking, says that a system is weakly stable if it is stable for all t  0, t 2 . In that reference new tools to prove weak stability are given based on Lyapunov like techniques. In this paper we show using simple Lyapunov stability theorems how one can guarantee that the trajectories do not escape a set (where the Jacobian does not degenerate) based on the basic assumption that the initial conditions are contained by and that the desired reference does not lay \too close" to its boundaries. Even though in [2] the authors consider intervals of time while we use subsets of coordinates, it is our belief that there is a strong relation between these concepts. Thus, our methodology of proof may contribute to open a new way for proving weak stability in the sense of [2]. Besides the fact that, looking at the constrained motion problem as part of the complete task problem, it is crucial to ensure that the model remains valid for all time in some interval (or equivalently for all q1(t) 2 1 ).

4 Proof of main result

It is worth recalling that under Assumption A1 it holds that the Jacobian of the constraints equation is full rank only if q1 (t) 2 1 8 t  t0 . However we do not suppose that this happens to hold for all t > t0 , then we must prove that under the conditions of Proposition 1, Assumptions A1 { A4 and for all initial conditions x0(t0 ) =4 col[~q1(t0 ); q~_ 1 (t0 ); #(t0 )] 2 x, the solution x(t) is always bounded, moreover we prove that q1 (t) 2 1 8 t  t0 . This constitutes the beginning and most important part of the proof while the second one concerns the uniform asymptotic stability of the error system.

4.1 Boundedness of solutions

A fundamental tool to prove our main result is the following lemma borrowed from [18]

Lemma 4 Let P1 , P2 be compact sets in B  IRn such that 



f0g  P 1  P1  P 2  P2  B  IRn  where P denotes the interior of P , and B is the closure of B . Let V : B  I 7! IR for all (x; t) 7! V (x; t) be a function of class C 1 and let a be a constant such that

(a) (8 t 2 I ) and (8 x 2 @P2 ), V (x; t)  a (b) (8 t 2 I ) and (8 x 2 @P1 ), V (x; t) < a  (c) (8 t 2 I ) and (8 x 2P2 ?P1 ), V_ (x; t)  0. If t0 2 I and x0 2 P1 , then the solution x(t; t0 ; x0 ) of the dierential equation x_ = f (t; x), where f (t; x) is continuous and locally Lipschitzian, is de ned and belongs to P2 for all t  t0 .  7

In order to apply the Lemma 4 we rst de ne 4 B = fx 2 IR3(n?m) : kq~1k < ; q_1 ; # 2 IRn?mg

next, consider the function >

V (x; t) = 21 q~_ 1 D q~_ 1 + k2p q~1 2 + 12 #>Kd B ?1 # + "q~1> q~_ 1 ? "#>q~_ 1 (18) where " is a small positive constant to be de ned. Using Property P1 and Assumption A3 it is easy to see that V (x; t) is a smooth function in (B  I ). Now, let us de ne 4 P1 =

4 P2 =

n

o

n

o

q~1 ; q~_ 1 ; # 2 IRn?m : kq~1 k; kq~_ 1 k; k#k  5

q~1 ; q~_ 1 ; # 2 IRn?m : kq~1 k; kq~_ 1 k; k#k  4

(19)

where 5 and 4 as in Proposition 1. Next, we prove that that the function (18) meets all conditions of Lemma 4. Condition (a). Consider the auxiliary function 4 1 _ 1> _ 1 kp

1

2 1 > ?1 W1 (x; t) = 4 q~ D q~ + 4 q~ + 4 # Kd B #; 4 then it can be easily shown that W2 (x; t) = V (x; t) ? W1 (x; t) is positive de nite if (  1=2 ) d k 1 m d  m 1 = 2 " < p min (kp dm ) ; bM 8

(20)

then we have that V (@P2  I )  W1 (@P2  I ), which in its turn implies that V (@P2  I )  sup W1 (x; t) = 41 (dm + kp + kdm =bM )24 t 2 I; x 2 @P2

then the rst condition holds with a  41 (dm + kp + kdm =bM )24 . Condition (b). First notice that, since x 2 @P1 then kq~1 k  5 and since qd1(t) 2 1 by hypothesis, it follows that q1 (t) is bounded. Now, let B5 ;qd1 be the ball centered at qd1 (t) and radius 5 , if (21) inf kqd1 (t) ? qb1 k > 5 1 qb 2@ 1

then, B5 ;qd1  1 . Since  > 5 , the latter happens to hold observing A3. Moreover, since kq1 (t) ? qd1 (t)k  5 it follows that q1(t) 2 B5 ;qd1 , hence q1 (t) 2 1. This implies that for all x 2 @P1 , there exists a dM such that 1 > dM > kD k in accordance with Property P1. Thus we can write for all (x; t) 2 (@P1  I ) 







V (x; t)  21 [" + kp ] kq~1 k2 + " + 21 dM kq~_ 1 k2 + 21 " + kbdM k#k2 m hence





V (@P1  I )  21 dM + kp + kbdM + 4" 25 m 8

(22) (23)

it then follows that both conditions (a) and (b) are met if s

4 > 5 2(dMd ++kpk++kdkM =b=bm + 4") m p dm M

(24)

D (q)q~1 + [C (q; q_) + C (q; q_d )]q~_ 1 + kpq~1 + Kd # = 0 #_ = ?A# + B q~_ 1

(25)

Condition (c). Consider the rst error equation, i.e. (8) and (14), using Property P3 we are

able to write

Now, using Properties P1 { P3 and Assumptions A2 { A4, we will prove that the time derivative of (18) along the trajectories of (25) is negative

de nite for all (x; t) 2 (P2  I ). First notice that under Assumption A3, if x 2 P2 then q~1  4 , and hence q1 2 1 . Thus, after some straightforward calculations one obtains that the time derivative of (18), is bounded for all (x; t) 2 (P2  I ), as 

T z

"

z #T 

1 V_ (x; t)  ? 12 kkq~#kk 1 ?" kkqq~~_ 1kk "

_1

? kkq~#kk z

#T z

1

}|

Q1 }|

"kp =dM ?"(kp + kdM )=dm ?"(kp + kdM )=dm 2kdm am =3bM Q2 }|

kp=2dM ?kc Bd=dm ?kcBd=dm bm =3

{ "

Q3

kq~1 k kq~_ 1k

{ 

kq~1k k#k

#

}|

"bm =3 ?"(kc Bd =dm + aM =2) ?"(kc Bd =dm + aM =2) kdm am =3bM {

2

zh "

}|



{

{ "

kq~_ 1k k#k

i ? kd3mb am ? " kddM k#k2 ? 3 bm ? kcBd ? 2"4 kc=dm ? " kq~_ 1k2 : M m Now Q1 , Q3 and 1 are positive de nite if   2d2m kp kdm am 4d2m kdm am bm a d k m m d  m " < min 3d b (k + k )2 ; 9b (2k B + a d )2 ; 3k b M M p M c d M m dM M dM while Q2 is positive de nite if 2 2 kp bm > 6kcdB2 d dM

and 2 > 0 if bm > 6 and

m

">

bm 6

kc Bd : 2kc 4

?

dm

#

(26) (27) (28) (29)

Conditions (20) and (27) hold for a suciently small " > 0, condition (28) holds for suciently large bm while the last condition imposes a lower bound on " which can be made arbitrarily small by increasing bm . As a matter of fact, it can be proven [11] that for suciently large A = aI and B = bI satisfying 4 121 4 kc b= (30) d with

m

 2 k d c m  Bd dM  1 > max  k d ;  k ; 1 4 p m 4 c 

9

(31)

V_ (x; t) is negative de nite for all (x; t) 2 (P2  I ). It follows that condition (c) of Lemma 4 holds as well, then kq~1 (t)k  4 for all t  t0 . Since 4 < , under Assumption A3 we conclude that q1(t) 2 1 for all t  t0, moreover from A1 it follows that q(t) 2 for all t  t0 .

4.2 Asymptotic stability

The proof of asymptotic stability follows directly from the results of previous section using the following theorem of [18] Theorem 5 Let V : P2  I 7! IR be a function of class C 1 and a Lyapunov function on P2  I . If (a) V (x; t) ! 0 when x ! 0 uniformly for t 2 I (b) V_ (x; t) is negative de nite on P2  I (c) V (x; t)  c for all (x; t) 2 P2  I , and c 2 IR then all solutions x(t; t0 ; x0 ) such that x(t; t0 ; x0 ) 2 P2 for every t  t0 tend to 0 as t ! 1 uniformly in t0 , x0 .  We proceed to prove that V (x; t) de ned by (18) meets all the conditions of Theorem 5. The rst condition follows straightforward under Assumptions A2 { A4 about the desired trajectory, while the second condition was already shown in the previous section. The third condition follows observing that, since q1 (t) 2 1 for all t  t0 we are able to write using (22)   V (P2  I )  V (@P2  I )  21 dM + kp + kbdM + 4" 24 (32) m In the previous section we proved that the solutions x(t; t0 ; x0 ) 2 P2 for all t  t0 thus, using Theorem 5, we conclude that the error system in the position direction, i.e. (25), is uniformly asymptotically stable. Now, looking at the second error equation i.e., in the force direction, we have using P3 that n o (kf + 1)~ = Z (q) [C (q; q_) + C (q; q_d )]q~_ 1 + H +> [kp q~1 + Kd #] : (33) The proof is completed by noticing that since q1 2 1 , then Z (q) is bounded. Moreover the right hand side terms of (33) tend uniformly asymptotically to zero, then it follows that ~ (t) ! 0, hence f~(t) ! 0 for all t  t0 and for all to 2 I . 

5 Simulation results For the sake of illustration, we have performed some brief simulations on a two-link planar robot like that of Figure 1. We have considered that l1 = 2m, l2 = 1m. The masses of the links are m1 = 5, m2 = 2:5, while the momenta of inertia about their centres of mass are I1 = 6:33, and I2 = 0:83. For both links, it was considered that the centres of mass are at the middle of the links. We have considered that the initial con guration of the manipulator is similar to that shown in Figure 1, hence 1 := fq1 2 IR : 0 < q1 < =6 ? 0:01g and we have chosen q1 to be the independent coordinate. The control gains were set to kp = 50000, Kd = diagf[20000; 20000]g, A = diagf[3000; 3000]g, and B = diagf[1000; 1000]g. The desired trajectory is q1d = 18 (1 + cos(6t)), while the desired force was set to 2N and f (t0 ) = 2:7N. In the rst part of Figure 2 we show the resulting trajectory q1 (t) and its reference, q1d (t). Then, we show the resulting force tracking error, f~. Notice that the trajectory is bounded between -0.1 and =6 ? 0:01, hence that q1(t) 2 1, as it was expected. 10

Actual and desired trajectory

Actual and desired force 3

0.3 force [N]

position [rad]

0.4 0.2 0.1 0

2.5

2

−0.1 −0.2 0

1

2

1.5 0

3

time [s]

1

2

3

time [s]

Figure 2: Actual and reference independent trajectories q1 (t) and q1d (t) and force values.

6 Concluding remarks We have studied the problem of output feedback tracking control of manipulators under holonomic constraints. We used a reduced order model whose particular feature, and advantage in relation to other models available in the literature, is that the generalized coordinates space is constrained to a smooth manifold where it can be guaranteed that the constraints Jacobian is regular. We proposed a controller which uses the output of a linear lter instead of the velocities vector, hence only position measurements are needed. We proved that the proposed control law keeps the independent trajectories bounded in the set where the solvability of the constraints equation is guaranteed. Furthermore, we proved local uniform asymptotic stability in the set in both, the force and the tracking directions, using the direct Lyapunov's method. We believe that our results and proof methodology may be extended and contribute to the more general problem of robot control during the complete task: free motion, impact, constrained motion.

References [1] S. Arimoto, Y.H. Liu, and T. Naniwa. Principle of orthogonalization for hybrid control of robot arms. In Proc. 12th. IFAC World Congress, Sydney, Australia, 1993. [2] B. Brogliato and S. Niculescu and M. Monteiro-Marques. On tracking control of rigid manipulators with unilateral constraints. In Proc. 4th. European Contr. Conf., Brussels, Belgium, 1997. Paper no. 872. [3] P. Belanger. Estimation of angular velocity and acceleration from shaft encoder measurements. In Proc. IEEE Conf. Robotics Automat., volume 1, pages 585{592, Nice, France, 1992. [4] A. Caiti and G. Cannata. Hybrid position/force control of constrained manipulators in presence of uncertainties. In Proc. 4th. Symposium on Robot Control, pages 647{652, Capri, Italy, 1994. [5] R. Carelli and R. Kelly. An adaptive impedance/force controller for robot manipulators. IEEE Trans. Automat. Contr., 36:967{971, 1991. [6] M. S. de Queiroz, J. Hu, D. Dawson, T. Burg, and S. Donepudi. Adaptive position/force control of robot manipulators without velocity measurements: Theory and experimentation. IEEE Trans. Syst. Man and Cybernetics, 1996. (to appear). [7] F. R. Gantmacher. The theory of matrices, vols. I and II. Chelsea publishing Co., New York, 1960. 11

[8] J. Hu, M. de Queiroz, T. Burg, and D. Dawson. Adaptive position/force of robot manipulators without velocity measurements. In Proc. IEEE Conf. Robotics Automat., pages 887{892, 1995. [9] H.P. Huang and W.L. Tseng. Asymptotic observer design for constrained robot systems. IEE Proceedings-D, 138:211{216, 1991. [10] H.N. Koivo and R.K. Kankaanrantaes. Dynamics and simulation of compliant motion of a manipulator. In Proc. IEEE Conf. Robotics Automat., volume 2, pages 163{173, 1988. [11] A. Loria. On output feedback control of Euler-Lagrange systems. PhD thesis, Univ. of Technology of Compiegne, Compiegne, France, October 1996. Available from http://www-ccec.ece.ucsb.edu/people/aloria/index.html. [12] A. Loria and R. Ortega. Force/position regulation for robot manipulators with unmeasurable velocities and uncertain gravity. Automatica, 32(6):939{943, 1996. See also: Proc. IFAC Symposium on Motion Control, Muncih, 1995. [13] H. McClamroch and D. Wang. Feedback stabilization and tracking of constrained robots. IEEE Trans. Automat. Contr., 33:419{426, 1988. [14] J. K. Mills and A. A. Goldenberg. Force and position control of manipulators during constrained motion tasks. IEEE Trans. Robotics Automat., 5(1):30{46, 1989. [15] E. Panteley and A. Stotsky. Adaptive trajectory/force control scheme for constrained robot manipulators. Int. J. Adapt. Control Signal Process., 7(6):489{496, 1993. [16] E. Panteley and A. Stotsky. Asymptotic stability of constrained robot motion observer based control schemes. In Proc. 2nd. European Contr. Conf., pages 1255{1260, Groningen, The Netherlands, 1993. [17] E. Panteley and A. Stotsky. Adaptive trajectory/force control scheme for constrained robot maqnipulators. Technical Report no. 112, Inst. for Problems of Mech. Engg., Academy of Sc. of Russia, June 1995. [18] N. Rouche and J. Mawhin. Ordinary dierential equations II: Stability and periodical solutions. Pitman publishing Ltd., London, 1980. [19] I. Sandberg. Global implicit function theorems. IEEE Trans. Circuits and Systems, 28:145{ 149, 1981. [20] D. Wang and H. McClamroch. Position force control for constrained manipulator motion: Lyapunov's direct method. IEEE Trans. Robotics Automat., 9:308{313, 1993. [21] B. Yao, S. P. Chan, and D. Wang. Robust motion and force control of robot manipulators in the presence of environmental constraint uncertainties. In Proc. 31st. IEEE Conf. Decision Contr., pages 1875{1880, Tucson, Arizona, 1992. [22] T. Yoshikawa. Dynamics hybrid position/force control of robot manipulators{description of hand constraints and calculation of joints driving forces. In Proc. IEEE Conf. Robotics Automat., pages 1393{1398, San Francisco, CA, 1986.

12