Optimal Attitude Control of a Rigid Body Using Geometrically Exact Computations on So(3)

Journal of Dynamical and Control Systems, Vol. 14, No. 4, October 2008, 465487 ( c2008)

OPTIMAL ATTITUDE CONTROLOF A RIGID BODY

USING GEOMETRICALLY EXACTCOMPUTATIONS ON SO(3)

T. LEE, M. LEOK, and N. H. MCCLAMROCH

Abstract. An ecient and accurate computational approach is pro-posed for a nonconvex optimal attitude control for a rigid body. Theproblem is formulated directly as a discrete time optimization prob-lem using a Lie group variational integrator. Discrete time necessaryconditions for optimality are derived, and an ecient computationalapproach is proposed to solve the resulting two-point boundary-valueproblem. This formulation wherein the optimal control problem issolved based on discretization of the attitude dynamics and deriva-tion of discrete time necessary conditions, rather than developmentand discretization of continuous time necessary conditions, is shownto have signicant advantages. In particular, the use of geometricallyexact computations on SO(3) guarantees that this optimal control ap-proach has excellent convergence properties even for highly nonlinearlarge angle attitude maneuvers.

1. Introduction

A discrete optimal control problem for attitude dynamics of a rigid bodyin the presence of an attitude dependent potential is considered. The ob-jective is to minimize the square of the l2 norm of external control torqueswhich transfer a given initial attitude and an initial angular momentum ofthe rigid body to a desired terminal attitude and a terminal angular mo-mentum during a xed maneuver time. The attitude of the rigid body isdened by the orientation of a body xed frame with respect to a referenceframe; the attitude is represented by a rotation matrix that is a 3 3 or-thogonal matrix with determinant of 1. The conguration manifold is thespecial orthogonal group, SO(3).

