Motion Planning around Obstacles with Convex Optimization

TL;DR

This paper introduces GCS, a convex optimization-based motion planner that reliably plans obstacle-avoiding trajectories in high-dimensional spaces, outperforming sampling-based methods in quality and efficiency.

Contribution

It presents a novel convex optimization framework for obstacle-aware motion planning that yields globally optimal trajectories with tight relaxations and bounds, applicable to complex robotic systems.

Findings

GCS finds higher-quality trajectories faster than sampling-based planners.

The convex relaxation is very tight, enabling effective rounding to global optima.

Demonstrated on diverse robotic platforms including quadrotors and manipulators.

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\'ezier 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.