Fast-RRT: A RRT-Based Optimal Path Finding Method

: As a sampling-based pathﬁnding algorithm, Rapidly Exploring Random Trees (RRT) has been widely used in motion planning problems due to the ability to ﬁnd a feasible path quickly. However, the RRT algorithm still has several shortcomings, such as the large variance in the search time, poor performance in narrow channel scenarios, and being far from the optimal path. In this paper, we propose a new RRT-based path ﬁnd algorithm, Fast-RRT, to ﬁnd a near-optimal path quickly. The Fast-RRT algorithm consists of two modules, including Improved RRT and Fast-Optimal. The former is aims to quickly and stably ﬁnd an initial path, and the latter is to merge multiple initial paths to obtain a near-optimal path. Compared with the RRT algorithm, Fast-RRT shows the following improvements: (1) A Fast-Sampling strategy that only samples in the unreached space of the random tree was introduced to improve the search speed and algorithm stability; (2) A Random Steering strategy expansion strategy was proposed to solve the problem of poor performance in narrow channel scenarios; (3) By fusion and adjustment of paths, a near-optimal path can be faster found by Fast-RRT, 20 times faster than the RRT* algorithm. Owing to these merits, our proposed Fast-RRT outperforms RRT and RRT* in both speed and stability during experiments.


Introduction
Scientifically, motion planning refers to finding a continuous feasible path, which starts in the initial state and ends in the target state. Motion planning plays a key role in many applications such as unmanned aerial vehicles (UAV), self-driving cars, and mobile robots, thus greatly emerging the development of motion planning algorithms.
In the past few decades, many motion planning algorithms have been proposed. One of the important categories is graph-based methods such as Dijkstra's algorithm [1] and A* algorithm [2]. They discretize the state space of the motion planning problem into a graph structure and then find a feasible path by using graph search methods. Among them, Dijkstra's algorithm is a breadth-first search algorithm, which can find an optimal path. While the A* algorithm introduces a heuristic function and accelerates the search speed of the D* algorithm. Extensive efforts were made to improve the performance of the A* algorithm. For instance, the Jump Point Search algorithm can speed up the A* algorithm by order of magnitude [3]. Liu et al. [4] further extended the Jump Point Search algorithm from 2D to a 3D environment. By virtue of introducing dynamic constraints, the Hybrid A* [5] can generate smooth paths to satisfy the robots. The graph-based algorithm is complete and resolution optimal, which means that if a feasible path exists, the graph-based method can find an optimal path; otherwise, it will return failure. However, graph-based methods do not perform well in large-scale problems (e.g., industrial robotic arms) because the search space obtained by the graph-based discretization of the state space is too large.
The sampling-based planning method is another important type of planning algorithm. Instead of discretizing the state space, sampling-based planning is to construct a graph or tree by randomly sampling in the state space. Compared with graph-based planning algorithms, sampling-based planning algorithms perform better in large-scale problems.
Recently, learning-based methods have been widely applied in motion planning research. Neural RRT* [12] uses deep learning to learn a distribution probability for sampling. RL-RRT [24] explores deep reinforcement learning policy as a local planner and uses distance function that learns by deep learning to bias tree-growth towards the target area. In [25], a learning algorithm combining Inverse Reinforcement learning and RRT* is used to learn the RRT*'s cost function. The DL-P-RRT* algorithm applies deep learning to the RRT* algorithm using the virtual artificial potential field to learn the artificial potential field function. Learning-based methods perform well in some environments, but they may suffer from bad generalization ability in new environments.
In this paper, we propose an RRT-based planning method for optimal motion planning, called Fast-RRT. Compared with RRT*, Fast-RRT accelerates the search speed of the optimal path by order of magnitude. To be specific, Fast-RRT divides the search for the optimal path into two steps. The Improved-RRT algorithm is first used to find a feasible path quickly, and then the Fast-Optimal algorithm fuses multiple paths to obtain an optimal path.
For Improved RRT, two important improvements were made compared to the RRT algorithm, such as Fast Sampling and Random Steering. Fast Sampling improves the search speed of the RRT algorithm by refusing to sample in the explored area and solves the problem of the large variance in the search time of the RRT algorithm. Random Steering randomly chooses the direction to expand when the expansion fails, which solves the problem of poor performance of the RRT algorithm in narrow channel scenarios. By introducing these two improvements, the Improved-RRT algorithm can quickly find a feasible solution. Besides, Fast-Optimal can obtain the optimal path by virtue of fusing multiple paths and has a faster convergence speed as compared to RRT*. After a new feasible path is searched by Improved-RRT, Fast-Optimal merges it with the current optimal path to obtain a better path. The fusion operation of Fast-Optimal also consists of two steps, including path fusion and path fine-tuning. Path fusion can fuse multiple initial paths to obtain a better path, while path fine-tuning can quickly adjust the fusion path, which speeds up the search for the optimal path. Due to these advantaged characteristics, the search speed of Fast-RRT for finding a near-optimal path is 20 times faster than the RRT* algorithm. Therefore, our Fast-RRT algorithm exhibits great potential in practical motion planning applications.
The rest of this paper is organized as follows. The formal definition of the motion planning problem and the necessary background are presented in Section 2. In Section 3, the proposed Fast-RRT method in this paper is be defined. Section 4 shows the simulation and evaluation of our experimental results. At last, a brief summary and outlook is be presented in Section 5.

