MC LC

M U..............................................................................................................1
CHNG 1. TNG QUAN.................................................................................3
1.1.

Mc tiu chung.......................................................................................3

1.2.

Mc tiu ring........................................................................................3

1.2.1. Tnh hnh nghin cu trong v ngoi nc........................................3

1.2.2. Ni dung nghin cu ca ti.......................................................10
CHNG 2. C S L THUYT....................................................................11
2.1.

C s l thuyt v m hnh ton ca Quadcopter.................................11

2.1.1. L thuyt iu khin Quadcopter....................................................11

2.1.2. C s l thuyt m hnh bay Quadcopter........................................12
2.2.

H thng dn ng qun tnh INS......................................................15

2.2.1. Khi nim........................................................................................15

2.2.2. Phn loi h thng dn ng qun tnh.........................................15
2.2.3. Cc sai s hay gp trong h thng INS............................................17
2.3.

B iu khin PID................................................................................17

2.3.1. H thng iu khin vng kn c bn..............................................17

2.3.2. Thut ton PID kinh in................................................................18
2.3.3. Gii thut PID s...............................................................................20
CHNG 3. THIT K V CH TO QUADCOPTER.................................22
3.1.

S khi ca h thng.......................................................................22

3.2. Tnh ton, chn khung v ng c cho my bay......................................23

3.3. Thit k h thng iu khin....................................................................25
3.4.

Xy dng chng trnh iu khin.......................................................37

3.4.1. Chng trnh khi tay iu khin....................................................37

3.4.2. Chng trnh khi giao tip my tnh..............................................38
3.4.3. Chng trnh khi iu khin trung tm.........................................39

CHNG 4. THC NGHIM V NH GI...............................................43

4.1.

Hnh nh phn cng ca c h thng....................................................43

4.2.

Th nghim khi giao tip my tnh.....................................................43

4.3.

H thng th nghim kh nng thng bng ca Quadcopter.................45

4.4.

Th nghim b iu khin PID............................................................46

KT LUN.........................................................................................................47
PH LC............................................................................................................ 48
TI LIU THAM KHO...................................................................................60

M U

Tnh cp thit ca ti
Hin nay, trong mt s lnh vc dn dng, qun s hay khoa hc v tr con
ngi dn thay th cc phng tin bay c ngi li bng cc thit b bay khng ngi
li, bi cc tnh nng u vit nh c kh nng hot ng t ng hoc l iu khin t
xa, c kh nng hot ng nhng ni m con ngi kh tip cn c, ngoi ra cn
c ng dng rng ri trong mt s lnh vc nh quan st ni la, kim tra mi
trng, phc v lm phim
Quadcopter l mt loi my bay m hnh vi c tnh n nh, d dng iu
khin, v ang l mt ti nng trong nghin cu, ch to my bay khng ngi
li. Vi sc hp dn v tnh thc t ca vn cng nim am m vi ti ca bn
than nn trong phm vi n tt nghip ca sinh vin ngnh C in t, c s
hng dn v gip ca ThS. Hong Th Huynh em quyt nh chn ti : Pht
trin h thng iu khin cho my bay m hnh Quadcopter lm n tt nghip.
ngha khoa hc v thc tin ca ti
Quadcopter l mt ti kh, i hi kin thc tng hp nhiu lnh vc nh:
c kh thit k, thit b bay, kh ng hc, ng lc hc, x l tn hiu s, mch in
t, vi iu khin, giao tip my tnh, truyn thng v tuyn, m hnh ha, m phng,
lp trnh iu khin, lp trnh giao din. V ngha khoa hc, h thng iu khin
Quadcopter c th dng trong cc phng nghin cu, th nghim v cm bin gc
nghing, h thng dn ng INS, th nghim v truyn dn d liu khng dy. V
ngha thc t, Quadcopter c ng dng rt nhiu trong i sng nh trong qun s,
cnh bo chy, camera quan st, phc v trong gii tr, lm phm
Ni dung nghin cu ca ti
Ni dung nghin cu trong khun kh n l:
-

Tm hiu l thuyt : c s l thuyt iu khin Quadcopter, cc chun giao

tip SPI, I2C, h thng nh v qun tnh INS, l thuyt iu khin PID.
- Tnh ton, thit k h thng iu khin.
- Ch to phn cng..
- Xy dng thut ton iu khin v chng trnh iu khin.
- Ghp ni, hon thnh sn phm.

Phng php v phng tin nghin cu

V phng php nghin cu:
-

Tm hiu cc ti liu v thit k hin c trong v ngoi nc.

Tm hiu v nm vng c s l thuyt.
Tnh ton, thit k, ch to h thng iu khin, tin hnh thc nghim nh
gi v hiu chnh h thng.

V phng tin nghin cu:

-

Thit k mch nguyn l v mch in bng phn mm Altium Designer.

Lp trnh vi iu khin trn phn mm PICC.

CHNG 1
TNG QUAN

1.1. Mc tiu chung

Thit k, ch to thnh cng m hnh my bay trc thng quadcopter iu khin
t xa qua sng RF c kh nng t cn bng, thu thp, x l d liu v truyn v h
thng gim st, hin th trn my vi tnh.
Lm quen vi quy trnh thit k h c in t:
Phn tch yu cu
Thit k
Xy dng, ch to, tch hp
Th nghim v nh gi
Kt lun
1.2. Mc tiu ring
Thit k, ch to thnh cng m hnh my bay trc thng quadcopter iu khin
t xa qua sng RF c kh nng t cn bng v c kh nng ghp ni vi module cm
bin thu thp d liu mi trng xung quanh, truyn d liu v h thng gim st v
hin th trn my vi tnh.
1.2.1.

Tnh hnh nghin cu trong v ngoi nc

a. Lch s pht trin ca my bay quadcopter

Etienne Oehmichen l nh khoa hc u tin th nghim v ch to quadcopter
trong nhng nm 1920. Trong su thit k m ng th, ng ch nht l thit k
th hai vi tm cnh qut, tt c c iu khin bng mt ng c duy nht. Thit k
ny l mt h khung thp, b tr hnh ch thp, bn u ca hnh ch thp c lp
cnh qut dng hai cnh. Gc nghing ca nhng cnh qut ny c th thay i c
bng cch un cong chng. Nm trong tm cnh qut quay theo mt phng nm ngang
gi my bay n nh theo chiu ngang. Mt cnh qut c gn u my bay iu
khin, cc cnh qut cn li c tc dng to lc y my bay i v pha trc. Vi
hn mt nghn chuyn bay th nghim vo gia nhng nm 1920, thit k ny bay
c kh n nh. Nm 1923, chic my bay ny c th bay c vi pht trong
3

khng kh. Ngy 14 thng 4 nm 1924, n bay c 360m. Sau , n hon

thnh c mt chuyn bay u tin vi khong cch l 1km trong 7 pht 40 giy. Sau
thi gian ny, chic my bay ny khng bao gi c s dng na..[6]

Hnh 1.1. Quadcopter ca Etienne Oehmichen

Sau Etienne Oehmichen, tin s George de Bothezat v Ivan Jerome pht trin
loi my bay ny vi su cnh qut mt cu trc hnh ch X. C my ny c iu
khin bng cch thay i n l hoc cng lc gc nghing ca cc cnh qut. Ngoi
ra, bn cnh qut nh c gn thm tr gip iu khin. Chic quadcopter ny
c t tn l Bch tuc bay, n bay cao c 5m so vi mt t. Nhng, do gi
thnh cao v kh nng bay thp nn d n ny b hy b ngay sau . [3]

Hnh 1.2. Bch tuc bay

4

Nm 1956, Convertawings mt ln na nghin cu v th nghim li thit k

trc thng 4 cnh qut c th nghim bi Etienne Oehmichen v George de
Bothezat trc . Bn cnh qut ca chic trc thng ny c b tr thnh tng cp
trn hai thanh ni song song. Chic trc thng ny c iu khin bng cch thay i
lc y ca cc cnh qut. Lc y ca cc cnh qut c cung cp bi hai ng c
truyn ng ti h thng cnh qut bng dy ai ch V. Thng 3 nm 1956 chuyn bay
c th nghim thnh cng, tuy nhin, d n ny cng sm chm dt v khng c
kinh ph.[3]

Hnh 1.3. Convertawings Model A

T , vic pht trin quadcopter khng cn c ch nhiu nh trc. Mi
cho n nhng nm gn y, Quadcopter mi c quan tm tr li v kh nng ng
dng rt ln trong lnh vc thit b bay m hnh.
b. Tnh hnh nghin cu nc ngoi
V tnh cht n gin trong kt cu c kh, linh ng trong vic iu khin cng
nh kh nng ng dng thc t cao, m hnh quadcopter v ang c nhiu hng
trn th gii nghin cu v pht trin tr thnh sn phm thng mi.
Di y l mt s sn phm in hnh:
5

