Efficient Dynamic Programming for Optimal Multi-Location Robot Rendezvous with Proofs

Transcription

1 Efficient Dynamic Programming for Otimal Multi-Location Robot Rendezvous with Proofs Ken Alton and Ian M. Mitchell Deartment of Comuter Science University of British Columbia Vancouver, BC, V6T 1Z4, Canada Technical Reort August 6, 2008 Abstract We resent an efficient dynamic rogramming algorithm to solve the roblem of otimal multi-location robot rendezvous. The rendezvous roblem considered can be structured as a tree, with each node reresenting a meeting of robots, and the algorithm comutes otimal meeting locations and connecting robot trajectories. The tree structure is exloited by using dynamic rogramming to comute solutions in two asses through the tree: an uwards ass comuting the cost of all otential solutions, and a downwards ass comuting otimal trajectories and meeting locations. The correctness and efficiency of the algorithm are analyzed theoretically, while a continuous robot arm roblem demonstrates the algorithm s racticality. This aer is an extended version of a aer to be resented at the 2008 IEEE Conference on Decision and Control [1]. It contains roofs of the theorems in Section IV-A that were omitted from the conference aer for sace reasons. An older version of the aer [2] focusses more on the discrete state sace algorithm and is more algorithmic and less mathematical in style. I. INTRODUCTION Path lanning is a central area of study in robotics, but most current algorithms find an efficient ath for only a single robot at a time. Coordinated ath lanning for multile robots has received increased attention recently. We focus here on a articular tye of coordinated robot ath lanning roblem and, in so doing, we are able to find a very efficient dynamic rogramming DP solution. More secifically, we examine otimal coordinated multi-robot multi-location rendezvous, an extension of the frugal feeding roblem considered in [3]. In a multile-robot scenario it is often useful for robots to meet to exchange fuel, cargo, and/or information. Given a hierarchical structure that describes which robots are to meet and which robots are to continue on to future meetings, our DP algorithm can comute the otimal meeting locations and the otimal aths between meetings with comlexity linear in the number of meetings. To simlify the scenarios, we assume central and comlete knowledge of the mas and robots states, and we ignore collisions between the robots. Desite this simlification, we believe that core asects of our DP algorithm for solving the robot rendezvous roblem can be utilized in real world robot alications. To demonstate the ractical otential of our algorithm, we use it to solve a continuous roblem involving robotic arms cooerating to deliver some cargo from a source to a destination through a worksace with obstacles. This robot arm roblem is a form of hybrid system because of the continuous arm dynamics and discrete meeting events. The main contribution of this aer is the resentation of a tree or outer DP rincile to find otimal solutions to a broad category of hierarchical multi-location robot rendezvous roblems. Furthermore, we show that for certain structures of the robot state sace, a state or inner DP rincile holds allowing commute costs for individual robots to be comuted efficiently. An imortant roerty of both the tree and state DP rinciles is that no calculation is done in a state sace with dimension greater than that of the individual robots. We use the alication of multi-location robot rendezvous as a concrete stand-in for any kind of otimal meeting roblem with a fixed tree structure. Another such alication might be the otimization of an industrial suly chain with a fixed tree structure. Moreover, the words meeting or suly chain seem to imly a temoral flow from leaves to root in the tree. However, our analysis also alies to disersion or distribution roblems with the oosite temoral flow or roblems with no clear temoral character as long as they have the aroriate tree structure. We define the tye of roblems considered and our notation in Section III. Mathematical analysis in Section IV shows that the hierarchical roblem structure imlies that a tree DP rincile can be used to efficiently comute costs of otential solutions. Section V resents two secial cases of the state sace and resulting state DP rinciles that allow well-known DP algorithms to be used. Finally, in Section VI we use the algorithm to solve a continuous robot-arm rendezvous roblem. II. RELATED WORK Research in robot ath lanning is considered a central endeavor in the develoment of mobile robotics. Aroaches

