LVTN PDF

LVTN: Robot Hai Bnh T Cn Bng (Two wheeled self balancing Robot )
LI CM N
Trong sut thi gian hc tp ti trng i hc Bch Khoa HQG Tp.
HCM, ti c cc Thy C khoa in in t, v c bit l cc Thy C
b mn iu khin t ng ging dy tn tnh, cho ti nhiu kin thc mi b
ch, bn cnh ti cng c cc bn b cng kha, cng lp ng gp nhiu
kin cng nh cc ti liu c gi tr. Xin gi n cc Thy C v cc bn li
cm n chn thnh nht.
Ti xin gi n c Bi Thanh Huyn li bit n su sc v dnh thi
gian qu bu hng dn, to iu kin thun li cng nh cho ti nhng li
khuyn b ch hon thnh lun vn ny.
Cui cng, ti xin cm n Cha M v cc anh ch em trong gia nh
ng vin v to iu kin gip ti vt qua nhng kh khn trong sut qu
trnh hc tp v nghin cu va qua.

Tp. H Ch Minh, ngy 11 thng nm 2013
Hunh Vn S

2
TM TT LUN VN
Robot Hai Bnh T Cn Bng (Two wheeled self balancing Robot ) c thit
k ch vi 2 bnh song song v ng trc nhau. Nguyn l gi cn bng cho Robot
l vic gi cho gc lch gia trc vung gc vi trc 2 bnh, vi phng ca trng lc
l khng i.
ti nghin cu xy dng hon chnh Robot hai bnh t cn bng c iu
khin v gim st t xa qua wifi vi hai ch hot ng
1. Ch iu khin Robot bng tay
ch ny ngi iu khin ra lnh t xa bng wifi cho Robot thng qua mt
giao din UI trn nn web, Robot c th thu thp d liu t mt s cm bin c
trang b, camera, v tr v gi v cho ngi iu khin.
2. Ch Robot t ng
ch ny Robot hot ng t hnh di chuyn ti mt v tr c cho trc
thng qua GPS, la bn in t, v cm bin o khong cch.
B no Robot l mt ng dng xy dng trn Android device c kt ni vi
mt main MCU PIC24F qua USB c vai tr nh ngoi vi iu khin ng c v thu
thp d liu.

3

MC LC

LI CM N ..................................................................................................... 2
TM TT LUN VN ..................................................................................... 3
MC LC ..................................................................................................... 4
DANH SCH HNH NH ................................................................................. 6
DANH SCH BNG BIU ............................................................................... 9
CHNG 1 TNG QUAN ........................................................................... 10
1.1 Mc tiu ................................................................................................. 10
1.2 Gii thiu tng quan .............................................................................. 10
CHNG 2 C S L THUYT .............................................................. 12
2.1 M hnh xe hai bnh t cn bng ........................................................... 12
2.1.1 Nguyn l hot ng ....................................................................... 12
2.1.2 M hnh ha Robot hai bnh t cn bng ....................................... 13
2.2 Cc phng php iu khin ................................................................. 17
2.2.1 Gii thiu ........................................................................................ 17
2.2.2 Phng php iu khin PID .......................................................... 17
2.2.3 Phng php iu khin m ........................................................... 28
CHNG 3 M HNH THC NGHIM .................................................. 42
3.1 Thit k phn cng ................................................................................ 42
3.1.1 Thit k c kh ................................................................................ 42
3.1.2 Cc mch in ................................................................................. 43
4
3.2 Xy dng phm mm iu khin ........................................................... 45
3.2.1 Ti u gii thut PID (dng ri rc trn ngn ng C) .................... 45
3.2.2 Cm bin v b lc b sung ............................................................ 51
3.2.3 Microchip Accessory Framework for Android v Open Accessory
Framework .................................................................................................. 58
3.2.4 S dng GPS v Google Map xc inh v tr ca robot ............. 61
3.2.5 Xy dng web server trn Android dng th vin NanoHttpd ....... 72
3.2.2 J Query AJ AX ............................................................................... 76
3.2.2 Giao din iu khin nn web: .......................................................... 79
1. S gii thut ...................................................................................... 81
CHNG 5 KT QU ................................................................................. 84
5.1 ng dng trn Android .......................................................................... 84
5.2 Giao din iu khin .............................................................................. 86
CHNG 6 KT LUN ............................................................................... 88
1. Kt lun .................................................................................................. 88
2. Hng pht trin ti .......................................................................... 88
TI LIU THAM KHO ................................................................................ 89

5
DANH SCH HNH NH
Hnh 1.2.1 M hnh h robot thng minh .......................................................... 11
Hnh 1.2.2 S m t kin trc b iu khin robot thng minh theo 3 tng 11
Hnh 2.1.1 Nguyn l hot ng ........................................................................ 12
Hnh 2.1.2 M hnh ha Robot .......................................................................... 13
Hnh 2.2.1 S khi b iu khin PID .......................................................... 19
Hnh 2.2.2 th PV theo thi gian, 3 gi tr Kp (Ki,Kd l hng s) ............... 19
Hnh 2.2.3 th PV theo thi gian, ng vi 3 gi tr Ki (Kp,Kd l hng s) . 21
Hnh 2.2.4 th PV theo thi gian, ng vi 3 gi tr Kd (Kp,Ki l hng s) . 22
Hnh 2.2.5 Min xc nh, min tin cyc a tp m .......................................... 29
Hnh 2.2.6 Mt s dng hm thuc .................................................................... 30
Hnh 2.2.7 bn dng hm thuc thng dng ................................................... 31
Hnh 2.2.8 B iu khin m MISO .................................................................. 39
Hnh 2.2.9 M t nguyn l iu khin m ....................................................... 40
Hnh 3.1.1 M hnh Robot ................................................................................. 42
Hnh 3.1.2 HTC EVO 3D .................................................................................. 43
Hnh 3.1.3 Main MCU PIC24F ......................................................................... 44
Hnh 3.1.4 Mch cu L298 ................................................................................ 44
Hnh 3.1.5 Mch ngun 9v ................................................................................ 44
Hnh 3.2.1 p ng khi thay i setpoint t ngt ........................................... 46
Hnh 3.2.2 p ng khi thay i setpoint t ngt sau khi c ti u ........... 47
Hnh 3.2.3 p ng khi thay i thng s PID nng ........................................ 47
Hnh 3.2.4 p ng khi thay i PID nng sau khi hiu chnh ......................... 49
Hnh 3.2.5 p ng ng ra so vi so vi gi tr iu khin .............................. 50
6
Hnh 3.2.6 p ng output sau khi hiu chnh .................................................. 51
Hnh 3.2.7 Trc ta nh hng cho cc cm bin ....................................... 52
Hnh 3.2.8 c tnh gc nghing t cm bin gia tc ....................................... 52
Hnh 3.2.9 S gii thut kt hp cm bin ................................................... 53
Hnh 3.2.10 p ng gc nghing sau khi s kt hp cm bin ....................... 54
Hnh 3.2.11 B lc b sung ............................................................................... 54
Hnh 3.2.12 Accessory Framwork ..................................................................... 58
Hnh 3.2.13 USB host mode v Accessory mode .............................................. 59
Hnh 3.2.14 Xc nh v tr t 3 v tinh ............................................................. 63
Hnh 3.2.15 A-GPS ............................................................................................ 64
Hnh 3.2.16 M hnh cp nht v tr................................................................... 65
Hnh 3.2.17 ng k API key ........................................................................... 66
Hnh 3.2.18 Giao din Demo ............................................................................. 67
Hnh 3.2.19 Xc nh Route gia A v B .......................................................... 68
Hnh 3.2.20 Xc nh gc gia 2 im .............................................................. 69
Hnh 3.2.21 Xc nh gc t 2 im ta a l ............................................. 70
Hnh 3.2.22 Khong cch gia 2 im ta a l ......................................... 71
Hnh 3.2.23 Gmap3 demo .................................................................................. 75
Hnh 3.2.24 Highcharts Demo ........................................................................... 76
Hnh 3.2.25 Giao din web ................................................................................ 79
Hnh 3.2.26 V p ng trn giao din Web ..................................................... 80
Hnh 3.2.27 Giao din trn Android device ....................................................... 80
Hnh 5.1.1 Thng bo kt ni USB ................................................................... 84
Hnh 5.1.2 ng dng sau khi khi ng............................................................ 85
7
Hnh 5.1.3 ng dng ch iu khin bng tay .......................................... 85
Hnh 5.2.1 Giao din iu khin ....................................................................... 86
Hnh 5.2.2 giao din iu khin chn ch iu khin bng tay .................... 87
Hnh 5.2.3 chn ta im B .......................................................................... 87

8
DANH SCH BNG BIU

Bng 2-1 So snh cc phng php iu chnh ................................................. 26
Bng 2-2 Tc ng ca vic tng mt thng s c lp .................................... 27
Bng 2-3 Phng php Zirgler-Nichols ............................................................. 27
Bng 3-1 So snh gi tr cc khu khi thay i thng s PID ........................... 48
Bng 3-2 Cu hnh phn cng USB ................................................................... 59
Bng 3-3 So snh gia GET v POST ............................................................... 78

9
CHNG 1 TNG QUAN
1.1 Mc tiu
Mc tiu ca ti nghin cu nguyn l hot ng, m hnh ton hc ca
Robot hai bnh t cn bng t thit k phn cng v xy dng hon chnh m hnh
Robot hai bnh t cn bng.
Trong , xy dng mt ng dng Web sever chy trn h iu hnh Android.
C nhim v ly d liu t cm bin, camera, microphonetrn Android Device, giao
tip vi PIC24F thng qua USB ra lnh iu khin Robot. iu khin v gim st
Robot trn giao din nn web qua wifi.
1.2 Gii thiu tng quan
Hng nghin cu v robot t hnh ng dng trong i sng hng ngy, c kh nng
tng tc vi ngi ang l mt trong nhng hng nghin cu rt c cng ng
cc nh khoa hc, k s robotics quan tm trn th gii. Do s pht trin vt bc v
cng ngh ch to cm bin, c bit l cc loi cm bin 3D, nh v, camera,
khin cho cc tng ng kinh ngc v tnh tng tc vi ngi v kh nng vn
hnh ca robot c dn hin thc ha.
Trong lnh vc robot phc v con ngi, robot khng ch di chuyn, m cn mang cc
vt dng thng ngy, trnh c cc vt cn (c nh, di chuyn c, ngi),
nhn bit c s thay i ca mi trng hot ng, quyt nh v vn hnh theo cc
tc v c hoch nh trc.
Kin trc hu ht cc b iu khin robot c th c chia thnh cc tng nh sau :
- Tng Realtime: l tng thp nht, gn lin vi phn cng (in t v c cu c kh).
Tng ny bao gm thu thp d liu t cc cm bin, iu khin cc c cu chp hnh.
Tng realtime i hi kht khe v thi gian thc.

10

Hnh 1.2.1 M hnh h robot thng minh
- Tng Ra quyt nh: t cc d liu cm bin thu thp c v cc lnh iu khin c
cu chp hnh, tng ny s da vo cc m hnh ton, cc thut ton a ra cc lnh
iu khin cc c cu chp hnh.
- Tng Chin lc: l tng s da vo cc thut ton tr tu nhn to AI ra cc quyt
nh v tc v tip theo cho robot.

Hnh 1.2.2 S m t kin trc b iu khin robot thng minh theo 3 tng
11
CHNG 2 C S L THUYT
Chng ny trnh by cc vn c s l thuyt lin quan n ti bao gm :
Nguyn l hot ng, m hnh ton hc ca Robot hai bnh t cn bng.
Cc phng php iu khin Robot nh : phng php iu khin PID,
phng php iu khin m.
2.1 M hnh xe hai bnh t cn bng
2.1.1 Nguyn l hot ng
M hnh xe hai bnh t cn bng hot ng da vo m hnh con lc ngc gn
vi h hai bnh xe c iu khin c lp vi nhau.

