Motion Planning & Trajectory Optimization

Path Planning

Path planning finds collision-free paths from a start to a goal in a given environment, a core problem in robot navigation and autonomous driving.

Graph Search Methods

A* Algorithm

A* is the most classic path planning algorithm, combining Dijkstra's optimality with greedy search efficiency:

\[f(n) = g(n) + h(n)\]

\(g(n)\): Actual cost from start to node \(n\)
\(h(n)\): Heuristic estimate from node \(n\) to goal
When \(h\) is admissible, A* guarantees finding the optimal path

Common heuristics: Euclidean distance, Manhattan distance, diagonal distance.

D and D Lite

Incremental search algorithms for dynamic environments:

D*: Efficient replanning when the environment changes (searches from goal to start)
D* Lite: Simplified version based on LPA (Lifelong Planning A)
Widely used in Mars rovers and mobile robot navigation

Jump Point Search (JPS)

Dramatically accelerates A* on uniform grids:

Exploits grid symmetry to skip unnecessary node expansions
Maintains A*'s optimality guarantee
Achieves order-of-magnitude speedup on large grid maps

Sampling-Based Methods

RRT (Rapidly-Exploring Random Tree)

Suitable for path planning in high-dimensional continuous spaces:

Randomly sample point \(q_{\text{rand}}\) in space
Find nearest node \(q_{\text{near}}\) in the tree
Extend a fixed step toward \(q_{\text{rand}}\) to get \(q_{\text{new}}\)
Add to tree if the path is collision-free

RRT*: Asymptotically optimal variant of RRT that uses rewiring to converge to optimal paths as sample count increases.

PRM (Probabilistic Roadmap)

Suitable for multi-query scenarios:

Construction phase: Randomly sample nodes in free space, connect feasible edges
Query phase: Connect start and goal to the roadmap, find path via graph search

PRM is better suited for static environments requiring multiple path queries.

Commonly used in game AI and indoor navigation:

Decompose walkable areas into convex polygon meshes
Establish adjacency relations between polygons
Path search operates on the polygon graph (coarse search + funnel algorithm refinement)

Pros: Small search space, natural paths, supports agents of different sizes.

Potential Field Method

Model the goal as an attractive source and obstacles as repulsive sources:

\[\mathbf{F}(\mathbf{q}) = \mathbf{F}_{\text{att}}(\mathbf{q}) + \mathbf{F}_{\text{rep}}(\mathbf{q})\]

Pros: Computationally simple, good real-time performance
Cons: Local minima problem — agents may get trapped at non-goal equilibrium points
Solutions: Random perturbation, virtual obstacles, combining with global planners

Trajectory Planning

Trajectory planning adds a time dimension on top of path planning, generating smooth trajectories that satisfy kinematic and temporal constraints.

Path vs. Trajectory

	Path	Trajectory
Time	No time information	Includes time parameter \(\mathbf{q}(t)\)
Output	Geometric curve in space	Position-velocity-acceleration as functions of time
Constraints	Geometric constraints (collision avoidance)	Kinematic/dynamic constraints (velocity, acceleration limits)

Spline Interpolation

Cubic Spline

Interpolates between waypoints using cubic polynomials, ensuring \(C^2\) continuity:

\[\mathbf{q}(t) = a_0 + a_1 t + a_2 t^2 + a_3 t^3\]

B-Spline

Provides better local control — modifying one control point only affects the local curve segment.

Bezier Curve

An \(n\)-th degree Bezier curve is defined by \(n+1\) control points:

\[\mathbf{B}(t) = \sum_{i=0}^{n} \binom{n}{i} (1-t)^{n-i} t^i \mathbf{P}_i, \quad t \in [0, 1]\]

Trajectory Optimization

Formulate trajectory generation as an optimization problem:

\[\min_{\mathbf{q}(t)} \int_0^T L(\mathbf{q}, \dot{\mathbf{q}}, \ddot{\mathbf{q}}) \, dt\]

Common objectives:

Minimum snap (4th derivative): \(\min \int \|\dddot{\mathbf{q}}(t)\|^2 dt\) — generates smooth trajectories (commonly used for drones)
Minimum jerk: \(\min \int \|\ddot{\mathbf{q}}(t)\|^2 dt\) — suitable for robotic arms
Minimum time: Minimizes arrival time subject to motion constraints

QP-Based Polynomial Trajectory Optimization

Parameterize trajectories as piecewise polynomials, converting the objective and constraints into a quadratic programming (QP) problem:

\[\min_{\mathbf{c}} \mathbf{c}^T \mathbf{Q} \mathbf{c} \quad \text{s.t.} \quad \mathbf{A}\mathbf{c} = \mathbf{b}\]

where \(\mathbf{c}\) is the polynomial coefficient vector. This is the standard approach in drone trajectory planning.

Time Allocation

Given waypoints, the traversal time for each segment must be determined:

Uniform allocation: Simple but suboptimal
Trapezoidal velocity profile: Acceleration-constant speed-deceleration
S-curve velocity profile: Bounded jerk for smoother motion
Time-optimal allocation: Optimal timing under velocity/acceleration constraints

Dubins Paths and Reeds-Shepp Paths

Shortest paths under kinematic constraints:

Dubins paths: For vehicles that can only move forward with a minimum turning radius — shortest paths consist of straight lines (S) and arcs (L/R), yielding 6 path types
Reeds-Shepp paths: Allow both forward and reverse motion — 46 possible path types

These form the foundation of low-level trajectory generation in autonomous driving.

Motion Planning & Dynamics

Motion planning finds feasible trajectories satisfying dynamics constraints in configuration space, a core problem in robotics.

Configuration Space (C-Space)

Represents all possible robot poses in a high-dimensional space:

Configuration \(\mathbf{q}\): Minimal parameter set fully describing the robot's pose
- Planar mobile robot: \(\mathbf{q} = (x, y, \theta)\), 3D
- 6-DOF manipulator: \(\mathbf{q} = (\theta_1, \ldots, \theta_6)\), 6D
- Humanoid robot: 30+ dimensions
C-free: Set of collision-free configurations
C-obstacle: Set of configurations in collision with obstacles

Motion planning essentially seeks continuous paths in C-free.

Kinematic Constraints

Holonomic Constraints

Constraints involving only configuration (position), not velocity:

\[f(\mathbf{q}) = 0\]

Example: Joint angle limits of manipulators. Systems with holonomic constraints can move instantaneously in any direction.

Nonholonomic Constraints

Constraints involving velocity that cannot be integrated into pure position constraints:

\[g(\mathbf{q}, \dot{\mathbf{q}}) = 0\]

Example: Vehicles cannot move laterally (\(\dot{y} = v \sin\theta\)). Nonholonomic constraints make planning significantly harder — the reachable space has higher dimensionality than the configuration space.

Dynamics Constraints

Accounting for forces and mass:

\[\mathbf{M}(\mathbf{q})\ddot{\mathbf{q}} + \mathbf{C}(\mathbf{q}, \dot{\mathbf{q}})\dot{\mathbf{q}} + \mathbf{g}(\mathbf{q}) = \boldsymbol{\tau}\]

\(\mathbf{M}\): Inertia matrix
\(\mathbf{C}\): Coriolis and centrifugal forces
\(\mathbf{g}\): Gravity term
\(\boldsymbol{\tau}\): Applied torques

Dynamics constraints extend the planning space from configuration space to state space \((\mathbf{q}, \dot{\mathbf{q}})\).

Sampling-Based Motion Planning

Kinodynamic RRT

Integrates dynamics constraints into RRT:

Samples states as \((\mathbf{q}, \dot{\mathbf{q}})\)
Extends via forward simulation applying control inputs
Uses local controllers to connect nearby nodes

SST (Stable Sparse RRT)

Asymptotically near-optimal kinodynamic planning algorithm
Maintains tree efficiency through sparsification and pruning