Sn phm X-Pro ca Draganfly Innovations

Hnh 1.4. Sn phm X-Pro ca Draganfly Innovations

Cc thng s k thut:
-

C th nng c ti a 1360g.

c iu khin bng b v tuyn 9 knh FM(PCM).

Khung c lm bng cht liu Carbon.

S dng pin 6000mAH 14,8V.

C trang b Camera.

Thi gian bay 20 pht.

Gi t 5000USD n 8000USD.

Tuy nhin, vic iu khin loi my bay ny ph thuc kh nhiu vo k nng v

kinh nghim ca ngi iu khin. Ngi s dng cn c thi gian lm quen v khng
c h thng t ng cn bng cng nh cc cm bin cn thit khc.[3]
6

Sn phm MD4 200 ca hng Microdrone

Hnh 1.5. Sn phm MD4 200

y l mt sn phm chuyn nghip vi tn gi l VTOL AUMAV (Vertical Take
Off and Landing Autonomous Unmanned Micro Aerial Vehicle) hay my bay siu nh
t ng ct cnh v h cnh thng ng. Loi ny c trang b h thng AAHRS
(Altitude and Attitude and Heading Reference System) hay cn gi l h thng tham
chiu cao, gc nghing v hng. Sn phm ny c trang b cc loi cm bin
gia tc, gyro, cm bin t trng, nhit - m, cm bin p sut. MD4 200 cn
c kh nng bay theo l trnh ci t sn nh c trang b h thng nh v ton cu
GPS.[3]
MD4 200 cng c trang b Camera chp nh v trinh thm trn khng.
Cc ph tng khc i km bao gm b thu pht tn hiu video v d liu. My tnh
hin th tnh trng ca my bay cng nh d liu thu c t cc cm bin c trang
b trn my bay.
Khung ca MD4 c lm bng cc bon nn c u im l nh, chu va p tt,
ngoi ra cn c tc dng nh mt v bc chng nhiu in t.
MD4 s dng motor c s vng quay ti a 2000 vng/pht nn n thp, thch
hp cho vic trinh thm hay quan st trn khng. MD4 cn c ch bo v, t h
cnh khi gn ht pin hoc mt tn hiu iu khin.
Sn phm ny s dng pin Lipo 14.8 V, 2300mAh, thi gian bay 20 pht. Gi
thnh ca sn phm ny l 5000USD.

Sn phm AR.Drone ca hng Parrot

Hnh 1.6. AR.Drone ca hng Parrot

AR.Drone l sn phm chy trn iPhone vi mc ch gii tr. Sn phm ny s
dng pin Lion c kh nng bay 15 pht, n c lm t si Carbon v xp vi kch
thc v khi lng rt nh ( AR.Drone ch nng 400g).[3]
AR,Drone c trang b 2 camera, kt ni v c iu khin trc tip t in
thoi qua sng wifi. Nhng hnh nh thu c t camera s hin th trc tip trn in
thoi gip ngi dng iu khin d dng v an ton hn.
AR.Drone c bn vi gi 299USD ti M.
Sn phm ca phng th nghim TEDGlobal

Hnh 1.7. Quadcopter ca TEDGlobal

Gn y, vo thng 6 nm 2013, Raffaello D'Andrea trnh din quadcopters
bay ca mnh. Chic quadcopter ny c kh nng suy ngh nh mt vn ng vin.
N c kh nng nh chnh xc qu bong c nm ra, gi thng bng ly nc trn
n, thc hin cc ng tc nho ln C th ni y l mt trong nhng bc tin rt
ln trong vic nghin cu v pht trin quadcopter.
8

c. Tnh hnh nghin cu trong nc

M hnh quadcopter do nhm k s L Cng Danh thc hin
Ngy 1/12/2009 s Khoa hc Cng ngh thnh ph H Ch Minh nghim
thu cng trnh Nghin cu, thit k v ch to m hnh my bay ln thng bn chong
chng c kh nng t di cn bng v di chuyn trong nh do k s L Cng Danh,
ging vin Khoa c kh, trng i hc giao thng vn ti thnh ph H Ch Minh
lm ch nhim, k s Phan Ngc Huy l ngi nghin cu chnh.
m bo nh ca m hnh, nhm nghin cu s dng vt liu l nhm
v si Carbon ch to khung my bay. So vi cc my bay m hnh ang bn trn
th trng, sn phm ny c mch iu khin phc tp hn, gip ngi s dng d
dng iu khin hn, ngi cha s dng bao gi cng c th iu khin c. [3]

Hnh 1.8. M hnh my bay nghim thu ti S Khoa hc Cng ngh

Ch to quadcopter ti cc cu lc b m hnh
Trong nm 2010 n nay, t thnh cng ca k s Phm Ngc Huy trn, vic
ch to quadcopter tr thnh tro lu ln trong cc nhm chi my bay m hnh.
Kt qu l nhiu m hnh quadcopter bay ln t am m ca nhng ngi ch to.
Tuy nhin, nhng m hnh ny thng c mua sn c phn mch iu khin, t
ngi t xy dng v ch to phn iu khin ny.
9

Hnh 1.9. M hnh quadcopter ti cu lc b m hnh

1.2.2.

Ni dung nghin cu ca ti

Quadcopter l mt ti kh, i hi kin thc tng hp nhiu lnh vc nh:

c kh thit k, thit b bay, kh ng hc, ng lc hc, x l tn hiu s, mch in
t, vi iu khin, giao tip my tnh, truyn thng v tuyn, m hnh ha, m phng,
lp trnh iu khin, lp trnh giao din Do , i hi mi thnh vin trong nhm
phi n lc ht mnh hon thnh c ti.
Vi mc tiu ring trnh by trn, n i dung nghin cu ca ti l:
- Tm hiu l thuyt : c s l thuyt iu khin Quadcopter, cc chun giao tip
SPI, I2C, h thng nh v qun tnh INS, l thuyt iu khin PID.
- Tnh ton, thit k h thng iu khin.
- Ch to phn cng cho m hnh.
- Xy dng thut ton iu khin v chng trnh iu khin.
- Ghp ni, hon thnh sn phm.

10

CHNG 2
C S L THUYT

2.1. C s l thuyt v m hnh ton ca Quadcopter

2.1.1. L thuyt iu khin Quadcopter
Cp cnh qut pha trc (front) v pha sau (back) quay ngc chiu kim ng
h, trong khi cp cnh bn phi (right) v bn tri (left) li quay thun chiu kim
ng h nhm cn bng moment xon c to ra bi cc cnh qut trn khung. C 4
cnh phi sinh ra mt lc y bng nhau khi Quadrocopter ct cnh v h cnh
(throttle up/down). Gc xoay (roll) c iu khin bng cch thay i tc gia
cnh bn phi v bn tri sao cho vn gi nguyn tng lc y sinh ra bi cp cnh
ny. Tng t nh vy, gc nghing (pitch) c iu khin bng thay i tc ca 2
cnh pha trc v pha sau m vn gi nguyn tng lc y. Trong khi , gc lch
(yaw) c iu khin nh vo s thay i tc ca cp cnh phi tri so vi tc
ca cp cnh trcsau m tng lc y 4 cnh vn khng i Quadrocopter gi
c cao (Hnh2.1). [4]

Hnh 2.1. Chuyn ng cn bn ca Quadcopter

11

2.1.2. C s l thuyt m hnh bay Quadcopter

a. L thuyt chuyn ng Quadcopter v m hnh ng lc hc
Quadcopter hot ng da trn s chuyn ng ca cc dng kh do cnh my
bay to ra di chuyn xung di, lm vt bay ln trn v s chnh lch vn tc ca cc
ng c s lm thay i hng bay ca Quadcopter (Hnh 2.2).

Hnh 2.2. Moment quay quanh cc trc

Xy dng m hnh ng lc hc:
Gi

e1

l h quy chiu qun tnh Tri t v

eB

l h quy chiu khung

Quadcopter (Hnh 2.3).

Quadcopter c nh hng nh ma trn xoay R:

C C C S S S C C S C S S

R S C S S S C C S S C C S
S

C S
C C

12

(2.1)

Lc sinh ra ca cc rotor :

Fi b.i 2 , i 1,2,3,4

Hnh 2.3. H quy chiu Quadcopter

Khi , lc nng cho c khung my bay l:
4

i 1

i 1

T Fi i 2

