TRAJECTORY TRACKING FOR FULLY ACTUATED MECHANICAL SYSTEMS

Transcription

1 TRAJECTORY TRACKING FOR FULLY ACTUATED MECHANICAL SYSTEMS Francesco Bullo Richar M. Murray Control an Dynamical Systems California Institute of Technology Pasaena, CA Fax : bullo@inra.caltech.eu murray@inra.caltech.eu Keywors : mechanical systems, nonlinear control, Lie groups, Riemannian geometry Abstract We present a general framework for the control of Lagrangian systems with as many inputs as egrees of freeom. Relying on the geometry of mechanical systems on manifols, we propose a esign algorithm for the tracking problem. The notions of error function an transport map lea to a proper efinition of configuration an velocity error. These are the crucial ingreients in esigning a proportional erivative feeback an feeforwar controller. The propose approach inclues as special cases a variety of results on control of manipulators, pointing evices an autonomous vehicles. 1. Introuction Mechanical control systems provie an important an challenging research area that falls between the stuy of classical mechanics an moern nonlinear control theory. From a theoretical viewpoint, the geometric structure of mechanical systems gives way to stronger control algorithms than those obtaine for generic nonlinear systems. Some results in this area are surveye for example in [12] an [10]. The riving applications are motion control problems in unerwater an aerospace environments, where relevant Lagrangian moels are available an a nonlinear analysis can successfully exploit this structure. This paper eals with the trajectory tracking problem for a class of Lagrangian systems. The control objective is to track a trajectory with exponential convergence rates in orer to guarantee performance an robustness. The mechanical systems we consier have Lagrangian equal to the kinetic energy an are fully actuate, that is, they have as many inepenent input forces as egrees of freeom. A wie variety of aerospace an unerwater vehicles, as well as robot manipulators, fulfill these assumptions. The main emphasis in this paper is on the fact that the configuration space of these systems is a generic manifol Q. The tracking problem for robot manipulators has receive much attention in the literature. Examples are the contributions in [16], [13] an [17], where asymptotic an exponential tracking are achieve via a nonlinear analysis. These results are now stanar in textbooks on control [12] an robotics [11]. Since then, similar techniques have been applie to the attitue control problem for satellites [18], an likewise to the attitue an position control for unerwater vehicles [5, Section 4.5.4]. A common feature in all these works is the preliminary choice of a parametrization, that is a choice of coorinates for the configuration manifol. The synthesis of both control law an relative Lyapunov function is performe in this specific chart. In this paper we propose a general framework that relies on the geometry of mechanical systems on manifols. In the spirit of [6], our approach avois the parametrization step an focuses irectly on how to efine a configuration an a velocity error on a manifol. The notions of error function an transport map yiel to a coorinatefree efinition of ifferences errors between configurations an between velocities. Aitionally, an intrinsic efinition for the feeforwar control is obtaine using the theory of Riemannian connections. These ieas lea to a coorinate inepenent system esign. The resulting control law, even though expresse in a specific set of coorinates, is not biase by that choice. Global stability is ensure as long as error function an transport map satisfy a compatibility conition. The resulting esign algorithm can then be applie to a variety of examples. We treat here only the case of a robot manipulator on R n an of a satellite on the group of rotations SO3. We refer to the report [3] for an integral version of this work, incluing the instructive case of an unerwater vehicle on the group of rotations an translations SE3. The paper is organize as follows. Section 2 reviews some concepts from Riemannian geometry an from mechanical systems. In Section 3 we efine error function an transport map. The main theorem is presente in Section 4 an two examples are iscusse in Section 5.

