July 9, :49 World Scientific Book - 9in x 6in ws-book. Publishers page

Transcription

1 Publishers page i

2 Publishers page ii

3 Publishers page iii

4 Publishers page iv

5 Contents 1. Tracking control of multiconstraint nonsmooth Lagrangian systems Introduction Basic concepts Typical task Exogenous signals entering the dynamics Stability analysis criteria Controller design Tracking control framework Design of the desired trajectories Design of qd ( ) and q d( ) on the phases I k Design of the desired contact force during constraint phases Strategy for take-off at the end of constraint phases Ω J k Closed-loop stability analysis Illustrative example Conclusions Bibliography 23 v

6 Chapter 1 Tracking control of multiconstraint nonsmooth Lagrangian systems 1.1 Introduction The control of mechanical systems subject to unilateral constraints has been the object of many studies in the past fifteen years. Such systems, which consist of three main ingredients (see (1.1) below) are highly nonlinear nonsmooth dynamical systems. Theoretical aspects of their Lyapunov stability and the related stabilization issues have been studied in [Brogliato (7); Leine and van de Wouw (19, 17); Tornambé (31)]. The specific yet important task of the stabilization of impacting transition phases was analyzed and experimentally tested in [Lee et al (16); Pagilla and Yu (28); Sekhavat et al (29); van Vliet et al (32); Volpe and and Khosla (33); Xu et al (34)]. From the point of view of tracking control of complementarity Lagrangian systems along general constrained/unconstrained paths, such studies focus on a module of the overall control problem. One of the first works formulating the control of complete robotic tasks via unilateral constraints and complementarity conditions was presented in [Huang and McClamroch (15)]. In that work the impacts were considered inelastic and the control problem was solved using a time optimal problem. The tracking control problem under consideration, involving systems that undergo transitions from free to constrained motions, and vice-versa, along an infinity of cycles, was formulated and studied in [Brogliato et al (9)] mainly for the 1-dof (degree-of-freedom) case and in [Bourgeot and Brogliato (5)] for the n-dof case. Both of these works consider systems with only one unilateral frictionless constraint. In this chapter we not only consider the multiconstraint case but the results in Section 1.7 relax some very hard to verify conditions imposed in [Bourgeot and Brogliato (5)] to assure the stability. Moreover the accurate design of the control law that guarantees the detach- 1

7 2 My Book Title ment from the constraints is formulated and incorporated in the stability analysis for the first time. Considering multiple constraints may be quite important in applications like virtual reality and haptic systems, where typical tasks involve manipulating objects modelled as rigid bodies [Faulring et al (11)] in complex environments with many unilateral constraints. We note that in the case of a single nonsmooth impact the exponential stability and bounded-input bounded state (BIBS) stability was studied in [Menini and Tornambè (24)] using a state feedback control law. A study for a multiple degree-of-freedom linear systems subject to nonsmooth impacts can be found in [Menini and Tornambè (25)]. That approach proposes a proportional-derivative control law in order to study BIBS stability via Lyapunov techniques. Other approaches for the tracking control of nonsmooth mechanical systems can be found in [Galeani et al (12); Menini and Tornambè (23); Pagilla (27)] and in [Leine and van de Wouw (18)]. The analysis and control of systems subject to unilateral constraints also received attention in [Bentsman and Miller (4)]. This chapter focuses on the problem of tracking control of complementarity Lagrangian systems [Moreau (26)] subject to frictionless unilateral constraints whose dynamics may be expressed as: M(X)Ẍ + C(X, Ẋ)Ẋ + G(X) = U + F (X)λ X 0 λ X F (X) 0, (1.1) Collision rule where X(t) R n is the vector of generalized coordinates, M(X) = M T (X) R n n is the positive definite inertia matrix, F (X) R m represents the distance to the constraints, C(X, Ẋ) is the matrix containing Coriolis and centripetal forces, G(X) contains conservative forces, λ X R m is the vector of the Lagrangian multipliers associated to the constraints and U R n is the vector of generalized torque inputs. For the sake of completeness we precise that denotes the Euclidean gradient F (X) = ( F 1 (X),..., F m (X)) R n m where F i (X) R n represents the vector of partial derivatives of F i ( ) w.r.t. the components of X. We assume that the functions F i ( ) are continuously differentiable and that F i (X) 0 for all X with F i (X) = 0. It is worth to precise here that for a given function f( ) its derivative w.r.t. the time t will be denoted by f( ). For any function f( ) the limit to the right at the instant t will be denoted by f(t + ) and the limit to the left will be denoted by f(t ). A simple jump of the function f( ) at the moment t = t l is denoted σ f (t l ) = f(t + l ) f(t l ). The Dirac measure at time t is δ t.

8 Tracking control of multiconstraint nonsmooth Lagrangian systems 3 Definition 1.1. A Linear Complementarity Problem (LCP) is a system given by: 0 λ Aλ + b 0 (1.2) Such an LCP has a unique solution for all b if and only if A is a P-matrix, i.e. all its principal minors are positive [Facchinei and Pang (10)]. The admissible domain associated to the system (1.1) is the closed set Φ where the system can evolve and it is described as follows: Φ = {X R n F (X) 0} = Φ i, 1 i m where Φ i = {X R n F i (X) 0} considering that a vector is non-negative if and only if all its components are non-negative. In order to have a wellposed problem with a physical meaning we consider that Φ contains at least a closed ball of positive radius. Definition 1.2. A singularity of the boundary Φ of Φ is the intersection of two or more codimension one surfaces Σ i = {X R n F i (X) = 0}. The presence of Φ may induce some impacts that must be included in the dynamics of the system. It is obvious that m > 1 allows both simple impacts (when one constraint is involved) and multiple impacts (when singularities or surfaces of codimension larger than 1 are involved). Let us introduce the following notion of p ɛ -impact. Definition 1.3. Let ɛ 0 be a fixed real number. We say that a p ɛ -impact occurs at the instant t if F I (X(t)) ɛ, F i (X(t)) = 0 where I 1,..., m, card(i) = p, F I (X) = (F i1 (X), F i2 (X),..., F ip (X)) T, i 1, i 2,...,i p I. If ɛ = 0 the p surfaces Σ i, i I are stroked simultaneously and a p impact occurs. When ɛ > 0 the system collides Φ in a neighborhood of the intersection i I Σ i (see Figure 1.1). Definition 1.4. [Moreau (26); Hiriart-Urruty and Lemaréchal (20)] The tangent cone to Φ = {X R n F i (X) 0, i = 1,..., m} at X R n is defined as: i I T Φ (X) = {z R n z T F i (X) 0, i = J(X)}

