A Bidirectional Interpolation Method for Post-Processing in Sampling-Based Robot Path Planning

This paper proposes a post-processing method called bidirectional interpolation method for sampling-based path planning algorithms, such as rapidly-exploring random tree (RRT). The proposed algorithm applies interpolation to the path generated by the sampling-based path planning algorithm. In this study, the proposed algorithm is applied to the path created by RRT-connect and six environmental maps were used for the verification. It was visually and quantitatively confirmed that, in all maps, not only path lengths but also the piecewise linear shape were decreased compared to the path generated by RRT-connect. To check the proposed algorithm’s performance, visibility graph, RRT-connect algorithm, Triangular-RRT-connect algorithm and post triangular processing of midpoint interpolation (PTPMI) were compared in various environmental maps through simulation. Based on these experimental results, the proposed algorithm shows similar planning time but shorter path length than previous RRT-like algorithms as well as RRT-like algorithms with PTPMI having a similar number of samples.


Introduction
This study deals with the path planning of a mobile robot [1]. Strictly speaking, path planning can be divided into global planning on an entire map and local planning on a portion of the map [2]. In this paper, path planning refers to global planning.
Path planning involves plotting a path that a mobile robot can follow to efficiently move from a starting point to a goal point in Euclidean space, avoiding obstacles, with respect to optimality, clearance and completeness [3]. Optimality refers to always being able to plan a path with the optimal path length. Clearance refers to how low the probability is that the mobile robot will collide with an obstacle. Completeness indicates that a path can always be planned in an environment in which a solution exists.
This paper mainly deals with the sampling-based rapidly-exploring random tree (RRT)-like algorithm [4]. The RRT-like algorithms are being applied in various ways. For example, there is a method that generates an optimal path by applying a triangular inequality [5] and a method applicable to kinodynamic planning [6]. The RRT algorithm can be summarized as a method of planning a path by repeatedly inserting a randomly sampled location as a child node in a tree with the starting point as the root node until reaching the goal point. In this method, the tree trunk extends in the shape of a stochastic fractal and attempts to reach the goal.
Sampling-based algorithms, including RRT algorithm, have the advantage of being able to plan a path with fewer computations than classical path planning algorithms such as those for visibility-graph-based [7], cell-decomposition-based [8] and potentialfield-based [9] methods. However, RRT does not guarantee optimality and probabilistic completeness [4]. Probabilistic completeness means that if the number of sampling nodes is unlimited, completeness is guaranteed, but if the number of sampling nodes is limited, completeness is not guaranteed.
The purpose of this study is to guarantee completeness, improve optimality and improve collision-avoidance of RRT-like algorithms for path planning. When a path is planned by the RRT algorithm, it has a stochastic fractal shape, and locally it tends to have a piecewise linear shape [10] as shown in Figure 1a. The piecewise linear contour can result in collisions [11] owing to the kinematic constraints [12] of the mobile robot, as shown in Figure 1b.
Sensors 2021, 21, x FOR PEER REVIEW 2 of 41 bilistic completeness [4]. Probabilistic completeness means that if the number of sampling nodes is unlimited, completeness is guaranteed, but if the number of sampling nodes is limited, completeness is not guaranteed. The purpose of this study is to guarantee completeness, improve optimality and improve collision-avoidance of RRT-like algorithms for path planning. When a path is planned by the RRT algorithm, it has a stochastic fractal shape, and locally it tends to have a piecewise linear shape [10] as shown in Figure 1a. The piecewise linear contour can result in collisions [11] owing to the kinematic constraints [12] of the mobile robot, as shown in Figure 1b. When the mobile robot moves along the planned path or local planning [13] path, it may collide with an obstacle as shown in Figure 2a because of kinematic constraints on the path with sharp angles. As this leads to serious concerns from the perspective of clearance, path planning must also consider kinodynamic planning [6,14,15]. In particular, as there are fewer waypoints on the planned path (a tree node in the RRT algorithm) or, alternatively, the distance between waypoints is high, kinematic error is more likely to occur. This may be exacerbated when the mobile robot travels at high speed and the control error increases. In this study, given that the RRT algorithm does not guarantee optimality, we aim to generate a path that is closer to the optimum. Simultaneously, the scope of the study includes solving the local clearance problem of the stochastic fractal-shaped path.
However, when the path is first plotted from the starting point to the goal point, only the first complete path is dealt with. That is, the complete path or convergence path that is closer to the optimum through additional sampling after the initial complete path is not considered within the scope of the current study. When the mobile robot moves along the planned path or local planning [13] path, it may collide with an obstacle as shown in Figure 2a because of kinematic constraints on the path with sharp angles. As this leads to serious concerns from the perspective of clearance, path planning must also consider kinodynamic planning [6,14,15]. In particular, as there are fewer waypoints on the planned path (a tree node in the RRT algorithm) or, alternatively, the distance between waypoints is high, kinematic error is more likely to occur. This may be exacerbated when the mobile robot travels at high speed and the control error increases. 021, 21, x FOR PEER REVIEW 2 of 41 bilistic completeness [4]. Probabilistic completeness means that if the number of sampling nodes is unlimited, completeness is guaranteed, but if the number of sampling nodes is limited, completeness is not guaranteed. The purpose of this study is to guarantee completeness, improve optimality and improve collision-avoidance of RRT-like algorithms for path planning. When a path is planned by the RRT algorithm, it has a stochastic fractal shape, and locally it tends to have a piecewise linear shape [10] as shown in Figure 1a. The piecewise linear contour can result in collisions [11] owing to the kinematic constraints [12] of the mobile robot, as shown in Figure 1b. When the mobile robot moves along the planned path or local planning [13] path, it may collide with an obstacle as shown in Figure 2a because of kinematic constraints on the path with sharp angles. As this leads to serious concerns from the perspective of clearance, path planning must also consider kinodynamic planning [6,14,15]. In particular, as there are fewer waypoints on the planned path (a tree node in the RRT algorithm) or, alternatively, the distance between waypoints is high, kinematic error is more likely to occur. This may be exacerbated when the mobile robot travels at high speed and the control error increases. In this study, given that the RRT algorithm does not guarantee optimality, we aim to generate a path that is closer to the optimum. Simultaneously, the scope of the study includes solving the local clearance problem of the stochastic fractal-shaped path.
However, when the path is first plotted from the starting point to the goal point, only the first complete path is dealt with. That is, the complete path or convergence path that is closer to the optimum through additional sampling after the initial complete path is not considered within the scope of the current study. In this study, given that the RRT algorithm does not guarantee optimality, we aim to generate a path that is closer to the optimum. Simultaneously, the scope of the study includes solving the local clearance problem of the stochastic fractal-shaped path.
However, when the path is first plotted from the starting point to the goal point, only the first complete path is dealt with. That is, the complete path or convergence path that is closer to the optimum through additional sampling after the initial complete path is not considered within the scope of the current study.
The RRT* algorithm, an improved version of the RRT algorithm, further optimizes the convergence path, wherein the convergence rate of the first complete path is higher Sensors 2021, 21, 7425 3 of 40 than that of the RRT algorithm. Consequently, it is possible to plan a path closer to the optimum. However, the amount of computation required to arrive at the convergence path is very high [16]. Recently, the post triangular processing of midpoint interpolation (PTPMI) method has been proposed to solve this problem [17]. Therefore, in this study, we prioritize finding a solution, which is the advantage of the sampling-based algorithm. In addition, the purpose of this study is to generate a path that is closer to the optimum for a similar computation time without significantly increasing the amount of computation compared to related studies.
We propose the bidirectional interpolation method for post-processing, which allows the RRT algorithm to plan a path that is closer to optimal and to mitigate the local clearance problem.
Visibility-graph-based path planning creates an optimal path. However, in order to make this path clearer in a real environment, it is necessary to use a sensor device with high resolution. The proposed method in this paper can generate a path that is close to optimal even in sensor equipment with low resolution. Therefore, the cost for the sensor could be saved.
The overall structure of this paper is as follows. Section 2 deals with related works. Section 2.1 deals with classical path planning and Section 2.2 deals with the samplingbased path planning algorithm. Section 3 deals with the proposed method (bidirectional interpolation method for post-processing). Section 4 deals with experimental results in which the path lengths and planning times are compared and analyzed through simulation to verify the performance of the proposed method.

Visibility Graph
The visibility graph algorithm guarantees optimality and completeness, but the clearance is very low. The cell decomposition algorithm guarantees completeness and high clearance, but does not guarantee optimality. The potential field algorithm has very high clearance, but cannot guarantee completeness and optimality by local minima [9].
In particular, as the visibility graph algorithm guarantees optimality in 2D configuration space [18], it is often used as a comparative experiment target for path planning method studies. We also compare the RRT algorithm post-processed using the proposed method to the visibility graph in terms of path length, in order to gauge their relative optimality.
The visibility graph algorithm was proposed by Tomas Lozano-Perez in 1979. As shown in Figure 3a, the start point, goal point and the vertices of all obstacle polygons are connected in a graph form. As shown in Figure 3b, it guarantees optimality by searching for the shortest path from the node at the starting point to the node at the goal. At this time, the case with an edge connecting a node and a node passing through an obstacle is excluded.

Limitation of Classical Path Planning
All the classical path planning methods, including the visibility graph algorithm, have their own advantages and disadvantages, but a common limitation is that they are difficult to apply to a dynamic environment because they involve a large amount of computation.
The path planning algorithms of the classical method introduced above plan the path using obstacle area information, unlike the sampling-based algorithm. As the visibility graph algorithm and the cell decomposition algorithm plan a path using the vertices of the obstacle polygon, the amount of computation becomes very large when the number of vertices of the entire obstacle is large. In addition, given that the visibility graph algorithm and cell decomposition algorithm can be applied only when the shape of the obstacle is polygonal, polygon approximation is required when a curved obstacle is given as an input value in a vector map rather than a grid map [19,20].

Limitation of Classical Path Planning
All the classical path planning methods, including the visibility graph algorithm, have their own advantages and disadvantages, but a common limitation is that they are difficult to apply to a dynamic environment because they involve a large amount of computation.
The path planning algorithms of the classical method introduced above plan the path using obstacle area information, unlike the sampling-based algorithm. As the visibility graph algorithm and the cell decomposition algorithm plan a path using the vertices of the obstacle polygon, the amount of computation becomes very large when the number of vertices of the entire obstacle is large. In addition, given that the visibility graph algorithm and cell decomposition algorithm can be applied only when the shape of the obstacle is polygonal, polygon approximation is required when a curved obstacle is given as an input value in a vector map rather than a grid map [19,20].
The precision of modern mobile robot sensing technology and equipment (such as radar and LiDAR) is very high, so obstacles can be mapped with high resolution (the number of polygonal vertices of obstacles is proportional to the sensing precision) [21]. Therefore, path planning through classical methods such as visibility graph algorithm is difficult to apply to dynamic environments. One solution is to reduce the computational amount of the classical method by reducing the number of obstacle polygon vertices through the polygon approximation algorithm. However, this distorts the actual shape, and therefore unintentional merging between polygons or obstacle collision problems caused by distorted areas can cause fatal problems in route planning.
Unlike the classical method, the sampling-based method does not actively use the obstacle area information for path planning; it is only used for obstacle collision inspection. Therefore, the amount of computation does not increase significantly depending on the type of obstacle. In other words, it is suitable as a modern mobile robot route planning method because it can plan a route in a short time even in a dynamic environment mapped at a high resolution.

