Table of Contents Author Guidelines Submit a Manuscript
Mathematical Problems in Engineering
Volume 2015, Article ID 790409, 14 pages
http://dx.doi.org/10.1155/2015/790409
Research Article

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 [911]. 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

Table 1: Parameters of numerical integration tests [19].

To ensure that numerical integration test is more comparable, we consider the 4th-order LG-GPC method and select three other 4th-order numerical methods for comparison, including ode45 [23], explicit 4th-order RKMK [16], and 4th-order LG-PC method [22]. Furthermore, we compare the proposed numerical method with the Lie group variational integrator (LGVI) which is also based on geometrically exact computations on SO(3) [19]. Among the previous methods, ode45 is the built-in integrator of MATLAB, which is a variable-step method and whose results would be used as the ground truth with a specified tolerance . RKMK is a Lie group method [16]. Note that [24] states that the interpolatory quadrature formula with Gauss points is of th order. To this end, both the 4th-order LG-GPC method and the LG-PC method depend on 2 LG points. In addition, in the LG-GPC and LG-PC methods, the iterations of the inner while loop and the threshold of the equivalent configuration (or velocity) deviation are set to be 30 and , respectively. Since LGVI has an implicit equation, we need to solve it by using the Newton iterative approach, where the termination error of iteration and the maximum estimated value of the function are set to be and 9000, respectively. Numerical integration tests are all carried out in the same stepsize.

Figure 1 gives the angular configuration deviation and the average runtime per update of LG-GPC method when compared with LG-PC, RKMK, and LGVI, respectively, at five different steps. Figure 1(a) shows the angular configuration deviation against steps by calculating , where denotes the angular configuration of the free-floating rigid body which is computed by ode45. It has been illustrated that the accuracy of LG-GPC is superior to those of LG-PC, RKMK, and LGVI. Figure 1(b) shows the average runtime per update of LG-GPC methods when compared with LG-PC, RKMK, and LGVI. As it can be seen, the computational efficiency of LG-GPC is inferior to that of RKMK, because RKMK is an explicit method. However, the proposed method requires less computation time than the classical LG-PC method and is nearly 1.5 times as efficient as the LG-PC method on average. More obviously, the computational efficiency of the proposed method is superior to that of LGVI, since the latter needs the Newton iteration approach to solve the implicit approach.

Figure 1: Accuracy and computational efficiency of LG-GPC when compared with LG-PC and RKMK.

Additionally, the Lie group structural conservativeness of LG-GPC is compared with those of LG-PC, RKMK, and LGVI. Considering that the angular configuration is parameterized by unit quaternion in the ode45 method, we only compare the proposed method with the LG-PC method, explicit RKMK, and LGVI. As mentioned above, since the group element on SO(3) satisfies , we compute the infinite norm of for evaluating the structural conservativeness [19]. Figure 2 illustrates that the LG-PC method belonging to the Euclidean space has no group structural conservativeness, because it is not a Lie group method. However, RKMK, LGVI, and LG-GPC method on the Lie group are able to preserve the Lie group structure with an accuracy approaching the machine accuracy , and what is more, the LG-GPC is even superior to RKMK and LGVI.

Figure 2: Lie group structural deviation of LG-GPC when compared with LG-PC, RKMK, and LGVI.

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

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

Consider the following OAC problem with differential kinematical and dynamical constraints on SO(3).

Problem 9 (OAC problem with kinematical and dynamical constraints on SO(3)). Minimize the continuous-time costwhere is the control vector and is the weighting matrix, where ,(1)satisfying the differential kinematical and dynamical constraints on SO(3),(2)and satisfying the boundary conditionswhere , , , and denote the initial angular configuration, the initial angular velocity, the terminal angular configuration, and the terminal angular velocity, respectively.

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

Different from the LG-PC direct transcription of an NLP problem in the Euclidean space [25], we transcribe the continuous Problem 9 into a discrete NLP problem by LG-GPC discretization. In general, an NLP problem has a standard form as follows.

Minimize the costsatisfying the equality and the inequality as follows:where the vector is the whole decision variable of the NLP problem, denotes the whole equality constraints, and denotes the whole inequality constraints. Denotewith the constraints vector of the NLP problem.

