PQ-RRT*: an improved path planning algorithm for mobile robots

Journal Pre-proof

PQ-RRT*: an improved path planning algorithm for mobile robots

Yanjie Li, Wu Wei, Yong Gao, Dongliang Wang, Zhun Fan

PII: S0957-4174(20)30249-9
DOI: https://doi.org/10.1016/j.eswa.2020.113425
Reference: ESWA 113425

To appear in: Expert Systems With Applications

Received date: 21 September 2019

Revised date: 25 January 2020
Accepted date: 30 March 2020

Please cite this article as: Yanjie Li, Wu Wei, Yong Gao, Dongliang Wang, Zhun Fan, PQ-RRT*: an
improved path planning algorithm for mobile robots, Expert Systems With Applications (2020), doi:
https://doi.org/10.1016/j.eswa.2020.113425

This is a PDF file of an article that has undergone enhancements after acceptance, such as the addition
of a cover page and metadata, and formatting for readability, but it is not yet the definitive version of
record. This version will undergo additional copyediting, typesetting and review before it is published
in its final form, but we are providing this version to give early visibility of the article. Please note that,
during the production process, errors may be discovered which could affect the content, and all legal
disclaimers that apply to the journal pertain.

© 2020 Published by Elsevier Ltd.

Highlights

 Propose a sampling-based asymptotically optimal path planning algorithm

 The proposed algorithm guarantees a fast convergence rate
 Theoretical proof of asymptotic optimality and fast convergence rate is given
 PQ-RRT*: an improved path planning algorithm for mobile robots

Yanjie Lia , Wu Weia,∗, Yong Gaoa , Dongliang Wanga , Zhun Fanb,c,d,e

a School of Automation Science and Engineering, South China University of Technology, Guangzhou 510641, China
b Department of Electronic and Information Engineering, Shantou University, Shantou 515063, China
c Key Lab of Digital Signal and Image Processing of Guangdong Province, Shantou University, Shantou 515063, China
d Key Laboratory of Intelligent Manufacturing Technology (Shantou University), Ministry of Education 515063, China
e State Key Lab of Digital Manufacturing Equipment and Technology, Huazhong University of Science and Technology, Wuhan 43003, China

Abstract
During the last decade, sampling-based algorithms for path planning have gained considerable attention. The RRT*, a
variant of RRT (rapidly-exploring random trees), is of particular concern to researchers due to its asymptotic optimal-
ity. However, the limits of the slow convergence rate of RRT* makes it inefficient for applications. For the purposes
of overcoming these limitations, this paper proposes a novel algorithm, PQ-RRT*, which combines the strengths of
P-RRT* (potential functions based RRT*) and Quick-RRT*. PQ-RRT* guarantees a fast convergence to an optimal
solution and generates a better initial solution. The asymptotic optimality and fast convergence of the proposed algo-
rithm are proved in this paper. Comparisons of PQ-RRT* with P-RRT* and Quick-RRT* in four benchmarks verify
the effectiveness of the proposed algorithm.
Keywords:
path planning, sampling-based algorithms, rapidly-exploring random tree (RRT), optimal path planning

1. Introduction

