Motion Planning around Obstacles with Convex Optimization

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.

Temporal Logic Motion Planning with Convex Optimization via Graphs of Convex Sets

“…The results in this paper are a substantial improvement on that basic idea. Our proposed approach results in an MICP with an even tighter convex relaxation: so tight, in fact, that it can be solved with convex optimization and rounding [6]. Interestingly, it is only through connections with older automata-theoretic methods that we are able to formulate this efficiently-solvable MICP.…”

“…The computational workhorse behind our proposed approach is Graphs of Convex Sets (GCS), a framework first introduced in [5] and applied to standard motion planning problems (reach a goal and avoid obstacles) in [6]. In this work, we are heavily inspired by [6], and show that the promising computational attributes of GCS can be applied to motion planning problems much more complex than the classical reach-avoid problem.…”

“…This MICP has a very tight convex relaxation, meaning it is fairly close to convex optimization and tends to be efficiently solved by branchand-bound solvers. The convex relaxation is so tight, in fact, that good solutions can often be found with a combination of convex optimization and rounding [6].…”

“…1a. While there are many important details in the transcription of problem (5) into a convex program, we refer the interested reader to [5,6] for these details. In this paper, we use the GCS tools provided in Drake [38].…”

“…Our key idea is to re-frame LTL motion planning as a shortest path problem in a Graph of Convex Sets (GCS) [5]. This results in an MICP with a very tight convex relaxation-so tight, in fact, that it can often be solved to global optimality with convex optimization and rounding [6]. Even if the approximate solution to this MICP is sub-optimal (bounds on sub-optimality are available [6] and convex optimization often finds the globally optimal solution in practice), any integer-feasible solution is guaranteed to satisfy the specification.…”