1 Introduction

Probabilistic Roadmaps (PRM) [1, 2] are extensively used for motion planning of a single or multiple mobile robots. The methods first construct a roadmap ( ζ <V,E> ) of the obstacle-free configuration space ( C free) by sampling of points which prospectively become the vertices of the roadmap ( V) and then connecting the neighboring vertices by using a local search algorithm using trajectories which become the edges of the roadmap ( E). A popular method is to use a straight line collision checking algorithm as the local planner. An all-connected approach checks for collisions between all pairs of vertices. The approach however has a very high computational complexity making it worthless when the initial roadmap needs to be computed in a small computational time. It is common to limit the collision-checking to only k-nearest neighbors [3, 4] or vertices within a radius of k [3, 5] to limit connectivity and reduce the computation time. The roadmap ( ζ) is then used for the online planning. Since the number of vertices and edges are limited, a near-real time response to the motion planning queries is realizable.

The algorithm comes under the class of sampling based approaches which try to generate a representation of the collision-free configuration space by generation of random samples. The sampling based approaches face the problem of narrow corridor. A narrow corridor has a very small volume of free-space, sandwiched between obstacles at a little distance in at least one dimension. The distance denotes the narrowness of the corridor. Hence the probability of generation of random samples inside the narrow corridors is very low which cannot be located easily. Several common techniques have been used for solving the narrow corridor problem. In obstacle-based sampling [6] a sample generated inside an obstacle is promoted to the nearest obstacle-free region. The samples generated inside the corridors hence include the samples generated in the nearby obstacles which make the narrow corridor, the probability of generation of which is significantly high. Gaussian sampling [7] is another popular method wherein the probability of acceptance of a sample diminishes in a Gaussian manner from the obstacle boundary. By encouraging the generation of samples near the obstacle boundary, the possibility of discovering the narrow corridor increases significantly.

Bridge-test [8] specifically aims in sampling inside the narrow corridor. In this method every sample is checked whether it lies inside a narrow corridor. For every sample the test generates two more samples in opposing directions. If both happen to be placed inside obstacles, while the initial sample is obstacle-free, the sample is said to be placed inside a narrow corridor. A variety of sampling techniques may be generated by their hybrids or adaptive combinations [8, 9]. The hybrids certainly benefit from the combined advantages of the base approaches, however getting a mix of narrow corridor biased sampling and a general obstacle-prone sampling is always hard. Besides, discovery of a narrow corridor doesn’t necessarily redundantly and adequately connect it to the rest of the roadmap, a challenge largely left in the literature.

A large and complicated configuration space may have multiple narrow corridors amidst numerous wide open spaces. The narrow corridors are initially hard to cite and connect in a roadmap architecture. While the roadmaps are intended to be initially constructed in an offline manner, requirements of rapidly changing environments, fast initial motion of the robot or robotic team, etc. may demand a fast and near-real time construction of the roadmap. Discovering all the narrow corridors and redundantly connecting them with the roadmap plays an important role towards the optimality and completeness of the solution. The roadmap must consist of the least number of vertices and edges (for minimizing roadmap construction and query time), while leading to discovery of all possible homotopic groups of paths between all possible sources and goals. Two paths are said to be in the same homotopic group [10] if one can be produced from the other by multiple infinitely small deformations, such that all intermediate paths are collision-free. The concept is shown in Fig. 1. Discovery of all homotopic groups guarantees a path for every pair of valid sources and goals, or in simple words the approach is complete. A deformation retract [11] is the roadmap formed by shrinking all open spaces and reducing them to simple curves. Even though a deformation retract of the configuration space serves as an ideal roadmap, some redundant nodes and edges may be acceptable and even preferable to cater to optimality.

Fig. 1
figure 1

Homotopies. Trajectory B and C belong to the same homotopic group, while A does not belong to the homotopic group of B and C

Full size image

Elimination of redundant edges and cycles is one of the most primitive ways of reducing the roadmap construction as well as the query time. Visibility PRM and related techniques [12, 13] sample out the configuration space with the same fundamental, however redundancy is needed to mine out all possible homotopic groups ultimately affecting the roadmap optimality. Redundant edges may later be admitted [14, 15]. An alternative to PRM is the Rapidly-exploring Random Tree (RRT) [16, 17] algorithm which is a single-query algorithm and attempts to quickly compute a path between the source and the goal without necessarily trying to explore the complete search space. RRT* [3, 18] algorithm can use excess computation time to iteratively improve the solution, thus addressing the issue of optimality in the use of RRT algorithm. The tree is re-wired every time a better path is discovered in order to maintain the tree-structure. The approach does not address the issue of narrow corridors, and the normal processing of the algorithm can take prolonged time to find a redundant and shorter way through a narrow corridor. Attempts are further made to extend the computational ability of RRTs for the production of roadmap, like the Sampling based Roadmap of Trees (SRT) [19] and Rapidly-exploring Random Graphs [20]. In an earlier work by the author, RRT style exploration was used to construct a redundant roadmap in a time-efficient iterative manner [21]. These techniques however are not well suited for narrow corridor biased sampling.

