You are on page 1of 37

Robot t hnh

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

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

Nguyn V Quang - on Nam Thi

Robot t hnh

PHN 1
GII BI TON TM NG
TRNH VT CN CHO ROBOT
T HNH

Gii thiu v robot t hnh


M hnh ng hc ca robot t hnh
Gii bi ton tm ng cho robot t hnh
9 t vn
9 Bi ton tm ng cc b (local navigation)
9 Bi ton tm ng ton cc (global
navigation)

Nguyn V Quang - on Nam Thi

Robot t hnh

I. Gii thiu v robot t hnh


Robot t hnh hay robot di ng (mobile robots, thng c gi tt l mobots) c
nh ngha l mt loi xe robot c kh nng t dch chuyn, t vn ng (c th lp trnh
li c) di s in khin t ng thc hin thnh cng cng vic c giao.
Theo l thuyt, mi trng hot ng ca robot t hnh c th l t, nc, khng kh,
khng gian v tr hay s t hp gia chng. a hnh b mt m robot di chuyn trn
c th bng phng hoc thay i, li lm.
Theo b phn thc hin chuyn ng, ta c th chia robot t hnh lm 2 lp: chuyn
ng bng chn (legged) v bng bnh (wheeled). Trong lp u tin, chuyn ng c
c nh cc chn c kh bt chc chuyn ng ca con ngi v ng vt (hnh 1.1g,
h). Robot loi ny c th di chuyn rt tt trn cc nh hnh li lm, phc tp. Tuy nhin,
cch phi hp cc chn cng nh vn gi vng t th l cng vic cc k kh khn.
Lp cn li (di chuyn bng bnh) t ra thc t hn, chng c th lm vic tt trn hu
ht cc a hnh do con ngi to ra. iu khin robot di chuyn bng bnh cng n
gin hn nhiu, gn nh lun m bo tnh n nh cho robot. Lp ny c th chia lm 3
loi robot: Loi chuyn ng bng bnh xe (ph bin) (hnh 1.1a, b, c, f, i), loi chuyn
ng bng vng xch (khi cn m men pht ng ln hay khi cn di chuyn trn vng
m ly, ct v bng tuyt) (hnh 1.1d, e), v loi hn hp bnh xe v xch (t gp).
Tim nng ng dng ca robot t hnh ht sc rng ln. C th k n robot vn
chuyn vt liu, hng ha trong cc ta nh, nh my, ca hng, sn bay v th vin;
robot phc v qut dn ng ph, khoang chn khng; robot kim tra trong mi trng
nguy him; robot canh gc, do thm; robot khm ph khng gian, di chuyn trn hnh
tinh; robot hn, sn trong nh my; robot xe ln phc v ngi khuyt tt; robot phc v
sinh hot gia nh v.v...
Mc d nhu cu ng dng cao, nhng nhng hn ch cha gii quyt c ca robot t
hnh, nh chi ph ch to cao, khng cho php chng c s dng rng ri. Mt
nhc im khc ca robot t hnh phi k n l cn thiu tnh linh hot v thch ng
khi lm vic nhng v tr khc nhau. Bi ton tm ng (navigation problem) ca robot
t hnh cng khng phi l loi bi ton n gin nh nhiu ngi ngh lc ban u.
Trong tiu lun ny, bi ton tm ng s c gii quyt mc khng qu phc tp.

Nguyn V Quang - on Nam Thi

Robot t hnh

a)

b)

c)

d)

f)

h)

e)

g)

i)

Hnh 1.1. Mt s hnh nh v robot t hnh


a) Robot Nomad 150. b) S kt hp gia robot t hnh Nomadic XR4000 v cnh tay
robot PUMA 560.
c) Robot con ra. d) Robot Houdini di chuyn bng bnh xch
e) Robot bn t ng nghin cu vng cc bc. f) Robot SRR v FIDO ca NASA
g) Robot t hnh bng chn Spiderbot di chuyn trn sao Ha. h)Robot su chn Genghis
i) Robot Meet Timbot dng test cc phn mm h thng nhng trong phng th nghim

Nguyn V Quang - on Nam Thi

Robot t hnh

II. M hnh ng hc robot t hnh


M hnh kt cu

Hnh 1.2. M hnh kt cu ca robot


