cs.ROMar 4, 2026

Perception-Aware Time-Optimal Planning for Quadrotor Waypoint Flight

Authors: Chao Qin, Jiaxu Xing, Rudolf Reiter, Angel Romero, Yifan Lin, Hugh H. -T. Liu, Davide Scaramuzza

Organizations: Authors are with the University of Toronto Institute for Aerospace Studies (UTIAS), Canada

Read PDF
Ask AI
Cite

Copy citation

DOINot available for this paper

Abstract

Agile quadrotor flight pushes the limits of control, actuation, and onboard perception. While time-optimal trajectory planning has been extensively studied, existing approaches typically neglect the tight coupling between vehicle dynamics, environmental geometry, and the visual requirements of onboard state estimation. As a result, trajectories that are dynamically feasible may fail in closed-loop execution due to degraded visual quality. This paper introduces a unified time-optimal trajectory optimization framework for vision-based quadrotors that explicitly incorporates perception constraints alongside full nonlinear dynamics, rotor actuation limits, aerodynamic effects, camera field-of-view constraints, and convex geometric gate representations. The proposed formulation solves minimum-time lap trajectories for arbitrary racetracks with diverse gate shapes and orientations, while remaining numerically robust and computationally efficient. We derive an information-theoretic position uncertainty metric to quantify visual state-estimation quality and integrate it into the planner through three perception objectives: position uncertainty minimization, sequential field-of-view constraints, and look-ahead alignment. This enables systematic exploration of the trade-offs between speed and perceptual reliability. To accurately track the resulting perception-aware trajectories, we develop a model predictive contouring tracking controller that separates lateral and progress errors. Experiments demonstrate real-world flight speeds up to 9.8 m/s with 0.07 m average tracking error, and closed-loop success rates improved from 55% to 100% on a challenging Split-S course. The proposed system provides a scalable benchmark for studying the fundamental limits of perception-aware, time-optimal autonomous flight.

Figures & tables

Fig. 3: Illustrations of three perceptual objectives verified in the proposed framework. The arrows represent the camera optical directions, and the triangles with filled colors indicate the camera’s FOV . (a) In the absence of specific perception objectives, the camera’s orientation is purely governed by time-optimal control, which could be detrimental to visual perception; (b) LA encourages the UAV to look at a future waypoint; (c) FOV keeps one or multiple target landmarks (e.g., the next racing gate) visible; (d) PUM trades off time optimality and information-theoretic position uncertainty for robust closed-loop execution.
Fig. 4: The framework accommodates gates representable by convex shapes, which constitute the majority of gates used in drone racing [ 10 ] .
Fig. 5: Diagram of quadrotor model with the world and body frames convention.
Fig. 6: Two types of geometry information for localization: (a) the relative geometry of multiple landmarks, where each landmark is treated as a single 3D point; or (b) features observable from a single landmark.
Fig. 7: Equidist projection model of fisheye cameras.
Fig. 8: The distance to a planar landmark can be approximately estimated by computing the ratio between the actual landmark size LL and its observed size ll in pixels. Consequently, we can derive the uncertainty of the distance measurement and use it as an estimate of the corresponding position uncertainty.
Fig. 9: Comparison of position uncertainty measures in an environment containing three square gates (black), each featuring four corner points. For each cell in the xyxy -plane, the camera’s optical axis is assumed to be aligned with the world xx -axis. Position uncertainty is calculated using three methods: (a) the sampling-based approach introduced in [ 55 ] , (b) the original FIM -based method ( 41 ), and (c) our proposed approximation ( 49 ). The results demonstrate that our derived measure closely approximates the true uncertainty.
Fig. 10: The optimization strategy proceeds through three stages. Step 1: A polynomial-based approach rapidly generates a sub-time-optimal trajectory. Step 2: increasing segment density approximates the time-optimal solution to establish initial guesses and optimal traversal points. Step 3: A multi-stage multiple-shooting framework refines the trajectory by constraining these points as waypoints and integrating perception objectives.
Fig. 11: Contouring error ec\mathbf{e}^{c} and progress error el\mathbf{e}^{l} .
 mm (kg)ll (m)Jdiag\mathbf{J}_{\text{diag}} (g ⋅\cdot m 2 )(f‾,f‾)(\underline{f},\overline{f}) (N)cτc_{\tau}ω‾\overline{\bm{\omega}} (rad s -1 )
