Confidence random tree-based algorithm for mobile robot path planning considering the path length and safety

Abstract

This article introduces a novel confidence random tree-based sampling path planning algorithm for mobile service robots operating in real environments. The algorithm is time efficient, can accommodate narrow corridors, enumerates possible solutions, and minimizes the cost of the path. These benefits are realized by incorporating notable approaches from other existing path planning algorithms into the proposed algorithm. During path selection, the algorithm considers the length and safety of each path via a sampling and rejection method. The algorithm operates as follows. First, the confidence of a path is computed based on the clearance required to ensure the safety of the robot, where the clearance is defined as the distance between the path and the closest obstacle. Then, the sampling method generates a tree graph in which the edge lengths are controlled by the confidence. In a low confidence space, such as a narrow corridor, the corresponding graph has denser samples with short edges while in a high confidence space, the samples are widely spaced with longer edges. Finally, a rejection method is employed to ensure a reasonably short computation time by optimizing the sample density by rejecting unnecessary samples. The performance of the proposed algorithm is validated by comparing the experimental results to those of several commonly used algorithms.

Introduction

Path planning is an essential function in mobile robot navigation. In such applications, planners are employed to identify solution paths in a sufficiently short amount of time, especially in real environments. In doing so, safety is a higher priority than the path length, because in actual environments, these robots share their work space with humans. The easiest way to maximize the safety of a given path is to maximize the clearance along the path, where the clearance is defined as the distance between the path and the closest obstacle. When choosing a path, to prevent the need for excessive detours, the trade-off between the path length and path clearance must be managed. In this article, the confidence random tree (CRT) algorithm will be applied to efficiently manage this trade-off.

Path planners can be classified into one of two categories: non-sampling and sampling. Non-sampling-based planners, such as the artificial potential field (APF) algorithm ¹ and the generalized Voronoi diagram (GVD), ^{2 –4} have been implemented in a variety of applications as they accommodate the safety objective by representing the collision-free space based on the clearance information. While the APF algorithm can efficiently find solutions via gradient descent, it suffers from a local minima problem. Several researchers have attempted to solve this problem ^5,6 by combining the APF approach with other techniques, such as the probabilistic road-map method (PRM) and GVD. ^7,8 A GVD is defined as a set of points that are equidistant from at least two obstacles. Some researchers have taken advantage of the maximum clearance afforded by a GVD ^9,10 ; however, this approach functions poorly in no-obstacle or sparse-obstacle environments. The explanation for this is as follows. Let us assume that only a few obstacles have been detected in a particular space. This may be due to the size of the space, because the range of the existing sensors is not sufficient or some sensors may be occluded. In this case, GVD-based planners generate excessive detours around any obstacles that do appear. This is because these planners are designed to include the medial axis (MA) of a space in the solution path, regardless of how large the space is.

Sampling-based approaches are described in several studies in the path planning domain, and their utility for various applications has been proven in multiple reports. Such approaches can be used in high dimensional spaces under nonholonomic constraints such as robot arm manipulators and steering applications. The solution path generated by sampling-based planners is guaranteed to asymptotically converge to the optimum when unlimited resources, such as time, are available. However, in many applications, the sampling algorithm must be terminated after a specified time rather than waiting indefinitely. Thus, sampling-based planners require terminal conditions, such as a maximum number of samples or a maximum permitted run time. The problem is that these terminal conditions are often determined by the user and may therefore not be appropriate. Consequently, the planner may fail to identify a solution path when the parameters are too low, or the search may consume an unreasonable amount of time if the parameters are set too high. Consequently, it is essential that when this approach is applied, users must identify parameters that are suitable for the specific application and work space of interest.

For mobile robots in real applications, path planners should be capable of optimizing the path length and safety in a reasonable amount of time while avoiding obstacles. In addition, planners should be robust in a variety of environments and only require a few parameters. This is because actual environments may have narrow corridors and/or large halls, and users configuring the planner cannot be expected to choose appropriate parameters for each environment. The proposed CRT algorithm accommodates the required features with a minimal set of parameters. In subsequent sections, the confidence will be defined and the sampling and rejection methods in the CRT algorithm will be detailed. The most important properties of the CRT algorithm are as follows:

Sample distribution control: In the CRT algorithm, the sample distribution is adjusted based on the confidence. When constructing a tree graph, the sampling and rejection methods are used to control the length of the edges, and thereby the sample density, as shown in Figure 1. In this way, the CRT approach can be used to navigate narrow corridors.

Dynamic programming: To identify a near-optimal solution while respecting the path length and safety objectives, the CRT algorithm locates suboptimal samples at each iteration. These are then used as the suboptimal samples in the next iteration instead of recomputing. A more detailed description of this process will be provided in the section on the CRT.

Non-parametric terminal condition: When the CRT method identifies a solution path, the path is considered to be a near-optimal path and no more samples are generated. This property is comparable to those of other optimal path planners that use the run time and number of samples as terminal conditions.

Figure 1.

Diagram showing the CRT algorithm in operation. A tree graph is created that extends from the initial state (red triangle) to the goal state (blue square) while the edge lengths are controlled by the confidence. (a) t = 10; (b) t = 20; (c) t = 30; (d) t = 42. CRT: confidence random tree.

Related work

In this section, sampling-based approaches will be discussed that are variants of the PRM and rapidly-exploring random trees (RRT) algorithms. ¹¹ RRT is a sampling-based path planner that is considered to represent the state-of-the-art, as is the PRM method. These algorithms can be used as single-query and multi-query path planners, respectively, and have been proven particularly useful in applications with many degrees of freedom. However, basic implementations are unable to generate solution paths in narrow spaces even if such solutions exist.

Environments containing narrow spaces, such as bug traps, are well suited to sampling-based approaches as these often require a high computational cost. ¹² An efficient strategy to overcome narrow space problems is known as free-space dilation, ^13,14 which dilates narrow spaces to improve the corresponding visibility. This approach requires the amount and method of dilation to be determined. The algorithm described in one published report ¹³ shrinks obstacles and robots toward their inner skeletons; however, it is unable to overcome the problems associated with the use of the GVD. The tetrahedralization of a polyhedral model can be implemented as another dilation method, ¹⁴ whereby a downscaled model M′ contained inside the original model M is generated. However, it is not always possible to apply tetrahedralization. Even if the aforementioned dilation method is able to successfully generate a downscaled model, the scale of the dilation is problematic. Although a multi-scale dilation method has also been proposed, ¹⁴ environments that include narrow spaces with different widths are still a challenge to navigate.

Another approach that has been proposed to overcome the narrow space problem is the non-uniform sampling strategy, which generates denser samples around obstacles based on a ray between a valid and an invalid configuration. ^15,16 Then, a valid state is identified via a bisection search along the ray. This allows the method to generate samples near obstacles. However, the distribution of the generated samples is sensitive to the invalid state positions and the configuration of the obstacles. A research retracing method has been also proposed, which generates samples in the contact space between collision free and collision spaces. ¹⁷ As an alternative to bisection search methods, uniform distance searches are able to locate samples that are more likely to be uniformly distributed. ¹⁸ In this algorithm, users define a line segment length l and step size t, and valid configuration states are found based on the l/t distance interval. An approach based on the simultaneous use of free and occupied space has been proposed to address narrow spaces ¹⁹ by constructing a road map in both the free and occupied space from which to infer the solution path. Local planners, which are usually implemented as edge validity checkers, generate samples to indicate when the validity changes during the checking process. This allows samples to be efficiently generated in narrow spaces. Strategies based on the use of Gaussian functions have also been proposed. ²⁰ In these approaches, a first state is randomly sampled before a distance is obtained from the Gaussian distribution. Then, a second sample is generated this distance away from the first state. If one state is valid while the other is invalid, the valid state is accepted as a road-map node. In this strategy, node creation is costly because it is rarely required. Furthermore, suitable Gaussian distributions are specified by the user. Sampling-based planners combined with the GVD method have been proposed to address the challenges with path clearance and narrow spaces. ^21,22 These algorithms create a graph that has a high clearance by moving all of the samples to the MA. This successfully produces a high clearance path but shares the limitations of the no-obstacle or spare-obstacle environments of the GVD approach.

