3D Coordinate Rotation Using Roll-Pitch-Yaw

Question

Given an orthogonal unit vector triad (i,j,k) described in the 3-dimensional coordinate space (X,Y,Z), as shown below.

I need to rotate the XYZ coordinate system such that X is collinear with i, Y is collinear with j, and Z is collinear with k. I am constrained to performing 3 rotations: First about the X axis, then about the ORIGINAL Y axis, and finally about the ORIGINAL Z axis, as shown below.

How can I determine the rotation angles a, b, and c? I realize that I can use pitch-roll-yaw rotation matrices to rotate one vector, but there is not a unique solution for the rotation of one vector. How can I combine the equations to find the unique solution that will rotate X, Y, and Z to be collinear with i,j,k?

Answer 1

Let's say your three orthogonal unit vectors are $$\hat{i} = \left [ \begin{matrix} i_x \\ i_y \\ i_z \end{matrix} \right ], \quad \hat{j} = \left [ \begin{matrix} j_x \\ j_y \\ j_z \end{matrix} \right ], \quad \hat{k} = \left [ \begin{matrix} k_x \\ k_y \\ k_z \end{matrix} \right ]$$ The rotation matrix $\mathbf{R}$ that rotates $x$ axis to $\hat{i}$, $y$ axis to $\hat{j}$, and $z$ axis to $\hat{k}$ is $$\mathbf{R} = \left [ \begin{matrix} i_x & j_x & k_x \\ i_y & j_y & k_y \\ i_z & j_z & k_z \end{matrix} \right ]$$ Because $\mathbf{R}$ is orthonormal, its inverse is its transpose; and the matrix $\mathbf{R}^{-1}$ that rotates $\hat{i}$ to $x$ axis, $\hat{j}$ to $y$ axis, and $\hat{k}$ to $z$ axis, is $$\mathbf{R}^{-1} = \mathbf{R}^T = \left [ \begin{matrix} i_x & i_y & i_z \\ j_x & j_y & j_z \\ k_x & k_y & k_z \end{matrix} \right ]$$

---

Euler and Tait-Bryan rotations are combinations of the three axis rotations, $$\begin{aligned} \mathbf{R}_x &= \left [ \begin{matrix} 1 & 0 & 0 \\ 0 & \cos\phi & -\sin\phi \\ 0 & \sin\phi & \cos\phi \end{matrix} \right ] \\ \mathbf{R}_y &= \left [ \begin{matrix} \cos\theta & 0 & \sin\theta \\ 0 & 1 & 0 \\ -\sin\theta & 0 & \cos\theta \end{matrix} \right ] \\ \mathbf{R}_z &= \left [ \begin{matrix} \cos\psi & -\sin\psi & 0 \\ \sin\psi & \cos\psi & 0 \\ 0 & 0 & 1 \end{matrix} \right ] \end{aligned}$$ where $\phi$, $\theta$, and $\psi$ specify the intrinsic rotations around the $x$, $y$, and $z$ axis, respectively. The combined rotation matrix is a product of three matrices (two or three of the above; first one can be repeated) multiplied together, the first intrinsic rotation rightmost, last leftmost.

The hard part, in my humble opinion, is to determine exactly which the correct order of rotations is. I do believe that in OP's case it is $\mathbf{R}_z \mathbf{R}_y \mathbf{R}_x$, but I am not sure; since this is apparently for an articulated arm of some sort, I would definitely numerically verify this first.

Except that I would not, because Euler/Tait-Bryan angles are evil due to their inherent ambiguity; I would use unit quaternions (versors) to describe the orientation instead. They're easier, logical, and well defined.

---

The problem at hand, is that at "run time", we have the nine numerical components of $\mathbf{R}$, but need to find out the angles $\phi$, $\theta$, and $\psi$.

If the correct rotation formalism indeed is $\mathbf{R} = \mathbf{R}_z \mathbf{R}_y \mathbf{R}_x$ then $$\mathbf{R} = \left [ \begin{matrix} \cos\theta \cos\psi & \sin\phi \sin\theta \cos\psi - \cos\phi \sin\psi & \cos\phi \sin\theta \cos\psi + \sin\phi \sin\psi \\ \cos\theta \sin\psi & \sin\phi \sin\theta \sin\psi + \cos\phi \cos\psi & \cos\phi \sin\theta \sin\psi - \sin\phi \cos\psi \\ -\sin\theta & \sin\phi \cos\theta & \cos\phi \cos\theta \end{matrix} \right ]$$ and comparing to $\mathbf{R}$, we pick the five entries with the simpler terms (and all Euler and Tait-Bryan angles have five such entries; their position and exact terms vary), finding that $$\left\lbrace \begin{aligned} i_x &= \cos\theta \cos\psi \\ i_y &= \cos\theta \sin\psi \\ i_z &= -\sin\theta \\ j_z &= \sin\phi \cos\theta \\ k_z &= \cos\phi \cos\theta \end{aligned} \right .$$ If we choose $-90° \le \theta \le +90°$, then $\cos\theta \ge 0$, and we can solve the angles, getting: $$\left\lbrace \begin{aligned} \cos\phi &= \frac{ k_z }{\sqrt{1 - i_z^2}} \\ \sin\phi &= \frac{ j_z }{\sqrt{1 - i_z^2}} \\ \cos\theta &= \sqrt{1 - i_z^2} \\ \sin\theta &= -i_z \\ \cos\psi &= \frac{ i_x }{\sqrt{1 - i_z^2}} \\ \sin\psi &= \frac{ i_y }{\sqrt{1 - i_z^2}} \end{aligned} \right . $$ In other words, $$\left\lbrace \begin{aligned} \phi &= \arctan\left(\frac{j_z}{k_z}\right) \\ \theta &= \arctan\left(\frac{-i_z}{\sqrt{1-i_z^2}}\right) = \arcsin\left(-i_z\right) \\ \psi &= \arctan\left(\frac{i_y}{i_x}\right) \end{aligned} \right . $$ On a microcontroller without hardware floating-point operation support, one can use the numeric $\sin$ and $\cos$ values as an unit vector (noting that they might not be exactly unit length, due to rounding errors when calculating the original matrix $\mathbf{R}$), and apply e.g. CORDIC to obtain the angles at desired precision.

Answer 2

Here's an intuitive approach with minimal formulas (and no matrices). It helps that the rotation angles in your example are all relatively small, that is, we do not need to consider rotations by more than $\frac\pi2$ radians about any of the axes.

The first rotation (about the $x$ axis) does not move the $x$ axis. The second rotation (about the $y$ axis) moves the $x$ axis, but only within the $x,z$ plane.

The rotation about the $z$ axis is therefore solely responsible for moving the image of the $x$ axis from the $x,z$ plane until it is parallel with $\hat \imath.$ We can determine the angle of rotation about the $z$ axis by reversing that rotation to bring $\hat\imath$ back into the $x,z$ plane. The required angle has magnitude $$\lvert \Delta c\rvert = \left\lvert\arctan\frac{\hat\imath_y}{\hat\imath_x}\right\rvert.$$

Now that we have the image of $\hat\imath$ in the $x,z$ plane, we see that it forms a right triangle with hypotenuse $1$ and legs $\hat\imath_z$ and $\sqrt{\hat\imath_x^2 + \hat\imath_y^2}.$ On examining the angles of this triangle, it should be clear that the rotation about the $y$ axis that returns the image of $\hat\imath$ to the $x$ axis has magnitude $$\lvert \Delta b\rvert = \left\lvert\arcsin\hat\imath_z\right\rvert.$$

The rotation about the $x$ axis is more difficult to work out because it has to account for the effects of the other two rotations. The inverxe $z$ rotation does not change the heights of the images of the vectors above the $x,y$ plane, that is, the images of $\hat\jmath$ and $\hat k$ still have $z$-components $\hat\jmath_z$ and $\hat k_z,$ respectively. The inverse $y$ rotation usually does change the $z$ coordinates of the images of the vectors, but it affects the images of $\hat\jmath$ and $\hat k$ in the same proportions, that is, the inverse $y$ rotation produces image vectors whose $z$ coordinates are $t\hat\jmath_z$ and $t\hat k_z,$ respectively, for some real number $t.$ So after reversing the $z$ rotation and the $y$ rotation, we end up with images of $\hat\jmath$ and $\hat k$ in the $y,z$ plane that have $z$-coordinates $t\hat\jmath_z$ and $t\hat k_z$; and since these are two perpendicular unit vectors, the right triangles they each form on the $y$ axis are congruent, so the image of $\hat\jmath$ in particular has $z$ coordinate $t\hat\jmath_z$ and $y$ coordinate $t\hat k_z$. Examining the angles of this triangle, clearly the angle required to rotate the image of $\hat\jmath$ back to the $y$ axis has magnitude $$\lvert \Delta z\rvert = \left\lvert\arctan\frac{t\hat\jmath_z}{t\hat k_z}\right\rvert = \left\lvert\arctan\frac{\hat\jmath_z}{\hat k_z}\right\rvert.$$

Additional inspection should reveal how the signs of the angles depend on the signs of the coordinates of $\hat\imath,$ $\hat\jmath,$ and $\hat k,$ confirming the results that were obtained via matrix algebra in another answer.

Things get a little more complicated if the rotations around the axes can be large, because you might need angles that are not in the ranges of $\arcsin$ and $\arctan.$ You would then need additional rules to obtain the correct angles. But it seems likely this concern does not apply to the rotations you need to work with.