○Draft — not verified

Theoretical Background: Path Planning Algorithms

Module 4 Theory: Path Planning

4.2 Configuration Space Representation

4.2.1 Configuration Space (C-Space)

The configuration space $C$ is the set of all possible robot configurations. For a point robot in 2D, $C = R^{2}$ . For a rigid body in 2D, $C = R^{2} \times S O (1)$ . For an $n$ -DOF manipulator, $C = R^{n}$ (joint space).

$C$ -space partitions into two disjoint sets:

C = C_{free} \cup C_{obs} (1)

$C_{free}$ : The set of collision-free configurations.

$C_{obs}$ : The set of configurations causing collision with obstacles.

Path Planning Goal: Find a continuous path from $q_{start}$ to $q_{goal}$ that lies entirely within $C_{free}$ .

4.2.2 Minkowski Sum for Obstacle Expansion

For a circular robot with radius $r$ , the C-space obstacle is the Minkowski sum:

C_{obs} = O \oplus R (2)

This inflates each obstacle boundary by the robot radius, reducing the problem to planning for a point robot. The theoretical advantage is that collision checking becomes a simple point-in-region test rather than a full geometric intersection.

4.2.3 Occupancy Grid as C-Space Approximation

For mobile robots, the 2D occupancy grid approximates C-space:

m (x, y) \in {free, occupied, unknown} (3)

Each cell stores the probability of being occupied, derived from sensor measurements. The grid resolution determines the trade-off between planning accuracy and computational cost.

4.3 Graph-Based Methods

4.3.1 Dijkstra’s Algorithm

Dijkstra’s algorithm finds the shortest path in a weighted graph by expanding nodes in order of increasing cost from the start.

Algorithm (Dijkstra):

DIJKSTRA(G, start, goal):
        dist[start]       = 0
        dist[all others]  = inf
        Q = priority queue with all vertices
        
        while Q is not empty:
        u = vertex in Q with min dist[u]
        remove u from Q
        if u == goal:
        return reconstruct_path(u)
        for each neighbor v of u:
        alt = dist[u] + weight(u, v)
        if alt < dist[v]:
        dist[v] = alt
        prev[v] = u

Complexity: $O ((V + E) lo g V)$ with a binary heap.

Properties: Guarantees the optimal (shortest) path. Explores in all directions because it uses no heuristic. Complete — finds a path if one exists.

4.3.2 A* Algorithm

A* extends Dijkstra by adding a heuristic function that guides the search toward the goal, reducing the number of explored nodes.

Cost Function:

f (n) = g (n) + h (n) (4)

where $g (n)$ is the actual cost from start to node $n$ , $h (n)$ is the estimated cost from $n$ to goal (heuristic), and $f (n)$ is the estimated total cost through $n$ .

Algorithm (A*):

A_STAR(G, start, goal):
        open_set    = {start}
        g[start]    = 0
        f[start]    = h(start, goal)
        
        while open_set is not empty:
        current = node in open_set with lowest f
        if current == goal:
        return reconstruct_path(current)
        remove current from open_set
        for each neighbor n of current:
        tentative_g = g[current] + cost(current, n)
        if tentative_g < g[n]:
        prev[n] = current
        g[n]    = tentative_g
        f[n]    = g[n] + h(n, goal)
        add n to open_set

Admissible Heuristics must never overestimate the true cost to goal:

Euclidean distance: $h (n) = (x_{n} - x_{g})^{2} + (y_{n} - y_{g})^{2}$ — suitable for any movement direction.

Manhattan distance: $h (n) = ∣ x_{n} - x_{g} ∣ + ∣ y_{n} - y_{g} ∣$ — for 4-connected grids only.

Diagonal distance: Accounts for diagonal movement cost in 8-connected grids.

Key Result: A* is complete and optimal when $h$ is admissible. Its complexity is $O (b^{d})$ in the worst case but typically much better with a well-chosen heuristic.

4.3.3 Dijkstra vs. A* Comparison

Heuristic usage: Dijkstra uses none; A* requires an admissible heuristic.

Optimality: Both are optimal (A* requires admissible $h$ ).

Nodes explored: Dijkstra explores more; A* focuses toward the goal.

Best application: Dijkstra when goal direction is unknown; A* when the goal position is known.

4.4 Sampling-Based Approaches

4.4.1 Rapidly-Exploring Random Trees (RRT)

RRT builds a tree by randomly sampling configurations and extending toward them, providing rapid exploration of high-dimensional C-spaces.

Algorithm (RRT):

RRT(q_start, q_goal, max_iterations):
        T.add_vertex(q_start)
        for i = 1 to max_iterations:
        q_rand = RANDOM_CONFIG()
        q_near = NEAREST(T, q_rand)
        q_new  = STEER(q_near, q_rand, step_size)
        if COLLISION_FREE(q_near, q_new):
        T.add_vertex(q_new)
        T.add_edge(q_near, q_new)
        if ||q_new - q_goal|| < threshold:
        return PATH(T, q_start, q_new)
        return FAILURE

