skip to main content


Title: Motion Planning around Obstacles with Convex Optimization
Trajectory optimization o↵ers 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 di↵erential constraints. Indeed, obstacles are the source of many textbook examples of problematic nonconvexities in the trajectory-optimization prob- lem. 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 round- ing 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.  more » « less
Award ID(s):
1830901
NSF-PAR ID:
10356380
Author(s) / Creator(s):
Date Published:
Journal Name:
ArXivorg
ISSN:
2331-8422
Format(s):
Medium: X
Sponsoring Org:
National Science Foundation
More Like this
  1. null (Ed.)
    Motion planning for high degree-of-freedom (DOF) robots is challenging, especially when acting in complex environments under sensing uncertainty. While there is significant work on how to plan under state uncertainty for low-DOF robots, existing methods cannot be easily translated into the high-DOF case, due to the complex geometry of the robot’s body and its environment. In this paper, we present a method that enhances optimization-based motion planners to produce robust trajectories for high-DOF robots for convex obstacles. Our approach introduces robustness into planners that are based on sequential convex programming: We reformulate each convex subproblem as a robust optimization problem that “protects” the solution against deviations due to sensing uncertainty. The parameters of the robust problem are estimated by sampling from the distribution of noisy obstacles, and performing a first-order approximation of the signed distance function. The original merit function is updated to account for the new costs of the robust formulation at every step. The effectiveness of our approach is demonstrated on two simulated experiments that involve a full body square robot, that moves in randomly generated scenes, and a 7-DOF Fetch robot, performing tabletop operations. The results show nearly zero probability of collision for a reasonable range of the noise parameters for Gaussian and Uniform uncertainty. 
    more » « less
  2. null (Ed.)
    Abstract

    Quadrotors can provide services such as infrastructure inspection and search-and-rescue, which require operating autonomously in cluttered environments. Autonomy is typically achieved with receding-horizon planning, where a short plan is executed while a new one is computed, because sensors receive limited information at any time. To ensure safety and prevent robot loss, plans must be verified as collision free despite uncertainty (e.g, tracking error). Existing spline-based planners dilate obstacles uniformly to compensate for uncertainty, which can be conservative. On the other hand, reachability-based planners can include trajectory-dependent uncertainty as a function of the planned trajectory. This work applies Reachability-based Trajectory Design (RTD) to plan quadrotor trajectories that are safe despite trajectory-dependent tracking error. This is achieved by using zonotopes in a novel way for online planning. Simulations show aggressive flight up to 5 m/s with zero crashes in 500 cluttered, randomized environments.

     
    more » « less
  3. Robust trajectory execution is an extension of cooperative collision avoidance that takes pre-planned trajectories directly into account. We propose an algorithm for robust trajectory execution that compensates for a variety of dynamic changes, including newly appearing obstacles, robots breaking down, imperfect motion execution, and external disturbances. Robots do not communicate with each other and only sense other robots’ positions and the obstacles around them. At the high-level we use a hybrid planning strategy employing both discrete planning and trajectory optimization with a dynamic receding horizon approach. The discrete planner helps to avoid local minima, adjusts the planning horizon, and provides good initial guesses for the optimization stage. Trajectory optimization uses a quadratic programming formulation, where all safety-critical parts are formulated as hard constraints. At the low-level, we use buffered Voronoi cells as a multi-robot collision avoidance strategy. Compared to ORCA, our approach supports higher-order dynamic limits and avoids deadlocks better. We demonstrate our approach in simulation and on physical robots, showing that it can operate in real time. 
    more » « less
  4. Robust trajectory execution is an extension of cooperative collision avoidance that takes pre-planned trajectories directly into account. We propose an algorithm for robust trajectory execution that compensates for a variety of dynamic changes, including newly appearing obstacles, robots breaking down, imperfect motion execution, and external disturbances. Robots do not communicate with each other and only sense other robots’ positions and the obstacles around them. At the high-level we use a hybrid planning strategy employing both discrete planning and trajectory optimization with a dynamic receding horizon approach. The discrete planner helps to avoid local minima, adjusts the planning horizon, and provides good initial guesses for the optimization stage. Trajectory optimization uses a quadratic programming formulation, where all safety-critical parts are formulated as hard constraints. At the low-level, we use buffered Voronoi cells as a multi-robot collision avoidance strategy. Compared to ORCA, our approach supports higher-order dynamic limits and avoids deadlocks better. We demonstrate our approach in simulation and on physical robots, showing that it can operate in real time. 
    more » « less
  5. Planning safe trajectories for nonlinear dynamical systems subject to model uncertainty and disturbances is challenging. In this work, we present a novel approach to tackle chance-constrained trajectory planning problems with nonconvex constraints, whereby obstacle avoidance chance constraints are reformulated using the signed distance function. We propose a novel sequential convex programming algorithm and prove that under a discrete time problem formulation, it is guaranteed to converge to a solution satisfying first-order optimality conditions. We demonstrate the approach on an uncertain 6 degrees of freedom spacecraft system and show that the solutions satisfy a given set of chance constraints. 
    more » « less