# Biased Sampling Potentially Guided Intelligent Bidirectional RRT∗ Algorithm for UAV Path Planning in 3D Environment

Authors: Xiaojing Wu; Lei Xu; Ran Zhen; Xueli Wu
Journal: Mathematical Problems in Engineering
 (2019)

---

## Abstract

During the last decade, Rapidly-exploring Random Tree star (RRT∗) algorithm based on sampling has been widely used in the field of unmanned aerial vehicle (UAV) path planning for its probabilistically complete and asymptotically optimal characteristics. However, the convergence rate of RRT∗ as well as B-RRT∗ and IB-RRT∗ is slow for these algorithms perform pure exploration. To overcome the weaknesses above, Biased Sampling Potentially Guided Intelligent Bidirectional RRT∗ (BPIB-RRT∗) algorithm is proposed in this paper, which combines the bidirectional artificial potential field method with the idea of bidirectional biased sampling. The proposed algorithm flexibly adjusts the sampling space, greatly reduces the invalid spatial sampling, and improves the convergence rate. Moreover, the deeply theoretical analysis of the proposed BPIB-RRT∗ algorithm is given regarding its probabilistic completeness, asymptotic optimality, and computational complexity. Finally, compared to the latest UAV path planning algorithms, simulation comparisons are demonstrated to show the superiority of our proposed BPIB-RRT∗ algorithm.


---

## Body

## 1. Introduction

Nowadays, Unmanned Aerial Vehicles (UAV) are being widely applied to military or civilian fields, e.g., mining surveillance [1], search and rescue operations [2], image recognition [3], cropland assessment [4], and so on. The increasing demands have brought in focus on reducing dependencies on human operators. Path planning plays an important role in enhancing the ability of autonomous and intelligent flight of UAV. In general, given the start and target locations, the goal of path planning is defined as planning a feasible path while satisfying certain criteria, such as distance, smoothness, and obstacle avoidance. Various methods for solving the path planning problem have been raised, such as Rapidly-exploring Random Tree (RRT) [5], Rapidly-exploring Random Tree star (RRT∗) [6], ant colony optimization [7], particle swarm optimization [8], simulated annealing method [9], and neural network method [10].In recent years, RRT and RRT∗ algorithms have been widely applied to solve the problem of path planning, such as [11, 12], as they performed very well without the information of obstacle configuration in high-dimensional spaces. Different from the traditional RRT algorithm, RRT∗ algorithm obtains the local optimal path by updating the old parent node constantly, which makes the global path tend to be optimal. Moreover, RRT∗ can provide a probabilistically complete and asymptotically optimal path. However, RRT and RRT∗ algorithms perform pure exploration, which can cause it to have very slow rate of convergence in highly dimensional environment. Moreover, the randomly sampled characteristics of RRT and RRT∗ algorithms make the generated path not smooth enough.In order to improve the convergence rate, variant improvements on RRT and RRT∗ algorithms have been extensively explored [13–15]. In [13], the D∗ Lite Rapidly-exploring Random Tree star (DL-RRT∗) algorithm has been proposed for path replanning in dynamic radioactive environments, which uses the expansion strength of grid search strategy from D∗ Lite to quickly find a high-quality initial path to accelerate the convergence rate in RRT∗. However, the path planning method used in [13] was based on single-tree version, so that there is still limitation in the convergence rate. In the work [14], the authors proposed the Bidirectional RRT (B-RRT) to build two trees rooted at the start and goal configurations, respectively. Reference [15] has extended B-RRT to exploit the possibilities of dynamically dividing up the whole map into four submaps and growing the random tree in every submap using Graphic Processing Unit (GPU) threads. In the other words, Quad-RRT [15] computed four random trees instead of two trees built by bidirectional approaches. The bidirectional tree version of RRT∗ known as Bidirectional RRT∗ (B-RRT∗) has been presented in [16], which employed a greedy connect heuristic for the connection of two directional trees. However, above-mentioned multiple tree versions of RRT and RRT∗ performed pure space exploration without using biased sample to guide random trees. Meanwhile, aforementioned algorithms are mainly effective for path planning in 2D environment.It is well known that a feasible path of UAV should be planned in 3D environment, for 3D environment is more applicable to low-altitude and terrain-following flight. The 3D environment path planning problem has been widely investigated in [17–20]. A bidirectional spline-RRT∗ path planning algorithm has been proposed in [17] for fixed-wing UAVs that can find paths in highly constrained planning environments. The proposed algorithm can improve the smoothness of the generated path, but does nothing to accelerate the convergence rate. The Intelligent Bidirectional-RRT∗ (IB-RRT∗) algorithm has been proposed in [18] for complex cluttered environments, which introduces an intelligent sample insertion heuristic for fast convergence to the optimal path solution by using uniform sampling heuristics. However, aforementioned variants of the RRT∗ algorithm still perform pure exploration without guide for sampling, which should make the path planning algorithm to have slow convergence rate and suffer in highly cluttered environments. Reference [19] presented the Potential Function Based-RRT∗ (P-RRT∗) algorithm for better convergence properties by introducing a Random Gradient Descent (RGD) heuristic. The RGD heuristic guides the randomly sampled states incrementally downhill in the direction of decreasing potential so that the convergence rate is improved. Then, the new bidirectional potential gradient heuristics was presented in [20] to potentially guide two rapidly-exploring random trees towards each other in the bidirectional sampling-based motion planning, and hence the two algorithms called Potentially Guided Bidirectional RRT∗ (PB-RRT∗) and Potentially Guided Intelligent Bidirectional RRT∗ (PIB-RRT∗) greatly improved the sampling efficiency, resulting in faster convergence rate and excellent performance in highly cluttered environment. However, the biased sampling technique was not introduced in the PIB-RRT∗ and PB-RRT∗ algorithms [20]. Moreover, the fixed attractive pole may give misleading information that points the random trees in the wrong direction. By introducing the idea of target gravity, the tGSRT algorithm was presented in [5]. The speed of the initial path search was increased, and the path was optimized by the genetic algorithm (GA) and smooth processing. Nevertheless, there are some limitations in the sampling procedure, and the path generated with jags is inevitable.In this paper, we present the concept of Biased Sampling Potentially Guided Intelligent Bidirectional RRT∗ (BPIB-RRT∗) for path planning of UAV, guiding two random trees towards each other by incorporating the proposed bidirectional biased sampling and bidirectional potential field heuristics. This paper presents the bidirectional biased sampling heuristic to provide a higher deviation for sampling in the right direction based on the relative position relation between the target trees with the sampling. Also, dynamic attractive pole based on the growth trend of the target trees as the bidirectional potential field heuristic is introduced into BI-RRT∗, which pulls the random trees towards each other fast. The idea of the BPIB-RRT∗ algorithm reduces the invalid nodes generated by random sampling in state space and enhances the guidance, which can improve the sampling efficiency of the algorithm. Moreover, the generated path is smoothed by B-spline to meet the path constraint conditions of UAV, which improves the flight ability. The algorithm is novel and can greatly improve the convergence rate and smoothness of the planning path for 3D environment.The remainder of the paper is divided into the following sections: Section2 provides the explanation of symbols which are used to describe the problem. Section 3 gives the related works and backgrounds of the BPIB-RRT∗ algorithm. Section 4 explains the BPIB-RRT∗ algorithm proposed in this paper. Section 5 presents the theoretical analyses of the BPIB-RRT∗ algorithm. Section 6 gives simulation results regarding the comparison of data and figures. Finally, conclusions are drawn in Section 7.


## 2. Problem Definition

