Efficient Optimal Multi-Location Robot Rendezvouskalton/papers/ubctechrep2007b.pdfEfficient Optimal Multi-Location Robot Rendezvous Ken Alton and Ian M. Mitchell Department of Computer

Efficient Optimal Multi-Location Robot Rendezvous

Ken Alton and Ian M. Mitchell

Department of Computer Science

University of British Columbia

UBC CS TR-2007-17

October 5, 2007

Abstract

We present an efficient algorithm to solve the problem of optimal multi-location robot rendezvous. The rendezvous

problem considered can be structured as a tree, with each node representing a meeting of robots, and the algorithm

computes optimal meeting locations and connecting robot trajectories. The tree structure is exploited by using dynamic

programming to compute solutions in two passes through the tree: an upwards pass computing the cost of all potential

solutions, and a downwards pass computing optimal trajectories and meeting locations. The correctness and efficiency

of the algorithm are analyzed theoretically, while a discrete robotic clinic problem and a continuous robot arm problem

demonstrate the algorithm’s practicality.

This is an extended version of a paper submitted to ICRA 2008 [1].

I. I NTRODUCTION

Path planning is a central area of study in robotics, but most current algorithms find an efficient path for only

a single robot at a time. Coordinated path planning for multiple robots has received increased attention recently,

but is often difficult because the complexity of many techniques increases considerably with each additional robot.

For example, using dynamic programming to find optimal collision-free paths is reasonable for 2 robots [2], but it

becomes intractable with the addition of more robots because the planning is done in the high dimensional cross

product of the robot state spaces, and the complexity of such algorithms grows exponentially with dimension.

We focus here on a particular type of coordinated robot path planning problem and, in so doing, we are able

to find a very efficient algorithmic solution. More specifically, we examine optimal coordinated multi-robot multi-

location rendezvous, an extension of the frugal feeding problem considered in [3]. Given a hierarchical structure

that describes which robots are to meet and which robots are to continue on to future meetings, our dynamic

programming algorithm can compute the optimal meeting locations and the optimal paths between meetings with

complexity linear in the number of meetings. To simplify the scenarios, we assume central and complete knowledge

of the map(s) and robots states, and we ignore collisions between the robots. Despite this simplification, we believe

that core aspects of our algorithm for solving the robot rendezvous problem can be utilized in real world robot

applications. To demonstate the practical potential of our algorithm, we use it to solve two example problems: a

discrete problem involving a meeting hierarchy in a robotic medical clinic and a continuous problem involving

robotic arms cooperating to deliver some cargo from a source to a destination.

The main contribution of this paper is the presentation of a dynamic programming principle (DPP) and related

algorithm that can efficiently find optimal solutions to a broad category of hierarchical multi-location robot ren-

dezvous problems. An important property of the algorithm is that no calculation is done in a state space with

dimension greater than that of the individual robots. We define the type of problems considered and our notation in

Section III. Mathematical analysis in Section IV shows that the hierarchical problem structure implies that a DPP

can be used to efficiently compute costs of potential solutions. The algorithm presented in Section V utilizes the

DPP to find optimal meeting locations and connecting trajectories with a complexity linear in the size of the meeting

hierarchy. Finally, in Section VI we use the algorithm to solve both a discrete robotic clinic and a continuous robot-

arm rendezvous problem. Although the continuous problem is not analyzed with the same rigour as the discrete

problem, we offer the example as a proof of concept but leave the theory for future work.

II. RELATED WORK

Research in robot path planning is considered a central endeavor in the development of mobile robotics [4].

Approaches to robot path planning are diverse and include potential function methods, sampling-based methods,

trajectory planning methods and combinations of these [5], [6]. Most related to this work are the dynamic pro-

gramming algorithms for solving shortest path problems on grids [7], [8]; however, most path planning research,

including that described in the above references, focuses on single robots.

In this paper, we investigate a dynamic programming method for the multi-location rendezvous of multiple robots.

This problem is distinct from the generalized Fermat-Torricelli problem that finds a unique point minimizing the

sum of distances from a given set of points [9]. The hierarchical facility location problem [10] involves finding the

location of facilities of several levels to serve customers efficiently. This problem is more difficult than the one we

consider because typically an optimal number of facilities and the tree structure linking facilities must be found in

addition to optimal locations for those facilities. For the robot meeting problem, we assume that this tree structure

already exists and develop an efficient algorithm to optimize the location of meetings (i.e. facilities).

This work was motivated by [3], in which the authors describe a robot refueling problem where the goal is

to find the optimal rendezvous locations for a fuel-tanker robot to meet individually with each of a collection of

worker robots. Our work extends the restricted locations case of that paper. We generalize the problem to include

any hierarchy of robot meetings, and show that efficiency can be gained by assuming that the potential meeting

locations are nodes in a spatial graph on which commuting costs are only defined between neighboring nodes.

Finally, we demonstrate that computation on a grid can be used to approximate a continuous problem with a

potentially complex cost function.

III. PROBLEM DESCRIPTION

A robot meeting involves one or more robots colocating at a state within a discrete space. Except for the final

meeting, one robot continues on from each meeting to the next meeting.1 Imagine a meeting tree, where meetings

begin at the leaves and progress through the tree culminating in one final meeting at the root. For this problem

we fix the meeting structure as well as which robots must attend any particular meeting, but we allow the meeting

locations to vary. In other words, we are not concerned with the combinatorial aspects of designing a sequence of

meetings, which may be required for a general hierarchical facility location problem [10]. We wish to minimize,

over all possible meeting locations, the cost of a given meeting tree. The cost of a meeting tree is measured by

summing the cost of all robot commutes between meetings as well as the cost of each meeting. The avoidance of

collisions between the robots during the commutes is not considered.

A. Formulation

We make some formal definitions regarding the space in which the robots move and the cost of movement within

that space. Let there be a finite discrete state spaceX . We use the wordsstateand location interchangeably. If

it is possible to commute from statex ∈ X to statey ∈ X say thaty is a neighbor ofx and writey ∈ N (x),

whereN (x) is the set of neighbors ofx. For convenience we define a setN−1(x) = {y | x ∈ N (y)}, the set of

nodes for whichx is a neighbor. Letd : X 2 → R+ be a positive commuting cost function, whered(x, y) gives the

cost of commuting from statex ∈ X to y ∈ X andd(x, y) =∞ if y /∈ N (x). Note that the above formulation is

equivalent to a directed graphG = (V, E), whereV = X , an edgee = (x, y) ∈ E if y ∈ N (x), and the weight of

e is d(x, y). Let z = (z1, z2, . . . zL), be a trajectory throughX with eachzl ∈ X and such thatzl+1 ∈ N (zl), for