Background
In this section, we present the related backgrounds of this paper. The formal definition of motion planning problems is first introduced, followed by discussing the related algorithms such as RRT and RRT*.

Problem Definition
Two issues of motion planning, including the feasible solution and the optimal problems, is be discussed and defined in a similar way to [26].
Let X ∈ R n be the state space of the planning problem, where n ∈ N is the dimension of the motion planning problem. X obs ∈ X is the obstacle space, which cannot be reached due to the collision with obstacles. X f ree = X/X obs represents the free state space.
x init ∈ X f ree and x goal ∈ X f ree are the initial state and the goal state, respectively, and X goal = x ∈ X x − x goal < r is the goal region. A feasible path is defined as a path σ : [0 : T] → X f ree such that σ(0) = x init and σ(T) ∈ X goal .
The feasible problem of motion planning is to find a feasible path if one exists; otherwise, it should report failure. Problem 1 defines the feasibility problem of path planning.

Problem 1 (Feasible Path Planning)
Given a state space of planning problem X ∈ R n , a free space X f ree , an initial state x init and a goal region X goal ∈ X f ree , find a path σ : [0 : T] → X f ree such that σ(0) = x init and σ(T) ∈ X goal , if one exists. If no feasible path exists, then report failure.
Path cost is an important concept in motion planning. By defining the cost function, a feasible path is assigned a non-negative cost. The optimal problem of motion planning is to find a path with minimal cost. Problem 2 formalizes the optimal problem of motion planning.
Problem 2 (Optimal Path Planning) Given a state space X ∈ R n , a free state space X f ree , an initial state x init and a goal region X goal ∈ X f ree , if a solution to problem1 exists, find a path σ : [0 : T] → X f ree such that σ(0) = x init , and σ(T) ∈ X goal and c(σ * ) = min σ∈∑ f ree c(σ).
The path-find algorithm based on sampling is a time-consuming process refereed to find an optimal path. Therefore, a near-optimal path is usually obtained through a certain number of iterations. The near-optimal problem refers to finding a path where the difference of cost and the optimal path is less than the threshold. Problem 3 formalizes the near-optimal problem of motion planning.
Problem 3 (Near-Optimal Path Planning) Given a state space X ∈ R n , a free state space X f ree , an initial state x init and a goal region X goal ∈ X f ree , if a solution to problem 1 exists, and σ * is the solution to Problem 2, find a path σ : [0 : T] → X f ree such that σ(0) = x init , and σ(T) ∈ X goal and c(σ) < c(σ * ) * (1 + threshold).

RRT and RRT*
The RRT algorithm is the basis of the proposed Fast-RRT, while RRT* is an important method to find the approximate optimal path. Therefore, the RRT and RRT* algorithms are introduced in Algorithm 1 and Algorithm 2, respectively.
The RRT is a single query search algorithm based on sampling, which can quickly find a feasible path. In the initialization phase, RRT builds a tree with the initial state x init as the node. In each iteration, RRT first randomly samples a state x rand in state space X f ree and select the nearest vertex x near . Then, the RRT algorithm will generate x new through the steering function, as shown in Figure 1. If the edge of {x near , x new } is obstacle-free, then x new will be added to the set of nodes, and {x near , x new } will be added to the set of edges. If x new is located in the target area X goal , the search is successful, and this result will be returned.
(); Figure 1. Schematic illustration of the expansion steps of the RRT algorithm. The RRT algorithm performs random sampling in the state space to obtain , and the vertex closest to is calculated as . Then, the random tree of RRT extends a certain distance from to to obtain .

Method
In this section, we introduce our Fast Rapidly-exploring Random Trees (Fast-RRT) algorithm. The framework of the proposed method is presented in Section 3.1, which consists of two modules, including Improved-RRT and Fast-Optimal. The details are introduced in Sections 3.2-3.3.

Framework of Fast-RRT
The detailed process of Fast RRT is shown in Algorithm 3, which includes two steps. The first step, called Improved-RRT, aims to find a feasible path, and the second step, called Fast-Optimal, aims to merge this feasible path with the current optimal path in Figure 1. Schematic illustration of the expansion steps of the RRT algorithm. The RRT algorithm performs random sampling in the state space to obtain q rand , and the vertex closest to q rand is calculated as q near . Then, the random tree of RRT extends a certain distance from q near to q rand to obtain q new .
The path found by RRT may be far away from the optimal path. RRT* overcomes this problem by introducing a rewire step. If the edge {x near , x new } is obstacle-free, then nodes with a distance less than r around x new will be calculated to determine the optimal parent node of x new . In addition, RRT* not only adds x new to the tree, but also considers it as a replacement parent node for existing neighboring nodes. Therefore, as the sampling times approach infinity, RRT* continuously adjusts the random tree and finally finds an optimal path. However, the RRT* is time-consuming and thus is not suitable for applications that need to find an optimal path quickly.

Algorithm 1. RRT
Input: x init , x goal and Map T.init()

