Abstract
Quadrotors can achieve aggressive flight by tracking complex maneuvers and rapidly changing directions. Planning for aggressive flight with trajectory optimization could be incredibly fast, even in higher dimensions, and can account for dynamics of the quadrotor, however, only provides a locally optimal solution. On the other hand, planning with discrete graph search can handle non-convex spaces to guarantee optimality but suffers from exponential complexity with the dimension of search. We introduce a framework for aggressive quadrotor trajectory generation with global reasoning capabilities that combines the best of trajectory optimization and discrete graph search. Specifically, we develop a novel algorithmic framework that interleaves these two methods to complement each other and generate trajectories with provable guarantees on completeness up to discretization. We demonstrate and quantitatively analyze the performance of our algorithm in challenging simulation environments with narrow gaps that create severe attitude constraints and push the dynamic capabilities of the quadrotor. Experiments show the benefits of the proposed algorithmic framework over standalone trajectory optimization and graph search-based planning techniques for aggressive quadrotor flight.
Introduction
Planning for "aggressive" quadrotor flight—such as racing through narrow apertures at high velocities—requires a delicate balance between global reasoning and dynamic feasibility. The environment is typically non-convex, filled with obstacles that can cause local trajectory optimizers to fail if they lack a globally informed initial guess. Conversely, while discrete graph search algorithms like A* excel at navigating complex topologies, they struggle with the "curse of dimensionality" when forced to account for a drone's full 12D state (position, velocity, acceleration, jerk, and snap).
To fly aggressively, a quadrotor must tilt its body significantly to achieve high accelerations. This means attitude (roll and pitch) becomes a critical constraint, especially when passing through narrow gaps where the drone's physical dimensions might exceed the gap's width unless it is oriented correctly.
The INSAT Framework
The authors propose INSAT (INterleaved Search And Trajectory optimization) to bridge the gap between discrete search and continuous optimization. The framework operates by splitting the planning workload into two coupled processes:
- Low-Dimensional Search: The algorithm performs a graph search in a reduced 5D space (position plus roll and pitch). This keeps the combinatorial complexity manageable while ensuring the planner "understands" the need for specific orientations.
- High-Dimensional Optimization: As the search expands nodes in the 5D graph, it invokes a high-dimensional (12D) trajectory optimizer. This optimizer calculates the actual cost and physical feasibility of transitions between nodes, leveraging the "differential flatness" of quadrotors to map high-order states back to flat outputs.
A key feature of INSAT is the Repair Phase. If the optimizer determines that a direct path to a new node is obstructed or dynamically impossible, the algorithm does not discard the node. Instead, it iterates through previous waypoints to find a new seed for the optimizer, effectively "stitching" together a feasible path. This ensures completeness up to the resolution of the discretization.
Experimental Evaluation
INSAT was evaluated in high-fidelity Gazebo simulations across several challenging environments, including standard benchmarks like the Willow Garage and the MIT Stata Center, as well as custom "Walls and Windows" scenarios featuring narrow holes smaller than the quadrotor's diameter.
Key Results
The performance of INSAT was compared against a search-based SE(3) planner (Base-A) and a standard polynomial trajectory planner (Base-B).
- Success Rate: In the "Walls and Windows" tests, INSAT achieved a 100% success rate. In contrast, Base-A's success rate dropped to 24% in highly constrained cases, and Base-B often failed to find any solution due to the lack of global reasoning.
- Solution Quality: INSAT consistently produced trajectories with lower costs (minimizing jerk and time) compared to the search-based baseline, often by a factor of 2x.
- Computational Efficiency: Despite the overhead of interleaving optimization, INSAT remained competitive. In the MIT Stata Center map, INSAT found solutions in approximately 5 seconds, whereas Base-A required over 80 seconds.
Discussion and Conclusion
One of the most notable behaviors observed was "energy accumulation." In extremely tight spaces, the planner would direct the drone to fly away from the goal momentarily to build the momentum and tilt angle required to clear a narrow gap—a maneuver characteristic of expert human piloting.
While INSAT provides a robust framework for aggressive flight, it remains dependent on the underlying discretization. Extremely fine maneuvers might still be missed if the grid is too coarse. The INSAT framework demonstrates that interleaving search and optimization allows for the generation of smooth, dynamically feasible trajectories that maintain global optimality guarantees, pushing the boundaries of autonomous quadrotor capabilities.
Summary updated