FASTER: Fast and Safe Trajectory Planner for Flights
in Unknown Environments
Abstract
Highspeed trajectory planning through unknown environments requires algorithmic techniques that enable fast reaction times while maintaining safety as new information about the operating environment is obtained. The requirement of computational tractability typically leads to optimization problems that do not include the obstacle constraints (collision checks are done on the solutions) or use a convex decomposition of the free space and then impose an adhoc time allocation scheme for each interval of the trajectory. Moreover, safety guarantees are usually obtained by having a local planner that plans a trajectory with a final “stop” condition in the freeknown space. However, these two decisions typically lead to slow and conservative trajectories. We propose FASTER (Fast and Safe Trajectory Planner) to overcome these issues. FASTER obtains highspeed trajectories by enabling the local planner to optimize in both the freeknown and unknown spaces. Safety guarantees are ensured by always having a feasible, safe backup trajectory in the freeknown space at the start of each replanning step. Furthermore, we present a Mixed Integer Quadratic Program formulation in which the solver can choose the trajectory interval allocation, and where a time allocation heuristic is computed efficiently using the result of the previous replanning iteration. This proposed algorithm is tested extensively both in simulation and in real hardware, showing agile flights in unknown cluttered environments with velocities up to m/s.
[name=B. L., color=blue]bl \setremarkmarkup(#2)
I Introduction
Navigating through unknown environments entails repeatedly generating collisionfree, dynamically feasible trajectories that are executed over a finite horizon. Similar to that in the Model Predictive Control (MPC) literature, safety is guaranteed by ensuring a feasible solution exists indefinitely. If we consider where , , are disjoint sets denoting freeknown, occupiedknown, and unknown space respectively, safety is guaranteed by constructing trajectories that are entirely contained in with a final stop condition. This can be achieved by generating motion primitives that do not intersect [1, 2, 3, 4], or by constructing a convex representation of to be used in an optimization [5, 6, 7]. However, both approaches lead to slow trajectories in scenarios where is small compared to . This paper presents an optimizationbased approach that reduces the aforementioned limitations by solving for two optimal trajectories at every planning step (see Fig. 1): one in , and another one in .
Decomposing the free space into overlapping polyhedra along a path connecting a start to goal location (see Fig. 1), the usual approach is to divide the total trajectory into intervals [5]. On one hand, this simplifies the problem because no integer variables are needed, as each interval is forced to be in one specific polyhedron. On the other hand, the time allocation problem becomes much harder, as there are different (time allocated for each interval ). The trajectory is also more conservative since the optimizer is only allowed to move the end points of each interval of the trajectory in the overlapping areas. To overcome these two problems, we propose the use of the same for all the intervals, and use intervals, encoding the optimization problem as a Mixed Integer Quadratic Program (MIQP). Moreover, and as the minimum feasible depends depends on the state of the UAV and on the specific shape of and at a specific replanning step, we also propose an efficient way to compute a heuristic of this using the result obtained in the previous replanning iteration.
In summary, this work has the following contributions:

A framework that ensures feasibility of the entire collision avoidance algorithm and guarantees safety without reducing the nominal flight speed by allowing the local planner to plan in while always having a safe trajectory in .

Reduced conservatism of the MIQP formulation for the interval and time allocation problem of the flight trajectories compared to prior work.

Extension of our previous work [8], (where we considered the interaction between the global and local planners) by proposing a way to compute very cheaply a heuristic of the costtogo needed by the local planner to decide which direction is the best one to optimize towards.

Simulation and hardware experiments showing agile flights in completely unknown cluttered environments, with velocities up to m/s.
Ii Related Work
Trajectory planning strategies for UAVs can be classified according to the operating space of the local planner and the specific formulation of the optimization problem.
With regard to the planning space of the local planner, several approaches have been developed. One approach is to use only the most recent perception data [4, 3, 2], which requires the desired trajectory to remain within the perception sensor field of view. An alternative strategy is to create and plan trajectories in a map of the environment using a history of perception data. Within this second category, some works [9, 8, 10] limit the local planner to generate trajectories only in freeknown space ( in Fig. 1), which guarantees safety if the local planner has a final stop condition. However, limiting the planner to operating in freeknown space and enforcing a terminal stopping condition can lead to conservative, slow trajectories (especially when much of the world is unknown). While allowing the local planner to optimize in both the freeknown and unknown space (), higher speeds can be obtained but with no guarantees that the trajectory is safe or will remain feasible.
As far as the optimization formulation is concerned, two approaches can be highlighted. The first does not include the obstacles in the optimization problem [8], leading to a closedform solution for the trajectory [1, 3, 4] or in general to very small computation times [8]. The computation time for these approaches are very low since obstacles are not explicitly considered in the trajectory generation. This enables multiple candidate trajectories to be generated (via sampling) and evaluated for collisions (using nearestneighbor search) at each planning stage. While these approaches are computationally efficient, they are unable to construct sophisticated maneuvers due to the discretization of the candidate trajectories, leading to slower trajectories in cluttered environments. The second approach is to include obstacle constraints directly in the optimization. This is usually done describing the free space by a set of overlapping polyhedra (also known as convex decomposition) [5, 6, 7]. The trajectory can then be parameterized by a sequence of third (or higher)degree polynomials. Bézier Curves [7], [11] or the sumofsquares condition [6, 12] can be used to guarantee the trajectory remains the overlapping polyhedra. Subsequently, there will be an interval (which polytope each polynomial is in) and a time allocation (how much time is assigned to each interval) problem. For the interval allocation, a typical the number of trajectory segments equals the number of polyhedra, and each polynomial segment is forced to be inside its corresponding polyhedron [7]. However, this can be very restrictive since the solver only has the freedom to select where the two endpoint points of each interval are placed in the overlapping regions. Another option, but with higher computation times, is to use binary decision variables [12, 6] to allow the solver to choose the specific interval allocation. For the time allocation, one can either use a fixed time allocation [5] or formulate a bilevel optimization to find the times [13], [7]. However, the first approach can be very conservative and can cause infeasibility in the optimization problem while the seconds leads to longer replanning times.
Iii Fast and Safe Trajectory Planner
Iiia Planning
The Fast and Safe Trajectory Planner (FASTER) uses hierarchical architecture where a longhorizon global planner guides a shorthorizon local planner to a desired goal location. The global planner used in this work is Jump Point Search (JPS). JPS finds the shortest piecewise linear path between two points in a 3D uniformlyweighted voxel grid, guaranteeing optimality and completeness but running an order of magnitude faster than A* [14], [5].
For the local planner, we distinguish these three different jerkcontrolled trajectories (some of the points will be precisely defined later, see Fig. 3):

Whole Trajectory: This trajectory goes from a start location to goal location , and it is contained in . It has a final stop condition.

Safe Trajectory: It goes from to , where is a point in the Whole Trajectory, and is any point inside the polyhedra obtained by doing a convex decomposition of . It is completely contained in (freeknown space), and it has also a final stop condition to guarantee safety.

Committed Trajectory: This trajectory consists of two pieces: The first part is the interval of the Whole Trajectory. The second part is the Safe Trajectory. It is also guaranteed to be inside (see explanation below). This trajectory is the one that the UAV will execute in case no feasible solutions are found in the next replanning steps.
The quadrotor is modeled using triple integrator dynamics with state vector and control input (where , , , and are the vehicle’s position, velocity, acceleration, and jerk, respectively).
Let denote the specific interval of the trajectory and the specific polyhedron. If is constrained to be constant in each interval , then the whole trajectory will be a spline consisting of third degree polynomials. Matching the cubic form of the position for each interval
with the expression of a cubic Bézier curve
we can solve for the four control points associated with each interval :
Let us denote the sequence of overlapping polyhedra as , and introduce binary variables ( variables for each interval ). As a Bézier curve is contained in the convex hull of its control points, we can ensure that the whole trajectory will be inside this convex corridor by forcing that all the control points are in the same polyhedron [7, 11] with the constraint , and at least in one polyhedron with the constraint . The optimizer is free to choose in which polyhedron exactly. The complete MIQP solved in each replanning step (using Gurobi, [15]) for both the Safe and the Whole trajectories is this one:
(1)  
s.t.  
In the optimization problem above, (same for every interval ) is computed as
(2) 
where , , are solution of the constantinput motions in each axis by applying , and respectively. is a factor that is obtained according to the solution of the previous replanning step (see Fig. 2): The optimizer will try values of (in increasing order) in the interval until the problem converges. Here, is the factor that made the problem feasible in the previous replanning step. Note that, if , then is a lower bound on the minimum time per interval required for the problem to be feasible.
Algorithm 1 summarizes the full approach (see also Fig. 3). Let be the current position of the UAV. The point is chosen in the Committed Trajectory of the previous replanning step with an offset from . This offset is computed by multiplying the total time of the previous replanning step by (typically ). The idea here is to dynamically change this offset to ensure that most of the times the solver is able to find the next solution in less than . Then, the final goal is projected into the sliding map (centered on the UAV) in the direction to obtain the point (line 1). Next, we run JPS from to (line 1) to obtain .
The local planner then must decide if the current JPS solution should be used to guide the optimization (lines 11). Instead of blindly trusting the last JPS solution () as the best direction for the local planner to optimize (note that JPS is a zeroorder model, without dynamics encoded), we take into account the dynamics of the UAV in the following way: First of all, we modify the so that it does not collide with the new obstacles seen (Fig. 4): we find the points and (first and last intersections of with ) and run JPS three times, so , and . Hence, the modified version, denoted by , will be the concatenation of these three paths. Then, we compute a lower bound on using Eq. 2 for both and , where and are the intersections of the previous JPS paths with a sphere of radius centered on . Next, we find the costtogo associated with each direction by adding this (or ) and the time it would take the UAV to go from (or ) to following the JPS solution and flying at . Finally, the one with lowest cost is chosen, and therefore . This will be the direction towards which the local planner will optimize.
The Whole Trajectory (lines 11) is obtained as follows. We do the convex decomposition [5] of around , which is the part of that is inside the sphere . This gives a series of overlapping polyhedra that we denote as . Then, the MIQP in (1) is solved using these polyhedral constraints to obtain the Whole Trajectory.
The Safe Trajectory is computed as in lines 11. First we choose the point along the Whole Trajectory with an offset from (this is computed by multiplying the previous replanning time by ), and run convex decomposition in using the part of that is in , obtaining the polyhedra . Then, we solve the MIQP from to any point inside (this point is chosen by the optimizer).
In both of the convex decompositions presented above, one polyhedron is created for each segment of the piecewise linear paths. To obtain a less conservative solution (i.e. bigger polyhedra), we first check the length of segments of the JPS path, creating more vertexes if this length exceeds certain threshold . Moreover, we truncate the number of segments in the path to ensure that the number of polyhedra found does not exceed a threshold . This helps reduce the computation times (see Sec. IV).
Finally (line 1), we compute the Committed Trajectory by concatenating the piece of the Whole Trajectory, and the Safe Trajectory. Note that in this algorithm we have run two decoupled optimization problems per replanning step: (1) one for the Whole Trajectory, and (2) one for the Safe Trajectory. This ensures that the piece is not influenced by the braking maneuver , and therefore guarantees a higher nominal speed on this first piece. The intervals and have been designed so that, with high probability, at least one replanning step can be solved within that interval. Moreover, to prevent the (very rare) cases where both and are in , but the piece is not, we check that piece against collision with . If any of the two optimizations in this algorithm fails, or the piece intersects , or the replanning step takes longer than , the UAV does not commit to a new trajectory in that replanning step, and continues executing the Committed Trajectory of the previous replanning step. Thus safety is guaranteed by construction: the UAV will only fly Committed Trajectories, which are always guaranteed to be in with a terminal stopping condition.
IiiB Mapping
For the mapping, we use a sliding map centered on the UAV that moves as the UAV flies. We fuse a depth map into the occupancy grid using the 3D Bresenham’s line algorithm for raytracing [16], and and are inflated by the radius of the UAV to ensure safety.
Iv Results
Iva Simulation
We first test FASTER in 10 random forest environments with an obstacle density of obstacles/m (see Fig. 5) and compare the flight distances achieved against the following seven approaches: Incremental approach (no goal selection), random goal selection, optimistic RRT (unknown space = free), conservative RRT (unknown space = occupied), “nextbestview” planner (NBVP) [17], Safe Local Exploration [10], (see [10] for details of all these approaches), and MultiFidelity [8].
The results are shown in Table III, which highlights that FASTER achieves a improvement in the distance. Completion times are compared in Table III to our previous proposed algorithm [8] (time values are not available for all other algorithms in Table III). FASTER achieves an improvement of in the completion time. The dynamic constraints imposed for the results of this table are (per axis) m/s, m/s, and m/s.
We also test FASTER using the bugtrap environment shown in Fig. 5, and obtain the results that appear on Table III. Both algorithms have a similar total distance, but FASTER achieves an improvement of on the total flight time. For both cases the dynamic constraints imposed are m/s, m/s, and m/s.
The timing breakdown of Alg. 1 as a function of the maximum number of polyhedra is shown in Fig. 6. The number of intervals was 10 for the Whole Trajectory and 7 for the Safe Trajectory. Note that the runtime for the MIQP of the Safe Trajectory is approximately constant as a function of . This is due to the fact that the Safe Trajectory is planned only in , and therefore most of the times . For the simulations and hardware experiments presented in this paper, was used. The runtimes for JPS as a function of the voxel size of the map for the forest simulation are available in Fig. 7 of [8]. All these timing breakdowns were measured using an Intel Core i77700HQ 2.8GHz Processor.
Method  Number of  Distance (m)  

Successes  Avg  Std  Max  Min  
Incremental  0         
Rand. Goals  10  138.0  32.0  210.5  105.6 
Opt. RRT  9  105.3  10.3  126.4  95.5 
Cons. RRT  9  155.8  52.6  267.9  106.2 
NBVP [17]  6  159.3  45.6  246.9  123.6 
SL Expl. [10]  8  103.8  21.6  148.3  86.6 
MultFid [8]  10  84.5  11.7  109.4  73.2 
FASTER  10  77.6  5.9  88.0  70.7 
Min/Max improvement (%)  8/51  43/89  20/67  3/43 
Method  Time (s)  

Avg  Std  Max  Min  
MultFid [8]  61.2  16.8  92.5  37.9 
FASTER  29.2  4.2  36.8  21.6 
Improvement (%)  52.3  75.0  60.2  43.0 
Method  Distance (m)  Time (s) 
MultFid [8]  56.8  37.6 
FASTER  55.2  13.8 
Improvement (%)  2.8  63.3 
IvB Hardware
The UAV used in the hardware experiments is shown in Fig. 7. The perception runs on the Intel^{®} RealSense, the mapper and planner run on the Intel^{®} NUC, and the control runs on the Qualcomm^{®} SnapDragon Flight. The attitude, IMU biases, position and velocity are estimated by fusing (via a Kalman filter) propagated IMU measurements with an external motion capture system.
The first and second experiments (Fig. 11 and 11) were done in similar obstacle environments with the same starting point, but with different goal locations. In the first experiment (Fig. 11), the UAV performs a 3D agile maneuver to avoid the obstacles on the table. In the second experiment (Fig. 11) the UAV flies through the narrow gap of the cardboard boxes structure, and then flies below the triangleshaped obstacle. In these two experiments, the maximum speed was m/s.
In the third and fourth experiments (Fig 11 and 11), the UAV must fly through a space with poles of different heights, and finally below the cardboard boxes structure to reach the goal, achieving a maximum speed of m/s.
All these hardware experiments are available on https://youtu.be/gwV0YRs5IWs.
For , the boxplots of the runtimes achieved on the forest simulation (measured on an Intel Core i77700HQ) and on the hardware experiments (measured on the onboard Intel NUC with the mapper and the RealSense also running on it) are shown in Fig. 12. For the runtimes of the MIQP for the Whole and the Safe Trajectories, the 75^{th} percentile is always below ms.
V Conclusions
This work presented FASTER, a fast and safe planner for agile flights in unknown environments. The key properties of this planner is that it leads to a higher nominal speed than other works by planning both in and , and ensures safety by having always a Safe Trajectory planned in at the beginning of every replanning step. FASTER was tested successfully both in simulated and in hardware flights, achieving velocities up to m/s.
All the GAZEBO worlds used for the simulation are available at https://github.com/jtorde.
Acknowledgment
Thanks to Boeing Research & Technology for support of the hardware, to my brother Pablo Tordesillas (ETSAMUPM) for his great help with some figures of this paper and to Parker Lusk (ACLMIT) for his help with the hardware. The authors would also like to thank John Carter and John Ware (CSAILMIT) for their help with the mapper used in this paper. Supported in part by Defense Advanced Research Projects Agency (DARPA) as part of the Fast Lightweight Autonomy (FLA) program, HR001115C0110. Views expressed here are those of the authors, and do not reflect the official views or policies of the Dept. of Defense or the U.S. Government.
References
 [1] Mark W Mueller, Markus Hehn, and Raffaello D’Andrea. A computationally efficient motion primitive for quadrocopter trajectory generation. IEEE Transactions on Robotics, 31(6):1294–1310, 2015.
 [2] Pete Florence, John Carter, and Russ Tedrake. Integrated perception and control at high speed: Evaluating collision avoidance maneuvers without maps. In Workshop on the Algorithmic Foundations of Robotics (WAFR), 2016.
 [3] Brett T Lopez and Jonathan P How. Aggressive 3D collision avoidance for highspeed navigation. In Robotics and Automation (ICRA), 2017 IEEE International Conference on, pages 5759–5765. IEEE, 2017.
 [4] Brett T Lopez and Jonathan P How. Aggressive collision avoidance with limited fieldofview sensing. In Intelligent Robots and Systems (IROS), 2017 IEEE/RSJ International Conference on, pages 1358–1365. IEEE, 2017.
 [5] Sikang Liu, Michael Watterson, Kartik Mohta, Ke Sun, Subhrajit Bhattacharya, Camillo J Taylor, and Vijay Kumar. Planning dynamically feasible trajectories for quadrotors using safe flight corridors in 3D complex environments. IEEE Robotics and Automation Letters, 2(3):1688–1695, 2017.
 [6] Robin Deits and Russ Tedrake. Efficient mixedinteger planning for uavs in cluttered environments. In 2015 IEEE international conference on robotics and automation (ICRA), pages 42–49. IEEE, 2015.
 [7] James A Preiss, Karol Hausman, Gaurav S Sukhatme, and Stephan Weiss. Trajectory optimization for selfcalibration and navigation. In Robotics: Science and Systems, 2017.
 [8] Jesus Tordesillas, Brett T Lopez, John Carter, John Ware, and Jonathan P How. Realtime planning with multifidelity models for agile flights in unknown environments. In 2019 IEEE International Conference on Robotics and Automation (ICRA). IEEE, 2019.
 [9] Tom Schouwenaars, Éric Féron, and Jonathan How. Safe receding horizon path planning for autonomous vehicles. In Proceedings of the Annual Allerton Conference on Communication Control and Computing, volume 40, pages 295–304. The University; 1998, 2002.
 [10] Helen Oleynikova, Zachary Taylor, Roland Siegwart, and Juan Nieto. Safe local exploration for replanning in cluttered unknown environments for microaerial vehicles. IEEE Robotics and Automation Letters, 3(3):1474–1481, 2018.
 [11] Ozgur Koray Sahingoz. Generation of bezier curvebased flyable trajectories for multiuav systems with parallel genetic algorithm. Journal of Intelligent & Robotic Systems, 74(12):499–511, 2014.
 [12] Benoit Landry, Robin Deits, Peter R Florence, and Russ Tedrake. Aggressive quadrotor flight through cluttered environments using mixed integer programming. In 2016 IEEE international conference on robotics and automation (ICRA), pages 1469–1475. IEEE, 2016.
 [13] Charles Richter, Adam Bry, and Nicholas Roy. Polynomial trajectory planning for aggressive quadrotor flight in dense indoor environments. In Robotics Research, pages 649–666. Springer, 2016.
 [14] Daniel Damir Harabor, Alban Grastien, et al. Online graph pruning for pathfinding on grid maps. In AAAI, 2011.
 [15] LLC Gurobi Optimization. Gurobi optimizer reference manual, 2018.
 [16] Jack E Bresenham. Algorithm for computer control of a digital plotter. IBM Systems journal, 4(1):25–30, 1965.
 [17] Andreas Bircher, Mina Kamel, Kostas Alexis, Helen Oleynikova, and Roland Siegwart. Receding horizon “nextbestview” planner for 3D exploration. In Robotics and Automation (ICRA), 2016 IEEE International Conference on, pages 1462–1468. IEEE, 2016.