This section will describe the notations that are used in this paper. The state space is represented by the setS⊂R3. Furthermore, the given configuration space is divided into obstacle regions and obstacle-free regions which are denoted by Sobs⊂S and Sfree=S\Sobs, respectively. Let Ta=Va,Ea⊂Sfree and Tb=Vb,Eb⊂Sfree represent two random trees which are initialized by Sinit∈Sfree and Sgoal∈Sfree as their root vertex, respectively. Va, Vb, Ea, and Eb denote the vertices and edges of two trees Ta and Tb. The closed ball of radius r>0 centred at s′ is defined as Os′,r≔s∈S∣s−s′≤r. Let k denote the number of iterations. Let ηN be the set of feasible path produced by an algorithm where ηN∈Sfree and N is the number of a complete path vertexes. The problem of UAV path planning is to find a feasible path ηN from the initial state to the target location in the least amount of time possible in the 3D environment. A complete feasible path is denoted by η0,1⟶η0=Sinit,η1=Sgoal⊂Sfree.Let the cost functionJ denote the cumulative sum of cost (in terms of Euclidean distance) of coordinate xn,yn,zn to coordinate xn+1,yn+1,zn+1, and this calculation process continues till n⟶N. The cost function J can be defined as(1)J=∑n=1NLn,where(2)Ln=xn+1−xn2+yn+1−yn2+zn+1−zn21/2.


## 3. Related Works

In this section, various RRT∗ algorithms are described briefly.


### 3.1. RRT∗ Algorithm

The RRT∗ [6] algorithm is explained in detail in Algorithm 1. The RRT∗ algorithm initializes the random tree T=V,E with initial location Sinit to start the iterative process (Line 1). The random sample qrand∈S is given by Sample () (Line 3). Then, the near vertices or the nearest vertex of qrand is obtained by NearVertices() or NearestVertex() (Line 4–7). The populated set Qnear is then sorted by the GetSortedList() procedure (Line 8). ChooseBestParent() returns qmin∈Qnear with minimum cost from the random tree T to qrand in obstacle-free space (Line 9). Once the best parent vertex is located, qmin is inserted into the random tree T=V,E by making qrand its child and the rewiring procedure is executed (Line 10–13). Algorithm iteration continues till k⟶K or the goal location is explored.Algorithm 1:
RRT
∗
S
init.
(1)
V ⟵ {Sinit}; E ⟵ Ø; T ⟵ (V, E);
(2)
for
k ⟵ 0 toKdo
(3)
qrand ⟵ Sample(k)
(4)
Qnear ⟵ NearVertices(qrand, T)
(5)
ifQnear = Ø then
(6)
Qnear ⟵ NearestVertex(qrand, T)
(7)
end
(8)
Ls ⟵ GetSortedList(qrand, Qnear)
(9)
qmin ⟵ ChooseBestParent(Ls)
(10)
ifqmin ≠ Ø then
(11)
T ⟵ InsertVertex(qrand, qmin, T)
(12)
T ⟵ RewireVertices(qrand, Ls, E)
(13)
end
(14)
end
(15)
return
T = (V, E)


### 3.2. B-RRT∗ Algorithm

Algorithm2 is given in this section, which outlines the implementation of Bidirectional RRT∗ (B-RRT∗) in detail.Algorithm 2:B-RRT∗ (Sinit, Sgoal).
(1)
V ⟵ {Sinit, Sgoal}; E ⟵ Ø; Ta ⟵ (Sinit, E); Tb ⟵ (Sgoal, E);
(2)
η
best ⟵ ∞
(3)
for
k ⟵ 0 toKdo
(4)
qrand ⟵ Sample(k)
(5)
qnearest ⟵ NearestVertex(qrand, Ta)
(6)
qnew ⟵ Extend(qnearest, qrand)
(7)
Qnear ⟵ NearVertices(qnew, Ta)
(8)
Ls ⟵ GetSortedList(qnew, Qnear)
(9)
qmin ⟵ ChooseBestParent(Ls)
(10)
ifqmin ≠ Ø then
(11)
T ⟵ InsertVertex(qnew, qmin, Ta)
(12)
T ⟵ RewriteVertices(qnew, Ls, E)
(13)
end
(14)
qconn ⟵ NearestVertex(qnew, Tb)
(15)
ηnew ⟵ Connect(qnew, qconn, Tb)
(16)
ifηnew ≠ Ø &&C(ηnew) < C(ηbest) then
(17)
ηbest ⟵ ηnew
(18)
end
(19)
SwapTrees(Ta, Tb)
(20)
end
(21)
return
T
a, Tb = (V, E)The B-RRT∗ [14] algorithm initializes two random trees Ta and Tb in the same manner as they were in the RRT∗ (Line 1). The B-RRT∗ [14] algorithm starts with sampling from the obstacle-free space Sfree, and the same process is acted on this random node qrand (Line 4–9). Then, qrand is inserted into the random tree Ta by InsertVertex() (Line 10–13), and the nearest node qconn from tree Tb is found (Line 14). If Connect() heuristic successfully connects random trees Ta and Tb, a feasible path ηnew from Sinit to Sgoal is generated (Line 15). The cost of ηnew is compared with the previously computed path cost ηbest to confirm an optimal path with less cost (Line 16–18). Then, random trees Ta and Tb are swapped, and the procedures mentioned above are executed on the other tree Tb again.


### 3.3. IB-RRT∗ Algorithm

This section explains the implementation of Intelligent Bidirectional RRT∗ [18] (IB-RRT∗). The IB-RRT∗ algorithm is proposed, which can use the GetBestTreeParent() heuristic to obtain the best parent from two trees Ta or Tb.The IB-RRT∗ [18] algorithm initializes the random tree Ta and Tb as well as B-RRT∗. The random vertex qrand is produced by Sample(). Then, the near neighbour vertices Qneara and Qnearb are generated by NeghboringVertices() from Ta and Tb, respectively. Both neighbouring vertex sets Qneara and Qnearb are sorted by the GetSortedList() procedure. The GetBestTreeParent() heuristic returns the best parent Pmin from Ta or Tb. Then, the qrand is inserted in the best selected tree, and the rewiring procedure is executed.


### 3.4. PIB-RRT∗ Algorithm

The PIB-RRT∗ [20] algorithm incorporates the Artificial Potential Fields (APF) into IB-RRT∗ by using the BPG() heuristic. The BPG() heuristic returns the potentially guided bidirectional randomly sampled state qpb. The BPG() heuristic is shown in Algorithm 3 and explained below.Algorithm 3:
BPG(qrand, k).
(1)
q
pb ⟵ qrand
(2)
if
k mod 2 = 0 then
(3)
fork ⟵ 0 tondo
(4)
Fatt = BPGgoal (Sgoal, qpb)
(5)
dnearest∗ ⟵ NearestObstacleSearch(Sobs, qpb)
(6)
if dnearest∗ ≤ dobs∗ then
(7)
returnqpb
(8)
else
(9)
qpb ⟵ qpb + ε Fatt/Fatt
(10)
else
(11)
fork ⟵ 0 tondo
(12)
Fatt = BPGinitt(Sinit, qpb)
(13)
dnearest∗ ⟵ NearestObstacleSearch(Sobs, qpb)
(14)
if dnearest∗ ≤ dobs∗ then
(15)
returnqpb
(16)
else
(17)
qpb ⟵ qpb + ε Fatt/Fatt
(18)
return
q
pbAlgorithm 4:BPIB-RRT∗(Sinit, Sgoal).
(1)
V
a ⟵ {Sinit}; Vb ⟵ {Sgoal}; Ea ⟵ Ø; Eb ⟵ Ø;
(2)
T
a ⟵ (Va, Ea); Tb ⟵ (Vb, Eb);
(3)
q
prev1 = Sinit
(4)
q
prev2 = Sgoal
(5)
η
best ⟵ ∞
(6)
Connection ⟵ True
(7)
for
k ⟵ 0 toKdo
(8)
qrand ⟵ Sample(k)
(9)
qbia ⟵ Bi-bias(qrand, k, qprev1, qprev2)
(10)
qop ⟵ BPF(qbia, k, qprev1, qprev2)
(11)
{Qneara, Qnearb} ⟵ NeighbouringVertices(qop, Ta, Tb)
(12)
if Qneara = Ø and Qnearb = Ø then
(13)
{Qneara, Qnearb} ⟵ NearestVertex(qop, Ta, Tb)
(14)
Connection ⟵ False
(15)
La ⟵ GetsortedList(qop, Qneara)
(16)
Lb ⟵ GetsortedList(qop, Qnearb)
(17)
{qparent, flag, ηbest} ⟵ GetBestTreeParent(La, Lb, Connection)
(18)
if (flag) then
(19)
Ta ⟵ InsertVertex(qop, qparent, Ta)
(20)
Ta ⟵ RewriteVertices(qop, La, Ea)
(21)
qprev1 ⟵ qop
(22)
else
(23)
Tb ⟵ InsertVertex(qop, qparent, Tb)
(24)
Tb ⟵ RewriteVertices(qop, Lb, Eb)
(25)
qprev2 ⟵ qop
(26)
E ⟵ Ea ∪ Eb
(25)
V ⟵ Va ∪ Vb
(26)
return {Ta, Tb} = (V, E)
(27)
η∗ ⟵ B-spline(Ta, Tb)
Step 1: Sampling optimization: randomly sampled stateqrand is obtained by the sampling process in the given state space. Then, the biased node qbia based on the growth trend of the target random tree is obtained by using the Bi-bias() heuristic. After being guided by the BPF() heuristic, randomly sampled state qrand turns into biased potentially guided bidirectional randomly sampled state qop such that qrand⟶qop, where qop∈Sfree. Bi-bias() and BPF() heuristics have been explained later in this section.
Step 2: Insert vertex: the GetBestTreeParent() heuristic finds the best parent for the stateqop obtained in Step 1 from random trees Ta or Tb and InsertVertex() insert the qop into the best selected tree. Step 1 and Step 2 continue till k⟶K.
Step 3: Generate the path. As iteration,k, goes from 0 to K, a feasible solution path η0,1 from initial to goal configuration is found. Then, the final path is smoothed by B-spline. Finally, BPIB-RRT∗ can provide a smooth feasible path. The pseudocode of BPIB-RRT∗ is given in Algorithm 4.BPG(): if the iteration number k is even, the potentially guided bidirectional randomly sampled state qpb is fed into the BPGgoal() heuristic, which is used to calculate the attractive force Fatt working on qpb. Comparing dnearest∗ with a constant dobs∗, qpb will be guided downhill in the direction of decreasing potential towards the goal region. Similarly, for odd iteration k, qpb is passed to the BPGinit() heuristic and attractive force Fatt is calculated. Then, qpb is pulled towards Sinit by the BPGinit() heuristic.


