PROPORTIONAL DERIVATIVE (PD) CONTROL ON THE EUCLIDEAN GROUP F. BULLO AND R. M. MURRAY. Division of Engineering and Applied Science

Transcription

1 PROPORTIONAL DERIVATIVE (PD) CONTROL ON THE EUCLIDEAN GROUP F. BULLO AND R. M. MURRAY Division of Engineering an Applie Science California Institute of Technology Pasaena, CA 95 CDS Technical Report 95- August, 995 Abstract. In this paper we stuy the stabilization problem for control systems ene on S E() (the special Eucliean group of rigi-boy motions) an its subgroups. Assuming one actuator is available for each egree of freeom, we exploit geometric properties of Lie groups (an corresponing Lie algebras) to generalize the classical proportional erivative (PD) control in a coorinate-free way. For the S O() case, the compactness of the group gives rise to a natural metric structure an to a natural choice of preferre control irection: an optimal (in the sense of geoesic) solution is given to the attitue control problem. In the S E() case, no natural metric is uniquely ene, so that more freeom is left in the control esign. Dierent formulations of PD feeback can be aopte by extening the S O() approach to the whole of SE() or by breaking the problem into a control problem on SO() R. For the simple S E() case, simulations are reporte to illustrate the behavior of the ierent choices. We also iscuss the trajectory tracking problem an show how to reuce it to a stabilization problem, mimicking the usual approach in R n. Finally, regaring the case of uneractuate control systems, we erive linear an homogeneous approximating vector els for stanar systems on S O() an S E(). Key wors an phrases. PD control, Eucliean group, nonlinear control, workspace control. Funing for this research was provie in part by NSF grant CMS-954. An abbreviate version of this paper can be foun in the Proc. of the 995 European Control Conference. Work performe in part while author was with the Dipartimento i Ingegneria Elettronica, Universita i Paova, Italy.

2 BULLO AND MURRAY Contents. Introuction. Systems on Lie groups 4.. Basic enitions an results 4.. The Jacobian of the exponential map 6.. Metric properties on compact Lie groups. PD control on SO() 4. PD control on SE() Proportional actions on SE() an rst orer systems Secon orer systems 5. Trajectory tracking Choices of error function on SE() Basic properties of ynamical systems on Lie groups Extening regulators to trajectory trackers 6. Linear an homogeneous approximations of systems on the Eucliean group Motivating example Jacobian linearization with respect to exponential coorinates Homogeneous approximations for SO() an SE() stanar systems 8 7. Summary an Conclusions 4 Acknowlegments 4 Appenix A. Time erivative of exponential coorinates on S E() 4 Appenix B. Proof of boun in Theorem 4 44 References 46

3 PD CONTROL ON THE EUCLIDEAN GROUP. Introuction We here consier the problem of controlling a (mechanical) system whose conguration space is a matrix Lie group: we focus on secon orer systems an attempt to generalize the stanar notion of proportional erivative feeback. One large class of applications which motivates this work is workspace control of robotic manipulators, where the en-eector conguration is naturally embee in SE() (see [6] for a escription of the workspace control problem an traitional solutions). While local solutions are easily obtaine, we hope that a more geometric approach will yiel avantages similar to those aore by the geometric approach to kinematics in [6]. Historically, nonlinear control systems ene on Lie groups have receive consierable attention in the literature: early work by Brockett [5, 7], Jurjevic an Sussman [5], an others has serve as motivation for more recent contributions by Walsh, Sarti, Sastry an Montgomery [9, ], Leonar an Krishnaprasa [, ], an Crouch an Silva Leite [], to name a few. Early works concentrate on problem formulation an controllability issues, while the more recent papers mainly consier constructive controllability: how to generate a feasible trajectory between two (or more) points on the conguration manifol given a limite number of actuators. Our approach in this paper is somewhat ierent. We concentrate on the problems of stabilization an trajectory tracking in the fully actuate case, where one actuator is available for each egree of freeom in the system. This is traitionally the situation for problems in robotic manipulation, satellite reorientation an 6 egree of freeom unerwater vehicles. We attempt to exploit the geometric properties of Lie groups an to generalize the classical proportional plus erivative feeback (PD) use for control of simple mechanical systems in R n. For the case of compact Lie groups, such as S O(), our results are completely general. For the non-compact case, we consier only control systems on S E() an on its subgroups, since those are the main systems of interest in our applications. The paper is organize as follows. In Section, we introuce basic an new results on systems ene on Lie groups. Section shows stabilization results for the compact case an in particular for SO(). Section 4 consiers the S E() case, a non-compact, non-semisimple group. Dierent metrics lea to ierent control laws. These results are then generalize to the trajectory tracking case in Section 5. In Section 6 we eal with uneractuate control systems an we show how the algebraic tools evelope in the previous sections lea to simple linear an homogeneous approximations for stanar systems on the Eucliean group. Section 7 iscusses the results.

4 4 BULLO AND MURRAY. Systems on Lie groups We here review the notations an give some algebraic results on Lie groups an on ynamical systems evolving on Lie groups. For a comprehensive introuction in the context of robotics, see [6, Appenix A]... Basic enitions an results. In the following we focus our attention on the matrix Lie group SE() an its proper subgroups, even though most of the results hol more generally. Let G SE() be a matrix Lie group an g se() its Lie algebra. A ynamical system with state g G evolves following _g = gv b = V s g; V b ; V s g; (.) where we can express the velocity in boy (V b ) or in spatial frame (V s ). To keep the notation consistent, we will use lower case symbols for elements in the group an upper case for elements in the algebra. Since the system _g = gv b is invariant uner left multiplication by constant matrices, we call it left invariant; corresponingly _g = V s g is sai to be right invariant. For all g G an all X; Y g, the ajoint map A g an the matrix commutator a X are ene as A g (Y ) = gy g? ; a X (Y ) = [X; Y ] = XY? Y X: On SE() an se() we represent a group element g = (R; p) SO() R an a velocity V = (b!; v) so() R using homogeneous coorinates, R p b! v g = ; an V = ; where the operator b : R! so() is ene so that bxy = x y for all x; y R. Writing V as column vector (!; v), simple algebra shows R b! A g = an a bpr R V = : (.) bp b! On SE() an its proper subgroups the exponential map exp : g! G is a surjective map an a local ieomorphism. Stanar computations show: Lemma (Exponential map). Given b so() an X = (b ; q) se(), b exp SO() (b b ) = I + sin k k k k + (? cos k k) expso() (b ) A( )q exp SE() (X) = ; k k (.) We will enote with G the generic Lie group (g being its Lie algebra), while for specic results we will refer to S E(), S O() etc.