Hnh 2.1.1 Nguyn l hot ng
Khi Robot ng cn bng th gc nghing ca thn Robot v trc thng ng
(trc ca trng lc trng trng) bng 0.
Khi Robot nghing v pha trc > 0, nu khng c iu khin th theo qun
tnh, hai bnh xe s chy v pha sau (pha ngc li ca thn Robot ang nghing),
dn n Robot s b ng. Nn trong trng hp ny, chng ta s iu khin cho hai
bnh xe chy v pha trc (pha m Robot ang nghing) nhm cho gc lch = 0,
Robot s cn bng tr li.
12
Khi Robot nghing v pha sau < 0, nu khng c iu khin th theo qun
tnh, hai bnh xe s chy v pha trc dn n Robot s b ng. Nn trong trng hp
ny chng ta s iu khin cho hai bnh xe chy v pha sau nhm cho gc lch bng
khng, Robot s cn bng tr li.
2.1.2 M hnh ha Robot hai bnh t cn bng

Hnh 2.1.2 M hnh ha Robot
Cc chuyn ng tuyn tnh ca khung xe c c trng bi v tr x (m)
v tc .( m/
2
)
xp , yp l v tri ca trng tm thn robot theo hng trc x v hng trc
y
Xoay quanh trc z c m t bi gc (rad) v vn tc gc
(rad/s).
Xoay quanh trc y c m t bi gc (rad) v vn tc gc
. (rad/s).
m l khi lng bnh xe (kg)
M l khi lng ca khung xe.(kg)
13
R l bn knh bnh xe.(m)
L l khong cch gia trc z v trng tm ca khung xe (m)
D l khong cch gia 2 bnh xe.(m)
Cl,Cr l moment tc dng ca ng c kt ni vi bnh xe tri v phi
(N/m)
g l gia tc trng trng (9.8m/
2
)
Xt bnh xe bn tri :
l l l
mx f H =
w
l l l
J
x C f R
R
=
Xt bnh xe bn phi :
mx f H
r r r
=
w
r r r
J
x C f R
R
=
i vi thn xe :

( ) sin ( ) cos ( )
( )
2
p l r
p l r l r l r
l r
l r
Mx H H
J V V L H H L C C
M y V V Mg
D
J H H
= +
= + + +
= +
= +

Ti trng tm :

sin
cos
p
p
x x L
y L
= +
=

Xem hai bn bnh robot l nh nhau ta c m hnh nh sau:
14

Hnh 2.1.3 M hnh ha robot gi s 2 bnh l nh nhau

Hnh 2.1.4 M hnh ha thn robot
1
0: (1)
2
1
0: (2)
2
0: (3)
x k H pH
y k pV V k
T k k M pH
F m x F F bx
F m y F F m g
M J M F r
= = +
= =
= =

Xt ti bnh xe :
0: (4)
0: (5)
x n n H
y n n V n
F m x F
F m y F m g
= =
= =

15
Sau khi kt hp cc phng trnh t 1-5,hai phng trnh chuyn ng thu
c:
2
2
2
( ) cos sin 2
1 1 1
( ) cos sin
2 2 2
n n n n n M
k M
k n n n
J L m Lm x gLm M
J M
m m x m L m L bx
R R

+ = +
+ + = +

n gin vic thit k b iu khin, cc phng trnh phi tuyn ca chuyn
ng phi c tuyn tnh, trng thi mong mun ca h thng ti nh gn bng 0.
V vy gi s
2
sin
cos 1
0

=
=
=

Ta c cc phng trnh tuyn tnh sau:
2
2
( ) 2
1 1
( )
2 2
n n n n n M
k M
k n n
J L m Lm x gLm M
J M
m m x m L bx
R R

+ = +
+ + =

Ta biu din 2 phng trnh ny di dng khng gian trng thi :
22 23 2
42 43 4
,
,
0 1 0 0 0
0 0
0 0 0 1 0
0 0
x x u
y Cx Du
x x
a a b x x
u
a a b

= +
= +
| | | | | | | |
| | | |
| | | |
= +
| | | |
| | | |
\ . \ . \ . \ .

Vi cc phn t ca ma trn A va B l :
16
22
23
42
43
2
4 2 2 2
10
,
14 5
5
,
14 5
5
,
14 5
7 5
,
14 5
10 5
,
14 5
5 7 5
.
14 5
k n
n
k n
k n
k n
k n
k n
n k n
k n n
b
a
m m
m
a
m m
b
a
Lm Lm
gm gm
a
Lm Lm
L r
b
m r m r
Lm m r m r
b
L m m r L m r
=
+
=
+
=
+
+
=
+
+
=
+
+ +
=
+

2.2 Cc phng php iu khin
2.2.1 Gii thiu
Robot hai bnh t cng bng l mt i tng c tnh n nh khng bn vng.
cng ging nh con lc ngc, Robot ch c mt im cn bng duy nht. l khi
trng tm ca Robot nm trong mt chn c to bi 2 bnh xe(=0). Nu trng
tm ca Robot lch khi mt chn th Robot s khng cn cn bng c na. khi
Robot ly li thng bng , Robot phi di chuyn theo hng m trng tm ca
Robot lch khi mt chn .
Nh vy gi Robot cn bng cn phi xy dng mt b iu khin khin hi
tip tnh ton quan h gia trng thi ca xe v vn tc cn cung cp cho Robot. C rt
nhiu phng php iu khin khc nhau nh LQR, fuzzy, PID.
2.2.2 Phng php iu khin PID
2.2.2.1 gii thiu tng quan
Mt b iu khin vi tch phn t l (b iu khin PID- Proportional Integral
Derivative) l mt c ch phn hi vng iu khin (b iu khin) tng qut c s
dng rng ri trong cc h thng iu khin cng nghip b iu khin PID c s
dng ph bin nht trong s cc b iu khin phn hi. Mt b iu khin PID tnh
17
ton mt gi tr "sai s" l hiu s gia gi tr o thng s bin i v gi tr t mong
mun. B iu khin s thc hin gim ti a sai s bng cch iu chnh gi tr iu
khin u vo. Trong trng hp khng c kin thc c bn v qu trnh, b iu
khin PID l b iu khin tt nht. Tuy nhin, t c kt qu tt nht, cc thng
s PID s dng trong tnh ton phi iu chnh theo tnh cht ca h thng-trong khi
kiu iu khin l ging nhau, cc thng s phi ph thuc vo c th ca h thng.
Gii thut tnh ton b iu khin PID bao gm 3 thng s ring bit, do i
khi n cn c gi l iu khin ba khu: cc gi tr t l, tch phn v o hm, vit
tt l P, I, v D. Gi tr t l xc nh tc ng ca sai s hin ti, gi tr tch phn xc
nh tc ng ca tng cc sai s qu kh, v gi tr vi phn xc nh tc ng ca tc
bin i sai s. Tng chp ca ba tc ng ny dng iu chnh qu trnh thng
qua mt phn t iu khin nh v tr ca van iu khin hay b ngun ca phn t gia
nhit. Nh vy, nhng gi tr ny c th lm sng t v quan h thi gian: P ph thuc
vo sai s hin ti, I ph thuc vo tch ly cc sai s qu kh, v D d on cc sai
s tng lai, da vo tc thay i hin ti.
Bng cch iu chnh 3 hng s trong gii thut ca b iu khin PID, b iu
khin c th dng trong nhng thit k c yu cu c bit. p ng ca b iu khin
c th c m t di dng nhy sai s ca b iu khin, gi tr m b iu
khin vt l im t v gi tr dao ng ca h thng. Lu l cng dng ca gii
thut PID trong iu khin khng m bo tnh ti u hoc n nh cho h thng.
Vi ng dng c th yu cu ch s dng mt hoc hai khu ty theo h thng.
iu ny t c bng cch thit t i li ca cc u ra khng mong mun v 0.
Mt b iu khin PID s c gi l b iu khin PI, PD, P hoc I nu vng mt cc
tc ng b khuyt. B iu khin PI kh ph bin, do p ng vi phn kh nhy i
vi cc nhiu o lng, tri li nu thiu gi tr tch phn c th khin h thng khng
t c gi tr mong mun.
18
2.2.2.2 l thuyt b iu khin PID

Hnh 2.2.1 S khi b iu khin PID
Phn ny ch m t dng song song hoc khng tng tc ca b iu khin
PID. S iu khin PID c t tn theo ba khu hiu chnh ca n, tng ca ba
khu ny to thnh bi cc bin iu khin (MV). Ta c:

trong , , v l cc thnh phn u ra t ba khu ca b iu
khin PID, c xc nh nh di y.
Khu t l

Hnh 2.2.2 th PV theo thi gian, 3 gi tr Kp (Ki,Kd l hng s)
Khu t l (i khi cn c gi l li) lm thay i gi tr u ra, t l vi
gi tr sai s hin ti. p ng t l c th c iu chnh bng cch nhn sai s
vi mt hng s K
p
, c gi l li t l.
19
Khu t l c cho bi:

trong
: tha s t l ca u ra
: li t l, thng s iu chnh
: sai s
: thi gian hay thi gian tc thi (hin ti)
li ca khu t l ln l do thay i ln u ra m sai s thay i nh. Nu
li ca khu t l qu cao, h thng s khng n nh (xem phn iu chnh vng).
Ngc li, li nh l do p ng u ra nh trong khi sai s u vo ln, v lm
cho b iu khin km nhy, hoc p ng chm. Nu li ca khu t l qu thp,
tc ng iu khin c th s qu b khi p ng vi cc nhiu ca h thng.
Droop( trt)
Nu khng c nhiu, iu khin t l thun ty s khng xc lp ti gi tr mong
mun ca n, nhng n vn duy tr mt ( trt) sai s n nh trng thi, l mt hm
ca li t l v li qu trnh. c bit, nu li qu trnh-trong khong thi
gian di b tri do thiu iu khin, nh vic lm mt mt l nung ti nhit phng-
c k hiu G v gi s sai s xp x l hng s, khi droop- trt xy ra khi
li khng i ny bng tha s t l ca u ra, vi sai s l tuyn
tnh, do Khi tha s t l, y vo thng s ti gi tr t,
c b chnh xc bi li qu trnh, n s ko thng s ra khi gi tr t. Nu
li qu trnh gim, khi lm lnh, th trng thi dng s nm di im t, ta gi l
"droop- trt".
Ch cc thnh phn dch chuyn (trung bnh di hn, thnh phn tn s khng)
ca li qu trnh mi tc ng ti trt-cc dao ng u hoc ngu nhin trn
hoc di thnh phn dch chuyn s b trit tiu. li qu trnh c th thay i theo
thi gian hoc theo cc thay i bn ngoi, v d nh nu nhit phng thay i, vic
lm lnh s nhanh hn hoc chm hn.
20
trt t l thun vi li qu trnh v t l nghch vi li t l, v l mt
khim khuyt khng th trnh c ca iu khin t l thun ty. trt c th
c gim bt bng cch thm mt tha s lch (cho im t trn gi tr mong
mun thc t), hoc sa i bng cch thm mt khu tch phn (trong b iu khin
PI hoc PID), s tnh ton lch thm vo mt cch hu hiu.
Bt chp trt, c l thuyt iu chnh ln thc t cng nghip ch ra rng
khu t l l cn thit trong vic tham gia vo qu trnh iu khin.
Khu tch phn

Hnh 2.2.3 th PV theo thi gian, ng vi 3 gi tr Ki (Kp,Kd l hng s)
Phn phi ca khu tch phn (i khi cn gi l reset) t l thun vi c bin
sai s ln qung thi gian xy ra sai s. Tng sai s tc thi theo thi gian (tch phn
sai s) cho ta tch ly b c hiu chnh trc . Tch ly sai s sau c
nhn vi li tch phn v cng vi tn hiu u ra ca b iu khin. Bin phn
phi ca khu tch phn trn tt c tc ng iu chnh c xc nh bi li tch
phn, .
Tha s tch phn c cho bi:

