MYC LUC MO DAU.. CHUONG I: TONG QUAN VE ROBOT VA DIEU KHIEN ROBOT. 1.1. Robot va robot cong nghigp 1.1.1. Vai nét lich sir phat tién ctia robot va robot cong nghigp 1.1.2. Robot va cong nghé cao 1.1.3. Dinh nghia vé robot cong nghigp 1.2. Cée phuong php diéu khién robot. 1.2.1, Phuong phap diéu khién dong hre hoe ngwoe 1.2.2. Phuong php diéu khién phan hdi phan ly phi tuy 1.2.3, Phuong phap digu khién thich nghi theo sai Igch 1.2.4, Phuong phap diéu khién thich nghi theo mé hinh mix 1. 1.2.6. Phuong phép diéu khién tragt. . 7 CHUONG I: MO TA DOI TUQNG - ROBOT SCARA SERPENT. 2.1. Mot sé loai robot Scara cia cae hing 5. Phuong phip diéu khién dong re hoe ngurge thich nghi, xudt 2.2. Cée théng 96 va ving lm vige ciia robot Scara Serpent 221 22.11 {go tay may robot Scara Serpent. iw hinh ciia robot Scara Serpent. 2.2.1.2. Cée thong 6 ky thudt etia robot Scara Serpent 2.2.2. Gidi han khdng gian lam vige cia robot Seara Serpent. 2.3. Dang hge robo 2.3.1. Dong hoe thug, ‘ara Serpent 2.3.2. Dong hoe nguye... 2.4. Dong lyre hge robot Seara Serpent 2.4.1, Ham Euler - Lagrange va cdc van dé dong lyre hgc..... 2.4.2. Dong hue hoe robot Scara Serpent sme 24.2.1. Tinh ton ding ning va thé niing cho timg khép. 2.4.2.2, Phurong trinh dong ke hoe 2.5. Mé ti d6i tugng bing hg phuong trinh trang thCHUONG III: XAY DUNG MO HINH HE THONG DIEU KHIEN . 3.1. Céu trie hg théng diéu khién robot. 3.2. Xay dung qu dao chuyén dng chuan, 3.2.1, Xée dinh gia tri q02 va gel 3.2.2. Phuong trinh doan ed. 3.2.3. Phuong trinh doan ac...... 3.2.4, Phuong trinh doan df. 3.3. Thiét ké b9 fu khién cho tay may robot Scara Serpent ba bie tyr do 3.3.1, Hé phuong trinh dng hue hoe Lagrange 3.3.2. Hé phuong trinh trang thai Ly chon phuong phap diéu khién va b6 digu khién PID... HUONG IV: MO PHONG VOI MO HiNH ROBOT SCARA SERPENT 4.1. Dat vind... Srtsseshe 4.2. So dd m6 hinh héa cdc khaiu cua hg thong, 4.2.1, Mé inh chung ctia robot 4.2.2. Mo hinh kk 4.2.3. MG hinh b@ dig Khién wa. i tao quy dao chuyén dong chuan 4.3. Giao dign chuong trinh mé phéng robot Seara Serpent 4.4. Két qua mé phong hé thong trén Matlab-Simulink.... 4.4.1. Thong sé cita robot va quy dao chuyén dng 4.4.2. Dic tinh cia hé théng khi robot lam vige véi tii khée nhau. KET LUAN « ‘TAL LIEU THAM KHAO PHY LUC 1: CAC SO DO KHOI. PHY LUC 2: CAC CHUONG TRINH M FILES 39 39MO BAU Theo qué trinh phat trién cia xi héi, nhu céu nding cao sin xuét va chét lnong sin phim ngay cang doi hoi tng dung rong rai ede phwrong tién ww déng héa sin xu. Xu hudng tao ra nhitng day chuyén va thiét bi tw dOng c6 tinh Tinh hogt cao da hinh, thanh va phat trién manh mé...Vi thé ngay cng ting nhanh nhu céu img dung ngudi may dé tgo ra cdc hé sin xudt ty dng linh hoat. Robot img dung rong rai va déng vai trd quan trong trong sin xuét cing nhur trong dai séng. Robot 14 co cit da chite nang c6 kha nang lap trinh duge ding dé di chuyén nguyén vat ligu, cfc chi , cde dung cu théng qua cae truyén dong duge lip trinh trade. Khoa hge robot chit yéu dura vio cic phép toan vé dai sé ma trén > Robot c6 thé thao tée nhur con ngudi va ¢6 thé hgp tie vii nhau mot cdnh thong ‘minh. > Robot 6 canh tay véi nhiéu bac ty do va cé thé thye hién durge cae chuyén dong nhur tay ngudi va diéu khién doc bing may tinh hose 6 thé dig khién bang chuong trinh durge nap sin trong chip trén bo mach digu khién robot. 'é he diéu khién robot e6 AO tin cdy, dO chinh xdc cao, gid thanh ha va tiét kigm nang lugng thi nhigm vy co ban la hé didu khién robot phai dim bio gid ti yéu clu cita céc dai Ingng digu chinh va diéu khién. Ngoai ra, hé diéu khién robot phi 4am bio én djnh dgng va finh, chéng duye nhidu trong va ngodi, ddng thoi khong gay téc hgi cho méi truéng nhu: tiéng dn qué mite quy dinh, séng hai cia dign ap va dong dign qua lon cho Indi dign vw. Khi thiét ké hé digu khién robot ma trong 6 str dung cdc hé digu chinh ty dong dong, cdn phai dim bao hé thuc hign duge tat cf ede yéu cu vé cong nghé, cfc chi tiéu chit hong va cée yéu edt kinh t2. Chat lug cia hé théng durge thé hign trong trang thai finh va trang théi déng. Trang thai tinh yéu cdu quan trong la d6 chinh xae chinh, Trang thai déng thi c6 yéu cdu vé d6 én dinh va cée chi tiéu vé chét 6 aid 6 lin dao Tugng dong 1a d6 qua digu chinh, cchinh, thoi gian digu chinh va khién robot, vige Iva chon sir dung céc b6 bid dong. Déi voi he dig Gi, cae logi dong co dign, cdc thiét bj do luimg, cam bién, cdc bO didu khién va dc bigt ld phuong phap diéu khién cé anh hurong rit lén dén chat rong diéu khién bam chinh xée quy dao cua he.Cée cng trinh nghién ciru vé hé théng digu khién robot tp trung chi yéu theo hai hung 1 sir dung cde mo hinh c6 dc tinh phi wyén 6 thé ude hong duge dé don agidn vige phan tich va thidt ké hoe dé ra cde thuat todn digu khién méi nham nang cao chat lgng dap img ciia robot, Dac diém oo ban cia hé thing diéu khign robot li thyre hign durge digu khign bam theo mot quy dao phite tap dat trade trong khdng gian, tuy nbién khi dich chuyén thi trong tim tia fe chuyén dong thanh phin va momen quén tinh cla hé s® thay d6i, digu d6 dn dén thong sé dng hoe cia hé cing thay dai theo quy dao chuyén dng va déng thoi xudt hign nhimg lye téc dgng qua lai, xuyén chéo gida cde chuyén dong thanh phan trong hg véi nhau. Céc yéu té trén tac dong sé lam cho hé diéu khién robot mang tinh phi tuyén man, gay cn tro rét Ion cho vige mé té va nhn dang chinh xéc hg théng digu khién robot. Do vay, khi digu khién robot bam theo quy dao dat true phai giai quyét duge nhting vin 48 seu: > Khe phuc cae lye tuong tic phy thuge vio van téc, gia t6c iia quy dao riéng cic chuyén déng thanh phin va quy’ dao chung cia ed hg nhw: hue quén tinh, hee ly tm, Ie ma sit v.. > Khi trong tém cita cc chuyén dng thanh phin va cia ca hé thay d6i theo quy dao riéng va chung kéo theo sur thay di eta cdc thong s6 dong hoe eta he, dieu 445 ddi hai phai e6 sy bién thign cdc tham s6 dua vio b@ digu khign tong ting 48 vin dam bio su can bing, én dinh va bén ving déng thoi vin bam theo duge quy dao dat Véi cong cy toan vi phan nguéi ta da c6 thé phan tich tinh diéu khién duge, tinh quan sit duge cho hg phi tuyén. Noi dung ciia dé tai nghién eiru nhur sau: 1. Tén dé this Nang cao chat long diéu khién cho robot Scara, 2. Co sis khoa hge va thye tién cia dé ti: Ung dung phuong phip digu khién dong lyre hc nguge cho diéu khién bén ving quy dao robot. 3. Mye dich cita d2 tat: Xa dmg cfu tne va thud toin digu khin robot Seara 4. NOi dung etia d@ tai, eae van a8 edn giai quyét:- Xi dung mé hinh ton hoc cho robot Seara Serpent 3 bac ty do. ~ Xi dung hg théng diu khin qu dao dat dé chinh xe cao, ~ Dinh gid chat luong hé thong bing mé phong. Noi dung cia Iwan van dé cp ti van dé “Nang cao chat Iwgng diéu khién robot Scara” voi muc tigu digu khién bén vig va bém chinh xée quy dao chuyén dong, Lun vin duge tinh bay thanh 4 chong v ni dung co ban cia timg chuong dig tm tit nh sa Chong I~ Téng quan vé robot vat diéu khién robot: Mo ta tong quan vé robot, Phan tich aw, nhuge diém eta mgt s6 phuromg php diéu khign robot 48 vi dang duye ép dung trong thye tla 48 ning cao 49 chinhxée didu ida qug ago robot Chuong II — M6 ti todn hge déi tuyng robot Scar: Nghién ciu mot robot trong ho Scara va di su vio phan tich mé hinh Robot Scara Serpent dé phuc va vige nghién ciru va kiém chimg co so Iy thuyét va cée phuong phap diéu khién duge Ira chon (Chuong ITI - Xay dyng m6 hinh hé théng digu khién: Thiét ké b6 digu khién dé nang cao chat Iugng hé théng diéu khién quy dao cho robot. Xfy dymg mé hinh hg théng didu khién cho robot Scara Serpent, xay dung md hinh md phong str dung phan. mém Matlab-Sumutink. ‘Chuong IV ~ Mé phéng véi m6 hinh robot Seara Serpent: Dinh hinh va kiém chtmg vé mat ly thayét co so thye tign cia dé tai cing nhw tinh kha thi cua phurong phap digu khién duge Ia chon khi ap dung cho hoat dong bén vimng ciia diéu khign gu dao robot Scara Serpent 3 bac tu do.CHUONGI TONG QUAN VE ROBOT VA DIEU KHIEN ROBOT 1.1, Robot va robot céng nghigp Vai nét lich sir phat trién ciia robot vA robot cng nghigp hin nguge dong thai gian ching ta o6 thé nhgn thay ring tir “Robot” aa xudt hign ti khd Mu, Nam 1921 nha viét kich Kareleapek ngudi Sée da vigt mot vo ich voi tya dé R.U.R (Rossums Universal Robot) mé ta vé mOt cuge ndi logn cita nhing c3 may phue dich. Tir “Robot” 6 day c6 nghia la nhang may méc biét Kim vige nh con nguwéi, C6 Ié d6 cing 1a mét goi ¥ cho nhimg nha sing ché k¥ thugt thye hign cic mo urée vé nhimg 08 may bit chude duge cde thao tic lao dong eu bip eva con ng Thoi gian sau d6 céc co cau diéu khién tir xa (Teleoperator) ra dai va ngiy mot phat trién hoan thign, Teleoperator la nhiing co ¢: inh hoc, nd bao gdm cdc hau, cée khop cing v6i cic day ching gin lién voi hé digu hanh la cénh tay cia ngwoi digu khién théng qua cae co céu khuéch dai co khi, Teleoperator 06 thé cim nim, ning ha, dich chuyén, xoay lit cée di tugng trong m@t khOng gian hoat ding hit dinh. Tuy ring cdc thao tdc khé tinh vi, khéo Ido nhumg tbe d6 hoat dng cham, lye te dung han ché va hé diéu khién chi thudin tug 1a co khi, Tir thap ky SO, sur phat trién dy hita hen eda ky thudt diéu khién theo chuong trinh s6 cimg va nginh vat ligu méi da lam chd da ving chée cho sy ra d0i cia cic co du diéu khién vo cp (servo mechanism) va cdc hg dign toan (computation). Ngay lip. tite y twomg két hop hé dia khién NC (Numerical control) véi cée co clu digu khign tr xa (Teleoperator) duge hinh thanh va trién khai nghién ettu, Sy phdi hop tuyét voi gitta kha nang linh hoat khéo Io cia Teleoperator véi 46 thong minh nhay bén ciia hé khién NC di dua ra két qua la mot hé may méc ty déng cao cép véi tén goi “Robot” Nam 1961 ngudi may cong nghigp (IR- industrial Robot) dau tién duge dua ra thj trudmg. Tiép theo dé céc nude khac cing bit dau sin xuat robot céng nghigp theo bin quyén cia M9, Anh (1967), Thuy Bién, Nhat (1968), Bite (1971) Ney nay, trén thé gigi e6 khoang 200 cong ty sin xudt IR, trong d6 6 Nhat 6 70, 6 cae nude Tay du c6 90, 6 My ¢6 30. Nhd ap dung rong rai cae tién b§ Khoa hockg thudt vé vi xir Ly, tin hoe fing nhu vat ligu méi nén sb Iugng robot céng nghigp da ting Ién nhanh chéng, gid thanh gidm di rd rét, tinh nang e6 nhiéu cai tién. Robot cong nghigp phat huy thé manh nhing Iinh vye nhu han hd quang, dite, kip rép, son phi, vi trong céc hé thang tr dong dig khign lign hop. 1.1.2, Robot va cong nghg cao Robot vai cong nghé cao la nhiing khai nigm tia sén xuat ty dong hod hign dai Céc mide cong nghigp phét trién da dura ra chién luge ding wr dong hod hign dgi (IR+High Tech) kéo cae xi nghigp cng nghigp diu tu 6 nurée ngodi (trade day vi If do hang the re mat) tro vé chinh quéc (ding lao dng 14 robot cong nghigp). Chinh pha cée aude nay d& 4p dung nhing bign phip bé tro hu higu nh: coi robot cong nghigp la nganh céng nghiép quan trong, xay dung nhigu chuong trinh nha muée vé Ap dung tién bd khoa hoc ky thudt vio san xudt robot. Khuyén khich bing cdch uu tién thué va dat ra nhiing quy ché cé Igi cho ca ngudi san xuat va ngudi sir dung robot céng nghigp. Nho vay chi sau m6t thdi gian ngén sir dung robot cng nghiép tré nén rong én da dang véi co sé ngudn dong Ine phit trién 1a “Iye dy” cua cdng nghé va “lye ké0” cita thi truémg, Mat dic diém quan trong cua robot cng nghigp la chiing cho phép dé ding két hgp nhing viée phy va chinh cia mOt qué trinh san xudt thanh mot day chuyén tw dong, So véi cée phuong tign ty dng hod khée, cée day chuyén ty dong ding robot ¢6 nhiéu uu diém hon nhu d& ding thay déi chuong trinh lam vige, e6 kha nang tgo ra dy chuyén ty dong tir cde may vgn nang, va c6 thé ty dong hod toan phan. Khi sir dung robot vio ede day chuyén ty dong, khéu chun bj kj thuat durge nit ngin di. Trong khi 6 véi thai gian tir lhe quyét dinh phuong din dén fic thiét ké xong mét day chuyén cde may tyr dng, mot mat hang hode quy trinh céng nghé d6 da 6 thé tro thanh lac hau Theo 35 ligu eta ce chuyén gia MF nghién cia v8 vin dé nay hi Khao st trén 70 48 ai in thiét ké thi véi qua nira s6 dé la phuong an ding cae may te dong chuyén dung phai t6n hon m@t nim, Vi thé, phwong an diing robot Unimate véi cdc may ty d6ng van nang durge dua vao sir dung va phat huy higu qua to én. Ky thuat robot c6 wu diém quan trong nhat fa tao nén kha nang linh hoat hod san xudt, Vige str dung may tinh dign ti, robot va may diéu khién theo churong trinh da cho phép tim durge nhiing phuong thire méi mé dé tao nén céc day chuyén tx dng cho 7sin xudt hang loat voi nhiéu mau, loai sin phim. Day chuyén ty dong “etmg” gdm. nhiéu thiét bj ty déng chuyén ding dai hdi vén dau tu 1én, tin nhidu thoi gian dé thiét ké va ché tao, trong Iie quy trinh céng nghé ludn ludn cai tién, nhu cau déi vei chat Iugng va quy cach ca sin pham luén luén thay d6i. Boi vay, nhu cau “mém” hoa hay linh hoat hea di ‘chuyén sin xudt ngay cang ting. K¥ thudt robot cng nghiép va may tinh da déng vai t quan trong trong cdng vige to ra cfc day chuyén tw dong Linh hoat, ‘Xudt phit tir nhu edu va kha nang linh hogt hoa san xudt, trong nhing nam gin day khong nhimg chi cae nha khoa hgc ma cae nha sin xuit da tp trung sich ¥ vio vige hinh thanh va ép dung céc he sin xudt ne d6ng linh hoat, goi tit a he sin xudt link hoat. Hé sin xuit linh hogt FMS (Flexible Manufacturing System) ngay nay thug bao gom céc thiét bj gia cng duge diéu khién bing chuong trinh sd, cae phuong tién ‘vn chuyén va keho chita trong phin xwong 48 duge ty dng hod vA nhém robot cong aghitp & cle vj tl trye tip v6i céc thiét bj gia cOng hole thyc hign céc nguyén cong phy. Viée diéu khién va kiém tra hoat dong cia to’in bd hé sin xudt linh hoat due thye hign bang may tinh, Uw diém néi bat ciia hé sin xuat Linh hoat la rat thich hop voi quy mé nho va vira, thich hop véi yéu cau Iuén Iudn thay ddi chat wong sin pham va quy trinh céng nghé. Béi vay, ngay nay hg sin xuat linh hoat thu hit sw chit ¥ khong nhitng & cic nude phat trién ma ngay ca & nhitng nude dang phét trién, Trong mot sé tai ligu nude ngoai FMS nhu 1a hé sin xudt cia tong lai (Future Manufactring System). 1.13. nh nghta v8 robot cng nghigp Robot céng nghigp la m6t may ty dong linh hogt thay thé timg phan hoae toan bb hoat déng co bap va hoat déng tr tug cua con ngudi trong nhigu kha ning thich nghi khée nhau, Vé mat co khi va didu khi ign ta, robot cong nghigp la sy «6 hop kha ning. hoat déng linh hoat cia cdc co cau diéu khién tir xa voi te dé phat trién ngay cang cao cia hé thong diéu khién theo chuong trinh s6 ciing nhu ky thudt ché tao cic cam bik cong nghé lip trinh va cac phat trién cia tri tug nhan tao, hé chuyén gia, Robot cong nghigp cd kha nang chuong trinh hod Tinh hoat trén nhiéu truc chuyén d6ng, bidu thi cho sé bac ur do cia ching. Robot céng nghigp duge trang bi 8nhiing ban tay may hod cae co cdu chap hanh giai quyét nhung nhigm vu xée dinh trong cdc qua trinh céng nghé: hoge trre tiép tham gia ede nguyén cOng (son, hin, lip rip cae cum thiét bj ...) hoe phye vu céc qua trinh t6 chire dong Iuu thong vat chat (chi tiét, dao cu, gé lip...) voi nhiing thao tac cim ndm, van chuyén va trao déi cdc déi tugng vat chat véi cde tram céng nghé trong mét hé théng may tw dong linh hoat. Ta c6 thé digm qua mét vai djnh nghia vé robot cong nghigp nhu sau: = Dinh nghia theo tigu chun AFNOR Fracais: Robot cong nghiép a mot co edu chuyén ddi tw dong ¢6 thé chong trinh hod, ip lai céc chuong trinh, tng hgp cde chuong trinh dat ra trén cde truc tog dQ, ¢6 kha nang dinh vi, dinh hudng, di chuyén céc déi trong vat chat theo nhitng chuong trinh thay déi da chong hod nhim thye hign cde nhigm vy cng nghé khée nhau. ~ Dinh ngha theo tiéu chun VDI 2806/BRD: Robot cong nghigp ld mot thiét bi c6 nhiéu true, thre hign cde chuyén dng tw dong c6 thé chuong trinh hod va ndi ghép cée chuyén dong cita ching trong cae khong céch tuyén tinh hay phi tuyén cia dng trink. Chiing duge diéu khién bai cfc bd hgp nhdt ghép ndi véi nhau, ¢6 kha nding hgc va nhé cdc chuong trinh, ching durgc trang bj dung eu hoae ede phurong tign cong nghé khae nhau dé thye hign ede nhigm va sain xudt nge tip va gin tgp, - Dinh nghia theo IOTC - 198¢ Robot cng nghigp la mot may tw dng lién két gita mot tay may va mot cum digu kehign chuong trinh hoa, the hign mét chu trinh eng nghé mét eich chit ding voi sur diéu khién c6 thé thay thé nhiing chite nding trong tur eiia con ngudi. Ban chat ciia cde dinh nghia trén cho ta thy mét ¥ nghia quan trong: robot cong nghigp phai duge lign hé chat ché vii may méc, cong cu va cée thiét bi cdng nehé tur dong khéc trong mt hg théng ty dong téng hop. Trong qué trinh phan khong thé quan nigm robot nln mOt don vi céu trie bigt lp, tri Tai A6 pha 18 hong thidt ké téng thé cia “h@ théng ty dong linh hogt robot hos” cho phép thich tng nhanh va don gidn khi nhigm vy san xudt thay di. Theo d6, cdc mdu hinh robot phai dam bio 6:= Tha phap cim nim, chuyén di t6i wu. = Trinh dé hanh nghé khén khéo, linh hoat, + Két cu phai tuan theo nguyén tic médun hoa. 1.2. Cae phuong php ditu khién robot Cho dén nay trong thue «, nhigu phuong phip va hg théng digu khién robot da durge thigt ké va sir dung, trong dé cde phuong phip digu khign chi ybu li: = Digu khién dong hye hgc nguge. = Digu khién phan héi phan ly phi tuyen. + Cae phuong phap digu khién thich nghi. + Didu khién thich nghi theo sai lech. ign khién thich nghi theo mé hinh mau (MRAC). + Didu khién dong Ine hoe nguoe thich nghi » Didu khign trugt Ching ta sé lan hrgt tim higu cée phuong phép didu khién robot dé biét duge wu nhurge diém cia timg phurong php. 1.2.1, Phuong phap diéu khién dong lve hoc nguge Nguyén Iy cua phuong phép nay la chon mt lugt digu khién phit hop dé khir thanh phan phi tuyén eda phurong trinh dng Ie hoe va phan ly dae tinh dng Iue hoe cia cdc khép nk a(t) = H(q) Rt) +h(q. + 2¢4) ay Néu ta biét cdc tham sé ciia robot ta 06 thé tinh duge cde ma trin H, h, g tir 4d 66 lugt digu khién, Ta =H(q)U + h(q.&) + 214) a2) cfn bling Ty, = T voi diéu kign H(q) # Ova @ U(vecto diéu khién phy ).hur vay dong lire hoc hé théng kin s& durge phan tich thanh hé phurong trinh vi phan tuyén tinh hé sé hing. &U Véi robot n khép néi tong dueng véi n hé con d9c lp Chon U la tin higu diéu khién phy 06 céu trie PID. Lie 46: w+ Ky (&-#+K,(q, a) +K, fq, ad (3) trong d8: q, , li bién khep dat va bién khép thye cia khép, & la toc do dat va tée dé thure cia khan. ue &+Kye+ ' u Tinh : K, 8+ K, fe(at H@U+h@.8+2@) Robot | Hin 1.1: So dB edu inde he diéu khién dng lye hoe nguge Va phuong trinh sai sé tong img sé la: etK,e+K,erKe=0 aay Cae hg 36 Kp, Kp, Ky duge chon theo diéu kign én dinh ciia Lyapunov dé sais gitta quy dao chuyén dng chudn va quy dao chuyén d6ng thy hdi ty tai diém 0 khong, phu thude vao digu kign ban dit Uu diém cia phuong phap nay [a khiF duge tinh phi tuyén va st ring bude trong phuong trinh dng Iue hoc. Niurge didm cia né 1A phai biét duge diy da chinh xée cdc théng s6 cing nhur dae tinh dong lye hoe robot, ding thii cing phat sinh tinh toan phy. Thug tod tinh ton didu khién U st lién quan eée phép tinh lrgng giée nén phai thye hign mot sbphép nhén ma tin veeto vi ma trgn phy. Thdi gian tinh toda Kim ta mgt yéu 08 anh hnrong dén sy han ché cua phwong phdp nay. 1.2.2. Phuong phap digu khién phin hii phan ly phi tuyén Phuong phap niy duge xy dung trén co sé ly thuyét cha digu khién phan ly ccho hé théng phi tuyén bing phan hdi tayén tinh hod tin higa ra, “Ti phurong trinh dong lye hg: 1(0) = H(q). Rt) + hg, + (4) as) Y=Y(q) Ma tran H khéng don nhat nén ta c6 thé viet lai nhur sau: &e) =[H@I 0) -TH@] .((hG.8)+ (0D. (16) Phuong trinh nay gdm cée phuong trinh vi phan cép hai cho mdi bién, vi 18 46 qua hai Lan vi phan phuong trinh dau ra thi hé s6 cua tin higu U sé khée 0. Lite nay tin higu U sé xuat hign trong phuong trinh dau ra: = yO) =[H@DI."e()-TH@I(0hG. 1+ Ig, @))) =H (X).U() + 8,00) (7) Voi: HX) = H(@)]," 80) = TI @),.h(.8)+18,@) x" -[so.e| Tin higu t(t) cua b6 diéu khién duge chgn sao cho dam bao hé thong phan ly a(t) = -H™ (X).[g°(X) + @'(X)— AE())) = -H(q)(-H"(q@).(h(q.@ + £(q)) + @'(X) = AEC) (8) = h(q.) + (q) -H (ar"(X)- AE)Trong d6: (x) = )10,,.y,""A = diag(, 2. ‘Tir phwrong trinh (1.8) ta nhan thdy tin higu diéu khién 1,(t) cho khép i chi phy thude vio cdc bién dong Ine hoe vi tin higu vio E(t). ‘Thay x(t) ir phuong trinh (1.8) vao phuong trinh (1.5) ta duge: H(q).Rt) +h, + 2G) h(q,4)+ 2(q) -H(a(X)- AB) (1.9) Hay: A(t) + a ,de(t) + ay.4,(t) = 2,¢,(t) (10) Phuong trinh (1.10) biéu thi vao ra phan ly ctia hg thong. Céc hg s6 ot, 6,02, duge chon theo tiéu chun én dinh 1.2.3. Phuong phap diéu khién thich Dya trén co sir ly thuyét sai léch, Lee va Chung da dé xuat thudt ton diéu khign dim bao robot Iudn bém quy dao chuyén dong dat trade véi pham vi chuyén dong rong va tai thay déi rong. Phuong phép diéu khién thich nghi theo sai Iéch duge xy dyng trén co sé phuong trinh sai Iéch tuyén tinh hoa Kin cn guy dao chuyén dong chuan, Hg théng 4 gdm hai kh (truyén thing - feedforward) va khéi phan hdi (feedback) nhu Himh 1.2. Quy dao chuyén |xa(t) | Phuong trinh | + i turomg, xi) dong chain ‘Newon-Buler @ su khién 19 dia idm He thong ain dang tdiaumiende | | Sithiphvong i hisu aeauy @ inh 1.2: He théng didu khinthich nghi theo sai Igch Khdi tién dinh tinh todn mé men cia robot img vi quy dao chuyén déng chun theo phuong trinh Newton-Euler. Khéi phan hdi thue hign tinh todn thanh ji wu mot ndc nham bi sai léch vi tri va te d6 cha, phin momen sai léch theo Tuat Khép doc theo quy dao chuyén dong chuan. Khéi danh gia tham s6 thyc hign theo 13so dé nhan dang binh phuong tdi thiéu thi gian thye dé quy cde tham s6 va hé 86. phan hdi ciia hé tuyén tinh hod durge cap nhat va chinh dink & mdi chu ky mau. ‘Mémen tong dat lén co cau chap hanh sé g6m hai thanh phan: mémen danh dinh duge tinh theo phuong trinh Newton-Euler tir khéi tién dinh va mémen bit sai Ich sé dugc tinh béi khdi phan hdi thyc hign theo luat ti wu mét cdp. Phuong phap diéu khién thich nghi theo sai I¢ch ¢6 hai wu diém co ban: N6 cho phép chuyén tir vin dé diéu khién phi tuyén vé diéu khién tuyén tinh quanh quy dgo chuan, Vige tinh ton mémen danh dinh cing nhw momen sai Ich duge thye hign déc lap va ding thai Tuy nhign phuong phap nay gp kno khin do khéi long tinh ton qua lon va do d6 thai gian tac dong sé cham, kho toi wu trong vige diéu khien robot, 1.2.4, Phuong phap diéu khién thich nghi theo md hinh miu “Trong s6 cée phuong phép itu khign thich nghi (iu khign thich nghi thong qua digu chinh hé s6 khuéch dai, digu khign thich nghi tu chinh, digu khién thich nghi theo m6 hinh chudn) thi phuong phép diéu khién thich nghi theo m6 hinh chun (Model Reference Adaptive Control - MRAC) duge sit dung rong rai nha va tong Gi d8 the hign, Nguyén ly co ban cia digu khién thich nghi theo mé hinh chun diya we sir Iya chon thich hop mé hinh ch va thugt tod thich nghi. Thudt tosin thich nghi dure tinh toan dya trén tin higu vao 1a sai Iéch gitta dau ra cua hé théng thye va mé hinh chi tir dua ra diéu chinh hé s8 khuéch dai phn hdi sao cho sai Igch 46 la nho abit. So dé khdi chung cia hé théng digu khién thich nghi theo md hinh chan duge trinh bay trén Hinh 1.3. Mé inh Yo huén—§ -_-——— Tinbigu 7 it inh 1.3: H¢ théng diéu khién thich nghi theo mé hinh chun. 14Phuong phap diéu khién thich nghi theo mé hinh chudn ¢6 mot s6 wu diém quan trong la né khéng bao gdm md hinh todn hoe phite tap va khéng phy thude véo tham s6 moi tring... Tuy nhién, phuong phap nay chi thye hign drge cho mé hinh don gin tuyén tinh voi gid thiét bé qua sw lién hé déng hye hoc gitta cae khép cita robot. Hon nita su én dinh cua hé théng kin cling 1a mot vin dé khé giai quyét vai tinh phi tuyén cao cia mé hinh d6ng lye hgc robot. 1.2.5, Phurong php didu khién dng lye hye ngurge thich nghi LA phwong phip téng hop cfc ky thuat nhim tw dong chinh dinh cée bo diéu chinh trong mach diéu khién nhim thyc hign hay duy tri & mot mie do nha dinh chat lugng cita hg khi thong s6 cia qui trink duge digu khién khong bi tragic hode thay 4di theo thoi gian. Vige phan tich eae hg théng diéu khién e6 chat Iugng cao luén 1a van dé trong tam trong qué trinh phat trién cua ly thuyét diéu khién ty dong néi chung va van dé nang cao chat lrgng hé théng diéu khién bam chinh xe quy dao chuyén dng ciia robot noi rigng. Tuy thude vio cae tiéu chudn phn loai ma c6 ede hi khién thich nghi khae nhau: Hé ©6 tin higu tim hay khong 6 tin higu tim; hé digu khién truc tiép hay gin tip; hé cue tri hay hé giai tich; hé e6 m6 hinh mau hay hé khéng o6 m6 hinh mau; hé tw chinh hay hé tw t6 chite wv... dang duge phat trign va va ap dung dé ting hgp cdc hé théng diéu khién quy dao véi chi tigu chat long cao, Phuong phap tong quat hoa céc hé thich nghi 6 ¥ nghia rat In trong vige bao quat mot s6 Iugng lon ee bai todn thieh nghi, don gian durge vige tim hiéu nguyén Iy eo ban cia ngay cd cic hg phite tap, trén co sir dé xy dung cée bai toin méi, ede thiét bj cy thé méi. 6 Cocau thich gh Nhin dang -@ Coc ditu ” — ign : Hinh 1.4: So dé khdi tng quat hé thich nghi. 15Vin dé diéu khién bam chinh xée quy dgo robot 1a mét van dé ludn nhan duge sy quan tim chit y. Hign nay sw phét trién mgnh mé cia ky thugt vé phan img va phin mém da cho phép gidm théi gian tinh toan, diéu dé dan t6i ning dong luc cho vige thie day sw phat trién cita cae hé thong digu khién quy dao thich nghi cho robot 1.2.6. Phuong phap digu khién trugt idu khién chuyén dong bit bidn véi nhigu logn va sy thay dBi thong <6 6 thé sit dung diéu khién & ché 46 trugt. Diéu khién kiéu tugt thude vé lop céic hé théng c6 chu tric thay déi (Variable Structure System - VSS) véi mach vong hai tiép khong lign uc. Phuong phép didu khién éu tnugt 6 die diém 1a tinh bén vimg rit cao do vay vige thiét ké b@ did khién c6 thé duge thye hign ma khéng cn biét chinh xic tit ci cde thong 86. Chi mot sé cae thong sé eo ban hoge mién gidi han cia ching 1a. da cho: vige thiét ké mot b6 digu khién trugt (Variable Structure Controller - VSC).CHUONG II MO TA DOI TUQNG - ROBOT SCARA SERPENT 2.1. Mot sé logi robot Scara eta cée hing sin xuit Robot Scara li mot trong nhing robot pho bign nhat trong céng nghigp. Chuyén dong cla robot nay rit don gin nhung Iai phi hgp véi ede day chuyén va img dung hou higu trong nhigm vy nhgt va 43 sin phim, Robot Seara (Selectively Compliant Articulated Robot Arm) 06 nghia la tay may lap rip chon Ic. Cau trie dong hoe loai tay may nay thude he phong sinh, e6 ede true quay, cae khép déu la thing dimg. No 6 edu tao hai khép & cénh tay, mot khép & cd tay va mot khop tinh tién, Cac khép quay host dong nha dgng co dign c6 phan hdi vj tri. Khop tinh tign hoat dng nho xi-lanh khi nén, true vit hode thank ring, Mét sé loai robot Scara cia cée hing sin xu Turbo Scara SR60 cis ing Bosch, Mn 2.1h: Assembly Scara Robot cia Hirata inh 21: Sara Robot cua EPSON, inh 2.1d: Scara Robe eta DENSO.inh 2. cara Robot of ADEPT. inh ‘cara Robot of RANOME inh 2. Scara Robot of STAUBLL. Hinh 2. {obot Seara Serpent ciia FEEDBACK‘Trong phin nay lun vin 98 xBy dung mB ta ton lige 46i tugng robot Scara Serpent ctia FEEDBACK. 2.2. Cée thong sé va viing lim vige ciia robot Seara Serpent Do chuyén déng cia robot Scara don gin, dé ding nén né due sit dung khé phd bién trong céng nghigp. O day nghién citu robot Scara Serpent (Hinh 2.3) mot ogi co ban trong nhém robot céng nghigp nay. Hinh 2.3: Robot Scara Serpent. Chiéu cao cha Robot ¢6 thé thay d4i d& dang bing cach thay ddi vi tri ga than robot trén try corbin, giip tay may thusinIgi trong vige thay déi céng vige Voi thiét ké dong co truyén dong cho c6 tay duge dat trén tryc co ban va lién hé véi cé tay bing dai truyn, nén né dim bio duge géc quay ciia cd tay khong thay déi trong qua trinh tay may chuyén dong. ‘Truytn dong cho 2 khép cia tay may va o6 tay bling dng co servo mot chidu 6 phan hdi vi tri tao thanh mét vong digu khién kin. Chuyén déng thing dimg duge thy hign bing piton Khi nén. Robot Scara Serpent o6 thé duge lip trinh tir may tinh biing céch dat dtr ligu cho indi try. Hoge digu khién bing tay sir dung thiét bj lai dign (steering) cho tay may diing cdc cugn day dign tir trong gid treo (pendant).2.2.1. Cu tgo tay may robot Seara Serpent 2.2.1.1, Cau hinh cita robot Seara Serpent Bao gm mot chudi cae thanh cing duge lign két véi nhau bai cde khop; Hinh 2.4: Cau hinh va cde hé tryc toa 46 gin trén Robot Scara Serpent. Robot Scara Serpent gdm 3 khép chuyén dong quay va m6t khép chuyén dong, tinh tién. Gin cho méi thanh ndi mét hé truc toa dé, ta c6: ~ Khép 1 quay quanh tre Z0g6¢ 6). ~ Khép 2 quay quanh true 2; e6e 0. - Khép 3 chuyén dong tinh tién theo true z2 doan ds, ~ Khép 4 quay quanh true 25 26¢ 0. 2.2.1.2. Cac thong so kf thuat cia robot Scara Serpent ‘Thong s6 cia déng co 1, 2, 3 tong img véi cde khép 1, 2, 4 ca tay may robot Scara Serpent (xem bang 2.1), = Déng ea 1 truyén dng cho kh6p 1 (main), = Dong co? truyén dong cho khép 2 (fore). = Déng co’3 truyén dong cho khép 4 - khép c6 tay (wrist). 20Bing 2.1: Thong s6 cdc dng co cia robot TT | Loai_ | UCV) | (A) | MONm) | N(wip) | POW) | J(Kgm’) | RC) | L(uH) | m(Ke) 1_|yzr } 12/48 2100} 15 |0,32.10* [1,38 | 100] 0,6 2 |sozF [12/48 2100} 15 _|0,32.10* [1,38 | 100 | 06 3 [az fas 2100 [26 [1s.i0* [09s 100 [i Bang 2.2: Cac thong 6 dng hoc ca robot Scara Serpent. TT [Thong sd _ | Kich thude dong hoc 1 [m=4Kg _[Khdi tuong thanh néi 1 m= 1.5 Kg | Khdi hugng thanh n6i 2. ms=2Kg_ | Khéi lugng thanh ndi 3, 4, = 0.25 m dai thanh néi gitta 2 khop main va fore 15m dai thanh néi gitia 2 khop fore va c6 tay 2 3 4 [m= 0.6 Keg | Khdi lugng thanh n6i 4, 5 6 7 dai thanh ndi dy phy thude vio eh& d6 lim vige ea tay may. 2.2.2. Gi6i han khdng gian lam vige cua robot Seara Serpent Cae bidn khép e6 ede gidi han gée quay nhur sau 4 -96° + 96° ( so voi truc Ox ). -115° + 115° (60 véi true thanh 1). Chuyén dong quay etia khép thir nhdt e6 hinh chiéu bing trong hé true toa a> OXi¥q va OX\Z, (Hinh 2.5) tong ting véi gde quay ting trong thye té 1 192". Chuyén dong quay ciia khop thir hai o8 hinh chiéu bing trong hé truc toa db OXY; vi OXY2 tuong fing véi zoe quay ting la 230°. Tir d6 ta c6 thé thay durge hinh chigu gigi han khong gian lam vige ctia né (Hinh 2.6) auHinh 2.5: Giéi han g6e quay cia 2 khép. Hinh 2.6: Gidi han khong gian lim vige eta robot Seara Serpent. Nhur vay khoang khéng gian ma tay may c6 thé véi ti la ton 66 hinh tru véi day c6 duomg gidi han bén trong li m6t cung tron c6 ban kinh r= 0.231 (m) va dudmg gidi han bén ngoai la dudmg tron ban kinh R = 0.4 (m), 2Khi biét duge vj tri no ma tay méy o6 thé dén duge ching ta cé thé Igp trinh trong Matlab dé tim vi tri, quy dao nado ma tay may e6 thé vuon t6i (durge xét déa & chuong II] trong ban Indn vain nay). 2.3. Dmg hgc robot ara Serpent Robot Scara Serpent e6 céu tric dong hoe dirge biéu dign nhur trén Minh 2.4 Robot ¢6 3 true quay va | ban kep, tuy nhién ba khp dong dau tién duge goi la bo phan co ban vi trac hét, nhs chting tay may c6 thé thye hign bude chi yéu trong thao the dinh vj, tte 1A dura ban kep dén Ian cin diém lam vigc, sau dé nko khép dong cdn Jai ban kep duge dinh huéng va vi chinh dén vj tri gia cng chinh xéc. 23.1. Déng hoc thuan Vige xay dung ede phwong trinh dong hoe thudn cia robot duge tién hinh twin tu theo cde bude sau Bude 1: Xéc dinh cdc hé toa dd Ta sir dung quy ude Denavit-Hartenberg dé m0 ta diy dit vi tri ea cia toin than robot cng nghigp. Hinh 2.4 m6 ta cae he trye toa d> gin véi cde Kite tay cita robot Scara Serpent. Bue 2: Xiy dung bang thong sé DH. Bang 2.3: Tham s6 Denavit— Hartenberg ca robot Scara Serpent Thanh noi }a(°) | a, O(rad) | d\(m) | Bi ‘Chuyén dong 1 0 ay a 0 0 | Quay 2 -180° Ja 0» 0 Oo Quay 3 0 0 0 dy ds Tinh tién 4 0 0 o 0 O. _ [ Quay: Khao sat vi 3 truc khop quay dau tién tong ting v6i quy dgo ctia khop quay 4 trong mgt phing OX,Y¢. Ma trn T, 1d ma tran biéu dign tay may robot trong hé truc toa d9 abe: Ty= Ar. AnAs.Ay 23cos0, -sinO,cosa, sinO,sina, a, cos0, sin0, cos, cosa, -cos0, sina, a, sin0, ey 0 sina, cosa, 4, 0 0 0 1 Thay s6 ligu trong bang tham s6 c6: cos0, -sin®, 0 a,.cosd, sin@, cos®, 0 a,sin®, 1 0 oo4 ‘cost, 0 a,.c0s0, sin, 0 a,sind, 0 a1 0 0 o 0 1 000 100 old ool cos, -sind, 0 0 a, =[9n8. 6050, 0 0 0 o 10 0 0 o1 > Ky higu: S; > Sind, ; C <> Cos) Sr €9 Sin: ; C2 <> Coss Sy Sin, ; Cz Cos0s Siz Sin(0)+02); Ciz = Cos(8;+02) 24> Cae bude tinh toan: Bude 1: 'T, =A, c, -S, 0 0 ee 10 oO. 00 00 1d, 01 Bude 4: T,="T, = A\'T, CS 0 aC][C.C,+88, $,C,-cS, 0 _|S, G 0 a,§,|/S.c,-cs, -~(C.C,+8,8,) 0 i “|o 0 1 0 0 0 -l a oo01 0 0 0 ciieristh eee uircee sade ses ese SIC3Cq +8984) +CUSICy —CSq) SSICq—C8u) -CVEICg +884) aySyp +848) ° ° 1d |22) ° ° o 4 25Ma tein" bigu dia tay may robot trong hs toa dé ede. Mat khae theo ky higu tng quat: iM, % 4 P,| tet=[ % % PB In, % 4, P, ooo 1 (23) ‘Voi: H,b,8,P Lin tugt 1 cdc vector dinh vi, vecto dinh hung, vecto ti va vecto vi tri dé biéu dign huréng va vi tri cua tay may trong khdng gian lam vige. ‘Tirma tran trén ta c6 hé phwong trinh dng hoc thudn tay méy robot: 0, = C(SsCy= C2 $y) + S(CCs* 8:83) 0, = Sy(SrCu- Cz $4) - Cy(C2Cu + $284) ‘Va h@ phuong trinh xae dinh vi tri caa diém tie dng cudi nh sau: aC + ay ay.S) + a: S12 sods 2.3.2. Dong hoe nguge Déng hoe nguge: xée dinh cde bién khép khi biét vi tr tay. Tir phuong trinh dng hoe thud e6: 26 (24) (2.5) (2.6) en 28) 29) 2.10) Qu) (2.12) (2.13) 2.14) (2.15)(2.16) 17) Tir dé tinh duge géc tan2(sin0;,c080) @.18) ‘Thé C,, $; vo phurong trinh (2.13) va (2.14) thu duge: (ar'aC).C,~ 08281 ps S.C, +(ap4asC2).8; = py Giai phurong trinh bac nhit vai an C,, S, va sir dung (2.16) thu durge : (a +a,C,)p, +aS,P, p+; e (a, #4,C.).p, Meese pi+P; 6 = atan2(S1.C,) (2.20) Tir phucmg trinh (2.15) t €6: 4s=-p, 2.21) ‘Mat kiade tir pharong trinh (2.4) 06: y= C(C2 Cu + S284) SSCs C28.) (2.22) Riit gon theo cae cdng thite rong gide thu duge: 1nx = €080,,£05(02-04) ~ sinO sin(@.-0,) = e05(0,+04-0,) 2.23) sin(®,+02-0,)= Jl-n; (2.24) @, +0, -0,) =atan2(J1=ni n,) 0, = 0, +0, —atan2(/1—n?,n,) (2.28) 7‘Vay hé phuong trinh dong hoc nguge ctia robot Scara Serpent la: (a +a,C,).p, -a.S.p, PL +P, 6, = atan2(S,,C,) s cos0, = Px *Py ai 78: 2. Ay sind, = + ((—cos"0,) 0, = atan2(S,,C,) —P, 0, =0, +0, ~atan2(J/l—n7,n,) 24. Dng lye hge robot Seara Serpent Dé m6 té mdi quan hé gitta luc, momen v6i vi tri, van te va gia tc cia di tugng robot, cin phai xdy dung duge phurong trinh dong lye hoc, tir dé phye vu cho cong vige thiét ké va di kién robot. Vi vay cn phi tinh ton day di cc thong sé cota dbi tuyng tniée Ehi dua vio mé phong. Phuong trinh dng lye hoc eta robot duge biéu dign nh sau: H(qyate h(q, 9 + g(a) = e(t) 2.27) Trong 46: = t(t) = Veeto [n x 1] hye déng tao nén &'n khép dong: 2) =[n@), , EADT! 2.28) = q(t) - Vecto [nx 1] bién khop: (0) = [auld 200, a,(0))" (2.29) = H(q)~Ma tran [nx ne ede phin tir Hy sau day: Hy = Y. Trace(U,LUS). G.k=1,2, 0) (2.30a) 28(2.306) ean 032) hy, = LTrace(U,,.,U; } (233) itm = (2.34) = g(q) - Vecto [n x 1] luc trong tring: 86) = [8s Bos Bal (235) 8,-¥(-mgu,'r) (236) ‘Véi robot Scara Serpent gdm 4 chuyén déng (3 chuyén dng quay va 1 chuyén dong tinh tién) va mé phong voi 3 chuyén déng quay 66 cée phwrong trinh ou thé sau: Vecto [4x Hue dong: e() = ftx(0, 2, OD), cA(0 Veto [4x1] bién khop: q(t) = [qu(t), gat, d3(0), qa(t)]" 2.4.1. Ham Euler - Lagrange va cae vn dé a1 hge Lagrange dinh nghia sy khée bigt gitta dong nang va thé ning cia hé théng: L=K-P ean Trongdé: — K : la dong nang eda hg théng. P : 1a téng thé nang cia hé thing. Do dé phuong tinh dng ive hge duge xée dinh bing bid thie (2.38) Trong dé: — q\ = 9, ddi voi khop quay; qi = 1, di véi khop tinh tién. ti: momen hve déng tai khép thi. 29Dong ning ciia khop thir i: K Emi + 1.0? 2.39 mii t FO; (2.39) Voi J; 14 momen gun tinh eta khép thi i. 2.4.2. Ding lyre hoc robot Seara Serpent Theo cau hinh, robot Scara Serpent c6 cae thong s6 Bang 2.4: Bang 2.4: Thong s6 cia robot Seara Serpent. Khop 1 Khép2 _[Khop3 _[Khép4 o % ds 6 a & dy 0 ™ m: ms my vi ¥: vs ve Chidu dai im kasi [iy la la le Gia sit khdi long nim & dau mit ede thanh ndi, ta ¢6 chiéu dai tim khéi chinh, 1a chigu dai ciia thank ndi: y= a, yp =a Ky higu chiéu dai cée khép: Hg qui chiéu gin véi te toa db (Osxoyoz) trén khop thir nhét, Khi dé mat phiing (Ooxoyo) 1a mat phing ding thé. 24.2.1. Tinh todn dng nding va thé nang eho timg khép > Khép | 1,,.c0s0, y, =1,,.sin0, 7,=0 30hin chia cia than néi s6 1 Ten ede trte x,y,z (080, Trong d6: J, momen quan tinh kh6p 1. P=0 K, eee mv) + Ie (2.40) +15,.08 atimytt.024 15,63 ime KE} + [h.c0s0, 6 +1s.005(0,+0,).( + BF 2 (8 + 6 Joost, 41) > Khép 3: x, =1).cos0, +1,.cos(0, +03) Ys =1,.sin0, +1,.sin(0, +0,) z=, aI-1,,sin0 0, in(0,+0,).(0,+0, ) Ys =1).c050,.0,+ cos(8,+0,)(6,+05) a \sieyieg 1, sin0,.0,— sin, +0,)40,+04)] “fh cod, 6 eos, 20,)60,403)] 15 2(0,48,)° +21,.1,.(0;+0,.0,) +15 (0,405) +2, 1.(0520,0)] (242) > Khép 4: x, =1),cos8, +1,.cos(@, +0,) y, =1,.sin®, +1,.sin(@, +0,) x, =-1,.sin®,.0,—1,.sin(0, +0,).(0,+05) ¥4 =1).c080, 0, +1;.cos(0,+0,).(0,+0,) 24= vis [-isino, 6-1, sin(0,+0,).0,+0, | ao £030 0), +1,.c0s(0,+0,).(0,+0, 4¥15,(0,+0, )* +2.1,4,.(07+0,.0,) +E 321 . 5 4.(0,40,40, > (8,404, (243) Tong dong ning cia héthéng K=K,+K:+K;+Ky (2.44) ‘at tim kh tai du mit cba thanh ni, ta 68: y= Ko 3[ (4m, 4m, +m) (on, 4m, 4m IH 13, 44,41] ort +(m, +m, +m,)l.1,.cos0,.& + [(m, +m, +m) +J, +4, Ee +. +m, +m, +m,).1,1,.c0s0, 64 +4Lim, +m,+m,).24+J,+I,]&+.. Plone m Bd EB 1B Tong thé nang cia hé thong: PPP Pst Py =-(ms+ meh 2.46) 2.4.2.2. Phuong trinh dong lye hoe Lagrange ctia hé théng: L=K-P Le dtm, +m, +m,+m,)E +(m, +m, +m,)E+5, 45, +1,)R+.. +(m, +m, +m,)1,1,.Cos0,.& +... +[(m, +m, +m, +3, +5, ] 68+... ean Bebe +. +(m, +m, +m,)I,1,.Cos Flo 4on, 4m HE AT or, tm 4 1 B+ 1 6A 1H Cm, em)el, 33Cac phin tira phuomeg tinh dGng Ine hoe: do = a ame 2a, 2.48) > Khép |: aaa 2 ao 28, (2.49) a & fim +m, +m, +m,).l +(m, +m, +m). +5, +1, +4, ].04.. 00, 4+2(m, +m, +m,).1,1,,c080,.0, +[(m, +m, +m I +I, +I, }.0,+ +(m, +m, +m, ).1,.c0s0,..0,+,.0, b da 2 2 << =[(m, +m, +m, +m); +(m, +m, +m,).; +5, +5, +4, ].0+. at 59, 42,(m, +m, +m, A. 1,.6050,.0,— 2.4m, +m, + M,)llysi08,.9,.94- 6 s9y +[(m, +m, +m) +3, +, ].0,+(m, +m, +m,).1.1,.c080,.0,4... (m, +m, +m,)1.1,.sin0,.03+J,.0, él 9 20, Qs!) >» Khép 2: oe (2.52) © dtag, 8: a a (m, +m, +m, +J, +5, ]&+(m, +m, +m,)h,.1,.Cosd, 6+ +[(m, +m, +m,)E +5, +5, +I. 34~[(m, +m, +m, lL, Sind, Be + (2.53) +[(m, +m, +m I +5, +5] 6+, 6 m, +m, +m,)h,1,(-Sin0, ).& —(m, +m, +m,)}1, Sind, BE (2.54) 2.55) a b 2.56) « 37) (2.58) a a5, 6,45,6,44,8, 20, » 2 x 5,645,0,45,6, 2.59) a 59, oL og ony 2.60) Nhur vay phuong trinh dng lye hoc cita hg théng duge biéu dign bing cic phuong trinh sau: 35T= Lm pany tm, 1 +J,,, +2.m,,,1).1,.Cos6,] + 24, + my,,4)1,.Cos0,) + 1,84. 261) =. ,,,.1,.1,.Sin0, 6 — 2m,,,.1,.1,. Sind, & 2 + Jay + My 4, Ly Cos0,, | E+ 2.62) CMa 13 +5) BE + 1, BE +m, 11, Sind, =m, #-m,g=F, (2.63) 1, = 50 50 + 16 2.64) Trong d6: me my F my + my + mg amy = mgm + my (2.65) msg =m + mys VOL my= my +m, may: khdi lugmg cha kop 4 m, —_: khOi lugng cua tai durge néi voi khop 4. Jig Sith tds Soy Jas Voile = Sant hy Jig $m men quin tinh eta khop 4, 4 16 men qui tinh cia tai duge n6i voi kh 4 Voi ny ty Lin lugt Li mémen d6ng tai cae khép quay 1, 2 va 4, ‘ty = Fy ld lye dng dat lén khép tinh tién 3 Néu chi xét momen dong véi 3 khép quay thi c6 thé viét gon lai ba phuong trinh dong hye hoe (2.61), (2.62), (2.64) dé tign cho qua trinh tinh todn nh sau: ut] [Hy Hy Ho H, SJ LHy Hy Hy 36u] [Hu Hp &) [78 + 2b vay fe f=) mmf aefs] re 266) uJ} [Hy Hy & 0 Céc thanh phan trong phuong trinh dong lie hoe durge xée dinh: Hy =m) #my E45... +2.m,11,.Cos0, Hy, =my li + Ja, +My l,4,.Cos0, Hy =Jy Hy = Hy my lit], en Ly 4, T=-m,,,1,1,Sin0, h, = TE + 276 hy 2.5. Mo ti d6t tugng bing hg phurong trinh trang that Dit bign trang thai cho timg khép nhwr saw: x= [xt x! xi] (2.68) (2.69) 2.70) (2.71) 37By =Xy ap? " 2 fee { 2 bid, bat, +a, 7 wpa. Pete tons (Ethan tbe fa Trong dé: ‘b4(X) 1a cae thanh phan twong img cia ma tran 1. by = (Halt, - 74) Belts HHa) , (HH, -H,H.) 1 b. HH. ~ HH, = (ls Halts) D, =detH =H, HH, + H,,HH,, +H,,H,,H, ~H,H.Hy ~Hy HH, —H,H.H, h(x) - .a() 1a than phan thir i cua vecto: ~ H'} ha (X) h,(X), —[by hy bh, + byshy] ay =-[b,,h, + bash, +b,,h,] (2.75) —[byyh, + bh, +b, hs] a, 38.CHUONG IIL XAY DUNG MO HINH HE THONG DIEU KHIEN 3.1. Cu trite hg théng diéu khién robot Robot thudng ty dong thye hign cic nhigm vy dich chuyén hoe cae thao tac Céc hoat dong cia robot thudmg duge lip tinh, vige thuc hign cée nhigm vy thuéng im thay 46i mot phn hode toan b9 vi tri ca robot trong khong gian, Cé thé coi robot hign dai nhur nhong ngw’si may, e6 thé the hign eée thao téc phite tap, 06 d6 chinh xae Quy dao Bo diéu : dat chinh Taymiy | ¢ Cam bién cao, Hinh 3.1: So dé cdu tric chung cia hé théng didu khién tay may robot, idu khién chuyén dong cua tay may trong khong gian la xée dinh n thanh phn mémen luc téng quit tac dng Ién ede kh6p, mémen lye téng quit duge cung cp bai co cu chap hanh, Qué trinh digu khién phai dim bao b6 digu khién sé thye hign digu khién chuyén dng cita cic khép theo quy dao q(t) sao cho a(t) ludn bim ga(t), voi gu(t) la veota quy dao chuyén dng mong musa, So dé céu trite chung cia hé théng diéu khign tay may cho 6 Hinh 3.1, tay my sé bam theo quy dao dt trade (durge Kip trinh sin hod e6 thé dura vio tir chuemg trinh phim mém két ndi bén ngoai) sir dung b6 diéu chinh dé diéu khién chuyén dong va Tye, sir dung cac cam bién dé thu nhiin céc théng s6 vé méi trig lam vige dam bao cho hogt dong bén ving, khir drge nhigu ngoai n, nhgn biét va dip img thich nghi duge véi nhing su thay d6i ctia moi tragng cting nhwr sy thay d6i cée tham sé dong. hoe cua cia hé thong. 393.2. Xiiy dung qui dao chuyén dng chuan ‘Trong didu khién quy dao robot, phai diéu khién tay robot bém theo mét quy dao xée dinh tnrde (quy dao chun). Quy dao chun dé 1a mgt ham phy thuge thai ian. Vige xy dyng quy dao chuyén dong chinh 1 phuong trinh mo ta quy dao robot theo théi gian. C6 hai bai ton thiét ké quy dgo cho robot - Thiét ké quy dgo cho tay robot: xdc dinh x(t), ya(t). = Thiét ké quy dao cho khép robot: xe dinh quit). Vige thiét ké quy dao khép robot 6 wu diém: q(t) la Iwong dat tre tigp, he digu khién 1a he digu khién vj tri khap, c6 thé xie dinh duge digu kign giang bude cua dong co va hé truyén dong, giai bai todn dong hoc nguee don gin. Nhuge diém Li kh6 dim bio chinh xde quy dao tay. ‘Thiét ké qug dao tay robot 6 i idm: luge, dt lv te tay robot nén Adm bio chinh xée quy dao, Nhuge diém: phai giai bai todn dong hoe nguge phite tap vi khdi Iagng tinh todn lin va khé tinh duge digu kign giing bude. Yeu cdu digu khién sao cho kh6p robot chuyén dong tir vi tri qo dén vi tri qe trong thoi gian ty, Voi dit kign ban déu nhwr vay e6 ce dang quy dao nhur sau: Dang quy dao bac 3 a(t) =a +b + et +d GB.) Vide thiét ké quy dao la xiie dinh cac hé sé a, b, ¢, d. > Dang quy dao 2~ ‘Trong qua trinh chuyén dng tir qo dén q. trai qua ba giai doan = Giai doan ting téc: q(t) 1a ham bac hai theo t ~ Giai dogn chuyén dng déu: q(t) la ham bac nhat theo t. = Giai doan giam te: q(t) la ham ba hai theo t > Dang quydao4—1~4 Khdp Robot chuyén dng tir qo dén qe trong thi gian t, biét: gO) = 4; A(t.) = 4.3 HO) =: t,) RO) = Ge; et.) = G2) 40Quy do chuyén dong c6 ba giai doan: - Giai doan ting téc: q(t) la him bie 4 theo t. ~ Giai doan chuyén dong déu: q(t) la him mht theo = Giai doan giam tc: q(t) la him bie 4 theo t Phan nay sé thiét ké quy dao chuyén dong ciia cdc khép cho robot vi dang quy dao 4 — 1 ~ 4, dimm bio d6 chinh xée khi xiy dmg quy dao chudin, Bai toiin xdy dung quy dao chudn 1a Iya chon quy dao chuyén dong gita vj tri diu va cuéi cia tay robot ‘i thd gian chuyén dong cho trade. Noi dung muc nay sé gidi quyét bai todn guy dao chuyén dong chufin cho cae bién khép. Ta chon mot bién khép bit ky qi thay cho géc quay 0, (i =1, 2, 3). Théi gian chuyén dng tir vi tri dau (xq, yo) toi vi tri cudi (xe, ye) Id te. Gia trj ban dau eta q tai thoi diém ban dau (t= 0) Ia go va gid tri tai t= te la qe. {a jac 8 atta.) [tad at.) = ae a ‘Do thi quy dao chuyén dong chuan 6 trén Hinh 3.2: 4q Hinh 3.2: Dang quy dgo chuyén dong chudn. 41Quy dgo chuyén dng xudt phat ti go dén ge 58 qua ba gini domn: gia tbc, chuyén dng v6i tée d6 khong déi va gidm te. Dé don gidin cho vige tinh to‘in, 6 thé chon tham sé t, bang 1/2 Khoang thai gian gia t6c hoge gidm tée. Ta cling xde dinh cde dim phy cia quy dao chuyén déng qo, va qa tai t= t, vat = tat l&: qo) = go Va da = cr NOi qo: va ges bing mot dung thing vi xde dinh diém go>, der 24, va Biing cich chon diém phy nhur trén, quy dao dogn ed 14 mot duéng thing véi téc 4 khong déi, quy dao doan ac va dfcé thé chon La da thite bac bén ¢6 dang: = ay + ayt + at” + ast! + ayt’, (G4) inh gia tri q02 va gel hur cach vé da trinh bay @ trén, duemg ndi be la durime thing. Gid tri qo va get tai t= 2t va t= ty 2ty €6 thé duge xéc dinh tir gid tri du qo va cudi qe dua trén phurong trinh bc nbat ciia during thing be: Ge = 4a a 6s) 174. 3.6) 4 E SE 66) 3.2.2. Phuong trinh dogn ed Quy dao doan ed 18 dudmg thing bién dign bai phuong tein: e7 VOi 2 StS 4-24) ‘Thay (3.5) vao (3.7) va viét gon Iai ta c6 durge: G8) G9)3.23. Phuong trinh dogn ac Quy dao doan ac biéu dién bai phuong trinh dang da thire bac bon (3.10): ac = Abse + tact + Anat” + aact” + aust’ 3.10) Cac h@ s6 cita phuong trinh doan ac duge xac dinh tir digu kign dau va cub Gal) B.12) Ly dao him ep 1 va cdp 2 cita (3.10) va sit dung (3.11) va (3.12), ete hé sb duge xée dink nhw sau: 6.13) 3.2.4, Phuong trinh doan df ‘Qu¥ dao doan df biéu dign boi phuong trinh dang da thite bac bén (3.14): ar dace + at + anait” + asait® + agaet® G.14) Cac h@ s6 cita phurong trinh doan ac duge xc dinh tir digu kign diu va cui ac(te - 26.) = da (ts 26) (t= 2t,)= (ty 26) B (1, —2t,) = & (ty - 24, )=0 G.15) u(t.) =a. (tu) =e 43Cudi cing cde hé sé drge xée dinh nh sau: (3.16) 16t! Nhu vay quy dao chuyén dong cua 3 khép 1, 2, 4 ctia Robot Serpent cé thé duge xée dinh bing cée phuromg trinh (3.8), (3.10) via 3.14) v6 phurong trinh duge xde dinh tir cde gid tri ca vi tri déu va vi tri cui turong img bai cde biéu thite: (3.9), (3.13) va B.16), 3.3. Thiét ké b9 digu khién cho tay may robot Seara Serpent ba bie ty do 3.3.1. HG phuong trinh dong lye hoc Lagrange Hg phuong trinh déng Ine hoc Lagrange cita tay may robot Sara Serpent dae vidt dur6i dang ma tran sau Hy Hy Hy [&] [had] [e,(@) t,|=|Hy Hy HH. | &|+] h,(g.4)]+| 2,(q) Guin H % v Hy Hy] @&) [a4] Lela) ty &) [TH + 2TH hay: | 73 [= |B ls) Te 1 & 0 Trong dé 1), t) va t lan ligt la cde momen digu khién tac dng lén khau 1, khau 2 va khau 4 (tir 2.61 dén 2,65). Voi ede tham s6 Hy, T durge cho theo (2.66), (2.67) (a xét 6 chuong 2)Bm, 8 4J,,+2.m,,,1,1,.Cos0, Mal? + Ia, +My, 1, Cost, H, T=-m,,,J)1,Sin0, h, = TE + 2TH va: Miay4= My + M2 + M3 +My; My34 = My + M3 + my M34 =m; + my 5 my = Myo +m 5 may: kh hugmg cia khap 4 m, —_: khOi lugng cua tai duge néi voi khop 4. J tla; de =h+des a= Jay m6 men quan tinh tia khép 4. J, :mé men quan tinh ciia tai duge ndi voi khép 4. 3.3.2. Hg phwrong trinh trang thai va tin higu vio U=/u, |=| 7, a} ty. 45HG phurong trinh vi phan trang thi ca céc khop 1, 2 vi4 duge viét nr sau: & Khép |: 4,004 Sb, G18) Hy =X Khop 2 ‘ 1 ee j=800+2.0,] ae gic Khép 4: 7 (3.20) &,=0,)+Db,u, eae Tir cde phuong trinh (3.18) dén (3.20), ta c6 hé phvrong trinh trang thai cua khép 1 va 2,4 duéi day: > Khop 1 [8 = Xo 7 (3.23) =a, +byu, +b wy +B, su, Voi: a(q. =-H'(@).h(. ay [by hy +byhy #byhy]=—[b yh, +b,.h] 6.24) by = 5 Haly —HoH) HH) by = (HaHa HH) Dy =det H =H,,H,,H,, +HjH,,H,, + HHH, — HHH, Hy sHy — Hy) Ha Hy (2H,; Hy, ~H,2)+J,(H, Hy; = Hj.) > Khop 2: =X, 5. KW, =a, +byu, +b,.u, +byu, G25) Voi: a, =—[b,,h, +b,,h, +b,,h,]= (3.26) 46Hy -HyHy) (HH, -H).H,)) Hy -HyH,,) a, +b,u,+b,,u, +b,u, G2n Voi: a, =-[b,h, +b,h, +b,h,]=-[b,,h, +b,,h, | (3.28) HaHa) b,, = ——(H,,H,, - HH, = BH ) Pha vt, trinh (3.23) Phang ss sa 25 - L Pane so eman(aan) | AAA fase Hinh 3.3: Mé inh héa dip ting du ra thyc eta robot. hur vay m6 hinh tay may robot ba bac tyr do 1 mot hé nhigu déu vao nhiéu diu ra, duge mé ta bing ba hé nho, mi hé trong img voi tig khdp 1, 2 va 4, duge dic trumg boi ba hg phurong trinh vi phan trang thai (3.23), (3.25) va (3.27). Céc he phuromg trinh nay c6 thé dung dé m6 hinh héa trén may tinh cing nhu dé téng hgp ludt didu Khién cho tay méy. 473.3.3. Lya chon phuong phap diéu khién va bo diéu khién PID Phuong phap digu khién duge Ira chon la phuong phap digu khién dng Iue hoc nguge voi diu vio bé digu khign Ia sai 1 tin higu dieu khién us, 6 bé diéu khién PID fa up ich gitta tin higu dat va tin higu dau ra, Bau ra PID Hin 3.4: So dd b9 digu khién PID Ham truyén etia bé digu khign: «(rot ots] Ts tinh don gian cua nd ca vé cau G.()=K, + Bo digu khién PID duge sit dung kha rong rai tric lin nguyén ly Kim vige. Muén hé théng ¢6 duge chat Iugng nhur mong mudn thi phai phan tich di twgng r9i trén co s6 46 chon cde tham sé Ke, Ks, Ky cho phi hgp. > Phuong phip Ziegler — Nichols. Ziegler ~ Nichols li phuong phap xéc dinh hé sé Kp, hing s6 thoi gian tich phan T; va hing s6 théi gian vi phan Tp dua trén dic tinh qué 46 cia hé théng digu khién. Cé hai phuong phép higu chinh Ziegler — Nichols déu huéng ti mye tigu dat 4 qué diéu chinh khoang 25%. - Phuong phap Ziegler ~ Nichols. Taromg hgp 1 Xée dinh théng s b9 diéu khién PID dyra vao dap img bac thang don vj cia hé ho (néu 46% trong khong chita ede Khaw tich phin hay nghigm phite lign hgp thi duong qua dé ciia d6i tong 06 dang chit S) 4g.Hinh 3.5: Bap ting bac thang don vi cia hé he. Ty: thoi gian tr8 To: hing s6 thoi gian ~ Phuong php Ziegler — Nichols. Trrtmg hop 2 T, vi Ty duge xe dinh bing fh ve duéng tgp tuyén voi dudmg cong $ tai diém uén, during sip tuyén nay edt true hodnh tei T, va durime y()=K fa diém 66 hodnh 49 Ts 5 Ko ots Khi dé md hinh déi tugng c6 dang: G(s) = Tea Bang 3.1: Thong s6 69 PID. Thong sd] Kp Tr To Bo DK P iK [oo 0 PL ost K | 103 0 PID LatyTiK [27 037, ‘Xie dinh théng s6 bé digu khién PID dura vio dip img cia hé kin 6 bién gidi én dinh, 49o Hinh 3. : Bap ting ctia hé kin 6 bién gid dn dinh. pat T, , To= 0, thay d6i Ky tir 0 t6i gid tr gidt han Ky, img voi dau ra he théng kin c6 dao dong & bién gi in dinh, Dao dong nay tuong img véi chu ky gidi han Ty, Buse 2: Thong sé bé PID duge xac dinh theo bang = Bang 3.2: Thong s6 b9 PID Thong so | Ke Tt To Bo DK P 05Ky | © 0 PL 045Ky [0.8375 [0 PID 0.6Ky [05ST | 0.125T voi: K, = 2. K, =K,.1, T, 50CHUONG IV MO PHONG VOI MO HiNH ROBOT SCARA SERPENT 4.1. Dat van dé Trong chwong IIT da tién hanh x4y dyng m6 hinh cia cde khau tinh toa hinh robot va b6 did khign PID, Sau d6 43 x4y dymg cde chung trinh phuc vu md phong va khdo sat cde ché dd im vige cia hé théng. Trong chuong nay sé trinh bay cfc két qua khdo sét nhin durge thong qua ede churong trinh m6 phéng néu trén, Thong, qua vige dinh gia sai s6 quy dao dat va quy dao thye dé danh gi chit hrong dita chinh bm chinh xée eta phurong php diu khign durge nghién cu Vige mo hinh hoa va nghién cir m6 phong hg théng diéu khién chayén déng due thyc hién trén nén Matlab-Simulink, giao dién GUI. 42. So dd mo inh hoa cdc khau ciia hg thong 4.2.1. M6 hinh chung eta robot Teoqurdao | 94 | Boditu | 4 — | Cocduchip chuyén kbiinms |" han ging chun men dong, (Robot) Phan hdi Hinh 4.1: So 3 khdi mé hinh héa robot Seara Serpent sit dung bo digu Khign momen dong. Déng lye hoc cita robot Seara Serpent durge mé ta bing hé phuong trinh trang thai (2.71), (2.72) va (2.73) tong img véi 3 khép quay cia robot. MBi hé phuong, trinh vi phan cho ting khép gdm hai phuong trinh vi phan v6i bién trang thai 1a g6e quay q,(x,)va tbe d6 gée cia khép (x,.). Cée bién trang thai d6 s& durge xée dinh biing tich phan cdc trang thai tuomg img (Hinh 3.3). 4.2.2. Mé hinh khéi tgo quy dao chuyén dong chuan Hinh 4.2 mé ta so dd khdi tao quy dgo chuyén dong chudn. Cac diéu kign dau va cudi ciia quy dao chuyén dong chuain dirge tinh thong qua cic toa do dat (xp, yo), 31(Xo ye) va thoi gian chuyén dng (1,). Quy dao chuyén dng chuin ea ci 3 khép duge xée dinh theo 3 giai doan: gia tc, te do khdng déi va giam téc, biéu dién bing ede phuong trinh (3.8), (3.10) va (3.14). 4a —- ia Quy dao au || J au L, ccna kop 1 = Saal ie 4a Dat toa 46 a 90) Quy dao G.¥9 chun kop 2 cS) 4s det Quy dao du au L, aud Kop 4 cae lel ae (ee Hinh 4.2: So dé khdi tao quy dao chuyén dong chun, 4.2.3. M6 hinh b6 diéu khién offs - oe feof i . + we +|:>+[ ee Hinh 4.3: M6 hinh b6 digu khién cho khép 1 cua robot Scara Serpent. Céc khép 2, 4 c6 mé hinh b@ digu Khién eding twong ty nhur khép 1 B6 tham s4 [Kp, Ki, Kp} ctia bd diéu khién PID due chon theo phuong phép Ziegler — Nichols. 4.3. Glao dign ehurong trinh mo phdng robot Scara Serpent Khi chay chuong trinh mo phong ta dnh Ignh >> RBSCARA_SP trén cita s6 chinh cia chong trinh Matlab, trén man hinh hign ra bang lam vige chinh nhur sau 52‘Hinh 4.4: Menu chinh ciia chuong trinh, Chuong trinh RBSCARA_SP gdm cic chite nang chinh sau Nh§p cic thong sé cho mé hinh. ong hoe thud. Dong hoc nguge. M6 phong. Hin th Thost. CChuromg trinh durge thigt kétrén gino dign GUI trong Matlab, véi ce nit thye hign cfc chite nang nhu trén, Céc chite nang ciia churong trinh durge gidi thigu nhu & dudi: Ce thong 86 cita robot Scara Serpent duge nhap vao khéi théng s6 cua robot hur Hinh 4.5. 33meta om nem: om lal 02k entero ean tengin [O15 moa «anc ooo onan s92) mosevaea (ape em): om tem ais sum: 15 269m ome ‘mss oars Min 8 mmo Hc ansrgam tn erent 92 aed ab wea onan th 2) me 2 anc) oot [Eee] [Eneumssenes) — [Stentneant>] — [seen> ] Hinh 4.5: Thong s6 cua m6 hinh robot Scara Serpent. Khi ta nhén vao nit DOng hoe thugn: Chuong trinh tinh vi tri diém téc dong cudi ciia robot khi bist gid tri gée quay cae khép nhu Hinh 4.6 ed DONG HOC THUAN ROBOT SCARA SERPENT Nhap vao gia ui goe quay cac khop (do): Vii diam tac dong cuoi cua robot (mm) = Theta (do) [2 Theta 2 (do) -< Return MainMen > Hinh 44 : Dong hoc thuan robot Scara Serpent. Nhép gid tri gée quay céc bién khop 1 va 2 véi gidi han gée quay nhw sau: 340; = -96° + 96° (so véi truc Ox); 02 = -115 ° + 115° (so véi true thanh 1), Chung trinh s& bao Ibi néu gid tri nhip vio khng nim trong gidi han eée quay: DONG HOC THUAN ROBOT SCARA SERPENT e Hay shap titel 95 tal 95) ea Toeta 2 (do): | 4.7: Dang hoe thuan robot Scara Serpent (bo 16). Khi ta nhin vio nit Dong hge nguge: Chuong trinh tinh gée quay ciia cée khop khi biét vi ti toa dQ ciia tay mAy robot nhur Hinh 4.8, ere siete Gee guy eae cae obo a al= —eD0P9 (do) = 10465 (snd) 20 fehd= — ep74 (a) = 150A (rad) 02 nts 67700 (am) = 19801 (md) Hinh 4.8: Dong hoc nguge robot Scara Serpent. 55‘Tay robot duge thiét ké chuyén dong theo mét quy dao xuat phat tir vi tri ban dau 06 tog do diém dau (xp, yp) dén vi tri cudi c6 toa dd diém cudi (x, y) vai thdi gian chuyén ‘dong [a ty (Hinh 4.2). Céc toa dé ban diu va két thie phai thoa man digu kign (4.1) va gid tri dinh vi N, phai thoa man digu kign (4.2). va X7+¥? <(0.4). a) Gia tri dinh vi: N= 1. (42) ‘Chuong trinh sé béo 181 néu gia tri (X,Y, N,) nh§p vao khong nim trong gidi Hinh 4.9: Dong hoc nguge robot Seara Serpent (bao 15i). 56hi ta nhan vao nit M6 phong: M6 hinh sé hign ra nhu Hin 4.10. Hinh 4.10: So 43 khdi mé hinh chudn robot Scara Serpent. Khi ta nhdn vao nit Hién thi: Dua ra bang théng sé ciia robot nhu Hinh 4.11, Hinh 4.11; Bang thong s6 cua robot Scara Serpent, 3744, Két qua mé phong hg thong trén Matlab-Simulink 4.4.1. Théng s6 cia robot va quy dgo chuyén dong Cée thong s6 danh dinh cia robot duge sit dung dé thiét ké, tinh todn va mo phong nhur sau: Bang 4.1 thong sé cila tay may. aad 1(m) I(m) | mékg) | J (kgm’) Khau 1 025 0.25 4 0.021 Khiu 2 0.15; 0.15 us| 0.0034 Khau 4 0 0 06 0,001 Robot duge thiét ké chuyén dong theo mot qu¥ dao ban dau c6 toa dd (Xo, Yo) dén vi tri cudi (x, ye) trong thoi gian chuyén dong ty véi ty la thoi gian gia t6e (hode giiim t6c). Tai dinh mite: m , = 2(kg ) va mémen quan tinh ctta tai J, = 0.0004 (ke.m’). Dap tmg nhanh: te= 1.2(9), &= 5(s). Dip img cham: t= 4(9),% 5(s), Bang 4.2: Cac tham sb dt. xm) [o [xm [02s yom) [03 [yam [o 44.2. Die tinh etia hg théng khi robot lim vige véi tai khite nhaw Trong phan nay ding 66 tham sé [Ks, Kj, Ky] cia 66 digu khién PID trong Matlab- Simulink la: Khép 1: PID! =[100, 0, 15] Khép 2: PID2 = [95, 0, 14] Khép 4: PIDS = [90, 0, 12] 58a. Khdo sat hg thong trong trudng hyp thoi gian dit t, nho, dap ing nhanh (ty = 1.266), t, = 0.15(8)) > Dac tinh ctia hg thong khi lim vige khing tai: uy coo dat 2S. z 2 g 05 a e g4 05|{— au daokhop 1 I ‘uy dao knop 2) uy dao khop 4) 0 02 0408 08 1 12 Time) Hinh 4.12: Quy dao dat cde khép hi kam vige khdng tai uy dao thue Goe quay eae khop (tad) ay 620 khop 4 02 ca 08 08 Time(s) 12 Hinh 4.13: Quy dao thye cdc khép khi Lim vie khong ta. 39‘Sai so goe quay ons; oe| oars) a po E 0.005 ' 4 0.008 + ee || el sokKop 1 oo) tei sokop2 1 | feiso kop 4 aaa 02 O4 06 08 Tine 6) Hinh 4.14: Sai s6 vé goc quay cac khép khi lam vige khong tai. Van oe ape at 05; o| 7 § 34 & Fee van toc khop 4) 4s 2 020408 08+ 42 rime (8) Hinh 4.15: Vin te g6e d8t ede khdp hi lim 60