OPTIMAL CONTROL OF UNDERACTUATED MECHANICAL SYSTEMS WITH SYMMETRIES

. The aim of this paper is to study optimal control problems for underactuated mechanical systems with symmetries using higher-order La- grangian mechanics. We variationally derive the corresponding Lagrange Poincar´e equations for second-order Lagrangians with constraints deﬁned on trivial principal bundles and apply them to study an optimal control problem for an underactuated vehicle.


LEONARDO COLOMBO AND DAVID MARTÍN DE DIEGO
have several advantages, including the possibility of applying variational integrators to solve optimal control problems. The construction of variational integrators preserving geometric structures and the simulation of this kind of optimal control problems will be studied in a future work.
2. Preliminaries: Euler-Poincaré equations and optimal control problems. Let G be a Lie group and consider the left-multiplication on itself Obviously L g is a diffeomorphism (the same is valid for the right-translation, but in the sequel we only work with the left-translation, for sake of simplicity). This left multiplication allows us to trivialize the tangent bundle T G T G → G × g , (g,ġ) −→ (g, g −1ġ ) = (g, T g L g −1ġ) = (g, ξ), where g = T e G is the Lie algebra of G and e is the neutral element of G. In the same way, we have the identification T T G ≡ T (G × g) ≡ G × 3g, where 3g denotes 3 copies of the Lie algebra g, that is, 3g := g × g × g.
The principle gives us the Euler-Lagrange equations where l * g = (T e L g ) * and ad * is the coadjoint representation of the Lie algebra g. In the case when L is G−invariant, that is, the Lagrangian does not depend on the first entry, we obtain the Euler-Poincaré equations 2.2. Optimal control of underactuated mechanical systems. A control system is called underactuated if the number of control inputs is less than the dimension of the configuration space. Let L : G × g → R be a Lagrangian function and consider the controlled Euler-Lagrange equations where {e a } are independent elements of g * and (u a ) are admissible controls (see [2]). Complete {e a } to be a basis {e a , e A } of g * . Let {e i } = {e a , e A } be the dual basis in g with the bracket relation [e i , e j ] = C k ij e k . The basis {e i } = {e a , e A } induces coordinates (y a , y A ) = (y i ) in g, that is, if e ∈ g then e = y i e i = y a e a + y A e A .
With this notation, the Euler-Lagrange equation with controls are re-written as The optimal control problem consist on finding a trajectory (g(t), y(t), u a (t)) of the state variables and control inputs satisfying Equations (3), given initial and final boundary conditions (g(t 0 ), y i (t 0 )), (g(t f ), y i (t f )), and minimizing the cost functional This optimal control problem is equivalent to a second-order variational problem with second-order (vakonomic) constraints (see [1] to see the proof of this equivalence) described by L : 3. Second-order Lagrange-Poincaré equations for systems with constraints on trivial principal bundles. In this section, we derive from a variational point of view, using Hamilton's principle, second-order Lagrange-Poincaré equations on trivial principal bundles. It is well known that Lagrange-Poincaré equations are a generalization of Euler-Poincaré equations (see [3]). First, we derive Euler-Lagrange equations for Lagrangians defined on T M × G × g, where G is a Lie group, g its associated Lie algebra and M an n−dimensional differentiable manifold. Secondly, using a left trivialization of the second-order tangent bundle T (2) G we obtain the second-order Euler-Lagrange equations for Lagrangians defined on T (2) M ×G×g×g.
Since the main application of this paper is optimal control of underactuated mechanical systems we obtain in (3.3) the second-order Lagrange-Poincaré equations for second-order Lagrangians subject to second-order (vakonomic) constraints.
3.1. Euler-Lagrange equations for trivial principal bundles. Now, we derive, from a variational point of view, the Euler-Lagrange equations on the trivial Let L : T Q → R be a Lagrangian function. Since T Q T M × T G and T G G × g from a left-trivialization, we consider our Lagrangian function as L : The motion of the mechanical system is described by the variational principle for all variations δq and δξ where δq(0) = δq(T ) = 0, q(t) ∈ M and δξ verifies , where η is an arbitrary curve on the Lie algebra with η(0) = 0 = η(T ) and δg = gη. This principle gives rise to the Euler-Lagrange equations on trivial principal bundles given by d dt for i = 1, . . . , n, and ad * is the coadjoint representation of the Lie algebra g. If the Lagrangian L is G-invariant the above equations are the Lagrange-Poincaré equations: Remark 1. Observe that if the Lagrangian does not depend on the variables in the manifold M, the equations of motion are rewritten as the usual Euler-Poincaré equations (2). Then, these last equations can be consider as a generalization of Euler-Poincaré equations (2).

3.2.
Second-order Lagrange-Poincaré equations for trivial principal bundles. In this subsection we deduce, from Hamilton's principle, Euler-Lagrange equations for Lagrangians defined on T (2) Q T (2) M ×G×2g from a left-trivialization; where 2g denotes two copies of the Lie algebra, that is 2g := g × g.

3.3.
Mechanical systems defined on second-order trivial principal bundles subject to constraints. Now, we consider a second-order Lagrangian systems determined by L : T (2) M × G × 2g → R subject to second-order (vakonomic) constraints Φ α : T (2) M × G × 2g → R, 1 ≤ α ≤ m. We denote by M the constraint submanifold locally determined by the vanishing of these m-constraints.

