J Intell Robot Syst DOI 10.1007/s10846-013-9861-3

K-Order Surrounding Roadmaps Path Planner for Robot Path Planning Yueqiao Li · Dayou Li · Carsten Maple · Yong Yue · John Oyekan

Received: 20 May 2011 / Accepted: 4 July 2013 © Springer Science+Business Media Dordrecht 2013

Abstract Probabilistic roadmaps are commonly used in robot path planning. Most samplingbased path planners often produce poor-quality roadmaps as they focus on improving the speed of constructing roadmaps without paying much attention to the quality. Poor-quality roadmaps can cause problems such as poor-quality paths, time-consuming path searching and failures in the searching. This paper presents a K-order surrounding roadmap (KSR) path planner which constructs a roadmap in an incremental manner. The planner creates a tree while answering a query, selects the part of the tree according to quality measures and adds the part to an existing roadmap which is obtained in the same way when answering the previous queries. The KSR path planner is able to construct high-quality roadmaps

Y. Li · D. Li (B) · C. Maple · Y. Yue · J. Oyekan Department of Computer Science and Technology, University of Bedfordshire, Park Square, Luton, LU1 3JU, UK e-mail: [email protected] Y. Li e-mail: [email protected] C. Maple e-mail: [email protected] Y. Yue e-mail: [email protected] J. Oyekan e-mail: [email protected]

in terms of good coverage, high connectivity, provision of alternative paths and small size. Comparison between the KSR path planner and Reconfigurable Random Forest (RRF), an existing incremental path planner, as well as traditional probabilistic roadmap (PRM) path planner shows that the roadmaps constructed using the KSR path planner have higher quality that those that are built by the other planners. Keywords Robot path planning · High-quality roadmaps · Sample-based path planning

1 Introduction Probabilistic sampling-based path planners are often used in robot path planning. These include Probabilistic roadmap methods (PRMs) [7], Tree based methods such as the Rapid-exploring Random Tree (RRT) algorithm [10], and incremental learning methods such as the Reconfigurable Random Forest (RRF) algorithm. Probabilistic roadmap methods (PRMs) were introduced by [7] and have been further developed in recent years [1, 2, 4, 13]. Given an environment, the planners first produce a roadmap by randomly sampling the environment and connect the sample configurations with lines if no obstacle is lying between the configurations. Then, they search for the shortest

J Intell Robot Syst

paths given initial and goal configuration pairs based on the roadmap, known as answering path queries. The construction of a roadmap is often time-consuming. Hence, roadmap construction has mainly focused on reducing time needed but not on the quality of roadmaps. Poor-quality roadmaps directly affect answering path queries. The paths produced may not be the shortest. The searching processes can be lengthy. The planners may even fail to find paths. Tree-based path planners [10] belong to another category of probabilistic sampling-based method. Given a path query, the planners guide two trees growing from the initial and the goal configurations respectively and towards to each other. A path is obtained when two branches, each from one of the two trees, joint together. Although growing two trees needs much less time than constructing a roadmap, as the trees will be discarded after answering the given query, the planners can only answer single path query each time. In [6] however, the researchers carried out a series of experiments to analyse the trade-off between maintaining a dynamic PRM and applying a tree-based planner to answer many path queries within changing environments. Their experiments showed that those maintaining a dynamic PRM were faster and more capable of planning difficult motions than tree-based methods in a changing environment [6]. An alternative way is to allow roadmaps to be constructed incrementally while answering path queries [11]. This method first grows trees while answering queries and retains some parts of the obtained trees to form a roadmap. It then employs the roadmap to answer new queries. In cases where the new queries cannot be answered by the roadmap, the method grows new trees and adds some parts of the new tree to the existing roadmap so that the roadmap is incrementally developed. This method overcomes the problems of both probabilistic sampling-based path planning and the tree-based path planning methods. First, it does not need a length pre-processing stage for which the probabilistic sampling-based planners require to develop roadmaps before actually answer any query. Secondly, it keeps some parts of trees for answering multiple queries. In addition to the advantage, this method provides the planner with opportunities of adding in quality

considerations when incrementally constructing a roadmap. The K-order surrounding roadmap (KSR) presented in this paper takes the opportunities to ensure the high quality of roadmaps in the way of using quality measures to decide which parts of newly growing up trees need to be retained and merged to an existing roadmap. Therefore, it is able to construct high-quality roadmaps. First, a KSR planner allows information about regions where vertices of an existing roadmap reside to be stored in the corresponding vertices. It then employs a vertex category classifier to classify all existing vertices into a number of categories according to the region information. According to the classification, the planner prunes the existing roadmap and adds useful circles using suitable strategies that are developed by taking into account of quality measures. This paper is organised as follows: Section 2 reviews the criteria of high quality roadmaps. Section 3 introduces KSR construction concept. The vertex category classifier is presented in Section 3.2. Section 4 explains the KSR path planner algorithm. The experiment results, analysis and results of comparisons with Reconfigurable Random Forest (RRF) are discussed in Section 5. Finally, conclusion and future work are presented in Section 6.

2 High Quality Roadmap Criteria Given a free configuration space, C f ree , a number of different roadmaps can be constructed. The quality of roadmaps has a direct impact on sampling-based path planning. Roadmap quality refers to the usefulness of roadmaps in the sense of answering path queries and the costs of constructing the roadmaps. Four criteria, namely, coverage, connectivity, useful circles and roadmap size, have been employed to evaluate the quality of roadmaps. 2.1 Coverage The prerequisite of making use of a roadmap to answer a path query Q(qi , qg ) is that qi and qg can be connected to the roadmap directly

J Intell Robot Syst

