Safe Interval Motion Planning for Quadrotors in Dynamic Environments

TL;DR

This work tackles quadrotor motion planning in highly dynamic 3D environments where moving obstacles render the problem non-convex and challenging for real-time solutions. It introduces a two-stage Safe Interval Motion Planning framework that combines a front-end dynamic connected visibility graph over edge-safe intervals with a back-end B-spline trajectory optimization inside spatial-temporal corridors, augmented by Uniform Temporal Visibility Deformation (UTVD) to assess topology across time. Key contributions include Safe Intervals Generation for Edges, a Dynamic Connected Visibility Graph with Guards and Connectors, UTVD-based topology evaluation, and probabilistic completeness and finite-horizon optimality analyses, validated by extensive simulations and hardware experiments that achieve over 95% success across density levels. The approach yields smoother, dynamically feasible trajectories suitable for practical deployment in highly dynamic settings, with potential for integration into onboard perception for enhanced robustness.

Abstract

Trajectory generation in dynamic environments presents a significant challenge for quadrotors, particularly due to the non-convexity in the spatial-temporal domain. Many existing methods either assume simplified static environments or struggle to produce optimal solutions in real-time. In this work, we propose an efficient safe interval motion planning framework for navigation in dynamic environments. A safe interval refers to a time window during which a specific configuration is safe. Our approach addresses trajectory generation through a two-stage process: a front-end graph search step followed by a back-end gradient-based optimization. We ensure completeness and optimality by constructing a dynamic connected visibility graph and incorporating low-order dynamic bounds within safe intervals and temporal corridors. To avoid local minima, we propose a Uniform Temporal Visibility Deformation (UTVD) for the complete evaluation of spatial-temporal topological equivalence. We represent trajectories with B-Spline curves and apply gradient-based optimization to navigate around static and moving obstacles within spatial-temporal corridors. Through simulation and real-world experiments, we show that our method can achieve a success rate of over 95% in environments with different density levels, exceeding the performance of other approaches, demonstrating its potential for practical deployment in highly dynamic environments.

Figures (7)

• Figure 1: A representative experiment of dynamic obstacle avoidance. The quadrotor plans a trajectory (purple) and navigates between static obstacles (cylinders) and moving obstacles (ground robots).
• Figure 2: In this example, red grids are occupied by moving obstacles at specific time durations, colored solid line segments represent multiple paths from start to end vertex, while red dash lines represent collisions detected, otherwise, they are black. According to the definition of UTVD, paths in red and blue belong to the same UTVD class. The path in green is in a different UTVD class.
• Figure 3: (a) Illustration of dynamically connected visibility graphs in dynamic environments. A dynamic connected graph has edges that are valid in their safe intervals. Valid paths in distinct UTVD classes are shown in purple and green. (b) Simulation result of the front-end graph search, the red objects are moving obstacles with trajectories in dash lines, and the black ones are static obstacles. The graph is shown in green, and multiple distinct paths are represented in yellow.
• Figure 4: Demonstration of spatial-temporal corridors inflation in one dimension, (a) denoted by position (x) versus time (t). The initial path and initial spatial-temporal corridors are represented in blue and green, static obstacles are shown in black while moving obstacles 1 and 2 are colored in red. (b) The inflated spatial-temporal corridors are shown in yellow, which are generated by seed points in triangular shapes. The optimized B-spline trajectory with control points is colored in purple.
• Figure 5: Simulation result (top-down view in 3-D environment) of the back-end optimization. Each cuboid is valid within its temporal intervals. The yellow cuboid demonstrates the spatial-temporal corridor.

Theorems & Definitions (3)

• Definition 1
• Definition 2
• Definition 3