PROPORTIONAL DERIVATIVE (PD) CONTROL ON THE EUCLIDEAN GROUP

Division of Engineering and Applied Science California Institute of Technology Pasadena, CA 91125 CDS Technical Report 95-010 August 11, 1995
F. BULLO AND R. M. MURRAY

control systems de ned on S E (3) (the special Euclidean group of rigid-body motions) and its subgroups. Assuming one actuator is available for each degree of freedom, we exploit geometric properties of Lie groups (and corresponding Lie algebras) to generalize the classical proportional derivative (PD) control in a coordinate-free way. For the S O(3) case, the compactness of the group gives rise to a natural metric structure and to a natural choice of preferred control direction: an optimal (in the sense of geodesic) solution is given to the attitude control problem. In the S E (3) case, no natural metric is uniquely de ned, so that more freedom is left in the control design. Di erent formulations of PD feedback can be adopted by extending the S O (3) approach to the whole of SE (3) or by breaking the problem into a control problem on SO(3) R3. For the simple S E (2) case, simulations are reported to illustrate the behavior of the di erent choices. We also discuss the trajectory tracking problem and show how to reduce it to a stabilization problem, mimicking the usual approach in Rn. Finally, regarding the case of underactuated control systems, we derive linear and homogeneous approximating vector elds for standard systems on S O(3) and S E (3).
Key words and phrases. PD control, Euclidean group, nonlinear control, workspace control. Funding for this research was provided in part by NSF grant CMS-9502224. An abbreviated version of this paper can be found in the Proc. of the 1995 European Control Conference. Work performed in part while author was with the Dipartimento di Ingegneria Elettronica, Universita di Padova, Italy.

Abstract. In this paper we study the stabilization problem for

BULLO AND MURRAY

Contents

1. Introduction 2. Systems on Lie groups 2.1. Basic de nitions and results 2.2. The Jacobian of the exponential map 2.3. Metric properties on compact Lie groups 3. PD control on S O(3) 4. PD control on S E (3) 4.1. Proportional actions on SE(3) and rst order systems 4.2. Second order systems 5. Trajectory tracking 5.1. Choices of error function on SE(3) 5.2. Basic properties of dynamical systems on Lie groups 5.3. Extending regulators to trajectory trackers 6. Linear and homogeneous approximations of systems on the Euclidean group 6.1. Motivating example 6.2. Jacobian linearization with respect to exponential coordinates 6.3. Homogeneous approximations for SO(3) and SE(3) standard systems 7. Summary and Conclusions Acknowledgments Appendix A. Time derivative of exponential coordinates on S E (3) Appendix B. Proof of bound in Theorem 4 References

3 4 4 6 12 13 16 17 20 28 28 29 30 34 34 36 38 41 42 42 44 46

PD CONTROL ON THE EUCLIDEAN GROUP

1. Introduction We here consider the problem of controlling a (mechanical) system whose con guration space is a matrix Lie group: we focus on second order systems and attempt to generalize the standard notion of proportional derivative feedback. One large class of applications which motivates this work is workspace control of robotic manipulators, where the end-e ector con guration is naturally embedded in S E (3) (see 26] for a description of the workspace control problem and traditional solutions). While local solutions are easily obtained, we hope that a more geometric approach will yield advantages similar to those a orded by the geometric approach to kinematics in 26]. Historically, nonlinear control systems de ned on Lie groups have received considerable attention in the literature: early work by Brockett 5, 7], Jurdjevic and Sussman 15], and others has served as motivation for more recent contributions by Walsh, Sarti, Sastry and Montgomery 29, 32], Leonard and Krishnaprasad 20, 21], and Crouch and Silva Leite 10], to name a few. Early works concentrated on problem formulation and controllability issues, while the more recent papers mainly consider constructive controllability: how to generate a feasible trajectory between two (or more) points on the con guration manifold given a limited number of actuators. Our approach in this paper is somewhat di erent. We concentrate on the problems of stabilization and trajectory tracking in the fully actuated case, where one actuator is available for each degree of freedom in the system. This is traditionally the situation for problems in robotic manipulation, satellite reorientation and 6 degree of freedom underwater vehicles. We attempt to exploit the geometric properties of Lie groups and to generalize the classical proportional plus derivative feedback (PD) used for control of simple mechanical systems in Rn. For the case of compact Lie groups, such as S O(3), our results are completely general. For the non-compact case, we consider only control systems on S E (3) and on its subgroups, since those are the main systems of interest in our applications. The paper is organized as follows. In Section 2, we introduce basic and new results on systems de ned on Lie groups. Section 3 shows stabilization results for the compact case and in particular for S O(3). Section 4 considers the S E (3) case, a non-compact, non-semisimple group. Di erent metrics lead to di erent control laws. These results are then generalized to the trajectory tracking case in Section 5. In Section 6 we deal with underactuated control systems and we show how the algebraic tools developed in the previous sections lead to simple linear and homogeneous approximations for standard systems on the Euclidean group. Section 7 discusses the results.

BULLO AND MURRAY

2. Systems on Lie groups We here review the notations and give some algebraic results on Lie groups and on dynamical systems evolving on Lie groups. For a comprehensive introduction in the context of robotics, see 26, Appendix A]. 2.1. Basic de nitions and results. In the following we focus our attention on the matrix Lie group S E (3) and its proper subgroups, even though most of the results hold more generally.1 Let G S E (3) be a matrix Lie group and g se(3) its Lie algebra. A dynamical system with state g 2 G evolves following g _ = gV b = V s g; V b; V s 2 g; (2.1) where we can express the velocity in body (V b ) or in spatial frame (V s ). To keep the notation consistent, we will use lower case symbols for elements in the group and upper case for elements in the algebra. Since the system g _ = gV b is invariant under left multiplication by constant matrices, we call it left invariant ; correspondingly g _ = V s g is said to be right invariant. For all g 2 G and all X; Y 2 g, the adjoint map Adg and the matrix commutator adX are de ned as Adg (Y ) = gY g ?1; adX (Y ) = X; Y ] = XY ? Y X: On S E (3) and se(3) we represent a group element g = (R; p) 2 S O(3) R3 and a velocity V = (! b ; v ) 2 so(3) R3 using homogeneous coordinates,

p ! b v g= R 0 1 ; and V = 0 0 ; where the operator b : R3 ! so(3) is de ned so that x by = x y for all 3 x; y 2 R . Writing V as column vector (!; v), simple algebra shows R 0 b 0 Adg = p and adV = ! : (2.2) bR R p b ! b On S E (3) and its proper subgroups the exponential map exp : g ! G is
a surjective map and a local di eomorphism. Standard computations show: Lemma 1 (Exponential map). Given b 2 so(3) and X = ( b; q) 2 se(3), expSO(3)( b) = I + sin k k k k + (1 ? cos k k) k k2 b expSE (3)(X ) = expSO(3) ( ) A( )q ; 0 1
1

b2

(2.3)

We will denote with G the generic Lie group (g being its Lie algebra), while for speci c results we will refer to S E (3), S O(3) etc.

PD CONTROL ON THE EUCLIDEAN GROUP

where k k is the standard Euclidean norm and

cos k k b + 1 ? sin k k b2 : A( ) = I + 1 ? k k k k k k k k2 Equation (2.3) is also known as Rodrigues' formula. In an open neighborhood of the origin dense in G, we de ne X = log(g ) 2 g to be the exponential coordinates of the group element g and we regard the logarithmic map as a local chart of the manifold G. Lemma 2 (Logarithmic map). Let (R; p) 2 S O(3) R3 be such that tr(R) 6= ?1. Then logSO(3)(R) = 2 sin (R ? RT )

2 so(3);
(2.4)

1 (tr(R) ? 1) and j j < . Also where satis es cos = 2 ?1 ( )p b logSE (3)(R; p) = 0 A 1 2 se(3);

where b = logSO(3)(R) and

1 b + ?1 ? (k k) b2 A( )?1 = I ? 2 k k2 and (y ) , (y=2) cot(y=2).

(2.5)

Note that elements of the Lie algebra g can represent a velocity as in equation (2.1) or can represent the matrix logarithm of the state (and should therefore be considered states) as in equation (2.4). We denote them with V = (! b ; v ) in the rst case and with X = ( b; q ) in the second (also we usually have g = (R; p) 2 S E (3)). Example 1 (A few useful identities). With the aid of Mathematica it is easy to verify the following identities:

A( )?1R( ) = R( )A( )?1 = A( )?T A( )R( ) = R( )A( ) = 2A(2 ) ? A( ) d A( ) = 1 (R ? A); dk k k k

where R( ) = expSO(3)( b).

(2.6) (2.7) (2.8)

Example 2 (Exponential and logarithmic map on S E(2)). Regarding the group of planar motion, let b : R ! so(2) map to 0 ? 0 : Given

6
b

BULLO AND MURRAY

sin cos b expSE (2)(X ) = expSO(2)( ) A( )q ; 0 1 sin ?(1 ? cos ) . where A( ) = 1 (1 ? cos ) sin 2 Let (R; p) 2 S O(2) R be such that tr(R) 6= ?2. Then logSO(2)(R) = b, where cos = R11, sin = R21 and j j < . Also ?1 b logSE (2) (R; p) = 0 A 1( )p 2 se(2); =2 . where b = logSO(2)(R) and A( )?1 = ? ( =) 2 () Note that singularity is at tr(R) = ?1 for S O(3) and tr(R) = ?2 for S O(2). 2.2. The Jacobian of the exponential map. We now want to compute explicit formulas that relate the time derivative of X (t) = log(g (t)) with the body and spatial velocities V b ; V s . For the linear time dependence case _ = Y = V b = V s ; for the generic case (X (t) = tY ), it is easy to show that X X = X (t) the relationship is not trivial. Theorem 1 (Integral Formulas). Let g(t) be a smooth curve on G, X (t) = log(g (t)) be the exponential coordinates of g (t), V b = g ?1 g _ the body velocity and V s = g _ g ?1 the spatial velocity. _ and V b ; V s through: Then we can relate X Z 1 _ )d ; V b = Ade? X t (X (2.9)

2 so(2) and X = (b; q) 2 se(2), the formulas above become exp ( b) = cos ? sin
SO(2)

Vs = de dt

Z 1

( )

Proof. For all 2 0; 1], de ne V b as the solution to the algebraic equation

_ )d : Ade X t (X
( )

(2.10) (2.11)

Vb : Note that we here want to compute explicitly V b V1b .

X (t) = e X (t)

Following 14], we prove the desired result by equating the two mixed derivatives of the smooth quantity f (t; ) = e X (t) in equation (2.11). We have d d e X (t) = X (t)e X (t) V b + e X (t) d ( V b )

d dt

de = X (t) dt

X (t)

d + e X (t) dd ( V b ):

(2.12)

PD CONTROL ON THE EUCLIDEAN GROUP