RPG0.70.125[2.4,1.8,3.7][0.0, 8.5]0.033[10, 10, 6]
EXP1.00.125[2.5,2.1,4.3][0.0, 8.5]0.013[10, 10, 6]
TABLE I: Quadrotor configurations
Fig. 12: TOWP trajectory (blue) versus TOGT trajectory (yellow) on the Split-S track. Note that while both trajectories satisfy the single-rotor thrust and body-rate constraints, the TOGT trajectory has more timing advantage as it makes better use of the navigable space of each gate.
 Optimization StepParamter NameSymbolValue
 Step IINum. of polynomial piecesMpM_{p}5
 Step IIIInitial sampling timeΔtˉ\Delta\bar{t}2 ms
Convergence toleranceϵtol\epsilon_{\text{tol}}1e−51e^{-5}
Max. iterationNmaxN_{\text{max}}5000
TABLE II: Parameters of optimization
TABLE III: Trajectory benchmarking in the Split-S track.
Fig. 13: Quantitative comparison of computational cost and optimized trajectory duration. Our TOGT formulation yields the best performance in both metrics.
 αmax  (∘)\alpha_{\text{max}}\;(^{\circ})βmax  (∘)\beta_{\text{max}}\;(^{\circ})Zmin  (m)Z_{\text{min}}\;(\text{m})θtilt  (∘)\theta_{\text{tilt}}\;(^{\circ})σu ⁣/ ⁣v  (px)\sigma_{u\!/\!v}\;(\text{px})
RPG128.172.20.330.010.0
EXP128.172.20.30.010.0
TABLE IV: FPV camera configurations
Perception TypeSymbolValue 
Motion Regularizationwjerkw_{\text{jerk}}1e−71e^{-7} 
LAwLAw_{\text{LA}}0.005 
λLA\lambda_{\text{LA}}3.0 
FOVwFOVw_{\text{FOV}}0.05 
PUMwPUMw_{\text{PUM}}0.0003 
λv\lambda_{v}10 
TABLE V: Parameters of perceptual costs
Fig. 14: Impact of various perception objectives on time-optimal trajectories and the evolution of position uncertainty. The circles represent the uncertainty area at discrete timestamps. Results demonstrate that trajectories optimized with PUM exhibit substantially lower position uncertainty than other methods.
Fig. 15: Distributions of visible landmarks/gates on Split-S track, which reveals that perception-aware flights boost the visibility substantially.
Fig. 16: Distributions of the derived analytical position uncertainty and the actual position uncertainty of the PnP method. To get the PnP uncertainty, we perturb the observed feature coordinates, re-evaluate the PnP problem 60 times, and compute the mean and covariance of the resulting pose estimates.
Fig. 17: Impact of the weight for the FOV constraints on the resulting flight behavior. As the penalty weight increases, the drone prioritizes visibility to the target gate more and eventually reaches a near-zero target-loss percentage. But it causes a longer time to complete the task.
Fig. 18: Closed-loop execution of planned trajectories with LA, FOV, and PUM objectives, tracked using the proposed MPCTC . We observe that the PUM (purple) yields the smoothest tracking trajectory.
TABLE VI: Close-loop execution benchmarking.
Fig. 19: Indoor Split-S track for experiment validation. The quadrotor platform is equipped with an NVIDIA Jetson Orin NX 16GB onboard computer and an IMX219 fisheye camera for image capture. Betaflight firmware is employed for low-level body-rate control.
Fig. 20: Tracking trajectories of standard MPC versus our MPCTC against reference time-optimal trajectories planned with (a) 70% and (b) 100% thrust limit. The standard MPC performance degrades significantly when insufficient margin is reserved to compensate for tracking errors or disturbances
Fig. 21: Executed trajectories with various perception objectives on a real-world Split-S track.
Fig. 22: FPV images of perception-aware time-optimal flights at six different locations. While LA and FOV exhibit similar views, PUM differs from them by looking toward directions where multiple gates are visible instead of only the upcoming one, such as locations L-1 and L-6.
Fig. 23: Landmark visibility and position estimate availability in the real-world experiments.
More in this topic

UAV Trajectory Optimization

UAV trajectory optimization plans feasible, safe flight paths that balance mission goals, energy use, sensing, and communication.

See all 48 papers

Explore similar work

CardsList
Feb 1, 2026

SPOT: Spatio-Temporal Obstacle-free Trajectory Planning for UAVs in Unknown Dynamic Environments

