Robust Real-time UAV Replanning Using Guided Gradient-based Optimization and Topological Paths

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 gradient-based trajectory optimization (GTO), which typically formulates trajectory replanning as a non-linear 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, GTO-based 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 non-convex 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 so-called 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 GTO-based replanning method, which comprises of a path-guided 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 sampling-based 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.

Fig. 1: A set of topologically distinct paths (left). Multiple trajectories generated with the guidance of different paths (right). Video demonstrating the proposed replanning method in aggressive flights is available at https://www.dropbox.com/sh/shw0k8v6ehxo4dr/AAAVD_2VR2GeR0gLuOK5WKdDa?dl=0.

We conduct extensive benchmark comparisons with state-of-the-art methods and challenging real-world 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 real-time 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 real-world experiments that validate the performance of our proposed method. The source code of our implementation will be released.

Ii Related Work

Ii-a Gradient-based Trajectory Optimization

GTO is one of the major trajectory generation approaches, which formulates the problem as a non-linear optimization that minimizes an objective function. Interest in this method is revived recently by[23], which generates discrete-time 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 continuous-time 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 high-quality initial path, which is found by an informed sampling-based path searching method. However, it only applies to low-speed flight. In [21], the trajectory is parameterized as a uniform B-spline. It shows that the continuity and locality properties of B-spline are particularly useful for trajectory replanning. [22] further exploits the convex hull property of B-spline 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 post-process 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.

Ii-B 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 2-D scenarios. To seek for 3-D homology classes, [1] exploit the theory of electromagnetism and propose a 3-D 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 3-D space is insufficient to encode the set of useful paths, as indicated in [8], since 3-D 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 time-consuming for online usage. Our topological path searching is conceptually closest to [8], but with a reinvented algorithm for real-time performance.

Fig. 2: Typical examples of optimization failure, where adjacent parts of a trajectory are pushed in opposing directions if the trajectory crosses the ”valley” (a) or ”ridge” (b) of the ESDF (denoted by orange dashed lines). Red arrows denote gradients of ESDF and yellow ones are gradients of the objective function.

Iii Path-Guided Trajectory Optimization

Iii-a 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.

Iii-B Problem Formulation

We propose PGO built upon our previous work [22] that represents trajectories as B-splines for more efficient cost evaluation. For a trajectory segment in collision, we reparameterize it as a $p_{b}$ degree uniform B-spline with control points ${Q_{0}, Q_{1}, \dots, Q_{N}}$ .

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 collision-free paths are readily available from standard methods like A* and RRT*. In our work, the paths are provided by the sampling-based topological path searching (Sect. IV). The first-phase objective function is:

f_{p 1} = λ_{1 s} f_{s} + λ_{1 g} f_{g}

(1)

where $f_{s}$ is the smoothness cost of the trajectory, while $f_{g}$ is the cost to penalize the distance between the guiding path and the B-spline trajectory. As in [22], $f_{s}$ is designed as a elastic band cost function²²2Only the subset of control points ${Q_{p_{b}}, Q_{p_{b} + 1}, \dots, Q_{N - p_{b}}}$ is optimized due to the boundary state constraints of the trajectory. $Q_{p_{b} - 2}$ , $Q_{p_{b} - 1}$ , $Q_{N - p_{b} + 1}$ and $Q_{N - p_{b} + 2}$ are needed to evaluate the smoothness. that simulates the elastic forces of a sequence of springs:

f_{s} = N - p_{b} + 1 \sum i = p_{b} - 1 {∥ ∥ Q_{i + 1} - 2 Q_{i} + Q_{i - 1} ∥ ∥}_{i + 1}^{2}

(2)

Because the shape of a B-spline is finely controlled by its control points, we utilize this property to simplify the design of $f_{g}$ . Each control point $Q_{i}$ is assigned with an associated point $G_{i}$ on the guiding path, which is uniformly sampled along the guiding path. Then $f_{g}$ is defined as the sum of the squared Euclidean distance between these point pairs:

f_{g} = N - p_{b} \sum i = p_{b} {∥ Q_{i} - G_{i} ∥}_{i}^{2}

