Building a grid

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

U8.4MappingUniversity35 min

Do this lesson in the simulator

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 bump(x, y, amount):
    c, r = int(x / CELL), int(y / CELL)
    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)
        r = 0.0
        while r < min(d, FAR) - CELL:
            bump(x + r * sx, y + r * sy, L_FREE)
            r += CELL / 2
        if d < FAR:
            bump(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.

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.

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.