Method
In this section, we introduce our Fast Rapidly-exploring Random Trees (Fast-RRT) algorithm. The framework of the proposed method is presented in Section 3.1, which consists of two modules, including Improved-RRT and Fast-Optimal. The details are introduced in Sections 3.2 and 3.3.

Framework of Fast-RRT
The detailed process of Fast RRT is shown in Algorithm 3, which includes two steps. The first step, called Improved-RRT, aims to find a feasible path, and the second step, called Fast-Optimal, aims to merge this feasible path with the current optimal path in order to obtain a near-optimal path.
For Improved RRT, two important improvements related to the sampling and the extended strategies were made compared to the baseline RRT algorithm, such as Fast Sampling and Random Steering. These strategies are introduced in Sections 3.2.1 and 3.2.2, respectively.
Fast-Optimal also consists of two parts, path fusion (Section 3.3.1) and path fine-tuning (Section 3.3.2). Path fusion can fuse multiple initial paths to obtain a better path, while path fine-tuning can quickly adjust the fusion path, which speeds up the search for the optimal path. Thus, our Fast-RRT can quickly find a near-optimal path by fusing multiple paths. By contrast, the RRT* adjusts the tree to find the near-optimal path, which is a time-consuming process. The Fast-Optimal algorithm is introduced in Section 3.3.

Algorithm 3. Fast-RRT Algorithm
Input: x init , x goal and Map Output: If T init Isnot None then T optimal ← FastOptimal T optimal , T init

Improved RRT
As an improved pathfinding algorithm, Improved-RRT introduces a fast sampling strategy and a random expansion strategy. The details of the proposed algorithm are shown in Algorithm 4. Fast Sampling improves the search speed of the RRT algorithm by refusing to sample in the explored area and solves the problem of the large variance in the search time of the RRT algorithm. Random Steering randomly chooses the direction to expand when the expansion fails, which solves the problem of poor performance of the RRT algorithm in narrow channel scenarios. By introducing these two improvements, the Improved-RRT algorithm can quickly find a feasible solution.

Algorithm 4. Improved RRT
Input: x init , X goal and Map Output: The basic RRT algorithm randomly samples a state in the entire state space and guides the tree to expand to it. When a new node is expanded by the RRT algorithm, the RRT calculates the distance d between the node and the target point. If d is less than the set threshold r, the algorithm then finds the target point until the end of searching; otherwise, the search continues. Therefore, every time a new node is expanded, the RRT algorithm detects the area with the node as the center and the distance from the center less than r. We defined this area as the explored area. The RRT algorithm can finally realize the exploration of all areas through continuous sampling and growth.
However, this sampling strategy may produce many invalid expansions and result in a large variance in the search time. An example of invalid expansion is shown in Figure 2. The blue areas in the figure indicate the explored area of three nodes. x rand is the state sampled by the RRT algorithm, x new is the state generated by steering operation, and then {x near , x new } is added to the edge set. However, this expansion is invalid because the red exploration area of the newly generated node completely falls within the original exploration area and fails to expand the newly explored area.
To address the above issue, we propose a sampling algorithm called Fast-Sampling. By refusing to sample in the explored area, Fast-Sampling can avoid a large number of invalid growths. Figure 3 indicates the principle of Fast-Sampling, where a new vertex x new is added to the tree, its surrounding area x aruond = x ∈ X x − x goal < r is set as the explored area. As shown in Figure 3, the sampled state x rand1 is located in the explored area, the corresponding guided expansion is then invalid. The Fast-Sampling samples another random state x rand2 , which guides the random tree T expand to an unexplored area. Compared with RRT, Fast-Sampling speeds up the exploration speed and reduces memory usage. Algorithm 5 presents the detail of Fast-Sampling. then { , } is added to the edge set. However, this expansion is invalid because the red exploration area of the newly generated node completely falls within the original exploration area and fails to expand the newly explored area. is the randomly sampled point, _ is the nearest neighbor of , and is the expansion point. Unfortunately, this step of expansion is invalid because the random tree is not brought close to the target point or expanded to a new area.
To address the above issue, we propose a sampling algorithm called Fast-Sampling. By refusing to sample in the explored area, Fast-Sampling can avoid a large number of invalid growths. Figure 3 indicates the principle of Fast-Sampling, where a new vertex is added to the tree, its surrounding area = { ∈ − < } is set as the explored area. As shown in Figure 3, the sampled state is located in the explored area, the corresponding guided expansion is then invalid. The Fast-Sampling samples another random state , which guides the random tree expand to an unexplored area. Compared with RRT, Fast-Sampling speeds up the exploration speed and reduces memory usage. Algorithm 5 presents the detail of Fast-Sampling.  x rand is the randomly sampled point, x _near is the nearest neighbor of x rand , and x new is the expansion point. Unfortunately, this step of expansion is invalid because the random tree is not brought close to the target point or expanded to a new area.
red exploration area of the newly generated node completely falls within the original exploration area and fails to expand the newly explored area. is the randomly sampled point, _ is the nearest neighbor of , and is the expansion point. Unfortunately, this step of expansion is invalid because the random tree is not brought close to the target point or expanded to a new area.
To address the above issue, we propose a sampling algorithm called Fast-Sampling. By refusing to sample in the explored area, Fast-Sampling can avoid a large number of invalid growths. Figure 3 indicates the principle of Fast-Sampling, where a new vertex is added to the tree, its surrounding area = { ∈ − < } is set as the explored area. As shown in Figure 3, the sampled state is located in the explored area, the corresponding guided expansion is then invalid. The Fast-Sampling samples another random state , which guides the random tree expand to an unexplored area. Compared with RRT, Fast-Sampling speeds up the exploration speed and reduces memory usage. Algorithm 5 presents the detail of Fast-Sampling.  Since the first sampling point x rand1 falls in the explored area, it is regarded as an invalid sampling. After re-sampling, the second sampling point x rand2 is obtained. x rand2 bootstrap tree expansion to obtain x new , thus avoiding invalid expansion.