2 2. Mathematical Preliminaries In this section we introuce the mathematical machinery neee for the remainer of the paper. For an introuction to Riemannian geometry we refer to [2] an [4]. For an introuction to mechanics we refer to [8] Elements of Riemannian geometry A Riemannian metric on a manifol Q is a smooth map that associates to each tangent space T q Q an inner prouct, q. An affine connection on Q is a smooth map that assigns to each pair of smooth vector fiels X, Y a smooth vector fiel X Y such that for all smooth functions f on Q 1. fx Y = f X Y, an 2. X fy = f X Y + L X fy where L X f enotes the Lie erivative of f with respect to X. In system of local coorinates q 1,...,q n we efine the Christoffel symbols by q i q j = Γ k ij q k. where the summation convention is enforce here an in what follows. Given any three vector fiels X, Y, Z on Q, we say that the affine connection on Q is torsion-free if [X, Y ] = X Y Y X an is compatible with the metric, if L X Y, Z = X Y, Z + Y, X Z. The Levi-Civita theorem states that on the Riemannian manifol Q there exists a unique affine connection, which is torsion-free an compatible with the metric. We call this the Riemannian connection on Q. We conclue with two useful efinitions. Given a real value function f on Q, the graient of f is the vector fiel f such that f, X L X f. Given a one form ω an a vector fiel X, the covariant erivative of ω with respect to X is the one form X ω such that for all vector fiels Y. X ω Y = L X ω Y ω X Y, 2.2. Computing covariant erivatives Loosely speaking, covariant erivatives are irectional erivatives of quantities efine on manifols. Equation 1 relates them to the notion of Lie ifferentiation, whereas equation 1 plays the role of the Leibniz rule. In the following we present some useful approaches on how to compute covariant erivatives. A first instructive case is when the manifol Q is a submanifol of R n an the Riemannian metric on Q is the one inuce by the Eucliean metric on R n. Then we can enote with π q the orthogonal projection from R n onto the tangent bunle T q Q. Given any two vector fiels X, Y on Q, it hols that X Y q 0 = π q0 Y qt, t t=0 where {qt, t R} is any curve on Q with q0 = q 0 an q0 = Xq 0. We refer to [2, Chapter VII] for more etails on this escription of covariant ifferentiation. In the general case, e.g. whenever the previous assumptions are not satisfie, we can express covariant erivatives in a system of local coorinates. The Christoffel symbols Γ k ij of a Riemannian connection can be compute as follows. Denoting with M ij = q, i q, we have j Γ k ij = 1 2 Mmk Mmj q i + M mi q j M ij q m, where {M ij } is the inverse of the tensor M. The covariant erivative of a vector fiel is then written as Y i X Y = q j Xj + Γ i jk Xj Y k q i Mechanical systems in a Riemannian context Here we escribe a mechanical system an its equations of motion in a coorinate free fashion. Key ieas are regaring the system s kinetic energy as a Riemannian metric an writing the Euler-Lagrange s equations in terms of the associate Riemannian connection. For a more complete treatment, see for example [7]. We start with some efinitions. A simple mechanical control system is efine by a Riemannian metric on a configuration manifol Q efining the kinetic energy, a function V on Q efining the potential energy, an m one-forms, F 1,..., F m, on Q efining the inputs. A simple mechanical system is sai to be fully actuate if for all q Q, the family of vectors {F 1 q,..., F m q} spans the whole vector space Tq Q, that is if there exists an inepenent input one form corresponing to each egree of freeom. Let M q : T q Q Tq Q enote the metric tensor associate to the kinetic energy an the corresponing Riemannian connection. Let qt Q be the configuration of the system an qt T q Q its velocity. Using the formalism introuce in the previous section, the force Euler-Lagrange equations can be written as q q = M 1 q V q + Ft, q, q, 1 where V q is the ifferential of the potential function V an where the resultant force Fq, t = F a qu a t is the input. In a system of local coorinates q 1,...,q n the previous equations rea q i + Γ i jk q j q k = M ij V q j + F j.

