Alternating Minimization Based Trajectory Generation for
Quadrotor Aggressive Flight

We use the time regularized quadratic cost over the whole trajectory, as the objective of the optimization:

where $D_{min}$ and $D_{max}$ are the lowest and the highest order of derivative to be penalized respectively, $w_{i}$ is the weight of the $i$-order derivative and $\rho$ is the weight of time regularization. The weight $\rho$ adjusts the aggressiveness of the trajectory [3], which allows total duration varies adaptively. For now, we consider the unconstrained optimization:

where free boundary conditions and durations are decision variables. $J(\mathbf{D}_{P},\mathbf{T}):=J(\mathbf{\Phi}(\mathbf{D}_{P},\mathbf{T}))$ is used for brevity.

The cost $J_{m}$ for the $m$-th piece can be calculated as

in which $\mathbf{Q}(T_{m})$ is a symmetric matrix [10] consisting of high powers of $T_{m}$, and $\operatorname*{Tr}\left\{{\cdot}\right\}$ is trace operation which only sums up diagonal costs produced in three dimensions. The overall objective can be formulated as

where $\mathbf{H}(\mathbf{T})$ is the direct sum of its $M$ diagonal blocks, and $\mathbf{C}$ is a permutation matrix. We make sure that the setting for $J$ is legal by assuming that the $\alpha$-sublevel set of $J(\mathbf{D}_{P},\mathbf{T})$ for any finite $\alpha$ is bounded and only consists of positive time allocation. For example, consecutive repeating waypoints with identical boundary conditions fixed in $\mathbf{D}_{F}$ are not allowed.

To optimize Eq. (9), we propose an AM-based algorithm, as shown in Alg. 1. Initially, $\mathbf{T}^{0}$ is solved for the provided $\mathbf{D}_{P}^{0}$. After that, the minimization of the objective function is done through a two-phase process, in which only one of $\mathbf{D}_{P}$ and $\mathbf{T}$ is optimized while the other is fixed.

In the first phase, the sub-problem

is solved for each $\mathbf{T}^{k}$. We employ the unconstrained QP formulation by Richter et al. [10], and briefly introduce it here. The matrix $\mathbf{R}(\mathbf{T})=\mathbf{C}^{\mathrm{T}}\mathbf{H}(\mathbf{T})\mathbf{C}$ is partitioned as

then the solution is be obtained analytically through

For efficiency, we solve the sparse linear system

through Sparse LU Factorization to get $\mathbf{D}_{P}^{*}(\mathbf{T})$ since $\mathbf{H}(\mathbf{T})$ and $\mathbf{C}$ are both sparse.

In the second phase, the sub-problem

is solved for each $\mathbf{D}_{P}^{k}$. In this phase, the scale of sub-problem can be reduced into each piece. Due to our representation of trajectory, once $\mathbf{D}_{P}$ is fixed, the boundary conditions $\mathbf{D}$ isolate each entry in $\mathbf{T}$ from the others. Therefore, $T_{m}$ can be optimized individually to get all entries of $\mathbf{T}^{*}(\mathbf{D}_{P})$. As for the $m$-th piece, its cost $J_{m}$ in (8) is indeed a rational function of $T_{m}$. We show the structure of $J_{m}$ and omit the deduction for brevity:

where $p_{n}=2D_{max}-1$ and $p_{d}=2(D_{max}-D_{min})+N-1$ are orders of numerator and denominator, respectively. The coefficient $\alpha_{i}$ is determined by $\mathbf{d}_{m}$. Due to positiveness of $J_{m}(T)$, we have $J_{m}(T)\rightarrow+\infty$ as $T\rightarrow+\infty$ or $T\rightarrow 0^{+}$. Therefore, the minimizer exists for

To find all candidates, we compute the derivative of (16):

The minimum exists in solutions of ${\mathrm{d}J_{m}(T)}/{\mathrm{d}T}=0$, which can be calculated through any modern univariate polynomial real-roots solver [15]. In this paper, we utilize the Continued Fraction method [16] to isolate all positive roots of any high order ($\geq 5$) polynomial. The second phase is completed by updating every entry $T_{m}^{*}(\mathbf{D}_{P})$ in $\mathbf{T}^{*}(\mathbf{D}_{P})$.

Alg. 1 is globally convergent. Moreover, it is faster than conventional gradient descent used in time allocation refinement, under no assumption on convexity.