Algorithm 5. Fast Sampling
Input: x rand Let x rand ← Uni f ormSample(); While x rand ∈ X explored do x rand ← Uni f ormSample();

Random Steering
In addition to Fast-Sampling, the Random Steering strategy was also proposed to solve the problem of poor performance of the RRT algorithm in narrow channel scenarios. The inferior performance of the RRT algorithm can be attributed to the invalid expansion introduced in Section 3.2.1, and a large number of failed expansions caused by the narrow Appl. Sci. 2021, 11, 11777 8 of 18 passages are shown in Figure 4. In order to pass through the narrow passages, the tree T should expand toward the right state space. While the expansion guided by x rand collides with an obstacle, resulting in the failed expansions.

Random Steering
In addition to Fast-Sampling, the Random Steering strategy was also proposed to solve the problem of poor performance of the RRT algorithm in narrow channel scenarios. The inferior performance of the RRT algorithm can be attributed to the invalid expansion introduced in Section 3.2.1, and a large number of failed expansions caused by the narrow passages are shown in Figure 4. In order to pass through the narrow passages, the tree should expand toward the right state space. While the expansion guided by collides with an obstacle, resulting in the failed expansions. To avoid this problem, we adjusted the expansion algorithm and named it as Fast-Expand. As shown in Figure 5, if the expansion guided by fails, Fast-Expand will randomly generate a direction for expansion. If edge ( , ) is obstacle-free, then they will be added to the tree; otherwise, another random state will be sampled by Fast-RRT. Fast-Expand enables the random tree of the Fast-RRT algorithm to pass through the narrow passage quickly. The details of the Fast-Expand algorithm are shown in Algorithm 6. To avoid this problem, we adjusted the expansion algorithm and named it as Fast-Expand. As shown in Figure 5, if the expansion guided by x rand1 fails, Fast-Expand will randomly generate a direction θ for expansion. If edge (x near , x new1 ) is obstacle-free, then they will be added to the tree; otherwise, another random state will be sampled by Fast-RRT. Fast-Expand enables the random tree of the Fast-RRT algorithm to pass through the narrow passage quickly. The details of the Fast-Expand algorithm are shown in Algorithm 6.
ppl. Sci. 2021, 11, x FOR PEER REVIEW 9 of 19 Figure 5. Schematic illustration of random expansion strategy. When the tree T expands towards , it randomly samples a direction θ to expand because it collides with an obstacle. In this way, the Fast-Expand samples another random tree and provides a successful pass through the narrow channel after getting in the next sampling.

Algorithm 6 Fast Expanding
Input: , , if ( , ) then Return Else Figure 5. Schematic illustration of random expansion strategy. When the tree T expands towards x rand1 , it randomly samples a direction θ to expand because it collides with an obstacle. In this way, the Fast-Expand samples another random tree and provides a successful pass through the narrow channel after getting x rand2 in the next sampling.

Fast-Optimal
Except for Improved RRT, Fast-Optimal is another important part of our Fast RRT algorithm. As an optimal pathfinding algorithm, Fast-Optimal is an asymptotically optimal algorithm that obtains a better path by fusing the new initial path sought with the current optimal paths. Compared with the RRT* algorithm, Fast-Optimal accelerates the convergence speed of the algorithm. The Fast-Optimal algorithm consists of two steps involving path fusion and path fine-tuning. Path fusion can fuse multiple initial paths to obtain a better path, while path fine-tuning can quickly adjust the fusion path, which speeds up the search for the optimal path. They are introduced in Sections 3.3.1 and 3.3.2, respectively.

