Force/Position Regulation for Robot Manipulators with Unmeasurable Velocities and Uncertain Gravity Antonio Loria and Romeo Ortega Universite de Tecnologie de Compiegne URA CNRS 87, HEUDIASYC, BP 69 6000, Compiegne, FRANCE e-mail: aloria@hds.univ-compiegne.fr rortega@hds.univ-compiegne.fr Abstract Force/position regulation of robotic manipulators with unmeasurable velocities and uncertain gravity forces is studied in this paper. We assume an elastically compliant environment with known stiness constant. Wang and McClamroch recently established local asymptotic stability of a simple PD regulator with compensation of the gravity and contact forces at their desired values. In this paper we extend this result proving that, if we replace the velocity measurement by its \dirty" derivative, we can establish semiglobal asymptotic stability. Introduction Several approaches to control force and position in robot manipulators have been proposed in the literature. Depending on the adopted model of contact force these schemes can be roughly classied as compliant (Spong and Vidyasagar 989), impedance (Hogan 985), and constrained motion control (McClamroch and Wang 988). A popular technique to simultaneously regulate position and force is hybrid control (Raibert and Craig 98), (Yoshikawa 986); and recently, parallel control introduced by (Chiaverini and Sciavicco 993). Adaptive implementations of these controllers have also been reported in, e.g., (Carelli et al. 990), (Carelli and Kelly 99), (Lozano and Brogliato 99), (Siciliano and Villani 993), (Panteley and Stotsky 993a). For a good review of the literature the reader should refer to (Chiaverini and Sciavicco 993), (Arimoto 99). On the other hand, it is now well known that the physical structure of the robot, and in particular its passivity property (Ortega and Spong 989), (Arimoto 99), can be suitably exploited to design robustly performant simple (proportional integral derivative (PID)-like) regulators with little robot prior knowledge. This fact, which provides the theoretical foundation of current robot control practice, has motivated several researchers to explore further advantages of these so-called passivity-based controllers. In a recent fundamental paper (Wang and McClamroch 993) established local asymptotic stability of a hybrid scheme consisting of a PD regulator with compensation of the gravity and contact forces at their desired values. This result, which naturally extends to the constrained motion case the seminal contribution of (Takegaki and Arimoto 98), relies on the well known fact (Arimoto 99) that the passivity property of the robot is preserved even when in contact with the environment. Two major drawbacks of the scheme in (Wang and McClamroch 993) are the requirement of measurement of motor speed and the knowledge of the gravity forces at the desired position. Speed measurement increases A. Loria's work was partially supported by CONACyT Mexico. The stability analysis requires the assumptions that the end eector does not loose contact with the environment and that the proportional gain is suciently large.
cost and imposes constraints on the achievable bandwidth. While a good estimate of the gravity forces is hardly available since the gravity force parameters depend on the payload, which is usually unknown. A mismatch in the estimation of this term leads to a shift in the equilibrium point, and consequently to a force/position steady-state error. High-gain feedback reduces {but does not eliminate{ this error, exciting on the other hand high frequency (e.g., bending) modes and increasing the noise sensitivity. To the best of our knowledge the study of force/position control problem whithout velocity measurements has only been studied in (Huang and Tseng 99) and very recently in (Panteley and Stotsky 993b). In (Huang and Tseng 99) the observer design of constrained robots is studied while in (Panteley and Stotsky 993b) design and stability of two position observer based controllers for constrained robots is investigated. On the other hand several solutions to the force/position control problem with uncertain gravity knowledge have appeared in the litterature, see for instance (Siciliano and Villani 993). Nevertheless, the problem of designing an asymptotically stable force/position regulator that does not require the exact knowledge of the gravity forces nor the measurement of speed is as yet unsolved. In this paper we propose a solution to this problem. To this end, we carry out our design using a robot model in task space (Khatib 987), and assume that the tool force is exerted in only one direction normal to an elastic environment whose stiness constant is exactly known. The main contribution of the paper is the proof that under these assumptions, we can design a semiglobally asymptotically stable regulator consisting of a PD and two integral terms. The present controller extends the results on PI D regulation of (Ortega et al. 995) to the constrained environment case observing that, under the compliant envrionment assumption, the force/position control problem can be reformulated as a pure position control problem with a suitable change of coordinates. The organization of this note is as follows. In section we introduce the constrained robot model and formulate the problem we solve later in section 3. Finally, we conclude with some remarks. Notation < mm {(m m) Euclidean space, k k - Euclidean norm, (), (){ smallest and largest eigenvalue, respectively. Model and problem formulation We consider the robot manipulator model in the tool coordinates (Khatib 987) D(x)x + C(x; _x) _x + g(x) = u? F () When m is equal to the number of joints n and the manipulator acts in a nonsingular conguration, x constitutes a set of Lagrangian generalized coordinates in which case, D(x) > 0 and assumes the role of a true inertia matrix (Khatib 987), otherwise it is only a psuedo inertia matrix. Throughout this work our attention is focused on non redundant robots, i.e. m = n. Under these assumptions, the relationship between the joints space and the task space, is given by D(x) = J?T (q)d(q)j? (q) C(x; _x) = J?T (q)c(q; _q)? D(x) J(q) q g(x) = J?T (q)g(q) u = J?T (q) () where q are the generalized coordinates, D(q) is the inertia matrix, C(q; _q) is the Coriolis and centrifugal generalized forces matrix, g(q) is the vector of gravitational forces and is the input force, all expressed in the joints referential frame; and nally, J(q) is the jacobian matrix.. Properties In the following we underline some well known properties of the model useful for our controller design See for instance (Chiaverini and Sciavicco 993)
Property.. Considering all revolute joints, the inertia matrix D(x) is lower and upper bounded by 0 < d m I < (D(x))I D(x) (D(x))I d M I < where I stands for the m m identity matrix. Property.. The matrix N = _ D(x)? C(x; _x) is skew-symmetric, that is, _D(x) = C(x; _x) + C T (x; _x) Furthermore, the matrix C(x; _x) is linear on _x and bounded on x, hence for some k c < + kc(x; _x)k k c k _xk Property.3. The generalized gravitational forces vector g(x) := @U g(x) @x satises @g(x) @x k g (3) for some k g < +, where U g (x) is the potential energy expressed in the operational space and is supposed to be bounded from below.. Assumptions Assumption.. Throughout this work, by `position' we mean both position and orientation while `force' stands for linear force and torque. In other words, we only deal with position and linear force control, hence m = 3. Assumption.. We consider the contact force to act only in the direction normal to the environment, for the sake of simplicity and without loss of generality we choose F := [0 0 f] T. Hence, we divide the generalized coordinates x into x := [x T p x n ] T where x p < and x n < are the parallel and normal coordinates, i.e. where x n := n T x; n T := [0 0 ] n T := x p := n T x 0 0 0 0 Assumption.3. We suppose the end{eector to be acting on an elastic environment with stiness k, then the force f is dened by f := k(x n? x n0 ) () where x n0 is the position in the direction of x n where no force is exerted. Using the above dened notation we are able to write 3 F := kn n T (x? 0 0 x n0 5) (5).3 Problem formulation In this section we formulate the problem we will solve. Output feedback force/position control problem Under the same condition A. above assume now A.' d m ; d M and k g are known A.3' Only position is available from measurement Then, design a controller that insures the desired equilibrium is semiglobally asymptotically stable. That is, for any closed set D in the state space we can nd controller gains, dependent on D, such that D, where is the domain of attraction of the desired equilibrium. 3
3 Main result Theorem 3.. Consider the model () in closed loop with the PI D control law u =?K p (x? x d )? K d # + F d + (6) _ =?K i (x? x d? #); (0) = 0 < 3 (7) bi p # = diag x; #(0) = # 0 < 3 (8) p + a i where p := d dt, b i > 0; a i > 0 and K p ; K i ; K d are positive denite diagonal matrices, K p satises and the high frequency gains b i of the lters (8) are such that K p > (k g + )I (9) b i > d M d m (0) with k g as in (3). Under these conditions, we can always nd a (suciently small) integral gain K i such that the equilibrium s := [x T p ; f; _x T p ; _ f; # T ; T ] T = [x T pd; f d ; 0; 0; 0; g T (x d )] T is asymptotically stable with a domain of attraction including the closed ball of radius c in <, where lim min(bi)! c =. In other words, the desired equilibrium is semiglobally asymptotically stable. Remark. It is worth mentionning that in the case where velocity measurements asre available, global asymptotic stability can be reached with a similar controller. This is stressed in theorem 3. below, the proof is not included in this note because of lack of space 3. Theorem 3.. Consider the model () in closed loop with the control law u =?K p (x? x d )? K d _x + F d + () _ =?K i (x? x d ); (0) = 0 R 3 () where F d := [0; 0; f d ] T, K d and K i are constant positive denite diagonal matrices and, for a given K i, K p is dened as where K 0 p satises K p := Kp 0 + K i (3) 0 := () + kx? x d k k g < (K 0 p ) (5) with k g as in (3). Under these conditions, we can always nd a (suciently small) positive constant 0, {independent of the initial conditions{, such that the equilibrium is GAS. [x T p ; f; _x T p ; _ f; T ] T = [x T pd; f d ; 0; 0; g T (x d )] T 3. Proof of theorem 3. The proof relies on classical Lyapunov theory, and is divided in three parts. First, we dene a suitable error equation for the closed loop system, whose (unique) equilibrium is at the desired value. Then, we propose a Lyapunov function candidate. Third, we prove that under the conditions of theorem 3. the proposed function qualies as a Lyapunov function, and establish the asymptotic stability of the equilibrium. Fourth, we dene a domain of attraction and prove that it can be arbitrarily enlarged. 3 Interested readers, please consult http://www.hds.univ-compiegne.fr/ aloria/publications.html#force
Error Equation First, let us dene where, for a given constant > 0, a K i is chosen such that thus insuring K 0 p > k gi. It is also useful as well to write so that, from (6) and dening ~x := x? x d we have K 0 p := K p? K i (6) I > K i (7) K 00 p := K 0 p + kn n T (8) K p ~x = K 00 p (x? ) + K i~x + g(x d )? kn n T ~x (9) where := x d + (K 00 p )? g(x d ). Hence using the denitions above we can direclty express the error dynamics of (6) { (8) plus () as D(x)x + C(x; _x) _x + g(x) + K 00 p (x? ) + K d# =? K i~x + ~ _# =?A# + B _x _~ =?K i (~x? #) where A := diagfa i g; B := diagfb i g and ~ :=? g(x d ). Now, dening leads us to the error equation z :=? K i~x + ~ (0) D(x)x + C(x; _x) _x + g(x) + K 00 p (x? ) + K d# = z _# =?A# + B _x _z =?K i (~x + _x? #) () Observe that, with the state vector s 0 := [~x T one. _x T # T z T ] T, the unique equilibrium point of () is the trivial Lyapunov Function Candidate We will now construct a Lyapunov function for () whose derivate is locally negative semidenite in ~x; _x; #. We propose V (~x; _x; #) = _xt D(x) _x + U g (x) + (x? )T K 00 p (x? ) + #T K d B? # + c where c :=? g(x d) T (K 00 p )? g(x d )? U g (x d ) is added to the systems total energy to enforce V (0) = 0. Taking the derivative of V we get then _V =?# T K d B? A# + _x T z To cancel the second right hand term we add to V the redened function V (z) := zt K i? z to get now _V + _ V =?# T K d B? A#? z T (~x? #) To remove the cross products above we add the cross term V 3 as before and the new one V (x; _x; #) :=?# T D(x) _x. Summing up all of them together we get our Lyapunov function candidate as V := V + V + (V 3 + V ) () 5
which after some straightforward calculations (Ortega et al 99) and a suitable partition yields _V? k~xk k#k z } { [(Kp 00 )? k g ]? (K p 00 )? (K d )? k g? (K p 00 )? (K d )? k g (K db? A) Q T k~xk k#k Q? T z } { k#k (K db? A)? (A)d M k#k k _xk? (A)d? M (B)d m k _xk? (B)d m? d M? k c k#k? k c k~xk k _xk?? (K db? A)? (K d ) k#k (3)? Asymptotic Stability Let us rst partition V as V = W + W + W 3 + W where W = 8 _xt D(x) _x + 8 ~xt K 00 p ~x + ~xt D(x) _x () W = 8 ~xt K 00 p ~x + U g + ~x T K 00 p (x d? ) + (x d? ) T K 00 p (x d? ) + c (5) W 3 = 8 _xt D(x) _x + #T K d B? #? # T D(x) _x (6) W = ~xt K 00 p ~x + zt K? i z T + #T K d B? # + _xt D(x) _x (7) Under the conditions of theorem 3., W is positive denite (Kelly et al. 99). W is positive denite if s (Kp 00) (8) d M while s (K d B? ) 8d M > (9) insures W 3 is positive denite. Thus, V is positive denite for suciently small, which entails, via (7), K i suciently small. We derive now sucient conditions for _ V to be locally negative semidenite in ~x; _x; #. If we have Q > 0. In a similar way, Q > 0 if The third right hand term of (3) is negative if [(K 00 p )? k g ](K d B? A) (K 00 p ) + (K d ) + k g > (30) k c (B)(K d A)d m (B)[ (A)d M ] > (3) (B)d m? d M > ksk (3) Observe that we give this condition in terms of the original state s, instead of s 0. This in order to derive the domain of attraction (and prove the semiglobal stability claim that requires arbitrarily small) in the coordinates s. 6
where the left hand side is positive due to (0). Finally, the last term in (3) is negative if (K d B? A) (K d ) > (33) Choosing suciently small insures (9) { (3) and (33) are satised. Therefore, (3) is locally negative semidenite and the equilibrium is stable in the sense of Lyapunov. Considering again denition (), asymptotic stability of the state s follows inmediately invoking LaSalle's invariance principle. Domain of attraction To dene the domain of attraction we will rst nd some positive constants, such that ksk V (s) ksk (3) Notice that V W (K 00 p )k~xk + (K d B? )k#k + d m k _xk + (K? i )kzk To obtain the lower bound in terms of s we need the following inequality 5 (K? i ) k~k (K i ) +? (K i ) k~xk kzk which leads to V (Kp 00 (K i ) (K i )? k~xk + (K d B? )k#k + + d m k _xk + (K i )? k~k (35) so we dene as := min (Kp 00 ) + (K i ) (K i )? ; (K d B? ); d m ; (K i )? In a similar manner, an upperbound on V is V (K 00 h p ) + k g + d M + (K i p 00 ) k~xk + ( + )d M k _xk + + dm + (K d B? ) k#k + (K i? )kzk where using (7) and after some straightforward calculations we write kzk (K + i ) k~k + (K i ) k~xk < k~k + k~xk thus dening, := max (K 00 h p ) + k g + d M + (K i p 00 ) + (K i ) ; ( + )d M ; dm + (K d B? ) (36) From (3) and (3) we conclude that the domain of attraction contains the set ksk c := r k c (B)d m? d M (37) 5 which follows inmediately from denition (0) and the \triangle" inequality 7
Semiglobal Stability To establish semiglobal stability we must prove that, with a suitable choice of the controller gains, we can arbitrarily enlarge the domain of attraction. To this end, we propose to increase (B). The key question here is whether this can be done without violating the order relationships between B and imposed by the stability conditions (9) { (3) and (33). Using the fact that (K D B? ) (K D )? (B), we see that the order relationship due to (9) is 6 ( (B)) = O(?= (B)), while that of (30) and (33) is ( (B)) = O(? (B)). The latter being implied by the former for B suciently large. Furthermore, note in (3) that considering (B) (B) <, is not dependent on B. On the other hand, for (B) suciently large we can always nd > 0 so that = c 3 = (B) and = c, where c 3 ; c are constants independent of B. Replacing this in (37) we get lim c = lim c p 5 (B) (B)! (B)! where c 5 is also independent of B. This proves that there exists > 0 such that ) the stability conditions, are satised; ) the domain of attraction is arbitrarily enlarged, that is, lim (B)! c =. The proof is completed choosing, for the given, (K i ) = O() and such that (7) holds. Acknowledgements Authors greatfully acknowledge Dr. Elena Panteley for the fruitful discussions on the topic of this work. Concluding remarks The force/position control problem has been visited. A drawback of our approach is the assumption of exactly knowing the environment stiness constant k was used to keenly compute some desired constant position x nd. Nevertheless, it is worthly remarking that this assumption is used in several results in the litterature. The assumption of velocity measurements often used in the litterature (with exception of (Huang and Tseng 99) and (Panteley and Stotsky 993b)) was relaxed to consider only position measurements. In such case, semiglobal asymptotic stability was proved, thus dening the domain of attraction in terms of the controller gains and showing that it can be arbitrarily enlarged. References Arimoto, S. (99). State-of-the-art and future research directions of robot control. In `Proc. th. Symposium on Robot Control'. Capri, Italy. pp. 3{. Carelli, R. and R. Kelly (99). `An adaptive impedance/force controller for robot manipulators'. IEEE Trans. Automat. Contr. 36, 967{97. Carelli, R., R. Kelly and R. Ortega (990). `Adaptive force control of robot manipulators'. International Journal of Control 5(), 37{5. Chiaverini, S. and L. Sciavicco (993). `The parallel approach to force/position control manipulators'. IEEE Trans. Robotics Automat. 9, 89{93. 6 A function of is denoted O( k ) when for all [0; ] its norm is less than c k, where c > 0; > 0 and k are some constants. 8
Hogan, N. (985). `Impedance control: An approach to manipulation. Parts I{III,'. ASME J. Dyn. Syst. Meas. Contr. 07, {. Huang, H. and W. Tseng (99). `Asymptotic observer design for constrained robot systems'. IEE Proceedings-D 38, {6. Khatib, O. (987). `A unied approach for motion and force control of robot manipulators: The operational space formulation,'. IEEE J. Robotics Automat. RA-3, 3{53. Lozano, R. and B. Brogliato (99). `Adaptive hybrid force-position control for redundant manipulators'. IEEE Trans. Automat. Contr. AC-37, 50{505. McClamroch, H. and D. Wang (988). `Feedback stabilization and tracking of constrained robots'. IEEE Trans. Automat. Contr. 33, 9{6. Ortega, R., A. Loria and R. Kelly (995). `A semiglobally stable output feedback PI D regulator for robot manipulators'. IEEE Trans. Automat. Contr. Ortega, R. and M. Spong (989). `Adaptive motion control of rigid robots: A tutorial'. Automatica 5-6, 877{888. Panteley, E. and A. Stotsky (993a). `Adaptive trajectory/force control scheme for constrained robot maqnipulators'. Int. J. Adapt. Control Signal Process. 7(6), 89{96. Panteley, E. and A. Stotsky (993b). Asymptotic stability of constrained robot motion observer based control schemes. In `Proc. nd. European Contr. Conf.'. Groningen, The Netherlands. Raibert, M. and J. Craig (98). `Hybrid position/force control of manipulators'. ASME J. Dyn. Syst. Meas. Contr. 03, 6{33. Siciliano, B. and L. Villani (993). `Adaptive force/position regulator for robot manipulators'. Int. J. Adapt. Control Signal Process. 7, 389{03. Spong, M. and M. Vidyasagar (989). Robot Dynamics and Control. John Wiley & Sons. New York. Takegaki, M. and S. Arimoto (98). `A new feedback method for dynamic control of manipulators'. ASME J. Dyn. Syst. Meas. Contr. 03, 9{5. Wang, D. and H. McClamroch (993). `Position force control for constrained manipulator motion: Lyapunov's direct method'. IEEE Trans. Robotics Automat. 9, 308{33. Yoshikawa, T. (986). Dynamics hybrid position/force control of robot manipulators{description of hand constraints and calculation of joints driving forces. In `Proc. IEEE Conf. Robotics Automat.'. San Francisco, CA. pp. 393{398. 9