(2.2)

Phng trnh m t gia tc ca Quadcopter:

..
x

..
..

r y g .

..

0
0
T

0 R
0
m

1

(2.3)

13

Phng trnh quan h gia ma trn qun tnh

v moment quay hi chuyn

I R I X , IY , I Z

, moment quay M

MG
..
..
.
I . I . M G M

(2.4)

Ta c moment quay hi chuyn ph thuc vo cc yu t vn tc xoay vi

u1 T , u2 , u3 , u4

ln lt l cc n v moment quay cc chuyn ng roll, picth, yaw

hay vn tc quay

u T u1 , u2 , u3

v vn tc gc

my bay s c:

g u 1 2 3 4

(2.5)

Kt hp (2.5) vi (2.3) v (2.4) ta c phng trnh ng lc hc ca Quadcopter

l:
u1
m
..
u
y cos sin sin sin cos . 1
m
..
u
z g cos cos . 1
m
..
. . I I
I .
L
y z R g u u2
Ix
Ix
Ix
..

x cos sin cos sin sin .

..

I z I x
I .
L
R g u u3

I I
Iy
y
y

..

. .

I x I y
1
u4
Iz
Iz

b. M hnh tnh ton kh ng hc

14

(2.6)

Vic tnh ton kh ng hc m t cc tc ng ca cnh qut khi quay trong

khng kh. Vi cc thng s:
din tch ca cnh qut,
lc y l:

TMT N

s kg / m3

l lc y ca cnh qut, hng ln,

Do lc y

l mt khng kh. Khi , ta c phng trnh

TMT 2 s S I 2 N
TMT WP

S m2

(2.7)

mg
4 ( Trng lng nng c bi 1 cnh qut).

Vn tc dng kh cho mi cnh qut l:

VI

WP / 2s S

(m/s)

(2.8)

2.2. H thng dn ng qun tnh INS

2.2.1.

Khi nim

INS l cm t vit tt ca Inertial Navigation System tm dch l h thng dn

ng qun tnh. H thng dn ng qun tnh l h thng s dng cc cm bin vn
tc gc v gia tc c lng v tr, vn tc ca vt th bay.[5]

15

Hnh 2.4. Trc ta ca h thng INS

Phn c bn ca h thng dn ng qun tnh l n v o qun tnh(IMU) gm
ba cm bin Gyro 1 trc hoc mt cm bin Gyro 3 trc cho php xc nh vn tc gc
nghing(Roll), gc chc(Pitch) v gc hng(Yaw) trong h ta vt th bay(Hnh
2.4). Ngoi cm bin Gyro, h thng INS cn c th c thm cm bin gia tc cho
php xc nh gia tc theo ba trc ca h ta vt th bay ny.
2.2.2.

Phn loi h thng dn ng qun tnh

C hai loi h thng dn ng qun tnh:

Kiu th nht c (Gimbal):
Cc cm bin Gyro v gia tc c gn trn n nh(Hnh 2.5). ny c
treo trong mt khung ba bc t do. n nh ny s chuyn ng c lp vi phng
tin mang n v ba trc ta ca ny lun khng i trong sut qu trnh chuyn
ng m c th l song song vi h ta dn ng. Cm bin Gyro gn trn s
o c s sai lch v gc ca so vi h ta dn ng v u ra ca n s tc
ng tr li thng qua lc moment tc ng ln trc cac-dang duy tr n nh.

16

Hnh 2.5. H thng kiu Gimbal

u im ca h thng kiu ny l c chnh xc kh cao do s dng cc cm
bin chnh xc v do khng gn trc tip vi vt th nn ngun tn hiu o khng b
nhiu. Tuy nhin, h thng kiu ny i hi cc thit b o rt phc tp v t tin.
Ngoi ra, cn c nhc im l khi lng ln nn kh hiu chnh v th nghim cc
b.[2]
Kiu th hai khng (Strapdown):
H thng dn ng qun tnh kiu th hai khng (Strapdown) c s dng
rng ri hn. h thng dn ng qun tnh kiu ny, cc cm bin Gyro v gia tc
c gn trc tip v c nh ln vt th. Lc ny, cc trc ca cm bin khng c
n nh trong khng gian m s thay i theo hng chuyn ng ca vt th. Cm
bin Gyro s xc nh tc gc quay ca h ta lin kt vi h ta dn ng.
Do gn trc tip vi vt th nn cc s liu o c t cc cm bin s c sai s ln
hn so vi h thng th nht. Tuy nhin, gi tr ny c th c hiu chnh vi vn tc
quay ca tri t xc nh chnh xc v tr v vn tc ca vt th bay. [2]
2.2.3.

Cc sai s hay gp trong h thng INS

C nhiu loi sai s trong cc h thng INS, ch yu l do cc cm bin qun

tnh gy nn. Bng 2.1 lit k mt s li gy ra bi cc cm bin gia tc v Gyro:
Bng 2.1. Li gy ra bi cm bin qun tnh[4]
17

Loi

Gy nn sai s

Li v tr khi lp t cm bin

Gc nghing, gc chc v gc hng

lch (offset) ca cm bin

gia tc

Li ra cm bin gia tc s b lch i

mt gi tr khng i. Gi tr ny li
thay i mi khi tt, bt thit b.

Hin tng lch v tri ca

cm bin gia tc

Vt th khng chuyn ng nhng

vn c vn tc gc khng i

Nhiu ngu nhin

Li ngu nhin trong o lng

2.3. B iu khin PID

2.3.1. H thng iu khin vng kn c bn
Mt h thng iu khin c bn gm ba thnh phn:
-

B iu khin

i tng iu khin

Thnh phn cm bin v cc thit b o lng

Hnh 2.6. Cu trc h thng iu khin vng kn

Mt h thng iu khin vng kn nh trn c th c biu din di dng
phng trnh vi phn tng qut:

d n y (t )
dy (t ) d n1 y (t )

F
t
,
y
(
t
),
,..., n 1 , r (t )

dt n
dt
dt

Trong :

18

(2.9)

r (t )

l u vo ca h thng

y (t )

l u ra ca h thng

F l mt hm s, thng thng, F l mt hm phi tuyn

Nu s dng phng trnh trng thi, h thng iu khin c th c m t bng

phng trnh:

u t h t , x t , r t

(2.10)

Trong :
-

u(t )

l tn hiu iu khin i tng

r (t )

l u vo ca h thng

x(t )

l ma trn trng thi

2.3.2. Thut ton PID kinh in

a. Lut iu khin P
Lut iu khin P to tn hiu iu khin t l vi tn hiu sai lch. Nu sai lch
nh, b iu khin to ra tn hiu iu khin nh. Nu sai lch ln, b iu khin to ra
tn hiu iu khin ln.
Phng trnh m t lut iu khin P:

u t K p .e t
Trong :
-

u(t )

l tn hiu iu khin

e(t )

l tn hiu sai lch

KP

l h s t l
19

(2.11)

b. Lut iu khin PI
Lut iu khin PI l s kt hp ca khu t l P v khu tch phn I c tc dng
loi b cc sai lch tnh. Khu tch phn I s lin tc thay i tn hiu iu khin cho
n khi sai lch bng 0. Lut iu khin PI c dng trong nhng h thng c ti
thay i trong khi lut iu khin P khng th gim sai lch xung mc chp nhn
c.
Phng trnh m t lut iu khin PI:

u t K p .e t K I . e t .dt

(2.12)

Trong :
-

u t

l tn hiu iu khin

e t

l tn hiu sai lch

K K

p
, I l h s ca khu t l v khu tch phn I
c. Lut iu khin PD

Lut iu khin PD l s kt hp ca khu vi phn D vi khu t l P nhm lm

gim dao ng ca h thng. Khu vi phn D c tc dng d on gi tr k tip ca
sai lch v thay i tn hiu iu khin sao cho ph hp. Lut iu khin PD c
dng trong cc h thng c ti thay i t ngt m lut iu khin P khng th gi sai
lch mc chp nhn c.
Phng trnh m t lut iu khin PD:

u (t ) K P .e(t ) K D .

de(t )
dt

Trong :
-

u t

l tn hiu iu khin

e t

l tn hiu sai lch

K K

p
, D l h s ca khu t l v khu vi phn D
d. Lut iu khin PID

20

(2.13)

Lut iu khin PID kt hp cc khu t l P, tch phn I v vi phn D. Khu tch

phn I dng loi b sai lch do s thay i ln ca ti. Khu vi phn D dng
gim dao ng do s thay i t ngt ca ti.
Phng trnh m t lut iu khin PID:

u (t ) K P .e(t ) K I . e(t ).dt K D .

