Optimal Control of a Rigid Body using Geometrically Exact Computations on SE(3)

Transcription

1 Proceedings of he 45h IEEE Conference on Decision & Conrol Mancheser Grand Hya Hoel San Diego, CA, USA, December 3-5, 6 ThA.6 Opimal Conrol of a Rigid Body using Geomerically Exac Compuaions on SE(3) Taeyoung Lee, N. Harris McClamroch Deparmen of Aerospace Engineering Universiy of Michigan, Ann Arbor, MI 489 ylee, nhm}@umich.edu Melvin Leo Deparmen of Mahemaics Purdue Universiy, Wes Lafayee, IN 4797 mleo@mah.purdue.edu Absrac Opimal conrol problems are formulaed and efficien compuaional procedures are proposed for combined orbial and roaional maneuvers of a rigid body in hree dimensions. The rigid body is assumed o ac under he influence of forces and momens ha arise from a poenial and from conrol forces and momens. The ey feaures of his paper are is use of compuaional procedures ha are guaraneed o preserve he geomery of he opimal soluions. The heoreical basis for he compuaional procedures is summarized, and examples of opimal spacecraf maneuvers are presened. I. INTRODUCTION Discree opimal conrol problems for ranslaional and roaional dynamics of a rigid body under a poenial are sudied. Opimal conrol of a rigid body arises in numerous engineering and scienific fields. These problems provide boh a heoreical challenge and a numerical challenge in he sense ha he configuraion space has a Lie group srucure denoed by SE(3) ha defines a fundamenal consrain. Opimal conrol problems on a Lie group have been sudied in [], []. These sudies are based on he drifless inemaics of a Lie group. The dynamics are ignored, and i is assumed ha elemens in he corresponding Lie algebra are conrolled direcly. General-purpose numerical inegraion mehods, including he popular Runge Kua schemes, ypically preserve neiher he group srucure of he configuraion space nor geomeric invarians of he dynamics. Geomeric srucurepreserving inegraors, referred o as Lie group variaional inegraors [3], preserve he group srucure wihou he use of local chars, reprojecion, or consrains, and hey have he desirable propery ha hey are symplecic and momenum preserving, and hey exhibi good energy behavior for an exponenially long ime period. This paper presens geomerically exac and numerically efficien compuaional approaches o solve opimal conrol problems of a rigid body on a Lie group, SE(3). The dynamics and he inemaics are discreized by a Lie group variaional inegraor, and discree opimaliy condiions are consruced. Efficien numerical algorihms o solve he necessary condiion are developed. This mehod provide a subsanial advanage over curren mehods for opimal This research has been suppored in par by NSF under gran DMS , and by a gran from he Racham Graduae School, Universiy of Michigan. This research has been suppored in par by NSF under gran ECS conrol on a Lie group in he sense ha he dynamics of a rigid body as well as he inemaics equaion are explicily uilized, and he proposed compuaional approaches respec he group srucure. This paper is organized as follows. In Secion II, a Lie group variaional inegraor is developed. Opimal conrol problems using impulsive conrols are sudied in Secion III, and opimal conrol problems wih smooh conrols are sudied in Secion IV. Numerical resuls for a rigid dumbbell spacecraf are given in Secion V. II. LIE GROUP VARIATIONAL INTEGRATOR ON SE(3) The configuraion space for he ranslaional and roaional moion of a rigid body is he special Euclidean group, SE(3) = R 3 s SO(3). We idenify he coangen bundle T SE(3) wih SE(3) se(3) by lef ranslaion, and we idenify se(3) wih R 6 by an isomorphism beween R 6 and se(3), and he sandard inner produc on R 6. We denoe he aiude, posiion, angular momenum, and linear momenum of he rigid body by (R, x, Π,γ) T SE(3). The coninuous equaions of moion are given by ẋ = γ m, () γ = f + u f, () Ṙ = RS(Ω), (3) Π+Ω Π=M + u m, (4) where Ω R 3 is he angular velociy, and u f,u m R 3 are he conrol force in he inerial frame and he conrol momen in he body fixed frame, respecively. The consan mass of he rigid body is m R, and J R 3 3 denoes he momen of ineria, i.e. Π=JΩ. The map S( ) :R 3 so(3) is an isomorphism beween so(3) and R 3 defined by he condiion S(x)y = x y for all x, y R 3. We assume ha he poenial is dependen on he posiion and he aiude; U( ) :SE(3) R. The corresponding force and he momen due o he poenial are given by f = U x, (5) M = r u r + r u r + r 3 u r3, (6) where r i,u ri R 3 are he ih row vecor of R and U R, respecively. Since he dynamics of a rigid body has he srucure of a Lagrangian or Hamilonian sysem, hey are characerized /6/$. 6 IEEE. 7