3 Note that the Euler-Lagrange s equations are coorinate inepenent intrinsic, in the sense that they are satisfie in every system of local coorinates. We shall say that the mechanical system 1 has boune inertia if there exist m 1 m 2 > 0 such that for all q Q it hols that m 1 sup Mq M q Mq inf q Q q Q M q M q Mq m 2, B1 where Mq is the operator norm on the inner prouct space T q Q, M q. Here an in the following, the tag Bn enotes some bouneness assumptions which will play a crucial role in later sections. 3. Geometric Description of Configuration an Velocity Error In this section we stuy the geometric objects involve in the controller esign. To measure the istance between reference an actual configuration, we introuce the notion of error function. To measure the istance between reference an actual velocity, we introuce the notion of transport map. A esign on two sphere manifol provies an example of our efinitions. Finally we stuy the time erivative of the transport map. Together with a issipation function, these ingreients are crucial in esigning a tracking control Error function an configuration error Let ϕ be a smooth real value function on Q Q. We shall call ϕ an error function if it is positive efinite, that is ϕq, r 0 for all q an r, an ϕq, r = 0 if an only if q = r. We shall say that the error function ϕ is symmetric, if ϕq, r = ϕr, q for all q an r. Let 1 ϕ an 2 ϕ enote the graient of ϕq, r with respect to its first an secon argument. We shall say that the error function ϕ is uniformly quaratic with constant L if for all ɛ > 0 there exist two constants b 1 b 2 > 0 such that ϕq, r < L ɛ implies b 1 1 ϕq, r 2 Mq ϕq, r b 2 1 ϕq, r 2 Mq. A1 Here an in the following, the tag An enotes some esign assumptions which will play a crucial role in later sections. When q an r are actual an reference configuration, we will sometimes call the quantity ϕq, r configuration error. As mentione above, the error function ϕ will be instrumental in esigning the proportional action Transport map an velocity error Given two points q, r Q, we shall call a linear map T q,r : T r Q T q Q a transport map if it is compatible with the error function, that is if 2 ϕq, r = T q,r 1ϕq, r, A2 where T q,r : T q Q T r Q is the ual map of T q,r. The transport map T is also require to be smooth, i.e., for all point r in Q an tangent vectors Y r in T r Q, the vector fiel T q,r Y r is smooth. Using a transport map, velocities belonging to ifferent tangent bunles can be compare. In the following, we shall call velocity error the quantity ė q T q,r ṙ T q Q. Note the slight abuse of terminology, given that the velocity error is not the time erivative of a position error. Also note that since the efinition of T an ė are equivalent, we will sometimes talk about compatibility between configuration an velocity errors. The next lemma provies some insight into the meaning of the velocity error an of conition A2. Lemma 1. Time erivative of an error function Let {qt, t R + } an {rt, t R + } be two smooth curves in Q. Let ϕ be an error function an T a compatible transport map. Then t ϕ qt, rt = 1 ϕ qt, rt ėt, t R +. The result can be restate as follows. As both q an r are functions of time, the time erivative of ϕ : Q Q R reuces to a erivative only with respect to the first argument L q, ṙ ϕ = L ė, 0 ϕ, where X, Y enotes a vector fiel on the prouct manifol Q Q. Last, we introuce the notion of issipation function, which will be useful in efining a erivative action. We efine a linear Rayleigh issipation function as a smooth, self-ajoint, positive efinite tensor fiel K q : T q Q T q Q. We shall say that K is boune if there exist 2 1 > 0 such that 2 sup K Mq inf K Mq 1, q Q q Q B2 where M is the operator norm for 1, 1 type tensors on T q Q inuce by the metric M q on T q Q Derivatives of the transport map an bouneness assumptions So far we have introuce configuration an velocity errors that will be key ingreients in esigning a proportional an erivative feeback in the next section. We now stuy how the transporte reference velocity T q,r ṙ varies as a function of both qt an r, ṙt. This will be useful in esigning the feeforwar action. We enote the total erivative of T q,r ṙ with D T ṙ t = q T ṙ + T ṙ, where the two terms are escribe as follows:

4 1. At r, ṙ fixe, T q,r ṙ is a vector fiel on Q an therefore its covariant erivative q T ṙ is well-efine on Q. We call covariant erivative of the transport map the map T : T q Q T r Q T q Q efine as X T Y r X T Y r, for all tangent vectors X T q Q an Y r T r Q. 2. At q fixe, T q,r ṙ is a vector on the vector space T q Q an therefore its time erivative is well-efine. We enote it with the symbol: T ṙ T q Q. We conclue the section with some bouneness assumptions. We shall say that the transport map T has boune covariant erivative an that the error function ϕ has boune secon covariant erivative if an sup T q,r M <, q,r Q Q sup 1 ϕq, r M <, q,r Q Q B3 B4 where M is the inuce operator norm on the inner prouct space T q Q, M q. We shall say that the twice ifferentiable curve {rt, t R + } Q is a reference trajectory with boune time erivative if sup ṙ Mr <. t R B5 Notice that a sufficient conition for the bouns B3 an B4 to hol, is that the quantities Tα i/ qk, 2 ϕ/ q i q j an Γ k ij q are boune. 4. Tracking on Manifols In this section we state an solve the exponential tracking problem. Since we are ealing with secon orer systems on a manifol, special care is neee in efining Lyapunov an exponential stability. We express the latter notions in terms of a total energy function, efine as the sum of a generalize potential the configuration error an kinetic energy the norm of the velocity error: W total q, q; r, ṙ ϕq, r ė 2 Mq. The control goal is to rive the total energy to zero, since at W total = 0 the state qt tracks the trajectory rt exactly. This is state as follows: Problem 2. Control objective Given a reference trajectory {rt, t R + }, esign a control law F = Fq, q; r, ṙ such that the total energy function W total is exponentially convergent to zero. Recall that by exponential convergence for W total we mean the existence of two positive constants k an λ such that W total t k W total 0e λt, where we write W total t for W total qt, qt; rt, ṙt. We are now reay to state the main result. Theorem 3. Exponential tracking Consier the mechanical system q q = M 1 q F, q Q an let the twice ifferentiable curve {rt, t R + } be a reference trajectory. Let ϕ be an error function, T be a transport map satisfying the compatibility conition A2 an K be a issipation function. If the control input is efine as F = F PD + F FF with F PD q, q; r, ṙ = 1 ϕq, r K ė F FF q, q; r, ṙ = M q q T q,r ṙ + Tq,r ṙ, then the curve qt = rt is Lyapunov stable, in the sense that W total t W total 0 from all initial conitions q0, q0. In aition, if the error function ϕ satisfies the quaratic assumption A1 with a constant L, an if the bouneness assumptions B1 B5 hol, then W total t converges exponentially to zero, from all initial conitions q0, q0 such that ϕ q0, r ė0 2 M < L. While similar results are quite common an wellestablishe in the robotics literature, note that the contribution of the previous theorem lies in a coorinate free esign performe on a generic manifol. We refer to the report [3] for the proof, but we inclue a few remarks in the following. The esign process an the theorem s results are global in the reference position rt but only local in the configuration q the error function ϕq, r must remain smaller than the parameter L. This cannot be avoie because of possible topological properties of the manifol Q. For aitional etails we refer to [6], where the global aspects of the point stabilization problem are iscusse. By constraining the choices of amissible couples ϕ, T, the compatibility conition A2 affects important esign aspects of F PD an F FF. For example, one particular transport map might generate a simple velocity error an a simple feeforwar control, but it might also require a complicate error function. The next section contains some examples of this traeoff. As expecte, the final control law is sum of a feeback an a feeforwar term. This is in agreement with the ieas expose in [10] on two egree of freeom system esign for mechanical systems. While the feeforwar term epens on the geometry of both the manifol an the mechanical system, the feeback term is esigne knowing

