Topological constraints in search-based robot path planning

Abstract

There are many applications in motion planning where it is important to consider and distinguish between different topological classes of trajectories. The two important, but related, topological concepts for classifying manifolds that are of importance to us are those of homotopy and homology. In this paper we consider the problem of robot exploration and planning in Euclidean configuration spaces with obstaclees to (a) identify and represent different homology classes of trajectories; (b) plan trajectories constrained to certain homology classes or avoiding specified homology classes; and (c) explore different homotopy classes of trajectories in an environment and determine the least cost trajectories in each class. We exploit theorems from complex analysis and the theory of electromagnetism to solve the problem 2-dimensional and 3-dimensional configuration spaces respectively. Finally, we describe the extension of these ideas to arbitrary D-dimensional configuration spaces. We incorporate these basic concepts to develop a practical graph-search based planning tool with theoretical guarantees by combining integration theory with search techniques, and illustrate it with several examples.

1 Introduction

1.1 Motivation: homotopy classes of trajectories

Homotopy classes of trajectories arise due to presence of obstacles in an environment. Two trajectories connecting the same start and goal coordinates are in the same homotopy class if they can be smoothly deformed into one another without intersecting any obstacle in the environment, otherwise they are in different homotopy classes. In many applications, it is important to distinguish between trajectories in different homotopy classes, as well as identify the different homotopy classes in an environment (e.g., trajectories that go left around a circle in two dimensions versus right). For example, in order to deploy a group of agents to explore an environment (e.g., for eliminating potential threats, searching for rewards/targets (Schwager et al. 2011), as well as for updating obstacle map of a partially known environment (Bourgault et al. 2002)), an efficient strategy ought to be able to identify the different homotopy classes and deploy one robot in each homotopy class. One may also wish to determine the least cost path for each robot constrained to or avoiding specified homotopy classes. In many problems the notion of visibility is linked intrinsically with homotopy classes. In tracking of uncertain agents in an environment with dynamic obstacles, the ability to deal with occlusions during a certain time frame is important (Zhou et al. 2006). A knowledge of the possible homotopy classes of trajectories that a target can take in the environment when it is occluded can help more efficient belief propagation.

Classification of homotopy classes in two-dimensional workspaces has been studied in the robotics literature using geometric methods (Grigoriev and Slissenko 1998; Hershberger and Snoeyink 1991), probabilistic road-map construction (Schmitzberger et al. 2002) techniques and triangulation-based path planning (Demyen and Buro 2006). In two dimensions, topological information can be extracted using geometric methods by counting the number of times a trajectory intersects boundaries of the obstacles or rays emanating from the obstacles, or informed ways of dividing the free space into cells to keep track of the sequence in which a trajectory visits those cells. To the best of our understanding, none of these methods in literature satisfactorily extend to configuration spaces of dimension higher than two. While in a 2-dimensional configuration space such methods can be used for telling whether or not two trajectories belong to the same homotopy class, efficient planning for least cost trajectories with homotopy class constraints is difficult using such representations even in 2-dimensions. Neither is it possible to efficiently explore/find optimal trajectories in different homotopy classes in an environment. To our knowledge, there has been no prior research on planning trajectories with topological constraints using search-based methods.

In this paper we propose a novel way of classifying and representing homology classes, a close analog of homotopy classes, in two and higher dimensional Euclidean configuration spaces, which are the types of configuration spaces we encounter most often in robot planning problems. For the 2-dimensional case we use theorems from complex analysis for developing a compact way of representing homology classes of trajectories, while for 3-dimensional configuration spaces we exploit theorems from electromagnetism.¹ Finally, we show that the formulae for 2 and 3 dimensional cases can in fact be extended to higher D-dimensional Euclidean configuration spaces with obstacles (Bhattacharya et al. 2011a). This is illustrated with examples in a 4-dimensional configuration space.

The novelty of our work lies in the fact that our proposed representation allows us to identify/distinguish trajectories in different classes and compute least-cost paths in non trivial configuration spaces with topological constraints using graph search-based planning algorithms. The representation we propose is designed to be independent of the type of the environment, the discretization scheme or cost function. Our proposed representation can also be used in configuration spaces with additional degrees of freedom that do not effect homotopy classes of the trajectories (e.g. for unicycle modes of mobile robots, the configuration space consists of variables X, Y and θ. But the last variable, θ, does not effect the homotopy classes of trajectories. Only the projection of the x–y plane is enough to capture the topological information).

Using such a representation we show that topological constraints can be seamlessly integrated with graph search techniques for determining optimal paths subject to constraints. We also discuss how this method can be used to explore multiple homotopy classes in an environment using a single graph search.

1.2 Capturing topological information in search-based planning

