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

This paper proposed a triangular inequality-based rewiring method for the rapidly exploring random tree (RRT)-Connect robot path-planning algorithm that guarantees the planning time compared to the RRT algorithm, to bring it closer to the optimum. To check the proposed algorithm’s performance, this paper compared the RRT and RRT-Connect algorithms in various environments through simulation. From these experimental results, the proposed algorithm shows both quicker planning time and shorter path length than the RRT algorithm and shorter path length than the RRT-Connect algorithm with a similar number of samples and planning time.


Introduction
With the recent Fourth Industrial Revolution, interest in mobile robots has increased in various fields such as robotics, smart factories, and autonomous driving [1]. Classical mobile robot path-planning algorithms can be classified into three broad categories [2]. The first is the road map approach algorithm [3], which is easy to implement by designing a map that represents a path that can be moved and plan through it. The second is cell decomposition algorithm [4], which creates a path by dividing the configuration space into cells and connecting each cell using a graph. The last is the artificial potential field algorithm [5], which creates an artificial potential field and directs the robot to the goal according to the flow of potential power.
'Optimality' means always ensuring the optimal path. 'Clearance' indicates a lower probability of collision between obstacles and the robot. 'Completeness' means that if a path exists, it can always be found. Optimality, clearance, and completeness are considered important in these classical algorithms and have been the main focus of study [6]. Particularly if completeness is not guaranteed by the robot path-planning algorithm, there is a problem that the path may not be found in finite time. This is a fatal problem in robot path planning.
Recently, sampling-based path-planning algorithms [7][8][9][10][11][12] such as rapidly exploring random tree (RRT) [13], which is quicker and less computationally intensive than classical algorithms, have been attracting attention. The main purpose of sampling-based algorithms is to find a path that can reach the goal as quickly as possible using randomly extracted sample points (random sampling). Unlike classical algorithms, sampling-based algorithms have difficulty fully reflecting the optimality and completeness. Therefore, most sampling-based algorithms claim 'Probabilistic completeness', which explains that they can be probabilistically close to complete when random sampling is repeated infinitely [14]. This means that it is difficult to guarantee the 'Planning time' (first path finding time), which refers to how quickly the path can be planned from the start point to the goal point, and the 'Convergence rate', which means iterative sampling to bring the path closer to the optimum after the first path has been found. If the situation does not allow enough time to plan the path, it can create a path that is more different from the optimal path. Even so, the sampling-based algorithm is mainly used in dynamic environments because it enables quicker path planning with very little planning time compared to classical algorithms.
To overcome these limitations of planning time and convergence rate, many studies are being conducted to expand the RRT algorithm. The RRT-Connect [15] algorithm finds a connected path more quickly than the RRT algorithm by setting the start point and goal point as the roots of separate trees and expanding both trees alternately. In addition, there are algorithms that optimize paths based on the principle of triangular inequality, such as RRT*-Smart algorithm [16] and Quick-RRT* algorithm [17], to derive a path that is close to the optimal. Many algorithms [18][19][20][21] that extend the RRT algorithm have been studied.
The above algorithms show more efficient performance by improving the RRT algorithm to overcome the limitations of sampling-based methods but they are still not perfect. Their limitations include being unable to derive the optimal length and there is room for improvement in terms of the number of operations and time. For example, the RRT* algorithm has rewiring(search for the parent node as a via point nearby a newly inserted node, where the addition of path length from the start point to the via point and path length from the via point to the newly inserted node in the tree is the optimized, and change the neighboring nodes to optimize the path length) and neighbor search (search for nodes nearby the node to be newly inserted in the tree) processes to obtain shorter path lengths than the RRT algorithm [18]. However, there is an efficiency trade-off in this process. In other words, while the convergence rate has improved, the planning time has significantly increased [22]. Therefore, the RRT* algorithm cannot be said to be better than the RRT algorithm in all performance metrics and it can be said that the RRT* algorithm gets closer to the optimum at the expense of planning time.
To overcome the limitation of getting closer to the optimum at the expense of planning time, this paper proposes a triangular inequality-based RRT-Connect algorithm that finds an ancestor node as a via point, where the addition of path length from the start point to the via point and path length from the via point to the newly inserted node is the most optimized, based on the principle of triangular inequality and RRT-Connect. The proposed algorithm shortens the planning time while also pursuing optimization through rewiring. In addition, we will verify the efficiency by comparing the RRT and RRT-Connect algorithms from previous studies through simulation experiments. As a result, this paper shows that the proposed algorithm has a shorter path length than the RRT and RRT-Connect algorithms without sacrificing other performance measures such as the number of samples or planning time.
The scope of the research we will cover is how much more quickly it can find the path and how much shorter the path is. This is because in a dynamic environment, it is more important to find a navigable path. In a dynamic environment, there may not be enough time for convergence. In other words, the purpose of our proposed algorithm is to improve the RRT-Connect algorithm so that it can find a shorter path over the same planning time (computation time before convergence or computation time for first path finding). Figure 1 shows an overview of the three main algorithms covered in this paper: RRT, RRT-Connect, and the proposed algorithm. In this figure, the start q 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. In this paper, Section 2 introduces the RRT algorithm, Section 3 introduces the RRT-Connect algorithm, and the triangular inequality-based RRT-Connect algorithm is proposed in Section 4. In detail, Section 4.1 shows the pseudocode of the proposed rewiring method through the principle of triangular inequality, which can be applied to the RRT-Connect algorithm, Section 4.2 shows the mathematical modeling of the proposed algorithm, Sections 4.3 and 4.4 show the pseudocode of each method of the RRT-Connect algorithm applying the proposed rewiring method, and Section 4.5 shows the path-planning process for the proposed algorithm that applies the proposed rewire method to the RRT-Connect algorithm. Section 5 shows the experimental environment and results to check the performance of the proposed algorithm and Section 6 presents the conclusion.