LEONARDO COLOMBO AND DAVID MARTÍN DE DIEGO
The variational principle for this kind of second-order mechanical systems is given by min A(c(t)) with c(t) = (q(t),q(t),q(t), g(t), ξ(t),ξ(t)), subject to Φ α (c(t)) = 0 with 1 ≤ α ≤ m  As in the case of systems with constraints on T M, by using the Lagrange multipliers theorem, we can characterize the regular critical points of the second-order variational problem with second-order constraints as an unconstrained variational problem for an extended Lagrangian system (see [5] for a detailed proof.). where λ = (λ 1 , . . . , λ m ) as regarded as generalized coordinates on R m and L :

Proposition 1. Variational problem with second-order (vakonomic) constraints:
Therefore, the equations of motion for a Lagrangian system subject to secondorder (vakonomic) constraints are: and are called trivialized second-order Euler-Lagrange equations with constraints defined on If the Lagrangian is G−invariant (that is, L does not depend of the variables on G) these equations are called second-order Lagrange-Poincaré with (vakonomic)

OPTIMAL CONTROL OF MECHANICAL CONTROL SYSTEMS WITH SYMMETRIES 155
4. Application to optimal control of underactuated mechanical systems. The aim of this section is to study optimal control problems for underactuated mechanical systems, that is, Lagrangian control systems such that the number of control inputs is less than the dimension of the configuration space. For this kind of mechanical control systems we consider as configuration space Q a trivial principal bundle Q = M ×G, where G is a Lie group and M is a n−dimensional differentiable manifold. In what follows we assume that all the control systems in this work are controllable, that is, for any two points q 0 and q f in the configuration space, there exits an admissible control u(t) defined on some interval [0, T ] such that the system with initial condition q 0 reaches the point q f in time T (see for more details [1] and [2]).

4.1.
Optimal control problem: Define the control manifold U ⊂ R r where u(t) ∈ U is the control parameter. Consider the left-trivialized Lagrangian L : T Q T M × g → R, (where g is the Lie algebra associated to the Lie group G). The equations of motion are the controlled Lagrange-Poincaré equations: where we denote by B a = {(µ a , η a )}, µ a (q) ∈ T * q M, η a (q) ∈ g * , a = 1, . . . , r; and i = 1, . . . , n. Here, we are assuming that {(µ a , η a )} are independent elements of Γ(T * M × g * ) and u a are the admissible controls. Taking this into account, the optimal control problem consists on finding trajectories (q(t), ξ(t), u(t)) of the state variables and control inputs satisfying (9), subject to initial conditions (q(0),q(0), ξ(0)) and final conditions (q(T ),q(T ), ξ(T )), and, extremizing the functional J (q,q, ξ, u) = T 0 C(q(t),q(t), ξ(t), u(t)) dt. (10) We can reformulate this optimal control problem as a second-order order variational problem subject to second-order constraints in the following way: complete B a to a basis {B a , B α } of the vector space T * M × g * . Take its dual basis {B a , B α } on Γ(T M × g) = X(M ) × C ∞ (M, g). If we denote by B a = {(X a , χ a )} ∈ Γ(T M × g) (resp. B α = {(X α , χ α )} ∈ Γ(T M × g)), where X a , X α ∈ X(M ); X a = X i a (q) ∂ ∂q i ; X α = X i α (q) ∂ ∂q i and χ a (q); χ α (q) ∈ g, q ∈ M then equations (9) are rewritten as d dt As mentioned before, the proposed optimal control problem is equivalent to a variational problem with second-order (vakonomic) constraints (see [1] and reference therein), where we define the Lagrangian L : T (2) M × 2g → R given, in the selected coordinates, by where C is the cost function considered in (10) and Moreover, since the system is underactuated, the Lagrangian system determined by L is subjected to the second-order constraints: Thus, this kind of problems naturally fits in the setting introduced in §3.3 by considering k = 2 and left invariance with respect to the Lie group G as it is illustrated by the following example.

4.2.
Optimal control of an underactuated vehicle: Consider a rigid body moving in SE(2) with a thruster to adjust its pose. The configuration of this system is determined by a tuple (x, y, θ, γ), where (x, y) is the position of the center of mass, θ is the orientation of the blimp with respect to a fixed basis, and γ the orientation of the thrust with respect to a body basis. Therefore, the configuration manifold is Q = SE(2) × S 1 (see [2]).
The Lagrangian of the system is given by its kinetic energy L(x, y, θ, γ,ẋ,ẏ,θ,γ) = 1 2 m(ẋ 2 +ẏ 2 ) + 1 2 J 1θ 2 + 1 2 J 2 (θ +γ) 2 , and the input forces are F 1 = cos(θ + γ) dx + sin(θ + γ) dy − l sin γdθ, where the control forces that we consider are applied to a point on the body with distance l > 0 from the center of mass, along the body x-axis. Note that this system is an example of underactuated mechanical system where the configuration space is a trivial principal bundle.
A basis of the Lie algebra se(2) ∼ = R 3 of SE(2) is given by