## 3.1. RRT∗ Algorithm

The RRT∗ [6] algorithm is explained in detail in Algorithm 1. The RRT∗ algorithm initializes the random tree T=V,E with initial location Sinit to start the iterative process (Line 1). The random sample qrand∈S is given by Sample () (Line 3). Then, the near vertices or the nearest vertex of qrand is obtained by NearVertices() or NearestVertex() (Line 4–7). The populated set Qnear is then sorted by the GetSortedList() procedure (Line 8). ChooseBestParent() returns qmin∈Qnear with minimum cost from the random tree T to qrand in obstacle-free space (Line 9). Once the best parent vertex is located, qmin is inserted into the random tree T=V,E by making qrand its child and the rewiring procedure is executed (Line 10–13). Algorithm iteration continues till k⟶K or the goal location is explored.Algorithm 1:
RRT
∗
S
init.
(1)
V ⟵ {Sinit}; E ⟵ Ø; T ⟵ (V, E);
(2)
for
k ⟵ 0 toKdo
(3)
qrand ⟵ Sample(k)
(4)
Qnear ⟵ NearVertices(qrand, T)
(5)
ifQnear = Ø then
(6)
Qnear ⟵ NearestVertex(qrand, T)
(7)
end
(8)
Ls ⟵ GetSortedList(qrand, Qnear)
(9)
qmin ⟵ ChooseBestParent(Ls)
(10)
ifqmin ≠ Ø then
(11)
T ⟵ InsertVertex(qrand, qmin, T)
(12)
T ⟵ RewireVertices(qrand, Ls, E)
(13)
end
(14)
end
(15)
return
T = (V, E)


## 3.2. B-RRT∗ Algorithm

Algorithm2 is given in this section, which outlines the implementation of Bidirectional RRT∗ (B-RRT∗) in detail.Algorithm 2:B-RRT∗ (Sinit, Sgoal).
(1)
V ⟵ {Sinit, Sgoal}; E ⟵ Ø; Ta ⟵ (Sinit, E); Tb ⟵ (Sgoal, E);
(2)
η
best ⟵ ∞
(3)
for
k ⟵ 0 toKdo
(4)
qrand ⟵ Sample(k)
(5)
qnearest ⟵ NearestVertex(qrand, Ta)
(6)
qnew ⟵ Extend(qnearest, qrand)
(7)
Qnear ⟵ NearVertices(qnew, Ta)
(8)
Ls ⟵ GetSortedList(qnew, Qnear)
(9)
qmin ⟵ ChooseBestParent(Ls)
(10)
ifqmin ≠ Ø then
(11)
T ⟵ InsertVertex(qnew, qmin, Ta)
(12)
T ⟵ RewriteVertices(qnew, Ls, E)
(13)
end
(14)
qconn ⟵ NearestVertex(qnew, Tb)
(15)
ηnew ⟵ Connect(qnew, qconn, Tb)
(16)
ifηnew ≠ Ø &&C(ηnew) < C(ηbest) then
(17)
ηbest ⟵ ηnew
(18)
end
(19)
SwapTrees(Ta, Tb)
(20)
end
(21)
return
T
a, Tb = (V, E)The B-RRT∗ [14] algorithm initializes two random trees Ta and Tb in the same manner as they were in the RRT∗ (Line 1). The B-RRT∗ [14] algorithm starts with sampling from the obstacle-free space Sfree, and the same process is acted on this random node qrand (Line 4–9). Then, qrand is inserted into the random tree Ta by InsertVertex() (Line 10–13), and the nearest node qconn from tree Tb is found (Line 14). If Connect() heuristic successfully connects random trees Ta and Tb, a feasible path ηnew from Sinit to Sgoal is generated (Line 15). The cost of ηnew is compared with the previously computed path cost ηbest to confirm an optimal path with less cost (Line 16–18). Then, random trees Ta and Tb are swapped, and the procedures mentioned above are executed on the other tree Tb again.


## 3.3. IB-RRT∗ Algorithm

This section explains the implementation of Intelligent Bidirectional RRT∗ [18] (IB-RRT∗). The IB-RRT∗ algorithm is proposed, which can use the GetBestTreeParent() heuristic to obtain the best parent from two trees Ta or Tb.The IB-RRT∗ [18] algorithm initializes the random tree Ta and Tb as well as B-RRT∗. The random vertex qrand is produced by Sample(). Then, the near neighbour vertices Qneara and Qnearb are generated by NeghboringVertices() from Ta and Tb, respectively. Both neighbouring vertex sets Qneara and Qnearb are sorted by the GetSortedList() procedure. The GetBestTreeParent() heuristic returns the best parent Pmin from Ta or Tb. Then, the qrand is inserted in the best selected tree, and the rewiring procedure is executed.


## 3.4. PIB-RRT∗ Algorithm

