Motion Planning

What is Motion Planning?

Motion planning is the problem of finding a feasible (and ideally optimal) path or trajectory for a robot from a start configuration to a goal configuration, avoiding collisions with obstacles and respecting kinematic and dynamic constraints. It is also known as the "piano mover's problem" — coined by Schwartz and Sharir (1983) — because it generalizes to moving any rigid body through a cluttered environment.

Path vs Trajectory

• Path — a geometric curve in configuration space from start to goal. No time parameterization. Answers "where" but not "when" or "how fast."
• Trajectory — a path with a time schedule: q(t) for t in [0, T]. Specifies positions, velocities, and accelerations at each time step. Required for control execution.

Inputs to the Planning Problem

• Robot model — geometry (shape, link lengths), kinematics (joint types, limits), and optionally dynamics (inertia, motor limits).
• Environment model — obstacles represented as geometric primitives (boxes, cylinders, meshes), occupancy grids, or signed distance fields.
• Start configuration — the robot's current joint angles/position.
• Goal specification — a target configuration, a target end-effector pose (with IK), or a goal region.
• Constraints — joint limits, velocity limits, torque limits, obstacle clearance, task constraints (e.g., keep a cup upright).

Computational Complexity

The general motion planning problem is PSPACE-hard (Reif, 1979). For a robot with n DOFs, the configuration space is n-dimensional. The obstacle region in C-space (C-obstacle) has complex geometry that is expensive to compute explicitly. This complexity motivates the use of sampling-based methods that avoid explicit C-obstacle computation.

Configuration Space

The configuration space (C-space), introduced by Tomas Lozano-Perez (1983), is the space of all possible configurations of the robot. Each point in C-space represents a complete specification of the robot's position — typically the vector of joint angles for a manipulator or (x, y, theta) for a planar mobile robot.

C-Space Decomposition

• C-free — the set of configurations where the robot does not collide with any obstacle or itself. This is where the robot can safely exist.
• C-obstacle — the set of configurations where the robot collides with an obstacle. This is the forbidden region.
• C-space boundary — configurations where the robot is in contact with an obstacle (touching but not penetrating).

Examples

Collision Checking

The most computationally expensive part of motion planning. Given a configuration q, determine whether the robot at q intersects any obstacle. Methods:

• Primitive-based — represent robot and obstacles as geometric primitives (spheres, capsules, boxes). GJK algorithm (Gilbert, Johnson, Keerthi, 1988) computes the minimum distance between convex shapes. EPA (Expanding Polytope Algorithm) refines penetration depth.
• Mesh-based — triangulated meshes for complex geometry. FCL (Flexible Collision Library, Pan et al., 2012) uses bounding volume hierarchies (BVH) with OBB or RSS bounding volumes. Used in MoveIt and Drake.
• Signed distance fields — precompute the distance to the nearest obstacle at every point in space. Collision check = lookup the distance value at the robot's geometry. Fast for static environments.

Completeness & Optimality

Different planning algorithms offer different guarantees:

Completeness

• Complete — guaranteed to find a solution if one exists, and report failure if none exists, in finite time. Cell decomposition and visibility graph methods are complete for polygonal environments. Impractical for high-dimensional C-spaces.
• Resolution-complete — complete up to a chosen resolution. Grid-based search is resolution-complete: it will find a path if one exists that passes through grid cells, but may miss paths in narrow passages between grid points.
• Probabilistically complete — the probability of finding a solution (if one exists) approaches 1 as planning time goes to infinity. RRT and PRM are probabilistically complete. In practice, this means they will find a solution given enough time, but cannot definitively report "no solution."

Optimality

• Optimal — finds the shortest/cheapest path. A* with an admissible heuristic is optimal. Dijkstra's algorithm is optimal.
• Asymptotically optimal — the solution converges to the optimal path as the number of samples grows. RRT* (Karaman & Frazzoli, 2011) and PRM* are asymptotically optimal. Basic RRT is NOT optimal.
• Bounded suboptimal — guarantees the solution cost is within a factor of the optimal. Anytime algorithms (ARA*, Anytime RRT*) provide progressively better solutions.

Graph Search Methods

Discretize C-space into a graph (grid, lattice, or visibility graph), then search for the shortest path.

Breadth-First Search (BFS)

Explores all nodes at the current depth before moving to the next depth level. Guaranteed to find the shortest path in an unweighted graph. Time and space complexity: O(V + E). Used for simple grid-based planning when all movements have equal cost.

Dijkstra's Algorithm