Kt cu robot la chn gm 4 bnh trong c 2 bnh ch ng (pha sau) v 2 bnh
ty ng c kh nng quay tu . Vi kt cu c kh ny robot c kh nng di chuyn rt
linh hot: tin, li v quay mt gc bt k.
ng hc ca robot
Gi s1 , s2 l
on dch chuyn
ca 2 bnh ch
ng. Ta c
s1 = 1 .r
s2 = 2 .r
vi 1 , 2 , r
ln lt l lng
dch chuyn quay
v bn knh ca 2
bnh ch ng.

Hnh 1.3. ng hc ca robot


T l khong cch gia 2 bnh. RD , l bn knh quay v gc dch chuyn ca robot
trong mt phng nm ngang. Khi :
T s + s1
s s1
RD = ( 2
) , = 2
2 s2 s1
T
Trong tiu lun ny, ta ch xt 2 trng hp c bit ca chuyn ng:
Khi s 2 = s1 : RD = , = 0 , robot i theo qu o l ng thng (tin hoc li)
Khi s 2 = s1 : RD = 0 , = 2s 2 / T , robot quay quanh im gia 2 bnh xe D.

Nguyn V Quang - on Nam Thi

Robot t hnh

x , y : lng dch chuyn theo 2 phng ca h ta gn vi robot:


x = RD sin() , y = RD (1 cos())
X , Y : lng dch chuyn ca robot theo 2 phng gn vi h ta gc:
X x y cos( )
Y = y x sin( )

V tr ca robot c xc nh bi ta gc ca D (XD v YD) + gc nh hng D , ta


ti thi im th (i) c xc nh nh sau:
D ( i ) D ( i 1) D ( i )

X D (i ) = X D (i 1) + X D ( i )
YD (i ) YD (i 1) YD (i )

III. Gii bi ton tm ng cho robot t hnh


1. t vn
D ng (navigation) l mt khoa hc (hay ngh thut) dn hng robot t hnh di
chuyn trong khng gian lm vic ca n (t, nc, khng kh...). Trong vn d
ng, bi ton c quan tm nhiu nht c l l tm ng v ch m khng chm vt
cn trn ng i. C hai loi bi ton tm ng cho robot: bi ton cc b (local) v bi
ton ton cc (global). Trong bi ton ton cc, bn mi trng lm vic ca robot
hon ton c bit trc, vn chnh phi gii quyt l tm c ng i cho robot
trc khi n xut pht. bi ton cc b, mi trng lm vic ca robot hon ton khng
c bit trc hoc ch bit c mt phn, robot hon ton phi nh vo s cm nhn
mi trng thng qua cm bin gn trn n d ng.
Bi ton ton cc t r li th l ta bit trc c ng i (ti u) ti ch hay khng
trc khi cho robot khi hnh. Tuy vy n c hn ch l i hi nhiu lnh tnh ton v
b nh, v c bit tnh hung xu c th xy ra nu bn mi trng lm vic khng
c khai bo chnh xc, yu cu bit trc hon ton mi trng hot ng cng l mt
nhc im ca n. Trong khi , robot tm ng cc b ch bit c thng tin xung
quanh qua sensor cm nhn mi trng gn cng. V th, robot tm ng cc b c th
khng hon thnh vic ti ch (mc d thc t c ng i ti ch), khi nim ti u
khng c ngha trong bi ton ny. Tuy nhin, yu cu tnh ton, dung lng nh thp
cng tnh linh hot cao (nh trnh c vt cn ngay c khi vt di ng) khin tm
ng cc b tr thnh mt cng c mnh ca robot t hnh. Hin nay, cch tip cn kt
hp gia cc b v ton cc ang ngy cng ph bin, gip robot t hnh tng tnh linh
hot v hiu qu.
Trong tiu lun, chng ti tp trung vo gii quyt c hai bi ton nhng mc n
gin. Mi trng hot ng ca robot l mt mt phng, c gii hn bi cc bc tng.
Cc vt cn c xem l vt cn hai chiu, tnh hon ton. Trong bi ton cc b, robot
c trang b 3 sensor xc nh khong cch vt cn, bit trc v tr ch (ch c th
pht sng robot hng theo). Trong bi ton ton cc, t u vo l v tr xut pht, v
tr ch, kch thc robot, bn hot ng, ta tm ra ng i robot t hnh. Mc
Nguyn V Quang - on Nam Thi

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:

Nguyn V Quang - on Nam Thi

Robot t hnh

(t ) : gc ch hng ca robot thi im t.


(t ) : gc hng v ch tnh t tm robot (im D).
a (t ) : gc dn hng cho robot trnh vt cn.
(t ) : gc dn hng iu khin chuyn ng cho robot.
Khi sensor khng pht hin vt cn th (t ) = (t ) , ngc li th (t ) = a (t ) .
Cch ng x ca robot khi sensor pht hin vt cn:
C 7 tnh hung khc nhau khi sensor pht hin vt cn, mi tnh hung cn c cch ng
x ring.

Hnh 1.6. Cc tnh hung pht hin vt cn


9 Tnh hung 1 (TH1): c 3 cm bin u pht hin vt cn (Hnh 1.6a)
+ Nu dl>dr th robot quay sang tri: a = ( BA, Ox)
+ Nu dl<=dr th robot quay sang phi: a = ( BC , Ox)

9 TH2: Ch c cm bin bn tri khng pht hin vt cn (Hnh 1.6b)


Robot quay sang tri: a = (CB, Ox)
9 TH3: Ch c cm bin bn phi khng pht hin vt cn (Hnh 1.6c)
Robot quay sang phi: a = ( AB, Ox)

Nguyn V Quang - on Nam Thi

Robot t hnh

9 TH4: Ch c cm bin gia khng pht hin vt cn (Hnh 1.6d)


