I Introduction
Unmanned aerial vehicles (UAVs) are gaining popularity for many applications such as autonomous inspection, transportation, and photography, in which the UAV is required to navigate along a global reference trajectory to finish its missions. To ensure safety, trajectory replanning is of vital importance to cope with unexpected obstacles.
The problem of trajectory replanning has been investigated actively. Recent works [12, 5, 21, 22, 6] reveal that gradientbased trajectory optimization (GTO), which typically formulates trajectory replanning as a nonlinear optimization problem that trades off smoothness, safety, and dynamically feasibility, is particularly effective for this problem. It is widely applied thanks to its convenience to deform an infeasible trajectory segment into a feasible one, with very high efficiency and low memory requirement.
Despite its advantages, GTObased replanning is cursed by the issue of local minima. The issue arises from the collision cost of the optimization, which should be evaluated according to the structure of the environment. Since there are multiple safe and unsafe regions, this cost is nonconvex by nature. This issue could cause fatal crashes since it frequently makes the trajectory get stuck in infeasible local minima and results in replanning failure. Besides, it also leads to the lack of optimality guarantee, as only a small fraction of solution space around the initial trajectory is searched. Consequently, the socalled local optimal trajectory is usually unsatisfactory for smooth and safe flights. Researchers have realized this critical issue and employed strategies like random restart [12] and iterative refinement [6] to relieve it. Nonetheless, the problem is not resolved essentially and prohibits GTO to be applied to more challenging scenarios such as aggressive flight.
In this paper, the local minima problem is addressed systematically by a new GTObased replanning method, which comprises of a pathguided optimization (PGO), an efficient algorithm to discover topologically distinct paths, and the parallel trajectory optimization guided by the paths. Firstly, we answer the question of how infeasible local minima of GTO can be reliably prevented. The typical reasons for infeasible local minima are investigated. Based on them, we propose PGO in which a geometric path is utilized to guide the optimization effectively so that the success of replanning is guaranteed. Secondly, we answer how optimality of the replanning can be improved considerably, with only minor computational overhead. We propose an efficient samplingbased topological path searching approach to extract a comprehensive set of paths that capture the structure of the environment. With the guidance of several carefully selected paths, PGO is invoked to explore the solution space more thoroughly. It consistently yields better replanning than previous methods, while the total computation time is comparable.
We conduct extensive benchmark comparisons with stateoftheart methods and challenging realworld experiments to validate our proposed replanning method. Results show that it is superior to previous ones in terms of significantly higher success rate and stronger optimality guarantee, with only slightly longer computational time induced by the topological path searching. The contributions of this paper are summarized as follows:
1) A robust optimization approach for realtime trajectory replanning named PGO, to boost the success rate of the replanning.
2) An efficient topological path searching algorithm, and its integration with the proposed PGO, to search the solution space more thoroughly and yield better replanning.
3) Benchmark comparisons and realworld experiments that validate the performance of our proposed method. The source code of our implementation will be released.
Ii Related Work
Iia Gradientbased Trajectory Optimization
GTO is one of the major trajectory generation approaches, which formulates the problem as a nonlinear optimization that minimizes an objective function. Interest in this method is revived recently by[23], which generates discretetime trajectories by minimizing its smoothness and collision costs using covariant gradient descent. [9] has a similar formulation, but solve the problem by sampling neighboring candidates iteratively. The stochastic sampling strategy partially overcome the local minima issue but is computationally intensive. [12] extended the method to continuoustime polynomial trajectories to avoid differential errors. It also does random trajectory perturbation and optimization restart for a higher success rate. However, the improvement is insignificant. [5] improve the success rate by providing a highquality initial path, which is found by an informed samplingbased path searching method. However, it only applies to lowspeed flight. In [21], the trajectory is parameterized as a uniform Bspline. It shows that the continuity and locality properties of Bspline are particularly useful for trajectory replanning. [22] further exploits the convex hull property of Bspline and improve the optimization efficiency and robustness by a large margin. However, given a poor initial trajectory in complex environments, this method still suffers. As a result, [6] adopts an iterative postprocess to improve the practical success rate of [22]. By far, local minima still remains a challenge, since no method copes with it essentially. In this paper, we propose PGO, which incorporates a geometric path in the optimization. As the path effectually guide the optimization to escape from infeasible local minima, the planning success rate is guaranteed. Moreover, multiple distinct paths produced by the topological path searching are integrated with the PGO to seek for plentiful locally optimal solutions, which ensures higher trajectory quality.
IiB Topological Path Planning
There are works utilizing the idea of topologically distinct paths for planning, in which paths belonging to different homotopy (homology) [18, 16, 17, 2, 1] or visibility deformation [8] classes are sought. [18] constructs a variant of probabilistic roadmap (PRM) to capture homotopy classes, in which path searching and redundant path filtering are conducted simultaneously. In contrast, [16, 17] firstly creates a PRM or Voronoi diagram, after which a homology equivalence relation based on complex analysis[2] is adopted to filter out redundant paths. These methods only apply to 2D scenarios. To seek for 3D homology classes, [1] exploit the theory of electromagnetism and propose a 3D homology equivalence relation. However, it requires occupied space to be decomposed into ”genus 1” obstacles, which is usually impractical. Besides, capturing only homotopy classes in 3D space is insufficient to encode the set of useful paths, as indicated in [8], since 3D homotopic paths may be too hard to deform into each other. To this end, [8] leverages a visibility deformation roadmap to search for a richer set of useful paths. [13, 3] convert maps built from SLAM systems into sparse graphs representing the topological structure of the environments. [8, 13, 3] focus on global offline planning and is too timeconsuming for online usage. Our topological path searching is conceptually closest to [8], but with a reinvented algorithm for realtime performance.
Iii PathGuided Trajectory Optimization
Iiia Optimization Failure Analysis
Previous work[19] showed that failure of GTO is relevant to unfavorable initialization, i.e., initial paths that pass through obstacles in certain ways usually get stuck. Examples and the underlying reason for this phenomenon are illustrated in Fig.2. Typical GTO methods incorporate the gradients of ESDF in a collision cost to push the trajectory out of obstacles. This cost is combined with the smoothness and dynamic feasibility cost to form an objective function, whose gradients iteratively deform the trajectory into a smooth and safe one. Yet there are some ”valleys” or ”ridges” in the ESDF, around which the gradients differ greatly. Consequently, if a trajectory is in collision and crosses such regions, the gradients of ESDF will change abruptly at some points. This can result in gradients of the objective function pushing different parts of the trajectory in opposing directions and fails the optimization.
Normally, such ”valleys” and ”ridges”, which corresponds to the space that has an identical distance to the surfaces of nearby obstacles, are difficult to avoid, especially in complex environments. Therefore, optimization depending solely on the ESDF fails inevitably at times. To solve the problem, it is essential to introduce extra information that can produce an objective function whose gradients consistently deform the trajectory to the free space.
IiiB Problem Formulation
We propose PGO built upon our previous work [22] that represents trajectories as Bsplines for more efficient cost evaluation. For a trajectory segment in collision, we reparameterize it as a degree uniform Bspline with control points .
PGO consists of two different phases. The first phase generates an intermediate warmup trajectory. As concluded above, external information should be included to effectually deform the trajectory, since solely applying the ESDF could be futile. We employ a geometric guiding path to attract the initial trajectory to the free space (depicted in Fig. 3(a)) since collisionfree paths are readily available from standard methods like A* and RRT*. In our work, the paths are provided by the samplingbased topological path searching (Sect. IV). The firstphase objective function is:
(1) 
where is the smoothness cost of the trajectory, while is the cost to penalize the distance between the guiding path and the Bspline trajectory. As in [22], is designed as a elastic band cost function^{2}^{2}2Only the subset of control points is optimized due to the boundary state constraints of the trajectory. , , and are needed to evaluate the smoothness. that simulates the elastic forces of a sequence of springs:
(2) 
Because the shape of a Bspline is finely controlled by its control points, we utilize this property to simplify the design of . Each control point is assigned with an associated point on the guiding path, which is uniformly sampled along the guiding path. Then is defined as the sum of the squared Euclidean distance between these point pairs:
(3) 
Notably, it is an unconstrained quadratic programming problem, so its optimal solution can be obtained in closed form. It outputs a smooth trajectory in the vicinity of the guiding path. Since the path is already collisionfree, usually the warmup trajectory is also so. Even though it is not completely collisionfree, its major part will be attracted to the free space. At this stage, the gradients of ESDF along the trajectory vary smoothly, and the gradients of the objective function (green arrows in Fig.3(b)) push the trajectory to the free space in consistent directions. Hence, standard GTO methods can be utilized to improve the trajectory.
In the second phase, we adopt our previous Bspline optimization method[22] to further refine the warmup trajectory into a smooth, safe, and dynamically feasible one, whose objective function is:
(4) 
is the collision cost evaluated on the ESDF, which grows rapidly if the trajectory gets close to obstacles. and penalize infeasible velocity and acceleration. The formulations of , , and are simplified based on the convex hull property of Bspline, thanks to which it suffices to constrain the control points of the Bspline to ensure safety and dynamic feasibility, without evaluating expensive line integrals. For brevity, we refer the readers to [22] for detailed formulations.
Although PGO has one more step of optimization compared with previous methods, it can generate better trajectories within shorter time. The firstphase takes only negligible time, but generate a warmup trajectory that is easier to be further refined, which improve the overall efficiency.
Iv Topological Path Searching
Given a geometric guiding path, our PGO method can replan a locally optimal trajectory. However, this trajectory is not necessarily satisfactory, even with the guidance of the shortest path, as seen in Fig. 4(b). Actually, it is difficult to determine the best guiding path, since the paths do not contain high order information (velocity, acceleration, etc.), and can not completely reflect the true motion. Searching a kinodynamic path [4, 11] may suffice, but it takes excessive time to obtain a promising path with boundary state constraints at the start and end of the replanned trajectory.
For a better solution, a variety of paths is required. We propose a samplingbased topological path searching to find a collection of distinct paths to guide the PGO. Although methods [8, 18, 13, 3] are for this problem, none of them runs in realtime in complex 3D environments. We redesign the algorithm carefully to solve this challenging problem in real time.
Iva Topology Equivalence Relation
Although the concept of homotopy is widely used, it captures insufficient useful trajectories in 3D environments, as shown in Fig. 5(a). [8] proposes a more useful relation in 3D space named visibility deformation (VD), but it is computationally expensive for equivalence checking. Based on VD, we define uniform visibility deformation (UVD), which also captures abundant useful trajectories, and is more efficient for equivalence checking.
Definition 1. Two trajectories , parameterized by and satisfying , belong to the same uniform visibility deformation class, if for all , line is collisionfree.
Fig. 5(b) gives an example of three trajectories belonging to two UVD classes. The relation between VD and UVD is depicted in Fig. 6. Both of them define a continuous map between two paths and , in which a point on is transformed to a point on through a straightline. The major difference is that for UVD, point is mapped to where , while for VD does not necessarily equals . In concept, UVD is less general and characterizes subsets of VD classes. Practically, it captures slightly more classes of distinct paths than VD, but is far less expensive ^{3}^{3}3To test VD relation, one should compute a visibility diagram and do path searching within it[8], which has higher complexity than testing UVD. for equivalence checking.
To test UVD relation, one can uniformly discretize to and check collision for lines . For the piecewise straight line paths (as in Alg. 1, Equivalent()), we simply parameterize it uniformly, so that for any except is the join points of two straight lines, .
IvB Topological Roadmap
Alg. 1 is used to construct a UVD roadmap capturing an abundant set of paths from different UVD classes. Unlike standard PRM, which results in many redundant loops, our method generates a more compact roadmap where each UVD class contains just one or a few paths (displayed in Fig. 7).
We introduce two different kinds of graph nodes, namely guard and connector, similar to the VisibilityPRM [20]. The guards are responsible for exploring different part of the free space, and any two guards and are not visible to each other (line is in collision). Before the main loop, two guards are created at the start point and end point . Every time a sampled point is invisible to all other guards, a new guard is created at this point (Line 67). To form paths of the roadmap, connectors are used to connect different guards (Line 719). When a sampled point is visible to exactly two guards, a new connector is created, either to connect the guards to form a topologically distinct connection (Line 1920), or to replace an existing connector to make a shorter path (Line 1617). Limits of time () or sampling number () are set to terminate the loop.
With the UVD roadmap, a depthfirst search augmented by a visited node list is applied to search for the paths between and , similar to [17].
IvC Path Shortening & Pruning
As shown in Fig. 7(a), some paths obtained from Alg. 1 may be detoured. Such paths are unfavorable, since in the first phase of PGO it can deform a trajectory excessively and make it unsmooth. Hence, Alg. 2 find a topologically equivalent shortcut path for each found by the depthfirst search (illustrated in Fig. 8). The algorithm uniformly Discretizes to a set of points . In each iteration, if a point in is invisible from the last point in (Line 3, 4), the center of the first occupied voxel blocking the view of is found (Line 5). This point is then pushed away from obstacles in the direction orthogonal to and coplanar to the ESDF gradient at (Line 6), after which it is appended to (Line 7). The process continues until the last point is reached.
V Realtime Topological Trajectory Replanning
Algorithms in Sect. IV output a fruitful set of paths that can guide trajectory optimization. We integrate them appropriately with the PGO for realtime replanning, as illustrated in Fig. 4. During the flight, a segment of the global trajectory within a specific horizon is checked periodically for safety. Once collisions are detected, topological roadmap construction is triggered within a cube, which is determined by the start and end position of the segment, and specifying the size of the cube. Paths extracted from the roadmap are shortened and pruned (Sect. IVC), after which each of the paths invokes an independent PGO.
Noticeably, the number of alternative UVD classes grows exponentially with the number of obstacles. So, in case of complex environments, it is computationally intractable to do optimization for all paths. For this reason, we only select the first shortest paths. Paths more than times longer than the shortest one are also excluded from optimization. Such strategies bound the complexity and will not miss the potentially optimal solution, because a very long path is very unlikely to result in the optimal trajectory.
Vi Results
Via Benchmark Comparisons
We first compare our local replanning method with two stateoftheart methods Ewok[21] and TRR[6]. Both methods parameterize local trajectory as uniform Bspline and use GTO to do replanning efficiently. TRR further exploits the convex hull property of Bspline to simplify the cost function. We compare all methods in random maps with 3 different densities of obstacles, with 10 random maps for each density, and 50 replanning tasks with random start and end position for each map, as shown in Fig. 9. All methods are initialized with the same straightline reference trajectory computed by [15]. The optimization time of Ewok and TRR is limited to 15 . For ours, the time for topological roadmap construction ^{4}^{4}4The computation time of topological path searching can not be determined exactly, because time for path shortening and pruning varies slightly in different environments. So we determine the roadmap sampling time empirically according to the desired time budget. and trajectory optimization is limited to 3 and 10 . We check for collisions and smoothness of replanned trajectories.
As is shown in Tab. I, our method outperforms both benchmark methods in terms of success rate and smoothness. Our method successfully finds feasible replanning in all environments, while the success rates of others decrease with increasing complexity of environments. Also, our method generates the smoothest trajectories. Our overall computation is only slightly longer, in which 57 is spent on topological path searching and on parallel optimization. It is notable that although less time is spent on optimization, the generated trajectories are better. The reasons are that the proposed twophases optimization (Sect. IIIB) is easier to converge, and that the parallel optimization (Sect. V) explores the solution space more thoroughly.
Density  Method 





