# Intrinsic Optimal Control for Mechanical Systems on Lie Group

Authors: Chao Liu; Shengjing Tang; Jie Guo
Journal: Advances in Mathematical Physics
 (2017)

---

## 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 onSO(3), the intrinsic optimal control method is used for a quadrotor rotation control problem and simulation results are provided to show the control performance.


---

## Body

## 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 SO(3) and SE(3) [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 SO(3) and SE(3) were designed and applied on a quadrotor [2, 11–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 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 standardSO(3). 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 groupG is a smooth manifold with embedded smooth group structure. q∈G is an element of the Lie group, and its tangent space is TqG. If q equals identity element e of the Lie group, the corresponding tangent space TeG is the Lie algebra space g. Lie algebra space g≃Rn is isomorphic to the Euclidean space and is a flat space, where n denotes the dimension of Lie group. Then tangent space of an arbitrary element in Lie group can be obtained by left translation action.Forq,h∈G, a map Lq:G→G, h→qh, if a vector field X in Lie group is Xqh=ThLqXh, where ThLq is the tangent map of Lq at h, the vector field X is left-invariant, and the map Lq is left translation map.On Lie group, the exponential mapexp: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].


### 2.2. Riemann Metric

Riemann metric is a second-order covariance tensorg:TG×TG→R. For all the elements q∈G, gq is symmetric positive definite bilinear form on the tangent space TqG. We denote the metric gq with symbol ·,·. The translation map on G induces an inertial tensor map on the tangent space I:TG→T∗G, where T∗G is the dual space of the tangent space TG. Using the inertial tensor, a left-invariant Riemann metric on Lie group G can be induced as Xq,Yq≜Iq-1Xqq-1Yq. Xq,Yq is the vector field of q∈G.


### 2.3. 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=XkEk and Y=YkEk, the Levi-Civita connection is given as(1)∇YX=dXkY+ωijkYiXjEk.The termsωijk are the connection coefficients in the frame Ek. The Riemann metric on Lie group G is left-invariant; then the connection coefficients are constant, which can be obtained by(2)ωijk=12Cijk-IksIirCjsr+IjrCisr,where Cijk are the structure constants of the frame Ek.


### 2.4. Mechanical Systems on Lie Group

If the configuration manifold of the mechanical system is a Lie groupG, 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 groupG, the kinetic energy of the mechanical system is defined as vq˙=(1/2)q˙,q˙, where q˙∈TqG, and a smooth Morse function uq 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(3)q˙=q·ξ,∇q˙q˙=q·I-1fcq+fdq,ξ+fuq,ξ,where fcq is the conservative force, fdq,ξ is the damp force, and fuq,ξ is the generalized control force. fcq, fdq,ξ, fuq,ξ∈T∗G, and covariant derivative ∇q˙q˙∈TG. fcq satisfies the condition du,q˙=-fcq,ξ. Note the fact that the Levi-Civita connection is left-invariant, and (3) can also be expressed as(4)q˙=q·ξ,(5)ξ˙=I-1adξ∗Iξ+fcq+fdq,ξ+fuq,ξ,where adξ∗ is the adjoint operator of the dual space of Lie algebra ξ∈g.


## 2.1. Lie Group and Lie Algebra

Lie groupG is a smooth manifold with embedded smooth group structure. q∈G is an element of the Lie group, and its tangent space is TqG. If q equals identity element e of the Lie group, the corresponding tangent space TeG is the Lie algebra space g. Lie algebra space g≃Rn is isomorphic to the Euclidean space and is a flat space, where n denotes the dimension of Lie group. Then tangent space of an arbitrary element in Lie group can be obtained by left translation action.Forq,h∈G, a map Lq:G→G, h→qh, if a vector field X in Lie group is Xqh=ThLqXh, where ThLq is the tangent map of Lq at h, the vector field X is left-invariant, and the map Lq is left translation map.On Lie group, the exponential mapexp: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].


## 2.2. Riemann Metric

Riemann metric is a second-order covariance tensorg:TG×TG→R. For all the elements q∈G, gq is symmetric positive definite bilinear form on the tangent space TqG. We denote the metric gq with symbol ·,·. The translation map on G induces an inertial tensor map on the tangent space I:TG→T∗G, where T∗G is the dual space of the tangent space TG. Using the inertial tensor, a left-invariant Riemann metric on Lie group G can be induced as Xq,Yq≜Iq-1Xqq-1Yq. Xq,Yq is the vector field of q∈G.


