Path Planning

How does a robot move from point A to point B without hitting obstacles? Path planning is one of the most practical problems in robotics, and it draws on nearly every linear algebra concept we’ve covered: vectors for direction computation, matrices for transforms between frames, gradients (derivatives of scalar fields) for descent directions, and eigenvalues for analyzing the conditioning of trajectories.

Configuration Space

Before planning a path, we need to define the space we’re planning in.

The workspace is the physical space the robot occupies (typically 2D or 3D). The configuration space (C-space) is the space of all possible joint configurations $\mathbf{q} = [q_1, q_2, \ldots, q_n]^T$ .

For a 2-link planar arm, C-space is the torus $(\theta_1, \theta_2) \in [-\pi, \pi) \times [-\pi, \pi)$ . For a mobile robot in the plane, C-space is $(x, y, \theta)$ .

Why Plan in C-Space?

In workspace, a robot arm is a complex shape that changes with configuration. In C-space, the robot is a single point — and obstacles become “inflated” regions called C-space obstacles. Path planning reduces to finding a path for a point that avoids these regions. This transformation simplifies the geometry enormously.

An obstacle in workspace maps to a C-space obstacle: the set of all configurations where the robot would collide with the obstacle. Computing C-space obstacles exactly is expensive for articulated arms, but for mobile robots it often reduces to Minkowski sums — another linear algebra concept.

Potential Field Methods

The artificial potential field approach treats the robot as a particle moving through a force field:

The goal creates an attractive potential that pulls the robot toward it
Each obstacle creates a repulsive potential that pushes the robot away
The robot follows the negative gradient (steepest descent) of the total potential

This is gradient descent applied to navigation — the same mathematical principle used in optimization and machine learning.

Attractive Potential

The simplest choice is a quadratic potential centered at the goal $\mathbf{q}_g$ :

U_{\text{att}}(\mathbf{q}) = \frac{1}{2} \xi \|\mathbf{q} - \mathbf{q}_g\|^2

where $\xi > 0$ is the attractive gain. The gradient is:

\nabla U_{\text{att}} = \xi (\mathbf{q} - \mathbf{q}_g)

This always points away from the goal, so the negative gradient $-\nabla U_{\text{att}}$ points toward it.

Linear Algebra Connection

The gradient $\nabla U$ is a vector that points in the direction of steepest increase. For the quadratic potential, the gradient is a linear function of position — it’s a matrix-vector multiplication (here, just $\xi I$ times the displacement vector). The negative gradient gives the steepest-descent direction, which is the basis for many optimization algorithms.

Repulsive Potential

Each obstacle has a repulsive field that activates only within a radius $\rho_0$ :

U_{\text{rep}}(\mathbf{q}) = \begin{cases} \frac{1}{2} \eta \left(\frac{1}{\rho(\mathbf{q})} - \frac{1}{\rho_0}\right)^2 & \text{if } \rho(\mathbf{q}) \leq \rho_0 \\ 0 & \text{if } \rho(\mathbf{q}) > \rho_0 \end{cases}

where $\rho(\mathbf{q})$ is the distance from $\mathbf{q}$ to the obstacle surface, $\eta > 0$ is the repulsive gain, and $\rho_0$ is the influence radius.

The gradient $\nabla U_{\text{rep}}$ points toward the obstacle (the direction of steepest increase in repulsive potential). The negative gradient gives the repulsive force that pushes the robot away:

\mathbf{F}_{\text{rep}} = -\nabla U_{\text{rep}} = \eta \left(\frac{1}{\rho} - \frac{1}{\rho_0}\right) \frac{1}{\rho^2} \hat{\mathbf{n}}

where $\hat{\mathbf{n}}$ is the unit vector pointing from the obstacle toward the robot.

Total Potential and Gradient Descent

The total potential is the sum of attractive and repulsive terms:

U(\mathbf{q}) = U_{\text{att}}(\mathbf{q}) + \sum_{i} U_{\text{rep},i}(\mathbf{q})

The path is generated by gradient descent:

\mathbf{q}_{k+1} = \mathbf{q}_k - \alpha \nabla U(\mathbf{q}_k)

where $\alpha$ is the step size. At each step, the robot moves in the direction of the total “force” — attracted to the goal and repelled by obstacles.

Gradient Descent Navigation Robotics Application

A mobile robot at $\mathbf{q} = (0, 0)$ must reach $\mathbf{q}_g = (5, 3)$ while avoiding an obstacle at $(2, 1)$ with radius 0.5 and influence radius $\rho_0 = 2$ .

At the robot’s current position:

Attractive force: $-\nabla U_{\text{att}} = -\xi(0 - 5, 0 - 3) = \xi(5, 3)$ . Points toward goal.
Distance to obstacle surface: $\rho = \sqrt{4 + 1} - 0.5 \approx 1.74 < \rho_0$ . Repulsive field is active.
Repulsive force pushes in direction $(-2, -1)/\sqrt{5}$ (away from obstacle).

