# Improved RRT-Connect Algorithm Based on Triangular Inequality for Robot Path Planning

^{1}

^{2}

^{*}

## Abstract

**:**

## 1. Introduction

_{start}and goal points are q

_{goal}, respectively. The RRT algorithm in Figure 1a shows that the path is expanded in a tree structure and the RRT-Connect algorithm in Figure 1b shows that the trees that are expanded at the start and goal points attract and connect each other. The proposed algorithm in Figure 1c shows that the RRT-Connect algorithm was rewired into a triangular inequality during path planning.

## 2. Rapidly Exploring Random Tree (RRT) Algorithm

_{new}is created at the node position q

_{near}of the tree T nearest to the random sample position q

_{rand}. Figure 2b shows the resultant path R among several candidate paths to the start position q

_{start}and the goal position q

_{goal}.

## 3. RRT-Connect Algorithm

#### 3.1. Pseudocode of the RRT-Connect Algorithm

_{a}and T

_{b}have q

_{start}and q

_{goal}as root nodes and these two trees randomly sample N times and aim to reach each other during their expansion. Unlike RRT, the RRT-Connect algorithm is divided into two methods: ‘Extend’ and ‘Connect’. The ‘Extend’ method (A2) creates q

_{new}from q

_{rand}in T

_{a}and extends from T

_{b}to the q

_{new}direction of T

_{a}, and the ‘Connect’ method (A3) determines whether the two trees T

_{a}and T

_{b}have reached each other; if they do, merge them into one tree to obtain a path P

_{reach}between the root nodes q

_{start}and q

_{goal}of the two trees.

Algorithm 1 Pseudocode of the RRT-Connect Algorithm | |

Input:q _{start} ← Start Point Positionq _{goal} ← Goal Point Positionλ ← Step Length C ← Position Set of All Boundary Points in All Obstacles Ν ← Number of Random Samples Output:R ← Result of Path R Initialize:T _{a} ← Null TreeT _{b} ← Null Treed _{shorter} ← 0 | |

BeginRRT-ConnectProcedure | |

1 | T_{a} ← Insert Root Node<q_{start}> to T_{a} |

2 | T_{b} ← Insert Root Node<q_{goal}> to T_{b} |

3 | While 1 ← n to N do |

4 | Generate n-th Random Sample |

5 | q_{rand} ← Position of n-th Random Sample |

6 | If Not Extend(T_{a}, T_{b}, q_{newB} ← Null, q_{rand}, λ, C) then |

7 | If Connect(P_{reach} ← Null Path, T_{a}, T_{b}, q_{newB}, λ) then |

8 | d_{reach} ← Distance of P_{reach} |

9 | If d_{shorter} = 0 or d_{shorter} > d_{reach} then |

10 | R ← P_{reach} |

11 | d_{shorter} ← d_{reach} |

12 | Swap(T_{a}, T_{b}) |

EndRRT-ConnectProcedure |

_{reach}is calculated for the path P

_{reach}from q

_{start}to q

_{goal}. At this time, if d

_{reach}is smaller than d

_{shorter}(the shortest path length until now) or P

_{reach}is the first path found (i.e., d

_{shorter}= 0), the resultant path R becomes P

_{reach}, and d

_{shorter}becomes d

_{reach}. At the end of the next N sampling, R becomes the final planned path. If the number of random sampling remains, the above process is repeated.

#### 3.2. Pseudocode of the Extend Method from the RRT-Connect Algorithm

_{rand}is inside a circle (or n-sphere) with the node position q

_{near}of the tree T

_{a}nearest the q

_{rand}position as the center and λ as the radius. If it is not located inside (False), q

_{newA}becomes the intersection of the circle (or n-sphere) with q

_{near}as the center and λ as the radius, and the line segment connecting q

_{rand}and q

_{near}. If it is determined that there is no obstacle between q

_{newA}and q

_{near}by the isTrapped function (False), q

_{newA}is inserted into the tree as a child node of q

_{near}of T

_{a}. If there is an obstacle (True), the ‘Extend’ method returns True (f

_{trap}) and terminates. Otherwise, it proceeds with the remaining process and returns False (f

_{trap}) when the process ends.

Algorithm 2 Pseudocode of the original ‘Extend’ method from the RRT-Connect Algorithm | |

Input:T _{a} ← Tree T_{a} from RRT-ConnectT _{b} ← Tree T_{b} from RRT-Connectq _{newB} ← Position q_{newB} from RRT-Connectq _{rand} ← Position q_{rand} from RRT-Connectλ ← Step Length λ from RRT-Connect C ← Position Set C from RRT-Connect Output:f _{trap} ← Result of Boolean f_{trap}T _{a} ← Result of Tree T_{a} //Return by ReferenceT _{b} ← Result of Tree T_{b} //Return by Referenceq _{newB} ← Result of Position q_{newB} //Return by ReferenceInitialize:f _{trap} ← False | |

Begin ExtendProcedure fromRRT-Connect | |

1 | q_{near} ← Find Position of Nearest Node in T_{a} from q_{rand} |

2 | If NotisInside(q_{near}, q_{rand}, λ) then |

3 | q_{newA} ← Position of the Intersection Point between the Line Segment connecting q_{rand} and q_{near} and a Circle with Radius λ centered at q_{near} //2D: Circle, 3D: Sphere, … |

4 | Else |

5 | q_{newA} ← q_{rand} |

6 | IfisTrapped(q_{newA}, q_{near}, C) then |

7 | f_{trap} ← True |

8 | Else |

9 | T_{a} ← Insert Node<q_{newA}> and Edge<q_{newA}, q_{near}> to T_{a} |

10 | q_{near} ← Find Position of Nearest Node in T_{b} from q_{newA} |

11 | If isInside(q_{near}, q_{newA}, λ) then |

12 | q_{newB} ← q_{near} |

13 | Else |

14 | q_{newB} ← Position of Intersection Point between Line Segment connecting q_{newA} and q_{near}, and Circle with Radius λ centered at q_{near} //2D: Circle, 3D: Sphere, … |

15 | While Not isTrapped(q_{newB}, q_{near}, C) do |

16 | T_{b} ← Insert Node<q_{newB}> and Edge<q_{newB}, q_{near}> to T_{b} |

17 | If Not isInside(q_{newA}, q_{newB}, λ) then |

18 | q_{near} ← q_{newB} |

19 | q_{newB} ← Position of Intersection Point between Line Segment connecting q_{newA} and q_{near}, and Circle with Radius λ centered at q_{near} //2D: Circle, 3D: Sphere, … |

20 | Else |

21 | Break |

EndExtendProcedure fromRRT-Connect |

_{a}and T

_{b}reach each other: First, the node T

_{b}nearest to q

_{newA}becomes the new q

_{near}. At this time, using the isInside function, it is determined whether q

_{newA}is inside a circle (or n-sphere) with q

_{near}as the center and λ as the radius, and if it is located inside (True), q

_{newB}becomes q

_{near}and is located inside. If not (False), q

_{newB}becomes the intersection of the circle (or n-sphere) with q

_{near}as the center and λ as the radius and the line segment connecting q

_{newA}and q