hd i _ (t)e X (t): = X (t) dt e X (t) + X Equations (2.12) and (2.13) give _ (t)e X (t); e X (t) dd ( V b ) = X or d ( V b ) = e? X (t)X _ ): _ (t)e X (t) = Ade? X t (X d We now integrate with respect to from 0 to 1 to obtain Z 1 b b b _ )d : V = 1 V1 ? 0 V0 = Ade? X t (X

Di erentiating with the reverse order yields

d de dt d

X (t)

(2.13)

( )

The corresponding equality on the spatial velocity follows from the basic equality V s = Adg (V b ) and a simple change of variable = 1 ? :

( )

Vs

= Adg = =
Z 1 Z 1

(V b ) = Ad

eX

Z 1

_ )d AdeX ? X (X _ )d : Ade X (X

_ )d Ade? X (X

With the same notation we have the following Jacobians: Theorem 2 (Di erential of exponential). Let g(t) be a smooth curve on G, X (t) = log(g(t)) be the exponential coordinates of g(t), V b = g ?1 g _ the body velocity and V s = gg _ ?1 the spatial velocity. _ and V b ; V s through: Then we can relate X 1 n Bn X b _ X = (?1) adn (2.14) X (V ); n ! n=0 1 X n adn (V s ); = B (2.15) X n n=0 ! where fBn g are the Bernoulli numbers. Remark 1. Note that equations (2.14) and (2.15) represent the in nitesimal version of the Campbell-Baker-Hausdor formula. Indeed, in their original work 9, 2, 12] similar relationships are derived. Proof. Recall the basic matrix equality 1 n n n X Ade? X ead? X = (?1) n! adX ; n=0

BULLO AND MURRAY

Z 1

and the simple equalities

0

ud

u = e ? 1;

Z 1

?u e? u d = 1 ?ue :
i

From previous lemma we have

Vb =
=

hZ 1

hZ 1

e?

adX d
i

_) (X _) (X

h ?u i _ ); = 1 ?ue (X (2.16) u=adX where the expression f (u)ju=adX means: take the Taylor expansion of f about u = 0 and substitute the linear operator adX for all u. That is:

e? u d

u=adX

1 X (?1)n adn (X b V = (n X _ ): + 1)! n=0

_ in equaWe now want to invert the linear relationship between V b and X tion (2.16). As it is proven in 22, Lemma 2], this can be easily done by inverting f (u): h u i b _= X 1 ? e?u u=ad (V ); which explicitly written as a matrix series is _= X
1 ( X
n=0
X

?1)nBn adn (V b): X n!

Similarly for the spatial velocity

1 i h X s _ = uu (V s ) = Bn adn X X (V ): e ? 1 u=adX n ! n=0

In the following we will sometime write equation (2.14) as _ = BX V b ; X where with the symbols BX we denote

BX =

1 X

(?1)n Bn adn : X n=0 (n)!

PD CONTROL ON THE EUCLIDEAN GROUP

Recalling that B2k+1 = 0 for all k > 0, the two series (2.14) and (2.15) di er only in the second addend: _ = V b ? B1 adX (V b ) + X 2 B2m ad2m (V b ) X (2 m=1 m)! 1 1 ad (V s ) + X B2m ad2m (V s ): =Vs+ B X 2 X m=1 (2m)!
1 X

Also, since B0 = 1; B1 = ?1=2; B2 = 1=6; B4 = ?1=30, the rst terms look like b _ = V b + 1 adX (V b ) + 1 ad2 X 2 12 X (V ) ? : : : s ) + 1 ad2 (V s ) + : : : : ad ( V =Vs? 1 X 2 12 X

Note that, for small X , the matrix series in equation (2.14) is full rank and _ = V b = V s. absolutely convergent, in particular at X = 0 we have X It is instructive now to sum the matrix series in equation (2.14) for the important cases of S O(3) and S E (3). In general, since the dimension of G is nite, say N , the rank of the linear operator adX is also at most N and by the Cayley-Hamilton theorem, there exist some function a1 (X ); : : : ; aN (X ) such that
+1 adN X =

N X i=1

ai (X ) adiX :

(2.17) (2.18)

Additionally note that adX X = 0 for all X 2 g, so that the rank of adX is at most N ? 1. We start by considering the S O(3) case: group elements are rotation matrices and we denote them with the standard symbol R. The natural isomorphism between the Lie algebra so(3) and R3 is given by the x b operator and satis es x b; y b] = (\ x y); so that the standard outer product on R3 corresponds to the bracket adX on so(3). Thus, for simplicity, we refer to R3 as the Lie algebra of S O(3). Simple computations show that equation (2.17) reduces to

x b3 = ?kxk2x b:

(2.19)

Lemma 3 (Time derivative of exponential coordinates on SO(3)). Let R(t) be a smooth curve on S O(3) such that tr(R(t)) = 6 ?1. Let b(t) = log(R(t))

10

BULLO AND MURRAY

_ the body angular vebe the exponential coordinates of R(t) and ! b = R?1 R locity. Then we have _ = !k + 1 ( ! ) + (k k)!? 2 b2 ? b + 1 ? (k k) (2.20) = I+1 2 k k2 ! where (y ) , (y=2) cot(y=2) and ! = !k + !? is the orthogonal decomposition of ! along spanf g and spanf g? . Remark 2. To the authors' knowledge this expression is novel and relates the time derivative of the angle-axis quantity with the body angular velocity ! . In a very peculiar way, it happens to hold that _ = A( )?T ! with A( )?1 de ned in (2.5). Proof. Identifying so(3) with R3, it holds
1 _ = ! + 1 b! + X B2m b2m!: 2 m=1 (2m)!

From equation (2.19) we have the relation b2m = ?k k2 b2(m?1) = (?1)m?1k k2(m?1) b2: Thus # " 1 2 X B2m 1 m?1 k k2m b ! _ = ! + b! + ( ? 1) 2 k k2 m=1 (2m)! 1 b! + ?1 ? (k k) b2 !; =!+ 2 k k2 where the last equality follows from the Taylor expansion of cot( ). Additionally notice that that is the orthogonal projection along the spanf g? . Thus we can write _ = ! + 1 b! ? (k k)prspanf g? ! 2 1 = ! + 2 b! ? (k k)!? b = !k + 1 2 ! + (1 ? (k k))!?

k k2 = ?prspanf

b2

g? ;

PD CONTROL ON THE EUCLIDEAN GROUP

11

where, once again, ! = !k + !? is the orthogonal decomposition of ! along spanf g and spanf g? , that is:

h ; !i ; !k , prspanf g (!) = h ; i !? , prspanf g? (!) = ! ? !k:

Also, note that this particular result can also be proved through the differentiation of Rodrigues' formula (2.3). Di erentiate Rodrigues' formula, multiply by g ?1 and express g ?1 g _ only as a function of b = log(g ). We also have a corresponding expressions for the S E (3) and S E (2) cases: Lemma 4 (Time derivative of exponential coordinates on S E(3)). Let g(t) = (R(t); p(t)) be a smooth curve on S E (3) such that tr(R(t)) 6= ?1. Let X (t) = ( b; q) = log(g(t)) be the exponential coordinates of g(t) and V b = g?1g _ be the body velocity. Then we have _ = B X (V b ) X 2 4 b = id + 1 2 adX +A(k k) adX +B (k k) adX (V ); where y2A(y) = 2 1 ? (y)] + 1 2 (y ) ? (y )] ; y 4B(y) = 1 ? (y)] + 1 2 (y ) ? (y )]; and (y ) = (y=2) cot(y=2), (y ) , (y=2)2 = sin2 (y=2). Additionally the operator BX can be written as ?T BX = A( ?) A( 0)?T : (2.21)
Proof. Consider the expression of adX in equation (2.2). If X = ( b; q ) 2 se(3), then simple algebraic computations show that 2 4 4 2 ad6 X = ?2k k adX ?k k adX : Substituting this relationship into equation (2.14) of Lemma 2, the result follows after tedious computations, see Appendix A. For the S E (2) case, it is possible to compute a more explicit expression: Lemma 5 (Time derivative of exponential coordinates on S E(2)). Let g(t) = (R(t); p(t)) be a smooth curve on S E (2) such that tr(R(t)) 6= ?2. Let X (t) = ( b; q) = log(g(t)) be the exponential coordinates of g(t) and V b = g?1g _ = (! b ; v ) be the body velocity.

12

BULLO AND MURRAY

d A( )?1 Aq + A?1 Rv _ =! d q_ = ! dd A( )?1 p + A?1 p = ?!A?1 dd A( )q + A?1 Rv = ? ! A?1 (R ? A)q + A?1 Rv = ! (I ? A?1 R)q + A?1 Rv = ! (I ? A?T )q + A?T v _ ?1 A = ?A?1 A _ and equation (2.8), equation (2.6). The where we used A nal result is obtained by substituting the de nition of A?T . 2.3. Metric properties on compact Lie groups. On any Lie group G, the Killing form h ; iK is de ned as the bilinear operator on g g: hX; Y iK , tr(adX adY ) 8X; Y 2 g: A Lie group is said to be semi-simple if h ; iK is nondegenerate. For compact Lie groups h ; iK is both nondegenerate and negative de nite, so that by

Then we have _=! q_ = ! (I ? A( )?T )q + A( )?T v ( ) =2 ?1=2 = ! (1 ? 1=(2 ))= (1 ? ( ))= q + ? =2 ( ) v: Proof. Di erentiating with respect to time q = A( )?1 p, we obtain

a simple multiplication with a negative constant, we can de ne an inner product on the Lie algebra g (e.g. on so(3) h ; i , ?1=4h ; iK ). An inner product de ned this way will satisfy the crucial property of Ad-invariance : hX; Y i = hAdg X; Adg Y i; 8g 2 G; where Ad is therefore an orthogonal operator of g. Equivalently the matrix commutator satis es hadZ X; Y i = ?hX; adZ Y i 8Z 2 g: (2.22) Now, an Ad-invariant inner product on the algebra g induces a Ad-invariant metric on the group G by either left or right translation: this gives the additional structure of a Riemannian manifold to the group G. Without entering details, we refer to 4] and we simply state the following result: Proposition 1. With respect to an Ad-invariant metric, the geodesics of G are the one parameter subgroups, that is the curves of the form exp(Y t), with Y 2 g constant. Furthermore, the distance between the element g and the identity eG = I 2 G is given by the norm of the logarithmic function: kgkG = hlog(g); log(g)i1=2: (2.23) The computational result we are interested in is an extension of Gauss's Lemma (see 4] and 8]), obtained thanks to property (2.22) and equation (2.23).

PD CONTROL ON THE EUCLIDEAN GROUP

13

Theorem 3 (Derivative of distance function). Let G be a compact Lie group with bi-invariant metric h ; i. Consider a smooth trajectory g (t) 2 G, such
that g (t) never passes through a singularity of the exponential map. Then 1 d kg k2 = hlog(g ); V b i = hlog(g ); V s i: 2 dt G