(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 collision-free, usually the warmup trajectory is also so. Even though it is not completely collision-free, 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 B-spline optimization method[22] to further refine the warmup trajectory into a smooth, safe, and dynamically feasible one, whose objective function is:

f_{p 2} = λ_{2 s} f_{s} + λ_{2 c} f_{c} + λ_{2 d} (f_{v} + f_{a})

(4)

$f_{c}$ is the collision cost evaluated on the ESDF, which grows rapidly if the trajectory gets close to obstacles. $f_{v}$ and $f_{a}$ penalize infeasible velocity and acceleration. The formulations of $f_{c}$ , $f_{v}$ , and $f_{a}$ are simplified based on the convex hull property of B-spline, thanks to which it suffices to constrain the control points of the B-spline 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 first-phase 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.

(a) The four trajectories are equivalent under the definition of homotopy, but represent substantially different motions of a quadrotor.

For a better solution, a variety of paths is required. We propose a sampling-based 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 real-time in complex 3-D environments. We redesign the algorithm carefully to solve this challenging problem in real time.

Iv-a Topology Equivalence Relation

Although the concept of homotopy is widely used, it captures insufficient useful trajectories in 3-D environments, as shown in Fig. 5(a). [8] proposes a more useful relation in 3-D 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.

1 Initialize()

AddGuard (G, s), AddGuard (G, g)

while $t \leq t_{m a x} \land N_{s a m p l e} \leq N_{m a x}$ do

p_{s} \leftarrow {Sample} ()

g_{v i s} \leftarrow {VisibleGuards} (G, p_{s})

if $g_{v i s} . {size} () == 0$ then

AddGuard (G, p_{s})

4 if $g_{v i s} . {size} () == 2$ then

p a t h_{1} \leftarrow {Path} (g_{v i s} [0], p_{s}, g_{v i s} [1])

d i s t i n c t \leftarrow T r u e

N_{s} \leftarrow {SharedNeighbors} (G, g_{v i s} [0], g_{v i s} [1])

for ${each} n_{s} \in N_{s}$ do

p a t h_{2} \leftarrow {Path} (g_{v i s} [0], n_{s}, g_{v i s} [1])

if ${Equivalent} (p a t h_{1}, p a t h_{2})$ then

d i s t i n c t \leftarrow F a l s e

if ${Len} (p a t h_{1}) < {Len} (p a t h_{2})$ then

{Replace} (G, p_{s}, n_{s})

9 break

11 if $d i s t i n c t$ then

AddConnector (G, p_{s}, g_{v i s} [0], g_{v i s} [1])

Algorithm 1 Topological Roadmap

Definition 1. Two trajectories $τ_{1} (s)$ , $τ_{2} (s)$ parameterized by $s \in [0, 1]$ and satisfying $τ_{1} (0) = τ_{2} (0), τ_{1} (1) = τ_{2} (1)$ , belong to the same uniform visibility deformation class, if for all $s$ , line $¯ ¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯ ¯ τ_{1} (s) τ_{2} (s)$ is collision-free.

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 $τ_{1} (s)$ and $τ_{2} (s)$ , in which a point on $τ_{1} (s)$ is transformed to a point on $τ_{2} (s)$ through a straight-line. The major difference is that for UVD, point $τ_{1} (s_{1})$ is mapped to $τ_{2} (s_{2})$ where $s_{1} = s_{2}$ , while for VD $s_{1}$ does not necessarily equals $s_{2}$ . 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 ³³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 $s \in [0, 1]$ to $s_{i} = i / K, i = 0, 1, . . ., K$ and check collision for lines $¯ ¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯¯ ¯ τ_{1} (s_{i}) τ_{2} (s_{i})$ . For the piece-wise straight line paths (as in Alg. 1, Equivalent()), we simply parameterize it uniformly, so that for any $s$ except $τ (s)$ is the join points of two straight lines, $∥ ∥ \frac{d τ (s)}{d s} ∥ ∥ = c o n s t$ .

Fig. 6: Comparison between UVD (left) and VD (right). Each red point on one path is transformed to a point (green) on the other path. Any two associated points correspond to the same parameter $s$ in UVD, but not in VD.

Iv-B Topological Roadmap

Alg. 1 is used to construct a UVD roadmap $G$ 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 Visibility-PRM [20]. The guards are responsible for exploring different part of the free space, and any two guards $g_{1}$ and $g_{2}$ are not visible to each other (line $¯ ¯¯¯¯¯¯¯¯ ¯ g_{1} g_{2}$ is in collision). Before the main loop, two guards are created at the start point $s$ and end point $g$ . Every time a sampled point is invisible to all other guards, a new guard is created at this point (Line 6-7). To form paths of the roadmap, connectors are used to connect different guards (Line 7-19). 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 19-20), or to replace an existing connector to make a shorter path (Line 16-17). Limits of time ( $t_{m a x}$ ) or sampling number ( $N_{m a x}$ ) are set to terminate the loop.

With the UVD roadmap, a depth-first search augmented by a visited node list is applied to search for the paths between $s$ and $g$ , similar to [17].

Iv-C Path Shortening & Pruning