2000 Mathematics Subject Classification. 49J15, 37M15.Key words and phrases. Optimal control, symplectic integrator.The rst and second authors have been partially supported by NSF (project Nos. DMS-0504747 and DMS-0726263). The rst and third authors have been partially supportedby NSF (project Nos. ECS-0244977 and CMS-0555797.

465

1079-2724/08/1000-0465/0 c 2008 Springer Science+Business Media, Inc.

DOI: 10.1007/s10883-008-9047-7

466 T. LEE, M. LEOK, and N. H. MCCLAMROCH

The dynamics of a rigid body has fundamental invariance properties. Inthe absence of nonconservative forces, total energy is preserved. A conse-quence of Noethers theorem is that symmetries in the Lagrangian resultin conservation of the associated momentum map. Furthermore, the con-guration manifold of the rigid body attitude dynamics is the Lie groupSO(3). General-purpose numerical integration methods, including the pop-ular RungeKutta schemes, typically preserve neither rst integrals nor thegeometric characteristics of the conguration manifold. In particular, theorthogonal structure of rotation matrices is not reserved numerically withstandard schemes.

A Lie group variational integrator that preserves those geometric featuresis presented in [19], and integrators on the conguration manifold SO(3) andSE(3) are developed in [10] and [15,16], respectively. These integrators areobtained from a discrete variational principle, and they exhibit the sym-plectic and momentum preservation properties, and good energy behaviorcharacteristic of variational integrators [21]. Since the rotation matrices areupdated by a group operation, they automatically evolve on the rotationgroup without the need for reprojection techniques or constraints [6].

Optimal attitude control problems are studied in [23]. The angular ve-locity of a rigid body is treated as a control input; an optimal angularvelocity that steers the rigid body is derived from the attitude kinematics.Continuous time optimal control problems on a Riemannian manifold arestudied in [4], where necessary conditions for optimality are derived froma variational principle. An optimal control problem based on discrete me-chanics is studied in [7]. The discrete equations of motion and the boundaryconditions are imposed as constraints, and the optimal control problem issolved by a general-purpose parameter optimization tool. This approachrequires large computation time, since the number of optimization parame-ters is proportional to the number of discrete time steps. Discrete necessaryconditions for optimal control of the attitude dynamics of a rigid body arepresented in [5].

This paper proposes an exact and ecient computational approach tosolve an optimal control problem associated with the attitude dynamics ofa rigid body that evolves on the conguration manifold SO(3). We assumethat the control inputs are parameterized by their value at each time step.A Lie group variational integrator on SO(3) that includes the eects ofexternal control inputs is developed using the discrete LagrangedAlembertprinciple. Discrete necessary conditions for optimality are obtained using avariational principle, while imposing the Lie group variational integrator asdynamic constraints.

The necessary conditions are expressed as a two-point boundary-valueproblem on TSO(3) and its dual. Sensitivity derivatives along an extremalsolution are developed by following the procedures presented in [11], and

OPTIMAL CONTROL OF A RIGID BODY ON SO(3) 467

they are used to construct an algorithm that solves the boundary-valueproblem eciently. Since the attitude of the rigid body is represented bya rotation matrix, and the orthogonal structure of rotation matrices is pre-served by the Lie group variational integrator, the discretization of theoptimal control problem does not exhibit singularities.

This paper is organized as follows. In Sec. 2, a Lie group variationalintegrator is developed using the discrete LagrangedAlembert principle.Necessary conditions for optimality and a computational approach to solvethe two-point boundary problem are presented in Sec. 3. Numerical resultsfor the attitude control of an underactuated 3D pendulum, and for a fullyactuated spacecraft in a circular orbit are given in Sec. 4.

2. Equations of Motion for the Attitude Dynamicsof a Rigid Body

In this section, we dene a rigid body model acting under a potentialeld and we develop discrete equations of motion for the attitude dynamicsof the rigid body, referred to as a Lie group variational integrator. Thesediscrete equations of motion are used as dynamic constraints for the optimalcontrol problem presented in Sec. 3.

2.1. Rigid body model. We consider the attitude dynamics of a rigidbody in the presence of an attitude dependent potential. The congurationmanifold is the Lie group, SO(3). We assume that the potential U() :SO(3) R is determined by the attitude of the rigid body, R SO(3).External control inputs generate moments about the mass center of the rigidbody. A spacecraft on a circular orbit including gravity gradient eects [11],a 3D pendulum [10], or a free rigid body can be modeled in this way. Thecontinuous equations of motion are given by

+ = M + Bu, (1)R = RS(), (2)

where R3 is the angular velocity of the body expressed in the bodyxed frame, and = J R3 is the angular momentum of the bodyfor a moment of inertia matrix J R33. The vector M R3 is themoment due to the potential, u Rm is the external control input, andB R3m is an input matrix. If the rank of the input matrix is lessthan 3, then the rigid body is underactuated. The matrix valued function,S() : R3 so(3) is an isomorphism dened such that S(x)y = x yfor all x, y R3. The moment due to the potential is determined by therelationships, S(M) = UR

TRRT UR , or more explicitly,

M = r1 ur1 + r2 ur2 + r3 ur3 , (3)


where ri, uri R13 are the ith row vectors of R and UR , respectively. Adetailed derivation of the above equations can be found in [10].

2.2. Lie group variational integrator. We describe a structure preserv-ing numerical integrator for the attitude dynamics of the rigid body, re-ferred to as a Lie group variational integrator. Since it is obtained from adiscrete variational approach, and it exactly preserves the momentum andsymplectic form, while exhibiting good energy behavior over exponentiallylong times. Since a Lie group numerical method [6] is explicitly adoptedin the variational development, the rotation matrix automatically remainson SO(3).

The Lie group variational integrator is obtained by following procedurescommonly adopted in Lagrangian mechanics. The variational approach isbased on the discretizing Hamilton principle rather than discretizing thecontinuous equations of motion. The velocity phase space of the continuousLagrangian is replaced by discrete variables, and a discrete Lagrangian ischosen. Taking a variation of the action sum dened as the summation of thediscrete Lagrangian, we obtain a Lagrangian form of the discrete equationsof motion using the LagrangedAlembert principle. A discrete version ofthe Legendre transformation yields discrete equations in Hamiltonian form.

The detailed derivation is presented in [10,15]. In this paper, we extendthese results to include the eects of external control inputs. Consider thexed integration step size h R. Let Rk SO(3) denote the attitude ofthe rigid body at time t = kh. We introduce a new variable Fk SO(3)dened by

Fk = RTk Rk+1, (4)

which represents a relative attitude between integration steps. If we ndFk SO(3) at each integration step, the rotation matrix is updated by mul-tiplication of two rotation matrices, i.e., Rk+1 = RkFk, which is the groupoperation on SO(3). In particular, we ensure that Fk is a rotation matrixby expressing it as Fk = exp(k), where k so(3) is a skew-symmetricmatrix and exp() denotes the matrix exponential. This guarantees thatthe rotation matrix evolves on SO(3) automatically. This is the approachof Lie group methods [6]. The following procedure provides an expressionfor Fk using the discrete LagrangedAlembert principle.

Using the kinematic relationship (2), S(k) can be approximated as

S(k) = RTk Rk RTk(

Rk+1 Rkh

)=

1h

(Fk I33) .


Using the above equation, the kinetic energy of the rigid body is approxi-mated as

T =12tr[S(k)JdS(k)T

] 12h2

tr[(Fk I33)Jd(Fk I33)T

]

=1h2

tr[(I33 Fk) Jd] ,where Jd R33 is a nonstandard moment of inertia matrix of the rigidbody dened in terms of the standard moment of inertia matrix J R33as Jd = 12 tr[J ] I33 J . Dene a discrete Lagrangian Ld as

Ld(Rk, Fk) =1htr[(I33 Fk) Jd] hU(Rk+1). (5)

This discrete Lagrangian is a rst-order approximation of the integral of thecontinuous Lagrangian over one integration step. Therefore, the followingaction sum, dened as the summation of the discrete Lagrangian, approx-imates the action integral; Gd =

N1k=0 Ld(Rk, Fk). Taking a variation of

the action sum, we obtain the discrete equations of motion using the dis-crete LagrangedAlembert principle. The variation of a rotation matrixcan be expressed using the exponential of a Lie algebra element:

Rk = Rk exp k,

where R and k so(3) is the variation expressed as a skew-symmetricmatrix. Thus, the innitesimal variation is given by

Rk =d

d

=0

Rk = Rkk. (6)

The LagrangedAlembert principle states that the following equation issatised for all possible variations k so(3):

N1k=0

1htr[(I33 Fk) Jd] hU(Rk+1)

N1k=0

h

2tr[k+1S(Buk+1)] = 0. (7)

Using the expression of the innitesimal variation of a rotation matrix (6)and using the fact that the variations vanish at the end points, the aboveequation can be written as

N1k=1

tr[k

{1h

(FkJd JdFk1) + hRTkU

Rk h

2S(Buk)

}]= 0.

Since the above expression should be zero for all possible variations k so(3), the expression in the braces should be symmetric. Then, the discreteequations of motion in Lagrangian form are given by

1h

(Fk+1Jd JdFk JdFTk+1 + FTk Jd

)= hS(Mk+1) + hS(Buk+1), (8)

Rk+1 = RkFk. (9)


Using the discrete version of the Legendre transformation, the discrete equa-tions of motion in Hamiltonian form are given by

hS(k) = FkJd JdFTk , (10)Rk+1 = RkFk, (11)

k+1 = FTk k + h (Mk+1 + Buk+1) . (12)

Given (Rk,k), we can obtain Fk by solving (10), and Rk+1 is obtainedby (11). The moment due to the potential Mk+1 can be calculated by(3). Finally, k+1 is updated by (12). This yields a map (Rk,k) (Rk+1,k+1), and this process can be repeated. The only implicit partis solving (10). We can express (10) in terms of a Lie algebra elementS(fk) = logm(Fk) so(3), and nd fk R3 numerically by a Newtoniteration. The relative attitude Fk is obtained by the exponential map:Fk = expS(fk). Therefore, we are guaranteed that Fk is a rotation matrix.

The Lie group variational integrator has the desirable properties thatthey are symplectic and momentum preserving, and they exhibit good en-ergy behavior for an exponentially long time period as they are derived fromthe discrete Hamilton principle. It also preserves the Lie group structurewithout the use of local charts, reprojection, or constraints, and it avoidssingularities and ambiguities associated with other attitude parameteriza-tions. This geometrically exact numerical integration method yields highlyecient and accurate computational algorithms for rigid body dynamics,and all of these properties are independent of the stepsize.

The order of the variational integrator is equal to the order of the cor-responding discrete Lagrangian. Consequently, the above Lie group vari-ational integrator is of rst-order since (5) is a rst-order approximation.While higher-order variational integrators can be obtained by modifying(5), we use the rst-order integrator because it yields a compact form forthe necessary conditions that preserves the geometry; these necessary con-ditions are developed in Section 3.

It is worthwhile noting that while the Lie group variational integratoradopted in this paper is formally rst-order, it is the Lie group analogue ofsymplectic Euler, which is symplectically equivalent [20, 24] to the second-order accurate StormerVerlet method [3]. This means that one can obtainthe StormerVerlet method by conjugating the symplectic Euler methodwith a symplectic transformation. In particular, numerical trajectories ofsymplectic Euler will shadow numerical trajectories obtained using StormerVerlet.

In practice, the shadowing result imparts the symplectic Euler methodwith the same desirable qualitative properties as StormerVerlet. Since onan appropriate choice of charts, our Lie symplectic Euler method reducesto symplectic Euler in coordinates, it follows that there is a corresponding


second-order Lie StormerVerlet method that our integrator is symplecti-cally equivalent to, and whose discrete trajectories are shadowed by ourmethod.

3. Discrete Optimal Control of the Attitude Dynamicsof a Rigid Body

We formulate a discrete optimal control problem for the attitude dynam-ics of a rigid body, and we derive necessary conditions for optimality usinga variational principle. The necessary conditions are expressed as a two-point boundary-value problem, and a computational approach to solve theboundary-value problem is proposed using sensitivity derivatives.

3.1. Problem formulation. A discrete time optimal control problem inSO(3) is formulated as a maneuver of a rigid body from a given initialattitude R0 SO(3) and an initial angular momentum 0 R3 to a desiredterminal attitude RdN SO(3) and a terminal angular momentum dN R3during a given maneuver time N . The performance index is the square ofthe l2 norm of the control input; the discrete equations of motion developedin the previous section are imposed as constraints:

given: R0,0, RdN ,dN , N,

minuk+1

J =N1k=0

h

2uk+12 ,

such that RN = RdN , N = dN ,

subject to (10), (11) and (12).

In [7], an optimal control problem based on discrete mechanics is consid-ered. The control inputs at each discrete step are considered as optimiza-tion parameters, and the discrete equations of motion and the boundaryconditions are imposed as constraints. The optimization problem is solvednumerically by a general-purpose parameter optimization tool such as se-quential quadratic programming (SQP). The same approach can be appliedto the above optimization problem. However, it has a large computationalburden since the number of optimization parameters, mN , is proportionalto the number of integration steps. Usually, a large time step size is chosento make the number of integration steps small, or the control inputs areapproximated by collocation points. The resulting control inputs tends tobe under-resolved and sub-optimal.

We derive necessary conditions for optimality using the standard calculusof variations. We assume that the control inputs are parameterized by theirvalue at each time step. The necessary conditions are expressed as a two-point boundary-value problem.


3.2. Necessary conditions of optimality. Dene an augmented perfor-mance index as

Ja =N1k=0

h

2uk+12 + 1,Tk S1

(logm(Fk RTk Rk+1)

)

+ 2,Tk{k+1 + FTk k + h (Mk+1 + Buk+1)} , (13)

where 1k, 2k R3 are Lagrange multipliers corresponding to the discrete

equations of motion. The augmented performance index is chosen such thatthe dimension of the multipliers is equal to the dimension of the rotation ma-trix and the angular momentum vector. The discrete kinematics equation(11) is transformed into a matrix logarithm form. The constraints aris-ing from the discrete kinematics equation (11) and the angular momentumequation (12) are explicitly applied. Equation (10) appears in the discreteequations of motion because we introduce the auxiliary variable Fk SO(3).The constraint (10) is considered implicitly when taking a variation of theperformance index.

Consider variations from a given trajectory denoted by k, Rk, Fk,and uk:

k = k + k, (14)

Rk = Rk exp S(k) = Rk + RkS(k) +O(2), (15)F k = Fk exp S(k) = Fk + FkS(k) +O(2), (16)

where k, k R3 so(3). The real space R3 is isomorphic to the Liealgebra so(3) according to the mapping S() : R3 so(3). The variations ofthe rotation matrices are expressed using the exponential of the Lie algebraelements. The corresponding innitesimal variations of k, Rk, and Fk aregiven by k, Rk = RkS(k), and Fk = FkS(k), respectively.

The variation of the augmented performance index is obtained from theabove expressions. Instead of taking a variation of the matrix logarithm in(13), we take a variation of the kinematics equation, (11) and we use it asa constrained variation. Since Fk = RTk Rk+1 by (11), the variation Fk isgiven by

Fk = RTk Rk+1 + RTk Rk+1.

Substituting the expression for the innitesimal variation of Rk, we obtain

FkS(k) = S(k)Fk + FkS(k+1).Multiplying both sides of the above equation by FTk and using the propertyS(RT x) = RT S(x)R for all R SO(3) and x R3, we obtain

k = FTk k + k+1. (17)We use (17) as a constrained variation equivalent to (11).


Now we develop another expression for a constrained variation using (10).Since we do not use (10) explicitly as a constraint in (13), k and Fk arenot independent. Taking a variation of (10), we obtain

hS(k) = FkS(k)Jd + JdS(k)FTk .

Using the properties,

S(Rx) = RS(x)RT , S(x)A + AT S(x) = S({tr[A] I33 A}x)for all x R3, A R33, and R SO(3), the above equation can be writtenas

hS(k) = S(Fkk)FkJd + JdFTk S(Fkk)

= S({tr[FkJd] I33 FkJd}Fkk).Thus, k is given by

k = Bkk, (18)where Bk = hFTk {tr[FkJd] I33 FkJd}1 R33. Equation (18) showsthe relationship between k and Fk.

Since the moment due to the potential Mk is dependent on the attitudeof the rigid body, the variation of the moment Mk can be written using avariation of the rotation matrix:

Mk =Mkk, (19)where Mk R33 is expressed in terms of the attitude of the rigid body,and the expression is determined by the potential eld. We present detailedexpressions of Mk in Sec. 4 for a 3D pendulum and for a spacecraft in acircular orbit. Using (17) and (18), Mk+1 is given by

Mk+1 =Mk+1k+1 =Mk+1FTk k +Mk+1Bkk. (20)Now, we take a variation of the augmented performance index (13) using

the constrained variations (17), (18), and (20). Using (17), the variation ofthe performance index is given by

Ja =N1k=0

huTk+1uk+1 + 1,Tk

{k + FTk k k+1

}

+ 2,Tk{k+1 + FTk k + FTk k + hMk+1 + hBuk+1} . (21)

Substituting (20) into (21) and rearranging, we obtain

Ja =N1k=0

huTk+1{uk+1 + BT 2k

} Tk+11k + Tk {Fk1k + hFkMTk+12k}

Tk+12k + Tk{Fk

2k + hBTkMTk+12k

}+ Tk

{S(FTk k)2k + 1k} .(22)


Substituting (18) into (22) and using the fact that the variations k, kvanish at k = 0, N , we obtain

Ja =N1k=1

huTk{uk + BT 2k1

}+ Tk

{1k1 + Fk1k + hFkMTk+12k}

+ Tk{ 2k1 + (Fk BTk S(FTk k) + hBTkMTk+1)2k + BTk 1k

}. (23)

Since Ja = 0 for all variations of uk, k, k which are independent,the expression in the braces are zero. Thus we obtain necessary conditionsfor optimality as follows:

k+1 = FTk k + h (Mk+1 + Buk+1) , (24)

hS(k) = FkJd JdFTk , (25)Rk+1 = RkFk, (26)

uk+1 = BT 2k, (27)[1k2k

]=

[ATk+1 CTk+1BTk+1 DTk+1

] [1k+12k+1

], (28)

where

Ak = FTk , (29)Bk = hFTk {tr[FkJd] I33 FkJd}1 , (30)Ck = hMk+1FTk , (31)Dk = FTk + S(FTk k)Bk + hMk+1Bk. (32)

In the above equations, the only implicit part is (25). For a given initialcondition (R0,0, 10,

20), we can nd F0 by solving (25). Then, R1 is

obtained from (26). Since u1 = BT 20 by (27), and M1 is a function ofR1, 1 can be obtained using (24). We solve (25) to obtain F1 using 1.Finally, 11,

21 are obtained from (28), since A1,B1, C1,D1 are functions of

R1,1, F1. This yields a map (R0,0, 10, 20) (R1,1, 11, 21), and this

process can be repeated.

3.3. Two-point boundary-value problem. The necessary conditions foroptimality are given as a 12 dimensional two-point boundary-value problemon T SO(3) and its dual space. This problem is to nd

1. attitude and angular momentum Rk and k,2. multiplier variables 1k and

2k,

3. control inputs ukfor k = {0, 1, . . . , N}, to satisfy simultaneously,

1. equations of motion (24), (25), (26),2. multiplier equations (28),


3. optimality condition (27),4. boundary conditions R0, 0, RN , N .

An iterative numerical method for the two-point boundary-value problemis presented. A nominal solution that satises some of the above conditionsis chosen, and this nominal solution is updated by successive linearizationso that the remaining conditions are also satised as the process converges.

We use a neighboring extremal method [2]. A nominal solution satis-es all of the necessary conditions except the boundary conditions. Theneighboring extremal method is characterized as an iterative algorithm forimproving estimates of the unspecied multiplier initial conditions so asto satisfy the specied terminal boundary conditions in the limit. This issometimes referred to as a shooting method. The optimality condition (27)is substituted into the equations of motion and the multiplier equations.The sensitivities of the specied terminal boundary conditions with respectto the unspecied initial multiplier conditions can be calculated by directnumerical dierentiation, or they can be obtained by a linear analysis. Themain advantage of the neighboring extremal method is that the number ofiteration variables is small. It is equal to the dimension of the equationsof motion. The diculty is that the extremal solutions are sensitive tosmall changes in the unspecied initial multiplier values. Therefore, it isimportant to compute the sensitivities accurately.

We use linear analysis to compute the sensitivities. The sensitivity modelis dened at the Lie algebra level as presented in [11]. It is natural to denethe sensitivity model in the Lie algebra, since the Lie algebra is a linearvector space. The sensitivity derivatives in the Lie algebra are related tothe original Lie group by the exponential map.

Using the variation models dened in (14), (15), the linearized equationsof motion for the attitude dynamics can be written as

[k+1

k+1

]=

[Ak BkCk Dk

] [k

k

]

[033

hBBT

]2k. (33)

Note that the homogeneous part of (33) is equivalent to equations that arethe dual of (28). The variation of the equations of motion is equivalent to thedual of the multiplier equations, so the variation of the multiplier equationsis equivalent to the second variation of the attitude dynamics equations.Together with (33), a tedious but straightforward application of (14), (15)to the multiplier equation gives the following expression for the sensitivityderivatives of the attitude, angular momentum, and the multipliers

[xk+1k+1

]= Ak

[xkk

], (34)


where xk = [Tk , Tk ]

T , k = [1,Tk ,

2,Tk ]

T R6, and Ak R1212 canbe suitably dened in terms of Rk,k. The solution of (34) is given by

[xNN

]=

(N1k=0

Ak

)[x00

]

[11 1221 22

] [x00

]. (35)

For the given two-point boundary-value problem x0 = 0 since the initialattitude and the initial angular momentum are given, and N is free. Thenwe obtain

xN = 120. (36)

The unspecied initial multipliers are 0, and the specied terminal bound-ary conditions are the terminal attitude RdN and the terminal angular mo-mentum dN . Thus, 12 represents the sensitivity of the specied terminalboundary conditions with respect to the unspecied initial multipliers. Us-ing this sensitivity, an initial guess of the unspecied initial conditions isiterated to satisfy the specied terminal conditions in the limit.

Table 1. NewtonArmijo iteration procedures

1: Guess an initial multiplier 0.2: Find k, Rk, 1k,

2k for k = 1, 2, , N using the initial conditions and

(24)(28).3: Compute the error in satisfaction of the terminal boundary condition;

N = S1(logm(RTNRdN)), N = dN N .

Error = [N ; N ].4: Set Errort = Error, i = 1.5: while Error > S .6: Find a line search direction; D = 112 .7: Set c = 1.8: while Errort > (1 2c)Error9: Choose a trial initial condition t0 = 0 + cD[N ; N ].

10: Find k, Rk, 1k,

2k for k = 1, 2, , N using the trial initial

conditions and (24)(28).11: Compute the error in satisfaction of the terminal boundary

conditiontN = S

1(logm(RTNRdN)), tN = dN N .Errort = [tN ; tN ].

12: Set c = c/2, i = i + 1.13: end while14: Set 0 = t0, Error = Error

t. (accept the trial)15: end while


Any type of Newton iteration can be applied to this problem using thesensitivity derivative as a gradient. In this paper, we use a line search withbacktracking algorithm, referred to as Newton-Armijo iteration in [9]. Theprocedure is summarized in Table 1, where i is the iteration index, andS , R are a stopping criterion and a scaling factor, respectively. Theouter loop nds a search direction by computing the sensitivity derivatives,and the inner loop performs a line search along the obtained direction; theerror in satisfaction of the terminal boundary condition is determined oneach iteration.

4. Numerical Computations

Numerical results are given for two optimal attitude control problems;optimal attitude control of an underactuated 3D pendulum and optimalattitude control of a fully actuated spacecraft on a circular orbit.

4.1. 3D Pendulum. A 3D pendulum is a rigid body supported at a fric-tionless pivot acting under the inuence of uniform gravity [22]. The gravitypotential, acting in the vertical or e3 direction, is given by

U(R) = mgeT3 R, (37)where m R is the mass of the pendulum, g R is the gravitationalacceleration, and R3 represents a vector from the pivot point to the masscenter of the pendulum in the body xed frame. The pendulum model isshown in Fig. 1(a) with the pivot located at the origin, and we assume thatthe pendulum is axially symmetric. The gravity moment and its variationsare given by

M = mgRT e3,M =M = mgS()S(RT e3).

There are two equilibrium manifolds; a hanging equilibrium manifold whenR = e3, an inverted equilibrium manifold when R = e3.

The properties of the axially symmetric pendulum are given by J =diag [0.156, 0.156, 0.3] kgm2, m = 1kg, and =

[0, 0, 34

]m. We assume

that the component of the control input along the axis of symmetry is zero;this corresponds to an underactuated 3D pendulum. The correspondinginput matrix is given by

B =

1 00 10 0

.

Two types of boundary conditions are considered. The rst maneuveris to transfer the 3D pendulum from a hanging equilibrium to an invertedequilibrium. The second maneuver is a 180 degree rotation about the un-controlled axis of symmetry starting in a hanging equilibrium. The terminal


e1e2

e3

(a) 3D Pendulum

E1

E2

E3 x

e1

e2

e3

(b) Spacecraft on a circular orbit

Fig. 1. Rigid body models

Table 2. Optimized performance index and violation ofterminal boundary condition

Case Model Jlogm(Rd,TN RN )

dN N(i) 3D Pendulum 1.52 1.77 10

14 7.08 1015(ii) 40.22 2.22 1016 2.55 1014(iii) Spacecraft 23.35 2.90 10

15 5.13 1015(iv) 70.74 7.31 1015 1.48 1014

attitude also lies in the hanging equilibrium manifold. Each maneuver iscompleted in 1 sec. The time step size is h = 0.001 sec and the number ofintegration steps is N = 1000. The corresponding boundary conditions aregiven by

(i) Rotation from a hanging equilibrium to an inverted equilibrium.

R0 = I33, RdN =

0 1 01 0 00 0 1

,

0 = 031, dN = 031.

(ii) Rotation from one hanging equilibrium to another hanging equilib-rium.

R0 = I33, RdN = diag[1,1, 1],0 = 031, dN = 031.

The optimized performance index and the violation of the terminalboundary condition are given in Table 2. The terminal boundary condi-tions are satised at the level of machine precision for both cases. Figures 2


and 3 show snapshots of the attitude maneuvers, the control input his-tory, and the angular velocity response. (Simple animations which showthese optimal attitude maneuvers of the 3D pendulum can be found athttp://www.umich.edu/ tylee.) The third component of the angular ve-locity is constant; this is a conservation property of the controlled axiallysymmetric 3D pendulum.

The optimized attitude maneuver of the rst case is an eigen-axis rotationabout the xed axis; [

2

2 ,

22 , 0]. In the second case, the rotation about the

axis of symmetry is induced from control moments about the rst and secondbody xed axes. The resulting attitude maneuver is more complicated, andit requires larger control inputs.

Figures 2(d) and 3(d) show the violation of the terminal boundary condi-tion according to the number of iterations in a logarithm scale. The circlesdenote outer iterations to compute the sensitivity derivatives. The inneriterations correspond to backtracking to decrease the step length along thesearch direction. For all cases, the initial guesses of the unspecied initialmultiplier are arbitrarily chosen such that the initial trial of control inputs isclose to zero throughout the maneuver time. The error in satisfaction of theterminal boundary condition of the rst case converges quickly to machineprecision; only 7 iterations are required. A longer number of iterations isrequired in the second case, but the error converges exponentially to ma-chine precision after the solution is close to the local minimum at the 55thiteration. These convergence rates are consistent with the quadratic rate ofconvergence of the Newton iteration, and demonstrate that the sensitivityderivatives are being computed accurately.

4.2. Spacecraft on a Circular Orbit. We consider a spacecraft on a cir-cular orbit about a large central body, including gravity gradient eects [25].The spacecraft model is shown at Fig. 1(b). The attitude of the spacecraftis represented with respect to the local vertical local horizontal (LVLH)axes. The gravity potential is given by

U(R) = GMr0

1220

(tr[J ] 3eT3 RJRT e3

),

where G R is the gravitational constant, M R is the mass of the centralbody, r0 R is the orbital radius, and 0 =

GMr30

R is the orbitalangular velocity. The gravity moment and its variations are given by

M = 320RT e3 JRT e3,

M =M = 320[ S(JRT e3)S(RT e3) + S(RT e3)JS(RT e3)

].

There are 24 distinct relative equilibria for which the principal axes areexactly aligned with the LVLH axes, and the spacecraft angular velocity isidentical to the orbital angular velocity of the LVLH coordinate frame.


We assume that the spacecraft is fully actuated. The corresponding inputmatrix is B = I33. The mass, length and time dimensions are normalizedby the mass of the spacecraft, a size scale factor of the spacecraft, and theorbital angular velocity 0 R, respectively. The mass property of thespacecraft is chosen as J = diag [1, 2.8, 2].

Two boundary conditions are considered. Each maneuver is a large atti-tude change completed in a quarter of the orbit, Tf = 2 . The time step sizeis h = 0.001 and the number of integration steps is N = 1571. The terminalangular momentum is chosen such that the terminal attitude is maintainedafter the maneuver.(iii) Rotation maneuver about the LVLH axis e1:

R0 = I33, RdN = diag [1,1,1] ,0 = 0JRT0 e2,

dN = 0JR

d,TN e2.

(iv) Rotation maneuver about the LVLH axes e1 and e2:

R0 = diag [1,1,1] , RdN =1 0 00 0 1

0 1 0

,

0 = 0JRT0 e2, dN = 0JR

d,TN e2.

The optimized performance index and the violation of the constraints aregiven in Table 2. Figures 4 and 5 show the attitude maneuver of the space-craft (clockwise direction), the control inputs, the angular velocity response,and the violation of the terminal boundary condition according to the num-ber of iterations.

4.3. Numerical properties. One of the distinct features of the Lie groupvariational integrator is that it preserves the symplectic structure and theLie group structure of the rigid body dynamics concurrently. The compu-tational properties of the Lie group variational integrator have been com-pared with a symplectic integrator that does not preserve the Lie groupstructure, and a Lie group method that does not preserve the symplecticstructure [16]. These numerical experiments demonstrate that it is crit-ical for long-time accuracy to preserve both structures as the rigid bodydynamics is a Lagrangian/Hamiltonian system evolving on a Lie group.

Therefore, the discrete dynamics are more faithful to the continuousequations of motion, and consequently more accurate solutions to the op-timal control problem are obtained. The external control inputs break theLagrangian and Hamiltonian system structure. But, the computational su-periority of the discrete mechanics still holds for controlled systems. It hasbeen shown that the discrete dynamics is more reliable even for controlledsystem as it computes the energy dissipation rate of controlled systemsmore accurately [21]. For example, this feature is extremely important in


computing accurate optimal trajectories for long term spacecraft attitudemaneuvers using low energy control inputs.

The discrete dynamics does not only provide an accurate optimal controlinput, but also enables us to nd it eciently. The neighboring extremalmethod or the shooting method are numerically ecient in the sense thatthe number of optimization parameters is minimized. But, this approachmay be prone to numerical ill-conditioning [1]. A small change in the ini-tial multiplier can cause highly nonlinear behavior of the terminal attitudeand angular momentum. It is dicult to compute the Jacobian matrix forNewton iterations accurately, and consequently, the numerical error maynot converge to zero within machine precision.

However, the numerical examples presented in this section show excellentnumerical convergence properties. They exhibit a quadratic rate of conver-gence. This is because the proposed computational algorithms on SO(3)are geometrically exact and numerically accurate. The attitude dynamicsof a rigid body arises from Hamiltonian mechanics, which have neutral sta-bility. The adjoint system is also neutrally stable. The proposed Lie groupvariational integrators and the discrete multiplier equations, obtained fromvariations expressed in the Lie algebra, can preserve the neutrally stabilityproperty. Therefore the sensitivity derivatives are computed accurately.

5. Conclusions

A discrete optimal control problem for the attitude dynamics of a rigidbody in the presence of an attitude dependent potential is studied. Theperformance index is the l2 norm of external control inputs and bound-ary conditions on the attitude and the angular momentum are prescribed.The attitude is represented by a rotation matrix in the Lie group, SO(3).This paper proposes three levels of geometrically exact computations onSO(3) to solve the optimal control problem; Lie group variational inte-grator, discrete-time necessary conditions for optimality, and discrete-timesensitivity derivatives.

The Lie group variational integrator obtained from a discrete variationalprinciple preserves the geometric features of the attitude dynamics of therigid body. It exhibits symplectic and momentum preservation properties,and good energy behavior characteristic of variational integrators. Since therotation matrices are updated by a group operation, the Lie group structureis also preserved.

The necessary conditions of optimality are derived by a variational prin-ciple. The Lie group variational integrators are imposed as constraints, andthe variation of the rotation matrices are expressed in terms of Lie alge-bra elements. The proposed discrete optimality conditions are the basis fora numerically ecient computational algorithms for the optimal attitudecontrol problem, since the implicit part of the optimality conditions occurs


in a single equation of one variable. This implicit condition can be solvedeasily by Newton iteration. Other algorithms require iteration on the entirediscrete time trajectory simultaneously.

The necessary conditions are expressed as a two-point boundary-valueproblem on TSO(3) and its dual space. The sensitivity derivatives aredeveloped in the Lie algebra, and the two-point boundary-value problemis solved using a neighboring extremal method. The neighboring extremalmethod is ecient for this class of optimal control problems because theresulting problem of satisfying the terminal boundary conditions has a smallnumber of variables. The main disadvantage is that a small change in theinitial multipliers can produce a very large change in the terminal condition.This can result in numerical ill-conditioning. The nonlinearity also makes ithard to construct an accurate estimate of the Jacobian matrix that is neededfor a Newton iteration. We address this issue in the paper by accuratelycomputing the sensitivity derivatives in the Lie algebra of SO(3). Thisyields an ecient method for solving the two-point boundary problem, andthe error in the terminal boundary conditions converges exponentially tomachine precision.

Numerical results for an optimal attitude control problem involving anunderactuated axially symmetric 3D pendulum and for an optimal attitudecontrol problem involving a fully actuated spacecraft on a circular orbitare given. The boundary conditions are chosen such that the resulting ma-neuvers are large angle attitude maneuver. It is shown that the proposednumerical computations on SO(3) are geometrically exact and highly e-cient.

In this paper, the performance index is chosen as a smooth function ofthe control input. But, the proposed discrete-time optimal control approachusing the Lie group variational integrator can be generalized in several ways,and actually, it has been applied to various optimal control problems forrigid bodies. For example, an optimal control problem on SE(3) is studiedin [14], and this is extended to incorporate a combinatorial optimal controlin [13]. Constraints on the control input are considered in [12, 17]. Thefuture work includes an optimal control problem for a multibody system.

References

1. J. T. Betts, Practical methods for optimal control using nonlinear pro-gramming. SIAM (2001).

2. A. E. Bryson and Yu-Chi Ho, Applied optimal control. HemispherePublishing Corporation (1975).

3. E. Hairer, C. Lubich, and G. Wanner, Geometric numerical integrationillustrated by the StormerVerlet method. Acta Numer. 12 (2003), 399450.


(a) Attitude maneuver (b) Control input u

(c) Angular velocity (d) Convergence rate

Fig. 2. Case (i): 3D pendulum rotation from a hangingequilibrium to an inverted equilibrium

4. I. I. Hussein and A. M. Bloch, Optimal control on Riemannian manifoldswith potential elds. In: Proc. IEEE Conf. on Decision and Control(2004), 19821987.

5. I. I. Hussein, M. Leok, A. K. Sanyal, and A. M. Bloch, A discrete vari-ational integrator for optimal control problems on SO(3). Proc. IEEEConf. on Decision and Control (2006), 66366641.

6. A. Iserles, H. Z. Munthe-Kaas, S. P. Nrsett, and A. Zanna, Lie-groupmethods. Acta Numer. 9 (2000), 215365.




Fig. 3. Case (ii): 3D pendulum rotation from one hangingequilibrium to another hanging equilibrium

7. O. Junge, J. E. Marsden, and S. Ober-Blobaum, Discrete mechanicsand optimal control. IFAC Congr., Praha (2005).

8. V. Jurdjevic, Geometric control theory. Cambridge Univ. Press (1997).9. C. T. Kelley, Iterative methods for linear and nonlinear equations. SIAM

(1995).10. T. Lee, M. Leok, and N. H. McClamroch, A Lie group variational in-

tegrator for the attitude dynamics of a rigid body with applications to




Fig. 4. Case (iii): Spacecraft rotation maneuver about theLVLH tangential axis

the 3D pendulum. Proc. IEEE Conf. on Control Applications (2005),962967.

11. , Attitude maneuvers of a rigid spacecraft in a circular orbit.Proc. Amer. Control Conf. (2005), 17421747.

12. , Optimal attitude control for a rigid body withsymmetry. Proc. Amer. Control Conf. (2007), 10731078.http://arxiv.org/abs/math.OC/06009482.




Fig. 5. Case (iv): Spacecraft rotation maneuver about theLVLH tangential and normal axes

13. , A combinatorial optimal control problem for spacecraft forma-tion reconguration. Proc. IEEE Conf. on Decision and Control (2007).http://arxiv.org/abs/math.OC/0702738.

14. , Optimal control of a rigid body using geometrically exact com-putations on SE(3). Proc. IEEE Conf. on Decision and Control (2006),21702175. http://arxiv.org/abs/math.OC/0602588.

15. , Lie group variational integrators for the full body problem.Comput. Methods Appl. Mech. Engineering 196 (2007), 29072924.


16. , Lie group variational integrators for the full body problem inorbital mechanics. Celest. Mech. Dynam. Astronomy 98 (2007), No. 2,121144.

17. , Time optimal attitude control for a rigid body. Proc. Amer.Control Conf. (2008).

18. M. Leok, Generalized Galerkin variational integrators. Preprint (2004),http://arxiv.org/math.NA/0508360.

19. , Foundations of computational geometric mechanics. CaliforniaInst. of Technology (2004).

20. T. R. Littell, R. D. Skeel, and M. Zhang, Error analysis of symplecticmultiple time stepping. SIAM J. Numer. Anal. 34 (1997), No. 5, 17921807.

21. J. E. Marsden and M. West, Discrete mechanics and variational inte-grators. Acta Numer. 10 (2001), 357514.

22. J. Shen, A. K. Sanyal, N. A. Chaturvedi, D. Bernstein, and N. H. Mc-Clamroch, Dynamics and control of a 3D pendulum. Proc. 43rd IEEEConf. on Decision and Control (2004), 323328.

23. K. Spindler, Optimal control on Lie groups with applications to attitudecontrol. Math. Control Signals Systems 11 (1998), 197219.

24. M. Suzuki, Improved Trotter-like formula. Phys. Lett. A 180 (1993),No. 3, 232234.

25. B. Wie, Space vehicle dynamics and control. AIAA (1998).

(Received June 30 2007, received in revised form October 04 2007)

Authors addresses:T. LeeDepartment of Aerospace Engineering, University of Michigan,2008 FXB, 1320 Beal Ave., Ann Arbor MI, 48109, USAE-mail: [email protected]

M. LeokDepartment of Mathematics, Purdue University,150 North University Street, West Lafayette, IN 47907, USAE-mail: [email protected]

N. H. McClamrochDepartment of Aerospace Engineering, University of Michigan,2008 FXB, 1320 Beal Ave., Ann Arbor MI, 48109, USAE-mail: [email protected]

/ColorImageDict > /JPEG2000ColorACSImageDict > /JPEG2000ColorImageDict > /AntiAliasGrayImages false /CropGrayImages true /GrayImageMinResolution 150 /GrayImageMinResolutionPolicy /OK /DownsampleGrayImages true /GrayImageDownsampleType /Bicubic /GrayImageResolution 150 /GrayImageDepth -1 /GrayImageMinDownsampleDepth 2 /GrayImageDownsampleThreshold 1.50000 /EncodeGrayImages true /GrayImageFilter /DCTEncode /AutoFilterGrayImages true /GrayImageAutoFilterStrategy /JPEG /GrayACSImageDict > /GrayImageDict > /JPEG2000GrayACSImageDict > /JPEG2000GrayImageDict > /AntiAliasMonoImages false /CropMonoImages true /MonoImageMinResolution 1200 /MonoImageMinResolutionPolicy /OK /DownsampleMonoImages true /MonoImageDownsampleType /Bicubic /MonoImageResolution 600 /MonoImageDepth -1 /MonoImageDownsampleThreshold 1.50000 /EncodeMonoImages true /MonoImageFilter /CCITTFaxEncode /MonoImageDict > /AllowPSXObjects false /CheckCompliance [ /None ] /PDFX1aCheck false /PDFX3Check false /PDFXCompliantPDFOnly false /PDFXNoTrimBoxError true /PDFXTrimBoxToMediaBoxOffset [ 0.00000 0.00000 0.00000 0.00000 ] /PDFXSetBleedBoxToMediaBox true /PDFXBleedBoxToTrimBoxOffset [ 0.00000 0.00000 0.00000 0.00000 ] /PDFXOutputIntentProfile (None) /PDFXOutputConditionIdentifier () /PDFXOutputCondition () /PDFXRegistryName (http://www.color.org?) /PDFXTrapped /False

/SyntheticBoldness 1.000000 /Description >>> setdistillerparams> setpagedevice

Optimal Attitude Control of a Rigid Body Using Geometrically Exact Computations on So(3)

Documents

Transcript of Optimal Attitude Control of a Rigid Body Using Geometrically Exact Computations on So(3)