Hi ngh ton quc v iu khin v T ng ho - VCCA-2011

VCCA-2011
M hnh ha ng lc hc Quadrotor
Modeling of Quadrotor dynamics
o Vn Hip, Trn Xun Diu, Phng Th Kin
Trng HVKTQS
e-Mail: xuandieuvtl@gmail.com


Tm tt
S dng cch tip cn C h nhiu vt, bi bo trnh
by kt qu xy dng m hnh ng lc ca m hnh
my bay ln thng dng Quadrotor, nghin cu cc
ng x ca m hnh khi thay i cc thng s u
vo. Kt qu kho st s l c s cho vic thit lp
thut ton v thit k h thng c kh v iu khin
my bay.

Abstract: In this paper, the authors used the
aprroach with Dynamics of Body systems to estalish
the mathematical model of a helicopter called
Quadrotor and investigated the behaviours of the
model by varying the input parameters. The results
from modelling are the fundament to estalish the
control algorithms and to design the mechanical parts.

K hiu
K hiu n v ngha
H, A
t
, J
T
,
J
R
, I, M

cc ma trn ca m hnh
e, e vc-t vn tc gc v ton
t sng vc-t vn tc gc
T, H ng nng v th nng
F
u
, F, Q*,
t

cc vc-t lc v m men

Ch vit tt
UAV thit b bay khng ngi li

1. t vn
Trong nhng nm gn y, cc thit b bay khng
ngi li (UAV) c c bit quan tm do nhu cu
thc t trong hot ng qun s nh do thm, kim tra
a hnh a vt...cng nh trong hot ng dn s
nh chp nh, gim st, quan trc... C rt nhiu loi
UAV c pht trin nh kinh kh cu, my bay
cnh c nh, trc thng r-to n, quadrotor,... Mi
loi UAV u c nhng u im v nhng hn ch
ca n. Quadrotor l loi my bay trc thng c 4
cnh qut vi nhng u im ni bt l ct cnh v h
cnh thng ng d dng, kch thc nh gn, gi n
nh mt v tr trong khng gian tt, c kt cu c
kh n gin v kh nng ti ln. Nghin cu m hnh
ng lc hc ca quadrotor l bc quan trng ban
u t a ra thut ton iu khin cng nh
thit k ch to.

2. M hnh ha ng lc hc
2.1 Cc c tnh Quadrotor
Quadrotor c 2 cp cnh qut gn vi 2 cp ng c
tng ng c b tr nh hnh 1. Cp ng c
trc-sau c iu khin quay theo chiu kim ng
h, trong khi cp ng c cn li c iu khin
quay ngc chiu kim ng h, cch b tr ny s lm
trit tiu nh hng ca cc m men cnh qut v cc
hiu ng kh ng hc.

H. 1 Cu trc ca h thng iu khin s


H. 2 (a) thay i gc pitch; (b) thay i gc roll; (c)
thay i gc yaw

Quadrotor hon ton khng cn a lc iu chnh
gc cnh nh trc thng r-to n. Ch cn iu khin
tc ca 4 ng c, ta hon ton c th c c cc
gc roll, pitch, yaw nh mong mun.
Nguyn l iu khin ca quadrotor rt n gin, c
th trnh by ngn gn nh sau: gi s lc u 4 rotor
306
Hi ngh ton quc v iu khin v T ng ho - VCCA-2011

VCCA-2011
c cng tc (c cng lng ga), thay i gc
pitch ta tng (gim) tc ca ng c trc ng
thi gim (tng) tc ca ng c sau; thay i gc
roll cng lm tng t; thay i gc yaw ta tng
(gim) tc ca cp ng c trc-sau v gim
(tng) tc ca cp ng c tri-phi. gi n
nh cao, ta nn gi tng lng ga khng i. S
quan h gia cc lc khi thay i cc gc c th
hin hnh 2 [3].
2.2 M hnh ng lc hc
Ta coi quadrotor l mt vt rn chu mt lc (hp lc
ca cc lc to ra bi cc ng c) v 3 m-men lm
quadrotor quay theo cc gc roll, pitch, yaw.
nghin cu ng lc hc ca m hnh quadrotor y
ta s dng phng trnh Lagrange II.