Mobile robots are gaining more and more attention as intelligent systems. Much attention has been devoted to
mobile robots since they can increase productivity and provide various conveniences. Mobile robots with autonomous
navigation capabilities are critical to machine intelligence. The basis of navigation is path planning (Huang et al.,
2019; Majeed & Lee, 2019; Lee et al., 2018), which consists of finding feasible paths from the start state to the goal
state without colliding with any obstacles. Applications of path planning algorithms include but are not limited to
industrial automation (Beyer et al., 2015; Pandini et al., 2017), graphical animation (Liu & Badler, 2003), autonomous
exploration (Atanacio-Jiménez et al., 2011), medical (Taylor et al., 2016; Valencia-Garcia et al., 2005) and robot
navigation (González et al., 2015).
The research on path planning is in full swing. Currently, path planning algorithms mainly include geometric
algorithms, artificial potential field methods, grid-based searches, and sampling-based algorithms. Typical represen-
tatives of geometric algorithms include visibility graph (Asano et al., 1985; Alexopoulos & Griffin, 1992; Maekawa
et al., 2010) and cell decomposition methods (Brooks & Lozano-Perez, 1985). However, these algorithms are mostly
limited to low-dimensional path planning problems. Artificial potential fields (APF) (Khatib, 1986), a method first
proposed by Khatib, should be given special consideration. APF assumes that there is a virtual force consisting of the
repulsive force of the obstacles and the attractive force of the goal region. The system proceeds according to their joint
force. Although the operation is easy, APF suffers from the problem of local minima (Koren & Borenstein, 1991).
When applying graph theory to a discretized state space, grid-based searches assume that each state corresponds to
a grid point. A well-known algorithm in grid-based algorithms is A* (Hart et al., 1968; Koenig et al., 2004; Stentz,

∗ Corresponding
author
Email addresses: 1073889317@qq.com (Yanjie Li), weiwu@scut.edu.cn (Wu Wei), andygao_scut@163.com (Yong Gao),
757613634@qq.com (Dongliang Wang), zfan@stu.edu.cn (Zhun Fan)

Preprint submitted to Expert Systems with Applications March 31, 2020

1997). Although it can guarantee resolution completeness and resolution optimality, computation time and memory
space will grow exponentially in dimensions.
Sampling-based algorithms have gained a considerable amount of attention as the result of their superior perfor-
mance in high-dimensional state spaces. These algorithms provide probabilistic completeness. Probabilistic com-
pleteness means that if feasible path exists, the probability that the algorithm cannot find the path tends towards
zero as the number of samples approaches infinity. Undisputed, the most widely used and influential algorithms to
date are probabilistic roadmaps (PRMs) (Kavraki & Latombe, 1994; Amato & Wu, 1996; Kavraki et al., 1996) and
rapidly-exploring Random Trees (RRTs) (LaValle, 1998). The PRM algorithm is a multiple-query method, which is
suitable for problems with prior knowledge of the environment. In practice, an environment is not necessarily known
in advance, and the multiple-queries methods do not always perform well. As single-query counterparts to PRMs,
RRTs have won the attention of the research community, due to their greater efficiency in practical applications. Due
to increased attention, a large number of RRT-variants have emerged in the last decade. Introducing a greedy heuris-
tic to RRT, a double tree algorithm, RRT-connect, was proposed. Unlike RRT, RRT-connect (Kuffner & LaValle,
2000) maintains two trees at the same time: one from the initial state and the other from the goal state. However,
both RRT and RRT-connect consider the cost of the found solution, so both cannot ensure optimality. Inspired by
the above idea, Karaman et al. (Karaman & Frazzoli, 2011) proposed the RRT* algorithm as an optimal variant of
RRT. Different from RRT, RRT* adopts optimisation modules, the ChooseParent and Rewire procedures, ensuring
the asymptotic optimality. An algorithm is said to be asymptotically optimal if the probability of finding the optimal
solution approaches one when the number of samples approaches infinity. RRT* is a milestone in the development of
RRT.
Although finding the optimal path solution is already a challenging task, ensuring fast convergence is also impor-
tant for path planning. For the purpose of overcoming the slow convergence rate of RRT*, a lot of work has gradually
unfolded. Enlightened by the idea of visibility graph technology, RRT*-Smart (Islam et al., 2012) introduces an in-
telligent sampling method to RRT*. RRT*-Smart accelerates the convergence rate and obtains an optimum or a near
optimum solution. However, the quality of the solution obtained by RRT*-Smart greatly depends on the initial so-
lution, which reduces the probability of finding a different homotopy class, thus violating the assumption of uniform
sampling of RRT*. As we know, as the solution optimisation process proceeds, the number of nodes will increase
infinitely. Therefore, the implementation of RRT* will be difficult in a computing system with limited memory. In
order to use the memory more efficiently, Olzhas et al. put forward RRT* FN (Adiyatov & Varol, 2013), an algorithm
with a fixed number of nodes. When the number of nodes exceeds a given value, it is important to remove nodes that
are not useful in reducing the cost. In addition to the above methods, it is worth noting the sampling heuristic known as
node rejection (Akgun & Stilman, 2011; Ferguson & Stentz, 2006). Informed RRT* (Gammell et al., 2014), inspired
by node rejection, uses a direct sampling method that samples in a hyper-ellipsoid. However, the algorithm will no
longer be applicable when the associated prolate hyperspheroid is larger than the domain of the planning problem.
A fundamental reason for the slow convergence rate of RRT* is its pure exploration, while APF (Khatib, 1986)
suffers from the problem of local minima due to its pure exploitation. Based on the above ideas, Qureshi et al.
proposed an improved algorithm, P-RRT* (Qureshi & Ayaz, 2016), which incorporates the APF into RRT*. It is the
addition of APF that provides a direction for exploration, giving P-RRT* a faster convergence than RRT*. P-RRT*
introduces APF to achieve a trade-off between exploration and exploitation, which is worth noting. Accelerating the
convergence rate of RRT* can not only start from the guided sampling process, but it can also be optimised from the
structure of RRT* itself. In this regard, Jeong et al. first point out that use of triangular inequality to improve the
ChooseParent and Rewire procedures, proposing Quick-RRT* (Jeong et al., 2019). Compared with RRT*, Quick-
RRT* allows a faster rate of convergence. Since Quick-RRT* is a tree-extending algorithm, any sampling strategy or
graph-pruning algorithm can be combined with Quick-RRT*.
This paper proposes a novel algorithm, PQ-RRT*, for the optimal path planning mobile robots. Compared with
P-RRT* and Quick-RRT*, PQ-RRT* generates a better initial solution and a fast convergence to optimal solution.
A theoretical proof is given for the completeness, asymptotic optimality and faster convergence of the proposed
algorithm. In addition, PQ-RRT* has the same computational complexity as P-RRT* and Quick-RRT*. Comparative
simulations are performed according to four benchmarks, which validate the effectiveness of the proposed algorithm.
The remaining sections of this paper are outlined as follows. Section 2 addresses the problem definition. Section
3 introduces the relevant prerequisites for the proposed algorithm. Section 4 explains the proposed algorithm. Section
5 presents analysis of completeness, optimality and computational complexity. Section 6 provides the simulation
2
results. Section 7 summarizes the main contributions and discusses some future research directions.

2. Problem definition

This section presents three motion planning problems to be solved. Let X ⊆ Rd be the configuration space, where
d ∈ N, d ≥ 2. Let Xobs ⊂ X be the obstacle region, and denote the obstacle-free space as X f ree = cl(X\Xobs ), where cl(·)
denotes the closure of a set. xinit and Xgoal are the initial configuration and the goal region, respectively. A continuous
function σ : [0, 1] 7→ X is called a path, if it has bounded variation. The path is collision-free, if σ(τ) ∈ X f ree for all
τ ∈ [0, 1].
A path planning problem is to find a collision-free path σ : [0, 1] 7→ X f ree that starts from the initial configuration
σ(0) = xinit and reaches the goal region σ(1) ∈ Xgoal and σ(τ) ∈ X f ree for all τ ∈ [0, 1]. If a path σ : [0, 1] 7→ X is
a collision-free path, σ(0) = xinit and σ(1) ∈ Xgoal , then it is called a feasible path. Given a triplet {xinit , Xobs , Xgoal },
path planning problem is to find a feasible path. Problem 1 presents the feasibility problem of path planning.
Problem 1. (Feasible Path Planning) Given a triplet {xinit , Xobs , Xgoal }, find a feasible path, if one exists. Report
failure if no such solution exists.
Let Σ denote the set of all paths, and Σ f easible is a set of all feasible paths. Let c(·) be the cost function in terms of
Euclidean distance function. Problem 2 formalizes the optimality problem of path planning.

Problem 2. (Optimal Path Planning) Given a triplet {xinit , Xobs , Xgoal } and a cost function c : Σ → R≥0 , find a feasible
path σ∗ such that c(σ∗ ) = min{c(σ) : σ ∈ Σ f easible }. Report failure if no such solution exists.
Let t ∈ R denote the time required by the algorithm to find a set of all feasible paths Σ f easible . The fast path
planning stated in Problem 3 demonstrates that optimal path solution must be found in least possible time.

