Robot di động theo dấu tường

ê ê ë é = ú ú ú û ù ê ê ê ë é w f f f v y x 10 0sin 0cos & & & [1-1] Vôùi v : vaän toác daøi cuûa Robot. w : vaän toác goùc cuûa Robot. Moái quan heä giöõa v , w vôùi vaän toác goùc cuûa hai baùnh xe nhö sau: ú û ù ê ë é ú û ù ê ë é - =ú û ù ê ë é ww w v RbR RbR lw rw //1 //1 [1-2] vôùi lww , rww : 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. Robot di ñoäng theo daáu töôøng 2. Toùm taét thuaät toaùn ñieàu khieån 9 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: ï î ï í ì -+= +-+-= = 222011 2112012 /sintan)(ˆ ˆsin]cos/ˆ)([ keedee ekeedevk vv w wwr r w www & [2-1] Trong ñoù: rv : vaän toác yeâu caàu 0d : khoaûng caùch yeâu caàu wˆ : öôùc löôïng vaän toác goùc cuûa töôøng 21 , kk : caùc tham soá cuûa boä ñieàu khieån ñeå oån ñònh Lyapunov 21 ,ee : sai soá veà khoaûng caùch vaø goùc giöõa robot vôùi töôøng v : vaän toác daøi cuûa robot w : 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. Robot di ñoäng theo daáu töôøng 3. Thieát keá vaø thöïc hieän phaàn cöùng 10 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. Robot di ñoäng theo daáu töôøng 3. Thieát keá vaø thöïc hieän phaàn cöùng 11 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: Robot di ñoäng theo daáu töôøng 3. Thieát keá vaø thöïc hieän phaàn cöùng 12 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) · Single 5V In-Circuit Serial Programming · In-Circuit Debugging · Vi xöû lyù truy caäp ñöôïc vaøo boä nhôù chöông trình · Ñ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 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 - Compare is 16-bit, ñoä phaân giaûi toái ña 200 ns - 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) Robot di ñoäng theo daáu töôøng 3. Thieát keá vaø thöïc hieän phaàn cöùng 13 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. Robot di ñoäng theo daáu töôøng 3. Thieát keá vaø thöïc hieän phaàn cöùng 14 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 Robot di ñoäng theo daáu töôøng 3. Thieát keá vaø thöïc hieän phaàn cöùng 15 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. 0 200 400 600 800 1000 1200 1400 1600 1800 0 5 10 15 20 25 30 PID,kp=8,ki=1,kd=1 - Dap ung cua bo dieu khien PID t(ms) v (rp m ) command response Hình 3.5 Ñaùp öùng cuûa boä ñieàu khieån PID vôùi kp=8, ki=1, kd=1 Robot di ñoäng theo daáu töôøng 3. Thieát keá vaø thöïc hieän phaàn cöùng 16 0 500 1000 1500 2000 2500 3000 0 5 10 15 20 25 30 PID,kp=8.2,ki=1,kd=0.8 - Dap ung cua bo dieu khien PID t(ms) v (rp m ) command response 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. Robot di ñoäng theo daáu töôøng 3. Thieát keá vaø thöïc hieän phaàn cöùng 17 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ø: )( )cos(* 21 2 02101 h ddarctge deddde w - =-= -=-= ff [3-1] 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 Robot di ñoäng theo daáu töôøng 3. Thieát keá vaø thöïc hieän phaàn cöùng 18 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: VCC CCP2 VCC R17 1k8 VCC VCC RE1 J3 Encoder 2 1 2 3 4 VCC J2 Encoder 1 1 2 3 4 CCP1 U5A 7474 2 3 5 6 4 1 D CLK Q Q PRE CLR R16 1k8 RE2 R15 1k8 VCC R18 1k8 U5B 7474 12 11 9 8 10 13 D CLK Q Q PRE CLR 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 Robot di ñoäng theo daáu töôøng 3. Thieát keá vaø thöïc hieän phaàn cöùng 19 3.6 Thieát keá caùc maïch ñieän töû 3.6.1 Maïch module master RX SW2 SW PUSHBUTTON 1 J1 PORT D 1 2 3 4 5 6 7 8 RE2 VCC LS2 SPEAKER J5 Serial line 1 2 3 4 R11 10k 1 2 3 4 SW5SW PUSHBUTTON 4 RE1 CCP1 C8 0.1uF VCC J10 TxCLK 1 2 Q2 C1815 R14 4k7 SDA U1 PIC16F877 2 3 4 5 6 7 33 34 35 36 37 38 39 40 15 16 17 18 23 24 25 26 31 32 1 13 14 11 12 8 9 10 19 20 21 22 27 28 29 30 RA0 RA1 RA2 RA3 RA4 RA5 RB0 RB1 RB2 RB3 RB4 RB5 RB6 RB7 RC0 RC1 RC2 RC3 RC4 RC5 RC6 RC7 GND VDD MCLR OSC1 O S C 2 VDD GND RE0 RE1 RE2 RD0 RD1 RD2 RD3 RD4 RD5 RD6 RD7 VCC J7 PORTB_L 1 2 3 4 J6 I2C line sup 1 2 3 4 VCC VCC SCL R10 10k SW1 RESET button CCP2 SW3 SW PUSHBUTTON 2 R13 4k7 R12 330 J8 IR2 1 2 3 J9 IR1 1 2 3 VCC SW4 SW PUSHBUTTON 3 TX + C9 470uF J4 I2C line 1 2 3 4 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. Robot di ñoäng theo daáu töôøng 3. Thieát keá vaø thöïc hieän phaàn cöùng 20 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. C8 0.1uF + C9 470uF BRAKE SW1 RESET button R14 4k7 U1 PIC16F877 2 3 4 5 6 7 33 34 35 36 37 38 39 40 15 16 17 18 23 24 25 26 31 32 1 13 14 11 12 8 9 10 19 20 21 22 27 28 29 30 RA0 RA1 RA2 RA3 RA4 RA5 RB0 RB1 RB2 RB3 RB4 RB5 RB6 RB7 RC0 RC1 RC2 RC3 RC4 RC5 RC6 RC7 GND VDD MCLR OSC1 O SC 2 VDD GND RE0 RE1 RE2 RD0 RD1 RD2 RD3 RD4 RD5 RD6 RD7 VCC VCC VCC VCC SDA PWM R11 10kLS2 SPEAKER J2 To ENCODER 1 2 3 4 TX 1 2 3 4 VCC R13 4k7 Q2 C1815 J1 4BIT7SEG_DISPLAY 1 2 3 4 5 6 7 8 9 J4 I2C line 1 2 3 4 R12 330 J5 Serial line 1 2 3 4 VCC RX DIR R10 10k U5A 7474 2 3 5 6 4 1 D CLK Q Q PRE CLR VCC SCL VCC J3 PWM select 1 2 3 4 VCC R16 1k8 R15 1k8 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. Robot di ñoäng theo daáu töôøng 3. Thieát keá vaø thöïc hieän phaàn cöùng 21 - 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: R6 5k C5 PWM BRAKE C1 0.1uF MOTOR OUT 2 D2 LED ISO1 PC817 1 2 3 4 R1 330 MOTOR OUT 1 VCC U4A 74LS04 1 2 C4 C3 0.1uF DIR C6 24VDC TLP250 U3 1 2 3 4 6 7 8 5 N.C. Anode Cathode N.C. VOUT VOUT Vs GND ISO2 PC817 1 2 3 4 C2 0.1uF D1 LED D3 LED R9 R4 100 24VDC R8 100 R2 680 R3 1k8 JP1 POWERS 1 2 3 4 R5 100 U2 LMD18200/TO 1 2 6 7 8 9 1011 3 4 5 BTP1 OUT1 VS GND CSOUT TFOUT OUT2BTP2 DIN BIN PIN GND JP2 To MOTOR 1 2 U4B 74LS04 3 4 MOTOR DRIVER BLOCK R7 5k C7 12VDC 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 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 22 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. 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 23 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 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 24 4.1.2 Giaûi thuaät cho slave module Hình 4.2 Löu ñoà giaûi thuaät cuûa slave module 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 25 4.2 Tieán haønh thí nghieäm Caùc thí nghieäm ñöôïc thöïc hieän nhö sau: * Moâi tröôøng hoaït ñoäng: goàm töôøng vaø saøn nhaø. Töôøng caàn ñöôïc loùt moät lôùp ñeäm ñeå coù beà maët eâm vaø trôn. Beà maët trôn ñeå giaûm löïc ma saùt taùc ñoäng leân robot thoâng qua caûm bieán tieáp xuùc, beà maët cuõng caàn eâm ñeå giaûm rung ñoäng cho caûm bieán, vì theo quan saùt, söï rung ñoäng ôû caûm bieán seõ gaây ra sai soá ñoïc encoder. Saøn nhaø cuõng ñöôïc loùt ñeäm ñeå taêng heä soá ma saùt giöõa baùnh xe vôùi saøn, vaø cuõng nhaèm giaûm rung ñoäng cho robot. * Robot di ñoäng: robot ñöôïc nuoâi baèng nguoàn ñieän ôû ngoaøi. Thoâng qua caùc thieát bò bieán theá, oån aùp, töø nguoàn ñieän xoay chieàu 220V/50Hz ta coù ñöôïc caùc nguoàn moät chieàu (5V, 12V vaø 24V) ñeå nuoâi robot. Treân module master cuûa robot coù caém daây noái vôùi maùy tính qua coång giao tieáp noái tieáp, ñeå robot truyeàn caùc keát quaû ño ñöôïc veà maùy tính. * Maùy tính: chuùng toâi duøng moät maùy tính laptop coù coång giao tieáp noái tieáp (chuaån RS-232) ñeå vöøa ra leänh cho robot, vöøa quan saùt caùc traïng thaùi cuûa robot. Maùy tính coù moät chöông trình daïng "terminal" ñeå göûi leänh vaø nhaän döõ lieäu qua coång noái tieáp, döõ lieäu nhaän ñöôïc seõ ñöôïc löu thaønh file keát quaû, file naøy seõ ñöôïc xöû lyù baèng phaàn meàm Matlab ñeå theå hieän keát quaû thí nghieäm qua caùc ñoà thò. Tieán trình thöïc hieän moät thí nghieäm goàm caùc böôùc: - Baät nguoàn ñieän, reset caùc vi ñieàu khieån treân robot, caøi ñaët thoâng soá cho chöông trình terminal treân maùy tính. Caøi ñaët vò trí chuaån cuûa caûm bieán, sau ñoù dòch chuyeån robot ñeå taïo ñoä leäch ban ñaàu cho caûm bieán. - Truyeàn caùc tham soá cho module master cuûa robot, caùc thoâng soá ñöôïc truyeàn theo thöù töï goàm: khoaûng caùch mong muoán d0, vaän toác mong muoán vr, thôøi gian laáy maãu st, caùc tham soá cuûa boä ñieàu khieån k1 vaø k2. - Ñôïi robot di chuyeån ñeán heát ñoaïn ñöôøng caàn thí nghieäm, ngöøng robot vaø löu keát quaû thí nghieäm. - Chaïy phaàn meàm Matlab ñeå xöû lyù keát quaû thí nghieäm vaø nhaän xeùt. Do bò giôùi haïn veà thôøi gian nghieân cöùu, caùc thí nghieäm treân töôøng cong chöa ñöôïc thöïc hieän. Chuùng toâi chæ xin trình baøy ôû ñaây caùc thí nghieäm robot di chuyeån theo töôøng thaúng. 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 26 Hình 4.3 Moâ hình thí nghieäm 4.3 So saùnh caùc keát quaû moâ phoûng vaø thí nghieäm 4.3.1 So saùnh keát quaû moâ phoûng baèng Matlab vôùi keát quaû thí nghieäm Chuùng toâi tieán haønh raát nhieàu thí nghieäm vôùi töôøng thaúng, sau ñaây laø moät soá thí nghieäm cho keát quaû ñieån hình vaø caùc nhaän xeùt. Baûng 4.1 Thoâng soá thí nghieäm Tham soá Giaù trò Ñôn vò Ghi chuù d0 0.25 m d0 laø gioáng nhau trong taát caû caùc thí nghieäm vr 0.05 m/s st 0.4 s st khaù lôùn, do boä ñieàu khieån ñoäng cô ñaùp öùng chaäm k1 3.5 k2 615 e1 ban ñaàu 0.0178 m Tính ngöôïc töø giaù trò hai encoder ôû caûm bieán e2 ban ñaàu -0.1955 rad Tính ngöôïc töø giaù trò hai encoder ôû caûm bieán Ñoà thò so saùnh giöõa keát quaû moâ phoûng vaø keát quaû thí nghieäm: 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 27 0.4 5 10 15 20 25 30 -1 -0.5 0 0.5 1 1.5 2 Experiment 14 - GIA TRI CUA BIEN w t(s) (ra d/ s) thuc mo phong Hình 4.4 So saùnh ñoà thò cuûa vaän toác robot 0.4 5 10 15 20 25 30 -10 -5 0 5 10 15 20 25 Experiment 14 - DO LECH KHOANG CACH e1 t(s) e 1 (m m ) mo phong thuc Hình 4.5 So saùnh ñoà thò cuûa sai soá khoaûng caùch 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 28 0.4 5 10 15 20 25 30 -15 -10 -5 0 5 10 15 20 Experiment 14 - DO LECH GOC e2 t(s) e 2 (d eg re e) mo phong thuc Hình 4.6 So saùnh ñoà thò cuûa sai soá goùc Nhaän xeùt 1: Keát quaû thöïc coù ñoà thò khaù gioáng daïng ñoà thò cuûa keát quaû moâ phoûng, ñieàu naøy cho thaáy vieäc moâ phoûng ñaõ chæ ñöôøng ñuùng ñaén cho thöïc nghieäm. Nhaän xeùt 2: Keát quaû thöïc khaù xaáu so vôùi keát quaû moâ phoûng. Ñieàu naøy cuõng deã hieåu vì robot thöïc cuûa chuùng ta chòu nhieàu taùc ñoäng cuûa caùc sai soá khi cheá taïo, caùc sai soá do linh kieän phaàn cöùng vaø caùc sai soá nhieãu, trong khi vieäc moâ phoûng ñöôïc thöïc hieän trong moâi tröôøng lyù töôûng. Nhaän xeùt 3: Boä ñieàu khieån laø hoäi tuï. Thôøi gian hoäi tuï laø khoaûng 10s theo moâ phoûng vaø khoaûng 15s theo keát quaû thí nghieäm (ta ñaùnh giaù giaù trò thôøi gian naøy laø döïa vaøo nhaän xeùt raèng töø thôøi ñieåm 15s, caùc giaù trò e1 vaø e2 khoâng thay ñoåi nhieàu, chuùng dao ñoäng ôû gaàn ñieåm caân baèng). 4.3.2 Caùc nhaän xeùt boå sung Ta xeùt theâm caùc ñoà thò keát quaû cuûa thí nghieäm ñaõ neâu ôû treân: 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 29 0 5 10 15 20 25 30 -1000 -500 0 500 1000 Experiment 14 - GIA TRI CUA BIEN encoder1 VA encoder2 t(s) e 1 , e 2 (p ul se ) encoder2 encoder1 Hình 4.7 Giaù trò cuûa caûm bieán 0.4 5 10 15 20 25 30 -1 -0.8 -0.6 -0.4 -0.2 0 0.2 0.4 0.6 0.8 1 Experiment 14 - GIA TRI CUA BIEN w VA ww all t(s) (ra d/ s) ww all w 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 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 30 0.4 5 10 15 20 25 30 -0.25 -0.2 -0.15 -0.1 -0.05 0 0.05 0.1 0.15 0.2 0.25 Experiment 14 - e1 VA e2 t(s) e 1 (m *1 0- 1 ) /e 2 (ra d) e2 e1 Hình 4.9 Bieán ñoåi cuûa caùc sai leäch trong quaù trình hoaït ñoäng 0 5 10 15 20 25 30 0 5 10 15 20 25 30 Experiment 14 - SO SANH wlef t VA wright t(s) w le ft/ w rig ht (r pm ) wright wlef t Hình 4.10 Giaù trò vaän toác ra leänh cho 2 baùnh xe 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 31 Nhaän xeùt 4: Boä ñieàu khieån laø oån ñònh vôùi ñoä leäch ban ñaàu naèm trong vuøng laân caän ñieåm caân baèng. (thaät ra thí nghieäm ñöôïc neâu treân ñaây coù ñoä leäch ban ñaàu naèm gaàn bieân cuûa vuøng laân caän ñoù). Hai thí nghieäm sau ñaây seõ cho thaáy toác ñoä hoäi tuï cuûa 2 sai soá khoaûng caùch vaø goùc: Baûng 4.2 Thoâng soá cuûa 2 thí nghieäm (TN) duøng ñeå so saùnh Tham soá Giaù trò Ñôn vò Ghi chuù TN1 TN2 d0 0.25 0.25 m vr 0.05 0.05 m/s st 0.4 0.4 s k1 3.5 3.5 k2 615 615 e1 ban ñaàu 0.0017 0.0160 m TH1: e1 ban ñaàu raát nhoû; TH2: e1 ban ñaàu lôùn e2 ban ñaàu -0.1203 0.0516 m TH1: e2 ban ñaàu lôùn; TH2: e2 ban ñaàu raát nhoû Caùc ñoà thò keát quaû ñöôïc cho ôû hình 4.11. Nhaän xeùt 5: Sai soá veà goùc (e2) hoäi tuï nhanh hôn sai soá veà khoaûng caùch (e1). Khi robot nhaän e1 ban ñaàu lôùn, thôøi gian hoäi tuï chung cuûa caû 2 sai soá seõ bò keùo daøi hôn so vôùi tröôøng hôïp e2 ban ñaàu lôùn. 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 32 0.4 5 10 15 20 25 30 35 -0.25 -0.2 -0.15 -0.1 -0.05 0 0.05 0.1 0.15 0.2 0.25 Experiment 10 - e1 VA e2 t(s) e 1 (m *1 0- 1 ) /e 2 (ra d) e2 e1 a. Ñoà thò e1 vaø e2 cuûa TN1 0.4 5 10 15 20 25 30 35 -0.25 -0.2 -0.15 -0.1 -0.05 0 0.05 0.1 0.15 0.2 0.25 Experiment 15 - e1 VA e2 t(s) e 1 (m *1 0- 1 ) /e 2 (ra d) e1 e2 b. Ñoà thò e1 vaø e2 cuûa TN2 Hình 4.11 So saùnh caùc ñoà thò e1 vaø e2 cuûa hai thí nghieäm Robot di ñoäng theo daáu töôøng 5. Keát luaän 33 5 KEÁT LUAÄN 5.1 Ñoä thích hôïp cuûa giaûi thuaät Luaän vaên naøy ñaõ kieåm nghieäm ñöôïc boä ñieàu khieån cho robot di ñoäng baùm töôøng. Boä ñieàu khieån ñöôïc duøng laø boä ñieàu khieån hoài tieáp taát caû traïng thaùi (full- state feedback controller). Keát quaû thí nghieäm ñaõ kieåm chöùng tính hoäi tuï vaø oån ñònh Lyapunov cuûa boä ñieàu khieån. 5.2 Nhöõng haïn cheá cuûa ñeà taøi 5.2.1 Veà vieäc cheá taïo phaàn cöùng Do chöa coù nhieàu kinh nghieäm trong vieäc gia coâng cô khí, chuùng toâi ñaõ phaûi maát raát nhieàu thôøi gian cho vieäc cheá taïo phaàn cô cuûa robot. Tuy vieäc cheá taïo coøn nhieàu sai soá, robot cuõng ñaït ñieàu kieän vöøa ñuû ñeå phuïc vuï cho vieäc thí nghieäm. Khuyeát ñieåm lôùn nhaát ôû robot naøy laø söû duïng caùc ñoäng cô cuõ (2 ñoäng cô mua ôû chôï ñoäng cô cuõ treân ñöôøng Vónh Vieãn, Q.10, Tp.HCM), vôùi caùc thoâng soá khoâng ñaûm baûo vaø moâmen taûi nhoû, ñoäng cô coù ñaùp öùng chaäm keùo theo thôøi gian laáy maãu cuûa heä thoáng phaûi lôùn, laøm cho keát quaû thí nghieäm khoâng toát nhö mong muoán. 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 Trong quaù trình thöïc hieän ñeà taøi, chuùng toâi quan saùt thaáy moät soá hieän töôïng coù theå gaây sai leäch ôû keát quaû. Nhaèm giuùp caùc baïn sinh vieân ñi sau tieát kieäm thôøi gian, chuùng toâi xin trình baøy caùc hieän töôïng ñoù vaø ñeà xuaát caùch khaéc phuïc. 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 Hieän töôïng Caùch khaéc phuïc Ñoä rô cuûa caùc khôùp noái giöõa truïc ñoäng cô (qua hoäp giaûm toác) vaø baùnh xe, baùn kính 2 baùnh daãn ñoäng khoâng baèng nhau. Haäu quaû: boä ñieàu khieån PID hoaït ñoäng khoâng toát. Gia coâng baùnh xe baèng kim loaïi (neân duøng nhoâm), coá gaéng giaûm sai soá khi khoan loã truïc baùnh xe. Robot di ñoäng theo daáu töôøng 5. Keát luaän 34 Rung ñoäng ôû caùc thanh tröôït vaø encoder treân caûm bieán seõ gaây ra sai soá cuûa caûm bieán, sai soá tích luyõ trong thôøi gian hoaït ñoäng cuûa robot seõ ñuû lôùn ñeå daãn ñeán keát quaû xaáu ôû boä ñieàu khieån. Gaù chaët encoder vaøo caûm bieán; söû duïng caùc thanh tröôït chính xaùc; giaûm rung ñoäng cho robot baèng vieäc taïo ra töôøng vaø saøn eâm. Chöông trình ñieàu khieån lôùn, khi bieân dòch baèng PICC coù theå khoâng hoaït ñoäng ñuùng (do loãi cuûa trình bieân dòch) Tieát kieäm boä nhôù chöông trình baèng caùch haïn cheá duøng bieán soá thöïc vaø caùc leänh xuaát/nhaäp qua coång noái tieáp, neân giôùi haïn chöông trình trong khoaûng <80% ROM. Keát quaû moâ phoûng coù theå toát vôùi nhieàu boä (k1,k2), nhöng khi ñöa vaøo robot thöïc thì chæ coù moät soá boä (k1,k2) thích hôïp. Caùc tham soá k1 vaø k2 cuûa boä ñieàu khieån caàn phaûi doø laïi trong khi thí nghieäm baèng caùch ñieàu chænh sau moãi thí nghieäm. 5.3 Höôùng nghieân cöùu tieáp Ñeå tieáp tuïc ñeà taøi naøy cho ñeán heát nhieäm vuï kieåm nghieäm giaûi thuaät, chuùng toâi xin ñeà nghò höôùng nghieân cöùu tieáp nhö sau: caûi tieán phaàn cô cuûa robot ñeå giaûm caùc sai soá cheá taïo, thay caùc ñoäng cô hieän coù baèng 2 ñoäng cô môùi vôùi boä truyeàn ñoäng coù tæ soá truyeàn lôùn, ñoàng thôøi gaén encoder coù ñoä phaân giaûi lôùn hôn vaøo ñoäng cô ñeå taêng khaû naêng ñieàu khieån vaän toác; coá gaéng giaûm thôøi gian laáy maãu cuûa chöông trình chính; laäp trình boä ñieàu khieån duøng boä quan saùt ñeå so saùnh toác ñoä hoäi tuï giöõa vieäc duøng boä ñieàu khieån naøy vôùi boä ñieàu khieån hoài tieáp taát caû traïng thaùi ñaõ thöïc hieän trong ñeà taøi. Robot di ñoäng theo daáu töôøng TAØI LIEÄU THAM KHAÛO 35 TAØI LIEÄU THAM KHAÛO Tieáng Vieät: [1] Traàn Höõu Queá, Veõ Kó Thuaät Cô Khí, Taäp 1&2, Nhaø xuaát baûn Giaùo Duïc, 2000 [2] Nguyeãn Doaõn Phöôùc, Phan Xuaân Minh & Haùn ThaønhTrung, Lyù Thuyeát Ñieàu Khieån Phi Tuyeán, Nhaø xuaát baûn Khoa Hoïc vaø Kyõ Thuaät, 2003, trang 53-75. [3] Nguyeãn Vieát Hieäp & Phaïm Ñình Anh Vuõ, Moâ hình hoaù – moâ phoûng vaø ñieàu khieån Robot theo daáu töôøng, Luaän vaên toát nghieäp Ñaïi hoïc, Ñaïi hoïc Baùch Khoa Tp.HCM, 2004. Tieáng nöôùc ngoaøi: [4] Detriche, Jean-Marie, Systemes robotiques et Mecatroniques, Cours d'Ecole Centrale Paris, 1999-2000. [5] Lagoudakis, Michail G., Mobile Robot Local Navigation with a Polar Neural Map, MSc Thesis, University of Southwestern Louisiana, 1998. [6] P. van Turennout, G. Honderd, L.J. van Schelven,Wall following control of a Mobile Robot, International Conference on Robotics and Automation, Nice, France, 1992, p. 280-285. [7] Medromi, J.Y. Tigli, and M.C. Thomas, Posture Estimation of a Mobile Robot: Observers-Sensors, Proceedings of the 1994 IEEE International Conference on Multisensor Fusion and Integration for Intelligent Systems, p. 661-666, 1994. [8] Urzelai J., Uribe J.P., and Ezkerra M., Fuzzy Controller for Wall-Following With a non-holonomous Mobile Robot, Proceedings of the 6th IEEE International Conference on Fuzzy System, Vol. 3, p.1361-1368, 1997. [9] A. Bemporad, M.D. Marco, and A. Tesi, Wall-Following Controllers for a Sonar Mobile Robot, Proceedings of the 36th Conference on Decision & Control, California USA, p. 3063-3068, 1997. [10] T. Yata, L. Kleeman, and S. Yuta, Wall Following Using Angle Information Measured by a Single Ultrasonic Transducer, Proceedings of the 1998 IEEE International Conference on Robotics and Automation, p1590-1596, 1998. Robot di ñoäng theo daáu töôøng TAØI LIEÄU THAM KHAÛO 36 [11] Chung Tan Lam, A nonlinear Feedback Control of Wall-Following Mobile Robot, Ms.C Thesis, Pukyong National University, Korea, 2004. [12] Rodney A. Brooks, A robust layered control system for a mobile robot, Research report of Massachusette Institute of Technology, 1985. [13] [14] [15] John A. Shaw, The PID Control Algorithm - How it works, how to tune it, and how to use it, 2nd edition, (ebook), 2003. Robot di ñoäng theo daáu töôøng PHUÏ LUÏC A 37 PHUÏ LUÏC A SÔ LÖÔÏC VEÀ CHUAÅN GIAO TIEÁP I2C [14] I2C laø chuaån giao tieáp ban ñaàu ñöôïc haõng Philips phaùt trieån ñeå phuïc vuï cho caùc giao tieáp vôùi Tivi, sau ñoù, do nhöõng lôïi theá cuûa chuaån giao tieáp naøy neân noù ñaõ trôû neân raát phoå bieán trong coâng nghieäp. Caùc taøi lieäu veà chuaån I2C ñöôïc cung caáp raát ñaày ñuû vaø chi tieát taïi trang web cuûa haõng Philips. Microchip ñaõ tích hôïp chuaån giao tieáp naøy vaøo phaàn cöùng cho moät soá vi ñieàu khieån, trong ñoù coù PIC16F877. Chuaån naøy ñöôïc bieát döôùi daïng chuaån giao tieáp ñoàng boä SSP. Trong chuaån giao tieáp SSP coù hai chuaån con laø chuaån SPI vaø I2C. SPI laø chuaån giao tieáp noái tieáp duøng 1 daây noái, vaø I2C laø chuaån giao tieáp noái tieáp 2 daây. Daây SDA laø daây ñeå truyeàn döõ lieäu vaø daây SCL laø daây ñeå giöõ nhòp cho döõ lieäu truyeàn. Chuaån I2C laø chuaån giao tieáp trong ñoù duøng moät thieát bò laøm Master, vaø caùc thieát bò khaùc trong maïng laø Slave trong moät thôøi ñieåm nhaát ñònh. Taïi moät thôøi ñieåm, Master coù quyeàn ñoïc vaø xuaát döõ lieäu qua taát caû caùc Slave thoâng qua ñòa chæ cuûa Slave ñoù. Master baét ñaàu quaù trình ñoïc hoaëc ghi döõ lieäu vaøo moät Slave baèng caùch ñaët tín hieäu Start (S) vaøo ñöôøng truyeàn (hình A.1 vaø A.2). Byte tieáp theo ñöôïc göûi ñi laø byte ñòa chæ cuûa Slave caàn giao tieáp. Quaù trình truyeàn nhaän ñöôïc keát thuùc baèng bit Stop (P). Sau ñaây laø bieåu ñoà thôøi gian truyeàn vaø nhaän trong chuaån giao tieáp I2C Hình A.1: Bieåu ñoà nhaän döõ lieäu, ñòa chæ 7 bit Robot di ñoäng theo daáu töôøng PHUÏ LUÏC A 38 Hình A.2: Bieåu ñoà truyeàn döõ lieäu, ñòa chæ 7 bit I2C phaàn cöùng cuûa PIC 16F877 hoã trôï hai cheá ñoä ñònh ñòa chæ 10 bit vaø 7 bit. Tuy nhieân, chuùng ta vaãn chæ thöôøng söû duïng ñòa chæ 7 bit, hoã trôï giao tieáp 128 thieát bò. Tuy vaäy, chuaån I2C cuõng gioáng nhö chuaån RS232, khi thao taùc vôùi caùc vi ñieàu khieån, coù theå ñöôïc thieát laäp baèng phaàn meàm. Moät laàn truyeàn ñòa chæ, seõ coù 8 bit, bit thaáp nhaát laø bit xaùc ñònh cheá ñoä ñoïc hoaëc ghi (R/W) (hình A.2). Chuaån I2C raát deã söû duïng, coù caùc toác ñoä truyeàn nhaän laø 100Kbps, 400Kbps vaø 1Mbps, vi ñieàu khieån PIC 16F877 hoã trôï toác ñoä 100Kbps vaø 400Kbps, nhanh hôn nhieàu laàn so vôùi chuaån RS232. Ngoaøi ra, khoâng caàn duøng baát kyø thieát bò chuyeån ñoåi naøo ñeå chuyeån ñoåi ñieän aùp tín hieäu, do vaäy, chuaån I2C thích hôïp nhaát cho caùc giao tieáp trong phaïm vi ngaén (döôùi 1m) giöõa caùc vi ñieàu khieån. Robot di ñoäng theo daáu töôøng PHUÏ LUÏC B 39 PHUÏ LUÏC B MAÕ NGUOÀN CAÙC CHÖÔNG TRÌNH Chöông trình cho master module: /************************ Description: This sourcecode is for the master module of the thesis "Study on Control of Wall-Following Mobile Robot" by Doan Minh Dang - P9900012 Start date: 2004.05.27 End date: 2004.07.03 ************************/ #include #device PIC16F877 *=16 ADC=10 #include #include #include #use delay(clock=20000000) #use rs232(baud=9600,parity=N,xmit=PIN_C6, rcv=PIN_C7) #use I2C(master, sda=PIN_C4, scl=PIN_C3) #use fast_IO(E) #priority ccp1,ccp2,rb,timer1 //address #define address_left 0xa0 #define address_right 0xc0 //constant float vr=0.05,d0=0,k1=2.5,k2=250;//unit: [vr]=m/s,[d0]=m Robot di ñoäng theo daáu töôøng PHUÏ LUÏC B 40 //variables int1 vr_ok=0; char s[5]; int1 stop1=0; int1 stop2=0; int1 online=0;//operating flag //int16 temp=0; for testing signed int16 encoder_1=0;//supplemental encoder in primitive unit, [encoder_1]=pulse signed int16 encoder_2=0;//main encoder in primitive unit, [encoder_2]=pulse //signed int16 encoder_1_old,encoder_2_old;for filtering signed int16 temp_int16=0; //float encoder_1_m=0;//d2, [encoder_1_m]=m //float encoder_2_m=0;//d1, [encoder_2_m]=m float d1,d2;//[d1]=m,[d2]=m float e1,e2,omega,delta_omega_mu,part1,omega_left,omega_right; float omega_mu=0; //float omega_mu_temp=0; //[e1]=mm //[e2]=rad //[omega]=rad //[omega_mu]=rad //[delta_omega_mu]=rad/(T*ms) (T=150) //[omega_left]=[omega_right]=round/minute (rpm) signed int16 omega_left_new,omega_right_new; //[omega_left_new]=[omega_right_new]=round/minute byte last_b; int1 count=0; Robot di ñoäng theo daáu töôøng PHUÏ LUÏC B 41 int cycle_count=0; int1 new_cycle=0; int number_cycle=2; signed int16 omega_left_last=0; //[omega_left_last]=round/minute signed int16 omega_right_last=0; //[omega_right_last]=round/minute void tilt_c5(int16 speak_time) {//master board will speak in specified time(ms) output_high(PIN_C5); delay_ms(speak_time); output_low(PIN_C5); } #SEPARATE void send_commands_via_i2c(int first_add,signed int16 first_data,int second_add,signed int16 second_data) { int hi,lo; //send command to first slave hi=make8(first_data,1); lo=make8(first_data,0); i2c_start(); i2c_write(first_add); delay_ms(1); i2c_write(lo);//send low byte of velocity delay_ms(1); i2c_write(hi);//then send high byte i2c_stop(); Robot di ñoäng theo daáu töôøng PHUÏ LUÏC B 42 //send command to second slave hi=make8(second_data,1); lo=make8(second_data,0); delay_ms(1); i2c_start(); i2c_write(second_add); delay_ms(1); i2c_write(lo); delay_ms(1); i2c_write(hi); i2c_stop(); } void calculate_slave_velocities() { //input: omega_left, omega_right - float, global variables //output: omega_left_new,omega_right_new - signed int16, global variables //This part is moved to a function to reduce memory cost //calculate new left and right velocities omega_left=(vr-omega*0.095)/0.033; omega_right=(vr+omega*0.095)/0.033; //send new omega to left and right modules omega_left_new=(signed int16)(omega_left*9.55); //convert from rad/s to rpm: (rad/s)*60/2pi=rpm omega_right_new=(signed int16)(omega_right*9.55); } #INT_RB Robot di ñoäng theo daáu töôøng PHUÏ LUÏC B 43 void rb_isr() { byte changes,new_b; new_b=input_b(); changes = last_b ^ new_b; last_b = new_b; if (bit_test(changes,5))//RB5: start button { //b5 went low stop1=!stop1;//if it is set, it will be clear, and vice versa } if (bit_test(changes,4))//RB4: stop button { //b4 went low if (stop1) stop2=1; } delay_ms(200); //debounce } #INT_CCP1 void vantoc1() { if (input(PIN_E1)) encoder_1++; else encoder_1--; } #INT_CCP2 void vantoc2() { if (input(PIN_E2)) encoder_2--; else encoder_2++; Robot di ñoäng theo daáu töôøng PHUÏ LUÏC B 44 } #INT_TIMER1 void timer1_isr() { set_timer1(34286); cycle_count++; if (cycle_count==number_cycle) { cycle_count=0; new_cycle=1; } } void main() { //INIT tilt_c5(500); set_tris_e(0x07); set_tris_b(0b00110000); last_b=input_b(); setup_timer_1(T1_INTERNAL|T1_DIV_BY_8); setup_CCP1(CCP_CAPTURE_FE); setup_CCP2(CCP_CAPTURE_FE); enable_interrupts(INT_CCP1); enable_interrupts(INT_CCP2); enable_interrupts(INT_RB); enable_interrupts(GLOBAL); //RECEIVE PARAMETERS //vr gets(s); Robot di ñoäng theo daáu töôøng PHUÏ LUÏC B 45 vr=atof(s); printf("Receive velocity desired: %f \n",vr); gets(s); number_cycle=atoi(s); printf("Sampling time=%u*0.05s \n",number_cycle); encoder_1=0; encoder_2=0; tilt_c5(1000); gets(s); k1=atof(s); printf("k1=%f",k1); gets(s); k2=atof(s); printf("k2=%f",k2); //first move: move straight forward with at speed=vr omega=0; calculate_slave_velocities(); printf("Go!\n"); send_commands_via_i2c (address_left,omega_left_new,address_right,omega_right_new); omega_left_last=omega_left_new; omega_right_last=omega_right_new; printf("l:%ld\nr:%ld\n",omega_left_new,omega_right_new); new_cycle=0; //up to here, it cost 5146 timer1 counts=8233us //it will ring 500-8=492ms tilt_c5(492); set_timer1(34286);//overflow after 3250 machine cycles=50ms Robot di ñoäng theo daáu töôøng PHUÏ LUÏC B 46 enable_interrupts(INT_TIMER1); //MAIN LOOP while ((!stop1)&&(!stop2)) { if (new_cycle) { //start new cycle //GET d1 AND d2 d2=(float)encoder_1/51000.0;//scale: 51000 pulse/m d1=(float)encoder_2/51000.0; //SEND ENCODERS' VALUES TO PC printf("1:%ld\n2:%ld\n",encoder_1,encoder_2); //CALCULATE e1 & e2 e2=atan((d1-d2)/0.030); e1=-d1*cos(e2); //calculate omega part1=vr-(e1+d0)*omega_mu/cos(e2); omega=k2*part1*e1-k1*sin(e2)+omega_mu; //calculate new omega_mu delta_omega_mu=e1*(e1+d0)*tan(e2)-sin(e2)/k2; delta_omega_mu=delta_omega_mu*0.05*number_cycle; omega_mu +=delta_omega_mu; //calculate data to send to slave calculate_slave_velocities(); //CHECK FOR EXCEEDED VELOCITIES if (omega_left_new>90)//maximum limit { printf("ERR1");//err1: Left velocity exceed the range Robot di ñoäng theo daáu töôøng PHUÏ LUÏC B 47 omega_left_new=90; } if (omega_left_new<-90)//maximum limit { printf("ERR2");//err1: Left velocity exceed the range omega_left_new=-90; } if (omega_right_new>90)//maximum limit { printf("ERR3");//err2: Right velocity exceed the range omega_right_new=90; } if (omega_right_new<-90)//maximum limit { printf("ERR4");//err2: Right velocity exceed the range omega_right_new=-90; } //SEND NEW VELOCITIES send_commands_via_i2c (address_left,omega_left_new,address_right,omega_right_new); printf("l:%ld\nr:%ld\n",omega_left_new,omega_right_new); omega_left_last=omega_left_new; omega_right_last=omega_right_new; //wait until next cycle new_cycle=0; } } //SEND STOP COMMAND Robot di ñoäng theo daáu töôøng PHUÏ LUÏC B 48 send_commands_via_i2c(address_left,0xffff,address_right,0xffff ); tilt_c5(200);//ringing for stop declaring delay_ms(200); tilt_c5(200); } Chương trình cho slave module: /************** Description: This program is used for controlling motor DC velocity with PID method. There are slight differences between the programs for left and right modules. Doan Minh Dang - P9900012 Start date: 2004.04.27 End date: 2004.06.20 **************/ #include #device PIC16F877 *=16 ADC=10 #fuses hs, nowdt, noprotect, nolvp, put, brownout #use delay(clock=20000000) #use rs232(baud=9600,parity=N,xmit=PIN_C6, rcv=PIN_C7) #use I2C(slave,sda=PIN_C4,scl=PIN_C3,address=0xa0,FORCE_HW) #include #include #include //the function run_motor(value,condition,direct,frequency) is used to control motor Robot di ñoäng theo daáu töôøng PHUÏ LUÏC B 49 //constant float kp=8.2; float ki=1; float kd=0.8; //variables int1 i2c_flag=0; int1 start_cycle=0; int1 stop=0; int i2c_error=0; int velocity_error=0; int16 times=0; int error_command; signed int count=0; signed int count_last=0; signed int delta_count=0; char s[7]; byte command_listen[3]; int i=0; signed int16 command_velocity=0; //[rpm] int1 direction; float real_velocity=0; signed int16 real_velocity_print; signed int16 duty=0; int16 PWMduty; float error_rpm_now=0; float error_rpm_last=0; float error_rpm_last_last=0; float p_value; float i_value; Robot di ñoäng theo daáu töôøng PHUÏ LUÏC B 50 float d_value; float control_value; ////////////////////////////////////////////////////////// /////////////////////////////// void tilt_a0(int16 speak_time) {//slave board will speak in specified time(ms) output_high(PIN_A0); delay_ms(speak_time); output_low(PIN_A0); } void convert_duty() { if (duty<0) { PWMduty=-duty; direction=0;//negative speed: rotate backward } else { PWMduty=duty; direction=1;//positive speed: rotate forward } //control_motor(dir=0): backward //control_motor(dir=1): forward if (PWMduty>1023) { PWMduty=1023; velocity_error++; Robot di ñoäng theo daáu töôøng PHUÏ LUÏC B 51 } if (64>PWMduty) { PWMduty=64; } } void PID_calculation() { p_value=kp*(error_rpm_now-error_rpm_last); i_value=ki*error_rpm_now; d_value=kd*(error_rpm_now-2*error_rpm_last +error_rpm_last_last); control_value=p_value+i_value+d_value; duty+=(signed int16)control_value; } ////////////////////////////////////////////////////////// ////////// #INT_SSP void ssp_isr() { if (i2c_poll()==FALSE) { i=0; } else { command_listen[i++]=i2c_read(); Robot di ñoäng theo daáu töôøng PHUÏ LUÏC B 52 // i=1; if (i==3) { times++; if ((command_listen[1]!=0xa0)&&(command_listen[2]!=0xa0)) //condition: data must be received correctly { i2c_flag=1; i=0; command_velocity=make16(command_listen[2],command_listen[1]); if (command_velocity==0xffff) //stop command { stop=1; } } else i2c_error++; } } } #INT_EXT void count_encoder() { if (input(PIN_B7)) count--;//backward rotate - left else count++;//forward - left //reverse for right module } ////////////////////////////////////////////////////////// ///// Robot di ñoäng theo daáu töôøng PHUÏ LUÏC B 53 #INT_TIMER1 void update_time() { start_cycle=1; set_timer1(15536); //update time = 10ms delta_count=count-count_last; count_last=count; //multiplier: 4ms - 7.5; 5ms - 6; 10ms - 3 real_velocity=(float)delta_count*3.0; error_rpm_last_last=error_rpm_last; error_rpm_last=error_rpm_now; error_rpm_now=(float)command_velocity-real_velocity; } ////////////////////////////////////////////////////////// ////// void main() { tilt_a0(400); set_tris_b(0x81); ext_int_edge(0,L_TO_H); control_motor(0,0,0,2); command_listen[0]=0; command_listen[1]=0; command_listen[2]=0; setup_timer_1(T1_INTERNAL); enable_interrupts(GLOBAL); enable_interrupts(INT_SSP); while (!i2c_flag) {}; delay_us(3385); Robot di ñoäng theo daáu töôøng PHUÏ LUÏC B 54 tilt_a0(500); set_timer1(15535); enable_interrupts(INT_EXT); enable_interrupts(INT_TIMER1); start_cycle=0; while (!stop) { if (start_cycle) { if (command_velocity!=0) { PID_calculation(); convert_duty(); control_motor(PWMduty,1,direction,2); } else {control_motor(0,1,0,2);}//stop start_cycle=0; } } control_motor(0,0,0,2); disable_interrupts(GLOBAL); disable_interrupts(INT_SSP); disable_interrupts(INT_EXT); disable_interrupts(INT_TIMER1); tilt_a0(200); delay_ms(200); tilt_a0(200); delay_ms(200); if ((i2c_error>0)||(velocity_error>0)) Robot di ñoäng theo daáu töôøng PHUÏ LUÏC B 55 { tilt_a0(1000); } while (1) { gets(s); error_command=atoi(s); switch (error_command) { case 1: printf("i2c_error:%u ",i2c_error); break; case 2: printf("velocity_error:%u",velocity_error); break; case 3: printf("times:%lu",times); break; } } }