I
AE
S In
t
er
na
t
io
na
l J
o
urna
l o
f
Art
if
icia
l In
t
ellig
ence
(
I
J
-
AI
)
Vo
l.
1
5
,
No
.
4
,
A
u
g
u
s
t 2
0
2
6
,
p
p
.
3
8
3
6
~
3
8
5
2
I
SS
N:
2
2
5
2
-
8
9
3
8
,
DOI
: 1
0
.
1
1
5
9
1
/ijai.v
15
.i
4
.
p
p
3
8
3
6
-
3
8
5
2
3836
J
o
ur
na
l ho
m
ep
a
g
e
:
h
ttp
:
//ij
a
i
.
ia
esco
r
e.
co
m
Experime
ntal v
a
li
da
tion o
f
v
isio
n
-
b
a
sed hy
brid nav
ig
a
tion for
diff
er
ential
-
d
riv
e
ro
bo
ts
T
ha
nh
Q
ua
ng
Duc
L
e
1
,
T
ro
ng
H
ieu Lu
u
2
1
F
a
c
u
l
t
y
o
f
El
e
c
t
r
i
c
a
l
a
n
d
El
e
c
t
r
o
n
i
c
s
En
g
i
n
e
e
r
i
n
g
,
V
i
n
h
L
o
n
g
U
n
i
v
e
r
s
i
t
y
o
f
Te
c
h
n
o
l
o
g
y
E
d
u
c
a
t
i
o
n
,
V
i
n
h
L
o
n
g
,
V
i
e
t
n
am
2
C
o
l
l
e
g
e
o
f
E
n
g
i
n
e
e
r
i
n
g
,
C
a
n
T
h
o
U
n
i
v
e
r
si
t
y
,
C
a
n
T
h
o
,
V
i
e
t
n
am
Art
icle
I
nfo
AB
S
T
RAC
T
A
r
ticle
his
to
r
y:
R
ec
eiv
ed
No
v
1
1
,
2
0
2
5
R
ev
is
ed
J
u
l 1
1
,
2
0
2
6
Acc
ep
ted
J
u
l 2
1
,
2
0
2
6
Re
li
a
b
le
m
o
b
il
e
-
ro
b
o
t
d
e
p
lo
y
m
e
n
t
re
q
u
ires
m
a
p
p
in
g
,
l
o
c
a
li
z
a
ti
o
n
,
g
l
o
b
a
l
p
lan
n
in
g
,
a
n
d
re
a
l
-
ti
m
e
o
b
sta
c
le
a
v
o
id
a
n
c
e
to
o
p
e
ra
te
c
o
n
siste
n
tl
y
u
n
d
e
r
se
n
so
r
n
o
ise
a
n
d
p
h
y
sic
a
l
c
o
n
stra
in
ts.
T
h
is
st
u
d
y
p
re
se
n
ts
a
v
is
io
n
-
b
a
se
d
h
y
b
rid
n
a
v
i
g
a
ti
o
n
fra
m
e
wo
rk
f
o
r
a
d
iffere
n
ti
a
l
-
d
r
iv
e
m
o
b
il
e
r
o
b
o
t
i
n
a
wa
re
h
o
u
se
-
li
k
e
e
n
v
ir
o
n
m
e
n
t
.
Th
e
m
a
in
c
o
n
tri
b
u
ti
o
n
li
e
s
in
th
e
sy
s
tem
-
lev
e
l
in
teg
ra
ti
o
n
a
n
d
e
x
p
e
rime
n
tal
v
a
li
d
a
ti
o
n
o
f
e
sta
b
li
s
h
e
d
tec
h
n
i
q
u
e
s
r
a
th
e
r
th
a
n
th
e
d
e
v
e
l
o
p
m
e
n
t
o
f
a
n
e
w
sta
n
d
a
lo
n
e
n
a
v
ig
a
ti
o
n
a
l
g
o
rit
h
m
.
A
c
e
il
in
g
-
m
o
u
n
te
d
c
a
m
e
ra
c
o
n
v
e
rts
t
o
p
-
v
ie
w
ima
g
e
s
in
to
a
n
o
c
c
u
p
a
n
c
y
-
g
ri
d
m
a
p
a
n
d
wo
rld
c
o
o
r
d
i
n
a
tes
,
wh
i
le
a
c
o
n
v
o
lu
ti
o
n
a
l
n
e
u
ra
l
n
e
two
r
k
(CNN
)
re
c
o
g
n
ize
s
g
o
a
l
m
a
rk
e
rs.
Od
o
m
e
try
–
a
u
g
m
e
n
ted
re
a
li
ty
U
n
iv
e
rsit
y
o
f
Co
r
d
o
b
a
(
ArU
c
o
)
fu
sio
n
is
u
se
d
t
o
c
o
rre
c
t
a
c
c
u
m
u
late
d
lo
c
a
li
z
a
ti
o
n
d
rift
.
P
a
rti
c
le
sw
a
rm
o
p
ti
m
iza
ti
o
n
(P
S
O)
g
e
n
e
ra
tes
sm
o
o
th
g
lo
b
a
l
p
a
t
h
s
o
ffli
n
e
,
wh
e
re
a
s
th
e
d
y
n
a
m
ic
wi
n
d
o
w
a
p
p
r
o
a
c
h
(DW
A)
p
e
rfo
rm
s
re
a
l
-
ti
m
e
lo
c
a
l
m
o
ti
o
n
c
o
n
tr
o
l
.
Ex
p
e
rime
n
ts
in
a
3
.
3
m
×
2
.
4
m
wo
rk
sp
a
c
e
a
c
h
iev
e
d
a
v
e
ra
g
e
o
b
sta
c
le
-
lo
c
a
li
z
a
ti
o
n
e
rro
rs
o
f
0
.
9
5
9
c
m
a
n
d
0
.
6
9
6
c
m
a
lo
n
g
th
e
x
-
a
n
d
y
-
a
x
e
s,
re
sp
e
c
ti
v
e
ly
,
a
n
d
a
g
o
a
l
-
re
c
o
g
n
it
i
o
n
a
c
c
u
ra
c
y
o
f
9
9
%
.
Th
e
DWA
c
o
n
tro
ll
e
r
re
q
u
ired
1
2
.
6
1
±2
.
4
2
m
s
p
e
r
c
y
c
le,
wh
il
e
ra
p
id
l
y
-
e
x
p
lo
ri
n
g
ra
n
d
o
m
tree
(
RRT
)
a
n
d
P
S
O
re
q
u
ire
d
4
–
7
s
a
n
d
4
7
7
–
6
9
2
s,
re
sp
e
c
ti
v
e
ly
,
f
o
r
g
lo
b
a
l
p
a
t
h
g
e
n
e
ra
ti
o
n
.
S
imu
latio
n
a
n
d
p
h
y
sic
a
l
e
x
p
e
rime
n
ts
c
o
n
firme
d
c
o
l
li
sio
n
-
fre
e
n
a
v
ig
a
ti
o
n
a
n
d
su
c
c
e
ss
fu
l
q
u
ick
r
e
sp
o
n
se
(
QR
)
-
c
o
d
e
-
b
a
se
d
g
o
o
d
s
i
n
sp
e
c
ti
o
n
,
d
e
m
o
n
stra
ti
n
g
t
h
e
fe
a
sib
il
it
y
o
f
th
e
p
ro
p
o
se
d
fra
m
e
wo
rk
f
o
r
sm
a
ll
,
stru
c
tu
re
d
in
d
o
o
r
e
n
v
ir
o
n
m
e
n
ts.
K
ey
w
o
r
d
s
:
Au
to
n
o
m
o
u
s
m
o
b
ile
r
o
b
o
t
Dy
n
am
ic
win
d
o
w
a
p
p
r
o
ac
h
Par
ticle
s
war
m
o
p
tim
izatio
n
Sen
s
o
r
f
u
s
io
n
s
y
s
tem
Vis
io
n
-
b
ased
n
av
ig
atio
n
s
y
s
tem
T
h
is i
s
a
n
o
p
e
n
a
c
c
e
ss
a
rticle
u
n
d
e
r th
e
CC B
Y
-
SA
li
c
e
n
se
.
C
o
r
r
e
s
p
o
nd
ing
A
uth
o
r
:
T
r
o
n
g
Hieu
L
u
u
C
o
lleg
e
o
f
E
n
g
i
n
ee
r
in
g
,
C
an
T
h
o
Un
iv
er
s
ity
3
/2
s
tr
ee
t,
Nin
h
Kieu
W
ar
d
,
C
an
T
h
o
C
ity
,
Vietn
am
E
m
ail:
lu
u
tr
o
n
g
h
ieu
@
ctu
.
ed
u
.
v
n
1.
I
NT
RO
D
UCT
I
O
N
Path
p
lan
n
in
g
is
a
co
r
e
task
i
n
au
to
n
o
m
o
u
s
r
o
b
o
t
n
a
v
ig
atio
n
,
wh
er
e
a
co
llis
io
n
-
f
r
ee
r
o
u
t
e
m
u
s
t
b
e
g
en
er
ated
f
r
o
m
a
s
tar
t
p
o
in
t
to
a
tar
g
et
in
a
clu
tter
ed
en
v
ir
o
n
m
en
t.
E
x
is
tin
g
ap
p
r
o
ac
h
e
s
in
clu
d
e
h
eu
r
is
tic
s
ea
r
ch
,
in
tellig
en
t
o
p
tim
izatio
n
,
an
d
r
ein
f
o
r
ce
m
en
t
lear
n
in
g
,
an
d
ea
c
h
m
eth
o
d
in
v
o
lv
es
tr
ad
e
-
o
f
f
s
i
n
co
m
p
u
tatio
n
tim
e,
ad
ap
ta
b
ilit
y
,
an
d
p
ath
q
u
ality
.
Am
o
n
g
class
ical
m
eth
o
d
s
,
∗
is
wid
ely
u
s
ed
b
ec
au
s
e
it
co
m
b
in
es
g
r
ap
h
s
ea
r
ch
with
h
eu
r
is
tic
g
u
id
an
ce
,
p
r
o
v
id
i
n
g
ef
f
icien
t
s
h
o
r
test
-
p
ath
s
ea
r
ch
an
d
p
r
ac
tical
p
er
f
o
r
m
an
ce
in
s
tatic
m
ap
s
.
Pre
v
io
u
s
s
tu
d
ies
r
ep
o
r
te
d
f
ast
s
ea
r
ch
s
p
ee
d
[
1
]
–
[
3
]
,
h
ig
h
s
u
cc
ess
r
ate
[
4
]
,
a
n
d
im
p
r
o
v
e
d
p
ath
s
m
o
o
th
in
g
p
e
r
f
o
r
m
a
n
ce
[
5
]
f
o
r
∗
in
s
tr
u
ctu
r
ed
en
v
ir
o
n
m
e
n
ts
,
b
u
t
its
e
f
f
ec
tiv
en
ess
ca
n
d
ec
r
ea
s
e
in
lar
g
e
-
s
ca
le
o
r
d
y
n
am
ic
s
ce
n
es th
at
r
eq
u
ir
e
f
r
eq
u
en
t r
ep
lan
n
i
n
g
.
T
o
im
p
r
o
v
e
f
lex
i
b
ilit
y
,
o
p
ti
m
izatio
n
-
b
ased
alg
o
r
ith
m
s
s
u
ch
as
ar
tific
ial
p
o
ten
tial
f
i
eld
(
APF)
,
g
en
etic
alg
o
r
ith
m
(
GA)
,
an
d
a
n
t
co
lo
n
y
o
p
tim
izatio
n
(
AC
O)
h
av
e
b
ee
n
in
v
esti
g
ated
.
APF
is
co
m
p
u
tatio
n
ally
Evaluation Warning : The document was created with Spire.PDF for Python.
I
n
t J Ar
tif
I
n
tell
I
SS
N:
2252
-
8
9
3
8
E
xp
erimen
ta
l v
a
lid
a
tio
n
o
f v
is
io
n
-
b
a
s
ed
h
yb
r
id
n
a
vig
a
tio
n
f
o
r
d
iffer
en
tia
l
…
(
Th
a
n
h
Qu
a
n
g
Du
c
Le
)
3837
lig
h
t
an
d
r
esp
o
n
d
s
q
u
ick
l
y
[
6
]
,
[
7
]
,
an
d
g
o
o
d
s
u
cc
ess
r
ates
h
av
e
b
ee
n
r
e
p
o
r
ted
th
r
o
u
g
h
p
r
o
p
er
p
ar
a
m
eter
tu
n
in
g
[
8
]
–
[
1
0
]
,
b
u
t
lo
ca
l
m
in
i
m
a
m
ay
o
cc
u
r
i
n
co
m
p
lex
o
b
s
tacle
co
n
f
ig
u
r
atio
n
s
[
1
1
]
.
GA
ca
n
ex
p
lo
r
e
a
wid
e
s
o
l
u
t
i
o
n
s
p
ac
e
a
n
d
p
r
o
d
u
c
e
n
e
a
r
-
o
p
t
i
m
a
l
p
a
t
h
s
[
1
2
]
,
[
1
3
]
w
i
th
d
y
n
a
m
i
c
o
b
s
t
a
c
l
e
a
v
o
i
d
a
n
c
e
c
a
p
a
b
i
l
i
t
y
[
1
4
]
,
[
1
5
]
,
alth
o
u
g
h
co
n
v
er
g
e
n
ce
ca
n
b
e
s
lo
w
o
r
p
r
e
m
atu
r
e
wh
e
n
p
o
p
u
latio
n
d
iv
er
s
ity
is
in
s
u
f
f
icie
n
t.
AC
O
ca
n
ac
h
iev
e
h
ig
h
s
ea
r
ch
e
f
f
icien
cy
[
1
6
]
an
d
g
en
e
r
ate
s
m
o
o
th
p
ath
s
[
1
7
]
–
[
1
9
]
,
b
u
t
it
m
ay
s
u
f
f
er
f
r
o
m
s
u
b
o
p
tim
al
co
n
v
er
g
en
ce
[
2
0
]
an
d
in
cr
e
ased
co
m
p
u
tatio
n
al
co
s
t
in
lar
g
e
p
r
o
b
lem
s
d
u
e
to
p
h
er
o
m
o
n
e
u
p
d
ates.
R
ein
f
o
r
ce
m
en
t
lear
n
in
g
h
as
also
b
ee
n
ap
p
lied
to
n
a
v
ig
atio
n
,
wh
er
e
Q
-
lear
n
in
g
ca
n
lear
n
p
o
licies
th
r
o
u
g
h
in
ter
ac
tio
n
with
o
u
t
a
co
m
p
let
e
m
o
d
el
a
n
d
m
ay
c
o
n
v
e
r
g
e
u
n
d
er
ce
r
tain
co
n
d
itio
n
s
[
2
1
]
–
[
2
3
]
;
h
o
wev
er
,
s
lo
w
co
n
v
er
g
en
ce
[
2
4
]
,
s
en
s
itiv
ity
t
o
lo
ca
l
o
p
tim
a
[
2
5
]
,
an
d
r
e
d
u
c
ed
ef
f
icien
c
y
in
h
ig
h
-
d
im
en
s
i
o
n
al
s
p
ac
es
r
em
ain
co
n
ce
r
n
s
.
Mo
n
te
C
ar
lo
-
b
ased
m
eth
o
d
s
,
s
u
ch
as
Mo
n
te
C
ar
lo
tr
ee
s
ea
r
ch
(
MCTS
)
an
d
its
v
ar
ian
ts
,
ca
n
im
p
r
o
v
e
g
lo
b
al
s
ea
r
ch
a
n
d
co
n
v
er
g
e
n
ce
p
r
o
p
er
ties
[
2
6
]
–
[
2
9
]
,
b
u
t
th
ey
s
till
r
eq
u
ir
e
ex
ten
s
iv
e
co
m
p
u
tatio
n
an
d
m
ay
b
e
lim
ited
in
s
tr
ict
r
ea
l
-
tim
e
s
ettin
g
s
.
Alth
o
u
g
h
th
e
alg
o
r
ith
m
s
h
av
e
ac
h
iev
ed
r
em
a
r
k
ab
le
p
r
o
g
r
es
s
in
n
a
v
ig
atio
n
r
esear
ch
,
m
o
s
t
o
f
th
em
ar
e
s
till
v
alid
ated
o
n
ly
th
r
o
u
g
h
s
im
u
latio
n
o
r
b
e
n
ch
m
ar
k
ch
allen
g
e
p
r
o
b
lem
s
.
I
n
p
r
ac
tice,
m
o
b
ile
r
o
b
o
ts
o
p
er
atin
g
i
n
r
ea
l
-
wo
r
l
d
en
v
ir
o
n
m
e
n
ts
—
s
u
ch
as
war
eh
o
u
s
es
o
r
f
ac
to
r
ies
—
m
u
s
t
d
ea
l
with
u
n
ce
r
tain
p
e
r
c
e
p
t
i
o
n
,
s
e
n
s
o
r
n
o
is
e
,
a
n
d
d
y
n
a
m
i
c
o
b
s
t
ac
l
es
.
U
n
d
e
r
s
u
c
h
c
o
n
d
i
t
i
o
n
s
,
t
r
a
d
i
t
i
o
n
al
o
r
p
u
r
e
ly
s
i
m
u
l
at
i
o
n
-
b
a
s
ed
alg
o
r
ith
m
s
o
f
ten
f
ac
e
d
if
f
ic
u
lties
in
m
ain
tain
in
g
s
m
o
o
th
m
o
tio
n
,
s
tab
le
lo
ca
lizatio
n
,
an
d
ad
ap
tiv
e
g
o
al
r
ec
o
g
n
itio
n
.
T
h
is
g
ap
h
ig
h
lig
h
ts
th
e
n
ec
ess
ity
f
o
r
a
m
o
r
e
i
n
teg
r
ated
an
d
ex
p
er
im
en
tally
v
alid
ated
n
av
i
g
atio
n
f
r
am
ewo
r
k
th
at
co
m
b
in
es th
e
s
tr
en
g
th
s
o
f
o
p
tim
izatio
n
,
p
e
r
c
ep
tio
n
,
an
d
co
n
tr
o
l.
T
o
ad
d
r
ess
th
e
g
a
p
b
etwe
e
n
th
eo
r
etica
l
p
ath
p
lan
n
i
n
g
an
d
p
r
ac
tical
d
ep
lo
y
m
en
t,
th
is
s
tu
d
y
in
tr
o
d
u
ce
s
a
n
in
te
g
r
ated
v
is
io
n
-
b
ased
n
av
ig
ati
o
n
f
r
am
ew
o
r
k
.
T
h
e
s
y
s
tem
i
n
teg
r
ates
g
lo
b
al
p
lan
n
in
g
v
ia
p
ar
ticle
s
war
m
o
p
tim
izatio
n
(
PS
O)
an
d
r
ea
l
-
tim
e
lo
ca
l
co
n
t
r
o
l
u
s
in
g
th
e
d
y
n
am
ic
win
d
o
w
ap
p
r
o
ac
h
(
DW
A)
.
T
h
e
m
ain
co
n
tr
i
b
u
tio
n
o
f
th
is
wo
r
k
lies
in
th
e
s
y
s
tem
-
l
ev
el
in
teg
r
atio
n
an
d
ex
p
e
r
im
en
tal
v
alid
atio
n
o
f
estab
lis
h
ed
n
av
ig
atio
n
tech
n
iq
u
es
r
ath
er
th
a
n
th
e
d
e
v
e
lo
p
m
en
t
o
f
a
f
u
n
d
am
en
tall
y
n
ew
alg
o
r
ith
m
.
Sp
ec
if
ically
,
th
e
p
r
o
p
o
s
ed
f
r
a
m
ewo
r
k
as f
o
llo
ws:
−
C
o
n
v
er
ts
ce
ilin
g
-
ca
m
e
r
a
im
ag
es
in
to
o
cc
u
p
a
n
cy
-
g
r
id
m
ap
s
an
d
wo
r
ld
co
o
r
d
in
ate
s
wh
ile
u
s
in
g
co
n
v
o
l
u
tio
n
al
n
e
u
r
al
n
etwo
r
k
(
C
NN
)
-
b
ased
r
ec
o
g
n
itio
n
to
id
en
tify
n
av
i
g
atio
n
tar
g
ets
.
−
C
o
m
b
in
es
o
f
f
lin
e
PS
O
g
lo
b
al
p
lan
n
in
g
,
r
ea
l
-
tim
e
DW
A
lo
c
al
co
n
tr
o
l,
an
d
o
d
o
m
etr
y
–
a
u
g
m
en
ted
r
ea
lity
Un
iv
er
s
ity
o
f
C
o
r
d
o
b
a
(
A
r
Uco
)
f
u
s
io
n
f
o
r
s
m
o
o
th
n
av
ig
atio
n
an
d
d
r
if
t c
o
r
r
ec
tio
n
.
−
Valid
ates
th
e
co
m
p
lete
s
y
s
tem
th
r
o
u
g
h
s
im
u
latio
n
an
d
r
e
al
-
wo
r
ld
ex
p
er
im
en
ts
in
v
o
lv
i
n
g
au
to
n
o
m
o
u
s
n
a
v
i
g
a
ti
o
n
,
g
o
o
d
s
i
n
s
p
e
cti
o
n
,
a
n
d
q
u
ic
k
r
esp
o
n
s
e
(
QR
)
-
c
o
d
e
r
ec
o
g
n
i
ti
o
n
i
n
wa
r
e
h
o
u
s
e
-
li
k
e
e
n
v
ir
o
n
m
e
n
t
.
2.
M
E
T
H
O
D
2
.
1
.
Sy
s
t
e
m
o
v
er
v
iew
T
h
e
in
tellig
en
t
co
n
tr
o
l
alg
o
r
it
h
m
is
d
esig
n
ed
t
o
p
er
f
o
r
m
h
i
g
h
-
lev
el
d
ec
is
io
n
-
m
ak
in
g
an
d
tr
ajec
to
r
y
g
en
er
atio
n
f
o
r
th
e
m
o
b
ile
r
o
b
o
t,
as
in
Fig
u
r
e
1
.
I
n
th
is
s
tu
d
y
,
a
PS
O
alg
o
r
ith
m
is
ad
o
p
t
ed
to
d
eter
m
in
e
t
h
e
o
p
tim
al
r
o
b
o
t
b
eh
a
v
io
r
an
d
n
a
v
ig
atio
n
p
ath
in
t
h
e
war
e
h
o
u
s
e
en
v
ir
o
n
m
e
n
t.
T
h
e
PS
O
m
o
d
u
le
p
r
o
ce
s
s
es
g
lo
b
al
in
f
o
r
m
atio
n
to
p
lan
co
llis
io
n
-
f
r
ee
an
d
ef
f
icien
t
tr
ajec
to
r
ies,
wh
ich
ar
e
th
en
tr
an
s
m
itted
wi
r
eless
ly
to
th
e
r
o
b
o
t
p
latf
o
r
m
f
o
r
e
x
ec
u
tio
n
.
Ad
d
iti
o
n
ally
,
a
ce
ilin
g
-
m
o
u
n
ted
ca
m
er
a
is
u
tili
ze
d
to
m
o
n
ito
r
th
e
r
o
b
o
t
’
s
p
o
s
itio
n
in
r
ea
l tim
e,
p
r
o
v
id
in
g
f
ee
d
b
ac
k
o
n
g
lo
b
al
lo
ca
lizatio
n
an
d
en
s
u
r
in
g
th
at
th
e
r
o
b
o
t a
cc
u
r
ately
f
o
llo
ws th
e
d
esire
d
tr
ajec
to
r
y
with
in
th
e
o
p
er
atio
n
al
ar
ea
.
Fig
u
r
e
1
.
I
n
tellig
en
t c
o
n
tr
o
l f
r
am
ewo
r
k
f
o
r
th
e
two
-
wh
ee
led
d
if
f
er
en
tial
-
d
r
iv
e
m
o
b
ile
r
o
b
o
t
T
h
e
r
o
b
o
t
p
latf
o
r
m
is
r
esp
o
n
s
ib
le
f
o
r
lo
w
-
le
v
el
m
o
tio
n
co
n
tr
o
l
an
d
r
ea
l
-
tim
e
o
p
er
ati
o
n
o
f
th
e
d
if
f
er
en
tial
-
d
r
iv
e
m
o
b
ile
r
o
b
o
t.
A
p
r
o
p
o
r
tio
n
al
–
in
te
g
r
al
–
d
er
iv
ativ
e
(
PID
)
co
n
t
r
o
ller
r
eg
u
lates
th
e
wh
ee
l
v
elo
cities
to
ac
h
iev
e
s
m
o
o
th
a
n
d
s
tab
le
m
o
tio
n
b
ased
o
n
th
e
r
ef
er
en
ce
tr
ajec
to
r
y
r
ec
eiv
e
d
f
r
o
m
t
h
e
h
ig
h
-
lev
el
Evaluation Warning : The document was created with Spire.PDF for Python.
I
SS
N
:
2
2
5
2
-
8
9
3
8
I
n
t J Ar
tif
I
n
tell
,
Vo
l.
1
5
,
No
.
4
,
Au
g
u
s
t 2
0
2
6
:
3
8
3
6
-
3
8
5
2
3838
co
n
tr
o
ller
.
T
h
e
r
o
b
o
t
in
teg
r
ates
m
u
ltip
le
o
n
b
o
ar
d
s
en
s
o
r
s
,
in
clu
d
in
g
a
lig
h
t
d
etec
tio
n
an
d
r
an
g
in
g
(
L
iDAR
)
s
en
s
o
r
f
o
r
o
b
s
tacle
d
etec
tio
n
an
d
av
o
id
a
n
ce
,
an
d
wh
ee
l
en
co
d
er
s
f
o
r
v
elo
city
a
n
d
d
is
p
l
ac
em
en
t
f
ee
d
b
ac
k
.
T
h
ese
s
en
s
o
r
d
ata
ar
e
c
o
n
tin
u
o
u
s
ly
ex
c
h
an
g
e
d
with
t
h
e
h
i
g
h
-
lev
el
co
n
tr
o
ller
to
u
p
d
ate
th
e
en
v
ir
o
n
m
en
t
m
ap
an
d
ad
ju
s
t
th
e
r
o
b
o
t’
s
m
o
v
e
m
en
t
d
y
n
a
m
ically
.
T
h
is
clo
s
ed
-
lo
o
p
c
o
n
tr
o
l
s
tr
u
ctu
r
e
en
s
u
r
es
ac
cu
r
ate
v
elo
city
tr
ac
k
in
g
,
e
f
f
ec
tiv
e
o
b
s
tacle
av
o
id
an
ce
,
an
d
s
af
e
au
to
n
o
m
o
u
s
n
av
ig
atio
n
in
war
e
h
o
u
s
e
en
v
ir
o
n
m
en
ts
f
o
r
au
to
m
ated
g
o
o
d
s
tr
a
n
s
p
o
r
tatio
n
.
2
.
2
.
Velo
cit
y
co
ntr
o
l
a
nd
po
s
e
estim
a
t
io
n o
f
t
he
m
o
bil
e
r
o
bo
t
2
.
2
.
1
.
K
inem
a
t
ic
m
o
del o
f
t
h
e
diff
er
ent
ia
l
-
driv
e
m
o
bil
e
r
o
bo
t
T
h
e
m
o
b
ile
r
o
b
o
t
u
s
ed
in
th
i
s
s
tu
d
y
is
a
d
if
f
er
en
tial
-
d
r
iv
e
ty
p
e
eq
u
ip
p
e
d
with
two
in
d
ep
en
d
en
tly
d
r
iv
en
wh
ee
ls
s
ep
ar
ated
b
y
a
f
ix
ed
d
is
tan
ce
,
wh
er
e
th
e
m
id
p
o
in
t
b
etwe
en
t
h
e
wh
ee
ls
.
T
h
e
lin
ea
r
v
elo
city
o
f
th
e
lef
t
wh
ee
l
an
d
r
ig
h
t
w
h
ee
l
ar
e
an
d
,
r
esp
ec
tiv
ely
.
R
o
b
o
t
m
o
v
es
f
o
r
war
d
,
b
ac
k
w
ar
d
an
d
r
o
tates
ar
o
u
n
d
its
in
s
tan
tan
eo
u
s
ce
n
te
r
o
f
cu
r
v
atu
r
e
(
I
C
C
)
as sh
o
wn
in
Fig
u
r
e
2
.
Fig
u
r
e
2
.
Kin
em
atic
a
n
d
o
d
o
m
etr
y
m
o
d
el
o
f
a
d
if
f
er
e
n
tial
-
d
r
i
v
e
m
o
b
ile
r
o
b
o
t
T
h
e
r
o
b
o
t lin
ea
r
a
n
d
an
g
u
lar
v
elo
city
ar
e
ca
lcu
lated
b
y
(
1
)
a
n
d
(
2
)
.
=
+
2
(
1
)
=
−
(
2
)
T
h
e
r
o
b
o
t
p
o
s
e
in
th
e
g
l
o
b
al
f
r
am
e
is
d
escr
ib
e
d
b
y
t
h
e
v
ec
to
r
=
[
,
,
]
,
wh
er
e
(
,
)
r
ep
r
esen
ts
th
e
p
o
s
itio
n
o
f
p
o
in
t
an
d
th
e
h
ea
d
in
g
an
g
le.
T
h
e
r
o
b
o
t’
s
k
in
em
atic
m
o
d
el
ca
n
b
e
ex
p
r
ess
ed
as
(
3
)
.
{
̇
=
̇
=
̇
=
(
3
)
O
v
er
a
s
am
p
lin
g
∆
,
m
o
b
ile
r
o
b
o
t in
th
e
g
lo
b
al
f
r
am
e
ca
n
b
e
d
e
s
cr
ib
ed
as
(
4
)
.
{
+
1
=
+
∆
+
1
=
+
∆
+
1
=
+
∆
(
4
)
2
.
2
.
2
.
Velo
cit
y
c
o
ntr
o
l us
ing
pro
po
rt
io
na
l
–
inte
g
ra
l
–
deriv
a
t
iv
e
T
o
m
ain
tain
ac
cu
r
ate
an
d
s
tab
le
m
o
tio
n
,
a
PID
co
n
tr
o
ller
is
im
p
lem
en
ted
f
o
r
ea
ch
wh
e
el
m
o
to
r
.
E
n
co
d
er
f
ee
d
b
ac
k
p
r
o
v
id
es
t
h
e
ac
tu
al
wh
ee
l
v
elo
cities
an
d
,
wh
ile
th
e
co
n
tr
o
ller
en
s
u
r
es
th
at
th
ey
f
o
llo
w
th
e
r
ef
e
r
en
ce
v
elo
cities
an
d
.
T
h
e
d
is
cr
ete
PID
co
n
tr
o
l la
w
is
ca
lcu
lated
as
(
5
)
.
[
]
=
[
]
+
∆
∑
[
]
=
0
+
[
]
−
[
−
1
]
∆
(
5
)
Evaluation Warning : The document was created with Spire.PDF for Python.
I
n
t J Ar
tif
I
n
tell
I
SS
N:
2252
-
8
9
3
8
E
xp
erimen
ta
l v
a
lid
a
tio
n
o
f v
is
io
n
-
b
a
s
ed
h
yb
r
id
n
a
vig
a
tio
n
f
o
r
d
iffer
en
tia
l
…
(
Th
a
n
h
Qu
a
n
g
Du
c
Le
)
3839
W
h
er
e
[
]
is
th
e
d
esire
d
v
el
o
city
at
s
am
p
le
;
[
]
=
[
]
−
[
]
is
th
e
v
elo
city
er
r
o
r
at
s
am
p
le
;
∈
{
,
}
is
lef
t o
r
r
ig
h
t w
h
ee
l
; a
n
d
,
,
ar
e
p
r
o
p
o
r
tio
n
al,
in
teg
r
al,
a
n
d
d
e
r
iv
ativ
e
g
ain
s
,
r
esp
ec
tiv
ely
.
T
h
e
PID
o
u
tp
u
t
[
]
g
en
er
ates
th
e
m
o
to
r
co
m
m
an
d
s
ig
n
al
(
e.
g
.
,
PW
M
d
u
ty
cy
cle)
.
B
o
th
w
h
ee
l
co
n
tr
o
ller
s
o
p
er
ate
s
im
u
ltan
e
o
u
s
ly
to
ac
h
iev
e
s
m
o
o
th
lin
ea
r
v
elo
city
an
d
a
n
g
u
lar
v
elo
cit
y
,
wh
ich
a
r
e
th
e
n
u
s
ed
b
y
th
e
DW
A
f
o
r
r
ea
l
-
t
im
e
tr
ajec
to
r
y
tr
ac
k
in
g
.
T
o
en
s
u
r
e
f
ea
s
ib
le
m
o
tio
n
,
th
e
f
o
llo
win
g
d
y
n
am
ic
co
n
s
tr
ain
ts
ar
e
ca
lcu
lated
(
6
)
.
{
|
−
|
≤
(
)
∆
|
−
|
≤
(
)
∆
(
6
)
W
h
er
e
(
)
an
d
(
)
ar
e
th
e
r
o
b
o
t’
s
m
ax
im
u
m
lin
ea
r
a
n
d
a
n
g
u
lar
ac
ce
ler
atio
n
s
.
2
.
2
.
3
.
O
do
m
e
t
ry
-
ba
s
ed
po
s
e
estim
a
t
io
n
T
h
e
r
o
b
o
t’
s
p
o
s
e
is
esti
m
at
ed
f
r
o
m
wh
ee
l
en
c
o
d
er
r
ea
d
in
g
s
u
s
in
g
o
d
o
m
etr
y
.
T
h
e
i
n
cr
em
en
tal
d
is
p
lace
m
en
ts
o
f
th
e
lef
t
an
d
r
ig
h
t
wh
ee
ls
in
a
s
am
p
lin
g
in
ter
v
al
ar
e
Δ
an
d
Δ
.
T
h
e
r
o
b
o
t
’
s
m
o
tio
n
in
cr
em
en
ts
ar
e
ca
lcu
lated
as
(
7
)
an
d
(
8
)
.
∆
=
∆
+
∆
2
(
7
)
=
∆
−
∆
(
8
)
T
h
e
u
p
d
ated
p
o
s
e
is
g
iv
en
b
y
(
9
)
.
{
+
1
=
+
∆
⋅
(
+
2
)
+
1
=
+
∆
⋅
(
+
2
)
+
1
=
+
(
9
)
2
.
3
.
M
o
bil
e
ro
bo
t
pa
t
h pla
n
nin
g
2
.
3
.
1
.
G
lo
ba
l pa
t
h pla
nn
ing
us
ing
pa
rt
icle
s
wa
rm
o
ptim
iza
t
io
n
Glo
b
al
p
ath
p
lan
n
in
g
is
an
ess
en
tial
p
r
o
b
lem
in
m
o
b
ile
r
o
b
o
t
n
av
ig
atio
n
,
esp
ec
ially
in
co
m
p
lex
en
v
ir
o
n
m
en
ts
th
at
co
n
tain
m
u
ltip
le
o
b
s
tacle
s
.
I
n
th
is
s
tu
d
y
,
th
e
PS
O
[
3
0
]
alg
o
r
it
h
m
is
ap
p
lied
to
g
en
er
ate
a
n
o
p
tim
al
p
ath
f
r
o
m
th
e
s
tar
tin
g
p
o
in
t
to
th
e
tar
g
et
p
o
in
t
wh
ile
en
s
u
r
in
g
th
at
th
e
r
o
b
o
t
av
o
id
s
co
llis
io
n
s
with
o
b
s
tacle
s
d
u
r
in
g
m
o
tio
n
.
T
h
e
g
lo
b
al
p
at
h
p
lan
n
in
g
p
r
o
ce
s
s
ca
n
b
e
d
escr
ib
ed
as
f
in
d
in
g
a
s
et
o
f
d
is
cr
ete
p
o
in
ts
g
en
er
ated
b
y
th
e
r
o
b
o
t a
s
it
m
o
v
es
th
r
o
u
g
h
th
e
en
v
ir
o
n
m
en
t,
wh
er
e
ea
ch
c
o
n
s
ec
u
tiv
e
s
eg
m
en
t
b
etwe
en
p
o
in
ts
d
o
es n
o
t i
n
ter
s
ec
t a
n
y
s
tatic
o
b
s
tacle
.
T
h
e
s
et
o
f
p
o
in
t
s
ca
n
b
e
r
e
p
r
esen
ted
as
(
1
0
)
.
=
{
,
1
,
2
,
…
,
,
}
(
1
0
)
T
h
e
to
tal
p
ath
le
n
g
th
is
ca
lcu
l
ated
as
(
1
1
)
.
=
∑
(
,
+
1
)
−
1
=
1
(
1
1
)
W
h
er
e
is
th
e
s
tar
t
p
o
in
t;
is
th
e
tar
g
et
p
o
i
n
t;
(
1
,
2
,
…
,
)
is
th
e
s
eq
u
e
n
ce
o
f
in
ter
m
e
d
iate
p
o
i
n
ts
in
t
h
e
en
v
ir
o
n
m
en
t;
(
,
+
1
)
is
th
e
d
is
tan
ce
b
etwe
en
co
n
s
ec
u
tiv
e
p
o
in
ts
;
an
d
+
1
,
an
d
is
th
e
to
tal
n
u
m
b
e
r
o
f
p
o
i
n
ts
in
th
e
s
et
.
T
o
m
ak
e
th
e
g
lo
b
al
p
ath
s
m
o
o
th
er
an
d
m
o
r
e
s
u
itab
le
f
o
r
a
m
o
b
ile
r
o
b
o
t,
a
cu
b
ic
s
p
lin
e
in
te
r
p
o
latio
n
m
eth
o
d
is
ap
p
lied
to
o
p
tim
iz
e
th
e
p
ath
.
T
h
is
p
r
o
ce
s
s
r
ed
u
ce
s
s
h
ar
p
tu
r
n
s
an
d
cr
ea
tes
a
co
n
tin
u
o
u
s
,
s
m
o
o
th
cu
r
v
e.
T
h
e
p
r
o
ce
d
u
r
e
f
o
r
g
e
n
e
r
atin
g
th
e
cu
b
ic
s
p
lin
e
p
ath
b
e
twee
n
two
p
o
in
ts
an
d
+
1
is
as f
o
llo
ws:
i)
Gen
er
ate
in
ter
p
o
lated
p
o
in
ts
b
etwe
en
ea
ch
p
air
o
f
p
o
in
ts
an
d
+
1
u
s
in
g
c
u
b
ic
s
p
lin
e
in
ter
p
o
la
tio
n
.
ii)
C
o
n
n
ec
t a
ll in
ter
p
o
lated
p
o
in
t
s
(
1
,
2
,
…
,
)
to
o
b
tain
a
c
o
n
tin
u
o
u
s
cu
b
ic
s
p
lin
e
p
ath
.
iii)
T
h
e
lo
ca
l sp
lin
e
len
g
t
h
(
,
+
1
)
b
etwe
en
two
p
o
in
ts
an
d
+
1
is
is
ca
lcu
la
ted
as
(
1
2
)
.
(
,
+
1
)
=
∑
∆
=
∑
√
(
∆
)
2
+
(
∆
)
2
−
1
=
1
−
1
=
1
(
1
2
)
Fin
ally
,
th
e
to
tal
cu
b
ic
s
p
lin
e
p
ath
len
g
th
ca
n
b
e
ex
p
r
ess
ed
as
(
1
3
)
.
T
h
is
cu
b
ic
s
p
lin
e
-
b
ased
o
p
tim
izatio
n
en
s
u
r
es
th
at
th
e
g
lo
b
al
p
ath
i
s
s
m
o
o
th
,
c
o
n
tin
u
o
u
s
,
a
n
d
f
ea
s
ib
le
f
o
r
r
ea
l
-
tim
e
n
av
ig
atio
n
o
f
th
e
d
if
f
er
en
tial
-
d
r
iv
e
m
o
b
ile
r
o
b
o
t.
=
∑
C
(
,
+
1
)
−
1
=
1
(
1
3
)
Evaluation Warning : The document was created with Spire.PDF for Python.
I
SS
N
:
2
2
5
2
-
8
9
3
8
I
n
t J Ar
tif
I
n
tell
,
Vo
l.
1
5
,
No
.
4
,
Au
g
u
s
t 2
0
2
6
:
3
8
3
6
-
3
8
5
2
3840
I
n
th
e
p
r
o
p
o
s
ed
s
y
s
tem
,
th
e
PS
O
alg
o
r
ith
m
s
ea
r
c
h
es
f
o
r
th
e
o
p
tim
al
g
lo
b
al
p
ath
r
ep
r
esen
ted
b
y
th
e
s
et
.
E
ac
h
p
a
r
ticle
in
th
e
s
war
m
en
co
d
es
a
p
o
ten
tial
tr
ajec
to
r
y
co
m
p
o
s
ed
o
f
th
e
co
o
r
d
in
ates
o
f
(
1
,
2
,
…
,
)
.
T
h
e
s
ea
r
ch
s
p
ac
e
co
r
r
esp
o
n
d
s
to
th
e
f
r
ee
-
s
p
ac
e
ce
lls
o
f
th
e
en
v
ir
o
n
m
en
t
m
ap
,
wh
e
r
e
0
r
ep
r
esen
ts
tr
av
er
s
ab
le
ar
ea
s
a
n
d
1
d
en
o
tes
o
b
s
tacle
s
.
E
ac
h
p
ar
ticle
h
as
a
p
o
s
itio
n
an
d
v
elo
city
at
iter
atio
n
.
T
h
e
q
u
ality
o
f
ea
ch
p
ath
is
ev
alu
ated
u
s
in
g
a
f
itn
ess
f
u
n
ctio
n
th
at
co
m
b
in
es
th
e
to
tal
p
ath
len
g
t
h
an
d
an
o
b
s
tacle
p
en
alty
ter
m
i
n
(
1
4
)
.
=
+
∙
(
1
4
)
W
h
er
e
is
ca
lcu
lated
f
r
o
m
(
1
0
)
an
d
(
1
1
)
,
obs
is
th
e
n
u
m
b
er
o
f
p
ath
s
eg
m
en
ts
in
ter
s
ec
tin
g
o
b
s
tacle
ce
lls
,
an
d
is
a
p
en
alty
co
ef
f
icien
t
th
at
d
is
co
u
r
ag
es
u
n
s
af
e
p
ath
s
.
T
h
e
v
elo
city
an
d
p
o
s
itio
n
o
f
ea
ch
p
ar
ticle
ar
e
u
p
d
ated
ac
c
o
r
d
in
g
to
th
e
s
tan
d
ar
d
PS
O
in
(
1
5
)
an
d
(
1
6
)
.
+
1
=
∙
+
1
1
(
,
−
)
+
2
2
(
−
)
(
1
5
)
+
1
=
+
+
1
(
1
6
)
W
h
er
e
is
th
e
in
er
tia
weig
h
t
;
1
an
d
2
ar
e
co
g
n
itiv
e
a
n
d
s
o
ci
al
lear
n
in
g
co
e
f
f
icien
ts
;
1
,
2
∈
[
0
,
1
]
ar
e
r
an
d
o
m
v
a
r
iab
les
in
tr
o
d
u
cin
g
s
to
ch
asti
c
ex
p
lo
r
atio
n
;
an
d
,
an
d
r
ep
r
esen
t
th
e
p
er
s
o
n
al
an
d
g
lo
b
al
b
est p
o
s
itio
n
s
,
r
esp
ec
tiv
ely
.
I
f
an
u
p
d
ated
way
p
o
i
n
t
lies
with
in
an
o
b
s
tacle
r
e
g
io
n
,
th
e
alg
o
r
ith
m
m
o
d
if
ies
its
d
ir
ec
ti
o
n
u
s
in
g
eith
er
a
r
ef
lectio
n
s
tr
ateg
y
o
r
an
ad
ap
tiv
e
p
e
n
alty
ad
ju
s
tm
en
t,
f
o
r
cin
g
th
e
p
ar
ticle
to
e
x
p
lo
r
e
v
alid
r
e
g
io
n
s
.
T
h
e
iter
ativ
e
p
r
o
ce
s
s
co
n
tin
u
e
s
u
n
til o
n
e
o
f
th
e
f
o
llo
win
g
co
n
d
itio
n
s
is
s
atis
f
ied
:
−
T
h
e
m
ax
im
u
m
n
u
m
b
er
o
f
iter
a
tio
n
s
is
r
ea
ch
ed
.
−
T
h
e
im
p
r
o
v
em
en
t i
n
b
etwe
en
iter
atio
n
s
is
n
eg
lig
ib
le.
−
T
h
e
s
war
m
co
n
v
er
g
es to
a
s
ta
b
le
g
lo
b
al
s
o
lu
tio
n
.
Up
o
n
co
n
v
er
g
e
n
ce
,
th
e
b
est
p
ath
o
b
tain
ed
f
r
o
m
t
h
e
s
war
m
is
s
elec
ted
as
th
e
g
lo
b
al
o
p
tim
al
p
ath
.
T
o
im
p
r
o
v
e
r
ep
r
o
d
u
cib
ilit
y
,
t
h
e
PS
O
h
y
p
er
p
a
r
am
eter
s
ettin
g
s
u
s
ed
f
o
r
g
lo
b
al
p
ath
p
lan
n
in
g
ar
e
s
u
m
m
ar
ized
in
T
ab
le
1
.
T
h
e
s
war
m
s
ize
was
s
et
to
=
1
,
000
,
th
e
in
er
tia
weig
h
t
was
f
ix
ed
=
1
.
0
,
an
d
th
e
co
g
n
itiv
e
a
n
d
s
o
cial
lear
n
in
g
co
ef
f
icien
ts
we
r
e
s
et
to
1
=
1
.
5
an
d
2
=
1
.
5
,
r
esp
ec
tiv
ely
.
T
h
e
o
p
tim
izatio
n
was
ter
m
in
ate
d
wh
en
th
e
m
ax
im
u
m
iter
atio
n
lim
it
=
400
was
r
ea
ch
ed
,
o
r
ea
r
lier
if
co
n
v
er
g
e
n
ce
was
o
b
s
er
v
ed
.
T
h
ese
s
ettin
g
s
p
r
o
v
id
e
a
p
r
ac
tical
b
alan
ce
b
etwe
en
s
o
lu
tio
n
q
u
ality
an
d
co
m
p
u
tatio
n
al
co
s
t
f
o
r
th
e
in
d
o
o
r
wo
r
k
s
p
ac
e
co
n
s
id
er
e
d
.
T
ab
le
1
.
PS
O
p
ar
am
eter
s
ettin
g
s
f
o
r
g
l
o
b
al
p
ath
p
lan
n
i
n
g
P
a
r
a
me
t
e
r
S
y
mb
o
l
R
e
c
o
mm
e
n
d
v
a
l
u
e
R
o
l
e
S
w
a
r
m si
z
e
1
0
0
0
N
u
mb
e
r
o
f
p
a
r
t
i
c
l
e
s
i
n
P
S
O
I
n
e
r
t
i
a
w
e
i
g
h
t
1
.
0
C
o
n
t
r
o
l
s
t
h
e
i
n
f
l
u
e
n
c
e
o
f
p
r
e
v
i
o
u
s
v
e
l
o
c
i
t
y
C
o
g
n
i
t
i
v
e
c
o
e
f
f
i
c
i
e
n
t
1
1
.
5
W
e
i
g
h
t
o
f
l
e
a
r
n
i
n
g
f
r
o
m t
h
e
p
a
r
t
i
c
l
e
’
s
b
e
s
t
p
o
s
i
t
i
o
n
S
o
c
i
a
l
c
o
e
f
f
i
c
i
e
n
t
2
1
.
5
W
e
i
g
h
t
o
f
l
e
a
r
n
i
n
g
f
r
o
m t
h
e
g
l
o
b
a
l
b
e
st
p
a
r
t
i
c
l
e
M
a
x
i
m
u
m
i
t
e
r
a
t
i
o
n
s
4
0
0
M
a
x
i
m
u
m
n
u
m
b
e
r
o
f
i
t
e
r
a
t
i
o
n
s
2
.
3
.
2
.
L
o
ca
l pa
t
h
pla
nn
ing
us
ing
dy
na
m
ic
wind
o
w
a
pp
ro
a
ch
T
h
e
m
o
tio
n
o
f
a
d
if
f
er
en
tial
-
d
r
iv
e
m
o
b
ile
r
o
b
o
t
ca
n
b
e
ex
p
r
ess
ed
b
y
th
e
f
o
llo
win
g
k
in
em
atic
eq
u
atio
n
s
[
3
1
]
.
T
h
e
DW
A
alg
o
r
ith
m
s
ea
r
ch
es
f
o
r
th
e
b
est
p
air
(
,
)
co
n
s
id
er
in
g
th
e
r
o
b
o
t’
s
k
i
n
em
atics
an
d
d
y
n
am
ic
lim
its
(
(
5
)
an
d
(
6)
)
.
T
h
e
f
ea
s
ib
le
win
d
o
w
is
d
ef
i
n
ed
b
y
(
1
7
)
.
{
≤
≤
,
≤
≤
|
−
|
≤
∆
,
|
−
|
≤
∆
(
1
7
)
An
d
s
af
ety
co
n
s
tr
ain
ts
in
(
1
8
)
.
(
,
)
≥
(
1
8
)
W
h
er
e
(
,
)
is
th
e
m
in
im
u
m
d
is
tan
ce
f
r
o
m
th
e
r
o
b
o
t
to
th
e
n
ea
r
est
o
b
s
tacle
an
d
is
th
e
m
in
i
m
u
m
s
af
ety
d
is
tan
ce
.
E
ac
h
p
air
(
,
)
cr
ea
tes
a
s
h
o
r
t,
p
r
ed
icted
tr
aje
cto
r
y
,
wh
ich
is
ev
alu
ated
b
y
a
c
o
m
p
o
s
ite
o
b
jectiv
e
f
u
n
ctio
n
(
1
9
)
.
Evaluation Warning : The document was created with Spire.PDF for Python.
I
n
t J Ar
tif
I
n
tell
I
SS
N:
2252
-
8
9
3
8
E
xp
erimen
ta
l v
a
lid
a
tio
n
o
f v
is
io
n
-
b
a
s
ed
h
yb
r
id
n
a
vig
a
tio
n
f
o
r
d
iffer
en
tia
l
…
(
Th
a
n
h
Qu
a
n
g
Du
c
Le
)
3841
(
,
)
=
∙
ℎ
(
,
)
+
∙
(
,
)
+
∙
(
,
)
(
1
9
)
W
h
er
e
(
,
)
m
ea
s
u
r
es
th
e
alig
n
m
en
t
b
etwe
en
th
e
r
o
b
o
t’
s
h
ea
d
in
g
an
d
th
e
d
ir
ec
tio
n
to
th
e
tar
g
et
;
(
,
)
ev
alu
ates
th
e
m
in
im
u
m
d
is
tan
ce
to
s
u
r
r
o
u
n
d
in
g
o
b
s
tacle
s
;
(
,
)
r
ep
r
esen
ts
th
e
m
ag
n
itu
d
e
o
f
f
o
r
war
d
s
p
ee
d
; a
n
d
,
,
ar
e
weig
h
tin
g
c
o
ef
f
icien
ts
th
at
b
alan
ce
th
ese
cr
iter
ia.
T
h
e
a
lg
o
r
i
t
h
m
e
v
a
l
u
a
t
e
s
a
l
l
f
e
a
s
i
b
l
e
(
,
)
p
a
i
r
s
w
i
th
i
n
th
e
d
y
n
a
m
i
c
w
i
n
d
o
w
a
n
d
s
e
l
e
c
t
s
t
h
e
p
a
i
r
w
i
t
h
th
e
h
ig
h
e
s
t
(
,
)
.
T
h
e
c
o
r
r
es
p
o
n
d
i
n
g
tr
a
j
e
c
to
r
y
i
s
th
e
n
e
x
e
cu
t
e
d
b
y
t
h
e
r
o
b
o
t
d
u
r
in
g
th
e
n
ex
t
c
o
n
tr
o
l
i
n
te
r
v
a
l
.
T
h
i
s
p
r
o
c
e
s
s
i
s
c
o
n
t
i
n
u
o
u
s
l
y
r
ep
e
a
t
ed
,
en
a
b
l
i
n
g
t
h
e
r
o
b
o
t
t
o
ad
ap
t
i
t
s
m
o
t
i
o
n
i
n
r
e
a
l
t
i
m
e.
T
h
e
D
W
A
p
ar
a
m
e
te
r
s
u
s
ed
f
o
r
r
e
a
l
-
t
im
e
l
o
ca
l
m
o
t
i
o
n
c
o
n
t
r
o
l
ar
e
s
u
m
m
a
r
iz
e
d
in
T
ab
l
e
2
.
T
h
e
v
e
l
o
c
i
ty
an
d
a
c
c
e
l
e
r
a
ti
o
n
l
i
m
i
t
s
w
er
e
s
e
lec
t
e
d
ac
c
o
r
d
in
g
t
o
th
e
p
h
y
s
i
ca
l
c
a
p
a
b
i
l
i
ty
o
f
d
i
f
f
e
r
en
t
i
a
l
-
d
r
i
v
e
r
o
b
o
t
,
w
h
i
l
e
o
b
j
e
c
t
iv
e
-
f
u
n
c
t
io
n
w
e
ig
h
t
s
w
e
r
e
e
m
p
i
r
i
c
a
l
ly
t
u
n
e
d
to
b
a
l
a
n
c
e
t
ar
g
e
t
a
l
ig
n
m
e
n
t,
o
b
s
t
a
c
l
e
c
l
e
a
r
a
n
c
e,
a
n
d
f
o
r
w
ar
d
m
o
t
i
o
n
.
T
ab
le
2
.
DW
A
p
ar
am
eter
s
ettin
g
s
P
a
r
a
me
t
e
r
S
y
mb
o
l
V
a
l
u
e
M
a
x
i
m
u
m
l
i
n
e
a
r
v
e
l
o
c
i
t
y
0
.
15
/
M
a
x
i
m
u
m
a
n
g
u
l
a
r
v
e
l
o
c
i
t
y
0
.
8
/
M
a
x
i
m
u
m
l
i
n
e
a
r
a
c
c
e
l
e
r
a
t
i
o
n
0
.
2
/
2
P
r
e
d
i
c
t
i
o
n
h
o
r
i
z
o
n
2
M
i
n
i
m
u
m
safe
t
y
d
i
st
a
n
c
e
0
.
1
m
H
e
a
d
i
n
g
w
e
i
g
h
t
0
.
4
C
l
e
a
r
a
n
c
e
w
e
i
g
h
t
0
.
4
V
e
l
o
c
i
t
y
w
e
i
g
h
t
0
.
2
2
.
4
.
E
x
perim
ent
s
et
up
2
.
4
.
1
.
E
nv
iro
nm
ent
s
et
up
a
nd
ha
rdwa
re
s
y
s
t
em
T
h
e
ex
p
er
im
en
tal
s
etu
p
is
illu
s
tr
ated
in
Fig
u
r
e
3
,
wh
ich
s
im
u
lates
th
e
m
o
v
em
en
t
an
d
s
elf
-
lo
ca
lizatio
n
o
f
a
two
-
w
h
e
eled
d
if
f
e
r
en
tial
-
d
r
iv
e
m
o
b
ile
r
o
b
o
t
o
p
er
ati
n
g
in
an
en
v
i
r
o
n
m
en
t
with
m
u
ltip
le
o
b
s
tacle
s
.
T
h
e
ex
p
er
im
e
n
t
was
co
n
d
u
cted
o
n
an
ex
p
e
r
im
en
t
al
ar
ea
with
a
to
tal
ar
ea
o
f
3
.
3
m
×
2
.
4
m
,
wh
er
e
o
b
s
tacle
s
wer
e
r
an
d
o
m
l
y
p
lac
ed
with
in
th
e
wo
r
k
s
p
ac
e
(
Fig
u
r
e
3
(
a)
)
.
T
h
e
m
o
b
ile
r
o
b
o
t
s
tar
ted
f
r
o
m
p
o
s
itio
n
0
an
d
was
p
r
o
g
r
am
m
ed
to
f
o
llo
w
a
tr
ajec
to
r
y
p
ass
in
g
th
r
o
u
g
h
all
n
u
m
b
er
e
d
tar
g
ets
in
s
eq
u
e
n
ce
0
→
1
→
2
→
3
→
4
→
5
→
0
(
Fig
u
r
e
3
(
b
)
)
.
E
ac
h
tar
g
et
p
o
s
itio
n
was
d
etec
ted
an
d
r
ec
o
r
d
e
d
u
s
in
g
an
o
v
er
h
ea
d
ce
ilin
g
ca
m
er
a,
wh
ich
p
r
o
v
id
ed
g
lo
b
a
l c
o
o
r
d
in
ate
r
ef
er
e
n
ce
s
d
u
r
in
g
th
e
ex
p
er
im
e
n
t.
T
h
e
r
o
b
o
t
u
s
ed
in
th
is
s
tu
d
y
is
s
h
o
wn
in
Fig
u
r
e
3
(
c)
,
a
n
d
its
p
h
y
s
ical
d
im
en
s
io
n
s
ar
e
lis
ted
in
T
ab
le
3
.
T
h
e
r
o
b
o
t
is
eq
u
ip
p
e
d
with
a
n
I
n
tel
R
ea
lSen
s
e
r
ed
,
g
r
ee
n
,
b
lu
e
–
d
ep
th
(
R
GB
-
D
)
ca
m
er
a
m
o
u
n
ted
at
th
e
f
r
o
n
t
to
ca
p
t
u
r
e
v
is
u
al
in
f
o
r
m
atio
n
,
an
d
two
-
wh
ee
l
en
c
o
d
er
s
f
o
r
o
d
o
m
etr
y
co
m
p
u
tatio
n
.
T
h
e
L
attePan
d
a
em
b
ed
d
e
d
co
m
p
u
ter
s
er
v
es
as
th
e
m
ain
co
n
tr
o
ller
,
in
teg
r
ate
d
with
a
m
icr
o
co
n
t
r
o
ller
r
esp
o
n
s
ib
le
f
o
r
d
r
i
v
in
g
th
e
d
ir
ec
t c
u
r
r
en
t (
DC
)
m
o
t
o
r
s
,
r
ea
d
in
g
s
en
s
o
r
d
ata,
a
n
d
c
o
m
m
u
n
icatin
g
with
th
e
h
o
s
t c
o
m
p
u
ter
.
T
o
esti
m
ate
th
e
r
o
b
o
t
p
o
s
itio
n
ac
cu
r
ately
with
in
th
e
wo
r
k
s
p
ac
e,
an
Ar
Uco
m
a
r
k
er
is
m
o
u
n
ted
o
n
th
e
to
p
o
f
th
e
r
o
b
o
t.
T
h
e
ce
ilin
g
c
am
er
a
is
u
s
ed
f
o
r
s
en
s
o
r
f
u
s
io
n
b
y
d
etec
tin
g
r
o
b
o
t
p
o
s
itio
n
s
an
d
ev
alu
atin
g
d
ata
with
r
o
b
o
t’
s
p
o
s
e
o
d
o
m
etr
y
.
I
t
co
n
tin
u
o
u
s
ly
d
etec
ts
th
is
m
ar
k
er
to
d
eter
m
i
n
e
th
e
r
o
b
o
t’
s
g
lo
b
al
co
o
r
d
in
ates
i
n
r
ea
l
tim
e.
T
h
e
p
o
s
itio
n
o
b
tain
ed
f
r
o
m
th
e
ce
ilin
g
ca
m
er
a
is
th
en
f
u
s
ed
with
th
e
in
cr
em
e
n
t
al
m
o
tio
n
d
ata
f
r
o
m
th
e
wh
ee
l e
n
co
d
er
s
to
u
p
d
ate
th
e
r
o
b
o
t p
o
s
e
o
n
th
e
wo
r
k
in
g
m
ap
.
T
h
is
h
y
b
r
id
lo
ca
lizatio
n
ap
p
r
o
ac
h
co
m
b
in
es
v
is
io
n
-
b
ased
g
lo
b
al
p
o
s
itio
n
in
g
an
d
o
d
o
m
etr
y
-
b
ased
lo
ca
l
e
s
tim
atio
n
,
ef
f
ec
tiv
ely
r
e
d
u
cin
g
ac
cu
m
u
lated
d
r
if
t
an
d
im
p
r
o
v
in
g
n
av
i
g
atio
n
ac
c
u
r
ac
y
d
u
r
in
g
o
p
e
r
atio
n
.
2
.
4
.
2
.
E
nv
iro
nm
ent
m
o
delin
g
a
nd
im
a
g
e
-
to
-
w
o
rld c
o
o
rdina
t
e
co
nv
er
s
io
n
T
h
e
wo
r
k
s
p
ac
e
e
n
v
ir
o
n
m
en
t
is
m
o
d
ele
d
th
r
o
u
g
h
an
im
ag
e
-
p
r
o
ce
s
s
in
g
p
ip
elin
e
th
at
ex
tr
ac
ts
g
eo
m
etr
ic
f
ea
tu
r
es
f
r
o
m
th
e
c
eilin
g
ca
m
er
a
an
d
co
n
v
er
ts
i
m
ag
e
co
o
r
d
in
ates
in
to
r
ea
l
-
w
o
r
ld
m
ea
s
u
r
e
m
en
ts
.
T
h
e
ca
p
tu
r
e
d
im
ag
e
is
f
ir
s
t
cr
o
p
p
ed
i
n
to
th
e
r
e
g
io
n
o
f
in
ter
e
s
t
an
d
co
n
v
er
te
d
f
r
o
m
th
e
R
GB
to
th
e
HSV
co
lo
r
s
p
ac
e.
Up
p
er
a
n
d
l
o
wer
HSV
th
r
esh
o
ld
s
[
,
,
]
an
d
[
,
,
]
is
th
en
ap
p
lied
to
g
en
er
ate
a
b
in
a
r
y
m
ask
th
at
h
i
g
h
lig
h
ts
o
b
s
tacle
s
u
r
f
ac
es.
Fig
u
r
e
4
s
h
o
ws
th
e
r
esu
ltin
g
b
in
a
r
y
m
ask
s
g
en
er
ated
b
y
th
is
p
ip
elin
e
f
o
r
two
d
if
f
er
en
t
ca
m
er
a
v
iews
u
s
ed
in
o
b
s
t
ac
le
d
etec
tio
n
:
t
h
e
s
u
r
f
ac
e
f
l
o
o
r
v
iew
an
d
th
e
s
id
e
f
lo
o
r
v
iew.
As
s
h
o
wn
in
Fi
g
u
r
e
4
(
a)
,
th
e
in
itial
m
ask
m
ay
co
n
tain
n
o
is
e
an
d
s
m
all
g
ap
s
alo
n
g
o
b
ject
b
o
u
n
d
ar
ies.
T
h
e
r
ef
o
r
e
,
Gau
s
s
ian
b
lu
r
r
in
g
is
u
s
ed
t
o
s
u
p
p
r
ess
h
ig
h
-
f
r
eq
u
e
n
cy
n
o
is
e,
an
d
m
o
r
p
h
o
lo
g
ical
clo
s
in
g
is
ap
p
lied
to
f
ill
s
m
all
h
o
les an
d
co
n
n
ec
t b
r
o
k
en
co
n
to
u
r
s
,
p
r
o
d
u
cin
g
a
clea
n
er
s
eg
m
en
tatio
n
r
esu
lt f
o
r
s
u
b
s
eq
u
en
t c
o
n
t
o
u
r
e
x
tr
ac
tio
n
an
d
m
ap
g
en
er
atio
n
,
as illu
s
tr
ated
in
Fig
u
r
e
4
(
b
)
.
Evaluation Warning : The document was created with Spire.PDF for Python.
I
SS
N
:
2
2
5
2
-
8
9
3
8
I
n
t J Ar
tif
I
n
tell
,
Vo
l.
1
5
,
No
.
4
,
Au
g
u
s
t 2
0
2
6
:
3
8
3
6
-
3
8
5
2
3842
(
a)
(
b
)
(
c)
Fig
u
r
e
3
.
Sy
s
tem
lay
o
u
t a
n
d
e
x
p
er
im
en
tal
s
p
ac
e
:
(
a
)
s
y
s
tem
lay
o
u
t
,
(
b
)
th
e
e
x
p
er
im
e
n
tal
s
p
ac
e
,
an
d
(
c)
two
-
wh
ee
led
d
if
f
er
e
n
tial m
o
b
ile
r
o
b
o
t
T
ab
le
3
.
Ph
y
s
ical
p
ar
a
m
eter
s
o
f
th
e
d
i
f
f
er
en
tial
-
d
r
iv
e
m
o
b
il
e
r
o
b
o
t
P
a
r
a
me
t
e
r
s
D
i
me
n
si
o
n
(
m)
S
i
z
e
0
.
2
×
0
.
1
5
×
0
.
2
W
h
e
e
l
b
a
s
e
0
.
2
5
W
h
e
e
l
r
a
d
i
u
s
0
.
0
3
2
5
(
a
)
(
b
)
Fig
u
r
e
4
.
I
m
ag
e
p
r
o
ce
s
s
in
g
p
i
p
elin
e
to
d
etec
t o
b
s
tacle
s
(
a)
b
in
ar
y
im
ag
e
o
f
th
e
s
u
r
f
ac
e
f
l
o
o
r
an
d
(
b
)
b
in
ar
y
im
a
g
e
o
f
th
e
s
id
e
v
i
ew
o
f
th
e
f
l
o
o
r
C
o
n
to
u
r
s
ar
e
ex
tr
ac
ted
an
d
f
ilter
ed
b
ased
o
n
th
e
ar
ea
co
n
d
itio
n
<
(
)
<
to
elim
in
ate
n
o
n
-
r
ele
v
an
t
o
b
ject
s
.
E
ac
h
v
alid
co
n
to
u
r
is
ap
p
r
o
x
im
ate
to
a
p
o
l
y
g
o
n
,
an
d
q
u
ad
r
ilater
al
co
n
to
u
r
s
ar
e
r
etain
ed
as
ca
n
d
id
ate
o
b
s
tacle
b
o
u
n
d
ar
ies.
T
h
e
f
o
u
r
c
o
r
n
er
p
o
in
ts
o
f
ea
ch
o
b
s
tacle
ar
e
th
en
c
o
n
v
e
r
ted
f
r
o
m
im
a
g
e
co
o
r
d
in
ates
(
,
)
to
m
etr
ic
co
o
r
d
i
n
ates
(
,
)
b
y
a
lin
ea
r
s
ca
lin
g
m
o
d
el
i
n
(
2
0
)
.
Evaluation Warning : The document was created with Spire.PDF for Python.
I
n
t J Ar
tif
I
n
tell
I
SS
N:
2252
-
8
9
3
8
E
xp
erimen
ta
l v
a
lid
a
tio
n
o
f v
is
io
n
-
b
a
s
ed
h
yb
r
id
n
a
vig
a
tio
n
f
o
r
d
iffer
en
tia
l
…
(
Th
a
n
h
Qu
a
n
g
Du
c
Le
)
3843
{
=
(
−
)
=
(
0
−
)
(
2
0
)
W
h
er
e
(
0
,
0
)
ar
e
th
e
im
ag
e
ce
n
ter
,
an
d
,
ar
e
th
e
co
n
v
er
s
io
n
f
ac
to
r
s
(
m
/p
ix
el)
alo
n
g
th
e
-
ax
es
a
n
d
-
ax
es,
r
esp
ec
tiv
ely
.
T
h
e
co
o
r
d
in
ate
s
y
s
tem
is
later
s
h
if
ted
s
o
th
at
th
e
o
r
ig
in
is
r
elo
ca
te
d
to
th
e
lo
wer
-
lef
t
co
r
n
er
o
f
th
e
w
o
r
k
s
p
ac
e
to
m
a
tch
th
e
r
o
b
o
t’
s
g
lo
b
al
m
ap
c
o
o
r
d
in
ate
f
r
am
e
as sh
o
wn
in
Fig
u
r
e
5
(
2
1
)
.
{
=
+
∆
=
+
∆
(
2
1
)
W
h
er
e
(
Δ
,
Δ
)
ar
e
tr
an
s
latio
n
o
f
f
s
ets b
etwe
en
th
e
im
ag
e
a
n
d
wo
r
k
s
p
ac
e
o
r
ig
in
s
.
T
h
e
r
esu
ltin
g
en
v
ir
o
n
m
e
n
t
is
r
ep
r
esen
ted
as
a
b
in
ar
y
o
cc
u
p
a
n
cy
g
r
id
with
d
im
en
s
io
n
s
2
4
0
×
3
3
0
,
in
wh
ich
ea
ch
ce
ll
co
r
r
esp
o
n
d
s
to
1
cm
2
o
f
r
ea
l
s
p
ac
e.
T
h
is
g
r
id
en
co
d
es
b
o
t
h
th
e
b
o
u
n
d
ar
y
walls
an
d
th
e
d
etec
ted
s
tatic
o
b
s
tacle
s
.
T
o
d
eter
m
in
e
th
e
r
o
b
o
t’
s
p
o
s
itio
n
,
an
Ar
Uco
m
a
r
k
er
is
m
o
u
n
ted
o
n
its
to
p
s
u
r
f
ac
e.
T
h
e
ce
n
tr
o
id
o
f
th
e
d
etec
ted
m
ar
k
er
in
im
ag
e
c
o
o
r
d
in
ates
(
,
)
is
co
n
v
er
ted
in
to
r
ea
l
-
wo
r
ld
co
o
r
d
in
ates
(
,
)
b
y
(
2
0
)
.
T
h
e
r
o
b
o
t’
s
o
r
ien
tat
io
n
is
esti
m
ated
f
r
o
m
th
e
d
ir
ec
tio
n
v
ec
to
r
b
etwe
en
th
e
to
p
-
lef
t
an
d
to
p
-
r
ig
h
t c
o
r
n
e
r
s
o
f
th
e
m
ar
k
e
r
(
Fig
u
r
e
6
)
(
2
2
)
.
=
−
1
(
−
−
)
(
2
2
)
W
h
er
e
(
,
)
an
d
(
,
)
ar
e
th
e
r
ea
l
-
wo
r
ld
co
o
r
d
i
n
ates
o
f
th
e
two
co
r
n
er
s
.
T
h
e
an
g
le
is
n
o
r
m
alize
d
with
in
th
e
r
an
g
e
[
0
,
2
]
,
co
u
n
ter
clo
c
k
wis
e
in
th
e
u
p
p
e
r
s
em
icir
cle
an
d
clo
ck
wis
e
o
th
e
r
wis
e.
Fo
r
id
en
tify
in
g
g
o
al
p
o
s
itio
n
s
,
d
ig
it
lab
els
attac
h
ed
to
th
e
f
lo
o
r
ar
e
d
etec
ted
u
s
in
g
a
C
o
n
v
Net
d
ee
p
lear
n
in
g
[
3
2
]
tr
ai
n
ed
o
n
th
e
MN
I
ST
d
ataset.
T
h
e
d
ataset
an
d
tr
ain
in
g
co
n
f
i
g
u
r
atio
n
ar
e
s
u
m
m
ar
ized
in
T
ab
le
4
.
T
h
e
ce
n
tr
o
id
o
f
ea
ch
r
ec
o
g
n
ized
d
ig
it
is
tr
an
s
f
o
r
m
e
d
in
to
r
ea
l
-
wo
r
l
d
co
o
r
d
in
ates u
s
in
g
(
2
0
)
,
d
ef
i
n
in
g
th
e
co
r
r
esp
o
n
d
in
g
tar
g
et
lo
ca
ti
o
n
s
in
th
e
wo
r
k
s
p
ac
e.
Fig
u
r
e
5
.
Geo
m
etr
ic
r
elatio
n
s
h
ip
b
etwe
en
th
e
im
a
g
e
co
o
r
d
in
ate
s
y
s
tem
an
d
r
ea
l
-
wo
r
ld
m
ap
c
o
o
r
d
in
ates
Fig
u
r
e
6
.
L
o
ca
lizatio
n
p
r
i
n
cip
l
e
u
s
in
g
Ar
Uco
m
a
r
k
er
an
d
o
r
ie
n
tatio
n
esti
m
atio
n
T
ab
le
4
.
Data
s
et
tr
ain
in
g
a
n
d
C
o
n
v
Net
tr
ain
in
g
c
o
n
f
ig
u
r
atio
n
D
a
t
a
s
e
t
P
a
r
a
me
t
e
r
s
I
n
p
u
t
i
m
a
g
e
s
i
z
e
28
×
2
8
p
i
x
e
l
N
u
mb
e
r
o
f
t
r
a
i
n
i
n
g
i
m
a
g
e
s
60
,
000
N
u
mb
e
r
o
f
t
e
st
i
n
g
i
ma
g
e
s
10
,
000
B
a
t
c
h
si
z
e
8
M
a
x
i
m
u
m
n
u
m
b
e
r
o
f
e
p
o
c
h
s
12
3.
RE
SU
L
T
S AN
D
D
I
SCU
SS
I
O
N
3
.
1
.
Vis
io
n
-
ba
s
ed
perc
ept
io
n a
nd
m
a
pp
ing
re
s
ults
3
.
1
.
1
.
O
bs
t
a
cle
det
ec
t
io
n a
nd
o
cc
up
a
ncy
-
g
rid g
ener
a
t
io
n
T
h
e
en
v
ir
o
n
m
en
tal
m
ap
was
co
n
s
tr
u
cted
f
r
o
m
to
p
-
v
iew
im
ag
es
ca
p
tu
r
ed
b
y
th
e
ce
ilin
g
ca
m
er
a
an
d
p
r
o
ce
s
s
ed
in
MA
T
L
AB
.
Af
ter
ap
p
ly
in
g
im
ag
e
th
r
esh
o
l
d
in
g
an
d
s
eg
m
en
tatio
n
,
th
e
o
b
s
t
ac
le
an
d
f
r
ee
-
s
p
ac
e
r
eg
io
n
s
wer
e
clea
r
ly
d
is
tin
g
u
is
h
ed
.
Fig
u
r
e
7
illu
s
tr
ates
th
e
r
esu
ltin
g
d
ig
ital
m
ap
,
wh
er
e
th
e
id
en
tifie
d
o
b
s
tacle
s
ar
e
r
ep
r
esen
ted
i
n
d
is
tin
ct
co
lo
r
r
e
g
io
n
s
t
o
f
ac
ilit
ate
r
o
b
o
t
n
a
v
ig
atio
n
an
d
en
v
i
r
o
n
m
en
t
m
o
d
elin
g
.
Fig
u
r
e
7
(
a
)
s
h
o
ws th
e
o
v
e
r
h
ea
d
ca
m
er
a
v
iew
u
s
ed
f
o
r
o
b
s
tac
le
d
etec
tio
n
an
d
co
o
r
d
in
ate
tr
a
n
s
latio
n
.
Ob
s
tacle
s
ar
e
d
etec
ted
an
d
en
clo
s
ed
b
y
b
o
u
n
d
in
g
b
o
x
es,
an
d
th
eir
r
ef
e
r
en
ce
p
o
in
ts
ar
e
e
x
p
r
ess
ed
in
th
e
r
o
b
o
t
b
ase
f
r
am
e
Evaluation Warning : The document was created with Spire.PDF for Python.
I
SS
N
:
2
2
5
2
-
8
9
3
8
I
n
t J Ar
tif
I
n
tell
,
Vo
l.
1
5
,
No
.
4
,
Au
g
u
s
t 2
0
2
6
:
3
8
3
6
-
3
8
5
2
3844
(
,
)
as
1
(
1
,
1
)
to
4
(
4
,
4
)
.
T
h
e
n
u
m
b
e
r
ed
m
ar
k
er
s
in
d
icate
g
o
al/lo
ca
tio
n
I
Ds
u
s
ed
f
o
r
n
av
ig
atio
n
.
Fig
u
r
e
7
(
b
)
p
r
esen
ts
th
e
g
en
er
ated
d
i
g
ital
m
ap
in
m
etr
ic
co
o
r
d
in
ates.
T
h
e
d
etec
ted
o
b
s
tacle
s
f
r
o
m
th
e
ca
m
er
a
im
ag
e
ar
e
co
n
v
er
t
ed
in
to
an
o
cc
u
p
an
cy
r
ep
r
esen
tatio
n
,
wh
er
e
b
lack
r
e
g
io
n
s
d
e
n
o
te
o
cc
u
p
ie
d
ce
lls
an
d
wh
ite
r
eg
i
o
n
s
d
en
o
te
f
r
ee
s
p
ac
e,
p
r
o
v
i
d
in
g
a
p
lan
n
i
n
g
m
ap
f
o
r
g
lo
b
al
a
n
d
lo
ca
l
n
av
ig
at
io
n
.
(
a)
(
b)
Fig
u
r
e
7
.
Go
al
p
o
s
itio
n
,
o
b
ject
d
etec
tio
n
,
an
d
co
o
r
d
in
ate
tr
a
n
s
latio
n
:
(
a)
o
b
s
tacle
d
etec
tio
n
an
d
(
b
)
d
ig
ital m
a
p
g
e
n
er
atio
n
Qu
an
titativ
e
r
esu
lts
o
f
o
b
s
tacle
lo
ca
lizatio
n
ac
cu
r
ac
y
ar
e
p
r
e
s
en
ted
in
T
ab
le
5
.
T
h
e
av
er
a
g
e
ab
s
o
lu
te
er
r
o
r
s
in
th
e
x
-
an
d
y
-
co
o
r
d
in
ates
ar
e
0
.
9
5
9
cm
an
d
0
.
6
9
6
cm
,
r
esp
ec
tiv
ely
,
in
d
icatin
g
t
h
at
th
e
v
is
io
n
-
b
ased
d
etec
tio
n
s
y
s
tem
ac
h
iev
es
h
i
g
h
lo
ca
lizatio
n
p
r
ec
is
io
n
.
T
h
ese
r
esu
lts
d
em
o
n
s
tr
ate
th
e
r
o
b
u
s
tn
ess
o
f
th
e
p
r
o
p
o
s
ed
im
ag
e
-
p
r
o
ce
s
s
in
g
p
i
p
elin
e
an
d
th
e
co
o
r
d
in
ate
tr
an
s
f
o
r
m
atio
n
m
o
d
el,
wh
ich
en
ab
le
r
eliab
le
o
b
s
tacle
id
en
tific
atio
n
an
d
m
ap
p
in
g
wi
th
in
th
e
r
ea
l
-
wo
r
ld
wo
r
k
s
p
ac
e.
T
ab
le
5
.
Ob
s
tacle
lo
ca
tio
n
d
et
ec
tio
n
r
esu
lts
O
b
st
a
c
l
e
s
R
e
a
l
c
o
o
r
d
i
n
a
t
e
(
,
)
I
mag
e
c
o
o
r
d
i
n
a
t
e
(
(
,
)
Er
r
o
r
O
b
st
a
c
l
e
1
1
(
1
,
1
)
2
0
4
.
5
0
7
5
.
0
0
2
0
4
.
5
7
7
4
.
6
8
0
.
0
7
-
0
.
3
2
2
4
0
.
0
0
7
5
.
0
0
2
4
3
.
5
1
7
5
.
3
1
3
.
5
1
0
.
3
1
2
4
0
.
0
0
6
0
.
0
0
2
4
3
.
8
2
6
0
.
3
1
3
.
8
2
0
.
3
1
2
0
4
.
5
0
6
0
.
0
0
2
0
4
.
8
9
5
9
.
6
8
0
.
3
9
-
0
.
3
2
O
b
st
a
c
l
e
2
2
(
2
,
2
)
6
0
.
0
0
8
6
.
5
0
5
7
.
7
6
8
7
.
4
4
-
2
.
2
4
0
.
9
4
1
0
2
.
0
0
8
6
.
5
0
1
0
3
.
0
8
8
7
.
4
4
1
.
8
0
0
.
9
4
1
0
2
.
0
0
6
0
.
0
0
1
0
3
.
0
8
5
9
.
6
8
1
.
0
8
-
0
.
3
2
6
0
.
0
0
6
0
.
0
0
5
7
.
7
6
5
9
.
6
8
-
2
.
2
4
-
0
.
3
2
O
b
st
a
c
l
e
3
3
(
3
,
3
)
2
1
0
.
0
0
1
7
7
.
0
0
2
1
1
.
2
7
1
7
8
.
4
0
1
.
2
7
1
.
4
0
2
5
1
.
0
0
1
7
7
.
0
0
2
5
3
.
4
0
1
7
9
.
0
4
2
.
4
0
2
.
0
4
2
5
1
.
0
0
1
5
0
.
0
0
2
5
4
.
0
4
1
5
0
.
9
5
3
.
0
4
0
.
9
5
2
1
0
.
0
0
1
5
0
.
0
0
2
1
1
.
5
9
1
5
0
.
3
1
1
.
5
9
0
.
3
1
O
b
st
a
c
l
e
4
4
(
4
,
4
)
9
2
.
0
0
1
8
0
.
0
0
9
0
.
6
3
1
8
2
.
2
3
-
1
.
3
7
2
.
2
3
1
1
6
.
0
0
1
8
0
.
0
0
1
1
6
.
8
0
1
8
2
.
2
3
0
.
8
0
2
.
2
3
1
1
6
.
0
0
1
5
6
.
0
0
1
1
6
.
8
0
1
5
6
.
3
8
0
.
8
0
0
.
3
8
9
2
.
0
0
1
5
6
.
0
0
9
0
.
6
3
1
5
6
.
3
8
0
.
6
3
0
.
3
8
A
v
e
r
a
g
e
e
r
r
o
r
0
.
9
5
9
0
.
6
9
6
3
.
1
.
2
.
Co
nv
o
lutio
na
l
neura
l
net
wo
rk
-
ba
s
ed
g
o
a
l
-
m
a
r
k
er
re
co
g
nitio
n
A
C
NN
C
o
n
v
Net
[
3
2
]
was
t
r
ain
ed
to
r
ec
o
g
n
ize
d
i
g
it
m
ar
k
er
s
s
er
v
in
g
as
g
o
al
id
e
n
tifie
r
s
f
o
r
th
e
m
o
b
ile
r
o
b
o
t
.
T
o
im
p
r
o
v
e
g
e
n
er
aliza
tio
n
,
d
ata
au
g
m
e
n
tatio
n
tech
n
iq
u
es
wer
e
ap
p
lied
d
u
r
in
g
tr
ain
in
g
.
T
h
e
tr
ain
in
g
co
n
f
ig
u
r
atio
n
is
s
u
m
m
ar
ized
in
T
ab
le
4
,
with
a
b
at
ch
s
ize
o
f
8
,
1
2
ep
o
c
h
s
,
an
d
ea
r
ly
s
to
p
p
in
g
e
n
ab
led
to
p
r
e
v
en
t
o
v
er
f
itti
n
g
.
As
s
h
o
wn
in
Fig
u
r
e
8
,
b
o
th
tr
ain
i
n
g
an
d
v
alid
atio
n
ac
c
u
r
ac
ies
in
cr
ea
s
ed
r
a
p
id
ly
,
r
ea
ch
in
g
a
p
p
r
o
x
im
ately
9
9
%
af
ter
th
e
f
o
u
r
th
ep
o
ch
,
w
h
ile
th
e
co
r
r
esp
o
n
d
in
g
lo
s
s
cu
r
v
es
d
ec
r
ea
s
ed
an
d
co
n
v
er
g
ed
s
m
o
o
th
ly
.
T
h
is
b
e
h
av
io
r
in
d
icate
s
th
at
th
e
m
o
d
el
lear
n
ed
ef
f
icien
tly
with
o
u
t
s
ig
n
s
o
f
o
v
er
f
itti
n
g
.
E
v
a
l
u
a
t
i
o
n
m
e
t
r
i
c
s
l
i
s
t
e
d
i
n
T
a
b
l
e
6
d
e
m
o
n
s
t
r
a
t
e
c
o
n
s
i
s
t
e
n
t
r
e
s
u
l
t
s
a
c
r
o
s
s
a
l
l
c
l
a
s
s
e
s
,
w
i
t
h
a
v
e
r
a
g
e
p
r
e
c
i
s
i
o
n
=
0
.
9
9
,
r
ec
all
=
0
.
9
9
,
an
d
F1
-
s
co
r
e
=
0
.
9
9
(
Fig
u
r
e
8
(
a)
)
.
T
h
e
u
n
if
o
r
m
ity
am
o
n
g
class
es,
with
s
ev
er
al
ac
h
iev
in
g
p
er
f
ec
t
s
co
r
es
(
1
.
0
0
)
,
c
o
n
f
ir
m
s
th
at
th
e
C
NN
m
o
d
el
is
well
-
b
alan
ce
d
an
d
u
n
b
iased
.
W
ith
an
o
v
er
all
ac
cu
r
ac
y
o
f
9
9
%
o
n
1
0
,
0
0
0
test
im
ag
e
s
,
th
e
tr
ain
e
d
m
o
d
el
p
r
o
v
es
e
f
f
ec
tiv
e
an
d
r
eliab
le
f
o
r
p
r
ac
t
ical
d
ep
lo
y
m
en
t
i
n
g
o
al
d
etec
tio
n
task
s
.
I
n
Fig
u
r
e
8
(
b
)
,
th
e
tr
ai
n
in
g
lo
s
s
an
d
v
alid
atio
n
lo
s
s
o
v
er
e
p
o
ch
s
.
B
o
th
cu
r
v
es
d
ec
r
ea
s
e
s
h
ar
p
ly
d
u
r
in
g
th
e
f
ir
s
t
f
ew
ep
o
ch
s
an
d
t
h
en
d
ec
lin
e
m
o
r
e
g
r
ad
u
ally
,
in
d
icatin
g
s
tab
le
co
n
v
er
g
en
c
e.
T
h
e
Evaluation Warning : The document was created with Spire.PDF for Python.
I
n
t J Ar
tif
I
n
tell
I
SS
N:
2252
-
8
9
3
8
E
xp
erimen
ta
l v
a
lid
a
tio
n
o
f v
is
io
n
-
b
a
s
ed
h
yb
r
id
n
a
vig
a
tio
n
f
o
r
d
iffer
en
tia
l
…
(
Th
a
n
h
Qu
a
n
g
Du
c
Le
)
3845
v
alid
atio
n
lo
s
s
r
em
ain
s
lo
w
an
d
r
elativ
ely
s
tab
le,
with
a
s
m
all
in
cr
ea
s
e
ar
o
u
n
d
ep
o
ch
s
1
0
–
1
1
,
wh
ich
m
ay
s
u
g
g
est
m
ild
o
v
e
r
f
itti
n
g
;
h
o
w
ev
er
,
th
e
o
v
e
r
all
g
ap
b
etwe
e
n
th
e
two
cu
r
v
es
s
tay
s
s
m
all,
im
p
ly
in
g
th
at
th
e
m
o
d
el
m
ain
tain
s
g
o
o
d
g
en
e
r
aliza
tio
n
p
er
f
o
r
m
an
ce
.
Fig
u
r
e
9
illu
s
tr
ates
th
e
f
in
al
lo
ca
lizatio
n
r
esu
lts
,
co
n
f
ir
m
in
g
th
at
th
e
p
r
o
p
o
s
ed
C
NN
-
b
ased
ap
p
r
o
ac
h
ca
n
ac
cu
r
ately
d
eter
m
in
e
t
h
e
p
o
s
itio
n
s
o
f
tar
g
et
p
o
in
ts
with
n
eg
lig
ib
le
er
r
o
r
s
—
s
u
f
f
icien
t f
o
r
p
r
ec
is
e
p
ath
p
lan
n
in
g
a
n
d
r
o
b
o
t n
a
v
ig
atio
n
i
n
s
tatic
en
v
ir
o
n
m
en
ts
.
(
a)
(
b)
Fig
u
r
e
8
.
Mo
d
el
tr
ain
in
g
an
d
v
alid
atio
n
o
f
(
a)
ac
cu
r
ac
y
an
d
(
b
)
lo
s
s
T
ab
le
6
.
T
h
e
C
NN
m
o
d
el
p
er
f
o
r
m
an
ce
P
r
e
d
i
c
t
e
d
R
e
c
a
l
l
F1
-
sc
o
r
e
0
1
.
0
0
1
.
0
0
1
.
0
0
1
1
.
0
0
1
.
0
0
1
.
0
0
2
0
.
9
9
0
.
9
9
0
.
9
9
3
0
.
9
9
1
.
0
0
0
.
9
9
4
0
.
9
9
0
.
9
9
0
.
9
9
5
1
.
0
0
0
.
9
9
0
.
9
9
6
1
.
0
0
1
.
0
0
1
.
0
0
7
0
.
9
9
0
.
9
9
0
.
9
9
8
0
.
9
9
1
.
0
0
1
.
0
0
9
0
.
9
9
0
.
9
9
0
.
9
9
A
v
e
r
a
g
e
w
e
i
g
h
t
e
d
0
.
9
9
0
.
9
9
0
.
9
9
Fig
u
r
e
9
.
Po
s
itio
n
d
etec
tio
n
3
.
2
.
L
o
ca
liza
t
io
n us
ing
o
do
m
et
ry
a
nd
ArU
co
co
rr
ec
t
io
n
T
wo
lo
ca
lizatio
n
m
eth
o
d
s
we
r
e
test
ed
:
i
)
u
s
in
g
o
d
o
m
et
r
y
f
r
o
m
wh
ee
l
e
n
co
d
e
r
s
an
d
ii
)
c
o
m
b
in
in
g
o
d
o
m
etr
y
with
Ar
Uco
-
m
ar
k
er
d
etec
tio
n
.
Fig
u
r
e
1
0
c
o
m
p
ar
es
th
e
tr
ajec
to
r
ies
esti
m
ated
b
y
th
e
two
lo
ca
lizatio
n
m
eth
o
d
s
with
th
e
r
ea
l
(
g
r
o
u
n
d
-
tr
u
t
h
)
tr
ajec
to
r
y
.
T
h
is
co
m
p
ar
is
o
n
q
u
an
tifi
es
th
e
lo
ca
lizatio
n
ac
cu
r
ac
y
im
p
r
o
v
em
e
n
t
ac
h
ie
v
ed
th
r
o
u
g
h
Ar
Uco
-
m
ar
k
er
co
r
r
ec
tio
n
.
I
n
d
etail,
Fig
u
r
e
1
0
(
a
)
s
h
o
ws
th
e
tr
ajec
to
r
y
esti
m
ated
b
y
o
d
o
m
etr
y
alo
n
e.
At
f
ir
s
t,
th
e
p
ath
m
atch
es
th
e
r
ea
l
tr
ajec
to
r
y
,
b
u
t
er
r
o
r
s
g
r
a
d
u
all
y
in
cr
ea
s
e
d
u
e
to
wh
ee
l
s
lip
an
d
en
co
d
e
r
n
o
is
e.
Fig
u
r
e
1
0
(
b
)
s
h
o
ws
th
e
r
esu
lt
o
f
th
e
co
m
b
in
ed
m
eth
o
d
.
W
h
en
th
e
r
o
b
o
t
d
etec
ts
Ar
Uco
m
a
r
k
er
s
,
its
g
lo
b
al
p
o
s
itio
n
is
co
r
r
ec
ted
,
wh
ich
r
em
o
v
es
th
e
d
r
if
t
f
r
o
m
o
d
o
m
etr
y
.
T
h
e
co
m
b
i
n
ed
ap
p
r
o
ac
h
ac
h
ie
v
es sm
aller
er
r
o
r
s
,
ar
o
u
n
d
0
.
0
1
–
0
.
0
5
m,
a
n
d
p
r
o
v
id
es m
o
r
e
s
tab
le
lo
ca
lizatio
n
.
Evaluation Warning : The document was created with Spire.PDF for Python.