de(t )
dt

(2.14)

Trong :
-

u t

l tn hiu iu khin

e t

l tn hiu sai lch

K p KI KD
,

l h s ca khu t l, khu tch phn v khu vi phn D

2.3.3. Gii thut PID s

Cng thc ca b iu khin PID trong (2.14) l dng hm lin tc ca bin e,
trong c c thnh phn tuyn tnh, o hm v tch phn. Tuy nhin, h thng my
tnh v vi iu khin l h thng s. Mun xy dng b iu khin PID trn my tnh
hay trn vi iu khin chng ta phi bit cch xp x phng trnh lin tc thnh dng
ri rc ( s ha). [9]
Trc ht, thnh phn t l tng i n gin v l quan h tuyn tnh
, chng ta ch cn ly trc tip cng thc ny m khng cn bt k xp x no.

K P .e(t )

Tip n, xp x cho o hm ca bin e. Gi h l thi gian ly mu, c th xp

x o hm bng s thay i ca e trong hai ln ly mu lin tip:

de e k e k 1

dt
h

e k

Trong ,
l gi tr hin ti ca e,
tc .(Hnh 2.6).

e k 1

21

(2.15)

l gi tr ca e trong ln ly mu

Hnh 2.7. Xp x o hm ca sai s e

Thnh phn tch phn c xp x bng din tch vng gii hn bi ng biu
din hm ca e v trc thi gian. Do vic tnh ton tch phn khng cn qu chnh xc,
c th dng phng php xp x n gin nht l xp x hnh ch nht ( sai s ca
phng php ny cng l ln nht).

Hnh 2.8. Xp x tch phn sai s e

22

Tch phn ca bin e c tnh bng tng din tch cc hnh ch nht ti mi thi
im ang xt. Mi hnh ch nht c chiu rng bng thi gian ly mu h v chiu cao
l gi tr sai s e ti thi im ang xt.(Hnh 2.7). Ta c:
k

edt e k .h
t

(2.16)

Tng hp cc xp x ta c cng thc ca b PID s l:

e k e k 1 k
u K p .e
e k .h
h
0

(2.17)

Trong , u l tn hiu iu khin.

Cng thc (2.17) c th d dng thc hin bng vi iu khin. Do , c th a
gii thut PID vo lp trnh vi iu khin iu khin thit b.

23

CHNG 3
THIT K V CH TO H THNG IU KHIN QUADCOPTER

3.1. S khi ca h thng

Hnh 3.1. S tng th h thng iu khin Quadcopter

H thng nhn chung gm 3 thnh phn chnh: khi tay iu khin, khi iu
khin trung tm v khi giao tip my tnh.
+ Khi tay iu khin: gm c mt vi iu khin c nhim v c gi tr iu
khin t joystick, quy i sang chui k t v truyn tn hiu iu khin v khi iu
khin trung tm qua sng RF.
+ Khi iu khin trung tm : gm c mt vi iu khin trung tm c nhim v
c d liu t cc cm bin Gyro v Gia tc ca khi IMU t tnh ton ra gc
nghing, nhn tn hiu iu khin t tay iu khin, tnh ton v xut tn hiu PWM
iu khin bn ng c qua cc khi iu tc ESC. Ngoi ra, vi iu khin ny cn c
nhim v truyn d liu v khi giao tip my tnh qua sng RF.
+ Khi giao tip my tnh: gm c mt vi iu khin v mt b thu RF c nhim
v nhn d liu t khi m hnh Quadcopter v truyn v my tnh thng qua RS232.

24

3.2. Tnh ton, chn khung v ng c cho my bay

Do iu kin hn ch v kinh ph, thi gian thc hin ngn v kh nng kin thc
trong lnh vc c kh cn hn hp nn nhm quyt nh mua khung c sn (Hnh
3.2) c thi gian tp trung vo phn iu khin.

Hnh 3.2. Khung Quadcopter c dng trong n

Tnh ton, chn ng c:
Tng khi lng 1.2 kg, kch thc 620x620x250mm mt khng kh trong
iu kin nhit

1.249 kg

250 C v p sut khng kh ti mt t khong 1atm:

m ,

4 0.125 0.049m ,ta tnh c tng lc nng:

2
S D

TMT ' 4TMT 4WP 4 2 s S I 2 mg 1.2 9,8 11.76 N

VI
Suy ra

TMT '
11.76

9.8 m / s
2s S
2 1.249 0.049

Theo tnh ton trn nng Quadcopter ln khi mt t, ta tnh chn nh sau:
ng c Himodel 2212 c thng s 1000kv(RPM/V),nng 47g, in p t 6-12V,
dng ln n 10A. y l ng c mt chiu khng chi than Brushless. Loi ng c
25

ny c tc cao, moment ln v c bit l khi lng nh, rt ph hp cho nng

my bay c nh.(Hnh 3.3).

Hnh 3.3. ng c Himodel 2212

B iu tc HobbyWing 20A, ngoi chc nng thay i dng cho ng c th
ESC nhn tn hiu iu bin rng xung PWM 50Hz t b iu khin v thay i
rng xung 1-2ms xc nh RPM ca ng c.(Hnh 3.4)

26

Hnh 3.4. B iu tc HobbyWing

Pin Lipo 3cell 2200mAh 30C v c dng 2.2x30C = 66A cp cho 4 ng c
hot ng tt trong khong 10 pht.

Hnh 3.5. Pin Lipo v cnh qut

Cnh loi APC 10x4.5inch to lc y khong 1.95N>>L/4.
3.3. Thit k h thng iu khin
Yu cu k thut t ra:
-

Tc x l nhanh, p ng kp vi tc iu khin

27

X l nhiu tt, loi b ti a nhiu xut hin trong qu trnh iu khin.

Tnh n nh v bn cao

3.3.1.

Gii thiu mt s linh kin dng trong h thng

a. Module thu pht RF nrF24L01

NRF24L01 l mt trong nhng dng sn phm ca nh sn xut Nordic (Nauy)
c chc nng thu pht tn hiu qua sng RF s dng in p thp (3.3V). (Hnh 3.4)

Hnh 3.6. Module NRF24L01

Mt vi im chnh ca module NRF24L01:
-

Thc hin chc nng thu pht d liu bng sng RF tn s 2.4GHz.

S dng dng sng iu ch GFSK.

- Tc truyn ti d liu trong khng kh t 1Mbp 2Mbp.

- Tiu th nng lng thp, s dng in p thp 3.3V
-

S dng giao thc phn cng Shockburst:

T ng to gi d liu (Preamble, Address, CRC).

T ng pht hin gi d liu v xc nhn.
La chn d liu truyn t 1 32 Byte.
C kh nng va thu v va truyn d liu, cu hnh bng chng trnh.
T ng xc nhn m ACK.
C 6 ng truyn dn truyn nhn d liu

- S dng giao thc SPI giao tip vi vi iu khin, tc truyn nhn ln

ti 10Mbp.

28

Trong giao tip vi vi iu khin, NRF24L01 ng vai tr l slave, vi iu khin

c th cu hnh cc chc nng, c ghi cc gi tr thanh ghi trong NRF24L01 thng
qua m lnh 8bit m nh sn xut cung cp qua giao tip SPI.

Hnh 3.7. S kt ni module NRF24L01 vi vi iu khin

b. Module cm bin gc MPU6050 GY521

Hnh 3.8. Module cm bin gc MPU6050

Module MPU6050(Hnh 3.6) l mt sn phm ca Invensense cho php o vn
tc gc 3 trc v gia tc ba trc. MPU6050 s dng in p thp 3.3V v giao tip vi
vi iu khin thng qua chun giao tip I2C vi tc ti a 400kHz. MPU6050 c
mt s tnh nng chnh nh sau:

29

C th la chn c di o sao cho ph hp. Cc mode

2 g , 4 g , 8 g , 16 g

250, 500, 1000, 2000 o

s i vi Gyro.
i vi gia tc,
- u ra 16bit cho c gia tc v gyro.
- Tch hp b lc thng thp k thut s.
- Chc nng t kim tra.
- Giao tip vi my tnh qua chun giao tip I2C ph bin vi tc ti a
400kHz. Hnh(3.7).

Hnh 3.9. S kt ni MPU6050 vi vi iu khin

c. ng c BLDC v b iu tc ESC
Nh ni trn, ng c dng trong ti l ng c Himodel c thng s
1000kv(RPM/V), in p t 6-12V, dng ln n 10A. ng c BLDC Himodel l
loi outrunner, bt ln m hnh theo kiu rear mount, c rotor l v bn ngoi gn cc
nam chm vnh cu quay trn theo trc ng c cn stator l li mang cc cun dy
ng yn.
Bng 3.1. Thng s k thut ca ng c Himodel
STT

