Multi-Robot Systems From Swarms to Intelligent Automata - Parker et al (Eds) Part 8 docx

M
er
g
in
g
Partial Maps without Usin
g
Odometr
y
141
T
able
2
.
R
esults of scan matching trials using different heuristic
s
S
uccesses Fa
il
ure
s
A
ll transformations 13
(
41.9%
)
18
(5
8.1%
)

Consecutive segments 21 (67.7%) 10 (32.3%
)
Random segments 10 (32.2%) 21 (67.8%
)
Hi
stogram
9(
29%
)
22
(
71%
)
e
rence, a
d
rast
i
cc
h
ange o
f
t
h
e
fi
e
ld
o
f

v
i
ew e
li
m
i
nates any common re
f
erenc
e
b
etween scans, t
h
us automat
i
c matc
hi
ng
i
s
i
mposs
ibl
e
.
5.2 Map Merging Experiments
W
e cons
id
ere

d
t
h
ese
q
uence com
p
ose
d
o
f
29 scan
s
S
1
,
S
2
, ,
S
29
(
Ta
bl
e 3).
T
h
e
i
ntegrat

i
on o
f
t
hi
s sequence o
f
part
i
a
l
maps
h
as
b
een
d
one o
ff
-
li
ne to tes
t
an
d
compare t
h
et
h
ree met

h
o
d
s. In a
ll
t
h
et
h
ree met
h
o
d
s, pro
bl
ems arose w
h
e
n
i
ntegrat
i
ng t
h
esu
b
-sequence
f
rom
S

2
5
to
S
2
7
w
hi
c
h
represents t
h
e
h
a
ll
(F
i
g. 4)
.
H
ere,
d
ue to a
d
rast
i
c rotat
i
on (a

b
out 100
d
egrees) o
f
t
h
ero
b
ot
i
n suc
h
an ope
n
an
dl
arge env
i
ronment, t
h
e part
i
a
l
maps
h
ave on
l
y one or two segments

in
c
ommon. In or
d
er to c
l
ose t
h
e
l
oop an
d
comp
l
ete t
h
e exper
i
ments t
h
ese part
i
a
l
m
aps were manua
ll
y
i
ntegrate

d
toget
h
er
i
na
ll
t
h
et
h
ree met
h
o
d
s
.
T
able
3
.
E
xper
i
menta
l
sequence o
f
part
i

a
l
maps (t
h
e segment
l
engt
h
s are
i
nm
m
)
Env
i
ronment Part
i
a
l
maps Avg # o
f
segments Avg
l
engt
h
o
f
segment
s
La

b
oratory
S
1
−
S
7
,
S
28
−
S
29
38
.
1
2
59
.
3
H
a
ll
wa
y
S
8
−
S
22

1
9
.
3
366
.
3
H
a
ll
S
23
−
S
2
7
1
5
.
6
607
Tota
l
S
1
−
S
29
2
4.

53
3
74.
5
Fig.
5
shows the final map (composed of 278 segments) obtained with th
e
sequent
i
a
l
met
h
o
d
.T
h
e sequent
i
a
l
met
h
o
d
cou
ld
not
i

ntegrate a
ll
t
h
e part
i
a
l
m
aps
i
nor
d
er to c
l
ose t
h
e
l
oop: t
h
e met
h
o
d
su
dd
en
l
y

f
a
il
e
d
w
h
en we tr
i
e
d
t
o
i
ntegrat
e
S
21
,w
hi
c
hh
as on
l
ya
f
ew s
h
ort segments
i

n common w
i
t
h
t
h
e rest o
f
t
h
ema
p.
Fig. 6 shows the final map (composed of
5
19 segments) obtained with th
e
tree met
h
o
d
. We app
li
e
d
t
h
e stan
d
ar
d

tree met
h
o
d
unt
il l
eve
l
3o
f
t
h
e tree
,
t
h
en we app
li
e
d
t
h
e
h
eur
i
st
i
c presente
di

n Sect
i
on 4 to spee
d
up t
h
e process
.
A
s we went
d
own
i
nt
h
e tree, t
h
es
i
ze o
f
t
h
e maps grew
l
arger an
dl
arger an
d
t

h
e execut
i
on o
f
MAT
CH
s
l
owe
dd
own. For examp
l
e, t
h
e
i
ntegrat
i
on o
f
tw
o
p
art
i
a
l
maps (compose
d

o
f
108 an
d
103 segments) at
l
eve
l
3o
f
t
h
e tree requ
i
re
s
142
Ami
g
oni, et al
.
1m
F
igure
5.
Th
e
fi
na
l

map o
b
ta
i
ne
d
w
ith the sequential method
.

c
2
004 b
y
I
EEE (Amigoni et al., 2004
)
1m
F
igure
6.
Th
e
fi
na
l
map o
b
ta
i

ne
d
w
ith the tree method
.

c
2
004 b
y
I
EEE (Amigoni et al., 2004)
1m
D
oor that has been closed a
f
ter
t
he passa
g
eo
f
the robot
F
igure
7.
Th
e
fi
na

l
map o
b
ta
i
ne
d
w
i
t
h
t
he pivot method by fusin
g
S
i
−
1
,
i
wi
t
h
S
¯
t
i
−
1
,

i
i
,
i
+
1
.

c
2
004
b
y IEEE (Am
i
gon
i
et a
l
.
,
2
004
)
1
m
F
igure
8.
Th
e

fi
na
l
map o
b
ta
i
ne
d
w
i
t
h
t
he pivot method by fusin
g
S
i
−
1
,
i
wi
t
h
S
¯
t
i
−

1
,
i
+
1
i
+
1
.

c
2
004
b
y IEEE (Am
i
gon
i
et a
l
.
,
2
004
)
12
.
8
s. Furt
h

ermore, as a
l
rea
d
y note
d
,w
h
en we
i
ntegrate
l
arge-s
i
ze
d
map
s
w
i
t
h
many re
d
un
d
ant spur
i
ous segments t
h

at represent t
h
e same part o
f
t
he
e
nv
i
ronment, t
h
e resu
l
t
i
ng maps are more no
i
sy
b
ecause o
f
t
h
e error
i
ntro
d
uce
d
w

h
en attempt
i
ng to
i
ntegrate maps w
i
t
h
many over
l
app
i
ng segments
.
M
er
g
in
g
Partial Maps without Usin
g
Odometr
y
1
4
3
F
i
g. 7 s

h
ows t
h
e
fi
na
l
map, compose
d
o
f
441 segments, o
b
ta
i
ne
d
w
i
t
h
t
he
pi
vot met
h
o
db
y
f

us
i
ng t
h
e part
i
a
l
map
S
i
−
1
,
i
wi
t
h
S
¯
t
i
−
1
,i
i
,
i
+
1

.T
h
e map
i
nF
i
g. 8
is
c
omposed of 3
5
8 segments and has been built by fusing the partial ma
p
S
i
−
1
,
i
wi
t
h
S
¯
t
i
−
1
,i
+

1
i
+
1
.T
hi
s map presents
f
ewer spur
i
ous segments an
d
appears more
“
c
l
ean”
.
6
. Conclusions
In t
hi
s paper we
h
ave presente
d
met
h
o
d

s
f
or matc
hi
ng pa
i
rs o
f
scans com
-
p
ose
d
o
f
segments an
df
or merg
i
ng a sequence o
f
part
i
a
l
maps
i
nor
d
er to

b
u
ild
ag
l
o
b
a
l
map. In
f
uture researc
h
we a
i
m at genera
li
z
i
ng t
h
ese met
h
o
d
s to case
s
w
h
ere t

h
eor
d
er
i
nw
hi
c
h
t
h
e part
i
a
l
maps
h
ave to
b
e
i
ntegrate
di
s not
k
nown
.
T
h
ese genera

li
ze
d
met
h
o
d
sw
ill
prov
id
eane
l
egant so
l
ut
i
on to t
h
e pro
bl
em o
f
m
u
l
t
i
ro
b

ot mapp
i
ng s
i
nce t
h
ey w
ill
wor
k
w
h
en part
i
a
l
maps are acqu
i
re
db
y
a
s
i
ng
l
ero
b
ot at
diff

erent t
i
mes as we
ll
as w
h
en acqu
i
re
db
y
diff
erent ro
b
ots
in
diff
erent
l
ocat
i
ons
.
A
cknowledgment
s
T
h
e aut
h

ors wou
ld lik
etot
h
an
k
Jean-C
l
au
d
e Latom
b
e
f
or
hi
s generous
h
os
-
pi
ta
li
ty at Stan
f
or
d
Un
i
vers

i
ty w
h
ere t
hi
s researc
h
was starte
d
, Héctor Gonzá
l
es
-
Baños
f
or s
h
ar
i
ng
hi
s programs an
d
expert
i
se w
i
t
h
co

ll
ect
i
ng
l
aser range sca
n
d
ata, Pao
l
o Mazzon
i
an
d
Emanue
l
eZ
i
g
li
o
li f
or t
h
e
i
n
i
t
i

a
li
mp
l
ementat
i
on o
f
t
h
e
f
us
i
on a
l
gor
i
t
h
m
.
R
eferences
Am
i
gon
i
, F., Gaspar
i

n
i
, S., an
d
G
i
n
i
, M. (2004). Scan matc
hi
ng w
i
t
h
out o
d
ometry
i
n
f
ormat
i
on
.
In
P
roc. of the IEEE Int’l Conference on Robotics and Automatio
n
, pages 3753–3758
.

B
urgard, W., Moors, M., and Schneider, F. (2002). Collaborative exploration of unknown en
-
v
ironments
w
ith teams of mobile robots. I
n
Advances in Plan-Based Control of Robotic
A
gents, pages 52–70. Springer-Verlag
.
F
enwick, J. W., Newman, P. M., and Leonard, J. J. (2002). Cooperative concurrent mappin
g
a
nd localization. I
n
P
roc. of the IEEE Int’l Conference on Robotics and Automatio
n
,
pages
1810–1817
.
G
onzáles-Baños, H. H. and Latombe, J. C. (2002). Navigation strategies for exploring indoo
r
e
nvironments.

