1
$\begingroup$

I am trying to write a autopilot script for Kerbal Space Program, which requires me to do some conversions between Tait-Bryan angles and quaternions. Unfortunately, KSP uses a left-handed coordinate system and different identities for each axis than anywhere else I've seen and I am stymied.

There are two important reference frames for this problem: the space vessel's orbital reference frame ("vorf") and the vessel's intrinsic reference frame ("vrf"). Pictures are worth a thousand words, so here are representations of these coordinates: vessel orbital reference frame, vessel intrinsic reference frame

It is possible to obtain the current rotation in the orbital reference frame as a quaternion of the form $(x, y, z, w)$ where $w$ is the real part.

Here's what I need math to do:

  1. Convert a desired pointing (yaw, pitch, roll) in the vorf to a quaternion $d$ in the vorf.
  2. Determine the difference between the desired rotation quaternion, $d$, and the current rotation quaternion $c$. I can do this by multiplying $c^{-1}\cdot d = diff$
  3. Determine the (yaw, pitch, roll) values for the quaternion $diff$. It doesn't much matter what reference frame they're in (I think...), since they are effectively differences/errors.
  4. Pass these angular values to the controls of the vessel. I have a PID controller all set up for this.

I need assistance with (1) and (3). No matter how I try and formulate my conversions, I can't get a conversion (yaw, pitch, roll) $\rightarrow$ quaternion $\rightarrow$ (yaw, pitch, roll) to give the same output as I give input. (This is not just a case of identity mod $\pi/2$ or something, the values seem nonsensical.)

The best I've done so far is (yaw, 0, 0) $\rightarrow$ quaternion $\rightarrow$ (yaw, 0, 0) and similar for the other axes, but once I add a second or third non-zero value, everything falls apart.

Can someone point me to a reference that explains the conversion between angles and quaternions this way or explain the procedure to transform one of the more standard representations into the one I require?

$\endgroup$
1
$\begingroup$

I think I have solved my problem. The main reference that finally clicked with me was this page, but I will continue and lay out my work here.

Here is a badly-drawn version of the coordinate system I'm using here (it looks better in my notes, I swear...) bad plane drawing*

  • $\psi$ represents a rotation around the $z$-axis, yaw
  • $\theta$ represents a rotation around the $x$-axis, pitch
  • $\phi$ represents a rotation around the $y$-axis, roll

The names of the angles are the traditional ones ($\psi$ for yaw, etc.) but the axes they are paired with are not.

Tait-Bryan rotations here will be performed in the order yaw, pitch, roll in the body-fixed reference frame. To apply them in the body-fixed frame, they must be applied in the reverse order: roll, pitch, yaw.

Angles to Quaternion

To convert from these three angles to a quaternion, I will use the definition of quaternion for each axis and then multiply them to get an overall quaternion.

$\begin{align} Q_p &= \left(\cos\frac{\theta}{2}, \sin\frac{\theta}{2}\left(\begin{array}{ccc}1\\0\\0\end{array}\right)\right)\\ Q_r &= \left(\cos\frac{\phi}{2}, \sin\frac{\phi}{2}\left(\begin{array}{ccc}0\\1\\0\end{array}\right)\right)\\ Q_y &= \left(\cos\frac{\psi}{2}, \sin\frac{\psi}{2}\left(\begin{array}{ccc}0\\0\\1\end{array}\right)\right) \end{align}$

Thus the overall quaternion I seek is $Q_R = Q_y\times Q_p\times Q_r$. Since the action of quaternions is applied right-to-left, this series of multiplications corresponds to the desired roll, pitch, yaw order.

Substituting and using the associative property:

$Q_R = \left(\cos\frac{\psi}{2} + \sin\frac{\psi}{2}\hat{k}\right)\times \left(\left(\cos\frac{\theta}{2} + \sin\frac{\theta}{2}\hat{i}\right)\times\left(\cos\frac{\phi}{2} + \sin\frac{\phi}{2}\hat{j}\right)\right)$

where $\hat{i}, \hat{j}, \hat{k}$ represent the three basis elements of the quaternions corresponding to rotations of $\pi/2$ about the $x$, $y$, and $z$ axes, respectively.

Performing the multiplication:

$\begin{align} Q_R =& \left(\cos\frac{\psi}{2} + \sin\frac{\psi}{2}\hat{k}\right)\times\left(\cos\frac{\theta}{2}\cos\frac{\phi}{2} + \cos\frac{\theta}{2}\sin\frac{\phi}{2}\hat{j} + \sin\frac{\theta}{2}\cos\frac{\phi}{2}\hat{i} + \sin\frac{\theta}{2}\sin\frac{\phi}{2}\hat{i}\hat{j}\right) \\ =& \cos\frac{\psi}{2}\cos\frac{\theta}{2}\cos\frac{\phi}{2} + \cos\frac{\psi}{2}\cos\frac{\theta}{2}\sin\frac{\phi}{2}\hat{j} + \cos\frac{\psi}{2}\sin\frac{\theta}{2}\cos\frac{\phi}{2}\hat{i} + \cos\frac{\psi}{2}\sin\frac{\theta}{2}\sin\frac{\phi}{2}\hat{i}\hat{j} +\\ & \sin\frac{\psi}{2}\cos\frac{\theta}{2}\cos\frac{\phi}{2}\hat{k} + \sin\frac{\psi}{2}\cos\frac{\theta}{2}\sin\frac{\phi}{2}\hat{k}\hat{j} + \sin\frac{\psi}{2}\sin\frac{\theta}{2}\cos\frac{\phi}{2}\hat{k}\hat{i} + \sin\frac{\psi}{2}\sin\frac{\theta}{2}\sin\frac{\phi}{2}\hat{k}\hat{i}\hat{j} \end{align}$