In terms of optimal path planners, the primary challenge is the time required to converge on the optimal path. The approach suggested in the literature ^{23 –29} is to reduce the search area and the calls to the local planner, as these effectively reduce the run time and improve the convergence speed. It has been proven that sampling-based approaches always find the optimal path, if such a path exists, given an unlimited amount of time. However, a downside of these approaches is that there is no certainty regarding the existence of an optimal path, how close the proposed path is to optimal, and how much time will be required to obtain this information. Another approach using a predetermined number of drawn samples has been proposed. ³⁰ It practically converges to an optimal solution and reports failure of finding solution when there is no more unvisited samples. However, the main contribution of this approach is not handling the narrow corridor problem and using lower bounds on cost. Recently, variants are proposed to handle these problems. ^24,31

Confidence random tree

In this section, the CRT algorithm will be discussed in terms of the sampling and rejection methods, which are designed to generate proximate results of the precise version of the CRT described in Figure 2. To ensure these methods are flexible enough to accommodate a variety of environments, the confidence is defined as a normalized clearance function

$q\mathrm{.}Confidence=\text{min}\left(\frac{clearance(q)}{c_{\text{max}}}\mathrm{,1}\right)$

1

where q is a state in the configuration space and $c_{\text{max}}$ is the maximum clearance parameter. Then, the tree edge distance d between q and its children q′ is defined as

$d=q\mathrm{.}Confidence\times c_{\text{max}}$

2

Figure 2.

Diagram showing the operation of the precise version of the CRT algorithm. In this case, the algorithm was implemented with a continuous-time interval, unlimited number of samples, and no rejection method. When this diagram is compared to the one in Figure 1, the results are similar. (a) t = 10; (b) t = 20; (c) t = 30; (d) t = 40. CRT: confidence random tree.

The strategy of the sampling and rejection methods can be varied based on d. For example, in a low confidence area, the sampling algorithm generates high-density samples with a shorter edge length. In contrast, in an open space, the samples have a maximum confidence value of 1, and the corresponding tree has the maximum edge length.

Now, assume there is a feasible path $p=\{q_{\text{start}},\cdots ,q_{\text{goal}}\}$ as a state sequence from the initial state $q_{\text{start}}$ to the goal state $q_{\text{goal}}$ . Then, the normalized maximum confidence path may be defined as

$p*=\underset{p\in \mathcal{P}}{\text{argmax}}\left(\frac{{\displaystyle \sum }q\mathrm{.}Confidence}{t}\right)$

3

where $\mathcal{P}$ is a set of feasible paths and t is the number of states in p. While this equation can be used to maximize the safety objective, it may produce an unnecessary detour as there is no limit on t. In addition, the solution path p* may contain unnecessary states in higher confidence areas. To prevent this, t should be constrained. To accomplish this, the CRT method locates a minimum t while $\mathcal{P}\ne \varnothing$ . In other words, the solution path length is minimized into $c_{\text{max}}\cdot t$ in the worst case because the tree edge distance is defined by equation (2). A solution p* can then be found that respects the path length and safety objectives. Briefly, the CRT method divides the search for a near optimal path into two parts:

• find the minimum number of path states t, and

• find the maximum confidence path that consists of t states.

The first part allows the CRT method to terminate immediately following the first iteration in which feasible paths are found, instead of generating excess samples. This is comparable to other path planners that rely on the run time and number of samples as terminal conditions. In the second part, dynamic programming ³² is employed. Dynamic programming partitions a complex problem into subproblems and then solves the subproblems just once. The solved subproblems are then saved and reused to avoid the need for recomputation. In the CRT approach, while a tree graph is generated by each iteration, child samples are optimized to have the maximum confidence as a subproblem. The optimized child samples then become parent samples in the next iteration. This process is discussed in detail in the context of equation (4) in the section titled “Rejection method.”

Main algorithm

The structure of the CRT algorithm is shown in Algorithm 1. Beginning from the initial state $q_{\text{start}}$ , a tree structure is constructed using the SAMPLING and REJECTION methods. When a sample reaches the goal state, the algorithm stops. If there are no samples in ${\mathcal{X}}_{\text{open}}$ for generating the next samples, the algorithm recognizes failure and terminates instead of generating excess samples. This is in contrast to the behavior of other sampling-based planners. Usually, other planners continue attempting to find a solution until their terminal conditions are satisfied, even if no solutions exist. On the other hand, the CRT algorithm identifies an explorable space by inspecting the remaining open set ${\mathcal{X}}_{\text{open}}$ . This allows the proposed algorithm to be practically applied in real applications.