Robot quay sang phi mt gc 90 (gn a = a 90 , sau hiu chnh
a [0,360) )
9 TH5: Ch c cm bin bn tri pht hin vt cn (Hnh 1.6e)
Robot quay sang phi mt gc 25 (gn a = a 25 , sau hiu chnh

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.

Hnh 1.7. Khi khong cch nh hn hoc bng DI, DJ,


DK tc l robot qu gn vt

Nguyn V Quang - on Nam Thi

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

Hnh 1.8. S qu trnh tm ng ca robot dng 3 cm bin

Nguyn V Quang - on Nam Thi

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.

Hnh 1.9. Mt qu trnh trnh vt cn ca robot (m phng)


Hi: Robot 3 cm bin c kh nng chm vt cn khng?
Tr li: Robot 3 cm bin vn c kh nng chm vt cn. Tuy nhin kh nng
t xy ra do robot c thm th tc li li khi cm bin pht hin vt cn qu
gn robot. Mun robot hon ton khng chm vt , ta cn c mt h cm bin
bao ph ton b mt vng xung quanh robot cng mt thut ton phc tp hn.
Hi: Robot c th v ti ch khng?
Tr li: Khng phi lc no robot cng v c ti ch, d trn l thuyt c
ng i ti ch, l mt nhc im ca cc thut ton n gin khi gii bi
ton cc b.
Hi: u, nhc im ca phng php ny l g?
Tr li: u im chnh ca phng php l robot ch phi s dng 3 cm bin
o khong cch cng mt thut ton n gin, m hiu qu qu trnh tm ng
ca robot kh cao.
Nhc im chnh l robot khng th xc nh y bin ca vt cn do ch c
3 cm bin, v vy dn n chuyn robot vn c th chm vt hoc khng v ti
c ch.

Nguyn V Quang - on Nam Thi

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.

Hnh 1.10. ng i ngn nht trong m


cung xon

C nhiu cch m hnh ha mi trng lm vic ca robot, ta xt mt m hnh hay c


s dng l m hnh bn khng gian kt hp (composite-space maps).
Trong bn khng gian kt hp, mi trng lm vic ca robot (2D) c chia lm cc
hnh vung nh bng cch k li (grid). Thng tin c lu tr ng vi mi l vt cn
c nm trn hay khng, thng tin ny c cho bi mt mng 2 chiu m mi chiu
theo phng x hay phng y xc nh v tr mi vung trn bn . tng hiu qu
ca bi ton, m hnh cn phi gn st thc t, tc ta phi thu nh kch thc ca vung
li n nh gi chnh xc v tr vt cn mc cho php.
Bin ca robot l ng trn bo v robot, tm ng trn l trung im ca 2 bnh ch
ng. Tm ng trn, bn knh ng trn c coi l tm v bn knh robot.Yu cu
ng trn ny khng c giao vi vng vt cn. Khc bi ton cc b (khi m ta
khng c thng tin no trc v vt cn), vt cn y c xc nh, tc l c th x
l thng tin vt cn trc khi cho robot chy. Nhm n gin ha bi ton, cc vt cn
c ko gin (expanded) theo mi pha mt lng bng ng bn knh ca robot, ng
thi robot c rt li cn ng mt im, im ny chnh l tm robot. Nh vy thay v
tm ng cho robot trnh vt cn ta ch cn tm ng cho im tm robot trnh cc vt
cn c ko gin. Vt cn c ko gin s c m hnh ha bng cc li nh
ni trn.

Nguyn V Quang - on Nam Thi

13

Robot t hnh

Hnh 1.11. Vt cn bn trong c gin ra mt khong bng bn knh robot, cc


li mu trng l cc li khng cha vt cn ( c ko gin)

Mi li c 8 lin k. Chuyn ng ca im tm robot c thc hin gia cc


li. im tm robot c xc nh l tm vung li. im tm c th chuyn ng
theo 8 hng n c tm ca cc lin k.

Hnh 1.12. Chuyn ng ca tm robot gia cc li

Nhim v dn ng cho robot do b xc nh ng i (path planner) m nhim. Cng


vic ca b xc nh bao gm:
+ S dng mt thut ton nhm m ha mt cu trc d liu tng ng vi cc li
vung (thng di dng mng 2 chiu).
+ T kt qu m ha, thc hin thut ton lp ng i cho robot.
Ta s dng mt thut ton m ha rt hiu qu da trn phng php bin i khong
cch (distance transform method) ca Kang v Jarvis (1986). Mc ch ca thut ton l
to ra 1 mng 2 chiu lu gi gi tr qung ng ngn nht ti ch ca mi li.
Bng cch ny, ta s gii c bi ton yu cu qung ng di chuyn t im xut pht
n im ch l ngn nht. Trong phng php ca Kang v Jarvis, b xc nh khi
u ch v lan truyn (propagate) cc khong cch (hay qung ng) qua khng
gian trng. Mt cch hnh tng ging nh khi ta nm mt hn xung ao, sng nc
lan ra, chng b trit tiu khi gp vt cn, ch no sng nc loang cng tha th qung
ng ti ch (v tr nm) cng ln (hnh 1.13).

Nguyn V Quang - on Nam Thi

14

Robot t hnh

Hnh 1.13. Hin tng lan truyn sng nc


Qung ng i c ca sng tng ng vi tha ca sng

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

1. Bn mi trng c k li thnh mt ma trn 1.. xmax ,1.. ymax


2. Cp pht mt mng 2 chiu 0... xmax +1, 0.. ymax +1 lu gi cc gi tr khong cch.
Cc hng v ct ngoi cng to thnh mt ng ch nht bao (gi l hnh ch nht canh
gi) nhm n gin tnh ton khng phi xt cc trng hp c bit khi li bin
khng c 8 k.
3. Khi to cho mng khong cch bng cch t ch =0 v tt c cc khc mt gi
tr kh ln= ( xmax . ymax )
4. Tnh ton khong cch bng php lp repeat , thc hin qut thun v qut ngc qua
mng.
Repeat { qut thun}
// qut thun t trn xung di
For cc hng ca bn (1.. ymax ) do
For cc ct ca bn (1.. xmax ) do
If li khng c vt cn Then
{Bt u tnh ton khong cch}
For mi trong 8 lin k ang xt
Tnh khong cch=gi tr khong cch mi k + khong cch k ti
ang xt.

Nguyn V Quang - on Nam Thi

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

Hnh 1.14. Khong cch ca mi k i vi ang xt ( en)

Hnh 1.15. Qut thun (3,1) v qut ngc (4,2)

Nguyn V Quang - on Nam Thi

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 8 6.2 5.2 3.4


55 55 6.6 4.8 3.8
9 55 6.2 5.2 3.4
55 7.6 6.6 4.8 3.8
55 55 6.2 5.2 4.2
c) Qut thun 2

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

7.4 6.4 5.4 4.4 3.4


55 55
5
4
3
8.2 55 5.4 4.4 3.4
7.8 6.8 5.8 4.8 3.8
8.2 7.2 6.2 5.2 4.2
d)Qut ngc 1

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

