Mathematical Problems in Engineering

Volume 2015, Article ID 790409, 14 pages

http://dx.doi.org/10.1155/2015/790409

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

College of Mechatronic Engineering and Automation, National University of Defense Technology, Changsha, Hunan 410073, China

Received 22 August 2015; Accepted 13 October 2015

Academic Editor: Naohisa Otsuka

Copyright © 2015 Xiaojia Xiang et al. This is an open access article distributed under the Creative Commons Attribution License, which permits unrestricted use, distribution, and reproduction in any medium, provided the original work is properly cited.

#### Abstract

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

#### 1. Introduction

Attitude control is one of the basic technologies that an object’s motioning relies on, and thus it has been the research focus in motion control technology. For object motion, to enhance its performance and security, it is required to save its energy consumption and advance control performance itself, and the new control methods continue to emerge. There is no doubt that optimal control theory is the most natural framework toward conformity with meeting the above requirements. However, except that the linear quadratic issues such simple question, analytical solutions are seldom available or even possible. As a result, more often than not, one resorts to numerical techniques. Numerical methods for solving optimal control problems are divided into two major classes [1]: indirect methods and direct methods. In an indirect method, the calculus of variations [2] and the Pontryagin maximum principle [3] are used to determine the first-order optimality conditions of the original optimal control problem, so, it converts the optimal control problem to a two-point boundary-value problem, including indirect shooting method [4] and indirect multiple-shooting method [5]. The primary advantage of the indirect methods lies in their high accuracy and assurances that the solution satisfies the necessary optimality conditions. However, indirect methods also have significant disadvantages. First, the necessary optimality conditions must be derived analytically; for most problems this derivation is difficult; Second, the radius of convergence is typically small. Third, a guess for the costates must be provided. In a direct method, the state and/or control of the optimal control problem is discretized in some manner and the problem is transcribed into a nonlinear programming problem (NLP), and, then, the NLP is solved using well-known optimization techniques, including direct shooting, collocation methods. Although not as high in precision as indirect methods, direct methods also have advantages because the optimality conditions do not need to be derived, and they have a large radius of convergence.

In general, the attitude of an object is represented by the Euler angle. However, when the object is regarded as a single rigid body in three-dimensional space, the configuration of its rotation actually constructs a special Lie group called the special orthogonal group SO(3) [6]. In recent years, some researchers have applied optimal control on a Lie group to attitude control of a 3D rotating rigid body, to reduce the computational burden, or to provide a coordinate-free approach to avoid expensive chart switching, special handling of singularities, and numerical drift [7, 8]. Bloch et al. [9] and Hussein et al. [10] study a discrete variational optimal control problem for the rigid body on SO(3), and they describe the dynamical equations of the mechanical system as a geometric structure-preserving model referred to as a Lie group variational integrator and then use Lagrange’s method to derive the discrete necessary conditions (i.e., two-point boundary-value problem) which are solved by a standard nonlinear root-finding method. Lee et al. [7] propose an accurate computational approach for a nonconvex optimal attitude control (OAC) for a rigid body, in which the use of geometrically exact computations on SO(3) guarantees that the approach has excellent convergence properties. Similar to [7], the problem is also formulated directly as a discrete time optimization problem using a Lie group variational integrator; the discrete time necessary conditions for optimality are derived and solved by an efficient computational shooting method. Kobilarov and Marsden [11] extend the above conclusion to more general optimal control problems of mechanical systems on Lie groups and develop an efficient numerical method that exploits the structure of the state space and preserves the system motion invariants. The above-mentioned works have some common features: first, instead of discretizing equations of motion, they all use a discrete variational integrator obtained from the discrete Lagrange-d’Alembert principle, a process that better approximates the equations of motion; second, they are all indirect optimal control methods. In this paper, we turn to direct methods to solve the OAC problem on SO(3). Among the direct methods, the collocation method, also known as a pseudo-spectral method, is widely used in rigid body attitude optimization (including UAV, satellite, space shuttle, launch vehicle, and supersonic aircraft) by virtue of its high accuracy and spectral (or exponential) convergence rate [1]. In the context of a Lie group, to our knowledge, Moulla et al. [12] are the first to propose the concept of the “geometric pseudo-spectral method.” They suggest a polynomial pseudo-spectral method that preserves the geometric structure of port-Hamiltonian systems, phenomenological laws, and conservation laws without introducing any uncontrolled numerical dissipation. However, their method is designed only for port-Hamiltonian systems in a special structure, which implies that the Dirac structure, therefore, could not be directly extended to the systems on SO(3). Thereby, how to apply the pseudo-spectral method to the OAC problem on SO(3) and conserve its geometric properties is still an open question, which is also the major topic studied in this paper.

In this work, we study a constrained attitude control problem for the rigid body on SO(3) based on pseudo-spectral collocation method. First, we establish a left-invariant rigid-body attitude dynamical model in body-fixed frame on SO(3). Second, for the left invariance of attitude configuration in the body-fixed frame, we provide a discretization mechanism known as the geometric collocation method on SO(3), which may improve the accuracy of evolving trajectory and the convergence rate through applying different collocation strategies to the configuration equation and the velocity equation, respectively. For the configuration equation, since its geometric properties could not be preserved if the general pseudo-spectral method is directly applied to it, drawing on Engø’s idea about the equivariant map [13], we transform the configuration equation on SO(3) into an equivalent equation in the corresponding Lie algebra space [14] and then the solutions to the equivalent equation at discrete points are transformed into configuration at corresponding points via the coordinate map. For the velocity equation, we can use the general pseudo-spectral method directly to compute the velocities at the discrete points that are the same as those of configuration, since the velocity belongs to the Lie algebra space that is isomorphic to the Euclidean space. Under the condition that it has the same 4th order and large stepsize, the proposed method is compared with the general Gauss pseudo-spectral method [15], explicit RKMK [16], and Lie group variational integrator [7, 8]. We find that the accuracy of the proposed method is superior to other methods, except that the explicit RKMK demonstrates a higher convergence rate than the other methods. Furthermore, the method could preserve the structural characteristics of the Lie group precisely. Finally, we apply the above numerical method to a constrained OAC problem. In addition, the numerical method is used to solve the constrained OAC problem on SO(3). The optimal control problem is transcribed into a nonlinear programming problem, in which the equivalent Lie algebra equation is considered as the defect constraints instead of configuration equation. The transcription method is coordinate-free and does not need chart switching or special handling of singularities. More importantly, with the numerical advantage of the geometric collocation method, the OAC method exhibits a high convergence rate.

The rest of this paper is organized as follows. Section 2 proposes a left-invariant rigid-body attitude dynamical model on SO(3). The geometric collocation method for the left-invariant attitude dynamics system on SO(3) is given in Section 3, where Section 3.1 provides a general pseudo-spectral collocation method in the Euclidean space, Section 3.2 presents the Legendre-Gauss pseudo-spectral collocation method on SO(3), and, then, the corresponding algorithm is given in Section 3.3, and numerical tests are carried out on a free-floating rigid body model in comparison with four other numerical methods in Section 3.4. In Section 4, the proposed collocation is applied to a typical OAC problem and compared with several prior works. Finally, the conclusions and future work are outlined in Section 5.

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

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

The motion of a rigid body on Lie group could be described in a space frame or a body-fixed frame. To avoid confusion, we would consider the motion of a single rigid body in a body-fixed frame in the sequel.

*Definition 1 (left-invariant mechanical systems on Lie group [6]). *In a body-fixed frame, the motion of a rigid body could be described as a left-invariant mechanical system as follows:where denotes the configuration of a rigid body in and denotes the velocity of the rigid body in the body-fixed frame, where , known as the Lie algebra, is the tangent space at the identity of and “” is the hat operator satisfying isomorphism with “” being its inverse mapping. represents the left translation; that is, , , and is its tangent space at . For the matrix group, such as SO(3), SE(2), and SE(3), (1) is equivalent to . “Left-invariant” implies that the mechanical system is invariant under left multiplication by constant matrices.

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

SO(3) is called the special orthogonal group, whose group element is the rotational component of the body-fixed frame relative to the space frame [6]. The left-invariant kinematics of attitude motion of a rigid body on SO(3) are given bywhere and denote the attitude configuration and angular velocity of a rotating rigid body on SO(3). The hat operator , for all , satisfying , where is vector cross product [6].

