Search-based Path Planning with Homotopy Class Constraints in 3D

Subhrajit BhattacharyaDepartment of Mechanical Engineering

and Applied MechanicsUniversity of Pennsylvania

Philadelphia, PA 19104

Maxim LikhachevRobotics Institute

Carnegie Mellon UniversityNSH 3211, 5000 Forbes Ave

Pittsburgh, PA 15213

Vijay KumarDepartment of Mechanical Engineering

and Applied MechanicsUniversity of Pennsylvania

Philadelphia, PA 19104

Abstract

Homotopy classes of trajectories, arising due to thepresence of obstacles, are defined as sets of trajecto-ries that can be transformed into each other by grad-ual bending and stretching without colliding with ob-stacles. The problem of exploring/finding the differenthomotopy classes in an environment and the problemof finding least-cost paths restricted to a specific ho-motopy class (or not belonging to certain homotopyclasses) arises frequently in such applications as pre-dicting paths for unpredictable entities and deploymentof multiple agents for efficient exploration of an en-vironment. In (Bhattacharya, Kumar, and Likhachev2010) we have shown how homotopy classes of trajec-tories on a two-dimensional plane with obstacles can beclassified and identified using the Cauchy Integral The-orem and the Residue Theorem from Complex Analy-sis. In more recent work (Bhattacharya, Likhachev, andKumar 2011) we extended this representation to three-dimensional spaces by exploiting certain laws from theTheory of Electromagnetism (Biot-Savart law and Am-pere’s Law) for representing and identifying homotopyclasses in three dimensions in an efficient way. Usingsuch a representation, we showed that homotopy classconstraints can be seamlessly weaved into graph searchtechniques for determining optimal path constrained tocertain homotopy classes or forbidden from others, aswell as for exploring different homotopy classes in anenvironment. 1

Homotopy Classes and Homology Classes ofTrajectories

Two trajectories τ1 and τ2 connecting the same start and endcoordinates, xs and xg respectively, are called homotopiciff one can be continuously deformed into the other with-out intersecting any obstacle (Figure 1). Sets of homotopictrajectories form homotopy classes.

Give two trajectories, one may naively attempt to checkif indeed one can be deformed into the other. However, such

Copyright © 2012, Association for the Advancement of ArtificialIntelligence (www.aaai.org). All rights reserved.

1This is a condensed, non-technical overview of work pre-viously published in the proceedings of Robotics: Science andSystems, 2011 conference (Bhattacharya, Likhachev, and Kumar2011).

τ1

τ2

τ3

O1

O2O3

O4

Figure 1: Illustration of homotopy and homology equiva-lences in 2 dimensions. In this example τ1 and τ2 are bothhomotopic (because of the existence of the sequence of tra-jectories shown by the dashed curves) as well as homolo-gous (because of the presence of the area shown by bluehashing). But τ3 is not homotopic nor homologous to either.

a process is highly non-trivial and may be extremely diffi-cult to automate. Even if one is able to check, using sucha method, whether of not two trajectories are homotopic, itis extremely difficult to incorporate the method in search-based planning algorithms to plan trajectories that are con-strained to or avoid certain homotopy classes.

Thus, what one desires is to construct a functional of thetrajectories, H(τ) (which we will call the H-signature ofτ ), such that its value will uniquely identify the homotopyclass of the trajectory (i.e. a complete invariant of homotopyclasses of trajectories). We also desire thatH be of the formof an integration, i.e., H(τ) =

∫τ

dh (where dh is some dif-ferential 1-form – a quantity that can be integrated along acurve). This will let us compute least-cost paths in non triv-ial configuration spaces with topological constraints usinggraph search-based planning algorithms.

It is possible to find such desired 1-forms, dh, as we didin our previous work for 2-dimensional configuration space(Bhattacharya, Kumar, and Likhachev 2010), where we ex-ploited some theorems from complex analysis. However, itcan be shown that by virtue of computing such integrals,what we end up obtaining from H(τ) are complete invari-ants for homology classes rather than homotopy classes. Ho-mology, although a close relative of homotopy and similarto it in many aspects, is subtly different from homotopy.Two trajectories τ1 and τ2 connecting the same start and end

