Motion planning around obstacles with convex optimization

Abstract

From quadrotors delivering packages in urban areas to robot arms moving in confined warehouses, motion planning around obstacles is a core challenge in modern robotics. Planners based on optimization can design trajectories in high-dimensional spaces while satisfying the robot dynamics. However, in the presence of obstacles, these optimization problems become nonconvex and very hard to solve, even just locally. Thus, when facing cluttered environments, roboticists typically fall back to sampling-based planners that do not scale equally well to high dimensions and struggle with continuous differential constraints. Here, we present a framework that enables convex optimization to efficiently and reliably plan trajectories around obstacles. Specifically, we focus on collision-free motion planning with costs and constraints on the shape, the duration, and the velocity of the trajectory. Using recent techniques for finding shortest paths in Graphs of Convex Sets (GCS), we design a practical convex relaxation of the planning problem. We show that this relaxation is typically very tight, to the point that a cheap postprocessing of its solution is almost always sufficient to identify a collision-free trajectory that is globally optimal (within the parameterized class of curves). Through numerical and hardware experiments, we demonstrate that our planner, which we name GCS, can find better trajectories in less time than widely used sampling-based algorithms and can reliably design trajectories in high-dimensional complex environments.

Computing Safe Stop Trajectories for Autonomous Driving Utilizing Clustering and Parametric Optimization

“…In the context of autonomous driving, there are strict requirements on robustness and real-time capabilities. There exist approaches to formulate convex problems for trajectory computations to overcome adversity posed by nonlinearity [9,11,12]. These procedures typically feature remarkable convergence performance but heavily restrict the problem formulation as all involved functions have to be convex.…”

“…Assume the sensitivity theorem (Theorem 3) holds for a reference solution z(p 0 ). For a parametrization p ∈ P, the term z[0] (p) is defined as (7), and z[k] (p) is defined as (12). Then, there exists a neighborhood U (p 0 ) of p 0 , such that for all p ∈ U (p 0 ), the following holds:…”