3. Tinh thé nang Viée tinh thé nang cua bé théng cting xudt phat tr tinh he nang ee timg khau va cua titng motor: =YW, +8) (3.20) oT Gia thiét cdc khau ran tuyét doi va luc duy nhat gay nén thé nang Li trong luc. khi dé thé nang cua cé¢ Khau duge tinh bai cong thie u,=-f el plat - om elp trong dé, g, 1A vector gia We trong Irudng ong hé eo so, nghiia La: 7 duoc dat theo phous thine ding 0,0, 2) néu wr Bo Thé nang cia motor: Um, OP, Thay 2 thanh phan trén vaio (3.10), dude cong thutc cudi ciing cua the nang: . (dF Dm, BoP HM ia ang thé nang tinh thong qua p, va p,, chi phy thude vio bien khép q ma khong phy thuge cic van toe khép ¢. 3.1.4, Phan tich § nghia co hoc cua mo hinh Lagrange Thay (3.9) va 3.11), vao (3.1, ta Linh duce Lagrange cila hé thong Mg) = THg.g)-Uty) =3ED8, (ig, + Sem COP tg) ty GPa Gl) AT Dé xay dung ma hinh déng hre dang (3.2). ta phi tinh cac duo hain: d ah od a db). —(— Agu, + 4 a ay at Op, y (a = at » fa ah, ) =A ad “ OG, Tee) =s p 2 StoD Py mM, Bo @ =P REQ) + my BEAL UD £ 4g) i Thay vav (3.2). nhao duce phuong trinh chuyén dong: Yo wd, +E DAs, FRIAS, i nh (313) Ab, 1 Ob OG, 2 ey, trong dé: A, ~ Tu (3.13) 66 thé rot ra cdc nhan xet sau: - Hé s6 4, bigu thi momen quan tinh tai true cla khép thi #, dai dién cho anh hudng cia gia te cia kheép j den khop i. - Thanh phan fi, khép j. Vi Gh,/ ay, = 0 nén h, biéu thi thanh phan ly tam déi vdi truc f cua van Ide hy =O. - Thanh phn b,, ,¢, biéu thi higu tmg Coriolis cua khép j va k doi v6i kh6p i - Thanh phan yg, bicu thi momen d6i véi khap do gia we trong truting. Dé cho day du hon, trong phuong tinh chuyén dong (3.13) can phai ¢6 mat momen ma sat va lye tong tac cha phdn cong tac dén déi tueng. 3.2. PHUONG PHAP NEWTON-EULER 3.2.1. M6 hinh dong Ine hoc Vai phucing phip Lagrange, mo hinh dong luc hoc cla lay may xuat phat tir tong nang luong (/grange) cla hé thdéng. Phuong phép Newron- Eder xy dung mé inh dua vio su cin bing cia hé luc tic dung fén he thong. N6 hinh thanh hé phuong trinh. cé thé giai bang thuat ton dé quy (recursive). Hinh 3.4 18 so do tinh dong luc hoc theo phuong phap Newton -Fuler.t t mat 4 Truc khop é Truc khdp eet Hinh 3. 4: Ser dé dong hoc dan dén cong thic Newton-Ewler Gia sir khau thir i cla tay may co kém motor dan dong khép thi i + / V6i cae thong s6 két cau sau m,- khoi long cita khau thet i, 7 tensor quan tinh cla khau tht /, J,- Momen quan tinh cla rotor, Toace Vector tir g6 ctia i-/ dén trong tam ©, To Vector tir gc ctla / dén trong tam C,, rp - Vector tit gée i-/ dén g6e i Cée van tc va gia toc duge dua vao tinh toan, gom c6: Por Van toc dai cua trong tam C,, DP - van toc dai cia géc toa dé i, @- van t6c gde cla khau /, @,- Van t6c géc cua rotor truc i, p>, gia te dai cita trong tam C. P a) gia toc dai cia géc toa do i, or gia tc géc cia trong tam C;, @ - gia t6c géc cita khau i, @ »~ gia te gc chia rotor, Q)- gia t6c trong trudng,Cie loai Lue va momen tic dung, gdm: f- dic cla Khau/ tie dung bee Khaw ef, Jig Vue efia khau é+ 7 tae dung fén khau i, #- momen cua khau / tac dig lén khau ¢-/. tinh theo true Ef. #4 -momen eta khau /:/ té dung len khau /, tinh theo true é. Chuycn dong tinh tich ciia trong tim dude mo ta bang cong thite Newton (3. fy ne thie Aider duce ding cho cheyen dng quay cla khau, long dé «© momen duoc Unh dr véi toa do Gong tim va troug hue mg, khong én momen, vi nd duge dat ngay tai trong tam: a i ee ea eZ) (3193 dt Dao ham cite thimh pivin thé nhai Ove phai: "ha yetea,smxtla), (8 16) dt Axo ham cua thank phaa the hai: J Fata PS da baer Sur + Gb 1, *2, (3.17) at Thay (3.16) ie fixe 1 (3.17) vao (3.15). nhan duge cong thite Furler: pee why oA ee 1d 4 0, «(10,) HRW Mata the Gorbi®. XZ IS) fafe tong quat co thé tim duoe bang céch chieu tye { (d6i vei khop truct) hode momen gt (déi véi khdép quay) lén true khép, cong thém ret quan tinh cua rotor: +h til +h D0) voi Kheip nian . END (3.19) vt khiip quay . Tinh gia toc cua khau cong thie Newtdn (3.14) va Kuler ( ie dil va gia We gée ella cde kha 18) déu chia thanb phan gia va cde khdp. Tuong tw ner tinh van tc trong chuong 2, ta tinh riéng cho trong. hop khop uuot va khop quay.Tinh gia toc dai - Déi voi khop truot, ky higu p,,, 2, lan lot 1a vector vi tri cia khop tf va khép /, r,), fa khoang cach giita 2 truc cia chiing, «, I khoding dich chuyén theo khép /, ta €6: Path, teeny.) Dao ham van téc_, theo thoi gian, duve: p. +dz,+d0, 2,5 %r tex dz, FO KO XK yd Pos 1d , ' ie WS AZ, ; ng Ey ads, +e, or. vie phuone tink ten duge: Thay * B= P., +42, , 4+ 2d. xz. +@.er .. be xfer} (3.20) - Doi voi khop quay: Pee Bo POH, Dao ham van t6e 7, theo thi gian. duro P= PR tO «xr Fen ef xr) (3.20) Téng hop lai. ta c6 cong thie tinh gia te poe eta khau thet / nhu sau: . Bas daa 42d,0,*%2,,40,x6 40, «69, x) eso, By +, *r FO, x1, ~r |) (2) tngng hop (1) dung cho khdp truot, muéng hgp (2) cho khép quay: Tinh gia toc goc - Déi vdi khdp trugt, vi @, = @,, nén: 0, =0,, (3.23) - O6i vai khép quay. view, = @ +92, nen 0,20, +92, ,4+80,*2,, (3.29) Téng hop lai, ta cé cong thie tinh gia t6c géc cba khau thit/ oh sau @.= Jou vii khetp iret aos 6, +82., +90 : voi khosp quay 7 55, Ghuong 4 CO SO DIEU KHIEN ROBOT Beng ho vi dng lee hoc tay may phuc vu vice phan tich ket eau ctta lay may. dong thdi cling dat ncn mong cho thist ké tay may. Mat khac. moi quan hé i thé eta phan cong tac trong ving , van toe, gia téc) cha cde khau, khép véi céc thong 96 déng luc hoc cita chting (momen tai cde khdp, dong ning va thé nang cia cac khau,.... cing rat cain thiét cho vige thict ke bo phin phat dong. Ve co ban. cic adi dung tren méi dé cap ti phan tery mid hoat dong cla nd. gia ede théng sé dong hoe (t Ni mot citch don pia, RBCN Lh mot tay may duoc digu kbién tr dong theo ebueng wink. No gém doi tong digu khién (phan tay may) va hé thong diéu khign. Nhi¢m vu cla bé thong diéu khién 1a diéu khign tay may thuc hién cdc nbiem vy dat re, nghia JA phan cong tic phai chuyén dong theo gus va the hin cae chite ning cong tac Nehién ctfu vé dieu khign robot dong cham téi "ie vain dé au - Quan hé gilt qu¥ dao hoat dong cia phin cong tac vai Ong hoe, déng lue hoe cita ay may, ‘dc thong s6 ~ Luat, phuong phap diéu khién va cau tre cua hé diéu khién. - Cde co cai cla hé thong diéu khién nhw: co cau phat dong, cam bién, be dieu khién.... cling cde co cau chuyén ddi va truyén tin higu gitta cht Cae vin dé wen lién quan dén nhiév ngdinh k¥ duat khée nhau: co khi, tuyén dong dign, diéu khién su dong, din uf, cong aghé thong tin... ma ranh gidi gitta ching ngay cing khé phan dinh. Trong chuong nay, ching quyét ohitag van dé shién vé cer Ah? trong diéu khién robot. 4.1. THIET KE QUY DAO Quy dao chuyén dong cia phan cong téc!"' 1a vain dé chung trong diéu khién robot. vi hoan than nhigm vy cu thé cba minh thi tude het phan “UE dom gidn, khi ndi dén quy dao, xin duge bd cym tir "phan cong tac” 36«¢ dinh, Noi u 16 co ban dé mo a hoat dong cua robot. Vic thié cung cap dit ligu dau vae cho he didu khién né! c diéu khién cong tae phai di chuyén thee dung quy dao > he khae, quy ke quy dao cling 1 co so tric tiép dao ia cho vi Trong tai Hidu nay. sin tam phan bret 2 thuat ngit sau: ~ Ping dich chuven (Path), 1a qué ch cha ede diém trong khong gian. Vi vay. nd chia dumy ede you 16 hinh hoc thudn ty. ~ Ong dao. chuyen dong, goi tt Ta qué dao (Trajectory) bao ham ca yéu 16 hink hoe ctia duemy dich chuyén Van yéu 16 thoi gian. ohur van te, gia tc. Vi vay hai toan thiét ke qué dgo lien ket céc an dé dong hoe vi dong lye hoc. Che yu 16 dd rae cha bai tod thiét ké quy dao g6m duténg dich chuvén va cde diéu kién ving bade ve dong hee va dong luc hi Pir ra cua bai todn 1A guy dae cia phan cong tic. Ndi chung, mé 1a chinh xde dudng dich chuyén 1a rat khd khan. Neudi 1a gidm bét cic tham 46 mo t& bang cach quy dinh cdc diém bién cila ving hoat dong, thém cae diem trang gian mA dutmg phai di qua, xfip xi (di suy) bing cae dutmg don giin. Tuong iy nb vay, yéu 16 thai gian cla qué dao khong thé xac dinh duoc cho timg diém. ma thudng quy dinh cho cd doan duimg. Chiing citing thutmg dude quy dinh bing cdc gid tri gidi han nhu van téc hay gia t6¢ cho phép, hoac gan cdc gid tri mac dinh. Rai todn thict ké quy dao duge dat ra ca trong khong gian khdp in ving cia quy dao, nhé chuyén, thudng duge mé ta trong viing hoat dong. Nguoc lai. Iue chuyén dong cia hé théng xuat phat tit cic khép nén vide xd dinh quy luat theo thdi gian cla cdc bién khép duge thuc hién trong khong gian khép. 4.1.1.9 in khop. Chuyén dong cia tay mdy thudng due mé ti Wong ving lam vice hoat dong, Cc diéu kign ring bude 8 cba duéme dich ¥ dao trong khong hen hang cde didn mit (gém diém dau. dt diém trung gian) va indi gian chuyén dong. Vi vay, dé thiét ke quy dao trong khong gian khdp phiti gidi bai tain dong hoc nguge dé xdc dinh gid tri cde bién Khép tai cic diém nét. Sau dé. thiét lap cdc ham Adi suy cudi cla quy dao, cé mot qty dé mé ta qué dao vira nhan duge. STThuat todn thiét ké quy dao trong khong gian khép phai dat cde diéu kién sau: ~ Khong doi héi tinh to’n quit nhiéi - Vi tri, van te va c6 thé ca gia t6e cla caéc khép phai duoc biéu dién bang céc ham lién we; - Giam thiéu céc higu tng bat Ioi, vi du quy dao khéng trom tru. Dang don gian cha quy dao la chuyén dong diém-diém (Point-te-point Motion). Néu them cac diém trung gian thi ket qua nay cd thé duge khai quat len thinh chuyén dong theo duong (Path Motion). Khong lam giam tinh téng quat, ching ta xét bai toan vai mot bién khop q(t). 4.1.1.1, Chuyén dong diém-diém Chuyén dong diém - diém ting dung cho mdt sé loai robot, ahr robot han diém, tin dinh, xép dé vat 1 Trong dang chuyén dong nay, ngudi ta chi quan tam dén cdc toa do diém dau, diém cudi cia duimg dich chuyén va thdi gian chuyén dong giita ching chi khong quan tam dén dang hink hoc cua dung dich chuyén. Nhiém vu duvic dat ra la xde dink qu§ dao chuyén dong thod man cdc yéu cau chung va cé thé them ca viéc cdi thign mot sé thong so cla quy dao. Gia str / 1a momen quan tinh cia mdt vat rin quanh truc quay cua nd. Cfin xde diah quy luat thay déi cia gdc g pitta gia tri dau g, va gia wi cudi q, trong khong thi gian ,. Luc phat dong 1a momen r tit mét motor. Bai todn ré rang la cé nhidu [di gidi. Ta thém tiéu chudn téi wu, vi du nang lugng tiéu thu én motor 1a nhé nhat. Tir d6, co mot tap hop gid ut g = @ a Ii gidi cba phuong tinh vi phan: la=t * thoa man diéu kién: “ fortiar=9, —4, j sao cho: 4 Jr (uidt > min a LOi gidi téng quat sé c6 dang da thee bac hai! ON) = at + biteVi vay, quy dao chuyén dong sé c6 dang da thite bac ba: sal ras + art ay (4.2) van téc thay déi theo guy luat bac hai: g(t) = Bae + ay +a, va gia t6c thay déi theo quy luat bac nhat ft) =6a,1+ 2a, De xie dinh 4 he s6, cén cho trude 4 diéu kién. Ching thuémg la vi ti dau va vi tri cudi g,, qq van Ie dau va van tc cudi g,, g» Thudmg chon 9, = 4, = 0. Cudi cing, cée hé s6 ong phuong trinh quy dao chuyén dong (4.2) duge xde dinh tr hé phuong trinh: aly’ + a) + at + dg = q, Buy + Dag + a, = Gy Trén hinh 4.1 1a d6 thi quan hé giifa vi tri, van t6c va gia t6¢ vGi thd: gian chuyén dong (s) cho trudng hop: diém (g6c xuat phat) g, =0, diém dich q, = 7, thoi gian chuyén dong t= /, van téc tai diém xuat phét va tai diém cudi §,= g,=9. Gidi hé tren véi ede gid tri ban déiu da cho, duoc: y= a, =9 a,=3H,d,=-20 Van toe o6 quy luat bac 2 viii gid tri cue dai: Gnu = 3at2 khi t= 12 Con gia téc bién thién theo guy luat bac nhat voi gid tri cuc dai: Gna = OT tai = Ovat=T Nhuoc diém cita quy Tuat nay 1a gia téc tai diém dau va diém cudi lén, sinh luc va dap do quan tinh. Mot dang quy dao thudng ding tong cong nghiép 1A dang da thie hon hop (Blended Polynominal). Doi véi quy dao dang nay, thugng chon quy luat van tc hinb thang (hinh 4, 2), 59vite: 3 . z = q O85 2 oe EG i he Ge yt ; vontee ‘A ‘ = g ia : | oO G4 oe on aA gia te 20 44 = eo tte y 2, ot t -20 0 07 04 08 Oo tI Hinh 4.1: Do thi viaet. van toc. gia Hinh 4. 2> Out daw voi guy Indt toe vehi spay hudt de thite hie 3 vin tic hinh thang Qué dao cd 3 doan: khdi dong {tang toc) véi gia tec khong ddéi: chuyén dong lisp voi van téc Khong déi: dén dich (gidm tc) voi gia toc khong Adi. Quy dao nhan duge gém 2 doan parahol. n6i nhau bing mot doan thang. Gid thiéi g, = ¢, = 0 VA thdi gian tang téc va thdi gian gidm téc bing nhau (4 c6 gid uj bang nhau & doan dau va doan cudi). Cac diéu kién trén din dén quy dao dot xtmg ve diém giita ¢,, = (q- q,2 tai t,, = 1/2 é dim bio guy dao 14 ham Lign tuc, van téc tai cdc diém tip gidp doan parabol vi doan thang khong duge nhay bac, nghia la: Gu TH ot, Ge (4,3) 60trong d6, q, 1a gid tri ma bien khop g dat 164i tai thoi diém két thic doan parabol 1, vii gia tc §,. Vi g (0) = 0, nen i =y,+—Gae 4.4 4. adele G4) Két hop (4.3) vi (4.4), nhan duge phuong tinh; . GHG Hg, HG a0 (4.5) Neéu cho tude of qd dp Sidi (4.5) trong khoang ¢, <7/2, onan duge: 4.6) Dé (4.6) 66 nghia, phai dim bio diéu kien: dig, —| : la, — 4 ? ’ (4. 7) Neu biéu thie wén nhan dau bing thi khong cé doan nam ngang ciia thi van t6c cé dang tam gidc. cde gid tri cho trude cba q,, q Va fp tit (4.7) tinh dutoe gia We @.. sau dé tinh 1. ahi (4.6), Cudi cling, quy dao duoc xde dinh tir 3 doan: tegen gets, gtti=ag, +q.t.tt-8,/2) 1 SESE, -8,. (4.8) 4 t,t SESE, Cha § ring, ap dung quy ludt van tc hinh thang Khong dam bao diéu kign 16i uu vé nang lugng (4.1) nhu dat duge véi qu? dao bac 3. N6 tang khodng 12,5¢% so véi gid tej 161 uu. 4.1.1.2. Chuyén dong theo duong Trong nhiéu hoat dong, vi du han hé quang, son, xép d@ vat lidu trong khong gian c6 nhiéu vat chuéng ngai,.. robot can due diéu khién theo duémg. Khi do, s6 diém cin xéc dinh tén mdi doan dudmg ién hon 2. D6 khong chi fi nhimg diém ma phan céng tac phai di qua, mA tai 46 c6 thé phai khong ché ca van toc va gia 16e chuyén dong dé dap ting yeu cau 61diém nay nhiéu hay it [A tuy theo yau cau cong nghé. Bai todn dat ra lA xde dinh quy dao qua N diém chét, Nhu vay. bién khép phai thod man N diéu kién rang budc. Dé thuc hien diéu dé, thé nghi dén quy dao dang da thie bac N-/. Tuy nhien, zidi php nay c cae nhuge diém: - Khong thé khéng ché dirae van 16¢ tai diém dau va diém cudi. - Bae etia da thtte cing cao thi kh nang dao dong cing !6n, anh hudag xau dén trang thai cong tic cia robot, - Dé chinh xdc tinh todn cdc hé so cha da thite giam khi bac thitc tang, ta da - Hé phuong trinh rang bude phutc tap v2 khé gidi, - Cac hé s6 cia da thuc phu thudc tat ca cde diem. Vi vay, khi can sip xép lai mi diém thi cling phai tinh toan lai ho&n toan. C6 thé khéc phuc cae huge diém én bang cdch thay thé qu da thie bing mot quy dao "lai", trong dé mot sé doan da thie due thay thé bang cdc doan da thife bac thap hon. Cac da thtic thay thé nay duge got la de tute ndi suv. Dé dam bao tinh lién tue cla van te tai diém chot, bac cla da thie ndi suy kdng dé uhd hon 3. Xé quy luat bién thién theo thdi pian cia mOt bien khép 4/1). Dudng cong bien thien ciia nd pom N-J doan da thite ndi suy bac ba /7ft), voi k= 1...., Nef. Ham g(t) nhan gid trig, tai thdi diém r= t, (A = 1,...N). Tai diém dau ft, = 0), gid wig, = 4). Tai diém cudi (ty = 4). gid tri gy = qp Cac gid tri g, chinh la dai dién cho cde diém chot cita qu¥ dao (hinh 4.3). Quy dao duge thiét ké can phai thoa man cdc diéu kién rang bude nhat dinh, C6 thé xem xét 3 trutmg hop sau: - Gid tri van tc gfr)tai cdc diém chét xc dinh, - Gid wi q¢(}tai cdc diém chét dude tinh theo cac chi tieu nhat dinh, - Dam bao tinh lién tuc cla gia toc Gf? } tai cdc diém chét. Sau day sé trinh bay chi tit hon vé mdi truéng hgp. 62Hinh 4. 3: Quy dao v6i cde diém chot va cde da thite néi suy © Da thie n6i say voi gid tri cho truéc ctia vdn toc tai cde diém chét C6 2 diév kién phai dirge dim bdo: ~ Cée da thie ndi suy phai di qua cdc digm chot, - Vf 1c tai cde diém chor phai bang gid tri dink tude Néu trén quy dao cé N diém choi thi sé da thite bac ba, noi suy (1) ndi lin lugt cdc digm q, va q,,, 18 N-1. Méi da thée phai thoa man céc digu kien sau: Md = ay Dg I = Gans d= 4 Ta) = Gaus Méi da thie néi suy (bac 3) <6 4 hé 86. Ching duoc xéc dinh bang cach gidi céc he phwong trinh dang (4.9). Can gidi N-/ he phuong tinh dé tim N-/ b9 he sé. Thudng gid tri van 1c tai diém dau va tai diém cuéi duge lay bing 0 (4, =4, =@). Tinh lién tuc cha van téc tai céc diém chét duoc dam (4.9) bao bai diéu kién: TE ft = Ty ltyy) wok = 1,..N-2 63Hinh 4.da la bidu dé vi tri, van toc va gia toe voi cdc 86 ligu sau: qy =O. qs Fla qa m2. STL SOS 2bE RES G20. Gem. Gear, @, <0, Tathay,¢ 6 van Voc 1A lien tue, cOn gia te khong Lien wwe. ° z giawe 2a g Bol “a0 a0 0 7 (a) Hinh 4.4: Qu dao vidi da thite duoc xdc dink bang 3 diéu kien khéc uate © Da thite noi suy voi gid tri van toe tinh todn tai cae diém chét Trong, truéng hop nay, gid tri in téc tai cdc diém chét dude tinh tir caéc diéu kién nhat dinh. Bang cach néi cdc diém chot bang cac doan An 16¢ tai cde diém chot duye tinh theo quy tc sau: G,=0 0 khi sentv, 2 sgnty, i, = 4H hs Le, So) bid syne, )= sgneyy)) ete dy =0trong d6, ¥, =(g, ~ quid ~ th) dIgu din dO déc cla dean thane treng khoang thoi gian /i; - ff. Hinh 4. 4b minh hoa truéng hyp n6i tren véi cde sé lieu eu g; = 0, Go 2age = Wg. = t= 00 = 22 8,5, g, 0, g, =0.Ta thay. van ioc lien toi gid Ui 0 t ic diém char, © Da thic noi suy vii gia t6e lién tuc tai cdc diém chot Ca 2 mudng hop ten déu khong dam bao duge tinh lien tue cia gia toc tui cde diém chét, Mudn dam bdo tinh lién tuc cia cd dutmg chuyén dong Jan van toc va gia (6c thi da thie ndi suy gitta 2 diém chot fién nhau phai thoa min cdc diéu kign rang buge sau: Meth d= dy Fy J HID} Thy ttl p= TT fh, { We i= Fa) Ai & phuong trinh nay phai ding thuat gidi dac biét ma khuon kho c6 han cila tai ligu nay khong cho phép trinh bay. dc gia c6 thé tham kido trong ti Liéu [5]. Két qda, nhan dug quy dao tron voi dutmg. chuyén dong, van toc va gia tc lién duc: Muon gidi Hinb 4. 4¢ minh hoa qu¥ dao tinh bang phuong phap noi tren vi cic sé ligu sau: 4, = 0, 4, = 24, q, = W2.q,= 00,5 0,0 = 2.4 9, =9, G, =0. Hai cap diém do khde nhau duoc chon dé so sénh JA h 0,5: 4, = 4,5 (06 thi née lién) vat, = 1.5 v4, + 3.5 (net ditt). Ta thay, dé thi van toc li dudng bac 2. con gia téc 1a duing bac 1. Mat khac, cap diém ao thd hai (luong img vei nét dit) din dén gia toe iGn hon visede, did Trung gian gan shau hen. £ : © Noi suy duong bac nhat bang céc doan parabot Mat trong nhimg dang don gian nhat cha quy dao tay may gém cic dogn thing, néi v6i nhau bang cdc doan parabol tai cdc diém chot. Gid sit ten qu¥ dao c6 N diém chot, ung voi thoi diém 4, tai dé bign khép dat gid tri g, voi k= /,..,N. Quy do nguyen thuy gdm ede doan thang ndi_ voi nhau tai cde diém chét. Dé dam bdo tinh lién tuc cla Man 16, tai cdc diém chot (gay khtic), dudng chuyén dong duoc noi bang cde dogn parabol (hinh 4. 5a). 5+ KTRB is; 65Hinh 4. 5: Noi suy quy dao bac nk bang cdc doan parabol (b) Ky higu At, = t,, - &, 1a khoang théi gian giifa q. va quis Athger 1a khodng thdi gian tuong (mg voi doan thang giifa hai diém q va quis Gux.;!a van tée khéng déi tuong ing voi khoang théi gian At,,,: ¢, la gia te tuong Ung voi doan néi parabo! va khodng thdi gian Af, Gia str gid tri cla cdc dai lung q,, At, . Af, duge cho trudc. Van téc va gia t6c tai cdc diém chét duoc tinh nhu sau: Ge ~ Git ot Gia = Ge T4en 4.12 a (412)Musn cho cdc diém dau va diém cuéi cia quy dao dat gid tri mong muén, nghia 18 g, = g, va gy = gy thi phai kéo dai them thoi gian: ty ot) +(4t, + 47,)/2, déng thoi van t6c tai cde diém dau vi diém cudi Gor =F xes1 =O. Chi ¥ rang quy dao nhan duge khong di qua cdc diém chot ma chi di gin ching. Gia te tai cde cung cong (g,) cang nhé thi quy dao cing di sat cde diém chét hon. Hinh 4.5b minh hoa qu dao véi 4G =. gy = 2m. qe = 2, Ge Ht, =O, =2,n=3,4,=5, Gg, =0, q,=0. Hai gid wi cla atjduoc dang la At) =0,2(nét lien) va At, = 0,6 (nét duit). Ta cing thay trong trudng hop thit hai (gia téc 1én)}, quy dao di xa cdc diém chot hom vi phai tra gid cho gia Wc Jén. 4.1.2. Quy dao trong khong gian cong tic Quy dao trong khéng gian khép mo 1a dign tign theo thai gian clia cc bien khép g(2), sao cho phan cong téc di chuyén thing tir diém ban dau dén diém cu6i hode di qua cdc diém trung gian. Thyc té, khi thiét ké quy dao trong khong gian khép khé c6 thé dam bao chuyén dong chinh xdc cla phan cong tac, vi anh hudng phi tuyén khi chuyén déi cdc quan hé dong hoc tit khéng gian khop sang khong gian cong téc. Muén cho chuyén dong ctia phéin c6ng tc theo ding 19 trinh da dinh trong khong gian cong tac, cén thiét k€ quy dao truc tiép trong chinh khong gian nay. Quy dao cé thé duoc xay dung bing cach noi suy dudng dich chuyén qua cic diém chét hoae xéc lap bang gidi tich ham chuyén ddng. Trong ca hai trudng hgp, dién tien thdi gian cla cdc bién trong khong gian cng tic duoc ding dé x4c dinh gid tri ciia cdc bién khdp theo thoi gian thuc, bang thuat todn nghich cia dong hoc tay may, Vi cdc gid tri nay 18 chudn dau vao ciia hé diéu khién, nén ngudi ta thudng ding phép vi nGi suy dong thang (linear microinterpolation). Bing cdéch d6 06 thé lang tan s6 cap nhat chudin dau vao dé cdi thin dac tinh dong luc hoc cla ‘hé thong. Ky hieu x 1a bién, dang dé miéu ta duong dich chuyén cia phan cong téc trong khong gian cong tac. Dién tign thei gian cia bién 46 6 thé duge chi dinh chinh x4c hon nhé N diém trung gian (diém chét), x4c dinh gid tr ca bién x tai cdc théi diém 4%, véi k = /,...N. Tuong tu nhu da tinh bay & phén ten, quy dao ¢6 thé duge hinh thanh bing céch noi suy 67ham vector gitta cdc diém chét. Ham dé c6 thé duge xde dinh nhis ap dung mot trong nhiing phuong phap noi suy da trinh bay & muc 4.1.1.2. 4.1.2.1. Cde nguyén t6 cia duing dich chuyén . Mot dung dich chuyén trong khong gian 6 thé duge mé t4 dudi dang tham s@ bing tap hop céc phin tr toi gidn (ede nguyen W - path primitives). Gia sit p 1a vector (3x1) va f(a) 1a mot ham vector lién tuc trong khoang /«,, o,/. Xét phuong trinh p=fio) (4.13) Khi o thay déi trong khoang /o, oj thi cdc gid tri tuong ung cla p hinh thanh mot dung trong khong gian. Phuong trinh (4. 13) 1a biéu dién theo tham s6 cia duing /”, trong dé dai yong vo hudng oa tham 56. Khi o tang, diém p di chuyén trén dutmg theo mot hudng nhat dinh. Dang cia F°va hudng ting cita né do ham ffo) quyét dinh. Duong duge 2oi 1a kin néu pio) = plo,). Neu plo) # p(o;) thi c6 duimg hd. ‘Tren mét dudng Feo hudng xdc dinh, cho mot diém p, 6 dinh lam g6c. Ta dinh nghia toa do s cia mot diém bat ky trén “la 46 dai cung tu p, dén p. Moi diém p én /tuong ing vGi mot gia tri toa do s. Vi vay, s 6 thé dugc ding nhu 1a mot tham 6 dé biéu dién duémg dich chuyén 7: pafisi (4. 14} Xé mot duimg (duoc biéu dién theo tham s6 s nhu (4.14). Tai méi diém p trén dé c6 thé xc dinh 3 vector don vi dic tre cho duong do. Hudng cia cdc vector don vi phu thude vao dac tinh cha dudng, con chiéu cia chting phu thude vao chiéu bin thién cia (4.14). Cach xic dinh cdc vector nhu sau: - Vector thit nhat, ky higu 1a 1, 18 vector tiép tuyén voi Flai p, hudng theo chiéu tang cla s. ~ Vector thit hai 14 vector phép tuyén chinh, ky higu 1a n. Né vuéng g6c véi vector f tai p va nim wong mat phdng mat tiép voi D tai p (mat phang O trén hinh 4. 6. Dé La vi tri gidi han cla mat phdng chita vector tigp tuyén # va diém p’ € I. khi p’ tién doc theo F dén p. Chiéu cha vector 2 duge x4c dinh sao cho trong ving Jan can cita p, dung nim cing phia vdi 1 so vdi mat phang chifa t vA 7.Hinh 4.6: Biéu dién ditong dich chuyén duci dang tham so - Vector thi ba, ky hiéu 1a 4, binh thanh véi ¢ va 2 mot tam dién thudn, aghia 14 chiéu cua (t, n, B) duge xdc dinh theo guy tdc ban tay phai. Theo dinh nghia vé toa dO s cla diém p trén dudng dich chuyén J) ta 6 cdc quan hé sau: (4.15) Sau day la hai dang nguyén 16 dién hinh, thutng ding nhat khi xay dung qu¥ dao wong khong gian céng tac. © Doan thdng trong khong gian cong tac Xé doan thang ndi hai diém p, va p, N6 duge biéu dién dudi dang tham s6 béi phuong trinh sau: rede ag Py (4. 16) pO Py Chit ¥ rang p(0) = p, va. pip, — p,|))=p,- Vi vay hung cia Pai tt P, den p,. 69Vi phan (4.16) theo » duce: (4.17) Trong trvdng hop nay khéng thé xdc dinh hé toa dO (1, 4. b) mot cach don tri. © Duong tron trong khéng gian céng téc Gia sit c6 duémg tron tong khong gian (hinh 4. 7), duac xde dinh boi: - Vector don vi 7 nam theo truc dudng tron, + Vector vj tri d cla mot diém trén truc dutng tron, ~ Vector vi tri p, cla mot diém nam trén dudng tron. Hinh 4.7: Biéu dién duong tron didi dang tham so Ky hieu 6 = p,- d. Néu p, khong nim wen truc, nghia 1a dudng tron khong suy bign thanh mot diém, thi phai cé quan hé: |o" 7} < (al Khi d6 c6 thé xde dinh tam cia dudmg tron thong qua vector sau: c d+(Srjr (4. 18) 70Cén biéu dién dudmg tron duéi dang him ciia toa dé s. Dé cho ham nay don gidn, cén chon mét hé toa d6 thich hop O'-x'y'x’, wong 46 O' tring voi tam dudng won: truc x’ hudéng theo chiéu vector p, - ¢; truc 2’ huéng theo 7, con y’ duge xéc dinh tir x’ va 2’ bang quy tac ban tay phdi. Toa do diém p trong hé toa do nay duge biéu dién qua vector pcos(s! p) Pisi=| psin(s!p} (4.19) 0 tiong dé p=||p, — el 1a ban kinh dudng tron va diém p, 1 géc toa do. Khi thay déi hé toa d6 thi phuong trinh biéu dién dudmg tron tro thanh ps) = ¢ + Rp{s) (4.20) trong d6 R 1a ma tran quay cla hé toa dé O’-x'y'z’ so véi hé O-xyz. Trong chuong 2, chiing ta da biéu dién ma tran nay dudi dang Ra ix’ yz’). Vi phan (4.20) theo s ta nhan duge cac vector van tdc va gia téc dudi dang ham sé cua toa d6 s nhu sau: 4 — sin(s/ p) Per cos(sf p) ds a “ —cos(s! p)l p =R\ ~sin(s! p)ip 4.21) a 0 4.1.2.2. Vi tri va hung trén quy dao Qu¥ dao trong khong gian cong tdc 1a t6 hop theo tinh tr nhat dinh cua cdc nguyén t6 da tinh bay 6 tren. Thiét ké quy dao trong khong gian cong tdc od nghia 1a xdc dinh ham x(z), biéu dién thé cla phan cong tdc x theo thdi gian #, trong d6 ¢ bién thién trong khoang tir ¢; dén % Thé cla mot vat trong khong gian cé 2 yéu t6: vi i va hudng. Khi vat chuyén dong doc theo dudng dich chuyén, khong chi vj uf ma cd huéng cia né thay déi theo thdi gian. Vi vay. "qu¥ dao" & day phai hiéu theo nghia rong, g6m ca thanh phan mo ta vi tri va thanh phén mo ta hudng. Trong 71chuong 2 chung ta da biéu dién thé cha phan cone tac qua-vi tri va hung cla nd nu sau: « A | ‘ Bai todn duoe tach thanh 2 phan: bigw dién’W rf va hung ci phan cong tac © Vi wi cua phan cong téc Cho p = fis) lA vector 3 x 1, biéu dién dudng dich chuyén Fdudi dang ham cia toa dé y, Ge toa dé cua phan cong técidi chuvén ti diém.p, dén diém p, tong khodng thai gian 1, Dé dan gidn, dat gde toa dé tai diém p,,. hung cia Mai tcp, dén p,. ¥ tren f(chinh Li do dai cung » gitia g6c p, dén ? Ja ham cli thoi gian 4, nghia IA: y = (2). Vi p = fish ta cé thé tinh van té& di Tuyen cba aiém puén duong F bing cach lay dao ham ctia p theo 't: ap a g OP x ay , ds trong 6 11a vector Hép tuvén cua dutie cone‘ dién do ldn cha vector van te tal p. Gia tri oda i's Nhe vay’ $ biéu in thién wr O (hi = &) bign thién theo quy lat parabol hoac Ninh thaiig; td theo ehiing ta str dung phép noi suy bac 3 hay bac nhat. va ed lai bang O khit = 2. Néu dutng dich chuyén 1A doan thang, duoc biéu dién boi (4.16) thi van tc va gia téc cla diém Pp duce tinh nhw sau vies bent? Ops Ip, “el Hones ci Néu dudng cong f 1a autme tron, Ta » gia te! : tly 1 ops sii('S/ p) pe i scalts 7 pil Le 7?cos(s! pil p ~ Ssin(s! p) p=R—S# sin(s! p)l p+ Scos(s! p) (4, 23) 6 Chi ¥ rang vector van téc huéng doc theo vector 1, cin gia téc cb 2 thanh phan; thanh phan Auéng tam theo phuong x, thanh phin tiép never theo phuong f. « Huéng cia phan cing téc Hudng cia phan cong tac, nhu da nghitn cifu tong chuong 2, duoc mo ta thang qua ma tran quity cla hé toa dO gan trén nd so véi hé toa dé co ai tovin guy dao chi phai quan tam ring ma tran quay nay 1a ham cita thoi gian. Chting ta cling di biét, ma tran quay c6 3 cét, tuong img vi 3 vector don vi (7, s, a) ca hé toa dO hién thdi. Tuy nhién img dyng truc ti¢p ma tran quay dé mo 1a huéng khéng c6 loi, vi phdi gidi quyét céc litn ket thita. C6 tutmg hop, vi du khi ngi suy dumg thang, céc vector dem vin. s, a khéng thé dam bao tinh vudng géc cia ching tai moi théi diém. Dé khéc phuc khé khan trén, nguéi ta ding phép m6 ta ti thiéu cia hudng bang cic bo ba géc ¢ = (@, 9 y) nhu géc Euler ZYZ hoac géc RPY. Khi phan cong téc chuyén déng doc theo dutmg dich chuyén, hé toa do @thay déi tir gid tri dén g, Tuong tu nhur doi vdi vi tri, c thé ding phép ndi suy bac 3 hofc bac nhat dé m6 ta hudng cia qu¥ dao. Khi 6, nhu da chi ra trong myc 4.1.1.2, van te géc « (c6'duan hé bac nhdt vei p) sé 1a ham lién tue doi véi thoi gian. Vi vay. voi @ va ¢ cho trudc, ta cling xdc dinh ditge hung, van’ tc goe va gia {6c thay déi hudng cua phan cong tac nhu sau: sé Trong bat yf) — [bad “al » (4.24) 73Mot phuong phap nia dé xay dug quy dao vé hudng la van dung phép quay quanh tyuc bat ky, di duoc trinh bay trong chuong 2. Néu cho trudéc 2 hé toa d6 dé cdc 6 géc tring nhau thi ludn ludn cé thé xéc dinh vector don vi r sao cho nhan duoc hé thif hai bing cach quay hé thi nhat mot g6c Yquanh true 7 Kg higu R, va 2, 1a hai ma wan quay cia hé & vj tri ban déu Oxy, va 6 vj tri cudi O-xpiz). Ching ta da c6 quan he R, = RR, tong dé Ne Me Ta R= RR, = hy hy Py Pop Fas Néu ding ma tran R(t) dé mo ta phép quay tir R, dén R, thi phai c6 RYO) = Tva Rit) = Ri. Nhu vay cé thé coi R14 phép quay quanh truc c6 dinh trong khéng gian. Géc quay 9 va vector don vi r ea truc quay cé thé tinh theo cdc céng thitc sau: Ty tte ths 8, =cos'¢ 5 } p [ert "= Sing, [tect véi sind, #0 (4.25) Pa Ma wan R(1) c6 thé duge viet thanh R//.9(7)) va tinh nho cong thitc (2.16). Néu xéc dinh dugc quy luat 52), v6i HO) = 0 va Ht) = 3, va tinh duge cdc thanh phan r,, r,, r, cla vector don vj cla truc c6 dinh thi cé thé xdc dinh ma tran quay tai théi diém ¢ bat ky: R(t)= RR (Ht) Ma tran trén mo td hudng cia phén cong tdc so v6i hé co sé duéi dang ham ctia thoi gian 7. Sau khi dudng dich chuyén duge xdc dinh trong khéng gian cong tac nhé céc ham p(t) va f(t) hodc R(t) thi, bang thuat todn déng hoc nguoc, 6 thé thiét lap quy dao tuong ting trong khong gian khop (1). 4.2. DIEU KHIEN CHUYEN BONG Sau khi nhan duge sé liu dau vao twong ting véi quy dao cia phéin céng tac hay cia khép, hé théng diéu khién phdi diéu khién robot chuyén 74dong theo ding quy daé dat ra. Van dé diéu khién robot noi chung rat phiic tap, vi ngoai viéc dim bao thuc hign quy dyo mot céch chinh xac, con phai gidi quyét vain dé (wong tac v6i doi tugng-cong tac. Tuy theo yeu céu sir dung robot, 6 rat nhiéu k¥ thuat diéu khién khac nhau duge img dung. chang han: - Diéu khién ty do va diéu khién c6 trong tac v6i doi tugng, ~ Digu khién trong khong gian khop va diéu khién trong khong gian lam viéc, » Diéu khién phan tin va diéu khién tap trong, - Diéu khign diém - diém va digu khién theo dudmg. Ngoai ra, k¥ thuat diéu khidn cén phy thude két cau phan co khi cha robot, vi du sit dung hé toa d9 dé - cdc hay hé toa do khdc, sit dung dong co chap hanh kiéu nso, cé ding wuyén dong co khi phu hay khong. ‘Truyén dong co khi phy cho phép str dung ving ¢6 loi nhat én dac tuyén cilia dong co, mé rong duge ving tuyén tinh ciia né,... nhumg lai chiu cdc téc dong phu, nhw bién dang, tén hao nang lugng do ma sat, khe ha, cde luc va momen phat sinh, nhu luc ly tam, dao dng, lye corioli Su lua chon gitfa diéu khién trong khong gian cong téc hay trong khong gian khép cing 1a van dé khd. Nhiém vu cia robot (tic 1a cla phan cong t4c} duge thiét lap trong khong gian cong tac, trong khi tac dong diéu khign lai dat vao céc khép nén big khép 1a déi tugng diéu khién truc tigp. Vi vay bai toan déng hoc nguoc bao gid cfing phai duge gidi, nlumg vj tf ciia né khéc nhau gida trudng hop diéu khién trong khong gian khdp va diéu khién trong khong gian cong tic. Khi diéu khien trong khong gian khép. bai toan dong hoc nguge duge giai trude dé chuyén cdc thong s6 tit khong gian cong tée sang khong gian khdp (hinh 4. 8). SRE Bogs ame Lofmarne fofownn Fo] wen FD > + 9 crew Hinh 4, 8: Su dé diéu khién wong khong gian khop 75