Thng s

Gi tr

Vn tc

1000KV

in p hot ng

9.6 14.8 V

Dng in cc i

25 A

Lc nng ti a

1200 gram

Kch thc cnh qut

<10x7 inch
30

Cnh qut c kch thc ph hp vi ng c s dng trong m hnh:

Hnh 3.10. Cnh qut s dng trong n

Cnh qut dng cho Quadcopter l loi c tc thp. Cn phi s dng 2 loi
cnh qut CCW( counter clockwise) v CW( clockwise) trong Quadcopter v c 2
ng c quay cng chiu kim ng h v 2 ng c cn li quay ngc chiu kim
ng h trit tiu moment xon sinh ra.
ng c BLDC hot ng cn c b iu tc ESC( Electronic Speed
Controller) bin i in p mt chiu thnh in p xoay chiu 3 pha cung cp cho
ng c. ESC c 2 dy ngun c ni vi pin Lipo, 3 dy ni vi ng c BLDC, 1
dy tn hiu PWM v 1 dy p ra 5V.

31

Hnh 3.11. ESC Hobbyking 20A dng trong m hnh

Bng 3.2. Thng s k thut ca ESC
STT

Thng s

Gi tr

in p hot ng

4.8 14.4 V

Dng in cp cc i

20 A

BEC

2A/5V

Tn s iu khin pha

8KHz

Cu hnh bng lp trnh

Cch ghp ni v iu khin ng c BLDC bng ESC:

ng c BLDC c iu khin bng b iu tc ESC c ch t bo v khi
qu ti, cnh bo khi ngun pin khng hoc khng c tn hiu iu khin. Cc cnh
bo c thng bo qua nhng ting bip, beep ca ng c, di ngn khc nhau m
nh sn xut a ra. Cch ghp ni ng c vi ESC c th hin trong hnh sau:

32

Hnh 3.12. Cch ghp ni ng c vi ESC

Ba dy ca ng c c ni vi ba dy mu xanh, v en lin nhau. Khi o
chiu 2 trong 3 dy vi nhau th ng c s quay ngc chiu. Trong ba dy nh, dy
quan trng nht l dy mu trng, y l dy tn hiu pwm nhn tn hiu iu khin t
vi iu khin iu khin tc ca ng c. Mt yu cu t ra l xung a vo
dy ny phi c tn s 50Hz v c ng rng xung t 1ms 2ms. Ngoi khong gi tr
ny, ng c s khng hot ng nn bt buc phi khng ch gi tr pwm trong
chng trnh iu khin.
Mt lu quan trng na l khng c php tng duty_cycle mt cch t ngt
v nh vy kh nng x l tn hiu ca ESC khng p ng kp, d gy li tn hiu v
kh kim sot.
d.Cc loi vi iu khin
Nh gii thiu phn trn, h thng gm c ba khi chnh, mi khi phi c
mt vi iu khin x l d liu. H vi iu khin PIC ca Microchip vi u im l
gi thnh r,y cc chc nng, h tr nhiu giao tip nga vi v quan trng dng
vi iu khin ny h tr lp trnh bng ngn ng C, d dng lp trnh dnh cho ngi
mi tip xc. V nhng l do trn, h vi iu khin PIC c chn s dng trong
n ny.
Hai dng vi iu khin dng trong n c gii thiu di y
Vi iu khin PIC16F877A
Vi iu khin PIC16F877A l mt vi iu khin ca hng Microchip, l mt vi
iu khin kh ph bin, y tnh nng, phc v hu ht cho cc ng dng thc t.
y l dng PIC kh d dnh cho nhng ngi mi lm quen vi vi iu khin thng
c dng hc tp v to nn tng cho vi iu khin PIC. Trong n ny,
PIC16F877A c dng trong khi tay iu khin v khi giao tip my tnh.

33

Hnh 3.13. Vi iu khin PIC16F877A

Vi iu khin PIC16F877A c rt nhiu chc nng hu dng, trong khun kh
n, cc chc nng c s dng l:
-

Ngt timer nh thi gian truyn tn hiu iu khin t tay iu khin ln

mch iu khin trung tm.
S dng 4 knh ADC 10bits c d liu t joystick.
S dng SPI mm giao tip vi NRF24L01.
Giao tip USART vi module PL2030 trong khi giao tip my tnh.
Vi iu khin PIC18F443

Trong khi iu khin trung tm, vi iu khin trong khi ny phi x l rt nhiu
vic: nhn tn hiu t tay iu khin sau quy i ra gc iu khin, tnh ton xut
tn hiu pwm iu khin bn ng c thng qua bn b iu tc ESC, gi d liu v
khi giao tip my tnh thng qua module NRF24L01. Vi iu khin phi mnh
lm c nhng vic . Vi 8 chn iu ch PWM trong c 4 knh PWM c lp,
PIC18F4431 l mt vi iu khin c Microchip khuyn dng iu khin ng c.
Cng vi cc chc nng c bn nh PIC16F877A, PIC18F4431 c chn s dng
trong khi ny.

34

Hnh 3.14. Vi iu khin PIC18F4431

Cc chc nng ca 18F4431 c dng trong khun kh n l:
-

Giao tip I2C vi module MPU600 GY521.

S dng SPI mm giao tip vi NRF24L01
S dng POWER PWM iu khin ng c thng qua b iu tc ESC
Ngt timer nh thi trong tnh ton gc nghing v tnh ton u ra ca b
iu khin PID.

3.3.2.

Thit k tay iu khin

Hnh 3.15. S khi tay iu khin

35

Nhim v ca tay iu khin l truyn tn hiu iu khin 4 knh, roll, pitch, yaw,
throttle n mch iu khin trung tm. Tay iu khin dng vi iu khin
PIC16F877A lm vi x l trung tm. PIC16F877A giao tip vi module thu pht RF
NRF24L01 qua chun giao tip SPI v c ADC 10 bit trn 4 knh ca joystick, quy
i sang chui v truyn v mch iu khin trung tm qua sng RF.

N
C
C
9 N
9
G
N
D
G
N
D
6
6
CA
N
C
1
4N
1
4
N
0
A
N
0
7
7
V
C
V
C
8
8

G
N
D
5
V
5
V
*
1
M
i
n
J
o
y
s
t
i
c
k
1
1
3
V
C
N
C
R
1
k
2
G
N
D
R
2
I
C
1
1
0
G
N
D
BTU
1O
G
N
D
1
k
12
V
P
1
4
0
P
G
D
3
P
1
M
L
R
/
V
P
R
B
7
/
P
G
D
V
C
A
N
0
2
3
9
C
B
T
1
3
.
V
3
.
V
N
R
A
0
N
0
6
C
2
3
8
B
T
2
4
1
A
N
1
C
E
C
S
N
1
1
R
B
5
G
N
D
A
N
1
3
4
4
7
G
N
D
S
K
M
O
I
2
/
2
4
5
6
A
N
1
2
5
3
6
I
R
Q
M
I
O
R
Q
R
A
3
N
3
R
B
3
/
P
G
M
7
8
6
5
M
O
S
1
2
5
V
G
N
D
G
N
D
4
T
0
C
K
I
R
B
2
V
C
9
1
0
7
4
C
N
5
5
/
4
1
N
C
G
N
DC15G
8
3
H
e
a
d
r
5
X
2
R
E
R
D
0
/
I
N
T
9
2
5
V
1
W
V
D
1
0
1
G
N
D
2
/
C
S
S
V
3
0
C
E
V
D
R
D
7
/
P
7
N
D
2
2
9
S
K
5
V
A
N
0
S
6
6
1
3
8
M
I
O
P
2
O
C
1
/
L
K
I
5
S
5
4
7
V
2
O
R
D
4
/
P
4
1
3
p
5
2
6
T
X
5
R
0
T
S
C
7
R
X
1
6
5
R
G
N
D
G
N
D
C
1
/
I
6
T
3
2
0
M
H
z
7
4
P
C
2
2
C
P
1
R
5
/
S
D
O
4
8
2
3
C
R
3
S
K
C
4
P
I
5
G
N
D3pR4LE12D
90116F8D
B
T
1
5
V
*
2
0
/
0
D
3
3
1
M
i
n
J
o
y
s
t
i
c
k
N
a
p
P
i
c
1
P
1
R
2
/
S
2
1
1
3
V
C
N
C
7
A
3
2
3
.
V
G
N
D
1
R
3
1
0
G
N
D
T
X
G
N
D
2
1
k
3
R
V
C
3
5V1Rk512
G
N
D
B
T
2
G
N
D
4
4
1
A
N
1
2
5
V
G
N
D
A
N
1
5
L
E
D
3
G
N
D
P
L
2
3
0
1
2
5
V
V
C
5
N
C
LLEED
2D
P
4
3.V1k12
G
N
D
5
V
1
3
.
23p
3 5VA
G
N
D
N
02G
N
D Jum
Hnh 3.16. S nguyn l tay iu khin

