I Introduction
Unmanned aerial vehicles (UAVs) are recently involved in more and more applications, such as industrial inspection, searchandrescue and package delivery. To achieve full autonomy in these scenarios, the motion planning module plays an essential role in generating safe and smooth motions.
Although plenty of works on quadrotor trajectory generation have been proposed, there are still two critical unsolved issues. Firstly, given limited time and onboard computing resources, no existing works guarantee to generate safe and kinodynamic feasible trajectory at a high success rate. However, the efficiency and robustness of the trajectory generation are essential. In many circumstances, such as a quadrotor flying at high speed in unknown environments, trajectories should be regenerated constantly in a very short time to avoid emergent threats. Secondly, to ensure the kinodynamic feasibility of the generated motions, constraints on velocity and acceleration are often enforced conservatively. As a result, the aggressiveness of the generated trajectories are often hard to be tuned to satisfy applications where a high flight speed is preferable.
In this paper, we propose a complete and robust online trajectory generation method to address these two issues systematically. A kinodynamic path searching based on heuristic search and linear quadratic minimumtime control is adopted. It searches efficiently for a safe, feasible and minimumtime initial path in the discretized control space. The initial path is then refined in a carefully designed Bspline optimization, which utilizes Bspline’s convex hull property to incorporate gradient information and dynamic constraints. It improves the initial path and converges quickly to a smooth, safe and dynamically feasible trajectory. Finally, the trajectory is represented as a nonuniform Bspline, for which we investigate the relations between the control points of derivatives and time allocation. Based on the relations, an iterative time adjustment method is adopted to squeeze infeasible velocity and acceleration out from the profiles while avoiding constraining them conservatively.
Compared to existing works, our proposed method is able to generate highquality trajectories in cluttered environments in a much shorter time with a higher success rate. It can generate aggressive motion under the premise of dynamic feasibility. We show the efficiency and robustness of our method in numerous simulational complex environments. We also demonstrate that our method is competent even for challenging fast flight when trajectories should be regenerated repeatedly in a very short time by realworld experiments. We summarize our contributions as follows:
1) We propose a robust and efficient systematic method, incorporating kinodynamic path searching, Bspline optimization and time adjustment, where safety, dynamic feasibility and aggressiveness are built from bottomup.
2) We present an optimization formulation based on the convex hull property of Bsplines that delicately incorporates gradient information and dynamic constraints, which converges quickly to generate smooth, safe and dynamically feasible trajectories.
3) We investigate the relations between the control points of derivatives and the time allocation of nonuniform Bsplines. A time adjustment method based on the relations is applied to guarantee feasible and nonconservative motion.
4) We present extensive simulation and realworld evaluation of our proposed method. The source code is released as a rospackage.
Ii Related Work
Ii1 Hardconstrained methods
The problems of trajectory generation have been addressed by some work recently. Hardconstrained methods are pioneered by minimumsnap trajectory generation [1], in which trajectories are represented as piecewise polynomials and generated by solving a quadratic programming(QP) problem. [2] shows that minimum snap trajectories can be obtained in closed form, in which the safety of the trajectories is ensured by iteratively adding intermediate waypoints. Works[3, 4, 5, 6, 7] generate trajectories in a twostep pipeline. Free space represented by a sequence of cubes[3, 8], spheres[4, 9] or polyhedrons[5] is firstly extracted, which is followed by convex optimization, which generates smooth trajectory within the feasible space. [6, 7] proposed a Bsplinebased kinodynamic search to find an initial trajectory which is then refined by an elastic band optimization approach. The use of uniform Bspline ensures dynamic feasibility but could generate conservative motion. One common drawback of these methods is that the time allocation of the trajectory is given by naive heuristics. However, a poorly chosen time allocation significantly reduce the quality of the trajectory. Besides, a feasible solution can only be obtained by iteratively adding more constraints and solving the quadratic programming problem, which is undesirable for realtime application. To address these problems, [8] proposed a method to search for a path with wellallocated time and guarantee the safety and kinodynamic feasibility of trajectory through optimization. Hardconstrained methods ensure global optimality by the convex formulation. However, distance to obstacles in the free space is ignored, which often results in trajectories being close to obstacles. Besides, the kinodynamic constraints are conservative, making the trajectory’s speed deficient for fast flight.
Ii2 Softconstrained methods
There are also methods formulating trajectory generation as a nonlinear optimization problem that takes smoothness and safety into account. [10] generates discretetime trajectories by minimizing its smoothness and collision costs using gradient descent methods.[11] has similar problem formulation, but the optimization is solved by a gradientfree sampling method. [12] extended them to continuoustime polynomial trajectories. Since the time parameterization is continuous, it avoids numeric differentiation errors and is more accurate to represent the motions of quadrotors. However, it suffers from a low success rate. To solve this problem, [13] finds a collisionfree initial path firstly using an informed samplingbased path searching method. This path serves as a higher quality initial guess of nonlinear optimization and thus improve the success rate. In [14], the trajectory is parameterized as a uniform Bspline. Since a Bspline is continuous by nature, there is no need to enforce continuity explicitly, which reduce the number of constraints. It is also particularly useful for local replanning thanks to its property of locality. Softconstrained methods utilize gradient information to push trajectory far from obstacles, but suffer from local minima and having no strong guarantee of success rate and kinodynamic feasibility. Our optimization method also utilizes gradient information to improve the safety of the trajectory. However, unlike previous methods in which computational expensive line integrals along the trajectory are calculated, the formulation is redesigned to be simpler based on the convex hull property of Bspline. It greatly improves the computation efficiency as well as the convergent rate.
Iii Kinodynamic Path Searching
Our frontend kinodynamic path searching module is originated from the hybridstate A* search first proposed for autonomous vehicle[15]. It searches for a safe and kinodynamic feasible trajectory that is minimal with respect to time duration and control cost in a voxel grid map. As shown in Alg.1 and in Fig.2, the searching loop is similar to the standard A* algorithm, where and refer to the open and closed set. Instead of straight lines, motion primitives respecting the quadrotor dynamic are used as graph edge. A structure Node is used to record a primitive, the voxel the primitives ends in and the and cost (Sect. IIIB). Primitives Expand() the voxel grid map iteratively and those ending up in the same voxel except the one with the smallest are pruned (Prune()). Then CheckFeasible() checks the safety and dynamic feasibility of the remained primitives. This process continues until any primitive reach goal or the AnalyticExpand() (Sect. IIIC) succeeds.
Iiia Primitives Generation
We first discuss the generation of motion primitives used in Expand(). The differential flatness of quadrotor systems allows us to represent the trajectory by three independent 1D timeparameterized polynomial functions[1]:
(1) 
where . From the view of quadrotor systems, it corresponds to a linear timeinvariant (LTI) system. Let
be the state vector. Let
be the control input. The state space model can be defined as:(2)  
The complete solution for the state equation is expressed as:
(3) 
which gives the trajectory of the quadrotor system whose initial state is and control input is .
In Expand(), given the current state of the quadrotor, a set of discretized control inputs is applied for duration . In practice, we select , which corresponds to a double integrator. Each axis is discretized uniformly as , which results in primitives.
IiiB Actual Cost and Heuristic Cost
As we aim to find a trajectory that is optimal in time and control cost, we define the cost of a trajectory as:
(4) 
Under this definition, EdgeCost() calculates the cost of a motion primitive generated with the discretized input and duration as .
Following the terminology of A*, we use to represent the actual cost of an optimal path from the start state to the current state . Let this optimal path consists of primitives, is calculated as: .
An admissible and informative heuristic is essential to speed up the searching as in A*. Hence, we also design a Heuristic(). We compute a closed form trajectory that minimizes from to the goal state by applying the Pontryagin’s minimum principle[16]:
(5)  
where are the current and goal position and velocity. is the cost defined by Equ.4. To find the optimal time that minimize the cost, we substitute into and find the roots of . The root making a minimum cost and feasible trajectory is selected and denoted as . We use as the heuristic . Finally, is defined as: .
IiiC Analytic Expansion
Due to the discretized control input, it is difficult to have a primitive end exactly in the goal state. To compensate for it and also to speed up the searching, we induce an analytic expansion scheme. When a node is popped from the open set, a trajectory from to is computed using the same approach in Sect. IIIB. If it passes the safety and dynamic feasibility check, the searching is terminated in advance. This strategy is effective for improving efficiency especially in sparse environments, since it has a higher success rate and terminates the searching earlier.
IiiD Optimality and Completeness
Theoretically, we can not guarantee the optimality and completeness of the path searching. However, the practical performance is satisfactory. For the optimality, evaluation (Sect. VIIA1) shows that the sacrifice of optimality is acceptable and adjustable. Besides, provided the initial path lies near the optimum, our optimization (Sect. IV) will find that optimum. For the completeness, evaluation (Sect. VIIA1) indicates that in practice it can find a feasible solution in most case. Also, our method can be extended to support variableduration primitives and a variableresolution voxel grid as described by [15] to make stronger completeness guarantees.
Iv Bspline Trajectory Optimization
As mentioned in Sect. IIID, the path produced by the path searching can be suboptimal. In addition, this path is often close to obstacle since distance information in the free space is ignored (Fig. 5). Therefore, we improve the smoothness and clearance of the path in the proposed Bspline optimization. The convex hull property of uniform Bsplines are utilized to incorporate gradient information from the Euclidean distance field and dynamic constraints, for which it converges within a very short duration to generate smooth, safe and dynamically feasible trajectories.
Iva Uniform Bsplines
A Bspline is a piecewise polynomial uniquely determined by its degree , a set of control points and a knot vector , in which , and . A Bspline trajectory is parameterized by time , where . For a uniform Bspline, each knot span has identical value . To evaluate the position at time , we first normalize as . Then the position can be evaluated using the matrix representation[17]:
(6)  
here is a constant matrix determined by . In our implementation, is set as . The evaluation of the derivatives is exactly the same, since the derivative of a Bspline is also a Bspline.
IvB Convex Hull Property
The convex hull property (Fig.3) is used extensively in our method to ensure both dynamic feasibility and safety.
For the dynamic feasibility, it suffices to constrain all velocity and acceleration control points and so that and . and are calculated by Equ.7, where is the knot span:
(7) 
For the safety of the Bspline, we need to ensure that all its convex hulls are collisionfree. Equivalently, we need to ensure that , where is the distance between any one occupied voxel and any one point in the convex hull (Fig.4). By the triangle inequality, we have , where is the distance between the voxel and any one control point. We also have , since is inside the convex hull. Combining them, is always valid. Therefore, if we ensure:
(8) 
then the convex hull is guaranteed to be collisionfree.
IvC Problem Formulation
For a degree Bspline trajectory defined by control points , we optimize the subset of control points . The first and last control points should not be changed because they determine the boundary state. The overall cost function is defined as:
(9) 
where and are smoothness and collision cost. and are soft limits on velocity and acceleration. and trade off the smoothness, safety and dynamic feasibility.
We define the smoothness cost by a function that captures the geometric information of the trajectory and does not depend on time allocation, unlike many recent works that use integral of the squared snap or jerk. The reason is that after optimization the time allocation may be adjusted (Sect. V). This will change the derivatives of the trajectory and make the optimized snap (jerk) less meaningful. We use an elastic band cost function ^{2}^{2}2The control points and are not optimized but needed to evaluate the overall smoothness. Similarly, some of them are included in Equ.14 as constant values to evaluate the derivatives. [18, 19]:
(10) 
From a physical standpoint, this formulation view a trajectory as an elastic band, where each term and is the joint force of two springs connecting the nodes and
respectively. If all terms equal to zero, all the control points would uniformly distribute in a straight line, which is ideally smooth.
Similarly, the collision cost is formulated as the repulsive force of the obstacles acting on each control point:
(11) 
where is the distance between and the closet obstacle. is a differentiable potential cost function with specifying the threshold of obstacle clearance:
(12) 
As shown in Sect.IVB, Equ.8 must be satisfied so that the trajectory is collisionfree. Since the collision cost pushes the control points away from obstacles, is apparent. Also, are tunable parameters depend solely on the parameterization of the Bspline. In practice, as long as we select that are significantly small (in our implementation ), the trajectory is safe in most cases. This may be invalid in extreme cases, for instance, the environment is very cluttered. Even so, we can reparameterize the Bspline to select smaller , after which Equ.8 will still be satisfied.
We penalize velocity or acceleration along the trajectory exceeding maximum allowable value and with a cost similar to Equ.12. The penalty for 1D velocity is:
(13) 
where . The acceleration penalty has identical form. Applying the convex hull property (Fig. 3), we define and so that infeasible velocity and acceleration control points are penalized:
(14) 
V Time Adjustment
Although we constrain kinodynamic feasibility in the path searching and optimization, sometimes we get infeasible trajectories. The basic reason is that gradient information tends to lengthen the overall trajectory while pushing it far from obstacles. Consequently, the quadrotor has to fly more aggressively in order to travel longer distance within the same time, which unavoidably causes over aggressive motion if the original motion is already near to the physical limits.
To guarantee dynamic feasibility, we adopt a time adjustment method based on the relations between the derivatives control points and the time allocation (knot spans) of the nonuniform Bspline. Thanks to the relations, we can change the flight aggressiveness as we expected by adjusting the associated time allocation. Thus dynamic feasibility can be ensured without overconservative constraints.
We first introduce the mathematic fundament of the time adjustment. Then the Alg. 2 is presented to tackle overaggressive trajectories.
Va Nonuniform Bspline
Nonuniform Bspline is a more general kind of Bspline. The only difference to uniform Bspline is that each of its knot span is independent to others. The control points of a nonuniform Bspline’s first and second order derivatives and can be computed by:
(15) 
By the convex hull property (Fig.3), to enforce the dynamic feasibility of a trajectory represented by a nonuniform Bspline, it suffices to enforce all control points of the first and second order derivatives within the feasible domain. We show that this can be achieved by changing the corresponding knot spans of the infeasible control points in Sect. VB.
VB Knot Spans Adjustment
Let be an infeasible control point of velocity. Let be the largest infeasible component and . From Equ.15 we know that is influenced by the duration . If we change this duration to , then changes to:
(16)  
Therefore, if we set , then the velocity is feasible, because .
The enforcement of acceleration feasibility is similar^{3}^{3}3We use the same notation as those in the last paragraph and do not define them explicitly for brevity. . We know that is actually influenced by since it is coupled with and . We change to for and we get:
(17)  
Similarly, let , then .
VC Iterative Time Adjustment
Based on the derivation in Sect.VB, Alg. 2 is adopted to enforce dynamic feasibility. It iteratively finds the infeasible velocity and acceleration control points and of the trajectory (Line 2) and adjust the corresponding knot spans (Lines 310). Because a knot span influences a few control points and vice versa, bounding and with two constant and slightly larger than (Line 5, 9) prevents any time span from being extended excessively.
Vi Implementation Details
Via Experiment Settings
The motion planning method proposed in this paper is implemented in C++11 with a general nonlinear optimization solver NLopt^{4}^{4}4https://nlopt.readthedocs.io/en/latest/. We set for the path searching, for the optimization and for the time adjustment in all experiments. We present two sets of realworld experiments to validate our proposed planning method.
Firstly, we conduct fast autonomous flight experiments in unknown cluttered environments (Sect.VIIB). We use a selfdeveloped quadrotor platform (Fig. 6(a)) equipped with a Velodyne VLP16 3D LiDAR. LOAM[20]
is adopted to estimate the pose of the quadrotor and generate a dense point cloud map. To obtain highrate state estimation for feedback control, we fused the laserbased estimation with IMU and sonar measurements by the extended Kalman filter (EKF). All modules including motion planning, state estimation, mapping and control run on a dualcore 3.00 GHz Intel i75500U processor, which has 8 GB RAM and 256 GB SSD.
Then, in Sect.VIIC, we focus on testing the fastreplanning capability of our proposed method in aggressive flight, for which we use a more lightweight and agile quadrotor platform (Fig. 6(b)). To eliminate uncertainties introduced by onboard sensings, accurate pose feedback is provided by the motion capture system OptiTrack^{5}^{5}5https://optitrack.com/ and the map of the environment is prebuilt. The motion planning and control modules run onboard on an Nvidia TX2 computer.
ViB Replanning Strategy
ViB1 Recedinghorizon Local Planning
When the quadrotor flies in an unknown environment, it has to replan its trajectory frequently due to the limited sensing range. To improve efficiency, we adopt a recedinghorizon planning scheme, in which trajectories are generated only within the known space (Fig.7). The path searching is terminated once a motion primitive ends outside this range and is followed by the optimization and time adjustment. Planning in the unknown space is often useless, thus such efforts can be saved.
ViB2 Replanning Triggering Mechanism
The replanning is triggered in two situations. Firstly, it is triggered if the current trajectory collides with newly emergent obstacles^{6}^{6}6The collision checking is conducted in the configuration space (Cspace), where the quadrotor is modeled as a point and obstacles are inflated by a radius. The computation cost of such checking is negligible. , which ensures that a new safe trajectory is available as soon as any collision is detected. Secondly, the planner is called at fixed intervals of time. It updates the trajectory periodically using the most uptodate environmental information.
ViC Euclidean Distance Field
We maintain an EDF of the voxel grid map for our optimization, which is computed by an efficient algorithm [21], where
is the number of updated voxel grids. To compensate for the discretized error of the EDF introduced by the voxel grid map and benefit the numeric optimization, trilinear interpolation is used to improve the accuracy of the distance and gradient information
[14]. Global update of the EDF is very costly and can block the planning module that is crucial for fast autonomous flight. To address this issue, we only update the voxel grids within the sensing range using an incremental update strategy[22].Vii Results
Viia Analysis and Comparisons
ViiA1 Comparison of Path Searching
We compare our path searching with method [23], both of which use the timeoptimal control formulation to generate primitives. The comparison is done on a map randomly deployed with 100 obstacles and the maximum velocity and acceleration limits are set as and respectively. Since the resolution of voxel grids is a critical factor for the performance of our proposed method, different resolutions are used for comprehensive evaluation (Tab.I, column 1, rows 35). For a fair comparison, we use the open source implementation of [23]. Results are listed in Tab.I.
As is shown in statics, both methods generate kinodynamic feasible trajectories. Our method is faster with one order of magnitude and tends to generate a path with a shorter duration. However, the control cost for it is slightly higher. As the voxel girds get coarser, the efficiency of our method increases at the expense of higher control cost and lower success rate. This trend is expected because pruning primitives with coarser voxel grids results in lower searching complexity, whereas more feasible (and maybe superior) paths are lost.