9 4 My Book Title ɛ Fig. 1.1 Illustration of 2 ɛ-impacts where J(X) {i {1,..., m} F i (X) 0} is the index set of active constraints. When X Φ \ Φ one has J(X) = and T Φ (X) = R n. The normal cone to Φ at X is defined as the polar cone to T Φ (X): N Φ (X) = {y R n z T Φ (X), y T z 0} The collision (or restitution) rule in (1.1), is a relation between the postimpact velocity and the pre-impact velocity. Among the various models of collision rules, Moreau s rule is an extension of Newton s law which is energetically consistent [Glocker (14); Mabrouk (21)] and is numerically tractable [Acary and Brogliato (1)]. For these reasons throughout this chapter the collision rule will be defined by Moreau s relation [Moreau (26)]: Ẋ(t + 1 l ) = (1 + e) arg min z T Φ(X(t l )) 2 [z Ẋ(t l )]T M(X(t l ))[z Ẋ(t l )] eẋ(t l ) (1.3) where Ẋ(t+ l ) is the post-impact velocity, Ẋ(t l ) is the pre-impact velocity and e [0, 1] is the restitution coefficient. Denoting by T the kinetic energy of the system, we can compute the kinetic energy loss at the impact time t l as [Mabrouk (21)]: T L (t l ) = 1 e ] [[Ẋ(t+l 2(1 + e) ) Ẋ(t l )]T M(X(t l ))[Ẋ(t+l ) Ẋ(t l )] 0 (1.4) The collision rule can be rewritten considering the vector of generalized velocities as an element of the tangent space to the configuration space of the system, equipped with the kinetic energy metric. Doing so (see [Brogliato (6)] 6.2), the discontinuous velocity components Ẋ ( ) norm Ẋnorm and the continuous ones Ẋ tang are identified. Precisely, = Ẋ ( ) tang n MẊ, M = M(X) where n represents the unitary normal vec- t

10 Tracking control of multiconstraint nonsmooth Lagrangian systems 5 tors n i = M 1 (X(t l )) F i(x(t l )) Fi(X(t l )) T M 1 (X(t l )) F i(x(t l )), i J(X(t l)) (see Definition 1.4) and t represents mutually independent unitary vectors t i such that t T i M(X(t l))n j = 0, i, j. In this case the collision rule (1.3) at the ( Ẋnorm (t + impact time t l becomes the generalized Newton s rule l ) ) Ẋ tang (t + l ) = ( Ẋnorm (t η l ) ) Ẋ tang (t l ), η = diag(e 1,..., e m, 0,..., 0) where e i is the restitution coefficient w.r.t. the surface Σ i. For the sake of simplicity we consider in this chapter that all the restitution coefficients are equal, i.e. e 1 =... = e m e. The structure of the chapter is as follows: in Section 1.2 one presents some basic concepts and prerequisites necessary for the further developments. Section 1.3 is devoted to the controller design. In Section 1.4 one defines the exogenous trajectories entering the dynamics. The desired contact-force that must occur on the phases where the motion is constrained, is explicitly defined in Section 1.5. Section 1.6 focuses on the strategy for take-off at the end of the constraint phases. The main results related to the closed-loop stability analysis are presented in Section 1.7. Examples and concluding remarks end the chapter. The following standard notations will be adopted: is the Euclidean norm, b p R p and b n p R n p are the vectors formed with the first p and the last n p components of b R n, respectively. N Φ (X p = 0) is the normal cone N Φ (X) to Φ at X when X satisfies X p = 0, λ min ( ) and λ max ( ) represent the smallest and the largest eigenvalues, respectively. 1.2 Basic concepts Typical task Following [Brogliato et al (9)] the time axis can be split into intervals Ω k and I k corresponding to specific phases of motion. Due to the singularities of Φ that must be taken into account, the constrained-motion phases need to be decomposed in sub-phases where some specific constraints are active. Between two such sub-phases a transition phase occurs only when the number of active constraints increases. This means that a typical task can be represented in the time domain as:

11 6 My Book Title t R + = Ω 0 k 1 ( I k ( mk i=1 Ω J k,i k )) (1.5) J k,mk J k+1,1, J k,mk J k,mk 1... J k,1 where the superscript J k,i = {j {1,..., m} F j (X) = 0} represents the set of active constraints during the corresponding motion phase, and I k denotes the transient between two Ω k phases when the number of active constraints increases. Without loss of generality we suppose that the system is initialized in the interior of Φ at a free-motion phase. The impacts during I k involve p = J k,1 constraints (p ɛ -impacts). Furthermore we shall prove that the first impact of I k is a p ɛ -impact with ɛ bounded by a parameter chosen by the designer. When the number of active constraints decreases there is no impact, thus no other transition phases are needed. We note that J k,i = corresponds to free-motion (F (X) > 0). Since the tracking control problem involves no difficulty during the Ω k phases, the central issue is the study of the passages between them (the design of transition phases I k and detachment conditions), and the stability of the trajectories evolving along (1.5) (i.e. an infinity of cycles). It is noteworthy that the passage Ω J k,i k Ω J k,i+1 k consists of detachments from some constraints. In Section 1.6 we consider that p constraints are active and we give the conditions to smoothly take-off from r of them. It is clear that once we know how to do that, we can manage all the transitions ( mentioned mk ) above. Throughout the chapter, the sequence I k i=1 ΩJ k,i k will be referred to as the cycle k of the system s evolution. For robustness reasons during transition phases I k we impose a closed-loop dynamics (containing impacts) that mimics somehow the bouncing-ball dynamics (see e.g. [Brogliato (6)]) Exogenous signals entering the dynamics In this section we introduce the trajectories playing a role in the dynamics and the design of the controller. Some instants that will be used further are also defined. X nc ( ) denotes the desired trajectory of the unconstrained system (i.e. the trajectory that the system should track if there were no constraints). We suppose that F (X nc (t)) < 0 for some t, otherwise the problem reduces to the tracking control of a system with no constraints.

12 Tracking control of multiconstraint nonsmooth Lagrangian systems 7 Xd ( ) denotes the signal entering the control input and playing the role of the desired trajectory during some parts of the motion. X d ( ) represents the signal entering the Lyapunov function. This signal is set on the boundary Φ after the first impact of each cycle. The signals Xd ( ) and X d( ) coincide on the Ω k phases while X nc ( ) is used to define everywhere Xd ( ) and X d( ). Throughout the chapter we consider I k = [τ0 k, tk f ], where τ 0 k is chosen by the designer as the start of the transition phase I k and t k f is the end of I k. We note that all superscripts ( ) k will refer to the cycle k of the system motion. We also use the following notations: t k 0 is the first impact during the cycle k, t k is the accumulation point of the sequence {t k l } l 0 of the impact instants during the cycle k (t k f tk ), τ1 k will be explicitly defined later and represents the instant when the signal Xd ( ) reaches a given value chosen by the designer in order to impose a closed-loop dynamics with impacts during the transition phases, is the desired detachment instant at the end of the phase Ω J k,i k. t k,i d It is noteworthy that t k 0, tk are state-dependent whereas τ0 k, τ 1 k and tk,i d are exogenous and imposed by the designer. To better understand the definition of these specific instants, in Figure 1.2 we represent the exogenous signals X nc ( ), X d ( ), Xd ( ) during a sequence ΩJ k 1 k 1 I k Ω J k,1 k Ω J k,2 k when the motion is simplified as follows: during the transition phase we take into account only the constraints that must be activated J k,1 \ J k 1,mk 1. at the end of the phase Ω J k,1 k we take into account only the constraints that must be deactivated J k,1 \ J k,2. The points A, A, A and C in Figure 1.2 correspond to the moments τ0 k, tk 0, tk f and tk,1 d respectively. We have seen that the choice of τ0 k plays an important role in the stability criterion given by Proposition 1.1. On the other hand in Figure 1.2 we see that starting from A the desired trajectory X d ( ) = Xd ( ) is deformed compared to X nc ( ). In order to reduce this deformation, the time τ0 k and implicitly the point A must be close to Φ. Further details on the choice of τ0 k will be given later. Taking into account just the constraints J k,1 \ J k,2 we can identify t k,1 d with the moment when X d ( ) and X nc ( ) rejoin at C.

13 8 My Book Title Φ X nc (t) = X d (t) = X d(t) A A X d (t) = X d(t) Φ C A B X d (t) X d (t) X nc (t) Fig. 1.2 The closed-loop desired trajectory and control signals In order to clarify the differences and the usefulness of the signals introduced above we present a simple example concerning a one-degree of freedom model. Example 1.1. Let us consider the following one degree-of-freedom dynamical system: (Ẍ Ẍ d ) + γ 2(Ẋ Ẋ d ) + γ 1(X Xd ) = λ 0 X λ 0 (1.6) Ẋ(t + k ) = enẋ(t k ) where X d ( ) is a twice differentiable signal and γ 1, γ 2 are two gains. The real number λ represents the Lagrange multiplier associated to the constraint X 0. X d a Φ X = 0 λ m = 1 Ox Fig. 1.3 One degree of freedom: unitary mass dynamics

14 Tracking control of multiconstraint nonsmooth Lagrangian systems 9 The system (1.6) represents the dynamics of a unitary mass lying in the right half plane (X 0) and moving on the Ox axis (see figure 1.3). Thus, the system dynamics is expressed by Ẍ = U + λ with the control law U = Ẍd γ 2(Ẋ Ẋ d ) γ 1(X Xd ). One considers that the Lyapunov function V (X, Ẋ, t) of the closed-loop system is given by a quadratic function of the tracking error. In order to stabilize the system on the constraint surface Φ there are two solutions. [1st solution] Setting Xd (t) = 0, Ẋ d (t) = 0 and Ẍ d (t) = 0 at the time t of contact with Φ. Therefore when the system reaches its equilibrium point on Φ one obtains the contact force λ = 0. It is noteworthy that, since we are interested to study a dynamics containing a constraint movement phase, we need a non-zero contact force when the system is stabilized on Φ. Therefore this type of solution is not convenient for the study developed in this work. Moreover the stabilization on Φ with a non zero tracking error on [t ɛ, t] is not obvious, especially in higher dimensions, so that the conditions for a perfect tangential approach are never met in practice. [2nd solution] Setting Xd (t) = a, a > 0, Ẋ d (t) = 0 and Ẍ d (t) = 0 on some interval of time when approaching Φ. Since the tracking error X = X Xd = X d at the equilibrium point we cannot use X in the definition of the Lyapunov function. Obviously the system reaches the desired position when X = 0. Thus we shall use the signal X d (t) = 0 in order to express the desired trajectory in the Lyapunov function and the corresponding tracking error will be given by X = X X d. Concluding, when the equilibrium point is reached one obtains X = 0, X = a, V (t, X = 0) = 0 and the corresponding contact force λ = γ 1 Xd = γ 1a > Stability analysis criteria The system (1.1) is a complex nonsmooth and nonlinear dynamical system which involves continuous and discrete time phases. A stability framework for this type of systems has been proposed in [Brogliato et al (9)] and extended in [Bourgeot and Brogliato (5)]. This is an extension of the Lyapunov second method adapted to closed-loop mechanical systems with unilateral constraints. Since we use this criterion in the following tracking control strategy it is worth to clarify the framework and to introduce some definitions. Let us define Ω as the complement in R + of I = I k and assume that k 1 the Lebesgue measure of Ω, denoted λ[ω], equals infinity. Consider x( )

15 10 My Book Title the state of the closed-loop system in (1.1) with some feedback controller U(X, Ẋ, X d, Ẋ d, Ẍ d ). Definition 1.5 (Weakly Stable System). The closed loop system is called weakly stable if for each ɛ > 0 there exists δ(ɛ) > 0 such that x(0) δ(ɛ) x(t) ɛ for all t 0, t Ω. The system is asymptotically weakly stable if it is weakly stable and lim t Ω, t x(t) = 0. Finally, the practical weak stability holds if there exists 0 < R < + and t < + such that x(t) < R for all t > t, t Ω. Weak stability is therefore Lyapunov stability without looking at the transition phases. Consider V ( ) such that there exists strictly increasing functions α( ) and β( ) satisfying the conditions: α(0) = 0, β(0) = 0 and α( x ) V (x, t) β( x ). Definition 1.6. A transition phase I k is called finite if it involves a sequence of impact times (t k l ) 0 l N, N with the accumulation point t k N < (for the sake of simplicity we shall denote the accumulation point by t k even if N < ). In the sequel all the transition phases are supposed finite, which implies that e < 1 (in [Ballard (3)] it is shown that e = 1 implies that t k = + ). The following criterion is inspired from [Bourgeot and Brogliato (5)], and will be used to study the stability of the system (1.1). Proposition 1.1 (Weak Stability). Assume that the task admits the representation (1.5) and that (a) λ[i k ] < +, k N, (b) outside the impact accumulation phases [t k 0, tk ] one has V (x(t), t) γv (x(t), t) for some constant γ > 0, (c) [ V (t k l+1 ) V (tk+ l ) ] K 1 V p1 (τ0 k ), k N for some p 1 0, K 1 0, l 0 (d) the system is initialized on Ω 0 such that V (τ0 1 ) 1, (e) V (t l 0σ k l ) K 2V p2 (τ0 k ) + ξ, k N for some p 2 0, K 2 0 and ξ 0. If p = min{p 1, p 2 } < 1 then V (τ k 0 ) δ(γ, ξ), k 2, where δ(γ, ξ) is a function that can be made arbitrarily small by increasing the value of γ. The system is practically weakly stable with R = α 1 (δ(γ, ξ)).

16 Tracking control of multiconstraint nonsmooth Lagrangian systems 11 Remark 1.1. Since the Lyapunov function is exponentially decreasing on the Ω k phases, assumption (d) in Proposition 1.1 means that the system is initialized on Ω 0 sufficiently far from the moment when the trajectory X nc ( ) leaves the admissible domain. Precisely, the weak stability is characterized by an almost decreasing Lyapunov function V (x( ). ) as illustrated in Figure 1.4. Fig. 1.4 Typical evolution of the Lyapunov function of weakly stable systems. Remark 1.2. It is worth to point out the local character of the stability criterion proposed by Proposition 1.1. This character is firstly given by condition d) of the statement and secondly by the synchronization constraints of the control law and the motion phase of the system (see (1.5) and (1.7) below). The practical stability is very useful because attaining asymptotic stability is not an easy task for the unilaterally constrained systems described by (1.1) especially when n 2 and M(q) is not a diagonal matrix (i.e. there are inertial couplings, which is the general case). 1.3 Controller design In order to overcome some difficulties that can appear in the controller definition, the dynamical equations (1.1) will be expressed in the generalized coordinates introduced by McClamroch & Wang [McClamroch and Wang (22)], which allow one to split the generalized coordinates into a normal and a tangential parts, with a suitable diffeomorphic transformation q =