Rapidly Exploring Random Tree (RRT) Algorithm
Rapidly exploring Random Tree (RRT) algorithm [13] is the most representative sampling-based path-planning algorithm. The RRT algorithm plans a path by gradually expanding a tree with a root node at the start point using random sampling. It is designed to handle non-holonomic constraints and high degrees of freedom [12].
When a random sample is generated in the configuration space, it tries to connect at a point separated by a preset step length from the node nearest to the random sample among nodes constituting the tree with the step length. If tree connections are possible, nodes are added to create an extended tree.
As mentioned in the introduction, this sampling-based path-planning algorithm uses randomly generated sample points to find a path that can reach the goal as quickly as possible, so it is difficult to sufficiently reflect the optimality and completeness. Figure 2 shows the path-planning process of the RRT algorithm. Figure 2a shows that q 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 .

RRT-Connect Algorithm
Path planning through the RRT algorithm may have a disadvantage in that since random samples appear with the same probability in all regions, the tree easily extends even in a direction irrespective of the goal, resulting in a long planning time and inefficiency. The RRT-Connect algorithm [15] proposed later has two new ideas as the method to compensate for the disadvantage of the RRT algorithm.
The first is that the start and goal points are each inserted as root nodes and extended in each direction alternately. The two trees extending from the start point and the goal point expand as if attracting one another (which prevents the tree and is a disadvantage of the RRT algorithm) is in a direction irrespective of the goal. This enhances the disadvantage of the planning time required to search for a path. The second is the concept of 'Extend', which continues extending to the other side of the tree if there are no collisions with obstacles when the tree extends. Through this, unlike the RRT algorithm that extends the maximum extension length when the sample is generated and is inserted into the tree, the tree continues to expand in the direction of the goal if there is no collision with an obstacle, so the path can be planned more quickly.
Path planning through the RRT-Connect algorithm can find a path quicker than the RRT algorithm, but the 'Extend' method does not work properly in complex environments with narrow paths and many obstacles and it can be difficult. In addition, the path planned using the RRT-Connect algorithm is far from the optimal length, so it does not properly reflect optimality.

Pseudocode of the RRT-Connect Algorithm
This section shows the pseudocode of the RRT-Connect algorithm used in the experiment in this paper that was designed based on [15] in which the RRT-Connect algorithm was proposed. The RRT-Connect algorithm can be represented by a main algorithm (A1) and two main methods (A2 and 3).
Algorithm 1 shows the pseudocode of RRT-Connect algorithm. Both of the two initial trees T 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 Position q goal ← Goal Point Position λ ← Step Length C ← Position Set of All Boundary Points in All Obstacles N ← Number of Random Samples Output: 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 When a path is created by the 'Connect' method, the distance d 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.

Pseudocode of the Extend Method from the RRT-Connect Algorithm
This section introduces the 'Extend' method used in pseudocode (A1) of the RRT-Connect algorithm in Section 3.1.
Algorithm 2 shows the pseudocode of the 'Extend' method in the RRT-Connect algorithm. The isInside function determines whether q 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. This is the process of making T 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.

