Mobile Robot path reconstruction by using IMU acceleration and Yaw angle

Question

I hope you can help me with my project.

I'm using a skid-steering wheeled mobile robot for autonomous navigation and I'd like to find a way to be able to perform path reconstruction in Matlab.

By using only the robot encoders (installed on the robot) and the yaw rate information (which come from a very accurate IMU sensor mounted on the robot frame), I can successfully do the path reconstruction. (I'm using XBOW-300CC sensor)

The problem is that I would like to try to reconstruct the path by using only the IMU yaw rate and the IMU acceleration values for X and Y axis.

I'm able to obtain velocity and distance by integrating two times the IMU acceleration values but my problem is that I don't know how to use this data.

Do I have to use a rotation matrix to pass from the IMU frame to the robot frame coordinates? I'm asking this because I use a rotation matrix for the encoder values which come from the robot encoder.

At the moment, I use these equations for robot encoders and IMU yaw rate:

tetha(i)=(yaw(i)+yaw(i-1))/2*(encoder(i)-encoder(i-1))+tetha(i-1); %trapezoidal integral

Rx=[1 0 0;0 cos(-roll_angle(i)) -sin(-roll_angle(i)); 0 sin(-roll_angle(i)) cos(-roll_angle(i))];
Ry=[cos(-pitch_angle(i)) 0 sin(-pitch_angle(i)); 0 1 0; -sin(-pitch_angle(i)) 0 cos(-pitch_angle(i))];
Rz=[cos(-tetha(i)) -sin(-tetha(i)) 0; sin(-tetha(i)) cos(-tetha(i)) 0; 0 0 1];
R2=Rz*Ry*Rx;

disp=R2 *[encoder_displacement(i) 0 0]';
X_r(i)=disp(1);
Y_r(i)=disp(2);
Z_r(i)=disp(3);

X(i)=x0+sum(X_r(1:i));
Y(i)=y0+sum(Y_r(1:i));
Z(i)=z0+sum(Z_r(1:i));

Do I still have to use R2 matrix?

Thank you a lot

Answer 1

If you aligned the IMU "well", then this should be a relatively trivial problem. Consider your statement that you can reconstruct the path using only "robot encoders" (wheel encoders?) and the yaw rate. Now, if you have the IMU configured such that the X-axis is the forward/reverse direction, then double-integrating the x-axis accelerometer should duplicate the wheel encoder information. The only information the Y-axis accelerometer should tell you is if the robot is sliding sideways.

If you have the IMU aligned "poorly" then you could have some projection of the x- and y-axis along the forward/reverse axis, at which point you would need to take the correct projection from each and add them together to get the forward distance.

Regarding rotation matrices, whatever you're doing currently to apply it to the (wheel encoder?) robot encoder should work the same when you convert the acceleration data to distance.

:EDIT:

Assuming your yaw angle is a z-axis rotation, your rotation matrix should look like:

$$ R = \left[ \begin{array}{ccc} \cos{\theta} & -\sin{\theta} & 0 \\ \sin{\theta} & \cos{\theta} & 0 \\ 0 & 0 & 1 \end{array} \right] \\ $$

You should have some starting heading, $\theta_0$. You also need a starting velocity $v_0$ and a starting position, $p_0$. Now, you get samples at some update rate, where the time between samples, the sample time, is $dT$. So, from physics, you can calculate your new distance and velocity:

$$ p_N = p_{N-1} + v_{N-1} dT + \frac{1}{2} a_N dT^2 \\ v_N = v_{N-1} + a_N dT \\ $$

where $N$ indicates the current sample and $N-1$ indicates the previous sample. In this instance your initial conditions, $p_0$ and $v_0$ are used and you get your first update, $p_1$ and $v_1$ from your first accelerometer reading. I've used $p$ for position instead of $x$ to try to keep the concepts separate - the accelerometer will give you some position, but it doesn't get to be a coordinate until you've reconciled it back to your world coordinates. This is done with the rotation matrix:

$$ \left[ \begin{array}{ccc} x_N \\ y_N \\ z_N \end{array} \right] = R_{N-1} * \left[ \begin{array}{ccc} p_N \\ 0 \\ 0 \end{array} \right] \\ $$

Here I've shown the $N-1$ rotation matrix, the rotation matrix based on the previous yaw reading (or starting heading, if you're considering the first sample), but your choice of which angle to use (previous sample, current sample, an average, etc.) shouldn't actually make a big difference for the same reason that trapezoidal integration and rectangular integration both converge on the same result. As long as the sample time is sufficiently short relative to the motion of your vehicle, any errors accumulated going one direction should generally be canceled out going the other.

I would say if you wanted the most reasonable yaw estimate, use the average of the previous and current headings: $$ R = (R_{N} + R_{N-1})/2 \\ $$

So, to summarize, I would try the following:

$$ \theta_N = \mbox{read(yaw)} \\ a_N = \mbox{read(x-axis IMU)} \\ $$

$$ \theta_{\mbox{mean}} = (\theta_N + \theta_{N-1})/2 \\ R = \left[ \begin{array}{ccc} \cos{\theta_{\mbox{mean}}} & -\sin{\theta_{\mbox{mean}}} & 0 \\ \sin{\theta_{\mbox{mean}}} & \cos{\theta_{\mbox{mean}}} & 0 \\ 0 & 0 & 1 \end{array} \right] \\ $$

$$ p_N = p_{N-1} + v_{N-1} dT + \frac{1}{2} a_N dT^2 \\ v_N = v_{N-1} + a_N dT \\ $$

$$ \left[ \begin{array}{ccc} x_N \\ y_N \\ z_N \end{array} \right] = R * \left[ \begin{array}{ccc} p_N \\ 0 \\ 0 \end{array} \right] \\ $$

Now, as a final note, again, I would leave the y-axis accelerometer out of everything because you'll find one of two scenarios:

1. The IMU is located on the center of rotation of the vehicle, so you don't actually ever get a y-axis acceleration during normal operation, or
2. The IMU is not located on the center of the rotation of the vehicle, so you get a y-axis acceleration any time you turn. In this instance, any y-axis acceleration only indicates rotation, which you already know from the z-axis rotation.

In both cases, assuming the vehicle isn't sliding sideways, the y-axis accelerometer isn't providing you any forward/reverse information. It's only providing you either nothing (case 1) or a more obfuscated form of rotation (case 2) that you already get from your yaw angle. So, just leave it off.