For this purpose, we refer to the LG-PC transcription process introduced in [25], for transcribing Problem 9 into an NLP problem. Thus, the OAC problem on SO(3) could be formulated as follows.

Algorithm 10 (transcription of the OAC problem on SO(3) based on LG-GPC discretization).
Step 1. Mapping of the decision variables.
The total vector of optimization variables is given aswhere is given aswhere the column vectors are given aswhere is the value of the th component of states at the th discretization point.
Similarly, is given aswhere the column vectors are given aswhere is the value of the th component of the control at the th LG collocation point.

Step 2. Construction of the constraint vector .

Step 2.1. Construction of the defect constraints .

For , refer to (12) and (13), and define the defect constraints as

For configuration , refer to (26) and (27), and define the other part of the defect constraints as

Then we can define the matrix as

The matrix can be then isomorphically mapped to the column vector of equality constraints using a pseudocoded version of the MATLAB column-stacking operator “:” as .

Step 2.2. Construction of the constraint vector , without inequality constraints.

We can represent the column vector as

Step 3. Automatic scaling of the NLP.

Step 4. Conversion of the NLP sparsity pattern.

Step 5. Separation of constraint and nonconstraint derivatives in the constraint Jacobian.

Step 6. Relation of the Lagrange multiplies of NLP with the costate of the continuous-time optimal control problem using costate mapping.

Step 7. Since solving the NLP from the geometric pseudo-spectral method results in controls at only the interior points, it is necessary to obtain the endpoint controls after the NLP is solved. The method for endpoint control computation is to employ the Pontryagin minimum principle using the endpoint values of the state and costate.

Remark 11. Since Steps 3~7 are more or less the same as those of [25], the repetitious details need not be given here.

4.3. Simulation Example

In this section, we take controlling an underactuated 3D pendulum as an example to examine the performance of the previous method. The 3D pendulum (illustrated in Figure 3) is, simply, a rigid body, supported at a fixed pivot point, which has three rotational degrees of freedom; it is acted on by a uniform gravity force and, perhaps, by control and disturbance forces and moments [26].

Figure 3: A schematic of a 3D rigid pendulum.

The attitude kinematics and dynamics of the pendulum on SO(3) are given bywhere the attitude configuration is a generalized coordinate of the rotation angular configuration of the body-fixed frame relative to the inertial frame , satisfyingwhere denote the Euler angles and the angular velocity vector in the body-fixed frame could be represented as , in which denote the angular velocity component, respectively, about -, -, and -body axes. Thus, is equal to . The conservative force could be easily obtained as , where is the mass of the pendulum and is body-fixed vector from the pivot to the center of mass of the pendulum. denotes the direction vector of gravity in the inertial coordinate frame. The nonconservative force satisfies with being the constant conjugate vector and the control vector could be represented as , in which denote the control components about - and -body axes, respectively. The detailed properties of the 3D pendulum are presented in Table 2.

Table 2: Properties of a 3D rigid pendulum [7].

Consider the OAC problem of the 3D pendulum on SO(3), which is to control the pendulum from the initial fixed angle and the initial fixed angular velocity to the terminal fixed angle and the terminal fixed angular velocity with the minimum moment of the external force. The simulation parameters of the OAC problem are presented in Table 3.

Table 3: The simulation parameters of the OAC problem.

The proposed optimal control method (LG-GPC) is also compared with the prior works in [7, 911], which are also based on geometrically exact computations (Lie group variational integrator) on SO(3). There are two major categories of methods for solving the discrete first-order optimal necessary conditions derived from the optimal control problem: the nonlinear root-finding method (NRF) and the shooting method. In terms of the former, Bloch et al. [9] and Hussein et al. [10] propose a geometric structure-preserving optimal control method on SO(3), which applies a standard Newton iterative approach to solve the discrete first-order necessary optimality conditions. However, this method does not consider the moment of the conservative force and therefore could not be compared with our method immediately. To this end, we resort to a more general discrete geometric optimal control method on Lie groups proposed in [11], which consider both the moment of conservative force and that of external nonconservative force. In this method, for the 3D pendulum, the dimension of discrete decision variables is with the discretization number being and the maximum iterative number being . Termination tolerances on the constraint violation, the function value, and argument are , , and , respectively. In terms of the latter, Lee et al. [7] use a line search with the backtracking algorithm, referred to as “Newton-Armijo iteration (NAI),” to solve the discrete first-order necessary optimality conditions, where the time stepsize is  s and the number of integration steps is . Figure 4 gives the optimal states and controls obtained from the LG-GPC method when compared with LG-PC and NRF.

