Search code examples
mathorientationsensorsquaternions

Align IMU orientations and then get relative rotations

I am using two IMUs of the same type (BHI160, i.e. orientation is relative to the north and on alignment with north, the IMU's local y-axis points into the north direction) on two objects, let's say pens, with the added difficulty that if I place the two objects in parallel, both IMUs' z-axes point upwards, but one IMU is rotated 180° around the z-axis relative to the other.

Now, if I understand the math here correctly, the quaternion data I receive from an IMU is the half-angle-rotation relative to the north direction, so that q * north_dir * q_inv = IMU_y_axis (with north_dir and IMU_y_axis being 3D vectors in global space, or pure quaternions for the sake of this computation).

Due to the rotation of the IMUs, I would assume that when both pens are pointing in the same direction, I should be able to compute the second pen's orientation as q_2 = q_rot_z * q_1, where q_rot_z equals a 90° rotation around the z-axis -- following the intuition that if I point both pens towards the north, I would obtain the global direction of pen 2's y-axis (i.e. pen 1's y-axis rotated around the z-axis by 180°) by computing q_rot_z * north_dir * q_rot_z_inv

Is it thus correct that if I want to know the relative rotation of the pen tips (say, the rotation I need to go from the first pen's tip to the tip of the second one), I need to compute q_r = q_2 * q_rot_z_inv * q_1_inv in order to get from tip 1 to tip 2 by computing q_r * q_1? Or does the "prior" rotation around the z-axis not matter in this case and I only need to compute q_r = q_2 * q_1_inv as usual?

Edit: this is basically an extension of this question, but I would like to know if the same answer also applies in my case or whether the known relative IMU rotation would in my case need to be included as well

Solution

  • Let's go through this step by step. You have a global coordinate system G, which is aligned to the north direction. It does not really matter how it is aligned or if it is aligned at all.

    Then we have to IMUs with their respective coordinate systems I1 and I2. The coordinate systems are given as the rotation from the global system to the local systems. In the following, we will use the notation R[G->I1] for that. This represents a rotation from G to I1. If you transform any vector in G with this rotation, you will get the same vector in I1 expressed in the coordinate system G. Let's denote the transformation of a vector v with transform T by T ° v. The following figure illustrates this:

    Transform

    In this figure, I have added a translation to the transform (which quaternions can of course not represent). This is just meant to make the point clearer. So, we have a vector v. The same vector can lie in either coordinate system G or I. And the transformed vector R[G->I] ° v represents the v of I in the coordinate system of G. Please make sure that this is actually the rotation that you get from the IMUs. It is also possible that you get the inverse transform (this would be the system transform view, whereas we use the model transform view). This changes little in the following derivations. Therefore, I will stick to this first assumption. If you need the inverse, just adjust the formulas accordingly.

    As you already know, the operation R ° v can be done by turning v into a pure quaternion, calculating R * v * conjugate(R), and turning it into a vector again (or work with pure quaternions throughout the process).

    Now the pens come into play. The pen has an intrinsic coordinate system, which you can define arbitrarily. From your descriptions, it seems as if you want to define it such that the pen's local y-axis points towards the tip. So, we have an additional coordinate system per pen with the according rotation R[I1->P1] and R[I2->P2]. We can concatenate the rotations to find the global orientations (* is quaternion multiplication):

    R[G->P1] = R[G->I1] * R[I1->P1]
R[G->P2] = R[G->I2] * R[I2->P2]

    In the way that you defined the pen's local coordinate system, we know that R[I1->P1] is the identity (the local coordinate system is aligned with the IMU) and that R[I2->P2] is a rotation of 180° about the z-axis. So, this simplifies to:

    R[G->P1] = R[G->I1]
R[G->P2] = R[G->I2] * RotateZ(180°)

    Note that the z-rotation is performed in the local coordinate system of the IMU (it is multiplied at the right side). I don't know why you think that it should be 90°. It is really a rotation of 180°.

    If you want to find the relative rotation between the tips, you first need to define in which coordinate system the rotation should be expressed. Let's say we want to express the rotation in the coordinate system of P1. Then, what you want to find is a rotation R[P1->P2], such that

    R[G->P1] * R[P1->P2] = R[G->P2]

    This solves to

    R[P1->P2] = conjugate(R[G->P1]) * R[G->P2]

    If you plug the above definitions in, you would get:

    R[P1->P2] = conjugate(R[G->I1]) * R[G->I2] * RotateZ(180°)

    And that's it.

    It is pretty likely that you want something slightly different. That's why I explained it in such detail, so you will be able to modify the calculations accordingly.