Now that we can control the speed of Colin’s wheels, we can tell how far Colin has moved using odometry. It involves counting the encoder ticks for Colin’s motors and integrating that information over time to determine Colin’s change in position. This method has the distinct advantage that it relies on the actual motion of Colin’s wheels, and thus doesn’t require absolute accuracy from the speed control algorithm. Odometry also provides a good motion model that can be used as part of a larger localization algorithm. As such it’s a good stepping stone toward my goal of making a simultaneous localization and mapping program for Colin!

This tutorial owes a lot to MIT’s primer on odometry and motor control. It does a great job explaining the theory behind odometry.

Theory Basics

The position of a robot in space is referred to as its pose, which is defined by six quantities: its translation in Cartesian coordinates (x, y, and z) and its rotation about those three axes (θ_x, θ_y, and θ_z). Luckily, a differential drive robot like Colin can only translate in two dimensions and rotate in one, so Colin’s pose can be defined by three quantities (x, y, and θ_z).

Let’s say Colin’s initial pose is (0, 0, 0) at $t=t_{0}$ . How can we determine his change in pose when $t=t_{0}+\Delta t$ where $\Delta t$ is the time interval between pose updates? Because we’re already using encoders to control Colin’s speed, it’s easy to keep track of the distance Colin’s wheels have turned. In fact, my Encoder class already does this with its getDistance() function.

Let’s say $d_{left}$ is the distance turned by the left wheel over $\Delta t$ , and $d_{right}$ is the same quantity for the right wheel. Knowing these two distances can tell us a couple things. If $d_{left}=d_{right}$ then Colin traveled in a straight line. If $d_{left}\gt d_{right}$ he turned to the right and if $d_{left}\lt d_{right}$ he turned left. We can also use $d_{left}$ and $d_{right}$ to calculate Colin’s exact translation and rotation.

To simplify things a bit we’ll assume Colin’s wheel speeds are constant, which adds a negligible amount of error as long as we keep $\Delta t$ small. This assumption means that Colin is always travelling along a circular arc. The length of this arc, $d_{center}$ is given by the average of $d_{left}$ and $d_{right}$ :

$d_{center}=\frac{d_{left}+d_{right}}{2}$

We’ll say that Colin’s rotation in radians over $\Delta t$ is $\phi$ . Also, let $r_{left}$ be the distance between the center of Colin’s arc of travel and his left wheel and $r_{right}$ be the same distance for the right wheel. This means that $d_{left}=\phi r_{left}$ and $d_{right}=\phi r_{right}$ . Also, $r_{left}=r_{right}+d_{wheels}$ where $d_{wheels}$ is the distance between Colin’s wheels. With a little bit of algebra we can show the following:

$\phi=\frac{d_{right}-d_{left}}{{d_{wheels}}}$

We can also calculate Colin’s change in his x and y coordinates via the following equations:

$x'=x+d_{center}cos(\theta)$

$y'=y+d_{center}sin(\theta)$

Where $x'$ and $y'$ are the new x and y position, respectively. It’s important to note that the above equations are simplified. They assume that Colin’s motion happens in two discrete phases: he rotates in place and then translates along a straight line. This is clearly not true, but as long as $\phi$ is small, the error introduced is negligible. This means that, as with our prior simplification, we need to keep $\Delta t$ small to make this work. I’m not going to go into all the details here, but if you’re interested you can find the full derivation in the MIT odometry tutorial.

So, now that we have worked out the mathematical underpinnings for odometry, we can translate this into code!

Odometry Code

The magic happens in my new DifferentialDrive library. We’ll just go over the odometry portion today, but DifferentialDrive allows the user to control an arbitrary differential drive robot by specifying the robot’s translational and angular velocities and, optionally, the distance the robot should travel. I’ll explain all of that in a later post and include some implementation examples as well!

void DifferentialDrive::updatePosition()
{
   // get the angular distance traveled by each wheel since the last update
   double leftDegrees = _leftWheel->getDistance();
   double rightDegrees = _rightWheel->getDistance();

   // convert the angular distances to linear distances
   double dLeft = leftDegrees / _degreesPerMillimeter;
   double dRight = rightDegrees / _degreesPerMillimeter;

   // calculate the length of the arc traveled by Colin
   double dCenter = (dLeft + dRight) / 2.0;

   // calculate Colin's change in angle
   double phi = (dRight - dLeft) / (double)_wheelDistance;
   // add the change in angle to the previous angle
   _theta += phi;
   // constrain _theta to the range 0 to 2 pi
   if (_theta > 2.0 * pi) _theta -= 2.0 * pi;
   if (_theta < 0.0) _theta += 2.0 * pi;

   // update Colin's x and y coordinates
   _xPosition += dCenter * cos(_theta);
   _yPosition += dCenter * sin(_theta);
}

The above function needs to be called every $\Delta t$ and, to keep the error from our simplifications small $\Delta t$ needs to be small. In my testing I’ve found that doing a position update with the same frequency as the updates for the PID motor controller (every 50ms) results in good accuracy over short distances. However, this update involves a significant amount of extra computation, and doing it 20 times per second might require an excessive amount of processor time if you’re trying to do a lot of other computation at the same time. I’ve found that doing position updates half as often (every 100ms) results in very little loss of accuracy, so it’s entirely possible to balance accuracy and the resources your program has to spare.

Further Work

First of all, we need to integrate the above update function into the larger class that controls Colin’s motion. I’ll demonstrate that in a later post and include some examples that show how to use the class in an Arduino sketch.

Also, odometry can only be used to calculate Colin’s position relative to his starting position. It cannot be used to determine his absolute position in a space unless his starting position is known.

The larger problem is that odometry is inherently inaccurate. Encoder ticks do not translate directly into distance traveled by the wheel because wheels slip, the wheels aren’t perfectly circular, the ground isn’t perfectly flat, encoder ticks might be missed, and the motor gearbox has backlash that isn’t accounted for in our model. This means that Colin’s position calculated from odometry will gradually diverge from his true position. We could use other methods that might be more accurate, such as optical flow and IMUs. However, any sensor we might use suffers from some inherent random error, known as noise, and this error will accumulate over time.

To compensate for this error we can calculate Colin’s probable position by incorporating data from another sensor. This is what I’ll be working on over the next several months. First I’ll develop a program to localize him to a pre-existing (or a priori) map, and then I’ll work on a program that allows him to build his map on the fly.

I should note that software for this purpose is already available as part of the robot operating system (ROS), but I’m not interested in pre-made solutions. My goal here is to develop these solutions myself so we can all learn the intimate details of their operation.

Bot Blog

building my robot army one sketch at a time

Daily Archives: November 19, 2016

Odometry With Arduino

Theory Basics

Odometry Code

Further Work