Figure 4: The optimal state and control of the pendulum obtained by LG-GPC, LG-PC, and NRF.

As seen from Figure 4, the results obtained from LG-GPC and LG-PC satisfy the boundary condition of states and control, except those of NRF, because the first two could consider constraints of states and control explicitly, the latter has no choice but to introduce a dummy factor. Table 4 illustrates the optimal costs and operation time of LG-GPC, LG-PC, NRF, and NAI. It is concluded from the optimal costs that the methods based on LGVI are superior to LG-GPC and LG-PC, since the former belong to indirect method that is higher in accuracy. However, the last two have not only larger convergence radius, but also higher convergence rate. With the numerical advantage of the geometric collocation discretization, LG-GPC has the least operation time.

Table 4: Optimized performance index and operation time.

5. Conclusions

This paper shows how to apply the pseudo-spectral collocation method to a constrained OAC problem for a rigid body on SO(3). First, we establish a left-invariant rigid-body attitude dynamical model in a body-fixed frame on SO(3). Then, we propose a geometric collocation method which has a comprehensive advantage in accuracy, computational efficiency, and the Lie group structural conservativeness. Preservation of key motion invariance leads to robust dynamics approximation. Finally, the proposed method is applied to a constrained OAC problem.

For future work, we will further compare the method with more typical numerical methods in the Euclidean space and on the Lie group, to test the efficiency of our method. Moreover, we will extend the proposed method to a broader category of left-invariant rigid-body dynamics systems in engineering.

Conflict of Interests

The authors declare that there is no conflict of interests regarding the publication of this paper.

Acknowledgment

This work was supported by the National Natural Science Foundation of China (61403410).