Algorithm 2 Pseudocode of the original 'Extend' method from the RRT-Connect Algorithm
Input: 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 If isTrapped(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 End Extend Procedure from RRT-Connect If there is no obstacle between q 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 . Figure 3 shows the 'Extend' method in the RRT-Connect algorithm. In detail, it shows that the first q 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.

Pseudocode of the Connect Method from the RRT-Connect Algorithm
This section introduces the 'Connect' method used in pseudocode (A1) of the RRT-Connect algorithm in Section 3.1.
Algorithm 3 shows the pseudocode of the 'Connect' method in the RRT-Connect algorithm. Here, T a , T b , and q newB are from the 'Extend' method (A2).
The tree merging process is as follows: create a path P 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. Figure 4 shows the 'Connect' method in the RRT-Connect algorithm. If the q 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 .

Proposed Triangular Inequality-Based RRT-Connect Algorithm
The proposed triangular inequality-based RRT-Connect algorithm is a rewire based on the principle of triangular inequality between nodes on a path planned in the RRT-Connect algorithm, so it is closer to the optimal compared to the RRT-Connect. This is like the RRT*-Smart algorithm [16] and Quick-RRT* [17] algorithms, which shorten their paths using the triangular inequality principle for the RRT algorithm. In this paper, the rewire part based on the triangular inequality principle is called the 'Triangular-Rewiring' method.
The proposed triangular inequality-based RRT-Connect algorithm requires the following assumptions:

1.
There is only one start point and one goal point even though the goal point may be changed incrementally as time goes on. 2.
The robot is capable of omnidirectional motion.
Therefore, this chapter introduces the proposed 'Triangular-Rewiring' method for the RRT-Connect algorithm, and performs mathematical modeling to confirm the validity that the proposed 'Triangular-Rewiring' method is always shorter when applied to the RRT-Connect algorithm. After checking through, we will propose how to apply the 'Triangular-Rewiring' method to the RRT-Connect algorithm.
The method of applying the RRT-Connect algorithm of the proposed 'Triangular-Rewiring' method is proposed when a new node is inserted into the tree in the 'Extend' method (A2) and 'Connect' method (A3), the main methods of the RRT-Connect algorithm introduced in Chapter 3. It is inserted after rewiring (or after determining) through the 'Triangular-Rewiring' method. That is, this chapter introduces the 'Extend' and 'Connect' methods to which the proposed 'Triangular-Rewiring' method is applied.

Pseudocode of the Proposed Triangular-Rewiring Method for the Improved RRT-Connect Algorithm
This section introduces the 'Triangular-Rewiring' method for the proposed triangular inequality-based RRT-Connect algorithm.
Algorithm 4 shows the pseudocode of the 'Triangular-Rewiring' method applicable in the 'Extend' (A2) and 'Connect' (A3) methods of the RRT-Connect algorithm. When inserting a new node and edge in T 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.
In the 'Extend' and 'Connect' methods, q 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.
The 'Triangular-Rewiring' process is as follows: Delete node where position q 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 .

Algorithm 4 Pseudocode of the Proposed 'Triangular-Rewiring' Method for the RRT-Connect Algorithm
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 End triangularRewiring Procedure from Extend, Connect

Mathematical Modeling of the Proposed Triangular Inequality-Based RRT-Connect Algorithm
This section introduces the mathematical modeling of the proposed triangular inequalitybased RRT-Connect algorithm. The results show that the proposed algorithm is more efficient in terms of path length than the RRT-Connect algorithm. For reference, this mathematical modeling is based on a two-dimensional Euclidean space.
Equations (1) and (2) define the path length d n (q i ) between an arbitrary node q i and its parent node in the RRT algorithm Here, q 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 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 In addition, consider starting with an arbitrary node q 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 ξ n (q i ), ξ n+1 (q i ) .
Equations (3) and (4) show the path length D R from the start position q start to the goal position q goal by the RRT algorithm Equation (3) shows when the (δ + 1)-th ancestor node from q goal is q start , where δ is the upper limit of δ ∑ n=0 d n q goal for obtaining the path length 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 ). Figure 5 shows an abstract process of the 'Triangular-Rewiring' method. As shown in Figure 5a, if the parent node of q 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): 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 .
If the distances between the edges connecting each node are the α between q 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 α + β ≥ γ Equations (7) and (8) show the distance relationship between the ancestor nodes of q child Equation (7) can be summarized as Equation (8) by substituting Equation (5), which represents the relationship between the n-th ancestor nodes of q child , with the distance as Equation (1) in Equation (6), which represents the distance between each node as a triangular inequality.
Equations (9)- (15) show that the path of the RRT algorithm applying the 'Triangular-Rewiring' method is always shorter or equal to that planned by the original RRT algorithm.
Equation (9) shows the sequence index k j to compare the distance u when applying the 'Triangular-Rewiring' method with distance d when this method is not applied Here, j is a sequence index for u. That is, k j can be considered a sequence index for d.
Currently, τ j is the number of times that rewiring occurs in the j-th. If this is summarized by Equation (1) for a distance based on an arbitrary node q i , it is as Equation (10). For example, as shown in Figure 5, if j is 0 and 1 a rewire occurs (τ 0 = 1), it can be represented in combination with the distance relationship of Equation (8) for q child , as in Equation (11) The result of Equation (11) can be generalized as shown in Equation (12 For d based on an arbitrary node q i , the path length k j ∑ n=j d n from the j-th to k j -th arbitrary sequence index is always longer or equal to the distance u k j of the k j -th sequence index. That is, in an arbitrary path, it can be confirmed that the distance u rewired by the 'Triangular-Rewiring' method is at least equal (if the distances of d and u are the same, the rewired line segments are on a straight line) or always shorter than d when not rewired. Figure 6 shows the 'Triangular-Rewiring' process for the path from q 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 (τ 0 = 2) between q 0 and q 3 (ξ 3 (q 0 )), and a total of one rewire occurred (τ 3 = 1) between q 5 (ξ 5 (q 0 )) and q 7 (ξ 7 (q 0 )). 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). 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 ξ n of q 0 ; (c) Each distance 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 u k j ; (e) Represent as the value of k j ; (f) Represent each node by the n-th ancestor ξ n of q 0 after method is applied.
Comparing Figure 6c,e, according to Equation (7), the rewired distance u 2 (q 0 ) is shorter than the path length 2 ∑ n=0 d n (q 0 ) from d 0 to d 2 and the rewired distance u 6 (q 0 ) is shorter than the path length 6 ∑ n=5 d n (q 0 ) from d 5 to d 6 . That is, when comparing before applying the 'Triangular-Rewiring' method in Figure 6a and after applied this method in Figure 6f, the path afterward looks shorter.
Equations (13) and (14) show the path length D R when the 'Triangular-Rewiring' method is not applied and the path length U R when the method has been applied for an arbitrary path (start position: q start , goal position: q goal ), as shown in Figure 6 k ϕ = δ, Equation (13) shows the upper limit when the index n of d is δ in Equation (3); when this is substituted into the sequence index k j , if k j is δ, j becomes ϕ. In that case, as in Equation (14), D R is used to compare the δ ∑ n=0 d n q goal shown in Equation (4)  Equation (15) shows when the equation summarized in Equation (14) is substituted into Equation (12) ∴ Finally, as can be confirmed using Equation (15), U R as a result of applying the 'Triangular-Rewiring' method to the distance of an arbitrary path (start position: q start , goal position: q goal ) is at least equal (If the distances of D and U are the same, when the rewired line segments are on a straight line) to or always shorter than D R ; as a result, this method is not applied.
Equations (16)- (18) show the path length D A of the path from the start position (root node) of T a to the last (inserted node) position q newA and the path length U A when the 'Triangular-Rewiring' method has been applied to the path. In addition, it shows that U A is at least equal to or always shorter than D A : Equations (19)- (21) show the path length D B of the path from the start position (root node) of T b to the last (inserted node) position q newB and the path length U B when the 'Triangular-Rewiring' method has been applied to the path. In addition, it shows that U B is at least equal to or always shorter than D B : Therefore, Equations (16) and (19) can be derived from Equations (3) and (13), Equations (17) and (20) from Equation (14), and Equations (18) and (21) from Equation (15).
As a result, Equations (22) and (23) show that RRT-Connect with the proposed 'Triangular-Rewiring' method is at least the same or better in terms of path length than the RRT-Connect algorithm without the method D R (Equation (4)), which refers to the path length of the RRT-Connect algorithm path without the 'Triangular-Rewiring' method, is represented by the sum of the distance D A of the partial path P a (Equation (17)), the distance D B of the partial path P b (Equation (20)), and the distance D(q newA , q newB ) between q newA and q newB as shown in Equation (22).
U R (Equation (14)), which refers to the path length of the RRT-Connect algorithm path with the 'Triangular-Rewiring' method, is equal to or shorter than the sum of the distance U A of the partial path P a for the RRT-Connect (Equation (17)), the distance U B of the partial path P b (Equation (20)), and the distance D(q newA , q newB ) between q newA and q newB as shown in Equation (22).
Here, Equation (23) shows that U R is at least equal to or shorter than D R in the RRT algorithm summarized in Equation (15), and it is used efficiently in the RRT-Connect algorithm.

