Inferring joint orientation on multi-joint arm

Question

Not a robotics question in the strictest sense, I guess, but related closely enough, I hope:

I have an arm-like articulated two(-plus-one)-joint appliance that I want to use as a 3D input device. It uses an angle measurement device and two IMUs, which are placed as depicted schematically below:

The blue boxes depict the positioning and orientation of the IMUs (the arrows point to the IMU's relative "forward" direction). The blue dot/highlighted angle represent the angle being measured. The base can rotate around its centre, counting as the third joint, technically, but shouldn't be of too much relevance here. The arm joint rooted at the base has two rotational degrees of freedom, indicated by the red-ish arrows (it can't rotate around the base's up-direction). The unfilled rectangle represents the data I'd like to infer from the other measurements.

edit: I cannot post more accurate schematics here, but if you want to visualise this apparatus a bit better, think of a Geomagic Phantom, except that the lower arm joint is not rotational, but is built more like a classic analog joystick.

Note: The positioning of the sensors, especially of the IMUs at the base and on the second arm joint are fixed, so please do not suggest changing these (I can't).

I'm now wondering how to compute the orientation of the middle link from the data I have: both IMUs return quaternions $q_0$ and $q_2$, respectively (relative to the magnetic north, measured from their relative forward-direction). My representation for the relative rotation between the two arm links is a quaternion ($q_a$) as well (even though it could just as well be directly represented as an angle, but since I'm performing quaternion math anyways, I might as well have it in this form, too). I'm pretty sure that there must be some way to basically compute what an IMU on the middle link (let's call it $q_1$) would measure from the data I have, but I'm not quite sure about my maths here...

My intuition was to compute $q_1 = q_0^{-1} * q_2 * q_a^{-1}$, following from the assumed identity $q_2 = q_0 * q_1 * q_a$, but that doesn't seem to hold. As I feared that the rotation of $q_2$ relative to the joint it resides on influences the computation result, I also computed $q_1 = q_0^{-1} * q_2 * q_{z\pi} * q_a^{-1}$, where $q_{z\pi}$ represents a 90° rotation around the up(=z)-axis.

However, my measurements still seem off when I visualise the movements (the $q_1$ movement seems exaggerated compared to the actually induced movement). What else might I miss here? Is my math faulty, or is it possibly only an implementation mistake I made?

EDIT2: I found that one major flaw of my maths was the lack of calibration. Adding a calibration pose, I was able to compute the relative orientation between the two IMUs in both the visual model and the actual device and go from there. However, to compute the lower link's orientation, I still rely on an equation like $q_1 = q^*_2 * q_a^{-1}$, with $q^*_2$ being the quaternion that rotates from the relative orientation between $q_0$ and $q_2$ in the calibration pose towards their current relative orientation. I'm still not quite sure if that equation is fully appropriate, but it appears to work okay so far.

Answer 1

What else might I miss here?

1. As pointed out by @Ben, a clear schematics of your robot to make it easily understandable from others.

2. Maybe it's me missing something but IMU are typically accelerometers and gyroscopes. Meaning that the raw outputs are linear accelerations and rotational velocities. Doing some projection of the gravity vector to retrieve orientation is prone to singularities.

3. Your problem seems to me relatively close to some pan-tilt control with 2 IMUs.

4. your intended use is very likely to prove problematic as the nozzel (or end of your arm) will not have the same orientation with respect to the base plan as the altitude evolves

5. Units quaternions represent a unit vector in the base they are expressed in, associated with a rotation around that vector. So you can not use the same 'simple' math than for rotation matrix, meaning that in order to combine rotation you can not simply multiply the quaternion but need a bit more cumbersome operation see.

6. I would suggest you to use transformation matrices composed of rotation matrices and translation vectors. This will prove easy to derivate from your clean model, and useful in the long run when you are going to go toward inverse kinematics of you robots. Note that the change from quaternion to rotation matrix is well documented.

edit : re work the explanation of the quaternion math

Answer 2

I might be missing something here, but if you are looking for the position of the tip of the arm, it should just be:

$x = l_1\cos(\beta) + (l_1+l_2)\cos(\alpha + \beta)$

$y = l_1\sin(\beta) + (l_1+l_2)\sin(\alpha + \beta)$

where $l_1$ is the length of the first link, $l_2$ of the second, $\alpha$ the relative angle of rotation that you indicate in your diagram, and $\beta$ the angle of rotation of the base arm (not indicated with any symbol in your diagram).