We address the problem of reactive motion planning for quadrotors operating in unknown environments with dynamic obstacles. Our approach leverages a 4-dimensional spatio-temporal planner, integrated with vision-based Safe Flight Corridor (SFC) generation and trajectory optimization. Unlike prior methods that rely on map fusion, our framework is mapless, enabling collision avoidance directly from perception while reducing computational overhead. Dynamic obstacles are detected and tracked using a vision-based object segmentation and tracking pipeline, allowing robust classification of static versus dynamic elements in the scene. To further enhance robustness, we introduce a backup planning module that reactively avoids dynamic obstacles when no direct path to the goal is available, mitigating the risk of collisions during deadlock situations. We validate our method extensively in both simulation and real-world hardware experiments, and benchmark it against state-of-the-art approaches, showing significant advantages for reactive UAV navigation in dynamic, unknown environments.

Astik Srivastava, Thomas J Chackenkulam, Bitla Bhanu Teja +2

Robotics Research Center, IIIT Hyderabad, India · IIT-BHU, India

UAV NavigationSafe Motion PlanningMotion PlanningDynamic Obstacle Avoidance
AskPDF
Oct 18, 2026

Learning Waypoints and Passage Events for Decentralized Multi-Quadrotor Trajectory Planning

Multi-quadrotor passage planning requires coordinated spatial routes and passage times under vehicle dynamics and actuation constraints. Optimizing these decisions jointly couples temporal allocation, nonlinear motion, and inter-vehicle interactions, creating a computational challenge for local trajectory generation. We propose a learning-optimization framework that predicts spatial waypoints and ordered passage events for independently solved quadrotor planners. A common physical time grid aligns the vehicles, with intermediate events fixed by the networks and terminal progress optimized locally to permit different completion metrics. Differentiable sequential convex programming incorporates rigid-body dynamics and rotor-thrust bounds, allowing collision and completion gradients to train both networks through the generated trajectories. In a two-quadrotor evaluation across 24 layouts, the adapted policy passes physical feasibility and sampled separation checks in all five held-out layouts. Their mean completion metric decreases by 1.52% relative to the inherited policy. The median forward time is 21.01 ms on the held-out exchange layout with matching numerical trajectory references. These results demonstrate that learned spatial and event parameters can coordinate independent rigid-body planners while preserving local vehicle constraints in the evaluated scenarios.

Yunhan Shi, Jia Deng, Zehao Jia +2

School of Astronautics, Beihang University, Beijing 100191, China · Astroup(Beijing) Technology Co., Ltd.

Multi-Robot Motion PlanningMulti-Robot CoordinationUAV Trajectory Optimization
AskPDF
Dec 13, 2025

Temporal Cascading of Planning and Control for Quadrotor MPC

Many aerial tasks involving quadrotors demand both instant reactivity and long-horizon planning for obstacle avoidance, energy efficiency, or trajectory tracking. High-fidelity models enable accurate control but are too slow for long horizons. Low-fidelity planners scale but cannot directly control the system, necessitating cascaded architectures. Prevailing hierarchical approaches plan with a simplified model and use a high-fidelity controller for tracking, yet this decomposition is inherently suboptimal. The controller is limited by the coarse plan, and conventional MPC alternatives shorten the horizon to stay real-time feasible. We present UNIQUE, an MPC architecture that replaces this hierarchical stacking with temporal cascading. The planning problem is formulated as the second-tail horizon of a single multi-phase MPC, rather than being solved separately. We align costs across horizons, derive feasibility constraints for the point-mass planning model, and introduce transition constraints that convert high-fidelity states into meaningful low-fidelity states. Parallel point-mass and mixed-integer solvers address nonconvexities while incorporating progressive 3D obstacle smoothing over the planning horizon. In simulations and real flights, under equal computational budgets, UNIQUE improves closed-loop tracking by up to 75% compared with standard MPC and hierarchical baselines. Ablations and Pareto analyses confirm performance gains across variations in horizon, constraint approximations, and smoothing schedules.

Rudolf Reiter, Chao Qin, Leonard Bauersfeld +1

Robotics and Perception Group, Department of Informatics, University of Zurich, Andreasstrasse 15, 8050 Zurich, Switzerland · Institute for Aerospace Studies, University of Toronto, 4925 Dufferin Street, Toronto, ON M3H 5T6, Canada

Robot Trajectory TrackingAerial RoboticsModel Predictive ControlUAV Trajectory OptimizationQuadrotor ControlSafe Motion Planning
AskPDF