I Introduction
In recent years, progresses on different aspects of unmanned aerial vehicles (UAVs), especially quadrotor autonomy have been achieved and promote autonomous navigation. Nonetheless, highspeed flight in unknown and highly cluttered environments still remains one of the biggest challenges toward full autonomy. To achieve fast flight, trajectory replanning is of vital importance to cope with previously unknown obstacles, guaranteeing smooth and safe navigation.
Although trajectory replanning has been investigated actively, most presented methods only apply to flights at a moderate speed. Several issues greatly hinder their usage in highspeed scenarios. (a) Flying in unknown environments at a high speed, the quadrotor should replan new trajectories to avoid unexpected obstacles in considerably short time, otherwise it crashes. However, most methods do not guarantee to find feasible trajectories given very limited time. (b) Current methods typically find a locally optimal trajectory confined within a topologically equivalent class, which does not necessarily contains a satisfactory solution for smooth and safe navigation, especially in fast flight. (c) Existing methods are unaware of environment perception, which can be fatal as the flight speed and obstacle density get high. Paying no attention to perception, the planned motions may lead to restricted visibility to the environments, which would in turn result in deficient information of the surrounding space necessary for safe navigation. The consequence of not considering perception in replanning can be better illustrated by Fig.1. To minimize the energy consumption, a trajectory near the wall is generated, along which the visibility toward the unknown space behind the corner is very limited. As a result the obstacle right behind the corner is invisible until the quadrotor turns right and gets very close, which ’surprises’ or even crashes the quadrotor. Instead of avoiding what are observed passively, actively observing and avoiding possible dangers is critical for safe highspeed flight. In this paper, we propose a Robust And Perceptionaware TrajectOry Replanning framework called RAPTOR to address these issues systematically. To ensure obtaining feasible trajectories within limited time, we present a pathguided gradientbased optimization method, which utilizes geometric guiding paths to eliminate infeasible local minima and guarantee the success of replanning. Also, to further improve the optimality of the replanning, we introduce an online topological path planning to extract a comprehensive set of paths that capture the structure of the environment. With the guidance of several distinctive paths, multiple trajectories are optimized in parallel, leading to a more thorough exploration of the solution space. The above mentioned method that address issues (a) and (b) were first proposed in our previous work [48]. However, it adopts the optimistic assumption and lack the awareness of environment perception, which restricts its capability in higher speed and more complex environments. To bridge this gap, we extend it with a perceptionaware planning strategy to enable faster and safer flight from two aspects. Firstly, a riskaware trajectory refinement approach is developed to incorporate with the optimistic planner. It identifies unknown regions along the optimistic trajectories that are potentially dangerous to the quadrotor. Visibility toward such regions along with safe reaction distance are enforced explicitly, ensuring that obstacles in unmapped areas become visible earlier and are avoidable by the quadrotor. Secondly, we incorporate the yaw angle of the quadrotor into a twostep motion planning framework. A optimal sequence of yaw angles that maximizes information gain and smoothness is searched in the discrete state space, which is further smoothed through optimization. The planned motions of yaw angle enable the quadrotor whose fieldofview (FOV) is limited to actively explore the unknown space to gain more relevant knowledge for the future flight. We conduct systematic evaluations on both the proposed perceptionaware planning strategy and the whole planning system, through benchmark comparisons and challenging realworld experiments. For the former, it is able to support fast and safe flight in challenging scenarios where traditional method fail to ensure safety. For the later, our planner outperform stateoftheart methods in several aspects in fast flight tasks. Extensive indoor and outdoor flight tests in complex environments also validates our planning system. The contributions of this paper are summarized as follows: 1) A topological pathsguided gradientbased replanning approach, that is capable of generating highquality trajectories in limited time. 2) A riskaware trajectory refinement approach, which enforces visibility and safe reaction distance to unknown obstacles. It improves the predictability and safety of fast flights. 3) A twostep yaw angle planning method, to actively explore the unknown environments and gather useful information for the flight. 4) Extensive simulation and realword tests that validate the proposed method. The source code of our system will be released as an opensource package.
Ii Related Work
Iia Quadrotor Trajectory Planning
Trajectory planning for quadrotor has been widely investigated. Existing methods can be categorized into hardconstrained methods and gradientbased optimization methods. Hardconstrained methods are pioneered by minimumsnap trajectory[27], in which piecewise polynomial trajectories are generated through quadratic programming(QP). [36] presented a closedform solution to minimum snap trajectories. [5, 14, 12, 13, 8] generate trajectories in a twostep pipeline. Safe regions around initial paths are extracted as convex flight corridors, within which QP is solved to generate smooth and safe trajectories. Among these methods, poorly chosen time allocation of piecewise polynomials usually lead to unsatisfying results. To this end, fast marching[14] and kinodynamic search[7, 8] are utilized to search for initial paths with more reasonable time allocation. [14] also proposed to represent trajectories as piecewise Bézier curves so that safety and dynamical feasibility are guaranteed. [43] adopted a mixed integer QP formulation to find a more reasonable time allocation of the trajectory. Another category is the gradientbased trajectory optimization (GTO) methods, which typically formulate trajectory generation as nonlinear optimization problems trading off smoothness, safety and dynamic feasibility. Recently works [31, 11, 21, 45, 49, 48] revealed that they are particularly effective for local replanning, which is a key component for highspeed flight in unknown environments. [35] proposed to optimize discretetime trajectory through covariant gradient descent, reviving the community’s interest in such methods. [19] presented a similar formulation, but solves the problem by sampling neighboring candidates iteratively. The stochastic sampling strategy partially overcomes the local minima issue but is computationally intensive. [31] extended it to continuoustime quadrotor trajectories and also adopted random optimization restarts to slightly relieve the typical local minima issue of such methods. In [11, 21], the optimization is combined with an informed samplingbased path searching to improve the success rate. [45] proposed to parameterize trajectories as uniform Bsplines, showing the usefulness of continuity and locality properties of Bspline for replanning. However, due to insufficient success rate and efficiency, [31, 11, 45] only apply to flight at a moderate speed. To this end, [49] further exploited Bspline to improve the efficiency and robustness. GTO methods are preferable for replanning due to high efficiency. However, their local minima issue may lead to undesired solutions. Our method lies in this category, and we resolve local minima by introducing topologically distinctive paths to guide optimization in parallel.
IiB Topological Path Planning
There are works utilizing the idea of topologically distinct paths for planning, in which paths belonging to different homotopy (homology) [40, 38, 39, 2, 1] or visibility deformation [17] classes are sought. [40] constructs a variant of probabilistic roadmap (PRM) to capture homotopy classes, in which path searching and redundant path filtering are conducted simultaneously. In contrast, [38, 39] firstly creates a PRM or Voronoi diagram, after which a homology equivalence relation based on complex analysis[2] is adopted to filter out redundant paths. These methods only apply to 2D scenarios. To seek for 3D homology classes, [1] exploit the theory of electromagnetism and propose a 3D homology equivalence relation. However, it requires occupied space to be decomposed into ”genus 1” obstacles, which is usually impractical. Besides, capturing only homotopy classes in 3D space is insufficient to encode the set of useful paths, as indicated in [17], since 3D homotopic paths may be too hard to deform into each other. To this end, [17] leverages a visibility deformation roadmap to search for a richer set of useful paths. [33, 4] convert maps built from SLAM systems into sparse graphs representing the topological structure of the environments. [17, 33, 4] focus on global offline planning and is too timeconsuming for online usage. Our topological path searching is conceptually closest to [17], but with a reinvented algorithm for realtime performance.
IiC Navigation in Unknown Environments
To deal with unknown environments in navigation, different strategies have been used. Many methods adopt the optimistic assumption [13, 23, 45, 49], which treats the unknown space as collisionfree. This strategy improves the chance of reaching goals, but may not guarantee safety. On the contrary, some other methods regard unknown space as unsafe and only allow motions within the knownfree space [24] or sensor FOV [26, 30]. In [25] the sensor FOV constraint is partially relaxed by choosing safe motion primitives generated in the past. Although these restrictions ensure safety, they lead to conservative motion. Recently [43] proposed a strategy that plans in both the knownfree and unknown space. Instead of being over optimistic about the unknown space, it always maintains backup trajectories to ensure safety. The limitation of the above mentioned strategies is the lack of environment perception awareness, which is of significant necessity in fast flight. Although much attention has been paid to planning with the awareness of localization [6, 47, 29] and target tracking [9, 18], less emphasis is put on environment perception. [37]
proposed a learned heuristic function to guide the path searching into areas with greater visibility to unknown space, but it may not generalize well to complex 3D environments.
[16] showed an integrated mapping and planning framework for active perception. The planner iteratively simulates future measurements after executing specific motions, predicts uncertainty of the map, and minimizes the replanning risk. Its main drawback is the prohibited runtime for online usage. In [32], a local planner is coupled with local exploration to safely navigate a cluttered environment. However, it conservatively selects intermediate goals within knownfree space, which restricts the flight speed. In this paper, we present a perceptionaware strategy to ensures that unknown dangers can be discovered and avoided early. It guarantees safety and does not lead to conservative behaviors.Iii System Overview
The proposed replanning system is illustrated in Fig.3. It takes the outputs of the global planning, dense mapping and state estimation modules, and deforms the global reference trajectory locally to avoid previously unknown obstacles. The replanning works in two steps. Firstly, the robust optimistic replanning generates multiple locally optimal trajectories in parallel through the pathguided optimization (Sect.IV). The optimization is guided by topologically distinctive paths extracted and carefully selected from the topological path searching, which will be detailed in Sect.V. Optimistic assumption is adopted in this step. Secondly, the perceptionaware planning strategy is utilized. The best trajectory among the locally optimal ones is further polished by a riskaware trajectory refinement, in which its safety and visibility to the unknown and dangerous space is improved, as presented in Sect.VI. Based on the refined trajectory, the yaw angle is planned to actively explore the unknown environments (Sect.VII). The global planning, mapping, estimation and controller are introduced briefly in Sect.VIIIA.
Iv PathGuided Trajectory Optimization
As showed in Sect.IIA, GTO methods are effective for replanning, but suffer from local minima. To further improve the robustness of replanning and ensure flight safety, we present PGO, which utilizes a geometric guiding path in the optimization to guarantee its success.
Iva Optimization Failure Analysis
Previous work[41] showed that failure of GTO is relevant to unfavorable initialization, i.e., initial paths that pass through obstacles in certain ways usually get stuck. Underlying reason for this phenomenon is illustrated in Fig.4. Typical GTO methods incorporate the gradients of a Euclidean signed distance field (ESDF) in a collision cost to push the trajectory out of obstacles. Yet there are some ”valleys” or ”ridges” in the ESDF, around which the gradients differ greatly. Consequently, if a trajectory is in collision and crosses such regions, the gradients of ESDF will change abruptly at some points. This can make gradients of the collision cost push different parts of the trajectory in opposing directions and fail the optimization. Normally, such ”valleys” and ”ridges”, which corresponds to the space that has an identical distance to the surfaces of nearby obstacles, are difficult to avoid, especially in complex environments. Therefore, optimization depending solely on the ESDF fails inevitably at times. To solve the problem, it is essential to introduce extra information that can produce an objective function whose gradients consistently deform the trajectory to the free space.
IvB Problem Formulation
We propose PGO built upon our previous work [49] that represents trajectories as Bsplines for more efficient cost evaluation. For a trajectory segment in collision, we reparameterize it as a degree uniform Bspline with control points and knot span .
PGO consists of two different phases. The first phase generates an intermediate warmup trajectory. As concluded above, external information should be included to effectually deform the trajectory, since solely applying the ESDF could be futile. We employ a geometric guiding path to attract the initial trajectory to the free space (depicted in Fig. 5(a)) since collisionfree paths are readily available from standard methods like A* and RRT*. In our work, the paths are provided by the samplingbased topological path searching (Sect. V). The firstphase objective function is:
(1)  
where is the smoothness term, while penalize the distance between the guiding path and the Bspline trajectory. As in [49], is designed as a elastic band cost function^{2}^{2}2Only the subset of control points is optimized due to the boundary state constraints of the trajectory. , , and are needed to evaluate the smoothness. that simulates the elastic forces of a sequence of springs. To simplify the design of , we utilize the property that the shape of a Bspline is finely controlled by its control points. Each control point is assigned with an associated point on the guiding path, which is uniformly sampled along the guiding path. Then is defined as the sum of the squared Euclidean distance between these point pairs. Notably, minimizing yields an unconstrained quadratic programming problem, so its optimal solution can be obtained in closed form. It outputs a smooth trajectory in the vicinity of the guiding path. Since the path is already collisionfree, usually the warmup trajectory is also so. Even though it is not completely collisionfree, its major part will be attracted to the free space. At this stage, the gradients of ESDF along the trajectory vary smoothly, and the gradients of the objective function push the trajectory to the free space in consistent directions. Hence, standard GTO methods can be utilized to improve the trajectory. In the second phase, we adopt our previous Bspline optimization method[49] to further refine the warmup trajectory into a smooth, safe, and dynamically feasible one, whose objective function is:
(2)  
Here is a penalty function for general variables:
(3) 
is the collision cost that grows rapidly if the trajectory gets closer than to obstacles, where is the distance of point in the ESDF. and penalize infeasible velocity and acceleration, in which and are the control points of velocity and acceleration. They can be evaluated by:
(4) 
and are singleaxis maximum velocity and acceleration. The formulations of , , and are based on the convex hull property of Bspline, thanks to which it suffices to constrain the control points of the Bspline to ensure safety and dynamic feasibility. For brevity, we refer the readers to [49] for more details. Although PGO has one more step of optimization compared with previous methods, it can generate better trajectories within shorter time. The firstphase takes only negligible time, but generate a warmup trajectory that is easier to be further refined, which improve the overall efficiency.
V Topological Path Searching
Given a geometric guiding path, our PGO method can obtain a locally optimal trajectory. However, this trajectory is restricted within a topologically equivalent class and not necessarily satisfactory, even with the guidance of the shortest path, as seen in Fig. 8(e) and 8(f). Actually, it is difficult to determine the best geometric path, since the paths do not contain high order information (velocity, acceleration, etc.), and can not completely reflect the true motion. Searching a kinodynamic path [22, 46] may suffice, but it takes excessive time to obtain a promising path with boundary state constraints at the start and end of the replanned trajectory. For a better solution, a variety of guiding paths are required. We propose a samplingbased topological path searching to find a collection of distinctive paths. Although methods [17, 40, 33, 4] are for this problem, none of them runs in realtime in complex 3D environments. We redesign the algorithm carefully to solve this challenging problem in real time.
Va Topology Equivalence Relation
Although the concept of homotopy is widely used, it captures insufficient useful trajectories in 3D environments, as shown in Fig. 6(a). [17] proposes a more useful relation in 3D space named visibility deformation (VD), but it is computationally expensive for equivalence checking. Based on VD, we define uniform visibility deformation (UVD), which also captures abundant useful trajectories, and is more efficient for equivalence checking.
Definition 1. Two trajectories , parameterized by and satisfying , belong to the same uniform visibility deformation class, if for all , line is collisionfree. Fig. 6(b) gives an example of three trajectories belonging to two UVD classes. The relation between VD and UVD is depicted in Fig. 7. Both of them define a continuous map between two paths and , in which a point on is transformed to a point on through a straightline. The major difference is that for UVD, point is mapped to where , while for VD does not necessarily equals . In concept, UVD is less general and characterizes subsets of VD classes. Practically, it captures slightly more classes of distinct paths than VD, but is far less expensive ^{3}^{3}3To test VD relation, one should compute a visibility diagram and do path searching within it[17], which has higher complexity than testing UVD. for equivalence checking. To test UVD relation, one can uniformly discretize to and check collision for lines . For the piecewise straight line paths (as in Alg. 1, Equivalent()), we simply parameterize it uniformly, so that for any except is the join points of two straight lines, .
VB Topological Roadmap
Alg.1 is used to construct a UVD roadmap capturing an abundant set of paths from different UVD classes. Unlike standard PRM containing many redundant loops, our method generates a more compact roadmap where each UVD class contains just one or a few paths (displayed in Fig.8(a)8(c)).
We introduce two different kinds of graph nodes, namely guard and connector, similar to the VisibilityPRM [42]. The guards are responsible for exploring different part of the free space, and any two guards and are not visible to each other (line is in collision). Before the main loop, two guards are created at the start point and end point . Every time a sampled point is invisible to all other guards, a new guard is created at this point (Line 67). To form paths of the roadmap, connectors are used to connect different guards (Line 719). When a sampled point is visible to exactly two guards, a new connector is created, either to connect the guards to form a topologically distinct connection (Line 1920), or to replace an existing connector to make a shorter path (Line 1617). Limits of time () or sampling number () are set to terminate the loop. With the UVD roadmap, a depthfirst search augmented by a visited node list is applied to search for the paths between and , similar to [39].
VC Path Shortening & Pruning
As shown in Fig. 8(d), some paths obtained from Alg. 1 may be detoured. Such paths are unfavorable, since in the first phase of PGO it can deform a trajectory excessively and make it unsmooth. Hence, Alg. 2 find a topologically equivalent shortcut path for each found by the depthfirst search (illustrated in Fig. 9). The algorithm uniformly Discretizes to a set of points . In each iteration, if a point in is invisible from the last point in (Line 3, 4), the center of the first occupied voxel blocking the view of is found (Line 5). This point is then pushed away from obstacles in the direction orthogonal to and coplanar to both and the ESDF gradient at (Line 6), after which it is appended to (Line 7). The process continues until the last point is reached. Although in Alg. 1, redundant connection between two guards are avoided, there may exist a small number of redundant paths between and (Fig. 8(d)). To completely exclude repeated ones, we check the equivalence between any two paths and only preserve topologically distinct ones. Also note that the number of distinctive paths grows exponentially with the number of obstacles. In case of complex environments, it is computationally intractable to use all paths to guide parallel optimization. For this reason, we only select the first shortest paths. Paths more than times longer than the shortest one are also pruned away. Such strategies bound the complexity and will not miss the potentially optimal solution, because a very long path is very unlikely to result in the optimal trajectory.
Vi Riskaware Trajectory Refinement
Our trajectory refinement takes the best trajectory from the parallel PGO as input, modifies it in its vicinity and outputs the refined trajectory (detailed in Alg.3). It starts by checking the visibility status of , after which the visibility to relevant unknown space and the safe reaction distance are enforced in the iterative refinement.
Via Checking Visibility Status
The visibility status is encoded by several variables: , , which are important information about the unknown space passed through by . Some of the involved variables are illustrated in Fig.10.
ViA1 Frontier Intersecting Point
As the unknown environment is only partially observed, at some time the trajectory exits the knownfree space and enters the unknown space. The position , should be prioritized for observation due to three reasons: First, it is highly relevant for the future flight, because it belongs to a promising trajectory going toward the goal. Second, it may be dangerous to the flight. In the worst case an unknown obstacle can be right adjacent to it. Third, it will be reached earlier compared to other unknown position along . Therefore in we search along with a discrete time step, recording the first unknown point and the corresponding time.
ViA2 Visibility Metric
During the flight, it is preferred that becomes visible to some preceding positions on . Quantitatively, we want some visibility level of to be not less than an expected level , so that not only is visible but also the visibility level is tolerant to external disturbance. To gauge how reliable is visible, we adopt the metric used by [18, 44], defining the visibility level from a position to as:
(5) 
which represents the the smallest Euclidean signed distance between the line segment and obstacles. The evaluation of Equ.5 requires traversing and checking the Euclidean signed distance for each point. Fortunately, an ESDF derived from the occupancy grid map is maintained by our mapping module for supporting the trajectory optimization (Sect.IV), so minimal distance can be queried in constant time.
ViA3 Critical View Direction
We are interested in whether can be viewed from preceding positions on , i.e., if for some time . The best case is that for all the visibility level is higher than , in which no modification to the trajectory is needed (Line 34). However, in cluttered environments may not be observable until some time , which means when and when . Here, the view direction where is a critical view direction, in which just reaches the desired level . In this case reports and the corresponding position and time .
ViB Iterative Refinement
As is the worst case, an unknown obstacle may be revealed right behind and block the trajectory. To ensure safety, we refine so that under the worst case the quadrotor will be able to avoid collision by taking some maneuvers whose singleaxis acceleration does not exceed the limit . We first check whether satisfies the worstcase safety criteria (Line 57). If it does not, the critical view and safe reaction distance constraints are enforced (Line 815).
ViB1 Worstcase Safety Criteria
As showed in Sect.VIA3, initially will not become reliably viewable until at . Suppose at the speed is , while the distance to is . Similar to [24], we check if Equ.6 holds:
(6) 
which means that if at the quadrotor sees an obstacle, it can decelerate to a stop before colliding with the obstacle right behind . compensates the quadrotor size and disturbance. If it is not true, extra constraints are added to meet this criteria.
ViB2 View and Safety Constraints
If initially it would be too late for collision avoidance, should be viewed at an earlier stage , so that maneuvers to avoid collision can be taken in advance. Given the critical view direction , this constraint can be expressed as:
(7)  
(8) 
The interpretation is that at the quadrotor should have already reached the ray from which is viewable (Fig.11). Besides the advanced observation, the distance from to should be sufficient for an emergency stop:
(9) 
where is the safe reaction distance that depends on the speed at the refined trajectory : . However, since the refined trajectory is not obtained yet, is not available at this stage. To solve this chickenandegg problem, we introduce an iterative strategy. At the beginning we use the average speed of the segment of between as an estimate of (Line 8), where is the start time of . Then the trajectory is refined with the view and safety constraints (Line 1012). After the refinement we check whether the new trajectory satisfies the safety criteria. If it does not, we increased the estimated speed with a factor slightly larger than 1 and redo the refinement (Line 1315). This strategy is complete because it only terminates after the safety criteria is indeed met. It also terminates quickly, since the speed along the smooth trajectory varies slightly and it only takes one or a few steps to find a good estimate of in practice. is essentially optimizing the trajectory with the newly introduced view and safety constraints. To incorporate them into our gradientbased optimization (Sect.IVB), Equ.79 are softconstrained by penalty functions:
(10)  
(11) 
Here is the penalty function (Equ.3). The Jacobian of these terms are:
(12) 
(13) 
Here Bspline control points associated with . The refinement minimizes the cost function:
(14) 
where is exactly the same as Equ.2. Note that applying the constraints to different generates different results. However, including into the optimization to find its best value would make the problem too difficult to be solved quickly. For simplicity, we first determine as the time that minimizes for the initial and use it for the refinement (Line 1112). Quantitatively, the selected leads to minimal violation of the constraints, therefore enforcing the constraints at also requires less modification to , which is a reasonably good choice.
Vii Yaw Angle Planning
Quadrotors typically have limited sensor FOVs. To improve flight safety, we plan the trajectory of yaw angle to actively observe the environments. Inspired by the recent twostep quadrotor motion planning paradigm [5, 14, 12, 13, 8, 49], we also decompose the yaw angle planning into a graph search problem and a trajectory optimization.
Viia Graph Search
ViiA1 Problem Modeling
We model a graph search problem to seek for an sequence of yaw angles along the refined trajectory that trades off the smoothness and information gain (IG, detailed in Sect.VIIA2) of the unknown space. Given the quadrotor trajectory (Sect.VI), a set of positions uniformly distributed along the trajectory at are selected. At expect , where the yaw angle is already determined by the current quadrotor’s state, several graph nodes are created, each of which associates a different angle and the IG at the state . For each pair of nodes associated with adjacent positions, a graph edge from to is created. This process construct a directed graph as shown in Fig.12. To find a sequence of yaw that maximizes IG and smoothness, we search a path in the graph that minimizes the cost , which can be solved efficiently by the Dijkstra algorithm:
(15) 
where is used to adjust the weighting of smoothness.
ViiA2 Information Gain
We employ a similar method to [3] which assesses potential IG as the number of unmapped voxels that comply with the camera model and are visible (not blocked by occupied voxels). However, the original method does raycasting for every voxels inside the camera FOV to validate their visibility, which is too expensive to function online. Therefore, we adapt it to better suit the realtime planning in several ways: (a) As is in [32], voxels inside the FOV are subsampled to approximate the actual gain, which leads to only slight error but great run time reduction. (b) The gains of different are evaluated in parallel. (c) We borrow the techniques from [28] to avoid repeated raycasting. As depicted in Fig.13(a), we notice that at one position where different are assessed, many voxels are in overlapping areas and are checked for visibility more than once. To avoid unnecessary repetition, we store the visibility of each voxel when it is checked for the first time, so that in subsequent check the visibility and be queried directly. In these ways, the overall IG evaluation time is reduced by over two orders of magnitude.
In the context of exploration[3], every voxel contributes equally to the IG of one quadrotor configuration, ensuring that all space can be covered by the sensors uniformly. However, in a pointtopoint navigation we do not aim at full coverage but prefer focusing on space relevant to the flight. In particular, unknown voxels closer to the trajectory and the current position have higher influence to the flight. Therefore, we use Equ.16 to bias the IG contribution of a visible voxel centered at :
(16) 
where and represent the lateral and longitudinal distance to trajectory (Fig.13(b)).
ViiB Yaw Angle Optimization
Given the optimal path searched through the graph, we compute the trajectory of yaw angle that is smooth, dynamically feasible and passes through the sequential angles . We parameterize as a uniform Bspline with control points and knot span . In this way, the convex hull property can be employed to ensure dynamic feasibility[49]. The problem is formulated as:
(17)  
Here the first term represents smoothness and the second term is a soft waypoint constraint enforcing to pass through . The last two terms are the soft constraints for dynamic feasibility, wherein and are the Bspline control points of angular velocity and acceleration:
(18) 
Thanks to the convex hull property of Bspline, the entire trajectory is guaranteed to be feasible given that the control points do not exceed the dynamic limits .
Comparison of four local planners in scene 1. (a) using planners A and B, the boxes in the back row are not discovered until the quadrotor gets very close. Consequently the quadrotor has to pause in emergency. (b) The 3D maps (colorful voxels) and executed trajectories (red curves) at the moment when
boxes in the back are just discovered. (c),(d): the quadrotor avoid all boxes successfully with planners D. More details are showed in the video.Viii Results
Viiia Implementation Details
We present tests in both real world and simulation. In realworld experiments, a customized quadrotor platform equipped with an Intel RealSense Depth Camera D435 is used. All the state estimation, mapping, planning and control modules run on an Intel Core i78550U CPU. For simulation, we use a simulating tool containing the quadrotor dynamics model, random map generator and depth image renderer. The dynamics model relies on a numeric ODE solver odeint^{4}^{4}4www.boost.org/doc/libs/1_73_0/libs/numeric/odeint/doc/html/index.html. The depth images are rendered in GPU by projecting point cloud of the surrounding obstacles onto the image plane. Random noises are added to them to better mimic the real measurements. All simulations run on an Intel Core i78700K CPU and GeForce GTX 1080 Ti GPU. The trajectory optimization is solved by a general nonlinear optimization solver NLopt^{5}^{5}5https://nlopt.readthedocs.io/en/latest/.
Planner  Strategy  Success Number  

