The International Journal of Robotics Research · Ji Yeong Lee Korea Institute of Science and Technology (KIST) Seoul, Korea [email protected] Howie Choset Carnegie Mellon University

http://ijr.sagepub.com

The International Journal of Robotics Research

DOI: 10.1177/0278364905053687 2005; 24; 343 The International Journal of Robotics Research

Ji Yeong Lee and Howie Choset S2xSensor-based Planning for a Rod-shaped Robot in Three Dimensions: Piecewise Retracts of R3

http://ijr.sagepub.com/cgi/content/abstract/24/5/343 The online version of this article can be found at:

Published by:

http://www.sagepublications.com

On behalf of:

Multimedia Archives

can be found at:The International Journal of Robotics Research Additional services and information for

http://ijr.sagepub.com/cgi/alerts Email Alerts:

http://ijr.sagepub.com/subscriptions Subscriptions:

http://www.sagepub.com/journalsReprints.navReprints:

http://www.sagepub.com/journalsPermissions.navPermissions:

http://ijr.sagepub.com/cgi/content/refs/24/5/343SAGE Journals Online and HighWire Press platforms):

(this article cites 12 articles hosted on the Citations

© 2005 SAGE Publications. All rights reserved. Not for commercial use or unauthorized distribution. at CARNEGIE MELLON UNIV LIBRARY on August 9, 2008 http://ijr.sagepub.comDownloaded from

http://www.ijrr.org/multimedia.html

http://ijr.sagepub.com/cgi/alerts

http://ijr.sagepub.com/subscriptions

http://www.sagepub.com/journalsReprints.nav

http://www.sagepub.com/journalsPermissions.nav

http://ijr.sagepub.com/cgi/content/refs/24/5/343


Ji Yeong LeeKorea Institute of Science and Technology (KIST)Seoul, [email protected]

Howie ChosetCarnegie Mellon UniversityPittsburgh, PA 15213, USA

Sensor-based Planningfor a Rod-shapedRobot in ThreeDimensions: PiecewiseRetracts of R

3 × S2R3 × S2

R3 × S2

Abstract

We present a new roadmap for a rod-shaped robot operating in athree-dimensional workspace, whose configuration space is diffeo-morphic to R

3 × S2. This roadmap is called the rod hierarchicalgeneralized Voronoi graph (rod-HGVG) and can be used to find apath between any two points in an unknown configuration space us-ing only the sensor data. More importantly, the rod-HGVG servesas a basis for an algorithm to explore an unknown configurationspace without explicitly constructing it. Once the rod-HGVG is con-structed, the planner can use it to plan a path between any two con-figurations. One of the challenges in defining the roadmap revolvesaround a homotopy theory result, which asserts that there cannot bea one-dimensional deformation retract of a non-contractible spacewith dimension greater than two. Instead, we define an exact cellu-lar decomposition on the free configuration space and a deformationretract in each cell (each cell is contractible). Next, we “connect”the deformation retracts of each of the cells using a roadmap ofthe workspace. We call this roadmap a piecewise retract because itcomprises many deformation retracts. Exploiting the fact that therod-HGVG is defined in terms of workspace distance measurements,we prescribe an incremental procedure to construct the rod-HGVGthat uses the distance information that can be obtained from con-ventional range sensors.

KEY WORDS—motion planning, sensor-based planning,rod-shaped robot,Voronoi diagram, roadmap, exploration, re-tract, piece-wise retract

1. Introduction

In this paper, we present a new structure for sensor-basedplanning for a rod-shaped robot (i.e., a line segment with a

The International Journal of Robotics ResearchVol. 24, No. 5, May 2005, pp. 343-383,DOI: 10.1177/0278364905053687©2005 Sage Publications

notion of direction) operating in a three-dimensional space.Unlike robots that can be modeled as a point, rod-shapedrobots have rotational degrees of freedom in addition to trans-lational degrees of freedom. This problem is challenging be-cause the robot is operating in a three-dimensional workspace,thus making the configuration space a five-dimensional non-Euclidean space. More specifically, the configuration space isdiffeomorphic toR

3 × S2.Our approach is based on a structure called a “roadmap”.

We interpret from the work of Canny (1988) that a roadmap isa one-dimensional subset of the configuration space that satis-fies the following properties: (i) accessibility, (ii) connectivity,and (iii) departability. This means that if there exists a pathbetween two configurations, we can determine a path by firstfinding a path to the roadmap from the start configuration,then following the roadmap, and then finding a path fromthe roadmap to the destination. If a planner can construct aroadmap in a given environment with sensor data, then theplanner has in essence explored the configuration space be-cause the planner can then use the roadmap to find a pathbetween any two configurations.

In this paper, we introduce a new roadmap for a rod in anunknown three-dimensional workspace, called therod hier-archical generalized Voronoi graph (rod-HGVG) (Figure 1).As its name suggests, the rod-HGVG is one of the roadmapsderived from the generalized Voronoi diagram (GVD; Auren-hammer 1991). As shown in Figure 2, the work in this paperrepresents a step toward the ultimate goal of motion planningfor highly articulated robots (such as snake robots). Previouswork in this line of research extended the GVD method intwo directions: the point-HGVG (Choset and Burdick 2000a,2000b), which is a roadmap for a point in a three-dimensionalworkspace, and the rod-HGVG (Choset and Lee 2001), whichis a roadmap for a rod in the plane. Naively one could view thework in this paper as a combination of these two roadmapsto define a new roadmap for the rod operating in a three-dimensional space.

343



344 THE INTERNATIONAL JOURNAL OF ROBOTICS RESEARCH / May 2005

Fig. 1. Samples from the swept volume of the rod-HGVG ina rectangular environment. The walls are removed to showthe inside of the box. The dark “lines” represent the config-urations that are three-way equidistant, and the gray regionsrepresent the configurations that are four-way equidistant. Forthis example, the set of three-way and four-way equidistantconfigurations form a roadmap of this environment.

GVD

HGVG:

DP31

Point

Rod

Convex

Highly

Articulated

Robot

Workspace : R Workspace : R

Rod-HGVG:

Definitions &Incremental ConstructionRod-HGVG:

DR22

...

2 3

DR21

O’Dunlaing & Yap [35]

Choset & Burdick [12]


Keerthi [17]

O’Dunlaing & Yap [34]

Brooks [8]


Pisula et al. [37]

Near Term Future Work

Long Term Goal

Fig. 2. The overview of roadmaps derived from the GVD. Thereferences indicate work related to each of the environmentsand the robots, and are not necessarily derived from the GVD.

We seek to define the rod-HGVG in terms of a deforma-tion retract (Bredon 1995). One of the challenges in definingthe rod-HGVG is due to a homotopy theory result (Bredon1995), which asserts that there cannot be a one-dimensionaldeformation retract of a non-contractible space with dimen-sion greater than two. Instead of defining the roadmap as adeformation retract of the entire configuration space, we firstdefine an exact cellular decomposition on the free configura-tion space so that each cell is contractible. Then, we connectthe deformation retracts of each of the cells using a roadmapof the workspace. We call this roadmap a piecewise retractbecause it comprises many deformation retracts.

It is important to note, however, when the planner uses therod-HGVG to either navigate or explore, it does not explicitlydefine or construct a decomposition. The decomposition itselfis just used to prove the roadmap properties of the rod-HGVG.

The outline of this paper is as follows. In Section 2, wediscuss the prior work on which this work is based. In Sec-tion 3, we define the rod-HGVG inR3, and, in Section 4, weshow that the rod-HGVG indeed satisfies the roadmap condi-tions (Canny 1988) under the assumption that the point-GVGis connected in a given environment. In Section 5, we brieflyshow how to construct the rod-HGVG incrementally. Then,in Section 6, we extend the rod-HGVG so that the rod-HGVGis connected if the point-HGVG (Choset and Burdick 2000a)is connected. The point-HGVG is an extension of the point-GVG, which is connected in a wider range of environmentsthan the point-GVG. Finally, in Section 7, we summarize thiswork and consider some future directions in which the currentwork can be extended.

2. Relation to Prior Work

2.1. Robot Motion Planning

Motion planning algorithms can be classified into classicalmethods and sensor-based methods according to what infor-mation is assumed or used by each of the algorithms. The clas-sical methods assume complete knowledge about the environ-ment, or even the configuration space, while the sensor-basedmethods perform the planning using only the sensor-providedinformation. The classical methods can generate more effi-cient paths compared to the sensor-based methods since theycan use information that the sensor-based methods cannot.

Motion planning methods—classical and sensor-based—include the potential field methods, cell decompositions(Latombe 1991) and roadmaps. The potential field methods(Khatib 1986; Khosla and Volpe 1988; Koren and Borenstein1991; Borenstein and Koren 1998; Chuang and Ahuja 1998)use an artificial vector field defined on the free space, whichpulls the robot toward the goal position and pushes the robotaway from the obstacle boundaries. Then, a path between twoconfigurations can be found by solving a differential equa-tion defined by the vector field. In general, potential fieldmethods are not complete since there can be local minima



Lee and Choset / Sensor-based Planning for a Rod-shaped Robot 345

where the robot may become trapped. The cell decomposi-tion methods (Brooks 1983; Brooks and Lozano-Pérez 1983;Kuan, Zamiska, and Brooks 1985; Noborio, Haniwa, andAri-moto 1989) first decompose the free space into cells suchthat, in each of the cells, the motion planning is simple, andthen represent the adjacency relation between cells using aconnectivity graph, which is used to find a path between thecells. A roadmap is a one-dimensional subset of the space,which is accessible to and from any point in the space and hasa connected component in each connected component of thefree space. The visibility graph (Nilsson 1969) is an exampleof a roadmap.

Recently, there have been active developments in the ran-dom or probabilistic methods (Overmars 1992). Specifically,the probabilistic community (Kavraki and Latombe 1994;Kavraki et al. 1996; Latombe et al. 1996; Kavraki 1997;LaValle 1998;Wilmarth,Amato, and Stiller 1999; Kuffner andLaValle 2000) has successfully demonstrated the capabilitiesof probabilistic roadmaps for highly articulated robots. Theseprobabilistic roadmaps are, in general, relatively easy to im-plement and can solve planning problems in high dimensionsquickly compared to other methods by trading off the notionof completeness with probabilistic completeness. This meansthat the probabilistic roadmaps may not be connected evenwhen the workspace is connected. Many probabilistic meth-ods do not construct the configuration space prior to planning,but still require prior knowledge about the workspace, whichimplies that they cannot be implemented in a sensor-basedway. However, recently, Yu and Gupta (1999) developed analgorithm that can construct a roadmap in a sensor-based wayusing probabilistic technique.

2.2. Related Roadmap Approaches

As we have noted before, the rod-HGVG is based on theroadmap approach. Canny (1988) developed a roadmap al-gorithm that can be used in any configuration space that canbe represented as a semi-algebraic set. More specifically, in aconfiguration space represented usingn polynomials of max-imum degreed for some positived, Canny showed that anymotion planning problem can be solved innk(logn)d(O(k4))

time using his roadmap algorithm, wherek is the number ofdegrees of freedom (Sharir 1997).

Ó’Dúnlaing and Yap (1985) first applied the GVD (seeFigure 3) to define a roadmap for a disk-shaped robot oper-ating in plane. The GVD is the set of points equidistant totwo obstacles, and can be defined using line-of-sight sensordata. In the plane, the GVD is a collection of one-dimensionalsets. Ó’Dúnlaing and Yap (1985) showed that the GVD is adeformation retract of the free space and thus can be used asa roadmap for the point robot in the plane. Later, motivatedby the sensor-based construction method of Rimon (1997),Choset and Burdick (2000a) developed a method to incre-mentally construct the GVD inR2.

Fig. 3. The point-GVD or the GVD is the set of pointsequidistant to two obstacles in the plane.

(a) (b)

Fig. 4. (a) The GVG is shown with dark lines. In this example,there are two disconnected components of the GVG. (b) TheHGVG, the GVG with the higher-order GVG edges forms aconnected set.

In a three-dimensional workspace, the GVD is two-dimensional and therefore cannot be used as a roadmap. Thegeneralized Voronoi graph (GVG) is the three-dimensionalanalog of the GVD and is defined as the set of points equidis-tant to three obstacles. The GVG is one-dimensional inR

3,but, unfortunately, the GVG does not form a connected set.Choset and Burdick (2000a) defined additional structures toconnect the disconnected components of GVG, resulting inthe hierarchical generalizedVoronoi graph (point-HGVG; seeFigure 4). They also provided an incremental construction al-gorithm for the point-HGVG that uses only range-sensor in-formation (Choset and Burdick 2000b). We review the point-HGVG in more detail in Section 6 when we extend the rod-HGVG using the point-HGVG. Also Pisula et al. (2000) con-sidered motion planning for an arbitrary body operating in thethree-dimensional space, using the point-GVG. However, therobot was restricted to performing translational motions only.

Finally, it is worth noting that one of the approaches toimprove the connectivity of the probabilistic roadmap is touse the GVD. For example, Foskey et al. (2000) developeda hybrid algorithm, which constructs a path for an arbitrarilyshaped-robot in three-dimensional space using a randomizedplanner along with the GVD.




2.3. Rod-related Work

The work of ÓDúnlaing, Sharir, and Yap (1986) motivatesour approach in that they use workspace distance measure-ments to define their roadmap. They extended their disk result(Ó’Dúnlaing and Yap 1985) to rod-shaped robots in a planarenvironment, using a concept called a “racetrack”, which isthe set of the points whose distance to the robot is smaller thansome fixed value. The backbone of their roadmap is the set ofconfigurations where three or more obstacles are tangent tothe racetrack, i.e., triple or more equidistant in the workspace.Cox and Yap (1991) developed an “on-line” strategy for rodpath-planning, which can be used for a start-goal problem,but does not construct a roadmap at all; this method is just anavigation algorithm.

Choset and Burdick (1996) and Choset and Lee (2001)developed a roadmap called the rod-HGVG in the plane(Figure 5). This structure provides the groundwork for theroadmap for the rod-shaped robot in three dimensions, whichis the prime contribution of this paper. The planar rod-HGVGconsists of two types of components: the first are the rod-GVG edges, which are the sets of configurations equidistant tothree-obstacles; the second are the one-tangent edges,1 whichare tangent to the point-GVG edges. In essence, they define acellular decomposition of the free configuration space, suchthat a deformation retract (the rod-GVG edge) can be definedin each cell, and connect up these deformation retracts us-ing another structure (the one-tangent edge) derived from theconnectivity of the workspace (the point-GVG). Both the rod-GVG edge and the one-tangent edges are defined in terms ofthe workspace distance function, and therefore the rod-HGVGcan be constructed using only sensor-provided information.

3. Rod Hierarchical Generalized VoronoiGraph in R

3R3

R3

Using distance measurements in workspace, we definethe rod-HGVG in a three-dimensional space, i.e., a five-dimensional configuration space. Throughout this section, weassume that the point-GVG is connected in the workspace,and we define the rod-HGVG which is connected under thisassumption.

3.1. Piecewise Retracts

The rod operating inR3 has five degrees of freedom (i.e., threetranslational and two rotational degrees of freedom) and hencelives in a five-dimensional configuration space diffeomorphicto R

3 × S2. As we mentioned above, the definition of the rod-HGVG is inspired by a structure called a “deformation retract”(Munkres 2000). For a given space, a deformation retract of aspace is a lower-dimensional subset of the space to which thespace itself can be continuously contracted. By definition, a

1. In Choset and Burdick (1996) and Choset and Lee (2001), the one-tangentedges are calledR-edges.

C1

C2

C3C4 C5

C6

CF146

CF456

R24

Fig. 5. Swept volumes (sampled placements) of the rod in aplanar workspace.CF146 andCF456 are the rod-GVG edgesdefined by the three obstacles with the corresponding indices.R24 is the one-tangent edge defined byC2 andC4.

deformation retract is connected for a connected space. Hence,if one can find a one-dimensional deformation retract of thefree configuration space, then one can use the deformationretract as a roadmap. For example, as we mentioned above,the GVD can be shown to be a deformation retract of the freespace in the plane (Ó’Dúnlaing andYap 1985), and thus it canbe used as a roadmap for a point operating in the plane.

However, it is not generally possible to define a one-dimensional deformation retract in a space with dimensiongreater than two (Bredon 1995). Since the dimensions of theconfiguration space for the rod in three dimensions is five,we cannot define a roadmap that is a one-dimensional defor-mation retract of the entire free configuration space of therod. Instead, our approach is to decompose the free config-uration space into contractable regions, and then to definedeformation retracts in each cell. These deformation retractsare defined so that the robot can access and trace the deforma-tion retract of each cell without computing the decompositionexplicitly.

In general, these deformation retracts are not connected toeach other and thus do not form a roadmap. To connect thedisconnected deformation retracts, we define new structuresusing the point-GVG, which, in some sense, represents thetopology of the workspace. Note that, in general, the point-GVG is not connected in a three-dimensional space populatedby obstacles. In this section, we assume that the point-GVGis connected in a given environment, and show that the rod-HGVG (the deformation retracts and the additional structuresdefined using the point-GVG) forms a connected set, and thuscan be used as a roadmap. We call this collection of deforma-tion retracts a piecewise retract. It is interesting to note thatsince the deformation retracts of the cells are connected using




the point-GVG, the planner is inferring the topology of theconfiguration space from the the topology of the workspace.

3.2. Overview of the Rod-HGVG Structures

Since the rod’s configuration space has five dimensions, it isnatural to first define a five-way equidistant structure, whichwe call the “rod-GVG edges”. As in the planar rod-HGVG,the rod-GVG edges are not connected to each other, and donot form a roadmap themselves. So, just like the planar rod-GVG, it seems natural to use the point-GVG edges inR

3 toconnect the rod-GVG edges. Here, we define the one-tangentedges, which are tangent to the point-GVG inR

3 just as theone-tangent edges for the planar rod-HGVG are tangent tothe point-GVG in the plane. That is, the one-tangent edgesare the set of configurations equidistant to three obstacles andtangent to the point-GVG.

Alas, there is still a problem: to transition from a five-wayequidistant structure to a three-way equidistant structure, itwould stand to reason that we would need an additional four-way equidistant structure. Moreover, there is a bigger prob-lem: there are environments where the rod-GVG edges do notexist at all. This is due to the fact that the rod operates in athree-dimensional workspace, in which, in general, no five-way equidistant configurations necessarily exist. Thus, if therod were not “long” or the environment were not cluttered,there would not be five-way equidistant configurations. In-stead, the four-way equidistant configurations always exist forthe rod with an arbitrary length in an arbitrary environment.This motivates us to use the four-way equidistant configura-tions as a building block for the rod-HGVG.

Actually, the sets of the four-way equidistant configura-tions, which are two-dimensional, are the first deformation re-tracts that we define for the rod-HGVG in a three-dimensionalspace. Like the rod-GVG edges of the planar rod-HGVG,the sets of four-way equidistant configurations induce a cel-luar decomposition of the free configuration space and can beshown to be deformation retracts of the cells.

For the moment, let us not worry about these deforma-tion retracts being two-dimensional. We connect these two-dimensional four-way equidistant deformation retracts withthe one-tangent edges and the rod-GVG edges. Here, we usethe topology of the workspace to ensure connectivity of astructure, albeit not a roadmap yet, in configuration space.

One can view the rod four-way equidistant faces as thestructure that links up the one-tangent edges. Recall that thepoint-GVG edges are connected together at the point-GVGmeetpoints, which are the four-way equidistant points. Thatis, the point-GVG meetpoints are the places where the pointrobot can move from one point-GVG edge to another point-GVG edge. Likewise, the rod four-way equidistant faces canbe seen as the set in which the rod travels from one one-tangentedge to another one-tangent edge.

In the final step, we recursively define a new decompositionon the two-dimensional deformation retracts and, within each

P

Q

(x, y, z)

x

y

z

θ

ϕ

Fig. 6. Rod-shaped robot in three-dimensional workspace.The arrowhead is used to distinguish between two endpointsof the rod. It has five degrees of freedom in three dimensions,and can be parametrized byx, y, z, θ , andϕ, where(x, y, z)

are the coordinates of one of the endpoints of the rod andθ andφ define the orientation of the rod represented as Euler angles.

cell, we define one-dimensional deformation retracts called“two-tangent edges”. These deformation retracts are calledtwo-tangent edges because they are tangent to a point two-way equidistant face,2 which is two-dimensional, whereas theone-tangent edges are tangent to a point three-way equidis-tant face (i.e., a point-GVG edge), which is one-dimensional.In summary, the rod-HGVG is defined as the union of thesethree components: rod-GVG edges, one-tangent edges, andtwo-tangent edges. In this section, we define each of the rod-HGVG components in detail, and we show that the rod-HGVGforms a roadmap in Section 4.

3.3. Rod and Distance Definitions

The definition of the rod and the distance function in threedimensions is analogous to that in plane.

DEFINITION 1. [Rod] A rodR is a line segment of lengthLwith a notion of direction, which has two endpointsP andQ ∈ R

3 and whose configuration can be parametrized byq = (x, y, z, θ, ϕ)T (Figure 6).

The coordinates(x, y, z) specify the position of the end-pointP , andθ andϕ define the orientation of the rod at a givenconfigurationq. The symbolR(q) denotes the set of points inR

3 occupied by the rod when the rod is at a configurationq.Also, we denoteP(q) andQ(q) as the positions of the end-points of the rod andPQ(q) as the vectorP(q)−Q(q) whenthe rod is at the configurationq.

We assume that the rod operates in a bounded workspacepopulated by the convex obstacles, denoted byCi . The con-cave obstacles can be modeled as a union of convex obstacles

2. The set of points in the workspace that are equidistant to two obstacles.




as discussed in Choset and Burdick (2000a). The free config-uration spaceFCS is the set of rod configurations that do notintersect any obstacles. In other words

FCS = {q ∈ R3 × S2 : R(q) ∩ Ci = ∅ ∀i}. (1)

As mentioned above, the rod-HGVG is defined usingworkspace distance measurements.

DEFINITION 2. [Rod–Distance Function] The rod distancefunctionDi : FCS → R is defined as

Di(q) = minr∈R(q),c∈Ci

‖r − c‖. (2)

We denoteri(q) as the closest point on the rod to the obstacleCi , andci(q) as the closest point on the obstacleCi to therod. The rod distance gradient is denoted by∇Di , and it isrepresented as a function of the vectorri − ci and the pointri (see Appendix A for the derivation of the rod distance gra-dient). This implies that the rod distance cannot be definedif ri cannot be uniquely defined. For example, if the rod isparallel to one of the flat faces of an obstacle, then the closestpoint between the rod and the obstacle cannot be uniquely de-fined. In this paper, we assume that the walls of the obstaclesare “slightly curved” so that the rod distance gradient can bealways defined.3

Note that we measure the distance in the workspace, andthis distance can be easily provided by the range sensorsplaced along the length of the rod. This distance function canbe measured using a series of range sensors along the length ofthe rod (see Figure 7). Each range sensor registers a distancemeasurement, and then one can find the location of the rangesensor where the distance measurement is smaller than thoseof the adjacent range sensors. Each of these local minima ofthe sensor readings corresponds to the distance to an obstaclevisible from the current rod configuration. That is, the numberof local minima of the sensor reading represents the numberof obstacles visible from the current configuration, and thepoint ri is the point where one of the local minima of thesensor reading is attained. Likewise, the closest point on theobstacleci , which is needed to trace the one-tangent edgesand the two-tangent edges, can also be computed from theposition ofri and the value of the sensor reading. This modelrequires an infinite number of sensors along the length of therod, but it can be reasonably approximated by a finite numberof range sensors. We delay investigating the issues of sensorquantization until future work.

3.4. Rod Four-way Equidistant Faces (Deformation Retract)

Now we define the rod four-way equidistant faces, whichare, as the name suggests, the sets of four-way equidistant

3. In simulation, where we have obstacles with flat faces, we can have in-finitely many closest points on the rod to the obstacle, lying on a closedinterval on the rod. In this case, we take the middle point as the closest pointri . Forci , we also use the same approach.

c1c2

c3C1

C2

r1

r2

r3

Fig. 7. The obstacles are identified using the local minimaof the sensor reading along the length of the rod. In thisexample, the rod “sees” three obstacles since there arethree local minima of the sensor reading atr1, r2, and r3.Note that even ifC2 is a single concave obstacle, the robotsees it as two obstacles. The locations ofc1, c2, andc3 canalso be computed fromri and the values of the sensor reading.

configurations. The rod four-way equidistant faces are two-dimensional and thus are not components of the rod-HGVG.However, they are important in our discussion, since they arethe first deformation retracts used in defining the rod-HGVG.Even if the rod in three dimensions has five degrees of free-dom, we do not consider the five-way equidistant configu-rations as deformation retracts, which, as we see later, maynot even exist at all. In contrast, the rod four-way equidis-tant faces are guaranteed to exist because of the boundednessassumption.

We define the rod four-way equidistant faces by intersect-ing the rod two-way equidistant faces, which are defined as

CFij = {q ∈ FS : 0 ≤ Di(q) = Dj(q) ≤ Dh(q)

∀h = i, j and∇Di(q) = ∇Dj(q)}. (3)

The last condition∇Di(q) = ∇Dj(q) guarantees thatCFij

is a four-dimensional structure under the pre-image theorem(Abraham, Marsden, and Raitu 1988).

Then, the rod three-way equidistant face is defined as

CFijk = CFij ∩ CFjk ∩ CFik. (4)

Note that for a rod operating in a three-dimensionalworkspace, the rod three-way equidistant face is a three-dimensional structure. Continuing this process, we define therod four-way equidistant face as follows.

DEFINITION 3. [Rod Four-way Equidistant Face]

CFijkl = CFijk ∩ CFikl ∩ CFjkl ∩ CFij l . (5)

Later, we show that the rod four-way equidistant faces aredeformation retracts of the subsets of the free configurationspace by defining a deformation retraction which is derived




from fixed-orientation gradient ascent. Since the configura-tion space for the rod with a fixed orientation is essentiallyjust the three-dimensional Euclidean space, one can easily seethat an arbitrary rod configuration can access a rod four-wayequidistant configuration.

If the rod were “short”,CFijkl would be homeomorphic toS2, the two-dimensional sphere. This can be easily shown asfollows. Let q be an arbitrary rod configuration in the freespace with orientation(θ, φ) ∈ S2. Consider a configura-tion space of the rod with fixed orientation of(θ, φ). Thisconfiguration space is a three-dimensional Euclidean spacebounded by convex obstacles, and thus we can find a four-way equidistant point in this configuration space, i.e., thefour-way equidistant configuration of the rod with the ori-entation(θ, φ) ∈ S2, which implies that the rod four-wayequidistant faceCFijkl is homeomorphic toS2. Also, if therod were short, there would be a one-to-one correspondencebetween the point-GVG meetpointsFijkl in a given workspaceandCFijkl in the configuration space derived from the givenworkspace. This can easily be seen; if the rod has zero length,then for any given point-GVG meetpointFijkl, there is a four-way equidistant face which is justCFijkl = {q ∈ FCS :(x(q), y(q), z(q)) = Fijkl, (θ(q), φ(q)) ∈ S2}.

If the rod were short, the rod four-way equidistant faceswould be disconnected from each other. From the observationabove about the relation between the rod four-way equidistantfaces and the point-GVG meetpoints, it is natural to connectthe rod four-way equidistant faces using point-GVG edges.Just as for the planar rod-HGVG, these structures are calledone-tangent edges and are defined as the set of three-wayequidistant rod configurations, which are tangent to the point-GVG. We discuss the one-tangent edges in more detail later.

If the rod were long, two of the rod four-way equidistantfacesCFijkl and CFiklm would intersect each other, and theintersection is a set of five-way equidistant configurations, ora rod-GVG edgeCFijklm, which we discuss later in more detail.Note that, by definition,CFijklm also contains the elements ofCFij lm, CFiklm, andCFjklm, as well as the elements ofCFijkl

andCFijkm. TheseCFij lm, CFiklm, andCFjklm are the four-wayequidistant faces, which do not have the corresponding point-GVG meetpoints and would not exist if the rod were short.In other words, even if there was no point in the workspaceequidistant to the obstaclesCi , Cj , Cl, andCm, there could beat least one rod configuration that is equidistant to these fourobstacles if the rod were “long”. Later, we need to considerthis fact in proving the connectivity of the rod-HGVG.

The rod-GVG edge is one of the boundary components ofthe rod four-way equidistant face. The other is the boundaryedges, which are the set of rod configurations that have zerodistance to the four obstacles.

DEFINITION 4. [Boundary edge]

Bijkl = {q ∈ cl(CFijkl) :Di(q) = Dj(q) = Dk(q) = Dl(q) = 0}, (6)

where cl(·) denotes the closure of a set. Again, the boundaryedges may not exist always. Recall that the boundary edge(i.e., {p ∈ R

3 : di(p) = dj (p) = 0}) is considered as acomponent of the point-HGVG. However, we do not considerthe boundary edges as components of the rod-HGVG. Themotivation for this will be clear in Section 4.2, but essen-tially we use the boundary edges as links that connect discon-nected components of the rod-HGVG, rather than a formalcomponent of the rod-HGVG, and we trace them only whennecessary.

3.5. Rod-GVG Edges

The rod-GVG edges are the first component of the rod-HGVGwhich we formally define. As discussed above, the rod-GVGedge is defined as the set of five-way equidistant rod config-urations, which also can be represented as the intersection ofthe rod four-way equidistant faces.

DEFINITION 5. [Rod-GVG edges]

CFijklm = CFijkl ∩ CFiklm ∩ CFjklm

∩ CFijkm ∩ CFij lm. (7)

In the rod configuration spaceR3×S2, the collection of rod-GVG edges is not necessarily connected. The rod-GVG edgesare homeomorphic toS1 if there are no boundary edges andnone of the rod-GVG edges intersects each other. Otherwise,the rod-GVG edges would be homeomorphic to the union ofopen intervals ofR. If the rod were long enough so that tworod-GVG edges intersect each other, arod-meet configurationCFijklmn is formed, which is a zero-dimensional set of rodconfigurations that are six-way equidistant.

In many environments, the rod-GVG edges may not evenexist at all. Figure 8 shows a rectangular environment, whereL > W,L > H , andCF12345 andCF12346 are the rod-GVGedges. IfL were substantially larger than the length of the rod,there could not be any six-way equidistant configurations, sothese two components cannot be connected. Now, ifW werealso substantially larger than the length of the rod, then no five-way equidistant configuration could exist either (Figure 9).Figure 1 also shows an example of a workspace where no rod-GVG edges exist. Figure 10 shows all of the rod GVG-edgesin a simple environment similar to that in Figure 8, where thereare four disconnected components of rod-GVG edges. (Onlytwo distinct components are identifiable in the figure, sincefor each of the identifiable components, there is another rod-GVG edge component that occupies same workspace volumebut “points” in the opposite direction.)

Unlike the rod-GVG edges for the planar rod-HGVG, therod-GVG edges in three dimensions are not considered to bethe deformation retracts of the subsets of the free configurationspace. Rather, the rod-GVG edges are the boundary compo-nents of the deformation retracts, which are the rod four-wayequidistant faces. Since a rod-GVG edge is the intersection




C1

C2

C3

C4

C5

C6

C1 : Left WallC2 : Front WallC3 : FloorC4 : CeilingC5 : Back WallC6 : Right Wall

CF12345CF12346

W

L

H

Fig. 8. Here, there are two sets of rod configurations thatare five-way equidistant. The configurations inCF12345 areequidistant toC1, C2, C3, C4, andC5, and the configurationsin CF12346 are equidistant toC1, C2, C3, C4, andC6. Note thatthey are not connected to each other.

C2

C3

C4

C5

l

H/2H/2W

H

Fig. 9. Side view of Figure 8 withL > W,L > H , but herethe length of the rod is smaller thanW − H , and thus therod cannot be four-way equidistant toC2, C3, C4, and C5.Therefore, five-way equidistant configurations do not existeither.

of two rod four-way equidistant faces, in some sense, the rod-GVG edges connect the disconnected rod four-way equidis-tant faces. Actually, we see later that the rod-GVG edges andthe one-tangent edges perform a dual role in connecting therod four-way equidistant faces.

3.6. One-tangent Edges

The second component of the rod-HGVG that we define isthe one-tangent edge. As we noted, the one-tangent edgesconnect the disconnected rod four-way equidistant faces. Theone-tangent edges are essentially identical to the one-tangentedges of the planar rod-HGVG edges, where the rod is “tan-gent” to the point-GVG edge (Figure 11).

Just as there is a close relationship between the point-GVGmeetpoints and the rod four-way equidistant faces, there is aclose relationship between the point-GVG edges and the one-tangent edges. If the rod were “short”, for any given point-GVG edgeFijk, we could find two connected components(disconnected from each other) of the one-tangent edgeRijk

due to the symmetry of the rod. That is, there are two compo-

C1

C2

C3

C4

C5

C6

CF12345

CF12346

Fig. 10. The rod-GVG in the above rectangular enclosurecontains two connected components as depicted by the twoswept volumes in the workspace. The environment is shownwith transparent walls so we can see inside. The rod-GVGedgeCF12345consists of the configurations that are equidistantto the obstaclesC1 (left wall), C2 (ceiling),C3 (right wall),C4

(floor), andC5 (front wall). The other rod-GVG edgeCF12346

is defined by the obstaclesC1, C2, C3, C4, andC6 (back wall).

nents ofRijk which occupy the same workspace volume, but“point” in opposite directions. Moreover, if the point-GVGedgeFijk terminates on two point-GVG meetpointsFijkl andFijkm, then the one-tangent edgeRijk, if it exists, would termi-nate on the rod four-way equidistant facesCFijkl andCFijkm.If one of the endpoints of theFijk is the obstacle boundary,then the one-tangent edgeRijk also has an endpoint on theobstacle boundary.

If the rod were long, there might not be anRijk for a givenpoint-GVG edgeFijk. However, as we show later, in that casethere must be a rod-GVG edge connecting the two rod four-way equidistant faces, which would have been connected bythe one-tangent edge. So, either a one-tangent edge or a rod-GVG edge connect two neighboring rod four-way equidistantfaces, but not both.

Now we define the one-tangent edge more precisely. WhenCi ,Cj , andCk define a point-GVG edgeFijk, we presume that:

• Di(q) = Dj(q) = Dk(q);

• the rod is “tangent” toFijk at a pointr.

From these two conditions, the following can be easilyshown:

• r is the closest point on the rod to the obstaclesCi , Cj ,andCk, i.e., forn = i, j, k, dn(r) ≤ dn(ro) ∀ro ∈ R(q).

That is, the pointr where the rodR(q) intersects the point-GVG edge is also the closest point on the rod to all of the threeclosest obstacles. The proof is similar to the case of the planarrod-HGVG (Choset and Burdick 1996), where the point ofintersection between the rod and the point-GVG edge is theclosest point on the rod to the two closest obstacles.




C1

C2

C3

q1

q2

q3

point-GVG edge

Fig. 11. The one-tangent edge whereq1, q2, andq3 are con-figurations on a one-tangent edge associated with obstaclesC1, C2, andC3. The rod intersects the point-GVG at either ofits endpoints (q1, q3) except where the distance attains a localminima, where the rod can “slide” alongq2.

This asserts thatri = rj = rk, whereri, rj , andrk are theclosest points on the rod to each obstacle, and the rod mustintersect the point-GVG edge at pointri . Moreover, it can beshown that the point of contactri on the rod is uniquely definedand must be one of the endpoints of the rod except at the localminima or local maxima of distance to the closest workspaceobstacle along the one-tangent edge. At the local minima andlocal maxima, the point of contact can be at any point on therod. More precisely, at the local maxima or local minima of thedistance to the closest obstacle, the point of contact is the pointp on the point-GVG edge where the distance to the closestobstacle attains a local maximum or a local minimum alongthe point-GVG edge. Then, the rod slides through this pointp as it traces the one-tangent edge, thus changing the point ofthe contact from one endpoint to another.4 Since the tangentdirection of the point-GVG is normal to the plane spanned bythe vectorsci −cj andci −ck, we have the following definitionfor the one-tangent edge.

DEFINITION 6. [One-tangent edge] A one-tangent edgeRijk

is defined as

Rijk = {q ∈ CFijk : 〈PQ(q), ci(q) − cj (q)〉 = 0

and〈PQ(q), ci(q) − ck(q)〉 = 0}. (8)

4. In general, there are no local maxima of the distance along the point-GVGedge, but if there is an isolated point-GVG edge, homeomorphic toS1 (sucha point-GVG edge is called a “cycle” in Choset and Burdick 2000a), there arelocal maxima of the distance along the point-GVG edge. However, as notedlater, since we assume that the point-GVG is connected in this paper, therecan be no point-GVG cycles, and hence no local maxima.

C1

C2

C3

C4

C5

C6

R234

Fig. 12. The swept volume of a single one-tangent edge.The one-tangent edgeR234 is defined by the obstaclesC2

(ceiling), C3 (right wall), andC4 (floor), and connects twojunction regions.

The conditions 〈PQ(q), ci(q) − cj (q)〉 = 0 and〈PQ(q), ci(q)−ck(q)〉 = 0 prescribe that the rod be tangent tothe point two-way equidistant facesFij andFik, respectively.From these two conditions, it follows that the rod is tangentto the point-GVG edgeFijk, which is intersection ofFij andFik. Note that it also follows that the rod is tangent toFjk aswell.

The rod finishes the tracing of a one-tangent edge when (i)it reaches a four-way equidistant configuration, (ii) it reachesan obstacle boundary, or (iii) it detects a cycle. A cycle in thepoint-GVG is a disconnected point-GVG edge diffeomorphicto S1, and if there is a cycle in the point-GVG, there can bea one-tangent edge cycle in the rod-HGVG also. However,since we consider only environments where the point-GVG isconnected, there is no one-tangent edge cycle in the contextof the current work. Therefore, we can ignore condition (iii).Figure 12 shows the swept volume of a one-tangent edge alongthe side of a rectangular enclosure and Figure 13 shows theswept volume of all of the one-tangent edges in the sameenvironment. Note that the one-tangent edges do not intersecteach other, i.e., the union of the one-tangent edges does notform a connected set.

3.7. Two-tangent Edges

Thus far, we have defined the rod-GVG edge, a five-wayequidistant structure, and the one-tangent edge, a three-wayequidistant structure. As mentioned above, neither the unionof the rod-GVG edges nor the union of the one-tangent edgesforms a connected set. Moreover, the union of the rod-GVGedges and the one-tangent edges does not form a connectedset either. Instead, they connect the disconnected rod four-way equidistant faces, which are two-dimensional. Later weformally show that the union of the rod four-way equidistant




C1

C2

C3

C4

C5

C6

R234

R345

Fig. 13. The swept volume of all of the one-tangent edges inthe same environment as Figures 10 and 12. Each one-tangentedge terminates either on the obstacle boundary or on afour-way equidistant configuration. For example, both of theend configurations of the one-tangent edgeR234 are four-wayequidistant configurations, but only one of the end configu-rations of the one-tangent edgeR345 is a four-way equidistantconfiguration and the other is a boundary configuration. Notethat the one-tangent edges are disconnected from each other.

faces, the one-tangent edges, and the rod-GVG edges formsa connected set. However, since the rod four-way equidis-tant faces are two-dimensional, we still need to define a one-dimensional structure on the rod four-way equidistant face toobtain a one-dimensional roadmap.

As already mentioned, in addition to being one-dimensional, this new four-way equidistant structure musthave the following properties. First, this one-dimensionalfour-way equidistant structure must form a connected set ona single rod four-way equidistant face. Otherwise we wouldneed additional structures to connect them. Secondly, this newfour-way equidistant structure must intersect the one-tangentedges terminating on the four-way equidistant face or the rod-GVG edges on the four-way equidistant face.This second con-dition gives us the motivation to define the one-dimensionalfour-way equidistant structure.

Consider the rod four-way equidistant faceCFijkl with one-tangent edgesRijk andRijl each terminating at configurationsin CFijkl (boundedness of the workspace ensures this can hap-pen). ForRijk, the rod lies in the tangent space associated withthe point-GVG edgesFijk. Recall that theFijk is the intersec-tion of the three point two-way equidistant faces.A point two-way equidistant faceFij is the set of points that are equidistantto two obstacles,Ci andCj , and has two dimensions. There-fore, if the rod is in the tangent space ofFijk, this implies thatthe rod lies in the tangent spaces ofFij ,Fjk, andFik at the sametime. Likewise, for the configurations inRijl, the rod lies intangent spacesFij ,Fil, andFjl. Note that, in both cases, the rod

lies in the tangent space associated withFij . Therefore, as ouradditional constraint, to travel fromRijk to Rijl alongCFijkl,we presume that the rod must remain in the tangent space ofFij . The edge formed onCFijkl with the additional constraintof staying in the tangent space ofFij is called a “two-tangentedge” because the rod lies in a two-dimensional tangent space(as opposed to a one-dimensional tangent space with the one-tangent edge). We denote a two-tangent edge asRij/kl, wherethe rod is equidistant toCi , Cj , Ck, andCl and is in the tan-gent space ofFij . Note that for a rod four-way equidistant faceCFijkl, there are four one-tangent edges that terminate on theset, and there can be six different two-tangent edges on thisface. If the rod were “short” andCFijkl homeomorphic toS2,then a two-tangent edgeRij/kl would be homeomorphic toS1.If the rod were long andCFijkl had “holes”,Rij/kl would bea union of sets, each homeomorphic to a closed interval ofR

whose boundary point is a configuration on either a boundaryedge or a rod-GVG edge.

Figure 14 shows an example where a point two-wayequidistant face is homeomorphic to a bounded subset of theplane. Here, the rod is “tangent” toFceiling,f loor when the rodis moving fromq2 to q3, and “tangent” toFf ront,f loor whenmoving fromq1 to q2. Figure 15 shows a single two-tangentedge in an environment and Figures 16 and 17 show all of thetwo-tangent edges in the same environment.

DEFINITION 7. [Two-tangent Edge] The two-tangent edge isformally defined as follows:

Rij/kl = {q ∈ CFijkl | 〈PQ(q), (ci(q) − cj (q))〉 = 0}. (9)

It can be shown that a two-tangent edgeRij/kl is a one-dimensional manifold (Appendix F). Since sensors can easilyprovide distance information and the planner can determinethe closest points on the rod to the closest obstacles fromthe sensor information, the two-tangent edge can be readilyconstructed using only sensor-provided information.

Now we have defined all of the necessary componentsof the rod-HGVG, which comprises rod-GVG edges, one-tangent edges, and two-tangent edges.

DEFINITION 8. [Rod Hierarchical Generalized VoronoiGraph] The collection of rod-GVG edges, one-tangent edges,and two-tangent edges forms the rod-HGVG, i.e.

Rod-HGVG= (⋃

i,j,k,l,m

CFijklm) ∪ (⋃i,j,k

Rijk) ∪ (⋃i,j,k,l

Rij/kl)

Table 1 shows the rough correspondence between the com-ponents of the point-HGVG, the rod-HGVG in the plane, andthe rod-HGVG in three dimensions. Note that we do not in-clude the boundary edges as components of the rod-HGVGbecause, as discussed in the next section, we do not try to traceall of the boundary edges, but instead use them as links thatconnect the disconnected components of the rod-HGVG.

Figure 18 shows the edges of the rod-HGVG near a cor-ner of the environment, where the two-tangent edges connect




C

C

C

Cceiling

left

floor

front

q1

q2

q3

q4

(a)

C

C

C

Cceiling

left

floor

front

q2

q3SS

floor, ceiling

C

C

C

Cceiling

left

floor

front

q1

q4

SSleft, front

C

C

C

Cceiling

left

floor

front

q2

q4Front, FloorSS

(b)

(c) (d)

Fig. 14. (a)q1, q2, q3, andq4 represent four-way equidistant configurations. They are also terminal configurations of differentone-tangent edges. (b), (c), and (d) show two-tangent edges that connect some pairs of one-tangent edges. The figures do notshow the complete two-tangent edges in each case. In these cases, each of the two-tangent edges are diffeomorphic toS1, i.e.,on each of the two-tangent edges, the rod can fully rotate until it reaches the original configuration.

a GVG-edge and one-tangent edges, and also connect one-tangent edges to other one-tangent edges. Figure 19 showsall of the edges in the same environment. Figure 20 showsan example of a “large” environment with a “short” rod (seealso Extension 1). Here all of theCFijkl are homeomorphicto S2, and thus there are no rod-GVG edges. Figure 20 showsfour one-tangent-edges connected by six two-tangent-edges,forming a connected set. In other words, it is possible forthe rod to travel from the endpoint of a one-tangent-edge tothe endpoint of any other one-tangent-edge using only onetwo-tangent-edge. Figure 21 shows all of the edges in the

same environment. Note that, in this example, the rod-HGVGforms a connected set without the rod-GVG edges. Figure 22shows another example of the rod-HGVG in a rectangular en-vironment with a block in the middle of the room (see alsoExtension 2).

4. Roadmap Properties of the Rod-HGVG in R3R3

R3

In this section, we discuss the roadmap properties of therod-HGVG. We recall that the roadmap is a union of one-




Table 1. Comparison Between the Components of the Point-GVG, Rod-HGVG in Plane, and Rod-HGVG in R3R3

R3

Rod-HGVG Rod-HGVG inPoint-GVG in the Plane Three Dimensions

Point-GVG Meetpoints Rod-GVG edges CFijkl (which leadto two-tangent edges)

Two-tan-edges

Point-GVG edges One-tan-edges One-tan-edges

Rod-GVG edges

Junction regions retract to Junction regions retractCFijk (rod-GVG edge) toCFijkl

(three-way equidistant) (four-way equidistant)

C1

C2

C3

C4

C5

C6

R45/23

Fig. 15. A single two-tangent edgeR45/23, equidistant to theC2 (ceiling), C3 (right wall), C4 (floor), andC5 (front wall),and “tangent” to the point two-way equidistant face definedby the obstaclesC4 (floor) andC5 (front wall) in the sameenvironment as Figure 10. This two-tangent edge is notdiffeomorphic toS1 since there is a rod-GVG edge, at whichthe two-tangent edge terminates.

dimensional subsets of the free configuration space, whichhas the following properties: (i) accessibility, (ii) departibility,and (iii) connectivity. In Section 4.1, we discuss accessibilityin detail, and connectivity is discussed in Sections 4.2–4.6.Departibility can be shown using accessibility applied in thereverse direction.

4.1. Accessibility

The rod accesses the rod-HGVG via four gradient ascent op-erations: the first three use a fixed orientation gradient∇Di(x)

that directs the rod to increase distance to the closest obstacleswhile maintaining its orientation; the last gradient ascent op-eration uses the full gradient∇Di(x). The sequence of the first

Fig. 16. All of the two-tangent edges in the same environmentas Figure 10.

three gradient ascent operations implicitly defines a functionH : FS × [0, 1] → ∪CFijkl. More specifically,H is definedas follows. First, while maintaining a fixed orientation, therod moves away from its closest obstacle, i.e., the rod tracesthe solution of

c(t) = ∇Di(c(t)), (10)

until it reaches a configuration equidistant to two obstaclesCi andCj . Then, while maintaining double equidistance anda fixed orientation, the rod moves away from the two closestobstacles until it reaches a configuration equidistant to threeobstaclesCi , Cj , andCk. That is, the rod traces the solution of

c(t) = πTc(t)CFij∇Di(c(t)), (11)

whereTc(t)CFij denotes the tangent space ofCFij at the con-figuration c(t) and πTc(t)CFij

is the projection operator ontothe tangent spaceTc(t)CFij . Finally, while maintaining tripleequidistance and a fixed orientation, the rod moves away from




Fig. 17. Top view of all of the two-tangent edges in the sameenvironment as Figure 16.

C1C2

C3

C4

C5

C6R25/34

R45/23

R234

R345

CF12345

Fig. 18. Swept volume of the edges of the rod-HGVG neara corner of an environment. The rod-GVG edge (CF12345

and the one-tangent edges (R234 andR345) are connected bytwo-tangent edges (R25/34 andR45/23).

the three closest obstacles until it reaches four-way equidis-tance. That is,

c(t) = πTc(t)CFijk∇Di(c(t)). (12)

whereTc(t)CFijk is the tangent space ofCFijk at the config-uration c(t). From the boundedness assumption, after thisoperation, the rod is guaranteed to terminate at a four-wayequidistant configuration. This sequence of gradient ascentoperations defines the mappingH . We assume that the pa-rametert is scaled appropriately so thatH(q, 0) = q andH(q, 1) ∈ ∪CFijkl. Note thatθ(q) = θ(H(q, t)) andφ(q) =φ(H(q, t)) for all t ∈ [0, 1].

The mapH directs the rod to a four-way equidistant con-figuration, which may not be on the rod-HGVG. To bringthe rod to the rod-HGVG, we define an additional mappingHJ : ∪CFijkl × [0, 1] → Rij/kl

⋃CFijklm that moves the rod

away from the four closest obstacles, using the full gradient

c(t) = πTc(t)CFijkl∇Di(c(t)). (13)

Fig. 19. Swept volume of all of the edges of the rod-HGVG.

one-tangent edges

one-tangent edges

two-tangent edges

Fig. 20. A set of one-tangent edges connected by two-tangentedges in an environment. There are no rod-GVG edges inthis environment and the union of the one-tangent edges andthe two-tangent edges forms a connected set.

This step terminates at either of the following: (i) five-wayequidistance (i.e., a rod-GVG edge) or (ii) a two-tangent edge.

In Appendix B, we show that once the rod achieves four-way equidistance, continued gradient ascent (full gradient as-cent) brings the rod either to a two-tangent edgeRij/kl or to arod-GVG edgeCFijklm.

This shows that an arbitrary rod configuration can accessa components of the rod-HGVG using a series of the dis-tance gradient ascents. Once the rod has accessed the rod-HGVG, it can then begin incrementally constructing the rod-HGVG using the previously defined numerical techniques. If




Fig. 21. All of the edges in an environment. There are norod-GVG edges in this example. Note that this is the sameenvironment as Figure 1.

Fig. 22. The rod-HGVG in a rectangular box environmentwith a block in the middle. The one-tangent edges “connect”two regions of the environment.

the rod-HGVG is connected, numerically constructing it en-sures complete exploration of a connected component of therod’s configuration space.

4.2. Connectivity

Now we prove the connectivity of the rod-HGVG, i.e., that therod-HGVG forms a connected set in a connected free config-uration space if the point-GVG is connected. The main ideais to decompose the free configuration space into retractiblesets, called “junction regions”. We define deformation retractsin each junction region and then use the point-GVG to con-nect up the deformation retracts of the junction regions. Theconnectivity is shown in two separate steps (Figure 23).

ConnectivityProof (Sec. 4.6)

Connectivityof � (Sec. 4.4)

Piecewise retractson aCFijkl (Sec.4.5)

Fig. 23. Overview of the connectivity proof.

Step 1. We show that the structure

S = ∪i,j,k,lCFijkl

⋃∪i,j,kRijk

⋃∪i,j,k,l,mCFijklm (14)

forms a connected set. Note thatCFijkl is two-dimensional,and thusS is not a roadmap, but it is a superset of the rod-HGVG. To prove this part of the connectivity, we use theassumption that the point-GVG is connected in a given envi-ronment.

Step 2. We show the connectivity of the union of the two-tangent edges and the rod-GVG edges on a singleCFijkl. Inthis step, we also show that the union of the two-tangent edgesand the rod-GVG edges contains the endpoints of the one-tangent edges terminating atCFijkl. We note that the unionof the two-tangent edges and the rod-GVG edges on a singleCFijkl by themselves may not be connected in some environ-ments. However, in these environments, it is shown that thedisconnected components of the rod-HGVG can be connectedusing the boundary edges.

From these two steps, we conclude that the rod-HGVGis connected. Before we proceed to the proof of the connec-tivity, we define and discuss the junction regions and theirdeformation retracts in Section 4.3.

4.3. Junction Regions

In this section we formally define the junction regions. Thereare two types of junction regions for the rod-HGVG in threedimensions. The first are the junction regions under mappingH , and the union of these junction regions is the free config-uration space, i.e., this induces a cellular decomposition ofthe free configuration space. The deformation retracts of thejunction regions under the mappingH are the rod four-wayequidistant faces which are two-dimensional, and are con-nected together by the rod-GVG edges and the one-tangentedges, which we prove in Section 4.4. The second type ofjunction region is the subjunction region under the mapping




HJ, which induces the cellular decomposition of a rod four-way equidistant faceCFijkl. The deformation retracts of thejunction regions under the mappingHJ are the two-tangentedges or rod-GVG edges.

The junction region under the mappingH is defined for-mally as follows.

DEFINITION 9. [Junction region] A junction regionJijkl isthe pre-image ofCFijkl underH : FS × [0, 1] → CFijkl, i.e.

Jijkl = {q ∈ FS : H(q, 1) ∈ CFijkl}. (15)

Using a similar approach as in Choset and Burdick (1996),H can be shown to be continuous in each of the connectedcomponents of a junction regionJijkl (see Appendix C), andmoreoverH(CFijkl, 1) = CFijkl. Therefore, the rod four-wayequidistant faceCFijkl is a deformation retract of the junctionregionJijkl.

Next, we define the subjunction region, which decomposesCFijkl. Recall that there are two one-dimensional structuresdefined on the rod four-way equidistant faceCFijkl: the rod-GVG edges and two-tangent edges. Therefore, there are twotypes of subjunction region under the mappingHJ, defined asfollows.

DEFINITION 10. [Subjunction region]

Jij/kl = {q ∈ CFijkl : HJ(q, 1) ∈ Rij/kl} (16)

and

Jijklm = {q ∈ CFijkl : HJ(q, 1) ∈ CFijklm}. (17)

In each cellJij/kl, HJ is continuous onJij/kl (for proof, seeAppendix D) andHJ(Rij/kl, 1) = Rij/kl. Therefore,Rij/kl is aone-dimensional deformation retract ofJij/kl. Similarly,HJ iscontinuous onJijklm andHJ(CFijklm, 1) = CFijklm. Therefore,CFijklm is a one-dimensional deformation retract ofJijklm.

If the rod were “short” enough so that no four-way equidis-tant faces intersect each other and there are no rod-GVG edgeson CFijkl, then the union ofJij/kl is equal toCFijkl. However,if there is a rod-GVG edgeCFijklm onCFijkl, we have the sub-junction Jijklm related to the rod-GVGCFijklm, which is theset of configurations that access the rod-GVG edges underthe mappingHJ.

Finally, we note that even if there are boundary edgesBijkl,we do not need to define subjunction regions for them. No con-figuration would access the boundary edge under the mappingHJ sinceHJ is a gradient ascent operation.

4.4. Connectivity of S

In this section, we show the connectivity of the set

S = (∪i,j,k,lCFijkl)⋃

(∪i,j,kRijk)⋃

(∪i,j,k,l,mCFijklm).

To show this we use the connectivity of the point-GVG and therelationship between the structures of the point-GVG and thestructures of the rod-HGVG. Then, just like the point-GVGedges connect the point-GVG meetpoints, we can show theone-tangent edges and the rod-GVG edges connect the rodfour-way equidistant faces. For this we first need to show thefollowing.

LEMMA 1. Let Jijkl andJijkm be two adjacent junction re-gions. If the one-tangent edgeRijk = ∅, thenCFijkl andCFij lm

intersect each other, and it follows that the rod-GVG edgeCFijklm = ∅.

The two junction regions are said to be adjacent if theyshare a common boundary. The proof of this lemma canbe found in Appendix D. From this, we can also show thefollowing.

LEMMA 2. Given two adjacent rod four-way equidistantfacesCFijkl andCFijkm, for which CFijkl ∪ CFijklm ∪ Rijk ∪CFijklm is a connected set, we have

Rijk = ∅ ⇔ CFijklm = ∅which is equivalent to

CFijklm = ∅ ⇔ Rijk = ∅.

The proof of this lemma can be found in Appendix E. Es-sentially, this lemma states that the one-tangent edge and therod-GVG edges perform a dual role in connecting the tworod four-way equidistant faces. That is, the two adjacent tworod four-way equidistant faces are connected either by a one-tangent edge or a rod-GVG edge, but not by both.

Before we prove the connectivity of the setS, we introducesome notations. In the following,fi andti denote the index setsof the obstacles withf our andthree elements, respectively.For example,Jf2 denotes a junction region wheref2 is anindex set of size four, e.g.,f2 = {3, 4, 6, 7}, andRt1 denotesa one-tangent edge with, for example,t1 = {1, 2, 5}.

The proof relies on the fact that there is a close relationshipbetween the components of the point-GVG and the compo-nents of the rod-HGVG. Recall that, in this section, we assumethat the point-GVG is connected in the workspace. If the point-GVG is connected, given two point-GVG meetpoints, we canfind a series of point-GVG meetpoints and the point-GVGedges which form a connected set themselves. Then using theclose relationship between the point-GVG structures and therod-HGVG structures discussed in the previous section, wefind a connected series of the rod four-way equidistant faces,the one-tangent edges and the rod-GVG edges.

If the rod were “short”, then there would be no rod-GVGedges and we could find a one-to-one correspondence betweenthe CFfi

and point-GVG meetpoints, and between the one-tangent edges and the point-GVG edges. In other words, fora given point-GVG edgeFijk, there exists a one-tangent edge




Rijk defined by the same three obstacles, and for a given point-GVG meetpointFijkl, there exists a rod four-way equidistantfaceCFijkl defined by the same four obstacles. If the rod were“long”, then there would exist rod-GVG edges that connectadjacentCFfi

, i.e., we do not need to use the one-tangentedges to connect them (Figure 24). In actuality, as shown inLemma 2, the one-tangent edges do not exist at all betweentwo rod four-way equidistant configurations if they intersecteach other.

Finally, recall that if the rod-GVG edge exists, then thereare rod-GVG structures which do not have correspondingpoint-GVG structures, as depicted above. In other words,CFfi

can exist withoutFfiexisting. However, note that such aCFfi

can only exist when there is a rod-GVG edge. For example, ifCF1234 andCF1235 intersect, the intersection formed isCF12345,and by definitionCF12345is also a subset ofCF1245, CF1345, etc.,which does not have corresponding point-GVG meetpoints.However, clearly, theseCFfi

intersect a rod-GVG edge and, inturn, intersect someCFfi

that have corresponding point-GVGmeetpoints. This implies that we need to consider only thoseCFijkl that have corresponding point-GVG meetpoints in thefollowing proof.

Now we prove the connectivity ofS.

LEMMA 3. In a given environment, if the point-GVG is con-nected and the configuration space is connected, the set

S = (∪i,j,k,lCFijkl)⋃

(∪i,j,kRijk)⋃

(∪i,j,k,l,mCFijklm)

forms a connected set.

Proof. Note that to show the connectivity of the setS is equiv-alent to showing that given two arbitrary configurationqs andqg onS, there exists a path between them that lies completelyon the setS. Since any rod configuration can access a rodfour-way equidistant configuration, we can, without loss ofgenerality, assume that the start and the goal configurationsare on some rod four-way equidistant facesCFfs

andCFfg,

respectively. Then, it suffices to show that there is a connectedseries of rod four-way equidistant faces, one-tangent edges,and rod-GVG edges that containCFfs

andCFfgto show the

connectivity.From the discussion above, givenCFfs

andCFfg, we can

find point-GVG meetpointsFfsandFfg

, which are defined bythe same set of respective obstacles. Then, since the point-GVG is connected, there is a sequence of index setsf1 (=fs), t1, f2, t2, . . . tn−1, fn (= fg), such that(∪Ffi

) ∪ (∪Fti )

is a connected set. Note that using our convention,Ffiis a

point-GVG meetpoint, andFti is a point-GVG edge and wehave the relationti = fi ∩ fi+1. We also define the sequencevi = fi ∪fi+1. Then, sinceFfi

andFfi+1 share the three closestobstacles between them,vi is an index set with five obstacles.

From the index setsfi found above, we find a sequence ofthe rod four-way equidistant facesCFf1, . . . , CFfn

. Then, ifwe show that the union of two adjacent rod four-way equidis-tant facesCFfi

andCFfi+1 with either the one-tangent edge

F1234

F1245

F1256

F1236F123

F124

F125

F126

(a)

CF1234

CF1245

CF1256

CF1236R123

R124

R125

R126

(b)

CF1234

CF1245

CF1256

CF1236

R124

R126

CF12346

CF12456

(c)

Fig. 24. (a) shows a portion of the point-GVG in an envi-ronment (not all the edges shown are labelled). (b) and (c)show the rod-HGVG in the same environment. If the rodwere short (b), then there would be a one-to-one mappingbetween the point-GVG edges and the one-tangent edges.Also there would be a one-to-one mapping between thepoint-GVG meetpoints and the rod four-way equidistantfaces, and the rod four-way equidistant faces would beconnected by the one-tangent edges exactly in the samemanner as the point-GVG meetpoints are connected by thepoint-GVG edges. If the rod were long, some of the rodfour-way equidistant faces would intersect each other, andform the rod-GVG edges between them. Also note that, inthis case, there would be some rod four-way equidistant facesthat do not have corresponding point-GVG meetpoints. Suchrod four-way equidistant faces are not shown in (c).




Rti or the rod-GVG edgeCFvi, forms a connected set, we can

conclude thatS is a connected set, using induction.If CFfi

∩ CFfi+1 = ∅, then the intersection of these twosets is actually a rod-GVG edgeCFvi

. Thus, it follows thatCFfi

∪CFvi∪CFfi+1 forms a connected set. IfCFfi

∩CFfi+1 =∅, then, by definition,CFvi

= ∅, and we need to show that thesetCFfi

∪ CFfi+1 ∪ Rti forms a connected set. IfCFvi= ∅,

from Lemma 1, we haveRti = ∅. Since the point-GVG edgeFti terminates at the two meetpointsFfi

andFfi+1, the one-tangent edgeRti has endpoints on both ofCFfi

andCFfi+1.This implies that the setCFfi

∪CFfi+1 ∪Rti is a connected set.This completes the proof that the set(∪CFfi

) ∪ (∪Rti ) ∪(∪CFvi

) forms a connected set, for a sequence offi and tifound as above and the rod-GVG edgesCFvi

formed betweenany adjacentCFfi

in the sequence. Sinceqs andqg are cho-sen arbitrarily from any junction regions, this implies that(∪fi∈FCFfi

) ∪ (∪ti∈T Rti ) ∪ (∪vi∈VCFvi) (whereF , T andV

are the set of index sets of three, four, and five indices, re-spectively). �

4.5. Connectivity of Two-tangent Edges and Rod-GVGEdges on a Single CFijklCFijklCFijkl

This is the second (and final) step in proving the connectivityof the rod-HGVG under the assumption that the point-GVGis connected in a given environment. More specifically, weshow that the union ofRij/kl andCFijklm on a singleCFijkl

forms a connected set, and we also show that the network oftwo-tangent edgesRij/kl contains the endpoints of the one-tangent edges (if they exist) terminating on the givenCFijkl.In addition, if there are holes onCFijkl, it is shown that theunion of the two-tangent edges and the boundary elementsof CFijkl forms a connected set. We consider this problem intwo subproblems (Figure 25): (i)CFijkl is homeomorphic toS2, and (ii)CFijkl is not homeomorphic toS2, i.e.,CFijkl hasboundaries.

4.5.1. CFijklCFijklCFijkl is homeomorphic to S2S2S2

This corresponds to the case where the rod is “short” or theenvironment is not cluttered. Note that ifCFijkl is homeo-morphic toS2, there are no rod-GVG edges onCFijkl sincerod-GVG edges are boundary components ofCFijkl. There-fore, we need to show that the union of two-tangent edges, bythemselves, forms a connected set. Also we need to show thatthis union contains the endpoints of the one-tangent edges.

To prove the connectivity of the union of the two-tangentedges, we need the following results whose proofs can befound in Appendices F and G.

LEMMA 4. A two-tangent edgeRij/kl is a one-dimensionalmanifold.

LEMMA 5. A two-tangent edgeRij/kl has one connectedcomponent onCFijkl, which is homeomorphic toS2.

ConnectivityProof

Connectivityof �

Piecewise retractson a singleCF ijkl

CFijkl -homeomorphicto S 2

CF ijkl - nothomeomorphicto S 2

Only the rod-GVGedges on theboundary ofCF ijkl

Only the boundaryedges on theboundary ofCF ijkl

Both of the rod-GVGedges and theboundary edges on theboundary ofCF ijkl

Fig. 25. Overview of the connectivity proof.

Now, we prove the connectivity of the two-tangent edgesand rod-GVG edges on a singleCFijkl, which is homeomor-phic toS2.

LEMMA 6. If CFijkl is homeomorphic toS2, then the union ofthe two-tangent edges onCFijkl forms a connected set. More-over, the two-tangent edges onCFijkl contain the endpoints ofthe one-tangent edges terminating onCFijkl.

Proof. Before we show that the union of the two-tangent edgesforms a connected set, we make some observations. Recallthat there are six two-tangent edges on aCFijkl. Also, recallthat there are four one-tangent edges, each terminating at somepoints inCFijkl. In fact, the endpoints of one-tangent edges are,by definition, the intersection of three two-tangent edges. Forexample, the endpoints ofRijk on CFijkl are the intersectionof Rij/kl, Rik/kl, andRjk/il.

Now we show that the union of the two-tangent edges formsa connected set, which contains the endpoints of the one-tangent edges terminating onCFijkl. Recall that, by definition,there are six two-tangent edges on aCFijkl. Without loss ofgenerality, we first consider one of the two-tangent edges, say,Rij/kl. SinceRij/kl has one connected component, it containsthe endpoints ofRijk and Rijl. Since the endpoints ofRijk

are also elements ofRik/jl and Rjk/il, and, by assumption,each two-tangent edge has one connected component, we cantrivially conclude that the union ofRij/kl, Rik/jl, andRjk/il

forms a connected set. Continuing this argument, we can showthat the union of six two-tangent edges onCFijkl forms aconnected set and contains all of the endpoints of the one-tangent edges. �

This completes the proof of the connectivity of the two-tangent edges on a rod four-way equidistant faceCFijkl. Notethat if CFijkl is homeomorphic toS2, the endpoints of the




one-tangent edges are also elements of two-tangent edges,i.e., the one-tangent edges terminate on the two-tangent edges.If all of CFijkl are homeomorphic toS2, the rod-HGVG isconnected since we have already established that the unionof CFijkl, Rijk, andCFijklm (which does not exist in this case)forms a connected set.

4.5.2. CFijklCFijklCFijkl is not homeomorphic to S2S2S2

Note that if CFijkl is not homeomorphic toS2, CFijkl mayhave boundaries on it. There can be two types of boundariesfor CFijkl: (a) the rod-GVG edges (i.e., five-way equidistantconfigurations); (b) the boundary edges (i.e.,Bijkl = {q ∈CFijkl : Di(q) = 0}). Note that a connected component ofthe boundary ofCFijkl can contain both a rod-GVG edgeand a boundary edge. Also note thatCFijkl can have multipledisconnected components, where each connected componentcontains a rod-GVG edge or a boundary edge or both in itsboundary.5 However, if a rod four-way equidistant faceCFijkl

has multiple disconnected components, this implies that thejunction region itself is disconnected also becauseCFijkl is atwo-dimensional deformation retract of the junction region.Therefore, we only need to consider the connectivity of therod-HGVG on a single connected component ofCFijkl.

We first consider the case where the boundary ofCFijkl con-sists only of the rod-GVG edge(s). Then we discuss the casewhere the boundary ofCFijkl contains the boundary edge(s)and no rod-GVG edges. Finally, we discuss the case where theboundary ofCFijkl contains both rod-GVG edges and bound-ary edges. We see that if there are boundary edges, the unionof the rod-HGVG edges together with all of the boundaryedges does not form a connected set. However, we show thatby selectively using the boundary edges, we can connect thedisconnected components of the rod-HGVG.

The Case of Rod-GVG Edge CFijklm. Note that there ac-tually can be more than one rod-GVG edge on a givenCFijkl.Initially, we assume that there is only one rod-GVG edgeCFijklm on CFijkl. The case of multiple rod-GVG edges willbe discussed later. Also note that here we assume that thereare no boundary edges onCFijkl.

It is shown that, ifCFijklm exists, the three of the six two-tangent edges onCFijkl (Rij/kl, Rik/jl, and Rjk/il) intersectCFijklm, and thus the union ofCFijklm and the three two-tangentedges form a connected set. In this sense, the one-tangent edgeRijk and the rod-GVG edgeCFijklm perform a dual role in con-necting the two-tangent edges onCFijkl. Note that to show theconnectivity, we only need to show that a rod-GVG edge mustintersect at least one two-tangent edge in a manner such thatthe the union of the rod-GVG edge and the two-tangent edgeit intersects is still a connected set.

Now we show that assumingCFijklm is the only boundary

5. Note that the “boundary edge” and the “boundary” ofCFijk (or of anystructure) are two different things.

component ofCFijkl, the union of the two-tangent edges andCFijklm forms a connected set.

LEMMA 7. If the rod-GVG edgeCFijklm is the only boundarycomponent ofCFijkl, then the union of the two-tangent edgeson CFijkl andCFijklm forms a connected set.

Proof. First we consider a structure, denoted byCFijkl/m,which is

CFijkl/m = {q ∈ FS|0 < Di(q) = Dj(q) =Dk(q) = Dl(q) ≤ Do(q),

o = m, i, j, k, l}. (18)

In other words,CFijkl/m is the rod four-way equidistant facein a workspace whereCm is removed. Also, we can simi-larly define a “virtual” two-tangent edgeRij/kl/m on CFijkl/m,by ignoringCm temporarily, i.e.,Rij/kl/m is the two-tangentedge defined onCFijkl/m rather than onCFijkl. ThenCFijkl/m

is homeomorphic toS2, since we assume thatCFijklm is theonly rod-GVG edge onCFijkl and there are no boundary edgeseither. Therefore, there is a connected network of two-tangentedgesRij/kl/m, Rik/jl/m, etc., onCFijkl/m, using Lemma 6.

By definition,CFijkl/m and the two-tangent edgesRij/kl/m onit containCFijkl and the two-tangent edgesRij/kl on it, respec-tively. In other words, the rod-GVG edge partitionsCFijkl/m

into CFijkl andCFijkl/m\CFijkl, whereCFijklm is the boundaryof each set. Also,CFijklm partitions∪Rij/kl/m into ∪Rij/kl and∪Rij/kl/m\∪Rij/kl. Note that there is a possibility that∪Rij/kl isempty if CFijklm exists, which implies thatCFijklm is the onlyrod-HGVG component onCFijkl. Therefore, in this case, theconnectivity of the rod-HGVG components onCFijkl is triv-ially satisfied. Thus, in the following, we assume that∪Rij/kl

is not-empty.First we claim that, ifCFijklm does not intersect∪Rij/kl,

then∪Rij/kl = ∪Rij/kl/m. This can be shown by contradiction.Assume∪Rij/kl = ∪Rij/kl/m. This means∪Rij/kl/m\∪Rij/kl =∅. Let q be a point on∪Rij/kl/m\ ∪ Rij/kl. Then, by definitionDm(q) < Di(q),6 and, sinceRij/kl is not an empty set, there isa configuration inq ′ in ∪Rij/kl, whereDi(q

′) < Dm(q ′), andobviously,q ′ ∈ ∪Rij/kl/m. Since∪Rij/kl/m is a connected set,there is a path betweenq andq ′ which lies on∪Rij/kl/m. Thenthere must be a configurationq ′′ on the path whereDm(q ′′) =Di(q

′′) from the continuity of the distance function. Thenq ′′

is, by definition, onCFijklm andRij/kl, i.e.,CFijklm ∩ Rij/kl =∅, which is a contradiction. Therefore, we have∪Rij/kl =∪Rij/kl/m.

Next we show thatCFijklm intersects the network of two-tangent edges, i.e.,∪Rij/kl. We show this by contradiction.Assume thatCFijklm does not intersect∪Rij/kl. Then, from theclaim above,∪Rij/kl = ∪Rij/kl/m, which forms a connectedset. This means there is at least one (actually six) point wherethree of the two-tangent edges intersect, and one of these is

6. Of course,Di(q) = Dj (q) = Dk(q) = Dl(q), which we omit from themain text for simplicity.




the intersection ofRij/kl, Rik/jl, andRjk/il. This point, by defi-nition, belongs toRijk. That is,Rijk is not an empty set, whichcontradicts Lemma 2. Thus,CFijklm must intersect∪Rij/kl.

Now we show thatCFijklm∪(∪Rij/kl) forms a connected set.Recall thatCFijklm and(∪Rij/kl) are subsets ofCFijkl. In otherwords, we need to show that given two arbitrary configurationsq1 andq2 onCFijklm ∪ (∪Rij/kl), there is a path betweenq1 andq2 that lies entirely onCFijklm ∪ (∪Rij/kl). Recall that the rod-GVG edgeCFijklm must intersect the two-tangent edges, andCFijklm is a connected set, because we assume thatCFijklm isthe only boundary component onCFijkl. Therefore, we canassume thatq1 andq2 are the elements of∪Rij/kl.

Recall thatCFijklm partitions ∪Rij/kl/m into ∪Rij/kl and∪Rij/kl/m\∪Rij/kl, and∪Rij/kl/m is a connected set. Now, giventwo configurationsq1 andq2 on ∪Rij/kl, note that there is apathP(t) betweenq1 andq2 that entirely lies onRij/kl/m. Thisis obvious sinceq1, q2 ∈ ∪Rij/kl ⊂ ∪Rij/kl/m and∪Rij/kl/m isa connected set. Now, we should find a path between themwhich lies on(CFijklm) ∪ (∪Rij/kl). If P(t) lies entirely on(∪Rij/kl), we are done. Otherwise, we can find a sequence ofti in [0, 1] such that the pathP(ti) lies on the boundary be-tween∪Rij/kl and∪Rij/kl/m\ ∪ Rij/kl, i.e., the rod-GVG edgeCFijklm.

SinceCFijklm is a connected set, there is a path betweenthe consecutiveP(ti) on CFijklm, which we denote bySi(t),i.e.,Si(t) is a continuous mapping from[0, 1] to CFijklm suchthat Si(0) = P(t2i ), Si(q) = P(t2i+1). Then we construct anew path betweenq1 andq2 by concatenating the segments ofP(t), which lie onRijkl, andSi(t), which lie onCFijklm, and theresulting path, by construction, lies on(CFijklm) ∪ (∪Rij/kl).Thus we have found a path between arbitrary configurationq0

andq1 on (CFijklm) ∪ (∪Rij/kl), and thus we have shown that(CFijklm) ∪ (∪Rij/kl) is a connected set.

In summary, we take the disconnected segments of thepathP(t) that lie on∪Rij/kl. Then, to construct a connectedpath, we connect them using the rod-GVG edge, which is aconnected set if it is the only boundary component ofCFijk.�

Now we can prove the connectivity of the rod-HGVG ona single rod four-way equidistant faceCFijkl with multiplerod-GVG edges.

LEMMA 8. If CFijkl is not homeomorphic toS2, but does notcontain boundary edges, the union of the rod-GVG edges andthe two-tangent edges onCFijkl is a connected set.

Proof. Assume that there aren rod-GVG edges,CFijklm1,CFijklm2, . . . , CFijklmn

. Now, defineCFijkl/m1 (ignoringCm1),CFijkl/m1m2 (ignoringCm1 andCm2),CFijkl/..., etc., until we haveCFijkl/{m1m2...mn}, which is homeomorphic toS2. Now we con-sider CFijkl/{m1m2...mn−1}, which has only one rod-GVG edge,i.e.,CFijklmn

, on it.Then, it is obvious that the “virtual”CFijklmn

(defined by ignoring all the obstacles fromCm1 to Cmn−1) andthe “virtual” two-tangent edges onCFijkl/{m1m2...mn−1} form a

CF1234

CF1234

CF1234/5

R12/34

R12/34

R12/34

R13/24

R13/24

R13/24

R14/23

R14/23

R14/23

CF12345

CF12345

Fig. 26. The case of a rod-GVG edgeCF12345 on CF1234.We assume thatCF12345 is the only boundary component onCF1234. Then, if we ignoreC5 temporarily, and defineCF1234/5

as the four-way equidistant face,CF1234/5 is homeomorphicto S2 and the union of the two-tangent edges onCF1234/5

form a connected set (top). Now we considerC5 and thusCF12345. Note thatCF12345 is homeomorphic toS1 becauseit is assumed thatCF12345 is the only boundary componentsof CFijkl. Then if CF12345 intersects at least one of thetwo-tangent edges, the union ofCF12345 and the union of thetwo-tangent edges form a connected set (center).




connected set, using the argument above. Let us call this setCXmn

. Then, we considerCmn−1 (still ignoring C1 to Cmn−2)andCFijkl/{m1m2...mn−2}. Temporarily ignoringCmn

also, we havea “virtual” CFijkln−1, which is homeomorphic toS1 and parti-tionsCFijkl/{m1m2...mn}. This “virtual” CFijklmn−1 also partitionsCXmn

into two sets, which we call “real”CXmnand “unreal”

CXmn. For the points on “real”CXmn

, the distance toCmn−1

is larger than the distance toCi , and for the configurationson “unreal” CXmn

, the distance toCmn−1 is smaller than thedistance toCi . Using a similar argument as above, we canshow that the union of “real”CXmn

and “virtual” CFijkln−1

forms a connected set. Now “real”CFijkln−1 is the same as“virtual” CFijkln−1, and we are done. (This case means that thetwo rod-GVG edges do not intersect each other.) Otherwisethis implies that on some points on “virtual”CFijkln−1, the dis-tance toCmn−1 must be smaller thanCmn

(since we are stillignoring all of the other obstacles). Using the continuity ofthe distance function, we can conclude that “real”CFijklmn−1

andCFijklmnintersect, and the union ofCFijklmn−1 andCFijklmn

forms a boundary ofCFijkl/{m1m2...mn−2}. The union ofCFijklmn−1

andCFijklmnis also a boundary of∪Rij/kl onCFijkl/{m1m2...mn−2},

and using a similar argument to that above, we can show theconnectivity of the union ofCFijklmn−1 andCFijklmn

and thetwo-tangent edges. Continuing in this way, considering oneadditional obstacle at a time, we can eventually conclude thatthe union of rod-GVG edges and the two-tangent edges formsa connected set. �

The Case of the Boundary Edge Bijkl . Here we considerthe case whereCFijkl is not homeomorphic toS2, and its onlyboundary components are the boundary edges. Unfortunately,if there is a boundary edge, there are examples where theunion of the two-tangent edges and all of the boundary edgeson CFijkl does not form a connected set. Therefore, we needto either introduce a new component to the rod-HGVG orprovide a linking strategy to connect the disconnected com-ponents of the rod-HGVG.

Consider an environment where the free space is the inte-rior of the regular tetrahedron. (Figure 28 and the figure onthe left in Figure 27). In this example, the length of the rod islonger than the height of the tetrahedron, but shorter than theheight of the triangular faces of the tetrahedron. Clearly, thepoint-GVG is connected. Also, with some reasoning, one cansee that the configuration space is connected, i.e., the rod canmove from any configuration to any other configuration. How-ever, the rod-HGVG is not connected. First, since there areonly four obstacles, there exist no rod-GVG edges.Also, sincethe length of the rod is larger than the height of the tetrahedron,there cannot be any one-tangent edges. However, there aretwo-tangent edges, which are disconnected from each other.(Recall that the two-tangent edges cannot be connected with-out one-tangent edges or rod-GVG edges.) That is, even ifthe configuration space is connected, the rod-HGVG is notconnected.

CF1234CF1234

R12/34R12/34

R13/24R13/24

R14/23R14/23

B1234

B1234

Fig. 27. The case of a boundary edgeB1234 on CF1234.Left: the boundary edgeB1234 intersects the network oftwo-tangent edges. In this case, the union of the two-tangentedges and the boundary edge forms a connected set. Right:the boundary edgeB1234 is isolated from other rod-HGVGcomponents. In this case, the union of the two-tangent edgesand the boundary edge does not form a connected set, butthe union of the two-tangent edges themselves still forms aconnected set. Therefore, we do not necessarily need to usethe boundary edge.

Hhq

Fig. 28. In this example, the environment is the interiorof a tetrahedron. If the length of the rod is less than theheight of the triangular face of the tetrahedron, but largerthan the height of the tetrahedron, the configuration spaceis connected. However, in this environment, there are onlytwo-tangent edges disconnected from each other, and noGVG edges or one-tangent edges. This implies that therod-HGVG is disconnected even if the configuration space isconnected.

To amend this problem, one might consider the boundaryedge as a component of the rod-HGVG. This is not surprisingconsidering that the boundary edge is also a component ofthe point-HGVG in a three-dimensional space. However, thiscauses another problem to arise, as can be seen in the exam-ple shown in Figure 30 (and also the figure on the right inFigure 27). Here, we have an isolated boundary edge whichdoes not intersect any rod-HGVG component. There are two




possible solutions to this problem. We can either define theboundary edge as a component of the rod-HGVG and try to de-vise a connecting scheme (and/or additional type(s) of edge, ifnecessary) for connecting isolated boundary edges, or we useboundary edges as links connecting disconnected rod-HGVGcomponents, without considering them as components of therod-HGVG. We take the latter approach, i.e., we will use theboundary edges only as links, and we do not try to trace all ofthe boundary edges.

This approach can be justified since the robot will neveractually reach a configuration on an isolated boundary edgeas the robot traces the rod-HGVG. Recall that to access therod-HGVG from an arbitrary configuration, the robot usesdistance gradient ascent. Thus, after the accessibility proce-dure, the robot will be at some configuration with distancegreater than zero,7 and start tracing the first rod-HGVG com-ponent, which is not a boundary edge. As the robot tracesthe rod-HGVG, it will encounter some boundary edges, butcannot encounter an isolated component of a boundary edge(by definition). Therefore, the isolated boundary edges can besafely ignored.

In summary, the planner traces the boundary edge only if itencounters a configuration on the boundary edge while tracingone of the rod-HGVG edges and does not make an attemptto find all of the boundary edges. Finally, we note that theboundary edges can also be traced using the same techniquedescribed in Section 5.8 This discussion leads to the followinglemma.

LEMMA 9. On a connected component ofCFijkl, the unionof the two-tangent edges onCFijkl and the boundary edgesthat intersect any of the two-tangent edges onCFijkl forms aconnected set.

The Case Where Both Boundary Edges and Rod-GVGEdges Exist on CFijkl . Finally, we consider the case whereboundary ofCFijkl contains both rod-GVG edges and bound-ary edges.

LEMMA 10. On a connected component ofCFijkl, the unionof the two-tangent edges, the rod-GVG edges, and the bound-ary edges that intersect any of the two-tangent edges or therod-GVG edges forms a connected set.

Proof. The proof for this case is essentially the same as theproof of Lemma 8. First, we consider the case in which thereis only one rod-GVG edgeCFijklm on CFijkl. As before, wetemporarily ignore the obstacleCm, and conclude that theunion of the two-tangent edge and the boundary edge formsa connected set, denotedS on CFijkl/m. Now if we consider

7. Actually, it may be the case that the distance is still zero after the gradientascent, but this implies that the accessibility fails. In other words, the robotis initially “stuck” in a component of the configuration space which does notcontain any configuration with distance greater than zero. In this case, theplanner will trace the boundary edge (if possible) and then terminate.8. In reality, we trace the set of configurations with the constant distanceδ ≈ 0 rather than the boundary edge itself.

xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx

xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx

xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx

xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx

xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx

xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx

xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx

Bijkl

Fij

Fik

Fjk

Fig. 29. The boundary edgeBijkl (some of the configurationsin Bijkl are shown as a solid arrow) connects the threetwo-tangent edgesRij/kl, Rik/jl, andRjk/il (which are definedusingFij , Fik, andFjk, respectively). The configurations inRij/kl, Rik/jl, andRjk/il are not shown.

xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx

q1q2

q3

C1

C2

C3 (back wall)

C4

p

Fig. 30. Here, the boundary edge (double dotted arrows)connects two disconnected components ofR13/24. Note thatq1, q2, andq3 all belong toR13/24, i.e., all of the configurationsare on the planep, but q1 belongs to a different componentof R13/24. Note thatq3 also belongs toR123.

the obstacleCm, thenCFijklm partitionsCFijkl/m intoCFijkl andCFijkl/m\CFijkl, andS intoS∩CFijkl andS∩(CFijkl/m\CFijkl).Then using a similar argument as before, we can concludethat the union ofS ∩ CFijkl andCFijklm forms a connectedset. For the case where there are multiple rod-GVG edges, wecan iterate the same argument on the union ofS ∩ CFijkl andCFijklm. �

Figure 31 shows an example of an environment withboundary edges. The free space in the example is the inte-rior of a regular tetrahedron, with the length of the rod longerthan the height of the tetrahedron, but shorter than the heightof the triangles which form the faces of the tetrahedron. Thus,the only components of the rod-HGVG in this environmentsare the two-tangent edges, which are disconnected from eachother. In this case, it is not difficult to see that the configura-tion space is connected.There are six two-tangent edges whichare not connected to each other, and actually each of the two-tangent edges also have multiple disconnected components.However, there are neither rod-GVG edges nor one-tangentedges in this environment. There are no rod-GVG edges, since




C1

C2

C3C4

B1234

R12/34

R23/14

Fig. 31. The free space is the interior of a tetrahedron,defined by the obstaclesC1, C2, C3, and C4. Here, twotwo-tangent edges,R12/34 and R23/14, are connected by aboundary edge componentB1234. (Not all of the two-tangentedges terminating on this boundary edge are shown.)

C1

C2

C3

C4

B1234

B1234

R13/24

Fig. 32. A two-tangent edge componentR13/24 terminates attwo disconnected boundary edge componentsB1234. In otherwords, the two disconnected boundary edge components ofthe boundary edgeB1234 are connected by two-tangent edgeR13/24.

there are only four obstacles, and there are no one-tangentedges, since for a rod configuration to be on a one-tangentedge, it must be tangent to a point-GVG edge, whose lengthis shorter than the length of the rod by design. Thus, there is norod configuration that can be tangent to any point-GVG edgewithout intersecting any obstacles. This means that the unionof the rod-HGVG components (i.e., two-tangent edges) doesnot form a connected set in this example. Thus, we need theboundary edges to connect those disconnected components.Figure 31 shows a boundary edge connecting two discon-nected two-tangent edges, and Figure 32 shows a two-tangentedge terminating at two different boundary edges. Figure 33shows all of the swept volume of all of the edges in the sameenvironment.

This completes the proof of the connectivity of the rod-HGVG components on a four-way equidistant faceCFijkl

which is not homeomorphic toS2.

Fig. 33. The swept volume of the two-tangent edges and theboundary edges inside a regular tetrahedron.

4.6. Connectivity of the Rod-HGVG

We are now ready to prove the connectivity of the rod-HGVGunder the assumption that the point-GVG is connected.

THEOREM 1. Assume that the point-GVG is connected andthe configuration space is connected. Then the rod-HGVG,together with the boundary edge components that intersect atleast one rod-HGVG component, forms a connected set.

Proof. The proof is just the straight application of Lemmas 3,6, 7, 8, 9, and 10. �

5. Incremental Construction of the Rod-HGVG

In this section we discuss an incremental construction proce-dure for the components of the rod-HGVG. Figure 34 showsa overall flow of the construction procedure.

5.1. Accessing the Rod-HGVG

The incremental construction starts with a series of gradientascents described in Section 4.1. As shown in Appendix B,after the gradient ascent, the robot is placed on a rod-GVGedge or a one-tangent edge. Depending on which edge therod is placed on, the planner invokes an appropriate methodto trace the current edge. The methods to trace each type ofedges are described below.

5.2. Tracing the Rod-HGVG Edges

Recall that the components of the rod-HGVG are defined us-ing the workspace distance function with some additional in-formation. The additional information includes the pointsri ,the closest point on the rod to each of the obstacles,ci , the




Begin

Access the rod-HGVGusing the gradient ascent from the closest obstacles

Determine Edge Type

Trace the rod-HGVG tothe chosen node

End

Trace rod-GVG edgeusing GGVG

Trace One-tangent edgeusing G1Tan

Trace Two-tangent edgeusing G2Tan

Find a node with untraced edges

Found

Not Found

Rod-GVG edge One-tangent edge

Two-tangent edge

Identify untraced edge(s)on the current node

Fig. 34. Overall flow of the algorithm.

closest point on each of the the obstacles, andPQ, the vectorparallel to the rod. Theoretically, all of this information can beeasily obtained from a range sensor placed along the length ofthe robot, and computed from the configuration of the robot.This work does not consider the issues of sensor noise or theother problems that can arise in implementation.

To trace the edges of the rod-HGVG, we use the curvetracing technique described in Choset and Burdick (2000b).A detailed description of the technique can be found in Chosetand Burdick (2000b) but, essentially, this method is a root trac-ing technique that consists of a predictor step and a correctorstep. The predictor step moves the robot in the tangent direc-tion of the curve it is tracing by a finite distance. This stepusually takes the robot off the curve. The corrector step thenbrings the robot back onto the curve, using Newton’s iterativeroot finder (Press et al. 2002).

More specifically a rod-GVG edgeCFijklm, a two-tangentedgeRij/kl, and a one-tangent edgeRijk can be representedrespectively as the zeros of

GGV G(q) =

(Di − Dj)(q)

(Di − Dk)(q)

(Di − Dl)(q)

(Di − Dm)(q)

,

G2T an(q) =

(Di − Dj)(q)

(Di − Dk)(q)

(Di − Dl)(q)

(P (q) − Q(q)) · (ci − cj )(q)

,

and

G1T an(q) =

(Di − Dj)(q)

(Di − Dk)(q)

(P (q) − Q(q)) · (ci − ck)(q)

(P (q) − Q(q)) · (ci − cj )(q)

.

The terms of the formDi − Dj represent the equidistant con-ditions and the terms of the formPQ · (ci − cj ) represent thetangency condition that the rod be tangent toFij . Then, us-ing a curve tracing method (Choset and Burdick 2000b), theplanner can construct the edges of the rod-HGVG by tracingthe root of the equations defined above depending on the typeof the edge it is tracing. Note that the equidistant conditioncan be reliably checked up to the sensor accuracy, and it isnot affected by the uncertainty in the localization since it isnot explicitly defined using the coordinate of the robot. Thetangency condition is not affected by localization error either,since, even if the coordinate ofci must be known to check the




tangency condition, the coordinate ofci is computed from theposition of the local minima of the distance on the rod and thevalue of the sensor reading. In other words, the coordinate ofci is computed relative to the coordinate of the rod configu-ration, and it also can be computed reliably up to the sensoraccuracy. Finally, the differential ofG(q), which is requiredto compute the tangent direction and the correcting plane, canbe also represented using only sensor-provided information.The detailed expression for the differential ofG(q) can befound in Appendix F.

5.3. Finding Edges to Explore

While tracing the rod-HGVG edges, the planner keeps trackof the “nodes” it encounters, and stores them in the queue.The nodes include

• the “meet configurations”CFijklmn, i.e., the six-wayequidistant configurations;

• the “boundary configurations”, i.e., the configurationwhere the distance to closest obstacles are zero;

• the configurations where a one-tangent edge and two-tangent edges intersect, i.e.,Rijk ∩ Rij/kl;

• the configurations where a two-tangent edge and a rod-GVG edge intersect, i.e.,CFijklm ∩ Rij/kl.

Depending on the type of the node, we can identify whatkinds of edges should be connected to the node. Table 2 sum-marizes the type of the path that can be connected to eachtype of node. Note that for the boundary configurations, noedges can be connected other than the edge terminating on it.At each node, the planner keeps track of the edges that arealready traced. Then, given the type of node, and the list ofedges that are connected, the planner can determine if thereare more edges to be traced from the current node and, if so,what kind of edges should be traced.

Then, after the construction of the current edge, the plannerfinds a node in the queue which has untraced edges. Then theplanner finds a path from the current node to the chosen nodeon the part of the rod-HGVG which is constructed up to thatpoint. The planner moves the robot to the chosen node, andthen begins tracing the untraced edges from the chosen node.By repeating this process until there is no node with untracededges, the planner can completely construct the rod-HGVGin the given environment.

5.4. Notes on Complexity of the Rod-HGVG

We have implemented our exploration algorithm on a 3.2 GHzPC using C++. In a simple rectangular environment (as shownin Figure 20 and Extension 1), in which there are no rod-GVG edges, it took 3 min to completely construct the rod-HGVG. In a more complex environment, where there are rod-GVG edges (Figure 21 and Extension 2), it took 12 min to

construct the rod-HGVG. However, most of the CPU timewas spent on computing the distance between the rod and theobstacles, as we did not try to optimize the distance computingfunction in the simulator. However, since we are consideringsensor-based planning, in actuality, this distance informationshould be provided from the sensors, and the time spent oncomputing this information should not be counted as part ofthe run-time overhead of our method. Therefore, the run-timeof the simulation does not provide appropriate measure ofthe performance of our method. A more meaningful measurewould be the complexity of the rod-HGVG itself, i.e., thenumber of edges and the nodes of the rod-HGVG, representedas a function of the complexity of the environment, i.e., thenumber of the obstaclesn.

Currently we are determining the exact complexity of therod-HGVG in three dimensions. Since the rod-HGVG is de-fined using the point-GVG, naturally, the complexity of therod-HGVG depends on the complexity of the point-GVG.First, we reason that the complexity of the planar rod-HGVGis the same as the complexity of the complexity of the point-GVD, which is O(n) wheren is the number of obstacles.9

This is because there is a close relationship between the com-ponents of the point-GVD and the components of the planarrod-HGVG. For example, in an environment with no rod-HGVG meetpoints (i.e., the four-way equidistant configura-tions), there is a one-to-one correspondence between the rod-GVG edges and the point-GVD meetpoints, and a two-to-one correspondence between the one-tangent edges and thepoint-GVD edges. This implies that, in this simple case, thecomplexity of the point-GVD and the planar rod-HGVG isthe same. If there are rod-HGVG meetpoints, then this simplerelationship does not hold. However, since using the fact thatthe two rod-GVG edges are either connected by a one-tangentedge or rod-HGVG meetpoint, a similar relationship could befound.

Likewise, we try to compute the complexity of the rod-HGVG in three dimensions using the complexity of the point-GVG and the point-HGVG. Unfortunately, there is no re-sult regarding the complexity of the point-GVG in three-dimensional space. If we assume all obstacles are isolatedpoints, then the number of edges and nodes of the Voronoidiagram isO(n2), wheren is the number of point obstacles(Aurenhammer and Klein 2000). In ad-dimensional spacewith point obstacles, the number ofi-dimensional faces ofthe Voronoi diagram isO(n�d/2�) (Aurenhammer and Klein2000). However, no general result is known when the obsta-cles are of general shape. Since the dimension of the configu-ration space is five, using the result for the point obstacles, thenumber of rod-GVG edges could naively be conjectured to beO(n�5/2�). However, as we have seen, there is a close relation-ship between the number of components of the rod-HGVG

9. If we assume that the obstacles are polygonal, the complexity of the point-GVD also depends on the number of the vertices of the obstacles. However,here we do not make such assumptions.




Table 2. Rod-HGVG Nodes and the Types of Rod-HGVG Edges That are Connected to Them

Type of Node Edges That are Connected to the Node

Meet configurationCFijklmn Rod-GVG edgesCFijklm, CFijkln, CFijkmn, CFij lmn, CFiklmn andCFjklmn

Boundary configurations (See text)

Rijk ∩ Rij/kl A one-tangent edgeRijk and two-tangent edgesRij/kl, Rik/jl andRjk/il

CFijklm ∩ Rij/kl A rod-GVG edgeCFijklm and two-tangent edgesRij/kl, Rij/km andRij/lm

and the number of components of the point-GVG. Thus, ourconjecture is that there is a constantC so that the numberof components of the rod-HGVG is bounded byC times thenumber of components of the point-GVG (i.e., the complexityof the rod-HGVG is alsoO(n2)). This conjecture is based onthe result for the point-GVD with point obstacles, which maynot hold for the obstacles of general shape. Also, in Section 6,we extend the rod-HGVG using the point-HGVG so that therod-HGVG is guaranteed to be connected in more general en-vironment. To compute the complexity of the rod-HGVG withthis extension, we need the complexity of the point-HGVGwhich is not known.

6. Extending the Rod-HGVG Using thePoint-HGVG

In Section 3, we used the point-GVG to define the rod-HGVGand to ensure its connectivity. Alas, if the point-GVG werenot connected, then the rod-HGVG might not be connected.In this section, we relax the assumption that the point-GVGbe connected, and extend the rod-HGVG to the more generalenvironment where the point-HGVG is connected. Since therod-HGVG is based on the point-GVG, it is natural to definenew structures based on the point-HGVG to connect the dis-connected components of the rod-HGVG. That is, we definenew structures in such a way that the rod-HGVG, with thesenew structures, is guaranteed to form a connected set in theenvironment where the point-HGVG is connected. Before wepresent the second-order structure of the rod-HGVG, we firstreview the components of the point-HGVG.

6.1. Point-HGVG in R3R3

R3

Choset and Burdick (2000a, 2000b) extended the point-GVGinto a three-dimensional space, resulting in the point hier-archical Voronoi graph (point-HGVG). As noted above, thepoint-GVG is, in general, not connected inR

3. To connect thepoint-GVG, they introduced new structures, called higher-order edges (Figure 4(b)). We consider an individual pointtwo-way equidistant faceFij , and we define new structureson it to connect the point-GVG edges, which are actually theboundary components ofFij .

Here we need to consider obstacles other than the clos-est obstacles. For a pointx in Fij , the obstaclesCi andCj

are called the first closest obstacles and the obstacle with adistance smaller than the other obstacles, exceptCi andCj ,is called the second closest obstacle. Since we consider thesecond closest obstacles as well as the first closest obstacles,we need to pay more careful attention to the definition of thedistance function. Letci(x) be the closest point onCi from thepointx, i.e.,ci(x) = argminc∈Ci

‖x−c‖. Note that, if there aremultiple obstacles, the pointci(x) may not be “visible” fromthe pointx. That is, the line segment connecting the pointsx andci(x) may not lie completely in the free space. In thiscase, we define the distance between the pointx andCi tobe infinity. This discussion leads us to use the “V -distancefunction” dv

i(x), which is defined as

dv

i(x) =

{minc∈Ci

‖x − c‖ if s(x, ci(x)) ∩ Ck = ∅ for all k

∞ otherwise(19)

wheres(x, ci(x)) is the line segment connecting the pointsx

andci(x).There are four type of higher-order edges inR

3: (i) second-order point-GVG edges; (ii) occluding edges; (iii) boundaryedges; (vi) floating boundary edges. The second-order point-GVG edgeFkl|Fij

is the subset ofFij which contains the pointssatisfyingdv

k(x) = dv

l(x) < dv

m(x) for all m = i, j, k, l. The

intersection of two second-order point-GVG edges is calledthe second-order point-GVG meetpoint, denoted byFklm|Fij

.Second-order point-GVG edges can be used to find a discon-nected point-GVG cycle, i.e., a point-GVG edge that is home-omorphic toS1 and is not connected to any other point-GVGedges.

The occluding edge arises because of the use of theV -distance function. An occluding edgeVkl|Fij

is defined as theset of points onFij where the distance to the second closestobstaclesCk andCl changes discontinuously.

The boundary edge is the set of points where the distancesto two closest obstacles are zero, and the floating boundaryedge is the set of points onFij where distance gradients tothe two closest obstacles become the same, i.e.,∇di(x) =∇dj (x). The boundary edge and floating boundary edge arerequired for completeness, but the simulation results from the




Ci

Cj

Cka

bp

cj

ck

Fig. 35. The closest point on the obstacleCj from the pointb is the pointcj . However, if the line segment connectingx

andcj intersects the first closest obstacleCi , the distance tothe obstacleCj is defined to be infinity for theV -distancefunction. As the robot moves from pointa towards pointb,the second closest obstacle changes fromCj toCk at the pointp, and the distance to the second closest obstacle changesdiscontinuously.

various environments seem to indicate that the point-HGVGforms a connected set without these higher-order edges.

Essentially, we are forming a cellular decomposition onFij

(the cells are called second-order generalizedVoronoi regions)and the higher-order edges are the boundary components ofthe cells. Unfortunately, in addition to these components, thepoint-HGVG also requires some linking strategies since theseadditional components do not guarantee that the point-HGVGis connected. Instead, these structures give clues about the“directions” to which the planner looks for the disconnectedcomponents of the point-HGVG components.A more detaileddiscussion on this subject can be found in Choset and burdick(2000a).

6.2. Second-order Components of the Rod-HGVG:Connecting the Rod-HGVG using the Point-HGVG

In this section, we define new components of the rod-HGVG,called higher-order edges, to connect the disconnected com-ponents of the rod-HGVG, and we discuss the constructionmethods for them. These new structures include (i) second-order one-tangent edges, (ii) second-order rod-GVG edges,and (iii) rod-occluding edges.

Since the rod-HGVG is defined using the point-GVG, it isnatural to use the point-HGVG to define the components ofthe rod-HGVG. Just as the higher-order components of thepoint-HGVG are defined on a two-way equidistant-faceFij ,the higher-order components of the rod-HGVG are definedon a two-way equidistant structure. This two-way equidistantstructure, denoted byRCij , is defined as

RCij = {q ∈ CFij : 〈ci(q) − cj (q), PQ(q)〉 = 0}. (20)

Essentially,RCij is the set of two-way equidistant configura-tions which are also tangent to the point two-way equidistantfaceFij . It can be shown thatRCij is a three-dimensional man-ifold. Note that a two-tangent edgeRij/kl can be defined as thesubset ofRCij , i.e.,Rij/kl is the set of configurations inRCij

that are also four-way equidistant. Also, a one-tangent edgeRijk can be viewed as the intersection ofRCij andRCjk. Thisproduces a motivation to define the higher-order rod-HGVGstructures onRCij , since we want the higher-order rod-HGVGstructures to connect the disconnected components of the rod-HGVG. Just as the second-order point-GVG edges result ina planar GVD-like structure defined on the two-dimensionalpoint two-way equidistant faceFij , the second-order edgesof the rod-HGVG result in a planar rod-HGVG-like structuredefined on the three-dimensional manifoldRCij . The rod-occluding edges are also defined onRCij , and allow the plan-ner to find the disconnected component of the rod-HGVG,which is not “visible” from the current configuration.

Before we define the new components of the rod-HGVG,we briefly mention that if there is a point-GVG cycle, therecan also be a one-tangent edge cycle, i.e., a one-tangent edgethat is homeomorphic toS1 and disconnected from any othercomponents of the rod-HGVG. Then, from the continuity ofthe distance function, the one-tangent edge cycle has the samenumber of local maxima and local minima of the distancealong it. This is a different situation from Section 3, where aone-tangent edge can have at most one local minimum of thedistance on it and no local maxima. At the local minima ofthe distance, the rod can slide forwards along the length of therod at the contact point between the rod and the point-GVG.At the local maxima, the rod also slides along the length ofthe rod, but here it moves backwards until the contact pointchanges from the one endpoint to the other, and then tracksthe point-GVG.

6.3. Second-order One-tangent Edges

The second-order one-tangent edge is analogous to thesecond-order point-GVG edge, or, as its name suggests, anal-ogous to the one-tangent edge of the planar rod-HGVG. Thisstructure is not necessarily connected to the disconnectedcomponents of the rod-HGVG, but provides a “bridge” fromwhich a link can be made to the disconnected components ofthe rod-HGVG.

The second-order one-tangent edges are defined as follows:

Rkl|RCij= {q ∈ RCij : (i)Dk(q) = Dl(q),

(ii )∇Dk(q) = ∇Dl(q),

(iii )〈ck(q) − cl(q), PQ(q)〉 = 0}. (21)

Essentially, the second-order one-tangent edge traces the




second-order point-GVG edgeFkl|Fij. SinceFkl|Fij

is the in-tersection ofFij andSSkl,10 and the rod must be tangent toboth of these planes to be an element ofRkl|RCij

, it followsthatri(q) = rj (q) = rk(q) = rl(q) for all of the rod configu-rations inRkl|RCij

.The boundary component, i.e., the endpoint, of the second-

order one-tangent edge is one of the following: (i) a bound-ary configuration (i.e., the distance to the closest obsta-cles becomes zero); (ii) a four-way equidistant configuration(i.e., a configuration onCFijkl); (iii) three-way equidistant tothe second closest obstacles (i.e., a configurationq whereDi(q) = Dj(q) < Dk(q) = Dl(q) = Dm(q) for an obstacleCm – we call this configuration the second-order connect-configuration); (iv) a configuration on an occluding edge.

When the rod reaches a boundary configuration, it simplystops tracing the edge, and returns to the previous node withexplored edges associated with it. When the rod reaches afour-way equidistant configuration, the rod is actually at aconfiguration in a two-tangent edgeRij/kl, since the rod is ata configuration inRCij . Moreover, sincerk(q) = rl(q) at q,this is a configuration onRkl/ij as well.11 If the rod reachesa configuration at which it is three-way equidistant to thesecond closest obstacles, as well as doubly equidistant to thefirst closest obstacles, then it is on a second-order rod-GVGedge, which is defined in the next section. The case of theoccluding edge is discussed in more detail below.

Figure 37 shows an example of the second-order one-tangent edges in a rectangular enclosure with a box inside.Just as the one-tangent edges of the planar rod-HGVG do notform a connected set by themselves, the union of the second-order one-tangent edges does not form a connected set, evenon a singleRCij .

6.4. Second-order Rod-GVG Edges

The second-order rod-GVG edge is analogous to second-orderpoint-GVG meetpoint, or to the rod-GVG edge for the pla-nar rod-HGVG. Just as the rod-GVG edges connect the end-points of the one-tangent edges for the planar rod-HGVG, thesecond-order rod-GVG edges connect the endpoints of thesecond-order one-tangent edges onRCij . The second-orderrod-GVG edge is defined as

CFklm|RCij= {q ∈ RCij : (i)Dk(q) = Dl(q) = Dm(q),

(ii )∇Dk(q) = ∇Dl(q), ∇Dl(q) = ∇Dm(q),

∇Dk(q) = ∇Dm(q)}. (22)

If the rod were small,CFklm|RCijwould be homeomorphic to

S1, and there would be three second-order one-tangent edgesemanating from it. If the rod were long,CFklm|RCij

would behomeomorphic to the union of the closed intervals ofR. In

10.SSkl = {p ∈ R3 : 0 < dk(p) = dl(p), ∇dk(p) = ∇dl(p)}.

11. Note that this does not mean this configuration is on a one-tangent edge,since there is no guarantee that〈ci − cl , PQ(q)〉 = 0 atq.

a second-orderone-tangent edge

a second-orderrod-GVG edge

Fig. 36. The rod-HGVG with the second-order edges withoutthe links. Note that there is a point-GVG edge (not shown)associated with the small box, the ceiling and the floor, andhence the one-tangent edge associated with the same set ofobstacles. Without linking, just as the point GVG-cycle isdisconnected from the other components of the point-HGVG,the one-tangent edge associated with the small box is dis-connected from the rod-HGVG, even after the second-orderedges are constructed.

this case, the boundary components ofCFklm|RCijare one of

the following: (i) a configuration on a rod-GVG edgeCFijklm;(ii) a configuration on an occluding edge; (iii) a configurationthat has the same distance to the four second closest obstacles(we call this the second-order rod-GVG meet-configuration).

In Figure 37, we can see that the second-order rod-GVGedges connect the second-order one-tangent edges that lie onthe point two-way equidistant face defined by the top andbottom walls.

6.5. Connecting to a One-tangent Edge Cycle

Like the second-order point-GVG edges, the second-orderone-tangent edges and the second-order rod-GVG edges donot actually connect two disconnected components of the rod-HGVG. Rather, from their structures, the planner can inferthat there is a disconnected component of the rod-HGVG andmake a connection to it. Here we describe how to make thisconnection.

For the point-HGVG, the existence of a set of second-order point-GVG edges, whose union is homeomorphic toS1, indicates that there could be a disconnected point-GVGcycle, i.e., a point-GVG edge that is homeomorphic toS1 anddisconnected from any other components of the point-GVG(Choset and Burdick 2000a). The planner accesses a discon-nected point-GVG cycle using a gradient ascent restricted to




Links

a one-tangentedge cycle

Fig. 37. The rod-HGVG in the same environment as Fig-ure 36, but with the links. Using the links, the planner canaccess and trace the inner one-tangent edge cycle. There arelinks from each of the configurations where a second-orderone-tangent edge and a second-order rod-GVG edge intersecteach other.

Fij from the second-point GVG meetpoints. In practice, it iscomputationally expensive to find a cycle in the point-GVGgraph. Instead, the planner attempts to link to a point-GVG cy-cle at every second-order point-GVG meetpoint. This meansthat some redundant links are formed in the graph, while thetime to search cycles is saved.

Now, note that if there is a point-GVG cycle, there can alsobe a one-tangent edge cycle, i.e., a one-tangent edge that ishomeomorphic toS1 and disconnected from any other com-ponents of the rod-HGVG. Just as the point-GVG cycle canbe linked from the second-order point-GVG meetpoint, theone-tangent edge cycle can be linked from the second-orderrod-GVG edge since the second-order rod-GVG edge is anal-ogous to the second-order point-GVG meetpoint. Here, as inthe point-HGVG, we try to connect to a one-tangent edge cy-cle from every second-order rod-GVG edge. Again, this willcreate some redundant links, while the time to search cyclesis saved.

Since the second-order rod-GVG edge is a one-dimensional structure rather than a zero-dimensional struc-ture, we need to choose specific configurations at which theconnecting procedure starts. The natural choice would be theconfigurations where a second-order rod-GVG edge and asecond-order one-tangent edge intersect each other, since theyare the nodes of the rod-HGVG.

The linking is achieved in two steps. From a configurationq ∈ CFklm|RCij

∩Rkl|RCij, the rod performs a gradient descent to

Ck, while staying onRCij , until it becomes three-way equidis-tant toCi , Cj , andCk. The rod is guaranteed to terminate ona three-way equidistant configuration from the continuity of

the distance function. Let this three-way equidistant config-uration be denoted byq ′. Fromq ′ the rod aligns itself to thepoint-GVG edge defined byCi , Cj , andCk by following thesolution of the curve

c(t) = πCFijk∩RCijv(c(t))

where π is a projection operator andv(q) is defined asv(q) = (

πFij(−∇dk (ri(q))) , 0

). In a sense, we move the

contact point between the rod andFij towards the point-GVGedgeFijk while keeping the rod onCFijk ∩ RCij . Figures 37and 38 show the same environment as in Figure 37, but withthe links to the inner one-tangent edge cycle constructed.Alsonote that there are some redundant links.

6.6. Rod-occluding Edges

The point-occluding edge for the point-HGVG is the set ofpoints onFij where there is a discontinuity in the distanceto the second closest obstacle. The motivation for access-ing and tracing the point-occluding edge is to look for thepoint-HGVG components that are not visible from the “cur-rent” point-HGVG component. It is assumed that these in-visible point-HGVG components are connected to the point-occluding edge.Thus, by tracing the point-occluding edge, theplanner can access and start tracing the disconnected point-HGVG components. Recall that the components of the point-HGVG are the boundary components of the second-orderVoronoi regions, which is defined on an individualFij , andthus if the point-occluding edge is not an isolated componentof the point-HGVG, it must intersect some other componentsof the point-HGVG.

We could define the rod-occluding edge as the set of con-figurations where the distance to the second closest obstaclechanges discontinuously, which is analogous to the defini-tion of the point-occluding edge. This, in turn, would implythat the rod-occluding edge is a boundary component of thesecond-order generalized Voronoi region (which could be de-fined on the rod two-way equidistant faceCFij , i.e., the set ofdouble equidistant configurations) in the configuration space.However, it should be noted that the other components of therod-HGVG are not necessarily the boundary components ofthe Voronoi region or any higher-order Voronoi region, whichcould be defined recursively on the boundary of the Voronoiregion. Thus, there is no guarantee that the rod-occluding edgedefined as the boundary component of the second-order gen-eralized Voronoi region intersects any other components ofthe rod-HGVG.

Instead, we define the rod-occluding edge in such a waythat the rod “traces” or “tracks” the point-occluding edge.By tracing, we mean that a rod-occluding edgeRVkl|RCij

isdefined such that the equation

Vkl|Fij= ∪q∈RVkl |RCij

ri(q) (23)




Links to aone-tangent edgecycle

Redundant links

One-tangentedge cycle

One-tangent edge

Second-orderrod-GVG edge

Second-orderone-tangent edge

Fig. 38. This figure shows part of the rod-HGVG in the environment shown in Figure 37 to illustrate, in more detail, the linksfrom the second-order connecting configurations to the inner one-tangent edge cycle. Note also that there are redundant linksto the one-tangent edges which do not form a cycle.

is satisfied, whereVkl|Fijis a point-occluding edge. This im-

plies that for any pointx in Vkl|Fij, there is a rod configu-

ration q in RVkl|CFijsuch thatri(q) = rj (q) = x. Now,

assume that a second-order point-GVG edgeFij |Flminter-

sects the point-occluding edgeVkl|Fijat a pointx ′. Then, if

eq. (23) is satisfied, there is a configurationq ′ ∈ RVkl|RCij,

such thatri(q′) = rj (q

′) = Vkl|Fij∩ Fij |Flm

. The configu-ration q ′ is not necessarily an element of the second-orderone-tangent edgeRkl|RCij

sinceq ′ may not be equidistant toCk andCl. However, fromq ′, we could reach a configurationon RVkl|RCij

by sliding and rotating the rod around the pointx ′ = Vkl|Fij

∩ Fij |Flm, which is not difficult since the rod is

already in the tangent space ofFij (because the rod-occludingedge is defined onRCij ) and from the pointri(q), the tangentdirection ofFij |Flm

can be readily computed. Similar state-ments can be made about the point-GVG edge intersectingthe point-occluding edge and the one-tangent edge associatedwith it. This means that if a point-HGVG component is con-nected to the point-occluding edge, the rod-HGVG structureassociated with that point-HGVG component can also be eas-ily detected and reached fromRVkl|RCij

.With this in mind, we define the rod-occluding edge as

follows:

RVkl|RCij= {q ∈ RCij : ri(q) ∈ Vkl|Fij

and

R(q) normal toVkl|Fij}. (24)

The second condition, i.e., the condition thatR(q) is normalto the point-occluding edge, is in some sense arbitrary, and therod-occluding edge can be defined in different ways as longas it satisfies eq. (23). We use this definition because, from thesimulation results of the point-HGVG in various workspaces,it is observed that, quite often, but not always, the point-HGVG components intersect the point-occluding edge at anangle ofπ/2. Thus, this definition of the rod-occluding edgecould make it easier to connect the rod-occluding edge andthe rod-HGVG components that intersect the rod-occludingedge.

The rod-occluding edge can be accessed from a config-uration in the one-tangent edge through a gradient descentrestricted toRCij . The planner starts the linking procedure onthe configuration in the one-tangent edge where the distance tothe closest obstacles attains a local minima.The planner termi-nates tracing the rod-occluding edge when (i) a rod-occludingedge cycle is detected, and (ii) the rod-occluding edge inter-sects another rod-occluding edge. Recall that for the point-HGVG, it was not possible to determine the tangent directionof the point-occluding edge precisely, and the planner in facttraces the point-occluding edge in a zigzag manner. We canassume that a similar kind of phenomenon would happen inthe case of rod-occluding edges defined using the rod-distancefunction. Then, it would not be easy to determine when therod should terminate tracing an occluding edge cycle, becauseit is not generally possible to align the rod precisely along the




point-occluding edge. However, with this definition, the rodcan terminate tracing the occluding-edge when the pointri(q)

becomes sufficiently close to the pointri(qs) of the startingconfigurationqs , even if there is some error in the orientation.

6.7. Tracing the Rod-occluding Edge

The second-order one-tangent edges and the second-orderrod-GVG edges can be constructed exactly the same way asthe rod-HGVG edges are constructed, as described in Lee,Choset, and Rizzi (2001). However, we cannot construct therod-occluding edge using the same technique, since the rod-occluding edge is defined using the discontinuity of a function.

Recall that, even for the point-HGVG, it is not possibleto trace the point-occluding edge precisely since the plannercannot determine the precise tangent direction of the occlud-ing edge. Likewise, we cannot expect the planner to trace therod-occluding edge precisely either. Therefore, we present anapproximate method to trace the rod-occluding edges.

First we make some observations. We define the point-occluding faceVkl as the set of points in the free workspace,where the distance to the second closest obstacle changes dis-continuously. Then the point-occluding edgeVkl|Fij

can bedefined as the intersection ofVkl andFij . Here, we assumethat given a rod configurationq (not necessarily on a rod-occluding edge), ifR(q) intersectsVkl, we can find the pointof the intersection, denotedokl(q). We also assume that wecan find the pointck(okl(q)), i.e., the closest point on the ob-stacleCk from the pointokl(q). Since we assume that we havea series of range sensors along the length of the rod, it willnot be difficult in practice to find the point where the dis-tance to the second closest obstacle changes discontinuously.Then, by definition of the point-occluding face, the line pass-ing throughokl(q)andck(okl(q)), denoted bylkl

occ(q), is tangent

to the boundary of the obstacleCi (assuming, without loss ofgenerality,Ci occludesCk), and also lies onVkl. This meansthat there is a planeT kl

occ(q) tangent toVkl alonglkl

occ(q). Finally,

if we consider the planeNklocc

(q) that intersectsT klocc

(q) at lklocc

and is normal toT klocc

(q), it is not difficult to see thatNklocc

(q)

is also normal toVkl, and more specifically,Nklocc

(q) is normalto Vkl|Fij

at the pointp = Vkl|Fij∩ Nkl

occ(q). This means that a

configurationq ′ such thatR(q ′) ⊂ Nklocc

(q), can easily accessthe rod-occluding edge by sliding onNkl

occ(q).

Now we describe a tracing method for the rod-occludingedge, which consists of two steps: (i) a prediction step; (ii) acorrecting step.

The prediction step from a rod configurationq on the oc-cluding edgeRVkl|RCij

, is a translation of the rod onTri (q)Fij

(i.e., the plane tangent toFij atri(q)), in a direction normal tothe vectorPQ(q). After the prediction step, the rod would notbe on the rod-occluding edge in general. More specifically,the rod might not be onRCij , and even if it is onRCij , itmight not be normal to the point-occluding edge.

Let q ′ be the configuration after the prediction step. Thecorrection step works in two steps. First it brings the rod onto

q q′ q′′

Vklθ

θ

δr

δt

q q′q′′

Vkl

θ

δr

δt

Fig. 39. First correction step. After making a prediction stepby translating byδt (from q to q ′), the rod is in general notnormal either to the occluding edge or the occluding faceVkl. Left: the curved occluding edge. Ifδt is small so thatthe occluding edge can be approximated by a circle, we haveθ = tan−1(δt/δokl). Right: the initial angle between the rodand the occluding is notπ/2. The correctingθ is computedby θ = tan−1(δt/δokl), whereδokl is the change of theokl onthe rod. Note that we have the same equation for both cases,and can make a correction by rotating the rod byθ , resultingin the configurationq ′′.

Nklocc

(q ′), i.e., after the first correcting step,R(q ′) ⊂ Nklocc

(q ′).Then while keeping the rod onNkl

occ(q ′), the second correction

step brings the rod back on a configuration inRCij , whichaccording to the discussion above, is an element ofRVkl|RCij

also.The first step can be carried out as follows. Assume that

initially the angle betweenR(q) andVkl|Fijis π/2+ θ . Then,

as shown in Figure 39, unlessθ = 0, the pointokl(q) changesits location on the rod, and we can approximately computethe correcting term asθ = tan−1(δt/δokl), whereδt is thetranslational step size andδokl is the change inokl(q) on therod. Then, by rotating the rod byθ aroundokl(q) on the planedefined byPQ(q) andPQ(q ′), we can bring the rod on theNkl

occ(q ′).

Letq ′′ denote the configuration after the first correction. Ingeneral,q ′′ will not be an element of the rod-occluding edgeyet and we need a second correcting step that brings the rodonto RCij . For this, the rod performs a gradient descent ofthe function{Di − Dj, 〈PQ, ci − cj 〉}(q), while keeping therod on the planeNkl

occ(q), which can be computed using the

vectorsPQ(q) andci(q) − okl(q) (Figure 40).This is the description of the tracing methods for the rod-

occluding edge. Note that for the first correction step, we as-sume that after taking a prediction step, the rod still intersectsthe point-occluding face. However, since the point-occludingedge and the point-occluding face can have sharp corners, itis possible that the rod may not intersect the point-occluding




q′′

q′′′okl(q′′)

ck(q′′)Nkl

occ(q′′)

Vkl

Vkl|Fij

Fij

Fig. 40. Second correction step: correcting ontoRCij . Fromthe configurationq ′′, the rod corrects toq ′′′ which is inRCij

by gradient descent. During this correction, the rod maintainstangency to the planeNkl

occ(q ′′).

face after the prediction step. Also, if initially the rod is tan-gent to the point-occluding edge, the rod will not intersectthe point-occluding face after the prediction step. Our currentwork involves development of additional methods to solvethese problems.

7. Conclusion

In this paper we introduce a new roadmap, called therod-HGVG, for a rod-shaped robot operating in a three-dimensional workspace. This roadmap is defined in terms ofworkspace distance and therefore can be constructed usingonly range-sensor information and without ever constructingthe configuration space.

Although the definition of the rod-HGVG and its incre-mental construction procedure are contributions unto them-selves, we feel that there are some deeper conceptual contri-butions. The first is the piecewise retract. The configurationspace for the rod-HGVG is five-dimensional, and, accordingto homotopy theory, a one-dimensional deformation retract ofthe space with dimension greater than two cannot be defined.Therefore, instead of defining a deformation retract for theentire free configuration space, the free configuration spaceis decomposed into cells in which deformation retracts canbe defined. First, the rod four-way equidistant face defines anexact cellular decomposition of the free configuration space,where each cell is called a junction region and the deforma-tion retract of a junction region is a rod four-way equidistantface. However, this deformation retract is two-dimensional,and thus we define another decomposition on it. That is, thetwo-tangent edges and the rod-GVG edges define another cel-lular decomposition of a rod four-way equidistant face, where

the deformation retract of a cell (called a subjunction region)is a two-tangent edge or a rod-GVG edge. The two-tangentedges and the rod-GVG edges in a single connected subjunc-tion region form a connected set, making the planning in asubjunction region trivial. In actuality, in this work, we aredefining a hierarchical piecewise retract: at the high level,the rod four-way equidistant faces form one piecewise retractconnected by point-GVG edges and, at the lower level, thetwo-tangent edges also form a deformation retract, which weshow form a connected set on the rod four-way equidistantfaces.

The second contribution of this work is that it uses theconnectivity of the workspace to infer connectivity proper-ties of the robot’s configuration space. The one-tangent edgesare used to connect the deformation retracts we defined, andthus guarantee the connectivity of the roadmap in configura-tion space. However, the one-tangent edges are defined us-ing point-GVG edge, which represent the connectivity of theworkspace. In some sense, the one-tangent edges form thebackbone of the rod-HGVG, rather than the deformation re-tracts, and the deformation retracts can be viewed as the struc-tures that link up the disconnected one-tangent edges.

The fact that we can use structures based on the workspacetopology as the backbone of the configuration space is notsurprising given the fact that the configuration space we areconsidering is not just an arbitrary five-dimensional space, buthas some special structure, i.e., it is diffeomorphic toR

3 ×S2.This observation is related to the fact that we used the two-dimensional rod four-way equidistant faces as the deforma-tion retracts instead of the five-way equidistant sets, i.e., therod-GVG edges. One of the reasons why we use the four-wayequidistant faces is that they always exist for rod robots op-erating in bounded three-dimensional workspaces. Moreover,a deformation retraction to four-way equidistant faces can beeasily defined using fixed-orientation gradient ascent, sincethe configuration space for a robot with a fixed orientationis essentially the three-dimensional Euclidean space. Fromthis observation, we believe that even for highly articulatedrobots operating in two- or three-dimensional workspace, thethree- and four-way equidistant faces (respectively) wouldbe important components of the roadmap. These three- orfour-way equidistant faces (respectively) would not be one-dimensional, but the one-dimensional edges could be definedon them using additional constraints such as the tangencycondition.

In this paper, we assumed that range sensors along therod have an infinite range. If the range sensors have only afinite range, and the environment is sparse, the rod-HGVG,as represented in this paper, could not be constructed in asensor-based way, since the distance to the closest obstacle(s)from some of the edges may exceed the sensor range. In suchcases, to explore the environment, some form of coveragewould be needed to find obstacles that are out of sensor rangefrom the current position of the robot. One solution would




be to adopt a scheme similar to that used in Acar, Choset,and Atkar (2001), in which they used a simple back-and-forthcoverage motion together with the point-GVG to efficientlycover a two-dimensional unknown space with range sensorswith a finite range. In a three-dimensional space, we first needto define lower-dimensional subsets of the space, and thenperform the coverage on lower-dimensional subsets.

Finally, as we mentioned before, this work is a step towardsthe motion planning of highly articulated robots. The nextstep is planning for a convex body. Then we will considermotion planning of a two-body robot operating in two- orthree-dimensional workspace. We believe these problems cangive us some insight about the issues relating to more generalmultibody problems.

Appendix A: Rod Gradient

Here, we derive the rod distance gradient. In Figure A1,X,Y , andZ denote the world coordinate frame, andXr , Yr , andZr denote the body fixed coordinate frame on R (the rod). Let(x, y, z)T be the origin of the body fixed coordinate frame,and letθ , ϕ be the orientation of the body fixed coordinateframe with respect to the world coordinate frame. Letc be theclosest point on the obstacleCi to the robot and letr be theclosest point toCi on the robot. Finally, let(a, b, c)T be thecoordinates ofr in the body fixed coordinate frame. Then theworld coordinate ofr is

r = x + a cosθ cosϕ − b sinθ + c cosθ sinϕ

y + a sinθ cosϕ + b cosθ + c sinθ sinϕ

z − a sinϕ + c cosϕ

.

Then, following similar steps to the two-dimensional case(Choset and Burdick 1996), the first three components of thedistance gradient are[

∂D

∂x,∂D

∂y,∂D

∂z

]= 1

Di(q)

[(rx − cx), (ry − cy), (rz − cz)

].

The remaining two rotational components are

∂D

∂θ= 1

Di(q)

⟨ cx − rx

cy − rycz − rz

,

aSθCϕ + bCθ + cSθSϕ

−aCθCϕ + bSθ − cCθSϕ

0

⟩

∂D

∂ϕ= 1

Di(q)

⟨ cx − rx

cy − rycz − rz

,

aCθSϕ − cCθCϕ

aSθSϕ + cSθCϕ

aCϕ + cCϕ

⟩

whereCθ andSθ denote cosθ and sinθ , respectively.

Appendix B: Accessibility Proof (Section 4.1)

In this appendix, we demonstrate that a rod starting at a four-way equidistant configuration will access either (i) a rod-GVGedge or (ii) a two-tangent edge via full gradient ascent of dis-tance to the four closest obstacles. Assume that the rod is

Cici R

r

XrYr

Zr

(x, y, z)ϕ

ϕ

θ

θ

X Y

Z

X ′ Y ′

Z ′

Fig. A1. World and body coordinate system for the rod.

four-way equidistant toCi , Cj , Ck, andCl, i.e., q ∈ CFijkl.Proving case (i) is trivial. Now, we prove case (ii), i.e., thegradient ascent will eventually direct the rod onto theRij/kl

when there are no five-way equidistant configurations. LetSSkl = {x ∈ R

3|dk(k) = dl, ∇dk = ∇dl}, i.e., SSkl is thetwo-way equidistant surface in the workspace. Then, since,by assumption, the rod has same distance toCk andCl, thesetR(q) must intersectSSkl. Figure B1 shows a rod config-uration which intersectsSSkl. Consider the plane formed bythe rod vectorPQ and the vectorrkck. Then let the global co-ordinate frame be located such that (i) the origin is located onP , (ii) the X- andY -axes are on the plane formed byPQ andrkck. Then theZ-axis of the global coordinate frame is per-pendicular to that frame. In that frame, the rod configurationis at(0, 0, 0, 0, 0) and the body fixed frame coordinate ofr is(a, 0, 0) with some positive valuea. Note that in this coordi-nate system, theθ component of the gradient rotates the rod“in” the plane, and theϕ component of the gradient rotatesthe rod out of the plane. We are interested in theθ component,which, for the obstacleCk, is

∂D

∂θ= 1

Di(q)

⟨[cx

k− rx

k, c

y

k − ry

k , 0]T

,

[ak sinθ, −ak cosθ, 0]T⟩.

Then, the two vectors(cxk− rx

kc

y

k − ry

k , 0)T and(ak sinθ, −ak

cosθ, 0)T are parallel. Thus,∂Dk/∂θ has a positive value, andthis means that the angle between the tangent plane onSSkl

and the rod increases as a result of gradient ascent. Now, weperform the same analysis withrlcl; the vectorrlcl is in the“opposite” direction of(al sinθ, −al cosθ, 0)T and the coor-dinate value ofal is larger sincerl is on the “other side” ofSSkl.This means that the gradient forCl has a negative componentof ∂Dl/∂θ on the plane defined byPQ andrlcl, with a largervalue than∂Dk/∂θ . This decreases the angle between the tan-gent plane and the rod. So, the gradient ascent decreases theangle between the tangent plane and the rod, thus forcing therod to become “tangent” to the tangent plane.




Clcl

Ck

ck

rk

P

Q

θSSkl

Clcl

rl

Ck

ck

rkP

Q

θ

θSSkl

(ak sin θ,−ak cos θ)(al sin θ,−al cos θ)

ak al

Fig. B1. The rod is equidistant to obstaclesCk andCl. Theclosest points on the rod and each obstacles are shown. Therod intersectsSSkl transversally. The rod is shown on theplane defined byPQ and rkck. In this figurerlcl is on thesame plane, but in general it may be on a different plane.

Appendix C: Continuity of the Mappings HHH andHJHJHJ (Section 4.1)

In this appendix, we prove that the mappingsH andHJ, de-fined in Section 4.1, are continuous mappings. We do thisby showing that the general gradient ascent operation from afixed set of obstacles is continuous. Then, sinceH andHJ areboth gradient operations from fixed sets of obstacles, they arecontinuous.

Given two pointsx1 and x2 ∈ Rm with |x1 − x2| = λ

and a convex obstacleC, let c1 andc2 be the closest pointson the obstacle tox1 andx2, respectively. Also, letd1 andd2

be their respective distances to the obstacle. Without loss ofgenerality, we assume thatd1 ≤ d2. If d2 − d1 > λ, thend2 > d1 + λ = |c1 − x1| + |x1 − x2| > |c1 − x2|, which is acontradiction. Therefore,

|d1 − d2| ≤ λ.

Also, for ci , let S1 andS2 be the spheres centered onx1 andx2 with radii d1 andd2, respectively, and letc1c2 be the linesegment connectingc1 andc2. Consider a line segment whoseendpoints are on each sphere such that the segment itself doesnot intersect the interior of either sphere. Since the centers ofthe sphere areλ apart, the length of this line segment is atmostλ = |x1 − x2|. Therefore,

|c1 − c2| ≤ λ. (25)

Given two configurations of the rod,q1 andq2, let qm bethe configuration such that�(qm) = �(q1) and P(qm) =P(q2). Then,d(q1, qm) ≤ d(q1, q2) andd(q2, qm) ≤ d(q1, q2)

(Figure C1). Then,

|r1 − rm| ≤ |P(q1) − P(qm)| ≤ d(q1, qm) ≤ d(q1, q2)

wherer1 andrm are the closest points on the rod to the obstacleat each configuration.

Clcl

Ck

ck

rk

P

Q

θSSkl

Clcl

rl

Ck

ck

rkP

Q

θ

θSSkl

(ak sin θ,−ak cos θ)(al sin θ,−al cos θ)

ak al

Fig. C1. Given two points,x1 andx2, the distance betweenthe closest points on the obstacle,c1 andc2, from each pointcannot be larger thanL, which is smaller than|x1 − x2|.

L

Ci

d1 d2

x1 x2

c1 c2

Fig. C2. When we rotateq1 to q ′1, the distanceDi(q) and the

closest point on the rodr(q1) do not change.

Now we considerqm andq2. Let ciri be the vector definedby two points,ci andri , and letPQ(qi) be the vector definedby P andQ, the two endpoints of the rod at configurationqi .Givenqi , if (PQ(qi))(ciri) = π/2, thenri is on eitherP orQ. For these configurations, we can find a rod configurationq ′

isuch thatP(qi) = P(q ′

i), ri = r ′

i, and (PQ(q ′

i))(ciri) =

π/212 (see Figure C2). So, givenqm andq2, let q ′m

andq ′2 be

such configurations for eachqm andq2. Then, |r2 − rm| =|r ′

2 − r ′m|, so, without loss of generality, we can assume that

(PQ(qm))(cmrm) = (PQ(q2))(c2r2) = π/2.Let ρi be the radius of curvature of the wall of the obstacle

at ci , andL be the length of the rod. Then

|rm − r2| ≤ M|(D(qm) + ρ(qm))�(qm)

−(D(q2) + ρ(q2))�(q2)|≤ M(max(D(qm), D(q2)) + ρ)d(qm, q2)

≤ M(max(D(qm), D(q2)) + ρ)d(q1, q2)

for some constantM (see Figure C3). Then, assuming thatthere is no “flat” wall,ρ < ρmax for some constantρmax , andsince the environment is bounded, i.e.,D(q) < Dmax , forsomeDmax . Thus,

|rm − r2| ≤ K ′d(q1, q2)

12.c′i= c1, r ′

i= ri .




r2rm

r′m

θ

θρ ρ′

D(q2)D(qm)

q2

qm

Ci

P

Fig. C3. Here,r2 and rm are the closest points on the rodat configurationsq2 and qm, respectively, andr ′

mis the

point on the rod atq2 corresponding torm at qm. Then,|rm − r2| ≤ |rm − r ′

m| + |r ′

m− r2| ≤ Lθ + (ρ(q2) + Di(q2))θ .

whereK ′ = (Dmax + ρmax). Therefore, for anyq1 andq2

|r1 − r2| ≤ |r1 − rm| + |rm − r2|≤ d(q1, q2) + K ′d(q1, q2) = Kd(q1, q2) (26)

for some constantK = (1 + K ′). Then, from eqs. (25) and(26),

|c1 − c2| ≤ |r1 − r2| ≤ Kd(q1, q2) (27)

for someK.Now, we want to show thatH(q, t) satisfies the Lipschitz

condition with respect toq in free space. First, for gradi-ent ascent from the closest obstacle,H (q, t) = H (q) =D1(H(q, t))∇(D1(H(q, t))), which is

H (q, t) = D1(H(q, t))∇D1(H(q, t))

=[ ∇d ′

1(r(H(q, t)))

U(H(q, t)) × ∇d ′1(r(H(q, t)))

](28)

where∇d ′1(r) = (r − c), andU(q) = r(q) − P(q). We want

to show that

|H (q1, t) − H (q2, t)| < M|q1 − q2|for some constantM.

|H (q1, t) − H (q2, t)| =[ ∇d ′1(r(H(q1, t)))

U(H(q1, t)) × ∇d ′1(r(H(q1, t)))

]

−[ ∇d ′

1(r(H(q2, t)))

U(H(q2, t)) × ∇d ′1(r(H(q2, t)))

](29)

Since

|∇d ′1(r(H(q1, t)) − ∇d ′

1(r(H(q2, t)))| =|(r1 − c1) − (r2 − c2)|≤ |r1 − r2| + |c1 − c2|

from eqs. (26) and (27),

|∇d ′1(r(H(q1, t)) − ∇d ′

1(r(H(q2, t)))| ≤ M1d(q1, q2)

for some constantM1. Since,

|(r1 − c1) − (r2 − c2)| < (|r1 − r2| + |c1 − c2|)< 2d(q1, q2),

it follows that

|∇dj (r1) − ∇dj (r2)| < 2d(q1, q2).

Also, for U(q1) andU(q2)

|U(H(q1, t)) − U(H(q2, t))| = |(r1 − P(H(q1, t)))

−(r2 − P(H(q2, t)))|= |(r1 − r2) − (P (H(q1, t))

−P(H(q2, t)))|< |(r1 − r2)| + |P(H(q1, t))

−P(H(q2, t))|.Therefore,

|U(H(q1, t)) − U(H(q2, t))| ≤ M2d(q1, q2)

for some constantM2.Hence,

|U(H(q1, t)) × ∇d ′1(r(H(q1, t)))

−U(H(q2, t)) × ∇d ′2(r(H(q2, t)))|

= |U(H(q1, t)) × (∇d ′1(r(H(q1, t))) − ∇d ′

2(r(H(q2, t))))

+(U(H(q1, t)) − U(H(q2, t))) × ∇d ′2(r(H(q2, t)))|

≤ |U(H(q1, t)) × (∇d ′1(r(H(q1, t))) − ∇d ′

2(r(H(q2, t))))|+|(U(H(q1, t)) − U(H(q2, t))) × ∇d ′

2(r(H(q2, t)))|≤ |U(H(q1, t))||(∇d ′

1(r(H(q1, t))) − ∇d ′2(r(H(q2, t))))|

+|(U(H(q1, t)) − U(H(q2, t)))||∇d ′2(r(H(q2, t)))|

≤ M ′d(q1, q2)|U(H(q1, t))| + M ′′d(q1, q2)|∇d ′

2(r(H(q2, t)))|for some constantsM ′ and M ′′. Since |U | ≤ L and|∇d ′

2(r(H(q2, t)))| = |r(H(q2, t)) − c(H(q2, t))| ≤ M ′′′ forsomeM ′′′ from the boundedness of the environment,

|U(H(q1, t)) × ∇d ′1(r(H(q1, t)))

−U(H(q2, t)) × ∇d ′2(r(H(q2, t)))|

≤ Nd(q1, q2)

for someN .So,H (q, t) also satisfies Lipschitz condition, i.e.,

|H (q1, t) − H (q2, t)| < Md(q1, q2)

for some constantM for anyq1 andq2. Therefore, there ex-ists H(q, t) which satisfies the above condition and is alsocontinuous ont and the initial condition, i.e.,q (Brand 1966).




For gradient ascent with double or triple equidistance, sinceH (q, t) = πT ∇(D1) or H (q, t) = ∇(D1 − D2), etc., it alsosatisfies the condition.

At this point, we could useH as our accessibility criterionand our deformation retraction. From here we can concludethatCFijk is a deformation retraction of a junction regionJijk

usingH . In implementation we useH , which is defined asH = H(q, t)/D(H(q, t)).

Appendix D: Proof of Lemma 1 (Section 4.4)

Here we prove Lemma 1 in Section 4.4, which states thatfor two adjacent junction regions,Jijkl andJijkm, if the one-tangent edgeRijk = ∅, thenCFijkl andCFij lm intersect eachother.

Proof. First, we define

SRijk = {q : 0 < Di(q) = Dj(q) = Dk(q),

R(q) is tangent toFijk}. (30)

Note that SRijk does not have the conditionDk(q) <

Dl(q), l = i, j, k, andSRti always exists even ifRti does not.Now SRijk is a subset of the union of two adjacent junctionregionsJijkl andJijkm, and thus can be viewed as the structurewhich connects the two junction regionsJijkl andJijkm.

Recall the definition ofCFijkl/m, introduced in Section 4.5:

CFijkl/m = {q ∈ FS|0 < Di(q) = Dj(q) =Dk(q) = Dl(q) ≤ Do(q),

o = m, i, j, k, l}.Clearly, CFijkl ⊂ CFijkl/m, and if CFijkl intersects anotherfour-way equidistant faceCFijkm, forming a rod-GVG edgeCFijklm, then one can sayCFijklm decomposesCFijkl/ into twosets:CFijkl and the setVijkl = CFijkl/m \ CFijkl = {q : 0 <

Dm(q) < Di(q) = Dj(q) = Dk(q) = Dl(q)}. Note thatVijkl may not exist at all, ifCFijkl does containCFijklm. On theother hand,CFijkl may not exist whileVijkl exists. Finally, ifbothCFijkl andVijkl exist, thenCFijklm exists, sinceCFijklm isa boundary betweenCFijkl andVijkl.

Now consider a configurationq in SRijk such that the con-tact point of the rod toFijk is Fijkm, a point-GVG meetpoint.At configurationq, Dm(q) ≤ Di(q). As the rod travels alongSRijk away fromq “towards” the obstacleCl, there must be aconfigurationq ′ whereDm = Di , i.e.,q ′ ∈ CF∗

ijkl, and there

is a configurationq ′′ in the neighborhood ofq ′ (onSRijk) suchthat Dm(q ′′) > Di(q

′′). Since we assume thatRijk does notexist, atq ′′, Di > Dl. From the continuity of the distance,we haveDm(q ′) = Di(q

′) ≥ Dl(q′). If equality holds, then

q ′ is an element ofCFijklm. If inequality holds, this impliesthatDm(q ′) = Di(q

′) = Dj(q′) = Dl(q

′) < Dl(q). This, inturn, means thatq ′ ∈ Vijlm and, from the comment above, thisimplies thatCFijklm is not an empty set. Thus, in both cases,CFijkl andCFijkm must intersect each other; that is,CFijklm isnot an empty set. �

Appendix E: Proof of Lemma 2 (Section 4.5)

Here we prove Lemma 2, which states that two adjacent rodfour-way equidistant faces are connected either by a one-tangent edge or a rod-GVG edge, but not by both.

Proof. First we define some notation. Given a set of configu-rationsSQ, let Dmin(S) be

Dmin(SQ) = minq∈SQ

D(q),

and, similarly, given a set of pointsSP in R, let dmin(SP ) be

dmin(SP ) = minx∈SP

d(x).

Recall that for a given configurationq in Rijk, D(q) = d(xq),wherexq is the “contact point” between the rod and the point-GVG edgeFijk. LetF R

ijkbe the set of “contact points” forRijk.

ThenF Rijk

⊂ Fijk. Therefore, we have

dmin(Fijk) ≤ dmin(FR

ijk) = Dmin(Rijk). (31)

Now, consider the configuration space for a rod with a fixedorientation, operating in the same environment, but withoutrotation. Then the configuration space for this robot is a three-dimensional Euclidean space, populated by convex obstacles.Therefore, we can define a point-GVG-like structure in thisspace. LetF (θ,φ)

ijk be the point-GVG edge in a configurationspace for a rod, with the fixed orientation(θ, φ), operating inthe given workspace without rotation. Also we defineF

(θ,φ)

ijkl

andF(θ,φ)

ijklm similarly. Then we have

dmin(F(θ,φ)

ijk ) ≤ dmin(Fijk). (32)

Now we prove the lemma. The proof is by contra-diction. Let us assume that bothRijk and CFijklm arenot empty sets. Consider a configurationqg in a rod-GVG edgeCFijklm. Then(x(qg), y(qg), z(qg)) is a five-wayequidistant pointF

(θ(qg),φ(qg))

ijklm in a three-dimensional fixed-orientation configuration space for a rod with the orientation(θ(qg), φ(qg)). By definition, this point is also inF

(θ(qg),φ(qg))

ijkl

and F(θ(qg),φ(qg))

ijkm , which are zero-dimensional. This means

F(θ(qg),φ(qg))

ijkl = F(θ(qg),φ(qg))

ijkm . SinceF(θ(qg),φ(qg))

ijkl andF(θ(qg),φ(qg))

ijkm

are the endpoints of the point-GVG edgeF(θ(qg),φ(qg))

ijk , such that

F(θ(qg),φ(qg))

ijkl andF(θ(qg),φ(qg))

ijkm are the same point, means that the

point-GVG edgeF(θ(qg),φ(qg))

ijk itself consists of only one point,i.e.,(x(qg), y(qg), z(qg)).

In other words, the point(x(qg), y(qg), z(qg)) is the onlypoint ofF

(θ(qg),φ(qg))

ijk . Using eqs. (31) and (32), we have

D(qg) = d(x(qg), y(qg), z(qg))

= dmin(F(θ(qg),φ(qg))

ijk )

≤ dmin(Fijk) ≤ Dmin(Rijk), (33)




i.e.,

D(qg) ≤ Dmin(Rijk). (34)

This is true for allqg in CFijklm. Now let us consider the con-figurationqr in Rijk, D(qr) > Dmin(Rijk). Recall that accessi-bility implies that there is a configurationH(qr, 1) in CFijkl

such thatD(H(qr, 1)) > D(qr), but this is impossible fromeq. (34). Therefore, it is not possible that bothRijk andCFijklm

are not empty sets. �

Appendix F: Proof of Lemma 4 (Section 4.5)

Here we prove Lemma 4, which is that a two-tangent edgeRij/kl is a one-dimensional manifold. The proof is by show-ing the gradient ofgij/kl, which defines the two-tangent edgeimplicitly, is full rank, and concluding that the two-tangentedge is a one-dimensional manifold by the pre-image theo-rem (Abraham, Marsden, and Raitu 1988).

Proof. Recall that the two-tangent edgeRij/kl can be consid-ered as the roots of

gij/kl =

Di − Dj

Di − Dk

Di − Dl

PQ · (ci − cj )

. (35)

We show thatRij/kl is a one-dimensional manifold byshowing that the differential ofgij/kl, dgij/kl, has a full rank(Canny 1988). Note that the first three rows are just the differ-ences between the distance function; thus, the differentials ofthese rows are just the differences between the distance gra-dient of each object. The last row, however, requires a carefulconsideration.

To derive the differential ofPQ · (ci − cj ), first, we derivedPQ. Since

PQ = l cosθ cosφ

l sinθ cosφ−l sinφ

,

we have

dPQ = 0 0 0 −l sinθ cosφ −l cosθ sinφ

0 0 0 l cosθ cosφ −l sinθ sinφ

0 0 0 0 −l cosφ

.

Let s = [− sinθ cosφ cosθ cosφ 0]T and t =[− cosθ sinφ − sinθ sinφ − cosφ]T (Figure F1). Thenwe have

dPQ = [0 0 0 ls lt

].

Note that boths andt are normal toPQ. Now, we derive theequation for dci , which we denote

dci = [cix ciy ci z ci θ ciφ

] ∈ R3×5, cix, ciy, ci z, ci θ , ciφ ∈ R

3.

(36)

PQ

θφ

x

y

z

t

s

Fig. F1. Rod configuration is represented usingZYZ-Eulerangles. The vectors is a vector on thexy-plane, normal to theprojection ofPQ onto thexy-plane (s is also normal toPQ),andt is a unit vector normal toPQ ands. Therefore, we havethe following relationships:(PQ/l) × t = s, t × s = PQ/l

ands × (PQ/l) = t.

First note thatPQ and the line defined byci + uni (c) (u ∈R) must intersect always (Figure F213). Therefore, using theexpression for the distance between two lines, we have

(PQ × ni ) · (P − ci) = 0. (37)

This should be satisfied as the rod makes a differential motion,and we have

d((PQ × ni ) · (P − ci)) = 0

by taking the differential of eq. (37).Thus we have

(ni × PQ)T((−DiLci

+ I )(cix)) + (PQ × ni )

T

1

00

= 0

(38)


+ I )(ciy)) + (PQ × ni )

T

0

10

= 0

(39)


+ I )(ci z)) + (PQ × ni )

T

0

01

= 0

(40)

13. In the figure, the notation is a little sloppy. To be more precise,PQ+dPQshould be writtenPQ + dPQ dq wherePQ ∈ R

3, dPQ ∈ R3×5 and dq =

(dx dy dz dθ dφ)T ∈ R5 and so on.




Ci

ci

ri

PQdPQ

PQ

cidcidci

ninidni

Fig. F2. PQ and the line defined byci + un (u ∈ R)must intersect, i.e.,(PQ × ni ) · (P − ci) = 0, and thusd((PQ × ni ) · (P − ci)) = 0. Also note that, whenri is aninterior point of PQ, as shown in this figure,PQ must benormal toni , thusPQ · ni = 0 and d(PQ · ni ) = 0.


+ I )(ci θ )) + (ls × ni ) · (P − ci) = 0

