# Geometric Pseudospectral Method on SE(3) for Rigid-Body Dynamics with Application to Aircraft

Authors: Jie Li; Honglei An; Huayong Zhu; Lincheng Shen; Bin Fang
Journal: Mathematical Problems in Engineering
 (2013)

---

## Abstract

General pseudospectral method is extended to the special Euclidean group SE(3) by virtue of equivariant map for rigid-body dynamics of the aircraft. On SE(3), a complete left invariant rigid-body dynamics model of the aircraft in body-fixed frame is established, including configuration model and velocity model. For the left invariance of the configuration model, equivalent Lie algebra equation corresponding to the configuration equation is derived based on the left-trivialized tangent of local coordinate map, and the top eight orders truncated Magnus series expansion with its coefficients of the solution of the equivalent Lie algebra equation are given. A numerical method called geometric pseudospectral method is developed, which, respectively, computes configurations and velocities at the collocation points and the endpoint based on two different collocation strategies. Through numerical tests on a free-floating rigid-body dynamics compared with several same order classical methods in Euclidean space and Lie group, it is found that the proposed method has higher accuracy, satisfying computational efficiency, stable Lie group structural conservativeness. Finally, how to apply the previous discretization scheme to rigid-body dynamics simulation and control of the aircraft is illustrated.


---

## Body

## 1. Introduction

The aircraft is usually regarded as a single rigid body in three-dimensional space. The dynamics of a rigid body is an important problem modeling the time evolution of aircraft and other vehicles [1]. In particular, in aircraft simulation and control, its authenticity affects the fidelity of the simulation and also determines whether it can truly reflect the performance of the designed controller to a certain extent. The rigid-body dynamics has fundamental motion invariants; for example, the flow of a Hamiltonian system is symplectic [2, 3], the total energy of a rigid body is conserved in the absence of nonconservative forces [4], and the momentum map associated with a symmetry of a rigid body is preserved [5]. Furthermore, the exact flow of a rigid body always stays on a configuration manifold, since the combined configurations of the translation and rotation of the rigid body construct a special Lie group called the special Euclidean group SE(3) [6]. The invariants often manifest through geometric characteristics of exact flow of a rigid body, such as symplecticity, first integrals, symmetry, and Lie group structure. However, for most rigid bodies, obtaining the analytical solution is quite difficult. Thus, under the condition of non analytical solution, it is desired to develop a numerical method whose iterates preserve the previous fundamental invariants [7]. For this purpose, the research area of development of numerical methods for rigid-body differential equations that preserve geometric properties of the numerical solution has been of great interest in recent years [8]. Simos [8, 9], Monovasilis et al. [3, 10], and Kalogiratou [11] construct several different multistep symplectic integrators based on symplectic geometry in order to preserve the Hamiltonian energy of rigid bodies as the harmonic oscillator, the pendulum, two-body problem, and orbital problem. Lee et al. [5] and Hussein et al. [12] adopt variational integrators to evolve the rigid-body dynamics, which ensure that while the total energy of the rigid body is conserved under conservative forces, the momentum associated with a symmetry of the system is also conserved. Iserles et al. [13] and Lee et al. [5] extend numerical methods in Euclidean space into Lie group, so that numerical flow of the rigid-body dynamics has Lie group structural conservativeness, which is also the main focus of this paper. Retaining motion invariants under discretization has been proven not only a nice mathematical property, but also the key to improved numerics, as they capture the right dynamics (even in long-time integration) and exhibit increased accuracy [2, 7, 13, 14]. Therefore, for driving the evolution of rigid-body dynamics, it is very important to develop a numerical method with preservation of the previous geometric characteristics.The difficulty of developing a numerical method on Lie group arises as the group is not the well-known Euclidean spaceℝn; consequently, the rigid-body dynamics evolving on SE(3) cannot be properly integrated by conventional numerical method, including the popular Runge-Kutta schemes, developed for ℝn. For this reason, one resorts to other means to drive the rigid-body dynamics forward while preserving the Lie group structure of the configuration. It can be roughly divided into two categories: classical Lie group methods which discretize the continuous equations of motion and Lie group variational integrators which discretize the variational principles of mechanics. Classical Lie group methods include the Munthe-Kaas (MK) method [15], the Crouch-Grossmann (CG) method [16], the Newmark-type method [17], and the commutator-free (CF) method [18], and so forth. The MK method is based on a differential equation on the Lie algebra and uses a single evaluation of the exponential map. The CG method updates the group elements by multiple evaluations using the exponential map. For detailed information, one can consult to [13]. Lie group variational integrators are proposed in recent years whose idea is to discretize the variational principles of mechanics: Hamilton’s principle or Lagrange-d’Alembert principle [1, 5, 14]. Both methods have their own advantage and disadvantage. The former has a better time performance, but its accuracy is not an advantage under the same order condition. The latter preserves exactly energy, momentum, symplectic structure, and group structure [12, 19] and offers a particularly robust and efficient framework for simulation [14], attitude control [4], motion control [1], and trajectory generating [14] of the aircraft and other vehicles, however; it has no advantage of accuracy, and its performance is subjected to the Newton-type solver for solving the equivalent vector equation [5, 14, 20]. Regardless of the classical Lie group methods or Lie group variational integrators, they are all based on Kenth Engø’s idea about equivariant map which transforms the differential equations evolving on a Lie group into an equivalent differential equation evolving on a Lie algebra corresponding to the Lie group [21].Since accuracy, time performance, conservativeness, and numerical stability of the aircraft rigid-body dynamics need to be considered comprehensively in aircraft simulation and control, we do not intend to adopt any of the previous methods but use the same idea of equivariant map to develop a new method on SE(3) for driving the evolution of aircraft dynamics. We resort to pseudospectral method, which is widely used in fluid mechanics, quantum mechanics, linear and nonlinear waves, aerospace, and other fields by virtue of its high accuracy, spectral (or exponential) convergence rates, requirement for less computer memory under the same precision condition, and so forth [22]. Furthermore, it is commonly used in optimal control of the aircraft and other vehicles [23] and thereby lay the foundation for our simulation and control application. Unlike variational integrator must analytically derive the first-order discrete necessary optimality, which, more often than not, is nontrival for most optimal control problems [23], typical pseudospectral method can be used to transcribe a continuous optimal control problem into a discrete nonlinear programming problem (NLP), and it has been shown that solving the NLP derived from the pseudospectral transcription of the Gauss form is exactly equivalent to solve the discrete first-order necessary conditions [23, 24]. However, when applied to rigid body dynamics directly, general pseudospectral method cannot preserve Lie group structure. For this purpose, a numerical method on SE(3) called geometric pseudospectral method is proposed in this work, which provides satisfactory accuracy, computational efficiency and preserves the essential Lie group structure. To our knowledge, Moulla et al. [25] was the first to pose the concept of “geometric pseudospectral method” not long ago. They suggested a polynomial pseudospectral method that preserves the geometric structure of port-Hamiltonian systems, the phenomenological laws, and the conservation laws without introducing any uncontrolled numerical dissipation. However, their method was designed only for port-Hamiltonian systems having a special structure, that is, the Dirac structure, thus it could not be directly extended to the systems on SE(3). Thereby, how to apply the pseudospectral method to a dynamics system on SE(3) and preserve essential Lie group structure of the system is still an open question, which is the main topic in this paper.In this work, we establish a completely rigid-body dynamics model of aircraft in body-fixed frame on SE(3). With respect to kinematics, due to the fact that applying the general pseudospectral method directly to the configuration equation of the rigid-body dynamics could not preserve the Lie group structure of the solution of the equation, drawing on the equivariant map, we transform the configuration equation on SE(3) into an equivalent equations in a Lie algebra space and accordingly give the top eight orders reduced truncated Magnus series expansion with its coefficientsα(κ) (refer to the appendix) of the solution of the equivalent equation under left trivialization. For the condition of each term in u[2q](t) being multivariate quadrature, we apply the general pseudospectral method to compute quadrature weights, so that obtaining the values of u[2q](t) at the collocation points and the endpoint (collectively referred to discrete points). Finally, we compute configurations at the discrete points via coordinate map. It is worth mentioning that the theories with respect to Lie group methods in the extensive literatures [13–15] are almost derived under right trivialization and thus are not suitable for our method, since our modeled aircraft configuration model is left invariant. To this end, we provide a relatively complete set of left trivialization framework for developing geometric pseudospectral method on SE(3). With respect to dynamics, considering the Lie algebra space is isomorphism to ℝn, we use the general pseudospectral method directly to compute the velocities at the same discrete points as that of configuration. Under the same 4th-order and the same large step size, the proposed method is compared with implicit RK, explicit RKMK, and Gauss pseudospectral method. Through a comprehensive comparison of accuracy, computational efficiency, and Lie group structural conservativeness, the proposed method has higher accuracy, satisfying computational efficiency, stable Lie group structural conservativeness. The proposed method is coordinate-free, no need to switch chart and special handling of singularities. Finally, we give a way of applying the proposed numerical method to rigid-body dynamics simulation and control of the aircraft.The rest of the paper is organized as follows. In Section2, a completely left invariant rigid-body dynamics model of the aircraft on SE(3) is established. Geometric pseudospectral method on SE(3) is developed in Section 3. In Section 3.1, for completeness, the basic idea of the general pseudospectral method in Euclidean space is roughly introduced. In Section 3.2, geometric pseudospectral method on SE(3) is developed under left trivialization, and a 4th-order geometric pseudospectral algorithm is given. In Section 3.3, numerical tests are carried out on a free-floating rigid-body model in comparison with four other numerical methods to validate numerical accuracy, computational efficiency, and structural conservativeness of the proposed method. Then, how to apply the proposed method to rigid-body dynamics simulation and control of the aircraft is illustrated in Section 4. Finally, the conclusions and future work are outlined in Section 5.


## 2. Aircraft Rigid-Body Dynamics Model on SE(3)

The special Euclidean group SE(3) is the semidirect product of SO(3) andℝ3, whose group element gab=(Rab,pab) consists of rotational component Rab∈SO(3) and translational component pab∈ℝ3 of the body-fixed frame {b}, relative to the space frame {a}, where SO(3)={Rab∈ℝ3×3∣RabRabT=I,detRab=+1} is called the special orthogonal group [6]. In this section, we will regard the aircraft as a single rigid body in three-dimensional space and establish a rigid-body dynamics model of the aircraft on SE(3).


### 2.1. Kinematics Model

The navigation equations of the aircraft are given by [26]
(1)[x˙y˙z˙]=Rab(t)[uvw],
where, x˙, y˙, and z˙ are the north, east, and vertical components of the aircraft velocity, respectively, in the locally level geographic frame on the surface of the Earth, u, v, and w denote X-axis component, Y-axis component, and Z-axis component of the aircraft airspeed, respectively, in the body-fixed frame, and Rab(t) is a sequence of ZYX Euler rotations from the body-fixed frame to the locally level geographic frame, given by(2)Rab(t)=[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θ].Denote[x,y,z]T and [u,v,w]T by paba(t) and υabb(t), respectively, then we have
(3)p˙aba(t)=Rab(t)·υabb(t).Also, the kinematic equations are given by [26]
(4)[ϕ˙θ˙ψ˙]=[p+tanθ(qsinϕ+rcosϕ)qcosϕ-rsinϕqsinϕ+rcosϕcosθ],
where, ϕ, θ, and ψ are roll angle, pitch angle, and yaw angle, respectively, which are commonly referred to as Euler angles; p, q, and r denote the roll rate, pitch rate, and yaw rate, respectively, about X-body axis, Y-body axis, and Z-body axis. Then, we have
(5)[pqr]=[10-sinθ0cosϕsinϕcosθ0-sinϕcosϕcosθ][ϕ˙θ˙ψ˙]=[ϕ˙-ψ˙sinθθ˙cosϕ+ψ˙sinϕcosθ-θ˙sinϕ+ψ˙cosϕcosθ].Denote[p,q,r]T by ωabb(t), and bring in a operator ·^:ℝ3→𝔰𝔬(3), for all y∈ℝ3, satisfying ω^abb(t)·y=ωabb(t)×y, where × is vector cross-product; we have ω^abb(t)=[0-rqr0-p-qp0] and obtain
(6)R˙ab(t)=Rab(t)·ω^abb(t).Therefore, from the preceding equation and (3), we can construct a kinematics model (configuration) g-˙ab(t)=g-ab(t)·V-abb(t), namely,
(7)[R˙ab(t)p˙aba(t)00]=[Rab(t)paba(t)01][ω^abb(t)υabb(t)00],
where g-ab(t) and V-abb(t) are, respectively, called the homogeneous representation of gab(t) and that of Vabb(t)=[ωabb(t),υabb(t)]T∈𝔰𝔢(3), 𝔰𝔢(3)=𝔰𝔬(3)×ℝ3 is the Lie algebra corresponding to SE(3) where 𝔰𝔬(3) is the Lie algebra corresponding to SO(3) [6]. The right hand of (7) is the infinitesimal generator of the action corresponding to Vabb(t), and it is left invariant under left multiplication by constant matrices [27]. These properties determine the trivialization way of local coordinate map in Section 3.2.1.


### 2.2. Dynamics Model

The force equations of the aircraft are given by [28]
(8)[u˙v˙w˙]=[r·v-q·wp·w-r·uq·u-p·w]+[-sinθsinϕ·cosθcosϕ·cosθ]g+[q-·smCX,tq-·smCY,tq-·smCZ,t]+[Tm00],
where, u˙, v˙, and w˙ denote X-axis component, Y-axis component, and Z-axis component, respectively, of the aircraft acceleration in the body-fixed frame, m is the aircraft mass, s is the wing reference area, q- is the free-stream dynamic pressure, T is the engine thrust, and CX,t, CY,t, and CZ,t denote total X-axis force coefficient, total Y-axis force coefficient, and total Z-axis force coefficient, respectively.One has(9)CX,t=CX(α,β)+c-q2VTCXq(α),CY,t=CY(α,β)+b2VT[CYr(α)r+CYp(α)p],CZ,t=CZ(α,β)+c-q2VTCZq(α).Denote[q-sCX,t,q-sCY,t,q-sCZ,t]T and [T,0,0]T by FAb(t) and FTb(t), respectively; the former represents aerodynamics, and the latter represents thrust. Then, we have
(10)mυ˙abb(t)=-mωabb(t)×υabb(t)+mgRabT(t)e3+FAb(t)+FTb(t).The moment equations of the aircraft are given by [26](11)[p˙q˙r˙]=[(Jxz(Jx-Jy+Jz)pq-(JzJz-JzJy+Jxz2)rq+Jzq-sbCl,t+Jxzq-sbCn,t)(JxJz-Jxz2)((Jz-Jx)pr-Jxz(p2-r2)+q-sc-Cm,t)Jy(((Jx-Jy)Jx+Jxz2)pq-Jxz(Jx-Jy+Jz)rq+Jxzq-sbCl,t+Jxq-sbCn,t)(JxJz-Jxz2)],where,b is wing span, Jx, Jy, and Jz are moment of inertia about X-axis, Y-axis, and Z-axis, respectively, Jxz is cross-product of inertia, and Cl,t, Cm,t, and Cn,t denote total rolling-moment coefficient, total pitching-moment coefficient, and total yawing-moment coefficient, respectively. The remaining parameters can refer to [26]
(12)Cl,t=Cl(α,β)+ΔCl,δa=20∘δa20+ΔCl,δr=30∘δr30+b2VT(Clr(α)r+Clp(α)p),Cm,t=Cm(α,β)ηδh(δh)+CZ,t(xcg,ref-xcg)+c-q2VTCmq(α),Cn,t=Cn(α,β)-CY,t(xcg,ref-xcg)c-b+ΔCn,δa=20∘δa20+ΔCn,δr=30∘δr30+b2VT(Cnr(α)r+Cnp(α)p).Denote[q-sbCl,t,q-sc-Cm,t,q-sbCn,t]T by TB(t), then we have
(13)Jω˙abb(t)=-ωabb(t)×(Jωabb(t))+TB(t),
where J=[Jx0-Jxz0Jy0-Jxz0Jz] is referred to as the inertia matrix of the rigid body.Therefore, from the preceding equation and (10), we can construct a dynamics model V˙abb(t)=Π-1·(B(Vabb(t))·Vabb(t)+F(t,g-ab(t))) in 𝔰𝔢(3), namely,
(14)[ω˙abb(t)υ˙abb(t)]=[J00mI3]-1·([Jωabb(t)^00-mω^abb(t)][ωabb(t)υabb(t)]+[TB(t)mgRabT(t)e3+FAb(t)+FTb(t)][ωabb(t)υabb(t)]).


## 2.1. Kinematics Model