Pseudocode of Proposed Extend Method for the Improved RRT-Connect Algorithm
This section introduces the 'Extend' method in the proposed triangular inequalitybased RRT-Connect algorithm. This proposed 'Extend' method (A5) replaces the 'Extend' method (A3) in the pseudocode of the RRT-Connect algorithm (A2).
Algorithm 5 is the application of the 'Triangular-Rewiring' method (A4) to the original 'Extend' method (A2) of the RRT-Connect algorithm. Compared to the original 'Extend' method, the part where a node is newly inserted in the tree in lines 9 and 16 is inserted through the 'Triangular-Rewiring' method. Other than that, the contents are the same as the original 'Extend' method. Figure 7 shows the application of the 'Triangular-Rewiring' method to Figure 3, which shows the 'Extend' method of the RRT-Connect algorithm. In T 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 .

Pseudocode of the Proposed Connect Method for the RRT-Connect Algorithm
This section introduces the 'Connect' method in the proposed triangular inequalitybased RRT-Connect algorithm. This proposed 'Connect' method (A6) replaces the 'Connect' method (A4) in the pseudocode of the RRT-Connect algorithm (A2).
Algorithm 6 is an application of the 'Triangular-Rewiring' method (A4) to the 'Connect' method (A3) of the RRT-Connect algorithm. Compared to the original 'Connect' method, it has been changed to apply the method to the merged tree by considering the 'Triangular-Rewiring' method when merging the path, which is in lines 5-10. Other than that, the contents are the same as the original 'Connect' method.
When paths P 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 .

Algorithm 5 Pseudocode of the Proposed 'Extend' Method for the RRT-Connect Algorithm
Input: q newA ← q rand 6 If isTrapped(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, . . .

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 End Connect Procedure from RRT-Connect Then, the 'Triangular-Rewiring' method is applied to this T 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). Figure 8 shows the 'Triangular-Rewiring' method applied to Figure 4, which shows the 'Connect' method of the RRT-Connect algorithm. When the paths P 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.  Figure 9 in this section shows the path-planning process of the proposed algorithm by applying the 'Triangular-Rewiring' method to the 'Extend' and 'Connect' methods of the RRT-Connect algorithm. 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 9 shows planning a path from the start position q start to the goal position q goal through the proposed algorithm, as shown in Figure 9a.