Low  Ewok[21]  15.00  88.0  9.0151  
TRR[6]  15.00  90.4  6.5102  
Proposed  5.75 + 10.00  100.0  5.4357  
Medium  Ewok[21]  15.00  81.4  9.5042  
TRR[6]  15.00  85.6  8.3942  
Proposed  6.83 + 10.00  100.0  6.7833  
High  Ewok[21]  15.00  78.8  9.4845  
TRR[6]  15.00  82.6  9.1762  
Proposed  7.05 + 10.00  100.0  7.7038 
ViB Aggressive Autonomous Flights
We conducted aggressive autonomous flight experiments to validate the robustness of our replanning method. The selfdeveloped drone is localized by a robust visualinertial state estimator
[14]. A local mapping framework [7] fuses the depth images from a stereo camera into a volumetric occupancy map and maintains an ESDF for online replanning. We use a geometric controller [10] for trajectory tracking. All modules run on an Intel Core i78550U CPU.The experiments are conducted in complex indoor and outdoor scenes. In each experiment, a straightline global reference trajectory is first generated using the approach [15]. During the flight, local trajectories within a horizon of 9 are replanned to avoid previously unknown obstacles while keeping the drone close to the global trajectory. Aggressive autonomous flights with very limited sensing range in both scenes are quite challenging, since safe trajectories should be generated frequently within extremely short time to cope with sudden and unexpected obstacles. The local trajectories, local maps and velocity profiles of one indoor and outdoor flights are shown in Fig. 10. We refer the readers to the video attachment for more details.
Vii Conclusions
In this paper, we propose a robust trajectory replanning method for autonomous quadrotor navigation. It overcomes local minima with the pathguided optimization, topological path searching and parallel trajectory optimization. Benchmark comparisons show its superior success rate and optimality than stateoftheart methods. Aggressive autonomous flight experiments in indoor and outdoor environments are presented to validate the robustness of our method.
Currently, the performance of topological path searching is satisfactory, but its completeness is not analyzed in detail. Also, we are not completely certain about the theoretical optimality of the replanning method. In the future, we will investigate these problems. We also plan to extend our method to environments with dynamic obstacles.
References
 [1] (2012) Topological constraints in searchbased robot path planning. Autonomous Robots 33 (3), pp. 273–290. Cited by: §IIB.