Hnh 3.17. S mch in tay iu khin

36

3.3.3.

Thit k mch iu khin trung tm

Mch iu khin trung tm c nhng chc nng v nhim v sau:

-

Nhn tn hiu iu khin t tay iu khin, t tnh ton ra gc nghing iu

khin a vo b iu khin PID.
c d liu Gyro v gia tc t cm bin MPU6050, tnh ton a ra gc
nghing ca m hnh ti thi im hin ti.
Tnh ton u ra ca b iu khin PID, t xut tn hiu PWM iu
khin ng c thng qua ESC.
Truyn d liu v khi giao tip my tnh gim st, hin th.

Hnh 3.18. S khi khi iu khin trung tm

Vi nhng chc nng v nhim v nh trn, vi iu khin trong mch iu khin
trung tm cn phi c b nh mnh, y cc giao tip ngoi vi v quan trng l
phi c phn cng ph hp xut c 4 tn hiu PWM iu khin 4 ng c cng
mt lc thng qua 4 ESC. Vi iu khin PIC18F44341 l mt vi iu khin mnh
lm c nhng iu . Vi 8K b nh chng trnh, 768byte ram cng vi 8
chn POWER_PWM vi 4 knh PWM c lp, PIC18F4431 c Microchip khuyn
dng trong lp trnh iu khin ng c.

37

Hnh 3.19. S nguyn l mch iu khin trung tm

Hnh 3.20. S mch in khi iu khin trung tm

38

3.3.4.

Thit k khi giao tip my tnh

Hnh 3.21. S khi khi giao tip my tnh

Khi giao tip my tnh c nhim v nhn d liu t khi iu khin trung tm
v truyn v my tnh qua RS232. Khi ny gm c mt vi iu khin PIC16F877A,
mt khi thu pht RF NRF24L01 v mt module giao tip my tnh PL2303.

P
1
P
2
V
P
3
.
V
3
.
V
1
2
5
C
E
C
S
N
2
3
4
5
V
G
N
D
S
K
M
O
I
3
5
6
P
M
I
O
R
Q
4
7
8
C
G
N
D
G
N
D
5
9
1
0
R
1
N
a
p
P
i
c
n
r
f
2
4
l
5
V
k
I
C
1
B
T
1
G
N
D
12
P1234567M
4
0
P
G
D
/8R
L
R
V
P
R
B
7
/
G
D
3
9
C
UO
NV
R
A
0
N
0
6
P
C
8
I
R
Q
L
E
D
1
1
1
R
B
5
R
2
7
M
O
S
12
2
/
2
4
3
6
C
N
A
3
N
3
R
B
3
/
P
G
M
1
k
5
E
L
E
D
3
4
T
0
C
K
I
R
B
2
4
S
K
3
.
V
5
/
4
1
3
M
I
O
R
E
R
D
0
/
I
N
T
G
N
D
9
2
5
V
1
W
V
D
1
0
1
G
N
D
P
3
2
/
C
S
S
5
V
3
0
3
.
V
L
E
D
2
V
D
R
D
7
/
P
7
5
R
3
G
N
D
2
2
9
T
X
12
13C
C
S
6
6
4
1
3
8
R
O
C
1
/
L
K
I
5
S
5
3
1
k
4
7
G
N
D
L
E
D
3
2
O
R
D
4
/
P
4
2
p
5
2
6
T
X
5
V
R
0
T
S
C
7
R
X
1
1
6
5
R
C
1
/
I
6
T
2
0
M
H
z
7
4
H
e
a
d
r
5
2
2
C
P
1
R
5
/
S
D
O
G
N
D
8
2
3
R
3
S
K
C
4
P
I
1
9
D
0
/
0
D
3
3
G
N
D3p 2016F871A
P1R2/S21
Hnh 3.22. Mch nguyn l khi giao tip my tnh

39

Hnh 3.23. S mch in khi giao tip my tnh

3.4.

Xy dng chng trnh iu khin

3.4.1.

Chng trnh khi tay iu khin

Thut ton chng trnh khi tay iu khin:

Hnh 3.24. S thut ton tay iu khin

40

3.4.2.

Chng trnh khi giao tip my tnh

Thut ton chng trnh khi giao tip my tnh:

Hnh 3.25. S thut ton khi giao tip my tnh

41

3.4.3.

Chng trnh khi iu khin trung tm

S khi h iu khin cn bng:

Hnh 3.26. S khi h iu khin cn bng

Trong h iu khin cn bng, b iu khin PID c chc nng tnh ton ra xung
PWM iu khin tc ng c qua b iu tc ESC da vo sai s gia gc
nghing mong mun v gc nghing thc t. Trong h ny, xung PWM iu khin
ng c trc v ng c sau s c tnh ton gia vo sai lch ca gc pitch( sai
lch theo trc x), xung PWM iu khin ng c tri v ng c phi s c tnh
ton gia trn sai lch gc roll ( sai lch theo trc y). S thut ton iu khin tng
ng c c trnh by di y (Hnh 3.21).

42

Hnh 3.27. S khi thut ton iu khin ng c

u ra ca b PID l lng cn tng ( hoc gim) ca tn hiu PWM h thng
cn bng ti v tr mong mun. Nh vy, mun h thng cn bng, ng c trc v
ng c sau phi tng hoc gim mt lng ng bng u ra ca b PID theo trc X,
ng c tri v ng c phi phi tng hoc gim mt lng ng bng u ra ca b
PID theo trc Y.
B iu khin PID c s ha v tnh ton trong vi iu khin c th hin
qua lu gii thut di y (Hnh 3.29).
Lu gii thut chng trnh tnh PID:

43

Hnh 3.28. Lu gii thut tnh PID

ng c c iu khin bng tn hiu PWM xut ra t vi iu khin thng qua
ESC. Trong vi iu khin, tn hiu PWM c thay i bng cch thay i gi tr duty
trong hm set_power_pwmx(duty). Gi tr duty ny c tnh da vo u ra ca b
iu khin PID theo tng trc. Lu gii thut iu khin ng c c th hin
trong hnh 3.29.
Lu gii thut chng trnh iu khin ng c:

Hnh 3.29. Lu gii thut chng trnh iu khin ng c

Chng trnh chnh c nhim v c d liu cc cm bin, tnh ton gc nghing
trong ngt timer, gi cc chng trnh con tnh ton PID, iu khin ng c. Lu
gii thut chng trnh chnh c trnh by trong hnh 3.30.

44

45

Hnh 3.30. Lu gii thut chng trnh chnh

CHNG 4
THC NGHIM V NH GI

4.1. Hnh nh phn cng ca c h thng

Hnh 4.1. Phn cng ca c h thng trong n

H thng bao gm ba khi c th hin trong hnh 4.1: khi tay iu khin,
khi iu khin trung tm v khi giao tip my tnh.
4.2. Th nghim khi giao tip my tnh
Di y l nhng hnh nh th nghim vic truyn nhn d liu gia khi iu
khin trung tm v khi giao tip my tnh. Vi iu khin trung tm truyn gi tr gc
nghing pitch, roll v khi giao tip my tnh. Khi giao tip my tnh li truyn gi tr
ny v my tnh qua RS232. Gi tr c hin th qua gc nghing ca hnh v trong
Labview.

46

Hinh 4.2. Th nghim truyn nhn vi module giao tip my tnh

nh gi
Gi tr c truyn v kh tt, d liu truyn v c hin th gn ging vi thi
gian thc. Khi quay m hnh bt u t 0 , sau 10 pht quay, a m hnh tr li v
tr ban u th sai s khong 1 n 1.5 c hai trc. Sai s ny l chp nhn c,
chng t vic chng tri v hiu chnh cho cm bin Gyro c thc hin tt.
47

4.3. H thng th nghim kh nng thng bng ca Quadcopter

H thng c th hin trong hnh 4.2

Hnh 4.3. H thng th nghim kh nng thng bng ca my bay

