The goals of this lab are to put theory on forward kinematics into practice and implement odometry on the E-Puck platform, and use the resulting localization mechanism to drive the robot to a desired location.

Prerequisites:

• Forward kinematics of a differential wheel platform (from lecture 3 and homework 1)
• A working obstacle-avoidance controller for the E-Puck (download lab3controller, thanks to Brian Carlsen, if you don’t have one that works)
• A world with a single E-Puck

Part I: Odometry

Odometry is the process of inferring your position by the integration of speed. With no obstacles we can calculate the distance and angle to the goal directly and write our controller accordingly. However, when we introduce obstacles with unknown locations, we cannot simply rely on our initial calculations. We need to instead adjust our path dynamically using odometry. In order to adjust the robot’s path we need the position of the robot in the inertial frame at time t. This can be accomplished using the forward kinematics from lecture 3. Lecture 3 provided us with the tools to find the robot’s speed within the inertial frame given the speed in the robot’s own frame. We can then integrate the speed of the robot in the inertial frame from time 0 to time t to find the position of the robot in the inertial frame at time t.

Review of lecture 3:

The speed of the robot in the inertial frame, $\dot{\xi_I}=[\dot{x_I}, \dot{y_I}, \dot{\theta_I}]^T$, is a transformation of the the speed of the robot in the robot’s frame, $\dot{\xi_R}=[\dot{x_R}, \dot{y_R}, \dot{\theta_R}]^T$. This transformation is accomplished using the rotational matrix,

$T(\theta)=\left(\begin{array}{ccc} cos(\theta) & -sin(\theta) & 0 \\ sin(\theta) & cos(\theta) & 0 \\ 0 & 0 & 1\end{array}\right)$,

derived from the angular relationship between the inertial and robot frames. Thus the speed of the robot in the inertial frame is given by, $\dot{\xi_I}=T(\theta)\dot{\xi_R}$. Further we found in lecture 3 that the value for $\dot{\xi_R}$ is a function of the individual wheel speeds, wheel radius, and axle length,

$\dot{\xi_R}=\left(\begin{array}{c} \dot{x_R}\\\dot{y_R}\\\dot{\theta_R}\end{array}\right)=\left(\begin{array}{c}\frac{r\dot{\phi_r}}{2}+\frac{r\dot{\phi_l}}{2}\\0\\\frac{r\dot{\phi_r}}{d}-\frac{r\dot{\phi_l}}{d}\end{array}\right)$

This gives us the final equation of lecture 3 relating the speed of the robot in the inertial frame to the wheel speeds of the E-Puck,

$\left(\begin{array}{c} \dot{x_I}\\\dot{y_I}\\\dot{\theta_I}\end{array}\right)=\left(\begin{array}{ccc} cos(\theta) & -sin(\theta) & 0 \\ sin(\theta) & cos(\theta) & 0 \\ 0 & 0 & 1\end{array}\right)\left(\begin{array}{c}\frac{r\dot{\phi_r}}{2}+\frac{r\dot{\phi_l}}{2}\\0\\\frac{r\dot{\phi_r}}{d}-\frac{r\dot{\phi_l} }{d}\end{array}\right)$

The setup and mathematics we are using in lab 4 are similar to those described in lecture 3. However, the world we are using for lab 4, lab4.wbt, is based on the Webot default settings and is oriented on the zx-plane rather than the xy-plane described in lecture 3. Likewise we will be using a robot that is also oriented on the zx-plane with the slight complication that the front of the robot is oriented toward the negative z-axis.

Exercises:

1. Derive an equation like the final one above that is based on the zx-orientation of our world and simplify to find $[\dot{Z_I}, \dot{X_I}, \dot{\theta_I}]^T$ in terms of the $r$, $\dot{\phi_r}$, and $\dot{\phi_l}$ of the E-Puck.
2. Set up Webots:

i. Create an empty directory for lab 4 by typing mkdir lab4 in the terminal.

ii. Open Webots, choose Your Project, and use the wizard to create a New Project Directory in the lab4 directory you just created.

iii. Download and extract the world, lab4.wbt and the protos directory from, lab4files.tar. The protos directory that you download needs to be merged with the protos directory that was created in part (b) and the lab4.wbt needs to be placed in the worlds folder of the same project overwriting the world that is already there.

iv. In the World View of Webots, open the world lab4.wbt. Use the wizard to create a New Robot Controller, and name it how you wish. After you create the new controller, go into the Scene Tree under the E-Puck and select this controller as the controller to use. Then resave the world.

1. The world you were given consists of an E-Puck and two goals. The first goal is green and the second is purple. Your task is to program your E-Puck to find and stop on these squares. Although you can do a simple point and shoot for the green square, we want you to create a controller that will plan the path dynamically. Your controller should update and print the robot’s position and rotation every time step. After you have achieved the green goal, incorporate the obstacle-avoidance you programmed last week to reach the purple goal.
2. In order to program position control, you will find that representing the error in polar coordinates facilitates the problem. Then, you can represent the robot’s position in terms of a distance and an angle to the goal. Let $(z,x)$ be the position of the robot and $(z_g,x_g)$ be the coordinates of the desired location. You can then calculate the distance of the robot to the goal by $d=\sqrt{(z-z_g)^2+(x-x_g)^2}$ and the angle as $atan2(\frac{x_g-x}{z_g-z})$.
3. Once your controller works sufficiently well on both goals, create an U-shaped obstacle, by moving the pieces of the yellow wall, that catches the robot on its way to the goal. You will observe that the robot will get trapped again and again in the U-obstacle. In order to escape this, you need path-planning.
4. Some useful information: the radius of the E-Puck is 0.0205m, the axle is 0.052m, the wheel speed is measured in rad/sec, but the speed unit is 0.00628 so multiply all speeds by this number. Remember also that the time step is not a second but rather 64ms. You can check your calculated position or angle against the actual position and angle by clicking on the E-Puck and then click on the translation or rotation fields in the Scene Tree. You can find the position of the E-Puck by integrating the speed equations you found in part (1). Although this sounds like a nasty bit of calculus, you can achieve the integration nicely in the while loop with a statement like z-coord += zDistanceTraveledInOneTimestep.
5. A note of encouragement: your odometry will only work well around the E-Puck’s initial position as the encoders are noisy and the E-Puck’s wheels slip. (This is faithfully simulated in Webots) so go easy on yourself if your E-Puck does not stop exactly in the middle of the green/purple square.

### 3 Responses to Introduction to Robotics, Lab #4: Odometry

1. Nate Lapinski says:

Under section 3, part ii, when calculating the distance of the robot from the goal, I think the subscripts may be backwards. Wouldn’t it be $\sqrt( (z_g - z)^2 + (x_g - x)^2 )$?
Not that it really matters, as you are squaring the value regardless…

2. […] environment. This is difficult, however, as the error between every snapshots – similar to odometry – accumulates.   The ICP algorithm also works in 3D where it allows to infer the change in […]