1 ≤ l ≤ L(z)− 1. We may useL in place ofL(z) where the trajectoryz is obvious. For example, we may refer

to the last element inz as simplyzL. Defineλd(z) to be the total cost of trajectoryz:

λd(z) =L(z)−1∑

l=1

d(zl, zl+1), (1)

whereL(z) is the number of nodes in trajectoryz. Defineδd(x, y) to be the minimum total cost of any trajectory

from x to y under the commuting cost functiond:

δd(x, y) = minz∈Z(x,y)

λd(z),

whereZ(x, y) is the set of all trajectories that begin atx and end aty. Note thatδd(x, x) = 0 and δd(x, y) > 0

for x 6= y.

A meeting tree structure is used to define the problem, as well as compute and store potential solutions. LetΥ

be ameeting treewith ρ being theroot meeting node. Each leaf node corresponds to a starting state of a single

robot, whereas each non-leaf node in the treeΥ corresponds to a single meeting of one or more robots. A meeting

1In fact, two or more robots may continue on from a meeting so long as they travel together in a group. Since the group moves as a single

entity to the next meeting, the essential properties of the problem remain the same.

of one robot involves requiring a robot to incur a meeting cost at some potentially unknown meeting location but

not to meet with any other robots before continuing on to its next meeting. From any meeting a single robot will

continue on to the next meeting, incurring a commuting cost, unless it is the root meeting, in which case the tree

of meetings is completed.

Eachmeeting nodeη contains the number of childrenK and an array of child nodesκ. To define a meeting

tree problem, each nodeη also contains a node problem definition, including a positivemeeting cost function

c : X → R+ and a positivecommuting cost functiond : X 2 → R+. The d function for the root nodeρ is of

no consequence to the problem and is considered undefined. Letc(x) = +∞ indicate that a meeting cannot take

place atx andd(x, y) = +∞ indicate that a robot cannot commute directly fromx to y. To define a meeting tree

solution, each nodeη must contain ameeting locationp ∈ X . We may also wish to determine ameeting-to-meeting

trajectory z for each nodeη with the exception of the rootρ. This trajectory is the path the robot follows from

the meeting corresponding toη to the parent meeting. Finally, for each nodeη we define ameeting value function

v : X → R and ameeting-plus-commute value functionw : X → R, which are explained further in Section IV and

are used by Algorithm 1 to compute the optimal meeting locations.

symbol type description

η.K Z+ number of children

η.κ meeting node array children

η.c function :X → R meeting cost function

η.d function :X 2 → R commuting cost function

η.p(∗) X meeting location

η.z(∗) a trajectory inX meeting-to-meeting trajectory

η.v(∗) function :X → R meeting value function

η.w(∗) function :X → R meeting-plus-commute value function

TABLE I

COMPONENTS OF MEETING NODEη. VARIABLES η.p, η.z, η.v, AND η.w MAY BE SUPERSCRIPTED USING∗ TO INDICATE THAT THEY

CORRESPOND TO AN OPTIMAL MEETING TREE.

As shown in Table I we use a dot notation to select components of a meeting nodeη. We define a recursive

function f that computes the value (i.e. total cost) of a meeting subtree rooted at nodeη.

f(η) =η.K∑k=1

[f(η.κ[k]) + δη.κ[k].d(η.κ[k].p, η.p)

]+ η.c(η.p), (2)

whereη.κ[k] is thekth child of η. Note thatf takes only a single nodeη as a parameter, but is really a function

of all meeting locations in the subtree rooted atη. This function adds the meeting costη.c(η.p) at the current

meeting location to the sum over all children of the valuef(η.κ[k]) of the meeting subtree rooted at the child

and the commuting costδη.κ[k].d(η.κ[k].p, η.p) from the child meeting location to the current meeting location. We

further define a related functiong that computes the value of a meeting subtree rooted at nodeη in addition to the

(a) (b) (c)

Fig. 1. (a) Map of medical clinic. The rooms in the clinic are labeled. A number represents the cost of the meeting or commute to which it is

adjacent; all unlabelled meetings and commutes have unit cost. (b) Meeting tree. (b) Legend of symbols used in the map and the meeting tree.

commute from the last meeting location to a statex ∈ X :

g(η, x) = f(η) + δη.d(η.p, x). (3)

Similar to f , g is a function of all meeting locations in the subtree rooted atη. Using functiong we can simplify

the definition off :

f(η) =η.K∑k=1

g(η.κ[k], η.p) + η.c(η.p). (4)

We use the dot notation on a treeΥ to select a particular component of all nodes; for example,Υ.p is a tree of

meeting locations. Such a treeΥ.p represents a potential solution to the problem definition treeΥ.(c, d). The goal

is to find a minimal-cost solution treeΥ.p∗ (which may not be unique) given problem treeΥ.(c, d):

Υ.p∗ ∈ arg minΥ.p

f(ρ). (5)

B. Example Problem: Patient Consultation

The setting of our example problem is a robotic medical clinic, a map of which is shown in Figure 1(a). The

state spaceX is represented by the bullets on the map and the possible commutes between states are represented

by dotted lines. The goal is to efficiently get a patient to a medical consultation with a robotic doctor and a robotic

nurse in a consultation room. The meeting tree used to define this problem is illustrated in Figure 1(b). The symbols

used to distinguish each type of meeting are labelled in Figure 1(c) and the states inX where each type of meeting

may occur are indicated by the appropriate meeting symbol on the map. In this example, at most one type of

meeting may be held at any one statex ∈ X , although in general this restriction is not required.

The patient begins in the patient room. One of 4 possible robotic wheelchairs (η = 4) must retrieve the patient

and take her to the consultation. The wheelchair may pick up the patient (η = ∇) in one of 3 states. One of 4

possible robotic nurses (η =⊗

) must attend the consultation, while picking up medications at the pharmacy on the

way. A robotic pharmacist must fill the medications (η =⊕

) at one of 4 states behind the counter of the pharmacy.

It then meets the nurse (η = �) at one of 3 states at the counter to hand over the medications. Finally, the doctor

must get from its initial state (η =./) to the consultation, while charging its batteries (η = ♦) at one of 4 charging

stations on the way. The consultation (ρ = ♥) may occur at one of 5 possible states.

Each meeting locationx has a costη.c(x) associated with it. Some meetings locations have a cost specified on the

map, while all others have unit cost. For a class of robots with multiple starting locations, such as the wheelchairs

and nurses, each robot within that class may have a different initial state and start-up cost but will otherwise be

identical, i.e., the costs for future commutes and meetings will not depend on the starting state of the robot. Each

commute from statex to y also has a costη.d(x, y) associated with it. Some commutes have a cost specified on

the map, while all others have unit cost. For simplicity, the commute costs for all robots are the same, with some

exceptions. One exception is that the pharmacist begins inside the rounded pharmacy counter and may only visit

