Transforming x,y,z acceleration into x and y tilt angles?

Question

I'm working with some sensors that gives me acceleration and gyroscope data (rotational velocity).

I've been able to transform the data into tilt angles:

mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

int xAng = map(ax, minAcel, maxAcel, -90, 90);
int yAng = map(ay, minAcel, maxAcel, -90, 90);
int zAng = map(az, minAcel, maxAcel, -90, 90);

//tilt angles
double x = RAD_TO_DEG * (atan2(-yAng, -zAng) + PI);
double y = RAD_TO_DEG * (atan2(-xAng, -zAng) + PI);

This is the code I use, utilizing the wire.h Arduino library. I don't exactly understand how this is happening though, and would like help understanding the geometry involved.

My intuition dictates that gyroscope rotational velocity data would help me get inclination, but I found a source that provided this code, and it works.

In this code: ax = acceleration for x, etc. The arduino map function just maps from one range to another. I don't really get the geometry! Can someone provide the actual equation, and help explain?

Answer 1

I will assume that x is supposed to be the amount of rotation around the $x$ axis. I will also assume that in the "starting" position, the direction of acceleration measured by the "z" sensor is straight up or down, and that the code int xAng = map(ax, minAcel, maxAcel, -90, 90) sets xAng to some constant $k$ times ax.

I will write this mathematically rather than as code, because as a programmer I find that the mathematical notation gives me more intuition about what the code is doing than I can easily get from the code itself. I will write $a_y$ instead of ay, $y_{\text{Ang}}$ instead of yAng, and so forth. I will write $\theta_x$ instead of x because $x$ looks too much like an $x$-coordinate, which is very different from a "tilt angle."

Note that DEG_TO_RAD is $\frac{180}{\pi}$ in mathematical notation. Also note that $\text{atan2}(y,z)$ will solve for an angle $\theta$ between $-\pi$ and $\pi$ and a positive number $r$ such that $y = r\sin\theta$ and $z = r\cos\theta,$ and return $\theta$. For example, $\text{atan2}(0,3) = 0$ and $\text{atan2}(0.4,0) = \frac\pi2.$

If my understanding of your situation is correct, when the array of sensors is in its rest position, the acceleration measured by $a_z$ is $9.8\ \text{m}/\text{s}^{-2},$ that is, it registers that the ground (or whatever the array is sitting on) is pushing it upward with a force of $mg$ (where $m$ is the array's mass and $g$ is the acceleration of gravity) to keep the array from falling. That is $a_z = g = 9.8$ (or whatever number $1$ g comes out to in the units your accelerometers use). At the same time $a_x = a_y = 0,$ since there is no other force on the array. Then $$y_{\text{Ang}} = k a_y = 0$$ and $$z_{\text{Ang}} = k a_z = k g.$$

Plug these into your formula for x and you get \begin{align} \theta_x &= \frac{180}{\pi} \left(\text{atan2}(-y_{\text{Ang}}, -z_{\text{Ang}}) + \pi\right) \\ &= \frac{180}{\pi} \left(\text{atan2}(0, -kg) + \pi\right) \\ &= \frac{180}{\pi} \left(\pi + \pi\right) \\ &= 360. \\ \end{align} But if $a_y$ is even very slightly positive, then \begin{align} \theta_x &= \frac{180}{\pi} \left(\text{atan2}(-y_{\text{Ang}}, -z_{\text{Ang}}) + \pi\right) \\ &= \frac{180}{\pi} \left(\text{atan2}(-ka_y, -kg) + \pi\right) \\ &\approx \frac{180}{\pi} \left(-\pi + \pi\right) \\ &= 0. \\ \end{align} Either answer seems appropriate for the initial position, since $360$ degrees "tilt" means it's exactly the same as how it started.

Now rotate $45$ degrees around the $x$ axis so that both the $y$ and $z$ axes are sloped upward at $45$ degree angles. Then $a_y = a_z = g \sin\frac\pi4 = \frac{\sqrt2}{2} g$ and \begin{align} \theta_x &= \frac{180}{\pi} \left(\text{atan2}(-y_{\text{Ang}}, -z_{\text{Ang}}) + \pi\right) \\ &= \frac{180}{\pi} \left(\text{atan2}\left(-k\frac{\sqrt2}{2} g, -k\frac{\sqrt2}{2} g\right) + \pi\right) \\ &\approx \frac{180}{\pi} \left(-\frac34\pi + \pi\right) \\ &= 45. \\ \end{align}

Now rotate further so the $y$ axis is straight up (total $90$ degrees rotation so far). Then Then $a_y = g,$ $a_z = 0,$ and \begin{align} \theta_x &= \frac{180}{\pi} \left(\text{atan2}(-y_{\text{Ang}}, -z_{\text{Ang}}) + \pi\right) \\ &= \frac{180}{\pi} \left(\text{atan2}\left(-kg, 0\right) + \pi\right) \\ &\approx \frac{180}{\pi} \left(-\frac12\pi + \pi\right) \\ &= 90. \\ \end{align}

The basic idea is you're using the accelerometers to tell you the components of the "up" direction in the directions of your three axes; then the atan2 function converts two components into an angle. You've arranged the order of inputs to atan2 so that when computing x you are computing how far the "up" direction has turned (relative to the sensor array) in the direction from the $z$ axis toward the $y$ axis; except you managed to get an answer $180$ degrees from the correct answer, so you had to add $\pi$ to correct it.

A simpler, equivalent calculation of x would be

    double x = RAD_TO_DEG * (atan2(yAng, zAng));

or even better

    double x = RAD_TO_DEG * (atan2(ay, az));

since multiplying both arguments of atan2 by the same constant has no effect on the return value.

I should note that this is "equivalent" in the sense that $350$ degrees is equivalent to $-10$ degrees; but to the extent that the results are numerically different, I think the simple results are better: if tilting the array a few degrees one way causes x to be $10,$ then it makes sense to me that tilting it the same amount in the opposite direction would cause x to be $-10,$ not $350.$

Of course if the array were actually subjected to a significant acceleration, not just sitting on the ground or on a table, then your calculations would give bad results. For example, if you put the array on a rocket sled with the $y$ axis pointing forward and fired up the rockets, your ay reading would max out and the formula would tell you the array had rotated through a significant angle (though less than $90$ degrees) even though it had not turned at all.