Path planning is one of the key parts of unmanned aerial vehicle (UAV) fast autonomous flight in an unknown cluttered environment. However, realtime and stability remain a significant challenge in the field of path planning. To improve the robustness and efficiency of the path planning method in complex environments, this paper presents RETRBG, a robust and efficient trajectory replanning method based on the guiding path. Firstly, a safe guiding path is generated by using an improved A* and path pruning method, which is used to perceive the narrow space in its surrounding environment. Secondly, under the guidance of the path, a guided kinodynamic path searching method (GKPS) is devised to generate a safe, kinodynamically feasible and minimumtime initial path. Finally, an adaptive optimization function with two modes is proposed to improve the optimization quality in complex environments, which selects the optimization mode to optimize the smoothness and safety of the path according to the perception results of the guiding path. The experimental results demonstrate that the proposed method achieved good performance both in different obstacle densities and different resolutions. Compared with the other stateoftheart methods, the quality and success rate of the planning result are significantly improved.
In recent years, more and more attention has been paid to unmanned aerial vehicles (UAVs), which have been widely used in such fields as urban planning [
Hardconstrained methods have been pioneered by the minimumsnap trajectory generation [
Compared with the hard constraint method, gradientbased optimization has better adaptability. There are many gradientbased trajectory optimization (GTO) methods [
Based on the discussion, both hardconstrained methods and gradientbased optimization methods suffer from the problem of low stability and adaptability. In this paper, we extend the work published by [
This paper designs an improved A* and path pruning method to generate a safe guiding path, which can be used to perceive the surrounding environment and guide the method to search and optimize the path.
Aiming to reduce the spatial information loss caused by discrete control space and improve the quality of the initial path, this paper proposes a guided kinodynamic path searching (GKPS) method based on the guiding path, which not only retains the advantages of the kinodynamic path searching but also improves the safety with the help of the guiding path.
Aiming to improve the performance of path optimization in the cluttered environment, this paper designs an adaptive optimization function with two modes. According to the perception result of the guiding path towards the narrow space, the function adaptively selects the optimization mode, which improves the optimization quality in narrow spaces.
Extensive simulation experiments are carried out with three stateoftheart methods, which validates the effectiveness of the proposed method. We also compare the proposed method with the method that only uses GKPS, which verifies the efficiency of the optimization part.
The rest of the paper is organized as follows:
In general, the generation of the UAV flight path is divided into two modules, the initial path searching and the path optimization. The main purpose of the initial path searching is to obtain a collisionfree and passable path, but it is unreasonable to directly deliver the path to the UAV for execution due to dynamic feasibility, safety, and energy consumption. The path optimization is designed to optimize the initial path into a path that not only meets the requirements of dynamic feasibilities but also achieves the best balance in terms of security and energy consumption. In order to obtain a path that can make the UAV fly quickly and efficiently in an unknown environment on the premise of ensuring safety, we adopt a guiding path to improve the trajectory replanning quality by leading the searching direction and optimization mode.
The algorithm is mainly divided into three parts: map maintenance, path searching, and path optimization. The main contents of each part are as follows:
Map maintenance: this algorithm maintains the occupancy grid map and Euclidean distance field map (EDF). The occupancy grid map is updated in realtime using the information obtained by sensors during flight, and the obstacle information is recorded to provide services for the generation of a guiding path. The Euclidean distance field is only maintained by using the information obtained from current sensors to serve path planning and optimization.
Path searching: At first, we use occupancy grid map and pose data to quickly generate a collisionfree and passable geometric guiding path. Then, using the obstacle information maintained by the occupancy grid map, we perceive the surrounding space that the guiding path passes through and judge whether it passes through a narrow space. Next, under the help of the guiding path, the guided kinodynamic path searching (GKPS) method is designed to quickly generate a dynamically feasible, safe, and lowcost initial path.
Path optimization: We design an adaptive optimization function with two optimization modes, normal optimization (NO) and pathguided optimization (PGO). According to the perception results of the guiding path to the narrow space, the path optimization mode is selected adaptively to optimize the initial path. Finally, a smooth, safe, and feasible path that meets the autonomous and fast flight of UAVs is generated.
The map stores the information of obstacles perceived by sensors, which is the basis of path planning. Due to the rapid flight of the UAV in an unknown environment, a reasonable and efficient map maintenance strategy can not only improve the speed of path replanning, but also ensure the quality of path planning. To improve the efficiency of map maintenance and ensure the quality of the map simultaneously, this paper adopts two kinds of maps—the occupancy grid map and EDF [
The occupancy grid map divides the space into a threedimensional grid with a fixed resolution and stores the environment information by corresponding the perceived obstacle information to the grid. The occupancy grid map is simple and easy to maintain, so this paper uses an occupancy grid map to store the obstacle information sensed by UAV sensors during flight, which provides a guarantee for planning a stable and safe local geometric path. For efficiency, we only update the grid within the sensor range when maintaining the map.
However, although the occupancy grid map can effectively maintain the information of obstacles, it can only provide help for the generation of the guidance path due to its fineness and can not provide accurate information in path optimization. To solve this problem, we maintain a local EDF for path optimization while maintaining the occupancy grid map. Although EDF also depends on a threedimensional grid, the accuracy of distance and gradient information can be improved by trilinear interpolation [
Path searching is the first step of path generation. The quality of the initial path not only determines the pressure of the optimization part but also affects the quality of the final flight path. Although the path optimization part can optimize the path, it is only a tradeoff between the smoothness, safety, and dynamic feasibility, the overall running direction and the route of the path do not change much, so a stable, safe, and smooth initial path is very important. This paper proposes an initial path searching method, namely guided kinodynamic path searching (GKPS) which uses a guiding path to lead the KPS. The method can not only obtain a stable, smooth, and safe initial path but also sense whether the path passes through the narrow space in advance to help the path optimization part. We will introduce our method in the following three parts: guiding path generation, scene perception, and guided kinodynamic path searching. The first part is used to introduce the generation method of the guiding path. The second part is to show how to perceive the narrow space by using the guiding path. And the third part is responsible for explaining how to use the guiding path to lead the KPS.
The purpose of this part is to generate a geometric guiding path with strong safety in the occupancy grid map. According to the different tasks, guiding path generation is divided into two subparts: improved A* and path pruning. The former is designed to generate an initial geometric path; the latter is designed to generate a reasonable guiding path by pruning the initial geometric path.
Improved A*
At present, many algorithms can be chosen to generate a geometric path, such as A* [
The main search process of improved A* is basically consistent with the standard A* algorithm. P and C in line 3 represent the unexpanded Node queue and the expanded Node set, respectively. Before each expansion, we use the function ReachGoal() and ReachEdge() in line 4 to check whether it reaches the target node or the boundary. Then, the function Expand() in line 6 is designed to expand adjacent nodes into the expendNodes set based on the current node, and the function Checkfeasible() in line 8 is used to determine whether the node in expendNodes is safe and reliable in the map environment. If a node meets the conditions, a structure Node is used to record the node index, parent node, g_{c}, and f_{c} cost, where g_{c} is the expansion cost between the node and its parent node, f_{c} is the total cost from the start point to the target point calculated by the sum of g_{c} and the result of the function Heuristic() in line 16. The function Heuristic() represents the cost between the current node to the target node, which is essential for path searching and its quality directly affects the speed and quality of path generation. Next, we add the Node to the queue P, and then choose the Node with the smallest f_{c} to do a new expansion until reaching the target node or boundary.
Different from the conventional A*, in order to avoid the path through a particularly narrow space, we modify the function Expand() in line 6 by a specific expansion strategy, where the node can only be expanded when no obstacle is in the s (we set s = floor(
Path Pruning
As shown in
The guiding path generated above is not only adopted to guide KPS to generate the initial path but is also used to guide the selection of the optimization mode according to the environmental information around the path. Therefore, after obtaining the path, as shown in
Although the guiding path generated above is safe, it is not suitable as the initial path because it is a geometric path without considering the dynamic feasibility. Therefore, in order to find a highquality initial path, the kinodynamic path searching (KPS) method [
However, as the KPS depends on the discrete control space, this method loses some spatial information. In order to solve the problem, as shown in
Primitives Generation
The differential flatness of the quadrotor UAV system enables us to represent the flight path by three independent 1D timeparameterized polynomial functions, as shown in Equation (2):
The complete solution for the state equation is expressed as:
In node expansion, we uniformly discretize the control space
However, not all generated nodes can be extended. We need to check the candidate node in the following aspects: (1) check whether the node is in the maintained local map; (2) check whether the state speed of the node exceeds the limit; (3) check whether the node moves far enough from the parent node; (4) discretely sample the extended path of the node, and judge whether the distance between the sampling point and the obstacle is greater than the specified threshold one by one. If the candidate node meets all the above conditions at the same time, it will be included in the extended node; otherwise, it will be abandoned.
Actual Cost
The purpose of GKPS is to generate a safe, dynamically feasible, and lowenergy initial path. The safety and dynamic feasibility have been guaranteed through Part A. To ensure the trajectory is optimal in time and control cost, this part designs the cost of a trajectory as:
Under this definition, we can calculate the expansion consumption from the parent node to the current node. Assuming the current node is
Adaptive Heuristic Function
Normally, the heuristic function is used to speed up the searching process. However, in order to reduce the impact of spatial information loss on the path quality, we limit the searching range of the KPS in the vicinity of the guiding path by designing a new heuristic function that can generate a safe and smooth path under the guidance of the guiding path.
Instead of regarding the minimum energy cost
However, due to the nonuniformity of the obstacles in the environment (there are spacious areas and narrow areas), it is difficult for a single fixed heuristic function to be efficient in a cluttered environment. In order to improve the quality of the initial path in the narrow areas, we adopt adaptive values for
Direct Expansion Strategy
GKPS is a method based on discretized control space. Although we add a guiding path to guide, it is still impossible to accurately reach the end position. In addition, when the target point appears in the radar range, if the current node is very close to the target point or the surrounding environment is relatively empty, according to the conventional searching method, it will waste a lot of time for useless expansion and unnecessary replanning. To solve the problem, [
As shown in
Judge whether the guiding path reaches the endpoint. If it reaches the endpoint, start the expansion strategy and carry out the next step (red line). Otherwise, carry out the normal GKPS process (black line);
Analyze the guiding path. If the number of remaining nodes N of the guiding path is not more than three, then it is considered that the road environment is relatively spacious, and directly carries out step 4. Otherwise, carry out the next step;
Carry out the normal GKPS, but after each node expansion, execute step 2;
Direct expansion process, which generates an accurate path from the current node
Although
The value of a Bspline of degree
The purpose of the optimization part is to improve the smoothness and clearance of the path by using a cost function to make a tradeoff between safety, smoothness, and dynamic feasibility. However, there are both spacious and narrow spaces in the actual flight environment, so it is difficult for a fixed cost function to maintain an efficient optimization performance throughout the whole flight. To solve this problem and improve the stability of the optimization module, we propose an adaptive optimization function according to the perception results of the guiding path. We define the overall function as:
As shown in
When we find a narrow space and the position of the narrow space is very close to the current position (
In addition, it should be noted that in order to make the generated flight trajectory safer, we take the distance between the trajectory and the obstacles into account in Equation (11). Although this can make the overall trajectory away from the obstacles, it also lengthens the trajectory. Therefore, the quadrotor has to fly a longer distance within the same time, which unavoidably causes over aggressive motion if the original motion is already near to the physical limits. For this, we use the time adjustment method [
To validate the effectiveness of the proposed realtime path planning method, as shown in
Our method is mainly for the unknown environment, so we need to constantly do replanning within the limited sensing according to certain rules to adapt to changes in the environment. In this paper, the replanning rules are designed as follows: (1) the distance between trajectory and obstacle is less than 0.1 m; (2) the flight distance of the UAV is more than 2 m. When either of the above two conditions are met, a replanning is carried out according to the latest environmental information.
We present tests in simulation and use a simulating tool containing the quadrotor dynamics model, random map generator, and local sensing filter. All simulations run on an Intel Core i98950HK CPU and GeForce P2000 GPU. The trajectory optimization is solved by a general nonlinear optimization solver Nlopt.
Obstacle density is a key parameter to simulate the complexity of the real scene. The larger the obstacle density, the more complex the environment, and the higher the requirements for the performance of the planning algorithms. Under different obstacle densities, we compared our method with FASTER, RET, and PGO. As shown in
As is shown in
As shown in
The resolution of voxel grids is a critical factor for the performance of the proposed method and RET due to the two methods both adopting the occupancy grid map in path searching. Therefore, we tested the effectiveness of the two methods in different grid resolutions. As shown in
As shown in
The proposed method is mainly divided into two parts: path searching and path optimization. The purpose of the path searching is to generate a highquality initial path that meets the dynamic constraints, and path optimization is used to improve the safety and smoothness of the initial path. In order to verify the necessity of the optimization part and its impact on the quality of path generation, we compared and analyzed the path generation results of the proposed method and the method that only uses the path searching part (noopt). The experimental results are shown in
As shown in
In this paper, a realtime path planning method based on geometric path guiding (RETRBG) is proposed, which can provide a stable and efficient planning service for UAV autonomous fast flight in an unknown environment. Based on the work published by [
In order to verify the effectiveness of the proposed method, we made a series of comparative experiments. At first, we compared the proposed method with several stateoftheart methods, namely FASTER, RET, and PGO under five different obstacle densities. The experimental results show that: (1) RET takes the shortest time to do a replanning, and its performance is better when the obstacle density is low. However, due to the fixed optimization function and the tradeoff between smoothness, safety, and dynamic feasibility in it, when the path is generated in a narrow space, the safety of the trajectory is easily weakened, which may cause the optimized path to be close to the obstacle or even pass through the obstacle; the method regards the area not perceived as a collisionfree area, and it only maintains the local map perceived by the field of view, so it is easy to fall into a dead end. The above two reasons are easy to make the planning efficiency and success rate decrease, and the number of replannings gradually increase. The distribution of the experimental results proves the analysis; with the increase of the obstacle density, it can be seen that its performance is unstable, and the quality of the flight trajectory and the success rate gradually decrease. (2) Due to the use of the guiding path, PGO avoids the local minimum problem that other gradientbased methods may suffer, which improves the success rate and the quality of the flight trajectory. However, it still suffers the problem of poor planning quality in narrow areas, and it takes the most time among the three methods to execute a replanning due to the process of the generation and selection of multiple paths. The experimental results also prove this, compared with RET, PGO obtains a high quality and success rate. (3) FASTER obtains highspeed trajectory by optimizing the trajectory in both the freeknown and unknown spaces. It also adopts a mixed integer quadratic program (MIQP) formulation to obtain a more reasonable time allocation of the trajectories. However, it costs much more time than other methods due to the MIQP formulation, and the path generated by this method may be very close to the obstacle due to the hardconstrained optimization. (4) Compared to them, the proposed method performs well in all aspects including average flight time, flight distance, energy cost, and planning success rate under different obstacle densities and grid resolutions. In the energy consumption and success rate, the proposed method outperforms others. In the flight time and distance, the proposed method is second only to FASTER. Meanwhile, according to the distribution of experimental results in different random maps under the same obstacle density, it can be seen that compared with the other methods, the experimental results of our method in different maps are more concentrated, which shows that the performance of the proposed method is more stable in different environments. The results can be explained by the following: At first, the proposed method uses the occupancy grid map to maintain the obstacle information perceived by the sensors in the flight process and to generate the geometric guiding path, which can effectively avoid the planning failure caused by the deadend of the flight. During the generation of the guiding path, we take the motion state into account, which avoids the sharp turn phenomenon caused by the deviation between the guiding direction and the moving direction. Then, aiming to improve the safety and reduce the influence of control state sampling in the KPS, we use the guiding path to guide the searching process. Finally, the proposed method uses the guiding path to perceive the surrounding environment and selects the suitable optimization mode according to the perception results, which significantly improve the quality of the trajectory and the ability to pass through the narrow space.
As shown in
However, although the proposed method has made some improvements in path searching and path optimization, there are still many problems that need to be improved. First of all, although the proposed method improves the quality of the initial path by GKPS that uses the KPS to generate the initial path under the guidance of the guiding path, the path search process will cost more time and the existence of guidance may lead to a relatively slow flight speed. Secondly, the proposed method adopts the time adjustment method proposed by RET. Although the time adjustment method can optimize the part that does not meet the dynamic feasibility, it does not speed up the part with a smaller speed. Moreover, the proposed method depends on the quality of the guide path, but the guide path does not take the factors of dynamic feasibility into account, so if there is an unreasonable guiding path, the quality of the flight trajectory will be poor.
In this paper, a robust and efficient trajectory replanning method based on the guiding path (RETRBG) for unknown environments is proposed. The method is mainly improved in two modules: path searching and path optimization. In the path searching module, a safe geometric guiding path is generated firstly in the occupancy grid map by using the improved A* and path pruning method to perceive the surrounding environment. Furthermore, a guided kinodynamic path searching (GKPS) is proposed that can generate an initial path that meets the safety and dynamic feasibility under the help of the guiding path. In the part of path optimization, an adaptive optimization function with two modes is devised. According to the perception result of the guiding path towards the narrow space, the function adaptively selects the optimization mode to optimize the trajectory. We compared the effectiveness of our method with FASTER, RET, and PGO under different obstacle densities, and compared the effectiveness of our method with RET under different grid resolutions. The experimental results show that the proposed method performs well both in the experiments of different obstacle densities and different grid resolutions. Although the replanning time is slightly higher, the performance of our method is more efficient and more stable in different cluttered environments. We compared and analyzed the path generated by the proposed method and noopt in different environments; the results verify the necessity of optimization and its impact on the quality of path generation.
We also look forward to the next work. First of all, the proposed method depends on the quality of the guiding path, so we will try to figure out how to design a better heuristic function that takes more dynamic feasibility into account to improve the quality of the guiding path in the future. Secondly, although we carried out an extensive effectiveness evaluation of our proposed method, the experiments were only carried out in simulation, so we will make a series of realworld experiments in the next work. Moreover, we will extend our method to adapt to the dynamic environment in the future.
Y.Z. contributed to the conceptualization of the work; Y.Z. proposed the methodology and designed the experiments; Y.Z. and J.D. performed the experiments and analyzed the data; Y.Z. prepared the original draft and all authors contributed to reviewing and editing the manuscript. This project is under the supervision of L.Y., Y.C., and Y.L. All authors have read and agreed to the published version of the manuscript.
This research was funded by National Key Research and Development Project of China (Grant No. 2020YFD1100200).
The authors declare no conflict of interest.
An overview of the proposed realtime replanning method for fast autonomous flight based on geometric path guiding. The red arrow represents the execution procedures of the method, and the black arrow represents that one part provides information for another part.
A detoured and long improved A* path (green path) is pruned. For each node of the path, its visibility to the last waypoint of the pruned path (red path) is checked. The blocking voxels (black voxel) are pushed and appended as new waypoints (orange voxel). Subfigure (
Using the guiding path (blue path) to perceive the narrow space. The green circle represents the point in spacious space, and the red circle represents the point in narrow space.
An illustration of the mechanism of the guided kinodynamic path searching (GKPS) method. Blue curves indicate the motion primitives generated by Equation (3). The green path is the guide path generated by pruning A*. The red curve is the result of the search.
The detailed process of GKPS, when the guiding path reaches the goal, the direct expansion strategy (red arrow) is executed. Otherwise, GKPS executes the normal process (black arrow).
A trajectory (blue line) is represented by a Bspline (k = 4), bounded by the corresponding convex hull of the control points (example convex hulls and segments are shown in gray and red). An illustration of ensuring a convex hull of the Bspline is collisionfree.
Trajectories are optimized by different optimization modes according to the different environments. The guiding path is the path generated in
Samples of the maps generated by different obstacle densities.
Results of the comparisons between the proposed method and FASTER, RET, PGO in different obstacle densities.
The distribution comparisons of the experimental results between the proposed method, FASTER, RET, and PGO in different obstacle densities.
Samples of the maps and trajectories generated by the proposed method (green) and RET (red) in different resolutions.
Results distribution of the comparisons between the proposed method and RET in different resolutions.
Samples of the trajectories generated by the proposed method (green) and noopt (red) in different densities.
Trajectories generated by the proposed method (green), FASTER (pink), RET (red), and PGO (blue) in different obstacle densities.
The comparisons of three planning methods in Replan.
Density (obs./m^{2})  Method  The Total Replan Time (s)  Number of Replan  The Time of Each Replan (s) 

0.2 




Faster  12.199  309.7  0.0394  
RET  0.0622  31.56  0.0019  
PGO  0.2371  42.2  0.0056  
0.25 




Faster  12.496  303.8  0.0411  
RET  0.0938  36.59  0.0025  
PGO  0.3796  55.55  0.0068  
0.3 




Faster  12.899  298.3  0.0432  
RET  0.1339  41.55  0.0032  
PGO  0.5089  66.1  0.0076  
0.35 




Faster  13.777  283.7  0.0486  
RET  0.186  49.23  0.0037  
PGO  0.4809  61.76  0.0077  
0.4 




Faster  13.795  284.4  0.0485  
RET  0.2655  53.54  0.0049  
PGO  0.5888  72.37  0.0081 
The comparisons of two planning method.
Resolution (m)  Method  Flight Time (s)  Flight Distance (m)  Energy (m^{2}/s^{5})  Replan Time (s)  Replan Num.  One  Success Rate (%) 

Repl (s)  
0.1 








RET  17.3451  38.8361  42.5453  0.0622  31.56  0.0019  96  
0.2 








RET  20.3822  42.0943  59.0091  0.1669  38.76  0.0043  88  
0.3 








RET  22.1387  45.8588  73.3257  0.1852  63.6  0.0029  78  
0.4 








RET  31.7207  56.7661  106.724  0.3858  98.91  0.0039  36 
The comparisons of the proposed method and noopt.
Density (obs./m^{2})  Method  Flight Time (s)  Flight Distance (m)  Energy (m^{2}/s^{5})  One Replan Time (s) 

0.2 





noopt  16.5556  38.078  84.2309  0.002  
0.3 





noopt  19.625  41.5584  116.6151  0.0023 