Multiple Robots

Suppose that multiple robots share the same world, . A path must be computed

W

for each robot that avoids collisions with obstacles and with other robots. In Chapter 4, each robot could be a rigid body, A, or it could be made of k attached bodies, A1, . . ., Ak. To avoid confusion, superscripts will be used in this section to denote different robots. The ith robot will be denoted by A . Suppose there are m robots, A , A , . . ., A . Each robot, A , has its associated C-space, C , and

i

1 2 m i i

its initial and goal configurations, qi

init

and q

i goal

, respectively.

• • 1. Problem Formulation

A state space is defined that considers the configurations of all robots simultane- ously,

X = C × C × · · · × C . (7.6)

1 2 m

A state x ∈ X specifies all robot configurations and may be expressed as x =

i

i=1

(q1, q2, . . . , qm). The dimension of X is N, which is N = 寸m dim(C ).

1

s

0 t

Figure 7.5: Vertical cell decomposition can solve the path tuning problem. Note that this example is not in general position because vertical edges exist. The goal is to reach the horizontal line at the top, which can be accomplished from any adjacent 2-cell. For this example, it may even be accomplished from the first 2-cell if the robot is able to move quickly enough.

There are two sources of obstacle regions in the state space: 1) robot-obstacle collisions, and 2) robot-robot collisions. For each i such that 1 ≤ i ≤ m, the subset of X that corresponds to robot A in collision with the obstacle region, O, is

i

obs = {x ∈ X | A (q ) ∩ O /= ∅}. (7.7)

X

i i

i

This only models the robot-obstacle collisions.

For each pair, A and A , of robots, the subset of X that corresponds to A in collision with A is

j

i j i

obs = {x ∈ X | A (q ) ∩ A (q ) /= ∅}. (7.8)

X

i i j j

ij

Both (7.7) and (7.8) will be combined in (7.10) later to yield Xobs.

Formulation 7.2 (Multiple-Robot Motion Planning)

• • • 1. The world W and obstacle region O are the same as in Formulation 4.1.
      2. There are m robots, 1, . . ., m, each of which may consist of one or more bodies.

A A

• • • 1. Each robot Ai, for i from 1 to m, has an associated configuration space, Ci.
      2. The state space X is defined as the Cartesian product

X = C1 × C2 × · · · × Cm. (7.9)

The obstacle region in X is

（ m ＼ （ ＼

I X

X

Xobs =

i obs

i=1

I ij,_I_i/=j

ij obs

, (7.10)

in which Xi

obs

and Xij

are the robot-obstacle and robot-robot collision states

from (7.7) and (7.8), respectively.

obs

• • • 1. A state xI ∈ Xfree is designated as the initial state, in which xI = (q , . . . , q ).

1 m

I

I

For each i such that 1 ≤ i ≤ m, qI specifies the initial configuration of A .

i

i

• • • 1. A state xG ∈ Xfree is designated as the goal state, in which xG = (q , . . . , q ).

1 m

G

G

• • • 1. The task is to compute a continuous path τ : [0, 1] → Xfree such that

τ (0) = xinit and τ (1) ∈ xgoal.

An ordinary motion planning problem? On the surface it may appear that there is nothing unusual about the multiple-robot problem because the formu- lations used in Chapter 4 already cover the case in which the robot consists of multiple bodies. They do not have to be attached; therefore, X can be considered as an ordinary C-space. The planning algorithms of Chapters 5 and 6 may be ap- plied without adaptation. The main concern, however, is that the dimension of X grows linearly with respect to the number of robots. For example, if there are 12 rigid bodies for which each has = SE(3), then the dimension of X is 6 12 = 72. Complete algorithms require time that is at least exponential in dimension, which makes them unlikely candidates for such problems. Sampling-based algorithms are more likely to scale well in practice when there many robots, but the dimension of X might still be too high.

i

C ·

Reasons to study multi-robot motion planning Even though multiple- robot motion planning can be handled like any other motion planning problem, there are several reasons to study it separately:

1. The motions of the robots can be decoupled in many interesting ways. This leads to several interesting methods that first develop some kind of partial plan for the robots independently, and then consider the plan interactions to produce a solution. This idea is referred to as decoupled planning.
2. The part of Xobs due to robot-robot collisions has a cylindrical structure, depicted in Figure 7.6, which can be exploited to make more efficient plan-

ning algorithms. Each Xij defined by (7.8) depends only on two robots. A

obs

