Formulation and Simulations of the Conserving Algorithm for Feedback Stabilization on Rigid Body Rotations

The problem of stabilization of rigid bodies has received a great deal of attention for many years. People have developed a variety of feedback control laws tomeet their design requirements andhave formulated various butmostly open loopnumerical algorithms for the dynamics of the corresponding closed loop systems. Since the conserved quantities such as energy, momentum, and symmetry play an important role in the dynamics, we investigate the conserved quantities for the closed loop control systems which formally or asymptotically stabilize rigid body rotation andmodify the open loop numerical algorithms so that they preserve these important properties. Using several examples, the authors first use the open loop algorithm to simulate the tumbling rigid body actions and then use the resulting closed loop one to stabilize them.


Introduction
The ideas of modern geometry may always find a role in the applications of the engineering sciences.Through geometry people can explore the structure of nonlinear problems in a natural and intuitive way.Feedback control of a rotating rigid body has become one of such research subjects.The model remains of fundamental interest from both a theoretical and applications point of view.From a theoretical point of view the nonlinear rigid body dynamics provides an important vehicle for the development of nonlinear control theory [1,2].At the same time the rigid body equations of motion appear in important applications ranging from spacecraft attitude control [3] to industrial robots [4].
For many systems it is essential that the algorithms used to simulate the dynamics produce physically meaningful results.To achieve this end algorithms should be based on physically sound models and capture without approximation the important physical characteristics of the actual system.Such quantities include symmetry, energy, momentum, Poisson, and symplectic structures.Several algorithms which preserve such invariants have been studied in detail.Early work in this area includes that of [5] who looked at energy and momentum conserving algorithms, [6] who focused on the integration of Poisson structure, [7] who considered algorithms which preserved symplectic structure, and [8] who used an imposed Lie-Poisson structure to determine the rigid body trajectory in Euclidean space.The conservation of such quantities is crucial in meaningful numerical simulation of systems possessing such structure.For example, consider the dynamics of a spin stabilized satellite.Such a satellite may have a rotational period on the order of several rotations per minute for the mission duration on the order of years.Failure to conserve angular momentum when simulating such a system will lead to significant errors and inaccurate conclusions about system performance.
In the area of stability analysis of uniform rigid body rotation the geometric framework has seen some development of methods.The energy-Casimir method was used in the context of Lie-Poisson systems to find stabilizing feedback control laws [9,10].A class of feedback control laws for the stabilization of uniform rigid body rotation was introduced in [11].The energy and momentum conserving algorithms described in [12] were used as the basic methodology for the numerical validation of these stabilizing control laws.These algorithms were suitably modified to incorporate feedback controls based on the methodology introduced and developed in these papers.
In this paper we will discuss the modifications which are necessary to apply the algorithm introduced in [12] to rigid body motion with feedback.While the modifications are straightforward they raise important issues with regard to numerical simulation of such systems.The results represent special cases of algorithms which conserve energy and momentum for general Hamiltonian systems [13,14].The remainder of the paper is then as follows.In the next section we outline the formulation of rigid body dynamics from a geometric point of view.We then show how the dynamics are modified when feedback is applied to the system.The following section then discusses the energy and momentum conserving algorithm.We then introduce and justify the modifications to the algorithm in the presence of feedback.We conclude this paper with several examples.

Rigid Body Dynamics
The unforced dynamics of a rigid body free to rotate about a fixed point possess two conserved quantities, the kinetic energy and the (spatially referenced) angular momentum.These quantities are constant with respect to unforced motion of the rigid body.If we allow this motion to be modified by state feedback, it is a remarkable fact that the closed loop systems also possess two quantities analogous to the energy and momentum of the original system [9].