[2]
(2010)
Searchbased path planning with homotopy class constraints.
In
TwentyFourth AAAI Conference on Artificial Intelligence
, Cited by: §IIB.  [3] (2018) Topomap: topological mapping and navigation based on visual slam maps. In 2018 IEEE International Conference on Robotics and Automation (ICRA), pp. 1–9. Cited by: §IIB, §IV.
 [4] (2018) Trajectory replanning for quadrotors using kinodynamic search and elastic optimization. In 2018 IEEE International Conference on Robotics and Automation (ICRA), pp. 7595–7602. Cited by: §IV.
 [5] (2017Sept) Gradientbased online safe trajectory generation for quadrotor flight in complex environments. In Proc. of the IEEE/RSJ Intl. Conf. on Intell. Robots and Syst.(IROS), pp. 3681–3688. Cited by: §I, §IIA.
 [6] (2019) Teachrepeatreplan: a complete and robust system for aggressive flight in complex environments. arXiv preprint arXiv:1907.00520. Cited by: §I, §I, §IIA, Fig. 9, §VIA, TABLE I.
 [7] (2019) FIESTA: fast incremental euclidean distance fields for online motion planning of aerial robots. arXiv preprint arXiv:1903.02144. Cited by: §VIB.
 [8] (2008) Path deformation roadmaps: compact graphs with useful cycles for motion planning. The International Journal of Robotics Research 27 (1112), pp. 1175–1188. Cited by: §IIB, §IVA, §IV, footnote 3.
 [9] (2011) STOMP: stochastic trajectory optimization for motion planning. In 2011 IEEE international conference on robotics and automation, pp. 4569–4574. Cited by: §IIA.
 [10] (2010Dec.) Geometric tracking control of a quadrotor uav on se (3). In Proc. of the IEEE Control and Decision Conf. (CDC), Atlanta, GA, pp. 5420–5425. Cited by: §VIB.
 [11] (2017Sept) Searchbased motion planning for quadrotors using linear quadratic minimum time control. In Proc. of the IEEE/RSJ Intl. Conf. on Intell. Robots and Syst.(IROS), pp. 2872–2879. Cited by: §IV.
 [12] (2016Oct.) Continuoustime trajectory optimization for online uav replanning. In Proc. of the IEEE/RSJ Intl. Conf. on Intell. Robots and Syst.(IROS), Daejeon, Korea, pp. 5332–5339. Cited by: §I, §I, §IIA.
 [13] (2018) Sparse 3d topological graphs for microaerial vehicle planning. In 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 1–9. Cited by: §IIB, §IV.
 [14] (2017) VINSmono: a robust and versatile monocular visualinertial state estimator. arXiv preprint arXiv:1708.03852. Cited by: §VIB.
 [15] (2013Dec.) Polynomial trajectory planning for aggressive quadrotor flight in dense indoor environments. In Proc. of the Intl. Sym. of Robot. Research (ISRR), pp. 649–666. Cited by: §VIA, §VIB.
 [16] (2015) Planning of multiple robot trajectories in distinctive topologies. In 2015 European Conference on Mobile Robots (ECMR), pp. 1–6. Cited by: §IIB.
 [17] (2017) Integrated online trajectory planning and optimization in distinctive topologies. Robotics and Autonomous Systems 88, pp. 142–153. Cited by: §IIB, §IVB.
 [18] (2002) Capture of homotopy classes with probabilistic road map. In IEEE/RSJ International Conference on Intelligent Robots and Systems, Vol. 3, pp. 2317–2322. Cited by: §IIB, §IV.
 [19] (2014) Motion planning with sequential convex optimization and convex collision checking. The International Journal of Robotics Research 33 (9), pp. 1251–1270. Cited by: §IIIA.
 [20] (2000) Visibilitybased probabilistic roadmaps for motion planning. Advanced Robotics 14 (6), pp. 477–493. Cited by: §IVB.
 [21] (2017) Realtime trajectory replanning for mavs using uniform bsplines and a 3d circular buffer. In 2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 215–222. Cited by: §I, §IIA, Fig. 9, §VIA, TABLE I.
 [22] (2019) Robust and efficient quadrotor trajectory generation for fast autonomous flight. IEEE Robotics and Automation Letters 4 (4), pp. 3529–3536. Cited by: §I, §IIA, §IIIB, §IIIB, §IIIB.
 [23] (2013) Chomp: covariant hamiltonian optimization for motion planning. The International Journal of Robotics Research 32 (910), pp. 1164–1193. Cited by: §IIA.
Comments
There are no comments yet.