Interleaving Discrete Search and Continuous Optimization for Kinodynamic Motion Planning

Abstract

Motion planning for dynamically complex robotic tasks requires explicit reasoning within constraints on velocity, acceleration, force/torque, and kinematics such as avoiding obstacles. To meet these constraints, planning algorithms must simultaneously make high-level discrete decisions and low-level continuous decisions. For example, pushing a heavy object involves making discrete decisions about contact locations and continuous decisions about fine interactions. Existing methods either search on a graph constructed by sampling or systematically discretizing the planning space or optimize parameterized trajectories subject to differential constraints. However, these methods suffer from combinatorial complexity or lack of convergence over long horizons. We hypothesize that these discrete high-level decisions often lie in a lower-dimensional subspace of the full planning space. Consequently, this thesis argues that by interleaving searching in this lower-dimensional discrete space with selective reasoning in the full planning space, we can efficiently discover global, long-horizon, dynamically rich capabilities to complete a task.

The goal of this thesis is to enable the automatic discovery of well-reasoned long-horizon plans for complex dynamical systems. To this end, we present the INterleaved Search And Trajectory optimization (INSAT) algorithm. INSAT combines the benefit of graph search-based planning algorithms to find paths over non-convex state spaces and that of trajectory optimization to find dynamically feasible trajectories in high-dimensional spaces. We demonstrate the effectiveness of INSAT in two challenging domains 1) aggressive quadrotor flight in large environments (in simulation) and 2) contact-rich manipulation of heavy objects in confined spaces. We also show that, by interleaving graph search and trajectory optimization, INSAT solves planning problems that a naively initialized trajectory optimization or standalone graph search does not solve.

The second half of the thesis explores three different ways of accelerating the INSAT planner: (1) CPU Parallelization: By leveraging recent advances in parallelized graph search, we enable CPU acceleration for optimization-based edge computation in INSAT, (2) Richer Planning Representation: We use sparse and optimization-friendly planning space representation called graphs of convex sets (GCS) to speed up planning with INSAT. We also demonstrate that using INSAT as a planner on graphs of convex sets (IxG) outperforms GCSOpt in some theoretical properties and runtime efficiency if all the edge constraints along any path in the underlying GCS can be satisfied, (3) Preprocessing: By computing a motion library offline we can guarantee constant-time solutions for any planning query to IxG. These methods collectively enhance the performance and efficiency of the INSAT planner, making it more capable of addressing complex planning tasks.