Process of the Proposed Triangular Inequality-Based RRT-Connect Algorithm
In Figure 9b, the first random sample is generated at position q 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.
In Figure 9c, a second random sample is generated at the q 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.
In Figure 9d, as shown in Figure 9c, a third random sample is created at the q 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.
In Figure 9e, the fifth random sample is generated at the q 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.

Experimental Results
To verify the performance of the proposed triangular inequality-based RRT-Connect algorithm in this paper, the RRT algorithm, the RRT-Connect algorithm, and the proposed algorithm are compared in various environment maps shown in the experimental environment through the simulator.
Each algorithm was implemented based on the pseudocode (A1-9) shown Sections 3 and 4 (For the RRT algorithm, refer to the pseudocode (AA1) in Appendix A), and the performance measures used for comparison of various algorithms are Number of samples (samples), Path length (pixels), and Planning time (milliseconds). Each performance measure is experimented with 50 trials from the same start point to the same goal point until the first path has been found). Among the performance measures, as the number of samples decreases, the cost of recalculation in a dynamic environment also decreases, and the path length is a measure of the optimality of the path-planning algorithm. In addition, the Step length (λ) is 30 pixels.

Experimental Environment
This section introduces the environment map used in the simulation and the simulator used in the simulation with the computer's performance. Figure 10 shows the eight environmental maps used in this experiment. The green circle (S) indicates the start point, the purple circle (G) indicates the goal point, and the black polygon on the yellow (blue in the analysis of the experimental results) border indicates to the obstacle. All maps are 600 (horizontal) * 600 (vertical) pixels. Many environmental maps were considered and used to verify the performance of various path-planning algorithms including the RRT algorithm, [23][24][25][26]. Which environment map to use is important because the expected performance measure varies depending on the obstacles' placement and shape among other properties.
In this paper, to check the proposed algorithm's performance, the eight maps shown in Figure 10 were benchmarked in the experimental environment of the paper [27] proposed by Jihee Han in 2017, and each map is expected to have the following features: Map 1 in Figure 10a seems to be an environment in which it is easy to verify the completeness of the path-planning algorithm. Map 2 in Figure 10b seems to be an environment in which it is also easy to verify the completeness of the path-planning algorithm, and the environment is mainly used to show the solution for the Local Minima problem [28] in the artificial potential field algorithm [26]. Map 3 in Figure 10c seems to be an environment in which it is easy to verify the optimality and completeness of the path-planning algorithm and is an environment that is unfavorable to random sampling path-planning algorithms such as the RRT algorithm. Map 4 in Figure 10d seems to be an environment in which it is easy to verify the optimality and the planning time for the path-planning algorithm, and the cell decomposition algorithm, which increases the computation cost as the angle of obstacle increases, is an unfavorable environment [29]. Map 5 in Figure 10e seems to be an environment in which it is also easy to verify the optimality and planning time of the path-planning algorithm; for the same reason as Map 4, the cell decomposition algorithm is an unfavorable environment. Map 6 in Figure 10f seems to be an environment in which it is easy to verify the optimality, completeness, and planning time of the path-planning algorithm, and it is an environment for comprehensively evaluating the performance. Map 7 in Figure 10g seems to be an environment in which it is easy to verify the completeness and optimality of the path-planning algorithm, and for the same reason as Map 2, it is the environment used in the Artificial Potential Field algorithm. Lastly, Map 8 in Figure 10h seems to be an environment in which it is easy to verify the completeness and planning time of the path-planning algorithm and yet is unfavorable to random sampling path-planning algorithms such as the RRT algorithm.
Since random sampling path-planning algorithms such as the RRT algorithm rely on probabilistic completeness, the number of samples and the planning time are significantly increased as long as there are narrow or fewer entrances for directions to the goal. Table 1 shows the specifications of the computer used in the simulation. The simulator was developed in C# language (Microsoft Visual Studio Community 2019 version 16.1.6; Microsoft. NET Framework version 4.8.03752), and except for the visual part, only a single thread was used for the calculation. Differences in planning time may occur depending on the computer's performance capability.