Path Fusion
Path fusion refers to intercepting a part of each of two paths to combine them into a better path. In this case, the evaluation index of the path is the path length. Therefore, path fusion is introduced to fuse multiple initial paths to obtain a better path with a shorter length than the two paths. Figure 6 illustrates the algorithm flow chart of path fusion. In each iteration, a random non-optimal path P new is obtained by the Imporved RRT algorithm. Due to the high efficiency of Fast-RRT, P new can be quickly obtained. Then P new is fused with the current optimal path P optimal to obtain a more optimal path, thus being the new P optimal . By continuously generating new paths and fusing them with the current optimal path, the path fusion algorithm will result in the final optimal path. The process of path fusion is shown in Figure 7. It consists of the following steps first step is to find the intersection, and the second step is to select the sub-path. In the step, the intersection points of paths ℎ and ℎ is be calculated and use divide the path into multiple sub-paths. Given that the nodes of the two paths do completely overlap, if the distance between and is less than the threshold , they are considered to be overlapped and set the midpoint overlap point. In the second step, we select sub-paths from ℎ and ℎ The process of path fusion is shown in Figure 7. It consists of the following steps: the first step is to find the intersection, and the second step is to select the sub-path. In the first step, the intersection points of paths path new and path best is be calculated and used to divide the path into multiple sub-paths. Given that the nodes of the two paths do not completely overlap, if the distance between point 1 and point 2 is less than the set threshold r, they are considered to be overlapped and set the midpoint point mid as overlap point. In the second step, we select sub-paths from path new and path obest to connect each intersection. As shown in Figure 6, the sub-path of P new connecting q start and q near is shorter than the sub-path of path best connecting q start and q new . Thus, the sub-path of path best is used to connect q start and q near . Similarly, the sub-path of path best is used to connect q near and q goal because it is shorter than that of path new . In this way, the two paths can be combined into a better path than the individual paths. By continuously fusing the current optimal path path best with the newly generated random path, the algorithm quickly obtains a near-optimal path. The details of the path fusion algorithm are shown in Algorithm 7. The process of path fusion is shown in Figure 7. It consists of the following steps: the first step is to find the intersection, and the second step is to select the sub-path. In the first step, the intersection points of paths ℎ and ℎ is be calculated and used to divide the path into multiple sub-paths. Given that the nodes of the two paths do not completely overlap, if the distance between and is less than the set threshold , they are considered to be overlapped and set the midpoint as overlap point. In the second step, we select sub-paths from ℎ and ℎ to connect each intersection. As shown in Figure 6, the sub-path of connecting and is shorter than the sub-path of ℎ connecting and . Thus, the sub-path of ℎ is used to connect and . Similarly, the sub-path of ℎ is used to connect and because it is shorter than that of ℎ . In this way, the two paths can be combined into a better path than the individual paths. By continuously fusing the current optimal path ℎ with the newly generated random path, the algorithm quickly obtains a near-optimal path. The details of the path fusion algorithm are shown in Algorithm 7.

Path Fine-Tuning
In order to speed up the path optimization, we also propose a path fine-tuning strategy that obtains a better path by simply adjusting the path obtained by the path-fusion algorithm. The process of the path fine-tuning is shown in Figure 8. The path composed of green points is obtained by the path-fusion algorithm, in which q near1 , q near2 and q near3 are the coincident points of P new and P optimal , respectively. straight line. The connection between and collides with the obstacle; thus, the original path is saved. In contrast, the connection between and does not collide with obstacles, so we can use straight to connect these two points because the direct connection is shorter than the original polyline connection. Therefore, the path fine-tuning algorithm can obtain a better path with few turns, which is more convenient for the robots to execute. The details of fast fine-tuning are shown in Algorithm 8. 1, 2, 3 are the intersection points of path fusion. Since the connection between 1 and 2 collides with obstacles, the original path is retained. In contrast, the connection between 2 and 3 is unobstructed so that the original path can be replaced by a new straight path. During the process of path fine-tuning, we try to connect each intersection with a straight line. The connection between q near1 and q near2 collides with the obstacle; thus, the original path is saved. In contrast, the connection between q near2 and q near3 does not collide with obstacles, so we can use straight to connect these two points because the direct connection is shorter than the original polyline connection. Therefore, the path fine-tuning algorithm can obtain a better path with few turns, which is more convenient for the robots to execute. The details of fast fine-tuning are shown in Algorithm 8.

Simulation and Result
In order to evaluate the performance of the proposed algorithm, a series of experiments are performed. As mentioned above, the Fast-RRT includes two modules, Improved RRT and Fast-Optimal. The Improved RRT algorithm can quickly find the initial path, while the Fast-Optimal algorithm can obtain a near-optimal path by combining the initial paths. Therefore, we first compared Improved-RRT with RRT to prove its efficiency in finding the initial solution. Then the comparison between Fast-Optimal and RRT is shown to prove the efficiency of the algorithm in finding the near-optimal solution.
Specifically, three experiments are designed to evaluate the efficiency of the proposed algorithm. The first experiment is to find a feasible path in a complex environment. The simulation environment is shown in Figure 9, which contains multiple obstacles of different shapes. The second experiment is to find a feasible path in a narrow passage scene, which is difficult for RRT. The simulation environment containing multiple narrow passages is shown in Figure 10. The third experiment is to find a near-optimal path by using the same environment as the first experiment. All experiments were run on a desktop computer, configured with Intel Core i7-7700k processor, 16 GB RAM, Windows 10, and Matlab R202. optimal solution.
Specifically, three experiments are designed to evaluate the efficiency of the proposed algorithm. The first experiment is to find a feasible path in a complex environment. The simulation environment is shown in Figure 9, which contains multiple obstacles of different shapes. The second experiment is to find a feasible path in a narrow passage scene, which is difficult for RRT. The simulation environment containing multiple narrow passages is shown in Figure 10. The third experiment is to find a near-optimal path by using the same environment as the first experiment. All experiments were run on a desktop computer, configured with Intel Core i7-7700k processor, 16 GB RAM, Windows 10, and Matlab R202.