without colliding into any obstacle. The existence of a collision-free connection from qi or qg to the roadmap depends on whether qi or qg is in the visibility domain [13] of the roadmap. The area of the visibility domain of a roadmap is called the covered C f ree area of the roadmap. The percentage of covered C f ree by a roadmap in the C f ree can be measured by the C f ree coverage ratio. Definition 1 (C f ree coverage ratio): C f ree coverage ratio (CCR) is the ratio of the volume of the covered C f ree area to the volume of the general C f ree area. A CCR can take a value in the range [0,1]. The higher the CCR is, the greater the area of a C f ree that is covered by the roadmap R. If a roadmap fully covers the C f ree , the CCR takes the value of 1. This is called total coverage [2]. Total coverage of a roadmap ensures that each configuration q such that q ∈ C f ree can be connected to at least one vertex of the roadmap without colliding into any obstacle. With a higher value of CCR, any given pair of connectable qi and qg is much more likely to be connected to the roadmap. Therefore, CCR has been used as a measure of roadmap quality. 2.2 Connectivity If there is an answerable path query Q(qi , qg ) and qi and qg are in the visibility domain of a roadmap,

the success of finding a path in the roadmap depends on the connectivity of the roadmap. If a roadmap representing a connected C f ree is a connected roadmap as illustrated in Fig. 1a, then the roadmap is considered to achieve maximal connectivity [2]. If a roadmap uses several miniroadmaps to represent a dis-connectedC f ree , as illustrated in Fig. 1b, and each mini-roadmap is a connected roadmap, then this roadmap achieves maximal connectivity. Maximal connectivity of a roadmap ensures a path in the roadmap for a answerable path query Q(qi , qg ), if qi and qg are in the visibility domain of the roadmap. On the other hand, a roadmap with poor connectivity is not useful when answering path queries even though the roadmap achieves total coverage. The number of mini-roadmaps used to represent the same connected C f ree can be employed to measure the connectivity of roadmaps. A roadmap with a smaller number of miniroadmaps representing a connected C f ree gains better connectivity than the roadmap with a larger number of mini-roadmaps to represent the same connected C f ree . 2.3 Useful Circles A high quality roadmap is expected to provide path planners with alternative paths for them to determine the effective (e.g. fewer detours) paths when answering queries. Paths connecting the same two configurations form circles in roadmaps. There are three different types of useful circles,

Fig. 1 Roadmaps achieving maximal connectivity

(a) A maximally connected roadmap representing a connected Cfree

(b) A maximally connected roadmap representing a disconnected Cfree

J Intell Robot Syst

namely, un-reducible circles, un-convertible circles and non-f irst-order deformation circles [5, 8, 12]. Homotopy preserving probabilistic roadmaps (HPPRs) [12] are those that are able to provide an exhaustive list of all such paths that they are not in the same homotopic class. Although an HPPR planner is able to construct roadmaps that contain only un-reducible circles, it is time-consuming to obtain the visibility graph of a configuration and check reducible paths that can be eliminated. Therefore, HPPR planners are only practically applicable to solve path planning problems in 2-D spaces. In Kamphuis et al. [8] a method of adding useful edges to a roadmap in order to construct unconvertible circles was presented. A parameter,T, is defined to measure the usefulness of an edge. The optimal value of T needs to be obtained from experiments before path planning. If an edge is Kuseful and K > T, the edge is added the roadmap to form a circle. However, there is no systematic way to search for the optimal value for T apart from experiments. Path deformation roadmaps (PDRs) [5] are able to capture representative paths of different homotopy classes and also contain additional circles constructed in the same homotopy class that cannot be easily deformed into each other. PDR planners present the exact method to measure the usefulness of a circle by introducing a new concept called K-order deformation to characterise particular subsets of paths in the same homotopic class. A PDR planner considers non-first-order deformation circles as useful circles. PDR planners are able to solve path planning problems in 3-D work spaces. They do not require a large number of experiments to determine the values of parameters as they employed an explicit measurement to evaluate the usefulness of edges.

connectivity and a high CCR. Useful circles can also introduce extra vertices.

3 K-order Surrounding Roadmaps 3.1 Surrounding Roadmaps Definition 2 (2-D surrounding roadmap): let O be a set of obstacles in a 2-D work space. A roadmap R = (V, E) is a 2-D surrounding roadmap (SR) if: • •

There exists one and only one non-first-order deformation circle in R to surround Osub for every obstacle Osub ⊆ O, For any two connectable configurations v1 and v2 (v1 , v2 ∈ V) such that v1 and v2 are in different non-first-different circles surrounding two different obstacle subsets, there exists a path in R to connect them.

Definition 3 (3-D surrounding roadmap): let O be a set of obstacles in a 3-D work space which is a connected area. A roadmap R = (V, E) representing a connectable C f ree is constructed by adding paths to connect two vertices v1 andv2 , such that v1 , v2 ∈ V, in order to construct a non-first-order deformation circle to surround an obstacle subset Osub such that Osub ⊆ O. The roadmap constructed using this method is called a 3-D surrounding roadmap. Definition 4 (K-order 3-D surrounding roadmap): let O be a set of obstacles in a 3-D work space. A 3-D surrounding roadmap R = (V, E) is called a K-order 3-D surrounding roadmap (KSR), if there are K non-first order deformation circles constructed in R.

2.4 Roadmap Size 3.2 Vertex Classification Roadmap size is defined by the number of vertices in a roadmap [7]. A smaller roadmap often implies shorter time in searching for a path and also requires less memory to store. Roadmap size sometimes contradicts with CCR, connectivity and the number of useful circles. With a small number of vertices, a roadmap can hardly have the maximal

In an SR, information about regions where the vertices of the roadmap reside is collected and stored in the vertices. This region information records nearby obstacle and narrow passage situations. This information is quality-related as it implies CCR, connectivity, as well as the

J Intell Robot Syst

freeDis, it is a cluttered vertex, such as vertex ve in Fig. 2. The regions near cluttered vertices are relatively complex, compared with the region near other types of vertices.

requirement for useful circles and roadmap size in the region. According to the region information, vertices of an SR can be classified into the following four categories: (a) Free vertex: if a vertex v in a free region where the distance to the nearest obstacle from v is larger than freeDis, then the vertex is call a free vertex, such as vertices va and vb in Fig. 2. freeDis is a user-defined radius specifying a circular area centred by the free vertex. This area is free of obstacles. (b) Near-obstacle vertex: if the distance between a vertex v and an obstacle is equal to or shorter than freeDis, then this vertex is called a near-obstacle vertex, such as vertex vc in Fig. 2. Such vertices are in the boundary area of a free region. One part of the region surrounding a near-obstacle vertex is relatively free and the other part is close to obstacles. (c) Narrow-passage vertex: if a vertex is in a passage of which the maximal width is shorter than passageH and the length is longer passageL, then it is called a narrow-passage vertex. For example, vertex vd in Fig. 2 is a narrow-passage vertex. passageH and passageL are user-defined values according to the size of a robot and the users definition of what constitutes a narrow passage in a region. (d) Cluttered vertex: if a vertex is close to more then two obstacles in the area specified by