Sampling-Based Path Planning Algorithms
Sampling-based path planning methods include the RRT algorithm and the probabilistic roadmap method (PRM) algorithm [22]. Well-known improvements to the RRT algorithm include RRT-connect [23], RRT* [14], informed RRT [24] and quick*-RRT [10] algorithms. This section deals only with RRT and RRT-connect algorithms. The precision of modern mobile robot sensing technology and equipment (such as radar and LiDAR) is very high, so obstacles can be mapped with high resolution (the number of polygonal vertices of obstacles is proportional to the sensing precision) [21]. Therefore, path planning through classical methods such as visibility graph algorithm is difficult to apply to dynamic environments. One solution is to reduce the computational amount of the classical method by reducing the number of obstacle polygon vertices through the polygon approximation algorithm. However, this distorts the actual shape, and therefore unintentional merging between polygons or obstacle collision problems caused by distorted areas can cause fatal problems in route planning.
Unlike the classical method, the sampling-based method does not actively use the obstacle area information for path planning; it is only used for obstacle collision inspection. Therefore, the amount of computation does not increase significantly depending on the type of obstacle. In other words, it is suitable as a modern mobile robot route planning method because it can plan a route in a short time even in a dynamic environment mapped at a high resolution.

Sampling-Based Path Planning Algorithms
Sampling-based path planning methods include the RRT algorithm and the probabilistic roadmap method (PRM) algorithm [22]. Well-known improvements to the RRT algorithm include RRT-connect [23], RRT* [14], informed RRT [24] and quick*-RRT [10] algorithms. This section deals only with RRT and RRT-connect algorithms.
As the purpose of this paper is specifically to improve the performance of the first complete path of the RRT algorithm, research on the efficient convergence path or the RRT* algorithm for improving convergence speed or the informed RRT algorithm for improving random sampling is not covered in this paper.

Rapidly Exploring Random Tree (RRT)
The RRT algorithm is a representative algorithm of the sampling-based path planning method, and was proposed by Steven M. LaValle in 1998 [4]. It is useful for path planning, considering the conditions of non-holonomic constraints, and is designed to have high degrees of freedom [6].
When a random sample is generated in the configuration space, as shown in Figure 4, the node closest to the location of the random sample is found among the nodes constituting the tree with the starting point as the root node. A new node is created at a position away from the node by step length (λ) in the direction of the random sample position and inserted into the tree. If the random sample position is closer than step length, a new node is created at the random sample position and inserted into the tree. This tree expansion process is repeated until the destination is reached. ning method, and was proposed by Steven M. LaValle in 1998 [4]. It is useful for path planning, considering the conditions of non-holonomic constraints, and is designed to have high degrees of freedom [6].
When a random sample is generated in the configuration space, as shown in Figure  4, the node closest to the location of the random sample is found among the nodes constituting the tree with the starting point as the root node. A new node is created at a position away from the node by step length (λ) in the direction of the random sample position and inserted into the tree. If the random sample position is closer than step length, a new node is created at the random sample position and inserted into the tree. This tree expansion process is repeated until the destination is reached. Figure 4. Process of RRT algorithm. A new node is created at location qnew separated by step length λ in the direction of the random sampling position qrand based on the random sampling position qrand and the nearest node (position) qnear in tree T with start point qstart as the root node.

RRT-Connect
As samples appear with the same probability in all regions, path planning through the RRT algorithm may have the disadvantage that the tree easily extends in several random directions irrespective of the destination, resulting in a longer convergence time and inefficiency. The RRT-connect algorithm [23] proposed by James J. Kuffner Jr. in 2000 aims to compensate for this disadvantage by adopting two major new strategies.
The first idea is swapping. It involves designating a starting point and a target point as each root node and alternately extending in the direction of each other. This prevents the tree from extending in a direction independent of its destination and reduces the time required to search for a path.
The second idea is extending. This means that when the tree is extended, it continues to extend to the tree on the other side if no collision with an obstacle occurs. As shown in Figure 5, if there is no collision with an obstacle when a sample is generated, the tree continues to expand as much as the step length in the direction of the opposite tree, so that the destination can be reached faster.
Through this idea, the path planning through the RRT-connect algorithm can find the first complete path at a much higher speed than the existing RRT algorithm. λ in the direction of the random sampling position q rand based on the random sampling position q rand and the nearest node (position) q near in tree T with start point q start as the root node.

RRT-Connect
As samples appear with the same probability in all regions, path planning through the RRT algorithm may have the disadvantage that the tree easily extends in several random directions irrespective of the destination, resulting in a longer convergence time and inefficiency. The RRT-connect algorithm [23] proposed by James J. Kuffner Jr. in 2000 aims to compensate for this disadvantage by adopting two major new strategies.
The first idea is swapping. It involves designating a starting point and a target point as each root node and alternately extending in the direction of each other. This prevents the tree from extending in a direction independent of its destination and reduces the time required to search for a path.
The second idea is extending. This means that when the tree is extended, it continues to extend to the tree on the other side if no collision with an obstacle occurs. As shown in Figure 5, if there is no collision with an obstacle when a sample is generated, the tree continues to expand as much as the step length in the direction of the opposite tree, so that the destination can be reached faster.

Triangular-RRT-Connect
Triangular-RRT-Connect [25] is a method that applies the triangular-rewiring method in the extend and connect stages of RRT-connect. Triangular-rewiring uses the principle of triangular inequality. As shown in Figure 6, when there are qchild, qparent which is the parent node of qchild and qancestor which is the parent node of qparent, triangular-rewiring eliminates the path through the qparent node when there is no obstacle between qchild and qancestor and connects directly between qchild and qancestor. Figure 5. Process of RRT-connect algorithm: (a) extend from tree T b with root as goal position q goal to tree T a with root as start position q start (T a 's q near extends in T a 's q newA direction); (b) as the paths P a and P b created in each tree are connected ("Connect") to each other, path P merged is created.
Through this idea, the path planning through the RRT-connect algorithm can find the first complete path at a much higher speed than the existing RRT algorithm.

Triangular-RRT-Connect
Triangular-RRT-Connect [25] is a method that applies the triangular-rewiring method in the extend and connect stages of RRT-connect. Triangular-rewiring uses the principle of triangular inequality. As shown in Figure 6, when there are q child , q parent which is the parent Sensors 2021, 21, 7425 6 of 40 node of q child and q ancestor which is the parent node of q parent , triangular-rewiring eliminates the path through the q parent node when there is no obstacle between q child and q ancestor and connects directly between q child and q ancestor . start position qstart (Ta's qnear extends in Ta's qnewA direction); (b) as the paths Pa and Pb created in each tree are connected ("Connect") to each other, path Pmerged is created.

Triangular-RRT-Connect
Triangular-RRT-Connect [25] is a method that applies the triangular-rewiring method in the extend and connect stages of RRT-connect. Triangular-rewiring uses the principle of triangular inequality. As shown in Figure 6, when there are qchild, qparent which is the parent node of qchild and qancestor which is the parent node of qparent, triangular-rewiring eliminates the path through the qparent node when there is no obstacle between qchild and qancestor and connects directly between qchild and qancestor.  Figure 7 shows "extend" and "connect" of triangular-RRT-connect. In triangular-RRT-connect, there is a tree extending from the goal point and a tree extending from the start point. In Figure 7a, TS and TG are trees extending from the start point and the goal point, respectively. Extend occurs at TG when sampling is performed at a random location qrand and a new node, qnewS, is created based on TS. At this time, the triangular-rewiring method works on all nodes generated during the "extend" process. Figure  7b is connect in triangular-RRT-connect. After the two trees are connected, the triangular-rewiring method is applied to the merged tree. Tconnect is a tree where TS and TG are connected.
(a) (b) Figure 6. Overview of "Triangular-Rewiring" method: (a) example tree, (b) result of "Triangular-Rewiring", (c) applied trigonometric inequality (α + β > γ). Figure 7 shows "extend" and "connect" of triangular-RRT-connect. In triangular-RRT-connect, there is a tree extending from the goal point and a tree extending from the start point. In Figure 7a, T S and T G are trees extending from the start point and the goal point, respectively. Extend occurs at T G when sampling is performed at a random location q rand and a new node, q newS , is created based on T S . At this time, the triangular-rewiring method works on all nodes generated during the "extend" process. Figure 7b is connect in triangular-RRT-connect. After the two trees are connected, the triangular-rewiring method is applied to the merged tree. T connect is a tree where T S and T G are connected.

Triangular-RRT-Connect
Triangular-RRT-Connect [25] is a method that applies the triangular-rewiring method in the extend and connect stages of RRT-connect. Triangular-rewiring uses the principle of triangular inequality. As shown in Figure 6, when there are qchild, qparent which is the parent node of qchild and qancestor which is the parent node of qparent, triangular-rewiring eliminates the path through the qparent node when there is no obstacle between qchild and qancestor and connects directly between qchild and qancestor.  Figure 7 shows "extend" and "connect" of triangular-RRT-connect. In triangular-RRT-connect, there is a tree extending from the goal point and a tree extending from the start point. In Figure 7a, TS and TG are trees extending from the start point and the goal point, respectively. Extend occurs at TG when sampling is performed at a random location qrand and a new node, qnewS, is created based on TS. At this time, the triangular-rewiring method works on all nodes generated during the "extend" process. Figure  7b is connect in triangular-RRT-connect. After the two trees are connected, the triangular-rewiring method is applied to the merged tree. Tconnect is a tree where TS and TG are connected.

Forward Interpolation Process
As shown in Figure 8a, when there is no obstacle between the newly inserted node and its grandparent node (collision-free), as shown in Figure 8b, a connection is made between the newly inserted node and its grandparent node, while its parent node is excluded (rewiring). Based on the trigonometric inequality property, this can be corrected to a path that is closer to the optimum than in the existing RRT algorithm.
If the current path is not collision-free from obstacle, as shown in Figure 9, the piecewise linear local path created between the node, its parent node and its grandparent node is optimized through interpolation. In this process, new nodes are interpolated into the existing path to deviate from the piecewise linear path, making it possible to create a smooth path.

