Optimal Time Allocation for Quadrotor Trajectory Generation

Fei Gao, William Wu, Jie Pan, Boyu Zhou and Shaojie Shen

Abstract— In this paper, we present a framework to dooptimal time allocation for quadrotor trajectory generation.Using this method, we can generate minimum-time piecewisepolynomial trajectories for quadrotor flights. We decouple thequadrotor trajectory generation problem into two folds. Firstlywe generate a smooth and safe curve which is parameterizedby a virtual variable. This curve named spatial trajectory isindependent of time and has fixed spatial properties. Then amapping function which decides how the quadrotor moves alongthe spatial trajectory respecting kinodynamic limits is found byminimizing total trajectory time. The mapping function mapsthe virtual variable to time is named temporal trajectory. Weformulate the minimum-time temporal trajectory generationproblem as a convex program which can be efficiently solved.We show that the proposed method can corporate with varioustypes of previous trajectory generation method to obtain theoptimal time allocation. The proposed method is integrated intoa customized light-weight quadrotor platform and is validatedby presenting autonomous flights in indoor and outdoor en-vironments. We release our code for time optimization as anopen-source ros-package.

I. INTRODUCTION

In recent years, micro aerial vehicles (MAVs), especiallyquadrotors, have drawn increasing attention on various ap-plications. Thanks to their mobility, agility, and flexibility,quadrotors can fly rapidly and safely through complex envi-ronments while avoiding collisions. One of the most crucialissues for quadrotor flights is the motion/path planning,which coordinates the quadrotor to navigate from a startpoint to a target location in cluttered environments. Beyondthe safety requirement, which has been satisfied in manyapproaches [1], [2], time optimality and kinodynamic feasi-bility are also significant issues we consider. On the premiseof satisfying kinodynamic limits, for most applications ofquadrotors, optimality in time means efficiency in operation.

For quadrotor motion planning and trajectory generation,the piecewise polynomial-based trajectory has been widelyadopted since [3] and [1], because of its superior represen-tative capability and concise formulation. Although variantsof piecewise polynomial trajectory have been proposed, timeallocation is still a bottleneck for it. We can use the scenarioof a car/quadrotor racing in a sharp turn for illustration.No matter how high is the speed before entering the sharpturn, the acceleration of the vehicle has to be bounded whileturning. In this case, a poor time allocation would takea significantly longer time for passing the sharp turn. In

All authors are with the Department of Electronic and ComputerEngineering, Hong Kong University of Science and Technology, HongKong, China. fgaoaa@ust.hk, wwuar@connect.ust.hk,jpanai@connct.ust.hk, uv.boyuzhou@gmail.com,eeshaojie@ust.hk

(a) Composite image of the indoor quadrotor flight.

(b) Composite image of the outdoor quadrotor flight.

Fig. 1. Composite images of the quadrotor flying in indoor and outdoorexperimnets. Our quadrotor platform equipped with a monocular cameraand an IMU along with an Intel i7 CPU. We demonstrate fast-speed flightsby using our proposed method in both indoor and outdoor environments.The video recording experiments is available in the attachment of this paper.

this paper, we present a framework to generate minimumtime trajectories under the constraints of physical limits. Theproposed framework is decoupled into two stages. Firstlywe generate trajectories in the spatial aspect. Instead ofparameterizing the trajectory directly by time, we generatetime independent trajectories in a virtual domain, with fixedgeometric distributions. Secondly, we bridge the spatial tra-jectory to the temporal information. The relations betweenthe virtual variable to time variable is found by minimizingthe total flight time globally considering the dynamicallimits. Therefore vehicles with our proposed time allocationmethod can pass the sharp turn with minimal time; relatedtests are given in Sect. V-C.2.

This work is motivated by the observation that for arobotics trajectory generation application, most of the casesthe geometrical and temporal information are not necessarilycoupled. For safety consideration, the trajectory should bewrapped to avoid obstacles [4] [5] or be bounded withinfree space [2] [6]. To this end, geometrical properties ofthe trajectory are the only concern. On the other side,for dynamical feasibility consideration, temporal informationof the trajectory, such as velocity and acceleration of thequadrotor should be bounded within the kinodynamic limits.In many works, although the kinodynamic is constrainedat the same time with the generation of the geometricaltrajectory [6] [7], it is highly dependent on the time allocationand is often too conservative to fully utilize the actuators forhigh-speed navigations. In this paper, we propose a methodto get the optimal time allocation for quadrotor trajectory

generation, and we summarize our contributions as:1) Decouple the trajectory generation problem into spa-

tial and temporal sub-problems. Moreover, utilize ourprevious methods [8], [6] to generate trajectories withspatial safety guarantee in a virtual domain.

2) Formulate the temporal trajectory optimization prob-lem as a convex SOCP (Second Order Conic Program)based on [9] and extend the approach to piecewisetrajectory formulation for a jerk-controlled quadrotor.