The total force (negative gradient) is a weighted sum of vectors — one pulling toward the goal and one pushing from the obstacle. The robot moves in this combined direction.

Local Minima: The Fundamental Limitation

The critical weakness of potential fields is the possibility of local minima — points where $\nabla U = 0$ but $\mathbf{q} \neq \mathbf{q}_g$ . This happens when the attractive and repulsive forces exactly cancel.

Local Minima

Local minima are configurations where the robot gets “stuck” — the gradient is zero, so gradient descent stops, but the robot hasn’t reached the goal. Common scenarios:

Obstacle directly between start and goal
Concave obstacle arrangements that create “traps”
Symmetric configurations where forces cancel

A related problem is oscillation in narrow passages: when a robot passes between two nearby obstacles, the opposing repulsive forces can cause the path to oscillate back and forth, leading to slow convergence or jittery motion.

Solutions include: randomized perturbations, navigation functions (special potential fields guaranteed to have no local minima), or switching to graph-based planners (like A* or RRT) which are complete but slower.

Trajectory Interpolation

Once a path is planned (as a sequence of waypoints), we need to convert it into a smooth trajectory — a time-parameterized path that specifies position, velocity, and acceleration at each instant.

Joint-Space Interpolation

The simplest approach: interpolate each joint angle independently.

Linear interpolation between configurations $\mathbf{q}_0$ and $\mathbf{q}_f$ :

\mathbf{q}(t) = \mathbf{q}_0 + \frac{t}{T}(\mathbf{q}_f - \mathbf{q}_0), \quad t \in [0, T]

This is a vector lerp — the same linear interpolation from Module 1, applied to joint vectors. It gives a straight line in C-space with constant joint velocities, but the velocity is discontinuous at the endpoints (jumps from 0 to a finite value).

Cubic polynomial for smooth start and stop:

\mathbf{q}(t) = \mathbf{q}_0 + 3\frac{(\mathbf{q}_f - \mathbf{q}_0)}{T^2} t^2 - 2\frac{(\mathbf{q}_f - \mathbf{q}_0)}{T^3} t^3

This satisfies $\mathbf{q}(0) = \mathbf{q}_0$ , $\mathbf{q}(T) = \mathbf{q}_f$ , $\dot{\mathbf{q}}(0) = \dot{\mathbf{q}}(T) = 0$ — the robot starts and stops smoothly.

Task-Space vs. Joint-Space

Joint-space interpolation: Interpolate joint angles directly. Simple, avoids singularities, but the end-effector path in workspace may be curved or unintuitive.
Task-space interpolation: Interpolate the end-effector position (and orientation) in Cartesian space. Gives straight-line paths in workspace but requires solving inverse kinematics at each step and may encounter singularities.

Joint-Space vs. Task-Space Robotics Application

A 2R arm moves from configuration A to configuration B:

In joint space: $\theta_1$ and $\theta_2$ change linearly. The end-effector traces a curved path (generally not a straight line).
In task space: The end-effector moves in a straight line from $(x_A, y_A)$ to $(x_B, y_B)$ . But the joint angles may change non-monotonically, and the path may pass through a singularity.

In practice, many industrial applications use task-space paths for the end-effector (straight-line tool motions) and fall back to joint-space when singularities are encountered.

Orientation Interpolation

For 3D robots, we also need to interpolate orientation. This is where quaternion SLERP from Module 4 becomes essential:

\mathbf{q}(t) = \text{SLERP}(\mathbf{q}_0, \mathbf{q}_f, t/T)

SLERP provides the shortest, constant-angular-velocity rotation path between two orientations — much better than interpolating Euler angles (which can cause gimbal lock or erratic motions).

Combining It All: The Planning Pipeline

A complete motion planning system for a robot arm typically follows this pipeline:

Task specification: Desired end-effector pose (from a higher-level planner or user)
Inverse kinematics: Convert desired pose to a goal configuration in C-space
Path planning: Find a collision-free path in C-space (potential fields, RRT, A*, etc.)
Trajectory generation: Time-parameterize the path with smooth velocity profiles
Velocity-level control: Use the Jacobian for real-time tracking (resolved-rate control from Lesson 2)

Every step involves linear algebra: transforms for step 1, matrix equations for step 2, gradients for step 3, polynomial vectors for step 4, and the Jacobian for step 5.

Beyond Potential Fields: Graph-Based Planners

Potential fields are fast and reactive, but they lack completeness — they can get stuck in local minima. Graph-based planners offer stronger guarantees. A* (on a discretized grid) is complete — it will always find a path if one exists. RRT (Rapidly-exploring Random Trees) and PRM (Probabilistic Roadmaps) sample configurations in continuous C-space and are probabilistically complete: the probability of finding a path approaches 1 as the number of samples increases. The tradeoff is computational cost — these planners are slower than potential fields but more reliable. In practice, many systems combine both: a graph-based planner for the global path and a potential field for local obstacle avoidance.