Problem 3. (Fast Path Planning) Find the optimal path solution in least possible time t ∈ R.

3. Related work

This section first introduces RRT*, which forms the basis of P-RRT* and Quick-RRT* algorithms, and then it
explains P-RRT* and Quick-RRT* subsequently. The above two algorithms are the cornerstones of the proposed
PQ-RRT* algorithm.

3.1. RRT*
This section formally introduces RRT*, which is an incremental sampling-based motion planning algorithm.
RRT* guarantees asymptotic optimality, that is, an almost-sure convergence to optimal solution. Before showing
the RRT* algorithm, a brief description of RRT will be provided. The inputs of RRT consist of the initial state, the
goal state (region) and the environment. The output is a graph including a feasible path. In each iteration, a sample
xrand is selected randomly from X f ree , then the closest vertex xnearest to the sample xrand is found in terms of distance
metric. The graph verifies whether there is a feasible local path σlocal from xnearest to xrand . If so, the sample xrand and
the local path σlocal are added to the tree. The above steps are repeated until a feasible path is found or the stop criteria
are met. Next, we will briefly describe RRT*, which is shown in Algorithm 1.

3
Algorithm 1 RRT*
1: V ← {xinit }; E ← ∅;
2: for i = 1 to n do
3: xrand ← S ampleFree(i);
4: xnearest ← Nearest(V, xrand );
5: σ ← S teer(xnearest , xrand );
6: if CollisionFree(σ) then
7: Xnear ← Near(V, xrand , min{γRRT ∗ (log(card(V)))/card(V)1/d , η});
8: (x parent , σ parent ) ← ChooseParent(Xnear , xnearest , σ);
9: V ← V ∪ {xrand } ;
10: E ← E ∪ (x parent , xrand );
11: G ← Rewire(G, xrand , Xnear );
12: end if
13: end for
14: return G = (V, E);

Unlike RRT, RRT* adopts two optimisation procedures, ChooseParent and Rewire. In the ChooseParent proce-
dure, RRT* searches the Xnear , (a set of vertices in a hypersphere of a specific radius centered at xrand ), to find the
optimum. Through the optimisation, a path has the lowest cost from the root xinit to xrand . The ChooseParent proce-
dure is outlined in Algorithm 2. After adding xrand to the tree, RRT* tries to optimise the cost of the element of Xnear
through xrand . If the local path σlocal from xrand to any element of Xnear , xnear , is collision-free, and the local path σlocal
has a lower cost than the current path, then the parent of xnear is replaced with xrand . Algorithm 3 presents the Rewire
procedure. Following this, the primitive procedures involved in RRT* are briefly explained.

Algorithm 2 ChooseParent(Xnear , xnearest , xrand , σnearest )

1: xmin ← xnearest ;

2: σmin ← σnearest ;
3: cmin ← Cost(xnearest + Cost(σnearest ));
4: for each xnear ∈ Xnear do
5: σ ← S teer(xnear , xrand );
6: c ← Cost(xnear + Cost(σ));
7: if c < cmin then
8: if CollisionFree(σ) then
9: xmin ← xnear ;
10: σmin ← c;
11: end if
12: end if
13: end for
14: return (xmin , σmin );

• SampleFree: It returns a random sample from X f ree .

4
 • Nearest: It returns the closest vertex from V in the graph G = (V, E) in terms of a given distance metric. In this
paper, we use Euclidean distance function.
• Near: Given a graph G = (V, E) and a configuration x, it returns the set of neighboring vertices of the sample x.
The set is the vertices contained in a ball of a radius r centered at x.

• CollisionFree: It checks whether the local path σ lies entirely in X f ree .

• Steer: Given two configurations x s , xt ∈ X, the function returns the line segment connecting x s to xt .

3.2. P-RRT*
In this section, we show Potential Function Based-RRT* (P-RRT*), which is an extension of RRT*. The RRT*
algorithm here is not the original version, but a slightly modified one. The objective of the modification is to reduce
the number of calls to the CollisionFree procedure (Perez et al., 2011). Besides using the modified version of RRT*,
the outstanding feature of P-RRT* is its incorporation of artificial potential field (APF) into RRT*. The pseudocode
for the P-RRT* algorithm is presented in Algorithm 4. Some new procedures used in P-RRT* are presented below.

• GetTuple: Given x prand and Xnear , returns Lnear , which is a sorted data structure in ascending order according to
their costs. Each element includes three data attributes, cost, xnear and local path σlocal .
• FindBestParent: Given Lnear and x prand , it returns x parent ∈ Xnear , which is the best parent of x prand .

Algorithm 3 Rewire(G,xrand , Xnear )

1: for each xnear ∈ Xnear do
2: σ ← S teer(xrand , xnear );
3: if Cost(xrand ) + Cost(σ) < Cost(xnear ) then
4: if CollisionFree(σ) then
5: x parent ← Parent(xnear );
6: E ← (E\{(x parent , xnear )}) ∪ {(xrand , xnear )};
7: end if
8: end if
9: end for
10: return G = (V, E);

5
Algorithm 4 P-RRT*
1: V ← {xinit }; E ← ∅;
2: for i = 1 to n do
3: xrand ← S ampleFree(i);
4: x prand ← RGD(xrand );
5: Xnear ← Near(V, x prand , min{γRRT ∗ (log(card(V)))/card(V)1/d , η});
6: if Xnear = ∅ then
7: Xnear ← Nearest(V, x prand )
8: end if
9: Lnear ← GetT uple(x prand , Xnear )
10: x parent ← FindBestParent(Lnear , x prand )
11: if x parent , ∅ then
12: V ← V ∪ {x prand } ;
13: E ← E ∪ (x parent , x prand );
14: G ← Rewire(G, x prand , Lnear );
15: end if
16: end for
17: return G = (V, E);