Thm. 1 shows that our algorithm shares the same global convergence rate as that of gradient descent with the best step-size chosen in each iteration [18]. The best step-size is practically unavailable. In contrast, our algorithm does not involve any step-size choosing in its iterations. Sub-problems in Eq. (11) and Eq. (15) both are solved exactly and efficiently due to their algebraic convenience. Therefore, Alg. 1 is faster than gradient-based 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 $J(\mathbf{D}_{P},\mathbf{T})$ is convex in $\mathbf{D}_{P}$ as proved in [17], it is a rational function which can have multiple local minima in $T_{m}$. Therefore, a case may occur where the initial time allocation falls into one of these local minima instead of the global minimum in $(0,+\infty)$. Under this situation, naturally, the global minimum in time allocation cannot be attained by gradient-based 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 first-order AM method [19]. Moreover, when the stationary point is a strict local minimum, we show that the convergence rate is faster.

Thm. 2 shows that a faster convergence rate $O(1/K)$ 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 $O(1/K^{2})$ of first-order methods or use second-order methods to achieve better performance. However, we still employ the first-order AM process because of its simplicity in implementation and its good performance when the trajectory is far from optimum.

The non-asymptotic 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 gradient-based methods may try bad step-size thus making no/negative progress.

A piece of the trajectory is denoted by

Constraint $G(p_{1}^{(i)}(t),p_{2}^{(i)}(t),p_{3}^{(i)}(t))<{0}$ over $[0,T]$ should be satisfied by the $i$-order derivative of the piece. It is required that $G$ has the form of a multivariate polynomial:

where $d_{g}$ is the highest degree. Many kinds of constraints can be expressed by $G$, such as the safe distance constraint to keep away from an obstacle located at $({0,0,0})^{\mathrm{T}}$:

or maximum speed constraint:

Provided with any piece $p(t)$, we check whether constraint $G$ is fulfilled for all $t\in[0,T]$. We define $\mathcal{G}(t):=G(p_{1}^{(i)}(t),p_{2}^{(i)}(t),p_{3}^{(i)}(t))$ which is indeed a polynomial of $t$. The procedure is as follows: Firstly, check the sign of $\mathcal{G}(0)$ and $\mathcal{G}(T)$. Then, If both two endpoints satisfy the constraint, we have to make sure the constraint is not violated inside the interval $(0,T)$. Instead of locating all extrema of $\mathcal{G}(t)$ and checking their values, we only need to check the existence of root of $\mathcal{G}(t)=0$ in the interval. If the equation has any root in $(0,T)$, then $p(t)$ is infeasible. Fortunately, it is convenient for a polynomial to achieve this leveraging Sturm’s Theory [20]. Now that neither $0$ nor $T$ is the root of $\mathcal{G}(t)=0$, we compute the Sturm sequence $g_{0}(t),g_{1}(t),g_{2}(t),\cdots$ by

where $Rem(g_{k-1}(t),g_{k}(t))$ is remainder in the Euclidean division of $g_{k-1}(t)$ by $g_{k}(t)$ [20]. When $g_{k}(t)$ becomes constant, we stop expanding this sequence. Let $V_{sign}(\tau)$ denote the number of sign variations of Sturm sequence at $t=\tau$, in which zero values should be ignored. Then the number of distinct roots inside $(0,T)$ equals $V_{sign}(0)-V_{sign}(T)$. Here the feasibility check is done for $G(\cdot,\cdot,\cdot)<{0}$. Sometimes, constraint has the form $G(\cdot,\cdot,\cdot)\leq{0}$. In practice, it can be equally handled by checking $G(\cdot,\cdot,\cdot)<\epsilon$, where $\epsilon$ is a small positive real number. What’s more, non-polynomial constraint can also be efficiently checked through its Taylor series within acceptable approximation error.

In conclusion, our method converts the feasibility check into the root existence check for polynomial, without computing the root itself. Our method is straightforward and involves no redundant operations, in comparison with methods used in [21] and [3].

For the Constrained Case, we enforce constraints on norms of derivatives of the trajectory:

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 $\mathbf{G}(\mathbf{D}_{P},\mathbf{T})\leq 0$ to denote that $\mathbf{\Phi}(\mathbf{D}_{P},\mathbf{T})$ is feasible. $\mathbf{G}(\mathbf{d}_{m},T_{m})\leq{0}$ is used to denote that the $m$-th piece is feasible. We say $\mathbf{G}(\mathbf{d}_{m},T_{m})\leq{0}$ is tight by meaning that, at least one constraint is tight at a $t$ on the $m$-th piece.