point, x = (q1, . . . , qm), is in Xobs if there exists i, j such that 1 ≤ i, j ≤ m

and Ai(qi) ∩ Aj (qj ) /= ∅, regardless of the configurations of the other m − 2

robots. For some decoupled methods, this even implies that Xobs can be completely characterized by 2D projections, as depicted in Figure 7.9.

Figur on X.

A7

A5

A4

A6

• 1. (b)

Figure 7.7: (a) A collection of pieces used to define an assembly planning problem;

• 1. assembly planning involves determining a sequence of motions that assembles the parts. The object shown here is assembled from the parts.

• If optimality is important, then a unique set of issues arises for the case of multiple robots. It is not a standard optimization problem because the per- formance of each robot has to be optimized. There is no clear way to combine these objectives into a single optimization problem without losing some crit- ical information. It will be explained in Section 7.7.2 that Pareto optimality naturally arises as the appropriate notion of optimality for multiple-robot motion planning.

Assembly planning One important variant of multiple-robot motion planning is called assembly planning; recall from Section 1.2 its importance in applications. In automated manufacturing, many complicated objects are assembled step-by- step from individual parts. It is convenient for robots to manipulate the parts one-by-one to insert them into the proper locations (see Section 7.3.2). Imagine a collection of parts, each of which is interpreted as a robot, as shown in Figure 7.7a. The goal is to assemble the parts into one coherent object, such as that shown in Figure 7.7b. The problem is generally approached by starting with the goal

configuration, which is tightly constrained, and working outward. The problem formulation may allow that the parts touch, but their interiors cannot overlap. In general, the assembly planning problem with arbitrarily many parts is NP- hard. Polynomial-time algorithms have been developed in several special cases. For the case in which parts can be removed by a sequence of straight-line paths, a polynomial-time algorithm is given in [973, 974].

• • 1. Decoupled planning

Decoupled approaches first design motions for the robots while ignoring robot- robot interactions. Once these interactions are considered, the choices available to each robot are already constrained by the designed motions. If a problem arises, these approaches are typically unable to reverse their commitments. Therefore, completeness is lost. Nevertheless, decoupled approaches are quite practical, and in some cases completeness can be recovered.

Prioritized planning A straightforward approach to decoupled planning is to sort the robots by priority and plan for higher priority robots first [320, 951]. Lower priority robots plan by viewing the higher priority robots as moving obstacles.

Suppose the robots are sorted as A , . . ., A , in which A has the highest priority.

i

1 m 1

Assume that collision-free paths, τi : [0, 1] → Cfree, have been computed for i

from 1 to n. The prioritized planning approach proceeds inductively as follows:

Base case: Use any motion planning algorithm from Chapters 5 and 6 to

compute a collision-free path, τ1 : [0, 1] → Cfree for A . Compute a timing

1

1

1

function, σ1, for τ1, to yield φ1 = τ1 ◦ σ1 : T → Cfree.

Inductive step: Suppose that φ1, . . ., φi−1 have been designed for A , . . ., A , and that these functions avoid robot-robot collisions between any of the first i − 1 robots. Formulate the first i − 1 robots as moving obstacles in

j

j

i−1

1

W. For each t ∈ T and j ∈ {1, . . . , i − 1}, the configuration q of each A is

φj (t). This yields Aj (φj (t)) ⊂ W, which can be considered as a subset of the obstacle O(t). Design a path, τi, and timing function, σi, using any of the time-varying motion planning methods from Section 7.1 and form φi = τi ◦σi.

Although practical in many circumstances, Figure 7.8 illustrates how completeness is lost.

A special case of prioritized planning is to design all of the paths, τ1, τ2,

. . ., τm, in the first phase and then formulate each inductive step as a velocity tuning problem. This yields a sequence of 2D planning problems that can be solved easily. This comes at a greater expense, however, because the choices are even more constrained. The idea of preplanned paths, and even roadmaps, for all robots independently can lead to a powerful method if the coordination of the robots is approached more carefully. This is the next topic.

Figure 7.8: If neglects the query for , then completeness is lost when using the prioritized planning approach. This example has a solution in general, but prioritized planning fails to find it.

1 2

A A

Fixed-path coordination Suppose that each robot A is constrained to follow a

i

i

path τi : [0, 1] → Cfree, which can be computed using any ordinary motion planning

technique. For m robots, an m-dimensional state space called a coordination space

