Teach-Repeat-Replan: A Complete and Robust System for Aggressive Flight in Complex Environments

07/01/2019 ∙ by Fei Gao, et al. ∙ 0

In this paper, we propose a complete and robust motion planning system for the aggressive flight of autonomous quadrotors. The proposed method is built upon on a classical teach-and-repeat framework, which is widely adopted in infrastructure inspection, aerial transportation, and search-and-rescue. For these applications, human's intention is essential to decide the topological structure of the flight trajectory of the drone. However, poor teaching trajectories and changing environments prevent a simple teach-and-repeat system from being applied flexibly and robustly. In this paper, instead of commanding the drone to precisely follow a teaching trajectory, we propose a method to automatically convert a human-piloted trajectory, which can be arbitrarily jerky, to a topologically equivalent one. The generated trajectory is guaranteed to be smooth, safe, and kinodynamically feasible, with a human preferable aggressiveness. Also, to avoid unmapped or dynamic obstacles during flights, a sliding-windowed local perception and re-planning method are introduced to our system, to generate safe local trajectories onboard. We name our system as teach-repeat-replan. It can capture users' intention of a flight mission, convert an arbitrarily jerky teaching path to a smooth repeating trajectory, and generate safe local re-plans to avoid unmapped or moving obstacles. The proposed planning system is integrated into a complete autonomous quadrotor with global and local perception and localization sub-modules. Our system is validated by performing aggressive flights in challenging indoor/outdoor environments. We release all components in our quadrotor system as open-source ros-packages.



There are no comments yet.


page 1

page 3

page 4

page 5

page 11

page 13

page 15

page 16

This week in AI

Get the week's most popular data science and artificial intelligence research sent straight to your inbox every Saturday.

I Introduction

As the development of autonomy in aerial robots, Micro Aerial Vehicle (MAV) has been more and more involved in our daily life. Among all applications emerged in recent years, quadrotor teach-and-repeat has shown significant potentials in aerial videography, industrial inspection, and human-robot interaction. In this paper, we investigate and answer the problem of what is the best way to incorporate a human’s intention in autonomous and aggressive flight, and what is a flexible, robust and complete aerial teach-and-repeat system.

(a) Snapshot of the indoor quadrotor flight.
(b) Snapshot of the outdoor quadrotor flight.
Fig. 1: Experiments in a challenging indoor drone racing site and an outdoor forest. A high-resolution video is available at: https://youtu.be/urEC2AAGEDs

There is a massive market for consumer drones nowadays. However, we observe that most of the operators of consumer drones are not professional pilots and would struggle in generating their ideal trajectory for a long time. In some scenarios, such as the drone racing or aerial filming, a beginner-level pilot is impossible to control the drone to finish the race safely or take an aerial video smoothly unless months of training. Also, there is considerable demand in applying drones to repetitive industrial inspections or search-and-rescue missions, where human provides a preferable routine. In these situations, demonstrating a desirable trajectory and letting the drone to repeat it is a common wish. However, the taught trajectory generated by an unskilled pilot is usually incredibly hard or dynamically infeasible to repeat, especially in some cluttered environments. Moreover, most of the vision-based teach-and-repeat applications [1][2][3], such as our previous work [1], are sensitive to changing environments. In [1], even the environment changes very slightly, the global map has to be rebuilt, and the teaching has to be redone.

Based on these observations, in this paper, instead of asking the drone to follow the human-piloted trajectory exactly, we only require the human operator to provide a rough trajectory with an expected topological structure. Such a human’s teaching trajectory can be arbitrarily slow or jerky, but it captures the rough route the drone is expected to fly. Our system then autonomously converts this poor teaching trajectory to a topological equivalent and energy-time efficient one with an expected aggressiveness. Moreover, during the repeating flight, our system locally observes environmental changes and re-plans sliding-windowed safe trajectories to avoid unmapped or moving obstacles. In this way, our system can deal with changing environments. Our proposed system extends the classical robotics teach-and-repeat framework and is named as teach-repeat-replan. It is complete, flexible, and robust.

In our proposed system, the surrounding environment is reconstructed by onboard sensors. Then the user’s demonstrated trajectory is recorded by virtually controlling the drone in the map with a joystick or remote controller. Afterward, we find a flight corridor that preserves the topological structure of the teaching trajectory. The global planning is decoupled as spatial and temporal planning sub-problems. Having the flight corridor, an energy-optimal spatial trajectory which is guaranteed to be safe, and a time-optimal temporal trajectory which is guaranteed to be physically feasible, are iteratively generated. In repeating, while the quadrotor is tracking the global spatial-temporal trajectory, a computationally efficient local map [4] is fused onboard by stereo cameras. Based on local observations, our proposed system uses a sliding-window fast re-planning method [5] to avoid possible collisions. The re-planning module utilizes gradient information to locally wrap the global trajectory to generate safe and kinodynamic feasible local plans against unmapped or moving obstacles.

The concept of generating optimal topology-equivalent trajectories for quadrotor teach-and-repeat was first proposed in our previous research [1]. In [1], once the repeating trajectory is generated, the drone executes it without any other considerations. In that work [1], the environment must remain intact during the repeating, and the localization of the drone is assumed to be perfect. These requirements are certainly not guaranteed in practice, therefore, prevent the system from being applied widely. In this paper, we extend the framework of the classical teach-and-repeat and propose several new contributions to make our system complete, robust, and flexible. Contributions of this paper are listed as:

  1. We advance our flight corridor generation method. The flight corridor we use now provides much more optimization freedom compared to our previous work [1]. The improvement of the flight corridor facilitates the generation of more efficient and smooth global trajectories. Moreover, we propose methods to accelerate the corridor generation on both CPU and GPU.

  2. We introduce our previous works on online mapping [4] and re-planning [5] into our system, to improve the robustness against errors of global maps, drifts of localization, and environmental changes and moving obstacles.

  3. We present a whole set of experiments and comparisons in various scenarios to validate our system.

  4. We release all components in the system as open-source packages, which include local/global planning, perception, and localization, and onboard controller.

Fig. 2:

The software architecture of our quadrotor system. Global mapping, planning, and visualization are running on a ground station, while state estimation, local sensing, and re-planning are running onboard.

Fig. 3: The hardware setting of our autonomous drone system.

In what follows, we discuss related literature in Sect. II and introduce our system in Sect. III. Our methods for finding a flight corridor consisting of large convex polyhedrons, and spatial-temporal trajectory optimization are detailed in Sect. IV and Sect. V, respectively. The local re-planning is introduced in Sect. VI. Experimental and benchmarked results are given in Sect. VII. The paper is concluded in Sect. VIII.

Ii Related Works

Robotics teach-and-repeat: Many robotics teach-and-repeat works, especially for mobile robots, have been published in recent years. Most of them focus on improving the accuracy or robustness in repeating/following the path by operators, which is fundamentally different from our motivation. A lidar-based teach-and-repeat system is proposed in [6], where laser scans are used to localize the ground vehicle against its taught path driven by the user. Furgale et al. [7] [8] also develop a lidar-based ground robot, which is specially designed for repeating long-term motions in highly dynamic environments. This system equips a local motion planner which samples local trajectory to avoid dynamic elements during route following. A map maintenance module is used to identify moving objects and estimate their velocities. An iterative learning controller is proposed in [9], to reduce the tracking error during the repeating of the robot. This controller can compensate disturbances such as unmodelled terrains and environmental changes by learning a feedforward control policy. Vision-based teaching-and-repeat systems are also proposed in several works, such as the visual localization used by the rover in [3]. In this work, the authors build a manifold map during the teaching and then use it for localization in the repeating, In [10], a multi-experience localization algorithm is proposed to address the issue of environmental changes. The ground robot is localized robustly against several past experiences. In [11] and [12], to further improve the accuracy and robustness in localization, illumination and terrain appearances are considered in their proposed visual navigation system used for teach-and-repeat, Compared to ground teach-and-repeat works, research on aerial teach-and-repeat is few. In [2], a vision-based drone is used to inspect infrastructure repetitively. In the teaching phase, the desired trajectory is demonstrated by the operator, and some keyframes in the visual SLAM are recorded as checkpoints. While repeating, local trajectories are generated to connect those checkpoints by using the minimum-snap polynomials [13]. To function properly, in this work, the teaching trajectory itself must be smooth, and the environments must have no changes during the whole repeating. In contrast, our proposed method can convert an arbitrarily poor path to a safe and efficient trajectory with expected flying aggressiveness. Also, our system is flexible. Since it records the teaching path by virtually controlling the drone in simulation, a manually piloted teaching process is not necessary. Finally, our proposed system is robust to environmental changing and can even avoid moving obstacles.