Using the standard results for multiplication of quaternion elements,

$\begin{array}{ccccc} \times & 1 & \hat{i} & \hat{j} & \hat{k} \\ 1 & 1 & \hat{i} & \hat{j} & \hat{k} \\ \hat{i} & \hat{i} & -1 & \hat{k} & -\hat{j} \\ \hat{j} & \hat{j} & -\hat{k} & -1 & \hat{i} \\ \hat{k} & \hat{k} & \hat{j} & -\hat{i} & -1 \end{array}$

these terms can be collected and placed in a column vector

$Q_R = \left(\begin{array}{c}w\\x\\y\\z\end{array}\right) =\left(\begin{array}{c} \cos\frac{\psi}{2}\cos\frac{\theta}{2}\cos\frac{\phi}{2} - \sin\frac{\psi}{2}\sin\frac{\theta}{2}\sin\frac{\phi}{2} \\ \cos\frac{\psi}{2}\sin\frac{\theta}{2}\cos\frac{\phi}{2} - \sin\frac{\psi}{2}\cos\frac{\theta}{2}\sin\frac{\phi}{2} \\ \cos\frac{\psi}{2}\cos\frac{\theta}{2}\sin\frac{\phi}{2} - \sin\frac{\psi}{2}\sin\frac{\theta}{2}\cos\frac{\phi}{2} \\ \cos\frac{\psi}{2}\sin\frac{\theta}{2}\sin\frac{\phi}{2} - \sin\frac{\psi}{2}\cos\frac{\theta}{2}\cos\frac{\phi}{2} \end{array}\right)$

and this is my first desired result!

Quaternion to Angles

The next step is to get back from a quaternion $(w,x,y,z)^T$ to the angles $\psi, \theta, \phi$. This eventually condenses down to three simple formulae, but the derivation takes a few steps.

First, realize that the rotation described by the quaternion can also be described by the product of three rotation matrices

$M_R = M_y\times M_p\times M_r = \left[\begin{array}{ccc} \cos\psi & -\sin\psi & 0 \\ \sin\psi & \cos\psi & 0 \\ 0 & 0 & 1 \end{array}\right]\times \left[\begin{array}{ccc} 1 & 0 & 0 \\ 0 & \cos\theta & -\sin\theta \\ 0 & \sin\theta & \cos\theta \end{array}\right]\times \left[\begin{array}{ccc} \cos\phi & 0 & \sin\phi \\ 0 & 1 & 0 \\ -\sin\phi & 0 & \cos\phi \end{array}\right]$

$M_R = \left[\begin{array}{ccc} \cos\psi\cos\phi-\sin\psi\sin\theta\sin\phi & -\sin\psi\cos\theta & \cos\psi\sin\phi+\sin\psi\sin\theta\cos\phi \\ \cos\psi\sin\theta\sin\phi+\sin\psi\cos\phi & \cos\psi\cos\theta & \sin\psi\sin\phi-\cos\psi\sin\theta\cos\phi \\ -\cos\theta\sin\phi & \sin\theta & \cos\theta\cos\phi \end{array}\right]$

The elements of this matrix can be used to extract expressions for the angles:

$\frac{M_{R21}}{M_{R22}} = \frac{-\sin\psi\cos\theta}{\cos\psi\cos\theta} = -\tan\psi, M_{R32} = \sin\theta, \frac{M_{R31}}{M_{R33}} = \frac{-\cos\theta\sin\phi}{\cos\theta\cos\phi} = -\tan\phi$

A quaternion can also be represented as a rotation matrix (some elements omitted since they are unncessary for this derivation--full matrix here: H-T-T-P://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToMatrix/index.htm):

$M_Q = \left[\begin{array}{ccc} - & 2xy-2wz & - \\ - & 1-2x^2-2z^2 & - \\ 2xz-2wy & 2yz+2wx & 1-2x^2-2y^2 \end{array}\right]$

Since, $M_R=M_Q$, the above expressions involving the elements of $M_R$ can be written with the elements of $M_Q$:

$\begin{align} -\tan\psi &= \frac{2xy-2wz}{1-2x^2-2z^2} &\Rightarrow& \psi = \arctan\left(\frac{-2xy+2wz}{1-2x^2-2z^2}\right) \\ \sin\theta &= 2yz+2wx &\Rightarrow& \theta = \arcsin\left(2yz+2wx\right)\\ -\tan\phi &= \frac{2xz-2wy}{1-2x^2-2y^2} &\Rightarrow& \phi = \arctan\left(\frac{-2xz+2wy}{1-2x^2-2y^2}\right) \end{align}$

Implementation Notes

There are two difficulties with implementing the conversion from quaternion to Tait-Bryan angles in a program. The first is what happens when the denominator in an $\arctan$ expression is 0. Most programming languages have solved this problem with a function arctan2(y,x) which computes $\arctan(y/x)$ while avoiding the possible divide-by-zero error. Check the documentation of your particular language, because some use the convention $\arctan(y/x)$ and some $\arctan(x/y)$.

The second difficulty involves the singularities of Tait-Bryan/Euler angles (gimbal lock), which are well-documented elsewhere. In this formulation, the singularities occur when $\theta = \pm \pi/2$. This can be solved with the technique described here under "Derivation of Equations": H-T-T-P://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToEuler/index.htm

*Anyone with the proper reputation, feel free to place that image inline and fix my extra links...

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.