method[23]  Mean  0.0592  8.439  11.42  100.0  
Max  0.8740  20.000  28.00  
Std  0.1060  2.970  4.07  

Mean  0.0047  7.719  14.42  100.0  
Max  0.1837  10.500  24.50  
Std  0.0113  0.909  3.60  

Mean  0.0018  7.696  15.08  100.0  
Max  0.0287  10.500  27.50  
Std  0.0017  0.917  3.78  

Mean  0.0017  7.645  16.20  77.8  
Max  0.0059  15.500  56.00  
Std  0.0007  1.295  6.21 
ViiA2 Comparison of Optimization
For the backend trajectory optimization, we conduct a comparison against our previous work[13]. Both of our previous method and the proposed method utilize the EDF for nonlinear optimization. For fairness, we use the same path given by our path searching as the initial value. Firstly, we compare the costs of the objective function with respect to time for both methods (Fig. 8(a)). Obviously, the cost of the proposed method drops rapidly within the first few milliseconds, while the other one decreases much slower. Secondly, comparison of smoothness (integral of the squared jerk) is conducted as shown in Fig.8(b) and Tab.II. Even though less time is given for the proposed method, the resulting trajectories are smoother.

Integral of Jerk ( 


Mean  Max  Std  
Previous[13]  43.913  181.495  18.394  0.010  
Proposed  35.932  131.913  13.118  0.001 
ViiB Onboard Autonomous Flight
We conducted fully autonomous fast flight experiments in a challenging unknown environment (Fig. 9(a)). To further challenge our method, we prune the global map using a sphere with a radius of that centered on the quadrotor and only use the map within this sphere for trajectory generation (Fig.9(b)9(d)) which is much smaller than our real perception range. The unstructured environment, limited perception range as well as the high flying speed pose a challenge to the motion planning module, as it should regenerate trajectory continually and rapidly upon sudden appearing of new threats. We refer the readers to the video attachment for more detailed information.
ViiC Aggressive Flight
The aggressive flight experiment is done in the environment depicted in Fig. 10. In the experiment, the goals of the quadrotor are changed constantly and arbitrarily by a human. As soon as a new goal is set, a new trajectory is replanned and executed immediately. The maximum velocity and acceleration are set as and respectively. This task is challenging in several aspects. Since the flight is aggressive and the changes in the goals are abrupt, the motion planning module should generate new trajectories in considerably short time to quickly react to the changes, so that the motion of the quadrotor is continuous and smooth. Also, as the environment is confined and cluttered, it is difficult to generate smooth, safe and dynamically feasible trajectories in a very short time. This experiment validates that our method can generate aggressive motion under the premise of feasibility. It also shows that our method can quickly generate a new trajectory in complex environments even if the goal is changed suddenly during the aggressive flight. More details are also included in the video.
Viii Conclusion
In this paper, we propose a novel online motion planning method for quadrotor autonomous navigation. We decouple the online fast motion planning problem as a frontend kinodynamic path searching and a backend nonlinear trajectory optimization. We adopt a kinodynamic path searching to find a safe, kinodynamic feasible and minimumtime initial path, which is further improved in smoothness and clearance by a gradientbased optimization. By utilizing the convex hull property of Bspline, we significantly improve the efficiency and convergent rate of the optimization compared to previous gradientbased planning methods. Finally, by representing the trajectory as a nonuniform Bspline, we adjust the time allocation according to a given expected flight aggressiveness. We validate our proposed method in various complex environments and the simulation. The competence of the method is also validated in challenging realworld tasks.
In the future, we plan to challenge our quadrotor system in extreme situations such as largescale or dynamic environments. Furthermore, we will extend our trajectory optimization method to swarm problems.
References
 [1] 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.
 [2] 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.
 [3] J. Chen, K. Su, and S. Shen, “Realtime safe trajectory generation for quadrotor flight in cluttered environments,” in Proc. of the IEEE Intl. Conf. on Robot. and Biom., Zhuhai, China, Aug. 2015.
 [4] 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.
 [5] 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 3d complex environments,” IEEE Robotics and Automation Letters (RAL), pp. 1688–1695, 2017.
 [6] W. Ding, W. Gao, K. Wang, and S. Shen, “Trajectory replanning for quadrotors using kinodynamic search and elastic optimization,” in 2018 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2018, pp. 7595–7602.
 [7] ——, “An efficient bsplinebased kinodynamic replanning framework for quadrotors,” arXiv preprint arXiv:1906.09785, 2019.
 [8] 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.
 [9] 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. [Online]. Available: https://onlinelibrary.wiley.com/doi/abs/10.1002/rob.21842
 [10] M. Zucker, N. Ratliff, A. D. Dragan, M. Pivtoraiko, M. Klingensmith, C. M. Dellin, J. A. Bagnell, and S. S. Srinivasa, “Chomp: Covariant hamiltonian optimization for motion planning,” The International Journal of Robotics Research, vol. 32, no. 910, pp. 1164–1193, 2013.
 [11] M. Kalakrishnan, S. Chitta, E. Theodorou, P. Pastor, and S. Schaal, “Stomp: Stochastic trajectory optimization for motion planning,” in 2011 IEEE international conference on robotics and automation. IEEE, 2011, pp. 4569–4574.
 [12] H. Oleynikova, M. Burri, Z. Taylor, J. Nieto, R. Siegwart, and E. Galceran, “Continuoustime 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.
 [13] F. Gao, Y. Lin, and S. Shen, “Gradientbased online safe trajectory generation for quadrotor flight in complex environments,” in Proc. of the IEEE/RSJ Intl. Conf. on Intell. Robots and Syst.(IROS), Sept 2017, pp. 3681–3688.
 [14] V. Usenko, L. von Stumberg, A. Pangercic, and D. Cremers, “Realtime trajectory replanning for mavs using uniform bsplines and a 3d circular buffer,” in 2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). IEEE, 2017, pp. 215–222.
 [15] D. Dolgov, S. Thrun, M. Montemerlo, and J. Diebel, “Path planning for autonomous vehicles in unknown semistructured environments,” The International Journal of Robotics Research, vol. 29, no. 5, pp. 485–501, 2010.
 [16] M. W. Mueller, M. Hehn, and R. D’Andrea, “A computationally efficient motion primitive for quadrocopter trajectory generation,” IEEE Transactions on Robotics, vol. 31, no. 6, pp. 1294–1310, 2015.
 [17] K. Qin, “General matrix representations for bsplines,” The Visual Computer, vol. 16, no. 3, pp. 177–186, 2000.
 [18] S. Quinlan and O. Khatib, “Elastic bands: Connecting path planning and control,” in [1993] Proceedings IEEE International Conference on Robotics and Automation. IEEE, 1993, pp. 802–807.
 [19] Z. Zhu, E. Schmerling, and M. Pavone, “A convex optimization approach to smooth trajectories for motion planning with carlike robots,” in 2015 54th IEEE Conference on Decision and Control (CDC). IEEE, 2015, pp. 835–842.
 [20] J. Zhang and S. Singh, “Loam: Lidar odometry and mapping in realtime,” in Proc. of Robot.: Sci. and Syst. (RSS), UCB, USA, July 2014, pp. 109–111.
 [21] P. F. Felzenszwalb and D. P. Huttenlocher, “Distance transforms of sampled functions,” Theory of computing, vol. 8, no. 1, pp. 415–428, 2012.

[22]
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.  [23] S. Liu, N. Atanasov, K. Mohta, and V. Kumar, “Searchbased motion planning for quadrotors using linear quadratic minimum time control,” in Proc. of the IEEE/RSJ Intl. Conf. on Intell. Robots and Syst.(IROS), Sept 2017, pp. 2872–2879.