Plan a path in a 3-D voxel map: UAV Lab

What you will do

You will implement a planner for a small 3-D voxel map: a low wall to fly over or around, a full-height wall with a window, and a pillar. You will write the 3-D octile heuristic, the obstacle inflation, A* with a weight, and a line-of-sight shortcut. Then you will use the planner to measure how the path and the search effort change with the weight and with the vehicle size, and to find the largest inflation radius at which a path still exists.

The map and the numbers are illustrative; they are chosen so that every experiment runs in well under a second.

Animation of an obstacle-inflation sweep on the voxel map of the lab. The inflation radius grows from zero to three and a half cells. Orange dots mark the free cells that become blocked by the inflation; the green line is the shortest path found on the inflated map. With a small radius the path flies through the window of the full-height wall; from a radius of one cell the window is closed and the path goes around the wall and over the low wall, with a larger cost; at three cells no path remains. A plot on the right shows the path cost against the radius. — A vehicle of radius $r$ needs obstacles inflated by $r$ plus a margin (and the half-diagonal of a cell). The sweep shows that a path can disappear abruptly when the radius passes a threshold. The costs are computed by running A* on the inflated map for fifteen radii. (Looping, about 5 seconds; drawn and computed for this course with illustrative values.)

Animation of a line-of-sight shortcut on the grid path of the lab, drawn in isometric view with the inflated obstacle cells as orange dots. The grid path is the thin blue line with a dot at each of its waypoints. From the current waypoint the algorithm tries the farthest later waypoint; a segment that is blocked is drawn in red and the next closer waypoint is tried, and a clear segment is drawn in green, accepted, and becomes the new start. Counters show the length and the number of waypoints before and after: the shortened path has far fewer waypoints and is shorter. — The shortcut walks the path greedily and tests each candidate segment on the inflated map by sampling it every quarter of a cell. The result is a path with a few waypoints that is still collision-free on the inflated map. The path and the tests are computed on the lab map with an inflation radius of 1.5 cells. (Looping, about 6 seconds; drawn and computed for this course with illustrative values.)

Animation of weighted A* on the three-dimensional voxel map of the lab, drawn in isometric view. The weight on the heuristic is raised from one to three. For each weight the cells expanded by the search are shown as blue dots and the resulting path in green; two plots on the right show the number of expansions and the path cost relative to the optimal cost, both against the weight. Expansions fall quickly when the weight rises above one, while the path cost stays at the optimum for small weights and then rises, always below the weight times the optimal cost. — Weighted A* uses $f=g+w\,h$ with $w\ge 1$ : the heuristic pulls the search harder towards the goal, which saves work, and the path is at most $w$ times as long as the optimal one. The numbers are computed by running the search on the lab map for thirteen weights between 1 and 3. (Looping, about 5 seconds; drawn and computed for this course with illustrative values.)

Before you start

Lesson M5-T17 (3-D path planning on voxel maps) and, for the search, lesson M5-T11 (A* and RRT path planning).
Python 3 with NumPy.

Starter

import heapq, math
import numpy as np

def make_world():
    occ = np.zeros((16, 16, 8), bool)
    occ[5:7, 0:11, 0:5] = True          # low wall: over it (z >= 5) or around it (y >= 11)
    occ[10:12, 5:16, :] = True          # full-height wall ...
    occ[10:12, 8:11, 2:4] = False       # ... with a window (y 8..10, z 2..3)
    occ[3:5, 8:10, 0:4] = True          # pillar
    return occ

N26 = [(i, j, k) for i in (-1, 0, 1) for j in (-1, 0, 1) for k in (-1, 0, 1) if (i, j, k) != (0, 0, 0)]
occ = make_world(); start, goal = (1, 2, 1), (14, 13, 2)

def h_oct3(a, b):
    # TODO 1: the 3-D octile distance. Sort the absolute offsets d1 <= d2 <= d3 and combine them with the costs 1, sqrt(2), sqrt(3)
    raise NotImplementedError

def inflate(occ, r):
    # TODO 2: return a copy of occ in which every cell whose centre is within r cells of an occupied cell is also blocked
    raise NotImplementedError