Scene 1  Scene 2  
A  Optimistic & Velocitytracking  0  0 
B  Optimistic & Active exploration  0  0 
C  Riskaware & Velocitytracking  2  0 
D  Riskaware & Active exploration  3  3 
ViiiA1 Global Planning
We use the approach [36] to compute global reference trajectories. Note that we focus on evaluating the local replanning system, therefore, naive global trajectories are given, such as straightline trajectory connecting the start and goal positions.
ViiiA2 Volumetric Mapping
In all tests, the quadrotor starts with no prior knowledge of the environments. A volumetric mapping framework [15] fuses the depth images from the stereo camera into a occupancy grid map. An ESDF is derived from the occupancy grid map using an efficient distance transform algorithm [10] to support the gradientbased optimization (Sect.IV) and visibility evaluation (Sect.VIA2
). Trilinear interpolation is also used to reduce the distance error induced by the discrete grid map.
ViiiA3 State Estimation and Control
We localize the drone by a robust visualinertial state estimator [34] in realworld tests. In simulation, ground truth odometry is generated by the quadrotor dynamics model. We use a geometric controller[20] to track both the position and yaw trajectory. The following evaluation is divided into two parts. The first part evaluates the perceptionaware planning strategy, the second part tests the whole replanning framework.
ViiiB Perceptionaware Planning Strategy
ViiiB1 Realworld Tests
We conduct comparative experiments to show the importance of introducing active perception. Specifically we compare the proposed strategy: the riskaware refinement (Sect.VI) and active exploration yaw (Sect.VII) with the commonly used ones: the optimistic assumption and the velocitytracking yaw. Optimistic assumption treats all unknown space as collisionfree, which is frequently adopted such as in [13, 23, 49, 45]. The velocitytracking yaw relates the desired yaw angle to the velocity: , to increase the chance of seeing obstacles. Four local planners listed in Tab.I are tested in two scenes. Each planner is tested 3 times in both scenes and we record the number of successful flights. The maximum velocity and acceleration are set as and . Whenever collision along the trajectory is detected and the collision point is closer than , emergency stop is conducted immediately for safety. In the first scene, a straightline global reference trajectory is given (Fig.14(a)). A large obstacle consisting of several boxes and boards are placed on the way. When the quadrotor approaches the obstacle, boxes in the front row will be revealed first, while others behind them are occluded and invisible at the beginning. Planners with optimistic assumption (A & B) are unaware of the potential danger behind. They simply replan trajectories to avoid the viewed boxes, along which there is low visibility to the boxes in the back, as showed in Fig.16(a). As a result, the quadrotor gets ’surprised’ by the occluded boxes afterwards and pauses in emergency, as showed in Fig.15(a) and 15(b). In contrast, planners with risk awareness (C & D) generate trajectories that deviate a bit more laterally, along which visibility toward the unknown area in the back is higher (Fig.16(b)). Therefore in both cases the quadrotor reach the goal more times. However, with the velocitytracking yaw (planner C), the quadrotor does not face toward the unknown area in the back quickly, which postpones the discovery of occluded boxes and causes 1 failure. In comparison, with the active exploration yaw (planner D) the quadrotor quickly turns toward the unknown area and observes the previously occluded boxes, enabling itself to take action earlier. This comparison is displayed in Fig.17. In the second scene, an obstacle is placed right behind the corner, which is invisible to the quadrotor until it turns right. The reference trajectory is set to pass through the obstacles deliberately (Fig.14(b)). In this scene, the quadrotor can only reach the goal safely with planner D. For other three planners, the quadrotor collides with the obstacle behind the corner, due to either the poor visibility of the replanned trajectories (planner A & B), or the delay of perception caused by the velocitytracking yaw (planner C). Note that even emergency stop is conducted, the quadrotor fail to avoid collision in time (Fig.18). The comparisons of the replanned trajectories and yaw angle are displayed in Fig.19 and Fig.20 respectively. The experiments demonstrate two critical factors to survive in highspeed flights: (a) having good visibility toward the unknown regions that will influence the flight and (b) looking toward the relevant direction to eliminate those unknown regions actively. The proposed method takes into account these factors and guarantees safety for fast flight. More details about the experiments are presented in the attached video.
ViiiB2 Benchmark Comparisons
We compare the proposed strategy with the safe local exploration (SLE) presented in [32] in simulation. This strategy originated from the ”nextbestview” planner [3] in the exploration literature, but is adapted for online functioning in goal reaching tasks. It repeatedly selects intermediate goals that are closer to the final goal and have higher information gain, after which a local planner replans new trajectories toward the goals. It also adopts the velocitytracking yaw, as is detailed in Sect.VIIIB1. To compare the strategies fairly, we integrated both of them with our robust optimistic replanning (Sect.IV, V). They are tested in random maps with different obstacle densities, trials are conducted for each map. We compare the number of successful flight, flight time and flight distance. Samples of the maps are displayed in Fig.22(a), 22(b). As is shown in Tab.II, the proposed strategy achieves higher number of successful flights when the scene gets more cluttered. Our strategy enforces visibility to dangerous unknown areas, and control the yaw angle to observe those areas actively. Therefore it can guarantee safety even the environment becomes very complex. For SLE, the velocitytracking yaw is the major cause of failure, as it may not face toward dangerous unknown regions in time, as has been shown in Sect.VIIIB1.
Besides, our strategy is also more beneficial to achieve lower flight distance and time than SLE. SLE only selects intermediate goals and plans within the known unoccupied space, which is conservative. Besides, since the selection of intermediate goals takes information gain into account, the quadrotor tends to take some detours to gather more information, which leads to longer flight distance and time. In contrast, our strategy plans in both the known and unknown space, allowing more aggressive behaviors under the premise of safety. Moreover, instead of treating all unknown areas equally, it only focus on observing areas that are more relevant to the flight, which eliminates many unnecessary detours and improve the overall flight efficiency.
ViiiC Replanning Framework for Fast Flight
ViiiC1 Benchmark Comparisons
We compare our replanning framework with several stateoftheart methods, FASTER [43], EWOK[45] and RE Traj.[49]. FASTER belongs to the hardconstrained category (Sect.IIA), and features maintaining a feasible and safe backup trajectory in the freeknown space at each replanning step to improve safety. It also adopts a mixed integer quadratic program (MIQP) formulation to obtain a more reasonable time allocation of the trajectories. Both [45, 49] belong to the gradientbased methods. They utilize a uniform Bspline trajectory representation to replan efficiently. [49] further exploits the convex hull property of Bspline and introduces a kinodynamic path searching to find more promising initial trajectories. We also test the four methods in 10 random maps with 5 obstacle densities. Note that all benchmarked methods are opensource and we use their default parameter settings. The number of successful flights, average flight distance, flight time, energy (integral of squared jerk), computation time of each replanning, and total replan number in each flight are recorded. Samples of the maps and the trajectories generated by the four methods are shown in Fig.22.