7.4 6.4 5.4 4.4 3.4 2.4


55 55
5
4
3
2
8.2 55 5.4 4.4 3.4 2.4
7.8 6.8 5.8 4.8 3.8 2.8
8.2 7.2 6.2 5.2 4.2 3.8
e) Qut ngc 2 ( hi t)

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

7.4 6.4 5.4 4.4 3.4 2.4 1.4 1 1.4


55 55
5
4
3
2
1 0 1
8.2 55 5.4 4.4 3.4 2.4 1.4 1 1.4
7.8 6.8 5.8 4.8 3.8 2.8 2.4 2 2.4
8.2 7.2 6.2 5.2 4.2 3.8 3.4 3 3.4
f) ng i
Hnh 1.16. Thut ton lan truyn khong cch cho bn 511

Nguyn V Quang - on Nam Thi

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.

Hnh 1.17. M phng robot tm ng ton cc


Hi: Phng php lan truyn khong cch lun tm c ng ngn nht?
Tr li: Phng php trn l phng php tm ng ngn nht, tuy vy hiu
qu ca phng php ch tt khi cc li c kch thc nh xc nh
ng bin ca vt cn. Nu kch thc li qu ln, phng php trn s
khng cn ngha ti u.
Hi: u im v nhc im ca phng php ny?
Tr li: u im l thut ton lan truyn n gin v hi t nhanh, to ra ng
i ngn nht cho robot ti ch m khng chm vt cn.
Nhc im ca phng php l ng i to ra gy khc, khng trn. V vy
cn p dng 1 thut ton khc lm trn ng i.

Nguyn V Quang - on Nam Thi

18

Robot t hnh

PHN 2
CU TO PHN CNG V CC
PHNG PHP IU KHIN
TRONG ROBOT T HNH

Mt s sensor thng dng trong robot t hnh


Mch iu khin robot
9 Khi ngun
9 Khi iu khin
9 Khi sensor
9 Khi cng sut
9 Khi hin th
Phng php iu khin Robot
9 Phng php lm encoder t chut my
tnh
9 Phng php qut n Led 7 thanh
9 Phng php PWM iu khin tc cho
Robot
Xy dng mt bi ton n gin cho Robot
Hn ch ca Robot v bin php khc phc

Nguyn V Quang - on Nam Thi

19

Robot t hnh

I. Cc loi cm bin
1. Sensor quang (Optical Sensor)

Hnh 2.1. Sensor quang


Nguyn tc hot ng:
Gm 2 n: mt n thu (dectector) v mt n pht (emitting diode). Khi nh sng n
pht chiu vo vt cn th nh sng phn x s c nhn bit n thu.
ng dng:
Nhn bit vt cn trong robot t hnh

2. a encoder

Hnh 2.2. a Encoder


Nguyn tc hot ng
Gm b pht, b thu v a trn (b x rnh hoc c m ho), nguyn l
hot ng gn ging trng hp trn.

Nguyn V Quang - on Nam Thi

20

Robot t hnh

ng dng
o chuyn ng quay

3. Cng tc hnh trnh

Hnh 2.3. Cng tc hnh trnh


Nguyn tc hot ng
Cu to l 1 cp tip im, bnh thng hai tip im khng ni vi nhau, khi
b n chng chm vo nhau.
ng dng
Trnh cho robot va p vo nhng vt cng

II. Mch iu khin Robot


1. Khi ngun
9 Ngun ng lc ly t c qui 12V, 5A
9 Ngun iu khin ly t c qui qua IC n p 7805
S ngun cung cp cho khi iu khin:

U20
LM7805
VI

GND

NGUON AC QUI

VO

NGUON DIEU KHIEN


D22
LED

C4
CAP

C8
CAP NP

C9
CAP NP

C5
CAP
R2

Hnh 2.4. Mch ngun

Nguyn V Quang - on Nam Thi

21

Robot t hnh

2. Khi iu khin
Vi iu khin h 8051- AT89C51
9 S chn linh kin

Hnh 2.5. Vi iu khin AT89C51


Vi iu khin 89C51 c 4 port c th lm nhim v vo v ra. Cc port 1,2,3 c in tr
trong ko ln nn khi dng lm u vo v u ra khng cn in tr ko ln, ring port 0
phi dng in tr ngoi ko ln. Do port 0 thng dng lm u vo cc cng tc
hnh trnh, cc cm bin.
9 Thng s ca vi iu khin 8051

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

