# Geometric Collocation Method on SO(3) with Application to Optimal Attitude Control of a 3D Rotating Rigid Body

Authors: Xiaojia Xiang; Lizhen Wu; Lincheng Shen; Jie Li
Journal: Mathematical Problems in Engineering
 (2015)

---

## Abstract

The collocation method is extended to the special orthogonal group SO(3) with application to optimal attitude control (OAC) of a rigid body. A left-invariant rigid-body attitude dynamical model on SO(3) is established. For the left invariance of the attitude configuration equation in body-fixed frame, a geometrically exact numerical method on SO(3), referred to as the geometric collocation method, is proposed by deriving the equivalent Lie algebra equation inso(3)  of the left-invariant configuration equation. When compared with the general Gauss pseudo-spectral method, the explicit RKMK, and Lie group variational integrator having the same order and stepsize in numerical tests for evolving a free-floating rigid-body attitude dynamics, the proposed method is higher in accuracy, time performance, and structural conservativeness. In addition, the numerical method is applied to solve a constrained OAC problem on SO(3). The optimal control problem is transcribed into a nonlinear programming problem, in which the equivalent Lie algebra equation is being considered as the defect constraints instead of the configuration equation. The transcription method is coordinate-free and does not need chart switching or special handling of singularities. More importantly, with the numerical advantage of the geometric collocation method, the proposed OAC method may generate satisfying convergence rate.


---

## Body

## 1. Introduction

Attitude control is one of the basic technologies that an object’s motioning relies on, and thus it has been the research focus in motion control technology. For object motion, to enhance its performance and security, it is required to save its energy consumption and advance control performance itself, and the new control methods continue to emerge. There is no doubt that optimal control theory is the most natural framework toward conformity with meeting the above requirements. However, except that the linear quadratic issues such simple question, analytical solutions are seldom available or even possible. As a result, more often than not, one resorts to numerical techniques. Numerical methods for solving optimal control problems are divided into two major classes [1]: indirect methods and direct methods. In an indirect method, the calculus of variations [2] and the Pontryagin maximum principle [3] are used to determine the first-order optimality conditions of the original optimal control problem, so, it converts the optimal control problem to a two-point boundary-value problem, including indirect shooting method [4] and indirect multiple-shooting method [5]. The primary advantage of the indirect methods lies in their high accuracy and assurances that the solution satisfies the necessary optimality conditions. However, indirect methods also have significant disadvantages. First, the necessary optimality conditions must be derived analytically; for most problems this derivation is difficult; Second, the radius of convergence is typically small. Third, a guess for the costates must be provided. In a direct method, the state and/or control of the optimal control problem is discretized in some manner and the problem is transcribed into a nonlinear programming problem (NLP), and, then, the NLP is solved using well-known optimization techniques, including direct shooting, collocation methods. Although not as high in precision as indirect methods, direct methods also have advantages because the optimality conditions do not need to be derived, and they have a large radius of convergence.In general, the attitude of an object is represented by the Euler angle. However, when the object is regarded as a single rigid body in three-dimensional space, the configuration of its rotation actually constructs a special Lie group called the special orthogonal group SO(3) [6]. In recent years, some researchers have applied optimal control on a Lie group to attitude control of a 3D rotating rigid body, to reduce the computational burden, or to provide a coordinate-free approach to avoid expensive chart switching, special handling of singularities, and numerical drift [7, 8]. Bloch et al. [9] and Hussein et al. [10] study a discrete variational optimal control problem for the rigid body on SO(3), and they describe the dynamical equations of the mechanical system as a geometric structure-preserving model referred to as a Lie group variational integrator and then use Lagrange’s method to derive the discrete necessary conditions (i.e., two-point boundary-value problem) which are solved by a standard nonlinear root-finding method. Lee et al. [7] propose an accurate computational approach for a nonconvex optimal attitude control (OAC) for a rigid body, in which the use of geometrically exact computations on SO(3) guarantees that the approach has excellent convergence properties. Similar to [7], the problem is also formulated directly as a discrete time optimization problem using a Lie group variational integrator; the discrete time necessary conditions for optimality are derived and solved by an efficient computational shooting method. Kobilarov and Marsden [11] extend the above conclusion to more general optimal control problems of mechanical systems on Lie groups and develop an efficient numerical method that exploits the structure of the state space and preserves the system motion invariants. The above-mentioned works have some common features: first, instead of discretizing equations of motion, they all use a discrete variational integrator obtained from the discrete Lagrange-d’Alembert principle, a process that better approximates the equations of motion; second, they are all indirect optimal control methods. In this paper, we turn to direct methods to solve the OAC problem on SO(3). Among the direct methods, the collocation method, also known as a pseudo-spectral method, is widely used in rigid body attitude optimization (including UAV, satellite, space shuttle, launch vehicle, and supersonic aircraft) by virtue of its high accuracy and spectral (or exponential) convergence rate [1]. In the context of a Lie group, to our knowledge, Moulla et al. [12] are the first to propose the concept of the “geometric pseudo-spectral method.” They suggest a polynomial pseudo-spectral method that preserves the geometric structure of port-Hamiltonian systems, phenomenological laws, and conservation laws without introducing any uncontrolled numerical dissipation. However, their method is designed only for port-Hamiltonian systems in a special structure, which implies that the Dirac structure, therefore, could not be directly extended to the systems on SO(3). Thereby, how to apply the pseudo-spectral method to the OAC problem on SO(3) and conserve its geometric properties is still an open question, which is also the major topic studied in this paper.In this work, we study a constrained attitude control problem for the rigid body on SO(3) based on pseudo-spectral collocation method. First, we establish a left-invariant rigid-body attitude dynamical model in body-fixed frame on SO(3). Second, for the left invariance of attitude configuration in the body-fixed frame, we provide a discretization mechanism known as the geometric collocation method on SO(3), which may improve the accuracy of evolving trajectory and the convergence rate through applying different collocation strategies to the configuration equation and the velocity equation, respectively. For the configuration equation, since its geometric properties could not be preserved if the general pseudo-spectral method is directly applied to it, drawing on Engø’s idea about the equivariant map [13], we transform the configuration equation on SO(3) into an equivalent equation in the corresponding Lie algebra space [14] and then the solutions to the equivalent equation at discrete points are transformed into configuration at corresponding points via the coordinate map. For the velocity equation, we can use the general pseudo-spectral method directly to compute the velocities at the discrete points that are the same as those of configuration, since the velocity belongs to the Lie algebra space that is isomorphic to the Euclidean space. Under the condition that it has the same 4th order and large stepsize, the proposed method is compared with the general Gauss pseudo-spectral method [15], explicit RKMK [16], and Lie group variational integrator [7, 8]. We find that the accuracy of the proposed method is superior to other methods, except that the explicit RKMK demonstrates a higher convergence rate than the other methods. Furthermore, the method could preserve the structural characteristics of the Lie group precisely. Finally, we apply the above numerical method to a constrained OAC problem. In addition, the numerical method is used to solve the constrained OAC problem on SO(3). The optimal control problem is transcribed into a nonlinear programming problem, in which the equivalent Lie algebra equation is considered as the defect constraints instead of configuration equation. The transcription method is coordinate-free and does not need chart switching or special handling of singularities. More importantly, with the numerical advantage of the geometric collocation method, the OAC method exhibits a high convergence rate.The rest of this paper is organized as follows. Section2 proposes a left-invariant rigid-body attitude dynamical model on SO(3). The geometric collocation method for the left-invariant attitude dynamics system on SO(3) is given in Section 3, where Section 3.1 provides a general pseudo-spectral collocation method in the Euclidean space, Section 3.2 presents the Legendre-Gauss pseudo-spectral collocation method on SO(3), and, then, the corresponding algorithm is given in Section 3.3, and numerical tests are carried out on a free-floating rigid body model in comparison with four other numerical methods in Section 3.4. In Section 4, the proposed collocation is applied to a typical OAC problem and compared with several prior works. Finally, the conclusions and future work are outlined in Section 5.


## 2. Kinematics and Dynamics of Attitude Motion of a Rigid Body on SO(3)

### 2.1. Left-Invariant Mechanical System in a Body-Fixed Frame

The motion of a rigid body on Lie groupG could be described in a space frame or a body-fixed frame. To avoid confusion, we would consider the motion of a single rigid body in a body-fixed frame in the sequel.Definition 1 (left-invariant mechanical systems on Lie groupG [6]).
In a body-fixed frame, the motion of a rigid body could be described as a left-invariant mechanical system as follows:(1)g˙=TeLg∘ξ^∈TgG,∀g∈G,where g denotes the configuration of a rigid body in G and ξ^=Tg-1g˙∈TeG denotes the velocity of the rigid body in the body-fixed frame, where TeG≔g, known as the Lie algebra, is the tangent space at the identity e of G and “∧” is the hat operator satisfying isomorphism ∙^:Rn→g with “∨” being its inverse mapping. Lg:G→G represents the left translation; that is, Lgh=gh, ∀h∈G, and TeLg is its tangent space at e. For the matrix group, such as SO(3), SE(2), and SE(3), (1) is equivalent to g˙=g·ξ^. “Left-invariant” implies that the mechanical system is invariant under left multiplication by constant matrices.


### 2.2. Kinematics and Dynamics of Attitude Motion of a Rigid Body on SO(3)

SO(3) is called the special orthogonal group, whose group element is the rotational component of the body-fixed frameb relative to the space frame a [6]. The left-invariant kinematics of attitude motion of a rigid body on SO(3) are given by(2)R˙abt=Rabt·ω^abbt≔Gωabbt,Rabt,where Rab(t) and ωabb(t) denote the attitude configuration and angular velocity of a rotating rigid body on SO(3). The hat operator ∙^:R3→so(3), for all y∈R3, satisfying ω^abb(t)·y=ωabb(t)×y, where × is vector cross product [6].And it is known that the attitude dynamics of the rigid body are generally satisfying [10, 11](3)ω˙abbt=J-1Jωabbt×ωabbt+Rab∗t∂RabtVRabt+Textt≔Fωabbt,Rabt,Textt,where J is referred to as the inertia matrix of the rigid body, Rab∗t∂RabtVRabt denotes the moment of the conservative force with VRabt being potential energy, and Textt≔Bt·ut is that of the nonconservative external force with Bt being the control conjugate vector.


## 2.1. Left-Invariant Mechanical System in a Body-Fixed Frame