(41)


+ I )(ciφ)) + (lt × ni ) · (P − ci) = 0

(42)

whereLp denotes the Weingarten map (Thorpe 1979) ofCi

at pointp, i.e.,Lp(v) = −∇vn.

Now, we derive the differential ofgij/kl:

dgij/kl =

dDi − dDj

dDi − dDk

dDi − dDl

d(PQ · (ci − cj ))

∈ R

4×5. (43)

Recall that

(dDi)T =

1

Di

ri − ci

(ri − ci) · (−u Sθ Cφ u Cθ Cφ 0)

(ri − ci) · (−u Cθ Sφ − u Sθ Sφ − u Cφ)

∈ R5 (44)

whereu = |P − ri |. Usings andt defined above, we have

(dDi)T = 1

Di

ri − ci

(ri − ci) · (us)(ri − ci) · (ut)

(45)

= ni

ni · (us)ni · (ut)

∈ R

5, whereni ∈ R3, (46)

sinceni = (ri − ci)/|ri − ci | = (ri − ci)/Di . Therefore, thefirst row of dgij/kl is

(dDi − dDj)T =

ni − nj

u(ni − nj ) · su(ni − nj ) · t

∈ R

5

whereni − nj ∈ R3, (47)

and the second and third rows have similar forms. The fourthrow of dgij/kl is