The PIB-RRT∗ [20] algorithm incorporates the Artificial Potential Fields (APF) into IB-RRT∗ by using the BPG() heuristic. The BPG() heuristic returns the potentially guided bidirectional randomly sampled state qpb. The BPG() heuristic is shown in Algorithm 3 and explained below.Algorithm 3:
BPG(qrand, k).
(1)
q
pb ⟵ qrand
(2)
if
k mod 2 = 0 then
(3)
fork ⟵ 0 tondo
(4)
Fatt = BPGgoal (Sgoal, qpb)
(5)
dnearest∗ ⟵ NearestObstacleSearch(Sobs, qpb)
(6)
if dnearest∗ ≤ dobs∗ then
(7)
returnqpb
(8)
else
(9)
qpb ⟵ qpb + ε Fatt/Fatt
(10)
else
(11)
fork ⟵ 0 tondo
(12)
Fatt = BPGinitt(Sinit, qpb)
(13)
dnearest∗ ⟵ NearestObstacleSearch(Sobs, qpb)
(14)
if dnearest∗ ≤ dobs∗ then
(15)
returnqpb
(16)
else
(17)
qpb ⟵ qpb + ε Fatt/Fatt
(18)
return
q
pbAlgorithm 4:BPIB-RRT∗(Sinit, Sgoal).
(1)
V
a ⟵ {Sinit}; Vb ⟵ {Sgoal}; Ea ⟵ Ø; Eb ⟵ Ø;
(2)
T
a ⟵ (Va, Ea); Tb ⟵ (Vb, Eb);
(3)
q
prev1 = Sinit
(4)
q
prev2 = Sgoal
(5)
η
best ⟵ ∞
(6)
Connection ⟵ True
(7)
for
k ⟵ 0 toKdo
(8)
qrand ⟵ Sample(k)
(9)
qbia ⟵ Bi-bias(qrand, k, qprev1, qprev2)
(10)
qop ⟵ BPF(qbia, k, qprev1, qprev2)
(11)
{Qneara, Qnearb} ⟵ NeighbouringVertices(qop, Ta, Tb)
(12)
if Qneara = Ø and Qnearb = Ø then
(13)
{Qneara, Qnearb} ⟵ NearestVertex(qop, Ta, Tb)
(14)
Connection ⟵ False
(15)
La ⟵ GetsortedList(qop, Qneara)
(16)
Lb ⟵ GetsortedList(qop, Qnearb)
(17)
{qparent, flag, ηbest} ⟵ GetBestTreeParent(La, Lb, Connection)
(18)
if (flag) then
(19)
Ta ⟵ InsertVertex(qop, qparent, Ta)
(20)
Ta ⟵ RewriteVertices(qop, La, Ea)
(21)
qprev1 ⟵ qop
(22)
else
(23)
Tb ⟵ InsertVertex(qop, qparent, Tb)
(24)
Tb ⟵ RewriteVertices(qop, Lb, Eb)
(25)
qprev2 ⟵ qop
(26)
E ⟵ Ea ∪ Eb
(25)
V ⟵ Va ∪ Vb
(26)
return {Ta, Tb} = (V, E)
(27)
η∗ ⟵ B-spline(Ta, Tb)
Step 1: Sampling optimization: randomly sampled stateqrand is obtained by the sampling process in the given state space. Then, the biased node qbia based on the growth trend of the target random tree is obtained by using the Bi-bias() heuristic. After being guided by the BPF() heuristic, randomly sampled state qrand turns into biased potentially guided bidirectional randomly sampled state qop such that qrand⟶qop, where qop∈Sfree. Bi-bias() and BPF() heuristics have been explained later in this section.
Step 2: Insert vertex: the GetBestTreeParent() heuristic finds the best parent for the stateqop obtained in Step 1 from random trees Ta or Tb and InsertVertex() insert the qop into the best selected tree. Step 1 and Step 2 continue till k⟶K.
Step 3: Generate the path. As iteration,k, goes from 0 to K, a feasible solution path η0,1 from initial to goal configuration is found. Then, the final path is smoothed by B-spline. Finally, BPIB-RRT∗ can provide a smooth feasible path. The pseudocode of BPIB-RRT∗ is given in Algorithm 4.BPG(): if the iteration number k is even, the potentially guided bidirectional randomly sampled state qpb is fed into the BPGgoal() heuristic, which is used to calculate the attractive force Fatt working on qpb. Comparing dnearest∗ with a constant dobs∗, qpb will be guided downhill in the direction of decreasing potential towards the goal region. Similarly, for odd iteration k, qpb is passed to the BPGinit() heuristic and attractive force Fatt is calculated. Then, qpb is pulled towards Sinit by the BPGinit() heuristic.


## 4. Biased Sampling Potentially Guided Intelligent Bidirectional RRT∗

In this section, we present our proposed algorithm, Biased Sampling Potentially Guided Intelligent Bidirectional RRT∗ (BPIB-RRT∗).


### 4.1. Algorithm Process

The detailed procedure of the proposed algorithm is explained as mentioned below.From aforementioned descriptions, the flow chart of the BPIB-RRT∗ algorithm can be presented in Figure 1.Figure 1
Flow chart of the BPIB-RRT∗ algorithm.Extra procedures employed by BPIB-RRT∗ are explained below, whereas the rest are exactly the same as they are for IB-RRT∗. Therefore, only the Bi-bias() heuristic and BPF() heuristic are explained in detail.


### 4.2.Bi-bias()

The random sampling process of the RRT∗ algorithm based on full space causes its convergence to the optimal solution very slow. Hence, the Bi-bias() heuristic based on the growth trend of the target tree is introduced into IB-RRT∗.qrand is generated by Sample() firstly and then passed to Bi-bias(). If the iteration count k is even, then the randomly sampled state qrand is pulled toward the random tree Tb. The bias direction can be obtained by analysing the relative position between qprev2 and qrand, where qprev2 is the new vertex of the target tree Tb. The Bi-bias() heuristic can provide a higher deviation in the right direction before generating the bias node qbia. If the iteration count k is odd, the random tree Ta attracts the randomly sampled state qrand. Similarly, the relative position between qprev1 and qrand is calculated to provide a higher deviation for qrand, where qprev1 is the new vertex of the target tree Ta.The bias formulas are shown below:(3)xbia=xrand+rand∗xprev∗−xrand,xrand≤xprev∗,xbia=xrand−rand∗xrand,xrand>xprev∗,(4)ybia=yrand+rand∗yprev∗−yrand,yrand≤yprev∗,ybia=yrand−rand∗yrand,yrand>yprev∗,(5)zbia=zrand+rand∗zprev∗−zrand,zrand≤zprev∗,zbia=zrand−rand∗zrand,zrand>zprev∗,where xrand,yrand,zrand denotes the random state coordinate, xprev1,yprev1,zprev1 and xprev2,yprev2,zprev2 represent the new vertex of target trees Ta and Tb, respectively. rand denotes the random function between 0 and 1. Bias node qbia=xbia,ybia,zbia is calculated by the aforementioned formulas.It can be seen from Figure2, there are eight relative positions between qprev∗ and qrand in state space. Take the spatial position shown in Figure 2 as an example, part of the Bi-bias() heuristic is shown in Algorithm 5. The calculation of other spatial relationships for qrand and qprev∗ is the same. By feeding back the vertex position of the two random trees, Bi-bias() updates the spatial position of random state qrand dynamically, improves the effectiveness of the sampling process, and furthermore reduces the number of iterations of the algorithm.Figure 2
The instance of spatial position.Algorithm 5:
Bi-bias(q
rand, k, qprev1, qprev2).
(1)
if
k
mod 2 = 0 then
(2)
if (xprev2 < xrand) then
(3)
if (yprev2 < yrand) then
(4)
if (zprev2 > zrand) then
(5)
xbia = xrand − rand∗ xrand
(6)
ybia = yrand − rand∗ yrand
(7)
zbia = zrand + rand∗ (zprev2 − zrand)
(8)
else
(9)
if (xprevl < xrand) then
(10)
if (yprevl < yrand) then
(11)
if (zprev1 > zrand) then
(12)
xbia = xrand − rand∗ xrand
(13)
ybia = xrand − rand∗ yrand
(14)
zbia = zrand − rand∗ (zprev1 − zrand)
(15)
returnqbia = (xbia, ybia,zbia)
(16)
end


### 4.3. BPF()