The motion of a rigid body on Lie groupG could be described in a space frame or a body-fixed frame. To avoid confusion, we would consider the motion of a single rigid body in a body-fixed frame in the sequel.Definition 1 (left-invariant mechanical systems on Lie groupG [6]).
In a body-fixed frame, the motion of a rigid body could be described as a left-invariant mechanical system as follows:(1)g˙=TeLg∘ξ^∈TgG,∀g∈G,where g denotes the configuration of a rigid body in G and ξ^=Tg-1g˙∈TeG denotes the velocity of the rigid body in the body-fixed frame, where TeG≔g, known as the Lie algebra, is the tangent space at the identity e of G and “∧” is the hat operator satisfying isomorphism ∙^:Rn→g with “∨” being its inverse mapping. Lg:G→G represents the left translation; that is, Lgh=gh, ∀h∈G, and TeLg is its tangent space at e. For the matrix group, such as SO(3), SE(2), and SE(3), (1) is equivalent to g˙=g·ξ^. “Left-invariant” implies that the mechanical system is invariant under left multiplication by constant matrices.


## 2.2. Kinematics and Dynamics of Attitude Motion of a Rigid Body on SO(3)

SO(3) is called the special orthogonal group, whose group element is the rotational component of the body-fixed frameb relative to the space frame a [6]. The left-invariant kinematics of attitude motion of a rigid body on SO(3) are given by(2)R˙abt=Rabt·ω^abbt≔Gωabbt,Rabt,where Rab(t) and ωabb(t) denote the attitude configuration and angular velocity of a rotating rigid body on SO(3). The hat operator ∙^:R3→so(3), for all y∈R3, satisfying ω^abb(t)·y=ωabb(t)×y, where × is vector cross product [6].And it is known that the attitude dynamics of the rigid body are generally satisfying [10, 11](3)ω˙abbt=J-1Jωabbt×ωabbt+Rab∗t∂RabtVRabt+Textt≔Fωabbt,Rabt,Textt,where J is referred to as the inertia matrix of the rigid body, Rab∗t∂RabtVRabt denotes the moment of the conservative force with VRabt being potential energy, and Textt≔Bt·ut is that of the nonconservative external force with Bt being the control conjugate vector.


## 3. Legendre-Gauss Geometric Pseudo-Spectral Collocation Method on SO(3)

### 3.1. Legendre-Gauss Pseudo-Spectral Collocation Method in the Euclidean Space

Before collocating the left-invariant mechanical system on SO(3), we briefly describe the basic principle of the Legendre-Gauss pseudo-spectral collocation (LG-PC) method [1]. Take, for example, solving the following differential equation in Rn at tf,(4)x˙tt=ft,xt,xt0=x0∈Rn,t0≤t≤tf.First, we equally divide a time interval[t0,tf] into several time subintervals [tk,tk+1] with the stepsize being h and transform the time interval t∈[tk,tk+1] into τ∈[-1,1] via the affine transformation t=[(tk+1-tk)·τ+(tk+1+tk)]/2. Thus, accordingly, (4) over [tk,tk+1] becomes(5)x˙ττ=tk+1-tk2fτ,xτ,-1≤τ≤1.Letτ1<⋯<τN be collocation points in [-1,1] and τ0=-1. We approximate the solution of (5) by the following formula:(6)xτ≈Xτ=∑i=0Nxi·Liτ,where Liτ=∏j=0,j≠iN(τ-τj)/(τi-τj) denotes Lagrange polynomials, satisfying the isolation property(7)Licj=δij=1,i=j0,i≠j.Equation (6) together with the isolation property leads to xi=x(τi), thereby X(τi)=x(τi).Through expressing the derivative of the Lagrange polynomials at the collocation points in differential matrix formD=[dji]∈RN×(N+1), where(8)dji=L˙icj=∑l=0N∏k=0,k≠i,lNcj-ck∏k=0,k≠iNci-ck,j=1,…,N,i=0,…,N,we can write (5) at the collocation points as a set of differential algebra equations:(9)∑i=0Ndji·Xci=tk+1-tk2fcj,Xcj,j=1,…,N.Based on the above equations, we establish the following defect equations,(10)ςj=∑i=0Ndji·Xci-tk+1-tk2fcj,Xcj,j=1,…,N,and apply iterative algorithms to the above equations so as to determine the approximation to x(τ) at the collocation points. Finally, according to the following formula, we obtain the approximation to (4) at the endpoint tk+1 of time subinterval [tk,tk+1]:(11)xtk+1≈Xtk+tk+1-tk2∑j=1Nϖj·fcj,Xcj,where ϖj=∫-11Lj(t)dt is quadrature weight corresponding to the collocation points. This approximate solution will be the initial value of x(t) over [tk+1,tk+2] and so on.Remark 2.
According to different selection methods of collocation points, pseudo-spectral collocation methods can be divided into two categories, namely, standard and orthogonal methods. Collocation points in orthogonal collocation are those obtained from the roots of either Chebyshev polynomialsTN(τ) or Legendre polynomials PN(τ) belonging to orthogonal polynomials [17]. The benefit of using orthogonal collocation over standard collocation is that the quadrature approximation to a definite integral is extremely accurate [1]. Furthermore, according to whether the endpoint is a collocation point, pseudo-spectral collocation methods fall into three general categories [18]: Gauss methods, where neither of the endpoints −1 or 1 is a collocation point; Radau methods, where at most one of the endpoints −1 or 1 is a collocation point; and Lobatto methods, where both the endpoints −1 and 1 are collocation points. In the rest of this paper, we will develop our geometric collocation method based on Legendre-Gauss points, since when Legendre-Gauss methods are used for transcribing the continuous optimal control problem into a discrete nonlinear programming problem, it does not suffer from a defect in the optimality conditions at the boundary points for the endpoints are not collocation points [15].


### 3.2. Legendre-Gauss Geometric Pseudo-Spectral Collocation Method on SO(3)

For attitude kinematics (referred to as the “configuration equation”), it is well-known that the solution of (2) stays on SO(3) for all t≥t0. How to use the pseudo-spectral collocation method to solve (2), while maintaining some exact geometric properties (such as Lie group invariants) of the configuration equation under discretization of Rab(t), is the main problem to be solved in this subsection. However, SO(3) is a nonlinear manifold, and using the pseudo-spectral collocation method directly to discretize the configuration could not preserve the geometric properties of its solution. Reference [13] indicated that any differential equation in the form of an infinitesimal generator on a homogeneous space (a manifold with a transitive Lie group action) is locally equivalent to a differential equation on a Lie algebra corresponding to the Lie group acting on the homogenous space. Also, in the context of SO(3), the Lie algebra so(3) corresponding to SO(3) is isomorphic to R3 [9–11]. For the above-mentioned reasons, the differential equation on so(3) is the natural choice of space for pseudo-spectral discretization, like many classical Lie group numerical methods. For this purpose, we transform (3) into an equivalent equation on so(3), then use the pseudo-spectral collocation method to solve the equivalent equation, and finally map its solution back to configuration space. As mentioned earlier, since the Lie algebra space is isomorphic to the Euclid space to which the attitudes dynamics belong, and before discretizing the configuration equation, we directly use the general pseudo-spectral method to collocate the angular velocities at the discrete points.


#### 3.2.1. Collocation of Angular Velocities at Discrete Points

For attitude dynamics, since its variableωabb(t) belongs to so(3) that is isomorphic to R3, (3) can be collocated at Gauss collocation points directly by the Legendre-Gauss pseudo-spectral collocation method as follows:(12)ωabbτj=tk+1-tk2∑i=1Nd~ji·Fωabbτi,Rabτi,Textτi-∑i=1Nd~ji·di0·ωabbtk,j=1,…,N.Similarly, the approximate solutions ofωabb(τj), j=1,…,N, are approached by several times of the Gauss-Seidal type iterative operation. Then, the approximation solutions are used to obtain the velocity at the endpoint tk+1:(13)ωabbtk+1=ωabbtk+tk+1-tk2∑i=1Nϖi·Fωabbτi,Rabτi,Textτi.


#### 3.2.2. Collocation of Configurations at Discrete Points

To transform (2) on SO(3) into an equivalent differential equation evolving on so(3), we briefly describe the basic idea of the equivalent map.Definition 3 (equivariant map [13]).
LetM and N be manifolds and let G be a Lie group which acts on M by Φg:M→M and on N by Ψg:N→N. A smooth map f:M→N is called equivariant with respect to these actions if, for all g∈G,(14)f∘Φg=Ψg∘fwhich indicates the following diagram commutes.
Diagram commutes of the equivariant map are as follows:(15)M⟶fN↑Φg14↑ΨgM⟶fN
First, from the definition of an action ofG on M, we can obtain an equivariant map Φy:G→M with respect to the left translation action Lg of G on itself and an action G of Φg on M:(16)Φy∘Lg=Φg∘Φy.
It is known that there is a local coordinate mapf:g→G on G with the Lie algebra g, and the most typical one is the exponential map exp:g→G. At this point, we need to find an action Bg of G on g such that f will be an equivariant map with Bg and the left action of G on itself, namely,(17)f∘Bg=Lg∘f.
In casef is the exponential map, Bg is nothing other than the well-known Baker-Campbell-Hausdorff (BCH) formula Bg(u)=log⁡(g·exp(u)), where log:G→g is called the logarithm map. Since the composition of two equivariant maps is also an equivariant map, we can construct an equivariant map Φy∘f from g to M with respect to the action Bg on g and Φ on M.
Diagram commutes of compositionΦy∘f are as follows:(18)g⟶fG⟶ΦyM↑Bexptξ17↑Lg16↑Φexptξg⟶fG⟶ΦyM
Theorem 3.6 in literature [13] states that if ϕ is an equivariant map, then the infinitesimal generators of the action with respect to the same element ξ∈g are ϕ-related vector fields. Thus, the infinitesimal generators ξg and ξM of the flows Bexptξ and Φexptξ, respectively, on g and M are Φy∘f-related; that is,(19)ξM∘Φy∘f=TΦy∘Tf∘ξg.
Finally, we need to determine whatξg is, and the following theorem gives the conditions that it needs to meet.Lemma 4 (see [13]).
Letf:g→G be a coordinate map on G and Φy∘f equivariant with respect to the flows Bexptξ and Φexptξ. The infinitesimal generator of Bg satisfying (19) is ξg(u)=dfu-1(υ). df:g→g is the trivialization Tf defined as dfu=TRfu-1∘Tfu.Based on Lemma4 and other previous conclusions, we can derive the equivalent equation on so3 of the left-invariant configuration equation.Proposition 5 (equivalent equation onso3 of the left-invariant configuration equation).
The configuration equation (2) has an equivalent equation on so3 as follows:(20)u˙t=df-ut-1ωabbt≔G~ωabbt,ut,tk≤t≤tk+1,where f:so3→SO3 is a local coordinate map on SO(3) with df being its differential.Proof.
Without any loss of generality, we assume that the solutionRabt of (2) can be written in the following form:(21)Rabt=Rabt0·fut.
Then, by differentiating it [20], we can obtain(22)ddtRabt=ddtRabt0·fut=Rabt0·dLfut∘df-utu˙t=Rabt0·fut·df-utu˙t.
Comparing the preceding formula with (2) and (21), we have(23)ωabbt=df-utu˙t.
Taking the inverse of the preceding formula, we obtain the equivalent equation (20) on so(3) of the left-invariant configuration equation (2).There are several choices of the coordinate mapf, such as the exponential map and Cayley map [21]. In case f is the exponential map “exp” the vector field df-u(t)-1 on so(3) can be represented as [20](24)dexp-ut-1ωabbt=ωabbt+12adutωabbt+∑k=2∞Bkk!adutkωabbt,where the adjoint operator adu:g→g is a Lie bracket or commutator, adu(ωabb(t))=[u,ωabb(t)], satisfying the recursive expression aduk(ωabb(t))=[u,aduk-1(ωabb(t))], and Bk are Bernoulli numbers [21]:(25)
B
k
=
1
6
,
-
1
30
,
1
42
,
-
1
30
,
5
66
,
-
691
2730
,
…
k
=
2
N
,
N
∈
Z
+
0
,
k
=
2
N
+
1
,
N
∈
Z
+
.Now, we solve (20) by the LG-PC method as follows:(26)uτj=tk+1-tk2∑i=1Nd~ji·G~ωabbτi,uabτi-∑i=1Nd~ji·di0·utk,j=1,…,N.In view of the mechanical system being autonomous, we may use Gauss-Seidal type iteration to update the approximate solution of (26) as soon as possible [22]. As mentioned earlier, after obtaining the approximate solution, the velocity at the endpoint tk+1 can be obtained according to the following formula:(27)utk+1=utk+tk+1-tk2∑i=1Nϖi·G~ωabbτi,uabτi.It is worth noting that, different from the LG-PC method in the Euclidean space, the initial valueu(tk) in [tk,tk+1] is not equal to the endpoint value u(tk) in [tk-1,tk]. It should be calculated through the logarithmic map log:SO(3)→so(3):(28)utk=logSO3⁡Rab-1tk·Rabtk,k=0,1,2,…with(29)logSO3⁡R=ϕ2sin⁡ϕR-RT∈so3,where ϕ satisfies cos⁡ϕ=(tr(R)-1)/2, ϕ<π and tr(R)≠-1. It is easy to know that u(tk),k=0,1,2,…, are identically equal to zero.At last, the attitude configurationRab(tk+1) at the endpoint tk+1 can be reconstructed from u(tk+1) according to Rab(tk+1)=R(tk)·f(u(tk+1)), where k∈Z+ and the Cayley map should be selected as the coordinate map f, for two reasons [11]: first, it provides only an approximation to the integral curve defined by the exponential map, which is however easy to calculate; second, it does not involve trigonometric functions so that its derivatives do not have any singularities and thus will not cause any difficulties when used by gradient-based methods for solving NLP problems. The Cayley map satisfies the following formula:(30)caySO3ωabbt=I3+44+ωabbt2ω^abbt+ω^abbt22,where · is a standard Euclidean norm.


### 3.3. Algorithm of the Legendre-Gauss Geometric Pseudo-Spectral Collocation Method on SO(3) with Application to Attitude Motion of a Rigid Body

In accordance with the aforementioned LG-GPC method, we develop a Legendre-Gauss geometric pseudo-spectral collocation algorithm for integrating the kinematics and dynamics of attitude motion of a rigid body on SO(3). This algorithm would be further used to discretize the OAC problem in Section4.Algorithm 6 (LG-GPC).
Inputs include time interval[t0,tf], stepsize h, number of LG points N, initial attitude configuration Rab(t0), initial angular velocity ωabb(t0), iterations of inner while loop num, and threshold of equivalent configuration (or velocity) deviation err.
Step 1. Initialization is as follows: let k be zero.
Step 2. Compute initial value of the equivalent configuration at t0 in terms of Rabt0:(31)ut0=logSO3Rab-1t0·Rabt0≡0.
Step 3 (while (tk<tf)).
Step 3.1. Compute the LG points τ1,…,τN in [tk,tk+1], differentiation matrix DN×N+1, and quadrature weights ϖ1,…,ϖN.
Step 3.2. Set the current iterative step number n=0 and configuration (or velocity) deviation e=1.
Step 3.3. Let ωabbnτi,i=1,…,N, be ωabbtk let and unτi,i=1,…,N be utk.
Step 3.4 (while (n<num and e>err)).
Step 3.4.1. Obtain the submatrix D~N×N of the matrix DN×N+1:(32)D~=D:,2:N+1.
Step 3.4.2. According to the following equations, obtain [ω˙abbn(τ1),…,ω˙abbnτN]T:(33)ω˙abbnτ1⋮ω˙abbnτN=tk+1-tk2Fωabbnτ1,Rabnτ1,Textτ1⋮FωabbnτN,RabnτN,TextτN-d01⋮d0Nωabbtk.
Step 3.4.3. Update angular velocities [ωabbn+1τ1,…,ωabbn+1τN]T at LG points:(34)ωabbn+1τ1⋮ωabbn+1τN=D~-1·ω˙abbnτ1⋮ω˙abbnτN.
Step 3.4.4. According to the following equations, obtain [u˙nτ1,…,u˙nτN]T:(35)u˙nτ1⋮u˙nτN=tk+1-tk2G~ωabbnτ1,unτ1⋮G~ωabbnτN,unτN-d01⋮d0Nutk.
Step 3.4.5. Update the equivalent configuration [un+1τ1,…,un+1τN]T at LG points:(36)un+1τ1⋮un+1τN=D~-1·u˙nτ1⋮u˙nτN.
Step 3.4.6. Update the configuration [Rabn+1τ1,…,Rabn+1τN]T at LG points:(37)Rabn+1τ1⋮Rabn+1τN=Rabtk·cayun+1τ1⋮Rabtk·cayun+1τN.
Step 3.4.7. Compute the equivalent configuration deviation and the velocity deviation within two adjacent iterative steps, and, then, let the larger one be the final deviation e at the current step:(38)e=maxabsdetRabn+1τ1TRabnτ1⋮detRabn+1τNTRabnτN∞-1,ωabbn+1τ1-ωabbnτ12⋮ωabbn+1τN-ωabbnτN2∞.
Step 3.4.8. Substitute ωabbn+1τi, un+1τi, and Rabn+1τi into ωabbnτi, unτi, and Rabnτi, respectively, where i=1,…,N, and let n=n+1.
Step 3.5.End while.
Step 3.6. Relying on the final iterative values ωabbnτi, Rabnτi with Textτi, i=1,…,N in inner while loop, compute the angular velocity ωabbtk+1 at the endpoint tk+1:(39)ωabbtk+1=ωabbtk+tk+1-tk2∑i=1Nϖi·Fωabbnτi,Rabnτi,Textτi.
Step 3.7. Also, based on the final iterative values ωabbnτi, unτi, i=1,…,N in the inner while loop, compute the equivalent configuration utk+1 at the endpoint tk+1:(40)utk+1=utk+tk+1-tk2∑i=1Nϖi·G~ωabbnτi,unτi.
Step 3.8. Based on the previous formula, compute the configuration Rabtk+1 at the endpoint tk+1:(41)Rabtk+1=Rabtk·cayutk+1.
Step 3.9. Let tk=tk+1, k=k+1, and utk=0.Step 4.End while.Remark 7.
The LG pointsτi, i=1,…,N, are the roots of the Legendre polynomial PNτ=dN[(τ2-1)N]/[(2NN!)dτN] of degree N. In fact, it is convenient to compute the LG points via the eigenvalues of the following tridiagonal Jacobi matrix:(42)J=0β1β10β2⋱⋱⋱⋱0βN-1βN-10,βn=n4n2-1.
The LG pseudo-spectral differentiation matrixD=[dji]∈RN×(N+1) is given as [22](43)
d
j
i
=
1
+
τ
j
P
˙
N
τ
j
+
P
N
τ
j
τ
j
-
τ
i
1
+
τ
i
P
˙
N
τ
i
+
P
N
τ
i
,
i
≠
j
1
+
τ
i
P
¨
N
τ
i
+
2
P
˙
N
τ
i
2
1
+
τ
i
P
˙
N
τ
i
+
P
N
τ
i
,
i
=
jwith(44)P˙N=-N+1PNτ1-τ2.
The LG weights corresponding to the LG points are computed as(45)ϖi=21-τi2P˙N2τi.Remark 8.
In Step 3.3, bothustepτ1 and ustepτ2 cannot be set to utk, because utk shows the projection of distance between Rabtk-1 and Rabtk on the tangent space.


### 3.4. Numerical Integration Tests of the LG-GPC Method

