Abstract
Probabilistic methods have been proven to be effective for robotic path planning in a geometrically complex environment. In this paper, we propose a novel approach, which utilizes a specialized roadmap expansion phase, to improve lazy probabilistic path planning. This expansion phase analyses roadmap connectivity information to bias sampling towards objects in the workspace that have not yet been navigated by the robot. A new method to reduce the number of samples required to navigate narrow passages is also proposed and tested. Experimental results show that the new algorithm is more efficient than the traditional path planning methodologies. It was able to generate solutions for a variety of path planning problems faster, using fewer samples to arrive at a valid solution.
1. Introduction
The general path planning problem is given as; “planning a collision free path for a robot made of an arbitrary number of polyhedral bodies among an arbitrary number of polyhedral obstacles, between two collision free positions of the robot” [1]. Path planning methods developed during earlier years approached this problem by generating explicit representations of the robots surrounding environment, so that it can be navigated via the use of mathematical algorithms. However, as path planning applications became more complex, these planners began to struggle with the amount of data required to process a valid solution.
A major breakthrough in the field of path planning came with the development of probabilistic path planning methods, such as the Probabilistic Roadmap (PRM) planner [2]. These planners utilize a randomized sampling based approach, which builds a simplified model of the robot's free space. This removes the high computational load involved in calculating an explicit representation of the environment. The random nature of sampling the environment ensures that probabilistic planners have a quality known as probabilistic completeness: if a solution is possible the planner will find it, provided that the time frame is not finite. Probabilistic methods, however, suffer from the fact that they cannot explicitly recognize if a solution will be geometrically impossible.
Probabilistic planners have been utilized extensively since their development and have been applied to a variety of different applications. The research presented in this paper focuses on the development of new, more effective, sampling methods which are implemented on a traditional lazy probabilistic path planner [3]. This is done through an iterative process of roadmap analysis and enhancement that intelligently guides the construction of the network graph. The overall result is a planner with a similar operation to traditional methods, but that is able to solve path planning queries in complex environments more quickly.
2. Background and Literature Review
Probabilistic methods generally operate on the premise of a robot and its associated configuration space (C space ). Loosely termed, C space is the group of all kinematically feasible configurations of a robot about its workspace. For path planning applications, it is common to separate C space into two distinct subsets; C free and C forbid . C free represents the free configuration space of the robot: a group of configurations that do not clash with the surrounding environment. Conversely C forbid , the forbidden configuration space, represents a collection of robot configurations that are generally speaking, probabilistic methods try to capture a simplified representation of the robot's C free space. Once the planner has a sufficient understanding of the regions the robot can move about in without clashing, a series of clash free motions to solve a given path planning query can be generated.
An important tool utilized by a variety of probabilistic planners is an undirected network graph, commonly referred to as the Roadmap (
A network, or roadmap, graph (Ģ) represents the C free space of a robots environment.
PRM planners were an early development in probabilistic methods that made effective use of the roadmap graph structure. PRM planners remain popular even today. They are effective for use in complex problems, whilst remaining relatively simple and robust in their operation. PRM planners operate in two distinct phases; the construction of
The probabilistic concepts and network structure used in PRM planners provide an effective framework to solve complex path planning problems. However much research has been devoted to improving the details of how these roadmap style planners operate. Observations of the PRM planners operation noted that the majority of computation time is devoted to clash checking robot configurations. In order to reduce the number of clash checks required to generate a solution, many heuristic sampling techniques were developed to replace basic random sampling strategies. Many of these heuristic sampling methods attempt to generate a portion of samples nearby the boundary of clash objects in the workspace. This was done explicitly using geometric information [7] or by using paired samples of specific clash criteria [8]. Other attempts involve partitioning the workspace in order to categorize the different regions in terms of their complexity [9, 10]. If the sampler knows the ‘difficult’ regions, it is able to bias its sampling to these spaces, in order to more effectively utilize computational resources. Another focus of research centres on probabilistic methods' weakness in sampling narrow passages in C
space
. Random sampling schemes employed by traditional PRM planners greatly reduce the likelihood of sampling multiple configurations inside these narrow passages. An effective method of addressing this is the bridge test, as shown in Figure 2 [11]. The bridge test will sample a pair of configurations. If these both fail, the midpoint is tested. If this midpoint is C
free
, only then is it inserted into
Bridge test criteria.
The ‘Lazy-PRM’ planner [3] provided a simple and robust method for reducing the number of clash checks to solve a given query. From a high level standpoint, the Lazy-PRM algorithm operates in a similar fashion to its predecessor, the traditional PRM. However a significant difference in operation comes from applying a delayed, or ‘lazy’, evaluation of clash. The algorithm initially constructs a kinematically sound roadmap, which is assumed to be entirely clash free. Then, repeatedly, the shortest available path through the roadmap is sent to the clash checker for evaluation. If a clash is detected on the path, the offending element (node/edge) is removed from
The PRM algorithm spends a large portion of its calculation time pre-processing the roadmap, making it more suited for applications in which a road map can be queried multiple times. Tree based algorithms were developed for applications that needed a solution generated rapidly for single use. [12] developed a notable variant of this style of planner, which grows its
Recent research focuses on developing planners for specific or complex applications, such as for use on robots with non-holonomic constraints [14], hyper redundant manipulators [6], robots operating in dynamic environments [4], or to generate paths that are more optimal [15]. It is important to note that recent research focuses predominantly on the modification of some component of the probabilistic methodology, rather than on entirely new planning methods; a testament to the reliability and power of probabilistic methods in general.
3. Lazy Significant Edge Algorithm (LSEA)
This section is devoted to describing a new method of lazy path planning. The method offers improvement over the traditional Lazy-PRM planner by reducing the amount of clash checks required to arrive at a solution and is also more effective at navigating difficult regions of C space .
3.1. Algorithm Overview
LSEA, like many other path planners, is an iterative based planner. It constructs an initial roadmap which is then analysed and augmented in a repetitive manner until a termination criteria is met. The algorithm is terminated if a solution to the given query is found, or if no solution is found within a given timeframe. The pseudo code of LSEA is shown in Figure 3. The planner begins by generating a roadmap of kinematically viable nodes and edges, which are initially assumed to be clash free. The algorithm searches for the shortest path through
Pseudo code of LSEA operation
The expansion phase is the most important stage of this algorithm. It uses the component connectivity information of the existing roadmap to determine which regions of the workspace have not been successfully navigated by the robot. Roadmap edges which pass through un-navigated clash objects, named significant edges, are used to bias the sampling strategy. The overall effect of the expansion phase is a scheme of sampling that adds more samples to regions of C space that have not been successfully navigated, effectively reducing the amount of redundant sampling carried out. In addition to the roadmap expansion, a type of ‘lazy’ bridge test (LBT) is implemented. LBT is only activated in certain instances and uses prior clash information to reduce the computational expense of traditional bridge testing techniques. Details of the algorithms' components and their methods of operation are detailed in the following chapter sections.
3.2. Construction of Initial Roadmap
To build the initial roadmap (
3.3. Selection of shortest path and clash test
There exist many different methods of selecting the shortest path through a connected network graph, each with varying levels of complexity and performance. LSEA utilizes the
When the shortest path through the roadmap is found, each element in the path is checked for clashes. All nodes along the path are tested first, followed by the edges. If any node/edge is found to be clashing, the process is terminated immediately and the offending element is removed from
3.4. Iterative Expansion Phase
In a complex path planning problem,
3.4.1. Significant edge search.
To determine a list of significant edges, we initially collect a group of candidate edges; edges that have been removed from
Graphic overview of the LSE algorithm operation
N samp samples are placed about each significant edge via the use of a bivariate distribution [3], centred at the mid-point of the edge. The shape/size of the distribution is controlled by the length and direction of the edge. The newly distributed nodes are then connected to Ģ in the same manner as during the construction phase. After all significant edges have been sampled, N rand nodes are also distributed randomly about the roadmap, in order to ensure probabilistic completeness and help the expansion of the roadmap over future iterations. In some rare instances no significant edges are present in Ģ, which indicates that whilst the current query has failed, the general exploration of C free is progressing. In these instances, random sampling carried out as normal to re-connect the roadmap and the algorithm progresses as normal. Once the expansion phase is completed, the roadmap's connectivity is returned to a sufficient state to continue the planners operation. The expanded roadmap is passed to the shortest path search and the algorithm continues its iterations. To reduce cyclic behaviour in roadmap expansion, significant edges are not used multiple times. If a significant edge has been defined once, it is not utilized by expansion phases over future iterations.
3.5. Lazy Bridge Tests (LBT)
Bridge testing is a proven method of effectively navigating complex regions and narrow passages [11]. The bridge test operates by testing pairs of samples, within a given distance of one another, continually until both samples are found to be clashing. Once this criteria is met, the midpoint of these samples is tested and only if this configuration is C
free
is it then placed in
In order to increase the efficiency of the traditional bridge test, a lazy method of bridge testing is implemented in LSEA. It operates by first collecting all nodes in
4. Experiment and Results
4.1. Experimental Setup
Three path planning problems, each featuring varying levels of complexity, were created to test LSEA. To evaluate performance, LSEA was benchmarked against the Lazy-PRM path planner [3]. Both algorithms were run 100 times in each environment and the results were averaged. Both algorithms utilize the same set of variables that control the operation of the planner. For each environment, these variables were optimized (see Table 1.) and implemented before the testing process. Both planners made use of the component-n [16] neighbour connection strategy, which provided better results for both planners in each of the given environments. The effectiveness of lazy bridge testing techniques was carried out separately. In the medium-scatter environment, LBT was compared to traditional bridge testing as a means to effectively sample inside narrow passages. All algorithms and environments were developed and tested in the MATLAB programming language.
Variables used for each of the three environments (sparse, medium, dense) for both LSEA and Lazy-PRM during testing.
Each of the three environments is a 2D plane scattered with randomly distributed objects to navigate. The robot is a planar robot with three degrees of freedom (x, y and rotation Rz). The sparse-scatter environment (Figure 5. detail i) features oddly shaped objects, sparsely distributed about the configuration space. The robot is relatively free to move about this environment and the robots orientation is not critical in order to pass between many of the objects. The second environment, medium-scatter, has more objects dispersed throughout. Their positions create several narrow passages the robot has to navigate in order to solve the given path planning query. The final environment has many smaller clash objects densely-scattered at random. The density of the clash objects drastically restricts the robot's motion and the orientation of the robot is critical in order to navigate through many sections of the workspace.
The testing environments, the robot is shown in the bottom left corner, and the goal is the black star in the top right. Detail: i) Sparse-Scatter ii) Medium-Scatter iii) Dense-Scatter
4.2 Results
4.2.1. Results of LSEA
The box plots in Figure 6 display the times taken per query for both planners. The whiskers encapsulate the entire spread of the data, the shaded box represents the lower quartile and the lighter shaded box above shows the upper quartile of the data. The boundary between these boxes represent the median time taken to complete the tests. Using the box plots, we can see that LSEA outperforms the traditional lazy planner in every environment. The improvement was smaller in the sparse scatter environment, but became more apparent in the medium and densely scattered environments.
Box plots of results. i) Sparse-scatter ii) Medium-scatter and iii) Dense-scatter.
In the sparsely-scattered environment LSEA provided some degree of improvement. It was observed that the sparseness of the obstacles meant that both planners were able to solve the majority of the problems with the initial roadmap alone, few expansion iterations were required. Both algorithms use the same methods for generating
Tabulated results for each environment: i) Sparse-scatter ii) Medium-Scatter iii) Dense-Scatter
4.2.2. Results of LBT
To test the effectiveness of Lazy Bridge testing, another testing scheme was created using the medium-scatter environment. This environment was used as it features scattered obstacles, as well as narrow passages which need to be navigated, providing a good environment to test the overall performance of LBT. To gain both an absolute and relative understanding of LBT effectiveness, two tests were created. In the first, the medium-scatter environment was used to test LSEA with and without LBT. LBT was found to improve the time taken to solve the query by 33.1%. When the planner is using LBT, fewer samples are required, because they are more effectively used during the process. The second test compared the effectiveness of LBT against traditional bridge testing techniques. Once again, the lazy method showed improved performance over the traditional method. A 17.8% reduction in time was observed and on average, 2934 fewer clash checks were required to reach a solution. These results can be attributed to the fact that LBT utilizes previously calculated clash results, reducing the overall amount of clash tests required to generate valid bridge samples. Granted, LBT has to substitute many more distance checks between nodes. However, the relative cost of these distance checks is small compared to the cost of the extra clash checks required when carrying out the traditional bridge testing. If LBT was employed on a more complex system, which requires a longer time to check the clash status of samples, a greater reduction in time would be observed.
5. Discussion
LSEA was developed as an improvement over the traditional Lazy-PRM planner [3]. From a high level standpoint, both algorithms operate in a similar fashion. The main operational difference between LSEA and Lazy-PRM comes from the roadmap expansion phase. Lazy-PRM carries out its roadmap expansion by adding additional samples to regions deemed likely to be at the boundary of clash objects. LSEA takes this expansion method a step further by adding its samples to regions nearby clash objects that have not yet been navigated by the robot. By doing this a solution can be discovered faster, as no time is wasted by continuing to sample about a clash object that has already been cleared.
The trade off to this approach is that whilst a solution can be found faster, there is the possibility that the optimality of the returned path may not be as good. If, for instance, the LSEA planner navigates a certain obstacle poorly, the likelihood of adding more samples to this region and improving this local path over future iterations is low. Whereas, if the Lazy-PRM navigates a certain object poorly, there is a greater chance that future iterations of the algorithm will continue to place samples about this object, which could improve the local path. This effect can be reduced considerably via post process optimization of the path solution.
The observed performance advantage LSEA has over Lazy-PRM is directly attributed to the reduced amount of clash checks required to solve a path planning query. In the 2D test environments used, these clash checks are relatively simple and can be carried out very rapidly (~0.0001 sec). If LSEA planner was implemented in a more complex system that requires a much longer time to process a clash check, we can expect the performance advantage of LSEA to be even higher.
6. Conclusions and Future Work
In this paper, a new path planning algorithm that utilizes connectivity information to bias the expansion of the roadmap was presented. The algorithm was tested in three environments featuring different levels of complexity. Its performance was benchmarked against a traditional Lazy-PRM planner. In each test LSEA outperformed its counterpart considerably, despite having a similar method of operation. It was able to solve path planning queries faster, utilizing fewer samples to achieve its solution. In more difficult environments, it was able to process a valid solution twice as fast as its traditional counterpart. A lazy method of bridge testing was also proposed and tested. The lazy method was observed to sample narrow passages more effectively, utilizing fewer clash checks in order to generate valid bridge samples.
Future work regarding LSEA involves adapting its operation to plan paths for 6DOF articulated manipulators operating in a 3D environment. In such a system, clash checking algorithms place an even larger burden on computational resources. It is envisaged that when LSEA is implemented on such a system, the reduction in required clash checks will further increase the performance advantage LSEA has over traditional Lazy-PRM methods. The LSEA algorithm is still in early stages of development. Many performance modifications are being investigated and further tuning of the algorithm for increased performance is to be carried out
Footnotes
7. Acknowledgments
This work was supported by the Defence Materials Technology Centre (DMTC), which was established and is supported by the Australian Government's Defence Future Capability Technology Centre (DFCTC) initiative.