_{near}. If q

_{newB}is created, then the following process is repeated until it can determine whether there is an obstacle between q

_{newB}and q

_{near}by the isTrapped function and if there is the obstacle between them (True) or if q

_{newB}reaches q

_{newA}by the isInside function.

_{newB}and q

_{near}(False), insert q

_{newB}into T

_{b}as a child node of q

_{near}. At this time, if the isInside function determines that q

_{newB}has not reached the λ radius with q

_{newA}as the center (False), q

_{near}becomes q

_{newB}and a new q

_{newB}will created from this q

_{near}.

_{newA}is created, and q

_{newB}is created with radius of length λ in the direction of q

_{newA}from the q

_{near}position in the figure. Clearly, T

_{b}extends in the T

_{a}direction for reach.

#### 3.3. Pseudocode of the Connect Method from the RRT-Connect Algorithm

_{a}, T

_{b}, and q

_{newB}are from the ‘Extend’ method (A2).

Algorithm 3 Pseudocode of the Original ‘Connect’ Method from the RRT-Connect Algorithm | |

Input:P _{reach} ← Path P_{reach} from RRT-ConnectT _{a} ← Tree T_{a} from RRT-ConnectT _{b} ← Tree T_{b} from RRT-Connectq _{newB} ← Position q_{newB} from RRT-Connectλ ← Step Length λ from RRT-Connect Output:f _{reach} ← Result of Boolean f_{reach}P _{reach} ← Result of Path P_{merged} //Return by ReferenceInitialize:f _{reach} ← False | |

BeginConnectProcedure fromRRT-Connect | |

1 | IfisInside(q_{newA}, q_{newB}, λ) then |

2 | P_{a} ← Path from Root Node [q_{start}] to Last Inserted Node [q_{newA}] in T_{a} |

3 | P_{b} ← Path from q_{newB} to Root Node [q_{goal}] in T_{b} |

4 | P_{connect} ← Path from Last Inserted Node [q_{newA}] in T_{a} to q_{newB} in T_{b} |

5 | P_{merged} ← Merge Path P_{a} to P_{b} via P_{connect} |

6 | f_{reach} ← True |

EndConnectProcedure fromRRT-Connect |

_{a}from the root node (q

_{start}) of T

_{a}to the last inserted node (q

_{newA}), and a path P

_{b}from q

_{newB}of T

_{b}to the root node (q

_{goal}). Then, create a path P

_{connect}from q

_{newB}of P

_{b}to the last inserted node (q

_{newA}) of T

_{a}and merge in the order of P

_{a}, P

_{connect}, and P

_{b}, thereby completing planning the path P

_{merged}from q

_{start}to q

_{goal}. After this, it returns True (f

_{trap}), and the ‘Connect’ method ends.

_{newB}of T

_{b}is extended in the direction of the q

_{newA}by the ‘Extend’ method shown in Figure 3, the point where the two trees merge (when q

_{newB}has expanded in the direction of q

_{newA}where T

_{a}enters the λ radius centered at q

_{newA}) with each other is the part marked as ‘Connect’. As a result, the path P

_{a}becomes from the position q

_{start}to the position q

_{newA}in T

_{a}, the path P

_{connect}goes from position q

_{newA}to position q

_{newB}and the path P

_{b}goes from position q

_{newB}to position q

_{goal}in T

_{b}. The merged path P

_{merged}goes from q

_{start}to q

_{goal}.

## 4. Proposed Triangular Inequality-Based RRT-Connect Algorithm

- There is only one start point and one goal point even though the goal point may be changed incrementally as time goes on.
- The robot is capable of omnidirectional motion.

#### 4.1. Pseudocode of the Proposed Triangular-Rewiring Method for the Improved RRT-Connect Algorithm

_{a}or T

_{b}in the ‘Extend’ method (A5), when a tree T

_{merged}(P

_{merged}) in which T

_{a}and T

_{b}trees are merged in the ‘Connect’ method is created (A6), rewiring is performed on the tree T.

Algorithm 4 Pseudocode of the Proposed ‘Triangular-Rewiring’ Method for the RRT-Connect Algorithm | |

Input:q _{child} ← Position {q_{new}/q_{newA}/q_{newB}} from {Extend/Connect}q _{parent} ← Position q_{near} from {Extend/Connect}T ← Tree {T _{merged}/T_{a}/T_{b}} from {Extend/Connect}C ← Position Set C from {Extend/Connect} Output:{T _{merged}/T_{a}/T_{b}} ← Result of T | |

BegintriangularRewiringProcedure fromExtend, Connect | |

1 | q_{ancestor} ← Position of Parent Node of q_{parent} in T |

2 | If NotisTrapped(q_{ancestor}, q_{child}, C) then |

3 | T ← Delete Node<q_{parent}>, Edge<q_{parent}, q_{child}> and Edge<q_{parent}, q_{ancestor}> from T |

4 | q_{parent} ← q_{ancestor} |

5 | q_{ancestor} ← Position of Parent Node of q_{ancestor} in T |

6 | While Not q_{ancestor} = Null do |

7 | If Not isTrapped(q_{ancestor}, q_{child}, C) then |

8 | T ← Delete Node<q_{parent}> and Edge<q_{parent}, q_{ancestor}> from T |

9 | q_{parent} ← q_{ancestor} |

10 | q_{ancestor} ← Position of Parent Node of q_{ancestor} in T |

11 | Else |

12 | Break |

13 | T ← Insert Edge<q_{parent}, q_{child}> to T |

14 | Else |

15 | T ← Insert Node<q_{child}> and Edge<q_{child}, q_{parent}> to T |

EndtriangularRewiringProcedure fromExtend, Connect |

_{new}(or q

_{newA}or q

_{newB}) is inserted as a q

_{child}and q

_{near}is inserted as a candidate for the node’s parent node. From q

_{parent}, the node’s parent node (a second ancestor node candidate based on q

_{child}) is called q

_{ancestor}. Next, it is determined whether an obstacle exists between q

_{ancestor}and q

_{child}(using the isTrapped function). If there is an obstacle (True), the ‘Triangular-Rewiring’ process is skipped and q

_{child}is inserted into the child node of q

_{parent}in T such that the contents of the ‘Extend’ and ‘Connect’ methods from the RRT-Connect algorithm are the same. If there is no obstacle (False), the ‘Triangular-Rewiring’ process proceeds.

_{parent}and the edges between q

_{child}and q

_{ancestor}nodes connected to q

_{parent}. In other words, it disconnects the existing q

_{parent}and q

_{child}and prepares to connect q

_{child}to q

_{ancestor}, the candidate parent node of q

_{child}. Again, q

_{parent}becomes its parent node q

_{ancestor}and q

_{ancestor}becomes the parent node of q

_{ancestor}. Then, as previously done, determine whether an obstacle exists between q

_{ancestor}and q

_{child}(using the isTrapped function). This iterative process continues until no q

_{ancestor}exists (when no parent node exists for the previous q

_{ancestor}, i.e., when q

_{ancestor}is q

_{start}) or an obstacle exists between q

_{child}and q

_{ancestor}. Then, in tree T, the last created q