states inside or on the counter. Furthermore, no other robot is permitted inside the pharmacy counter—they may

only go as far as states on the counter. Another exception is that the wheelchair robot is not permitted in the nurses

room or charge room while carrying the patient.

IV. DYNAMIC PROGRAMMING

Define theoptimal meeting value functionη.v∗(x) as

η.v∗(x) = minΥ.p, η.p=x

f(η). (6)

In other words,η.v∗(x) specifies the minimal total cost of the meeting subtree rooted atη subject to the condition

that the meeting corresponding toη is located atx. Define theoptimal meeting-plus-commute value functionη.w∗(x)

as

η.w∗(x) = minΥ.p

g(η, x). (7)

In other words,η.w∗(x) specifies the minimal total cost of the meeting subtree rooted atη and the commute from

the meeting locationη.p to x. Because of the recursive definitions off in (4) andg in (3), η.v∗ andη.w∗ can be

computed independent of any ancestors ofη.

We observe three important properties of the meeting tree problem that allow optimal meeting locationsΥ.p∗

(and optimal meeting-to-meeting trajectoriesΥ.z∗) to be found efficiently. The first observation is that the value

functionsη.v∗ andη.w∗ of any nodeη can be computed using just the value functionsη.κ[k].w∗ of the children

of η (see Theorem 1). This property allowsη.v∗ andη.w∗ to be computed for all nodesη in a single pass through

the treeΥ from the leaves towards the root (see Algorithm 2). The second observation is that the valueη.w∗(x)

for any nodeη and any statex ∈ X can be computed using only the valueη.v∗(x) at the same statex ∈ X and the

valuesη.w∗(y) at the neighboring statesy ∈ N−1(x) (see Theorem 2). This property allows Dijkstra’s algorithm

to be used to computeη.w∗ in a single pass through the states inX (see Algorithm 3). The final observation is

that the value functionsΥ.v∗ andΥ.w∗ together contain sufficient information to findΥ.p∗ andΥ.z∗ in a single

pass through the treeΥ from the root towards the leaves (see Theorem 3 and Algorithms 1 and 4). We formalize

the first two observations below, and the third in the next section.

The following theorem establishes a dynamic programming principle that for any nodeη defines the value function

η.v∗ using only the value functionsη.κ[k].w∗ of the children ofη and the meeting cost functionη.c, and defines

the value functionη.w∗ using only the value functionη.v∗ and the commute cost functionη.d.

Theorem 1:The value functionη.v∗ satisfies

η.v∗(x) =∑

k

η.κ[k].w∗(x) + η.c(x), (8)

for all x ∈ X for all nodesη in Υ. The value functionη.w∗ satisfies

η.w∗(x) = miny∈X

[η.v∗(y) + δη.d(y, x)] , (9)

for all x ∈ X for all nodesη in Υ.

The proof first shows that (8) holds for the case whereη is a leaf. It then shows that (8) holds for the alternative

case whereη has children. Finally, it shows that (9) holds for all nodesη in Υ.

Proof: Let η be a leaf inΥ and letx ∈ X . The following equation begins with the definition ofη.v∗ from

(6):

η.v∗(x) = minΥ.p, η.p=x

f(η)

= minΥ.p, η.p=x

η.c(η.p)

= η.c(x).

The second equality uses the definition of functionf from (4), consideringη has no children. The final equality is

due to the constraintη.p = x in the minimization. Since the sum in (8) disappears whenη has no children, (8) is

satisfied.

Let η be a node inΥ with one or more children. The following equation begins with the definition ofη.v∗ from

(6):

η.v∗(x) = minΥ.p, η.p=x

f(η)

= minΥ.p, η.p=x

{∑k

g(η.κ[k], η.p) + η.c(η.p)

}

= minΥ.p

∑k

g(η.κ[k], x) + η.c(x)

=∑

k

minΥ.p

g(η.κ[k], x) + η.c(x)

=∑

k

η.κ[k].w∗(x) + η.c(x).

The second equality uses the definition of functionf from (4). For the third equality, the constraintη.p = x is

removed by replacingη.p with x in the expression. Alsoη.c(x) is removed from the minimization since it does

not depend onΥ.p. For the fourth equality, themin operator is brought inside the sum because the∑

operator

is monotone in each addend andg(η.κ[k], x) for the kth child depends only on the meeting positionsp of nodes

within the the subtree rooted atη.κ[k]. The final equality follows from the definition ofη.w∗ from (7) and results

in the RHS of (8).

Therefore, since we have considered both the cases whereη is a leaf and whereη has children, (8) holds for all

nodesη in Υ for all x ∈ X .

Now let η be any node inΥ. The following equation begins with the definition ofη.w∗ from (7):

η.w∗(x) = minΥ.p

g(η, x)

= miny∈X

{min

Υ.p, η.p=yg(η, x)

}= min

y∈X

{min

Υ.p, η.p=y[f(η) + δη.d(η.p, x)]

}= min

y∈X

{min

Υ.p, η.p=yf(η) + δη.d(y, x)

}= min

y∈X[η.v∗(y) + δη.d(y, x)] .

The second equality holds because minimizing overΥ.p with fixed η.p = y and then minimizing over allη.p = y

has the same result as miniming overΥ.p all at once. The third equality is by (3). For the fourth equality we use

the constraintη.p = y to replaceη.p with y in δη.d(η.p, x) and then remove it from the inner minimization since it

is independent of the minimizer. The final equality follows from the definition ofη.v∗ in (6) and results in the RHS

of (9). Therefore, since we have not assumed a particularx ∈ X , (9) holds for all nodesη in Υ for all x ∈ X .

Corollary 1: The value functionsη.v∗ andη.w∗ satisfy

η.w∗(x) ≤ η.v∗(x), (10)

for all x ∈ X for all nodesη in Υ. Furthermore, for

x ∈ arg minx∈X

η.w(x), (11)

we haveη.w(x) = η.v(x).

Proof: Let x ∈ X andη be a node inΥ. By (9), we have

η.w∗(x) = miny∈X

[η.v∗(y) + δη.d(y, x)]

≤ η.v∗(x) + δη.d(x, x) = η.v∗(x)(12)

Let x be as defined in (11). Assume, for the moment, thatη.w(x) < η.v(x). Then by (9) there exists some state

y ∈ X such thaty 6= x and

η.w(x) = η.v(y) + δη.d(y, x)

Becauseδη.d(y, x) > 0 for y 6= x and by (12) we have

η.w(x) = η.v(y) + δη.d(y, x)

> η.v(y) ≥ η.w(y),

contradicting (11). Consequently, the assumption must be false and by (12) we haveη.w(x) = η.v(x).

The following theorem establishes a dynamic programming principle that for any nodeη and anyx ∈ X defines