3.3 Collecting Region Information The region information can be collected during the process of incrementally constructing an SR using RRT_extension function [10] and RRT_connection function [9]. These two functions are used to grow Rapid-exploring Random Tree (RRT) in incremental path planning [11]. The two functions are repeatedly applied to a number of directions from a vertex when a RRT branch grows from that vertex. They return successful or failed extension/connection information that indicates the existence of obstacles in the corresponding directions. The collections of this information describe how crowded in terms of obstacles in a region that surrounds the vertex, as described in the following. (a) Failed extension ratio of a vertex, v, is defined as Fail Ext Ratio (v) =

N f ail Ext (v) N f ail Ext (v) + NsuccExt (v)

where N f ail Ext (v) is the number of failed extensions from v and NsuccExt (v) the number of successful extension from v. The non-zero

Fig. 2 Categories of roadmap vertices

A

ve nearL

C

nearH

vd

B

freeDis

E

va

vc

vb

D

freeDis

F

J Intell Robot Syst

value of FailExtRatio(v) means that vis near to obstacles. (b) Successful connection ratio of a vertex, v, is defined as SuccConRatio (v) =

NsuccCon (v) N f ailCon (v) + NsuccCon (v)

where NsuccCon (v) the number of successful connections from v and N f ailCon (v) is the number of failed connections from v. If the value of SuccConRatio(v) is relatively high, it is possible that the area surrounding v is relatively free. (c) The failed extension ratio of Sd (v) is defined as Fail Ext (Sd (v))

m

i=0 N f ail Ext (vi ) m N f ail Ext (vi ) + i=0 i=0 NsuccExt (vi )

= m

where Sd (v) be a set of vertices including a vertex v and all its neighbouring vertices within a region which is specified by a centre which is v and a radium d in an SR. If the values of both FailExtRatio(v) and FailExtRatio(Sd (v)) are zero, v must be a free vertex. Fig. 3 Roadmap vertex classifier

(d) The successful connection ratio of Sd (v) is defined as SuccConRatio (Sd (v)) m i=0 NsuccCon (vi ) m = m N succCon (vi ) + i=0 i=0 N f ailCon (vi ) If the value of SuccCon(Sd (v)) is large and the value of FailExtRatio(v) is not zero, then it is possible that v is near-obstacle vertex. (e) The free vertices ratio in Sd (v) is defined as FreeVerticesRatio (Sd (v)) =

N f ree (Sd (v)) Ntotal (Sd (v))

If a vertex v is not free vertex and FreeVerticesRation(Sd (v)) of v is not zero, the vertex is a near-obstacle vertex. On the other hand, these two functions are only applied a limited times. Hence, the region information obtained only partly reflects obstacle distribution near the vertex. Region information stored in neighbouring vertices of that vertex helps to build up a more comprehensive image about the region of the vertex. Let d be a certain value, a set of neighbouring vertices, Sd (v), of a given vertex, v, is formed with those that connect to v directly or indirectly in a roadmap within d.

J Intell Robot Syst

A decision tree was developed as a vertex classifier, as illustrated in Fig. 3.

4 K-order Surrounding Roadmap Path Planner A K-order Surrounding Roadmap (KSR) path planner was developed to construct high quality roadmaps while answering path queries based on an incremental learning path planning method. The KSR path planner algorithm records only those new vertices that are informative in terms of representing regional environment while answering queries to minimise roadmap size without reducing the coverage and the connectivity of the roadmap and without breaking useful circles in the roadmap. A roadmap vertex classifier is employed for the purpose of deciding which new vertex is informative. The retained vertices are then employed to check whether nonfirst-order circles containing no redundant paths can be constructed. Since all the useful circles in a roadmap are connected to each other if the roadmap achieves maximal connectivity, a Korder surrounding roadmap is constructed after a Fig. 4 Pseudo-code for the algorithm of function KSR_planner

certain number of path queries are answered with the trade-off between the criteria size and other criteria given in Section 2. Once the roadmap is able to answer path queries without need for extra vertices and edges and no useful circles can be constructed by employing the initial and the goal configurations of the newly raised path query, the roadmap becomes a high quality KSR. The pseudo-code for the algorithm of function KSR_planner is displayed in Fig. 4.

4.1 Answering Path Queries Function AnswerQuery in the KSR path planner is used to answer a query, Query(qi , qg ). It is able to evaluate the quality of the existing roadmap R and increase the coverage and the connectivity of R according to the evaluation result besides producing a path to answer Query (qi , qg ). Each path query is used to check the coverage and the connectivity of the existing roadmap. In cases where the initial or the goal configurations (qi or qg ) cannot be connected to the existing roadmap, more vertices will be needed to cover the area in which qi or qg reside to improve the coverage

J Intell Robot Syst

of the roadmap. In cases where no path can be found in the roadmap to connect qi or qg when both qi and qg can be connected to the roadmap without obstacle collision, once again, more vertices are needed to improve the connectivity of the roadmap. Figure 5 gives the pseudo-code for the algorithm of function AnswerQuery. The KSR path planner evaluates the quality of the existing roadmap by connecting qi and qg to every miniroadmap r in R. If both qi and qg are connected to the same r, a path is obtained and the existing roadmap is able to answer Query(qi , qg ) without

Fig. 5 Pseudo-code for the algorithm of function AnswerQuery

the requirement to increase its size. Then, the evaluation result is GOOD. Otherwise the evaluation result is POOR, which means that the planner needs to add more vertices and edges in R to increase the coverage and connectivity of the existing roadmap in order to find a path connecting qi and qg . The planner grows two trees Ti from qi and Tg from qg and adds Ti and Tg into R. If qi and qg cannot be connected to any mini-roadmap in R before adding Ti and Tg to R, then qi and qg are considered not in the visibility domain of R. Ti and Tg are then added to R to improve the coverage of R. Ti or Tg will be merged into