In this subsection, we will test the validity and efficiency of the proposed method through numerical integration of the following free-floating rigid body kinematics (46) and dynamics (47) on SO(3). Note that (46) and (47) are equivalent to (6) and (7) under the condition of zero external force. Table 1 summarizes the parameters used by numerical tests. Consider(46)R˙abt=Rabt·ω^abbt≔Gωabbt,Rabt,(47)ω˙abbt=J-1Jωabbt×ωabbt≔Fωabbt,Rabt.Table 1
Parameters of numerical integration tests [19].
Physical quantity
Symbol
Value
Initial time
t
0
0 s
Terminal time
t
f
240 s
Moment of inertia
J
diag
⁡
1,2.8,2kg⋅m2
Initial configuration
R
a
b
t
0
I
3
×
3
Initial angular velocity
ω
a
b
b
t
0
0.5
,
-
0.5,0.4
T rad/sTo ensure that numerical integration test is more comparable, we consider the 4th-order LG-GPC method and select three other 4th-order numerical methods for comparison, including ode45 [23], explicit 4th-order RKMK [16], and 4th-order LG-PC method [22]. Furthermore, we compare the proposed numerical method with the Lie group variational integrator (LGVI) which is also based on geometrically exact computations on SO(3) [19]. Among the previous methods, ode45 is the built-in integrator of MATLAB, which is a variable-step method and whose results would be used as the ground truth with a specified tolerance 10-14. RKMK is a Lie group method [16]. Note that [24] states that the interpolatory quadrature formula with N Gauss points is of 2Nth order. To this end, both the 4th-order LG-GPC method and the LG-PC method depend on 2 LG points. In addition, in the LG-GPC and LG-PC methods, the iterations of the inner while loop num and the threshold of the equivalent configuration (or velocity) deviation err are set to be 30 and 10-14, respectively. Since LGVI has an implicit equation, we need to solve it by using the Newton iterative approach, where the termination error of iteration and the maximum estimated value of the function are set to be 1e-14 and 9000, respectively. Numerical integration tests are all carried out in the same stepsize.Figure1 gives the angular configuration deviation and the average runtime per update of LG-GPC method when compared with LG-PC, RKMK, and LGVI, respectively, at five different steps. Figure 1(a) shows the angular configuration deviation against steps by calculating logSO(3)(Rode45TRab)2, where Rode45 denotes the angular configuration of the free-floating rigid body which is computed by ode45. It has been illustrated that the accuracy of LG-GPC is superior to those of LG-PC, RKMK, and LGVI. Figure 1(b) shows the average runtime per update of LG-GPC methods when compared with LG-PC, RKMK, and LGVI. As it can be seen, the computational efficiency of LG-GPC is inferior to that of RKMK, because RKMK is an explicit method. However, the proposed method requires less computation time than the classical LG-PC method and is nearly 1.5 times as efficient as the LG-PC method on average. More obviously, the computational efficiency of the proposed method is superior to that of LGVI, since the latter needs the Newton iteration approach to solve the implicit approach.Figure 1
Accuracy and computational efficiency of LG-GPC when compared with LG-PC and RKMK.
(a)
Average orientation deviation versus steps
(b)
Average runtime per updateAdditionally, the Lie group structural conservativeness of LG-GPC is compared with those of LG-PC, RKMK, and LGVI. Considering that the angular configuration is parameterized by unit quaternion in the ode45 method, we only compare the proposed method with the LG-PC method, explicit RKMK, and LGVI. As mentioned above, since the group element on SO(3) satisfiesRabRabT=I, we compute the infinite norm of RabRabT-I3×3 for evaluating the structural conservativeness [19]. Figure 2 illustrates that the LG-PC method belonging to the Euclidean space has no group structural conservativeness, because it is not a Lie group method. However, RKMK, LGVI, and LG-GPC method on the Lie group are able to preserve the Lie group structure with an accuracy approaching the machine accuracy 10-15, and what is more, the LG-GPC is even superior to RKMK and LGVI.Figure 2
Lie group structural deviation of LG-GPC when compared with LG-PC, RKMK, and LGVI.


## 3.1. Legendre-Gauss Pseudo-Spectral Collocation Method in the Euclidean Space

Before collocating the left-invariant mechanical system on SO(3), we briefly describe the basic principle of the Legendre-Gauss pseudo-spectral collocation (LG-PC) method [1]. Take, for example, solving the following differential equation in Rn at tf,(4)x˙tt=ft,xt,xt0=x0∈Rn,t0≤t≤tf.First, we equally divide a time interval[t0,tf] into several time subintervals [tk,tk+1] with the stepsize being h and transform the time interval t∈[tk,tk+1] into τ∈[-1,1] via the affine transformation t=[(tk+1-tk)·τ+(tk+1+tk)]/2. Thus, accordingly, (4) over [tk,tk+1] becomes(5)x˙ττ=tk+1-tk2fτ,xτ,-1≤τ≤1.Letτ1<⋯<τN be collocation points in [-1,1] and τ0=-1. We approximate the solution of (5) by the following formula:(6)xτ≈Xτ=∑i=0Nxi·Liτ,where Liτ=∏j=0,j≠iN(τ-τj)/(τi-τj) denotes Lagrange polynomials, satisfying the isolation property(7)Licj=δij=1,i=j0,i≠j.Equation (6) together with the isolation property leads to xi=x(τi), thereby X(τi)=x(τi).Through expressing the derivative of the Lagrange polynomials at the collocation points in differential matrix formD=[dji]∈RN×(N+1), where(8)dji=L˙icj=∑l=0N∏k=0,k≠i,lNcj-ck∏k=0,k≠iNci-ck,j=1,…,N,i=0,…,N,we can write (5) at the collocation points as a set of differential algebra equations:(9)∑i=0Ndji·Xci=tk+1-tk2fcj,Xcj,j=1,…,N.Based on the above equations, we establish the following defect equations,(10)ςj=∑i=0Ndji·Xci-tk+1-tk2fcj,Xcj,j=1,…,N,and apply iterative algorithms to the above equations so as to determine the approximation to x(τ) at the collocation points. Finally, according to the following formula, we obtain the approximation to (4) at the endpoint tk+1 of time subinterval [tk,tk+1]:(11)xtk+1≈Xtk+tk+1-tk2∑j=1Nϖj·fcj,Xcj,where ϖj=∫-11Lj(t)dt is quadrature weight corresponding to the collocation points. This approximate solution will be the initial value of x(t) over [tk+1,tk+2] and so on.Remark 2.
According to different selection methods of collocation points, pseudo-spectral collocation methods can be divided into two categories, namely, standard and orthogonal methods. Collocation points in orthogonal collocation are those obtained from the roots of either Chebyshev polynomialsTN(τ) or Legendre polynomials PN(τ) belonging to orthogonal polynomials [17]. The benefit of using orthogonal collocation over standard collocation is that the quadrature approximation to a definite integral is extremely accurate [1]. Furthermore, according to whether the endpoint is a collocation point, pseudo-spectral collocation methods fall into three general categories [18]: Gauss methods, where neither of the endpoints −1 or 1 is a collocation point; Radau methods, where at most one of the endpoints −1 or 1 is a collocation point; and Lobatto methods, where both the endpoints −1 and 1 are collocation points. In the rest of this paper, we will develop our geometric collocation method based on Legendre-Gauss points, since when Legendre-Gauss methods are used for transcribing the continuous optimal control problem into a discrete nonlinear programming problem, it does not suffer from a defect in the optimality conditions at the boundary points for the endpoints are not collocation points [15].


## 3.2. Legendre-Gauss Geometric Pseudo-Spectral Collocation Method on SO(3)

For attitude kinematics (referred to as the “configuration equation”), it is well-known that the solution of (2) stays on SO(3) for all t≥t0. How to use the pseudo-spectral collocation method to solve (2), while maintaining some exact geometric properties (such as Lie group invariants) of the configuration equation under discretization of Rab(t), is the main problem to be solved in this subsection. However, SO(3) is a nonlinear manifold, and using the pseudo-spectral collocation method directly to discretize the configuration could not preserve the geometric properties of its solution. Reference [13] indicated that any differential equation in the form of an infinitesimal generator on a homogeneous space (a manifold with a transitive Lie group action) is locally equivalent to a differential equation on a Lie algebra corresponding to the Lie group acting on the homogenous space. Also, in the context of SO(3), the Lie algebra so(3) corresponding to SO(3) is isomorphic to R3 [9–11]. For the above-mentioned reasons, the differential equation on so(3) is the natural choice of space for pseudo-spectral discretization, like many classical Lie group numerical methods. For this purpose, we transform (3) into an equivalent equation on so(3), then use the pseudo-spectral collocation method to solve the equivalent equation, and finally map its solution back to configuration space. As mentioned earlier, since the Lie algebra space is isomorphic to the Euclid space to which the attitudes dynamics belong, and before discretizing the configuration equation, we directly use the general pseudo-spectral method to collocate the angular velocities at the discrete points.


### 3.2.1. Collocation of Angular Velocities at Discrete Points

For attitude dynamics, since its variableωabb(t) belongs to so(3) that is isomorphic to R3, (3) can be collocated at Gauss collocation points directly by the Legendre-Gauss pseudo-spectral collocation method as follows:(12)ωabbτj=tk+1-tk2∑i=1Nd~ji·Fωabbτi,Rabτi,Textτi-∑i=1Nd~ji·di0·ωabbtk,j=1,…,N.Similarly, the approximate solutions ofωabb(τj), j=1,…,N, are approached by several times of the Gauss-Seidal type iterative operation. Then, the approximation solutions are used to obtain the velocity at the endpoint tk+1:(13)ωabbtk+1=ωabbtk+tk+1-tk2∑i=1Nϖi·Fωabbτi,Rabτi,Textτi.


### 3.2.2. Collocation of Configurations at Discrete Points

