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

Algorithm 4

BPIB-RRT^∗(S_init, S_goal).

(1)	Va ⟵ {Sinit}; Vb ⟵ {Sgoal}; Ea ⟵ Ø; Eb ⟵ Ø;
(2)	Ta ⟵ (Va, Ea); Tb ⟵ (Vb, Eb);
(3)	qprev1 = Sinit
(4)	qprev2 = Sgoal
(5)	ηbest ⟵ ∞
(6)	Connection ⟵ True
(7)	for k ⟵ 0 to K do
(8)	qrand ⟵ Sample(k)
(9)	qbia ⟵ Bi-bias(qrand, k, qprev1, qprev2)
(10)	qop ⟵ BPF(qbia, k, qprev1, qprev2)
(11)	{, } ⟵ NeighbouringVertices(qop, Ta, Tb)
(12)	if  = Ø and  = Ø then
(13)	{, } ⟵ NearestVertex(qop, Ta, Tb)
(14)	Connection ⟵ False
(15)	La ⟵ GetsortedList(qop, )
(16)	Lb ⟵ GetsortedList(qop, )
(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 state is obtained by the sampling process in the given state space. Then, the biased node 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 turns into biased potentially guided bidirectional randomly sampled state such that , where . 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 state obtained in Step 1 from random trees or and InsertVertex() insert the into the best selected tree. Step 1 and Step 2 continue till .
	Step 3: Generate the path. As iteration, , goes from to , a feasible solution path 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.