Luận văn Robot di động theo dấu tường
ĐẠI HỌC QUỐC GIA TP. HỒ CHÍ MINH
TRƯỜNG ĐẠI HỌC BÁCH KHOA
KHOA CƠ KHÍ
BOÄ MOÂN CÔ ÑIEÄN TÖÛ
&››
BK
LUẬN VĂN TỐT NGHIỆP ĐẠI HỌC
ROBOT DI ĐỘNG THEO
DẤU TƯỜNG
Sinh viên thực hiện: DOÃN MINH ĐĂNG
MSSV: P9900012
Cán bộ hướng dẫn : TS. NGUYỄN TẤN TIẾN
CHƯƠNG TRÌNH ĐÀO TẠO KỸ SƯ CHẤT LƯỢNG CAO
KHÓA 1: 1999 - 2004
TP.Hồ Chí Minh, 07/2004
Lôøi caûm ôn
Ñeå thöïc hieän ñeà taøi, taùc giaû ñaõ nhaän ñöôïc raát nhieàu söï chæ daãn, giuùp ñôõ vaø
ñoäng vieân quyù baùu cuûa nhieàu ngöôøi, thieáu moät trong caùc söï giuùp ñôõ ñoù cuõng coù theå
laøm cho ñeà taøi khoâng ñaït keát quaû nhö hieän nay.
Tröôùc heát, em xin baøy toû loøng caûm ôn saâu saéc ñoái vôùi thaày TS. Nguyeãn Taán
Tieán, ngöôøi thaày höôùng daãn ñaõ taän tình chæ cho em phöông phaùp nghieân cöùu khoa
hoïc, thaày cuõng ñaõ cung caáp cho em raát nhieàu kieán thöùc chuyeân saâu ñeå thöïc hieän ñeà
taøi.
Em cuõng voâ cuøng caûm ôn coâ Th.S Traàn Thò Ngoïc Dung vaø caùc thaày coâ ôû
chöông trình ñaøo taïo Kyõ sö chaát löôïng cao, thaày TS. Nguyeãn Vaên Giaùp vaø caùc thaày
coâ ôû boä moân Cô Ñieän Töû, Khoa Cô khí, Tröôøng Ñaïi Hoïc Baùch Khoa Tp.HCM ñaõ
tham gia quaù trình ñaøo taïo vaø höôùng daãn em trong suoát thôøi gian hoïc ñaïi hoïc, nhôø
caùc thaày coâ maø em coù ñuû kieán thöùc vaø loøng töï tin ñeå thöïc hieän ñeà taøi nghieân cöùu
naøy cuõng nhö caùc ñeà taøi trong töông lai.
Beân caïnh ñoù, söï hôïp taùc vaø giuùp ñôõ cuûa baïn beø vaø caùc theá heä ñaøn anh cuõng
giuùp toâi raát nhieàu trong vieäc thöïc hieän ñeà taøi naøy. Em xin ñöôïc caûm ôn KS. Löu
Tuaán Anh, Khoa Coâng ngheä Vaät Lieäu, ngöôøi ñaõ höôùng daãn em ñi vaøo nghieân cöùu
veø robot, Th.S Traàn Vaên Tuøng vaø caùc baïn ôû Phoøng thí nghieäm Thieát Keá Maùy ñaõ
tích cöïc giuùp ñôõ em trong thôøi gian thöïc hieän ñeà taøi. Toâi cuõng xin chaân thaønh caûm
ôn caùc baïn cuøng hoïc lôùp Cô Ñieän Töû – Vieät Phaùp 99, ñaëc bieät laø caùc baïn Ñoaøn
Hieäp, Nguyeãn Anh Kieät, Phaïm Huyønh Phong, Nguyeãn Minh Trung, nhöõng ngöôøi
cuøng nghieân cöùu veà robot di ñoäng ñaõ cho toâi caùc yù kieán ñoùng goùp quyù giaù!
Con cuõng xin caûm ôn gia ñình ñaõ luoân chaêm soùc vaø quan taâm ñeán vieäc hoïc cuûa
con, con voâ cuøng caûm ôn vaø luoân töï haøo vì coù Boá, Meï, Chò luoân ñoäng vieân con
trong quaù trình hoïc taäp.
Vaø cuoái cuøng, toâi xin göûi lôøi caûm ôn tôùi nhöõng ngöôøi ñaõ tham gia giuùp ñôõ toâi
trong quaù trình thöïc hieän luaän vaên maø toâi chöa neâu teân ôû ñaây, söï giuùp ñôõ cuûa hoï duø
ít hay nhieàu cuõng ñoùng goùp moät phaàn vaøo keát quaû thöïc hieän ñeà taøi toát nghieäp naøy.
Tp. Hoà Chí Minh, ngaøy 19 thaùng 7 naêm 2004
Doaõn Minh Ñaêng
Muïc luïc
Lôøi caûm ôn ..........................................................................................................................................................
Muïc luïc...............................................................................................................................................................i
Danh muïc caùc hình veõ ...............................................................................................................................iii
Danh muïc caùc baûng.....................................................................................................................................iii
Toùm taét ñeà taøi................................................................................................................................................iv
Abstract.............................................................................................................................................................v
1 Toång quan vaø ñaët vaán ñeà .......................................................................................................................1
1.1 Giôùi thieäu chung veà robot........................................................................................ 1
1.2 Toång quan veà caùc baøi toaùn cuûa robot di ñoäng [5] ...................................................... 4
1.3 Baøi toaùn di chuyeån theo töôøng vaø caùc nghieân cöùu lieân quan ...................................... 5
1.3.1 Giôùi thieäu baøi toaùn ........................................................................................ 6
1.3.2 Moâ hình toaùn hoïc.......................................................................................... 6
1.3.3 Muïc tieâu ñieàu khieån...................................................................................... 8
1.4 Phöông phaùp giaûi quyeát vaán ñeà................................................................................ 8
2 Toùm taét thuaät toaùn ñieàu khieån............................................................................................................9
2.1 Moâ hình boä ñieàu khieån............................................................................................ 9
2.2 Ñaëc tính boä ñieàu khieån (theo keát quaû chöùng minh vaø moâ phoûng) ............................... 9
3 Thieát keá vaø thöïc hieän phaàn cöùng....................................................................................................10
3.1 Kieán truùc robot ..................................................................................................... 10
3.2 Vi ñieàu khieån PIC 16F877[13]............................................................................... 11
3.3 Thieát keá khung giao tieáp I2C................................................................................. 13
3.3.1 Lyù do söû duïng giao tieáp I2C......................................................................... 13
3.3.2 Khung giao tieáp I2C trong robot................................................................... 13
3.4 Thieát keá ñeá di chuyeån vaø boä ñieàu khieån ñoäng cô.................................................... 14
3.4.1 Thieát keá ñeá di chuyeån ................................................................................. 14
3.4.2 Boä ñieàu khieån PID [15] ............................................................................... 14
3.5 Thieát keá caûm bieán................................................................................................. 16
3.5.1 Moâ hình toaùn hoïc cuûa caûm bieán................................................................... 16
3.5.2 Thöïc hieän caûm bieán .................................................................................... 17
3.6 Thieát keá caùc maïch ñieän töû..................................................................................... 19
3.6.1 Maïch module master................................................................................... 19
3.6.2 Maïch module slave..................................................................................... 20
4 Thöïc hieän boä ñieàu khieån vaø kieåm chöùng giaûi thuaät............................................................... 22
4.1 Sô ñoà giaûi thuaät chöông trình ................................................................................. 22
4.1.1 Giaûi thuaät cho master module ...................................................................... 23
4.1.2 Giaûi thuaät cho slave module ........................................................................ 24
4.2 Tieán haønh thí nghieäm............................................................................................ 25
4.3 So saùnh caùc keát quaû moâ phoûng vaø thí nghieäm ......................................................... 26
4.3.1 So saùnh keát quaû moâ phoûng baèng Matlab vôùi keát quaû thí nghieäm .................... 26
4.3.2 Caùc nhaän xeùt boå sung.................................................................................. 28
5 Keát luaän......................................................................................................................................................33
i
5.1 Ñoä thích hôïp cuûa giaûi thuaät.................................................................................... 33
5.2 Nhöõng haïn cheá cuûa ñeà taøi...................................................................................... 33
5.2.1 Veà vieäc cheá taïo phaàn cöùng .......................................................................... 33
5.2.2 Nhöõng hieän töôïng aûnh höôûng ñeán keát quaû vaø caùch khaéc phuïc........................ 33
5.3 Höôùng nghieân cöùu tieáp.......................................................................................... 34
TAØI LIEÄU THAM KHAÛO......................................................................................................................35
PHUÏ LUÏC A .................................................................................................................................................37
PHUÏ LUÏC B..................................................................................................................................................39
ii
Danh muïc caùc hình veõ
Hình 1.1 Moät soá hình aûnh veà robot vaø caùc öùng duïng ................................................4
Hình 1.2 Moâ hình baøi toaùn robot di ñoäng baùm töôøng ................................................7
Hình 3.1 Sô ñoà khoái cuûa all-following mobile robot ...............................................11
Hình 3.2 Sô ñoà chaân PIC 16F877 ............................................................................12
Hình 3.3 Moâ hình ñeá di chuyeån laät ngöôïc ...............................................................14
Hình 3.4 Boä ñieàu khieån PID vaän toác theo moâ hình song song ................................14
Hình 3.5 Ñaùp öùng cuûa boä ñieàu khieån PID vôùi kp=8, ki=1, kd=1.............................15
Hình 3.6 Ñaùp öùng cuûa boä ñieàu khieån PID vôùi kp=8.2, ki=1, kd=0.8.......................16
Hình 3.7 Moâ hình toaùn hoïc cuûa caûm bieán ...............................................................17
Hình 3.8 Phaàn ñeäm tín hieäu töø encoder vaøo vi ñieàu khieån ôû module master..........18
Hình 3.9 Hình chuïp module caûm bieán.....................................................................18
Hình 3.10 Sô ñoà nguyeân lyù cuûa maïch module master.............................................19
Hình 3.11 Hình chuïp module master.......................................................................20
Hình 3.12 Sô ñoà nguyeân lyù khoái xöû lyù chính cuûa module slave..............................20
Hình 3.13 Sô ñoà nguyeân lyù khoái khueách ñaïi coâng suaát cuûa module slave..............21
Hình 3.14 Hình chuïp module slave .........................................................................21
Hình 4.1 Löu ñoà giaûi thuaät cuûa master module .......................................................23
Hình 4.2 Löu ñoà giaûi thuaät cuûa slave module..........................................................24
Hình 4.3 Moâ hình thí nghieäm...................................................................................26
Hình 4.4 So saùnh ñoà thò cuûa vaän toác robot...............................................................27
Hình 4.5 So saùnh ñoà thò cuûa sai soá khoaûng caùch .....................................................27
Hình 4.6 So saùnh ñoà thò cuûa sai soá goùc....................................................................28
Hình 4.7 Giaù trò cuûa caûm bieán .................................................................................29
Hình 4.8 Giaù trò vaän toác goùc cuûa robot vaø vaän toác goùc (öôùc löôïng) cuûa töôøng .......29
Hình 4.9 Bieán ñoåi cuûa caùc sai leäch trong quaù trình hoaït ñoäng ................................30
Hình 4.10 Giaù trò vaän toác ra leänh cho 2 baùnh xe .....................................................30
Hình 4.11 So saùnh caùc ñoà thò e1 vaø e2 cuûa hai thí nghieäm.......................................32
Danh muïc caùc baûng
Baûng 1.1 Toùm taét lòch söû phaùt trieån cuûa coâng ngheä robot .........................................2
Baûng 4.1 Thoâng soá thí nghieäm ................................................................................26
Baûng 4.2 Thoâng soá cuûa 2 thí nghieäm (TN) duøng ñeå so saùnh...................................31
Baûng 5.1 Caùc hieän töôïng aûnh höôûng ñeán keát quaû vaø caùch khaéc phuïc ....................33
iii
Toùm taét ñeà taøi
Trong thôøi ñaïi coâng nghieäp ngaøy nay, Robot ngaøy caøng ñöôïc söû duïng phoå
bieán trong saûn xuaát cuõng nhö trong cuoäc soáng cuûa con ngöôøi. Robot ñaõ coù moät vò trí
quan troïng khoù coù theå thay theá ñöôïc, noù giuùp con ngöôøi ñeå laøm vieäc trong caùc ñieàu
kieän nguy hieåm, khoù khaên. Ngoaøi ra, Robot coøn ñöôïc duøng vaøo caùc lónh vöïc thaùm
hieåm khoâng gian, quaân söï, giaûi trí… Lónh vöïc Robot di ñoäng ñang ngaøy caøng chieám
ñöôïc söï quan taâm cuûa caùc nhaø nghieân cöùu vaø xaõ hoäi. Töø tình hình thöïc teá ñoù, vieäc
xaây döïng caùc chöông trình hoaït ñoäng cho caùc Robot laø ñieàu thieát yeáu ñaëc bieät ñoái
vôùi caùc Robot di ñoäng. Baøi toaùn Robot di ñoäng baùm töôøng (wall-following problem)
laø moät trong caùc baøi toaùn thöôøng gaëp cuûa Robot kieåu phaûn xaï (reactive paradigm),
noù ñaõ ñöôïc giaûi baèng nhieàu caùch khaùc nhau. Trong ñeà taøi "Robot di ñoäng theo daáu
töôøng", baøi toaùn Robot di ñoäng baùm töôøng ñöôïc giaûi quyeát baèng moät boä ñieàu khieån
hoài tieáp ñaày ñuû traïng thaùi maø keát quaû ñaõ ñöôïc chöùng minh baèng moâ phoûng. Moät
caûm bieán tieáp xuùc duøng caùc encoder ñöôïc taïo ra ñeå söû duïng cho robot. Moâ hình
robot ñöôïc cheá taïo ñeå tieán haønh thí nghieäm nhaèm kieåm chöùng giaûi thuaät cuûa boä
ñieàu khieån. Keát quaû thí nghieäm laø caên cöù ñeå phaùt trieån boä ñieàu khieån daønh cho baøi
toaùn wall-following trong caùc Robot sau naøy.
iv
Abstract
This project studies on control of a wall-following mobile robot. The wall is
assumed unknown. A tactile sensor is constructed to measure the angle and the
distance of mobile robot relatively to the wall that the mobile robot must follow. A
nonlinear controller is built based on Lyapunov stability. The experiment has been
carried out to verify the study. Based on this result, the proposed controller can be
used for control of a wall-following mobile robot problem.
v
Robot di ñoäng theo daáu töôøng
1. Toång quan vaø ñaët vaán ñeà
1
TOÅNG QUAN VAØ ÑAËT VAÁN ÑEÀ
1.1 Giôùi thieäu chung veà robot
Khaùi nieäm Robot theo nghóa chung thöôøng ñöôïc hieåu ñoàng nghóa vôùi khaùi
nieäm töï ñoäng hoaù coâng nghieäp, ñieàu naøy chæ ñuùng moät phaàn bôûi vì: thöù nhaát, Robot
chæ laø moät thaønh phaàn trong heä thoáng töï ñoäng hoaù, thöù hai laø töï thaân vieäc trình baøy,
mieâu taû Robot trong sinh hoaït xaõ hoäi ít nhieàu phoùng ñaïi.
Nhöõng Robot xuaát hieän laàn ñaàu tieân ôû NewYork vaøo ngaøy 9/10/1922 trong vôû
kòch”Rossum’s Universal Robot” cuûa nhaø soaïn kòch ngöôøi Tieäp Khaéc laø Karen
Chapek, coøn töø Robot laø moät caùch goïi khaùc cuûa töø Robota-theo tieáng Tieäp coù nghóa
laø coâng vieäc lao dòch. Khi ñoù, Karen Chapek cho raèng Robot laø nhöõng ngöôøi maùy
coù khaû naêng laøm vieäc nhöng khoâng coù khaû naêng suy nghó.
Gaàn moät theá kyû tieáp theo, khaùi nieäm robot ñaõ lieân tuïc ñöôïc phaùt trieån, ñoùng
goùp theâm bôûi nhieàu nhaø nghieân cöùu, nhieàu coâng ty chuyeân veà lónh vöïc robot. Döôùi
ñaây laø baûng toùm taét quaù trình lòch söû hình thaønh vaø phaùt trieån cuûa coâng ngheä cheá
taïo robot, vaø nhöõng taùc ñoäng cuûa khoa hoïc cuõng nhö xaõ hoäi ñoái vôùi töøng thôøi kyø [4].
1
Robot di ñoäng theo daáu töôøng
1. Toång quan vaø ñaët vaán ñeà
Baûng 1.1 Toùm taét lòch söû phaùt trieån cuûa coâng ngheä robot
Moác Nghieân cöùu vaø phaùt
ÖÙng duïng trong
coâng nghieäp
Kyõ thuaät hoã Caùc yeáu toá
thôøi
gian
1920
trieån
trôï
aûnh höôûng
Khaùi nieäm robot xuaát
hieän trong tieåu thuyeát
Phaùt minh ra caùnh tay
maùy
1940
1950
1960
Phaùt sinh khaùi nieäm
robot thoâng minh
Giôùi thieäu veà
boä nhôù voøng
Giôùi thieäu veà robot Phaùt trieån robot Maùy
ñöôïc ñieàu khieån baèng trong coâng nghieäp duøng transitor
tính
maùy tính
ÖÙng
duïng
ôû Giôùi thieäu vi
Taêng cöôøng nghieân NASA vaø NAVY xöû lyù
cöùu
Robot coù trí thoâng Söï boäc phaùt laàn Phaùt trieån vi Söï haïn cheá
1970
1980
minh nhaân taïo
ñaàu tieân veà robot xöû lyù
cuûa
kinh teá
neàn
Robot duøng trong Robot
nhöõng coâng vieäc nguy nghieäp thöïc teá vaø
hieåm (1983) caùc öùng duïng roäng Kyõ
coâng Kyõ thuaät soá
Nhu
caàu
taêng cöôøng
thuaät töï ñoäng
raõi khaùc
quang
Giôùi thieäu veà Ñieàu
robot thoâng minh logic
trong saûn xuaát
khieån Robot gaây
1990
2000
neân
thất
nghiệp
Nghieân
veà robot trí
thoâng
nhaân tạo
cöùu
minh
Robot gioáng con Caùc tieán boä
ngöôøi veà cô khí
2
Robot di ñoäng theo daáu töôøng
1. Toång quan vaø ñaët vaán ñeà
Tröôùc nhöõng naêm 1970, ngöôøi ta chæ taäp trung vaøo vieäc phaùt trieån nhöõng robot
tay maùy hoaït ñoäng trong caùc nhaø maùy coâng nghieäp. Sau ñoù môùi xuaát hieän nhöõng
khaùi nieäm veà robot thoâng minh, vaø caùc nghieân cöùu baét ñaàu taäp trung hôn vaøo robot
di ñoäng. Moät trong nhöõng chuyeân gia ñaàu ngaønh veà robot di ñoäng laø Hans P.
Moravec (baét ñaàu nghieân cöùu töø naêm 1964), vaø hieän nay, chuyeân nghieân cöùu veà
robot di ñoäng laø Sebastien Thruns.
Caùc robot di ñoäng coù ngöôøi ñieàu khieån ñaõ ñöôïc duøng cho caùc muïc ñích quaân
söï, caùc nhieäm vuï nguy hieåm nhö phaù mìn, thaêm doø ñaùy ñaïi döông, haàm moû, kieåm
tra caùc ñöôøng oáng ngaàm, hay thaêm doø sao Hoaû…
Saûn phaåm robot di ñoäng ñöôïc saûn xuaát ñaïi traø vaø ñöa vaøo thò tröôøng laàn ñaàu
tieân laø robot huùt buïi Roomba vaø Trilobite cuûa haõng Electrolux naêm 2003.
ÔÛ hình 1.1 laø moät soá hình aûnh veà caùc Robot vaø öùng duïng cuûa noù:
a. Robot töï haønh Sojourner thaùm hieåm sao Hoaû
b. Robot doø mìn
3
Robot di ñoäng theo daáu töôøng
1. Toång quan vaø ñaët vaán ñeà
c. Tay maùy duøng trong coâng nghieäp
d. Robot huùt buïi Trilobite
Hình 1.1 Moät soá hình aûnh veà robot vaø caùc öùng duïng
1.2 Toång quan veà caùc baøi toaùn cuûa robot di ñoäng [5]
Vaán ñeà "navigation" (taïm dòch laø "di chuyeån") laø vaán ñeà troïng taâm cuûa robot
di ñoäng. Ñeå di chuyeån ñöôïc, robot phaûi thöïc hieän moät loaït caùc taùc vuï, moãi taùc vuï
gaén vôùi moät baøi toaùn nhoû trong baøi toaùn "navigation". Caùc baøi toaùn ñoù goàm:
Mapping: laø coâng vieäc laäp baûn ñoà moâi tröôøng hoaït ñoäng cuûa robot. Neáu
khoâng ñöôïc cung caáp döõ lieäu tröôùc thì robot phaûi coù khaû naêng laäp baûn ñoà.
Positioning: laø vieäc ñònh vò, robot phaûi coù khaû naêng bieát ñöôïc mình ñang ôû
ñaâu trong baûn ñoà toaøn cuïc hoaëc ñòa phöông.
Path planning: laø vieäc hoaïch ñònh ñöôøng ñi saép tôùi cuûa robot, sau khi noù bieát
ñöôïc baûn ñoà vaø bieát mình ñang ôû vò trí naøo.
Motion control: laø vieäc ñieàu khieån cho robot di ñoäng, töùc laø ñieàu khieån caùc cô
caáu ñeå robot ñi theo con ñöôøng thu ñöôïc töø baøi toaùn "path planning".
4
Robot di ñoäng theo daáu töôøng
1. Toång quan vaø ñaët vaán ñeà
Obstacle avoidance: laø nhieäm vuï traùnh chöôùng ngaïi vaät khi robot ñang di
chuyeån.
1.3 Baøi toaùn di chuyeån theo töôøng vaø caùc nghieân cöùu
lieân quan
Baøi toaùn di chuyeån theo töôøng:
Vieäc di chuyeån theo töôøng (wall following) laø moät taùc vuï thöôøng thaáy ôû robot
di ñoäng, trong caùc moâi tröôøng bieát tröôùc hoaëc khoâng bieát tröôùc. Taùc vuï naøy ñöôïc
duøng vôùi caùc nhieäm vuï: traùnh chöôùng ngaïi vaät, ñi theo töôøng bieát tröôùc, ñi theo
töôøng khoâng bieát tröôùc.
Moät soá nghieân cöùu ñaõ thöïc hieän:
Turennout et al., 1992 [6]: moät boä ñieàu khieån hoài tieáp duøng boä quan saùt ñeå öôùc
löôïng khoaûng caùch vaø goùc giöõa robot vôùi töôøng, duøng caûm bieán sieâu aâm.
Medromi et al., 1994 [7]: duøng moät boä quan saùt ñeå öôùc löôïng traïng thaùi cuûa heä
phi tuyeán vôùi ñaàu vaøo khoâng bieát tröôùc.
Urzelai, J. et al., 1997 [8]: söû duïng boä ñieàu khieån môø ñeå ñieàu khieån robot
VEA-II duøng caûm bieán sieâu aâm.
Bemporad et al., 1997 [9]: ñöa ra höôùng tieáp caän duøng söï choàng chaát caûm bieán
ñeå öôùc löôïng toaï ñoä cuûa robot, trong ñoù duøng moät boä loïc Kalman ñeå keát hôïp tín
hieäu töø caûm bieán sieâu aâm vaø caûm bieán ñoä dòch chuyeån.
Yata et al., 1998 [10]: ñöa ra phöông phaùp baùm töôøng söû duïng vieäc ño goùc
nhôø caûm bieán sieâu aâm.
Chung Tan Lam et al, 2004 [11]: moät boä ñieàu khieån hoài tieáp phi tuyeán khi bieát
khoaûng caùch vaø goùc giöõa robot vôùi töôøng, moät boä ñieàu khieån döïa treân boä quan saùt
khi chæ bieát khoaûng caùch giöõa robot vôùi töôøng, duøng caûm bieán cô. Caùc boä ñieàu
khieån oån ñònh theo tieâu chuaån Lyapunov, caùc keát quaû moâ phoûng vaø thöïc nghieäm ñaõ
chöùng toû hieäu quaû cuûa caùc boä ñieàu khieån.
Nguyeãn Vieát Hieäp vaø Phaïm Ñình Anh Vuõ [3]: thieát keá boä ñieàu khieån hoài tieáp
phi tuyeán vaø boä ñieàu khieån döïa treân boä quan saùt ñeå ñieàu khieån robot ñi theo töôøng,
moâ phoûng caùc boä ñieàu khieån ñeå kieåm chöùng tính hoäi tuï vaø oån ñònh.
5
Robot di ñoäng theo daáu töôøng
1. Toång quan vaø ñaët vaán ñeà
Caùc keát quaû nghieân cöùu treân cho thaáy höôùng giaûi quyeát baøi toaùn robot di
chuyeån theo töôøng baèng caùc boä ñieàu khieån hoài tieáp phi tuyeán laø moät höôùng ñi thích
hôïp. ÔÛ ñeà taøi cuûa Nguyeãn Vieát Hieäp vaø Phaïm Ñình Anh Vuõ, caùc boä ñieàu khieån ñaõ
ñöôïc ñöa ra vaø chöùng minh baèng lyù thuyeát vaø moâ phoûng, nhöng chöa ñöôïc kieåm
nghieäm thöïc teá. Döïa treân thaønh quaû ñoù, luaän vaên naøy coù nhieäm vuï laø: Kieåm chöùng
lyù thuyeát nghieân cöùu veà baøi toaùn robot di chuyeån theo töôøng ñaõ ñöôïc xaây döïng
trong luaän vaên toát nghieäp ñi tröôùc.
Ñeå thöïc hieän muïc tieâu treân, luaän vaên naøy taäp trung vaøo caùc vaán ñeà sau:
- Nghieân cöùu caùc boä ñieàu khieån cho robot baùm töôøng.
- Thieát keá vaø cheá taïo moät ñeá di chuyeån (mobile platform).
- Thieát keá vaø cheá taïo moät caûm bieán tieáp xuùc gaén leân robot ñeå do sai soá giöõa
robot vôùi töôøng.
- Thieát keá vaø thöïc hieän caùc maïch ñieàu khieån cho robot.
- Laäp trình cho robot ñeå hieän thöïc caùc boä ñieàu khieån.
- Thí nghieäm vaø so saùnh keát quaû thí nghieäm vôùi keát quaû moâ phoûng.
- Nhaän xeùt keát quaû vaø keát luaän.
1.3.1 Giôùi thieäu baøi toaùn
Thieát keá vaø thöïc hieän boä ñieàu khieån duøng ñeå ñieàu khieån Mobile Robot di
chuyeån doïc theo töôøng vôùi vaän toác vaø khoaûng caùch töø Robot ñeán töôøng cho tröôùc.
Giaû thieát cuûa baøi toaùn:
* Ño ñöôïc khoaûng caùch d töø töôøng ñeán Robot.
* Ño ñöôïc goùc leäch giöõa Robot vaø töôøng.
* Töôøng laø ñöôøng cong trôn baát kyø vôùi baùn kính cong cuûa töôøng khoâng nhoû
hôn khoaûng caùch d.
1.3.2 Moâ hình toaùn hoïc
Moâ hình robot di ñoäng baùm töôøng ñöôïc cho nhö hình 1.2
6
Robot di ñoäng theo daáu töôøng
1. Toång quan vaø ñaët vaán ñeà
Hình 1.2 Moâ hình baøi toaùn robot di ñoäng baùm töôøng
Trong ñoù:
* (x,y): heä truïc toaï ñoä tuyeät ñoái.
* R: baùn kính baùnh xe cuûa Mobile Robot.
* b: khoaûng caùch töø taâm Mobile Robot ñeán baùnh xe.
* W:giao ñieåm cuûa truïc Y cuûa Mobile Robot vôùi töôøng.
* t: tieáp tuyeán vôùi töôøng taïi ñieåm W.
φ
*
*
: goùc ñònh höôùng cuûa Mobile Robot.
: goùc nghieâng cuûa töôøng.
φw
* d :khoaûng caùch töø Mobile Robot ñeán tieáp tuyeán t.
* d0 :khoaûng caùch yeâu caàu töø Mobile Robot ñeán töôøng.
e2
* : goùc leäch giöõa mobile robot vôùi töôøng.
Phöông trình ñoäng hoïc cuûa Mobile robot ñaõ ñöôïc nghieân cöùu bôûi caùc coâng
trình tröôùc ñaây cuûa nhieàu taùc giaû [6]. Vaän toác ñöôïc bieåu dieãn nhö sau:
7
Robot di ñoäng theo daáu töôøng
1. Toång quan vaø ñaët vaán ñeà
é &ù
x
cosφ 0
ù
é
ê
v
é ù
ê ú
ú
&
y = sinφ
0
1
ê ú
ê ú
ê
ê
ë
ú
ω
ë û
ê ú
&
ú
0
φ
û
ë û
[1-1]
v
Vôùi : vaän toác daøi cuûa Robot.
ω
: vaän toác goùc cuûa Robot.
ω
v
Moái quan heä giöõa , vôùi vaän toác goùc cuûa hai baùnh xe nhö sau:
ω
1/ R b / R
v
é
rw ù
ú
é
ê
ùé ù
=
ê
úê ú
ωlw
1/ R - b / R ω
ë ûë û
ë
û
[1-2]
ωlw ωrw
vôùi
,
: vaän toác goùc cuûa baùnh xe traùi vaø baùnh xe phaûi cuûa Mobile Robot.
1.3.3 Muïc tieâu ñieàu khieån
Yeâu caàu cuûa baøi toaùn laø phaûi ñieàu khieån robot chaïy theo bieân daïng song song
vôùi töôøng, sao cho caùc sai soá khoaûng caùch (e1) vaø (e2) hoäi tuï veà 0, robot hoaït ñoäng
oån ñònh trong moät vuøng laân caän ñieåm hoäi tuï.
1.4 Phöông phaùp giaûi quyeát vaán ñeà
Ñeå giaûi quyeát baøi toaùn robot di ñoäng ñi theo töôøng, hai boä ñieàu khieån ñaõ ñöôïc
trình baøy trong ñeà taøi luaän vaên toát nghieäp ngaønh Cô Ñieän Töû, tröôøng ÑHBK
Tp.HCM cuûa hai sinh vieân Nguyeãn Vieát Hieäp vaø Phaïm Ñình Anh Vuõ. Moät boä ñieàu
khieån laø hoài tieáp ñaày ñuû traïng thaùi (full-state feedback), moät boä ñieàu khieån laø döïa
treân boä quan saùt (observer-based) [2]. Trong ñeà taøi naøy, chuùng toâi söû duïng caùc keát
quaû ñoù ñeå thöïc hieän caùc boä ñieàu khieån, nhaèm kieåm tra tính chính xaùc cuûa keát quaû
moâ phoûng.
Theo giaû thieát cuûa baøi toaùn, ta bieát ñöôïc giaù trò cuûa caû 2 bieán traïng thaùi laø e1
vaø e2, nhö vaäy ta coù theå söû duïng moät boä hoài tieáp taát caû bieán traïng thaùi (full-state
feedback controller). Boä ñieàu khieån naøy seõ ñöôïc ñeà caäp ñeán ôû chöông sau.
8
Robot di ñoäng theo daáu töôøng
2. Toùm taét thuaät toaùn ñieàu khieån
2
TOÙM TAÉT THUAÄT TOAÙN ÑIEÀU KHIEÅN
2.1 Moâ hình boä ñieàu khieån
Coâng thöùc cuûa boä ñieàu khieån full-state feedback:
ì
ï
í
ï
î
v = vr
ˆ
ˆ
ω = k [v - (e + d )ω / cose ]e - k sin e + ω
2
r
1
0
w
2
1
1
2
w
&
ˆ
ωw = e1 (e1 + d0 )tan e2 - sine2 / k2
[2-1]
Trong ñoù:
vr
: vaän toác yeâu caàu
d0
: khoaûng caùch yeâu caàu
ˆ
ω
: öôùc löôïng vaän toác goùc cuûa töôøng
k1,k2
e1,e2
v
: caùc tham soá cuûa boä ñieàu khieån ñeå oån ñònh Lyapunov
: sai soá veà khoaûng caùch vaø goùc giöõa robot vôùi töôøng
: vaän toác daøi cuûa robot
ω
: vaän toác goùc cuûa robot (vaän toác cuûa vectô chæ höôùng robot)
2.2 Ñaëc tính boä ñieàu khieån (theo keát quaû chöùng minh
vaø moâ phoûng)
Qua keát quaû chöùng minh vaø moâ phoûng, boä ñieàu khieån naøy coù caùc tính chaát
sau:
- OÅn ñònh theo tieâu chuaån Lyapunov daïng 2.
- Caùc sai soá e1 vaø e2 hoäi tuï veà zero sau thôøi gian 8-10s, vôùi caùc tham soá moâ
phoûng laáy theo moâ hình thöïc.
9
Robot di ñoäng theo daáu töôøng
3. Thieát keá vaø thöïc hieän phaàn cöùng
3
THIEÁT KEÁ VAØ THÖÏC HIEÄN PHAÀN CÖÙNG
3.1 Kieán truùc robot
Robot ñöôïc thieát keá theo kieåu kieán truùc SA (subsumption architect) [12], phaân
thaønh 2 lôùp:
- Lôùp döôùi laø caùc boä ñieàu khieån ñoäng cô, boä thu tín hieäu caûm bieán.
- Lôùp treân laø boä ñieàu khieån trung taâm. Boä ñieàu khieån naøy khoâng can thieäp vaøo
hoaït ñoäng cuûa caùc boä phaän ôû lôùp döôùi.
Sô ñoà khoái ñieàu khieån ñöôïc cho ôû hình 3.1, goàm 01 khoái xöû lyù chính (master
module) vaø 02 khoái xöû lyù phuï (slave module).
Master module coù caùc chöùc naêng:
- Ñoïc tín hieäu töø caûm bieán, tính caùc sai soá veà khoaûng caùch vaø goùc giöõa robot
vôùi töôøng.
- Duøng giaûi thuaät ñieàu khieån full-state feedback ñeå tìm caùc vaän toác daøi vaø vaän
toác goùc môùi cho robot, nhaèm giaûm caùc sai soá.
- Chuyeån vaän toác cuûa robot thaønh vaän toác cuûa 2 baùnh xe, göûi caùc giaù trò ñoù cho
caùc slave module.
Slave module coù caùc chöùc naêng:
- Nhaän leänh töø master module (vaän toác mong muoán cuûa moãi baùnh xe).
- Tính vaän toác hieän thôøi cuûa robot qua soá xung hoài tieáp trong 1 chu kyø.
- Duøng giaûi thuaät ñieàu khieån PID ñeå tính giaù trò PWM, nhaèm ñieàu khieån baùnh
xe quay vôùi vaän toác ñöôïc ra leänh.
Giao tieáp giöõa master module vaø 2 slave module ñöôïc thöïc hieän döïa treân
chuaån giao tieáp I2C, chuaån naøy seõ ñöôïc giaûi thích roõ hôn ôû ñoaïn döôùi.
10
Robot di ñoäng theo daáu töôøng
3. Thieát keá vaø thöïc hieän phaàn cöùng
Hình 3.1 Sô ñoà khoái cuûa Wall-following mobile robot
3.2 Vi ñieàu khieån PIC 16F877[13]
Vi ñieàu khieån ñöôïc choïn duøng trong ñeà taøi laø loaïi vi ñieàu khieån PIC 16F877
cuûa coâng ty Microchip. Sau ñaây laø vaøi neùt chính cuûa vi ñieàu khieån naøy:
11
Robot di ñoäng theo daáu töôøng
3. Thieát keá vaø thöïc hieän phaàn cöùng
Boä xöû lyù chính:
· Loaïi boä xöû lyù: RISC CPU
· Chæ coù taát caû 35 leänh
· Haàu heát caùc leänh laø 1 chu kyø maùy,
chæ caùc leânh reõ nhaùnh laø 2 chu kyø
· Taàn soá toái ña: vôùi thaïch anh 20
MHz – chu kyø maùy laø 200ns
· Löu ñöôïc 8K leänh trong boä nhôù
chöông trình, moãi leänh 14 bits.
· Coù 368 x 8 bytes boä nhôù döõ lieäu
(RAM)
· Coù 256 x 8 bytes boä nhôù
EEPROM
· Nhieàu loaïi ngaét (14 loaïi)
· Power-on Reset (POR)
· Power-up Timer (PWRT) vaø
· Oscillator Start-up Timer (OST)
· Watchdog Timer (WDT) vôùi boä
dao ñoäng tích hôïp beân trong
· Baûo maät chöông trình
· Coù cheá ñoä SLEEP ñeå tieát kieäm
naêng löôïng
· In-Circuit Serial Programming
(ICSP - chuaån ghi boä nhôù chöông
trình khi vi xöû lyù vaãn ôû trong
maïch)
Hình 3.2 Sô ñoà chaân PIC 16F877
Thieát bò ngoaïi vi:
· Timer0: 8-bit timer/counter vôùi boä chia
tröôùc 8-bit
· Timer1: 16-bit timer/counter vôùi boä chia
tröôùc, coù theå hoaït ñoäng trong cheá ñoä
SLEEP
· Timer2: 8-bit timer/counter vôùi boä chia
tröôùc vaø chia sau 8-bit
· 2 boä tích hôïp Capture, Compare, PWM
- Capture 16-bit, ñoä phaân giaûi toái ña 12.5
ns
· Single 5V In-Circuit Serial
Programming
- Compare is 16-bit, ñoä phaân giaûi toái ña
200 ns
· In-Circuit Debugging
· Vi xöû lyù truy caäp ñöôïc vaøo boä nhôù
chöông trình
- PWM coù ñoä phaân giaûi toái ña 10-bit
· 8 keânh bieán ñoåi Analog-to-Digital 10-bit.
· Synchronous Serial Port (SSP) vôùi 2
chuaån: SPI (Master mode) vaø I2C
(Master/Slave)
· Universal Synchronous Asynchronous
Receiver Transmitter (USART/SCI)
· Parallel Slave Port (PSP) 8-bits, coù caùc
chaân ñieàu khieån RD, WR vaø CS
· Brown-out Reset (BOR)
· Ñieän aùp hoaït ñoäng roäng: 2.0V ñeán
5.5V
· Chòu ñöôïc nhieät ñoä trong moâi
tröôøng coâng nghieäp
· Naêng löôïng tieâu thuï ít:
< 0.6 mA ôû 3V, 4 MHz
20 mmA ôû 3V, 32 kHz
< 1 mmA ôû cheá ñoä standby
12
Robot di ñoäng theo daáu töôøng
3. Thieát keá vaø thöïc hieän phaàn cöùng
3.3 Thieát keá khung giao tieáp I2C
3.3.1 Lyù do söû duïng giao tieáp I2C
Vi ñieàu khieån PIC 16F877 hoã trôï nhieàu chuaån giao tieáp, trong ñoù coù chuaån
giao tieáp I2C ñöôïc söû duïng laøm cho giao tieáp giöõa caùc module trong robot naøy.
Chuùng toâi choïn söû duïng chuaån I2C vì moät soá lyù do sau:
- I2C hoã trôï moät maïng nhieàu thieát bò keát noái vôùi nhau. Soá thieát bò toái ña coù theå
coù trong maïng I2C goàm 1 module master vaø 127 module slave (hieän taïi môùi duøng 2
module slave). Nhö vaäy robot coøn nhieàu khaû naêng môû roäng veà sau.
- Chuaån I2C laø chuaån thoâng duïng, ñöôïc söû duïng nhieàu trong caùc linh kieän
duøng cheá taïo robot di ñoäng (nhö caûm bieán sieâu aâm SRF08, caûm bieán la baøn
CMPS03). Neáu sau naøy cheá taïo moät robot di ñoäng döïa treân cô sôû robot baùm theo
töôøng, ta cuõng deã daøng tích hôïp caùc linh kieän khaùc vaøo robot.
- Moâi tröôøng laäp trình ñang duøng hoã trôï toát cho vieäc giao tieáp baèng I2C.
Chuùng toâi ñaõ thöû nghieäm vaø thaáy chöông trình giao tieáp hoaït ñoäng oån ñònh.
Ñeå hieåu theâm veà chuaån giao tieáp I2C, xin xem phuï luïc A.
3.3.2 Khung giao tieáp I2C trong robot
Caùch thöùc truyeàn tín hieäu qua I2C trong robot ñöôïc quy ñònh nhö sau:
- Module master chæ ghi giaù trò vaøo caùc module slave, khoâng coù cheá ñoä ñoïc.
- Ñòa chæ cuûa module slave ñieàu khieån di chuyeån baùnh traùi laø 0xA0, cuûa
module ñieàu khieån di chuyeån baùnh phaûi laø 0xC0. Moãi khi muoán göûi döõ lieäu cho
module slave naøo, module master göûi 1 byte ñòa chæ cuûa slave ñoù, sau ñoù göûi tieáp 2
byte döõ lieäu.
- Döõ lieäu göûi töø module master ñeán module slave laø moät bieán soá nguyeân 16
bit. Tröôùc khi göûi noù phaûi ñöôïc caét thaønh 2 byte, sau khi module slave nhaän thì
gheùp 2 byte ñoù trôû laïi thaønh soá nguyeân 16 bit ban ñaàu.
- ÔÛ moãi chu kyø hoaït ñoäng, module master göûi cho moãi module slave moät döõ
lieäu, laø vaän toác yeâu caàu module slave ñaït ñöôïc. 2 döõ lieäu ñöôïc göûi lieân tieáp ñeå
giaûm ñoä treã ñaùp öùng giöõa 2 module slave.
13
Robot di ñoäng theo daáu töôøng
3. Thieát keá vaø thöïc hieän phaàn cöùng
3.4 Thieát keá ñeá di chuyeån vaø boä ñieàu khieån ñoäng cô
3.4.1 Thieát keá ñeá di chuyeån
Caùc thaønh phaàn cuûa ñeá di chuyeån:
- 2 baùnh daãn ñoäng, gaén vaøo 2 ñoäng cô coù hoäp giaûm toác, coù hoài tieáp baèng
encoder quang. Moãi ñoäng cô ñöôïc ñieàu khieån bôûi 1 maïch ñieän rieâng. Maïch ñieàu
khieån, ñoäng cô vaø baùnh xe caáu thaønh moät module di chuyeån, robot coù 2 module di
chuyeån traùi vaø phaûi taùch rôøi nhau.
- Baùnh tuyø ñoäng laø moät baùnh caàu, ñaët ôû sau xe.
Baûn veõ thieát keá: xem baûn veõ moâ hình robot (baûn veõ ñính keøm).
Hình 3.3 Moâ hình ñeá di chuyeån laät ngöôïc
3.4.2 Boä ñieàu khieån PID [15]
Moâ hình ñieàu khieån PID ñöôïc söû duïng ôû ñaây laø:
Hình 3.4 Boä ñieàu khieån PID vaän toác theo moâ hình song song
14
Robot di ñoäng theo daáu töôøng
3. Thieát keá vaø thöïc hieän phaàn cöùng
Boä PID ñöôïc söû duïng ñeå ñaûm baûo vaän toác quay maø module ñieàu khieån trung
taâm ra leänh cho module di chuyeån thöïc hieän.
- Ñaàu vaøo cuûa boä PID: vaän toác yeâu caàu, ñôn vò laø voøng/phuùt.
- Tín hieäu hoài tieáp: vaän toác hieän thôøi, töø soá xung encoder ñoïc ñöôïc trong 1 chu
kyø, ñoåi ra voøng/phuùt.
- Ñoái töôïng ñieàu khieån: vaän toác ñoäng cô.
- Ñaàu ra cuûa boä PID: giaù trò chu kyø ñoä roäng xung (PWM duty) cuûa ñieän aùp hai
ñaàu ñoäng cô.
Phöông phaùp thöïc hieän boä ñieàu khieån PID vaän toác: do khoâng coù nhieàu thôøi
gian ñeå tìm caùc thoâng soá cuûa ñoäng cô nhaèm moâ hình hoaù ñoäng cô, chuùng toâi laäp
trình boä ñieàu khieån ñoäng cô treân vi ñieàu khieån PIC vôùi caùc giaù trò kp, ki vaø kd thay
ñoåi ñöôïc. Khi hieäu chænh töø töø caùc tham soá kp, ki vaø kd vaø xem ñaùp öùng cuûa boä ñieàu
khieån, chuùng toâi löïa choïn ñöôïc boä tham soá thích hôïp cho boä ñieàu khieån.
Ta coù theå thaáy ñaùp öùng cuûa boä PID thay ñoåi theo söï thay ñoåi nhoû cuûa caùc heä
soá ñieàu khieån thoâng qua caùc ñoà thò ôû hình 3.5 vaø hình 3.6. Boä PID ñöôïc trình baøy ôû
hình 4.6 ñöôïc xem laø toát hôn boä PID ôû hình 3.5 (ñaùp öùng ñaït möùc yeâu caàu, ít dao
ñoäng), ñoù laø boä PID toát nhaát maø chuùng toâi tìm ñöôïc cho caùc module di chuyeån cuûa
robot.
PID,kp=8,ki=1,kd=1 - Dap ung cua bo dieu khien PID
30
25
command
20
15
10
5
response
0
0
200
400
600
800
1000
1200
1400
1600
1800
t(ms)
Hình 3.5 Ñaùp öùng cuûa boä ñieàu khieån PID vôùi kp=8, ki=1, kd=1
15
Robot di ñoäng theo daáu töôøng
3. Thieát keá vaø thöïc hieän phaàn cöùng
PID,kp=8.2,ki=1,kd=0.8 - Dap ung cua bo dieu khien PID
30
response
25
command
20
15
10
5
0
0
500
1000
1500
t(ms)
2000
2500
3000
Hình 3.6 Ñaùp öùng cuûa boä ñieàu khieån PID vôùi kp=8.2, ki=1, kd=0.8
Caùc chæ tieâu cuûa boä ñieàu khieån PID:
- Thôøi gian ñaït möùc (rise time): 400ms
- Ñoä voït loá (overshoot): 40% (töông öùng vôùi 2 xung encoder/10ms)
- Thôøi gian xaùc laäp (settling time): 1000ms
Nhaän xeùt: Do ñoäng cô ñöôïc söû duïng laø moät ñoäng cô khoâng toát (coâng suaát 4W,
toác ñoä toái ña khoaûng 1000 voøng/phuùt, tæ soá hoäp giaûm toác laø 10), coäng vôùi encoder coù
ñoä phaân giaûi khoâng cao (giaù trò voït loá 40% töông öùng vôùi 2 xung encoder trong 1
chu kyø laáy maãu laø 10ms), hôn nöõa ta laïi söû duïng ñoäng cô ôû toác ñoä quay thaáp, neân
keát quaû cuûa boä ñieàu khieån PID khoâng ñöôïc toát. Tuy nhieân, neáu khoâng coù caùc thaønh
phaàn I vaø D thì boä ñieàu khieån P thoâng thöôøng seõ khoâng theå ñaùp öùng ñöôïc vaän toác
mong muoán trong thôøi gian ngaén, ñoù laø lyù do phaûi söû duïng boä ñieàu khieån PID.
3.5 Thieát keá caûm bieán
3.5.1 Moâ hình toaùn hoïc cuûa caûm bieán
Vieäc thieát keá moät caûm bieán toát coù yù nghóa raát quan troïng trong robot di ñoäng
naøy, ñeå laøm ñöôïc ñieàu ñoù, ta caàn moät moâ hình toaùn hoïc hôïp lyù. Moät soá moâ hình
toaùn hoïc cuûa caûm bieán ñaõ ñöôïc nghieân cöùu, cuoái cuøng chuùng toâi ñöa ra moâ hình
toaùn hoïc ôû hình 3.7 ñeå thöïc hieän caûm bieán cho robot.
16
Robot di ñoäng theo daáu töôøng
3. Thieát keá vaø thöïc hieän phaàn cöùng
Hình 3.7 Moâ hình toaùn hoïc cuûa caûm bieán
Caûm bieán ñöôïc set vò trí ban ñaàu öùng vôùi e2=0 vaø d=d0. Khi robot chuyeån
ñoäng, 2 thanh tröôït tieáp xuùc vôùi töôøng qua 2 con laên taïi caùc tieáp ñieåm W vaø W'. Do
baùn kính cong cuûa töôøng lôùn, ta xem töôøng trong ñoaïn WW' laø thaúng. Hôn nöõa, nhôø
r nhoû neân e3 khoâng ñaùng keå, ta xem e3=0. Coâng thöùc tính e1 vaø e2 laø:
e1 = d - d0 = d1 *cos(e2 ) - d0
[3-1]
d1 - d2
e2 = φ - φw = arctg(
)
h
vôùi d1, d2: ñoä dòch chuyeån cuûa thanh tröôït 1 vaø 2
h: khoaûng caùch giöõa 2 thanh tröôït.
3.5.2 Thöïc hieän caûm bieán
Module caûm bieán bao goàm 3 boä phaän chính: 2 thanh tröôït coù con laên ôû ñaàu ñeå
tieáp xuùc vôùi töôøng, 2 encoder ñeå doø ñoä dòch chuyeån cuûa caùc thanh tröôït vaø 1 vi ñieàu
khieån ñeå ñoïc ñoä dòch chuyeån.
Thanh tröôït ñöôïc gaén loø xo (ôû ñaây duøng daây thun) ñaåy ra ñeå luoân tieáp xuùc vôùi
töôøng. Treân moãi thanh tröôït coù gaén daây keùo doïc theo thanh, khi thanh tröôït chuyeån
ñoäng, daây keùo seõ keùo con laên chuyeån ñoäng. Do con laên ñöôïc noái chaët vôùi truïc
17
Robot di ñoäng theo daáu töôøng
3. Thieát keá vaø thöïc hieän phaàn cöùng
encoder neân chuyeån ñoäng cuûa con laên seõ laøm quay ñóa cuûa encoder, vôùi moãi dòch
chuyeån nhoû cuûa ñóa, encoder seõ göûi xung veà cho vi ñieàu khieån ñeå xöû lyù. Moãi
encoder truyeàn tín hieäu veà cho vi ñieàu khieån qua 2 ñöôøng A vaø B, chuùng ñöôïc cho
vaøo moät IC flip-flop ñeå xaùc ñònh chieàu quay cuûa encoder. Vi ñieàu khieån nhaän tín
hieäu töø caùc encoder qua 2 coång CCP (capture) vaø duøng 2 chaân digital input (RE1 vaø
RE2) ñeå nhaän bieát chieàu quay göûi töø IC flip-flop, caùc xung göûi töø encoder vaøo coång
CCP seõ taïo ra ngaét (interrupt) ñeå tieän vieäc tính toaùn treân vi ñieàu khieån, tín hieäu ôû
caùc chaân digital input seõ cho vi ñieàu khieån bieát ñöôïc xung ñoù öùng vôùi chuyeån ñoäng
vaøo hay ra cuûa thanh tröôït.
Sô ñoà maïch thu tín hieäu töø encoder:
J2
U5A
RE1
5
6
2
3
4
3
2
1
Q
Q
D
CLK
VCC
VCC
1
4
CLR
PRE
R15
1k8
R16
1k8
CCP1
Encoder 1
7474
VCC
J3
U5B
RE2
9
8
12
11
4
3
2
1
Q
Q
D
CLK
VCC
VCC
13
10
CLR
PRE
R17
1k8
R18
1k8
CCP2
Encoder 2
7474
VCC
Hình 3.8 Phaàn ñeäm tín hieäu töø encoder vaøo vi ñieàu khieån ôû module master
Keát quaû thöïc hieän:
Hình 3.9 Hình chuïp module caûm bieán
18
Robot di ñoäng theo daáu töôøng
3. Thieát keá vaø thöïc hieän phaàn cöùng
3.6 Thieát keá caùc maïch ñieän töû
3.6.1 Maïch module master
VCC
SW1
R10 10k
C8
0.1uF
+
C9
470uF
RESET button
SW5
SW PUSHBUTTON 4
U1
SW4 SW PUSHBUTTON 3
SW3 SW PUSHBUTTON 2
J9
1
2
3
4
5
6
7
8
9
40
39
38
37
36
35
34
33
32
31
30
29
28
27
22
21
20
19
13
MCLR RB7
RA0
1
2
3
RB6
RB5
RB4
RB3
RB2
RB1
RB0
J8
J7
RA1
RA2
RA3
RA4
RA5
RE0
RE1 VDD
RE2 GND
VDD RD7
GND RD6
RC0 RD5
RC1 RD4
RC2 RD3
RC3 RD2
RC4 RD1
RC5 RD0
RC6 OSC1
RC7
SW2
3
2
1
4
3
2
1
SW PUSHBUTTON 1
J10
TxCLK
IR1
1
J1
RE1
2
VCC
RE2
10
IR2
PORTB_L
VCC 11
8
7
6
5
4
3
2
1
R11
10k
12
15
16
17
18
23
24
25
26
LS2
CCP2
CCP1
Q2 R12 330
C1815
SPEAKER
VCC
J6
J4
PIC16F877
PORT D
R13
4k7
1
2
3
4
1
2
3
R14
4k7
SCL
SDA
4
J5
I2C line
VCC
I2C line sup
4
3
2
1
VCC
TX
RX
Serial line
Hình 3.10 Sô ñoà nguyeân lyù cuûa maïch module master
Caùc thaønh phaàn vaø chöùc naêng cuûa chuùng trong maïch master:
- Vi ñieàu khieån PIC 16F877: boä xöû lyù cuûa caû maïch.
- Ngoõ giao tieáp noái tieáp: ñeå giao tieáp vôùi maïch naïp vaø chöông trình kieåm tra
treân maùy tính.
- Ngoõ giao tieáp I2C: ñeå giao tieáp vôùi 2 vi ñieàu khieån cuûa caùc module slave.
- 2 ngoõ noái vôùi encoder ñeå ñoïc tín hieäu töø caûm bieán, coù moät IC flip-flop ñeå
ñeäm cho tín hieäu töø encoder.
- 4 nuùt baám phuïc vuï vieäc nhaän leänh töø ngöôøi söû duïng.
- 1 loa daønh ñeå baùo hieäu caùc giai ñoaïn trong chöông trình.
19
Robot di ñoäng theo daáu töôøng
3. Thieát keá vaø thöïc hieän phaàn cöùng
Hình 3.11 Hình chuïp module master
3.6.2 Maïch module slave
Goàm 2 khoái: khoái xöû lyù chính vaø khoái khueách ñaïi coâng suaát.
VCC
SW1
R10 10k
C8
0.1uF
+
C9
470uF
RESET button
J2
R11
10k
U1
U5A
1
2
3
4
LS2
1
2
3
4
5
6
7
8
40
39
38
37
36
35
34
33
32
31
30
29
28
27
22
21
20
19
13
5
6
2
3
MCLR RB7
RA0
Q
Q
D
RB6
RB5
RB4
RB3
RB2
RB1
RB0
RA1
RA2
RA3
RA4
RA5
RE0
RE1 VDD
RE2 GND
VDD RD7
GND RD6
RC0 RD5
RC1 RD4
RC2 RD3
RC3 RD2
RC4 RD1
RC5 RD0
RC6 OSC1
RC7
CLK
Q2 R12 330
VCC
VCC
C1815
1
4
CLR
PRE
J1
To ENCODER
SPEAKER
R15
1k8
R16
1k8
7474
VCC
9
10
VCC
9
8
7
6
5
4
3
2
1
VCC 11
12
15
16
17
18
23
24
25
26
BRAKE
J3
PWM
DIR
1
3
2
4
PWM select
4BIT7SEG_DISPLAY
VCC
J4
PIC16F877
R13
4k7
1
2
3
4
R14
4k7
SCL
SDA
J5
I2C line
VCC
4
3
2
1
VCC
TX
RX
Serial line
Hình 3.12 Sô ñoà nguyeân lyù khoái xöû lyù chính cuûa module slave
Caùc thaønh phaàn trong khoái xöû lyù chính:
- Vi ñieàu khieån PIC 16F877: boä xöû lyù cuûa module.
- Ngoõ giao tieáp noái tieáp: ñeå giao tieáp vôùi maïch naïp vaø chöông trình kieåm tra
treân maùy tính.
- Ngoõ giao tieáp I2C: ñeå giao tieáp vôùi vi ñieàu khieån cuûa module master.
20
Robot di ñoäng theo daáu töôøng
3. Thieát keá vaø thöïc hieän phaàn cöùng
- Ngoõ noái vôùi encoder ñeå ñoïc tín hieäu hoài tieáp, coù moät IC flip-flop ñeå ñeäm cho
tín hieäu töø encoder.
- 1 loa daønh ñeå baùo hieäu.
Khoái khueách ñaïi coâng suaát:
JP1
D1 LED
D2 LED
D3 LED
R1
R2
R3
330
680
1k8
1
2
3
4
GND
VCC
12VDC
24VDC
POWERS
R4
100
100
ISO1 PC817
C1
R6
0.1uF
5k
BRAKE
C4
C5
R5
ISO2 PC817
C2
R7
0.1uF
5k
DIR
U2
JP2
MOTOR OUT 1
MOTOR OUT 2
1
11
2
10
BTP1
BTP2
OUT1
OUT2
1
2
PWM
4
3
5
8
9
To MOTOR
C6
BIN
DIN
PIN
CSOUT
TFOUT
U4A
7
GND
24VDC
C7
6
74LS04
VS
U3
R9
U4B
R8
C3
100
LMD18200/TO
1
8
N.C.
Vs
3
4
2
3
7
6
Anode
Cathode VOUT
VOUT
0.1uF
74LS04
4
5
N.C.
GND
TLP250
MOTOR DRIVER BLOCK
Hình 3.13 Sô ñoà nguyeân lyù khoái khueách ñaïi coâng suaát cuûa module slave
Caùc thaønh phaàn trong khoái khueách ñaïi coâng suaát:
- Chip ñieàu khieån ñoäng cô LMD18200.
- 3 opto ñeå caùch ly caùc ngoõ vaøo cuûa chip LMD18200, baûo veä phaàn maïch phía
tröôùc.
- Caùc coång logic ñeå naén tín hieäu cho ngoõ vaøo PWM.
Hình 3.14 Hình chuïp module slave
21
Robot di ñoäng theo daáu töôøng
4. Thöïc hieän boä ñieàu khieån
vaø kieåm chöùng giaûi thuaät
4
THÖÏC HIEÄN BOÄ ÑIEÀU KHIEÅN
VAØ KIEÅM CHÖÙNG GIAÛI THUAÄT
4.1 Sô ñoà giaûi thuaät chöông trình
Giaûi thuaät cho robot di ñoäng theo töôøng ñöôïc thöïc hieän nhôø 2 chöông trình,
moät chöông trình daønh cho master module (chöông trình chính) vaø chöông trình kia
daønh cho slave module (chöông trình phuï).
Chöông trình chính coù nhieäm vuï laø boä ñieàu khieån full-state feedback cuûa toaøn
heä thoáng, chöông trình phuï coù nhieäm vuï laø boä ñieàu khieån PID vaän toác cho moãi baùnh
xe. Chöùc naêng cuï theå cuûa töøng chöông trình ñöôïc ñeà caäp ôû phaàn 3.1.
Caùc chöông trình ñöôïc laäp trình baèng ngoân ngöõ C, bieân dòch cho caùc vi ñieàu
khieån PIC baèng trình bieân dòch PIC-C, sau ñoù naïp vaøo caùc vi ñieàu khieån (naïp
chöông trình chính vaøo vi ñieàu khieån ôû master module, naïp chöông trình phuï vaøo 2
vi ñieàu khieån ôû 2 slave module).
Maõ nguoàn cuûa caùc chöông trình: xem phuï luïc B.
22
Robot di ñoäng theo daáu töôøng
4. Thöïc hieän boä ñieàu khieån
vaø kieåm chöùng giaûi thuaät
4.1.1 Giaûi thuaät cho master module
Hình 4.1 Löu ñoà giaûi thuaät cuûa master module
23
Tải về để xem bản đầy đủ
62 trang
01/09/2024
390
Bạn đang xem 30 trang mẫu của tài liệu "Luận văn Robot di động theo dấu tường", để tải tài liệu gốc về máy hãy click vào nút Download ở trên.
File đính kèm:
- luan_van_robot_di_dong_theo_dau_tuong.pdf
