NEIGHBORING EXTREMAL OPTIMAL CONTROL FOR MECHANICAL SYSTEMS ON RIEMANNIAN MANIFOLDS

. In this paper, we extend neighboring extremal optimal control, which is well established for optimal control problems deﬁned on a Euclidean space (see, e.g., [8]) to the setting of Riemannian manifolds. We further specialize the results to the case of Lie groups. An example along with simulation results is presented.


1.
Introduction. Neighboring extremal optimal control (NEOC) is well established for optimal control problems (OCPs) defined on a Euclidean space (see, e.g., [8]). NEOC provides a sensitivity-based fast correction to an optimal control for changes in the initial conditions and/or parameters, thereby providing a form of a local feedback and enhancing the real-world applicability of optimal control. The configuration space for most mechanical systems that perform large maneuvers is not a Euclidean space. For instance, the configuration space of a spacecraft modeled as a rigid body is the Lie group SE(3) = SO(3) R 3 (see, e.g., [21]). With this motivation, in this paper, we extend NEOC to OCPs for mechanical systems evolving on Riemannian manifolds. This extension and rigorous treatment of the underlying details represent the main contribution of this paper. We will first briefly discuss NEOC for OCPs defined on a Euclidean space. In what follows, we will suppress the explicit dependence of the state, costate and control trajectories on time unless otherwise necessary.
1.1. Neighboring extremal optimal control. We will first review some background material also covered in [20], where NEOC is discussed in the setting of R n . Consider a parameter dependent OCP, where the objective is to minimize a cost functional given by where x(·) ∈ AC([0, T ], R n ), u(·) ∈ L ∞ ([0, T ], R m ), p ∈ R l is a parameter, K : R n × R l → R, L : R n × R m × R l → R and f : R n × R m × R l → R n are functions of class C 2 . Let (x * p , u * p ) be a solution for the OCP (1)- (2), where u * p (t) denotes the optimal control, which satisfies the Lagrange multiplier rule in a normal form (see, e.g., [6]). Let λ * p be the solution corresponding to (x, u) = (x * p , u * p ) of the following costate equationλ where λ(·) ∈ AC([0, T ], R n ), H is the Hamiltonian and H(x, u, λ, p) := L(x, u, p) + λ T f (x, u, p). Altogether, (x * p , u * p , λ * p ) satisfy the following necessary conditions for optimalityẋ Suppose there is a small variation in the initial condition and/or the parameter, and we would like to update the optimal control. Instead of solving the original OCP again, we employ a first-order approximation of the necessary conditions for optimality around the nominal trajectory. This approximation is based on the linearized relations (see, e.g., [8], [17], [18], [19]) Under the the second-order sufficient optimality condition (see, e.g., [17], [19]), (6)- (8) represents the optimality condition for the following OCP (see, e.g., [8], [17], [18], [19]) min δu(·) subject to the perturbed dynamics where the matrices in the cost functional (9) and the Jacobian matrices in the dynamic constraint (10) are evaluated at the nominal trajectories. The optimal control for the OCP (9)-(10) is given by where all partial derivative matrices are evaluated at the nominal trajectories and δλ(t) is a perturbation from λ * (t), ultimately expressible in terms of δx(t) and δp. The updated control is now calculated as the sum of u * (t) and δu * (t) and can be used directly or to warm start an optimizer for parameter p + δp. This is the basic idea behind NEOC. For a detailed description of NEOC see [8]. For a mathematically rigorous introduction to NEOC see [30]. Remark 1. The OCP (9)-(10) is known as the accessory minimum problem in the calculus of variations (see, e.g., [32]). If there is no variation in the initial condition, i.e., the initial condition remains fixed, then δx(0) = 0 and similarly, if there is no variation in the parameter, i.e., the parameter remains fixed, then δp = 0. Note that it is also possible to obtain the solution in the conventional NEOC setting (see, e.g., [8]), by adding p as a state, withṗ = 0.
In the extension of NEOC to the Riemannian manifold setting, we will be using a few concepts from Riemannian geometry. We refer the reader unfamiliar with Riemannian geometry to [16], [26]. Before proceeding further, we define some notation which will be used in the paper.
2. Notation. We denote an n-dimensional complete connected Riemannian manifold by Q and the Riemannian metric by ·, · . For q ∈ Q, the tangent space of Q at q is denoted by T q Q. The tangent bundle is denoted by T Q. The cotangent space corresponding to T q Q is denoted by T * q Q. The cotangent bundle is denoted by T * Q. Forvq := (q,v) ∈ T Q, the tangent space of T Q atvq is denoted by Tvq T Q. The natural pairing between T * q Q and T q Q is denoted by ·(·). The musical isomorphism associated with the Riemannian metric ·, · is denoted by · , where · : T * Q → T Q. The natural projection map is denoted by π, where π : T Q → Q. The real vector space of all smooth vector fields is denoted by X(Q). The real vector space of all smooth covector fields is denoted by X * (Q). The unique Levi-Civita connection is denoted by ∇. The covariant derivative is denoted by D dt . The Lie bracket is denoted by [·, ·]. The curvature tensor of the connection ∇ is denoted by R(·, ·)·. The exponential map is denoted by exp. The interior of a set is denoted by int. The linear span of a set of vectors is denoted by span.
The rest of the paper is organized as follows. In Section 3, we consider a particular OCP, which will be used to illustrate NEOC. In Section 4, the OCP is solved (only for the sake of completeness) using Lagrange multipliers and the corresponding variational equations are also derived. In Section 5, the OCP is solved (only for the sake of completeness) as a variational problem and the corresponding variational equation is also given. In Section 6, the results are specialized to the case when Q is a compact semisimple Lie group and an example along with simulation results is presented. Finally, in Section 7 we make some concluding remarks with possible directions for future research.
is complete (see, e.g., [22]). Consider the following OCP min u(·) Dv where Note that in general, the n-tuple of control inputs [u 1 . . . u n ] T are constrained to take values in the set U ⊂ R n (nonempty, connected, with 0 ∈ int(U) and also generally assumed to be compact and convex). In a more general setting, e.g., when admissible controls are only assumed to be measurable locally bounded mappings taking values in the set U, more technical assumptions are needed (see, e.g., [2], [10]) but we do not consider such a setting in this paper.