Artificial potential field (APF) has a good application in obstacle avoidance technology of UAV. The APF algorithm constructs attractive field and repulsive force field based on the target point and obstacles in the planning space, respectively. The goal region pulls the robot towards it, and the obstacles repel the robot away from the obstacle configuration spaceSobs. The force exerted to the robot is equal to the resultant force of attractive force and repulsive force at this location [21]. APF is simple to operate and has a fast convergence speed. It could get stuck in the local minima for it performs pure exploitation, and it is greedy. APF is smooth in mathematic which means the tangent and the velocity are always continuous [22] and improves UAV stability. The equation of attraction field is discussed below:(6)UAttq=12kAttρ2q,Sgoal,where UAttq is the attraction field, kAtt is the attraction field coefficient, q is the current position of the UAV, Sgoal is the goal location, and ρq,Sgoal is the distance between the current point and the goal location. The attraction field force FAttq is generated by taking a negative gradient(7)FAttq=−∇UAttq=kAttρq,Sgoal.The repulsive fieldURepq is generated by all obstacles Sobs∈S.(8)URepq=∑i=1IURepiq,where URepiq is the repulsion field generated by the ith obstacle at q.(9)URepiq=12λ1ρq,qobsi−1ρ0i2,ρq,qobsi<ρ0i,0,ρq,qobsi≥ρ0i,where λ is the repulsive field coefficient, ρ0i is the effective radius of the ith repulsive field, and ρq,qobsi is the distance between the ith obstacle and the current position q. Correspondingly, let FRepiq sign the repulsion force,(10)FRepiq=FRep1iq,ρq,qobsi<ρ0i,0,ρq,qobsi≥ρ0i,where FRep1iq=λ1/ρq,qobsi−1/ρ0i1/ρ2q,qobsi∇ρq,qobsi.The resultant force on the UAV at pointq is Fq.(11)Fq=FAttq+∑i=1IFRepiq.Incorporating APF into IB-RRT∗ by using the Bidirectional Potential Field heuristic (BPF()), pseudocode of the BPF() heuristic is presented in Algorithm 6. The current vertices of two random trees qprev1 and qprev2 are first fed into the BPF() algorithm. When the number of iterations k is even, the new vertex qprev2 of the Tb is treated as the target point of the bias point qbia, and the attraction force based on the qprev2 is calculated. dnearesti∈Dnearest is the distance between the bias point qbia and the ith obstacle. ρ0i denotes the radius of repulsion field. If dnearesti is less than ρ0i, qbia will be guided toward the direction of the resultant force Fq based on the repulsion force and the attraction force, otherwise only attraction force FAttq works on qbia. Then, the optimal random node qop is obtained. If the iteration count k is odd, the BPF() heuristic treats the new vertex qprev1 of the Ta as the attraction field of the bias point qbia. Similarly, qbia is directed downhill in direction of the decreasing potential by computing the resultant force, and the optimal random node qop is produced.Algorithm 6:BPF (qbia, k, qprev1, qprev2).
(1)
q
op ⟵ qbia
(2)
ifkmod 2 = 0 then
(3)
FAtt = computeAttForce(qprev2, qop)
(4)
Dnearest = ρ(Sobs, qop)
(5)
if dnearesti ≤ ρ0i then
(6)
FRep = computeRepForce(qop, Sobs)
(7)
qop = qop + FRep/FRep+FAtt/FAtt
(8)
else
(9)
qop = qop + FAtt/FAtt
(10)
end
(11)
else
(12)
FAtt = computeAttForece(qprev1, qop)
(13)
Dnearest = ρ(Sobs, qop)
(14)
if dnearesti ≤ ρ0i then
(15)
FRep = computeRepForce(qop, Sobs)
(16)
qop = qop + FRep/FRep+FAtt/FAtt
(17)
else
(18)
qop = qop + FAtt/FAtt
(19)
end
(20)
end
(21)
return
q
op


### 4.4. Path Generation Using B-Spline

The curvature of path generated by the RRT∗ algorithm is noncontinuous. Therefore, the initial path cannot meet the flight constraints of UAV. To overcome this shortcoming, B-spline is introduced into BPIB-RRT∗ due to its high flexibility and arbitrary continuity. The path vertices generated by the BPIB-RRT∗ algorithm are fed into B-spline as its control points to generate a smooth path with continuous curvature [23]. The equation of the B-spline curve can be presented by the following equation:(12)pt=∑i=0npi•Fi,kt,where pii=0,1,…,n are the characteristic nodes of the spline curve. The ith B-spline basis function of degree k, written as Fi,ki=0,1,…,n, can be expressed as follows:(13)F0,3t=161−t3,F1,3t=163t3−6t2+4,F2,3t=16−3t3+3t2+3t+1,F3,3t=16t3,where t∈0,1.


## 4.1. Algorithm Process

The detailed procedure of the proposed algorithm is explained as mentioned below.From aforementioned descriptions, the flow chart of the BPIB-RRT∗ algorithm can be presented in Figure 1.Figure 1
Flow chart of the BPIB-RRT∗ algorithm.Extra procedures employed by BPIB-RRT∗ are explained below, whereas the rest are exactly the same as they are for IB-RRT∗. Therefore, only the Bi-bias() heuristic and BPF() heuristic are explained in detail.


## 4.2.Bi-bias()

The random sampling process of the RRT∗ algorithm based on full space causes its convergence to the optimal solution very slow. Hence, the Bi-bias() heuristic based on the growth trend of the target tree is introduced into IB-RRT∗.qrand is generated by Sample() firstly and then passed to Bi-bias(). If the iteration count k is even, then the randomly sampled state qrand is pulled toward the random tree Tb. The bias direction can be obtained by analysing the relative position between qprev2 and qrand, where qprev2 is the new vertex of the target tree Tb. The Bi-bias() heuristic can provide a higher deviation in the right direction before generating the bias node qbia. If the iteration count k is odd, the random tree Ta attracts the randomly sampled state qrand. Similarly, the relative position between qprev1 and qrand is calculated to provide a higher deviation for qrand, where qprev1 is the new vertex of the target tree Ta.The bias formulas are shown below:(3)xbia=xrand+rand∗xprev∗−xrand,xrand≤xprev∗,xbia=xrand−rand∗xrand,xrand>xprev∗,(4)ybia=yrand+rand∗yprev∗−yrand,yrand≤yprev∗,ybia=yrand−rand∗yrand,yrand>yprev∗,(5)zbia=zrand+rand∗zprev∗−zrand,zrand≤zprev∗,zbia=zrand−rand∗zrand,zrand>zprev∗,where xrand,yrand,zrand denotes the random state coordinate, xprev1,yprev1,zprev1 and xprev2,yprev2,zprev2 represent the new vertex of target trees Ta and Tb, respectively. rand denotes the random function between 0 and 1. Bias node qbia=xbia,ybia,zbia is calculated by the aforementioned formulas.It can be seen from Figure2, there are eight relative positions between qprev∗ and qrand in state space. Take the spatial position shown in Figure 2 as an example, part of the Bi-bias() heuristic is shown in Algorithm 5. The calculation of other spatial relationships for qrand and qprev∗ is the same. By feeding back the vertex position of the two random trees, Bi-bias() updates the spatial position of random state qrand dynamically, improves the effectiveness of the sampling process, and furthermore reduces the number of iterations of the algorithm.Figure 2
The instance of spatial position.Algorithm 5:
Bi-bias(q
rand, k, qprev1, qprev2).
(1)
if
k
mod 2 = 0 then
(2)
if (xprev2 < xrand) then
(3)
if (yprev2 < yrand) then
(4)
if (zprev2 > zrand) then
(5)
xbia = xrand − rand∗ xrand
(6)
ybia = yrand − rand∗ yrand
(7)
zbia = zrand + rand∗ (zprev2 − zrand)
(8)
else
(9)
if (xprevl < xrand) then
(10)
if (yprevl < yrand) then
(11)
if (zprev1 > zrand) then
(12)
xbia = xrand − rand∗ xrand
(13)
ybia = xrand − rand∗ yrand
(14)
zbia = zrand − rand∗ (zprev1 − zrand)
(15)
returnqbia = (xbia, ybia,zbia)
(16)
end


## 4.3. BPF()