CHOMP and TrajOpt

CHOMP (Covariant Hamiltonian Optimization for Motion Planning):

Models trajectories as functions, optimizing smoothness + collision cost
Uses covariant gradient descent

TrajOpt (Trajectory Optimization):

Uses Sequential Convex Programming
Linearizes collision constraints and solves iteratively

Multi-Robot Motion Planning

Joint motion planning for multiple robots has dimensionality that is multiples of single-robot planning:

Coupled methods: Plan in the joint configuration space (dimension explosion)
Decoupled methods: Plan separately then coordinate (may have no solution)
Priority-based methods: Plan sequentially by priority; later robots avoid earlier ones
CBS (Conflict-Based Search): Plan independently first, then branch-and-resolve on conflicts

Planning-Control Bridge

The bridge between planning and control is the critical link that translates high-level decisions into executable actions. In real systems, planning and control are not isolated but must work in tight coordination.

Hierarchical Planning-Control Architecture

Typical autonomous systems adopt a layered architecture:

Layer	Function	Frequency	Example
Task Planning	High-level goal decomposition	~0.1 Hz	"Go from A to B to pick up package"
Behavior Decision	Select behavioral mode	~1 Hz	Lane change, car following, overtaking
Motion Planning	Generate reference trajectory	~10 Hz	Local trajectory optimization
Control Execution	Track reference trajectory	~100 Hz	PID, MPC

Each layer operates at a different time scale: higher layers provide references, lower layers ensure execution.

Model Predictive Control (MPC)

MPC is one of the most important methods bridging planning and control — essentially a receding-horizon planner:

\[\min_{\mathbf{u}_0, \ldots, \mathbf{u}_{N-1}} \sum_{k=0}^{N-1} \ell(\mathbf{x}_k, \mathbf{u}_k) + V_f(\mathbf{x}_N)\]

\[\text{s.t.} \quad \mathbf{x}_{k+1} = f(\mathbf{x}_k, \mathbf{u}_k), \quad \mathbf{x}_k \in \mathcal{X}, \quad \mathbf{u}_k \in \mathcal{U}\]

Core idea:

At each time step, solve a finite-horizon optimization problem
Execute only the first control input
Obtain new state feedback and re-solve

Advantages:

Naturally handles constraints (state constraints, input constraints)
Combines planning (foresight) with control (feedback)
Provides some robustness to model errors and disturbances

Computational challenge: Must solve the optimization problem within the control period — demanding real-time performance.

Linear MPC vs. Nonlinear MPC

	Linear MPC	Nonlinear MPC
Model	Linear: \(\mathbf{x}_{k+1} = A\mathbf{x}_k + B\mathbf{u}_k\)	Nonlinear: \(\mathbf{x}_{k+1} = f(\mathbf{x}_k, \mathbf{u}_k)\)
Optimization	QP (efficiently solvable)	NLP (computationally expensive)
Application	Locally linearized scenarios	Large-range nonlinear motion

Real-Time Replanning

Real environments are dynamic, and planning must be able to adapt quickly:

Conditions triggering replanning:

Obstacle state changes (new obstacles detected)
Current trajectory becomes infeasible (excessive deviation)
Goal changes
Environment prediction updates

Real-time strategies:

Anytime algorithms: Can return the current best solution at any time; longer computation yields better solutions (e.g., ARA*)
Incremental methods: Reuse previous planning results, only update changed parts (e.g., D* Lite)
Parallel planning: Plan a new trajectory in the background while executing the current one

Integrating Planning and Learning

Modern systems increasingly incorporate learning methods into the planning-control framework:

Learning-based MPC: Neural networks learn cost functions or constraints for MPC
Learning-based path planning: Deep learning predicts heuristic functions or directly outputs paths
Diffusion model planning: Models planning as denoising trajectories from noise (Diffuser)
End-to-end methods: From perception directly to control, bypassing explicit planning (but poor interpretability)

Key challenge: Balancing the flexibility of learning with the safety guarantees of planning.