J Intell Robot Syst

other mini-roadmaps in R repeatedly by executing function MergeTrees until Ti and Tg connect to each other and a path is found. 4.2 Pruning Roadmaps The pruning process aims to minimise the size of a roadmap. A smaller roadmap consumes less memory space and enables a path planner to spend less time searching for a path. However, tradeoffs must be made between size and other high quality roadmap criteria as explained in Section 2. The trade-off is that the deletion of vertices in order to improve the quality of the roadmap in terms of decreasing the size of the roadmap cannot decrease the quality of the roadmap in terms of coverage, connectivity and number of useful circles. Therefore, the pruning process should not break the roadmap connectivity and the existing useful circles in the roadmap or decrease the coverage of the roadmap. There-

Fig. 6 Pseudo-code for the algorithm of function PruneRoadmap

fore, function PruneRoadmap only deletes useless vertices which do not contribute to the coverage and the connectivity of the roadmap and do not construct the existing circles in the roadmap. The usefulness of a vertex depends on its local environment. Function PruneRoadmap employs a vertex category classifier to classify the vertices in a roadmap and analyses the local environment of the vertices. Then function PruneRoadmap selects suitable strategies for the vertices in various environments to identify the usefulness of the vertices and deletes useless vertices. Figure 6 presents the pseudo-code for the algorithm of function PruneRoadmap. Function PruneRoadmap is executed after function AnswerQuery adds vertices and edges to a roadmap R in order answer a query and returns POOR. Some part of R may exist and be classified already when the last path query is answered and the classification feature values of the vertices in this part are not changed by the newly added

J Intell Robot Syst

vertices obtained when the new path query is answered. Therefore, function PruneRoadmap only prunes that part of R which is constructed by the newly added vertices and the vertices whose classification feature values are changed by the newly added vertices. A list of such vertices, vertexList, is constructed. The classification features of every vertex in vertexList are calculated according to the region information in each vertex and its neighbouring vertices. The vertex category classifier is then employed to classify all the vertices in vertexList into the categories. After the classification, function PruneRoadmap groups the vertices in vertexList according to their categories. All the vertices in a group are connected with each other and in the same category. The groups can be classified into free groups, cluttered groups, narrow-passage groups and near-obstacle groups according to the category of vertices in the groups. Therefore, a group is a connected mini-roadmap containing the vertices with similar characteristics. The vertices connecting to other vertices which are not in the same group are marked as group connector vertices. All the group connector vertices are marked as useful vertices, as keeping them retains the connectivity of the existing roadmap. Figure 7 illustrates various groups; all the vertices in hollow points in each group are group connector vertices. After the classification and grouping of the vertices, function PruneRoadmap selects suitable strategies to check the usefulness

of each vertex in various groups. Then, function DeleteVertices stores the region information of useless vertices to one of its neighbouring vertices and deletes all useless vertices. Function DeleteVertices checks every vertex v in vertexList. If v is useless and v is a leaf vertex or root vertex with only one child, v is deleted. If the distance between v and its neighbouring vertex is equal to or less than extendStep, the region information of v will be moved to the neighbouring vertex before the deletion. extendStep is a user defined parameter. It is the length of an edge connecting two vertices. This process will be repeated until there are no useless vertices in vertexList. There are four strategies developed to check the usefulness of the vertices in various groups according to the types of vertices. They are explained as follows: 1. Strategy_1: Strategy_1 is applied in a free group. All the group connector vertices and the vertices which are employed to construct useful circles are marked as useful vertices. Keeping these vertices retains the connectivity between the group and the remainder of the roadmap and dose not break existing useful circles. As the vertices on the path connecting these useful vertices retain the connectivity of these useful vertices in the group, they are marked as useful vertices as well. As the local environment where the group resides is a free region, it does not require more vertices to

Fig. 7 Groups of vertices

Cluttered group Narrow-passage group

Free group Near-obstacle group

J Intell Robot Syst Fig. 8 Roadmaps before and after pruning for free group

Free group

Free group

(a) Before deleting useless vertices

represent the region. Therefore, other vertices which have not been marked can be marked as useless vertices and deleted from the group. Figure 8 illustrates the roadmaps before and after pruning based on Strategy_1 for the free group of the roadmap. 2. Strategy_2: Strategy_2 is applied in a cluttered group. All the group connector vertices and the vertices which are employed to construct useful circles are marked as useful vertices. All the vertices on the path connecting these useful vertices are marked as useful vertices. As the environment where the cluttered group resides is complex with many obstacles, the roadmap in this environment should contain long paths around each obstacle and avoid useless leaf vertices on the path. If a leaf vertex has sibling vertices where the distance between the vertices and one of its sibling vertices is shorter than extendStep, that means the distance between these two sibling vertices is small and one vertex is enough to represent the area surrounding these two sibling

(b) After deleting useless vertices

vertices. Such a vertex is considered as a useless leaf vertex and marked as useless vertex. If a leaf vertex has a distance between the leaf vertex and its ancestor vertices shorter than 2 × extendStep, that means the distance between the leaf vertex and its ancestor vertex is small and the ancestor itself can represent the area surrounding the leaf vertex, the vertex is considered as a useless leaf vertex and marked as useless vertices. Then, all the other vertices which have not been marked are marked as useful vertices. Figure 9 illustrates the roadmaps before and after pruning based on Strategy_2 for the cluttered group of the roadmap. 3. Strategy_3: Strategy_3 is applied in a narrowpassage group. All the group connector vertices and the vertices which are employed to construct useful circles are marked as useful vertices. All the vertices on the path connecting these useful vertices are marked as useful vertices. As the environment region where the narrow-passage group resides is a

Fig. 9 Roadmaps before and after pruning for cluttered group

Cluttered group

(a) Before deleting useless vertices

Cluttered group

(b) After deleting useless vertices

J Intell Robot Syst Fig. 10 Roadmaps before and after pruning for narrow-passage group

Narrow-passage group

Narrow-passage group

(a) Before deleting useless vertices