## 2.3. 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=XkEk and Y=YkEk, the Levi-Civita connection is given as(1)∇YX=dXkY+ωijkYiXjEk.The termsωijk are the connection coefficients in the frame Ek. The Riemann metric on Lie group G is left-invariant; then the connection coefficients are constant, which can be obtained by(2)ωijk=12Cijk-IksIirCjsr+IjrCisr,where Cijk are the structure constants of the frame Ek.


## 2.4. Mechanical Systems on Lie Group

If the configuration manifold of the mechanical system is a Lie groupG, 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 groupG, the kinetic energy of the mechanical system is defined as vq˙=(1/2)q˙,q˙, where q˙∈TqG, and a smooth Morse function uq 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(3)q˙=q·ξ,∇q˙q˙=q·I-1fcq+fdq,ξ+fuq,ξ,where fcq is the conservative force, fdq,ξ is the damp force, and fuq,ξ is the generalized control force. fcq, fdq,ξ, fuq,ξ∈T∗G, and covariant derivative ∇q˙q˙∈TG. fcq satisfies the condition du,q˙=-fcq,ξ. Note the fact that the Levi-Civita connection is left-invariant, and (3) can also be expressed as(4)q˙=q·ξ,(5)ξ˙=I-1adξ∗Iξ+fcq+fdq,ξ+fuq,ξ,where adξ∗ is the adjoint operator of the dual space of Lie algebra ξ∈g.