Forward Interpolation Process
As shown in Figure 8a, when there is no obstacle between the newly inserted node and its grandparent node (collision-free), as shown in Figure 8b, a connection is made between the newly inserted node and its grandparent node, while its parent node is excluded (rewiring). Based on the trigonometric inequality property, this can be corrected to a path that is closer to the optimum than in the existing RRT algorithm.
(a) (b) Figure 8. Overview of rewiring step in forward interpolation process: (a) line γ from node q0 to its grandparent node q2 in tree R is collision-free (distance: γ < α + β); (b) rewiring: grandparent node q2 of node q0 is connected to q0 as parent node, and origin parent node q1 is excluded from tree R.
If the current path is not collision-free from obstacle, as shown in Figure 9, the piecewise linear local path created between the node, its parent node and its grandparent node is optimized through interpolation. In this process, new nodes are interpolated into the existing path to deviate from the piecewise linear path, making it possible to create a smooth path.
Quick-RRT* [10] and triangular-RRT-connect [25] algorithms aim to create a path that is close to optimal using triangular inequality. However, as the node is deleted in the process, the distance between the waypoints on the planned path is longer than that of the RRT algorithm, so the sharp angle on the path line is deepened.
(a) (b) Figure 9. Overview of interpolation step in forward interpolation process: (a) line γ from node q0 to its grandparent node q2 in tree R is trapped; (b) interpolation: node qa between node q0 and node q1 and node qb between node q1 and node q2 are created. After connecting the parent node of q0 with qa, the parent of qa with qb and the parent of qb with q2, node q1 is excluded.
Forward interpolation process is effective for all path planning algorithms, such as RRT algorithm, where optimality is not guaranteed and a local piecewise linear path appears. After the route is planned, it can be applied as a post-processing technique.
Compared to the classical path planning methods [3], the major advantage of the sampling-based path planning method is the high planning speed owing to a small amount of computation, so there is a prerequisite that the amount of added computation should not be large compared to that required by the existing RRT algorithm. Figure 8. Overview of rewiring step in forward interpolation process: (a) line γ from node q 0 to its grandparent node q 2 in tree R is collision-free (distance: γ < α + β); (b) rewiring: grandparent node q 2 of node q 0 is connected to q 0 as parent node, and origin parent node q 1 is excluded from tree R.
(a) (b) Figure 8. Overview of rewiring step in forward interpolation process: (a) line γ from node q0 to its grandparent node q2 in tree R is collision-free (distance: γ < α + β); (b) rewiring: grandparent node q2 of node q0 is connected to q0 as parent node, and origin parent node q1 is excluded from tree R.
If the current path is not collision-free from obstacle, as shown in Figure 9, the piecewise linear local path created between the node, its parent node and its grandparent node is optimized through interpolation. In this process, new nodes are interpolated into the existing path to deviate from the piecewise linear path, making it possible to create a smooth path.
Quick-RRT* [10] and triangular-RRT-connect [25] algorithms aim to create a path that is close to optimal using triangular inequality. However, as the node is deleted in the process, the distance between the waypoints on the planned path is longer than that of the RRT algorithm, so the sharp angle on the path line is deepened.
(a) (b) Figure 9. Overview of interpolation step in forward interpolation process: (a) line γ from node q0 to its grandparent node q2 in tree R is trapped; (b) interpolation: node qa between node q0 and node q1 and node qb between node q1 and node q2 are created. After connecting the parent node of q0 with qa, the parent of qa with qb and the parent of qb with q2, node q1 is excluded.
Forward interpolation process is effective for all path planning algorithms, such as RRT algorithm, where optimality is not guaranteed and a local piecewise linear path appears. After the route is planned, it can be applied as a post-processing technique.
Compared to the classical path planning methods [3], the major advantage of the sampling-based path planning method is the high planning speed owing to a small amount of computation, so there is a prerequisite that the amount of added computation should not be large compared to that required by the existing RRT algorithm. Figure 9. Overview of interpolation step in forward interpolation process: (a) line γ from node q 0 to its grandparent node q 2 in tree R is trapped; (b) interpolation: node q a between node q 0 and node q 1 and node q b between node q 1 and node q 2 are created. After connecting the parent node of q 0 with q a , the parent of q a with q b and the parent of q b with q 2 , node q 1 is excluded.
Quick-RRT* [10] and triangular-RRT-connect [25] algorithms aim to create a path that is close to optimal using triangular inequality. However, as the node is deleted in the process, the distance between the waypoints on the planned path is longer than that of the RRT algorithm, so the sharp angle on the path line is deepened.
Forward interpolation process is effective for all path planning algorithms, such as RRT algorithm, where optimality is not guaranteed and a local piecewise linear path appears. After the route is planned, it can be applied as a post-processing technique.
Compared to the classical path planning methods [3], the major advantage of the sampling-based path planning method is the high planning speed owing to a small amount of computation, so there is a prerequisite that the amount of added computation should not be large compared to that required by the existing RRT algorithm.
Forward interpolation process was designed based on the polygon approximation algorithm [18,26]. As shown in Figure 10, the constant value (ε > 0) of ε (the threshold of minimum collision stability) determines how closely the path is approximated to the obstacle. d n in Figure 10 follows Equation (1): For an arbitrary waypoint q i , the value of d decreases by 1/2 as interpolation proceeds (n). The initial value d 0 is the height of the triangle consisting of the length α from q i to the next waypoint of q i , the length β from the next waypoint of q i to the next waypoint of the next waypoint of q i and the length γ from q i to the next waypoint of the next waypoint of q i (γ < α + β). The value of d n becomes (d n − 1)/2. As the path gets closer to the obstacle as d gets smaller, it is compared to ε functions as a measure to confirm clearance.
Forward interpolation process was designed based on the polygon approximation algorithm [18,26]. As shown in Figure 10, the constant value (ε > 0) of ε (the threshold of minimum collision stability) determines how closely the path is approximated to the obstacle.
(a) (b) Figure 10. Condition of interpolation step at forward interpolation process: (a) interpolation continues: the height d0 of a triangle made from waypoint q0, q1 and q2 in random path is higher than ε; (b) interpolation break: the height d1 of a triangle made from midpoint mF(q0,1) (between q0 and q1), node q1 and midpoint mF(q1,1) (between q1 and q2) is less than ε. dn in Figure 10 follows Equation (1): For an arbitrary waypoint qi, the value of d decreases by 1/2 as interpolation proceeds (n). The initial value d0 is the height of the triangle consisting of the length α from qi to the next waypoint of qi, the length β from the next waypoint of qi to the next waypoint of the next waypoint of qi and the length γ from qi to the next waypoint of the next waypoint of qi(γ < α + β). The value of dn becomes (dn − 1)/2. As the path gets closer to the obstacle as d gets smaller, it is compared to ε functions as a measure to confirm clearance.
However, optimality and clearance are conflicting properties; as shown in Figure  11, the smaller the ε value (the minimum value of ε: 0), the higher the optimality and the lower the clearance. Conversely, the larger the ε value, the higher the clearance and the lower the optimality. Therefore, ε should be set to an appropriate value based on the environment.
(a) (b) (c) Figure 11. Difference according to ε value in forward interpolation process: (a) result when the value of ε is equal to random value n; (b) result when ε value is more than random value n; (c) result when ε value is less than random value n. Figure 10. Condition of interpolation step at forward interpolation process: (a) interpolation continues: the height d 0 of a triangle made from waypoint q 0 , q 1 and q 2 in random path is higher than ε; (b) interpolation break: the height d 1 of a triangle made from midpoint m F (q 0 ,1) (between q 0 and q 1 ) , node q 1 and midpoint m F (q 1 ,1) (between q 1 and q 2 ) is less than ε.
However, optimality and clearance are conflicting properties; as shown in Figure 11, the smaller the ε value (the minimum value of ε: 0), the higher the optimality and the lower the clearance. Conversely, the larger the ε value, the higher the clearance and the lower the optimality. Therefore, ε should be set to an appropriate value based on the environment.
(a) (b) Figure 10. Condition of interpolation step at forward interpolation process: (a) interpolation continues: the height d0 of a triangle made from waypoint q0, q1 and q2 in random path is higher than ε; (b) interpolation break: the height d1 of a triangle made from midpoint mF(q0,1) (between q0 and q1), node q1 and midpoint mF(q1,1) (between q1 and q2) is less than ε. dn in Figure 10 follows Equation (1): For an arbitrary waypoint qi, the value of d decreases by 1/2 as interpolation proceeds (n). The initial value d0 is the height of the triangle consisting of the length α from qi to the next waypoint of qi, the length β from the next waypoint of qi to the next waypoint of the next waypoint of qi and the length γ from qi to the next waypoint of the next waypoint of qi(γ < α + β). The value of dn becomes (dn − 1)/2. As the path gets closer to the obstacle as d gets smaller, it is compared to ε functions as a measure to confirm clearance.
However, optimality and clearance are conflicting properties; as shown in Figure  11, the smaller the ε value (the minimum value of ε: 0), the higher the optimality and the lower the clearance. Conversely, the larger the ε value, the higher the clearance and the lower the optimality. Therefore, ε should be set to an appropriate value based on the environment.
(a) (b) (c) Figure 11. Difference according to ε value in forward interpolation process: (a) result when the value of ε is equal to random value n; (b) result when ε value is more than random value n; (c) result when ε value is less than random value n. Figure 11. Difference according to ε value in forward interpolation process: (a) result when the value of ε is equal to random value n; (b) result when ε value is more than random value n; (c) result when ε value is less than random value n.

Pseudocode of Forward Interpolation Process
Forward interpolation process is a post-processing method applied after the path is planned in RRT-like algorithms. Mathematical modeling is based on a two-dimensional Euclidean space.
Algorithm 1 presents the pseudocode of forward interpolation process. Two functions can be called internally: post triangular (Algorithm 2) and interpolation (Algorithm 3).

Algorithm 1 Pseudocode of Forward Interpolation Process.
Input: R ← path from {RRT/RRT-Connect/Tri-RRT/Tri-RRT-Connect/ . . . } C ← position set of all (measured) boundary points in all (known) obstacles ε ← threshold value of minimum clearance Output: If not isTrapped(q child , q ancestor , C) Then R ← postTriangular(R, ε, t, f modify ) 9 Else q parent ← next point of q child in R End Algorithm 2 Pseudocode of the Post Triangular Function.

Input:
q ancestor ← next point of q parent in R 4 R ← Delete path<q child , q parent > and path<q parent , q ancestor > from R 5 R ← Insert path<q child , q ancestor > to R 6 f modify ← true End Algorithm 3 Pseudocode of the Interpolation Function.

Input:
d ← altitude of triangle consisting of q child , q parent , and q ancestor with base<q child , q ancestor > 2 m a ← midpoint between q child and q parent Sensors 2021, 21, 7425 10 of 40 3 m b ← midpoint between q parent and q ancestor 4 While true Do 5 If d >= ε Then 6 If not isTrapped(m a , m b , C) Then 7 R ← Delete path<q child , q parent > and path<q parent , q ancestor > from R 8 R ← Insert path<q child , m a >, path<m a , m b >, and path<m b , q ancestor > to R 9 f modify ← true 10 Break 11 m a ← midpoint between m a and q parent 14 m b ← midpoint between m b and q parent 15 The input value of forward interpolation process consists of the path R planned through the RRT-like algorithms, the obstacle area information C and the threshold value ε of the minimum clearance.
f modify is a variable that checks 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 path correction does not occur when the process is repeated, 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 refers to the starting point, which is the first point of R.
In R, when the first starting point is q child , the next point is q parent and the next point after that point is called q ancestor , the algorithm checks whether the line between q child and q ancestor is collision-free (isTrapped() function). If it is collision-free, the postTriangular() function is called; if not, the interpolation() function is called. The postTriangular() function connects q child and q ancestor , and q parent is excluded from the existing path. The interpolation() function finds a random point between (q child and q parent ) and (q parent and q ancestor ) that is collision-free when connected (interpolation), and rewires q child , q ancestor and the two points found. If R and t are updated by the postTriangular() or interpolation() function, q child (the t-th waypoint of R), q parent and q ancestor are updated accordingly. If q parent is the last point in R, f modify is checked. Otherwise, the above process is repeated again for the updated q child and q ancestor .
Here, the path modification by the postTriangular() function deletes the existing waypoints and creates a path that is close to optimal, but has the effect of sharpening the path shape. Path modification by the interpolation() function has the effect of creating a path that is close to an optimal path and smoothing the path shape while adding/inserting new waypoints between existing waypoints. For creating a path that is close to optimal, the postTriangular() function modifies the path more efficiently than the interpolation() function.

Pseudocode of the Post Triangular Function from Forward Interpolation Process
The input value of the postTriangular() function consists of the path R, the waypoint index t and f modify , which states whether the path has been modified by forward interpolation process.
Rewiring is performed on the t-th waypoint q child of R, the next point q parent and the next point q ancestor of q parent . First, the path between q child and q parent and the existing path between q parent and q ancestor are deleted. Then, the path between q child and q ancestor is inserted. Finally, f modify returns 'true' because the path has been modified.

Pseudocode of Interpolation Function from Forward Interpolation Process
As shown in Figure 12, the interpolation of forward interpolation process is performed at three points (random interpolation point (q 0 ), the next interpolation point (q 1 ) and the next interpolation point (q 2 ) of point q 1 ). It aims to find the interpolation point (m F (q 0 ), m F (q 1 )) that is collision-free from the obstacle between the waypoints (q 0~q1 , q 1~q2 ) while descending in the direction of the midpoint (q 1 ).
The input value of the postTriangular() function consists of the path R, the waypoint index t and fmodify, which states whether the path has been modified by forward interpolation process.
Rewiring is performed on the t-th waypoint qchild of R, the next point qparent and the next point qancestor of qparent. First, the path between qchild and qparent and the existing path between qparent and qancestor are deleted. Then, the path between qchild and qancestor is inserted. Finally, fmodify returns 'true' because the path has been modified.

Pseudocode of Interpolation Function from Forward Interpolation Process
As shown in Figure 12, the interpolation of forward interpolation process is performed at three points (random interpolation point (q0), the next interpolation point (q1) and the next interpolation point (q2) of point q1). It aims to find the interpolation point (mF(q0), mF(q1)) that is collision-free from the obstacle between the waypoints (q0~q1, q1~q2) while descending in the direction of the midpoint (q1). The interpolation point m F follows Equations (2) and (3): First, ξ() is a function that receives a random node as a variable and returns the parent node of that node. The n-th square of ξ() (n ≥ 0) can be expressed as in Equation (2), when n = 0, ξ 0 (q i ) := q i holds.
When the k-th interpolation point of a random waypoint q i is m F (q i ,k), the 0-th interpolation point itself becomes q i . The first interpolation point is the midpoint of q i and the next point ξ(q i ) of q i , and the second interpolation point becomes the midpoint of m F (q i ,1) and Algorithm 3 is the pseudocode of the interpolation() function in forward interpolation process.
The input value of the interpolation() function of forward interpolation process consists of the path R, the obstacle area information C, the waypoint index t and whether to modify the path f modify from in forward interpolation process.
A triangle is made of three waypoints (the t-th waypoint q child of R, the next point q parent of q child and the next point q ancestor of q parent ), and the height d of the triangle can be found. m a is the midpoint of q child and q parent , and m b is the midpoint of q parent and q ancestor . If the path between m a and m b is collision-free from the obstacle (isTrapped()), the existing path between q child and q parent is deleted and the path between q child and m a , the path between m a and m b and the path between m b and q ancestor are inserted. Furthermore, as the path has been modified, f modify becomes 'true', returns it, and the function ends. If the distance between m a and m b is not collision-free from obstacles, the value of d is 1/2, m a is the midpoint of m a and q parent , and m b is updated to the midpoint of m b and q parent , so it must be determined whether m a and m b are collision-free from obstacles. This iterative process proceeds until a case is found in which m a and m b are collisionfree from obstacles or d becomes smaller than ε. If d becomes smaller than ε, the value of t is increased by 1 and the function is terminated.

Backward Interpolation Process
As shown in Figure 13, a collision-free interpolation point is found while descending in the direction of the midpoint (q 1 ) among the three points (a random interpolation point (q 0 ), the next interpolation point (q 1 ) of q 0 and the next interpolation point (q 2 ) of q 1 ). From the interpolation point, it ascends again in the direction of the obstacle as far as possible (d >= ε), and a waypoint collision-free from obstacle is found between the interpolation point and the waypoint (m F (q 0 )~q 1 , q 1~mF (q 1 )).
tween ma and mb and the path between mb and qancestor are inserted. Furthermore, as the path has been modified, fmodify becomes 'true', returns it, and the function ends. If the distance between ma and mb is not collision-free from obstacles, the value of d is 1/2, ma is the midpoint of ma and qparent, and mb is updated to the midpoint of mb and qparent, so it must be determined whether ma and mb are collision-free from obstacles. This iterative process proceeds until a case is found in which ma and mb are collision-free from obstacles or d becomes smaller than ε. If d becomes smaller than ε, the value of t is increased by 1 and the function is terminated.

Backward Interpolation Process
As shown in Figure 13, a collision-free interpolation point is found while descending in the direction of the midpoint (q1) among the three points (a random interpolation point (q0), the next interpolation point (q1) of q0 and the next interpolation point (q2) of q1). From the interpolation point, it ascends again in the direction of the obstacle as far as possible (d >= ε), and a waypoint collision-free from obstacle is found between the interpolation point and the waypoint (mF(q0)~q1, q1~mF(q1)).
The interpolation point mr follows Equation (4): The u-th interpolated point in the direction of the obstacle from the k-th interpolation point mF(qi,k) of a random waypoint qi is called mR(qi, k, u) (the value of k is fixed). At this time, if u is 0, it becomes the midpoint of mF(qi, k) and mF(qi, k−1). If u is 1, it is the midpoint of mR(qi, k, 0) and mF(qi, k−1). That is, mR(qi, k, u) (u > 0) becomes the midpoint between mR(qi, k, u−1) and mF(qi, k−1). For reference, d becomes (dk+u−1)/2. Here, (when u is 0) mF(qi, k) (and mF(ξ 2 (qi),k)) goes down in the ξ(qi) direction and becomes the first obstacle collision point. mF(qi, k−1) (and mF(ξ 2 (qi), k−1)) is the point at which it did not collide with the last obstacle. Therefore, Equation (4) interpolates within the region between the obstacle collision point and the obstacle non-impact collision point.
Accordingly, the proposed method can obtain a path that is close to optimal compared to the existing PTPMI [17] method.
The interpolation point m r follows Equation (4): The u-th interpolated point in the direction of the obstacle from the k-th interpolation Here, (when u is 0) m F (q i , k) (and m F (ξ 2 (q i ),k)) goes down in the ξ(q i ) direction and becomes the first obstacle collision point. m F (q i , k − 1) (and m F (ξ 2 (q i ), k − 1)) is the point at which it did not collide with the last obstacle. Therefore, Equation (4) interpolates within the region between the obstacle collision point and the obstacle non-impact collision point. m R (q i , k, u) in Equation (4) can also be expressed as Equation (7) through Equations (5) and (6): It starts with m F (q i ,k) and ξ(q i ) when u = 0. Then, m R (q i ,k,u) is found as a point that divides the line segment connecting the previous two points m R (q i ,k,u − 1) and m R (qi,k,u − 2) in a 3:1 ratio (k value is fixed).
In the end, Equation (7) shows the same result as Equation (4), and it is more efficient in terms of the space complexity of the algorithm.

Pseudocode Backward Interpolation Process
Algorithm 4 presents the pseudocode of the interpolation() function of backward interpolation process.