Method 





0.2  SLE[32]  10  43.475  20.012  
Proposed  10  41.822  16.608  
0.25  SLE[32]  10  44.682  20.593  
Proposed  10  42.133  19.338  
0.3  SLE[32]  9  44.874  22.316  
Proposed  10  42.982  19.483  
0.35  SLE[32]  9  46.093  23.577  
Proposed  10  45.436  22.050  
0.4  SLE[32]  8  47.817  27.903  
Proposed  10  46.776  23.639 
As displayed in Fig.21, our method outperform others in the aspects of flight distance, flight time and energy consumption, with competitive computation efficiency. FASTER rarely fails in the tests, thanks to the backup trajectories. However, due to the computationally demanding MIQP formulation, its overhead is higher. The other two benchmarked methods are more efficient. However, EWOK suffers from the local minima issue, so it usually fails or outputs lowquality solutions in dense environments. The kinodynamic path searching and Bspline optimization adopted by RE Traj. relieve the local minima significantly. Nonetheless, due to the lack of perception consideration, the succuss number is mediocre in dense environments. Compared to them, the proposed method search the solution space effectively with the guidance of topologically distinctive paths, and generates highquality trajectories consistently. Safety is also reenforced by introducing perception awareness.
ViiiC2 Indoor Flight Test
We conduct aggressive flight experiments in three indoor scenes (Fig.2, 23) to validate our planning system. Various types of obstacles are placed randomly and densely to make up the challenging flight environments. Distance of neighboring obstacles are only around 1 meter, making the space for safe navigation very limited. Besides, the high obstacle density makes visibility to the environment very restricted, since many obstacles are occluded by others, which poses greater challenges to the replanning algorithm. In each experiment, the final goal is set to 14 away from the quadrotor. Straightline global reference trajectories are given and local replanning is conducted within a horizon of 7 . Samples of the online generated map and executed trajectories are presented in Fig.24. The velocity profile of one flight are showed in Fig.25(a), in which the maximum speed is and average speed is . The flight distance and time are and respectively. We refer the readers to the attached video for more tests.
ViiiC3 Outdoor Flight Test
Finally, we conduct fast flight tests in three different outdoor scenes, as displayed in in Fig.26, to validate our planning method in natural environments. The outdoor environments are typically unstructured and irregular, where the quadrotor should perform agile 3D maneuvers to avoid obstacles such as rocks and branches and leaves of trees. Note that despite the outdoor environments, we do not use external devices for localization. Results of the online generated map and executed trajectories are presented in Fig.27. In the first scene, the quadrotor flies through the forest to the goal away from the initial position. The velocity profile is showed in Fig.25(b). The maximum speed is and average speed is . The flight takes and . In the second scene, the quadrotor flies up a slope to the first goal, after which it flies to the second goal. The first goal is away and the change in height is . The second goal is far. The whole flight takes and . The third scene is a larger forest, where the goal is set to away. The flight distance is , which takes to finish. The maximum and average speed are and . More details of the flights are showed in the attached video.
Ix Conclusions
In this paper, we propose a robust and perceptionaware replanning method for highspeed quadrotor autonomous navigation. The pathguided optimization and topological path searching are devised to escape from local minima and explore the solution space more thoroughly, through which higher robustness and optimality guarantee are obtained. The robust planner is further enhanced by the perceptionaware strategy, which takes special caution about regions that may be dangerous to the quadrotor. The yaw angle of the quadrotor is also planned to actively explore the environments, especially areas that are relevant to the future flight. The planning system is evaluated comprehensively through benchmark comparisons. We integrate the planning method with global planning, state estimation, mapping, and control into a quadrotor platform and conduct extensive challenging indoor and outdoor flight tests. Results show that the proposed method is robust and capable of supporting fast and safe flights. We release the implementation of our system to the community.
References
 [1] (2012) Topological constraints in searchbased robot path planning. Autonomous Robots 33 (3), pp. 273–290. Cited by: §IIB.