narrow-passage region, the longest path which goes through the passage should be kept. If there is only one connector, the vertex which is the furthest vertex from the connector vertex and the vertices on the path to connect the group connector vertex and the vertex are marked as useful vertices. If there are no connector vertices in the narrow-passage group, the pair of vertices with the longest distance between them among all pairs of vertices in the group is selected. The pair of vertices and all the vertices on the path to connect them are marked as useful vertices. Figure 10 illustrates the roadmaps before and after pruning based on Strategy_3 for the narrow-passage group of the roadmap. 4. Strategy_4: Strategy_4 is applied in a nearobstacle group. All the group connector vertices and the vertices which are employed to construct useful circles are marked as useful vertices. All the vertices on the path connecting these useful circles are marked as useful

Fig. 11 Roadmaps before and after pruning for near-obstacle group

(b) After deleting useless vertices

vertices. As the environment region where the near-obstacle group resides is a region which contains an area close to obstacles and a relatively free area, the roadmap in this region must pass along the obstacles which are close to this region to represent the region environment. All the vertices whose failExt are not zero are marked as useful vertices. The vertices on the path connecting these useful vertices are marked as useful vertices as well. Then all the other vertices which have not been marked are marked as useless vertices. Figure 11 illustrates the roadmaps before and after pruning based on Strategy_4 for the nearobstacle group of the roadmap.

The function PruneRoadmap is able to prune a roadmap without breaking the connectivity of the roadmap and the existing circles in the roadmap by employing various pruning strategies. Figure 12

Near-obstacle group

(a) Before deleting useless vertices

Near-obstacle group

(b) After deleting useless vertices

J Intell Robot Syst Fig. 12 Roadmaps obtained after pruning for the roadmap in Fig. 8

illustrates the roadmap obtained after executing the function PruneRoadmap for the roadmap by applying suitable strategies in various environment regions. 4.3 Adding Useful Circles The process of adding useful circles aims to add new edges between pairs of vertices in the same mini-roadmap r in order to construct useful circles. An edge connecting two vertices in the same mini-roadmap can be added to r if the edge cannot be deformed into any paths connecting the vertices in r. This means that all the circles in the roadmap are non-first-order circles. If the edge can be visibly deformed into a path in r, the edge is a redundant path. Therefore, the redundancy test should be carried out before adding any edge. Searching for a valid path in the visibility diagram of the edge and a path that connects the same two vertices can be employed to check the redundancy of the edge. If a path is searched out from the visibility diagram, it means the edge can be visibly deformed into the path and the edge is redundant. Otherwise, the edge is not a redundant edge. As the redundancy test is time consuming, not all edges connecting every pair of vertices in r are checked when constructing useful circles. The candidate vertices which are employed to add edges between them are selected before conducting the redundancy test. All the vertices in a connected roadmap are grouped according to the categories of the vertices as discussed in Section 4. As a group connector vertex of a free group resides far from the connected vertices which are not in the free group, they are most possibly the ver-

tices to construct useful circles and are added to candidatesList. In a cluttered group, the group connector vertices and the vertices are added to candidatesList. The group connector vertices are marked as group candidates vertices because only group candidates are employed to connect to vertices in other groups in order to construct useful circles to surround the cluttered regions. The candidate vertices in the cluttered group can be connected to each other to construct useful circles to surround the obstacles inside the region. In a narrow-passage group, only the connector vertices and leaf vertices are added to candidatesList. Because they reside at the end of the passage and are much more likely to be employed to construct useful circles than the other vertices in the narrow-passage region. In a near-obstacle region, all the group connector vertices and leaf vertices are added to candidatesList because they can be representative vertices in the group to construct useful circles to surround the obstacles which the group is near to. Function AddUsefulCircles attempts to add edges on each mini-roadmap r in R to construct useful circles. Figure 13 displays the pseudocode for the algorithm of function AddUsefulCircles. A list of candidate vertices candidatesList is generated for each r. For every newly added vertex v in candidatesList, if v is a cluttered vertex and has not been marked as a group candidate vertex, function AddUsefulCircles only tries to add edges between v to other candidate vertices in the same cluttered group. This is because more useful circles are required inside a cluttered region containing many obstacles, compared to other regions. Otherwise, function

J Intell Robot Syst Fig. 13 Pseudo-code for the algorithm of AddUsefulCircles function

AddUsefulCircles tries to add edges between v and other candidate vertices in the different groups to construct useful circles to surround the regions. Function AddUsefulCircles tests whether the path segment between two candidates is collision-free or not. If it is a collision-free path segment, function AddUsefulCircles checks the redundancy of the path segment. If the path segment is not redundant with respect to any paths connecting these two candidate vertices in r, a new edge connecting these two candidate vertices is added to the roadmap. Then function AddUsefulCircles removes these two candidate vertices from candidatesList to avoid additional redundancy tests being performed.

5 Experiment Results and Analysis 5.1 Performance Evaluation of the KSR Path Planner The performance of the KSR path planner was evaluated by employing the planner to construct roadmaps in six different experimental scenarios. These scenarios are as follows: 1. Simple scenario: The simple scenario describes a box robot with three dofs that moves in an environment consisting of nine cubical obstacles, which is illustrated in Fig. 14a. The distance between each pair of obstacles is

J Intell Robot Syst Fig. 14 Experimental scenarios

robot

robot

(b) Clutter scenario

(a) Simple scenario

robot

robot

(d) Room scenario

(c) Wrench scenario robot

(e) Tube scenario

(f) Tube scenario

relatively large, compared to the size of the robot. Therefore, the roadmap representing this environment contains free vertices and near-obstacle vertices. 2. Clutter scenario: the clutter scenario describes a box robot with three dofs that moves in an environment consisting of five hundred uni-

formly distributed tetrahedral obstacles (see Fig. 14b). The roadmap representing this environment contains cluttered vertices only. 3. Wrench scenario: the wrench scenario describes a wrench robot with six dofs that moves in an environment consisting of twelve obstacles, as shown in Fig. 14c. The size of

J Intell Robot Syst