2 45h IEEE CDC, San Diego, USA, Dec. 3-5, 6 ThA.6 by symplecic, momenum and energy preserving properies. These geomeric feaures deermine he qualiaive behavior of he rigid body dynamics, and hey can serve as a basis for heoreical sudy of rigid body dynamics. In conras, he mos common numerical inegraion mehods, including he widely used Runge-Kua schemes, neiher preserve he Lie group srucure nor hese geomeric properies. In addiion, sandard Runge-Kua mehods fail o capure he energy dissipaion of a conrolled sysem accuraely [4]. Addiionally, if we inegrae (3) by a ypical Runge-Kua scheme, he quaniy R T R ineviably drifs from he ideniy marix as he simulaion ime increases. I is ofen proposed o parameerize (3) by Euler angles or uni quaernions. However, Euler angles are no global expressions of he aiude since hey have associaed singulariies. Uni quaernions do no exhibi singulariies, bu are consrained o lie on he uni hree-sphere S 3, and general numerical inegraion mehods do no preserve he uni lengh consrain. Therefore, quaernions have he same numerical drif problem. Renormalizing he quaernion vecor a each sep ends o brea oher conservaion properies. Furhermore, uni quaernions, which are diffeomorphic o SU(), double cover SO(3). So here are ineviable ambiguiies in expressing he aiude. In [3], Lie group variaional inegraors are inroduced by explicily adaping Lie group mehods [5] o he discree variaional principle [4]. They have he desirable propery ha hey are symplecic and momenum preserving, and hey exhibi good energy behavior for an exponenially long ime period. They also preserve he Euclidian Lie group srucure wihou he use of local chars, reprojecion, or consrains. These geomerically exac numerical inegraion mehods yield highly efficien and accurae compuaional algorihms for rigid body dynamics. They avoid singulariies and ambiguiies. Using he resuls presened in [6], a Lie group variaional inegraor on SE(3) for equaions () (4) is given by x + = x + h ( ) m γ + h f + u f, (7) m γ + = γ + h ( ) f + u f + h ( ) f + + u f +, (8) hs(π + h (M + u m )) = F J d J d F T, (9) R + = R F, () Π + = F T Π + h F T (M + u m )+ h ( M+ + u m ) +, () where he subscrip denoes he h discree variables for a fixed inegraion sep size h R. J d R 3 3 is a nonsandard momen of ineria marix defined by J d = r[j] I 3 3 J. F SO(3) is he relaive aiude beween adjacen inegraion seps. For given (R,x, Π,γ ) and conrol inpus, (9) is solved o find F. Then (R +,x + ) are obained by (),(7). Using (5),(6), (f +,M + ) are compued, and hey are used o find (Π +,γ + ) by (),(8). This yields amap(r,x, Π,γ ) (R +,x +, Π +,γ + ),and his process is repeaed. The only implici par is (9). The acual compuaion of F is done in he Lie algebra so(3) of dimension 3, and he roaion marices are updaed by muliplicaion. This approach is compleely differen from inegraion of he inemaics equaion (3); here is no excessive compuaional burden. I can be shown ha his inegraor has second order accuracy. The properies of hese discree equaions of moion are discussed in more deail in [3], [6]. III. OPTIMAL IMPULSIVE CONTROLOFARIGID BODY We formulae an opimal impulsive conrol problem for a rigid body on SE(3), and we develop sensiiviy derivaives. They are used in our compuaional mehod for solve opimal impulsive conrol problems. A. Problem formulaion An opimal impulsive conrol problem is formulaed as a maneuver of a rigid body from a given iniial configuraion (R,x, Π,γ ) o a desired configuraion described by (RN,x N, Π N,γ N ) T SE(3) C(R N,x N, Π N,γ N )= }, where C( ) : T SE(3) R c during he given maneuver ime N. Two impulsive conrol inpus are applied a he iniial ime and he erminal ime. We assume ha he conrol inpus are purely impulsive, which means ha each impulse changes he momenum of he rigid body insananeously, bu i does no have any effec on he posiion and he aiude of he rigid body a ha insan. The moion of he rigid body beween he iniial ime and he erminal ime is unconrolled. i.e. u f = um =. The performance index is he sum of he magniudes of he iniial impulse and he erminal impulse. I is equivalen o minimizing he sums of he iniial momenum change and he erminal momenum change. We ransform his opimal impulsive conrol problem ino a parameer opimizaion problem. Le (Π +,γ+ ) be he iniial momenum afer he iniial impulsive conrol. Then, he erminal saes are deermined by he discree equaions of moion, and he momenum afer he erminal impulsive conrol, (Π + N,γ+ N ), can be compued by he erminal consrain. Therefore, he performance index and he consrain are compleely deermined by (Π +,γ+ ). Thus, he opimal impulsive conrol on SE(3) is formulaed as given :(R,x, Π,γ ),N min J = Π + Π + Π + γ + γ,γ+ + Π + N Π N + γ + N γ N, such ha C(R N,x N, Π + N,γ+ N )=, subjec o discree equaions of moion (7) (). If he desired values for all of he erminal saes are specified by he consrains, hen here is no freedom for opimizaion. This problem degeneraes o a wo poin boundary value problem on SE(3), which can be considered as an exension of he Lamber problem for he resriced wo body problem. A similar opimal conrol problem for aiude dynamics of a rigid body on SO(3) is sudied in [7]. 7