Finds the shortest path in a weighted graph. Maintains a priority queue of nodes ordered by distance from the start. Explores the nearest unexplored node at each step. Optimal but explores in all directions uniformly — inefficient when the goal location is known. Time complexity: O((V + E) log V) with a binary heap.

A* Search

The most important search algorithm in robotics. Combines Dijkstra's actual cost g(n) with a heuristic estimate h(n) of the remaining cost to the goal:

A* expands the node with the lowest f(n). If h(n) is admissible (never overestimates the true cost) and consistent (satisfies the triangle inequality), A* is optimal and explores the minimum number of nodes among admissible algorithms. Published by Hart, Nilsson, and Raphael (1968) at SRI International.

Common Heuristics

• Euclidean distance — straight-line distance to goal. Admissible in continuous space.
• Manhattan distance — sum of absolute differences in each dimension. Admissible for 4-connected grids.
• Diagonal distance — accounts for diagonal movement. Admissible for 8-connected grids.
• Dubins/Reeds-Shepp — for car-like robots, the minimum-length path respecting the turning radius. Used in autonomous driving path planning.

Weighted A*

Use f(n) = g(n) + w * h(n) with w > 1. This inflates the heuristic, making the search more greedy (faster) at the cost of optimality. The solution cost is at most w times the optimal cost. In practice, w = 1.5-3 gives a good speed/quality trade-off.

D* and D* Lite

Incremental search algorithms for dynamic environments. When the environment changes (new obstacle detected), D* Lite (Koenig & Likhachev, 2002) efficiently repairs the existing search tree rather than replanning from scratch. Used in Mars rovers (Spirit and Opportunity used a variant of D*) and autonomous vehicles.

Lattice-Based Planning

Discretize the C-space into a state lattice: a regular graph where edges correspond to dynamically feasible motion primitives (precomputed short trajectories that respect the robot's dynamics). Pivonka and Kelly (2005) introduced lattice planners for field robots. Used in the DARPA Urban Challenge (CMU's Boss) and many autonomous driving systems. Graph search (A* or ARA*) on the lattice guarantees feasible, dynamically consistent paths.

Potential Fields

Artificial potential field methods, introduced by Oussama Khatib (1986), treat the robot as a particle in a potential field. The goal generates an attractive potential that pulls the robot toward it; obstacles generate repulsive potentials that push the robot away. The robot follows the negative gradient of the total potential.

Attractive Potential

Typically a quadratic function of distance to the goal: U_att(q) = 0.5 * k_att * ||q - q_goal||^2. The gradient is a constant force toward the goal. For large distances, a conic (linear) potential prevents excessive force.

Repulsive Potential

Active only within a threshold distance d_0 from obstacles: U_rep(q) = 0.5 * k_rep * (1/d(q) - 1/d_0)^2 when d(q) < d_0, and 0 otherwise. The force grows rapidly as the robot approaches an obstacle, creating a repulsive "force field."

Local Minima Problem

The fatal flaw of potential fields: the robot can get trapped in local minima where attractive and repulsive forces balance. Classic example: a U-shaped obstacle between the robot and goal. Solutions:

• Navigation functions (Rimon & Koditschek, 1992) — specially designed potential functions with exactly one minimum (the goal). Guaranteed to work in certain environment classes but hard to construct in general.
• Random walks — add random noise to escape local minima. Not guaranteed to work.
• Hybrid approaches — use potential fields for reactive obstacle avoidance but a global planner (A*, RRT) for overall path planning.

Practical Use

Despite the local minima problem, potential fields are widely used as a reactive layer for real-time obstacle avoidance, especially in mobile robotics. The Vector Field Histogram (VFH, Borenstein & Koren, 1991) and its successor VFH+ are practical implementations used in ROS's navigation stack.

Sampling-Based Planning

Instead of explicitly constructing C-free (computationally intractable for high-DOF robots), sampling-based methods probe C-space by randomly sampling configurations and checking them for collisions. This approach scales to high-dimensional C-spaces where grid-based methods are infeasible.

Why Sampling Works

• Collision checking for a single configuration is fast (milliseconds). Building a complete C-obstacle representation is not.
• Random sampling covers the space uniformly over time (probabilistic completeness).
• No explicit discretization — works in continuous C-spaces of any dimension.
• The curse of dimensionality that kills grid-based methods is manageable: sampling-based methods scale much better to 6+ DOF.

Sampling Strategies