I
nt’l Journal of Robotics Research,21
(
10-11
)
:829–848
.
G
rimson, W. E. L.
(
1990
)
.
O
bject recognition by computer: the role of geometric constraint
s
.
T
he MIT Press.
K
o, J., Stewart, B., Fox, D., and Konolige, K. (2003). A practical, decision-theoretic approac
h
t
o multi-robot mappin
g
and exploration. I
n
Proc. o
f
the IEEE/RSJ Int’l Con

f
erence on Intel
-
l
i
g
ent Robots and S
y
stem
s
,
pa
g
es 3232–3238
.
K
onoli
g
e, K., Fox, D., Limketkai, B., Ko, J., and Stewart, B. (2003). Map mer
g
in
g
for distrib
-
u
ted robot navi
g
ation. I
n
P

roc. o
f
the IEEE/RSJ Int’l Con
f
erence on Intelligent Robots an
d
Sy
stem
s
.
144
Ami
g
oni, et al
.
L
u, F. and Milios, E. (1997). Robot pose estimation in unknown environments by matching 2
D
r
ange scans.
J
ournal of Intelligent and Robotic System
s
, 18(3):249–275.
M
artignoni III, A. and Smart, W. (2002). Localizing while mapping: A segment approach. I
n
P
roc. of the Eighteen National Conference on Artificial Intelligenc
e

, pages 959–960
.
S
immons, R. G., Apfelbaum, D., Burgard, W., Fox, D., Moors, M., Thrun, S., and Younes
,
H
. (2000). Coordination for multi-robot exploration and mapping. I
n
P
roc. of the National
Conference on Artificial Intelligenc
e
, pages 852–858
.
T
hrun, S., Burgard, W., and Fox, D. (2000). A real-time algorithm for mobile robot mappin
g
w
ith applications to multi-robot and 3D mapping. I
n
Proc. of the IEEE Int’l Conference on
R
obotics and Automatio
n
,
pages 321–328
.
W
eiss, G., Wetzler, C., and Puttkamer, E. V. (1994). Keeping track of position and orientation o
f

m
oving indoor systems by correlation of range-finder scans. I
n
P
roc. o
f
the IEEE/RSJ Int’l
C
on
f
erence on Intelligent Robots and System
s
,pa
g
es 12–16
.
Z
han
g
, L. and Ghosh, B. (2000). Line se
g
ment based map buildin
g
and localization usin
g
2
D
l
aser ran
g

efinder. I
n
Proc. o
f
the IEEE Int’l Con
f
erence on Robotics and Automatio
n
, pages
2
538–2543
.
D
ISTRIBUTED COVERAGE OF
U
NKN
O
WN
/U
N
S
TR
UC
T
U
RED
ENVIR
O
NMENT
S

BY
M
OBILE SENSOR NETWORKS
Ioann
i
sRe
kl
e
i
t
is
Current
l
yatt
h
e Cana
d
ian Space Agency, Cana
da
∗
yiannis
@
cim.mc
g
ill.ca
Ai
Peng New
D
SO Nationa
l

La
b
oratories, Singapor
e
naipen
g@
dso.or
g
.s
g
H
o
wi
e
Ch
oset
M
ec
h
anica
l
Engineering Department, Carnegie Me
ll
on University, US
A
choset
@
cmu.ed
u
Abs

tr
act
I
nt
hi
s paper we present an a
l
gor
i
t
h
m
i
cso
l
ut
i
on
f
or t
h
e
di
str
ib
ute
d
, comp
l
et

e
c
overage, path planning problem. Real world applications such as lawn mow
-
i
ng, chemical spill clean-up, and humanitarian de-mining can be automated b
y
t
he employment of a team of autonomous mobile robots. Our approach builds o
n
a single robot coverage algorithm. A greedy auction algorithm (a market base
d
m
echanism) is used for task reallocation among the robots. The robots are ini
-
t
ially distributed through space and each robot is allocated a virtually bounde
d
area to cover. Communication between the robots is available without any re
-
strictions.
K
eywords:
M
ulti-Robot coverage, Automated De-mining, Market-based approach, Mors
e
d
ecompositio
n
∗

W
or
kd
one w
hil
e at Carne
gi
eMe
ll
on Un
i
vers
i
t
y.
14
5
L
.E. Parker et al.
(
eds.)
,
M
ulti-Robot Systems. From Swarms to Intelligent Automata. Volume III
,
1
4
5
–1
55

.

c
200
5
S
pringer. Printed in the Netherlands
.
1
4
6
Rekleitis
,
et al
.
1. Introduction
T
h
e tas
k
o
f
cover
i
ng an un
k
nown env
i
ronment, common
i

n many app
li
ca
-
t
i
ons,
i
so
fhi
g
hi
nterest
i
n a num
b
er o
fi
n
d
ustr
i
es. Among t
h
em are manu
f
ac
-
turers o
f

automate
d
vacuum/carpet c
l
ean
i
ng mac
hi
nes an
dl
awn mowers, emer
-
g
ency response teams suc
h
as c
h
em
i
ca
l
or ra
di
oact
i
ve sp
ill d
etect
i
on an

d
c
l
ean
-
u
p, an
dh
uman
i
tar
i
an
d
e-m
i
n
i
ng. In a
ddi
t
i
on,
i
nterest
i
ng t
h
eoret
i

ca
l
pro
bl
em
s
h
ave emerge
d
espec
i
a
ll
y
i
nt
h
e areas o
f
pat
h
p
l
ann
i
ng, tas
k
(re)a
ll
ocat

i
on an
d
mu
l
t
i
-ro
b
ot cooperat
i
on
.
T
h
e goa
l
o
f
comp
l
ete coverage
i
stop
l
an a pat
h
t
h
at wou

ld
gu
id
earo
b
o
t
to pass an en
d
-e
ff
ector (
i
n our case equ
i
va
l
ent to t
h
e
f
ootpr
i
nt o
f
t
h
ero
b
ot

)
o
ver every access
ibl
e area o
f
t
h
e targete
d
env
i
ronment. In t
h
es
i
ng
l
ero
b
o
t
c
ase, prev
i
ous wor
kh
as pro
d
uce

d
a
l
gor
i
t
h
ms t
h
at guarantee comp
l
ete coverag
e
of
an un
k
nown ar
bi
trar
y
env
i
ronment. Intro
d
uc
i
n
g
mu
l

t
i
p
l
ero
b
ots prov
id
e
s
ad
vanta
g
es
i
n terms o
f
e
ffi
c
i
enc
y
an
d
ro
b
ustness
b
ut

i
ncreases t
h
ea
lg
or
i
t
h
m
ic
c
omp
l
ex
i
t
y
.
C
entra
li
nt
h
emu
l
t
i
-ro
b

ot approac
hi
st
h
e
i
ssue o
f
commun
i
cat
i
on. W
h
e
n
c
ommunication is restricted to close proximit
y
(Latimer-IV et al., 2002) or lin
e
o
fsi
g
ht (Rekleitis et al., 2004) the robots have to remain to
g
ether in order t
o
a
void coverin

g
the same area multiple times. When unrestricted communi
-
c
ation is available then the robots can disperse throu
g
h the environment an
d
p
roceed to cover different areas in parallel, constantl
y
updatin
g
each other o
n
their pro
g
ress. The challen
g
e in this case is to allocate re
g
ions to each robo
t
such that no robot sta
y
s idle (thus all finish coverin
g
around the same time) an
d
a

lso to reduce the amount of time spent commutin
g
amon
g
the different re
g
ion
s
instead of coverin
g
. Providin
g
an optimal solution for minimizin
g
travel tim
e
is an NP-hard problem as it can be mapped into a multiple travelin
g
salesma
n
p
roblem. An auction mechanism is used in order to re-allocate re
g
ions to b
e
c
overed between robots in such a wa
y
that the path traveled between re
g

ion
s
is reduced. The auction mechanism is a
g
reed
y
heuristic based on the
g
enera
l
market based a
pp
roach
.
o
stripe
Deployment
vehicle
py
y
(not
i
n sca
l
e
)
Robots
F
igure
1

Al
arge un
k
nown
a
rea is divided up in vertica
l
stripes. Each covering robot i
s
a
ssigned a stripe to cover. A
d
eployment vehicle is utilize
d
t
hat distributes the robots at
t
he beginning of the stripes
.
T
he robots do not know th
e
l
ayout at the interior of eac
h
stripe.
M
ulti-Robot Distributed Covera
ge
1

4
7
W
e assume t
h
at t
h
ero
b
ots
k
now t
h
e
i
r
p
os
i
t
i
on an
d
or
i
entat
i
on w
i
t

h
res
p
ec
t
to a g
l
o
b
a
l
re
f
erence
f
rame (e.g. v
i
a access to a GPS system). T
h
ero
b
ot sensor
s
are a
bl
eto
d
etect
b
ot

h
stat
i
co
b
stac
l
es an
d
mo
bil
ero
b
ots
,
an
d diff
erent
i
at
e
b
etween t
h
etwo.T
h
e sensors
h
ave
li

m
i
te
d
range an
d
a goo
d
angu
l
ar reso
l
ut
i
on
.
T
h
ewor
ki
ng para
di
gm
i
n our approac
hi
st
h
e app
li

cat
i
on o
fh
uman
i
tar
i
a
n
d
e-m
i
n
i
ng. A team o
f
ro
b
ots
i
s
d
ep
l
oye
d
a
l
ong one s

id
eo
f
a
fi
e
ld
to
b
ec
l
eare
d,
at regu
l
ar
i
nterva
l
s (as
i
nF
i
g. 1). T
h
e
i
nter
i
or o

f
t
h
e
fi
e
ld i
sun
k
nown, part
i
a
lly
c
overe
d
w
i
t
h
o
b
stac
l
es, an
ddi
v
id
e
di

nto a num
b
er o
f
v
i
rtua
l
str
i
pes equa
l
t
o
t
h
e num
b
er o
f
ro
b
ots. Eac
h
ro
b
ot
i
sa
ll

ocate
di
n
i
t
i
a
ll
yt
h
e respons
ibili
ty o
f
t
he
str
i
pe
i
t
i
sp
l
ace
d
at, an
d
t
h

e coverage starts
.
In t
h
e next sect
i
on we present re
l
evant
b
ac
k
groun
d
on t
h
e Coverage tas
k
an
d
on t
h
e mar
k
et
b
ase
d
approac
h

. Sect
i
on 3 prov
id
es an overv
i
ew o
f
ou
r
a
lg
or
i
t
h
man
d
t
h
e next Sect
i
on presents our exper
i
menta
l
resu
l
ts
i

nmu
l
t
i
p
le
simulated environments. Finall
y
, Section
5
provides conclusions and futur
e
wor
k
.
2. Related
W
ork
T
hi
swor
k
emp
l
oys a s
i
ng
l
ero
b

ot coverage a
l
gor
i
t
h
m
f
or eac
hi
n
di
v
id
ua
l
r
o
b
ot an
d
an auct
i
on mec
h
an
i
sm to negot
i
ate among ro

b
ots w
hi
c
h
areas eac
h
r
o
b
ot wou
ld
cover. Due to space
li
m
i
tat
i
ons we w
ill b
r
i
e
fl
y out
li
ne t
h
ema
j

o
r
approac
h
es
i
nmu
l
t
i
-ro
b
ot coverage (
f
or a more
d
eta
il
e
d
survey p
l
ease re
f
e
r
to
(
Re
kl

e
i
t
i
seta
l
., 2004
))
an
d
t
h
en we w
ill di
scuss re
l
ate
d
wor
k
on mar
k
e
t
b
ase
d
mec
h
an

i
sms
i
nmo
bil
ero
b
ot
i
cs. F
i
na
ll
y, we present a
b
r
i
e
f
overv
i
e
w
of
re
l
evant term
i
no
l

ogy use
di
n coverage an
d
exact ce
ll
u
l
ar
d
ecompos
i
t
i
on
.
T
hi
swor
k
ta
k
es root
i
nt
h
e Boustrop
h
e
d

on
d
ecompos
i
t
i
on (C
h
oset an
d
P
i
gnon
,
1997), w
hi
c
hi
s an exact ce
ll
u
l
ar
d
ecompos
i
t
i
on w
h

ere eac
h
ce
ll
can
b
e covere
d
w
i
t
h
s
i
mp
l
e
b
ac
k
-an
d
-
f
ort
h
mot
i
ons.
D

eterm
i
n
i
st
i
c approac
h
es
h
ave
b
een use
d
to cover spec
i
a
li
ze
d
env
i
ronment
s
(
But
l
er et a
l
., 2001) somet

i
mes resu
l
t
i
n
gi
n repeat covera
g
e (Lat
i
mer-IV et a
l
.
,
2
002, Kuraba
y
ashi et al., 199
6
, Min and Yin, 1998). Non-deterministic ap
-
p
roac
h
es
i
nc
l
u

d
et
h
e use o
f
neura
l
networ
k
s (Luo an
d
Yan
g
, 2002), c
h
em
i
ca
l
traces (Wa
g
ner et a
l
., 1999), an
d
swarm
i
nte
llig
ence (Ic

hik
awa an
d
Hara, 1999
,
Bruemmer et al., 2002, Batalin and Sukhatme, 2002
)
. The non-deterministi
c
approaches can not
g
uarantee complete covera
g
e
.
2.1 Market-based A
pp
roach in Robotics
Cooperat
i
on an
d
tas
k
a
ll
ocat
i
on among mo
bil

ero
b
ots
i
s cruc
i
a
li
nmu
l
t
i-
r
o
b
ot app
li
cat
i
ons. To
f
ac
ili
tate tas
k
re-a
ll
ocat
i
on a new met

h
o
d
o
l
ogy
b
ase
d
o
n mar
k
et economy
h
as ga
i
ne
d
popu
l
ar
i
ty. For a compre
h
ens
i
ve survey p
l
eas
e

r
e
f
er to (D
i
as an
d
Stentz, 2001). Current
l
y mar
k
et
b
ase
d
approac
h
es
h
av
e
b
een use
d
to so
l
ve t
h
emu
l

t
i
-ro
b
ot tas
k
a
ll
ocat
i
on pro
bl
em (Go
ldb
ergeta
l
.
,
1
4
8
Rekleitis
,
et al
.
2003)
i
nt
h
e

d
oma
i
ns o
f
:ex
pl
orat
i
on (Ber
h
au
l
teta
l
., 2003, D
i
as an
d
Stentz
,
2003),
f
a
il
ure/ma
lf
unct
i
on

d
etect
i
on an
d
recovery (D
i
as et a
l
., 2004), an
db
o
x
p
us
hi
ng (Ger
k
ey an
d
Matar
i
c, 2002).
2.2 Boustro
p
hedon/Morse Decom
p
osition
C
ell Boundary

Swee
p
D
i
rect
i
o
n
slice
e
C
ell
O
b
stac
le
F
igure
2
Illust
r
ates the te
rm
s
b
orrowed from single robot
c
overage with a single robo
t
a

nd one obstacle in the tar
-
g
et environment. The robo
t
i
s performing coverage with
simple up-and-down motions
.
To
b
etter
d
escr
ib
et
h
emu
l
t
i
-ro
b
ot coverage a
l
gor
i
t
h
m, we

b
orrow t
h
e
f
o
l-
l
ow
i
ng terms
f
rom s
i
ng
l
ero
b
ot coverage
:
sl
ice
,
c
e
ll
,
s
weep
d

irectio
n
,
an
d
cr
i
t-
i
ca
l
poin
t
(
see F
i
g. 2). A
sl
ice
i
sasu
b
sect
i
on o
fa
c
e
ll
c

overe
db
yas
i
ng
l
e,
i
n our case vert
i
ca
l,
mot
i
on. A ce
ll
i
s a reg
i
on
d
e
fi
ne
db
yt
h
e Boustrop
h
e

d
o
n
d
ecompos
i
t
i
on w
h
ere connect
i
v
i
ty
i
s constant. In our current wor
k
ace
ll is
f
urt
h
er constra
i
ne
db
yt
h
e

b
oun
d
ar
i
es o
f
t
h
e str
i
pe (t
h
e space a
ll
ocate
d
to
a
r
o
b
ot
)
. Swee
pd
irectio
n
re
f

ers to t
h
e
di
rect
i
on t
h
es
li
ce
i
s swept. Last
l
y,
a
cr
i
t-
ica
lp
oin
t
r
epresents a po
i
nt on an o
b
stac
l

ew
hi
c
h
causes a c
h
ange
i
nt
h
ece
ll
c
onnect
i
v
i
ty. T
h
ecr
i
t
i
ca
l
po
i
nts
h
ave

b
een
d
escr
ib
e
di
n
l
engt
hi
n (Acar an
d
Ch
oset, 2000) (see F
i
g. 3a
f
or an overv
i
ew). We a
l
so
b
orrow t
h
e concept o
f
a
Ree

b
grap
h
, a grap
h
representat
i
on o
f
t
h
e target env
i
ronment w
h
ere t
h
eno
d
e
s
a
re t
h
ecr
i
t
i
ca
l

po
i
nts an
d
t
h
ee
d
ges are t
h
ece
ll
s(F
i
g. 3
b
)
.
3
. Algorithm Overview
O
ur approac
h
cons
i
sts o
f
two
b
e

h
av
i
ours, exp
l
orat
i
on an
d
coverage. T
he
r
o
b
ots
i
n
i
t
i
a
ll
y try to trace t
h
e out
li
ne o
f
t
h

e areas ass
i
gne
d
to t
h
em
i
nor
d
er t
o
b
e more
k
now
l
e
d
gea
bl
ea
b
out t
h
e genera
ll
ayout o
f
t

h
e
f
ree space. T
h
e con
-
nect
i
v
i
ty o
f
t
h
e
f
ree space
i
s recor
d
e
di
n a grap
h
t
h
at cons
i
sts o

f
t
h
e Ree
b
grap
h
a
ugmente
d
w
i
t
h
extra no
d
es (terme
d
Ste
i
ner po
i
nts) p
l
ace
d
at t
h
e
b

oun
d
ar
i
e
s
of
t
h
e ass
i
gne
d
str
i
pes
f
or eac
h
ro
b
ot. T
h
ee
d
ges o
f
t
h
e grap

h
represent area
s
of
access
ibl
e unexp
l
ore
d
space an
d
eac
h
e
d
ge
b
e
l
ongs to a ro
b
ot. Dur
i
ng t
he
e
xp
l
orat

i
on p
h
ase t
h
ero
b
ots exc
h
ange
i
n
f
ormat
i
on an
dif
t
h
e str
i
pearo
b
ot
h
a
s
M
ulti-Robot Distributed Covera
ge

1
4
9
R
e
v
erse
C
on
v
ex
C
onca
v
e
For
w
ard
Sweep D
i
rect
i
on
(
a
)
E3
E
1
P1

P2
C1
C2
E2
C3
P
3
E
4
C4
P4
Cell Boundaries
(b)
b
F
igure
3.
(a) Dep
i
cts t
h
e
f
our types o
f
cr
i
t
i
ca

l
po
i
nts,
b
ase
d
on concav
i
ty an
d
t
h
e sur
f
ac
e
normal vector parallel to the sweep direction. Note that the shaded areas are obstacles and th
e
arrows represent the normal vectors. (b) Here a simple Reeb graph is overlaid on top of a simpl
e
elliptical world with one obstacle. P1-P4 are critical points which represent graph nodes. E1-E
4
r
epresent edges which directly map to cells C1-C4.
ass
i
gne
di
s not

f
u
ll
yexp
l
ore
d
,t
h
en, t
h
at ro
b
ot ca
ll
s an auct
i
on
f
or t
h
e tas
k
o
f
e
xp
l
or
i

ng t
h
e rema
i
n
i
ng area o
f
t
h
e str
i
pe
.
3
.1 Coo
p
erative Ex
p
loration
T
h
ero
b
ot uses t
h
ecyc
l
ea
l

gor
i
t
h
m
d
eve
l
ope
di
ns
i
ng
l
ero
b
ot Morse De
-
c
ompos
i
t
i
on
f
or exp
l
orat
i
on o

f
t
h
e str
i
pe
b
oun
d
ary. T
h
ecyc
l
e pat
hi
sas
i
mp
le
cl
ose
d
pat
h
,
i
.e.,
b
y execut
i

ng t
h
ecyc
l
ea
l
gor
i
t
h
mt
h
ero
b
ot a
l
ways comes
b
ac
k
to t
h
epo
i
nt w
h
ere
i
t
h