the robot is relatively large compared to the spaces between the obstacles. The movements of the robot are constrained in such an environment. Therefore, the roadmap representing this environment contains cluttered vertices. 4. Room scenario: the room scenario describes a table robot with six dofs that moves in an environment consisting of eight rooms, which is represented in Fig. 14d. The width of the doors and the corridor connecting the rooms are narrow compared to the size of the robot. The table robot is required to rotate in order to pass through doors and move along the corridor. However, since the depth of the door is thin and the length of the corridor between doors is short, the vertices in the roadmap to represent these areas are cluttered vertices. Therefore, the roadmap representing the environment contains free vertices, cluttered vertices and near-obstacle vertices. 5. Tube scenario: the tube scenario describes a box robot with three dofs that moves in an environment consisting of a tube obstacle, which is shown in Fig. 14e. The tube connects two obstacle free areas. There is a narrow passage inside the tube. This narrow passage is not straight which means that it is more difficult for the robot to travel through the tube. The roadmap representing this environment contains free vertices, narrow-passage vertices and near-obstacle vertices. 6. House scenario: the house scenario describes a bird robot with six dofs that moves in a house environment, an example of which is shown in Fig. 14f. There are rooms and narrow corridors in the house. One room contains more pieces of furniture to construct a cluttered environment. The roadmap representing this environment contains all categories of vertices. The KSR path planer constructs roadmaps while answering path queries in all experimental environmental scenarios. All the path queries employed in each experiment were generated randomly. The experimented results show that the roadmaps constructed by the KSR path planner to answer 100 path queries in all the experimental

environments achieves total coverage and maximal connectivity and contains useful circles. Although it is possible to add more links to construct useful circles by answering more queries, the performance of the KSR path planner can be explored by evaluating the roadmaps constructed by the KSR path planner to answer 100 path queries. The quality improvement trend of roadmaps constructed in various experimental environments can be shown during the process of answering 100 path queries. Therefore, number of randomly generated path queries employed in each experiment was set to be 100 (Fig. 15). Since the KSR path planner is an incremental path planning algorithm, the quality of the roadmaps improve markedly during the first few path queries, and the quality of the roadmaps are changed slightly when the roadmaps are able to answer most of the path queries. Therefore, the performance of the KSR path planner is recorded after the KSR path planner answers 5, 10, 20, 50, 80, 100 path queries in each experiment. The performance of path planners can be measured and compared based on the quality of the roadmaps constructed by these path planners and the time required to construct the roadmaps and answer path queries. The quality of a roadmap can be measured in terms of coverage by calculating the CCR; connectivity by counting the number of mini-roadmaps; useful circles by counting the number of useful circles and the roadmap size by calculating the number of vertices in the roadmap, as explained in Section 3. There are two commonly used methods to measure the efficiency of a path planner for constructing a roadmap and answering path queries. One is to compute the CPU running time. The other one is to calculate the number of collision detections employed by a path planner. This is because implementing collision detection is the most time-consuming (CPU time-consuming) process [7]. Therefore, the fewer the collision detections or the less CPU time required by a path planner to construct roadmaps and answer path queries, the more efficient the path planner is. The higher quality of the roadmap constructed by a path planner, the more effective the path planner is. As all path planners investigated in this research are sampling-based path planners, the

J Intell Robot Syst Fig. 15 Roadmaps constructed in room scenario

(a) 5 path queries

(b) 10 path queries

(c) 20 path queries

(d) 50 path queries

(e) 80 path queries

(f) 100 path queries

roadmaps constructed (even the same path planner) may vary. Even when the same 100 path queries are employed to construct roadmaps by the same incremental path planner, the roadmaps produced can vary. Therefore, each path planner was executed ten times with the same experimental settings to capture the general performance of the path planner and the average (over all runs) is calculated in order to evaluate the performance of the path planners.

To evaluate the performance of the KSR path planner, 10 groups of queries were answered in each scenario. Each query group consisted of 100 randomly generated queries. The quality of the roadmaps constructed by the KSR path planner was measured against the quality criteria, namely, size, connectivity, coverage and useful circles. The efficiency of the KSR path planner to construct roadmaps is measured by the number of collision detections. The number of collision detections is

J Intell Robot Syst

1400 1200 Simple

roadmap size

1000

Clutter

800

Wrench

600

Room Tube

400

House

200 0 0

50 100 150 number of answered path queries

(b) Roadmap quality versus connectivity

(a) Roadmap quality versus size 120.0% 100.0% Simple Coverage

80.0%

Clutter Wrench

60.0%

Room Tube

40.0%

House 20.0% 0.0% 0 50 100 150 number of answered path queries

number of collision detection/10 path queries

(c) Roadmap quality versus CCR

(d) Roadmap quality versus useful circles

350 300 Simple

250

Clutter

200

Wrench

150

Room Tube

100

House

50 0 0 50 100 150 number of answered path queries

(e) The efficiency of the KSR path planner Fig. 16 Performance of KSR path planner

recorded after 5, 10, 20, 50, 80, 100 queries were answered since the last iteration. The average number of collision detections per path query is calculated. Figure 16 shows an example of KSR path planer to construct a roadmap incrementally in room scenario. The mean performance values of the KSR path planner to construct roadmaps while answering the ten groups of queries in each scenario are given in Tables 1, 2, 3, 4, 5 and 6.

The following conclusions can be drawn from these tables: (a) The trend of roadmap quality constructed by the KSR path planner, in terms of connectivity, coverage, and useful circles if upwards as more queries are answered in each experimental scenario. (b) The number of collision detections required to answer a query decreases as more queries

J Intell Robot Syst Table 1 The performance of the KSR path planner in the simple scenario Quality Env Simple

Number of query

Size

Connectivity

Coverage (%)

Useful circles

Collision detections/query

5 10 20 50 80 100

34 87 135 261 261 261

1 1 1 1 1 1

93.2 98.3 98.3 100.0 100.0 100.0

3 9 10 12 12 12

76 36 40 24 12 10

Table 2 The performance of the KSR path planner in the clutter scenario Quality Env Clutter

Number of query

Size

Connectivity

Coverage (%)

Useful circles

Collision detections/query

5 10 20 50 80 100

119 299 661 997 1,241 1,304

3 2 1 1 1 1