(d(PQ · (ci − cj )))T = (dPQ)T(ci − cj )

+d(ci − cj )TPQ (48)

= dPQT(ci − ri + rj − cj )

+d(ci − ri + rj − cj )TPQ (49)

= DidPQT(−ni + nj )

+d(−ni + nj )TPQ. (50)

So, our goal now is to show that the last row of eq. (43) islinearly independent of the other rows of eq. (43). We do thisin three cases, depending on the location ofri andrj on therod.

Case 1 (ri = rj = P ). In this case, we haveu = 0, andtherefore

(dDi − dDj)T =

ni − nj

00

. (51)

Note that in this caseciθ = 0 andciφ = 0, so eq. (48) becomes

(d(PQ · (ci − cj )))T = (dPQ)T(ci − cj )

+d(ci − cj )TPQ (52)

=

(cix − cj x) · PQ

(ciy − cj y) · PQ

(ciy − cj y) · PQ

ls · (ci − cj )

lt · (ci − cj )

. (53)

Recall thats and t are the last two columns of dPQ. Sincebothls · (ci − cj ) andlt · (ci − cj ) cannot be zero at the sametime (if so, that would imply(ci − cj ) is normal tot ands,and then this implies that(ci − cj ) is parallel toPQ, which isnot possible), so the fourth row is linearly independent of thefirst row.