Phillips et al. [22] expanded the trees towards better areas, wherein the affinity towards a region was measured by the order and degree of the node in the graph, the number of neighbors and its cost as computed by the A* algorithm. The different metrics made the nodes inside narrow corridors more likely to expand, however did not necessarily encourage entering and thereafter travelling the corridor in order to fully travel through the corridor with the goal at the other side. Kim et al. [23] proposed a Linear Predictor based Uniform State Sampler, wherein the aim is to generate uniform samples and to further compute a control signal that can make the transition. Local linearity was assumed in the derivation of this control signal. The approach was implemented in parallel using a GPU. This approach as well was not centric to narrow corridors.

Approaches like graph spanners [24, 25] restrict the number of edges by removing redundant edges if they do not result in getting paths worse by a factor or more than k. However they do not solve the problem of timely roadmap construction at the first instance. Given an initial roadmap, approaches like Adaptive Roadmap [26, 27] and Elastic Roadmap [28, 29] can be used for maintenance and extension of the roadmap for a dynamic environment. If initially a good roadmap can be obtained, it becomes possible to model the environment dynamics to adapt the roadmap as the environment changes. The adaptations may even be applied to only the trajectory being followed by the robot (e.g. [30]), obtained from some roadmap based approach.

The key contributions of the work are: (a) A narrow corridor sampling strategy is proposed which combines the obstacle based sampling strategy along with a bridge-test sampling strategy. The resulting sampling strategy is a single hybrid strategy and not adaptive independent calls to different strategies in an ensemble architecture popular in the literature. (b) The narrow corridor sampling strategy is called in conjuncture with the other strategies to suit diverse environment types including the ones with numerous narrow corridors, no narrow corridors and wide open spaces. (c) It is proposed to add relevant and strategized vertices in the edge connectivity mechanism to aid in redundant roadmap connectivity. (d) A multi-strategized edge connectivity mechanism is proposed to aid in redundant connectivity between all vertices of the roadmap, thereby also leading to complete homotopic group discovery. (e) The proposed approach is seen to be significantly better than obstacle based and uniform sampling.

The paper is organized as follows. Section 2 presents the algorithm framework. This includes the roadmap generation phase (Sections 2.1–2.3) as well as the query phase (Section 2.4). The roadmap generation phase includes the addition of vertices (Section 2.1) and edges (Section 2.2), followed by the pseudo-codes (Section 2.3). The results are given in Section 3. Section 4 gives the conclusions.

2 Algorithm

The first task associated with the algorithm is to construct a roadmap ( ζ <V,E> ) consisting of the vertices V and edges E, which can then be used for robot path planning. Let the configuration space be given by C and let the free configuration space be given by C free. Let C obs be the obstacle prone configuration space, which gives C free = C∖C obs. Non-strictly, first the vertices are computed. Once a set of vertices are available, they are connected by adding some edges.

2.1 Computing Vertices

The first task is computation of the vertices that make up the roadmap. Three strategies are proposed to generate samples which become the vertices of the roadmap:

  • Narrow Corridor Sampling Strategy, which attempts to sample out all narrow corridors;

  • Obstacle based Sampling Strategy, which produces samples around obstacles which are critical for a roadmap; and

  • Uniform Sampling, to add samples for optimality in case of wide open spaces.

The first strategy is known as Narrow Corridor Sampling strategy, using which the emphasis is only to find the samples which lie inside some narrow corridor. Narrow corridors are important to sample out almost completely in order to expect discovery of all possible homotopic groups in the generated roadmap between all possible sources and goals.

In this strategy, we first sample out a point in the obstacle prone configuration space ( q o = U(C obs)). A random direction is chosen. The sample is made to travel in the random direction, until it reaches the free configuration space. This advancement of the sample from obstacle-prone configuration space to the free configuration space is shown by (2). The algorithm uses a bi-resolution strategy to search for\(\phantom {\dot {i}\!}q_{o}^{free} \)by first searching at a higher resolution by making steps of large sizes. The first point in the free configuration space is taken. A travel is then made inward towards the obstacle using a finer resolution by travelling with small step sizes to precisely get the obstacle boundary.

$$\begin{array}{@{}rcl@{}} q_{o}^{free} &=& q_{o} +d\left( {\hat{{\theta} }} \right):q_{o} +d\left( {\hat{{\theta} }} \right)\in C^{free},\\ &&q_{o} +d_{1} \left( {\hat{{\theta} }} \right)\in C^{obs}\forall d_{1} <d \end{array} $$
(1)

Here \(\phantom {\dot {i}\!}\hat {{\theta }}\) is a randomly sampled direction. If \(\phantom {\dot {i}\!}\hat {{\theta } }\) does not result in a point in the free but bounded configuration space, a new direction is chosen.