as starte
d
.T
hi
s same cyc
l
ea
l
gor
i
t
h
m
i
s use
df
or
b
ot
h
e
xp
l
orat
i
on an
d
coverage. Be
f
ore

d
escr
ibi
ng t
h
ecyc
l
ea
l
gor
i
t
h
m, we nee
d
t
o
d
e
fi
ne 2 terms:
l
app
i
ng an
d
wa
ll f
o
ll

ow
i
ng. Lapp
i
ng
i
st
h
e mot
i
on a
l
ong t
he
s
li
ces w
hil
ewa
ll f
o
ll
ow
i
ng
i
st
h
e mot
i

on a
l
ong o
b
stac
l
e
b
oun
d
ar
i
es. A s
i
mp
le
c
yc
l
ea
l
gor
i
t
h
m execut
i
on w
ill
cons

i
st o
ff
orwar
dl
app
i
ng,
f
orwar
d
wa
ll f
o
l-
l
ow
i
ng, reverse
l
app
i
ng an
d
reverse wa
ll f
o
ll
ow
i

ng (as s
h
own
i
nF
i
g. 4a). T
hi
s
i
ssu
ffi
c
i
ent
f
or exp
l
or
i
ng t
h
e str
i
pe
b
oun
d
ary
.

To exp
l
a
i
nt
h
e cooperat
i
ve exp
l
orat
i
on a
l
gor
i
t
h
m, we w
ill l
oo
k
at an exam
-
pl
e. F
ig
.4
b
s

h
owsanun
k
nown space w
i
t
h
as
i
n
gl
eo
b
stac
l
e,
b
e
i
n
gdi
v
id
e
d
i
nto
6
stripes. The Reeb
g

raph of each robot is initialized with 2 critical point
s
(
S
t
art an
d
E
n
d) and
5
Steiner points (representin
g
the stripe boundaries).
T
h
ero
b
ots access t
h
e
i
r respect
i
ve str
i
pes an
d
per
f

orm
i
n
i
t
i
a
l
exp
l
orat
i
on us
-
i
n
g
the c
y
cle al
g
orithm (forward lappin
g
, forward wall followin
g
, reverse lap-
p
in
g
and reverse wall followin

g
). Durin
g
exploration, the robots modif
y
thei
r
knowled
g
e of the environment b
y
updatin
g
the Reeb
g
raph as the
y
discove
r
c
ritical points and new information about the Steiner points. After completin
g
ac
y
cle, each robot shares its updated partial Reeb
g
raph with the rest of th
e
r
obots. At the end of the initial exploration, the updated

g
lobal Reeb
g
raph i
s
as shown in Fi
g
.4c
.
150
Rekleitis
,
et al
.
R
e
v
erse
Lappin
g
Lappin
g
For
w
ar
d
Forward wal
l
Fo
ll

ow
i
ng
R
everse wal
l
Fo
ll
ow
i
n
g
(
a
)
str
ip
e
b
oun
d
ar
i
es
critical
p
oint
ste
i
ner

p
o
i
n
t
S
E
Initial Au
g
mented Reeb Graph
(b)
b
1
2
3
4
5
6
1
2
6
str
ip
e
b
oun
d
ar
i
es

cr
i
t
i
ca
l

p
o
i
nt
ste
i
ner
p
o
i
n
t
E
A
u
g
mente
d
Ree
b
Grap
h
A

f
ter In
i
t
i
a
l
Exp
l
orat
i
on
S
(c)
1
2
3
4
5
6
1
2
6
3
5
4
stripe boundaries
critical point
steiner poin
t

E
S
F
i
na
l
Ree
b
Gra
ph
A
f
ter Ex
pl
orat
i
on
i
s Com
pl
ete
(d)
d
F
igure
4.
(a) A simple cycle path consisting of forward lapping, forward wall following,
(c)()
(d)(d)
r

everse lapping and reverse wall following. (b) Simple environment with initial Augmente
d
R
eeb Graph. (c) Initial exploration of stripes. (d) The final Reeb Graph after exploration i
s
c
omplete
.
In t
h
e
p
rocess o
f
ex
pl
orat
i
on, t
h
ero
b
ots w
ill
rea
li
ze t
h
at t
h

ere are s
p
ace
s
i
nt
h
e
i
r str
i
pe t
h
at t
h
ey are not a
bl
e to reac
h
eas
il
y. T
h
ose ro
b
ots t
h
at are
in
suc

h
as
i
tuat
i
on w
ill f
ormu
l
ate t
h
e unreac
h
a
bl
e port
i
ons o
f
t
h
e str
i
pe as auct
i
o
n
tas
k
san

d
ca
ll
auct
i
ons to re-a
ll
ocate t
h
ese parts o
f
t
h
e
i
r str
i
pe. In t
hi
s manner
,
c
ooperat
i
ve exp
l
orat
i
on
i

sac
hi
eve
d
.F
i
g. 4
d
s
h
ows t
h
e comp
l
ete
d
Ree
b
Grap
h
af
ter exp
l
orat
i
on
i
s comp
l
ete. Ro

b
ots t
h
at
d
o not
h
ave any exp
l
orat
i
on tas
ks
c
an start per
f
orm
i
ng part
i
a
l
coverage o
fk
nown str
i
pes
i
nor
d

er not to wast
e
t
i
me. Coverage o
f
ace
ll i
s cons
id
ere
d
an atom
i
c tas
k
,t
h
us a ro
b
ot t
h
at
h
a
s
starte
d
cover
i

ng a ce
ll
wou
ld fi
n
i
s
h
cover
i
ng
i
t
b
e
f
ore start
i
ng anot
h
er tas
k
.
T
h
eg
l
o
b
a

l
Ree
b
grap
hi
sup
d
ate
d
to represent t
h
e
i
ncrease
dk
now
l
e
d
ge o
f
t
he
e
nv
i
ronment
.
3
.2 Cooperative Coverage

Af
ter a
ll
t
h
e str
i
pe
b
oun
d
ar
i
es are comp
l
ete
l
yexp
l
ore
d
(
f
u
ll
y connecte
d
Ree
b
grap

h
w
i
t
h
out Ste
i
ner po
i
nts), t
h
ece
ll
s are owne
db
yt
h
ero
b
ot t
h
at
di
s
-
c
overe
d
t
h

em. T
h
eenv
i
ronment
i
s
f
u
ll
y represente
db
yt
h
e Ree
b
grap
h
,
h
ence
it
i
s
d
ecompose
di
nto a set o
f
connecte

d
ce
ll
s(t
h
eun
i
on o
f
a
ll
t
h
ece
ll
s represent
s
M
ulti-Robot Distributed Covera
ge
151
t
h
e
f
ree s
p
ace), an
d
a

ll f
ree s
p
ace
i
sa
ll
ocate
d
to t
h
ero
b
ots. Next t
h
ero
b
ot
s
p
rocee
d
to cover t
h
ece
ll
sun
d
er t
h

e
i
rc
h
arge. Coverage o
f
as
i
ng
l
ece
ll i
st
he
same as s
i
ng
l
ero
b
ot Morse Decompos
i
t
i
on;
if
t
h
ere are no o
b

stac
l
es w
i
t
hi
nt
he
c
e
ll
,t
h
e coverage
i
s a ser
i
es o
f
s
i
mp
l
ecyc
l
e pat
h
s. I
f
t

h
ere are o
b
stac
l
es w
i
t
hin
t
h
ece
ll
,t
h
ero
b
ot per
f
orms
i
ncrementa
l
mo
difi
cat
i
on o
f
t

h
e Ree
b
grap
h
w
i
t
hin
t
h
at ce
ll
an
d
s
h
ares t
h
e
i
n
f
ormat
i
on w
i
t
h
t

h
eot
h
er ro
b
ots. I
f
t
h
ere
i
saro
b
o
t
t
h
at
i
sw
i
t
h
out a tas
ki
tca
ll
s an auct
i
on to o

ff
er
i
ts serv
i
ce to ot
h
er ro
b
ots. I
f
a
ll
ro
b
ots
h
ave comp
l
ete
d
t
h
e
i
rce
ll
coverage an
d
t

h
ere are no uncovere
d
ce
lls
i
nt
h
e Ree
b
grap
h
,t
h
en t
h
ero
b
ots return to t
h
e
i
r start
i
ng pos
i
t
i
ons an
dd

ec
l
ar
e
t
h
eenv
i
ronment covere
d.
3
.3 Auctioning Task
s
A
s
i
mp
l
e auct
i
on mec
h
an
i
sm
i
s use
d
to
i

nvest
i
gate t
h
e
f
eas
ibili
ty o
f
auct
i
o
n
to ena
bl
e cooperat
i
on among ro
b
ots. At any auct
i
on a s
i
ng
l
e tas
ki
s auct
i

one
d
o
ut. In genera
l
,t
h
e auct
i
on mec
h
an
i
sm operates as
f
o
ll
ows: (a) A ro
b
ot
di
s
-
c
overs a new tas
k
an
d
ca
ll

s an auct
i
on w
i
t
h
an
i
n
i
t
i
a
l
est
i
mate
d
cost.
(b)
Ot
h
e
r
r
o
b
ots t
h
at are

f
ree to per
f
orm t
h
e tas
k
at a
l
ower est
i
mate
d
cost,
bid f
or t
h
e
tas
k
.
(
c
)
W
h
en t
h
e auct
i

on t
i
me en
d
s, t
h
e auct
i
oneer se
l
ects t
h
ero
b
ot w
i
t
h
t
he
l
owest
bid
an
d
ass
i
gns t
h
e tas

k
.T
h
ew
i
nn
i
ng ro
b
ot a
dd
st
h
e tas
ki
nto
i
ts tas
kli
st
an
d
con
fi
rms t
h
at
i
t accepts t
h

e tas
kb
y sen
di
ng an accept-tas
k
message
b
ac
k
to t
h
e auct
i
oneer
.
T
h
e auct
i
oneer
d
e
l
etes t
h
e auct
i
on tas
k

an
d
t
h
e tas
k
auct
i
o
n
p
rocess conc
l
u
d
es. As state
di
nt
h
e prev
i
ous sect
i
ons, auct
i
on
i
s use
di
ntw

o
separate ways:
f
or cooperat
i
ve exp
l
orat
i
on, an
df
or cooperat
i
ve coverage
.
D
ur
i
ng exp
l
orat
i
on, a ro
b
ot can encounter a s
i
tuat
i
on w
h