def astar(occ, s, g, w=1.0):
    # TODO 3: A* on the 26-connected grid with step cost = Euclidean length of the move and f = g + w * h.
    #         Return (path as a list of cells, cost, number of expanded cells). w = 0 gives Dijkstra's algorithm.
    raise NotImplementedError

def shortcut(occ, path):
    # TODO 4: from each waypoint jump to the farthest later waypoint that is still in line of sight (sample the segment every 0.25 cells)
    raise NotImplementedError

Starter: the map is given; implement the four functions.

Reference solution

Show the solution (try first)

import heapq, math
import numpy as np

def make_world():
    occ = np.zeros((16, 16, 8), bool)
    occ[5:7, 0:11, 0:5] = True          # low wall: over it (z >= 5) or around it (y >= 11)
    occ[10:12, 5:16, :] = True          # full-height wall ...
    occ[10:12, 8:11, 2:4] = False       # ... with a window (y 8..10, z 2..3)
    occ[3:5, 8:10, 0:4] = True          # pillar
    return occ

N26 = [(i, j, k) for i in (-1, 0, 1) for j in (-1, 0, 1) for k in (-1, 0, 1) if (i, j, k) != (0, 0, 0)]

def h_oct3(a, b):
    d = sorted(abs(a[i] - b[i]) for i in range(3))
    return (math.sqrt(3) - math.sqrt(2)) * d[0] + (math.sqrt(2) - 1) * d[1] + d[2]

def inflate(occ, r):
    """Block every cell whose centre is within r cells of an occupied cell."""
    pts = np.argwhere(occ); out = occ.copy(); idx = np.argwhere(np.ones_like(occ))
    for c in pts:
        out[tuple(idx[np.linalg.norm(idx - c, axis=1) <= r].T)] = True
    return out

def astar(occ, s, g, w=1.0, climb=1.0):
    """Weighted A*: f = g + w h. `climb` multiplies the cost of the vertical part of a move (1 = no penalty)."""
    if occ[s] or occ[g]: return None, None, 0
    openq = [(w * h_oct3(s, g), 0.0, s)]; gs = {s: 0.0}; parent = {s: None}; closed = set()
    while openq:
        _, gc, u = heapq.heappop(openq)
        if u in closed: continue
        closed.add(u)
        if u == g: break
        for d in N26:
            v = (u[0] + d[0], u[1] + d[1], u[2] + d[2])
            if not all(0 <= v[i] < occ.shape[i] for i in range(3)) or occ[v] or v in closed: continue
            ng = gc + math.sqrt(d[0] ** 2 + d[1] ** 2 + (climb * d[2]) ** 2)
            if ng < gs.get(v, 1e18): gs[v] = ng; parent[v] = u; heapq.heappush(openq, (ng + w * h_oct3(v, g), ng, v))
    if g not in parent: return None, None, len(closed)
    path = []; u = g
    while u is not None: path.append(u); u = parent[u]
    return path[::-1], gs[g], len(closed)

def clear_segment(occ, a, b, step=0.25):
    a, b = np.array(a, float) + 0.5, np.array(b, float) + 0.5; n = max(1, int(np.linalg.norm(b - a) / step))
    return all(not occ[tuple(np.floor(a + (b - a) * t / n).astype(int))] for t in range(n + 1))

def shortcut(occ, path):
    out = [path[0]]; i = 0
    while i < len(path) - 1:
        j = len(path) - 1
        while j > i + 1 and not clear_segment(occ, path[i], path[j]): j -= 1
        out.append(path[j]); i = j
    return out

length = lambda pts: sum(math.dist(pts[i], pts[i + 1]) for i in range(len(pts) - 1))
occ = make_world(); s, g = (1, 2, 1), (14, 13, 2)

# 1. the optimal path and what the heuristic saves
path, cost, n_a = astar(occ, s, g); _, cost_d, n_d = astar(occ, s, g, w=0.0)       # w = 0 is Dijkstra
print(f"A*: cost {cost:.2f}, {n_a} expansions; Dijkstra: cost {cost_d:.2f}, {n_d} expansions")
assert abs(cost - cost_d) < 1e-9 and n_a < n_d / 5