## 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 conditionq0∈G, ξ0∈g, and t0, we consider the optimization problem(6)minfu∈g∗⁡Jq0,ξ0,t0,fu=minfu∈g∗⁡∫t0∞Cqt,ξt,futdt,subject to the kinematic equation (4) and the kinetics equation (5), where Cqt,ξt,fut is an incremental cost item and is described in a quadratic form C=(1/2)logq2+(1/2)ξ2+(α/2)fut2.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 logq2=log⁡q,log⁡q [5]. The incremental cost is similar to LQR problem in the linear system. (1/2)logq2 and (1/2)ξ2 represent the Riemann metric of system configuration error and corresponding velocity error, respectively. (α/2)fut2 indicates the control energy. Weight α>0 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 forcefcq and damp force fdq,ξ are all known, the feedback control of dynamic (5) is designed as(7)fu=Iv-adξ∗Iξ+fcq+fdq,ξ.In (5), the inertial tensor map I of the mechanical systems is positive, and then the inverse tensor map I-1:T∗G→TG can be found all the time. Using the feedback control (7), the system dynamic equation (5) is transfer to(8)ξ˙=v,where v∈TG 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:(9)minv∈TG⁡Jq0,ξ0,t0,v=minv∈TG⁡∫t0∞12log⁡q2+12ξ2+α2v2dt,subject to q˙=q·ξ and ξ˙=v.Using the dynamic programming approach, the optimal control problem can be studied by looking at the time-invariant value functionVq,ξ, which should be a unique viscosity equation for the Hamilton-Jacobi-Bellman equation. According to [21–23], the value function Vq,ξ satisfies the equation(10)Hq,ξ,grad⁡V=0and Hamiltonian function(11)Hq,ξ,p=minv∈TG⁡Lq,ξ,v+p·Fq,ξ,v,where Lq,ξ,v=1/2logq2+1/2ξ2+(α/2)v2 is the Lagrange form in the optimal objective (9) and p is the Lagrangian multiplier vector. Fq,ξ,v=q·ξ,vT is the kinetic and dynamic function vector. gradV is the gradient of the value function Vq,ξ and gradV=∂V/∂q,∂V/∂ξT. Note that the value function Vq,ξ is time-invariant; then we have ∂V/∂t=0.The value functionVq,ξ satisfies Hq,ξ,gradV=0, which means (12)Hq,ξ,grad⁡V=minv∈TG⁡Lq,ξ,v+grad⁡V·Fq,ξ,v=0.Proposition 1.
The optimal controlv∗, which satisfies (12), is(13)v∗=-1α·∂V∂ξ∈TG,and the corresponding value function Vq,ξ is the solution of the partial differential equation: (14)αlog⁡q2+αξ2+2α∂V∂q,q·ξ-∂V∂ξ·∂V∂ξ=0.Proof.
Define a functionM≔Lq,ξ,v+grad⁡V·Fq,ξ,v and H=minv∈TG⁡M=0; we can get(15)M=Lq,ξ,v+grad⁡V·Fq,ξ,v=12log⁡q2+12ξ2+α2v2+∂V∂q,∂V∂ξ·q·ξ,vT=12log⁡q2+12ξ2+α2v2+∂V∂q,q·ξ+∂V∂ξ,v,which is a quadratic-like form. The minimal value and corresponding control v∗ can be obtained by specialties of a quadratic equation. Then (16)H=minv∈TG⁡M=αlog⁡q2+αξ2+2α∂V∂q,q·ξ-∂V∂ξ·∂V∂ξ.The corresponding control is v∗=-(1/α)·(∂V/∂ξ). And value function V satisfies the equation H=0. Note that the solution for the quadratic equation is only valid when restricted to an open set.
The proof is completed.Proposition 2.
V
q
,
ξ
=
(
k
1
/
2
)
l
o
g
q
2
+
(
k
2
/
2
)
ξ
2
+
(
k
3
/
2
)
l
o
g
q
+
ξ
2 is the solution of partial differential equation (14) with the coefficients conditions: (17)k1=-a-a+2a3/21/2a-1/2,k2=a+2a3/21/2-a1/2,k3=a1/2.Proof.
To obtain the gradient of the value functionVq,ξ, the time derivate is required. (18)ddtVq,ξ=k1log⁡q,ξ+k2ξ,v+k3log⁡q+ξ,ξ+v=k1log⁡q,ξ+k2ξ,v+k3log⁡q,ξ+k3log⁡q,v+k3ξ,ξ+k3ξ,v=k1+k3log⁡q,ξ+k2+k3ξ,v+k3log⁡q,v+k3ξ,ξ=k1+k3log⁡q+k3ξ,ξ+k2+k3ξ+k3log⁡q,v.
Note the relationship between time derivate and partial derivatives; then(19)ddtVq,ξ=∂V∂q·q˙+∂V∂ξ·ξ˙=∂V∂q,q˙+∂V∂ξ,ξ˙=∂V∂q,q·ξ+∂V∂ξ,v.
The Riemann metric is left-invariant, which means∂V/∂q,q·ξ=qT(∂V/∂q),ξ; then (20)ddtVq,ξ=qT∂V∂q,ξ+∂V∂ξ,v.
Comparing (18) with (20), the partial derivatives of the value function can be expressed as(21)qT∂V∂q=k1+k3log⁡q+k3ξ,∂V∂ξ=k2+k3ξ+k3logq.
Taking (21) into (14), the equation is(22)αlog⁡q2+αξ2+2αk1+k3log⁡q+k3ξ,ξ-k2+k3ξ+k3log⁡q,k2+k3ξ+k3log⁡q=0.
Simplifying the equation with the properties of Riemann metric, we have(23)α-k32log⁡q2+α+2αk3-k2+k32ξ2+-2k3k2+k3+2αk1+k3log⁡q,ξ=0.
To make arbitraryq and ξ satisfy the identical equation (23), the coefficients have to meet the following conditions: (24)α-k32=0,α+2αk3-k2+k32=0,-2k3k2+k3+2αk1+k3=0.
Then four sets of solutions of (24) can be obtained as(25)Γ1:k1=-α-α+2α3/21/2α-1/2k2=α+2α3/21/2-α1/2k3=α1/2,Γ2:k1=-α+α+2α3/21/2α-1/2k2=-α+2α3/21/2-α1/2k3=α1/2,Γ3:k1=α-α+2α3/21/2α-1/2k2=α-2α3/21/2+α1/2k3=-α1/2,Γ4:k1=α+α+2α3/21/2α-1/2k2=-α-2α3/21/2+α1/2k3=-α1/2.
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 Proposition1, the suboptimal feedback control of the mechanical system on Lie groups (4) and (8) is (26)v∗=-1α·k2+k3ξ+k3log⁡q.
Finally, for a general mechanical system on Lie groups (4) and (5), the optimal control is (27)fu=I-1α·k2+k3ξ+k3logq-adξ∗Iξ+fcq+fdq,ξ.
Note that the topological structure of the optimal control (27) is a geometric PD feedback control frame as shown in [8]. With kP=k3 and kd=k2+k3, it is proved that if kP and kd are positive, the geometric PD control law locally exponentially stabilizes the state q at the identity element (see [8], Theorem6).
Note the weightα>0, to make sure that the optimal control law can stabilize the states q and ξ; kP and kd 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].