5 PD CONTROL ON THE EUCLIDEAN GROUP 5 where k k is the stanar Eucliean norm an? cos k k b A( ) = I + k k k k +? sin k k k k b k k : Equation (.) is also known as Rorigues' formula. In an open neighborhoo of the origin ense in G, we ene X = log(g) g to be the exponential coorinates of the group element g an we regar the logarithmic map as a local chart of the manifol G. Lemma (Logarithmic map). Let (R; p) SO()R be such that tr(r) 6=?. Then log SO() (R) = sin (R? RT ) so(); where satises cos = (tr(r)? ) an jj <. Also log SE() (R; p) = where b = logso() (R) an an (y), (y=) cot(y=). b A? ( )p A( )? = I? b +?? (k k) b se(); (.4) k k (.5) Note that elements of the Lie algebra g can represent a velocity as in equation (.) or can represent the matrix logarithm of the state (an shoul therefore be consiere states) as in equation (.4). We enote them with V = (b!; v) in the rst case an with X = (b ; q) in the secon (also we usually have g = (R; p) SE()). Example (A few useful ientities). With the ai of Mathematica it is easy to verify the following ientities: where R( ) = exp SO() (b ). A( )? R( ) = R( )A( )? = A( )?T (.6) A( )R( ) = R( )A( ) = A( )? A( ) (.7) k k A( ) = (R? A); (.8) k k Example (Exponential an logarithmic map on S E()). Regaring the? group of planar motion, let b : R! so() map to : Given

6 6 BULLO AND MURRAY b so() an X = (b ; q) se(), the formulas above become? sin cos exp SO() (b cos ) = sin expso() (b ) exp SE() (X) = where A() = sin?(? cos ) (? cos ) sin Let (R; p) SO()R be such that tr(r) 6=?. Then log SO() (R) =, b where cos = R, sin = R an jj <. Also b A log SE() (R; p) =? ()p se(); where b () = = logso() (R) an A()? =.?= () A()q. Note that singularity is at tr(r) =? for SO() an tr(r) =? for SO()... The Jacobian of the exponential map. We now want to compute explicit formulas that relate the time erivative of X(t) = log(g(t)) with the boy an spatial velocities V b ; V s. For the linear time epenence case (X(t) = ty ), it is easy to show that _ X = Y = V b = V s ; for the generic case X = X(t) the relationship is not trivial. Theorem (Integral Formulas). Let g(t) be a smooth curve on G, X(t) = log(g(t)) be the exponential coorinates of g(t), V b = g? _g the boy velocity an V s = _gg? the spatial velocity. Then we can relate _X an V b ; V s through: V b = V s = Z Z ; A e?x(t)( _X); (.9) A e X(t)( _X): (.) Proof. For all [; ], ene V b as the solution to the algebraic equation t ex(t) = e X(t) V b : (.) Note that we here want to compute explicitly V b V b. Following [4], we prove the esire result by equating the two mixe erivatives of the smooth quantity f(t; ) = e X(t) in equation (.). We have ex(t) = X(t)e X(t) V b t + ex(t) (V b) = X(t) ex(t) + e X(t) t (V ): b (.)

7 PD CONTROL ON THE EUCLIDEAN GROUP 7 Dierentiating with the reverse orer yiels ex(t) h = X(t) ex(t)i + _X(t)e X(t) : (.) t t Equations (.) an (.) give e X(t) (V b ) = X(t)e _ X(t) ; or (V b ) = e?x(t) _X(t)e X(t) = A e?x(t)( X): _ We now integrate with respect to from to to obtain V b = V b? V b = Z A e?x(t)( _X): The corresponing equality on the spatial velocity follows from the basic equality V s = A g (V b ) an a simple change of variable =? : V s = A g (V b ) = A e X = = Z Z Z A e X?X( _X) A e X( _X): A e?x ( _X) With the same notation we have the following Jacobians: Theorem (Dierential of exponential). Let g(t) be a smooth curve on G, X(t) = log(g(t)) be the exponential coorinates of g(t), V b = g? _g the boy velocity an V s = _gg? the spatial velocity. Then we can relate _X an V b ; V s through: _X = = n= n= (?) n B n n! where fb n g are the Bernoulli numbers. a n X(V b ); (.4) B n n! an X(V s ); (.5) Remark. Note that equations (.4) an (.5) represent the innitesimal version of the Campbell-Baker-Hausor formula. Inee, in their original work [9,, ] similar relationships are erive. Proof. Recall the basic matrix equality A e?x e a?x = n= (?) n n a n X ; n!

8 8 BULLO AND MURRAY an the simple equalities Z e u = eu? ; u From previous lemma we have V b = hz hz Z i e? a X ( _X) i e?u =? e?u : u = e?u ( _X) u=a X h? e?u i = ( _X); (.6) u u=a X where the expression f(u)j u=ax means: take the Taylor expansion of f about u = an substitute the linear operator a X for all u. That is: V b = n= (?) n (n + )! an X ( _X): We now want to invert the linear relationship between V b an _X in equation (.6). As it is proven in [, Lemma ], this can be easily one by inverting f(u): _ X = h u i? e?u which explicitly written as a matrix series is _X = n= Similarly for the spatial velocity _X = h u e u? i (?) n B n n! u=a X (V s ) = u=a X (V b ); a n X(V b ): n= B n n! an X (V s ): In the following we will sometime write equation (.4) as _X = B X V b ; where with the symbols B X we enote B X = n= (?) n B n (n)! a n X :

