Intrinsic Optimal Control for Mechanical Systems on Lie Group

The intrinsic infinite horizon optimal control problem of mechanical systems on Lie group is investigated. The geometric optimal control problem is built on the intrinsic coordinate-free model, which is provided with Levi-Civita connection. In order to obtain an analytical solution of the optimal problem in the geometric viewpoint, a simplified nominal system on Lie group with an extra feedback loop is presented. With geodesic distance and Riemann metric on Lie group integrated into the cost function, a dynamic programming approach is employed and an analytical solution of the optimal problem on Lie group is obtained via the HamiltonJacobi-Bellman equation. For a special case on SO(3), the intrinsic optimal control method is used for a quadrotor rotation control problem and simulation results are provided to show the control performance.


Introduction
Traditional methods describe the mechanical system in a flat Euclidean space with local coordinate, and the problem caused by local coordinate is inevitable, such as the singularity and ambiguity caused by Euler angles [1].Lie group is an effective and reliable tool to represent the states of mechanical systems in intrinsic coordinate-free approach.The pose (i.e., position and attitude) of mechanical systems can be described as an element of Lie group and the velocity can be defined on corresponding tangent space.Without local coordinates, the system model based on Lie group is concise and compact [2].By using the geometric method, the proper geometric characteristics of the mechanical system are preserved and the geometric viewpoint of the system is provided.
Many conventional works in nonlinear control theory have been developed in flat space framework with local coordinates [3].However, those control methods cannot be applied to the system represented by Lie group directly.Thus a compatible intrinsic geometric control method for the system on Lie group is required.Bullo and Murray provided the controllability condition on Lie group and presented a geometric PD control framework for fully actuated mechanical systems on SO(3) and SE(3) [4][5][6].The configuration error was described with geodesics on Lie group and exponential convergence of the energy function was obtained.Maithripala designed an intrinsic Luenberger observer on Lie group with intrinsic information and provide a coordinate-free tracking controller for mechanical systems on Lie group [7][8][9].He also introduced an intrinsic geometric PID method with covariant differentiation for left-invariant or right-invariant system [3,10].Bullo et al. used a smooth Morse function as the configuration error function which induced the configuration error in a nature coordinate on Lie group.Then geometric PD controllers on SO(3) and SE(3) were designed and applied on a quadrotor [2,[11][12][13].Moreover, Lee et al. provided the computational optimal geometric method on SO(3) [14,15] and employed the Pontryagin maximum principle to attain solutions of an open loop time-optimal problem.Spindler provided the differential equations which the optimal controls must satisfy via Pontryagin maximum principle.And the proposed results were applied to a spacecraft [16].Saccon et al. used the LQR-like closed-loop optimal control method on SO(3) with Euclidean distance [17].And Berkane and Tayebi utilized geodesics distance on SO(3) to replace the Euclidean distance and obtained an analogy Riccati equation [18].However, with the ignorance of the kinetics, those closed-loop optimal solutions only work for the kinematics of the system.This paper extends the closed-loop optimal control method for a class of mechanical systems on Lie group considering both kinematics and kinetics.With the intrinsic 2 Advances in Mathematical Physics model of a class of mechanical systems on Lie group, a feedback control loop in corresponding tangent space is provided via feedback linearization method, and a simpler nominal model of the mechanical system on Lie group is obtained.This approach is to ensure that the analytical optimal solution is attainable.The cost function is built based on Riemann metric and dynamic programming approach is adopted to solve the optimal control problem.The optimal solution of the nominal system on Lie group is presented with the viscosity solutions of Hamilton-Jacobi-Bellman equation.Finally, the intrinsic optimal control method is applied to quadrotor rotation dynamics, whose configuration manifold is standard SO(3).Performances of the intrinsic optimal control method are demonstrated through comprehensive simulations.

Lie Group and Riemann Manifold
2.1.Lie Group and Lie Algebra.Lie group G is a smooth manifold with embedded smooth group structure.q ∈ G is an element of the Lie group, and its tangent space is  q G.If q equals identity element e of the Lie group, the corresponding tangent space  e G is the Lie algebra space g.Lie algebra space g ≃ R  is isomorphic to the Euclidean space and is a flat space, where  denotes the dimension of Lie group.Then tangent space of an arbitrary element in Lie group can be obtained by left translation action.
For q, h ∈ G, a map  q : G → G, h → qh, if a vector field X in Lie group is X(qh) =  h  q X(h), where  h  q is the tangent map of  q at h, the vector field X is left-invariant, and the map  q is left translation map.
On Lie group, the exponential map exp : g → G is a local diffeomorphism.The Lie algebra space g can be used to represent elements of Lie group G via exponential map.The inverse map of exponential map is logarithmic map log : G → g.The logarithmic map can be regarded as a local chart of the Lie group.Each element in the Lie group can be expressed in Lie algebra space via the logarithmic map.For a mechanical system, its pose can be described as a unique element of a Lie group; a continuous movement of the mechanical system can be described as a smooth integral curve on the Lie group.Its velocity is defined on the tangent space of each element in the integral curve.A comprehensive introduction of Lie group and Lie algebra can be found in [19,20].

Riemann Metric.
Riemann metric is a second-order covariance tensor g : G × G → R. For all the elements q ∈ G, g q is symmetric positive definite bilinear form on the tangent space  q G.We denote the metric g q with symbol (⋅, ⋅).The translation map on G induces an inertial tensor map on the tangent space I : G →  * G, where  * G is the dual space of the tangent space G.Using the inertial tensor, a left-invariant Riemann metric on Lie group G can be induced as (X(q), Y(q)) ≜ I(q −1 X(q))(q −1 Y(q)).X(q), Y(q) is the vector field of q ∈ G.

Levi-Civita Connection. With the Riemann metric (⋅, ⋅)
on G, there is a unique torsion-free connection, which is Levi-Civita connection.For vector fields X The terms    are the connection coefficients in the frame {E k }.The Riemann metric on Lie group G is left-invariant; then the connection coefficients are constant, which can be obtained by where    are the structure constants of the frame {E k }.

Mechanical Systems on Lie Group.
If the configuration manifold of the mechanical system is a Lie group G, the velocity can be defined on the tangent space.The Riemann metric on the tangent space can be used to describe the kinetic energy of the mechanical system.And a smooth Morse function related to the configuration q ∈ G can be found to describe the potential energy [3].Generalized forces are all defined in the cotangent space, which is the dual space of the tangent space.
With the Riemann metric on Lie group G, the kinetic energy of the mechanical system is defined as V( q ) = (1/2)( q , q ), where q ∈ T q G, and a smooth Morse function (q) is used to define the potential energy on q ∈ G. Then the intrinsic Euler-Poincare equations of the mechanical system on Lie group are given by q = q ⋅ , where f c (q) is the conservative force, f d (q, ) is the damp force, and f u (q, ) is the generalized control force.f c (q), f d (q, ), f u (q, ) ∈  * G, and covariant derivative ∇ q q ∈ G.f c (q) satisfies the condition (, q ) = −(f c (q), ).Note the fact that the Levi-Civita connection is left-invariant, and (3) can also be expressed as where  *  is the adjoint operator of the dual space of Lie algebra  ∈ g.

Intrinsic Optimal Problem on Lie Group
3.1.Problem Statement.For a mechanical system on Lie group, the generic second-order geometric optimal control problem can be formulated as follows.Given the initial condition q 0 ∈ G,  0 ∈ g, and  0 , we consider the optimization problem subject to the kinematic equation ( 4) and the kinetics equation (5), where (q(), (), f  ()) is an incremental cost item and is described in a quadratic form  = (1/2)‖log (q The incremental cost item means the geometric state error and control input are considered in the cost function.log : G → g is the logarithm map on the Lie group, which can find a corresponding element on Lie algebra space for an arbitrary element of the Lie group. = log(q) ∈ g is the exponential coordinates of the element q ∈ G, and the geodesic distance between the element q ∈ G and identity e ∈ G can be given by the metric of the exponential coordinates ‖log (q)‖ 2 = (log(q), log(q)) [5].The incremental cost is similar to LQR problem in the linear system.(1/2)‖log (q)‖ 2 and (1/2)‖‖ 2 represent the Riemann metric of system configuration error and corresponding velocity error, respectively.(/2)‖f  ()‖ 2 indicates the control energy.Weight  > 0 is related to the control energy consumption.

Dynamic Equation Feedback Decouple.
For system (5), the dynamic equation is on Lie algebra space.Even though the Lie algebra space is flat and isomorphic to the Euclidean space, the system is coupled.This may lead to an extreme complex partial differential equation in the nonlinear optimal problem, and the analytical solution of the partial differential equation is almost impossible to obtain.To obtain the analytical solution, an extra feedback loop is used to decouple the system dynamic equation.
With the appropriate assumptions that conservative force f c (q) and damp force f d (q, ) are all known, the feedback control of dynamic ( 5) is designed as In (5), the inertial tensor map I of the mechanical systems is positive, and then the inverse tensor map I −1 :  * G → G can be found all the time.Using the feedback control (7), the system dynamic equation ( 5) is transfer to where k ∈ G is the virtual control term of the system.And ( 8) is the nominal dynamic system of (5) with the feedback (7).Then the optimal problem is considered with the kinematics (4) and nominal dynamics (8).

Infinite Horizon Optimal Control Solution.
Consider the following optimal control problem of mechanical system on Lie group: subject to q = q ⋅  and ξ = k.
Using the dynamic programming approach, the optimal control problem can be studied by looking at the timeinvariant value function (q, ), which should be a unique viscosity equation for the Hamilton-Jacobi-Bellman equation.According to [21][22][23], the value function (q, ) satisfies the equation  (q, , grad V) = 0 (10) and Hamiltonian function where (q, , k) = ((1/2)‖log(q)‖ 2 + (1/2)‖‖ 2 + (/2)‖k‖ 2 ) is the Lagrange form in the optimal objective (9) and p is the Lagrangian multiplier vector.F(q, , k) = [q ⋅ , k] T is the kinetic and dynamic function vector.grad V is the gradient of the value function (q, ) and grad  = [/q, /] T .Note that the value function (q, ) is time-invariant; then we have / = 0.The value function (q, ) satisfies (q, , grad V) = 0, which means Proposition 1.The optimal control k * , which satisfies (12), is and the corresponding value function (q, ) is the solution of the partial differential equation: Proof.Define a function  fl (q, , k) + grad V ⋅ F(q, , k) and  = min k∈G  = 0; we can get which is a quadratic-like form.The minimal value and corresponding control k * can be obtained by specialties of a quadratic equation.Then The corresponding control is k * = −(1/) ⋅ (/).And value function  satisfies the equation  = 0. Note that the solution for the quadratic equation is only valid when restricted to an open set.The proof is completed.
Proposition 2. (q, ) = ( 1 /2)‖log(q)‖ 2 + ( 2 /2)‖‖ 2 + ( 3 /2)‖log(q) + ‖ 2 is the solution of partial differential equation ( 14) with the coefficients conditions: Proof.To obtain the gradient of the value function (q, ), the time derivate is required.   (q, ) =  1 (log (q) , ) +  2 (, k) +  3 (log (q) , ) +  3 (log (q) , k) = (( 1 +  3 ) log (q) +  3 , ) Note the relationship between time derivate and partial derivatives; then The Riemann metric is left-invariant, which means (/q, q ⋅ ) = (q T (/q), ); then Comparing (18) with (20), the partial derivatives of the value function can be expressed as Taking ( 21) into ( 14), the equation is Simplifying the equation with the properties of Riemann metric, we have To make arbitrary q and  satisfy the identical equation ( 23), the coefficients have to meet the following conditions: Then four sets of solutions of ( 24) can be obtained as However, some of the solutions can not make sure that the control law stabilizes the states.Then we will choose the suitable solution via stability theory of dynamic system.
With Proposition 1, the suboptimal feedback control of the mechanical system on Lie groups (4) and ( 8) is Finally, for a general mechanical system on Lie groups (4) and ( 5), the optimal control is Note that the topological structure of the optimal control (27) is a geometric PD feedback control frame as shown in [8].With   =  3 and   =  2 +  3 , it is proved that if   and   are positive, the geometric PD control law locally exponentially stabilizes the state q at the identity element (see [8], Theorem 6).
Note the weight  > 0, to make sure that the optimal control law can stabilize the states q and ;   and   should be positive.Then Γ 1 is the unique suitable solution of (24).
The proof is completed.
The optimal control (27) is not depending on local coordinates.It only uses the intrinsic information of the mechanical system, and the optimal control is intrinsic.Note that the optimal control (27) is very similar to the solution of LQR problem for a time-invariant linear system [24].

Simulation
To evaluate the effectiveness of the proposed control algorithm (27) for a class of mechanical systems on Lie group, simulations are carried out with the proposed intrinsic optimal control method in terms of an intrinsic geometric quadrotor rotation dynamics on Lie group SO(3).Simulations are developed using MATLAB/Simulink, and the Crouch-Grossman numerical integration method is adapted to protect the geometric structure of the Lie group [25].By default, MATLAB/Simulink uses 16 digits of precision.The simulation time step is 0.01 s.
Without considering the damping force f d (q, ) and the conservative force f c (q) = 0 for rotation dynamics, the coordinate-free geometric rotation dynamic of the quadrotor is [26] where R ∈ SO(3) is the configuration of the quadrotor rotation,  = [ 1 ,  2 ,  3 ] T ∈ R 3 is the rotation speed on the body-fixed frame,  *  J = − × J, M ∈  * SO(3) is the control moment, and hat map ⋅ : R 3 → so(3) is a Lie algebra isomorphism Assume that the quadrotor is axial symmetry, and the inertial tensor is given as J = diag{0.0114,0.0114, 0.0227} kg⋅m 2 .The initial conditions are given as R 0 = [  1.By using the optimal control law (27), the optimal infinite horizon regulation control results are shown in Figures 1-6.
With exponential coordinate X = log(R) ∈ so(3) ≃ R 3×3 , the configuration error of the quadrotor attitude can be defined as e  = X and configuration error function is defined as (R) = ‖X‖ 2 , where ‖X‖ 2 = √ X T X is 2-norm of the vector X ∈ R 3 .The analytic formula of the logarithmic map on SO(3) can be found in [5,18].For the optimal infinite horizon regulation control problem, the configuration error will converge to zero with time as shown in Figure 1.When (R) = 0, R = I ∈ SO(3) and (R, 0) is the stable point of quadrotor rotation dynamics (28).The configuration errors of the quadrotor attitude with different weights are presented in Figure 2. As shown, Larger weight  means less control and the poorer dynamic performance.This is analogous to LQR method for a linear system.
The rotation speeds and control moment inputs are shown in Figures 3 and 4, respectively.Figure 1 indicates that a smaller  leads to a faster system response speed and results in a higher bandwidth.We define the total consumption of control moment as  con = ∫  virtual control energy.With the same initial conditions and inertial tensor, the smaller virtual control results in slower rotation speeds and smaller control moments.

Conclusion
Using intrinsic information of mechanical systems on Lie group, a geometric optimal control problem is investigated.A decoupling feedback loop is adopted to guarantee that    the analytic solution can be obtained.Using dynamic programming approach, Hamilton-Jacobi-Bellman equation is derived to obtain the analytical solution of the geometric optimal control problem.The effectiveness of the proposed optimal control algorithm is illustrated via simulations.Future work includes considering the particular external disturbances and uncertainties and approaches to obtain the intrinsic information with conventional sensors.

Figure 4 :
Figure 4: Control moments with different weights.

Figure 5 :
Figure 5: Optimal virtual control signal with different weights.

Table 1 :
Optimal control gains with different weight.

Table 2 :
Control energy consumption with different weights.