Find Initial Path
This experiment is to test the algorithm's ability to find an initial solution, which refers to finding an obstacle-free path from to . The RRT algorithm is used as a benchmark algorithm to verify the efficiency of the Improved-RRT algorithm. The simulation environment used for this experiment is shown in Figure 8. The length and width of the environment are 1200 and 900, respectively. is the distance from the initial state to the goal state , and its value is set to 1000. Different step sizes affect the complexity of the problem. In our case, we designed multiple sets of experiments and set the step sizes to 10, 20, 30, 40, and 50, respectively. As the step size decreases, the searching number required to find a feasible solution increase along with the complexity of the problem.
The results of the Fast-RRT algorithm and RRT algorithm in a test are shown in Figure 11. The red points are the random sampling state of the algorithm, the green line is the random tree generated by the algorithm, and the blue line is the feasible path obtained by the search. Obviously, the sampling points of the RRT algorithm are

Find Initial Path
This experiment is to test the algorithm's ability to find an initial solution, which refers to finding an obstacle-free path from x start to x goal . The RRT algorithm is used as a benchmark algorithm to verify the efficiency of the Improved-RRT algorithm. The simulation environment used for this experiment is shown in Figure 8. The length and width of the environment are 1200 and 900, respectively. d goal is the distance from the initial state x start to the goal state x goal , and its value is set to 1000. Different step sizes affect the complexity of the problem. In our case, we designed multiple sets of experiments and set the step sizes to 10, 20, 30, 40, and 50, respectively. As the step size decreases, the searching number required to find a feasible solution increase along with the complexity of the problem.
The results of the Fast-RRT algorithm and RRT algorithm in a test are shown in Figure 11. The red points are the random sampling state of the algorithm, the green line is the random tree generated by the algorithm, and the blue line is the feasible path obtained by the search. Obviously, the sampling points of the RRT algorithm are randomly distributed at any position in the state space. By contrast, the sampling points of the Fast-RRT algorithm are sparsely distributed around the random tree and densely distributed in the area far from the random tree. Fast-RRT avoids sampling in the area that the random tree has reached, which is beneficial for guiding the random tree to expand to the unknown area. At the same time, compared with the RRT algorithm, Fast-RRT uses fewer nodes to cover the space due to the presence of the abundant random trees of the RRT algorithm, resulting in more invalid extensions. Finally, Fast-RRT has multiple random edges near the boundary of the obstacle. Although it increases memory consumption, the Fast-RRT facilitates the random tree to bypass the obstacles to reach the target point quickly. (a) (b) Figure 11. Comparison of the operation results of (a) the improved RRT algorithm and (b) the RRT algorithm. The sampling points of the Improved RRT algorithm are more densely distributed in the area where the random tree has not reached, and its random tree is more efficient to achieve the state space. As for the RRT, the sampling points are evenly distributed throughout the space. Compared with Improved RRT, it uses more nodes to achieve coverage of the state space.
Finally, the efficiency of the algorithm was evaluated by search time and memory consumption. Specifically, the number of nodes in the random tree is used to evaluate memory consumption. The average and variance are used as two indicators for the characterization of the search time. The average value of the search time represents the average performance of the algorithm, while the variance indicates its stability, which is both very important for practical applications (e.g., robots). In our case, RRT and Fast-RRT were run 100 times, and then the running time and the number of nodes of the random tree were recorded. The running time of the two algorithms and the average and variance of the number of nodes were further calculated. Figure 12 shows the results of the running time. Compared with the RRT algorithm, the average and variance of the search time of the Fast-RRT algorithm are significantly reduced. When the step size is set to 50, the average and variance of the search time are 0.016 s and 0.009 s, respectively. By contrast, for the RRT algorithm, the average and variance of the search time are 0.043 s and 0.043 s, respectively. Correspondingly, the average search time and the variance within the Fast-RRT algorithm are only 37.2% and 20.9% of the RRT algorithm, respectively. As the step size decreases, the complexity of the problem increases along with the gap between them. When the step size is set to 10, the average and variance of the search time of the Fast-RRT algorithm are only 23.5% and 8.3% of the RRT algorithm. Figure 11. Comparison of the operation results of (a) the improved RRT algorithm and (b) the RRT algorithm. The sampling points of the Improved RRT algorithm are more densely distributed in the area where the random tree has not reached, and its random tree is more efficient to achieve the state space. As for the RRT, the sampling points are evenly distributed throughout the space. Compared with Improved RRT, it uses more nodes to achieve coverage of the state space.
Finally, the efficiency of the algorithm was evaluated by search time and memory consumption. Specifically, the number of nodes in the random tree is used to evaluate memory consumption. The average and variance are used as two indicators for the characterization of the search time. The average value of the search time represents the average performance of the algorithm, while the variance indicates its stability, which is both very important for practical applications (e.g., robots). In our case, RRT and Fast-RRT were run 100 times, and then the running time and the number of nodes of the random tree were recorded. The running time of the two algorithms and the average and variance of the number of nodes were further calculated. Figure 12 shows the results of the running time. Compared with the RRT algorithm, the average and variance of the search time of the Fast-RRT algorithm are significantly reduced. When the step size is set to 50, the average and variance of the search time are 0.016 s and 0.009 s, respectively. By contrast, for the RRT algorithm, the average and variance of the search time are 0.043 s and 0.043 s, respectively. Correspondingly, the average search time and the variance within the Fast-RRT algorithm are only 37.2% and 20.9% of the RRT algorithm, respectively. As the step size decreases, the complexity of the problem increases along with the gap between them. When the step size is set to 10, the average and variance of the search time of the Fast-RRT algorithm are only 23.5% and 8.3% of the RRT algorithm.