3. PD control on S O(3) We begin with the problem of stabilizing a control system evolving on a compact, semisimple Lie group. Without loss of generality we will here consider only the S O(3) case. As explained in the previous section, a biinvariant Riemannian metric is naturally de ned on S O(3) and allow us to easily design appropriate Lyapunov functions. We begin by brie y describing our approach for a simple rst order system on S O(3), described as in equation (2.1) by g _ = gV b . Consider the natural candidate Lyapunov function 1 kg k2 ; W (g) = 2 SO(3) and assume we can directly control the quantity V b 2 so(3) to any desired value (i.e. the system is fully actuated). Then the proportional control action V b = ?kp log(g); kp > 0; (3.1) leads to _ (g (t)) = hlog(g ); ?kp log(g )i = ?2kpW; W thanks to Theorem 3. Thus, for this rst order system, a logarithmic control law ensures exponential stability for all initial conditions g (0) such that tr(g (0)) 6= ?1. Now, motivated by standard control problems in mechanics and robotics, we consider the stabilization problem for second order systems, that is for systems where we have full control over forces (accelerations) rather than velocities. A second order system on S O(3) has the form ( g _ = gV b (3.2) _ b = f (g; V b ) + U; V where g 2 S O(3) is the con guration of the system, f (g; V b ) 2 so(3) is the internal drift, and U 2 so(3) is the control input. Note that we once again assume that the system is fully actuated. To regulate the con guration g to the identity matrix I 2 S O(3), we couple the proportional action (3.1) with a derivative term, i.e. with a term proportional to the velocity V b . Theorem 4 (PD plus feedforward control on S O(3)). Consider the system in equation (3.2) and let Kp and Kd be symmetric, positive de nite gains.

14

BULLO AND MURRAY

Then the control law

notation, we can de ne the candidate Lyapunov function as " # idso(3) idso(3) log(g ) 1 log( g ) W = 2 h V b ; id ?1 V b iso(3) so(3); Kp so(3) where idso(3) is the identity map on so(3), the inner product is taken in so(3) so(3) and is taken small enough. The closed loop system satis es g _ = gV b _ b = ?Kp log(g ) ? Kd V b : V We now drop the subscript and write the previous system in exponential coordinates X = log(g ) 2 so(3) to obtain 8 1 n X > > b _ = (?1) Bn adn <X X (V ) = BX V n ! n=0 > > : _ V = ?KpX ? Kd V;
P
n

U = ?f (g; V b ) ? Kp log(g) ? Kd V b ; (3.3) exponentially stabilizes the state g at I 2 S O(3) from any initial condition tr(g (0)) 6= ?1 and for all Kp and V b (0) such that kV b(0)k2 : (3.4) min (Kp) > 2 ? kg(0)k2 SO(3) where min (Kp) is the minimum eigenvalue of Kp. Proof. We will here rely on the properties of the inner product on so(3). Let idso(3) be the identity automorphism of so(3). With a slight abuse of

(?1) Bn n where we have de ned BX , 1 n=0 n! adX . Di erentiating with respect to time our candidate Lyapunov function we have d W = hX; B V i + hV; K ?1V _i X p _ i + hBX V; V i + hX; V

dt

?1(?KpX ? Kd V )i = hX; V i + hV; Kp + hBX V; V i + hX; ?KpX ? Kd V i

?1Kd V i ? hX; KdV i = ? hX; KpX i ? hV; Kp + hBX V; V i The last term can be upper bounded by hV; V i using Lemma 11 in Appendix B, so that d X X ?1 dt W 2 h V ; Q V iso(3) so(3)

PD CONTROL ON THE EUCLIDEAN GROUP

15

where

Q =

Kp Kd =2

?1 is equal to the where we use the fact that the maximum eigenvalue of Kp inverse of the minimum eigenvalue of Kp. By the previous equation, g (t) can never become a rotation of radians and therefore the singularity of the logarithmic function is never reached.

is positive de nite for small . Local exponential stability is therefore proven. We now show that condition (3.4) provides a su cient bound in order for the closed loop trajectories to avoid the singularity of the logarithmic map. Note that W0(t) is a non increasing function (since Q0 is negative semide nite) and that 1 kg (t)k2 SO(3) W0 (t) W0 (0) 2 b ?1 b = 1 kg (0)k2 SO(3) + hV (0); Kp V (0)iso(3) 2 1 kg (0)k2 + ?1 )kV b (0)k2 max(Kp SO(3) 2 1 kg (0)k2 + 1 kV b (0)k2 < 2=2; SO(3) 2 (K )
min

Kd =2 ? 1 Kp Kd ? idso(3)

Notice that the proof follows the same steps as the usual one in Rn. The introduction of the cross term, proportional to a small , is a well-known trick. See, for example, Wen and Bayard 33] or Murray et al 26]. Remark 3. We have written the control law (3.1) and Theorem 4 in terms of the body velocity V b , i.e. we assumed \body- xed" control inputs. A dual version can be easily written for the opposite case of \spatial- xed" _ s = f (g; V s ) + U . Thanks to Theorem 3 a control inputs, i.e. for the case V logarithmic control law is the correct choice also for this case. Example 3 (Orientation control of a satellite). A standard example of a control problem on a compact Lie group is attitude control of a satellite. In the literature, various PD control laws based on di erent parametrization of the manifold S O(3) have been proposed: Euler angles 30], Gibb's vectors 31] and unit quaternions 34]. In particular, Wen and KreutzDelgado 34] introduce the idea that the \error measure should correspond to the topology of the error space". Here we additionally require that the error measure correspond to the (natural) metric of the Riemannian manifold S O(3). The second order model of a satellite is
(

g _ = g! b b; J! _ b = f (g; ! b) + ;

16

BULLO AND MURRAY

where the control inputs is the total torque applied to the satellite either by momentum wheels or by gas jet actuators. The internal drift is

f (g; ! b) =

gT m0 ; !b] momentum wheels J! b ; ! b] gas jet (Euler equations).

Following early work by Koditschek 19], we introduce a slight modi cation to the design of Theorem 4 and we adopt the modi ed Lyapunov function p kg k2 + 1 h! b ; J! b iR + hlog(g ); J! bi W=k SO (3) 2 2 where the second term has the interpretation of kinetic energy. This leads to the feedback law = ?kp log(g ) ? Kd! b (3.5)
3

where we write the control law in R3 making use of the isomorphism b given in Section 2. Note that in equation (3.5) we are not canceling the nonlinear Coriolis forces, but instead we are exploiting their intrinsic passivity properties. This procedure is what we refer to as Koditschek's approach 19]; its drawback is that we need to restrict ourselves to scalar proportional gains kp (the derivative gain Kd can remain a (positive de nite) matrix). This is somehow a characteristic behavior (see the complete example on S E (3) in the following section for more details). This feedback has strong similarities to the ones already proposed in the literature: it is instructive to compare it with the equivalent proposed by Wen and Kreutz-Delgado 34]. Both laws consist of the sum of a proportional and derivative action, where they di er is in the expression of the proportional term. In particular along the \geodesic" direction (equal to the rotation axis of the attitude matrix g ), the two laws di er in the intensity of control action. Our feedback relies on the notion of group norm (as de ned in equation (2.23)) and is proportional to this quantity. Instead the control laws proposed by Wen and Kreutz-Delgado are based on either the 2-norm of the unit quaternion or the 2-norm of the vector quaternion, and therefore exert an action proportional to either sinkg k or 2 sin(kg k=2). 4. PD control on S E (3) We now consider the extension of the results in the previous section to S E (3), the special Euclidean group of rigid-body motions. As described in the introduction, this Lie group is common in robotic applications. Unfortunately, since this S E (3) is not compact, the results of the previous section cannot be extended directly. As before, we begin by studying the simple rst order case and we then couple proportional with derivative action for second order systems. Finally we apply our results to the case of mechanical manipulators and we then report some simulations.

PD CONTROL ON THE EUCLIDEAN GROUP

17

4.1. Proportional actions on SE(3) and rst order systems. The geometric properties of the group S E (3) have received much attention in the recent control literature 7, 26] and a very complete treatment is contained in 28]. A well-known negative result is the following: no symmetric bilinear form on se(3) can be both positive-de nite and Ad-invariant. There is therefore an algebraic obstruction to the procedure we have followed for the S O(3) case. Recall the design procedure: we need a positive-de nite bilinear form (hence an inner product) to construct a Lyapunov function W , and we need the Ad-invariance of this form to compute the time derivative of W (Theorem 3). Therefore we here brie y consider bilinear forms de ned on se(3). Let Vi = (!i ; vi) for i = 1; 2, we have 1. A linear combination of Klein and Killing form: the most generic Adinvariant form on se(3) looks like ? hV1; V2iAd?inv = h!1; !2i + h!1; v2i + h!2; v1i ; where with h ; i we indicate the standard inner product on R3, 2. The standard inner product on se(3) = R6: discard the Lie algebra structure of se(3) and write hV1; V2iR = h!1; !2i + hv1; v2i: (4.1) Hence we are left with two possible design choices: as proportional action we can insist on the logarithm function (which no longer corresponds to the geodesic direction of a Riemannian metric), or (giving up the Adinvariance) we can still regard S E (3) as a metric space with respect to the inner product (4.1) and compute the correct proportional action within this new framework. 2 The two procedures are illustrated in Figure 1 for the case of left invariant control systems g _ = gV b ; the following two lemmas formalize this discussion. Lemma 6 (Logarithmic feedback). Consider the left invariant system g _= gV b on S E(3) and let kp > 0. Then the control law V b = ? k!0I3 (k! +0kv )I3 log(g) (4.2) exponentially stabilizes the state g at I with time constant kp, from any initial condition g (0) = (R(0); p(0)) such that tr(R(0)) 6= ?1. Proof. In exponential coordinates log(g ) = X = ( b; q ) 2 se(3), the closedloop system is _ = BX V b: X
6 2 Given an inner product on g, we can extend it to the whole T G by either left or right translation: we end up therefore with a metric structure on G. We refer to 4] for a detailed treatment of this standard construction.

18

BULLO AND MURRAY

Using the equations (2.18) and (2.21), we have B X = X and B 0 =

X X

= ?k! X ? kv A( 0 )?T q : Separating the rotational and translational parts _ = ?k! q_ = ?k! q ? kv A( )?T q: Regarding the rotational part, exponential stability is proven for all R such that tr R 6= ?1 (in order for the exponential coordinates to be de ned). Regarding the translational part, consider the candidate Lyapunov function 1 kq k2. Its time derivative satis es W=2 _ = ? B X k! X + k v 0 X q

so that

A( )?T q

