DYNUS: Uncertainty-aware Trajectory Planner in Dynamic Unknown Environments

TL;DR

DYNUS tackles planning in dynamic unknown environments by integrating a spatio-temporal Global Planner (DGP), a Safe Corridor-based framework, and a fast hard-constraint local optimizer with variable elimination. It combines JPS for speed and Dynamic A* for dynamic obstacles, and employs exploratory, safe, and contingency trajectories to adapt to changes, all while accounting for obstacle uncertainty with AEKF-based tracking and constant-acceleration predictions. A decoupled yaw optimization with graph search and B-spline fitting, plus frontier-based exploration, enhances robustness and information gathering. Empirical results in simulation and hardware across UAVs, wheels, and quadrupeds show 100% success in dynamic scenarios, fast travel times (roughly 25% faster than baselines in some cases), and real-time onboard operation on diverse platforms.

Abstract

This paper introduces DYNUS, an uncertainty-aware trajectory planner designed for dynamic unknown environments. Operating in such settings presents many challenges -- most notably, because the agent cannot predict the ground-truth future paths of obstacles, a previously planned trajectory can become unsafe at any moment, requiring rapid replanning to avoid collisions. Recently developed planners have used soft-constraint approaches to achieve the necessary fast computation times; however, these methods do not guarantee collision-free paths even with static obstacles. In contrast, hard-constraint methods ensure collision-free safety, but typically have longer computation times. To address these issues, we propose three key contributions. First, the DYNUS Global Planner (DGP) and Temporal Safe Corridor Generation operate in spatio-temporal space and handle both static and dynamic obstacles in the 3D environment. Second, the Safe Planning Framework leverages a combination of exploratory, safe, and contingency trajectories to flexibly re-route when potential future collisions with dynamic obstacles are detected. Finally, the Fast Hard-Constraint Local Trajectory Formulation uses a variable elimination approach to reduce the problem size and enable faster computation by pre-computing dependencies between free and dependent variables while still ensuring collision-free trajectories. We evaluated DYNUS in a variety of simulations, including dense forests, confined office spaces, cave systems, and dynamic environments. Our experiments show that DYNUS achieves a success rate of 100% and travel times that are approximately 25.0% faster than state-of-the-art methods. We also evaluated DYNUS on multiple platforms -- a quadrotor, a wheeled robot, and a quadruped -- in both simulation and hardware experiments.

Figures (31)

• Figure 1: DYNUS system overview: Point cloud data from both the LiDAR and depth camera are processed by the Octomap-based Map Manager. Point cloud data is also sent to the Dynamic Obstacle Tracker, where dynamic obstacles are detected, clustered, and matched with previously observed obstacles. The AEKF estimates the current positions of dynamic obstacles, and a constant acceleration model is used to predict their future trajectories. The voxel map and predicted future trajectories of dynamic obstacles ($\mathbf{p}_{\text{obst}}(t)$) are sent to the DGP and Safe Corridor Generation modules. The DGP module computes a global path, and the Safe Corridor Generation module produces a series of overlapping polyhedra. These polyhedra are then used in the Trajectory Optimization module to ensure the safety of the planned trajectory. Note that the obstacles and safe polyhedra are colored red, purple, and blue according to their predicted time stamps. The optimized trajectory ($\mathbf{p}(t)$) is passed to the Yaw Optimization module, where a sequence of yaw angles is generated by the Graph Search module and then smoothed using the Yaw Fitting module. $\mathbf{p}(t)$ and $\mathbf{\psi}(t)$ are then sent to the low-level controller.
• Figure 2: Trajectory planning framework: $\mathcal{U}$ represents unknown space, Point L is the current agent position, Point A is the replanning starting point, Point H is the point where the exploratory trajectory enters the unknown space, Point E is the goal for the exploratory trajectory, and Point S is the endpoint of the committed safe trajectory. The subscripts denote the replanning iteration. The agent first generates an exploratory trajectory that may go into the unknown space $\mathcal{U}$. The agent then attempts to generate safe trajectories along the exploratory trajectory from Point A to H. The agent initially commits to a trajectory that begins at point A as an exploratory trajectory, but switches to a safe trajectory that ends at point S. If a dynamic obstacle deviates from their predicted motion, they may collide with the committed future trajectory of the agent. In such cases, the agent discards the previously committed trajectory and commits to the next available safe trajectory. Case 1 illustrates this behavior: the agent, located at $L_{k+2}$ between the third and fourth safe trajectories, commits to the fourth safe trajectory. In some situations, even the next available safe trajectory is in collision. In such cases (Case 2), the agent generates a contingency trajectory from its current location ($L_{k+2}$), as detailed in Section \ref{['subsubsec:contingency_trajectory']}.
• Figure 3: Illustration of DGP with Dynamic Obstacles: The global path (red arrows) avoids dynamic obstacles. The blue box represents a tracked dynamic obstacle, while the red ellipsoid indicates the estimation uncertainty. The size of the ellipsoid along the $x$, $y$, and $z$ axes reflects the uncertainty magnitude — larger ellipsoids correspond to greater uncertainty, resulting in stronger repulsion forces.
• Figure 4: Static Obstacle Trajectory Push: (a) Original global path (red) from DGP. (b) Look-ahead direction, its discretized points, and the detected occupied points and their mean position. (c) Final adjusted path, pushed away from the obstacle. This mean position is stored and used to push the path at the next iteration.
• Figure 5: Time allocation and parallelization: At iteration $i$, the second largest factor is the fastest successful factor; therefore, at iteration $i+1$, the factors are set such that this second largest factor is the median of the factors. At iteration $i+1$, all the factors failed, so we initialize the factor at iteration $i+2$ with the largest factor of iteration $i+1$ to be the smallest factor at iteration $i+2$. Note that each iteration has a different set of conditions (e.g., obstacles, initial/final states); therefore, we try the largest factor from iteration $i+1$ again at $i+2$. At iteration $i+2$, the second smallest factor is the fastest successful factor; therefore, at iteration $i+3$, the factors are set such that this factor is the median of the factors. This process continues until the agent reaches the goal.