trong
: tha s tch phn ca u ra
21
: li tch phn, 1 thng s iu chnh
: sai s
: thi gian hoc thi gian tc thi (hin ti)
: mt bin tch phn trung gian
Khu tch phn (khi cng thm khu t l) s tng tc chuyn ng ca qu trnh
ti im t v kh s d sai s n nh vi mt t l ch ph thuc vo b iu khin.
Tuy nhin, v khu tch phn l p ng ca sai s tch ly trong qu kh, n c th
khin gi tr hin ti vt l qua gi tr t (ngang qua im t v to ra mt lch
vi cc hng khc). tm hiu thm cc c im ca vic iu chnh li tch
phn v n ca b iu khin, xin xem phn iu chnh vng lp.
Khu vi phn

Hnh 2.2.4 th PV theo thi gian, ng vi 3 gi tr Kd (Kp,Ki l hng s)
Tc thay i ca sai s qua trnh c tnh ton bng cch xc nh dc
ca sai s theo thi gian (tc l o hm bc mt theo thi gian) v nhn tc ny
vi li t l . Bin ca phn phi khu vi phn (i khi c gi l tc )
trn tt c cc hnh vi iu khin c gii hn bi li vi phn, .
Tha s vi phn c cho bi:

22
trong
: tha s vi phn ca u ra
: li vi phn, mt thng s iu chnh
: Sai s
: thi gian hoc thi gian tc thi (hin ti)
Khu vi phn lm chm tc thay i ca u ra b iu khin v c tnh
ny l ang ch nht t ti im t ca b iu khin. T , iu khin vi phn
c s dng lm gim bin vt l c to ra bi thnh phn tch phn v tng
cng n nh ca b iu khin hn hp. Tuy nhin, php vi phn ca mt tn hiu
s khuch i nhiu v do khu ny s nhy hn i vi nhiu trong sai s, v c
th khin qu trnh tr nn khng n nh nu nhiu v li vi phn ln. Do
mt xp x ca b vi sai vi bng thng gii hn thng c s dng hn. Chng hn
nh mch b sm pha.
Tm tt
Khu t l, tch phn, vi phn c cng li vi nhau tnh ton u ra ca b
iu khin PID. nh ngha rng l u ra ca b iu khin, biu thc cui cng
ca gii thut PID l:

trong cc thng s iu chnh l:
Kp : li t l, gi tr cng ln th p ng cng nhanh do sai s cng ln,
b khu t l cng ln. Mt gi g li t l qu ln s dn n qu trnh mt n nh
v dao ng.
Ki : li tch phn, gi tr cng ln ko theo sai s n nh b kh cng nhanh.
i li l vt l cng ln: bt k sai s m no c tch phn trong sut p ng
qu phi c trit tiu tch phn bng sai s dng trc khi tin ti trng thi n
nh.
23
KD : li vi phn, gi tr cng ln cng gim vt l, nhng li lm chm
p ng qu v c th dn n mt n nh do khuch i nhiu tn hiu trong php
vi phn sai s
2.2.2.3 iu chnh vng lp
iu chnh mt vng iu khin l iu chnh cc thng s iu khin ca n
( li/di t l, li tch phn/reset, li vi phn/tc ) ti gi tr p ng iu
khin ti u. n nh (dao ng bin) l mt yu cu cn bn, nhng ngoi ra, cc
h thng khc nhau, c nhng hnh vi khc nhau, nhng ng dng khc nhau c
nhng yu cu khc nhau, v vi yu cu li mu thun vi nhau. Hn na, vi qu
trnh c mt mc phi tuyn no y khin cc thng s lm vic tt iu kin y
ti s khng lm vic khi qu trnh khi ng t khng ti; iu ny c th khc phc
bng chng trnh li (s dng cc thng s khc nhau cho nhng khu vc hot
ng khc nhau). Cc b iu khin PID thng cung cp cc iu khin c th chp
nhn c thm ch khng cn iu chnh, nhng kt qu ni chung c th c ci
thin bng cch iu chnh k lng, v kt qu c th khng chp nhn c nu iu
chnh km.
iu chnh PID l mt bi ton kh, ngay c khi ch c 3 thng s v v
nguyn tc l d miu t, bi v n phi tha mn cc tiu chun phc tp nm
trong Nhng hn ch ca iu khin PID. V vy c nhiu phng php khc nhau
iu chnh vng lp, v cc k thut phc tp hn l ti cho nhiu pht minh sng
ch; phn ny miu t vi phng php th cng truyn thng iu chnh vng lp.
n nh
Nu cc thng s ca b iu khin PID ( li ca khu t l, tch phn v vi
phn) c chn sai, u vo qu trnh iu khin c th mt n nh, v cc khc bit
u ra ca n, c hoc khng c dao ng, v c gii hn ch bi s bo ha hoc
t gy c kh. S khng n nh c gy ra bi s d tha li, nht l khi xut
hin tr ln.
24
Ni chung, n nh ca p ng (ngc vi bt nh) phi tha mn v
qu trnh phi khng c dao ng v bt k s kt hp no gia cc iu khin qu
trnh v im t, mc d i khi n nh bin c th c chp nhn hoc yu cu.
Ti u ha hnh vi
Ti u ha hnh vi trong thay i qu trnh hoc thay i im t khc nhau
ty thuc vo ng dng.
Hai yu cu c bn l n nh (trit tiu nhiu-n nh ti mt im t cho
trc) v t hiu chnh lnh (thc hin cc thay i im t)-hai yu cu ty thuc
vo vic cc bin iu khin theo di gi tr mong mun c tt hay khng. Cc tiu
chun c bit v t hiu chnh lnh bo gm thi gian khi ng v thi gian xc lp.
Mt vi qu trnh phi ngn khng cho php cc bin qu trnh vt l qu im t
nu, th d, iu ny c th mt an ton. Cc qu trnh khc phi ti thiu ha nng
lng tiu hao khi tin ti mt im t mi.
Tng quan cc phng php
C nhiu phng php khc nhau iu chnh vng lp PID. Nhng phng
php hu hiu nht thng bao gm nhng trin khai ca vi dng m hnh x l, sau
chn P, I, v D da trn cc thng s ca m hnh ng hc. Cc phng php iu
chnh th cng tng i khng hiu qu lm, c bit nu vng lp c thi gian p
ng c tnh bng pht hoc lu hn.
La chn phng php thch hp s ph thuc phn ln vo vic c hay khng
vng lp c th iu chnh "offline", v p ng thi gian ca h thng. Nu h thng
c th thc hin offline, phng php iu chnh tt nht thng bao gm bt h thng
thay i u vo tng bc, tn hiu o lng u ra l mt hm thi gian, s dng
p ng ny xc nh cc thng s iu khin.

25

Phng
php
u im Khuyt im
iu chnh
th cng
Khng cn hiu bit v ton. Phng
php online.
Yu cu nhn vin
c kinh nghim.
Ziegler-
Nichols
Phng php chng minh. Phng
php online.
lm ri lon qu
trnh, mt s th nghim
v li, phi iu chnh
nhiu ln
Cc cng c
phn mm
iu chnh chc chn. Phng php
online hoc offline. C th bao gm phn
tch cc van v cm bin. Cho php m
phng trc khi ti xung thc thi.
Gi c cao, v phi
hun luyn.
Cohen-Coon x l cc m hnh tt.
Yu cu kin thc
ton hc. Phng php
offline. Ch tt i vi cc
qu trnh bc mt.
Bng 2-1 So snh cc phng php iu chnh
iu chnh th cng
Nu h thng phi duy tr trng thi online, mt phng php iu chnh l thit
t gi tr u tin ca Ki v Kd bng khng. Tng dn Kp cho n khi u ra ca
vng iu khin dao ng, sau Kp c th c t ti xp x mt na gi tr
p t c p ng 1/4 gi tr suy gim bin . Sau tng Ki n gi tr ph hp
sao cho thi gian x l. Tuy nhin, Ki qu ln s gy mt n nh. Cui cng,
26
tng Kd, nu cn thit, cho n khi vng iu khin nhanh c th chp nhn c
nhanh chng ly li c gi tr t sau khi b nhiu. Tuy nhin, Kd qu ln s gy p
ng d v vt l. Mt iu chnh cp tc ca vng iu khin PID thng hi qu l
mt t khi tin ti im t nhanh chng. Tuy nhin, vi h thng khng chp nhn xy
ra vt l, trong trng hp , ta cn mt h thng vng kn gim l, thit t mt gi
tr Kp nh hn mt na gi tr Kp gy ra dao ng.

Bng 2-2 Tc ng ca vic tng mt thng s c lp
Phng php ZieglerNichols
Mt phng php iu chnh theo kinh nghim khc l phng php Ziegler
Nichols, c a ra bi J ohn G. Ziegler v Nathaniel B. Nichols vo nhng nm
1940. Ging phng php trn, li Ki v Kd lc u c gn bng khng.
li P c tng cho n khi n tin ti li ti hn, Ku, u ra ca vng iu khin
bt u dao ng. Ku v thi gian dao ng Pu c dng gn li nh sau:

Bng 2-3 Phng php Zirgler-Nichols
Phn mm iu chnh PID
Hu ht cc ng dng cng nghip hin i khng cn iu chnh vng iu
khin s dng cc phng php tnh ton th cng nh trn na. Thay vo , phn
mm iu chnh PID v ti u ha vng lp c dng m bo kt qu chc chn.
Nhng gi phn mm ny s tp hp d liu, pht trin cc m hnh x l, v xut
27
phng php iu chnh ti u. Vi gi phn mm thm ch cn c th pht trin vic
iu chnh bng cch thu thp d liu t cc thay i tham kho.
iu chnh PID bng ton hc to ra mt xung trong h thng, v sau s
dng p ng tn s ca h thng iu khin thit k cc gi tr ca vng iu khin
PID. Trong nhng vng lp c thi gian p ng ko di nhiu pht, nn chn iu
chnh bng ton hc, bi v vic th sai thc t c th ko di nhiu ngy tm im
n nh cho vng lp. Gi tr ti u th kh tm hn. Vi b iu khin s cn c chc
nng t iu chnh, trong nhng thay i rt nh ca im t cng c gi ti
qu trnh, cho php b iu khin t mnh tnh ton gi tr iu chnh ti u.
Cc dng iu chnh khc cng c dng ty theo tiu chun nh gi kt qu
khc nhau. Nhiu pht minh hin nay c nhng sn vo trong cc module phn
mm v phn cng iu chnh PID.
2.2.3 Phng php iu khin m
Khi nim v logic m c gio s L.A Zadeh a ra ln u tin nm 1965,
ti trng i hc Berkeley, bang California - M. T l thuyt m c pht
trin v ng dng rng ri.
Nm 1970 ti trng Mary Queen, London - Anh, Ebrahim Mamdani dng
logic m iu khin mt my hi nc m ng khng th iu khin c bng k
thut c in. Ti c Hann Zimmermann dng logic m cho cc h ra quyt nh.
Ti Nht logic m c ng dng vo nh my x l nc ca Fuji Electronic vo
1983, h thng xe in ngm ca Hitachi vo 1987.
L thuyt m ra i M, ng dng u tin Anh nhng pht trin mnh m
nht l Nht. Trong lnh vc T ng ho logic m ngy cng c ng dng rng
ri. N thc s hu dng vi cc i tng phc tp m ta cha bit r hm truyn,
logic m c th gii quyt cc vn m iu khin kinh in khng lm c.
2.2.3.1 Khi nim c bn
hiu r khi nim M l g ta hy thc hin php so snh sau :
28
Trong ton hc ph thng ta hc kh nhiu v tp hp, v d nh tp cc s
thc R, tp cc s nguyn t P={2,3,5,...} .. Nhng tp hp nh vy c gi l tp hp
kinh in hay tp r, tnh R y c hiu l vi mt tp xc nh S cha n phn
t th ng vi phn t x ta xc nh c mt gi tr y=S(x).
Gi ta xt pht biu thng thng v tc mt chic xe mt : chm, trung
bnh, hi nhanh, rt nhanh. Pht biu CHM y khng c ch r l bao nhiu
km/h, nh vy t CHM c min gi tr l mt khong no , v d 5km/h - 20km/h
chng hn. Tp hp L={chm, trung bnh, hi nhanh, rt nhanh} nh vy c gi l
mt tp cc bin ngn ng. Vi mi thnh phn ngn ng ca pht biu trn nu n
nhn c mt kh nng th tp hp F gm cc cp c gi l tp m.
2.2.3.2 nh ngha tp m
Tp m F xc nh trn tp kinh in B l mt tp m mi phn t ca n l mt
cp gi tr ( , ( ))
F
x x , vi x X v ( )
F
x l mt nh x
( ): [ 01]
F
x B

