Intern
ati
o
n
a
l Jo
urn
a
l
o
f
R
o
botics
a
nd Au
tom
a
tion
(I
JR
A)
V
o
l.
4, N
o
. 1
,
Mar
c
h
20
15
,
pp
. 1
~
1
8
I
S
SN
: 208
9-4
8
5
6
1
Jo
urn
a
l
h
o
me
pa
ge
: h
ttp
://iaesjo
u
r
na
l.com/
o
n
lin
e/ind
e
x.ph
p
/
IJRA
Lead
er-
F
ollower Trackin
g
System
for Agri
cultural
Vehicl
es
:
Fusion of Laser and Odometry
P
o
siti
oning Using Ext
e
nded
Kalman Filter
Linhuan
Z
h
ang,
Tom
o
hir
o
Takigawa, Tofael
Ah
amed
Graduate School of Life and
Env
i
ronmen
tal Scien
ces, Univ
ersity
o
f
Tsukuba,
1-1-1 Tennod
ai,
Tsukuba, 305
-85
72, Japan
Article Info
A
B
STRAC
T
Article histo
r
y:
Received
J
u
l 10, 2014
Rev
i
sed
Sep
11
, 20
14
Accepte
d Oct 5, 2014
The
aim
of th
is
res
ear
ch wa
s
to
dev
e
lop
a safe human-driven
an
d
autonomous leader-follower
tr
acking s
y
st
em fo
r an
autonomou
s tractor. To
enabl
e
th
e
tra
c
k
ing s
y
s
t
em
,
a
las
e
r
rang
e f
i
nder (LRF
)-b
as
ed l
a
ndm
ark
detection
s
y
stem was designed
to obs
erve
the relativ
e positio
n between
a
lead
er and
a fo
l
l
ower us
ed in
a
g
ricultu
ral
opera
tions
. Th
e vir
t
u
a
l fol
l
ower-
based fo
rm
ation
-
track
ing
algor
it
hm
wa
s dev
e
lo
ped
to
minimize
tracking
errors and ensur
e
safety
. An ex
tended
Ka
l
m
a
n
filt
e
r
(E
KF) wa
s impl
e
me
nt
e
d
for fusing LRF and odom
etr
y
position to
ensure st
abili
t
y
of t
r
ack
in
g in noi
s
y
farmland cond
itions. Simulations
were c
ondu
cted
for tracking
the lead
er in
small and large sinusoidal cu
r
v
ed path
s. Simulated results v
e
rified high
accur
a
c
y
of fo
r
m
ation tra
c
king
,
s
t
able v
e
loc
i
t
y
,
and regul
at
ed s
t
eering
angle
of th
e follower.
The tr
acking
method
confirmed
the fo
llower
could follow
the
lead
er with
a re
quired form
ation
safel
y
and stead
il
y in no
is
y
con
d
itions.
Th
e
EKF helped to
improve observation
ac
c
u
ra
cy
, ve
l
o
ci
ty
,
and steering
angle
s
t
abili
t
y
o
f
the
f
o
llower.
As
a r
e
s
u
lt of
th
e
im
proved
accu
rac
y
of
obs
ervation
and motion action, the tr
acking
performan
ce fo
r lateral,
longitudinal, an
d
heading
were
al
so im
proved
aft
e
r th
e
EKF was
im
plem
ented
in
the
tra
c
king
sy
s
t
e
m
.
Keyword:
D
a
ta Fu
sing
Ex
tend
Kalm
a
n
Filter
Form
at
i
on of m
u
lt
i
p
l
e
ro
bot
s
LRF-landm
a
rk
No
nl
i
n
ea
r a
n
d
no
n
hol
on
om
i
c
Copyright ©
201
5 Institut
e
o
f
Ad
vanced
Engin
eer
ing and S
c
i
e
nce.
All rights re
se
rve
d
.
Co
rresp
ond
i
ng
Autho
r
:
Tom
ohi
ro
Ta
ki
gawa
,
Gra
d
uat
e
Sc
ho
ol
o
f
Li
fe a
n
d
En
vi
ro
nm
ent
a
l
Sci
e
nces
,
U
n
i
v
er
sity
of
Tsuk
ub
a,
1-
1-
1 Ten
n
oda
i
,
Tsu
k
uba
, 30
5-
8
5
7
2
,
Japa
n
Em
ai
l
:
t
a
ki
gaw
a
.t
om
ohi
ro
.f
f
@
u
.
t
s
u
k
u
b
a.ac
.j
p
1.
INTRODUCTION
Th
e ch
alleng
es of
h
i
gh
-qu
a
lity ag
ricu
ltural p
r
od
u
c
tion
an
d lack
of
farmin
g
workforce d
e
m
a
n
d
ch
ang
e
s in
t
h
e trad
ition
a
l ag
ricu
ltu
ral
p
r
od
u
c
tion
system
. Th
e d
e
v
e
lo
p
m
en
t of au
to
no
m
o
u
s
ag
ri
cu
ltu
ral
mach
in
ery creates th
e
o
ppo
rtu
n
ity to
sh
ift
con
v
e
n
tion
a
l
ag
ricu
ltu
re to
an
in
tellig
en
t
ag
ricu
ltu
ral sy
ste
m
.
Aut
o
n
o
m
ous a
g
ri
c
u
l
t
u
ral
m
a
chi
n
e
r
y
co
ul
d e
n
su
re
preci
se
op
eration
,
i
n
crease produ
ctiv
ity, min
i
m
i
ze th
e size
o
f
th
e r
e
qu
ir
ed
w
o
r
k
fo
r
c
e,
an
d
i
m
p
r
ov
e p
r
odu
ctio
n.
In t
h
e pa
st
fe
w deca
des
,
n
u
m
erous
st
u
d
i
e
s ha
ve bee
n
p
e
rf
orm
e
d o
n
n
a
vi
gat
i
o
n
of a
u
t
o
nom
ou
s
t
r
act
ors
,
i
n
cl
u
d
i
n
g
po
si
t
i
oni
ng
,
dri
v
i
n
g,
a
n
d
st
eeri
n
g
c
ont
rol
f
unct
i
o
ns.
A
d
vance
d
sensi
n
g
t
ech
nol
ogi
es
,
cont
rol
t
h
eo
ri
e
s
, a
n
d
hi
gh
ac
curacy
c
o
nt
r
o
l
o
f
a
u
t
o
nom
ou
s t
r
act
o
r
s
ha
ve
bee
n
de
vel
o
p
e
d
[1]
-
[
4]
.
H
o
weve
r,
m
o
st
of t
h
e
pr
evi
o
us
resea
r
c
h
foc
u
se
d
o
n
t
h
e
na
vi
gat
i
o
n
of
a si
ngl
e
t
r
ac
t
o
r.
T
h
ere
i
s
i
n
fact
a
st
r
o
ng
n
eed
f
o
r
co
op
eratio
n
b
e
tween m
u
ltip
le
m
ach
in
es in ag
ricu
ltu
ral
op
eratio
n. On
e o
f
th
e
typ
i
cal
applicatio
n
s
o
f
m
u
ltip
le
machines ca
n
be
obse
rve
d
in harves
t
o
p
erat
i
ons
. D
u
ri
ng
h
a
rvest
ope
rat
i
o
ns,
a
f
o
l
l
o
wer
t
r
act
or
i
s
re
qui
red
t
o
k
eep fo
rm
atio
n
with
a leader co
m
b
in
e
(Fig
.1
a)
o
r
op
eratio
n
o
f
m
u
ltip
le h
a
rvesters (Fig.1b). Th
i
s
is an
ard
u
ous
a
n
d
d
a
nge
r
ous
t
a
s
k
fo
r
d
r
i
v
er
s
w
h
o
ha
ve t
o
f
o
cu
s t
h
ei
r
at
t
e
nt
i
o
n
f
o
r
a l
o
n
g
t
i
m
e. A
n
a
u
t
o
n
o
m
ous
Evaluation Warning : The document was created with Spire.PDF for Python.
I
S
SN
:
2
089
-48
56
IJR
A
V
o
l
.
4,
No
. 1,
M
a
rc
h 20
1
5
:
1 – 1
8
2
track
ing
fo
llower is a
go
od cho
i
ce
for
not on
ly so
l
v
ing th
e safety
p
r
o
b
l
em
b
u
t
also
im
p
r
ov
ing
work
i
n
g
p
e
rform
a
n
ce [5
]. Co
m
p
ared
to
th
e
n
a
v
i
g
a
tio
n
of a sing
le
tracto
r
robo
t, a
m
u
lti-tracto
r
ro
bo
t system
is
m
u
ch
m
o
re di
ffi
c
u
l
t
and
i
n
vol
ves a
m
u
ch hi
g
h
er
o
r
der
o
f
c
o
m
p
l
e
xi
t
y
[6]
.
(a)
(
b
)
Fig
.
1
.
Op
eration
o
f
m
u
ltip
le a
g
ricu
ltu
ral m
ach
in
es;
(a) Com
b
in
e an
d
t
r
acto
r;
(b) Mu
ltip
l
e
h
a
rv
esters
Co
n
s
i
d
eri
n
g track
ing stab
ility and safety, t
h
e prim
ary p
r
ob
lem
is d
e
sign
i
n
g a su
itab
l
e
sen
s
ing system
th
at can
pr
o
v
i
d
e c
o
nst
a
nt
an
d
preci
se
obs
er
vat
i
o
n
o
f
t
h
e rel
a
t
i
v
e
p
o
s
i
t
i
on o
f
t
w
o
v
e
hi
cl
es. T
h
e se
nsi
n
g sy
st
em
sho
u
l
d
be c
o
m
p
et
ent
un
de
r c
h
a
ngi
n
g
m
ovem
e
nt
and
post
u
re
of
t
h
e t
w
o
vehi
cl
e
s
.
A m
a
st
er-sl
a
ve t
r
act
or
-
b
ase
d
a
u
t
o
t
r
acki
n
g sy
st
e
m
was de
vel
o
p
e
d u
s
i
n
g a R
T
K-
GPS a
n
d Gy
rosc
o
p
e w
h
i
c
h
pr
o
v
i
d
e
d
p
o
si
t
i
onal
i
n
f
o
rm
at
ion
[
7
]
.
The sy
st
em
was t
e
st
ed
usi
n
g
Fen
d
t
9
36
m
odel
t
r
act
ors
,
a
n
d the trac
king e
r
ror
was less t
h
an
20 cm
on a
curve
d
p
a
th
. Th
e lack of sign
al co
rrectio
n
du
ring
i
n
terrup
tio
ns in th
e
GPS and
th
e add
ition
a
l
co
st
o
f
a
GPS and
Gyroscop
e m
a
k
e
th
is system
su
bop
tim
a
l
fo
r so
lv
ing
th
e track
ing
prob
lem
.
Ad
d
itio
n
a
lly, GPS
p
r
ov
ides o
n
l
y
ab
so
lu
te po
sitio
n
info
rm
atio
n
.
Wh
ile
track
i
n
g
, we n
eed
t
o
con
tinu
a
lly u
p
d
a
te th
e relativ
e
d
i
stan
ce between
th
e lead
er and
th
e fo
llower.
If th
e
GP
S signal is in
terrup
ted
,
th
ere is a
p
o
ssib
ility o
f
co
llisio
n
i
n
t
r
ack
i
n
g
,
or
th
e d
e
v
e
l
o
p
m
en
t o
f
a
larg
e o
f
fset du
e
t
o
th
e
loss o
f
u
p
d
a
ted
po
sitional
in
fo
rm
atio
n.
To
ov
erco
me
th
e
l
i
m
i
t
a
t
i
ons o
f
r
e
l
a
t
i
v
e di
st
anc
e
m
easurem
ent usi
n
g
GPS
a
n
d sa
fet
y
c
once
r
ns,
a l
o
w
c
o
st
and
preci
se
ul
t
r
aso
n
i
c
sensor-based syste
m
has bee
n
successfully
applied on
a
n
aut
o
trac
king system
for mu
ltiple c
o
m
b
ines [8].
Howev
e
r, th
e
sh
ort
d
e
tectio
n d
i
stan
ce and
l
i
m
i
ted
d
e
tectio
n
a
ngl
e of
t
h
e ul
t
r
aso
n
i
c
se
ns
or o
f
t
e
n resul
t
ed
i
n
l
o
ss
of
t
h
e t
a
rg
et
. I
n
ot
he
r re
s
earch
,
posi
t
i
o
n
det
ect
i
o
n
of a
l
eader
ve
hi
cl
e was c
o
nd
uct
e
d
usi
n
g
a LR
F
,
whi
c
h
w
a
s
installed
o
n
a
fo
llow
e
r
v
e
h
i
cle [
9
].
Th
e
lead
er
b
ody
r
ecog
n
i
ze-
b
ased
algor
ith
m
w
a
s o
b
s
erv
e
d th
at
a
nota
b
le m
easurem
ent error
would
occur i
n
t
h
e c
u
rve
d
p
a
t
h
.
H
o
we
ver
,
usi
n
g
a
n
LR
F
was
c
onsi
d
er
ed as
a
pote
n
tial
m
e
thod for determ
ining
m
u
ltiple
tractors’ rela
tive
position. As the
LRF
coul
d
not only provi
de
the
d
i
stan
ce to
an o
b
j
ect bu
t also
po
sition
,
mo
tio
n, and
d
i
rectio
n
qu
ick
l
y
with
h
i
g
h
accu
racy
o
v
e
r a wid
e
det
ect
i
o
n
an
gl
e
[
10]
.
In the t
r
acki
n
g of a leade
r
-fol
lowe
r, th
ere are li
mitat
i
o
n
s
of th
e con
t
ro
l syste
m
d
u
e
to
no
n
lin
ear an
d
no
n
hol
on
om
i
c
con
s
t
r
ai
nt
s
[
1
1
]
. I
n
t
h
e m
u
l
t
i
-ro
b
o
t
navi
gat
i
o
n sy
st
em
, t
h
e l
eader
al
way
s
c
ont
rol
s
i
t
s
ow
n
way
an
d m
o
tio
n strateg
y
, and
t
h
e
fo
llower
d
ecides its action based on th
e rel
a
tiv
e
p
o
s
ition
an
d cu
rren
t
act
io
n
of
th
e lead
er.
On th
e o
t
h
e
r h
a
nd
, i
n
a
d
y
n
a
mic track
ing
syste
m
, th
ere is an
u
n
a
vo
id
ab
le ti
m
e
d
e
lay for the
fo
llower to resp
ond
. A con
t
ro
l law th
at
can
q
u
i
ck
ly an
d c
o
r
r
ectly
res
p
o
n
d
to
trac
king
er
ro
r is
re
qui
red
.
Sim
u
l
a
t
i
on of
sl
i
d
i
n
g
m
ode
c
ont
rol
an
d si
m
p
l
e
P
D
co
n
t
ro
l w
e
r
e
pr
opo
sed
t
o
so
lv
e th
e
tr
ack
ing pr
ob
l
e
m
f
o
r
m
a
st
er–sl
a
ve t
r
act
or
s [
1
2]
. T
h
ese t
w
o m
e
t
hods
we
re c
o
m
p
are
d
, a
n
d i
t
was
obs
er
ved
t
h
at
t
h
e sl
i
d
i
n
g m
ode
cont
rol
had
bet
t
er per
f
o
rm
ance i
n
im
pro
v
i
n
g
l
a
t
e
ral
offset
a
nd s
p
aci
n
g
c
o
n
t
rol
s
. St
ri
ct
fee
dbac
k
c
ont
rol
usi
n
g
Lyapunov’s
se
cond m
e
thod,
base
d
on t
h
e c
h
aine
d
form
,
was
succe
ssful
ly designe
d
for sol
v
ing m
u
ltip
le non-
hol
onom
ic
m
o
bile robot
proble
m
s
[13]
,
[14]
. Model pre
d
ictive control [15]
and
recedi
n
g
horizon c
ont
rol [16]
were also su
ccessfu
lly u
ltili
zed
for
add
r
essin
g
th
e fo
rm
atio
n
con
t
ro
l
p
r
ob
lem
o
f
mu
ltip
le non
ho
l
o
no
m
i
c
m
o
b
ile ro
bo
ts.
In an a
dve
rse
envi
ronm
ent such
as agricul
t
ural ope
rations,
a prop
e
r
se
nsi
n
g a
n
d c
o
n
t
rol
sy
st
em
alone
are
insufficient to e
n
sure the
stability and sa
fety trac
king
of t
h
e le
a
d
er a
n
d the
fol
l
owe
r
. Because the
r
e
are sources
of noise from
sensors,
for exa
m
ple, farmland surface c
o
nditi
ons ca
use a
large
odom
etry error
[1
7]
, l
a
ser det
ect
i
on i
s
affect
ed by
s
w
i
n
gi
n
g
o
f
t
h
e ve
hi
cl
e bo
dy
[
18]
, a
nd
d
u
st
an
d st
ro
n
g
su
ns
hi
ne
m
a
ke
l
a
ser det
ect
i
o
n
di
f
f
i
c
ul
t
[
19]
.
Th
us,
o
b
t
a
i
n
i
n
g
co
rrect
o
b
s
e
rvat
i
o
n i
n
f
o
r
m
at
i
on fr
om
noi
sy
si
g
n
al
s i
s
an
ot
her
Evaluation Warning : The document was created with Spire.PDF for Python.
IJECE
ISS
N
:
2088-8708
Title o
f
ma
nu
scrip
t
is sh
o
r
t
an
d clea
r, imp
lies resea
r
ch resu
lts (First Au
tho
r
)
3
k
e
y issu
e
fo
r
stab
ility an
d safety track
i
ng
. Unfortun
ately,
n
o
t
m
u
ch
research
h
a
s b
e
en
p
e
rfo
r
m
e
d
t
o
so
l
v
e
noi
se
re
d
u
ct
i
o
n
pr
o
b
l
e
m
s
du
r
i
ng
t
h
e
f
o
rm
at
ion
t
r
ac
ki
n
g
o
f
m
u
l
tip
le tractors i
n
ag
ricu
ltu
ral app
licatio
n
.
As an
effect
i
v
e m
e
t
hod
, E
K
F
was
m
o
st
oft
e
n ap
p
l
i
e
d fo
r
dat
a
f
u
si
ng a
n
d n
o
i
s
e
red
u
ct
i
o
n [
2
0]
. A LR
F-
base
d
EKF
p
o
s
ition
estimatio
n
system
i
n
tree
fru
it orch
ard
s
was
d
e
sig
n
e
d
,
and
fi
eld
tests sho
w
ed
th
at th
e positio
n
est
i
m
a
ti
on sy
s
t
em
wor
k
s
wi
t
h
s
u
f
f
i
c
i
e
nt
a
ccuracy
[
21]
.
To
o
v
erc
o
m
e
t
h
e p
r
obl
em
s of
n
o
i
s
y
co
n
d
i
t
i
ons
d
e
scr
i
b
e
d abov
e, LR
F-
landmar
k
and
Odom
etry-based
fusi
ng can be
us
ed
in th
e
cu
rv
e-p
a
th fo
rmatio
n
track
ing
.
On
e
po
ten
tial mean
s is a LRF-land
m
a
rk
-b
ased
syste
m
, wh
ere a v
i
rt
ual fo
llo
wer-b
a
sed
feedb
ack
cont
rol
sy
st
em
an
d
E
K
F
f
u
si
ng
sy
st
em
cou
l
d
be
i
m
pl
em
ent
e
d
t
o
get
h
e
r
.
The
LR
F-l
a
n
d
m
ark-
base
d m
e
t
h
o
d
coul
d be
use
d
t
o
det
ect
t
h
e rel
a
t
i
v
e po
si
t
i
on
bet
w
ee
n t
h
e l
eade
r
an
d
t
h
e fol
l
o
we
r.
The LR
F c
o
ul
d
be
im
plem
ented on the follower, and
refl
ectors
m
ounte
d
on t
h
e leade
r
can
be use
d
as la
ndm
a
rks. Utilizing t
h
e
g
e
o
m
etric relatio
n
s
h
i
p b
e
t
w
een
th
e LRF and th
e land
m
a
rk
s, th
e
relativ
e
po
sitio
n b
e
t
w
een
th
e lead
er and
th
e
fo
llower cou
l
d easily
b
e
calcu
l
ated
. Land
mark
d
e
tectio
n
h
a
s alread
y
b
e
en u
tilized and has
prov
en to hav
e
h
i
gh
p
r
ecisi
o
n
and
stab
ility in
o
u
r
p
r
ev
i
o
us research
[22]. Th
e v
i
rtu
a
l
fo
llo
we
r-b
a
sed
feedb
a
ck
-t
rack
ing
alg
o
rith
m
h
a
s th
e po
ten
tial to
en
sure safe
track
ing
,
wh
ere th
e
v
i
rtu
a
l
fo
llo
wer can
main
tain
th
e
requ
ired
p
o
s
ition
w
ith
t
h
e lead
er
.
Th
e EK
F can
b
e
used
to fu
se
o
d
o
m
etr
y
d
a
ta w
ith
LRF po
sitio
n. In
t
h
e odometer
s,
th
e ro
tary
en
co
d
e
rs in
stalled on rear
wheels and
t
h
e
l
i
n
e
a
r e
n
c
ode
r i
n
s
t
al
l
e
d o
n
st
ee
r
i
ng
pa
rt
pr
o
v
i
d
e t
h
e
position a
nd
posture of the
vehicles. Enc
o
ders ca
n su
pport ra
pid a
n
d
accurate
data collection,
but their
dra
w
backs come from
accum
u
la
ted errors
and they are
se
nsitive to sl
ope
and une
v
en surface
[23]. By
fusi
ng
o
d
o
m
etry d
a
ta an
d
laser-b
a
sed
po
sition
i
ng
d
a
ta with
an
EKF, t
h
e bou
nded
no
isy laser
sig
n
a
l can
ov
erco
m
e
the
unbounded accum
u
lated e
r
ro
r of
odom
etry [24]. A s
h
ort-duration sm
ooth signal
of odom
etry can al
so
be
u
s
ed
to
su
ppo
rt th
e laser po
si
tio
n
,
an
d
t
h
en
a real-v
al
u
e
n
e
ar-estim
ated
relativ
e p
o
s
ition
b
e
tween
th
e lead
er
an
d
th
e fo
llo
wer
can
b
e
d
e
termin
ed
.
Thu
s
, t
h
e
o
b
j
ectiv
es of th
is
research
were as
fo
llows:
1
)
To d
e
v
e
lop a track
i
ng
sy
ste
m
fo
r m
u
ltip
le agricu
ltural
m
ach
in
ery co
m
b
in
atio
n
s
,
with
a lead
er
an
d a
fol
l
o
wer
,
i
n
cl
u
d
i
n
g
vi
rt
ual
f
o
l
l
o
we
r-
base
d fe
edbac
k
c
o
nt
r
o
l
.
2
)
To d
e
v
e
lop
a laser-lan
d
m
ark
b
a
sed
t
r
ack
i
n
g system
to
id
en
tify th
e
relativ
e po
sition
b
e
tween
t
h
e leader an
d
th
e fo
llower.
3
)
To in
trod
u
c
e an EKF
fu
si
n
g
system
to
im
p
r
o
v
e
accuracy o
f
th
e leader-fo
llower rel
a
tiv
e po
sitio
n
in
th
e
vi
rt
ual
fol
l
owe
r
-
b
ase
d
fee
d
ba
ck c
ont
rol
sy
st
em
.
2.
LEADER
-
F
O
LLOWER F
O
RM
ATIO
N
S
Y
STEM
In t
h
i
s
resea
r
c
h
, a l
eade
r
-
f
ol
l
o
wer
-
ba
sed
fo
r
m
at
i
on sy
st
em
was
pr
op
ose
d
.
The
poi
nt
an
d t
h
e ar
r
o
w i
n
(Fig.2
a) represen
t
th
e req
u
i
red
p
o
s
itio
n
o
f
the fo
llo
wer an
d
a v
i
rt
u
a
l fo
llower was im
ag
e
d
th
ere
(Fig.2b).
(a)
(b)
Fi
g.
2. R
e
l
a
t
i
o
n
s
hi
p
o
f
l
eade
r
,
fol
l
o
wer
an
d
vi
rt
ual
f
o
l
l
o
wer:
(a) R
e
q
u
i
r
ed
f
o
rm
ati
on;
(
b
)
P
o
si
t
i
on
of
vi
rt
ua
l
fo
llower.
Evaluation Warning : The document was created with Spire.PDF for Python.
I
S
SN
:
2
089
-48
56
IJR
A
V
o
l
.
4,
No
. 1,
M
a
rc
h 20
1
5
:
1 – 1
8
4
Th
e relativ
e
positio
n
b
e
t
w
een
th
e lead
er and
v
i
rtu
a
l fo
llowe
r was set
t
o
a c
onst
a
nt
. B
y
t
r
a
c
ki
n
g
t
h
e
po
si
t
i
on
of
vi
rt
ual
fol
l
o
we
r, sa
fet
y
and
r
e
qui
red
fo
rm
ati
on
bet
w
ee
n t
h
e l
eade
r
an
d
t
h
e f
o
l
l
o
we
r c
oul
d be
o
b
t
a
i
n
ed.
In
ad
d
ition
,
th
e inform
at
io
n
abou
t th
e v
e
l
o
city an
d steeri
n
g
ang
u
l
ar v
e
lo
city o
f
th
e
lead
er
co
u
l
d
b
e
sen
t
fro
m
th
e
lead
er t
o
th
e follo
wer.
3.
FOR
M
ATIO
N TR
A
C
KI
N
G
AL
GO
RIT
H
M
Th
is section
describ
e
s a so
l
u
tio
n of t
h
e
fo
rm
a
tio
n
-
trackin
g
prob
lem
fo
r th
e lead
er
an
d fo
llower
tractors
.
T
h
e l
o
cation
of the
lead
er, th
e v
i
rtu
a
l fo
llower, and th
e
follo
we
r a
r
e
defin
e
d
as l
o
cation
s
0
P
,
1
P
,and
2
P
. T
h
ese
three
points
are
locate
d
on t
h
e m
i
ddle of t
h
eir rea
r
a
x
les
(Fig.3).
T
h
e state
of t
h
e l
eader, t
h
e
virtual
fo
llower, an
d th
e
fo
llower can
b
e
ex
pressed
as:
[]
Ll
l
l
l
Xx
y
(
1
)
[]
V
F
vf
vf
vf
vf
Xx
y
(
2
)
[]
Ff
f
f
f
Xx
y
(
3
)
whe
r
e
,
ll
x
y
,
,
vf
v
f
x
y
,and
,
f
f
x
y
ar
e gl
o
b
al
co
o
r
di
nat
e
s
on
0
P
,
1
P
,and
2
P
;
,,
lv
f
f
are
headi
n
g angles
of the
leade
r
,
the vi
rtual
foll
owe
r
,
an
d t
h
e
follo
wer; a
n
d
,,
lv
f
f
are their steeri
n
g a
ngles
of
front wheels
.
Fi
g.
3. Leade
r
-
f
o
l
l
o
we
r fo
rm
ati
on
t
r
acki
n
g
m
odel
.
C
onsi
d
eri
ng t
h
e o
p
e
r
at
i
onal
m
ode an
d
w
o
r
k
i
n
g st
y
l
e o
f
ag
ri
cul
t
u
ral
ope
rat
i
o
n, t
h
e
fo
rm
ati
on-
k
eep
i
n
g
prob
lem in
th
is research
can
be stated as follows:
the leader is
dr
i
v
i
ng
o
n
a gi
ve
n pat
h
an
d as
k
s
t
h
e
fo
llower to
k
e
ep
a
relativ
e d
i
stan
ce
01
d
and a rel
a
tive angle
01
; th
is requ
ired
stat
e is th
e v
i
rtu
a
l
fo
llower. If
the state de
viation
,,
ee
e
xy
b
e
tween
t
h
e fo
llo
wer an
d t
h
e
v
i
rtu
a
l
fo
llower can alway
s
conv
erg
e
to zero, th
en
t
h
e re
qui
red
fo
rm
ati
on bet
w
e
e
n t
h
e l
e
a
d
er
and t
h
e f
o
l
l
o
w
e
r co
ul
d
be m
a
i
n
t
a
i
n
ed
. B
y
chan
gi
n
g
t
h
e r
e
l
a
t
i
v
e
distance of
01
d
and a relative a
n
gle
01
, fo
rm
ati
on
c
a
n be vari
e
d
.
3.
1. Ki
nem
a
ti
c
M
o
del
Acco
r
d
i
n
g t
o
t
h
e ca
r-l
i
k
e
ki
n
e
m
a
t
i
c
m
odel
,
t
h
e
rear
-w
hee
l
dri
v
e
ki
nem
a
t
i
c
equat
i
o
n
f
o
r
bot
h t
h
e
lead
er and
th
e
fo
llower is
g
i
ven
b
y
th
e
fo
llowing
ex
pressi
on
:
co
s
sin
ta
n
v
x
v
y
v
L
w
(4
)
Evaluation Warning : The document was created with Spire.PDF for Python.
I
J
ECE
I
S
SN
:
208
8-8
7
0
8
Title o
f
ma
nu
scrip
t
is sh
o
r
t
an
d clea
r, imp
lies resea
r
ch resu
lts (First Au
tho
r
)
5
Wh
ere v is t
h
e v
e
l
o
city and
w is th
e steeri
n
g an
gu
la
r velo
city of t
h
e
v
e
h
i
cle and L is
th
e leng
th
o
f
th
e
wh
eelb
ase. Th
e
state of
th
e
v
i
rtu
a
l
fo
llo
wer d
e
p
e
n
d
s
on th
e
g
l
ob
al coord
i
n
a
tes an
d
relativ
e
position
(Fig.3).
Th
e state o
f
th
e
v
i
rt
ual fo
llower can b
e
exp
r
essed as:
01
01
01
01
co
s(
)
sin(
)
vf
l
l
vf
l
l
vf
l
vf
l
vf
l
vf
l
xx
d
yy
d
vv
ww
(
5
)
As m
e
ntioned
abo
v
e, t
h
e f
o
r
m
ation-trac
kin
g
p
r
o
b
le
m
can be th
ou
g
h
t of
as
the tracki
n
g problem
of
the follower
and
the virtual
follo
wer, a
n
d i
f
t
h
e state
error
,,
ee
e
xy
asym
ptotically approaches
zero, t
h
e
desire
d f
o
rm
ation
can
be
ke
pt.
The
fo
rm
ation-tracki
n
g
error
between the follower an
d
the virtual follower
can
be e
x
p
r
esse
d a
s
:
co
s
s
i
n
0
sin
c
os
0
00
1
ev
f
v
f
f
v
f
ev
f
v
f
f
v
f
ef
v
f
x
xx
yy
y
(
6
)
A fee
d
back
co
ntr
o
l law ca
n b
e
o
b
tained ac
c
o
r
d
in
g
to
the
f
orm
ation-trac
k
ing e
r
r
o
r
an
d c
ont
rol in
p
u
t
of
the
virtual
f
o
llowe
r.
The
e
x
p
r
essi
on
o
f
fe
edbac
k
c
o
ntr
o
l can
be
refe
rr
e
d
to
as
[2
5]
:
,
(
,,
,
,
)
(
,,
,
,
)
f
f
e
e
e
vf
vf
e
e
e
l
l
vw
f
x
y
v
w
f
x
y
v
w
(
7
)
Whe
r
e
,
f
f
vw
is the cont
rol in
put
of
the follo
we
r.
The vi
rtual f
o
llowe
r co
ntr
o
l inp
u
t
,
vf
vf
vw
wa
s
equal t
o
the leader control input
,
ll
vw
and t
r
ansm
it
ted from
the leader to t
h
e foll
ower.
3.2. L
R
F
-
base
d F
o
llower Formati
on Tr
ac
king Err
o
r
Observati
o
n
The f
orm
ation trackin
g
e
r
r
o
r
,,
ee
e
xy
was calc
u
lated
unde
r
global st
ates. If
both t
h
e leader a
n
d the
follo
wer
we
re
equi
ppe
d
with
a GP
S, t
h
e
glo
b
al state o
f
bot
h
was a
v
ailabl
e; ho
we
ver
,
th
e fo
rm
ation-tra
ckin
g
pr
o
b
lem
s
discussed i
n
this
res
earch
are
base
d
on
som
e
assu
m
p
tions:
1)
In the case
of an a
u
to-dri
ve
n leade
r
,
only the leade
r
is equipped with
GPS and
t
h
e follower is
GPS-free but
equi
pped wit
h
the LRF for
obtaini
ng
relative
position in
form
at
ion of t
h
e leader.
Th
i
s
m
eans that
only the
global state of
leader is a
v
aila
bl
e, a
n
d
the
f
o
llowe
r ca
n
onl
y
obtain its
posit
ion
relative to the leader.
2) F
u
rthe
rm
ore, in the case
of t
h
e h
u
m
a
n-dri
ven lea
d
er,
bot
h the lead
e
r
an
d the
follo
wer a
r
e n
o
t eq
uip
p
e
d
with
GPS, and
in this cond
ition, it is im
possi
ble for th
e leader and the
foll
o
wer to
get thei
r global states.
Th
us in
b
o
th
the G
PS e
q
uip
p
ed
aut
o
-
d
rive
n l
eade
r
a
n
d the
hum
an-drive
n leade
r
cases, it is
i
m
possible for
the GPS-free
fo
llower to
obtain its global
position. To
sol
v
e this
problem
,
the
LRF-landm
ark
obs
er
vation
sy
stem
could
de
tect the relativ
e distance
and the
relative a
ngle
betwee
n
the leade
r
a
n
d the
follower.
Furtherm
ore, the l
a
ser-detected relative pos
ition can
be used to esti
m
a
te the form
ation tracking
er
ro
r.
Three la
n
d
m
a
r
k
s
were c
o
nsid
ered
o
n
the lea
d
er
(s
ho
w
n
in t
h
e re
d
dotted
c
i
rcle) (Fi
g
.
4
)
,
a
nd a
n
LR
F
was
on t
h
e foll
owe
r
.
To
facilitate the calculation,
we m
ount
ed the
first landm
a
rk on
the
m
i
ddle point of front
axles a
n
d t
h
e t
h
ir
d la
ndm
ark
o
n
the
m
i
ddl
e point
of the
leader rea
r
axl
e
s
0
P
. T
h
e
LRF
was
place
d at
the
m
i
ddle p
o
int
o
f
t
h
e
follo
we
r
r
ear a
x
les
1
P
. T
h
e
distance
f
rom
the
first la
n
d
m
a
rk to the third landm
ark is equal
to the lengt
h
of the leader (
3
lL
).
The l
o
cation of the t
h
ird landm
ark ca
n
be u
s
ed to
re
pre
sen
t
the locati
o
n
of the lea
d
er
, a
nd t
h
e locatio
n
of the LR
F ca
n be
use
d
to re
prese
nt the loc
a
tion o
f
the f
o
l
l
owe
r
. It is clear that
the laser detect
ion
of
3
d
and
3
repre
sents the relative dista
n
ce and the relative angle
betwee
n the leade
r
a
nd
the follower. Thus, t
h
e l
o
cal coordinate system
was es
tablished based on
t
h
e follo
wer and t
h
e
position
of the
leader a
n
d
was obtained using
Evaluation Warning : The document was created with Spire.PDF for Python.
I
S
SN
:
2
089
-48
56
IJR
A
V
o
l
.
4,
No
. 1,
M
a
rc
h 20
1
5
:
1 – 1
8
6
_3
3
_3
3
co
s
sin
lF
lF
xd
yd
(8
)
Here
, the
__
(,
)
lF
lF
xy
m
ean
s th
e lead
er l
o
catio
n
u
n
d
e
r th
e fo
llo
wer-b
a
sed
lo
cal coo
r
d
i
nate (Fig.5).
Usi
n
g t
h
e
ge
o
m
et
ri
cal
rel
a
t
i
ons
hi
p bet
w
ee
n t
h
e
LR
F and th
e land
m
a
rk
s, th
e
relativ
e
head
ing
an
g
l
e
betwee
n the
le
ader and t
h
e follower could also
be calc
u
lated as:
22
2
33
1
3
33
co
s
(
)
2
ld
d
arc
ld
(9)
Ob
vi
o
u
sl
y
,
t
h
e
fol
l
o
we
r-
base
d l
o
cal
headi
n
g
angl
e
of
th
e lead
er is equ
a
l to th
e relativ
e
h
e
ad
ing
an
g
l
e
and
i
s
gi
ve
n by
_
lF
e
(10)
The form
ation-tracki
n
g
error
(,
,
)
ee
e
xy
could
be easi
l
y calculated
unde
r the
lea
d
er-ba
s
ed loca
l
coo
r
di
nat
e
sy
st
em
, fol
l
o
wi
n
g
t
h
e rel
a
t
i
ons
hi
p o
f
co
o
r
di
nat
e
s bet
w
ee
n t
h
e l
eader
, t
h
e f
o
l
l
o
wer
,
an
d t
h
e
vi
rt
ual
fo
llow
e
r (Fig
.5).
__
__
__
ef
L
v
f
L
ef
L
v
f
L
ef
L
v
f
L
xx
x
yy
y
(11)
whe
r
e
__
_
(,
,
)
fL
fL
fL
xy
re
prese
n
ts the l
o
cal
state of t
h
e
followe
r
unde
r the leade
r
-ba
s
ed l
o
cal
coo
r
di
nat
e
sy
st
em
and
__
_
(,
,
)
vf
L
v
f
L
vf
L
xy
rep
r
esen
ts th
e lo
cal
state o
f
th
e v
i
rtual fo
llow
e
r
u
n
d
e
r th
e lead
er-
base
d l
o
cal
c
o
or
di
nat
e
.
The a
b
o
v
e e
q
u
a
t
i
on
(1
1
)
ca
n
fu
rt
he
r t
r
a
n
s
f
o
r
m
t
o
t
h
e f
o
l
l
o
wer
-
ba
sed
l
o
ca
l
co
or
di
nat
e
an
d
desc
ri
be
d
as:
_0
1
0
1
_0
1
0
1
_
co
s
s
in
0
c
o
s
si
n
c
o
s
0
s
i
n
00
1
0
el
F
el
F
el
F
xx
d
yy
d
(12)
Fi
g.
4. Laser
-
l
a
ndm
ark det
ect
i
o
n
m
odel
.
Fi
g.
5. C
o
or
di
n
a
t
e
s of
t
h
e l
e
a
d
er a
n
d
t
h
e
fo
llow
e
r track
i
n
g
system
.
Evaluation Warning : The document was created with Spire.PDF for Python.
IJEC
E
ISS
N
:
2
0
8
8
-
87
08
Title of manuscript is short
and clear,
implies research results (First Author)
7
3.
3. E
x
te
nded
Kal
m
an Fi
l
t
er
The fee
d
back
cont
rol law
wa
s established
u
nde
r th
e ideal
kinem
a
tic assum
p
tion o
f
n
o
s
e
ns
or
noise
and
distur
ba
nc
e. I
n
real
co
n
d
itions, t
h
e
f
o
r
m
ation-trac
k
in
g e
r
r
o
r
detected
by
th
e LR
F
in E
q
.
1
2
,
od
o
m
eter
data
of t
h
e le
ader
(,
)
ll
vw
,
an
d
od
ometer
d
a
ta of
th
e
f
o
llower
(,
)
f
f
vw
are co
rr
up
ted
with
er
ro
rs and
n
o
i
se.
Additionally, the
LRF
data are low
upda
te f
r
eq
ue
ncy
,
a
n
d
they
are
als
o
n
o
isy
in
a
n
a
d
v
e
rse e
n
vir
onm
ent.
T
o
i
m
prove t
h
e st
ability of t
h
e
form
ation
controller, an EKF
was i
n
troduced
to
reduce the
m
odel error and
fuse
the LRF
observation
and odom
eter
data.
The
nonlinear leader-follower m
odel described the stat
e transition
under a
control
input, and
obs
er
vation
m
odel
desc
ribe
d
the o
b
se
rvati
on
u
nde
r c
u
r
r
e
n
t
state, can
be e
x
press
ed
res
p
ect
ively
as f
o
llow
s
:
1
(,
,
)
kk
k
k
X
fX
U
V
(
1
3
)
(,
)
kk
k
Z
hX
W
(
1
4
)
Whe
r
e
k
X
is the cu
rre
nt state vect
or
re
prese
ntin
g
the l
eade
r
-foll
o
we
r
relative state at tim
e
instant
k
;
1
k
X
is the previ
ous state
vector at ti
m
e
instant
1
k
;
k
U
is the input
vector incl
udi
ng the i
n
put of
leader
(,
)
ll
vw
and follower
(,
)
f
f
vw
;
k
Z
is estim
a
te obs
ervatio
n
vecto
r
f
rom
LR
F at tim
e
instant k;
k
V
and
k
W
are
noises
f
rom
od
om
eter data an
d LR
F
o
b
se
rva
tion, a
n
d th
eir
cova
riance
m
a
trices we
re
defi
ned
as
k
Q
and
k
R
.
The
EKF i
n
clude
two steps which a
r
e
prediction
step
and c
o
rrection step, a
n
d
dat
a
fusion wa
s
practiced
th
r
o
u
g
h
rec
u
rsiv
e
the two ste
p
s. The
prediction step pre
d
icts the current lead
er-follower relative
state
k
X
base
d
o
n
the n
onlinea
r
sy
stem
m
odel
()
f
;estim
ated the
observation
k
Z
fr
o
m
the cu
rre
nt
estim
a
te
state
k
X
based
o
n
t
h
e
ob
ser
v
ation
m
odel
()
h
; an
d
pre
diction t
h
e stat
e err
o
r c
ova
ria
n
ce m
a
trix
k
P
:
1
ˆ
(,
,
0
)
kk
k
Xf
X
U
(1
5)
(,
0
)
kk
Zh
X
(
1
6
)
,1
,
,
,
ˆ
T
T
kx
k
k
x
k
v
k
k
v
k
PJ
P
J
J
Q
J
(1
7)
The c
o
rrection step upd
ates the Kalm
an gai
n
k
K
. The estim
ate
ˆ
k
X
and
state error covaria
n
ce m
a
trix
ˆ
k
P
were
co
rrecte
d
by
inte
gratin
g
the o
b
se
rvati
on
f
unctio
n
()
h
whe
n
the
LRF observatio
n is available:
,,
()
TT
T
kk
k
k
k
k
w
k
k
w
k
KP
H
H
P
H
J
R
J
(1
8)
ˆ
[(
,
0
)
]
kk
k
k
k
XX
K
Z
h
X
(1
9)
ˆ
()
kk
k
k
P
IK
H
P
(2
0)
whe
r
e
1
ˆ
k
X
re
prese
nt the
correcte
d
state and
1
ˆ
k
P
represent
the
c
o
rrected state e
r
ror c
o
varia
n
ce
matrix at previous tim
e.
,
x
k
J
and
k
H
represe
nt t
h
e Jac
obea
n
s
o
f
sy
stem
fu
nctio
n
()
f
and o
b
ser
v
atio
n f
uncti
o
n
()
h
with respect t
o
state
k
X
and
ob
servatio
n
k
Z
,
,
vk
J
and
,
wk
J
are
Jacobea
n
s of system
function
()
f
and
obs
er
vation
f
u
nction
()
h
with
respect to i
n
put
k
U
an
d ob
ser
v
a
tion
k
Z
, an
d
I
is defi
ned
as an identity matrix.
3.4 Accomplishment
of the EKF
B
a
sed
o
n
t
h
is
resea
r
c
h
,
the
leade
r
-
follo
w
e
r
relative sta
t
e vect
or
an
d
state
ob
ser
v
a
tion
vect
or
assum
i
ng
n
o
n
o
ise
is defi
ned
as:
1
,,
1
2
,
1
2
,
ˆ
(,
,
0
)
[,
,
,
]
T
kk
k
lk
f
k
k
k
Xf
X
U
d
(
2
1
)
,,
,
(,
0
)
[
,
,
]
T
k
k
f
l
a
se
r
k
lase
r
k
l
a
se
r
k
Z
hX
d
(2
2
)
Evaluation Warning : The document was created with Spire.PDF for Python.
ISSN
:
2
089
-48
56
IJRA
V
o
l
.
4,
No
. 1,
Marc
h 20
1
5
:
1 – 1
8
8
whe
r
e
,
lk
is heading a
n
gle of t
h
e leader,
,,
,
f
k
flase
r
k
is th
e h
e
ad
ing
an
g
l
e
o
f
th
e
fo
llo
w
e
r,
12
,
,
,
kl
a
s
e
r
k
dd
is th
e relativ
e
d
i
stan
ce
fro
m
0
P
to
2
P
, and
12
,
,
,
kl
a
s
e
r
k
is th
e relativ
e an
g
l
e
betw
een
line
02
PP
and
hea
d
i
n
g p
o
s
i
t
i
on of
t
h
e fol
l
owe
r
(
F
i
g
.
6
).
(a)
(
b
)
Fi
g.
6.
Leade
r
-
f
o
l
l
o
we
r t
r
ac
ki
n
g
m
odel
:
(a) Relativ
e
positio
n
u
n
d
e
r odo
m
e
try; (b
) Relativ
e p
o
sitio
n
u
n
d
e
r LRF.
By an
alyzin
g
t
h
e
relatio
n
b
e
t
w
een th
e lead
er an
d th
e
fo
ll
ow
er, we
g
e
t th
e fo
llo
w
i
n
g
equatio
n
s
:
12
12
12
1
2
co
s(
)
si
n(
)
lf
f
lf
f
xx
d
yy
d
(23)
D
i
fferen
tiatio
n of th
e abov
e eq
u
a
tion
s
w
ith re
spect to tim
e
and com
b
ining Eq.4 yields
12
12
12
12
12
12
12
ta
n
ta
n
sin(
)(
sin
s
in
)
c
o
s
(
)
(
c
o
s
c
o
s
)
1
[co
s
(
)
(
s
i
n
s
i
n
)
s
i
n(
)
(
co
s
c
o
s
)]
t
a
n
l
l
l
f
f
f
fl
l
f
f
f
l
l
f
f
f
fl
l
f
f
f
l
l
f
f
f
v
L
v
L
d
vv
v
v
v
vv
v
v
dL
(2
4)
The a
b
ove
equations s
h
ow the leader-follo
wer relative
state base
d
on
odometer.
N
o
te th
a
t
th
e h
e
ad
ing a
n
g
l
e
s
of
th
e
le
ade
r
an
d th
e
f
o
llo
w
er
l
and
f
i
n
E
q
.
2
4
are
i
n
gl
obal
coo
r
di
nat
e
s.
A
s
di
sc
usse
d
ab
ove
, i
t
i
s
i
m
possi
bl
e t
o
obt
ai
n t
h
e
gl
o
b
al
co
or
di
nat
e
s
i
n
t
h
i
s
res
earc
h
d
u
e
t
o
t
h
e
abse
nce of GPS and Gyrosc
ope. Tran
sfo
r
m
i
n
g
t
h
e lead
er-fo
llo
w
e
r relative st
ate function of Eq.24 t
o
leader-
b
a
sed
lo
cal coo
r
d
i
n
a
tes, th
e
esti
m
a
te o
f
lead
er-fo
llow
e
r
relativ
e state at
ti
m
e
in
st
ant k can
be m
odified as
fo
llow
s
:
12
,,
_,
1
,
,
_,
1
_
,
_
,
_,
12
,
1
2
,
1
1
2
,
,
1
12
,
,
12
,
1
12
,
12
,
1
,
12
,
1
ta
n
t
a
n
ˆ
()
ˆ
ˆˆ
()
ˆ
ˆ1
(t
a
n
)
ˆ
fk
l
k
fL
k
f
k
l
k
s
fL
k
f
L
k
l
L
k
fL
k
kk
k
k
k
s
k
fk
kk
kf
k
s
k
vv
T
LL
Xd
d
d
d
a
c
b
d
T
v
ac
bd
T
d
L
(25)
w
h
er
e
Evaluation Warning : The document was created with Spire.PDF for Python.
IJECE
ISSN
:
208
8-8
7
0
8
Title o
f
ma
nu
scrip
t
is sh
o
r
t
an
d clea
r, imp
lies resea
r
ch resu
lts (First Au
tho
r
)
9
_,
1
2
,
1
_,
1
2
,
1
,_
,
,
,
_
,
ˆˆ
si
n(
)
,
co
s(
)
sin
,
co
s
fL
k
k
fL
k
k
fk
f
L
k
l
k
f
k
f
L
k
ab
cv
d
v
v
,,
,
lk
f
k
vv
is th
e v
e
l
o
city o
f
th
e lead
er an
d th
e
fo
llow
e
r,
and
,,
,
lk
f
k
is th
e
steering
ang
l
e
o
f
th
e leader
and t
h
e f
o
l
l
o
w
e
r. Bot
h
vel
o
c
i
t
y
and st
eeri
n
g an
gl
es
a
r
e obtained from
enco
d
e
rs at ti
me in
stan
t
k
.
S
T
is th
e
ti
m
e
in
terv
al.
Th
e state ev
o
l
u
tio
n fro
m
ti
me in
stan
t
k-
1
to
k+1
, in
wh
ich left-sid
e
v
e
h
i
cl
es represe
n
t the c
o
rrected
state
1
ˆ
k
X
at th
e p
r
evio
u
s
tim
e in
stan
t and
righ
t-si
de v
e
h
i
cles rep
r
esen
t th
e prettiest state
k
X
at current (Fig.7).
As a
n
im
portant step, the lea
d
er-ba
s
ed l
o
ca
l coordi
nates
were
update
d
at each pre
d
iction ste
p
,
which m
eant
that the
hea
d
ing a
ngle
of the
leader
under t
h
e leader-based
local coordi
nat
e
s was
a c
o
n
s
t
a
nt
eq
ual
t
o
zer
o:
_,
_,
1
ˆ
0
lL
k
l
L
k
(26)
D
u
ring
th
e time in
terv
al
s
T
, bo
th
th
e lead
er an
d
t
h
e fo
llow
e
r
c
h
ange
d their state. Equation (25)
ex
pressed
u
pdated
inform
at
i
o
n of l
eade
r
-based loc
a
l coordinates, and
esti
m
a
tio
n
o
f
th
e lead
er-fo
l
lo
wer
relativ
e state in
th
e lead
e
r
-ba
s
ed l
o
cal coordinate system
from
time
k-1
to
k
. Est
a
bl
i
s
hi
ng
an
d u
pdat
i
n
g
t
h
e
leader-base
d
c
o
ordinates i
n
a tim
e
ly
m
a
nner m
a
de
track
i
n
g po
ssi
b
l
e even
t
h
oug
h th
ere w
e
re
no
G
P
S and
G
y
roscop
e i
n
t
h
e
fo
llow
e
r and
h
e
lp
ed
to elimin
at
e the effe
ct of increm
en
tal erro
r of en
cod
e
rs.
Following
the
odom
etry-base
d
state
estim
a
t
e
Eq
.2
5, t
h
e syste
m
an
d i
n
pu
t
Jaco
b
e
an
s
,
x
k
J
and
,
vk
J
can
be gi
ve
n (see Ap
pe
ndi
x):
For t
h
e state observation vect
or
k
Z
, an
d f
o
l
l
o
wi
ng t
h
e LRF-l
a
n
d
m
a
rk m
e
t
hod
as descri
be
d i
n
sect
i
o
n
2.2, t
h
e leader-follower relative state
obse
r
v
a
t
i
on pr
o
b
l
e
m
can be fo
u
n
d
a
s
:
3,
3,
[,
,
]
T
k
kk
k
Z
d
(27)
Based
on
t
h
e ab
ov
e
ob
serv
atio
n
fu
n
c
tion Eq
.27
and
t
h
e
LRF-land
m
a
r
k
ob
serv
ation
calcu
latio
n
i
n
Eqs.
9
a
n
d 10
,
t
h
e ob
ser
v
at
i
o
n
Jaco
bean
k
H
and
,
wk
J
can be gi
ve
n (see
A
ppe
n
d
i
x
):
The
sy
st
em
and
o
b
ser
v
at
i
o
n
fu
nct
i
o
ns
was
defi
ned
a
n
d
Ja
cobea
n
s
fu
nct
i
ons
were
cal
c
u
l
a
t
e
d. O
n
ce
th
e Jaco
b
e
an
s
fun
c
tion
s
are
k
now
n,
K
a
lm
a
n
g
a
in
, th
e lead
er-fo
llow
e
r
relativ
e state ob
serv
ation
,
and
state
err
o
r
co
vari
a
n
c
e
m
a
t
i
r
x can
be
f
o
u
n
d
usi
n
g E
q
s.
15
t
o
2
0
.
(a)
(b)
Fi
g.
7.
Leade
r
-
F
ol
l
o
wer
rel
a
t
i
v
e st
at
e e
vol
ut
i
on:
(a)
F
r
om
t
i
m
e
k-1
to
k
;
(b) F
r
om
tim
e
k
to
k+
1
.
In
t
h
is research
, t
h
e inpu
t of syste
m
fu
n
c
ti
o
n
()
f
is th
e en
coder inform
atio
n
o
f
steering
ang
l
e and
v
e
lo
city. Und
e
r farm
lan
d
cond
itio
n
s
, th
e no
i
s
e cov
a
rian
ce
matrix
Q
fo
r t
h
e
enco
de
rs ca
n
b
e
de
fi
ne
d as:
Evaluation Warning : The document was created with Spire.PDF for Python.
I
S
SN:
2
089
-48
56
IJR
A
V
o
l. 4,
No
. 1,
M
a
rc
h 20
1
5
:
1 – 1
8
10
2
2
2
2
00
0
00
0
00
0
00
0
vl
vf
l
f
Q
(
2
8
)
B
e
sides, t
h
e
o
b
ser
v
atio
n
was
assum
e
d to
be
p
r
o
v
ide
d
by
t
h
e LR
F
(
S
IC
K
LM
S
21
1
)
, a
n
d
data we
re
relative distance and a
ngle
of each landm
a
rk from
the
LRF. T
h
e
obse
r
va
tion
noise c
ovariance m
a
trix R for
the LRF ca
n
be
expressed as:
2
2
2
00
00
00
d
d
ang
R
(
2
9
)
whe
r
e
0
.
19
8
,
0.10
5
0.00
13
,
0
.00
3
8
vl
v
f
l
f
d
ang
mr
a
d
s
s
mr
a
d
As
n
o
t
ed
above,
th
e LRF ob
ser
v
a
tion
info
rmatio
n
is
no
t always av
ailab
l
e b
ecau
s
e
o
f
its lo
w
up
d
a
te
fre
que
ncy
(c
o
m
pared with o
dom
etry
sy
stem
)
and p
r
o
p
a
g
a
tion
delay
of
LR
F sig
n
als.
T
hus
, t
h
e c
o
rrec
tion ste
p
is o
n
ly
pr
ocess
e
d
whe
n
t
h
e L
R
F o
b
ser
v
ation is available. This m
eans that
If LR
F ob
ser
v
a
tion
is
a
v
ailabl
e:
The posteriori
estim
a
te
ˆ
k
X
and state error c
o
va
riance m
a
trix
ˆ
k
P
coul
d
be calcul
a
ted by
f
u
sin
g
th
e
od
om
etry
based
pri
ori estim
ate state
k
X
and
LR
F
-base
d
o
b
ser
v
a
tion
results
k
Z
u
s
ing
Eq
s.19
an
d 20
.
If LR
F ob
ser
v
a
tion gets delay
e
d:
The posteriori
estim
a
te
ˆ
k
X
and
state erro
r c
o
varia
n
ce m
a
trix
ˆ
k
P
ap
pr
ox
im
ate
l
y ad
op
t th
e
pr
iori
esti
m
a
te state
k
X
and state error c
ova
riance
m
a
tr
ix
k
P
for calculating
next tim
e instant
k+1
:
ˆ
kk
XX
(
3
0)
ˆ
kk
PP
(
3
1)
4.
RES
U
LTS
A
N
D
DI
SC
US
S
I
ON
Sim
u
lation we
re exec
uted to evaluate the
EK
F
relative
position estim
ate algorithm
base
d on the
leader-followe
r
feedbac
k
control system
.
T
h
e
sim
u
lat
o
r was d
e
sign
ed u
s
ing
C++
bu
ild
er
XE3
.
Th
e
sim
u
lation wa
s con
d
u
cted
fo
r a h
u
m
a
n-dri
ven tr
actor e
q
uip
p
ed
with r
o
tary
enco
der
s
on t
h
e rea
r
an
d f
r
o
n
t
wheels
to
rec
o
r
d
vel
o
city
a
n
d
steeri
n
g a
n
gle
of
the
tra
c
tor.
A
wirele
ss L
A
N m
o
d
u
l
e was
incl
ud
ed
f
o
r
transm
itting da
ta to the f
o
llo
wer
.
A
follo
we
r m
i
ght also be
equi
ppe
d
with
the enc
ode
rs a
nd t
h
e wi
reless
LAN
and can be im
ple
m
ented sam
e
as lead
er. To
measure t
h
e rel
a
tive position
of
the leader, a LR
F was considered
on the
follower to m
easure t
h
e leader’s
position usi
n
g artificial landm
arks.
As
disc
usse
d
abo
v
e,
the
hu
m
a
n-d
rive
n le
ader
an
d
the
auto
nom
ous
f
o
llowe
r c
a
n
b
e
use
d
f
o
r
har
v
estin
g
o
p
e
r
ations
.
I
n
o
u
r
sim
u
lation, t
r
ajectory
o
f
t
h
e
leader
was
gi
ve
n a
s
a
sm
all an
d a
la
rge
c
u
r
v
a
t
ures
sinus
oidal
path
. The s
p
ee
d
of
the leader
was
set at 1.2 m
/
s whe
n
on t
h
e s
m
all curvatu
re
s an
d 0
.
8 m
/
s on t
h
e
large c
u
rvature
s
. The
wheelbase lengt
h bot
h
of the lea
d
er
and the follower
were set
at 1.53 m
,
equal to the
refe
rence
pa
ra
m
e
ter of the
Ku
b
o
ta KL
2
1
m
odel tractor.
Ad
ditionally
, t
h
e lim
its of sp
eed, steeri
n
g a
n
g
u
lar
velocity
and
steerin
g an
gle w
e
re de
fine
d as
ma
x
1.6
v
m/s
,
ma
x
0.
3
8
w
rad/s
,
an
d
45.0
, respectively.
The tim
e for data transm
issio
n
usi
n
g wirele
ss LAN and
L
R
F scan inte
rval was sim
u
lated as
20 m
s
and
200
ms
,
res
p
ectivel
y
.
N
o
te that t
h
e laser
detectio
n
fre
que
ncy
w
a
s set lo
wer
th
an th
e
usual
f
r
e
que
ncy
of
L
M
S 2
1
1
.
And t
h
e tim
e interval Ts
was
100
ms
.
In the sim
u
lati
on, sensor
noise
was selected based on the farm
land
condition and prev
i
o
us studies.
The
n
o
ise
o
f
o
dom
etry
(enc
o
d
ers
)
was
gene
rated
by
ra
nd
o
m
functio
ns,
a
n
d
a
dde
d
to
th
e vel
o
city
an
d
steerin
g
angle
of
the le
ader a
n
d the
f
o
llower
we
re ar
ou
n
d
0.
03
2 m
/
s an
d
0.
05
2
4
r
a
d/s [
2
6,
27]
.
The
noise
o
f
L
R
F wa
s
Evaluation Warning : The document was created with Spire.PDF for Python.