2 to robot ath lanning are diverse and include otential function methods, samling-based methods, trajectory lanning methods and combinations of these [4], [5]. Most related to this aer are the DP algorithms for solving shortest ath roblems on grids [6], [7]; however, most ath lanning research, including that described in the above references, focuses on single robots. In this aer, we investigate a DP method for the multilocation rendezvous of multile robots. This roblem is distinct from the generalized Fermat-Torricelli roblem that finds a unique oint minimizing the sum of distances from a given set of oints [8]. Given a set of vertices, the Steiner tree roblem is to find the lowest cost tree connecting these vertices [9]. The hierarchical facility location roblem [10] involves finding the location of facilities of several levels to serve customers most efficiently. Both the Steiner and facilities roblems are more difficult than the rendezvous roblem because the combinatorial asect of determining the tree structure must be solved in addition to finding the otimal locations of intermediate nodes i.e. meeting nodes in the tree. For the rendezvous roblem, we assume that this tree structure already exists and develo an efficient algorithm to otimize the location of meetings. This work was motivated by [3], in which the authors describe a robot refueling roblem where the goal is to find the otimal 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 aer. We generalize the roblem to include any hierarchy of robot meetings and any monotone meeting cost aggregation function, and show that algorithmic efficiency can be gained by assuming that the otential meeting locations are nodes in a satial grah on which commuting costs are only defined between neighboring nodes. Finally, we demonstrate that comutation on a grid can be used to aroximate a continuous roblem with a otentially comlex cost function. This continuous robot meeting roblem is an examle of a hybrid system. The fast marching method, a DP algorithm, was used to find an otimal ath through a hybrid system in [11]. Our dynamic rogramming algorithm for the continuous case solves a distinct hybrid roblem involving a hierarchy of meetings. III. PROBLEM DESCRIPTION A robot meeting involves one or more robots colocating at a state within a state sace. Excet 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 rogress through the tree culminating in one final meeting at the root. For this roblem we fix the meeting structure as well as which robots must attend any articular meeting, but we allow the meeting locations to vary. In other words, we are not concerned with the combinatorial asects of determining the meeting tree, which may be required 1 In fact, two or more robots may continue on from a meeting so long as they travel together in a grou. Since the grou moves as a single entity to the next meeting, the essential roerties of the roblem remain the same. for a general hierarchical facility location roblem [10] or the Steiner tree roblem. We wish to minimize, over all ossible meeting locations, the total cost of a given meeting tree. The total cost of a meeting tree considers the cost of robot commutes between meetings as well as the cost of the meetings themselves. The avoidance of collisions between the robots during the commutes is not considered. symbol tye descrition Υ set of nodes meeting tree node set Kη Υ 0 Z + number of children κη, k Υ Z + Υ children X set of states state sace ξη, x, ω Υ X R Kη R meeting cost aggregation δη, x, y Υ X 2 0 R + commuting cost η Υ X meeting location fη Υ R subtree cost gη, x Υ X R subtree-lus-commute cost η Υ X otimal meeting location vη, x Υ X R otimal subtree cost wη, x Υ X R otimal subtree-lus-commute cost TABLE I SYMBOLS: THE UPPER BLOCK INCLUDES SYMBOLS USED TO DEFINE A PROBLEM INSTANCE. THE MIDDLE BLOCK INCLUDES SYMBOLS USED TO DEFINE A SOLUTION. THE LOWER BLOCK INCLUDES SYMBOLS USED TO COMPUTE AN OPTIMAL SOLUTION SEE SECTION IV. The first art of the roblem instance definition is a meeting tree. Let there be a set of meeting tree nodes Υ. The term node or meeting may be used to refer to a meeting tree node η Υ. We let ρ Υ be the root node in the meeting tree. The function K : Υ 0 Z + secifies the number of children of each node η Υ. The function κ : Υ Z + Υ secifies the children of each node η Υ. For convenience, we also define a function ι : Υ Υ that secifies the arent of each node η Υ. The exression ιρ is considered undefined. The second art of a roblem instance definition is the state sace X. The term state or location may be used to refer to an element x X. The last art of the roblem instance definition secifies the costs of comonents of a otential meeting tree solution. Define the commuting cost function δ : Υ X 2 0 R +. The exression δη, x, y is the cost of commuting from x X to y X after the meeting η Υ. Let δη, x, y = 0 if x = y and δη, x, y > 0 if x y. If δη, x, y = then it is not ossible to commute from x to y. Define the meeting cost aggregation function ξ : Υ X R K R. The exression ξη, x, ω aggregates the costs ω R Kη of arriving at meeting η Υ at location x X. We assume this function to be nondecreasing in each element ω k of ω. If ξη, x, ω = for all ω then the meeting η cannot take lace at location x. Useful examles of ξ include K ξη, x, ω = ω k + γη, x 1 and ξη, x, ω = max ω k + γη, x, 1 k K

