On the Enhancement of Optimality in the RRT-Connect Robot Path Planning Algorithm

This paper proposed a Triangular Inequality based rewiring method for the RRT(Rapidly exploring Random Tree)-Connect robot path planning algorithm that guarantees the convergence time than the RRT algorithm, to enhance the optimality. To check the performance of the proposed algorithm, this paper compared with the RRT and RRT-Connect algorithms in various environments through simulation. From these experimental results, the proposed algorithm shows both quick convergence time and better optimality than the RRT algorithm, and more optimal than RRT-Connect algorithm with the similar number of sampling and convergence time.


Introduction
With the recent 4 th Industrial Revolution, interest in mobile robots is increasing in various fields such as robotics, smart factories, and autonomous driving [1]. Classical mobile robot path planning algorithms could be classified into three broad categories [2]. The first is the Road Map Approach algorithm [3], that easy to implement by designing a map that represents a path can be moved and plan through it. The second is Cell Decomposition algorithm [4], that creates a path by dividing the configuration space to cell and connecting each cell using a graph. The last is the Artificial Potential Field algorithm [5], that creates an artificial potential field and moves the robot to the goal according to the flow of potential power.
These classical algorithms include the Optimality, which means how close the created path is to the shortest distance, the Clearance, which means how lower probability of collision between obstacles and a robot, the Completeness, which means that if a path exists it can always be found are considered important and these have been mainly studied [6].
Recently, sampling based path planning algorithms [7][8][9][10][11][12] such as Rapidly exploring Random Tree (RRT) [13] which is quickly and less computationally than classical algorithms, has been attracting attention. The main purpose of the 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, the sampling based algorithm is difficult to fully reflect the optimality and the completeness. Therefore, most of the sampling based algorithms claim Probabilistic Completeness, which explains that can be probabilistically close to completeness when random sampling is repeated infinitely [14]. This means that it is difficult to guarantee the Convergence time, which means how quickly plan the path from a start point to a goal point, and if a path planning is required within a short time, the path differ from the optimal path can be created. Even so, the sampling based algorithm is mainly used in a dynamic environment because it enables more quickly path planning with very little convergence time than the classical algorithms.
To overcome this limitation, many studies are being conducted to expand the RRT algorithm. The RRT-Connect [15] algorithm is an algorithm to find a connected path more quickly than RRT algorithm by setting the starting point and the goal point as the roots of separate trees and expanding the two trees alternately. In addition, there is an algorithm of optimizing a path 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 in order to overcome the limitations of the sampling based method, but they are not perfect, such as not being able to derive the optimal distance, or there is room for improvement in terms of number of operations or time. Therefore, in order to overcome the limitations of this sampling based algorithm, this paper proposes a Triangular Inequality based RRT-Connect algorithm that finds and wires the highest ancestral existing node can be wired while alternately expanding in two trees rooted at the starting point and the goal point. The proposed algorithm shortens the convergence time and at the same time pursues optimization through rewiring. In addition, we will verify the efficiency by comparing the RRT algorithm and RRT-Connect algorithm, which are previous studies, through simulation experiments (This paper proposes a methodology and an algorithm that applies the Triangular Inequality principle to the RRT-Connect algorithm. The RRT*-Smart and Quick-RRT* algorithms that applied the Triangular Inequality principle to the RRT algorithm are not treated as comparative experiments in this paper).  Figure 1 shows an overview of the three algorithms mainly covered in this paper: RRT, RRT-Connect, and the proposal. In this figure, the start point is qstart, and the goal point is qgoal. The RRT algorithm in Figure 1 (a) shows that the path is expanded in a tree structure, and the RRT-Connect algorithm in Figure 1 (b) shows that the trees that are expanded at the start and goal points each attract and connect each other. The proposed algorithm in Figure 1 (c) shows that the RRT-Connect algorithm was rewired into a Triangular Inequality during the path planning.
In this paper, Chapter 2 introduces the RRT algorithm, Chapter 3 introduces the RRT-Connect algorithm, and the Triangular Inequality based RRT-Connect algorithm proposed in Chapter 4. In detail, Section 4.1 shows the pseudocode of the proposed rewire 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, and Section 4.3 shows the pseudocode of each method of the RRT-Connect algorithm applying the proposed rewire method. The experimental environment and results to check the performance of the proposed algorithm in Chapter 5 are shown, and the conclusion is shown in Chapter 6. expand a tree with a root node at the start point by 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 the nodes constituting the tree with the step length. If tree connections are possible, add nodes to create an extended tree.
As mentioned in introduction, this sampling based path planning algorithm uses randomly generated sample points to find one path that can reach the goal as quickly as possible, so it is difficult to sufficiently reflect the optimality and completeness.