The RGD procedure is presented in Algorithm 5 in detail. The random sample xrand is adjusted under the attractive
force of the goal region Xgoal , obtaining an improved sample x prand . The NearestObstacle procedure computes the
∗
shortest distance from x prand to the obstacle space Xobs . There are three parameters k, λ and dobs used in RGD. Here, k
∗
is a limited number of iterations and the constant dobs is a very small distance from Xobs , and λ is a small incremental
step towards the target. Obviously, these parameters need to be adjusted, but the parameter tuning is omitted here. In
∗
this paper, for P-RRT* and PQ-RRT* λ = dobs = 0.1, while k = 80.

Algorithm 5 RGD(xrand )
1: x prand ← xrand ;

2: for n = 1 to k do
3: F~att ← (xgoal − x prand );
4: dmin ← NearestObstacle(Xobs , x prand );
∗
5: if dmin ≤ dobs then
6: return x prand ;
7: else
~
8: x prand ← x prand + λ F~att ;
|Fatt |
9: end if
10: end for
11: return x prand ;

6
3.3. Quick-RRT*
Quick-RRT*, as shown in Algorithm 6, has two special adjustments. They are the ChooseParent and the Rewire
procedures. In the ChooseParent procedure associated with Quick-RRT*, the search range of the possible parent
vertices of xrand includes not only Xnear but also the ancestry of Xnear up to a predefined depth. Obviously, expanding
the search scope can better optimise the generated tree. However, expanding the search scope does not increase
the computation time significantly, for this element of Xnear usually shares a common parent. Like the ChooseParent
procedure, the Rewire procedure also considers the ancestry of the vertex xrand . The Rewire procedure of Quick-RRT*
is presented in Algorithm 7.

Algorithm 6 Quick-RRT*)
1: V ← {xinit }; E ← ∅;
2: for i = 1 to n do
3: xrand ← S ampleFree(i);
4: xnearest ← Nearest(V, xrand );
5: σ ← S teer(xnearest , xrand );
6: if CollisionFree(σ) then
7: Xnear ← Near(V, xrand , min{γRRT ∗ (log(card(V)))/card(V)1/d , η});
8: X parent ← Ancestry(G, Xnear );
9: (x parent , σ parent ) ← ChooseParent(Xnear ∪ X parent , xnearest , σ);
10: V ← V ∪ {xrand } ;
11: E ← E ∪ (x parent , xrand );
12: G ← Rewire-Q-RRT*(G,xrand ,Xnear );
13: end if
14: end for
15: return G = (V, E);

Note that there is a new procedure Ancestry. Related functions of this procedure are described below.

• ancestor: Given a graph G = (V, E), a vertex p and a constant number d ∈ N, it returns the d − th parent of p.
• Ancestry: Given a graph G = (V, E) and a vertex p, if the depth d is 0, returns ∅, otherwise returns
d
∪ ancestor(G, p, i).
i=1

7
Algorithm 7 Rewire-Q-RRT*(G,xrand ,Xnear )
1: for each xnear ∈ Xnear do
2: for each x f rom ∈ {xrand } ∪ ancestor(G, xrand ) do
3: σ ← S teer(x f rom , xnear );
4: if Cost(x f rom + Cost(σ) < Cost(xn ear)) then
5: if CollisionFree(σ) then
6: x parent ← Parent(xnear );
7: E ← E\{(x parent , xnear )} ∪ {(x f rom , xnear )};
8: end if
9: end if
10: end for
11: end for
12: return G = (V, E);

4. PQ-RRT*

This section presents the details of the proposed PQ-RRT* algorithm. In order to further accelerate the conver-
gence rate, this paper proposes the PQ-RRT* algorithm based on Quick-RRT* and P-RRT*. Algorithm 8 shows the
pseudocode for the PQ-RRT* algorithm.
PQ-RRT* has three main improvements when compared with RRT*. The first point is to change the sampling
strategy by adopting the attractive force of the target region. The second point is expanding the search scope of the
ChooseParent procedure. The search scope contains not only the neighbor Xnear but also the ancestry of the Xnear . The
last one is the improvement of the Rewire-PQ-RRT* procedure, which is similar to the second point.

Algorithm 8 PQ-RRT*
1: V ← {xinit }; E ← ∅;
2: for i = 1 to n do
3: xrand ← S ampleFree(i);
4: x prand ← RGD(xrand );
5: xnearest ← Nearest(V, x prand );
6: σ ← steer(xnearest , x prand );
7: if CollisionFree(σ) then
8: Xnear ← Near(V, x prand , min{γRRT ∗ (log(card(V)))/card(V)1/d , η});
9: X parent ← Ancestry(G, Xnear );
10: (x parent , σ parent ) ← ChooseParent(Xnear ∪ X parent , xnearest , σ);
11: V ← V ∪ {xrand } ;
12: E ← E ∪ (x parent , xrand );
13: G ← Rewire-PQ-RRT*(G,xrand ,Xnear );
14: end if
15: end for
16: return G = (V, E);

8
5. Analysis
This section presents the completeness, asymptotic optimality, fast convergence rate and computational complex-
ity of the proposed algorithm. Some symbols used in the analysis are described below. Let ALG denote the label of
the algorithms mentioned in the following. Let VnALG and EnALG be the vertices and edges in the graph GnALG generated
by an algorithm after n iterations.

5.1. Probabilistic completeness

Problem 1 is considered in this section. It is widely recognised that most sampling-based algorithms can guarantee
probabilistic completeness. The concept of probabilistic completeness is formalised below.
Definition 1. (Probabilistic completeness) Given a triplet {xinit , Xobs , Xgoal }, an algorithm ALG is said to be probabilis-
tically complete if for any robustly feasible path planning problems, limn→∞ P(VnALG ∩ Xgoal , ∅) = 1 and the graph
returned by ALG includes a path connecting the root xinit to xgoal ∈ Xgoal .
Karaman et al. have proved that RRT provides probabilistic completeness and RRT* inherits this property of RRT
(Karaman & Frazzoli, 2011). As a result, the probabilistic completeness of P-RRT* and Quick-RRT* algorithms has
been deduced (Jeong et al., 2019; Qureshi & Ayaz, 2016). The probabilistic completeness of the proposed algorithm
PQ-RRT* is stated in Theorem 1.
Theorem 1. (Probabilistic completeness of PQ-RRT*) For any given robustly feasible path planning problems
{xinit , Xobs , Xgoal }, the probability of finding a feasible solution approaches one, i.e.,