45.5 63.2 82.8 95.6 100.0 100.0

1 5 16 35 39 43

288 245 192 131 81 11

Table 3 The performance of the KSR path planner in the wrench scenario Quality Env Wrench

Number of query

Size

Connectivity

Coverage (%)

Useful circles

Collision detections/query

5 10 20 50 80 100

145 369 711 883 956 992

1 1 2 1 1 1

73.2 89.4 96.1 100.0 100.0 100.0

0 2 4 6 13 16

210 187 167 98 54 14

Table 4 The performance of the KSR path planner in the room scenario Quality Env Room

Number of query

Size

Connectivity

Coverage (%)

Useful circles

Collision detections/query

5 10 20 50 80 100

134 236 375 411 462 487

4 2 2 1 1 1

43.9 74.8 87.3 100.0 100.0 100.0

0 0 2 2 3 3

183 164 150 98 45 13

J Intell Robot Syst Table 5 The performance of the KSR path planner in the tube scenario Quality Env Tube

Number of query

Size

Connectivity

Coverage (%)

Useful circles

Collision detections/query

5 10 20 50 80 100

160 239 376 296 305 305

2 2 1 1 1 1

56.7 91.2 100.0 100.0 100.0 100.0

0 0 0 0 0 0

185 136 111 45 31 24

were answered. The efficiency of the KSR path planner to answer path queries trends to be higher and higher. To further explore the quality of the roadmaps constructed by the KSR path planner in various experimental scenarios, experimental results of each individual criterion given in the Tables 1–6 were also plotted in Fig. 16. Figure 16a shows the changes in size of the all roadmaps developed in various scenarios. The increase in size can be neglected after a number of queries were answered. This is because the roadmaps constructed after these queries were answered were able to represent the environments and there is no need to add more vertices to the roadmaps. Figure 16b gives the changes in connectivity of all the roadmaps. The number of mini-roadmaps of each scenario was different at the beginning, converging to one at the end, which means that the roadmap eventually achieves maximal connectivity. This is because queries may be spread over different parts of the environment at the beginning in a scenario. These mini-roadmaps represented these parts independently. When more vertices were generated, these mini-roadmaps became more likely to be connected.

Figure 16c shows the coverage of all the roadmaps. The CCR of the roadmaps increased as more queries were answered until the roadmaps achieved total coverage. This is because the size of the roadmaps increased and only the vertices that can contribute to the quality of a roadmap, including coverage, were kept, while answering queries. Therefore the CCR of the roadmaps increased as progressively more vertices were added to the roadmap. Figure 16d shows the number of useful circles contained in each roadmap. It can be observed that the numbers of the useful circles increased in all scenarios. As more vertices were added in the roadmaps, there were more opportunities for the KSR path planner to construct useful circles. More useful circles were able to be added to the complex environment consisting of a large number of obstacles, such as house and clutter scenarios. Figure 16e shows the efficiency of the KSR path planner in answering queries. The efficiency is measured by the number of collision detections as described in 6.1. The KSR path planner became more efficient as the quality of a roadmap improved. The KSR path planner usually requires a large number of collision detections at the early stage of the roadmap construction

Table 6 The performance of the KSR path planner in the house scenario Quality Env House

Number of query

Size

Connectivity

Coverage (%)

Useful circles

Collision detections/query

5 10 20 50 80 100

224 364 514 632 710 713

3 2 1 1 1 1

36.7 56.8 77.6 92.3 100.0 100.0

0 3 6 14 20 22

265 210 138 81 27 16

J Intell Robot Syst

when the roadmaps are not able to answer a newly raised query. Once the roadmap becomes good enough to answer queries, the planner will only employ collision detection during the process of connecting the initial and the goal configurations of queries to the roadmaps. Since collision detection is the most time-consuming function of a path planner, the ability to answer queries of the roadmap increases as the number of collision detections required to answer queries decreases.

5.2 Comparison Between KSR Path Planner and a RRF (Reconfigurable Random Forest) Path Planer The second batch of experiments presented in this section compares the KSR pruning with the pruning process (RRF pruning) employed by the RRF path planer [11]. The roadmaps were constructed by employing the function AnswerQuery to answer ten path queries without any pruning process

Fig. 17 The roadmaps obtained for comparing FFR pruning and KSR pruning

(a) The roadmap before pruning process

(b) The roadmap after RRF pruning process

(c) The roadmap after KSR pruning process

J Intell Robot Syst Table 7 Comparing KSR pruning and RRF pruning Env

Size BP

Simple Clutter Wrench Room Tube House

312 473 683 305 772 524

AP KSR

RRF

72 293 352 197 272 324

41 360 498 249 583 448

Connectivity

Coverage

CPU time

BP

AP

BP

KSR

RRF

KSR

RRF

(%)

KSR (%)

RRF (%)

1 2 1 1 1 3

1 2 1 2 1 3

1 2 1 2 1 2

97.3 60.2 86.4 71.5 90.2 69.1

97.3 60.2 86.4 71.5 90.2 69.1

97.3 60.2 86.4 71.5 90.2 69.1

1.2 2.3 1.6 1.8 2.7 3.5

10.7 29.2 15.3 19.9 32.1 30.8

AP

BP before pruning; AP after pruning

in all experimental scenarios. The roadmaps were then pruned by the KSR pruning method and the RRF pruning method. Figure 17 illustrates an example of this experiment in using a tube scenario. Figure 17a presents a roadmap constructed by the function AnswerQuery. The roadmaps obtained by applying the RRF pruning and the KSR pruning are shown in Fig. 17b and c, respectively. The process of constructing a roadmap and applying KSR pruning and RRF pruning in each scenario was repeated ten times. Table 7 shows the mean values to evaluate the qualities of the roadmaps and the CPU time consumed by applying the KSR pruning method and RRF pruning methods. The following conclusions can be drawn by comparing the KSR and the RRF pruning methods: (a) Neither pruning process method affects the quality of a roadmap in terms of connectivity and coverage. The KSR pruning consists of deleting useless vertices which do not contribute to improvement of the roadmap quality in terms of coverage and connectivity. The RRF pruning approach consists of deleting a vertex if the distance between it and the vertices near it is shorter than a pre-defined value. As the pre-defined value should be defined as a very short distance and all of the children of the deleted vertex will be merged with the trees after it is deleted, the coverage and connectivity of the roadmap does not change. (b) The KSR pruning method is able to reduce the size of a roadmap more effectively. This is because KSR pruning takes the region

