Motion Planning

Problem Formulation

The motion planning problem (piano mover's problem): given a robot with geometry, a workspace with obstacles, a start configuration, and a goal configuration, find a continuous collision-free path from start to goal, or determine that none exists.

The problem is fundamental to robotics, game AI, autonomous vehicles, and computational biology (protein folding).

Complexity: deciding if a path exists for a robot with $d$ degrees of freedom among polyhedral obstacles is PSPACE-complete (Reif, 1979). For fixed $d$, polynomial algorithms exist but with impractical exponents.

Configuration Space

The configuration space (C-space) $\mathcal{C}$ is the space of all possible configurations of the robot. Each point in $\mathcal{C}$ fully specifies the robot's position and orientation.

• Translating robot in 2D: $\mathcal{C} = \mathbb{R}^2$ (position only)
• Translating and rotating rigid body in 2D: $\mathcal{C} = \mathbb{R}^2 \times S^1$ (3 DOF)
• Rigid body in 3D: $\mathcal{C} = \mathbb{R}^3 \times SO(3)$ (6 DOF)
• $n$-link planar arm: $\mathcal{C} = (S^1)^n$ ($n$ DOF, a torus)
• Humanoid robot: $\mathcal{C} \approx \mathbb{R}^{30+}$

C-space obstacle $\mathcal{C}_{\text{obs}}$: the set of configurations where the robot intersects an obstacle. For a translating convex robot $R$ and convex obstacle $O$: $\mathcal{C}_{\text{obs}} = O \oplus (-R)$ (Minkowski sum with the reflected robot).

Free space $\mathcal{C}_{\text{free}} = \mathcal{C} \setminus \mathcal{C}_{\text{obs}}$: the space of collision-free configurations.

Motion planning reduces to finding a path in $\mathcal{C}_{\text{free}}$ from $q_{\text{start}}$ to $q_{\text{goal}}$.

Exact Methods

Visibility Graphs

For a polygonal translating point robot in 2D among polygonal obstacles:

The shortest path lies on the visibility graph: nodes are start, goal, and obstacle vertices; edges connect mutually visible pairs.

Construction: $O(n^2 \log n)$ for $n$ total obstacle vertices (check visibility for all $O(n^2)$ pairs). Can be improved to $O(n^2)$ or output-sensitive $O(n \log n + k)$ where $k$ is the number of visible pairs.

Shortest path: Dijkstra on the visibility graph. Total: $O(n^2 \log n)$.

For non-point convex robots: grow obstacles by the Minkowski sum of the robot, then plan for a point.

Cell Decomposition (Exact)

Decompose $\mathcal{C}_{\text{free}}$ into simple cells and build an adjacency graph.

Vertical decomposition (trapezoidal decomposition): for 2D polygonal obstacles, draw vertical lines from each vertex to the nearest obstacle above and below. Creates $O(n)$ trapezoids. Compute via sweep line in $O(n \log n)$.

Adjacency graph: nodes = cells, edges between adjacent cells. Path planning = graph search (BFS/DFS) from the cell containing $q_{\text{start}}$ to the cell containing $q_{\text{goal}}$.

Cylindrical algebraic decomposition (CAD): exact decomposition for semi-algebraic C-spaces. Doubly exponential in dimension; impractical beyond $d \approx 4$.

Exact Cell Decomposition for Higher DOF

Canny's roadmap algorithm (1988): constructs a 1D roadmap (retract of $\mathcal{C}_{\text{free}}$) in polynomial time for fixed $d$. Theoretical breakthrough but impractical. Complexity: $O(n^d \text{polylog}(n))$ for $n$ obstacle features in $d$ dimensions.

Sampling-Based Methods

Dominant in practice for high-dimensional C-spaces ($d \geq 4$). Trade completeness for practical efficiency.

Probabilistic Roadmap (PRM)

Algorithm (Kavraki, Svestka, Latombe, Overmars, 1996):

Learning phase:

1. Sample $N$ random configurations in $\mathcal{C}$
2. Discard those in $\mathcal{C}_{\text{obs}}$ (collision check)
3. For each free sample, attempt to connect to $k$ nearest neighbors via local planner (straight-line interpolation with collision checking)
4. Store the resulting graph (roadmap)

Query phase: connect $q_{\text{start}}$ and $q_{\text{goal}}$ to the roadmap, then search the graph (A*, Dijkstra).

Properties:

• Probabilistically complete: if a path exists, PRM finds it with probability approaching 1 as $N \to \infty$
• Works well in high-dimensional spaces
• Multi-query: roadmap can be reused for different start/goal pairs
• Struggles with narrow passages: probability of sampling inside a narrow corridor is proportional to its volume, which can be exponentially small

Narrow passage strategies: Gaussian sampling (sample pairs, keep the one in free space near obstacles), bridge test (sample in obstacles, check midpoint), medial axis sampling.

RRT (Rapidly-Exploring Random Tree)

Algorithm (LaValle, 1998):

1. Initialize tree $T$ with $q_{\text{start}}$
2. Repeat: a. Sample random configuration $q_{\text{rand}}$ b. Find nearest node $q_{\text{near}}$ in $T$ c. Extend from $q_{\text{near}}$ toward $q_{\text{rand}}$ by step size $\delta$, yielding $q_{\text{new}}$ d. If the path from $q_{\text{near}}$ to $q_{\text{new}}$ is collision-free, add $q_{\text{new}}$ to $T$
3. If $q_{\text{new}}$ is close to $q_{\text{goal}}$, attempt direct connection

Properties:

• Voronoi bias: larger Voronoi cells (unexplored regions) are more likely to attract samples, driving rapid exploration
• Probabilistically complete
• Single-query (tree is specific to start/goal)
• No asymptotic optimality guarantee (paths can be arbitrarily suboptimal)

Bi-directional RRT (RRT-Connect): grow trees from both $q_{\text{start}}$ and $q_{\text{goal}}$, attempt to connect them. Significantly faster in practice. Kuffner and LaValle (2000).

RRT* (Optimal RRT)

Algorithm (Karaman and Frazzoli, 2011):

Modifies RRT with two key changes:

1. Near-neighbor rewiring: when adding $q_{\text{new}}$, check all nodes within radius $r = \gamma (\log n / n)^{1/d}$ and choose the parent that minimizes cost-to-come
2. Rewire neighbors: check if routing through $q_{\text{new}}$ improves the cost of nearby nodes; if so, rewire

Properties:

• Asymptotically optimal: the path cost converges to the optimal cost as $n \to \infty$ (with probability 1)
• Convergence rate: $O(n^{-1/d})$ for the cost gap, which is slow in high dimensions
• Significantly slower per iteration than RRT due to near-neighbor queries

Variants: Informed RRT* (restricts sampling to an ellipsoidal region after finding an initial path), BIT* (batch informed trees), AIT* (adaptive), LBTRRT, FMT*.

Potential Fields

Artificial potential field method (Khatib, 1986):

Define a potential function $U(q) = U_{\text{att}}(q) + U_{\text{rep}}(q)$:

• Attractive: $U_{\text{att}}(q) = \frac{1}{2}k_a \|q - q_{\text{goal}}\|^2$ (quadratic pull toward goal)
• Repulsive: $U_{\text{rep}}(q) = \frac{1}{2}k_r \left(\frac{1}{d(q)} - \frac{1}{d_0}\right)^2$ if $d(q) \leq d_0$, else 0 (where $d(q)$ is distance to nearest obstacle)

Follow the negative gradient: $\dot{q} = -\nabla U(q)$.

Advantages: simple, reactive, real-time capable. Computes velocities directly.

Fatal flaw: local minima. The gradient can be zero at points other than the goal. Common example: U-shaped obstacle.

Mitigations:

• Navigation functions (Rimon-Koditschek, 1992): potential functions with a unique minimum at the goal. Exist for sphere-worlds; construction for general environments is complex
• Randomized escape: add random perturbation when stuck
• Harmonic potential functions: solutions to Laplace's equation have no local minima (but are expensive to compute)
• In practice, potential fields are combined with global planners (potential field for local obstacle avoidance, RRT for global path finding)

Kinodynamic Planning

Kinodynamic planning considers velocity, acceleration, and dynamic constraints: plan in the state space $(q, \dot{q})$ subject to differential constraints $\dot{x} = f(x, u)$ where $u$ is a control input.

Challenges:

• State space is higher-dimensional (position + velocity)
• Cannot simply interpolate between states (must satisfy dynamics)
• Boundary value problem for local steering is often hard (nonlinear dynamics)

Approaches:

• Kinodynamic RRT: extend using forward simulation with random control inputs. The local planner applies a control for a fixed duration and simulates the dynamics
• State lattice: precompute motion primitives (feasible trajectory segments) and search a lattice graph. Used in autonomous driving (Pivtoraiko, Kelly)
• Trajectory optimization: optimize a trajectory directly (CHOMP, TrajOpt, GPMP2). Treat as a constrained optimization problem with collision avoidance as constraints
• LQR-RRT*: use LQR as the local steering function and cost-to-go metric for RRT*. Provides asymptotic optimality for linear systems

Differential constraints: car-like robots (Dubins/Reeds-Shepp curves provide optimal primitives), quadrotors (differentially flat, enabling trajectory planning in flat output space), manipulators (joint torque limits).

Multi-Robot Motion Planning

Plan collision-free paths for $k$ robots simultaneously. C-space is the product: $\mathcal{C} = \mathcal{C}_1 \times \cdots \times \mathcal{C}_k$, with dimension $\sum d_i$.

Centralized (plan in joint C-space): optimal but exponential in $k$. Impractical for $k > 3-4$.

Decoupled approaches:

• Prioritized planning: plan for robots in priority order; each robot treats previously planned robots as moving obstacles. Fast but incomplete
• Velocity tuning: plan paths independently, then adjust velocities to avoid collisions along the paths

CBS (Conflict-Based Search) (Sharon et al., 2015): optimal multi-agent pathfinding on graphs. Two levels:

• High level: search over conflicts (pairs of agents colliding)
• Low level: plan individual agents with constraints from resolved conflicts
• Optimal and complete; exponential worst case but fast in practice

MAPF (Multi-Agent Path Finding): combinatorial variant on graphs. Extensively studied: makespan/sum-of-costs objectives, various optimality guarantees (ICTS, EPEA*, M*, ECBS).

Navigation Meshes

Represent walkable space as a mesh of convex polygons (typically triangles or convex cells).

Construction:

1. Start with the environment geometry (3D mesh, 2D floor plan)
2. Voxelize and identify walkable surfaces
3. Build a Recast navigation mesh: voxelize $\to$ heightfield $\to$ walkable regions $\to$ contour $\to$ convex polygon mesh

Pathfinding on navmesh:

1. Locate start/goal polygons
2. A* search on the polygon adjacency graph (cost = distance between polygon edges or centroids)
3. String-pulling (funnel algorithm): given the corridor of polygons, find the shortest path through the portal edges. Simple Funnel algorithm (Lee and Preparata, 1984): $O(n)$ for $n$ portals

Advantages: compact representation, supports efficient pathfinding for many agents simultaneously, handles complex 3D environments. Industry standard for game AI (Recast/Detour library by Mikko Mononen).

Dynamic obstacles: carve temporary holes in the navmesh, or use local avoidance (ORCA/RVO) on top of global navmesh paths.

Local Avoidance

ORCA (Optimal Reciprocal Collision Avoidance) (van den Berg et al., 2011): each agent computes a set of velocities guaranteed to be collision-free for a short time horizon (assuming other agents cooperate). Reduces to a linear program per agent per timestep.

RVO (Reciprocal Velocity Obstacles): each agent takes half the responsibility for avoiding each pairwise collision. Produces smooth, oscillation-free motion.

These are used as a local layer on top of global path planning (navmesh A*), handling dynamic obstacles and agent-agent interactions in real time.