Robot Motion Planning

Overview

The goal of motion planning is to find a collision-free path or trajectory for a robot from a start configuration to a goal configuration. Motion planning is the bridge connecting perception and control, and is a core capability for achieving autonomous manipulation.

graph LR
    A["Perception/Task Description"] --> B["Configuration Space Modeling"]
    B --> C["Path Planning"]
    C --> D["Trajectory Optimization"]
    D --> E["Control Execution"]
    style A fill:#e8f4fd,stroke:#2196F3
    style C fill:#e1f5fe,stroke:#03A9F4
    style D fill:#fff3e0,stroke:#FF9800
    style E fill:#f3e5f5,stroke:#9C27B0

1 Configuration Space (C-space)

1.1 Definition

Configuration space \(\mathcal{C}\) is the set of all possible configurations of the robot. For an \(n\)-DOF robot, the configuration space is an \(n\)-dimensional space.

For a planar mobile robot: \(\mathcal{C} = \mathbb{R}^2\) or \(SE(2)\)
For an \(n\)-DOF manipulator: \(\mathcal{C} = \mathbb{T}^n\) (\(n\)-dimensional torus, each joint angle \(\in [0, 2\pi)\))
For a free-flying rigid body: \(\mathcal{C} = SE(3) = \mathbb{R}^3 \times SO(3)\)

1.2 Obstacle Mapping

Workspace obstacles \(\mathcal{O}_{\text{work}}\) are mapped to the configuration space to form C-space obstacles \(\mathcal{C}_{\text{obs}}\):

\[ \mathcal{C}_{\text{obs}} = \{q \in \mathcal{C} \mid \mathcal{A}(q) \cap \mathcal{O}_{\text{work}} \neq \emptyset\} \]

where \(\mathcal{A}(q)\) is the spatial region occupied by the robot in configuration \(q\).

Free space:

\[ \mathcal{C}_{\text{free}} = \mathcal{C} \setminus \mathcal{C}_{\text{obs}} \]

Challenges of High-Dimensional C-space

For high-DOF robots, explicitly computing \(\mathcal{C}_{\text{obs}}\) is infeasible. In practice, collision checking is used for implicit evaluation.

1.3 Collision Detection

Collision detection is the most time-consuming operation in planning. Common methods:

GJK Algorithm: Distance computation between convex bodies
FCL (Flexible Collision Library): Supports multiple geometry types
Bounding Volume Hierarchies (BVH): AABB, OBB, used for acceleration

2 Sampling-Based Planning

2.1 RRT (Rapidly-exploring Random Tree)

RRT is the most classic sampling-based planning algorithm (LaValle, 1998).

Core Idea: Starting from the initial configuration, gradually expand a search tree through random sampling until the tree reaches the goal region.

Algorithm Pseudocode:

RRT(q_start, q_goal, K):
    T.init(q_start)
    for k = 1 to K:
        q_rand ← RANDOM_CONFIG()
        q_near ← NEAREST(T, q_rand)
        q_new  ← STEER(q_near, q_rand, Δq)
        if COLLISION_FREE(q_near, q_new):
            T.add_vertex(q_new)
            T.add_edge(q_near, q_new)
            if ||q_new - q_goal|| < ε:
                return PATH(T, q_start, q_new)
    return FAILURE

Key Operations:

Operation	Description
`RANDOM_CONFIG()`	Uniformly sample in \(\mathcal{C}\); with probability \(p_{\text{goal}}\), sample \(q_{\text{goal}}\) directly (goal bias)
`NEAREST(T, q)`	Find the nearest node in the tree to \(q\)
`STEER(q_1, q_2, \Delta q)`	Advance from \(q_1\) toward \(q_2\) by step size \(\Delta q\)
`COLLISION_FREE`	Check whether the path segment is collision-free

Properties:

Probabilistically Complete: If a solution exists, the probability of finding it approaches 1 as \(K \to \infty\)
Not Optimal: The found path is typically not the shortest

2.2 RRT* (Asymptotically Optimal RRT)

RRT (Karaman & Frazzoli, 2011) adds two operations to RRT, making it asymptotically optimal*:

Improvement 1: Choose Parent (Select Optimal Parent Node)