Quadrotor trajectory planning: Trajectory optimization is essential in generating safe and executable repeating trajectories from poor teaching ones. The minimum-snap trajectory optimization is proposed by Mellinger [13], where piecewise polynomials are used to represent the quadrotor trajectory and are optimized by quadratic programs (QP). A method for solving a closed-form solution of the minimum snap is proposed in [14]. In this work, a safe geometric path is firstly found to guide the generation of the trajectory. By adding intermediate waypoints to the path iteratively, a safe trajectory is finally generated after solving the minimum-snap problem several times. Our previous works [15] [16] [17] carve a flight corridor consisting of simple convex shapes (sphere, cube) in a complex environment. The flight corridor constructed by a series of axis-aligned cubes or spheres can be extracted very fast on occupancy map or Kd-tree. Then we use the flight corridor and physical limits to constrain a piecewise Bézier curve, to generate a guaranteed safe and kinodynamic feasible trajectory, Other works are proposed to find general convex polyhedrons for constraining the trajectory. In [18], a piecewise linear path is used to guide and initialize the polyhedron generation. In [19], by assuming all obstacles are convex, SDP and QP are iteratively solved to find the maximum polyhedron seeded at a random coordinate in 3-D space. Gradient information in maps is also valuable for local trajectory optimization. In CHOMP [20], the trajectory optimization problem is formulated as a nonlinear optimization over the penalty of safety and smoothness. In [21][22] and [23], gradient-based methods are combined with piecewise polynomials for local planning of quadrotors. In this paper, we also utilize gradient-based optimization for local re-planning.

Time optimization or so-called time parametrization is used to optimize the time profile of a trajectory, given the physical limits of a robot. Methods can be divided as direct methods [24] and indirect methods[25]. Direct methods generate an optimal spatial-temporal trajectory directly in the configuration space. For indirect methods, a trajectory independent of time is firstly generated, the relationship between time and the trajectory is optimized by an additional optimization process. The method in [26] finds a mapping function between the time and the trajectory, which is done by recursively adding key points into the function, and squeeze out the infeasibility of the time profile. This method obtains an optimal local solution and is computationally expensive. [25] also proposes a mapping function, which maps time to a virtual parametrization of the trajectory. The mapping function is then optimized under a complicated nonlinear formulation. However, the global optimality is not guaranteed, and a feasible initial solution is necessary to bootstrap the optimization. Convex optimization [27] and numerical integration [28] are two typical methods of robotics time optimal path parametrization (TOPP) problem. Although numerical integration [28] [29] has shown superior performance in computing efficiency, convex optimization [27] has the advantage of adding regularization terms other than total time into its objective function. This specialty suits well for our application where the user defines the expected aggressiveness of the drone, and sometimes the drone may not be expected to fly as fast as possible. As for the efficiency, since we do temporal optimization off-line before the repeating, computing time is not critical.

Iii System Overview

Iii-a System Architecture

The overall software and hardware architecture of our quadrotor system are shown in Fig. 2 and 3. The global mapping, flight corridor generator, and global spatial-temporal planner are done on an off-board computer. Other online processings are running onboard on the drone during the flight. Before teaching, the global map is built by onboard sensors. During teaching, a flight corridor is generated by inflating the teaching trajectory. Then the spatial and temporal trajectories are optimized iteratively within the flight corridor under a coordinate descent scheme [30]. The local planner using gradient-based optimization is running onboard to avoid unexpected obstacles observed in the repeating flights. For trajectory tracking, we use a geometric controller [31]. And the attitude is stabilized by the autopilot.

Iii-B Globally Consistent Localization and Mapping

We use VINS [32], a robust visual-inertial odometry (VIO) framework, to localize the drone. Moreover, the loop closure detection and global pose graph optimization are used in our system, to globally correct the pose estimation. The global mapping is done by fusing depth measurements from the stereo cameras with the pose estimation. By using our previous research on deformable map [33], our global mapping module maintains a series of sub-maps with each attached to a keyframe. In this way, the map is attached to the pose graph and is therefore globally driftless. During the mapping, when a loop closure is detected, keyframes in the global pose graph are corrected, and all sub-maps are deformed accordingly. The global pose graph optimization is also activated during the repeating. When loop closure is detected, the pose of the drone is corrected accordingly to eliminate the drift.

Iii-C Global Spatial-Temporal Planning

For an extremely poor teaching trajectory, both the geometric shape and time profile of it is far from optimal and therefore useless, or even harmful for conducting optimization. However, the topological information of the teaching trajectory is essential since it reflects the human’s intention. To preserve the topological information, we group the free space around the teaching trajectory to form a flight corridor (Sect. IV). The corridor contains the teaching trajectory within it, shares the same topological structure, and provides large freedom for optimization. It’s hard to concurrently optimize a trajectory spatially and temporally in the flight corridor. However, generating a safe spatial trajectory given a fixed time allocation (Sect. V-A) and optimizing the time profile of a fixed spatial trajectory (Sect. V-B) are both conquerable. Therefore, we iteratively optimize the trajectory in the space-time joint solution space by designing a coordinate descent [30] framework. An objective with weighting energy and time duration is defined for optimization. We firstly generate a spatial trajectory whose energy is minimized, then we use the temporal optimization to obtain the optimal time profile of it. The optimal time profile is used to parametrize a trajectory again for spatial optimization. The spatial-temporal optimizations are done iteratively until the total cost cannot be reduced any more.

Iii-D Local Collision Avoidance

In practice, the accumulated drift of VIO is unavoidable, and the recall rate of loop closure is unstable. Although we have built a dense global map, when the drift is significant and not corrected by loop detection in time, the quadrotor may have collisions with obstacles. Moreover, the environment may change or contain moving obstacles. Our previous work [1] has to re-build the map when changes happen and can not deal with dynamic obstacles. To resolve the above issues, we integrate our previous local map fusion module [4] into our system to detect collisions locally and serves the local trajectory optimization. Also, we propose a sliding-window local replanning method based on our previous research on quadrotor local planning [5], to avoid collisions on the flight.

In the repeating phase, the drone controls its yaw angle to face its flying direction and build a local map by stereo cameras. We consistently check the local trajectory within a replanning time horizon. If collisions along the local trajectory are reported, replanning is triggered to wrap the trajectory out of obstacles by gradient-based optimization [5].

Fig. 4: An illustration of free space captured by an axis-aligned cube and a general polyhedron. Obstacles are shown in dashed lines. The blue curve is the teaching trajectory of humans. The red triangle is the seed for finding local free space. The axis-aligned cube and a corresponding general convex polyhedron are shown in yellow and green, respectively.
(a) Axis-aligned cube, side view
(b) Front view
(c) General convex polyhedron, side view
(d) Front view
Fig. 5: The comparison of an axis-aligned cube and a general convex polyhedron. The cube and the polyhedron are generated to the largest volume they may have, from the same seed coordinate. The way to inflate the cube is stated in our previous paper [1]. The method to find the general free polyhedron will be detailed later in Sect. IV-A.

Iv Flight Corridor Generation

As stated in Sect. III-C, the first step of our global planning is to build a flight corridor around the teaching trajectory for spatial-temporal trajectory optimization. In our previous work [1], the flight corridor is constructed by finding a series of axis-aligned cubes, which may sacrifice much space, especially in a highly nonconvex environment, as is shown in Fig 4. A more illustrative comparison is shown in Fig. 5, where the convex polyhedron captures much more free space than the simple cube. Using simple axis-aligned cubes significantly limit the solution space of trajectory optimization, which may result in a poor solution. What’s more, in situations where the free space is very limited, such as flying through a very narrow circle, a cube-based corridor [1] may even fail to cover all teaching trajectory and result in no solutions existing in the corridor. Therefore, to utilize the free space more sufficiently and adapt to even extremely cluttered maps, we propose a method to generate general, free, large convex polyhedrons.