ere t
h
e str
i
pe
i
t
is
e
xp
l
or
i
n
gi
s
di
v
id
e
di
nto two (or more)
di
sconnecte
d
parts (see
f
or examp
le
the middle stripe in Fi

g
.
5
a) because of an obstacle. The robot starts with for
-
war
dl
app
i
n
g
, encounters t
h
eo
b
stac
l
ean
d
per
f
orms wa
ll f
o
ll
ow
i
n
g
.T

h
ewa
ll
f
o
ll
ow
i
n
gb
e
h
av
i
our
b
r
i
n
g
s
i
ttot
h
e str
i
pe
b
oun
d

ar
y
assoc
i
ate
d
w
i
t
h
revers
e
l
appin
g
. As a result, the robot infers that there exists a disconnected stripe. A
t
this point, it will formulate a new stripe to be explored and calls an auction fo
r
this new exploration task. Please note that the robots
g
enerall
y
do not have suf
-
ficient information to know accuratel
y
the cost of performin
g
the exploratio

n
task. It can onl
y
estimate the cost based on whatever information is available
.
Cost is the onl
y
parameter that decides the winnin
g
robot in an auction and i
t
i
s thus the factor that determines the qualit
y
of cooperation. The estimation o
f
the cost can be potentiall
y
a complex function of man
y
variables (such as tim
e
s
p
ent, fuel ex
p
ended,
p
riorities of the task, ca
p

abilities of the robot). For thi
s
i
nvesti
g
ation, the task cost for the bidder is estimated based on 2 components
:
(
a) Access cost: Based on the bidder’s current estimated end
p
oint (the
p
oin
t
where its currentl
y
executin
g
atomic task will end), this is the shortest Manhat
-
tan distance to access the new stripe; (b) Exploration cost: Assuming that th
e
15
2 Rekleitis
,
et al
.
r
o
b

ot can access t
h
e
d
es
i
re
dp
o
i
nt
i
nt
h
e str
ip
e, t
hi
s
i
st
h
em
i
n
i
mum
di
stanc
e

t
h
at
i
t nee
d
s to trave
li
nor
d
er to exp
l
ore t
h
e str
i
pe comp
l
ete
l
y (as parts o
f
t
he
str
i
pe cou
ld
a
l

rea
d
y
h
ave
b
een exp
l
ore
d
,t
h
e start
i
ng po
i
nt o
f
t
h
eexp
l
orat
i
o
n
c
ou
ld
resu

l
t
i
n
diff
erent costs
f
or
diff
erent ro
b
ots
)
.
Wh
en an
i
n
i
t
i
a
l
est
i
mate o
f
t
h
ece

ll
s
i
sava
il
a
bl
e(exp
l
orat
i
on
i
s comp
l
ete
)
t
h
ero
b
ot t
h
at
h
as
di
scovere
d
ace

ll i
s
i
n
i
t
i
a
ll
y respons
ibl
e
f
or cover
i
ng
i
t. T
he
r
o
b
ot w
i
t
h
out any tas
k
sw
ill

o
ff
er
i
ts serv
i
ce
b
ya
l
so ca
lli
ng an auct
i
on. An
y
r
o
b
ot t
h
at
h
as extra ce
ll
s(
l
ess t
h
ece

ll
t
h
at
i
t
i
s current
l
y cover
i
ng) w
ill
o
ff
e
r
o
ne o
f
t
h
ece
ll
s,
b
ase
d
on t
h

e auct
i
oneer’s pos
i
t
i
on. Eac
h
ro
b
ot w
i
t
h
out extr
a
c
e
ll
sw
ill
est
i
mate t
h
e current ce
ll
wor
kl
oa

d
an
d
o
ff
er to s
h
are
i
ts ce
ll
coverag
e
tas
kifi
t
i
s greater t
h
anat
h
res
h
o
ld
.T
h
e auct
i
oneer pre

f
ers to ta
k
eover a ce
ll
r
at
h
er t
h
an to s
h
are coverage o
f
ace
ll
.Itw
ill
use t
h
e est
i
mate
ddi
stance t
o
a
ccess t
h
ece

ll
asase
l
ect
i
on cr
i
ter
i
a
if
t
h
ere are more t
h
an one ce
ll
on o
ff
er
.
4. Ex
p
erimental Result
s
T
h
e
di
str

ib
ute
d
coverage a
l
gor
i
t
h
mwas
i
mp
l
emente
di
ns
i
mu
l
at
i
on us
i
n
g
P
l
ayer an
d
Stage (Ger

k
ey et a
l
., 2001) w
i
t
h
3ro
b
ots. We a
d
opte
d
a
hi
g
hl
y
di
s
-
tr
ib
ute
d
system arc
hi
tecture
b
ecause

i
t can qu
i
c
kl
y respon
d
to pro
bl
ems
i
nvo
l
v
-
i
ng one (or a
f
ew) ro
b
ots, an
di
s more ro
b
ust to po
i
nt
f
a
il

ures an
d
t
h
ec
h
ang
i
n
g
d
ynam
i
cs o
f
t
h
e system. Our arc
hi
tecture
i
s
b
ase
d
on t
h
e
l
ayere

d
approac
h
t
h
at
h
as
b
een use
df
or many s
i
ng
l
e-agent autonomous systems (Sc
h
rec
k
eng
-
h
ost et a
l
., 1998, Wagner et a
l
., 2001). We are emp
l
oy
i

ng two
l
ayers
f
or eac
h
r
o
b
ot
i
nstea
d
o
f
t
h
e tra
di
t
i
ona
l
t
h
ree
l
ayers: P
l
ann

i
ng an
d
Be
h
av
i
our. T
he
up
-
p
e
r
l
ayer cons
i
sts o
f
Pl
anne
r
a
n
d
Mo
d
e
l
a

n
d
t
h
e
l
ower
l
ayer
is
B
e
h
a
v
iou
r
.
Mo
d
e
li
sw
h
ere t
h
e Ree
b
grap
h

res
id
es. P
l
anner
i
sw
h
ere Morse Decompos
i
-
t
i
on, auct
i
on mec
h
an
i
sm, tas
k
sc
h
e
d
u
li
ng an
d
tas

k
mon
i
tor
i
ng ta
k
ep
l
ace. T
he
Be
h
av
i
our process serves t
h
e same
f
unct
i
on as
i
n tra
di
t
i
ona
ll
a

y
ere
d
arc
hi
tec
-
ture, contro
lli
n
g
t
h
ero
b
ots to per
f
orm atom
i
c tas
k
s suc
h
as Goto, Fo
ll
ow Wa
ll
an
d
Lapp

i
n
g.
A
sample environment for testin
g
the al
g
orithm is shown in Fi
g
.
5
a. Eac
h
r
obot is allocated a stripe and the Planner of each robot receives the stripe in
-
f
ormation. The Planner determines the point where it wants to access the strip
e
and sends the wa
y
-point to the Behaviour process for execution. After access
-
i
n
g
the stripe, the Behaviour process sends a messa
g
e to the Planner informin

g
the Planner that access of the stri
p
e is com
p
leted. Based on the stri
p
e infor
-
m
ation and the robot pose, the Planner plans for Forward Lappin
g
and send
s
this task to the Behaviour. The Behaviour executes the forward lappin
g
task
.
For this task, the 3 robots experience different terminatin
g
conditions becaus
e
o
f the environment: The left and the ri
g
ht robots complete the exploration o
f
their stripes without an
y
problems. The middle robot realizes that it can no

t
c
om
p
lete the ex
p
loration of its stri
p
e and calls an auction. The robot on th
e
M
ulti-Robot Distributed Covera
ge
153
(
a
)
(b)
b
(
c
)
F
igure
5.
(
a) T
h
eenv
i

ronment an
d
t
h
et
h
ree ro
b
ots at t
h
e start
i
ng pos
i
t
i
on
i
n Stage. (
b
)T
he
traces of the robots (marked as circles which are smaller than the footprint) and the critica
l
p
oints encountered. (c) The augmented Reeb graph with the critical points (circles) and th
e
Steiner points (crosses)
.
ri

g
h
tw
i
ns t
h
e auct
i
on an
d
procee
d
stoexp
l
ore t
h
e rema
i
n
i
ng part o
f
t
h
em
id-
dl
e str
i
pe. In t

h
e mean t
i
me t
h
e
l
e
f
tan
d
m
iddl
ero
b
ots start part
i
a
l
coverage
.
F
i
na
ll
yw
h
en exp
l
orat

i
on
i
s comp
l
ete t
h
ero
b
ots exc
h
ange ce
ll
sv
i
a auct
i
on an
d
c
ompletely cover the environment. Fig.
5
c shows the Reeb graph after ex
-
p
loration is completed. Fig.
5
b shows the trace of the three robots plotted a
s
ci

rc
l
es (t
h
e trace
i
s sma
ll
er t
h
an t
h
ero
b
ot
f
ootpr
i
nt
f
or
ill
ustrat
i
on purposes).
D
ur
i
ng our exper
i

ments t
h
ero
b
ots cont
i
nuous
l
yexp
l
ore
d
an
d
covere
d
t
he
e
nv
i
ronment. A
f
ter a
f
ew auct
i
ons
i
twas

i
mposs
ibl
e to pre
di
ct w
hi
c
h
tas
k
wa
s
sc
h
e
d
u
l
e
d
next
b
y eac
h
ro
b
ot. It
i
s wort

h
not
i
ng t
h
oug
h
t
h
at t
h
e
di
stance trav
-
el
e
db
y eac
h
ro
b
ot was approx
i
mate
l
yt
h
e same t
h

us s
h
ow
i
ng t
h
at t
h
ewor
kl
oa
d
was
di
str
ib
ute
d
even
l
y
.
5.
S
ummar
y
In t
hi
s paper we presente
d

an a
l
gor
i
t
h
m
i
c approac
h
to t
h
e
di
str
ib
ute
d
, com
-
pl
ete coverage, pat
h
p
l
ann
i
ng pro
bl
em. Un

d
er t
h
e assumpt
i
on o
f
g
l
o
b
a
l
com
-
m
un
i
cat
i
on among t
h
ero
b
ots, eac
h
ro
b
ot
i

