I
nte
rna
t
io
na
l J
o
urna
l o
f
Ro
bo
t
ics a
nd
Aut
o
m
a
t
io
n
(
I
J
R
A)
Vo
l.
9
,
No
.
4
,
Dec
em
b
er
2
0
2
0
,
p
p
.
2
5
6
~
2
7
0
I
SS
N:
2089
-
4
8
5
6
,
DOI
: 1
0
.
1
1
5
9
1
/
i
jr
a
.
v9
i
4
.
pp
2
5
6
-
2
7
0
256
J
o
ur
na
l ho
m
ep
a
g
e
:
h
ttp
:
//ij
r
a
.
ia
esco
r
e.
co
m
The qua
dro
tor dy
na
mic mo
deling
a
nd study
of
meta
-
heuristic
a
lg
o
rithms perfor
ma
nce on o
ptimiz
a
tion o
f
PID
cont
ro
ller inde
x
to co
ntrol a
ng
les
a
nd t
ra
cking
the
ro
ute
At
a
J
a
ha
ng
ir
M
o
s
ha
y
ed
i
1
,
M
a
hy
a
r
G
heibo
lla
hi
2
,
L
ief
a
L
ia
o
3
1,
3
S
c
h
o
o
l
o
f
I
n
fo
rm
a
ti
o
n
E
n
g
i
n
e
e
rin
g
,
Un
i
v
e
rsity
o
f
S
c
ien
c
e
a
n
d
Te
c
h
n
o
l
o
g
y
,
C
h
in
a
2
Kh
o
m
e
in
ish
a
h
r
Bra
n
c
h
,
Az
a
d
U
n
iv
e
rsit
y
,
Ira
n
Art
icle
I
nfo
AB
S
T
RAC
T
A
r
ticle
his
to
r
y:
R
ec
eiv
ed
Ma
y
1
2
,
2
0
20
R
ev
is
ed
J
u
n
7
,
2
0
20
Acc
ep
ted
Au
g
4
,
2
0
20
In
t
h
e
las
t
d
e
c
a
d
e
,
b
e
c
a
u
se
o
f
th
e
u
n
iq
u
e
s
p
e
c
if
ica
ti
o
n
o
f
v
e
r
t
ica
l
f
l
iers
,
sc
ie
n
ti
s
ts
a
n
d
r
e
se
a
r
c
h
e
rs
h
a
d
a
s
p
e
c
ial
f
o
c
u
s
o
n
t
h
e
m
.
T
h
e
p
a
r
ti
c
u
lar
a
b
il
it
ies
o
f
t
h
e
se
fl
ier
s
c
a
n
b
e
m
e
n
ti
o
n
e
d
s
u
c
h
a
s:
h
i
g
h
m
a
n
e
u
v
e
r
a
b
i
li
t
y
,
l
o
w
e
x
p
e
n
se
s
,
d
e
c
re
a
se
i
n
r
a
d
a
r
i
d
e
n
ti
fi
e
r
a
n
d
l
o
w
t
h
r
e
a
t
f
o
r
t
h
e
h
u
m
a
n
l
ife
.
T
h
e
y
a
l
so
h
a
v
e
n
o
l
imi
ta
ti
o
n
i
n
d
im
e
n
si
o
n
.
M
o
re
o
v
e
r
,
b
e
c
a
u
se
o
f
s
o
m
e
a
p
p
l
ic
a
ti
o
n
s
li
k
e
p
h
o
t
o
g
ra
p
h
y
,
t
o
p
o
g
ra
p
h
y
,
n
e
ws
c
o
v
e
r
a
g
e
,
st
u
d
y
o
f
p
o
we
r
l
i
n
e
s
a
n
d
a
e
r
o
l
o
g
y
a
n
a
l
y
si
s,
t
h
e
y
c
a
n
b
e
n
o
ta
b
le
f
o
r
u
si
n
g
.
T
h
e
se
f
li
e
rs
a
ls
o
a
re
s
i
g
n
i
fi
c
a
n
t
l
y
im
p
o
rt
a
n
t
b
e
c
a
u
se
o
f
m
o
n
i
t
o
r
i
n
g
i
n
u
r
b
a
n
re
g
io
n
s,
a
g
r
ic
u
l
t
u
ra
l
h
a
rv
e
st
a
n
d
sp
ra
y
p
o
i
s
o
n
,
i
ll
e
g
a
l
im
p
o
r
ts
,
e
x
p
o
r
ts
a
d
m
i
n
is
tra
ti
o
n
a
n
d
f
ire
d
i
s
ti
n
c
ti
o
n
i
n
o
r
d
e
r
t
o
c
o
n
tr
o
l
t
h
e
fi
re
.
Be
s
i
d
e
s
,
se
e
k
a
n
d
re
sc
u
e
m
i
ss
i
n
g
p
e
o
p
l
e
a
n
d
a
ls
o
n
a
t
u
ra
l
d
i
sa
s
ters
c
a
n
b
e
p
re
-
d
e
te
r
m
i
n
e
d
w
h
ic
h
c
a
u
s
e
s
s
ti
m
u
l
u
s
i
n
v
e
sti
g
a
t
o
r
s
t
o
a
c
t
a
n
d
p
u
t
d
i
ffe
re
n
t
t
o
p
ics
i
n
f
ro
n
t
o
f
t
h
e
m
.
O
n
e
o
f
t
h
e
s
e
f
ie
ld
s
is
u
si
n
g
m
e
ta
-
h
e
u
ri
st
ic
a
l
g
o
r
i
t
h
m
s
wi
t
h
t
h
e
c
a
p
a
b
i
l
it
y
o
f
u
si
n
g
i
n
c
o
n
tr
o
l
s
y
ste
m
s.
T
h
e
P
ID
c
o
n
t
r
o
l
ler
a
s
a
c
las
s
ic
m
o
d
e
l
h
a
s
s
o
m
e
li
m
i
tat
i
o
n
s
,
b
u
t
b
y
o
p
t
im
iza
t
i
o
n
o
f
s
p
e
c
ia
l
i
n
d
e
x
t
h
r
o
u
g
h
m
e
ta
-
h
e
u
ri
st
ic
a
l
g
o
r
it
h
m
s,
i
t
h
a
s
s
h
o
w
n
a
c
c
e
p
ta
b
le
re
s
u
lt
s.
I
n
t
h
is
s
t
u
d
y
,
fi
rs
t,
t
h
e
h
is
t
o
r
y
o
f
v
e
r
t
ica
l
f
li
e
rs
a
n
d
q
u
a
d
r
o
t
o
r
a
re
i
n
v
e
s
ti
g
a
te
d
.
T
h
e
n
,
a
f
te
r
a
r
e
v
iew
o
f
o
v
e
r
u
se
d
m
e
th
o
d
s,
t
h
e
q
u
a
d
r
o
t
o
r
c
o
n
tr
o
l
h
a
s
b
e
e
n
d
o
n
e
.
A
ft
e
rwa
r
d
,
t
h
e
c
i
n
e
m
a
ti
c
a
n
d
d
y
n
a
m
ic
o
f
q
u
a
d
r
o
t
o
r
is
p
re
se
n
te
d
.
Ne
x
t
b
y
d
e
si
g
n
i
n
g
o
f
P
ID
c
o
n
t
ro
l
ler
,
P
ID
i
n
d
e
x
o
p
t
im
iza
t
i
o
n
b
y
n
a
t
u
r
e
in
s
p
i
re
d
a
l
g
o
r
it
h
m
,
p
a
r
ti
c
le sw
a
rm
o
p
t
im
iza
t
io
n
(
P
S
O)
,
g
e
n
e
ti
c
a
l
g
o
r
it
h
m
s
(G
A)
,
a
n
d
f
i
re
f
ly
a
l
g
o
r
it
h
m
s
(F
A)
h
a
v
e
b
e
e
n
st
u
d
ie
d
.
Dy
n
a
m
ic
s
y
s
tem
,
c
o
n
tr
o
ll
e
r
a
n
d
m
e
n
t
i
o
n
e
d
o
p
t
imiz
a
t
i
o
n
m
e
t
h
o
d
s
o
f
P
ID
c
o
n
tr
o
l
le
r
i
n
d
e
x
h
a
v
e
a
ls
o
b
e
e
n
im
p
lem
e
n
te
d
i
n
M
AT
LA
B
s
o
f
tw
a
re
.
A
ls
o
,
wit
h
d
u
e
a
t
te
n
t
i
o
n
t
o
t
h
e
c
o
m
p
a
ris
o
n
c
r
it
e
r
ia
t
h
e
P
ID
-
P
S
O
c
o
n
tr
o
ll
e
r
h
a
s
s
h
o
w
n
t
h
e
b
e
s
t
p
e
r
fo
rm
a
n
c
e
.
Ne
x
t
,
b
y
a
p
p
l
y
i
n
g
c
h
a
ll
e
n
g
i
n
g
ro
u
tes
,
t
h
e
sta
b
i
l
it
y
o
f
c
o
n
tr
o
l
ler
i
n
t
h
e
s
im
u
l
a
ti
o
n
is
e
v
a
l
u
a
te
d
.
T
h
e
n
,
m
a
k
i
n
g
q
u
a
d
r
o
t
o
r
i
s
d
o
n
e
i
n
p
ra
c
t
ice
a
l
o
n
g
wi
t
h
i
n
t
r
o
d
u
c
i
n
g
t
h
e
u
se
d
im
p
lem
e
n
tat
i
o
n
o
f
P
ID
-
P
S
O
c
o
n
tr
o
l
le
r
re
s
u
lt
s
o
n
t
h
e
re
a
l
r
o
b
o
t,
a
n
d
it
s
sta
b
il
it
y
is
e
v
a
l
u
a
te
d
p
ra
c
t
ica
l
l
y
.
K
ey
w
o
r
d
s
:
Fire
f
ly
alg
o
r
ith
m
Gen
etic
alg
o
r
ith
m
Op
tim
izatio
n
alg
o
r
ith
m
s
Par
ticle
s
war
m
o
p
tim
izatio
n
alg
o
r
ith
m
PID
co
n
tr
o
ller
Qu
ad
r
o
to
r
R
o
b
o
t
UAV
Un
m
an
n
ed
f
lier
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
:
Ata
J
ah
an
g
ir
Mo
s
h
ay
ed
i,
Sch
o
o
l
of
I
n
f
o
r
m
atio
n
E
n
g
in
e
er
in
g
,
Un
iv
er
s
ity
of
Scien
ce
an
d
T
ec
h
n
o
lo
g
y
,
J
ian
g
x
i,
C
h
in
a.
E
m
ail: a
jm
@
jx
u
s
t.e
d
u
.
cn
1.
I
NT
RO
D
UCT
I
O
N
I
n
r
ec
en
t y
ea
r
s
,
q
u
a
d
r
o
to
r
u
s
ag
e
to
d
o
d
if
f
er
e
n
t ta
s
k
s
is
r
aisi
n
g
.
Hig
h
m
an
eu
v
er
ab
ilit
y
,
lo
w
ex
p
en
s
es,
d
ec
r
ea
s
e
th
e
u
s
e
o
f
r
ad
ar
id
en
tifie
r
,
lo
w
th
r
ea
t
f
o
r
h
u
m
a
n
life
an
d
n
o
lim
itatio
n
o
n
d
im
en
s
io
n
,
ar
e
th
e
f
ac
to
r
s
o
f
lead
in
g
to
m
o
r
e
atten
tio
n
[
1
]
.
T
h
e
q
u
ad
r
o
to
r
is
an
u
n
m
a
n
n
ed
f
lier
with
th
e
f
r
am
e
s
h
a
p
e
o
f
X
a
n
d
H.
I
t
is
Evaluation Warning : The document was created with Spire.PDF for Python.
I
n
t
J
R
o
b
&
A
u
to
m
I
SS
N:
2089
-
4
8
5
6
Th
e
q
u
a
d
r
o
t
o
r
d
yn
a
mic
mo
d
elin
g
a
n
d
s
tu
d
y
o
f m
eta
-
h
eu
r
is
tic
a
lg
o
r
ith
ms
... (
A
ta
J
a
h
a
n
g
ir
Mo
s
h
a
ye
d
)
257
th
e
s
u
b
g
r
o
u
p
o
f
m
u
ltiro
to
r
with
th
e
h
elp
o
f
4
f
a
n
s
to
p
r
o
p
el
f
o
r
ce
,
as
a
q
u
a
d
(
f
o
u
r
)
ca
lled
q
u
ad
r
o
to
r
.
I
t
is
als
o
ch
an
g
e
-
a
b
le
ac
co
r
d
in
g
to
th
e
f
r
am
e
an
d
e
q
u
ip
m
e
n
t.
T
h
ese
s
u
p
p
lies
h
av
e
f
o
u
r
o
p
e
r
ato
r
s
o
r
r
o
to
r
at
th
e
e
n
d
o
f
ea
ch
ar
m
wh
ich
in
th
eir
s
tr
u
ctu
r
e,
th
e
ar
m
s
ar
e
s
y
m
m
etr
ical,
s
am
e
d
is
tan
ce
an
d
s
am
e
d
im
en
s
io
n
.
T
h
e
r
o
to
r
s
ar
e
T
u
r
n
in
g
in
clo
ck
wis
e
an
d
co
u
n
ter
clo
c
k
wis
e
m
an
n
er
to
m
o
v
e
th
e
b
o
d
y
[
2
]
.
I
n
g
en
er
al,
co
n
t
r
o
llin
g
q
u
ad
r
o
to
r
m
o
v
em
en
ts
is
d
o
n
e
b
y
s
ettin
g
en
g
in
e
v
elo
city
.
T
o
co
n
tr
o
l
r
o
b
o
t
m
o
v
em
en
t
s
3
an
g
les
o
f
‘
r
o
ll
'
,
‘
p
itch
'
an
d
‘
y
aw
'
s
h
o
u
ld
b
e
s
et.
Mo
v
in
g
u
p
is
d
o
n
e
b
y
u
s
in
g
s
p
ee
d
ad
ju
s
tm
en
ts
in
ev
er
y
r
o
to
r
th
r
o
u
g
h
p
u
ls
e
an
d
wid
th
m
ed
icatio
n
t
o
g
et
f
av
o
r
ab
le
o
u
tp
u
t
[
3
]
.
T
h
e
q
u
a
d
r
o
to
r
s
with
th
e
h
elp
o
f
f
o
u
r
r
o
to
r
s
an
d
s
ep
ar
ate
p
r
o
p
eller
an
d
p
ai
r
wis
e
tu
r
n
in
g
in
t
h
e
o
p
p
o
s
ite
d
ir
ec
tio
n
m
ak
e
n
eu
tr
al
to
r
q
u
e
an
d
it
lead
s
to
th
e
r
esu
ltin
g
f
o
r
ce
[
4
,
5]
.
Gen
e
r
ally
,
th
e
f
lier
is
im
ag
in
ed
in
th
e
s
p
ac
e
as
a
f
r
ee
o
b
ject
an
d
with
6
d
eg
r
ee
s
o
f
f
r
ee
d
o
m
.
So
,
th
e
f
lier
’
s
p
o
s
itio
n
an
d
its
s
itu
atio
n
ca
n
b
e
d
escr
ib
ed
co
m
p
letely
.
T
h
e
p
o
s
itio
n
is
co
n
tain
in
g
th
r
ee
lin
ea
r
an
d
th
r
ee
cir
cu
lar
m
o
v
em
en
ts
co
n
s
is
tin
g
o
f
t
u
r
n
in
g
ar
o
u
n
d
X
-
ax
is
(
r
o
ll
)
,
tu
r
n
in
g
a
r
o
u
n
d
Y
-
a
x
is
(
p
itch
)
an
d
tu
r
n
in
g
ar
o
u
n
d
Z
-
a
x
is
(
y
aw
)
[
4
]
to
co
n
tr
o
l
an
d
tu
n
e
th
e
Qu
ad
r
o
t
o
r
co
n
tr
o
ller
an
d
v
a
r
io
u
s
m
et
h
o
d
s
ar
e
in
v
esti
g
ated
wh
ich
m
en
tio
n
e
d
in
o
u
r
p
r
ev
i
o
u
s
r
esear
ch
p
ap
e
r
[
6
]
.
I
n
th
is
s
tu
d
y
af
ter
m
o
d
ellin
g
th
e
Qu
a
d
r
o
to
r
a
n
d
im
p
ly
i
n
g
th
e
r
ea
l
p
ar
am
eter
to
th
e
m
o
d
el,
th
e
PID
p
ar
a
m
eter
a
r
e
t
u
n
ed
with
t
h
e
h
elp
o
f
th
r
ee
f
am
o
u
s
m
eta
-
h
eu
r
is
tic
alg
o
r
ith
m
s
an
d
th
e
n
af
te
r
s
elec
tin
g
th
e
b
est
p
er
f
o
r
m
a
n
ce
th
e
ex
tr
ac
ted
v
alu
es
ar
e
lo
ad
e
d
o
n
r
ea
l
p
latf
o
r
m
an
d
th
e
r
ea
l
p
er
f
o
r
m
an
ce
a
n
d
s
im
u
latio
n
ar
e
co
m
p
ar
ed
.
I
n
th
is
r
esear
ch
p
ap
er
th
e
Qu
ad
r
o
to
r
m
o
d
elin
g
a
n
d
its
r
elativ
e
p
ar
am
eter
wh
ich
is
u
s
ed
to
s
im
u
late
th
e
Qu
ad
r
o
to
r
ar
e
d
escr
ib
ed
in
Sectio
n
1
.
T
h
en
,
in
Sectio
n
2
th
e
PID
co
n
tr
o
ller
an
d
t
h
r
ee
Me
t
a
h
eu
r
is
tic
o
p
tim
izatio
n
alg
o
r
ith
m
s
n
am
ed
p
ar
ticle
s
war
m
o
p
tim
izatio
n
(
PSO
)
,
g
en
etic
alg
o
r
ith
m
s
(
GA
)
,
an
d
f
ir
ef
ly
alg
o
r
ith
m
s
(
FA
)
ar
e
elab
o
r
ated
.
Sectio
n
4
d
escr
ib
es
t
h
e
s
im
u
latio
n
r
esu
lt
an
d
co
m
p
a
r
is
o
n
o
f
m
e
n
tio
n
e
d
alg
o
r
ith
m
p
e
r
f
o
r
m
an
ce
.
T
h
is
c
o
m
p
ar
is
o
n
is
b
ased
o
n
q
u
ad
r
o
t
o
r
m
o
d
elin
g
in
th
e
MA
T
L
A
B
s
o
f
twar
e
to
o
p
tim
ize
th
e
PID
co
n
tr
o
ller
an
d
th
e
q
u
ad
r
o
to
r
s
tab
ilit
y
with
th
e
f
o
u
r
d
ef
in
ed
p
ath
s
.
I
n
s
ec
tio
n
5
,
to
co
m
p
a
r
e
th
e
s
im
u
latio
n
an
d
r
ea
l
s
itu
atio
n
,
th
e
ex
tr
ac
ted
o
p
tim
ized
p
ar
am
eter
f
r
o
m
PS
O
alg
o
r
ith
m
ar
e
im
p
lem
en
ted
o
n
r
ea
l
p
latf
o
r
m
.
T
h
en
,
it
c
o
m
p
ar
es
th
e
er
r
o
r
b
etwe
en
th
e
p
l
an
n
ed
p
at
h
an
d
r
ea
l
r
o
b
o
t
m
o
v
em
en
t.
T
h
e
p
ap
e
r
e
n
d
ed
with
th
e
co
n
clu
s
io
n
in
s
ec
tio
n
6
.
T
h
e
m
ain
k
ey
p
o
in
t
in
th
is
s
tu
d
y
is
th
e
co
m
p
r
ess
io
n
s
tu
d
y
o
f
r
ea
l
p
er
f
o
r
m
an
ce
an
d
s
im
u
latio
n
o
f
d
esig
n
ed
u
n
m
a
n
n
ed
ae
r
ial
v
eh
i
cle
(
UAV
)
wit
h
th
e
m
eta
-
h
eu
r
is
tic
alg
o
r
ith
m
s
in
o
r
d
er
to
o
p
tim
ize
th
e
PID
co
n
tr
o
ller
.
2.
SYST
E
M
M
O
D
E
L
I
NG
T
h
e
s
y
s
tem
m
o
d
elin
g
in
clu
d
ed
cin
em
atic
an
d
d
y
n
am
ic
q
u
ad
r
o
t
o
r
,
d
r
iv
en
b
y
New
to
n
‒
E
u
ler
eq
u
atio
n
s
.
I
n
o
r
d
er
to
s
tu
d
y
t
h
e
q
u
ad
r
o
to
r
m
o
v
em
en
t
I
ts
n
ee
d
ed
to
h
av
e
two
co
o
r
d
in
ate
s
y
s
tem
s
[
7
]
.
On
e
r
elate
d
to
t
h
e
g
r
o
u
n
d
co
o
r
d
in
a
te
d
ev
ice
a
n
d
an
o
th
e
r
r
elate
d
t
o
s
p
ac
e
wh
ic
h
r
e
q
u
ir
es
E
u
ler
’
s
an
g
les
.
I
n
E
u
ler
’
s
an
g
les
th
e
a
n
g
les
ar
e
r
esu
lte
d
f
r
o
m
th
e
r
o
tatio
n
o
f
th
e
b
o
d
y
c
o
o
r
d
in
ate
s
y
s
tem
an
d
r
elativ
e
to
th
e
ea
r
th
co
o
r
d
in
ate
d
ev
ice
(
XYZ
)
as f
o
llo
ws.
a.
E
u
ler
an
g
les
Fo
r
f
in
d
in
g
p
o
s
itio
n
o
f
q
u
ad
r
o
to
r
in
th
e
s
p
ac
e
,
th
e
f
r
am
e
co
o
r
d
in
ate
s
y
s
tem
s
h
o
u
ld
b
e
p
lac
ed
to
war
d
th
e
g
r
o
u
n
d
c
o
o
r
d
i
n
ate
s
y
s
tem
.
T
h
e
p
o
s
itio
n
o
f
q
u
a
d
r
o
to
r
in
th
e
s
p
ac
e
is
d
escr
ib
ed
b
y
lo
ca
tio
n
v
ec
to
r
,
b
u
t
t
o
d
ef
in
e
its
o
r
ien
tatio
n
i
n
th
e
s
p
ac
e,
th
e
E
u
ler
a
n
g
les
ar
e
r
eq
u
ir
ed
[
7
]
.
T
h
ese
an
g
les
ar
e
th
e
r
esu
lts
o
f
s
p
in
n
i
n
g
ar
o
u
n
d
ax
is
:
Φ
:
r
o
ll
an
g
le
(
r
o
tat
in
g
ar
o
u
n
d
X
-
ax
is
)
,
θ
:
p
itch
a
n
g
le
(
r
o
tatin
g
a
r
o
u
n
d
Y
-
ax
is
)
,
ψ
:
y
aw
an
g
le
(
r
o
tatin
g
ar
o
u
n
d
Z
-
a
x
is
)
.
See
Fig
u
r
e
1.
Fig
u
r
e
1
.
C
in
em
atic
m
o
d
el
o
f
th
e
q
u
ad
r
o
to
r
b
o
d
y
As
its
n
am
e
im
p
lies
E
ar
th
f
r
am
e
co
n
s
id
er
in
g
(
Fig
u
r
e
1
)
as
an
in
er
tial
f
r
am
e
it
u
s
es
t
h
e
N
-
E
-
D
n
o
tatio
n
wh
er
e
th
e
ax
es p
o
in
t
to
th
e
n
o
r
th
,
ea
s
t a
n
d
d
o
wn
wa
r
d
s
r
esp
ec
tiv
ely
.
B
esid
es,
th
e
b
o
d
y
f
r
am
e
is
at
th
e
ce
n
ter
o
f
th
e
q
u
ad
r
o
to
r
b
o
d
y
,
with
its
x
-
ax
is
aim
in
g
to
war
d
s
p
r
o
p
eller
1
,
y
-
ax
is
p
o
in
tin
g
to
war
d
s
p
r
o
p
elle
r
2
an
d
th
e
z
-
ax
is
is
in
d
icatin
g
to
th
e
g
r
o
u
n
d
.
T
h
e
q
u
ad
r
o
to
r
o
r
i
en
tatio
n
is
d
escr
ib
ed
b
y
u
s
in
g
r
o
ll,
p
itch
,
a
n
d
y
aw
an
g
les
(
φ
,
θ
an
d
ψ
)
r
ep
r
esen
tin
g
r
o
tatio
n
s
ab
o
u
t
th
e
x
,
y,
an
d
z
-
ax
es,
r
esp
ec
tiv
ely
.
C
o
n
s
id
er
in
g
th
e
o
r
d
er
o
f
Evaluation Warning : The document was created with Spire.PDF for Python.
I
SS
N
:
2
0
8
9
-
4
8
5
6
I
n
t
J
R
o
b
&
A
u
to
m
,
Vo
l.
9
,
N
o
.
4
,
Dec
em
b
er
2
0
2
0
:
x
x
–
xx
258
r
o
tatio
n
to
b
e
r
o
ll
(
φ
)
,
p
itc
h
(
θ
)
th
en
y
aw
ψ
)
,
th
e
r
o
tatio
n
m
atr
ix
r
wh
ich
is
d
er
iv
ed
b
ased
o
n
t
h
e
s
eq
u
en
ce
o
f
p
r
in
cip
le
r
o
tatio
n
s
s
h
o
wn
in
(
1
)
.
R
=
[
C
θ
C
ψ
S
ϕ
S
θ
S
ψ
C
ϕ
S
θ
C
ψ
+
S
ϕ
S
ψ
C
θ
S
ψ
S
ϕ
S
θ
S
ψ
+
C
ϕ
C
ψ
C
ϕ
S
θ
S
ψ
−
S
ϕ
C
ψ
−
S
θ
S
ϕ
C
θ
C
ϕ
C
θ
]
(
1
)
As
it
s
h
o
wn
in
(
1
)
,
c
an
d
s
r
e
p
r
esen
t
co
s
an
d
s
in
as
th
e
d
er
iv
atio
n
o
f
th
e
r
o
tatio
n
.
T
h
e
r
o
tat
io
n
m
atr
ix
R
will
b
e
u
s
ed
in
f
r
am
in
g
th
e
d
y
n
am
ics
m
o
d
el
o
f
th
e
q
u
a
d
r
o
to
r
,
a
n
d
its
im
p
o
r
tan
ce
is
d
u
e
to
th
e
f
ac
t
th
at
s
o
m
e
s
tates
ar
e
m
ea
s
u
r
ed
in
t
h
e
b
o
d
y
f
r
am
e
(
e.
g
.
th
e
th
r
u
s
t
f
o
r
ce
s
s
h
ap
ed
b
y
th
e
p
r
o
p
eller
s
)
alth
o
u
g
h
s
o
m
e
o
th
er
s
ar
e
m
ea
s
u
r
e
d
in
t
h
e
in
er
tial
f
r
am
e
(
e.
g
.
t
h
e
g
r
a
v
itatio
n
al
f
o
r
ce
s
an
d
th
e
q
u
ad
r
o
to
r
'
s
p
o
s
itio
n
)
.
T
h
er
ef
o
r
e,
to
h
a
v
e
a
r
elatio
n
b
etwe
en
b
o
th
t
y
p
es
o
f
s
tates,
o
b
s
er
v
atio
n
was
d
o
n
e
f
o
r
a
tr
an
s
f
o
r
m
atio
n
f
r
o
m
o
n
e
f
r
am
e
.
T
o
o
b
tain
d
ata
ab
o
u
t
th
e
a
n
g
u
lar
v
elo
city
o
f
th
e
q
u
a
d
r
o
to
r
,
ty
p
ically
a
n
o
n
-
b
o
ar
d
i
n
er
tial
m
ea
s
u
r
em
en
t
u
n
i
t
(
I
MU
)
is
h
i
r
ed
to
h
a
v
e
th
e
v
el
o
city
in
th
e
b
o
d
y
co
o
r
d
in
ate
f
r
am
e.
T
h
e
n
to
r
elate
th
e
E
u
ler
r
ates,
as in
(
2
)
:
η
̇
=
[
ϕ
̇
θ
̇
ψ
̇
]
T
(
2
)
th
at
ar
e
m
ea
s
u
r
ed
in
th
e
in
er
tial
f
r
am
e
an
d
a
n
g
u
la
r
b
o
d
y
r
ates
ω
=
[
p
q
r
]
T
,
a
tr
an
s
f
o
r
m
atio
n
is
n
ee
d
ed
as (
3
)
:
ω
=
R
r
η
̇
(
3
)
wh
er
e
s
h
o
wn
in
(
4
)
:
R
r
=
[
1
0
−
s
in
ϕ
0
c
os
ϕ
s
in
ϕ
/
c
os
θ
0
−
c
os
ϕ
c
os
ϕ
/
c
os
θ
]
(
4
)
I
n
(
4
)
,
‘
S’
is
th
e
ab
b
r
ev
iatio
n
o
f
‘
s
in
’
an
d
‘
C
’
is
th
e
ab
b
r
ev
i
atio
n
o
f
‘
co
s
’
an
d
p
ar
am
eter
s
ϕ
,
θ
,
ψ
ar
e
r
o
ll,
p
itch
,
an
d
y
aw
an
g
les,
r
esp
ec
tiv
ely
.
I
ts
n
o
ted
th
at
Ar
o
u
n
d
th
e
h
o
v
e
r
p
o
s
itio
n
,
s
m
a
ll
an
g
le
ass
u
m
p
tio
n
is
m
ad
e
wh
er
e
c
os
φ
≡
1
,
C
os
θ
≡
1
,
an
d
s
in
φ
≡
0
,
Sin
θ
≡
0
.
T
h
u
s
R
r
ca
n
b
e
s
im
p
lifie
d
to
an
id
en
tity
m
atr
ix
I
[
7
]
.
T
o
p
r
esen
t
o
f
m
o
d
elin
g
,
th
e
s
y
s
tem
is
r
e
q
u
ir
ed
o
f
to
r
q
u
e
ca
lcu
latio
n
o
f
q
u
ad
r
o
to
r
,
s
wir
l
m
o
v
em
en
t
eq
u
atio
n
s
,
th
e
n
o
n
-
g
r
av
itatio
n
al
f
o
r
ce
o
f
q
u
ad
r
o
to
r
an
d
d
y
n
am
ic
o
f
r
o
to
r
s
,
wh
i
ch
is
d
escr
ib
in
g
in
th
e
f
o
llo
win
g
.
b.
T
h
e
q
u
a
d
r
o
to
r
to
r
q
u
e
Am
o
n
g
ap
p
lied
f
o
r
ce
to
tu
r
n
,
th
er
e
is
a
f
o
r
ce
,
ca
lled
ae
r
o
d
y
n
a
m
ics
o
r
liftin
g
f
o
r
ce
a
n
d
th
er
e
is
a
p
r
o
d
u
ce
d
to
r
q
u
e
,
ca
lled
ae
r
o
d
y
n
am
ic
to
r
q
u
e
w
h
ich
h
as sh
o
wn
in
(
5
)
.
M
B
=
[
lK
f
(
−
Ω
2
2
+
Ω
4
2
)
lK
f
(
Ω
1
2
−
Ω
2
2
)
K
M
(
Ω
1
2
−
Ω
2
2
−
Ω
3
2
−
Ω
4
2
)
]
(
5
)
I
n
(
5
)
M
B
is
th
e
g
en
er
al
eq
u
atio
n
o
f
to
r
q
u
e
ap
p
lie
d
o
n
a
q
u
ad
r
o
to
r
,
1
is
ar
m
q
u
ad
r
o
to
r
,
K
f
is
a
n
ae
r
o
d
y
n
am
ic
f
o
r
ce
a
n
d
Ω
1
is
th
e
v
elo
city
o
f
th
e
f
ir
s
t
r
o
to
r
,
Ω
2
is
th
e
v
elo
city
o
f
th
e
th
ir
d
r
o
t
o
r
,
a
n
d
Ω
4
is
th
e
v
elo
city
o
f
th
e
f
o
u
r
th
r
o
t
o
r
.
c.
Swirl
m
o
v
em
en
t e
q
u
atio
n
s
Swirl
m
o
v
em
en
t
eq
u
atio
n
o
f
q
u
ad
r
o
to
r
is
tak
en
f
r
o
m
New
to
n
'
s
s
ec
o
n
d
law
an
d
g
r
o
u
n
d
in
e
r
tia
f
r
am
e
wh
ich
is
s
h
o
wn
in
(
6
)
[
7
]
.
m
r
̈
=
[
0
0
mg
]
+
RF
B
(
6
)
As
it
h
as
s
h
o
wn
in
(
6
)
,
r
=
[
x
y
z
]
T
,
w
h
ich
r
ep
r
esen
t
th
e
d
is
tan
ce
o
f
q
u
ad
r
o
to
r
to
th
e
in
e
r
tial
f
r
am
e,
m
(
q
u
a
d
r
o
t
o
r
weig
h
t)
,
g
(
q
u
a
d
r
o
to
r
weig
h
t)
an
d
F
B
(
n
o
n
-
g
r
av
itatio
n
al
f
o
r
ce
ap
p
lied
o
n
q
u
a
d
r
o
to
r
)
an
d
R
is
th
e
co
n
n
ec
tio
n
o
f
two
f
r
am
es
co
o
r
d
in
ate
s
y
s
tem
o
f
q
u
ad
r
o
t
o
r
to
war
d
co
o
r
d
i
n
at
e
s
y
s
tem
[
7
]
.
d.
No
n
-
g
r
a
v
itatio
n
al
f
o
r
ce
ap
p
lie
d
o
n
th
e
q
u
a
d
r
o
to
r
W
h
en
ev
er
th
e
q
u
a
d
r
o
t
o
r
is
in
th
e
h
o
r
iz
o
n
tal
o
r
ien
tatio
n
s
,
th
e
p
r
o
d
u
ce
d
f
o
r
ce
b
y
tu
r
n
i
n
g
f
an
s
is
th
e
o
n
ly
n
o
n
-
g
r
av
itatio
n
al
f
o
r
ce
ap
p
lied
o
n
it
wh
ich
is
co
r
r
esp
o
n
d
in
g
s
q
u
ar
e
an
g
u
lar
v
el
o
city
o
f
f
an
(
6
)
.
So
,
n
o
n
o
n
-
g
r
av
itatio
n
al
f
o
r
ce
is
e
f
f
ec
tiv
e
o
n
q
u
a
d
r
o
to
r
.
I
n
th
is
ca
s
e
F
B
is
d
ef
in
ed
s
u
ch
as (
7
)
.
Evaluation Warning : The document was created with Spire.PDF for Python.
I
n
t
J
R
o
b
&
A
u
to
m
I
SS
N:
2089
-
4
8
5
6
Th
e
q
u
a
d
r
o
t
o
r
d
yn
a
mic
mo
d
elin
g
a
n
d
s
tu
d
y
o
f m
eta
-
h
eu
r
is
tic
a
lg
o
r
ith
ms
... (
A
ta
J
a
h
a
n
g
ir
Mo
s
h
a
ye
d
)
259
F
B
=
[
0
0
−
K
f
(
Ω
1
2
+
Ω
2
2
+
Ω
3
2
+
Ω
4
2
)
]
(
7
)
I
n
(
7
)
,
Ω
1
is
th
e
f
ir
s
t
Ω
2
is
s
ec
o
n
d
,
Ω
3
is
th
e
th
ir
d
r
o
to
r
v
elo
city
,
Ω
4
is
th
e
f
o
u
r
th
r
o
to
r
v
el
o
city
,
a
n
d
K
f
is
th
e
ae
r
o
d
y
n
am
ic
f
o
r
ce
[
7
]
.
e.
R
o
to
r
s
d
y
n
am
ics
Usu
ally
b
r
u
s
h
less
m
o
to
r
(
B
L
DC
)
u
s
ed
in
th
e
q
u
ad
r
o
to
r
ar
e
DC
m
o
to
r
s
o
r
b
r
u
s
h
less
,
wh
ic
h
h
av
e
h
ig
h
to
r
q
u
e
an
d
lo
w
f
r
ictio
n
.
T
h
e
d
y
n
am
ic
o
f
DC
an
d
b
r
u
s
h
less
m
o
to
r
,
in
n
o
r
m
al
co
n
d
itio
n
s
,
i
s
lik
e
th
e
r
eg
u
lar
l
y
f
ix
ed
en
g
in
es,
wh
ich
h
as sh
o
wn
in
(
8
)
.
υ
=
R
mot
K
mot
J
r
Ω
̇
i
+
K
mot
Ω
i
+
K
M
R
mot
Ω
i
2
(
8
)
I
n
(
8
)
,
K
mot
is
m
o
to
r
to
r
q
u
e
c
o
n
s
tan
t,
K
M
is
ae
r
o
d
y
n
am
ic
m
o
m
en
t
c
o
n
s
tan
t,
R
mot
is
m
o
to
r
cir
cu
it
r
esis
tan
ce
an
d
J
r
is
r
o
to
r
'
s
in
er
tia
[
7
]
.
f.
T
h
e
s
tate
s
p
ac
e
m
o
d
el
Ob
tain
in
g
th
e
s
tate
s
p
ac
e
m
o
d
el,
h
elp
s
to
d
esig
n
o
f
th
e
co
n
tr
o
ller
a
n
d
it
is
r
e
q
u
ir
ed
X
s
ate
v
ar
iab
le
an
d
th
eir
c
o
r
r
esp
o
n
d
in
g
elem
e
n
t.
g.
X
s
tate
v
ar
iab
le
C
o
n
s
id
er
in
g
th
e
s
tate
v
ec
to
r
o
f
th
e
q
u
a
d
r
o
to
r
to
b
e
(
9
)
:
X
=
[
1
2
3
4
5
6
7
8
9
10
11
12
]
T
(
9
)
wh
ich
is
m
ap
p
ed
t
o
th
e
d
e
g
r
ee
s
o
f
f
r
ee
d
o
m
o
f
th
e
q
u
ad
r
o
to
r
i
n
th
e
f
o
llo
win
g
m
an
n
er
(
1
0
)
[
7
]
.
X
=
[
ϕ
ϕ
̇
θ
θ
̇
ψ
ψ
̇
z
z
̇
̇
̇
]
T
(
1
0
)
I
n
(
1
0
)
,
x
,
y
,
a
n
d
z
ar
e
th
e
p
o
s
itio
n
s
;
ϕ
,
θ
,
an
d
ψ
ar
e
th
e
E
u
ler
an
g
les
;
an
d
th
eir
d
e
r
iv
atio
n
w
h
ich
in
d
icate
s
th
e
co
m
p
o
n
e
n
t
v
elo
city
.
T
h
e
s
tate
s
p
ac
e
v
ec
to
r
is
n
o
tab
le
b
e
ca
u
s
e
o
f
th
e
ca
p
ab
ilit
y
o
f
s
h
o
win
g
p
o
s
itio
n
an
g
le
an
d
th
e
lin
ea
r
v
elo
city
o
f
q
u
ad
r
o
to
r
[
7
]
.
h.
I
n
p
u
t
v
ar
iab
le
o
f
th
e
s
tate
v
ec
t
o
r
T
h
e
p
a
r
am
eter
U
as
a
co
n
tr
o
l
in
p
u
t
v
ec
to
r
co
n
tain
in
g
o
f
f
o
u
r
in
p
u
ts
;
U
1
th
r
o
u
g
h
U
4
is
d
ef
in
ed
as,
s
h
o
wn
in
(
1
1
)
.
U
=
[
U
1
U
2
U
3
U
4
]
(
1
1
)
Acc
o
r
d
in
g
t
o
(
1
1
)
,
ea
c
h
in
p
u
t
it c
an
b
e
s
h
o
wn
as
(
1
2
)
–
(
1
6
)
.
U
1
=
K
f
(
Ω
1
2
−
Ω
2
2
+
Ω
3
2
−
Ω
4
2
)
(
1
2
)
U
2
=
K
f
(
Ω
2
2
+
Ω
4
2
)
(
1
3
)
U
3
=
K
f
(
Ω
1
2
−
Ω
3
2
)
(
1
4
)
U
4
=
K
M
(
Ω
1
2
−
Ω
2
2
+
Ω
3
2
−
Ω
4
2
)
(
1
5
)
I
n
th
e
ab
o
v
e
r
elatio
n
s
,
k
f
is
ae
r
o
d
y
n
a
m
ic
f
o
r
ce
s
,
k
m
is
f
ix
e
d
t
o
r
q
u
e
an
d
is
th
e
a
n
g
u
la
r
v
e
lo
city
r
esu
lted
f
r
o
m
r
o
to
r
s
.
Fin
ally
,
th
e
r
elatio
n
o
f
(
1
2
)
–
(
1
5
)
ca
n
b
e
s
h
o
wn
as th
e
m
atr
ix
f
o
r
m
as sh
o
wn
i
n
(
1
6
)
[
7
]
.
[
U
1
U
2
U
3
U
4
]
=
[
K
f
K
f
K
f
K
f
0
−
K
f
0
K
f
K
f
0
−
K
f
0
−
K
M
−
K
M
K
M
−
K
M
]
[
Ω
1
2
Ω
2
2
Ω
3
2
Ω
4
2
]
(
1
6
)
Acc
o
r
d
in
g
to
r
elatio
n
(
1
6
)
,
u
1
is
th
e
p
o
we
r
o
f
th
e
f
o
u
r
r
o
u
ter
s
th
ar
g
iv
es
th
e
q
u
a
d
r
o
to
r
h
eig
h
t
f
r
o
m
Z
to
Z
.
u
2
is
th
e
r
esu
lt
o
f
t
h
e
d
if
f
er
en
ce
in
to
r
q
u
e
b
etwe
en
e
n
g
i
n
e
an
d
4
,
wh
ic
h
r
esu
lts
in
a
q
u
ad
r
o
to
r
r
o
tatio
n
ar
o
u
n
d
th
e
r
o
ll
an
g
le
f
r
o
m
to
.
u
3
is
th
e
d
if
f
er
en
ce
o
f
to
r
q
u
e
b
etwe
en
th
e
m
o
to
r
s
7
an
d
3
th
at
r
o
tates
th
e
q
u
ad
r
a
n
t
ar
o
u
n
d
th
e
an
g
le
o
f
th
e
p
itch
f
r
o
m
to
.
u
4
r
esu
lts
f
r
o
m
th
e
to
r
q
u
e
d
i
f
f
er
en
ce
b
etwe
en
th
e
two
Evaluation Warning : The document was created with Spire.PDF for Python.
I
SS
N
:
2
0
8
9
-
4
8
5
6
I
n
t
J
R
o
b
&
A
u
to
m
,
Vo
l.
9
,
N
o
.
4
,
Dec
em
b
er
2
0
2
0
:
x
x
–
xx
260
p
air
o
f
m
o
to
r
s
clo
ck
an
d
clo
ck
wis
e
an
d
it
r
o
tates
ar
o
u
n
d
th
e
an
g
le
o
f
y
o
u
r
f
r
o
m
t
o
.
T
h
e
s
p
ee
d
o
f
th
e
r
o
to
r
s
ca
n
b
e
ca
lcu
lated
b
y
th
e
co
n
tr
o
l
i
n
p
u
t.
T
h
er
e
f
o
r
e,
t
h
e
in
v
e
r
s
e
r
elatio
n
b
etwe
en
th
e
co
n
tr
o
l
in
p
u
t
an
d
th
e
r
o
to
r
s
p
ee
d
ca
n
b
e
o
b
tain
e
d
as (
1
7
)
[
8
].
[
Ω
1
2
Ω
2
2
Ω
3
2
Ω
4
2
]
=
[
1
4K
f
0
1
2K
f
1
4K
M
1
4K
f
−
1
2K
f
0
−
1
4K
M
1
4K
f
0
−
1
2K
f
1
4K
M
1
4K
f
1
2K
f
0
−
1
4K
M
]
[
U
1
U
2
U
3
U
4
]
(
1
7
)
I
n
th
e
ab
o
v
e
r
elatio
n
s
,
is
th
e
s
p
ee
d
o
f
co
n
s
tan
t
to
r
q
u
e,
k
m
is
ae
r
o
d
y
n
a
m
ic
f
o
r
ce
an
d
K
f
is
th
e
an
g
le
o
f
r
o
to
r
s
.
I
n
th
e
r
elatio
n
(
1
7
)
th
e
co
n
tr
o
l
in
p
u
ts
ca
n
b
e
ca
lcu
lated
b
y
tak
in
g
r
o
o
t
f
r
o
m
th
e
r
o
to
r
s
p
ee
d
[9
]
.
At
th
e
en
d
,
b
y
tak
in
g
th
e
s
q
u
ar
e
r
o
o
t
o
f
t
h
at,
th
e
v
elo
city
o
f
r
o
t
o
r
s
ca
n
b
e
ca
lcu
lated
f
r
o
m
th
e
co
n
tr
o
l
in
p
u
ts
as
s
h
o
wn
in
(
1
8
)
:
Ω
1
=
√
1
4K
f
U
1
1
2K
f
U
3
+
1
4K
M
U
4
Ω
2
=
√
1
4K
f
U
1
1
2K
f
U
2
+
1
4K
M
U
4
Ω
3
=
√
1
4K
f
U
1
1
2K
f
U
3
+
1
4K
M
U
4
Ω
4
=
√
1
4K
f
U
1
1
2K
f
U
2
+
1
4K
M
U
4
(
1
8
)
T
ak
in
g
th
e
s
q
u
ar
e
r
o
o
t
o
f
th
at
,
th
e
r
o
t
o
r
s
'
v
elo
cities
ca
n
b
e
ca
lcu
lated
f
r
o
m
th
e
c
o
n
tr
o
l
i
n
p
u
ts
as
f
o
llo
ws.
I
n
th
e
ab
o
v
e
eq
u
atio
n
,
K
f
is
ae
r
o
d
y
n
am
ic
f
o
r
ce
,
K
M
is
co
n
s
tan
t to
r
q
u
e,
an
d
Ω
is
th
e
an
g
u
lar
v
elo
city
o
f
r
o
to
r
s
.
i.
R
o
tatio
n
al
eq
u
atio
n
o
f
m
o
tio
n
I
t
l
ea
d
s
to
d
y
n
am
ic
eq
u
atio
n
s
o
f
th
e
s
y
s
tem
ar
e
d
escr
ib
ed
in
(
1
9
)
to
(
2
1
)
.
ϕ
̈
=
l
I
U
2
−
J
r
I
θ
̇
Ω
r
+
I
I
ψ
̇
θ
̇
U
2
−
I
zz
I
θ
̇
ψ
̇
(
1
9
)
θ
̈
=
l
I
U
3
−
J
r
I
ϕ
̇
Ω
r
+
I
zz
I
ϕ
̇
ψ
̇
−
I
I
ψ
̇
ϕ
̇
(
2
0
)
ψ
̈
=
l
I
zz
U
4
−
I
I
zz
θ
̇
ϕ
̇
+
I
I
zz
ϕ
̇
θ
̇
(
2
1
)
B
y
s
elec
t
th
e
co
n
tr
o
l
in
p
u
t
v
ec
to
r
U,
it
is
c
lear
th
at
th
e
r
o
tati
o
n
al
s
u
b
s
y
s
tem
is
f
u
lly
ac
tu
at
ed
,
it
is
o
n
ly
r
elian
t
o
n
th
e
r
o
tatio
n
al
s
tate
v
ar
iab
le
s
x
1
to
x
6
th
at
co
r
r
esp
o
n
d
to
φ,
φ,
θ
,
θ
̇
,
ψ
,
ψ
̇
r
esp
ec
tiv
ely
.
j.
T
r
an
s
latio
n
al
eq
u
atio
n
o
f
m
o
ti
o
n
:
C
o
n
s
id
er
in
g
(
1
0
)
an
d
(
6
)
th
e
t
o
tal
in
s
e
r
ted
to
r
q
u
e
o
n
th
e
UA
V
ca
n
b
e
wr
itten
as (
2
2
)
[
7
]:
F
b
=
[
0
0
−
U
1
]
(
2
2
)
wh
ich
af
ter
r
e
p
lacin
g
th
e
p
ar
a
m
eter
in
(
4
)
,
(
2
3
)
to
(
2
5
)
ca
n
b
e
s
u
b
s
titu
ted
:
x
̈
=
−
U
1
m
(
s
in
ϕ
s
in
ψ
+
c
os
ϕ
c
os
ψ
s
in
θ
)
(
2
3
)
̈
=
−
U
1
m
(
c
os
ϕ
s
in
ψ
s
in
θ
−
c
os
ψ
s
in
ϕ
)
(
2
4
)
Evaluation Warning : The document was created with Spire.PDF for Python.
I
n
t
J
R
o
b
&
A
u
to
m
I
SS
N:
2089
-
4
8
5
6
Th
e
q
u
a
d
r
o
t
o
r
d
yn
a
mic
mo
d
elin
g
a
n
d
s
tu
d
y
o
f m
eta
-
h
eu
r
is
tic
a
lg
o
r
ith
ms
... (
A
ta
J
a
h
a
n
g
ir
Mo
s
h
a
ye
d
)
261
z
̈
=
g
−
−
U
1
m
(
c
os
ϕ
c
os
θ
)
(
2
5
)
B
ased
o
n
m
en
tio
n
ed
eq
u
atio
n
s
,
J
r
is
r
o
to
r
s
to
r
q
u
e,
I
XX
,
I
YY
,
I
ZZ
ar
e
th
e
to
r
q
u
e
f
r
o
m
in
er
tia
in
th
e
m
ajo
r
ax
is
o
f
b
o
d
y
f
r
am
e
ϕ
̇
θ
̇
ψ
̇
ar
e
v
elo
city
,
ψ
̈
ϕ
̈
θ
̈
ar
e
in
s
tan
t
m
o
m
en
tu
m
o
f
an
g
u
lar
E
u
ler
[
7
]
.
Fin
ally
,
ac
co
r
d
in
g
to
th
e
r
e
q
u
ir
ed
p
ar
am
eter
s
b
y
p
h
y
s
ical
s
im
u
latio
n
o
f
q
u
ad
r
o
to
r
in
C
AT
I
A
s
o
f
twar
e,
s
o
m
e
q
u
ad
r
o
to
r
p
ar
am
eter
s
ar
e
c
o
m
p
u
ted
a
n
d
th
e
r
est
o
f
th
e
m
ar
e
esti
m
ated
b
y
s
im
ilar
p
h
y
s
ics
o
f
r
o
b
o
t’
s
ev
alu
atio
n
wh
ic
h
h
av
e
s
h
o
w
n
in
T
a
b
le
1
.
As
it
h
as
p
r
esen
ted
in
T
a
b
le
1
,
I
XX
,
I
YY
,
I
ZZ
an
d
I
r
o
t
o
r
ar
e
th
e
m
o
m
en
ts
o
f
in
e
r
tia
ar
o
u
n
d
X
-
ax
is
,
Y
-
ax
is
,
Z
-
ax
is
an
d
m
o
t
o
r
ax
is
r
esp
ec
tiv
ely
,
m
is
th
e
weig
h
t
o
f
m
o
to
r
,
b
is
th
e
p
r
o
p
eller
r
esu
lt f
o
r
ce
in
d
e
x
,
d
is
th
e
p
r
o
p
eller
c
o
u
n
te
r
p
o
i
s
e
i
n
d
ex
,
g
is
th
e
ea
r
th
g
r
av
itatio
n
al
f
o
r
ce
.
T
ab
le
1
.
Qu
a
d
r
atu
r
e
p
ar
am
eter
s
test
ed
No
P
a
r
a
me
t
e
r
s
C
o
n
t
e
n
t
U
n
i
t
1
5
.
49
∗
10
−
4
2
2
5
.
49
∗
10
−
4
2
3
8
.
28
∗
10
−
4
2
4
5
.
51
∗
10
−
5
2
5
0
.
620
6
2
.
9
∗
10
−
5
2
7
0
.
14
8
1
.
1
∗
10
−
6
2
9
9
.
81
−
2
3.
P
I
D
CO
NT
RO
L
L
E
R
T
h
e
PID
c
o
n
tr
o
ller
b
elo
n
g
s
t
o
th
o
s
e
g
r
o
u
p
o
f
co
n
tr
o
ller
s
,
wh
ich
wo
r
k
b
ased
o
n
th
e
f
ee
d
b
ac
k
,
an
d
m
o
r
e
f
a
m
o
u
s
as
a
class
ical
co
n
tr
o
llin
g
an
d
,
it’s
d
esig
n
an
d
im
p
lem
en
tatio
n
is
v
e
r
y
c
u
s
to
m
ar
y
[
8
]
.
On
e
o
f
th
e
g
r
ea
t c
h
allen
g
es o
f
t
h
e
f
lie
r
r
o
b
o
t
p
ilo
ts
is
o
b
tain
in
g
s
u
itab
le
PID
in
d
ex
ac
co
r
d
in
g
to
th
e
p
h
y
s
ical
s
tr
u
ctu
r
e
o
f
q
u
ad
r
o
to
r
.
Mo
s
t
o
f
th
e
q
u
ad
r
o
to
r
’
s
p
ilo
ts
tr
y
th
e
f
alse
an
d
tr
u
e
m
eth
o
d
wh
ic
h
lead
s
to
m
a
k
e
s
o
m
a
n
y
p
r
o
b
lem
s
an
d
h
ar
d
s
h
ip
t
o
th
e
r
o
b
o
t.
So
,
o
b
tain
in
g
th
e
PID
in
d
ex
es
is
s
ig
n
if
ican
t
[
9
]
.
T
o
c
o
n
tr
o
l
o
f
m
en
tio
n
e
d
p
ar
am
eter
s
,
f
ir
s
t,
PID
co
n
tr
o
ller
b
y
m
eta
-
h
eu
r
is
tic
alg
o
r
ith
m
s
h
av
e
b
ee
n
u
s
ed
wh
ich
h
av
e
b
ee
n
d
escr
ib
e
d
i
n
th
e
f
o
llo
win
g
.
E
q
u
atio
n
(
2
6
)
i
s
th
e
g
en
e
r
al
eq
u
atio
n
f
o
r
PI
D
co
n
tr
o
ller
wh
ich
K
p
is
p
r
o
p
o
r
t
io
n
al,
g
ain
,
K
d
is
d
er
iv
ativ
e
g
ain
an
d
K
i
is
in
teg
r
at
o
r
g
ain
[
1
0
]
.
(
)
=
(
)
+
(
)
⁄
+
∫
(
)
(
2
6
)
Acc
o
r
d
in
g
t
o
(
1
0
)
,
th
e
c
o
n
tr
o
l
in
p
u
ts
,
an
d
PID
c
o
n
tr
o
ller
o
f
th
e
q
u
ad
r
o
to
r
a
r
e
d
ef
i
n
ed
s
u
ch
as
(
1
1
)
t
o
(
2
6
)
[
1
1
,
1
2
]
.
I
n
th
e
a
b
o
v
e
eq
u
atio
n
s
,
p
ar
am
ete
r
s
K
p
,
K
dz
,
K
iz
r
esp
ec
tiv
ely
ar
e
c
o
n
tr
o
lle
d
p
r
o
p
o
r
tio
n
a
l
,
in
teg
r
ato
r
an
d
d
er
i
v
ate
g
ain
,
in
h
eig
h
t
co
n
tr
o
l.
T
h
e
in
d
ex
e
s
o
f
K
p
φ
,
K
d
φ
,
K
i
φ
ar
e
c
o
n
tr
o
l
g
ain
s
to
c
o
n
tr
o
l
‘
r
o
ll
’.
K
p
θ
,
K
d
θ
,
K
i
θ
ar
e
co
n
tr
o
lled
g
ain
s
to
co
n
tr
o
l
‘
p
itch
’
an
d
K
p
ψ
,
K
d
ψ
,
K
i
ψ
ar
e
co
n
tr
o
l
g
ai
n
s
to
co
n
t
r
o
l
‘
y
aw
’
[
1
3
,
1
4
]
.
I
n
o
r
d
er
to
f
in
d
t
h
e
PID
p
ar
am
eter
,
Z
ie
g
ler
‒
Nich
o
ls
r
u
les
ar
e
an
in
n
o
v
ativ
e
m
eth
o
d
o
f
co
n
tr
o
llin
g
p
a
r
am
eter
s
in
o
b
t
ain
in
g
v
alu
es
o
f
in
teg
r
al
d
e
r
iv
ativ
e
p
r
o
p
o
r
tio
n
al
co
n
tr
o
ller
co
ef
f
icien
ts
.
T
h
is
m
eth
o
d
h
as
b
ee
n
d
ev
elo
p
ed
b
y
Z
ieg
ler
an
d
Nich
o
ls
.
I
n
t
h
is
s
tate,
th
e
s
y
s
tem
r
ea
ch
es
th
e
b
o
u
n
d
a
r
y
o
f
s
atiab
ilit
y
an
d
th
e
p
e
r
io
d
o
f
k
u
f
lu
ctu
atio
n
an
d
its
o
u
t
p
u
t
a
r
e
f
in
ed
o
s
cillatin
g
[
1
5
]
.
T
o
u
s
e
th
is
m
eth
o
d
,
th
e
q
u
ad
r
o
to
r
lin
ea
r
d
y
n
am
ic
s
h
o
u
ld
b
e
m
o
d
eled
.
T
ab
le
2
s
h
o
ws
th
e
Z
ieg
ler
‒
Nich
o
ls
r
u
les
f
o
r
ad
ju
s
tin
g
co
n
tr
o
l
g
ain
s
[
1
6
]
.
T
ab
le
2
.
C
o
ef
f
icien
ts
o
f
PID
c
o
n
tr
o
l
ler
r
esp
o
n
s
e
with
th
e
s
te
p
in
p
u
t
f
u
n
ctio
n
No
P
a
r
a
me
t
e
r
s
1
P
i
t
c
h
97
1
.
5
0
.
8
2
R
o
l
l
63
3
.
7
0
.
2
3
Y
a
w
43
4
.
8
0
.
1
3
.
1
.
P
I
D
o
ptim
iza
t
io
n
I
n
o
r
d
er
to
o
p
tim
ize
th
e
PID
p
ar
am
eter
,
an
a
p
p
r
o
p
r
iate
c
o
s
t
f
u
n
ctio
n
m
u
s
t
b
e
c
o
n
s
id
er
ed
in
o
r
d
er
to
ap
p
r
o
p
r
iately
q
u
a
n
tify
th
e
co
n
tr
o
l
in
ter
est
in
th
e
co
n
tr
o
l
a
lg
o
r
ith
m
.
T
h
e
co
s
t
f
u
n
ctio
n
ca
n
b
e
d
ef
in
e
d
o
n
th
e
b
asis
o
f
d
esira
b
le
ch
ar
ac
ter
is
tics
.
Fo
r
ex
am
p
le,
th
e
am
o
u
n
t
o
f
its
ex
ce
s
s
i
s
s
p
ec
if
ied
b
y
M
p
,
tim
e
to
ascen
t
s
p
ec
if
ied
b
y
t
r
last
in
g
r
esp
o
n
s
e
v
alu
e
is
d
ef
in
ed
b
y
e
ss
an
d
s
ettlin
g
tim
e
is
d
ef
in
ed
b
y
t
s
.
T
h
e
p
en
alty
Evaluation Warning : The document was created with Spire.PDF for Python.
I
SS
N
:
2
0
8
9
-
4
8
5
6
I
n
t
J
R
o
b
&
A
u
to
m
,
Vo
l.
9
,
N
o
.
4
,
Dec
em
b
er
2
0
2
0
:
x
x
–
xx
262
f
u
n
ctio
n
m
et
h
o
d
is
u
s
ed
to
d
ef
in
e
th
e
co
s
t
f
u
n
ctio
n
f
o
r
o
p
tim
izatio
n
alg
o
r
ith
m
s
.
Acc
o
r
d
in
g
to
th
is
m
eth
o
d
,
th
e
co
s
t f
u
n
ctio
n
is
d
ef
in
ed
as (
2
7
)
:
=
−
(
+
)
+
(
1
−
−
)
(
+
)
(
2
7
)
As
m
en
tio
n
ed
in
(
2
7
)
,
v
al
u
e
ca
n
b
e
ch
an
g
ed
.
C
o
n
s
id
e
r
in
g
th
e
s
am
e
c
o
n
d
itio
n
o
f
i
n
f
lu
en
cin
g
th
e
in
ten
d
ed
attr
ib
u
tes
o
n
th
e
co
s
t
f
u
n
ctio
n
,
t
h
is
v
alu
e
is
eq
u
al
0
.
6
9
[
1
7
–
20
]
.
B
ased
o
n
th
e
co
s
t
f
u
n
ctio
n
d
ef
in
itio
n
,
th
e
p
ar
am
eter
s
o
f
ea
ch
r
esp
o
n
s
e
o
f
o
p
tim
izatio
n
p
r
o
b
lem
s
s
u
ch
as
K
p
z
,
K
dz
,
K
iz
ar
e
r
esp
ec
tiv
ely
th
e
p
r
o
p
o
r
tio
n
al
-
d
e
r
iv
ativ
e
an
d
in
teg
r
atio
n
g
ain
s
in
co
n
tr
o
l
h
eig
h
t.
T
h
e
K
p
φ
,
K
d
φ
,
K
i
φ
ar
e
th
e
co
n
tr
o
l
g
ain
f
o
r
r
o
ll
co
n
tr
o
l.
K
p
θ
,
K
d
θ
,
K
i
θ
ar
e
th
e
co
n
tr
o
l
g
ain
f
o
r
p
itch
c
o
n
tr
o
l
an
d
K
p
ψ
,
K
d
ψ
,
K
i
ψ
ar
e
th
e
c
o
n
tr
o
l
g
ain
f
o
r
y
aw
c
o
n
tr
o
l
wh
ich
is
twelv
e
p
ar
am
eter
s
alto
g
eth
er
.
I
n
o
th
er
wo
r
d
s
,
ea
ch
r
esp
o
n
s
e
o
f
p
r
o
b
l
em
s
ar
e
in
d
icatin
g
twelv
e
v
alu
es
f
o
r
PID
in
d
e
x
a
n
d
th
e
d
u
ty
o
f
th
e
d
ef
in
ed
alg
o
r
ith
m
s
is
f
in
d
in
g
th
e
b
est
an
s
wer
co
n
s
id
er
in
g
all
in
tr
o
d
u
cin
g
d
e
f
in
ed
p
ar
am
ete
r
’
s
[
1
5
]
.
I
n
PID
in
d
e
x
es
o
p
ti
m
izatio
n
,
th
e
n
atu
r
e
-
i
n
s
p
ir
ed
alg
o
r
ith
m
h
as
u
s
ed
[
1
6
]
wh
ic
h
ar
e
d
escr
ib
ed
s
h
o
r
tly
as f
o
llo
win
g
.
3
.
1
.
1
.
P
SO
a
lg
o
rit
hm
T
h
e
PS
O
alg
o
r
ith
m
is
tak
en
f
r
o
m
th
e
ag
g
r
eg
atio
n
o
f
p
ar
ticl
es
an
d
it
was
in
s
p
ir
ed
f
r
o
m
b
i
r
d
'
s
tr
o
o
p
an
d
f
is
h
es.
T
h
ese
k
in
d
s
o
f
b
ir
d
s
ar
e
p
u
ttin
g
in
a
s
ea
r
ch
in
g
s
p
ac
e
o
f
p
ix
el
tab
le
in
r
a
n
d
o
m
an
d
in
e
v
er
y
r
ep
etitio
n
,
th
e
clo
s
ed
n
eig
h
b
o
r
o
f
p
ar
ticle
h
as
s
elec
ted
an
d
s
p
ee
d
o
f
th
e
p
a
r
ticle
is
r
ep
lace
d
with
th
e
s
p
ee
d
o
f
its
clo
s
est
n
eig
h
b
o
r
[
17
]
.
T
h
is
ac
tio
n
ca
u
s
es
th
e
f
ast
m
o
v
em
en
t
o
f
a
g
r
o
u
p
in
th
e
s
a
m
e
d
ir
ec
tio
n
with
u
n
ce
r
tain
m
o
v
em
en
t
an
d
c
o
n
v
er
g
es
with
o
u
t c
h
a
n
g
in
g
[
18
]
.
3
.
1
.
2
.
GA
Gen
e
is
th
e
m
ain
s
av
in
g
u
n
it
in
g
en
etic.
I
n
s
id
e
ce
lls
,
g
en
es
ar
e
co
n
n
ec
ted
to
ea
c
h
o
th
er
t
o
p
r
o
d
u
c
e
ch
r
o
m
o
s
o
m
e.
T
h
is
n
ew
ce
ll
c
alled
‘
d
ip
lo
id
’
an
d
it
ca
u
s
ed
t
h
r
o
u
g
h
m
eio
s
is
.
I
n
m
eio
s
is
,
ea
ch
ch
r
o
m
o
s
o
m
e
o
f
th
e
d
ip
lo
id
ce
lls
m
ak
es
a
co
p
y
o
f
th
em
s
elv
es.
T
h
en
,
ch
r
o
m
o
s
o
m
es
g
r
o
u
p
(
n
ew
an
d
m
ain
)
ar
e
co
m
p
o
u
n
d
e
d
to
ea
ch
o
th
er
[2
1
]
.
Af
te
r
it,
ch
r
o
m
o
s
o
m
es
will
s
ep
ar
ate
o
n
e
m
o
r
e
tim
e
an
d
4
d
ip
lo
i
d
ce
lls
ar
e
m
ad
e.
Mu
tatio
n
ca
n
tak
e
p
lace
in
ea
ch
s
tag
e.
E
v
er
y
s
in
g
le
c
h
an
g
e
i
n
ch
r
o
m
o
s
o
m
es tr
an
s
f
er
to
th
e
ch
ild
r
e
n
[
2
2
]
.
3
.
1
.
3
.
FA
FA
is
b
ased
o
n
f
lick
er
in
g
wh
i
ch
is
d
is
cu
s
s
ed
u
p
to
n
o
w.
T
h
e
b
ase
o
f
f
lick
er
i
n
g
is
ju
s
t
lik
e
a
f
i
r
ef
ly
’
s
b
eh
av
io
r
.
T
h
e
f
lick
er
in
g
o
f
th
ese
in
s
ec
ts
is
b
ased
o
n
a
b
io
l
o
g
ical
illu
m
in
atio
n
p
r
o
ce
s
s
.
S
o
,
d
ete
r
m
in
atio
n
o
f
co
r
r
ec
t
f
u
n
ctio
n
o
f
th
e
s
ig
n
al
p
r
o
d
u
cin
g
s
y
s
tem
is
y
et
to
b
e
d
is
cu
s
s
ed
.
T
h
e
b
ase
o
f
f
li
ck
er
in
g
is
to
cr
ea
te
a
co
n
n
ec
tio
n
an
d
attr
ac
t
th
e
o
p
p
o
s
ite
s
ex
o
r
b
ait
to
h
u
n
t.
I
n
g
en
er
al,
f
lick
er
in
g
ca
n
b
e
u
s
ed
as
a
p
r
o
tectio
n
aler
t
m
ec
h
an
is
m
[
2
3
]
.
Acc
o
r
d
in
g
to
th
e
r
elate
d
lig
h
t
cr
iter
i
a,
th
e
lig
h
t
in
ten
s
ity
I
,
in
t
h
e
d
is
tin
ct
d
is
tan
ce
o
f
a
to
war
d
th
e
s
o
u
r
ce
lig
h
t
g
en
e
r
atio
n
,
h
as th
e
r
ev
er
s
e
r
atio
wi
th
th
e
s
q
u
ar
e
v
alu
e
o
f
th
is
d
is
tan
ce
.
I
n
th
e
o
th
er
wo
r
d
s
,
af
ter
in
cr
ea
s
in
g
d
is
tan
ce
f
r
o
m
r
eso
u
r
ce
an
d
lig
h
t
attr
ac
tio
n
b
y
t
h
e
air
,
it’s
in
ten
s
ity
g
o
es
wea
k
[2
4
,
2
5
]
.
I
n
o
r
d
er
to
co
m
p
a
r
e
o
p
tim
iza
tio
n
m
eth
o
d
s
o
f
p
ar
am
eter
s
t
h
e
s
am
e
c
o
n
d
itio
n
s
b
etwe
en
m
en
tio
n
ed
al
g
o
r
ith
m
s
h
av
e
b
ee
n
c
o
n
s
id
er
ed
.
T
ab
le
2
h
as
s
h
o
wn
t
h
e
al
g
o
r
i
th
m
r
ep
etitio
n
an
d
co
n
s
id
er
atio
n
.
As
it
is
s
h
o
wn
in
T
ab
le
3
,
th
e
co
m
p
o
n
en
ts
w
h
ich
ca
n
b
e
d
ef
in
ed
in
t
h
e
s
am
e
wa
y
,
a
r
e
in
clu
d
in
g
th
e
p
o
p
u
latio
n
s
ize
in
th
e
m
o
s
t
alg
o
r
ith
m
r
e
p
etitio
n
.
So
m
e
o
f
p
a
r
am
eter
s
also
ar
e
r
ela
ted
to
th
e
in
ter
n
al
s
ettin
g
o
f
alg
o
r
ith
m
s
an
d
th
e
y
ar
e
n
o
t
co
m
m
o
n
b
etwe
en
alg
o
r
ith
m
s
.
T
h
ese
p
ar
a
m
eter
s
h
a
v
e
b
ee
n
d
eter
m
in
ed
an
d
s
et
b
y
th
e
h
elp
o
f
v
al
u
es in
th
e
r
esear
ch
a
r
ticles o
r
th
ey
s
et
ex
p
er
im
en
tally
wh
ich
h
av
e
s
h
o
wn
T
ab
le
4
.
I
n
T
ab
le
4
,
th
e
PS
O
alg
o
r
ith
m
p
ar
am
eter
s
,
σ
is
th
e
p
ar
t
icl
e
v
elo
city
,
c
1
c
2
ar
e
th
e
lo
ca
l
im
p
ac
t
in
d
ex
es.
W
ith
atten
tio
n
to
th
e
lo
ca
l
p
ar
ticle
an
d
th
e
b
est
m
ajo
r
p
ar
ticle,
w
is
th
e
ad
d
ed
weig
h
t
in
d
ex
in
th
e
r
elatio
n
o
f
s
p
ee
d
u
p
d
ate.
I
n
th
e
GA
alg
o
r
ith
m
s
,
p
c
is
th
e
p
o
s
s
ib
ilit
y
o
f
cr
o
s
s
in
g
,
p
m
is
th
e
p
o
s
s
ib
ilit
y
o
f
ch
an
g
in
g
in
b
it,
M
u
is
th
e
m
u
tati
o
n
in
d
ex
.
T
h
e
p
ar
am
eter
s
o
f
FA
s
,
I
0
is
th
e
lig
h
t
s
o
u
r
ce
in
ten
s
ity
an
d
γ
i
s
th
e
lig
h
t
ab
s
o
r
p
tio
n
in
d
e
x
.
β
0
is
th
e
eq
u
iv
alen
t
o
f
ea
ch
f
ir
ef
ly
’
s
attr
ac
tio
n
r
ate,
α
is
th
e
r
an
d
o
m
p
ar
am
eter
s
,
an
d
δ
is
th
e
u
n
if
o
r
m
d
is
tr
ib
u
t
io
n
ch
an
g
in
g
r
a
n
g
e.
T
ab
le
3
.
T
h
e
v
alu
es o
f
f
ix
ed
p
ar
am
eter
s
f
o
r
test
in
g
th
r
ee
alg
o
r
ith
m
s
N
a
me
A
mo
u
n
t
Th
e
si
z
e
o
f
t
h
e
i
n
i
t
i
a
l
p
o
p
u
l
a
t
i
o
n
50
M
o
s
t
r
e
p
l
i
c
a
t
e
d
a
l
g
o
r
i
t
h
ms
30
T
ab
le
4
.
Sp
ec
if
ic
p
ar
am
eter
s
v
alu
es f
o
r
s
ettin
g
ea
ch
o
f
th
e
th
r
ee
alg
o
r
ith
m
s
A
l
g
o
r
i
t
h
m
PSO
GA
FA
Th
e
p
a
r
a
m
e
t
e
r
s
o
f
e
a
c
h
a
l
g
o
r
i
t
h
m
a
n
d
t
h
e
i
r
v
a
l
u
e
s
w
1
P
C
0
.
5
γ
1
c
1
2
P
M
0
.
3
β
0
2
c
2
2
δ
0
.
0
5
α
0
.
2
σ
0
.
1
M
u
0
.
5
δ
0
.
0
5
Evaluation Warning : The document was created with Spire.PDF for Python.
I
n
t
J
R
o
b
&
A
u
to
m
I
SS
N:
2089
-
4
8
5
6
Th
e
q
u
a
d
r
o
t
o
r
d
yn
a
mic
mo
d
elin
g
a
n
d
s
tu
d
y
o
f m
eta
-
h
eu
r
is
tic
a
lg
o
r
ith
ms
... (
A
ta
J
a
h
a
n
g
ir
Mo
s
h
a
ye
d
)
263
4.
SI
M
UL
A
T
I
O
N
Af
ter
th
e
m
o
d
elin
g
,
th
e
Q
u
ad
r
o
to
r
th
e
f
ir
s
t
s
tep
o
f
th
is
r
e
s
ea
r
ch
is
to
test
th
e
m
o
d
el
p
e
r
f
o
r
m
a
n
ce
with
th
r
ee
Me
ta
-
Heu
r
is
tic
Alg
o
r
ith
m
s
o
f
PS
O,
GA
,
an
d
FA t
o
d
eter
m
in
e
t
h
e
PID
co
n
tr
o
ller
p
ar
am
eter
s
.
4
.
1
.
P
SO
a
lg
o
rit
hm
I
n
o
r
d
er
to
d
eter
m
in
e
PID
co
n
tr
o
ller
in
d
ex
,
o
p
tim
ized
b
y
PSO
alg
o
r
ith
m
,
to
co
n
tr
o
l
p
itch
,
r
o
ll,
an
d
y
aw
f
o
r
s
tep
f
u
n
ctio
n
th
e
alg
o
r
ith
m
o
p
tim
izatio
n
p
r
o
ce
s
s
h
a
s
s
h
o
wn
in
Fig
u
r
e
s
2
(a
)‒
(
c)
.
I
t
is
n
o
ted
th
at
with
r
esp
ec
t
to
th
e
d
ir
ec
t
in
f
er
n
al
r
elatio
n
o
f
E
u
ler
a
n
g
les
th
e
b
est
co
s
t
i
s
r
ec
o
g
n
izab
le
in
ea
ch
f
ig
u
r
e.
As
it
is
s
h
o
wn
in
Fig
u
r
e
2
,
th
e
alg
o
r
ith
m
in
th
e
co
n
tr
o
l
o
f
r
o
ll,
p
itch
,
an
d
y
aw
a
n
g
les
h
as
p
r
o
p
er
f
u
n
ctio
n
an
d
o
v
er
s
h
o
o
t
r
ate,
s
tead
y
s
tate
er
r
o
r
,
s
ettlin
g
tim
e,
h
av
e
s
h
o
wn
lo
w
in
cr
ea
s
in
g
tim
e
an
d
th
e
alg
o
r
ith
m
h
as
r
ea
ch
ed
t
o
d
am
p
in
g
in
2
4
r
ep
etitio
n
s
.
W
ith
atten
tio
n
to
Fig
u
r
e
2
,
th
e
PID
in
d
ex
o
f
p
ar
ticles
s
war
m
o
p
tim
iz
atio
n
h
as
s
h
o
wn
in
T
ab
le
5
.
W
ith
r
esp
ec
ted
to
th
e
c
o
n
tr
o
ller
p
er
f
o
r
m
a
n
ce
p
r
esen
t
ed
in
Fig
u
r
e
2
,
th
e
f
u
n
ctio
n
al
c
r
iter
ia
o
b
tain
e
d
f
r
o
m
th
e
PS
O
alg
o
r
ith
m
h
a
v
e
s
h
o
wn
in
T
ab
le
6
.
(
a)
(
b
)
(
c)
(
d
)
Fig
u
r
e
2
.
PID
-
PS
O:
(
a)
p
itch
,
(
b
)
r
o
ll,
(
c)
y
aw,
an
d
(
d
)
b
est co
s
t
T
ab
le
5
.
PID
-
PS
O
co
n
tr
o
ller
r
esp
o
n
s
e
co
ef
f
icien
ts
to
s
tack
f
u
n
ctio
n
i
n
p
u
t
No
P
a
r
a
me
t
e
r
K
I
K
P
K
D
1
P
i
t
c
h
0
.
5
4
9
2
6
9
7
0
.
7
0
6
5
9
9
9
5
3
.
7
8
8
5
1
2
R
o
l
l
0
.
6
3
6
7
2
6
5
0
.
9
6
0
5
0
9
4
5
3
.
8
5
2
7
4
3
Y
a
w
0
.
6
2
6
4
0
4
2
.
8
3
4
0
6
7
9
.
4
0
8
6
9
T
ab
le
6
.
PID
-
PS
O
co
n
tr
o
ller
p
er
f
o
r
m
a
n
ce
m
etr
ics
No
P
a
r
a
me
t
e
r
R
i
se
t
i
m
e
S
e
t
t
l
i
n
g
t
i
m
e
S
t
e
a
d
y
st
a
t
e
O
v
e
r
sh
o
o
t
1
P
i
t
c
h
0
.
0
7
0
0
0
.
0
6
7
9
0
.
0
0
0
5
1
.
8
1
8
6
2
R
o
l
l
0
.
1
0
0
0
0
.
0
9
9
4
0
.
0
0
0
0
0
.
0
7
2
1
3
Y
a
w
0
.
0
9
0
0
0
.
0
8
5
8
0
.
0
0
1
1
1
.
6
3
3
0
I
n
T
ab
le
6
,
th
e
m
ax
im
u
m
v
alu
e
o
f
o
v
er
s
h
o
o
t
cr
iter
ia
is
r
elate
d
to
th
e
p
itch
p
ar
a
m
eter
an
d
th
e
m
in
im
u
m
v
alu
e
is
r
elate
d
to
th
e
r
o
ll.
I
n
th
e
s
tead
y
s
tate
er
r
o
r
cr
iter
ia,
alm
o
s
t
all
o
f
th
em
ar
e
ze
r
o
.
I
n
th
e
s
ettlin
g
tim
e,
th
e
m
ax
im
u
m
v
alu
e
is
r
elate
d
to
th
e
p
itc
h
p
ar
am
eter
a
n
d
th
e
m
i
n
im
u
m
v
alu
e
is
r
elate
d
to
th
e
r
o
ll
an
d
in
cr
ea
s
in
g
tim
e,
th
e
m
ax
im
u
m
v
al
u
e
is
r
elate
d
to
th
e
r
o
ll
an
d
t
h
e
m
in
im
u
m
v
alu
e
b
elo
n
g
s
to
th
e
p
itch
.
Evaluation Warning : The document was created with Spire.PDF for Python.
I
SS
N
:
2
0
8
9
-
4
8
5
6
I
n
t
J
R
o
b
&
A
u
to
m
,
Vo
l.
9
,
N
o
.
4
,
Dec
em
b
er
2
0
2
0
:
x
x
–
xx
264
4
.
2
.
GA
T
h
e
r
esu
lts
o
f
PID
co
n
tr
o
lle
r
o
p
tim
izin
g
b
y
GA
in
co
n
tr
o
lli
n
g
p
itch
,
r
o
ll,
an
d
y
aw
an
g
les,
to
th
e
s
tep
f
u
n
ctio
n
,
h
av
e
s
h
o
wn
in
Fig
u
r
e
s
3
(
a)
‒
(
c
)
an
d
th
e
alg
o
r
ith
m
o
p
tim
izatio
n
p
r
o
ce
s
s
h
as
s
h
o
wn
in
Fig
u
r
e
3
(
d
)
.
As
it
h
as
b
ee
n
s
h
o
wn
in
F
ig
u
r
e
3
,
th
e
alg
o
r
ith
m
h
as
n
o
g
o
o
d
o
p
er
atio
n
in
to
co
n
tr
o
l
o
f
r
o
ll,
p
itch
,
an
d
y
aw
an
g
les.
So
,
it c
o
u
ld
n
o
t tr
ac
k
t
h
e
f
u
n
ctio
n
p
r
o
p
er
l
y
.
(
a)
(
b
)
(
c)
(
d
)
Fig
u
r
e
3
.
PID
-
GA:
(
a)
p
itch
,
(
b
)
r
o
ll,
(
c)
y
aw,
an
d
(
d
)
b
est co
s
t
T
h
e
o
v
er
s
h
o
o
t
r
ate,
s
tead
y
s
t
ate
er
r
o
r
an
d
s
ettlin
g
tim
e
,
s
h
o
ws
th
e
v
er
y
lo
w
i
n
cr
ea
s
in
g
tim
e
a
n
d
th
e
alg
o
r
ith
m
in
2
5
r
ep
etitio
n
s
h
as
r
ea
ch
ed
to
d
am
p
in
g
,
with
atten
tio
n
to
f
ig
u
r
es
th
e
PID
in
d
ex
es
o
f
GA
an
d
co
n
tr
o
ller
r
esp
o
n
s
e
a
h
a
v
e
s
h
o
wn
in
T
ab
les
7
a
n
d
8
.
I
n
T
a
b
le
8
,
th
e
m
a
x
im
u
m
v
alu
e
o
f
o
v
er
s
h
o
o
t
c
r
iter
ia
is
r
elate
d
to
y
aw
p
a
r
am
eter
a
n
d
th
e
m
in
im
u
m
v
alu
e
is
r
elate
d
to
th
e
r
o
ll.
I
n
s
tead
y
s
tate
er
r
o
r
cr
iter
ia
alm
o
s
t
all
o
f
th
e
v
alu
es
ar
e
ze
r
o
.
I
n
s
ettlin
g
tim
e,
th
e
m
ax
im
u
m
v
alu
e
b
elo
n
g
s
th
e
y
aw
p
ar
a
m
eter
an
d
th
e
m
in
im
u
m
v
alu
e
b
elo
n
g
s
th
e
p
itch
a
n
d
in
r
is
e
tim
e,
th
e
m
ax
im
u
m
v
alu
e
co
n
n
ec
ted
to
th
e
p
it
ch
p
ar
am
eter
a
n
d
th
e
m
in
im
u
m
v
alu
e
co
n
n
ec
ted
to
th
e
r
o
ll.
T
ab
le
7
.
T
h
e
PID
-
GA
co
n
tr
o
ller
co
ef
f
icien
ts
No
P
a
r
a
me
t
e
r
K
I
K
P
K
D
1
P
i
t
c
h
0
.
6
2
1
7
9
6
.
9
5
5
8
5
3
8
.
0
3
6
1
2
R
o
l
l
0
.
2
9
8
1
8
1
0
.
6
4
5
6
4
2
4
2
.
0
0
1
7
3
Y
a
w
0
.
5
0
9
0
0
5
7
.
2
3
9
4
6
4
7
.
6
4
0
7
T
ab
le
8
.
T
h
e
PID
-
GA
co
n
tr
o
ller
r
esp
o
n
s
e
co
ef
f
icien
ts
to
th
e
s
tep
in
p
u
t
No
P
a
r
a
me
t
e
r
R
i
se
t
i
m
e
S
e
t
t
l
i
n
g
t
i
m
e
S
t
e
a
d
y
st
a
t
e
O
v
e
r
sh
o
o
t
1
P
i
t
c
h
0
.
7
4
0
0
0
.
1
1
1
8
0
.
0
0
0
0
2
6
.
3
0
0
5
2
R
o
l
l
0
.
1
0
0
0
0
.
3
3
8
9
0
.
0
0
0
0
4
.
4
8
4
2
3
Y
a
w
0
.
2
9
0
0
1
.
1
2
3
8
0
.
0
0
1
7
2
9
.
6
0
8
9
4
.
3
.
FA
T
h
e
r
esu
lts
o
f
PID
co
n
tr
o
ller
,
wh
ich
o
p
tim
ized
b
y
FA
in
co
n
tr
o
llin
g
y
aw,
r
o
ll
an
d
p
itch
an
g
les
to
th
e
s
tep
f
u
n
ctio
n
i
n
p
u
t
a
r
e
p
r
e
s
en
ted
in
Fig
u
r
e
s
4
(
a)
‒
(
c)
an
d
th
e
alg
o
r
ith
m
o
p
tim
izatio
n
p
r
o
ce
s
s
h
as
s
h
o
wn
in
Evaluation Warning : The document was created with Spire.PDF for Python.
I
n
t
J
R
o
b
&
A
u
to
m
I
SS
N:
2089
-
4
8
5
6
Th
e
q
u
a
d
r
o
t
o
r
d
yn
a
mic
mo
d
elin
g
a
n
d
s
tu
d
y
o
f m
eta
-
h
eu
r
is
tic
a
lg
o
r
ith
ms
... (
A
ta
J
a
h
a
n
g
ir
Mo
s
h
a
ye
d
)
265
Fig
u
r
e
4
(
d
)
.
Acc
o
r
d
in
g
to
th
e
Fig
u
r
es
4
(
a)
,
(
b
)
,
(
c
)
th
e
PID
in
d
ex
es
o
f
FA
h
av
e
s
h
o
wn
i
n
T
ab
le
9
.
As
it
is
s
h
o
wn
in
Fig
u
r
e
4
,
th
e
alg
o
r
it
h
m
s
h
ad
a
g
o
o
d
p
er
f
o
r
m
a
n
ce
to
co
n
tr
o
l
o
f
r
o
ll,
p
itch
a
n
d
y
aw
al
s
o
o
v
er
s
h
o
o
t
r
ate,
s
tead
y
s
tate
er
r
o
r
an
d
s
ettlin
g
tim
e,
s
h
o
ws
th
e
v
er
y
lo
w
in
cr
ea
s
in
g
tim
e.
M
o
r
eo
v
er
,
alg
o
r
ith
m
in
2
7
r
ep
etitio
n
s
h
av
e
r
ea
ch
ed
to
d
am
p
i
n
g
.
T
h
e
with
atten
tio
n
to
th
e
co
n
tr
o
ller
f
u
n
ctio
n
,
i
n
F
ig
u
r
e
s
4
(
a)
‒
(
c
)
th
e
o
p
er
atin
g
p
ar
am
eter
s
o
f
th
e
F
A
h
av
e
s
h
o
wn
i
n
T
ab
le
s
9
a
n
d
1
0
.
(
a)
(
b
)
(
c)
(
d
)
Fig
u
r
e
4
.
PID
-
FA: (
a)
p
itch
,
(
b
)
r
o
ll,
(
c)
y
aw,
an
d
(
d
)
b
est co
s
t
T
ab
le
9
.
PID
-
FA c
o
n
tr
o
ller
r
e
s
p
o
n
s
e
co
ef
f
icien
ts
to
in
p
u
t o
f
t
h
e
s
tep
f
u
n
ctio
n
No
P
a
r
a
me
t
e
r
K
I
K
P
K
D
1
P
i
t
c
h
0
.
0
5
7
8
5
0
9
6
8
.
3
6
0
6
1
.
7
1
2
0
4
2
R
o
l
l
0
.
6
6
5
2
5
5
7
5
.
3
0
9
9
3
.
2
2
2
9
9
3
Y
a
w
0
.
0
1
4
2
.
6
7
4
5
6
.
0
6
7
5
7
T
ab
le
10
.
Per
f
o
r
m
a
n
ce
m
etr
ic
s
f
o
r
th
e
PID
-
FA c
o
n
tr
o
ller
No
P
a
r
a
me
t
e
r
R
i
se
t
i
m
e
S
e
t
t
l
i
n
g
t
i
m
e
S
t
e
a
d
y
st
a
t
e
O
v
e
r
sh
o
o
t
1
i
t
c
h
0
.
1
0
0
0
.
0
9
7
8
0
.
0
0
2
6
0
.
3
4
9
4
2
R
o
l
l
0
.
1
7
0
0
0
.
1
6
9
3
0
.
0
0
0
0
0
.
0
5
1
0
3
Y
a
w
0
.
3
5
0
0
0
.
3
4
5
3
0
.
0
0
5
5
1
.
0
3
2
6
I
n
T
ab
le
1
0
,
th
e
m
ax
im
u
m
v
alu
e
o
f
o
v
er
s
h
o
o
t
cr
iter
ia
b
elo
n
g
s
to
th
e
y
aw
p
a
r
a
m
eter
an
d
th
e
m
in
im
u
m
v
alu
e
r
elate
d
to
th
e
r
o
ll
p
ar
am
eter
.
I
n
s
tead
y
s
tate
er
r
o
r
,
alm
o
s
t
all
v
alu
es
a
r
e
ze
r
o
.
I
n
s
ettlin
g
tim
e,
th
e
m
ax
im
u
m
v
alu
e
is
c
o
n
n
ec
ted
th
e
y
aw
p
ar
am
eter
a
n
d
th
e
m
in
im
u
m
v
alu
e
is
co
n
n
ec
ted
th
e
p
itch
.
I
n
th
e
in
cr
ea
s
in
g
tim
e,
th
e
m
ax
i
m
u
m
v
alu
e
is
r
elate
d
to
th
e
p
itch
.
4
.
4
.
Co
m
pa
riso
ns
o
f
P
I
D
co
ntr
o
l
ler
o
ptim
iza
t
io
n
m
et
ho
ds
As
alr
ea
d
y
s
tated
,
th
e
f
iv
e
cr
it
er
ia
o
f
o
v
e
r
s
h
o
o
t,
s
tead
y
s
tate
er
r
o
r
,
s
ettlin
g
tim
e,
r
is
in
g
tim
e
ar
e
u
s
ed
to
co
m
p
ar
e
t
h
e
co
ef
f
icien
ts
o
f
PID
tu
n
in
g
v
ia
th
e
m
en
tio
n
ed
alg
o
r
ith
m
s
.
th
is
co
m
p
ar
is
o
n
ar
e
s
u
m
m
ar
ies
in
T
ab
les
11
–
13
f
o
r
p
itch
,
r
o
ll
an
d
y
aw
p
a
r
am
eter
s
.
As
it
s
h
o
wn
in
T
ab
le
1
1
,
t
h
e
co
n
tr
o
l
ler
p
er
f
o
r
m
an
ce
i
n
p
itch
p
ar
a
m
eter
,
t
h
e
PID
_
FA
,
PID
_
PS
O
alg
o
r
ith
m
s
s
h
o
w
b
etter
p
er
f
o
r
m
an
ce
in
o
v
er
s
h
o
o
t
t
h
an
t
h
e
two
Z
ieg
ler
‒
Nich
o
ls
m
eth
o
d
s
an
d
PID
_
GA.
T
h
e
v
alu
e
o
f
s
tead
y
s
tate
er
r
o
r
in
all
alg
o
r
ith
m
s
ar
e
v
er
y
lo
w
an
d
th
e
s
ettlin
g
tim
e
p
ar
am
eter
s
,
f
o
r
th
e
PID
_
PS
O
an
d
PID
_
F
A,
p
er
f
o
r
m
b
etter
in
co
m
p
ar
e
to
th
e
two
o
th
er
alg
o
r
ith
m
s
.
T
ab
le
1
2
s
h
o
ws
th
e
co
m
p
a
r
is
o
n
o
f
th
e
c
o
n
tr
o
ll
er
’
s
p
er
f
o
r
m
an
ce
in
th
e
r
o
ll
p
ar
am
eter
.
W
h
ich
it
s
h
o
ws
th
e
PID
_
FA
an
d
PID
_
PS
O
alg
o
r
ith
m
s
,
th
er
e
is
a
b
et
ter
p
er
f
o
r
m
an
ce
in
th
e
o
v
er
s
h
o
o
t
an
d
s
ettlin
g
tim
e
cr
iter
ia
h
av
e
b
etter
p
er
f
o
r
m
a
n
ce
to
war
d
s
Z
ie
g
ler
‒
Nich
o
ls
an
d
PID
_
GA.
Evaluation Warning : The document was created with Spire.PDF for Python.