Building a grid

Eight beams, a known pose, and a map that fills in while the robot drives.

U8.4MappingUniversity35 min

Print it: worksheet (PDF) · answers (PDF) · as a web page

Guide: SLAM explained: simultaneous localisation and mapping

Everything so far was one beam from one place. A map is the same operation run several thousand times.

every tick:
    pose = where the robot is and which way it faces
    for each (angle, range) in scan():
        run the inverse sensor model into the grid

That is the whole of occupancy grid mapping with known poses. There is no more to it, and it is worth noticing how little there is, because the difficulty in real mapping is entirely in the pose.

Storing the grid

A flat list is faster than a list of lists and the indexing is one multiplication:

grid = [0.0] * (W * W)
i = row * W + col

Zero is unknown, which means an empty map costs nothing to initialise and needs no separate "have I seen this" array.

The pose goes in first

The single most important line in a mapper is the one that fetches the pose, and it is the one most likely to be subtly wrong.

• Which frame? position() here is relative to where the robot started, not to the mat. Add the start position, or the whole map comes out shifted and the walls land off the edge.
• Which instant? The pose when the scan was taken, not the pose now. At 20 cm/s and a 100 ms lag that is 2 cm of smear on every wall.
• Degrees or radians? heading() is degrees. math.sin wants radians. A map built in the wrong units is unmistakable and still catches people.

Watching it fill

from bugbot import *
import math
connect()

CELL, W = 5.0, 40
L_OCC, L_FREE, L_MAX, FAR = 0.85, -0.4, 8.0, 170.0
START_X, START_Y = 100.0, 40.0
grid = [0.0] * (W * W)

def cell(x, y):
    return int(x / CELL), int(y / CELL)

def bump(c, r, amount):
    if 0 <= c < W and 0 <= r < W:
        grid[r * W + c] = max(-L_MAX, min(L_MAX, grid[r * W + c] + amount))

def integrate():
    px, py = position()
    x, y, h = START_X + px, START_Y + py, heading()
    for a, d in scan():
        th = math.radians(h + a)
        sx, sy = math.sin(th), math.cos(th)
        free = set()                    # each cell the beam crosses, once
        r = 0.0
        while r < min(d, FAR) - CELL:
            free.add(cell(x + r * sx, y + r * sy))
            r += CELL / 2
        for c, row in free:
            bump(c, row, L_FREE)
        if d < FAR:
            bump(*cell(x + d * sx, y + d * sy), L_OCC)

forward(55)
for tick in range(50):
    integrate()
    plot("known", sum(1 for v in grid if abs(v) > 1.0) / float(W * W))
    if position()[1] > 55:
        stop()
    wait(0.1)
stop()

for row in range(36, 18, -1):
    print(str(row).rjust(3), "".join("#" if grid[row * W + c] > 1 else ("." if grid[row * W + c] < -1 else " ")
                                     for c in range(6, 34)))

Run this in the simulator

A wedge of dots opening out ahead of the robot, and a line of hashes across the top where the wall is. That is a map.

Look at the line of hashes closely. It is two rows deep, rows 29 and 30, but the wall's face is at y = 150, the bottom edge of row 30, so row 29 is floor. The readings here wobble by about 2 cm. One that comes back short puts its hit in row 29, while the free marking stops a cell short of every hit and reaches row 29 only when a reading comes back long, and not always then. Over this run row 29 took 171 hits and 147 passes, and the cell straight ahead of the robot took 23 hits and 13 passes, which is +19.55 against -5.2, so it sits at the clamp as firmly as the wall does. That is why walls in a grid come out about a cell thick, on the side the robot saw them from.

The known fraction

Plot the proportion of cells the map has an opinion about and you get the most useful single number in exploration. It rises steeply at first, because the first scan from a new place is nearly all new information, then flattens as the robot re-observes what it already knows.

When the curve flattens, driving further in the same direction is buying nothing. That observation is the whole of U8.6.

Reading an answer off the map

The point of a map is that it can be measured. To find the near face of a wall, walk the rows from the bottom and take the first one with occupied cells in it. That answer is better than any single reading, because it has averaged over every beam that ever crossed those cells, and it is available from anywhere on the mat rather than only from in front of the wall. Expect it to come out about a cell early, for the reason above: here the first occupied row is row 29, whose centre is 147.5, against a face at 150.

Be a little careful about what counts. A single occupied cell can be a stray reflection, so requiring several in a row before believing a wall is normal, and is the simplest form of the filtering that a real system does with morphological operations.

Task: build a grid while you drive

The robot starts at (100, 40). Drive at least 40 cm up the mat, integrating every scan into a log odds grid. Plot known, then print known:, the fraction of the grid your map has an opinion about, and wall y:, where your map puts the near face of the wall.

from bugbot import *
import math
connect()

CELL, W = 5.0, 40
L_OCC, L_FREE, L_MAX, FAR = 0.85, -0.4, 8.0, 170.0
START_X, START_Y = 100.0, 40.0
grid = [0.0] * (W * W)

Challenges

1. Corrupt the pose on purpose by adding 10 degrees to the heading. What does the wall look like now?
2. Count how many cells ever change from free to occupied or back. What does a large number mean?
3. Print the map with one character per cell every 2 seconds and watch the wedge grow.