Remark 2.
It is possible to generalize the idea presented in this paper to a cost functional, which has a more general form with a more complicated dynamic constraint (see, e.g., [21]). We choose to work with the cost functional (12) and the class of fully-actuated controlled mechanical systems for which the Lagrangian where v q ∈ T q Q, as the solution for (P) has a nice geometric interpretation thereby helping to present the main idea of the paper clearly and avoid unnecessary mathematical complications. One can also extend the idea presented in this paper to the class of under-actuated controlled mechanical systems (with a more general Lagrangian), using the method of Lagrange multipliers (see, e.g., [6]) but we leave such an extension to future work. A somewhat related paper, which is similar in spirit to our paper, is [33]. However, [33] does not treat NEOC. In fact, (P) is equivalent to the well known Riemannian geodesic problem (see, e.g., [6]). The local existence and uniqueness of the solution for (P) follow from the theorems on local existence and uniqueness of the solution for ordinary differential equations. The equations of motion for the class of fully-actuated controlled mechanical systems with the Lagrangian defined above are given by where q : [0, T ] → Q. The vertical lift of a vector field X on Q is the vector field X vlift on T Q given by where v q ∈ T q Q. In local coordinates, (16) has a simple interpretation. Let (q 1 , . . . , q n ) be the local coordinates for Q and (q 1 , . . . , q n , v 1 , . . . , v n ) be the corresponding local coordinates for T Q.
. . , X n ) are the component functions of X in some given chart. We can now re-write (15) as followsγ where γ : [0, T ] → T Q and Z is the geodesic spray associated with the connection ∇. In local coordinates, . It is not difficult to see that (15) is equivalent to (17). Indeed, in local coordinates, (15) has the following form Observe that (18) is a system of second-order ordinary differential equations on Q, which is equivalent to a system of first-order ordinary differential equations on T Q of the formq The connection ∇ induces an Ehresmann connection on π : T Q → Q such that, for all v q ∈ T q Q, there is a splitting of T vq T Q into a horizontal subspace and a vertical subspace, i.e., It is now easy to verify that with respect to the above splitting, for all v q ∈ T q Q, Z(v q ) ∈ H vq (T Q) and X vlift (v q ) ∈ V vq (π). For more details, see [1], [3], [4], [5], [6], [11], [31]. In view of the above discussion, we note that (13)- (14) are equivalent to (17). Using the splitting of T vq T Q discussed above, for all r ∈ T vq T Q, r can be uniquely written as follows where r h ∈ H vq (T Q) and r v ∈ V vq (π). For all pairs r 1 , r 2 ∈ T vq T Q, the Riemannian metric (Sasaki metric) on T Q is obtained in terms of the Riemannian metric on Q as follows It is now easy to verify that (12) is well defined, since 1 T 0 u(t), u(t) dt. For more details, see [28], [29], [31]. Before we proceed further, we introduce the concept of a variation (see, e.g., [1], [6], [10], [16], [24], [26]). Let Ω denote the set of all C 2 curves on Q satisfying the boundary conditions (13)- (14). The set Ω is also referred to as the path space of Q (see, e.g., [26]). For a curve q(t) ∈ Ω, T q(t) Ω is a vector space consisting of all C 2 vector fields w(t) along q(t) such that w(0) = 0 and w(T ) = 0.
To derive NEOC for (P), we first obtain the nominal trajectory, by solving (P) using two methods. The first method is solving (P) using Lagrange multipliers and the second method is solving (P) as a variational problem. 4. Solution using Lagrange multipliers. We proceed by following the same procedure as given in [15] and defining the augmented cost functional as follows where λ 1 (·), λ 2 (·) ∈ C 1 ([0, T ], T * q(·) Q). We will now fix some notation.

