I Introduction
Recently, our community has witnessed the development of planning methods for quadrotors. Splinebased methods, which decompose the spatial and temporal parameters of a planning problem and focus on optimizing its spatial part, are widely applied for realtime applications [1, 2, 3].
Although splinebased methods can efficiently and accurately generate energyoptimal solutions for online usage, they usually omit temporal planning for simplicity. A typical spatialtemporal joint planning problem has high nonlinearity and nonconvexity coming from its objective and constraints. Since temporal planning has underlying coupling with spatial parameters and implicit gradients, the spatialtemporal joint optimization cannot be solved by general nonlinear programming (NLP) in realtime. Even though existing methods can provide online motion planning without temporal planning, they are often too conservative to be used for autonomous flights with high aggressiveness. To bridge this gap, we propose a framework to split the spatial and temporal aspects of a trajectory optimization problem, then solve them alternately. With our method, we can obtain the energytime joint optimal trajectory in milliseconds.
The proposed method is based on the naturally alternating structure of the spatialtemporal trajectory optimization and designed to have guaranteed optimality and efficiency. Our method is motivated by the fact that a polynomial trajectory with an odd order can be uniquely determined by its endpoint derivatives, i.e., the boundary condition, and the time duration. For a piecewise polynomial, once all boundary conditions are fixed, each piece of the trajectory depends only on its time duration, which can be optimized separately. By utilizing the widelyadopted linearquadratic objective
[4] of the optimization, the optimal time durations can be updated efficiently. Moreover, inspired by [2], the closedform solution is adopted to update derivatives on waypoints. Based on the above observations, the joint optimization can be efficiently processed by an alternating minimization (AM) procedure [5]. With our method, a largescale joint optimization can be done in a few milliseconds to generate optimal trajectory with the best time allocation.To the best knowledge of us, the proposed method is the first one that generates trajectories for a quadrotor with the spatialtemporal optimality, in such a limited time. Summarizing our contributions in this work:

An unconstrained alternating minimization algorithm is proposed to generate spatialtemporal optimal trajectories efficiently, with a proven nonasymptotic rate of the global/local convergence.

A computationally efficient feasibility check method is designed for a wide range of constraints in our method.

A constrained alternating minimization algorithm is constructed to optimize feasible trajectories in a recursive fashion, with global convergence verified.

