Interleaving Graph Search and Trajectory Optimization for Aggressive Quadrotor Flight

Ramkumar Natarajan, Howie Choset, Maxim Likhachev

TL;DR

The INSAT framework combines the global reasoning of discrete graph search with the local precision of trajectory optimization to enable aggressive quadrotor flight through complex, narrow environments. By interleaving these two processes, the algorithm achieves a "best of both worlds" result: it guarantees completeness (up to discretization) while producing smooth, dynamically feasible trajectories that standalone methods often miss.

What Problem Does This Paper Address?

Planning for "aggressive" quadrotor flight—think racing drones zipping through narrow windows at high speeds—is a balancing act between two difficult requirements.

First, you need global reasoning. The environment is full of obstacles (non-convex), meaning a simple local controller will likely get stuck behind a wall. Discrete graph search (like A*) is great at finding a path through a maze, but it suffers from the "curse of dimensionality." To account for a drone's full dynamics (velocity, acceleration, jerk), the search space becomes too massive to compute quickly.

Second, you need dynamic feasibility. A drone can't just turn on a dime; it has to tilt its body to accelerate. Trajectory optimization handles these continuous constraints beautifully but is a "local" method—if you start it with a bad initial guess, it will fail to find a solution in a cluttered room.

The authors address the gap where existing planners either fail to find a path through narrow gaps (due to sparse discretization) or fail to find a path at all in complex layouts (due to local minima).

Methods

The authors introduce INSAT (INterleaved Search And Trajectory optimization). The core innovation is how it splits the workload:

  1. Low-Dimensional Search: The algorithm performs a graph search in a 5D space (position + roll/pitch). This keeps the combinatorial problem manageable.
  2. High-Dimensional Optimization: As the search expands nodes in the 5D graph, it triggers a high-dimensional (12D) trajectory optimizer to calculate the actual cost and feasibility of moving between those points.
  3. Differential Flatness: The paper leverages the "differential flatness" of quadrotors—a mathematical property that allows complex 12D states (including jerk and snap) to be mapped back to simpler "flat" outputs like position and heading.
  4. The Repair Phase: If the optimizer finds that a direct path to a new node is blocked or physically impossible, INSAT doesn't just give up. It enters a "repair phase," iterating through previous waypoints to find a new "seed" for the optimizer, effectively "stitching" together a feasible path.
  5. Completeness: Unlike many hybrid methods, the authors provide a proof that if a feasible "tunnel" exists in the discretized space, INSAT is guaranteed to find it.

Datasets

The researchers evaluated INSAT in high-fidelity simulations (Gazebo) using several environments:

  • Walls and Windows: A custom-generated environment with parallel walls and narrow holes smaller than the quadrotor's diameter, forcing the drone to tilt aggressively to pass through.
  • Willow Garage: A standard 2.5D and 3D benchmark map of a cluttered office space.
  • MIT Stata Center: A complex 3D indoor environment used to test the planner's ability to handle varying obstacle distributions.

Key Insights and Ideas

  • Interleaving vs. Sequential: Most planners do "search then optimize." INSAT does "search while optimizing." This allows the cost of the actual, smooth trajectory to inform the search direction in real-time.
  • Attitude as a Constraint: In narrow gaps, the drone's orientation (attitude) is just as important as its position. By including roll and pitch in the low-dimensional search, the planner "knows" it needs to tilt to fit through a window.
  • Energy Accumulation: One of the most fascinating behaviors observed was the drone performing "periodic swings." In tight spaces where it couldn't accelerate instantly, the planner chose to fly away from the goal briefly to build up the momentum and tilt angle needed to clear a narrow gap—a hallmark of truly aggressive, expert-level piloting.

Key Results

The authors compared INSAT against two baselines: Base-A (a search-based SE(3) planner) and Base-B (a standard polynomial trajectory planner).

  • Success Rate: In the "Walls and Windows" tests, INSAT achieved a 100% success rate, whereas Base-A dropped as low as 24% in highly constrained scenarios. Base-B often failed entirely because it couldn't handle the global topology of the walls.
  • Solution Quality: INSAT consistently produced lower "Solution Costs" (a measure of jerk and time) than the search-based baseline, often by a factor of nearly 2x.
  • Efficiency: While graph search is often slow, INSAT's 3D planning times were competitive. In the MIT Stata Center map, INSAT found solutions in ~5 seconds, while Base-A took over 80 seconds.

Limitations and Future Work

  • Discretization Dependency: While the trajectory optimization is continuous, the underlying search still relies on a grid. If the grid is too coarse, the planner might still miss extremely tight maneuvers.
  • Simulation vs. Reality: The paper focuses on simulation. While Gazebo is high-fidelity, real-world factors like wind gusts, sensor noise, and motor latency weren't the primary focus of this specific study.
  • Generalization: The authors note that while this works for quadrotors due to differential flatness, applying it to systems without this property (like certain walking robots) would require a different mapping technique.

The code for INSAT is open-sourced, providing a robust starting point for researchers looking to push the limits of autonomous drone racing and search-and-rescue.

Summary updated

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.
BibTeX
@article{natarajan2021interleaving,
  author = {Ramkumar Natarajan and Howie Choset and Maxim Likhachev},
  title = {{Interleaving Graph Search and Trajectory Optimization for Aggressive Quadrotor Flight}},
  journal = {IEEE Robotics Autom. Lett.},
  year = {2021},
  url = {https://openreview.net/forum?id=hQILPwo0-0},
}

Loading related content...

Figure