# 2. weighted A*: fewer expansions, cost within w times optimal
rows = []
for w in (1.0, 1.5, 2.0, 3.0):
    _, c, n = astar(occ, s, g, w=w); rows.append((w, n, c)); print(f"w = {w}: {n:4d} expansions, cost {c:.2f} ({c / cost:.2f} x optimal)")
assert all(c <= w * cost + 1e-9 for w, n, c in rows) and rows[-1][1] <= rows[0][1]

# 3. how large can the vehicle be? sweep the inflation radius
last_ok = None
for r in (0.0, 0.5, 1.0, 1.5, 2.0, 2.5, 3.0, 4.0):
    p, c, _ = astar(inflate(occ, r), s, g)
    print(f"inflation {r:3.1f} cells: " + (f"cost {c:.2f}" if p else "no path"))
    if p: last_ok = r
assert last_ok is not None and 1.5 <= last_ok < 4.0                    # some route exists up to a radius of at least 1.5 cells, and none at 4

# 4. shortcut on the inflated map keeps the clearance
occ_i = inflate(occ, 1.5); pi, ci, _ = astar(occ_i, s, g); sc = shortcut(occ_i, pi)
print(f"waypoints {len(pi)} -> {len(sc)}, length {length(pi):.2f} -> {length(sc):.2f} cells")
assert len(sc) < len(pi) and all(clear_segment(occ_i, sc[i], sc[i + 1]) for i in range(len(sc) - 1))

# 5. a penalty on vertical motion: less climbing, possibly a longer path
climb_total = lambda p: sum(max(0, p[i + 1][2] - p[i][2]) for i in range(len(p) - 1))
pc, cc, _ = astar(occ, s, g, climb=3.0)
print(f"climb: {climb_total(path)} cells -> {climb_total(pc)} cells with a vertical penalty of 3")
assert climb_total(pc) <= climb_total(path)

Solution with five experiments and their checks.

Experiments

Check that the path is collision-free and that its cost never falls below the straight-line distance between start and goal.
Tabulate expansions and path cost for $w=1$ , 1.5, 2, 3. At which $w$ does the cost first exceed the optimal one, and is it always within $w$ times the optimum?
Sweep the inflation radius in steps of 0.25 cells between 2.5 and 3.0 and find the largest radius that still has a path. Convert it to metres for a cell size of 0.2 m and compare with a vehicle of 0.35 m radius plus a 0.1 m margin and the half-diagonal of a cell.
Plot the unshortened and shortened paths in a 3-D scatter plot (matplotlib) and mark the window. On the raw map the path flies through the window; find the smallest inflation radius at which it stops using the window, and check that the shortened path on the inflated map stays clear of every wall.
Double the resolution (repeat every voxel twice along each axis, and double the start and goal coordinates) and compare the number of expansions with the factor of eight in cells.

How to know it worked

The starter functions are implemented and the solution’s assertions pass for your version.
A* and Dijkstra give the same path cost and A* expands far fewer cells.
You have a table of expansions and cost against the weight, and the largest inflation radius with a path.
The shortened path is clear on the inflated map and has fewer waypoints.

If it does not work

Troubleshooting
Symptom	Likely cause	What to check
A* returns a longer path than Dijkstra	The heuristic overestimates (for example the 2-D octile distance applied to three axes, or the wrong sort order of the offsets)	Test $h(u)\le \mathrm{cost}(u,\,v)+h(v)$ on random neighbours, and check that $h$ equals the true cost on an empty map
No path appears after inflation	The start or goal lies inside the inflated obstacles, or the only route was closed by the inflation	Print which cells are blocked near the start and the goal and reduce the radius, or move them
The shortcut path touches the wall	The line-of-sight check used the raw map, or the sampling step is too coarse	Check on the inflated map and sample every quarter of a cell or finer
Planning is slow	Neighbours are generated with Python loops over a large map	Limit the search to a local window, use a coarser map, or raise the weight

References

Record your progress

Saved in this browser. This lab runs on your own machine, so the site cannot check it for you.