The Open Loop Rigid Body Dynamics.
The dynamics of the rigid body free to move about a fixed point benefit from a geometric description [15,16].The configuration of the rigid body at any time can be described by an element of the special orthogonal group, that is, the set of all orthogonal matrices with determinant equal to one.We denote this group as (3).This group is a Lie group.
If we consider a motion of this rigid body this is represented by a continuous curve of elements of (3) parameterized by time.The tangent space to any point Λ in (3) consists of the tangent vectors to all the possible curves passing through such a point; we denote it by  Λ (3).Since (3) is a Lie group, it has an associated Lie algebra which we denote by (3).We can identify any vector in the tangent space  Λ (3) with two elements in the Lie algebra of (3) by translating this element to the identity by left or right translation.These two elements in the Lie algebra correspond to the convected angular velocity W and the spatial angular velocity w.In addition to the tangent space at the current configuration we can define a cotangent space  * Λ (3) through the duality pairing ⟨, w⟩ = (1/2)w  .As with an element of the tangent space, an element of the cotangent space can be translated to two elements in the dual to the Lie algebra  * (3).For the natural pairing these two elements correspond to the convected angular momentum Π and the spatial angular momentum .
The rigid body is a Hamiltonian system which has a phase space, a Hamiltonian, and a symplectic form.The phase space is denoted by  and defined as the cotangent bundle of (3) Locally we coordinatize elements of this phase space as (Λ, ) with Λ a rotation matrix and  a three-element angular momentum vector.We define a Hamiltonian function (Λ, ) on this phase space.In spatial coordinates Here I = ΛJΛ  denotes the spatially referenced inertia matrix and J is the body fixed inertia.Hamiltonian's equations are then Application of the Legendre transformation yields where w ∈ R 3 lies in the space of Lie algebra with the standard isomorphism ̂: R 3 →  (3).Note that R represents the set of real numbers.By standard arguments for Hamiltonian systems one then shows that the energy is conserved [2].
For the rigid body the symmetry induced by the action of (3) on the phase space provides an additional conserved quantity, the angular momentum.This symmetry is used to define a momentum map J :  →  * (3) corresponding to the invariance under rigid body rotations.It is easily shown that this corresponds to the classical notion of angular momentum; that is where u is an external moment applied to the system.Consider a general form of velocity feedback for the rigid body given by where a = ΛAΛ  , b = ΛB and A ∈ R 3×3 , B ∈ R 3 are constant matrices.Using ( 7) in ( 6) we obtain the closed loop dynamics Remarkably, with a more general definition of the phase space this system possesses conserved quantities analogous to the energy and momentum of the free rigid body.In what follows we will consider a special case of ( 8) by setting a = 0 in (7).Such feedback arises, for example, in the case of a spacecraft carrying momentum wheels.The second equation of ( 8) then takes the form The phase space of this system is the cotangent bundle of (3).We define the comomentum vector as  * = Iw + b.The vector b has the effect of "shifting" the angular momentum due to the rotation of the rigid body.It is straightforward to verify that  * = 0 along the trajectory.The Hamiltonian has the property that Ḣ = 0 along the flow given by ( 8) from which we conclude that the Hamiltonian is constant.
As before there exists a momentum map J which is conserved by the flow.This momentum map reflects the invariance of the dynamics with respect to rigid body rotations.It is shown that and that J = 0 is then a direct consequence of Noether's theorem [2].

Conserving Algorithms
Discretization of the closed loop dynamics (4) will not in general preserve the conserved quantities of energy and momentum.In fact, standard numerical methods such as Runge-Kutta or direct integration will in general remain valid for only short periods of time.For long term dynamics to retain the conserved quantities we must use algorithms which have been designed for this purpose.
Reference [17] developed a Lie-Poisson Hamilton-Jacobi theory leading to a symplectic integrator that preserves exactly the Lie-Poisson structure but does not typically conserve the energy.Actually it showed that the energy conserving algorithms cannot be symplectic at the same time in the nonintegrable or reduced space.Another almost Poisson symplectic integrator introduced by [18] with midpoint rule conserves the first integrals containing only linear and quadratic terms, especially the energy, but only conserves the Poisson structure up to the second order.Reference [12] constructed energy-momentum algorithms by balancing the spatial angular momentum which also has the second order accuracy in the Poisson structure that is compared with the symplectic integrator in [13].
In contrast with the previous algorithms for the open loop systems, we are interested in the closed loop systems based on the energy-momentum algorithm with a family of stabilizing feedback introduced in the work of [19].The idea can be found in [15,16] that for the Hamiltonian system (rigid body rotation in this case) with symmetries induced by the action of a Lie group ((3) here) on the phase space, there exist conserved energy and momentum map along the integral line of the dynamics.The reason we choose energy-momentum conserving algorithm is that the spatial angular momentum plays the most important role in the rigid body rotation and is dealt with explicitly in this algorithm.