Algorithm 1.

Confidence random tree.

Sampling method

An overview of the sampling method is shown as Algorithm 2. In an SE(2) space that consists of $(x,y,\theta )$ , this method first locates the best pose of q using FIND_BEST_POSE, which identifies samples with the maximum confidence by generating random poses without changing the location. Then, the sampler generates n samples d far from q in which d is defined as per equation (2) and can be considered as the weighted distance $c_{\text{max}}$ by $q\mathrm{.}Confidence$ (Figure 3). This strategy ensures that samples are placed with various densities. Samples that have a smaller confidence than $c_{\text{min}}$ are removed from the valid sample set, because they are regarded as inefficient when constructing the tree graph. After generating samples for all $q\in {\mathcal{X}}_{\text{open}}$ , the algorithm returns the new sample set ${\mathcal{X}}_{\text{new}}$ as the result.

Algorithm 2.

Sampling method.

Figure 3.

Depiction of the sampling method. In this example, the algorithm selected eight samples from q; however, two samples (empty small yellow circles) were dropped because they had a lower confidence than $c_{\text{min}}$ .

Rejection method

The rejection method is an important part of the CRT algorithm, as it not only is used to maintain the adjusted sample distribution, it is also employed to find suboptimal samples via dynamic programming. Assume the CRT algorithm has a large n in Algorithm 2. From the initial state, the algorithm generates n samples in the first iteration. Without a rejection method, the number of samples would exponentially increase as $n^t$ at the t th iteration and cause the corresponding computation time to be excessive in real applications. Thus, the rejection method prevents the revisiting of previously explored areas and accepts only a few suboptimal samples.

To explain the use of suboptimal samples, consider that the sampling method generates a set of samples ${\mathcal{X}}_{\text{new}}$ containing $q,q^{\prime }$ samples from $q_{\text{start}}$ $\{q_a,q_b,q_c,q_d\}$ , as shown in Figure 4. In this example, $q_b$ cannot be a goal state at this iteration because the CRT algorithm would have terminated at the previous iteration while finding a solution path $p*=\{q_{\text{start}},q_b\}$ . If $q_c$ is a goal state, the feasible paths are $p^{\prime }=\{q_{\text{start}},q^{\prime },q_c\}$ and $p=\{q_{\text{start}},q,q_c\}$ . In this case, the CRT method should identify p as a solution path, because it has a higher path confidence than p′. Now, let $q_c$ be anywhere in the yellow check pattern area in Figure 4. If this is the case, the solution path that includes $q_c$ as a goal state must also contain q, because that state has a higher confidence. In this case, we define q as a suboptimum q* for $q_c$ . Then, the solution path can be expressed as $p_3^{*}=\{q_{\text{start}},q*,q_c\}$ , where $p_3^{*}$ indicates a solution path consisting of three states. By recursion, another solution path to $q_d$ can be identified by considering q as $q_{\text{start}}$ and generating new samples from q. If a suboptimal sample q* for $q_d$ is found in the new generated samples, the solution path to $q_d$ is $p_4^{*}=\{p_2^{*},q^{*},q_d\}$ , where $p_2^{*}=\{q_{\text{start}},q\}$ . In this manner, a general solution path can be identified that contains t states to $q_{\text{goal}}$ , as follows

$p_t^{*}=\{p_{t-2}^{*},q^{*},q_{\text{goal}}\}$

4

in which $t>2$ . When $t=2$ , the solution path is $\{q_{\text{start}},q_{\text{goal}}\}$ . This equation demonstrates that the CRT method is able to identify the solution path using dynamic programming. At each iteration, the CRT algorithm will find q* from the previous result to next goal. Next, the non-suboptimal samples q′ are rejected because if the samples q′ from $q_{\text{start}}$ are in the circle of q (in other words, the q′ are on the red dotted line in Figure 4), then they are defined as non-suboptimal for $q_c$ in the yellow check pattern area. This process iterates for all samples in ${\mathcal{X}}_{\text{new}}$ in order of confidence, and the remaining samples are expressed as ${\mathcal{X}}_{\text{accept}}$ .

Figure 4.

