Apologies if this has already been answered, but I haven't been able to get a clear answer from looking on Stack Exchange so-far.
I'm trying to solve a camera stabilization problem. I have a 2-axis actively-driven gimbal system (pitch and yaw only). I am doing all my internal mathematics for the stabilisation using quaternion representation, but to actually convert that into drive commands for the gimbal, I need to convert my internal quaternions to pitch and yaw commands for the gimbal at the end of the workflow.
I have an IMU mounted to the gimbal housing, i.e. the body frame of the vehicle carrying the gimbal. I am using quaternions to do the underlying stabilisation mathematics, as I want the final algorithm to be able to run on a relatively low computational power 32-bit microcontroller. My intended work-flow is as follows:
- Get the desired camera pointing direction from the user in world-frame (Azimuth and Elevation)
- Convert that target into a quaternion representing the rotation required to drive the gimbal from world-frame home to the target location.
- Get a quaternion representing the rotation between the world frame and the body frame of the vehicle from the IMU.
- Perform appropriate inversions and quaternion multiplications to end up with a quaternion representing the rotation from the body frame of the vehicle to the target. This step may involve an additional quaternion representing the static offset between the IMU and vehicle frame, as my IMU mountings are not perfectly aligned with the gimbal axes.
- Convert this quaternion back into Azimuth and Elevation commanded positions for my Gimbal, and a Roll correction to be sent to the host for the software de-rotation.
The main step I'm having trouble with at the moment is (5), I can't find any sort of decent reference for converting this final quaternion back into a general Euler angle solution. At the moment I'm not 100% sure whether what I want is a 1-2-3 or a 3-2-1 Euler solution, so I want to have the ability to implement either when it comes down to it, giving me some ability to tweak/debug.
If anyone also has any gotchas they can see with this plan of attack, feel free to let me know about them. I'm an embedded control expert, and although I have some understanding of how quaternions work and what they're good for, I wouldn't be surprised to find I'd missed some crucial step in this process. My gimbal system has a limited pitch range, so given that the reference frame I'm converting back into Euler is the gimbal control frame, I don't expect gimbal lock to be an issue. I can identify the 90-degree pitch-up as higher than my motion capability and immediately discard that as an unachievable command. (I think I'm correct in saying that yaw and roll become coupled when the gimbal begins to lock looking almost directly up or down, but pitch will remain valid???)
EDIT: I've discovered since posting this question that what I was really after are apparently Tait-Bryan angles not Euler angles. The information in the question is still valid however, I'm after conversions from quaternion representation to intrinsic Roll-Pitch-Yaw and Yaw-Pitch-Roll representations.
Best Answer
After a lot of searching, I have finally found a reference that has what I needed. Although it doesn't appear in a 'Quaternion to Euler' or 'Quaternion to Tait-Bryan' google search, when I gave up and started looking for 'Quaternion to Axis-Angle' with the intention of going through that representation as an intermediate step, I came across the following wonderful document:
Technical Concepts
Orientation, Rotation, Velocity and Acceleration, and the SRM
Version 2.0, 20 June 2008
Author: Paul Berner, PhD
Contributors: Ralph Toms, PhD, Kevin Trott, Farid Mamaghani, David Shen, Craig Rollins, Edward Powell, PhD
http://www.sedris.org/wg8home/Documents/WG80485.pdf
It covers a lot of the formalisms, but most importantly, shows derivations and solutions for 3-1-3 and 3-2-1 Euler angle representation. It also seems to cover inter-conversion between pretty much every other rotation representation I'm aware of, and so I would also recommend it as a good general reference.
Oh, and the actual solution for a 3-2-1 ($z-y-x$) Tait-Bryan rotation convention from that reference: $$ \phi = \operatorname{arctan2}\left(q_2 q_3 + q_0 q_1,\frac{1}{2}-(q_1^2 + q_2^2)\right) \\ \theta = \arcsin(-2(q_1 q_3 - q_0 q_2)) \\ \psi = \operatorname{arctan2}\left(q_1 q_2 + q_0 q_3,\frac{1}{2}-(q_2^2 + q_3^2)\right) $$
Note that the gimbal-lock situation occurs when $2(q_1 q_3 + q_0 q_2) = \pm1$ (which gives a $\theta$ of $\pm \frac{\pi}{2} $), so it can be clearly identified before you attempt to evaluate $\phi$ and $\psi$.
(Convention for arctan2 is $\operatorname{arctan2}(y, x)$, as hinted on page 3.)