Where am I? Odometry, and why it always drifts

The question every mobile robot asks

A robot vacuum maps your flat, a delivery robot crosses a hall, a rover drives on Mars. Before any of them can decide where to go, they need to know where they are. Indoors there is no GPS, and even outdoors GPS is metres off, which is the width of a corridor.

The simplest answer: count your own steps. If you know how far each wheel turned, you can add up the movements and know where you are. This is called odometry.

Pose: three numbers

Where a robot “is” on a flat floor takes three numbers, together called its pose:

	Meaning	Unit
$x$	how far along the floor, east	metres (or cm)
$y$	how far along the floor, north	metres (or cm)
$\theta$ (theta)	which way it faces, measured anticlockwise from the $x$ axis	radians

Two robots at the same $(x, y)$ facing different ways are in different poses, and they will see completely different things. The heading $\theta$ turns out to be the most important of the three, as you will see below.

From encoder counts to centimetres

A wheel encoder gives counts: little pulses as the wheel turns. To use them you need two numbers you measure on your own robot: the counts per wheel turn $N$, and the wheel radius $r$. One turn rolls the wheel by its circumference $2\pi r$, so one count is

$$\text{distance per count} = \frac{2\pi r}{N}$$

Example: the Mecanum Omni platform has 60 mm wheels, so $r$ = 3.0 cm and one turn is $2\pi \cdot 3.0 \approx$ 18.85 cm. Suppose your calibration measured $N$ = 390 counts per turn (yours will be different: the kit’s calibrate.encoders() measures it). Then one count is 18.85 / 390 ≈ 0.048 cm, about half a millimetre. 1000 counts on the left wheel means it rolled about 48 cm.

Odometry for a two-wheeled robot

Two wheels, distance $b$ between them (the track), wheel radius $r$. In a short time step the left wheel’s encoder says it rolled a distance $s_L$, the right one $s_R$:

$$s_L = r\,\Delta\varphi_L \qquad s_R = r\,\Delta\varphi_R$$

The robot’s centre moved forward by the average, and turned by the difference:

$$\Delta s = \frac{s_L + s_R}{2} \qquad \Delta\theta = \frac{s_R - s_L}{b}$$

Why the difference? If the right wheel rolls further than the left, the robot swings to the left, like a tank. The wider the track $b$, the less a given difference turns it.

Then update the position, heading in the direction the robot faces:

$$\begin{aligned} x &\leftarrow x + \Delta s \cos\theta \\ y &\leftarrow y + \Delta s \sin\theta \\ \theta &\leftarrow \theta + \Delta\theta \end{aligned}$$

Do this 50 times per second and you have a position.

from math import cos, sin

x = y = theta = 0.0

def odometry_step(s_left, s_right, track):
    global x, y, theta
    ds = (s_left + s_right) / 2
    dtheta = (s_right - s_left) / track
    x += ds * cos(theta + dtheta / 2)     # use the heading in the middle of the step
    y += ds * sin(theta + dtheta / 2)
    theta += dtheta

A worked example: driving a curve

Keep that same step (1.0 cm left, 1.2 cm right) for 100 steps. The robot drives part of a circle. Its radius is

$$R = \frac{b}{2}\cdot\frac{s_L + s_R}{s_R - s_L} = 5 \cdot \frac{2.2}{0.2} = 55 \text{ cm}$$

and after 100 steps it has turned $100 \cdot 0.02$ = 2 rad (about 115°). On a perfect circle that ends at $x = R\sin 2 \approx$ 50.01 cm, $y = R(1-\cos 2) \approx$ 77.89 cm.

What does the code say? With the heading from the middle of each step (as above): (50.01, 77.89). With the heading from the start of each step: (50.79, 77.39), almost 1 cm off after only 1.1 m of driving, and the error keeps growing. The midpoint trick costs nothing, so always use it.

Why it drifts

Every step adds a tiny error:

• the wheel slips a little when starting, turning or on dust,
• the wheel radius is not exactly what you measured, and it changes as the tyre is squashed,
• the encoder counts in steps, so a fraction of a step is lost each time,
• the floor is not perfectly flat.

A distance error grows slowly. A heading error is much worse: be off by 1° in direction, and after driving 5 metres you are already 9 cm to the side. After 20 metres, 35 cm. And the robot cannot notice it, because it only ever looks at its own wheels.