_{parent}is inserted as the parent node of q

_{child}.

#### 4.2. Mathematical Modeling of the Proposed Triangular Inequality-Based RRT-Connect Algorithm

_{i}and its parent node in the RRT algorithm

_{i}refers to the i-th inserted arbitrary node and takes the x and y coordinate values of the node as an element. The ξ function receives an arbitrary node as a variable and returns the parent node of this node. Equation (1) obtains the distance between an arbitrary node q

_{i}and its parent node, which can be summarized as a function ${\mathrm{\mathbb{d}}}_{n}$ as in Equation (2). Here, n is the distance between the ancestor node and its parent node, based on an arbitrary node. That is, the ξ function to the power of n (n ≥ 0) can be represented as ${\xi}^{n}\left({q}_{i}\right):=\stackrel{n}{\left(\stackrel{\u23de}{\xi \circ \xi \circ \dots \circ \xi}\right)}\left({q}_{i}\right)$; when n is 0, ${\xi}^{0}\left({q}_{i}\right):={q}_{i}$ holds.

_{i}and going back to the parent node to find the distance between the n-th ancestor node and the (n + 1)-th ancestor node; this can be represented as $D\left({\xi}^{n}\left({q}_{i}\right),{\xi}^{n+1}\left({q}_{i}\right)\right)$.

_{start}to the goal position q

_{goal}by the RRT algorithm

_{goal}is q

_{start}, where δ is the upper limit of $\sum}_{n=0}^{\delta}{\mathrm{\mathbb{d}}}_{n}\left({q}_{goal}\right)$ for obtaining the path length ${\mathbb{D}}_{R}$ in Equation (4). In other words, Equation (4) is the sum of the distances from q

_{goal}to the first ancestor node (parent node) of q

_{goal}and the distance from the first ancestor node (parent node) of q

_{goal}to the second ancestor of q

_{goal}, …, and (δ − 1)-th ancestor node to the δ-th ancestor node (q

_{start}).

_{child}is q

_{parent}, the parent node of q

_{parent}is q

_{ancestor}, and q

_{ancestor}is the second ancestor of q

_{child}, this can be represented as Equation (5):

_{child}and q

_{parent}, the β between q

_{parent}and q

_{ancestor}, and the γ between q

_{child}and q

_{ancestor}is as shown in Figure 5c, this can be represented as Equation (6) using the principle of the triangular inequality

_{child}

_{child}, with the distance as Equation (1) in Equation (6), which represents the distance between each node as a triangular inequality.

_{j}to compare the distance $\mathrm{\mathbb{u}}$ when applying the ‘Triangular-Rewiring’ method with distance $\mathrm{\mathbb{d}}$ when this method is not applied

_{j}can be considered a sequence index for $\mathrm{\mathbb{d}}$. Currently, ${\tau}_{j}$ is the number of times that rewiring occurs in the j-th.

_{i}, it is as Equation (10). For example, as shown in Figure 5, if j is 0 and 1 a rewire occurs (${\tau}_{0}$ = 1), it can be represented in combination with the distance relationship of Equation (8) for q

_{child}, as in Equation (11)

_{i}, the path length $\sum}_{n=j}^{{k}_{j}}{\mathrm{\mathbb{d}}}_{n$ from the j-th to k

_{j}-th arbitrary sequence index is always longer or equal to the distance ${\mathrm{\mathbb{u}}}_{{k}_{j}}$ of the k

_{j}-th sequence index. That is, in an arbitrary path, it can be confirmed that the distance $\mathrm{\mathbb{u}}$ rewired by the ‘Triangular-Rewiring’ method is at least equal (if the distances of $\mathrm{\mathbb{d}}$ and $\mathrm{\mathbb{u}}$ are the same, the rewired line segments are on a straight line) or always shorter than $\mathrm{\mathbb{d}}$ when not rewired.

_{start}to q

_{goal}based on Equations (5)–(12) (at this time, it is assumed that the node of the path shown in the figure is not positioned in a straight line). As shown in Figure 6b, a total of two rewires occurred (${\tau}_{0}=2$) between q

_{0}and q

_{3}(${\xi}^{3}\left({q}_{0}\right)$), and a total of one rewire occurred (${\tau}_{3}=1$) between q

_{5}(${\xi}^{5}\left({q}_{0}\right)$) and q

_{7}(${\xi}^{7}\left({q}_{0}\right)$). In that case, as shown in Figure 6e, k

_{0}is 2, k

_{1}is 3, k

_{2}is 4, and k

_{3}is 6 according to Equation (9).

_{start}, goal position: q

_{goal}), as shown in Figure 6

_{j}, if k

_{j}is δ, j becomes φ. In that case, as in Equation (14), ${\mathbb{D}}_{R}$ is used to compare the $\sum}_{n=0}^{\delta}{\mathrm{\mathbb{d}}}_{n}\left({q}_{goal}\right)$ shown in Equation (4) with ${\mathbb{U}}_{R}$, reflecting the sequence k

_{j}. It can be represented as $\sum}_{j=0}^{\phi}{\displaystyle \sum}_{n={{k}^{\prime}}_{j}}^{{k}_{j}}{\mathrm{\mathbb{d}}}_{n}\left({q}_{goal}\right)$, and ${\mathbb{U}}_{R}$ can be represented as $\sum}_{j=0}^{\phi}{\mathrm{\mathbb{u}}}_{{k}_{j}}\left({q}_{goal}\right)$.

_{start}, goal position: q

_{goal}) is at least equal (If the distances of $\mathbb{D}$ and $\mathbb{U}$ are the same, when the rewired line segments are on a straight line) to or always shorter than ${\mathbb{D}}_{R}$; as a result, this method is not applied.

_{a}to the last (inserted node) position q

_{newA}and the path length ${\mathbb{U}}_{A}$ when the ‘Triangular-Rewiring’ method has been applied to the path. In addition, it shows that ${\mathbb{U}}_{A}$ is at least equal to or always shorter than ${\mathbb{D}}_{A}$:

_{b}to the last (inserted node) position q

_{newB}and the path length ${\mathbb{U}}_{B}$ when the ‘Triangular-Rewiring’ method has been applied to the path. In addition, it shows that ${\mathbb{U}}_{B}$ is at least equal to or always shorter than ${\mathbb{D}}_{B}$:

_{a}(Equation (17)), the distance ${\mathbb{D}}_{B}$ of the partial path P

_{b}(Equation (20)), and the distance $D\left({q}_{newA},{q}_{newB}\right)$ between q

_{newA}and q

_{newB}as shown in Equation (22).

_{a}for the RRT-Connect (Equation (17)), the distance ${\mathbb{U}}_{B}$ of the partial path P

_{b}(Equation (20)), and the distance $D\left({q}_{newA},{q}_{newB}\right)$ between q

_{newA}and q

_{newB}as shown in Equation (22).

#### 4.3. Pseudocode of Proposed Extend Method for the Improved RRT-Connect Algorithm

Algorithm 5 Pseudocode of the Proposed ‘Extend’ Method for the RRT-Connect Algorithm | |