Artificial potential field (APF) has a good application in obstacle avoidance technology of UAV. The APF algorithm constructs attractive field and repulsive force field based on the target point and obstacles in the planning space, respectively. The goal region pulls the robot towards it, and the obstacles repel the robot away from the obstacle configuration spaceSobs. The force exerted to the robot is equal to the resultant force of attractive force and repulsive force at this location [21]. APF is simple to operate and has a fast convergence speed. It could get stuck in the local minima for it performs pure exploitation, and it is greedy. APF is smooth in mathematic which means the tangent and the velocity are always continuous [22] and improves UAV stability. The equation of attraction field is discussed below:(6)UAttq=12kAttρ2q,Sgoal,where UAttq is the attraction field, kAtt is the attraction field coefficient, q is the current position of the UAV, Sgoal is the goal location, and ρq,Sgoal is the distance between the current point and the goal location. The attraction field force FAttq is generated by taking a negative gradient(7)FAttq=−∇UAttq=kAttρq,Sgoal.The repulsive fieldURepq is generated by all obstacles Sobs∈S.(8)URepq=∑i=1IURepiq,where URepiq is the repulsion field generated by the ith obstacle at q.(9)URepiq=12λ1ρq,qobsi−1ρ0i2,ρq,qobsi<ρ0i,0,ρq,qobsi≥ρ0i,where λ is the repulsive field coefficient, ρ0i is the effective radius of the ith repulsive field, and ρq,qobsi is the distance between the ith obstacle and the current position q. Correspondingly, let FRepiq sign the repulsion force,(10)FRepiq=FRep1iq,ρq,qobsi<ρ0i,0,ρq,qobsi≥ρ0i,where FRep1iq=λ1/ρq,qobsi−1/ρ0i1/ρ2q,qobsi∇ρq,qobsi.The resultant force on the UAV at pointq is Fq.(11)Fq=FAttq+∑i=1IFRepiq.Incorporating APF into IB-RRT∗ by using the Bidirectional Potential Field heuristic (BPF()), pseudocode of the BPF() heuristic is presented in Algorithm 6. The current vertices of two random trees qprev1 and qprev2 are first fed into the BPF() algorithm. When the number of iterations k is even, the new vertex qprev2 of the Tb is treated as the target point of the bias point qbia, and the attraction force based on the qprev2 is calculated. dnearesti∈Dnearest is the distance between the bias point qbia and the ith obstacle. ρ0i denotes the radius of repulsion field. If dnearesti is less than ρ0i, qbia will be guided toward the direction of the resultant force Fq based on the repulsion force and the attraction force, otherwise only attraction force FAttq works on qbia. Then, the optimal random node qop is obtained. If the iteration count k is odd, the BPF() heuristic treats the new vertex qprev1 of the Ta as the attraction field of the bias point qbia. Similarly, qbia is directed downhill in direction of the decreasing potential by computing the resultant force, and the optimal random node qop is produced.Algorithm 6:BPF (qbia, k, qprev1, qprev2).
(1)
q
op ⟵ qbia
(2)
ifkmod 2 = 0 then
(3)
FAtt = computeAttForce(qprev2, qop)
(4)
Dnearest = ρ(Sobs, qop)
(5)
if dnearesti ≤ ρ0i then
(6)
FRep = computeRepForce(qop, Sobs)
(7)
qop = qop + FRep/FRep+FAtt/FAtt
(8)
else
(9)
qop = qop + FAtt/FAtt
(10)
end
(11)
else
(12)
FAtt = computeAttForece(qprev1, qop)
(13)
Dnearest = ρ(Sobs, qop)
(14)
if dnearesti ≤ ρ0i then
(15)
FRep = computeRepForce(qop, Sobs)
(16)
qop = qop + FRep/FRep+FAtt/FAtt
(17)
else
(18)
qop = qop + FAtt/FAtt
(19)
end
(20)
end
(21)
return
q
op


## 4.4. Path Generation Using B-Spline

The curvature of path generated by the RRT∗ algorithm is noncontinuous. Therefore, the initial path cannot meet the flight constraints of UAV. To overcome this shortcoming, B-spline is introduced into BPIB-RRT∗ due to its high flexibility and arbitrary continuity. The path vertices generated by the BPIB-RRT∗ algorithm are fed into B-spline as its control points to generate a smooth path with continuous curvature [23]. The equation of the B-spline curve can be presented by the following equation:(12)pt=∑i=0npi•Fi,kt,where pii=0,1,…,n are the characteristic nodes of the spline curve. The ith B-spline basis function of degree k, written as Fi,ki=0,1,…,n, can be expressed as follows:(13)F0,3t=161−t3,F1,3t=163t3−6t2+4,F2,3t=16−3t3+3t2+3t+1,F3,3t=16t3,where t∈0,1.


## 5. Analysis

### 5.1. Probabilistic Completeness

LetDkap denote the random variable of the distance with the minimum cost from node p∈Sfree to the first random tree Ta, and dka is the value of the random variable Dkap, where k is the number of iterations for BPIB-RRT∗. Similarly, let Dkbp denote the random variable of the minimum distance cost between nodes p∈Sfree and Tb , and dkb is the value of the random variable. Dkpa,pb is the random variable of the distance with the minimum distance cost from Ta to Tb, where pa and pb belong to Ta and Tb, respectively. dkpa,pb is the value of the random variable Dkpa,pb.Probabilistic Completeness: for a robust feasible path planning problemSfree,Sinit,Sgoal, when the number of iterations k approaches infinity, the distance between pa and pb is less than a constant τ. The probability of finding a feasible solution goes to one.(14)limk⟶∞Pdkpa,pb<τpa,pb∈Sfree=1.Theorem 1.
The BPIB-RRT∗ algorithm is a probabilistically complete algorithm. In the three-dimensional solution space S, as the number of iterations approaches infinity, probability of finding a feasible path from the initial to goal region goes to one.(15)limk⟶∞Pdkpa,pb<τpa,pb∈Sfree=1.Proof.
The closed ball of radiusτ>0 centred at pa∈Ta is defined as Oapa. Similarly, for pb∈Tb the closed ball is defined as Obpb.(16)Oapa:Oa,τ≔p∈Sp−pa≤τ,(17)Obpb:Ob,τ≔p∈Sp−pb≤τ.
LetOa′pa denote the intersection of the obstacle-free space Sfree and the closed ball Oapa. Ob′pb is denoted in the same way as Oa′pa.(18)Oa′pa=Oapa∩Sfree,(19)Ob′pb=Obpb∩Sfree.
Fork=0, d0pa,pb=Sinit,Sgoal, where Sinit,Sgoal denotes the Euclidean distance from the initial to goal region. BPIB-RRT∗ guides two rapidly-exploring random trees towards each other by using the proposed Bi-bias() and BPF() heuristics. Therefore, the distance between Ta and Tb is decreasing.(20)Pdkpa,pb<τ>0.
Fork=n, it is assumed that all vertices in Ta are outside the ball Ob′pb.(21)Ta∩Ob′pb=∅.
Similarly,(22)Tb∩Oa′pa=∅.
For the next growth of the random treeTa, according to the extension mode of BPIB-RRT∗, if qop=pb, the mean of Dk+1apb is as shown below:(23)EDk+1apb∣qop=pb<EDkapb,otherwise, the mean of Dk+1apb is given by following:(24)EDk+1apb∣qop≠pb≤EDkapb.
Thus,(25)EDk+1apb=Eα•Dk+1apbqop=pb+1−α•Dk+1apb∣qop≠pb<EDkapb,where 0<α<1.
Similarly, for the next growth of the random treeTb, if qop=pa, then the mean of Dk+1bpa is as given below:(26)EDk+1bpaqop=pa<EDkbpa;otherwise, the mean of Dk+1bpa is expressed by the following form:(27)EDk+1bpaqop≠pa≤EDkbpa.
Therefore,(28)EDk+1bpa=Eβ•Dk+1bpaqop=pa+1−β•Dk+1bpaqop≠pa<EDkbpa,where 0<β<1.
Referring to (25) and (28), it is obvious that the distance between the vertex pa∈Ta and the vertex pb∈Tb is decreasing.
As the number of iterationsk reaches infinity, the probability of finding a feasible path approaches one.(29)limk⟶∞Pdkpa,pb<τpa,pb∈Sfree=1.
The BPIB-RRT∗ algorithm does not change the random sampling characteristic of the RRT∗ which ensures probabilistic completeness.


### 5.2. Asymptotic Optimality