9 PD CONTROL ON THE EUCLIDEAN GROUP 9 Recalling that B k+ = for all k >, the two series (.4) an (.5) ier only in the secon aen: _X = V b? B a X(V b ) + = V s + B a X(V s ) + m= m= B m (m)! am X (V b ) B m (m)! am X (V s ): Also, since B = ; B =?=; B = =6; B 4 =?=, the rst terms look like _X = V b + a X(V b ) + a X(V b )? : : : = V s? a X(V s ) + a X(V s ) + : : : : Note that, for small X, the matrix series in equation (.4) is full rank an absolutely convergent, in particular at X = we have _X = V b = V s. It is instructive now to sum the matrix series in equation (.4) for the important cases of SO() an SE(). In general, since the imension of G is nite, say N, the rank of the linear operator a X is also at most N an by the Cayley-Hamilton theorem, there exist some function a (X); : : : ; a N (X) such that Aitionally note that a N+ X = NX i= a i (X) a i X : (.7) a X X = (.8) for all X g, so that the rank of a X is at most N?. We start by consiering the S O() case: group elements are rotation matrices an we enote them with the stanar symbol R. The natural isomorphism between the Lie algebra so() an R is given by the bx operator an satises [bx; by] = (x \ y); so that the stanar outer prouct on R correspons to the bracket a X on so(). Thus, for simplicity, we refer to R as the Lie algebra of SO(). Simple computations show that equation (.7) reuces to bx =?kxk bx: (.9) Lemma (Time erivative of exponential coorinates on SO()). Let R(t) be a smooth curve on SO() such that tr(r(t)) 6=?. Let b (t) = log(r(t))

10 BULLO AND MURRAY be the exponential coorinates of R(t) an b! = R? _R the boy angular velocity. Then we have _ =!k + (!) + (k k)!? = I + b +?? (k k) b k k! (.) where (y), (y=) cot(y=) an! =! k +!? is the orthogonal ecomposition of! along spanf g an spanf g?. Remark. To the authors' knowlege this expression is novel an relates the time erivative of the angle-axis quantity with the boy angular velocity!. In a very peculiar way, it happens to hol that _ = A( )?T! with A( )? ene in (.5). Proof. Ientifying so() with R, it hols _ =! + b! + m= From equation (.9) we have the relation Thus bm =?k k b(m?) B m (m)! b m!: = (?) m? k k (m?) b : _ =! + b " # B! + m b (m)! (?)m? k k m k k! m= =! + b! +?? (k k) b k k!; where the last equality follows from the Taylor expansion of cot(). Aitionally notice that b k k =?pr spanf g?; that is the orthogonal projection along the spanf g?. Thus we can write _ =! + b!? (k k)pr spanf g?! =! + b!? (k k)!? =! k + b! + (? (k k))!?

11 PD CONTROL ON THE EUCLIDEAN GROUP where, once again,! =! k +!? is the orthogonal ecomposition of! along spanf g an spanf g?, that is:! k, pr spanf g (!) = h ;!i h ; i ;!?, pr spanf g?(!) =!?! k : Also, note that this particular result can also be prove through the ifferentiation of Rorigues' formula (.). Dierentiate Rorigues' formula, multiply by g? an express g? _g only as a function of b = log(g). We also have a corresponing expressions for the S E() an S E() cases: Lemma 4 (Time erivative of exponential coorinates on S E()). Let g(t) = (R(t); p(t)) be a smooth curve on S E() such that tr(r(t)) 6=?. Let X(t) = (b ; q) = log(g(t)) be the exponential coorinates of g(t) an V b = g? _g be the boy velocity. Then we have where _X = B X (V b ) = i + a X +A(k k) a X +B(k k) a 4 X (V b ); y A(y) = [? (y)] + [(y)? (y)] ; y 4 B(y) = [? (y)] + [(y)? (y)]; an (y) = (y=) cot(y=), (y), (y=) = sin (y=). Aitionally the operator B X can be written as B X = A( )?T? A( )?T : (.) Proof. Consier the expression of a X in equation (.). If X = (b ; q) se(), then simple algebraic computations show that a 6 X =?k k a 4 X?k k 4 a X : Substituting this relationship into equation (.4) of Lemma, the result follows after teious computations, see Appenix A. For the SE() case, it is possible to compute a more explicit expression: Lemma 5 (Time erivative of exponential coorinates on S E()). Let g(t) = (R(t); p(t)) be a smooth curve on S E() such that tr(r(t)) 6=?. Let X(t) = (b ; q) = log(g(t)) be the exponential coorinates of g(t) an V b = g? _g = (b!; v) be the boy velocity.

12 BULLO AND MURRAY Then we have _ =! _q =! (I? A()?T )q + A()?T v (? ())=?= =! = (? ())= q + () =?= () Proof. Dierentiating with respect to time q = A()? p, we obtain _q =! A()? p + A? _p =?!A? A()q + A? Rv =! A()? Aq + A? Rv v: =?! A? (R? A)q + A? Rv =! (I? A? R)q + A? Rv =! (I? A?T )q + A?T v where we use _A? A =?A? A_ an equation (.8), equation (.6). The nal result is obtaine by substituting the enition of A?T... Metric properties on compact Lie groups. On any Lie group G, the Killing form h; i K is ene as the bilinear operator on g g: hx; Y i K, tr(a X a Y ) 8X; Y g: A Lie group is sai to be semi-simple if h; i K is nonegenerate. For compact Lie groups h; i K is both nonegenerate an negative enite, so that by a simple multiplication with a negative constant, we can ene an inner prouct on the Lie algebra g (e.g. on so() h; i,?=4h; i K ). An inner prouct ene this way will satisfy the crucial property of A-invariance: hx; Y i = ha g X; A g Y i; 8g G; where A is therefore an orthogonal operator of g. Equivalently the matrix commutator satises ha Z X; Y i =?hx; a Z Y i 8Z g: (.) Now, an A-invariant inner prouct on the algebra g inuces a A-invariant metric on the group G by either left or right translation: this gives the aitional structure of a Riemannian manifol to the group G. Without entering etails, we refer to [4] an we simply state the following result: Proposition. With respect to an A-invariant metric, the geoesics of G are the one parameter subgroups, that is the curves of the form exp(y t), with Y g constant. Furthermore, the istance between the element g an the ientity e G = I G is given by the norm of the logarithmic function: kgk G = hlog(g); log(g)i = : (.) The computational result we are intereste in is an extension of Gauss's Lemma (see [4] an [8]), obtaine thanks to property (.) an equation (.).