Interactive Visualization

Explore potential field path planning. The green circle is the start, the gold star is the goal, and red circles are obstacles. The blue arrows show the gradient field (negative gradient = descent direction). Adjust the potential field parameters and obstacle configuration to see how the planned path changes.

Controls

Goal Position

Goal x = 3.5

-44

Goal y = 3.0

-44

Potential Field Gains

ξ (attractive) = 1.0

0.13

η (repulsive) = 2.0

0.15

ρ₀ (influence range) = 1.5

0.33

Obstacles (3)

Show gradient field

Show planned path

Show potential heatmap

Path Status

Path length: 99 steps

Path may be trapped in local minimum

Try this: Move the goal around and watch the gradient field reorient. Add obstacles and observe the repulsive field deflecting the path. Increase η to make obstacles more repulsive. Try creating a configuration where the path gets stuck in a local minimum — the fundamental limitation of potential fields.

Practice Problems

Compute the attractive gradient $\nabla U_{\text{att}}$ at $\mathbf{q} = (2, 3)$ for a goal at $\mathbf{q}_g = (5, 1)$ with $\xi = 1.5$ .
A circular obstacle at $(0, 0)$ with radius 0.5 and $\rho_0 = 2$ exerts a repulsive force on a robot at $(1.5, 0)$ . What is $\rho$ (distance to obstacle surface)? Is the repulsive field active?
Describe a configuration of obstacles that would create a local minimum for a potential field planner. Where would the robot get stuck?
A 2R arm interpolates from $\mathbf{q}_0 = (0°, 90°)$ to $\mathbf{q}_f = (90°, 0°)$ using linear interpolation over $T = 2$ seconds. What is $\mathbf{q}(1)$ ? What is the joint velocity $\dot{\mathbf{q}}$ ?
Why is quaternion SLERP preferred over Euler angle interpolation for orientation trajectories?

Answers

$\nabla U_{\text{att}} = 1.5 \cdot (2 - 5, 3 - 1) = 1.5 \cdot (-3, 2) = (-4.5, 3.0)$ . The negative gradient $-\nabla U_{\text{att}} = (4.5, -3.0)$ points toward the goal.
Distance to center: $\|(1.5, 0) - (0, 0)\| = 1.5$ . Distance to surface: $\rho = 1.5 - 0.5 = 1.0$ . Since $\rho = 1.0 < \rho_0 = 2.0$ , the repulsive field is active.
A U-shaped obstacle arrangement directly between start and goal creates a local minimum in the concavity. The attractive force pulls the robot into the “U” while repulsive forces from the walls cancel the forward component. The robot gets stuck at the bottom of the “U” where all forces balance. Another simple case: a single obstacle exactly on the line between start and goal, with symmetric approach.
$\mathbf{q}(1) = (0, 90°) + \frac{1}{2}(90° - 0°, 0° - 90°) = (45°, 45°)$ . The joint velocity is constant: $\dot{\mathbf{q}} = \frac{1}{2}(90°, -90°) = (45°, -45°)$ per second.
Euler angle interpolation can: (a) pass through gimbal lock singularities, (b) take the “long way around” (non-shortest path), (c) produce varying angular velocity. SLERP on quaternions guarantees the shortest great-circle path on SO(3), maintains constant angular velocity, and has no singularities. It’s the unique minimum-torque rotation path between two orientations.

Key Takeaways

Configuration space reduces a complex robot to a point, simplifying collision checking
Potential fields use attractive + repulsive forces; the path follows the negative gradient
The gradient $\nabla U$ is a vector computed from partial derivatives — linear algebra at work
Local minima are the fundamental limitation of potential fields
Trajectory interpolation converts waypoints to smooth, time-parameterized paths
Joint-space interpolation is simple and singularity-free; task-space gives intuitive Cartesian paths
The complete planning pipeline — from task specification to velocity control — uses linear algebra at every step

Course Summary

Congratulations — you’ve completed all five modules. Here’s what you’ve built, from foundations to applications:

Module	Core Concept	Robotics Application
1. Foundations	Vectors, dot/cross products	Direction, distance, torque
2. Matrices	Multiplication, determinants, inverses	Systems of equations, transforms
3. Transformations	Rotation matrices, homogeneous coordinates	Pose representation, frame chains
4. Advanced	Eigenvalues, frames, quaternions	Stability, sensor fusion, smooth interpolation
5. Applications	Kinematics, Jacobian, planning	Robot control, dexterity, navigation

Every concept in this course feeds into the next. Vectors compose into matrices. Matrices compose into transforms. Transforms chain into kinematics. Kinematics differentiate into the Jacobian. And the Jacobian drives real-time control and planning. Linear algebra is the language that makes it all precise, composable, and computable.