Case 2 (ri is an interior point of PQ). In this case,PQmust be normal tori −ci = Di(ni ), i.e.,PQ ·(ri −ci) = 0, andafter a small movement of the rod, this condition must alsobe satisfied and thus d(PQ · (ri − ci)) = 0 also (Figure F2).




So, using(ri − ci) = Dini , we have

0 = dPQT(ri − ci) + d(ri − ci)TPQ (54)

= dPQT(ri − ci) + Did(ni )TPQ (55)

= dPQT(ri − ci) − Di

Lcicix

Lcici y

Lcici z

Lcici θ

Lciciφ

PQ. (56)

Therefore, we have

dPQT(ri − ci) = Di

Lcicix

Lcici y

Lcici z

Lcici θ

Lciciφ

PQ. (57)

Substituting eq. (57) into eq. (49), we have (see Figure F3)

(d(PQ · (ci − cj )))T =

((I − DiLci)cix) · PQ

((I − DiLci)ciy) · PQ

((I − DiLci)ci z) · PQ

((I − DiLci)ci θ ) · PQ

((I − DiLci)ciφ) · PQ

−

((I − DjLcj)cj x

) · PQ((I − DjLcj

)cj y) · PQ

((I − DjLcj)cj z

) · PQ((I − DjLcj

)cj θ) · PQ