3) Integrate the proposed method into an autonomousquadrotor platform and present real-time onboarddemonstrations. Fast autonomous flights in indoor andoutdoor environments are presented. The source codewill be released as an add-on to any other piecewisepolynomial trajectory generation methods.

We discuss related literature in Sect. II. Our safe spatialtrajectory generation and minimal-time temporal trajectorymethods are detailed in Sect. III and Sect. IV, respectively.In Sect. V, benchmark results as well as indoor and outdoorexperimental results which show fast autonomous flights arepresented. The paper is concluded in Sect. VI.

II. RELATED WORK

For safe trajectory generation, many methods wereproposed, ranging from soft constrained formulation tohard constrained formulation. Gradient-based methods likeCHOMP [4] formulates the trajectory optimization problemas a nonlinear optimization over the penalty of the safety andthe smoothness. In [5] and [8] the gradient-based methodsare applied on piecewise polynomial trajectories. Mellingeret al. [3] pioneered a minimum-snap trajectory generationalgorithm, where piecewise polynomial functions representthe trajectory, and the trajectory generation problem is for-mulated as a quadratic program (QP) problem. Closed formsolution for minimum snap trajectory generation is obtainedin [1], where the safety of the trajectory is achieved by itera-tively adding intermediate waypoints in a discrete path foundby RRT*. Our previous work [6] carves a flight corridorconsisting of convex regions against the environments andconfines the trajectory within the corridor by utilizing theconvex hull property of Bernstein polynomial basis.

The time parameterization or so-called time allocationdominates the progress of the trajectory in the time domain,and is regarded as one of the main reasons leads to sub-optimality [1]. One way to achieve reasonable time alloca-tion is by iteratively refining allocated times using gradientdescent [1]. However, iteratively regenerating the trajectoryprevents this method to be used in real-time, especially whenthe trajectory generation is costly. In our previous work [10],we allocate times for each piece of the trajectory accordingto the Euclidean distance between the segmentation pointsand rely on the overlapping regions of the flight corridorto adjust the time allocation. Another standard way is usingsome heuristic [11] [12] to allocate the time. All of the abovemethods have no guarantee on the optimality of the time.

Minimum time trajectory or so-called time-optimal tra-jectory generation aims at fully utilizing the ability of

actuators of mobile robots to travel as fast as possible withoutviolating kinodynamic limits. Methods can be divided intodirect methods [13] which directly generate spatial-temporaloptimal trajectory and indirect methods [14] which generatethe trajectory independent of time firstly and then find therelationship between the trajectory and time. The proposedapproach in this paper is an indirect method. For directmethods, a recent result is presented in [13], where thetrajectory generation problem is formulated as a minimum-time optimal control based graph search. The linear modelof the quadrotor is established, and a graph consists ofminimum time controlled motion primitives is formed. Thismethod considers the time optimality when searching a safeand dynamical feasible trajectory towards the goal. However,the complexity of the search grows exponentially as thegraph size increases. Also, linearizing the quadrotor modelat hover point can lead to high error when flying at highacceleration. In [14], the geometric shape of the trajectoryis fixed at a virtual domain firstly, which is the same asour method. Then the mapping function from the virtualparameter to time is optimized by nonlinear optimization.Compared to our approach, this method needs a kinodynamicfeasible initial solution, and the convergence rate is slow.Also, it cannot guarantee the global optimality. The approachproposed in [15] also fixes the geometric trajectory andfinds the mapping function by iteratively adds mappingpoints along the spatial trajectory to squeeze the kinodynamicfeasibility. However, this method also needs numerous nu-merical iterations, making it unsuitable for onboard usage.Our proposed method in this paper is based on an indirectmethod used for robotic-arm path tracking [9]. For findingthe optimal mapping function from the virtual domain tothe time domain, the virtual variable is discretized. Thisdiscretization is essential since it makes the problem easilytractable in a convex formulation. In this paper, we develop aframework based on [9] to get the optimal time allocation ofpiecewise polynomial trajectory for quadrotors and apply theproposed method on applications of fast quadrotor flights.

III. SAFE SPATIAL TRAJECTORY GENERATION

A. Corridor-Based Method

As presented in our previous work [6], we firstly extractthe free space in environments to form a flight corridorconsisting of convex shapes and then constrain piecewiseBezier curve entirely within these convex shapes. The Beziercurve consists of Bernstein polynomial basis with respect toa virtual parameter s is written as:

Bj(s) = c0jb0n(s) + . . .+ cnj b

nn(s) =

n∑i=0

cijbin(s), (1)

where [c0j , c1j , ..., c

nj ] is the set of control points of the jth

piece of the Bezier curve. b(n) is the Bernstein polynomialbasis, n is the degree. Suppose we denote the piecewiseBezier curve in one dimension µ out of x, y, z as fµ(s). In ourapplications, since we only want to fix the geometrical shapeof the trajectory in s domain, we simply set the duration of s

in each piece of the trajectory as 1. For generating a minimaljerk curve, the objective is written as:

J =

x,y,z∑µ

∫ s(T )

0

(d3fµ(s)

ds3

)2

ds, (2)

where T is the total time of the trajectory and s(T ) is thecorresponding s value. Note here although the minimumjerk objective can efficiently smooth the trajectory, it hasa totally different physical meaning compared to the one intime domain. In practice, the minimal 3rd order objectivein s domain often results an un-necessary longer trajectorysince we set s ∈ [0, 1] for all pieces of the trajectory ignoringthe length of the segment. To overcome this disadvantage,we add a minimal arc length regularization term into thisobjective. The arc length of a given curve fµ(s) is:

L =

∫ s(T )

0

√f ′x(s)2 + f ′y(s)2 + f ′z(s)

2ds, (3)

which is a line integral and can not be written in a closedform. We instead use L2 as the regularization term andthe objective function is ws · J + wl · L2, where ws andwl are relative weights for minimizing jerk in s domainand length of the curve. The detailed objective is in aquadratic formation and is omitted here for brevity. Utilizingthe convex hull property, the entire curve can be boundedwithin the flight corridor by adding safety constraints onall control points. The trajectory generation problem is aquadratic program (QP) which can be solved in polynomialtime. For details about the formulation and the applicationwe refer readers to our previous publication [6].

B. Gradient-Based Method

For another category of the trajectory generation, thetrajectory is optimized as a nonlinear optimization whichtakes into account the smoothness, safety and dynamicalfeasibility of the trajectory. By using the spatial-temporaldecoupling strategy, the dynamical feasibility terms can bedropped. Thus the objective function can be written as f =ws · J + wc · C, where J is exactly the same as Equ.3. Thecollision cost C is the line integral of the arc length along thetrajectory and can be discretized for numerical calculation.The collision cost C in s domain is:

C =

∫ s(T )

0

c(p(s))√f ′x(s)2 + f ′y(s)2 + f ′z(s)

2ds

=

N∑k=0

c(p(sk))√f ′x(sk)2 + f ′y(sk)2 + f ′z(sk)2δs,

(4)

where c(p) is the penalty function and is designed as anexponential function of distance in our previous work [8].The trajectory generation problem is in a nonlinear opti-mization formulation. By using numerical optimization tech-niques such as Levenberg-Marquardt method, the nonlinearoptimization results in a smooth and collision-free trajectory.Note here that the dropping of dynamical feasibility termssignificantly reduces computation time and improves theconvergence rate, which is detailed in V-A.

IV. MINIMUM-TIME TEMPORAL TRAJECTORYGENERATION

A. Representing the Temporal Trajectory

For a piecewise spatial trajectory defined in virtual domains, the geometric properties such as the x, y, z positions andtheir derivatives are fixed. However, the kinodynamic prop-erties of the quadrotor such as the velocity and accelerationare decided by a piecewise mapping function which mapseach s value to a time value t. In this paper, we name thetrajectory of velocity and acceleration in the time domainas temporal trajectory. Our motivation in this section is todesign a mapping function which can generate motion asfast as possible without violating the kinodynamic limits ofthe quadrotor. Suppose we have already solved the spatialtrajectory fµ(s) and denote the relation between the variables of the trajectory to time t as a function s(t). Obviously,s(t) is also a piecewise function which can be written asfollows:

s(t) =

s0(t), s1(0) = 0, s0(T0) = 1, s0 ∈ [0, 1],

s1(t), s1(0) = 0, s1(T1) = 1, s1 ∈ [0, 1],...

...sm(t), sm(0) = 0, sm(Tm) = 1, sm ∈ [0, 1],

(5)where T0, T1, ...Tm are unknown time durations in eachpieces of the temporal trajectory.

Another obvious observation is that s(t) must be a mono-tonically increasing function since physically variable timecan only increase. Therefore the following condition holds:

s(t) ≥ 0. (6)

By chain rule, we can re-write the temporal trajectory, i.e.the velocity and acceleration of the quadrotor as:

vµ(t) = fµ(s) = f ′µ(s) · s,aµ(t) = fµ(s) = f ′µ(s) · s+ f ′′µ (s) · s2. (7)

Here we use the notation c = dc/dt, c = d2c/dt2 to indicatetaking the 1st and 2nd order derivative of a variable c withrespect to time, and use c′ = dc/ds, c′′ = d2c/ds2 forderivatives with respect to s.

B. Piecewise Minimum-Time Optimization

1) Objective: The objective J of the temporal trajectorygeneration is to minimize the total time, which is written as:

J = T =

∫ T

0

1dt. (8)

With the fact s = ds/dt and in each piece of the trajectorysi(t) ∈ [0, 1], the time T can be re-written as:

T =

m∑i=0

∫ 1

0

1

sids. (9)

In minimum-time optimal control problem [16], the energyof the control input is penalized in the objective functionto trade-off the time optimality and control extremeness. In

our problem, we can also incorporate the regularization onchanging rate of s in our objective:

J =

m∑i=0

∫ 1

0

( 1

si+ ρ · si2

)ds, (10)

where ρ is the weighting parameter. Following the directtranscription method in [9], we introduce two additionalfunctions a(s) and b(s) which are also piecewise and satisfy:

a(s) = s, (11)

b(s) = s2. (12)

Then the objective is derived as to minimize:

J =

m∑i=0

∫ 1

0

( 1√bi(s)

+ ρ · ai(s)2)ds, (13)

where the subscript i of ai(s) and bi(s) shares the samemeaning as in si. Several observations obviously hold fol-lowing the formulation above, including:

bi(s) ≥ 0, (14)b′i(s) = 2 · ai(s). (15)

2) Continuity Constraints: The generated spatial-temporaltrajectory must be continuous at several order derivatives tomeet the requirements in quadrotor control. According toour experience and the actuator model of the quadrotor, werequire the continuity up to acceleration holds between eachpieces of the trajectory.

The continuity of position is achieved by the spatial trajec-tory in Sec. III. For velocity and acceleration, the continuitiesare enforced by setting equality constraints on the joint pointbetween two consecutive pieces of the temporal trajectory,for the ith and (i + 1)th piece trajectories, in dimensionµ ∈ {x, y, z} we have:

f ′i,µ(1) ·√bi(1) = f ′i+1,µ(0) ·

√bi+1(0), (16)

f ′i,µ(1) · ai(1) + f ′′i,µ(1) · bi(1)

=f ′i+1,µ(0) · ai+1(0) + f ′′i+1,µ(0) · bi+1(0) (17)

3) Kinodynamic Constraints: To ensure the kinodynamicfeasibility, we enforce constraints on velocity and acceler-ation in each piece of the trajectory. Constraints hold ins ∈ [0, 1] at x, y, z dimensions are written as:

− vmax ≤ f ′i,µ(s) ·√b(s) ≤ vmax, (18)

− amax ≤ f ′i,µ(s) · a(s) + f ′′i,µ(s) · b(s) ≤ amax, (19)

where vmax and amax are the kinodynamic limits of thequadrotor decided by the actuator models.

4) Boundary Constraints: In practice, the trajectory isgenerated from the initial state of the quadrotor to a final state(normally but not necessarily zero state). For the temporaltrajectory, the boundary condition is to meet the initialvelocity and acceleration a0, v0, and the terminal velocity

and acceleration af , vf . The constraints are:

f ′0,µ(0) ·√b0(0) = v0µ, (20)

f ′m,µ(1) ·√bm(1) = vfµ, (21)

f ′0,µ(0) · a0(0) + f ′′0,µ(0) · b0(0) = a0µ, (22)

f ′m,µ(1) · am(1) + f ′′m,µ(1) · bm(1) = afµ, (23)

Note that if the initial and final states are both static, theremust exist a solution satisfying the boundary constraints.Otherwise, the optimization program may be infeasible.

C. Convex SOCP Reformulation

To make the above optimization problem easily solvable,we would like to re-formulate the objective and constraintsunder the disciplined convex formalism. To this end, the keystep is to discretize the virtual parameter s into grids [9]for each piece of the trajectory. Then a(s) is regarded aspiecewise constant in each grid of s and therefore b(s) ispiecewise linear according to Equ. 15. For each piece ofthe trajectory, si ∈ [0, 1] is discretized to ski , where k =0, 1, ...,K, according to a given number K. We have ski −sk−1i = ∆s = 1/K. For each piece, ai(s) and bi(s) aremodeled by introducing discrete aki and bki , for which bki isassigned at each ski and aki is assigned at the middle of skiand sk+1

i . Then b(s) is written as piecewise linear function:

b(s) = bki +bk+1i − bki

∆s· (s− ski ). (24)

According to Equs. 14 and 15, we have

bki ≥ 0, (25)

bk+1i − bki = 2 ·∆s · aki . (26)

The objective function in Equ. 13 is derived as:

J =

m∑i=0

K−1∑k=0

(2√

bk+1i +

√bki

+ ρ · aki2)·∆s, (27)

which is equivalent to the quadratic function:m∑i=0

K−1∑k=0

(2 · dki + ρ · aki

2)·∆s, (28)

by introducing slack variables dki , plus additional constraints:

1√bk+1i +

√bki

≤ dki , k = 0, ...K − 1, i = 0, ...m. (29)

Equ. 29 are transformed to quadratic forms as

1

ck+1i + cki

≤ dki , k = 0, ...K − 1, i = 0, ...m. (30)

cki ≤√bki , k = 0, ...K, i = 0, ...m. (31)

by introducing slack variables cki in each segment.Equ. 30 is equivalent to the typical formulation of rotated

quadratic cones:

2 · dki ·(ck+1i + cki

)≥√

22, (32)

which is shorthanded in

(dki , ck+1i + cki ,

√2) ∈ Q3

r. (33)