lim P(∃xgoal ∈ VnPQ−RRT ∗ ∩ Xgoal such that xinit is connected to xgoal ∈ Xgoal ) = 1.
n→∞

Proof of Theorem 1. First, we select the same samples as P-RRT*. Moreover, Quick-RRT* just changes the growth
trend of the tree but not the connectivity of the tree. Hence the PQ-RRT* provides probabilistic completeness as
P-RRT*.

5.2. Asymptotic optimality

RRT* was proposed by Karaman et al., who proved that RRT does not possess the property of asymptotic opti-
mality that is possessed by RRT*. The related definitions and concepts required to prove the asymptotic optimality of
PQ-RRT* are given in the following words.
Let η be a non-negative real number. For a state y ∈ X f ree , By,η denotes a closed ball region of radius η centered at
y. Xintη := {y ∈ X f ree |By,η ⊆ X f ree } and Xextη := X f ree \Xintη .
Definition 2. (strong η - clearance) A path is said to have strong η - clearance if and only if all points of the path
belong to Xintη .
Definition 3. (weak η - clearance) A path σ1 : [0, 1] is said to have weak η - clearance if there exists a path σ2 : [0, 1]
and function φ : [0, 1] such that φ(0) = σ1 , φ(1) = σ2 and φ(τ) is strong η - clearance for τ ∈ (0, 1].
Let YnALG be a random variable, which denotes the cost of the minimum-cost solution in the graph returned by ALG
after n iterations.
Definition 4. (Asymptotic optimality) For any path planning problems, an algorithm ALG is said to be asymptotically
optimal if ALG will return a graph including a minimum-cost solution as the number of samples tends to infinity. It
can be expressed clearly by the following formula.
!
P lim sup YnALG = L∗ = 1
n→ ∞

∗
In the above formula, L represents the cost of the optimal solution. In fact, P-RRT* introduces intelligent sam-
pling heuristic into RRT* to direct the random samples (Qureshi & Ayaz, 2016). However, the other procedures are
the same as RRT*. Therefore, P-RRT* inherits the property of asymptotic optimality. Similarly, PQ-RRT* can be
understood as an optimisation version of P-RRT*, so the proposed PQ-RRT* algorithm also possesses the asymptotic
optimality property.
9
5.3. Fast convergence to optimal solution
The proof of fast convergence requires a definition, which shows a critical property of the optimal solution.

Definition 5. (Optimal path planning) If a collision-free path has weak η - clearance, the path is said to be optimal.

Similar to the proof of asymptotic optimality, the property of fast convergence of PQ-RRT* mainly hinges on the
P-RRT* algorithm. The following theorem presents the reason why P-RRT* provides fast convergence.

Theorem 2. (Potential guided sampling heuristic

 RGD(x)) The RGD(x) heuristic guides the random samples towards
the goal region in such a manner so that P x prand ∈ Xextη > 0.
From Theorem 2 and Definition 5, a conclusion that P-RRT* has the property of fast convergence to optimal
solution is obtained. Since Quick-RRT* provides a new optimisation frame, the rest procedures of PQ-RRT* are the
same as P-RRT*, it can be concluded that PQ-RRT* inherits the property of fast convergence of P-RRT*.

5.4. Computational complexity

In this section, the computational complexity of the proposed PQ-RRT* algorithm is introduced. Let S nALG be the
number of procedures executed by ALG after n iterations. Theorem 3 presents the fact that PQ-RRT* has the same
computational complexity as P-RRT* and Quick-RRT*.
Theorem 3. There exist two constants α1 and α2 such that

SPQ−RRT∗
n
lim E[ ] ≤ α1 ,
n→∞ SP−RRT∗
n

SPQ−RRT∗
n
lim E[ ] ≤ α2 .
n→∞ SQuick−RRT∗
n

Proof of Theorem 3. Let’s first analyze the first formula. The difference between PQ-RRT* and P-RRT* is the in-
tegration of Quick-RRT*. The contributions of Quick-RRT* are the improvements on optimisation procedures, the
ChooseParent and the Rewire. Since most neighbor vertices have a common ancestor, Quick-RRT* does not greatly
increase the computational complexity. Similarly, for the second formula, RGD procedure is considered undoubtedly.
For the RGD procedure, it does not increase the number of the samples, but improves the quality of the sample. Hence
the two algorithms have the same asymptotic computational complexity.

6. Simulation results

In this section, PQ-RRT* is compared with the existing algorithms, P-RRT* and Quick-RRT*, in accordance with
four benchmarks: two 2-dimensional environments for P-RRT* and two environments for Quick-RRT*. P-RRT* and
Quick-RRT* are used for comparative analysis, since they are typical examples of fast convergence algorithms. Due
to the randomness of sampling-based algorithms, each algorithm was run 100 times.
∗
The parameters in the simulation are λ, dobs , k and the depth d. For a fair comparison, the simulation parameters
are the same for all algorithms. In the ChooseParent procedure, d = 2, and in the Rewire procedure, d = 1. Two
evaluation indicators are utilized to compare the performance of the three algorithms: ’l f ’, the length of the first found
solution, and ’T 5% ’, the time to find a solution of ’1.05 ∗ loptimal ’, where loptimal is the length of optimal solution. For
the purpose of demonstrating the better performance of the PQ-RRT* algorithm, the same random seed is used in
the SampleFree procedure in each comparison. Note that these algorithms will stop once the approximate optimal
solutions are found. The simulations are implemented on an Intel Xeon(R) E3-1240 CPU with 8G of RAM. The
simulation platform is Matlab. Four test environments are the same size, 100 · 100. The Fail value is the number of
failures. Failure means an algorithm cannot find the approximate optimal solution within 300 secs. The simulation
environments are shown in Figure 1.

10
 100 100

90 90

80 80

70 70

60 60

50 50

40 40

30 30

20 20

10 10

0 0
0 20 40 60 80 100 0 20 40 60 80 100 120

(a) 2d-1 (b) 2d-2

100 100

90 90

80 80

70 70

60 60

50 50

40 40

30 30

20 20

10 10

0 0
0 20 40 60 80 100 0 20 40 60 80 100