sa
ll
ocate
d
an area o
f
t
h
eun
k
now
n
e
nv
i
ronment to cover. An auct
i
on mec
h
an
i
sm
i
s emp
l
oye
di
nor
d
er to

f
ac
ili
tat
e
c
ooperat
i
ve
b
e
h
av
i
our among t
h
ero
b
ots an
d
t
h
us
i
mprove t
h
e
i
r per
f

ormance
.
In our approac
h
no ro
b
ot rema
i
ns
idl
ew
hil
et
h
ere are areas to
b
e covere
d.
For
f
uture wor
k
,wewou
ld lik
e to compare t
h
e per
f
ormance
b

etween t
he
di
str
ib
ute
d
approac
hd
escr
ib
e
dh
ere w
i
t
h
t
h
e
f
ormat
i
on-
b
ase
d
approac
h
w

i
t
h
li
m
i
te
d
commun
i
cat
i
on presente
di
n (Re
kl
e
i
t
i
seta
l
., 2004). Augment
i
ng t
he
c
ost
f
unct

i
on to ta
k
e
i
nto account
i
n
di
v
id
ua
l
ro
b
ot capa
bili
t
i
es (espec
i
a
ll
y
in
h
eterogeneous teams)
i
san
i

mportant extens
i
on. Accurate
l
oca
li
zat
i
on
i
s
a
m
a
j
or c
h
a
ll
en
g
e
i
nmo
bil
ero
b
ot
i
cs; we wou

ld lik
etota
k
ea
d
vanta
g
eo
f
t
he
m
eet
i
n
g
o
f
t
h
ero
b
ots
i
nor
d
er to
i
mprove t
h

e
l
oca
li
zat
i
on qua
li
t
y
v
i
a coopera
-
t
i
ve
l
oca
li
zat
i
on (Roume
li
ot
i
san
d
Re
kl

e
i
t
i
s, 2004). F
i
na
lly
,
d
eve
l
op
i
n
g
mor
e
154
Rekleitis
,
et al
.
a
ccurate cost est
i
mates
f
or t
h

e
diff
erent tas
k
s
i
s one o
f
t
h
e
i
mme
di
ate o
bj
ec
-
t
iv
es
.
A
cknowledgment
s
T
h
e aut
h
ors w

i
s
h
to t
h
an
k
E
d
war
d
Ran
ki
n
f
or
hi
s
h
e
l
pw
i
t
h
t
h
ea
l
gor

i
t
hm
i
m
pl
ementat
i
on an
d
ex
p
er
i
mentat
i
on; V
i
ncent Lee-S
h
ue an
d
Sam Sonne
f
o
r
p
rov
idi
ng va

l
ua
bl
e
i
nput
d
ur
i
ng t
h
e ear
l
y stages o
f
t
hi
s pro
j
ect; Berna
di
ne D
i
as
,
Dann
i
Go
ldb
erg, Ro

b
Z
l
ot an
d
Marc Z
i
n
kf
or t
h
e
i
r
h
e
l
pw
i
t
h
t
h
e Mar
k
et
b
ase
d
a

pproac
h
.F
i
na
ll
y, Lu
i
za So
l
omon prov
id
e
d
va
l
ua
bl
e
i
ns
i
g
h
ts on t
h
eso
f
twar
e

d
es
i
gn an
d
an
i
mp
l
ementat
i
on o
f
a grap
h
c
l
ass. Furt
h
ermore, we wou
ld lik
et
o
a
c
k
now
l
e
d

ge t
h
e generous support o
f
t
h
e DSO Nat
i
ona
l
La
b
orator
i
es, S
i
nga
-
p
ore, t
h
eO
ffi
ce o
f
Nava
l
Researc
h
,an

d
t
h
e Nat
i
ona
l
Sc
i
ence Foun
d
at
i
on
.
Reference
s
A
car,E.U.an
d
C
h
oset, H. (2000). Cr
i
t
i
ca
l
po
i

nt sens
i
ng
i
nun
k
nown env
i
ronments. I
n
P
roc. o
f
t
he IEEE International Conference on Robotics & Automatio
n
.
B
atalin, M. A. and Sukhatme, G. S. (2002). Spreading out: A local approach to multi-robo
t
c
overage. I
n
6
th International Symposium on Distributed Autonomous Robotics System
s
,
F
ukuoka, Japan
.

B
erhault, M., Huang, H., Keskinocak, P., Koenig, S., Elmaghraby, W., Griffin, P., and Kleywegt
,
A
. (2003). Robot exploration with combinatorial auctions. I
n
I
EEE/RSJ Int. Conference on
I
ntelligent Robots and System
s
,
volume 2, pages 1957–1962
.
B
ruemmer, D. J., Dudenhoeffer, D. D., Anderson, M. O., and McKay, M. D. (2002). A roboti
c
s
warm for spill finding and perimeter formation
.
Spectru
m
.
B
utler, Z., Rizzi, A., and Hollis, R. (2001). Distributed coverage of rectilinear environments. I
n
P
roc. o
f
the Workshop on the Algorithmic Foundations o

f
Robotic
s
.
C
hoset, H. and Pignon, P. (1997). Coverage path planning: The boustrophedon cellular decom
-
p
osition. In International Con
f
erence on Field and Service Robotic
s
,
Canberra
,
Australia
.
D
ias, M. B. and Stentz, A. T. (2001). A market a
pp
roach to multirobot coordination. Technica
l
R
eport CMU-RI -TR-01-26, Robotics Institute, Carne
g
ie Mellon Universit
y
, Pittsbur
g
h, PA

.
D
ias, M. B. and Stentz, A. T. (2003). Traderbots: A market-based a
pp
roach for resource, role
,
a
n
d
tas
k
a
ll
ocat
i
on
i
nmu
l
t
i
ro
b
ot coor
di
nat
i
on. Tec
h
n

i
ca
l
Re
p
ort CMU-RI -TR-03-19, Ro
-
b
ot
i
cs Inst
i
tute, Carne
gi
eMe
ll
on Un
i
vers
i
t
y
,P
i
tts
b
ur
gh
,PA
.

Di
as, M. B., Z
i
nc
k
, M., Z
l
ot, R., an
d
Stentz, A. T.
(
2004
)
.Ro
b
ust mu
l
t
i
ro
b
ot coor
di
nat
i
o
n
i
n
dy

nam
i
cenv
i
ronments. I
n
Internationa
l
Con
f
erence on Ro
b
otics & Automatio
n
, pages
3
43
5
–3442, New Orleans, LA
.
G
er
k
e
y
,B.an
d
Matar
i
c, M. (2002). So

ld
!: auct
i
on met
h
o
d
s
f
or mu
l
t
i
ro
b
ot coor
di
nat
i
on
.
IEEE
T
ran
s
action
s
on Ro
b
otic

s
an
d
Automatio
n
,18
(5)
:7
5
8 – 768
.
G
erke
y
, B. P., Vau
g
han, R. T., StÃ
ÿy
, K., Howard, A., Sukhatme, G. S., and Mataric, M. J
.
(
2001). Most va
l
ua
bl
ep
l
a
y
er:Aro

b
ot
d
ev
i
ce server
f
or
di
str
ib
ute
d
contro
l
.I
n
I
EEE
/
R
SJ
I
nternationa
l
Con
f
erence on Inte
ll
igent Ro

b
ots an
d
Systems (IROS 2001
)
,
pa
g
es 122
6
–1231
,
W
a
il
ea, Hawa
ii.
G
o
ldb
er
g
, D., C
i
c
i
re
ll
o, V., D
i

as, M. B., S
i
mmons, R., Sm
i
t
h
,S.F.,an
d
Stenz, A. (2003)
.
M
ar
k
et-
b
ase
d
mu
l
t
i
-ro
b
ot p
l
ann
i
ng
i
na

di
str
ib
ute
dl
ayere
d
arc
hi
tecture. I
n
M
u
l
ti-Ro
b
ot Sys-
M
ulti-Robot Distributed Covera
ge
155
tems: From Swarms to Intelligent Automata: Proceedings from the 2003 International Work
-
shop on Multi-Robot System
s
,
volume 2, pages 27–38. Kluwer Academic Publishers
.
I
chikawa, S. and Hara, F. (1999). Characteristics of object-searching and object-fetching behav

-
i
ors of multi-robot system using local communication. I
n
IEEE International Conference o
n
S
ystems, Man, and Cybernetics, (IEEE SMC ’99
)
, volume 4, pages 775 –781
.
K
urabayashi, D., Ota, J., Arai, T., and Yoshida, E. (1996). Cooperative sweeping by multipl
e
m
obile robots. In
1
996 IEEE International Conference on Robotics and Automatio
n
,
vol-
u
me 2, pages 1744 –1749
.
L
atimer-IV, D., Srinivasa, S., Lee-Shue, V., Sonne, S. S., Choset, H., and Hurst, A.
(
2002
)
.To

-
w
ard sensor based coverage with robot teams. I
n
Proc. 2002 IEEE International Conference
o
n Robotics & Automation
,
.
L
uo, C. and Yang, S. (2002). A real-time cooperative sweeping strategy for multiple cleanin
g
r
obots. In
I
EEE Internatinal S
y
mposium on Intelli
g
ent Contro
l
,pa
g
es 660 –665
.
M
in, T. W. and Yin, H. K. (1998). A decentralized approach for cooperative sweepin
g
b
y

mul
-
t
i
p
le mobile robots. In
1
998 IEEE/RSJ International Con
f
erence on Intelligent Robots an
d
Sy
stem
s
,
volume 1, pa
g
es 380–385
.
R
ekleitis, I., Lee-Shue, V., New, A. P., and Choset, H.
(
2004
)
. Limited communication, multi
-
r
o
b
ot team

b
ase
d
covera
g
e. I
n
IEEE Internationa
l
Con
f
erence on Ro
b
otics an
d
Automatio
n
,
p
a
g
es 34
6
2–34
6
8, New Orleans, LA
.
R
oume
li

ot
i
s, S. I. an
d
Re
kl
e
i
t
i
s, I. M. (2004). Propa
g
at
i
on o
f
uncerta
i
nt
yi
n cooperat
i
ve mu
l
t
i-
r
o
b
ot

l
oca
li
zat
i
on: Ana
ly
s
i
san
d
exper
i
menta
l
resu
l
ts
.
A
utonomou
s
Ro
b
ot
s
,17
(
1
)