The constrained version of our method is shown in Alg. 2. An initial feasible trajectory $\mathbf{P}^{0}$ can be constructed from conservative time allocation. The spatial-temporal parameters $(\mathbf{D}_{P}^{0},\mathbf{T}^{0})$ are then recovered from the trajectory, which is used in the consequent two-phase constrained minimization.

In the first phase, $\mathbf{T}^{k}$ is fixed. An illustration is provided in Fig. 3(a), where the unconstrained minimum $\widetilde{\mathbf{D}}_{P}$ is obtained as is done in Alg. 1. The trajectory $\mathbf{\Phi}(\widetilde{\mathbf{D}}_{P},\mathbf{T}^{k})$ may not be feasible. Since the feasibility of $(\mathbf{D}_{P}^{k},\mathbf{T}^{k})$ is ensured in last iteration, a line search is done as

where $\mathbf{D}(\lambda)$ is the convex combination of $\widetilde{\mathbf{D}}_{P}$ and $\mathbf{D}_{P}^{k}$. Convexity of $J(\cdot,\mathbf{T}^{k})$ implies the convexity of $J(\mathbf{D}(\cdot),\mathbf{T}^{k})$. Moreover, $\lambda=0$ is a feasible solution, while $\lambda=1$ is the unconstrained global minimum. We simply take $\lambda^{*}=1$ if it is feasible. If not, a bisection procedure is done on the interval $[0,1]$. In this procedure, the feasibility check method described in Sec. V-A is employed to shrink the interval. The procedure stops at an acceptable interval length, with $\lambda^{*}$ taking the feasible lower bound. After that, we update $\mathbf{D}_{P}^{k+1}$ by $\mathbf{D}(\lambda^{*})$. Meanwhile, a set $\Delta$ is maintained to store indices of tightened pieces.

In the second phase, $\mathbf{D}_{P}^{k+1}$ is fixed. An illustration is given in Fig. 3(b). Each entry in $\mathbf{T}^{k}$ is updated by solving

As stated in Alg. 1, all extrema of $J_{m}(T)$ can be computed exactly. However, the constrained minimum may not exist in them. It can be any $\widetilde{T}$ at which some constraints are exactly tightened. When infeasible extremum exists, $\widetilde{T}$ must be located between any infeasible extremum and the neighboring feasible one or $T_{m}^{k}$. A bisection procedure with feasibility check suffices to compute $\widetilde{T}$. After that, we compare $J_{m}(T)$ on those feasible extrema together with $\widetilde{T}$.

When all iterations are done, the set $\Delta$ indicates pieces stuck by active constraints. If $\Delta$ is not empty, we recursively apply Alg. 2 on split sub-trajectories, while boundary conditions of pieces indexed by $\Delta$ should be totally fixed. Finally, $\mathbf{P}^{*}$ 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].

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 sampling-based 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 sampling-based 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 closed-form solution only for low order ($\leq 4$). In comparison, our method is able to do a solid feasibility check within $1\mu s$.

Secondly, we conduct the benchmark comparison of our trajectory optimization method against state-of-the-art methods. The benchmark is done as follows: We generate a sequence of waypoints by random walk, of which the step is uniformly distributed over $[-3.0m,8.0m]$ for each axis. The maximum speed and acceleration rates are set to $5.0m/s$ and $3.5m/s^{2}$, respectively. The derivatives on the first and the last waypoints are set to zero. The objective function is set as $\rho=512.0$, $D_{max}=D_{min}=3$, $w_{3}=1.0$ and $N=5$. 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 $0.001$. 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 i7-8700 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].

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 $60$ pieces within $5ms$, i.e., $150Hz$ at least. However, both benchmarked methods fail to accomplish real-time 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.

To validate the performance of our method in real-world applications, we deploy it on a self-developed compact quadrotor platform. The proposed method is implemented with C++ 11, and all tasks are conducted on an onboard computer with Intel Core i7-8550U CPU. The pose of our quadrotor is obtained through a robust visual-inertial 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 pre-built, 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 $v_{max}=4.0m/s$, $a_{max}=4.5m/s^{2}$ and $\rho=1024.0$. The aggressive flight along with the generated trajectory is shown in Fig. 7. More details are included in the attached video.