AN OVERVIEW OF LIE GROUP VARIATIONAL INTEGRATORS AND THEIR APPLICATIONS TO OPTIMAL CONTROL

Transcription

1 AN OVERVIEW OF LIE GROUP VARIATIONAL INTEGRATORS AND THEIR APPLICATIONS TO OPTIMAL CONTROL MELVIN LEOK Abstract. We introduce a general framewor for te construction of variational integrators of arbitrarily ig-order tat incorporate Lie group tecniques to automatically remain on a Lie group, wile retaining te geometric structure-preserving properties caracteristic of variational integrators, including symplecticity, momentum-preservation, and good long-time energy beavior. Tis is acieved by constructing G-invariant discrete Lagrangians in te context of Lie group metods troug te use of natural carts and interpolation at te level of te Lie algebra. In te presence of symmetry, te reduction of tese G-invariant Lagrangians yield a iger-order analogue of discrete Euler Poincaré reduction. As an illustrative example, we consider te full body problem from orbital mecanics, wic is concerned wit te dynamics of rigid bodies in space interacting under teir mutual gravitational potential. Te importance of simultaneously preserving te symplectic and Lie group properties of te full body dynamics is demonstrated in numerical simulations comparing Lie group variational integrators wit integrators tat are not symplectic or do not preserve te Lie group structure. Lastly, we demonstrate te application of Lie group variational integrators to te construction of optimal control algoritms on Lie groups, and describe a modified sceme tat improves te numerical efficiency of te computation, wile maintaining te accuracy of te computed solutions. Contents 1. Introduction 1 2. General Teory of Lie Group Variational Integrators 2 3. Lie Group Variational Integrators for te Full Body Problem Discrete Optimal Control on Lie Groups Conclusions 28 References Introduction Many problems involving long-time integration in science and engineering, suc as solar system dynamics (see, for example, Sussman and Wisdom, 1992 and molecular dynamics (see, for example, Seel et al., 1997, involve systems tat are igly nonlinear, and sensitive to small perturbations. Consequently, accurately computing particular trajectories for long-time integration is typically proibitively expensive, and it is instead desirable to construct simulations tat correctly reflect qualitative properties of te system. Geometric integrators are numerical metods tat preserve te geometric structure of a continuous dynamical system (see, for example, Hairer et al., 26; Leimuler and Reic, 24, and references terein. In te problems we consider, te underlying geometric structure affects te qualitative beavior of solutions, and as suc, numerical metods tat preserve te geometry of a problem typically yield more qualitatively accurate simulations. Tis qualitative property of geometric integrators can be better understood by adopting te viewpoint tat a numerical metod is a discrete dynamical system tat approximates te flow of te continuous system (see, for example, Benettin and Giorgilli, 1994; Tang, 1994, as opposed to te traditional 1

2 2 MELVIN LEOK view tat a numerical metod approximates individual trajectories. In particular, tis viewpoint allows questions about long-time stability to be addressed, wic would oterwise be difficult to answer. We consider geometric integrators based on discretizing Hamilton s principle in te context of discrete mecanics (see, for example, Marsden and West (21, tat yield variational integrators wic are automatically symplectic and momentum-preserving, as well as exibiting good energy beavior. By incorporating essential ideas from Lie group metods (see, for example, Iserles et al. (2, we obtain a general framewor for constructing Lie group variational integrators of arbitrarily ig-order tat retain te structure-preserving properties of variational integrators, wile automatically evolving on Lie groups witout te use of reprojection, constraints, and local coordinates. As we will demonstrate, in addition to exibiting excellent geometric conservation properties, Lie group variational integrators are exceptionally efficient numerically. Furtermore, we will sow ow Lie group variational integrators serve as te basis of an efficient, geometrically exact algoritm for solving optimal control problems on Lie groups. Outline. After recalling te construction of variational integrators, in 2 we will develop a general framewor for constructing Lie group variational integrators of arbitrarily ig order, and te discrete Euler Poincaré reduction of suc scemes. In 3, we explicitly construct a Lie group variational integrator for te full body problem from celestial mecanics, and perform an in dept numerical comparison wit non-symplectic, symplectic, and Lie group metods of te same order of accuracy. Lastly, in 4, we demonstrate ow Lie group variational integrators can be adapted to optimal control problems on Lie groups. Related Literature. Tis paper is intended to be a self-contained survey of te ongoing researc on Lie group variational integrators, and teir applications to celestial and astrodynamics simulations, and geometric and optimal control. It is based on a body of related literature tat as been performed by te autor and is collaborators. Te general teory of Lie group variational integrators was developed in Leo (24, and adapted to rigid body dynamics applications in Lee et al. (25, 26a, 27a,b. Numerical comparisons for astrodynamics applications were performed in Fanestoc et al. (26, and Lie group variational integrators were applied to actual simulations of binary near-eart asteroids in Sceeres et al. (26. Te abstract formulation of discrete optimal control problems on Lie groups is introduced in Bloc et al. (26; Hussein et al. (26, and applications to te satellite control are considered in Lee et al. (26b, 27c,e,f. Tese tecniques are also applied to te problem of attitude estimation based on te use of ellipsoidal bounds on uncertainty in Lee et al. (26c, 27d; Sanyal et al. ( General Teory of Lie Group Variational Integrators We will review some of te previous wor on discrete mecanics (see, for example, Marsden and West (21, and te construction of ig-order variational integrators, before constructing Lie group analogues of variational integrators. Furtermore, we will consider te discrete Euler Poincaré reduction (see, for example, Marsden et al. (1999 of tese Lie group variational integrators Standard Formulation of Discrete Mecanics. Te standard formulation of discrete variational mecanics (see, for example, Marsden and West (21 is to consider te discrete Hamilton s principle, δs d =, were te discrete action sum, S d : Q n+1 R, is given by n 1 S d (q, q 1,..., q n = L d (q i, q i+1. i=

3 LIE GROUP VARIATIONAL INTEGRATORS AND THEIR APPLICATIONS TO OPTIMAL CONTROL 3 Configuration Space (q, q T Q Configuration Space (q, q +1 Q Q Lagrangian L(q, q Discrete Lagrangian L d (q, q +1 Action Integral G = R t f t L(q, q dt Action Sum G d = P L d (q, q +1 Variation δg = d dɛ Gɛ = Legendre transform. p = FL(q, q Variation δg d = d dɛ Gɛ d = Legendre transform. p = FL(q, q Euler Lagrange Eqn. d L L = dt q q Hamilton s Eqn. q = H p, ṗ = H q Dis. E-L Eqn. D q L d 1 +D q L d = Dis. Hamilton s Eqn. p = D q L d, p +1 = D q+1 L d Figure 1: Procedures to derive continuous and discrete equations of motion Te discrete Lagrangian, L d : Q Q R, is a generating function of te symplectic flow, and is an approximation to te exact discrete Lagrangian, L exact d (q, q 1 = L(q 1 (t, q 1 (tdt, were q 1 ( = q, q 1 ( = q 1, and q 1 satisfies te Euler Lagrange equation in te time interval (,. Te exact discrete Lagrangian is related to te Jacobi solution of te Hamilton Jacobi equation. Te discrete variational principle ten yields te discrete Euler Lagrange (DEL equation, D 2 L d (q, q 1 + D 1 L d (q 1, q 2 =, wic yields an implicit update map (q, q 1 (q 1, q 2 tat is valid for initial conditions (q, q 1 tat as sufficiently close to te diagonal of Q Q. Te relationsip between continuous and discrete variational mecanics is summarized in Figure Lie Group Variational Integrators. Here, we will introduce iger-order Lie group variational integrators. Te basic idea beind all Lie group tecniques is to express te update map of te numerical sceme in terms of te exponential map, g 1 = g exp(ξ 1, and tereby reduce te problem to finding an appropriate Lie algebra element ξ 1 g, suc tat te update sceme as te desired order of accuracy. Tis is a desirable reduction, as te Lie algebra is a vector space, and as suc te interpolation of elements can be easily defined. In our construction, te interpolatory metod we use on te Lie group relies on interpolation at te level of te Lie algebra. For a more in dept review of Lie group metods, please refer to Iserles et al. (2. In te case of variational Lie group metods, we will express te variational problem in terms of finding Lie algebra elements, suc tat te discrete action is stationary.

4 4 MELVIN LEOK As we will consider te reduction of tese iger-order Lie group integrators, we will cose a construction tat yields a G-invariant discrete Lagrangian wenever te continuous Lagrangian is G-invariant. Tis is acieved troug te use of G-equivariant interpolatory functions, and in particular, natural carts on G Galerin Variational Integrators. We first recall te construction of iger-order Galerin variational integrators, as originally described in Marsden and West (21. Given a Lie group G, te associated state space is given by te tangent bundle T G. In addition, te dynamics on G is described by a Lagrangian, L : T G R. Given a time interval [, ], te pat space is defined to be and te action map, S : C(G R, is given by C(G = C([, ], G = {g : [, ] G g is a C 2 curve}, S(g L(g(t, ġ(tdt. We approximate te action map, by numerical quadrature, to yield S s : C([, ], G R, S s (g b i L(g(c i, ġ(c i, were c i [, 1], i = 1,..., s are te quadrature points, and b i are te quadrature weigts. Recall tat te discrete Lagrangian sould be an approximation of te form L d (g, g 1, ext S(g. g C([,],G,g(=g,g(=g 1 If we restrict te extremization procedure to te subspace spanned by te interpolatory function tat is parameterized by s + 1 internal points, ϕ : G s+1 C([, ], G, we obtain te following discrete Lagrangian, L d (g, g 1 = Te interpolatory function is G-equivariant if ext S(T ϕ(g ν ; g ν G;g =g ;g s =g 1 = ext g ν G;g =g ;g s =g 1 b i L(T ϕ(g ν ; c i. ϕ(gg ν ; t = gϕ(g ν ; t. Lemma 2.1. If te interpolatory function ϕ(g ν ; t is G-equivariant, and te Lagrangian, L : T G R, is G-invariant, ten te discrete Lagrangian, L d : G G R, given by L d (g, g 1 = ext b i L(T ϕ(g ν ; c i, g ν G;g =g ;g s =g 1 is G-invariant. Proof. L d (gg, gg 1 = ext g ν G; g =gg ; g s =gg 1 = ext g ν g 1 G;g =g ;g s =g 1 = ext g ν G;g =g ;g s =g 1 b i L(T ϕ( g ν ; c i, b i L(T ϕ(gg ν ; c i, b i L(T L g T ϕ(g ν ; c i,

5 LIE GROUP VARIATIONAL INTEGRATORS AND THEIR APPLICATIONS TO OPTIMAL CONTROL 5 = ext g ν G;g =g ;g s =g 1 = L d (g, g 1, b i L(T ϕ(g ν ; c i, were we used te G-equivariance of te interpolatory function in te tird equality, and te G-invariance of te Lagrangian in te fort equality. Remar 2.1. Wile G-equivariant interpolatory functions provide a computationally efficient metod of constructing G-invariant discrete Lagrangians, we can construct a G-invariant discrete Lagrangian (wen G is compact by averaging an arbitrary discrete Lagrangian. In particular, given a discrete Lagrangian L d : Q Q R, te averaged discrete Lagrangian, given by L d (q, q 1 = 1 L d (gq, gq 1 dg G g G is G-equivariant. Terefore, in te case of compact symmetry groups, a G-invariant discrete Lagrangian always exists Natural Carts. Following te construction in Marsden et al. (1999, we use te group exponential map at te identity, exp e : g G, to construct a G-equivariant interpolatory function, and a iger-order discrete Lagrangian. As sown in Lemma 2.1, tis construction yields a G-invariant discrete Lagrangian if te Lagrangian itself is G-invariant. In a finite-dimensional Lie group G, exp e is a local diffeomorpism, and tus tere is an open neigborood U G of e suc tat exp 1 e : U u g. Wen te group acts on te left, we obtain a cart ψ g : L g U u at g G by ψ g = exp 1 e L g 1. We would lie to construct an interpolatory function tat is described by a set of control points {g ν } s ν= in te group G at control times = d < d 1 < d 2 <... < d s 1 < d s = 1. Our natural cart based at g induces a set of control points ξ ν = ψ 1 g (g ν in te Lie algebra g at te same control times. Let l ν,s (t denote te Lagrange polynomials associated wit te control times d ν, wic yields an interpolating polynomial at te level of te Lie algebra, Applying ψ 1 g ξ d (ξ ν ; τ = ξ ν lν,s (τ. ν= yields an interpolating curve in G of te form, ( s ϕ(g ν ; τ = ψ 1 g ψ g ν= (gν l ν,s (τ, were ϕ(d ν = g ν for ν =,..., s. Furtermore, tis interpolant is G-equivariant, as sown in te following Lemma. Lemma 2.2. Te interpolatory function given by ( s ϕ(g ν ; τ = ψ 1 g ψ g ν= (gν l ν,s (τ, is G-equivariant. Proof. ( s ϕ(gg ν ; τ = ψ 1 (gg ψ gg ν= (ggν l ν,s (τ ( s = L gg exp e ν= exp 1 e ((gg 1 (gg ν l ν,s (τ ( s = L g L g exp e ν= exp 1 e ((g 1 g 1 gg ν l ν,s (τ

6 6 MELVIN LEOK ( s = L g ψ 1 g ν= exp 1 e = L g ψ 1 g ( s ν= ψ g (gν l ν,s (τ L (g 1(gν l ν,s (τ = L g ϕ(g ν ; τ. Remar 2.2. In te proof tat ϕ is G-equivariant, it was important tat te base point for te cart sould transform in te same way as te internal points g ν. As suc, te interpolatory function will be G-equivariant for a cart tat it based at any one of te internal points g ν tat parameterize te function, but will not be G-equivariant if te cart is based at a fixed g G. Witout loss of generality, we will consider te case wen te cart is based at te first point g. We will now consider a discrete Lagrangian based on te use of interpolation in a natural cart, wic is given by L d (g, g 1 = ext g ν G;g =g ;g s =g 1 b i L(T ϕ({g ν } s ν=; c i. g1 To furter simplify te expression, we will express te extremal in terms of te Lie algebra elements ξ ν associated wit te ν-t control point. Tis relation is given by and te interpolated curve in te algebra is given by wic is related to te curve in te group, Te velocity ξ = g 1 ġ is given by ξ ν = ψ g (g ν, ξ(ξ ν ; τ = ξ κ lκ,s (τ, κ= g(g ν ; τ = g exp(ξ(ψ g (g ν ; τ. ξ(τ = g 1 ġ(τ = 1 ξ κ lκ,s (τ. κ= Using te standard formula for te derivative of te exponential, were T ξ exp = T e L exp(ξ dexp adξ, dexp w = n= w n (n + 1!, we obtain te following expression for discrete Lagrangian, ( L d (g, g 1 = ext b i L L g exp(ξ(c i, ξ ν g;ξ =;ξ s =ψ g (g 1 T exp(ξ(cil g T e L exp(ξ(ci dexp ( ξ(c adξ(ci i. More explicitly, we can compute te conditions on te Lie algebra elements for te expression above to be extremal. Tis implies tat ( L d (g, g 1 = b i L L g exp(ξ(c i, T exp(ξ(cil g T e L exp(ξ(ci dexp ( ξ(c adξ(ci i

7 LIE GROUP VARIATIONAL INTEGRATORS AND THEIR APPLICATIONS TO OPTIMAL CONTROL 7 wit ξ =, ξ s = ψ g (g 1, and te oter Lie algebra elements implicitly defined by [ L = b i g (c it exp(ξ(cil g T e L exp(ξ(ci dexp adξ(ci lν,s (c i for ν = 1,..., s 1, and were + 1 L ġ (c itexp(ξ(c 2 L i exp(ξ(c i Te 2 L exp(ξ(ci ddexp adξ(ci l ν,s (c i ddexp w = n= w n (n + 2!. Tis expression for te iger-order discrete Lagrangian, togeter wit te discrete Euler Lagrange equation, D 2 L d (g, g 1 + D 1 L d (g 1, g 2 =, yields a iger-order Lie group variational integrator Higer-Order Discrete Euler Poincaré Equations. In tis section, we will apply discrete Euler Poincaré reduction (see, for example, Marsden et al. (1999 to te Lie group variational integrator we derived previously, to construct a iger-order generalization of discrete Euler Poincaré reduction Reduced Discrete Lagrangian. We first proceed by computing an expression for te reduced discrete Lagrangian in te case wen te Lagrangian is G-invariant. Recall tat our discrete Lagrangian uses G- equivariant interpolation, wic, wen combined wit te G-invariance of te Lagrangian, implies tat te discrete Lagrangian is G-invariant as well. We compute te reduced discrete Lagrangian, l d (g 1 g 1 L d (g, g 1 = L d (e, g 1 g 1 = ext ξ ν g;ξ =;ξ s =log(g 1 = ext ξ ν g;ξ =;ξ s =log(g 1 g1 g1 ( b i L L e exp(ξ(c i, ], T exp(ξ(cil e T e L exp(ξ(ci dexp ( ξ(c adξ(ci i ( b i L exp(ξ(c i, T e L exp(ξ(ci dexp ( ξ(c adξ(ci i. Setting ξ =, and ξ s = log(g 1 g 1, we can solve te stationarity conditions for te oter Lie algebra elements {ξ ν } s 1 ν=1 using te following implicit system of equations, [ L = b i g (c it e L exp(ξ(ci dexp adξ(ci lν,s (c i + 1 ] L ġ (c ite 2 L exp(ξ(ci ddexp adξ(ci l ν,s (c i were ν = 1,..., s 1. Tis expression for te reduced discrete Lagrangian is not fully satisfactory owever, since it involves te Lagrangian, as opposed to te reduced Lagrangian. If we revisit te expression for te reduced discrete Lagrangian, l d (g 1 g 1 = ext ξ ν g;ξ =;ξ s =log(g 1 g1 ( b i L exp(ξ(c i, T e L exp(ξ(ci dexp ( ξ(c adξ(ci i,

8 8 MELVIN LEOK we find tat by G-invariance of te Lagrangian, eac of te terms in te summation, ( L exp(ξ(c i, T e L exp(ξ(ci dexp ( ξ(c adξ(ci i, can be replaced by were l : g R is te reduced Lagrangian given by ( l dexp ( ξ(c adξ(ci i, l(η = L(L g 1g, T L g 1ġ = L(e, η, were η = T L g 1ġ g. From tis observation, we ave an expression for te reduced discrete Lagrangian in terms of te reduced Lagrangian, l d (g 1 g 1 = ext ξ ν g;ξ =;ξ s =log(g 1 g1 ( b i l dexp ( ξ(c adξ(ci i. As before, we set ξ =, and ξ s = log(g 1 g 1, and solve te stationarity conditions for te oter Lie algebra elements {ξ ν } s 1 ν=1 using te following implicit system of equations, [ ] l = b i η (c i ddexp adξ(ci l ν,s (c i, were ν = 1,..., s Discrete Euler Poincaré Equations. As sown above, we ave constructed a iger-order reduced discrete Lagrangian tat depends on f +1 g g We will now recall te derivation of te discrete Euler Poincaré equations, introduced in Marsden et al. (1999. Te variations in f +1 induced by variations in g, g +1 are computed as follows, δf +1 = g 1 δg g 1g +1 + g 1 δg +1 = T R f+1 ( g 1 δg + Ad f+1 g +1 δg +1. Ten, te variation in te discrete action sum is given by δs = = = = = =1 l d(f +1 δf +1 l d(f +1 T R f+1 ( g 1 δg + Ad f+1 g +1 δg +1 [ l d (f 1 T R f 1 Ad f 1 l d(f +1 T R f+1 wit variations of te form ϑ = g 1 δg. In computing te variation of te discrete action sum, we ave collected terms involving te same variations, and used te fact tat ϑ = ϑ N =. Tis yields te discrete Euler Poincaré equation, ] ϑ, l d(f 1 T R f 1 Ad f 1 l d(f +1 T R f+1 =, = 1,..., N 1. For ease of reference, we will recall te expressions from te previous discussion tat define te iger-order reduced discrete Lagrangian, ( l d (f +1 = b i l dexp ( ξ(c adξ(ci i,

9 LIE GROUP VARIATIONAL INTEGRATORS AND THEIR APPLICATIONS TO OPTIMAL CONTROL 9 were and ξ(ξ ν ; τ = ξ =, ξ κ lκ,s (τ, κ= ξ s = log(f +1, and te remaining Lie algebra elements {ξ ν } s 1 ν=1, are defined implicitly by [ ] l = b i η (c i ddexp adξ(ci l ν,s (c i, for ν = 1,..., s 1, and were ddexp w = n= w n (n + 2!. Wen te discrete Euler Poincaré equation is used in conjunction wit te iger-order reduced discrete Lagrangian, we obtain te iger-order Euler Poincaré equations Example: Lie Group Velocity Verlet. We will now construct a Lie group analogue of te velocity Verlet metod for te free rigid body.te velocity Verlet metod can be derived from te context of discrete mecanics by considering te following discrete Lagrangian, L d (q, q +1 = 2 [ L ( q, q +1 q + L ( q +1, q ] +1 q, wic corresponds to using a piecewise linear interpolant, and te trapezoidal rule to approximate te integral. In te case of te free rigid body, te Lagrangian is given by, L(R, Ṙ = 1 2 ΩJΩT = 1 2 tr[ S(ΩJ d S(Ω T ]. Here, J d is a modified moment of inertia tat is related to te usual moment of inertia by te relations, J d = 1 2 (tr[j] I 3 3 2J, and J = tr[j d ] I 3 3 J d. From te inematic relation S(Ω = R T Ṙ, we ave tat, S(Ω = R T R +1 R Ṙ R = 1 (F I 3 3, were F = R T R +1. Ten, te discrete Lagrangian for te velocity Verlet metod applied to te free rigid body is given by, L d (R, R +1 = tr[ (F I 3 3 T J d (F I 3 3 ] = 1 2 tr[ (F I 3 3 (F I 3 3 T ] J d = 1 tr[(i 3 3 F J d ], were in te second to last equality, we used te fact tat tr[ab] = tr[ba], and in te last equality, we used te fact tat F is an ortogonal matrix, J d is symmetric, and tr[ab] = tr [ B T A T ]. Recall tat RT R δr = RT (δrr T. Furtermore, te variation of R is given by, δr = R η,

10 1 MELVIN LEOK were η so(3 is a variation represented by a sew-symmetric matrix and vanises at = and = N. We may now compute te constrained variation of F = R T R, wic yields, δf = δr T R +1 + R T δr +1 = η R T R +1 + R T R +1 η +1 = η F + F η +1. Define te discrete action sum to be Taing constrained variations of F yields, δs d = = S d = = L d (R, F. 1 {tr[ η +1J d F ] + tr[η F J d ]}. Using te fact tat te variations η vanis at te endpoints, we may reindex te sum to obtain, δs d = =1 1 tr[η (F J d J d F 1 ]. Te discrete Hamilton s principle states tat te variation of te discrete action sum sould be zero for all variations tat vanis at te endpoints. Since η is an arbitrary sew-symmetric matrix, for te discrete action sum to be zero, it is necessary for (F J d J d F 1 to be symmetric, wic is to say tat, F +1 J d J d F T +1 J d F + F T J d =. Tis implicit equation for F +1 in terms of F, togeter wit te reconstruction equation R +1 = R F, yields te Lie group analogue of te velocity Verlet metod. In practice, in time marcing te numerical solution, we need to solve te above equation for F +1 SO(3 given F. Tis equation is linear in F +1, but it is implicit due to te nonlinear constraint F T +1 F +1 = I 3 3. Since J d F F T J d is a sew-symmetric matrix, it may be represented as S(g, were g R 3, wic reduces te equation to te form, (2.1 F J d J d F T = S(g. We now introduce two iterative approaces to solve (2.1 numerically. Exponential map. An element of a Lie group can be expressed as te exponential of an element of its Lie algebra, so F SO(3 can be expressed as an exponential of S(f so(3 for some vector f R 3. Te exponential can be written in closed form, using Rodrigues formula, (2.2 F = exp S(f = I Substituting (2.2 into (2.1, we obtain S(g = sin f f sin f f 1 cos f S(f + f 2 S(f 2. 1 cos f S(Jf + f 2 S(f Jf. Tus, (2.1 is converted into te equivalent vector equation g = G(f, were G : R 3 R 3 is given by (2.3 G(f = sin f f 1 cos f Jf + f 2 f Jf. We use te Newton metod to solve g = G(f, wic gives te iteration (2.4 f i+1 = f i + G(f i 1 (g G(f i.

11 LIE GROUP VARIATIONAL INTEGRATORS AND THEIR APPLICATIONS TO OPTIMAL CONTROL 11 We iterate until g G(f i < ɛ for a small tolerance ɛ >. Te Jacobian G(f in (2.4 can be expressed as cos f f sin f G(f = f 3 Jff T sin f + f J sin f f 2(1 cos f + f 4 (f Jf f T + 1 cos f f 2 { S(Jf + S(fJ}. Cayley transformation. Similarly, given f c R 3, te Cayley transformation is a local diffeomorpism tat maps S(f c so(3 to F SO(3, were (2.5 F = cay S(f c = (I S(f c (I 3 3 S(f c 1. Substituting (2.5 into (2.1, we obtain a vector equation G c (f c = equivalent to (2.1 (2.6 and its Jacobian G c (f c is written as G c (f c = g + g f c + (g T f c f c 2Jf c =, G c (f c = S(g + (g T f c I f c g T 2J. Ten, (2.6 is solved by using Newton s iteration (2.4, and te rotation matrix is obtained by te Cayley transformation. For bot metods, numerical experiments sow tat 2 or 3 iterations are sufficient to acieve a tolerance of ɛ = Numerical iteration wit te Cayley transformation is a faster by a factor of 4-5 due to te simpler expressions in te iteration. It sould be noted tat since F = exp S(f or F = cay S(f c, it is automatically a rotation matrix, even wen te equation g = G(f is not satisfied to macine precision. Tese computational approaces are distinguised from solving te implicit equation (2.1 wit 9 variables and 6 constraints. In te next section, we will consider a more involved numerical example for a system of rigid bodies interacting under teir mutual gravitational potential. 3. Lie Group Variational Integrators for te Full Body Problem Te full body problem in orbital mecanics treats te dynamics of non-sperical rigid bodies in space interacting under teir mutual potential. Since te mutual gravitational potential of distributed rigid bodies depends on bot te position and te attitude of te bodies, te translational and te rotational dynamics are coupled in te full body problem. For example, te orbital motion and te attitude dynamics of a very large spacecraft in te Eart s gravity field are coupled, and te dynamics of a binary asteroid pair, wit non-sperical mass distributions of te bodies, involves coupled orbital and attitude dynamics. Recently, interest in te full body problem as increased, as it is estimated tat up to 16% of near-eart asteroids are binaries (Margot et al., 22. After introducing te continuous formulation of te full body model, we will construct a Lie group velocity Verlet metod for tis system, as introduced in 2.4. We ten discuss in detail some of its numerical conservation properties, and compare its performance wit oter second-order metods Full body models. Maciejewsi (1995 presented te continuous equations of motion for te full body problem in Hamiltonian form witout providing a formal derivation. Here, we formulate te problem in terms of Hamilton s variational principle using te Lagrangian formalism. We ten discretize te Lagrangian, and compute te proper form for te variations of Lie group elements in te configuration space, wic leads to a systematic derivation of te discrete equations of motion.

12 12 MELVIN LEOK 3.2. Inertial coordinates. Te configuration space of a rigid body is SE(3 = R 3 s SO(3, were SO(3 denotes te group of 3 3 ortogonal matrices wit unit determinant, and s represents a semi-direct product. We derive continuous equations of motion for n rigid bodies. We define an inertial frame and a body-fixed frame for eac body, and assume tat te origin of te i-t body-fixed frame is located at te center of mass of te i-t body. For te i-t body, te position of te center of mass in te inertial frame, and te attitude, wic is a rotation matrix from te body-fixed frame to te inertial frame, are represented by (x i, R i SE(3. Te translational velocity in te inertial frame and te angular velocity in te body-fixed frame are represented by v i, Ω i R 3. Te subscript i denotes te i-t rigid body. Te inematic equations are given by (3.1 (3.2 ẋ i = v i Ṙ i = R i S(Ω i, were S( : R 3 so(3 is te isomorpism between te Lie algebra so(3, wic represents 3 3 sewsymmetric matrices, and R 3 defined by te condition tat S(xy = x y for any x, y R 3. Te mass and te moment of inertia matrix of te i-t body is denoted by m i R and J i R 3 3, respectively. We construct a nonstandard moment of inertia matrix J di R 3 3 by (3.3 J di = ρ i ρ T i dm i, B i were ρ i R 3 is te position of a mass element of te i-t body in its body-fixed frame. It can be sown tat te standard moment of inertia matrix J i = B i S(ρ i T S(ρ i dm i R 3 3 is related to te nonstandard moment of inertia matrix by te following properties. (3.4 (3.5 J i = tr[j di ] I 3 3 J di, S(J i Ω i = S(Ω i J di + J di S(Ω i, for any Ω i R 3. Conversely, one can obtain te nonstandard moment of inertia from te standard momentum of inertia from te following relation, (3.6 J di = 1 2 tr[j i] I 3 3 J i. Te linear momentum in te inertial frame and te angular momentum in te body-fixed frame are denoted by γ i = m i v i and Π i = J i Ω i R 3, respectively, for te i-t body. Lagrangian. Given (x i, R i SE(3, te inertial position of a mass element of te i-t body is given by x i + R i ρ i, were ρ i R 3 denotes te position of te mass element in te body-fixed frame. Ten, te inetic energy of te i-t body B i can be written as T i = 1 ẋ i + 2 Ṙiρ i 2 dm i. B i Using te fact tat B i ρ i dm i = and (3.2, te inetic energy T i can be rewritten in terms of te nonstandard moment of inertia matrix as T i (ẋ i, Ω i = 1 ẋ i 2 + S(Ω i ρ i 2 dm i, 2 B i (3.7 = 1 2 m i ẋ i tr[ S(Ω i J di S(Ω i T ],

13 LIE GROUP VARIATIONAL INTEGRATORS AND THEIR APPLICATIONS TO OPTIMAL CONTROL 13 Te gravitational potential energy U : SE(3 n R is given by U(x 1,..., x n, R 1,..., R n = 1 n (3.8 2 i,j=1 i j were G is te universal gravitational constant. Ten, te Lagrangian for n rigid bodies, L : TSE(3 n R, is given by (3.9 L(x 1, ẋ 1, R 1, Ω 1,..., x n, ẋ n, R n, Ω n = Te action integral is defined to be n B i B j Gdm i dm j x i + R i ρ i x j R j ρ j, 1 2 m i ẋ i tr[ S(Ω i J di S(Ω i T ] U(x 1,..., x n, R 1... R n. (3.1 G = tf t L(x 1, ẋ 1, R 1, Ω 1,..., x n, ẋ n, R n, Ω n dt. Discrete Lagrangian. In continuous time, te structure of te inematic equation Ṙi = R i S(Ω i ensures tat R i evolves on SO(3 automatically. Here, we introduce a new variable F i SO(3 defined suc tat = R i F i, i.e. R i+1 (3.11 F i = R T i R i+1. Tus, F i represents te relative attitude between two integration steps, and by requiring tat F i SO(3, we guarantee tat R i evolves on SO(3 automatically. Tis is a consequence of te fact tat te Lie group is closed under te group operation of matrix multiplication. Using te inematic equation Ṙi = R i S(Ω i, te sew-symmetric matrix S(Ω can be approximated as S(Ω i = Ri T Ṙ i Ri T R i+1 R i (3.12 = 1 (F i I 3 3. Te velocity ẋ i can be approximated simply by (x i+1 x i /. Using tese approximations of te angular and linear velocity, te inetic energy of te it body given in (3.7 can be approximated as ( 1 T i (ẋ i, Ω i T i (x i +1 x i, 1 (F i I 3 3, = m i x i+1 x i tr[ (F i I 3 3 J di (F i I 3 3 T ], = m i xi+1 x i tr[(i 3 3 F i J di ]. A discrete Lagrangian L d : SE(3 n SE(3 n R is constructed suc tat it approximates a segment of te action integral (3.1, n 1 L d = 2 m i xi+1 x i 2 1 ( tr[(i 3 3 F i J di ] 2 U(x 1,..., R n 2 U(x 1 +1,..., R n+1. Tis discrete Lagrangian is self-adjoint (Hairer et al., 26, and self-adjoint numerical integration metods ave even order, so we are guaranteed tat te resulting integration metod is at least second-order accurate. Variations of discrete variables. Te variations of te discrete variables are cosen to respect te geometry of te configuration space SE(3. Te variation of x i is given by x ɛ i = x i + ɛδx i + O(ɛ 2,

14 14 MELVIN LEOK were δx i R 3 and vanises at = and = N. Te variation of R i is given by (3.14 δr i = R i η i, were η i so(3 is a variation represented by a sew-symmetric matrix and vanises at = and = N. Te variation of F i can be computed from te definition F i = R T i R i+1 to give (3.15 δf i = δr T i R i+1 + R T i δr i+1, = η i R T i R i+1 + R T i R i+1 η i+1, = η i F i + F i η i+1. Discrete Hamilton s principle. To obtain te discrete equations of motion in Lagrangian form, we compute te variation of te discrete Lagrangian from (3.14 and (3.15 to give n 1 δl d = m i(x i+1 x i T (δx i+1 δx i + 1 tr[( ] η i F i F i η i+1 Jdi ( T U δx i + U T +1 δx i+1 + [ ] 2 x i x i+1 2 tr η i Ri T U + η i+1 R T U ( i R +1, i R i+1 were U = U(x 1,..., R n denotes te value of te potential at t = + t. Define te action sum as (3.17 G d = = L d (x 1, x 1+1, R 1, F 1,..., x n, x n+1, R n, F n. Te discrete action sum G d approximates te action integral (3.1, because te discrete Lagrangian approximates a segment of te action integral. Substituting (3.16 into (3.17, te variation of te action sum is given by δg d = = n { 1 δx T i +1 m i(x i+1 x i U +1 2 x i+1 { + tr [η i+1 1 J d i F i + U 2 RT +1 i +1 R i+1 } { + δx Ti 1 m i(x i+1 x i U 2 }] [ + tr η i { 1 F i J di + 2 RT i Using te fact tat δx i and η i vanis at = and = N, we can reindex te summation, wic is te discrete analogue of integration by parts, to yield n { } 1 δg d = δx i m ( i 2x U xi+1 i + x i 1 + x =1 i [ { }] 1 ( + tr η i Fi J di J di F i 1 + R T U i. R i Hamilton s principle states tat δg d sould be zero for all possible variations δx i R 3 and η i so(3 tat vanis at te endpoints. Terefore, te expression in te first brace sould be zero, and since η i is sew-symmetric, te expression in te second brace sould be symmetric. Discrete equations of motion. We obtain te discrete equations of motion for te full body problem, in Lagrangian form, for bodies i (1, 2,, n as ( ( xi+1 2x U i + x i 1 =, x i x i U R i } }].

15 (3.19 (3.2 LIE GROUP VARIATIONAL INTEGRATORS AND THEIR APPLICATIONS TO OPTIMAL CONTROL 15 were M i R 3 is given by ( (F i+1 J di J di F T i+1 J di F i + F T i J di = S(M i+1, R i+1 = R i F i, M i = r i1 u i1 + r i2 u i2 + r i3 u i3, were r ip, u ip R 1 3 are pt row vectors of R i and U R i, respectively. Given initial conditions (x i, R i, x i1, R i1, we can obtain x i2 from (3.18. Ten, F i is computed from (3.2, and F i1 can be obtained by solving te implicit equation (3.19. Finally, R i2 is found from (3.2. Tis yields an update map (x i, R i, x i1, R i1 (x i1, R i1, x i2, R i2, and tis process can be repeated. As discussed above, equations (3.18 troug (3.2 defines a discrete Lagrangian map tat updates x i and R i. Te discrete Legendre transformation relates te configuration variables x i, R i and te corresponding momenta γ i, Π i. Tis induces a discrete Hamiltonian map tat is equivalent to te discrete Lagrangian map. Te discrete equations of motion for te full body problem, in Hamiltonian form, can be written for bodies i (1, 2,, n as (3.22 (3.23 (3.24 (3.25 (3.26 x i+1 = x i + m i γ i 2 2m i U x i, γ i+1 = γ i 2 U U +1, x i 2 x i+1 S(Π i + 2 M i = F i J di J di F T i, Π i+1 = F T i Π i + 2 F T i M i + 2 M i +1., R i+1 = R i F i. Given (x i, γ i, R i, Π i, we can find x i1 from (3.22. Solving te implicit equation (3.24 yields F i, and R i1 is computed from (3.26. Ten, (3.23 and (3.25 gives γ i1, and Π i1. Tis defines te discrete Hamiltonian map, (x i, γ i, R i, Π i (x i1, γ i1, R i1, Π i1, and tis process can be repeated Properties of te Lie group variational integrator. Since te LGVI is obtained by discretizing Hamilton s principle, it is symplectic and preserves te structure of te configuration space, SE(3, as well as te relevant geometric features of te full two rigid body problem, and te conserved first integrals of total linear and angular momenta. Te total energy exibits small bounded oscillations about its initial value, but tere is no tendency for te mean of te oscillation in te total energy to drift (increase or decrease from te initial value for exponentially long times. Te LGVI preserves te group structure. By using te computational approac described in 2.4, te matrices F i representing te cange in relative attitude are guaranteed to be rotation matrices. Te group operation of te Lie group SO(3 is matrix multiplication. Since te rotation matrices R i are updated using te group operation, tey automatically evolve on SO(3 witout constraints or reprojection. Terefore, te ortogonal structure of te rotation matrices is preserved, and te attitude of eac rigid body is determined accurately and globally witout te need to use local carts (parameterizations suc as Euler angles or quaternions. Tese exact geometric properties of te discrete flow not only generate improved qualitative beavior, but also allow for accurate long-time simulation. Tis geometrically exact numerical integration metod yields a igly efficient and accurate computational algoritm for te full rigid body problem. For arbitrary saped rigid bodies suc as binary asteroids, tere is a large burden in computing te mutual gravitational forces and moments, so te number of force and moment evaluations sould be minimized. We ave seen tat te LGVI requires only one suc evaluation

16 16 MELVIN LEOK per integration step, te minimum number of evaluations consistent wit te presented LGVI aving secondorder accuracy (because it is a self-adjoint metod. Witin te LGVI, implicit equations must be solved at eac time step to determine te matrix-multiplication updates for rotation matrices. However te LGVI is only wealy implicit in te sense tat te iteration for eac implicit equation is independent of te muc more costly gravitational force and moment computation. Te computational load to solve eac implicit equation is negligible; only two or tree iterations are typically required. Tis is addressed in 2.4 by expressing F i as te exponential function of an element of te Lie algebra so(3. Altogeter, te entire metod could be considered almost explicit. Te LGVI is a fixed step size integrator, but all of te properties above are independent of te step size. Consequently, we can acieve te same level of accuracy wile coosing a larger step size as compared to oter numerical integrators of te same order. All of tese features are revealed by numerical simulations in 3.4 and in te wor by Fanestoc et al. (26. In 3.4, te LGVI is compared wit oter second-order geometric integrators: a symplectic Runge Kutta metod and a Lie group metod. In Fanestoc et al. (26, te LGVI is directly compared wit te 7(8t order Runge Kutta Felberg metod (RK78 for two octaedral rigid bodies. It is sown tat te LGVI requires 8 times less computational load tan RK78 for similar error measures, and te accuracy of te LGVI is maintained for exponentially long time. Te trajectories computed using RK78 are unreliable for te long time simulation of te full two rigid body dynamics Numerical simulations. We simulate te dynamics of two simple dumbbell bodies acting under teir mutual gravity. Eac dumbbell model consists of two equal rigid speres and a rigid massless connecting rod. Tis dumbbell rigid body model as a simple closed form for te mutual gravitational potential given by U(X, R = 2 p,q=1 Gm 1 m 2 /4 X + ρ 2p + Rρ 1q, were G is te universal gravitational constant, m i R is te total mass of te it dumbbell, and ρ ip R 3 is a vector from te origin of te body-fixed frame to te pt spere of te it dumbbell in te it body-fixed frame. Te vectors ρ i1 = [l i /2,, ] T, ρ i2 = ρ i1, were l i is te lengt between te two speres. Mass, lengt and time dimensions are normalized. Te mass and lengt of te second dumbbell are twice tat of te first dumbbell. Te oter simulation parameters are cosen suc tat te total linear momentum in te inertial frame is zero and te relative motion between two bodies are near-elliptic orbits. Te trajectories of te dumbbell bodies are sown in Figure 2. We compare te computational properties of te Lie group variational integrator (LGVI wit oter second-order numerical integration metods; an explicit Runge Kutta metod (RK, a symplectic Runge Kutta metod (SRK, and a Lie group metod (LGM. One of te distinct features of te LGVI is tat it preserves bot te symplectic property and te Lie group structure of te full rigid body dynamics. A comparison can be made between te LGVI and oter integration metods tat preserve eiter none or one of tese properties: an integrator tat does not preserve any of tese properties (RK, a symplectic integrator tat does not preserve te Lie group structure (SRK, and a Lie group integrator tat does not preserve symplecticity (LGM. Tese metods are implemented by an explicit mid-point rule, an implicit mid-point rule, and te Crouc-Grossman metod presented in Hairer et al. (26 for te continuous equations of motion, respectively. For te LGVI, te discrete equations of motion given by (3.18 troug (3.21 are used. All of tese integrators are second-order accurate. A comparison wit a iger-order integrator can be found in Fanestoc et al. (26. Figure 3(a sows te computed total energy response over 3 seconds wit an integration step size =.2 sec. For te LGVI, te total energy is nearly constant, and tere is no tendency to drift, wile

17 LIE GROUP VARIATIONAL INTEGRATORS AND THEIR APPLICATIONS TO OPTIMAL CONTROL 17 Figure 2: Trajectories of two dumbbell bodies in te inertial frame (Te initial orbit is sown wit solid lines and snapsots of dumbbell body maneuver. te oter integrators fail to preserve te total energy. Tis can be observed in Figure 3(b, were te mean total energy deviations are sown for varying integration step sizes. It is seen tat te total energy errors of te SRK metod is close to te RK metod, but te total energy error of te LGVI is smaller by several orders of magnitude. Figure 3(c sows te mean ortogonality errors. Te LGVI and te LGM conserve te ortogonal structure at an error level of 1 1, wile te RK and te SRK do not. Tese computational comparisons suggest tat for numerical integration of Hamiltonian systems evolving on a Lie group, suc as full body problems, it is critical to preserve bot te symplectic property and te Lie group structure. For te RK and te SRK, te ortogonality error in te rotation matrix corrupts te attitude of te rigid bodies. Te accumulation of tis attitude degradation causes significant errors in te computation of te gravitational forces and moments, wic are dependent upon te position and te attitude, and affects te accuracy of te entire numerical simulation. Te LGM conserves te ortogonal structure of rotation matrices numerically, but it does not respect te caracteristics of te Hamiltonian dynamics properly as a non-symplectic integrator; tis causes a drift of te computed total energy. Te LGVI is a geometrically exact integration metod in te sense tat it preserves all of te features of te full rigid body dynamics concurrently. Consequently, te LGVI yields numerical trajectories wic are more qualitatively accurate, and te qualitative advantages of te LGVI become more pronounced as te lengt of te simulation is increased. Computational efficiency is compared in Figure 3(d, were CPU times of all metods are sown for varying step sizes. Te SRK requires te largest CPU time, since it involves te solution of an implicit equation in 36 variables at eac integration step. Te RK and te LGM require similar CPU times since bot are explicit. It is interesting to see tat te implicit LGVI actually requires less CPU time tan te explicit metods RK and LGM. Tis follows from te fact tat te second-order explicit metods RK and LGM require two evaluations of te expensive force and moment computations at eac step. Te LGVI requires only one evaluation at eac step in addition to te solution of an implicit equation (3.24. Te implicit equation can be solved efficiently using te computational approac described in 2.4 and ence it taes less time tan te evaluation of te forces and moments. Te difference is furter increased as te rigid body model becomes more complicated since it involves a larger computation burden in computing te gravitational forces and moments. Based on tese properties, we claim tat te LGVI is almost explicit. Tis comparison demonstrates te superior computational efficiency of te LGVI.

18 18 MELVIN LEOK E.159 RK SRK LGM LGVI mean Δ E RK SRK LGM LGVI time (a Computed total energy for 3 seconds Step size (b Mean total energy error E E vs. step size mean I R T R CPU time (sec Step size (c Mean ortogonality error I R T R vs. step size Step size (d CPU time vs. step size Figure 3: Computational properties of explicit Runge Kutta (RK, symplectic Runge Kutta (SRK, Lie group metod (LGM, and Lie group variational integrator (LGVI.

19 LIE GROUP VARIATIONAL INTEGRATORS AND THEIR APPLICATIONS TO OPTIMAL CONTROL 19 In summary, from Figures 3(b and 3(d, we see tat te LGVI requires 16 times less CPU time tan te LGM, 35 times less CPU time tan te RK, and 98 times less CPU time tan te SRK, to acieve a similar total energy error in tis computational example for te full body problem. 4. Discrete Optimal Control on Lie Groups In tis section, we apply te Lie group variational integrators wic ave been introduced in te previous section to te problem of optimal control on Lie groups. We will first derive first-order optimality conditions for discrete optimal solution, and ten describe an efficient sooting based metod for solving te discrete optimal control problem. Our approac to discretizing te optimal control problem is in contrast to traditional tecniques suc as collocation, werein te continuous equations of motion are imposed as constraints at a set of collocation points. In our approac, modeled after Junge et al. (25, te discrete equations of motion are derived from a discrete variational principle, and tis induces constraints on te configuration at eac discrete time step. Tis approac yields discrete dynamics tat are more faitful to te continuous equations of motion, and consequently yields more accurate numerical solutions to te optimal control problem. Tis feature is extremely important in computing accurate (suboptimal trajectories for long-term spacecraft attitude maneuvers. For te purpose of numerical simulation, te corresponding discrete optimal control problem is posed on te discrete state space as a two-stage discrete variational problem. In te first step, we derive te discrete dynamics for te rigid body in te sense of Lie group variational integrators. Tese discrete equations are ten imposed as constraints to be satisfied by te extremal solutions to te discrete optimal control problem, and we obtain te discrete extremal solutions in terms of te given terminal states. We formulate an optimal control problem for a rigid body on SE(3 assuming tat control forces and moments are applied during te maneuver. Necessary conditions for optimality are developed and computational approaces are presented to solve te corresponding two-point boundary value problem Lie Euler variational integrator. In order to simplify te form of te necessary conditions for te optimal control problem, we consider a first-order variant of te Lie group variational integrator. Define a discrete Lagrangian L d as (4.1 L d (R, F = 1 tr[(i 3 3 F J d ] U(R +1. Tis discrete Lagrangian is a first-order approximation of te integral of te continuous Lagrangian over one integration step. Terefore, te action sum, G d = = L d(r, F, wic is defined to be te summation of te discrete Lagrangian, approximates te action integral. Taing a variation of te action sum, we obtain te discrete equations of motion using te discrete Lagrange d Alembert principle. Te variation of a rotation matrix can be expressed using te exponential of a Lie algebra element: R ɛ = R e ɛη, were ɛ R and η so(3 is te variation expressed as a sew-symmetric matrix. Tus, te infinitesimal variation is given by δr = R η. Te Lagrange d Alembert principle states tat te following equation is satisfied for all possible variations η so(3. (4.2 δ = 1 tr[(i 3 3 F J d ] U(R +1 = 2 tr[η +1S(Bu +1 ] =. Here, te second summation approximates te virtual wor done by te external forces. Using te expression of te infinitesimal variation of a rotation matrix and using te fact tat te variations vanis at te end

20 2 MELVIN LEOK points, te above equation can be written as [ { 1 tr η (F J d J d F 1 + R T U }] R 2 S(Bu =. =1 Since te above expression sould be zero for all possible variations η so(3, te expression in te braces sould be symmetric. Ten, te discrete equations of motion in Lagrangian form are given by 1 ( F+1 J d J d F J d F+1 T + F T (4.3 J d = S(M+1 + S(Bu +1, (4.4 R +1 = R F. Using te discrete version of te Legendre transformation, te discrete equations of motion in Hamiltonian form are given by (4.5 (4.6 (4.7 S(Π = F J d J d F T, R +1 = R F, Π +1 = F T Π + (M +1 + Bu +1. Given (R, Π, we can obtain F by solving (4.5, and R +1 is obtained by (4.6. Finally, Π +1 is updated by (4.7. Tis yields a map (R, Π (R +1, Π +1, and tis process can be repeated. Te only implicit part is solving (4.5. We can express (4.5 in terms of a Lie algebra element S(f = logm(f so(3, and find f R 3 numerically by a Newton iteration. Te relative attitude F is obtained by te exponential map: F = e S(f. Terefore we are guaranteed tat F is a rotation matrix. Te order of te variational integrator is equal to te order of te corresponding discrete Lagrangian. Consequently, te above Lie group variational integrator is of first-order since (4.1 is a first-order approximation. Wile iger-order variational integrators can be obtained by modifying (4.1, we use te first-order integrator because it yields a compact form for te necessary conditions tat preserves te geometry; tese necessary conditions are developed in 4.4. Also, in 4.5, we will see tat wile our metod is formally first-order, it sadows te numerical trajectory of a second-order metod Problem formulation. An optimal impulsive control problem is formulated as a maneuver of a rigid body from a given initial configuration (R, x, Π, γ to a desired configuration (R d N, xd N, Πd N, γd N during te given maneuver time N. Control inputs are parameterized by teir value at eac time step. Te performance index is te square of te weigted l 2 norm of te control inputs. given: (x, γ, R, Π, (x d N, γ d N, R d N, Π d N, N, min J = u +1 2 (uf +1 T W f u f (um +1 T W m u m +1, = suc tat (x N, γ N, R N, Π N = (x d N, γ d N, R d N, Π d N, subject to discrete equations of motion, were W f, W m R 3 3 are symmetric positive definite matrices. Here, we use our Lie Euler variational integrator (4.5-(4.7, as it yields a compact form for te necessary conditions, wic are developed in Sensitivity derivatives. Variational model. Te variation of g η se(3 and te exponential map as = (R, x SE(3 can be expressed in terms of a Lie algebra g ɛ = g exp ɛη.

21 LIE GROUP VARIATIONAL INTEGRATORS AND THEIR APPLICATIONS TO OPTIMAL CONTROL 21 Te corresponding infinitesimal variation is given by δg = d dɛ g exp ɛη = T e L g η. ɛ= Using omogeneous coordinates (Murray et al., 1993, te above equation is written as [ ] [ ] [ ] δr δx R x = S(ζ χ, 1 [ ] R S(ζ = R χ (4.8, were ζ, χ R 3 so tat (S(ζ, χ se(3. Tis gives an expression for te infinitesimal variation in terms of te Lie algebra. Ten, small perturbations from te given discrete trajectory on T SE(3 can be written as (4.9 (4.1 (4.11 (4.12 x ɛ = x + ɛδx, γ ɛ = γ + ɛδγ, Π ɛ = Π + ɛδπ, R ɛ = R + ɛr S(ζ + O(ɛ 2, were δx, δγ, δπ, ζ R 3. Since te force due to te potential depends on te position and te attitude, its variation can be written as δf = d dɛ f (x + ɛδx, R + ɛr S(ζ. ɛ= Since tis operation is linear in δx and ζ, we can express δf as (4.13 δf = F x δx + F R ζ, were F x, F R R 3 3. Similarly, te variation of te moment due to te potential can be written as (4.14 δm = M x δx + M R ζ, were M x, M R R 3 3. Since F = R T R +1, te infinitesimal variation δf is given by δf = δr T R +1 + R T δr +1, = S(ζ F + F S(ζ +1. We can also express δf = F S(ξ for ξ R 3, using (4.8. Using te property S(R T x = R T S(xR for all R SO(3 and x R 3, we obtain (4.15 ξ = F T ζ + ζ +1. Linearized equations of motion. Substituting te variation model (4.9 (4.14 and te constrained variation (4.15 into te equations of motion (3.22 (3.26, and ignoring iger-order terms, te linearized equation of motion can be written as (4.16 z +1 = A z, were z = [δx ; δγ ; ζ ; δπ ] R 12, and te matrix A R can be suitably defined. Te solution of (4.16 is given by ( (4.17 z N = A z = Φz, =