the valueη.w∗ at x using only the valueη.v∗ at x; the valuesη.w∗ at y, such thaty ∈ N−1(x); and the commute

costsη.d from y to x, such thaty ∈ N−1(x).

Theorem 2:The value functionη.w∗ satisfies

η.w∗(x) = min{

η.v∗(x), miny∈N−1(x)

[η.w∗(y) + η.d(y, x)]}

(13)

for all x ∈ X for all nodesη in Υ.

Proof: Let η be a node inΥ and letx ∈ X . Let y∗ ∈ X be such that

y∗ ∈ arg miny∈X

[η.v∗(y) + δη.d(y, x)] . (14)

If y∗ = x, then we have by (9) and (14)

η.w∗(x) = η.v∗(x). (15)

Otherwise,y∗ 6= x. Assume, for the moment, there exists somey ∈ N−1(x) such that

η.w∗(y) + η.d(y, x) < η.v∗(y∗) + δη.d(y∗, x),

Then by (9) there exists some statey ∈ X such that

η.w∗(y) + η.d(y, x) = η.v∗(y) + δη.d(y, y) + η.d(y, x)

< η.v∗(y∗) + δη.d(y∗, x),

contradicting (14). Thus, our assumption was incorrect and for ally ∈ N−1(x)

η.w∗(y) + η.d(y, x) ≥ η.v∗(y∗) + δη.d(y∗, x). (16)

Let z∗ ∈ Z(y∗, x) be a minimal-cost trajectory fromy∗ to x; i.e.,

δη.d(y∗, x) = λη.d(z∗). (17)

Let y = z∗L−1 and notey ∈ N−1(x). Let z ∈ Z(y∗, y) be such that

z = (z∗1 , z∗2 , . . . , z∗L−1).

We have

η.w∗(y) + η.d(y, x) ≤ η.v∗(y∗) + δη.d(y∗, y) + η.d(y, x)

= η.v∗(y∗) + λη.d(z) + η.d(y, x)

= η.v∗(y∗) + λη.d(z∗)

= η.v∗(y∗) + δη.d(y∗, x)

(18)

The inequality is by (9). The first equality is because in order thatz∗ be a minimal-cost trajectory fromy∗ to x, z

must be a minimal-cost trajectory fromy∗ to y. The second equality results fromz∗ being the same asz with the

commute fromy to x appended. The final equality is by (17).

Because (16) holds for ally ∈ N−1(x) and (18) holds fory ∈ N−1(x) we know that

η.v∗(y∗) + δη.d(y∗, x) = miny∈N−1(x)

[η.w∗(y) + η.d(y, x)] , (19)

for the case wheny∗ 6= x.

The proof results can be summarized by

η.w∗(x) = miny∈X

[η.v∗(y) + δη.d(y, x)]

= min{

η.v∗(x),miny 6=x

[η.v∗(y) + δη.d(y, x)]}

= min{

η.v∗(x), miny∈N−1(x)

[η.w∗(y) + η.d(y, x)]}

The first equality is by (9), the second equality follows from (15), and the final equality follows from (19).

V. A LGORITHM

We propose Algorithm 1 to find the set of optimal meeting locations, i.e.Υ.p∗ from (5). AssumeFindSolution (ρ)

has just been called. The algorithm first computes the value functionsΥ.v andΥ.w for all nodes exceptρ by calling

the recursive functionFindValue for each child ofρ. It then computesρ.v : X → R, which specifies the minimal

meeting tree cost given the root meeting is located at eachx ∈ X . Next it assignsρ.p the minimal-cost meeting

location. Finally, the algorithm computes the meeting locationsΥ.p for all other nodes by calling the recursive

function FindMeetingLocation for each child ofρ.

Input : ρ

for k ← 1 : ρ.K do FindValue (ρ.κ[k])1

ρ.v ←∑ρ.K

k=1 ρ.κ[k].w + ρ.c2

ρ.p← arg minx∈X ρ.v(x)3

ρ.z ← [ρ.p]4

for k ← 1 : ρ.K do FindMeetingLocation (ρ.κ[k], ρ.p)5

Algorithm 1 : FindSolution

Input : η

for k ← 1 : η.K do FindValue (η.κ[k])1

η.v ←∑η.K

k=1 η.κ[k].w + η.c2

FindCommuteValue (η)3

Algorithm 2 : FindValue

Input : η

η.w ← η.v1

Q ← X2

while Q 6= ∅ do3

x∗ ← arg minx∈Q η.w(x)4

Q ← Q \ {x∗}5

foreach x ∈ N (x∗) ∩Q do6

η.w(x)← miny∈N−1(x) [η.w(y) + η.d(y, x)]7

end8

end9

Algorithm 3 : FindCommuteValue (Dijkstra’s)

Input : η,x

if η.w(x) = η.v(x) then1

η.p← x2

η.z ← [η.p]3

for k ← 1 : η.K do FindMeetingLocation (η.κ[k], η.p)4

else5

y∗ ← arg miny∈N−1(x) [η.w(y) + η.d(y, x)]6

FindMeetingLocation (η, y∗)7

η.z ← [η.z, x]8

end9

Algorithm 4 : FindMeetingLocation

A. Correctness

Here we prove that Algorithm 1 results in an optimal set of meeting locationsΥ.p∗. We first prove lemmas

regarding the results ofFindCommuteValue , FindValue , andFindMeetingLocation .

Lemma 1:Let η be a meeting node inΥ. Assuming thatη.v = η.v∗, a call to FindCommuteValue (η)

terminates withη.w = η.w∗.

Proof: We introduce an additional statex, such thatx /∈ X and define the extended state spaceX = X ∪{x}.

We then define the extended commuting cost funtionη.d : X ×X → R+, similar tod but also including commuting

costs from statex to all states inX given by the valuesη.v; that is,

η.d(x, y) =

η.v(y), if x = x,

η.d(x, y), otherwise.

Then, a call toFindCommuteValue (η) is the equivalent of running Dijkstra’s algorithm [7] with initial statex,

which terminates after finding the minimum cost over all trajectories fromx to any nodex ∈ X . In other words,

a call toFindCommuteValue (η) terminates and we have for anyx ∈ X

η.w(x) = δη.d(x, x) = miny∈X

[η.v(y) + δη.d(y, x)] .

Therefore,

η.w(x) = miny∈X

[η.v∗(y) + δη.d(y, x)] = η.w∗(x),

due to the assumptionη.v = η.v∗ and (9).

Lemma 2:Let η be any node exceptρ in Υ. A call to FindValue (η) terminates withη.v = η.v∗ and η.w =

η.w∗for all nodesη (including η = η) in the subtree rooted atη.

Proof: The following is an inductive proof showing that after executingFindValue (η), whenη is a leaf