Input:T _{a} ← Tree T_{a} from RRT-ConnectT _{b} ← Tree T_{b} from RRT-Connectq _{newB} ← Position q_{newB} from RRT-Connectq _{rand} ← Position q_{rand} from RRT-Connectλ ← Step Length λ from RRT-Connect C ← Position Set C from RRT-Connect Output:f _{trap} ← Result of Boolean f_{trap}T _{a} ← Result of Tree T_{a} //Return by ReferenceT _{b} ← Result of Tree T_{b} //Return by Referenceq _{newB} ← Result of Position q_{newB} //Return by ReferenceInitialize:f _{trap} ← False | |

BeginExtendProcedure fromRRT-Connect | |

1 | q_{near} ← Find Position of Nearest Node in T_{a} from q_{rand} |

2 | If NotisInside(q_{near}, q_{rand}, λ) then |

3 | q_{newA} ← Position of Intersection Point between Line Segment connecting q_{rand} and q_{near}, and Circle with Radius λ centered at q_{near} //2D: Circle, 3D: Sphere, … |

4 | Else |

5 | q_{newA} ← q_{rand} |

6 | IfisTrapped(q_{newA}, q_{near}, C) then |

7 | f_{trap} ← True |

8 | Else |

9 | T_{a} ← triangularRewiring(q_{newA}, q_{near}, T_{a}, C) |

10 | q_{near} ← Find Position of Nearest Node in T_{b} from q_{newA} |

11 | If isInside(q_{near}, q_{newA}, λ) then |

12 | q_{newB} ← q_{near} |

13 | Else |

14 | q_{newB} ← Position of Intersection Point between Line Segment connecting q_{newA} and q_{near}, and Circle with Radius λ centered at q_{near} //2D: Circle, 3D: Sphere, … |

15 | While Not isTrapped(q_{newB}, q_{near}, C) do |

16 | T_{b} ← triangularRewiring(q_{newB}, q_{near}, T_{b}, C) |

17 | If Not isInside(q_{newA}, q_{newB}, λ) then |

18 | q_{near} ← q_{newB} |

19 | q_{newB} ← Position of Intersection Point between Line Segment connecting q_{newA} and q_{near}, and Circle with Radius λ centered at q_{near} //2D: Circle, 3D: Sphere, … |

20 | Else |

21 | Break |

EndExtendProcedure fromRRT-Connect |

_{a}, q

_{newA}and q

_{start}are rewired and q

_{near}and q

_{goal}, and q

_{newB}and q

_{goal}are rewired sequentially in the process of extending from T

_{b}to T

_{a}.

#### 4.4. Pseudocode of the Proposed Connect Method for the RRT-Connect Algorithm

Algorithm 6 Pseudocode of the Proposed ‘Connect’ Method for the RRT-Connect Algorithm | |

Input:P _{reach} ← Path P_{reach} from RRT-ConnectT _{a} ← Tree T_{a} from RRT-ConnectT _{b} ← Tree T_{b} from RRT-Connectq _{newB} ← Position q_{newB} from RRT-Connectλ ← Step Length λ from RRT-Connect Output:f _{reach} ← Result of Boolean f_{reach}P _{reach} ← Result of Path P_{merged} //Return by ReferenceInitialize:f _{reach} ← False | |

BeginConnectProcedure fromRRT-Connect | |

1 | IfisInside(q_{newA}, q_{newB}, λ) then |

2 | P_{a} ← Path from Root Node [q_{start}] to Last Inserted Node [q_{newA}] in T_{a} |

3 | P_{b} ← Path from q_{newB} to Root Node [q_{goal}] in T_{b} |

4 | P_{connect} ← Path from Last Inserted Node [q_{newA}] in T_{a} to q_{newB} in T_{b} |

5 | T_{merged} ← Tree Structure with Merge Path P_{a} to P_{b} via P_{connect}//1st Insert: q_{start}, …, n-th Insert: q_{newA}, (n + 1)-th Insert: q_{newB}, …, Last Insert: q_{goal} to T_{merged} |

6 | For i ← Inserted Index of q_{newA} in T_{merged} to (Number of Node in T_{merged}) – 1 do |

7 | q_{new} ← (i – 1)-th Inserted Node in T_{merged} |

8 | q_{near} ← i-th Inserted Node in T_{merged} |

9 | T_{merged} ← triangularRewiring(q_{new}, q_{near}, T_{merged}, C) |

10 | P_{merged} ← Path from Root Node [q_{start}] to Last Inserted Node [q_{goal}] in T_{merged} |

11 | f_{reach} ← True |

EndConnectProcedure fromRRT-Connect |

_{a}and P

_{b}merge in a tree structure of line 5, nodes on the path are inserted in the order of P

_{a}, P

_{connect}, and P

_{b}in the merged tree T

_{merged}. That is, in T

_{merged}, the root node becomes q

_{start}, and when the n-th inserted node at a certain point is q

_{newA}, which is the last inserted node of T

_{a}, the (n + 1)-th inserted node becomes q

_{newB}, which is the last inserted node of T

_{b}. In addition, the last inserted node of T

_{merged}becomes q

_{goal}.

_{merged}. Since it is applied to the tree itself, it determines whether rewiring is possible for all nodes inserted in the tree, and rewires and updates the tree if possible. However, since each node from T

_{a}to T

_{b}is inserted into T

_{merged}, it is not necessary to rewire T

_{a}for which the ‘Triangular-Rewiring’ process has already been performed. Therefore, the ‘Triangular-Rewiring’ process proceeds in the direction of T

_{b}from the q

_{newA}sequence inserted in T

_{merged}. Here, if q

_{newA}is the i-th inserted node, the first node pair to be determined is the (i − 1)-th node q

_{new}(as q

_{child}) and i-th node q

_{near}(as q

_{parent}). When all nodes inserted in T

_{merged}have been determined, the tree structure T

_{merged}is converted into the path P

_{merged}and the method terminates (True).

_{a}and P

_{b}created from the trees T

_{a}and T

_{b}are merged and the ‘Triangular-Rewiring’ method has been applied (assuming there is no obstacle between q

_{start}and q

_{goal}), the result is P

_{merged}in which q

_{start}and q

_{goal}are connected with a straight line.

#### 4.5. Process of the Proposed Triangular Inequality-Based RRT-Connect Algorithm

_{start}to the goal position q

_{goal}through the proposed algorithm, as shown in Figure 9a.

_{rand}and q

_{newA}is created at a position separated by the length of λ from q

_{start}in the direction of the position, and q

_{newA}is extended once by the length of λ in the direction of q

_{newA}from q

_{goal}. At this time, since there is no intermediate node between q

_{newA}and q

_{start}, the ‘Triangular-Rewiring’ process is skipped.

_{rand}position, and in the direction of the position, q

_{newA}is updated at a location separated by λ length from the nearest node q

_{near}in the tree and rewired between q

_{newA}and q

_{goal}. In this case, since the tree on the opposite side collides with an obstacle to extend in the q

_{newA}direction, the ‘Extend’ process is skipped. In addition, it is assumed that Swap occurs between T

_{a}with initial q

_{start}as the root node and T

_{b}with initial q

_{goal}as the root node between each figure.