13 PD CONTROL ON THE EUCLIDEAN GROUP Theorem (Derivative of istance function). Let G be a compact Lie group with bi-invariant metric h; i. Consier a smooth trajectory g(t) G, such that g(t) never passes through a singularity of the exponential map. Then t kgk G = hlog(g); V b i = hlog(g); V s i:. PD control on SO() We begin with the problem of stabilizing a control system evolving on a compact, semisimple Lie group. Without loss of generality we will here consier only the S O() case. As explaine in the previous section, a biinvariant Riemannian metric is naturally ene on S O() an allow us to easily esign appropriate Lyapunov functions. We begin by briey escribing our approach for a simple rst orer system on SO(), escribe as in equation (.) by _g = gv b. Consier the natural caniate Lyapunov function W (g) = kgk SO() ; an assume we can irectly control the quantity V b so() to any esire value (i.e. the system is fully actuate). Then the proportional control action leas to V b =?k p log(g); k p > ; (.) _W (g(t)) = hlog(g);?k p log(g)i =?k p W; thanks to Theorem. Thus, for this rst orer system, a logarithmic control law ensures exponential stability for all initial conitions g() such that tr(g()) 6=?. Now, motivate by stanar control problems in mechanics an robotics, we consier the stabilization problem for secon orer systems, that is for systems where we have full control over forces (accelerations) rather than velocities. A secon orer system on SO() has the form ( _g = gv b _V b = f(g; V b ) + U; (.) where g SO() is the conguration of the system, f(g; V b ) so() is the internal rift, an U so() is the control input. Note that we once again assume that the system is fully actuate. To regulate the conguration g to the ientity matrix I S O(), we couple the proportional action (.) with a erivative term, i.e. with a term proportional to the velocity V b. Theorem 4 (PD plus feeforwar control on S O()). Consier the system in equation (.) an let K p an K be symmetric, positive enite gains.

14 4 BULLO AND MURRAY Then the control law U =?f(g; V b )? K p log(g)? K V b ; (.) exponentially stabilizes the state g at I SO() from any initial conition tr(g()) 6=? an for all K p an V b () such that kv b ()k min (K p ) >? kg()k : (.4) SO() where min (K p ) is the minimum eigenvalue of K p. Proof. We will here rely on the properties of the inner prouct on so(). Let i so() be the ientity automorphism of so(). With a slight abuse of notation, we can ene the caniate Lyapunov function as W = h log(g) V b ; " iso() i so() i so() K? p # log(g) V b i so()so(); where i so() is the ientity map on so(), the inner prouct is taken in so() so() an is taken small enough. The close loop system satises _g = gv b _V b =?K p log(g)? K V b : We now rop the subscript an write the previous system in exponential coorinates X = log(g) so() to obtain 8 >< >: _X = n= (?) n B n n! _V =?K p X? K V; where we have ene B X, P a n X (V b ) = B X V (?) n B n n= a n n! X. Dierentiating with respect to time our caniate Lyapunov function we have t W = hx; B X V i + hv; K p? _V i + hb X V; V i + hx; V _ i = hx; V i + hv; K? p (?K p X? K V )i + hb X V; V i + hx;?k p X? K V i =? hx; K p Xi? hv; K p? K V i? hx; K V i + hb X V; V i The last term can be upper boune by hv; V i using Lemma in Appenix B, so that t W? X X h ; Q V i V so()so()

15 where PD CONTROL ON THE EUCLIDEAN GROUP 5 Q = K p K = K = K p?k? i so() is positive enite for small. Local exponential stability is therefore proven. We now show that conition (.4) provies a sucient boun in orer for the close loop trajectories to avoi the singularity of the logarithmic map. Note that W (t) is a non increasing function (since Q is negative semienite) an that kg(t)k SO() W (t) W () = kg()k SO() + hv b (); K p?v b ()i so() kg()k SO() + max(k? p )kv b ()k kg()k SO() + min (K p ) kv b ()k < =; where we use the fact that the maximum eigenvalue of K p? is equal to the inverse of the minimum eigenvalue of K p. By the previous equation, g(t) can never become a rotation of raians an therefore the singularity of the logarithmic function is never reache. Notice that the proof follows the same steps as the usual one in R n. The introuction of the cross term, proportional to a small, is a well-known trick. See, for example, Wen an Bayar [] or Murray et al [6]. Remark. We have written the control law (.) an Theorem 4 in terms of the boy velocity V b, i.e. we assume \boy-xe" control inputs. A ual version can be easily written for the opposite case of \spatial-xe" control inputs, i.e. for the case _ V s = f(g; V s ) + U. Thanks to Theorem a logarithmic control law is the correct choice also for this case. Example (Orientation control of a satellite). A stanar example of a control problem on a compact Lie group is attitue control of a satellite. In the literature, various PD control laws base on ierent parametrization of the manifol S O() have been propose: Euler angles [], Gibb's vectors [] an unit quaternions [4]. In particular, Wen an Kreutz- Delgao [4] introuce the iea that the \error measure shoul correspon to the topology of the error space". Here we aitionally require that the error measure correspon to the (natural) metric of the Riemannian manifol SO(). The secon orer moel of a satellite is ( _g = gb! b ; J _! b = f(g;! b ) + ;

16 6 BULLO AND MURRAY where the control inputs is the total torque applie to the satellite either by momentum wheels or by gas jet actuators. The internal rift is ( [g f(g;! b ) = T m ;! b ] momentum wheels [J! b ;! b ] gas jet (Euler equations). Following early work by Koitschek [9], we introuce a slight moication to the esign of Theorem 4 an we aopt the moie Lyapunov function W = k p kgk SO() + h!b ; J! b ir + hlog(g); J!b i where the secon term has the interpretation of kinetic energy. This leas to the feeback law =?k p log(g)? K! b (.5) where we write the control law in R making use of the isomorphismb given in Section. Note that in equation (.5) we are not canceling the nonlinear Coriolis forces, but instea we are exploiting their intrinsic passivity properties. This proceure is what we refer to as Koitschek's approach [9]; its rawback is that we nee to restrict ourselves to scalar proportional gains k p (the erivative gain K can remain a (positive enite) matrix). This is somehow a characteristic behavior (see the complete example on S E() in the following section for more etails). This feeback has strong similarities to the ones alreay propose in the literature: it is instructive to compare it with the equivalent propose by Wen an Kreutz-Delgao [4]. Both laws consist of the sum of a proportional an erivative action, where they ier is in the expression of the proportional term. In particular along the \geoesic" irection (equal to the rotation axis of the attitue matrix g), the two laws ier in the intensity of control action. Our feeback relies on the notion of group norm (as ene in equation (.)) an is proportional to this quantity. Instea the control laws propose by Wen an Kreutz-Delgao are base on either the -norm of the unit quaternion or the -norm of the vector quaternion, an therefore exert an action proportional to either sinkgk or sin(kgk=). 4. PD control on SE() We now consier the extension of the results in the previous section to S E(), the special Eucliean group of rigi-boy motions. As escribe in the introuction, this Lie group is common in robotic applications. Unfortunately, since this S E() is not compact, the results of the previous section cannot be extene irectly. As before, we begin by stuying the simple rst orer case an we then couple proportional with erivative action for secon orer systems. Finally we apply our results to the case of mechanical manipulators an we then report some simulations.

17 PD CONTROL ON THE EUCLIDEAN GROUP Proportional actions on SE() an rst orer systems. The geometric properties of the group S E() have receive much attention in the recent control literature [7, 6] an a very complete treatment is containe in [8]. A well-known negative result is the following: no symmetric bilinear form on se() can be both positive-enite an A-invariant. There is therefore an algebraic obstruction to the proceure we have followe for the SO() case. Recall the esign proceure: we nee a positive-enite bilinear form (hence an inner prouct) to construct a Lyapunov function W, an we nee the A-invariance of this form to compute the time erivative of W (Theorem ). Therefore we here briey consier bilinear forms ene on se(). Let V i = (! i ; v i ) for i = ;, we have. A linear combination of Klein an Killing form: the most generic Ainvariant form on se() looks like hv ; V i A?inv = h! ;! i +? h! ; v i + h! ; v i ; where with h; i we inicate the stanar inner prouct on R,. The stanar inner prouct on se() = R 6 : iscar the Lie algebra structure of se() an write hv ; V ir 6 = h! ;! i + hv ; v i: (4.) Hence we are left with two possible esign choices: as proportional action we can insist on the logarithm function (which no longer correspons to the geoesic irection of a Riemannian metric), or (giving up the Ainvariance) we can still regar SE() as a metric space with respect to the inner prouct (4.) an compute the correct proportional action within this new framework. The two proceures are illustrate in Figure for the case of left invariant control systems _g = gv b ; the following two lemmas formalize this iscussion. Lemma 6 (Logarithmic feeback). Consier the left invariant system _g = gv b on SE() an let k p >. Then the control law V b =? k! I log(g) (4.) (k! + k v )I exponentially stabilizes the state g at I with time constant k p, from any initial conition g() = (R(); p()) such that tr(r()) 6=?. Proof. In exponential coorinates log(g) = X = (b ; q) se(), the closeloop system is _X = B X V b : Given an inner prouct on g, we can exten it to the whole T G by either left or right translation: we en up therefore with a metric structure on G. We refer to [4] for a etaile treatment of this stanar construction.

18 8 BULLO AND MURRAY Using the equations (.8) an (.), we have so that B X X = X an B X q _X =? B X k! X + k v q = A( )?T q =?k! X? k v Separating the rotational an translational parts _ =?k! _q =?k! q? k v A( )?T q: A( )?T q Regaring the rotational part, exponential stability is proven for all R such that tr R 6=? (in orer for the exponential coorinates to be ene). Regaring the translational part, consier the caniate Lyapunov function W = kqk. Its time erivative satises t W =?hq; k!q + k v A( )?T qi =?k! kqk? k v kq k k + (k k)kq? k ; where last equality is obtaine using the enition of A? in equation (.5) an where q = q k + q? is the orthogonal ecomposition of q along spanf g an spanf g?. Thus local exponential stability is proven also for the translational part. Finally, since is a ecreasing function of time, the close loop trajectories will not encounter the singularity points of the logarithmic function, as long tr(r()) 6=?. The secon approach is base on the ecomposition of the control system on SE() into a control system on SO() R. Recall the notation introuce in Section : g = (R; p), V s = (b! s ; v s ), V b = (b! b ; v b ). The original systems _g = gv b an _g = V s g reuce to _ R = R b! b _p = R v b an _ R = b! s R _p =! s p + v s : Inee aopting the bilinear form (4.) involves applying a proportional action along geoesic irections for both the subsystems in SO() an R (therefore we call such approach ouble-geoesic). Lemma 7 (Double-geoesic feeback). Consier the left invariant control system _g = gv b on SE() an let K v ; K! be positive enite symmetric gains. Then the control law (! b =?K! log SO() (R) v b =?R T K v p :

19 PD CONTROL ON THE EUCLIDEAN GROUP 9 Figure. Proportional actions on S E(). From left to right: logarithmic function (-parameter subgroups on SE()) an ouble-geoesics for SO() R. Each point g SE() is epicte as a frame on the plane. exponentially stabilizes the state g at I, from any initial conition g() = (R(); p()) such that tr(r()) 6=?. Proof. In rotational an translation coorinates the close loop system is ( _ R = R(?K! log(r)) _p =?K v p: Remark 4 (Symmetries in the control laws). Similar versions of the two lemmas can be easily written for the right invariant case ( _g = V s g) an some instructive behavior can be easily escribe. In the following, let g left (t) an g right (t) the solutions to the left an right close loop systems: To examine the logarithmic control law applie to a right invariant system, recall the basic Lie group ientity A g log(g) log(g). Then the close-loop systems (with unit gains), _g left =?g left log(g left ) an _g right =? log(g right )g right ; are the same ierential equation an we have g left () = g right () =) g left (t) = g right (t) (4.) Regaring the ouble-geoesic control law we can state a similar but opposite result. The control law for this case is (! s =?K! log SO() (R) v s =?K v p;

20 BULLO AND MURRAY an (with unit gains) the left an right close-loop control systems are ( R _ ( =?R log SO() (R) R _ =? log SO() (R)R an _p =?p _p =? log SO() (R)p? p: Then easy algebraic steps show g left () = g? right () =) g left(t) = g? right (t): (4.4) Note the peculiar corresponence between (4.) an (4.4). 4.. Secon orer systems. We now apply these proportional strategies, couple with a erivative term, to secon orer, fully actuate systems on S E(). Consier the left invariant secon orer system ( _g = gv b (4.5) _V b = f(g; V b ) + U; where f(g; V b ); U se() are internal rift an control input. The previous iscussion leas to the two theorems: Theorem 5 (Regulation via the ouble-geoesic law). Consier the system in equation (4.5) an let K! ; K v an K be the positive enite gains. Then the control law U(g; V b ) =?f(g; V b K! log )? SO() (R) R T? K V b ; (4.6) K v p exponentially stabilizes the state g at I from any initial conition g() = (R(); p()) with tr(r()) 6=? an for all K! an! b () such that k! b ()k min (K! ) >? kr()k : (4.7) SO() Proof. The scalar gain case (K v = K! = k p ) amits a stanar proof ientical to the one of Theorem 4, but with the caniate Lyapunov function W (g; V b ) = k p krk SO() + kpk + hv b ; V b ir " # 6 logso() (R) + h R T p ; V b ir 6: For the matrix gain case the notation becomes more involve but the algebraic steps are the same. In particular, also the suciency of conition (4.7) (exactly corresponing to conition (.4)) follows from the same steps as in Theorem 4. Theorem 6 (Regulation via the logarithm function). Consier the system in equation (4.5) an let K p an K be positive-enite gains. Then the control law U(g; V b ) =?f(g; V b )? K p log(g)? K V b ; (4.8)

21 PD CONTROL ON THE EUCLIDEAN GROUP locally exponentially stabilizes the state g at I SE(). Furthermore, if scalar gains are employe (K p = k p I 6 an K = k I 6 ), then the control law in (4.8) exponentially stabilizes the state g at I from any initial conition g() = (R(); p()) with tr(r()) 6=? an for all k p an! b () such that k p > k! b ()k? kr()k : (4.9) SO() Proof. Given the enition of matrix logarithm on SE() in Lemma, the feeback law in equation (4.8) is equal up to higher orer terms to the one in equation (4.6). Thus local exponential stability is ensure. For the scalar gains case, we can prove almost global exponential stability. Consier the close loop system: _g = gv (4.) _V =?k p log(g)? k V; (4.) starting from initial conitions (g(); V ()) = (g ; V ) SE() se(). Since equation (4.) is linear, we can ecompose the solution V as the sum of two components V = V hom + V par, where V hom (t) = V exp(?t=k ) an V par is the solution of (4.) with zero initial conition (an consiering the log(g) term as an external isturbance). Notice now that the manifol M = f(g; V ) : V = log(g); for some Rg SE() se() is invariant for the system of ODEs (4.) an (4.) with initial conitions (g(); V ()) = (g ; ). For, consier the system expresse in exponential coorinates an subsitute X = V to obtain _X = B X X _X = B X V _V =?k p X? k V =X _V =? k p V? k V =? kp + k Hence for all t, X(t) span X() an V (t) span X(), provie V () = X(). It is now easy to show that the invariant manifol M is stable, since assuming X = x vers(x()) an V = v vers(x()) we have V: _x = v _v =?k p x? k v ) x =?k p x? k _x: (4.)

22 BULLO AND MURRAY The proof is now complete by noting that the original system (4.) an (4.) can be written as _X = B X V = B X V par + B X V hom _V =?k p X? k V =? k p X? k V par? k V hom BX V where the isturbance hom ecreases to zero exponentially. Lemma 4.7?k V hom in Khalil [8] applies proving local exponential stability. Aitionally, if conition (4.9) is satise, then with the same bouning technique in the proof of Theorem 4, we can prove that no singularity will be encountere by the close loop trajectories. Remark 5. As usual we can exten to the right invariant case ( _g = V b g) all we have one for the left one. For both systems the logarithmic control law (in Theorem 6) is ientical. The ouble-geoesic law applie to a right system has the slightly ierent expression: U(g; V s ) =?f(g; V s K! log )? SO() (R)? K V s : K v p Example 4 (Workspace control of mechanical systems). As in Example for S O(), we here apply our control strategies to fully actuate mechanical systems. Examples of this class of systems are robotic manipulators an 6 egree of freeom (DOF) unerwater vehicles. We assume here that a change of coorinates an inputs has alreay been applie to the system so that our moel is escribe by ( _g = gv b M(g) _ V b =?C(g; V b )V b? N(g; V b ) + U; where M(g) is the inertia matrix, C(g; V b ) is the Coriolis matrix an N(g; V b ) is use to moel friction an gravity. The kinetic energy of this mechanical system is compute with the positive enite form (4.) (couple with the left translation of the velocity gv b ). Hence, for this class of systems, we are naturally lea to prefer the ouble-geoesic control law over the logarithmic one: U(g; V b ) = N(g; V b k! log )? SO() (R) k v R T? K V b : (4.) p Exponential stability is prove through the Lyapunov function W (g; V b ) = k! krk SO() + k v kpk + hv b ; M(g)V b ir 6 logso() (R) + h R T ; M(g)V b p ir 6: Once again, in writing equation (4.) we take avantage of the passivity properties of the Coriolis term C(g; V b )V b an we compensate only for

23 PD CONTROL ON THE EUCLIDEAN GROUP N(g; V b ). Rather than canceling both terms, this approach appears to be a more natural way of controlling fully actuate mechanical systems, see Koitschek's early work [9] for more etails. A few remarks:. The control law in equation (4.) has the usual avantages of PD control escribe in [6]: ease of computation an no knowlege of the exact system's parameters require.. A secon approach woul involve a typical \compute torque" technique, where the Coriolis term C is explicitely compensate for. In this latter case, the logarithmic control law of Theorem 6 can be applie. Future avenues of research consists in the application of the logarithmic control law for the case of robotic manipulators for the purpose of hybri (position/force) control an the stuy from a (Lie group) algebraic viewpoint if simplications occur in the expression of the Jacobian manipulator (again, when logarithmic control law is applie). Example 5 (Position an attitue stabilization of planar rigi boy). To compare the two classes of controllers presente above, we consier the problem of stabilizing a planar rigi boy. Note that the subgroup of the planar motions SE() contains still most of the complexity an richness of the full SE() case. We have simulate the feeback laws escribe in Theorem 5 an 6 (ouble-geoesic an logarithmic laws for left invariant systems), an in Remark 5 (ouble-geoesic an logarithmic law for right systems). As foreseen from theoretical consierations, the logarithmic control law generates the same close-loop trajectories for both the right an the left invariant systems. The shape of the trajectories for all of the cases varies consierably epening on the size of the initial angle error an on the gain values: for all cases we picke an initial rotational error equal to = an we choose two sets of scalar gains: (k p ; k ) = (; ) an (k p ; k ) = (; ). We here report the SE() trajectories for the 4 controllers with the rst set of gains (Figures an ) an the corresponing velocity proles for the two left invariant controllers, (Figures 5 an 6). Also we show in Figure 4 how the trajectories change when a low erivative gain is applie (low with respect to a constant proportional gain). Looking at the plots in Figure an a few simple remarks can be mae:. In agreement with the fact that the various feebacks are equal in the rotational part, the angular behavior is the same in all simulations.. All the control laws seem to converge at a very similar rate in both the rotational (of course) an translational part. This is also preictable since ientical gains are applie. Inee, quantitative results (which we on't report for brevity) inicate that the various input norms for the logarithmic control law are larger than for the ouble-geoesic strategy. Typically the logarithmic inputs woul be about % larger than the ouble geoesics (see Figure 5 an 6).

24 4 BULLO AND MURRAY. Qualitatively, the clearest ierence regars the opposite haneness of the various control laws. Corresponing to a choice of left invariant control system the logarithmic an ouble-geoesic feebacks will follow quite ierent paths even from a simple qualitative viewpoint (Figure ). In the right invariant case instea the haneness is the same, but the ouble-geoesic law shows a more curve behavior (Figure ). 4. The ierence in the shape of trajectories becomes even clearer in secon simulation in Figure 4 where a low erivative gain k is employe an where the eects of the ierent proportional actions is therefore emphasize. Note the oscillatory behaviour of both close loop system: the values (k p ; k ) = (; ) correspons to an slightly ampe secon orer systems. This kin of behaviour seems therefore mantaine by our nonlinear moels. The issues escribe in Remark 4 on symmetries of control laws an the proof of Theorem 6, n clear illustration in the case of high erivative gain (rst set of simulations). Notice that: 5. both left an right invariant close loop systems with logarithmic control law remain on the -parameter subgroup of S E() etermine by the initial conitions (see right pictures in Figure an 4). In Figure 5 an 6 we report the time evolution of the velocity V = (v x ; v y ). In the logarithmic control case, since the state remains on a -parameter subgroup, the ratio of the inputs v x =v y remains constant uring the simulation, see Figure 6 compare to Figure 5. These facts are preicte an are at the basis of the proof of Theorem moulo the iering initial conitions, we recover the trajectories of the left ouble-geoesic close loop system by inverting the right oublegeoesic trajectories.

25 PD CONTROL ON THE EUCLIDEAN GROUP 5 Left system with Double Geoesic control law Left system with Log control law.5.5 y (cm) y (cm) x (cm).5.5 x (cm) Figure on S E(). From left to right: ouble-geoesic control law as in Theorem 5 an logarithmic control law as in Theorem 6. Each point g SE() is epicte as a frame on the plane. Note the opposite haneness of the two control strategies.. Trajectories of left invariant control systems Right system with Double Geoesic control law Right system with Log control law.5.5 y (cm) y (cm) x (cm).5.5 x (cm) Figure. Trajectories of right invariant control systems on S E(). From left to right: ouble-geoesic control law as in Remark 5 an logarithmic control law as in Theorem 6.

26 6 BULLO AND MURRAY Left system with Double Geoesic control law Left system with Log control law.5.5 y (cm) y (cm) x (cm).5.5 x (cm) Figure 4. Trajectories of left invariant control systems on SE() with low erivative gain k. From left to right: ouble-geoesic control law as in Theorem 5 an logarithmic control law as in Theorem 6. Note that the state of the close loop system with logarithmic control law (on the right) remains on the -parameter subgroup etermine by the initial conition. The ouble-geoesic control law instea gives rise to a spiraling (x; y) motion (on the left).

27 PD CONTROL ON THE EUCLIDEAN GROUP 7 v (cm/sec) t (sec) v (cm/sec) t (sec) v (cm/sec) t (sec) v (cm/sec) t (sec) v (cm/sec) t (sec) v (cm/sec) t (sec) v (cm/sec) t (sec) v (cm/sec) t (sec) Figure 5. Velocity proles of ouble-geoesic control law for the simulation epicte in Figure. The eight gures show the time evolution of v x (t); v y (t) from the eight initial conitions: the location of the 8 pictures correspons to the 8 initial conition in the (x; y) plane. v (cm/sec) t (sec) v (cm/sec) t (sec) v (cm/sec) t (sec) v (cm/sec) t (sec) v (cm/sec) t (sec) v (cm/sec) t (sec) v (cm/sec) t (sec) v (cm/sec) t (sec) Figure 6. Same as above, but for the logarithmic control law. Note that the input magnitue is slightly larger for logarithmic rather than for ouble-geoesic control law. Also, in the logarithmic case, the ratio v x =v y is constant for all initial conitions, since the state g SE() remains on the -parameter subgroup exp( log(g())) for all time.

28 8 BULLO AND MURRAY 5. Trajectory tracking We escribe here a general approach to trajectory tracking problems for secon orer systems ene on Lie groups. In particular, by exploiting the group structure we reuce the tracking problem to a stabilization one for an appropriately ene error system. In the following, we will assume that we are given a left invariant, secon orer control system on G ( _g = gv; _V = f(g; V ) + U; (5.) an a control law U = Z(g; V ) that makes the close loop riftless system _g = gv; V _ = Z(g; V ) locally asymptotically (exponentially) stable at the ientity e G = I. We want to esign a control law that makes the state g track a reference trajectory g G escribe by: _g = V g, for V (t) g. 5.. Choices of error function on SE(). In orer to ene a correct notion of error function, we exploit the natural group structure of the con- guration manifol, see for example [4]. Given the interpretation of group elements on S E() as coorinate frames, the natural choice of state error is e, g? g; which represents the reference frame as \seen" from the state of the system. In other wors, if g represents the boy frame an g the esire frame, then e is the relative g to g frame. Decomposing this error in its rotational an translational components, we have: e = R T R R T (p? p ) : (5.) We call this the natural error. An equivalent enition woul be g? g e?, since, as escribe in Section 4, controlling e or e? is the same control problem. If we iscar the physical reasoning associate with the natural enition, other choices of conguration error are possible. In particular the following two appear appealing:. Dene the reciprocal error as e recip, gg? = RR T p? RR T p ; (5.) where we exchange the orer of multiplication.. In keeping with the notation in [6], ene the hybri error as e hybri, R T R p? p ; (5.4) where we keep the natural error choice for the rotational part (we coul instea have RR T ).

29 PD CONTROL ON THE EUCLIDEAN GROUP 9 Even though these latter two enitions seem also rather natural, the Lie group structure of the original problem is not taken into account. It happens in particular that reciprocal an hybri error (between boy an esire frame) epen on the arbitrary choice of inertial frame. For, recall that a change of inertial frame correpons to a left translation. Thus consier the map L g such that L g g = g g an L g g = g g. Then, if g = (R ; p ), we have e R RR recip = T RT p? R RR T RT p e R hybri = T R R (p? p ) : Note that for the hybri error, this inertial frame epeency simply implies the presence of an arbitrary rotation in the translational part. For the reciprocal error instea, more evient eects appear: for example, even for p = p, i.e. for overlapping frames, e recip \sees" some translational error if R 6= R. This will reect in a control law with non-zero translational input, which is unoubtely unesire. Eventually note that the rawbacks just escribe aect also the inverse enitions e? recip an e? hybri. Therefore, after this theoretical iscussion, we ten to prefer the natural choice (5.); the simulations performe later will further clarify the nal choice. 5.. Basic properties of ynamical systems on Lie groups. We characterize here the behavior of the composition of systems ene on the group G. Recall that, given l; r G an L; R g, we call _ l = ll a left control system an _r = Rr a right control system. Also, given two systems with state g(t); h(t) G, we call the inverse system the one corresponing to the state g(t)? an the prouct system the one corresponing to the state g(t)h(t). By performing some chain rule ierentiations, we have: Lemma 8 (Time erivative of compose systems). With the notation just introuce it hols an _l? =?Ll? _r? =?r? R;? (gh) (gh) t = A h?(g? _g) + h? h _ (gh) (gh)? t = _gg? + A g ( hh _? ); where the ajoint map A is ene in Section. By means of these basic results, we are able to escribe straightforwarly how, for instance, the prouct of two left control systems evolves in time: ;

30 BULLO AND MURRAY letting _ l = l L an _ l = l L, we have t l l = l l (A l? L + L ): (5.5) Lemma 9 (Derivative of ajoint map). Let U(t) g, l _ = ll an _r = Rr, with l; r G an L; R g. Then A t l(t) U(t) = A l U _ + Al [L; U]; A t r(t) U(t) = A r U _ + [R; Ar U]: Now, recalling equation (5.5), we can ene l, l l an V, A l? L. Lemma 9 gives: ( _l = l V _V = A l? _L + [A l? L ; L ] + _ L ; L + (5.6) which shows how we can write in full generality the secon orer ynamics of the combination of Lie group systems. 5.. Extening regulators to trajectory trackers. We are now able to formulate the following general solution: Theorem 7 (Trajectory tracking). Consier the system in equation (5.), the asymptotic (exponential) regulator law Z(g; V ) an the esire trajectory g (t). Dene the conguration error e, g? g G an the velocity error V e, V? A g? V g. Then the control law where U = U (g; V ) + U tr (g; V; V ; _ V ) + Z(e; V e ) (5.7) U (g; V ) =?f(g; V ) U tr (g; V; V ; _ V ) = A g?( _ V ) + [A g?(v ); V ]; makes the conguration error e locally asymptotically (exponentially) approach the ientity I G. Proof. The result follows straightforwarly from equation (5.6). Inee consier the error e = g? g ans its velocity V e = V? A g? V. Then the aena in equation (5.7) cancel out exactly the extra pieces in equation (5.6) an the close loop system satises _V e = Z(e; V e ): By assumption, asymptotic (exponential) stability is proven. A few comments: rst, for the SO() case we can simplify the tracking law by ening U tr = A _ g? V. The PD control law in equation (.) woul still ensure exponential stability thanks to the orthogonality properties iscusse in Subsection..

31 PD CONTROL ON THE EUCLIDEAN GROUP Secon, in stating Theorem 7, we assume our control system to be left invariant an our trajectory to be right invariant. These two choices are suite to the kin of mechanical systems we are intereste in, such as example satellite reorienting an robotic manipulation. However, similar versions of the theorem can be state using any combination of right an left invariant systems. Eventually, Theorem 7 can be state for the other enitions of error function: reciprocal an hybri.. For the case of e recip, the results obtaine in the previous subsection lea straightforwarly to ( _erecip = e recip (A g V? V ), e recip V e _V e = A g (f(g; V ) + U)? _ V + [V ; A g V ] an similarly to equation (5.7) we set U = A g? U + U tr + Z(e recip ; V e ) ; where U tr = _ V? [V ; A g V ] an U =?f(g; V ).. For the hybri error instea, we want to keep the computations in local coorinates (R; p): from enition (5.4) we have p e, p? p an R e = R T R. For the rotational part R e SO(), we apply Theorem 7; for the translational part we have _p e = Rv? v p e = R(f t (g; V ) + a)? _v + R(! v); where a R is the acceleration input an f t is the rift in the translational coorinates (see equation (5.)). So we can write U = (; a), where is choosen from the previous theorem an a PD controller for the translation part gives a = a + a tr + R T (?k p p e? k _p e ) (5.8) where a tr = v! an a =?f t (g; V ). Example 6 (Position an attitue tracking on S E()). To compare the various error choice presente above, we consier the trajectory tracking problem for a planar rigi boy. This control problem moels for example a robot manipulator spray painting planar objects. We have simulate the trajectory tracking strategies just escribe for the secon orer system (5.) with no internal rift: f(g; V ) =. To recall the notation, our system is 4 x _y 5 = 4 cos? sin sin cos 5 4! v x v y 5 an 4 _! _v x _v y 5 = U: The reference trajectory has the shape of an character in the (x; y) variables an it species the reference angle to be parallel to the normal