η.v = η.v∗ andη.w = η.w∗; and for anyη with children, if η.κ[k].w = η.κ[k].w∗ for all children, thenη.v = η.v∗

andη.w = η.w∗.

Let η be a leaf inΥ. Call FindValue (η). Line 1 in Algorithm 2 terminates with no effect sinceη.K = 0. Line

2 setsη.v = η.c, so by (8), consideringη has no children,η.v = η.v∗. After executing Line 3, by Lemma 1, we

haveη.w = η.w∗. Thus, a call toFindValue (η) terminates withη.v = η.v∗ and η.w = η.w∗.

Let η be any node exceptρ in Υ with finite η.K ≥ 1. Assume a call toFindValue (η) terminates with

η.κ[k].w = η.κ[k].w∗ (20)

for 1 ≤ k ≤ η.K. Then thefor loop at Line 1 terminates sinceη.K is finite. Line 2 sets

η.v(x)←η.K∑k=1

η.κ[k].w(x) + η.c(x),

so by (20) and (8), we haveη.v = η.v∗. By Lemma 1, execution of Line 3 terminates withη.w = η.w∗. Thus, a

call to FindValue (η) terminates withη.v = η.v∗ and η.w = η.w∗.

Now let η be any node exceptρ in Υ. By induction on the finite node hierarchy of the subtree rooted atη, a

call to FindValue (η) terminates withη.v = η.v∗ and η.w = η.w∗.

We make some further definitions for use in the remaining lemmas and Theorem 3 in this section. We say that the

subtree rooted atη is proper and writeφ(η) = True if for all nodesη in the subtree rooted atη, η.κ[k].zL = η.p

for 1 ≤ k ≤ η.K and η.p = η.z1. In other words,φ(η) = True if all the meeting-to-meeting trajectories in the

subtree rooted atη are connected to one another at the appropriate meeting locations. The functionφ can be defined

recursively as

φ(η) :=η.K∧k=1

[φ(η.κ[k]) ∧ (η.κ[k].zL = η.p)] ∧ (η.p = η.z1). (21)

We also define a recursive functionh that computes the total cost of all trajectories and meetings in the subtree

rooted at nodeη.

h(η) =η.K∑k=1

h(η.κ[k]) + η.c(η.p) + λη.d(η.z), (22)

Note that althoughh only takes a nodeη as a parameter, it is really a function of all meeting-to-meeting trajectories

and meeting locations in the subtree rooted atη.

The following condition is used in Lemmas 3, 4, and 5, and Theorem 3. It is a precondition for correct execution

of FindMeetingLocation and is shown in Theorem 3 to be a postcondition of the execution of Line 1 in

Algorithm 1.

Condition 1: For all nodesη exceptρ in Υ, η.v = η.v∗ andη.w = η.w∗.

The following condition is used in Lemmas 3 and 5. It is a precondition for the correct execution of thethen block of

Algorithm 4. It says roughly that given a nodeη, for all childrenη.κ[k] and all statesx ∈ X , FindMeetingLocation (η.κ[k], x)

terminates with a correct subtree of total costη.κ[k].w(x).

Condition 2: Given a nodeη in Υ, for 1 ≤ k ≤ η.K and allx ∈ X a call toFindMeetingLocation (η.κ[k], x)

terminates withφ(η.κ[k]) = True , η.κ[k].zL = x, andh(η.κ[k]) = η.κ[k].w(x).

The following condition is used in Lemmas 4 and 5. It is a precondition for the correct execution of theelseblock of

Algorithm 4. It says roughly that given a statex ∈ X , for all statesx with lesserη.w, FindMeetingLocation (η, x)

terminates with a correct subtree of total costη.w(x).

Condition 3: Given a statex ∈ X , for all x ∈ X such thatη.w(x) < η.w(x) a call toFindMeetingLocation (η, x)

terminates withφ(η) = True , η.zL = x, andh(η) = η.w(x).

The next two lemmas are used as inductive steps in the inductive argument of Lemma 5. The following lemma

considers the recursion occurring in thethen block of Algorithm 4.

Lemma 3:Let η be any node exceptρ in Υ andx ∈ X such that

η.w(x) = η.v(x). (23)

Let Conditions 1 and 2 hold. Then, a call toFindMeetingLocation (η, x) terminates withφ(η) = True ,

η.zL = x, andh(η) = η.w(x).

Proof: Let η be any node exceptρ in Υ andx ∈ X such that (23) holds. Let Conditions 1 and 2 hold. Call

FindMeetingLocation (η, x).

Because (23) holds, thethen block in Algorithm 4 is entered. After executing Line 2,

η.p = x. (24)

After executing Line 3,

η.z1 = η.zL = [η.p] = [x]. (25)

Due to Condition 2, for1 ≤ k ≤ η.K the call toFindMeetingLocation (η.κ[k], η.p) terminates. Since thefor

loop iterates a finiteη.K times, the call toFindMeetingLocation (η, x) terminates. Also due to the Condition

2, for 1 ≤ k ≤ η.K, we haveφ(η.κ[k]) = True andη.κ[k].zL = η.p. As a result and sinceη.z1 = η.p, we have

φ(η) = True by (21). Furthermore, we have

h(η) =η.K∑k=1

h(η.κ[k]) + η.c(η.p)

=η.K∑k=1

η.κ[k].w(η.p) + η.c(η.p)

= η.v(η.p) = η.w(x)

The first equality is by (22) becauseλη.d(η.z) = λη.d([x]) = 0. The second equality is becauseh(η.κ[k]) =

η.κ[k].w(η.p) for 1 ≤ k ≤ η.K, due to Condition 2. The third equality is by Condition 1 and (8). The final equality

is by (24) and (23). Therefore, a call toFindMeetingLocation (η, x) terminates withφ(η) = True , η.zL = x,

andh(η) = η.w(x).

The following lemma considers the recursion occurring in theelseblock of Algorithm 4.

Lemma 4:Let η be any node exceptρ in Υ andx ∈ X such that (23) fails to hold. Let Conditions 1 and 3 hold.

Then, a call toFindMeetingLocation (η, x) terminates withφ(η) = True , η.zL = x, andh(η) = η.w(x).

Proof: Let η be any node exceptρ in Υ andx ∈ X such that (23) fails to hold. Let Conditions 1 and 3 hold.

Call FindMeetingLocation (η, x).

Because (23) fails to hold, theelseblock is entered. After executing Line 6, we know that

η.w(x) = η.w(y∗) + η.d(y∗, x) (26)

by Condition 1, (13) and the fact that (23) does not hold. Sinceη.d(y∗, x) is positive we haveη.w(y∗) < η.w(x).

Thus, by Condition 3, after executingFindMeetingLocation (η, y∗), FindMeetingLocation terminates