To transform (2) on SO(3) into an equivalent differential equation evolving on so(3), we briefly describe the basic idea of the equivalent map.Definition 3 (equivariant map [13]).
LetM and N be manifolds and let G be a Lie group which acts on M by Φg:M→M and on N by Ψg:N→N. A smooth map f:M→N is called equivariant with respect to these actions if, for all g∈G,(14)f∘Φg=Ψg∘fwhich indicates the following diagram commutes.
Diagram commutes of the equivariant map are as follows:(15)M⟶fN↑Φg14↑ΨgM⟶fN
First, from the definition of an action ofG on M, we can obtain an equivariant map Φy:G→M with respect to the left translation action Lg of G on itself and an action G of Φg on M:(16)Φy∘Lg=Φg∘Φy.
It is known that there is a local coordinate mapf:g→G on G with the Lie algebra g, and the most typical one is the exponential map exp:g→G. At this point, we need to find an action Bg of G on g such that f will be an equivariant map with Bg and the left action of G on itself, namely,(17)f∘Bg=Lg∘f.
In casef is the exponential map, Bg is nothing other than the well-known Baker-Campbell-Hausdorff (BCH) formula Bg(u)=log⁡(g·exp(u)), where log:G→g is called the logarithm map. Since the composition of two equivariant maps is also an equivariant map, we can construct an equivariant map Φy∘f from g to M with respect to the action Bg on g and Φ on M.
Diagram commutes of compositionΦy∘f are as follows:(18)g⟶fG⟶ΦyM↑Bexptξ17↑Lg16↑Φexptξg⟶fG⟶ΦyM
Theorem 3.6 in literature [13] states that if ϕ is an equivariant map, then the infinitesimal generators of the action with respect to the same element ξ∈g are ϕ-related vector fields. Thus, the infinitesimal generators ξg and ξM of the flows Bexptξ and Φexptξ, respectively, on g and M are Φy∘f-related; that is,(19)ξM∘Φy∘f=TΦy∘Tf∘ξg.
Finally, we need to determine whatξg is, and the following theorem gives the conditions that it needs to meet.Lemma 4 (see [13]).
Letf:g→G be a coordinate map on G and Φy∘f equivariant with respect to the flows Bexptξ and Φexptξ. The infinitesimal generator of Bg satisfying (19) is ξg(u)=dfu-1(υ). df:g→g is the trivialization Tf defined as dfu=TRfu-1∘Tfu.Based on Lemma4 and other previous conclusions, we can derive the equivalent equation on so3 of the left-invariant configuration equation.Proposition 5 (equivalent equation onso3 of the left-invariant configuration equation).
The configuration equation (2) has an equivalent equation on so3 as follows:(20)u˙t=df-ut-1ωabbt≔G~ωabbt,ut,tk≤t≤tk+1,where f:so3→SO3 is a local coordinate map on SO(3) with df being its differential.Proof.
Without any loss of generality, we assume that the solutionRabt of (2) can be written in the following form:(21)Rabt=Rabt0·fut.
Then, by differentiating it [20], we can obtain(22)ddtRabt=ddtRabt0·fut=Rabt0·dLfut∘df-utu˙t=Rabt0·fut·df-utu˙t.
Comparing the preceding formula with (2) and (21), we have(23)ωabbt=df-utu˙t.
Taking the inverse of the preceding formula, we obtain the equivalent equation (20) on so(3) of the left-invariant configuration equation (2).There are several choices of the coordinate mapf, such as the exponential map and Cayley map [21]. In case f is the exponential map “exp” the vector field df-u(t)-1 on so(3) can be represented as [20](24)dexp-ut-1ωabbt=ωabbt+12adutωabbt+∑k=2∞Bkk!adutkωabbt,where the adjoint operator adu:g→g is a Lie bracket or commutator, adu(ωabb(t))=[u,ωabb(t)], satisfying the recursive expression aduk(ωabb(t))=[u,aduk-1(ωabb(t))], and Bk are Bernoulli numbers [21]:(25)
B
k
=
1
6
,
-
1
30
,
1
42
,
-
1
30
,
5
66
,
-
691
2730
,
…
k
=
2
N
,
N
∈
Z
+
0
,
k
=
2
N
+
1
,
N
∈
Z
+
.Now, we solve (20) by the LG-PC method as follows:(26)uτj=tk+1-tk2∑i=1Nd~ji·G~ωabbτi,uabτi-∑i=1Nd~ji·di0·utk,j=1,…,N.In view of the mechanical system being autonomous, we may use Gauss-Seidal type iteration to update the approximate solution of (26) as soon as possible [22]. As mentioned earlier, after obtaining the approximate solution, the velocity at the endpoint tk+1 can be obtained according to the following formula:(27)utk+1=utk+tk+1-tk2∑i=1Nϖi·G~ωabbτi,uabτi.It is worth noting that, different from the LG-PC method in the Euclidean space, the initial valueu(tk) in [tk,tk+1] is not equal to the endpoint value u(tk) in [tk-1,tk]. It should be calculated through the logarithmic map log:SO(3)→so(3):(28)utk=logSO3⁡Rab-1tk·Rabtk,k=0,1,2,…with(29)logSO3⁡R=ϕ2sin⁡ϕR-RT∈so3,where ϕ satisfies cos⁡ϕ=(tr(R)-1)/2, ϕ<π and tr(R)≠-1. It is easy to know that u(tk),k=0,1,2,…, are identically equal to zero.At last, the attitude configurationRab(tk+1) at the endpoint tk+1 can be reconstructed from u(tk+1) according to Rab(tk+1)=R(tk)·f(u(tk+1)), where k∈Z+ and the Cayley map should be selected as the coordinate map f, for two reasons [11]: first, it provides only an approximation to the integral curve defined by the exponential map, which is however easy to calculate; second, it does not involve trigonometric functions so that its derivatives do not have any singularities and thus will not cause any difficulties when used by gradient-based methods for solving NLP problems. The Cayley map satisfies the following formula:(30)caySO3ωabbt=I3+44+ωabbt2ω^abbt+ω^abbt22,where · is a standard Euclidean norm.


## 3.2.1. Collocation of Angular Velocities at Discrete Points

For attitude dynamics, since its variableωabb(t) belongs to so(3) that is isomorphic to R3, (3) can be collocated at Gauss collocation points directly by the Legendre-Gauss pseudo-spectral collocation method as follows:(12)ωabbτj=tk+1-tk2∑i=1Nd~ji·Fωabbτi,Rabτi,Textτi-∑i=1Nd~ji·di0·ωabbtk,j=1,…,N.Similarly, the approximate solutions ofωabb(τj), j=1,…,N, are approached by several times of the Gauss-Seidal type iterative operation. Then, the approximation solutions are used to obtain the velocity at the endpoint tk+1:(13)ωabbtk+1=ωabbtk+tk+1-tk2∑i=1Nϖi·Fωabbτi,Rabτi,Textτi.


## 3.2.2. Collocation of Configurations at Discrete Points

To transform (2) on SO(3) into an equivalent differential equation evolving on so(3), we briefly describe the basic idea of the equivalent map.Definition 3 (equivariant map [13]).
LetM and N be manifolds and let G be a Lie group which acts on M by Φg:M→M and on N by Ψg:N→N. A smooth map f:M→N is called equivariant with respect to these actions if, for all g∈G,(14)f∘Φg=Ψg∘fwhich indicates the following diagram commutes.
Diagram commutes of the equivariant map are as follows:(15)M⟶fN↑Φg14↑ΨgM⟶fN
First, from the definition of an action ofG on M, we can obtain an equivariant map Φy:G→M with respect to the left translation action Lg of G on itself and an action G of Φg on M:(16)Φy∘Lg=Φg∘Φy.
It is known that there is a local coordinate mapf:g→G on G with the Lie algebra g, and the most typical one is the exponential map exp:g→G. At this point, we need to find an action Bg of G on g such that f will be an equivariant map with Bg and the left action of G on itself, namely,(17)f∘Bg=Lg∘f.
In casef is the exponential map, Bg is nothing other than the well-known Baker-Campbell-Hausdorff (BCH) formula Bg(u)=log⁡(g·exp(u)), where log:G→g is called the logarithm map. Since the composition of two equivariant maps is also an equivariant map, we can construct an equivariant map Φy∘f from g to M with respect to the action Bg on g and Φ on M.
Diagram commutes of compositionΦy∘f are as follows:(18)g⟶fG⟶ΦyM↑Bexptξ17↑Lg16↑Φexptξg⟶fG⟶ΦyM
Theorem 3.6 in literature [13] states that if ϕ is an equivariant map, then the infinitesimal generators of the action with respect to the same element ξ∈g are ϕ-related vector fields. Thus, the infinitesimal generators ξg and ξM of the flows Bexptξ and Φexptξ, respectively, on g and M are Φy∘f-related; that is,(19)ξM∘Φy∘f=TΦy∘Tf∘ξg.
Finally, we need to determine whatξg is, and the following theorem gives the conditions that it needs to meet.Lemma 4 (see [13]).
Letf:g→G be a coordinate map on G and Φy∘f equivariant with respect to the flows Bexptξ and Φexptξ. The infinitesimal generator of Bg satisfying (19) is ξg(u)=dfu-1(υ). df:g→g is the trivialization Tf defined as dfu=TRfu-1∘Tfu.Based on Lemma4 and other previous conclusions, we can derive the equivalent equation on so3 of the left-invariant configuration equation.Proposition 5 (equivalent equation onso3 of the left-invariant configuration equation).
The configuration equation (2) has an equivalent equation on so3 as follows:(20)u˙t=df-ut-1ωabbt≔G~ωabbt,ut,tk≤t≤tk+1,where f:so3→SO3 is a local coordinate map on SO(3) with df being its differential.Proof.
Without any loss of generality, we assume that the solutionRabt of (2) can be written in the following form:(21)Rabt=Rabt0·fut.
Then, by differentiating it [20], we can obtain(22)ddtRabt=ddtRabt0·fut=Rabt0·dLfut∘df-utu˙t=Rabt0·fut·df-utu˙t.
Comparing the preceding formula with (2) and (21), we have(23)ωabbt=df-utu˙t.
Taking the inverse of the preceding formula, we obtain the equivalent equation (20) on so(3) of the left-invariant configuration equation (2).There are several choices of the coordinate mapf, such as the exponential map and Cayley map [21]. In case f is the exponential map “exp” the vector field df-u(t)-1 on so(3) can be represented as [20](24)dexp-ut-1ωabbt=ωabbt+12adutωabbt+∑k=2∞Bkk!adutkωabbt,where the adjoint operator adu:g→g is a Lie bracket or commutator, adu(ωabb(t))=[u,ωabb(t)], satisfying the recursive expression aduk(ωabb(t))=[u,aduk-1(ωabb(t))], and Bk are Bernoulli numbers [21]:(25)
B
k
=
1
6
,
-
1
30
,
1
42
,
-
1
30
,
5
66
,
-
691
2730
,
…
k
=
2
N
,
N
∈
Z
+
0
,
k
=
2
N
+
1
,
N
∈
Z
+
.Now, we solve (20) by the LG-PC method as follows:(26)uτj=tk+1-tk2∑i=1Nd~ji·G~ωabbτi,uabτi-∑i=1Nd~ji·di0·utk,j=1,…,N.In view of the mechanical system being autonomous, we may use Gauss-Seidal type iteration to update the approximate solution of (26) as soon as possible [22]. As mentioned earlier, after obtaining the approximate solution, the velocity at the endpoint tk+1 can be obtained according to the following formula:(27)utk+1=utk+tk+1-tk2∑i=1Nϖi·G~ωabbτi,uabτi.It is worth noting that, different from the LG-PC method in the Euclidean space, the initial valueu(tk) in [tk,tk+1] is not equal to the endpoint value u(tk) in [tk-1,tk]. It should be calculated through the logarithmic map log:SO(3)→so(3):(28)utk=logSO3⁡Rab-1tk·Rabtk,k=0,1,2,…with(29)logSO3⁡R=ϕ2sin⁡ϕR-RT∈so3,where ϕ satisfies cos⁡ϕ=(tr(R)-1)/2, ϕ<π and tr(R)≠-1. It is easy to know that u(tk),k=0,1,2,…, are identically equal to zero.At last, the attitude configurationRab(tk+1) at the endpoint tk+1 can be reconstructed from u(tk+1) according to Rab(tk+1)=R(tk)·f(u(tk+1)), where k∈Z+ and the Cayley map should be selected as the coordinate map f, for two reasons [11]: first, it provides only an approximation to the integral curve defined by the exponential map, which is however easy to calculate; second, it does not involve trigonometric functions so that its derivatives do not have any singularities and thus will not cause any difficulties when used by gradient-based methods for solving NLP problems. The Cayley map satisfies the following formula:(30)caySO3ωabbt=I3+44+ωabbt2ω^abbt+ω^abbt22,where · is a standard Euclidean norm.


