Motion planning

Overview

Motion planning finds a way for a robot to move from a start configuration to a goal without collisions. It then turns that path into a trajectory that the joint controllers can track. The output is a sequence of joint positions, or base poses for a mobile robot, with times attached. This hub is for engineers who call a planner from a library and want to know why it fails or runs slowly.

Planning happens in configuration space, the space of all joint values. A 6-DoF arm has a 6-dimensional configuration space. Obstacles in the workspace map to irregular regions in that space, and no practical planner computes those regions. Planners only ask a collision checker whether one configuration, or a short motion between two, is free. That query dominates run time.

Key ideas

Search on a graph: A* and lattices

On a grid or a lattice of motion primitives, planning becomes graph search. A* (Hart, Nilsson and Raphael, 1968) expands the node with the lowest cost so far plus a heuristic estimate of the cost to go. If the heuristic never overestimates, the first path A* returns is optimal on that graph. Grid search is complete and repeatable, so mobile robot global planners still rely on it. Nav2 ships NavFn, Theta* and the Smac planners, which search 2D grids and state lattices for a base. The price is memory. A grid grows exponentially with dimension, so search works for a base in (x, y, heading) and fails for a 7-DoF arm.

Sampling-based planners: PRM and RRT

Sampling planners never build the grid. A probabilistic roadmap (Kavraki et al., 1996) scatters random collision-free configurations, links near neighbours with straight motions that pass the collision check, and answers queries by graph search on that roadmap. It pays off when many queries share one static scene. A rapidly exploring random tree (LaValle, 1998) grows a tree from the start towards random samples and suits one-off queries in a scene that changes. RRT-Connect grows two trees, one from each end, until they meet.

These planners are probabilistically complete. If a path exists, the chance of finding it tends to one as sampling continues. Plain RRT and PRM say nothing about path quality. Karaman and Frazzoli (2011) showed that the RRT* and PRM* variants converge towards the optimal path, at the cost of many more collision checks. The raw output is jerky and longer than it needs to be, so a shortcutting or smoothing pass always follows. OMPL implements the PRM, RRT, EST, KPIECE and FMT families, among others, behind one interface.

Trajectory optimisation

Optimisation-based planners start from an initial guess, often a straight line in joint space. They push it away from obstacles while minimising a cost such as path length or squared velocity. CHOMP (Zucker et al., 2013) follows gradients of a signed distance field. TrajOpt (Schulman et al., 2014) solves a sequence of convex problems with collision constraints linearised at each step. STOMP perturbs the trajectory with noise and keeps the better samples, so it needs no gradients. MoveIt 2 ships CHOMP and STOMP as planner plugins, and Drake provides kinematic trajectory optimisation and graphs of convex sets. Optimisers are fast and give smooth motion, but they are local. A poor initial guess can end in a local minimum that still passes through an obstacle. A common pattern is to seed the optimiser with a sampling planner’s path.

Collision checking and time parameterisation

A collision checker tests pairs of shapes. Libraries such as FCL use bounding volume hierarchies to reject distant pairs cheaply and compute exact contact or distance only for close pairs. Full CAD meshes are slow. Primitives (boxes, cylinders, capsules) or convex decompositions are much cheaper to check. Adjacent links that always touch must be listed in an allowed collision matrix, or the planner reports every configuration as in collision. A motion between two configurations is checked at discrete points, so a thin obstacle can slip between two checks when the resolution is too coarse.

A geometric path has no time. Time parameterisation chooses how fast to move along the path so that every joint stays within its velocity and acceleration limits. The time-optimal version drives at least one joint to a limit at every instant. TOPP-RA (Pham and Pham) solves it by reachability analysis: it discretises the path and solves a small linear program at each point. MoveIt 2 uses Time-Optimal Trajectory Generation (TOTG) by default, and can apply Ruckig afterwards to limit jerk.

Where to start

Begin with Grid path planning, which builds A* on an occupancy grid. Inverse kinematics shows how a goal pose becomes a goal configuration, the first step of every arm query. The differential drive A* project plans and tracks a path for a simulated mobile robot. For arms, Simulate an arm in MuJoCo gives you a model to check collisions against.

Tools and platforms

Common mistakes

• Planning to a pose when inverse kinematics has several solutions, then seeing the elbow flip. Pick the solution closest to the current configuration, or plan to a joint goal.
• Building collision models from full CAD meshes. Use primitives or convex hulls with a few millimetres of padding.
• Changing the gripper and leaving the allowed collision matrix as it was.
• Treating a planner timeout as proof that no path exists. Sampling planners are only probabilistically complete.
• Sending an untimed path to a controller, or timing it with datasheet limits and no margin.
• Comparing planners on a single run. Sampling planners are random, so compare success rate and median time over many seeds.

Going deeper

• Hart, Nilsson and Raphael, A Formal Basis for the Heuristic Determination of Minimum Cost Paths, 1968.
• Kavraki, Švestka, Latombe and Overmars, Probabilistic roadmaps for path planning in high-dimensional configuration spaces, 1996.
• LaValle, Rapidly-exploring random trees: A new tool for path planning, TR 98-11, Iowa State University, 1998, listed on his RRT publications page.
• Karaman and Frazzoli, Sampling-based Algorithms for Optimal Motion Planning, 2011.
• Zucker et al., CHOMP: Covariant Hamiltonian optimization for motion planning, 2013, and Schulman et al., Motion planning with sequential convex optimization and convex collision checking, 2014.
• Pham and Pham, A New Approach to Time-Optimal Path Parameterization based on Reachability Analysis, arXiv 2017.
• LaValle, Planning Algorithms, Cambridge University Press, 2006, free online from the author.
• The MoveIt 2 time parameterisation tutorial.