with φ(η) = True , η.zL = y∗, and h(η) = η.w(y∗). Executing Line 8 extendsη.z by appendingx. It is clear

from (21) thatφ(η) = True continues to hold. Also, after Line 8,η.zL = x. Finally, in executing Line 8,h(η)

increases byη.d(y∗, x). Thus, due to Condition 3 and (26) we have

h(η) = η.w(y∗) + η.d(y∗, x) = η.w(x).

Therefore, a call toFindMeetingLocation (η, x) terminates withφ(η) = True , η.zL = x, andh(η) = η.w(x).

Lemma 5:Let Condition 1 hold. Then, for all nodesη exceptρ in Υ and for allx ∈ X , a call toFindMeetingLocation (η, x)

terminates withφ(η) = True , η.zL = x, andh(η) = η.w(x).

Proof: Let Condition 1 hold. The following is an inductive proof.

First, we consider the base case whereη is a leaf andx is as defined in (11). Becauseη has no children,

Condition 2 holds vacuously. Furthermore, by Corollary 1, we haveη.w(x) = η.v(x). Thus by Lemma 3, a call to

FindMeetingLocation (η, x) terminates withφ(η) = True , η.zL = x, andh(η) = η.w(x).

Second, we use induction onx ∈ X in order of increasingη.w(x). We assume Condition 3 for the inductive

step. When (23) holds, by Lemma 3, a call toFindMeetingLocation (η, x) terminates withφ(η) = True ,

η.zL = x, and h(η) = η.w(x). Alternatively, when (23) does not hold, by Condition 3 and Lemma 4, a call to

FindMeetingLocation (η, x) terminates withφ(η) = True , η.zL = x, andh(η) = η.w(x). The recursion is

finite since|X | is finite andη.w(x) decreases for each recursive call toFindMeetingLocation (η, x). Thus,

by induction, we have that for allx ∈ X a call toFindMeetingLocation (η, x) terminates withφ(η) = True ,

η.zL = x, andh(η) = η.w(x).

Next, we consider the case whereη has children andx is as defined in (11). We assume Condition 2 for the induc-

tive step. By Corollary 1, we haveη.w(x) = η.v(x). Thus by Lemma 3, a call toFindMeetingLocation (η, x)

terminates withφ(η) = True , η.zL = x, andh(η) = η.w(x). Last, we consider the case whereη has children

and Condition 3 holds. Using an inductive argument identical to that above for the case whereη is a leaf, we

have that for allx ∈ X a call to FindMeetingLocation (η, x) terminates withφ(η) = True , η.zL = x, and

h(η) = η.w(x).

Therefore, by induction on the finite node hierarchy of the subtree rooted atη, for all nodesη exceptρ in

Υ and for all x ∈ X , a call to FindMeetingLocation (η, x) terminates withφ(η) = True , η.zL = x, and

h(η) = η.w(x).

Theorem 3:A call to FindSolution (ρ), terminates withφ(η) = True , Υ.p = Υ.p∗, andΥ.z = Υ.z∗.

Proof: Begin executingFindSolution (ρ). After executing Line 1 in Algorithm 1, Condition 1 holds by

Lemma 2. In particular, by Condition 1 we haveρ.κ[k].v = ρ.κ[k].v∗ andρ.κ[k].w = ρ.κ[k].w∗ for 1 ≤ k ≤ ρ.K.

After executing Line 2, by (8) we have

ρ.v = ρ.v∗. (27)

Following the execution of Line 3, we have

ρ.v(ρ.p) = minx∈X

ρ.v(x)

= minx∈X

ρ.v∗(x)

= minx∈X

[min

Υ.p, ρ.p=xf(ρ)

]= min

Υ.pf(ρ).

(28)

The first equality is from Line 3, sinceρ.p minimizesρ.v. The second equality is by (27) and the third equality

is by (6). The final equality holds because minimizingf(ρ) for each possibleρ.p = x ∈ X , and then minimizing

over all ρ.p = x ∈ X is equivalent to minimizingf(ρ) over all possibleΥ.p.

After executing Line 4 we haveρ.z1 = ρ.zL = [ρ.p]. Since Condition 1 holds, by Lemma 5, execution of Line

5 terminates and for1 ≤ k ≤ η.K, we haveφ(ρ.κ[k]) = True , ρ.κ[k].zL = ρ.p, andh(ρ.κ[k]) = ρ.κ[k].w(ρ.p).

As a result and sinceρ.z1 = ρ.p, we haveφ(ρ) = True by (21). Furthermore, we have

h(ρ) =ρ.K∑k=1

h(ρ.κ[k]) + ρ.c(ρ.p)

=ρ.K∑k=1

ρ.κ[k].w(ρ.p) + ρ.c(ρ.p)

= ρ.v(ρ.p) = minΥ.p

f(ρ).

(29)

The first equality is by (22) becauseλρ.d(ρ.z) = λρ.d([ρ.p]) = 0. The second equality is becauseh(ρ.κ[k]) =

ρ.κ[k].w(ρ.p), for 1 ≤ k ≤ ρ.K. The third equality is by Condition 1, (27), and (8). The final equality is by (28).

Sinceφ(ρ) = True the meeting treeΥ is proper. Furthermore, the total costh(ρ) of the meeting treeΥ is

optimal by (29). Therefore,Υ.p = Υ.p∗ andΥ.z = Υ.z∗.

We have not only proved thatΥ.p = Υ.p∗, but in the process we have demonstrated thatΥ.v = Υ.v∗, Υ.w =

Υ.w∗, and Υ.z = Υ.z∗. Accordingly, in the sequel we considerΥ.(v, w, p, z) that result from the execution of

Algorithm 1 to be optimal, i.e.,Υ.(v, w, p, z) = Υ.(v∗, w∗, p∗, z∗).

B. Complexity

Algorithm 1 consists of a single leaves-to-root pass where at each meeting nodeη.w is computed usingFindCommuteValue ,

followed by a single root-to-leaves pass where at each meeting nodeη.p is computed usingFindMeetingLocation .

We assume that|N (x)| < a and |N−1(x)| < b for some constantsa and b independent of|X | (e.g., if X is a

Cartesian grid, thena = b = 2d, whered is the number of dimensions, no matter what the resolution of the grid).

In this case,FindCommuteValue can be implemented efficiently using Dijkstra’s algorithm (i.e. Algorithm

3), which has complexityO(|X ||N (x)| log |X |) = O(|X | log |X |) if the arg min in Line 4 is implemented by

maintaining a min-heap sorted onη.w(x). On the other hand, the complexity ofFindMeetingLocation is

O(|X ||N−1(x)|) = O(|X |), since the meeting-to-meeting trajectoryη.z can pass through at most every nodex ∈ X