Vic th nghim cn bng cho Quadcopter l kh kh, do cha bit chc m hnh
ch to ra c cn bng hay khng, vic iu khin p ng ng mong mun hay
cha. Nu khng c phng php th nghim ng c th dn ti va p, ri v v
hng hc cc thit b in t, gy thit hi ln v thi gian v chi ph. H thng th
nghim sau c th lm gim nhng thit hi . H thng gm c 4 dy di c ni

48

vi 4 ca quadcopter v ni chc xung t. Khi m hnh chy, nu quadcopter

cha c thng bng, 4 dy ny s c nhim v gi thng bng cho quadcopter,
chng ta c th th nghim c thng bng bng cch ni lng t t cc dy, nu ni
lng ht cc dy m quadcopter vn thng bng th b iu khin c ch to
thnh cng. Ngoi ra, cn c 1 dy ni vi trng tm ca quadcopter v ni ln trn c
nhim v gi thng bng cho quadcopter khi quadcopter cha khi ng.
4.4. Th nghim b iu khin PID
B iu khin PID c th nghim bng cch khi cha lp cnh, cho ng c
quay v nghing m hnh theo cc gc pitch, roll tng ng xem p ng ca ng c
c ng vi lut iu khin PID khng. Khi t gc nghing mong mun l pitch = 0
v roll = 0 ( m hnh v tr thng bng).Kt qu cho thy, khi quay m hnh theo
chiu dng ca trc x ( m hnh chi xung), tc ca ng c pha trc tng,
pha sau gim v khi quay m hnh theo chiu dng ca trc y (m hnh nghing sang
bn tri), tc ca ng c bn tri tng, tc ca ng c bn phi tng v ngc
li. Nh vy, cc ng c hot ng ng theo nguyn tc ca b iu khin PID.
nh gi
B iu khin PID hot ng ng theo nguyn tc iu khin PID, tuy nhin,
cha th ch to thnh cng m hnh cn bng cho my bay. Nguyn nhn l do vic
chng nhiu cho mch iu khin cn km, gii thut iu khin cn cha tt,vi iu
khin cn phi tnh ton qu nhiu nn nhiu lc chip b treo. Hn na, vic chn h
s cho b iu khin cn cha ng khin cho p ng ca m hnh khng c nh
mong mun.

49

KT LUN

Kt qu t c
Vi cc phng hng, mc tiu ra, qua qu trnh thc hin, n thc
hin c nhng kt qu sau:
-

Hiu c nguyn l hot ng v l thuyt iu khin m hnh my bay Quadcopter.

Xy dng ch to thnh cng phn cng cho h thng iu khin Quadcopter c th
nhn c tn hiu iu khin t xa v truyn d liu v h thng gim st trn my
tnh.
Xy dng v th nghim thnh cng truyn nhn gc nghing ca m hnh v my
tnh qua sng RF. Kt qu t c kh kh quan, vic hiu chnh v chng tri cho
cm bin Gyro c thc hin tt.
Th nghim thnh cng b iu khin PID, m hnh p ng ng vi lut iu
khin PID.
Xy dng thnh cng b lc b trn vi iu khin.
Chng trnh iu khin c xy dng thnh cc m un, d tha k v pht trin sau
ny.
Hn ch ca n v bin php khc phc
My bay cha bay ln thnh cng do thut ton iu khin cha tt, vn x l
nhiu cha c x l trit ,vi iu khin hay b treo trong qu trnh hot ng. Mt
khc, di iu chnh ca b PWM cn qu nh nn ng c cha th p ng tt c.
C th khc phc c nhng hn ch ny bng cch dng nhng vi iu khin c tc
x l d liu cao hn nh dng PIC24F, dsPIC, thit k mch cch ly, chng nhiu
cho vi iu khin. Tng phn di cho b PWM bng cch dng ngt timer iu
ch PWM.
Hng pht trin ca n

X l nhng hn ch ca n my bay c th bay ln thnh cng, hot ng tt.

Tch hp thm Camera gim st truyn hnh nh v my tnh ng dng trong theo
di.
Tch hp h thng nh vi GPS, xy dng thm chng trnh iu khin Autopilot.
PH LC

Code chng trnh chnh

50

#include "main.h"
#include "NRF24L01/NRF24L01Lib.h"
#include "NRF24L01_2/NRF24L01Lib2.h"
#include "mpu6050/mpu.h"
#include <string.h>
#include <stdlib.h>
//=======================================

#define MOTOR_MIN 375

#define MOTOR_MAX 750
#define MOTOR_RANGE 375
#define POWER_PWM_PERIOD 1870

float XErr,YErr;
float PreXErr,PreYErr;
float XIntegral=0,YIntegral=0;
float XDiff,YDiff;
float Kp=2,Kd=0.005,Ki=1.88;
float pPartX=0,dPartX=0,iPartX=0;//Cac thanh phan P,I,D cua bo dieu khien
float pPartY=0,dPartY=0,iPartY=0;//Cac thanh phan P,I,D cua bo dieu khien
float OutputX,OutputY;
float CtrlXAngle=0,CtrlYAngle=0;
int16 dutyFront,dutyBack,dutyLeft,dutyRight;
int16 ga;

float pitch,roll;
int16 GyroX,GyroY,GyroZ,AccelX,AccelY,AccelZ;
51

int8 enable_display = 0;
int16 GyroXOffset,GyroYOffset,GyroZOffset;
char txBuf[32],rxBuf[32];
char str[20];

void angleTransfer(float x,float y);

void CatChuoi(char string[30],char x[4],char y[4]);
void setMotorFront(int16 duty);
void setMotorBack(int16 duty);
void setMotorRight(int16 duty);
void setMotorLeft(int16 duty);
void PID_Calculator();
void updateMotor();
void updateMotor2();

#int_RTCC
void ngat_timer0(void)
{
disable_interrupts(GLOBAL);
set_timer0 (23);
pitch = GetPitchAngle(pitch,GyroX,AccelY,AccelZ);
roll = GetRollAngle(roll,GyroY,AccelX,AccelZ);
enable_display =1;

void main()
52

init_MPU6050 ();
spi1Init (); // Khoi tao NRF
spi2Init ();
init_NRF24L011 () ;
init_NRF24L012();
setup_timer_0 (RTCC_INTERNAL|RTCC_DIV_256|RTCC_8_BIT);
set_timer0 (23);
enable_interrupts (GLOBAL);
enable_interrupts(INT_TIMER0);
setup_power_pwm_pins(PWM_BOTH_ON, PWM_BOTH_ON, PWM_BOTH_ON,
PWM_BOTH_ON);
setup_power_pwm(PWM_CLOCK_DIV_128|PWM_FREE_RUN, 1, 0,
POWER_PWM_PERIOD, 0, 1,0);

delay_ms (1000) ;
GyroXOffset = GetGyroXOffset ();
GyroYOffset = GetGyroYOffset ();
GyroZOffset = GetGyroXOffset ();
enable_display=0;
pitch = 0.0;
roll = 0.0;
setRxMode2 ();
setTxMode1 ();

while (TRUE)

53

{
GyroX = GetData (GYRO_XOUT_H) - GyroXOffset;
GyroY = GetData (GYRO_YOUT_H) - GyroYOffset;
GyroZ = GetData (GYRO_ZOUT_H) - GyroZOffset;
AccelX = GetData (ACCEL_XOUT_H);
AccelY = GetData (ACCEL_YOUT_H);
AccelZ = GetData (ACCEL_ZOUT_H);
if (enable_display == 1) {
angleTransfer(pitch,roll);
PID_Calculator();
updateMotor();
enable_display = 0;
enable_interrupts(GLOBAL);
enable_interrupts(INT_TIMER0);
}
}
}
Th vin giao tip NRF24L01
#ifndef __NRF24L01Lib_H
#define __NRF24L01Lib_H

//***********************************CHAN
NRF24L01**********************************
#define CE PORTD7
#define SCK PORTD6
#define MISO PORTD5

//Chan In

#define CSN PORTB1

54

#define MOSI PORTB2

#define CE_DDR

DDRD7

#define SCK_DDR DDRD6

#define MISO_DDR DDRD5
#define CSN_DDR

DDRB1

#define MOSI_DDR DDRB2

/
*********************************************************************
****************/
#define READ_REG

0x00

//

#define WRITE_REG

0x20 //

#define RD_RX_PLOAD

0x61

#define WR_TX_PLOAD

0xA0

#define FLUSH_TX

0xE1

#define FLUSH_RX

0xE2

#define REUSE_TX_PL
#define NOP

//

//
//

0xE3

0xFF

//

//

//

//*************************************SPI(nRF24L01)******************
**********************************
#define CONFIG

0x00 //

#define EN_AA

0x01 //

#define EN_RXADDR
#define SETUP_AW

0x03 //