3 where γη, x is the cost of meeting η being located at x X. The definition is alicable for the roblem of minimizing the sum of all commuting costs and meeting costs for a meeting tree, which is essentially what is done in [3]. The max definition is alicable for minimizing the critical ath in a meeting tree; for examle, minimizing the time elased before the root meeting is comleted. A otential solution to a roblem instance is given by a meeting location function : Υ X. The exression η is the location of meeting η Υ. The total cost of a otential solution is defined recursively in terms of the commuting cost function and meeting cost aggregation function. The recursive definition includes a subtree cost function f : Υ R and a subtree-lus-commute cost function g : Υ X R. The exression fη is the total cost of the meeting subtree rooted at node η, while the exression gη, x is the cost of a meeting subtree rooted at node η in addition to the commuting cost from the meeting location η to the state x X. The combined definition of functions f and g is where fη = ξ η, η, [gκη, k, η] Kη gη, x = fη + δη, η, x, ω 1 [ω k ] K ω 2 =. = ω RK. ω K 2a 2b Note that f and g take a single node η as a arameter, but are really functions of all locations of the meetings in the subtree rooted at η. The subtree cost fη is defined as an aggregation ξ of subtree-lus-commute costs gκη, k, η, each associated with child κη, k. In turn, each cost gκη, k, η is the sum of the cost fκη, k of the subtree rooted at κη, k and the commuting cost δκη, k, κη, k, η from the child meeting location to the current meeting location. The base case for the recursion occurs when η is a child since fη = ξ η, η, [gκη, k, η] 0 = ξη, η, []. 3 An otimal solution to a roblem instance is a meeting location function which minimizes the total cost of the meeting tree: arg min fρ. 4 Table I rovides a summary of symbols for defining a roblem instance and solution. IV. TREE DYNAMIC PROGRAMMING Define the otimal subtree cost function v : Υ X R as vη, x fη. 5, η=x In other words, v secifies the minimal total cost of the subtree rooted at η subject to the condition that the meeting η is located at x. Define the otimal subtree-lus-commute cost function w : Υ X R as wη, x gη, x. 6 In other words, wη, x secifies the minimal total cost of the subtree rooted at η and the commuting cost from the meeting location η to x. Because of the recursive definition of f and g in 2, vη, x and wη, x do not deend on any ancestor nodes of η. We observe two imortant roerties of the meeting tree roblem that allow an otimal solution to be found efficiently. The first observation is that the functions v and w can be comuted in a single ass through the meeting tree from the leaves towards the root, as shown in Theorem 1. The second observation is that the otimal meeting locations can be comuted in a single ass through the meeting tree from the root towards the leaves, given that we already know the value function v, as shown in Theorem 2. A. Theory The following theorem establishes a DP rincile for v and w. It shows how v and w can be comuted recursively. The theorem defines the otimal subtree cost vη, x of node η at location x in terms of the children s otimal subtreelus-commute costs wκη, k, x at the same location. In turn, the theorem defines the otimal subtree-lus-commute costs wη, x of node η at location x in terms of the otimal subtree costs vη, y at each location y. Theorem 1: The otimal subtree cost function v satisfies for all nodes η Υ and for all x X vη, x = ξ η, x, [wκη, k, x] Kη. 7 Furthermore, the otimal subtree-lus-commute cost function w satisfies for all nodes η Υ such that η ρ and for all x X wη, x [vη, y + δη, y, x]. 8 The roof first shows that 7 holds for the case where η is a leaf. It then shows that 7 holds for the alternative case where η has children. Finally, it shows that 8 holds for all nodes η other than ρ. Proof: Let η Υ such that Kη = 0. By the definition of v in 5 and the base case for f in 3, we know that 7 is satisfied. Now let η Υ such that Kη > 0. The following equation begins with the definition of v from 5: vη, x fη, η=x ξ η, η, [gκη, k, η] Kη, η=x ξ η, x, [gκη, k, x] Kη = ξ η, x, [min gκη, k, x] Kη = ξ η, x, [wκη, k, x] Kη.