_{rand}position and at a position separated by the length of λ in the position direction, at the node q

_{near}that is nearest among nodes in the tree in the position direction, It shows updating q

_{newA}to a position that is the length of λ and rewires it between q

_{newA}and q

_{start}. Here, since it also collides with an obstacle to extend in the direction of q

_{newA}from the tree on the opposite side, the ‘Extend’ process is skipped.

_{rand}position and q

_{newA}is located at a position separated by the length of λ in the direction of the position, and q

_{newA}is also at a position separated by the length of λ from the nearest node q

_{near}among nodes in the tree toward the position. It is shown when updating that q

_{newA}merges into one tree through the ‘Connect’ process because q

_{newA}is within range of the center of q

_{newB}and the radius of λ. It is assumed that the fourth random sample between Figure 9d,e is generated inside the obstacle, so the q

_{newA}generation process is skipped. Figure 9f shows the result of path R created as a merged tree by ‘Connect’ as shown in Figure 9e.

## 5. Experimental Results

#### 5.1. Experimental Environment

#### 5.2. Experimental Results and Analysis for Each Map

#### 5.3. Experimental Results and Analysis in Total

## 6. Conclusions

## Author Contributions

## Funding

## Institutional Review Board Statement

## Informed Consent Statement

## Conflicts of Interest

## Appendix A. Details of the RRT Algorithm

#### Appendix A.1. Pseudocode of the RRT Algorithm

Algorithm A1 Pseudocode of the RRT Algorithm | |

Input:q _{start} ← Position of Start Pointq _{goal} ← Position of Goal Pointλ ← Step Length C ← Position Set of All Boundary Points in All Obstacles N ← Number of Random Samples Output:R ← Result of Path R Initialize:T ← Null Treed _{shorter} ← 0 | |

BeginRRTProcedure | |

1 | T ← Insert Root Node<q_{start}> to T |

2 | While 1 ← n to N do |

3 | Generaten-th Random Sample |

4 | q_{rand} ← Position of n-th Random Sample |

5 | q_{near} ← Find Position of Nearest Node in T from q_{rand} |

6 | If Not isInside(q_{near}, q_{rand}, λ) then |

7 | q_{new} ← Position of Intersection Point between Line Segment connecting q_{rand} and q_{near}, and Circle with Radius λ centered at q_{near} //2D: Circle, 3D: Sphere, … |

8 | Else |

9 | q_{new} ← q_{rand} |

10 | If Not isTrapped(q_{new}, q_{near}, C) then |

11 | T ← Insert Node<q_{new}> and Edge<q_{new}, q_{near}> to T |

12 | If isInside(q_{new}, q_{goal}, λ) then |

13 | T ← Insert Node<q_{goal}> and Edge<q_{new}, q_{goal}> to T |

14 | P_{reach} ← Path from Last Inserted Node [q_{goal}] to Root Node [q_{start}] in T |

15 | d_{reach} ← Distance of P_{reach} |

16 | If d_{shorter} = 0 or d_{shorter} > d_{reach} then |

17 | R ← P_{reach} |

18 | d_{shorter} ← d_{reach} |

19 | T ← Delete Node<q_{goal}> and Edge<q_{new}, q_{goal}> from T |

EndRRTProcedure |

_{start}, and this q

_{start}is the start point. From this q

_{start}to the goal position q

_{goal}, random sample is generated N times, as required until the tree is expanded. q

_{rand}is position of generated random sample, in which the node nearest to the tree T is q

_{near}. At this time, the position q

_{new}created later varies depending on whether q

_{rand}is located inside a circle (or n-sphere), with q

_{near}as the center and step length λ as the radius. The function that determines this is isInside (AA2), and if q

_{rand}is located inside the circle (or n-sphere) (True), q

_{new}becomes q

_{rand}, and if it is not located inside (False), q

_{new}becomes the intersection point between the line segment connecting q

_{rand}and q

_{near}and the circle with λ centered at q

_{near}. If there is no obstacle between q

_{new}and q

_{near}(False), q

_{new}is inserted into the tree as a child node of q

_{near}of T. Currently, the function that determines whether an obstacle exists between q

_{new}and q

_{near}is isTrapped (AA3) (in the isTrapped function, C refers to the set of obstacles).

_{goal}is inside the radius of the λ with the newly inserted q

_{new}as the center, it is considered to have reached the goal point (by isInside function). If it is reached (True), q

_{goal}is inserted as a child node of node q

_{new}of T.

_{reach}is calculated for the path P

_{reach}to q

_{start}and q

_{goal}. Currently, if d

_{reach}is smaller than the path length d

_{shorter}or reached first (d

_{shorter}= 0), the result path R becomes P

_{reach}, and d

_{shorter}becomes d

_{reach}. At the end of the next N sampling, R becomes the final planned path.

_{goal}and the edge connected to this node must be deleted from the tree, T. Otherwise, the tree structure will break due to cyclic. As a result, when a graph structure is formed, the cost of path search increases rapidly.

#### Appendix A.2. Pseudocode of the Functions Used in the RRT Algorithm

Algorithm A2 Pseudocode of the isInside Function from the RRT Algorithm | |

Input:q _{center} ← Position {q_{near}/q_{new}} from RRTq _{target} ← Position {q_{rand}/q_{goal}} from RRTλ ← Step Length λ from RRT Output:f ← Result of Boolean f Initialize:f ← False | |

BeginisInsideProcedure fromRRT | |

1 | d ← Distance of q_{center} to q_{target} |

2 | Ifλ ≥ d then |

3 | f ← True |

EndisInsideProcedure fromRRT |

_{rand}exists inside a circle (or n-sphere) with q

_{near}as the center and λ as the radius, or q

_{goal}exists inside a circle (or n-sphere) with q

_{new}as the center and λ as the radius.

_{rand}, q

_{goal}, ...) is called q

_{target}, and the center of radius (q

_{near}, q

_{new}, ...) is called q

_{center}. When the distance between q

_{center}and q

_{target}is d, if this d is less than or equal to λ, it is determined that q

_{target}is the inside position (True).

_{new}and q

_{near}.

Algorithm A3 Pseudocode of the isTrapped Function from the RRT Algorithm | |

Input:q _{new} ← Position q_{new} from RRTq _{near} ← Position q_{near} from RRTC ← Position Set of All Boundary Points in All Obstacles C from RRT Output:f ← Result of Boolean f Initialize:n ← 1 f ← True | |

BeginisTrappedProcedure fromRRT | |

1 | l_{q} ← Line Segment connecting q_{new} and q_{near} |

2 | c ← Position Set of All Boundary Points of n-th Inserted Obstacle in C |

3 | l_{c} ← Line Segment connecting Last Inserted Position and 1st Inserted Position in c |

4 | i ← 1 |

5 | While Not Intersect between l_{q} and l_{c} do |

6 | l_{c} ← Line Segment connecting i-th Inserted Position and (i + 1)-th Inserted Position in c |

7 | i ← i + 1 |

8 | If i = (Number of Position in c) – 1 then |

9 | If Intersect between l_{q} and l_{c} then |

10 | Break |

