^•- - s t 4i5} ^ KS^" N G H I E N c u t ] , I T N G D p N G THU4>T TOAN T H I C H N G H I DIEU KHIEN CHUYEN D O N G R O B O T T R O N G K H O N G GIAN K H P RESEACH, APPLY ADAPTIVE A L G O R I T H M T O C O N T R O L R O B O T M O V E M E N T IN J O I N T SPACE Dao IVlinh Tudn', Nguyen Viet Ngir' Tran Due Thuan^, Nguyen Hiiii Thung^ Trudng Dgi Hgc Su Phgm Ky Thudt Hung Yin Vien Khoa Hoc vd Cdng Nghi Qudn Su Hgc Vien Ky Thudt Qudn Su Tom tdt: De ndng cao chdt lugng chuyen ddng eua robot cdng nghiip cae bd dieu khiin sir dung cdc thudt todn dieu khiin nhu dieu khien trugt, PID bii trudng, tuyen tinh hda ehinh xdc Tuy nhiin dieu kien ldm viec cua robot edng nghiep cdc tham sd ddng hgc thay ddi vi du nhu trgng lugng cua vdt cdnh tay robot gdp md men qudn tinh thay ddi ldm dnh hudng din chuyin ddng cua robot De hgn ehi vd gidm dnh hudng dd den ehuyen ddng cua robot, su dung thudt todn diiu khiin thich nghi vdi mue tieu thich nghi theo su thay ddi cdc thdng sd ddng hgc Tir khda: Diiu khiin robot, dieu khien thich nghi, dieu khien he ihdng ed tham sd khdng dimg I Xay d u n g t h u a t todn dieu khien thich nghi Xuat phat tir phuong trinh dpng hgc ciia Robot, ta xac dinh duac momen hoac luc cdn thiet dat len cac khdp cua Robot la M M = H ( Q ) Q + V ( Q , Q ) + G(Q) (i.l) Khi ddt mo men M vao cac khdp, thi cac khdp s6 quay hoac tinh tien mdt luong la Q; VdiT=[T,;T,; _TJ;Q=[q,;q,; q„] Trong do, n la sd bae tu eua robot M = H(Q)Q- r H ( Q ) + S(Q,Q) C(0,Q) G(Q) = Y ( Q , Q , Q , Q ) P Y (Q, Q Q Q) P gpi la ma tran hoi quy P bao gdm cac phan tu: P = [m,; m^; I,; IJ Ta CO luat cap nhap de tinh p la: P-r"'Y'^(v,v,Q,Q)r Khi dd gia tdc cua goc quay cac khdp la: Q = H " ' * [ T - V - G ] (1.2) Trong do, H, V, G la cdc phan quan tinh, tucmg h6 va trgng trudng thu duoc tir dgng hgc ciia robot T la mo men lay tu bp dieu khien Khi robot lam viec lam goc quay cua cac khdp va van tde gde va qudn tinh cac khdp thay doi Gia su cac thong s6 dgng hgc ciia he thong khdng biit ehinh xdc la m^, I^ ta udc luong mot lugng ciia m , I nen cae mo men hodc luc can thiet dua vao he thdng eac khdp de dieu khien Robot vdi luat dilu khien de xuat la: M = H ( Q ) ; + C ( Q , Q ) v + G(Q) + Kor (1.3) va tu ta tinh dupc tl Tn |AH-H(Q)-H(Q) Dat • j A C - C ( Q , Q ) - C ( Q , Q ) IAG = G- G thay vao phuong trinh (I) ta cd: (H + AH)v + (C + AC)v + (G + AG) + Kcr = HQ + CQ + G(Q) H(V - Q) + C(v - Q) + AH.v + AC.v + AG = - K o r Y(v,v,Q.QlAP fv = q d - L ( q d - q ) Trongdd: •jv = q d - L ( q d - q ) j- (14) Suy ra' H r - C r + Y(V,V,Q,Q)AP = lr = (qd-q)+L(qd-q)J Vdi: q la cae gia tri ddt ciia cdc khdp L la ma tran dudng cheo xae dinh duang Va ta phai dua vao ludt ude lugng tu phucmg trinh ddng hgc (1.5) Chirng minh t h u a t to^n thicb nghi Chgn ham Lyapunov sau: r p H ( Q ) r + AP'rAp] Khoa hoc&C6ngnghe-S6 02/2014 (1.6) 51 Trong F la ma Iran dudng cheo xdc dinh ducmg cap n Ma theo dinh luat cdp nhap thi: P-r"'Y^(v,v,Q,Q)r bot RRT nhu sau: Tu phuang trinh ddng luc hpc cua robot: M = H ( Q ) Q + V ( Q , Q ) + G(Q) (2.1) Trong do, H la phdn quan tinh ciia robot Ta ling dung thuat toan dilu khien thich nghi de dieu khien Chuyin dgng cho mgt cdnh tay robot cu the la robot RRT Vdi so cdc khdp chuyen ddng ciia robot dugc xdy dung nhu binh ve: maAP = P - P - A P ^ P Suyra: P = r " ' Y ^ ( v , v , Q , Q ) r Ma AP = P - P - AP = P Suy ra: -^(AP^rAP)-AP^rAP = A P ' ^ r f y'^r - AP'^Y'^r - r^YAP Nhan ve ciia phuang trinh (2) vdi r ta duge: y[r'^H(Q)r^ + T^Y^] ^"jf^ :: „.Efe& Suy he on dinh theo tieu ehuan Lyapunov U n g dung thuat toan thich nghi dieu khien chuyen dong ciia robot R R T Ithong gian khdp Ta xdy dung dugc s a khoi dieu khien ro- m3q3Vs2- i ; 12(S2~ l ) Hmh2.\ Robot RRT Chuydn ddng eua robot gom khdp: I.Chuyen dgng quay quanh true Z^ 2.Chuyen dgng quay quanh true Z, Chuyen ddng tinh tien theo phucmg true Z d2m3(s2- l ) - dimaqalsj- l) j2 I q2m3 "T - •'" d2m3q3 V la phan tuong h6 cua robot: V = in3S2e2q3qiq2 m3q3(s2~ IJqiqa d2m2S2C2qiq2 4 ' 2d2m3C2S2qiq2~d2m3(s2- ) q i q ~ 2d2m2S2C2q3qiq2 d2m2S2C2qiqi V21 — m3q3q2q3'' t-2d2m3S2e2qiqi + + d2m3qjq: • - d2m3q2q2 52 m3S2C2q3qiqj , , + 2d2m3S2C2q3qiqi ni3q2q3q2 , (2 ,\ • ""aqs (s2~l)qiqi ''d2m3',S2~" \)qiqi+ Khoa hoc & Cong nghe - So 02/2014 Bg dilu khiln tinh mo men T cua robot dugc ap dung la: T = Hv + C + Gv + Kar G la phdn trudng cua robot- (2.2) tC2gq3 , d2m2C2g "• J '-d2m3i m3S2g Trong K^ la ma tran dudng cheo xac dinh ducmg (3 x 3), |3(s2-l) q2m2(si-l) d2m3(s2- l)— -d2m3q3(s2- l) d2m2 I d2m3 ^ d2m3q3 0 mscagqs , d2m2C2g , -^ 2 ^ macag n3S2g Trong dd: bam C dugc xac dinh tii ham V V ( Q , Q ) - C ( Q , Q ) Q - [ y H ( Q ) + S(Q,Q)]Q (2.3) Suy ra: Hinh 2.1 ^tr dd khdi dieu khien robot RRT khdng gian khdp Khoa hoc & Cdng nghf - S6 02/2014 53 C la ham C voi cac tham so uoc luong eua robot RRT V = qd-L(qd-q) v = qa-L(qd-q) r = ( q d - q ) + L(qd-q) Vl Vl ; V2 V — Tl ; r— V2 12 l-r3 V3 V3 - [m2; m3; Ii; I2; I3]; Cac gia tri dat ciia cac khdp la gdc quay cua khdp mpt, goc quay khdp va chieu dai tien khdp chinh la cac gia tri dat mong muon qd2 i qdi qdi ;