Experimental Results and Analysis for Each Map
This section checks the experimental results (on average, the number of samples, path length, and planning time) of each algorithm: RRT, RRT-Connect, the proposed algorithm in the eight environment maps ( Figure 10) presented in the experimental environment. Each map shows a figure of the path-planning result (of one trial) for each algorithm (Figures 11-18) and the experimental results for the performance measure are shown numerically in a table (Tables 2-9). The figure for each algorithm is for one trial rather than the average of repeated trials and it may differ from the performance measure both visually and by the average numerical performance measure of the repeated trials shown in the table. In particular, the number of samples differs greatly.
The values shown in Tables 2-9 can be expressed as Equations (24) and (25) as Here, A cmp (i) refers to the performance value of each algorithm shown in Tables 2-9, cmp is the algorithm to be compared, i is the index of the environment map (X-axis in Figures 19, 20 and 21b, k is the repeat index, and T is the number of repeats (a cmpk (i) is the value of the performance measure a for the k-th implementation of the cmp algorithm in Map i). Fifty repetitions are performed for the experiment in this paper. That is, Equation A shows the average value of the performance when it is repeated T times to check the performance of a certain algorithm in Map i, Here, x cmp (i) refers to the Y-axis in Figures 19, 20 and 21a and A is the value of the corresponding performance measure of the algorithm to be compared (A RRT is the value of the RRT algorithm).
In each path-planning result figure, the white circles indicate nodes on the path and the yellow line segments indicate edges between nodes. The gray circles and segments are paths (trees) that have been excluded during path planning. In each path-planning result table, based on 100% of the RRT algorithm for each performance measure, the difference is indicated along with the value of the corresponding performance measure unit. Figure 11 shows the path-planning results of Map 1 among the environmental maps for each algorithm. Visually, the number of samples looks similar to the RRT-Connect algorithm in Figure 11b and the proposed algorithm in Figure 11c is comparable to the RRT algorithm in Figure 11a, and the path length looks similar for all three algorithms.  Table 2 shows the path-planning results (after repeating the trial 50 times) in Map 1 for each algorithm. The average number of samples is the smallest in RRT-Connect algorithm at 60%, and the proposed algorithm is 68% compared to the RRT algorithm, which is 8% less efficient than the RRT algorithm compared to the RRT-Connect algorithm. The average path length is shortest for the proposed algorithm at 89% compared to the RRT algorithm, with little difference in the RRT-Connect algorithm at 100%, and 11% less efficient than the proposed algorithm. The average planning time is the shortest for the RRT-Connect algorithm at 58% compared to the RRT algorithm, and the proposed algorithm is 83% compared to the RRT algorithm, i.e., 15% less efficient than the RRT algorithm. Table 2. Experimental result of Map 1 (the parentheses to the right of each value are relative ratios based on RRT 100% (x cmp (1))).  Figure 12 shows the path-planning results of Map 2 among the environmental maps for each algorithm. Visually, the number of samples looks similar for the RRT-Connect algorithm in Figure 12b and the proposed algorithm in Figure 12c compared to the RRT algorithm in Figure 12a, and the path length looks shortest for the proposed algorithm.  Table 3 shows the path-planning result (after repeating the trials 50 times) in Map 2 for each algorithm. The average number of samples is smallest in the RRT-Connect algorithm at 37%, and the proposed algorithm is 42% compared to the RRT algorithm, which is 5% less efficient than RRT algorithm compared to the RRT-Connect algorithm. The average path length of the proposed algorithm is the shortest at 81% compared to the RRT algorithm, while the RRT-Connect algorithm is 98%, which is 17% less efficient than the RRT algorithm compared to the proposed algorithm. The average planning time for the proposed algorithm and the RRT-Connect shows the same performance as the RRT algorithm. 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 (2))). (2) Figure 13 shows the path planning results of Map 3 among the environmental maps for each algorithm. Visually, the number of samples looks similar for the RRT-Connect algorithm in Figure 13b and the proposed algorithm in Figure 13c compared to the RRT algorithm in Figure 13a, and the path length looks shortest for the proposed algorithm.  Table 4 shows the result (after repeating the trial 50 times) of path planning in Map 3 for each algorithm. The average number of samples is smallest in the RRT-Connect algorithm at 75%, and the proposed algorithm is 77% compared to the RRT algorithm, which is 2% less efficient than the RRT algorithm compared to the RRT-Connect algorithm. The average path length of the proposed algorithm is the shortest at 77% compared to the RRT algorithm and the RRT-Connect algorithm is 97%, which is 20% less efficient than the RRT algorithm compared to the proposed algorithm. The average planning time is smallest for the RRT-Connect algorithm at 35%, and the proposed algorithm is 36% compared to the RRT algorithm, which is 1% less efficient than the RRT algorithm compared to the RRT-Connect algorithm. 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 (3))). (3) Figure 14 shows the path planning results of Map 4 among the environmental maps for each algorithm. Visually, the number of samples looks smallest for the RRT-Connect algorithm in Figure 14b compared to the others and the path length looks shortest for the proposed algorithm in Figure 14c.  Table 5 shows the result (after repeating the trial 50 times) of path planning in Map 4 for each algorithm. The average number of samples is smallest in the RRT-Connect algorithm at 10%, and the proposed algorithm is 11% compared to the RRT algorithm, which is 1% less efficient than the RRT algorithm compared to the RRT-Connect algorithm. The average path length of the proposed algorithm is the shortest at 75% compared to the RRT algorithm and the RRT-Connect algorithm is 83%, which is 8% less efficient than the RRT algorithm compared to the proposed algorithm. The average planning time is not different by 100% compared to the RRT algorithm, and the proposed algorithm is 133% compared to the RRT algorithm, i.e., 33% less efficient than the others. Table 5. Experimental result of Map 4 (The parentheses to the right of each value are relative ratios based on RRT 100% (x cmp (4))). (4) Figure 15 shows the path planning results of Map 5 among the environmental maps for each algorithm. Visually, the number of samples looks similar for the RRT-Connect algorithm in Figure 15b and the proposed algorithm in Figure 15c compared to the RRT algorithm in Figure 15a, and the path length looks similar for the RRT-Connect algorithm and the proposed algorithm.  Table 6 shows the results (after repeating the trial 50 times) of path planning in Map 5 for each algorithm. The average number of samples is smallest in RRT-Connect algorithm at 18%, and the proposed algorithm is 20% compared to the RRT algorithm, which is 9% less efficient than the RRT algorithm compared to the RRT-Connect algorithm. The average path length of the proposed algorithm is the shortest at 84% compared to the RRT algorithm and the RRT-Connect algorithm is 106%, which is 22% less efficient compared to the proposed algorithm. The average planning time for the proposed algorithm and the RRT-Connect algorithm is 15% over the RRT algorithm, showing the same performance. 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 (5))). (5) Figure 16 shows the path-planning results of Map 6 among the environmental maps for each algorithm. Visually, the number of samples looks smallest for the proposed algorithm in Figure 16c compared to others, and the path length looks shortest for the proposed algorithm.  Table 7 shows the result (after repeating the trial 50 times) of path planning in Map 6 for each algorithm. The average number of samples is smallest in the proposed algorithm at 26% and the RRT-Connect algorithm is 34% compared to the RRT algorithm, which is 8% less efficient than RRT algorithm compared to the proposed algorithm. The average path length of the proposed algorithm is the shortest at 75% compared to the RRT algorithm, and the RRT-Connect algorithm is 88%, which is 13% less efficient than the proposed algorithm. The average planning time is smallest in the proposed algorithm at 44%, and the RRT-Connect is 67% compared to the RRT algorithm, which is 23% less efficient than the RRT algorithm compared to the proposed algorithm. 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 (6))). (6) Figure 17 shows the path planning results of Map 7 among the environmental maps for each algorithm. Visually, the number of samples looks smallest for the proposed algorithm in Figure 17c compared to others, and the path length looks shortest for the proposed algorithm.  Table 8 shows the result (after repeating the trial 50 times) of path planning in Map 7 for each algorithm. The average number of samples is smallest in RRT-Connect algorithm at 54%, and the proposed algorithm is 56% compared to the RRT algorithm, which is 2% less efficient than RRT algorithm compared to the RRT-Connect algorithm. The average path length of the proposed algorithm is shortest at 75% compared to the RRT algorithm and the RRT-Connect algorithm is 96%, which is 21% less efficient compared to the proposed algorithm. The average planning time is smallest in the proposed algorithm at 60%, and RRT-Connect is 80% compared to the RRT algorithm, which makes it 20% less efficient than the RRT algorithm compared to the proposed algorithm. Table 8. Experimental result of Map 7 (the parentheses to the right of each value are relative ratios based on RRT 100% (x cmp (7))). (7) Figure 18 shows the path planning results of Map 8 among the environmental maps for each algorithm. Visually, the number of samples looks similar for the RRT-Connect algorithm in Figure 18b and the proposed algorithm in Figure 18c compared to the RRT algorithm in Figure 18a, and the path length looks shortest for the proposed algorithm.  Table 9 shows the result (after repeating the trial 50 times) of path planning in Map 8 for each algorithm. The average number of samples is smallest in the proposed algorithm at 17%, and the RRT-Connect algorithm is 18% compared to the RRT algorithm, which is 1% less efficient than RRT algorithm compared to the proposed algorithm. The average path length of the proposed algorithm is the shortest at 84% compared to the RRT algorithm, and the RRT-Connect algorithm is 98%, which is 14% less efficient compared to the proposed algorithm. The average planning time of the proposed algorithm and the RRT-Connect algorithm is 3% over the RRT algorithm, showing the same performance. 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 (8))). (8)