To test whether the point \(\phantom {\dot {i}\!}q_{o}^{free} \) represents a point inside a narrow corridor, bridge-test is used. A point q D is generated at a distance of D from \(\phantom {\dot {i}\!}q_{o}^{free} \) (that is \(q_{D} =q_{o}^{free} +D\left ({\hat {{\theta }}} \right ))\). If q D is collision-prone, the point \(q_{o}^{free} \)is said to be inside a narrow corridor of maximum width D. This is because an obstacle exists at direction \(\phantom {\dot {i}\!}-\hat {{\theta } }\) from \(\phantom {\dot {i}\!}q_{o}^{free} \)known from the sampling philosophy of \(\phantom {\dot {i}\!}q_{o}^{free} \). Subsequently it is known that an obstacle exists at the direction \(\phantom {\dot {i}\!}-\hat {{\theta } }\) from \(\phantom {\dot {i}\!}q_{o}^{free} \) known from the generation of q D . Since an obstacle exists at both the opposing directions, the bridge-test criterion holds. Here D is the maximum width of the narrow corridor that the algorithm attempts to sample, and is an algorithm parameter.

The attempt is to sample out the point in the middle of the narrow corridor so as to facilitate easier connection of the sample to the rest of the roadmap, for which a point in the other corridor boundary is required given by (2). The sampled point inside the narrow corridor is given by (3). The notations are explained in Fig. 2.

$$\begin{array}{@{}rcl@{}} q_{D}^{free} &=& q_{D} -d\left( {\hat{{\theta} }} \right):q_{D} -d\left( {\hat{{\theta} }} \right)\in C^{free},\\ &&q_{D}-d_{1} \left( {\hat{{\theta} }} \right)\in C^{obs}\forall d_{1} <d. \end{array} $$
(2)
$$ q_{C} =\left( {q_{o}^{free} +q_{D}^{free}} \right)/2. $$
(3)
Fig. 2
figure 2

Sampling inside a narrow corridor

Full size image

The least distance between any two vertices of a roadmap is taken to be Δ ( ≤D). This disallows placing vertices too close to each other, thus resulting in too many vertices. In order for the vertex q C to be admissible in the roadmap with vertices V, this distance condition must hold (that is ∥q C −v∥≥Δ∀v∈V).

The maximum attempts of finding a narrow corridor are restricted to n C . A good narrow corridor sampling strategy must find all the narrow corridors, if they exist, while exit gracefully after a small computation if the environment is free of narrow corridors. The proposed approach uses a combination of obstacle-based sampling strategy along with a bridge-test to quickly discover the narrow corridor. The use of bridge-test alone results in poor probability of finding a narrow corridor (which is still larger than a uniform sampling), while obstacle based sampling alone can result in generation of most of the samples around obstacles and not necessarily the narrow corridors, if the narrow corridor is a small part of the otherwise cluttered environment.

Samples inside the narrow corridors are of significant value in quick generation of roadmaps. However the strategy is supplemented by two other strategies, which also suit cases of low or no occurrence of narrow corridors; and spaces with sparsely occupied obstacles. The second strategy of use is an obstacle based sampling strategy where random samples in the obstacle prone configuration space are generated ( q o = U(C obs)) and then moved to the nearest point in the free configuration space (\(\phantom {\dot {i}\!}q_{o}^{free})\) as indicated in (2). The strategy is same as the initial generation of sample at the obstacle boundary in the narrow corridor sampling and hence the discussions are not being repeated. For a sample to be admissible as per this strategy, it is required to have at least a clearance of Δ, which gives enough scope for the edge connection algorithm to connect the sample redundantly to the rest of the roadmap. For this reason the \(\phantom {\dot {i}\!}q_{o}^{free} \) sample obtained (2) is further moved in the same direction by a magnitude of Δ to obtain a sample q B given by (4).

$$ q_{B} =q_{o}^{free} +{\Delta} \left( {\hat{{\theta}}} \right). $$
(4)

The sample is admitted in the roadmap if it is collision-free and at a distance of more than Δ from any other vertex in the roadmap (that is q B ∈C free∧∥q B −v∥≥Δ∀v∈V). Failure of admissibility due to the lack of maintenance of the minimal clearance is an indication of a narrow corridor which is admissible as per the narrow corridor sampling strategy. The strategy is applied for a maximum of n B attempts.

The third and the last strategy is to uniformly sample the free configuration space ( q = U(C free)). The strategy is applied for a very small number of attempts ( n U ) as it is only of value in sparsely occupied scenarios while in some cases it may also result in connecting some challenging samples to the rest of the roadmap.

The heuristics behind the individual sampling strategies are taken straight from the literature, which are adapted for the specific use. The parameters D and Δ are rather standard from the literature and may hence be conveniently set based on the literature studies. The parameters n C , n B and n U are important parameters for the algorithm functioning. However these are easy to set, knowing that the success rate of the narrow corridor sampler is much lower than the obstacle based sampler, which is still lower than the uniform based sampler, while the importance of the individual samplers is in the other order. So the maximum percentage of time is allocated to the narrow corridor sampler, a smaller one to the obstacle based sampler, while for the uniform sampler a very few samples need to be inserted. Using success rates, connectivity measures, obstacle density measures, etc. can help in parameter settings, however these methods themselves add more parameters to the algorithm.

2.2 Computing Edges

