Source-linked AI summary
Robust Real-time UAV Replanning Using Guided Gradient-based Optimization and Topological Paths
Boyu Zhou, Fei Gao, Jie Pan, Shaojie Shen
TL;DR
GTO-based UAV replanning can fail in infeasible local minima, compromising safety and trajectory quality. The paper combines path-guided optimization with topological path searching and parallel trajectory optimization, yielding superior benchmark replanning and robust aggressive-flight performance.
Problem
GTO-based replanning suffers from local minima that can produce infeasible trajectories and unsatisfactory smoothness, while selecting a best guiding path is difficult.
Method
The method uses PGO with efficiently searched topologically distinct paths, running independent guided optimizations to explore the solution space more thoroughly.
Results
Benchmark comparisons report higher success rate, smoother trajectories, and better replanning than competing methods, with only slightly longer computation.
Takeaways & Limitations
The combined approach supports robust real-time quadrotor replanning in challenging aggressive autonomous flights.
Takeaways & Limitations
The completeness of topological path searching and the theoretical optimality of replanning are not fully analyzed, and dynamic obstacles remain future work.
Abstract
from arXiv · showhide
Gradient-based trajectory optimization (GTO) has gained wide popularity for quadrotor trajectory replanning. However, it suffers from local minima, which is not only fatal to safety but also unfavorable for smooth navigation. In this paper, we propose a replanning method based on GTO addressing this issue systematically. A path-guided optimization (PGO) approach is devised to tackle infeasible local minima, which improves the replanning success rate significantly. A topological path searching algorithm is developed to capture a collection of distinct useful paths in 3-D environments, each of which then guides an independent trajectory optimization. It activates a more comprehensive exploration of the solution space and output superior replanned trajectories. Benchmark evaluation shows that our method outplays state-of-the-art methods regarding replanning success rate and optimality. Challenging experiments of aggressive autonomous flight are presented to demonstrate the robustness of our method. We will release our implementation as an open-source package.
I. INTRODUCTION
GTO enables efficient UAV trajectory replanning but can become trapped in infeasible local minima, threatening safety and smoothness. The paper proposes PGO plus topological path search and parallel optimization to explore solutions more thoroughly and improve replanning.
- Background: GTO efficiently deforms infeasible trajectory segments while trading off smoothness, safety, and dynamic feasibility.It is widely used because it requires low memory and offers high computational efficiency.
- Problem: Non-convex collision costs can trap GTO in infeasible local minima, causing replanning failure and limiting optimality.The issue arises because environments contain multiple safe and unsafe regions, while only a small solution-space neighborhood is searched.
- Contributions: PGO uses a geometric guiding path to reliably prevent infeasible local minima and boost replanning success.The method investigates typical failure causes and guides optimization toward feasible trajectories.
- Contributions: Topological path searching extracts distinct paths, and parallel PGO optimizations use them to explore the solution space more thoroughly.The resulting computation time is reported as comparable to previous methods while producing better replanning.
- Evaluation: Benchmark comparisons and real-world aggressive-flight experiments validate higher success rate, stronger optimality, and robustness, with only slightly longer computation.The implementation is planned for open-source release.
II. RELATED WORK
Related work develops GTO, topological path planning, and analyses of GTO failures. Existing path-class representations can miss useful 3-D alternatives or impose impractical computational and modeling requirements.
- Gradient-based trajectory optimization: GTO formulates trajectory generation as nonlinear optimization over smoothness and collision costs, using gradient descent or iterative neighboring-candidate sampling.Stochastic sampling can partially mitigate local minima but is computationally intensive.
- Topological path planning: Topological planners search paths across homotopy, homology, or visibility-deformation classes using structures such as PRMs and Voronoi diagrams.These approaches differ in how they construct and filter equivalent paths.
- GTO failure modes: GTO failures occur when ESDF gradients push adjacent trajectory parts in opposing directions across environmental valleys or ridges.This explains how unfavorable initialization can leave trajectories stuck during optimization.
III. PATH-GUIDED TRAJECTORY OPTIMIZATION
The paper addresses GTO's infeasible local minima by first guiding a colliding B-spline trajectory toward free space, then refining it with ESDF-based optimization. This two-phase PGO improves trajectory quality and efficiency while supporting safety and dynamic feasibility.
- Optimization Failure Analysis: GTO can fail when ESDF-based optimization follows unfavorable valleys and ridges, leaving trajectories trapped in infeasible local minima.These failures arise because collision costs are non-convex and ESDF gradients may not consistently direct trajectories toward free space.
- Path-Guided Optimization: PGO first uses a geometric guiding path to attract the original B-spline trajectory into free space and generate a warmup trajectory.The guiding path is incorporated through a cost penalizing distances between uniformly paired B-spline control points and path points.
- Path-Guided Optimization: The first-phase objective combines a smoothness cost with a guiding-path distance cost, while optimizing the B-spline control points under boundary-state constraints.The smoothness term uses an elastic-band formulation, and only the permitted subset of control points is optimized.
- Trajectory Refinement: The warmup trajectory is then refined with ESDF collision costs plus velocity and acceleration penalties to obtain a smooth, safe, dynamically feasible trajectory.B-spline convex-hull properties allow safety and dynamic-feasibility constraints to be enforced through control points without expensive line integrals.
- Performance: The additional optimization phase incurs negligible first-phase time while producing better trajectories within shorter overall time.The warmup trajectory makes subsequent refinement easier, improving overall efficiency despite adding an optimization step.
IV. TOPOLOGICAL PATH SEARCHING
The method searches for multiple distinct paths rather than relying on a single guiding path, because shortest-path guidance may produce unsatisfactory motion. It redesigns topological path searching for real-time use in complex 3-D environments.
- Motivation: A single geometric guiding path can yield a locally optimal but unsatisfactory trajectory because paths omit high-order motion information.The shortest path may induce sharp turns, while kinodynamic path searching is described as too slow under boundary-state constraints.
- Topological Path Searching: The proposed sampling-based topological search finds a collection of distinct paths to guide PGO and broaden the available trajectory solutions.Extracted paths are shortened and pruned before guiding parallel trajectory optimizations.
- Real-Time Search: Existing topological-path methods do not run in real time in complex 3-D environments, motivating a careful redesign of the search algorithm.The stated goal is to retain useful path diversity while meeting real-time replanning demands.
A. Topology Equivalence Relation
UVD is introduced as a more efficient alternative to VD for capturing useful trajectories in 3-D environments, where homotopy alone is insufficient.
- UVD captures abundant useful trajectories in 3-D environments while making equivalence checking more efficient than VD.
- Algorithm 1 constructs the topological roadmap used to represent paths under the proposed equivalence relation.
2 AddGuard(G, s), AddGuard(G, g)
The method defines UVD by requiring synchronized point correspondence between trajectories and tests equivalence through uniformly parameterized paths.
- Two trajectories belong to the same UVD class when they share endpoints and corresponding points remain connected by collision-free line segments.
- The comparison illustrates that associated points share parameter s in UVD but not necessarily in VD.
- UVD maps points with equal parameter s, whereas VD permits different parameters and is more general.
- UVD equivalence is tested by uniformly discretizing s in [0, 1] and checking collision along corresponding connecting lines.
B. Topological Roadmap
The topological roadmap uses guards and connectors to compactly represent paths from distinct UVD classes, then searches the roadmap for routes between the start and goal.
- The roadmap captures many paths from different UVD classes while avoiding the redundant loops produced by standard PRM.
- Guards explore distinct free-space regions, while connectors form roadmap paths between guards.
- A depth-first search with a visited-node list finds paths between the start and goal in the UVD roadmap.
C. Path Shortening & Pruning
The method shortens detoured roadmap paths and removes redundant routes while preserving topological distinctness for trajectory optimization.
- Detoured paths are shortened because they can cause excessive deformation and unsmooth trajectories during PGO.
- The roadmap figure shows guards occupying regions, connectors forming shorter connections, and paths grouped into UVD classes.
- The shortcut procedure discretizes each path, checks visibility, and adds pushed blocking-voxel centers as new waypoints when visibility fails.
- Paths are compared for equivalence so that only topologically distinct routes remain.
- Algorithm 2 is designated for finding a shortcut path for each roadmap path.
V. REAL-TIME TOPOLOGICAL TRAJECTORY REPLANNING
The replanning pipeline selects a bounded set of short topological paths and uses them to guide parallel trajectory optimization. Benchmark comparisons report feasible replanning in all tested environments, smoother trajectories, and only slightly longer computation.
- Pipeline: The system triggers topological roadmap construction when collisions are detected in a local global-trajectory segment.The roadmap is built within a cube determined by the segment endpoints and size parameters (r_x, r_y, r_z).
- Path selection: Because alternative UVD classes grow exponentially with obstacle count, the method retains only the first K_max shortest paths and excludes paths exceeding r_max times the shortest path.These bounds limit complexity while excluding paths considered unlikely to yield the optimum.
- Benchmark setup: The evaluation compares the proposed method with Ewok and TRR across three obstacle densities using 500 random replanning tasks per density.All methods use uniform B-spline local trajectories and gradient-based optimization; TRR additionally exploits the B-spline convex-hull property.
- Results: The proposed method finds feasible replanning in all tested environments and produces the smoothest trajectories, whereas benchmark success rates decline as environment complexity increases.The reported overall computation is only slightly longer, with 5–7 ms for topological path searching and 10 ms for parallel optimization.
- Results: Parallel optimization explores the solution space more thoroughly, while the two-phase optimization is easier to converge despite less time being spent on optimization.The table reports comparisons of replanning methods, including collision and smoothness evaluation.
B. Aggressive Autonomous Flights
Aggressive autonomous flights were tested in complex indoor and outdoor scenes with limited sensing and frequent local replanning. The experiments support the method’s robustness, while the paper identifies incomplete analysis of topological-search completeness and theoretical optimality as limitations.
- System setup: The indoor and outdoor flights used visual-inertial localization, stereo-depth volumetric mapping with an ESDF, and geometric trajectory tracking.All modules ran on an Intel Core i7-8550U CPU.
- Flight behavior: Topologically distinct candidate trajectories were generated to avoid obstacles while staying close to the global reference, after which the best trajectory was selected and executed.The figure describes this process for both indoor and outdoor environments.
- Experimental setting: Experiments replanned 9 m local trajectories around previously unknown obstacles while keeping the drone close to a straight-line global reference.The flights used very limited sensing ranges, requiring safe trajectories to be generated frequently.
- Validation: The authors conducted aggressive autonomous flight experiments in indoor and outdoor scenes to validate the replanning method’s robustness.The conclusion links this validation to the proposed combination of path-guided optimization, topological path searching, and parallel trajectory optimization.
- Limitations: The completeness of topological path searching has not been analyzed in detail, and the method’s theoretical optimality remains uncertain.The authors plan to extend the method to dynamic obstacles in future work.