lational part. Finally, since is a decreasing function of time, the closed loop trajectories will not encounter the singularity points of the logarithmic function, as long tr(R(0)) 6= ?1. The second approach is based on the decomposition of the control system on S E (3) into a control system on S O(3) R3. Recall the notation introduced in Section 2: g = (R; p), V s = (! b s ; v s), V b = (! b b ; v b). The original b s systems g _ = gV and g _ = V g reduce to _ = R! _ = ! R bb R bs R and p _ = !s p + vs: p _ = R vb Indeed adopting the bilinear form (4.1) involves applying a proportional action along geodesic directions for both the subsystems in S O(3) and R3 (therefore we call such approach double-geodesic). Lemma 7 (Double-geodesic feedback). Consider the left invariant control system g _ = gV b on S E (3) and let Kv ; K! be positive de nite symmetric gains. Then the control law ( !b = ?K! logSO(3)(R) v b = ?RT Kv p

d W = ?hq; k q + k A( )?T qi ! v dt = ?k! kq k2 ? kv kqkk2 + (k k)kq?k2 ; where last equality is obtained using the de nition of A?1 in equation (2.5) and where q = qk + q? is the orthogonal decomposition of q along spanf g and spanf g? . Thus local exponential stability is proven also for the trans-

PD CONTROL ON THE EUCLIDEAN GROUP

19

Figure 1. Proportional actions on S E (2).

From left to right: logarithmic function (1-parameter subgroups on S E (2)) and double-geodesics for S O(2) R2. Each point g 2 S E(2) is depicted as a frame on the plane.

exponentially stabilizes the state g at I , from any initial condition g (0) = (R(0); p(0)) such that tr(R(0)) 6= ?1. Proof. In rotational and translation coordinates the closed loop system is ( _ = R(?K! log(R)) R p _ = ?Kv 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 ) and some instructive behavior can be easily described. In the following, let gleft(t) and gright(t) the solutions to the left and right closed loop systems: To examine the logarithmic control law applied to a right invariant system, recall the basic Lie group identity Adg log(g ) log(g ). Then the closed-loop systems (with unit gains), g _ left = ?gleft log(gleft) and g _ right = ? log(gright)gright; are the same di erential equation and we have gleft(0) = gright(0) =) gleft(t) = gright(t) (4.3) Regarding the double-geodesic control law we can state a similar but opposite result. The control law for this case is ( !s = ?K! logSO(3)(R) v s = ?Kv p;

20
(

BULLO AND MURRAY

and (with unit gains) the left and right closed-loop control systems are ( _ = ? logSO(3)(R)R _ = ?R logSO(3) (R) R R and p _ = ? logSO(3)(R)p ? p: p _ = ?p

Then easy algebraic steps show ?1 (0) =) gleft(t) = g ?1 (t): (4.4) gleft(0) = gright right Note the peculiar correspondence between (4.3) and (4.4). 4.2. Second order systems . We now apply these proportional strategies, coupled with a derivative term, to second order, fully actuated systems on S E (3). Consider the left invariant second order system ( g _ = gV b (4.5) _ b = f (g; V b ) + U; V where f (g; V b ); U 2 se(3) are internal drift and control input. The previous discussion leads to the two theorems: Theorem 5 (Regulation via the double-geodesic law). Consider the system in equation (4.5) and let K! ; Kv and Kd be the positive de nite gains. Then the control law SO(3)(R) ? K V b ; U (g; V b ) = ?f (g; V b ) ? K! log (4.6) d T
exponentially stabilizes the state g at I from any initial condition g (0) = (R(0); p(0)) with tr(R(0)) 6= ?1 and for all K! and ! b (0) such that k!b(0)k2 : (4.7) ( K ) > min ! 2 ? kR(0)k2 SO(3) Proof. The scalar gain case (Kv = K! = kp ) admits a standard proof identical to the one of Theorem 4, but with the candidate Lyapunov function
p 2 b b W (g; V b ) = k k2 SO(3) + kpk + hV ; V iR 2 kR " # logSO(3)(R) + h ; V b iR : RT p
6 6

R Kv p

For the matrix gain case the notation becomes more involved but the algebraic steps are the same. In particular, also the su ciency of condition (4.7) (exactly corresponding to condition (3.4)) follows from the same steps as in Theorem 4. Theorem 6 (Regulation via the logarithm function). Consider the system in equation (4.5) and let Kp and Kd be positive-de nite gains. Then the control law U (g; V b) = ?f (g; V b ) ? Kp log(g) ? KdV b ; (4.8)

PD CONTROL ON THE EUCLIDEAN GROUP

21

locally exponentially stabilizes the state g at I 2 S E (3). Furthermore, if scalar gains are employed (Kp = kp I6 and Kd = kd I6 ), then the control law in (4.8) exponentially stabilizes the state g at I from any initial condition g (0) = (R(0); p(0)) with tr(R(0)) 6= ?1 and for all kp and ! b (0) such that

kp >

k!b(0)k2 : 2 ? kR(0)k2 SO(3)

(4.9)

Proof. Given the de nition of matrix logarithm on S E (3) in Lemma 2, the feedback law in equation (4.8) is equal up to higher order terms to the one in equation (4.6). Thus local exponential stability is ensured. For the scalar gains case, we can prove almost global exponential stability. Consider the closed loop system: g _ = gV (4.10) _ = ?kp log(g ) ? kdV; V (4.11)

starting from initial conditions (g (0); V (0)) = (g0; V0) 2 S E (3) se(3). Since equation (4.11) is linear, we can decompose the solution V as the sum of two components V = Vhom + Vpar, where Vhom(t) = V0 exp(?t=kd ) and Vpar is the solution of (4.11) with zero initial condition (and considering the log(g ) term as an external disturbance). Notice now that the manifold M = f(g; V ) : V = log(g); for some 2 Rg S E(3) se(3) is invariant for the system of ODEs (4.10) and (4.11) with initial conditions (g (0); V (0)) = (g0; 0). For, consider the system expressed in exponential coordinates _ = BX V X _ = ?kp X ? kd V V and subsitute X = V to obtain _ = BX X X = X _ = ? kp V ? kd V = ? kp + kd V: V Hence for all t, X (t) 2 span X (0) and V (t) 2 span X (0), provided V (0) = X (0). It is now easy to show that the invariant manifold M is stable, since assuming X = x vers(X (0)) and V = v vers(X (0)) we have x _ =v _ (4.12) v _ = ?kp x ? kd v ) x = ?kp x ? kd x:

22

BULLO AND MURRAY

Example 4 (Workspace control of mechanical systems). As in Example 3 for S O(3), we here apply our control strategies to fully actuated mechanical systems. Examples of this class of systems are robotic manipulators and 6 degree of freedom (DOF) underwater vehicles. We assume here that a change of coordinates and inputs has already been applied to the system so that our model is described by ( g _ = gV b _ b = ?C (g; V b )V b ? N (g; V b ) + U; M (g)V where M (g ) is the inertia matrix, C (g; V b ) is the Coriolis matrix and N (g; V b ) is used to model friction and gravity. The kinetic energy of this mechanical system is computed with the positive de nite form (4.1) (coupled with the left translation of the velocity gV b ). Hence, for this class of systems, we are naturally lead to prefer the double-geodesic control law over the logarithmic one: U (g; V b ) = N (g; V b ) ? k! logSOT(3) (R) ? KdV b : (4.13)
Exponential stability is proved through the Lyapunov function

BX Vhom decreases to zero exponentially. Lemma 4.7 where the disturbance ? kd Vhom in Khalil 18] applies proving local exponential stability. Additionally, if condition (4.9) is satis ed, then with the same bounding technique in the proof of Theorem 4, we can prove that no singularity will be encountered by the closed loop trajectories. Remark 5. As usual we can extend to the right invariant case (g _ = V bg) all we have done for the left one. For both systems the logarithmic control law (in Theorem 6) is identical. The double-geodesic law applied to a right system has the slightly di erent expression: SO(3)(R) ? K V s : U (g; V s ) = ?f (g; V s ) ? K! log d Kp
v

The proof is now complete by noting that the original system (4.10) and (4.11) can be written as _ = BX V X = BX Vpar + BX Vhom _ = ?kp X ? kd V = ? kpX ? kd Vpar ? kd Vhom V

kv R p

Once again, in writing equation (4.13) we take advantadge of the passivity properties of the Coriolis term C (g; V b )V b and we compensate only for

kv 2 b b W (g; V b ) = k2! kRk2 SO(3) + 2 kpk + hV ; M (g )V iR logSO(3)(R) + h ; M (g)V b iR : RT p

6

PD CONTROL ON THE EUCLIDEAN GROUP

23

a more natural way of controlling fully actuated mechanical systems, see Koditschek's early work 19] for more details. A few remarks: 1. The control law in equation (4.13) has the usual advantages of PD control described in 26]: ease of computation and no knowledge of the exact system's parameters required. 2. A second approach would involve a typical \computed torque" technique, where the Coriolis term C is explicitely compensated for. In this latter case, the logarithmic control law of Theorem 6 can be applied. Future avenues of research consists in the application of the logarithmic control law for the case of robotic manipulators for the purpose of hybrid (position/force) control and the study from a (Lie group) algebraic viewpoint if simpli cations occur in the expression of the Jacobian manipulator (again, when logarithmic control law is applied). Example 5 (Position and attitude stabilization of planar rigid body). To compare the two classes of controllers presented above, we consider the problem of stabilizing a planar rigid body. Note that the subgroup of the planar motions S E (2) contains still most of the complexity and richness of the full S E (3) case. We have simulated the feedback laws described in Theorem 5 and 6 (double-geodesic and logarithmic laws for left invariant systems), and in Remark 5 (double-geodesic and logarithmic law for right systems). As foreseen from theoretical considerations, the logarithmic control law generates the same closed-loop trajectories for both the right and the left invariant systems. The shape of the trajectories for all of the cases varies considerably depending on the size of the initial angle error and on the gain values: for all cases we picked an initial rotational error equal to =2 and we choose two sets of scalar gains: (kp; kd) = (1; 2) and (kp; kd) = (1; 1). We here report the S E (2) trajectories for the 4 controllers with the rst set of gains (Figures 2 and 3) and the corresponding velocity pro les for the two left invariant controllers, (Figures 5 and 6). Also we show in Figure 4 how the trajectories change when a low derivative gain is applied (low with respect to a constant proportional gain). Looking at the plots in Figure 2 and 3 a few simple remarks can be made: 1. In agreement with the fact that the various feedbacks are equal in the rotational part, the angular behavior is the same in all simulations. 2. All the control laws seem to converge at a very similar rate in both the rotational (of course) and translational part. This is also predictable since identical gains are applied. Indeed, quantitative results (which we don't report for brevity) indicate that the various input norms for the logarithmic control law are larger than for the double-geodesic strategy. Typically the logarithmic inputs would be about 10% larger than the double geodesics (see Figure 5 and 6).

N (g; V b ). Rather than canceling both terms, this approach appears to be

24

BULLO AND MURRAY

3. Qualitatively, the clearest di erence regards the opposite handedness of the various control laws. Corresponding to a choice of left invariant control system the logarithmic and double-geodesic feedbacks will follow quite di erent paths even from a simple qualitative viewpoint (Figure 2). In the right invariant case instead the handedness is the same, but the double-geodesic law shows a more curved behavior (Figure 3). 4. The di erence in the shape of trajectories becomes even clearer in second simulation in Figure 4 where a low derivative gain kd is employed and where the e ects of the di erent proportional actions is therefore emphasized. Note the oscillatory behaviour of both closed loop system: the values (kp ; kd) = (1; 1) corresponds to an slightly damped second order systems. This kind of behaviour seems therefore mantained by our nonlinear models. The issues described in Remark 4 on symmetries of control laws and the proof of Theorem 6, nd clear illustration in the case of high derivative gain ( rst set of simulations). Notice that: 5. both left and right invariant closed loop systems with logarithmic control law remain on the 1-parameter subgroup of S E (3) determined by the initial conditions (see right pictures in Figure 2 and 4). In Figure 5 and 6 we report the time evolution of the velocity V = (vx ; vy ). In the logarithmic control case, since the state remains on a 1-parameter subgroup, the ratio of the inputs vx =vy remains constant during the simulation, see Figure 6 compared to Figure 5. These facts are predicted and are at the basis of the proof of Theorem 6. 6. modulo the di ering initial conditions, we recover the trajectories of the left double-geodesic closed loop system by inverting the right doublegeodesic trajectories.

PD CONTROL ON THE EUCLIDEAN GROUP

Left system with DoubleGeodesic control law Left system with Log control law

25

0.5

0.5

y (cm)

y (cm) 1 0.5 0 x (cm) 0.5 1

0.5

0.5

1 1 0.5 0 x (cm) 0.5 1

on S E (2). From left to right: double-geodesic control law as in Theorem 5 and logarithmic control law as in Theorem 6. Each point g 2 S E (2) is depicted as a frame on the plane. Note the opposite handedness of the two control strategies.
Right system with DoubleGeodesic control law Right system with Log control law 1 1

Figure 2. Trajectories of left invariant control systems

0.5

0.5

y (cm)

y (cm) 1 0.5 0 x (cm) 0.5 1

0.5

0.5

1 1 0.5 0 x (cm) 0.5 1

Figure 3. Trajectories of right invariant control systems

on S E (2). From left to right: double-geodesic control law as in Remark 5 and logarithmic control law as in Theorem 6.

26

BULLO AND MURRAY

Left system with DoubleGeodesic control law Left system with Log control law

0.5

0.5

y (cm)

y (cm) 1 0.5 0 x (cm) 0.5 1

0.5

0.5

1 1 0.5 0 x (cm) 0.5 1

on S E (2) with low derivative gain kd . From left to right: double-geodesic control law as in Theorem 5 and logarithmic control law as in Theorem 6. Note that the state of the closed loop system with logarithmic control law (on the right) remains on the 1-parameter subgroup determined by the initial condition. The double-geodesic control law instead gives rise to a spiraling (x; y ) motion (on the left).

Figure 4. Trajectories of left invariant control systems

PD CONTROL ON THE EUCLIDEAN GROUP

v (cm/sec) v (cm/sec) v (cm/sec) 0.5 0 0.5 0 5 t (sec) 0.5 0 0.5 0 5 t (sec) 0.5 0 0.5 0 5 t (sec)

27

v (cm/sec)

0 0.5 0 5 t (sec)

v (cm/sec)

0.5

0.5 0 0.5 0 5 t (sec)

v (cm/sec)

v (cm/sec)

0 0.5 0 5 t (sec)

0 0.5 0 5 t (sec)

v (cm/sec)

0.5

0.5

0.5 0 0.5 0 5 t (sec)

Figure 5. Velocity pro les of double-geodesic control law for the simulation depicted in Figure 2. The eight gures show the time evolution of vx (t); vy (t) from the eight initial conditions: the location of the 8 pictures corresponds to the 8 initial condition in the (x; y ) plane.
v (cm/sec) v (cm/sec) 0 0.5 0 5 t (sec) 0 0.5 0 5 t (sec) v (cm/sec) 0.5 0.5 0.5 0 0.5 0 5 t (sec)

v (cm/sec)

0 0.5 0 5 t (sec)

v (cm/sec)

0.5

0.5 0 0.5 0 5 t (sec)

v (cm/sec)

v (cm/sec)

0 0.5 0 5 t (sec)

0 0.5 0 5 t (sec)

v (cm/sec)

0.5

0.5

0.5 0 0.5 0 5 t (sec)

Figure 6. Same as above, but for the logarithmic control

law. Note that the input magnitude is slightly larger for logarithmic rather than for double-geodesic control law. Also, in the logarithmic case, the ratio vx =vy is constant for all initial conditions, since the state g 2 S E (2) remains on the 1-parameter subgroup exp( log(g (0))) for all time.

28

BULLO AND MURRAY

5. Trajectory tracking We describe here a general approach to trajectory tracking problems for second order systems de ned on Lie groups. In particular, by exploiting the group structure we reduce the tracking problem to a stabilization one for an appropriately de ned error system. In the following, we will assume that we are given a left invariant, second order control system on G ( g _ = gV; (5.1) _ V = f (g; V ) + U; and a control law U = Z (g; V ) that makes the closed loop driftless system _ = Z (g; V ) locally asymptotically (exponentially) stable at the g _ = gV; V identity eG = I . We want to design a control law that makes the state g track a reference trajectory gd 2 G described by: g _ d = Vd gd , for Vd (t) 2 g. 5.1. Choices of error function on SE(3). In order to de ne a correct notion of error function, we exploit the natural group structure of the conguration manifold, see for example 34]. Given the interpretation of group elements on S E (3) as coordinate frames, the natural choice of state error is which represents the reference frame as \seen" from the state of the system. In other words, if g represents the body frame and gd the desired frame, then e is the relative g to gd frame. Decomposing this error in its rotational and translational components, we have: T R RT (p ? pd ) d e = Rd : (5.2) 0 1 We call this the natural error. An equivalent de nition would be g ?1 gd e?1 , since, as described in Section 4, controlling e or e?1 is the same control problem. If we discard the physical reasoning associated with the natural de nition, other choices of con guration error are possible. In particular the following two appear appealing: 1. De ne the reciprocal error as
?1 = erecip , ggd ?1g; e , gd

where we exchange the order of multiplication. 2. In keeping with the notation in 26], de ne the hybrid error as

T RRT d p ? RRd pd ; 0 1

(5.3)

ehybrid ,

where we keep the natural error choice for the rotational part (we could instead have RRT d ).

RT d R p ? pd ; 0 1

(5.4)

PD CONTROL ON THE EUCLIDEAN GROUP

29

Even though these latter two de nitions seem also rather natural, the Lie group structure of the original problem is not taken into account. It happens in particular that reciprocal and hybrid error (between body and desired frame) depend on the arbitrary choice of inertial frame. For, recall that a change of inertial frame correponds to a left translation. Thus consider the map Lg such that Lg g = g0 g and Lg gd = g0gd . Then, if g0 = (R0; p0), we have
0 0 0

0 1 Note that for the hybrid error, this inertial frame depedency simply implies the presence of an arbitrary rotation in the translational part. For the reciprocal error instead, more evident e ects appear: for example, even for p = pd, i.e. for overlapping frames, erecip \sees" some translational error if R 6= Rd. This will re ect in a control law with non-zero translational input, which is undoubtedly undesired. Eventually note that the drawbacks just described a ect also the inverse 1 ?1 de nitions e? recip and ehybrid. Therefore, after this theoretical discussion, we tend to prefer the natural choice (5.2); the simulations performed later will further clarify the nal choice. 5.2. Basic properties of dynamical systems on Lie groups. We characterize here the behavior of the composition of systems de ned on the group G. Recall that, given l; r 2 G and L; R 2 g, we call l_ = lL a left control system and r _ = Rr a right control system. Also, given two systems with state g (t); h(t) 2 G, we call the inverse system the one corresponding to the state g (t)?1 and the product system the one corresponding to the state g(t)h(t). By performing some chain rule di erentiations, we have: Lemma 8 (Time derivative of composed systems). With the notation just introduced it holds l_?1 = ?Ll?1 r _ ?1 = ?r?1 R; and _ _ ) + h?1 h (gh)?1 d(gh) = Adh? (g ?1g
where the adjoint map Ad is de ned in Section 2. By means of these basic results, we are able to describe straightforwardly how, for instance, the product of two left control systems evolves in time:

T RT p ? R0 RRT RT pd d 0 d 0 e0recip = R0RR ; 0 1 T e0 = R Rd R0 (p ? pd ) :

hybrid

dt d(gh) (gh)?1 = g _ h?1 ); _ g ?1 + Adg (h dt

1

30

BULLO AND MURRAY

letting l_1 = l1L1 and l_2 = l2L2 , we have

which shows how we can write in full generality the second order dynamics of the combination of Lie group systems. 5.3. Extending regulators to trajectory trackers. We are now able to formulate the following general solution: Theorem 7 (Trajectory tracking). Consider the system in equation (5.1), the asymptotic (exponential) regulator law Z (g; V ) and the desired trajectory ?1 g 2 G and the velocity error gd(t). De ne the con guration error e , gd Ve , V ? Adg? Vd 2 g. Then the control law _ d) + Z (e; Ve) U = U (g; V ) + Utr(g; V; Vd; V (5.7) where U (g; V ) = ?f (g; V ) _ d ) + Adg? (Vd ); V ]; _ d) = Adg? (V Utr(g; V; Vd; V makes the con guration error e locally asymptotically (exponentially) approach the identity I 2 G. Proof. The result follows straightforwardly from equation (5.6). Indeed con?1g ans its velocity Ve = V ? Ad ? Vd . Then the adsider the error e = gd gd denda in equation (5.7) cancel out exactly the extra pieces in equation (5.6) and the closed loop system satis es _ e = Z (e; Ve ): V By assumption, asymptotic (exponential) stability is proven. A few comments: rst, for the S O(3) case we can simplify the tracking law _ d . The PD control law in equation (3.3) would still by de ning Utr = Adg? V ensure exponential stability thanks to the orthogonality properties discussed in Subsection 2.3.
1 1 1 1 1

d l l = l l (Ad ? L + L ): (5.5) 12 1 2 l dt 1 2 Lemma 9 (Derivative of adjoint map). Let U (t) 2 g, l_ = lL and r_ = Rr, with l; r 2 G and L; R 2 g. Then d _ dt Adl(t) U (t) = Adl U + Adl L; U ]; d Ad U (t) = Ad U r _ + R; Adr U ]: dt r(t) Now, recalling equation (5.5), we can de ne l12 , l1 l2 and V12 , Adl? L1 + L2. Lemma 9 gives: ( l_12 = l12V12 (5.6) _ 12 = Adl? L _ 1 + Adl? L1 ; L2] + L _ 2; V
2 1 2 1 2 1 2 1