:41–
5
4.
Sc
h
rec
k
en
gh
ost, D., Bonasso, P., Korten
k
amp, D., an
d
R
y
an, D. (1998). T
h
ree t
i
er arc
hi
tectur
e
f
or contro
lli
n
g
space
lif

e support s
y
stems. I
n
IEEE Int. Joint S
y
mposia on Inte
ll
i
g
ence an
d
Sy
stem
s
,
pa
g
es 19
5
– 201
.
W
a
g
ner, I., L
i
n
d
en

b
aum, M., an
d
Bruc
k
ste
i
n, A. (1999). D
i
str
ib
ute
d
cover
i
n
gby
ant-ro
b
ot
s
u
s
i
n
g
evaporat
i
n
g

traces
.
IEEE Tran
s
action
s
on Ro
b
otic
s
an
d
Automatio
n
,1
5
(
5
):918–933
.
W
a
g
ner, M., Aposto
l
opou
l
os, D., S
hill
cutt, K., S

h
ama
h
, B., S
i
mmons, R., an
d
W
hi
tta
k
er, W
.
(2001). T
h
esc
i
ence autonom
y
s
y
stem o
f
t
h
e noma
d
ro
b
ot. I

n
I
EEE Internationa
l
Conferenc
e
o
nRo
b
otic
s
an
d
Automatio
n
,
vo
l
ume 2, pages 1742 – 1749
.
V
MO
TI
O
N PLANNIN
G
AND
CO
NTR
O

L
R
EAL-TIME MULTI-ROBOT MOTION
PLANNING WITH SAFE DYNAMICS
∗
J
ames Bruce an
d
Manue
l
aVe
l
oso
Computer
S
cience Departmen
t
Carnegie Mellon Universit
y
P
ittsburgh PA 15213, US
A
{
jb
ruce,mmv
}
@
cs.cmu.ed
u
Abs

tr
act
Thi
s paper
i
ntro
d
uces a mot
i
on p
l
ann
i
ng system
f
or rea
l
-t
i
me contro
l
o
f
mu
l
t
i
p
le
h

igh performance robots in dynamic and unpredictable domains. It consists o
f
a randomized realtime path planner, a bounded acceleration motion control sys
-
t
em, and a randomized velocity-space search for collision avoidance of multipl
e
m
oving robots. The realtime planner ignores dynamics, simplifying planning
,
w
hile the motion control ignores obstacles, allowing a closed form solution
.
T
his allows up to five robots to be controlled 60 times per second, but collision
s
c
an arise due to dynamics. Thus a randomized search is performed in the robot’
s
v
elocity space to find a safe action which satisfies both obstacle and dynamic
s
c
onstraints. The system has been fully implemented, and empirical results ar
e
p
resented
.
K
e

y
words:
r
ealtime path planning, multirobot navigatio
n
1. Introduction
All
mo
bil
ero
b
ots s
h
are t
h
e nee
d
to nav
i
gate, creat
i
ng t
h
e pro
bl
em o
f
mo
-
t

i
on p
l
ann
i
ng. W
h
en mu
l
t
i
p
l
ero
b
ots are
i
nvo
l
ve
d
,t
h
eenv
i
ronment
b
ecome
s
d

ynam
i
c, an
d
w
h
en no
i
se or externa
l
agents are present, t
h
eenv
i
ronment a
l
s
o
b
ecomes unpre
di
cta
bl
e. T
h
us t
h
e mot
i
on p

l
ann
i
ng system must
b
ea
bl
etocop
e
w
i
t
hd
ynam
i
c, unpre
di
cta
bl
e
d
oma
i
ns. To ta
k
ea
d
vantage o
fhi
g

h
per
f
ormanc
e
r
o
b
ots, an
d
respon
d
qu
i
c
kl
y to externa
l
c
h
anges
i
nt
h
e
d
oma
i
n, t
h

e system mus
t
a
l
so run at rea
l
-t
i
me rates. F
i
na
ll
y, nav
i
gat
i
on at
hi
g
h
spee
d
s means respect
-
i
ng
d
ynam
i
cs constra

i
nts
i
nt
h
ero
b
ot mot
i
on to avo
id
co
lli
s
i
ons w
hil
e stay
i
n
g
∗
Thi
swor
k
was supporte
dby
Un
i
te

d
States Department o
f
t
h
e Inter
i
or un
d
er Grant No. NBCH-1040007,
and b
y
Rockwell Scientific Co., LLC under subcontract No. B4U528968 and prime contract No. W911W6-
0
4-C-0058 with the US Arm
y
. The views and conclusions contained herein are those of the authors, and
d
o not necessar
ily
re
fl
ect t
h
e pos
i
t
i
on or po
li

c
y
o
f
t
h
e sponsor
i
n
gi
nst
i
tut
i
ons, an
d
no o
ffi
c
i
a
l
en
d
orsemen
t
s
hould be inferred
.
1

59
L.E. Parker et al.
(
eds.)
,
M
ulti-Robot S
y
stems. From Swarms to Intelli
g
ent Automata. Volume III
,
1
59
–170.

c
2
005
Sprin
g
er. Printed in the Netherlands
.
160
B
ruce and
V
elos
o
w

i
t
hi
nt
h
eo
p
erat
i
ona
lb
oun
d
so
f
t
h
ero
b
ot. W
h
en mu
l
t
ipl
ero
b
ots are
i
ntro

-
d
uce
d
,t
h
e system must
fi
n
d
so
l
ut
i
ons w
h
ere no ro
b
ots co
llid
ew
hil
e sat
i
s
f
y
i
n
g

e
ac
h
ro
b
ot’s mot
i
on constra
i
nts. T
hi
s paper
d
escr
ib
es a
d
oma
i
nw
i
t
h
t
h
es
e
p
ropert
i

es, an
d
a mot
i
on p
l
ann
i
ng system w
hi
c
h
sat
i
s
fi
es t
h
e requ
i
rements
f
o
r
i
t. In t
h
e rema
i
n

d
er o
f
t
hi
s sect
i
on, t
h
e
d
oma
i
nw
ill b
e presente
d
,an
df
o
l-
l
ow
i
ng sect
i
ons w
ill d
escr
ib

et
h
et
h
ree ma
j
or parts o
f
t
h
e system ment
i
one
d
ab
ove. T
h
e system w
ill
t
h
en
b
e eva
l
uate
di
n
i
ts ent

i
rety as to
h
ow we
ll i
tso
l
ve
s
nav
i
gat
i
on tas
k
s
.
F
igure
1.
T
wo teams are s
h
own p
l
ay
i
ng soccer
i
nt

h
eRo
b
oCup sma
ll
s
i
ze
l
eague (
l
e
f
t), an
d
t
he the overall system architecture for CMDragons (right)
.
T
h
e mot
i
vat
i
ng
d
oma
i
n
f

or t
hi
swor
ki
st
h
eRo
b
oCup F180 “sma
ll
s
i
ze
”
l
eague (Kitano et al., 199
5
). It involves teams of five small robots, each up t
o
18cm in diameter and 1
5
cm height. The robot teams are entered into a compe
-
t
i
t
i
on to p
l
ay soccer aga

i
nst opponent teams
fi
e
ld
e
db
yot
h
er researc
h
groups
.
Dur
i
ng t
h
e game no
h
uman
i
nput
i
sa
ll
owe
d
,t
h
us t

h
etworo
b
ot teams mus
t
c
ompete us
i
ng
f
u
ll
autonomy
i
n every aspect o
f
t
h
e system. T
h
e
fi
e
ld
o
f
p
l
ay
is

a
carpet measur
i
ng 4.9m
b
y 3.8m, w
i
t
h
a 30cm
b
or
d
er aroun
d
t
h
e
fi
e
ld f
or po
-
s
i
t
i
on
i
ng outs

id
et
h
e
fi
e
ld
o
f
p
l
ay (suc
h
as
f
or
f
ree
ki
c
k
s). An ear
li
er (
h
a
lf
s
i
ze

)
v
ers
i
on o
f
t
h
e
fi
e
ld i
sp
i
cture
d
on t
h
e
l
e
f
to
f
F
i
gure 1. O
ffb
oar
d

commun
i
ca
-
t
i
on an
d
computat
i
on
i
sa
ll
owe
d
,
l
ea
di
ng near
l
y every team to use a centra
li
ze
d
a
pproac
hf
or most o

f
t
h
ero
b
ot contro
l
.T
h
e
d
ata
fl
ow
i
n our s
y
stem, t
y
p
i
ca
l
o
f
most teams,
i
ss
h
own on t

h
er
igh
to
f
F
ig
ure 1 (Bruce et a
l
., 2003). Sens
i
n
gis
p
rov
id
e
dby
two or more over
h
ea
d
cameras,
f
ee
di
n
gi
nto a centra
l

computer t
o
p
rocess the ima
g
e and locate the 10 robots and the ball on the field 30-
6
0 time
s
p
er second. These locations are fed into an extended Kalman filter for trackin
g
a
nd velocit
y
estimation, and then sent to a “soccer” module which implement
s
the team strate
gy
usin
g
various techniques. The soccer s
y
stem implement
s
most of its actions throu
g
h a navi
g
ation module, which provides path plan

-
nin
g
, obstacle avoidance, and motion control. Finall
y
, velocit
y
commands ar
e
sent to the robots via a serial radio link. Due to its com
p
etitive nature, team
s
M
otion Planning with Safe Dynamic
s
161
h
ave pus
h
e
d
ro
b
ot
i
c tec
h
no
l

ogy to
i
ts
li
m
i
ts, w
i
t
h
t
h
e sma
ll
ro
b
ots trave
lli
n
g
ov
er 2
m
/
s
,
acce
l
erat
i

ons
b
etween
3
−
6
m
/
s
2
,an
dki
c
ki
ng t
h
ego
lf b
a
ll
use
din
t
h
e game at 4
−
6
m
/
s

.
T
h
e
i
r spee
d
s requ
i
re every mo
d
u
l
e to run
i
n rea
l
t
i
m
e
t
om
i
n
i
m
i
ze
l

atency, w
hil
e
l
eav
i
ng enoug
h
comput
i
ng resources
f
or t
h
eot
h
e
r
mo
d
u
l
es to operate. In a
ddi
t
i
on, t
h
ea
l