Fundamentally the aim of this step is to connect the vertices generated by the strategies mentioned in Section 2.1 into a roadmap structure using edges ( E). However the process of connecting the different vertices may involve adding new vertices. At any time the roadmap may have vertices in disjoint set of connected components. Two vertices a and b lie in the same connected component if there is a path from a to b and vice versa as per the current roadmap. The edge connectivity function is forcibly made symmetric and hence all edges are non-directed and a reverse path always exists. Initially all vertices are into different disconnected components. Some of the connections are very easy and less time consuming to make. Hence, stating from an all-disjoint graph, the algorithm initially adds up all simple connections, resulting in only a few disjoint components. The addition of edges then aims at making sure all the components are connected, to aid in completeness and discovery of all homotopic groups; as well as to add redundant connections for the sake of optimality. The former is done by focusing on means to add vertices and edges in pursuit of connections between otherwise disconnected components. The latter is done by exploration through different strategies, namely leaf exploration, hash based exploration and random exploration. The connected components along with the associated terminology are better explained by Fig. 3.

Fig. 3
figure 3

Disjoint set of vertices

Full size image

2.2.1 Terminology

Let κ(a) denote the collection of connected vertices to which a belongs. Initially all vertices are disconnected as no edges exist and hence κ(a)={a}∀a∈V. Since connectivity of the roadmap is of importance and disconnected components are discouraged, we maintain the following data structures throughout the edge connectivity procedure:

  • | κ(a),κ(b)|: The distance between the connected components κ(a) and κ(b), defined as the minimum distance between any vertex x in κ(a) with any vertex y in κ(b), or (5).

$$ \left| {\kappa \left( a \right),\kappa \left( b \right)} \right|=\min\limits_{x\in \kappa \left( a \right),y\in \kappa \left( b \right)} \left\| {x-y} \right\|. $$
(5)

  • x[ κ(a),κ(b)]: The vertex x in κ(a) which records the minimum distance of | κ(a),κ(b)|with any vertex y in κ(b), or (6).

$$ x\left[ {\kappa \left( a \right),\kappa \left( b \right)} \right]=\arg \min_{x\in \kappa \left( a\right),\forall y\in \kappa \left( b\right)} \left\| {x-y} \right\|. $$
(6)

  • y[ κ(a),κ(b)]: The vertex y in κ(b) which records the minimum distance of | κ(a),κ(b)|with any vertex x in κ(a), or (7).

$$ y\left[ {\kappa \left( a \right),\kappa \left( b \right)} \right]=\arg \min_{y\in \kappa \left( b \right),\forall x\in \left( a \right)} \left\| {x-y} \right\|. $$
(7)

  • κ: The set of distinct non-components in the roadmap. Initially all components are non-connected and hence κ = V. In general, \(\phantom {\dot {i}\!}\kappa =\bigcup \limits _{a}{\kappa \left ( a \right )} \).

  • Failed( κ(a), κ(b)): The number of failed attempts in connecting two disjoint sets vertices κ(a) and κ(b).

  • Ψ(z): The configuration space is mapped to a lower dimensional space Ψ(.) using a hash function and every cell z of the reduced space stores a count of the total number of vertices which lie within that cell. The maximum occupancy of the cell z, denoted by | Ψ(z)|, is given as the volume of the free configuration space ( C free) covered by that cell. The ratio Ψ(z)/| Ψ(z)|is known as the desirability ratio of the cell and corresponds to the degree of under-occupancy of the cell.

2.2.2 Initial edge connection

A limited connectivity roadmap is aimed, wherein connections are attempted between any vertex to the neighboring k vertices. Initially there are no edges and hence an initial attempt to get some connected roadmap is made by trying an initial edge connection strategy. The roadmap (not necessarily completely connected) is then extended by adding edges (and vertices) using a variety of other strategies.

Initially an attempt is made to connect every vertex to the neighboring k vertices. Initially only straight line connections between the vertices are attempted. In order to test for an edge between a vertex v 1 and a neighboring vertex v 2, one travels in a straight line from v 1 to v 2. If no obstacle is encountered on the way, the edge is added. However it is possible that the traversal from v 1 to v 2 results in encountering an obstacle at a position o 1. The point o 1 is a good candidate for inclusion in the roadmap, however as per the sampling strategy a clearance of Δ is preferable unless the sample is inside a narrow corridor. So the point o 1 is moved at a distance of Δ away from the obstacles by travelling the same distance towards v 1. The same concepts are given by (9)–(10).

$$\begin{array}{@{}rcl@{}} o_{1} &=&v_{1} +d\left( {\hat{{\theta} }} \right):v_{1} +d_{1} \left( {\hat{{\theta} }} \right)\in C^{free}\forall d_{1} <d, \\ &&v_{1}+d\left( {\hat{{\theta} }} \right)\in C^{obs}. \end{array} $$
(8)
$$ o_{1}^{\Delta} =o_{1} -{\Delta} \left( {\hat{{\theta} }} \right). $$
(9)

The vertex \(\phantom {\dot {i}\!}o_{1}^{\Delta } \) can only be added if the distance between points \(\phantom {\dot {i}\!}o_{1}^{\Delta } \) and v 1 is at least 2 Δ apart from the usual admissibility conditions for vertices. Distances smaller than this magnitude reflect that the points \(\phantom {\dot {i}\!}o_{1}^{\Delta } \)and v 1 are nearly the same and hence an additional vertex is not preferable. The notations are shown in Fig. 4.

Fig. 4
figure 4