PD CONTROL ON THE EUCLIDEAN GROUP

31

Second, in stating Theorem 7, we assumed our control system to be left invariant and our trajectory to be right invariant. These two choices are suited to the kind of mechanical systems we are interested in, such as example satellite reorienting and robotic manipulation. However, similar versions of the theorem can be stated using any combination of right and left invariant systems. Eventually, Theorem 7 can be stated for the other de nitions of error function: reciprocal and hybrid. 1. For the case of erecip, the results obtained in the previous subsection lead straightforwardly to ( e _recip = erecip (Adgd V ? Vd ) , erecipVe _ e = Adg (f (g; V ) + U ) ? V _ d + Vd; Adgd V ] V and similarly to equation (5.7) we set U = Adg? U + Utr + Z (erecip; Ve) ; _ d ? Vd; Adgd V ] and U = ?f (g; V ). where Utr = V 2. For the hybrid error instead, we want to keep the computations in local coordinates (R; p): from de nition (5.4) we have pe , p ? pd and Re = RT d R. For the rotational part Re 2 S O(3), we apply Theorem 7; for the translational part we have p _e = Rv ? vd pe = R(ft (g; V ) + a) ? v _ d + R(! v ); where a 2 R3 is the acceleration input and ft is the drift in the translational coordinates (see equation (5.1)). So we can write U = ( ; a), where is choosen from the previous theorem and a PD controller for the translation part gives a = a + atr + RT (?kp pe ? kdp _e ) (5.8) where atr = v ! and a = ?ft (g; V ). Example 6 (Position and attitude tracking on S E(2)). To compare the various error choice presented above, we consider the trajectory tracking problem for a planar rigid body. This control problem models for example a robot manipulator spray painting planar objects. We have simulated the trajectory tracking strategies just described for the second order system (5.1) with no internal drift: f (g; V ) = 0. To recall the notation, our system is 2 32 3 2 3 _3 21 0 0 ! ! _ 4 x _ x 5 = U: _ 5 = 4 0 cos ? sin 5 4 vx 5 and 4 v 0 sin cos vy v _y y _ The reference trajectory has the shape of an character in the (x; y ) variables and it speci es the reference angle to be parallel to the normal
1

