Abstract
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 Hamilton-Jacobi-Bellman equation. For a special case on , the intrinsic optimal control method is used for a quadrotor rotation control problem and simulation results are provided to show the control performance.
1. 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 and [4–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–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 and were designed and applied on a quadrotor [2, 11–13]. Moreover, Lee et al. provided the computational optimal geometric method on [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 with Euclidean distance [17]. And Berkane and Tayebi utilized geodesics distance on 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 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 . Performances of the intrinsic optimal control method are demonstrated through comprehensive simulations.
2. Lie Group and Riemann Manifold
2.1. Lie Group and Lie Algebra
Lie group is a smooth manifold with embedded smooth group structure. is an element of the Lie group, and its tangent space is . If equals identity element of the Lie group, the corresponding tangent space is the Lie algebra space . Lie algebra space 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 , a map , , if a vector field in Lie group is , where is the tangent map of at , the vector field is left-invariant, and the map is left translation map.
On Lie group, the exponential map is a local diffeomorphism. The Lie algebra space can be used to represent elements of Lie group via exponential map. The inverse map of exponential map is logarithmic map . 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].
2.2. Riemann Metric
Riemann metric is a second-order covariance tensor . For all the elements , is symmetric positive definite bilinear form on the tangent space . We denote the metric with symbol . The translation map on induces an inertial tensor map on the tangent space , where is the dual space of the tangent space . Using the inertial tensor, a left-invariant Riemann metric on Lie group can be induced as . is the vector field of .
2.3. Levi-Civita Connection
With the Riemann metric on , there is a unique torsion-free connection, which is Levi-Civita connection. For vector fields and , the Levi-Civita connection is given as
The terms are the connection coefficients in the frame . The Riemann metric on Lie group is left-invariant; then the connection coefficients are constant, which can be obtained bywhere are the structure constants of the frame .
2.4. Mechanical Systems on Lie Group
If the configuration manifold of the mechanical system is a Lie group , 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 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 , the kinetic energy of the mechanical system is defined as , where , and a smooth Morse function is used to define the potential energy on . Then the intrinsic Euler-Poincare equations of the mechanical system on Lie group are given bywhere is the conservative force, is the damp force, and is the generalized control force. , , , and covariant derivative . satisfies the condition . Note the fact that the Levi-Civita connection is left-invariant, and (3) can also be expressed aswhere is the adjoint operator of the dual space of Lie algebra .
3. 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 , , and , we consider the optimization problemsubject to the kinematic equation (4) and the kinetics equation (5), where is an incremental cost item and is described in a quadratic form .
The incremental cost item means the geometric state error and control input are considered in the cost function. 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. is the exponential coordinates of the element , and the geodesic distance between the element and identity can be given by the metric of the exponential coordinates [5]. The incremental cost is similar to LQR problem in the linear system. and represent the Riemann metric of system configuration error and corresponding velocity error, respectively. indicates the control energy. Weight is related to the control energy consumption.
3.2. 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 and damp force are all known, the feedback control of dynamic (5) is designed as
In (5), the inertial tensor map of the mechanical systems is positive, and then the inverse tensor map can be found all the time. Using the feedback control (7), the system dynamic equation (5) is transfer towhere 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).
3.3. Infinite Horizon Optimal Control Solution
Consider the following optimal control problem of mechanical system on Lie group:subject to and .
Using the dynamic programming approach, the optimal control problem can be studied by looking at the time-invariant value function , which should be a unique viscosity equation for the Hamilton-Jacobi-Bellman equation. According to [21–23], the value function satisfies the equationand Hamiltonian functionwhere is the Lagrange form in the optimal objective (9) and is the Lagrangian multiplier vector. is the kinetic and dynamic function vector. is the gradient of the value function and . Note that the value function is time-invariant; then we have .
The value function satisfies , which means
Proposition 1. The optimal control , which satisfies (12), isand the corresponding value function is the solution of the partial differential equation:
Proof. Define a function and ; we can getwhich is a quadratic-like form. The minimal value and corresponding control can be obtained by specialties of a quadratic equation. Then The corresponding control is . And value function satisfies the equation . Note that the solution for the quadratic equation is only valid when restricted to an open set.
The proof is completed.
Proposition 2. is the solution of partial differential equation (14) with the coefficients conditions:
Proof. To obtain the gradient of the value function , the time derivate is required. Note the relationship between time derivate and partial derivatives; thenThe Riemann metric is left-invariant, which means ; then Comparing (18) with (20), the partial derivatives of the value function can be expressed asTaking (21) into (14), the equation isSimplifying the equation with the properties of Riemann metric, we have To make arbitrary and satisfy the identical equation (23), the coefficients have to meet the following conditions: Then four sets of solutions of (24) can be obtained asHowever, 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 and , it is proved that if and are positive, the geometric PD control law locally exponentially stabilizes the state at the identity element (see [8], Theorem6).
Note the weight , to make sure that the optimal control law can stabilize the states and ; and should be positive. Then 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].
4. 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 . 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 and the conservative force for rotation dynamics, the coordinate-free geometric rotation dynamic of the quadrotor is [26]where is the configuration of the quadrotor rotation, is the rotation speed on the body-fixed frame, , is the control moment, and hat map is a Lie algebra isomorphism
Assume that the quadrotor is axial symmetry, and the inertial tensor is given as kg·m2. The initial conditions are given as and . The control gains with different weights are shown in Table 1. By using the optimal control law (27), the optimal infinite horizon regulation control results are shown in Figures 1–6.
With exponential coordinate , the configuration error of the quadrotor attitude can be defined as and configuration error function is defined as , where is 2-norm of the vector . The analytic formula of the logarithmic map on 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 , and 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 , and the consumption of virtual control is . Control energy consumption is shown in Table 2. The results indicate that a smaller leads to smaller virtual control energy. With the same initial conditions and inertial tensor, the smaller virtual control results in slower rotation speeds and smaller control moments.
According to (9) and (27), the minimized function values of different weights are shown in Figure 6.
5. 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.
Conflicts of Interest
The authors declare that there are no conflicts of interest regarding the publication of this paper.
Acknowledgments
This work is supported by the National Natural Science Foundation of China under Grant no. 11572036. The authors also thank Yun Yuhang and Wang Tianning for their useful comments and language editing which have greatly improved the manuscript.