[2]
(2010)
Searchbased path planning with homotopy class constraints.
In
TwentyFourth AAAI Conference on Artificial Intelligence
, Cited by: §IIB.  [3] (2018) Receding horizon path planning for 3d exploration and surface inspection. Auton. Robots 42 (2), pp. 291–306. Cited by: §VIIA2, §VIIA2, §VIIIB2.
 [4] (2018) Topomap: topological mapping and navigation based on visual slam maps. In 2018 IEEE International Conference on Robotics and Automation (ICRA), pp. 1–9. Cited by: §IIB, §V.
 [5] (201508) Realtime safe trajectory generation for quadrotor flight in cluttered environments. In Proc. of the IEEE Intl. Conf. on Robot. and Biom., Zhuhai, China. Cited by: §IIA, §VII.
 [6] (2016) Perceptionaware path planning. IEEE Trans. Robot. (TRO), pp. Epub–ahead. Cited by: §IIC.
 [7] (2018) Trajectory replanning for quadrotors using kinodynamic search and elastic optimization. In Proc. of the IEEE Intl. Conf. on Robot. and Autom. (ICRA), pp. 7595–7602. Cited by: §IIA.
 [8] (2019) An efficient bsplinebased kinodynamic replanning framework for quadrotors. IEEE Transactions on Robotics 35 (6), pp. 1287–1306. Cited by: §IIA, §VII.
 [9] (2018) PAMPC: perceptionaware model predictive control for quadrotors. In Proc. of the IEEE/RSJ Intl. Conf. on Intell. Robots and Syst.(IROS), pp. 1–8. Cited by: §IIC.
 [10] (2012) Distance transforms of sampled functions. Theory of computing 8 (1), pp. 415–428. Cited by: §VIIIA2.
 [11] (2017Sept) Gradientbased online safe trajectory generation for quadrotor flight in complex environments. In Proc. of the IEEE/RSJ Intl. Conf. on Intell. Robots and Syst.(IROS), pp. 3681–3688. Cited by: §IIA.
 [12] (2016) Online quadrotor trajectory generation and autonomous navigation on point clouds. In Proc. of the IEEE Intl. Sym. on Safety, Security, and Rescue Robotics (SSRR), lausanne, switzerland, pp. 139–146. Cited by: §IIA, §VII.
 [13] (2018) Flying on point clouds: online trajectory generation and autonomous navigation for quadrotors in cluttered environments. Journal of Field Robotics. External Links: Link Cited by: §IIA, §IIC, §VII, §VIIIB1.
 [14] (201805) Online safe trajectory generation for quadrotors using fast marching method and bernstein basis polynomial. In Proc. of the IEEE Intl. Conf. on Robot. and Autom. (ICRA), Brisbane, Australia. Cited by: §IIA, §VII.
 [15] (2019) FIESTA: fast incremental euclidean distance fields for online motion planning of aerial robots. arXiv preprint arXiv:1903.02144. Cited by: §VIIIA2.
 [16] (2017) Planning highspeed safe trajectories in confidencerich maps. In Proc. of the IEEE/RSJ Intl. Conf. on Intell. Robots and Syst.(IROS), pp. 2880–2886. Cited by: §IIC.
 [17] (2008) Path deformation roadmaps: compact graphs with useful cycles for motion planning. The International Journal of Robotics Research 27 (1112), pp. 1175–1188. Cited by: §IIB, §VA, §V, footnote 3.
 [18] (2019) Online trajectory generation of a mav for chasing a moving target in 3d dense environments. arXiv preprint arXiv:1904.03421. Cited by: §IIC, §VIA2.
 [19] (2011) STOMP: stochastic trajectory optimization for motion planning. In 2011 IEEE international conference on robotics and automation, pp. 4569–4574. Cited by: §IIA.
 [20] (2010) Geometric tracking control of a quadrotor uav on se (3). In Decision and Control (CDC), 2010 49th IEEE Conference on, pp. 5420–5425. Cited by: §VIIIA3.
 [21] (2018) Autonomous aerial navigation using monocular visualinertial fusion. Journal of Field Robotics 35 (1), pp. 23–51. Cited by: §IIA.
 [22] (2017Sept) Searchbased motion planning for quadrotors using linear quadratic minimum time control. In Proc. of the IEEE/RSJ Intl. Conf. on Intell. Robots and Syst.(IROS), pp. 2872–2879. Cited by: §V.
 [23] (2017) Planning dynamically feasible trajectories for quadrotors using safe flight corridors in 3d complex environments. IEEE Robotics and Automation Letters (RAL), pp. 1688–1695. Cited by: §IIC, §VIIIB1.
 [24] (2016) High speed navigation for quadrotors with limited onboard sensing. In Proc. of the IEEE Intl. Conf. on Robot. and Autom. (ICRA), pp. 1484–1491. Cited by: §IIC, §VIB1.
 [25] (2017) Aggressive collision avoidance with limited fieldofview sensing. In Proc. of the IEEE/RSJ Intl. Conf. on Intell. Robots and Syst.(IROS), pp. 1358–1365. Cited by: §IIC.
 [26] (2017) Aggressive 3d collision avoidance for highspeed navigation.. In Proc. of the IEEE Intl. Conf. on Robot. and Autom. (ICRA), pp. 5759–5765. Cited by: §IIC.
 [27] (201105) Minimum snap trajectory generation and control for quadrotors. In Proc. of the IEEE Intl. Conf. on Robot. and Autom. (ICRA), Shanghai, China, pp. 2520–2525. Cited by: §IIA.
 [28] (2018) Cblox: a scalable and consistent tsdfbased dense mapping approach. In Proc. of the IEEE/RSJ Intl. Conf. on Intell. Robots and Syst.(IROS), pp. 995–1002. Cited by: §VIIA2.
 [29] (2019) Perceptionaware trajectory generation for aggressive quadrotor flight using differential flatness. In 2019 American Control Conference (ACC), pp. 3936–3943. Cited by: §IIC.
 [30] (2019) Searchbased 3d planning and trajectory optimization for safe micro aerial vehicle flight under sensor visibility constraints. In Proc. of the IEEE Intl. Conf. on Robot. and Autom. (ICRA), pp. 9123–9129. Cited by: §IIC.
 [31] (2016Oct.) Continuoustime trajectory optimization for online uav replanning. In Proc. of the IEEE/RSJ Intl. Conf. on Intell. Robots and Syst.(IROS), Daejeon, Korea, pp. 5332–5339. Cited by: §IIA.
 [32] (2018) Safe local exploration for replanning in cluttered unknown environments for microaerial vehicles. IEEE Robotics and Automation Letters 3 (3), pp. 1474–1481. Cited by: §IIC, §VIIA2, §VIIIB2, TABLE II.
 [33] (2018) Sparse 3d topological graphs for microaerial vehicle planning. In Proc. of the IEEE/RSJ Intl. Conf. on Intell. Robots and Syst.(IROS), pp. 1–9. Cited by: §IIB, §V.
 [34] (2017) VINSmono: a robust and versatile monocular visualinertial state estimator. arXiv preprint arXiv:1708.03852. Cited by: §VIIIA3.
 [35] (200905) CHOMP: gradient optimization techniques for efficient motion planning. In Proc. of the IEEE Intl. Conf. on Robot. and Autom. (ICRA), pp. 489–494. Cited by: §IIA.
 [36] (2013Dec.) Polynomial trajectory planning for aggressive quadrotor flight in dense indoor environments. In Proc. of the Intl. Sym. of Robot. Research (ISRR), pp. 649–666. Cited by: §IIA, §VIIIA1.
 [37] (2016) Learning to plan for visibility in navigation of unknown environments. In International Symposium on Experimental Robotics, pp. 387–398. Cited by: §IIC.
 [38] (2015) Planning of multiple robot trajectories in distinctive topologies. In 2015 European Conference on Mobile Robots (ECMR), pp. 1–6. Cited by: §IIB.
 [39] (2017) Integrated online trajectory planning and optimization in distinctive topologies. Robotics and Autonomous Systems 88, pp. 142–153. Cited by: §IIB, §VB.
 [40] (2002) Capture of homotopy classes with probabilistic road map. In Proc. of the IEEE/RSJ Intl. Conf. on Intell. Robots and Syst.(IROS), Vol. 3, pp. 2317–2322. Cited by: §IIB, §V.
 [41] (2014) Motion planning with sequential convex optimization and convex collision checking. The International Journal of Robotics Research 33 (9), pp. 1251–1270. Cited by: §IVA.
 [42] (2000) Visibilitybased probabilistic roadmaps for motion planning. Advanced Robotics 14 (6), pp. 477–493. Cited by: §VB.
 [43] (2019) FASTER: fast and safe trajectory planner for flights in unknown environments. In Proc. of the IEEE/RSJ Intl. Conf. on Intell. Robots and Syst.(IROS), Cited by: §IIA, §IIC, 22(c), §VIIIC1.
 [44] (2004) Visibility and its dynamics in a pde based implicit framework. Journal of Computational Physics 199 (1), pp. 260–290. Cited by: §VIA2.
 [45] (2017) Realtime trajectory replanning for mavs using uniform bsplines and a 3d circular buffer. In Proc. of the IEEE/RSJ Intl. Conf. on Intell. Robots and Syst.(IROS), pp. 215–222. Cited by: §IIA, §IIC, 22(c), §VIIIB1, §VIIIC1.
 [46] (201305) Kinodynamic RRT*: asymptotically optimal motion planning for robots with linear dynamics. In Proc. of the IEEE Intl. Conf. on Robot. and Autom. (ICRA), Germany, pp. 5054–5061. Cited by: §V.
 [47] (2018) Perceptionaware receding horizon navigation for mavs. In Proc. of the IEEE Intl. Conf. on Robot. and Autom. (ICRA), pp. 2534–2541. Cited by: §IIC.
 [48] (2020) Robust realtime uav replanning using guided gradientbased optimization and topological paths. In Proc. of the IEEE Intl. Conf. on Robot. and Autom. (ICRA), Note: to appear External Links: Link Cited by: §I, §IIA.
 [49] (2019) Robust and efficient quadrotor trajectory generation for fast autonomous flight. IEEE Robotics and Automation Letters 4 (4), pp. 3529–3536. Cited by: §IIA, §IIC, §IVB, §IVB, §VIIB, §VII, 22(c), §VIIIB1, §VIIIC1.
Comments
There are no comments yet.