Algorithm 4 Pseudocode of Backward Interpolation Process.
Input: R ← path R from ForwardInterpolationProcess C ← position set C from ForwardInterpolationProcess ε ← threshold value ε from ForwardInterpolationProcess t ← point index t from ForwardInterpolationProcess f modify ← boolean f modify from ForwardInterpolationProcess Output: R ← modified path R t ← updated point index t //return by reference f modify ← result of boolean f modify //return by reference Initialize: q child ← t-th point in R q parent ← next point of q child in R q ancestor ← next point of q parent in R Procedure interpolation from ForwardInterpolationProcess Begin 1 d ← altitude of triangle consisting of q child , q parent , and q ancestor with base<q child , q ancestor > 2 m a ← midpoint between q child and q parent 3 m b ← midpoint between q parent and q ancestor 4 While true Do 5 If d >= ε Then 6 If not isTrapped(m a , m b , C) Then 7 R ← Delete path<q child , q parent > and path<q parent , q ancestor > from R  The input value of the interpolation() function of backward interpolation process consists of path R, obstacle area information C, waypoint index t and path modification f modify .
Compared to the interpolation() function in forward interpolation process, lines 8-20 have been inserted in this interpolation() function. These contents are interpolated again in the direction of the obstacle after the unidirectional (q parent direction) interpolation is completed (when collision-free from the obstacle). From the 8th line, m backA is the point where the line segment connecting q parent and m a is externalized in a 3:1 ratio, m backB is the point where the line connecting q parent and m b is externalized in a 3:1 ratio, m freeA is m a and m freeB is m b . If the route between m backA and m backB is collision-free from the obstacle (isTrapped()), m a is updated to m backA and m b to m backB . If it is not collision-free from the obstacle, based on the current m a and m b , a path connecting q child and m a , a path connecting m a and m b and a path connecting m b and q ancestor are inserted, and the function is terminated. In the case of being collision-free from the obstacle, if m a and m b are updated, d becomes 1/2. If d is less than ε, the value of t is incremented by 1 and the function terminates. Otherwise, m backA is updated to the point where the line segment connecting m freeA and m a is externalized in a 3:1 ratio, m backB is updated to the point where the line segment connecting m freeB and m b is externalized in a 3:1 ratio, m freeA is updated to m a and m freeB to m b , and the previous process is repeated. Figure 14 shows the overall flowchart of bidirectional interpolation method. Here, ξ t (q goal ) means the t-th next waypoint from the starting point q goal of the path R, and ξ t+n (q goal ) means the n-th next waypoint in the ξ t (q goal ). That is, there are n waypoints between ξ t (q goal ) and ξ t+n (q goal ).      (e) (f) Figure 15. Process of bidirectional interpolation method: (a) (t = 1) between the waypoints q7 and q2 is not collision-free from the obstacle; (b) find the interpolation point m(q7) between q7 and q8 and the interpolation point m(q8) between q8 and q2, insert the interpolation points between the paths and delete the q8 path; (c) the route between q7 and q10 (from m(q8)) is collision-free from the obstacle, so join the path and delete path q9 (from m(q7)); (d) the route between q7 and q2 is not collision-free from the obstacle; (e) in the process of interpolation between q7 and q2, assuming that d becomes smaller than ε, move the focusing point (ξ t (qgoal)) to the next point (t ← t + 1) and (t = 2). The route between q10 and q3 is collision-free from the obstacle; (f) as it is collision-free from the obstacle from q10 to qstart, q10 and qstart are connected, the waypoint between them is deleted. Figure 15a starts when the waypoint index t is 1. That is, as ξ t (qgoal) is ξ(qgoal), it becomes q7 in the figure. As q7 and the next waypoint q2 are not collision-free from the obstacle, interpolation proceeds. Figure 15b shows that the interpolation point m(q7) of q7~q8 and the interpolation point m(q8) of q8~q2 are free from obstacle collision. This is a case where the vertical distance d between the obstacle and the line segment formed by the interpolation points is smaller than the set threshold ε. The interpolation points m(q7) and m(q8) are inserted between the existing paths q7 to q2, and the path is modified. Existing paths q7 to q8 and q8 to q2 are deleted, and paths q7 to m(q7), m(q7) to m(q8), and m(q8) to q2 are inserted. In Figure 15c, the existing paths q7~q9, q9~q10 are deleted and q7~q10 is inserted because the distance between q7 and q10 is free from obstacle collision. In this case, q9 and q10 refer to m(q7) and m(q8) in Figure 15b. In Figure 15d, interpolation is performed for q7~q10 and q10~q2 because the line between q7 and q2 is not collision-free from the obstacle. Accordingly, index t becomes 2, and the focused waypoint becomes q10, which is ξ 2 (qgoal). At this time, it can be seen that the space between q10 and q3 is free from obstacle collision. Figure 15f shows that all the waypoints on the path q10~qstart are free from obstacle collision, so the existing path between q10~qstart is deleted, and a path that connects qstart in a straight line is inserted in q10. Finally, the path R is modified to (qgoal, q7, q10, qgoal).

Experimental Results
To check the performance of the bidirectional interpolation method proposed in this paper, the path planning results between visibility graph, RRT-connect, PTPMI and bidirectional interpolation method upon various environments were compared through simulation.
The performance measure is the average path length (px) and planning time (ms) until the first complete path is created when each algorithm (excluding visibility graph algorithm) is repeated 100 times (sampling location changes with every trial).

Experimental Environment
This section introduces the environment map used in the simulation and the computer specifications (i.e., hardware) for the simulation. Figure 16 shows the six environmental maps used in the experiment. Here, the green circle (S) refers to the starting point, and the purple circle (G) refers to the destination point. A black polygon with a yellow border (blue in the experimental results) indicates an obstacle. The size of all environment maps is 600 × 600 px, and the step length is 30 px. Figure 15. Process of bidirectional interpolation method: (a) (t = 1) between the waypoints q 7 and q 2 is not collision-free from the obstacle; (b) find the interpolation point m(q 7 ) between q 7 and q 8 and the interpolation point m(q 8 ) between q 8 and q 2 , insert the interpolation points between the paths and delete the q 8 path; (c) the route between q 7 and q 10 (from m(q 8 )) is collision-free from the obstacle, so join the path and delete path q 9 (from m(q 7 )); (d) the route between q 7 and q 2 is not collision-free from the obstacle; (e) in the process of interpolation between q 7 and q 2 , assuming that d becomes smaller than ε, move the focusing point (ξ t (q goal )) to the next point (t ← t + 1) and (t = 2). The route between q 10 and q 3 is collision-free from the obstacle; (f) as it is collision-free from the obstacle from q 10 to q start , q 10 and q start are connected, the waypoint between them is deleted. Figure 15a starts when the waypoint index t is 1. That is, as ξ t (q goal ) is ξ(q goal ), it becomes q 7 in the figure. As q 7 and the next waypoint q 2 are not collision-free from the obstacle, interpolation proceeds. Figure 15b shows that the interpolation point m(q 7 ) of q 7~q8 and the interpolation point m(q 8 ) of q 8~q2 are free from obstacle collision. This is a case where the vertical distance d between the obstacle and the line segment formed by the interpolation points is smaller than the set threshold ε. The interpolation points m(q 7 ) and m(q 8 ) are inserted between the existing paths q 7 to q 2 , and the path is modified. Existing paths q 7 to q 8 and q 8 to q 2 are deleted, and paths q 7 to m(q 7 ), m(q 7 ) to m(q 8 ), and m(q 8 ) to q 2 are inserted. In Figure 15c, the existing paths q 7~q9 , q 9~q10 are deleted and q 7~q10 is inserted because the distance between q 7 and q 10 is free from obstacle collision. In this case, q 9 and q 10 refer to m(q 7 ) and m(q 8 ) in Figure 15b. In Figure 15d, interpolation is performed for q 7~q10 and q 10~q2 because the line between q 7 and q 2 is not collision-free from the obstacle. Accordingly, index t becomes 2, and the focused waypoint becomes q 10 , which is ξ 2 (q goal ). At this time, it can be seen that the space between q 10 and q 3 is free from obstacle collision. Figure 15f shows that all the waypoints on the path q 10~qstart are free from obstacle collision, so the existing path between q 10~qstart is deleted, and a path that connects q start in a straight line is inserted in q 10 . Finally, the path R is modified to (q goal , q 7 , q 10 , q goal ).

Experimental Results
To check the performance of the bidirectional interpolation method proposed in this paper, the path planning results between visibility graph, RRT-connect, PTPMI and bidirectional interpolation method upon various environments were compared through simulation.
The performance measure is the average path length (px) and planning time (ms) until the first complete path is created when each algorithm (excluding visibility graph algorithm) is repeated 100 times (sampling location changes with every trial).

Experimental Environment
This section introduces the environment map used in the simulation and the computer specifications (i.e., hardware) for the simulation. Figure 16 shows the six environmental maps used in the experiment. Here, the green circle (S) refers to the starting point, and the purple circle (G) refers to the destination point. A black polygon with a yellow border (blue in the experimental results) indicates an obstacle. The size of all environment maps is 600 × 600 px, and the step length is 30 px.  Various environmental maps were considered and utilized to gauge the performance of bidirectional interpolation method. The environment maps used are important because the results of the performance measurement expected during the experiment are different depending on their composition, i.e., the number, arrangement and shape of the obstacles within the map. In this paper, the six environmental maps shown in Figure  16 are used to verify the performance of bidirectional interpolation method. These maps are part of the experimental environment [27] proposed by Jihee Han in 2017, and the following characteristics and efficiency of performance measures are expected for each map. Figure 16a shows Map 1, an environment in which the completeness of the path planning method can be easily verified, which is also an environment mainly used to show the local minima problem solving in the potential field algorithm [28]. Figure 16b shows Map 2, in which the optimality and completeness of the path planning method can be verified. Figure 16c shows Map 3, which is suited to verifying the optimality and completeness. Figure 16d shows Map 4, which is suited to verifying the optimality of the path planning method, as well as the planning time, because it is composed of obstacles (50 squares) that resemble a curved shape. Figure 16e shows Map 5, which is an environment in which it is easy to comprehensively verify the optimality and completeness of the path planning method as well as the planning time. Figure 16f shows Map 6, in which it is easy to verify the completeness and planning time of the path planning method. Furthermore, Map 6 is an environment that is unfavorable to sampling-based path planning methods such as the RRT algorithm.
As the sampling-based path planning method relies on probabilistic completeness, the number of sampling times and the planning time required increase considerably as there are narrow or few entrances in the direction to the destination. Various environmental maps were considered and utilized to gauge the performance of bidirectional interpolation method. The environment maps used are important because the results of the performance measurement expected during the experiment are different depending on their composition, i.e., the number, arrangement and shape of the obstacles within the map. In this paper, the six environmental maps shown in Figure 16 are used to verify the performance of bidirectional interpolation method. These maps are part of the experimental environment [27] proposed by Jihee Han in 2017, and the following characteristics and efficiency of performance measures are expected for each map. Figure 16a shows Map 1, an environment in which the completeness of the path planning method can be easily verified, which is also an environment mainly used to show the local minima problem solving in the potential field algorithm [28]. Figure 16b shows Map 2, in which the optimality and completeness of the path planning method can be verified. Figure 16c shows Map 3, which is suited to verifying the optimality and completeness. Figure 16d shows Map 4, which is suited to verifying the optimality of the path planning method, as well as the planning time, because it is composed of obstacles (50 squares) that resemble a curved shape. Figure 16e shows Map 5, which is an environment in which it is easy to comprehensively verify the optimality and completeness of the path planning method as well as the planning time. Figure 16f shows Map 6, in which it is easy to verify the completeness and planning time of the path planning method. Furthermore, Map 6 is an environment that is unfavorable to sampling-based path planning methods such as the RRT algorithm.
As the sampling-based path planning method relies on probabilistic completeness, the number of sampling times and the planning time required increase considerably as there are narrow or few entrances in the direction to the destination. Table 1 summarizes the performance of the computer used in the simulation. The simulator used for the simulation 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. There may be differences in planning time during simulation depending on computer performance. Therefore, in the experiment in this study, the planning time is compared not absolutely but relatively, based on the RRT-connect algorithm.