information of vertices into account and employs the most suitable pruning strategies accordingly. These strategies allow only those vertices that are necessary for representing the corresponding regions to be kept. However, the RRF pruning method only deletes a vertex if there is another vertex such that the distance between the two vertices is shorter than a pre-defined value. If there is no such vertex in the constructed roadmap, no vertices will be deleted. (c) The RRF pruning method is much more time-consuming. The reason is that this method employs collision detections while merging vertices and the collision detections are the most time-consuming processes in path planning. On the other hand, the KSR pruning method does not require any collision detection, which is quite efficient when deleting useless vertices.

6 Conclusion A novel KSR path planner was developed based on the analysis of the quality problems of existing roadmap construction algorithms, and different environment types. The KSR path planner is an incremental learning path planning algorithm. It consists of the following main modules: answering path queries, vertex category classifier, pruning KSR and adding useful circles. The KSR path planner constructs a KSR while answering path queries. Once a path query is raised, the KSR path planner answers the query by connecting the initial tree (takes the initial configuration of the

J Intell Robot Syst

query as the root) and goal tree (takes the goal configuration of the query as the root) to the existing roadmap in order to find a path to answer the path query. The connectivity and the C f ree coverage ratio are improved and the region information of the vertices in the existing roadmap is recorded in the process of answering path queries. After the query is answered, the KSR path planner employs the vertex category classifier to identify various local environments where the vertices reside by analysing the region information stored in the vertices and their neighbours. The KSR path planner then selects suitable strategies to prune the roadmap in order to control the roadmap size and add useful circles according to the local environment. The quality of KSR can be improved incrementally while answering more and more path queries until it can answer any path query and no useful circles can be added to the roadmap. Experimental results show that the KSR path planner can construct a roadmap and improve the quality of the roadmap incrementally while answering path queries until the roadmap can answer all the path queries without any pre-processing stage. The roadmap constructed by the KSR path planner then achieves better quality than the roadmaps constructed by Reconfigurable Random Forest (RRF) path planner and traditional probabilistic roadmap (PRM) path planner. Since the structure of a roadmap can be divided into several parts and some execution processes of a path planner can be adjusted and applied to all parts at the same time without affecting the executions in other parts, parallel application is an effective and commonly used method [3, 4] to improve the performance of a path planner. The KSR constructed by the KSR path planner can be divided into several parts. Also there are some processes of the KSR path planner that can be executed on all parts in parallel, such as the vertex classification process, KSR pruning and adding useful circles. With the knowledge of the environment gained by employing the vertex category classifier, more sophisticated strategies can be created to improve the quality of the roadmaps more efficiently and effectively, such as new sampling strategies to generate more samples in complex and difficult

environmental regions, adding circle strategies based on various environment regions in order to reduce the required number of collision detections. A dynamic environment is an environment with moving obstacles. A KSR constructed by the KSR path planner is a roadmap with a structure which allows the existence of several mini-roadmaps. With moving obstacles, some existing vertices of a KSR may not be collision free and need to be deleted from the existing roadmap. A miniroadmap may be split into two or more miniroadmaps. However, this modification of the existing roadmap does not change the structure of the KSR. The KSR path planner is still able to answer new queries and improve the quality of the existing roadmap after the movement of obstacles. Therefore, the KSR path planner can be further developed to suit dynamic environments in the future.

References 1. Bohlin, R., Kavraki, L.E.: Path planning using lazy PRM. IEEE Int. Conf. Robot. Autom. 1, 521–528 (2000) 2. Geraerts, R., Overmars, M.H.: Creating High-quality roadmaps for motion planning in virtual environments. IEEE/RSJ Int. Conf. Intell. Robot. Syst. 4355–4361 (2006) 3. Isto, P.: A parallel motion planner for systems with many degrees of freedom. Int. Conf. Adv. Robot. 339– 344 (2001) 4. Isto, P.: Constructing probabilistic roadmaps with powerful local planning and path optimization. IEEE Int. Conf. Robot. Autom. 3, 2323–2328 (2002) 5. Jaillet, L., Simeon, T.: Path deformation roadmaps. In: International Workshop on Algorithmic Foundations of Robotics (2006) 6. Kallman, M., Mataric, M.: Motion planning using dynamic roadmaps. IEEE Int. Conf. Robot. Autom. 5, 4399–4404 (2004) 7. Kavraki, L.E., Svestka, P., Latombe, J.C., Overmars, M.H.: Probabilistic roadmaps for path planning in highdimensional configuration spaces. IEEE Trans. Robot. Autom. 12, 566–580 (1996) 8. Kamphuis, A., Mooijekind, M., Nieuwenhuisen, D., Overmars, M.H.: Automatic construction of high quality roadmaps for path planning in games. In: International Conference of Computer Games on Artifical Intelligence, Design and Education, pp. 285–292 (2004) 9. Kuffner, J.J., LaValle, S.M.: RRT-connect: an efficient approach to single-query path planning. IEEE Int. Conf. Robot. Autom. 2, 995–1001 (2000)

J Intell Robot Syst 10. LaValle, S.M.: Rapidly-Exploring Random Trees: A New Tool for Path Planning. Technical Report 98-11, Computer Science Department, Iowa State University, October 1998 (1996) 11. Li, T.Y., Shie, Y.C.: An incremental learning approach to motion planning with roadmap management. IEEE Int. Conf. Robot. Autom. 4, 4311–3416 (2002)

12. Schmitzberger, E., Bouchet, J.L., Dufaut, M., Wolf, D., Husson, R.: Capture of homotopy classes with probabilistic roadmap. IEEE Int. Conf. Intell. Robot. Syst. 3, 2317–2322 (2002) 13. Simeon, T., Laumond, J.P., Nissoux, C.: Visibility based probabilistic roadmaps for motion planning. Adv. Robot. J. 14(6), 177–494 (2000)