H. 3 Cc h quy chu ca quadrotor
Chn h quy chiu qun tnh Fi v h quy chiu ng
gn vi vt Fb c th hin hnh 3 [3]. Gc ta
ca h quy chiu Fb c gn vi trng tm ca
quadrotor.
Cc ta suy rng i vi quadrotor c chn nh
sau:
6 5 4 3 2 1
, , , , , q q q q z q y q x = | = u = = = =
,
hay
1 2 3 4 5 6
[ ]
T
q q q q q q = q
.
y, (x, y, z) l ta ca trng tm C ca m hnh
quadrotor trong h quy chiu qun tnh v , u, | l
ba gc yaw, pitch v roll th hin hng ca
quadrotor.
Ta c th chuyn h ta Fi sang h ta Fb bng
cc php bin i sau:
- Tnh tin Fi dc theo trc z0 mt on z, c h
ta hin thi F1;
- Tnh tin F1 dc theo trc x1 mt on x, c ta
hin thi F2;
- Tnh tin F2 dc theo trc y2 mt on y, c ta
hin thi F3;
- Quay F3 quanh trc x3 mt gc |, ta c h ta
hin thi F4;
- Quay F4 quanh trc y4 mt gc u, ta c h ta
hin thi F5;
- Quay F5 quanh trc z5 mt gc, ta c h ta
ng gn vi vt Fb.
Ma trn ca php bin i t h quy chiu Fi sang h
quy chiu Fb s l:


H
0 0 0 1
(
(
+ +
(
=
( + +
(

c c s s s x
c s s s c s s s c c c s y
c s c s s s s c c s c c z
u u u
u | | u | | u |
u | | u | | u |
. (1)
Theo [2], t ma trn chuyn i ta nhn c ma trn
cosin ch hng khi thc hin php chuyn i lin
tip t h quy chiu Fi sang h quy chiu Fb l:

t
c c s c s
c s s s c s s s c c c s
c s c s s s s c c s c c
u u u
u | | u | | u |
u | | u | | u |
(
(
= + +
(
( + +

A
(2)
v ta trng tm l:
| |
1 2 3
T
q q q =
C
r
(3)

o hm biu thc trn trong h quy chiu qun tnh
ta c vn tc ca khi tm

| |
1 2 3
T
C C
q q q = = v r
(4)
T ma trn cosin ch hng, theo [1] ta d dng xc
nh c ton t sng ca vc-t vn tc gc trong
h quy chiu qun tnh:

4 5 6 5 6 4 5 6 5 6
4 5 6 5 6 6 4 5
4 5 6 5 6 6 4 5
0
0
0
T
t t
q c c q s q c s q c
q c c q s q q s
q c s q c q q s
+ (
(
= +
(
( +

= A A e
(5)
T ton t sng (5) ta xc nh c vn tc gc
ca quadrotor trong h quy chiu qun tnh:

| |
6 4 5 4 5 6 5 6 4 5 6 5 6
= + + +
T
q q s q c s q c q c c q s e
(6)
Phng trnh Lagrange loi II i vi vt rn (m
hnh quadrotor) l

Q
q q q
*
| | | | | | c c c
= +
| | |
c c c
\ . \ . \ .
T T T
d T T
dt
H
(7)
trong :
T l ng nng ca vt rn
H - l th nng trng lc ca vt rn
Q* l vc-t lc suy rng.
ng nng ca quadrotor c xc nh theo cng
thc
q M(q)q
1
T
2
=
T
trong :
M(q) l ma trn khi lng suy rng,
T T
T T R R
m M(q) = J J +J IJ
.
J
T
l Jacobi tnh tin, c xc nh theo cng thc

1 0 0 0 0 0
0 1 0 0 0 0
0 0 1 0 0 0
T
(
c
(
= =
(
c
(

C
r
J
q
(8)
J
R
l Jacobi quay, c xc nh theo cng thc

5
6 5 6
6 5 6
0 0 0 0 1
0 0 0 0
0 0 0 0
R
s
s c c
c c s
(
c
(
= =
(
c
(

J
q
e
(9)
307
Hi ngh ton quc v iu khin v T ng ho - VCCA-2011

VCCA-2011
I l Ten-x qun tnh khi ca quadrotor i vi h
quy chiu qun tnh, theo [1] n c xc nh theo
cng thc
I AI A =
T
b
(10)
I
b
l Ten-x qun tnh khi ca quadrotor i vi h
quy chiu ng, do quadrotor c hnh dng i xng
trc v cc trc ta c chn trng vi trc i
xng, nn theo [1] ta c

0 0
0 0
0 0
x
b y
z
I
I
I
(
(
=
(
(

I
(11)
Thay cc biu thc (2) v (11) vo biu thc (10) ta
c Ten-x qun tnh khi ca quadrotor i vi h
quy chiu qun tnh.
Th nng trng lc c xc nh theo cng thc
0
T
m H =
C
g r
(12)
trong :
g
0
l vc-t gia tc trng trng,
0
[0 0 ]
T T
g = g
Thay (3) v vc-t gia tc trng trng vo biu thc
(12) ta c
H = mgz
(13)
Vc-t lc suy rng gm 2 thnh phn chnh, l cc
lc v cc m men nh c a ra trn. Do
Q F
*
[ ] =
T T T
t
(14)
Lc to ra t 4 ng c xt trong h quy chu Fb l
[0 0 ]
T
u
f = F
.
y,
f l lc tng hp sinh ra t cc ng c
1 2 3 4
f f f f f = + + + , f
i
= k
f
e
2
i
, i = 1...4
vi k
f
l hng s dng ph thuc vo tng loi cnh
qut, e
i
l vn tc gc ca ng c th i.
Do , F
u
trong h quy chiu Fi c xc nh theo
cng thc
F = A
t
F
f
=
| |
5 6 5 6 5
T
s f s c f c c f
(15)
Vc-t m men t l
[ ] ,
T
u |
t t t = t


trong

1 2 3 4
,
M M M M
t t t t t = + + +

2 4
( ) , u u l
u
t =
3 1
( ) , u u l
|
t =
y,
Mi
t l cc m men c to ra t cc ng c
Mi, l l khong cch t cc ng c ti trng tm ca
quadrotor. Ch rng t
Mi
= k
M
e
2
i
, i = 1...4, k
M
l cc
hng s dng. t k = k
M
/k
f
, t ta d dng c t
Mi

= k f
i
.
S dng phn mm Maple xy dng cc cng thc
v lp h phng trnh vi phn. T phng trnh
Lagrange II (2.7) ta c h 6 phng trnh vi phn
sau:

f s q m
5 1
=
(16)
f c s q m
5 6 2
= (17)
mg f c c q m =
5 6 3
(18)
) ( ) 2 2 (
4 4 4 4
2
5 5 5
2
4 5
2
4 5 5 5 6 5 6 4 y x y z y x x z z
I s c I s c q I c I c I c c I c c I c q q I s q I q + + + + + + +

t = + ) (
4 4
2
5 4 4
2
5
2
6 y x
I c s c I c s c q
(19)
) 2 2 ( ) ( 2 ) (
2
4 5 5
2
4 5 5 5 6 4 4 4 4 4 5 4
2
4
2
4 5 y y x z x x y x y x
I c c I c I c c I c I c q q I c s I c s q q I c c I I q + + + + + +


u
t = + + + ) ( ) (
4 4 5 4 4 5 6 5 5
2
4 5 5 5 5 5
2
4 5
2
6 y z y y x
I c s c Ix s c c q I c s I c s c I s c I s c c q
(20)
x x y z x y y z z
I c c q q I s c c I s c c q I s q I c c I c c I c I c I q
2
4 5 5 4 4 4 5 4 4 5 5 5 4
2
4
2
5
2
4
2
5
2
5
2
5 6
2 ( ) ( ) ( + + + + + +


) 2 ( 2 ) 5 2 5
5 5 5
2
4 5 5 5
2
4 5 5 6 5 5 5
2
4 z x y y x z y y
I c s I s c c I s c c I s c q q I c I c I c c I c + + + + +


|
t = + + + ) ( ) ( 2
4 4 5 4 4 5
2
5 4 4
2
5 4 4
2
5 6 4 x y y x
I s c s I s c s q I c s c I s c c q q
(21)
2.3 M phng ng lc hc quadrotor
Trong mc ny, ta s s dng Matlab-Simulink
gii h phng trnh gm 6 phng trnh vi phn t
(16) n (23). Kt qu ca php gii m t ng x ca
h khi chu cc tc ng.
M phng cn phi c thc hin khng nh tnh
ng n cc gi thit ca chng ta khi thit lp m
hnh ton ca Quadrotor. Trn c s tnh ng n
ca m hnh ton, khi thay i cc tc ng vo h
(cc thng s ca ng c) ta s thu c cc kt qu
ng x ca h thng.

Thng s K
hiu
Gi tr n v
Khi lng ca
Quadrotor
m 0,8 kg
M men qun tnh theo
trc x
I
x
0,0115 kg.m
2
M men qun tnh theo
trc y
I
y
0,015 kg.m
2

M men qun tnh theo
trc z
I
z
0,0228 kg.m
2

Gia tc trng trng g 9,81 m/s
2
Khong cch t tm
Quadrotor n tm cc
ng c
l 0,25 m
H s t l gia m
men ng c v lc
ng c
k 0,001 Nm/N

Cc gi tr u ca tt c cc thng s trng thi u
c gi tr bng khng.
308
Hi ngh ton quc v iu khin v T ng ho - VCCA-2011


H. 4 M hnh gii h phng trnh vi phn trong Simulink

M hnh hnh 4 m phng nguyn l iu khin
cc thng s u vo (lc cc ng c f
i
) t
c cc thay i ca cc thng s u ra (x, y, z, |,
u, ) nh trnh by phn 2. Khi tnh ton lc
ng c hnh 4 th hin rng buc nh thay
i cao, thay i cc gc roll, pitch, yaw..., khi
ny c u ra l cc lc m cc ng c cn to ra.
Khi tnh ton lc v m men cho h phng trnh
vi phn tnh ton, a ra hp lc v m men
gm f, t

, t
u
, t
|
. Khi s-function (quadrotor) c
lp gii h phng trnh da trn cc thng s
c cung cp t khi tnh ton lc v m men cho
h phng trnh vi phn. u ra ca khi s-function
l cc thng s ng hc quan tm ca quadrotor.
Cc kt qu m phng gm:

M phng 1: Khi cc ng c cha cung cp lc.


H. 5 dch chuyn theo trc x, y, z theo thi gian v
cc gc |, u, theo thi gian
Kt qu ny cho thy khi cc ng c khng cung
cp lc th quadrotor ri t do. Kt qu ny th hin
ng x ng n ca m hnh, kt qu ny cng c
th kim nghim thng qua cng thc n gin h =
0,5gt
2
. gi c cao gc ta (z = 0) th
mi ng c cn phi cung cp cho h mt lc l
m.g/4 = 1,962N.
M phng 2: Quadrotor bay ln, mi ng c phi
cung cp cho h mt lc ln hn 1,962N. y ta
m phng ng x ca h khi cc ng c cung cp
cng mt lc 3N.


H. 6 dch chuyn theo trc x, y, z theo thi gian v
cc gc |, u, theo thi gian
M phng 3: Quadrotor bay ln ng thi quay
gc roll (|), ng c trc v sau cung cp mt lc
nh nhau ( y l 3N), ng c bn tri cung cp
mt lc ln hn (3,2N) trong khi ng c bn phi
cung cp mt lc nh hn (2,8N).
Ta thy khi iu khin ng thi cao v gc roll,
nu lc ca cc ng c khng ln th quadrotor
s mt cao, s lao xung v cng gy ra dch
chuyn theo phng y (hnh 7).

H. 7 dch chuyn theo trc x, y, z theo thi gian v
cc gc |, u, theo thi gian

M phng 4: Quadrotor bay ln ng thi quay
gc pitch (u), ng c tri v phi cung cp mt lc
nh nhau ( y l 3N), ng c trc cung cp
mt lc ln hn (3,2N) trong khi ng c sau cung
cp mt lc nh hn (2,8N).
309
Hi ngh ton quc v iu khin v T ng ho - VCCA-2011

VCCA-2011

H. 8 dch chuyn theo trc x, y, z theo thi gian v
cc |, u, theo thi gian
Tng t nh vi m phng 3, nhng y s xy
ra dch chuyn theo phng x.

M phng 5: Quadrotor bay ln ng thi quay
gc yaw (), ng c tri v ng c phi cung cp
cng mt lc ln ( y l 3,2N), ng c trc v
sau cung cp cng mt lc nh ( y l 2,8N).


H. 9 dch chuyn theo trc x, y, z theo thi gian v
cc |, u, theo thi gian
y khng thy c s nh hng ca iu khin
gc yaw n thng s u ra cn li, nhng gc
yaw thay i chm hn rt nhiu so vi thay i
ca gc roll, pitch trong cng mt iu kin, iu
ny cho thy thc t iu khin gc yaw cng
tng i kh.
3.
Kt lun
Vi cch tip cn c hc h nhiu vt, trong bi bo
ny, nhm tc gi trnh by kt qu nghin cu
m hnh ng lc hc ca quadrotor. Nhm tc gi
s dng phng php Phng trnh Lagrange II
lp h phng trnh ng lc hc quadrotor khc
hon ton so vi cc bi bo trc y s dng
Phng php Newton-Euler. Phng trnh
Lagrange II lun a ra s lng phng trnh ti
thiu (bng s bc t do ca h), cch thnh lp h
phng trnh cng n gin hn c bit phng
php ny rt tng thch vi cc phn mm tnh
ton nh Maple v Matlab-Simulink.
Cc ng x ca m hnh ng lc hc c m
phng phn 4 chng t tnh ng n ca m
hnh ton v ph hp vi ng x trong thc t. M
hnh ny l c s thit k h iu khin v tnh
ton kt cu c kh ca quadrotor - nhng ni dung
nghin cu tip theo ca nhm tc gi.


Ti liu tham kho
[1] Nguyn Vn Khang: ng lc hc h nhiu
vt. NXB Khoa hc v K thut, H Ni
2007.
[2] o Vn Hip: K thut robot. NXB Khoa
hc v K thut (in ln 2), H Ni 2004
[3] Pedro Castillo, Rogelio Lozano and Alejandro
E.Dzul: Modelling and Control of Mini-
Flying Machines, Springer, Compigne,
France 2004.
[4] Jens Wittenburg: Dynamics of Body systems
(Second Edition), Springer, Karlsruhe 2007.

Trn Xun Diu sinh nm
1983. Anh tt nghip i hc
ti Hc vin k thut qun s
nm 2007. Sau khi ra trng
anh l nghin cu vin ca
Vin tn la Vin k thut
qun s. Hin nay anh ang
theo hc cao hc chuyn ngnh
c in t ti Hc vin k thut qun s. Hng
nghin cu chnh l ng lc hc v iu khin
robot, iu khin my li tn la.

Phng Th Kin sinh nm
1982. Anh tt nghip i
hc ti Hc vin k thut
qun s nm 2007. Sau khi
ra trng anh l tr l k
thut cng ty thng tin M3-
Tp on vin thng qun
i Viettel. Hin nay anh
ang theo hc cao hc
chuyn ngnh c in t ti
Hc vin k thut qun s. Hng nghin cu
chnh l ng lc hc v iu khin robot.

310