gor
i
t
h
ms must operate ro
b
ust
l
y
d
ue t
o
th
e
f
u
ll
autonomy requ
i
rement.
Nav
i
gat
i
on
i
sacr
i
t
i

ca
l
component
i
nt
h
e overa
ll
system
d
escr
ib
e
d
a
b
ove
,
a
n
d
t
h
e one we w
ill f
ocus on
i
nt
hi
s paper. T

h
e system
d
escr
ib
e
dh
ere
i
s mean
t
a
sa
d
rop-
i
n rep
l
acement
f
or t
h
enav
i
gat
i
on mo
d
u
l

e use
d
success
f
u
ll
y
b
you
r
t
eam s
i
nce 2002, an
db
u
ildi
ng on exper
i
ence ga
i
ne
d
s
i
nce 1997 wor
ki
ng on
f
as

t
nav
i
gat
i
on
f
or sma
ll hi
g
h
per
f
ormance ro
b
ots (Bow
li
ng an
d
Ve
l
oso, 1999). Fo
r
o
ur mo
d
e
l
,wew
ill

assume a centra
li
ze
d
system, a
l
t
h
oug
h
t
h
e
i
nteract
i
on re
-
q
u
i
re
db
etween ro
b
ots w
ill b
em
i
n

i
m
i
ze
d
w
hi
c
h
s
h
ou
ld
ma
k
e
d
ecentra
li
zat
i
on
a
s
tra
igh
t
f
orwar
d

extens
i
on. It compr
i
ses o
f
t
h
ree cr
i
t
i
ca
l
parts; A pat
h
p
l
anner, a
mot
i
on contro
l
s
y
stem, an
d
ave
l
oc

i
t
y
space searc
hf
or sa
f
e mot
i
ons. A
l
t
h
ou
gh
mot
i
vate
dby
t
h
e spec
ifi
c requ
i
rements o
f
t
h
eRo

b
oCup
d
oma
i
n,
i
ts
h
ou
ld be
of g
enera
l
app
li
ca
bili
t
y
to
d
oma
i
ns w
h
ere severa
l
ro
b

ots must nav
ig
ate w
i
t
hin
a
closed space where both hi
g
h performance and safet
y
are desired
.
2. Path Plannin
g
For pat
h
p
l
ann
i
ng, t
h
enav
i
gat
i
on system mo
d
e

l
st
h
eenv
i
ronment
i
n2D
,
an
d
a
l
so
i
gnor
i
ng
d
ynam
i
cs constra
i
nts, w
hi
c
h
w
ill i
nstea

db
e
h
an
dl
e
db
y
a
l
ater mo
d
u
l
e. T
h
ea
l
gor
i
t
h
m use
di
st
h
e ERRT extens
i
on o
f

t
h
e RRT-Goa
l
B
i
a
s
pl
anner (LaVa
ll
e, 1998, LaVa
ll
ean
d
James J. Ku
ff
ner, 2001). Due to t
h
e spee
d
of
t
h
ea
l
gor
i
t
h

m,anewp
l
an can
b
e constructe
d
eac
h
contro
l
cyc
l
e, a
ll
ow
i
ng
it
to trac
k
c
h
anges
i
nt
h
e
d
ynam
i

cenv
i
ronment w
i
t
h
out t
h
e nee
df
or rep
l
ann
i
n
g
h
eur
i
st
i
cs. A more t
h
oroug
hd
escr
i
pt
i
on o

f
our prev
i
ous wor
k
on ERRT can
b
e
f
oun
di
n (Bruce an
d
Ve
l
oso, 2002). S
i
nce t
h
en, a new more e
ffi
c
i
ent
i
mp
l
e
-
m

entat
i
on
h
as
b
een comp
l
ete
d
,
b
ut t
h
eun
d
er
l
y
i
ng a
l
gor
i
t
h
m
i
st
h

e same. It
is
d
escr
ib
e
dh
ere
i
n enoug
hd
eta
il
to
b
eun
d
erstoo
df
or t
h
e eva
l
uat
i
ons
l
ater
i
nt

he
p
aper
.
Rap
idl
y-exp
l
or
i
ng ran
d
om trees (RRTs) (LaVa
ll
e, 1998) emp
l
oy ran
d
om
-
i
zat
i
on to exp
l
ore
l
ar
g
e state spaces e

ffi
c
i
ent
ly
,an
df
orm t
h
e
b
as
i
s
f
or a
f
am
ily
of
pro
b
a
bili
st
i
ca
lly
comp
l

ete, t
h
ou
gh
non-opt
i
ma
l
,
ki
no
dy
nam
i
c pat
h
p
l
anner
s
(
LaVa
ll
ean
d
James J. Ku
ff
ner, 2001). T
h
e

i
r stren
g
t
hli
es
i
nt
h
at t
h
e
y
can e
f-
ficientl
y
find plans in relativel
y
open or hi
g
h dimensional spaces because the
y
avoid the state explosion that discretization faces. A basic plannin
g
al
g
orith
m
usin

g
RRTs is shown in Fi
g
ure 2, and the steps are as follows: Start with a
trivial tree consistin
g
onl
y
of the initial confi
g
uration. Then iterate: With prob
-
abilit
y
p
,
find the nearest
p
oint in the current tree and extend it toward th
e
g
oa
l
g
.
Extendin
g
means addin
g
a new point to the tree that extends from

a
16
2
B
ruce and
V
elos
o
p
o
i
nt
i
nt
h
e tree towar
d
g
w
hil
ema
i
nta
i
n
i
ng w
h
atever mot
i

on constra
i
nts ex
i
st
.
Al
ternat
i
ve
l
y, w
i
t
h
pro
b
a
bili
ty
1
−
p
,p
i
c
k
apo
i
n

t
x
un
if
orm
l
y
f
rom t
h
e con
fi
g-
u
rat
i
on space,
fi
n
d
t
h
e nearest po
i
nt
i
nt
h
e current tree, an
d

exten
di
ttowar
d
x
.
T
h
us t
h
e tree
i
s
b
u
il
tupw
i
t
h
a com
bi
nat
i
on o
f
ran
d
om exp
l

orat
i
on an
dbi
ase
d
mot
i
on towar
d
st
h
e goa
l
con
fi
gurat
i
on
.
Start with q-ini
t
tar
g
e
t
e
xten
d
s

te
p

2
ste
p

1
s
te
p

8

F
igure
2.
E
xamp
l
e growt
h
o
f
an RRT tree
f
or severa
l
steps. Eac
hi

terat
i
on, a ran
d
om targe
t
i
s chosen and the closest node in the tree is “extended” toward the target, adding another nod
e
t
o the tree
.
To convert t
h
eRRTa
l
gor
i
t
h
m
i
nto a p
l
anner, we nee
d
two s
i
mp
l

ea
ddi
t
i
ons
.
O
ne
i
s to restr
i
ct t
h
e tree to
f
ree s
p
ace, w
h
ere
i
tw
ill
not co
llid
ew
i
t
h
o

b
stac
l
es
.
T
hi
s can
b
e accomp
li
s
h
e
db
yon
l
ya
ddi
ng no
d
es
f
or extens
i
ons t
h
at w
ill
not

hit
ob
stac
l
es. To ma
k
et
h
e tree
i
ntoap
l
anner, we on
l
y nee
d
to stop once t
h
e tre
e
h
as reac
h
e
d
apo
i
nt su
ffi
c

i
ent
l
yc
l
ose to t
h
e goa
ll
ocat
i
on. Because t
h
e root o
f
t
h
e tree
i
st
h
e
i
n
i
t
i
a
l
pos

i
t
i
on o
f
t
h
ero
b
ot, trac
i
ng up
f
rom any
l
ea
f
g
i
ves a va
lid
p
at
h
t
h
roug
hf
ree space
b

etween t
h
at
l
ea
f
an
d
t
h
e
i
n
i
t
i
a
l
pos
i
t
i
on. T
h
us
fi
n
di
n
g

al
ea
f
near t
h
e goa
li
ssu
ffi
c
i
ent to so
l
ve t
h
ep
l
ann
i
ng pro
bl
em
.
Execut
i
on Exten
d
e
d
RRT (ERRT) a

dd
st
h
e not
i
on o
f
a waypo
i
nt cac
h
e
,
w
hi
c
hi
sa
fi
xe
d
-s
i
ze
l
ossy store o
f
no
d
es

f
rom success
f
u
l
p
l
ans
i
n prev
i
ous
i
terat
i
ons o
f
t
h
ep
l
anner. W
h
enever a p
l
an
i
s
f
oun

d
,a
ll
no
d
es a
l
ong t
h
e pat
h
a
re a
dd
e
d
to t
h
e cac
h
ew
i
t
h
ran
d
om rep
l
acement o
f

prev
i
ous entr
i
es. T
h
en
d
ur
-
i
n
g
p
l
ann
i
n
g
, ran
d
om tar
g
ets are now c
h
osen
f
rom t
h
ree sources

i
nstea
d
o
f
two
.
In ot
h
er wor
d
s, w
i
t
h
pro
b
a
bili
t
y
p
i
tp
i
c
k
st
h
e

g
oa
l
,w
i
t
h
pro
b
a
bili
t
y
q
i
tp
i
c
k
s
a
ran
d
om state
f
rom t
h
ewa
y
po

i
nt cac
h
e, an
d
w
i
t
h
t
h
e rema
i
n
i
n
g1
−
p
−
q
i
t
p
icks a random state in the environment
.
In order to implement ERRT we need a
n
e
xten

d
o
perator, a distance func
-
tion between robot states, a distribution for
g
eneratin
g
random states in th
e
e
nvironment, and a wa
y
of determinin
g
the closest point in a tree to a
g
ive
n
tar
g
et state. Our implementation uses Euclidean distance for the distance func
-
tion and the uniform distribution for
g
eneratin
g
random states. The neares
t
state in the tree is determined usin

g
KD-Trees, a common technique for speed
-
in
g
up nearest nei
g
hbor queries. Finall
y
th
e
e
xten
d
o
perator it simpl
y
steps a
fixed distance alon
g
the path from the current state to the tar
g
et. For a planne
r
i
g
norin
g
d
y

namics, this is the simplest wa
y
to
g
uarantee the new state returne
d
is closer to the intermediate tar
g
et than the parent. Our step size is set to th
e