In search-based planning algorithms one typically starts by discretizing a given environment to create a graph $\mathcal{G} = (\mathcal{V}, \mathcal{E})$. Starting form an initial vertex, $v_{s} \in \mathcal{V}$, a typical graph-search algorithm expands the nodes of the graph by traversing the edges. Values are maintained and associated with each expanded node that capture the metric information (distance/cost) of shortest path leading to the expanded node from v _s . For example, A* search algorithm maintains two functions $g,f:\mathcal{V}\rightarrow\mathbb{R}$. g(v) is the cost of the current path from the start node to node v, and f(v)=g(v)+h(v) is an estimate of the total distance from start to goal going through v. The algorithm maintains an open set, the set of nodes to be expanded. Each time it expands a node v, it updates the values of g(v′) for each neighbor v′ of v, by adding to g(v) the cost of the edge $c(\overline{vv'})$ (the update happens only if the newly computed value is lower than the previous value). This process continues until a desired vertex $v_{g} \in\mathcal{V}$ is reached (Hart et al. 1968).

The fact that the value of g(v′) can be computed from $g(v) + c(\overline{vv'})$ is due to the fact that the cost function is additive (i.e. if α and β are two curves that share a common end point, then c(α⊔β)=c(α)+c(β), where “⊔” indicates the disjoint union, and represent the total curve formed by the two curves together). This is because the metric information about the underlying space is captured using a differential 1-form (‘quantities’ that can be integrated over 1-dimensional manifolds, the trajectories (Bott and Tu 1982; Talpaert 2000)), namely the infinitesimal length/cost, dl. The cost of an edge, e, of the graph is then computed as an integral of the form $c(e) = \int_{e} \mathcal {J}(l)\mathrm{d}{l}$ (with some scaling function $\mathcal{J}$). This implies, in an arbitrary graph search algorithm, during the expansion of the vertices of the graph, the cost of the shortest path up to a vertex that is being expanded can simply be computed by adding to that of its parent (in terms of sequence of expansions) the cost of the edge connecting to it. This additive property of length/cost is key in developing such graph search algorithms.

While the differential 1-form, $\mathcal {J}(l)~\mathrm{d}{l}$, yields metric information, there are other differential 1-forms that can incorporate other information about the underlying space and can be used for guiding the search algorithm. The main idea in this paper is to determine a differential 1-forms that encodes topological information about the space and let us guide the search accordingly.

1.3 H-signature as class invariants for trajectories

We consider a very general differential 1-form in a given D-dimensional configuration space $\mathcal{C}$. If x ₁,x ₂,…,x _D are the coordinate variables describing the configuration space, a general differential 1-form can be written as dh:=f ₁(x)dx ₁+f ₂(x)dx ₂+⋯+f _D (x)dx _D . Thus, for any given trajectory/curve, τ, in this configuration space, one can compute $\mathcal{H}(\tau) = \int_{\tau} \mathrm{d} {h}$. We call this the H-signature of τ. In Sects. 3.2 and 4.2.2 we will design the differential 1-forms, and hence the H-signature of a trajectory, for the 2- and 3-dimensional configuration spaces respectively, such that they are invariants for homology classes of trajectories.

We want to design the 1-form dh and the H-signature of a trajectory such that it is an invariant across trajectories in the same homotopy class. However, because we use 1-forms and their integrals along closed curves to classify trajectories, we naturally obtain invariants for homology classes of trajectories (Hatcher 2001; Rotman 1988; Bott and Tu 1982). But in most practical robotics problems the notion of homology and homotopy of trajectories can be used interchangeably, especially when finding the least cost path. This is discussed in greater detail with examples in Sects. 5.1 and 6.

2 Homotopy and homology classes of trajectories

Definition 1

(Homotopic trajectories)

Two trajectories τ ₁ and τ ₂ connecting the same start and end coordinates, x _s and x _g respectively, are homotopic iff one can be continuously deformed into the other without intersecting any obstacle.

Formally, if $\tau_{1}: [0,1]\rightarrow\mathcal{C}$ and $\tau_{2}: [0,1]\rightarrow\mathcal{C}$ represent the two trajectories (with τ ₁(0)=τ ₂(0)=x _s and τ ₁(1)=τ ₂(1)=x _g ), then τ ₁ is homotopic to τ ₂ iff there exists a continuous map $\eta: [0,1]\times[0,1] \rightarrow\mathcal{C}$ such that η(α,0)=τ ₁(α) ∀α∈[0,1], η(β,1)=τ ₂(β) ∀β∈[0,1], and η(0,γ)=x _s ,η(1,γ)=x _s ∀γ∈[0,1]. Alternatively, in the notation of Hatcher (2001), τ ₁ and τ ₂ are homotopic iff τ ₁⊔−τ ₂ belongs to the trivial class of the first homotopy group of $\mathcal {C}$, denoted by $\pi_{1}(\mathcal{C})$. That is, $[\tau_{1} \sqcup-\tau_{2}] = \mathbf{0} \in\pi_{1}(\mathcal{C})$.

Definition 2

(Homologous trajectories)

Two trajectories τ ₁ and τ ₂ connecting the same start and end coordinates, x _s and x _g respectively, are homologous iff τ ₁ together with τ ₂ (the later with opposite orientation) forms the complete boundary of a 2-dimensional manifold embedded in $\mathcal{C}$ not containing/intersecting any of the obstacles.

Formally, in the notation of Hatcher (2001), τ ₁ and τ ₂ are homologous iff τ ₁⊔−τ ₂ belongs to the trivial class of the first homology group of $\mathcal{C}$, denoted by $H_{1}(\mathcal{C})$. That is, $[\tau_{1} \sqcup-\tau_{2}] = \mathbf{0} \in H_{1}(\mathcal{C})$.

A set of homotopic trajectories form a homotopy class, while a set of homologous trajectories form a homology class.

At an intuitive level the above two definitions may appear equivalent. For example, in Fig. 1(a), τ ₁ is homotopic to τ ₂ since one can be continuously deformed into the other via a sequence of trajectories marked by the dashed curves. As a consequence, the area swept by this continuous deformation, A, forms a 2-dimensional region in the free configuration space whose boundary is the closed loop τ ₁⊔−τ ₂. Indeed, the one-way implication is true as shown below.

Fig. 1

Illustration of homotopy and homology equivalences. In this example τ ₁ and τ ₂ are both homotopic as well as homologous

Lemma 1

If two trajectories are homotopic, they are homologous.

Proof

This follows directly from the Hurewicz theorem (Hatcher 2001) that guarantees the existence of an homomorphism from the homotopy groups to the homology groups of an arbitrary space. □

The converse of Lemma 1 does not always hold true. There are subtle difference between homology and homotopy in spite of their similar notions, and one can create examples where two trajectories are not homotopic in spite of being homologous.

Homotopy equivalence arises naturally in many robotics problems. On the other hand, homology is less natural. However, it is much simpler to compute homologies. One can establish direct correspondence between homology groups of trajectories and differential 1-forms whose integrals yield homology class invariants for trajectories via the De Rham theorem (Bott and Tu 1982). Since, according to the discussion of Sect. 1.2, we desire such differential forms, the rest of the paper will be developed with homology classes of trajectories under consideration rather than their homotopy classes. The assumption will be that in many of the practical robotics problems where homotopy classes of trajectories are of greater significance, homology classes of trajectories will serve as a fair analog. We will justify this claim in Sect. 5.1 and through experimental results (Sect. 6).

To clarify the distinction between homotopy equivalence and homology equivalence of trajectories, we present two examples where homology is not same as homotopy. The first example is in 2-dimensions. In Fig. 2(a) we observe that the trajectories τ ₁ and τ ₂ are not homotopic, but they are homologous (since their H-signatures, as defined in Sect. 3.2, are equal). This is seen perhaps more easily by considering the interior defined by the union of the areas marked by A ₁ and A ₂ which indeed forms the boundary for τ ₁⊔−τ ₂. In Fig. 2(b), one can observe that the two trajectories are not homotopic. However, they are homotopic if we only consider S ₁ or S ₂ but not both. Hence their H-signatures are the same (i.e. they are homologous). Thus, if we were exploring different homotopy classes in this environment using the described method, we would be finding one trajectory for these two homotopy classes.

Fig. 2

Examples where the trajectories are homologous, but not homotopic

3 H-signature in 2-dimensional Euclidean configuration space

We consider a 2-dimensional subset of ℝ² as the configuration space. The obstacles are thus punctures or discontinuities in that subset. The approach for designing a H-signature for such a 2-dimensional configuration space is based on theorems from Complex Analysis, specifically the Cauchy Integral theorem and Residue theorem.

3.1 Background: complex analysis

Cauchy Integral Theorem

The Cauchy Integral Theorem states that if f:ℂ→ℂ is an holomorphic (analytic) function in some simply connected region $\mathcal{R} \subset\mathbb{C}$, and γ is a closed oriented (i.e. directed) contour completely contained in $\mathcal{R}$, then the following holds,

$$ \oint_{\gamma} f(z) \mathrm{d}{z} = 0 $$

(1)

Moreover, if z ₀ is a point inside the region enclosed by γ, which has an anti-clockwise (or positive) orientation, then for the function F(z)=f(z)/(z−z ₀) with a simple pole at z ₀, the following holds

$$ \oint_{\gamma}\frac{f(z) \mathrm {d}{z}}{z - z_0} = 2 \pi i f(z_0) $$

(2)

The Residue Theorem

A direct consequence of the Cauchy Integral Theorem, the Residue Theorem, states that, if $F:\mathcal{R}\rightarrow\mathbb{C}$ is a function defined in some simply connected region $\mathcal{R} \subset\mathbb{C}$ that has simple poles at the distinct points $a_{1},a_{2},\ldots,a_{M} \in \mathcal{R}$, and holomorphic (analytic) everywhere else in $\mathcal{R}$, and say γ is a closed positively oriented Jordan curve completely contained in $\mathcal{R}$ and enclosing only the points $a_{k_{1}},a_{k_{1}},\ldots,a_{k_{m}}$ out of the poles of F, then the following holds,

$$ \oint_{\gamma} F(z) \mathrm{d}{z} = 2 \pi i \sum _{l=1}^m \lim_{\xi \rightarrow a_{k_l}} (\xi- a_{k_l}) F(\xi) $$

(3)

The scenario is illustrated in Fig. 3(b).

Fig. 3

Cauchi Integral Theorem and Residue Theorem

It is important to note that in both the Cauchy Integral Theorem and the Residue Theorem the value of the integrals are independent of the exact choice of the contour γ as long as the mentioned conditions are satisfied (see Fig. 3(a)).

3.2 Designing a H-signature

We exploit the above theorems for designing a differential 1-form that can be used to construct a homology class invariant for 2-dimensional configuration space.

We start by representing the 2-dimensional configuration space as a subset of the complex plane ℂ. Thus a point in the configuration space, $(x,y)\in\mathcal{C}$, is represented as x+iy∈ℂ. The obstacles are assumed to be simply-connected regions in ℂ and are represented by $\mathcal{O}_{1}, \mathcal{O}_{2}, \ldots, \mathcal{O}_{N}$.

Construction 1

(Representative points)

We define one “representative point” in each connected obstacle such that it lies in the interior of the obstacle. The exact location of the representative points is not of particular significance as long as they each lie inside the respective obstacles. Thus we define the points $\zeta_{l}\in \mathcal{O}_{l}$, ∀l=1,…,N. Figure 4(a) shows such representative points inside three obstacles.

Fig. 4

Two trajectories in same and different homotopy classes

Definition 3

(Obstacle Marker Function)

For a given set of “representative points”, we define the “Obstacle Marker Function” function $\mathcal{F}: \mathbb{C}\rightarrow \mathbb{C}^{N}$ as follows,

$$ \mathcal{F}(z) = \left [ \begin{array}{c} \frac{f_1(z)}{z-\zeta_1} \\[5pt] \frac{f_2(z)}{z-\zeta_2} \\ \vdots\\ \frac{f_N(z)}{z-\zeta_N} \end{array} \right ] $$

(4)

where f _l , l=1,2,…,N are analytic functions over entire ℂ such that f _l (ζ _l )≠0, ∀l. Typical examples of such f _l are polynomials in z.

Thus, $\mathcal{F}$ is a complex vector function, the l ^th component of which has a single simple pole/singularity at ζ _l .

Definition 4

(H-signature in 2-dimensional configuration space)

For the given configuration space and set of obstacles, we define the obstacle marker function as described above, and hence define the H-signature of a trajectory τ the vector function $\mathcal {H}_{2}:C_{1}(\mathbb{C})\rightarrow\mathbb{C}^{N}$

$$\mathcal{H}_2(\tau) = \int_{\tau}\mathcal{F}(z) \mathrm{d}{z} $$

where C ₁(ℂ) is the set of all curves/trajectories in ℂ.

It is to be noted that the value of the H-signature of a trajectory in the 2-dimensional configuration space is simply a vector of N complex numbers.

Lemma 2

Two trajectories τ ₁ and τ ₂ connecting the same points in the described 2-dimensional configuration space are homologous if and only if $\mathcal{H}_{2}(\tau_{1}) = \mathcal{H}_{2}(\tau_{2})$.

Proof

We note that by changing the orientation of a path over which an integration is being performed, we change the sign of the integral. If τ is a path, its oppositely oriented path is represented as −τ. Thus, as we see from Fig. 4(a), τ ₁ along with −τ ₂ forms a positively oriented closed loop.

If τ ₁ and τ ₂ are in the same homology class, the area enclosed by τ ₁ and τ ₂ does not contain any of the “representative points”, ζ _i , hence rendering the function $\mathcal{F}$ analytic in that region. Hence from the Cauchy Integral Theorem we obtain,

where the 0 in bold implies that it is a N-vector of zeros.

If τ ₁ and τ ₂ are in different homology classes, we can easily note that the closed positive contour formed by τ ₁ and −τ ₂ will enclose one or more of the obstacles, and hence their corresponding “representative points”. This is illustrated in Fig. 4(b). Let us assume that enclosed “representative points” are $\zeta_{\kappa_{1}},\zeta_{\kappa _{2}},\ldots ,\zeta_{\kappa_{n}}$. Moreover we note that at least one component of the vector function $\mathcal{F}$ has a simple pole at ζ _l for each l=1,2,…,N. Thus, by the Residue Theorem and Definition 3,

Hence proved. □

We have hence shown that $\mathcal{H}_{2}$ gives a homology invariant for trajectories in 2-dimensional Euclidean configuration space with obstacles.

3.3 Computation for a line segment

As discussed earlier in Sect. 1.2, and will be discussed later in Sect. 5, we discretized the given configuration space and create a graph out of it. In many practical implementations we assume that every edge in the graph is a line segment. Thus it is for those line segments that we really need to compute the H-signatures. Thus it is important that we are able to do so efficiently. In this section we will show how to compute the H-signature for a small line segment in a 2-dimensional configuration space using a closed-form formula.

Given a line segment e connecting points z ₁ and z ₂, we can parametrize the segment using the variable z=(1−λ)z ₁+λz ₂, where λ∈[0,1] is the parameter. Thus we have,

(5)

For computing the H-signature of e={z ₁→z ₂} analytically, we assume that f _l are chosen to be constants. Let f _l =A _l  (const.) for all l=1,2,…,N.

Now, a standard integration result gives for the l ^th component of $\mathcal{H}_{2}(e)$

However we note that the logarithm of a complex number does not have an unique value. For any z′∈ℂ, ln(z′)=ln(|z′|e ^{i(arg(z′)+2kπ)})=ln(|z′|)+i(arg(z′)+2kπ), ∀k=0,±1,±2,… (where arg(x+iy)=atan2(y,x)). Hence, following the assumption that e is a small line segment, we choose the smallest of all the possible values over different k’s. Thus, the l ^th component of $\mathcal{H}_{2}(e)$ is computed as,

$$\begin{aligned} &A_l \bigl[ \ln\bigl(|z_2-\zeta_l|\bigr) - \ln\bigl(|z_1-\zeta_l|\bigr) \\ &\quad{}+ i \operatorname{absmin}_{k\in\mathbb{Z}} \bigl( \arg(z_2- \zeta_l) - \arg(z_1-\zeta_l) + 2 k\pi\bigr) \bigr] \end{aligned} $$

where $\operatorname{absmin}_{k\in\mathbb{Z}} G(k)$ returns the value of G(k) that has the minimum absolute value (i.e. closest to 0) over all k∈ℤ. Typically, we can get away with checking a few values of k around 0 and picking the local minimum, since the value of arg(z ₂−ζ _l )−arg(z ₁−ζ _l )+2kπ is monotonic in k.

4 H-signature in 3-dimensional Euclidean configuration space

While in the two-dimensional case, theoretically any finite obstacle on the plane can induce multiple homotopy and homology classes for trajectories joining two points, the notion of homotopy/homology classes in three dimensions can only be induced by obstacles with genus ² one or more, or with obstacles stretching to infinity. Figure 6 shows some examples of obstacles that can or cannot induce such classes for trajectories. A sphere or a solid cube, for example, cannot induce multiple homotopy classes in an environment.

4.1 Background: electromagnetism

Biot-Savart law

Consider a single hypothetical current-carrying curve (a current conducting wire) embedded in a 3-dimensional space carrying a steady current of unit magnitude (Fig. 5(a)). There is no source for the current nor any sink—only a steady flow persisting inside the conductor due to absence of any dissipation. It is to be noted that such a steady current is possible iff the curve is closed (or open, but extending to infinity, where we close the curve using a loop at infinity. See Fig. 7(a) and Construction 2). We denote the curve by $\mathcal{S}$. Then, according to the Biot-Savart Law (Griffiths 1998), the magnetic field B at any arbitrary point r in the space, due to the current flow in $\mathcal{S}$, is given by,

$$ \mathbf{B}(\mathbf{r}) = \frac{\mu_0}{4 \pi} \int_{\mathcal{S}} \frac {(\mathbf{x}-\mathbf{r}) \times\mathrm{d}\mathbf{x} }{\| \mathbf {x}-\mathbf {r} \|^3} $$

(6)

where, x, the integration variable, represents the coordinate of a point on $\mathcal{S}$, and dx is an infinitesimal element on $\mathcal{S}$ along the direction of the current flow.

Fig. 5

Theorems from electromagnetism, and their application in defining H-signature in 3-dimensions

Ampere’s law

While Biot-Savart law gives a recipe for computing the magnetic field from a given current configuration, Ampere’s Law (Griffiths 1998), in a sense, gives the inverse of it. Given the magnetic field B at every point in the space, and a closed loop γ (Fig. 5(a)), the line integral of B along γ gives the current enclosed by the loop γ. That is,

$$ \varXi(\mathcal{C}) := \int_{\gamma} \mathbf{B}(\mathbf{l}) \cdot \mathrm{d}\mathbf{l} = \mu_0 I_{encl} $$

(7)

where, l, the integration variable, represents the coordinate of a point on γ, and dl is an infinitesimal element on $\mathcal{C}$.

In Biot-Savart Law and Ampere’s Law one can conveniently choose the constant μ ₀ to be equal to 1 by proper choice of units. Moreover, by choice, the value of the current flowing in the conductor is unity. Thus, for any closed loop γ, the value of Ξ(γ) is zero iff γ does not enclose the conductor, otherwise it is ±1 (the sign depends on the direction of integration performed on γ). Thus in Fig. 5(a), Ξ(γ ₁)=1 and Ξ(γ ₂)=0.

Definition 5

(Simple Homotopy-Inducing Obstacle in 3-dimensional Configuration Space)

A Simple Homotopy-Inducing Obstacle (SHIO) is a bounded obstacle of genus 1, for example a torus (Figs. 6(a), 6(b)) or a knot (Fig. 6(e)).

Fig. 6

Examples of obstacles in 3-D. (a)–(e) induce homotopy classes, (f) does not

4.2 Designing a H-signature

For the 2-dimensional case, each obstacle on the plane that induces the notion of multiple homotopy classes was assigned a representative point. Analogously, for the 3-dimensional case, we need to define a skeleton for every SHIO. Intuitively, a skeleton of a 3-dimensional obstacle is a 1-dimensional curve that is completely contained inside the obstacle such that the surface of the obstacle can be “shrunk” onto the skeleton in a continuous fashion without altering the topology of the surface of the obstacle. Formally, we define the skeleton of an obstacle in terms of homotopy equivalence.

Definition 6

(Skeleton)

A 1-dimensional manifold, S, is called a skeleton of a SHIO, $\mathcal{O}$, iff S is homeomorphic to $\mathbb{S}^{1}$ (a circle), S is completely contained inside $\mathcal{O}$, and if S and $\mathcal{O}$ are homotopy equivalent.

Thus, the fact that τ ₁ and τ ₂ are of the same or of different homotopy/homology classes is not altered by replacing $\mathcal{O}$ by S.

In the literature, algorithms for constructing skeletons of solid objects is a well-studied (Blum 1967; Jain 1989). However in the present context we have a much relaxed notion of skeleton. While we can adopt any of the different existing algorithms for automated construction of skeleton from a 3-dimensional obstacles, this discussion is out of the scope of the present work. Figure 6(a) demonstrates skeletons for several genus 1 obstacles.

4.2.1 Conversion of generic obstacles into SHIOs

Given a set of obstacles in a three-dimensional environment, we perform the following two constructions/reduction on the obstacles so that the only kind of obstacle we have in the environment are Simple Homotopy-Inducing Obstacles.

Construction 2

(Closing infinite, unbounded obstacles)

In most of the problems that we are concerned with, the domain in which the trajectories of the robots lie is finite and bounded. This gives us the freedom of altering/modifying the obstacles or parts of obstacles lying outside that domain without altering the problem. One consequence of this freedom is that we can close infinite and unbounded obstacles (e.g. Fig. 6(d)) at a large distance from the domain of interest (Fig. 7(a)).

Fig. 7

Illustration of Constructions 2 and 3

Construction 3

(Decomposing obstacles with genus >1)

After closing all infinite, unbounded obstacles in an environment according to Construction 2, if there is an obstacle with genus k (e.g. Fig. 6(c)), we can decompose/split it into k obstacles, possibly overlapping and touching each other, but each with genus 1 (Fig. 7(b)). This does not change the obstacles or the problem in any way. This construction just changes the way we identify obstacles and construct their skeletons. For example in Fig. 7(b) the original obstacle $\mathcal{O}$ with genus 2 is realized as two obstacles $\mathcal{O}_{1}$ and $\mathcal{O}_{2}$, each with genus 1 and overlapping each other. The decomposition of obstacles into SHIOs allows us define k skeletons for each obstacle of genus k and simplify computations of h-signatures of trajectories.

Note that in this paper we do both constructions manually—the automation of these steps is beyond the scope of this paper.

4.2.2 Skeleton of SHIOs as current carrying curves for H-signature construction

Construction 4

(Modeling skeleton of a SHIO as a current carrying manifold)

Given m obstacles in an environment, $\mathcal{O}_{1}, \mathcal{O}_{2}, \dots, \mathcal{O}_{m}$, with genus k ₁,k ₂,…,k _m respectively, we can construct M=k ₁+k ₂+⋯+k _m skeletons from M SHIOs (obtained using Constructions 2 and 3), namely S ₁,S ₂,…,S _M . Each S _i is a closed, connected, boundary-less 1-dimensional manifold. We model each of them as a current-carrying conductor carrying current of unit magnitude (Figs. 6(a), 7(a)). The direction of the currents is not of importance, but by convention, each is of unit magnitude.

Definition 7

(Virtual Magnetic Field due to a Skeleton)

Given S _i , the skeletons of a Simple Homotopy-Inducing Obstacle, we define a Virtual Magnetic Field vector at a point r in the space due to the current in S _i using Biot-Savart Law as follows,

$$ \mathbf{B}_i(\mathbf{r}) = \frac{1}{4 \pi} \int _{S_i} \frac {(\mathbf {x}-\mathbf{r}) \times\mathrm{d}\mathbf{x} }{\| \mathbf{x}-\mathbf {r} \|^3} $$

(8)

where, x, the integration variable, represents the coordinates of a point on S _i , and dx is an infinitesimal element on S _i along the chosen direction of the current flow in S _i .

Definition 8

(H-signature in 3-dimensional Configuration Space)

Given an arbitrary trajectory, τ, in the 3-dimensional environment with M skeletons, we define the H-signature of τ to be the function $\mathcal{H}_{3}: C_{1}(\mathbb{R}^{3}) \rightarrow \mathbb{R}^{M}$,

$$ \mathcal{H}_3(\tau) = \bigl[h_1(\tau),\ h_2( \tau),\ \dots,\ h_M(\tau) \bigr]^T $$

(9)

where, C ₁(ℝ³) is the space of all curves/trajectories in ℝ³, and

$$ h_i(\tau) = \int _{\tau} \mathbf{B}_i(\mathbf{l}) \cdot\mathrm{d}\mathbf{l} $$

(10)

is defined in an analogous manner as the integral in Ampere’s Law. In defining h _i , B _i is the Virtual Magnetic Field vector due to the unit current through skeleton S _i , l is the integration variable that represents the coordinate of a point on τ, and dl is an infinitesimal element on τ.

It is to be noted that the value of the H-signature of a trajectory in the 3-dimensional configuration space is simply a vector of M real numbers.

Lemma 3

Two trajectories τ ₁ and τ ₂ connecting the same points in the described 3-dimensional configuration space are homologous if and only if $\mathcal{H}_{3}(\tau_{1}) = \mathcal{H}_{3}(\tau_{2})$.

Proof

Since τ ₁ and τ ₂ connect the same points, τ ₁⊔−τ ₂, i.e. τ ₁ and −τ ₂ together (where −τ indicates the same curve as τ, but with the opposite orientation) form a closed loop in the 3-dimensional environment (Fig. 5(b)). We replace the obstacles $\mathcal{O}_{1}, \mathcal{O}_{2}, \dots, \mathcal{O}_{m}$ in the environments with the skeletons S ₁,S ₂,…,S _M .

Consider the presence of just the skeleton S _i . By the direct consequence of Ampere’s Law and our construction in which a unit current flows through S _i , the value of

$$h_i(\tau_1 \sqcup-\tau_2) = \int _{\tau_1 \sqcup-\tau_2} \mathbf{B}_i(\mathbf{l}) \cdot\mathrm {d}\mathbf{l} $$

is non-zero if and only if the closed loop formed by τ ₁⊔−τ ₂ encloses the current carrying conductor S _i (i.e. there does not exist a surface not intersecting S _i , the boundary of which is τ ₁⊔−τ ₂). For example, in Fig. 5(b), h _p (τ ₁⊔−τ ₂)=1 and h _q (τ ₁⊔−τ ₂)=0. Now, by the definition of line integration we have the following identity,

(11)

Thus, h _i (τ ₁)=h _i (τ ₂) if and only if the closed loop formed by τ ₁ and τ ₂ does not enclose S _i (i.e. homologous in presence of S _i ).

Now in presence of skeletons S ₁,S ₂,…,S _M the same argument extends for each skeleton individually. Thus τ ₁ and τ ₂ are homologous if an only if $\mathcal{H}_{3}(\tau_{1}) = \mathcal{H}_{3}(\tau_{2})$. □

Hence we have shown that the proposed formula for H-signature is a homology class invariant for trajectories in 3-D.

4.3 Computation for a line segment

Once again, we are interested in efficient computation of the H-signature for small line segments since those are the ones that will make up edges of the graph formed by discretization of the environment. For all practical applications we assume that a skeleton of an obstacle, S _i , is made up of finite number (n _i ) of line segments: $S_{i} = \{ \overrightarrow{\mathbf{s}_{i}^{1} \mathbf{s}_{i}^{2}}, \overrightarrow{\mathbf{s}_{i}^{2} \mathbf{s}_{i}^{3}}, \dots, \overrightarrow{\mathbf{s}_{i}^{n_{i} -1} \mathbf{s}_{i}^{n_{i}}}, \overrightarrow{\mathbf{s}_{i}^{n_{i}} \mathbf{s}_{i}^{1}} \}$ (Fig. 8(a)). Thus, the integration of (8) can be split into summation of n _i integrations,

$$ \mathbf{B}_i(\mathbf{r}) = \frac{1}{4 \pi} \sum _{j=1}^{n_i} \int_{\overrightarrow{\mathbf{s}_i^j \mathbf{s}_i^{j'}}} \frac{(\mathbf{x}-\mathbf{r}) \times\mathrm{d}\mathbf{x} }{\| \mathbf {x}-\mathbf{r} \|^3} $$

(12)

where j′≡1+(jmodn _i ). It is to be noted that a skeleton of an unbounded obstacle created from Construction 2 can be made up of finite and few line segments. The only feature of such a skeleton might be that some of the points that make up the line segments ($\mathbf{s}_{i}^{j}$) might be located at a large distance from the domain of interest, which is used to close the skeleton.

Fig. 8

Closed-form, analytic computation of virtual magnetic field vector

One advantage of this representation of skeletons is that for the straight line segments, $\overrightarrow{\mathbf{s}_{i}^{j} \mathbf{s}_{i}^{j'}}$, the integration can be computed analytically. Specifically, using a result from (Griffiths 1998) (also, see Fig. 8(b)),

(13)

where, d,p and $\mathbf{p'}$ are functions of $\mathbf{s}_{i}^{j}, \mathbf{s}_{i}^{j'}$ and r (Fig. 8(b)), and can be expressed as,

$$ \mathbf{p} = \mathbf{s}_i^j - \mathbf{r},\quad \mathbf{p'} = \mathbf{s}_i^{j'} - \mathbf{r},\quad \mathbf{d} = \frac{(\mathbf {s}_i^{j'} - \mathbf{s}_i^j) \times( \mathbf{p} \times \mathbf{p'} ) }{\| \mathbf{s}_i^{j'} - \mathbf{s}_i^j \|^2} $$

(14)

We define and write $\boldsymbol {\Phi}(\mathbf{s}_{i}^{j}, \mathbf{s}_{i}^{j'}, \mathbf{r})$ for the RHS of (13) for notational convenience. Thus we have,

$$ \mathbf{B}_i(\mathbf{r}) = \frac{1}{4 \pi} \sum _{j=1}^{n_i} \boldsymbol {\Phi}\bigl(\mathbf{s}_i^j, \mathbf{s}_i^{j'}, \mathbf{r}\bigr) $$

(15)

where, j′≡1+(jmodn _i ).

Given a small line segment, e, we can now compute the H-signature, $\mathcal{H}(e) = [h_{1}(e),~h_{2}(e),~\dots,~h_{M}(e)]^{T}$, where,

$$ h_i(e) = \frac{1}{4 \pi} \int_{e} \sum_{j=1}^{n_i} \boldsymbol {\Phi}\bigl(\mathbf{s}_i^j, \mathbf{s}_i^{j'}, \mathbf{l}\bigr) \cdot\mathrm{d}\mathbf{l} $$

(16)

can be computed numerically. For the numerical integration, in all our experimental results we used the GSL (GNU Scientific Library), which has a highly efficient implementation of adaptive integration algorithms with desired precision. We used a cache for storing the H-signature of edges that has been computed in order to avoid re-computation.

4.4 ‘Looping’ and ‘non-looping’ trajectories

“Looping” of a trajectory around an obstacle (Fig. 9(a)) is similar in essence to non-Jordan curves on two-dimensional planes. However in three dimensions their precise definition is difficult. In fact, the notions of looping and non-looping is imprecise when the skeleton of the obstacle is complex as a knot (Fig. 9(b)). However, equipped with the definition of H-signature, we propose the following definition. A more elaborate discussion on this can be found in Bhattacharya et al. (2011b).

Fig. 9

Looping vs. non-looping trajectories

Definition 9

(Non-looping trajectory w.r.t. S _i )

A trajectory τ is said to be non-looping w.r.t. S _i if h _i (τ)∈(−1,1). The value is in [0,1) if the trajectory goes around S _i in accordance with the right-hand rule with thumb pointing along the direction of the current in S _i . If the direction is opposite, the value lies in (−1,0].

This definition, in many cases, conform to our general intuition of non-looping trajectories. A natural consequence of this definition is the notion of a trajectory that is in a “Complementary Class” of a non-looping trajectory, i.e. one that goes on the opposite side of every obstacle.

Definition 10

(Complementary Homotopy Class)

Given a trajectory τ that is non-looping w.r.t. all the skeletons in the environment (i.e. h _i (τ)∈(−1,1) ∀i=1,2,…,M), we define the Complementary Homotopy Class of the homotopy class of τ to be the one for which the h-signature is $\mathcal{H}(\tau) - \operatorname {sign}(\mathcal{H}(\tau))$, where $\operatorname {sign}(\cdot)$ gives the vector of signs of the elements of its input vector.

5 H-signature augmented graph

Once we have the means of computing H-signature for each edge (small line segments), we introduce the concept of H-signature augmented graph. Typically, a graph $\mathcal{G} = \{\mathcal{V}, \mathcal{E}\}$ is created for the purpose of graph-search based planning by discretization of an environment, placing a vertex at each discretized cell, and by connecting the neighboring cells with edges (see Fig. 10 for an example in 2-dimensional configuration space). In the following discussion we perform a construction without distinguishing between 2 and 3-dimensional configuration spaces explicitly, once we have discretized the environment, and perform a general treatment with the graph $\mathcal {G}$. For H-signature trajectories or line segments we use the generic function $\mathcal{H}$, which we understand to be $\mathcal {H}_{2}$ or $\mathcal{H}_{3}$ depending on the dimensionality of the configuration space.

Fig. 10

Graph formed by uniform discretization of configuration space, followed by placing nodes in the accessible cells and connecting each node with its neighbors. Dark cells are the inaccessible ones occupied by obstacles

Let v _s be the start coordinate in the configuration space, and v _g be the goal coordinate. By Lemma 2 or 3, any two trajectories from v _s to v that belong to the same homology class will have the same H-signature. The H-signature can assume different, but discrete values corresponding on the class of the trajectory. We also write $\mathcal{P}(\mathbf{v}_{s},\mathbf{v})$ to denote the set of all trajectories from v _s to v, and $\widetilde {\mathbf{v}_{s} \mathbf{v}} \in\mathcal{P}(\mathbf{v}_{s},\mathbf {v})$ to denote a particular trajectory in that set.

Definition 11

(Allowed and Blocked Homology Classes)

Suppose it is required that we restrict all our search for trajectories connecting v _s and v _g to certain homology classes, or not allow some other. We denote the set of allowed H-signatures of trajectories leading up to v _g by the set $\mathcal{A}$, and the set of blocked H-signatures as $\mathcal{B}$. $\mathcal{A}$ and $\mathcal{B}$ are essentially complement of each other ($\mathcal{A}\cup\mathcal{B}=\mathcal{U}$, where the universal set, $\mathcal{U}$, is the set of the H-signatures of all the classes of trajectories joining v _s and v _g ), and $\mathcal{B}$ can be an empty set when all classes are allowed.

We define the H-signature augmented graph of $\mathcal{G}$ as the graph $\mathcal{G}_{H}(\mathcal{G}) = \{\mathcal{V}_{H}, \mathcal {E}_{H}\}$, such that each node in this new graph has the H-signature of a trajectory leading up to the coordinate of the node from v _s appended to it. That is, each node in this augmented graph is given by $\{\mathbf{v}, \mathcal{H}(\widetilde{\mathbf{v}_{s}\mathbf {v}})\}$, for some $\widetilde{\mathbf{v}_{s}\mathbf{v}} \in\mathcal {P}(\mathbf{v}_{s},\mathbf{v})$. Thus, corresponding to a given $\mathbf{v}\in\mathcal{V}$, since there are discrete homology classes of trajectories from v _s to v, there are a discrete number of the augmented states, $\{ \mathbf{v},\mathbf{h}\} \in\mathcal{V}_{H}$, where h is a M-vector of reals (M being the number of representative points or the number of SHIOs depending on whether it’s a 2 or 3-dimensional configuration space) and assumes the values of the H-signatures corresponding to the discrete homology classes. Thus, we define the H-signature augmented graph of $\mathcal{G}$ as follows,

$$\mathcal{G}_H = \{ \mathcal{V}_H, \mathcal{E}_H \} $$

where,

• 1.
  $$\mathcal{V}_H = \left \{ \{\mathbf{v},\mathbf{h}\} \left \vert \begin{array}{l} \mathbf{v} \in\mathcal{V}, \text{ and}, \\ \mathbf{h} = \mathcal{H}(\widetilde{\mathbf{v}_s\mathbf{v}}) \text{ for some trajectory}\\ \quad\widetilde{\mathbf{v}_s\mathbf{v}}\in\mathcal {P}(\mathbf{v}_s,\mathbf{v}), \text{ and}, \\ \mathbf{h} \in\mathcal{A}\ (\text{equivalently, } \mathbf{h} \notin\mathcal{B}) \\ \quad\text{when } \mathbf{v}=\mathbf{v}_g \end{array} \right . \right \} $$

• 2. An edge $\{ \{\mathbf{v},\mathbf{h}\} \rightarrow\{ \mathbf {v'},\mathbf{h}'\} \}$ is in $\mathcal{E}_{H}$ for $\{\mathbf {v},\mathbf {h}\} \in\mathcal{V}_{H}$ and $\{\mathbf{v'},\mathbf{h}'\} \in \mathcal {V}_{H}$, iff

  • i. The edge $\{\mathbf{v} \rightarrow\mathbf{v'}\} \in \mathcal {E}$, and,

  • ii. $\mathbf{h}' = \mathbf{h} + \mathcal{H}(\mathbf{v} \rightarrow\mathbf{v'})$, where, $\mathcal{H}(\mathbf{v} \rightarrow \mathbf{v'})$ is the H-signature of the edge $\{\mathbf{v} \rightarrow\mathbf{v'}\} \in\mathcal{E}$.

• 3. The cost/weight associated with an edge $\{ \{\mathbf {v},\mathbf{h}\} \rightarrow\{\mathbf{v'},\mathbf{h}'\} \}$ is same as that associated with edge $\{\mathbf{v} \rightarrow\mathbf {v'}\} \in\nobreak \mathcal{E}$.

The consequence of point 3 in the above definition is that an admissible heuristics for search in $\mathcal{G}$ will remain admissible in $\mathcal{G}_{H}$. That is, if f(v,v _g ) was the heuristic function in $\mathcal{G}$, we define f _H ({v,h},{v _g ,h′})=f(v,v _g ) as the heuristic function in $\mathcal{G}_{H}$ for any $\mathbf {h}'\in\mathcal{A}$.

The consequence of augmenting each node of $\mathcal{G}$ with a H-signature is that now nodes are distinguished not only by their coordinates, but also the H-signature of the trajectory followed to reach it. Typically we use graph search algorithms like A* (or variants like D* or D*-lite) where nodes in the graph $\mathcal{G}_{H}$ are expanded starting from the node {v _s ,0} (where by 0 we mean a M-dimensional vector of zeros).

The topology of this augmented graph for a 2-dimensional case is illustrated in Fig. 11. A goal state v _g is the same in $\mathcal{G}$ irrespective for the path (τ ₁ or τ ₂) taken to reach it. Whereas in the H-signature augmented graph, the states are differentiated by the additional value of h _g . We can perform a graph search in the augmented graph, $\mathcal{G}_{H}$, using any standard graph search algorithm starting from the state {v _s ,0}. The goal state (i.e. the state, upon expansion of which we stop the graph search) is potentially any of the states {v _g ,h _g } for any $\mathbf{h}_{g} \in \mathcal{A}$ (or $\mathbf{h}_{g} \notin\mathcal{B}$ if $\mathcal{B}$ is provided instead of $\mathcal{A}$). We can use the same heuristic that we would have used for searching in $\mathcal{G}$, i.e. f _H (v,h)=f(v). It is to be noted that $\mathcal {G}_{H}$ is essentially an infinite graph, even if $\mathcal{G}$ is finite. However the search algorithm needs to expand only a finite number of states. Since for a given v, the states {v,h} can assume some discrete values of h (corresponding to the different homology classes). To determine if $\{ \mathbf{v},\mathbf{h}'_{g}\}$ and $\{\mathbf{v},\overline{\mathbf {h}}_{g}\}$ are the same states, we can simply compare the values of $\mathbf {h}'_{g}$ and $\overline{\mathbf{h}}_{g}$.

Fig. 11

The topology of the augmented graph, $\mathcal {G}_{H}$ (right), compared against $\mathcal{G}$ (left), for a cylindrically discretized 2-dimensional configuration space around a circular obstacle

5.1 Uses of the H-signature augmented graph

There are primarily two distinct but related ways we would like to use the H-signature augmented graph with search algorithms:

• i. Exploration of environment for different homotopy classes of trajectories connecting v _s and v _g : For this problem, whenever we expand a state $\{ \mathbf{v}_{g}, \tilde {\mathbf{h}} \} \in\mathcal{V}_{H}$, for some $\tilde{\mathbf{h}} \notin \mathcal{B}$, we store the path up to that node, and continue expanding more states until the desired number of classes are explored. Although H-signature is a homology class invariant, and not a homotopy class invariant, by Lemma 1, two trajectories are homotopic implies that they are homologous. Thus, two trajectories that are homotopic will be in the same homology class, and hence their H-signatures will be the same. Thus, in such problems where we find least cost trajectories with different H-signatures in a configuration space using the said method, we are always guaranteed to obtain trajectories in distinct homotopy classes as well.

• ii. Planning with H-signature constraint: For searches with H-signature constraint, we stop upon expansion of a goal coordinate $\{ \mathbf{v}_{g}, \tilde{\mathbf{h}} \}$ for some $\tilde{\mathbf{h}} \notin\mathcal{B}$ (or equivalently, $\tilde {\mathbf{h}} \in\nobreak \mathcal{A}$).

5.2 Theoretical analysis

Theorem 1

If $\mathbf{P}_{H}^{*} = \{\{\mathbf{v}_{1},\mathbf{h}_{1}\}, \{ \mathbf{v}_{2},\mathbf{h}_{2}\}, \ldots, \{\mathbf{v}_{p},\mathbf {h}_{p}\}\}$ is an optimal path in $\mathcal{G}_{H}$, then the path P ^∗={v ₁,v ₂,…,v _p } is an optimal path in the graph $\mathcal{G}$ satisfying the H-signature constraints specified by $\mathcal{A}$ and $\mathcal{B}$.

Proof

By construction of $\mathcal{G}_{H}$, the path {v ₁,v ₂,…,v _p } satisfies the given H-signature constraints. Moreover by definition, $\mathbf {P}_{H}^{*}$ is a minimum cost path in $\mathcal{G}_{H}$. Since the cost function in $\mathcal{G}_{H}$ is the same as the one in $\mathcal{G}$ and does not involve h _j , it follows that the projection of $\mathbf{P}_{H}^{*}$ on $\mathcal{G}$ given by P ^∗={v ₁,v ₂,…,v _p } is an optimal path in the graph $\mathcal{G}$ satisfying the constraints defined in $\mathcal{G}_{H}$. □

6 Results

The method described in this paper was implemented in C++ and MATLAB. In the sections below we present results in 2, 3 and 4-dimensional configuration spaces.

6.1 Two-dimensional configuration space

6.1.1 Path prediction by homotopy class exploration

Figure 12(a) shows a large 1000×1000 discretized environment with circular and rectangular obstacles. We explore trajectories in different classes in order of their path costs using the method ‘i.’ described in Sect. 5.1. The implementation was done in C++ running on an Intel Core 2 Duo processor with 2.1 GHz clock-speed and 4 GB RAM. All the different trajectories in different homotopy classes were determined in a single run of graph search on $\mathcal{G}_{H}$ as described earlier.

Fig. 12

Exploring homotopy classes in 1000×1000 discretized environments to find least cost paths in each

As discussed earlier, in such exploration problems, although we use the H-signature as the class invariants in the search algorithms, since non-homologous trajectories are guaranteed to be non-homotopic, we are guaranteed to obtain trajectories in different homotopy classes.

We also constructed 10 such environments using random circular and rectangular obstacles. Figure 12(c) demonstrate the efficiency of the searches. The time indicates the cumulative time during the search until a shortest-path trajectory in a particular homotopy class is found. This is relevant to problems of tracking dynamic entities, such as people, where one often needs to predict possible paths in order to bias the tracker or to deal with occlusion by anticipating where the dynamic entity will appear. Since people can choose different paths to their destinations, we need to be able to predict least cost paths that lie in different homotopy classes.

6.1.2 H-signature constraint: H-signature defined by key-points

Figure 13(a) demonstrates an example where we define homology classes using a sample (suboptimal) trajectory specified by key-points. One can then compute the H-signature for such a trajectory. It can then be used to search $\mathcal{G}_{H}$ for an optimal path in the same class (or different) as the sample trajectory (Fig. 13(b)).

Fig. 13

Homotopy class constraint determined using suboptimal key-point generated trajectory

Although technically we have imposed homology class constraint by imposing the H-signature constraint, we observe that the optimal trajectory that we obtain is in fact in the same homotopy class as the key-point generated trajectory. In fact we observe that in most robotics planning problems imposing H-signature constraints indeed impose the corresponding homotopy class constraint as well.

6.1.3 Multiple robot visibility problem

The problem of path planning for multiple robots with visibility constraints can also make use of our approach. If one robot needs to plan its path such that it is never obstructed from the view of another robot by some obstacle, we can apply the technique of planning with H-signature constraint to obtain the desired trajectories. In Fig. 14(a)–(c) two robots plan trajectories to their respective goals. The robot on the right needs to plan a trajectory such that it is in the “visibility” of the robot on the left, whose trajectory is given. Thus, in order to determine the H-signature of the desired homotopy class it first constructs a suboptimal path by connecting its own start and goal points to the start and goal of the left robot, such that the trajectory of the left robot is completely contained in it (Fig. 14(b)) as key points. The H-signature of this path gives the desired homology class, thus re-planning with that class as the only allowed class gives the desired optimal plan (Fig. 14(c)).

Fig. 14

100×100 discretized environment with 2 representative points on the central large connected walls

The natural constraint in this situation is that of homotopy. But we once again observe that even imposing the H-signature constraint we do obtain trajectory in the desired homotopy class.

6.1.4 Arbitrary cost functions

Our method is not limited to Euclidean length cost functions. It can deal with arbitrary cost functions. For example, in Fig. 15 there are two large obstacles and a communication base to the left of the environment marked by the bold dotted line, x=0. An agent is supposed to plan its path from the bottom to the top of the environment, while minimizing a weighted sum of the length of the trajectory and the distance of the trajectory from the communication base. Thus, in this case, besides the transition costs of the states in $\mathcal{G}$, each state, $z=x+iy\in \mathcal{G}$, is assigned a cost w⋅x, the penalty on separation from the communication base. Thus the net penalized cost of the trajectory, τ, that is being minimized is of the form c=∫ _τ ds+w∫ _τ x(s)ds, where x is the x-coordinate of the points on the trajectory, parametrized by s, the length of the trajectory. The trajectories in Figs. 15(a) and (b) with penalty weights w=0 and w=0.01 respectively have H-signature of h ₀. Blocking this class, but having a small penalty over distance from communication base gives the trajectory in Fig. 15(d) that passes close to the communication base.

Fig. 15

Planning with non-Euclidean length as cost as well as homotopy class constraint

6.1.5 Implementation using visibility graph

To demonstrate the versatility of the proposed algorithm we implemented it using a Visibility Graph as the state graph, $\mathcal{G}$. Figure 16 shows the visibility graph generated in an environment with polygonal obstacles and the shortest paths in 9 homotopy classes that we explore. Obstacles were inflated in order to incorporate collision safety and circular obstacles were approximated by polygons. Representative points were placed only on the large obstacles (i.e., relevant obstacles that contribute towards the practical notion of homotopy classes, and not, for example, ones created by sensor noise—determined by threshold on diameter and marked by blue circles in the figure) and visibility graph was constructed. A* search was used for searching the H-signature augmented graph. The implementation was made in MATLAB. The average run-time of the search until the 9th homotopy class was explored was 0.4 seconds and about 100 states were expanded.

Fig. 16

Exploring homotopy classes using a Visibility Graph

6.2 Three-dimensional configuration space

The first 3-dimensional domain in which we implement the planning algorithm is the space of 3 spatial dimensions, X,Y and Z. We also demonstrate the algorithm in the 3-dimensional space of X, Y and time, i.e. an environment with planar dynamic obstacles (Sect. 6.2.5).

For a problem in 3 spatial dimensions, the domain of interest is bounded by upper and lower limits of the 3 coordinates. The domain is then uniformly discretized into cubic cells and a node of $\mathcal{G}$ is placed at the center of each cell. Connectivity is established between a node and its 26 neighbors (all cells that share at least one corner, edge or face with it). Each edge is bi-directional and its cost is the Euclidean length.

6.2.1 Simple environments with bounded obstacles

Figure 17(a) demonstrates a simple environment, 20×20×18 discretized, with two torus-shaped obstacles. The skeleton of each obstacle is made up of line segments passing through the central axis of the cylindrical segments. Here we restrict search to non-looping trajectories (see Bhattacharya et al. 2011b for a precise definition). That is, we set $\mathcal{B} = \{ \mathbf{h}=[h_{1},h_{2}]^{T}\mid|h_{1}| > 1 \text{ or } |h_{2}| > 1 \}$. We search for 4 homotopy classes of trajectories connecting a given start and goal coordinate. As shown in Fig. 17(a), the algorithm finds four such trajectories: (i) going through hoops 1 and 2; (ii) going through hoop 1 but not through hoop 2; (iii) going through hoop 2 but not through hoop 1; and (iv) not going through either hoops. According to Theorem 1 each path is the least cost one in the graph and in its respective homotopy class.

Fig. 17

Exploring homotopy classes in X–Y–Z space

Figure 17(b) shows the exploration of 4 homotopy classes in and around a room with windows on each wall. The skeletons for this obstacle are defined as loops around each window according to Construction 3. The trivial shortest path from the given start to goal configuration goes outside the room (the dark violet trajectory). Trajectories in other homotopy classes pass through the room.

6.2.2 Environment with unbounded pipes

Figure 18(a) shows a more complex environment consisting of 7 pipes stretching to infinity. The workspace of choice is 44×44×44 discretized, with the start and goal coordinates at two opposite corners of the discretized space. In Fig. 18(a) we find the least cost paths in 10 different homotopy classes.

Fig. 18

An environment with 7 unbounded pipes

6.2.3 Planning with H-signature constraint

Figure 18(b) demonstrates a planning problem with H-signature constraint. The darker trajectory is the global least cost path found from a search in $\mathcal{G}$ for the given start and goal coordinates. The H-signature for that trajectory was computed, and hence we computed the signature of the complementary class (i.e. the class corresponding to the trajectory that passes on the other side of every SHIO—see Bhattacharya et al. (2011b) for a precise definition), and put only that in $\mathcal{A}$. The lighter trajectory is the one planned with that $\mathcal{A}$ as the set of allowed H-signature. This trajectory goes on the opposite side of each and every pipe in the environment as compared to the darker trajectory.

We note that in this example the notion of complementary homology class concurs with that of complementary homotopy class.

6.2.4 Search speed and efficiency

We now present the running time for the case in Fig. 18(a). The environment, as described earlier, is 44×44×44 discretized, and hence $\mathcal{G}$ contains 85184 nodes. Due to each node being connected to 26 of its neighbors, there are almost 13 times as many edges in $\mathcal{G}$. The program was run on a Intel Core 2 Duo processor with 2.1 GHz clock-speed and 3 GB RAM. We first compute the values of $\mathcal{H}(e)$ for all edges $e\in \mathcal{E}$ and store them in a cache, which takes about 2273 s. Then we perform the A* search in $\mathcal{G}_{H}$, using the values from the cache whenever required. By doing so we eliminate the requirement of re-computing the h-signatures of the edges every time we perform a search, even with changed start and goal coordinates. The search for the 10 homotopy classes in Fig. 18(a) took about 30s and expansion of 521692 nodes in $\mathcal{G}_{H}$. Figure 19 shows the cumulative time required and the number of nodes in $\mathcal{G}_{H}$ expanded.

Fig. 19

Cumulative time taken and number of states expanded while searching $\mathcal{G}_{H}$ for 10 homotopy classes in the problem of Fig. 18(a)

6.2.5 Planning in 2-dimensional plane with moving obstacles

The next 3-dimensional domain that we experiment with is that of the two-dimensional plane, but with dynamic entities. Thus the variables of interest are X,Y and time. The node set was formed by uniform discretization of the domain of interest. The connectivity of the graph is such that the time variable can increase only in the positive direction (each node connected to 9 neighboring nodes in next time step, including the same x & y). The cost of an edge, e, with differences in the coordinates of its end points Δx,Δy and Δt is computed as $c(e) = \sqrt{\Delta x^{2} + \Delta y^{2} + \epsilon\Delta t^{2}}$, where ϵ is a small value for avoiding zero cost edges in $\mathcal{G}_{H}$. The skeleton of the moving obstacles are the curves traced by their centers (yellow dots on the oscillating rectangles in Online Resource 1) in the X–Y–Time space. The skeletons are closed outside and far from the discretized domain (Construction 2). Note that in doing so, segments of the skeleton may point along negative time. However that does not effect the planning since the X–Y–Time space itself can be treated no differently from ℝ³.

The first supplementary video (Online Resource 1) shows the exploration of 4 homotopy classes in X–Y–Time domain. The environment is 40×40 discretized in X and Y directions, and have 100 discretization cells in time. There are two dynamic rectangular obstacles, that undergo a known oscillatory motion inside a narrow passage between other static obstacles. The 4 different trajectories in the different homotopy classes are marked by different colors as well as different numbers at their current locations. The blue trajectory (3) passes above both the obstacles. The red trajectory (2) passes above the right obstacles, but not the left one. The light blue-gray trajectory (1) passes above the obstacle on the left, but not one on the right. The dark gray trajectory (0) is the trivial shortest path. The trajectories in the non-trivial homotopy classes go behind the obstacles, a region that would otherwise not be visited by the least cost path without any H-signature consideration.

6.3 Four-dimensional configuration space

A natural extension of the example provided in Sect. 6.2.5 would be to explore homotopy classes of trajectories in a 3-dimensional space with moving obstacles. However that makes the configuration space a 4-dimensional one consisting of the coordinates X, Y, Z and Time. So far we have developed representations of H-signature for 2 and 3 dimensional configuration spaces. While doing so we have noticed the similarity in the approach of both. In fact it is possible to unify the formulae and generalize it to arbitrary dimensional configuration space using certain concepts from algebraic and differential topology. We discuss the derivation of such a formula in Bhattacharya et al. (2011a), where we use exterior calculus and the Stokes theorem (Talpaert 2000; Flanders 1989; Svec 2001) to design differential 1-forms in arbitrary D-dimensional configuration spaces, the integration of which serve as the desired H-signature in such spaces. This analysis is reminiscent of a more general treatment that we are currently investigating (Bhattacharya et al. 2012).

Thus we present a result in a X–Y–Z–Time configuration space. The second supplementary video (Online Resource 2) shows the exploration of 3 homotopy classes in a 4-dimensional configuration space consisting of a dynamic obstacle in 3-dimensions. The loop-shaped obstacle is rotating about an axis. The blue axes are the X,Y and Z axes. The apparent rotation of the axes themselves is due to movement of the camera for viewing the trajectories from different angles. As we observe in the video, trajectories numbered 0 and 1 take off from the start coordinate (green dot) and move towards the “center” of the loop. They wait there while 2 takes a different homotopy class to reach the center later. From there 0 and 2 head together towards the goal (red dot), while 1 wait to take a different trajectory to the goal. Thus the 3 trajectories are in different homotopy classes.

7 Conclusion

In this paper we have proposed a novel and efficient way of representing topological information of trajectories for robot motion planning. We have shown that this representation is well suited for use with graph search techniques for finding least cost paths respecting given homology class constraints as well as for exploring different homotopy classes in an environment. The method is independent of the discretization scheme, cost function or the search algorithm used. We have demonstrated the efficiency, applicability and versatility of the method in our results with examples in two, three and four dimensions.