Motion Planning with Precedence Specifications via Augmented Graphs of Convex Sets

TL;DR

This paper introduces an exact convex partitioning and augmented graph approach for trajectory planning that satisfies obstacle avoidance and key-door precedence constraints, outperforming existing methods.

Contribution

The paper presents a novel convex partitioning of free space and an augmented graph encoding precedence constraints, enabling efficient exact trajectory planning.

Findings

The method efficiently solves key-door precedence planning problems.

It is several orders of magnitude faster than recent state-of-the-art methods.

Numerical experiments validate the effectiveness of the proposed pipeline.

Abstract

We present an algorithm for planning trajectories that avoid obstacles and satisfy key-door precedence specifications expressed with a fragment of signal temporal logic. Our method includes a novel exact convex partitioning of the obstacle free space that encodes connectivity among convex free space sets, key sets, and door sets. We then construct an augmented graph of convex sets that exactly encodes the key-door precedence specifications. By solving a shortest path problem in this augmented graph of convex sets, our pipeline provides an exact solution up to a finite parameterization of the trajectory. To illustrate the effectiveness of our approach, we present a method to generate key-door mazes that provide challenging problem instances, and we perform numerical experiments to evaluate the proposed pipeline. Our pipeline is faster by several orders of magnitude than recent state-of-the art methods that use general purpose temporal logic tools.