Experimental Results and Analysis in Total
This section comprehensively presents the experimental results (on average, number of samples, path length, and planning time) for each algorithm: RRT, RRT-Connect, and the proposed triangular inequality-based RRT-Connect algorithm, in the eight environmental maps ( Figure 10) shown in Section 5.2. Figures 19a-21a show the performances of the RRT-Connect algorithm and the proposed algorithm when the RRT algorithm's performance is set to 100% for each environment map. The (b) of each figure shows the performance average of all environment maps for each algorithm. The values shown in (a) of Figures 19-21 can be expressed as in Equations (24) and (25) and the values shown in (b) can be expressed as Equation (26) Here, X cmp refers to the Y-axis in (b) of Figures 19-21 and M is the number of environment maps used in the experiment. The experiment in this paper includes eight maps. That is, Equation (26) shows the average value of I for all maps in Equation (25).  As shown in Figure 19b, the average number of samples for all environment maps was 38% less in the RRT-Connect algorithm and 40% less in the proposed algorithm compared to the RRT algorithm. The proposed algorithm is 2% less efficient than the RRT-Connect algorithm. Table 10 is the data table of Figure 19a. The proposed algorithm shows better performance than the RRT-Connect algorithm for Maps 6 and 8 and the RRT-Connect algorithm shows better performance than the proposed algorithm in Maps 1-5 and 7. However, the difference is not significant for most of the maps, such as showing a 2% difference from the map average. There are cases in which the proposed algorithm is 1-8% better than the RRT-Connect algorithm and there are cases in which the RRT-Connect algorithm is 1-8% better than the proposed algorithm.  Figure 20 shows the average path length [%] compared to the RRT algorithm for Maps 1-8 (after repeating the trials 50 times), and the average path length (%) compared with the average result of each algorithm for each map (again after repeating the trials 50 times) where the result of the RRT algorithm was considered as 100%.
As shown in Figure 20b, the average path length for all environment maps was 96% less in the RRT-Connect algorithm and 80% less in the proposed algorithm compared to the RRT algorithm. The proposed algorithm is 16% more efficient than the RRT-Connect algorithm.  Table 11 is the data table of Figure 20a. The proposed algorithm shows better performance than the RRT-Connect algorithm for all maps. The proposed algorithm is 8-21% better than the RRT-Connect algorithm.  Figure 21 shows the average planning time [%] compared to the RRT algorithm for each map (after repeating the trials 50 times), and the average planning time (%) compared with the average result of each algorithm for each map (after repeating the trials 50 times) when the result of RRT algorithm is considered 100%. As shown in Figure 21b, the average planning time for all environment maps was 51% less in the RRT-Connect algorithm and 53% less in the proposed algorithm compared to the RRT algorithm. The proposed algorithm was 2% less efficient than the RRT-Connect algorithm. Table 12 is the data table of Figure 21a. The proposed algorithm shows the same or better performance for Maps 2 and 5-8 than the RRT-Connect algorithm. It shows worse performance for Maps 1, 3, and 4 than the RRT-Connect algorithm. However, most of the maps show no significant difference, such as showing a 2% difference from the map average. There are cases in which the proposed algorithm is 20-23% better than the RRT-Connect algorithm and there are cases where the RRT-Connect algorithm is 1-33% better than the proposed algorithm.