4.1.
Notation. For any smooth vector field y = n i=1 y i (t)X i (q) along the curve q, with velocity vector field v, Dy dt = n i=1ẏ , or in shorthand is written as Dy dt =ẏ + ∇ v y. Using this shorthand, Dy ∂ =0 = δy + ∇ w y. Similarly, for any smooth covector field α = n i=1 α i (t)ω i (q) along the curve q, with velocity vector field v, Dα dt = n i=1α , or in shorthand is written as Dα dt =α + ∇ v α. Using this shorthand, Dα ∂ =0 = δα + ∇ w α. For more details, see [15]. Before we proceed further, we need a few lemmas.  The necessary conditions for a normal extremal (see, e.g., [6]) for (P) are obtained by setting The above condition, with the use of Lemmas 4.1-4.2, gives the following where we have used integration by parts along with the fact that the one-parameter variation q is proper. We are now ready to state a theorem.
We assume that the nominal solution has been obtained for a fixed initial condition. Suppose there is a small variation in the initial condition and we would like to update the optimal control for (P). Instead of solving (P) from scratch, we employ NEOC as described previously. We will now fix some more notation.

Notation.
In what follows, we use superscript n to denote the nominal trajectory and the corresponding vector and covector fields. The one-parameter variation of q n (t) is denoted by q n . Note that the one-parameter variation of q n (t) is not proper as there is a small variation in the initial condition. The vector field v n (t) := ∂q n ∂t (t, 0) is the velocity vector field along q n (t) and the vector field w n (t) := ∂q n ∂ (t, 0) is the variation vector field associated with the one-parameter variation q n .
Employing the NEOC approach described previously, the variational equations for (22)-(25) are given as follows Note that the change in the control trajectory corresponding to the change in the initial condition is given by . Before we proceed further, we need a few lemmas. Lemma 4.4 ([16], [26]). Given any smooth vector field y along q , then Remark 3. Note that the definition of the curvature tensor of the connection ∇ used in this paper, differs by a negative sign from the one defined in [16], [26].
We are now ready to state two theorems.
Proof. Using Lemma 4.2, (26) can be re-written as follows The above equation gives the followinġ Using the symmetry of the connection ∇, the above equation can be re-written as followsẇ n = δv n + [w n , v n ].

NEOC FOR MECHANICAL SYSTEMS ON RIEMANNIAN MANIFOLDS 265
Using Lemma 4.4,(27) can be re-written as follows The above equation gives the following δv n + ∇ẇn v n + ∇ w nv n + ∇ v n δv n + ∇ v n ∇ w n v n + R(w n , v n )v n = δλ n 2 + ∇ w n λ n 2 . Substitutingẇ n = δv n + [w n , v n ] into the above equation, gives the following δv n + ∇ v n δv n + ∇ δv n v n + ∇ w nv n + ∇ w n ∇ v n v n = δλ n 2 + ∇ w n λ n 2 . Similarly, the other two variational equations can be derived using Lemma 4.5.
Theorem 4.7. The variational equations (30)- (31) give the following Jacobi equa- Proof. Substituting (30) into (31), gives the following Using the symmetry of the connection ∇, the above equation can be re-written as follows Using the definition of the curvature tensor of the connection ∇, the above equation can be re-written as follows Remark 5. It should be noted that (34) plays a crucial role in determining conjugate points for (P). It is also worthwhile to note that (34) corresponds to (3.3) in Theorem 4 of [9], where the case of a Lie group has been considered but not in a control theoretic setting.