Trong
F
gi l hm thuc v B l tp nn.
2.2.3.3 cc thut ng trong logic m

Hnh 2.2.5 Min xc nh, min tin cyc a tp m
29
cao tp m F l gi tr sup ( )
F
h x = , trong sup ( )
F
x ch gi tr nh
nht trong tt c cc chn trn ca hm ( )
F
x .
Min xc nh ca tp m F, k hiu l S l tp con tho mn :
sup ( ) { | ( ) 0}
F F
h x x B x = = >
Min tin cy ca tp m F, k hiu l T l tp con tho mn :
{ | ( ) 0}
F
T x B x = =
Cc dng hm thuc (membership function) trong logic m
C rt nhiu dng hm thuc nh : Gaussian, Pl-shape, S-shape, Sigmoidal, Z-
shape ...

Hnh 2.2.6 Mt s dng hm thuc
30

Hnh 2.2.7 bn dng hm thuc thng dng
Hm thnh vin dng tam gic: Hm thnh vin dng tam gic c xc nh
bi 3 tham s {a,b,c} nh sau:

S dng hm Max, Min ta c th vit li phng trnh trn nh sau :

Cc tham s {a,b,c} (a<b<c) xc nh ta x ti 3 gc ca tam gic.
2.2.3.4 Bin ngn ng
Bin ngn ng l phn t ch o trong cc h thng dng logic m. y cc
thnh phn ngn ng ca cng mt ng cnh c kt hp li vi nhau.
minh ho v hm thuc v bin ngn ng ta xt v d sau :
Xt tc ca mt chic xe mt, ta c th pht biu xe ang chy:
Rt chm (VS)
31
Chm (S)
Trung bnh (M)
Nhanh (F)
Rt nhanh (VF)
Nhng pht biu nh vy gi l bin ngn ng ca tp m. Gi x l gi tr ca
bin tc , v d x =10km/h, x = 60km/h ... Hm thuc tng ng ca cc bin ngn
ng trn c k hiu l : ( ), ( ), ( ), ( ), ( )
VS S M F VF
x x x x x

Hnh 2.2.8 Hm lin thuc v bin ngn ng
Nh vy bin tc c hai min gi tr :
Min cc gi tr ngn ng :
N ={ rt chm, chm, trung bnh, nhanh, rt nhanh }
Min cc gi tr vt l :
{ | 0} V x B x =

Bin tc c xc nh trn min ngn ng N c gi l bin ngn ng.
Vi mi x B ta c hm thuc :
{ ( ), ( ), ( ), ( ), ( )}
X VS S M F VF
x x x x x x =

V d hm thuc ti gi tr r x=65km/h l :
(65)
X
={ 0;0;0.75;0.25;0 }
32
2.2.3.1.4 cc php ton trn tp m
cho X,Y l hai tp m trn khng gian nn B, c cc hm thuc tng ng l
,
x y
. khi :
Php hp hai tp m X Y
Theo lut Max ( ) { ( ), ( )}
X Y X y
b Max b b
=
Theo lut Sum ( ) {1, ( ) ( )}
X Y X y
b Min b b
= +
Tng trc tip ( ) ( ) ( ) ( ). ( )
X Y X y X y
b b b b b
= +
Php giao hai tp m X Y
Theo lut Min ( ) { ( ), ( )}
X Y X y
b Min b b
=
Theo lut Lukasiewicz ( ) {0, ( ) ( ) 1}
X Y X y
b Max b b
= +
Theo lut Prod ( ) ( ). ( )
X Y X y
b b b
=
Php b tp m ( ) 1 ( )
X X
b b =
2.2.3.1.5 Lut hp thnh
Mnh hp thnh
V d iu khin mc nc trong bn cha, ta quan tm n 2 yu t :
+Mc nc trong bn L ={rt thp, thp, va}
+Gc m van ng dn G = {ng, nh, ln}
Ta c th suy din cch thc iu khin nh th ny :
Nu mc nc = rt thp Th gc m van = ln
Nu mc nc = thp Th gc m van = nh
Nu mc nc = va Th gc m van = ng
Trong v d trn ta thy c cu trc chung l Nu A th B . Cu trc ny gi
l mnh hp thnh, A l mnh iu kin, C A B = l mnh kt lun.
33
nh l Mamdani :
ph thuc ca kt lun khng c ln hn ph thuc iu kin
Nu h thng c nhiu u vo v nhiu u ra th mnh suy din c dng
tng qut nh sau :
... ...
i i i i
if N n and M m and Then R r and K k and = = = =

Lut hp thnh
Lut hp thnh l tn gi chung ca m hnh biu din mt hay nhiu hm
thuc cho mt hay nhiu mnh hp thnh.
Cc lut hp thnh c bn :
+Lut Max - Min
+Lut Max - Prod
+Lut Sum Min
+Lut Sum Prod
a.Thut ton xy dng mnh hp thnh cho h SISO
Lut m cho h SISO c dng If A Then B
Chia hm thuc ( )
A
x thnh n im
i
x , i =1,2,,n
Chia hm thuc ( )
B
x thnh m im y
j
, j =1,2,,n
Xy dng ma trn quan h m R
1 1 1 11 1
2 1 2 21 2
1 1
( , ) ... ... ( , ) ... ...
( , ) ... ... ( , ) ... ...
... ... ... ... ... ... ... ...
( , ) ... ... ( , ) ... ...
R R m m
R R m m
R n R n m n nm
x y x y r r
x y x y r r
R
x y x y r r