Fig. 8: A detoured and long path (green line) is shortened. For each discretized point on the original path (green point), its visibility to the last waypoint of the shortened path (red points) is checked. The center points of the blocking voxel (dashed) are pushed and appended as new waypoints.

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 $P_{s}$ for each $P_{r}$ found by the depth-first search (illustrated in Fig. 8). The algorithm uniformly Discretizes $P_{r}$ to a set of points $P_{d}$ . In each iteration, if a point $p_{d}$ in $P_{d}$ is invisible from the last point in $P_{s}$ (Line 3, 4), the center of the first occupied voxel blocking the view of $P_{s} . back ()$ is found (Line 5). This point is then pushed away from obstacles in the direction orthogonal to $l_{d}$ and coplanar to the ESDF gradient at $p_{b}$ (Line 6), after which it is appended to $P_{s}$ (Line 7). The process continues until the last point is reached.

Although in Alg. 1, redundant connection between two guards are avoided, there may exist a small number of redundant paths between $s$ and $g$ (Fig. 7(a)). To completely exclude repeated ones, we check the equivalence between any two paths and only preserve topologically distinct ones.

P_{d} \leftarrow {Discretize} (P_{r}), P_{s} \leftarrow {P_{d} . {front} ()}

for ${each} p_{d} \in P_{d}$ do

l_{d} \leftarrow {Line} (P_{s} . {back} (), p_{d})

if $\neg {LineVisib} (l_{d})$ then

p_{b} \leftarrow {BlockPoint} (l_{d})

p_{o} \leftarrow {PushAwayObs} (p_{b}, l_{d})

Ps.{push\_back}(po)

Ps.{push\_back}(Pd.{back}())

Algorithm 2 Finding a shortcut path

P_{s}

for

P_{r}

V Real-time 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 real-time 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 $(r_{x}, r_{y}, r_{z})$ specifying the size of the cube. Paths extracted from the roadmap are shortened and pruned (Sect. IV-C), 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 $K_{m a x}$ shortest paths. Paths more than $r_{m a x}$ 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

Vi-a Benchmark Comparisons

We first compare our local replanning method with two state-of-the-art methods Ewok[21] and TRR[6]. Both methods parameterize local trajectory as uniform B-spline and use GTO to do replanning efficiently. TRR further exploits the convex hull property of B-spline 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 straight-line reference trajectory computed by [15]. The optimization time of Ewok and TRR is limited to 15 $m s$ . For ours, the time for topological roadmap construction ⁴⁴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 $m s$ and 10 $m s$ . 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 5-7 $m s$ is spent on topological path searching and $10 m s$ 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 two-phases optimization (Sect. III-B) is easier to converge, and that the parallel optimization (Sect. V) explores the solution space more thoroughly.

Fig. 9: Benchmark comparisons of the proposed method (red) with Ewok[21] (green) and TRR[6] (blue) in environments of different complexities.

Density

Method

Comp.

time (

m s

)

Success

rate (%)

Smoothness

(

m^{2} / s^{5}

)

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

TABLE I: Comparisons of replanning methods.

Vi-B Aggressive Autonomous Flights

We conducted aggressive autonomous flight experiments to validate the robustness of our replanning method. The self-developed drone is localized by a robust visual-inertial 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 i7-8550U CPU.

The experiments are conducted in complex indoor and outdoor scenes. In each experiment, a straight-line global reference trajectory is first generated using the approach [15]. During the flight, local trajectories within a horizon of 9 $m$ 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.

Fig. 10: Autonomous flight experiments in indoor (top) and outdoor (buttom) environments. A set of topologically distinct candidate trajectories are generated to avoid obstacles while keeping the drone close to the global reference trajectory (black). The best one (red) is selected and executed.

Fig. 11: Indoor and outdoor autonomous aggressive flights.

Vii Conclusions