((I − DjLcj)cj φ

) · PQ

.

(58)

Sinceri can be represented as

ri = ci + Dini ,

we have

ri θ = ciθ + Diθni + Dini θ (59)

= ciθ + u(s · ni )ni − DiLi(ci θ ) (60)

= (I − DiLci)ci θ + u(s · ni )ni , (61)

usingDiθ = us · ni14 andni θ = −Li(ci θ ), or we can write

(I − DiLci)ci θ = ri θ − u(s · ni )ni . (62)

We can derive similar equations for the other coordinates.Substituting eq. (62) and similar equations for the other coor-dinates into eq. (58), we have

(d(PQ · (ci − cj )))T =

PQ · (rix − rj x)

PQ · (riy − rj y)

PQ · (ri z − rj z)

PQ · (ri θ − rj θ)

PQ · (riφ − rj φ)

. (63)

14. As in eq. (36),Diθ denotes the differential ofDi in θ coordinates, etc.

P

Q

ri

ci

Ci

ni

ni × PQ

(I − DiLci)cix

Fig. F3. Whenri is an interior point ofPQ, PQ, andni arenormal to each other. Also note that the vector(I −DiLci

)cix

lies on the plane defined byPQ and ni × PQ, since cix

lies on the tangent space ofCi at ci , and Lci(cix), which