Description of suboptimal states. When q has the highest confidence in ${\mathcal{X}}_{\text{new}}$ and $q_c$ is in the yellow check pattern area, a solution path p* from $q_{\text{start}}$ to $q_c$ is always $\{q_{\text{start}},q,q_c\}$ even if $\{q_{\text{start}},q^{\prime },q_c\}$ is a feasible path, as per the section titled “Rejection method.”

The entire rejection process is described in Algorithm 3. First, the samples ${\mathcal{X}}_{\text{new}}$ are sorted in order of confidence by SORT_CONFIDENCE. Then, the samples in previously explored areas are rejected as described in lines 7–12 of the algorithm. This is performed by finding ${\mathcal{X}}_{\text{close}}$ , which includes all parents. The search for q* proceeds as shown in lines 14–18. After each iteration in Algorithm 3, q is always the largest sample in ${\mathcal{X}}_{\text{new}}$ as a result of the sorting process. After rejecting the values of q′ in the circle around q, q is moved from ${\mathcal{X}}_{\text{new}}$ to ${\mathcal{X}}_{\text{accept}}$ by line 19. The result of the rejection method for the example is shown in Figure 5.

Algorithm 3.

Rejection method.

Figure 5.

Depiction of the rejection method. This process starts with the sample with the highest confidence $q={\mathcal{X}}_{\text{new}}\mathrm{.}BACK$ . If the distance between q and q′ is smaller than d, then q′ is rejected.

The rejection method as described is a key element of the CRT method as it efficiently prevents an exponential increase in the number of samples while still accepting suboptimal samples. However, it results in errors in the case of $q_a$ , which is in the circle around q′ but is not in the circle around q, as shown in Figure 4. A solution path to $q_a$ should include q′ as a suboptimal sample and will be $p_3^{*}=\{q_{\text{start}},q^{\prime },q_a\}$ . However, due to the non-suboptimal assumption, the generated solution path will be $p_4^{*}$ . In rare cases, this may cause errors in small areas and may result in a slightly longer solution path.

Analysis

The proposed planning algorithm is comparable to other sampling-based planners and locates a near-optimal solution via dynamic programming and non-parametric terminal conditions. In addition, the algorithm can accommodate narrow corridors, which is not the case for sampling-based planners. This is possible because the sampling and rejection methods increase the sample density in narrow spaces.

The term n is a key parameter in the CRT algorithm. The possibility of exponential growth in $n^t$ is eliminated because the rejection method ensures that the samples increase linearly. When n becomes too large, the samples are placed in a hexagonal formation using only six neighbors in open spaces, as shown in Figure 6. This control of the distribution allows the CRT algorithm to identify a near optimal solution path in a reasonable run time. However, the rejection method means that finding an optimal path is not guaranteed even if unlimited time is provided. This is the main difference with other sampling-based path planners that guarantee probabilistic completeness. The proposed algorithm locates a near optimal solution in a reasonable amount of time instead of the unpredictable run time required to find an optimal solution asymptotically. The efficiency of the proposed algorithm will be discussed in the next section.

Figure 6.

Depiction of the hexagonal formation of samples. A number of neighbor samples cannot be larger than six even if n becomes too large.

Experiments

The CRT algorithm was implemented using the open motion planning library (OMPL), ³³ which includes many previously implemented sampling-based planners. Because the CRT algorithm uses two objectives, the other algorithms used for comparison should optimize a similar number of objectives. Four algorithms that can support similar objective functions were tested, namely, the BIT*, ³⁴ PRM, ⁷ PRM*, ³⁵ and RRT* ³⁵ algorithms. Then, a cost function $cost=20/Clearance+Pathlength$ was defined for these algorithms. The four environments used to test the planners are shown in Figure 7. All algorithms were tested in an SE(2) space with a $(x,y,\theta )$ configuration and the default parameters from the OMPL. The parameters of the CRT algorithm were set for each environment to be $c_{\text{max}}=10$ , $c_{\text{min}}=1$ , and $n=24$ . In the first experiment, the run for each planner was terminated when the first solution was found to determine the run time to find a solution path. In the second experiment, the run time was used as the terminal condition for all algorithms except the CRT to measure the optimization performance. The resulting solution paths were evaluated based on the normalized clearance, path length, run time, and success rate. The normalized clearance measurement was computed using equation (3). It was noted that in the CRT results, the variances were much smaller than those of the other algorithms. If unlimited time was allowed for the other sampling-based planners, only one optimal solution was generated in each iteration. In this case, the variances of the measurements were essentially zero, which implies that the paths in the CRT results robustly converged to a near optimal point despite completing in a reasonable amount of time. The quantitative measurement of these comparison is shown in Figure 8.