4 The second equality uses the definition of function f from 2a. For the third equality, the constraint η = x is removed by relacing η with x in the exression. For the fourth equality, the min oerator is brought inside ξ and alied directly to each k-indexed element of the vector because ξ is nondecreasing in each such element and gκη, k, x for the kth child deends only on the meeting ositions of nodes within the the subtree rooted at κη, k. The final equality follows from the definition of w from 6 and results in the RHS of 7. Therefore, since we have considered both the cases where Kη = 0 and Kη > 0 and we have not assumed a articular x X, 7 holds for all nodes η Υ for all x X. Now let η Υ such that η ρ. The following equation begins with the definition of w from 6: wη, x gη, x min min, η=y, η=y gη, x [fη + δη, η, x] min fη + δη, y, x, η=y [vη, y + δη, y, x]. The second equality holds because minimizing over with fixed η = y and then minimizing over all η = y has the same result as miniming over all at once. The third equality is by 2b. For the fourth equality we use the constraint η = y to relace η with y in δη, η, x and then remove it from the inner minimization since it is indeendent of the minimizer. The final equality follows from the definition of v in 5 and results in the RHS of 8. Therefore, since we have not assumed a articular x X, 8 holds for all nodes η Υ such that η ρ and for all x X. The following theorem shows how can be comuted recursively with ρ as the base case, given that v is already known. The theorem defines the otimal root meeting location ρ as the location that minimizes over all locations x X the otimal subtree cost vρ, x. The theorem then defines the otimal meeting locations η for η ρ in terms of otimal subtree costs vη, y at each location y X and the arent s otimal meeting location ιη. Theorem 2: Let : Υ X be a solution to 4. For the root meeting ρ, the following holds: ρ argmin vρ, x. 9 x X Furthermore, for all nodes η Υ such that η ρ, the following holds: η argmin [vη, y + δη, y, ιη]. 10 Proof: Assume 9 is false. Then we have min fρ x X vρ, x x X < vρ, ρ min fρ, ρ=x, η= ρ fρ fρ. 11 The first equality holds because minimizing over with fixed η = x and then minimizing over all η = x has the same result as miniming over all at once. The second equality is by 5. The inequality holds because of our assumtion that 9 is false. The third equality is also by 5. The final equality is by 4. The result in 11 is a contradiction, so the assumtion must be false. Therefore, 9 holds. Let η Υ such that η ρ. Assume 10 is false. Then we have min gη, ιη = wη, ιη [vη, y + δη, y, ιη] < vη, η + δη, η, ιη, η= η [fη + δη, η, ιη], η= η gη, ιη gη, ιη. 12 The first equality holds by 6. The second equality is by 8. The inequality holds because of our assumtion that 10 is false. The third equality holds by 5 and because δη, η, ιη can be brought inside the min without affecting the result. The fourth equality is by 2b. The final equality holds by Lemma 1. The result in 12 is a contradiction, so the assumtion must be false. Therefore, 10 holds. Lemma 1: Let : Υ X be a solution to 4. Let η Υ such that η ρ. Let q = ιη. Then min gη, q gη, q. 13, η= η Proof: Let η Υ such that η ρ. Assume 13 is false. Then we know that min gη, q < min gη, q, η= η Let ˇ : Υ X be such that gη, q = ˇ arg min gη, q. 14 Define Υ Υ to be the set of nodes in the subtree rooted at η, including node η. Define : Υ X as ˇ η, if η η = Υ η, otherwise

5 Then we have by 2, 14, the monotonicity of ξ, and 4 fρ = < fρ = fρ, a contradiction. Therefore, our assumtion is false and 13 holds. B. Algorithm Comlexity A DP algorithm based on the results of Theorems 1 and 2 can be used to find an otimal meeting location function. The algorithm first comutes in a leaves-to-root ass the value functions v and w for all nodes η Υ such that η ρ using 7 and 8. It then comutes vρ, x using 7. Next it comutes ρ using 9. Finally, the algorithm comutes in a root-to-leaves ass the meeting locations η for each η Υ such that η ρ using 10. We assume that the comlexity of evaluating ξη, x, ω or δη, y, x is O1. For a single η Υ and all x X, the comlexity of comuting vη, x using 7 is O X, while the comlexity of comuting wη, x using 8 involves a minimization over all states y X and so is O X 2. On the other hand, for a single node η the comlexity of comuting η using 10 involves a minimization over all states y X and so is O X. Since there are Υ nodes and v and w are comuted in a single leaves-to-root ass of the meeting tree, while is comuted in a single root-to-leaves ass of the meeting tree, the comlexity of comuting w dominates the other costs and the overall comlexity of the algorithm is O Υ X 2. In the next section we examine how w can be comuted more efficiently in order to imrove the overall comlexity in cases where the state sace X contains additional structure. V. STATE DYNAMIC PROGRAMMING We examine two secial cases of the roblem formulated in Section III. In the first case, the state sace X is the set of nodes of a discrete satial grah and the cost of commuting between two neighboring states is secified. In the second case, X is a continuous state sace and a limit on the seed of commuting in each direction is secified. In both these cases instead of secifying δ directly, we exloit the structure of the state sace and only secify the commuting costs locally. We then define δ using a state DP rincile, distinct from the tree DP rincile of Theorem 1. This state DP rincile can be used to comute w for a single node η using a DP algorithm that is more efficient than alying 8 directly. Comuting wη, x for any node η Υ and state x X can be viewed as an otimal starting roblem. We otimize over starting states y as well as over ossible trajectories to determine the otimal subtree-lus-commute cost wη, x from otimal subtree costs vη, y. Let ζ be a trajectory through X and let λζ be the total cost of traversing trajectory ζ. We have δη, x, y = min λζ, 15 ζ Zx,y where Zx, y is the set of all trajectories that begin at x and end at y. Then from 8 [ ] wη, x vη, y + min. 16 ζ Zy,x λζ We examine for each secial case a state DP rincile based on this exression. Note that the dynamic rogramming is done for each robot indeendently. A. Discrete State Sace Let X be the finite set of nodes of a directed satial grah. In this case, Dijkstra s algorithm can be used to comute wη, x for each node η in a single ass through the states in X. A discrete robot clinic consultation roblem that fits into this category is solved in [2]. We let N x be the set of neighbors of x. For convenience we define a set N 1 x = y x N y, the set of states for which x is a neighbor. Let c : Υ X 2 R + be a ositive direct commuting cost function, where cη, x, y gives the cost of commuting directly from state x X to y X. If y / N x then cη, x, y = for all η Υ. Let ζl, 1 l L ζ be a trajectory of L ζ states through X with each ζl X and such that ζl + 1 N ζl, for 1 l L ζ 1. We may use L in lace of L ζ where the trajectory ζ is obvious. Define λζ as L 1 λζ = cη, ζl, ζl + 1. l=1 We observe that the value wη, x for any node η Υ and any state x X can be comuted using only the value vη, x at the same state x X and the values wη, y at the neighboring states y N 1 x. The following roosition establishes a DP rincile based on this roerty and is equivalent to Theorem 2 in [2]. Proosition 1: The value function w satisfies for all nodes η Υ and for all x X max wη, x vη, x, max y N 1 x [wη, x wη, y cη, y, x] = As long as N x and N 1 x are indeendent of X, the values w for each node η can be comuted efficiently using a slight modification of Dijkstra s algorithm. The udate in Dijkstra s algorithm is relaced by 17. Also, the locations η for each node η can be comuted using a discrete steeest descent algorithm, which moves backwards along an otimal meeting-to-meeting trajectory in arg min ζ Z η, ιη λζ until the condition wη, ζl = vη, ζl is satisfied. At this oint an otimal meeting location η has been reached. The imlementation of these algorithms in the context of the tree DP algorithm is resented in [2]. B. Continuous State Sace Let X R d be a comact, connected state sace. Let ζt, t [T ζ 0, T ζ f ] be a trajectory through X. Constrain ζt = dζt dt Aη, ζt 18