and at each node consider at most|N−1(x)| neighbors to extend the trajectory. Consequently, the complexity of

FindCommuteValue dominates that ofFindMeetingLocation . Thus, the overall complexity of Algorithm

1 isO(M |X | log |X |), whereM is the number of nodes inΥ.

VI. EXAMPLES

We solve two example problems: The first is the discrete robot clinic consultation defined earlier, and the second

is a continuous robot arm cargo transport which we describe below.

A. Patient Consultation

We solve the problem defined in Section III-B using Algorithm 1. Figure 2(a) shows the resulting minimal-cost

solution tree including optimal meeting locationsΥ.p∗ and optimal meeting-to-meeting trajectoriesΥ.z∗. After

meeting the pharmacist, the nurse backtracks over its previous trajectory to its starting location and then on to the

(a) (b)

Fig. 2. (a) A minimal-cost solution tree for the patient consultation problem. Optimal meeting locationsΥ.p∗ are shown using the symbols

defined in Figure 1(c). Thehold consultationmeeting values♥.v∗ are indicated in parentheses at each of the 5 possible locations. An optimal

meeting location♥.p∗ is indicated with∗. Corresponding optimal meeting-to-meeting trajectoriesΥ.z∗ are indicated with thick solid lines. The

pharmacy counter and back wall is indicated with thin solid lines. (b) Value functions and optimal trajectories of thebegin pharmacistL

and

begin nurseN

meeting nodes. The optimal meeting valuesη.v∗ are indicated in parentheses and the optimal meeting-plus-commute values

η.w∗ have no parentheses. The valuesL

.v∗ andL

.w∗ are indicated inside the pharmacy counter while the valuesN

.v∗ andN

.w∗ are

indicated outside. Optimal meeting locationsL

.p∗ andN

.p∗ are indicated with∗. The optimalget medslocation�.p∗ is indicated with�.

consultation. The trajectories of the wheelchair (with patient) and the nurse overlap for one segment just before the

optimal consultation location♥.p∗ is reached. Figure 2(a) shows that the root meeting location♥.p is a location

with minimal ♥.v(♥.p) = 37, in accordance with the assignment of♥.p in Line 3 of Algorithm 1. Note that the

two consultation locations in the left-most consultation room both have a meeting value of 37 and it does not matter

which is chosen.

We use Figure 2(b) to illustrate how Algorithm 1 calls Algorithms 2 and 3 to calculateη.v∗ andη.w∗, respectively.

The figure indicates the value functions of two meeting nodes⊕

(for nodes inside the pharmacy counter) and⊗(for nodes outside the pharmacy counter), each a leaf node which Algorithm 2 takes as itsη parameter at

the bottom of its recursive hierarchy. Forη a leaf node, Line 2 assigns the meeting valueη.v = η.c (see the

numbers in parentheses in Figure 2(b)). Then, in Line 3 the call toFindCommuteValue (η) computes for each

reachable state the meeting-plus-commute valueη.w in order of increasing value (see the unparenthesized numbers

in Figure 2(b)). In particular, consider the values for⊕

within the counter in the figure computed by the call

FindCommuteValue (⊕

). For x one of the upper two states within the counter,⊕

.w(x) =⊕

.v(x) = 1, since

it is cheaper to begin the pharamacist atx than it is to begin the pharmacist at some other state and commute tox.

Alternatively, forx one of the lower two states within the counter,⊕

.w(x) = 2 <⊕

.v(x), since it is cheaper to

begin the pharamacist at the state directly abovex and commute down tox. Finally, for x on the counter, the value⊕.w(x) = 3 results from a commute from a neighboring state, since the pharmacist cannot start on the counter.

Analogously, the meeting-plus-commute values⊗

.w(x) for statesx outside and on the counter are calculated by

the callFindCommuteValue (⊗

).

Fig. 3. Meeting tree for the robot arm problem. Begin roboti is indicated by bi. Pickup cargo for robot 1 is indicated by m1. Pass cargo from

robot i to robotj is indicated by mij. Dropoff cargo for robot 3 is indicated by m3.

We also use Figure 2(b) to illustrate how Algorithm 1 later calls Algorithm 4 to calculateη.z∗ andη.p∗. Nodes⊕and

⊗are leaf nodes, each of which is passed as theη parameter to Algorithm 4 at the bottom of its recursive

hierarchy. For any meeting nodeη, the while loop in Algorithm 4 computesη.z by following a steepest descent

path onη.w until a nodex is reached such thatη.w(x) = η.v(x), with x being assigned to the meeting location

η.p. In particular, consider the call toFindMeetingLocation (⊕

,�.p∗) with �.p∗ indicated in the Figure. At

first, x = �.p∗, which is not an allowedbegin pharmacistlocation (i.e.,⊕

.v(x) =∞ 6=⊕

.w(x)), so thewhile

loop condition is met. The only neighboring state is the lower-left state inside the counter so it is prepended to⊕.z in Line 8 of Algorithm 4. For the lower-left state, the meeting-plus-commute value is less than the meeting

value, so the loop condition is met. The upper-left state inside the counter is the minimizing neighbor in Line 6,

so it is prepended to⊕

.z. For this state, the meeting-plus-commute value equals the meeting value so the loop is

terminated. Thebegin pharmacistlocation⊕

.p is assigned the upper-left state. Analogously,⊗

.z and⊗

.p are

calculated by the callFindMeetingLocation (⊗

,�.p∗).

B. Robot Arms

We solve a robot arm meeting problem using an extension of Algorithm 1 modified to handle the continuous

state spaces of the robot arms. The problem involves three robot arms cooperating to transport cargo from a cargo

pickup location in the top-right of the workspace to a cargo dropoff location in the top-left of the workspace. Robot

1 is attached to the lower-right corner of the workspace and has two rotational joints. The primary joint has an

angular range ofπ/2 radians, such that the primary segment cannot swing out of the workspace, and the secondary

joint has an angular range of2π radians, but the secondary segment cannot swing through the primary segment.

Robot 3 is attached to the lower-left corner of the workspace and otherwise has the same properties as robot 1.

Robot 2 moves on a sliding joint along the top of the workspace. It also has a rotational joint that moves through

an angle ofπ radians, such that the arm cannot swing out of the workspace. Each robot begins such that its end

effector is in a circular starting area. Robot 1 must pick up the cargo and pass it to robot 2, then robot 2 must

pass the cargo to robot 3, who drops off the cargo. For two robots to “meet,” their end effectors must approach

within a small neighborhood of one another. The goal is to find meeting locations and connecting state trajectories

that minimize the cost of transporting the cargo across the workspace. The corresponding meeting tree is shown in

Figure 3, and the resulting robot arm motions are depicted in Figure 4.

(a) (b)