• Uniform random — sample uniformly in C-space. Simple and probabilistically complete but wastes samples in large empty regions.
• Goal-biased — with probability p_goal (typically 0.05-0.1), sample the goal configuration instead of random. Dramatically speeds up convergence.
• Gaussian sampling — sample near obstacle surfaces (where narrow passages are). Sample a pair of points; if one is in collision and the other is free, keep the free one. Boor et al. (1999).
• Bridge sampling — find points in narrow passages by sampling pairs where both are in collision and the midpoint is free. Hsu et al. (2003).
• Informed sampling — after finding an initial solution, restrict sampling to the prolate hyperellipsoid that could improve the solution. Used in Informed RRT* (Gammell et al., 2014).

RRT and Variants

The Rapidly-exploring Random Tree (RRT), introduced by Steven LaValle (1998), is the most influential sampling-based planning algorithm. It incrementally builds a tree rooted at the start configuration, expanding toward random samples to explore C-free.

Basic RRT Algorithm

function RRT(q_start, q_goal, max_iter):
    T.init(q_start)
    for i = 1 to max_iter:
        q_rand = random_config()        // uniform random in C-space
        q_near = nearest(T, q_rand)     // nearest node in tree
        q_new = steer(q_near, q_rand, step_size)  // extend toward q_rand
        if collision_free(q_near, q_new):
            T.add_node(q_new)
            T.add_edge(q_near, q_new)
            if distance(q_new, q_goal) < threshold:
                return extract_path(T, q_start, q_new)
    return FAILURE

Key Properties

• Voronoi bias — nodes with larger Voronoi regions (less explored areas) are more likely to be selected as q_near, causing the tree to grow preferentially toward unexplored space.
• Probabilistically complete — as iterations go to infinity, the probability of finding a path (if one exists) goes to 1.
• Not optimal — the first path found is typically far from the shortest path. The tree's random growth produces jagged, suboptimal paths.
• Fast in high dimensions — scales well to 6-30+ DOF because it only requires point sampling and local collision checking.

RRT-Connect

Kuffner & LaValle (2000) introduced bidirectional RRT: grow two trees, one from start and one from goal, and try to connect them. The CONNECT heuristic extends the tree greedily (multiple steps) toward the other tree's nearest node rather than taking a single step. RRT-Connect is typically 10-100x faster than basic RRT and is the default planner in MoveIt (the ROS manipulation planning framework).

RRT* (Asymptotically Optimal)

Karaman & Frazzoli (IJRR 2011) proved that basic RRT converges to a suboptimal path with probability 1. They introduced RRT*, which adds two operations to make the algorithm asymptotically optimal:

• Near neighbors — instead of connecting q_new to only q_near, consider all nodes within a radius r_n = gamma * (log(n)/n)^{1/d} (where d is the C-space dimension).
• Rewiring — if connecting through q_new gives a shorter path to any near neighbor, update the tree.

RRT* converges to the optimal path as the number of samples increases, but convergence is slow. In practice, it produces significantly better paths than RRT given enough time.

Other RRT Variants

PRM and Roadmaps

The Probabilistic Roadmap Method (PRM), introduced by Kavraki, Svestka, Latombe, and Overmars (1996), takes a different approach: build a reusable graph (roadmap) of C-free in a preprocessing phase, then query it for any start-goal pair.

Two-Phase Approach

Construction phase (offline):

• Sample N random configurations in C-space.
• For each sample, check if it is collision-free. Discard colliding samples.
• For each free sample, attempt to connect it to its k nearest neighbors with straight-line paths. Check each edge for collisions.
• Result: a graph (roadmap) embedded in C-free.

Query phase (online):

• Connect the start and goal configurations to the nearest roadmap nodes.
• Search the roadmap graph (Dijkstra or A*) for the shortest path.
• Multiple queries can reuse the same roadmap — amortizing construction cost.

PRM Strengths and Weaknesses

• Strength: Multi-query — build once, query many times. Ideal for fixed environments with repeated planning (industrial workcells).
• Strength: Captures the connectivity of C-free — can detect if start and goal are in different connected components.
• Weakness: Narrow passages — if samples don't fall in a narrow corridor, the roadmap won't contain paths through it. Addressed by Gaussian and bridge sampling strategies.
• Weakness: Construction cost — building a dense roadmap in high dimensions is expensive. Not suitable for single-query problems or dynamic environments.

Visibility PRM

Simeon et al. (2000) proposed keeping only "guard" and "connector" nodes: a new sample becomes a guard if it sees no existing guards, or a connector if it connects two guards. This produces a much sparser roadmap with the same coverage.

Lazy PRM