Given a cost functionJ•, for an optimal path η∗, the optimal cost J∗ can be confirmed by Jη∗=J∗. Let η∗⊂Sfree denote the optimal path such that limk⟶∞ηk=η∗ and limk⟶∞Jηk=Jη∗=J∗, where η∗:0,1⟶η∗0=Sinit,η∗1=Sgoal. Let CkBPIB−RRT∗ denote the random variable of the minimum path cost.Theorem 2.
As the total number of iterationsk reaches infinity, the cost of path generated by BPIB-RRT∗ converges to the optimal cost J∗.(30)Plimk⟶∞supCkBPIB−RRT∗=J∗=1.Sketch of Proof.
In reference to Theorem 14 in [11], similar to RRT∗, the BPIB-RRT∗ attempts to establish a connection between the roadmap vertexes within a ball of dynamic radius rkBPIB−RRT∗ that Ta and Tb can grow towards each other, where rkBPIB−RRT∗ is a function of k. The connection radius rkBPIB−RRT∗ is denoted as(31)rkBPIB‐RRT∗=γBPIB‐RRT∗logkk1/d,where γBPIB−RRT∗>γBPIB−RRT∗∗=21+1/d1/dμSfree/ζd1/d. μ• denotes the Lebesgue measure, and ζd denotes the volume of the unit ball in the d-dimensional Euclidean space. As the same procedures happened in BPIB-RRT∗, according to Lemma 56, 71, and 72 in [12], it can be proved that BPIB-RRT∗ is asymptotically optimal.


### 5.3. Computational Complexity

LetMkBPIB−RRT∗ denote the mean of the total computation performed by BPIB-RRT∗ with k iterations. Theorem 3 proposes that the computational complexity of BPIB-RRT∗ is a constant multiple higher than IB-RRT∗.Theorem 3.
There exists a constantϕ∈R+ such that it satisfies the following formula:(32)limk⟶∞supEMkBPIB−RRT∗MkIB−RRT∗≤ϕ.Sketch of Proof.
Compared to IB-RRT∗, Bi-bias() and BPF() heuristics are introduced into BPIB-RRT∗. Notice that Bi-bias() and BPF() heuristics are both performed in a constant number of iterations and independent of the number of vertices in the tree. BPF() has to calculate all obstacles for the bias point qbia∈Sfree which requires at least Ωlog10n time. Furthermore, BPIB-RRT∗ calls NearestVertex(), NeighbouringVertices(), Steer(), and ObstacleFree() procedures for both trees Ta and Tb just like IB-RRT∗ which adds up a computation constant. Hence, as seen in Theorem 3, BPIB-RRT∗ only vary from IB-RRT∗ by ϕ in terms of computational complexity ratio.


## 5.1. Probabilistic Completeness

LetDkap denote the random variable of the distance with the minimum cost from node p∈Sfree to the first random tree Ta, and dka is the value of the random variable Dkap, where k is the number of iterations for BPIB-RRT∗. Similarly, let Dkbp denote the random variable of the minimum distance cost between nodes p∈Sfree and Tb , and dkb is the value of the random variable. Dkpa,pb is the random variable of the distance with the minimum distance cost from Ta to Tb, where pa and pb belong to Ta and Tb, respectively. dkpa,pb is the value of the random variable Dkpa,pb.Probabilistic Completeness: for a robust feasible path planning problemSfree,Sinit,Sgoal, when the number of iterations k approaches infinity, the distance between pa and pb is less than a constant τ. The probability of finding a feasible solution goes to one.(14)limk⟶∞Pdkpa,pb<τpa,pb∈Sfree=1.Theorem 1.
The BPIB-RRT∗ algorithm is a probabilistically complete algorithm. In the three-dimensional solution space S, as the number of iterations approaches infinity, probability of finding a feasible path from the initial to goal region goes to one.(15)limk⟶∞Pdkpa,pb<τpa,pb∈Sfree=1.Proof.
The closed ball of radiusτ>0 centred at pa∈Ta is defined as Oapa. Similarly, for pb∈Tb the closed ball is defined as Obpb.(16)Oapa:Oa,τ≔p∈Sp−pa≤τ,(17)Obpb:Ob,τ≔p∈Sp−pb≤τ.
LetOa′pa denote the intersection of the obstacle-free space Sfree and the closed ball Oapa. Ob′pb is denoted in the same way as Oa′pa.(18)Oa′pa=Oapa∩Sfree,(19)Ob′pb=Obpb∩Sfree.
Fork=0, d0pa,pb=Sinit,Sgoal, where Sinit,Sgoal denotes the Euclidean distance from the initial to goal region. BPIB-RRT∗ guides two rapidly-exploring random trees towards each other by using the proposed Bi-bias() and BPF() heuristics. Therefore, the distance between Ta and Tb is decreasing.(20)Pdkpa,pb<τ>0.
Fork=n, it is assumed that all vertices in Ta are outside the ball Ob′pb.(21)Ta∩Ob′pb=∅.
Similarly,(22)Tb∩Oa′pa=∅.
For the next growth of the random treeTa, according to the extension mode of BPIB-RRT∗, if qop=pb, the mean of Dk+1apb is as shown below:(23)EDk+1apb∣qop=pb<EDkapb,otherwise, the mean of Dk+1apb is given by following:(24)EDk+1apb∣qop≠pb≤EDkapb.
Thus,(25)EDk+1apb=Eα•Dk+1apbqop=pb+1−α•Dk+1apb∣qop≠pb<EDkapb,where 0<α<1.
Similarly, for the next growth of the random treeTb, if qop=pa, then the mean of Dk+1bpa is as given below:(26)EDk+1bpaqop=pa<EDkbpa;otherwise, the mean of Dk+1bpa is expressed by the following form:(27)EDk+1bpaqop≠pa≤EDkbpa.
Therefore,(28)EDk+1bpa=Eβ•Dk+1bpaqop=pa+1−β•Dk+1bpaqop≠pa<EDkbpa,where 0<β<1.
Referring to (25) and (28), it is obvious that the distance between the vertex pa∈Ta and the vertex pb∈Tb is decreasing.
As the number of iterationsk reaches infinity, the probability of finding a feasible path approaches one.(29)limk⟶∞Pdkpa,pb<τpa,pb∈Sfree=1.
The BPIB-RRT∗ algorithm does not change the random sampling characteristic of the RRT∗ which ensures probabilistic completeness.


## 5.2. Asymptotic Optimality

Given a cost functionJ•, for an optimal path η∗, the optimal cost J∗ can be confirmed by Jη∗=J∗. Let η∗⊂Sfree denote the optimal path such that limk⟶∞ηk=η∗ and limk⟶∞Jηk=Jη∗=J∗, where η∗:0,1⟶η∗0=Sinit,η∗1=Sgoal. Let CkBPIB−RRT∗ denote the random variable of the minimum path cost.Theorem 2.
As the total number of iterationsk reaches infinity, the cost of path generated by BPIB-RRT∗ converges to the optimal cost J∗.(30)Plimk⟶∞supCkBPIB−RRT∗=J∗=1.Sketch of Proof.
In reference to Theorem 14 in [11], similar to RRT∗, the BPIB-RRT∗ attempts to establish a connection between the roadmap vertexes within a ball of dynamic radius rkBPIB−RRT∗ that Ta and Tb can grow towards each other, where rkBPIB−RRT∗ is a function of k. The connection radius rkBPIB−RRT∗ is denoted as(31)rkBPIB‐RRT∗=γBPIB‐RRT∗logkk1/d,where γBPIB−RRT∗>γBPIB−RRT∗∗=21+1/d1/dμSfree/ζd1/d. μ• denotes the Lebesgue measure, and ζd denotes the volume of the unit ball in the d-dimensional Euclidean space. As the same procedures happened in BPIB-RRT∗, according to Lemma 56, 71, and 72 in [12], it can be proved that BPIB-RRT∗ is asymptotically optimal.


## 5.3. Computational Complexity

LetMkBPIB−RRT∗ denote the mean of the total computation performed by BPIB-RRT∗ with k iterations. Theorem 3 proposes that the computational complexity of BPIB-RRT∗ is a constant multiple higher than IB-RRT∗.Theorem 3.
There exists a constantϕ∈R+ such that it satisfies the following formula:(32)limk⟶∞supEMkBPIB−RRT∗MkIB−RRT∗≤ϕ.Sketch of Proof.
Compared to IB-RRT∗, Bi-bias() and BPF() heuristics are introduced into BPIB-RRT∗. Notice that Bi-bias() and BPF() heuristics are both performed in a constant number of iterations and independent of the number of vertices in the tree. BPF() has to calculate all obstacles for the bias point qbia∈Sfree which requires at least Ωlog10n time. Furthermore, BPIB-RRT∗ calls NearestVertex(), NeighbouringVertices(), Steer(), and ObstacleFree() procedures for both trees Ta and Tb just like IB-RRT∗ which adds up a computation constant. Hence, as seen in Theorem 3, BPIB-RRT∗ only vary from IB-RRT∗ by ϕ in terms of computational complexity ratio.