Nguyn V Quang - on Nam Thi

22

Robot t hnh

9 Ch nh thi 13 bit: B nh thi 8 bit, nh t l trc 5 bit.


9 Ch nh thi 16 bit, khng nh t l trc.
9 Ch t np li 8 bit. THx lu gi gi tr s t np vo TLx khi c trn.
9 Ch nh thi chia tch.
Trong gii hn bi ton robot t hnh ta ch tp trung phn tch ch 2 ca b nh
thi (ch np li 8 bit).
Ch np li 8 bit:
9 l b nh thi 8 bit nn ch cho php cc gi tr t 00 n FFH c np
vo thanh ghi TH ca b nh thi.
9 Sau khi TH c np gi tr 8 bit th 8051 sao ni dung vo TL v b inh
thi c khi ng.
9 B nh thi sau khi c khi ng th bt u m tng bng cch tng
thanh ghi TL. B nh thi m cho n khi t gi tr ti hn l FFH. Khi
quay vng t FFH v 00 th c b nh thi TF c thit lp.
9 Khi thanh ghi TL quay t FFH v 00, c TF c bt 1ln th thanh ghi TL t
ng np li gi tr ban u c gi trong thanh ghi TH.
9 C TF c t xo v b nh thi li bt u mt chu trnh mi m khng
cn bt k mt s can thip no ca lp trnh vin.
Minh ho cho ch t np li 8 bit:

Hnh 8.6. Ch np li 8 bits

Nguyn V Quang - on Nam Thi

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

Hnh 2.6 S mch iu khin

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:

Hnh 2.7. Cp thu pht

Tn hiu t 2 n thu c a vo cc b khuch i thut ton. Cc u cn


li ca b khuch i c a cc in p chun vo so snh. Khi khng c

Nguyn V Quang - on Nam Thi

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

Hnh 2.8. Mch sensor


Hi:
C th chnh c khong cch pht hin vt khng?
Tr li:
C bng cch iu chnh cc bin tr t cc ngng so snh, tuy nhin khong cch
chnh c khng ln (c khong 1mm).

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:

Hnh 2.9. IC74138

Nguyn V Quang - on Nam Thi

25

Robot t hnh

Hnh 2.10. Bng gi tr ca IC 74138


3 tn hiu u vo mch gii m c ni vi cc chn ca vi iu khin. Bng vic thay
i cc gi tr a ra 3 tn hiu iu khin ta s chn uc cc n khc nhau.
Minh ho: Khi 3 tn hiu iu khin bng 000 th n 1 (ni vi !Y0) s c chn, 001
th n 2 c chn.
9 Mch d liu
S dng b m ULN 2803

Hnh 2.11. ULN2803


Tn hiu u vo IC l 8 tn hiu ni t vi iu khin. Bng vic thay i gi tr a ra ta
s hin th cc s khc nhau trn led_7 nt.
Hi:
Kh nng ca phng php iu khin l g?
Tr li:
C th iu khin nhiu nht l 8 n Led 7 nt, v s dng ht 11 chn vo ra ca vi
iu khin.

Nguyn V Quang - on Nam Thi

26

Robot t hnh

5. Khi cng sut iu khin ng c


9 S dng IC L298
9 c tnh IC
in p 40V, dng 5A
Tch hp trong IC l 2 mch cu H do c kh nng iu khin 2 chiu 2
ng c DC
9 S mch cng sut iu khin 2 ng c DC
12V

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

Hnh 2.12. Mch cng sut


Hi:
u im ca L298 l g?
Tr li:
L298 c u im:
Kh nng iu khin d dng (4chn iu khin 2 ng c)
Khng xy ra hin tng ngn mch trong mch cu H nh trong cc mch
cu thng thng do c thm IC logic 7432 mch ngoi
Mch gn,tin cy
Hi:
Nhc im L298 l g?
Tr li:
Nhc im duy nht l gi thnh cao.

Nguyn V Quang - on Nam Thi

27

Robot t hnh

III. Cc phng php iu khin trong Robot t hnh


1. Phng php to encoder t chut my tnh

Hnh 2.13. Cu to bn trong chut my tnh


