Sampling-based Multi-robot Motion Planning
Zhi Yan, Nicolas Jouandeau and Arab Ali Cherif
Advanced Computing Laboratory of Saint-Denis (LIASD)
Paris 8 University, 93526 Saint-Denis, France
Multi-robot systems, Coordination, Motion planning, Sampling-based method.
This paper describes a sampling-based approach to multi-robot motion planning. The proposed approach
is centralized, which aims to reduce interference between mobile robots such as collision, congestion and
deadlock, by increasing the number of waypoints. The implementation based on occupancy grid map is
decomposed into three steps: the first step is to identify primary waypoints by using the Voronoi diagram, the
second step is to generate additional waypoints by sampling the Voronoi diagram, and the last step is to assign
the waypoints to robots by using the Hungarian method. The approach has been implemented and tested in
simulation and the experimental results show a good system performance for multi-robot motion planning.
In robotics, the motion planning problem is to pro-
duce a continuous motion from one configuration to
another in configuration space, while avoiding colli-
sion with obstacles. The motion is represented as a
path in configuration space. Motion planning is em-
inently necessary for mobile robot since, by defini-
tion, a robot accomplishes tasks by moving in the
real world (Latombe, 1991). Environments for au-
tonomous mobile robot are usually represented by an
occupancy grid map. For most of motion planning
algorithms, the robot is reduced to a point in a two-
dimensional plane.
Multi-robot motion planning is one of the most
challenging tasks in multi-robot systems. It should
consider not only the obstacles (whether static or dy-
namic) in the environment, but also the possible in-
terference between robots. When robots in a team
are used to perform independent tasks in a shared
workspace, each one will become a mobile obstacle
for the other. Thus each robot should take into ac-
count the movement of other robots. One well-studied
example is the multi-robot space sharing problem
ager and Nebel, 2001) (Luo et al., 2003) (Marcolino
and Chaimowicz, 2009a) (Marcolino and Chaimow-
icz, 2009b) (Luna and Bekris, 2011).
Multi-robot environment must definitely be dy-
namic, in which motion planning is inherently diffi-
cult. Even for a simple case in two dimensions, the
planning is NP-hard and is not solvable in polyno-
mial time (Guo and Parker, 2002). A very common
problem in the motion planning for a group of mobile
robots is when multiple robots move to the same way-
point, causing collision, congestion and deadlock, and
thus affects system performance. We define this kind
of dynamic standstill of system caused by waypoint
conflict as the multi-robot waiting situation problem.
Because, for better or worse, a robot should wait until
the other pass first, or all robots should wait for plan-
ner to replan their local trajectories. Therefore, some
kind of mechanism is needed to coordinate the motion
of robots. In this paper, we present a sampling-based
approach based on centralized decision making mech-
anism to multi-robot motion planning. The basic idea
of the proposed approach is to reduce the problem
of waiting situation between robots by increasing the
number of waypoints. The implementation includes
three critical steps:
The first step is toward identifying primary way-
points on top of an occupancy grid map by using
the Voronoi diagram.
The second step is generating additional way-
points on top of the Voronoi diagram by using a
sampling-based method.
The third step is toward assigning the waypoints
to robots by using the Hungarian method.
The remainder of the paper is organized as fol-
lows: Section 2 describes an overview of some related
works; Section 3 describes our sampling-based ap-
proach; Section 4 describes the experimental results
Yan Z., Jouandeau N. and Ali Cherif A. (2013).
Sampling-based Multi-robot Motion Planning.
In Proceedings of the 10th International Conference on Informatics in Control, Automation and Robotics, pages 549-554
DOI: 10.5220/0004605005490554
obtained with our approach; and the paper is con-
cluded in Section 5 at last.
The major approaches to multi-robot motion planning
are expanded from the results of single-robot system.
(Bennewitz et al., 2001) presented an approach based
on A* path planning algorithm to optimize the priori-
ties for decoupled and prioritized path planning meth-
ods for groups of mobile robots. Their approach is
a randomized method which repeatedly reorders the
robots to find a sequence for which a plan can be com-
puted and to minimize the overall path lengths. (Guo
and Parker, 2002) proposed an approach based on D*
path planning algorithm which contain two modules:
path planning and velocity planning. The detailed
process can be described as follows: first, each robot
plans its own path independently, then a coordina-
tion diagram is constructed based on collision checks
among all robot paths. One approach named artificial
potential field (APF) is used extensively for multi-
robot formation control which is usually linked with
multi-robot motion planning problem. (Tanner and
Kumar, 2005) presented a strategy which can ensure
almost global asymptotic convergence of the robots
to a particular oriented formation shape, while guar-
anteeing collision avoidance in the process.
The approaches often used for high-dimensional
motion planning problems can also be efficiently ap-
plied to deal with low-dimensional problems. There
are two major families of approaches known as cell
decomposition and skeletonization. The cell decom-
position method often applied for multi-robot area
coverage problem. (Hazon et al., 2006) presented
an online robust multi-robot spanning tree coverage
(ORMSTC) algorithm based on an approximate cel-
lular decomposition for covering an unknown envi-
ronment. The algorithm is called online because in
which the robots do not have apriori knowledge of
the work area, and it is robust and complete because
in which as long as a single robot is able to move, the
coverage will be completed. A typical skeletoniza-
tion method is Voronoi diagram which is often ap-
plied to the problem of robotic exploration. (Wurm
et al., 2008) presented a strategy for coordinating a
team of exploring robots by using Voronoi diagram
for the segmentation of the environment map. The
strategy extracts the critical points (Thrun, 1998) as
the exploration targets to assign to robots. An al-
ternative to the Voronoi diagrams is the probabilis-
tic roadmap (PRM) (Kavraki et al., 1996) which is
a sampling-based method. (
Svestka and Overmars,
1995) presented a PRM-based approach for multiple
nonholonomic car-like robots motion planning in the
same static workspace, in which the roadmaps for the
composite robot are derived from roadmaps for the
underlying simple robots, and the latter is computed
by a probabilistic single-robot learning method.
The sampling-based methods are currently con-
sidered state-of-the-art for robot motion planning in
complex environments. Because unlike the incremen-
tal heuristic search (such as A* and D*) and the topo-
logical map (such as Voronoi diagram) methods, their
running time does not grow exponentially with the di-
mension of the configuration space, but also easier
to implement. Yet, the sampling-based methods are
probabilistically complete, and can not determine if
no solution exists.
3.1 Voronoi Diagram
The first step of our approach is to identify primary
waypoints on top of the occupancy grid map by ap-
plying the Voronoi diagram. The Voronoi diagram is
a very popular graph partitioning method for the prob-
lem of map segmentation (Thrun, 1998) (Zivkovic
et al., 2006) (Wurm et al., 2008). For a given map
m, we define for each point p in the free space S of
m, a set O
(m) which is made up of closest obstacle
points to p. While the Voronoi diagram can be repre-
sented by a graph G(m) = (V, E), where V is the set
of points in O
(m) that are equidistant to at least two
nearest obstacles, and E is the set of edges connecting
two adjacent points in V of m:
V = p S|
2 (1)
E = (p, q)|p, q V, p adjacent to q (2)
Figure 1 shows the Voronoi diagrams of two experi-
mental maps.
Then the topological map is built based on the
Voronoi diagram. The grid map is decomposed into
a small set of regions separated by narrow passages.
These narrow passages, which are called critical lines,
are found by analyzing a skeleton of the environment.
The partitioned map is mapped into an isomorphic
graph, where nodes correspond to regions and arcs
connect neighboring regions. This graph is the topo-
logical map (Thrun, 1998).
On the basis of the topological map, a series of
points called critical points is generated, at which the
distance to the nearest obstacle in the map is a lo-
cal minimum. In other words, the critical points are
(a) map A (b) map B
(c) map A (d) map B
Figure 1: Voronoi diagrams of two experimental maps. (a)
and (b) depict the original maps. (c) and (d) depict the gen-
erated Voronoi diagrams.
(a) map A (b) map B
Figure 2: The primary waypoints extracted by using the
Voronoi diagram.
the midpoints of the critical lines. These points are
considered as the primary waypoints for mobile robot
motion planning. Figure 2 illustrates the results of the
first step.
3.2 Sampling-based Method
The second step of the approach is to generate addi-
tional waypoints on top of the Voronoi diagram by
applying a PRM-like method. The original PRM is
presented by (Kavraki et al., 1996), of which the ba-
sic algorithmic framework can be summarized as fol-
Firstly, randomly sample a sufficient number of
points in the configuration space (denoted by C)
of the robot, keeping any that are not in collision
with obstacles (denoted by C
). This creates a
point set in the the free configuration space (de-
noted by C
f ree
Secondly, using a local planner (such as straight
line planner), attempt to connect pairs of sam-
(a) (b)
(c) (d)
Figure 3: Generation of the probabilistic roadmap. (a)
shows a configuration space in which we will build a proba-
bilistic roadmap. (b) shows the random points generated in
f ree
. (c) shows the roadmap constructed by connecting the
samples relatively close using the straight line planner. (d)
shows a path found by querying the constructed roadmap
using the A* algorithm.
ples that are relatively close to each other by thor-
oughly sampling and collision checking configu-
rations between them. This creates a graph data
structure called a roadmap.
Finally, to query the roadmap, first attempt to con-
nect the start and goal configurations to the exist-
ing graph. If that is successful, search the graph
for a path from start to goal using any standard
graph search method (such as A* algorithm).
Like most of motion planning algorithms, the
PRM-like method often computes a collision free
roadmap in a configuration space where the object is
reduced to a point. This point represents the robot’s
model in the environment and the dimension of con-
figuration space is equal to the number of degrees of
freedom of the mechanical system. Then the algo-
rithm searches a free path for a point in the roadmap.
Figure 3 illustrates the process of generating a proba-
bilistic roadmap for an example occupancy grid map
with 80 random samples.
An extension of the original PRM, which is called
medial axis based PRM (i.e. MAPRM) (Wilmarth
et al., 1999), aims to efficiently retract any sampled
configuration, free or not, onto the medial axis of the
free space without having to compute the medial axis
explicitly. This strategy is applied to our approach
with some small modifications. The implementation
details can be seen in Algorithm 1. When the random
(a) map A (b) map B
Figure 4: The additional waypoints extracted by using the
sampling-based method with 400 random samples.
configuration p is sampled, the nearest obstacle con-
figuration q to p should be found, and another config-
uration r is set to fix p at the center of q and r. If r is a
free configuration, then p will be moved to their cen-
ter and r will be pushed to a farther location. This step
is repeated until r becomes an obstacle configuration.
The algorithm produces a self adaptive medial axis
synthesis, which is fitted to the clearance of each lo-
cal free space. The results of applying this algorithm
can be seen in Figure 4.
Algorithm 1: Medial axis based PRM.
Require: N, the sufficient number of points to gener-
Ensure: N points in C
f ree
by sampling on the medial
1: repeat
2: Generate a uniformly random point p in C.
3: if p C
f ree
4: Find the nearest point q C
to p.
5: r = 2p q
6: while r C and r C
f ree
7: p =
8: r = 2p q
9: end while
10: if r C
11: p is added to the set of medial axis.
12: end if
13: end if
14: until N points have been generated.
3.3 Hungarian Method
The last step is to assign the waypoints to robots by
using the Hungarian method. The Hungarian method
is a combinatorial optimization algorithm which can
solve the assignment problem in polynomial time
)). It was developed and published by (Kuhn,
1955). In the context of our research, given n way-
points and n robots, we would like to find the best way
(cost minimizing assignment) to assign the waypoints
to the robots. The implementation details on this issue
by using the Hungarian method can be summarized in
the following four steps:
1. Construct a n × n matrix containing the cost of as-
signing each waypoint to a robot. The cost could
be the distance or energy required for the robot to
reach the waypoint.
2. The minimum element in each row is subtracted
from all the elements in that row, and the mini-
mum element in each column is subtracted from
all the elements in that column.
3. Find the minimum number of lines required to
cover all the zeros across rows and columns. If
the number of the lines is n, then the optimal way-
point allocation plan can be given by selecting
a combination from the modified cost matrix in
such a way that the sum is zero. Otherwise, con-
tinue with Step 4.
4. Find the smallest element which is not covered by
any of the lines. Then subtract it from each ele-
ment which is not covered by the lines and add it
to each element which is covered by a vertical and
a horizontal line. Continue with Step 3.
The implementation of the Hungarian method de-
scribed above requires a square cost matrix, which
means the number of waypoints and the number of
robots should be equal, thus each robot can only move
to a waypoint. On the contrary, if the cost matrix is
not square, then we have to augment it into a square
matrix by adding zero rows or columns. Suppose
there are three robots, r
, r
and r
, and only two way-
points, p
and p
, then the 3 × 3 cost matrix can be
represented as follows:
C =
, p
) cost(r
, p
) 0
, p
) cost(r
, p
) 0
, p
) cost(r
, p
) 0
Finally we combine the three methods described
above (i.e. the Voronoi diagram, the probabilistic
roadmap and the Hungarian method), and take into
account both the primary waypoints and the addi-
tional waypoints to coordinate multi-robot motion
4.1 Experimental Setup
Our approach has been implemented and tested in
simulation with the 2D multiple robots simulator
Player/Stage (Gerkey et al., 2003). The experiment
Figure 5: Four robots explore an unknown environment co-
operatively. The green robot and the orange robot are ex-
ploring different rooms. The red robot and the yellow robot
are exploring corridors along different paths.
is to explore an unknown indoor environment by us-
ing a group of virtual Pioneer 2-DX robot equipped
with a laser range finder which can provide 361 sam-
ples with 180 degrees field of view and a maximum
range of 8 meters. Every robot is equipped with
an abstract localization device which models the im-
plementation of SLAM (Simultaneous Localization
And Mapping), and a navigation device which per-
forms the wavefront propagation algorithm (Latombe,
1991) for global path planning and the vector field
histogram algorithm (Ulrich and Borenstein, 1998)
for goal seeking and local obstacle avoidance. A cen-
tral planning server is assumed, which is able to com-
municate with all robots and assign the exploration
tasks to each one. In addition, all the robots share an
occupancy grid map with respect to the structure of
the environment during their exploration mission.
A different number of robots (from two to six)
combining with two environments is used to evaluate
our approach. In addition, a comparison with exper-
imental results of our approach and a Voronoi-based
approach is given. Figure 1 ((a) and (b)) depicts two
experimental maps used in our simulation which are
enclosed spaces with 14 meters long and 16 meters
wide. The ratio between real-world time and simula-
tion time is about 1:3. All experiments reported in this
paper were carried out on a system with an Intel Core
2 Duo E8400 3.00GHz processor, an Intel Q43 Ex-
press chipset and two DDR2 800MHz 1024MB dual
channel memory. A screenshot of our implementation
can be seen in Figure 5.
1 2 3 4 5 6 7
exploration time gain [%]
number of robots
map A
(a) map A
1 2 3 4 5 6 7
exploration time gain [%]
number of robots
map B
(b) map B
Figure 6: Exploration time gained by using our sampling-
based approach compared with the Voronoi-based ap-
4.2 Experimental Results
The results of the experiments are given in Figure
6. We measured the exploration time gained by
our sampling-based approach and compared to the
Voronoi-based approach. In each plot, the abscissa
denotes the team size of the mobile robots, the or-
dinate denotes the time gain in percent of the total
exploration time, and the error bar indicates the con-
fidence interval of each corresponding gain of robot
team size with the 0.95 confidence level. 10 runs are
performed for each team size. Figure 6 shows that an
exploration time saving of 5.8% to 16.1% in map A
and 6.4% to 15.3% in map B is obtainable under our
sampling-based approach compared to the Voronoi-
based approach. These results show that our tech-
nique could improve performance of the multi-robot
motion planning.
We also counted the average number of waypoint
conflicts in each experiment map, as shown in Table
1. This table shows that the waypoint conflict is obvi-
ously reduced by using our approach compared to the
Table 1: The Number of Waypoint Conflicts by Using Our
Sampling-based Approach Compared with The Voronoi-
based Approach
(a) map A
# robots 2 3 4 5 6
Sampling-based 2.5 2.9 4.3 3.1 3.4
Voronoi-based 6.2 6.7 11.6 8.1 10.2
(b) map B
# robots 2 3 4 5 6
Sampling-based 0.0 1.4 2.7 2.9 2.4
Voronoi-based 4.1 6.2 6.4 7.7 10.7
Voronoi-based approach. For this reason, the problem
of waiting situation such as collision, congestion and
deadlock has been alleviated and the system perfor-
mance is improved.
In this paper, we presented a multi-robot motion plan-
ning approach based on sampling method. This ap-
proach is designed to relieve multi-robot waiting situ-
ation problem such as collision, congestion and dead-
lock, by increasing the number of waypoints for mo-
bile robots. The proposed approach includes three
main steps: the first step is to identify primary way-
points on top of an occupancy grid map by using the
Voronoi diagram, the second step is to generate ad-
ditional waypoints on top of the Voronoi diagram by
using the sampling-based method, and the third step
is to assign the waypoints to robots by using the Hun-
garian method. The efficiency of our approach was
verified by simulation and experimental results.
Bennewitz, M., Burgard, W., and Thrun, S. (2001). Op-
timizing schedules for prioritized path planning of
multi-robot systems. In Proceedings of ICRA’01,
pages 271–276, Seoul, Korea.
Gerkey, B. P., Vaughan, R. T., and Howard, A. (2003). The
Player/Stage project: Tools for multi-robot and dis-
tributed sensor systems. In Proceedings of ICAR’03,
pages 317–323, Coimbra, Portugal.
Guo, Y. and Parker, L. E. (2002). A distributed and optimal
motion planning approach for multiple mobile robots.
In Proceedings of ICRA’02, pages 2612–2619, Wash-
ington, DC, USA.
Hazon, N., Mieli, F., and Kaminka, G. A. (2006). Towards
robust on-line multi-robot coverage. In Proceedings
of ICRA’06, pages 1710–1715, Orlando, FL, USA.
ager, M. and Nebel, B. (2001). Decentralized collision
avoidance, deadlock detection, and deadlock resolu-
tion for multiple mobile robots. In Proceedings of
IROS’01, pages 1213–1219, Maui, HI, USA.
Kavraki, L. E., Kolountzakis, M. N., and Latombe, J.-C.
(1996). Analysis of probabilistic roadmaps for path
planning. In Proceedings of ICRA’96, pages 3020–
3025, Minneapolis, MN, USA.
Kuhn, H. W. (1955). The hungarian method for the assign-
ment problem. Naval Research Logistics Quarterly,
Latombe, J.-C. (1991). Robot Motion Planning. Kluwer
Academic Publishers.
Luna, R. and Bekris, K. E. (2011). Efficient and complete
centralized multi-robot path planning. In Proceedings
of IROS’11, pages 3268–3275, San Francisco, CA,
Luo, C., Yang, S. X., and Stacey, D. A. (2003). Real-
time path planning with deadlock avoidance of multi-
ple cleaning robots. In Proceedings of ICRA’03, pages
4080–4085, Taipei, Taiwan.
Marcolino, L. S. and Chaimowicz, L. (2009a). Traffic
control for a swarm of robots: Avoiding group con-
flicts. In Proceedings of IROS’09, pages 1949–1954,
St. Louis, MO, USA.
Marcolino, L. S. and Chaimowicz, L. (2009b). Traffic con-
trol for a swarm of robots: Avoiding target conges-
tion. In Proceedings of IROS’09, pages 1955–1961,
St. Louis, MO, USA.
Svestka, P. and Overmars, M. H. (1995). Coordinated mo-
tion planning for multiple car-like robots using prob-
abilistic roadmaps. In Proceedings of ICRA’95, pages
1631–1636, Nagoya, Japan.
Tanner, H. G. and Kumar, A. (2005). Towards decentraliza-
tion of multi-robot navigation functions. In Proceed-
ings of ICRA’05, pages 4132–4137, Barcelona, Spain.
Thrun, S. (1998). Learning metric-topological maps for in-
door mobile robot navigation. Artificial Intelligence,
Ulrich, I. and Borenstein, J. (1998). VFH+: Reliable obsta-
cle avoidance for fast mobile robots. In Proceedings
of ICRA’98, pages 1572–1577, Leuven, Belgium.
Wilmarth, S. A., Amatoy, N. M., and Stiller, P. F. (1999).
MAPRM: A probabilistic roadmap planner with sam-
pling on the medial axis of the free space. In Pro-
ceedings of ICRA’99, pages 1024–1031, Detroit, MI,
Wurm, K. M., Stachniss, C., and Burgard, W. (2008). Coor-
dinated multi-robot exploration using a segmentation
of the environment. In Proceedings of IROS’08, pages
1160–1165, Nice, France.
Zivkovic, Z., Bakker, B., and Kr
ose, B. (2006). Hierarchi-
cal map building and planning based on graph parti-
tioning. In Proceedings of ICRA’06, pages 803–809,
Orlando, FL, USA.