# Mobile Robot path reconstruction by using IMU acceleration and Yaw angle

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

• I forgot to say that the IMU sensor if fixed on the robot frame. – Marcus Barnet Sep 17 '15 at 10:33
• What exactly are "robot encoders"? I searched "XBOW-300CC" and all I saw were go-karts and motorcycles. – Chuck Sep 17 '15 at 13:01
• Xbow IMU 300-CC is the IMU sensors: memsic.cn/… this is the user manual – Marcus Barnet Sep 17 '15 at 13:05
• First things first. Is this a 3D system? If it isn't, then why do I see three 3D-rotation matrices? If it is, then why are you integrating rotational velocity? – JJM Driessen Sep 18 '15 at 20:24
• Yes, it is a 3D system. Where do you see that I'm integrating the rotational velocity? :( – Marcus Barnet Sep 19 '15 at 15:13

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.

• Thank you, chuck, for your support. I'm using Wheel encoders and they are mounted on the motor shaft after the gear box. If I can well understand what you explained, I can directly use the double integration in place of the wheel encoder information? In this case, should I use only the X-axis acceleration or should I use also the Y-axis? The X-axis is in the forward/reverse direction and the Y-axis in on the left-right direction. – Marcus Barnet Sep 17 '15 at 13:02
• Is it correct to use the same rotation matrix for the double integration even if the acceleration values come from the IMU frame? – Marcus Barnet Sep 17 '15 at 13:03
• Do you have suggestions on how add the X and Y-axis information together to get the correct forward distance, please? Or can you suggest me any documents to read to learn how to do this? – Marcus Barnet Sep 17 '15 at 13:08
• @MarcusBarnet I added content. Let me know if this clears everything up for you. – Chuck Sep 17 '15 at 14:22
• your comment is very pretty straightforward and I think I really got how to set up the equations. I'm going to read again your comment in order to better understand them and then I'll try to put them in Matlab. Just an observation: I'm trying to investigate about slipping effects on skid-steering robot during turning operations. In this case, do you think the Y-axis could give me useful information or can I just use the X-axis information? Thank you! – Marcus Barnet Sep 17 '15 at 16:34