6 where Aη, x R d is a comact, convex action set containing the origin in its interior. We may use T 0 and T f in lace of T ζ 0 and T ζ f where the trajectory ζ is obvious. Define λζ as λζ = T ζ f T ζ A DP rincile for this roblem can be stated in the form of a differential variational inequality that includes a Hamilton-Jacobi PDE as a comonent. A closely related variational inequality for an otimal stoing roblem is derived in [12]. Proosition 2: The value function w satisfies in the viscosity sense for all nodes η Υ and for all x X max wη, x vη, x, max [D xwη, x a 1] = 0. a Aη,x 20 It is usually not ossible to solve this variational inequality exactly. Instead we may comute an aroximate solution w by solving a discretized inequality on a grid. For examle, the state sace may be discretized into an orthogonal grid and the satial gradient D x wη, x in 20 relaced with an uwind, first-order finite difference aroximation. Aroaches for solving the discretized Hamilton-Jacobi PDE include sweeing methods [13], the Fast Marching Method FMM [14], [15], and Ordered Uwind Methods OUMs [16]. The discretized Hamilton-Jacobi PDE can be modified to form a monotone discretization of the variational inequality [17] which can be solved by any of the above methods. Other aroriate DP algorithms [7] may also be used to solve the discretized variational inequality. Aroximate meeting locations η for each η can be comuted using a continuous steeest descent algorithm. Assuming v and w have been calculated, the steeest descent solves the following ODE to determine an aroximately otimal trajectory from ιη backwards towards η: d ζ dt = argmax [D x wη, x a]. 21 a Aη,x For examle, in a simle imlementation, the ODE 21 is discretized using forward Euler and the gradient D x wη, x is determined by a first-order finite difference scheme. The comutation of the trajectory is comleted and η is found when wη, ζt vη, ζt. C. Algorithm Comlexity We first consider the case of the discrete satial grah. Dijkstra s algorithm has comlexity O X N x log X = O X log X if it is imlemented by maintaining a minhea sorted on wη, x. The comlexity of the steeest descent algorithm is O X N 1 x = O X, since the otimal meeting-to-meeting trajectory can ass through each node x X at most once and at each node consider at most N 1 x neighbors to extend the trajectory. Consequently, the comlexity of comuting w using Dijkstra s algorithm still dominates the other costs. However, the overall comlexity of the tree DP algorithm has been reduced to O Υ X log X. Fig. 1. Meeting tree for the robot arm roblem. Begin robot i is indicated by bi. Picku cargo for robot 1 is indicated by m1. Pass cargo from robot i to robot j is indicated by mij. Drooff cargo for robot 3 is indicated by m3. We now consider the use of FMM or OUMs for the continuous state sace case. FMM and OUMs are generalizations of Dijkstra s algorithm which have comlexity O X log X, where X is the discretized state sace. So, in this case the overall comlexity of the tree DP algorithm is O Υ X log X. VI. EXAMPLE We solve a continuous robot arm cargo transort roblem. The roblem involves three robot arms cooerating to transort cargo from a cargo icku location in the to-right of the worksace to a cargo drooff location in the to-left of the worksace. Robot 1 is attached to the lower-right corner of the worksace and has two rotational joints. The rimary joint has an angular range of π/2 radians, such that the rimary segment cannot swing out of the worksace, and the secondary joint has an angular range of 2π radians, but the secondary segment cannot swing through the rimary segment. Robot 3 is attached to the lower-left corner of the worksace and otherwise has the same roerties as robot 1. Robot 2 moves on a sliding joint along the to of the worksace. It also has a rotational joint that moves through an angle of π radians, such that the arm cannot swing out of the worksace. Each robot begins such that its end effector is in a circular starting area. Robot 1 must ick u the cargo and ass it to robot 2, then robot 2 must ass the cargo to robot 3, who dros off the cargo. For two robots to meet, their end effectors must aroach within a small neighborhood of one another. The goal is to find meeting locations and connecting state trajectories that minimize the total cost of transorting the cargo across the worksace. The corresonding meeting tree is shown in Figure 1, and the resulting robot arm motions are deicted in Figure 2. Each robot arm has 2 degrees of freedom, one for each of its joints. Consequently, each has a continuous 2-dimensional state sace. Let a robot s state sace X exclude those states that result in the intersection of the robot with an obstacle. Let Aη, x from 20 be defined as Aη, x = a a 1 1 for all η and all x X. We use the Manhattan norm to constrain Aη, x because we wish to minimize the sum of the joint costs for each robot [18]. The total cost of transorting the cargo is the sum of the costs of the robot