Conclusions
In this paper, we proposed a triangular inequality-based RRT-Connect algorithm using triangular inequality principles to overcome the limitations in the optimality of the RRT-Connect algorithm. We verified the validity of the 'Triangular-Rewiring' method based on the triangular inequality principle and applied it to the RRT-Connect algorithm to bring it closer to the optimum. In addition, to check performance indicators such as the number of samples for finding the first path, path length, and planning time of the proposed algorithm, we compared between the RRT and RRT-Connect algorithms across a total of eight environments through simulation. On average, the proposed algorithm showed 20% better efficiency than the RRT algorithm and 16% better efficiency than the RRT-Connect algorithm in path length and 47% better efficiency than the RRT algorithm in planning time but 2% worse efficiency than the RRT-Connect algorithm. In conclusion, the proposed algorithm showed shorter paths than the RRT-Connect algorithm with a similar number of samples and planning time.
However, one of the limitations of the proposed algorithm is the Kinodynamic planning problem [17]. When the intermediate node disappears by 'Triangular-Rewiring' method, a non-differentiable piecewise linear section with sharp corner may occurs, which cause a problem related with the kinematic constraint of the robot. Informed Consent Statement: Not applicable.

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

Appendix A. Details of the RRT Algorithm
This section shows the pseudocode of the RRT algorithm used in the experiment of this paper, designed based on the paper [13] in which the RRT algorithm was proposed. The RRT algorithm can be represented by one main algorithm (Algorithm A1: AA1) and two additional functions (Algorithm A2: AA2 and Algorithm A3: AA3). 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).
If q 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.
For the tree T thus completed, the distance d 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.
If the number of random samples remains, the above process is repeated. At this time, q 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
This section introduces additional functions used in pseudocode of the RRT algorithm (AA1) in Section A.1. The isTrapped (AA2) function determines whether an obstacle collides, and the isInside function (AA3) determines if the point exists inside the radius.
Algorithm A2 Pseudocode of the isInside Function from the RRT Algorithm Input: q center ← Position {q near /q new } from RRT q target ← Position {q rand /q goal } from RRT λ ← Step Length λ from RRT Output: f ← Result of Boolean f Initialize: f ← False Begin isInside Procedure from RRT 1 d ← Distance of q center to q target 2 If λ ≥ d then 3 f ← True End isInside Procedure from RRT Algorithm A2 shows the isInside function among RRT pseudocodes shown in AA1. In the RRT algorithm, it is determined whether q 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.
In the isInside function, the position to be determined (q 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).
Algorithm A3 shows the isTrapped function among RRT algorithm pseudocodes shown in Algorithm A1. In the RRT algorithm, it is used to determine whether an obstacle exists between the line segment connecting q new and q near .
If the line segment connecting q 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.