Experimental Results and Analysis for Each Map
In this section, the experimental results of applying the algorithms to the environment map presented in Figure 16 are stated and analyzed. The algorithms used are visibility graph, RRT-connect, triangular-RRT-connect, PTPMI and the proposed algorithm. PTPMI and the proposed algorithm were applied to the path created by RRT-connect, and the ε value was set to 50, 30 and 10 px, respectively. As ε requires a higher amount of computation as it decreases, it was set to an appropriate value nearby depending on the step length (30 px) of the experimental environment.
The experimental results will show the planned path for each algorithm for each map and show the piecewise linear shape of the path. In addition, as the results of the visibility graph for each map are presented together, the optimality of each algorithm could be visually confirmed.
The contents to be checked through the table are the path length and planning time, which are performance measures. The length of the path created through each algorithm and the relative ratio for the length of the path created by the visibility graph were considered.
PTPMI and the proposed algorithm are methods for post-processing the generated path. In this study, given that PTPMI and the proposed algorithm are applied based on RRT-connect, the planning time is compared based on the RRT-connect algorithm. The planning time is compared to basic RRT-connect and the difference between planning times of PTPMI and the proposed algorithm must be checked. The path length and planning time are presented in Table 2. Table 2. Experimental result based on RRT-connect of Map 1 (the parentheses to the right of each value of path length are the relative ratios based on visibility graph (253 px)).  Figure 17 shows the path planning results for Map 1 for each algorithm. Looking at the generated path (yellow line), compared to Figure 17a, which is the result of RRT-connect, when PTPMI (Figure 17d-f) and the proposed algorithm (Figure 17g-i) were each applied for post-processing, the piecewise linear shape with sharp curves was reduced. In addition, it can be seen that the smaller the ε value, the higher the similarity with the path generated by the visibility graph (Figure 15c). Sensors 2021, 21, x FOR PEER REVIEW 20 of 41   Table 2 summarizes the experimental results numerically for Map 1 based on RRT-connect among the presented environmental maps. It can be seen that the path length is the shortest when the ε value of the proposed algorithm is 10 (px) and is also closest to the path generated by the visibility graph (relative ratio is 257 (px)/253 (px), which is about 101%). The planning time was approximately 1 ms when the ε value of the proposed algorithm was 50 (px), and it takes less than 1 ms in most cases except for this case, similar to the standard RRT-connect. Thus, in Map 1, it can be confirmed that the proposed algorithm is more efficient and optimal than PTPMI. Table 3 summarizes the experimental results numerically for Map 1 based on triangular-RRT-connect. It can be seen that the path length is the shortest when the ε value of the proposed algorithm is 10 (px) and is also closest to the path generated by the visibility graph (relative ratio is 257 (px)/253 (px), which is about 101%). The planning time takes less than 1 ms in all cases, similar to the standard RRT-connect. Thus, in Map 1, it can be confirmed that the proposed algorithm is a little more efficient and optimal than PTPMI. Figure 18 shows the path planning results for Map 2 for each algorithm. Looking at the generated path (yellow line), compared to Figure 18a, which is the result of RRT-connect, when PTPMI (Figure 18d-f) and the proposed algorithm (Figure 18g-i) were each applied for post-processing, the piecewise linear shape with sharp curves was reduced. In addition, it can be seen that the smaller the ε value, the higher the similarity with the path generated by the visibility graph (Figure 16c). Table 3. Experimental result based on triangular-RRT-connect of Map 1 (the parentheses to the right of each value of path length are the relative ratios based on visibility graph (253 px)).  Table 2 summarizes the experimental results numerically for Map 1 based on RRTconnect among the presented environmental maps. It can be seen that the path length is the shortest when the ε value of the proposed algorithm is 10 (px) and is also closest to the path generated by the visibility graph (relative ratio is 257 (px)/253 (px), which is about 101%). The planning time was approximately 1 ms when the ε value of the proposed algorithm was 50 (px), and it takes less than 1 ms in most cases except for this case, similar to the standard RRT-connect. Thus, in Map 1, it can be confirmed that the proposed algorithm is more efficient and optimal than PTPMI. Table 3 summarizes the experimental results numerically for Map 1 based on triangular-RRT-connect. It can be seen that the path length is the shortest when the ε value of the proposed algorithm is 10 (px) and is also closest to the path generated by the visibility graph (relative ratio is 257 (px)/253 (px), which is about 101%). The planning time takes less than 1 ms in all cases, similar to the standard RRT-connect. Thus, in Map 1, it can be confirmed that the proposed algorithm is a little more efficient and optimal than PTPMI. Table 3. Experimental result based on triangular-RRT-connect of Map 1 (the parentheses to the right of each value of path length are the relative ratios based on visibility graph (253 px)).  Figure 18 shows the path planning results for Map 2 for each algorithm. Looking at the generated path (yellow line), compared to Figure 18a, which is the result of RRT-connect, when PTPMI (Figure 18d-f) and the proposed algorithm (Figure 18g-i) were each applied for post-processing, the piecewise linear shape with sharp curves was reduced. In addition, it can be seen that the smaller the ε value, the higher the similarity with the path generated by the visibility graph (Figure 16c).   Table 4 summarizes the experimental results numerically for Map 2 based on RRT-connect. The path length is the shortest when the ε value of the proposed algorithm is 10 (px) and is closest to the visibility graph (relative ratio is 1223/1172, which is about 104%). It can be seen that the planning time takes longer than RRT-connect when post-processing techniques are applied. However, as expected, as the value of ε decreased, PTPMI and the proposed algorithm did not take longer. Rather, it can be seen that in the proposed algorithm, which requires more steps than PTPMI, the case where ε: 10 (px) (which is expected to take the longest time) shows the smallest difference from RRT-connect. This deviation in planning time is not due to any issues related to the post-processing technique, but is due to the random sampling effect of the RRT-like algorithms. In other words, it is difficult to find a solution for Map 2 using the RRT-based algorithm. In summary, in Map 2, it was confirmed that the proposed algorithm guarantees the optimality of the path length compared to other algorithms. Table 4. Experimental result based on RRT-connect of Map 2 (the parentheses to the right of each value of path length are relative ratios based on visibility graph (1172 px)).  Table 4 summarizes the experimental results numerically for Map 2 based on RRTconnect. The path length is the shortest when the ε value of the proposed algorithm is 10 (px) and is closest to the visibility graph (relative ratio is 1223/1172, which is about 104%). It can be seen that the planning time takes longer than RRT-connect when post-processing techniques are applied. However, as expected, as the value of ε decreased, PTPMI and the proposed algorithm did not take longer. Rather, it can be seen that in the proposed algorithm, which requires more steps than PTPMI, the case where ε: 10 (px) (which is expected to take the longest time) shows the smallest difference from RRT-connect. This deviation in planning time is not due to any issues related to the post-processing technique, but is due to the random sampling effect of the RRT-like algorithms. In other words, it is difficult to find a solution for Map 2 using the RRT-based algorithm. In summary, in Map 2, it was confirmed that the proposed algorithm guarantees the optimality of the path length compared to other algorithms.  Table 5 summarizes the experimental results numerically for Map 2 based on triangular-RRT-connect. The path length is the shortest when the ε value of the proposed algorithm is 10 (px) and is closest to the visibility graph (relative ratio is 1229/1172, which is about 105%). However, as expected, as the value of ε decreased, PTPMI and the proposed algorithm did not take longer. Even some results (ε: 30 px, 10 px) have a shorter planning time than triangular-RRT-connect. Rather, it can be seen that in the proposed algorithm, which requires more steps than PTPMI, the case where ε: 10 (px) (which is expected to take the longest time) shows shorter time than triangular-RRT-connect. This difference in planning time is not due to any issues related to the post-processing technique, but is due to the random sampling effects of the RRT-like algorithms. In other words, it is more difficult to find a solution for Map 2 using the RRT-based algorithm. In summary, in Map 2, it was confirmed that the proposed algorithm guarantees the optimality of the path length compared with other algorithms. Table 5. Experimental result based on triangular-RRT-connect of Map 2 (the parentheses to the right of each value of path length are relative ratios based on visibility graph (1172 px)).  Figure 19 shows the path planning results for Map 3 for each algorithm. Looking at the generated path (yellow line), compared to Figure 19a, which is the result of RRT-connect, when PTPMI (Figure 19d-f) and the proposed algorithm (Figure 19g-i) were each applied for post-processing, the piecewise linear shape with sharp curves was reduced. In addition, it can be seen that the smaller the ε value, the higher the similarity with the path generated by the visibility graph (Figure 17c).  Table 5 summarizes the experimental results numerically for Map 2 based on triangular-RRT-connect. The path length is the shortest when the ε value of the proposed algorithm is 10 (px) and is closest to the visibility graph (relative ratio is 1229/1172, which is about 105%). However, as expected, as the value of ε decreased, PTPMI and the proposed algorithm did not take longer. Even some results (ε: 30 px, 10 px) have a shorter planning time than triangular-RRT-connect. Rather, it can be seen that in the proposed algorithm, which requires more steps than PTPMI, the case where ε: 10 (px) (which is expected to take the longest time) shows shorter time than triangular-RRT-connect. This difference in planning time is not due to any issues related to the post-processing technique, but is due to the random sampling effects of the RRT-like algorithms. In other words, it is more difficult to find a solution for Map 2 using the RRT-based algorithm. In summary, in Map 2, it was confirmed that the proposed algorithm guarantees the optimality of the path length compared with other algorithms. Figure 19 shows the path planning results for Map 3 for each algorithm. Looking at the generated path (yellow line), compared to Figure 19a, which is the result of RRT-connect, when PTPMI (Figure 19d-f) and the proposed algorithm (Figure 19g-i) were each applied for post-processing, the piecewise linear shape with sharp curves was reduced. In addition, it can be seen that the smaller the ε value, the higher the similarity with the path generated by the visibility graph (Figure 17c). Table 5. Experimental result based on triangular-RRT-connect of Map 2 (the parentheses to the right of each value of path length are relative ratios based on visibility graph (1172 px)).    Table 6 summarizes the experimental results numerically for Map 3 based on RRTconnect. The case where the ε value of the proposed algorithm is 10 (px) results in the shortest path length compared to other cases and is closest to the path generated by the visibility graph (the relative ratio is 726/714, which is about 102%). It can be seen that the planning time of RRT-connect is shorter than that of PTPMI and BPTPMI. The biggest difference from RRT-Connect occurs when using the proposed algorithm with ε: 10 px. However, the difference between the values is very insignificant at 3 ms. Furthermore, the shortest planning time occurred when using the proposed algorithm with ε:50 px. At this time, compared to RRT-connect, the path was reduced by 218 px, and compared to PTPMI, it was reduced by 4 px. This means that for Map 3, the proposed algorithm guarantees optimality compared to other algorithms and has a similar planning time to basic RRT-connect. Table 6. Experimental result based on RRT-connect of Map 3 (the parentheses to the right of each value of path length are relative ratios based on visibility graph (714 px)).  Table 7 summarizes the experimental results numerically for Map 3 based on triangular-RRT-connect. The case where the ε value of the proposed algorithm is ε: 10 px results in the shortest path length compared to other cases and is closest to the path generated by the visibility graph (the relative ratio is 727/714, which is about 102%). It can be seen that the planning time of triangular-RRT-connect is shorter than that of PTPMI and BPTPMI. The biggest difference from triangular-RRT-connect occurs when using the proposed algorithm. However, the difference between the values is very insignificant at 2 ms. Furthermore, the shortest path length occurred when using the proposed algorithm with ε: 10 px. At this time, compared to triangular-RRT-connect, the path was reduced by 86 px, and compared to PTPMI, it was reduced by 7 px. This means that for Map 3, the proposed algorithm guarantees optimality compared with other algorithms and has a similar planning time to basic RRT-connect. Table 7. Experimental result based on triangular-RRT-connect of Map 3 (the parentheses to the right of each value of path length are relative ratios based on visibility graph (714 px)).  Figure 20 shows the path planning results for Map 4 for each algorithm. Looking at the generated path (yellow line), compared to Figure 20a, which is the result of RRT-connect, when PTPMI (Figure 20d-f) and the proposed algorithm (Figure 20g-i) were each applied for post-processing, the piecewise linear shape with sharp curves was reduced. In addition, it can be seen that the smaller the ε value, the higher the similarity with the path generated by the visibility graph (Figure 18c).   Table 8 summarizes the experimental results numerically for Map 4 based on RRT-connect. It can be seen that the case where the ε value of the proposed algorithm is 10 (px) results in the shortest path length compared to other cases and is closest to the path generated by the visibility graph (relative ratio is 475/470, which is about 101%). It can be seen that the planning time of RRT-connect is shorter than that of PTPMI and bidirectional interpolation method. The biggest difference from RRT-connect occurs when using PTPMI with ε: 30 px. However, the difference between the values is very insignificant at 3 ms. Furthermore, the shortest planning time occurs when using the proposed algorithm with ε: 50 px. At this time, compared to RRT-connect, the path length is reduced by 72 px and has the same planning time as PTPMI. This means that the proposed algorithm for Map 4 guarantees optimality compared to other algorithms and has a similar planning time to basic RRT-connect. Table 8. Experimental result based on RRT-connect of Map 4 (the parentheses to the right of each value of path length are relative ratios based on visibility graph (470 px)).  Table 8 summarizes the experimental results numerically for Map 4 based on RRTconnect. It can be seen that the case where the ε value of the proposed algorithm is 10 (px) results in the shortest path length compared to other cases and is closest to the path generated by the visibility graph (relative ratio is 475/470, which is about 101%). It can be seen that the planning time of RRT-connect is shorter than that of PTPMI and bidirectional interpolation method. The biggest difference from RRT-connect occurs when using PTPMI with ε: 30 px. However, the difference between the values is very insignificant at 3 ms. Furthermore, the shortest planning time occurs when using the proposed algorithm with ε: 50 px. At this time, compared to RRT-connect, the path length is reduced by 72 px and has the same planning time as PTPMI. This means that the proposed algorithm for Map 4 guarantees optimality compared to other algorithms and has a similar planning time to basic RRT-connect.  Table 9 summarizes the experimental results numerically for Map 4 based on triangular-RRT-connect. It can be seen that the case where the ε value of the proposed algorithm is 10 (px) results in the shortest path length compared to other cases and is closest to the path generated by the visibility graph (relative ratio is 479/470, which is about 102%). Planning time is expressed as 1-2 ms in all cases. The biggest difference from triangular-RRT-connect occurs when using proposed algorithm with ε: 10 px. However, the difference between the values is very insignificant at 1 ms. This means that the proposed algorithm for Map 4 guarantees optimality compared to other algorithms and has a similar planning time to basic triangular-RRT-connect. Table 9. Experimental result based on triangular-RRT-connect of Map 4 (the parentheses to the right of each value of path length are relative ratios based on visibility graph (470 px)).