Adding new vertices in initial edge connectivity

Full size image

v 1 and v 2 may be far apart and in many cases belonging to different connected components of the roadmap. Absence of a straight line connection necessitates the use of local search methods to aim a connection. In this implementation redundant connections or useful cycles are also of value and hence attempting such connections is of value. Local search algorithms usually take a significant time, especially if the participating vertices are far away. A better way of attempting connections is by trying to add vertices which result in a connection between the two vertices. Searching for such bridging vertices in the entire region may be too costly with not necessarily a good probability of connection and a single vertex may not be capable of connecting the two vertices.

Hence a different strategy is used in the proposed algorithm. With a single connection attempt from v 1 to v 2 an obstacle prone region around \(\phantom {\dot {i}\!}o_{1}^{\Delta } \) has already been identified. Similarly in the connection attempt from v 2 to v 1, obstacle region around \(\phantom {\dot {i}\!}o_{2}^{\Delta } \)would be identified and a vertex added. In a couple of connection attempts which is not cost intensive, two points \(\phantom {\dot {i}\!}o_{1}^{\Delta } \)and \(\phantom {\dot {i}\!}o_{2}^{\Delta } \)are easily obtained. The difficult problem of connecting v 1 and v 2, which may be far apart or separated with complex and unknown obstacle grids, has now been reduced to the problem of connecting points \(\phantom {\dot {i}\!}o_{1}^{\Delta } \) and \(\phantom {\dot {i}\!}o_{2}^{\Delta } \), which may be close and separated by just an obstacle corner, building a small (non-straight) trajectory around \(\phantom {\dot {i}\!}o_{1}^{\Delta } \) and \(\phantom {\dot {i}\!}o_{2}^{\Delta } \)may be much easier. This connection is done using the regular set of edge connection strategy.

2.2.3 Connect disconnected vertices strategy

After some initial edges have been quickly added using the strategy detailed above, the regular connection strategies continue. The first strategy is the connect disconnected vertices strategy. This strategy aims at taking two disconnected vertex sets ( κ(a) and κ(b)). Since no connection exists between any vertex in κ(a) with any other vertex in κ(b), there is a loss of completeness and needs to be addressed as far as possible. There are two aspects of the strategy. The first aspect is to select the disjoint pairs κ(a) and κ(b) between which a connection must be attempted. The second aspect deals with what one can do so as to contribute towards eventually connecting the two.

Considering the first aspect to select a disjoint pair. The first set ( κ(a)) is chosen randomly out of the available sets κ. It is evident that vertex sets closer to κ(a) are more likely to be easily connected by addition of a few suitable vertices in between. However even if two sets are very close to each other, connecting them may not always be possible due to the presence on an impassable obstacle wall in the middle, and hence other alternatives also need to be explored. This motivates the use of probabilistic selection of the second set, such that the nearer be the disjoint set, more is the probability of its selection. The second set κ(b) is chosen randomly from a Roulette Wheel Selection where any set i has a probability of selection as given by (10).

$$ P\left( {\kappa \left( i \right)} \right)=\frac{1\left/{\left| {\kappa \left( i \right),\kappa \left( a \right)} \right|}\right.}{\sum\limits_{i} 1{\left/{\left| {\kappa \left( i \right),\kappa \left( a \right)} \right|}\right.}} . $$
(10)

The second aspect is to attempt connecting the two disjoint vertices. This is done by growing one of the disconnected set of vertices towards the other set. By repeated multiple such attempts over the course of time, it is likely that the two disjoint sets eventually get connected if such a connection is possible. The basic philosophy is motivated from the RRT style expansion, wherein a tree is continuously expanded in the configuration space. Using the same philosophy, in order to expand the roadmap, a vertex must be selected. The closest pair of vertices that contribute towards the shortest distance between the two disconnected set of vertices, that is x[ κ(a), κ(b)] and y[ κ(b), κ(b)] are taken. In order to expand the roadmap κ(a), the closest vertex to κ(b) or x[ κ(a), κ(b)] is a good choice. A straight line expansion towards the corresponding vertex y[ κ(b), κ(b)] could have been suggestive, but considering that straight line connections between the vertex pair has potentially already failed, only exploration is necessary. Expansion is done by adding a new vertex c at a distance of δ in a random direction from x[ κ(a)] given by (11). c is called as a bridging vertex, since it attempts to bridge the two disconnected set of vertices.

$$ c=x\left[ {\kappa \left( a \right),\kappa \left( b \right)} \right]+\delta \left( {\hat{{\theta} }} \right). $$
(11)

The vertex c is added only if the path from x[ κ(a)] to c is collision free, in which case the corresponding edge is also added. The path from c to y[ κ(b)] may not be collision-free and is not a criterion for admissibility of c. If the path comes out to be collision-free, the corresponding edge is also added and the two disjoint vertex sets are said to be merged and the updated metrics are computed. However if a feasible c cannot be computed, the failed attempts between the pair (Failed( κ(a), κ(b))) is added by 1. The selection of pair κ(a) and κ(b) can only happen if the failed attempts of the particular pair are less than a threshold of η. If multiple vertices are needed to connect the two disjoint sets, each such vertex can be added in separate iterations. Addition of a new vertex c also means that the connectivity of c with the nearest k vertices will also be checked, however here no new vertex may be added.