Since the human’s teaching trajectory may be arbitrarily jerky, we cannot assume there is a piecewise linear path to initiate the polyhedron generation, as in [18]. Also, we make no requirements on the convexity of obstacles in the map as in [19]. Our method is based on convex set clustering, which is similar to [34], but is different and advanced at:

  1. We make no assumption on the growing directions of convex clusters and generate completely collision-free polyhedrons based on our dense occupancy map.

  2. We introduce several careful engineering considerations which significantly speed-up the clustering.

  3. We fully utilize the parallel structure of this algorithm and accelerate it over an order of magnitude in GPUs.

  4. We introduce a complete pipeline from building the convex polyhedron clusters to establishing constraints in trajectory optimization.

Iv-a Convex Cluster Inflation

Fig. 6: An illustration of the convex cluster inflation. In (a) and (b), all qualified neighbor voxels are added to the convex cluster. In (c) and (d), since an occupied voxel occludes a ray (the green arrow) to one of the clustered voxels, the testing voxel (in yellow) is excluded to the convex cluster.

The core algorithm for the construction of the flight corridor is to find the largest convex free polyhedron at a given coordinate. In this paper, we use an occupancy grid map to represent the environment. Each polyhedron in the flight corridor is the convex hull of a voxel set, which is convex and contains only free voxels. The voxel set is found by clustering as many free voxels as possible around an arbitrary seed voxel. In this paper, we name the voxel set as convex cluster, and the process of finding such a set as convex cluster inflation. Our method for finding such a convex cluster is based on the definition of the convex set:

Definition: A set

in a vector space over

is called a convex set if the line segment joining any pair of points of lies entirely in [35].

The pipeline for iteratively inflating such a cluster while preserving convexity is stated in Alg. 1. Our method operates on a 3D occupancy map where voxels are labeled as obstacle or free. Three voxel sets are maintained in the algorithm. stands for the targeting convex voxel cluster. is the set of voxels that are tentative to be added to in this iteration. And contains newly added voxels which preserve the convexity. The cluster inflation starts by adding the seed voxel to , and adding all neighboring voxels of to . In an iteration, each voxel in is checked whether it can preserve convexity using the function CHECK_CONVEXITY(, ). This function, as is shown in Alg. 2, casts rays from to each existing voxels in . According to the definition of the convex set, with is convex if and only if all rays are collision-free. Based on this criteria, qualified voxels are considered as active voxels and are added into and . Neighboring voxels of all active voxels are traversed and collected by the function GET_NEIGHBORS() for next iteration. The inflation ends when becomes empty, which implies no additional voxels can be added into . Fig. 6 also illustrates the procedure of the convex cluster inflation.