7 on a discretized orthogonal grid of the robot state sace. The reasons for solving the corresonding Eikonal PDE and methods for doing so are discussed in [18]. We use 19 to measure the cost of a robot trajectory in the robot state sace, but the occurance of a meeting deends on the end effector location in the worksace. For this reason, we modify the algorithm to emloy two grids for each meeting node: a worksace grid for the values v and a robot state grid for the values w. For the solution illustrated in Figure 2 we use a worksace grid of nodes, a state grid of nodes for robots 1 and 3, and a state grid of nodes for robot 2. During the comutation of v, w, and η, forward kinematics are used to ma robot states to end effector locations in the worksace. More details on how the algorithm uses the worksace grid and robot state grids can be found in [2]. VII. CONCLUSION We have secified a class of multi-location robot rendezvous roblems for which dynamic rogramming can be used to find otimal solutions. Aroriate dynamic rogramming rinciles have been resented and used to construct an efficient algorithm. The alicability of the algorithm has been demonstrated on a continuous robot arm examle. Fig. 2. 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 comletion of the motions. The starting area for the end effector of each robot arm is indicated by a large circle. The square box on the to-right/to-left is the icku/drooff 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 icku area. b Robot 1 moves from the cargo icku 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 drooff area. state trajectories, so we define ξ as in 1. There are no meeting costs incurred when two robots meet meetings mij in Figure 1. 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 icku meeting m1 or drooff meeting m3 within the resective icku or drooff area but there is an infinite cost for cargo icku or drooff outside the aroriate area. We modify the algorithm described in Section V-B to solve the robot arm roblem. We use FMM to solve the discretized variational inequality. More secifically, for each meeting node we aroximately solve an Eikonal-tye variational inequality max wη, x vη, x, D x wη, x 1 = 0. ACKNOWLEDGMENT This work was suorted by a Discovery grant from the National Sciences and Engineering Research Council of Canada. We thank the authors of [3] for motivation. REFERENCES [1] K. Alton and I. M. Mitchell, Efficient dynamic rogramming for otimal multi-location robot rendezvous, in Proceedings of the IEEE Conference on Decision and Control, 2008, to aear. [2], Efficient otimal multi-location robot rendezvous, Deartment of Comuter Science, University of British Columbia, Tech. Re. TR , [3] Y. Litus, R. T. Vaughan, and P. Zebrowski, The frugal feeding roblem: Energy-efficient, multi-robot, multi-lace rendezvous, in Proceedings of IEEE ICRA, 2007, [4] H. Choset, K. M. Lynch, S. Hutchinson, G. Kantor, W. Burgard, L. E. Kavraki, and S. Thrun, Princiles of Robot Motion: Theory, Algorithms, and Imlementations. Cambridge, Massachusetts and London, England: The MIT Press, [5] S. M. Lavalle, Planning Algorithms. Cambridge University Press, [6] E. W. Dijkstra, A note on two roblems in connection with grahs, Numer. Math., vol. 1, no. 1, , Dec [7] D. P. Bertsekas, Dynamic Programming and Otimal Control: Second Edition. Belmont, Massachusetts: Athena Scientific, [8] Y. Kuitz and H. Martini, Geometric asects of the generalized Fermat-Toricelli roblem, Bolyai Society Mathematical Studies, vol. 6, , [9] Wikiedia, Steiner tree, 2008, [accessed 6-August-2008]. [Online]. Available: htt://en.wikiedia.org/wiki/steiner%5ftree [10] G. Sahina and H. Sralb, A review of hierarchical facility location models, Comuters and Oerations Research, vol. 34, no. 8, , [11] M. S. Branicky, R. Hebbar, and G. Zhang, A fast marching algorithm for hybrid systems, in Proceedings of the 38th Conference on Decision and Control, 1999, [12] M. Bardi and I. Cauzzo-Dolcetta, Otimal Control and Viscosity Solutions of Hamilton-Jacobi-Bellman Equations. Boston and Basel and Berlin: Birkhauser, 1997.

8 [13] J. Qian, Y. Zhang, and H. Zhao, A fast sweeing method for static convex Hamilton-Jacobi equations, J. Sci. Comut., vol. 31, no. 1-2, , May [14] J. N. Tsitsiklis, Efficient algorithms for globally otimal trajectories, in Proceedings of the 33rd Conference on Decision and Control, 1994, [15] J. A. Sethian, A fast marching level set method for monotonically advancing fronts, Proc. Natl. Acad. Sci., vol. 93, , [16] J. A. Sethian and A. Vladimirsky, Ordered uwind methods for static Hamilton-Jacobi equations: Theory and algorithms, SIAM J. Numer. Anal., vol. 41, no. 1, , [17] A. M. Oberman, Convergent difference schemes for degenerate ellitic and arabolic equations: Hamilton-Jacobi equations and free boundary roblems, SIAM J. Numer. Anal., vol. 44, no. 2, , [18] K. Alton and I. M. Mitchell, Otimal ath lanning under different norms in continuous state saces, in Proceedings of the International Conference on Robotics and Automation, 2006,