is defined that schedules the motions of the robots along their paths so that they will not collide [746]. One important feature is that time will only be implicitly represented in the coordination space. An algorithm must compute a path in the coordination space, from which explicit timings can be easily extracted.

For m robots, the coordination space X is defined as the m-dimensional unit cube X = [0, 1]m. Figure 7.9 depicts an example for which m = 3. The ith

coordinate of X represents the domain, Si = [0, 1], of the path τi. Let si denote a point in Si (it is also the ith component of x). A state, x ∈ X, indicates the

configuration of every robot. For each i, the configuration qi ∈ Ci is given by

qi = τi(si). At state (0, . . . , 0) ∈ X, every robot is in its initial configuration,

I = τi(0), and at state (1, . . . , 1) ∈ X, every robot is in its goal configuration,

q

i

G = τi(1). Any continuous path, h : [0, 1] → X, for which h(0) = (0, . . . , 0) and

q

i

h(1) = (1, . . . , 1), moves the robots to their goal configurations. The path h does

not even need to be monotonic, in contrast to prioritized planning.

One important concern has been neglected so far. What prevents us from designing h as a straight-line path between the opposite corners of [0, 1]m? We have not yet taken into account the collisions between the robots. This forms an obstacle region Xobs that must be avoided when designing a path through X.

Thus, the task is to design h : [0, 1] → Xfree, in which Xfree = X \ Xobs.

The definition of Xobs is very similar to (7.8) and (7.10), except that here the state-space dimension is much smaller. Each qi is replaced by a single parameter. The cylindrical structure, however, is still retained, as shown in Figure 7.9. Each cylinder of Xobs is

i j

ij obs

X

= {(s1, . . . , sm) ∈ X | A (τi(si)) ∩ A (τj (sj ))

∅}, (7.11)

which are combined to yield

obs

Xobs =

I

ij, i/=j

Xij . (7.12)

_s_2 _s_3 _s_3

_s_1 _s_1 _s_2

_s_3

_s_1

Figure 7.9: The obstacles that arise from coordinating m robots are always cylin-

drical. The set of all 1 m(m − 1) axis-aligned 2D projections completely charac-

2

terizes Xobs.

Standard motion planning algorithms can be applied to the coordination space because there is no monotonicity requirement on h. If 1) W = R2, 2) m = 2

(two robots), 3) the obstacles and robots are polygonal, and 4) the paths, τi, are piecewise-linear, then Xobs is a polygonal region in X. This enables the methods

of Section 6.2, for a polygonal Cobs, to directly apply after the representation of

Xobs is explicitly constructed. For m > 2, the multi-dimensional version of vertical cell decomposition given for m = 3 in Section 6.3.3 can be applied. For general coordination problems, cylindrical algebraic decomposition or Canny’s roadmap algorithm can be applied. For the problem of robots in = R2 that either translate or move along circular paths, a resolution complete planning method based on the exact determination of Xobs using special collision detection methods is given in [886].

W

For very challenging coordination problems, sampling-based solutions may

yield practical solutions. Perhaps one of the simplest solutions is to place a grid over X and adapt the classical search algorithms, as described in Section

• • 1. [606, 746]. Other possibilities include using the RDTs of Section 5.5 or, if the multiple-query framework is appropriate, then the sampling-based roadmap methods of 5.6 are suitable. Methods for validating the path segments, which were covered in Section 5.3.4, can be adapted without trouble to the case of coordination spaces.

Thus far, the particular speeds of the robots have been neglected. For expla- nation purposes, consider the case of m = 2. Moving vertically or horizontally in X holds one robot fixed while the other moves at some maximum speed. Moving diagonally in X moves both robots, and their relative speeds depend on the slope of the path. To carefully regulate these speeds, it may be necessary to reparam- eterize the paths by distance. In this case each axis of X represents the distance traveled, instead of [0, 1].

Fixed-roadmap coordination The fixed-path coordination approach still may not solve the problem in Figure 7.8 if the paths are designed independently. For- tunately, fixed-path coordination can be extended to enable each robot to move over a roadmap or topological graph. This still yields a coordination space that has only one dimension per robot, and the resulting planning methods are much closer to being complete, assuming each robot utilizes a roadmap that has many alternative paths. There is also motivation to study this problem by itself because of automated guided vehicles (AGVs), which often move in factories on a net- work of predetermined paths. In this case, coordinating the robots is the planning problem, as opposed to being a simplification of Formulation 7.2.