#define SETUP_RETR
#define RF_CH
#define RF_SETUP

0x02 //

0x04 //

0x05 //
0x06 //
55

#define STATUS

0x07 //

#define OBSERVE_TX
#define CD

0x08 //

0x09 //

#define RX_ADDR_P0

0x0A //

#define RX_ADDR_P1

0x0B //

#define RX_ADDR_P2

0x0C //

#define RX_ADDR_P3

0x0D //

#define RX_ADDR_P4

0x0E //

#define RX_ADDR_P5

0x0F //

#define TX_ADDR

0x10 //

#define RX_PW_P0

0x11 //

#define RX_PW_P1

0x12 //

#define RX_PW_P2

0x13 //

#define RX_PW_P3

0x14 //

#define RX_PW_P4

0x15 //

#define RX_PW_P5

0x16 //

#define FIFO_STATUS

0x17 //

//********************************************************************
******************
#define TX_ADR_WIDTH 5

// 5 uints TX address width

#define RX_ADR_WIDTH 5

// 5 uints RX address width

#define TX_PLOAD_WIDTH 32

// 32 uints TX payload

#define RX_PLOAD_WIDTH 32

// 32 uints TX payload

unsigned char TxBuf[32]=

{

56

0x01,0x02,0x03,0x4,0x05,0x06,0x07,0x08,
0x09,0x10,0x11,0x12,0x13,0x14,0x15,0x16,
0x17,0x18,0x19,0x20,0x21,0x22,0x23,0x24,
0x25,0x26,0x27,0x28,0x29,0x30,0x31,0x32,
};
//********************************************************************
**********
uchar TX_ADDRESS[TX_ADR_WIDTH]={0x43,0x34,0x01,0x01,0x10}; // XAC
DINH 1 DIA CHI TX TINH
uchar RX_ADDRESS[RX_ADR_WIDTH]={0x43,0x34,0x01,0x01,0x10}; // XAC
DINH 1 DIA CHI RX TINH

/*-----------------Cac Ham Giao Tiep SPI----------------------------------------*/

void spiInit(void){
// Cau hinh huong du lieu cho cac chan I/O
CE_DDR = DDROUT;

// Chan CE la Output

SCK_DDR = DDROUT; // Chan SCK la Output

MISO_DDR = DDRIN; // Chan MISO la Input
CSN_DDR = DDROUT; // Chan CSN la Output
MOSI_DDR = DDROUT;
}

uchar spiTransfer(uchar bytes)

{
uchar counter;
for(counter = 8; counter; counter--)
{
if (bytes& 0x80)
57

MOSI = 1;
else
MOSI = 0;
bytes<<= 1;
SCK = 1; /* a slave latches input data bit */
if (MISO)
bytes |= 0x01;
SCK = 0; /* a slave shifts out next output data bit */
}
return(bytes);
}

void spiWriteReg(uchar spiCommand,uchar content)

{
CSN = 0;
spiTransfer (spiCommand); //VIET 1 BYTE LENH SPI
spiTransfer (content); //VIET 1 BYTE NOI DUNG
CSN = 1;
}

uchar spiReadReg(uchar reg)

{
uchar regValue;
CSN = 0;
spiTransfer (reg);
regValue = spiTransfer (0);
CSN = 1;
58

return regValue;
}

void spiReadBuff(uchar reg,uchar *buf, uchar bytes)

{
uchar i;

CSN = 0;
spiTransfer(reg); //VIET GIA TRI reg
for (i = 0; i < bytes; i++)//VONG LAP bytes LAN, VOI bytes DUOC DINH TRUOC
{
buf[i] = spiTransfer (0); //LUU GIA TRI VAO MANG BUF
}

CSN = 1;
}

void spiWriteBuff (uchar reg, uchar * buf, uchar bytes)

{
uchar i; //BIEN i
CSN = 0;
spiTransfer(reg); //VIET GIA TRI reg
for (i = 0; i < bytes; i++)//VONG LAP bytes LAN, VOI bytes DUOC DINH TRUOC
{
spiTransfer ( * buf++); //VIET GIA TRI BYTE BUF
}

59

CSN = 1;
}

/* -----------------Cac ham giao tiep NRF24L01-----------------------------------*/

void init_NRF24L01 (void)
{
CE = 0;
CSN = 1;
SCK = 0;
spiWriteBuff(WRITE_REG + TX_ADDR, TX_ADDRESS,
TX_ADR_WIDTH); //GUI DIA CHI TX nRF24L01
spiWriteBuff(WRITE_REG + RX_ADDR_P0, RX_ADDRESS,
RX_ADR_WIDTH); //THIET LAP KENH 0 SAU do GIAI QUYET CAC DIA CHI
TX
spiWriteReg(WRITE_REG + EN_AA, 0x01); //THIET LAP KENH 0 NEN DUOC
TU DONG
spiWriteReg(WRITE_REG + EN_RXADDR, 0x01); //TRUY CAP KENH 0 CHO
PHEP ?
spiWriteReg(WRITE_REG + RF_CH, 0); // CHON KENH TAN SO RF 0
spiWriteReg(WRITE_REG + RX_PW_P0, RX_PLOAD_WIDTH); //THIET LAP
DE CHON CHIEU DAI VA CHIEU DAI LIE?
spiWriteReg(WRITE_REG + RF_SETUP, 0x07); // TX_PWR:0dBm,
Datarate:1Mbps, KHUECH DAI TIENG ON THAP TANG?
spiWriteReg(WRITE_REG + CONFIG, 0x0e); // THIET LAP CONFIG CRC, DE
TAO RA DIEN NANG LUONG PWR_UP VE CHE do PHAT
}

void setRxMode(void)
{
60

CE = 0;
spiWriteReg(WRITE_REG + CONFIG, 0x0f); // Enable CRC, 2 byte CRC, Recive
CE = 1;
delay_us (130); //
}

void setTxMode(void)
{
CE = 0;
spiWriteReg (WRITE_REG + CONFIG, 0x0e); // Enable CRC, 2 byte CRC, Send
CE = 1;
delay_us (130); //
}

uchar nRF24L01_RxPacket (uchar * rxBuf)

{
unsigned char revale = 0;
unsigned char sta;

//if (RX_DR) // Data in RX FIFO

sta = spiReadReg(STATUS); // Read Status
if ( (sta&0x40) != 0) // Data in RX FIFO
{
CE = 0; //SPI
spiReadBuff (RD_RX_PLOAD, rxBuf, TX_PLOAD_WIDTH); // read receive
payload from RX_FIFO buffer
revale = 1;

61

spiWriteReg (WRITE_REG + STATUS, sta);

return revale;
}

void nRF24L01_TxPacket (uchar *txBuf)

{
CE = 0;
spiWriteBuff (WRITE_REG + RX_ADDR_P0, TX_ADDRESS,
TX_ADR_WIDTH); // Send Address
spiWriteBuff (WR_TX_PLOAD, txBuf, TX_PLOAD_WIDTH); //send data
spiWriteReg (WRITE_REG + CONFIG, 0x0e); // Send Out
CE = 1;
}

// --------------------END OF FILE-----------------------// -------------------------------------------------------

62

TI LIU THAM KHO

Ting Vit
[1]

L Cng Danh, Nghin cu, thit k v ch to m hnh my bay ln thng bn

chong chng c kh nng t cn bng v di chuyn trong nh, tp.HCM,
10/2009.

[2]

Nguyn Quang Huy, H thng dn ng qun tnh, tp ch khoa hc v cng

ngh hng hi s 23, 8/2010.

[3]

Nguyn Hi ng Tm, Nguyn L Nht Thng, Nghin cu v ch to m

hnh my bay Quadrocopter,TP.HCM, 1/2011.

[4]

Lm Ngc Tm, Thit k v ch to m hnh bay Quadcopter, hi ngh sinh

vin nghin cu khoa hc ln th 8, Nng, 10/2012.

[5]

Trn c Tn, Hunh Hu Tu, nh v v dn ng trong th k 21, i

hc cng ngh, HQG H Ni.
Ting Anh

[6]

Hemant L. Jadhav, Application of mechatronics in design and control of a quad

copter flying robot for aerial surveillance, Excel Journal of Engineering
Technology and Management Science (An International Multidisciplinary
Journal), January - June 2013-14.

[7]

Sherryl H. Stovall, Basic Inertial Navigation, Naval Air Warfare Center Weapons
Division.

[8]

Tommaso Bresciani, Modelling, Identification and Control of a Quadrotor

Helicopter, Lund University, Sweden, 10/2008.

Website
[9]

www.hocavr.com

[10] www.clbmohinh.com

63