DATN quadrocopter
-
Upload
huynh-van-khanh -
Category
Documents
-
view
226 -
download
0
Transcript of DATN quadrocopter
-
8/3/2019 DATN quadrocopter
1/114
i
QUADROCOPTER
GVHD: Th.S
SVTH : MSSV: 06111089
SVTH : MSSV: 06111096
1 11
-
8/3/2019 DATN quadrocopter
2/114
ii
---o0o---
---o0o---
MSSV: 06111089 MSSV: 0611109661111
1.1.
BAY QUADROCOPTER
1.2.
- Quad
- Quadrocopter
- LabVIEW
1.: 30/08/20102. 21/01/20113.. 4. ThS.
..................... ....................................................
........................................................................................................................
-
8/3/2019 DATN quadrocopter
3/114
iii
g
TP. 11
Th.S
-
8/3/2019 DATN quadrocopter
4/114
iv
N
TP. ng 11
Th.S
-
8/3/2019 DATN quadrocopter
5/114
v
,
- , trong
hoa C
Bhp tP.HCM
V, , ,- , .
01 01 11
-
8/3/2019 DATN quadrocopter
6/114
vi
Quad Quadrotor H
(Inertial)hai
PIDcho
-
8/3/2019 DATN quadrocopter
7/114
vii
ABSTRACT
A Quadrocopter, also called a Quadrotor Helicopter, is an aircraft that is lifted by
four rotors on a cross-shaped frame.
We succeeded in building a Quadrocopter with stiff and lightweight structure.
Our Quadrocopter in-flight dynamics was measured via an IMU (InertialMeasurement Unit) which equipped with a dual-axis gyroscope and a three axis tilt-
sensor. A microcontroller took these sensor inputs and performed PID control
algorithm on the four motors by varying PWM signals sent to each of the motors.
-
8/3/2019 DATN quadrocopter
8/114
viii
i
ii
iii iv
v
vi
Abstract vii
viii
x
xiii
xiv
1.1 Quadrocopter 1
1
3
1.2 4
1.2.1 Quadrocopter 4
1.2.2 7
1.2.3 10
1.3 12
1.4 13
2:
2.1 Quadrocopter 14
2.2 Quadrocopter 17
2.2.1 17
2.2.2 20
2.2.3 -Euler 21
2.2.4 27
2.3 32
2.4 36
2.4.1 36
2.4.2 ? 36
2.4.3 38
-
8/3/2019 DATN quadrocopter
9/114
ix
2.4.4 38
2.4.5 40
2.5 - LabVIEW 42
2.5.1 LabVIEW 42
2.5.2 LabVIEW 43
QUADROCOPTER
3.1 46
3.2 47
3.3 51
3.3.1 52
3.3.2 53
3.3.3 Module RF 57
3.3 58
3.3.5 59
3.4 61
4.1 Quadrocopter 64
4.1.1 X 64
4.1.2 T 65
4.1.3 X 66
4.2 LabVIEW 67
4.2.1 67
4.2.2 69
4.3 73
4.3.1 Motor 73
77
5.1 84
5.2 85
5.3 85
86
Quadrocopter 87P VATmega128 88
-
8/3/2019 DATN quadrocopter
10/114
x
-rex 600 1
1
2Quadrocopter 2
Quadrocopter 3
4
- Richet Gyroplane No.1 5
Quad 5
6
6
- 7
- 8
8
9
9
- 10
11
Quad 11Quad 12
Quadrocopter 14
15
15
16
16
2.6 Yaw 17 17
Quadrocopter 20
25
28
29
2.12 31
32
32
33
-
8/3/2019 DATN quadrocopter
11/114
xi
33
34
35
35
37 37
38
40
40
41
LabVIEW 42
LabVIEW 43
44
44
45
m 45
LabVIEW 45
46
2 47 48
49
49
50
50
50
51
10 52
1 52
2 53
53
4 54
5 55
6 56
7 578 57
-
8/3/2019 DATN quadrocopter
12/114
xii
9 58
20 Brushless motor FC2835-10T 59
21 -1060Rx3 59
2 60
23 ESC HiModel GX-40A 604 ESC Hobbywing Pentium 30A 61
5 61
6 62
7 62
H8 63
64
66 67
67
68
68
68
68
68
69
70
71
71
72
72
73
768 2 76
77
78
79
80
81
82
5 83
-
8/3/2019 DATN quadrocopter
13/114
xiii
57
58
-10T 59
HiModel GX-40A 60
A 61
74
75
-
8/3/2019 DATN quadrocopter
14/114
xiv
I. :
X m xE
Y m yE
Z m zE
rad
rad
rad
u m/s xB
v m/s yBw m/s zB
p rad/s xB
q rad/s yB
r rad/s zB
E m E
E m E
E m/s2
E rad E
E rad/s E
rad/s2
VE m/s vector vE
VB m/s vector B
B m/s2 vB
B rad/s vector B
rad/s
2
- E
-
- vector v theo
-
- vector vn tc t
-
-
8/3/2019 DATN quadrocopter
15/114
xv
-
FE
N theo
FB
N theo E
GF
N
B
GF N
GB() -
GH -
UB -
U1 N
U2 Nm
U3 Nm
U4 Nm B
B Nm moment theo
R -
T -
J -
EB - B
EH() - H
MB -
MH - ma tr thng u H
OB () - B
OH() - H
CB() -
CH() - HS(.. .) -
rad/s 1 rad/s 2 rad/s 3 rad/s
4 rad/s
TMT N
TBET N
-
8/3/2019 DATN quadrocopter
16/114
xvi
QBET Nm A kg/s p1 Pa
p2 Pa
p Pa p- Pa
v1 m/s
v2 m/s
vI m/s
v m/s
v- m/s
vV m/s vH m/s it
P rad/s
dDBET N/m
dLBET N/m
dFBET N/m c
dTBET N/m
dHBET N/m I rad
Io rad
Itw rad
I rad
II.
a rad
-12
A m2
75.5 x 10-3
b N s2
54,2 x 10-6
bBET N s2
54,2 x 10-6
bMT N s2
54,2 x 10-6
c m 0.02
CD - 0.05 CL - 0.05 (lift)
-
8/3/2019 DATN quadrocopter
17/114
xvii
d N m s2
1.1 x 10-6
g m/s2
9.81 gia t
TPJ Nms
2 104x10-6
l m 0.22 Quad
m kg 1.365 Quadrocopter
NB - 2
A kg/m3
29
-
8/3/2019 DATN quadrocopter
18/114
1
1.1 nhau giQuadrocopter [1]
(Quadrocopter, Quadrotor hay Quad---, gi sau:
1.1.1
Quad
-rex 600
-
8/3/2019 DATN quadrocopter
19/114
2
3 Helicopter
Quad--
Quadrocopter
--- - Quad
Quadrocopter.
-
8/3/2019 DATN quadrocopter
20/114
3
Quad
Quad
nh.Quad
5 Quadrocopter
-sau:
-:
-: t 4
-
---sau).1.1.2
Quad (gyroscope) (accelero-meter)
Quad
Quad
-
8/3/2019 DATN quadrocopter
21/114
4
Quad
1.2 1.2.1 Quadrocopter
C
1.6
Quadrocopter Charles Richet Richet Gyroplane No.1y--Quadrocopter 75Kg -
.5m.
Quadro-
copter
-
8/3/2019 DATN quadrocopter
22/114
5
nh 1.7 Breguet- Richet Gyroplane No.1
Quadrocopter 82Quadrocopter .
1.8 Quad
-
8/3/2019 DATN quadrocopter
23/114
6
Quadrocopter
Quadrocopter
1956, Convertawings l t Oemichen G. de Bothezat .B theo song song. . hai . hai xoay .
Convertawings Model A
Quad, Quadrocopter v bay . [14]
-
8/3/2019 DATN quadrocopter
24/114
7
1.2.2
Quadrocopter
a-anfly Innovations [1]
11 -Innovations
-
-
-
-
-
-
-
-[1]
(Vertical Take Off and Landing Autonomous Unmanned Micro Aerial Vehicle) hay
AAHRS ( Altitude and Attitude and H
MD4-
-
8/3/2019 DATN quadrocopter
25/114
8
Khung MD4--
-
-
12 -
13
cParrot
AR.- ,
b
-
8/3/2019 DATN quadrocopter
26/114
9
Augmented
14
dMikroKopter[13]
15 -
Quad
-
8/3/2019 DATN quadrocopter
27/114
10
1.2.3 trong a. Quadrocopter
t.HCM - .HCM.
16 M-
Theo PGS- - .HCM,
b. Quadrocopter t[12]
QuadQuad
scope
-
8/3/2019 DATN quadrocopter
28/114
11
17
c.Quadrocopter
Quad - Quad
, H cho
8 QuadrotoH
scope
-
8/3/2019 DATN quadrocopter
29/114
12
9 QuadKQ
, .
1.3 Quadrocopter
v.vDo trong
- Quadrocopter.
---
-
8/3/2019 DATN quadrocopter
30/114
13
c
- Quadrocopter l PWM.
- (gyroscope accelerometer) Quad .
1.4 :
-
-
- Th
:
- Quadrocopter.
- .
-
- LabVIEW.
-
8/3/2019 DATN quadrocopter
31/114
14
2.1 Quadrocopter [2]
Quadrocopter
(front) (back) (right) (left) oment 4 Quadrocopter (throttle up/down) xoay (roll) . (yaw) i so vsau Quadrocopter.
, Quadrocopter motor Quadrocopter
Quadrocopter.
Quadrocopter:
-
8/3/2019 DATN quadrocopter
32/114
15
Quad2Quadrocopter.
a. Hover: Quad1 = 2 = 3 = 4 = H).
2 Hover
b. Throttle: QuadQuad
3 Throttle
+ b.HAHQuad
AQuadrocopter.
c. Roll: Quad moment c Xb
-
8/3/2019 DATN quadrocopter
33/114
16
4 Roll
+
b.
HABABH.
d. Pitch:Quadrocopter bay lui b.
5 Pitch
+ : quanh b.HABABH.
e. Yaw:Quadrocopter b theo sau2 , 2 Quadrocopter quay quanh trc Zb theo quay) quanh trc Zb theo
-
8/3/2019 DATN quadrocopter
34/114
17
6 Yaw
:+ b.HABABH.
2.2 Quadrocopter [2]2.2.1
7 -
E[m] E [rad] Quadrocopter.
- Quadrocopter, d VB [m/s], vB [rad/s], [N], vmoment [Nm] Quadrocopter.
-
8/3/2019 DATN quadrocopter
35/114
18
OE: gc . xE: yE: zE: (xE,yE O
B: gQuadrocopter.
xB: Quadrocopter. yB: Quadrocopter. zB:
- Vector vE [m] EB :
E = [X Y Z ]T
(2.1)
- E [rad]
E
T(2.2)
=> EE :
= [E E]T = [X Y Z ]
T(2.3)
- VB [m/s]:
VB = [u v w ]T
(2.4)
-
B
[rad/s]:
B = [p q r]T
(2.5)
=> :
= [VBB]
T= [u v w p q r]
T(2.6)
- V
R, T
J.
- R
R = R(, z) R(, y) R(, x) = (2.7)
ck = cosk, sk = sin k, tk= tan k.
XoayE (yaw): = (2.8)
-
8/3/2019 DATN quadrocopter
36/114
19
1 (pitch): R(, y)= (2.9)
2 (roll): R(, x)= (2.10)
- T :
T = (2.11)- J :
J = (2.12)3x3
- J : =J . (2.13)
-
VE = E =R .VB (2.14)
- T:
=T .B (2.15)
-
8/3/2019 DATN quadrocopter
37/114
20
2.2.2
Quadrocopter.
8 Quadrocopter
m.E = FE
=> m. = R.FB
=> m(
) = R.FB
=> m( ) = R.FB=>
m( ) =FB (2.16)
m [kg] Quadrocopter.
FE
[N] v
E [m/s
2]
[m/s
2]
[-] I. =
=> I. =T.
=> I. +x (I.)= T. (2.17)
I [Nms2]
[rad/s2] vector
[rad/s2] vector
-
8/3/2019 DATN quadrocopter
38/114
21
hai 2.162.17), :
+ = (2.18)
I33 3x3.F
Bent
Quadrocopter.
2.2.3 Newton-Euler
:
.
.
2.13)
= J (2.13)
J
E E Quadrocopter
= [E E]T = [X Y Z ]
T(2.3)
VB [m/sB[rad/sQuadrocopter
= [VBB] T = [u v w p q r] T (2.6)
n tJ
J =
(2.12)Ma trn quay Rn chuyn vT
R = (2.7)T =
(2.11)
-
8/3/2019 DATN quadrocopter
39/114
22
2.18):
+ = (2.18)
OB
I
Quadrocopter
2.19):
=[ ] T = [Fx Fy Fz x y z]T (2.19)2.18
MB +CB() = (2.20)
MBCB()
2.21
MB =
=
(2.21)
MBthe
2.22
CB ( ) = =
(2.22)k theo
2.23):
T
(k) = = (2.23)
2.20
Quad Quadrocopter. Quadrocopter.
-
8/3/2019 DATN quadrocopter
40/114
23
GB(2
(2.24)GB().
GB() = BGF = 1R EGF = T
R
=
(2.24)
BGF E
GF
R 1R
TR
.
hai
Quadrocopte(2.25):
OB
() 41
TP
k
J
= TPJ TPJ (2.25)
OB ( TPJ 104x10-6 Nms2)
(2.26)
[rad/s]
[rad/s] 2.25):
= 1+23+4 = (2.26)1.2.3.
4
ba
-
8/3/2019 DATN quadrocopter
41/114
24
moment EB2 UBb [Ns2]d [Nms2 ] sau.
(2.27)
Quadrocopter:
UBEB2 =
=
(2.27)
Quadrocopter
,
,
moment .
EB(2.28):
EB =
(2.28)
T2.20 ng lc hc Quadrocopter 2.29):
MB +CB() = GB() + OB () + EB 2 (2.29)
Sp xp l2.29n tc ttheo h trc B:
= MB(CB() +GB() + OB ( ) + EB 2) (2.30)
u thi dng h
TPJ
TPJ
(2.31)
-
8/3/2019 DATN quadrocopter
42/114
25
(2.32)
IXX, IYY , IZZc c:
Idch chuyn = I + MD2
vi M [Kg] ng ca vt th.D [m] dch chuyn ca vt th so vi tr.
=> IXX = M (W2/12 + H
2/12) + M (DY
2+ DZ
2)
IYY = M (L2/12 + H
2/12) + M (DX
2+ DZ
2)
IZZ = M (W2/12 + L
2/12) + M (DX
2+ DY
2)
9
H thng hc ca Quadrocopter h2.31c vit trongh trc B c cp, tham chic s dng rng 6 bc tng h hng lc hc n kt hp gih tr
trc biudin trong h trc mi, tm g tr u mc
chng lc hc kt hp vu khin (nhthng ca h tr(2.33) biu din vector vn tc tng trc H :
= [E B]T = [XYZ]
T(2.33)
H thng lc h trc vit li dng ma trn theo(2.34):
MH +CH() = GH + OH(+EH(2 (2.34)
c t trc H.
Ma tr th trc H MH bng vi ma trh tr2.212.35):
-
8/3/2019 DATN quadrocopter
43/114
26
MH = MB = =
(2.35)
i, ma tr H CH() ng vi matrn Coriolis tr2.36):
CH() = =
(2.36)
Vector hp d trc H GH
(2.37 thn c ng hc.
GH = EGF
=
(2.37)
u ng con quay hi chuyi, bch trc B. Ma trt hi chuytheo h tr2.252.38).
OH( =OB () TP
J TPJ
(2.38)
Ma trn chuy trc H EH tr
n cn quay . Kt qu c
ma trn chuyng vi tc ch2.39):
EH(2 =
EB 2 =
(2.39)
Sp xp l2.34n tc ttheo h trc H:
= MH(CH() +GH + OH() +EH() 2) (2.40)
-
8/3/2019 DATN quadrocopter
44/114
27
X Y Z
TPJ
TPJ
(2.41)
-Euler cQuadrocopter.
qu trc B,c cho 2.32).
2.2.4
- - MT)
- - BET)
g
-
-
-
-
-
TMT [N]
v- [m/s]
v1 [m/s]
v2 [m/s] v+ [m/s]
-
8/3/2019 DATN quadrocopter
45/114
28
p- [Pa]
p1 [Pa] .
p2 [Pa]
p+ [Pa]
10
1 2).TMT = A(p1p2)
TMT = A (v- - v+) = A A v1 (v- - v+) A [m2 A
A [kg/m3
1
v2 -sau:
p- +A v-2 = p1 + A v12
p2 +A v22 = p+ + A v+2
+ = p-
v1 = (v++ v-)/2
vI = (v1v-) = (v+v-)/2
-
8/3/2019 DATN quadrocopter
46/114
29
TMT =2 A A v1 vI
v- = 0, suy ra v1 = vI
MT = Wp =
WP = 2A A vI2 vI =
[m/s]
[-
= vI / (H RP).
HP
2.11
11
I [rad]
T
-
8/3/2019 DATN quadrocopter
47/114
30
vT [m/s] H V
I t
dDBET
dLBET dFBET [N/m] BET BET
dFBETdTBETngang dHBET[N/m].
v
H
vv = vI = P RP
vH = P r = P RP (r/ RP)
P
dLBET = 0.5 A vH2CL c dr.
dLBET = 0.5 A vH2CD c dr.
CL[-D[-
L a [rad-1
a-1].
CL = a =a (II)
Io tItw
I = Io - Itw
I:
I =
dLBET =A vH2a (Io - Itw -) c dr
theo dTBET I
dTBET = dLBET cos I - dDBET sin I dLBET
TBETdTBET B [-] lB).
-
8/3/2019 DATN quadrocopter
48/114
31
TBET = NB (dTBET / dr)dr = NBA a c P2 RP3Io/6 - Itw/8 - /4). ngang dHBET
I
dHBET = dDBET cos I + dLBET sin I dDBET + dLBET
BETBET
QBET [Nm] = NB ((dBBET / dr) + (dLBET / dr) ) r dr= NBA c P
2RP
4(CD/8 +a Io/6 - Itw/8 - /4)
c. c
qua
12
.
.
-
8/3/2019 DATN quadrocopter
49/114
32
2.3 [3] [4]
g.
3
4
-
8/3/2019 DATN quadrocopter
50/114
33
BLDC cho. Electronic Speed Controller)
, pha.
BLDC, nrotor
5
ESC 50Hz.
6
-
8/3/2019 DATN quadrocopter
51/114
34
7
-
- 1 (xungBEMFBack ElectroMagnetic Field pulses).
xung BEMF.
-
8/3/2019 DATN quadrocopter
52/114
35
8
9 [13]
. Khi
-
8/3/2019 DATN quadrocopter
53/114
36
g
moment
2.4 [5] [6]2.4.1
X(k)=S(k)+N(k)
0)(
1
M
kNM
k
M
k
M
k
kSkX11
)()(
2.4.2
"A New Approach to Linear Filtering and Prediction Problems"
-
8/3/2019 DATN quadrocopter
54/114
37
trong.
.
accelerometer gyroscope.
2.20
:
2.21
-
8/3/2019 DATN quadrocopter
55/114
38
2.4.3
Kalman x
2.22
2.22
2.4.4
1 1 1* * w
k k k k x A x B u
(1)
kkkvxHz * (2)
A-xA
Buul .
-
8/3/2019 DATN quadrocopter
56/114
39
w~N(0,Q)
v ~N(0,R)
.
tr
kkkxxe
kkkxxe
c:
TkkkeeEP
TkkkeeEP
sau:
kkkkxHzKxx ** (3)
kxH *
kkkxHzKe **
kkktheo K, tak
1)(
RHHPHPKT
k
T
kk
Kk
1-
2-
-
8/3/2019 DATN quadrocopter
57/114
40
ta 1
k
x -
1ku (g 00 zHx ta 2.23.
2.23
2.4.5
2.24 Bode
-
8/3/2019 DATN quadrocopter
58/114
41
2.25
, gyroscope, ...
-
8/3/2019 DATN quadrocopter
59/114
42
.
2.5 - LabVIEW2.5.1 LabVIEW
LabVIEW (
National Instruments, . LabVIEW ,... (
LabVIEW ysinh, v.v
LabVIEW:
-
-
LabVIEW
-
8/3/2019 DATN quadrocopter
60/114
43
-
- Lab,...
- LabVIEW.
- ...
2.5.2 LabVIEWLabVIEW VI (Virtual Instrument).
*. Front Panel Block Diagram.
2.27 Front PDLabVIEW
LabVIEW :
- Panel):LabVIEW (controls) (indicators)
-
8/3/2019 DATN quadrocopter
61/114
44
2.28 Front panel
Controls = Inputs Indicators = Outputs
, trong
2.29 trong Front Panel
- Diagram):
.
-
8/3/2019 DATN quadrocopter
62/114
45
30 Block Diagram
, Block Diagram funtions)
Panel.
31
LabVIEW i
32 LabVIEW
-
8/3/2019 DATN quadrocopter
63/114
46
QUADROCOPTER
3.1
Khung
Quadrocopter
Joysticks
Pin 9V
Pin Lipo
ESCs
Motors
Thu RF
:
:
:
RS-232
- Volt rollpitch
- Quadrocopter
-
-
8/3/2019 DATN quadrocopter
64/114
47
3.2 thanh
Quadrocopter Quadrocopter
o.
Quadrocopter lhanh D/.
thanh 20mm = 254mm),
.0mm.
kQuadrocopterphay CNC .
3.2
,
-
8/3/2019 DATN quadrocopter
65/114
48
Sau m
khi
3.3
Quad
-
-
8/3/2019 DATN quadrocopter
66/114
49
- prop saver
-
3.4
3.5
-
8/3/2019 DATN quadrocopter
67/114
-
8/3/2019 DATN quadrocopter
68/114
51
-
40%
,Newton-Euler
3.9
3.3 :
-
-
-
Quadrocopter
-
8/3/2019 DATN quadrocopter
69/114
52
3.3.1
3.10
(Spread Spectrum
3.11
-
8/3/2019 DATN quadrocopter
70/114
53
3.12
-
3.3.2
-
8/3/2019 DATN quadrocopter
71/114
54
3.14
-
-
- - .
-
-
8/3/2019 DATN quadrocopter
72/114
55
-
8/3/2019 DATN quadrocopter
73/114
56
3.16
ATmegaATmega8 MHz ra
Baud khi UART, , ATmega
ATmega - -bit.
-
8/3/2019 DATN quadrocopter
74/114
57
.
3.17
3.3.3 Module RF 915MHz UART 8-bit,
19200bps, c.
3.18 Module RF UART
3.1
-
8/3/2019 DATN quadrocopter
75/114
58
3.3.4 IMU (Inertial Measurement Unit
, 6 :.
IMU 5 (IDG650) gyro LISY300AL .
3.19 LISY300AL
3.2
LISY300AL IDG650 MMA7361L
3.3 V 3.3 V 3.3 V
00 o/s 2000o/s
440 o/s1.5 g6.0 g
3.3 mV/o/s0.5 mV/
o/s
2.27 mV/o/s
800 mV/g
206 mV/g
4.5 KHz 25 KHz 6.0 KHz
/ / 400 Hz
,. Tuy
ra
gyroscope .
-
8/3/2019 DATN quadrocopter
76/114
59
3.3.5 T
FC2835-10T1.2Kg.
3.20 Brushless motor FC2835-10T
BLDC FC2835-10T
3.3 T-10T
STT
1
2
3 25 (Ampere)
4 1200 (gram)
5 < 10x7 (inch)
21 EP-1060Rx3
-
8/3/2019 DATN quadrocopter
77/114
60
22 EPP1045
Quader). tractor , Quad . -
cho[9]
: cho .
BLDC
3.23 ESC HiModel GX-40A
3.4 TESC HiModel GX-40A
STT
1 (Volt)
2
3 BEC (battery eliminator circuit) 2A / 5V
4 8KHz5 Programmable No
-
8/3/2019 DATN quadrocopter
78/114
61
3.24 ESC Hobbywing Pentium 30A
3.5 THobbywing Pentium 30A
STT
1 5.6 6.8 (Volt)
2
3 BEC (battery eliminator circuit) 2A / 5V4 8KHz
5 Programmable Yes
brushless DC motor:
ESC .0.0
.0.0ms.ATmega12 2 Timer 16-6
, BLDC Quadrocopter .
.
3.4
_
PIDESC
IMU
Quadro-copter
, =0
,
Sai PWM
3pha
3.25
-
8/3/2019 DATN quadrocopter
79/114
62
3.26
.
,
3.27
-
8/3/2019 DATN quadrocopter
80/114
63
pitch, roll
:
Start
(throttle, roll, pitch, yaw)
(roll, pitch, yaw)
3.28
-
8/3/2019 DATN quadrocopter
81/114
64
4.1. Quadrocopter
4.1.1 Xo [10]Vector
).
ra
tsinXZ = sin(Axz) = Rx/ Rxz ; cosZX = cos(Axz) = Rz/ Rxz
sinYZ = sin(Ayz) = Ry/ Ryz ; cosZY = cos(Ayz) = Rz/ Ryz
=> tanXZ = tan(Axz) = sinXZ / cosXZ = Rx/ Rz
tanYZ = tan(Ayz) = sinYZ / cosYZ = Ry/ Rz
=> Axz = arctanXZ = atan2(Rx, Rz)
Ayz = arctanYZ = atan2(Ry, Rz)
Rx Ry RzngADC 10-bit:
Rx = (ADC_Rx * Vref/ 1023VzeroGx) / SensitivityRy = (ADC_Ry * Vref/ 1023VzeroGy) / SensitivityRz = (ADC_Rz * Vref/ 1023VzeroGz) / Sensitivity
gyroscope :RateAxz = (ADC_GyroXZ * Vref/ 1023VzeroRatexz) / SensitivityRateAyz = (ADC_GyroYZ * Vref/ 1023VzeroRateyz) / Sensitivity
-
8/3/2019 DATN quadrocopter
82/114
65
4.1.2 Kalman
: [11]
{// Inputsu = gyros;y = angle;
// Update the state estimate// x = A * x + B * ux_00 += (A_01 * x_10) + (B_00 * u);
// Compute the innovation -- error between measured value and state
// inn = y - c * xinn_00 = y - x_00;
// Compute the covariance of the innovation// s = C * P * C' + Szs_00 = P_00 + Sz;
// Compute AP matrix for use below// AP = A * PAP_00 = P_00 + A_01 * P_10;AP_01 = P_01 + A_01 * P_11;AP_10 = P_10;AP_11 = P_11;
// Compute the kalman gain matrix// K = A * P * C' * inv(s)K_00 = AP_00 / s_00;K_10 = AP_10 / s_00;
// Update the state estimate// x = x + K * innx_00 += K_00 * inn_00;x_10 += K_10 * inn_00;
// Compute the new covariance of the estimation error// P = A * P * A' - K * C * P * A' + Sw
APAT_00 = AP_00 + (AP_01 * A_01);APAT_01 = AP_01;APAT_10 = AP_10 + (AP_11 * A_01);APAT_11 = AP_11;KCPAT_00 = (K_00 * P_00) + (K_00 * P_01) * A_01;KCPAT_01 = (K_00 * P_01);KCPAT_10 = (K_10 * P_00) + (K_10 * P_01) * A_01;KCPAT_11 = (K_10 * P_01);P_00 = APAT_00 - KCPAT_00 + Sw_00;P_01 = APAT_01 - KCPAT_01;P_10 = APAT_10 - KCPAT_10;P_11 = APAT_11 - KCPAT_11 + Sw_11;
// Outputreturn = x_00;}
-
8/3/2019 DATN quadrocopter
83/114
66
4.1.3 Kalman
- = A_01 = B_00-= 0.3 rad = 17.2o
- Q angle = 0.0001
Q gyro = 0.0003
2
accelerometer
con quay gyroscope
-
8/3/2019 DATN quadrocopter
84/114
67
4.2. LabVIEW [7] [8]4.2.1
3 LabVIEW
ATmegaLabVIEW
4 LabVIEW
-
8/3/2019 DATN quadrocopter
85/114
68
SA
- time out, stop bit, VISA resource name ().
4.5 K
- VISA
6 K
- V
4.7 K
4.8 K
Code script Node:
4.9 script Node
-
8/3/2019 DATN quadrocopter
86/114
69
4.2.2 QuadrocopterQuadrocopte 4
hai pitch, roll.
4.10 Quadrocopter
Quad
-
8/3/2019 DATN quadrocopter
87/114
70
:
4.11 Quad
Quadrocopter.
-
8/3/2019 DATN quadrocopter
88/114
71
4.12 Block Diagram c
i sub VI:
- VI Artificial Horizon/Roll
4.13 ial Horizon
-
8/3/2019 DATN quadrocopter
89/114
72
- K: Quad
4.14
Node
rong LabVIEW c 3D LabVIEW.
5 3D
-
8/3/2019 DATN quadrocopter
90/114
73
4.3. 4.3.1 Motor
90o
E-4
Motor
E-4
Motor
E-4
Motor
Scale
1KgBalance
Thrust
16
khung Quadrocoptero.
HiModel GX-40A,motor FC2835-10T GWS EP-1060Rx3. ATmegaTimer 16-
Phase and Frequency Correct, gi
ESC. [15]
Hobbywing Pentium30sang EPP1045. ATmegaTimer 16- Fast PWM 2 4.
Sau
-
8/3/2019 DATN quadrocopter
91/114
74
4.1 (1)
Thrust (g) Front Back Right Left 10 1107 - 1107 - 1107 - 1107 -
20 1123 16 1123 16 1123 16 1123 16
30 1135 12 1134 11 1132 9 1131 8
40 1142 7 1141 7 1140 8 1140 950 1155 13 1148 7 1148 8 1145 5
60 1164 9 1158 10 1156 8 1154 9
70 1172 8 1167 9 1168 12 1164 10
80 1181 9 1173 6 1175 7 1172 8
90 1190 9 1184 11 1184 9 1183 11
100 1202 12 1192 8 1192 8 1188 5
110 1205 3 1203 11 1199 7 1194 6
120 1213 8 1205 2 1204 5 1204 10
130 1227 14 1212 7 1206 2 1208 4
140 1233 6 1218 6 1216 10 1214 6
150 1240 7 1230 12 1233 17 1223 9
160 1253 13 1238 8 1241 8 1237 14
170 1261 8 1250 12 1250 9 1245 8
180 1268 7 1258 8 1260 10 1253 8
190 1270 2 1268 10 1270 10 1261 8
200 1276 6 1273 5 1273 3 1269 8
210 1285 9 1283 10 1281 8 1272 3
220 1294 9 1289 6 1286 5 1280 8
230 1302 8 1297 8 1294 8 1290 10
240 1310 8 1307 10 1302 8 1300 10
250 1323 13 1315 8 1307 5 1317 17
260 1335 12 1328 13 1319 12 1323 6
270 1343 8 1337 9 1330 11 1336 13
280 1357 14 1343 6 1340 10 1341 5
290 1362 5 1355 12 1349 9 1347 6
300 1366 4 1361 6 1354 5 1352 5
310 1368 2 1370 9 1360 6 1359 7
320 1376 8 1379 9 1371 11 1367 8
330 1387 11 1386 7 1380 9 1376 9
340 1398 11 1397 11 1390 10 1385 9
350 1403 5 1402 5 1399 9 1396 11
360 1410 7 1410 8 1409 10 1402 6
370 1415 5 1417 7 1417 8 1418 16
380 1423 8 1425 8 1424 7 1426 8
390 1431 8 1434 9 1430 6 1431 5
400 1440 9 1440 6 1434 4 1437 6
410 1448 8 1445 5 1439 5 1442 5
420 1452 4 1452 7 1448 9 1449 7
430 1462 10 1460 8 1459 11 1454 5
440 1464 2 1467 7 1466 7 1460 6
450 1468 4 1470 3 1471 5 1466 6
-
8/3/2019 DATN quadrocopter
92/114
75
4.2 (2)
Thrust (g) Front Back Right Left 10 2100 - 2214 - 2214 - 2214 -
20 2130 30 2235 21 2235 21 2235 21
30 2160 30 2250 15 2255 20 2255 20
40 2190 30 2270 20 2278 23 2275 20
50 2222 32 2280 10 2285 7 2285 10
60 2255 33 2300 20 2310 25 2300 15
70 2285 30 2320 20 2327 17 2315 15
80 2315 30 2340 20 2343 16 2344 29
90 2345 30 2350 10 2355 12 2355 11
100 2375 30 2370 20 2375 20 2370 15
110 2403 28 2380 10 2390 15 2382 12
120 2430 27 2400 20 2402 12 2396 14130 2455 25 2406 6 2414 12 2410 14
140 2485 30 2412 6 2430 16 2420 10
150 2510 25 2430 18 2443 13 2435 15
160 2530 20 2445 15 2460 17 2452 17
170 2560 30 2465 20 2477 17 2470 18
180 2590 30 2480 15 2495 18 2480 10
190 2620 30 2495 15 2510 15 2500 20
200 2645 25 2510 15 2525 15 2520 20
210 2670 25 2528 18 2541 16 2535 15
220 2696 26 2539 11 2550 9 2550 15230 2715 19 2550 11 2568 18 2570 20
240 2740 25 2570 20 2580 12 2584 14
250 2770 30 2590 20 2605 25 2600 16
260 2800 30 2605 15 2625 20 2620 20
270 2825 25 2620 15 2640 15 2640 20
280 2855 30 2635 15 2655 15 2656 16
290 2885 30 2652 17 2673 18 2670 14
300 2915 30 2670 18 2683 10 2677 7
310 2945 30 2675 5 2695 12 2690 13
320 2975 30 2690 15 2710 15 2705 15330 3010 35 2705 15 2725 15 2720 15
340 3040 30 2730 25 2740 15 2737 17
350 3070 30 2750 20 2757 17 2759 22
360 3100 30 2770 20 2777 20 2772 13
370 3120 20 2790 20 2797 20 2786 14
380 3150 30 2802 12 2810 13 2805 19
390 3180 30 2816 14 2822 12 2820 15
400 3200 20 2832 16 2832 10 2832 12
ESC Hobbywing Pentium 30A ESC HiModelGX-
-
8/3/2019 DATN quadrocopter
93/114
76
17
18
1100
1150
1200
1250
1300
1350
1400
1450
1500
10
30
50
70
90
110
130
150
170
190
210
230
250
270
290
310
330
350
370
390
410
430
450
Back Front Right Left
2100
2300
2500
2700
2900
3100
10
30
50
70
90
110
130
150
170
190
210
230
250
270
290
310
330
350
370
390
Back Front Right Left
-
8/3/2019 DATN quadrocopter
94/114
77
-
- ,
- Hobbywing Pentium 301045.
4.3.2
19
Trong Quad
yaw
cho
-
- Ziegler
-
- CohenCoon:
-
8/3/2019 DATN quadrocopter
95/114
78
, Ziegler
- .
-
0gi
.0
20
-
8/3/2019 DATN quadrocopter
96/114
79
21 roll
.o.
,
-
8/3/2019 DATN quadrocopter
97/114
80
22 Ipitch
-
8/3/2019 DATN quadrocopter
98/114
81
23 ID roll
-
8/3/2019 DATN quadrocopter
99/114
82
-
-
-
- 10o10o.
- Khi go.
24
c. t chung
-
- o tt p o.
- ,
-
8/3/2019 DATN quadrocopter
100/114
83
25 Quadrocopter
Trong
.
-
8/3/2019 DATN quadrocopter
101/114
84
5
5.1.
Quadrocopter.
--
-
--
-Quad
-
-
-
- LabVIEW
-
8/3/2019 DATN quadrocopter
102/114
85
5.2.
-
-
-
-
5.3. -Quadrocopter . . .
.
.v.v...
- wifi
-Quadrocopte
-
8/3/2019 DATN quadrocopter
103/114
86
[1] ,TP.HCM, 10/2009.
[2] Tommaso Bresciani, Modelling, Identification and Control of a Quadrotor
Helicopter, Lund University, Sweden, 10/2008.
[3]
[4] BLDC Motor Modelling and Control AMatlab/Simulink Implementation, Chalmers University, Sweden, 05/2005.
[5]
07/2009.
[6] .
[7] Prentice Hall, LabVIEW for Everyone: Graphical Program Made Easy and
Fun, Third Edition, 27/07/2006.
[8] National Instruments, LabVIEW Basics I & II Course Manual version 6.0,
09/2010.
[9] RC Groups, Quadrocopter and Tricopter Info Mega Link Index, 12/2010.URL http://www.rcgroups.com/forums/showthread.php?t=1097355
[10] Starlino,A Guide To using IMU (Accelerometer and Gyroscope Devices) in
Embedded Applications, 12/2010.
URL http://www.starlino.com/imu_guide.html
[11] SparkFun Electronics, Help with implementing Kalman filter for IMU 5?,
09/2010.
URL http://forum.sparkfun.com/viewtopic.php?t=6186
[12] .
URL http://www.clbmohinh.com/forum/[13] MikroKopter, 01/2011.
URL http://www.mikrokopter.de/
https://www.mikrocontroller.com/
[14] Multicopter Wiki,Multicopter Table, 12/2010.
URL http://multicopter.org/wiki/Multicopter_Table
[15] Machine Intelligence Laboratory, Generating PWM signals using Timers in
the ATmega chip, 08/2010.
URL http://mil.ufl.edu/~achamber/servoPWMfaq.html
-
8/3/2019 DATN quadrocopter
104/114
87
A
BN VQUADROCOPTER
-
8/3/2019 DATN quadrocopter
105/114
88
VATMEGA128
module gyro e ATmega128
- Flash: 128 Kbytes- EEPROM: 4Kbytes- SRAM: 4Kbytes- 64 thanh ghi I/O- - - -bit (Timer/Counter 0,2)- -bit (Timer/Counter 1,3)- - - - -bit -bit )- -bit- -bit- - - - WI- - - - V - 5.5V
-
8/3/2019 DATN quadrocopter
106/114
89
ATmega, -bit
ATmega128
B.1 ADC trong ATmega128Trong ATmega -bit.
ADCL.
-
.
-
8/3/2019 DATN quadrocopter
107/114
90
- ---bit -700KHz -so10-bit). bit
B.2
- Free running: ,
- Single conversion:
1/ ADC Multiplexer Selection RegisterADMUX:thanh ghi o analog.
2/ ADC Control and Status Register AADCSRA:
3/ The ADC Data RegisterADCL ADCH:
WizardAVR ,
-
- xung clock,
(8-bit, 10-bit)
-
8/3/2019 DATN quadrocopter
108/114
91
B.2 USART trong ATmega128B.2.1
USART (Universal Synchronous and Asynchronous serial Receiver and
Transmitter - btnntbvbb)
- : (Trans
-
B.2.2 ATmega128ATmega
-
8/3/2019 DATN quadrocopter
109/114
92
SART
Clock Generation, Transmitter, Receiver. parity errors.
1/ Internal Clock Generation The USART Baud Rate GeneratorUBRR: thanh ghi.
2/ USART I/O Data RegisterUDRn:
3/ USART Control and Status Register AUCSRnA
4/ USART Control and Status Register BUCSRnB
5/ USART Control and Status Register CUCSRnC6/ USART Baud Rate RegistersUBRRnL UBRRnH:
-
8/3/2019 DATN quadrocopter
110/114
93
5 UART
- .
- /.
B.3 PWM trong ATmega128ATmega-bit
-bit.
(Timer/Counter).
- BOTTOM: bTimer/Counter
- MAX: Timer/Counter Timer/Counter 8-bit MAX 16-bit MAX
- TOP: khi Timer/Counter MAX, TOP thanh
ghi
-
8/3/2019 DATN quadrocopter
111/114
94
Timer/Counter 1 (Timer/Counter 3 ):
1/ Timer/Counter 1 Control Register ATCCR1A:
Bit 7:2 COMnX1:0 (X= A,B,C): Compare Output Mode for channel X: OCnB, OCnC (n=1,3).
Bit 1:0 WGMn1:0: Waveform Generation Mode: kTimer/Counter 1.
2/ Timer/Counter 1 Control Register BTCCR1B:
Bit 2:0CS12:0: Clock Select: imer/Counter.
-
8/3/2019 DATN quadrocopter
112/114
95
3/ Timer/Counter1TCNT1H TCNT1L: t16-bit thanh ghi TCNT1L. Thanh ghi. (n=1,3) ghi OCRnX (n=1,3. X=A,B,C)
4/ Output Compare Registers:
Output Compare Register 1 AOCR1AH OCR1AL
Output Compare Register 1 BOCR1BH OCR1BL
Output Compare Register 1 BOCR1CH OCR1CL
-ng xung ng
5/ Input Capture Register 1 ICR1H ICR1L: thanh ghi Input C
TOP 6/ Timer/Counter Interrupt Mask RegisterTIMSK:
Ba (PWM) imer/Counter 1:
- Fast PWM:1 BOTTOMTOP BOTTOM
6
Fast PWM 1, 8, 64, 256, 1024):
- Phase Correct PWM: (hight resolution). , BOTTOM TOP BOTTOM. H Fast PWM. ICRn X.
-
8/3/2019 DATN quadrocopter
113/114
96
7
Phase Correct PWM:
- Phase and Frequency Correct PWMBOTTOM TOP
8
-
8/3/2019 DATN quadrocopter
114/114
Phase Correct PWM:
- Hai c PWM.
- Tast PWM.
- F thanh ghi ICRn, ta.
CodeWizardAVR: