Motion Planning

1. The Goal Behind Obstacles

In an open workspace $\mathcal{W} \subset \mathbb{R}^2$, moving between two points is trivial: simply drive along the straight line connecting start $S$ and goal $G$. In real industrial facilities, warehouses, and homes, the direct line of sight is obstructed by walls, machines, and tables.

Consider a mobile base situated at start coordinate $S = (1.0, 2.0)\,\text{m}$ tasked with reaching target $G = (9.0, 8.0)\,\text{m}$. Two rectangular physical barriers block direct line-of-sight travel:

• Obstacle 1 spans $x \in [4.0, 5.5]\,\text{m}, y \in [1.0, 4.5]\,\text{m}$.
• Obstacle 2 spans $x \in [4.0, 5.5]\,\text{m}, y \in [5.1, 9.0]\,\text{m}$.

The straight line connecting $S$ and $G$ intersects Obstacle 1, meaning an uninformed drive command results in an immediate crash. The robot must formulate a sequence of collision-free movements around physical obstacles.

2. The Robot Footprint Problem

Between Obstacle 1 ($y \le 4.5\,\text{m}$) and Obstacle 2 ($y \ge 5.1\,\text{m}$) lies a gap of width:

$$w_{\text{gap}} = 5.1 - 4.5 = 0.60\,\text{m}$$

If the robot were an infinitesimal mathematical point ($R = 0$), it could slip through this $0.60\,\text{m}$ opening without difficulty. However, real robots occupy physical space. Our differential-drive mobile base has a circular chassis with radius $R = 0.40\,\text{m}$, yielding an overall diameter of:

$$D = 2R = 0.80\,\text{m}$$

Because $0.80\,\text{m} > 0.60\,\text{m}$, the physical body cannot squeeze through the gap. Checking collisions only at the robot's center coordinate produces catastrophic failures where the wheels and bumper wedge violently into the barrier walls.

3. Configuration Space and Obstacle Dilation

Rather than testing complex geometry collisions at every microstep along a path, motion planning transforms the problem into Configuration Space ($\mathcal{C}$-space). In $\mathcal{C}$-space, every robot posture is mapped to a single mathematical point $q \in \mathcal{C}$.

To preserve safety, all physical workspace obstacles $\mathcal{W}_{\text{obs}}$ are inflated outward by the robot's geometry through a Minkowski sum with a circular structuring element of radius $R = 0.40\,\text{m}$:

$$\mathcal{C}_{\text{obs}} = \mathcal{W}_{\text{obs}} \oplus \mathcal{B}_R$$

When Obstacle 1 and Obstacle 2 are dilated by $0.40\,\text{m}$, their boundary boundaries expand:

• Obstacle 1 upper boundary extends from $4.50\,\text{m}$ to $4.90\,\text{m}$.
• Obstacle 2 lower boundary extends from $5.10\,\text{m}$ to $4.70\,\text{m}$.

Because $4.90\,\text{m} > 4.70\,\text{m}$, the dilated obstacle shells overlap by $0.20\,\text{m}$. The narrow gap vanishes entirely in $\mathcal{C}_{\text{obs}}$, proving rigorously that the passage is impassable. The planning algorithm can now treat the robot as a single point navigating free space $\mathcal{C}_{\text{free}} = \mathcal{C} \setminus \mathcal{C}_{\text{obs}}$.

4. Discretizing into an Occupancy Grid

Continuous space contains an uncountably infinite number of coordinates. To perform systematic search, the planner overlays a discrete 2D occupancy grid onto $\mathcal{C}$-space.

Each square cell has a side length of $\Delta = 0.50\,\text{m}$. Cells intersecting $\mathcal{C}_{\text{obs}}$ are marked as blocked, while cells completely inside $\mathcal{C}_{\text{free}}$ are marked as traversable.

Robot transitions are defined through an 8-connected graph:

• Orthogonal steps (North, South, East, West) have cost $c_{\text{orth}} = \Delta = 0.50\,\text{m}$.
• Diagonal steps (Northeast, Northwest, Southeast, Southwest) have cost $c_{\text{diag}} = \sqrt{2}\Delta \approx 0.71\,\text{m}$.

This graph topology translates spatial navigation into a classic shortest-path network search.

5. Scoring Cells with A*

Uninformed algorithms like Breadth-First Search or Dijkstra search blindly in all directions. The $A^*$ algorithm accelerates search by directing exploration toward the goal using a heuristic evaluation function:

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

Where:

• $g(n)$ is the exact accumulated path cost from the start node $S$ to current node $n$.
• $h(n)$ is the estimated remaining distance from node $n$ to goal $G$.

For a candidate node at $n = (3.5, 4.5)\,\text{m}$ with prior travel cost $g(n) = 4.32\,\text{m}$, the Euclidean distance heuristic to goal $G = (9.0, 8.0)\,\text{m}$ evaluates to:

$$h(n) = \sqrt{(9.0 - 3.5)^2 + (8.0 - 4.5)^2} = \sqrt{5.5^2 + 3.5^2} = \sqrt{30.25 + 12.25} \approx 6.52\,\text{m}$$

The total priority score is:

$$f(n) = 4.32 + 6.52 = 10.84\,\text{m}$$

Because Euclidean distance never overestimates true travel distance ($h(n) \le h^*(n)$), the heuristic is strictly admissible, guaranteeing that $A^*$ returns the mathematically optimal shortest path.

6. Frontier Expansion via Priority Queue

The $A^*$ algorithm maintains two sets:

• Open Set: A priority queue (min-heap) of discovered nodes awaiting evaluation, keyed on lowest $f(n)$.
• Closed Set: A registry of fully evaluated nodes whose optimal routes from start have already been established.

At each iteration, the planner extracts the top node from the min-heap ($f = 10.84\,\text{m}$), examines its valid 8-connected neighbors in $\mathcal{C}_{\text{free}}$, updates tentative $g$-costs, and inserts unvisited neighbors into the heap.

The search wavefront naturally elongates into a directed cone toward the goal. When the obstacle forces the wavefront sideways, $A^*$ explores around the perimeter without wasting compute searching empty rooms behind the start coordinate.

7. Sampling-Based Planning in High Dimensions

Grid search works well in 2D ($x, y$) or 3D ($x, y, \theta$), but collapses on robotic arms. A 7-DoF manipulator requires 7 joint angles $q = (\theta_1, \dots, \theta_7)$ to define its configuration. Discretizing each joint into just 50 bins requires:

$$N_{\text{cells}} = 50^7 \approx 7.8 \times 10^{11} \text{ cells}$$

This astronomical memory footprint is the Curse of Dimensionality. To plan motions for high-DoF manipulators, roboticists use sampling-based algorithms such as Rapidly-exploring Random Trees (RRT) and Probabilistic Roadmaps (PRM).

RRT builds a search tree incrementally:

1. Sample a random configuration $q_{\text{rand}} \in \mathcal{C}_{\text{free}}$.
2. Find the nearest existing tree vertex $q_{\text{near}}$.
3. Extend a branch from $q_{\text{near}}$ toward $q_{\text{rand}}$ by step size $\epsilon$.
4. Check whether the swept link volume intersects obstacles. If collision-free, add new node $q_{\text{new}}$ to the tree.

RRT provides probabilistic completeness: as sample count approaches infinity, the probability of finding a path (if one exists) approaches 1, without ever constructing a discrete global grid.

8. Geometric Path vs Kinodynamic Trajectory

A path found by $A^*$ or RRT is purely geometric: an ordered sequence of spatial points $q(s)$. It specifies where the robot should be, but says nothing about when it should arrive or how fast its motors should spin.

If a motor controller attempts to execute a piecewise linear path with sharp 90-degree corners, velocity along the initial axis must drop to zero instantaneously while lateral velocity spikes instantaneously. This requires infinite acceleration ($a = \infty$), which triggers motor current limits, wheel skid, and severe tracking deviation.

A kinodynamic trajectory parameterizes the geometric path with respect to time:

$$\tau: [0, T] \to \mathcal{C}, \quad t \mapsto (q(t), \dot{q}(t), \ddot{q}(t))$$

Trajectory generators assign trapezoidal or S-curve velocity profiles subject to physical actuator limits:

$$|\dot{q}(t)| \le v_{\max} = 1.0\,\text{m/s}, \quad |\ddot{q}(t)| \le a_{\max} = 0.5\,\text{m/s}^2$$

9. Path Smoothing and Feedback Tracking

To bridge the gap between discrete search and physical actuators, the raw geometric path passes through two stages:

1. Spline Smoothing: Fitting cubic B-splines or quintic polynomials through waypoints replaces jagged corners with continuous curvature ($C^2$ continuity), ensuring that required steering rates remain smooth and bounded.
2. Closed-Loop Trajectory Tracking: While executing the trajectory, external disturbances and wheel slip cause real-world position $x(t)$ to drift from planned reference $x_d(t)$. Real-time controllers (Pure Pursuit, Model Predictive Control, or PID) continuously measure tracking error $e(t) = x_d(t) - x(t)$ and adjust wheel speeds to steer back onto the reference path.

10. Where It Breaks

Classic motion planning algorithms operate under the simplifying assumption that the environment is static and completely known. Real operational deployments break these assumptions:

1. Dynamic Moving Obstacles: A warehouse forklift or walking pedestrian moves into the planned path after computation completes. Because static $A^*$ assumed fixed geometry, the robot executes its trajectory into the newly occupied space unless protected by real-time reactive collision monitors.
2. Heavy Replanning Latency: If an obstacle blocks the corridor, triggering a complete global re-search can consume $2.5\,\text{s}$ of CPU time. At a travel speed of $1.0\,\text{m/s}$, the robot covers $2.5\,\text{m}$ before the new path is computed.
3. Kinodynamic Infeasibility: Non-holonomic vehicles (like cars with Ackermann steering or differential bases) cannot move sideways. If a geometric planner ignores differential drive kinematic constraints ($\dot{y}\cos\theta - \dot{x}\sin\theta = 0$), the resulting path cannot be physically followed.

The Quick Version

• Workspace obstacles block line-of-sight paths between start and goal coordinates.
• Dimensionless point assumptions fail because physical robot chassis collide with narrow gaps.
• Configuration space inflates obstacles by robot geometry so the robot becomes a point.
• Occupancy grids discretize continuous space into graph nodes with weighted transition costs.
• A* uses admissible heuristics to guarantee finding the shortest path without overestimation.
• Priority queues focus frontier expansion directly toward the goal without blind flooding.
• Sampling-based planners overcome the curse of dimensionality for multi-joint robotic arms.
• Geometric paths lack timing; kinodynamic trajectories assign feasible velocities and accelerations.
• Spline smoothing and closed-loop feedback controllers track reference trajectories on hardware.
• Dynamic moving obstacles and replanning latency invalidate precomputed static motion plans.