And it is known that the attitude dynamics of the rigid body are generally satisfying [10, 11]where is referred to as the inertia matrix of the rigid body, denotes the moment of the conservative force with being potential energy, and is that of the nonconservative external force with being the control conjugate vector.

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

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

Before collocating the left-invariant mechanical system on SO(3), we briefly describe the basic principle of the Legendre-Gauss pseudo-spectral collocation (LG-PC) method [1]. Take, for example, solving the following differential equation in at ,

First, we equally divide a time interval into several time subintervals with the stepsize being and transform the time interval into via the affine transformation . Thus, accordingly, (4) over becomes

Let be collocation points in and . We approximate the solution of (5) by the following formula:where denotes Lagrange polynomials, satisfying the isolation property

Equation (6) together with the isolation property leads to , thereby .

Through expressing the derivative of the Lagrange polynomials at the collocation points in differential matrix form , where

we can write (5) at the collocation points as a set of differential algebra equations:

Based on the above equations, we establish the following defect equations,and apply iterative algorithms to the above equations so as to determine the approximation to at the collocation points. Finally, according to the following formula, we obtain the approximation to (4) at the endpoint of time subinterval :where is quadrature weight corresponding to the collocation points. This approximate solution will be the initial value of over and so on.

*Remark 2. *According to different selection methods of collocation points, pseudo-spectral collocation methods can be divided into two categories, namely, standard and orthogonal methods. Collocation points in orthogonal collocation are those obtained from the roots of either Chebyshev polynomials or Legendre polynomials belonging to orthogonal polynomials [17]. The benefit of using orthogonal collocation over standard collocation is that the quadrature approximation to a definite integral is extremely accurate [1]. Furthermore, according to whether the endpoint is a collocation point, pseudo-spectral collocation methods fall into three general categories [18]: Gauss methods, where neither of the endpoints −1 or 1 is a collocation point; Radau methods, where at most one of the endpoints −1 or 1 is a collocation point; and Lobatto methods, where both the endpoints −1 and 1 are collocation points. In the rest of this paper, we will develop our geometric collocation method based on Legendre-Gauss points, since when Legendre-Gauss methods are used for transcribing the continuous optimal control problem into a discrete nonlinear programming problem, it does not suffer from a defect in the optimality conditions at the boundary points for the endpoints are not collocation points [15].

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

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

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

For attitude dynamics, since its variable belongs to that is isomorphic to , (3) can be collocated at Gauss collocation points directly by the Legendre-Gauss pseudo-spectral collocation method as follows:

Similarly, the approximate solutions of , , are approached by several times of the Gauss-Seidal type iterative operation. Then, the approximation solutions are used to obtain the velocity at the endpoint :

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

To transform (2) on SO(3) into an equivalent differential equation evolving on , we briefly describe the basic idea of the equivalent map.

*Definition 3 (equivariant map [13]). *Let and be manifolds and let be a Lie group which acts on by and on by . A smooth map is called equivariant with respect to these actions if, for all ,which indicates the following diagram commutes.

Diagram commutes of the equivariant map are as follows:First, from the definition of an action of on , we can obtain an equivariant map with respect to the left translation action of on itself and an action of on :It is known that there is a local coordinate map on with the Lie algebra , and the most typical one is the exponential map . At this point, we need to find an action of on such that will be an equivariant map with and the left action of on itself, namely,In case is the exponential map, is nothing other than the well-known Baker-Campbell-Hausdorff (BCH) formula , where is called the logarithm map. Since the composition of two equivariant maps is also an equivariant map, we can construct an equivariant map from to with respect to the action on and on .

Diagram commutes of composition are as follows:Theorem 3.6 in literature [13] states that if is an equivariant map, then the infinitesimal generators of the action with respect to the same element are -related vector fields. Thus, the infinitesimal generators and of the flows and , respectively, on and are -related; that is,Finally, we need to determine what is, and the following theorem gives the conditions that it needs to meet.

Lemma 4 (see [13]). *Let be a coordinate map on and equivariant with respect to the flows and . The infinitesimal generator of satisfying (19) is . is the trivialization defined as .*

Based on Lemma 4 and other previous conclusions, we can derive the equivalent equation on of the left-invariant configuration equation.

