An Enhancement Method of Rapidly-exploring Random Tree Robot Path Planning using Midpoint Interpolation

To solve the problem that sampling-based Rapidly-exploring Random Tree (RRT) method is difficult to guarantee optimality. This paper proposed the Post Triangular Processing of Midpoint Interpolation method minimized the planning time and shorter path length of the sampling-based algorithm. The proposed Post Triangular Processing of Midpoint Interpolation method makes a closer to the optimal path and somewhat solves the sharp path problem through the interpolation process. The experiments were conducted to verify the performance of the proposed method. Applying the method proposed in this paper to the RRT algorithm increases the efficiency of optimization compared to the planning time.


Introduction
The goal of path planning [1] is to plan a path for a mobile robot to proceed from a starting point to a destination point in Euclidean space as efficiently as possible while avoiding obstacles while maintaining optimality, clearance, and completeness. optimality refers to always planning a path with the ideal path length, clearance refers to how unlikely it is for obstacles and the mobile robot to collide. completeness is defined as the property to construct a path from a start point to a destination point without colliding with obstacles.
Sampling-based RRT (Rapidly-exploring Random Tree) algorithm [2], it is difficult to ensure optimality. As shown in Figure 1(a), the RRT algorithm is a path planning that involves repeatedly adding a randomly sampled position as a child node in a tree with the starting point as the root node until the destination point is reached. The tree extends out in the shape of a stochastic fractal as shown in Figure 1(b) and has an algorithm of locating the destination point with this algorithm.
The RRT algorithm and other sampling-based algorithms [3][4] offer the benefit of planning a path in a shorter time with less computing than traditional path planning algorithms like Visibility Graph-based [5], Cell Decomposition-based [6], and Potential Field-based [7]. On the other hand, it does not ensure optimality and has the drawback of being probabilistically assured completeness. The latter is also known as Probabilistic completeness [8], which implies that completeness is assured when the number of random samples is infinite, but not always when the number of random samples is limited. The goal of this research is to look at the RRT algorithm, which ensures completeness and performs better than the related works. The proposed "Post Triangular Processing of Midpoint Interpolation" method is effective in path planning algorithms that do not guarantee optimality, such as the RRT algorithm that locally piecewise linear shape, and can be used as a post-processing method after a path has been planned using one of these algorithms.
The sampling-based path planning algorithm's primary strength is the fast-planning speed based on the small amount of computation compared to the traditional path planning algorithms [3]. Since this proposed method has this strength, should not be slow compared to the RRT algorithm.
Performance verification using simulation in various environments and mathematical modeling was used to validate the performance of the proposed method in this paper. The case in which the proposed algorithm to the sampling-based path planning method is applied and the case in which it is not applied are compared through simulation. Here, the planning time and path length of the first complete path that first reaches a destination point from a starting point are evaluated.

Rapidly-exploring Random Tree (RRT)
Steven M. LaValle proposed the RRT algorithm in 1998 [2], which is a representative algorithm of the sampling-based path planning algorithm. It is designed to have a lot of degrees of freedom and is useful for planning a path under non-holonomic constraints. As shown in Figure 2, when a random sample is generated in the Configuration Space, the nearest node to the position of the random sample is identified among the nodes constituting the tree with the starting point as the root node. A new node is generated at the random sample position and inserted into the tree if the random sample position is nearer than the step length. The process of tree extension is repeated till the destination point is achieved.