3 45h IEEE CDC, San Diego, USA, Dec. 3-5, 6 ThA.6 B. Sensiiviy derivaives Variaional model: The variaion of g = (R,x ) SE(3) can be expressed in erms of a Lie algebra elemen η se(3) and he exponenial map as g ɛ = g exp ɛη. The corresponding infiniesimal variaion is given by δg = d dɛ g exp ɛη =T e L g η. ɛ= Using homogeneous coordinaes [8], he above equaion is wrien in a marix equaion as [ ] [ ][ ] δr δx R x = S(ζ ) χ, [ ] R S(ζ = ) R χ, () where ζ,χ R 3 so ha (S(ζ ),χ ) se(3). This gives an expression for he infiniesimal variaion of a Lie group elemen in erms of is Lie algebra. Then, small perurbaions from a given rajecory on T SE(3) can be wrien as x ɛ = x + ɛδx, (3) γ ɛ = γ + ɛδγ, (4) Π ɛ =Π + ɛδπ, (5) R ɛ = R + ɛr S(ζ )+O(ɛ ), (6) where δx,δγ,δπ,ζ are considered in R 3. We derive expressions for he consrained variaion of F using () and (6). Since F = R T R + by (), he infiniesimal variaion δf is given by δf = δr T R + + R T δr + = S(ζ )F + F S(ζ + ). We can also express δf = F S(ξ ) for ξ R 3, using (). Using he propery S(R T x)=r T S(x)R for all R SO(3) and x R 3, we obain he consrained variaion of F ξ = F T ζ + ζ +. (7) Linearized equaions of moion: Subsiuing he variaion model (3) (6) and he consrained variaion (7) ino he equaions of moion (7) (), and ignoring higher order erms, he linearized equaion of moion can be wrien as z + = A z, (8) where z =[δx ; δγ ; ζ ; δπ ] R, and A R can be suiably defined. The soluion of (8) is obained as z N =Φz, (9) where Φ R represens he sensiiviy derivaives of he erminal sae wih respec o he iniial sae on SE(3). C. Compuaional approach We solve he opimal impulsive conrol problem by he Sequenial Quadraic Programming (SQP) mehod using analyical expressions for he gradiens of he performance index and he consrains. The exac compuaion of he gradiens are crucial for efficien numerical opimizaion. For he given problem, δx = ζ =since he iniial posiion and he iniial aiude are fixed. Thus, (9) is wrien as δx N Φ Φ 4 [ ] δγ N ζ N = Φ Φ 4 δγ + Φ 3 Φ 34 δπ +, () δπ N Φ 4 Φ 44 where Φ ij R 3 3, i, j (,, 3, 4) are submarices of Φ. The above equaion represens he sensiiviies of he erminal sae wih respec o he iniial momenum (Π +,γ+ ). Therefore, we can obain expressions for gradiens of he performance index and he consrains, and any Newon ype numerical approach can be applied. IV. OPTIMAL CONTROL OF A RIGID BODY We formulae an opimal conrol problem for a rigid body on SE(3) assuming ha conrol forces and momens are applied during he maneuver. Necessary condiions for opimaliy are developed and compuaional approaches are presened o solve he corresponding wo poin boundary value problem. A. Problem formulaion An opimal impulsive conrol problem is formulaed as a maneuver of a rigid body from a given iniial configuraion (R,x, Π,γ ) o a desired configuraion (RN d,xd N, Πd N,γd N ) during he given maneuver ime N. Conrol inpus are parameerized by heir value a each ime sep. The performance index is he square of he weighed l norm of he conrol inpus. given: (x,γ,r, Π ), (x d N,γN,R d N, d Π d N ), N, N h min J = u + (uf + )T W f u f + + h (um +) T W m u m +, = such ha (x N,γ N,R N, Π N )=(x d N,γN,R d N, d Π d N), subjec o discree equaions of moion (7) (), where W f,w m R 3 3 are symmeric posiive definie marices. Here we use a modified version of he discree equaions of moion wih firs order accuracy, because i yields a compac form for he necessary condiions, which are developed he following subsecion. A similar opimal conrol problem for aiude dynamics on SO(3) is sudied in [9]. B. Necessary condiions for opimaliy Define an augmened performance index as J a = N = h (uf + )T W f u f + + h (um +) T W m u m + + λ,t x + + x + h } m γ } + λ,t γ + + γ + hf + + hu f + + λ 3,T S ( logm(f R T R + ) ) + λ 4,T Π+ + F T Π + h ( M + + u m )} +, 7

