Project: map the mat

An unknown layout, a grid built from scratch, and a measurement taken off the map.

U8.7MappingUniversity50 min

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

Guide: SLAM explained: simultaneous localisation and mapping

A wall runs right across the mat with one way through it, and the robot does not know where. Build a map good enough to find the doorway, and report where it is.

This is the test that matters for a map. Not whether the picture looks convincing, but whether a number taken off it is right.

What the program has to do

1. Sweep. Turn on the spot, integrating every scan. One rotation from one place gives 360 degrees of bearings and a shadow behind every obstacle.
2. Move, and sweep again. A second viewpoint fills in most of the first one's shadows and confirms the walls that both saw. Confirmation matters as much as coverage: a cell that two viewpoints agree on is worth far more than one that a single beam clipped.
3. Measure the map. Find the lowest row with a proper run of occupied cells: that is the near face of the wall. Then walk that row and find the run of clear columns with wall on both sides. That is the doorway, and its middle is the answer.

Reading a doorway out of a grid

The pattern is worth learning because it generalises. Reduce the grid to a one dimensional signal, then find the feature in the signal.

band = [any cell in rows face..face+2 of this column is occupied for each column]

Now band is a row of booleans across the mat, wall everywhere except the doorway. Find the longest run of False with True on both sides, and the centre of that run in centimetres is the gap.

Bounding it on both sides is what makes it robust. Beyond the ends of the wall there is also clear space, but it is not enclosed by wall, so it cannot be mistaken for a door. Taking the longest such run rather than the first also survives a stray column that the robot happened never to observe.

Two things will otherwise catch you out. The mat's own edge is an obstacle and will be mapped as one, so the lowest occupied row is the edge behind the robot, not the wall: look above the robot's start and leave the border columns out of the count. And after a full turn on the spot the robot is facing in some arbitrary direction, so forward() will take it sideways across the mat. Turn back to a known heading before driving anywhere, or steer in the world frame with the inverse kinematics from U2.

The structure

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, 25.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)
        free = set()                         # each cell the beam crosses, once
        r = 0.0
        while r < min(d, FAR) - CELL:
            free.add((int((x + r * sx) / CELL), int((y + r * sy) / CELL)))
            r += CELL / 2
        for c, row in free:
            bump(c * CELL, row * CELL, L_FREE)
        if d < FAR:
            bump(x + d * sx, y + d * sy, L_OCC)

drive(0, 0, 30)
for tick in range(120):
    integrate()
    plot("known", sum(1 for v in grid if abs(v) > 1.0) / float(W * W))
    wait(0.1)
stop()

for row in range(30, 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(W)))

Run this in the simulator

Print the map. A wall of hashes with a hole in it, and a fan of dots reaching through the hole to the far side of the mat, because the beams that went through did not come back until they met the far edge.

What to look at when it is wrong

• The wall is two cells thick, or three. Mostly sensor noise, as in U8.4: short readings put hits in the floor cell in front of the wall, and the free marking stops a cell short and rarely takes them away. Any pose error adds to it. Not fatal, and it is why the band covers a few rows rather than one, but it is also why the lowest occupied row comes out about a cell in front of the real face.
• The doorway is in the wrong place by exactly one cell. Rounding. Check whether you are reporting the edge of a cell or its centre.
• There are two doorways. A column of wall the robot never saw. Require the run to be at least two cells and take the longest.
• The far side is all unknown. Correct, and honest. The robot has not been through the door.

Task: map the mat

The robot starts at (100, 25) facing up the mat. Map it, and print explored:, wall y: and gap x:. Do not touch anything.

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, 25.0
grid = [0.0] * (W * W)

Challenges

1. Report the width of the doorway as well as its middle, and say how accurate you expect that to be.
2. Build the map twice, once from one viewpoint and once from three, and compare how many cells are still unknown.
3. Add 5 degrees of error to every heading and measure how far the reported doorway moves.

What comes next

There is now a map, and there is a gap in the wall the robot has never driven through. U9 is planning: Dijkstra and A star over exactly this grid, RRT for when the state space is too large to enumerate, and potential fields for when the answer has to be computed at every tick rather than once. The map you have just built is the input to all three.