(c) maze-1 (d) maze-2

Figure 1: Four test environments. (a) and (b) are two-dimensional environments for P-RRT*, (c) and (d) are maze environments for Quick-RRT*.
(green star: start state, red circle: goal region)

6.1. 2d-1
Environment 2d-1 is shown in Figure 1(a). Figure 2 shows the performance of the three algorithms in environment
2d-1. In Figure 2, the generated paths of PQ-RRT*(l f = 89.2985, T 5% = 1.667), P-RRT*(l f = 96.3998, T 5% = 6.558)
and Quick-RRT*(l f = 101.7946, T 5% = 32.949) are shown. It can be seen from the results of this run that the proposed
algorithm, PQ-RRT*, has a better initial solution and a faster convergence rate than the other two algorithms. In this
environment, loptimal = 72.8829.

(a) PQ-RRT* (b) P-RRT* (c) Quick-RRT*

Figure 2: Performance of the three algorithms in the environment 2d-1.

The statistical results of 100 simulations are described by the boxplots. The unit of the ordinate of the boxplot is
11
seconds. The simulation results of l f and T 5% are shown in the Figure 3 and Figure 4 respectively. In the boxplots,
p-q denotes PQ-RRT*, p denotes P-RRT*, and q denotes Quick-RRT*. Although the median of l f for PQ-RRT* is
slightly higher than the median of l f for Quick-RRT*, PQ-RRT* is better than Quick-RRT* overall. It can be clearly
seen from Figure 4 that in the environment of 2d-1 PQ-RRT* and P-RRT* have a faster convergence rate than Quick-
RRT*. The statistics in Table 1 indicate that PQ-RRT* has better stability. Based on the above analysis, PQ-RRT*
performs better than P-RRT* and Quick-RRT* in 2d-1.

2d-1 first solution 2d-1 time

240 140

220 120

200
100
180
80
160

60
140

120 40

100 20

80
0

p-q p q p-q p q

Figure 3: l f in the 2d-1 Figure 4: T 5% in the 2d-1

6.2. 2d-2
Environment 2d-2 is shown in Fig 1(b). Figure 5 shows the performance of the three algorithms. In Figure 5,
the generated paths of PQ-RRT*(l f = 56.1574, T 5% = 0.781), P-RRT*(l f = 56.5157, T 5 % = 4.064) and Q-RRT*(l f =
54.598, T 5% = 33.031) are shown. From the results of this run, we can draw a conclusion that the proposed algorithm,
PQ-RRT*, outperforms the other two algorithms in this comparison. In the environment 2d-2, loptimal = 52.0711.

Table 1: Comparing algorithms on the quality of the initial solution and convergence rate in 2d-1(Std represents the standard deviation)

Algorithm Mean Std Min Max Fail

lf 96.8742 10.1366 76.2692 128.8751
PQ-RRT* 0
T 5% 4.3554 2.6687 0.547 14.596
lf 120.4426 15.5077 84.9359 157.7636
P-RRT* 0
T 5% 4.8352 2.9337 1.14 19.475
lf 104.0857 32.7497 73.7135 241.232
Quick-RRT* 0
T 5% 23.7181 22.5681 1.512 140.67

The statistical results of 100 simulations are shown in the boxplots. Figure 6 and Figure 7 plot the results of l f and
T 5 % respectively. It can be clearly seen from Figure 6 and Figure 7 and Table 2 that PQ-RRT* is outstanding on the
two indicators, l f and T 5 %. Based on the above analysis, PQ-RRT* performs better than P-RRT* and Quick-RRT* in
2d-2.

12
 PQ-RRT *
100

90

80

70

60

50

40

30

20

10

0
0 20 40 60 80 100

(a) PQ-RRT* (b) P-RRT* (c) Q-RRT*

Figure 5: Performance of the three algorithms in the environment 2d-2.

2d-2 first solution 2d-2 time

300
140

130 250

120
200
110

100 150

90
100
80

70
50
60
0
50
p-q p q p-q p q

Figure 6: l f in the 2d-2 Figure 7: T 5% in the 2d-2

Table 2: Comparing algorithms on the quality of the initial solution and convergence rate in 2d-2

Algorithm Mean Std Min Max Fail

lf 60.3689 3.8795 52.3068 70.9071
PQ-RRT* 0
T 5% 9.2958 7.4132 0.187 40.259
lf 65.6481 7.5297 55.095 105.559
P-RRT* 0
T 5% 11.9609 10.6067 0.619 56.207
lf 73.4725 21.052 54.1837 144.1125
Quick-RRT* 0
T 5% 40.0247 40.5819 1.51 297.421

6.3. maze-1
Environment maze-1 is shown in Figure 1(c). Figure 8 shows the performance of the three algorithms. In Figure
8, the generated paths of PQ-RRT*(l f = 243.824, T 5% = 2.849), P-RRT*(l f = 247.7364, T 5% = 4.67) and Q-RRT*(l f =
252.1599, T 5% = 4.723) are shown. As can be seen in Figure 8, PQ-RRT* can converge to the near optimal solution
more quickly and has a lower cost initial solution. In the environment maze-1, loptimal = 219.8094. PQ-RRT* requires
fewer samples and less time to obtain the near optimal solution.
The statistical results of 100 simulations are shown in the boxplots. Figure 9 and Figure 10 depict the performance
of the algorithms. It can be seen from the box-plots of Figure 9 and Figure 10 and Table 3 that PQ-RRT* is superior
to P-RRT* and Quick-RRT*. It should be pointed out that the failed experiments are not included in the calculation of
the mean, standard deviation, maximum and minimum. From the above analysis, it can be concluded that PQ-RRT*
performs better than P-RRT* and Quick-RRT* in maze-1.
13
 (a) PQ-RRT* (b) P-RRT* (c) Q-RRT*

Figure 8: Performance of the three algorithms in the environment maze-1

maze-1 first solution maze-1 time

310 500

300

290 400

280
300
270

260
200
250

240 100

230
0
220
p-q p q p-q p q

Figure 9: l f in the maze-1 Figure 10: T 5% in the maze-1

Table 3: Comparing algorithms on the quality of the initial solution and convergence rate in maze-1