## 3.3. Algorithm of the Legendre-Gauss Geometric Pseudo-Spectral Collocation Method on SO(3) with Application to Attitude Motion of a Rigid Body

In accordance with the aforementioned LG-GPC method, we develop a Legendre-Gauss geometric pseudo-spectral collocation algorithm for integrating the kinematics and dynamics of attitude motion of a rigid body on SO(3). This algorithm would be further used to discretize the OAC problem in Section4.Algorithm 6 (LG-GPC).
Inputs include time interval[t0,tf], stepsize h, number of LG points N, initial attitude configuration Rab(t0), initial angular velocity ωabb(t0), iterations of inner while loop num, and threshold of equivalent configuration (or velocity) deviation err.
Step 1. Initialization is as follows: let k be zero.
Step 2. Compute initial value of the equivalent configuration at t0 in terms of Rabt0:(31)ut0=logSO3Rab-1t0·Rabt0≡0.
Step 3 (while (tk<tf)).
Step 3.1. Compute the LG points τ1,…,τN in [tk,tk+1], differentiation matrix DN×N+1, and quadrature weights ϖ1,…,ϖN.
Step 3.2. Set the current iterative step number n=0 and configuration (or velocity) deviation e=1.
Step 3.3. Let ωabbnτi,i=1,…,N, be ωabbtk let and unτi,i=1,…,N be utk.
Step 3.4 (while (n<num and e>err)).
Step 3.4.1. Obtain the submatrix D~N×N of the matrix DN×N+1:(32)D~=D:,2:N+1.
Step 3.4.2. According to the following equations, obtain [ω˙abbn(τ1),…,ω˙abbnτN]T:(33)ω˙abbnτ1⋮ω˙abbnτN=tk+1-tk2Fωabbnτ1,Rabnτ1,Textτ1⋮FωabbnτN,RabnτN,TextτN-d01⋮d0Nωabbtk.
Step 3.4.3. Update angular velocities [ωabbn+1τ1,…,ωabbn+1τN]T at LG points:(34)ωabbn+1τ1⋮ωabbn+1τN=D~-1·ω˙abbnτ1⋮ω˙abbnτN.
Step 3.4.4. According to the following equations, obtain [u˙nτ1,…,u˙nτN]T:(35)u˙nτ1⋮u˙nτN=tk+1-tk2G~ωabbnτ1,unτ1⋮G~ωabbnτN,unτN-d01⋮d0Nutk.
Step 3.4.5. Update the equivalent configuration [un+1τ1,…,un+1τN]T at LG points:(36)un+1τ1⋮un+1τN=D~-1·u˙nτ1⋮u˙nτN.
Step 3.4.6. Update the configuration [Rabn+1τ1,…,Rabn+1τN]T at LG points:(37)Rabn+1τ1⋮Rabn+1τN=Rabtk·cayun+1τ1⋮Rabtk·cayun+1τN.
Step 3.4.7. Compute the equivalent configuration deviation and the velocity deviation within two adjacent iterative steps, and, then, let the larger one be the final deviation e at the current step:(38)e=maxabsdetRabn+1τ1TRabnτ1⋮detRabn+1τNTRabnτN∞-1,ωabbn+1τ1-ωabbnτ12⋮ωabbn+1τN-ωabbnτN2∞.
Step 3.4.8. Substitute ωabbn+1τi, un+1τi, and Rabn+1τi into ωabbnτi, unτi, and Rabnτi, respectively, where i=1,…,N, and let n=n+1.
Step 3.5.End while.
Step 3.6. Relying on the final iterative values ωabbnτi, Rabnτi with Textτi, i=1,…,N in inner while loop, compute the angular velocity ωabbtk+1 at the endpoint tk+1:(39)ωabbtk+1=ωabbtk+tk+1-tk2∑i=1Nϖi·Fωabbnτi,Rabnτi,Textτi.
Step 3.7. Also, based on the final iterative values ωabbnτi, unτi, i=1,…,N in the inner while loop, compute the equivalent configuration utk+1 at the endpoint tk+1:(40)utk+1=utk+tk+1-tk2∑i=1Nϖi·G~ωabbnτi,unτi.
Step 3.8. Based on the previous formula, compute the configuration Rabtk+1 at the endpoint tk+1:(41)Rabtk+1=Rabtk·cayutk+1.
Step 3.9. Let tk=tk+1, k=k+1, and utk=0.Step 4.End while.Remark 7.
The LG pointsτi, i=1,…,N, are the roots of the Legendre polynomial PNτ=dN[(τ2-1)N]/[(2NN!)dτN] of degree N. In fact, it is convenient to compute the LG points via the eigenvalues of the following tridiagonal Jacobi matrix:(42)J=0β1β10β2⋱⋱⋱⋱0βN-1βN-10,βn=n4n2-1.
The LG pseudo-spectral differentiation matrixD=[dji]∈RN×(N+1) is given as [22](43)
d
j
i
=
1
+
τ
j
P
˙
N
τ
j
+
P
N
τ
j
τ
j
-
τ
i
1
+
τ
i
P
˙
N
τ
i
+
P
N
τ
i
,
i
≠
j
1
+
τ
i
P
¨
N
τ
i
+
2
P
˙
N
τ
i
2
1
+
τ
i
P
˙
N
τ
i
+
P
N
τ
i
,
i
=
jwith(44)P˙N=-N+1PNτ1-τ2.
The LG weights corresponding to the LG points are computed as(45)ϖi=21-τi2P˙N2τi.Remark 8.
In Step 3.3, bothustepτ1 and ustepτ2 cannot be set to utk, because utk shows the projection of distance between Rabtk-1 and Rabtk on the tangent space.


## 3.4. Numerical Integration Tests of the LG-GPC Method

In this subsection, we will test the validity and efficiency of the proposed method through numerical integration of the following free-floating rigid body kinematics (46) and dynamics (47) on SO(3). Note that (46) and (47) are equivalent to (6) and (7) under the condition of zero external force. Table 1 summarizes the parameters used by numerical tests. Consider(46)R˙abt=Rabt·ω^abbt≔Gωabbt,Rabt,(47)ω˙abbt=J-1Jωabbt×ωabbt≔Fωabbt,Rabt.Table 1
Parameters of numerical integration tests [19].
Physical quantity
Symbol
Value
Initial time
t
0
0 s
Terminal time
t
f
240 s
Moment of inertia
J
diag
⁡
1,2.8,2kg⋅m2
Initial configuration
R
a
b
t
0
I
3
×
3
Initial angular velocity
ω
a
b
b
t
0
0.5
,
-
0.5,0.4
T rad/sTo ensure that numerical integration test is more comparable, we consider the 4th-order LG-GPC method and select three other 4th-order numerical methods for comparison, including ode45 [23], explicit 4th-order RKMK [16], and 4th-order LG-PC method [22]. Furthermore, we compare the proposed numerical method with the Lie group variational integrator (LGVI) which is also based on geometrically exact computations on SO(3) [19]. Among the previous methods, ode45 is the built-in integrator of MATLAB, which is a variable-step method and whose results would be used as the ground truth with a specified tolerance 10-14. RKMK is a Lie group method [16]. Note that [24] states that the interpolatory quadrature formula with N Gauss points is of 2Nth order. To this end, both the 4th-order LG-GPC method and the LG-PC method depend on 2 LG points. In addition, in the LG-GPC and LG-PC methods, the iterations of the inner while loop num and the threshold of the equivalent configuration (or velocity) deviation err are set to be 30 and 10-14, respectively. Since LGVI has an implicit equation, we need to solve it by using the Newton iterative approach, where the termination error of iteration and the maximum estimated value of the function are set to be 1e-14 and 9000, respectively. Numerical integration tests are all carried out in the same stepsize.Figure1 gives the angular configuration deviation and the average runtime per update of LG-GPC method when compared with LG-PC, RKMK, and LGVI, respectively, at five different steps. Figure 1(a) shows the angular configuration deviation against steps by calculating logSO(3)(Rode45TRab)2, where Rode45 denotes the angular configuration of the free-floating rigid body which is computed by ode45. It has been illustrated that the accuracy of LG-GPC is superior to those of LG-PC, RKMK, and LGVI. Figure 1(b) shows the average runtime per update of LG-GPC methods when compared with LG-PC, RKMK, and LGVI. As it can be seen, the computational efficiency of LG-GPC is inferior to that of RKMK, because RKMK is an explicit method. However, the proposed method requires less computation time than the classical LG-PC method and is nearly 1.5 times as efficient as the LG-PC method on average. More obviously, the computational efficiency of the proposed method is superior to that of LGVI, since the latter needs the Newton iteration approach to solve the implicit approach.Figure 1
Accuracy and computational efficiency of LG-GPC when compared with LG-PC and RKMK.
(a)
Average orientation deviation versus steps
(b)
Average runtime per updateAdditionally, the Lie group structural conservativeness of LG-GPC is compared with those of LG-PC, RKMK, and LGVI. Considering that the angular configuration is parameterized by unit quaternion in the ode45 method, we only compare the proposed method with the LG-PC method, explicit RKMK, and LGVI. As mentioned above, since the group element on SO(3) satisfiesRabRabT=I, we compute the infinite norm of RabRabT-I3×3 for evaluating the structural conservativeness [19]. Figure 2 illustrates that the LG-PC method belonging to the Euclidean space has no group structural conservativeness, because it is not a Lie group method. However, RKMK, LGVI, and LG-GPC method on the Lie group are able to preserve the Lie group structure with an accuracy approaching the machine accuracy 10-15, and what is more, the LG-GPC is even superior to RKMK and LGVI.Figure 2
Lie group structural deviation of LG-GPC when compared with LG-PC, RKMK, and LGVI.


## 4. OAC on SO(3) Based on LG-GPC Discretization

### 4.1. Description of the OAC Problem on SO(3)

Consider the following OAC problem with differential kinematical and dynamical constraints on SO(3).Problem 9 (OAC problem with kinematical and dynamical constraints on SO(3)).
Minimize the continuous-time cost(48)J=12∫t0tfut,PutRmdt,where ut∈Rm is the control vector and P∈Rm×m is the weighting matrix, where m≤3,(1)
satisfying the differential kinematical and dynamical constraints on SO(3),(49)R˙abt=Rabt·ω^abbt≔Gωabbt,Rabt,ω˙abbt=J-1Jωabbt×ωabbt+Rab∗t∂RabtVRabt+Textt≔Fωabbt,Rabt,Textt(2)
and satisfying the boundary conditions(50)Rabt0=R0,ωabbt0=ω0,Rabtf=Rf,ωabbtf=ωf,where R0, ω0, Rf, and ωf denote the initial angular configuration, the initial angular velocity, the terminal angular configuration, and the terminal angular velocity, respectively.