11 | n ← n + 1 |

12 | If n > (Number of Position Set in C) then |

13 | f ← False |

14 |
Break |

15 | Else |

16 | c ← Position Set of All Boundary Points of n-th Inserted Obstacle in C |

17 | l_{c} ← Line Segment connecting Last Inserted Position and 1st Inserted Position in c |

18 | i ← 1 |

EndisTrappedProcedure fromRRT |

_{new}and q

_{near}is l

_{q}, and the set of positions formed by the n-th obstacle in the set of obstacles C is c, then l

_{c}is the i-th and (i + 1)-th positions inserted in c (and the last and first position). It is determined whether it intersects with l

_{q}for all line segments l

_{c}in the set of positions of all obstacles, c, that C has. Currently, if any intersect occurs, AA3 returns True and stops the procedure immediately. Otherwise, it determines all the line segments that can be l

_{c}and returns False.

#### Appendix A.3. Basic Mathematical Modeling of the RRT Algorithm

_{new}is calculated from the coordinate value of q

_{rand}in the RRT algorithm

_{near}to q

_{rand}and from q

_{near}to q

_{new}

_{new}can be derived as shown in Equations (A4) and (A5). In this case, d ≤ λ refers to a case where q

_{rand}exists in a position inside the λ radius.

## References

- Schwab, K. The Fourth Industrial Revolution; Crown Business: New York, NY, USA, 2017. [Google Scholar]
- Sariff, N.; Buniyamin, N. An overview of autonomous mobile robot path planning algorithms. In Proceedings of the IEEE 4th Student Conference on Research and Development, Selangor, Malaysia, 28–29 June 2006; pp. 183–188. [Google Scholar]
- Roy, D. Visibility graph based spatial path planning of robots using configuration space algorithms. Int. J. Robot. Autom.
**2009**, 24, 1–9. [Google Scholar] [CrossRef] - Katevas, N.I.; Tzafestas, S.G.; Pnevmatikatos, C.G. The approximate cell decomposition with local node refinement global path planning method: Path nodes refinement and curve parametric interpolation. J. Intell. Robot. Syst.
**1998**, 22, 289–314. [Google Scholar] [CrossRef] - Warren, C.W. Global Path Planning using Artificial Potential Fields. In Proceedings of the International Conference on Robotics and Automation, Scottsdale, AZ, USA, 14–19 May 1989; Volume 1, pp. 316–321. [Google Scholar]
- LaValle, S.M. Motion planning part II: Wild frontiers. IEEE Robot. Autom. Mag.
**2011**, 18, 108–118. [Google Scholar] - Mac, T.T.; Copot, C.; Tran, D.T.; De Keyser, R. Heuristic approaches in robot path planning: A survey. Robot. Auton. Syst.
**2016**, 86, 13–28. [Google Scholar] [CrossRef] - Paden, B.; Čáp, M.; Yong, S.Z.; Yershov, D.; Frazzoli, E. A survey of motion planning and control techniques for self-driving urban vehicles. IEEE Trans. Intell. Veh.
**2016**, 1, 33–55. [Google Scholar] [CrossRef][Green Version] - Karaman, S.; Frazzoli, E. Incremental sampling based algorithms for optimal motion planning. arXiv
**2010**, arXiv:1005.0416. [Google Scholar] - Brunner, M.; Bruggemann, B.; Schulz, D. Hierarchical Rough Terrain Motion Planning using an Optimal Sampling based Method. In Proceedings of the IEEE International Conference on Robotics and Automation, Karlsruhe, Germany, 6–10 May 2013; pp. 5539–5544. [Google Scholar]
- Adiyatov, O.; Varol, H.A. Rapidly-exploring Random Tree Based Memory Efficient Motion Planning. In Proceedings of the IEEE International Conference on Mechatronics and Automation, Takamatsu, Japan, 4–7 August 2013; pp. 354–359. [Google Scholar]
- LaValle, S.M.; Kuffner, J.J., Jr. Randomized kinodynamic planning. Int. J. Robot. Res.
**2001**, 20, 378–400. [Google Scholar] [CrossRef] - LaValle, S.M. Rapidly-Exploring Random Trees: A New Tool for Path Planning; Springer: London, UK, 1998. [Google Scholar]
- Englot, B.; Hover, F.S. Sampling based coverage path planning for inspection of complex structures. In Proceedings of the ICAPS 2012, 22nd International Conference on Automated Planning and Scheduling, Atibaia, Sao Paulo, Brazil, 25–29 June 2012. [Google Scholar]
- Kuffner, J.J., Jr.; LaValle, S.M. RRT-connect: An Efficient Approach to Single-query Path Planning. In Proceedings of the IEEE International Conference on Robotics and Automation, San Francisco, CA, USA, 24–28 April 2000; Volume 2, pp. 995–1001. [Google Scholar]
- Islam, F.; Nasir, J.; Malik, U.; Ayaz, Y.; Hasan, O. Rrt*-smart: Rapid Convergence Implementation of rrt* towards Optimal Solution. In Proceedings of the IEEE International Conference on Mechatronics and Automation, Chengdu, China, 5–8 August 2012; pp. 1651–1656. [Google Scholar]
- Jeong, I.-B.; Lee, S.-J.; Kim, J.-H. Quick-RRT*: Triangular inequality based implementation of RRT* with improved initial solution and convergence rate. Expert Syst. Appl.
**2019**, 123, 82–90. [Google Scholar] [CrossRef] - Karaman, S.; Frazzoli, E. Sampling based algorithms for optimal motion planning. Int. J. Robot. Res.
**2011**, 30, 846–894. [Google Scholar] [CrossRef] - Gammell, J.D.; Srinivasa, S.S.; Barfoot, T.D. Informed RRT*: Optimal Sampling based Path Planning Focused via Direct Sampling of an Admissible Ellipsoidal Heuristic. In Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems, Chicago, IL, USA, 14–18 September 2014; pp. 2997–3004. [Google Scholar]
- Klemm, S.; Oberländer, J.; Hermann, A.; Roennau, A.; Schamm, T.; Zollner, J.M.; Dillmann, R. RRT*-Connect: Faster, Asymptotically Optimal Motion Planning. In Proceedings of the IEEE International Conference on Robotics and Biomimetics, Zhuhai, China, 6–9 December 2015; pp. 1670–1677. [Google Scholar]
- Choudhury, S.; Scherer, S.; Singh, S. RRT*-AR: Sampling based Alternate Routes Planning with Applications to Autonomous Emergency Landing of a Helicopter. In Proceedings of the IEEE International Conference on Robotics and Automation, Karlsruhe, Germany, 6–10 May 2013; pp. 3947–3952. [Google Scholar]
- Noreen, I.; Amna, K.; Zulfiqar, H. A comparison of RRT, RRT* and RRT*-smart path planning algorithms. Int. J. Comput. Sci. Netw. Secur.
**2016**, 16, 20. [Google Scholar] - Da Silva Arantes, M.; Toledo, C.F.M.; Williams, B.C.; Ono, M. Collision-free encoding for chance-constrained nonconvex path planning. IEEE Trans. Robot.
**2019**, 35, 433–448. [Google Scholar] [CrossRef] - Nazarahari, M.; Khanmirza, E.; Doostie, S. Multi-objective multi-robot path planning in continuous environment using an enhanced genetic algorithm. Expert Syst. Appl.
**2019**, 115, 106–120. [Google Scholar] [CrossRef] - Sung, I.; Choi, B.; Nielsen, P. On the training of a neural network for online path planning with offline path planning algorithms. Int. J. Inf. Manag.
**2020**, 102142. [Google Scholar] [CrossRef] - Jeon, G.-Y.; Jung, J.-W. Water sink model for robot motion planning. Sensors
**2019**, 19, 1269. [Google Scholar] [CrossRef] [PubMed][Green Version] - Han, J. Mobile robot path planning with surrounding point set and path improvement. Appl. Soft Comput.
**2017**, 57, 35–47. [Google Scholar] [CrossRef] - Yoon, H.U.; Lee, D.-W. Subplanner algorithm to escape from local minima for artificial potential function based robotic path planning. Int. J. Fuzzy Log. Intell. Syst.
**2018**, 18, 263–275. [Google Scholar] [CrossRef] - Jung, J.-W.; So, B.-C.; Kang, J.-G.; Lim, D.-W.; Son, Y. Expanded Douglas–Peucker polygonal approximation and opposite angle based exact cell decomposition for path planning with curvilinear obstacles. Appl. Sci.
**2019**, 9, 638. [Google Scholar] [CrossRef][Green Version]