5.
Solution as a variational problem. We will follow the same procedure as given in [6]. Before we proceed further, we need a lemma. [26]). Given w, x, y, z ∈ X(Q), then R(x, y)z, w = R(w, z)y, x .
The necessary conditions for a normal extremal for (P) are obtained by setting where we have used integration by parts twice along with the fact that the oneparameter variation q is proper. We are now ready to state a theorem.
Remark 6. It is sometimes appropriate to assume that Q is parallelizable (see, e.g., [6]). This means that there exist smooth vector fields form an orthonormal basis for T q Q, for all q ∈ Q. Given smooth vector fields {X i } n i=1 on Q, there exist unique smooth covector fields {ω i } n i=1 on Q such that the covectors {ω i (q)} n i=1 are the dual basis for T * q Q, for all q ∈ Q. Equivalently, the assumption that Q is parallelizable means that T Q is a trivial bundle. The assumption that Q is parallelizable is restrictive in some sense but it is satisfied for the case of Lie groups (see, e.g., [22]), which are of special interest.

Theorem 5.2 ([27]).
A necessary condition for a curve q(·) ∈ C 2 ([0, T ], Q) to be a normal extremal for (P) is that the velocity vector field v = dq dt satisfies the following equation Remark 7. In [15], it has been shown that (22)- (25) are equivalent to (35). In the case when Q = R n , with the standard inner product, the covariant derivative is the usual derivative and R = 0. We now see that (35) simplifies to the equation .... q = 0, which shows that each coordinate function of a normal extremal q for (P) is a cubic spline. So, Theorem 5.2 may be viewed as a generalization of cubic splines to the setting of Riemannian manifolds (see, e.g., [27]). Also, in the case, when one considers (P) with multiple way points (see, e.g., [14]), NEOC can be used to update the "cubic splines", with respect to the changes in data.
We do not give all the details, as they are similar to the previous section. The variational equation for (35) is given as follows Note that the change in the control trajectory corresponding to the change in the initial condition is given by D 2 q ∂t 2 =0 . We will now specialize the results to the case of Lie groups.
6. Application to Lie groups. We will now present NEOC for OCPs for mechanical systems evolving on Lie groups but before proceeding, we will fix some more notation.
6.1. Notation. We will denote an n-dimensional compact semisimple Lie group by G and its Lie algebra by g. The left translation map on G is denoted by L g and the tangent map of L g at h ∈ G is denoted by T h L g . The dual space of g (space of linear functionals α : g → R) is denoted by g * . The map ad : g → gl(g) is the adjoint representation of g. For x ∈ g, the adjoint action of x on g is given by the endomorphism ad x : g → g, with ad x (y) = [x, y], for all y ∈ g. The map ad * : g → gl(g * ) is the coadjoint representation of g. For x ∈ g, the coadjoint action of x on g * is given by the endomorphism ad * x : g * → g * , with ad * x α(y) = α([x, y]), for all y ∈ g, α ∈ g * . The inverse of the exponential map is denoted by log. The trace of a matrix is denoted by tr. The 2-sphere is denoted by S 2 .
We will still retain the same notation (P), in the case when Q = G. We are now ready to state a lemma.
We assume that the nominal solution has been obtained for a fixed initial condition. Suppose there is a small variation in the initial condition and we would like to update the optimal control for (P). Instead of solving (P) from scratch, we employ NEOC as described previously. The variational equations for (37)-(40) are given as followsẇ δλ n 1 = ad * δv n λ n 1 + ad * v n δλ n 1 , To illustrate NEOC for OCPs for mechanical systems evolving on Lie groups, we now consider an example, which is a slightly modified form of the example presented in [15].
The time taken to re-solve the OCP (45)-(47) is 5.58 (sec) approximately and using NEOC is 3.58 (sec) approximately on a 3.4 GHz Intel Core i7-2600K desktop computer with 16 GB of RAM.
7. Conclusions and future work. In this paper, we extended NEOC, which is well established for OCPs defined on a Euclidean space to the setting of Riemannian manifolds. We further specialized the results to the case of Lie groups. We presented        an example, along with simulation results, which validates the ideas presented in this paper. NEOC described in this paper only gives a prediction step and not a correction step. To improve the solution, a prediction step can be augmented by a correction step. In the future, we intend to extend the method presented in this paper to include a correction step as well along with the generalization to a more general cost function, with a more complicated dynamic constraint (such as the one considered in [21]).