Motion Planning around Obstacles with Convex Optimization
TL;DR GCS plans collision-free trajectories as a shortest path through a graph of convex safe regions, solved as one convex program plus a cheap rounding step. On 100 random quadrotor environments 95% of its plans are within 1% of the global optimum, and on a 7-DoF arm it finds shorter paths than PRM in less time.
What they do
Trajectory optimization handles dynamics but relies on local solvers that can fail in cluttered spaces; sampling-based planners struggle in high dimensions and with continuous differential constraints; mixed-integer planners are globally optimal but can take minutes. GCS assumes the free configuration space is covered by convex “safe” regions and casts trajectory design as a shortest-path problem in a Graph of Convex Sets. The mixed-integer program has a very tight convex relaxation, so one convex solve plus rounding usually yields a globally optimal trajectory and a certified optimality bound.
How it works
- Graph. One vertex per safe region, edges between overlapping regions, plus source and target vertices for the boundary conditions. Each region carries a Bézier trajectory segment and a Bézier time-scaling curve.
- Convex constraints. Control points inside a region keep the whole segment inside it; continuity, velocity limits and boundary values become linear or convex constraints on control points. Duration, length and energy costs use convex upper bounds, giving typically an SOCP.
- Rounding. Relaxed edge variables act as probabilities for a randomized depth-first search that collects up to 10 distinct paths; each is refined by a small convex program. Comparing relaxed and rounded costs bounds the optimality gap.
- Regions are exact for polyhedral obstacles; for arms they come from IRIS, seeded by hand with inverse kinematics.
Results
- 2,500-cell maze: the relaxation is already binary, so globally optimal without rounding, using 5,198 binaries where a prior MICP formulation would need 6.25 million.
- Quadrotor in 100 random buildings: 95% of plans within 1% of the global optimum (worst 2.9%); solves take median 3.7 s, maximum 31.2 s.
- 7-DoF KUKA iiwa, five tasks: GCS paths are shorter than PRM and PRM with short-cutting, and GCS runs faster than plain PRM.
- 14-DoF dual arm: certified gaps of 3.3%, 2.0% and 0.6%, solved in 4.0, 8.4 and 12.9 s.
Limitations
Needs a convex decomposition of free space; for arms it is approximate, hand-seeded and, in the fast IRIS variant used, not certified collision-free. Dynamics, task-space constraints and exact higher-derivative penalties are nonconvex in this formulation (derivatives are only regularized). MOSEK needed a slower setting to avoid numerical issues. All experiments are in simulation, and the implementation is more involved than sampling-based planners.
Figures
Abstract
Trajectory optimization offers mature tools for motion planning in high-dimensional spaces under dynamic constraints. However, when facing complex configuration spaces, cluttered with obstacles, roboticists typically fall back to sampling-based planners that struggle in very high dimensions and with continuous differential constraints. Indeed, obstacles are the source of many textbook examples of problematic nonconvexities in the trajectory-optimization problem. Here we show that convex optimization can, in fact, be used to reliably plan trajectories around obstacles. Specifically, we consider planning problems with collision-avoidance constraints, as well as cost penalties and hard constraints on the shape, the duration, and the velocity of the trajectory. Combining the properties of Bézier curves with a recently-proposed framework for finding shortest paths in Graphs of Convex Sets (GCS), we formulate the planning problem as a compact mixed-integer optimization. In stark contrast with existing mixed-integer planners, the convex relaxation of our programs is very tight, and a cheap rounding of its solution is typically sufficient to design globally-optimal trajectories. This reduces the mixed-integer program back to a simple convex optimization, and automatically provides optimality bounds for the planned trajectories. We name the proposed planner GCS, after its underlying optimization framework. We demonstrate GCS in simulation on a variety of robotic platforms, including a quadrotor flying through buildings and a dual-arm manipulator (with fourteen degrees of freedom) moving in a confined space. Using numerical experiments on a seven-degree-of-freedom manipulator, we show that GCS can outperform widely-used sampling-based planners by finding higher-quality trajectories in less time.
Summaries are written automatically from each paper's text. Figures and videos come from the paper and its project page; all credit goes to the authors.