Motion Planning & Collision Avoidance

Finding collision-free paths automatically. Learn the configuration space (C-space) and C-obstacles, collision checking (bounding volumes, GJK, distance fields), complete planners (visibility graphs, cell decomposition), grid search (Dijkstra, A*), sampling-based planners (PRM, RRT, RRT-Connect, RRT*), path shortcutting and smoothing, artificial potential fields and local minima, trajectory optimisation, planning for manipulators with MoveIt, and task and motion planning — with NumPy code running an RRT in a 2-D configuration space around an obstacle.

In changing environments, robots can't rely on hand-taught paths. Motion planners compute collision-free paths from start to goal automatically — in the high-dimensional joint space of an arm or the plane of a mobile robot.

Sampling-based planners like RRT made planning in 6–7 dimensions practical and are the default in frameworks such as MoveIt.

An analogy

RRT is like exploring a dark maze with a torch: throw a ball in a random direction, walk from your nearest explored spot a step toward it, and repeat — quickly spreading a tree of explored paths until one reaches the exit.

Configuration space

Instead of planning the robot's whole body among obstacles, plan a single point in C-space, where each coordinate is a joint value. Obstacles map to C-obstacles (complex shapes); the robot is collision-free when its configuration lies in C_free. A mobile robot's disk-shaped body is handled by inflating obstacles by its radius (Minkowski sum).

Planning algorithm families

Main approaches:

Collision checking

Usually the most expensive part. Robots and environments use simplified geometry (capsules, convex hulls, meshes, octomaps from sensors); broad phase (bounding volume hierarchies) prunes pairs, narrow phase (GJK/EPA) computes distance or penetration. Signed distance fields enable gradient-based optimisation. Edges between samples must be checked at sufficient resolution, not just endpoints.

Planning in practice

MoveIt (ROS 2) combines OMPL sampling planners, collision checking (FCL), IK, constraints (keep a cup upright) and time parameterisation. Mobile robots layer global planners (A*, hybrid A*, lattice planners) with local planners (DWA, TEB, MPC) in Nav2. Task and motion planning decides action sequences (pick A, place B) jointly with motions. Learning-based planners and GPU-parallel planners (cuRobo) now plan in milliseconds.

Formulas

Worked example: C-space of a planar mobile robot

Given: Robot moving in the plane with heading

Find: Dimension.

Answer: 3-D C-space

Worked example: A* evaluation

Given: g = 12, h = 5 (Euclidean to goal)

Find: f.

Answer: 17 — expanded before nodes with larger f

Worked example: Path overhead

Given: RRT path 1.517, straight 1.131

Find: Overhead.

Answer: 34% longer — smooth it

An RRT planner around an obstacle

import numpy as np
# Sampling-based motion planning: a simple RRT in a 2-D configuration space with one rectangular obstacle
rng = np.random.default_rng(4)
start, goal = np.array([0.1, 0.1]), np.array([0.9, 0.9])
obstacle = (0.3, 0.7, 0.35, 0.65)                     # xmin, xmax, ymin, ymax
def free(p): return not (obstacle[0] <= p[0] <= obstacle[1] and obstacle[2] <= p[1] <= obstacle[3])
def edge_free(a, b): return all(free(a + (b - a) * s) for s in np.linspace(0, 1, 20))
nodes, parent, step = [start], [-1], 0.08
for it in range(3000):
    sample = goal if rng.random() < 0.1 else rng.random(2)
    i = int(np.argmin([np.linalg.norm(n - sample) for n in nodes]))
    d = sample - nodes[i]
    new = nodes[i] + d / (np.linalg.norm(d) + 1e-12) * min(step, np.linalg.norm(d))
    if edge_free(nodes[i], new):
        nodes.append(new); parent.append(i)
        if np.linalg.norm(new - goal) < step and edge_free(new, goal):
            nodes.append(goal); parent.append(len(nodes) - 2); break
path, k = [], len(nodes) - 1
while k != -1:
    path.append(nodes[k]); k = parent[k]
length = sum(np.linalg.norm(path[i] - path[i + 1]) for i in range(len(path) - 1))
print(f"RRT: {len(nodes)} nodes after {it + 1} iterations, path of {len(path)} waypoints, length {length:.3f}")
print(f"straight-line distance {np.linalg.norm(goal - start):.3f} (blocked by obstacle: {not edge_free(start, goal)})")
# RRT: 36 nodes after 48 iterations, path of 20 waypoints, length 1.517
# straight-line distance 1.131 (blocked by obstacle: True)

The straight line is blocked, so the RRT grows a tree with 10% goal bias and finds a detour after only 48 samples. The raw path (1.52) is 34% longer than straight — real planners shortcut and smooth the path afterwards, or use RRT* to converge toward the shortest path. The same code works for a 6-D joint space by changing the sampling and collision function.

Common mistakes

Applications

Key takeaways

Going further