Search for the optimal parent node within the neighborhood \(r_n\) of \(q_{\text{new}}\):

\[ q_{\text{parent}}^* = \arg\min_{q \in \text{Near}(T, q_{\text{new}}, r_n)} \left[ \text{Cost}(q) + c(q, q_{\text{new}}) \right] \]

where the neighborhood radius:

\[ r_n = \gamma \left(\frac{\log n}{n}\right)^{1/d} \]

Improvement 2: Rewire

Check whether nodes in the neighborhood can obtain a shorter path through \(q_{\text{new}}\); if so, update the parent.

for q_near in Near(T, q_new, r_n):
    if Cost(q_new) + c(q_new, q_near) < Cost(q_near):
        if COLLISION_FREE(q_new, q_near):
            T.rewire(q_near, parent=q_new)

2.3 PRM (Probabilistic Roadmap)

PRM is suitable for multi-query scenarios (planning multiple paths in the same environment).

Two Phases:

Learning Phase: Sample \(N\) nodes in free space, connect neighboring nodes to form a roadmap
Query Phase: Connect the start and goal to the roadmap, search for a path using A* or Dijkstra

BUILD_ROADMAP(N, k):
    V ← {}
    E ← {}
    for i = 1 to N:
        q ← RANDOM_FREE_CONFIG()
        V.add(q)
        neighbors ← K_NEAREST(V, q, k)
        for q_n in neighbors:
            if COLLISION_FREE(q, q_n):
                E.add(q, q_n)
    return Graph(V, E)

2.4 Method Comparison

Method	Query Mode	Optimality	Computational Cost	Suitable Scenarios
RRT	Single	No	Low	High-dimensional, fast exploration
RRT*	Single	Asymptotically Optimal	Medium	Quality paths needed
PRM	Multi	Asymptotically Optimal	High preprocessing	Static environment, multiple queries
Bi-RRT	Single	No	Low	Narrow passages

3 Search-Based Planning

3.1 A* Algorithm

Search for the shortest path on a discretized C-space (grid):

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

\(g(n)\): Actual cost from start to \(n\)
\(h(n)\): Heuristic estimate (must be admissible, i.e., \(h(n) \leq h^*(n)\))

3.2 Hybrid A*

Combines continuous state space with discrete search, commonly used for vehicle motion planning. Node expansion considers kinematic constraints (e.g., minimum turning radius).

4 Trajectory Optimization

Paths generated by sampling-based methods are typically not smooth or efficient. Trajectory optimization refines rough paths into high-quality trajectories through continuous optimization.

4.1 Problem Formulation

\[ \min_{\xi} \; \mathcal{J}[\xi] = \int_0^T F(\xi(t), \dot{\xi}(t), \ddot{\xi}(t)) \, dt \]

\[ \text{s.t.} \quad \xi(0) = q_{\text{start}}, \quad \xi(T) = q_{\text{goal}}, \quad \xi(t) \in \mathcal{C}_{\text{free}} \; \forall t \]

Typical cost functionals include:

Smoothness: \(\int \|\ddot{\xi}\|^2 dt\) (minimize acceleration)
Obstacle cost: \(\int c(\xi(t)) dt\) (higher cost closer to obstacles)
Time-optimal: \(\min T\)

4.2 CHOMP (Covariant Hamiltonian Optimization for Motion Planning)

CHOMP (Ratliff et al., 2009) uses covariant gradient descent to directly optimize trajectories.

Cost Function:

\[ \mathcal{U}[\xi] = \underbrace{\frac{1}{2} \int_0^1 \|\dot{\xi}(t)\|^2 dt}_{\text{Smoothness cost}} + \lambda \underbrace{\int_0^1 c(\xi(t)) \|\dot{\xi}(t)\| dt}_{\text{Obstacle cost}} \]

Obstacle cost field \(c(x)\):

\[ c(x) = \begin{cases} -d(x) + \frac{1}{2}\epsilon & \text{if } d(x) < 0 \\ \frac{1}{2\epsilon}(d(x) - \epsilon)^2 & \text{if } 0 \leq d(x) \leq \epsilon \\ 0 & \text{if } d(x) > \epsilon \end{cases} \]