Proposed Post Triangular Processing Method of Midpoint Interpolation
The proposed Post Triangular Processing of Midpoint Interpolation method can be applied to path planning algorithms that do not guarantee optimality such as RRT (Rapidly-exploring Random Tree) algorithm, rewire and midpoint interpolation based on the triangular inequality principle.
(a) (b) Figure 3. Summary of Post Triangular Processing of Midpoint Interpolation method: (a) When the line segment γ with node q0 and its grandparent node q2 in tree R is not free from obstacle collision; (b) Grandparent node q2 of node q0 is connected as the parent node of node q0 and the parent node q1 is deleted from the tree.
The basic principle is that the node serving as a waypoint in the planned path checks whether an obstacle collides with its own grandparent node, and if it is free from obstacle collision, the grandparent node rewires to the parent node. If it is not free from obstacle collision, as shown in Figure 3, the locally piecewise linear shape path created between the node, its parent node and grandparent node is made a more optimal path through the interpolation process. In this process, a new node is interpolated into the path and deviates from the piecewise linear, so it has the advantage of being able to somewhat solve the sharp path problem (The problem that a mobile robot that has kinematic constraints because the slope is not smooth).  Figure 4. Interpolation function of Post Triangular Processing of Midpoint Interpolation method: (a) When the height d0 of a triangle formed by waypoints q0, q1 and q2 of the path is greater than ε (interpolation continuation); (b) When the height d1 of the triangle formed by the midpoints mF(q0,1) and q1 of q0 and q1 and the midpoint mF(q1,1) of q1 and q2 is less than ε (interpolation stop).
This Post Triangular Processing of Midpoint Interpolation method was designed based on the Polygon approximation algorithm [9][10], so as shown in Figure 4, the path is calculated through a constant value called ε (the threshold of minimum clearance) (ε > 0). It determines how closely the obstacle is approximated, or in other words, how close to the optimal path it is. Here in Figure 4, d follows Equation 1: For the waypoint qi, the value of d decreases by 1/2 as interpolation proceeds (n). The initial value d0 is the height of the triangle formed by the three line segments α, β, γ (γ<α+β) and the value of dn become (dn-1)/2, when let α be the line segment from qi to the first and next waypoints, β to be the line segment from qi to the first next waypoint and the second next waypoint, γ to be the line segment from qi to the second next waypoint. This d values as a measure to confirm the clearance of the obstacle compared to ε. This is because the smaller the d, the closer the path to the obstacle. However, since optimality and clearance are opposite attributes, the closer ε is to 0 as shown in Figure 5, the more similar the path or path length to the visibility graph, but it is not smooth path [11], The farther away from 0 (within a significant value where the path is modified), the farther from the optimum, but a smooth (kinetic) path is made, so ε should be set to a suitable value according to the environment.
The following Algorithm 1 shows the pseudocode of the proposed Post Triangular Processing of Midpoint Interpolation method. It is mainly composed of Post Triangular function (Algorithm 2) and Interpolation function (Algorithm 3). While fmodify Do If not isTrapped(qchild, qancestor, C) Then The input value of the Post Triangular Processing of Midpoint Interpolation method consists of the path R planned through the path planning algorithm such as the RRT algorithm, the obstacle area information C, and the threshold value ε of the minimum clearance.
The fmodify is a variable that determines whether the input path R has been modified by this method, and if the path is modified even once, the entire process is repeated. If the path modification does not occur in the process of repeating again, the algorithm is terminated. t refers to the index of the waypoint of R that is currently focused. That is, if t is 0, it means the starting point, which is the first point of R.
In R, when the first focusing point is qchild, the next point of that point qchild is qparent, and the next point of that point qparent is mentioned qancestor, it is determined whether the distance between qchild and qancestor is free from obstacle collision (isTrapped() function). If it is free from collision, it calls postTriangular(), otherwise it calls interpolation(). postTriangular() connects qchild and qancestor like the Triangular Rewiring method [4], and the qparent between them is deleted from the path. interpolation() finds (interpolates) the point between qchild and qparent, and between qparent and qancestor that are free from obstacle collision when connected and rewire qchild, qancestor and those two points found. If R and t are updated due to postTriangular() or interpolation(), update qchild (the t-th waypoint of R), qparent and qancestor accordingly. If qparent is the last point in R, check fmodify. Otherwise, repeat the above process again for the updated qchild and qancestor.
Here, path modification by postTriangular() deletes the waypoints and makes a more optimal path, but has the effect of sharpening the path shape, and path modification by interpolation() creates new waypoint between the waypoints. Adding/inserting has the effect of making a more optimal path while also smoothing the path shape. Of course, in the effect of making a more optimal path, path modification by postTriangular() is more efficient than path modification by interpolation().
The input value of postTriangular() of the Post Triangular Processing of Midpoint Interpolation method consists of the path R, the focusing point index t, and the path modification fmodify from the Post Triangular Processing of Midpoint Interpolation method.
Rewiring is performed on the t-th waypoint qchild of R, the next point qparent, and the next point qancestor of that point again. First, delete the path between qchild and qparent, and the path between qparent and qancestor. Then insert the path between qchild and qancestor. Finally, fmodify returns 'true' because the path has been modified. Here, Path<A, B> means a partial path from the waypoint A to the waypoint B in the complete path.  mF(q0,1), q1 and the midpoint mF(q1,2) between q1, the interpolation point mF(q1,1) are free from obstacle collision.
The proposed Post Triangular Processing of Midpoint Interpolation method is to find a free interpolation point (mF(q0), mF(q1)) from obstacle collisions between waypoints (q0~q1, q1~q2) while descending in the direction of the midpoint (q1) as shown in Figure 6 in the interpolation process.
However, the interpolation point mF follows Equation 2: When the k-th interpolation point of the interpolation point qi is mF(qi,k), the 0-th interpolation point becomes itself qi, and the 1-st interpolation point is the midpoint of qi and the point ξ(qi) next to qi. , and the 2-nd interpolation point becomes the midpoint of mF (qi,1) and ξ(qi). Here, ξ() is a function that receives the node as a variable and returns the parent node of that node. The n-squared (n≥0) of the ξ() function can be expressed as ( ) ∶= ( ∘ ∘ … ∘ ⏞ )( ), and if n is 0, 0 ( ) ∶= holds. That is, mF (qi,k) (k>0) becomes the midpoint between mF (qi,k-1) and ξ(qi). At this time, d becomes (dk-1)/2.
The following Algorithm 3 shows the pseudocode of interpolation() of the proposed Post Triangular Processing of Midpoint Interpolation method.