Figure 7.

Tested environments are displayed. The presented solution paths in each images are generated by CRT: (a) map 1 (maze); (b) map 2 (unique solution maze); (c) map 3 (bug trap); (d) map 4 (random polygon). CRT: confidence random tree.

Figure 8.

Results of the tested planners in four environments. The standard deviations are displayed as black bars. The left-hand column shows the results of the experiment in which finding a first solution was used as the terminal conditions, and the right-hand column shows the results when the run time were used as the terminal conditions.

Finding a first solution terminal condition

In this experiment, all planners terminated immediately when a solution path was found. This was repeated 20 times in each environment. However, the RRT* algorithm in the map 2 environment failed to generate a solution path in a run time of $1000\hspace{0.17em}\text{s}$ in any trial. Thus, the corresponding results are not included for this algorithm for this experiment.

The normalized clearance results of the CRT method were higher than those of the others in all environments while the path length results were either similar or shorter than those of the others. This is significant because it demonstrates that the CRT method was able to successfully accommodate the path length and clearance objectives. The run time results for the CRT method were impressive, especially for maps 1, 2, and 3. The run times had not only a shorter mean value but a smaller standard deviation as well. This was due to the ability to navigate narrow corridors. Thus, it can be said that the CRT algorithm effectively solved the narrow corridor problem in the tested environments.

Time limited terminal condition

Usually sampling-based planners that optimize the cost function generate more than one feasible path. For this reason, in the second experiment, the compared planners were found to have a 10 times longer run time than the CRT method. As was the case in the previous test, the RRT* algorithm was unable to generate a solution path for map 2 and generated only one solution among 20 trials for maps 1 and 3.

The path length and clearance results for all tested algorithms were similar. For map 3, the other planners had better results in terms of the normalized clearance with similar path lengths. This was due to the sampling strategy of the CRT method. In narrow spaces, such as the corridor of the bug trap, the CRT algorithm generates more samples, as shown in Figure 7(c). This decreases the normalized clearance measurement, because the summation of the clearances is divided by the number of path states. In contrast, the other planners had a high normalized clearance, because they generated only a few samples in the corridor of the bug trap. In terms of the success rate, the results from the CRT method were remarkable. Despite being given 10 times more time than the CRT method, the other planners had low success rates for maps 2 and 3. This demonstrates that the proposed CRT method is robust in environments with narrow corridors.

Conclusions and future work

In this article, we proposed a novel CRT-based sampling path planner that considers path length and safety objectives. The CRT method defines an objective function (equation (3)) and identifies p* in two steps: finding the minimal t and maximizing the confidence in the path. By minimizing t, the solution path length is minimized such that $c_{\text{max}}\cdot t$ is maximized. Then, the maximum clearance is found via dynamic programming. The samples generated by the CRT method are distributed based on the confidence. This accommodates the narrow corridor problem as denser samples are generated in areas of low confidence. The advantage of the CRT method is that the run time is reasonable and only a few parameters are required when searching for an optimal solution path. Even if an optimal solution is not found, the algorithm is able to identify a reasonable solution and is thus well suited to real applications. The algorithm was validated by experiment, the results of which are detailed in the section titled “Experiments.” The CRT method was found to provide results comparable to other methods in terms of the run time, especially in high complexity environments containing narrow corridors.

The CRT algorithms was implemented for human robot interaction (HRI) scenario, such as a cafe or restaurant. To prevent the risk of collision and to ensure the safety of humans in the area of the robot in these environments, robots should control their speed based on the clearance. This is achieved using the confidence as a speed factor. In this way, the confidence can be used both in constructing path and in controlling the behavior of the robot. In this article, we focused on an SE(2) space, which consists of $(x,y,\theta )$ . This is complex enough for mobile robot navigation but is not in the domain of sampling-based path planners. In the future, we plan to extend the CRT algorithm to the SE(3) space by employing a sphere instead of a circle. When this is implemented, it would position the CRT algorithm as a suitable path planner for unmanned aerial vehicles in which there are stringent requirements on the safety and run time.