Energy-Momentum Conserving Algorithm for Open Loop
System.We first review the open loop energy-momentum conserving algorithm of [12] in this section and then discuss the modifications for the closed loop system in the following section.
The equations of motion of a rigid body in spatial coordinate are expressed in (4).They can be interpreted in body coordinate with applied torque as with initial conditions, where Λ ∈ ( 3) is an element of the configuration space, W ∈ ( 3) is an element of the velocity field, U = Λ  u is the body fixed applied torque, and J is the body fixed moment of inertia.
The update of the configuration here is the exponential map exp: (3) → (3) from the Lie algebra to the group, which has the closed form called Rodrigues formula where Θ ∈ R 3 is the convected relative rotation vector lying in the linear vector space of Lie algebra.Therefore the open loop energy-momentum conserving algorithm for the initial value problem ( 12) is based on the Newmark algorithm [20] outlined as follows.
To accomplish the iteration by the Newton-Raphson method we need to linearize (18) about (Λ, Θ).Equation ( 18) is nonlinear in the finite rotation vector Θ.By [21] the linearization needs the following operations of the directional derivative in the direction where (⋅) ⋅ ΔΘ is the directional derivative of (⋅) in the direction ΔΘ and T : R ( Thus the tangent matrix K calculated by linearization is derived.We will use these equations again to derive new tangent matrix K * for the closed loop systems later.
It can be shown that for the choice of / = 2 this algorithm conserves energy exactly and thus is unconditionally stable [12].Moreover, if we choose  = 1 and  = 1/2, the updated angular acceleration A +1 does not depend on the initial A +1 during each time interval and we may avoid the accumulated errors from A  .Moreover, it can also be proved that if, in addition, we use the trapezoidal rule for the applied torque, that is,  = 1/2, then the algorithm is second order accurate both in configuration and velocities.

New Result for Closed Loop
System.When we close the loop with the linear stabilizing feedback of U = B × W by energy-momentum method mentioned in [20], (12) become which are the equations of motion for the closed loop system in body coordinate.As in Section 2.2 we define the convected shifted angular momentum Π * = JW+B.Clearly the norm of Π * is constant along the flow.We can also define the Hamiltonian  = (1/2)(Π * − B) ⋅ J −1 (Π * − B) in the dual Lie algebra, or as normally  = (1/2)JW ⋅ W in the Lie algebra, and both can be easily seen to be conserved.
Since we have closed the system with the constant feedback gain, there is no need to trace the control input and we do not need the parameter  anymore in our algorithm.We only have to set the constant vector B beforehand.
In the closed loop energy-momentum conserving algorithm we hope to have the same nice properties as those in the open loop one.We can achieve that by modifying the algorithm in the previous section.First, with the open loop algorithm at hand (24) can be discretized in the form between any typical time interval [  ,  +1 ] ⊂ [0, ] with the following: It can be proved that the norm of the shifted angular momentum is conserved in the closed loop algorithm when / = 2.
In addition to conservation of norm of the shifted momenta the direction of the shifted spatial angular momentum is also conserved if we enforce that  +1 + b +1 =   + b  .To do this we just replace Step 3 in the previous section by to modify the algorithm.Note that the linear feedback needs not to stabilize the rigid body rotation.In general, any feedback in the form U = B × W would conserve the shifted spatial angular momentum which is actually the momentum map for the Hamiltonian system.We will see this in some examples later.
The conservation of energy depends on properly choosing the parameters  and  as well.From the kinematic equations ( 16), (17) and the fact that exp( Θ)Θ = Θ we update where W *  = W  +(1/2)ℎA  .Besides, (16) yields W +1 +W  = (/ℎ)Θ + (2 − /)W *  .By eliminating W +1 in the previous equations and substituting into the balance equation it turns out to be the following: It can also be proved that the energy is conserved in the closed loop algorithm when / = 2. Again, if we choose the parameters  = 1 and  = 1/2, we are free from the error propagation of angular acceleration A  and the closed loop algorithm would be second order accurate.Moreover, it is unconditionally stable since the energy is conserved.
The linearization process of ( 18) starts from the balance equation ( 29) with the orientation update ( 17) which is a nonlinear equation in Θ. Define then the linearization of G(Θ) with respect to Θ becomes where Θ 0 is the fixed point at which the linearization is exerted.By ( 22) and ( 23) with the definition (32), we can compute the linearization of G(Θ) along ΔΘ in body coordinate as follows: Note that there is a term − B in the tangent matrix K * that is different from the result ( 20) of open loop system.This means we need to put one more term in dealing with the linearization of the balance equation for the closed loop system.
Based on the analysis above we treat the constant vector B as a controller and put it inside the algorithm.Modify the linearization process in the previous section and replace the momenta Π and  by Π * and  * to construct the new closed loop algorithm.

Numerical Examples
There are three examples that we are going to discuss in this section.The first two examples are rigid body rotations about the intermediate and arbitrary axes.The last example is a rotation with the asymptotic stabilizing control.

Intermediate Axis Stabilizing Control.
The linear feedback U = B × W introduced here is based on the energymomentum method and is a robustly stable controller in the face of uncertainties of moment of inertia.We will use the open loop algorithm to simulate the tumbling action and then use the closed loop one to stabilize it.
Initially we perturb the angular velocity a little bit to W 0 = (0.01, 20.01, 0.01)  so that the numerical instability occurs.See Figure 1.Without applied torque the rigid body tumbles and its convected angular momentum Π is shown in Figure 2(a).The energy H and momentum map  are conserved in Figures 2(b) and 2(c), respectively.
With the stabilizing vector B = (0.0, 40.0, 0.0)  corresponding to  = 2 (see [19]) the closed loop algorithm generates the constant shifted momenta Π * and  * in Figure 3, while the original  loses its stability.Actually  * is our momentum map in this case.The energy in Figure 3(c) shows that the closed loop system is Hamiltonian.
Strictly speaking, B = (0.0, 40.0, 0.0)  is the stabilizing control vector for W 0 = (0.0, 20.0, 0.0)  instead of the perturbed W  0 ; therefore the shifted body angular momentum actually oscillates nearby the axis W 0 due to the initial W  0 and the oscillation cannot be eliminated by our control vector B. We will resolve this problem by the variable structure control later in Section 4.3.
Without applied torque the system's convected momentum Π tumbles as shown in Figure 4 and the conserved energy H and momentum map  are shown in Figure 5.
Applying the stabilizing vector B = (6.0,12.0, 1.0)  , corresponding to  = 2 again, in the closed loop algorithm the energy H, momentum map  * , and stabilized convected momentum Π * are stable in Figure 6.
In addition, we apply the same vector B = (6.0,12.0, 1.0)  to the rotation of the same system about another arbitrary axis W 0 = (6.0,2.0, 0.0)  .As expected before, we can observe in Figure 7 that the energy H and momentum map  * are still conserved, just like the unforced rigid body rotations.If the initial body angular momentum of a closed loop rigid body system is away from any of the stable equilibria, the angular momentum will follow a periodic orbit about that equilibrium on the momentum ball.We say this equilibrium is Lyapunov stable in contrast with asymptotically stable.
For different feedback controls there are different shapes of the periodic orbits, especially the orbit of the body angular momentum projected on the plane perpendicular to the stabilized axis.Based on this fact, we can develop an asymptotic stabilizing control to asymptotically pull the initially perturbed angular momentum back to the designated equilibrium axis by using the variable structure control method.We next give an example to clarify this idea.Consider the same rigid body with the initial body angular velocity W 0 = (0.0, 20.0, 0.0)  (or angular momentum Π 0 = (0.0, 80.0, 0.0)  ) as in Section 4.1.This system rotates about the intermediate axis and thus is unstable.The projected phase flow around the intermediate axis on Π 1 -Π 3 plane is plotted in Figure 8.It shows that the intermediate axis is a saddle point.
With the feedback control law U = AW × W the closed loop equation becomes where J  is the modified moment of inertia.The first quadratic stabilizing control matrix in this example is chosen to be A (1) = diag (−2, 0, −1) , and the associate modified moment of inertia and control law are respectively.Figure 9 shows its phase flow on Π 1 -Π 3 plane.We can see the rotation about the intermediate axis (the center) has been stabilized and any of other rotations deviating from the center undergoes a periodic motion.Observe that in the first and third quadrants the flow goes away from the origin and in the second and fourth quadrants the flow goes near the center.
The second stabilizing control matrix we use is and its modified moment of inertia and control law are respectively.We plot the phase flow in Figure 10.Again, we observe that the flow in the first and third quadrants approaches the center and the flow in the second and fourth quadrants goes in the other way.
In order to asymptotically stabilize the rotation near the intermediate axis we combine these two control laws and take advantage of both feedbacks in alternative quadrants by switching; that is, (40) Figure 11 demonstrates the simulation of the trajectory on Π 1 -Π 3 plane when starting at Π 0 = (20.0,80.0, 0.0)  .As we expect the angular momentum asymptotically approaches the intermediate axis.Once the approach achieves some small level, the switching control can be turned off and any of stabilizing control family can be used to keep the intermediate axis rotation stable.
It should be noted that when the angular momentum is away from the equilibrium both control laws do work on the  (1) .system.Therefore, the energy is changing but converges to a constant associated with the intermediate axis rotation, and the spatial angular momentum has an attitude drift in space.As long as the initial perturbation is not large the deviations in both properties would not be significant.

Conclusion
In this paper we have outlined the formulations of rigid body dynamics from a geometric point of view for both open loop systems and closed loop ones with a family of the stabilizing linear feedback.It turns out that the momentum map of the closed loop system needs to be modified such that it preserves the analogous conserved quantities as well as the open loop system.To be more specific, we have to translate the spatial angular momentum by an amount of the constant feedback gains B and calculate all properties in this shifted dual to the Lie algebra.We also reviewed the energy-momentum conserving algorithm and made some modifications for 10 Mathematical Problems in Engineering  (2) .  (1and A (2) .the closed loop systems.We derived the conditions of choosing the parameters of Newmark algorithm to make the modified conserving algorithm unconditionally stable and second order accurate and conserve the magnitude and direction of momentum and the energy.A correction term also needs to be placed into the tangent matrix in the linearization process.Several numerical examples have been used to demonstrate the validity of the closed loop algorithm.The energy and shifted momentum map are conserved in these cases, and even if the feedback vector B does not stabilize the system.

4. 3 .
Asymptotic Stabilizing Control.In (7) the quadratic gyroscopic control aw × w modifies the phase flow in order to make some designated axes become equilibria, and the linear part b × w shifts the angular momentum to do the same job.In what follows, we consider the special case u = aw × w (or U = AW × W in body coordinates) by letting b = 0.

Figure 1 :Figure 2 :
Figure 1: Tumbling rigid body about intermediate axis without control.

3 Π 1 Figure 11 :
Figure 11: Phase flow near intermediate axis on Π 1 -Π 3 plane for a rigid body rotation about intermediate axis with switching control consisting of A(1) and A(2) .