Narrow Passages Scene
The ability of our proposed Fast-RRT algorithm is also evaluated to find a feasible path in an environment with narrow passages. The algorithm needs to find an obstacle-free path from x start to x goal . Due to the requirement of a large number of sampling, the RRT algorithm does not perform well in making the random tree pass through these narrow channels. The environment, in this case, is shown in Figure 9. The length and width of the environment are 1200 and 900, respectively. There are three narrow passages in the environment, and the width of the channel is set to 80. x start is the start state and x goal is the goal state. d goal is the distance from x start to x goal , which is set to 1000.
The average and variance of the search time are used to evaluate the efficiency of the Improved RRT algorithm, and the RRT algorithm is used as the benchmark algorithm. Several groups of experiments were performed with the step sizes set to 10, 20, 30, 40, and 50, respectively. For each experiment, we ran the RRT algorithm and the Improved-RRT algorithm 100 times, respectively. Then the average and variance of the search time of these algorithms were calculated, and obtained results are shown in Figure 14.
Several groups of experiments were performed with the step sizes set to 10,20,30,40, and 50, respectively. For each experiment, we ran the RRT algorithm and the Improved-RRT algorithm 100 times, respectively. Then the average and variance of the search time of these algorithms were calculated, and obtained results are shown in Figure 14.
The results show that the average and variance of the search time within the Improved-RRT algorithm are significantly smaller than those of the RRT algorithm. When the step size is set to 10,20,30,40, and 50, the average search time of the Improved-RRT algorithm is 0.567 s, 0.146 s, 0.076 s, 0.047 s, and 0.035 s, respectively. The values are 15.3%, 15.1%, 17.1%, 17.0%, 16.8% of the RRT algorithm, respectively. Moreover, the variance of search time of the Improved-RRT algorithm is 0.480 s, 0.101 s, 0.056 s, 0.050 s, and 0.032 s, which are 16.5%, 15.9%, 10.5%, 11.5%, 11.7% of the RRT algorithm, respectively. Therefore, the Improved-RRT algorithm can find a feasible path faster than the RRT algorithm and has better stability. Compared with the results of Section 3.1, the difference between the average and variance of the search time of these two algorithms is further increased, indicating that the random expansion strategy proposed in this paper has a significant effect on the pathfinding problem of narrow passage environment.

Find Near-Optimal Path
The algorithm's ability to find a near-optimal path is also measured. The algorithm needs to find an obstacle-free path from to , whose length differs from the optimal path's length is less than threshold . The environment used in this experiment is the same as Section 3.1, the environment is shown in Figure 8, the length of the The results show that the average and variance of the search time within the Improved-RRT algorithm are significantly smaller than those of the RRT algorithm. When the step size is set to 10, 20, 30, 40, and 50, the average search time of the Improved-RRT algorithm is 0.567 s, 0.146 s, 0.076 s, 0.047 s, and 0.035 s, respectively. The values are 15.3%, 15.1%, 17.1%, 17.0%, 16.8% of the RRT algorithm, respectively. Moreover, the variance of search time of the Improved-RRT algorithm is 0.480 s, 0.101 s, 0.056 s, 0.050 s, and 0.032 s, which are 16.5%, 15.9%, 10.5%, 11.5%, 11.7% of the RRT algorithm, respectively. Therefore, the Improved-RRT algorithm can find a feasible path faster than the RRT algorithm and has better stability. Compared with the results of Section 3.1, the difference between the average and variance of the search time of these two algorithms is further increased, indicating that the random expansion strategy proposed in this paper has a significant effect on the pathfinding problem of narrow passage environment.