(c) (d)

Fig. 4. Robot arm motions for a solution meeting tree. The sequence of figures is a time series with (a) showing the beginning of the robot

arm motions and (d) showing the completion of the motions. The starting area for the end effector of each robot arm is indicated by a circle.

The square box on the top-right/top-left is the pickup/dropoff location for the cargo. A small circle at the end effector of a moving robot arm

indicates that the robot is currently carrying the cargo. (a) Robot 1 moves from its starting location to cargo pickup area. (b) Robot 1 moves

from the cargo pickup box to meet robot 2, while robot 2 moves from its starting location to meet robot 1. (c) Robot 2 moves from its meeting

with robot 1 to meet robot 3, while robot 3 moves from its starting location to meet robot 2. (d) Robot 3 moves from its meeting with robot 2

to the cargo dropoff area.

Each robot arm has 2 degrees of freedom, one for each of its joints. Consequently, each has a continuous

2-dimensional state space. Let

ζ(t) = (ζ1(t), ζ2(t)), for t0 < t < tf ,

be a continuous trajectory through the 2D state space of a robot, wheret0 is the starting time andtf is the finishing

time. The cost of a robot state trajectory is given by the continuous analogue of (1)

λ(ζ(·)) =∫ tf

t0

‖ζ(t)‖1dt =∫ tf

t0

[|ζ1(t)|+ |ζ2(t)|

]dt. (30)

However, the cost of a robot arm passing through an obstacle is considered infinite. The total cost of transporting

the cargo is the sum of the costs of the robot state trajectories. There are no meeting costs incurred when two

robots meet (meetings mij in Figure 3). Also, there is no cost for a robot to begin (meetings bi) within its circular

starting area but there is an infinite cost for a robot to begin outside its starting area. Finally, there is no cost for

cargo pickup (meeting m1) or dropoff (meeting m3) within the respective pickup or dropoff area but there is an

infinite cost for cargo pickup or dropoff outside the appropriate area.

We modify Algorithm 1 to solve the continuous robot arm problem. Since the robot arms are operating within a

continuous state space we use the Fast Marching Method in place of Algorithm 3 (Dijkstra’s Algorithm), which is

designed to compute value functions on discrete graphs. More specifically, for each meeting node we approximately

solve the Eikonal equation

‖∇w(x)‖∞ = 1,

on a discretized grid of the robot state space. The reasons for solving such an Eikonal equation and methods for

doing so are discussed in more depth in [2].

We use (30) to measure the cost of a robot trajectory in the robot state space, but the occurance of a meeting

depends on the end effector position in the workspace. For this reason, we modify Algorithm 2 to employ two grids

for each meeting node: a workspace grid for computing meeting valuesη.v and a robot state grid for computing

meeting-plus-commute valuesη.w. For the solution illustrated below we use a workspace grid of401× 201 nodes,

a state grid of401 × 101 nodes for robot 1 and 3, and a state grid of201 × 201 nodes for robot 2. The meeting

values are computed on the workspace grid using the same formula as in Line 2, but beforehand the child meeting-

plus-commute values must be mapped from the child robot state grids onto the workspace grid. Such mapping is

done using kinematics to determine the end effector position corresponding to the robot state and then taking the

nearest workspace grid node to the end effector position. Next, the meeting values from the workspace grid must be

mapped to the state grid beforeFindCommuteValue (η) is called in Line 3 to compute the meeting-plus-commute

values. We avoid using inverse kinematics without changing the asymptotic complexity of the algorithm by iterating

though all state grid nodes and using kinematics to determine the nearest workspace node in the same manner as

above.

Some modifications are also required for Algorithm 4. First, thewhile loop should be replaced by an ODE

solver that computes a steepest descent trajectory on an interpolated meeting-plus-commute value function in the

robot state space. Second, the loop condition must compare the interpolated meeting-plus-commute value to the

interpolated meeting value at the kinematically-determined end effector location in the workspace.

VII. C ONCLUSION

We have specified a class of multi-location robot rendezvous problems for which dynamic programming can

be used to find optimal solutions. An appropriate dynamic programming principle has been presented and used

to construct an efficient algorithm. The applicability of the algorithm has been demonstrated on a discrete robotic

clinic example and the algorithm has been modified slightly and applied to a continuous robot arm example.

There are several ways this work could be extented. Firstly, in this paper we have defined the cost of a meeting

tree to be a sum of costs of its constituent trajectories and meetings. However, the summation in (4) could be

replaced with any monotone function without compromising the dynamic programming principle stated in Theorem

1. For example, one could use a maximum in place of the summation to solve a minimal-time multi-location

robot rendezvous problem. Secondly, the modified algorithm for solving the continuous rendezvous problem can be

examined with more rigour. Lastly, some computed meeting-plus-commute (and meeting) values may be so large

that they can be ruled-out as possible contributors to the problem solution. It may be possible to increase algorithm

efficiency by computing meeting-plus-commute (and meeting) values at states only if they may have influence.

ACKNOWLEDGMENT

This work was supported by a Discovery grant from the National Sciences and Engineering Research Council

of Canada.

REFERENCES

[1] K. Alton and I. Mitchell, “Efficient optimal multi-location robot rendezvous,” submitted to ICRA 2008.

[2] ——, “Optimal path planning under different norms in continuous state spaces,” inProceedings of the International Conference on Robotics

and Automation, 2006, pp. 866–872.

[3] Y. Litus, R. T. Vaughan, and P. Zebrowski, “The frugal feeding problem: Energy-efficient, multi-robot, multi-place rendezvous,” in

Proceedings of IEEE ICRA, 2007, pp. 27–32.

[4] J.-C. Latombe,Robot Motion Planning. Boston: Kluwer Academic Publishers, 1981.

[5] H. Choset, K. M. Lynch, S. Hutchinson, G. Kantor, W. Burgard, L. E. Kavraki, and S. Thrun,Principles of Robot Motion: Theory,

Algorithms, and Implementations. Cambridge, Massachusetts and London, England: The MIT Press, 2005.

[6] S. M. Lavalle,Planning Algorithms. Cambridge University Press, 2006.

[7] E. W. Dijkstra, “A note on two problems in connection with graphs,”Numer. Math. 1, pp. 269–271, 1959.

[8] D. P. Bertsekas,Dynamic Programming and Optimal Control: Second Edition. Belmont, Massachusetts: Athena Scientific, 2000.

[9] Y. Kupitz and H. Martini, “Geometric aspects of the generalized Fermat-Toricelli problem,”Bolyai Society Mathematical Studies, vol. 6,

pp. 55–129, 1997.

[10] G. Sahina and H. Sralb, “A review of hierarchical facility location models,”Computers and Operations Research, vol. 34, no. 8, pp.

2310–2331, 2007.