Logo
ScienceStack
Table of Contents
Fetching ...
Sign In

ZAPP! Zonotope Agreement of Prediction and Planning for Continuous-Time Collision Avoidance with Discrete-Time Dynamics

Luca Paparusso, Shreyas Kousik, Edward Schmerling, Francesco Braghin, Marco Pavone

TL;DR

ZAPP addresses the challenge of mismatched representations between predictive motion models and planning under uncertainty in multi-agent, continuous-time scenarios. It reformulates prediction outputs as zonotopes to create continuous-time reachable sets and derives differentiable collision-avoidance constraints that integrate with gradient-based trajectory optimization, using a predictor such as Trajectron++ for multi-modal forecasts. Key contributions include converting unbounded, multi-modal predictions into zonotopes, extending discrete-time predictions to continuous time with differentiable collision checking, and providing a simulation framework that demonstrates safer trajectories than baselines in interactive scenes. The approach offers a path toward robust, real-time, prediction-aware planning for mobile robots, with potential hardware validation and further extensions to strict safety guarantees.

Abstract

The past few years have seen immense progress on two fronts that are critical to safe, widespread mobile robot deployment: predicting uncertain motion of multiple agents, and planning robot motion under uncertainty. However, the numerical methods required on each front have resulted in a mismatch of representation for prediction and planning. In prediction, numerical tractability is usually achieved by coarsely discretizing time, and by representing multimodal multi-agent interactions as distributions with infinite support. On the other hand, safe planning typically requires very fine time discretization, paired with distributions with compact support, to reduce conservativeness and ensure numerical tractability. The result is, when existing predictors are coupled with planning and control, one may often find unsafe motion plans. This paper proposes ZAPP (Zonotope Agreement of Prediction and Planning) to resolve the representation mismatch. ZAPP unites a prediction-friendly coarse time discretization and a planning-friendly zonotope uncertainty representation; the method also enables differentiating through a zonotope collision check, allowing one to integrate prediction and planning within a gradient-based optimization framework. Numerical examples show how ZAPP can produce safer trajectories compared to baselines in interactive scenes.

ZAPP! Zonotope Agreement of Prediction and Planning for Continuous-Time Collision Avoidance with Discrete-Time Dynamics

TL;DR

ZAPP addresses the challenge of mismatched representations between predictive motion models and planning under uncertainty in multi-agent, continuous-time scenarios. It reformulates prediction outputs as zonotopes to create continuous-time reachable sets and derives differentiable collision-avoidance constraints that integrate with gradient-based trajectory optimization, using a predictor such as Trajectron++ for multi-modal forecasts. Key contributions include converting unbounded, multi-modal predictions into zonotopes, extending discrete-time predictions to continuous time with differentiable collision checking, and providing a simulation framework that demonstrates safer trajectories than baselines in interactive scenes. The approach offers a path toward robust, real-time, prediction-aware planning for mobile robots, with potential hardware validation and further extensions to strict safety guarantees.

Abstract

The past few years have seen immense progress on two fronts that are critical to safe, widespread mobile robot deployment: predicting uncertain motion of multiple agents, and planning robot motion under uncertainty. However, the numerical methods required on each front have resulted in a mismatch of representation for prediction and planning. In prediction, numerical tractability is usually achieved by coarsely discretizing time, and by representing multimodal multi-agent interactions as distributions with infinite support. On the other hand, safe planning typically requires very fine time discretization, paired with distributions with compact support, to reduce conservativeness and ensure numerical tractability. The result is, when existing predictors are coupled with planning and control, one may often find unsafe motion plans. This paper proposes ZAPP (Zonotope Agreement of Prediction and Planning) to resolve the representation mismatch. ZAPP unites a prediction-friendly coarse time discretization and a planning-friendly zonotope uncertainty representation; the method also enables differentiating through a zonotope collision check, allowing one to integrate prediction and planning within a gradient-based optimization framework. Numerical examples show how ZAPP can produce safer trajectories compared to baselines in interactive scenes.
Paper Structure (32 sections, 2 theorems, 19 equations, 3 figures, 1 table)

This paper contains 32 sections, 2 theorems, 19 equations, 3 figures, 1 table.

Table of Contents

  1. Introduction
  2. Problem Formulation
  3. Setup
  4. Agents, Modes, and Dynamics
  5. Reachable Sets
  6. Problem Statement
  7. Proposed Method
  8. Zonotope Preliminaries
  9. Reformulation for Implementation
  10. Dynamics via a Prediction Model
  11. Reformulation
  12. Implementation
  13. Decision Variable Implementation
  14. Discrete-Time Reachable Sets via a Prediction Model
  15. Reformulation
  16. ...and 17 more sections

Key Result

Proposition 1

Consider the zonotopes $\mathcal{Z}_1 = {{\mathscr{Z}\!}\!\left(c_1,G_2\right)}$ and $\mathcal{Z}_2 = {{\mathscr{Z}\!}\!\left(c_2,G_2\right)}$. Then, $\mathcal{Z}_1 \cap \mathcal{Z}_2 = \emptyset$ if and only if $c_1 \not\in {{\mathscr{Z}\!}\!\left(c_2,[G_1,G_2]\right)}$.

Figures (3)

  • Figure 1: Overview of approach and notation. Existing interaction prediction approaches typically use coarse time discretizations to enable long-term predictions, but this can lead to missed collision detections in motion planning. We use zonotopes (blue and red polygons) to represent uncertain, continuous-time reachable sets (light blue and red tubes) for agents in interactive scenes (ego agent in blue, other agent $i$ in red). Note that approximating the reachable sets in discrete time (polygons with thick outlines) may still be insufficient for detecting collisions. To find an ego motion plan, we propose a numerical trajectory optimization approach with zonotope collision-avoidance constraints. The zonotopes are backed out from the outputs of a neural network predictive model, which also provides gradients for trajectory optimization (green dashed arrows). We also illustrate the ego goal $x_{\mathrm{\textnormal{g}}}$, agents' state history $X_0$, and static obstacles $\mathcal{X}_{\mathrm{\textnormal{obs}}}$.
  • Figure 2: ZAPP takes as input a nominal ego-agent trajectory (black dashed line), which is used to predict the nominal continuous-time predictions (blue tubes), and finally steers the motion plan to avoid collision (blue line).
  • Figure 3: Example of a completed scenario in the robustness test (w/o DC). ZAPP is able to lead ego to the goal (from left to right) without crashes, implementing receding horizon motion planning (trajectories at each time step shown in blue). To ease visualization, the surrounding agents' trajectories are shown at discrete times with color fading from dark to light as time passes.

Theorems & Definitions (3)

  • Proposition 1: guibas2003zonotopes
  • Proposition 2: althoff2010reachability
  • Remark 3