### 4.2. Transcription of the OAC on SO(3) into an NLP Problem Based on LG-GPC Discretization

Different from the LG-PC direct transcription of an NLP problem in the Euclidean space [25], we transcribe the continuous Problem 9 into a discrete NLP problem by LG-GPC discretization. In general, an NLP problem has a standard form as follows.Minimize the cost(51)fzsatisfying the equality and the inequality as follows:(52)zmin≤z≤zmax,cEz=0,cl,min≤clz≤cl,max,where the vector z∈Rnx is the whole decision variable of the NLP problem, cE(z) denotes the whole equality constraints, and cl(z) denotes the whole inequality constraints. Denote(53)cz≔cEzclzwith the constraints vector of the NLP problem.For this purpose, we refer to the LG-PC transcription process introduced in [25], for transcribing Problem 9 into an NLP problem. Thus, the OAC problem on SO(3) could be formulated as follows.Algorithm 10 (transcription of the OAC problem on SO(3) based on LG-GPC discretization).
Step 1. Mapping of the decision variables.
The total vector of optimization variablesz∈Rnx(N+2)+nuN is given as(54)z=zx,zuT,where zx∈Rnxnx=6 is given as(55)zx=χ1⋮χnx,where the column vectors χj are given as(56)χj=χj,1⋮χj,N+2,j=1,…,nx,where χj,kj=1,…,nx;k=1,…,N+2 is the value of the jth component of states at the kth discretization point.
Similarly,zu∈Rnunu=m is given as(57)zu=σ1⋮σnu,where the column vectors σj are given as(58)σj=uj,1⋮uj,N,j=1,…,nu,where uj,kj=1,…,nu;k=1,…,N is the value of the jth component of the control at the kth LG collocation point.Step 2. Construction of the constraint vector cz.Step 2.1. Construction of the defect constraints cEz.Forωabb, refer to (12) and (13), and define the defect constraints δkk=1,…,N+1 as(59)δk=∑i=0Ndkiωabbτi-tf-t02Fωabbτk,Rabτk,Textτk,k=1,…,N,δN+1=ωabbtf-ωabbt0-∑i=0N∑k=1Nϖkdkiωabbτi.For configurationRab, refer to (26) and (27), and define the other part of the defect constraints δk′k=1,…,N+1 as(60)uτi=cay-1Rabtk-1Rabτi,i=1,…,N,δk′=∑i=0Ndkiuτi-tf-t02G~ωabbτk,uτk,k=1,…,N,δN+1′=utf-ut0-∑i=0N∑k=1Nϖkdkiωabbτi.Then we can define the matrixΔ as(61)Δ=δ1Tδ1′T⋮⋮δN+1TδN+1′TN+1×6.The matrixΔ can be then isomorphically mapped to the column vector of equality constraints using a pseudocoded version of the MATLAB column-stacking operator “:” as Δ:=cEz.Step 2.2. Construction of the constraint vector cz, without inequality constraints.We can represent the column vectorcz as(62)cz=cEz.Step 3. Automatic scaling of the NLP.Step 4. Conversion of the NLP sparsity pattern.Step 5. Separation of constraint and nonconstraint derivatives in the constraint Jacobian.Step 6. Relation of the Lagrange multiplies of NLP with the costate of the continuous-time optimal control problem using costate mapping.Step 7. Since solving the NLP from the geometric pseudo-spectral method results in controls at only the interior points, it is necessary to obtain the endpoint controls after the NLP is solved. The method for endpoint control computation is to employ the Pontryagin minimum principle using the endpoint values of the state and costate.Remark 11.
Since Steps 3~7 are more or less the same as those of [25], the repetitious details need not be given here.


### 4.3. Simulation Example

In this section, we take controlling an underactuated 3D pendulum as an example to examine the performance of the previous method. The 3D pendulum (illustrated in Figure3) is, simply, a rigid body, supported at a fixed pivot point, which has three rotational degrees of freedom; it is acted on by a uniform gravity force and, perhaps, by control and disturbance forces and moments [26].Figure 3
A schematic of a 3D rigid pendulum.The attitude kinematics and dynamics of the pendulum on SO(3) are given by(63)R˙abt=Rabt·ω^abbt,ω˙abbt=J-1Jωabbt×ωabbt+mgρ×RabTte3+But,where the attitude configuration Rabt is a generalized coordinate of the rotation angular configuration of the body-fixed frame O-X′Y′Z′ relative to the inertial frame O-XYZ, satisfying(64)Rab=cos⁡θcos⁡ψ-cos⁡ϕsin⁡ψ+sin⁡ϕsin⁡θcos⁡ψsin⁡ϕsin⁡ψ+cos⁡ϕsin⁡θcos⁡ψcos⁡θsin⁡ψcos⁡ϕcos⁡ψ+sin⁡ϕcos⁡θsin⁡ψ-sin⁡ϕcos⁡ψ+cos⁡ϕsin⁡θsin⁡ψ-sin⁡θsin⁡ϕcos⁡θcos⁡ϕcos⁡θ,where ϕ,θ,ψ denote the Euler angles and the angular velocity vector ωabbt in the body-fixed frame could be represented as p,q,rT, in which p,q,r denote the angular velocity component, respectively, about X-, Y-, and Z-body axes. Thus, ω^abbt is equal to 0,-r,q;r,0,-p;-q,p,0. The conservative force Rab∗t∂RabtVRabt could be easily obtained as mgρ×RabTte3, where m is the mass of the pendulum and ρ is body-fixed vector from the pivot to the center of mass of the pendulum. e3=0,0,1T denotes the direction vector of gravity in the inertial coordinate frame. The nonconservative force Textt satisfies But with B being the constant conjugate vector and the control vector ut could be represented as u1,u2T, in which u1,u2 denote the control components about X- and Y-body axes, respectively. The detailed properties of the 3D pendulum are presented in Table 2.Table 2
Properties of a 3D rigid pendulum [7].
Physical quantity
Symbol
Value
Mass
m
1 kg
Moment of inertia
J
0.156
0
0
0
0.156
0
0
0
0.3 kg⋅m2
Vector of pivot
ρ
0,0
,
0.75
T m
Conjugate vector
B
1
0
0
0
1
0
T
Weighting matrix
P
I
2
×
2Consider the OAC problem of the 3D pendulum on SO(3), which is to control the pendulum from the initial fixed angleϕ0,θ0,ψ0 and the initial fixed angular velocity p0,q0,r0 to the terminal fixed angle ϕf,θf,ψf and the terminal fixed angular velocity pf,qf,rf with the minimum moment of the external force. The simulation parameters of the OAC problem are presented in Table 3.Table 3
The simulation parameters of the OAC problem.
Decision variable
Initial value
Terminal value
Boundary condition
Initial guess
ϕ (rad)
0
π
[
-
π
,
π
]
0
θ (rad)
0
0
[
-
π
,
π
]
0
ψ (rad)
0
π/2
[
-
π
,
π
]
0
p (rad/s)
0
0
[
-
π
,
π
]
0
q (rad/s)
0
0
[
-
π
,
π
]
0
r (rad/s)
0
0
[
-
π
,
π
]
0
u
1 (N⋅m)
—
—
[
-
5,5
]
0
u
2 (N⋅m)
—
—
[
-
5,5
]
0The proposed optimal control method (LG-GPC) is also compared with the prior works in [7, 9–11], which are also based on geometrically exact computations (Lie group variational integrator) on SO(3). There are two major categories of methods for solving the discrete first-order optimal necessary conditions derived from the optimal control problem: the nonlinear root-finding method (NRF) and the shooting method. In terms of the former, Bloch et al. [9] and Hussein et al. [10] propose a geometric structure-preserving optimal control method on SO(3), which applies a standard Newton iterative approach to solve the discrete first-order necessary optimality conditions. However, this method does not consider the moment of the conservative force and therefore could not be compared with our method immediately. To this end, we resort to a more general discrete geometric optimal control method on Lie groups proposed in [11], which consider both the moment of conservative force and that of external nonconservative force. In this method, for the 3D pendulum, the dimension of discrete decision variables is 32N+1 with the discretization number N being 15 and the maximum iterative number being 2e4. Termination tolerances on the constraint violation, the function value, and argument are 1e-6, 1e-4, and 1e-6, respectively. In terms of the latter, Lee et al. [7] use a line search with the backtracking algorithm, referred to as “Newton-Armijo iteration (NAI),” to solve the discrete first-order necessary optimality conditions, where the time stepsize is h=0.001 s and the number of integration steps is N=1000. Figure 4 gives the optimal states and controls obtained from the LG-GPC method when compared with LG-PC and NRF.Figure 4
The optimal state and control of the pendulum obtained by LG-GPC, LG-PC, and NRF.
(a)
Angular configuration versus time
(b)
Angular configuration versus time
(c)
Angular velocity versus time
(d)
Angular velocity versus time
(e)
Control versus time
(f)
Control versus timeAs seen from Figure4, the results obtained from LG-GPC and LG-PC satisfy the boundary condition of states and control, except those of NRF, because the first two could consider constraints of states and control explicitly, the latter has no choice but to introduce a dummy factor. Table 4 illustrates the optimal costs J and operation time t of LG-GPC, LG-PC, NRF, and NAI. It is concluded from the optimal costs that the methods based on LGVI are superior to LG-GPC and LG-PC, since the former belong to indirect method that is higher in accuracy. However, the last two have not only larger convergence radius, but also higher convergence rate. With the numerical advantage of the geometric collocation discretization, LG-GPC has the least operation time.Table 4
Optimized performance index and operation time.
LG-GPC
LG-PC
NRF
NAI
J
11.3466
10.9628
2.1044
1.52
t
4.3838
4.8782
154.7895
—


## 4.1. Description of the OAC Problem on SO(3)

Consider the following OAC problem with differential kinematical and dynamical constraints on SO(3).Problem 9 (OAC problem with kinematical and dynamical constraints on SO(3)).
Minimize the continuous-time cost(48)J=12∫t0tfut,PutRmdt,where ut∈Rm is the control vector and P∈Rm×m is the weighting matrix, where m≤3,(1)
satisfying the differential kinematical and dynamical constraints on SO(3),(49)R˙abt=Rabt·ω^abbt≔Gωabbt,Rabt,ω˙abbt=J-1Jωabbt×ωabbt+Rab∗t∂RabtVRabt+Textt≔Fωabbt,Rabt,Textt(2)
and satisfying the boundary conditions(50)Rabt0=R0,ωabbt0=ω0,Rabtf=Rf,ωabbtf=ωf,where R0, ω0, Rf, and ωf denote the initial angular configuration, the initial angular velocity, the terminal angular configuration, and the terminal angular velocity, respectively.


