How to compute the orientation error between two 3D coordinate frames?

4.9k Views Asked by At

For my master thesis in robotics I have to compute the orientation error between two coordinate frames, called E and H. Their orientation is expressed through rotation matrices (3x3) with respect to a "world frame", W. Remark: writing $R_A^B$ I'm indicating the orientation of frame A with respect to frame B.

What I have understood is that I have to do the following operation:

\begin{equation}R_{error} = R_E^W * R_W^H\end{equation}

is it correct? If yes, that error in which frame is expressed? Thanks a lot, Ale

1

There are 1 best solutions below

0
On BEST ANSWER

Convert the two orientations to versors also known as unit quaternions and rotation quaternions, using one of the methods described in the Rotation matrix Wikipedia article.

Let's say the two versors describing the two orientations are $\mathbf{q}_1$ and $\mathbf{q}_2$, $$\begin{aligned} \mathbf{q}_1 &= ( w_1, x_1, y_1, z_1 ) \\ \mathbf{q}_2 &= ( w_2, x_2, y_2, z_2 ) \\ \end{aligned}$$ and difference in the two orientations is $\mathbf{q}_\Delta$, $$\mathbf{q}_2 = \mathbf{q}_\Delta \mathbf{q}_1 \quad \iff \quad \mathbf{q}_\Delta = \mathbf{q}_2 \mathbf{q}_1^{-1}$$ Note that we use quaternion operations here: Hamilton product for multiplying the two orientations. For unit quaternions, the reciprocal is the same as conjugate; and is just negating the vector components ($x$, $y$, and $z$).

(Note that negating all four components of the orientation does not change the orientation, it just switches between the "long way" and "short way" around the great circle on the unit sphere. If you interpolate between two quaternions, if their dot product $q_1 q_2 + x_1 x_2 + y_1 y_2 + z_1 z_2$ is negative, you interpolate the long way around; negating all four components of one of the quaternions lets you interpolate the short way around instead, without affecting the start and end orientations at all.)

This means that the error orientation, or the rotation needed to transform orientation $\mathbf{q}_1$ to $\mathbf{q}_2$ is $\mathbf{q}_\Delta$, $$\mathbf{q}_\Delta = \left( q_\Delta, x_\Delta, y_\Delta, z_\Delta \right)$$ where $$\left\lbrace \begin{aligned} q_\Delta &= q_1 q_2 + x_1 x_2 + y_1 y_2 + z_1 z_2 \\ x_\Delta &= q_1 x_2 - x_1 q_2 + y_1 z_2 - z_1 y_2 \\ y_\Delta &= q_1 y_2 - y_1 q_2 - x_1 z_2 + z_1 x_2 \\ z_\Delta &= q_1 z_2 - q_2 z_1 + x_1 y_2 - y_1 x_2 \\ \end{aligned}\right.$$ If you need the angle $\theta$ between the two orientations, use $$\theta = \operatorname{atan2}\left(\sqrt{x_\Delta^2 + y_\Delta^2 + z_\Delta^2} , \quad q_\Delta \right)$$

This is because rotation of $\theta$ around an unit axis vector $(x, y, z)$, $x^2 + y^2 + z^2 = 1$, is described by versor $$\mathbf{q} = \left( \cos\frac{\theta}{2}, \quad x \sin\frac{\theta}{2}, \quad y \sin\frac{\theta}{2}, \quad z \sin\frac{\theta}{2} \right)$$