Find Near-Optimal Path
The algorithm's ability to find a near-optimal path is also measured. The algorithm needs to find an obstacle-free path from x start to x goal , whose length differs from the optimal path's length is less than threshold r. The environment used in this experiment is the same as Section 3.1, the environment is shown in Figure 8, the length of the environment is 1200, the width of the environment is 900, and d goal is 1000. For this environment, the length of the optimal path is 1035.
Similarly, the average and variance of the search time are applied to evaluate the efficiency of the algorithm. The differences between the length of the near-optimal path and the length of the optimal path are set to 5%, 10%, 15%, 20%, and 25%, respectively. At a threshold of 5%, we need to find a path with a length of less than 1087, as the length of the optimal path is 1035. The RRT* is used as a control algorithm to verify the efficiency of the proposed algorithm. The step size is also set to 30 as the same as the Fast-RRT algorithm. In each experiment, we ran the Fast-RRT algorithm and the RRT* algorithm 100 times and recorded the running time. Finally, the average and variance of the search time of these two algorithms were calculated. The final results are presented in Figure 15. environment is 1200, the width of the environment is 900, and is 1000. For this environment, the length of the optimal path is 1035.
Similarly, the average and variance of the search time are applied to evaluate the efficiency of the algorithm. The differences between the length of the near-optimal path and the length of the optimal path are set to 5%, 10%, 15%, 20%, and 25%, respectively. At a threshold of 5%, we need to find a path with a length of less than 1087, as the length of the optimal path is 1035. The RRT* is used as a control algorithm to verify the efficiency of the proposed algorithm. The step size is also set to 30 as the same as the Fast-RRT algorithm. In each experiment, we ran the Fast-RRT algorithm and the RRT* algorithm 100 times and recorded the running time. Finally, the average and variance of the search time of these two algorithms were calculated. The final results are presented in Figure 15. Figure 15. Comparison of the time required for Fast-RRT and RRT* algorithms to find an approximate optimal path when the gap between the approximate optimal path and the theoretical optimal path is different thresholds. It can be seen that compared with the RRT* algorithm, the Fast-RRT algorithm has an order of magnitude advantage in search time.
As expected, the average and variance of the search time within the Fast-RRT algorithm is smaller than those of the RRT* algorithm. At a threshold of 5%, the RRT* algorithm shows an average and variance of the search time of 13.34 s and 5.75 s, respectively. Impressively, the average and variance of the search time of the Fast-RRT algorithm are 0.322 s and 0.207 s, respectively, which is only 2.4% and 3.6% of the RRT* algorithm. When the threshold is set up to 10%, 15%, 20%, and 25%, the average search time of the Fast-RRT algorithm is 2.9%, 3.4%, 4.0%, and 6.1% of the RRT* algorithm, respectively. Therefore, the search speed of the Fast-RRT algorithm is 20 times faster than the RRT* algorithm. Moreover, the variance of search time within the Fast-RRT algorithm is 3.4%, 2.4%, 2.0%, and 2.1% of the RRT* algorithm, demonstrating enhanced stability.
Thanks to the superior performance mentioned above, our Fast-RRT possesses great potential in the design and navigation of robots. After using sensors such as lasers and depth cameras to build an environment map, our Fast-RRT algorithm can be used to find a feasible path from the starting point to the target point. Moreover, due to the advantages of fast search speed and small search time variance, our Fast-RRT can be further applied to UAV navigation and other path search tasks that require high real-time performance. Figure 15. Comparison of the time required for Fast-RRT and RRT* algorithms to find an approximate optimal path when the gap between the approximate optimal path and the theoretical optimal path is different thresholds. It can be seen that compared with the RRT* algorithm, the Fast-RRT algorithm has an order of magnitude advantage in search time.
As expected, the average and variance of the search time within the Fast-RRT algorithm is smaller than those of the RRT* algorithm. At a threshold of 5%, the RRT* algorithm shows an average and variance of the search time of 13.34 s and 5.75 s, respectively. Impressively, the average and variance of the search time of the Fast-RRT algorithm are 0.322 s and 0.207 s, respectively, which is only 2.4% and 3.6% of the RRT* algorithm. When the threshold is set up to 10%, 15%, 20%, and 25%, the average search time of the Fast-RRT algorithm is 2.9%, 3.4%, 4.0%, and 6.1% of the RRT* algorithm, respectively. Therefore, the search speed of the Fast-RRT algorithm is 20 times faster than the RRT* algorithm. Moreover, the variance of search time within the Fast-RRT algorithm is 3.4%, 2.4%, 2.0%, and 2.1% of the RRT* algorithm, demonstrating enhanced stability.
Thanks to the superior performance mentioned above, our Fast-RRT possesses great potential in the design and navigation of robots. After using sensors such as lasers and depth cameras to build an environment map, our Fast-RRT algorithm can be used to find a feasible path from the starting point to the target point. Moreover, due to the advantages of fast search speed and small search time variance, our Fast-RRT can be further applied to UAV navigation and other path search tasks that require high real-time performance.

Conclusions
In summary, we proposed a new RRT-based path planning algorithm, Fast-RRT, to improve the speed and stability of finding the initial path. Therefore, two improvements, such as sampling in the unexplored space and random expansion, were performed. Furthermore, a new algorithm for finding a new near-optimal path was further proposed to obtain a near-optimal path by combining and adjusting multiple feasible paths.
Compared with the RRT and RRT* algorithms, the proposed that Fast-RRT possesses remarkable advantages in speed and stability. For instance, within the Fast-RRT algorithm, the average search time and its variance to find a feasible path significantly reduced compared to the RRT algorithm. At the same time, the search speed of Fast-RRT for finding a near-optimal path is 20 times faster than the RRT* algorithm. Therefore, our Fast-RRT algorithm exhibits great potential in practical motion planning applications. To further improve the performance of our algorithm, the combination of the Fast-RRT algorithm and other algorithms, such as Bidirectional RRT, Kinodynamic RRT*, and Information RRT*, will also be investigated in the future. At the same time, the application scenarios of our Fast RRT algorithm will also be extended from two-dimensional to multi-dimensional, as well as in actual motion planning tasks in future work.

Institutional Review Board Statement: Not applicable.
Informed Consent Statement: Not applicable.

Data Availability Statement:
The data presented in this paper are available on request from the first author.

Conflicts of Interest:
The authors declare no conflict of interest.