Perception-Aware Time-Optimal Planning for Quadrotor Waypoint Flight

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

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

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

Panel of Figure 1 — 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 ] .

Panel of Figure 3 — Fig. 5: Diagram of quadrotor model with the world and body frames convention.

Panel of Figure 4 — 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 $L$ and its observed size $l$ in pixels. Consequently, we can derive the uncertainty of the distance measurement and use it as an estimate of the corresponding position uncertainty.

Panel of Figure 7 — 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 $xy$ -plane, the camera’s optical axis is assumed to be aligned with the world $x$ -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 $\mathbf{e}^{c}$ and progress error $\mathbf{e}^{l}$ .

	$m$ (kg)	$l$ (m)	$\mathbf{J}_{\text{diag}}$ (g $\cdot$ m 2 )	$(\underline{f},\overline{f})$ (N)	$c_{\tau}$	$\overline{\bm{\omega}}$ (rad s -1 )
RPG	0.7	0.125	[2.4,1.8,3.7]	[0.0, 8.5]	0.033	[10, 10, 6]
EXP	1.0	0.125	[2.5,2.1,4.3]	[0.0, 8.5]	0.013	[10, 10, 6]

TABLE I: Quadrotor configurations

Panel of Figure 11 — 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 Step	Paramter Name	Symbol	Value
Step II	Num. of polynomial pieces	$M_{p}$	5
Step III	Initial sampling time	$\Delta\bar{t}$	2 ms
	Convergence tolerance	$\epsilon_{\text{tol}}$	$1e^{-5}$
	Max. iteration	$N_{\text{max}}$	5000

TABLE II: Parameters of optimization

Panel of Table 13 — 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.

	$\alpha_{\text{max}}\;(^{\circ})$	$\beta_{\text{max}}\;(^{\circ})$	$Z_{\text{min}}\;(\text{m})$	$\theta_{\text{tilt}}\;(^{\circ})$	$\sigma_{u\!/\!v}\;(\text{px})$
RPG	128.1	72.2	0.3	30.0	10.0
EXP	128.1	72.2	0.3	0.0	10.0

TABLE IV: FPV camera configurations

Perception Type	Symbol	Value
Motion Regularization	$w_{\text{jerk}}$	$1e^{-7}$
LA	$w_{\text{LA}}$	0.005
LA	$\lambda_{\text{LA}}$	3.0
FOV	$w_{\text{FOV}}$	0.05
PUM	$w_{\text{PUM}}$	0.0003
PUM	$\lambda_{v}$	10

TABLE V: Parameters of perceptual costs

Panel of Figure 17 — 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.

Panel of Figure 20 — 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.

Panel of Table 22 — TABLE VI: Close-loop execution benchmarking.

Panel of Figure 23 — 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.

Panel of Figure 26 — 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.

UAV Trajectory Optimization

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

See all 48 papers

SPOT: Spatio-Temporal Obstacle-free Trajectory Planning for UAVs in Unknown Dynamic Environments
Feb 1, 2026Astik Srivastava, Thomas J Chackenkulam, Bitla Bhanu Teja +2UAV Navigation Safe Motion Planning
Robotics Research Center, IIIT Hyderabad, India · IIT-BHU, India
Learning Waypoints and Passage Events for Decentralized Multi-Quadrotor Trajectory Planning
Oct 18, 2026Yunhan Shi, Jia Deng, Zehao Jia +2Multi-Robot Motion Planning Multi-Robot Coordination
School of Astronautics, Beihang University, Beijing 100191, China · Astroup(Beijing) Technology Co., Ltd.
Temporal Cascading of Planning and Control for Quadrotor MPC
Dec 13, 2025Rudolf Reiter, Chao Qin, Leonard Bauersfeld +1Robot Trajectory Tracking Aerial Robotics
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

Feb 1, 2026J/K move · Enter open · S save

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

Astik Srivastava, Thomas J Chackenkulam, Bitla Bhanu Teja, Antony Thomas+1

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

UAV Navigation Safe Motion Planning Motion Planning Dynamic Obstacle Avoidance

Open paper AskPDF arXiv

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.

Abstract

UAV Trajectory Optimization

Explore similar work

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

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

Temporal Cascading of Planning and Control for Quadrotor MPC