17 12 My Book Title Q(X). We suppose that the generalized coordinates transformation holds globally in Φ, which may obviously not be the case in general. However, the study of the singularities that might be generated by the coordinates transformation is out of the scope of this chapter. Let us consider D = [I. O] R m n, I R m m the identity matrix. The new coordinates will be [ ] q 1 q = Q(X) R n q1, with q =, q 1 = 1. such that Φ = {q Dq 0} 1. q 2 The tangent cone T Φ (q 1 = 0) = {v Dv 0} is the space of admissible velocities on the boundary of Φ. The controller used here consists of different low-level control laws for each phase of the system. More precisely, the switching controller can be expressed as U nc for t Ω k T (q)u = Ut J for t I k (1.7) Uc J for t Ω J k ( ) T1 (q) where T (q) = R n n is full-rank under some basic assumptions T 2 (q) (see [McClamroch and Wang (22)]). The dynamics becomes: M 11 (q) q 1 + M 12 (q) q 2 + C 1 (q, q) q + g 1 (q) = T 1 (q)u + λ M 21 (q) q 1 + M 22 (q) q 2 + C 2 (q, q) q + g 2 (q) = T 2 (q)u q 1 i 0, q1λ i (1.8) i = 0, λ i 0, 1 i m Collision rule where the set of complementary relations can be written more compactly as 0 λ Dq 0. In the sequel U nc coincides with the fixed-parameter controller proposed in [Slotine and Li (30)] and the closed-loop stability analysis of the system is based on Proposition 1.1. First, let us introduce some notations: q = q q d, q = q qd, s = q + γ 2 q, s = q + γ 2 q, q e = q d γ 2 q where γ 2 > 0 is a scalar gain and q d ( ), qd ( ) represent the desired trajectories defined in the previous section. Using the above notations the controller is given by U nc = M(q) q e + C(q, q) q e + G(q) γ 1 s Ut T (q)u J = U nc, t t k 0 U t J = M(q) q e + C(q, q) q e + G(q) γ 1 s,, t > t k (1.9) 0 Uc J = U nc P d + K f (P q P d ) 1 In particular it is implicitly assumed that the functions F i ( ) in (1.1) are linearly independent. q m 1