4 45h IEEE CDC, San Diego, USA, Dec. 3-5, 6 ThA.6 where λ i R3 are Lagrange mulipliers. The consrain (9) is considered implicily using a consrained variaion. Using he variaional model (3) (6), he consrained variaion (7), and he fac ha he variaions vanish a =,N, we obain he infiniesimal variaion of J a as δj a = N = + hδu m,t hδu f,t } W f u f + λ Wm u m + λ 4 } + z T λ + A T } λ, where λ =[λ ; λ ; λ3 ; λ4 ] R, and A R is presened in (8). Since δj a = for all variaions, we obain necessary condiions for opimaliy as follows. x + = x + h m γ, () γ + = γ + hf + + hu f +, () hs(π )=F J d J d F T, (3) R + = R F, (4) Π + = F T Π + hm + + hu m +, (5) u f + = Wf λ, (6) u m + = Wm λ 4, (7) λ = A T +λ +. (8) In he above equaions, he only implici par is (3). For a given iniial condiion (R,x, Π,γ ) and λ, we can find F by solving (3). Then, R,x is obained by (4),(), and he conrol inpu u f,um is obained by (6),(7). γ, Π can be obained by (),(5). Now we compue (R,x, Π,γ ). We solve (3) o find F. Finally, λ can be obained by (8). This yields a map (R,x, Π,γ ),λ } (R,x, Π,γ ),λ }, and his process can be repeaed. C. Compuaional Approach The necessary condiions for opimaliy are expressed in erms of a wo poin boundary problem on T SE(3) and is dual. This problem is o find he opimal discree flow, muliplier, and conrol inpus o saisfy he equaions of moion () (5), opimaliy condiions (6),(7), muliplier equaions (8), and boundary condiions simulaneously. We use a neighboring exremal mehod []. A nominal soluion saisfying all of he necessary condiions excep he boundary condiions is chosen. The unspecified iniial muliplier is updaed by successive linearizaion so as o saisfy he specified erminal boundary condiions in he limi. This is also referred o as a shooing mehod. The main advanage of he neighboring exremal mehod is ha he number of ieraion variables is small. In oher approaches, he iniial guess of conrol inpu hisory or muliplier variables are ieraed, so he number of opimizaion parameers are proporional o he number of discree ime seps. The difficuly is ha he exremal soluions are sensiive o small changes in he unspecified iniial muliplier values. The nonlineariies also mae i hard o consruc an accurae esimae of sensiiviy, and i may resul in numerical illcondiioning. Therefore, i is imporan o compue he sensiiviies accuraely o apply he neighboring exremal mehod. Here he opimaliy condiions (6) and (7) are subsiued ino he equaions of moion and he muliplier equaions. The sensiiviies of he specified erminal boundary condiions wih respec o he unspecified iniial muliplier condiions is obained by a linear analysis. Similar o (8), he linearized equaions of moion can be wrien as z + = A z + A δλ, (9) where A = hdiag[,w f,,wm ] R.Wecan linearize he muliplier equaions (8) o obain δλ = A +z + + A T +δλ +, (3) where A + R can be defined properly. The soluion of he linear equaions (9) and (3) can be obained as [ ] [ ][ ] zn Ψ Ψ = z δλ N Ψ Ψ, δλ where Ψ ij R. For he given wo poin boundary value problem z = since he iniial condiion is fixed, and λ N is free. Thus, z N =Ψ δλ. (3) The marix Ψ represens he sensiiviy of he specified erminal boundary condiions wih respec o he unspecified iniial mulipliers. Using his sensiiviy, an iniial guess of he unspecified iniial condiions is ieraed o saisfy he specified erminal condiions in he limi. Any ype of Newon ieraion can be applied. We use a line search wih bacracing algorihm, referred o as Newon- Armijo ieraion in []. The procedure is summarized as follows. : Guess an iniial muliplier λ. : Find x,γ, Π,R,λ using () (8). 3: Compue he erminal B.C. error; Error = z N. 4: Se Error =Error, i =. 5: while Error >ɛ S. 6: Find a line search direcion; D =Ψ. 7: Se c =. 8: while Error > ( αc)error 9: Choose a rial muliplier λ = λ + cdz N. : Find x,γ, Π,R,λ using () (8). : Compue he error; Error = z N. : Se c = c/, i = i +. 3: end while 4: Se λ = λ, Error = Error. (accep he rial) 5: end while Here i is he number of ieraions, and ɛ S,α R are a sopping crierion and a scaling facor, respecively. The ouer loop finds a search direcion by compuing he sensiiviy derivaives, and he inner loop performs a line search o find he larges sep size c R along he search direcion. The error in saisfacion of he erminal boundary condiion is deermined a each inner ieraion. 73