32

BULLO AND MURRAY

vector of the planar curve. Thus the reference trajectory is discontinuous in (t) and in the linear velocities vx (t) and vy (t). To explicitely show the di erences due to the various error choices, we intentionally run the simulations without including the additional term Utr (no U is necessary since the system we simulate is driftless). Indeed, assuming proportional and derivative gains are high enough, a simple PD control without feedforward does the job nicely; we picked (kp ; kd) = (3; 6). Recalling the PD approach in equation (5.8) for the hybrid error case, for natural and reciprocal error functions we employed the double-geodesic control law as the Z regulator in Theorem 7. We report in Figure 7 and 8 the time evolution of the state g = ( ; x; y ) 2 S E (2) depicted as a straight line on the plane. In the rst gure we report the natural and hybrid error cases, while in the second Figure we show the reciprocal error wihtout and with left translation by g 0 = ( =2; 1; 1). The simulations start from the upper left corner of the character and run clockwise: the linear velocity of the reference path is constant and equal to 1cm/sec, the angular velocity is always equal to zero (but the angle evolution has steps at each corner). A few conclusions can be easily drawn: 1. Theorem 7 with natural error function and double-geodesic regulator is exactly equivalent to hybrid error function and double-geodesic approach described in equation (5.8). It turns out therefore that, even though the hybrid error de nition is inertial frame dependent, this dependence disappears in the closed loop (the correct simpli cations take place). 2. The reciprocal error function (coupled with the approach in Theorem 7 and a double-geodesic regulator) shows both the drawbacks discussed in Subsection 5.1: an error in the rotational part a ects the translational part and an inertial frame translation changes the closed loop trajectories (compare left to right picture in Figure 8). We eventually draw some conclusions: the theoretical analysis in the previous subsections and the numerical simulations here presented, suggest that the natural error function de ned in equation (5.2) is the correct choice. While the reciprocal error function shows some evident drawbacks, the hybrid error in equation (5.4) (coupled with the strategy described in the previous subsection) represents an equivalent choice. Nevertheless the de nition of hybrid error on S E (3) relies on the natural error function for the rotational part and needs a distinct control design (it is not possible to extend point stabilizers into trajectory trackers in a straightforward manner).

PD CONTROL ON THE EUCLIDEAN GROUP

Doublegeodesic control law with natural error 3.5 3.5 Hybrid error and doublegeodesic strategy

33

2.5

2.5

2 y (cm) y (cm) 0.5 0 0.5 1 x (cm) 1.5 2 2.5 3

1.5

1.5

0.5

0.5

0.5 1

0.5 1

0.5

0.5

1 x (cm)

1.5

2.5

systems. On the left natural error, on the right hybrid error: they are identical pictures! As the pictures con rm, it is equivalent to apply a double-geodesic strategy in Theorem 7 with the natural error function or to use the hybrid error de nition and then use a double-geodesic strategy in local coordinates (5.8).
Doublegeodesic control law with reciprocal error 4 3.5 3 2.5 2 y (cm) 1.5 1 0.5 0 0.5 1 1 y (cm) 4 3.5 3 2.5 2 1.5 1 0.5 0 0.5 1 1 Doublegeodesic control law with reciprocal error

Figure 7. Trajectory tracking for left invariant second order

1 x (cm)

1 x (cm)

Figure 8. Trajectory tracking with Theorem 7 applied with reciprocal error function. The simulation on the right shows the dependence of the scheme on an arbitrary left translation. As described in Subsection 5.1, errors in the rotational part a ect the translational part.

34

BULLO AND MURRAY

6. Linear and homogeneous approximations of systems on the

Euclidean group

So far we have focused our attention on fully actuated control systems, where the number of indipendent control inputs is equal to number of position variables (and in our case to the dimension of the group). More challenging control problems arise when dealing with underactuted mechanical systems possibly de ned on manifolds of the form G M . A classic example of a purely Lie group system is the underactuated satellite model, which can be described as g _ = g ( b1u1 + : : : + bm um ); with bi 2 g and m < dim(G). More generally many interesting locomotion systems have also internal \shape" variables. For example the model of a car with two trailers falls into this class and can be written as 3^ 2 1 5 v 0 g _ = g4 1 tan(r2) L r _2 = 1 tan(r1) ? 1 tan(r2) v L1 cos(r2) L2 r _1 = !; where g 2 S E (2) and the operator b maps R3 to se(2) in the standard way. We now concentrate on a toy example and show how to generalize some of the techniques introduced so far. 6.1. Motivating example. Following 32] and 27], we write a simpli ed kinematic model for aircrafts and underwater vehicles on S E (3) as 2 3 0 ?!3 !2 v 6 !3 0 ?!1 0 7 7 g _ = g6 4 ?!2 !1 0 0 5; 0 0 0 0 where we assume we only have 4 independent actuators: velocity along the x axis and the three angular velocities. We want to globally track a reference trajectory g _ = gVd, where Vd lies in the subspace of feasible velocities. De ne the error attitude as Re = RT d R and the error position as ze = T R (p ? pd ) (see later for an interpretation). Then, with some easy algebra, we write the error equations as _ e = Re ! be R 3 2 3 2 vd v (6.1) z_e = ze ! + 4 0 5 ? RT e4 0 5 0 0 where !e = ! ? RT e !d .
2

PD CONTROL ON THE EUCLIDEAN GROUP

35

Lemma 10 (An asymptotically stable tracking feedback). Given the system (6.1),
let ze1 be the rst component of ze and be = log(Re). Then the control law v = vd ? k1ze1 (6.2)

1 ? cos k ek z sin k e k z c be ? ! = RT e !d ? k2 e + k k k k2 be e
e e

2 4

vd
0 0

5 (6.3)

ensures global asymptotic stability as long as vd 6= 0. Proof. Consider the candidate Lyapunov function 1 kR k2 + 1 kz k2; W (Re; ze) = 2 e SO(3) 2 e and di erentiate with respect to time
2

where we exploit as usual the properties of exponential coordinates. Substituting (6.2) we have

d W = h ; ! i + hz ; 4 0 5 ? RT 4 0 5i; e e e e dt 0 0
2

vd

and plugging in Rodriguez's formula from Lemma 1

d 2 T 4 5i dt W = ?k1 ze1 + h e; !ei + hze; (I ? Re ) 0 0

2 4 2 4

vd

d W = ?k z2 + h ; ! i + hz ; b sin k ek I ? 1 ? cos k ek c 1 e1 e e e e dt k ek k ek2 e

sin k e k I ? 1 ? cos k ek c 2 + h ;! i?h ;z = ?k1 ze e e e be 1 k ek k ek2 e

vd vd

0 5i 0
3

0 5i: 0 The control law in equation (6.3) cancels out exactly the last addend and gives proving the Lyapunov stability of the closed-loop system. We can now invoke Lasalle's principle to prove asymptotic stability: the closed-loop trajectories of the system converge asymptotically to the largest invariant set contained in _ = 0g = f(Re ; ze ) 2 S E (3) : Re = I and ze1 = 0g ; f(Re; ze) : W and since in the dynamic equation of the closed-loop system reduces to
2

d W = ?k z2 ? k k k2 = ?k z 2 ? k kR k2 ; 1 e1 2 e 1 e1 2 e SO(3) dt

(6.4)

0 = !e = z be 4 0 0

vd

5;

36

BULLO AND MURRAY

then, as long as vd 6= 0, the largest invariant set contained in f(I; 0)g.

is the set

Despite the successful analysis for this toy example, the kind of technique we have pursued so far cannot be generalized to solve generic point stabilization problems and tracking problems for second order driftledd systems or underactuated systems with drift (for example). For these cases various approaches have been developed in the literature. Here we restrict our interest to the following issues: 1. Jacobian linearization for gain scheduling design. For this standard approach to trajectory tracking we here derive simpli ed Jacobian linearizations of systems de ned on S O(3) and S E (3). 2. Homogeneous approximations for point stabilization. We refer the reader to 24] and 25] for an introduction to this subject. We here want to describe a simpli ed way to obtain these approximations for systems de ned on S O(3) and S E (3). In the following our attention will focus on mechanical systems de ned on Lie groups and actuated by only body xed forces. Example of this sort are car with trailers, satellites and buoyant underwater vehicles (basically everytime gravity can be neglected). Even though the following analysis can be generalized to full second order models, we here restrict ourselves to to kinematic ( rst order) Lie group systems and derive linear and homogeneous approximations for them. Since the mechanical system is left invariant, we deal with the standard Lie group ODE: g _ = gV; V 2 g: (6.5) Recall that, as described in Section 2, the previous equation can be represented in exponential coordinates by the series expansion in equation (2.14), _= X
1 X

where X = log(g ) 2 g. Out of this series expansion we will easily derive linear and homogeneous approximations. 6.2. Jacobian linearization with respect to exponential coordinates. For a gain scheduling solution to the tracking problem, we are interested in computing the Jacobian linearization of the standard system (6.5) along a reference trajectory gd (t) 2 G, such that g _ d = gdVd . As suggested in the ?1 g to be the error state and obtain the error previous section, we set e = gd equation e _ = ?Vd e + eV: (6.7)

(?1)n Bn adn (V ); X n! n=0

(6.6)

PD CONTROL ON THE EUCLIDEAN GROUP

37

If exponential coordinates are used to parametrize the group G, the results in Lemma 2 (equations (2.14) and (2.15)) lead to _ = (V ? Vd ) + 1 adE (V + Vd ) + o(kE k2); E 2 where E = log(e) 2 g, since in equation (6.7) V is a body- xed quantity and ?V s is a spatial- xed one. Recalling the skew symmetry of the linear map adX , namely adX Y = ? adY X , we have _ = (V ? Vd ) ? 1 ad(V +Vd ) E + o(kE k2) E 2 and the linearization about E = 0 and V = Vd is easily computed as _ = (? adVd )E + (V ? Vd ): E (6.8) Note the particularly simple expression of this Jacobian linearization. This is of concrete interest in designing gain scheduling controllers with global tracking properties. In the following we apply this expression to the x03 and S E (3) cases. Remark 6 (Jacobian linearization of system on S O(3)). Consider the er3 ror attitude Re = RT d R 2 S O(3) and its exponential coordinate e 2 R such that Re = exp( be). Then equation (6.8) tells us that the linearization of system in equation (6.7) about e = 0 and ! = !d is _ e = (?! bd ) e + (! ? !d ); (6.9) 2 3 0 !d;3 ?!d;2 !d;1 5 e + I3(! ? !d) = 4 ?!d;3 0 (6.10) !d;2 ?!d;1 0 = A e + B (! ? !d ): Note the simplicity of this formula: the A matrix depends linearly on the desired angular velocity and the B is the identity. No trigonometric functions are present. Remark 7 (Jacobian linearization of systems on S E(3)). Even though equation (6.8) holds in S E (3) case as well, we here consider a di erent parametrization rather than exponential coordinates. This allows consistency with the following subsection on homogeneous approximations. ?1g 2 S E (3); in its rotational and transConsider the error matrix e = gd T lational parts it reads (Re; pe ) = (RT d R; Rd (p ? pd )). We parametrize the attitude matrix Re with its exponential coordinates e 2 R3 as for the pure S O(3) case. Regarding the translational part, we choose ze = RT e pe as our coordinates in order to have the velocity v come into play linearly (also this choice shows its usefulness later in the design of homogeneous approximations). By de nition we have T ze = RT (6.11) e pe = R (p ? pd )