## 4.2. Transcription of the OAC on SO(3) into an NLP Problem Based on LG-GPC Discretization

Different from the LG-PC direct transcription of an NLP problem in the Euclidean space [25], we transcribe the continuous Problem 9 into a discrete NLP problem by LG-GPC discretization. In general, an NLP problem has a standard form as follows.Minimize the cost(51)fzsatisfying the equality and the inequality as follows:(52)zmin≤z≤zmax,cEz=0,cl,min≤clz≤cl,max,where the vector z∈Rnx is the whole decision variable of the NLP problem, cE(z) denotes the whole equality constraints, and cl(z) denotes the whole inequality constraints. Denote(53)cz≔cEzclzwith the constraints vector of the NLP problem.For this purpose, we refer to the LG-PC transcription process introduced in [25], for transcribing Problem 9 into an NLP problem. Thus, the OAC problem on SO(3) could be formulated as follows.Algorithm 10 (transcription of the OAC problem on SO(3) based on LG-GPC discretization).
Step 1. Mapping of the decision variables.
The total vector of optimization variablesz∈Rnx(N+2)+nuN is given as(54)z=zx,zuT,where zx∈Rnxnx=6 is given as(55)zx=χ1⋮χnx,where the column vectors χj are given as(56)χj=χj,1⋮χj,N+2,j=1,…,nx,where χj,kj=1,…,nx;k=1,…,N+2 is the value of the jth component of states at the kth discretization point.
Similarly,zu∈Rnunu=m is given as(57)zu=σ1⋮σnu,where the column vectors σj are given as(58)σj=uj,1⋮uj,N,j=1,…,nu,where uj,kj=1,…,nu;k=1,…,N is the value of the jth component of the control at the kth LG collocation point.Step 2. Construction of the constraint vector cz.Step 2.1. Construction of the defect constraints cEz.Forωabb, refer to (12) and (13), and define the defect constraints δkk=1,…,N+1 as(59)δk=∑i=0Ndkiωabbτi-tf-t02Fωabbτk,Rabτk,Textτk,k=1,…,N,δN+1=ωabbtf-ωabbt0-∑i=0N∑k=1Nϖkdkiωabbτi.For configurationRab, refer to (26) and (27), and define the other part of the defect constraints δk′k=1,…,N+1 as(60)uτi=cay-1Rabtk-1Rabτi,i=1,…,N,δk′=∑i=0Ndkiuτi-tf-t02G~ωabbτk,uτk,k=1,…,N,δN+1′=utf-ut0-∑i=0N∑k=1Nϖkdkiωabbτi.Then we can define the matrixΔ as(61)Δ=δ1Tδ1′T⋮⋮δN+1TδN+1′TN+1×6.The matrixΔ can be then isomorphically mapped to the column vector of equality constraints using a pseudocoded version of the MATLAB column-stacking operator “:” as Δ:=cEz.Step 2.2. Construction of the constraint vector cz, without inequality constraints.We can represent the column vectorcz as(62)cz=cEz.Step 3. Automatic scaling of the NLP.Step 4. Conversion of the NLP sparsity pattern.Step 5. Separation of constraint and nonconstraint derivatives in the constraint Jacobian.Step 6. Relation of the Lagrange multiplies of NLP with the costate of the continuous-time optimal control problem using costate mapping.Step 7. Since solving the NLP from the geometric pseudo-spectral method results in controls at only the interior points, it is necessary to obtain the endpoint controls after the NLP is solved. The method for endpoint control computation is to employ the Pontryagin minimum principle using the endpoint values of the state and costate.Remark 11.
Since Steps 3~7 are more or less the same as those of [25], the repetitious details need not be given here.


## 4.3. Simulation Example

In this section, we take controlling an underactuated 3D pendulum as an example to examine the performance of the previous method. The 3D pendulum (illustrated in Figure3) is, simply, a rigid body, supported at a fixed pivot point, which has three rotational degrees of freedom; it is acted on by a uniform gravity force and, perhaps, by control and disturbance forces and moments [26].Figure 3
A schematic of a 3D rigid pendulum.The attitude kinematics and dynamics of the pendulum on SO(3) are given by(63)R˙abt=Rabt·ω^abbt,ω˙abbt=J-1Jωabbt×ωabbt+mgρ×RabTte3+But,where the attitude configuration Rabt is a generalized coordinate of the rotation angular configuration of the body-fixed frame O-X′Y′Z′ relative to the inertial frame O-XYZ, satisfying(64)Rab=cos⁡θcos⁡ψ-cos⁡ϕsin⁡ψ+sin⁡ϕsin⁡θcos⁡ψsin⁡ϕsin⁡ψ+cos⁡ϕsin⁡θcos⁡ψcos⁡θsin⁡ψcos⁡ϕcos⁡ψ+sin⁡ϕcos⁡θsin⁡ψ-sin⁡ϕcos⁡ψ+cos⁡ϕsin⁡θsin⁡ψ-sin⁡θsin⁡ϕcos⁡θcos⁡ϕcos⁡θ,where ϕ,θ,ψ denote the Euler angles and the angular velocity vector ωabbt in the body-fixed frame could be represented as p,q,rT, in which p,q,r denote the angular velocity component, respectively, about X-, Y-, and Z-body axes. Thus, ω^abbt is equal to 0,-r,q;r,0,-p;-q,p,0. The conservative force Rab∗t∂RabtVRabt could be easily obtained as mgρ×RabTte3, where m is the mass of the pendulum and ρ is body-fixed vector from the pivot to the center of mass of the pendulum. e3=0,0,1T denotes the direction vector of gravity in the inertial coordinate frame. The nonconservative force Textt satisfies But with B being the constant conjugate vector and the control vector ut could be represented as u1,u2T, in which u1,u2 denote the control components about X- and Y-body axes, respectively. The detailed properties of the 3D pendulum are presented in Table 2.Table 2
Properties of a 3D rigid pendulum [7].
Physical quantity
Symbol
Value
Mass
m
1 kg
Moment of inertia
J
0.156
0
0
0
0.156
0
0
0
0.3 kg⋅m2
Vector of pivot
ρ
0,0
,
0.75
T m
Conjugate vector
B
1
0
0
0
1
0
T
Weighting matrix
P
I
2
×
2Consider the OAC problem of the 3D pendulum on SO(3), which is to control the pendulum from the initial fixed angleϕ0,θ0,ψ0 and the initial fixed angular velocity p0,q0,r0 to the terminal fixed angle ϕf,θf,ψf and the terminal fixed angular velocity pf,qf,rf with the minimum moment of the external force. The simulation parameters of the OAC problem are presented in Table 3.Table 3
The simulation parameters of the OAC problem.
Decision variable
Initial value
Terminal value
Boundary condition
Initial guess
ϕ (rad)
0
π
[
-
π
,
π
]
0
θ (rad)
0
0
[
-
π
,
π
]
0
ψ (rad)
0
π/2
[
-
π
,
π
]
0
p (rad/s)
0
0
[
-
π
,
π
]
0
q (rad/s)
0
0
[
-
π
,
π
]
0
r (rad/s)
0
0
[
-
π
,
π
]
0
u
1 (N⋅m)
—
—
[
-
5,5
]
0
u
2 (N⋅m)
—
—
[
-
5,5
]
0The proposed optimal control method (LG-GPC) is also compared with the prior works in [7, 9–11], which are also based on geometrically exact computations (Lie group variational integrator) on SO(3). There are two major categories of methods for solving the discrete first-order optimal necessary conditions derived from the optimal control problem: the nonlinear root-finding method (NRF) and the shooting method. In terms of the former, Bloch et al. [9] and Hussein et al. [10] propose a geometric structure-preserving optimal control method on SO(3), which applies a standard Newton iterative approach to solve the discrete first-order necessary optimality conditions. However, this method does not consider the moment of the conservative force and therefore could not be compared with our method immediately. To this end, we resort to a more general discrete geometric optimal control method on Lie groups proposed in [11], which consider both the moment of conservative force and that of external nonconservative force. In this method, for the 3D pendulum, the dimension of discrete decision variables is 32N+1 with the discretization number N being 15 and the maximum iterative number being 2e4. Termination tolerances on the constraint violation, the function value, and argument are 1e-6, 1e-4, and 1e-6, respectively. In terms of the latter, Lee et al. [7] use a line search with the backtracking algorithm, referred to as “Newton-Armijo iteration (NAI),” to solve the discrete first-order necessary optimality conditions, where the time stepsize is h=0.001 s and the number of integration steps is N=1000. Figure 4 gives the optimal states and controls obtained from the LG-GPC method when compared with LG-PC and NRF.Figure 4
The optimal state and control of the pendulum obtained by LG-GPC, LG-PC, and NRF.
(a)
Angular configuration versus time
(b)
Angular configuration versus time
(c)
Angular velocity versus time
(d)
Angular velocity versus time
(e)
Control versus time
(f)
Control versus timeAs seen from Figure4, the results obtained from LG-GPC and LG-PC satisfy the boundary condition of states and control, except those of NRF, because the first two could consider constraints of states and control explicitly, the latter has no choice but to introduce a dummy factor. Table 4 illustrates the optimal costs J and operation time t of LG-GPC, LG-PC, NRF, and NAI. It is concluded from the optimal costs that the methods based on LGVI are superior to LG-GPC and LG-PC, since the former belong to indirect method that is higher in accuracy. However, the last two have not only larger convergence radius, but also higher convergence rate. With the numerical advantage of the geometric collocation discretization, LG-GPC has the least operation time.Table 4
Optimized performance index and operation time.
LG-GPC
LG-PC
NRF
NAI
J
11.3466
10.9628
2.1044
1.52
t
4.3838
4.8782
154.7895
—


## 5. Conclusions

This paper shows how to apply the pseudo-spectral collocation method to a constrained OAC problem for a rigid body on SO(3). First, we establish a left-invariant rigid-body attitude dynamical model in a body-fixed frame on SO(3). Then, we propose a geometric collocation method which has a comprehensive advantage in accuracy, computational efficiency, and the Lie group structural conservativeness. Preservation of key motion invariance leads to robust dynamics approximation. Finally, the proposed method is applied to a constrained OAC problem.For future work, we will further compare the method with more typical numerical methods in the Euclidean space and on the Lie group, to test the efficiency of our method. Moreover, we will extend the proposed method to a broader category of left-invariant rigid-body dynamics systems in engineering.



---

*Source: 790409-2015-12-07.xml*