Performance
TriangularRRT  Figure 21 shows the path planning results for Map 5 for each algorithm. Looking at the generated path (yellow line), compared to Figure 21a, which is the result of RRT-connect, when PTPMI (Figure 21d-f) and the proposed algorithm (Figure 21g-i) were each applied for post-processing, the piecewise linear shape with sharp curves was reduced. In addition, it can be seen that the smaller the ε value, the higher the similarity with the path generated by the visibility graph (Figure 19c).  Table 9 summarizes the experimental results numerically for Map 4 based on triangular-RRT-connect. It can be seen that the case where the ε value of the proposed algorithm is 10 (px) results in the shortest path length compared to other cases and is closest to the path generated by the visibility graph (relative ratio is 479/470, which is about 102%). Planning time is expressed as 1-2 ms in all cases. The biggest difference from triangular-RRT-connect occurs when using proposed algorithm with ε: 10 px. However, the difference between the values is very insignificant at 1 ms. This means that the proposed algorithm for Map 4 guarantees optimality compared to other algorithms and has a similar planning time to basic triangular-RRT-connect. Figure 21 shows the path planning results for Map 5 for each algorithm. Looking at the generated path (yellow line), compared to Figure 21a, which is the result of RRT-connect, when PTPMI (Figure 21d-f) and the proposed algorithm (Figure 21g-i) were each applied for post-processing, the piecewise linear shape with sharp curves was reduced. In addition, it can be seen that the smaller the ε value, the higher the similarity with the path generated by the visibility graph (Figure 19c).   Table 10 summarizes the experimental results numerically for Map 5 based on RRT-connect. It can be seen that the case where the ε value of the proposed algorithm is 10 (px) results in the shortest path length compared to other cases, and the path is closest to the one generated by the visibility graph (the relative ratio is 646/576, which is about 112%). It can be seen that the planning time of RRT-connect is shorter than that of PTPMI and bidirectional interpolation method. The biggest difference from RRT-connect occurs when using PTPMI with ε: 10 px. However, the difference between the values is very insignificant at 3 ms. In other situations, it can be confirmed that the planned time is always 2 ms. This means that the proposed algorithm guarantees optimality for traversing Map 5 compared to other algorithms and the planning time differs from RRT-connect by 1 ms. Table 10. Experimental result based on RRT-connect of Map 5 (the parentheses to the right of each value of path length are relative ratios based on visibility graph (576 px)). Table 11 summarizes the experimental results numerically for Map 5 based on triangular-RRT-connect. It can be seen that the case where the ε value of the proposed algorithm is 10 (px) results in the shortest path length compared to other cases, and the path is closest to the one generated by the visibility graph (the relative ratio is 641/576, which is about 111%). Planning time is expressed as 1-2 ms in all cases. The biggest difference from triangular-RRT-connect occurs when using proposed algorithm with ε: 50 px. However, the difference between the values is very insignificant at 1 ms. This means that the proposed algorithm guarantees optimality compared with other algorithms and the planning time differs from triangular-RRT-connect by 1 ms. Figure 22 shows the path planning results for Map 6 for each algorithm. Looking at the generated path (yellow line), compared to Figure 22a, which is the result of  Table 10 summarizes the experimental results numerically for Map 5 based on RRTconnect. It can be seen that the case where the ε value of the proposed algorithm is 10 (px) results in the shortest path length compared to other cases, and the path is closest to the one generated by the visibility graph (the relative ratio is 646/576, which is about 112%). It can be seen that the planning time of RRT-connect is shorter than that of PTPMI and bidirectional interpolation method. The biggest difference from RRT-connect occurs when using PTPMI with ε: 10 px. However, the difference between the values is very insignificant at 3 ms. In other situations, it can be confirmed that the planned time is always 2 ms. This means that the proposed algorithm guarantees optimality for traversing Map 5 compared to other algorithms and the planning time differs from RRT-connect by 1 ms.  Table 11 summarizes the experimental results numerically for Map 5 based on triangular-RRT-connect. It can be seen that the case where the ε value of the proposed algorithm is 10 (px) results in the shortest path length compared to other cases, and the path is closest to the one generated by the visibility graph (the relative ratio is 641/576, which is about 111%). Planning time is expressed as 1-2 ms in all cases. The biggest difference from triangular-RRT-connect occurs when using proposed algorithm with ε: 50 px. However, the difference between the values is very insignificant at 1 ms. This means that the proposed algorithm guarantees optimality compared with other algorithms and the planning time differs from triangular-RRT-connect by 1 ms.  Figure 22 shows the path planning results for Map 6 for each algorithm. Looking at the generated path (yellow line), compared to Figure 22a, which is the result of RRT-connect, when PTPMI (Figure 22d-f) and the proposed algorithm (Figure 22g-i) were each applied for post-processing, the piecewise linear shape with sharp curves was reduced. In addition, it can be seen that the smaller the ε value, the higher the similarity with the path generated by the visibility graph (Figure 20c). Table 12 summarizes the experimental results numerically for Map 6 based on RRTconnect. The case where the ε value of the proposed algorithm is ε: 10 px results in the shortest path length compared to other cases and is closest to the visibility graph (the relative ratio is 1187/1165, which is about 101%). It can be seen that the planning time of RRT-Connect is shorter than that of PTPMI and the bidirectional interpolation method. The biggest difference from RRT-connect occurs when ε: 50 px of the algorithm is to be bounded. However, the difference between the values is very insignificant at 6 ms. Furthermore, the shortest planning time occurred when using PTPMI with ε: 50 px. This means that the proposed algorithm guarantees optimality compared to other algorithms, but takes an average of 4 ms longer for Map 6. were each applied for post-processing, the piecewise linear shape with sharp curves was reduced. In addition, it can be seen that the smaller the ε value, the higher the similarity with the path generated by the visibility graph (Figure 20c).   Table 12 summarizes the experimental results numerically for Map 6 based on RRT-connect. The case where the ε value of the proposed algorithm is ε: 10 px results in the shortest path length compared to other cases and is closest to the visibility graph (the relative ratio is 1187/1165, which is about 101%). It can be seen that the planning time of RRT-Connect is shorter than that of PTPMI and the bidirectional interpolation method. The biggest difference from RRT-connect occurs when ε: 50 px of the algorithm is to be   Table 13 summarizes the experimental results numerically for Map 6 based on triangular RRT-connect. The case where the ε value of the proposed algorithm is ε: 10 px results in the shortest path length compared to other cases and is closest to the visibility graph (the relative ratio is 1186/1165, which is about 102%). The planning time is similar to triangular-RRT-connect with the proposed method. The biggest difference from triangular-RRT-connect occurs when ε: 50 px of the algorithm is to be bounded. However, the difference between the values is very insignificant at 1 ms. Furthermore, the shortest planning time occurred when using PTPMI with ε: 30 px. This means that the proposed algorithm guarantees optimality compared with other algorithms. Table 13. Experimental result based on triangular-RRT-connect of Map 6 (the parentheses to the right of each value of path length are relative ratios based on visibility graph (1165 px)).