**Figure 1.**Overview of the algorithms in this paper: (

**a**) RRT; (

**b**) RRT-Connect; (

**c**) the proposed algorithm.

**Figure 2.**RRT algorithm: (

**a**) Process when q

_{new}is created; (

**b**) After the random sampling has ended.

**Figure 5.**Abstract process of the ‘Triangular-Rewiring’ method: (

**a**) Example tree; (

**b**) After rewiring between q

_{child}and q

_{ancestor}; (

**c**) At this time, α is the distance between q

_{child}and q

_{parent}, β is the distance between q

_{parent}and q

_{ancestor}, and γ is the distance between q

_{child}and q

_{ancestor}.

**Figure 6.**Detailed process of the ‘Triangular-Rewiring’ method: (

**a**) Each node q for index i (at this time, q

_{start}is same as q

_{7}and q

_{goal}is same as q

_{0}); (

**b**) Represent each node using the n-th ancestor ${\xi}^{n}$ of q

_{0}; (

**c**) Each distance ${\mathrm{\mathbb{d}}}_{n}$ between the n-th and (n + 1)-th ancestor nodes of q

_{0}; (

**d**) When the ‘Triangular-Rewiring’ method is applied and rewired by distance ${\mathrm{\mathbb{u}}}_{{k}_{j}}$; (

**e**) Represent as the value of k

_{j}; (

**f**) Represent each node by the n-th ancestor ${\xi}^{n}$ of q

_{0}after method is applied.

**Figure 9.**Detailed process of the proposed algorithm: (

**a**) Start position q

_{start}from tree T

_{a}and goal position q

_{goal}from tree T

_{b}; (

**b**) Create q

_{newA}nearest to T

_{a}from 1

^{st}random sampling position q

_{rand}and create q

_{newB}from q

_{goal}nearest to T

_{b}; (

**c**) Create new q

_{newA}from q

_{near}nearest to T

_{b}from the second random sampling position q

_{rand}and rewire between q

_{newA}and q

_{goal}the ancestor of the q

_{newA}; (

**d**) Create a new q

_{newA}from q

_{near}nearest to T

_{a}from the third random sampling position q

_{rand}and rewire between q

_{newA}and q

_{start}with the ancestor of q

_{newA}; (

**e**) Create new q

_{newA}from q

_{near}nearest to T

_{a}from the fifth random sampling position q

_{rand}and connect between q

_{newA}and q

_{newB}nearest to T

_{b}from q

_{newA}; (

**f**) Result of Path R from q

_{start}to q

_{goal}.

**Figure 10.**Maps for the experiment: (

**a**) Map 1; (

**b**) Map 2; (

**c**) Map 3; (

**d**) Map 4; (

**e**) Map 5; (

**f**) Map 6; (

**g**) Map 7; (

**h**) Map 8.

**Figure 19.**Experimental results in total for the average number of samples (for first path finding): (

**a**) result of each map compared with the RRT algorithm (${x}_{cmp}\left(i\right)$); (

**b**) average result compared with the RRT algorithm (${X}_{cmp}$).

**Figure 20.**Experimental results in total for the average path length: (

**a**) result of each map compared with the RRT algorithm (${x}_{cmp}\left(i\right)$); (

**b**) average result compared with the RRT algorithm (${X}_{cmp}$).

**Figure 21.**Experimental results in total on the average planning time: (

**a**) result of each map compared with the RRT algorithm (${x}_{cmp}\left(i\right)$); (

**b**) average result compared to the RRT algorithm (${X}_{cmp}$).

H/W | Specification |
---|---|

CPU | Intel Core i7-6700k 4.00 GHz (8 CPUs) |

RAM | 32,768 MB (32 GB DDR4) |

VGA | Nvidia GeForce GTX 1080 (VRAM 8 GB) SLI (x2) |

**Table 2.**Experimental result of Map 1 (the parentheses to the right of each value are relative ratios based on RRT 100% (${x}_{cmp}\left(1\right)$)).

$\mathit{Performance}\left({\mathit{A}}_{\mathit{c}\mathit{m}\mathit{p}}\left(1\right)\right)$ | RRT | RRT-Connect | Proposed Algorithm |
---|---|---|---|

Avg. num. of samples (samples) | 1216 (100) | 729 (60) | 823 (68) |

Avg. path length (px) | 1341 (100) | 1343 (100) | 1200 (89) |

Avg. planning time (ms) | 12 (100) | 7 (58) | 10 (83) |

**Table 3.**Experimental result of Map 2 (the parentheses to the right of each value are the relative ratios based on RRT 100% (${x}_{cmp}\left(2\right)$)).

$\mathit{Performance}\left({\mathit{A}}_{\mathit{c}\mathit{m}\mathit{p}}\left(2\right)\right)$ | RRT | RRT-Connect | Proposed Algorithm |
---|---|---|---|

Avg. num. of samples (samples) | 271 (100) | 101 (37) | 113 (42) |

Avg. path length (px) | 598 (100) | 613 (98) | 484 (81) |

Avg. planning time (ms) | 6 (100) | 3 (50) | 3 (50) |

**Table 4.**Experimental result of Map 3 (the parentheses to the right of each value are the relative ratios based on RRT 100% (${x}_{cmp}\left(3\right)$)).

$\mathit{Performance}\left({\mathit{A}}_{\mathit{c}\mathit{m}\mathit{p}}\left(3\right)\right)$ | RRT | RRT-Connect | Proposed Algorithm |
---|---|---|---|

Avg. num. of samples (samples) | 6106 (100) | 4574 (75) | 4679 (77) |

Avg. path length (px) | 1934 (100) | 1871 (97) | 1489 (77) |

