DREAM: Decentralized Real-time Asynchronous Probabilistic Trajectory Planning for Collision-free Multi-Robot Navigation in Cluttered Environments

TL;DR

DREAM tackles collision-free, multi-robot navigation in cluttered environments with static and interactive dynamic obstacles by introducing probabilistic obstacle representations and a decentralized, real-time planning pipeline. The method combines goal-directed discrete search via cost algebraic A* with DSHT-based inter-robot safety and a BEZier-trajectory optimization that preserves computed collision probabilities; it explicitly models dynamic obstacle interactivity through movement and interaction vector fields and online behavior-model predictions. The approach accounts for sensing/prediction uncertainties and asynchronous inter-robot communication, showing up to 1.68× higher single-robot and 2.15× higher multi-robot success rates over baselines, and validating feasibility on real quadrotors. These results demonstrate DREAM’s potential to enable robust, collision-free, real-time navigation in complex, interactive environments with practical considerations like communication delays and perception noise.

Abstract

Collision-free navigation in cluttered environments with static and dynamic obstacles is essential for many multi-robot tasks. Dynamic obstacles may also be interactive, i.e., their behavior varies based on the behavior of other entities. We propose a novel representation for interactive behavior of dynamic obstacles and a decentralized real-time multi-robot trajectory planning algorithm allowing inter-robot collision avoidance as well as static and dynamic obstacle avoidance. Our planner simulates the behavior of dynamic obstacles, accounting for interactivity. We account for the perception inaccuracy of static and prediction inaccuracy of dynamic obstacles. We handle asynchronous planning between teammates and message delays, drops, and re-orderings. We evaluate our algorithm in simulations using 25400 random cases and compare it against three state-of-the-art baselines using 2100 random cases. Our algorithm achieves up to 1.68x success rate using as low as 0.28x time in single-robot, and up to 2.15x success rate using as low as 0.36x time in multi-robot cases compared to the best baseline. We implement our planner on real quadrotors to show its real-world applicability.

Figures (9)

• Figure 1: Static obstacles (gray) are modeled using their shapes and existence probabilities. Dynamic obstacles (cyan) are modeled using their shapes, current positions, and a probability distribution over their behavior models, each of which comprises a movement and an interaction model. Teammates (orange) are modeled using discretized separating hyperplane trajectories (DSHTs). The planner selects a goal position on a desired trajectory (red), plans a spatiotemporal discrete path (blue) to the goal position while minimizing the probability of collision with static and dynamic obstacles and DSHT violations, and solves a quadratic program to fit a smooth trajectory (green) to the discrete plan while preserving the collision probabilities computed and DSHT hyperplanes not violated during search.
• Figure 2: Planning pipeline.Inputs (purple), outputs (brown), and stages (teal) of our planning algorithm are shown. All stages are run in every planning iteration with updated inputs and a new trajectory is produced. Our algorithm is executed in a receding horizon fashion, in which, a long trajectory is planned, executed for a short duration, and a new trajectory is planned from scratch. In our experiments, we run our algorithm in $\approx 5Hz - 20Hz$.
• Figure 3: Goal selection. The goal selection stage selects the goal position $\mathbf{g}_i$ to plan to on the desired trajectory $\mathbf{d}_i$ (red) and the time $T_i'$ at which it should be (or should have been) reached. It finds the closest point $\mathbf{x}$ on $\mathbf{d}_i$ to the current robot position $\mathbf{p}_{i,0}^{self}$ and its time point $\tilde{T}$, and finds the smallest time point $T_i'$ that is greater than the time point that is one desired time horizon away from $\tilde{T}$, i.e., $\tilde{T}+\tau_i$, at which the robot is collision free against all static obstacles with existence probability greater than $p_i^{min}$.
• Figure 4: Discrete search. A sample discrete state sequence, the associated meta data, and computed cost terms are shown. The computed state sequence has six states: $x^{1:6}$. $x^3$ and $x^5$ are expanded with ROTATE actions from $x^2$ and $x^4$ respectively, therefore, their information is not shown here to reduce clutter. The robot initially does not collide with any static or dynamic obstacle and does not violate any DSHT hyperplane at $x^1$. While traversing the first segment from $x^1$ to $x^2$ (red), it collides with dynamic obstacle B's second behavior model and violates two hyperplanes in the DSHT against teammate D. While traversing the second segment from $x^3$ to $x^4$ (blue), it violates $2$ more hyperplanes from the DSHT against teammate D. While traversing the last segment from $x^5$ to $x^6$ (orange), it collides with the static obstacle with existence probability $0.12$, first two behavior models of dynamic obstacle C and violates a hyperplane in the DSHT against teammate A.
• Figure 5: Static and dynamic obstacle collision constraints. Given the gray static obstacle $j\in\mathcal{O}_i$ with shape $\mathcal{Q}_{i,j}$ and the green sweep $\zeta_{i,l}^{robot}$ of $\mathcal{R}_i^{robot}$ from $x^l.\mathbf{p}$ to $x^{l+1}.\mathbf{p}$, we compute the blue support vector machine hyperplane between them. We compute the orange separating hyperplane by snapping it to $\mathcal{Q}_{i,j}$. The robot should stay in the safe side of the orange hyperplane. We shift orange hyperplane to account for robot's collision shape $\mathcal{R}_i^{robot}$ and compute the magenta hyperplane $\mathcal{H}_{\zeta_{i,l}^{robot},\mathcal{Q}_{i,j}}$. The Bézier curve $\mathbf{f}_{i,l}(t)$ is constrained by $\mathcal{H}_{\zeta_{i,l}^{robot},\mathcal{Q}_{i,j}}$ to avoid $\mathcal{Q}_{i,j}$. To avoid the dynamic obstacle $j\in \mathcal{D}_i$ moving from $\tilde{\mathbf{p}}^{dyn}_{i, j, k}$ to $\mathbf{p}^{dyn}_{i, j, k}$, the support vector machine hyperplane between the region $\zeta^{dyn}_{i,j,k,l}$ swept by $\mathcal{R}_{i,j}^{dyn}$ and $\zeta_{i,l}^{robot}$ is computed. The same snap and shift operations are conducted to compute the magenta hyperplane $\mathcal{H}_{\zeta_{i,l}^{robot},\zeta^{dyn}_{i,j,k,l}}$, constraining $\mathbf{f}_{i,l}(t)$.

Theorems & Definitions (9)

• Definition 1
• Definition 2
• Definition 3
• Definition 4
• Definition 5
• Proposition 1
• proof
• Proposition 2
• proof