5 45h IEEE CDC, San Diego, USA, Dec. 3-5, 6 ThA.6 V. NUMERICAL EXAMPLES A. Resriced Full Two Body Problem We sudy a maneuver of a rigid spacecraf under a cenral graviy field. We assume ha he mass of he spacecraf is negligible compared o he mass of a cenral body, and we consider a fixed frame aached o he cenral body as an inerial frame. The resuling model is a Resriced Full Two Body Problem (RFBP). The spacecraf is modeled as a dumbbell, which consiss of wo equal spheres and a massless rod. The graviaional poenial is given by U(x, R) = GMm q= x + Rρ q, (3) where G R is he graviaional consan, M,m R are he mass of he cenral body, and he mass of he dumbbell, respecively. The vecor ρ q R 3 is he posiion of he qh sphere from he mass cener of he dumbbell expressed in he body fixed frame (q, }). The mass, lengh, and ime dimensions are normalized by he mass of he dumbbell, he radius of a reference circular orbi, and is orbial period. B. Opimal Impulsive Conrol We sudy an impulsive orbial ransfer problem wih an aiude change. Iniially, he spacecraf is on a reference circular orbi. We consider wo cases. In he firs case, he spacecraf moves o a desired circular orbi and he desired values for all of he erminal sae are specified. There is no freedom for opimizaion, and he resuling problem is a wo poin boundary value problem on SE(3). This maneuver can be considered as a generalizaion of Hohmann ransfer []. The desired maneuver involves doubling he orbial radius in addiion o a large angle aiude change. In he second case, he erminal consrains are relaxed such ha he spacecraf is allowed o ransfer o any poin on he desired orbi. The desired erminal orbi is described by is orbial radius r d R, and a direcional vecor e n S normal o he orbial plane. Two consrains are imposed o locae he dumbbell in he desired orbial plane wih he desired orbial radius, and one consrain is applied o align he dumbbell o he normal direcion. The gradiens of he performance index and he consrains are obained by using (). We use Malab fmincon funcion as an implemenaion of he SQP algorihm. Figures and show he spacecraf maneuver, and linear velociy and angular velociy responses, where red circles denoe he velociies before he iniial impulse and he velociies afer he erminal impulse. Thus, differences beween solid lines and red circles are proporional o he impulsive conrols. (Simple animaions which show hese maneuvers of he spacecraf can be found a hp:// ylee.) The error in saisfacion of he erminal boundary value of he firs case is The performance index and he maximum violaions of he consrains for he second case are.35 and , respecively. C. Opimal Conrol We sudy an opimal orbial ransfer problem o increase he orbial inclinaion by 6 deg, and an orbial capure problem o he reference circular orbi. Figures 3 and 4 show he opimized spacecraf maneuver, conrol inpus hisory. For each case, he performance indices are 3.3 and.9, and he maximum violaions of he consrain are and 3.6 3, respecively. Figures 3.(b) and 4.(b) show he violaion of he erminal boundary condiion according o he number of ieraions in a logarihmic scale. Red circles denoe ouer ieraions in Newon-Armijo ieraion o compue he sensiiviy derivaives. For all cases, he iniial guesses of he unspecified iniial muliplier are arbirarily chosen. The error in saisfacion of he erminal boundary condiion converges quicly o machine precision afer he soluion is close o he local minimum a around h ieraion. These convergence resuls are consisen wih he quadraic convergence raes expeced of Newon mehods wih accuraely compued gradiens. The neighboring exremal mehod, also referred o as he shooing mehod, is numerically efficien in he sense ha he number of opimizaion parameers is minimized. Bu, his approach may be prone o numerical ill-condiioning [3]. A small change in he iniial muliplier can cause highly nonlinear behavior of he erminal aiude and angular momenum. I is difficul o compue he gradien for Newon ieraions accuraely, and he numerical error may no converge. However, he numerical examples presened in his paper show excellen numerical convergence properies. This is because he proposed compuaional algorihms on SE(3) are geomerically exac and numerically accurae. The dynamics of a rigid body arises from Hamilonian mechanics, which have neural sabiliy, and is adjoin sysem is also neurally sable. The proposed Lie group variaional inegraor and he discree muliplier equaions, obained from variaions expressed in he Lie algebra, preserve he neural sabiliy propery numerically. Therefore he sensiiviy derivaives are compued accuraely. VI. CONCLUSIONS Opimal conrol problems for combined orbial and roaional maneuvers of a rigid body are formulaed and efficien compuaional procedures are proposed. The dynamics are discreized by a Lie group variaional inegraor, and sensiiviy derivaives are developed by a linear analysis. Discree necessary condiions for opimaliy are consruced, and he corresponding wo poin boundary value problem is solved efficienly. This approach is geomerically exac in he sense ha he Lie group variaional inegraor preserves he group srucure as well as he geomeric invarian properies, and he sensiiviy derivaives are expressed in erms of is Lie algebra. Since he configuraion of a rigid body is defined globally using an elemen of SE(3), his approach compleely avoids singulariy or ambiguiy arising from oher represenaions such as Euler angles and quaernions. Numerical examples show he efficiency of he proposed compuaional approach. 74

6 45h IEEE CDC, San Diego, USA, Dec. 3-5, 6 ThA.6 5 Error (a) Spacecraf maneuver (a) Spacecraf maneuver Ieraion (b) Convergence rae (b) Velociy v (c) Angular velociy Ω (c) Conrol force u f (d) Conrol momen u m Fig.. TPBVP: Orbial radius change Fig. 3. Opimal conrol: Orbial inclinaion change 5 Error (a) Spacecraf maneuver (a) Spacecraf maneuver Ieraion (b) Convergence rae x (b) Velociy v (c) Angular velociy Ω (c) Conrol force u f 5 x (d) Conrol momen u m Fig.. Opimal impulsive conrol: Orbial radius change Fig. 4. Opimal conrol: Orbial capure REFERENCES [] K. Spindler, Opimal conrol on Lie groups wih applicaions o aiude conrol, Mahemaics of Conrol, Signals, and Sysems, vol., pp. 97 9, 998. [] S. Sasry, Opimal conrol on Lie groups, in Proceedings of he Third Inernaional Congress on Indusrial and Applied Mahemaics (ICIAM), 995. [3] T. Lee, M. Leo, and N. H. McClamroch, A Lie group variaional inegraor for he aiude dynamics of a rigid body wih applicaions o he 3D pendulum, in Proceedings of he IEEE Conference on Conrol Applicaions, 5, pp [4] J. E. Marsden and M. Wes, Discree mechanics and variaional inegraors, Aca Numerica, vol., pp ,. [5] A. Iserles, H. Z. Munhe-Kaas, S. P. Nørse, and A. Zanna, Lie-group mehods, Aca Numerica, vol. 9, pp ,. [6] T. Lee, M. Leo, and N. H. McClamroch, Lie group variaional inegraors for he full body problem, Compuer Mehods in Applied Mechanics and Engineering, 5, submied. [Online]. Available: hp://arxiv.org/mah.na/58365 [7], Aiude maneuvers of a rigid spacecraf in a circular orbi, in Proceedings of he American Conrol Conference, 6, pp [Online]. Available: hp://arxiv.org/mah.na/5999 [8] R. M. Murray, Z. Li, and S. S. Sasry, A Mahemaical Inroducion o Roboic Manipulaion. CRC Press, 993. [9] T. Lee, M. Leo, and N. H. McClamroch, Opimal aiude conrol of a rigid body using geomerically exac compuaions on SO(3), Journal of Opimizaion Theory and Applicaions, 6, submied. [Online]. Available: hp://arxiv.org/mah.oc/644 [] A. E. Bryson and Y.-C. Ho, Applied Opimal Conrol. Hemisphere Publishing Corporaion, 975. [] C. T. Kelley, Ieraive Mehods for Linear and Nonlinear Equaions. SIAM, 995. [] J. M. A. Danby, Fundamenals of Celesial Mechanics. Willmann Bell Inc., 988. [3] J. T. Bes, Pracical Mehods for Opimal Conrol Using Nonlinear Programming. SIAM,. 75