In recent years, progresses on different aspects of unmanned aerial vehicles (UAVs), especially quadrotor autonomy have been achieved and promote autonomous navigation. Nonetheless, high-speed 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 high-speed 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 high-speed flight. In this paper, we propose a Robust And Perception-aware TrajectOry Replanning framework called RAPTOR to address these issues systematically. To ensure obtaining feasible trajectories within limited time, we present a path-guided gradient-based 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 . 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 perception-aware planning strategy to enable faster and safer flight from two aspects. Firstly, a risk-aware 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 two-step 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 field-of-view (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 perception-aware planning strategy and the whole planning system, through benchmark comparisons and challenging real-world 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 state-of-the-art 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 paths-guided gradient-based replanning approach, that is capable of generating high-quality trajectories in limited time. 2) A risk-aware trajectory refinement approach, which enforces visibility and safe reaction distance to unknown obstacles. It improves the predictability and safety of fast flights. 3) A two-step yaw angle planning method, to actively explore the unknown environments and gather useful information for the flight. 4) Extensive simulation and real-word tests that validate the proposed method. The source code of our system will be released as an open-source package.
Ii Related Work
Ii-a Quadrotor Trajectory Planning
Trajectory planning for quadrotor has been widely investigated. Existing methods can be categorized into hard-constrained methods and gradient-based optimization methods. Hard-constrained methods are pioneered by minimum-snap trajectory, in which piecewise polynomial trajectories are generated through quadratic programming(QP).  presented a closed-form solution to minimum snap trajectories. [5, 14, 12, 13, 8] generate trajectories in a two-step 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 and kinodynamic search[7, 8] are utilized to search for initial paths with more reasonable time allocation.  also proposed to represent trajectories as piecewise Bézier curves so that safety and dynamical feasibility are guaranteed.  adopted a mixed integer QP formulation to find a more reasonable time allocation of the trajectory. Another category is the gradient-based trajectory optimization (GTO) methods, which typically formulate trajectory generation as non-linear 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 high-speed flight in unknown environments.  proposed to optimize discrete-time trajectory through covariant gradient descent, reviving the community’s interest in such methods.  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.  extended it to continuous-time 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 sampling-based path searching to improve the success rate.  proposed to parameterize trajectories as uniform B-splines, showing the usefulness of continuity and locality properties of B-spline for replanning. However, due to insufficient success rate and efficiency, [31, 11, 45] only apply to flight at a moderate speed. To this end,  further exploited B-spline 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.
Ii-B 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  classes are sought.  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 is adopted to filter out redundant paths. These methods only apply to 2D scenarios. To seek for 3D homology classes,  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 , since 3D homotopic paths may be too hard to deform into each other. To this end,  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 time-consuming for online usage. Our topological path searching is conceptually closest to , but with a reinvented algorithm for real-time performance.
Ii-C 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 collision-free. 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 known-free space  or sensor FOV [26, 30]. In  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  proposed a strategy that plans in both the known-free and unknown space. Instead of being over optimistic about the unknown space, it always maintains back-up 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. 
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. 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 , a local planner is coupled with local exploration to safely navigate a cluttered environment. However, it conservatively selects intermediate goals within known-free space, which restricts the flight speed. In this paper, we present a perception-aware 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 path-guided 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 perception-aware planning strategy is utilized. The best trajectory among the locally optimal ones is further polished by a risk-aware 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.VIII-A.
Iv Path-Guided Trajectory Optimization
As showed in Sect.II-A, 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.
Iv-a Optimization Failure Analysis
Previous work 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.
Iv-B Problem Formulation
We propose PGO built upon our previous work  that represents trajectories as B-splines for more efficient cost evaluation. For a trajectory segment in collision, we reparameterize it as a degree uniform B-spline 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 collision-free paths are readily available from standard methods like A* and RRT*. In our work, the paths are provided by the sampling-based topological path searching (Sect. V). The first-phase objective function is:
where is the smoothness term, while penalize the distance between the guiding path and the B-spline trajectory. As in , is designed as a elastic band cost function222Only 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 B-spline 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 collision-free, usually the warmup trajectory is also so. Even though it is not completely collision-free, 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 B-spline optimization method to further refine the warmup trajectory into a smooth, safe, and dynamically feasible one, whose objective function is:
Here is a penalty function for general variables:
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:
and are single-axis maximum velocity and acceleration. The formulations of , , and are based on the convex hull property of B-spline, thanks to which it suffices to constrain the control points of the B-spline to ensure safety and dynamic feasibility. For brevity, we refer the readers to  for more details. Although PGO has one more step of optimization compared with previous methods, it can generate better trajectories within shorter time. The first-phase 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 sampling-based 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 real-time in complex 3D environments. We redesign the algorithm carefully to solve this challenging problem in real time.
V-a 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).  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 collision-free. 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 straight-line. 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 333To test VD relation, one should compute a visibility diagram and do path searching within it, 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 piece-wise 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, .
V-B 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 Visibility-PRM . 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 6-7). To form paths of the roadmap, connectors are used to connect different guards (Line 7-19). 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 19-20), or to replace an existing connector to make a shorter path (Line 16-17). Limits of time () or sampling number () are set to terminate the loop. With the UVD roadmap, a depth-first search augmented by a visited node list is applied to search for the paths between and , similar to .
V-C 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 depth-first 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 Risk-aware 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.
Vi-a 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.
Vi-A1 Frontier Intersecting Point
As the unknown environment is only partially observed, at some time the trajectory exits the known-free 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.
Vi-A2 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:
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.
Vi-A3 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 3-4). 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 .
Vi-B 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 single-axis acceleration does not exceed the limit . We first check whether satisfies the worst-case safety criteria (Line 5-7). If it does not, the critical view and safe reaction distance constraints are enforced (Line 8-15).
Vi-B1 Worst-case Safety Criteria
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.
Vi-B2 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:
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:
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 chicken-and-egg 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 10-12). 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 13-15). 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 gradient-based optimization (Sect.IV-B), Equ.7-9 are soft-constrained by penalty functions:
Here is the penalty function (Equ.3). The Jacobian of these terms are:
Here B-spline control points associated with . The refinement minimizes the cost function:
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 11-12). 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 two-step 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.
Vii-a Graph Search
Vii-A1 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.VII-A2) 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:
where is used to adjust the weighting of smoothness.
Vii-A2 Information Gain
We employ a similar method to  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 real-time planning in several ways: (a) As is in , 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  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, 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 point-to-point 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 :
where and represent the lateral and longitudinal distance to trajectory (Fig.13(b)).
Vii-B 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 B-spline with control points and knot span . In this way, the convex hull property can be employed to ensure dynamic feasibility. The problem is formulated as:
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 B-spline control points of angular velocity and acceleration:
Thanks to the convex hull property of B-spline, 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 whenboxes 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-a Implementation Details
We present tests in both real world and simulation. In real-world 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 i7-8550U 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 odeint444www.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 i7-8700K CPU and GeForce GTX 1080 Ti GPU. The trajectory optimization is solved by a general non-linear optimization solver NLopt555https://nlopt.readthedocs.io/en/latest/.
|Scene 1||Scene 2|
|A||Optimistic & Velocity-tracking||0||0|
|B||Optimistic & Active exploration||0||0|
|C||Risk-aware & Velocity-tracking||2||0|
|D||Risk-aware & Active exploration||3||3|
Viii-A1 Global Planning
We use the approach  to compute global reference trajectories. Note that we focus on evaluating the local replanning system, therefore, naive global trajectories are given, such as straight-line trajectory connecting the start and goal positions.
Viii-A2 Volumetric Mapping
In all tests, the quadrotor starts with no prior knowledge of the environments. A volumetric mapping framework  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  to support the gradient-based optimization (Sect.IV) and visibility evaluation (Sect.VI-A2
). Trilinear interpolation is also used to reduce the distance error induced by the discrete grid map.
Viii-A3 State Estimation and Control
We localize the drone by a robust visual-inertial state estimator  in real-world tests. In simulation, ground truth odometry is generated by the quadrotor dynamics model. We use a geometric controller to track both the position and yaw trajectory. The following evaluation is divided into two parts. The first part evaluates the perception-aware planning strategy, the second part tests the whole replanning framework.
Viii-B Perception-aware Planning Strategy
Viii-B1 Real-world Tests
We conduct comparative experiments to show the importance of introducing active perception. Specifically we compare the proposed strategy: the risk-aware refinement (Sect.VI) and active exploration yaw (Sect.VII) with the commonly used ones: the optimistic assumption and the velocity-tracking yaw. Optimistic assumption treats all unknown space as collision-free, which is frequently adopted such as in [13, 23, 49, 45]. The velocity-tracking 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 straight-line 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 velocity-tracking 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 velocity-tracking 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 high-speed 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.
Viii-B2 Benchmark Comparisons
We compare the proposed strategy with the safe local exploration (SLE) presented in  in simulation. This strategy originated from the ”next-best-view” planner  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 velocity-tracking yaw, as is detailed in Sect.VIII-B1. 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 velocity-tracking yaw is the major cause of failure, as it may not face toward dangerous unknown regions in time, as has been shown in Sect.VIII-B1.
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.
Viii-C Replanning Framework for Fast Flight
Viii-C1 Benchmark Comparisons
We compare our replanning framework with several state-of-the-art methods, FASTER , EWOK and RE Traj.. FASTER belongs to the hard-constrained category (Sect.II-A), and features maintaining a feasible and safe back-up trajectory in the free-known 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 gradient-based methods. They utilize a uniform B-spline trajectory representation to replan efficiently.  further exploits the convex hull property of B-spline 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 open-source 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.
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 back-up 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 low-quality solutions in dense environments. The kinodynamic path searching and B-spline 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 high-quality trajectories consistently. Safety is also reenforced by introducing perception awareness.
Viii-C2 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. Straight-line 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.
Viii-C3 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.
In this paper, we propose a robust and perception-aware replanning method for high-speed quadrotor autonomous navigation. The path-guided 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 perception-aware 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.
-  (2012) Topological constraints in search-based robot path planning. Autonomous Robots 33 (3), pp. 273–290. Cited by: §II-B.
Search-based path planning with homotopy class constraints.
Twenty-Fourth AAAI Conference on Artificial Intelligence, Cited by: §II-B.
-  (2018) Receding horizon path planning for 3d exploration and surface inspection. Auton. Robots 42 (2), pp. 291–306. Cited by: §VII-A2, §VII-A2, §VIII-B2.
-  (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: §II-B, §V.
-  (2015-08) Real-time safe trajectory generation for quadrotor flight in cluttered environments. In Proc. of the IEEE Intl. Conf. on Robot. and Biom., Zhuhai, China. Cited by: §II-A, §VII.
-  (2016) Perception-aware path planning. IEEE Trans. Robot. (TRO), pp. Epub–ahead. Cited by: §II-C.
-  (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: §II-A.
-  (2019) An efficient b-spline-based kinodynamic replanning framework for quadrotors. IEEE Transactions on Robotics 35 (6), pp. 1287–1306. Cited by: §II-A, §VII.
-  (2018) PAMPC: perception-aware model predictive control for quadrotors. In Proc. of the IEEE/RSJ Intl. Conf. on Intell. Robots and Syst.(IROS), pp. 1–8. Cited by: §II-C.
-  (2012) Distance transforms of sampled functions. Theory of computing 8 (1), pp. 415–428. Cited by: §VIII-A2.
-  (2017-Sept) Gradient-based 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: §II-A.
-  (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: §II-A, §VII.
-  (2018) Flying on point clouds: online trajectory generation and autonomous navigation for quadrotors in cluttered environments. Journal of Field Robotics. External Links: Cited by: §II-A, §II-C, §VII, §VIII-B1.
-  (2018-05) 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: §II-A, §VII.
-  (2019) FIESTA: fast incremental euclidean distance fields for online motion planning of aerial robots. arXiv preprint arXiv:1903.02144. Cited by: §VIII-A2.
-  (2017) Planning high-speed safe trajectories in confidence-rich maps. In Proc. of the IEEE/RSJ Intl. Conf. on Intell. Robots and Syst.(IROS), pp. 2880–2886. Cited by: §II-C.
-  (2008) Path deformation roadmaps: compact graphs with useful cycles for motion planning. The International Journal of Robotics Research 27 (11-12), pp. 1175–1188. Cited by: §II-B, §V-A, §V, footnote 3.
-  (2019) Online trajectory generation of a mav for chasing a moving target in 3d dense environments. arXiv preprint arXiv:1904.03421. Cited by: §II-C, §VI-A2.
-  (2011) STOMP: stochastic trajectory optimization for motion planning. In 2011 IEEE international conference on robotics and automation, pp. 4569–4574. Cited by: §II-A.
-  (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: §VIII-A3.
-  (2018) Autonomous aerial navigation using monocular visual-inertial fusion. Journal of Field Robotics 35 (1), pp. 23–51. Cited by: §II-A.
-  (2017-Sept) Search-based 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.
-  (2017) Planning dynamically feasible trajectories for quadrotors using safe flight corridors in 3-d complex environments. IEEE Robotics and Automation Letters (RA-L), pp. 1688–1695. Cited by: §II-C, §VIII-B1.
-  (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: §II-C, §VI-B1.
-  (2017) Aggressive collision avoidance with limited field-of-view sensing. In Proc. of the IEEE/RSJ Intl. Conf. on Intell. Robots and Syst.(IROS), pp. 1358–1365. Cited by: §II-C.
-  (2017) Aggressive 3-d collision avoidance for high-speed navigation.. In Proc. of the IEEE Intl. Conf. on Robot. and Autom. (ICRA), pp. 5759–5765. Cited by: §II-C.
-  (2011-05) 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: §II-A.
-  (2018) C-blox: a scalable and consistent tsdf-based dense mapping approach. In Proc. of the IEEE/RSJ Intl. Conf. on Intell. Robots and Syst.(IROS), pp. 995–1002. Cited by: §VII-A2.
-  (2019) Perception-aware trajectory generation for aggressive quadrotor flight using differential flatness. In 2019 American Control Conference (ACC), pp. 3936–3943. Cited by: §II-C.
-  (2019) Search-based 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: §II-C.
-  (2016-Oct.) Continuous-time 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: §II-A.
-  (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: §II-C, §VII-A2, §VIII-B2, TABLE II.
-  (2018) Sparse 3d topological graphs for micro-aerial vehicle planning. In Proc. of the IEEE/RSJ Intl. Conf. on Intell. Robots and Syst.(IROS), pp. 1–9. Cited by: §II-B, §V.
-  (2017) VINS-mono: a robust and versatile monocular visual-inertial state estimator. arXiv preprint arXiv:1708.03852. Cited by: §VIII-A3.
-  (2009-05) 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: §II-A.
-  (2013-Dec.) 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: §II-A, §VIII-A1.
-  (2016) Learning to plan for visibility in navigation of unknown environments. In International Symposium on Experimental Robotics, pp. 387–398. Cited by: §II-C.
-  (2015) Planning of multiple robot trajectories in distinctive topologies. In 2015 European Conference on Mobile Robots (ECMR), pp. 1–6. Cited by: §II-B.
-  (2017) Integrated online trajectory planning and optimization in distinctive topologies. Robotics and Autonomous Systems 88, pp. 142–153. Cited by: §II-B, §V-B.
-  (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: §II-B, §V.
-  (2014) Motion planning with sequential convex optimization and convex collision checking. The International Journal of Robotics Research 33 (9), pp. 1251–1270. Cited by: §IV-A.
-  (2000) Visibility-based probabilistic roadmaps for motion planning. Advanced Robotics 14 (6), pp. 477–493. Cited by: §V-B.
-  (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: §II-A, §II-C, 22(c), §VIII-C1.
-  (2004) Visibility and its dynamics in a pde based implicit framework. Journal of Computational Physics 199 (1), pp. 260–290. Cited by: §VI-A2.
-  (2017) Real-time trajectory replanning for mavs using uniform b-splines and a 3d circular buffer. In Proc. of the IEEE/RSJ Intl. Conf. on Intell. Robots and Syst.(IROS), pp. 215–222. Cited by: §II-A, §II-C, 22(c), §VIII-B1, §VIII-C1.
-  (2013-05) 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.
-  (2018) Perception-aware receding horizon navigation for mavs. In Proc. of the IEEE Intl. Conf. on Robot. and Autom. (ICRA), pp. 2534–2541. Cited by: §II-C.
-  (2020) Robust real-time uav replanning using guided gradient-based optimization and topological paths. In Proc. of the IEEE Intl. Conf. on Robot. and Autom. (ICRA), Note: to appear External Links: Cited by: §I, §II-A.
-  (2019) Robust and efficient quadrotor trajectory generation for fast autonomous flight. IEEE Robotics and Automation Letters 4 (4), pp. 3529–3536. Cited by: §II-A, §II-C, §IV-B, §IV-B, §VII-B, §VII, 22(c), §VIII-B1, §VIII-C1.