References

  1. A. V. Rao, “A survey of numerical methods for optimal control,” in Proceedings of the AAS/AIAA Astrodynamics Specialist Conference, AAS Paper 09-334, AAS/AIAA, Pittsburgh, Pa, USA, August 2009.
  2. F. Lewis and V. Syrmos, Optimal Control, John Wiley & Sons, 1995.
  3. L. S. Pontryagin, V. Boltyanskii, R. Gamkrelidze, and E. Mischenko, The Mathematical Theory of Optimal Processes, Interscience Publishers, New York, NY, USA, 1962.
  4. H. B. Keller, Numerical Solution of Two Point Boundary Value Problems, SIAM, 1976.
  5. J. Stoer and R. Bulirsch, Introduction to Numerical Analysis, vol. 12 of Texts in Applied Mathematics, Springer, New York, NY, USA, 2002. View at Publisher · View at Google Scholar
  6. F. Bullo and R. Murray, “Proportional derivative (PD) control on the Euclidean group,” in Proceedings of the European Control Conference (ECC '95), Rome, Italy, September 1995.
  7. T. Lee, M. Leok, and N. H. McClamroch, “Optimal attitude control of a rigid body using geometrically exact computations on SO(3),” Journal of Dynamical and Control Systems, vol. 14, no. 4, pp. 465–487, 2008. View at Publisher · View at Google Scholar · View at Scopus
  8. T. Lee, Computational Geometric Mechanics and Control of Rigid Bodies, ProQuest, Ann Arbor, Mich, USA, 2008.
  9. A. M. Bloch, I. I. Hussein, M. Leok, and A. K. Sanyal, “Geometric structure-preserving optimal control of a rigid body,” Journal of Dynamical and Control Systems, vol. 15, no. 3, pp. 307–330, 2009. View at Publisher · View at Google Scholar · View at Zentralblatt MATH · View at Scopus
  10. I. I. Hussein, M. Leok, A. K. Sanyal, and A. M. Bloch, “A discrete variational integrator for optimal control problems on SO(3),” in Proceedings of the 45th IEEE Conference on Decision & Control, pp. 6636–6641, IEEE, San Diego, Calif, USA, December 2006. View at Scopus
  11. M. B. Kobilarov and J. E. Marsden, “Discrete geometric optimal control on lie groups,” IEEE Transactions on Robotics, vol. 27, no. 4, pp. 641–655, 2011. View at Publisher · View at Google Scholar · View at Scopus
  12. R. Moulla, L. Lefèvre, and B. Maschke, “Geometric pseudospectral method for spatial integration of dynamical systems,” Mathematical and Computer Modelling of Dynamical Systems, vol. 17, no. 1, pp. 85–104, 2011. View at Publisher · View at Google Scholar · View at Zentralblatt MATH · View at Scopus
  13. K. Engø, “On the construction of geometric integrators in the RKMK class,” Tech. Rep. 158, Department of Computer Science, University of Bergen, Bergen, Norway, 1998. View at Google Scholar
  14. R. M. Murray, Z. Li, and S. S. Sastry, A Mathematical Introduction to Robotic Manipulation, CRC Press, 1994.
  15. D. A. Benson, A gauss pseudospectral transcription for optimal control [Ph.D. thesis], Department of Aeronautics and Astronautics, Massachusetts Institute of Technology, Cambridge, Mass, USA, 2004.
  16. H. Munthe-Kaas, “Runge-kutta methods on Lie group,” BIT, vol. 35, pp. 572–587, 1995. View at Google Scholar
  17. J. S. Hesthaven, S. Gottlieb, and D. Gottlieb, Spectral Methods for Time-Dependent Problems, Cambridge University Press, 2007. View at Publisher · View at Google Scholar
  18. J. T. Betts, “Survey of numerical methods for trajectory optimization,” Journal of Guidance, Control, and Dynamics, vol. 21, no. 2, pp. 193–207, 1998. View at Publisher · View at Google Scholar · View at Scopus
  19. T. Lee, N. H. McClamroch, and M. Leok, “A lie group variational integrator for the attitude dynamics of a rigid body with applications to the 3D pendulum,” in Proceedings of the IEEE Conference on Control Applications (CCA '05), pp. 962–967, IEEE, Toronto, Canada, August 2005. View at Publisher · View at Google Scholar
  20. A. Iserles, H. Z. Munthe-Kaas, S. P. Nørsett, and A. Zanna, “Lie-group methods,” Acta Numerica, vol. 9, pp. 215–365, 2000. View at Publisher · View at Google Scholar
  21. A. Iserles and S. P. Nørsett, “On the solution of linear differential equations in Lie groups,” Philosophical Transactions of the Royal Society A, vol. 357, no. 1754, pp. 983–1020, 1999. View at Publisher · View at Google Scholar · View at Scopus
  22. L. N. Trefethen, Spectral Methods in Matlab, SIAM, Philadelphia, Pa, USA, 2000. View at Publisher · View at Google Scholar · View at MathSciNet
  23. J. R. Dormand and P. J. Prince, “A family of embedded Runge-Kutta formulae,” Journal of Computational and Applied Mathematics, vol. 6, no. 1, pp. 19–26, 1980. View at Publisher · View at Google Scholar · View at Scopus
  24. E. Hairer, C. Lubich, and G. Wanner, Geometric Numerical Integration: Structure-Preserving Algorithms for Ordinary Differential Equations, vol. 31 of Springer Series in Computational Mathematics, Springer, Berlin, Germany, 2002.
  25. A. V. Rao, D. A. Benson, C. L. Darby et al., “Algorithm 902: GPOPS, a MATLAB software for solving multiple-phase optimal control problems using the gauss pseudospectral method,” ACM Transactions on Mathematical Software, vol. 37, no. 2, article 22, 2010. View at Publisher · View at Google Scholar
  26. J. Shen, A. K. Sanyal, N. A. Chaturvedi, D. Bernstein, and N. H. McClamroch, “Dynamics and control of a 3D pendulum,” in Proceedings of the 43rd IEEE Conference on Decision and Control (CDC '04), pp. 323–328, Paradise Island, Bahamas, December 2004.