Bohlin & Kavraki (2000): delay collision checking of edges until they are needed for a query. Assume all edges are valid, find the shortest path, then check edges along that path for collisions. If a collision is found, remove the edge and replan. Saves enormous computation when most edges are never used.

Trajectory Optimization

Instead of searching through discrete samples or graph nodes, trajectory optimization treats motion planning as a continuous optimization problem: find the trajectory q(t) that minimizes a cost functional subject to constraints.

General Formulation

CHOMP (Covariant Hamiltonian Optimization for Motion Planning)

Ratliff et al. (2009, CMU) proposed CHOMP: represent the trajectory as a sequence of waypoints and optimize using functional gradient descent. The cost combines a smoothness term (penalizes acceleration) and an obstacle cost (based on signed distance to obstacles). CHOMP uses precomputed signed distance fields for fast gradient computation. Available in MoveIt.

STOMP (Stochastic Trajectory Optimization for Motion Planning)

Kalakrishnan et al. (2011, USC) introduced STOMP: generate noisy trajectory samples around the current trajectory, evaluate their costs, and update the trajectory as a cost-weighted average. Unlike CHOMP, STOMP does not require gradient information — it works with arbitrary cost functions (useful when the cost is non-differentiable). Also available in MoveIt.

TrajOpt

Schulman et al. (2014, UC Berkeley) formulated motion planning as sequential convex optimization. Obstacle avoidance is a constraint (not a cost term), using signed distances and their linear approximations. Supports continuous-time collision checking (swept volumes). Fast convergence. Used in planning for manipulation and autonomous driving.

Direct Collocation and Direct Transcription

Transcribe the continuous-time optimal control problem into a finite-dimensional nonlinear program (NLP) by discretizing the trajectory into N time steps. Decision variables: states and controls at each knot point. Dynamics are enforced as equality constraints (collocation = using polynomial interpolation between knot points). Solved with NLP solvers (IPOPT, SNOPT). This is the standard approach in Drake (MIT) for kinodynamic planning.

Trajectory Optimization vs Sampling-Based Planning

In practice, the best systems combine both: use RRT/PRM for an initial feasible path, then refine with trajectory optimization for smoothness and dynamic feasibility.

Real-Time Replanning

Static planning assumes a known, fixed environment. Real robots operate in dynamic environments where obstacles move, new obstacles appear, and the environment is only partially known. Real-time replanning adapts the plan as new information arrives.

Approaches

• Replan from scratch — when the environment changes, discard the old plan and plan anew. Simple but wasteful. Only works if the planner is fast enough (< 100ms for reactive behavior).
• Incremental search — repair the existing plan rather than rebuilding. D* Lite (Koenig & Likhachev, 2002) efficiently updates a graph search when edge costs change. Used when obstacle information updates incrementally.
• Anytime planning — find an initial feasible solution quickly, then improve it over time. ARA* (Likhachev et al., 2003) starts with an inflated heuristic for speed, then reduces inflation for quality. Returns the best solution found within the time budget.
• Elastic bands / elastic strips — maintain a path that deforms in real-time to avoid moving obstacles. TEB (Timed Elastic Band) is used in ROS's local planner for mobile robots. Quinlan & Khatib (1993) introduced elastic strips for manipulators.
• MPPI (Model Predictive Path Integral) — a sampling-based MPC approach. At each step, sample many trajectory rollouts, weight them by cost, and compute the weighted average as the optimal action. GPU-parallelizable. Used in aggressive autonomous driving (Williams et al., 2017, Georgia Tech) and legged locomotion.

Dynamic Obstacle Handling

• Velocity obstacles (Fiorini & Shiller, 1998) — represent moving obstacles in velocity space. The set of robot velocities that would lead to collision within a time horizon. ORCA (Optimal Reciprocal Collision Avoidance, van den Berg et al., 2011) extends this for multi-robot systems.
• Predicted trajectories — estimate future obstacle positions using constant-velocity, constant-acceleration, or learned motion models. Plan in space-time (x, y, t) to avoid predicted positions.
• Safety guarantees — Hamilton-Jacobi reachability analysis computes the set of states from which the robot can always avoid collision, regardless of obstacle behavior. Conservative but provably safe.

Planning Frequency Hierarchy

• Global planner (1-10 Hz) — plans the overall path from start to goal. A*, PRM, or RRT on a global map.
• Local planner (10-50 Hz) — follows the global path while reacting to local obstacles. DWA (Dynamic Window Approach), TEB, or potential fields.
• Reactive controller (100-1000 Hz) — low-level obstacle avoidance reflex. Emergency stop, potential fields, or simple distance thresholds.

Applications

References & Further Reading