where \(d(x)\) is the signed distance from point \(x\) to the nearest obstacle.

Covariant Gradient Update:

\[ \xi \leftarrow \xi - \frac{1}{\eta} A^{-1} \bar{\nabla} \mathcal{U} \]

where \(A\) is the smoothness metric matrix, and the use of \(A^{-1}\) ensures the update itself is smooth.

4.3 TrajOpt (Sequential Convex Optimization)

TrajOpt (Schulman et al., 2014) uses sequential convex optimization to handle non-convex constraints.

Core Idea: Linearize non-convex collision avoidance constraints and solve convex subproblems within a trust region.

\[ \min_{\xi} \; f_{\text{smooth}}(\xi) \]

\[ \text{s.t.} \quad \hat{c}_j(\xi) \leq 0, \quad j = 1, \ldots, m \quad \text{(linearized collision constraints)} \]

\[ \|\xi - \xi_k\| \leq \Delta \quad \text{(trust region constraint)} \]

Each step solves a quadratic program (QP), progressively approaching a feasible optimal solution.

4.4 Method Comparison

Method	Optimization Variables	Collision Handling	Convergence
CHOMP	Waypoint sequence	Cost field gradient	Local optimum, needs good initialization
TrajOpt	Waypoint sequence	Convex constraints	Local optimum, fast convergence
STOMP	Waypoint sequence	Random sampling	Gradient-free
iLQR/DDP	Control sequence	Cost terms	Local optimum, considers dynamics

5 Planning in Dynamic Environments

5.1 Online Replanning

D / D Lite: Incremental search algorithms that only update affected portions when the environment changes
ERRT: Extended RRT, reuses information from previous plans

5.2 Velocity Space Methods

Velocity Obstacles (VO): Computes collision cones in velocity space
ORCA: Optimal Reciprocal Collision Avoidance, suitable for multi-agent systems

5.3 Reactive Planning

Artificial Potential Fields: Goals generate attractive fields, obstacles generate repulsive fields

\[ F_{\text{total}} = -\nabla U_{\text{att}}(q) - \nabla U_{\text{rep}}(q) \]

Local Minima Problem

Potential field methods suffer from local minima, where the robot may get stuck at non-goal points. This can be resolved through random perturbation or combining with a global planner.

6 Integration with Learning

The combination of motion planning and reinforcement learning is a hot research direction (see Reinforcement Learning for Robotics for details).

6.1 Learned Heuristics

Use neural networks to predict sampling distributions, guiding RRT/PRM to sample in promising regions
MPNet (Qureshi et al., 2019): Learns a local planning policy from current configuration to goal

6.2 Diffusion Models for Planning

Diffuser (Janner et al., 2022): Models trajectory generation as a denoising diffusion process
Planning as Inference: Casts the planning problem as probabilistic inference

6.3 End-to-End Policies

Directly learn the mapping from observations to actions, bypassing explicit planning:

Imitation learning + trajectory optimization
Vision-based motion planning (ViNT, NoMaD)

7 Practical Considerations

7.1 Planning Pipeline

A typical robot motion planning pipeline:

Task Planning: High-level decisions (grasp first, then place)
Motion Planning: Generate collision-free paths
Trajectory Parameterization: Add time information (trapezoidal velocity profile, S-curve)
Smoothing and Post-processing: B-spline fitting, shortcutting
Control Execution: Joint-space tracking

7.2 Common Tools

Tool	Language	Features
OMPL	C++/Python	Open-source planning library with many sampling algorithms
MoveIt	C++/Python	ROS-integrated, industrial-grade motion planning
Drake	C++/Python	Includes trajectory optimization and control
CuRobo	Python/CUDA	NVIDIA-developed, GPU-accelerated planning

References

LaValle, S. M. (2006). Planning Algorithms. Cambridge University Press.
Karaman, S. & Frazzoli, E. (2011). Sampling-based algorithms for optimal motion planning. IJRR.
Ratliff, N. et al. (2009). CHOMP: Gradient optimization techniques for efficient motion planning. ICRA.
Schulman, J. et al. (2014). Motion planning with sequential convex optimization and convex collision checking. IJRR.
Lynch, K. M. & Park, F. C. (2017). Modern Robotics. Cambridge University Press.