is the change of the normal vector along the directionof cix , also lies on the tangent space ofCi at ci . Finally,note that eqs. (38)–(42) involve the expression of the form(ni × PQ)T

((−DiLci

+ I )(cix))

and eq. (58) involves theexpression of the form(ni × PQ) · (

(−DiLci+ I )(cix)

).

That is, those equations have the expression for thePQ andni × PQ components of the vector(−DiLci

+ I )(cix). Thesame can be said ofciy , ciz, etc.

Now, we show that this vector is linearly independent ofeq. (47) and, for this, it is enough to show that the three-vectorwhose components are the first three elements of eq. (63) islinearly independent of the three-vector whose componentsare the first elements of eq. (47), which is, in fact, parallel to

ni − nj . (64)

Note that eq. (64) is also parallel toci − cj .Now, we only need to consider the translational motion of

the rod since the first three components of eq. (63) representthe differential in translational coordinates. A translationaldifferential motion of the rod can be decomposed intoni , PQ,andni × PQ components, which we can write as

dh = dhni+ dhPQ + dhni×PQ. (65)

For this motion, the differential change inri can be written as

dri = drini+ driPQ + drini×PQ. (66)

Note that for the motion in theni direction, dhni, the change

in ri is exactly the same as dhni. For motion in thePQ direction,