## 6. Simulation Results

In order to analyse the effectiveness of BPIB-RRT∗, the 3D simulation examples will be made for three different 3D configuration spaces in this section.Three different 3D configuration spaces are shown in Figure3. The initial and goal locations are fixed at 0,0,0 and 100,100,100, respectively in Map1 and Map2. The initial and goal locations are fixed at 0,0,5 and 100,100,5 in Map3. Spherical obstacles are generated in the configuration spaces to examine the efficiency of our proposed BPIB-RRT∗ algorithm.Figure 3
3D configuration spaces. (a) Map1, (b) Map2, and (c) Map3.
(a)
(b)
(c)The maximum number of algorithm iterations is set as106. If the total number of iterations exceeds the maximum, the simulation returns a feasible path or a failed one. BPIB-RRT∗, PIB-RRT∗, tGSRT, and RRT∗ are examined in three aforementioned different 3D environments, and the simulation results are shown in Figures 4–9. From Figures 4–9, it can be obtained that PIB-RRT∗, tGSRT, and BPIB-RRT∗ are effective for UAV path planning in 3D environments, whereas the traditional RRT∗ algorithm may be failed. Besides, all the paths generated by our proposed BPIB-RRT∗ algorithm can avoid obstacles, which have been obviously shown in Figures 7–9 with different visual angles. Table 1 shows the parameters obtained while running 50 times of the four different algorithms with maximum, minimum, and average iterations and path cost (in terms of Euclidean metric). The conclusions of the examination and the simulations are discussed in detail below.Figure 4
Simulation results of RRT∗ in three different 3D environments. (a) Map1, (b) Map2, and (c) Map3.
(a)
(b)
(c)Figure 5
Simulation results of PIB-RRT∗ in three different 3D environments. (a) Map1, (b) Map2, and (c) Map3.
(a)
(b)
(c)Figure 6
Simulation results of tGSRT in three different 3D environments. (a) Map1, (b) Map2, and (c) Map3.
(a)
(b)
(c)Figure 7
Simulation results of BPIB-RRT∗ in Map1. (a) AZ = −37.5, EL = 30, and (b) AZ = −150, EL = 12.
(a)
(b)Figure 8
Simulation results of BPIB-RRT∗ in Map2. (a) AZ = −37.5, EL = 30, and (b) AZ = 31, EL = 18.
(a)
(b)Figure 9
Simulation results of BPIB-RRT∗ in Map3. (a) AZ = −37.5, EL = 30, and (b) AZ = −130, EL = 22.
(a)
(b)Table 1
Simulation result data.
Environment
Algorithm
Path cost (km)
Max iteration
Min iteration
Average iteration
Time (s)
Failed rate (%)
Map1
RRT∗
203
15607
6870
13920.768
680.453
76
B-RRT∗
203
780
389
509.447
30.129
PIB-RRT∗
203
231
192
205.473
25.627
—
tGSRT
204.861
316
121
198.65
54.982
—
BPIB-RRT∗
203
129
98
104.368
8.532
—
Map2
RRT∗
202
15893
7378
12685.151
684.434
70
B-RRT∗
202
732
387
420.196
33.647
PIB-RRT∗
202
221
179
194.473
23.633
—
tGSRT
205.797
433
192
278.35
60.663
—
BPIB-RRT∗
202
199
168
179.947
19.537
—
Map3
RRT∗
168.656
17428
12453
15527.745
862.011
62
B-RRT∗
168.656
621
389
453.169
34.551
PIB-RRT∗
168.656
251
156
204.483
25.739
—
tGSRT
195.709
754
636
705.974
45.422
—
BPIB-RRT∗
168.656
178
164
169.427
17.018
—Figure4 shows the simulation results of RRT∗ in three different 3D maps. As seen from Table 1, RRT∗ performs pure exploration that too many invalid samples lower efficiency of the algorithm. It may be hard or need a lot of iterations for RRT∗ to find an optimal path. Therefore, a longer planning time is necessary for RRT∗ to converge to an optimal solution. Meanwhile, RRT∗ is failed to converge to the optimal solution 76%, 70%, and 62% of the times in three different maps, respectively.The path planning results in three different environments for PIB-RRT∗ are illustrated in Figure 5. Compared with the RRT∗ algorithm, the PIB-RRT∗ algorithm converges to the optimal solution faster. Random trees are potentially guided by bidirectional potential gradient heuristics towards each other to improve the sampling efficiency and reduces the planning time of the PIB-RRT∗ in three different maps (Time = 25.627, 23.633, 25.739). However, the fixed attractive pole inside the BPG() heuristic still produced invalid sampling points. Also, the smoothness of the path is not considered in PIB-RRT∗.It can be seen from Figure6 that the tGSRT algorithm can search the path effectively in three different maps. However, the path generated by tGSRT algorithm with jags which cannot meet the constraint conditions of smooth and the jags may cause unstability for the flight of UAV. Furthermore, compared with the other algorithms, the paths generated by the tGSRT algorithm in three different maps are not the optimal solution (Path Cost = 204.861, 205.797, 195.709). Meanwhile, the planning time is increased (Time = 54.982, 60.663, 45.422) due to the evolutionary process (selection, crossover and mutation) of the genetic algorithm.The simulation results in three maps of BPIB-RRT∗ are shown in Figures 7–9. Also, different visual angle of results are given to present obviously. As seen from Table 1, based on the Bi-bias() heuristic and BPF() heuristic, the optimal solution is found in the Map1, most quickly with more efficient sampling strategy by BPIB-RRT∗(Time = 8.532 s). Comparing with PIB-RRT∗(Average Iterations = 205.473), the average number of iterations of BPIB-RRT∗ is reduced (Average Iterations = 104.368). To improve the flight ability of the BPIB-RRT∗-generated path, the path vertices sequence is fed into B-spline to robustly produce a smooth optimal path. Figure 8 is another 3D environment with obstacles gradually growing larger, and BPIB-RRT∗ is the first to find the optimal path(Time = 19.537 s), whereas PIB-RRT∗ and B-RRT∗ are the second (Time = 23.633 s) and the third (Time = 33.647 s) converge to the optimal path. Figure 9 is the results of Map3, and UAV can smoothly pass through the obstacles. As seen from Table 1, BPIB-RRT∗ is the fastest to find the optimal path (Time = 17.018), comparing with PIB-RRT∗ (Time = 25.739) and RRT∗ (Time = 862.011).Based on aforementioned analyses, it can be known that the proposed BPIB-RRT∗ algorithm can solve the path planning problem for UAV in complex 3D environment and avoid obstacle with excellent performance. Moreover, comparing with other variants of the RRT∗ algorithm, the average number of iterations is minimal and the convergence rate is the fastest by using the BPIB-RRT∗ algorithm.


## 7. Conclusions

In this paper, a new sampling-based approach defined as the BPIB-RRT∗ algorithm is proposed, which can obtain the optimal path in the complex 3D configuration space. Higher deviation can be provided by the Bi-bias() heuristic which can adjust the space of the sample and improve the accuracy of it. The BPF() heuristic is introduced into the BPIB-RRT∗ algorithm to guide the path node by the resultant force based on the goal node and the obstacle and reduce the blindness of the algorithm which expedites the convergence rate. Also, the iteration number of the BPIB-RRT∗ algorithm can be reduced benefited by the aforementioned heuristics. Finally, the curvature of path turns into continuous by B-spline which guarantees the stability of UAV. Meanwhile, it is strictly proven that as the number of iteration reaches infinity, the probability of finding a feasible solution is limited to 1. Computational complexity has been testified to be both theoretically and experimentally equal to the IB-RRT∗ algorithm, and the presented algorithm inherits the asymptotic optimality of RRT∗. Comparing with RRT∗ and other existing algorithms, the superiority of the proposed BPIB-RRT∗ algorithm has been verified in the comparison experiments.



---

*Source: 5157403-2019-11-12.xml*