T
E
L
KO
M
N
I
KA
T
e
lec
om
m
u
n
icat
ion
,
Com
p
u
t
i
n
g,
E
lec
t
r
on
ics
an
d
Cont
r
ol
Vol.
18
,
No.
3
,
J
une
2020
,
pp.
1514
~
1521
IS
S
N:
1693
-
6930
,
a
c
c
r
e
dit
e
d
F
ir
s
t
G
r
a
de
by
Ke
me
nr
is
tekdikti
,
De
c
r
e
e
No:
21/E
/KP
T
/2018
DO
I
:
10.
12928/
T
E
L
KO
M
NI
KA
.
v18i3.
14829
1514
Jou
r
n
al
h
omepage
:
ht
tp:
//
jour
nal.
uad
.
ac
.
id/
index
.
php/T
E
L
K
OM
N
I
K
A
S
t
e
r
e
o vi
si
on
-
b
ase
d
o
b
st
ac
le
a
voi
d
an
c
e
m
od
u
l
e
on
3D
p
oi
n
t
c
lo
u
d
d
a
t
a
E
k
o
P
u
r
b
o
Wa
h
yon
o
1
,
E
n
d
ah
S
u
r
yawat
i
Ni
n
gr
u
m
2
,
Raden
S
an
ggar
De
want
o
3
,
Da
d
e
t
P
r
am
ad
i
h
an
t
o
4
1
D
ep
ar
t
men
t
o
f
E
l
ect
r
i
cal
E
n
g
i
n
eeri
n
g
,
Po
l
i
t
ek
n
i
k
E
l
e
k
t
r
o
n
i
k
a
N
e
g
eri
S
u
rab
a
y
a,
In
d
o
n
e
s
i
a
2
D
ep
a
r
t
men
t
o
f
Me
ch
a
n
i
ca
l
an
d
E
n
er
g
y
E
n
g
i
n
eeri
n
g
,
Po
l
i
t
ek
n
i
k
E
l
e
k
t
r
o
n
i
k
a
N
eg
er
i
Su
rab
a
y
a,
In
d
o
n
es
i
a
3
D
ep
ar
t
men
t
o
f
In
f
o
rmat
i
o
n
E
n
g
i
n
eeri
n
g
,
Po
l
i
t
ek
n
i
k
E
l
e
k
t
r
o
n
i
k
a
N
e
g
eri
S
u
rab
a
y
a,
In
d
o
n
e
s
i
a
4
Ro
b
o
t
i
cs
an
d
In
t
el
l
i
g
en
t
Sy
s
t
em
Cen
t
er,
In
d
o
n
e
s
i
a
Ar
t
icle
I
n
f
o
AB
S
T
RACT
A
r
ti
c
le
h
is
tor
y
:
R
e
c
e
ived
Aug
23
,
2019
R
e
vis
e
d
J
a
n
29
,
2020
Ac
c
e
pted
F
e
b
26
,
2020
T
h
i
s
p
a
p
er
d
eal
s
i
n
b
u
i
l
d
i
n
g
a
3
D
v
i
s
i
o
n
-
b
a
s
ed
o
b
s
t
acl
e
av
o
i
d
an
ce
an
d
n
av
i
g
a
t
i
o
n
.
In
o
r
d
er
fo
r
a
n
a
u
t
o
n
o
mo
u
s
s
y
s
t
em
t
o
w
o
rk
i
n
rea
l
l
i
fe
c
o
n
d
i
t
i
o
n
,
a
cap
ab
i
l
i
t
y
o
f
g
a
i
n
i
n
g
s
u
rro
u
n
d
i
n
g
e
n
v
i
ro
n
men
t
d
a
t
a,
i
n
t
er
p
ret
t
h
e
d
a
t
a
an
d
t
ak
e
a
p
p
r
o
p
r
i
at
e
ac
t
i
o
n
i
s
n
ee
d
ed
.
O
n
e
o
f
t
h
e
req
u
i
re
d
ca
p
ab
i
l
i
t
y
i
n
t
h
i
s
mat
t
er
fo
r
an
au
t
o
n
o
mo
u
s
s
y
s
t
em
i
s
a
cap
ab
i
l
i
t
y
t
o
n
av
i
g
a
t
e
cl
u
t
t
ere
d
,
u
n
o
rg
a
n
i
ze
d
en
v
i
ro
n
men
t
an
d
av
o
i
d
i
n
g
c
o
l
l
i
s
i
o
n
w
i
t
h
a
n
y
p
res
e
n
t
o
b
s
t
ac
l
e,
d
efi
n
ed
as
an
y
d
at
a
w
i
t
h
v
ert
i
cal
o
ri
e
n
t
a
t
i
o
n
a
n
d
ab
l
e
t
o
t
a
k
e
d
ec
i
s
i
o
n
w
h
en
e
n
v
i
ro
n
men
t
u
p
d
at
e
ex
i
s
t
.
Pro
p
o
s
e
d
i
n
t
h
i
s
w
o
rk
are
t
w
o
-
s
t
e
p
s
t
rat
eg
y
o
f
ex
t
ract
i
n
g
t
h
e
o
b
s
t
ac
l
e
p
o
s
i
t
i
o
n
an
d
o
ri
en
t
at
i
o
n
fro
m
p
o
i
n
t
cl
o
u
d
d
at
a
u
s
i
n
g
p
l
an
e
b
a
s
ed
s
eg
me
n
t
a
t
i
o
n
a
n
d
t
h
e
r
es
u
l
t
an
t
s
e
g
men
t
at
i
o
n
are
ma
p
p
ed
b
a
s
ed
o
n
o
b
s
t
ac
l
e
p
o
i
n
t
p
o
s
i
t
i
o
n
rel
at
i
v
e
t
o
camera
u
s
i
n
g
o
ccu
p
a
n
cy
g
r
i
d
map
t
o
acq
u
i
re
o
b
s
t
ac
l
e
cl
u
s
t
er
p
o
s
i
t
i
o
n
an
d
reco
rd
ed
t
h
e
o
ccu
p
an
c
y
g
ri
d
map
fo
r
fu
t
u
re
u
s
e
an
d
g
l
o
b
al
n
av
i
g
a
t
i
o
n
,
o
b
s
t
acl
e
p
o
s
i
t
i
o
n
g
a
i
n
e
d
i
n
g
ri
d
map
i
s
u
s
e
d
t
o
p
l
an
t
h
e
n
av
i
g
a
t
i
o
n
p
at
h
t
o
w
ard
s
t
ar
g
et
g
o
a
l
w
i
t
h
o
u
t
g
o
i
n
g
t
h
r
o
u
g
h
o
b
s
t
acl
e
p
o
s
i
t
i
o
n
an
d
mo
d
i
fy
t
h
e
n
a
v
i
g
at
i
o
n
p
a
t
h
t
o
a
v
o
i
d
c
o
l
l
i
s
i
o
n
w
h
en
e
n
v
i
ro
n
men
t
u
p
d
at
e
i
s
p
res
e
n
t
o
r
p
l
a
t
fo
rm
mo
v
eme
n
t
i
s
n
o
t
a
l
i
g
n
e
d
w
i
t
h
n
a
v
i
g
at
i
o
n
p
a
t
h
b
as
ed
o
n
t
i
med
e
l
as
t
i
c
b
an
d
met
h
o
d
.
K
e
y
w
o
r
d
s
:
3D
Vis
ion
Obs
tac
le
a
voidanc
e
Obs
tac
le
e
xtr
a
c
ti
on
T
im
e
d
e
las
ti
c
ba
nd
Th
i
s
i
s
a
n
o
p
en
a
c
ces
s
a
r
t
i
c
l
e
u
n
d
e
r
t
h
e
CC
B
Y
-
SA
l
i
ce
n
s
e
.
C
or
r
e
s
pon
din
g
A
u
th
or
:
E
ko
P
ur
bo
W
a
hyono,
De
pa
r
tm
e
nt
of
E
lec
tr
ica
l
E
nginee
r
ing,
P
oli
teknik
E
plekt
r
onika
Ne
ge
r
i
S
ur
a
ba
ya
,
Ka
mpus
P
E
NS
,
J
l.
R
a
ya
I
T
S
,
Ke
puti
h,
Ke
c
.
S
ukoli
lo,
Kota
S
B
Y,
J
a
wa
T
im
u
r
60111
,
S
u
r
a
ba
ya
,
I
ndon
e
s
ia.
E
mail:
e
ko
.
pw00@
gmail.
c
om
1.
I
NT
RODU
C
T
I
ON
R
e
c
e
nt
de
ve
lopm
e
nt
on
a
utonom
ous
platf
o
r
m
ge
a
r
e
d
towa
r
ds
a
s
s
is
ti
ng
human
in
thei
r
da
il
y
a
c
ti
vit
y
manda
tes
a
ll
a
utonom
ous
platf
or
m
to
ha
ve
a
c
a
pa
bil
it
y
to
pe
r
c
e
ive
the
s
ur
r
ounding
e
nvi
r
onment,
a
n
d
make
a
s
ui
table
de
c
is
ion
f
or
a
c
ti
on.
One
e
s
s
e
nti
a
l
a
c
ti
on
is
to
s
a
f
e
ly
na
vigate
c
lut
ter
e
d
e
nvir
onment
a
nd
a
vo
idi
ng
c
oll
is
ion
with
a
ny
obs
tac
le
that
pr
e
s
e
nts
withi
n
the
e
nvir
onment.
One
c
ha
ll
e
nge
pr
e
s
e
nt
is
a
voidi
ng
c
oll
is
ion
with
obs
tac
le
a
nd
na
vigating
in
a
n
uns
tr
u
c
tur
e
d
e
nvir
onment
[
1]
.
S
ince
uns
tr
uc
tur
e
d
e
nvi
r
onment
ha
ve
s
o
many
pos
s
ibi
li
ti
e
s
of
obs
tac
le
po
s
it
ion,
or
ienta
ti
on
a
nd
or
s
ha
pe
,
c
ha
ll
e
nge
pr
e
s
e
nt
invol
ve
d
in
ga
ini
ng
obs
tac
le
pos
it
ion,
o
r
ienta
ti
on
a
nd
s
ize
in
or
de
r
to
s
a
f
e
ly
pla
n
a
pa
th
to
a
void
a
c
oll
is
ion
.
One
c
omm
only
us
e
d
s
olut
ion
to
de
a
l
with
thes
e
pr
oblems
is
3D
v
is
ion
[
2
,
3]
ba
s
e
d
s
olut
ion
to
g
a
in
obs
tac
le
pos
it
ion,
o
r
ienta
ti
on
a
nd
s
ize
s
ince
with
3D
v
is
ion
the
s
ys
tem
did
not
ne
e
d
to
r
e
c
ognize
a
nd
lea
r
n
the
objec
t
to
a
c
qui
r
e
the
inf
o
r
m
a
ti
on
on
obje
c
t
pos
it
ion,
or
ienta
ti
on
a
nd
s
ize
s
ince
the
r
a
w
da
ta
it
s
e
lf
is
a
lr
e
a
dy
3D.
M
a
ny
s
olut
ions
e
xis
t
on
the
f
ield
of
ga
ini
ng
3D
inf
or
mation
f
r
om
the
e
nvir
onment
,
on
e
of
the
s
olut
ion
is
us
ing
c
omput
e
r
s
ter
e
o
vis
ion
[
4
–
6]
to
Evaluation Warning : The document was created with Spire.PDF for Python.
T
E
L
KO
M
NI
KA
T
e
lec
omm
un
C
omput
E
l
C
ontr
o
l
Ster
e
o
v
is
ion
-
bas
e
d
obs
tacle
av
oidance
module
on
3D
point
c
loud
data
(
E
k
o
P
ur
bo
W
ahy
ono
)
1515
c
r
e
a
te
a
n
point
c
loud
da
ta
[
7]
.
On
s
ol
ving
the
pr
o
blems
on
3D
vis
ion
a
nd
obs
tac
le
a
voidanc
e
,
a
s
th
e
s
ys
tem
be
c
ome
pr
ogr
e
s
s
ively
mo
r
e
c
om
plex
a
nd
de
mandi
ng
with
pr
oc
e
s
s
ing
powe
r
,
the
de
mands
f
o
r
mor
e
d
e
tails
on
input
on
e
nvir
onment
a
nd
f
a
s
ter
e
xe
c
uti
on
t
im
e
be
c
ome
mor
e
c
lea
r
.
F
r
om
the
input
pr
o
c
e
s
s
ing
s
ide,
with
3D
v
is
ion
that
c
r
e
a
te
a
3D
r
e
pr
e
s
e
ntation
of
s
ur
r
ounding
e
nvir
onment
with
gr
e
a
t
de
tails
c
omes
with
the
pr
ice
of
e
nor
mous
da
ta
s
ize
a
nd
de
mand
mor
e
pr
oc
e
s
s
ing
p
owe
r
to
pr
oc
e
s
s
the
da
ta.
One
s
olut
ion
to
thi
s
p
r
obl
e
m
is
to
r
e
duc
e
da
ta
s
ize
while
ke
e
ping
the
c
ha
r
a
c
ter
is
ti
c
s
,
nor
mally
thi
s
r
e
s
ult
in
r
e
duc
e
d
e
nvi
r
onme
nt
de
tails
.
R
AN
S
AC
(
r
a
ndom
s
a
mpl
e
c
ons
e
ns
u
s
)
ba
s
e
d
met
hod
with
S
AC
model
[
8
]
is
one
of
the
mos
t
us
e
d
method
us
e
d
to
r
e
duc
e
da
ta
s
ize
without
s
a
c
r
if
icing
too
m
uc
h
de
tails
on
e
nvir
o
nment
da
ta
a
nd
r
obus
t
in
pr
e
s
e
nc
e
of
nois
e
[
9]
,
T
his
wor
k
us
e
s
Na
N,
pa
s
s
thr
ough
a
n
d
voxe
l
gr
id
f
il
ter
ba
s
e
d
on
R
AN
S
AC
to
r
e
duc
e
da
ta
s
ize
.
T
o
f
ur
ther
r
e
duc
e
c
omput
a
ti
on,
pa
r
a
ll
e
l
c
omput
ing
a
r
e
us
e
d
[
9
–
11
]
.
B
a
s
e
d
f
r
o
m
[
9]
c
ompar
is
on
thi
s
w
or
k
us
e
d
C
UD
A
(
c
om
pute
uni
f
ied
de
vice
a
r
c
hit
e
c
tur
e
)
pa
r
a
ll
e
l
c
omput
ing
ba
s
e
d
on
[
12]
.
T
he
r
e
a
s
on
f
o
r
pa
r
a
ll
e
l
c
omput
ing
is
to
mi
nim
ize
e
xe
c
uti
on
ti
me
in
o
r
de
r
t
o
e
na
ble
the
obs
tac
le
a
voidanc
e
modul
e
to
be
in
a
p
r
oc
e
s
s
ing
unit
c
ombi
ne
d
with
a
nother
f
unc
ti
on
f
o
r
plat
f
or
m
move
ment
a
nd
a
utom
a
ti
on.
E
nvir
onment
da
ta
mus
t
be
c
las
s
if
ied
int
o
r
e
gion
s
with
s
im
il
a
r
c
ha
r
a
c
ter
is
ti
c
to
f
a
c
il
it
a
te
f
u
r
ther
pr
oc
e
s
s
ing.
One
of
da
ta
c
las
s
if
ying
is
da
ta
s
e
gmenta
ti
on.
T
his
pr
oc
e
s
s
wa
s
r
e
quir
e
d
to
dif
f
e
r
e
nti
a
te
da
ta
to
a
c
qu
ir
e
us
a
ble
e
nvir
onment
da
ta
[
13
-
16]
.
T
his
wor
k
us
ing
plane
-
ba
s
e
d
s
e
gmenta
ti
on
to
d
if
f
e
r
e
nti
a
te
be
tw
e
e
n
obs
tac
le
whic
h
de
f
ined
a
s
a
ny
point
c
loud
da
ta
wit
h
ve
r
ti
c
a
l
p
lane
or
ienta
ti
on
a
nd
the
hor
izonta
l
wa
lk
ing
plane
it
s
e
lf
.
T
his
s
tep
is
us
e
d
to
s
e
gment
the
wa
lki
ng
plane
a
nd
obs
ta
c
le
da
ta.
Obs
tac
le
da
ta,
a
r
e
pr
e
s
e
ntation
of
e
nvir
onment
c
ondit
ion
a
r
e
us
e
d
t
o
plan
the
c
oll
is
ionf
r
e
e
pa
th.
S
ince
the
platf
o
r
m
moveme
nt
li
e
s
on
x,
y
a
xis
,
the
obs
tac
le
da
ta
in
f
or
m
o
f
po
int
c
loud
c
a
n
be
c
onve
r
ted
to
gr
id
map
[
17
,
18]
.
R
e
c
e
nt
r
e
s
e
a
r
c
h
pr
oduc
e
a
n
a
lgor
it
hm
that
not
only
a
ble
to
plan
s
a
f
e
pa
th
but
a
ls
o
de
a
l
with
e
nvir
o
nment
c
ha
nge
a
nd
plat
f
or
m
dis
tur
ba
nc
e
s
that
c
a
us
e
the
platf
or
m
to
s
tr
a
y
f
r
om
planne
d
pa
th.
One
of
the
a
lgor
it
hm
is
ti
med
-
e
las
ti
c
-
ba
nd
[
19]
.
M
a
ny
r
e
s
e
a
r
c
h
ha
s
be
e
n
done
on
im
p
r
oving
T
E
B
(
ti
me
d
e
las
ti
c
ba
nd
)
method
[
20
–
24]
,
thi
s
wor
k
ba
s
e
t
he
T
E
B
a
ppli
c
a
ti
on
f
r
om
r
e
s
e
a
r
c
h
by
[
20
,
22
].
T
he
f
inal
goa
l
of
thi
s
wor
k
is
a
s
ys
tem
modul
e
wi
th
in
tegr
a
ted
p
r
oc
e
s
s
in
g
f
or
3D
point
c
loud
da
ta
s
im
il
a
r
to
[
25]
wi
th
a
n
out
put
of
obs
tac
le
de
tec
ti
on
a
nd
obs
tac
le
pos
it
ion
inf
or
mation
to
pr
oduc
e
pa
th
planning
with
a
n
a
bil
it
y
of
upda
ti
n
g
the
pa
th
to
de
a
l
with
e
nvir
onment
c
ha
nge
s
a
nd
or
platf
or
m
de
viation
while
r
e
c
or
d
the
e
nvir
onment
da
ta
f
o
r
f
ut
ur
e
us
a
ge
a
nd
s
ubs
e
que
nt
global
pa
th
planning.
2.
P
ROP
OS
E
D
S
Y
S
T
E
M
OVE
RV
I
E
W
B
e
low
a
r
e
the
ge
ne
r
a
l
ove
r
view
of
s
teps
take
n
t
o
pr
oc
e
s
s
e
nvir
o
nment
da
ta
to
plan
the
obs
tac
le
a
voidanc
e
a
c
ti
on:
−
P
r
e
-
P
r
oc
e
s
s
ing.
R
a
w
point
c
loud
da
ta
f
r
om
a
s
ter
e
o
c
a
mer
a
a
r
e
f
il
te
r
e
d
to
down
s
a
mpl
e
the
da
ta
us
ing
voxe
l
gr
id
method
a
nd
r
e
move
unne
c
e
s
s
a
r
y
point
s
.
−
Obs
tac
le
e
xtr
a
c
ti
on.
E
nvi
r
onment
da
ta
in
f
or
m
of
p
oint
c
loud
a
r
e
s
e
pa
r
a
ted
be
twe
e
n
ve
r
ti
c
a
l
a
nd
hor
iz
ontal
plane
ba
s
e
d
on
point
da
ta
or
ienta
ti
on,
e
nvir
onme
nt
int
e
r
pr
e
ted
int
o
a
g
r
id
to
ge
t
the
in
f
or
mation
o
f
s
a
f
e
movi
ng
s
pa
c
e
a
nd
obs
tac
le
a
r
e
a
,
the
r
e
s
ult
a
nt
gr
id
a
r
e
r
e
c
or
de
d
f
o
r
f
utur
e
us
e
a
nd
global
na
vigation
.
−
P
a
th
p
lanning.
P
lan
a
pa
th
towa
r
ds
goa
l
thr
ough
s
a
f
e
movi
ng
a
r
e
a
us
ing
oc
c
upa
nc
y
map,
ke
e
ping
c
l
e
a
r
of
obs
tac
les
a
r
e
a
while
upda
ti
ng
e
nvir
onment
gr
id
a
nd
a
da
pt
the
pa
th
whe
n
e
nv
ir
onment
da
ta
a
nd
pla
tf
or
m
pos
it
ion
a
r
e
c
ha
nge
d,
pub
li
s
h
a
r
r
a
y
of
wa
y
point
s
a
long
the
global
pa
th
a
nd
c
r
e
a
te
ne
w
wa
ypoint
s
a
r
r
a
y
towa
r
ds
global
pa
th
tar
ge
t
.
3.
RE
S
E
AR
CH
M
E
T
HO
D
3.
1.
P
o
in
t
c
lou
d
e
n
g
in
e
On
c
a
mer
a
in
it
ializa
ti
on,
c
a
mer
a
c
a
li
br
a
ti
on
a
nd
pr
e
-
s
e
t
pa
r
a
mete
r
s
a
r
e
loade
d
to
e
na
ble
im
a
ge
a
c
quis
it
ion.
Da
ta
take
n
f
r
o
m
S
ter
e
o
2D
c
a
mer
a
with
f
oc
a
l
po
int
a
nd
ba
s
e
li
ne
a
r
e
s
ter
e
o
ma
tche
d,
a
nd
tr
iangula
ted
to
ga
in
de
pth
da
ta
by
:
=
(
∗
)
(
,
−
,
)
(
1)
whe
r
e
,
a
nd
,
a
r
e
matc
hing
pixel
be
twe
e
n
lef
t
a
nd
r
i
ght
c
a
mer
a
r
e
s
pe
c
ti
ve
ly,
the
r
e
s
ult
a
nt
de
p
th
da
ta
is
c
ombi
ne
d
with
R
GB
da
ta
f
r
om
the
c
a
mer
a
a
nd
c
onve
r
ted
int
o
3D
poin
t
c
loud
da
ta
f
or
mat
by
nor
malizing
the
pixel
c
oor
dinate
(
,
,
)
int
o
3D
c
oor
dinate
(
,
,
)
with
c
a
mer
a
ba
s
e
li
ne
us
ing
:
=
,
=
∗
,
=
∗
(
2)
to
be
p
r
oc
e
s
s
e
d
a
f
ter
wa
r
ds
by
s
ubs
e
que
nt
s
ub
-
s
y
s
tems
.
P
oint
c
loud
r
e
s
ult
a
nt
of
1
a
nd
2
ha
ve
pa
r
a
ll
e
l
plane
s
in
r
e
a
l
c
ondit
ion
s
hown
a
s
pa
r
a
ll
e
l
p
lane
s
,
ther
e
f
o
r
e
e
li
mi
na
ti
ng
the
ne
e
d
of
c
a
lcula
ti
ng
c
a
mer
a
pe
r
s
pe
c
ti
ve
.
All
thes
e
pr
oc
e
s
s
e
s
a
r
e
done
in
a
n
e
a
r
ly
s
tage
ins
ide
the
c
a
mer
a
it
s
e
lf
.
Evaluation Warning : The document was created with Spire.PDF for Python.
I
S
S
N
:
1693
-
6930
T
E
L
KO
M
NI
KA
T
e
lec
omm
un
Co
mput
E
l
C
ontr
o
l
,
Vol.
18
,
No
.
3
,
J
une
2020:
1514
-
1521
1516
3.
2.
F
il
t
e
r
in
g
S
ince
de
ns
e
point
c
loud
da
ta
while
pr
ov
ide
mor
e
de
tails
on
e
nvir
onment
lea
ds
to
longer
e
xe
c
uti
on
ti
me
ne
e
de
d
f
or
pr
oc
e
s
s
ing,
the
r
e
f
or
e
a
method
f
or
ke
e
ping
e
nvi
r
onment
de
tails
while
r
e
duc
ing
po
int
c
ount
that
lea
ds
to
f
a
s
ter
e
xe
c
uti
on
is
ne
e
de
d
[
26
]
.
T
his
s
y
s
tem
us
e
s
voxe
l
gr
id
f
il
ter
ing
whic
h
g
r
oup
da
ta
wit
h
s
im
il
a
r
c
ha
r
a
c
ter
is
ti
c
s
a
nd
r
e
plac
e
th
e
gr
oup
a
s
ingl
e
da
ta
t
ha
t
r
e
pr
e
s
e
nt
the
whole
gr
oup,
thi
s
f
il
ter
wa
s
c
hos
e
n
f
or
the
r
obus
t
c
ha
r
a
c
ter
is
ti
c
o
f
the
f
il
ter
.
P
oint
c
loud
da
ta
is
down
-
s
a
mpl
e
d
us
ing
voxe
l
gr
id
method.
A
de
f
ined
Or
iente
d
bounding
box
a
long
with
the
m
ini
mum
a
nd
maximum
va
lue
o
f
poin
t
c
loud
in
,
,
a
xis
a
r
e
us
e
d
to
c
a
lcula
te
voxe
l
gr
id
in
s
ize
of
×
×
whic
h
de
f
ined
a
s
:
(
0
<
<
,
0
<
<
,
0
<
<
)
(
3)
w
it
h
the
voxe
l
g
r
id
s
ize
de
f
ined
,
a
s
s
ign
e
a
c
h
gr
i
d
to
e
a
c
h
point
i
n
point
c
loud
.
A
f
ter
e
a
c
h
g
r
id
a
s
s
igned
a
nd
the
c
e
nter
gr
a
vit
y
point
f
or
e
a
c
h
gr
id
with
,
,
a
nd
,
,
a
s
the
outm
os
t
point
o
f
the
gr
id
c
a
lcula
ted
by
:
=
+
2
,
=
+
2
,
=
+
2
(
4)
a
ll
point
s
in
voxe
l
a
r
e
r
e
plac
e
d
by
c
e
nter
g
r
a
vit
y
point
of
e
a
c
h
gr
id
with
f
inal
pos
it
i
on
point
s
e
t
´
.
P
r
oc
e
s
s
a
ll
voxe
l
gr
id
to
a
c
quir
e
´
by:
′
=
(
0
<
<
,
0
<
<
,
0
<
<
)
(
5)
De
f
ined
s
ize
of
a
voxe
l
is
us
e
d
to
g
r
oup
ne
igh
bour
ing
point
o
f
a
n
ini
ti
a
l
point
P
i
with
s
im
il
a
r
c
ha
r
a
c
ter
is
ti
c
s
a
nd
us
ing
the
P
i
a
s
c
e
ntr
oid
a
ll
othe
r
point
r
e
moved
lea
ving
the
c
e
ntr
oid
point
da
ta
a
f
t
e
r
wa
r
ds
.
S
mall
voxe
l
s
ize
,
while
ke
e
p
mor
e
de
tails
of
r
a
w
p
oint
c
loud
da
ta
,
lea
ds
to
longer
e
xe
c
uti
on
ti
me
whi
le
bigger
voxe
l
s
ize
ha
ve
s
maller
e
xe
c
uti
on
ti
me
but
los
e
s
m
or
e
de
tails
.
R
e
s
ul
tant
of
thi
s
s
tep
a
r
e
a
s
s
hown
on
F
ig
ur
e
1
w
hil
e
voxe
l
gr
id
is
s
lowe
r
,
it
r
e
pr
e
s
e
nts
the
unde
r
lyi
ng
s
ur
f
a
c
e
mo
r
e
a
c
c
ur
a
tely
without
s
a
c
r
if
icing
ke
ypoint
of
3D
po
int
c
loud
da
ta
[
27
].
T
h
is
c
ha
r
a
c
ter
is
ti
c
will
e
na
ble
the
s
e
gmenta
ti
on
pr
oc
e
s
s
to
s
e
gment
the
d
a
ta
pr
ope
r
ly.
T
he
r
e
s
ult
a
nt
da
ta
f
r
o
m
th
is
s
tep
s
ti
ll
c
ont
a
ins
a
ll
da
ta
with
in
c
a
mer
a
’
s
f
ield
of
view
,
s
ince
th
e
s
ys
tem
do
not
ne
e
d
r
oof
inf
o
r
mation
a
nd
e
xis
tenc
e
o
f
r
oof
da
ta
in
oc
c
upa
nc
y
gr
id
map
pr
oc
e
s
s
c
a
n
lea
d
to
no
a
va
il
a
ble
movi
ng
s
pa
c
e
,
he
nc
e
the
r
e
moval
of
r
oof
point
c
lo
ud
da
ta
is
ne
e
de
d.
T
he
s
ys
tem
us
e
a
pa
s
s
thr
ough
f
i
lt
e
r
with
maximum
ve
r
ti
c
a
l
pa
r
a
mete
r
s
to
e
xc
lude
r
oof
point
c
loud
[
27
]
,
i
f
pa
s
s
thr
ough
f
il
ter
is
not
e
xis
t,
the
r
oof
s
e
gmente
d
will
be
de
f
ined
a
s
a
n
obs
ta
c
le
s
ince
t
he
r
oof
plane
it
s
e
lf
li
e
s
pa
r
a
ll
e
l
with
wa
lki
ng
pl
a
ne
,
thi
s
ha
ppe
ne
d
be
c
a
us
e
the
s
e
gmenta
ti
on
pr
oc
e
s
s
only
s
e
pa
r
a
te
ve
r
ti
c
a
l
a
nd
hor
izonta
l
point
c
loud
da
ta
o
r
i
e
ntation.
B
e
c
a
us
e
the
r
oo
f
will
not
hinder
platf
o
r
m
moveme
nt
a
nd
including
r
oof
a
s
obs
tac
les
r
e
s
ult
in
no
f
r
e
e
ob
s
tac
le
a
r
e
a
,
the
s
ys
tem
e
xc
lude
c
e
il
ing
point
c
loud
da
ta
f
r
om
be
ing
p
r
oc
e
s
s
e
d
by
the
s
e
gmenta
ti
on
pr
oc
e
s
s
by
f
il
ter
the
da
ta
us
ing
pa
s
s
thr
ough
method:
=
(
|
<
<
)
(
6)
w
it
h
P
a
s
the
r
e
s
ult
a
nt
point
c
loud,
a
s
a
n
indi
vidua
l
point
,
m
a
s
the
mi
nim
um
z
a
xis
va
lue
a
nd
n
a
s
ma
xim
um
z
a
xis
va
lue.
F
igur
e
1.
F
il
te
r
ing
r
e
s
ult
Evaluation Warning : The document was created with Spire.PDF for Python.
T
E
L
KO
M
NI
KA
T
e
lec
omm
un
C
omput
E
l
C
ontr
o
l
Ster
e
o
v
is
ion
-
bas
e
d
obs
tacle
av
oidance
module
on
3D
point
c
loud
data
(
E
k
o
P
ur
bo
W
ahy
ono
)
1517
3.
3.
S
e
gm
e
n
t
at
ion
Af
ter
pr
e
-
pr
oc
e
s
s
ing
s
tep,
the
point
c
l
oud
da
ta
s
e
gmente
d
to
ga
in
inf
o
r
mation
on
wa
lki
ng
plane
a
s
f
r
e
e
m
oving
r
e
gion
a
nd
obs
tac
le
da
ta.
W
it
h
th
e
de
f
ini
ti
on
o
f
wa
lki
ng
plane
a
s
a
plane
in
,
a
xis
.
B
y
de
f
ini
ng
the
p
lane
’
s
mathe
matica
l
model
a
s
:
=
+
+
+
=
0
(
7)
w
he
r
e
,
,
on
(
8
)
a
r
e
the
pa
r
a
mete
r
s
o
f
the
mathe
matica
l
model
of
the
plane
a
nd
k
,
l,
m
a
r
e
the
indepe
nde
nt
va
r
iable
s
of
the
model
to
de
f
ine
a
plane
.
T
o
de
ter
mi
ne
if
point
=
(
0
,
0
,
0
)
f
i
t
plane
=
×
+
×
+
×
+
,
the
pe
r
pe
ndicula
r
dis
t
a
nc
e
of
the
point
to
th
e
plan
e
wa
s
c
a
lcula
ted
us
ing
:
(
,
)
=
|
×
0
+
×
0
+
×
0
+
|
√
2
+
2
+
2
2
(
8)
I
f
the
point
is
be
low
the
th
r
e
s
hold
va
lue,
it
c
a
n
be
c
ons
ider
e
d
a
s
a
n
inl
ier
to
the
plane
.
B
y
us
ing
(
7
)
to
de
f
ine
the
wa
lki
n
g
plane
model
a
nd
is
olate
the
wa
lki
ng
plane
po
int
c
loud
da
ta
f
r
om
other
s
us
ing
(
8
)
r
e
s
ult
e
d
in
s
e
pa
r
a
ti
on
of
the
plane
in
li
e
r
f
r
o
m
other
point
,
th
e
s
ys
tem
ha
s
ga
ined
both
f
r
e
e
r
e
gion
in
f
or
m
of
in
li
e
r
point
s
a
nd
the
obs
tac
le
point
c
loud
da
ta
it
s
e
lf
to
p
lan
t
he
obs
tac
le
a
voidanc
e
pa
th.
R
e
s
ult
of
thi
s
s
tep
i
s
s
hown
by
F
ig
ur
e
2
.
F
igur
e
2.
S
e
gmenta
ti
on
s
tep
c
ompar
is
on
3.
4.
Occ
u
p
an
c
y
gr
id
m
ap
S
e
gmente
d
point
c
loud
da
ta
a
r
e
us
e
d
f
o
r
buil
ding
oc
c
upa
nc
y
gr
id
map
to
a
c
quir
e
obs
tac
le
pos
it
ion
r
e
lative
to
the
p
latf
o
r
m
a
nd
the
s
ubs
e
que
nt
map
c
a
n
be
r
e
c
or
de
d
f
o
r
f
utur
e
us
e
o
r
f
or
global
pa
th
pla
nning
to
a
lr
e
a
dy
r
e
c
or
de
d
a
r
e
a
.
Oc
c
upa
nc
y
gr
id
map
c
r
e
a
ted
us
ing
obs
tac
le
point
c
loud
da
ta
to
de
ter
mi
ne
if
a
gr
id
oc
c
upied
by
a
n
obs
tac
le
that
will
c
a
us
e
c
oll
is
ion
[
18]
.
E
a
c
h
c
e
ll
C
i
=
x,
y
with
a
f
ixed
s
ize
f
or
e
a
c
h
point
P
i
=
x,
y,
z
{
∀
,
=
,
∀
ℎ
(
9)
T
his
s
tep
ga
ins
a
r
e
a
s
wh
e
r
e
the
platf
or
m
c
a
n
s
a
f
e
ly
move
without
r
is
k
of
c
oll
is
ion
without
t
he
ne
e
d
of
s
ingl
e
ou
t
e
ve
r
y
obs
tac
le
a
nd
c
a
lcula
te
the
c
e
nt
r
oid
a
nd
pos
it
ion
o
f
e
a
c
h
obs
tac
le
by
mea
s
ur
ing
e
a
c
h
a
r
e
a
a
nd
c
ompar
in
g
the
wid
th
with
the
mi
nim
u
m
width
r
e
quir
e
d
by
the
platf
o
r
m.
T
he
r
e
s
ult
a
s
s
hown
on
F
ig
ur
e
3
.
3.
5.
P
at
h
p
lan
n
in
g
T
his
pa
r
t
o
f
the
s
ys
tem
ha
ve
two
pa
r
a
ll
e
l
pr
oc
e
s
s
,
the
f
i
r
s
t
one
,
pa
th
planne
r
c
r
e
a
te
a
g
lobal
s
olut
ion
f
or
the
platf
or
m
to
move
towa
r
ds
the
tar
ge
t.
T
h
e
s
e
c
ond
s
ys
tem
ha
ndle
dis
tur
ba
nc
e
f
or
c
e
on
a
platf
or
m
a
nd
output
c
or
r
e
c
ti
on
c
omm
a
nd
to
de
a
l
with
s
mall
c
ha
nge
s
in
e
nvir
onment
a
nd
une
xpe
c
ted
o
bs
tac
le.
B
a
s
e
d
on
ti
med
-
e
las
ti
c
-
ba
nd
method
by
Quinlan
[
19]
with
de
ve
lopm
e
nt
on
s
pa
r
s
e
model
[
21]
to
a
dva
nc
e
d
tr
a
jec
tor
ies
[
20]
with
M
P
C
a
ppr
oa
c
h
towa
r
ds
T
E
B
methods
[
22]
.
Ove
r
view
of
thi
s
s
tep
is
a
s
s
h
hown
on
F
ig
ur
e
4
.
T
his
method
wa
s
us
e
d
be
c
a
us
e
the
b
e
ha
vior
of
the
p
latf
or
m
is
not
de
ter
mi
ne
d
a
t
the
p
lanning
leve
l,
ye
t
loca
l
dis
tur
ba
nc
e
s
will
not
hinder
the
s
ys
tem
a
bi
li
ty
to
r
e
a
c
h
goa
ls
.
T
his
s
ys
tem
de
a
ls
with
loca
l
dis
t
ur
ba
nc
e
s
by
de
f
or
mi
ng
the
pa
th
whe
n
e
nvir
onme
nt
c
ha
nge
s
a
r
e
de
tec
ted.
De
s
pit
e
thi
s
s
tep,
the
s
y
s
tem
s
ti
ll
m
a
int
a
in
a
c
ompl
e
te
c
oll
is
ion
-
f
r
e
e
pa
th
towa
r
ds
a
de
ter
mi
ne
d
goa
l.
F
ind
the
s
hor
tes
t
pa
th
towa
r
ds
de
ter
mi
ne
d
go
a
ls
.
Evaluation Warning : The document was created with Spire.PDF for Python.
I
S
S
N
:
1693
-
6930
T
E
L
KO
M
NI
KA
T
e
lec
omm
un
Co
mput
E
l
C
ontr
o
l
,
Vol.
18
,
No
.
3
,
J
une
2020:
1514
-
1521
1518
F
igur
e
3.
Oc
c
upa
nc
y
gr
id
map
r
e
s
ult
F
igur
e
4.
P
a
th
planning
(
,
)
=
{
0
m
in
{
(
(
,
)
)
+
(
,
)
}
∞
(
10)
W
it
h
(
10)
pr
oduc
e
s
glo
ba
l
pa
th
r
e
s
ult
that
us
e
d
by
T
E
B
to
c
r
e
a
te
a
s
e
r
ies
of
wa
ypoint
s
.
T
he
s
hor
tes
t
p
a
th,
take
n
a
s
a
s
e
r
ies
of
a
f
ixed
number
of
wa
ypoint
with
to
l
e
r
a
nc
e
r
a
dius
whic
h
c
ompr
is
e
.
=
(
)
(
11)
Ar
r
a
y
of
wa
ypoint
c
a
n
be
de
not
e
d
a
s
:
=
{
}
=
1
,
2
,
…
.
,
(
12)
Evaluation Warning : The document was created with Spire.PDF for Python.
T
E
L
KO
M
NI
KA
T
e
lec
omm
un
C
omput
E
l
C
ontr
o
l
Ster
e
o
v
is
ion
-
bas
e
d
obs
tacle
av
oidance
module
on
3D
point
c
loud
data
(
E
k
o
P
ur
bo
W
ahy
ono
)
1519
T
his
s
ys
tem
doe
s
not
pur
s
ue
f
a
s
tes
t
tr
a
jec
tor
y
a
nd
in
a
c
ontext
of
t
im
e
d
e
las
ti
c
ba
nd
,
it
only
o
pti
mi
z
e
s
the
loca
ti
on
of
int
e
r
media
te
wa
ypoint
a
s
the
s
ys
tem
doe
s
not
ha
ve
a
bounda
r
y
of
f
inal
platf
or
m
s
tate
,
the
r
e
f
o
r
e
the
c
ha
nge
in
ti
me
f
or
e
a
c
h
c
ons
e
c
uti
ve
wa
ypoint
(
∆
)
a
r
e
f
ixed
a
nd
c
a
n
be
de
noted
a
s
(
13
)
.
=
{
Δ
}
=
1
,
2
,
…
,
−
1
(
13)
T
im
e
d
e
las
ti
c
ba
nd
f
unc
ti
on
c
ons
is
ted
of
(
14)
.
∶
=
(
,
)
(
14)
Optim
a
l
ba
nd
c
a
lcula
ti
on
is
c
a
lcula
ted
by
mi
nim
izi
ng
the
objec
ti
ve
f
unc
ti
on
.
(
)
=
∑
Γ
k
(
)
(
15)
W
it
h
a
n
a
r
r
a
y
of
wa
ypoint
s
P
i
f
r
om
pa
th
planni
ng
r
e
s
ult
,
the
s
ys
tem
c
ompar
e
c
ur
r
e
nt
platf
or
m
pos
it
ion
a
nd
or
ienta
ti
on
inf
or
mation
with
wa
ypoint
s
c
oor
di
na
te
a
nd
upda
te
wa
ypoint
s
a
r
r
a
y
w
he
n
the
s
ys
tem
a
c
hieve
wa
ypoint
s
or
if
the
s
ys
tem
s
tr
a
y
f
r
om
g
lobal
pa
t
h
a
nd
pr
e
vious
wa
ypoint
a
r
r
a
y.
4.
RE
SU
L
T
S
A
ND
AN
AL
YSI
S
T
he
r
e
s
ult
of
voxe
l
gr
id
f
il
ter
ing
with
0
.
05
m
s
ize
voxe
l,
the
r
e
s
ult
a
nt
c
loud
s
hows
a
dr
op
of
98.
2%
in
point
c
loud
c
ount
f
r
om
921
600
point
to
16664
p
oint
.
W
it
h
on
ly
1.
8
%
of
the
da
ta
r
e
mains
while
maintains
s
im
il
a
r
c
ha
r
a
c
ter
is
ti
c
s
with
the
r
a
w
da
ta.
T
his
r
e
s
ult
s
hows
that
voxe
l
g
r
id
p
r
e
s
e
r
ve
d
unde
r
lyi
n
g
plane
of
the
r
a
w
point
c
loud
da
ta
while
r
e
duc
ing
po
int
c
loud
da
ta
s
ize
a
s
s
hown
on
F
ig
ur
e
1.
F
i
lt
e
r
ing
s
ub
-
s
y
s
tem
take
s
86
ms
a
ve
r
a
ge
e
xe
c
uti
on
ti
me
with
720p
r
a
w
da
ta
s
ize
with
a
n
a
c
c
e
ptable
r
e
s
ult
f
o
r
s
e
gmenta
ti
on
while
r
e
duc
ing
the
da
ta
s
ize
a
nd
r
e
moval
o
f
unne
c
e
s
s
a
r
y
da
ta
p
r
ove
n
by
the
s
uc
c
e
s
s
on
s
e
gmenta
ti
on
s
tep
a
s
s
hown
on
2a
,
f
a
s
ter
e
xe
c
uti
on
ti
me
lea
ds
to
mo
r
e
los
s
o
f
de
tails
,
whic
h
r
e
s
ult
in
f
a
il
ur
e
of
s
e
gmenta
ti
on.
E
xe
c
uti
on
ti
me
f
or
f
il
te
r
ing
s
ubs
ys
tem
c
a
n
be
im
pr
ove
d
by
t
he
us
a
ge
of
pa
r
a
ll
e
l
c
omput
ing
f
or
e
a
c
h
f
il
te
r
ing
model
f
or
f
a
s
ter
e
xe
c
uti
on.
S
im
il
a
r
method
c
a
n
be
a
pp
li
e
d
f
or
s
e
gmenta
ti
on
s
ubs
ys
tem
whic
h
take
s
260
ms
a
ve
r
a
ge
e
xe
c
uti
on
ti
me.
S
hor
ter
e
xe
c
uti
on
ti
me
lea
ds
to
f
a
s
ter
map
upda
te
a
nd
f
a
s
ter
de
c
is
ion
r
e
s
ult
.
S
e
gmenta
ti
on
r
e
s
ult
e
d
in
a
ll
point
c
loud
da
ta
with
ve
r
ti
c
a
l
plan
e
or
i
e
ntation
a
r
e
s
e
pa
r
a
ted
f
r
om
the
poi
nt
c
loud
da
ta
with
hor
izonta
l
plane
or
ienta
ti
on
.
C
ompar
e
d
to
c
olo
ur
ba
s
e
d
point
c
loud
c
lus
ter
ing,
the
r
e
s
ult
s
ho
ws
that
the
c
olour
ba
s
e
d
s
e
gmenta
ti
on
r
e
s
ult
a
r
e
s
ti
ll
de
tec
ti
ng
s
ome
ve
r
ti
c
a
l
objec
t
a
s
a
s
a
me
gr
oup
a
s
the
h
or
izonta
l
wa
lki
ng
plane
,
while
th
e
s
ys
tem’
s
s
e
gmenta
ti
on
ba
s
e
d
on
e
a
c
h
point
or
ienta
ti
on
s
e
pa
r
a
te
a
ll
point
s
with
ve
r
ti
c
a
l
plane
or
ienta
ti
on.
De
s
pit
e
s
ys
tem
s
uc
c
e
s
s
of
s
e
gme
ntation
be
twe
e
n
hor
izonta
l
wa
lki
ng
plane
a
nd
obs
tac
le
da
ta,
the
s
ys
tem
f
a
il
to
pr
oc
e
s
s
s
ome
da
ta
whe
n
s
ter
e
o
m
a
tchin
g
f
a
il
ur
e
e
xis
t,
thi
s
c
ha
r
a
c
ter
is
ti
c
s
make
ne
ighbour
ing
pixel
with
a
s
igni
f
ica
nt
dis
tanc
e
in
3D
e
nvi
r
onment
r
e
s
ult
in
a
n
a
lm
os
t
ho
r
izonta
l
g
r
a
dient,
the
r
e
f
or
e
th
e
s
ys
tem
s
e
gmenta
ti
on
doe
s
not
s
e
gment
thi
s
a
r
e
a
.
T
he
c
om
pa
r
is
on
be
twe
e
n
plane
ba
s
e
d
s
e
gmenta
ti
on
a
nd
c
ol
our
ba
s
e
d
s
e
gmenta
ti
on
a
r
e
s
hown
on
F
ig
ur
e
2
o
c
c
upa
nc
y
gr
i
d
map
r
e
s
ult
e
d
in
in
f
or
mation
of
e
a
c
h
obs
tac
le
da
ta
pos
it
ion
ther
e
f
or
e
s
a
f
e
movi
ng
a
r
e
a
a
r
e
a
c
quir
e
d
whe
r
e
no
obs
tac
le
a
r
e
pr
e
s
e
nt.
T
he
s
ys
tem
will
c
a
lcu
late
the
s
a
f
e
a
r
e
a
a
r
ound
t
he
obs
ta
c
le
gr
id
to
a
c
c
ount
f
or
platf
or
m
s
ize
a
nd
odometr
y
dr
i
f
t.
All
thes
e
inf
or
mation
a
r
e
s
uc
c
e
s
s
f
ull
y
r
e
c
or
de
d
pr
ove
n
by
the
a
bil
it
y
o
f
the
s
ys
tem
to
plan
na
vigation
pa
th
thr
ough
a
lr
e
a
dy
mappe
d
a
r
e
a
F
ig
ur
e
3
s
how
that
whe
r
e
ob
s
tac
le
point
c
lou
d
e
xis
t,
the
c
or
r
e
s
ponding
c
e
ll
s
s
how
a
s
blac
k
c
e
ll
s
with
no
f
a
il
ur
e
f
or
a
ll
obs
tac
le
point
inf
or
mation
.
P
a
th
planning
r
e
s
ult
e
d
the
s
hor
tes
t
pa
th
in
r
e
ga
r
d
to
platf
or
m
s
ize
to
a
void
c
oll
is
ion
with
obs
tac
le
a
r
e
a
a
nd
modi
f
y
ini
ti
a
l
pa
th
to
tar
ge
t
to
a
void
a
n
obs
tac
le
a
nd
a
da
pt
to
ne
w
e
nvi
r
onment
da
ta
thi
s
is
pr
ove
n
by
movi
ng
the
c
a
mer
a
a
c
c
or
ding
to
pa
th
planning
r
e
s
ult
e
d
in
a
r
r
ivi
ng
to
tar
ge
t
c
o
or
dinate
without
a
ny
c
oll
is
ion
s
hown
on
F
ig
ur
e
5
(
a
)
a
nd
F
ig
ur
e
5
(
b
)
s
hows
the
s
a
me
goa
l
pos
it
ion
with
upda
ted
e
nvir
onment
in
f
or
mation
a
n
d
platf
o
r
m
pos
it
ion.
F
ig
ur
e
5
(
b
)
s
hows
the
s
ys
tem
upda
te
the
pa
th
pla
n
towa
r
ds
the
goa
l
whe
n
the
platf
o
r
m
is
moved
ou
ts
ide
the
planne
d
pa
th
a
nd
ne
w
obs
tac
le
inf
o
r
mation
is
a
c
quir
e
d
f
r
o
m
the
e
nvir
onment
a
nd
upda
te
pa
th
whe
n
the
platf
or
m
moved
ins
ide
planne
d
p
a
th
F
ig
ur
e
5
(
c
)
.
T
he
s
ys
tem
only
pr
oduc
e
goa
l
a
r
r
a
y
pos
it
ion
a
s
lo
ng
a
s
the
a
r
e
a
width
f
o
r
the
pa
th
planning
a
r
e
wide
r
than
the
r
e
quir
e
d
wid
th
of
the
p
latf
or
m
a
nd
will
ter
mi
na
te
the
pa
th
planning
whe
n
the
r
e
qui
r
e
d
c
ondit
ion
a
r
e
not
met
,
t
he
s
ys
tem
will
pr
odu
c
e
pa
th
a
s
c
los
e
a
s
the
ta
r
ge
t
a
s
pos
s
ibl
e
whe
n
the
c
ondit
ion
a
r
e
met,
s
hown
on
F
ig
u
r
e
5
(
d
)
,
f
a
r
thes
t
a
r
r
ow
’
s
pos
it
ion
ha
ve
a
r
e
a
width
of
m
or
e
than
0.
7
m,
o
r
s
hown
a
t
the
F
ig
u
r
e
5
(
d
)
a
s
the
blue
c
ir
c
le
diame
ter
a
r
e
7
gr
id
with
e
a
c
h
gr
id
ha
ve
the
width
of
0.
1
m,
while
f
u
r
ther
pos
s
ibl
e
pos
it
ion
did
not
met
t
his
r
e
quir
e
ment
,
ther
e
f
or
e
the
s
ys
tem
a
r
e
f
ul
f
il
li
ng
t
he
tar
ge
t
of
c
r
e
a
ti
ng
s
a
f
e
pa
th
without
pos
s
ibl
e
c
oll
is
ion.
Evaluation Warning : The document was created with Spire.PDF for Python.
I
S
S
N
:
1693
-
6930
T
E
L
KO
M
NI
KA
T
e
lec
omm
un
Co
mput
E
l
C
ontr
o
l
,
Vol.
18
,
No
.
3
,
J
une
2020:
1514
-
1521
1520
(
a
)
(
b)
(
c
)
(
d)
F
igur
e
5.
Obs
tac
le
a
voidanc
e
pa
th
modi
f
ica
ti
on
,
(
a
)
ini
ti
a
l
pa
th
da
ta
,
(
b
)
a
f
ter
e
nvir
onment
a
nd
pos
it
i
on
upda
te
(
c
)
Af
ter
pos
it
ion
upda
te
,
(
d)
M
ini
mum
wid
th
r
e
quir
e
ment
not
met
4.
CONC
L
USI
ON
F
il
ter
ing
s
ub
-
s
ys
tem
pe
r
f
or
m
a
mi
c
a
bly
with
tot
a
l
e
xe
c
uti
on
ti
me
of
96ms
on
a
v
e
r
a
ge
.
T
his
r
e
s
ult
of
e
xe
c
uti
on
ti
me
is
done
on
720p
r
a
w
point
c
lo
ud
da
ta
s
ize
with
0.
05
m
voxe
l
s
ize
a
nd
l
im
it
pa
s
s
thr
ough
of
1.
2m
a
bove
g
r
ound.
Al
l
s
ub
-
s
ub
-
s
ys
tem
of
f
il
ter
i
ng
s
tep
pe
r
f
or
m
a
s
int
e
nde
d.
Obs
tac
le
E
xtr
a
c
ti
on
s
u
b
-
s
y
s
tem
e
xtr
a
c
t
a
ll
obs
tac
le
da
ta
a
nd
mappe
d
with
a
ppr
opr
iate
r
e
s
ult
c
ompar
e
d
to
the
r
e
a
l
e
nvir
onment
.
W
hil
e
s
e
gmenta
ti
on
pr
oc
e
s
s
ha
ve
a
point
of
f
a
il
u
r
e
whe
n
the
r
e
s
ult
a
nt
of
s
ter
e
o
matc
hing
did
not
p
r
oduc
e
t
he
s
a
me
r
e
s
ult
a
s
the
r
e
a
l
c
ondit
ion
whic
h
r
e
s
ult
in
s
e
gment
a
ti
on
f
a
il
u
r
e
wh
il
e
oc
c
upa
nc
y
gr
id
map
pr
oduc
e
s
t
he
e
xa
c
t
r
e
s
ult
of
e
ve
r
y
s
e
gmente
d
obs
tac
le
da
ta.
P
a
th
planning
s
ub
-
s
ys
t
e
m
pr
oduc
e
s
a
ppr
opr
iate
pa
th
a
nd
c
or
r
e
c
ti
on
c
omm
a
nd
whe
n
de
a
li
ng
with
e
nvi
r
onment
a
nd
platf
or
m
upda
te
while
ke
e
ping
t
he
r
e
c
or
de
d
da
ta
f
or
f
utur
e
us
e
a
n
d
f
or
global
na
vigation.
T
his
s
ys
tem
did
not
de
a
l
with
global
pa
th
pla
nning
with
pr
e
vious
ly
mappe
d
e
nvir
onment
a
n
d
ha
s
no
s
upe
r
pos
it
ion
odometr
y
pr
e
s
e
nt.
T
his
s
ys
tem
ha
s
pr
ovided
a
c
a
pa
bil
it
y
o
f
obs
tac
le
a
voidanc
e
ba
s
e
d
on
3D
point
c
loud
da
ta
int
e
r
pr
e
ted
by
e
qua
ti
on
10
to
f
or
m
ba
s
ic
int
e
ll
igenc
e
ba
s
e
d
on
T
E
B
model
to
de
a
l
with
c
oll
i
s
ionf
r
e
e
pa
th
planning
a
nd
de
a
ls
with
platf
or
m
de
viation.
RE
F
E
RE
NC
E
S
[1
]
K
.
Sab
e
,
et
al
.,
“O
b
s
t
a
cl
e
av
o
i
d
an
ce
an
d
p
at
h
p
l
a
n
n
i
n
g
fo
r
h
u
ma
n
o
i
d
ro
b
o
t
s
u
s
i
n
g
s
t
ere
o
v
i
s
i
o
n
,
”
IE
E
E
In
t
e
r
n
a
t
i
o
n
a
l
Co
n
f
e
r
en
ce
o
n
R
o
b
o
t
i
cs
a
n
d
A
u
t
o
m
a
t
i
o
n
,
v
o
l
.
1
,
p
p
.
5
9
2
–
5
9
7
,
2
0
0
4
.
[2
]
S.
K
u
mar,
D
.
G
u
p
t
a,
a
n
d
S.
Y
a
d
av
,
“
S
en
s
o
r
f
u
s
i
o
n
o
f
l
as
er
&
s
t
ereo
v
i
s
i
o
n
camera
f
o
r
d
e
p
t
h
es
t
i
ma
t
i
o
n
an
d
o
b
s
t
a
cl
e
av
o
i
d
a
n
ce,
”
In
t
e
r
n
a
t
i
o
n
a
l
Jo
u
r
n
a
l
o
f
Co
m
p
u
t
e
r
A
p
p
l
i
c
a
t
i
o
n
s
,
v
o
l
.
1
,
n
o
.
2
6
,
p
p
.
2
2
–
27,
2
0
1
0
.
[3
]
P.
S.
Sh
arma
an
d
D
.
N
.
G
.
Ch
i
t
al
i
y
a,
“O
b
s
t
ac
l
e
av
o
i
d
a
n
ce
u
s
i
n
g
s
t
ere
o
v
i
s
i
o
n
:
a
s
u
r
v
ey
,
”
I
n
t
e
r
n
a
t
i
o
n
a
l
Jo
u
r
n
a
l
o
f
In
n
o
va
t
i
ve
R
e
s
ea
r
ch
i
n
Co
m
p
u
t
e
r
a
n
d
Co
m
m
u
n
i
ca
t
i
o
n
E
n
g
i
n
ee
r
i
n
g
,
v
o
l
.
3
,
n
o
.
1
,
p
p
.
2
4
-
2
9
,
2
0
1
5
.
[4
]
A
.
H
i
d
al
g
o
-
Pa
n
i
a
g
u
a,
et
a
l
.,
“A
c
o
mp
ara
t
i
v
e
s
t
u
d
y
o
f
p
ara
l
l
e
l
R
A
N
S
A
C
i
m
p
l
eme
n
t
a
t
i
o
n
s
i
n
3
D
s
p
a
ce,
”
In
t
.
J.
P
a
r
a
l
l
e
l
P
r
o
g
r
a
m
.
,
v
o
l
.
4
3
,
n
o
.
5
,
p
p
.
7
0
3
–
7
2
0
,
2
0
1
5
.
[5
]
D
.
Pramad
i
h
an
t
o
,
e
t
al
.,
“Merg
i
n
g
o
f
d
ep
t
h
i
mag
e
b
et
w
e
en
s
t
ereo
camera
a
n
d
s
t
r
u
ct
u
re
s
e
n
s
o
r
o
n
ro
b
o
t
fl
o
w
v
i
s
i
o
n
,
”
In
t
.
J
.
o
n
A
d
va
n
.
S
ci
.
,
E
n
g
i
n
ee
.
a
n
d
In
f
o
r
m.
Tech
.
,
v
o
l
.
7
,
n
o
.
3
,
p
p
.
1
0
1
4
-
1
0
2
5
,
2
0
1
7
.
[6
]
S.
A
.
W
as
k
i
t
h
o
,
et
a
l
.,
“Fl
o
W
v
i
s
i
o
n
:
D
e
p
t
h
i
mag
e
en
h
an
ceme
n
t
b
y
c
o
mb
i
n
i
n
g
s
t
er
eo
RG
B
-
d
ep
t
h
s
en
s
o
r,
”
In
t
e
r
n
a
t
i
o
n
a
l
Co
n
f
er
e
n
ce
o
n
Kn
o
w
l
ed
g
e
Cr
e
a
t
i
o
n
a
n
d
I
n
t
e
l
l
i
g
e
n
t
Co
m
p
u
t
i
n
g
,
p
p
.
1
8
2
–
1
8
7
,
2
0
1
6
.
Evaluation Warning : The document was created with Spire.PDF for Python.
T
E
L
KO
M
NI
KA
T
e
lec
omm
un
C
omput
E
l
C
ontr
o
l
Ster
e
o
v
is
ion
-
bas
e
d
obs
tacle
av
oidance
module
on
3D
point
c
loud
data
(
E
k
o
P
ur
bo
W
ahy
ono
)
1521
[7
]
S.
G
h
o
s
h
an
d
J
.
Bi
s
w
a
s
,
“J
o
i
n
t
p
ercep
t
i
o
n
a
n
d
p
l
a
n
n
i
n
g
f
o
r
effi
c
i
en
t
o
b
s
t
ac
l
e
av
o
i
d
an
ce
u
s
i
n
g
s
t
ere
o
v
i
s
i
o
n
,
”
IE
E
E
/
RSJ
I
n
t
er
n
at
i
o
n
al
C
o
n
fere
n
ce
o
n
I
n
t
e
l
l
i
g
e
n
t
R
o
b
o
t
s
an
d
Sy
s
t
em
s
,
p
p
.
1
0
2
6
–
1
0
3
1
,
2
0
1
7
.
[8
]
R.
B.
Ru
s
u
an
d
S.
Co
u
s
i
n
s
,
“3
D
i
s
h
ere:
P
o
i
n
t
Cl
o
u
d
L
i
b
rar
y
(PCL
),
”
IE
E
E
I
n
t
e
r
n
a
t
i
o
n
a
l
Co
n
f
e
r
en
ce
o
n
R
o
b
o
t
i
c
s
a
n
d
A
u
t
o
m
a
t
i
o
n
,
p
p
.
1
–
4
,
2
0
1
1
.
[9
]
S.
A
.
W
as
k
i
t
h
o
,
e
t
al
.,
“Po
i
n
t
cl
o
u
d
l
i
b
rar
y
(PCL
):
Mo
d
u
l
e
s
amp
l
e
co
n
s
e
n
s
u
s
,
”
In
t
er
n
a
t
i
o
n
a
l
C
o
n
f
er
e
n
ce
o
n
Kn
o
wl
e
d
g
e
Cr
ea
t
i
o
n
a
n
d
In
t
e
l
l
i
g
e
n
t
C
o
m
p
u
t
i
n
g
,
p
p
.
1
8
2
–
1
8
7
,
2
0
1
6
.
[1
0
]
A
.
H
i
d
a
l
g
o
-
Pan
i
ag
u
a,
et
al
.,
“A
co
mp
arat
i
v
e
s
t
u
d
y
o
f´
Paral
l
e
l
RA
N
SA
C
i
m
p
l
eme
n
t
a
t
i
o
n
s
i
n
3
D
s
p
ace,
”
In
t
er
n
a
t
i
o
n
a
l
Jo
u
r
n
a
l
o
f
P
a
r
a
l
l
el
P
r
o
g
r
a
m
m
i
n
g
,
v
o
l
.
4
3
,
n
o
.
5
,
p
p
.
7
0
3
–
7
2
0
,
2
0
1
5
.
[1
1
]
R.
B.
Ru
s
u
an
d
S.
Co
u
s
i
n
s
,
“3
D
i
s
h
ere:
P
o
i
n
t
Cl
o
u
d
L
i
b
rar
y
(PCL
),
”
IE
E
E
I
n
t
e
r
n
a
t
i
o
n
a
l
Co
n
f
e
r
en
ce
o
n
R
o
b
o
t
i
c
s
a
n
d
A
u
t
o
m
a
t
i
o
n
,
v
o
l
.
4
2
,
n
o
.
3
,
p
p
.
1
-
4
,
2
0
1
1
.
[1
2
]
L
.
V
i
n
et
an
d
A
.
Z
h
ed
a
n
o
v
,
“A
mi
s
s
i
n
g
’
fami
l
y
o
f
c
l
as
s
i
ca
l
o
rt
h
o
g
o
n
al
p
o
l
y
n
o
m
i
al
s
,
”
Jo
u
r
n
a
l
o
f
P
h
ys
i
cs
A
:
M
a
t
h
e
m
a
t
i
c
a
l
a
n
d
T
h
eo
r
et
i
ca
l
,
v
o
l
.
4
4
,
n
o
.
8
,
p
p
.
0
8
5
2
0
1
,
2
0
1
1
.
[1
3
]
C.
Mo
ren
o
an
d
M.
L
i
,
“A
co
mp
arat
i
v
e
s
t
u
d
y
o
f
fi
l
t
eri
n
g
met
h
o
d
s
fo
r
p
o
i
n
t
cl
o
u
d
s
i
n
real
-
t
i
me
v
i
d
e
o
s
t
ream
i
n
g
,
”
Lect
u
r
e
No
t
e
s
i
n
E
n
g
i
n
eer
i
n
g
a
n
d
Co
m
p
u
t
e
r
S
c
i
en
ce,
v
o
l
.
1
,
p
p
.
3
8
8
-
3
9
3
,
2
0
1
6
.
[1
4
]
A
.
N
g
u
y
en
an
d
B.
L
e,
“3
D
p
o
i
n
t
cl
o
u
d
s
e
g
men
t
at
i
o
n
:
A
s
u
r
v
ey
,
”
6
t
h
IE
E
E
Co
n
f
e
r
en
ce
o
n
R
o
b
o
t
i
cs
,
A
u
t
o
m
a
t
i
o
n
a
n
d
M
ech
a
t
r
o
n
i
c
s
(R
A
M
),
p
p
.
2
2
5
–
2
3
0
,
2
0
1
3
.
[1
5
]
B.
Su
n
an
d
P.
Mo
rd
o
h
a
i
,
“O
ri
e
n
t
e
d
p
o
i
n
t
s
am
p
l
i
n
g
fo
r
p
l
a
n
e
d
et
ec
t
i
o
n
i
n
u
n
o
r
g
an
i
ze
d
p
o
i
n
t
cl
o
u
d
s
,
”
In
t
er
n
a
t
i
o
n
a
l
Co
n
f
er
e
n
ce
o
n
R
o
b
o
t
i
c
s
a
n
d
A
u
t
o
m
a
t
i
o
n
,
p
p
.
2
9
1
7
–
2
9
2
3
,
2
0
1
9
.
[1
6
]
F.
A
.
L
i
mb
erg
er
an
d
M.
M.
O
l
i
v
ei
ra,
“Real
-
t
i
me
d
et
ect
i
o
n
o
f
p
l
an
ar
reg
i
o
n
s
i
n
u
n
o
rg
a
n
i
ze
d
p
o
i
n
t
cl
o
u
d
s
,
”
P
a
t
t
er
n
R
eco
g
n
i
t
i
o
n
,
v
o
l
.
4
8
,
n
o
.
6
,
p
p
.
2
0
4
3
–
2
0
5
3
,
2
0
1
5
.
[1
7
]
X
.
L
i
n
,
J
.
R.
Cas
as
,
an
d
M.
Pard
as
,
“3
D
p
o
i
n
t
cl
o
u
d
s
eg
men
t
a
t
i
o
n
u
s
i
n
g
a
fu
l
l
y
co
n
n
ec
t
ed
co
n
d
i
t
i
o
n
a
l
ran
d
o
m
fi
el
d
,
”
2
5
t
h
E
u
r
o
p
e
a
n
S
i
g
n
a
l
P
r
o
ce
s
s
i
n
g
Co
n
f
e
r
en
ce,
v
o
l
.
2
0
1
7
,
p
p
.
6
6
–
70
,
2
0
1
7
.
[1
8
]
Y
.
L
i
,
an
d
Y
.
Ru
i
ch
e
k
,
“Bu
i
l
d
i
n
g
v
ari
a
b
l
e
res
o
l
u
t
i
o
n
o
ccu
p
a
n
cy
g
r
i
d
map
fro
m
s
t
ere
o
s
c
o
p
i
c
s
y
s
t
em
&#
x
2
0
1
4
;
A
q
u
a
d
t
ree
b
a
s
ed
a
p
p
r
o
ach
,
”
IE
E
E
I
n
t
e
l
l
i
g
e
n
t
V
e
h
i
c
l
es
S
y
m
p
o
s
i
u
m
,
p
p
.
7
4
4
–
7
4
9
,
2
0
1
3
.
[1
9
]
E
.
H
o
rv
a
t
h
an
d
C.
R.
Po
zn
a,
“Pro
b
ab
i
l
i
s
t
i
c
o
ccu
p
an
c
y
g
ri
d
m
ap
b
u
i
l
d
i
n
g
fo
r
N
eo
b
o
t
i
x
MP5
0
0
ro
b
o
t
,
”
1
3
t
h
W
o
r
ks
h
o
p
o
n
P
o
s
i
t
i
o
n
i
n
g
,
Na
v
i
g
a
t
i
o
n
a
n
d
Co
m
m
u
n
i
ca
t
i
o
n
s
,
p
p
.
1
–
4
,
2
0
1
6
.
[2
0
]
S.
Q
u
i
n
l
a
n
,
an
d
O
.
K
h
a
t
i
b
,
“E
l
as
t
i
c
b
a
n
d
s
:
c
o
n
n
ect
i
n
g
p
at
h
p
l
a
n
n
i
n
g
an
d
c
o
n
t
ro
l
,
”
P
r
o
cee
d
i
n
g
s
IE
E
E
I
n
t
er
n
a
t
i
o
n
a
l
Co
n
f
er
e
n
ce
o
n
R
o
b
o
t
i
c
s
a
n
d
A
u
t
o
m
a
t
i
o
n
,
p
p
.
8
0
2
–
8
0
7
,
2
0
1
6
.
[2
1
]
M.
K
el
l
er,
e
t
a
l
.,
“Pl
an
n
i
n
g
o
f
o
p
t
i
ma
l
co
l
l
i
s
i
o
n
a
v
o
i
d
a
n
ce
t
raj
ec
t
o
r
i
es
w
i
t
h
t
i
med
e
l
as
t
i
c
b
a
n
d
s
,
”
IF
A
C
P
r
o
cee
d
i
n
g
s
V
o
l
u
m
es
,
v
o
l
.
4
7
,
n
o
.
3
,
p
p
.
9
8
2
2
–
9
8
2
7
,
2
0
1
4
.
[2
2
]
C.
Ro
s
man
n
,
et
al
.
,
“E
ffi
ci
e
n
t
t
ra
j
ect
o
ry
o
p
t
i
m
i
zat
i
o
n
u
s
i
n
g
a
s
p
ar
s
e
mo
d
el
,
”
2
0
1
3
E
u
r
o
p
e
a
n
Co
n
f
e
r
e
n
ce
o
n
M
o
b
i
l
e
R
o
b
o
t
s
,
p
p
.
1
3
8
–
1
4
3
,
2
0
1
3
.
[2
3
]
C.
Ro
s
man
n
,
F.
H
o
ffma
n
n
,
an
d
T
.
Ber
t
ram,
“T
i
me
d
-
E
l
as
t
i
c
-
Ban
d
s
f
o
r
t
i
me
-
o
p
t
i
mal
p
o
i
n
t
t
o
-
p
o
i
n
t
n
o
n
l
i
n
ear
m
o
d
e
l
p
red
i
ct
i
v
e
co
n
t
r
o
l
,
”
E
u
r
o
p
e
a
n
C
o
n
t
r
o
l
Co
n
f
e
r
en
ce,
p
p
.
3
3
5
2
–
3
3
5
7
,
2
0
1
5
.
[2
4
]
F.
U
l
b
ri
ch
,
et
al
.
,
“St
a
b
l
e
t
i
med
e
l
as
t
i
c
b
a
n
d
s
w
i
t
h
l
o
o
s
e
en
d
s
,
”
IE
E
E
In
t
el
l
i
g
en
t
V
e
h
i
c
l
es
S
ym
p
o
s
i
u
m
,
p
p
.
1
8
6
–
1
9
2
,
2
0
1
7
.
[2
5
]
B.
Mag
y
ar
,
et
al
.
,
“T
i
med
-
el
as
t
i
c
b
an
d
s
fo
r
man
i
p
u
l
a
t
i
o
n
mo
t
i
o
n
p
l
an
n
i
n
g
,
”
IE
E
E
R
o
b
o
t
i
c
s
a
n
d
A
u
t
o
m
a
t
i
o
n
Let
t
er
s
,
v
o
l
.
4
,
n
o
.
4
,
p
p
.
3
5
1
3
–
3
5
2
0
,
2
0
1
9
.
[2
6
]
C.
Mo
ren
o
an
d
M.
L
i
,
“A
co
mp
arat
i
v
e
s
t
u
d
y
o
f
fi
l
t
eri
n
g
met
h
o
d
s
fo
r
p
o
i
n
t
cl
o
u
d
s
i
n
real
-
t
i
me
v
i
d
e
o
s
t
ream
i
n
g
,
”
In
t
e
r
n
a
t
i
o
n
a
l
Co
n
f
er
e
n
ce
o
n
Kn
o
w
l
ed
g
e
Cr
e
a
t
i
o
n
a
n
d
I
n
t
e
l
l
i
g
e
n
t
Co
m
p
u
t
i
n
g
,
p
p
.
1
8
2
–
1
87
,
2
0
1
6
.
[2
7
]
J
.
Bed
k
o
w
s
k
i
,
et
al
.,
“O
p
en
s
o
u
rc
e
ro
b
o
t
i
c
3
D
map
p
i
n
g
framew
o
r
k
w
i
t
h
RO
S
r
o
b
o
t
o
p
era
t
i
n
g
s
y
s
t
em,
PCL
Po
i
n
t
Cl
o
u
d
L
i
b
rar
y
an
d
Cl
o
u
d
Co
m
p
are,
”
In
t
e
r
n
a
t
i
o
n
a
l
Co
n
f
er
e
n
ce
o
n
E
l
ec
t
r
i
ca
l
E
n
g
i
n
ee
r
i
n
g
a
n
d
In
f
o
r
m
a
t
i
cs
,
p
p
.
6
4
4
–
6
4
9
,
2
0
1
5
.
Evaluation Warning : The document was created with Spire.PDF for Python.