Generative multi-robot task and motion planning over long horizons

Abstract

The state of the art practice in robotics planning is to script behaviors manually, where each behavior is typically precomputed in advance. However, in order for robots to be able to act robustly and adapt to novel situations, they need to be able to plan sequences of behaviors and activities autonomously. Since the conditions and effects of these behaviors are tightly coupled through time, state and control variables, many problems require that the tasks of activity planning and trajectory optimization are considered together. There are two key issues underlying effective hybrid activity and trajectory planning: the sufficiently accurate modeling of robot dynamics and the capability of planning over long horizons. Hybrid activity and trajectory planners that employ mixed integer programming within a discrete time formulation are able to accurately model complex dynamics for robot vehicles, but are often restricted to relatively short horizons. On the other hand, current hybrid activity planners that employ continuous time formulations can handle longer horizons but they only allow actions to have continuous effects with constant rate of change, and restrict the allowed state constraints to linear inequalities. This greatly limits the expressivity of the problems that these approaches can solve. In this work we present Scotty, a planning system for hybrid activity and trajectory planning problems. Unlike other continuous time planners, Scotty can solve a broad class of expressive robotic planning problems by supporting convex quadratic constraints on state variables and control variables that are jointly constrained and that affect multiple state variables simultaneously. In order to efficiently generate practical plans for coordinated mobile robots over long horizons, our approach employs recent methods in convex optimization combined with methods for planning with relaxed planning graphs and heuristic forward search. The contributions of this thesis are threefold. First, we introduce a convex, goal-directed scheduling and trajectory planning problem. To solve this problem, we present the ScottyConvexPath planner, which reformulates the problem as a Second Order Cone Program (SOCP). Our formulation allows us to efficiently compute robot trajectories with first order dynamics over long horizons. While straightforward formulations are not convex, we present a convex model that does not require state, control or time discretization. Second, we introduce the ScottyActivity planner, a state of the art hybrid activity and trajectory planner that interleaves heuristic forward search with delete relaxations and consistency checks using our convex model. Finally, we present ScottyPath, a qualitative state plan planner that computes control and obstacle-free state trajectories for robots in order to satisfy the temporally extended goals and constraints that ScottyActivity imposes. ScottyPath finds obstacle-free paths in which all robots are guaranteed to always remain within obstacle-free safe regions, which are computed in advance. We introduce several new robotic planning domains, that we use to evaluate the scalability of our planning system and compare the performance of our approach against other prior methods. Our results show that ScottyActivity performs similarly to other state of the art heuristic forward search activity planners, while solving much more expressive robotic planning problems. On the other hand, ScottyPath can generate obstacle-free paths where robots are contained in obstacle-free convex regions more than two orders of magnitude faster than alternative mixed-integer approaches.