Algorithm Mean Std Min Max Fail

lf 239.4538 5.3222 223.6241 248.227
PQ-RRT* 0
T 5% 9.5649 4.32 2.818 22.464
lf 244.1686 4.482 231.8931 253.6486
P-RRT* 4
T 5% 64.2541 65.9887 4.67 280.973
lf 250.4689 19.7013 226.6428 315.0957
Quick-RRT* 0
T 5% 23.6302 23.3234 2.768 162.794

6.4. maze-2
Environment maze-2 is shown in Figure 1(d). Figure 11 shows the performance of the three algorithms. In
Figure 11, the generated paths of PQ-RRT*(l f = 209.2523, T 5% = 2.346), P-RRT*(l f = 216.7866, T 5% = 14.363) and
Q-RRT*(l f = 221.9119, T 5% = 13.921) are shown. Figure 11 demonstrates that the proposed algorithm PQ-RRT*
obtains better performance. In the environment maze-2, loptimal = 204.1923.

14
 (a) PQ-RRT* (b) P-RRT* (c) Q-RRT*

Figure 11: Performance of the three algorithms in the environment maze-2.

maze-2 first solution maze-2 time

290 1600

280 1400

270 1200

260 1000

250
800

240
600
230
400
220
200
210
0

p-q p q p-q p q

Figure 12: l f in the maze-2 Figure 13: T 5% in the maze-2

The statistical results of 100 simulations are shown in the boxplots. Figure 12 and Figure 13 show the comparison
of the two indicators, l f and T 5% , respectively. In Figure 12, the median of PQ-RRT* is slightly higher than that of
Quick-RRT*, but Quick-RRT* has many abnormalities that getting the initial solutions takes a long time. In Figure
13, PQ-RRT* has a faster convergence rate than the other two algorithms. In Table 4, the last column demonstrates
PQ-RRT* performs best. Combining two performance indicators, PQ-RRT* performs best in maze-2.

Table 4: Comparing algorithms on the quality of the initial solution and convergence rate in maze-2

Algorithm Mean Std Min Max Fail

lf 213.3854 3.1562 208.4424 225.4243
PQ-RRT* 0
T 5% 6.2253 4.2351 1.583 23.938
lf 223.8193 5.0614 213.1984 238.2166
P-RRT* 16
T 5% 62.1314 64.2663 4.722 288.623
lf 214.1641 13.4942 205.3227 292.9553
Quick-RRT* 1
T 5% 30.1505 43.3519 0.493 248.213

The performance of the navigation is usually subject to the initial solution of path planning since the robot will
follow the initial solution at the beginning. The boxplots show that the proposed algorithm can generate a relatively
better solution. A better initial solution means more energy saving under the same conditions. Moreover, the near
optimal solution obtained by PQ-RRT* requires fewer nodes which make memory utilization more efficient. The
quality of a path planning algorithm depends not only on asymptotic optimality but also on the convergence rate.

15
The boxplots of the indicator T 5% show that PQ-RRT* has a fast convergence to an optimal solution compared with
P-RRT* and Quick-RRT*. From comparisons with P-RRT* and Quick-RRT*, we draw a conclusion that PQ-RRT*
performs best overall. PQ-RRT* generates a better initial solution and a fast convergence rate.

7. Conclusion

There has been a great deal of recent research on sampling-based path planning algorithms. RRT* is an optimal
algorithm, but it has a slow rate of convergence. In order to address this problem, this paper proposed an improved
RRT* algorithm, PQ-RRT*. This paper proves that the proposed algorithm is complete, asymptotically optimal and
provides fast convergence to optimal solution. Moreover, PQ-RRT* has the same computational complexity.
Although PQ-RRT* is a promising algorithm, it also has its limitations. When applying PQ-RRT* to mobile
robots, the kinematic constraints of the robot must be considered. Another point is that the parameters in the proposed
algorithm may not perform best in other complex environments. Therefore, in our future proceedings, we will consider
the kinematic constraints and explore an adaptive parameter method for the proposed algorithm. The flexibility of the
robot arm can carry out more complex tasks, so applying PQ-RRT* to the robot arm is a potential direction. Static
path planning is mostly studied at the present stage, but the actual environments are dynamic, so it is necessary to
study the performance of PQ-RRT* in dynamic path planning.

Credit authorship contribution statement

All authors contributed to this work. Conceptualization, Y.L. and W.W.; methodology, Y.L. and W.W.; software,
Y.L., Y.G. and D.W.; validation, Y.G., D.W.; writing-original draft, Y.L.; writing-review and editing, Y.L., W.W. and
Z.F.

Conflict of interest

Authors declare that they have no conflict of interest.

Funding

This work was supported by the National Natural Science Foundation of China [grant number 61573148]; the Sci-
ence and Technology Planning Project of Guangdong Province [grant numbers 2015B010919007, 2016A040403012,
180917144960530]; the Science and Technology Planning Project of Guangzhou [grant number 201604016014]; and
the Project of Educational Commission of Guangdong Province [grant number 2017KZDXM032].

References
Adiyatov, O., & Varol, H. A. (2013). Rapidly-exploring random tree based memory efficient motion planning. In 2013 IEEE International
Conference on Mechatronics and Automation (pp. 354–359). IEEE.
Akgun, B., & Stilman, M. (2011). Sampling heuristics for optimal motion planning in high dimensions. In 2011 IEEE/RSJ International Conference
on Intelligent Robots and Systems (pp. 2640–2645). IEEE.
Alexopoulos, C., & Griffin, P. M. (1992). Path planning for a mobile robot. IEEE Transactions on systems, man, and cybernetics, 22, 318–322.
Amato, N. M., & Wu, Y. (1996). A randomized roadmap method for path and manipulation planning. In Proceedings of IEEE international
conference on robotics and automation (pp. 113–120). IEEE volume 1.
Asano, T., Asano, T., Guibas, L., Hershberger, J., & Imai, H. (1985). Visibility-polygon search and euclidean shortest paths. In 26th Annual
Symposium on Foundations of Computer Science (sfcs 1985) (pp. 155–164). IEEE.
Atanacio-Jiménez, G., González-Barbosa, J.-J., Hurtado-Ramos, J. B., Ornelas-Rodrı́guez, F. J., Jiménez-Hernández, H., Garcı́a-Ramirez, T., &
González-Barbosa, R. (2011). Lidar velodyne hdl-64e calibration using pattern planes. International Journal of Advanced Robotic Systems, 8,
59.
Beyer, T., Jazdi, N., Göhner, P., & Yousefifar, R. (2015). Knowledge-based planning and adaptation of industrial automation systems. In 2015
IEEE 20th Conference on Emerging Technologies & Factory Automation (ETFA) (pp. 1–4). IEEE.
Brooks, R. A., & Lozano-Perez, T. (1985). A subdivision algorithm in configuration space for findpath with rotation. IEEE Transactions on
Systems, Man, and Cybernetics, (pp. 224–233).
Ferguson, D., & Stentz, A. (2006). Anytime RRTs. In 2006 IEEE/RSJ International Conference on Intelligent Robots and Systems (pp. 5369–5375).
IEEE.