The navigation equations of the aircraft are given by [26]
(1)[x˙y˙z˙]=Rab(t)[uvw],
where, x˙, y˙, and z˙ are the north, east, and vertical components of the aircraft velocity, respectively, in the locally level geographic frame on the surface of the Earth, u, v, and w denote X-axis component, Y-axis component, and Z-axis component of the aircraft airspeed, respectively, in the body-fixed frame, and Rab(t) is a sequence of ZYX Euler rotations from the body-fixed frame to the locally level geographic frame, given by(2)Rab(t)=[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θ].Denote[x,y,z]T and [u,v,w]T by paba(t) and υabb(t), respectively, then we have
(3)p˙aba(t)=Rab(t)·υabb(t).Also, the kinematic equations are given by [26]
(4)[ϕ˙θ˙ψ˙]=[p+tanθ(qsinϕ+rcosϕ)qcosϕ-rsinϕqsinϕ+rcosϕcosθ],
where, ϕ, θ, and ψ are roll angle, pitch angle, and yaw angle, respectively, which are commonly referred to as Euler angles; p, q, and r denote the roll rate, pitch rate, and yaw rate, respectively, about X-body axis, Y-body axis, and Z-body axis. Then, we have
(5)[pqr]=[10-sinθ0cosϕsinϕcosθ0-sinϕcosϕcosθ][ϕ˙θ˙ψ˙]=[ϕ˙-ψ˙sinθθ˙cosϕ+ψ˙sinϕcosθ-θ˙sinϕ+ψ˙cosϕcosθ].Denote[p,q,r]T by ωabb(t), and bring in a operator ·^:ℝ3→𝔰𝔬(3), for all y∈ℝ3, satisfying ω^abb(t)·y=ωabb(t)×y, where × is vector cross-product; we have ω^abb(t)=[0-rqr0-p-qp0] and obtain
(6)R˙ab(t)=Rab(t)·ω^abb(t).Therefore, from the preceding equation and (3), we can construct a kinematics model (configuration) g-˙ab(t)=g-ab(t)·V-abb(t), namely,
(7)[R˙ab(t)p˙aba(t)00]=[Rab(t)paba(t)01][ω^abb(t)υabb(t)00],
where g-ab(t) and V-abb(t) are, respectively, called the homogeneous representation of gab(t) and that of Vabb(t)=[ωabb(t),υabb(t)]T∈𝔰𝔢(3), 𝔰𝔢(3)=𝔰𝔬(3)×ℝ3 is the Lie algebra corresponding to SE(3) where 𝔰𝔬(3) is the Lie algebra corresponding to SO(3) [6]. The right hand of (7) is the infinitesimal generator of the action corresponding to Vabb(t), and it is left invariant under left multiplication by constant matrices [27]. These properties determine the trivialization way of local coordinate map in Section 3.2.1.


## 2.2. Dynamics Model

The force equations of the aircraft are given by [28]
(8)[u˙v˙w˙]=[r·v-q·wp·w-r·uq·u-p·w]+[-sinθsinϕ·cosθcosϕ·cosθ]g+[q-·smCX,tq-·smCY,tq-·smCZ,t]+[Tm00],
where, u˙, v˙, and w˙ denote X-axis component, Y-axis component, and Z-axis component, respectively, of the aircraft acceleration in the body-fixed frame, m is the aircraft mass, s is the wing reference area, q- is the free-stream dynamic pressure, T is the engine thrust, and CX,t, CY,t, and CZ,t denote total X-axis force coefficient, total Y-axis force coefficient, and total Z-axis force coefficient, respectively.One has(9)CX,t=CX(α,β)+c-q2VTCXq(α),CY,t=CY(α,β)+b2VT[CYr(α)r+CYp(α)p],CZ,t=CZ(α,β)+c-q2VTCZq(α).Denote[q-sCX,t,q-sCY,t,q-sCZ,t]T and [T,0,0]T by FAb(t) and FTb(t), respectively; the former represents aerodynamics, and the latter represents thrust. Then, we have
(10)mυ˙abb(t)=-mωabb(t)×υabb(t)+mgRabT(t)e3+FAb(t)+FTb(t).The moment equations of the aircraft are given by [26](11)[p˙q˙r˙]=[(Jxz(Jx-Jy+Jz)pq-(JzJz-JzJy+Jxz2)rq+Jzq-sbCl,t+Jxzq-sbCn,t)(JxJz-Jxz2)((Jz-Jx)pr-Jxz(p2-r2)+q-sc-Cm,t)Jy(((Jx-Jy)Jx+Jxz2)pq-Jxz(Jx-Jy+Jz)rq+Jxzq-sbCl,t+Jxq-sbCn,t)(JxJz-Jxz2)],where,b is wing span, Jx, Jy, and Jz are moment of inertia about X-axis, Y-axis, and Z-axis, respectively, Jxz is cross-product of inertia, and Cl,t, Cm,t, and Cn,t denote total rolling-moment coefficient, total pitching-moment coefficient, and total yawing-moment coefficient, respectively. The remaining parameters can refer to [26]
(12)Cl,t=Cl(α,β)+ΔCl,δa=20∘δa20+ΔCl,δr=30∘δr30+b2VT(Clr(α)r+Clp(α)p),Cm,t=Cm(α,β)ηδh(δh)+CZ,t(xcg,ref-xcg)+c-q2VTCmq(α),Cn,t=Cn(α,β)-CY,t(xcg,ref-xcg)c-b+ΔCn,δa=20∘δa20+ΔCn,δr=30∘δr30+b2VT(Cnr(α)r+Cnp(α)p).Denote[q-sbCl,t,q-sc-Cm,t,q-sbCn,t]T by TB(t), then we have
(13)Jω˙abb(t)=-ωabb(t)×(Jωabb(t))+TB(t),
where J=[Jx0-Jxz0Jy0-Jxz0Jz] is referred to as the inertia matrix of the rigid body.Therefore, from the preceding equation and (10), we can construct a dynamics model V˙abb(t)=Π-1·(B(Vabb(t))·Vabb(t)+F(t,g-ab(t))) in 𝔰𝔢(3), namely,
(14)[ω˙abb(t)υ˙abb(t)]=[J00mI3]-1·([Jωabb(t)^00-mω^abb(t)][ωabb(t)υabb(t)]+[TB(t)mgRabT(t)e3+FAb(t)+FTb(t)][ωabb(t)υabb(t)]).


## 3. Geometric Pseudospectral Method on SE(3)

### 3.1. General Pseudospectral Method in Euclidean Space