And Equ. 31 can be written in the typical formulation ofquadratic cones:(

bki + 1)2 ≥ (bki − 1

)2+(2 · cki

)2, (34)

and be shorthanded as

(bki + 1, bki − 1, 2cki ) ∈ Q3. (35)

Finally, an additional slack variable t is added to convert thequadratic objective in Equ. 28 to an affine form as

m∑i=0

K−1∑k=0

2∆s · dki + ρ ·∆s · t, (36)

with a rotated quadratic cone

2 · t · 1 ≥m∑i=0

K−1∑k=0

(aki )2, (37)

i.e.(t, 1, a) ∈ Q2+K·(m+1)

r , (38)

where a = [a00, a10, ..., a

K−1m ] is the vectorized variable

consisting of all aki assigned to each segment.Up to now, the original objective function in Equ. 13

is converted to an affine function (Equ. 36) with severalinduced rotated second-order cones (Equ. 33 and 38) andsecond-order cones (Equs. 35). Back to constraints in theoriginal problem, the kinodynamic constraints (Sec. IV-B.3)are written in discrete form as:

−vmax ≤f ′i,µ(ski ) ·√bki ≤ vmax, k = 0, ...K. (39)

−amax ≤f ′i,µ(sk+1/2i ) · aki + (40)

f ′′i,µ(sk+1/2i ) · bk+1/2

i ≤ amax, k = 0, ...K − 1

where sk+1/2i = (ski + sk+1

i )/2 and bk+1/2i = (bki + bk+1

i )/2corresponding to aki , for i = 0, ...,m. Equation 40 is in affineand Equation 39 can also be re-written as affine:

f ′2i,µ(ski ) · bki ≤ v2max, (41)

Continuity constraints (Sec. IV-B.2) on velocity are writtenin discrete form as:

f ′i,µ(ski ) ·√bKi = f ′i+1,µ(s0i+1) ·

√b0i+1. (42)

Since the spatial trajectory is continuous at the joint posi-tion, which means f ′i,µ(sk) = f ′i+1,µ(s0). Equation. 42 isequivalent to

bKi = b0i+1. (43)

Unlike bki is evaluated exactly at ski , aki is evaluatedat (ski + sk+1

i )/2, so no aki is assigned exactly at thejoint position of each two pieces of the temporal trajectory.Instead, the last element aK−1i in the ith segment is separatedby ∆s with the first element a0i+1 in the (i+ 1)th segment,as shown in Fig. 2. Thus we set in-equality constraints tobound the transitions of accelerations between every two

Fig. 2. bk and ak assigned in the ith (in blue) and (i + 1)th (in red)pieces of the temporal trajectory corresponding to . bk is indicated by circleswhile ak is indicated by triangle. The continuity constraint in velocity isenforced at the joint point between two segments and is shown in theblack rectangle. And the constraint on acceleration is enforced in the purplerectangle between aK−1

i and a0i+1.

segments. Besides, although we assume ak is piecewiseconstant, we would like to bound the changing rate ofacceleration considering the aggressiveness in control. Wewrite the affine in-equality constraints for bounding thetransitions of acceleration whether within one segment oracross two segments:

−λa ≤f ′i,µ(sk+1/2i ) · aki + f ′′i,µ(s

k+1/2i ) · bk+1/2

i − (44)

f ′i,µ(sk+3/2i ) · ak+1

i − f ′′i,µ(sk+3/2i ) · bk+3/2

i ≤ λa,

− λa ≤ f ′i,µ(s0+1/2i ) · a0i + f ′′i,µ(s

0+1/2i ) · b0+1/2

i − (45)

f ′i−1,µ(sK−1/2i−1 ) · aK−1i−1 − f

′′i−1,µ(s

K−1/2i−1 ) · bK−1/2i−1 ≤ λa,

where λa is a given parameter to limit the changing rate ofacceleration along the temporal trajectory. Note here λa isnot jerk, since time difference between ski and ski+1 cannotbe determined in the optimization.

For boundary constraints, velocity constraints are applieddirectly as:

f ′0,µ(0) ·√b00 = v0µ, (46)

f ′m,µ(1) ·√bkm = vfµ. (47)

For acceleration’s boundary conditions, we set in-equalityconstraints for the same reason as in Equs. 44 and 45.

−λa ≤f ′0,µ(s1/20 ) · a00 + f ′′0,µ(s

1/20 ) · b1/20 − a0µ ≤ λa, (48)

−λa ≤f ′m,µ(sK−1/2m ) · aK−1m + (49)

f ′′m,µ(sK−1/2m ) · bK−1/2m − afµ ≤ λa.

Up to now, the objective is written in affine (hTd + ρ · t)alongside with second order cones. In-equality and equalityconstraints are abstracted and written in affine (Aeq · x andAeq · x = beq), where d consists of all variables dk andx vectorizes all optimizated variables ak, bk, ck, dk. We canwrite the temporal trajectory generation problem as:

min hTd + ρ · t,s.t. Aeq · x = beq,

Aie · x ≤ bie,

(t, 1, a) ∈ Q2+K·(m+1)r , (50)

(dki , ck+1i + cki ,

√2) ∈ Q3

r, k = 0, 1, ...,K − 1,

(bki + 1, bki − 1, 2cki ) ∈ Q3, k = 0, 1, ...,K,

which is a typical convex Second Order Cone Program(SOCP) and can be solved in polynomial time by off-theshelf convex solver.

V. RESULTS

A. Comparisons with Time-parameterized trajectory

We present the comparisons between the proposed timeoptimization method in this paper to our previous work [6],in which the kinodynamic limits are enforced based on giventime allocations. In the comparison ws and wl are set as 1.0and 2.0. The velocity and acceleration limits are ±2m/sand ±1m/s2. Results are given in Fig. 3. We can see fromthe results that our proposed method gives a much shortertime for finishing the trajectory respecting the kinodynamiclimits. In the test, the optimal time calculated by the proposedmethod in this paper is 31.47s and the time based on theheuristic in our previous work is 56.52s, respectively. Thegenerated velocities and accelerations approach the limits asmuch as possible by using our proposed approach. In ourprevious method [6], although trajectories generated basedon a given time allocation can be entirely constrained withinthe physical bound, it is almost always no possibility to fullyutilize the quadrotor’s actuators to do a full-speed cruise. Inour previous work, the constraints on kinodynamic feasibilityare met easily when one of the segments of the trajectoryhas reached its limit. Moreover, if an over-aggressivenesstime allocation is given, the trajectory generation problemis infeasible at all. While in this paper we optimize thetime allocation globally and the fastest motion concerningthe kinodynamical bounds are finally obtained.

(a) Trajectories in a random map.

(b) Velocities and accelerations against time.

Fig. 3. Comparisons of the proposed method against our previousmethod [6]. Trajectories are generated in a random forest with targetrandomly selected as in Fig. 3(a). Trajectories using previous method arein blue curves and trajectories with time optimization are in green curves.Resulted velocities and accelerations against time are given in Fig. 3(b).

(a) Sample test 1. (b) Sample test 2.

(c) Objective descent curve. (d) Comparison of the cost reduction.

Fig. 4. Comparisons of the generated trajectories w\ and w\o temporaloptimization. Trajectories are generated in a random forest with targetsrandomly selected as in Figs. 4(a), 4(b). The comparison of the objectivedropping is in 4(c), which corresponds to the instance in 4(b). The trajectorywith spatial-temporal decoupling is in red curve and the other is in bluecurve. The result of the reduction of final objective cost is given in Fig. 4(d).

Decoupling the trajectory generation problem into spa-tial and temporal layer not only achieves time optimalitybut also benefits the optimization procedure in the solelyspatial aspect. Here we present numerical tests by usingour previous gradient-based method [8] to show the differ-ences with/without the spatial-temporal decoupling. Underthe proposed framework, the dynamical feasibility cost termis removed from the nonlinear optimization, makes con-vergence significantly improved. We do 500 random testsin each different obstacle density. As shown in Fig. 4,by separating the temporal information, the objective valueconverges much faster, and the resulted trajectories are notonly smoother but also have higher clearance. We alsocompare the cost reduction, which is defined as the droppingratio of the objective from the beginning of the nonlinearoptimization, in a given time 50ms. As in Fig. 4(d), byseparating the temporal trajectory, the objective converges tomuch lower values in all obstacle densities quickly, while thetime-dependent trajectory converges much slower, especiallywhen the obstacle density is high. The reason is that theremoval of the dynamic feasibility cost terms saves muchtime of evaluation in each iteration, and also reduces thenon-convexity of the objective function.

B. Study on Parameter Settings

Here we show the results of optimizing the temporal tra-jectory with different parameter settings. Two key parametersaffecting the performance most are the control weightingcoefficients ρ which regularizes s, and K which decides theresolution of b(s) and a(s).

1) Varying ρ: The weighting coefficient ρ of the controlenergy regularization term balances the expectation of min-imal time and aggressiveness of control effort. Results ofapplying different ρ values on a fixed spatial trajectory withK = 50 are given in Figs. 5(a) to 5(d). As is shown in

the result, when ρ = 0, the resulted temporal trajectory hasthe most aggressiveness. There are peaks in acceleration andsharp transitions in velocity. As ρ increases, the accelerationand velocity become smoother, and all sharp transitions areeliminated, as a cost, the total time of the trajectory is longer.

2) Varying K: In our approach, the parameter K di-rectly decides the number of grids in s ∈ [0, 1]. Here wepresent results of optimizing the temporal trajectory withdifferent K values with ρ = 10. As in Figs. 5(e) to 5(h),increasing K improves the optimality of the final solutionsignificantly when K is small. However, when K is large,the improvement is not very notable. The number of K canbe viewed as the resolution of the optimal solution. As moregrids in the s domain are divided, more freedom (decisionvariables) is provided to solve the problem with the cost thatthe computational complexity increases. According to ourexperience, the computational time of the SOCP increaseslinearly with K. Moreover, we suggest to set K in 30 ∼ 50for normal applications.