Ta ch ti 2 b phn ca chut:
9 2 a trn b x rnh
9 2 n pht v 4 n thu (2 n thu tch hp trong mt n thu mu en- xem nh)
Khi ta kt hp 2 b phn ny ca chut ta s nhn c 2 encoder. im mnh ca cp
encoder ny l chng vung gc vi nhau gn nh tuyt i (do cng ngh ch to chut
t n chnh xc rt cao). Nh vy ta hon ton c th s dng chng o 2
chuyn ng vung gc vi nhau.
Hi:
Ti sao trong mi mt n thu ca chut sao tch hp 2 n thu bn trong?
Tr li:
nhn bit chiu quay ca a trn.
S mch s dng chut lm encoder:

Nguyn V Quang - on Nam Thi

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

Hnh 2.14. Mch nhn bit chiu quay


Chiu quay ca a trn nhn bit thng qua chui xung nhn c hai tn hiu ra :
OUT 1 v OUT 2.
Chiu quay thun (theo qui c ca ngi s dng): ..11..01..10..11..01..10..11
Chiu quay ngc: ..11..10..01..11..10..01..11
T ta xy dng c thut ton nhn chiu quay ca chut:

Nguyn V Quang - on Nam Thi

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

Hnh 2.15. Lu nhn bit chiu quay

2. Phng php qut n led 7 thanh


Trong h vi iu khin 8051 (chp AT89C51), c hai b timer 0 v b timer 1. Ta dng
mt b thc hin vic qut n.
Ta dng mt bin t thc hin vic chn n:
Lu thut ton chn n:

Nguyn V Quang - on Nam Thi

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

Khi ngt th 2 xy ra:

Dng Timer 1

t=1?

chn n 2;
gi tr n 2 a ra port 3 ;
t=2 ;

Chy Timer 1

Nguyn V Quang - on Nam Thi

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

Nguyn V Quang - on Nam Thi

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.

3. Phng php to xung PWM iu khin tc cho robot


S dng Timer 0 ca vi iu khin to ra xung PWM. Timer 0 dng trong ch
np li 8 bit.
Ta nh ngha bt pwm cha gi tr iu khin. Khi ng timer 0, bt pwm=1. Khi
xy ra ngt th nht:

Dng Timer 0

Gi tr np li mi = 255-gi tr np li mi;
o pwm;

Khi ng li Timer;

Hnh dng xung PWM:

Nguyn V Quang - on Nam Thi

33

Robot t hnh

TH0 ban u

255

IV.

Xy dng mt bi ton cho Robot t hnh

Bi ton: Robot tm ng n ch, khi trn ng i c 2 vt cn. Robot t trnh vt


cn v tm n ch. Do hn ch v vn k thut, robot cha s dng c cc phng
php Phn 1, thay vo chng ti ch dng mt thut ton n gin iu khin
robot.

Hnh 2.16. Mt s hnh nh ca Robot

Nguyn V Quang - on Nam Thi

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

Hnh 2.17. Lu hot ng ca robot


Nguyn V Quang - on Nam Thi

35

Robot t hnh

V.

Hn ch Robot v phng php khc phc


9 Ch trnh c vt c mu ti hoc mu sng (do sensor ch chnh c mt
ngng)
Khc phc
B tr mt dy sensor xen k thnh tng cp, mt sensor nhn vt sng ng
cnh mt sensor nhn vt ti.
9 Cha c khng nhn ch r rng, mi ch dng li vic tnh qung ng
robot i c.
Khc phc
Gn thm cc cm bin ln robot v d nh cc cm bin nhit (i vi ch l
ngun nhit), b thu sng siu m (i vi ch l cc b pht sng siu m),..
9 Kh nng i thng cn hn ch
Khc phc
S dng chip linh hot hn vi iu khin AT 89C51 v d PIC,AVR,..

Kt cu c kh vng chc v chnh xc.

Nguyn V Quang - on Nam Thi

36

Robot t hnh

Ti liu tham kho


[1] Gii bi ton cc b trnh vt cn cho robot t hnh- Bo co Hi ngh C-in t
ln th I
[2] n tt nghip: Thit k kt cu c kh v h thng iu khin Robot t hnh
ATPRO (K45-KSTN-HBKHN)
[3] H vi iu khin 8051- Tng Vn On
[4] Datasheetarchive.com

Nguyn V Quang - on Nam Thi

37

You might also like