Efficient and Robust Time-Optimal Trajectory Planning and Control for Agile Quadrotor Flight

Ziyu Zhou, Gang Wang, Jian Sun, Jikai Wang, Jie Chen

TL;DR

This paper introduces a two-layer framework for agile quadrotor flight that slashes the time needed to plan time-optimal trajectories from hours to seconds. By combining a segmented planning strategy with a "time-adaptive" Model Predictive Control (tMPC) that adjusts tracking progress on-the-fly, the authors achieve high-speed racing (10.6 m/s) that is both computationally efficient and robust to real-world disturbances.

What Problem Does This Paper Address?

Agile flight—like drone racing—requires flying at the absolute physical limits of a quadrotor. To do this, you need a time-optimal trajectory: the fastest possible path that respects the drone's complex, underactuated dynamics.

The challenge is twofold:

  1. Planning is slow: Current state-of-the-art methods (like Complementary Progress Constraints or CPC) treat the time at which a drone passes a waypoint as a variable. This makes the math so complex that solving it can take minutes or even hours, making real-time replanning impossible if a gate moves.
  2. Tracking is brittle: When flying at the limit, even tiny errors in the physics model or a gust of wind can cause a standard controller to fail because it's trying to hit a specific point at a specific microsecond that is no longer physically reachable.

Methods

The authors propose a "divide and conquer" approach:

1. Segmented Time-Optimal Planning

Instead of letting the optimizer struggle to decide when the drone hits a waypoint, the authors fix the waypoint nodes. They divide the trajectory into segments between waypoints and assign a specific number of discrete points to each. Crucially, they allow the sampling interval ($dt_i$) for each segment to be an optimization variable. This simplifies the optimization problem enough to be solved in seconds using standard interior-point methods, while still producing trajectories that are nearly as fast as the "perfect" theoretical ones.

2. Time-adaptive Model Predictive Control (tMPC)

Standard MPC tries to minimize the distance between the drone and a reference point at a fixed time. If the drone falls behind, the MPC might demand impossible thrust to "catch up." The proposed tMPC adds a clever twist: it treats the starting time of the reference trajectory ($t_0$) as a variable to be optimized in every control loop. This allows the drone to effectively "slow down" or "speed up" the reference clock based on its actual performance and external disturbances, significantly increasing robustness.

Datasets

The paper does not use a traditional static dataset. Instead, it evaluates the system through:

  • Simulations: Using the drone configuration parameters from Foehn et al. (a standard benchmark in the field).
  • Real-world Experiments: A physical quadrotor (PX4-Vision frame with an Intel NUC11) flying through a course of five 0.8m diameter circles in an indoor space equipped with an OptiTrack motion capture system.

Key Insights and Ideas

  • The "Good Enough" Initialization: The authors found that solving a "warm-up" version of the problem (using penalty functions for constraints) provides an excellent starting point for the main optimizer, preventing it from getting stuck in local minima.
  • Temporal Flexibility vs. Spatial Accuracy: By allowing the sampling time ($dt$) to vary per segment, the planner gains the flexibility to find the physical limits of the drone without the massive overhead of progress-measure variables used in previous work.
  • Clock Synchronization: The tMPC's ability to shift the reference trajectory in time acts as a buffer against model mismatch. If the drone is heavier than expected, the tMPC simply "drags" the reference clock back slightly rather than crashing while trying to maintain an impossible schedule.

Key Results

  • Massive Speedup: For an 8-waypoint course, the proposed method solved the planning problem in roughly 1.7 seconds, compared to 2,350 seconds (nearly 40 minutes) for the state-of-the-art CPC method.
  • High Performance: In real-world tests, the drone reached a maximum speed of 10.6 m/s with a tracking error of less than 0.22m.
  • Robustness: In simulations with model mismatches (e.g., changing the drone's mass or drag coefficients), the tMPC maintained high accuracy and stability where a standard MPC failed or significantly deviated.
  • Dynamic Replanning: The system successfully replanned and tracked a new trajectory in just 0.12 seconds when a waypoint (a hoop held by a person) was moved mid-flight.

Limitations and Future Work

  • Yaw Constraints: The current model does not explicitly constrain the quadrotor's yaw angle. In many applications (like filming or search and rescue), the orientation of the camera is critical.
  • External Localization: The real-world experiments relied on an OptiTrack system. The authors acknowledge that moving toward onboard, sensor-based state estimation (like vision-based systems) is a necessary next step for "in the wild" applications.
  • Obstacle Avoidance: While the planner handles waypoints expertly, it does not yet incorporate general collision avoidance for arbitrary obstacles in the environment.

Summary updated

Abstract

Agile quadrotor flight relies on rapidly planning and accurately tracking time-optimal trajectories, a technology critical to their application in the wild. However, the computational burden of computing time-optimal trajectories based on the full quadrotor dynamics (typically on the order of minutes or even hours) can hinder its ability to respond quickly to changing scenarios. Additionally, modeling errors and external disturbances can lead to deviations from the desired trajectory during tracking in real time. This letter proposes a novel approach to computing time-optimal trajectories, by fixing the nodes with waypoint constraints and adopting separate sampling intervals for trajectories between waypoints, which significantly accelerates trajectory planning. Furthermore, the planned paths are tracked via a time-adaptive model predictive control scheme whose allocated tracking time can be adaptively adjusted on-the-fly, therefore enhancing the tracking accuracy and robustness. We evaluate our approach through simulations and experimentally validate its performance in dynamic waypoint scenarios for time-optimal trajectory replanning and trajectory tracking.
BibTeX
@article{zhou2023efficient,
  author = {Ziyu Zhou and Gang Wang and Jian Sun and Jikai Wang and Jie Chen},
  title = {{Efficient and Robust Time-Optimal Trajectory Planning and Control for Agile Quadrotor Flight}},
  journal = {IEEE Robotics Autom. Lett.},
  year = {2023},
  url = {https://openreview.net/forum?id=73PnizjkEO},
}

Loading related content...

Figure