Geometric structure-preserving optimal control of a rigid body

GEOMETRIC STRUCTURE-PRESERVING OPTIMAL CONTROL OF THE RIGID BODY ANTHONY M. BLOCH, ISLAM I. HUSSEIN, MELVIN LEOK, AND AMIT K. SANYAL Abstract. In this paper we study a discrete variational optimal control problem for the rigid body. The cost to be minimized is the external torque applied to move the rigid body from an initial condition to a pre-specified terminal condition. Instead of discretizing the equations of motion, we use the discrete equations obtained from the discrete Lagrange–d’Alembert principle, a process that better approximates the equations of motion. Within the discrete-time setting, these two approaches are not equivalent in general. The kinematics are discretized using a natural Lie-algebraic formulation that guarantees that the flow remains on the Lie group SO(3) and its algebra so(3). We use Lagrange’s method for constrained problems in the calculus of variations to derive the discrete-time necessary conditions. We give a numerical example for a three-dimensional rigid body maneuver. 1. Introduction This paper deals with a structure-preserving computational approach to the optimal control problem of minimizing the control effort necessary to perform an attitude transfer from an initial state to a prescribed final state, in the absence of a potential field. The configuration of the rigid body is given by the rotation matrix from the body frame to the spatial frame, which is an element of the group of orientation-preserving isometries in R3 . The state of the rigid body is described by the rotation matrix and its angular velocity. To motivate the computational approach we adopt in the discrete-time case, we first revisit the variational continuous-time optimal control problem. The continuoustime extremal solutions to this optimal control problem have certain special features, since they arise from variational principles. General numerical integration methods, including the popular Runge-Kutta schemes, typically preserve neither first integrals nor the characteristics of the configuration space. Geometric integrators are the class of numerical integration schemes that preserve such properties, and a good survey can be found in [3]. Techniques particular to Hamiltonian systems are also discussed in [10] and [18]. Our approach to discretizing the optimal control problem is in contrast to traditional techniques such as collocation, wherein the continuous equations of motion are imposed as constraints at a set of collocation points. In our approach, modeled after [7], the discrete equations of motion are derived from a discrete variational principle, and this induces constraints on the configuration at each discrete time step. This approach yields discrete dynamics that are more faithful to the continuous equations of motion, and consequently yields more accurate solutions to the optimal Date: Received: date / Revised: date. 1 2 ANTHONY M. BLOCH, ISLAM I. HUSSEIN, MELVIN LEOK, AND AMIT K. SANYAL control problem that is being approximated. This feature is extremely important in computing accurate (sub)optimal trajectories for long-term spacecraft attitude maneuvers. For example, in [5], the authors propose an imaging spacecraft formation design that requires a continuous attitude maneuver over a period of 77 days in a low Earth orbit. Hence, the attitude maneuver has to be very accurate to meet tight imaging constraints over long time ranges. While the discrete optimal control method presented here is illustrated using the Lie group SO(3) of rotation matrices, and its corresponding Lie algebra so(3) of skew-symmetric matrices, we have derived the method with sufficient generality to address the problem of optimal control on arbitrary Lie groups with the drift vector field given by geodesic flow on the group, and it therefore widely applicable. For example, in inter-planetary orbit transfers, one is interested in computing optimal or suboptimal trajectories on the group of rigid body motions SE(3) with a high degree of accuracy. Similar requirements also apply to the control of quantum systems. For example, efficient construction of quantum gates is a problem on the unitary Lie group SU(N ). This is an optimal control problem, where one wishes to steer the identity operator to the desired unitary operator (see, for example, [9] and [17]). Moreover, an important feature of the way we discretize the optimal control problem is that it is SO(3)-equivariant. The SO(3)-equivariance of our numerical method is desirable, since it ensures that our results do not depend on the choice of coordinates and coordinate frames. This is in contrast to methods based on coordinatizing the rotation group using quaternions, (modified) Rodrigues parameters, and Euler angles, as given in the survey [20]. Even if the optimal cost function is SO(3)-invariant, as in [19], the use of generalized coordinates imposes constraints on the attitude kinematics. For the purpose of numerical simulation, the corresponding discrete optimal control problem is posed on the discrete state space as a two stage discrete variational problem. In the first step, we derive the discrete dynamics for the rigid body in the context of discrete variational mechanics [14]. This is achieved by considering the discrete Lagrange–d’Alembert variational principle [8] in combination with essential ideas from Lie group methods [6], which yields a Lie group variational integrator [?] that is a symplectic-momentum integrator that explicitly preserves the Lie group structure of the configuration space. These discrete equations are then imposed as constraints to be satisfied by the extremal solutions to the discrete optimal control problem, and we obtain the discrete extremal solutions in terms of the given terminal states. The paper is organized as follows. As motivation, in Section 2, we study the minimum control effort optimal control problem in continuous-time. In Section 3, we study the corresponding discrete-time optimal control problem. In Section 3.1 we state the optimal control problem and describe our approach. In Section 3.2, we derive the discrete-time equations of motion for the rigid body starting with the discrete Lagrange–d’Alembert principle. These equations are used in Section 3.3 to obtain the solution to the discrete optimal control problem. In Section 4, we describe an algorithm for solving the general nonlinear, implicit necessary conditions for SO(3) and give numerical examples for rest-to-rest and slew-up spacecraft maneuvers. GEOMETRIC STRUCTURE-PRESERVING OPTIMAL CONTROL OF THE RIGID BODY 3 2. Continuous-Time Results 2.1. Problem Formulation. In this paper, the natural pairing between so∗ (3) and so(3) is denoted by h·, ·i. Let  ·, ·  and  ·, · ∗ denote the standard (induced by the Killing form) inner product on so(3) and so∗ (3), respectively. The inner product  ·, · ∗ is naturally induced from the standard norm  ξ, ω = − 12 Tr(ξ T ω), for all ξ, ω ∈ so(3), through D E  η, ϕ ∗ = η, ϕ] = hη, ωi = ξ [ , ω (1) = ξ, ω , where ϕ = ω [ ∈ so∗ (3) and η = ξ [ ∈ so∗ (3), with ξ, ω ∈ so(3) and [ and ] are the musical isomorphisms with respect to the standard metric  ·, · . On so(3), these isomorphisms correspond to the transpose operation. That is, we have ϕ = ω T and η = ξT . Let J : so(3) → so∗ (3) be the positive definite inertia operator. It can be shown that (2) hJ(ξ), ωi = hJ(ω), ξi . On so(3), J is given by J(ξ) = Jξ + ξJ, where J is a positive definite symmetric matrix (see, for example, [15]). Moreover, we also have J(η ] )] = (Jη T + η T J)T = J(η), which is an abuse of notation since η ∈ so∗ (3). For the sake of generality and mathematical precision we will use the general definitions, though it helps to keep the above identifications for so(3) in mind. In this section we review some continuous-time optimal control results using a simple optimal control example on SO(3). The problem we consider is that of minimizing the norm squared of the control torque τ ∈ so∗ (3) applied to rotate a rigid body subject to the Lagrange–d’Alembert principle for the rigid body1 whose configuration is given by R ∈ SO(3) and body angular velocity is given by Ω ∈ so(3). We require that the system evolve from an initial state (R0 , Ω0 ) to a final state (RT , ΩT ) at a fixed terminal time T . Before proceeding with a statement of the optimal control problem, we first define variations of the rigid body configuration R and its velocity Ω. Given a curve R(t) on SO(3), variations of the curve are given by R (t) := R(t, ) that satisfies R(t, 0) = R(t). Let W(t) ∈ so(3) be the variation vector field [1] given by W(t) = (R(t)) −1 δR(t), where δR(t) = ∂R (t)/∂ =0 ∈ TR(t) SO(3). Since we will be concerned with variations that keep the endpoints fixed, we have the property that W(0) = 0, W(T ) = 0. The variation in the velocity vector field is denoted by δΩ. We now state the minimum control effort optimal control problem. Problem 2.1. Minimize (3) J = 1 2 Z T  τ , τ ∗ dt 0 1This is equivalent to constraining the problem to satisfy the rigid body equations of motion given by equations (7). However, for the sake of generality that will be appreciated in the discretetime problem, we choose to treat the Lagrange–d’Alembert principle as the constraint as opposed to the rigid body equations of motion. Both are equivalent in the continuous-time case but are generally not equivalent in the discrete-time case. 4 ANTHONY M. BLOCH, ISLAM I. HUSSEIN, MELVIN LEOK, AND AMIT K. SANYAL subject to (1) satisfying Lagrange–d’Alembert principle: Z T Z T 1 δ (4) hJ (Ω) , Ωi dt + hτ , Wi dt = 0, 0 2 0 for a variation vector field W(t), and subject to Ṙ = RΩ, (2) and the boundary conditions R(0) = R0 , Ω(0) = Ω0 , (5) R(T ) = RT , Ω(T ) = ΩT . We now show that the constraint of satisfying the Lagrange–d’Alembert principle leads to the following problem formulation, where the rigid body equations of motion replace the Lagrange–d’Alembert principle. Problem 2.2. Minimize J = (6) 1 2 Z T  τ , τ ∗ dt 0 subject to (1) the kinematics and dynamics (7) Ṙ = RΩ Ṁ = ad∗Ω M + τ = [M, Ω] + τ , where M = J(Ω) ∈ so∗ (3) is the momentum, (2) and the boundary conditions R(0) = R0 , Ω(0) = Ω0 , (8) R(T ) = RT , Ω(T ) = ΩT . ∗ In the above, ad is the dual of the adjoint representation, ad, of so(3) and is given by ad∗ξ η = −[ξ, η] ∈ so∗ (3), for all ξ ∈ so(3) and η ∈ so∗ (3). Recall that the bracket is defined by [ξ, ω] = ξω − ωξ. 2.2. The Lagrange–d’Alembert Principle and the Rigid Body Equations of Motion. In this section we derive the forced rigid body equations of motion (equations (7)) from the Lagrange–d’Alembert principle, using a direct derivation based on Section 13.5 of [15]. First, we take variations the kinematic condition Ω = R−1 Ṙ to obtain δΩ =  of  −1 −1 −1 δ Ṙ . As defined previously, we have W = R−1 δR and, −R (δR) R Ṙ + R therefore, Ẇ = −R−1 ṘR−1 δR + R−1 δ Ṙ = −ΩW + R−1 δ Ṙ, since δ Ṙ = Hence, we have δΩ = −WΩ + ΩW + Ẇ = adΩ W + Ẇ. (9) Taking variations of the Lagrange–d’Alembert principle we obtain Z T hJ (Ω) , δΩi + hτ , Wi dt = 0. 0 Using the variation in equation (9) and integrating by parts, we obtain Z TD E T 0= −Ṁ + ad∗Ω M + τ , W dt + [hJ (Ω) , W(t)i]0 , 0 d dt δR. GEOMETRIC STRUCTURE-PRESERVING OPTIMAL CONTROL OF THE RIGID BODY 5 where M = J (Ω) and we used the identity (10) hη, adω ξi = had∗ω η, ξi , η ∈ so∗ (3), ω, ξ ∈ so(3). This completes the proof that the Problem (2.1) is equivalent to Problem (2.2). In Section 2.3, we demonstrate how the necessary conditions for Problem (2.2) are derived using a variational approach. 2.3. Continuous-Time Variational Optimal Control Problem. A direct variational approach is used here to obtain the differential equation that satisfies the optimal control Problem (2.2). A Second Order Direct Approach. “Second order” is used here to reflect the fact that we now study variations of second order dynamical equations as opposed to the kinematic direct approach studied in Section 2.2. We now give the resulting necessary conditions using a direct approach as in [15]. We already computed the variations of R and Ω. These were as follows: δR = RW and δΩ = adΩ W + Ẇ. We now compute the variation of Ṁ with the goal of obtaining the proper variations for τ :     d δΩ + R (W, Ω) Ω , δ Ṁ = J δ Ω̇ = J dt where R is the curvature tensor on SO(3). The curvature tensor R arises due to the identity (see [16], page 52) ∂ ∂ ∂ ∂ Y− Y = R(W, Y)Ω, ∂ ∂t ∂t ∂ where Y ∈ TSO(3) is any vector field along the curve R(t) ∈ SO(3). Taking variations of Ṁ = ad∗Ω M + τ , we obtain δ Ṁ = ad∗δΩ M + ad∗Ω δM + δτ . We now have the desired variation in τ : d (11) δτ = J (R (W, Ω) Ω) + J (δΩ) − ad∗δΩ M − ad∗Ω δM. dt Taking variations of the cost functional (6) we obtain: Z T
d J(¨ ς ) − ad∗Ω (J(ς)) ˙ + η̇ − δJ = ad∗ς M dt 0  
[ + R J(ς)] , Ω Ω + ad∗Ω ad∗ς M − ad∗Ω η, W dt, where ς = τ ] ∈ so(3) and η = J (adΩ ς) ∈ so∗ (3). In obtaining the above expression, we have used integration by parts and the boundary conditions (8), equations (9) and (11), and the identities (1), (2) and (10). Hence, we have the following theorem. Theorem 2.1. The necessary optimality conditions for the problem of minimizing (6) subject to the dynamics (7) and the boundary conditions (8) are given by the single fourth order2 differential equation 0 = J(¨ ς ) − ad∗Ω (J(ς)) ˙ + η̇ −
d ad∗ς M dt h   i[
] + R (J(ς)) , Ω Ω + ad∗Ω ad∗ς M − ad∗Ω η, 2Second order in τ and fourth order in R. 6 ANTHONY M. BLOCH, ISLAM I. HUSSEIN, MELVIN LEOK, AND AMIT K. SANYAL as well as the equations (7) and the boundary conditions (8), where ς and η are as defined above. Note that for a compact semi-simple Lie group G with Lie algebra g, the curvature tensor, with respect to a bi-invariant metric, is given by (see [16]): (12) R (X, Y) Z = 1 adadX Y Z, 4 for all X, Y, Z ∈ g. Remark 2.1. Note that the equations of motion that arise from the Lagrange– d’Alembert principle are used to define the dynamic constraints. So, in effect, we are minimizing J subject to satisfying the Lagrange–d’Alembert principle for the rigid body. Analogously, the discrete version of the Lagrange–d’Alembert principle will be used to derive the discrete equations of motion in the discrete optimal control problem to be studied in Section 3.3. This view is in line with the approach in [7] in that we do not discretize the equations of motion directly, but, instead, we discretize the Lagrange–d’Alembert principle. These two approaches are not equivalent in general. 3. Discrete-Time Results 3.1. Problem Formulation. In this section we give the discrete version of the problem introduced in Section 2.1. So, we consider minimizing the norm squared of the control torque τ subject to satisfaction of the discrete Lagrange–d’Alembert principle for the rigid body whose configuration and body angular velocity at time step tk are given by Rk ∈ SO(3) and Ωk ∈ so(3), respectively. The kinematic constraint may be expressed as (13) Rk+1 = Rk exp (hΩk ) = Rk gk , where h is the integration time step, exp : so(3) → SO(3) is the exponential map and gk = exp(hΩk ). The boundary conditions are given by (R∗0 , Ω∗0 ) and (R∗N , Ω∗N −1 ), where t0 = 0 and N = T /h is such that tN = T . More generally, one considers the ansatz Rk+1 = Rk exp (Ω(h)), where Ω(·) is an interpolatory curve in so(3) parameterized by the angular velocity at internal nodal points. This allows one to construct Lie group variational integrators of arbitrarily high order [11]. To simplify the subsequent treatment, we adopt (13) as the kinematic constraint, which yields a first-order accurate Lie symplectic Euler method, which will nevertheless have effective order two as it is symplectically conjugate to the second-order accurate Lie Störmer–Verlet method (see, §3.4). The reason we constrain Ω at t = h(N −1) instead of at t = hN will become clear when we derive the discrete equations of motion in Section 3.2. A simple explanation for this is that a constraint on Ωk ∈ so(3) corresponds, by left translations to a constraint on Ṙk ∈ TRk SO(3). In turn, in the discrete setting and depending on the choice of discretization, this corresponds to a constraint on the neighboring discrete points . . . , Rk−2 , Rk−1 , Rk+1 , Rk+2 , . . .. With our choice of discretization (equation (13)), this corresponds to constraints on Rk and Rk+1 . Hence, to ensure that the effect of the terminal constraint on Ω is correctly accounted for, the constraint must be imposed on ΩN −1 , which entails some constraints on variations at both RN −1 and RN . We will return to this point later in the paper. GEOMETRIC STRUCTURE-PRESERVING OPTIMAL CONTROL OF THE RIGID BODY 7 The discrete kinematic constraint ensures that the sequence Rk stays on the rotation group, since the exponential of the angular velocity matrix Ωk , which is in the algebra so(3), is a rotation matrix, and the rotation group is closed under matrix multiplication. This is natural to do in the context of discrete variational numerical solvers (for both initial value and two point boundary value problems). Following the methodology of [7], we have the following optimal control problem. Problem 3.1. Minimize J = (14) N X 1 k=0 2  τ k , τ k ∗ subject to (1) satisfying the discrete Lagrange–d’Alembert principle: (15) δ N −1 X k=0 N X 1 hJ (Ωk ) , Ωk i + hτ k , Wk i = 0, 2 k=0 R∗0 , R∗N and Rk+1 = Rk gk , k = 0, 1, . . . , N − 1, subject to R0 = RN = where Wk is the variation vector field at time step tk satisfying δRk = Rk Wk , (2) and the boundary conditions R0 = R∗0 , Ω0 = Ω∗0 , RN = R∗N , ΩN −1 = Ω∗N −1 . (16) In Problem 3.1, the discrete Lagrange–d’Alembert principle is used to derive the equations of motion for the rigid body with initial and terminal configuration constraints. Hence, we get a two point boundary value problem. The full configuration and velocity boundary conditions come into the picture when we study the optimal control problem. We will show that the constraint of satisfying the Lagrange– d’Alembert principle in Problem 3.1 leads to the following problem formulation, where the discrete rigid body equations of motion replace the Lagrange–d’Alembert principle constraint. Only when addressing the following optimal control problem will we need to include the velocity boundary conditions in the derivation. Problem 3.2. Minimize (17) J = N X 1 k=0 2  τ k , τ k ∗ subject to (1) the discrete kinematics and dynamics Rk+1 = Rk gk , k = 0, . . . , N − 1 Mk = Ad∗gk (hτ k + Mk−1 ) , k = 1, . . . , N − 1, (18) Mk = J (Ωk ) , k = 0, . . . , N − 1, (2) and the boundary conditions R0 = R∗0 , Ω0 = Ω∗0 , (19) RN = R∗N , ΩN −1 = Ω∗N −1 . 8 ANTHONY M. BLOCH, ISLAM I. HUSSEIN, MELVIN LEOK, AND AMIT K. SANYAL Regarding terminal velocity conditions, note that in the second of equations (18) if we let k = N we find that ΩN appears in the equation. A constraint on ΩN dictates constraints at the points RN and RN +1 through the first equation in (18). Since we only consider time points up to t = N h, we can not allow k = N in the second of equations (18) and hence our terminal velocity constraints are posed in terms of ΩN −1 instead of ΩN . As mentioned above, Wk is a variation vector field associated with the perturbed group element Rk . Likewise, we need to define a variation vector field associated with the element gk = exp(hΩk ). First, let the perturbed variable gk be defined by gk = gk exp(hδΩk ), (20) where δΩk = Note that gk (21) =0 ∂Ωk ∂ . =0 = gk as desired. Moreover, we have δgk = gk (hδΩk ) exp(hδΩk ) =0 = hgk δΩk . This will be needed later when taking variations. 3.2. The Discrete Lagrange–d’Alembert Principle and the Rigid Body Equations of Motion. In this section we derive the discrete forced rigid body equations of motion (18) from the discrete Lagrange–d’Alembert principle. We begin by computing the constrained variation associated with the kinematic constraint (13). Taking variations of the kinematic constraint, we obtain −1 −1 −R−1 k (δRk ) Rk Rk+1 + Rk δRk+1 = hgk · δΩk , which is equivalent to −Wk gk + gk Wk+1 = hgk δΩk , or i 1h δΩk = (22) −Adg−1 Wk + Wk+1 . k h Note that this is an expression over the Lie algebra so(3). After simple algebraic and re-indexing operations, the Lagrange–d’Alembert principle gives     1 ∗ 1 0 = τ 0 − Adg−1 J (Ω0 ) , W0 + τ N + J (ΩN −1 ) , WN 0 h h   N −1 X 1 1 + τ k − Ad∗g−1 J (Ωk ) + J (Ωk−1 ) , Wk . k h h k=1 where we have used equation (22). By the boundary conditions R0 = R∗0 and RN = R∗N , we have W0 = 0 and WN = 0. Since δΩk , k = 0, . . . , N − 1, and Wk , k = 1, . . . , N − 1, are arbitrary and independent, then the Lagrange– d’Alembert principle requires that the equations (18) hold true. The variables Mk , k = 0, . . . , N − 1, are of course nothing but the discrete angular momentum of the rigid body. The equations (18) can be viewed in two ways. The first is to consider the two point boundary value problem where we retain the terminal condition on RN . In this case a (constrained) variety of a combination of control torques τ k , k = 0, . . . , N , and initial velocity conditions Ω0 can be chosen to drive the rigid body GEOMETRIC STRUCTURE-PRESERVING OPTIMAL CONTROL OF THE RIGID BODY 9 from the initial condition R0 to the terminal condition RN . The second view is to treat it as an initial value problem by ignoring any terminal configuration constraints. In this case WN 6= 0 and any combination of control torques τ k , k = 0, . . . , N , and initial velocity conditions Ω0 can be chosen freely. Simulation Results. To test our results, we re-write the discrete equations (18) for the subgroup SO(2). For SO(2) we have     cos θk − sin θk 0 −ωk Rk = , Ωk = (23) sin θk cos θk ωk 0 and  (24) exp (Ωk ) = cos ωk sin ωk The inertia operation is simply given by  0 J (Ωk ) = (25) Iωk − sin ωk cos ωk −Iωk 0  .  , where I is the mass moment of inertia of the body about the out-of-plane axis. One can check that Adexp(ω) ξ = ξ and that Ad∗exp(ω) η = η, for all ξ, ω ∈ so(2) and η ∈ so∗ (2). Then the equations (18) (treated as an initial value problem) are given for SO(2) by θk+1 = θk + hωk , k = 0, . . . , N − 1 h τk + ωk−1 , k = 1, . . . , N − 1 I in addition to the initial conditions θ0 = θ0∗ , ω0 = ω0∗ . To verify the accuracy of our numerical computation, we give the corresponding continuous-time equations of motion for the planar rigid body on SO(2) using equations (7). The Lie bracket on SO(2) is identically equal to zero. Hence, one can check that the equations (7) are given by θ̇ = ω, ω̇ = τI , where θ, ω and τ are the continuous time angular position, velocity and
torque, respectively. We integrate the equations using the torque τ (t) = sin πt 2 , t ∈ [0, T ]. We use the following parameters for our simulations: T = 10, I = 1, θ(0) = 3, ω(0) = 4 and we try three different time steps corresponding to N = 1000, 1500, 2000. The error between the continuous- and discrete-time values of θ and ω are given in Figure (1). Note that the accuracy of the simulation improves with increasing N . (26) ωk = Remark 3.1. Note that the discrete-time equations (26) correspond to the Euler approximation for the equations of motion. This is a check that our method returns something familiar for a simple example as the planar rigid body. However, we emphasize that on SO(3) the discretization will not necessarily be equivalent to any of the classical discretization schemes. The discretization will generally result in a set of nonlinear implicit algebraic equations. 3.3. Discrete-Time Variational Optimal Control Problem. We now address Problem 3.2 by computing the constrained variation δτ k arising from the discrete equations of motion. Using equation (22) and taking the variation of the second equation in (18), we obtain 10 ANTHONY M. BLOCH, ISLAM I. HUSSEIN, MELVIN LEOK, AND AMIT K. SANYAL Q ฀ W ฀ Figure 1. Error dynamics on SO(2). (27) δτ k = Ad∗g−1 k   1h i 1  −1 Wk , J (Ωk ) −1 Wk J W − Ad W − Ad + k+1 k+1 gk gk h2 h   1 − 2 J Wk − Adg−1 Wk−1 , k−1 h for k = 1, . . . , N − 1. Taking variations of the cost functional (17) and substituting from equation (27) one obtains after a tedious but straight forward computation an expression for δJ in terms of δτ k : GEOMETRIC STRUCTURE-PRESERVING OPTIMAL CONTROL OF THE RIGID BODY 11 δJ = N −1 X k=1 "   1h i 1  −1 Wk + −1 Wk , J (Ωk ) J W − Ad W − Ad k+1 k+1 gk gk k h2 h # E D E  D 1  − 2 J Wk − Adg−1 Wk−1 , τ ]k + δτ 0 , τ ]0 + δτ N , τ ]N . k−1 h Ad∗g−1 When δJ is equated to zero (and after some algebraic rearrangement), one can obtain the boundary conditions on τ 0 , τ 1 , τ N −1 , τ N from the resulting equations below: τ 0 =0   1   ] ] ∗ −1 τ + Ad Ad J τ −1 J 1 1 g g1 1 h2  i 1 ∗ h ] − Adg−1 J (Ω1 ) , Adg−1 τ 1 1 1 h   1   ]  0 = − 2 J τ N −1 + Ad∗g−1 J Adg−1 τ ]N −1 N −1 N −1 h h  i 1 ∗ − Adg−1 J (ΩN −1 ) , Adg−1 τ ]N −1 N −1 N −1 h τ N =0 0=− as well as discrete evolution equations that are written in algebraic nonlinear form as: (28) 0=−          1 ] ] ] ] ∗ ∗ −1 τ −1 τ J τ − Ad τ − J Ad + Ad Ad −1 J −1 J k k+1 k gk−1 k−1 gk gk gk h2   h  i 1 h  i 1 Ad∗g−1 J (Ωk ) , Adg−1 τ ]k − − J (Ωk−1 ) , Adg−1 τ ]k−1 , k k−1 k h h for k = 2, . . . , N − 2. This result is summarized in the following theorem. Theorem 3.1. The necessary optimality conditions for the discrete Problem 3.2 are Rk+1 =Rk gk , k = 1, . . . , N − 2 Mk =Ad∗gk (hτ k + Mk−1 ) , k = 1, . . . , N − 1      1 0 = − 2 J τ ]k − Ad∗g−1 J τ ]k+1 k h     − J Adg−1 τ ]k−1 + Ad∗g−1 J Adg−1 τ ]k k−1 k k  h  i 1 − Ad∗g−1 J (Ωk ) , Adg−1 τ ]k k k h h  i  1 − J (Ωk−1 ) , Adg−1 τ ]k−1 , k = 2, . . . , N − 2 k−1 h Mk =J (Ωk ) , k = 0, . . . , N − 1, and the boundary conditions R0 =R∗0 , R1 = R∗0 g0∗ , Ω0 = Ω∗0  12 ANTHONY M. BLOCH, ISLAM I. HUSSEIN, MELVIN LEOK, AND AMIT K. SANYAL ∗ RN =R∗N , RN −1 = R∗N gN −1
−1 , ΩN −1 = Ω∗N −1 τ 0 =τ N = 0,
∗ ∗ where g0∗ = exp(hΩ∗0 ) and gN −1 = exp hΩN −1 . The following discussion shows that while our discrete approximation (13) is formally first-order accurate, it is symplectically equivalent to the second-order accurate Störmer–Verlet method, and hence has effective order two. 3.4. Lie Symplectic Euler and Symplectic Equivalence. Notice that the discrete Lagrangian adopted in our paper is obtained by approximating the velocity as R t2a constant over the timestep h, and by approximating the integral in time by f (t)dt ≈ (t2 − t1 )f (t1 ). In the Lie group setting, the constant angular velocity t1 approximation corresponds to the condition, Rk+1 = Rk exp(hΩk ) or equivalently, 1 exp−1 (R−1 k Rk+1 ). h When we let G = Rn , and we adopt the notation (q, v) ∈ T Rn , we obtain, qk+1 − qk , vk = h which is a usual finite-difference approximation for the velocity. Consider then a Lagrangian of the form, 1 L(q, v) = vT M v − V (q). 2 Approximating the action integral from 0 to h using a constant velocity approximation and a quadrature formula, yields, Z h Z h   qk+1 − qk  qk+1 − qk  dt ≈ hL qk , . L(q(t), v(t))dt ≈ L q(t), h h 0 0 We then choose as our discrete Lagrangian,     qk+1 − qk  1 qk+1 − qk T  qk+1 − qk  Ld (qk , qk+1 ) = hL qk , =h M − V (qk ) . h 2 h h Ωk = The discrete Euler–Lagrange equations, D2 Ld (qk−1 , qk ) + D1 Ld (qk , qk+1 ) = 0, yields,  q  q − q ∂V k k−1 k+1 − qk −M −h (qk ) = 0, h h ∂q which induces an implicit update map (qk−1 , qk ) 7→ (qk , qk+1 ). To obtain the corresponding Hamiltonian update map, we push-forward this algorithm to T ∗ Q by using the discrete fiber derivative FLd : Q×Q → T ∗ Q, which takes (qk , qk+1 ) 7→ (qk+1 , D2 Ld (qk , qk+1 )). In particular, we have that,  q k+1 − qk , pk+1 = D2 Ld (qk , qk+1 ) = M h which implies M (29) qk+1 = qk + hM −1 pk+1 . GEOMETRIC STRUCTURE-PRESERVING OPTIMAL CONTROL OF THE RIGID BODY 13 This allows us to rewrite the discrete Euler–Lagrange equations as, ∂V pk − pk+1 − h (qk ) = 0, ∂q or equivalently, ∂V (30) (qk ). pk+1 = pk − h ∂q Now, (29) and (30) are precisely the symplectic Euler method applied to the corresponding Hamiltonian vector field, as we shall see. The corresponding Hamiltonian is given by, 1 H(q, p) = pT M −1 p + V (q). 2 Hamilton’s equations yield, !     ∂H M −1 p q̇ ∂p = . = − ∂V ṗ − ∂H ∂q ∂q The symplectic Euler method has the form, qk+1 = qk + hq̇(qk , pk+1 ), pk+1 = pk + hṗ(qk , pk+1 ), which yields, qk+1 = qk + hM −1 pk+1 ,   ∂V (qk ) , pk+1 = pk + h − ∂q which is precisely what we obtained in (29) and (30). This demonstrates that our method is the generalization of the symplectic Euler method to Lie groups, which has important numerical consequences. While symplectic Euler is formally firstorder accurate, it is symplectically equivalent [21, 12] to the second-order accurate Störmer–Verlet method [4]. This means that one can obtain the Störmer–Verlet method FSV by conjugating the symplectic Euler method FE with a symplectic transformation T , FSV = T FE T −1 . In particular, numerical trajectories of symplectic Euler will shadow numerical trajectories obtained using Störmer–Verlet. Consider the implications of this symplectic equivalence for our discrete optimal control problem. Let the boundary conditions be specified by q0 , qN , and assume that we use Störmer–Verlet to propN agate the solution, then the boundary condition is expressed as, qN = FSV q0 = −1 N N −1 −1 N −1 (T FE T ) q0 = T FE T q0 , which is equivalent to q̃N = T qN = FE T q0 = FEN q̃0 . This implies that if we preprocess the boundary conditions q0 , qN , to obtain q̃0 = T −1 q0 , q̃N = T −1 qN , we could use symplectic Euler at the internal stages to propagate the states and costates, and then postprocess them to obtain the trajectory one would have obtained by using Störmer–Verlet. In practice, the shadowing result imparts the symplectic Euler method with the same desirable qualitative properties as Störmer–Verlet, and it is not necessary to postprocess the numerical solutions in order to achieve accurate results. Since on an appropriate choice of charts, our Lie symplectic Euler method reduces to 14 ANTHONY M. BLOCH, ISLAM I. HUSSEIN, MELVIN LEOK, AND AMIT K. SANYAL symplectic Euler in coordinates, it follows that there is a corresponding secondorder Lie Störmer–Verlet method that our method is symplectically equivalent to, and in particular, our method has effective order two. 4. Numerical Approach and Results The multiplier-free version of the first-order optimality equations, equation (28), in combination with the boundary conditions, R0 = R∗0 , RN = R∗N , Ω0 = Ω∗0 , and ΩN −1 = Ω∗N −1 , leave the torques τ 1 , . . . , τ N −1 , and the angular velocities Ω1 , . . . , ΩN −2 as unknowns. By substituting the relations gk = exp(hΩk ), Mk = J(Ωk ), we can rewrite the necessary conditions (28) as follows, 0=− 1 J(τ ]k ) − Ad∗exp(−hΩk ) J(τ ]k+1 ) h2 − J(Adexp(−hΩk−1 ) τ ]k−1  + Ad∗exp(−hΩk ) J(Adexp(−hΩk ) τ ]k ) h i 1 ∗ − Adexp(−hΩk ) J(Ωk ), Adexp(−hΩk ) (τ ]k ) h i 1h J(Ωk−1 ), Adexp(−hΩk−1 ) (τ ]k−1 ) , − h where k = 2, . . . , N − 2, and the discrete evolution equations, given by line 2 of (18), can be written as 0 =J(Ωk ) − Ad∗exp(hΩk ) (hτ k + J(Ωk−1 )), where k = 1, . . . , N − 1. In addition, we use the boundary conditions on R0 and RN , together with the update step given by line 1 of (18) to give the last constraint,   0 = log R−1 R exp(hΩ ) . . . exp(hΩ ) 0 0 N −1 , N where log is the logarithm map on SO(3). Note that while we use the direct variational approach to obtain the discrete extremal solutions, an alternate way to obtain the discrete extremal solutions would be to use Pontryagin’s maximum principle. In particular, Bonnans and LaurentVarin [2] show that these two approaches are equivalent in the context of symplectic partitioned Runge-Kutta schemes. At this point it should be noted that one important advantage of the manner in which we have discretized the optimal control problem is that it is SO(3)equivariant. This is to say that if we rotated all the boundary conditions by a fixed rotation matrix, and solved the resulting discrete optimal control problem, the solution we would obtain would simply be the rotation of the solution of the original problem. This can be seen quite clearly from the fact that the discrete problem is expressed in terms of body coordinates, both in terms of body angular velocities and body forces. In addition, the initial and final attitudes R0 and RN only enter in the last equation as a relative rotation. The SO(3)-equivariance of our numerical method is desirable, since it ensures that our results do not depend on the choice of coordinate frames. This is in GEOMETRIC STRUCTURE-PRESERVING OPTIMAL CONTROL OF THE RIGID BODY 15 contrast to methods based on coordinatizing the rotation group using quaternions and Euler angles. Each of the equations above take values in so(3). Consider the Lie algebra isomorphism between R3 and so(3) given by the hat map,   0 −v3 v2 0 −v1  , v = (v1 , v2 , v3 ) 7→ v̂ =  v3 −v2 v1 0 which maps 3-vectors to 3 × 3 skew-symmetric matrices. In particular, we have the following identities, [û, v̂] = (u × v)ˆ, AdA v̂ = (Av)ˆ. Furthermore, we identify so(3)∗ with R3 by the usual dot product, that is to say if Π, v ∈ R3 , then hΠ, v̂i = Π · v. With this identification, we have that Ad∗A−1 Π = AΠ. Using the identities above, we write the necessary conditions using matrixvector products and cross products. Then, each of the equations can be interpreted as 3-vector valued functions, and the system of equations can be considered as a 3(2N − 3)-vector valued function, which is precisely the dimensionality of the unknowns. This reduces the discrete optimal problem to a nonlinear root finding problem. The nonlinear system of equations was solved in MATLAB using the fsolve routine, where the Jacobian is constructed column by column, and the k-th column is computed using the following approximation [13], 1 ∂F (x) = Im[F(x + iek )], ∂xk  √ where i = −1, ek is a basis vector in the xk direction, and  is of the order of machine epsilon. This method is preferable to a finite-difference approximation, since it does not suffer from round-off errors, which would otherwise limit how small  can be. In our numerical simulation, we computed an optimal trajectory for a rest-torest maneuver, as illustrated in Figure (2). Here, the maneuver time is 12.8sec, N = 128, and the moment of inertia is given by   13.25 −7.80 −11.40 4.71  . J =  −7.80 16.25 −11.40 4.71 18.37 The prescribed maneuver corresponds to a rotation by π3 about the x-axis. Since the moment of inertia tensor is not a multiple of the identity, and the x-axis does not correspond to the axis of minimal inertia, the optimal trajectory does not just involve a pure rotation about the x-axis. It is worth noting that the results are not rotationally symmetric about the midpoint of the simulation interval, which is due to the fact that our choice of update, Rk+1 = Rk exp(hΩk ), does not exhibit timereversal symmetry. In a forthcoming publication, we will introduce a reversible algorithm to address this issue. In particular, this will involve explicitly computing the stationarity conditions for the discrete optimal control problem constrained by the time-symmetric Lie Störmer–Verlet method. We also present results for an optimal slew-up maneuver, illustrated in Figure (3). This uses the same moment of inertia tensor as in the previous simulation, and the desired maneuver involves a rotation of π6 about the x-axis from rest to a final 16 ANTHONY M. BLOCH, ISLAM I. HUSSEIN, MELVIN LEOK, AND AMIT K. SANYAL  angular velocity of ΩN −1 = 0.3 N = 128. 0.2 T 0.3 , over a maneuver time of 12.8sec, and 5. Conclusion In this paper we studied the continuous- and discrete-time optimal control problem for the rigid body, where the cost to be minimized is the external torque applied to move the rigid body from an initial condition to some pre-specified terminal condition. In the discrete setting, we use the discrete Lagrange–d’Alembert principle to obtain the discrete equations of motion. The kinematics were discretized to guarantee that the flow in phase space remains on the Lie group SO(3) and its algebra so(3). We described how the necessary conditions can be solved for the general three-dimensional case and gave a numerical example for a three-dimensional rigid body maneuver. The synthesis of variational mechanics with discrete-time optimal control is particularly advantageous from the point of view of computational efficiency, since the symplectic Euler method is symplectically conjugate to the Störmer–Verlet method, and hence has effective order two. Consequently, for our discrete-time optimal control method, the cost functional converges at a rate which is characteristic of a second-order method, while being based on a first-order method that is computationally cheaper. Currently, we are investigating the use of the Pontryagin’s maximum principle with Lie group methods in continuous- and discrete-time to obtain the necessary conditions. Additionally, we wish to generalize the result to general Lie groups that have applications other than the rigid body motion on SO(3). In particular, we are interested in controlling the motion of a rigid body in space, which corresponds to motion on the non-compact Lie group SE(3). Acknowledgments The research of Islam Hussein was supported by a WPI Faculty Development Grant. The research of Melvin Leok was partially supported by NSF grants DMS0504747, DMS-0726263, and CAREER Award DMS-0747659. The research of Anthony Bloch was supported by NSF grants DMS-030583, and CMS-0408542. The research of Amit Sanyal was partially supported by a University of Hawaii Faculty Development Grant. References [1] Agrachev, A.A., Sachkov, Y.: Control Theory from the Geometric Viewpoint. SpringerVerlag, New York, NY (2004) [2] Bonnans, J., Laurent-Varin, J.: Computation of order conditions for symplectic partitioned runge-kutta schemes with application to optimal control. Num. Math. 103, 1–10 (2006) [3] Hairer, E., Lubich, C., Wanner, G.: Geometric Numerical Integration. Springer, Berlin (2002) [4] Hairer, E., Lubich, C., Wanner, G.: Geometric numerical integration illustrated by the Störmer-Verlet method. Acta Numer. 12, 399–450 (2003) [5] Hussein, I.I., Scheeres, D.J., Hyland, D.C.: Interferometric observatories in Earth orbit. Journal of Guidance, Control and Dynamics 27(2), 297–301 (2004) [6] Iserles, A., Munthe-Kaas, H., Nørsett, S.P., Zanna, A.: Lie group methods. Acta Numerica 9, 215–265 (2000) [7] Junge, O., Marsden, J.E., Ober-Blöbaum, S.: Discrete mechanics and optimal control. IFAC Congress, Praha (2005) GEOMETRIC STRUCTURE-PRESERVING OPTIMAL CONTROL OF THE RIGID BODY 17 [8] Kane, C., Marsden, J.E., Ortiz, M., West, M.: Variational integrators and the newmark algorithm for conservative and dissipative mechanical systems. International Journal of Numerical Methods in Engineering 49(10), 1295–1325 (2000) [9] Khaneja, N., Glaser, S.J., Brockett, R.W.: Sub-Riemannian geometry and optimal control of three spin systems. Physical Review A 65, 032,301 (2002) [10] Leimkuhler, B., Reich, S.: Simulating Hamiltonian Dynamics, Cambridge Monographs on Applied and Computational Mathematics, vol. 14. Cambridge University Press, Cambridge (2004) [11] Leok, M.: Generalized galerkin variational integrators (2004). Preprint, arXiv:math.NA/0508360 [12] Littell, T., Skeel, R., Zhang, M.: Error analysis of symplectic multiple time stepping. SIAM J. Numer. Anal. 34(5), 1792–1807 (1997) [13] Lyness, J., Moler, C.: Numerical differentiation of analytic functions. SIAM J. Numer. Anal. 4, 202–210 (1967) [14] Marsden, J., West, M.: Discrete mechanics and variational integrators. Acta Numerica 10, 357–514 (2001) [15] Marsden, J.E., Ratiu, T.S.: Introduction to Mechanics and Symmetry. Springer-Verlag, New York, NY (1999) [16] Milnor, J.: Morse Theory. Princeton University Press, Princeton, NJ (1963) [17] Palao, J.P., Kosloff, R.: Quantum computing by an optimal control algorithm for unitary transformations. Physical Review Letters 89, 188,301 (2002) [18] Sanz-Serna, J.M., Calvo, M.P.: Numerical Hamiltonian Problems, Applied Mathematics and Mathematical Computation, vol. 7. Chapman and Hall, London (1994) [19] Schaub, H., Junkins, J.L., Robinett, R.D.: New attitude penalty functions for spacecraft optimal control problems. AIAA Guidance, Navigation, and Control Conference (1996) [20] Scrivener, S.L., Thompson, R.C.: Survey of time-optimal attitude maneuvers. Journal of Guidance, Control, and Dynamics 17(2), 225–233 (1994) [21] Suzuki, M.: Improved Trotter-like formula. Phys. Lett. A 180(3), 232–234 (1993) Alexander Ziwet Collegiate Professor of Mathematics, Mathematics, University of Michigan E-mail address: [email protected] Assistant Professor, Mechanical Engineering, Worcester Polytechnic Institute E-mail address: [email protected] Assistant Professor, Mathematics, Purdue University E-mail address: [email protected] Assistant Professor, Mechanical Engineering, University of Hawaii E-mail address: [email protected] ANTHONY M. BLOCH, ISLAM I. HUSSEIN, MELVIN LEOK, AND AMIT K. SANYAL 0.15 !1 !2 0.1 !3 ! 0.05 0 !0.05 0 2 4 6 8 10 12 8 10 12 t 1 " 1 "2 0.5 "3 " 0 !0.5 !1 0 2 4 6 t (a) Angular velocity and control torques 1.5 !1 ! 1 2 ! 3 ! 0.5 0 !0.5 0 2 4 6 8 10 12 8 10 12 t 1.5 1 |! | 18 0.5 0 0 2 4 6 t (b) Principal axis and angle (c) Instantaneous rotation axis Figure 2. Discrete optimal rest-to-rest maneuver in SO(3). GEOMETRIC STRUCTURE-PRESERVING OPTIMAL CONTROL OF THE RIGID BODY 19 0.3 !1 !2 0.1 !3 ! 0.2 0 !0.1 !0.2 0 2 4 6 8 10 12 t 6 "1 "2 4 " "3 2 0 !2 0 2 4 6 8 10 12 t (a) Angular velocity and control torques 0.8 !1 0.6 !2 ! 0.4 ! 3 0.2 0 !0.2 0 2 4 6 8 10 12 8 10 12 t 0.8 |! | 0.6 0.4 0.2 0 0 2 4 6 t (b) Principal axis and angle (c) Instantaneous rotation axis Figure 3. Discrete optimal slew-up maneuver in SO(3). View publication stats