C. On-board Flight Tests

The method proposed in this paper1 is implemented inC++11 using a general convex solver Mosek2. The flightexperiments are done on a self-developed quadrotor plat-form which is specially designed for high-speed flights. Allprocessings are done on an onboard dual-core 2.40 GHzIntel i7-5500U processor, which has 8 GB RAM and 64 GBSSD. In the test, several waypoints are sent to the quadrotor,and a spatial trajectory in the virtual domain s with fixedgeometric shape is generated onboard. The spatial trajectoryis then be parameterized to time t by using our proposedtime optimization method, and the quadrotor is commandedto track the trajectory. The procedure repeats in severalloops. We assume our quadrotor has different kinodynamiclimits and optimize the minimal-time temporal trajectoryto approach such limits in each loop. In experiments, thekinodynamic limits are raised in each loop.

1) Indoor Test: We conduct indoor flights with the posefeed by the indoor localization system OptiTrack3. A figureshows the flight is in Fig. 1(a). In this test, a series ofwaypoints are sent to the quadrotor to generate a spatialtrajectory. Initially, the kinodynamic limits vmax and amaxare 1m/s and 0.5m/s2 in velocity and acceleration. Andλa and ρ are set as 0.05 and 0. K is set as 30. Thequadrotor repeats the tracking of the trajectory. In each loop,vmax, amax and λa are increased by 1m/s, 0.5m/s2 and0.1 until velocity limit reaches 6m/s and acceleration limitreaches 6m/s2. The result presents the profiles of velocityand acceleration in the 12th loop of the flight is given inFig. 6. Average computing time in generating the optimaltemporal trajectory is 62.41ms.

1Source code of the proposed method will be released in https://github.com/HKUST-Aerial-Robotics/TimeOptimizer af-ter the publishing of this paper.

2https://www.mosek.com3http://optitrack.com/

Fig. 6. The result of a sample flight in the indoor experiment. In the flightthe maximum velocity and acceleration allowed for the quadrotor are set as6m/s and 6m/s2. The generated highest velocity and acceleration in oneaxis are 5.11m/s and 6m/s2.

2) Outdoor Test: We use visual-inertial fusion [17] withthe minimum sensing suite: one camera and one IMU, forpose feedback in outdoor experiments. The kinodynamiclimits are increased by 1.5.0m/s and 2.0m/s2 from theinitial value of 2.0m/s and 3.0m/s2. Other parameters arethe same as indoor tests. We set virtual obstacles in theoutdoor experiment to mimic the complex trajectory in fullyautonomous flights, as shown in Fig. 7. A snapshot showsthe flight is in Fig. 1(b). The result presents the profile ofvelocity and acceleration in the 3rd loop of the flight is givenin Fig. 8. Average computing time in generating the optimaltemporal trajectory is 108.94ms in this experiment. We alsomake an experiment as a response to the sharp turning case inSect. I. The result is shown in Fig. 9. More details and onlinevisualizations about the indoor and outdoor experiment aregiven in the attached video.

Fig. 7. The spatial trajectory generated in the outdoor environment withvirtual obstacles placed randomly. Our quadrotor platform does not equiponline mapping capabilities. Therefore we place virtual obstacles in thisexperiment to mimic the real applications in fully autonomous navigation.

VI. CONCLUSION AND FUTURE WORK

In this paper, we propose a method to generate the piece-wise minimal-time polynomial trajectory for quadrotors. Wedecouple the traditional time-dependent trajectory genera-tion problem into spatial and temporal layers and proposea method to generate time-optimal trajectory efficiently.The effectiveness and efficiency of the proposed methodare validated in both numerical simulations and onboardexperiments. The decomposition and optimization achievethe full usage of actuators for quadrotors to cruise in full-speed. We plan to integrate the proposed method into a

(a) Spatial trajectory. (b) ρ = 0 (c) ρ = 1 (d) ρ = 10

(e) Spatial trajectory. (f) K = 1 (g) K = 10 (h) K = 80

Fig. 5. Comparisons of the proposed method with different control energy regularization weight ρ and with different discretization number K. InFigs. 5(a) ∼ 5(d), K fixes at 50 and ρ varies. Higher ρ results in a smoother acceleration profile with longer total time. In Figs. 5(e) ∼ 5(h), ρ fixes at10 and K varies. Higher K means finer resolution of the final solution, which results in a shorter total time.

Fig. 8. The result of a sample flight in the outdoor experiment. In the flight,the maximum velocity and acceleration allowed for the quadrotor are set as5.0m/s and 7.0m/s2. The generated highest velocity and acceleration inone axis are 5.0m/s and 7.0m/s2.