18 Tracking control of multiconstraint nonsmooth Lagrangian systems 13 where γ 1 > 0 is a scalar gain, K f > 0, P q = D T λ and P d = D T λ d is the desired contact force during persistently constrained motion. It is clear that during Ω J k not all the constraints are active and, therefore, some components of λ and λ d are zero. In order to prove the stability of the closed-loop system (1.7) (1.9) we will use the following positive definite function: V (t, s, q) = 1 2 st M(q)s + γ 1 γ 2 q T q (1.10) 1.4 Tracking control framework Design of the desired trajectories In this chapter we treat the tracking control problem for the closed-loop dynamical system (1.7) (1.9) with the complete desired path a priori taking into account the complementarity conditions and the impacts. In order to define the desired trajectory let us consider the motion of a virtual and unconstrained particle perfectly following a trajectory (represented by X nc ( ) on Figure 1.2) with an orbit that leaves the admissible domain for a given period. Therefore, the orbit of the virtual particle can be split into two parts, one of them belonging to the admissible domain (inner part) and the other one outside the admissible domain (outer part). In the sequel we deal with the tracking control strategy when the desired trajectory is constructed such that: (i) when no activated constraints, it coincides with the trajectory of the virtual particle (the desired path and velocity are defined by the path and velocity of the virtual particle, respectively), (ii) when p m constraints are active, its orbit coincides with the projection of the outer part of the virtual particle s orbit on the surface of codimension p defined by the activated constraints (X d between A and C in Figure 1.2), (iii) the desired detachment moment and the moment when the virtual particle re-enters the admissible domain (with respect to p m constraints) are synchronized. Therefore we have not only to track a desired path but also to impose a desired velocity allowing the motion synchronization on the admissible domain. The main difficulties here consist of:

19 14 My Book Title stabilizing the system on Φ during the transition phases I k and incorporating the velocity jumps in the overall stability analysis; deactivating some constraints at the moment when the unconstrained trajectory re-enters the admissible domain with respect to them; maintaining a persistently constrained motion between the moment when the system was stabilized on Φ and the detachment moment. Remark 1.3. The problem can be relaxed considering that we want to track only a desired path like X nc ( ) (without imposing a desired velocity on the inner part of the desired trajectory and/or a given period to complete a cycle). In this way the synchronization problem (iii) disappears and we can assume there exists a twice differentiable desired trajectory outside [t k 0, tk f ] that assures the detachment when the force control is dropped. In other words, in this case we have to design the desired trajectory only during I k phases Design of q d ( ) and q d( ) on the phases I k During the transition phases the system must be stabilized on Φ. Obviously, this does not mean that all the constraints have to be activated (i.e. q1(t) i = 0, i = 1,..., m). Let us consider that only the first p constraints (eventually reordering the coordinates) define the border of Φ where the system must be stabilized. The following methodology will be used to define qd ( ): 1) During a small period δ > 0 chosen by the designer the desired velocity becomes zero preserving the twice differentiability of qd ( ). For instance we can use the following definition: ( qd (t) = qnc τ0 k + (t τ 0 k δ)2 (t τ0 k) ) δ 2, t [τ0 k, τ 0 k + δ] which means qd (τ 0 k + δ) = qd (τ 0 k ) = q nc (τ0 k ), q d (τ 0 k + δ) = 0 and q d (τ 0 k ) = q nc (τ0 k ) 2) The last n p components of qd ( ) are frozen: (qd ) n p (t) = qnc n p (τ 0 k ), t (τ 0 k + δ, tk f ] (1.11) 3) For a fixed ϕ > 0 the moment τ1 k is chosen by the designer as the instant when the limit conditions ( qd) i (τ k 1 ) = νv 1/3 (τ0 k), ( q d) i (τ k 1 ) = 0, i = 1,..., p, hold. On [τ0 k + δ, τ 1 k) we define q d ( ) as a twice differentiable decreasing signal. Precisely, denoting t = t (τ 0 k +δ) τ1 k (τ 0 k +δ), the components