The proposed method is integrated into an autonomous quadrotor system then evaluated by realworld experiments as well as extensive benchmarks. The source code is released for the reference of the community.
In what follows, we discuss related literature in Sec. II. Preliminaries of this paper are given in Sec. III. The proposed spatialtemporal trajectory generation method for unconstrained and constrained planning cases are detailed in Sec. IV and V, respectively. Experiments and benchmarks are given in Sec. VI. The paper is concluded in Sec. VII.
Ii Related Work
For quadrotor planning, polynomial splines have long been used for trajectory parametrization since [1], because of their flexibility and analytical convenience. In [1], the minimization of squared derivatives is used as the objective of quadratic programming (QP), which can be solved efficiently and accurately. Based on this formulation, intensive works have been recently proposed. A method for obtaining a closedform solution of the above QP program is proposed in [2], where a safe geometric path guides the generation of the trajectory to ensure its safety. By recursively adding intermediate waypoints to the path, a safe trajectory is finally generated after solving the minimumsnap problem several times. In [6, 7, 8]
, safe and dynamically feasible trajectories are online generated within a safe flight corridor, which excludes all obstacles in complex environments. However, in these methods, the time allocation of the piecewise trajectory is predefined or online adjusted by heuristics. Although these heuristics are cheap to compute, the trajectories generated are often far less optimal and overconservative, making them incapable of highspeed flights.
To address the time allocation problem, Mellinger et al. [1]
compute the projected gradient with respect to durations on a hyperplane where the sum is fixed. They optimize time allocation through backtracking gradient descent. Temporal scaling is applied on the solution until dynamical feasibility is achieved. Both the finite difference and scaling used in this method are expensive operations when the number of trajectory pieces is large. Liu et al.
[6] propose a way to calculate the proper scaling factor such that a single scaling operation suffices, while it only applies for resttorest trajectories. Sun et al. [9]formulate the problem as a twolevel optimization. They estimate the projected gradient analytically through the dual solution of the lowlevel QP, which improves accuracy compared with the numerical gradient. Nonetheless, the projected gradient is still inconvenient to compute and inefficient for nonlinear optimization. To avoid this situation, Richter et al.
[10] use total duration as a regularization term, thus making each duration an independent variable. The time allocation is optimized through gradient descent, while actuator constraints are also fulfilled by scaling. However, the optimal ratio of time allocation under the constrained case may differ a lot from the unconstrained case. Consequently, scaling can ruin a trajectory where constraints are violated on a very short piece. Burri et al. [3] optimize the squared total duration instead. They soften all constraints by penalizing them in the objective and optimize durations through an NLP solver, while the pieces number is limited.Gradientbased direct optimization is not satisfactory when computational overhead or solution quality is critical. To address this, Gao et al. [11] propose a method which decouples geometrical and temporal information of a trajectory. They use QP to generate a spatial trajectory with guaranteed safety in a virtual domain. A temporal trajectory is then optimized through secondorder conic programming (SOCP) based on direct collocation [12], which maps time to the virtual domain. Their method achieves near realtime. The drawback is that the spatial trajectory can restrict the aggressiveness in consequent optimization, resulting in slow motions. In [13], they improve the above method by alternating minimization between coefficients of the twolayer parametrization. Time durations of the optimized temporal trajectory is used to regenerate a spatial trajectory, on which the optimization is applied again. Although highquality trajectories can be obtained by this process, several rounds of QP and SOCP make it unapplicable to use online. Moreover, this method lacks theoretical convergence analysis and only works in the resttorest case. Almeida et al. [14]
propose a machine learning method to train a supervised neural network offline, thus online application is able to refine good initial guesses in realtime. However, the neural network has to be trained case by case.
In this paper, we adopt the time regularized objective [10] as well as the alternating minimization framework [13]. For efficiency, the spatial and temporal parameters are alternately updated in separate phases. Each minimization phase exploits the objective structure and is solved algebraically, making it free from gradient estimation and stepsize choosing. To handle various constraints, we also design a simple yet solid feasibility checker. The proposed framework is able to generate aggressive trajectories at extremely high frequency and not limited to the resttorest case.
Iii Preliminaries
Differential flatness of quadrotor dynamics is validated by Mellinger et al. [1]. It means the trajectory planning for a quadrotor can be done solely in the translational space. The kinodynamic feasibility is implicitly transformed into smoothness of the trajectory. Then, actuator constraints can be enforced by restricting norms on highorder derivatives.
In this paper, we employ the piecewise polynomial trajectory, with each piece denoted as an order polynomial:
(1) 
where is the coefficient matrix, is the duration and is a basis function.
It is worth noting that we only consider oddorder polynomial trajectories. Since is odd, the mapping is bijective between the coefficient matrix and the boundary condition. To further explain this, consider derivatives of up to order:
(2) 
we have where
(3) 
We denote and by and , respectively. The boundary condition of this polynomial is described by the tuple . The following mapping holds:
(4) 
where is the mapping matrix. is a nonsingular square matrix only if is an odd number. Otherwise, becomes overdetermined, which means for any given and , there may not exist polynomial whose satisfies (4).
Moreover, the inverse can be implemented with zero overhead when is an odd number. Burri et al. [3] explore the structure of and find that can be computed more efficiently through its SchurComplement, which only involves submatrix inverse. We take things one step further. Actually, all entries of are power functions of , thus GaussianElimination is applied to get its analytic form. Consequently, timeconsuming operation is no longer needed when is computed online. To achieve this, we precompute matrices offline, where :
Following mapping matrices are computed online:
Therefore, provided with an odd order, we show the practical equivalence between the tuple and in the sense of polynomial representation.
Consequently, we consider an piece trajectory parametrized by time allocation as well as boundary conditions of all pieces, as shown in Fig. 2. The trajectory is defined by
(5) 
where lies in the th piece and is a boundary condition of the th piece. This definition implicitly involves order continuity at boundaries of each piece. Normally, some entries in are fixed, such as the position of waypoints [2]. We split into two parts, the fixed part which is viewed as constant, and the free part which is to be optimized. Then, the whole trajectory can be fully determined by .
Iv SpatialTemporal Trajectory Optimization: Unconstrained Case
In this section, we describe our method for jointly optimizing spatial and temporal parameters of a trajectory, for the Unconstrained Case, where no constraint is considered.
Iva Optimization Objective
We use the time regularized quadratic cost over the whole trajectory, as the objective of the optimization:
(6) 
where and are the lowest and the highest order of derivative to be penalized respectively, is the weight of the order derivative and is the weight of time regularization. The weight adjusts the aggressiveness of the trajectory [3], which allows total duration varies adaptively. For now, we consider the unconstrained optimization:
(7) 
where free boundary conditions and durations are decision variables. is used for brevity.
The cost for the th piece can be calculated as
(8) 
in which is a symmetric matrix [10] consisting of high powers of , and is trace operation which only sums up diagonal costs produced in three dimensions. The overall objective can be formulated as
(9) 
(10) 
where is the direct sum of its diagonal blocks, and is a permutation matrix. We make sure that the setting for is legal by assuming that the sublevel set of for any finite is bounded and only consists of positive time allocation. For example, consecutive repeating waypoints with identical boundary conditions fixed in are not allowed.
IvB Unconstrained Trajectory Optimization
To optimize Eq. (9), we propose an AMbased algorithm, as shown in Alg. 1. Initially, is solved for the provided . After that, the minimization of the objective function is done through a twophase process, in which only one of and is optimized while the other is fixed.
In the first phase, the subproblem
(11) 
is solved for each . We employ the unconstrained QP formulation by Richter et al. [10], and briefly introduce it here. The matrix is partitioned as
(12) 
then the solution is be obtained analytically through
(13) 
For efficiency, we solve the sparse linear system
(14) 
through Sparse LU Factorization to get since and are both sparse.
In the second phase, the subproblem
(15) 
is solved for each . In this phase, the scale of subproblem can be reduced into each piece. Due to our representation of trajectory, once is fixed, the boundary conditions isolate each entry in from the others. Therefore, can be optimized individually to get all entries of . As for the th piece, its cost in (8) is indeed a rational function of . We show the structure of and omit the deduction for brevity:
(16) 
where and are orders of numerator and denominator, respectively. The coefficient is determined by . Due to positiveness of , we have as or . Therefore, the minimizer exists for
(17) 
To find all candidates, we compute the derivative of (16):
(18) 
The minimum exists in solutions of , which can be calculated through any modern univariate polynomial realroots solver [15]. In this paper, we utilize the Continued Fraction method [16] to isolate all positive roots of any high order () polynomial. The second phase is completed by updating every entry in .
IvC Convergence Analysis
Alg. 1 is globally convergent. Moreover, it is faster than conventional gradient descent used in time allocation refinement, under no assumption on convexity.
Theorem 1.
Consider the process described in Algorithm 1. Provided with any , the following inequality always holds for the th iteration:
where and are both constant, is Frobenius norm. It shows the process globally converges to a stationary point with nonasymptotic sublinear rate .
Proof.
See [17] for details. ∎
Thm. 1 shows that our algorithm shares the same global convergence rate as that of gradient descent with the best stepsize chosen in each iteration [18]. The best stepsize is practically unavailable. In contrast, our algorithm does not involve any stepsize choosing in its iterations. Subproblems in Eq. (11) and Eq. (15) both are solved exactly and efficiently due to their algebraic convenience. Therefore, Alg. 1 is faster than gradientbased methods in practice.
Another key advantage of our algorithm is its capability of escaping from a local minimum in the time optimization. Watching Eq. (9), despite is convex in as proved in [17], it is a rational function which can have multiple local minima in . Therefore, a case may occur where the initial time allocation falls into one of these local minima instead of the global minimum in . Under this situation, naturally, the global minimum in time allocation cannot be attained by gradientbased methods. However, with our method, all local minima are compared directly. Thus, the situation can be avoided.
It is worth noting that, here the global optimality is not guaranteed because our algorithm still exploits local structure of the problem. Although convergence to a stationary point is ensured, we argue that strict saddle points are theoretically and numerically unstable for our firstorder AM method [19]. Moreover, when the stationary point is a strict local minimum, we show that the convergence rate is faster.
Theorem 2.
Let denote any strict local minimum of to which Alg. 1 converges. There exist and , such that
for all , where .
Proof.
See [17] for details. ∎
Thm. 2 shows that a faster convergence rate can be attained for a strict local minimum than the general case in Thm. 1. Note that it is possible to accelerate our method to attain the optimal rate of firstorder methods or use secondorder methods to achieve better performance. However, we still employ the firstorder AM process because of its simplicity in implementation and its good performance when the trajectory is far from optimum.
The nonasymptotic property implies that the convergence is bounded strictly by the rate, rather than approximated. This property, along with the monotone decrease of the objective, shows guaranteed progress in each iteration, while gradientbased methods may try bad stepsize thus making no/negative progress.
V SpatialTemporal Trajectory Optimization: Constrained Case
In this section, we present our method to incorporate safety and dynamical feasibility constraints into our optimization process. To begin with, we introduce a computationally efficient feasibility check method that applies to a wide range of constraints. Then this method is used in a constrained trajectory optimization process.
Va Computationally Efficient Feasibility Check
A piece of the trajectory is denoted by
(19) 
Constraint over should be satisfied by the order derivative of the piece. It is required that has the form of a multivariate polynomial:
(20) 
where is the highest degree. Many kinds of constraints can be expressed by , such as the safe distance constraint to keep away from an obstacle located at :
or maximum speed constraint:
Provided with any piece , we check whether constraint is fulfilled for all . We define which is indeed a polynomial of . The procedure is as follows: Firstly, check the sign of and . Then, If both two endpoints satisfy the constraint, we have to make sure the constraint is not violated inside the interval . Instead of locating all extrema of and checking their values, we only need to check the existence of root of in the interval. If the equation has any root in , then is infeasible. Fortunately, it is convenient for a polynomial to achieve this leveraging Sturm’s Theory [20]. Now that neither nor is the root of , we compute the Sturm sequence by
(21)  
where is remainder in the Euclidean division of by [20]. When becomes constant, we stop expanding this sequence. Let denote the number of sign variations of Sturm sequence at , in which zero values should be ignored. Then the number of distinct roots inside equals . Here the feasibility check is done for . Sometimes, constraint has the form . In practice, it can be equally handled by checking , where is a small positive real number. What’s more, nonpolynomial constraint can also be efficiently checked through its Taylor series within acceptable approximation error.
VB Constrained Trajectory Optimization
For the Constrained Case, we enforce constraints on norms of derivatives of the trajectory:
(22)  
(23)  
(24) 
Generally, the constraint does not have to be like (23). If only a constraint is representable in (20) and its feasible solution can be constructed, then it can be handled in our optimization. With a slight abuse of notation, we use to denote that is feasible. is used to denote that the th piece is feasible. We say is tight by meaning that, at least one constraint is tight at a on the th piece.
The constrained version of our method is shown in Alg. 2. An initial feasible trajectory can be constructed from conservative time allocation. The spatialtemporal parameters are then recovered from the trajectory, which is used in the consequent twophase constrained minimization.
In the first phase, is fixed. An illustration is provided in Fig. 3(a), where the unconstrained minimum is obtained as is done in Alg. 1. The trajectory may not be feasible. Since the feasibility of is ensured in last iteration, a line search is done as
(25) 
where is the convex combination of and . Convexity of implies the convexity of . Moreover, is a feasible solution, while is the unconstrained global minimum. We simply take if it is feasible. If not, a bisection procedure is done on the interval . In this procedure, the feasibility check method described in Sec. VA is employed to shrink the interval. The procedure stops at an acceptable interval length, with taking the feasible lower bound. After that, we update by . Meanwhile, a set is maintained to store indices of tightened pieces.
In the second phase, is fixed. An illustration is given in Fig. 3(b). Each entry in is updated by solving
(26) 
As stated in Alg. 1, all extrema of can be computed exactly. However, the constrained minimum may not exist in them. It can be any at which some constraints are exactly tightened. When infeasible extremum exists, must be located between any infeasible extremum and the neighboring feasible one or . A bisection procedure with feasibility check suffices to compute . After that, we compare on those feasible extrema together with .
When all iterations are done, the set indicates pieces stuck by active constraints. If is not empty, we recursively apply Alg. 2 on split subtrajectories, while boundary conditions of pieces indexed by should be totally fixed. Finally, is updated and returned. The recursive process is essential, since it ensures that pieces with no room for optimization do not prevent other pieces to decrease the objective. Alg. 2 is globally convergent to a solution set where constraints are tight or local minimum is attained, which can be checked by Zangwill’s theorem [22].
Vi Results
Via Comparison of Feasibility Check Methods
Firstly, we compare our feasibility check method with Mueller’s recursive bound check [21], Burri’s analytical extrema check [3], as well as the widely used samplingbased check. In each case, 1000 trajectory pieces are randomly generated along with velocity constraints to estimate average time consumption. As is shown in Fig. 4, our method outperforms all other methods in computation speed because of its resolution independence and scalability with higher polynomial orders. The recursive check and samplingbased check may have false positives under rough temporal resolution. The efficiency of analytical check and recursive check deteriorates with higher polynomial orders, because both of them involve roots finding which has closedform solution only for low order (). In comparison, our method is able to do a solid feasibility check within .
ViB Benchmark for Trajectory Optimization Methods
Secondly, we conduct the benchmark comparison of our trajectory optimization method against stateoftheart methods. The benchmark is done as follows: We generate a sequence of waypoints by random walk, of which the step is uniformly distributed over
for each axis. The maximum speed and acceleration rates are set to and , respectively. The derivatives on the first and the last waypoints are set to zero. The objective function is set as , , and . For a given number of pieces, each method is applied to 1000 sequences of waypoints. The optimization process stops until the relative decrease of objective is less than . The cost is then normalized by the cost of Alg. 1. An illustration in Fig. 5 shows that the minimized cost in unconstrained case can be used as a baseline. All comparisons are conducted on an Intel Core i78700 CPU under Linux environment.We compare our method with Richter’s method [2] and Mellinger’s method [1]. Richter’s method optimizes trajectory derivatives on waypoints through an unconstrained QP while time allocation is adjusted by gradient descent and scaling. To use it in constrained case, we soften the constraints by penalizing them in objective function as suggested in [3] and directly optimize the time allocation through NLopt [23]. Mellinger’s method optimizes the time allocation with total duration fixed first, using backtracking gradient descent (BGD). Then dynamical feasibility is ensured through time scaling, of which the ratio is properly calculated using Liu’s method [6].
, the performance of different methods are provided. Dashed lines indicate standard deviation.
As is shown in Fig. 6(b), our Alg. 2 has the fastest speed and the lowest cost when constraints are taken into consideration. Our method is capable of computing trajectories with pieces within , i.e., at least. However, both benchmarked methods fail to accomplish realtime computing for trajectories with more pieces. Moreover, our Alg. 2 always obtains better trajectories in terms of the cost function, while benchmarked methods cannot fully utilize the capability of system dynamics.
ViC Aggressive Flight Experiment
To validate the performance of our method in realworld applications, we deploy it on a selfdeveloped compact quadrotor platform. The proposed method is implemented with C++ 11, and all tasks are conducted on an onboard computer with Intel Core i78550U CPU. The pose of our quadrotor is obtained through a robust visualinertial state estimator [24]. Besides, no external positioning system nor offboard computing is used. A geometric controller is employed for trajectory tracking control [25].
The experiment is conducted in a complex indoor scene, which is shown in Fig. 1. A globally consistent map of the scene is prebuilt, from which some compulsory waypoints are selected offline. At the beginning of each flight, an optimal path is produced by RRT* [26], which starts from an initial position and passes all compulsory waypoints. Our method generates an optimal trajectory online based on this path within milliseconds. Immediately, the quadrotor starts its aggressive flight. Different from parameters used in benchmark, we set , and . The aggressive flight along with the generated trajectory is shown in Fig. 7. More details are included in the attached video.
Vii Conclusion
In this paper, we propose an efficient trajectory generation method for quadrotor aggressive flight, which has guaranteed convergence and feasibility. Benchmarks for components in our method show its superior computation speed, trajectory quality as well as scalability against stateoftheart methods. Aggressive flight experiments in limited space with dense obstacles validate the practical performance of our method. Currently, in the proposed framework, positions of waypoints are fixed during optimization. However, the method is underlying compatible with waypoints as part of decision variables. Our feasibility checker also supports various safety constraints. In the future, we plan to apply and improve our method in timecritical largescale motion planning scenarios where complex spatial constraints exist.
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., Shanghai, China, May 2011.
 [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, Singapore, Dec 2013.
 [3] M. Burri, H. Oleynikova, M. Achtelik, and R. Siegwart, “Realtime visualinertial mapping, relocalization and planning onboard mavs in unknown environments,” in Proc. of the IEEE/RSJ Intl. Conf. on Intell. Robots and Syst., Hamburg, Germany, Sep 2015.
 [4] D. P. Bertsekas, Dynamic Programming and Optimal Control, 1995.
 [5] A. Beck, FirstOrder Methods in Optimization, 2017.
 [6] 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.
 [7] 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., Brisbane, Australia, May 2018.
 [8] L. CamposMacías, D. GómezGutiérrez, R. AldanaLópez, R. de la Guardia, and J. I. ParraVilchis, “A hybrid method for online trajectory planning of mobile robots in cluttered environments,” IEEE Robotics and Automation Letters (RAL), vol. 2, pp. 935–942, 2017.
 [9] W. Sun, G. Tang, and K. Hauser, “Fast UAV trajectory optimization using bilevel optimization with analytical gradients,” ArXiv, vol. abs/1811.10753, 2018.
 [10] A. Bry, C. Richter, A. Bachrach, and N. Roy, “Aggressive flight of fixedwing and quadrotor aircraft in dense indoor environments,” The International Journal of Robotics Research, vol. 34, pp. 1002–969, 2015.
 [11] 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., Madrid, Spain, Oct 2018.
 [12] D. Verscheure, B. Demeulenaere, J. Swevers, J. De Schutter, and M. Diehl, “Timeoptimal path tracking for robots: A convex optimization approach,” IEEE Transactions on Automatic Control, vol. 54, no. 10, pp. 2318–2327, 2009.
 [13] F. Gao, L. Wang, B. Zhou, L. Han, J. Pan, and S. Shen, “Teachrepeatreplan: A complete and robust system for aggressive flight in complex environments,” ArXiv, vol. abs/1907.00520, 2019.
 [14] M. M. de Almeida, R. Moghe, and M. R. Akella, “Realtime minimum snap trajectory generation for quadcopters: Algorithm speedup through machine learning,” in Proc. of the IEEE Intl. Conf. on Robot. and Autom., Montreal, Canada, May 2019.
 [15] M. Sagraloff and K. Mehlhorn, “Computing real roots of real polynomials,” Journal of Symbolic Computation, vol. 73, pp. 46–86, 2013.
 [16] E. P. Tsigaridas and I. Z. Emiris, “Univariate polynomial real root isolation: Continued fractions revisited,” in Proc. of the Conf. on Ann. Euro. Sym., Zwitserland, Zurich, Sep 2006.
 [17] Z. Wang, X. Zhou, C. Xu, and F. Gao, “Detailed proofs of alternating minimization based trajectory generation for quadrotor aggressive flight,” https://arxiv.org/abs/2002.09254.
 [18] Y. Nesterov, Lectures on Convex Optimization, 2018.
 [19] J. D. Lee, I. Panageas, G. Piliouras, M. Simchowitz, M. I. Jordan, and B. Recht, “Firstorder methods almost always avoid strict saddle points,” Mathematical Programming, vol. 176, pp. 311–337, 2019.
 [20] S. Basu, R. Pollack, and M.F. Roy, Algorithms in Real Algebraic Geometry, 2003.
 [21] M. W. Mueller, M. Hehn, and R. D’Andrea, “A computationally efficient motion primitive for quadrocopter trajectory generation,” IEEE Transactions on Robotics, vol. 31, pp. 1294–1310, 2015.
 [22] W. I. Zangwill, Nonlinear programming: A Unified Approach, 1972.
 [23] S. G. Johnson, “The NLopt nonlinearoptimization package.”
 [24] T. Qin, P. Li, and S. Shen, “VINSMono: A robust and versatile monocular visualinertial state estimator,” IEEE Transactions on Robotics, vol. 34, no. 4, pp. 1004–1020, 2018.
 [25] T. Lee, M. Leok, and N. H. McClamroch, “Geometric tracking control of a quadrotor UAV on SE(3),” in Proc. of the IEEE Control and Decision Conf., Atlanta, Georgia, USA, Dec 2010.
 [26] S. Karaman and E. Frazzoli, “Samplingbased algorithms for optimal motion planning,” The International Journal of Robotics Research, vol. 30, pp. 846 – 894, 2011.