Avg. planning time (ms) | 866 (100) | 299 (35) | 313 (36) |

**Table 5.**Experimental result of Map 4 (The parentheses to the right of each value are relative ratios based on RRT 100% (${x}_{cmp}\left(4\right)$)).

$\mathit{Performance}\left({\mathit{A}}_{\mathit{c}\mathit{m}\mathit{p}}\left(4\right)\right)$ | RRT | RRT-Connect | Proposed Algorithm |
---|---|---|---|

Avg. num. of samples (samples) | 290 (100) | 28 (10) | 32 (11) |

Avg. path length (px) | 711 (100) | 588 (83) | 534 (75) |

Avg. planning time (ms) | 3 (100) | 3 (100) | 4 (133) |

**Table 6.**Experimental result of Map 5 (the parentheses to the right of each value are the relative ratios based on RRT 100% (${x}_{cmp}\left(5\right)$)).

$\mathit{Performance}\left({\mathit{A}}_{\mathit{c}\mathit{m}\mathit{p}}\left(5\right)\right)$ | RRT | RRT-Connect | Proposed Algorithm |
---|---|---|---|

Avg. num. of samples (samples) | 371 (100) | 68 (18) | 74 (20) |

Avg. path length (px) | 554 (100) | 588 (106) | 465 (84) |

Avg. planning time (ms) | 13 (100) | 2 (15) | 2 (15) |

**Table 7.**Experimental result of Map 6 (the parentheses to the right of each value are the relative ratios based on RRT 100% (${x}_{cmp}\left(6\right)$)).

$\mathit{Performance}\left({\mathit{A}}_{\mathit{c}\mathit{m}\mathit{p}}\left(6\right)\right)$ | RRT | RRT-Connect | Proposed Algorithm |
---|---|---|---|

Avg. num. of samples (samples) | 541 (100) | 184 (34) | 140 (26) |

Avg. path length (px) | 886 (100) | 778 (88) | 668 (75) |

Avg. planning time (ms) | 9 (100) | 6 (67) | 4 (44) |

**Table 8.**Experimental result of Map 7 (the parentheses to the right of each value are relative ratios based on RRT 100% (${x}_{cmp}\left(7\right)$)).

$\mathit{Performance}\left({\mathit{A}}_{\mathit{c}\mathit{m}\mathit{p}}\left(7\right)\right)$ | RRT | RRT-Connect | Proposed Algorithm |
---|---|---|---|

Avg. num. of samples (samples) | 436 (100) | 235 (54) | 244 (56) |

Avg. path length (px) | 898 (100) | 862 (96) | 674 (75) |

Avg. planning time (ms) | 5 (100) | 4 (80) | 3 (60) |

**Table 9.**Experimental result of Map 8 (The parentheses to the right of each value are the relative ratios based on RRT 100% (${x}_{cmp}\left(8\right)$)).

$\mathit{Performance}\left({\mathit{A}}_{\mathit{c}\mathit{m}\mathit{p}}\left(8\right)\right)$ | RRT | RRT-Connect | Proposed Algorithm |
---|---|---|---|

Avg. num. of samples (samples) | 17,033 (100) | 3031 (18) | 2954 (17) |

Avg. path length (px) | 1611 (100) | 1576 (98) | 1358 (84) |

Avg. planning time (ms) | 4501 (100) | 119 (3) | 125 (3) |

**Table 10.**Experimental results in total for the average number of samples (for first path finding) [%].

Algorithm (cmp) | Performance Ratio Based on RRT $\left({\mathit{x}}_{\mathit{c}\mathit{m}\mathit{p}}\left(\mathit{i}\right)\right)$ | Avg. $\left({\mathit{X}}_{\mathit{c}\mathit{m}\mathit{p}}\right)$ | |||||||
---|---|---|---|---|---|---|---|---|---|

Map 1 | Map 2 | Map 3 | Map 4 | Map 5 | Map 6 | Map 7 | Map 8 | ||

RRT | 100 | 100 | 100 | 100 | 100 | 100 | 100 | 100 | 100 |

RRT-Connect | 60 | 37 | 75 | 10 | 18 | 34 | 54 | 18 | 38 |

Proposed | 68 | 42 | 77 | 11 | 20 | 26 | 56 | 17 | 40 |

Algorithm (cmp) | Performance Ratio Based on RRT $\left({\mathit{x}}_{\mathit{c}\mathit{m}\mathit{p}}\left(\mathit{i}\right)\right)$ | Avg. $\left({\mathit{X}}_{\mathit{c}\mathit{m}\mathit{p}}\right)$ | |||||||
---|---|---|---|---|---|---|---|---|---|

Map 1 | Map 2 | Map 3 | Map 4 | Map 5 | Map 6 | Map 7 | Map 8 | ||

RRT | 100 | 100 | 100 | 100 | 100 | 100 | 100 | 100 | 100 |

RRT-Connect | 100 | 98 | 97 | 83 | 106 | 88 | 96 | 98 | 96 |

Proposed | 89 | 81 | 77 | 75 | 84 | 75 | 75 | 84 | 80 |

Algorithm (cmp) | Performance Ratio Based on RRT $\left({\mathit{x}}_{\mathit{c}\mathit{m}\mathit{p}}\left(\mathit{i}\right)\right)$ | Avg. $\left({\mathit{X}}_{\mathit{c}\mathit{m}\mathit{p}}\right)$ | |||||||
---|---|---|---|---|---|---|---|---|---|

Map 1 | Map 2 | Map 3 | Map 4 | Map 5 | Map 6 | Map 7 | Map 8 | ||

RRT | 100 | 100 | 100 | 100 | 100 | 100 | 100 | 100 | 100 |

RRT-Connect | 58 | 50 | 35 | 100 | 15 | 67 | 80 | 3 | 51 |

Proposed | 83 | 50 | 36 | 133 | 15 | 44 | 60 | 3 | 53 |

Publisher’s Note: MDPI stays neutral with regard to jurisdictional claims in published maps and institutional affiliations. |

© 2021 by the authors. Licensee MDPI, Basel, Switzerland. This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution (CC BY) license (http://creativecommons.org/licenses/by/4.0/).

## Share and Cite

**MDPI and ACS Style**

Kang, J.-G.; Lim, D.-W.; Choi, Y.-S.; Jang, W.-J.; Jung, J.-W. Improved RRT-Connect Algorithm Based on Triangular Inequality for Robot Path Planning. *Sensors* **2021**, *21*, 333.
https://doi.org/10.3390/s21020333

**AMA Style**

Kang J-G, Lim D-W, Choi Y-S, Jang W-J, Jung J-W. Improved RRT-Connect Algorithm Based on Triangular Inequality for Robot Path Planning. *Sensors*. 2021; 21(2):333.
https://doi.org/10.3390/s21020333

**Chicago/Turabian Style**

Kang, Jin-Gu, Dong-Woo Lim, Yong-Sik Choi, Woo-Jin Jang, and Jin-Woo Jung. 2021. "Improved RRT-Connect Algorithm Based on Triangular Inequality for Robot Path Planning" *Sensors* 21, no. 2: 333.
https://doi.org/10.3390/s21020333