ri does not change. For dhni×PQ, the change inri is parallel tothe tangent space ofCi at ci (and not necessarily parallel todhni×PQ). So we can rewrite eq. (66) as (Figure F4)

dri = drini+ driPQ + drini×PQ (67)

= dhni+ 0 + (dhni×PQ + dvPQ)., (68)




dhdhni

dhPQ

dhni×PQ

cici

c′i

riri

r′i

Fig. F4. Translational motion of the rod can be decomposedinto three directions:ni , PQ, andni × PQ.

Here, dvPQ is the change ofri in the PQ direction, whosemagnitude is linear on the magnitude of dhni×PQ, i.e.,

dvPQ = ai |dhni×PQ|uPQ. (69)

whereuPQ is a unit vector parallel toPQ. Then, we have

PQ · rix = PQ · (dhni+ 0 + dhni×PQ

+ai |dhni×PQ|uPQ) (70)

= ai |PQ||dhni×PQ| (71)

whereai is a proportional constant.Therefore, for the first three components of eq. (63) we

have

[ciuni×PQ − cj unj×PQ

](72)

which cannot be parallel toni − nj .

Case 3 (ri = rj = Q). In this case, dri = dQ, i.e.

dri = dQ = 1 0 0

0 1 00 0 1

ls lt

, (73)