τ1S

G

τ2

-τ2ζ1

ζ3

ζ2

(a) In same Homotopy class,forming a closed contour

τ1

S

G

ζa

τ2

ζκ1

ζκ2

ζb

ζc

(b) In different Homotopyclasses, enclosing obstacles

Figure 2: Two trajectories in same and different homotopyclasses in 2 dimensions

coordinates, xs and xg respectively, are homologous iff τ1together with τ2 (the later with opposite orientation) formsthe complete boundary of a 2-dimensional region embeddedin the configuration space not containing/intersecting any ofthe obstacles (Figure 1).

It can in fact be shown that homotopic trajectories are al-ways homologous (equivalently, trajectories that are not ho-mologous are not homotopic either), but the converse maynot always be true. However, the difference between the twoappear infrequently in practical robot configuration spaces,and as we demonstrate through our results, homology servesas a fair analog of homotopy in most practical robotics prob-lems.

H-signature as Topological Invariants

Background: H-signature in 2-D

The basic principle (Bhattacharya, Kumar, and Likhachev2010) in solving the problem in 2-dimensions was basedon the Residue Theorem from Complex Analysis. We repre-sented the two dimensional plane in which the robot’s pathis to be planned by the complex plane (i.e. a point (x, y) onit is represented as z = x + iy). We hence defined the H-signature (which we previously called the L-value in (Bhat-tacharya, Kumar, and Likhachev 2010)) of a trajectory, τ ,as a complex path integral of a complex vector function asfollows,

H2(τ) =

∫τ

f1(z)z−ζ1f2(z)z−ζ2

...fM (z)z−ζM

dz (1)

The quantity inside the integration (a complex vector) is ananalytic function everywhere in the complex plane, exceptfor distinct points, ζi, which we called representative points,placed on the obstacles (Figure 2), where the function haspoles. As a consequence of this it could be shown usingResidue Theorem that for two trajectories τ1 and τ2 con-necting the same start and goal points in a 2-dimensionalconfiguration space,H2(τ1) = H2(τ2) if the trajectories arein the same homotopy class, but the values are different ifthey are in different homology classes.

vs

vg

(a) A torus-shapedgenus 1 obstacle.

vs

vg

(b) A genus 2 obsta-cle.

vs

vg

(c) A solid cubedoes not inducehomotopy classes.

Figure 3: Examples of obstacles in 3-D. (a-b) induce homo-topy classes, (c) does not.

Si

(a) Skeleton of a genericgenus 1 obstacle is modeledas a current-carrying conduc-tor, Si.

Si

vs

vg

Sj

(b) Theorems from electro-magnetism then gives us ho-motopy class invariants fortrajectories.

Figure 4: Skeletons of obstacles in 3-D are modeled as cur-rent carrying conductor.

H-signature in 3-DJust as we exploited theorems from complex analysis in 2dimensions for constructing the homotopy class invariants,we can exploit certain theorems from Electromagnetism toachieve the same in 3 dimensions. In 3 dimensions multi-ple homotopy classes can only be induced by obstacles withgenus 2 one or more, or with obstacles stretching to infinity.Figure 3 shows some examples of obstacles that can or can-not induce such classes for trajectories. A sphere or a solidcube, for example, cannot induce multiple homotopy classesin an environment.

Analogous to the representative points in the 2-dimensional case, in 3 dimensions we need to considerclosed curves that represent the obstacles of genus 1 orhigher (Figure 4(a)). These curves are the skeletons of theobstacles – 1-dimensional curves that are homotopy equiv-alents (Hatcher 2001) of the obstacles. We represent theseskeletons by Si, where i = 1, 2, · · · ,M .

The key idea in designing a H-signature for solid obsta-cles in 3-dimensions is to model these skeletons of the obsta-cles as “virtual conductors” carrying unit current. Upon do-ing so, using the Biot-Severts Law and Ampere’s Law (Grif-fiths 1998), one can design the H-signature for trajectoriesτ in 3-dimensions as follows,

H3(τ) =

∫τ

B1(l)B2(l)

...BM (l)

dl (2)

