Path Planning

In 2017, a Boston Dynamics Atlas robot fell off a box during a demo. The failure was not in perception or control - the path planner had found a valid configuration sequence but the trajectory optimizer had not enforced the center-of-mass constraint. Path planning without proper motion planning produces plans that look correct on screen but make the robot fall over in the real world.

• **Waymo** uses a 3-layer stack: a route planner on HD maps, a behavior planner at 10 Hz, and a motion planner at 20 Hz that generates jerk-limited trajectories for its autonomous vehicles handling 50,000+ rides per week in San Francisco.
• **Amazon Robotics** operates 750,000+ robots in fulfillment centers using PRM-based planners pre-built offline for each warehouse layout, enabling each robot to replan in under 5 ms when another robot blocks its path.
• **Franka Emika** (now Franka Robotics) ships a TOPP-RA motion planner in every Panda arm, automatically computing minimum-time trajectories within joint torque limits without any user configuration - enabling rapid deployment in 3,000+ research labs worldwide.

Rapidly-exploring Random Trees (RRT)

RRT grows a tree from the start configuration by randomly sampling the configuration space and extending toward samples. Each iteration picks a random q_rand, finds the nearest tree node q_near, and steps a distance epsilon toward q_rand to produce q_new - if the segment is collision-free, q_new joins the tree. The expected coverage grows as O(log n / n) over n samples, and the algorithm is probabilistically complete: given enough time it finds a path if one exists.

**RRT* (Karaman & Frazzoli, 2011)** adds rewiring: when q_new is added, it checks all nodes within a shrinking radius r_n = gamma*(log(n)/n)^(1/d) and rewires to minimize path cost. RRT* is asymptotically optimal - the path cost converges to the global optimum as n → infinity. Waymo uses a variant of RRT* for offline HD-map corridor generation.

Probabilistic Roadmap Method (PRM)

PRM splits planning into two phases. The learning phase samples N random configurations from free C-space and connects each to its k nearest free-space neighbors, building a roadmap graph. The query phase connects start and goal to the roadmap and runs Dijkstra or A* on the graph. PRM is ideal when many queries share the same static environment - the roadmap is built once and reused across thousands of queries.

**Lazy PRM** defers collision checking to query time: edges are added without checking during the learning phase and only validated when a path candidate is found. This halves build time in environments where most edges are free. NASA JPL used a PRM variant to pre-plan 35,000+ waypoints for Mars rover Perseverance's traverse planning offline.

Artificial Potential Fields

Potential field planning treats the goal as an attractive well and obstacles as repulsive hills. The robot follows the negative gradient of the total potential U = U_att + U_rep. The attractive potential U_att = 0.5 * zeta * d(q, q_goal)^2 pulls toward the goal; the repulsive potential U_rep = 0.5 * eta * (1/d - 1/d_0)^2 for d < d_0 pushes away from obstacles within influence distance d_0. The resulting force F = -grad(U) drives motion at every timestep without search.

**Local minima** are the fundamental failure mode of potential fields. A configuration where attractive and repulsive forces cancel - but is not the goal - causes the robot to stop permanently. Solutions include: (1) adding random perturbation when stuck, (2) using harmonic functions (guaranteed no local minima in 2D), or (3) using potential fields only for local avoidance while a global planner (RRT/PRM) handles global structure. Boston Dynamics' Atlas uses exactly this hybrid: MPC for global footstep planning + potential fields for real-time collision avoidance.

Complete Motion Planning Stacks

Production robot systems layer multiple planning levels. The global planner (RRT*, A*, Dijkstra) finds a collision-free path in configuration space ignoring dynamics. A trajectory optimizer (CHOMP, TrajOpt, or MPC) smooths and time-parameterizes the path subject to velocity, acceleration, and jerk limits. A local planner (DWA, TEB, potential fields) handles real-time obstacle avoidance within a short horizon. The controller (PID, feedforward, impedance control) tracks the planned trajectory. This is the MoveIt! stack used in 80%+ of industrial ROS robots.

**TOPP-RA (Time-Optimal Path Parameterization with Reachability Analysis)** converts a geometric path into a minimum-time trajectory respecting joint velocity, acceleration, and torque limits. It solves a 1D reachability problem along path length s, running in O(N) rather than O(N^2) for N waypoints. Franka Emika's production stack uses TOPP-RA for all pick-and-place motion generation.

Key ideas

• **RRT** builds a tree by random sampling and stepwise extension - probabilistically complete and effective in high-dimensional C-spaces. **RRT*** adds rewiring for asymptotic optimality.
• **PRM** amortizes planning cost by building a roadmap once for a static environment, then answering thousands of queries via graph search - ideal for repeated planning in fixed workspaces.
• **Production stacks** layer global planners (RRT*, A*), trajectory optimizers (TOPP-RA, TrajOpt), and local planners (DWA, MPC) at different frequencies to balance plan quality against real-time reactivity.

Related topics

Path planning draws on optimization, graph algorithms, and control theory:

• SLAM — SLAM provides the map (occupancy grid or landmark set) that the path planner queries for collision checking - without localization, path planning has no obstacle information.
• Reinforcement Learning — Model-free RL (especially TD3, SAC) can learn navigation policies end-to-end, bypassing explicit path planning - but requires 10-100x more data and is harder to guarantee safety than classical planners.

Вопросы для размышления

• A warehouse robot uses PRM built on a Monday. On Tuesday, a new shelving unit is added. Which specific edges in the roadmap are invalid, and what is the most efficient way to repair the roadmap without full rebuild?
• Potential fields fail at local minima. How would you detect that the robot is stuck in a local minimum at runtime, and what recovery behavior would preserve safety while escaping it?
• A 7-DOF robot arm needs to pick objects at 1,000 different positions in a fixed cell. Compare the total planning time if RRT* is used for each query vs. PRM built once. What is the crossover N where PRM becomes more efficient?

Связанные уроки

• alg-14-dijkstra