2.2.4 Leaf expansion strategy

The next strategy is the leaf vertex expansion strategy. The aim of this strategy is to grow the roadmap outward, trying to reach the unexplored areas of the configuration space. Such expansions also aid in travelling inside the narrow corridors, while sampling the narrow corridor may itself not be enough. The strategy is motivated from the Expansive Search Trees. In this strategy a random leaf vertex is selected from the entire roadmap in pursuit of its expansion. The leaf node in a roadmap is the one which has a single parent linking it to the rest of the roadmap. Such leafs are normally found at the edges of the expansive cover of the roadmap and are good candidates for extension of the cover of the roadmap. A random leaf node ( q L ) is selected. The node is expanded in a random direction by a distance of δ to create a new vertex c similar to (11). The vertex c is added if a straight line collision-free path from q to c is possible along with the other necessary conditions. On addition of the vertex, connections with the neighboring k vertices are also checked.

2.2.5 Hash expansion strategy

Even though a complete coverage of the entire configuration space is not the aim of the work, as it contradicts the aim of a quick initial roadmap generation, a small attempt is made in the direction for the cases where prolonged computation is possible using the hash expansion strategy. The strategy attempts to ensure enough samples in all areas of the configuration space, and suggests sampling around the areas which are otherwise not adequately represented in the roadmap. Even though it is more beneficial to have samples inside the narrow corridors and near the obstacle boundaries, complete absence of samples in large volumes of the configuration space can result in poor roadmaps affecting the optimality and completeness. Sometimes ensuring a good coverage of the configuration space also results in samples in areas which probabilistically somehow get left off, and the strategy many times provides good samples for providing good connectivity of the roadmap.

The entire configuration space is mapped to a lower dimensional hash, with every cell of the hash corresponding to some volume of the high dimensional configuration space. Ψ(z) denotes the total number of samples in the cell z, or the total number of samples in the volume covered by the cell z. | Ψ(z)| is the total volume of the configuration space covered by the cell z. The desirability ratio ( Ψ(z)/| Ψ(z)|) points to the scarcity of the samples in the cell, in realization to its free volume, and thus the desirability to produce more vertices in the region. A poor desirability ratio Ψ(z)/| Ψ(z)| means a need to expand in that region. Hence a region z is randomly selected using a Roulette Wheel Selection with the probability of selection of the region z given by (12).

$$ P\left( z \right)=\frac{1-{\psi \left( z \right)} \left/{\left| {\psi \left( z \right)} \right|}\right.}{\sum\limits_{z} {\left( {1-{\psi \left( z \right)} \left/{\left| {\psi \left( z \right)} \right|}\right.} \right)}}. $$
(12)

A random sample c is generated which maps to the cell z of the map. The closest node in the roadmap ( v 1) is computed. If a collision-free path from v 1 to c exists along with the necessary admissibility criterions of c, the node c and the corresponding edge are added. Connectivity to the nearest k neighboring edges is checked.

2.2.6 Random expansion strategy

The last strategy is the random expansion strategy. The strategy is inspired by the RRT and similarly the Rapidly-exploring Random Graphs. Here the entire roadmap, acting as a graph, is expanded in a random direction. The strategy aims at adding random samples, while still maintaining a connected structure and not adding to the overhead of checking for edges. Many times such expansions can help in growing the roadmap to induce redundant cycles and otherwise samples which induce optimality.

In this strategy, a random sample ( q = U(C)) is generated. If the sample is collision-prone ( q∈C obs), it is promoted to the nearest free configuration space using (2) to generate a new sample \(\phantom {\dot {i}\!}q_{o}^{free} \). The sample (\(\phantom {\dot {i}\!}q_{o}^{free} )\) is used to attract the roadmap towards itself. The nearest vertex in the roadmap ( v 1) is extended towards the sample to create a vertex ( c), which is added as per the necessary admissibility criterions. This is another way of expanding the roadmap to other areas. The expansion methodology is the same as that of the RRT and hence a detailed discussion is avoided.

The strategies are implemented with probabilities p κ , p L , p Ψ and p R such that p κ + p L + p Ψ + p R =1. Initially, if the roadmap has disconnected components, the first strategy is dominant. However once the entire roadmap has been connected or some disconnected components have reached the failed attempts threshold, this strategy simply vanishes. The other strategies are for utilizing the excess computation, making redundant connections and useful cycles, and trying new ways to connect disconnected components. Accordingly the probabilities may be set. A good strategy will be to adapt these strategies based on the performance parameters. Currently however the probabilities are kept as fixed and the adaptive variants will be studied in the future.

The only other parameters are k and δ, which are rather standard and well-researched in the literature. The parameter δ is similar to the step size parameter of the Rapidly-exploring Random Trees. A common approach is to change k as per the vertices in the roadmap, and similarly to change δ based on the distance from the obstacles. Both are good techniques well researched in the literature, however the parameters are kept constant here due to simplicity.

2.3 Pseudo codes

The algorithm is presented as pseudo codes 1 to 10. The different components correspond to the concepts discussed in Sections 2.1 and 2.2.