20 Tracking control of multiconstraint nonsmooth Lagrangian systems 15 ( ) q i d ( ), i = 1,..., p of (q d ) p ( ) are defined as: { ( ) q i a i d (t) = 3 (t ) 3 + a i 2 (t ) 2 + a i 0, t [τ 0 k + δ, min{τ 1 k; tk 0 }] ϕv 1/3 (τ0 k), t (min{τ 1 k; tk 0 }, tk f ] (1.12) where V ( ) is defined in (1.10) and the coefficients are: a i 3 = 2[( q i) nc (τ k 0 ) + ϕv 1/3 (τ0 k)] a i 2 = 3[( q i) nc (τ k 0 ) + ϕv 1/3 (τ0 k)] a i 0 = ( q i) (1.13) nc (τ k 0 ) The rationale behind the choice of qd ( ) is on one hand to assure a robust stabilization on Φ, mimicking the bouncing-ball dynamics; on the other hand to enable one to compute suitable upper-bounds that will help using Proposition 1.1 (hence V 1/3 ( ) terms in (1.12) with V ( ) in (1.10)). Remark 1.4. Two different situations are possible. The first one is given by t k 0 > τ 1 k and we shall prove that in this situation all the jumps of the Lyapunov function in (1.10) are negative. The second situation was pointed out in [Bourgeot and Brogliato (5)] and is given by t k 0 < τ 1 k. In this situation the first jump at t k 0 in the Lyapunov function may be positive. It is noteworthy that qd ( ) will then have a jump at the time tk 0 since (qd i ) (t k+ 0 ) = ϕv 1/3 (τ0 k ), i = 1,..., p (see (1.12)). In order to limit the deformation of the desired trajectory qd ( ) w.r.t. the unconstrained trajectory q nc ( ) during the I k phases (see Figures 1.2), we impose in the sequel qp nc (τ 0 k ) ψ (1.14) where ψ > 0 is chosen by the designer. It is obvious that a smaller ψ leads to smaller deformation of the desired trajectory and to smaller deformation of the real trajectory as we shall see in Section 1.8. Nevertheless, due to the tracking error, ψ cannot be chosen zero. We also note that qp nc (τ0 k ) ψ is a practical way to choose τ0 k. During the transition phases I k we define (q d ) n p (t) = (qd ) n p (t). Assuming a finite accumulation period, the impact process can be considered in some way equivalent to a plastic impact. Therefore, (q d ) p ( ) and ( q d ) p ( ) are set to zero on the right of t k Design of the desired contact force during constraint phases For the sake of simplicity we consider the case of the constraint phase, J with J = {1,..., p}. Obviously a sufficiently large desired Ω J k

21 16 My Book Title contact force P d assures a constrained movement on Ω J k. Nevertheless at the end of the Ω J k phases a detachment from some surfaces Σ i has to take place. It is clear that a take-off implies not only a well-defined desired trajectory but also some small values of the corresponding contact force components. On the other hand, if the components of the desired contact force decrease too much a detachment can take place before the end of the Ω J k phases which can generate other impacts. Therefore we need a lower bound of the desired force which assures the contact during the Ω J k phases. Dropping the time argument, the dynamics of the system on Ω J k can be written as { M(q) q + F (q, q) = Uc + D T p λ p 0 q p λ p 0 (1.15) where F (q, q) = C(q, q) q + G(q) and D p = [I p. O] R p n. On Ω J k the system is permanently constrained which implies q p ( ) = 0 and q p ( ) = 0. In order to assure these conditions it is sufficient to have λ p > 0. ( ) C(q, q)p,p C(q, q) p,n p In the following let us denote C(q, q) = C(q, q) n p,p C(q, q) and n p,n p ( ) [M 1 (q)] p,p [M 1 (q)] p,n p M 1 (q) = [M 1 (q)] n p,p [M 1 (q)] where the meaning of each n p,n p component is obvious. Proposition 1.2. On Ω J k the constraint motion of the closed-loop system (1.15),(1.7),(1.9) is assured if the desired contact force is defined by (λ d ) p β M p,p (q) ( [M 1 (q)] p,p C p,n p (q, q)+ 1 + K f (1.16) + [M 1 (q)] p,n p C n p,n p (q, q) + γ 1 [M 1 ) (q)] p,n p sn p where M p,p (q) = ( ) [M 1 1 ( ) (q)] p,p = Dp M 1 (q)dp T 1 is the inverse of the Delassus matrix (see [Acary and Brogliato (1); Brogliato (6)] for the definition) and β R p, β > 0. Remark 1.5. The control law used in this chapter with the design of λ d described above leads to the following closed-loop dynamics on Ω J k. M p,n p (q)ṡ n p + C p,n p (q, q)s n p = (1 + K f )(λ λ d ) p M n p,n p (q)ṡ n p + C n p,n p (q, q)s n p + γ 1 s n p = 0 q p = 0, λ p = β. It is noteworthy that the closed-loop dynamics is nonlinear and therefore, we do not use the feedback stabilization proposed in [McClamroch and Wang (22)].

22 Tracking control of multiconstraint nonsmooth Lagrangian systems Strategy for take-off at the end of constraint phases Ω J k We have discussed in the previous sections the necessity of a trajectory with impacts in order to assure the robust stabilization on Φ in finite time and, the design of the desired trajectory to stabilize the system on Φ. Now, we are interested in finding the conditions on the control signal Uc J that assure the take-off at the end of the constrained phases Ω J k. We consider the phase Ω J k expressed as the time interval [tk f, tk d ). The dynamics on [tk f, tk d ) is given by (1.15) and the system is permanently constrained, which implies q p ( ) = 0 and q p ( ) = 0. Let us also consider that the first r constraints (r < p) have to be deactivated. Thus, the detachment takes place at t k d if q r (t k+ d ) > 0 which requires λ r(t k d ) = 0. The last p r constraints remain active which means λ p r (t k d ) > 0. To simplify the notation we drop the arguments t and q in many equations of this section. We decompose the LCP matrix as: ( ) (1 + K f )D p M 1 (q)dp = A1 (q) A 2 (q) A 2 (q) T A 3 (q) with A 1 R r r, A 2 R r (p r) and A 3 R (p r) (p r). Proposition 1.3. For the closed-loop system (1.15), (1.7), (1.9) the decrease of the active constraints number from p to p r (with r < p), is possible if ( (λd ) r (t k d ) ) ( (A1 A (λ d ) p r (t k d ) = 2 A 1 ) 1 ( ) 3 AT 2 br A 2 A 1 3 b p r) C1 C 2 + A 1 ( 3 bp r A T 2 (λ ) d) r (1.17) where b p b(q, q, U nc ) D p M 1 (q)[u nc F (q, q)] 0 and C 1 R r, C 2 R p r such that C 1 0, C 2 > Closed-loop stability analysis In the case Φ = R n, the function V (t, s, q) in (1.10) can be used in order to prove the closed-loop stability of the system (1.8), (1.9) (see for instance [Brogliato et al (8)]). In the case studied here (Φ R n ) the analysis becomes more complex.

23 18 My Book Title To simplify the notation V (t, s(t), q(t)) is denoted as V (t). In order to introduce the main result of this chapter we make the next assumption, which is verified in practice for dissipative systems. Assumption 1.1. The controller U t in (1.9) assures that all the transition phases are finite (see Definition 1.6) and the accumulation point t k is smaller than t k,1 d for all k N. Since outside [t k 0, t k f ] we will show that the Lyapunov function exponentially decreases, we may presume that all the impacts take place during I k. Lemma 1.1. Consider the closed-loop system (1.7)-(1.9) with (q d ) p( ) defined on the interval [τ k 0, t k 0] as in (1.12)-(1.11). Let us also suppose that condition b) of Proposition 1.1 is satisfied. The following inequalities hold: q(t k 0 ) V (τ0 k), s(t k 0 γ 1 γ ) 2 ( q(t k 0 ) 2 λ min (M(q)) + γ2 2V (τ0 k) λ min (M(q)) ) γ 1 V 1/2 (τ k 0 ) (1.18) Furthermore, if t k 0 τ k 1 and tk 0 is a p ɛ k -impact one has (q d ) p (t k 0 ) ɛ k + V (τ k 0 ) γ 1 γ 2 ( q d ) p (t k 0 ) K + K V 1/3 (τ k 0 ) (1.19) where ɛ k max{ψ, pϕv 1/3 (τ0 k) + V (τ k 0 ) γ 1γ 2 }, K = 6pψ and δ K = τ k 1 τ k 0 6 ( p 2 τ1 k τ 0 k δ + (1 + ) p)ϕ (ϕ + ψ) + ψϕ γ1 γ 2 The main result of this chapter can be stated as follows. Theorem 1.1. Let Assumption 1 hold, e [0, 1) and (qd ) p( ) defined as in (1.12)-(1.11). The closed-loop system (1.7)-(1.9) initialized on Ω 0 such that V (τ0 0 ) 1, satisfies the requirements of Proposition 1.1 and is therefore practically weakly stable with the closed-loop state x( ) = [s( ), q( )] and R =

24 Tracking control of multiconstraint nonsmooth Lagrangian systems 19 e γ(tk f tk ) (1 + K 1 + K 2 + ξ)/ρ where ρ = min{λ min (M(q))/2; γ 1 γ 2 } K 1 = 3γ 1 γ 2 ϕ(ψ + pϕ + 1 γ1 γ2 ) [ K 2 = λ max (M(q)) 3KK (K ) 2 2γ 2 + γ 2 ψk + + 4γ 2 λ min (M(q))γ 1 + (K + K) ( γ 2 pϕ + 3 γ2 γ 1 + ( 2 + γ 2 λ min (M(q)) + 3 γ2 + ψγ 2 (2 + γ 1 γ2 γ 1 ) 2 λ min (M(q)) + K ) 2 λ min (M(q)) (ψ + pϕ) + γ2 2 ϕ2 p 2 ) ] + γ 2 2ψϕ p Remark 1.6. Since the closed-loop system (1.7)-(1.9) satisfies the requirements of Proposition 1.1 one also deduces V (τ0 k) δ(γ, ξ), so ɛ k max{ψ, pϕδ(γ, ξ) 1/3 + δ(γ,ξ) γ 1γ 2 }, k 1. In other words the sequence {ɛ k } k is uniformly upperbounded and the upperbound can be decreased by adjusting the parameters ψ and γ. γ Illustrative example Let us consider a two-link planar manipulator with admissible domain Φ = {(x, y) y 0, 0.7 x 0}. Let us also consider an unconstrained desired trajectory given by the circle {(x, y) (x 0.7) 2 + y 2 = 0.5} that violates both constraints. In other words, the two-link planar manipulator must track a quarter-circle; stabilize on and then follow the line Σ 1 = {(x, y) y = 0}; stabilize on the intersection of Σ 1 and Σ 2 = {(x, y) x = 0.7}; detach from Σ 1 and follow Σ 2 until the unconstrained circle re-enters Φ and finally take-off from Σ 2 in order to repeat the previous steps. Therefore, we have: R + = Ω 0 I 1 Ω J1,1 1 I 2 Ω J2,1 2 Ω J2,2 2 Ω J2,3 2 I 3 Ω J3,1 3 I 4... with J 1,1 = {1}, J 2,1 = {1, 2}, J 2,2 = {2}, J 2,3 =, etc. We note that during I 2k+1 the system is stabilized on Σ 1 (1-impacts) while during I 2k the system is stabilized on Σ 1 Σ 2 (2 ɛk impacts). We define task1= Ω 0 I 1 Ω J1,1 1 I 2 Ω J2,1 2 Ω J2,2 2 and task2= Ω J2,3 2 I 3 Ω J3,1 3 I 4 Ω J4,1 4 Ω J4,2 4. Each task actually corresponds to a complete tracking of the quarter-circle. The numerical values used for the dynamical model are again l 1 = l 2 =

25 20 My Book Title 0.5m, I 1 = I 2 = 1kg.m 2, m 1 = m 2 = 1kg and the restitution coefficient e = 0.7. The impacts are imposed by ϕ = 100 in (1.12) (1.13) and the beginning of transition phases are defined using ψ = 0.05 in (1.14). We impose a period of 10 seconds for two consecutive cycles and we simulate the dynamics during 60 seconds. The simulations have been performed with the siconos platform available at: To evaluate the performances of the controller, two criteria will be used:c 1 = max t T 2 (q(t))u(t) and C 2 = q(t) 2 + q(t) 2 dt where = [0, t 2,2 ] for task1 and = [t2,2 ] for task2. d d, t4,2 d U1max U2max Umax gamma2 Fig. 1.5 C 1 (γ 2 ) on task1 (solid) and task2 (dashed) with γ 1 = 25 From Figures one deduces that the best performances in terms of both criteria are obtained when γ 2 is small (around 10) and γ 1 γ 2 is around 80. However, if γ 2 is too small then Assumption 1.1 is violated. It is noteworthy that the both criteria significantly decrease from task1 to task Conclusions In this chapter we have proposed a methodology to study the tracking control of fully actuated Lagrangian systems subject to multiple frictionless unilateral constraints and multiple impacts. The main contribution of the work is twofold: first, it formulates a general control framework and second,

26 Tracking control of multiconstraint nonsmooth Lagrangian systems 21 Critère gamma2 Critère 3 10 x gamma2 Fig. 1.6 Left: C 2 (γ 2 ) on task1; Right: C 2 (γ 2 ) on task2 with γ 1 = U1max U2max Umax gamma1 Fig. 1.7 C 1 (γ 1 ) on task1 (solid) and task2 (dashed) with γ 1 γ 2 = 800 it provides a stability analysis for the class of systems under consideration. It is noteworthy that even in the simplest case of only one frictionless unilateral constraint the chapter already presents some notable improvements with respect to the existing works. Precisely, the stability analysis result is significantly more general than those presented in [Bourgeot and Brogliato (5)] and [Brogliato et al (9)] and, each element entering the dynamics (desired trajectory, contact force) is explicitly defined. Numerical simulations

27 22 My Book Title Critère Critère x gamma gamma1 Fig. 1.8 Left: C 2 (γ 1 ) on task1; Right: C 2 (γ 1 ) on task2 with γ 1 γ 2 = 800 are done with the Siconos software platform [Acary and Brogliato (1)] in order to illustrate the results.

28 Bibliography Acary, V. and Brogliato, B.: Numerical Methods for Nonsmooth Dynamical Systems, Lecture Notes in Applied and Computational Mechanics, vol 35, Springer- Verlag, Berlin Heidelberg, (2008). Acary, V. and Pérignon, F.: An introduction to Siconos, INRIA Technical report RT 340, (2007) Siconos open-source software available at: Ballard, P.: Formulation and well-posedness of the dynamics of rigid body systems with perfect unilateral constraints, Phil. Trans. R. Soc. Lond. A359, , (2001). Bentsman, J. and Miller, B. M.: Dynamical systems with active singularities of elastic type: A modeling and controller synthesis framework, IEEE Transactions on Automatic Control, vol.52, No.1, 39-55, (2007). Bourgeot, J.-M. and Brogliato, B.:Tracking control of complementarity lagrangian systems, International Journal of Bifurcation and Chaos, 15(6), , (2005). Brogliato, B.: Nonsmooth Mechanics, Springer CCES, London, (1999), 2nd Ed. Brogliato, B.: Absolute stability and the Lagrange-Dirichlet theorem with monotone multivalued mappings, Systems and Control Letters, vol.51, , Brogliato, B., Lozano, R., Maschke, B. and Egeland, O.: Dissipative Systems Analysis and Control. Theory and Applications, Springer CCES, London, (2007), 2nd Ed. Brogliato, B., Niculescu, S.-I., Orhant, P.: On the control of finite-dimensional mechanical systems with unilateral constraints, IEEE Trans. Autom. Contr. 42(2), , (1997). Facchinei, F. and Pang, J.S.: Finite-dimensional Variational Inequalities and Complementarity Problems, Springer Series in Operations Research, New-York, (2003). Faulring, E.L. and Lynch, K.M. and Colgate, J.E. and Peshkin, M.A: Haptic display of constrained dynamic systems via admittance displays, IEEE Transactions on Robotics, vol.23, No.1, , (2007). Galeani, S., Menini, L., Potini, A. and Tornambè, A.: Trajectory tracking for a particle in elliptical billiards, International Journal of Control, vol.81(2),

29 24 My Book Title 213, (2008). Glocker, C.: Set Value Force Laws: Dynamics of Non-Smooth Systems Lecture Notes in Applied Mechanics, Vol.1, Springer, (2001). Glocker, C.: Concepts for Modeling Impacts without Friction, Acta Mechanica, Vol. 168, 1-19, (2004). Huang, H. P. and McClamroch, N. H.: Time optimal control for a robotic contour following problem, IEEE J. Robotics and Automation, Vol. 4, , (1988). Lee, E. and Park, J. and Loparo, K.A. and Schrader, C.B. and Chang, P.H.: Bang-bang impact control using hybrid impedance/time-delay control, IEEE/ASME Transactions on Mechatronics, vol.8, No.2, , (2003). Leine, R.I. and van de Wouw, N.: Stability properties of equilibrium sets of nonlinear mechanical systems with dry friction and impact, Nonlinear Dynamics, vol.51(4), , (2008). Leine, R.I. and van de Wouw, N.: Uniform convergence of monotone measure differential inclusions: with application to the control of mechanical systems with unilateral constraints, Int. Journal of Bifurcation and Chaos, vol.18 (5), , (2008). Leine, R.I. and van de Wouw, N.: Stability and Convergence of Mechanical Systems with Unilateral Constraints, Lecture Notes in Applied and Computational Mechanics, vol.36, Springer Verlag Berlin Heidelberg, (2008). Hiriart-Urruty J.B. and Lemaréchal, C.: Fundamentals of Convex Analysis, Springer-Verlag, Grundlehren Texts in Mathematics, (2001). Mabrouk, M.: A unified variational model for the dynamics of perfect unilateral constraints, European Journal of Mechanics A/Solids, 17(5), , (1998). McClamroch, N.H. and Wang, D.: Feedback stabilization and tracking of constrained robots, IEEE Trans. Autom. Contr. 33, , (1988). Menini, L. and Tornambè, A.: Asymptotic tracking of periodic trajectories for a simple mechanical system subject to nonsmooth impacts, IEEE Trans. Autom. Contr. 46, , (2001). Menini, L. and Tornambè, A: Exponential and BIBS stabilisation of one degree of freedom mechanical system subject to single non-smooth impacts, IEE Proc.- Control Theory Appl. Vol.148, No. 2, , (2001). Menini, L. and Tornambè, A: Dynamic position feedback stabilisation of multidegrees-of-freedom linear mechanical system subject to nonsmooth impacts, IEE Proc.- Control Theory Appl. Vol.148, No. 6, , (2001). Moreau, J. J: Unilateral contact and dry friction in finite freedom dynamics, Nonsmooth Mechanics and Applications, CISM Courses and Lectures, Vol. 302 (Springer-Verlag), (1988). Pagilla, P.R.: Control of contact problem in constrained Euler-Lagrange cystems, IEEE Transactions on Automatic Control, vol.46, No. 10, , (2001). Pagilla, P.R. and Yu, B. An Experimental Study of Planar Impact of a Robot Manipulator, IEEE/ASME Transactions on Mechatronics, vol.9, No. 1, , (2004). Sekhavat, P. and Sepehri, N. and Wu, Q.: Impact stabilizing controller for hydraulic actuators with friction: Theory and experiments, Control Engineering Practice, vol.14, , (2006).

30 Bibliography 25 Slotine, J. J. and Li, W.: Adaptive manipulator control: A case study, IEEE Trans. Automatic Control, vol.33, , (1988). Tornambé, A.: Modeling and control of impact in mechanical systems: Theory and experimental results, IEEE Transactions on Automatic Control, vol.44, No. 2, , (1999). van Vliet, J. and Sharf, I. and Ma, O.: Experimental validation of contact dynamics simulation of constrained robotic tasks, International Journal of Robotics Research, vol.19, No.12, , (2000). Volpe, R. and and Khosla, P.: A theoretical and experimental investigation of impact control for manipulators, International Journal of Robotics Research, vol.12, No.4, , (1993). Xu, W.L.and Han, J.D. and Tso, S.K.: Experimental study of contact transition control incorporating joint acceleration feedback, IEEE/ASME Transactions on Mechatronics, vol.5, No.3, , (2000).