End
The input value of interpolation() of the Post Triangular Processing of Midpoint Interpolation method consists of the path R, the obstacle area information C, the focusing point index t, and the path modification fmodify from the Post Triangular Processing of Midpoint Interpolation method.
From the t-th waypoint qchild of R, the next point qparent, and the next point qancestor of that point, the height d of the triangle is obtained, ma is the midpoint of qchild and qparent, and mb is the midpoint of qparent and qancestor. If the path between ma and mb is free from obstacle collision (isTrapped()), delete the path between qchild and qparent, and insert the path between qchild and ma, the path between ma and mb, and the path between mb and qancestor. Also, since there is a modified the path, fmodify becomes 'true', returns it, and the method terminates. If the line segment between ma and mb is not free from obstacles, the value of d is decreases by 1/2, ma is updated to the midpoint of ma and qparent, and mb is updated to the midpoint of mb and qparent, determine whether ma and mb are free from obstacles again. This repeat process proceeds until a case is found in which ma and mb are free from obstacles or d becomes smaller than ε. If d becomes smaller than ε, the value of t is increased by 1 and the method is terminated. The following Figure 7 shows the overall flow chart of the proposed Post Triangular Processing of Midpoint Interpolation method. Here, ξ t (qgoal) means the t-th next waypoint from the starting point qgoal of the path R, and ξ t+n (qgoal) means the n-th next waypoint in the ξ t (qgoal). That is, there are n waypoints between ξ t (qgoal) and ξ t+n (qgoal).

Experimental Results
The path between RRT in various environment maps through simulation and the RRT algorithm to which the proposed Post Triangular Processing of Midpoint Interpolation method is applied were used to validate the performance of the method proposed in this paper and the results of path planning were compared.
The average value after repeated trial 100 times (sampling position is changed for each trial) of the path length(px) and the planning time(ms) of the first complete path is the performance measures compared (The first complete path that first reaches a destination point from a starting point are evaluated).
Various environmental maps were examined and used to validate the performance of the proposed path planning algorithm in related works. Since the efficiency of the performance measures expected during the experiment somewhat varies based on the composition, such as the number, location, or shape of obstacles, it is consequential to choose which environment map to utilize. The four environment maps used in the experiment are shown in Figure 8. The Four environment maps are partially referred to the experimental environment proposed by Jihee Han in 2017 [12]. All environment maps are 600*600px in size, with a 30px step length. The start point(S) is represented by the green circle, while the destination point(G) is represented by the purple circle. An obstacle is a black polygon with a yellow contour (blue in the experimental results).
Map 1 of Figure 8(a) appears to be an efficient environment for validating optimality and completeness, it is weakly environment to sampling-based path planning algorithms like the RRT algorithm. Lots of samplings are required since the probability of finding a solution is low. Map 2 of Figure 8(b) appears to be an efficient environment for validating optimality and completeness of the path planning algorithm. Map 3 of Figure 8(c) is an environment that is efficient for validating the optimality and the planning time of the path planning algorithm as it is consists of obstacles (50 vertices) that approximate circle. Map 4 of Figure 8(d) is an environment that is efficient for validating the optimality and the planning time of the path planning algorithm and is a weak environment to samplingbased path planning algorithms such as the RRT algorithm.
The number of samples and planning time required shoots up in case the path to the destination point is narrow or few entrances, since the sampling-based path planning algorithm depends on probabilistic completeness. The specification of the computer used in the simulation is shown in Table 1. The simulator used for the simulation [4] was developed based on C# WPF (Microsoft Visual Studio Community 2019 Version 16.1.6 Microsoft .NET Framework Version 4.8.03752), and only a single thread was used for calculations except for the visual part. generally, depending on the specification of the computer, the result of performance Measurement such as planning time may be variations during the simulation.
Validate the experimental results (path length, planning time) in the four environment maps that the Post Triangular Processing of Midpoint Interpolation method (ε: 50, 30, 10px) is applied to the RRT algorithm and its path planning results. Since ε requires a higher amount of computation as it gets smaller, it was set to a suitable value nearby according to the 30px step length of the experimental environment.
The experimental results for each map are consist of a figure and table. The figure is a path planning result for each algorithm is shown in the case of a single trial (the figure for each algorithm is not the result of repeated trials). The table is shown an average value that results of planning time and path length from path planning repeated trials. There may be a significant difference between the performance observed visually and the numerical results in the table for one of the repeated trials. The form of the planned path for each algorithm is shown in the figure for visual refer. furthermore, the proposed Post Triangular Processing of Midpoint Interpolation method is used to see whether there is a region where the piecewise linear form path is smoothed. The path planning results as shown in Figure 9 for map 1 among the specified environment maps for each algorithm. In terms of appearance, the one applied with the Post Triangular Processing of Midpoint Interpolation method (ε: 10px) seems to have a smooth slope compared to other algorithms, and the path length with the Post Triangular Processing of Midpoint Interpolation method (ε: 10px) seems to be the shortest. The path planning results(average value after repeated trial 100 times) as shown in Table 2 for map 1 among the specified environment maps for each algorithm. The path length applying the Post Triangular Processing of Midpoint Interpolation method (ε: 10px) becomes 62% (1224/1994(%)) compared to the RRT, which is the shortest compared to other algorithms, and the planning time is all similar. The path planning results as shown in Figure 10 for map 2 among the specified environment maps for each algorithm. In terms of appearance, the one applied with the Post Triangular Processing of Midpoint Interpolation method (ε: 10px) seems to have a smooth slope compared to other algorithms, and the path length with the Post Triangular Processing of Midpoint Interpolation method (ε: 10px) seems to be the shortest. Table 3. Experimental results of map 2 (The parentheses on the right of each number (average of repeated 100 times) are relative ratios to RRT results (values less than 1 are counted as 1)). The path planning results(average value after repeated trial 100 times) as shown in Table 3 for map 2 among the specified environment maps for each algorithm. The path length applied with the Post Triangular Processing of Midpoint Interpolation method (ε: 10px) becomes 74% (730/986(%)) compared to the RRT, which is the shortest compared to other algorithms, and the planning time is similar to the RRT algorithm when the Post Triangular Processing of Midpoint Interpolation method (ε: 50px) is applied. However, the absolute time difference is 1ms, and the difference seems to be large when the method is applied because it is an environment that requires less planning time.
The path planning results as shown in Figure 11 for map 3 among the specified environment maps for each algorithm. In terms of appearance, the one applied with the Post Triangular Processing of Midpoint Interpolation method (ε: 10px) seems to have a smooth slope compared to other algorithms, and the path length with the Post Triangular Processing of Midpoint Interpolation method (ε: 10px) seems to be the shortest. Planning time (ms) 6 (100%) The path planning results(average value after repeated trial 100 times) as shown in Table 4 for map 3 among the specified environment maps for each algorithm. The path length applying the Post Triangular Processing of Midpoint Interpolation method (ε: 10px) becomes 82% (505/613(%)) compared to the RRT, which is the shortest compared to other algorithms, and the planning time is the most similar to that of the Post Triangular Processing of Midpoint Interpolation method (ε: 10px) as it takes 16% more than the RRT algorithm. However, the absolute time difference is 2ms, and since it is an environment that requires less planning time, the difference appears to be large when the method is applied. The path planning results as shown in Figure 12 for map 4 among the specified environment maps for each algorithm. In terms of appearance, the one applied with the Post Triangular Processing of Midpoint Interpolation method (ε: 30px) seems to have a smooth slope compared to other algorithms, and the path length with the Post Triangular Processing of Midpoint Interpolation method (ε: 10px) seems to be the shortest. Table 5. Experimental results of map 4 (The parentheses on the right of each number (average of repeated 100 times) are relative ratios to RRT results (values less than 1 are counted as 1)). The path planning results(average value after repeated trial 100 times) as shown in Table 5 for map 4 among the specified environment maps for each algorithm. The path length applying the Post Triangular Processing of Midpoint Interpolation method (ε: 10px) becomes 77% (1190/1536(%)) compared to RRT, which is the shortest compared to other algorithms, and the planning time is similar for all.

Performance
Consequently, the Post Triangular Processing of Midpoint Interpolation method (ε: 10px) performed well in the path length aspect for all maps, demonstrating that the proposed method is efficient in terms of optimality.

Conclusion
In this research, the "Post Triangular Processing of Midpoint Interpolation" method minimized the planning time and shorter path length of the sampling-based algorithm.
The proposed Post Triangular Processing of Midpoint Interpolation method made a more to closer optimal path and somewhat solves the sharp path problem through the interpolation process. furthermore, all path planning algorithms that plan a locally piecewise linear path could apply the proposed algorithm. This strength has the significance that it can be applied not only to the algorithm presented in this paper but also to various path planning algorithms. The proposed method validated the performance after applied to the RRT algorithm and its path planning results through simulation. It was verified that the path length was shortened by 18-38% (average 26%) depending on the threshold ε when applied to the RRT algorithm in the four different environment maps. As a result, the RRT algorithm applying the proposed Post Triangular Processing of Midpoint Interpolation method showed a more optimal path.