38

BULLO AND MURRAY

_ T (p ? pd ) + RT (p z_e = ?R _?p _d ) T T = ?! b R (p ? pd ) + R (Rv ? Rd vd ) = ?! b ze + (v ? RT e vd ): We now substitute in e for Re using Lemma 1 sin k e k b + 1 ? cos k ek b2 v ; z_e = ?! b ze + v ? I ? k ek e k e k2 e d and we linearize about ( e; ze ) = (0; 0) and (!; v ) = (!d ; vd): z_e = ?! b ze + (v ? vd ) + be vd
bd = ?vd ?!

and di erentiating

For the full system on S E (3) we have therefore d e =? ! bd 0 e + I

ze

+ (v ? vd ):
6

Note how again, in equation (6.12), the A matrix depends linearly on the desired velocities, the B matrix is the identity and neither of the two depend on trigonometric functions of the state; the same linearization is obtained using both mixed coordinates ( e ; pe) and exponential coordinates Xe = log(e). We here adopt the choice in equation (6.11) for physical reasoning and for consistency with the following section on homogeneous approximations. As a nal note, the Jacobians we have computed have the advantadge over more standard approaches of being independent of rotation matrices. If only body xed forces act on the system, then this property is also mantained by the full state Jacobian linearization. This helps in two ways: rst global properties are improved, since no singularity is encountered and second, modern gain scheduling techniques for linear parameter-dependent plants can be more easily applied, see for example 16, 1]. 6.3. Homogeneous approximations for SO(3) and SE(3) standard systems. Time-varying homogeneous feedback has proven to be a viable tool to achieve exponential point stabilization of nonlinear driftless control systems. Central point in the theory of homogeneous feedback are vector elds invariant under certain standard transformations (called dilations). These vector elds are called homogeneous (degree zero) and their basic property is the equivalence of uniform asympototic stability and global exponential stability. 3 A classic reference is 11], while modern developments
More precisely only a slightly weaker de nition of exponential stability so called exponential stability is achieved, see 25] for the details.
3

vd ! b bd ze dt ze 6 where ( e ; pe) = (log(Re ); RT e pe ) 2 R .

! ? !d ; v ? vd

(6.12)

PD CONTROL ON THE EUCLIDEAN GROUP

39

where b = log(R) 2 so(3), (y ) , (y=2) cot(y=2) and ! = !k + !? is the orthogonal decomposition of ! along spanf g and spanf g? . Consider now the kinematic model of a satellite actuated by only two momentum wheels (see 23] with zero total angular momentum): _ = R(!1e R b1 + !2 b e2 ): Since b e1 ; b e2 ] = e b3 , the dilation associated with this control system is (1; 1; 2). With respect to exponential coordinates, it is straightforward to compute an homogeneous degree one approximation. Indeed equation (6.13) (assuming !3 = 0) as 2 2 3 2 32 3 3 0 0 ! ! 1 2 1 1 d 4 5 = 4 ! 5 + 1 4 0 0 ?! 5 4 5 + o(k k2); 2 2 dt 2 2 ?!2 !1 0 1 0 3 3
2

can be found in the work of Kawsky 17] and Hermes 13]. In particular, in this latter reference, approximating homogeneous expansions are de ned and their control theoretical application studied. Regarding the stabilzation problem for nonlinear driftless control systems, the use of time-varying (or discontinuous) feedbacks is proven necessary by the famous Brockett's theorem 6]. On the other hand smooth time-periodic stabilizers with garanteed asymptotic stability show rather slow convergence rates, so that the application of homogeneous feedbacks becomes appropriate. A comprehensive description of this succesfull approach is described in 25]. We here concer ourselves with the problem of computing homogeneous approximations to left invariant vector elds on the Lie groups S O(3) and S E (3). We compute these approximating vector elds after expressing the original system in exponential coordinates: it is possible to simply read o the homogeneous expansion directly from equation (6.6). Instead of dealing with the very general case, we show here a few simple examples and we _ = R! start by focusing on the S O(3) case. From Lemma 3, we have that R b translates into _ = ! + 1 ( ! ) ? ?1 ? (k k) !? ; (6.13) 2

=6 4
2 3 2

and the approximating vector elds are

!1 + 1 2 !2 3 2 1 !2 ? 2 !1 3 7 5 + o(k k ) 1 2 (!1 2 ? !2 1)
3 2 3

d4 dt

Once again, we are therefore dealing with the standard Brockett's nonholonomic system 6].

1 !1 1 5 = 4 0 5 !1 + 4 !2 2 5=4 1 2 1 3 2 (!1 2 ? !2 1) 2

?1 2 1

0 1

3 5 !2 :

(6.14)

40

BULLO AND MURRAY

Regarding the standard S E (3) system, by applying the same transformation of the previous subsection (that is going from, p to z = RT p), we (once again) have _ = R! R b z_ = v ? ! z As instructive example, we assume to be dealing with an underactuated system with inputs v = 0 0 v T and ! = !1 !2 0 T (being a little sloppy with notation). Then the dilation is (1; 1; 2; 2; 2; 1) with respect to the standard basis on T S E (3) = S E (3) se(3). We deal with the rotation part the same way we have done in the pure rotational case obtaining (6.14). For the translational part one obtains 0 0 0 !2 z1 1 d4z 5 4 5 4 5 4 0 0 ?!1 z2 2 = 0 + dt z z3 v ?!2 !1 0 z3 2 3 2 3 0 !2 z3 4 5 4 ?!1z3 5 = 0 + v ?!2 z1 + !1z2
2 3 2 3 2 32 3 5

and de ning v = v ? !2 z1 + !1 z2 , we have an homogeneous approximation for the full state model as
2

d6 6 dt 6 6
4

6 6

1 2 3 z1 z2 z3

3 7 7 7 7 7 7 5

6 6 6 6 6 6 4

0 6 07 7 6 7 0 7v + 6 6 6 07 7 6 05 4 1

?z3
0

6 1 2 7 7 6 2 7 !1 + 6 7 6 0 7 5 4

1 0

3 7

2 6

7 7 ?1 1 2 7 !2 7 6 z3 7

0 1

3 7

0 0

Example 7 (Kinematic car on S E(2)). The approach described above is a generalization to S E (3) of the standard transformation that puts the kinematic car model into chain form. Consider the simple kinematic model
_=! x _ = v cos y _ = v sin ; also written as

d x v dt y = R( ) 0 :

PD CONTROL ON THE EUCLIDEAN GROUP

41

1 T x Then by de ning z z2 = R y and z0 = , we obtain the transformed system as z_0 = ! z_1 = v + !z2 z_2 = ?!z1 : By rede ning v = v + !z2 we have transformed our original system into chained form. For more details we refer to 24].

7. Summary and Conclusions In this paper we have generalized proportional derivative control laws for systems in Rn to systems on matrix Lie groups. In the compact case (e.g. S O(3)) we make use of the natural metric structure of the con guration space and give completely general results. Similarities with existing control laws by Wen and Kreutz 34] are discussed. We also design generalized PD control laws for S E (3), where no natural metric structure is present: di erent possible choices depend on whether S E (3) is treated as a direct or semi{direct product of S O(3) and R3 for the purposes of controller synthesis. We then show an additional advantage to using the group structure by extending controllers designed for stabilization to controllers for trajectory tracking. The group operation naturally de nes a notion of error function with the same dynamics as the original system; as in the the linear Rn case, we track the desired trajectory by stabilizing the error to zero. Additionally we discuss various choices of error function and we evaluate them through a theoretical and numerical study: a natural approach turns out to be the most appropriate. Many of the results stated for the Euclidean group S E (3) have a much wider scope and hold for generic Lie groups. Regarding control problems related to underactuated mechanical systems (point stabilization and trajectory tracking), the methods we have illustrated allow us to gain some insight into how to design gain scheduled linear controllers for trajectory tracking and time-varying exponential stabilizers through the use of homogeneous feedback laws. In doing this, the notion of group error plays an important role in understanding global properties and the notion of exponential coordinates helps in deriving rather simple linear and homogeneous approximating vector elds. As a future direction of research, we plan to focus on mechanical systems with symmetries, where the use of geometric techniques allows the system dynamics to be split into a set of reduced dynamics and a principal connection which describes the reconstruction process (for a discussion see 3]). In this setting, the dynamics of the system have the form ? g _ = g ?A(x)x _ + I?1(x) Adg M (x)x = N (x; x _) + u

42

BULLO AND MURRAY

where x 2 M describes the base manifold (shape space), g 2 G gives the ber coordinates, and the remaining notation is as described in 3]. We retrieve the equations considered here when A(x) = ?I , = 0, and dim(M ) = n.
Acknowledgments

The authors would like to thank Prof. G. Picci and Prof R. Frezza for their constant and enthusiastic support. Particular thanks go to Prof. C. I. Byrnes, Prof. C. Martin, Prof. S. S. Sastry and Prof. T. Taylor, who showed interest and enthusiasm at the time this work still had to be completed.
Appendix A. Time derivative of exponential coordinates on S E (3)

We here give the proof of Lemma 4 in Section 2. 1 ad + BX = id + 2 X

1 X

Proof. We here want to compute an explicit expression for the quantity

on the Lie algebra se(3). The operator adX is nite rank and satis es 2 4 4 2 ad6 X = ?2k k adX ?k k adX ; where X = ( b; q ) 2 se(3) and k k is the standard norm in R3. Therefore, m for all m > 2, we can express ad2 X as a linear combination through some 2 coe cients am and bm of adX and ad4 X: m 2 4 ad2 X = am adX +bm adX : Both series of coe cients fam g and fbm g obey the same di erence equation cm = ?2k k2cm?1 ? k k4cm?2 ; (A.1) but with di erent initial conditions: a1 = 1 b1 = 0 a2 = 0 b2 = 1 The solution of the di erence equation (A.1) is of the form cm = (?1)mk k2m( 1 + 2m); with free parameters i . Substituting the initial conditions we obtain am = (?1)mk k2m?2(?2 + m) m 1 bm = (?1)mk k2m?4(?1 + m) m 1:

B2m ad2m X (2 m=1 m)!

PD CONTROL ON THE EUCLIDEAN GROUP

43

