Professional Documents
Culture Documents
Li ni u
Robot t hnh (Mobile Robot) l mt thnh phn c vai tr quan trng trong ngnh
Robot hc. Cng vi s pht trin mnh m ca cc h thng C - in t, robot t hnh
ngy mt c hon thin v cng cho thy li ch ca n trong cng nghip v sinh hot.
Mt vn rt c quan tm khi nghin cu v robot t hnh l lm th no robot
c th di chuyn ti mt v tr xc nh m c th t ng trnh c cc chng ngi
vt trn ng i. Tiu lun ny c mc ch a ra cc phng php gii quyt vn
trn. Trong phn mt, chng ti xt 2 cch gii bi ton tm ng cc b v ton cc
i vi mt robot c m hnh ng hc cho trc, sau tin hnh m phng th nghim
trn my tnh. Phn hai ca tiu lun tp trung vo gii thiu cc cm bin, cc mch
phn cng iu khin mt robot t hnh c th cho chy th.
Chng em xin chn thnh cm n nhng gi , ch dn rt quan trng cho ti ny
ca PGS.TS T Duy Lim. Thy cng cng cp cho chng em nhng kin thc v ti
liu Robotics cn thit trong hc k va qua. Chng em hy vng y s l mt tiu lun
c ch cho nhng ai yu thch ti ny.
H Ni, ngy 30/10/2005
Sinh vin:
Nguyn V Quang
on Nam Thi
Robot t hnh
MC LC
PHN 1
GII BI TON TM NG TRNH VT CN CHO ROBOT T HNH ... 3
I. Gii thiu v robot t hnh......................................................................................... 4
II. M hnh ng hc robot t hnh ............................................................................... 6
III. Gii bi ton tm ng cho robot t hnh .............................................................. 7
1. t vn .............................................................................................................. 7
2. Bi ton tm ng cc b..................................................................................... 8
3. Bi ton tm ng ton cc ................................................................................ 13
PHN 2
CU TO PHN CNG V CC PHNG PHP IU KHIN TRONG
ROBOT T HNH ........................................................................................................ 19
I. Cc loi cm bin ..................................................................................................... 20
1. Sensor quang (Optical Sensor)............................................................................ 20
2. a encoder ......................................................................................................... 20
3. Cng tc hnh trnh............................................................................................. 21
II. Mch iu khin Robot ........................................................................................... 21
1. Khi ngun........................................................................................................... 21
2. Khi iu khin.................................................................................................... 22
3. Khi sensor .......................................................................................................... 24
4. Khi hin th ........................................................................................................ 25
5. Khi cng sut iu khin ng c....................................................................... 27
III. Cc phng php iu khin trong Robot t hnh ................................................ 28
1. Phng php to encoder t chut my tnh ....................................................... 28
2. Phng php qut n led 7 thanh....................................................................... 30
3. Phng php to xung PWM iu khin tc cho robot ............................. 33
IV. Xy dng mt bi ton cho Robot t hnh........................................................... 34
V. Hn ch Robot v phng php khc phc .......................................................... 36
Robot t hnh
PHN 1
GII BI TON TM NG
TRNH VT CN CHO ROBOT
T HNH
Robot t hnh
Robot t hnh
a)
b)
c)
d)
f)
h)
e)
g)
i)
Robot t hnh
Robot t hnh
X D (i ) = X D (i 1) + X D ( i )
YD (i ) YD (i 1) YD (i )
Robot t hnh
ch cui cng l robot t v c ti ch khng chm vt cn m khng cn bt c s
tr gip no t bn ngoi.
2. Bi ton tm ng cc b
Gii quyt hon ton bi ton ny kh khn hn nhiu so vi gii quyt bi ton ton
cc. Mt cch tm ng cc b kh hon ho l phng php bn nron cc (Polar
Neurol Map) ca Michail G. Lagoudakis (1998) dng 16-32 cm bin gn trn con robot
Normad (hnh 1.1a, b) to trng cm bin bao ph vng xung quanh robot, thut ton s
dng l thut ton nh x nron tng i phc tp. Do gii hn ca tiu lun, phng
php ny khng c cp n.
Phng php c cp n y s dng 3 cm bin d ng xc nh khong cch
t trung im hai bnh xe ti vt cn. Ty vo khong cch ti vt cn ng vi tng cm
bin, ta xc nh c phng hng di chuyn robot ph hp.
tng ca phng php xut pht t mt bi bo C in t
[1]. Tuy nhin lut dn hng robot trong bi bo cn kh
n gin, iu kin yu cu hi th (nh khong cch cc vt
cn phi ln hn kch thc robot), kt qu m phng cho
thy thut ton ca bi bo t ra nhiu bt cp: robot nhiu lc
chm vt cn, lp i lp li mt on ng v kh thot ra
c. Da theo nguyn l ca thut ton , chng ti c
nhng thay i v b sung em li kt qu tm ng hiu
qu hn.
Hnh 1.4. Ba cm bin ca robot
Cc cm bin c t lch mt gc (trong phn m phng, chn = 25 ). Khong
cch ln nht (tnh t D) m cc cm bin c th nhn din c vt cn l dmax; dmax
v phi m bo sao cho cm bin c vng kim tra rng khi tin thng, robot c
th nhn din c vt cn.
Y
ch
(t )
X
Hnh 1.5. Khng gian lm vic ca robot trong mt phng 2 chiu
Gi:
Robot t hnh
Robot t hnh
a [0,360) )
9 TH6: Ch c cm bin bn phi pht hin vt cn
Robot quay sang tri mt gc 25 (gn a = a + 25 , sau hiu chnh
a [0,360) )
9 TH7: Ch c cm bin gia pht hin vt cn (Hnh 1.6f)
Robot quay sang tri mt gc 90 (gn a = a 90 , sau hiu chnh
a [0,360) )
Khi robot ang quay hng v ch, nu pht hin vt cn n s tin hnh chuyn hng
quay lun. m bo iu ny, trong qu trnh quay hng v ch, mi ln robot quay
mt gc n v (v) nh, ta phi cp nht cm bin.
Sau khi robot chuyn hng ri tin thng mt on, vn c kh nng robot chm vt cn
(nht l nhng ng ct), v vy khi robot qu gn vt ta tin hnh li li 1 on, quay
mt gc 45 sang phi (gn a = a 45 , hiu chnh a [0,360) ), sau
cp nht cm bin.
10
Robot t hnh
V ti
ch?
Dng robot
S
Quay mt gc v
hng ti ch
C vt
cn?
hng
ti ch?
B li chuyn hng
robot
Tin thng mt on
v
C vt
cn?
tin
Qu gn
vt cn?
Li mt on
Quay mt gc 45
sang phi
11
Robot t hnh
Kt qu m phng cho thy >90% cc trng hp robot c th v n ch m khng
chm vt cn.
12
Robot t hnh
3. Bi ton tm ng ton cc
Khc vi tm ng cc b, tm ng ton cc gii quyt ng i cho robot khi ta
bit trc bn ca mi trng hot ng, tc ngoi im khi u v im ch ta cn
bit c v tr v hnh dng ca cc vt cn. Tm ng ton cc ti u (qung ng
ngn nht) em li nhng kt qu th v nh hnh di.
13
Robot t hnh
14
Robot t hnh
Thut ton s dng mt mng hai chiu i din cho mi li, gi tr ca mng l
khong cch ngn nht m li n ch.
Ta khi u cho mng cc gi tr khong cch tng i ln ng vi mi li, c th
bng xmax . y max trong xmax l s li trong mt hng, ymax l s li trong mt ct,
ngoi tr ch c t bng 0 (v 0 l khong cch t ch n chnh n). Thut ton
s kim tra s tn ti ca vt cn, v khng tnh ton cho cc phn t mng ng vi
li c vt cn. V vy thut ton lp ng i sau ny khng cn phi kim tra vung
li c vt cn hay khng, vt cn c t gi tr khong cch ln nn ng i t nhin
s khng qua chng.
Thut ton lan truyn
15
Robot t hnh
If khong cch ny < gi tr khong cch hin ti ca ang xt Then
Gn gi tr khong cch ca ang xt bng khong cch mi
// qut thun t di ln trn
For cc hng ca bn ( ymax ..1) do
For cc ct ca bn (1.. xmax ) do
{Lm tng t trn}
{Qut ngc}
// qut ngc t di ln trn
For cc hng ca bn ( ymax ..1) do
For cc ct ca bn ( xmax ..1) do
{Lm tng t trn}
// qut ngc t trn xung di
For cc hng ca bn (1.. ymax ) do
For cc ct ca bn ( xmax ..1) do
{Lm tng t trn}
Until Mng khong cch hi t
S dng phng php qut thun v ngc (hnh 1.15) gip gi tr khong cch ca cc
li t nh nht, mi ln qut (duyt) mt , gi tr ca khong cch hin ti ca
c so snh vi tng gi tr khong cch mi lin k + khong cch lin k ti n
(khong cch gia 2 nm ngang hoc dc so vi nhau l 1, gia 2 cho nhau l
2 (hnh 1.14)). Gi tr khong cch no nh nht s c gn li cho ang xt. Qu
trnh qut s hon tt (hi t) khi mng khng thay i gi tr khong cch na.
+ 2
+1
+ 2
+1
+1
+ 2 +1
+ 2
16
Robot t hnh
55
55
55
55
55
55
55
55
55
55
55
55
55
55
55
55
55
55
55
55
55
55
55
55
55
55 55 55
55 55 55
55 55 55
55 55 55
55 55 55
a) Khi to
55
55
55
55
55
55
55
55
55
55
55
55
55
55
55
55
55
55
55
55
55
55
55
55
55
55
55
55
55
55
55
55
55
55
55
55
0
55
55
55
55 55 55 55 1.4 1
55 55 55 55
1 0
55 55 55 2.4 1.4 1
55 55 3.8 2.8 2.4 2
55 5.2 4.2 3.8 3.4 3
b) Qut thun 1
2.4
2
2.4
2.8
3.8
1.4
1
1.4
2.4
3.4
55
55
55
55
55
1.4
1
1.4
2.4
3.4
1
0
1
2
3
1.4
1
1.4
2.4
3.4
9.4
10.6
10.2
9.8
10.2
8.4
55
9.2
8.8
9.2
2.4
2
2.4
2.8
3.8
1.4
1
1.4
2.4
3.4
1
0
1
2
3
1.4
1
1.4
2.4
3.4
9.4
9.8
10.2
9.8
10.2
8.4
55
9.2
8.8
9.2
1.4
1
1.4
2.4
3.4
1
0
1
2
3
1.4
1
1.4
2.4
3.4
9.4
9.8
10.2
9.8
10.2
8.4
55
9.2
8.8
9.2
17
Robot t hnh
Hnh 1.16 m t mt v d tm ng i cho mt bn 511. Cc gch en l cc c
vt cn. Ch sau mt ln lp repeat, cc gi tr khong cch hi t. Sau khi to gi tr
khong cch cho mi , ng i c xc nh bng thut ton n gin sau.
Thut ton lp ng i
Bt u xut pht
Xt cc lin k xem no c gi tr khong cch < gi tr khong cch ca xut pht
hay khng.
If khng c Then kt thc, khng tm c ng i
Else {C ng i}
a xut pht vo ng i
Repeat
Xt cc lin k tm c gi tr khong cch thp nht
Chuyn ti lin k
a vo ng i
Until ti ch
End
Hnh 1.16f) cho thy mt ng i t ta (1, 3) ti ch c ta (10,2).
Di y l hnh nh m phng robot tm ng ton cc.
18
Robot t hnh
PHN 2
CU TO PHN CNG V CC
PHNG PHP IU KHIN
TRONG ROBOT T HNH
19
Robot t hnh
I. Cc loi cm bin
1. Sensor quang (Optical Sensor)
2. a encoder
20
Robot t hnh
ng dng
o chuyn ng quay
U20
LM7805
VI
GND
NGUON AC QUI
VO
C4
CAP
C8
CAP NP
C9
CAP NP
C5
CAP
R2
21
Robot t hnh
2. Khi iu khin
Vi iu khin h 8051- AT89C51
9 S chn linh kin
c tnh
ROM
RAM
B nh thi
Chn vo/ra
Cng ni tip
Ngun ngt
S lng
4K
128 byte
2
32
1
6
9 B nh thi
8051 c hai b nh thi l timer 0 v timer 1. C hai b nh thi u c di l 16
bit. 8051 c cu trc 8 bit nn mi b nh thi c truy nhp di dng hai thanh
ghi c lp l byte thp v byte cao.
Timer 0: TH0 (byte cao) v TL0 (byte thp)
Timer 1: TH1 (byte cao) v TL1 (byte thp)
Hai b nh thi c 4 ch hot ng
22
Robot t hnh
23
Robot t hnh
S mch iu khin
5VVDK
5V
R4
100
SW2
2
3
4
5
6
7
8
9
10
R13
R14
C4
5VVDK
10U
J5
R
R
R3
8.2K
2
1
P0_0
P0_1
P0_2
P0_3
P0_4
P0_5
P0_6
P0_7
EA
P1_0
P1_1
P1_2
P1_3
P1_4
P1_5
P1_6
P1_7
39
38
37
36
35
34
33
32
1
2
3
4
5
6
7
8
19
18
CON2
31
9
EA
Y1
C8
40
P0.0/AD0
P0.1/AD1
P0.2/AD2
P0.3/AD3
P0.4/AD4
P0.5/AD5
P0.6/AD6
P0.7/AD7
P2.0/A8
P2.1/A9
P2.2/A10
P2.3/A11
P2.4/A12
P2.5/A13
P2.6/A14
P2.7/A15
P3.0/RXD
P3.1/TXD
P3.2/INT0
P3.3/INT1
P3.4/T0
P3.5/T1
P3.6/WR
P3.7/RD
P1.0
P1.1
P1.2
P1.3
P1.4
P1.5
P1.6
P1.7
XTAL1
XTAL2
ALE/PROG
PSEN
EA/VPP
RST
AT89C51
C9
J7
VCC
1
1
1
1
1
1
1
1
1
GND
5VVDK
U5
SW3
START
20
RN1
0VVDK
21
22
23
24
25
26
27
28
10
11
12
13
14
15
16
17
30
29
P2_0 RXD 1
P2_1 TXD 2
P2_2
RN3
P2_3D15
5VVDK
CON2
D16
2
1
D17
3
1
D18
4
1
5
1
LED
LED
RXD LED
R29
TXD LED
RESISTOR DIP 4
START
P3_3
P3_4
P3_5
R
P3_6
RN4
SW1
P3_7
P2_3
1
2
1
3
P2_2
1
4
P2_1
1
5
P2_0
5VVDK
J8
RESISTOR DIP 4
SW DIP-4
P3_3
1
P3_4
2
P3_5
3
P3_6
4
P3_7
5
0V
CON5
3. Khi sensor
9 Nhim v
Pht hin vt cn, nu c vt cn th pht ra tn hiu bo cho IC bit
9 Thnh phn
Dng cp LED thu pht hng ngoi
B khuch i thut ton (so snh vi tn hiu t sensor, to ra tn hiu
bo cho vi iu khin bit)
9 Nguyn l hot ng
LED thu bnh thng c in tr rt ln c 10K, nhng khi c nh sng LED
pht chiu vo th in tr gim xung c 100 Ohm. Nh nguyn l ny ta thit
k mch pht hin vt nh sau:
24
Robot t hnh
vt cn (ngha l khng c nh sng n n thu) th u ra P0.0 v P0.1 nhn
c tn hiu 0, ngc li khi c vt cn th u ra P0.0 v P0.1 nhn c tn
hiu 1.
R16
5VSS RESISTOR VAR
sensor1 3 +
5VSS
11
2 -
GND
R18
RESISTOR VAR
5 +
sensor2
U51A
LM324
U51B
7
11
6 -
P0_0
P0_1
LM324
4. Khi hin th
9 Mch gii m s dng IC 74138: 3 u vo v 8 u ra thc hin nhim v qut
n
S chn linh kin:
25
Robot t hnh
26
Robot t hnh
5
7432
P1_2 11
P1_3 10
0VVDK
1
8
U3A
P1_0
P1_1
14
13
6
5VVDK
3
0VVDK
7
7432
C6
D21
D25
DIODEDIODE
1A1
1A2
1Y 1
1Y 2
2A1
2A2
2Y 1
2Y 2
CON2
15
2
7
9
Motor Left
J3
D23
D24
1E
2E
1
DC21
DC22
2
D28
D26
DIODEDIODE
Motor Right
CON2
1EN
2EN
0V
5VVDK
5
24V14 VCC1
VCC2
C5 L298
D27
DIODEDIODE
GND
P1_3
5VVDK
D22
12
P1_0 3
P1_1 4
U3B
P1_2
14
Signal
U7
Control
J2
DC11
1
DC12
2
0V
27
Robot t hnh
28
Robot t hnh
5V
5V
R1
U1
X1
D1
DEN PHAT
U2A
7 +
R2
6 -
OUT 1
LM339
VIN
GND
VOUT
R4
RESISTOR VAR
X2
U3A
R3
7 +
6 -
DEN THU
5V
OUT 2
LM339
0
R5
RESISTOR VAR
29
Robot t hnh
Bt u
so snh
(1,1)
so snh
(1,0)
Chiu ngc
N
N
so snh
(1,0)
Chiu thun
so snh
(0,1)
Chiu ngc
N
N
so snh
(0,1)
Chiu thun
so snh
(1,1)
Chiu ngc
N
Chiu thun
Kt thc
30
Robot t hnh
Interrupt 3 ( ngt cho timer 1)
Ngt th nht xy ra:
Dng Timer 1
t=0?
chn n 1;
gi tr n 1 a ra port 3 ;
t=1 ;
Chy Timer 1
Dng Timer 1
t=1?
chn n 2;
gi tr n 2 a ra port 3 ;
t=2 ;
Chy Timer 1
31
Robot t hnh
Khi ngt th 3 xy ra:
Dng Timer 1
t=2?
chn n 3;
gi tr n 3 a ra port 3 ;
t=3 ;
Chy Timer 1
Khi ngt th 4 xy ra:
Dng Timer 1
t=3?
chn n 4;
gi tr n 4 a ra port 3 ;
t=0 ;
Chy Timer 1
Gi tr ca bin chn n quay li ban u. Khi c ngt tip theo xy ra th n th nht
c chn, qu trnh lai lp li theo chu trnh trn. Do tc qut xy ra rt nhanh, mt
32
Robot t hnh
ngi li c s lu nh trn vng mc nn c cm gic s hin th trn n khng b gin
on.
Hi: Phng php qut n c u im g?
Tr li:
Nh phng php qut n ta tit kim c s chn s dng ca vi iu khin
thc hin nhim v qut n, nh m ta c kh nng thc hin nhiu nhim
v khc.
Minh ho cho iu ny ta xt trng hp khi khng s dng phng php qut n.
Mi mt n led 7 thanh nu dng gii m led 7 thanh th chng ta cn 4 tn hiu
iu khin qun l gi tr trn cc n. Nh vy trong bi ton ca chng ta c 4
n th s phi mt t nht 16 chn ca vi iu khin. Khi chng ta tng s n s
dng ln th s chn ta s dng cng tng. y l nhc im ln nht ca phng
php khng qut n. Trong kh , nu s dng phng php qut n th ch vi
8 chn vi iu khin ta c th iu khin vi s lng n khng hn ch. Khi tng
n ta chi vic tng thm mt gi tr cho bin qut n t.
Hi:
Nhc im ca phng php qut n l g?
Tr li:
Nhc im duy nht ca phng php qut n l phng php lp trnh kh.
Dng Timer 0
Gi tr np li mi = 255-gi tr np li mi;
o pwm;
Khi ng li Timer;
33
Robot t hnh
TH0 ban u
255
IV.
34
Robot t hnh
Begin
i thng;
N
gp
vt?
Y
dng:
i li:
quay
i thng;
gp
vt?
Y
dng:
i li:
quay tri:
i thng;
Y
End
35
Robot t hnh
V.
36
Robot t hnh
37