wheres andt are three-dimensional column vectors as definedabove. Since

ci = ri − Dini , (74)

we have

dci = dri − dDini − Dini (75)

= dri − ni

[nT

ils · ni lt · ni

] + DiLci(dci). (76)

SinceCi is convex andni is defined to be the normal vectorpointing away from the obstacleCi , the curvatures are nega-tive. Then,(I − DiLci

) is non-singular since the eigenvaluesof (I −DiLci

) are the principal curvatures, andDi > 0. Thus,

rearranging eq. (76), we obtain

dci = (I − DiLci)−1(dri − ni

[nT

ils · ni lt · ni

])

(77)

= (I − DiLci)−1

I3×3 − ninT

i

l(s − (s · ni )ni )

l(t − (t · ni )ni )

T

. (78)

Note thats − (s · ni )ni andt − (t · nini ) are the respectivecomponents ofs andt, which are parallel to the tangent spaceof Ci . Let Ui = (I3×3 − ninT

i). Essentially,Ui projects a vec-

tor onto the tangent space ofCi at ci . Then, eq. (78) can berewritten as

dci = (I − DiLci)−1Ui

[I3×3 ls lt

]. (79)

Now, letWi = (I − DiLci)−1Ui , and substituting eq. (79)

into eq. (48), we have

(d(PQ · (ci − cj )))T = (dPQ)T(ci − cj ) + d(ci − cj )

TPQ

= Di

[03×3 ls lt

]T(−ni + nj )

+ (Wi

[I3×3 ls lt

]−Wj

[I3×3 ls lt

])TPQ (80)

= 0

lDis · (−ni + nj )

lDit · (−ni + nj )

+ (Wi − Wj)

TPQlsT

j(Wi − Wj)

TPQltT

j(Wi − Wj)

TPQ

. (81)

If (Wi − Wj)TPQ is not linearly dependent ofni − nj , which

are the first three components of d(Di −Dj), d(PQ · (ci −cj ))

is linearly independent of d(Di − Dj). If (Wi − Wj)TPQ is

linearly dependent onni −nj , one can write(Wi −Wj)TPQ =

K(ni − nj ). Then, eq. (81) becomes

(d(PQ · (ci − cj )))T =

K(ni − nj )

lDis · (nj − ni ) + lKsTj(ni − nj )

lDit · (nj − ni ) + lKtTj(ni − nj )

, (82)

which cannot be parallel to d(Di − Dj) for Di > 0. Thus, weconclude that for each of the three cases, a two-tangent-edgeis a one-dimensional manifold. �

Appendix G. Proof of Lemma 5 (Section 4.5)

Here we prove Lemma 5, which states that a two-tangent edgehas one connected component on a rod four-way equidistantfaceCFijkl homeomorphic toS2.

Proof. First, let us consider a rod with zero length, denotedlo. In other words,lo is a point with direction. Forlo, each




Rij/kl has one component homeomorphic toS1. This is truebecause if any configurationqo belongs toRij/kl, then all ofthe configurations with endpoints located at the same point asthe endpoint ofqo, and having the direction such that they aretangent toFij , also belong toRij/kl. This is because all suchconfigurations occupy the same workspace volume asqo, andhence all such configurations are four-way equidistant toCi ,Cj , Ck, andCl, and do not intersect any of the obstacles. Now,imagine the change inRij/kl as the length of the rod increasesin the same environment. (We still assume thatCFijkl is home-omorphic toS2 in this process.) Now assume that for somevalue of the length, we have aRij/kl with two or more com-ponents. SinceRij/kl changes “continuously” as the length ofthe rod changes, this means that there must be some “critical”length of the rod at which either one component shrinks toa point or two components intersect each other transversally.However, this means that at this “critical” length,Rij/kl is nota manifold. This is contradictory to Lemma 4, which does notdepend on the length of the rod. Therefore,Rij/kl can haveonly one component. �

Appendix H: Index to Multimedia Extensions

We have included some animations of the rod tracing the rod-HGVG in three dimensional spaces (see below). The multi-media extension page can be found at http://www.ijrr.org.

Extension 1 shows the rod exploring a rectangular enclo-sure shown in Figures 20 and 21. In this example, there isno rod-GVG edges since the rod is “short”. Therefore, therod-HGVG forms a connected set with only the one-tangentedges (shown in sky blue) and the two-tangent edges (shownin yellow).

Extension 2 shows the rod exploring the space shown inFigure 22, a rectangular enclosure with a wall in the middle.In this example, there are the rod-GVG edges (shown in pink)near the both ends of the room.

Table of Multimedia ExtensionsExtension Type Description

1 Video The rod-HGVG in a rectangularenvironment, withoutthe rod-GVG edges

2 Video The rod-HGVG in a largerectangular environment,with the rod-GVG edges

References

Abraham, R., Marsden, J. E., and Raitu, T. 1988.Manifolds,Tensor Analysis and Applications, 2nd edition, Springer-Verlag, New York.

Acar, E. U., Choset, H., and Atkar, P. N. 2001. Completesensor-based coverage with extended-range detectors: a

hierarchical decomposition in terms of critical points andVoronoi diagrams.Proceedings of the 2001 IEEE/RSJ In-ternational Conference on Intelligent Robots and Systems(IROS), Maui, HI, October.

Aurenhammer, F. 1991.Voronoi diagrams—a survey of a fun-damental geometric data structure.ACM Computing Sur-veys 23(3):345–405.

Aurenhammer, F. and Klein, R. 2000. Voronoi diagrams.Handbook of Computational Geometry, J. Sack and G.Urrutia, editors, Elsevier, Amsterdam, Chapter V, pp. 201–290.

Borenstein, J. and Koren, Y. 1998. Real-time obstacle avoid-ance for fast mobile robots.IEEE Transactions on Systems,Man and Cybernetics 19(5):1179–1187.

Brand, L. 1966.Differential and Difference Equations, Wiley,New York.

Bredon, G. 1995.Topology and Geometry, 3rd edition,Springer-Verlag, Berlin.

Brooks, R. A. 1983. Solving the find-path problem by goodrepresentation of free space.IEEE Transaction on Systems,Man and Cybernetics 13(3):190–197.

Brooks, R. A. and Lozano-Pérez, T. 1983. A subdivision al-gorithm in configuration space for Findpath with rotation.Proceedings of the International Joint Conference on Arti-ficial Intelligence, Karlsruhe, Germany, August, pp. 799–806.

Canny, J. F. 1988.The Complexity of Robot Motion Planning,MIT Press, Cambridge, MA.

Choset, H. and Burdick, J. 2000a. Sensor-based motion plan-ning: the hierarchical generalized Voronoi graph.Interna-tional Journal of Robotics Research 19(2):96–125.

Choset, H. and Burdick, J. 2000b. Sensor-based motion plan-ning: incremental construction of the hierarchical gener-alized Voronoi graph.International Journal of RoboticsResearch 19(2):126–148.

Choset, H. and Burdick, J. W. 1996. Sensor-based planningfor a planar rod robot.Proceedings of the IEEE Interna-tional Conference on Robotics and Automation (ICRA),Minneapolis, MN.

Choset, H. and Lee, J. Y. 2001. Sensor-based construction ofa retract-like structure for a planar rod robot.IEEE Trans-action of Robotics and Automation 17(4):435–449.

Chuang, J. H. and Ahuja, N. 1998. An analytically tractablepotential fidel model of free space and its application inobstacle avoidance.IEEE Transactions on Systems, Manand Cybernetics – Part B: Cybernetics 28(5):729–736.

Cox, J. and Yap, C. K. 1991. On-line motion planning: caseof a planar rod.Annals of Mathematics and Artificial In-telligence 3:1–20.

Dattasharma, A. and Sathiya Keerthi, S. 1995. An aug-mented Voronoi roadmap for 3D translational motionplanning for a convex polyhedron moving amidst con-vex polyhedral obstacles.Theoretical Computer Science140(2):205–230.




Foskey, M., Garber, M., Lin, M., and Monocha, D. 2000.A Voronoi-based hybrid motion planner for rigid bodies.Technical Report TR00-025, UNC Chapel Hill ComputerScience.

Kavraki, L. 1997. Part orientation with programmable vectorfields: two stable equilibria for most parts.Proceedings ofthe IEEE International Conference on Robotics and Au-tomation (ICRA), Albuquerque, NM, April.

Kavraki, L. and Latombe, J.-C. 1994. Randomized prepro-cessing of configuration space for path planning: articu-lated robots.Proceedings of the IEEE/RSJ InternationalConference on Intelligent Robots and Systems (IROS), Mu-nich, Germany, pp. 1764–1771.

Kavraki, L., Svestka, P., Latombe, J.-C., and Overmars, M.1996. Probabilistic roadmaps for path planning in high-dimensional configuration spaces.IEEE Transactions onRobotics and Automation 12(4):566–580.

Khatib, O. 1986. Real-time obstacle avoidance for manipula-tors and mobile robots.International Journal of RoboticsResearch 5:90–98.

Khosla, P. and Volpe, R. 1988. Superquadric artificial poten-tial for obstacle avoidance and approach.Proceedings ofthe IEEE Conference on Robotics and Automation (ICRA),Philadelphia, PA, pp. 1778–1784.

Koren, Y. and Borenstein, J. 1991. Potential field methodsand their inherent limitations for mobile robot navigation.Proceedings of the IEEE Conference on Robotics and Au-tomation (ICRA), Sacramento, CA, pp. 1398–1404.

Kuan, D. T., Zamiska, J. C., and Brooks, R. A. 1985. Naturaldecomposition of free space for path planning.Proceed-ings of the IEEE International Conference on Robotics andAutomation (ICRA), St. Louis, MO, pp. 168–173.

Kuffner, J. J. and LaValle, S. M. 2000. RRT-connect: an ef-ficient approach to single-query path planning.Proceed-ings of the IEEE International Conference on Roboticsand Automation (ICRA), San Francisco, CA, April 24–28,pp. 995–1001.

Latombe, J.-C. 1991.Robot Motion Planning, Kluwer Aca-demic, Boston, MA.

Latombe, J.-C., Kavraki, L. E., Svestka, P., and Overmars, M.1996. Probabilistic roadmaps for path planning in high-dimensional configuration spaces.IEEE Transactions onRobotics and Automation 12(4):566–580.

LaValle, S. M. 1998. Rapidly-exploring random trees: a newtool for path planning. Technical Report TR 98-11, Com-puter Science Department, Iowa State University, October.

Lee, J. Y., Choset, H., and Rizzi, A. A. 2001. Sensor-basedplanning for rod-shaped robots in three dimensions: piece-wise retracts ofR3 × S2. Proceedings of the IEEE Inter-national Conference on Robotics and Automation (ICRA),Seoul, Korea, May 21–26, pp. 991–999.

Munkres, J. R. 2000.Topology, Prentice-Hall, EnglewoodCliffs, NJ.

Nilsson, N. J. 1969. A mobile automation: an application ofartificial intelligence techniques.Proceedings of 1st Inter-national Joint Conference on Artificial Intelligence, Wash-ington, DC.

Noborio, H., Haniwa, T., andArimoto, S. 1989.A feasible mo-tion planning algorithm for a mobile robot on a quadtreerepresentation.Proceedings of IEEE International Confer-ence on Robotics and Automation (ICRA), Scottsdale, AZ,May, pp. 327–332.

Ó’Dúnlaing, C., Sharir, M., andYap, C. K. 1986. GeneralizedVoronoi diagrams for moving a ladder. I: topological anal-ysis.Communications on Pure and Applied Mathematics39:423–483.

Ó’Dúnlaing, C. and Yap, C. K. 1985. A “retraction” methodfor planning the motion of a disc.Journal of Algorithms6:104–111.

Overmars, M. H. 1992. A random approach to motion plan-ning. Technical Report RUU-CS-92-32, Department ofComputational Science, Utrecht University, Utrecht, theNetherlands, October.

Pisula, C., Hoff, K., Lin, M., and Manocha, D. 2000. Random-ized path planning for a rigid body based on hardware ac-celerated Voronoi sampling.Proceedings of the 4th Work-shop on Algorithmic Foundations of Robotics (WAFR),Hanover, NH, March 16–18.

Press, W. H., Teukolsky, S.A.,Vetterling, W. T., and Flannery,B. P. 2002.Numerical Recipes in C++: The Art of Scien-tific Computing, 2nd edition, Cambridge University Press,Cambridge.

Rimon, E. 1997. Construction of C-space roadmaps from localsensory data. What should the sensors look for?Algorith-mica 17(4):357–379.

Sharir, M. 1997. Algorithmic motion planning.Handbook ofDiscrete and Computational Geometry, J. E. Goodman andJ. O’Rourke, editors, CRC Press, Boca Raton, FL, pp. 733–754.

Thorpe, J. A. 1979.Elementary Topics in Differential Geom-etry, Springer-Verlag, Berlin.

Wilmarth, S. A., Amato, N. M., and Stiller, P. F. 1999. Motionplanning for a rigid body using random networks on themedial axis of the free space.Proceedings of the 15th An-nual ACM Symposium on Computational Geometry, Mi-ami Beach, FL, June, pp. 173–180.

Yu,Y. and Gupta, K. 1999. On sensor-based roadmap: a frame-work for motion planning for a manipulator arm in un-known environment.Proceedings of the IEEE/RSJ Inter-national Conference on Intelligent Robotics and Systems,Kyongju, Korea, pp. 1919–1924.



Documents

The International Journal of Robotics Research · Ji Yeong Lee Korea Institute of Science and Technology (KIST) Seoul, Korea [email protected] Howie Choset Carnegie Mellon University