It is then possible to evalute A and B . To simplify notation, let y = k k and (y ) = (y=2) cot(y=2); we have 1 (?1)m B 1 (?1)m B X X 2 1 2 m 2m 2my 2m 2 m A = ? y2 y + 2y 2 (2 m )! (2 m )! m=1 m=1 ? d = y22 1 ? (y ) + 21 y 2 y dy (y) where in the last equality we used the Taylor expansion of cot( ) and the fact that

Plugging this result in the de nition of BX , we have 1 B X 2m (a ad2 +b ad4 ) adX + BX = id + 1 2 (2 m )! m X m X m=1 1 ad +A ad2 +B ad4 ; = id + 2 X X X where 1 1 B X 2m a = X (?1)m B2m k k2m?2(?2 + m) A= m m=1 (2m)! m=1 (2m)! 1 1 B X 2m b = X (?1)mB2m k k2m?4(?1 + m): B= m m=1 (2m)! m=1 (2m)!

(A.2)

f (x) =

1 X

A very similar decomposition holds for B ; eventually, since d (y) = d y cot( y ) = 1 cot( y ) ? y=4 ; dy dy 2 2 2 2 sin2 (y=2) we get ? (y=2)2 ( y ) ? A = y22 1 ? (y) + 21 y2 sin2 (y=2) and ? (y=2)2 : ( y ) ? B = y14 1 ? (y) + 21 y4 sin2 (y=2) Expression (A.2) is now completely known. To prove equation (2.21), we 4 write out explicitely what ad2 X and adX looks like. From equation (2.2) ad2 X
"

n=0

an xn

1 X 0 xf (x) = annxn : n=1

b2

b2

ad4 X

"

b4

b4

44

BULLO AND MURRAY

so that 4 ad +A ad2 BX = id + 1 X +B adX 2 X # " " # " b4 0 b 0 b2 0 1 I 0 3 + B = 0 I +2 + A ? b4 p b b ? b2 3 # " 1b b2 b4 0 = I3 + 2 + A + B 1 b ? I3 + 2 + A b2 + B b4

Recall now that b4 = ?k k2 b2 and substitute in the expressions for A and B. We have # " 1 2 2 b b I + + ( A ? k k B ) 0 BX = 3 2 1 b + (A ? k k2B ) b2 ? I3 + 2 " # 1 b+?1 ? (k k) b2 I + 0 3 2 ? = b b2 ? I3 + 1 2 + 1 ? (k k) Now it is enough to note that the (1; 1) and the (2; 2) blocks are equal and that the (1; 1) block is exactly the B b operator on S O(3). From Lemma 3 we have ?T BX = A( ?) A( 0)?T :
Appendix B. Proof of bound in Theorem 4

Let h ; i be the standard inner product on so(3) = R3 introduced in Section 2. We have: Lemma 11 (Bound on cross term). Let g(t) be a smooth curve on S O(3), X (t) = log(g(t)) 2 so(3) the exponential coordinates of g(t) and V = g ?1 g _ the body velocity. If tr(g (t)) 6= ?1 for all t, then the following bounds hold:

We here want to prove an analytic bound required by Theorem 4 in Section 3. Recall the de nition in the proof of Theorem 4: BX : so(3) ! so(3) 1 n Bn X adn V 7! (?1) X (V ) n ! n=0

d X; V i kV k2; (B.1) kVkk2 + kV?k2 (kX k) h dt where (y ) = (y=2) cot(y=2) and V = Vk + V? is the orthogonal decomposition of V along spanfX g and span X ? .

PD CONTROL ON THE EUCLIDEAN GROUP

45

Proof.

d X; V i = hB V; V i h dt X

1 X B2m ad2m (V ); V i = hV + 1 ad ( V ) + X 2 X (2 m=1 m)!

= kV k2 +

1 B 1 had (V ); V i + X 2m had2m (V ); V i X X 2 m=1 (2m)!

= kVkk2 + kV? k2 + Note now that, since then

B2m had2m (V ); V i: X m=1 (2m)! (B.2) (B.3)

1 X

n?1 hV; adn X (V )i = ?hadX (V ); adX (V )i; m m m 2 hV; ad2 X (V )i = (?1) k adX (V )k :

Substituting, we have

1 (?1)m B X d 2m k adm (V )k2 2 2 h dt X; V i = kVkk + kV?k + X m=1 (2m)!

(?1)m B2m kadm (V )k2 ; 1+ = kVk ? X ? 2 m=1 (2m)! kV?k where in the last equality we used adX (V ) = adX (V? ). n m Now noting that k adn X (vers V? ])k kX k , and that (?1) B2m ?jB2m j, we get the two bounds ( 1 jB j 2) X d V 2 m ? m 2 2 h dt X; V i = kVkk + kV?k 1 ? (2m)! adX kV k ? m=1 2 2 2 kVkk + kV?k = kV k and ( 1 jB j 2) X V d 2m adm ? h dt X; V i = kVkk2 + kV?k2 1 ? (2m X kV k )! ? m=1 ( ) 1 jB j X 2m kX k2m kVkk2 + kV?k2 1 ? (2 m )! m=1 = kVkk2 + kV? k2 (kX k); where the last equality holds thanks to the Taylor expansion of cot( ).

k2 + kV

k2

1 X

46

BULLO AND MURRAY

References
1] P. Apkarian, J.M. Biannic, and P. Gahinet. Self-scheduled H-in nity control of missile via linear matrix inequalities. J. of Guidance Control and Dynamics, 18 (3):532{538, May 1995. 2] H. F. Baker. Alternants and continuous groups. Proc. London Mathematical Society, 3:24{47, 1904. Second Series. 3] A. M. Bloch, P. S. Krishnaprasad, J. E. Marsden, and R. M. Murray. Nonholonomic mechanical systems with symmetry. Technical Report CIT/CDS 94{013, California Institute of Technology, 1994. Submitted to Archive for Rational Mechanics and Analysis. Available electronically via http://avalon.caltech.edu/cds. 4] W. M. Boothby. An Introduction to Di erentiable Manifolds and Riemannian Geometry. Academic Press, New York, 1975. 5] R. W. Brockett. System theory on group manifolds and coset spaces. SIAM Journal of Control, 10(2):265{284, 1972. 6] R. W. Brockett. Control theory and singular Riemannian geometry. In Peter Hilton and Gail Young, editors, New Directions in Applied Mathematics, 1981. 7] R. W. Brockett. Some mathematical aspects of robotics. In R. W. Brockett, editor, Proc. of Symposia in Applied Mathematics, volume 41, pages 16{40, 1990. 8] F. Bullo, R. M. Murray, and A. Sarti. Control on the sphere and reduced attitude stabilization. In Nonlinear Control Systems Design Symposium, June 1995. Also Technical Report CIT/CDS 95-005, available electronically via http://avalon.caltech.edu/cds. 9] J. E. Campbell. On a law of combination of operators. Proc. London Mathematical Society, 29:14{32, 1897. 10] P. Crouch and F. Silva Leite. Geometry and the dynamic interpolation problem. In American Control Conference, pages 1131{1136, Boston, June 1991. 11] W. Hahn. Stability of Motion. Springer Verlag, New York, NY, 1967. 12] F. Hausdor . Die symbolische Exponentialformel in der Gruppentheorie. Berichte der Sachsichen Akademie der Wissenschaften, 58:19{48, 1906. Leipzig. 13] H. Hermes. Nilpotent approximations of control systems and distributions. SIAM Journal of Control and Optimization, 24(4):731{736, 1986. 14] C. Herz. The derivative of the exponential map. Proceeding of the American Mathematical Society, 112(3):909{911, July 1991. 15] V. Jurdjevic and H. Sussmann. Control systems on Lie groups. J. Di erential Equations, 12(2):313{329, 1972. 16] I. Kaminer, A. Pascoal, P. Khargonekar, and C. Silvestre. A velocity algorithm for the implementation of gain-scheduled controllers with applications to rigid body motion control. In IEEE Conf. on Decision and Control, pages 1043{1048, San Antonio, Texas, December 1993. 17] M. Kawski. Homogeneous stabilizing feedback laws. Control-Theory and Advanced Technology, 6(4):497{516, 1990. 18] H. Khalil. Nonlinear Systems. Macmillan Publishing Company, 1992. 19] D. E. Koditschek. The application of total energy as a Lyapunov function for mechanical control systems. In P. S. Krishnaprasad J. E. Marsden and J. C. Simo, editors, Dynamics and Control of Multibody Systems, volume 97, pages 131{157. AMS, 1989. 20] N. E. Leonard. Averaging and Motion Control of Systems on Lie Groups. PhD thesis, University of Maryland, 1994. Institute for System Research. 21] N. E. Leonard and P. S. Krishnaprasad. Motion control of drift-free, left-invariant systems on Lie groups. IEEE Trans. Automatic Control, 1995. (to appear). 22] W. Magnus. On the exponential solution of di erential equations for a linear operator. Communications on Pure and Applied Mathematics, VII:649{673, 1954. 23] J. E. Marsden. Lectures on Mechanics. Cambridge University Press, New York, 1992.

PD CONTROL ON THE EUCLIDEAN GROUP

47

24] R. T. M'Closkey and R. M. Murray. Exponential stabilization of driftless nonlinear control systems via time-varying, homogeneous feedback. In IEEE Conf. on Decision and Control, pages 1317{1322, 1994. 25] R. T. M'Closkey and R. M. Murray. Exponential stabilization of driftless nonlinear control systems using homogeneous feedback. Technical Report CIT/CDS 95{012, California Institute of Technology, 1995. Submitted to IEEE Transactions on Automatic Control. Available electronically via http://avalon.caltech.edu/cds. 26] R. Murray, Z. Li, and S. S. Sastry. A Mathematical Introduction to Robotic Manipulation. CRC Press, Boca Raton, Florida, 1994. 27] Y. Nakamura and S. Savant. Nonlinear tracking control of autonomous underwater vehicles. In IEEE Conf. on Robotics and Automation, pages A4{A9, Nice, France, May 1992. 28] F. C. Park. Distance matrics on the rigid{body motions with applications to mechanism design. ASME J. Mechanical Design, pages 48{54, March 1995. 29] A. Sarti, G. Walsh, and S. S. Sastry. Steering left-invariant control system on matrix Lie groups. In IEEE Conf. on Decision and Control, pages 3117{3121, 1993. 30] S. Singh. Nonlinear adaptive attitude control of spacecraft. IEEE Trans. Aerospace and Electronic System, AES-23:371{380, May 1987. 31] J.-J. E. Slotine and M. D. Di Benedetto. Hamiltonian adaptive control of spacecraft. IEEE Trans. Automatic Control, AC-35:848{852, July 1990. 32] G. C. Walsh, R. Montgomery, and S. S. Sastry. Optimal path planning on matrix Lie groups. In IEEE Conf. on Decision and Control, pages 1312{1318, December 1994. 33] J. T.-Y. Wen and D. S. Bayard. A new class of control laws for robotic manipulators. Part I: Non{adaptive case. Int. Journal of Control, 47(5):1361{1385, 1988. 34] J. T.-Y. Wen and K. Kreutz-Delgado. The attitude control problem. IEEE Trans. Automatic Control, AC-36:1148{1162, October 1991.

California Institute of Technology, Mail Code 104-44, Pasadena, CA 91125

E-mail address : [email protected]