Pseudocode 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 (A1) and two additional functions (A2-3 Algorithm 1 shows the pseudocode of RRT algorithm. The root node of the initial tree T has qstart, and this qstart is the start point. From this qstart to the goal position qgoal, random sample is generated N times and aims to reach while the tree is expanded. qrand is position of generated random sample, and in this qrand, a node nearest to the tree T is qnear. At this time, the position qnew created later varies depending on whether qrand is located inside a circle (or n-sphere) with qnear as the center and step length λ as the radius. The function that determines this is isInside (A2), and if qrand is located inside the circle (or n-sphere) (True), qnew becomes qrand, and if it is not located inside (False), qnew becomes intersection point between line segment connecting qrand and qnear, and circle with λ centered at qnear. If there is no obstacle between qnew and qnear (False), qnew is inserted into the tree as a child node of qnear of T. Currently, the function that determines whether an obstacle exists between qnew and qnear is isTrapped (A3) (in the isTrapped function, C refers to the set of obstacles).
If there is the qgoal inside the radius of the λ with the newly inserted qnew as the center, it is considered to have reached the goal point (by isInside function). If it is reached (True), qgoal is inserted as a child node of node qnew of T.
For the tree T thus completed, the distance dreach is calculated for the path Preach to qstart and qgoal. Currently, if dreach is smaller than the optimal path distance doptima or reached first (doptima = 0), the optimal path R becomes Preach, and doptima becomes dreach. 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. At this time, qgoal 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.   Figure 2 (a) shows that qnew is created at the node position qnear of the tree T nearest to the random sample position qrand. Figure 2 (b) shows the optimal path R among several candidate paths to the start position qstart and the goal position qgoal.

Pseudocode of the functions used in the RRT Algorithm
This section introduces additional functions used in pseudocode of the RRT algorithm (A1) in Section 2.1. The isTrapped function determines whether an obstacle collides, and the isInside function determines if the point exists inside the radius.

Algorithm 2.
Pseudocode of the isInside Function from the RRT Algorithm.

Input:
End isInside Procedure from RRT Algorithm 2 shows the isInside function among RRT pseudocodes shown in Algorithm 1. In RRT algorithm, it is determined whether qrand exists inside a circle (or n-sphere) with qnear as the center and λ as the radius, or qgoal exists inside a circle (or n-sphere) with qnew as the center and λ as the radius.
In the isInside function, the position to be determined (qrand, qgoal, ...) is called qtarget, and the center of radius (qnear, qnew, ...) is called qcenter. When the distance between qcenter and qtarget is d, if this d is less than or equal to λ, it is determined that qtarget is the inside position (True). End isTrapped Procedure from RRT Algorithm 3 shows the isTrapped function among RRT algorithm pseudocodes shown in Algorithm 1. In RRT algorithm, it is used to determine whether an obstacle exists between the line segment connecting qnew and qnear.
If the line segment connecting qnew and qnear is lq, and the set of positions formed by the n-th obstacle in the set of obstacles C is c, lc is the i-th and (i + 1)-th positions inserted in c (and the last and 1 st position). It is determined whether it intersects with lq for all line segments lc in the set of positions of all obstacles c that C has. At this time, if any intersect occurs, it returns True and stops the procedure immediately. Otherwise, it determines all the line segments that can be lc and returns False.

The 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 convergence time and inefficient. 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 the start point and the goal point are inserted as each root nodes and extended in each direction alternately. The two trees extending from the start point and the goal point expand as if attracting each other, so that preventing the tree, which is the disadvantage of RRT algorithm, is in a direction irrespective of the goal. This has the enhancement of disadvantage the convergence time required to search for a path. The second is the concept of Extend, which continues to extend to the other side of the tree if there is no collision with an obstacle when the tree extends. Through this, unlike the RRT algorithm that extends the maximum extension length when the sample is generated and is inserted to the tree, the tree continues to expand in the direction of the goal if there is no collision with an obstacle, so that a path can be planned more quickly.
Path planning through the RRT-Connect algorithm can find a path at a quickly than the RRT algorithm, but in a complex environment with narrow paths and many obstacles, the Extend method does not work properly and it can be difficult. Also, the path planned through the RRT-Connect algorithm is far from the optimal distance, so it does not properly reflect the optimality.

Pseudocode of the RRT-Connect Algorithm
This section shows the pseudocode of the RRT-Connect algorithm used in the experiment of this paper, designed based on the paper [15] in which the RRT-Connect algorithm was proposed. The RRT-Connect algorithm can be represented by one main algorithm (A4) and two main methods (A5-6).  End RRT-Connect Procedure Algorithm 4 shows the pseudocode of RRT-Connect algorithm. Each of the initial two trees Ta and Tb has qstart and qgoal as root nodes, and these two trees randomly sample N times and aim to reach each other during expansion. Unlike RRT algorithm, RRT-Connect algorithm is divided into two methods: Extend and Connect. The Extend method (A5) creates qnew from qrand in Ta and extends from Tb to the qnew direction of Ta, and the Connect method (A6) determines whether the two trees Ta and Tb have reached each other, and if they do, merge them into one tree to obtain a path Preach between the root nodes qstart and qgoal of the two trees.
If a path is created in the Connect method, the optimal path R in N sampling times is obtained for this path Preach in the same way as RRT algorithm.

Pseudocode of the Extend method from the RRT-Connect Algorithm
This section introduces the Extend method used in pseudocode (A4) of the RRT-Connect algorithm in Section 3.1.

End Extend Procedure from RRT-Connect
Algorithm 5 shows the pseudocode of the Extend method in the RRT-Connect algorithm. The initial part is the same as the RRT algorithm (A1). isInside function determines whether qrand is inside a circle (or n-sphere) with the node position qnear of the tree Ta nearest to the qrand position as the center and λ as the radius. If it is not located inside (False), qnewA becomes the intersection of the circle (or nsphere) with qnear as the center and λ as the radius, and the line segment connecting qrand and qnear. If it is determined that there is no obstacle between qnewA and qnear by the isTrapped function (False), qnewA is inserted into the tree as a child node of qnear of Ta. If there is an obstacle (True), the Extend method returns True (ftrap) and terminates. If not, proceeds with the remain process, and returns False (ftrap) when the process is over. This is the process of making Ta and Tb reach each other: First, the node of Tb nearest to qnewA becomes the new qnear. At this time, by the isInside function, it is determined whether qnewA is inside a circle (or n-sphere) with qnear as the center and λ as the radius, and if it is located inside (True), qnewB becomes qnear and is located inside If not (False), qnewB becomes the intersection of the circle (or n-sphere) with qnear as the center and λ as the radius, and the line segment connecting qnewA and qnear. If qnewB is created, then repeats the following process until it determine whether there is an obstacle between qnewB and qnear by the isTrapped function and if there is the obstacle between them (True) or that qnewB reaches qnewA by the isInside function.
If there is no obstacle between these qnewB and qnear (False), insert qnewB into Tb as a child node of qnear. At this time, if it is determined by the isInside function that qnewB has not reached the λ radius with qnewA as the center (False), qnear becomes the qnewB, and a new qnewB will created from this qnear.  Figure 3 shows the Extend method in the RRT-Connect algorithm. In detailed, it shows that the first qnewA is created, and qnewB is created by a radius of λ length in the direction of qnewA from the qnear position in the figure. It can be seen that Tb extends in the Ta direction for reach.

Pseudocode of the Connect method from the RRT-Connect Algorithm
This section introduces the Connect method used in pseudocode (A4) of the RRT-Connect algorithm in Section 3.1.

Algorithm 6.
Pseudocode of the Original Connect Method from the RRT-Connect Algorithm. Algorithm 6 shows the pseudocode of the Connect method in the RRT-Connect algorithm. these Ta, Tb and qnewB from the Extend method (A5).

Input:
The tree merging process is as follows: Create a path Pa from the root node (qstart) of Ta to the last inserted node (qnewA), and a path Pb from qnewB of Tb to the root node (qgoal). Then, create the path Pconnect from qnewB of Pb to the last inserted node (qnewA) of Ta, and merge in the order of Pa, Pconnect, and Pb, thereby complete planning the path Pmerged from qstart to qgoal. After that, it returns True (ftrap), and the Connect method ends.

Proposed Triangular Inequality based RRT-Connect Algorithm
The Proposed Triangular Inequality based RRT-Connect algorithm is rewire based on the principle of Triangular Inequality between nodes on a path planned in the RRT-Connect algorithm, thereby increasing the optimality compared to the RRT-Connect. This is similar to the RRT*-Smart algorithm [16] and Quick-RRT* [17] algorithms, which have improved their optimality by using the Triangular Inequality principle for the RRT algorithm. In this paper, the part to rewire based on the Triangular Inequality principle is called the Triangular Rewiring method.
Therefore, this chapter introduce the proposed Triangular Rewiring method for the RRT-Connect algorithm, and mathematical modeling is performed to confirm the validity that the proposed Triangular Rewiring method is always more optimal 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 (A5) and Connect method (A6), 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, Extend method and Connect method to which the proposed Triangular Rewiring method is applied are introduced in this chapter. Algorithm 7 shows the pseudocode of the Triangular Rewiring method applicable in the Extend (A5) and Connect (A6) methods of the RRT-Connect algorithm. When inserting a new node and edge in Ta or Tb in Extend method (A8), when a tree Tmerged (Pmerged) in which Ta and Tb trees are merged in Connect method is created (A9), rewiring is performed on the tree T.

Pseudocode of Proposed Triangular Rewiring Method for the RRT-Connect Algorithm
In the Extend or Connect method, qnew (or qnewA or qnewB) as a qchild and qnear as a candidate for the parent node of the node are insert. And from qparent, the parent node of the node (a second ancestor node candidate based on qchild) is called a qancestor. Next, it is determined whether an obstacle exists between qancestor and qchild (by isTrapped function), and if there is an obstacle (True), the Triangular Rewiring process is skipped, and qchild is inserted into the child node of qparent in T so that the contents of the Extend and Connect method 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 qparent and the edges between qchild and qancestor nodes connected to the qparent. In other words, it disconnects the existing qparent and qchild, and prepares to connect the qchild to the qancestor, the parent node candidate of the qchild. Again, qparent becomes its parent node qancestor, and the qancestor becomes the parent node of qancestor. Then, as previously done, determine an obstacle exists between qancestor and qchild (by isTrapped function). This iterative process is continued until no qancestor exists (When the parent node of the previous qancestor does not exist, that is, when qancestor is qstart) or an obstacle exists between qchild and qancestor. Then, in the tree T, the last created qparent is inserted as the parent node of qchild.

Mathematical Modeling of Proposed Triangular Inequality based RRT-Connect Algorithm
This section introduces the mathematical modeling of the proposed Triangular Inequality based RRT-Connect algorithm. As a result, it is shown that the proposed algorithm is more efficient in terms of optimality than the RRT-Connect algorithm. For reference, this mathematical modeling is based on a two-dimensional Euclidean space.

Basic Mathematical Modeling of the RRT Algorithm
This paragraph introduces basic mathematical modeling in the RRT algorithm. The following Equation 1-5 shows that the coordinate value of qnew is calculated from the coordinate value of qrand in the RRT algorithm: Equation 2-3 shows the relationship between d and λ in Equation 1 through the similarity ratios from qnear to qrand and from qnear to qnew: Through these equations, the x and y coordinate values of qnew can be derived as shown in Equation 4-5. In this case, d ≤ λ refers to a case where qrand exists in a position inside the λ radius.
The following Equation 6-7 shows that the path distance ( ) between an arbitrary node qi and its parent node in RRT algorithm: qi refers to the i-th inserted arbitrary node and has 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. In Equation 6, the distance between an arbitrary node qi and its parent node is obtained, which can be summarized as a function as in Equation 7. At this time, n is the distance between the ancestor node and the parent node of the ancestor node, based on an arbitrary node. That is, the ξ function to the power of n (n ≥ 0) can be represented as ( ) ∶= ( ∘ ∘ … ∘ ⏞ )( ), and when n is 0, 0 ( ) ∶= holds. In addition, suppose that starting with an arbitrary node qi 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 ( ( ), +1 ( )).
Equation [8][9] shows that the total path distance from the start position qstart to the goal position qgoal by RRT algorithm: Equation 8 shows when the (δ + 1)-th ancestor node from qgoal is qstart, where δ is the upper limit of ∑ ( ) =0 in obtaining the total distance of Equation 9. In other words, Equation 9 is the sum of the distances from qgoal to the first ancestor node (parent node) of qgoal, and the distance from the first ancestor node (parent node) of qgoal to the second ancestor of qgoal, …, and (δ -1)-th ancestor node to the δ-th ancestor node (qstart).

Mathematical Modeling of Proposed Triangular Rewiring Method for the RRT-Connect Algorithm
This paragraph shows the mathematical modeling when the Triangular Rewiring method (A7) of Section 4.1 is applied to the RRT algorithm and the mathematical modeling when applied to the RRT-Connect algorithm.
(a) (b) (c) Figure 5. Abstract process of the Triangular Rewiring method: (a) Example for tree; (b) After rewired between qchild and qancestor; (c) At this time, the α is a distance between qchild and qparent, the β is a distance between qparent and qancestor, and the γ is a distance between qchild and qancestor. Figure 5 shows an abstract process of the Triangular Rewiring method. As shown in Figure 5 (a), if parent node of qchild is qparent and parent node of qparent is qancestor, and qancestor is the second ancestor of qchild, this can be represented as Equation 10: If the distances of the edge connecting each node are, the α between qchild and qparent, the β between qparent and qancestor, and the γ between qchild and qancestor as shown in Figure 5 (c), it can be represented as Equation 11 using the principle of the triangular inequality: The following Equations 12-13 show the distance relation between ancestor nodes of qchild: ∴ ( ℎ , ( ℎ )) + ( ( ℎ ), 2 ( ℎ )) ≥ ( ℎ , 2 ( ℎ )), Equation 12 can be summarized to Equation 13 by substituting Equation 10, which represents the relationship between the n-th ancestor nodes of qchild, with the distance as Equation 6 in Equation 11, which represents the distance between each node as a triangular inequality.
The following Equation 14-20 shows that the path of the RRT algorithm applying the Triangular Rewiring method is always shorter or equal to the path planned by the original RRT algorithm. Equation 14 shows the sequence index kj for comparing the distance when the Triangular Rewiring method is applied with the distance when this method is not applied: In this case, j is a sequence index for . That is, kj can be said to be a sequence index for . Currently, is the number of times that rewire occurs in the j-th. If this is summarized by Equation 6 for a distance based on an arbitrary node qi, it is as Equation 15. For example, as shown in Figure 5, if j is 0 and 1 rewire occurs ( 0 = 1), it can be represented in combination with the distance relationship of Equation 13 for qchild, as in Equation 16: The result of Equation 16 can be generalized as shown in Equation 17: For based on an arbitrary node qi, it is shown that the total distance ∑ = from the j-th to kj-th arbitrary sequence index is always longer or equal to the distance of the kj-th sequence index.
That is, in an arbitrary path, it can be confirmed that the distance rewired by the Triangular Rewiring method is at least the equal (If the distances of and are the same, the rewired line segments are on a straight line) or always shorter in distance than when not rewired.  Figure 6 shows the Triangular Rewiring process for the path from qstart to qgoal based on Equation 10-17 (at this time, it is assumed that the node of the path shown in the figure does not positioned in a straight line). As shown in Figure 6 (b), a total of 2 rewires occurred ( 0 = 2) between q0 and q3 ( 3 ( 0 )), and a total of 1 rewire occurred ( 3 = 1) between q5 ( 5 ( 0 )) and q7 ( 7 ( 0 )). In that case, as shown in Figure 6 (e), k0 is 2, k1 is 3, k2 is 4, and k3 is 6 by Equation 14.
Comparing Figure 6 (c) and Figure 6 (e), according to Equation 17, the rewired distance 2 ( 0 ) is shorter than the total distance ∑ ( 0 ) 2 =0 from 0 to 2 , the rewired distance 6 ( 0 ) is shorter than the total distance ∑ ( 0 ) 6 =5 from 5 to 6 . That is, when comparing before applying the Triangular Rewiring method in Figure 6 (a) and after applied this method in Figure 6 (f), applied the method looks shorter.
The following Equation 18-19 shows the total distance when the Triangular Rewiring method is not applied and the total distance when the method is applied, for an arbitrary path (start position: qstart, goal position: qgoal), as shown in Figure 6: Equation 18 shows the upper limit when the index n of d is δ in Equation 8, and when this is substituted into the sequence index kj, if kj is δ, j becomes φ. In that case, as in Equation 19, is to compare the ∑ ( ) =0 shown in Equation 9 with , reflecting the sequence kj, it can be represented as ∑ ∑ ( )
The following Equation 20 shows that the equation summarized in Equation 19 is substituted into Equation 17: Finally, as can be confirm in Equation 20, as the result of applying the Triangular Rewiring method to the distance of an arbitrary path (start position: qstart, goal position: qgoal) is at least equal (If the distances of and are the same, when the rewired line segments are on a straight line) to or always shorter than as the result of this method is not applied. The following Equation 21-23 shows the total distance of the path from the start position (root node) of Ta to the last (inserted node) position qnewA, and the total distance when the Triangular Rewiring method is applied to the path. Also, it shows that is at least equal to or always shorter than : The following Equation 24-26 shows the total distance of the path from the start position (root node) of Tb to the last (inserted node) position qnewB, and the total distance when the Triangular Rewiring method is applied to the path. Also, it shows that is at least equal to or always shorter than : As a result, the following Equations 27-28 show that RRT-Connect to which the proposed Triangular Rewiring method is applied is at least the same or better in terms of optimality (total path distance) than RRT-Connect algorithm to which the method is not applied: (Eq. 9), which refers to the total distance of the RRT-Connect algorithm path when the Triangular Rewiring method is not applied, represented by the sum of the distance of the partial path Pa (Eq. 22), the distance of the partial path Pb (Eq. 25), and the distance ( , ) between qnewA and qnewB as shown in Equation 27.
And (Eq. 19), which refers to the total distance of the RRT-Connect algorithm path when the Triangular Rewiring method is applied, represented by equal to or shorter than the sum of the distance of the partial path Pa of the RRT-Connect (Eq. 22), the distance of the partial path Pb (Eq. 25), and the distance ( , ) between qnewA and qnewB as shown in Equation 27. In that case, Equation 28 shows that is at least equal to or shorter than in the RRT algorithm summarized in Equation 20, and it is also efficiently used in the RRT-Connect algorithm.

Pseudocode of Proposed Extend Method for the RRT-Connect Algorithm
This section introduces the Extend method in the proposed Triangular Inequality based RRT-Connect algorithm. This proposed Extend method (A8) replaces the Extend method (A6) in pseudocode of the RRT-Connect algorithm (A5).

Input:
Ta ← Tree Ta

End Extend Procedure from RRT-Connect
Algorithm 8 is the application of the Triangular Rewiring method (A7) to the original Extend method (A5) of the RRT-Connect algorithm. Compared to the original Extend method, in lines 9 and 16, the part where a node is newly inserted in the tree is inserted through the Triangular Rewiring method. Other than that, the contents are the same as the original Extend method.

Pseudocode of Proposed Connect Method for the RRT-Connect Algorithm
This section introduces the Connect method in the proposed Triangular Inequality based RRT-Connect algorithm. This proposed Connect method (A9) replaces the Connect method (A7) in pseudocode of the RRT-Connect algorithm (A5). Algorithm 9 is an application of the Triangular Rewiring method (A7) to the Connect method (A6) of the RRT-Connect algorithm. Compared to the original Connect method, it has been changed to applying the method to the merged tree by considering the Triangular Rewiring method when merging the path, which is lines 5-10. Other than that, the contents are the same as the original Connect method.

Algorithm 9. Pseudocode of the Proposed Connect
When paths Pa and Pb are merged in a tree structure of line 5, nodes on the path are inserted in the order of Pa, Pconnect, and Pb in the merged tree Tmerged. That is, in Tmerged, the root node becomes qstart, and when the n-th inserted node at a certain point is qnewA, which is the last inserted node of Ta, the (n + 1)-th inserted node becomes qnewB, which is the last inserted node of Tb. Also, the last inserted node of Tmerged becomes qgoal.
Then, Triangular Rewiring method is applied to this Tmerged. Since it is applied to the tree itself, it determined whether rewire is possible for all nodes inserted in the tree, and if possible, rewires and updates the tree. However, since each node from Ta to Tb is inserted into Tmerged, it is not necessary to rewire Ta for which the Triangular Rewiring process has already been performed. Therefore, the Triangular Rewiring process proceeds in the direction of Tb from the qnewA sequence inserted in Tmerged. At this time, if qnewA is the i-th inserted node, the first node pair to be determined is (i -1)-th node qnew (as qchild) and i-th node qnear (as qparent). When all the nodes inserted in Tmerged are determined, the tree structure Tmerged is converted to the path Pmerged, and the method is terminated (True).   To do in Figure 9 is to plan a path from the start position qstart to the goal position qgoal through the proposed algorithm, as shown in Figure 9 (a).

Process of Proposed Triangular Inequality based RRT-Connect Algorithm
In Figure 9 (b), the first random sample is generated at the qrand position, and qnewA is created at a position separated by the length of λ from qstart in the direction of the position, and qnewA is extended once by the length of λ in the direction of qnewA from qgoal. At this time, since there is no intermediate node between qnewA and qstart, the Triangular Rewiring process is skipped.
In Figure 9 (c), a second random sample is generated at the qrand position, and in the direction of the position, qnewA is updated at a location separated by λ length from the nearest node qnear in the tree and rewired between qnewA and qgoal. In this case, since the tree on the opposite side collides with an obstacle to extend in the qnewA direction, the Extend process is skipped. Also, it is assumed that Swap occurs between Ta with initial qstart as the root node and Tb with initial qgoal as the root node between each figure.
In Figure 9 (d), as shown in Figure 9 (c), a third random sample is created at the qrand position, and at a position that is separated by the length of λ in the position direction, at the node qnear nearest among the nodes in the tree in the position direction, It shows updating qnewA to a position that is the length of λ and rewires it between qnewA and qstart. In this case, also since it collides with an obstacle to extend in the direction qnewA from the tree on the opposite side, the Extend process is skipped.
In Figure 9 (e), the fifth random sample is generated at the qrand position, and qnewA is located at a position separated by the length of λ in the direction of the position, and qnewA at a position separated by the length of λ from the nearest node qnear among nodes in the tree toward the position It is shown that updating is and that qnewA is merged into one tree through the Connect process because qnewA has entered the range with the center of qnewB and the radius of λ. It is assumed that the fourth random sample between Figure 9 (d) and Figure 9 (e) is generated inside the obstacle, so that the qnewA generation process is skipped. The last Figure 9 (f) shows the result of path R created as a merged tree by Connect as shown in Figure 9 (e).

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 with each other in various environment maps shown in the experimental environment through the simulator.
Each algorithm was implemented based on the pseudocode shown in Chapters 2 to 4, and the compared performance measure is the average, Number of sampling, Optimal path distance (pixels), and Convergence time (milliseconds) for all trials for 50 times (from the start point to the goal position until the first path plan is completed). Among the performance measures, as the number of sampling decreases, the cost of recalculation in a dynamic environment is decreases, and the optimal path distance is a measure of the optimality of path planning algorithm.

Experimental Environment
This section introduces the environment map used in the simulation, and the simulator used in the simulation with the performance of the computer.
The following Figure 10 shows the eight environmental maps used in the experiment. The green circle (S) means to the start position, the purple circle (G) means to the goal position, and the black polygon on the yellow (blue in the analysis of the experimental results) border means to the obstacle. And all maps are 600 (horizontal) * 600 (vertical) pixels. To verify the performance of various path planning algorithms including the RRT algorithm, many environmental maps were considered and used [22][23][24][25]. It is important which environment map to use because the expected performance measure varies depending on the placement, shape, and other property of obstacles.
In this paper, to check the performance of the proposed algorithm, the eight maps shown in Figure 10 were benchmarked in the experimental environment of the paper [26] proposed by Jihee Han in 2017, and each map is expected to have the following features: Map 1 in Figure 10 (a) seems to be an environment that it is easy to verify the completeness of the path planning algorithm. Map 2 in Figure 10 (b) seems to be an environment that it is also easy to verify the completeness of the path planning algorithm, and it is the environment mainly used to show the solution of the Local Minima problem in the Artificial Potential Field algorithm [25]. Map 3 in Figure 10 (c) seems to be an environment that 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 of Figure 10 (d) seems to be an environment that it is easy to verify the optimality and convergence time of the path planning algorithm, and the Cell Decomposition algorithm, which increases the computation cost as the angle of obstacle increases, is an unfavorable environment [27]. Map 5 in Figure 10 (e) seems to be an environment that it is also easy to verify the optimality and convergence time of the path planning algorithm, and for the same reason as Map 4, the Cell Decomposition algorithm is an unfavorable environment. Map 6 in Figure 10 (f) seems to be an environment that it is easy to verify the optimality, completeness and convergence time of the path planning algorithm, and it is an environment for comprehensively evaluating the performance. Map 7 in Figure 10 (g) seems to be an environment that it is easy to verify the completeness and optimality of the path planning algorithm, and for the same reason as Map 2, it can be seen as the environment used in the Artificial Potential Field algorithm. Lastly, Map 8 in Figure 10 (h) seems to be an environment that is easy to verify the completeness and convergence time of the path planning algorithm and is an environment 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 sampling and the convergence time are extremely increased and longer as there are narrow or fewer entrances of directions to the goal.  Table 1 shows the performance 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 is used for calculation. The difference in convergence time may occur depending on the computer performance.

Analysis of Experimental Results of Each Map
This section checks the experimental results (on average, number of sampling, optimal path distance, convergence time) of each algorithm: RRT, RRT-Connect, the proposal in the eight environment maps (Fig. 10) presented in the experimental environment. In each map, a figure of the path planning result (of one trial) for each algorithm is shown, and the experimental results for the performance measure are shown numerically in a table (The figure for each algorithm is for one trial, not the average of repeated trials, and it may be different from the performance measure by visually and the average numerical performance measure of the repeated trials shown in the table. In particular, the number of sampling differs a lot).
In each path planning result figure, the white circle means the node on the path, and the yellow line segment means the edge between each node. Gray circles and segments are paths (trees) that are 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. In visually, the number of sampling looks similar for the RRT-Connect algorithm in Figure  11 (b) and the proposed algorithm in Figure 11 (c) compared to the RRT algorithm in Figure 11 (a), and the optimal path distance looks similar for all three algorithms.  Table 2 shows the result (for repeated trial 50 times) of path planning in Map 1 for each algorithm. The average number of sampling is the least in RRT-Connect algorithm at 60%, and the proposed algorithm is 68% compared to RRT algorithm, which is 8% less efficient than RRT algorithm compared to RRT-Connect algorithm. The average optimal path distance is the 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 convergence time is the shortest by the RRT-Connect algorithm at 58% compared to the RRT algorithm, and the proposed algorithm is 83% compared to the RRT algorithm, which is 15% less efficient than the RRT algorithm.  Figure 12 shows the path planning results of Map 2 among the environmental maps for each algorithm. In visually, the number of sampling looks similar for the RRT-Connect algorithm in Figure  12 (b) and the proposed algorithm in Figure 12 (c) compared to the RRT algorithm in Figure 12 (a), and the optimal path distance looks shortest for the proposed algorithm.  Table 3 shows the result (for repeated trial 50 times) of path planning in Map 2 for each algorithm. The average number of sampling is the least in RRT-Connect algorithm at 37%, and the proposed algorithm is 42% compared to RRT algorithm, which is 5% less efficient than RRT algorithm compared to RRT-Connect algorithm. The average optimal path distance of the proposed algorithm is the shortest at 81% compared to the RRT algorithm, and the RRT-Connect algorithm is 98%, 17% less efficient than the RRT algorithm compared to the proposed algorithm. The average convergence time of the proposed algorithm and the RRT-Connect shows the same performance over the RRT algorithm.  Figure 13 shows the path planning results of Map 3 among the environmental maps for each algorithm. In visually, the number of sampling looks similar for the RRT-Connect algorithm in Figure  13 (b) and the proposed algorithm in Figure 13 (c) compared to the RRT algorithm in Figure 13 (a), and the optimal path distance looks shortest for the proposed algorithm.  Table 4 shows the result (for repeated trial 50 times) of path planning in Map 3 for each algorithm. The average number of sampling is the least in RRT-Connect algorithm at 75%, and the proposed algorithm is 77% compared to RRT algorithm, which is 2% less efficient than RRT algorithm compared to RRT-Connect algorithm. The average optimal path distance of the proposed algorithm is the shortest at 77% compared to the RRT algorithm, and the RRT-Connect algorithm is 97%, 20% less efficient than the RRT algorithm compared to the proposed algorithm. The average convergence time is the least in RRT-Connect algorithm at 35%, and the proposed algorithm is 36% compared to RRT algorithm, 1% less efficient than RRT algorithm compared to RRT-Connect algorithm.  Figure 14 shows the path planning results of Map 4 among the environmental maps for each algorithm. In visually, the number of sampling looks least for the RRT-Connect algorithm in Figure  14 (b) compared to others, and the optimal path distance looks shortest for the proposed algorithm in Figure 14 (c).  Table 5 shows the result (for repeated trial 50 times) of path planning in Map 4 for each algorithm. The average number of sampling is the least in RRT-Connect algorithm at 10%, and the proposed algorithm is 11% compared to RRT algorithm, 1% less efficient than RRT algorithm compared to RRT-Connect algorithm. The average optimal path distance of the proposed algorithm is the shortest at 75% compared to the RRT algorithm, and the RRT-Connect algorithm is 83%, 8% less efficient than the RRT algorithm compared to the proposed algorithm. The average convergence time is not different by 100% compared to the RRT algorithm, and the proposed algorithm is 133% compared to RRT algorithm, 33% less efficient than others.  Figure 15 shows the path planning results of Map 5 among the environmental maps for each algorithm. In visually, the number of sampling looks similar for the RRT-Connect algorithm in Figure  15 (b) and the proposed algorithm in Figure 15 (c) compared to the RRT algorithm in Figure 15 (a), and the optimal path distance also looks similar for the RRT-Connect algorithm and proposed algorithm.  Table 6 shows the result (for repeated trial 50 times) of path planning in Map 5 for each algorithm. The average number of sampling is the least in RRT-Connect algorithm at 18%, and the proposed algorithm is 20% compared to RRT algorithm, 9% less efficient than RRT algorithm compared to RRT-Connect algorithm. The average optimal path distance 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 convergence time of the proposed algorithm and the RRT-Connect algorithm is 15% over the RRT algorithm, showing the same performance.  Figure 16 shows the path planning results of Map 6 among the environmental maps for each algorithm. In visually, the number of sampling looks least for the proposed algorithm in Figure 16 (c) compared to others, and the optimal path distance also looks shortest for the proposed algorithm.  Table 7 shows the result (for repeated trial 50 times) of path planning in Map 6 for each algorithm. The average number of sampling is the least in proposed algorithm at 26%, and the RRT-Connect algorithm is 34% compared to RRT algorithm, 8% less efficient than RRT algorithm compared to proposed algorithm. The average optimal path distance 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 compared to the proposed algorithm. The average convergence time is the least in the proposed algorithm at 44%, and the RRT-Connect is 67% compared to RRT algorithm, 23% less efficient than RRT algorithm compared to proposed algorithm.  Figure 17 shows the path planning results of Map 7 among the environmental maps for each algorithm. In visually, the number of sampling looks least for the proposed algorithm in Figure 17 (c) compared to others, and the optimal path distance also looks shortest for the proposed algorithm.  Table 8 shows the result (for repeated trial 50 times) of path planning in Map 7 for each algorithm. The average number of sampling is the least in RRT-Connect algorithm at 54%, and the proposed algorithm is 56% compared to RRT algorithm, 2% less efficient than RRT algorithm compared to RRT-Connect algorithm. The average optimal path distance of the proposed algorithm is the 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 convergence time is the least in the proposed algorithm at 60%, and the RRT-Connect is 80% compared to RRT algorithm, 20% less efficient than RRT algorithm compared to proposed algorithm.  Figure 18 shows the path planning results of Map 8 among the environmental maps for each algorithm. In visually, the number of sampling looks similar for the RRT-Connect algorithm in Figure  18 (b) and the proposed algorithm in Figure 18 (c) compared to the RRT algorithm in Figure 18 (a), and the optimal path distance looks shortest for the proposed algorithm.  Table 9 shows the result (for repeated trial 50 times) of path planning in Map 8 for each algorithm. The average number of sampling is the least in proposed algorithm at 17%, and the RRT-Connect algorithm is 18% compared to RRT algorithm, 1% less efficient than RRT algorithm compared to proposed algorithm. The average optimal path distance 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 convergence time of the proposed algorithm and the RRT-Connect algorithm is 3% over the RRT algorithm, showing the same performance.

Analysis of the Total Experimental Results
This section comprehensively present the experimental results (on average, number of sampling, optimal path distance, convergence time) of each algorithm: RRT, RRT-Connect and Proposed Triangular Inequality based RRT-Connect Algorithm, in the 8 environmental maps (Fig. 10) shown in Section 5.2.  Figure 19 shows the average number of sampling [%] compared with the RRT algorithm for each map from Map 1 to Map 8 (for repeated trial 50 times), and the average number of sampling [%] compared with the average result of each algorithms for each map (for repeated trial 50 times) when the result of RRT algorithm is considered as 100%.
As shown in Figure 19 (b), the average number of sampling 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.  60  37  75  10  18  34  54  18  38   Proposal  68  42  77  11  20  26  56  17  40   Table 10 is the data table of Figure 19 (a). The proposed algorithm shows better performance for Map 6 and Map 8 than the RRT-Connect algorithm, and the RRT-Connect algorithm shows better performance than the proposed algorithm in Map 1, Map 2, Map 3, Map 4, Map 5 and Map 7. However, most of the map there is no significant difference, such as showing a 2% difference from the map average. There are cases where the proposed algorithm is max 8% to min 1% better than the RRT-Connect algorithm, and there are cases where RRT-Connect algorithm is max 8% to min 1% better than the proposed algorithm.  Figure 20 shows the average optimal path distance [%] compared with the RRT algorithm for each map from Map 1 to Map 8 (for repeated trial 50 times), and the average optimal path distance [%] compared with the average result of each algorithms for each map (for repeated trial 50 times) when the result of RRT algorithm is considered as 100%.

RRT-Connect
As shown in Figure 20 (b), the average optimal path distance 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 20 (a). The proposed algorithm shows better performance for all maps than the RRT-Connect algorithm. The proposed algorithm is max 21% to min 8% better than the RRT-Connect algorithm.  As shown in Figure 21 (b), the average convergence 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 is 2% less efficient than the RRT-Connect algorithm.  Table 12 is the data table of Figure 21 (a). The proposed algorithm shows the same or better performance for Map 2, Map 5, Map 6, Map 7 and Map 8 than the RRT-Connect algorithm. And it shows better performance for the Map1, Map 3 and Map 4 than the algorithm proposed by the RRT-Connect algorithm. However, most of the map there is no significant difference, such as showing a 2% difference from the map average. There are cases where the proposed algorithm is max 23% to min 20% better than the RRT-Connect algorithm, and there are cases where the RRT-Connect algorithm is also max 33% to min 1% better than the proposed algorithm.

Conclusion
In this paper, we proposed a Triangular Inequality based RRT-Connect algorithm using Triangular Inequality principles to overcome the limitation of the optimality of RRT-Connect algorithm. We verified the validity of the Triangular Rewiring method based on Triangular Inequality principle and applied it to the RRT-Connect algorithm to enhance the optimality. Also, to check the performance such as number of sampling for first path finding, optimal path distance and convergence time of the proposed algorithm, we compared between the RRT and RRT-Connect algorithms in a total of 8 environments through simulation. The proposed algorithm showed, on average, 20% better efficiency than RRT algorithm and 16% better efficiency than RRT-Connect algorithm in the optimality, and 47% better efficiency than the RRT algorithm in the convergence time, but 2% slightly worse efficiency than RRT-Connect algorithm. In conclusion, the proposed algorithm shows more optimal than RRT-Connect algorithm with similar number of sampling and convergence time.