In this paper, we propose a robust trajectory replanning method for autonomous quadrotor navigation. It overcomes local minima with the path-guided optimization, topological path searching and parallel trajectory optimization. Benchmark comparisons show its superior success rate and optimality than state-of-the-art 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] S. Bhattacharya, M. Likhachev, and V. Kumar (2012) Topological constraints in search-based robot path planning. Autonomous Robots 33 (3), pp. 273–290. Cited by: §II-B.
[2] S. Bhattacharya (2010) Search-based path planning with homotopy class constraints. In
Twenty-Fourth AAAI Conference on Artificial Intelligence
, Cited by: §II-B.
[3] F. Blochliger, M. Fehr, M. Dymczyk, T. Schneider, and R. Siegwart (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: §II-B, §IV.
[4] W. Ding, W. Gao, K. Wang, and S. Shen (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] F. Gao, Y. Lin, and S. Shen (2017-Sept) Gradient-based 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, §II-A.
[6] F. Gao, L. Wang, B. Zhou, L. Han, J. Pan, and S. Shen (2019) Teach-repeat-replan: a complete and robust system for aggressive flight in complex environments. arXiv preprint arXiv:1907.00520. Cited by: §I, §I, §II-A, Fig. 9, §VI-A, TABLE I.
[7] L. Han, F. Gao, B. Zhou, and S. Shen (2019) FIESTA: fast incremental euclidean distance fields for online motion planning of aerial robots. arXiv preprint arXiv:1903.02144. Cited by: §VI-B.
[8] L. Jaillet and T. Siméon (2008) Path deformation roadmaps: compact graphs with useful cycles for motion planning. The International Journal of Robotics Research 27 (11-12), pp. 1175–1188. Cited by: §II-B, §IV-A, §IV, footnote 3.
[9] M. Kalakrishnan, S. Chitta, E. Theodorou, P. Pastor, and S. Schaal (2011) STOMP: stochastic trajectory optimization for motion planning. In 2011 IEEE international conference on robotics and automation, pp. 4569–4574. Cited by: §II-A.
[10] T. Lee, M. Leoky, and N. H. McClamroch (2010-Dec.) 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: §VI-B.
[11] S. Liu, N. Atanasov, K. Mohta, and V. Kumar (2017-Sept) Search-based 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] H. Oleynikova, M. Burri, Z. Taylor, J. Nieto, R. Siegwart, and E. Galceran (2016-Oct.) Continuous-time 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, §II-A.
[13] H. Oleynikova, Z. Taylor, R. Siegwart, and J. Nieto (2018) Sparse 3d topological graphs for micro-aerial vehicle planning. In 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 1–9. Cited by: §II-B, §IV.
[14] T. Qin, P. Li, and S. Shen (2017) VINS-mono: a robust and versatile monocular visual-inertial state estimator. arXiv preprint arXiv:1708.03852. Cited by: §VI-B.
[15] C. Richter, A. Bry, and N. Roy (2013-Dec.) 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: §VI-A, §VI-B.
[16] C. Rösmann, F. Hoffmann, and T. Bertram (2015) Planning of multiple robot trajectories in distinctive topologies. In 2015 European Conference on Mobile Robots (ECMR), pp. 1–6. Cited by: §II-B.
[17] C. Rösmann, F. Hoffmann, and T. Bertram (2017) Integrated online trajectory planning and optimization in distinctive topologies. Robotics and Autonomous Systems 88, pp. 142–153. Cited by: §II-B, §IV-B.
[18] E. Schmitzberger, J. Bouchet, M. Dufaut, D. Wolf, and R. Husson (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: §II-B, §IV.
[19] J. Schulman, Y. Duan, J. Ho, A. Lee, I. Awwal, H. Bradlow, J. Pan, S. Patil, K. Goldberg, and P. Abbeel (2014) Motion planning with sequential convex optimization and convex collision checking. The International Journal of Robotics Research 33 (9), pp. 1251–1270. Cited by: §III-A.
[20] T. Siméon, J. Laumond, and C. Nissoux (2000) Visibility-based probabilistic roadmaps for motion planning. Advanced Robotics 14 (6), pp. 477–493. Cited by: §IV-B.
[21] V. Usenko, L. von Stumberg, A. Pangercic, and D. Cremers (2017) Real-time trajectory replanning for mavs using uniform b-splines and a 3d circular buffer. In 2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), pp. 215–222. Cited by: §I, §II-A, Fig. 9, §VI-A, TABLE I.
[22] B. Zhou, F. Gao, L. Wang, C. Liu, and S. Shen (2019) Robust and efficient quadrotor trajectory generation for fast autonomous flight. IEEE Robotics and Automation Letters 4 (4), pp. 3529–3536. Cited by: §I, §II-A, §III-B, §III-B, §III-B.
[23] M. Zucker, N. Ratliff, A. D. Dragan, M. Pivtoraiko, M. Klingensmith, C. M. Dellin, J. A. Bagnell, and S. S. Srinivasa (2013) Chomp: covariant hamiltonian optimization for motion planning. The International Journal of Robotics Research 32 (9-10), pp. 1164–1193. Cited by: §II-A.

Robust Real-time UAV Replanning Using Guided Gradient-based Optimization and Topological Paths

Alternating Minimization Based Trajectory Generation for Quadrotor Aggressive Flight

Robust and Efficient Quadrotor Trajectory Generation for Fast Autonomous Flight

Two-step Planning of Dynamic UAV Trajectories using Iterative δ-Spaces

RAPTOR: Robust and Perception-aware Trajectory Replanning for Quadrotor Fast Flight

TGK-Planner: An Efficient Topology Guided Kinodynamic Planner for Autonomous Quadrotors

Online State-Time Trajectory Planning Using Timed-ESDF in Highly Dynamic Environments

Training Recurrent Neural Networks via Dynamical Trajectory-Based Optimization

I Introduction

Ii Related Work