figure b
figure c
figure d
figure e
figure f
figure g
figure h
figure i
figure j
figure k

2.4 Robot motion planning

The aim of the current work is to generate a roadmap for a given configuration space, such that nearly all the homotopic groups are discovered. Much research on using the roadmap for the task of motion planning of a single and multiple robots has already been performed by the author (e.g. [30]). A brief summary to motion planning of a single robot is taken in this section for the sake of completeness.

The roadmap produced is used for the motion planning of a robot. It is assumed that the source ( S) and goal ( G) are already known. These are attempted to be connected to the nearest k edges of the roadmap to carry out the search. The extra vertex corresponding to the source and goal, and the corresponding edges are temporarily added in the roadmap. The robot is planned using an A* algorithm [31]. The algorithm finds a path from a given source node to a given goal node by maintaining a fringe of nodes to be explored and a set of processed nodes. The node with the least expected cost to goal is picked from the fringe and added to the set of processed nodes, while all non-explored nodes connected to the node being processed are added to the fringe. Initially the fringe is initialized by addition of the source alone. The algorithm relies upon cost functions. The historic cost indicates the cost of getting to the node from the source, which for this problem is the addition of step costs of all elementary steps. An edge corresponding to every set of nodes is given a step cost equal to the Euclidian distance between the nodes. The heuristic cost estimates the shortest distance possible from the node to the goal, which for this problem is taken as the Euclidian distance to goal. The total cost is the sum of historic and heuristic costs.

Post-processing [32] is commonly applied to the outputs of the A* algorithm. Post-processing mechanisms use a local search to reduce the path length, increase clearance and increase smoothness of the path. Since a local search is utilized, post-processing is not very time consuming and a few iterations can drastically remove the pitfalls of the A* algorithm path. Since the aim of the current work is primarily to produce a good roadmap, post-processing is not applied to the final path to show the raw results using the roadmap alone. A small post-processing can many times result in good paths even for poor roadmaps in scenarios where the final path is rather simplistic.

The chief aim behind the work is to use the homotopic conscious roadmap so generated for the motion planning of multiple mobile robots. The naive methods have already been tried by the author (e.g. [30]). The centralized approach is not realizable for a large number of robots, while a decentralized approach may result in too much congestion around a few areas with the robots not considering the plans of other robots for initial coarser/roadmap level planning. The overall aim is to coordinate the robots in a coarser level planning as was done in [33], the method was specifically designed for a road-like scenario and is not generalizable for open mobile robotic environments. The first step in the direction has been made by capturing all homotopic groups with some redundant connections which do not largely affect the computation time. The next task of using this roadmap for coordination at the coarser/roadmap level planning is a challenge in itself and will be taken in the future research of the author.

3 Results

The algorithm is tested using simulations. The algorithm was tested against a variety of scenarios, only two of those are discussed in detail here to save space. The first scenario is shown in Fig. 5 and comprises of multiple narrow corridors which are hard to discover and connect, along with some wide open spaces.

Fig. 5
figure 5

Results for a synthetic scenario (a) Initial Vertices. Blue vertices are from narrow corridor sampling, Green from obstacle sampling and Red from Uniform Sampling. b Final Roadmap. Connections to neighbouing k vertices are shown by a dotted line while edges by other strategies are shown by bold solid lines. Black nodes and edges denote additional nodes placed in initial edge connectivity strategy. c Path Traced by a robot

Full size image

The algorithm was executed assuming a corridor width D of 50 units. The strategies of narrow corridor sampling, obstacle based sampling and uniform sampling were executing for a maximum of 100, 40 and 15 failed attempts or iterations. The distance threshold Δ was fixed as 15 units while the step size δ was fixed to be 15 units. Maximum failed attempts for connecting disjoint vertices ( η) was kept as 50. The main edge connectivity loop was applied for a maximum of 3000 iterations, with the strategies being called with probabilities p κ =0.4, p L =0.2, p Ψ=0.2, p R =0.2.

Figure 5a shows the initial vertices generated, which are connected using the edge connection strategy shown in Fig. 5b. In both the figures a rectangular robot of a reasonable size is taken and hence the corridors are narrower than they appear. A 3-D configuration space is superimposed on the workspace for viewing. The path traced by the robot is shown in Fig. 5c.

Two metrics are taken to assess the performance of the algorithm. These are: success ratio, which is the ratio of the total number of test cases for which the algorithm can find a path, to the total number of test cases given; and path length ratio, which is the ratio of the path length to the Euclidian distance between the source and the goal averaged for all the test cases. The metrics averaged for 100 test cases are presented in Fig. 6. The figures have been smoothened to a small extent by the use of moving average method. It can be seen that the algorithm quickly converges to a 100 % success rate and near-optimal path length along with increase in the number of iterations. However the computational cost also shows an increase. Even for a challenging scenario, it is possible to get good results in small computational time.

Fig. 6
figure 6

Performance on the scenario (a) Path Length, b Success Rate and c Execution Time v/s Number of Iterations

Full size image