One way to obtain completeness for Formulation 7.2 is to design the indepen- dent roadmaps so that each robot has its own garage configuration. The conditions for a configuration, qi, to be a garage for i are 1) while at configuration qi, it is impossible for any other robots to collide with it (i.e., in all coordination states for which the ith coordinate is qi, no collision occurs); and 2) qi is always reachable by from xI . If each robot has a roadmap and a garage, and if the planning method for X is complete, then the overall planning algorithm is complete. If the planning method in X uses some weaker notion of completeness, then this is also maintained. For example, a resolution complete planner for X yields a resolution complete approach to the problem in Formulation 7.2.

A

i

A

Cube complex How is the coordination space represented when there are mul- tiple paths for each robot? It turns out that a cube complex is obtained, which is a special kind of singular complex (recall from Section 6.3.1). The coordination space for m fixed paths can be considered as a singular m-simplex. For example, the problem in Figure 7.9 can be considered as a singular 3-simplex, [0, 1]3 X. In Section 6.3.1, the domain of a k-simplex was defined using Bk, a k-dimensional ball; however, a cube, [0, 1]k, also works because Bk and [0, 1]k are homeomorphic. For a topological space, X, let a k-cube (which is also a singular k-simplex),

→

• • • 1. (b)

Figure 7.10: (a) An example in which moves along three paths, and moves along one. (b) The corresponding coordination space.

1 2

A A

口 k, be a continuous mapping σ : [0, 1]k → X. A cube complex is obtained by connecting together k-cubes of different dimensions. Every k-cube for k ≥ 1 has 2k faces, which are (k − 1)-cubes that are obtained as follows. Let (s1, . . . , sk) denote a point in [0, 1]k. For each i ∈ {1, . . . , k}, one face is obtained by setting

si = 0 and another is obtained by setting si = 1.

The cubes must fit together nicely, much in the same way that the simplexes of a simplicial complex were required to fit together. To be a cube complex, must be a collection of simplexes that satisfy the following requirements:

K

1. Any face, 口 k−1, of a cube 口 k ∈ K is also in K.
2. The intersection of the images of any two k-cubes 口 k, 口′k , is either empty or there exists some cube, 口i for i < k, which is a common face of both 口k and 口′k.

∈ K

∈ K

Let Gi denote a topological graph (which may also be a roadmap) for robot A .

i

The graph edges are paths of the form τ : [0, 1] → Cfree. Before covering formal

i

definitions of the resulting complex, consider Figure 7.10a, in which 1 moves

A

along three paths connected in a “T” junction and 2 moves along one path. In this case, three 2D fixed-path coordination spaces are attached together along one common edge, as shown in Figure 7.10b. The resulting cube complex is defined by three 2-cubes (i.e., squares), one 1-cube (i.e., line segment), and eight 0-cubes (i.e., corner points).

A

Now suppose more generally that there are two robots, A1 and A2, with asso- ciated topological graphs, G1(V1, E1) and G2(V2, E2), respectively. Suppose that G and G2 have n1 and n2 edges, respectively. A 2D cube complex, K, is obtained as follows. Let τi denote the ith path of G1, and let σj denote the jth path of G2. A 2-cube (square) exists in K for every way to select an edge from each graph. Thus, there are n1n2 2-cubes, one for each pair (τi, σj ), such that τi ∈ E1 and σj ∈ E2. The 1-cubes are generated for pairs of the form (vi, σj ) for vi ∈ V1 and σj ∈ E2, or (τi, vj ) for τi ∈ E1 and vj ∈ V2. The 0-cubes (corner points) are reached for each pair (vi, vj ) such that vi ∈ V1 and vj ∈ V2.

If there are m robots, then an m-dimensional cube complex arises. Every m-cube corresponds to a unique combination of paths, one for each robot. The (m 1)-cubes are the faces of the m-cubes. This continues iteratively until the 0-cubes are reached.

−

Planning on the cube complex Once again, any of the planning methods described in Chapters 5 and 6 can be adapted here, but the methods are slightly complicated by the fact that X is a complex. To use sampling-based methods, a dense sequence should be generated over X. For example, if random sampling is used, then an m-cube can be chosen at random, followed by a random point in the cube. The local planning method (LPM) must take into account the con- nectivity of the cube complex, which requires recognizing when branches occur in the topological graph. Combinatorial methods must also take into account this connectivity. For example, a sweeping technique can be applied to produce a ver- tical cell decomposition, but the sweep-line (or sweep-plane) must sweep across the various m-cells of the complex.