2The genus of an obstacle refers to the number of holes or han-dles (Munkres 1999).

where,Bi(r) =

1

4π

∫Si

(x− r)× dx‖x− r‖3 (3)

is a “virtual magnetic field vector” due to the virtual cur-rent flowing through the skeletons. Using Biot-Severts Lawand Ampere’s Law, it can be shown that for two trajectoriesτ1 and τ2 connecting the same start and goal points in a 3-dimensional configuration space, H3(τ1) = H3(τ2) if thetrajectories are in the same homotopy class, but the valuesare different if they are in different homology classes (Fig-ure 4(b)).

Incorporating H-signature in Graph-search BasedAlgorithmsHaving designed a H-signature for 3-dimensional configu-ration spaces in form of an integral, the approach in graphsearch-based planning is very similar to the one we adoptedin (Bhattacharya, Kumar, and Likhachev 2010). We con-struct an augmented graph from the given discrete graphrepresentation of the environment, such that each vertex inthis new graph has the H-signature of a path leading up tothe coordinate of the vertex from vs (start vertex) appendedto it. The consequence of augmenting each vertex of orig-inal graph, G, with a H-signature is that now vertices aredistinguished not only by their coordinates, but also the H-signature of the trajectory followed to reach it. Since theH-signature is in form of an integration, during expansionof the vertices in the search algorithm, its value for newlyexpanded vertices can be easily computed by adding to thevalue of its parent the H-signature of the edge connectingthem. Planning trajectories in this augmented graph thus al-lows us impose constraints on theH-signature of the desiredtrajectories as well as find optimal trajectories in differenthomotopy classes in the environment. For more details onthe graph construction the reader may refer to (Bhattacharya,Likhachev, and Kumar 2011).

ResultsFigure 5(a)-(c) shows examples where we find least costtrajectories in different homotopy classes in a few environ-ments by searching in the the augmented graphs. In eachof these simulations there were certain pre-computations in-volved, where we computed the H-signature for every edgein the original graph. This pre-computation, that needs to beperformed only once for a given environment, takes about15 minutes. The searches in the augmented graphs for find-ing trajectories in different homotopy classes itself took lessthan a minute.

Figure 5(d) demonstrates a planning problem with H-signature constraint. The darker trajectory is the global leastcost path found from a search in the original graph for thegiven start and goal coordinates. The H-signature for thattrajectory was computed, and hence we computed the signa-ture of the complementary class (i.e the class correspondingto the trajectory that passes on the other side of every ob-stacle). The lighter trajectory is the one planned with thatH-signature as constraint. Thus, this trajectory goes on theopposite side of each and every pipe in the environment ascompared to the darker trajectory.

(a) Two hoops. (b) A room with windows.

(c) Exploring 10 distinct ho-motopy classes.

(d) Plan in the complemen-tary homotopy class of theleast cost path.

Figure 5: Exploring homotopy classes and planning withH-signature constraints (Bhattacharya 2012).

For more details, results in other interesting configura-tion spaces (including one in X − Y − Time) as well asfor higher dimensional extensions, please see (Bhattacharya,Likhachev, and Kumar 2011) and (Bhattacharya 2012).

ReferencesBhattacharya, S.; Kumar, V.; and Likhachev, M. 2010.Search-based path planning with homotopy class con-straints. In Proceedings of the Twenty-Fourth AAAI Con-ference on Artificial Intelligence.Bhattacharya, S.; Likhachev, M.; and Kumar, V. 2011. Iden-tification and representation of homotopy classes of trajec-tories for search-based path planning in 3d. In Proceedingsof Robotics: Science and Systems.Bhattacharya, S. 2012. Topological and Geometric Tech-niques in Graph Search-based Robot Planning. Ph.D.Dissertation, University of Pennsylvania, Philadelphia, PA,USA. Supervisors – Vijay Kumar and Maxim Likhachev.Griffiths, D. J. 1998. Introduction to Electrodynamics (3rdEdition). Benjamin Cummings.Hatcher, A. 2001. Algebraic Topology. Cambridge Univer-sity Press.Munkres, J. 1999. Topology. Prentice Hall.