General pseudospectral method will be used for computing the velocities in the Lie algebra. For this purpose, we briefly describe the basic principle of general pseudospectral method [24]. Let us take solving the following differential equation in ℝn at tf, for example,
(15)x˙(t)=f(t,x(t)),x(t0)=x0∈ℝn,t0≤t≤tf.Firstly, we equally divide a time interval[t0,tf] into several time subintervals [tk,tk+1] having a length h and transform the time interval t∈[tk,tk+1] to the time interval τ∈[-1,1] via the affine transformation
(16)t=tk+1-tk2τ+tk+1+tk2.Thus, accordingly, (15) over [tk,tk+1] becomes
(17)dx(τ)dτ=tk+1-tk2f(τ,x(τ)),-1≤τ≤1.Letc1<⋯<cN be collocation points in [-1,1] and c0=-1. We approximate the solution of (17) by the following formula:
(18)x(τ)≈X(τ)=∑i=0Nxi·Li(τ),
where Li(τ)=∏j=0,j≠iN(τ-cj)/(ci-cj) is the Lagrange polynomials, satisfying the isolation property
(19)Li(cj)=δij={1,i=j,0,i≠j.Equation (18) together with the isolation property leads to the fact that
(20)xi=x(ci)
thus X(ci)=x(ci).Through expressing the derivative of the Lagrange polynomials at the collocation points in differential matrix formD=[dji]∈ℝN×(N+1),
(21)dji=L˙i(cj)=∑l=0N∏k=0,k≠i,lN(cj-ck)∏k=0,k≠iN(ci-ck),=L˙i(cj)=j=1,…,N,i=0,…,N.We can write (17) at the collocation points as a set of differential algebra equations as follows:
(22)∑i=0Ndji·X(ci)=tk+1-tk2f(cj,X(cj)),j=1,…,N.Based on the previous equations, we establish defect equations(23)ςj=∑i=0Ndji·X(ci)-tk+1-tk2f(cj,X(cj)),j=1,…,N
and apply iterative algorithms to the previous equations so that determining the approximation to x(τ) at the collocation points. Finally, according to the following formula, we obtain the approximation to (15) at the endpoint tk+1 of time subinterval [tk,tk+1]:
(24)x(tk+1)≈X(tk)+tk+1-tk2∑j=1Nωj·f(cj,X(cj)),
where ωj=∫-11Lj(t)dt is quadrature weight corresponding to the collocation points. This approximate solution will be initial value of x(t) over [tk+1,tk+2] and so on.According to different selection methods of collocation points, pseudospectral methods can be divided into the standard method and the orthogonal method. Common collocation points in the orthogonal method are those obtained from the roots of either Chebyshev polynomialsTN(t) or Legendre polynomials PN(t) belonging to the orthogonal polynomial [29]. The benefit of using the orthogonal method over the standard method is that the quadrature approximation to a definite integral is extremely accurate [24]. Furthermore, according to whether the endpoint is or is not a collocation point, pseudospectral methods fall into three general categories [30]: Gauss methods, neither of the endpoints −1 or 1, is a collocation point; Radau methods, at most one of the endpoints −1 or 1, are a collocation point; Lobatto methods, both of the endpoints −1 and 1, are collocation points. In the rest of this paper, we will develop our geometric pseudospectral method based on Legendre-Gauss points, abbreviated as Gauss points in the following, for two main reasons: firstly, there is an intimate relationship between Gauss method and some implicit RK method whose coefficients in the Butcher Tableau can be used by our method for computing configuration [2]; secondly, when Gauss method is used for transcribing the continues optimal control problem into a discrete nonlinear programming problem, it does not suffer from a defect in the optimality conditions at the boundary points due to the endpoints being not collocation points [23].


### 3.2. Geometric Pseudospectral Method on SE(3)

First recall and rewrite the aircraft rigid-body dynamics model Equations (7) and (14) in Section 2 as follows:
(25)g-˙ab(t)=g-ab(t)·V-abb(t),(26)V˙abb(t)=Π-1·(B(Vabb(t))·Vabb(t)+F(t,g-ab(t)))≜f(Vabb(t),g-ab(t)).For the kinematics Equation (25), it is well known that the solution of (25) stays on SE(3) for all t≥t0. How to use pseudospectral method to solve (25) while preserving the structural feature of the differential equation under discretization of g-ab(t) is the main problem to be solved in this section. In ℝn, both the solution space and the tangent space are linear vector space. Classical numerical methods, including general pseudospectral method, just rely on domain space consisting of vectors. However, the special Euclidean group is a nonlinear manifold, so using classical numerical methods directly to solve differential equation on SE(3) will be not able to preserve its Lie group structure. Reference [21] 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 shown to be locally equivalent to a differential equation on the Lie algebra corresponding to the Lie group acting on the homogenous space. Also, a Lie algebra corresponding to the Lie group is a vector space with the additional structure of a commutator. For the previous reasons, the Lie algebra is the natural choice of space for our geometric pseudospectral method. First, we will apply the equivariant map to transform (25) evolving on SE(3) into an equivalent differential equation evolving in 𝔰𝔢(3). Next, we use the Gauss pseudospectral method to solve the equivalent differential equation in order to obtain pth-order truncated approximation of the equation at the Gauss points and the endpoints, followed by mapping the approximate solution back to configuration space via local coordinate map f.For the dynamics equation (26), because the velocity Vabb(t) belongs to 𝔰𝔢(3), and actually 𝔰𝔢(3) is isomorphic to the ℝ6, we can use Gauss pseudospectral method directly on (26) to compute the velocities at the Gauss points and the endpoint.


#### 3.2.1. Computing Configurations at the Gauss Points and the Endpoint

In order to transform (25) on SE(3) into an equivalent differential equation evolving on 𝔰𝔢(3), we briefly describe the basic idea of equivariant map.Definition 1 (equivariant map).
Letℳ and 𝒩 be manifolds, and let G be a Lie group which acts on ℳ by Φg:ℳ→ℳ and on 𝒩 by Ψg:𝒩→𝒩. A smooth map f:ℳ→𝒩 is called equivariant with respect to these actions if, for all g∈G,
(27)f∘Φg=Ψg∘f
that is, the following diagram commutes of equivariant map f:
(28)
First, from the definition of an action ofG on ℳ, we can obtain an equivariant map Φy:G→ℳ with respect to the left translation action Lg of G on itself and an action G of Φg on ℳ as follows:
(29)Φy∘Lg=Φg∘Φy.
It is known that there is a local coordinate mapf:𝔤→G on G with the Lie algebra 𝔤; the most typical one is exponential map exp:𝔤→G. At this point, we need to find an action Bg of G on 𝔤 such that f will be an equivariant map with Bg and the left action of G on itself, namely,
(30)f∘Bg=Lg∘f.
In the case wheref is the exponential map, Bg is nothing else than the well-known Baker-Campbell-Hausdorff (BCH) formula:(31)Bg(u)=log(g·exp(u)),
where log:G→𝔤 is called the logarithm map. Since composition of two equivariant maps is also an equivariant map, we can construct an equivariant map Φy∘f from 𝔤 to ℳ with respect to the action Bg on 𝔤 and Φ on ℳ. Diagram commutes of composition Φy∘f are as follows:
(32)
The theorem 3.6 of [21] stated that if ϕ is an equivariant map, then the infinitesimal generators of the action with respect to the same element ξ∈𝔤 are ϕ-related vector fields. Thus, the infinitesimal generators ξ𝔤 and ξℳ of the flows Bexp(tξ) and Φexp(tξ), respectively, on 𝔤 and ℳ are Φy∘f-related; that is,
(33)ξℳ∘Φy∘f=TΦy∘Tf∘ξ𝔤.
Finally, we need to determine whatξ𝔤 is, and the following theorem gives the conditions that it needs to meet.Theorem 2 (see [21]).
Letf:𝔤→G be a coordinate map on G and Φy∘f equivariant with respect to the flows Bexp(tξ) and Φexp(tξ). The infinitesimal generator of Bg satisfying (33) is ξ𝔤(u)=dfu-1(υ). df:𝔤→𝔤 is the trivialization of Tf, defined as dfu=TRf(u)-1∘Tfu.The following commutative diagram describing the equivariant maps of compositionΦy∘f (left) and its infinitesimal generator (right) sums up the previous processes:
(34)Also, [21] stated that any differential equation on ℳ which can be written as an infinitesimal generator fits into the previous framework, including all Lie type equations, isospectral flows, and rigid frames. It is noted that, as mentioned in Section 2.1, the right hand of (7) or (25) is an infinitesimal generator on SE(3), and here we assume that the solution g-ab(t) of (25) can be written as the following form [13]:
(35)g-ab(t)=g-ab(t0)·f(u(t)).In order to obtain the explicit expression ofu(t), we need to solve the following differential equation:
(36)u˙(t)=df-u-1(Vabb(t)).For deriving the previous formula, we differentiate (35) as follows:
(37)ddtg-ab(t)=ddt(g-ab(t0)·f(u(t)))=g-ab(t0)·dLf(u(t))∘df-u(t)(u˙(t))=g-ab(t0)·f(u(t))·df-u(t)(u˙(t)).
Comparing (37) with (25) and (35), we have
(38)Vabb(t)=df-u(t)(u˙(t)).
Taking the inverse of previous formula, we obtain (36).It seems from the equivariant map and the previous formula derivation that solving equation (25) can be transformed into a research on the equivalent equation (36), and the manifold ℳ is SE(3) at this point. df-u-1(Vabb(t)) has different forms, according to the different choices of local coordinate map f. In the case where f is the exponential map, the vector field df-u-1 on 𝔤 can be represented as [13]
(39)dexp-u-1(Vabb(t))=Vabb(t)+12adu(Vabb(t))+∑k=2∞Bkk!aduk(Vabb(t)),
where the adjoint operator adu:𝔤→𝔤 is Lie bracket or commutator, adu(Vabb(t))=[u,Vabb(t)], satisfying recursive expression aduk(Vabb(t))=[u,aduk-1(Vabb(t))], and Bk are Bernoulli numbers [31] as follows:
(40)Bk={16,-130,142,-130,566,-6912730,…,k=2N,N∈ℤ+,0,k=2N+1,N∈ℤ+.In the context of rigid-body motion, the right trivialization corresponds to the differential equation with tangent vectorsAY∈𝔛(ℳ), A∈𝔤, and Y∈G, and the left trivialization corresponds to the differential equation with tangent vectors YA∈𝔛(ℳ),A∈𝔤, and Y∈G. The former represents the change of configuration in a space frame, and the latter represents the change of configuration in the body-fixed frame. Here, let df be the left trivialization of Tf, since the configuration model established in Section 2.1 is in the body-fixed frame. In the case of df being the left trivialization and f being the exponential map, we can find an explicit approximation to u(t) called the Magnus series expansion via applying Picard iterations to (36) [31] as follows:
(41)u(t)=∫0tVabb(ξ1)dξ1+12∫0t[∫0ξ1Vabb(ξ2)dξ2,Vabb(ξ1)]dξ1+112∫0t[∫0ξ1Vabb(ξ2)dξ2,[∫0ξ1Vabb(ξ2)dξ2,Vabb(ξ1)]]×dξ1+⋯.The previous formula can be rewritten as an infinite sumu(t)=∑k=0∞Hk(t), where each Hk(t) is a linear combination of terms that have equal numbers k of integrals (exclude the outermost integral from 0 to t) and commutators. We denote each term of Hk(t) by Cκ(ξ) and associate it with a binary tree κ∈𝕋k, where 𝕋k is a set which includes all trees with the same number k of integrals and commutators. Then, we can rewrite (41) as
(42)u(t)=∑k=0∞∑κ∈𝕋kα(κ)∫0tCκ(ξ)dξ,t≥0,
where
(43)α(κ)=B~kk!∏i=1kα(κi),k∈ℤ+.It is worthwhile to note that, unlike right trivialization in [13], here, we take left-trivialized tangent of local coordinate map, thus
(44)B~k={12,fork=1,Bk,fork≠1.Based on the binary tree theory, we can find the followingpth-order approximate solution of (36),
(45)u[p](t)=∑m=0p-1∑κ∈𝔽mα(κ)∫0tCκ(ξ)dξ,
where 𝔽m is the set of all m-power rooted trees. Thereby, we give the Magnus series terms with its coefficients α(κ) under left trivialization in Table 2, which correspond to the top eight orders rooted trees.Then, we use Gauss pseudospectral method to solve (45), so that we get the approximate solution of (36) at the Gauss points tk<τ1,…,τN<tk+1 and the endpoint tk+1≜tk+h. To begin with, assume that all of velocities Vabb(τi) at the Gauss points are known previously; we use the Lagrange polynomials at these points to approximate the velocity at any time t∈[tk,tk+1] as follows:
(46)Vabb(t)≈∑i=1NLi(t-tkh)·Vabb(τi).Next, through transforming (46) into [0,1] via an affine transformation ξ=(t-tk)/h, and inserting it to (45), we have
(47)u[p](t)=I1(t)+12I2(t)+14I3(t)+112I4(t)+124I5(t)+124I6(t)+18I8(t)+⋯,
where each term I(t) is the following form of multivariate quadrature:
(48)I(t)=∫0t∫0ξm2∫0ξm3⋯∫0ξmsℒ(h·Vabb(ξ1),…,h·Vabb(ξs))dξs⋯dξ2dξ1=∫Sℒ(h·Vabb(ξ1),…,h·Vabb(ξs))dξs⋯dξ2dξ1,
where S={ξ1,…,ξs∈ℝ|ξ1∈[0,t],ξℓ∈[0,ξmℓ],mℓ∈{1,2,…,ℓ-1},ℓ=2,3,…,s} and ℒ denotes multivariate function.The same as the univariate case, we use the following quadrature formulae (49) and (50) to evaluate the approximate solution of (48) at τ1,…,τN and tk+1, respectively:
(49)I(τi)=∫S~iℒ(h·Vabb(ξ1),…,h·Vabb(ξs-1))dξs-1⋯dξ2dξ1≈∑k∈Cs-1Nakiℒ(h·Vabb(ξk1),…,h·Vabb(ξks-1)),aki=∫S~i∏ℓ=1s-1Lkℓ(ξℓ)dξℓ,k1,k2,…,ks-1∈k,
where S~i={ξ1,…,ξs-1∈ℝ∣ξ1∈[0,ci],ξℓ∈[0,ξmℓ],mℓ∈{1,2,…,ℓ-1},ℓ=2,3,…,s-1}, and Cs-1N is the set of all combinations of (s-1)-tuples k from the set {1,2,…,N}, excluding s-1 repeats of the same number in the set.One has(50)I(tk+1)=∫S~ℒ(h·Vabb(ξ1),…,h·Vabb(ξs))dξs⋯dξ2dξ1≈∑k∈CsNbkℒ(h·Vabb(ξk1),…,h·Vabb(ξks))bk=∫S~∏ℓ=1sLkℓ(ξℓ)dξℓ,k1,k2,…,ks∈k,
where S~={ξ1,…,ξs∈ℝ|ξ1∈[0,1],ξℓ∈[0,ξmℓ],mℓ∈{1,2,…,ℓ-1},ℓ=2,3,…,s} and CsN is similar to Cs-1N.Through computingI(t) of (47) term by term, we obtain pth-order approximate solution u[p](t) of u(t) at the discrete points. As seen from (48), I(t) depends on the velocities at the Gauss points, and the velocities need to be solved in an iterative manner which will be explained in next section. Thereby, the solutions of configuration at the Gauss points need to be also determined in an iterative manner. Finally, we use u[p](τi) at the Gauss points to compute configuration corresponding to the same points via the following formula:
(51)g-ab(τi)=g-ab(tk)·f(u[p](τi)).There are several choices of local coordinate mapf:𝔤→G, such as exponential map and Cayley map [21]. We incline to the Cayley map, since it is very easy to compute, despite it is only an approximation to the integral curve defined by exponential map as follows:
(52)caySE(3)(u)=[caySO(3)(ω)Acay(ω)·υ01]caySO(3)(ω)=I3+44+∥ω∥2(ω^+(ω^)22),
where ∥·∥ is a standard Euclidean norm, and
(53)Acay(ω)=24+∥ω∥2(2I3+ω^).In the same way, afteru[p](tk+1) is obtained via evaluating I(tk+1) termwise, we use (52) to update configuration g-ab(tk+1) at the endpoint tk+1,
(54)g-ab(tk+1)=g-ab(tk)·f(u[p](tk+1)).


#### 3.2.2. Computing Velocities at the Gauss Points and the Endpoint

As mentioned above, here, we can directly use general pseudospectral in Section3.1 to compute velocities at the Gauss points τ1,…,τN and the endpoint tk+1. However, due to (26) being implicit, we have to use iterative algorithm to solve it. When computing configurations in the preceding section, we assumed that Vabb(τi),i=1,…,N in (46) are known in advance. Hence, we need to give their solutions, and we are going to do it in this section.To begin with, we write (26) at all of the Gauss points in a compact matrix form,
(55)[Vabb(new)(τ1)Vabb(new)(τ2)⋮Vabb(new)(τN)]=tk+1-tk2D~-1·[Π-1·(B(Vabb(old)(τ1))·Vabb(old)(τ1)+F(τ1,g-ab(old)(τ1)))Π-1·(B(Vabb(old)(τ2))·Vabb(old)(τ2)+F(τ2,g-ab(old)(τ2)))⋮Π-1·(B(Vabb(old)(τN))·Vabb(old)(τN)+F(τ2,g-ab(old)(τN)))]-D~-1·[d10d20⋮dN0]·Vabb(tk),
where D~∈ℝN×N is the submatrix of D in Section 3.1, and it is a reversible matrix,
(56)D~=[d11d12⋯d1Nd21d22⋯d2N⋮⋮⋱⋮dN1dN2⋯dNN].By computing the velocity deviation between adjacent iterative steps, we determine whether or not to terminate the iterative process. It is noteworthy that we must ensure that computing configurations and velocities at the same Gauss points.Finally, after the final satisfying iterative results of the velocities at the Gauss points are obtained, we use the following formula to compute velocity at the endpoint:(57)Vabb(tk+1)=Vabb(tk)+tk+1-tk2×∑i=1Nωi·f(Vabb(new)(τi),g-ab(new)(τi)).


#### 3.2.3. 4th-Order Algorithm of Geometric Pseudospectral Method

In accordance with the aforementioned basic principle of geometric pseudospectral method, we develop a 4th-order algorithm for the rigid-body dynamics evolution over time.Implementation and the Order Condition. Legendre-Gauss point τi is the root of Legendre polynomial PN(τ)=(1/2NN!)(dN/dτN){[τ2-1]N} of degree N. In fact, a convenient way to compute the Legendre-Gauss points is via the eigenvalues of the following tridiagonal Jacobi matrix:
(58)J=[0β1β10β2⋱⋱⋱⋱0βN-1βN-10],βn=n4n2-1.Due to the fact that (25)-(26) are implicit, therefore, we have to use iterative algorithm to update the velocities and configurations at the Gauss points. In the interactive processes, we need to compute the velocity deviation and configuration deviation, respectively, between the adjacent interative steps. For velocity deviation, since the Lie algebra space 𝔰𝔢(3) in which the velocities belong is isomorphic to ℝ6, so we are able to use minus directly. However, for configuration deviation, because configuration space is a nonlinear manifold, we introduce the following natural error of configuration [27]:
(59)e=(g-ab(step+1))-1g-ab(step)=[(R(step+1))TR(step)(R(step+1))T(p(step)-p(step+1))01].By the way, the reason why we choose the previous metric is its intuitive physical meaning; taking SE(2) as an example, the 2-norm∥(R(step+1))TR(step)∥2 denotes angle difference Δθ=θ(step+1)-θ(step) between configurations, and the 2-norm ∥(R(step+1))T(p(step)-p(step+1))∥2 denotes distance difference Δd=(x(step+1)-x(step))2+(y(step+1)-y(step))2 between configurations. The last one required to pay attention to is to ensure computing configurations and velocities at the same Gauss points.Turning to order condition of the algorithm, there are something worthy of our attention. Firstly, if we consider the time symmetry of the Magnus series expansion, the number of terms belong to (47) can be further reduced [13]. According to the time symmetry, function u[p](t) can be expanded as the odd power of time t, and then u[2q](t)=u2q-1(t)+𝒪(t2q+1). For this purpose, we provide the top eight orders reduced truncated Magnus series expansion in Table 3 which have been used in step 2.2.3.3 and step 2.2.6.1 of Algorithm 1. Secondly, let the Magnus series expansion be truncated up to the pth-order trees for g-ab(tk+1) and p-1th-order trees for the intermediate stages g-ab(τi), then the resulting schemes have pth-order [32]. Moreover, [2] stated that the interpolatory quadrature formula with N Gauss points is 2Nth-order. In summary, our 4th-order geometric pseudospectral method only depends on two Gauss points.Algorithm 1:4th-order geometric pseudospectral algorithm on SE(3).
Step1. Initialization:
Initialize time interval[t0,tf], step length h, configuration g-ab(t0), velocity Vabb(t0), and let k be 0;
Step2. Main loop:
Step 2.1. Compute the Gauss points {τ1,τ2} in [tk,tk+1], corresponding differentiation matrix D2×3
and quadrature weights {ω1,ω2};
Step 2.2. Compute velocities Vabb(τ1) and Vabb(τ2) at the Gauss points:
Step 2.2.1. Set the number of iterations, threshold of iterations deviation and let current step be 1;
Step 2.2.2. Let initial value of bothg-ab(step)(τ1) and g-ab(step)(τ2) be g-ab(tk), that of both Vabb(step)(τ1) and Vabb(step)(τ2) be Vabb(tk);
Step 2.2.3. Child loop:
Step 2.2.3.1. According to the following equations, obtain [V˙abb(step)(τ1),V˙abb(step)(τ2)]T,
[V˙abb(step)(τ1)V˙abb(step)(τ2)]=tk+1-tk2[Π-1·(B(Vabb(step)(τ1))·Vabb(step)(τ1)+F(τ1,gab(step)(τ1)))Π-1·(B(Vabb(step)(τ2))·Vabb(step)(τ2)+F(τ2,gab(step)(τ2)))]-[d01d02]Vabb(tk)
Step 2.2.3.2. Update velocities [Vabb(step+1)(τ1),Vabb(step+1)(τ2)]T at the Gauss point,
[Vabb(step+1)(τ1)Vabb(step+1)(τ2)]=D~-1[V˙abb(step)(τ1)V˙abb(step)(τ2)]
Step 2.2.3.3. Compute 4th-order reduced truncated Magnus series expansion [u[4](τ1),u[4](τ2)]T at the Gauss points,
[
u
[
4
]
(
τ
1
)
u
[
4
]
(
τ
2
)
]
=
[
1
4
h
·
V
a
b
b
(
step
)
(
τ
1
)
+
(
1
4
-
3
6
)
h
·
V
a
b
b
(
step
)
(
τ
2
)
-
1
2
(
5
72
-
3
24
)
h
2
·
[
V
a
b
b
(
step
)
(
τ
1
)
,
V
a
b
b
(
step
)
(
τ
2
)
]
(
1
4
+
3
6
)
h
·
V
a
b
b
(
step
)
(
τ
1
)
+
1
4
h
·
V
a
b
b
(
step
)
(
τ
2
)
-
1
2
(
-
5
72
-
3
24
)
h
2
·
[
V
a
b
b
(
step
)
(
τ
1
)
,
V
a
b
b
(
step
)
(
τ
2
)
]
]
Step 2.2.3.4. Using the Cayley map, update configurations [g-ab(step+1)(τ1),g-ab(step+1)(τ2)]Tat the Gauss points,
[
g
-
a
b
(
step
+
1
)
(
τ
1
)
g
-
a
b
(
step
+
1
)
(
τ
2
)
]
=
[
g
-
a
b
(
t
k
)
·
cay
(
u
[
4
]
(
τ
1
)
)
g
-
a
b
(
t
k
)
·
cay
(
u
[
4
]
(
τ
2
)
)
]
Step 2.2.3.5. Compute the configuration deviation and velocity deviation respectively between the adjacent steps,
and let the larger one between them be iterative deviation at the current step,
norm
(
g
-
a
b
(
err
)
)
=
∥
[
lo
g
SE
(
3
)
(
g
-
a
b
(
step
+
1
)
(
τ
1
)
-
1
·
g
-
a
b
(
step
)
(
τ
1
)
)
lo
g
SE
(
3
)
(
g
-
a
b
(
step
+
1
)
(
τ
2
)
-
1
·
g
-
a
b
(
step
)
(
τ
2
)
)
]
∥
2, norm(Vabb(err))=∥[∥Vabb(step+1)(τ1)-Vabb(step)(τ1)∥2∥Vabb(step+1)(τ2)-Vabb(step)(τ2)∥2]∥∞
Step 2.2.3.6. Substitute g-ab(step+1) and Vabb(step+1) at τ1 and τ2 into g-ab(step) and Vabb(step) respectively, at the same time, let step plus 1;
Step 2.2.3.7. Compute the difference between iterative deviation and specified tolerance,
so that determine whether or not to terminate the iterative process.
Step 2.2.4. End child loop;
Step 2.2.5. Use the final iterative resultsg-ab(step) and Vabb(step) in child loop to compute the velocity Vabb(tk+1) at the endpoint tk+1,
V
a
b
b
(
t
k
+
1
)
=
V
a
b
b
(
t
k
)
+
t
k
+
1
-
t
k
2
∑
i
=
1
2
ω
i
·
Π
-
1
·
(
B
(
V
a
b
b
(
step
)
(
τ
i
)
)
V
a
b
b
(
step
)
(
τ
i
)
+
F
(
τ
i
,
g
-
a
b
(
step
)
(
τ
i
)
)
)
Step 2.2.6. Compute configurationg-ab(tk+1) at tk+1:
Step 2.2.6.1. Compute 4th-order reduced truncated Magnus series expansion u[4](tk+1) at tk+1,
u
[
4
]
(
t
k
+
1
)
=
1
2
h
·
(
V
a
b
b
(
step
)
(
τ
1
)
+
V
a
b
b
(
step
)
(
τ
2
)
)
+
3
12
h
2
·
[
V
a
b
b
(
step
)
(
τ
1
)
,
V
a
b
b
(
step
)
(
τ
2
)
]
Step 2.2.6.2. Use the Cayley map to update configuration g-ab(tk+1),
g-ab(tk+1)=g-ab(tk)·cay(u[4](tk+1))
Step 2.2.7. Comparetk+1 with tf, so that determine whether or not to terminate the main loop;
Step3. End loop.


### 3.3. Numerical Test on Free-Floating Rigid Body

In this section, the performance of Algorithm1 is tested through evolution of the following free-floating rigid body dynamics equations (60)-(61) on SE(3). Note that, under the condition of no external force, (60)-(61) are equivalent to (25)-(26). Table 1 summarizes the parameters used in the numerical test as follows:
(60)[R˙ab(t)p˙aba(t)00]=[Rab(t)paba(t)01][ω^abb(t)υabb(t)00],(61)[ω˙abb(t)υ˙abb(t)]=[J00mI3]-1[Jωabb(t)^00-mω^abb(t)][ωabb(t)υabb(t)].Table 1
Parameters of numerical test.
Time
t
∈
[
0,240
] s
Mass
m
=
1 kg
Moment of inertia
J
=
diag
[
1,2.8,2
] kg·m2
Initial orientation
R
a
b
(
0
)
=
I
3
×
3
Initial position
p
a
b
a
(
0
)
=
[
0,0
,
0
]
T m
Initial linear velocity
υ
a
b
b
(
0
)
=
[
0,0
,
1
]
T m/s
Initial angular velocity
ω
a
b
b
(
0
)
=
[
1,1
,
0
]
T rad/sTable 2
Magnus series terms corresponding to the top eight orders rooted trees.
p
𝕋
k
𝔽
m
∫
0
t
C
τ
(
ξ
1
)
d
ξ
1
I
(
t
)
α
(
κ
)
1
𝕋
0
𝔽
0
∫
0
t
V
a
b
b
(
ξ
1
)
d
ξ
1
I
1
(
t
)
1
3
𝕋
1
𝔽
2
∫
0
t
∫
0
ξ
1
[
V
a
b
b
(
ξ
2
)
,
V
a
b
b
(
ξ
1
)
]
d
ξ
2
d
ξ
1
I
2
(
t
)
1/2
4
𝕋
2
𝔽
3
∫
0
t
∫
0
ξ
1
∫
0
ξ
2
[
[
V
a
b
b
(
ξ
3
)
,
V
a
b
b
(
ξ
2
)
]
,
V
a
b
b
(
ξ
1
)
]
d
ξ
3
d
ξ
2
d
ξ
1
I
3
(
t
)
1/4
4
𝕋
2
𝔽
3
∫
0
t
∫
0
ξ
1
∫
0
ξ
1
[
V
a
b
b
(
ξ
2
)
,
[
V
a
b
b
(
ξ
2
)
,
V
a
b
b
(
ξ
1
)
]
]
d
ξ
2
d
ξ
2
d
ξ
1
I
4
(
t
)
1/12
5
𝕋
3
𝔽
4
∫
0
t
∫
0
ξ
1
∫
0
ξ
2
∫
0
ξ
2
[
[
V
a
b
b
(
ξ
3
)
,
[
V
a
b
b
(
ξ
3
)
,
V
a
b
b
(
ξ
2
)
]
]
,
V
a
b
b
(
ξ
1
)
]
d
ξ
3
d
ξ
3
d
ξ
2
d
ξ
1
I
5
(
t
)
1/24
5
𝕋
3
𝔽
4
∫
0
t
∫
0
ξ
1
∫
0
ξ
1
∫
0
ξ
2
[
V
a
b
b
(
ξ
2
)
,
[
[
V
a
b
b
(
ξ
3
)
,
V
a
b
b
(
ξ
2
)
]
,
V
a
b
b
(
ξ
1
)
]
]
d
ξ
3
d
ξ
2
d
ξ
2
d
ξ
1
I
6
(
t
)
1/24
7
𝕋
3
𝔽
6
∫
0
t
∫
0
ξ
1
∫
0
ξ
2
∫
0
ξ
1
[
[
V
a
b
b
(
ξ
3
)
,
V
a
b
b
(
ξ
2
)
]
,
[
V
a
b
b
(
ξ
2
)
,
V
a
b
b
(
ξ
1
)
]
]
d
ξ
3
d
ξ
2
d
ξ
2
d
ξ
1
I
7
(
t
)
1/24
5
𝕋
3
𝔽
4
∫
0
t
∫
0
ξ
1
∫
0
ξ
2
∫
0
ξ
3
[
[
[
V
a
b
b
(
ξ
4
)
,
V
a
b
b
(
ξ
3
)
]
,
V
a
b
b
(
ξ
2
)
]
,
V
a
b
b
(
ξ
1
)
]
d
ξ
4
d
ξ
3
d
ξ
2
d
ξ
1
I
8
(
t
)
1/8
5
𝕋
3
𝔽
4
∫
0
t
∫
0
ξ
1
∫
0
ξ
1
∫
0
ξ
1
[
V
a
b
b
(
ξ
2
)
,
[
V
a
b
b
(
ξ
2
)
,
[
V
a
b
b
(
ξ
2
)
,
V
a
b
b
(
ξ
1
)
]
]
]
d
ξ
2
d
ξ
2
d
ξ
2
d
ξ
1
I
9
(
t
)
0
6
𝕋
4
𝔽
5
∫
0
t
∫
0
ξ
1
∫
0
ξ
2
∫
0
ξ
3
∫
0
ξ
4
[
[
[
[
V
a
b
b
(
ξ
5
)
,
V
a
b
b
(
ξ
4
)
]
,
V
a
b
b
(
ξ
3
)
]
,
V
a
b
b
(
ξ
2
)
]
,
V
a
b
b
(
ξ
1
)
]
d
ξ
5
d
ξ
4
d
ξ
3
d
ξ
2
d
ξ
1
I
10
(
t
)
1/16
6
𝕋
4
𝔽
5
∫
0
t
∫
0
ξ
1
∫
0
ξ
2
∫
0
ξ
3
∫
0
ξ
3
[
[
[
V
a
b
b
(
ξ
4
)
,
[
V
a
b
b
(
ξ
4
)
,
V
a
b
b
(
ξ
3
)
]
]
,
V
a
b
b
(
ξ
2
)
]
,
V
a
b
b
(
ξ
1
)
]
d
ξ
4
d
ξ
4
d
ξ
3
d
ξ
2
d
ξ
1
I
11
(
t
)
1/48
8
𝕋
4
𝔽
7
∫
0
t
∫
0
ξ
1
∫
0
ξ
2
∫
0
ξ
2
∫
0
ξ
3
[
[
[
V
a
b
b
(
ξ
4
)
,
V
a
b
b
(
ξ
3
)
]
,
[
V
a
b
b
(
ξ
3
)
,
V
a
b
b
(
ξ
2
)
]
]
,
V
a
b
b
(
ξ
1
)
]
d
ξ
4
d
ξ
3
d
ξ
3
d
ξ
2
d
ξ
1
I
12
(
t
)
1/48
6
𝕋
4
𝔽
5
∫
0
t
∫
0
ξ
1
∫
0
ξ
2
∫
0
ξ
2
∫
0
ξ
3
[
[
V
a
b
b
(
ξ
3
)
,
[
[
V
a
b
b
(
ξ
4
)
,
V
a
b
b
(
ξ
3
)
]
,
V
a
b
b
(
ξ
2
)
]
]
,
V
a
b
b
(
ξ
1
)
]
d
ξ
4
d
ξ
3
d
ξ
3
d
ξ
2
d
ξ
1
I
13
(
t
)
1/48
6
𝕋
4
𝔽
5
∫
0
t
∫
0
ξ
1
∫
0
ξ
2
∫
0
ξ
2
∫
0
ξ
2
[
[
V
a
b
b
(
ξ
3
)
,
[
V
a
b
b
(
ξ
3
)
,
[
V
a
b
b
(
ξ
3
)
,
V
a
b
b
(
ξ
2
)
]
]
]
,
V
a
b
b
(
ξ
1
)
]
d
ξ
3
d
ξ
3
d
ξ
3
d
ξ
2
d
ξ
1
I
14
(
t
)
0
7
𝕋
4
𝔽
6
∫
0
t
∫
0
ξ
1
∫
0
ξ
1
∫
0
ξ
2
∫
0
ξ
3
[
[
[
V
a
b
b
(
ξ
4
)
,
V
a
b
b
(
ξ
3
)
]
,
V
a
b
b
(
ξ
2
)
]
,
[
V
a
b
b
(
ξ
2
)
,
V
a
b
b
(
ξ
1
)
]
]
d
ξ
4
d
ξ
3
d
ξ
2
d
ξ
2
d
ξ
1
I
15
(
t
)
1/48
7
𝕋
4
𝔽
6
∫
0
t
∫
0
ξ
1
∫
0
ξ
1
∫
0
ξ
2
∫
0
ξ
2
[
[
V
a
b
b
(
ξ
3
)
,
[
V
a
b
b
(
ξ
3
)
,
V
a
b
b
(
ξ
2
)
]
]
,
[
V
a
b
b
(
ξ
2
)
,
V
a
b
b
(
ξ
1
)
]
]
d
ξ
3
d
ξ
3
d
ξ
2
d
ξ
2
d
ξ
1
I
16
(
t
)
1/144
7
𝕋
4
𝔽
6
∫
0
t
∫
0
ξ
1
∫
0
ξ
1
∫
0
ξ
2
∫
0
ξ
2
[
[
V
a
b
b
(
ξ
3
)
,
V
a
b
b
(
ξ
2
)
]
,
[
[
V
a
b
b
(
ξ
3
)
,
V
a
b
b
(
ξ
2
)
]
,
V
a
b
b
(
ξ
1
)
]
]
d
ξ
3
d
ξ
3
d
ξ
2
d
ξ
2
d
ξ
1
I
17
(
t
)
1/48
7
𝕋
4
𝔽
6
∫
0
t
∫
0
ξ
1
∫
0
ξ
1
∫
0
ξ
1
∫
0
ξ
2
[
[
V
a
b
b
(
ξ
3
)
,
V
a
b
b
(
ξ
2
)
]
,
[
V
a
b
b
(
ξ
2
)
,
[
V
a
b
b
(
ξ
2
)
,
V
a
b
b
(
ξ
1
)
]
]
]
d
ξ
3
d
ξ
2
d
ξ
2
d
ξ
2
d
ξ
1
I
18
(
t
)
0
6
𝕋
4
𝔽
5
∫
0
t
∫
0
ξ
1
∫
0
ξ
1
∫
0
ξ
2
∫
0
ξ
3
[
V
a
b
b
(
ξ
2
)
,
[
[
[
V
a
b
b
(
ξ
4
)
,
V
a
b
b
(
ξ
3
)
]
,
V
a
b
b
(
ξ
2
)
]
,
V
a
b
b
(
ξ
1
)
]
]
d
ξ
4
d
ξ
3
d
ξ
2
d
ξ
2
d
ξ
1
I
19
(
t
)
1/48
6
𝕋
4
𝔽
5
∫
0
t
∫
0
ξ
1
∫
0
ξ
2
∫
0
ξ
3
∫
0
ξ
3
[
V
a
b
b
(
ξ
2
)
,
[
[
V
a
b
b
(
ξ
4
)
,
[
V
a
b
b
(
ξ
4
)
,
V
a
b
b
(
ξ
3
)
]
]
,
V
a
b
b
(
ξ
1
)
]
]
d
ξ
4
d
ξ
4
d
ξ
3
d
ξ
2
d
ξ
1
I
20
(
t
)
1/144
8
𝕋
4
𝔽
7
∫
0
t
∫
0
ξ
1
∫
0
ξ
1
∫
0
ξ
1
∫
0
ξ
2
[
V
a
b
b
(
ξ
2
)
,
[
[
V
a
b
b
(
ξ
3
)
,
V
a
b
b
(
ξ
2
)
]
,
[
V
a
b
b
(
ξ
2
)
,
V
a
b
b
(
ξ
1
)
]
]
]
d
ξ
3
d
ξ
2
d
ξ
2
d
ξ
2
d
ξ
1
I
21
(
t
)
0
6
𝕋
4
𝔽
5
∫
0
t
∫
0
ξ
1
∫
0
ξ
1
∫
0
ξ
1
∫
0
ξ
2
[
V
a
b
b
(
ξ
2
)
,
[
V
a
b
b
(
ξ
2
)
,
[
[
V
a
b
b
(
ξ
3
)
,
V
a
b
b
(
ξ
2
)
]
,
V
a
b
b
(
ξ
1
)
]
]
]
d
ξ
3
d
ξ
2
d
ξ
2
d
ξ
2
d
ξ
1
I
22
(
t
)
0
6
𝕋
4
𝔽
5
∫
0
t
∫
0
ξ
1
∫
0
ξ
1
∫
0
ξ
1
∫
0
ξ
1
[
V
a
b
b
(
ξ
2
)
,
[
V
a
b
b
(
ξ
2
)
,
[
V
a
b
b
(
ξ
2
)
,
[
V
a
b
b
(
ξ
2
)
,
V
a
b
b
(
ξ
1
)
]
]
]
]
d
ξ
2
d
ξ
2
d
ξ
2
d
ξ
2
d
ξ
1
I
23
(
t
)
−1/720Table 3
The top eight orders reduced truncated Magnus series expansion.
Order
q
p
Reduced Magnus series expansion
2
1
1
u
[
2
]
(
t
)
=
I
1
(
t
)
4
2
3
u
[
4
]
(
t
)
=
I
1
(
t
)
+
1
2
I
2
(
t
)
6
3
5
u
[
6
]
(
t
)
=
I
1
(
t
)
+
1
2
I
2
(
t
)
+
1
4
I
3
(
t
)
+
1
12
I
4
(
t
)
+
1
24
I
5
(
t
)
+
1
24
I
6
(
t
)
+
1
8
I
8
(
t
)
8
4
7
u
[
8
]
(
t
)
=
I
1
(
t
)
+
1
2
I
2
(
t
)
+
1
4
I
3
(
t
)
+
1
12
I
4
(
t
)
+
1
24
I
5
(
t
)
+
1
24
I
6
(
t
)
+
1
8
I
8
(
t
)
+
1
16
I
10
(
t
)
+
1
48
I
11
(
t
)
+
1
48
I
13
(
t
)
+
1
48
I
15
(
t
)
+
1
48
I
19
(
t
)
+
1
144
I
20
(
t
)
-
1
720
I
23
(
t
)
+
1
24
I
7
(
t
)
+
1
48
I
15
(
t
)
+
1
144
I
16
(
t
)
+
1
48
I
17
(
t
)For making the numerical test more comparable, we select four 4th-order numerical methods used to compare with geometric pseudospectral method (GPM), including ode45 [33], implicit 4th-order Runge-Kutta (RKi), explicit 4th-order RKMK (RKMK) [13], and 4th-order Gauss pseudospectral method (PM) [24], where iterative number and tolerance of the child loop of GPM and PM are set to 10 and 10-14, respectively. Among the previous methods, ode45 is the build-in integrator from MATLAB which is a variable-step method and whose results would be used as the ground truth with a specified tolerance 10-14. Both RKi and PM are the methods in Euclidean space. RKMK is an approximate Runge-Kutta method on Lie group [15]. It is worth mentioning that each test must be carried out under the same step size.In order to intuitively present the difference between these methods, the 240 seconds evolving trajectories of the free-floating rigid body for a large step size are shown in Figure1. As seen from the evolution versus time, the positions of PM is closest to the ground truth, and that of GPM take second place, but that of RKi and RKMK deviate from the true values to varying degrees. The potential reason resulted in accuracy of GPM being lower than PM is that position of PM is computed by iterative operator with specified iterative numbers and tolerance directly, but position of GPM is truncated to a specific order before iterative operator. Despite inferior to PM, the proposed method is obviously superior to RKi and RKMK in position accuracy. As can be seen, error accumulates faster in translational components paba than in rotational components Rab. This could be explained by the fact that rotation is decoupled from translation and hence has its own error dynamics, whereas translation suffers from small cumulative errors in rotation. In addition, we also give the total computational time in the first line and the average time per step in parenthesis. Obviously, RKMK needs the least time since it is an explicit method. Both GPM and PM need less time than RKi. From orientation, linear velocity, and angular velocity, there is no obvious difference, so we need to further compare these methods quantitatively.Figure 1
Position (paba), orientation (Rab), linear velocity (υabb), and angular velocity (ωabb) versus time and total computational time with average time per step for the free-floating rigid body when step size is equal to 0.25 s.In order to quantitatively compare the proposed method with the other three methods, deviation statistics and runtime statistics over 10 runs using a wide range of initial conditions on a 240 seconds evolution are provided in Figure2. Figure 2(a) shows the position deviation against timesteps. It conforms that the accuracy of the GPM is superior to RKi and RKMK under the same large step size, although it is inferior to PM in average position deviation, and the relationship of position deviation between the four methods is consistent with that of Figure 1. In Figure 2(b), we give the orientation deviation against timesteps by calculating the metric ∥logSO(3)(Rode45TRab)∥2 of SO(3) [27], where Rode45 denotes the orientation of the free-floating rigid body which is computed by ode45. It should be noted that Rode45 transforms from the unit quaternion [26], since in ode45 unit quaternion is selected for parameterizing orientation at this point. Figure 2(c) shows the averaged computation time of four methods. As can be seen, computational efficiency of GPM is inferior to that of RKMK, because RKMK is an explicit method. However, our method requires less computation time than RKi and is nearly twice as efficient as RKi on average.Accuracy and computational efficiency of GPM compared with PM, RKMK, and RKi.
(a)
Average position deviation
(b)
Average orientation deviation
(c)
Average runtime per updateFinally, we examine the conservativeness of Lie group structure. Considering that configuration is parameterized by unit quaternion in ode45 and implicit RK method, we only compare the proposed method with explicit RKMK and Gauss pseudospectral method. As mentioned, the group element of SO(3) satisfiesRabRabT=I, and we compute deviation ∥I3×3-RTR∥∞ for evaluating structural conservativeness of the algorithms [5]. Figure 3 illustrates that Gauss pseudospectral method in Euclidean space has no conservativeness of Lie group structure, since it is not a Lie group method; RKMK and geometric pseudospectral method on Lie group are able to preserve Lie group structure with accuracy approaching to machine accuracy.Lie group structural deviation.
(a)
Gauss pseudospectral method
(b)
Explicit RKMK
(c)
Geometric pseudospectral method


## 3.1. General Pseudospectral Method in Euclidean Space

General pseudospectral method will be used for computing the velocities in the Lie algebra. For this purpose, we briefly describe the basic principle of general pseudospectral method [24]. Let us take solving the following differential equation in ℝn at tf, for example,
(15)x˙(t)=f(t,x(t)),x(t0)=x0∈ℝn,t0≤t≤tf.Firstly, we equally divide a time interval[t0,tf] into several time subintervals [tk,tk+1] having a length h and transform the time interval t∈[tk,tk+1] to the time interval τ∈[-1,1] via the affine transformation
(16)t=tk+1-tk2τ+tk+1+tk2.Thus, accordingly, (15) over [tk,tk+1] becomes
(17)dx(τ)dτ=tk+1-tk2f(τ,x(τ)),-1≤τ≤1.Letc1<⋯<cN be collocation points in [-1,1] and c0=-1. We approximate the solution of (17) by the following formula:
(18)x(τ)≈X(τ)=∑i=0Nxi·Li(τ),
where Li(τ)=∏j=0,j≠iN(τ-cj)/(ci-cj) is the Lagrange polynomials, satisfying the isolation property
(19)Li(cj)=δij={1,i=j,0,i≠j.Equation (18) together with the isolation property leads to the fact that
(20)xi=x(ci)
thus X(ci)=x(ci).Through expressing the derivative of the Lagrange polynomials at the collocation points in differential matrix formD=[dji]∈ℝN×(N+1),
(21)dji=L˙i(cj)=∑l=0N∏k=0,k≠i,lN(cj-ck)∏k=0,k≠iN(ci-ck),=L˙i(cj)=j=1,…,N,i=0,…,N.We can write (17) at the collocation points as a set of differential algebra equations as follows:
(22)∑i=0Ndji·X(ci)=tk+1-tk2f(cj,X(cj)),j=1,…,N.Based on the previous equations, we establish defect equations(23)ςj=∑i=0Ndji·X(ci)-tk+1-tk2f(cj,X(cj)),j=1,…,N
and apply iterative algorithms to the previous equations so that determining the approximation to x(τ) at the collocation points. Finally, according to the following formula, we obtain the approximation to (15) at the endpoint tk+1 of time subinterval [tk,tk+1]:
(24)x(tk+1)≈X(tk)+tk+1-tk2∑j=1Nωj·f(cj,X(cj)),
where ωj=∫-11Lj(t)dt is quadrature weight corresponding to the collocation points. This approximate solution will be initial value of x(t) over [tk+1,tk+2] and so on.According to different selection methods of collocation points, pseudospectral methods can be divided into the standard method and the orthogonal method. Common collocation points in the orthogonal method are those obtained from the roots of either Chebyshev polynomialsTN(t) or Legendre polynomials PN(t) belonging to the orthogonal polynomial [29]. The benefit of using the orthogonal method over the standard method is that the quadrature approximation to a definite integral is extremely accurate [24]. Furthermore, according to whether the endpoint is or is not a collocation point, pseudospectral methods fall into three general categories [30]: Gauss methods, neither of the endpoints −1 or 1, is a collocation point; Radau methods, at most one of the endpoints −1 or 1, are a collocation point; Lobatto methods, both of the endpoints −1 and 1, are collocation points. In the rest of this paper, we will develop our geometric pseudospectral method based on Legendre-Gauss points, abbreviated as Gauss points in the following, for two main reasons: firstly, there is an intimate relationship between Gauss method and some implicit RK method whose coefficients in the Butcher Tableau can be used by our method for computing configuration [2]; secondly, when Gauss method is used for transcribing the continues optimal control problem into a discrete nonlinear programming problem, it does not suffer from a defect in the optimality conditions at the boundary points due to the endpoints being not collocation points [23].


## 3.2. Geometric Pseudospectral Method on SE(3)

First recall and rewrite the aircraft rigid-body dynamics model Equations (7) and (14) in Section 2 as follows:
(25)g-˙ab(t)=g-ab(t)·V-abb(t),(26)V˙abb(t)=Π-1·(B(Vabb(t))·Vabb(t)+F(t,g-ab(t)))≜f(Vabb(t),g-ab(t)).For the kinematics Equation (25), it is well known that the solution of (25) stays on SE(3) for all t≥t0. How to use pseudospectral method to solve (25) while preserving the structural feature of the differential equation under discretization of g-ab(t) is the main problem to be solved in this section. In ℝn, both the solution space and the tangent space are linear vector space. Classical numerical methods, including general pseudospectral method, just rely on domain space consisting of vectors. However, the special Euclidean group is a nonlinear manifold, so using classical numerical methods directly to solve differential equation on SE(3) will be not able to preserve its Lie group structure. Reference [21] 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 shown to be locally equivalent to a differential equation on the Lie algebra corresponding to the Lie group acting on the homogenous space. Also, a Lie algebra corresponding to the Lie group is a vector space with the additional structure of a commutator. For the previous reasons, the Lie algebra is the natural choice of space for our geometric pseudospectral method. First, we will apply the equivariant map to transform (25) evolving on SE(3) into an equivalent differential equation evolving in 𝔰𝔢(3). Next, we use the Gauss pseudospectral method to solve the equivalent differential equation in order to obtain pth-order truncated approximation of the equation at the Gauss points and the endpoints, followed by mapping the approximate solution back to configuration space via local coordinate map f.For the dynamics equation (26), because the velocity Vabb(t) belongs to 𝔰𝔢(3), and actually 𝔰𝔢(3) is isomorphic to the ℝ6, we can use Gauss pseudospectral method directly on (26) to compute the velocities at the Gauss points and the endpoint.


### 3.2.1. Computing Configurations at the Gauss Points and the Endpoint

In order to transform (25) on SE(3) into an equivalent differential equation evolving on 𝔰𝔢(3), we briefly describe the basic idea of equivariant map.Definition 1 (equivariant map).
Letℳ and 𝒩 be manifolds, and let G be a Lie group which acts on ℳ by Φg:ℳ→ℳ and on 𝒩 by Ψg:𝒩→𝒩. A smooth map f:ℳ→𝒩 is called equivariant with respect to these actions if, for all g∈G,
(27)f∘Φg=Ψg∘f
that is, the following diagram commutes of equivariant map f:
(28)
First, from the definition of an action ofG on ℳ, we can obtain an equivariant map Φy:G→ℳ with respect to the left translation action Lg of G on itself and an action G of Φg on ℳ as follows:
(29)Φy∘Lg=Φg∘Φy.
It is known that there is a local coordinate mapf:𝔤→G on G with the Lie algebra 𝔤; the most typical one is exponential map exp:𝔤→G. At this point, we need to find an action Bg of G on 𝔤 such that f will be an equivariant map with Bg and the left action of G on itself, namely,
(30)f∘Bg=Lg∘f.
In the case wheref is the exponential map, Bg is nothing else than the well-known Baker-Campbell-Hausdorff (BCH) formula:(31)Bg(u)=log(g·exp(u)),
where log:G→𝔤 is called the logarithm map. Since composition of two equivariant maps is also an equivariant map, we can construct an equivariant map Φy∘f from 𝔤 to ℳ with respect to the action Bg on 𝔤 and Φ on ℳ. Diagram commutes of composition Φy∘f are as follows:
(32)
The theorem 3.6 of [21] stated that if ϕ is an equivariant map, then the infinitesimal generators of the action with respect to the same element ξ∈𝔤 are ϕ-related vector fields. Thus, the infinitesimal generators ξ𝔤 and ξℳ of the flows Bexp(tξ) and Φexp(tξ), respectively, on 𝔤 and ℳ are Φy∘f-related; that is,
(33)ξℳ∘Φy∘f=TΦy∘Tf∘ξ𝔤.
Finally, we need to determine whatξ𝔤 is, and the following theorem gives the conditions that it needs to meet.Theorem 2 (see [21]).
Letf:𝔤→G be a coordinate map on G and Φy∘f equivariant with respect to the flows Bexp(tξ) and Φexp(tξ). The infinitesimal generator of Bg satisfying (33) is ξ𝔤(u)=dfu-1(υ). df:𝔤→𝔤 is the trivialization of Tf, defined as dfu=TRf(u)-1∘Tfu.The following commutative diagram describing the equivariant maps of compositionΦy∘f (left) and its infinitesimal generator (right) sums up the previous processes:
(34)Also, [21] stated that any differential equation on ℳ which can be written as an infinitesimal generator fits into the previous framework, including all Lie type equations, isospectral flows, and rigid frames. It is noted that, as mentioned in Section 2.1, the right hand of (7) or (25) is an infinitesimal generator on SE(3), and here we assume that the solution g-ab(t) of (25) can be written as the following form [13]:
(35)g-ab(t)=g-ab(t0)·f(u(t)).In order to obtain the explicit expression ofu(t), we need to solve the following differential equation:
(36)u˙(t)=df-u-1(Vabb(t)).For deriving the previous formula, we differentiate (35) as follows:
(37)ddtg-ab(t)=ddt(g-ab(t0)·f(u(t)))=g-ab(t0)·dLf(u(t))∘df-u(t)(u˙(t))=g-ab(t0)·f(u(t))·df-u(t)(u˙(t)).
Comparing (37) with (25) and (35), we have
(38)Vabb(t)=df-u(t)(u˙(t)).
Taking the inverse of previous formula, we obtain (36).It seems from the equivariant map and the previous formula derivation that solving equation (25) can be transformed into a research on the equivalent equation (36), and the manifold ℳ is SE(3) at this point. df-u-1(Vabb(t)) has different forms, according to the different choices of local coordinate map f. In the case where f is the exponential map, the vector field df-u-1 on 𝔤 can be represented as [13]
(39)dexp-u-1(Vabb(t))=Vabb(t)+12adu(Vabb(t))+∑k=2∞Bkk!aduk(Vabb(t)),
where the adjoint operator adu:𝔤→𝔤 is Lie bracket or commutator, adu(Vabb(t))=[u,Vabb(t)], satisfying recursive expression aduk(Vabb(t))=[u,aduk-1(Vabb(t))], and Bk are Bernoulli numbers [31] as follows:
(40)Bk={16,-130,142,-130,566,-6912730,…,k=2N,N∈ℤ+,0,k=2N+1,N∈ℤ+.In the context of rigid-body motion, the right trivialization corresponds to the differential equation with tangent vectorsAY∈𝔛(ℳ), A∈𝔤, and Y∈G, and the left trivialization corresponds to the differential equation with tangent vectors YA∈𝔛(ℳ),A∈𝔤, and Y∈G. The former represents the change of configuration in a space frame, and the latter represents the change of configuration in the body-fixed frame. Here, let df be the left trivialization of Tf, since the configuration model established in Section 2.1 is in the body-fixed frame. In the case of df being the left trivialization and f being the exponential map, we can find an explicit approximation to u(t) called the Magnus series expansion via applying Picard iterations to (36) [31] as follows:
(41)u(t)=∫0tVabb(ξ1)dξ1+12∫0t[∫0ξ1Vabb(ξ2)dξ2,Vabb(ξ1)]dξ1+112∫0t[∫0ξ1Vabb(ξ2)dξ2,[∫0ξ1Vabb(ξ2)dξ2,Vabb(ξ1)]]×dξ1+⋯.The previous formula can be rewritten as an infinite sumu(t)=∑k=0∞Hk(t), where each Hk(t) is a linear combination of terms that have equal numbers k of integrals (exclude the outermost integral from 0 to t) and commutators. We denote each term of Hk(t) by Cκ(ξ) and associate it with a binary tree κ∈𝕋k, where 𝕋k is a set which includes all trees with the same number k of integrals and commutators. Then, we can rewrite (41) as
(42)u(t)=∑k=0∞∑κ∈𝕋kα(κ)∫0tCκ(ξ)dξ,t≥0,
where
(43)α(κ)=B~kk!∏i=1kα(κi),k∈ℤ+.It is worthwhile to note that, unlike right trivialization in [13], here, we take left-trivialized tangent of local coordinate map, thus
(44)B~k={12,fork=1,Bk,fork≠1.Based on the binary tree theory, we can find the followingpth-order approximate solution of (36),
(45)u[p](t)=∑m=0p-1∑κ∈𝔽mα(κ)∫0tCκ(ξ)dξ,
where 𝔽m is the set of all m-power rooted trees. Thereby, we give the Magnus series terms with its coefficients α(κ) under left trivialization in Table 2, which correspond to the top eight orders rooted trees.Then, we use Gauss pseudospectral method to solve (45), so that we get the approximate solution of (36) at the Gauss points tk<τ1,…,τN<tk+1 and the endpoint tk+1≜tk+h. To begin with, assume that all of velocities Vabb(τi) at the Gauss points are known previously; we use the Lagrange polynomials at these points to approximate the velocity at any time t∈[tk,tk+1] as follows:
(46)Vabb(t)≈∑i=1NLi(t-tkh)·Vabb(τi).Next, through transforming (46) into [0,1] via an affine transformation ξ=(t-tk)/h, and inserting it to (45), we have
(47)u[p](t)=I1(t)+12I2(t)+14I3(t)+112I4(t)+124I5(t)+124I6(t)+18I8(t)+⋯,
where each term I(t) is the following form of multivariate quadrature:
(48)I(t)=∫0t∫0ξm2∫0ξm3⋯∫0ξmsℒ(h·Vabb(ξ1),…,h·Vabb(ξs))dξs⋯dξ2dξ1=∫Sℒ(h·Vabb(ξ1),…,h·Vabb(ξs))dξs⋯dξ2dξ1,
where S={ξ1,…,ξs∈ℝ|ξ1∈[0,t],ξℓ∈[0,ξmℓ],mℓ∈{1,2,…,ℓ-1},ℓ=2,3,…,s} and ℒ denotes multivariate function.The same as the univariate case, we use the following quadrature formulae (49) and (50) to evaluate the approximate solution of (48) at τ1,…,τN and tk+1, respectively:
(49)I(τi)=∫S~iℒ(h·Vabb(ξ1),…,h·Vabb(ξs-1))dξs-1⋯dξ2dξ1≈∑k∈Cs-1Nakiℒ(h·Vabb(ξk1),…,h·Vabb(ξks-1)),aki=∫S~i∏ℓ=1s-1Lkℓ(ξℓ)dξℓ,k1,k2,…,ks-1∈k,
where S~i={ξ1,…,ξs-1∈ℝ∣ξ1∈[0,ci],ξℓ∈[0,ξmℓ],mℓ∈{1,2,…,ℓ-1},ℓ=2,3,…,s-1}, and Cs-1N is the set of all combinations of (s-1)-tuples k from the set {1,2,…,N}, excluding s-1 repeats of the same number in the set.One has(50)I(tk+1)=∫S~ℒ(h·Vabb(ξ1),…,h·Vabb(ξs))dξs⋯dξ2dξ1≈∑k∈CsNbkℒ(h·Vabb(ξk1),…,h·Vabb(ξks))bk=∫S~∏ℓ=1sLkℓ(ξℓ)dξℓ,k1,k2,…,ks∈k,
where S~={ξ1,…,ξs∈ℝ|ξ1∈[0,1],ξℓ∈[0,ξmℓ],mℓ∈{1,2,…,ℓ-1},ℓ=2,3,…,s} and CsN is similar to Cs-1N.Through computingI(t) of (47) term by term, we obtain pth-order approximate solution u[p](t) of u(t) at the discrete points. As seen from (48), I(t) depends on the velocities at the Gauss points, and the velocities need to be solved in an iterative manner which will be explained in next section. Thereby, the solutions of configuration at the Gauss points need to be also determined in an iterative manner. Finally, we use u[p](τi) at the Gauss points to compute configuration corresponding to the same points via the following formula:
(51)g-ab(τi)=g-ab(tk)·f(u[p](τi)).There are several choices of local coordinate mapf:𝔤→G, such as exponential map and Cayley map [21]. We incline to the Cayley map, since it is very easy to compute, despite it is only an approximation to the integral curve defined by exponential map as follows:
(52)caySE(3)(u)=[caySO(3)(ω)Acay(ω)·υ01]caySO(3)(ω)=I3+44+∥ω∥2(ω^+(ω^)22),
where ∥·∥ is a standard Euclidean norm, and
(53)Acay(ω)=24+∥ω∥2(2I3+ω^).In the same way, afteru[p](tk+1) is obtained via evaluating I(tk+1) termwise, we use (52) to update configuration g-ab(tk+1) at the endpoint tk+1,
(54)g-ab(tk+1)=g-ab(tk)·f(u[p](tk+1)).


### 3.2.2. Computing Velocities at the Gauss Points and the Endpoint

As mentioned above, here, we can directly use general pseudospectral in Section3.1 to compute velocities at the Gauss points τ1,…,τN and the endpoint tk+1. However, due to (26) being implicit, we have to use iterative algorithm to solve it. When computing configurations in the preceding section, we assumed that Vabb(τi),i=1,…,N in (46) are known in advance. Hence, we need to give their solutions, and we are going to do it in this section.To begin with, we write (26) at all of the Gauss points in a compact matrix form,
(55)[Vabb(new)(τ1)Vabb(new)(τ2)⋮Vabb(new)(τN)]=tk+1-tk2D~-1·[Π-1·(B(Vabb(old)(τ1))·Vabb(old)(τ1)+F(τ1,g-ab(old)(τ1)))Π-1·(B(Vabb(old)(τ2))·Vabb(old)(τ2)+F(τ2,g-ab(old)(τ2)))⋮Π-1·(B(Vabb(old)(τN))·Vabb(old)(τN)+F(τ2,g-ab(old)(τN)))]-D~-1·[d10d20⋮dN0]·Vabb(tk),
where D~∈ℝN×N is the submatrix of D in Section 3.1, and it is a reversible matrix,
(56)D~=[d11d12⋯d1Nd21d22⋯d2N⋮⋮⋱⋮dN1dN2⋯dNN].By computing the velocity deviation between adjacent iterative steps, we determine whether or not to terminate the iterative process. It is noteworthy that we must ensure that computing configurations and velocities at the same Gauss points.Finally, after the final satisfying iterative results of the velocities at the Gauss points are obtained, we use the following formula to compute velocity at the endpoint:(57)Vabb(tk+1)=Vabb(tk)+tk+1-tk2×∑i=1Nωi·f(Vabb(new)(τi),g-ab(new)(τi)).


### 3.2.3. 4th-Order Algorithm of Geometric Pseudospectral Method

In accordance with the aforementioned basic principle of geometric pseudospectral method, we develop a 4th-order algorithm for the rigid-body dynamics evolution over time.Implementation and the Order Condition. Legendre-Gauss point τi is the root of Legendre polynomial PN(τ)=(1/2NN!)(dN/dτN){[τ2-1]N} of degree N. In fact, a convenient way to compute the Legendre-Gauss points is via the eigenvalues of the following tridiagonal Jacobi matrix:
(58)J=[0β1β10β2⋱⋱⋱⋱0βN-1βN-10],βn=n4n2-1.Due to the fact that (25)-(26) are implicit, therefore, we have to use iterative algorithm to update the velocities and configurations at the Gauss points. In the interactive processes, we need to compute the velocity deviation and configuration deviation, respectively, between the adjacent interative steps. For velocity deviation, since the Lie algebra space 𝔰𝔢(3) in which the velocities belong is isomorphic to ℝ6, so we are able to use minus directly. However, for configuration deviation, because configuration space is a nonlinear manifold, we introduce the following natural error of configuration [27]:
(59)e=(g-ab(step+1))-1g-ab(step)=[(R(step+1))TR(step)(R(step+1))T(p(step)-p(step+1))01].By the way, the reason why we choose the previous metric is its intuitive physical meaning; taking SE(2) as an example, the 2-norm∥(R(step+1))TR(step)∥2 denotes angle difference Δθ=θ(step+1)-θ(step) between configurations, and the 2-norm ∥(R(step+1))T(p(step)-p(step+1))∥2 denotes distance difference Δd=(x(step+1)-x(step))2+(y(step+1)-y(step))2 between configurations. The last one required to pay attention to is to ensure computing configurations and velocities at the same Gauss points.Turning to order condition of the algorithm, there are something worthy of our attention. Firstly, if we consider the time symmetry of the Magnus series expansion, the number of terms belong to (47) can be further reduced [13]. According to the time symmetry, function u[p](t) can be expanded as the odd power of time t, and then u[2q](t)=u2q-1(t)+𝒪(t2q+1). For this purpose, we provide the top eight orders reduced truncated Magnus series expansion in Table 3 which have been used in step 2.2.3.3 and step 2.2.6.1 of Algorithm 1. Secondly, let the Magnus series expansion be truncated up to the pth-order trees for g-ab(tk+1) and p-1th-order trees for the intermediate stages g-ab(τi), then the resulting schemes have pth-order [32]. Moreover, [2] stated that the interpolatory quadrature formula with N Gauss points is 2Nth-order. In summary, our 4th-order geometric pseudospectral method only depends on two Gauss points.Algorithm 1:4th-order geometric pseudospectral algorithm on SE(3).
Step1. Initialization:
Initialize time interval[t0,tf], step length h, configuration g-ab(t0), velocity Vabb(t0), and let k be 0;
Step2. Main loop:
Step 2.1. Compute the Gauss points {τ1,τ2} in [tk,tk+1], corresponding differentiation matrix D2×3
and quadrature weights {ω1,ω2};
Step 2.2. Compute velocities Vabb(τ1) and Vabb(τ2) at the Gauss points:
Step 2.2.1. Set the number of iterations, threshold of iterations deviation and let current step be 1;
Step 2.2.2. Let initial value of bothg-ab(step)(τ1) and g-ab(step)(τ2) be g-ab(tk), that of both Vabb(step)(τ1) and Vabb(step)(τ2) be Vabb(tk);
Step 2.2.3. Child loop:
Step 2.2.3.1. According to the following equations, obtain [V˙abb(step)(τ1),V˙abb(step)(τ2)]T,
[V˙abb(step)(τ1)V˙abb(step)(τ2)]=tk+1-tk2[Π-1·(B(Vabb(step)(τ1))·Vabb(step)(τ1)+F(τ1,gab(step)(τ1)))Π-1·(B(Vabb(step)(τ2))·Vabb(step)(τ2)+F(τ2,gab(step)(τ2)))]-[d01d02]Vabb(tk)
Step 2.2.3.2. Update velocities [Vabb(step+1)(τ1),Vabb(step+1)(τ2)]T at the Gauss point,
[Vabb(step+1)(τ1)Vabb(step+1)(τ2)]=D~-1[V˙abb(step)(τ1)V˙abb(step)(τ2)]
Step 2.2.3.3. Compute 4th-order reduced truncated Magnus series expansion [u[4](τ1),u[4](τ2)]T at the Gauss points,
[
u
[
4
]
(
τ
1
)
u
[
4
]
(
τ
2
)
]
=
[
1
4
h
·
V
a
b
b
(
step
)
(
τ
1
)
+
(
1
4
-
3
6
)
h
·
V
a
b
b
(
step
)
(
τ
2
)
-
1
2
(
5
72
-
3
24
)
h
2
·
[
V
a
b
b
(
step
)
(
τ
1
)
,
V
a
b
b
(
step
)
(
τ
2
)
]
(
1
4
+
3
6
)
h
·
V
a
b
b
(
step
)
(
τ
1
)
+
1
4
h
·
V
a
b
b
(
step
)
(
τ
2
)
-
1
2
(
-
5
72
-
3
24
)
h
2
·
[
V
a
b
b
(
step
)
(
τ
1
)
,
V
a
b
b
(
step
)
(
τ
2
)
]
]
Step 2.2.3.4. Using the Cayley map, update configurations [g-ab(step+1)(τ1),g-ab(step+1)(τ2)]Tat the Gauss points,
[
g
-
a
b
(
step
+
1
)
(
τ
1
)
g
-
a
b
(
step
+
1
)
(
τ
2
)
]
=
[
g
-
a
b
(
t
k
)
·
cay
(
u
[
4
]
(
τ
1
)
)
g
-
a
b
(
t
k
)
·
cay
(
u
[
4
]
(
τ
2
)
)
]
Step 2.2.3.5. Compute the configuration deviation and velocity deviation respectively between the adjacent steps,
and let the larger one between them be iterative deviation at the current step,
norm
(
g
-
a
b
(
err
)
)
=
∥
[
lo
g
SE
(
3
)
(
g
-
a
b
(
step
+
1
)
(
τ
1
)
-
1
·
g
-
a
b
(
step
)
(
τ
1
)
)
lo
g
SE
(
3
)
(
g
-
a
b
(
step
+
1
)
(
τ
2
)
-
1
·
g
-
a
b
(
step
)
(
τ
2
)
)
]
∥
2, norm(Vabb(err))=∥[∥Vabb(step+1)(τ1)-Vabb(step)(τ1)∥2∥Vabb(step+1)(τ2)-Vabb(step)(τ2)∥2]∥∞
Step 2.2.3.6. Substitute g-ab(step+1) and Vabb(step+1) at τ1 and τ2 into g-ab(step) and Vabb(step) respectively, at the same time, let step plus 1;
Step 2.2.3.7. Compute the difference between iterative deviation and specified tolerance,
so that determine whether or not to terminate the iterative process.
Step 2.2.4. End child loop;
Step 2.2.5. Use the final iterative resultsg-ab(step) and Vabb(step) in child loop to compute the velocity Vabb(tk+1) at the endpoint tk+1,
V
a
b
b
(
t
k
+
1
)
=
V
a
b
b
(
t
k
)
+
t
k
+
1
-
t
k
2
∑
i
=
1
2
ω
i
·
Π
-
1
·
(
B
(
V
a
b
b
(
step
)
(
τ
i
)
)
V
a
b
b
(
step
)
(
τ
i
)
+
F
(
τ
i
,
g
-
a
b
(
step
)
(
τ
i
)
)
)
Step 2.2.6. Compute configurationg-ab(tk+1) at tk+1:
Step 2.2.6.1. Compute 4th-order reduced truncated Magnus series expansion u[4](tk+1) at tk+1,
u
[
4
]
(
t
k
+
1
)
=
1
2
h
·
(
V
a
b
b
(
step
)
(
τ
1
)
+
V
a
b
b
(
step
)
(
τ
2
)
)
+
3
12
h
2
·
[
V
a
b
b
(
step
)
(
τ
1
)
,
V
a
b
b
(
step
)
(
τ
2
)
]
Step 2.2.6.2. Use the Cayley map to update configuration g-ab(tk+1),
g-ab(tk+1)=g-ab(tk)·cay(u[4](tk+1))
Step 2.2.7. Comparetk+1 with tf, so that determine whether or not to terminate the main loop;
Step3. End loop.


## 3.2.1. Computing Configurations at the Gauss Points and the Endpoint

In order to transform (25) on SE(3) into an equivalent differential equation evolving on 𝔰𝔢(3), we briefly describe the basic idea of equivariant map.Definition 1 (equivariant map).
Letℳ and 𝒩 be manifolds, and let G be a Lie group which acts on ℳ by Φg:ℳ→ℳ and on 𝒩 by Ψg:𝒩→𝒩. A smooth map f:ℳ→𝒩 is called equivariant with respect to these actions if, for all g∈G,
(27)f∘Φg=Ψg∘f
that is, the following diagram commutes of equivariant map f:
(28)
First, from the definition of an action ofG on ℳ, we can obtain an equivariant map Φy:G→ℳ with respect to the left translation action Lg of G on itself and an action G of Φg on ℳ as follows:
(29)Φy∘Lg=Φg∘Φy.
It is known that there is a local coordinate mapf:𝔤→G on G with the Lie algebra 𝔤; the most typical one is exponential map exp:𝔤→G. At this point, we need to find an action Bg of G on 𝔤 such that f will be an equivariant map with Bg and the left action of G on itself, namely,
(30)f∘Bg=Lg∘f.
In the case wheref is the exponential map, Bg is nothing else than the well-known Baker-Campbell-Hausdorff (BCH) formula:(31)Bg(u)=log(g·exp(u)),
where log:G→𝔤 is called the logarithm map. Since composition of two equivariant maps is also an equivariant map, we can construct an equivariant map Φy∘f from 𝔤 to ℳ with respect to the action Bg on 𝔤 and Φ on ℳ. Diagram commutes of composition Φy∘f are as follows:
(32)
The theorem 3.6 of [21] stated that if ϕ is an equivariant map, then the infinitesimal generators of the action with respect to the same element ξ∈𝔤 are ϕ-related vector fields. Thus, the infinitesimal generators ξ𝔤 and ξℳ of the flows Bexp(tξ) and Φexp(tξ), respectively, on 𝔤 and ℳ are Φy∘f-related; that is,
(33)ξℳ∘Φy∘f=TΦy∘Tf∘ξ𝔤.
Finally, we need to determine whatξ𝔤 is, and the following theorem gives the conditions that it needs to meet.Theorem 2 (see [21]).
Letf:𝔤→G be a coordinate map on G and Φy∘f equivariant with respect to the flows Bexp(tξ) and Φexp(tξ). The infinitesimal generator of Bg satisfying (33) is ξ𝔤(u)=dfu-1(υ). df:𝔤→𝔤 is the trivialization of Tf, defined as dfu=TRf(u)-1∘Tfu.The following commutative diagram describing the equivariant maps of compositionΦy∘f (left) and its infinitesimal generator (right) sums up the previous processes:
(34)Also, [21] stated that any differential equation on ℳ which can be written as an infinitesimal generator fits into the previous framework, including all Lie type equations, isospectral flows, and rigid frames. It is noted that, as mentioned in Section 2.1, the right hand of (7) or (25) is an infinitesimal generator on SE(3), and here we assume that the solution g-ab(t) of (25) can be written as the following form [13]:
(35)g-ab(t)=g-ab(t0)·f(u(t)).In order to obtain the explicit expression ofu(t), we need to solve the following differential equation:
(36)u˙(t)=df-u-1(Vabb(t)).For deriving the previous formula, we differentiate (35) as follows:
(37)ddtg-ab(t)=ddt(g-ab(t0)·f(u(t)))=g-ab(t0)·dLf(u(t))∘df-u(t)(u˙(t))=g-ab(t0)·f(u(t))·df-u(t)(u˙(t)).
Comparing (37) with (25) and (35), we have
(38)Vabb(t)=df-u(t)(u˙(t)).
Taking the inverse of previous formula, we obtain (36).It seems from the equivariant map and the previous formula derivation that solving equation (25) can be transformed into a research on the equivalent equation (36), and the manifold ℳ is SE(3) at this point. df-u-1(Vabb(t)) has different forms, according to the different choices of local coordinate map f. In the case where f is the exponential map, the vector field df-u-1 on 𝔤 can be represented as [13]
(39)dexp-u-1(Vabb(t))=Vabb(t)+12adu(Vabb(t))+∑k=2∞Bkk!aduk(Vabb(t)),
where the adjoint operator adu:𝔤→𝔤 is Lie bracket or commutator, adu(Vabb(t))=[u,Vabb(t)], satisfying recursive expression aduk(Vabb(t))=[u,aduk-1(Vabb(t))], and Bk are Bernoulli numbers [31] as follows:
(40)Bk={16,-130,142,-130,566,-6912730,…,k=2N,N∈ℤ+,0,k=2N+1,N∈ℤ+.In the context of rigid-body motion, the right trivialization corresponds to the differential equation with tangent vectorsAY∈𝔛(ℳ), A∈𝔤, and Y∈G, and the left trivialization corresponds to the differential equation with tangent vectors YA∈𝔛(ℳ),A∈𝔤, and Y∈G. The former represents the change of configuration in a space frame, and the latter represents the change of configuration in the body-fixed frame. Here, let df be the left trivialization of Tf, since the configuration model established in Section 2.1 is in the body-fixed frame. In the case of df being the left trivialization and f being the exponential map, we can find an explicit approximation to u(t) called the Magnus series expansion via applying Picard iterations to (36) [31] as follows:
(41)u(t)=∫0tVabb(ξ1)dξ1+12∫0t[∫0ξ1Vabb(ξ2)dξ2,Vabb(ξ1)]dξ1+112∫0t[∫0ξ1Vabb(ξ2)dξ2,[∫0ξ1Vabb(ξ2)dξ2,Vabb(ξ1)]]×dξ1+⋯.The previous formula can be rewritten as an infinite sumu(t)=∑k=0∞Hk(t), where each Hk(t) is a linear combination of terms that have equal numbers k of integrals (exclude the outermost integral from 0 to t) and commutators. We denote each term of Hk(t) by Cκ(ξ) and associate it with a binary tree κ∈𝕋k, where 𝕋k is a set which includes all trees with the same number k of integrals and commutators. Then, we can rewrite (41) as
(42)u(t)=∑k=0∞∑κ∈𝕋kα(κ)∫0tCκ(ξ)dξ,t≥0,
where
(43)α(κ)=B~kk!∏i=1kα(κi),k∈ℤ+.It is worthwhile to note that, unlike right trivialization in [13], here, we take left-trivialized tangent of local coordinate map, thus
(44)B~k={12,fork=1,Bk,fork≠1.Based on the binary tree theory, we can find the followingpth-order approximate solution of (36),
(45)u[p](t)=∑m=0p-1∑κ∈𝔽mα(κ)∫0tCκ(ξ)dξ,
where 𝔽m is the set of all m-power rooted trees. Thereby, we give the Magnus series terms with its coefficients α(κ) under left trivialization in Table 2, which correspond to the top eight orders rooted trees.Then, we use Gauss pseudospectral method to solve (45), so that we get the approximate solution of (36) at the Gauss points tk<τ1,…,τN<tk+1 and the endpoint tk+1≜tk+h. To begin with, assume that all of velocities Vabb(τi) at the Gauss points are known previously; we use the Lagrange polynomials at these points to approximate the velocity at any time t∈[tk,tk+1] as follows:
(46)Vabb(t)≈∑i=1NLi(t-tkh)·Vabb(τi).Next, through transforming (46) into [0,1] via an affine transformation ξ=(t-tk)/h, and inserting it to (45), we have
(47)u[p](t)=I1(t)+12I2(t)+14I3(t)+112I4(t)+124I5(t)+124I6(t)+18I8(t)+⋯,
where each term I(t) is the following form of multivariate quadrature:
(48)I(t)=∫0t∫0ξm2∫0ξm3⋯∫0ξmsℒ(h·Vabb(ξ1),…,h·Vabb(ξs))dξs⋯dξ2dξ1=∫Sℒ(h·Vabb(ξ1),…,h·Vabb(ξs))dξs⋯dξ2dξ1,
where S={ξ1,…,ξs∈ℝ|ξ1∈[0,t],ξℓ∈[0,ξmℓ],mℓ∈{1,2,…,ℓ-1},ℓ=2,3,…,s} and ℒ denotes multivariate function.The same as the univariate case, we use the following quadrature formulae (49) and (50) to evaluate the approximate solution of (48) at τ1,…,τN and tk+1, respectively:
(49)I(τi)=∫S~iℒ(h·Vabb(ξ1),…,h·Vabb(ξs-1))dξs-1⋯dξ2dξ1≈∑k∈Cs-1Nakiℒ(h·Vabb(ξk1),…,h·Vabb(ξks-1)),aki=∫S~i∏ℓ=1s-1Lkℓ(ξℓ)dξℓ,k1,k2,…,ks-1∈k,
where S~i={ξ1,…,ξs-1∈ℝ∣ξ1∈[0,ci],ξℓ∈[0,ξmℓ],mℓ∈{1,2,…,ℓ-1},ℓ=2,3,…,s-1}, and Cs-1N is the set of all combinations of (s-1)-tuples k from the set {1,2,…,N}, excluding s-1 repeats of the same number in the set.One has(50)I(tk+1)=∫S~ℒ(h·Vabb(ξ1),…,h·Vabb(ξs))dξs⋯dξ2dξ1≈∑k∈CsNbkℒ(h·Vabb(ξk1),…,h·Vabb(ξks))bk=∫S~∏ℓ=1sLkℓ(ξℓ)dξℓ,k1,k2,…,ks∈k,
where S~={ξ1,…,ξs∈ℝ|ξ1∈[0,1],ξℓ∈[0,ξmℓ],mℓ∈{1,2,…,ℓ-1},ℓ=2,3,…,s} and CsN is similar to Cs-1N.Through computingI(t) of (47) term by term, we obtain pth-order approximate solution u[p](t) of u(t) at the discrete points. As seen from (48), I(t) depends on the velocities at the Gauss points, and the velocities need to be solved in an iterative manner which will be explained in next section. Thereby, the solutions of configuration at the Gauss points need to be also determined in an iterative manner. Finally, we use u[p](τi) at the Gauss points to compute configuration corresponding to the same points via the following formula:
(51)g-ab(τi)=g-ab(tk)·f(u[p](τi)).There are several choices of local coordinate mapf:𝔤→G, such as exponential map and Cayley map [21]. We incline to the Cayley map, since it is very easy to compute, despite it is only an approximation to the integral curve defined by exponential map as follows:
(52)caySE(3)(u)=[caySO(3)(ω)Acay(ω)·υ01]caySO(3)(ω)=I3+44+∥ω∥2(ω^+(ω^)22),
where ∥·∥ is a standard Euclidean norm, and
(53)Acay(ω)=24+∥ω∥2(2I3+ω^).In the same way, afteru[p](tk+1) is obtained via evaluating I(tk+1) termwise, we use (52) to update configuration g-ab(tk+1) at the endpoint tk+1,
(54)g-ab(tk+1)=g-ab(tk)·f(u[p](tk+1)).


## 3.2.2. Computing Velocities at the Gauss Points and the Endpoint

As mentioned above, here, we can directly use general pseudospectral in Section3.1 to compute velocities at the Gauss points τ1,…,τN and the endpoint tk+1. However, due to (26) being implicit, we have to use iterative algorithm to solve it. When computing configurations in the preceding section, we assumed that Vabb(τi),i=1,…,N in (46) are known in advance. Hence, we need to give their solutions, and we are going to do it in this section.To begin with, we write (26) at all of the Gauss points in a compact matrix form,
(55)[Vabb(new)(τ1)Vabb(new)(τ2)⋮Vabb(new)(τN)]=tk+1-tk2D~-1·[Π-1·(B(Vabb(old)(τ1))·Vabb(old)(τ1)+F(τ1,g-ab(old)(τ1)))Π-1·(B(Vabb(old)(τ2))·Vabb(old)(τ2)+F(τ2,g-ab(old)(τ2)))⋮Π-1·(B(Vabb(old)(τN))·Vabb(old)(τN)+F(τ2,g-ab(old)(τN)))]-D~-1·[d10d20⋮dN0]·Vabb(tk),
where D~∈ℝN×N is the submatrix of D in Section 3.1, and it is a reversible matrix,
(56)D~=[d11d12⋯d1Nd21d22⋯d2N⋮⋮⋱⋮dN1dN2⋯dNN].By computing the velocity deviation between adjacent iterative steps, we determine whether or not to terminate the iterative process. It is noteworthy that we must ensure that computing configurations and velocities at the same Gauss points.Finally, after the final satisfying iterative results of the velocities at the Gauss points are obtained, we use the following formula to compute velocity at the endpoint:(57)Vabb(tk+1)=Vabb(tk)+tk+1-tk2×∑i=1Nωi·f(Vabb(new)(τi),g-ab(new)(τi)).


## 3.2.3. 4th-Order Algorithm of Geometric Pseudospectral Method

In accordance with the aforementioned basic principle of geometric pseudospectral method, we develop a 4th-order algorithm for the rigid-body dynamics evolution over time.Implementation and the Order Condition. Legendre-Gauss point τi is the root of Legendre polynomial PN(τ)=(1/2NN!)(dN/dτN){[τ2-1]N} of degree N. In fact, a convenient way to compute the Legendre-Gauss points is via the eigenvalues of the following tridiagonal Jacobi matrix:
(58)J=[0β1β10β2⋱⋱⋱⋱0βN-1βN-10],βn=n4n2-1.Due to the fact that (25)-(26) are implicit, therefore, we have to use iterative algorithm to update the velocities and configurations at the Gauss points. In the interactive processes, we need to compute the velocity deviation and configuration deviation, respectively, between the adjacent interative steps. For velocity deviation, since the Lie algebra space 𝔰𝔢(3) in which the velocities belong is isomorphic to ℝ6, so we are able to use minus directly. However, for configuration deviation, because configuration space is a nonlinear manifold, we introduce the following natural error of configuration [27]:
(59)e=(g-ab(step+1))-1g-ab(step)=[(R(step+1))TR(step)(R(step+1))T(p(step)-p(step+1))01].By the way, the reason why we choose the previous metric is its intuitive physical meaning; taking SE(2) as an example, the 2-norm∥(R(step+1))TR(step)∥2 denotes angle difference Δθ=θ(step+1)-θ(step) between configurations, and the 2-norm ∥(R(step+1))T(p(step)-p(step+1))∥2 denotes distance difference Δd=(x(step+1)-x(step))2+(y(step+1)-y(step))2 between configurations. The last one required to pay attention to is to ensure computing configurations and velocities at the same Gauss points.Turning to order condition of the algorithm, there are something worthy of our attention. Firstly, if we consider the time symmetry of the Magnus series expansion, the number of terms belong to (47) can be further reduced [13]. According to the time symmetry, function u[p](t) can be expanded as the odd power of time t, and then u[2q](t)=u2q-1(t)+𝒪(t2q+1). For this purpose, we provide the top eight orders reduced truncated Magnus series expansion in Table 3 which have been used in step 2.2.3.3 and step 2.2.6.1 of Algorithm 1. Secondly, let the Magnus series expansion be truncated up to the pth-order trees for g-ab(tk+1) and p-1th-order trees for the intermediate stages g-ab(τi), then the resulting schemes have pth-order [32]. Moreover, [2] stated that the interpolatory quadrature formula with N Gauss points is 2Nth-order. In summary, our 4th-order geometric pseudospectral method only depends on two Gauss points.Algorithm 1:4th-order geometric pseudospectral algorithm on SE(3).
Step1. Initialization:
Initialize time interval[t0,tf], step length h, configuration g-ab(t0), velocity Vabb(t0), and let k be 0;
Step2. Main loop:
Step 2.1. Compute the Gauss points {τ1,τ2} in [tk,tk+1], corresponding differentiation matrix D2×3
and quadrature weights {ω1,ω2};
Step 2.2. Compute velocities Vabb(τ1) and Vabb(τ2) at the Gauss points:
Step 2.2.1. Set the number of iterations, threshold of iterations deviation and let current step be 1;
Step 2.2.2. Let initial value of bothg-ab(step)(τ1) and g-ab(step)(τ2) be g-ab(tk), that of both Vabb(step)(τ1) and Vabb(step)(τ2) be Vabb(tk);
Step 2.2.3. Child loop:
Step 2.2.3.1. According to the following equations, obtain [V˙abb(step)(τ1),V˙abb(step)(τ2)]T,
[V˙abb(step)(τ1)V˙abb(step)(τ2)]=tk+1-tk2[Π-1·(B(Vabb(step)(τ1))·Vabb(step)(τ1)+F(τ1,gab(step)(τ1)))Π-1·(B(Vabb(step)(τ2))·Vabb(step)(τ2)+F(τ2,gab(step)(τ2)))]-[d01d02]Vabb(tk)
Step 2.2.3.2. Update velocities [Vabb(step+1)(τ1),Vabb(step+1)(τ2)]T at the Gauss point,
[Vabb(step+1)(τ1)Vabb(step+1)(τ2)]=D~-1[V˙abb(step)(τ1)V˙abb(step)(τ2)]
Step 2.2.3.3. Compute 4th-order reduced truncated Magnus series expansion [u[4](τ1),u[4](τ2)]T at the Gauss points,
[
u
[
4
]
(
τ
1
)
u
[
4
]
(
τ
2
)
]
=
[
1
4
h
·
V
a
b
b
(
step
)
(
τ
1
)
+
(
1
4
-
3
6
)
h
·
V
a
b
b
(
step
)
(
τ
2
)
-
1
2
(
5
72
-
3
24
)
h
2
·
[
V
a
b
b
(
step
)
(
τ
1
)
,
V
a
b
b
(
step
)
(
τ
2
)
]
(
1
4
+
3
6
)
h
·
V
a
b
b
(
step
)
(
τ
1
)
+
1
4
h
·
V
a
b
b
(
step
)
(
τ
2
)
-
1
2
(
-
5
72
-
3
24
)
h
2
·
[
V
a
b
b
(
step
)
(
τ
1
)
,
V
a
b
b
(
step
)
(
τ
2
)
]
]
Step 2.2.3.4. Using the Cayley map, update configurations [g-ab(step+1)(τ1),g-ab(step+1)(τ2)]Tat the Gauss points,
[
g
-
a
b
(
step
+
1
)
(
τ
1
)
g
-
a
b
(
step
+
1
)
(
τ
2
)
]
=
[
g
-
a
b
(
t
k
)
·
cay
(
u
[
4
]
(
τ
1
)
)
g
-
a
b
(
t
k
)
·
cay
(
u
[
4
]
(
τ
2
)
)
]
Step 2.2.3.5. Compute the configuration deviation and velocity deviation respectively between the adjacent steps,
and let the larger one between them be iterative deviation at the current step,
norm
(
g
-
a
b
(
err
)
)
=
∥
[
lo
g
SE
(
3
)
(
g
-
a
b
(
step
+
1
)
(
τ
1
)
-
1
·
g
-
a
b
(
step
)
(
τ
1
)
)
lo
g
SE
(
3
)
(
g
-
a
b
(
step
+
1
)
(
τ
2
)
-
1
·
g
-
a
b
(
step
)
(
τ
2
)
)
]
∥
2, norm(Vabb(err))=∥[∥Vabb(step+1)(τ1)-Vabb(step)(τ1)∥2∥Vabb(step+1)(τ2)-Vabb(step)(τ2)∥2]∥∞
Step 2.2.3.6. Substitute g-ab(step+1) and Vabb(step+1) at τ1 and τ2 into g-ab(step) and Vabb(step) respectively, at the same time, let step plus 1;
Step 2.2.3.7. Compute the difference between iterative deviation and specified tolerance,
so that determine whether or not to terminate the iterative process.
Step 2.2.4. End child loop;
Step 2.2.5. Use the final iterative resultsg-ab(step) and Vabb(step) in child loop to compute the velocity Vabb(tk+1) at the endpoint tk+1,
V
a
b
b
(
t
k
+
1
)
=
V
a
b
b
(
t
k
)
+
t
k
+
1
-
t
k
2
∑
i
=
1
2
ω
i
·
Π
-
1
·
(
B
(
V
a
b
b
(
step
)
(
τ
i
)
)
V
a
b
b
(
step
)
(
τ
i
)
+
F
(
τ
i
,
g
-
a
b
(
step
)
(
τ
i
)
)
)
Step 2.2.6. Compute configurationg-ab(tk+1) at tk+1:
Step 2.2.6.1. Compute 4th-order reduced truncated Magnus series expansion u[4](tk+1) at tk+1,
u
[
4
]
(
t
k
+
1
)
=
1
2
h
·
(
V
a
b
b
(
step
)
(
τ
1
)
+
V
a
b
b
(
step
)
(
τ
2
)
)
+
3
12
h
2
·
[
V
a
b
b
(
step
)
(
τ
1
)
,
V
a
b
b
(
step
)
(
τ
2
)
]
Step 2.2.6.2. Use the Cayley map to update configuration g-ab(tk+1),
g-ab(tk+1)=g-ab(tk)·cay(u[4](tk+1))
Step 2.2.7. Comparetk+1 with tf, so that determine whether or not to terminate the main loop;
Step3. End loop.


## 3.3. Numerical Test on Free-Floating Rigid Body

In this section, the performance of Algorithm1 is tested through evolution of the following free-floating rigid body dynamics equations (60)-(61) on SE(3). Note that, under the condition of no external force, (60)-(61) are equivalent to (25)-(26). Table 1 summarizes the parameters used in the numerical test as follows:
(60)[R˙ab(t)p˙aba(t)00]=[Rab(t)paba(t)01][ω^abb(t)υabb(t)00],(61)[ω˙abb(t)υ˙abb(t)]=[J00mI3]-1[Jωabb(t)^00-mω^abb(t)][ωabb(t)υabb(t)].Table 1
Parameters of numerical test.
Time
t
∈
[
0,240
] s
Mass
m
=
1 kg
Moment of inertia
J
=
diag
[
1,2.8,2
] kg·m2
Initial orientation
R
a
b
(
0
)
=
I
3
×
3
Initial position
p
a
b
a
(
0
)
=
[
0,0
,
0
]
T m
Initial linear velocity
υ
a
b
b
(
0
)
=
[
0,0
,
1
]
T m/s
Initial angular velocity
ω
a
b
b
(
0
)
=
[
1,1
,
0
]
T rad/sTable 2
Magnus series terms corresponding to the top eight orders rooted trees.
p
𝕋
k
𝔽
m
∫
0
t
C
τ
(
ξ
1
)
d
ξ
1
I
(
t
)
α
(
κ
)
1
𝕋
0
𝔽
0
∫
0
t
V
a
b
b
(
ξ
1
)
d
ξ
1
I
1
(
t
)
1
3
𝕋
1
𝔽
2
∫
0
t
∫
0
ξ
1
[
V
a
b
b
(
ξ
2
)
,
V
a
b
b
(
ξ
1
)
]
d
ξ
2
d
ξ
1
I
2
(
t
)
1/2
4
𝕋
2
𝔽
3
∫
0
t
∫
0
ξ
1
∫
0
ξ
2
[
[
V
a
b
b
(
ξ
3
)
,
V
a
b
b
(
ξ
2
)
]
,
V
a
b
b
(
ξ
1
)
]
d
ξ
3
d
ξ
2
d
ξ
1
I
3
(
t
)
1/4
4
𝕋
2
𝔽
3
∫
0
t
∫
0
ξ
1
∫
0
ξ
1
[
V
a
b
b
(
ξ
2
)
,
[
V
a
b
b
(
ξ
2
)
,
V
a
b
b
(
ξ
1
)
]
]
d
ξ
2
d
ξ
2
d
ξ
1
I
4
(
t
)
1/12
5
𝕋
3
𝔽
4
∫
0
t
∫
0
ξ
1
∫
0
ξ
2
∫
0
ξ
2
[
[
V
a
b
b
(
ξ
3
)
,
[
V
a
b
b
(
ξ
3
)
,
V
a
b
b
(
ξ
2
)
]
]
,
V
a
b
b
(
ξ
1
)
]
d
ξ
3
d
ξ
3
d
ξ
2
d
ξ
1
I
5
(
t
)
1/24
5
𝕋
3
𝔽
4
∫
0
t
∫
0
ξ
1
∫
0
ξ
1
∫
0
ξ
2
[
V
a
b
b
(
ξ
2
)
,
[
[
V
a
b
b
(
ξ
3
)
,
V
a
b
b
(
ξ
2
)
]
,
V
a
b
b
(
ξ
1
)
]
]
d
ξ
3
d
ξ
2
d
ξ
2
d
ξ
1
I
6
(
t
)
1/24
7
𝕋
3
𝔽
6
∫
0
t
∫
0
ξ
1
∫
0
ξ
2
∫
0
ξ
1
[
[
V
a
b
b
(
ξ
3
)
,
V
a
b
b
(
ξ
2
)
]
,
[
V
a
b
b
(
ξ
2
)
,
V
a
b
b
(
ξ
1
)
]
]
d
ξ
3
d
ξ
2
d
ξ
2
d
ξ
1
I
7
(
t
)
1/24
5
𝕋
3
𝔽
4
∫
0
t
∫
0
ξ
1
∫
0
ξ
2
∫
0
ξ
3
[
[
[
V
a
b
b
(
ξ
4
)
,
V
a
b
b
(
ξ
3
)
]
,
V
a
b
b
(
ξ
2
)
]
,
V
a
b
b
(
ξ
1
)
]
d
ξ
4
d
ξ
3
d
ξ
2
d
ξ
1
I
8
(
t
)
1/8
5
𝕋
3
𝔽
4
∫
0
t
∫
0
ξ
1
∫
0
ξ
1
∫
0
ξ
1
[
V
a
b
b
(
ξ
2
)
,
[
V
a
b
b
(
ξ
2
)
,
[
V
a
b
b
(
ξ
2
)
,
V
a
b
b
(
ξ
1
)
]
]
]
d
ξ
2
d
ξ
2
d
ξ
2
d
ξ
1
I
9
(
t
)
0
6
𝕋
4
𝔽
5
∫
0
t
∫
0
ξ
1
∫
0
ξ
2
∫
0
ξ
3
∫
0
ξ
4
[
[
[
[
V
a
b
b
(
ξ
5
)
,
V
a
b
b
(
ξ
4
)
]
,
V
a
b
b
(
ξ
3
)
]
,
V
a
b
b
(
ξ
2
)
]
,
V
a
b
b
(
ξ
1
)
]
d
ξ
5
d
ξ
4
d
ξ
3
d
ξ
2
d
ξ
1
I
10
(
t
)
1/16
6
𝕋
4
𝔽
5
∫
0
t
∫
0
ξ
1
∫
0
ξ
2
∫
0
ξ
3
∫
0
ξ
3
[
[
[
V
a
b
b
(
ξ
4
)
,
[
V
a
b
b
(
ξ
4
)
,
V
a
b
b
(
ξ
3
)
]
]
,
V
a
b
b
(
ξ
2
)
]
,
V
a
b
b
(
ξ
1
)
]
d
ξ
4
d
ξ
4
d
ξ
3
d
ξ
2
d
ξ
1
I
11
(
t
)
1/48
8
𝕋
4
𝔽
7
∫
0
t
∫
0
ξ
1
∫
0
ξ
2
∫
0
ξ
2
∫
0
ξ
3
[
[
[
V
a
b
b
(
ξ
4
)
,
V
a
b
b
(
ξ
3
)
]
,
[
V
a
b
b
(
ξ
3
)
,
V
a
b
b
(
ξ
2
)
]
]
,
V
a
b
b
(
ξ
1
)
]
d
ξ
4
d
ξ
3
d
ξ
3
d
ξ
2
d
ξ
1
I
12
(
t
)
1/48
6
𝕋
4
𝔽
5
∫
0
t
∫
0
ξ
1
∫
0
ξ
2
∫
0
ξ
2
∫
0
ξ
3
[
[
V
a
b
b
(
ξ
3
)
,
[
[
V
a
b
b
(
ξ
4
)
,
V
a
b
b
(
ξ
3
)
]
,
V
a
b
b
(
ξ
2
)
]
]
,
V
a
b
b
(
ξ
1
)
]
d
ξ
4
d
ξ
3
d
ξ
3
d
ξ
2
d
ξ
1
I
13
(
t
)
1/48
6
𝕋
4
𝔽
5
∫
0
t
∫
0
ξ
1
∫
0
ξ
2
∫
0
ξ
2
∫
0
ξ
2
[
[
V
a
b
b
(
ξ
3
)
,
[
V
a
b
b
(
ξ
3
)
,
[
V
a
b
b
(
ξ
3
)
,
V
a
b
b
(
ξ
2
)
]
]
]
,
V
a
b
b
(
ξ
1
)
]
d
ξ
3
d
ξ
3
d
ξ
3
d
ξ
2
d
ξ
1
I
14
(
t
)
0
7
𝕋
4
𝔽
6
∫
0
t
∫
0
ξ
1
∫
0
ξ
1
∫
0
ξ
2
∫
0
ξ
3
[
[
[
V
a
b
b
(
ξ
4
)
,
V
a
b
b
(
ξ
3
)
]
,
V
a
b
b
(
ξ
2
)
]
,
[
V
a
b
b
(
ξ
2
)
,
V
a
b
b
(
ξ
1
)
]
]
d
ξ
4
d
ξ
3
d
ξ
2
d
ξ
2
d
ξ
1
I
15
(
t
)
1/48
7
𝕋
4
𝔽
6
∫
0
t
∫
0
ξ
1
∫
0
ξ
1
∫
0
ξ
2
∫
0
ξ
2
[
[
V
a
b
b
(
ξ
3
)
,
[
V
a
b
b
(
ξ
3
)
,
V
a
b
b
(
ξ
2
)
]
]
,
[
V
a
b
b
(
ξ
2
)
,
V
a
b
b
(
ξ
1
)
]
]
d
ξ
3
d
ξ
3
d
ξ
2
d
ξ
2
d
ξ
1
I
16
(
t
)
1/144
7
𝕋
4
𝔽
6
∫
0
t
∫
0
ξ
1
∫
0
ξ
1
∫
0
ξ
2
∫
0
ξ
2
[
[
V
a
b
b
(
ξ
3
)
,
V
a
b
b
(
ξ
2
)
]
,
[
[
V
a
b
b
(
ξ
3
)
,
V
a
b
b
(
ξ
2
)
]
,
V
a
b
b
(
ξ
1
)
]
]
d
ξ
3
d
ξ
3
d
ξ
2
d
ξ
2
d
ξ
1
I
17
(
t
)
1/48
7
𝕋
4
𝔽
6
∫
0
t
∫
0
ξ
1
∫
0
ξ
1
∫
0
ξ
1
∫
0
ξ
2
[
[
V
a
b
b
(
ξ
3
)
,
V
a
b
b
(
ξ
2
)
]
,
[
V
a
b
b
(
ξ
2
)
,
[
V
a
b
b
(
ξ
2
)
,
V
a
b
b
(
ξ
1
)
]
]
]
d
ξ
3
d
ξ
2
d
ξ
2
d
ξ
2
d
ξ
1
I
18
(
t
)
0
6
𝕋
4
𝔽
5
∫
0
t
∫
0
ξ
1
∫
0
ξ
1
∫
0
ξ
2
∫
0
ξ
3
[
V
a
b
b
(
ξ
2
)
,
[
[
[
V
a
b
b
(
ξ
4
)
,
V
a
b
b
(
ξ
3
)
]
,
V
a
b
b
(
ξ
2
)
]
,
V
a
b
b
(
ξ
1
)
]
]
d
ξ
4
d
ξ
3
d
ξ
2
d
ξ
2
d
ξ
1
I
19
(
t
)
1/48
6
𝕋
4
𝔽
5
∫
0
t
∫
0
ξ
1
∫
0
ξ
2
∫
0
ξ
3
∫
0
ξ
3
[
V
a
b
b
(
ξ
2
)
,
[
[
V
a
b
b
(
ξ
4
)
,
[
V
a
b
b
(
ξ
4
)
,
V
a
b
b
(
ξ
3
)
]
]
,
V
a
b
b
(
ξ
1
)
]
]
d
ξ
4
d
ξ
4
d
ξ
3
d
ξ
2
d
ξ
1
I
20
(
t
)
1/144
8
𝕋
4
𝔽
7
∫
0
t
∫
0
ξ
1
∫
0
ξ
1
∫
0
ξ
1
∫
0
ξ
2
[
V
a
b
b
(
ξ
2
)
,
[
[
V
a
b
b
(
ξ
3
)
,
V
a
b
b
(
ξ
2
)
]
,
[
V
a
b
b
(
ξ
2
)
,
V
a
b
b
(
ξ
1
)
]
]
]
d
ξ
3
d
ξ
2
d
ξ
2
d
ξ
2
d
ξ
1
I
21
(
t
)
0
6
𝕋
4
𝔽
5
∫
0
t
∫
0
ξ
1
∫
0
ξ
1
∫
0
ξ
1
∫
0
ξ
2
[
V
a
b
b
(
ξ
2
)
,
[
V
a
b
b
(
ξ
2
)
,
[
[
V
a
b
b
(
ξ
3
)
,
V
a
b
b
(
ξ
2
)
]
,
V
a
b
b
(
ξ
1
)
]
]
]
d
ξ
3
d
ξ
2
d
ξ
2
d
ξ
2
d
ξ
1
I
22
(
t
)
0
6
𝕋
4
𝔽
5
∫
0
t
∫
0
ξ
1
∫
0
ξ
1
∫
0
ξ
1
∫
0
ξ
1
[
V
a
b
b
(
ξ
2
)
,
[
V
a
b
b
(
ξ
2
)
,
[
V
a
b
b
(
ξ
2
)
,
[
V
a
b
b
(
ξ
2
)
,
V
a
b
b
(
ξ
1
)
]
]
]
]
d
ξ
2
d
ξ
2
d
ξ
2
d
ξ
2
d
ξ
1
I
23
(
t
)
−1/720Table 3
The top eight orders reduced truncated Magnus series expansion.
Order
q
p
Reduced Magnus series expansion
2
1
1
u
[
2
]
(
t
)
=
I
1
(
t
)
4
2
3
u
[
4
]
(
t
)
=
I
1
(
t
)
+
1
2
I
2
(
t
)
6
3
5
u
[
6
]
(
t
)
=
I
1
(
t
)
+
1
2
I
2
(
t
)
+
1
4
I
3
(
t
)
+
1
12
I
4
(
t
)
+
1
24
I
5
(
t
)
+
1
24
I
6
(
t
)
+
1
8
I
8
(
t
)
8
4
7
u
[
8
]
(
t
)
=
I
1
(
t
)
+
1
2
I
2
(
t
)
+
1
4
I
3
(
t
)
+
1
12
I
4
(
t
)
+
1
24
I
5
(
t
)
+
1
24
I
6
(
t
)
+
1
8
I
8
(
t
)
+
1
16
I
10
(
t
)
+
1
48
I
11
(
t
)
+
1
48
I
13
(
t
)
+
1
48
I
15
(
t
)
+
1
48
I
19
(
t
)
+
1
144
I
20
(
t
)
-
1
720
I
23
(
t
)
+
1
24
I
7
(
t
)
+
1
48
I
15
(
t
)
+
1
144
I
16
(
t
)
+
1
48
I
17
(
t
)For making the numerical test more comparable, we select four 4th-order numerical methods used to compare with geometric pseudospectral method (GPM), including ode45 [33], implicit 4th-order Runge-Kutta (RKi), explicit 4th-order RKMK (RKMK) [13], and 4th-order Gauss pseudospectral method (PM) [24], where iterative number and tolerance of the child loop of GPM and PM are set to 10 and 10-14, respectively. Among the previous methods, ode45 is the build-in integrator from MATLAB which is a variable-step method and whose results would be used as the ground truth with a specified tolerance 10-14. Both RKi and PM are the methods in Euclidean space. RKMK is an approximate Runge-Kutta method on Lie group [15]. It is worth mentioning that each test must be carried out under the same step size.In order to intuitively present the difference between these methods, the 240 seconds evolving trajectories of the free-floating rigid body for a large step size are shown in Figure1. As seen from the evolution versus time, the positions of PM is closest to the ground truth, and that of GPM take second place, but that of RKi and RKMK deviate from the true values to varying degrees. The potential reason resulted in accuracy of GPM being lower than PM is that position of PM is computed by iterative operator with specified iterative numbers and tolerance directly, but position of GPM is truncated to a specific order before iterative operator. Despite inferior to PM, the proposed method is obviously superior to RKi and RKMK in position accuracy. As can be seen, error accumulates faster in translational components paba than in rotational components Rab. This could be explained by the fact that rotation is decoupled from translation and hence has its own error dynamics, whereas translation suffers from small cumulative errors in rotation. In addition, we also give the total computational time in the first line and the average time per step in parenthesis. Obviously, RKMK needs the least time since it is an explicit method. Both GPM and PM need less time than RKi. From orientation, linear velocity, and angular velocity, there is no obvious difference, so we need to further compare these methods quantitatively.Figure 1
Position (paba), orientation (Rab), linear velocity (υabb), and angular velocity (ωabb) versus time and total computational time with average time per step for the free-floating rigid body when step size is equal to 0.25 s.In order to quantitatively compare the proposed method with the other three methods, deviation statistics and runtime statistics over 10 runs using a wide range of initial conditions on a 240 seconds evolution are provided in Figure2. Figure 2(a) shows the position deviation against timesteps. It conforms that the accuracy of the GPM is superior to RKi and RKMK under the same large step size, although it is inferior to PM in average position deviation, and the relationship of position deviation between the four methods is consistent with that of Figure 1. In Figure 2(b), we give the orientation deviation against timesteps by calculating the metric ∥logSO(3)(Rode45TRab)∥2 of SO(3) [27], where Rode45 denotes the orientation of the free-floating rigid body which is computed by ode45. It should be noted that Rode45 transforms from the unit quaternion [26], since in ode45 unit quaternion is selected for parameterizing orientation at this point. Figure 2(c) shows the averaged computation time of four methods. As can be seen, computational efficiency of GPM is inferior to that of RKMK, because RKMK is an explicit method. However, our method requires less computation time than RKi and is nearly twice as efficient as RKi on average.Accuracy and computational efficiency of GPM compared with PM, RKMK, and RKi.
(a)
Average position deviation
(b)
Average orientation deviation
(c)
Average runtime per updateFinally, we examine the conservativeness of Lie group structure. Considering that configuration is parameterized by unit quaternion in ode45 and implicit RK method, we only compare the proposed method with explicit RKMK and Gauss pseudospectral method. As mentioned, the group element of SO(3) satisfiesRabRabT=I, and we compute deviation ∥I3×3-RTR∥∞ for evaluating structural conservativeness of the algorithms [5]. Figure 3 illustrates that Gauss pseudospectral method in Euclidean space has no conservativeness of Lie group structure, since it is not a Lie group method; RKMK and geometric pseudospectral method on Lie group are able to preserve Lie group structure with accuracy approaching to machine accuracy.Lie group structural deviation.
(a)
Gauss pseudospectral method
(b)
Explicit RKMK
(c)
Geometric pseudospectral method


## 4. Simulation and Control Application of Aircraft

It is seen from the numerical test in the previous section that the proposed method has better accuracy, stable structural conservativeness, and satisfying computational efficiency. Thus, it is able to meet the fidelity and timeliness requirements of aircraft dynamics simulation. In view of the aircraft dynamics is complex and underactuated, we adopt optimal control to generate flyable trajectories of aircraft. As mentioned in Section1, pseudospectral method of the Gauss form can be easily applied to optimal control of the aircraft or other vehicles and satisfies optimization condition. Reference [23] shows that solving the NLP derived from the pseudospectral transcription of the Gauss form is exactly equivalent to solve discrete first-order necessary conditions. The KKT conditions of the NLP are exactly equivalent to the discrete first-order necessary conditions of the Bolza problem. And, KKT multipliers can be mapped to the costate of the continuous-time optimal control problem via costate mapping principle. Because our geometric pseudospectral discretization scheme is also based on Gauss points, we can use it to transcribe a continuous aircraft optimal control problem into a discrete nonlinear programming problem (NLP). Then, the resulting NLP can be solved by many well-known optimization techniques [34, 35]. For this purpose, we follow the Gauss pseudospectral transcription approach introduced in [36]. The optimal control problem can be formulated as in Algorithm 2.Algorithm 2:Problem formulation of the NLP.
Compute: g-ab,Vabb,T,δa,δr,δe at τj, j=1,…,N+1
Minimizing: Cost Function
Subject to:
g
-
0
=
g
-
i
,
V
0
=
V
i, g-N+1=g-f,VN+1=Vf,
i
,
f denote initial time and terminal time.
and
(i) dynamics constraints: (25), (26)
(ii) path constraints
(iii) decision variables bounds:
g-(τi)∈[g-l,g-u],V(τi)∈[Vl,Vu]
T
(
τ
i
)
∈
[
T
l
,
T
u
]
,
δ
a
∈
[
δ
a
l
,
δ
a
u
],
δ
r
∈
[
δ
r
l
,
δ
r
u
]
,
δ
e
∈
[
δ
e
l
,
δ
e
u
]
where l,u denote lower limit and upper limit.Depending on the specified scenario, cost functions include minimum control effort, minimum time, obstacle avoidance, and the quickest manoeuver. For further details regarding the optimization setup, one can consult to [36]. In this work, referring to the geometric optimal control framework proposed in [37], rotation component Rab of gab is not treated as our decision variable, for three reasons: firstly, during the optimization, Rab can be reconstructed from ω^abb internally through evaluating (45), (51), and (54); secondly, the Lie group structural conservativeness could not be considered by NLP solvers, unless additional constraint with respect to structural conservativeness is introduced, which may affect optimality; moreover, computational efficiency is improved. We take obstacle avoidance as our example, with optimal control which is implemented using the combination of GPOPS [36] in MATLAB and SNOPT package in TOMLAB version 7.7 [38]. Figure 4 shows simulation results for an aircraft avoiding obstacles. Their detailed analysis is the subject of on-going work and will be presented in a future paper.Figure 4
Simulation results for the aircraft avoiding obstacles.


## 5. Conclusions

A completely left invariant rigid-body dynamics model of aircraft on SE(3) is established. For the left invariance of rigid-body dynamics model in body-fixed frame, an equivalent differential equation on𝔰𝔢(3) is given under left trivialization. Accordingly, based on Magnus series expansion and exponential map, the solutions of configuration equation at the Gauss points and the endpoint are obtained. Velocities on 𝔰𝔢(3) at the discrete points are computed based on general pseudospectral method, since 𝔰𝔢(3) is isomorphic to ℝ6. Thereby, a 4th-order numerical method called geometric pseudospectral method is developed. Through a series of numerical tests and comparison with other numerical methods on Lie group and Euclidean space, the proposed method has a comprehensive advantage in accuracy, computational efficiency, and Lie group structural conservativeness. Finally, the way of applying our method to aircraft simulation and optimal control is illustrated.For the future work, we will further analyze its performance in aircraft simulation and control. For presence of a large number of tedious commutator operator in multivariate quadrature, we will try to find an alternative method to simplify the calculation process so that extend our method to the higher order. Moreover, we will extend the proposed method to a broader kind of left invariant rigid-body dynamics systems in engineering.



---

*Source: 595086-2013-04-28.xml*