5 only the configuration manifol Q. We expect the ieas of configuration an velocity error to be relevant for more general secon orer nonlinear systems on manifols. 5. Applications an Extensions We present only two applications of Theorem 3 an we refer to the report [3] for aitional examples an etails A robot manipulator on R n In this section, we shall recover the stanar results on tracking control of manipulators containe in [11]. Let q R n be the joint variables an Mq be the inertia matrix of the manipulator. The esign escribe in Section 3 is performe as follows. Let ϕq, r = 1 2 q rt K p q r be the quaratic error function an as T q R n = T r R n, let the transport map be equal to the ientity matrix: T q,r = I n. Assumptions A1 an A2 are easily verifie. To esign the feeforwar action, we compute the covariant erivative { of T. Let q,..., 1 q n } be the stanar basis in R n, let {i, j, k,...} be inices over q an {α, β,...} be inices over r. Then one can show that I n i αj = I n i α q j + Γ i jk I n k α = Γi jα, Therefore, in contrast to a naive guess, the covariant erivative of the ientity map is ifferent from zero. The control law is F PD = K p q r K q ṙ F FF = Mq q I n ṙ + ṙ = Mq Γ i jα q j ṙ α + r Mq r + Cq, qṙ, q i where C, is the Coriolis matrix typically encountere in robotics. The control law F = F PD + F FF agrees with the one presente in [11, Chapter 4, Section 5.3] uner the name of augmente PD control. The assumptions B1 B5 can be written in terms of M q, Γ k ij an ṙ being boune over t R an q R n A satellite on the rotation group SO3 In the next two sections we esign tracking controllers for mechanical systems efine on the group of rotations SO3 an on the group of rigi motions SE3. We focus on rigi boies with boy-fixe forces an invariant kinetic energy, as satellites an unerwater vehicles. Nevertheless our treatment is relevant also for workspace control of robot manipulators. This section presents the attitue control problem for a satellite. The configuration of the satellite rigi boy is the rotation matrix R representing the position of a frame fixe with the rigi boy with respect to an inertially fixe frame. A rotation matrix on R 3 is an element on the special orthogonal group SO3 = {R R 3 3 RR T = I 3, etr = +1}. The kinematic equation escribing the evolution of Rt is Ṙ = R Ω where Ω R 3 is the boy angular velocity expresse in the boy frame. Recall that the matrix Ω is efine such that Ωx = Ω x for all x R 3 an it belongs to the space of skew symmetric matrices so3 = {S R 3 3 S T = S}. We refer to [11] for aitional etails. The kinetic energy of the rigi boy is 1 2 ΩT JΩ, where the inertia matrix J is symmetric an positive efinite. The Euler equations escribing the time evolution of Ω are J Ω = JΩ Ω + f, 2 where f R 3 is the resultant torque acting on the boy. Error functions Let {R t, t R + } enote the reference attitue trajectory corresponing to a esire or reference frame an let Ω = R T Ṙ enote the reference velocity in the reference frame. Using the group operation, we efine right an left attitue errors as R e,r R T R an R e,l RR T. 3 The matrix R e,r is the relative rotation from the boy frame to the reference frame. Two error functions are then efine as ϕ r R, R φr e,r an ϕ l R, R φr e,l, where φ : SO3 R + is efine as [6] φr e 1 2 tr K p I 3 R e. If the eigenvalues {k 1, k 2, k 3 } of the symmetric matrix K p satisfy k i + k j > 0 for i j, then both error functions ϕ l an ϕ R are symmetric, positive efinite an quaratic with constant L = min i j k i +k j. Locally near the ientity the function φ assigns a weight k 2 + k 3 to a rotation error about the first axis an similarly for the other axes. Velocity errors To efine compatible velocity errors, we compute the time erivative of the two error functions. Let the matrix skewa enote 1 2 A AT an let enote the inverse operator to : R 3 so3. We have t ϕ r = skewk p R e,r T Ω e,r 4 t ϕ l = skewk p R e,l T R Ω e,l, 5 where we efine right an left velocity errors in the boy frame as Ω e,r Ω R T e,rω an Ω e,l Ω Ω. Note the slightly improper woring, since a velocity error ė = Ṙ T Ṙ lives on the tangent bunle T R SO3. A precise statement is ė l = RΩ e,l Ṙ RR T Ṙ ė r = RΩ e,r Ṙ Ṙ R T R.