( (
( (
( (
= =
( (
( (

Hm thuc '( )
B
y u ra ng vi gi tr r u vo
k
x c gi tr
34
'( ) .
T
B
y a R = Vi '( ) {0,0,0,...,0,1,0,...}
T
B
y a = = S 1 ng vi v tr th
k.
Trong trng hp u vo l gi tr m A th l :
1 2 3
'( ) { , , ,..., }
B m
y l l l l =
vi maxmin{ , }
k i ik
l a r =
b.Thut ton xy dng mnh hp thnh MISO :
Lut m cho h MISO c dng :
1 1 2 2
" " if cd A and cd A and Then rs B = = =
Cc bc xy dng lut hp thnh R :
Ri rc cc hm thuc
1 1 2 2
( ), ( ), , ( ), ( )
A A An n B
x x x y
Xc nh tho mn H cho tng vct gi tr r u vo
1 2
{ , , , }
n
x c c c =
trong
i
c l mt trong cc im mu ca ( )
Ai i
x T suy ra
1 1 2 2
{ ( ), ( ), , ( )}
A A An n
H Min c c c =
Lp ma trn R gm cc hm thuc gi tr m u ra cho tng vct gi tr m
u vo: '( ) { , ( )}
B B
y Min H y = hoc '( ) . ( )}
B B
y H y =
2.2.3.1.6Gii m
Gii m l qu trnh xc nh gi tr r u ra t hm thuc ju
B
(y) ca tp m
B. C 2 phng php gii m :
1.Phng php cc i
Cc bc thc hin :
Xc nh min cha gi tr y, y l gi tr m ti '( )
B
y t Max
{ | '( ) }
B
G y Y y H = =
Xc nh y theo mt trong 3 cch sau :
35
+Nguyn l trung bnh
+Nguyn l cn tri
+Nguyn l cn phi

Hnh 2.2.9 M t cc phng php gii m
+Nguyn l trung bnh
1 2
'
2
y y
y
+
=
+Nguyn l cn tri : chn
1
' y y =
+Nguyn l cn phi: chn
2
' y y =
2. Phng php trng tm:
im y c xc nh l honh ca im trng tm min c bao bi trc
honh v ng '( )
B
y .
Cng thc xc nh :
( )
'
( )
s
s
y y dy
y
y dy
trong S l min xc nh ca tp m B
Phng php trng tm cho lut Sum-Min
Gi s c m lut iu khin c trin khai, k hiu cc gi tr m u ra cc
lut iu khin th k l
'
( )
B k
y th vi quy tc Sum-Min hm thuc s l :
'
1
'( ) ( )
m
B
B k
k
y y
=
=
, v y c xc nh :
36
'
'
'
'
1
1 1
1 1
1
( )
( ( ))
'
( )
( )
m
m m
k B k
B k
k
s k k
m m
m
k
B k
B k
k k
s
k
s
y y dy
y y M
y
y dy A
y dy

=
= =
= =
=
| |
|
\ .
= = =
| |
|
\ .

Trong
'
( )
i
B k
s
M y y dy =
,
'
( )
i
B k
s
A y dy =
i=1,2,,m

Hnh 2.2.10 Hm lin thuc c dng hnh thang

Xt ring cho trng hp cc hm thuc c dng hnh thang nh hnh 2.20 :
2 2 2 2
2 1 2 1
2 1
(3 3 3 3 )
6
(2 2 )
2
k
k
H
M m m b a m b ma
H
A m m a b
= + + +
= + +

Hai cng thc trn c th p dng cho lut Max-Min
Phng php cao:
Trong cng thc
'
'
'
'
1
1 1
1 1
1
( )
( ( ))
'
( )
( )
m
m m
k B k
B k
k
s k k
m m
m
k
B k
B k
k k
s
k
s
y y dy
y y M
y
y dy A
y dy

=
= =
= =
=
| |
|
\ .
= = =
| |
|
\ .

37
Nu cc hm thuc c dng Singleton th ta c
1
1
'
m
k k
k
m
k
k
y H
y
H
=
=
==

vi
'
( )
k k
B k
H y =
y l phng php gii m theo cao.
2.2.3.1.7 M hnh m Tagaki-Sugeno
M hnh m m ta ni n trong cc phn trc l m hnh Mamdani. u im
ca m hnh Mamdani l n gin, d thc hin nhng kh nng m t h thng khng
tt. Trong k thut iu khin ngi ta thng s dng m hnh m Tagaki-Sugeno
(TS).
Tagaki-Sugeno a ra m hnh m s dng c khng gian trng thi m ln m
t linh hot h thng. Theo Tagaki/Sugeno th mt vng m
k
LX c m t bi lut
:If ( ) ( )
k k k
sk
R x LX then x A x x B x u = = + (2.1)
Lut ny c ngha l: nu vct trng thi X nm trong vng
k
LX th h thng
c m t bi phng trnh vi phn cc b ( ) ( )
k k
x A x x B x u = + . Nu ton b cc
lut ca h thng c xy dng th c th m t ton b trng thi ca h trong ton
cc. Trong (2.1) ma trn ( )
k
A x v ( )
k
B x l nhng ma trn hng ca h thng trng
tm ca min
k
LX c xc nh t cc chng trnh nhn dng. T rt ra c :
w ( ( ) ( ) )
k k
k
x A x x B x u = +
(2.2)
vi w ( ) [0,1]
k
x l tho mn chun ho ca x* i vi vng m
k
LX
Lut iu khin tng ng vi (2.1) s l :
:if ( )
k k
ck
R x LX then u K x x = =

V lut iu khin cho ton b khng gian trng thi c dng:
38
1
w ( )
N
k
k
k
u x x
=
=

T (2.1) v (2.2) ta c phng trnh ng hc cho h kn:
w ( )w ( ( ) ( ) ( ))
k k l
k l
x x A x x B x K x x = +

2.2.3.1 B iu khin m
2.2.3.2.1 Cu trc mt b iu khin m
Mt b iu khin m gm 3 khu c bn:
+Khu m ho
+Thc hin lut hp thanh
+Khu gii m
Xt b iu khin m MISO sau, vi vct u vo
| |
1 2 n
X u u u =

Hnh 2.2.11 B iu khin m MISO
39
2.2.3.2.2 Nguyn l iu khin m

Hnh 2.2.12 M t nguyn l iu khin m
Cc bc thit k h thng iu khin m.
+Giao din u vo gm cc khu: m ha v cc khu hiu chnh nh t l,
tch phn, vi phn ...
+Thip b hp thnh : s trin khai lut hp thnh R
+Giao din u ra gm : khu gii m v cc khu giao din trc tip vi i
tng.
2.2.3.2.3 Thit k b iu khin m
Cc bc thit k:
B1 : nh ngha tt c cc bin ngn ng vo/ra.
B2 : Xc nh cc tp m cho tng bin vo/ra (m ho).
+ Min gi tr vt l ca cc bin ngn ng.
+ S lng tp m.
+ Xc nh hm thuc.
+ Ri rc ho tp m.
40
B3 : Xy dng lut hp thnh.
B4 : Chn thit b hp thnh.
B5 : Gii m v ti u ho.Nhng lu khi thit k BK m
Nhng lu khi thit k BK m
+ Khng bao gi dng iu khin m gii quyt bi ton m c th d
dng thc hin bng b iu khin kinh in.
+ Khng nn dng BK m cho cc h thng cn an ton cao.
+ Thit k BK m phi c thc hin qua thc nghim.
Phn loi cc BK m
+ iu khin Mamdani (MCFC)
+ iu khin m trt (SMFC)
+ iu khin tra bng (CMFC)
+ iu khin Tagaki/Sugeno (TSFC)

41
CHNG 3 M HNH THC NGHIM
3.1 Thit k phn cng
3.1.1 Thit k c kh
M hnh Robot c thit k nh hnh 3.1.1

Hnh 3.1.1 M hnh Robot
42
3.1.2 Cc mch in
Android Device:

Hnh 3.1.2 HTC EVO 3D
Thit b chy h iu hnh Android c s dng l in thoi Android HTC
EVO 3D vi cc thng s cu hnh :
CPU: Dual-Core, 1.2GHz Qualcomm MSM8660 (Snapdragon)
GPU Qualcomm Adreno 220
Memory: 1 GB RAM
Storage: 1 GB
3-axis accelerometer
3-axis gyroscope
Digital compass
Proximity sensor
Ambient light sensor
Rear camera: Dual 5 megapixel autofocus
Front camera: 1.3 megapixel
Wi-Fi (802.11b/g/n)
Micro USB
Other Wi-Fi Hotspot, GPS navigation
43
MAIN MCU : PIC 24F256GB110 : PIC24F l dng c h tr rt tt chun
USB OTG

Hnh 3.1.3 Main MCU PIC24F
Mch cu H dng IC L298

Hnh 3.1.4 Mch cu L298

Power Supply: Ngun xung dng ic LM2596

Hnh 3.1.5 Mch ngun 9v
44
3.2 Xy dng phm mm iu khin
3.2.1 Ti u gii thut PID (dng ri rc trn ngn ng C)
3.2.1.1 Ri rc ha
c th s dng PID trn vi iu khin ta phi thit kt mt b PID s, yu
cu b iu khin PID phi c ri rc ha.

Ri rc ha :
doubl e I nput , Out put , Set poi nt ;
doubl e er r Sum, l ast Er r ;
doubl e kp, ki , kd;
voi d Comput e( )
{
doubl e er r or = Set poi nt - I nput ;
er r Sum+= ( er r or * t i meChange) ;
doubl e dEr r = ( er r or - l ast Er r ) / t i meChange;
Out put = kp * er r or + ki * er r Sum+ kd * dEr r ;
l ast Er r = er r or ;
}
y l phng trnh PID c bn nht, dng PID s ta phi xp x o hm v
tch phn vi mt hng bin s thi gian:
t i meChange = now l ast Ti me
Nhng iu ny c th khng cn thit vi vi x l dng ngt, hot to mt
vng lp cho hm PID nhanh.nn ta c th b i gi tr timeChange ny.
3.2.1.2 X l vic thay i t ngt Setpoint gi l hin tng Derivative Kick
Nh trong hnh 2.2.5 th khi thay i im t Setpoint h thng s to ra mt
gi tr t bin ti output, v ta c error=Setpoint-Input nn bt k thay i trong im
t cng nh hng ngay lp tc ti error
45

Hnh 3.2.1 p ng khi thay i setpoint t ngt
Gii php :

error =Setpoint - Input;
errSum +=error;
dErr =(error - lastErr);
Output =kp * error +ki * errSum +kd * dErr;
lastErr =error;

thay th bng :
error =Setpoint - Input;
errSum +=error;
dInput =(Input - lastInput);
Output =kp * error +ki * errSum - kd * dInput;
lastInput =Input;
46

kt qu : p ng ng ra khng cn hin tng Derivative Kick

Hnh 3.2.2 p ng khi thay i setpoint t ngt sau khi c ti u
3.2.1.3 Thay i thng s Kp,Ki,Kd khi h thng ang chy
H thng PID s hot ng bt thng khi mi bt u thay i thng s PID
khi n ang chy (thay thng s PID nng)

Hnh 3.2.3 p ng khi thay i thng s PID nng
47

Bng 3-1 So snh gi tr cc khu khi thay i thng s PID
L do ca s bt thng l khu tch phn khng p ng kp trong
qu trnh thay i
Gii php : ta thay i

Bng :

Ta thy 2 biu thc l tng ng nhng lc ny khi thay i Ki th
khu tch phn kp thi thay i theo
Ta c gii thut PID s l :
er r or = Set poi nt - I nput ;
I Ter m+= ( ki * er r or ) ;
dI nput = ( I nput - l ast I nput ) ;
Out put = kp * er r or + I Ter m- kd * dI nput ;
l ast I nput = I nput ;

48
kt qu :

Hnh 3.2.4 p ng khi thay i PID nng sau khi hiu chnh

3.2.1.4 Windup
u ra ca PID s lun c tuyn tnh vi u vo ca duty khi s dng
iu khin bng PWM, nhng PID s khng th bit c chng ta s dng
tm gi tr ca duty nh th no nn gi tr u ra ca PID khng nh ta mong
mun v vy thng phi hiu chnh sau khi c c gi tr u ra.
49

Hnh 3.2.5 p ng ng ra so vi so vi gi tr iu khin
Gii php :
Output =kp * error +ITerm - kd * dInput;
if (output >maxLimit){
iTerm -=output maxLimit;
output =maxLimit;}
else if (output <minLimit){
iTerm +=minLimit output;
output =minLimit;}
Vi maxlimit v minlimit l gi tr min max ca duty.
Kt qu :

50
Hnh 3.2.6 p ng output sau khi hiu chnh
3.2.1.5 Gii thut PID sau khi ti u
Sau khi kt hp tt c cc gi php nhm ti u gii thut PID s ta c gii
thut PID cui cng c xy dng cho vi iu khin nh sau :

doubl e I nput , Out put , Set poi nt ;
doubl e I Ter m, l ast I nput ;
doubl e kp, ki , kd;
doubl e out Mi n, out Max;
voi d Comput e( )
{

doubl e er r or = Set poi nt - I nput ;
I Ter m+= ( ki * er r or ) ;
i f ( I Ter m> out Max) I Ter m= out Max;
el se i f ( I Ter m< out Mi n) I Ter m= out Mi n;
doubl e dI nput = ( I nput - l ast I nput ) ;
Out put = kp * er r or + I Ter m- kd * dI nput ;
i f ( Out put > out Max) Out put = out Max;
el se i f ( Out put < out Mi n) Out put = out Mi n;
l ast I nput = I nput ;

}

voi d Set Out put Li mi t s( doubl e Mi n, doubl e Max)
{
i f ( Mi n > Max) r et ur n;
out Mi n = Mi n;
out Max = Max;

i f ( Out put > out Max) Out put = out Max;
el se i f ( Out put < out Mi n) Out put = out Mi n;

i f ( I Ter m> out Max) I Ter m= out Max;
el se i f ( I Ter m< out Mi n) I Ter m= out Mi n;
}
3.2.2 Cm bin v b lc b sung
robot t c v tr cn bng, mc tiu ca vng iu khin l iu chnh v
tr v vn tc cc bnh xe gc nghing ca robot vn n nh mt gi tr xc nh
trc. vn tnh ton chnh xc gc nghing ca robot t cc cm bin l khu then
cht.
Android device c tch hp mt s cm bin
Cm bin gia tc - Acceleration sensor
Cm bin trng lc - Gravity sensor
51
Con quay hi chuyn - Gyroscope
Cm bin t trng Geomangnetic field sensor
Vi trc ta c nh hng nh hnh bn di

Hnh 3.2.7 Trc ta nh hng cho cc cm bin
3.2.2.1 c tnh gc nghing t cm bin gia tc (Accelerometer)

Hnh 3.2.8 c tnh gc nghing t cm bin gia tc
Gi s cm bin gia tc c t trn cng v vung gc vi trc thng ng
ca Robot. Gc nghing c th c tnh nh sau :
52
2 2
arctan arcsin arcsin
x x x
y
x y
A A A
A g
A A
= = =
+

Cm bin gia tc c u ra khng chnh xc v li b nhiu (noise) ln.
tnh gc lch trn Android device ngi ta thng kt hp gi tr u ra ca
cm bin gia tc v cm bin t trng (Magnetometer ), trong cm bin gia tc
cung cp cc vector gravity v t k hot ng nh mt la bn.
3.2.2.2 c tnh gc nghing t con quay hi chuyn (gyroscope)
Con quay hi chuyn o vn tc gc thay i trn cc trc ca Robot t ta
c th tnh c gc quay trong mt thi gian nht nh
2
1
( ) ( )
t
t
t G t dt =

Cm bin gyro c gi tr chnh xc hn v c thi gian p ng rt ngn nhng
p ng b tri dt (drift)
3.2.2.3 Sensor Fusion
gii quyt nhc im ca 2 loi cm bin trn, nhiu ca cm bin gia tc
v tri trn cm bin gyro. D liu ca Gyro dng nh gi thay i trong thi gian
ngn v gi tr cm bin gia tc c dng nh thng tin h tr trong thi gian di.
iu ny tng ng vic ta s dng mt b lc thng thp cho tn hiu ca cm bin
gia tc v t trng, v mt b lc thng cao cho tn hiu con quay hi chuyn

Hnh 3.2.9 S gii thut kt hp cm bin
53
V kt qu sau khi s dng 2 b lc l gc nghing chnh xc ca robot

Hnh 3.2.10 p ng gc nghing sau khi s kt hp cm bin
B lc b sung (Complementary Filter)

Hnh 3.2.11 B lc b sung
54
B lc b sung c thit kt hp nht gi tr cc cm bin c gi tr
gc nghing tt nht
*( yr * ) (1 )* _ angle A angle g o dt A x acc = + +

Trong A l yu t quyt nh phn trm s tin tng cho gyro v accel, vi
v d trn A l 0.98 iu c ngha vi mi vng lp ca b lc 98% gi tr gc o
n t gi tr gc o c cng vi gc o ca cm bin gyro cn li 2% gi tr n t
cm bin accel.
Hng s thi gian c th c hiu l ranh gii s tin tng vo gyro hay
accel, trong v d trn vi khong thi gian ngn hn 0,49s th gyro ng tin cy v
c lc. vi thi gian ln hn 0,49 th accel bt u c lc
Tn s ly mu gc nghing sau khi s dng b lc cho cm bin:
c logcat trn Android device cho gi tr gc nghing sau khi s dng b lc
b sung cho cm bin Accel v gyro trong 1 giy
12- 11 23: 29: 34. 027: D/ posi t i on( 20505) : >>>or i ent at i onVal s = 88. 57214
55
T gi tr tr v ta tnh c thi gian ly mu gc nghing trung bnh l 19,38
ms , tng ng vi tn s l 51,6 HZ.
3.2.2.4 Rotation Vector Sensor
Rotation vector l mt cm bin o c xy dng t s kt hp ca cc cm
bin cn thit c trang b trn Android device cng vi cc b lc, nhm to ra mt
cm bin dng nh hng tt nht cho thit b.
Ba yu t ca cm bin :
x * sin ( / 2)
56
y * sin ( / 2)
z * sin ( / 2)
s dng Rotation vector tnh gc nghing cho robot:
pr i vat e f i nal Sensor Event Li st ener mLi st ener = new Sensor Event Li st ener ( ) {
publ i c voi d onSensor Changed( Sensor Event event ) {
synchr oni zed ( t hi s) {
swi t ch ( event . sensor . get Type( ) ) {
/ / . . .
case Sensor . TYPE_ROTATI ON_VECTOR:
mROTATI ON_VECTOR = event . val ues;

Sensor Manager . get Rot at i onMat r i xFr omVect or ( mRot at i onMat r i x, event . val u
es) ;

Sensor Manager . r emapCoor di nat eSyst em( mRot at i onMat r i x, Sensor Manager . AX
I S_X, Sensor Manager . AXI S_Z, mRot at i onMat r i x) ;
Sensor Manager . get Or i ent at i on( mRot at i onMat r i x,
or i ent at i onVal s) ;

or i ent at i onVal s[ 0] =( f l oat ) Mat h. t oDegr ees( or i ent at i onVal s[ 0] ) ;


br eak;
}
So snh p ng gia sensor fusion v rotation vector:

T th cho thy gi tr gc nghing ca 2 phng php l gn nh tng
ng, sai s ti nh nhiu l khong 0.05
57
3.2.3 Microchip Accessory Framework for Android v Open Accessory Framework
3.2.3.4 gii thiu chung
Microchips Accessory Framework For Android : l mt th vin ca
Microchip cung cp mt c ch truyn d liu n v i t mt ng dng Android
thng qua USB cho vi iu khin PIC
Open Accessory API /open Accessory Framework : y l API /Framework
trong mi trng pht trin Android cho php cc ng dng Android truyn d liu
vo v ra thng qua cng usb, c cung cp thng qua SDK ca Google
Trong ti ny sinh vin khng i su vo phn tch hot ng ca
Microchips Accessory Framework For Android, Open Accessory API /open
Accessory Framework v USB host stack m s trnh by cch thc thit lp th vin
s dng cc th vin ny trn vi iu khin PIC24F v trong ng dng Android.
3.2.3.5 Microchips Accessory Framework For Android
Microchips Accessory Framework For Android c vit trn nn Microchips
USB host stack.
Android Accessory Driver khi host mode l client Driver trn cng trn
Microchip USB host stack

Hnh 3.2.12 Accessory Framwork
58

Hnh 3.2.13 USB host mode v Accessory mode
Th vin c vit ti u cho cc dng PIC24 v PIC32, trong ti ny sinh
vin s dng dng PIC24F c h tr full speed (12Mbits/s cc ch Device,
host, OTG) USB 2.0 OTG.
Yu cu phn cng :
PIC MCU I/0 Pin Usage
D+(io) USB D+differential data signal
D- (io) USB D- differential data signal
VBUS (input) Senses USB power (does not operate bus powered)
VUSB (input) Power input for the USB D+/D- transceivers
VBUS0N (output) Enables or disables VBus power supply
Bng 3-3 Cu hnh phn cng USB

59
stack lun c gi ta t n trong vng lp ca chng trnh :

i nt mai n( voi d)
{
//Initialize the USB stack
USBI ni t i al i ze( 0) ;
//Pass my accessory information to the Android client driver
Andr oi dAppSt ar t ( &myDevi ceI nf o) ;
whi l e( 1)
{
//Keep the USB stack running
USBTasks( ) ;
//Do my application specific stuff here
//...
}
}
Gi data : trong v d l gi 1 byte data (pushButtonValue)
whi l e( 1)
{
USBTasks( ) ;
//Do some extra stuff here to see if the buttons have updated
i f ( wr i t eI nPr ogr ess == TRUE )
{
4
i f ( Andr oi dAppI sWr i t eCompl et e( devi ce_handl e, &er r or Code, &si ze) == TRUE)
{
wr i t eI nPr ogr ess = FALSE;
i f ( er r or Code ! = USB_SUCCESS)
{
//Error
DEBUG_ERROR( " Er r or t r yi ng t o compl et e wr i t e" ) ;
}
}
}
i f ( ( but t onsNeedUpdat e == TRUE) && ( wr i t eI nPr ogr ess == FALSE) )
{
r esponse_packet . command = COMMAND_UPDATE_PUSHBUTTONS;
r esponse_packet . dat a = pushBut t onVal ues;
er r or Code = Andr oi dAppWr i t e( devi ce_handl e, ( BYTE*) &r esponse_packet , 2) ;
i f ( er r or Code ! = USB_SUCCESS )
{
DEBUG_ERROR( " Er r or t r yi ng t o send but t on updat e" ) ;
}
el se
{
but t onsNeedUpdat e = FALSE;
wr i t eI nPr ogr ess = TRUE;
}
}
}
Nhn data : nhn 1 byte data
whi l e( 1)
{
//Keep the stack running
USBTasks( ) ;
//Do some extra stuff here
i f ( r eadI nPr ogr ess == FALSE)
{
60
er r or Code = Andr oi dAppRead( devi ce_handl e,
( BYTE*) &command_packet ,
( DWORD) si zeof ( ACCESSORY_APP_PACKET) ) ;
4
i f ( er r or Code ! = USB_SUCCESS)
{
//Error
DEBUG_ERROR( " Er r or t r yi ng t o st ar t r ead" ) ;
}
el se
{
r eadI nPr ogr ess = TRUE;
}
}
i f ( Andr oi dAppI sReadCompl et e( devi ce_handl e, &er r or Code, &si ze) == TRUE)
{
r eadI nPr ogr ess = FALSE;
//We've received a command over the USB from the Android device.
i f ( er r or Code == USB_SUCCESS)
{
//We've received data, process it here (or elsewhere if desired)
swi t ch( command_packet . command)
{
case COMMAND_SET_LEDS:
Set LEDs( command_packet . dat a) ;
br eak;
def aul t :
//Error, unknown command
DEBUG_ERROR( " Er r or : unknown command r ecei ved" ) ;
br eak;
}
}
el se
{
//Error
DEBUG_ERROR( " Er r or t r yi ng t o compl et e r ead r equest " ) ;
}
}
}

3.2.3.6 Open Accessory Framework
Mt vi iu khin hoc mt thit b kt ni vi Android device qua cng USB
phi c h tr giao thc Android Open Accessory (AOA), do phn cng USB
khng th cung cp y yu cu v dng din nn AOA yu cu vi iu khin hoc
ph kin kt ni phi ng vai tr l mt USB host, ngha l phi cung cp nng lng
500mA ti 5V cho kt ni.
3.2.4 S dng GPS v Google Map xc inh v tr ca robot
3.2.4.4 GPS
H thng nh v ton cu GPS (Global Positioning System) l h thng xc
nh v tr da trn v tr ca cc v tinh nhn to, do b quc phng hoa k thit k,
61
xy dng, vn hnh v qun l. Trong cng mt thi im, ta ca mt im trn
mt t s c xc nh nu xc nh c khong cch t im n t nht ba v
tinh.
Tuy c qun l bi B Quc phng Hoa K, chnh ph Hoa K cho php
mi ngi trn th gii s dng mt s chc nng ca GPS min ph, bt k quc tch
no
3.2.4.4.1 Phn loi
H thng nh v ton cu ca M l h dn ng da trn mt mng li 24
qu v tinh c B Quc phng Hoa K t trn qu o khng gian.
Cc h thng dn ng truyn thng hot ng da trn cc trm pht tn
hiu v tuyn in. c bit n nhiu nht l cc h thng sau: LORAN (LOng
RAnge Navigation) hot ng gii tn 90-100 kHz ch yu dng cho hng hi, hay
TACAN (TACtical Air Navigation) dng cho qun i M v bin th vi
chnh xc thp VOR/DME VHF (Omnidirectional Range/Distance Measuring
Equipment) dng cho hng khng dn dng.
Gn nh ng thi vi lc M pht trin GPS, Lin X cng pht trin mt h
thng tng t vi tn gi GLONASS. Hin nay Lin minh Chu u ang pht trin
h dn ng v tinh ca mnh mang tn Galileo. Trung Quc th pht trin h thng
nh v ton cu ca mnh mang tn Bc u bao gm 35 v tinh.
3.2.4.4.2 S hot ng ca GPS
24 v tinh (21 v tinh hot ng v 3 v tinh d phng) nm trn cc qu o
xoay quanh tri t. Chng cch mt t 20.200 km, bn knh qu o 26.600 km.
Chng chuyn ng n nh v quay hai vng qu o trong khong thi gian gn 24
gi vi vn tc 7 nghn dm mt gi. Cc v tinh trn qu o c b tr sao cho cc
my thu GPS trn mt t c th nhn thy ti thiu 4 v tinh vo bt k thi im no.
Cc v tinh GPS bay vng quanh Tri t pht tn hiu c thng tin xung Tri
t. Cc my thu GPS nhn thng tin ny v bng php tnh lng gic tnh c
chnh xc v tr ca ngi dng. V bn cht my thu GPS so snh thi gian tn hiu
62
c pht i t v tinh vi thi gian nhn c chng. Sai lch v thi gian cho bit
my thu GPS cch v tinh bao xa. Ri vi nhiu qung cch o c ti nhiu v
tinh my thu c th tnh c v tr ca ngi dng v hin th ln bn in t ca
my.
My thu phi nhn c tn hiu ca t nht ba v tinh tnh ra v tr hai chiu
(kinh v v ) v theo di c chuyn ng. Khi nhn c tn hiu ca t nht
4 v tinh th my thu c th tnh c v tr ba chiu (kinh , v v cao). Mt
khi v tr ngi dng tnh c th my thu GPS c th tnh cc thng tin khc, nh
tc , hng chuyn ng, bm st di chuyn, khong hnh trnh, qung cch ti
im n, thi gian Mt Tri mc, ln v nhiu th khc na.

Hnh 3.2.14 Xc nh v tr t 3 v tinh
3.2.4.5 V tr v bn
Android cung cp cc API cho php ngi dng truy cp vo cc dch v nh
v c h tr trn thit b thng qua gi android.location. kt hp vi Google Map
API android ly d liu google map t google, t xy dng ng dng xc nh v
tr ca robot trn google map.
3.2.4.5.1 Xc nh v tr robot
xc nh v tr ca mt thit b android ngi dng c th s dng GPS hoc
Android's Network Location Provider.
63
Mc d GPS th chnh xc nht nhng n ch hot ng tt ngoi tri, v tiu
th nng lng ln.
Network Location Provider xc nh v tr ngi s dng thng qua cc thp di
ng v cc tn hiu wifi, cung cp v tr c trong nh v ngoi tri, p ng nhanh hn
v tiu th t nng lng hn.
c c v tr tt nht bn c th s dng c 2 phng php trn.

Hnh 3.2.15 A-GPS
Ly v tr chnh xc t GPS c th c sai st v khng chnh xc, c rt nhiu
nguyn nhn bao gm:
Gi chm ca tng i lu v tng ion Tn hiu v tinh b chm i khi xuyn
qua tng kh quyn.
Tn hiu i nhiu ng iu ny xy ra khi tn hiu phn x t nh hay cc
i tng khc trc khi ti my thu.
Li ng h my thu ng h c trong my thu khng chnh xc nh ng
h nguyn t trn cc v tinh GPS.
Li qu o Cng c bit nh li thin vn, do v tinh thng bo v tr
khng chnh xc.
S lng v tinh nhn thy Cng nhiu qu v tinh c my thu GPS nhn
thy th cng chnh xc. Nh cao tng, a hnh, nhiu lon in t hoc i
khi thm ch tn l dy c th chn thu nhn tn hiu, gy li nh v hoc
64
khng nh v c. Ni chung my thu GPS khng lm vic trong nh, di
nc hoc di t.
Che khut v hnh hc iu ny lin quan ti v tr tng i ca cc v tinh
thi im bt k. Phn b v tinh l tng l khi cc qu v tinh v tr to
cc gc rng vi nhau. Phn b xu xy ra khi cc qu v tinh trn
mt ng thng hoc cm thnh nhm.
M hnh xc nh v tr :

Hnh 3.2.16 M hnh cp nht v tr
Trong m hnh ny mt chu k c cp nht v tr chnh xc cho robot c th
din ra nh sau
1 Bt u ng dng
2 Cp nht t cc ngun cung cp v tr (gps, Cell-id, wifi)
3 Kt hp tt c cc thng tin c c a ra v tr hin ti tt nht
4 Tam ngng thu thp v tr
5 Tip tc cp nht v tr, kt hp vi v tr c c c tnh v tr hin ti
3.2.4.5.2 Google Map Android API V2
Vi Google Map Android API V2 ngi dng c th thm cc bn da trn
d liu Google Map vo cc ng dng ca mnh
Trong ti ny ng dng c xy dng xc nh v tr hin ti ca robot,
vic nhng bn vo ng dng cho ta ci nhn trc quan v tr hin ti ca robot trn
bn , t gi d liu v ngi iu khin.
to mt ng dng Android c s dng Google Map Android API V2 ta thc
hin cc bc :
65
1: Ci t Android SDK
2: Ti v v cu hnh Google Play Services
3: ng k API key ( lm iu ny, bn s cn phi ng k mt d n trong
Google API Console, v nhn c mt giy chng nhn k cho ng dng ca bn.)

Hnh 3.2.17 ng k API key

66
Demo bn : mt demo c vit trn java cho thy cch gi bn , to mt
marker v tr ta (-33.867, 151.206)
i mpor t com. googl e. andr oi d. gms. maps. *;
i mpor t com. googl e. andr oi d. gms. maps. model . *;
i mpor t andr oi d. app. Act i vi t y;
i mpor t andr oi d. os. Bundl e;

publ i c cl ass MapPane ext ends Act i vi t y {

@Over r i de
pr ot ect ed voi d onCr eat e( Bundl e savedI nst anceSt at e) {
super . onCr eat e( savedI nst anceSt at e) ;
set Cont ent Vi ew( R. l ayout . map_act i vi t y) ;

/ / Get a handl e t o t he Map Fr agment
Googl eMap map = ( ( MapFr agment ) get Fr agment Manager ( )
. f i ndFr agment ByI d( R. i d. map) ) . get Map( ) ;

Lat Lng sydney = new Lat Lng( - 33. 867, 151. 206) ;

map. set MyLocat i onEnabl ed( t r ue) ;
map. moveCamer a( Camer aUpdat eFact or y. newLat LngZoom( sydney, 13) ) ;

map. addMar ker ( new Mar ker Opt i ons( )
. t i t l e( " Sydney" )
. sni ppet ( " The most popul ous ci t y i n Aust r al i a. " )
. posi t i on( sydney) ) ;
}
}

Hnh 3.2.18 Giao din Demo
67
3.2.4.6 Xc nh cc route gia hai im AB
Trong ch t ng robot di chuyn t A n B vi A l v tr hin ti ca
robot v B l v tr ngi iu khin cung cp.
c th vch mt ng i t A n B Google Map cung cp cho ngi dng
d liu cc im route trung gian gia 2 im m ngi dng yu cu tm ng. t
cc im route ny s dng thut ton tnh gc v khong cch t 2 im ta ta c
th di chuyn robot theo cc ng thng nh trn AB.

Hnh 3.2.19 Xc nh Route gia A v B
Kt qu thu c l mt ma trn cha tt c ta ca cc route.
12- 15 23: 37: 57. 670: D/ posi t i on( 22095) : >>>>>>>poi nt s =
[ l at / l ng: ( 10. 77431, 106. 65149) ,
l at / l ng: ( 10. 77433, 106. 65167) ,
l at / l ng: ( 10. 77432, 106. 65181) ,
l at / l ng: ( 10. 77432, 106. 65182) ,
l at / l ng: ( 10. 77432, 106. 65182) ,
l at / l ng: ( 10. 77428, 106. 65181) ,
l at / l ng: ( 10. 77425, 106. 65181) ,
l at / l ng: ( 10. 77425, 106. 65181) ,
l at / l ng: ( 10. 77426, 106. 65182) ,
l at / l ng: ( 10. 77429, 106. 65211) ,
l at / l ng: ( 10. 77428, 106. 65212) ,
l at / l ng: ( 10. 77426, 106. 65213) ,
l at / l ng: ( 10. 77432, 106. 65241) ,
l at / l ng: ( 10. 77432, 106. 65242) ,
l at / l ng: ( 10. 77462, 106. 65239) ,
l at / l ng: ( 10. 77485, 106. 65242) ]
68
3.2.4.7 Thut ton xc nh gc v khong cch t hai im ta
Thut ton xc nh gc gia 2 im ta

Hnh 3.2.20 Xc nh gc gia 2 im
Nu c nh A(x0, y0) th B(x1,y1) c th nm trn 4 gc phn t ly A lm
gc ta nn ta dng hm actan2 tnh gc cho tt c cc trng hp

V l do ta v tr nhn c l kinh v v a l, c ngha l v tr nhn
c nm trong h ta 3 chiu, vi gi s tri t hnh cu trn vi bn knh
6.371,01 km, ta xy dng thut ton tnh gc nh sau:
69

Hnh 3.2.21 Xc nh gc t 2 im ta a l
= atan2( sin().cos(
2
), cos(
1
).sin(
2
) sin(
1
).cos(
2
).cos() )
vi = lng2 lng1 (hiu 2 kinh )

2
=lat2 (v v tr 2)

1
=lat1 (v v tr 1)
cng thc trn tr v gi tr t (, ], nn dng cng thc bn di hiu
chnh min gi tr v [0, 2).
d
=( * 180 / + 360 ) % 360
chng trnh java tnh ton gc gia hai im :
cl ass Gr eat Ci r cl eBear i ng
{
st at i c publ i c doubl e i ni t i al ( doubl e l at 1, doubl e l ong1, doubl e l at 2, doubl e
l ong2)
{
r et ur n ( _bear i ng( l at 1, l ong1, l at 2, l ong2) + 360. 0) %360;
}

st at i c publ i c doubl e f i nal ( doubl e l at 1, doubl e l ong1, doubl e l at 2, doubl e
l ong2)
{
r et ur n ( _bear i ng( l at 2, l ong2, l at 1, l ong1) + 180. 0) %360;
}

st at i c pr i vat e doubl e _bear i ng( doubl e l at 1, doubl e l ong1, doubl e l at 2,
doubl e l ong2)
{
st at i c doubl e degToRad = Mat h. PI / 180. 0;
doubl e phi 1 = l at 1 * degToRad;
doubl e l am1 = l ong1 * degToRad;

70
r et ur n Mat h. at an2( Mat h. si n( l am2- l am1) *Mat h. cos( phi 2) ,
Mat h. cos( phi 1) *Mat h. si n( phi 2) -
Mat h. si n( phi 1) *Mat h. cos( phi 2) *Mat h. cos( l am2- l am1)
) * 180/ Mat h. PI ;
}
}
Thut ton xc nh khong cch gia 2 im ta

Hnh 3.2.22 Khong cch gia 2 im ta a l
Thut ton c xy dng nh sau:
d = r * arccos (sin ( 1 ) * sin ( 2 ) + cos ( 1 ) * cos ( 2 ) * cos ( 2 - 1 ))
vi
r l bn knh tri t 6371,01 (km)
1, 2 l v ca hai im (lat)
1, 2 l kinh ca hai im (lng)
V tri t khng phi l hnh cu tuyt i nn gii thut c sai s khong 5%
Chng trnh java xy dng thut ton :
cl ass Gr eat Ci r cl eDi st ance
{
publ i c st at i c doubl e ki l omet er s ( doubl e l at 1, doubl e l ong1, doubl e l at 2,
doubl e l ong2)
{
st at i c doubl e degToRad= Mat h. PI / 180. 0;
71

r et ur n 6371. 01 * Mat h. acos( Mat h. si n( phi 1) * Mat h. si n( phi 2) +
Mat h. cos( phi 1) * Mat h. cos( phi 2) * Mat h. cos( l am2 - l am1) ) ;
}
}

3.2.5 Xy dng web server trn Android dng th vin NanoHttpd
NanoHttpd l mt th vin m ngun m v min ph dng tao ra mt web
server nh nhng vo cc ng dng trn Android, n c vit thnh mt file *java
duy nht. Ti thi im hin ti n vn ang c pht trin bi ngi dng.
3.2.5.4 Cc tnh nng ct li
Ch c mt file *java duy nht h tr HTTP
Khng h tr cu hnh ring, qun l ng nhp, qun l kt ni.
H tr SSL
H tr cookie
H tr GET v POST
H tr PUT
H tr single and multi-value parameters
H tr upload file
Khng to caches
Khng gii hn bng thng, yu cu thi gian hoc kt ni ng thi
Chim b nh rt thp
Kt ni lin tc (Connection "keep-alive")
3.2.5.5 S dng NanoHttpd
C th s dng samples trong th vin NanoHttpd nhng trc tip vo ng
dng trn Android hoc C th t to mt class extends NanoHTTPD nh code mu
bn di :

publ i c cl ass DebugSer ver ext ends NanoHTTPD {
72
/**
* Constructs an HTTP server on given port.
*/
publ i c DebugSer ver ( ) {
super ( 8080) ;
}

@Over r i de
publ i c Response ser ve( St r i ng ur i , Met hod met hod, Map<St r i ng, St r i ng>
header , Map<St r i ng, St r i ng> par ms, Map<St r i ng, St r i ng> f i l es) {
St r i ngBui l der sb = new St r i ngBui l der ( ) ;
sb. append( " <ht ml >" ) ;
sb. append( " <head><t i t l e>Debug Ser ver </ t i t l e></ head>" ) ; . append( " <body>" ) ;
sb. append( " <h1>Response</ h1>" ) ;
sb. append( " <p><bl ockquot e><b>URI - </ b> " ) . append( ur i ) . append( " <br / >" ) ;
sb. append( " <b>Met hod- </ b" ) . append( met hod) . append( " </ bl ockquot e></ p>" )
sb. append( " <h3>Header s</ h3><p><bl ockquot e>" ) . append( header ) . append( " </ bl ockquot e></ p
>" ) ;
sb. append( " <h3>Par ms</ h3><p><bl ockquot e>" ) . append( par ms) . append( " </ bl ockquot e
></ p>" ) ;
sb. append( " <h3>Fi l es</ h3><p><bl ockquot e>" ) . append( f i l es) . append( " </ bl ockquot e
></ p>" ) ;
sb. append( " </ body>" ) ;
sb. append( " </ ht ml >" ) ;
r et ur n new Response( sb. t oSt r i ng( ) ) ;
}

publ i c st at i c voi d mai n( St r i ng[ ] ar gs) {
Ser ver Runner . r un( DebugSer ver . cl ass) ;
}
}
3.2.5.6 JavaScript JQuery v cc th vin JQuery
Sau khi xy dng thnh cng mt web server, ta s dng cc cng c v ngn
ng lp trnh web xy dng mt trang web mang giao din UI bn trong server c
nhim v trao i d liu t ngi iu khin (client) ti Android device ( server)
J avaScript l mt scripting language trong HTML, c coi l mt ngn ng
lp trnh web cao cp, c chy ngay trong trnh duyt web v khng cn phi bin
dch bi b vi x l ca my tnh nh cc ngn ng lp trnh khc (C , C++)
J Query l mt th vin J avaScript, vi J Query vic lp trnh javaScript tr nn
n gin hn.
Trong ti ny ngoi mt s m J avaScript thun ty n gin c vit trc
tip trong file Index.html th sinh vin cn s dng th vin J Query Mobile ,J Query
plugin Gmap3 v Highcharts, cc th vin trn c xy dng rt ln v bao hm rt
nhiu chc nng, gii hn ti ny sinh vin s trnh by c bn vic s dng hai
th vin ny tao ra giao din UI trn nn web.
73
3.2.5.6.1 JQuery Mobile
J Query Mobile s dng d liu HTML5 to ra giao din web ti u ha cho
thit b di ng c mng hnh cm ng. thay v to thuc tnh trn html nh bnh
thng by gi vi th vin JQuery Mobile ta d dng to v thao tc vi cc thuc
tnh c thit k cho giao din cm ng.
Ci t :
<scr i pt sr c=" ht t p: / / code. j quer y. com/ j quer y- 1. 8. 2. mi n. j s" ></ scr i pt >
<scr i pt sr c=" ht t p: / / code. j quer y. com/ mobi l e/ 1. 2. 0/ j quer y. mobi l e-
1. 2. 0. mi n. j s" ></ scr i pt >
3.2.5.6.2 JQuery plugin Gmap3
JQuery plugin Gmap3 l plugin to v qun l Google Map chy trn nn
web. Vic s dng J Query plugin Gmap3 s n gin ha vic nhng mt bn ca
Google vo giao din nn web.
Ci t
<scr i pt t ype=" t ext / j avascr i pt " sr c=" / admi n/ gmap3. mi n. j s" ></ scr i pt >
Hin th bn
$( " #my_map" ) . gmap3( ) ;
Quy nh cc thuc tnh CSS
#my_map{
hei ght : 350px;
wi dt h: 600px;
}

74

Hnh 3.2.23 Gmap3 demo
3.2.5.6.3 Highcharts 3.0
Highcharts 3.0 l th vin c vit bng HTML5/javascrip cung cp cho
ngi dng cng c trc quan v mt biu trn nn web. Th vin ny h tr tt
vic v biu dng, tr, ct, hnh trn
Ci t
<scr i pt
sr c=" ht t p: / / aj ax. googl eapi s. com/ aj ax/ l i bs/ j quer y/ 1. 8. 2/ j quer y. mi n. j s" ></ scr i pt >

<scr i pt sr c=" ht t p: / / code. hi ghchar t s. com/ hi ghchar t s. j s" ></ scr i pt >

75

Hnh 3.2.24 Highcharts Demo
3.2.2 JQuery AJAX
AJ AX l mt phng php trao i d liu vi my ch, v cp nht cc phn
ca trang web m khng cn phi ti li ton b trang.
AJ AX =Asynchronous J avaScript v XML
AJAX cho php cc trang web c cp nht khng ng b bng cch trao i
mt lng nh d liu vi my ch. iu ny c ngha l n c th cp nht mt phn
trang web m khng cn ti li ton b trang.
Cc trang web c in, (m khng s dng AJAX) phi ti li ton b trang nu ni
dung cn thay i.
V d v cc ng dng s dng AJAX: Google Maps, Gmail, Youtube, Facebook v
cc tab.
AJAX da trn cc tiu chun internet , v l s kt hp ca :
i tng XMLHttpRequest (trao i d liu khng ng b vi my ch)
JavaScript / DOM ( hin th / tng tc vi thng tin)
CSS ( to kiu d liu)
XML (thng c s dng nh l nh dng chuyn d liu)
76
3.2.2.1 JQuery DOM
jQuery i km vi mt lot cc phng php lin quan ti DOM (document
object model) lm cho n d dng truy cp v thao tc cc yu t v thuc tnh
Ba phng php jQuery cho thao tc DOM l:
text () thit lp hoc tr v ni dung vn bn ca cc yu t la chn
html () thit lp hoc tr v ni dung ca cc yu t c la chn (bao
gm c nh du HTML)
val () thit lp hoc tr v gi tr ca cc trng mu

77
3.2.2.2 GET v POST
GET v POST l hai phng php c s dng yu cu d liu t my ch.
GET POST
BACK
button/Reload
Harmless Data will be re-submitted (the
browser should alert the user that
the data are about to be re-
submitted)
Bookmarked Can be bookmarked Cannot be bookmarked
Cached Can be cached Not cached
Encoding type application/x-www-form-
urlencoded
application/x-www-form-urlencoded
or multipart/form-data. Use
multipart encoding for binary data
History Parameters remain in browser
history
Parameters are not saved in browser
history
Restrictions
on data
length
Yes, when sending data, the
GET method adds the data to
the URL; and the length of a
URL is limited (maximum URL
length is 2048 characters)
No restrictions
Restrictions
on data type
Only ASCII characters allowed No restrictions. Binary data is also
allowed
Security GET is less secure compared to
POST because data sent is part
of the URL

Never use GET when sending
passwords or other sensitive
information!
POST is a little safer than GET
because the parameters are not
stored in browser history or in web
server logs
Visibility Data is visible to everyone in
the URL
Data is not displayed in the URL
Bng 3-3 So snh gia GET v POST
S dng GET gi yu cu ti server:
$. aj ax( {
t ype: " GET" ,
ur l : basi cURL + " ur l " ,
cache: f al se,
dat a: " l abel =" + dat a_send,
success: f unct i on
}) ;

78
on m trn thc hin vic gi 1 gi tr data ti url v yu cu gi tr d
liu v function.
3.2.2 Giao din iu khin nn web:
Giao din iu khin c xy dng trn nn web nn c th chy trn hu ht
cc thit b vi cc h iu hnh khc nhau c kt ni wifi. Hin ti th giao din ch
ti u cho trnh duyt chrome v h iu hnh windows (vi cc trnh duyt v h iu
hnh khc th c th cc thnh phn trong giao din khng c b tr hp l)
3.2.2.1 Thnh phn ca giao din:
1 Hin th camera trn robot
2 Hin th bn , hin th v tr hin ti ca robot, ly v tr gi xung cho
robot
3 Button iu khin robot ch iu khin bng tay
4 Hin th trc quan hng hin ti ca robot so vi hng bc a l
5 Hin th thng tin mt s cm bin v cc gi tr cn thu thp t robot
6 Hin th th, p ng

Hnh 3.2.25 Giao din web
79

Hnh 3.2.26 V p ng trn giao din Web
3.2.2.2 Giao din chng trnh trn Android Device

Hnh 3.2.27 Giao din trn Android device
80
1. S gii thut

Hnh 3.3.1 S khi robot
Hnh 3.3.1 m t s khi ca robot, Android Device kt ni vi MCU
PIC24F bng USB, ng dng trn h iu hnh android gi gi tr phn trm duty cho
PIC24 t xut xung PWM iu khin ng c.
Android Device to mt web server, ngi iu khin thng qua giao din web
ra lnh v gim st robot bng cc thit b c kt ni wifi.
81

Hnh 3.3.2 S khi iu khin Robot
Hnh 3.3.2 m t s khi hot ng ca robot. Khi robot khi ng, kt ni
USB gia Android Device v MCU PIC24F s c gi v thng bo kt ni. sau khi
ngi iu khin ng kt ni bng tay thnh cng th ng dng trn Android
Device s c khi chy, web server s bt u c khi to bng vic kim tra kt
ni, xc nh a ch IP, v cung cp giao din UI trn a ch ny vi port l 8080.
ng thi ng dng s thu thp d liu cc cm bin Accel, gyro, encoder, iu
khin robot gi cn bng.
Ngi iu khin s truy cp vo a ch m ng dng cung cp s dng
giao din iu khin robot. Giao din iu khin s cung cp cho ngi iu khin
hnh nh t Camera, m thanh t microphone, v tr ca robot trn bn , cc thng s
cm bin trn robot. Ngi iu khin chn ch , v ra lnh iu khin trn giao
din cho robot trong phm vi c kt ni wifi.
82
Vi Ch iu khin bng tay, ngi iu khin c th ra lnh cho robot i
ti, li, r tri,r phi, thay i tc v thu thp d liu t cm bin, camera
Trong ch iu khin bng tay, ngi iu khin ra lnh cho robot chuyn
sang ch t ng, chn v tr ta yu cu robot di chuyn ti v gi xung robot.
Robot s di chuyn ti ta c chn vi la bn in t, GPS v cm bin o
khong cch.
S khi vng lp iu khin cn bng

Hnh 3.3.3 Vng lp iu khin cn bng
83
CHNG 5 KT QU
5.1 ng dng trn Android
Sau khi robot khi ng s c mt thng bo kt ni nh hnh 5.1.1

Hnh 5.1.1 Thng bo kt ni USB

84

Sau khi ng kt ni USB ng dng trn Android bt u chy web server
nh hnh 5.1.2

Hnh 5.1.2 ng dng sau khi khi ng
Sau khi ngi iu khin chn ch iu khin bng tay robot chuyn sang
ch iu khin bng tay nh hnh 5.1.3

Hnh 5.1.3 ng dng ch iu khin bng tay
85
Sau khi ngi iu khin chn ta v tr im B v chuyn robot sang ch
t ng, robot s di chuyn t hnh vi giao din ng dng nh hnh 5.1.4

5.2 Giao din iu khin
Ngi iu khin truy cp vo a ch trn giao din ng dng trong trng
hp ny l : 192.168.161.101:8080 (thay i vi mi kt ni mng c IP khc nhau)
vo giao din iu khin nh hnh 5.2.1

Hnh 5.2.1 Giao din iu khin
86
T giao din iu khin ngi dng chn ch iu khin bng tay robot
bt u hot ng v gim st v iu khin robot trn giao din nh hnh 5.2.2

Hnh 5.2.2 giao din iu khin chn ch iu khin bng tay
Ngi iu khin chn v tr im B trn bn v gi ta im ny xung
robot (hnh 5.2.3), ng thi ch iu khin t ng s a robot v hot ng t
hnh.

Hnh 5.2.3 chn ta im B
87
CHNG 6 KT LUN
1. Kt lun
ti xy dng thnh cng Robot hai bnh t cn bng vi mt ng dng
chy trn h iu hnh Android iu khin v gim st robot.
V c s l thuyt, m hnh ha robot, v cc phng php iu khin c
nm vng t xy dng thut ton iu khin robot.
V phn gim st v iu khin t xa cho robot hon thnh nhim v gi d
liu t robot v ngi iu khin v nhn lnh t ngi iu khin qua wifi
Xy dng thnh cng mt ng dng web server trn Android dng thu thp
gi tr cm bin, hnh nh, m thanh, v tr trn robot gi ti ngi iu khin cng
nh nhn lnh t ngi iu khin.
Xy dng thnh cng cc thut ton tnh gc lch ca robot bng b lc b
sung v s dng API ca Android vi sai s nh v tn s p ng nhanh. Xy
dng gii thut PID vi cc thut ton ti u. Tnh ton gc v khong cch t ta
2 im v gii thut di chuyn t A B dng GPS v la bn in t.
2. Hng pht trin ti
V hn ch ca chip x l trn Android device c dng ch c xung nhip
1.2GHz nn demo x l nh dng th vin OpenCV p ng khng tt nn khng
c a vo ti.
V vy hng pht trin l nng cp Android device v dng th vin OpenCV
hon thin bi ton di chuyn robot ch t ng.

88
TI LIU THAM KHO
[1] Lun vn thc s (2010) robot hai bnh t cn bng Nguyn Gia Minh Tho.
[2] wiki, http://vi.wikipedia.org/wiki/b_iu_khin_PID
[3] Bi ging, l thuyt iu khin m - Nguyn th phng h.
[4] TKJ Electronic, Balanduino Balancing Robot Kit (Kickstarter),
http://blog.tkjelectronics.dk/
[5] http://developer.android.com/
[6] http://www.microchip.com/
[7] http://nanohttpd.com/
[8] http://playground.arduino.cc/Code/PIDLibrary
[9] http://www.w3schools.com/
[10] http://www.x-firm.com/

89

LVTN PDF

Uploaded by

Document Information

Original Title

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

LVTN PDF

Uploaded by

Copyright:

Available Formats

LVTN: Robot Hai Bnh T Cn Bng (Two wheeled self balancing Robot )

You might also like