Experimental Results and Analysis
In this section, the experimental results of Maps 1 to 6 are summarized. Table 14 is a table summarizing the experimental results on the path length. It can be seen that, for all maps, the proposed algorithm creates a shorter path compared to RRT-connect. The RRT-connect algorithm generates a path whose length is approximately 138% ((150 + 157 + 140 + 122 + 132 + 127)/6) longer on average compared to the visibility graph. Similarly, PTPMI generated about 113% longer paths with ε: 50 px, about 108% with ε: 30 px and about 105% longer with ε: 10 px compared to the visibility graph. In the case of the proposed algorithm, it can be seen that the path generated on average is about 112% longer with ε: 50 px, about 108% with ε: 30 px and about 104% longer at ε: 10 px compared to the visibility graph. Thus, the proposed algorithm has a path length closer to the visibility graph as the value of epsilon decreases. Based on the ε: 10 px of the proposed algorithm, the average path length decreased by about 34% compared to RRT-connect, and it was improved by about 1% compared to PTPMI.  Table 15 is a table summarizing the experimental results on the path length based on triangular-RRT-connect. It can be seen that, for all maps, the proposed algorithm creates a shorter path compared to triangular-RRT-connect. The triangular-RRT-connect algorithm generates a path whose length is approximately 115% ((111 + 126 + 114 + 109 + 117 + 111)/6) longer on average compared to the visibility graph. Similarly, PTPMI generated about 112% longer paths with ε: 50 px, about 108% with ε: 30 px and about 105% longer with ε: 10 px compared to the visibility graph. In the case of the proposed algorithm, it can be seen that the path generated on average is about 112% longer with ε: 50 px, about 108% with ε: 30 px and about 104% longer at ε: 10 px compared to the visibility graph. Thus, the proposed algorithm has a path length a little closer to the visibility graph as the value of epsilon decreases. Based on the ε: 10 px of the proposed algorithm, the average path length decreased by about 11% compared to triangular-RRT-connect, and it was improved by about 1% compared to PTPMI.  Table 16 summarizes the experimental results on the planning time. In all maps, it can be seen that the proposed algorithm takes longer than RRT-connect. However, the difference is not large. Based on ε: 10 px, which was confirmed to be closest to optimality through Table 13, the biggest difference with RRT-connect, of 4 ms, occurs for Map 6. As the proposed algorithm comprises an additional procedure to approach obstacles compared to PTPMI, it is predicted that it would require more planning time compared to PTPMI. The case in which the proposed algorithm takes the most time compared to PTPMI is when the ε value in Map 2 is 30 px, and the difference between the two is 8 ms. However, there are cases where the time of the proposed algorithm is reduced compared to PTPMI. In particular, for Map 2, when ε: 10 px, the proposed algorithm requires 223 ms, whereas PTPMI requires 250 ms, which is a reduction of 27 ms for the proposed algorithm. This may be due to the reason that the planning time of the proposed algorithm is more affected by the random sampling effects, an intrinsic problem of the RRT-series algorithm, than by the time required to process the additional procedure. Table 17 summarizes the experimental results on the planning time. It can be seen that the proposed algorithm takes longer than triangular-RRT-connect in most maps. However, the difference is not large. Based on ε: 10 px, which was confirmed to be closest to optimality through Table 14, the biggest difference with RRT-Connect, of 2 ms, occurs for Map 3. Even in Maps 2 and 6, it can be seen that the time is reduced. %clearpage As the proposed algorithm comprises an additional procedure to approach obstacles compared to PTPMI, it is predicted that it would require more planning time compared to PTPMI. The case in which the proposed algorithm takes the most time compared to PTPMI is when the ε value in Map 2 is 50 px, and the difference between the two is 8 ms. However, there are cases where the time of the proposed algorithm is reduced compared to PTPMI. In particular, for Map 2, when ε: 30 px, the proposed algorithm requires 181 ms, whereas PTPMI requires 194 ms, which is a reduction of 13 ms for the proposed algorithm. Thus, it can be confirmed that the planning time of the proposed algorithm is more affected by the probabilistic integrity, an intrinsic problem of the RRT-like algorithms, than by the time required to process the additional procedure. Figure 23 shows the overall result for Map 1. The rectangle represents worst path length, the circle represents average path length and the triangle represents best path length. First of all, it can be seen that the results similar to the visibility graph appear in all cases except for RRT-connect in best path length (triangle). Average path length (circle) is shorter when the post-processing method (PTPMI or proposed method) is applied than the original RRT-connect and triangular-RRT-connect. It can be seen that worst path length (rectangle) gradually approaches average as the ε value decreases in the post-processing method. In other words, if the post-processing method is applied, worst path length (rectangle) is enhanced. Moreover, the proposed method enhanced worst path length better than PTPMI.
Sensors 2021, 21, x FOR PEER REVIEW 37 of 41 cle) is shorter when the post-processing method (PTPMI or proposed method) is applied than the original RRT-connect and triangular-RRT-connect. It can be seen that worst path length (rectangle) gradually approaches average as the ε value decreases in the post-processing method. In other words, if the post-processing method is applied, worst path length (rectangle) is enhanced. Moreover, the proposed method enhanced worst path length better than PTPMI.  Figure 24 shows the overall result for Map 2. It can be seen that the path length is improved in all cases (best, worst, average) when the post-processing method is applied rather than the original algorithm. In addition, as the ε value decreased in the post-processing method, the path length was enhanced in all cases (best, worst, average). If the ε value is the same, the proposed method is more enhanced than the PTPMI in all cases (best, worst, average).  Figure 24 shows the overall result for Map 2. It can be seen that the path length is improved in all cases (best, worst, average) when the post-processing method is applied Sensors 2021, 21, 7425 36 of 40 rather than the original algorithm. In addition, as the ε value decreased in the postprocessing method, the path length was enhanced in all cases (best, worst, average). If the ε value is the same, the proposed method is more enhanced than the PTPMI in all cases (best, worst, average).  Figure 24 shows the overall result for Map 2. It can be seen that the path length is improved in all cases (best, worst, average) when the post-processing method is applied rather than the original algorithm. In addition, as the ε value decreased in the post-processing method, the path length was enhanced in all cases (best, worst, average). If the ε value is the same, the proposed method is more enhanced than the PTPMI in all cases (best, worst, average).   Figure 25 shows the overall result for Map 3. It can be seen that the path length is improved in all cases (best, worst, average) when the post-processing method is applied rather than the original algorithm. In addition, as the ε value decreased in the postprocessing method, the path length was enhanced in all cases (best, worst, average). If the ε value is the same, the proposed method is more enhanced than the PTPMI in all cases (best, worst, average).  Figure 25 shows the overall result for Map 3. It can be seen that the path length is improved in all cases (best, worst, average) when the post-processing method is applied rather than the original algorithm. In addition, as the ε value decreased in the post-processing method, the path length was enhanced in all cases (best, worst, average). If the ε value is the same, the proposed method is more enhanced than the PTPMI in all cases (best, worst, average).  Figure 26 shows the overall result for Map 4. It can be seen that the results similar to the visibility graph appear in all cases except for RRT-connect in best path length (triangle). When the post-processing method is applied, if ε value is reduced, average and worst are enhanced. Moreover, if ε value is the same in the post-processing method, the proposed method enhanced worst more than PTPMI.  Figure 26 shows the overall result for Map 4. It can be seen that the results similar to the visibility graph appear in all cases except for RRT-connect in best path length (triangle). When the post-processing method is applied, if ε value is reduced, average and worst are enhanced. Moreover, if ε value is the same in the post-processing method, the proposed method enhanced worst more than PTPMI.  Figure 26 shows the overall result for Map 4. It can be seen that the results similar to the visibility graph appear in all cases except for RRT-connect in best path length (triangle). When the post-processing method is applied, if ε value is reduced, average and worst are enhanced. Moreover, if ε value is the same in the post-processing method, the proposed method enhanced worst more than PTPMI.  Figure 27 shows the overall result for Map 5. It can be seen that the results similar to the visibility graph appear in all cases. When the post-processing method is applied, if ε value is reduced, worst is enhanced. Moreover, if ε value is the same in the post-processing method, the proposed method improves worst more than PTPMI.  Figure 27 shows the overall result for Map 5. It can be seen that the results similar to the visibility graph appear in all cases. When the post-processing method is applied, if ε value is reduced, worst is enhanced. Moreover, if ε value is the same in the post-processing method, the proposed method improves worst more than PTPMI.  Figure 28 shows the overall result for Map 6. It can be seen that the path length is improved in all cases (best, worst, average) when the post-processing method is applied rather than the original algorithm. In addition, as the ε value reduced in the post-processing method, the path length was enhanced in all cases (best, worst, average). If the ε value is the same, the proposed method is more enhanced than the PTPMI in all cases (best, worst, average).  Figure 28 shows the overall result for Map 6. It can be seen that the path length is improved in all cases (best, worst, average) when the post-processing method is applied rather than the original algorithm. In addition, as the ε value reduced in the post-processing method, the path length was enhanced in all cases (best, worst, average). If the ε value is the same, the proposed method is more enhanced than the PTPMI in all cases (best, worst, average).  Figure 28 shows the overall result for Map 6. It can be seen that the path length is improved in all cases (best, worst, average) when the post-processing method is applied rather than the original algorithm. In addition, as the ε value reduced in the post-processing method, the path length was enhanced in all cases (best, worst, average). If the ε value is the same, the proposed method is more enhanced than the PTPMI in all cases (best, worst, average).

Conclusions
In this paper, we proposed the bidirectional interpolation method. The proposed method can minimize the planning time and overcome the limit of optimality of sampling-based algorithms and kinodynamic error.
It was confirmed that bidirectional interpolation method plans a path close to the optimum when applied to the existing RRT-like algorithms through mathematical modeling. Simulations were performed to confirm the performance of bidirectional interpolation method. In six different environment maps, it was confirmed that the path length was shortened by 34% on average compared to when the basic RRT-connect algorithm

Conclusions
In this paper, we proposed the bidirectional interpolation method. The proposed method can minimize the planning time and overcome the limit of optimality of samplingbased algorithms and kinodynamic error.
It was confirmed that bidirectional interpolation method plans a path close to the optimum when applied to the existing RRT-like algorithms through mathematical modeling. Simulations were performed to confirm the performance of bidirectional interpolation method. In six different environment maps, it was confirmed that the path length was shortened by 34% on average compared to when the basic RRT-connect algorithm was applied, and the path length was, on average, only 4% longer than the visibility graph. In addition, bidirectional interpolation method has the advantage of being applicable to all path planning methods that plan a locally piecewise linear path.
Compared with the PTPMI algorithm, it was confirmed that the proposed method shows a little better but is staggered with performance by 1-3% in terms of path length, and there was no significant difference in terms of planning time.
In most cases with all maps, the proposed method shows that worst path length was greatly reduced when the post-processing method was applied. In addition, if the ε value is the same, worst path length of the proposed algorithm is improved over PTPMI.
In this paper, a method applicable to the RRT-likes is proposed. However, the proposed method is a technique that can be applied to sampling-based planning algorithms such as RRT. Therefore, it can be applied in various fields where motion planning of robots is used, such as mobile robots, manipulators and drones.