Fig. 9. The experiment of the sharp turning for the case in Sect. I. Thecolor of the trajectory indicates the magnitude of speed. Red color indicatesthe highest speed (5m/s) while the green indicates the lowest speed inthe flight. The quadrotor is shown in a black model. The velocity andacceleration of the vehicle are shown in green and yellow arrows.

complete autonomous quadrotor with online perception likeour previous work [6], to achieve high-speed autonomousflights in cluttered environments. The biggest challenge is,when the initial and final states of the temporary optimizationare not both zero, there may not exists a feasible solution.We are going to investigate the feasibility in the future.

REFERENCES

[1] C. Richter, A. Bry, and N. Roy, “Polynomial trajectory planning foraggressive quadrotor flight in dense indoor environments,” in Proc. ofthe Intl. Sym. of Robot. Research, Singapore, Dec. 2013.

[2] R. Deits and R. Tedrake, “Efficient mixed-integer planning for UAVsin cluttered environments,” in Proc. of the IEEE Intl. Conf. on Robot.and Autom. Seattle, Washington, USA: IEEE, May 2015, pp. 42–49.

[3] D. Mellinger and V. Kumar, “Minimum snap trajectory generation andcontrol for quadrotors,” in Proc. of the IEEE Intl. Conf. on Robot. andAutom., Shanghai, China, May 2011, pp. 2520–2525.

[4] N. Ratliff, M. Zucker, J. A. Bagnell, and S. Srinivasa, “Chomp:Gradient optimization techniques for efficient motion planning,” inProc. of the IEEE Intl. Conf. on Robot. and Autom., May 2009.

[5] H. Oleynikova, M. Burri, Z. Taylor, J. Nieto, R. Siegwart, andE. Galceran, “Continuous-time trajectory optimization for online uavreplanning,” in Proc. of the IEEE/RSJ Intl. Conf. on Intell. Robots andSyst., Daejeon, Korea, Oct. 2016.

[6] F. Gao, W. Wu, Y. Lin, and S. Shen, “Online safe trajectory gener-ation for quadrotors using fast marching method and bernstein basispolynomial,” in Proc. of the IEEE Intl. Conf. on Robot. and Autom.,Brisbane, Australia, May 2018.

[7] L. Campos-Macıas, D. Gomez-Gutierrez, R. Aldana-Lopez, R. de laGuardia, and J. I. Parra-Vilchis, “A hybrid method for online trajectoryplanning of mobile robots in cluttered environments,” IEEE Roboticsand Automation Letters, vol. 2, no. 2, pp. 935–942, 2017.

[8] F. Gao, Y. Lin, and S. Shen, “Gradient-based online safe trajectorygeneration for quadrotor flight in complex environments,” in Proc. ofthe IEEE/RSJ Intl. Conf. on Intell. Robots and Syst., Sept 2017.

[9] D. Verscheure, B. Demeulenaere, J. Swevers, J. De Schutter, andM. Diehl, “Time-optimal path tracking for robots: A convex opti-mization approach,” IEEE Transactions on Automatic Control, vol. 54,no. 10, pp. 2318–2327, 2009.

[10] F. Gao and S. Shen, “Online quadrotor trajectory generation andautonomous navigation on point clouds,” in Proc. of the IEEE Intl.Sym. on Safety, Security, and Rescue Robotics, Lausanne, Switzerland,Oct 2016.

[11] S. Liu, M. Watterson, K. Mohta, K. Sun, S. Bhattacharya, C. J.Taylor, and V. Kumar, “Planning dynamically feasible trajectories forquadrotors using safe flight corridors in 3-d complex environments,”IEEE Robotics and Automation Letters, 2017.

[12] M. Shomin and R. Hollis, “Fast, dynamic trajectory planning for adynamically stable mobile robot,” in Proc. of the IEEE/RSJ Intl. Conf.on Intell. Robots and Syst. IEEE, 2014, pp. 3636–3641.

[13] S. Liu, N. Atanasov, K. Mohta, and V. Kumar, “Search-based motionplanning for quadrotors using linear quadratic minimum time control,”in Proc. of the IEEE/RSJ Intl. Conf. on Intell. Robots and Syst., 2017.

[14] M. Roberts and P. Hanrahan, “Generating dynamically feasible trajec-tories for quadrotor cameras,” ACM Transactions on Graphics (TOG),vol. 35, no. 4, p. 61, 2016.

[15] J. Jamieson and J. Biggs, “Near minimum-time trajectories for quadro-tor uavs in complex environments,” in Proc. of the IEEE/RSJ Intl.Conf. on Intell. Robots and Syst. IEEE, 2016, pp. 1550–1555.

[16] E. Verriest and F. Lewis, “On the linear quadratic minimum-timeproblem,” IEEE transactions on automatic control, vol. 36, no. 7, pp.859–863, 1991.

[17] T. Qin, P. Li, and S. Shen, “Vins-mono: A robust and versatilemonocular visual-inertial state estimator,” arXiv:1708.03852, 2017.