Properties: Probabilistically complete (finds a path with probability 1 as iterations approach infinity). Not optimal — paths are typically jagged and suboptimal. Exhibits Voronoi bias, meaning it naturally explores toward large unexplored regions.

4.4.2 RRT* (Optimal RRT)

RRT* extends RRT with two key additions that guarantee asymptotic optimality.

Rewiring: After adding $q_{new}$ , check whether nearby vertices can be reached more cheaply through $q_{new}$ and update parent pointers accordingly.

Neighbourhood Search: Consider all vertices within a radius:

r_{n} = γ (\frac{lo g n}{n})^{1/ d} (5)

where $n$ is the current number of vertices and $d$ is the C-space dimension.

Algorithm (RRT* additions after inserting $q_{new}$ ):

Q_near = NEAR(T, q_new, r_n)
        
        # Choose best parent for q_new
        q_min = q_near
        c_min = cost(q_near) + dist(q_near, q_new)
        for q in Q_near:
        if COLLISION_FREE(q, q_new):
        c = cost(q) + dist(q, q_new)
        if c < c_min:
        q_min = q
        c_min = c
        parent[q_new] = q_min
        
        # Rewire existing vertices through q_new
        for q in Q_near:
        if COLLISION_FREE(q_new, q):
        if cost(q_new) + dist(q_new, q) < cost(q):
        parent[q] = q_new

Key Result: RRT* converges to the optimal path as $n \to \infty$ , though each iteration is more expensive than basic RRT due to the neighbourhood search and rewiring.

4.4.3 Probabilistic Roadmaps (PRM)

PRM is a two-phase approach: build a roadmap offline, then query it for specific start-goal pairs.

Construction Phase:

PRM_BUILD(N, k):
        V = {}, E = {}
        for i = 1 to N:
        q = RANDOM_FREE_CONFIG()
        V.add(q)
        Q_near = K_NEAREST(V, q, k)
        for q_near in Q_near:
        if COLLISION_FREE(q, q_near):
        E.add(q, q_near)
        return Graph(V, E)

Query Phase: Connect start and goal to the nearest roadmap vertices, then search the graph using A* or Dijkstra.

Multi-query advantage: The roadmap is built once and reused for many queries, making PRM efficient for static environments with repeated planning needs. It is not suited for dynamic environments or single-query problems.

4.5 Potential Field Methods

4.5.1 Artificial Potential Field

The robot moves under the influence of an artificial potential function composed of attractive and repulsive components.

Attractive Potential (toward goal):

U_{att} (q) = \frac{1}{2} ξ ∥ q - q_{goal} ∥^{2} (6)

Repulsive Potential (away from obstacles):

U_{rep} (q) = ⎩ ⎨ ⎧ \frac{1}{2} η (\frac{1}{d ( q )} - \frac{1}{d _{0}})^{2} 0 if d (q) \leq d_{0} if d (q) > d_{0} (7)

where $d (q)$ is the distance to the nearest obstacle, $d_{0}$ is the influence distance threshold, and $ξ$ , $η$ are positive scaling constants.

Total Potential:

U (q) = U_{att} (q) + U_{rep} (q) (8)

Force (negative gradient of potential):

F (q) = - \nabla U (q) = F_{att} (q) + F_{rep} (q) (9)

F_{att} (q) = - ξ (q - q_{goal}) (10)

F_{rep} (q) = ⎩ ⎨ ⎧ η (\frac{1}{d ( q )} - \frac{1}{d _{0}}) \frac{1}{d ( q ) ^{2}} \nabla d (q) 0 if d (q) \leq d_{0} if d (q) > d_{0} (11)

4.5.2 Local Minima Problem

The main theoretical limitation of potential fields is that the robot can become trapped in local minima where $F = 0$ but $q \neq = q_{goal}$ . This occurs when attractive and repulsive forces exactly cancel.

Random Walk: Add random perturbation to escape, sacrificing determinism for practical effectiveness.

Navigation Functions: Special potential functions mathematically guaranteed to have only one minimum (at the goal). Constructing them requires complete knowledge of the environment.

Hybrid Approach: Use potential fields for local reactive planning while a global planner (A*, RRT) provides waypoints to follow.

4.6 Optimization-Based Planning

4.6.1 Trajectory Optimization Formulation

Planning can be formulated as a constrained optimization problem:

minimize subject to \int_{0}^{T} L (q (t), \overset{q}{˙} (t)) d t q (0) = q_{start}, q (T) = q_{goal} q (t) \in C_{free}, \forall t \in [0, T] (12)

Common cost functions:

Minimum time: $L = 1$
Minimum energy: $L = ∥ τ ∥^{2}$
Minimum jerk: $L = ∥ q ... ∥^{2}$ (produces smooth trajectories)

4.6.2 CHOMP (Covariant Hamiltonian Optimization for Motion Planning)

CHOMP optimizes an initial trajectory using functional gradient descent:

cost (ξ) = f_{obs} (ξ) + λ f_{smooth} (ξ) (13)

where $f_{obs}$ is the obstacle cost (penetration depth), $f_{smooth}$ is the smoothness cost (integral of squared velocity or acceleration), and $λ$ is the trade-off parameter controlling the balance between collision avoidance and trajectory smoothness.

4.7 Summary of Planning Method Properties

Dijkstra: Complete, optimal, $O ((V + E) lo g V)$ complexity, best for small discrete spaces.

A*: Complete, optimal with admissible heuristic, $O (b^{d})$ complexity, best for grid-based planning with known goal.

RRT: Probabilistically complete, not optimal, $O (n)$ per iteration, best for high-dimensional single-query problems.

RRT*: Probabilistically complete, asymptotically optimal, $O (n lo g n)$ per iteration, best for optimal high-dimensional planning.

PRM: Probabilistically complete, asymptotically optimal, $O (n lo g n)$ construction, best for multi-query static environments.

Potential Field: Incomplete (local minima), not globally optimal, $O (1)$ per step, best for real-time reactive control.

CHOMP: Incomplete, locally optimal, $O (n^{3})$ per iteration, best for generating smooth trajectories.

4.8 Non-Holonomic Feasibility

All the planners above (A*, Dijkstra, RRT/RRT*, PRM, potential fields) search the geometric configuration space $C$ . For an omnidirectional (holonomic) robot that is enough — every geometric path is executable. For a differential-drive robot it is not. The non-holonomic constraint derived in Module 2.2,

\overset{x}{˙} sin θ - \overset{y}{˙} cos θ = 0, (14)

forbids instantaneous lateral motion. A straight diagonal segment produced by A* on an occupancy grid is geometrically short but kinematically infeasible: the robot cannot slide sideways onto it. The planner’s output and the robot’s executable motion manifold do not coincide.

Three practical ways to reconcile this gap:

Motion primitives / kinodynamic planners. Replace straight edges with Dubins or Reeds–Shepp arcs (minimum turning radius $R_{m i n}$ ), or use state-lattice planners that only expand nodes reachable by feasible controls. RRT* with Dubins steering is the most common.
Path post-processing. Keep the geometric plan, then smooth it with CHOMP (§4.6.2) or B-spline fitting under curvature bounds $∣ κ ∣ \leq 1/ R_{m i n}$ . Cheap, but fails when the planner’s path is tighter than $R_{m i n}$ .
Tracking controller absorbs the gap. The most common deployment pattern: a geometric planner feeds waypoints to a Pure Pursuit or DWA controller (Module 5.4) that respects the kinematic constraint at tracking time. This works when the reference path is gently curved and the controller has enough speed margin to cut corners, which is why it is paired with inflated obstacles (§4.2 equation 2).

When you implement planners for Assignment 2 on the warehouse map, state explicitly which of these three strategies each of your planners (A*, RRT*, PRM) relies on. A* and PRM on an 8-connected grid produce paths that assume option 3; RRT* can be configured with Dubins steering for option 1.

Integration: Theory to Practice

In ROS 2, the Nav2 stack implements several of these algorithms. The global planner typically uses A* or Dijkstra on the costmap (an occupancy grid implementing Equation (3)), while the local planner uses a variant of potential fields or trajectory optimization for real-time obstacle avoidance. The costmap performs Minkowski expansion (Equation (2)) via the inflation layer, which inflates obstacles by the robot footprint radius. RRT and PRM are used in MoveIt for manipulator planning in higher-dimensional C-spaces. The planner selection depends on the dimensionality of C-space, whether the environment is static or dynamic, and whether optimality or speed is prioritised.

Theoretical Design Choices

A* is preferred over Dijkstra for mobile robot navigation because the Euclidean heuristic is trivially admissible in physical space and dramatically reduces node expansion. RRT is chosen for high-dimensional problems (manipulators with 6+ DOF) because grid-based methods suffer exponential growth with dimension, while RRT’s per-iteration cost is independent of dimensionality. RRT* is used when path quality matters because basic RRT produces arbitrarily suboptimal paths, whereas the rewiring step in RRT* guarantees convergence to the optimal solution. Potential fields are retained for local planning despite the local minima problem because their $O (1)$ computation time enables real-time reactive control at sensor rates, and the hybrid approach with a global planner eliminates the local minima issue in practice. CHOMP is selected when trajectory smoothness is critical (e.g., manipulator motion near humans) because the smoothness cost directly penalises jerk, producing comfortable and predictable motions.