2:Seed Voxel: , Grid Map: , Convex Cluster: ,
3:Candidate Voxel Set: , Active Voxel Set:
4:Input: ,
5:function CONVEX_INFLATION(, )
9:     while  do
10:         for each  do
11:              if CHECK_CONVEXITY(, then
14:              end if
15:         end for
17:          GET_NEIGHBORS()
19:     end while
20:     Output:
21:end function
Algorithm 1 Convex Cluster Inflation
1:Notation: Ray Cast:
2:function CHECK_CONVEXITY(, )
3:     for each  do
4:          CAST_RAY(, )
5:         if PASS_OBS(then
6:              return False
7:         end if
8:     end for
9:     return True
10:end function
Algorithm 2 Convexity Checking

Having a convex cluster consists of a number of voxels, we convert it to the algebraic representation of a polyhedron for following spatial trajectory optimization. Quick hull algorithm [36] is adopted here for quickly finding the convex hull of all clustered voxels. The convex hull is in vertex representation (V-representation)

, and is then converted to its equivalent hyperplane representation (H-representation) by using double description method 

[37]. The H-representation of a 3-D polyhedron is a set of affine functions:


where is the normal vector of the 3-D hyperplane and is a constant.

Iv-B CPU Acceleration

As is shown in Alg. 1, determining whether a voxel preserves the convexity needs to conduct ray-casting to all existing voxels in the convex cluster. Iterating with all voxels and rays makes this algorithm impossible to run in real-time, especially when the occupancy grid map has a fine resolution. To make the polyhedron generated in real-time, we take careful engineering considerations on the implementations and propose some critical techniques to significantly increase the overall efficiency.

Iv-B1 Polyhedron Initialization

We initialize each convex cluster as an axis-aligned cube using our previous method [15], which can be done very fast since only index query ((1)) operations are necessary. After inflating the cube to its maximum volume, as in Fig. 4, we switch to the convex clustering to further group convex free space around the cube.

The proposed polyhedron initialization may result in a final polyhedron different from the one which is clustered from scratch. This is because an axis-aligned cube only inflates in directions while a convex cluster grows in all possible directions (26-connections in a 3D grid map). However, this initialization process is reasonable. Our purpose is not making each polyhedron optimal but capturing as much as possible free space than a simple cube cannot. In practice, the initialization provides a fast discovery of nearby space which is easy to group, and does not prevent the following convex cluster inflation to refine the polyhedron and find sizeable free space. In Sect. VII-C1, we show that the initialization process significantly improves the computing efficiency with only a neglectable sacrifice on the volume of the final polyhedron.

Iv-B2 Early Termination

We label all voxels in the cluster as inner voxels which inside the convex cluster, and outer voxels which on the boundary of the convex cluster. When traversing a ray from a candidate voxel to a voxel in the convex cluster, we early terminate the ray casting when it arrives at a voxel labeled as inner.

Theorem 1: The early termination at inner voxels is sufficient for checking convexity.

Proof: According to the definition of convex set, a ray connecting an inner voxel to any other voxel in the convex cluster lies entirely in the convex cluster. Hence, the extension line of an inner voxel must lie inside the convex cluster and therefore it must pass the convexity check.

Iv-B3 Voxel Selection

To further reduce the number of voxels that need to be cast rays, given a candidate voxel, only outer voxels are used to check its convexity.

Theorem 2: Using outer voxels of a convex cluster is sufficient for checking convexity.

Proof: Obviously, the convex cluster is a closed set with outer voxels at its boundary. The candidate voxel is outside this set. Therefore, casting a ray from any inner voxel to the candidate voxel must pass one of the outer voxels. According to Theorem 1, checking convexity of this ray can terminate after the ray passes an outer voxels, which means for a candidate voxel, checking rays cast to outer voxels is sufficient.

By introducing above techniques, the proposed convex cluster inflation can work in real time for a mediate grid resolution () on CPUs. The efficacy of these techniques is numerically validated in Sect. VII-C1.

Iv-C GPU Acceleration

We propose a parallel computing scheme that significantly speeds up the inflation by one order of magnitude where a GPU is available. As is shown in Sec. IV-A, when the convex cluster discovers a new neighboring voxel, sequentially traversing and checking all rays is naturally parallelizable. With the help of many core GPUs, we can cast rays and check collisions parallelly. Moreover, to fully utilize the massively parallel capability of a GPU, reduce the serialize operations, and minimize the data transferring between CPU and GPU, we examine all potential voxels of the cluster parallelly in one iteration. Instead of discovering a new voxel and checking its rays, we find all neighbors of the active set and check their rays all in parallel. The detailed procedure is presented in Alg. 3, where GET_NEIGHBORS() collects all neighbors of a set of voxels, and PARA_CHECK_CONVEXITY(, ) checks the convexity of all candidate voxels parallelly in GPUs. Note that in the serialized version of the proposed method, the voxel discovered earlier may prevent later ones from being clustered, as is illustrated in Fig. 6. However, in the parallel clustering, all voxels examined at the same time may add conflicting voxels to the cluster. Therefore, we introduce an additional variable, to record sequential information of voxels. As shown in Alg. 4, the kernel function is running on the GPU per block. It checks the ray cast from every candidate voxel in to a cluster voxel in and to each other candidate voxel which has a prior index. After that, the function CHECK_RESULTS() selects all qualified voxels and adds them into . Firstly, candidate voxels that have collisions with are directly excluded. Then, candidate voxels having collisions with other candidates that have already been added into are excluded. In this way, we finally get the same results as in the serialized version of the clustering. The efficacy of the parallel computing is shown in Sect. VII-C1.

2:Parallel Raycasting Result:
7:     while  do
8:         /* GPU data uploads */
9:          PARA_CHECK_CONVEXITY(, )
10:         /* GPU data downloads */
11:          CHECK_RESULTS()
13:          GET_NEIGHBORS()
14:     end while
15:     Output:
16:end function
Algorithm 3 Parallel Convex Cluster Inflation
2:     for each  do
4:         for each  do
5:              /* Kernel function starts */
6:               CAST_RAY(, )
7:              if PASS_OBS(then
9:              end if
10:              /* Kernel function ends */
11:         end for
12:         for each  AND  do
13:              /* Kernel function starts */
14:               CAST_RAY(, )
15:              if PASS_OBS(then
18:              end if
19:              /* Kernel function ends */
20:         end for
21:     end for
22:     return
23:end function
25:function CHECK_RESULTS()
26:     for each  do
27:         if  then
29:         else if  then
30:              for each  do
31:                  if  then
32:                       go to 26
33:                  end if
34:              end for
36:         end if
37:     end for
38:     return
39:end function
Algorithm 4 Parallel Convexity Checking

Iv-D Corridor Generation and Loop Elimination

Since the trajectory provided by a user may behave arbitrarily jerky and contain local loops, we introduce a specially designed mechanism to elliminate unnecessary loops, i.e., repeatable polyhedrons. The exclusion of repeatable polyhedrons is essential since in following trajectory optimization (Sect. V), each polyhedron is assigned with one piece of the trajectory. Repeatable polyhedrons would result in an optimized trajectory loops as the user does, which is obviously not efficient. The pipeline of the corridor generation is shown in Alg. 5 and Fig. 7. At the beginning of the teaching, the flight corridor is initialized by finding the maximum polyhedron around the position of the drone. Then as the human pilots the drone to move, we keep checking the drone’s position. If it goes outside the last polyhedron ([-1]), we further check whether the drone discovers new free space or not. If the drone is contained within the second last polyhedron ([-2]), we can determine that the teaching trajectory has a loop, as shown in Fig. 7(c). Then, the last polyhedron in the corridor is regarded as repeatable and is therefore popped out from the corridor. Otherwise, as shown in Fig. 7(d), the drone is piloted to discover new space. Then a new polyhedron is inflated and added to the tail of the corridor. The corridor generation is terminated when the teaching finish. The final flight corridor shares the same topological structure with the teaching trajectory since no obstacles are included in the corridor. And it has no unnecessary loops.

1:Notation: Flight Corridor , Drone Position , Convex Polyhedron
2:Initialize :
5:while  Teaching  do
6:      UPDATE_POSE()
7:     if  OUTSIDE(, [-1])  then
8:         if  INSIDE(, [-2])  then
9:              .pop_back()
10:         else
11:               = CONVEX_INFLATION()
12:              .push_back()
13:         end if
14:     end if
15:end while
Algorithm 5 Flight Corridor Generation
Fig. 7: The flight corridor generation process. Red dots are coordinates along the teaching trajectory. (b), a new convex polyhedron is generated and added to the flight corridor when the drone leaves the corridor enters undiscovered space. (c), the drone leaves the last polyhedron and returns back to the second to last one, so the last polyhedron is deleted from the corridor.

V Spatial-Temporal Global Trajecotry Optimization

V-a Spatial Trajecotry Optimization

For the spatial optimization, we use the Bernstein basis to represent the trajectory as a piecewise Bézier curve, since it can be easily constrained in the flight corridor by enforcing constraints on control points. An -order Bernstein basis is:


where is the degree of the basis, is the binomial coefficient and is the variable parameterizing the trajectory. An -piece piecewise Bézier curve is written as:


For the piece of the curve, is the control point, and is the time duration. The spatial trajectory is generated in dimensions, and . is omitted in following derivation for brevity. In this equation, is scaled by since a standard Bézier curve is defined on .

Follow the formulation in minimum-snap [13], the squared jerk is minimized in this paper. Since the order derivative of a curve corresponds to the angular velocity, the minimization of jerks alleviates the rotation and therefore facilitates visual tracking. The objective of the piecewise curve is:


which is in a quadratic form denoted as . Here is composited by all control points in x, y, z dimensions. is a semi-definite Hessian matrix.

For a Bézier curve, its higher order derivatives can be represented by linear combinations of corresponding lower-order control points. For the and order derivatives of the piece of the curve in Eq. 3, we have:


V-A1 Boundary Constraints

The trajectory has the boundary constraints on the initial state () and the final state () of the quadrotor. Since a Bézier curve always passes the first and last control points, we enforce the boundary constraints by directly setting equality constraints on corresponding control points in each dimension:


V-A2 Continuity Constraints

For ensuring smoothness, the minimum-jerk trajectory must be continuous for derivatives up to -order at all connecting points on the piecewise trajectory. The continuity constraints are enforced by setting equality constraints between corresponding control points of two consecutive curves. For the and pieces of the curve, we can write the equation in each dimension as:


V-A3 Safety Constraints

The safety of the trajectory is guaranteed by enforcing each piece of the curve to be inside the corresponding polyhedron. Thanks to the convex hull property, an entire Bézier curve is confined within the convex hull formed by all its control points. Therefore we constrain control points using hyperplane functions obtained in Eq. IV-A. For the control point of the piece of the trajectory in dimensions, constraints are:


Constraints in Equs. V-A1 and V-A2 are affine equality constraints () and Eq. V-A3 is in affine in-equality formulation (). Finally, the spatial trajectory optimization problem is formulated as a QP as follows:

s.t. (9)

Unlike our previous works on corridor constrained trajectory [15, 17], here the kinodynamic feasibility (velocity and acceleration) is not guaranteed by adding higher-order constraints into this program, but by temporal optimization (Sect. V-B). For a rest-to-rest trajectory, the program in Eq. V-A3 is always mathematically feasible.

V-B Temporal Trajectory Optimization

Fig. 8: The effect of the temporal optimization. and are the time profile of the spatial trajectory before and after optimization.

In spatial optimization, a corridor-constrained spatial trajectory is generated given a fixed time allocation. To optimize the trajectory temporally, we design a re-timing function to map the original time variable to a variable . The relation between and is shown in Fig. 8. In this paper, the re-timing function is named as the temporal trajectory, and finding the optimal is called the temporal optimization. For the N-piece spatial curve defined in Equ. 3, we write as a corresponding N-piece formulation:


where are original time durations of the spatial curve , and are time durations after temporal optimization. Since physically time only increases, is a monotonically increasing function. Therefore we have . For clarity, in what follows, we use to denote taking derivatives with respect to , and for taking derivatives with respect to . By substituting with in

and taking derivatives with chain rule, we can write the velocity as:

and acceleration as:


The velocity and acceleration are also piecewise functions.

V-C Minimum-Time Formulation

V-C1 Objective

The total time of the temporal trajectory can be written as:


considering . We can introduce a regularization term that penalizes the changing rate of , to trade-off the minimization of time and control extremeness, or so-called motion aggressiveness, in our final temporal trajectory. The objective function is then written as:


where is a weight of the aggressiveness. By setting a larger we can obtain more gentle motions in the temporal trajectory. If , the temporal optimization is solved for generating motions as fast as possible. The motions generated with a large can be viewed in our previous work [1].

Following the direct transcription method in [27], and are introduced as two additional piecewise functions:


According to the relationship between and , we can have:


Then the objective function in Equ. 13 is re-formulated as:


V-C2 Constraints

The continuities of are enforced by setting constraints between every two consecutive pieces of it. In each dimension , we have:


Then, to satisfy the initial and the final velocity and acceleration , we set boundary constraints:


Finally, kinodynamic feasibility constraints are set as:


where and are the physical limits of the drone.

V-C3 SOCP Re-formulation

The above optimization problem has convex objective and constraints and is, therefore, a convex program. To make it easily solvable, for each piece of the trajectory, is discretized to according to a given resolution . . Then, becomes piecewise constant at each discretization point. According to Equ. 15, is piecewise linear. In this way, and are modeled by a series of discrete variables and , where is evaluated at and is evaluated at .

By applying the above discretization, the objective in Eq. 16 is derived as:


which is mathematically equivalent to the affine formulation:


by introducing and


as slack variables and additional constraints.

Eq. 27 is further derived to a quadratic form:


by introducing as slack variables .

Eq. 28 can be formulated as a standard rotated quadratic cone:


which is denoted as


Also, Equ. 29 can be written as a standard (non-rotated) quadratic cone:


and is denoted as


Finally, a slack variable is introduced to transform the objective in Equ. 26 to an affine function:


with a rotated quadratic cone:




where contains in all pieces of the trajectory.

Also, the discretization is applied to and in constraints listed in Sect. V-C2. Details are omitted for brevity. After that, we re-formulate these constraints as affine equality and in-equality functions. Besides, although we assume is piecewise constant, we bound the changing rate of considering the response time of the actuators of our quadrotor. We also write this changing rate constraint in an affine form:


where (not jerk) is a pre-defined bound of the changing rate of acceleration. Since the difference of between and cannot be determined during the optimization, we only bound the changing rate of in domain.

The temporal optimization problem in Sect. V-C is formulated as a standard Second Order Cone Program (SOCP):


Here and consist of all and . is the resolution of discretization of the problem. The effect of different and to the temporal trajectory and a more detailed derivation of the SOCP can be viewed in [38].

In our teach-repeat-replan system, since the global repeating trajectory always has static initial and final states, Equ. V-C3 is always mathematically feasible regardless of the solution of spatial optimization. Because a feasible solution of the optimization program can always be found by infinitely enlarging the time. Combined with the fact that the spatial optimization also always has a solution (Sect. V-A), once a flight corridor is given, a spatial-temporal trajectory must exist.

Vi Online Local Re-planning

In our previous work [1], once the global planning finished, the drone would execute the trajectory without other considerations. This strategy is based on assumptions that 1) the map of the environment is perfectly built and remains intact; 2) globally consistent pose estimation is provided. We use a VIO system with loop closure to correct local pose drifts, and our dense map is globally deformed according to the global pose graph. However, the first assumption does not always hold, especially when new obstacles suddenly appear or the environment changes. As for the second assumption, our global pose estimation relies on the loop closure detection, which also does not guarantee an extremely high recall rate. In situations where there are significant pose drifts but no timely loop closure corrections, the drone may have collisions with obstacles, as in Fig. 9.

Vi-a Local Re-planning Framework

To address above issues fundamentally, we propose a local re-planning framework which reactively wraps the global trajectory to avoid unmodeled obstacles. A sliding local map is maintained onboard, where obstacles are fused, and an ESDF (Euclidean Signed Distance Field) [39] is updated accordingly. Note that the dense global map is attached to the global pose graph but the local map introduced here is associated with the local VIO frame and sliding with the drone.

Fig. 9: An illustration of colliding with obstacles when there are significant pose drifts but no timely loop closure corrections. Obstacles are depicted in the global frame. The flight path of the drone in the VIO frame is shown in the red curve. But the actual trajectory in the global frame is the blue curve, which collides with obstacles on the global map.

Vi-A1 ESDF Mapping

Fig. 10: The local occupancy map its corresponding ESDF map visualized at a given height of .

We adopt our previous work FIESTA [4], which is an advanced incremental ESDF [40] mapping framework, to build the local map for online re-planning. FIESTA fuses the depth information into a voxel-hashed occupancy map [41] and updates the distance value of voxels as few as possible using a breadth-first search (BFS) framework. It is lightweight, efficient, and produces near-optimal results. Details can be checked in [4]. The ESDF is necessary for the following gradient-based trajectory wrapping. An example of a local occupancy map and its corresponding ESDF map are shown in Fig. 10. Note, in our system the range of the local map is decided by the range of current depth observation.

Vi-A2 Sliding Window Re-planning

Fig. 11: An illustration of the online re-planning mechanism. The blue and green curves are the global trajectory and the actual flight path of the drone, respectively. The purple curve and dots are the global trajectory in the sliding window and its corresponding control points. The red curve and dots are the re-planned local trajectory and its control points. The yellow frustum shows the sensing horizon of the drone.

Due to the limited onboard sensing range and computing resources, it is impossible and unnecessary to conduct global re-planning. In this article, we maintain a temporal sliding window over the global trajectory and conduct local re-planning within it. As is shown in Fig. 11, when obstacles are observed to block the trajectory in the sliding window, a re-planed trajectory is generated to avoid obstacles, and rejoin the global trajectory afterward.

Vi-B Gradient-Based B-spline Optimization

Vi-B1 B-spline Trajectory Formulation

A B-spline is a piecewise polynomial function defined by a series of control points and knot vector . For a -degree B-spline, we have . Following the matrix representation of the De Boor–Cox formula [42], the value of a B-spline can be evaluated as:


here is a constant matrix depends only on . And , for .

Vi-B2 B-spline Initialization

We initialize the local trajectory optimization by re-parameterizing the trajectory in the re-planning horizon as a uniform B-spline. The reason we use uniform B-spline is that it has a simple mathematical formula that is easy to evaluate in the following optimization. For a uniform B-splines, each knot span has identical value . The local trajectory is first discretized to a set of points according to a given . Then these points are fitted to a uniform B-spline by solving a min-least square problem.

Note that, a degree uniform B-spline is naturally order continuous between consecutive spans. Therefore, there is no need to enforce continuity constraints in the following optimization explicitly. Besides, for a degree B-spline trajectory defined by control points, the first and last control points are fixed due to the continuous requirement of the starting and ending states of the local trajectory.

Vi-B3 Elastic Band Optimization

The basic requirements of the re-planed B-spline are three-folds: smoothness, safety, and dynamical feasibility. We define the smoothness cost using a jerk-penalized elastic band cost function[43, 44]:


which can be viewed as a sum of the squared jerk of control points on the B-spline. Note here we use this formulation which is independent of the time parametrization of the trajectory instead of the traditional time integrated cost function [13]. Because the time duration in each span of the B-spline may be adjusted after the optimization (Sec. VI-B4), Eq. VI-B3 captures the geometric shape of the B-spline regardless of the time parametrization. Besides, Eq. VI-B3 bypasses the costly evaluation of the integration and is, therefore, more numerically robust and computationally efficient in optimization.

The safety and dynamical feasibility requirements of the B-spline are enforced as soft constraints and added to the cost function. Also, the collision cost , dynamical feasibility cost , and are evaluated at only control points. The collision cost is formulated as the accumulated L2-penalized closest distance to obstacles along the trajectory, which is written as


where is the distance between to its closet obstacle and is recorded in the ESDF. is defined as


where is the expected path clearance. and are applied to velocities and accelerations, which exceed the physical limits. The formulations of and are similar to Eq. 41 and are omitted here. The overall cost function is:


where are weighting coefficients. can be minimized for a local optimal solution by general optimization methods such as Gauss-Newton or Levenberg-Marquardt.

Vi-B4 Iterative Refinement

In the above-unconstrained optimization problem, although collisions and dynamical infeasibilities are penalized, there is no hard guarantee on generating a strictly feasible solution. To improve the success rate in practice, we add a post-process to refine the trajectory iteratively. In each iteration, we check collisions and feasibilities of all optimized control points. If collisions are detected, we increase the collision term by increasing and solve the optimization problem (Eq. 43) again.

Since we wrap the local trajectory to go around obstacles, the trajectory is always lengthened after the optimization. Consequently, using the original time parametrization will unavoidably result in a higher aggressiveness, which means the quadrotor tends to fly faster. Then its velocity and acceleration would easily exceed the predefined limits. Therefore, we adjust the time parameterization of the local trajectory to squeeze out dynamical infeasibilities. We slightly enlarge infeasible knots spans of the B-spline by the following heuristic.


where is a constant slightly larger than . are infeasible velocity and acceleration and are maximum allowed acceleration and velocity of the drone. The time duration is iteratively enlarged until obtaining a feasible solution or exceeding the maximum iteration limit. If no feasible solution exists after the time adjustment, is increased, and the trajectory is optimized again.

Vii Results

Vii-a Implementation Details

The global planning method proposed in this paper is implemented with a QP solver OOQP222http://pages.cs.wisc.edu/~swright/ooqp/ and a SOCP solver Mosek333https://www.mosek.com. The local re-planning depends on a nonlinear optimization solver NLopt444https://nlopt.readthedocs.io. The source code of all modules in our quadrotor system, including local/global localization, mapping, and planning, are released as ros-packages555https://github.com/HKUST-Aerial-Robotics/Teach-Repeat-Replan for the reference of community. Readers of this paper can easily replicate all the presented results. The state estimation, pose graph optimization, local mapping, local re-planning, and the controller is running onboard on a Manifold-2C666https://store.dji.com/product/manifold-2?vid=80932 mini-computer. Other modules are running on an off-board laptop which has a GTX 1080777https://www.nvidia.com/en-us/geforce/20-series/ graphics card.

Our global map is built to attach to a global pose graph. Both the map and the pose graph are saved for repeating. Before the repeating, the drone is handheld to close the loop of the current VIO frame with the saved global pose graph. The relative transformation of these two frames is used to project the control commands to the VIO frame. During the repeating, pose graph optimization is also activated to calculate the pose drift and compensate for the control command.

(a) The flight corridor catpures local free space.
(b) The local re-planning trajectory and current depth image.
Fig. 12: The trajectory generated in a complex simulated environment. The flight corridor consists of large free convex polyhedrons are shown in (a), and the optimized space-time trajectory is shown in (b).

Vii-B Simulated Flight Test

We first test our global and local planning methods in simulations. The simulated environments are randomly deployed with various types of obstacles and circles for drone racing, as shown in Fig. 12. The simulating tool we use is a light-weight simulator MockaFly888https://github.com/HKUST-Aerial-Robotics/mockasimulator, which contains quadrotor dynamics model, controller, and map generator. And the simulator’s is also released as an open-source package with this paper. In the simulation, a drone is controlled by a joystick to demonstrate the teaching trajectory. The simulated drone is equipped with a depth camera whose depth measurements are real-time rendered in GPU by back-projecting the drone’s surrounding obstacles. We randomly add noise on the depth measurements to mimic a real sensor. The re-planning module is activated in the simulation and is triggered by the noise added on the depth. The teaching trajectory and the flight corridor is shown in Fig. 12(a). The global trajectory, local re-planned trajectory, and depth measurement are shown in Fig. 12(b). More details about the simulation are presented in the attached video.

Vii-C Benchmark Comparisons

Vii-C1 Corridor Generation

We test the performance of the flight corridor generation methods (Sect. IV), to show the efficacy of the proposed techniques for CPU (Sect. IV-B) and GPU (Sect. IV-C) accelerations. For convenience, we denote the basic process for doing convex cluster inflation as CPU_raw; CPU_raw added cube initialization as CPU+; the one with cube initialization, vertex selection and early termination as CPU++; and the parallel version of the convex cluster inflation as GPU. We first compare the time consumed for finding the largest flight corridor with these methods, to validate the improvements of efficiency by using our proposed CPU and GPU acceleration techniques. Then, we compare the ratio of space capturing by methods with and without the polyhedron initialization, and by our previous method [1]. The motivation of the latter comparison is two-fold:

  1. It serves to show superior performance by replacing cubes with polyhedrons.

  2. As discussed in Sect. IV-B, the initialization process would result in different final clustering results compared to the pure convex cluster inflation. This comparison also validates that the initialization process only makes neglectable harm to free space capturing.

We generate 10 random maps, with 10 20 random teaching trajectories given in each map. The average length of teaching trajectories is . Results are given in Tabs. I and II.

Fig. 13: The flight corridor generated with and without the initialization process. Polyhedrons with bounding edges in white and red are found by methods with and without the initialization, respectively.
Computing Time (s) GPU CPU++ CPU+ CPU_raw
Res. = 0.031 0.111 0.162 0.359
Res. = 0.055 0.310 0.503 1.309
Res. = 0.169 1.423 2.803 9.583
Res. = 0.942 13.940 30.747 141.659
Res. = 3.660 71.862 157.181 927.131
TABLE I: Comparison of Computing Time of Corridor Generation

As shown in Tab. I, as the resolution of the map being finer, the computing time of the simple convex cluster inflation quickly becomes unacceptable huge. In CPU, with the help of polyhedron initialization, the computational efficiency is improved several times. Moreover, according to Tab. I, introducing the voxel selection and early termination can increase the speed more than one order of magnitude in a fine resolution. The efficacy of the GPU acceleration is even more significant. As shown in Tab. I, the GPU version improves the computing speed 30 times at a fine resolution (), and 10 times at a coarse resolution (). For a finer resolution, more candidate voxels are discovered in one iteration of Alg. 3, thus more computations are conducted parallelly to save time.

Space Ratio (%) w/ Initialization w/o Initialization Previous [1]
Res. = 99.22 100.00 82.28
Res. = 99.56 100.00 82.92
Res. = 98.93 100.00 81.82
Res. = 97.06 100.00 82.78
Res. = 97.14 100.00 83.03
TABLE II: Comparison of Space Captured of Corridor Generation

For the second comparison, we count the number of free voxels included in the flight corridor found by each method. At each resolution, we take the result of the method without initialization as 100% and compare others against it. Tab. II indicates two conclusions:

  1. Using polyhedrons instead of axis-aligned cubes can significantly increase the volume of the flight corridor.

  2. Using initialization only slightly sacrifices the volume of the flight corridor. And the sacrifice is neglectable in a medium or coarse resolution ().

The first conclusion holds because a simple cube only discovers free space in directions and sacrifices much space in a highly nonconvex environment, as in Fig. 4. The second conclusion comes from the fact that in a highly nonconvex environment, a regular shaped polyhedron (a cube) does not prevent the following voxel clustering in its nearby space. It shows that the initialization plus the clustering refinement does not harm the volume of the final polyhedron, and is acceptable in practice, especially for a resolution not very fine.

Vii-C2 Global Planning

Fig. 14: The comparison of trajectories optimized by different methods. The manual flight trajectory is shown as the purple curve. Blue, red, green, and yellow trajectories are generated by our proposed method, our previous method [1], gradient-based method [45] and waypoints-based method [14].
Method Length (m) Time (s) Energy
Proposed Method 84.607 55.154 83.350
Previous Method 86.723 57.736 89.883
Gradient-based [45] 89.622 111.398 109.575
Waypoint-based [14] 97.045 94.895 204.267
TABLE III: Comparison of Trajectory Optimization

We compare the proposed global planning method against our previous work [1] and other representative optimization-based trajectory generation methods, such as the waypoint-based method [14] and the gradient-based method [45]. For the latter two benchmarked methods, there is no explicit way to capture the topological structure of the teaching trajectory. Therefore, we convert the teaching trajectory to a piecewise path by recursively finding a collision-free straight line path along with it. Then we use this path to initialize the waypoint-based [14] method and the gradient-based method [45]. Benchmarked methods are also integrated into the coordinate descent framework with temporal optimization. Some parameters dominate the performance of these benchmarked methods, especially for the gradient-based method [45] where the trade-off between collision and smoothness is essential. For a fair comparison, parameters are tuned to achieve the best performances before the test. We randomly generate 10 simulated environments with dense obstacles, as in Sect. VII-B, and conduct 10 teach-and-repeat trials in each map. A sample result of generated trajectories is shown in Fig. 14.

As shown in Tab. III, our proposed method outperforms in all length, time, and energy aspects. The waypoint-based [14] method and the gradient-based [45] method both require a piecewise linear path as initialization. The waypoint-based [14] method can only add intermediate waypoints on the initial path. Therefore, it is mostly dominated by its initialization and tends to output a solution with low quality. The gradient-based [45] method has no such restriction and can adjust the path automatically by utilizing gradient information. However, its optimization formulation is underlying non-convex, since the collision cost is defined on a non-convex ESDF. Therefore, the gradient-based [45] method always finds a locally optimal solution around its initial guess. Compared to these two methods, our method is initialization-free. Both the spatial and temporal optimization of our proposed method enjoys the convexity in its formulation. They are guaranteed to find the global energy-optimal and time-optimal solutions in the flight corridor. Naturally, a smoother trajectory also tends to generate a faster time profile. So finally, under the same coordinate descent framework, our method always outperforms [45] and  [14]. Compared to [1], the advanced corridor generation proposed in this paper (Sect. IV) can always capture more free space than using our previous axis-aligned corridor. Naturally, it provides much more freedom for global planning and results in much better solutions.

(a) The indoor testing environment.
(b) The global consistent dense map.
Fig. 15: The experimental set-up of the fast indoor drone racing flights. (a), the obstacles deployment. (b), the pre-built globally consistent map.
Fig. 16: Snapshots of the fast autonomous flight in a static indoor environment. The maximum velocity and acceleration for the quadrotor are set as and .

Vii-D Indoor Flight Test

Vii-D1 Fast Flight in a Static Environment

Firstly, we conduct experiments in a cluttered drone racing scenario. This experiment validates the robustness of our proposed system, and also pushes the boundary of aggressive flight of quadrotors. Several different types of obstacles, including circles, arches, and tunnels, are deployed randomly to composite a complex environment, as shown in Fig. 15(a). The smallest circle only has a diameter of , which is very narrow compared to the tip-to-tip size of our drone. The maximum velocity and acceleration of the drone are set as and , respectively. And the parameter in Equ. 13 is set as 0, which means the quadrotor is expected to fly as fast as possible as long as it respects the kinodynamic limits. A dense global consistent map is pre-built using the method stated in Sect. III-B. During the teaching phase, the quadrotor is virtually piloted by a human to move amid obstacles. Then the quadrotor autonomously converts this teaching trajectory to a global repeating trajectory and starts to track it. Snapshots of the drone in the flight are shown in Fig. 16. The teaching trajectory and the convex safe flight corridor are visualized in Fig 17(a). And the global repeating trajectory is in Fig. 17(b).

(a) The teaching trajectory and the flight corridor.
(b) The spatial-temporal optimal repeating trajectory.
Fig. 17: An overview of the teaching trajectory, flight corridor, and repeating trajectory in a pre-built dense map. The colored code indicates the height of obstacles. The flight corridor is represented by transparent blue polyhedrons in (a). The global trajectory, local trajectory, quadrotor flight path are shown in blue, green, and purple curves, respectively.

Vii-D2 Local Re-planning Against Unknown Obstacles

(a) Side-view.
(b) Onboard first-person view.
Fig. 18: The local re-planning experiment against unmapped and moving obstacles. The drone and unmapped obstacles are labeled by the red and blue dashed rectangles, respectively, for clear visualization.
(a) Re-planning, unmapped obstacle.
(b) Re-planning, moving obstacle.
(c) Overview of all re-planning trajectories.
Fig. 19: Indoor flight in a dynamic environment. In (a) and (b), the unmapped new obstacle and moving obstacles are labeled by red dashed rectangles, and colored voxels represent local obstacle maps. In (c), colored voxels show the global map. Other marks are interpreted as the same as in previous figures.

Our system can deal with changing environments and moving obstacles. In this experiment, we test our system also in the drone racing site to validate our local re-planning module. Several obstacles are moved or added to change the drone racing environment significantly, and some others are dynamically added during the repeating flight, as shown in Fig. 18 In this experiment, the maximum velocity and acceleration for the quadrotor are set as and . The local ESDF map is sliding with the drone using a ring-buffered updating mechanism [46]. The resolution of the local perception is . The size of the map is decided by points observed spreading in the current frame. The horizon and frequency of the local re-planning are and , respectively. Re-planning is triggered 8 times during the flight in this experiment, and local safe and dynamical feasible splines are generated on time accordingly. Local trajectories, local maps, and the overview of this experiment are shown in Fig. 19. We refer readers to the attached video for more details.

(a) Outdoor experimet, trial 1.
(b) Outdoor experimet, trial 2.
Fig. 20: Snapshots of the experiments in outdoor environments.
Fig. 21: The repeating trajectory in outdoor experiments, trial 1. Marks are interpreted as the same as in previous figures.
(a) A close-up of outdoor experiments, trial 2.
(b) An overview of outdoor experiments, trial 2.
Fig. 22: Outdoor flight, trial 2. Marks are interpreted as the same as in previous figures.

Vii-E Outdoor Flight Test

Finally, we conduct quadrotor flight experiments with a much higher aggressiveness in two different outdoor scenes, as in Fig. 20, to show the robustness of our system in natural environments. Although these experiments are conducted outdoor, GPS or other external positioning devices are not used. The teach-repeat-replan pipeline is as the same as before indoor experiments VII-D. The maximum allowed velocity and acceleration limits for these two trials are set as , and , , respectively. The drone’s desired and estimated positions and velocities in the second trial are given in Fig. 23, which shows acceptable tracking errors. Since the flight speed is significantly higher than indoor experiments, we set a smaller re-planning horizon as . Results such as the global and local trajectory and the global map are visualized in Figs. 21 and 22. More clearly visualizations of outdoor experiments are given in the video.

Fig. 23: Profiles of the desired and estimated position and velocity. The position and velocity are estimated by our localization module VINS [32].

Viii Conclusion

In this paper, we propose a framework, teach-repeat-replan for quadrotor aggressive flights in complex environments. The main idea of this work is to find the topological equivalent free space of the user’s teaching trajectory, use spatial-temporal trajectory optimization to obtain an energy-efficient repeating trajectory, and incorporate online perception and re-planning to ensure the safety against environmental changes and moving obstacles. The teaching process is conducted by virtually controlling the drone in simulation. The generated repeating trajectory captures users’ intention and respect an expected flight aggressiveness, which enables autonomous flights much more aggressive than human’s piloting in complex environments. The online re-planning guarantees the safety of the flight and also respects the reference of the repeating trajectory.

To group large free space around the teaching trajectory, we propose a GPU-accelerated convex polyhedron clustering method to find a flight corridor. The optimal global trajectory generation problem is decoupled as spatial and temporal sub-problems. Then these two sub-problems are iteratively optimized under the coordinate descent framework. Moreover, we incorporate the local perception and local trajectory re-planning modules into our framework to deal with environmental changes, dynamic obstacles, and localization drifts.

The proposed system is complete and robust. Users of our system do not have to pilot the drone carefully to give a teaching trajectory. Instead, an arbitrarily jerky/poor trajectory can be converted to an efficient and safe global trajectory. Moreover, when the environment changes or the global localization drifts, the local perception and re-planning modules guarantee the safety of the drone while tracking the global trajectory. Our system is also flexible and easily replicable, as evidenced by various types of experiments presented in this paper, and a third-party application999Flight demonstration at the Electrical and Mechanical Services Department (EMSD), Hong Kong government. Video: https://youtu.be/Ut8WT0BURrM. We release all components of our system for the reference of the community.


  • [1] F. Gao, L. Wang, K. Wang, W. Wu, B. Zhou, L. Han, and S. Shen, “Optimal trajectory generation for quadrotor teach-and-repeat,” IEEE Robotics and Automation Letters (RA-L), 2019.
  • [2] M. Fehr, T. Schneider, M. Dymczyk, J. Sturm, and R. Siegwart, “Visual-inertial teach and repeat for aerial inspection,” arXiv preprint arXiv:1803.09650, 2018.
  • [3] P. Furgale and T. D. Barfoot, “Visual teach and repeat for long-range rover autonomy,” Journal of Field Robotics, vol. 27, no. 5, pp. 534–560, 2010.
  • [4] L. Han, F. Gao, B. Zhou, and S. Shen, “Fiesta: Fast incremental euclidean distance fields for online motion planning of aerial robots,” arXiv preprint arXiv:1903.02144, 2019.
  • [5] B. Zhou, F. Gao, L. Wang, C. Liu, and S. Shen, “Robust and efficient quadrotor trajectory generation for fast autonomous flight,” IEEE Robotics and Automation Letters (RA-L), 2019.
  • [6] C. Sprunk, G. D. Tipaldi, A. Cherubini, and W. Burgard, “Lidar-based teach-and-repeat of mobile robot trajectories,” in Proc. of the IEEE/RSJ Intl. Conf. on Intell. Robots and Syst.(IROS), 2013, pp. 3144–3149.
  • [7] P. Furgale, P. Krüsi, F. Pomerleau, U. Schwesinger, F. Colas, and R. Siegwart, “There and back again-dealing with highly-dynamic scenes and long-term change during topological/metric route following,” in ICRA14 Workshop on Modelling, Estimation, Perception, and Control of All Terrain Mobile Robots, 2014.
  • [8] P. Krüsi, B. Bücheler, F. Pomerleau, U. Schwesinger, R. Siegwart, and P. Furgale, “Lighting-invariant adaptive route following using iterative closest point matching,” Journal of Field Robotics, vol. 32, no. 4, pp. 534–564, 2015.
  • [9] C. J. Ostafew, A. P. Schoellig, and T. D. Barfoot, “Visual teach and repeat, repeat: Iterative learning control to improve mobile robot path tracking in challenging outdoor environments,” in Proc. of the IEEE/RSJ Intl. Conf. on Intell. Robots and Syst.(IROS), 2013, pp. 176–181.
  • [10] M. Paton, K. MacTavish, M. Warren, and T. D. Barfoot, “Bridging the appearance gap: Multi-experience localization for long-term visual teach and repeat,” in Proc. of the IEEE/RSJ Intl. Conf. on Intell. Robots and Syst.(IROS), 2016, pp. 1918–1925.
  • [11] M. Paton, K. MacTavish, C. J. Ostafew, and T. D. Barfoot, “It’s not easy seeing green: Lighting-resistant stereo visual teach & repeat using color-constant images,” in Proc. of the IEEE Intl. Conf. on Robot. and Autom. (ICRA), 2015, pp. 1519–1526.
  • [12] L.-P. Berczi and T. D. Barfoot, “It’s like déjà vu all over again: Learning place-dependent terrain assessment for visual teach and repeat,” in Proc. of the IEEE/RSJ Intl. Conf. on Intell. Robots and Syst.(IROS), 2016, pp. 3973–3980.
  • [13] D. Mellinger and V. Kumar, “Minimum snap trajectory generation and control for quadrotors,” in Proc. of the IEEE Intl. Conf. on Robot. and Autom. (ICRA), Shanghai, China, May 2011, pp. 2520–2525.
  • [14] C. Richter, A. Bry, and N. Roy, “Polynomial trajectory planning for aggressive quadrotor flight in dense indoor environments,” in Proc. of the Intl. Sym. of Robot. Research (ISRR), Dec. 2013, pp. 649–666.
  • [15] F. Gao, W. Wu, Y. Lin, and S. Shen, “Online safe trajectory generation for quadrotors using fast marching method and bernstein basis polynomial,” in Proc. of the IEEE Intl. Conf. on Robot. and Autom. (ICRA), Brisbane, Australia, May 2018.
  • [16] F. Gao and S. Shen, “Online quadrotor trajectory generation and autonomous navigation on point clouds,” in Proc. of the IEEE Intl. Sym. on Safety, Security, and Rescue Robotics (SSRR), lausanne, switzerland, 2016, pp. 139–146.
  • [17] F. Gao, W. Wu, W. Gao, and S. Shen, “Flying on point clouds: Online trajectory generation and autonomous navigation for quadrotors in cluttered environments,” Journal of Field Robotics, 2018.
  • [18] S. Liu, M. Watterson, K. Mohta, K. Sun, S. Bhattacharya, C. J. Taylor, and V. Kumar, “Planning dynamically feasible trajectories for quadrotors using safe flight corridors in 3-d complex environments,” IEEE Robotics and Automation Letters (RA-L), pp. 1688–1695, 2017.
  • [19] R. Deits and R. Tedrake, “Computing large convex regions of obstacle-free space through semidefinite programming,” in Algorithmic Foundations of Robotics XI.   Springer, 2015, vol. 107, pp. 109–124.
  • [20] N. Ratliff, M. Zucker, J. A. Bagnell, and S. Srinivasa, “Chomp: Gradient optimization techniques for efficient motion planning,” in Proc. of the IEEE Intl. Conf. on Robot. and Autom. (ICRA), May 2009, pp. 489–494.
  • [21] H. Oleynikova, M. Burri, Z. Taylor, J. Nieto, R. Siegwart, and E. Galceran, “Continuous-time trajectory optimization for online uav replanning,” in Proc. of the IEEE/RSJ Intl. Conf. on Intell. Robots and Syst.(IROS), Daejeon, Korea, Oct. 2016, pp. 5332–5339.
  • [22] Y. Lin, F. Gao, T. Qin, W. Gao, T. Liu, W. Wu, and S. Shen, “Autonomous aerial navigation using monocular visual-inertial fusion,” Journal of Field Robotics, vol. 35, no. 1, pp. 23–51, 2018.
  • [23] H. Oleynikova, C. Lanegger, Z. Taylor, M. Pantic, M. Alexander, R. Siegwart, and J. Nieto, “An open-source system for vision-based micro-aerial vehicle mapping, planning, and flight in cluttered environments,” arXiv preprint arXiv:1812.03892, 2019.
  • [24] H. M. Choset, S. Hutchinson, K. M. Lynch, G. Kantor, W. Burgard, L. E. Kavraki, and S. Thrun, Principles of robot motion: theory, algorithms, and implementation.   MIT press, 2005.
  • [25] M. Roberts and P. Hanrahan, “Generating dynamically feasible trajectories for quadrotor cameras,” ACM Transactions on Graphics (TOG), vol. 35, no. 4, p. 61, 2016.
  • [26] J. Jamieson and J. Biggs, “Near minimum-time trajectories for quadrotor uavs in complex environments,” in Proc. of the IEEE/RSJ Intl. Conf. on Intell. Robots and Syst.(IROS), 2016, pp. 1550–1555.
  • [27] D. Verscheure, B. Demeulenaere, J. Swevers, J. De Schutter, and M. Diehl, “Time-optimal path tracking for robots: A convex optimization approach,” IEEE Transactions on Automatic Control, vol. 54, no. 10, pp. 2318–2327, 2009.
  • [28] Q.-C. Pham, “A general, fast, and robust implementation of the time-optimal path parameterization algorithm,” IEEE Transactions on Robotics, vol. 30, no. 6, pp. 1533–1540, 2014.
  • [29] H. Pham and Q.-C. Pham, “A new approach to time-optimal path parameterization based on reachability analysis,” IEEE Transactions on Robotics, vol. 34, no. 3, pp. 645–659, 2018.
  • [30] S. J. Wright, “Coordinate descent algorithms,” Mathematical Programming, vol. 151, no. 1, pp. 3–34, 2015.
  • [31] T. Lee, M. Leoky, and N. H. McClamroch, “Geometric tracking control of a quadrotor uav on se (3),” in Proc. of the IEEE Control and Decision Conf. (CDC), Atlanta, GA, Dec. 2010, pp. 5420–5425.
  • [32] T. Qin, P. Li, and S. Shen, “Vins-mono: A robust and versatile monocular visual-inertial state estimator,” IEEE Transactions on Robotics, vol. 34, no. 4, pp. 1004–1020, 2018.
  • [33] K. Wang, F. Gao, and S. Shen, “Real-time scalable dense surfel mapping,” in Proc. of the IEEE Intl. Conf. on Robot. and Autom. (ICRA), 2019.
  • [34] F. Blochliger, M. Fehr, M. Dymczyk, T. Schneider, and R. Siegwart, “Topomap: Topological mapping and navigation based on visual slam maps,” in Proc. of the IEEE Intl. Conf. on Robot. and Autom. (ICRA), 2018, pp. 1–9.
  • [35] S. Lay, Convex sets and their applications.   Courier Corporation, 2007.
  • [36] C. B. Barber, D. P. Dobkin, D. P. Dobkin, and H. Huhdanpaa, “The quickhull algorithm for convex hulls,” ACM Transactions on Mathematical Software (TOMS), vol. 22, no. 4, pp. 469–483, 1996.
  • [37] K. Fukuda and A. Prodon, “Double description method revisited,” in Franco-Japanese and Franco-Chinese Conference on Combinatorics and Computer Science.   Springer, 1995, pp. 91–111.
  • [38] F. Gao, W. Wu, J. Pan, B. Zhou, and S. Shen, “Optimal time allocation for quadrotor trajectory generation,” in Proc. of the IEEE/RSJ Intl. Conf. on Intell. Robots and Syst.(IROS), Madrid, Spain, Oct 2018.
  • [39] P. F. Felzenszwalb and D. P. Huttenlocher, “Distance transforms of sampled functions,” Theory of computing, vol. 8, no. 1, pp. 415–428, 2012.
  • [40] T. Schouten and E. L. van den Broek, “Incremental distance transforms (idt),” in

    2010 20th International Conference on Pattern Recognition

    .   IEEE, 2010, pp. 237–240.
  • [41] M. Klingensmith, I. Dryanovski, S. Srinivasa, and J. Xiao, “Chisel: Real time large scale 3d reconstruction onboard a mobile device using spatially hashed signed distance fields.” in Proc. of Robot.: Sci. and Syst. (RSS), 2015.
  • [42] C. de Boor, “Subroutine package for calculating with b-splines.” Los Alamos Scientific Lab., N. Mex., Tech. Rep., 1971.
  • [43] S. Quinlan and O. Khatib, “Elastic bands: Connecting path planning and control,” 1993, pp. 802–807.
  • [44] Z. Zhu, E. Schmerling, and M. Pavone, “A convex optimization approach to smooth trajectories for motion planning with car-like robots,” in Proc. of the IEEE Control and Decision Conf. (CDC), 2015, pp. 835–842.
  • [45] F. Gao, Y. Lin, and S. Shen, “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), Sept 2017, pp. 3681–3688.
  • [46] V. Usenko, L. von Stumberg, A. Pangercic, and D. Cremers, “Real-time trajectory replanning for mavs using uniform b-splines and a 3d circular buffer,” in Proc. of the IEEE/RSJ Intl. Conf. on Intell. Robots and Syst.(IROS), 2017, pp. 215–222.