## 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 conditionq0∈G, ξ0∈g, and t0, we consider the optimization problem(6)minfu∈g∗⁡Jq0,ξ0,t0,fu=minfu∈g∗⁡∫t0∞Cqt,ξt,futdt,subject to the kinematic equation (4) and the kinetics equation (5), where Cqt,ξt,fut is an incremental cost item and is described in a quadratic form C=(1/2)logq2+(1/2)ξ2+(α/2)fut2.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 logq2=log⁡q,log⁡q [5]. The incremental cost is similar to LQR problem in the linear system. (1/2)logq2 and (1/2)ξ2 represent the Riemann metric of system configuration error and corresponding velocity error, respectively. (α/2)fut2 indicates the control energy. Weight α>0 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 forcefcq and damp force fdq,ξ are all known, the feedback control of dynamic (5) is designed as(7)fu=Iv-adξ∗Iξ+fcq+fdq,ξ.In (5), the inertial tensor map I of the mechanical systems is positive, and then the inverse tensor map I-1:T∗G→TG can be found all the time. Using the feedback control (7), the system dynamic equation (5) is transfer to(8)ξ˙=v,where v∈TG 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:(9)minv∈TG⁡Jq0,ξ0,t0,v=minv∈TG⁡∫t0∞12log⁡q2+12ξ2+α2v2dt,subject to q˙=q·ξ and ξ˙=v.Using the dynamic programming approach, the optimal control problem can be studied by looking at the time-invariant value functionVq,ξ, which should be a unique viscosity equation for the Hamilton-Jacobi-Bellman equation. According to [21–23], the value function Vq,ξ satisfies the equation(10)Hq,ξ,grad⁡V=0and Hamiltonian function(11)Hq,ξ,p=minv∈TG⁡Lq,ξ,v+p·Fq,ξ,v,where Lq,ξ,v=1/2logq2+1/2ξ2+(α/2)v2 is the Lagrange form in the optimal objective (9) and p is the Lagrangian multiplier vector. Fq,ξ,v=q·ξ,vT is the kinetic and dynamic function vector. gradV is the gradient of the value function Vq,ξ and gradV=∂V/∂q,∂V/∂ξT. Note that the value function Vq,ξ is time-invariant; then we have ∂V/∂t=0.The value functionVq,ξ satisfies Hq,ξ,gradV=0, which means (12)Hq,ξ,grad⁡V=minv∈TG⁡Lq,ξ,v+grad⁡V·Fq,ξ,v=0.Proposition 1.
The optimal controlv∗, which satisfies (12), is(13)v∗=-1α·∂V∂ξ∈TG,and the corresponding value function Vq,ξ is the solution of the partial differential equation: (14)αlog⁡q2+αξ2+2α∂V∂q,q·ξ-∂V∂ξ·∂V∂ξ=0.Proof.
Define a functionM≔Lq,ξ,v+grad⁡V·Fq,ξ,v and H=minv∈TG⁡M=0; we can get(15)M=Lq,ξ,v+grad⁡V·Fq,ξ,v=12log⁡q2+12ξ2+α2v2+∂V∂q,∂V∂ξ·q·ξ,vT=12log⁡q2+12ξ2+α2v2+∂V∂q,q·ξ+∂V∂ξ,v,which is a quadratic-like form. The minimal value and corresponding control v∗ can be obtained by specialties of a quadratic equation. Then (16)H=minv∈TG⁡M=αlog⁡q2+αξ2+2α∂V∂q,q·ξ-∂V∂ξ·∂V∂ξ.The corresponding control is v∗=-(1/α)·(∂V/∂ξ). And value function V satisfies the equation H=0. Note that the solution for the quadratic equation is only valid when restricted to an open set.
The proof is completed.Proposition 2.
V
q
,
ξ
=
(
k
1
/
2
)
l
o
g
q
2
+
(
k
2
/
2
)
ξ
2
+
(
k
3
/
2
)
l
o
g
q
+
ξ
2 is the solution of partial differential equation (14) with the coefficients conditions: (17)k1=-a-a+2a3/21/2a-1/2,k2=a+2a3/21/2-a1/2,k3=a1/2.Proof.
To obtain the gradient of the value functionVq,ξ, the time derivate is required. (18)ddtVq,ξ=k1log⁡q,ξ+k2ξ,v+k3log⁡q+ξ,ξ+v=k1log⁡q,ξ+k2ξ,v+k3log⁡q,ξ+k3log⁡q,v+k3ξ,ξ+k3ξ,v=k1+k3log⁡q,ξ+k2+k3ξ,v+k3log⁡q,v+k3ξ,ξ=k1+k3log⁡q+k3ξ,ξ+k2+k3ξ+k3log⁡q,v.
Note the relationship between time derivate and partial derivatives; then(19)ddtVq,ξ=∂V∂q·q˙+∂V∂ξ·ξ˙=∂V∂q,q˙+∂V∂ξ,ξ˙=∂V∂q,q·ξ+∂V∂ξ,v.
The Riemann metric is left-invariant, which means∂V/∂q,q·ξ=qT(∂V/∂q),ξ; then (20)ddtVq,ξ=qT∂V∂q,ξ+∂V∂ξ,v.
Comparing (18) with (20), the partial derivatives of the value function can be expressed as(21)qT∂V∂q=k1+k3log⁡q+k3ξ,∂V∂ξ=k2+k3ξ+k3logq.
Taking (21) into (14), the equation is(22)αlog⁡q2+αξ2+2αk1+k3log⁡q+k3ξ,ξ-k2+k3ξ+k3log⁡q,k2+k3ξ+k3log⁡q=0.
Simplifying the equation with the properties of Riemann metric, we have(23)α-k32log⁡q2+α+2αk3-k2+k32ξ2+-2k3k2+k3+2αk1+k3log⁡q,ξ=0.
To make arbitraryq and ξ satisfy the identical equation (23), the coefficients have to meet the following conditions: (24)α-k32=0,α+2αk3-k2+k32=0,-2k3k2+k3+2αk1+k3=0.
Then four sets of solutions of (24) can be obtained as(25)Γ1:k1=-α-α+2α3/21/2α-1/2k2=α+2α3/21/2-α1/2k3=α1/2,Γ2:k1=-α+α+2α3/21/2α-1/2k2=-α+2α3/21/2-α1/2k3=α1/2,Γ3:k1=α-α+2α3/21/2α-1/2k2=α-2α3/21/2+α1/2k3=-α1/2,Γ4:k1=α+α+2α3/21/2α-1/2k2=-α-2α3/21/2+α1/2k3=-α1/2.
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 Proposition1, the suboptimal feedback control of the mechanical system on Lie groups (4) and (8) is (26)v∗=-1α·k2+k3ξ+k3log⁡q.
Finally, for a general mechanical system on Lie groups (4) and (5), the optimal control is (27)fu=I-1α·k2+k3ξ+k3logq-adξ∗Iξ+fcq+fdq,ξ.
Note that the topological structure of the optimal control (27) is a geometric PD feedback control frame as shown in [8]. With kP=k3 and kd=k2+k3, it is proved that if kP and kd are positive, the geometric PD control law locally exponentially stabilizes the state q at the identity element (see [8], Theorem6).
Note the weightα>0, to make sure that the optimal control law can stabilize the states q and ξ; kP and kd 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].