The sideways error is the distance times the sine of the heading error: $5 \text{ m} \cdot \sin 1° \approx$ 8.7 cm.

Two kinds of error

	Example	How it grows
systematic (always the same way)	wheel radius measured 1 % too big	in a straight line: 1 % of every distance
random (different each step)	small slips, bumps, counting steps	like a drunk walk: slower at first, but it never stops

You can fix systematic errors by calibrating: drive exactly 2 m, see what odometry says, and correct the radius. Random errors you cannot calibrate away.

Watching random drift grow

Here is an experiment you can run on any computer. 200 imaginary robots each try to drive 5 m straight, 1 cm per step. Each wheel step is randomly off by about 2 %. Odometry assumes perfect 1 cm steps, so it says every robot ends at exactly (500, 0). Where do they really end?

import random
from math import cos, sin, sqrt, degrees

TRACK = 10.0        # cm
NOISE = 0.02        # each wheel step is off by about 2 %
ROBOTS = 200

def drive_straight(steps):
    """One robot tries to drive straight, 1 cm per step. Returns where it really ends up."""
    x = y = theta = 0.0
    for _ in range(steps):
        s_left = 1.0 * (1 + random.gauss(0, NOISE))     # what the wheel really did
        s_right = 1.0 * (1 + random.gauss(0, NOISE))
        ds = (s_left + s_right) / 2
        dtheta = (s_right - s_left) / TRACK
        x += ds * cos(theta + dtheta / 2)
        y += ds * sin(theta + dtheta / 2)
        theta += dtheta
    return x, y, theta

def spread(values):
    mean = sum(values) / len(values)
    return sqrt(sum((v - mean) ** 2 for v in values) / len(values))

for metres in (1, 2, 3, 4, 5):
    ends = [drive_straight(metres * 100) for _ in range(ROBOTS)]
    print("%d m: sideways spread %5.1f cm, heading spread %4.1f deg"
          % (metres, spread([e[1] for e in ends]), degrees(spread([e[2] for e in ends]))))

A typical run prints something like this (yours will differ a little, it is random):

Distance	Sideways spread	Heading spread
1 m	1.6 cm	1.5°
2 m	4.6 cm	2.3°
3 m	8.2 cm	2.8°
4 m	12.9 cm	3.2°
5 m	18.4 cm	3.8°

Looking at the world: landmarks

Open your eyes in a room you know, and you recognise the door and the window. From them you know exactly where you are, however wrong your step count was. A robot does the same with a distance sensor, a lidar or a camera: it sees landmarks (walls, corners, objects) and corrects its position.

That needs a map of the landmarks. But to build a map, you need to know where you were when you saw each landmark. Which needs a map. This chicken-and-egg problem is called SLAM: Simultaneous Localisation And Mapping. The robot builds the map and finds itself in it at the same time, each one improving the other.

SLAM in pieces

A SLAM system is built from a few parts. Each one gets its own chapter in this course:

Part	Question it answers	Chapter
motion model	“I drove like this. Where could I be now, and how unsure am I?”	2
probability	“How do I combine an unsure guess with an unsure measurement?”	3
map	“What is free and what is a wall?”	4
localisation	“Given a map, where am I?”	5, 6
scan matching	“How did I move between two looks at the world?”	7
loop closure and optimisation	“I am back at the start. How do I fix everything I got wrong on the way?”	9, 10
path planning	“I have a map and know where I am. How do I get over there without hitting anything?”	11
exploration	“Which part of the map is still missing, and where should I go to see it?”	12

Chapter 8 puts the pieces together on a real small robot, and chapter 13 shows how a full, professional SLAM system does the same job.

Check yourself

What the full course covers

1. Where am I? Odometry, and why it always drifts (this chapter)
2. How robots move: motion models for differential and mecanum drive
3. Probability for robots: Bayes’ rule, Gaussians and noise
4. Seeing the world: range sensors and the occupancy grid
5. Many guesses at once: the particle filter
6. One smart guess: the Kalman filter and landmarks
7. Matching what you see with what you saw: scan matching
8. Building a map with a small robot and a laptop
9. Coming home: loop closure
10. Fixing the whole path: pose-graph optimisation
11. Using the map: planning a path and driving it
12. Exploring on its own: frontier-based exploration
13. Real SLAM: ROS 2, slam_toolbox and a small lidar