Proposition 5 (equivalent equation on of the left-invariant configuration equation). *The configuration equation (2) has an equivalent equation on as follows:where is a local coordinate map on SO(3) with being its differential.*

*Proof. *Without any loss of generality, we assume that the solution of (2) can be written in the following form:Then, by differentiating it [20], we can obtainComparing the preceding formula with (2) and (21), we haveTaking the inverse of the preceding formula, we obtain the equivalent equation (20) on of the left-invariant configuration equation (2).

There are several choices of the coordinate map , such as the exponential map and Cayley map [21]. In case is the exponential map “” the vector field on can be represented as [20]where the adjoint operator is a Lie bracket or commutator, , satisfying the recursive expression , and are Bernoulli numbers [21]:

Now, we solve (20) by the LG-PC method as follows:

In view of the mechanical system being autonomous, we may use Gauss-Seidal type iteration to update the approximate solution of (26) as soon as possible [22]. As mentioned earlier, after obtaining the approximate solution, the velocity at the endpoint can be obtained according to the following formula:

It is worth noting that, different from the LG-PC method in the Euclidean space, the initial value in is not equal to the endpoint value in . It should be calculated through the logarithmic map :withwhere satisfies , and . It is easy to know that , are identically equal to zero.

At last, the attitude configuration at the endpoint can be reconstructed from according to , where and the Cayley map should be selected as the coordinate map , for two reasons [11]: first, it provides only an approximation to the integral curve defined by the exponential map, which is however easy to calculate; second, it does not involve trigonometric functions so that its derivatives do not have any singularities and thus will not cause any difficulties when used by gradient-based methods for solving NLP problems. The Cayley map satisfies the following formula:where is a standard Euclidean norm.

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

In accordance with the aforementioned LG-GPC method, we develop a Legendre-Gauss geometric pseudo-spectral collocation algorithm for integrating the kinematics and dynamics of attitude motion of a rigid body on SO(3). This algorithm would be further used to discretize the OAC problem in Section 4.

*Algorithm 6 (LG-GPC). *Inputs include time interval , stepsize , number of LG points , initial attitude configuration , initial angular velocity , iterations of inner while loop , and threshold of equivalent configuration (or velocity) deviation .*Step 1*. Initialization is as follows: let be zero.*Step 2*. Compute initial value of the equivalent configuration at in terms of :*Step 3* (*while* ()).*Step 3.1*. Compute the LG points in , differentiation matrix , and quadrature weights .*Step 3.2*. Set the current iterative step number and configuration (or velocity) deviation .*Step 3.3*. Let , be let and be .*Step 3.4* (*while* ( and )).*Step 3.4.1*. Obtain the submatrix of the matrix :*Step 3.4.2*. According to the following equations, obtain :*Step 3.4.3*. Update angular velocities at LG points:*Step 3.4.4*. According to the following equations, obtain :*Step 3.4.5*. Update the equivalent configuration at LG points:*Step 3.4.6*. Update the configuration at LG points:*Step 3.4.7*. Compute the equivalent configuration deviation and the velocity deviation within two adjacent iterative steps, and, then, let the larger one be the final deviation at the current step:*Step 3.4.8*. Substitute , , and into , , and , respectively, where , and let .*Step 3.5*.* End while*.*Step 3.6*. Relying on the final iterative values , with , in inner while loop, compute the angular velocity at the endpoint :*Step 3.7*. Also, based on the final iterative values , , in the inner while loop, compute the equivalent configuration at the endpoint :*Step 3.8*. Based on the previous formula, compute the configuration at the endpoint :*Step 3.9*. Let , , and .

*Step 4*.* End while*.

*Remark 7. *The LG points , , are the roots of the Legendre polynomial of degree . In fact, it is convenient to compute the LG points via the eigenvalues of the following tridiagonal Jacobi matrix:The LG pseudo-spectral differentiation matrix is given as [22]withThe LG weights corresponding to the LG points are computed as

*Remark 8. *In Step 3.3, both and cannot be set to , because shows the projection of distance between and on the tangent space.

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

In this subsection, we will test the validity and efficiency of the proposed method through numerical integration of the following free-floating rigid body kinematics (46) and dynamics (47) on SO(3). Note that (46) and (47) are equivalent to (6) and (7) under the condition of zero external force. Table 1 summarizes the parameters used by numerical tests. Consider