16
Gammell, J. D., Srinivasa, S. S., & Barfoot, T. D. (2014). Informed RRT*: Optimal sampling-based path planning focused via direct sampling of
an admissible ellipsoidal heuristic. In 2014 IEEE/RSJ International Conference on Intelligent Robots and Systems (pp. 2997–3004). IEEE.
González, D., Pérez, J., Milanés, V., & Nashashibi, F. (2015). A review of motion planning techniques for automated vehicles. IEEE Transactions
on Intelligent Transportation Systems, 17, 1135–1145.
Hart, P. E., Nilsson, N. J., & Raphael, B. (1968). A formal basis for the heuristic determination of minimum cost paths. IEEE transactions on
Systems Science and Cybernetics, 4, 100–107.
Huang, Y., Li, Z., Jiang, Y., & Cheng, L. (2019). Cooperative path planning for multiple mobile robots via hafsa and an expansion logic strategy.
Applied Sciences, 9, 672.
Islam, F., Nasir, J., Malik, U., Ayaz, Y., & Hasan, O. (2012). RRT*-Smart: Rapid convergence implementation of RRT towards optimal solution.
In 2012 IEEE International Conference on Mechatronics and Automation (pp. 1651–1656). IEEE.
Jeong, I.-B., Lee, S.-J., & Kim, J.-H. (2019). Quick-RRT*: Triangular inequality-based implementation of RRT* with improved initial solution
and convergence rate. Expert Systems with Applications, 123, 82–90.
Karaman, S., & Frazzoli, E. (2011). Sampling-based algorithms for optimal motion planning. The international journal of robotics research, 30,
846–894.
Kavraki, L., & Latombe, J.-C. (1994). Randomized preprocessing of configuration space for path planning: Articulated robots. In Proceedings of
IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS’94) (pp. 1764–1771). IEEE volume 3.
Kavraki, L. E., Svestka, P., Latombe, J.-C., & Overmars, M. H. (1996). Probabilistic roadmaps for path planning in high-dimensional configuration
spaces. IEEE transactions on Robotics and Automation, 12, 566–580.
Khatib, O. (1986). Real-time obstacle avoidance for manipulators and mobile robots. In Autonomous robot vehicles (pp. 396–404). Springer.
Koenig, S., Likhachev, M., & Furcy, D. (2004). Lifelong planning A*. Artificial Intelligence, 155, 93–146.
Koren, Y., & Borenstein, J. (1991). Potential field methods and their inherent limitations for mobile robot navigation. In Proceedings. 1991 IEEE
International Conference on Robotics and Automation (pp. 1398–1404). IEEE.
Kuffner, J. J., & LaValle, S. M. (2000). RRT-connect: An efficient approach to single-query path planning. In Proceedings 2000 ICRA. Millennium
Conference. IEEE International Conference on Robotics and Automation. Symposia Proceedings (Cat. No. 00CH37065) (pp. 995–1001). IEEE
volume 2.
LaValle, S. M. (1998). Rapidly-exploring random trees: A new tool for path planning, .
Lee, H.-Y., Shin, H., & Chae, J. (2018). Path planning for mobile agents using a genetic algorithm with a direction guided factor. Electronics, 7,
212.
Liu, Y., & Badler, N. I. (2003). Real-time reach planning for animated characters using hardware acceleration. In Proceedings 11th IEEE
International Workshop on Program Comprehension (pp. 86–93). IEEE.
Maekawa, T., Noda, T., Tamura, S., Ozaki, T., & Machida, K.-i. (2010). Curvature continuous path generation for autonomous vehicle using
b-spline curves. Computer-Aided Design, 42, 350–359.
Majeed, A., & Lee, S. (2019). A new coverage flight path planning algorithm based on footprint sweep fitting for unmanned aerial vehicle
navigation in urban environments. Applied Sciences, 9, 1470.
Pandini, M. M., Spacek, A. D., Neto, J. M., & Junior, O. H. A. (2017). Design of a didatic workbench of industrial automation systems for
engineering education. IEEE Latin America Transactions, 15, 1384–1391.
Perez, A., Karaman, S., Shkolnik, A., Frazzoli, E., Teller, S., & Walter, M. R. (2011). Asymptotically-optimal path planning for manipulation
using incremental sampling-based algorithms. In 2011 IEEE/RSJ International Conference on Intelligent Robots and Systems (pp. 4307–4313).
IEEE.
Qureshi, A. H., & Ayaz, Y. (2016). Potential functions based sampling heuristic for optimal path planning. Autonomous Robots, 40, 1079–1093.
Stentz, A. (1997). Optimal and efficient path planning for partially known environments. In Intelligent Unmanned Ground Vehicles (pp. 203–220).
Springer.
Taylor, R. H., Menciassi, A., Fichtinger, G., Fiorini, P., & Dario, P. (2016). Medical robotics and computer-integrated surgery. In Springer handbook
of robotics (pp. 1657–1684). Springer.
Valencia-Garcia, R., Martinez-Béjar, R., & Gasparetto, A. (2005). An intelligent framework for simulating robot-assisted surgical operations.
Expert Systems with Applications, 28, 425–433.

17