6 These equalities also motivate the names left an right. A left right velocity error is obtaine by left right translation of the velocity Ṙ. Next we escribe compatible couples of configuration an velocity errors. Equation 4 suggests that a right attitue error R T R an a right velocity error Ω RT R Ω are compatible. This couple is the most common choice in the literature, see for example [9], [6], an [18]. Left attitue an velocity error appear less frequently. With this choice both the velocity error an, as we show below, the feeforwar control have a simple expression. Remarkably, when the gain K p is a scalar multiple of the ientity k p I 3, the left an right error functions are equal an it is possible to use ϕ e,r with Ω e,l. Finally we summarize the esign process. Lemma 4. Consier the system in equation 2. Let {R t, t R + } enote the reference trajectory an let Ω = R T Ṙ enote its boy-fixe velocity. Corresponing to the two choices of attitue error, we efine f r = skewk p R e,r K Ω e,r + Ω JR T e,rω +JR T e,r Ω f l = R T skewk pr e,l K Ω e,l + Ω JΩ + J Ω where K is a positive efinite matrix an K p is a symmetric matrix with eigenvalues {k 1, k 2, k 3 } such that k i +k j > 0 for i j. Then, for both choices of attitue error, the total energy φr e Ω e 2 J converges exponentially to zero from all initial conitions R0, Ω0 such that φr e Ω e0 2 J < min i j k i + k j. 6. Summary an Conclusions This work unveils the geometry an the mechanics of the tracking problem for fully actuate Lagrangian systems. The esign process in Section 3 allows us to characterize in an intrinsic way a tracking controller. The basic answere questions concern how to efine configuration an velocity errors an how to compute the feeforwar control. Almost global stability an local exponential convergence are proven in full generality. Our framework successfully unifies a variety of examples. We have here presente only two of them an we refer to the report [3] for an integral version of this work, incluing proofs an aitional etaile examples. This work provies coorinate free esign techniques for nonlinear mechanical systems. Other recent papers on moeling [1], controllability [7] an ynamic feeback linearization [14] share the same geometric tools. A parallel avenue of research relies on the Hamiltonian formulation of mechanical systems, see for example [12] an [15]. These geometric techniques are a promising starting point in the esign of control policies for uneractuate systems. Acknowlegments The first author woul like to thank Dr. Anrew D. Lewis an Professor Jerrol E. Marsen for helpful iscussions. This work was supporte in part by the National Science Founation uner Grant CMS References [1] A. M. Bloch an P. E. Crouch. Nonholonomic control systems on Riemannian manifols. SIAM Journal of Control an Optimization, 331: , January [2] W. M. Boothby. An Introuction to Differentiable Manifols an Riemannian Geometry. Acaemic Press, New York, secon eition, [3] F. Bullo an R. M. Murray. Tracking for fully actuate mechanical systems: a geometric framework. Technical Report Caltech/CDS , California Institute of Technology, January Moifie version submitte to Automatica. Available electronically via [4] M. P. Do Carmo. Riemannian Geometry. Birkhäuser, Boston, [5] T. I. Fossen. Guiance an Control of Ocean Vehicles. John Wiley an Sons, New York, NY, [6] D. E. Koitschek. The application of total energy as a Lyapunov function for mechanical control systems. In J. E. Marsen, P. S. Krishnaprasa, an J. C. Simo, eitors, Dynamics an Control of Multiboy Systems, volume 97, pages AMS, [7] A. D. Lewis an R. M. Murray. Controllability of simple mechanical control systems. SIAM Journal of Control an Optimization, 353, May [8] J. E. Marsen an T. S. Ratiu. Introuction to Mechanics an Symmetry. Springer Verlag, New York, NY, [9] G. Meyer. Design an global analysis of spacecraft attitue control systems. Technical report, NASA, March Tech. Report R-361. [10] R. M. Murray. Nonlinear control of mechanical systems: a Lagrangian perspective. In IFAC Symposium on Nonlinear Control Systems NOLCOS, pages , Lake Tahoe, CA, June [11] R. M. Murray, Z. X. Li, an S. S. Sastry. A Mathematical Introuction to Robotic Manipulation. CRC Press, Boca Raton, Floria, [12] H. Nijmeijer an A. van er Schaft. Nonlinear Dynamical Control Systems. Springer Verlag, New York, NY, [13] B. Paen an R. Panja. Globally asymptotically stable PD+ controller for robot manipulators. International Journal of Control, 476: , [14] M. Rathinam an R. M. Murray. Configuration flatness of Lagrangian systems uneractuate by one control. SIAM Journal of Control an Optimization, To appear. [15] J. C. Simo, D. R. Lewis, an J. E. Marsen. Stability of relative equilibria I: The reuce energy momentum metho. Archive for Rational Mechanics an Analysis, 115:15 59, [16] M. Takegaki an S. Arimoto. A new feeback metho for ynamic control of manipulators. Journal of Dynamic Systems, Measurement, an Control, 102: , June [17] J. T.-Y. Wen an D. S. Bayar. A new class of control laws for robotic manipulators. Part I: Non aaptive case. International Journal of Control, 475: , [18] J. T.-Y. Wen an K. Kreutz-Delgao. The attitue control problem. IEEE Transactions on Automatic Control, AC- 36: , October 1991.