## 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 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 forcefdq,ξ and the conservative force fcq=0 for rotation dynamics, the coordinate-free geometric rotation dynamic of the quadrotor is [26](28)R˙=Rω^Jω˙+ω×Jω=M,where R∈SO(3) is the configuration of the quadrotor rotation, ω=ω1,ω2,ω3T∈R3 is the rotation speed on the body-fixed frame, adω∗Jω=-ω×Jω, M∈T∗SO(3) is the control moment, and hat map ·^:R3→so(3) is a Lie algebra isomorphism(29)ω^=0-ω3ω2ω30-ω1-ω2ω10.Assume that the quadrotor is axial symmetry, and the inertial tensor is given asJ=diag⁡0.0114,0.0114,0.0227 kg·m2. The initial conditions are given as R0=0.2919-0.07210.95370.45460.8877-0.0721-0.84150.45460.2919 and ω0=0.010.010.01. 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.Table 1
Optimal control gains with different weight.
α
k
1
k
2
k
3
0.01
0.9954
0.0095
0.1
0.05
0.9794
0.0454
0.2236
0.1
0.9614
0.0878
0.3162
0.5
0.8467
0.3916
0.7071Figure 1
Configuration error function values with different weights.Figure 2
Configuration errors with different weights.Figure 3
Rotation speeds with different weights.Figure 4
Control moments with different weights.Figure 5
Optimal virtual control signal with different weights.Figure 6
Minimized function valueJ∗ with different weights.With exponential coordinateX^=log⁡R∈so(3)≃R3×3, the configuration error of the quadrotor attitude can be defined as eR=X and configuration error function is defined as ψR=X2, where X2=XTX is 2-norm of the vector X∈R3. 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 Figures3 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 Mcon=∫t0tfMxt+Myt+Mztdt, and the consumption of virtual control is vcon=∫t0tfvxt+vyt+vztdt. 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.Table 2
Control energy consumption with different weights.
α
M
c
o
n
v
c
o
n
0.01
0.1213
9.9075
0.05
0.1053
8.6259
0.1
0.0984
8.0812
0.5
0.0801
6.6065According to (9) and (27), the minimized function values J∗=Jq0,ξ0,t0,v∗ 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.



---

*Source: 6302430-2017-07-12.xml*