Rigid Body Kinematics Package Tutorial

Authors: 	Hanspeter Schaub and John L. Junkins
Affiliation:	Texas A&M University
Date:		August 4th, 1999

Abstract

The rigid body kinematics package provides many commonly used  functions in dealing with rigid body attitude coordinates.  Transformations between the direction cosine matrix, Euler parameters, Gibbs vector, modified Rodrigues parameters, principal rotation vector and the 12 sets of Euler angles are provided.  Whenever possible, direct analytical transformations between these sets are used to provide computationally efficient code.  Furthermore, the ability to perform two successive rotations and compute the resulting overall attitude vector is provided for each attitude coordinate choice.  Similarly, the package allows for the relative attitude vector between two given attitude vectors to be computed.  Finally, subroutines are provided that compute the matrix relating the attitude parameter rates to the body angular velocity vector, along with the analytic inverse of this matrix.  With these routines the attitude vector time derivative is computed for a given body angular velocity vector.

Load Package

To load the package first add the directory where the package is located to the $Path variable so that Mathematica can find the package (in the following you need to replace "directorypath" with the actual directory path on your machine):

AppendTo[$Path, "directorypath"]

Now load the package:

Needs["RigidBodyKinematics`"]

Coordinate Transformations

It this section we illustrate how to make use of the attitude coordinate transformation command.  Many types of attitude coordinates exist to describe the orientation of a rigid body or reference frame, but they all exhibit different singular behavior.  For example, the commonly used Euler 3-2-1 (yaw, pitch and roll) angles go singular when describing a +/- 90 degree pitch rotation.

Let θ be a 3-2-1 Euler angle vector whose components are the standard yaw, pitch and roll angles.

θ = {30, -20, 45} (π/180) // N

Note that the angles must be entered in radians, not degrees.  The corresponding direction cosine matrix [C(θ)] is computed through

\(\(Cθ\  = \ Euler3212C[θ]\ ; 
MatrixForm[Cθ]\ 
    \)\)

Let us verify that this 3x3 matrix is indeed orthogonal by verify that its inverse is equal to its transpose.

MatrixForm[Cθ.Transpose[Cθ]]

Using the rigid body kinematics commands, we are easily able to translate the yaw, pitch and roll angles into other attitude coordinates.  For example, the corresponding Euler parameters (quaternions) are

EP  =  Euler3212EP[θ]

The corresponding Modified Rodrigues Parameter (MRP) vector is

MRP  =  Euler3212MRP[θ]

Note that this function always returns the MRP vector with a vector magnitude less than or equal to 1.  The second feasible MRP set, also referred to as the shadow MRPs, is found through

MRP2  =  MRPshadow[MRP]

The function MRPswitch checks to see if the magnitude is of the MRP vector is larger than a given scalar value.  If yes, than the attitude vector is mapped to the alternate, shadow set.

MRPswitch[MRP2, 1]

To guaranteed that both MRP and MRP2 represent the same attitude, we translate each set back to the direction cosine matrix.

MRP2C[MRP] // MatrixForm

MRP2C[MRP2] // MatrixForm

Clearly each MRP vector is a parameterization of the same rotation matrix [C].

Attitude Vector Addition and Subtraction

Assume the first aircraft attitude is given by the yaw, pitch and roll angles

θ = {30, 10, 0} (π/180) // N

A second aircraft a its attitude given in terms ofthe MRP vector

σ  =  {.2, -.1, .1}

To find the relative attitude of the first aircraft to the second aircraft we use the sub... command.  Note that the MRP vector must first be translated into 3-2-1 Euler angles.  The rotational additiona and subtraction command assume both attitude descriptions are given in the same type of coordinates.

relativeAttitude  =  subEuler321[θ, MRP2Euler321[σ]]

To verify this answer, we can add the relative attitude to the MRP vector and verify that the original yaw, pitch and roll angles of the first aircraft are retrieved.

addEuler321[MRP2Euler321[σ], relativeAttitude]

Kinematic Differential Equations

The differential kinematic equation of the various attitude coordinates can be written in the form

\(TraditionalForm\`\(d(θ)\)\/dt = \(\([\)\(B(θ)\)\(]\)\) ω\)

where ω is the body angular velocity vector.  Note that the [B] matrix is defined here such that it always linearizes as the identity matrix for small angles.  Therefore, for the MRPs, the differential kinematic equation is

((d(σ))/dt) = (1/4)[
          B(σ)] ω

For example, to compute the [B] matrix for the Gibbs vector, the following command is invoked.

MatrixForm[BmatGibbs[{q1, q2, q3}]]

To compute the time derivative of a Gibbs vector q given the angular velocity vector ω we type

MatrixForm[dGibbs[{q1, q2, q3}, {ω1, ω2, ω3}]]

The following example illustrates how these differential kinematic equations can be used to solve for the attitude time history given a time varying body angular velocity vector.  Let ω be defined as

ω[t_] := {Sin[(t/3)], Cos[(t/6)], Sin[2 t]}

The attitude time history is solved first using the MRPs.  Whenever the magnitude of the MRP vector grows larger than 1, this implies that the body has rotated more than 180 degrees from the original attitude.

ansMRP=
NDSolve[
{s1'[t]==dMRP[{s1[t], s2[t], s3[t]}, ω[
                      t]][[1]],
s2'[t]==dMRP[{s1[t], s2[t], s3[t]}, ω[
                      t]][[2]],
s3'[t]==dMRP[{s1[t], s2[t], s3[t]}, ω[
                      t]][[3]],
s1[0] == 0,
s2[0] == 0,
s3[0] == 0},
{s1[t], s2[t], s3[t]},{t, 0, 10}]

Plot[(Sqrt[s1[t]^2 + s2[t]^2 + s3[t]^2]) /. ansMRP, {t, 0, 10}, PlotRange -> All];

The same attitude time history is now solved using the classical Rodrigues parameter (Gibbs vector).  Since these parameters are singular whenever a 180 degree rotation is performed, this integration becomes singular as the MRP vector magnitude becomes 1.

ansGibbs  = 
NDSolve[
{q1'[
              t] == dGibbs[{q1[t], q2[t], q3[t]}, ω[
                  t]][[1]], 
q2'[
              t] == dGibbs[{q1[t], q2[t], q3[t]}, ω[
                  t]][[2]], 
q3'[
              t] == dGibbs[{q1[t], q2[t], q3[t]}, ω[
                  t]][[3]], 
q1[0] == 
            0, 
q2[0] == 0, 
q3[0] == 
            0}, 
{q1[t], q2[t], q3[t]}, {t, 0, 
          10}]

Plot[(Sqrt[q1[t]^2 + q2[t]^2 + q3[t]^2]) /. ansGibbs, {t, 0, 10}, 
        PlotRange -> {0, 10}];