The performance of the algorithm is compared with 2 techniques: obstacle based sampling and uniform sampling technique. The only performance criterion common to the 3 algorithms is the computational cost. The 3 algorithms were executed a number of times by increasing the number of iterations (proposed algorithm) or the roadmap size (others). The success rate and path length ratio obtained for the approaches is plotted in Fig. 7. It can be seen that the proposed algorithm is significantly better than the other two approaches. Although it initially resulted in poor path lengths, as the focus was primarily narrow corridor discovery, it quickly surpassed the other two algorithms; while also consistently resulting in a higher success ratio.

Fig. 7
figure 7

Comparison of the proposed algorithm with uniform and obstacle based sampling methods

Full size image

An additional test case is presented for the problem and its comparisons are made with the same other approaches to make the claims more convincing. The scenario had a few narrow corridors of varying widths and some open spaces in different parts of the roadmap. The second scenario is shown in Fig. 8. Figure 8a shows the initial vertices, Fig. 8b shows the roadmap hence generated and Fig. 8c shows the motion of a robot for some source and goal. It can be seen that the algorithm was again able to produce a roadmap, discovering all narrow corridors and connecting them with the rest of the roadmap.

Fig. 8
figure 8

Results for a scenario 2 (a) Initial Vertices. b Final Roadmap. c Path Traced by a robot

Full size image

The performance metrics of success rate, path length and computational time are studied to assess the working of the algorithm. The performance metrics for different iterations are shown in Fig. 9. It can be seen that the time to construct a roadmap with respectable path lengths and success ratio is still very small. The success rate converges to the maximum value in a few iterations only, while the path length metric shows a reasonable performance by quickly getting closer to the optimal values.

Fig. 9
figure 9

Performance on scenario 2 (a) Success Rate, b Path Length and c Execution Time v/s Number of Iterations

Full size image

Comparisons with the other methods are shown in Fig. 10. It can be seen that even for this test case, the proposed algorithm is significantly better than the other approaches. Since the scenario had enough open spaces and most of the corridors of respectable widths, some path using a longer homotopic path group was always possible. Hence the success rate does not show much trend among the methods. However the fact that all homotopic groups were not discovered by the other methods is clear from the metric of path length. Not discovering a narrow corridor led to the robot taking a longer path length of a different homotopic group.

Fig. 10
figure 10

Performance on scenario 2 (a) Success Rate, (b) Path Length and (c) Execution Time v/s Number of Iterations

Full size image

4 Conclusions

In this paper a new method was proposed to quickly generate good roadmaps which result in discovering all narrow corridors and conveniently connecting them with the rest of the roadmap. A multi-strategized initial vertex generation and edge connection strategy was proposed, which very quickly resulted in a redundantly connected roadmap discovering all narrow corridors. The later iterations, if available, were used for increasing the optimality and adding good redundant edges. The approach was adjudged better than the uniform and obstacle based sampling using the metrics of success rate and path length over computation time. The proposed method hence acts as a mechanism to quickly compute a homotopy conscious roadmap which has vital use in the applications of motion planning for single and multiple mobile robots, and complex mission planning.

The main strength of the approach is a multi-strategized approach for generation of both the vertices and edges. Each strategy is called with a certain probability, and hence the contributions of the different strategies can be varied. Currently the probabilities are fixed, however considering the basic motivation, it is possible to set good values to the probabilities by intuition. This further gives the capability to add any other mechanism to aid the current set of strategies. The strategies together can cater to a wide variety of scenarios. Further, the strength of the work is in the specific mechanism of generation of samples inside the narrow corridors by traversing from inside the obstacle to the outside, and thereafter placing the sample in the middle of the corridor for better connectivity. The strengths of the approach are clear from the experimental results which showcased clearly a better performance as compared to the other sampling methods.

The major limitation of the work is that spending excessive time in search for narrow corridors and otherwise difficult regions can be wasteful if the scenario is inherently simple. Without investing examination time one cannot determine if the scenario is easy or complex, so there is less that one can do about expenditure of this computation time. Further, the performance of the algorithm largely depends upon the parameters, which are currently static and manually set. Making these parameters adaptive is the solution, however more parameters also need time to get to their optimal values. Continuously growing the roadmap can result in better paths and high success rates, however will make the algorithm computationally expensive in the online query phase. A consciousness to the same needs to be put in. The approach aims to connect different sub-sets of disjoint vertices in different connection attempts. Attempting connection between one pair only with an apt choice of vertex, and thereafter giving high local search time for making the connections as in Sampling based Roadmap of Trees algorithm can be a good alternative. This however can be fatal when the selected pair cannot be connected due to obstacles. The balance between local search attempts and attempting connections between multiple vertex pairs is critical to the approach, and needs to be studied by designing appropriate metrics.

The roadmap construction was the first and most important step towards the overall problem of mission planning of multiple mobile robots. The aim is to use the roadmap to schedule different robots at different parts of the map at different times, much like the routing algorithms in traffic scenarios. The homotopic conscious roadmap gives a basis to formulate coarser level planners and some coordination mechanism for the task. The actual task of planning and coordination will be taken in the future. Further, the task to adapt the roadmap in a dynamic environment, incrementally build the roadmap as the robots move and use the roadmap for multi-robot planning and mission planning tasks needs to be undertaken in the future.