NGHIEN CIJfU-TRAODOl TiNH TOflN MO MEN HE ROBOT DCr D^N DONG CO LIEN KET PHI HOLONOM CALCULATE THE MOMENT OF REDUDANT ROBOT SYSTEMS WHICH HAVE NONHOLONOMIC CONTRAINTS PGS, TS Chu Anh My, ThS DflPng Xuan Bien, TS Do Ti^n Lap, ThS Le Chi Thanh, KS Hoang Diifc Long Hpe vien Ky thuat Qudn sfl T6M TAT Xdc dinh qui ludt mo men ddn dgng cdc khdp robot cd y nghia rdt quan trgng thiet keket cdu robot, liia chon cO cdu dan dgng vd ddc biet Id tinh todn cdc ludt dieu khien Bdi bdo ndy, trinh bdy mgt phUOngphdp xdc dinh quy ludt mo men ddn dgng cdc khdp ciia he tay mdy n khdu tich hgp tren mo dun di dgng (robot dU ddn dgng cd cdc lien ket phi holonom) Vdi ddu vdo Id qui ludt chuyen dgng cho trUdc ciia khdu cdng tdc, chung ta se xdc dinh dUdc qui ludt md men ddn dgng cdc khdp tren cd sd thiet ldp cdc phUdng trinh dgng hoc, cdc phUdng trinh lien ket phi holonom, cdc phUdng trinh vi phdn chuyen dgng cua he Vdn di dUdc gidi quyet hieu qud thong qua ly thuyet cd hoc he nhieu vdt vd mgt so ki thudt tinh todn, lap trinh Tfl khoa: Moomen he robot dU ddn dgng, lien ket phi holonom ASBTRACT Determinedness the laws of moment of robots joints is very important meaning in design robots structure, choose mechanical system and calculation the laws of control specially This paper present a method to determine the laws of moment of robot's joints in manipulator system with n-links are intergrated on mobile modul (redundant robot has nonholonomic constraints) If we have the laws of moving of task links, we will determine the laws of moment ofjoints by design the kinematic equations, nonholonomic contracts equations, differential motion equations of system The problem is solved efficiently through mechanical bodies system and some methods to calculate, to program Keywords: Moment of redundant robot systems, nonholonomic contraints TAP CHl CO KHi V I £ T NAM V Sd (Thang nam 201 NGHIEN ClJfU-TRAO 061 I.DATVANDE Trong thiet ke, che tao va dieu khien cdc he robot-tay mdy cd md hinh ca he dfl dan ddng va phi holomon (mdt tay may n khau dU dan dpng gd tren mot robot di ddng nhfl cdc robot qudn sU cd gd md dun ban sung, md dun tay mdy, ), ngudi ta phdi gidi quyet nhieu van de phflc tap, cd van de tinh toan co hoc vd dieu khien Viec tinh todn md men dan ddng vd dieu khien cdc khdp cua robot cho khau cdng tdc bdm chinh xae qui dao, dap flng tdt cdc yeu eau lam viec la van de can nghien cflu gidi quyet vd cho den van chfla dflpc de cap mpt each sdu sdc Cdc cdng trinh nghien cflu gan day nhU: Wiens 13], Hoostmans [4], Liu vd Lewis [5], Qing va I-Ming [2] mdi chi de cap den mdt vdi trfldng hpp vdi md hinh cP hoc cu the da dflpc gian Iflpc va chfla thudn lpi cho viee phdt trien cae thudt todn dieu khien XAC DINH QUY L U A T M MEN DAN DONG CAC K H O P C U A H E TAY MAY N K H A U T i C H H O P TREN MO DUN DI DONG 2.1 Mo hinh cP h^ dfl dan ddng co lien k^t phi holonom Oo = (0|^i(jjc.Z|;) h? qui chira quan Irali Of = {OfSfifif) !a bf 193 dp roboi goc tga ii) , mdy; q =[q,v *im] '• Vec tP cdc tpa dp suy rpng toi thieu cua toan he; qe=[qvv q^ flmj: Vec to cac tpa dp suy rpng dfl cua todn he _ «*/ ki\ Ma tran chuyen doi thuan nhdt gifla O^ va O,, R^/ la ma tran quay, *^k! Id vec tfl tinh tien; ^k! '• Khoi tam cua vdt Oj.; tOj^,: Vec tP van toe gdc cua vat O^; v^y: Vec to van tdc khoi tam cila vat O^; C*,S*: Cac ham Iflpng gidc Cos vd Sin; H,W' la cdc kich thflde hinh bpc; R: Bdn kinh cua bdnh xe robot; '^Gp : la vec to van tdc khoi tdm cua than robot 0^; icGp: la vec to van tde gdc eua than robot O^ inuig t m klioi iam cua robot di d^og tryc x, song song vih duvMg lam cua banti chu dong, y^ aim lien pbuong phia dau robot O; = {OiXLih-L> la bf Ipa dg banb irai goc Oi tning vol Sdioi tam nia banh Zi lai Iboi dirai dau 2.2 Phflpng trinh d6ng hoc vi tri khau cong tac (khau thui n) cua tay may song song -.mip xi a m g phmmg voi i , Of- (Os.TjiJitid 141? Hpa ilg banh phai dugc diDh nghia tuong Tu Oi i ^ ( O ^ , J , ; J M = n la cac he laa da khap iria lo) may O, dugc dinh iipMa ihf» qui t ^ Denavit' Hartenberg (D-H) Hinh Md hinh ca he vd djnh nghia cdc he tga Md hinh gdm robot di ddng bdnh cd tich hpp I tay mdy n khau Gia thiet robot di ddng tren nen phdng va he dupe xem la he nhieu vat ran tuyet ddi Ta dinh nghia: ^w - \_^L ?/f J • Vec to dai so cdc gdc quay banh trdi vd bdnh phai; q^, =[_.Vp yp iPpj :Vec t o v i t r i c u a r o b o t t r o n g O^- , v d i ^^ Id g d c q u a y c u a he O quanh z^; Qm = L^i ^2 Qui luat vi tri khau cdng tac cua tay mdy khdng gian (h$ O^.) dflpc xac dinh neu cho trfldc qui luat bien thien cua eac bien khdp, hay cac tpa dp suy rdng dii cua he q(t) Iheo cdch tiep can ca he nhieu vat, ta xac dinh vi tri tflong ddi gifla vdt ndy so vdi vdt khac thdng qua xdc dinh cac ma tran chuyen ddi thuan nhat giiia robot di ddng vdi nen, gifla khau cua tay mdy vdi robot di dpng, gifla khau vdi khdu vd tuan tfl cho den het khau thfl n Sau thiet lap vd bien ddi, ta cd dflpc vi tri cua khdu cong tac: • • • ?M J : Vec to cde tpa dp suy rpng ciia tay TAP CHi CO KHi VIET NAM S6 (Thang nam 2013) ;q.) (1) NGHIEN c f l u - TRAO DOI Trongdd: p„ = {i„ Y , ] Idvectpvitricua khaucdngtac, ^ = [ / ^ ( q J ^fqj Id diem dinh vi, Y „ = [ , ^ ( q J ^2(1.-) Id cdc tham so hfldng cua h^{q,)f ^i{^s)f khdu; khdu tay may, mdt cdch rieng re Thiet lap phifpng trinh vi phan chuyen dpng cua cP he Sfl dung dang thflc Euler-Lagrange, ta cd A ( q j f Id vec to cdc he phflpng trinh vi phan chuyen ddng eua he phi holonom nhfl sau: fM = [fM fiM ham dpng bpc thudn M(q.)q,-t-C(q,,qJq,+G(q,)-Q-H»'"i 0q, =0 Xac d | n h cac phflPng t r i n h lien ket phi holonom De bdnh xe robot ludn ti^p xuc vd ldn khdng trUpt tren mat nen, van tdc tUc thdi tflong ddi gifla bdnh vd mat nen tai diem tiep xuc phdi ludn bang khdng sudt qud trinh chuyen ddng Lien ket la lien ket van tdc, khdng tich phdn trflc tiep de dfla ve dang lien ket hinh hoc dflpc, nen dflpc gpi la lien ket phi holonom Nghia Id, vi tri khdi tam eua than robot khdng dflpc xac dinh mdt cdch tfldng minh theo cae gid tri gdc quay cua banh chu dpng, ma phai thdng qua cdc phflong trinh rang bude van tdc Qua viec xdc dinh van tdc ddi vd van tdc gdc cua robot (thdn robot di d0ng) O,,, vd quan he van tdc gifla than robot vd cde bdnh robot, khdng gian khdp cua cd he, ta viet dflpc he phflpng trinh lien ket phi holonom: 04^ vdi: M =^/«,J^,Jj-, +Jfi,',JR, 1^ ^^ tran khdi Iflpng suyrdngC(q^,qg) la ma tran cdc thdnh phan Coriolis va ly tam; G (q^) la ma tran cae phan lUc suy rdng cLia trpng lUc; Q la vec to lUc suy rdng dUpc xdc dinh theo nguyen ly cdng do; >- =\_^-^ ^-2 - ^ J la vec to eac nhdn tfl Lagrange He phflong trinh (4) la he phflpng trinh vi phan dai sd, viet cho cdc tpa dp suy rdng dfl q =[^1, q,- q-nj rat khd giai, khd sfl dung de tinh todn md men, phat trien cdc ludt dieu khien De thudn tien, ta bien ddi (4) ve dang thflc viet eho eac tpa dp suy rdng du thdng qua cae phep bien doi dao ham: S,"MS,q + S,^(MS,+CS,)q + S j G = S j Q (2) Trong do: ©la ma tran lien ket phi holonom (4) (5) 2.6 Phflong phap tinh toan md men dan ddng toan he va Q[ la van tdc suy rdng dfl Cho trUde qui dao khdu cdng tdc 2.4 Xdc dinh van t6c khau cong tac tren tay may Tfl phflong trinh (1) ta viet dflpc van tdc tuy?t ddi eua khdu cdng tdc: p„ =Jcq,,+J!'q,» (3) p-.(')= 5.W ^^^ Id vi tri vd A(,„ hUdng cua khau cdng tdc theo thdi gian, can phdi xdc dinh cdc thdnh phan cua vec to T|rU[ri(/) Vcfi 1, ^ [ j " khdng gian cdng tdc, vdi rjir) 0 r,(/) T„{t)J J' jld ma trdn lacobian cd Viet lai cdng thflc (5) nhfl sau: 6x(n + 3)vd J"ccf x tflpng flng vdi phan robot, 1^ cd x « tflong flng vdi phan tay mdy Phflpng t rinh (3) cho thay, van tdc cua khau cdng tdc dflpc xac dfnh tfl van toe cua robot vd van tdc cua cdc TAP CHi CO KHf V I £ T M^q+C,q-KG,-F, =x (6) v d i M^ =MS^^ , v.;^ = MS^+CS, , G,^ - G , F,^ ^ J' F , Viec xdc dmh T(?)theo phflOng phap giai'' NAM Sd (Thdng ndm 2013} NGHIEN CUfU-TRAODPl Bang 1: Cdc thong so cd hi tich la rat kho khan nen cac T(t) tai cac budc thdi gian (t) duoc xac dinh bang phifong phap so Tii cac gia tri p„(r, )ta phai xac dinh diioc cac q(r,) ,q(/,), q((,), c a c m a t i a n M,{q),C,(q,q), G,(q), Banh Than Robot Tay may K.liiong (kg) m, = m =1 „ (l^m') 2.10' m„_, 4.10-= m, = 18 F^ (q) Til khai trien Taylor, ta co the tinh duac: q ( = i(',-,)+J"[q(',-.)][p(0-p(',-.)] I,,(kgni') l„(kgm=) 3.10' 3.10-= 3.10' 3.10 = 2,5.10 = 2.10 = (7) Talaic6:o>„(t,) = AG»,(;,)A5„(t,-).Vay fM) hoan toan xac dinh d u o c theo "M Cac kich thildc (m) 3.10-= R=0,l;H=0,l;W=l;d,=0.4; a2=0,5 Ket qud tinh todn va md phdng dflpc chi tren cae hinh 3,4,5,6,7 eae bfldc thdi gian Trong trfldng hop tong qudt nhat: q{t,) = Ji{t,)p„{',) (8) vdi Jf^ la ma tran tfla nghich dao suy rdng cua ma tran J„ = '"' " [ J „ De dang tinh duoc: KJ ' * q(',)=J»('-)[p,(':)-J»(',)'!(',)] (9) Hinh Qui dgo cita robot-tay mdy Nhfl vdy, tai moi thdi diem ta hoan todn tinh dflpc bang ede cdng thflc (7,8,9) va (6) 2.7 Vi du minh hpa qua trinh tinh toan Ta xet mdt vi du sau: Mdt tay may mdt bac tfl quay ga tren robot di ddng nhfl hinh ve (hinh 2) Qui dao edng tdc C ( x ( / ) , j ( ? ) ) Hinh D6 thi Hi' ^R- Hmh Hi robot-tay mdy bdc tU vd cdc thong sd cd he TAP CHI CO KHI VIET NAM *•* Sd (Thang ndm 2013) NGHIEN CtfU-TRAO 061 KET LUAN Bdi toan tinh todn md men he robot-tay mdy n khdu, lien ket phi holonom, dfl dan ddng da dflpc phat bieu vd gidi quyet, cd vi du minh hpa kem theo Cac ket qua chinh bao gdm: (i) he phflong trinh vi phdn chuyen dpng todn he viet cho cdc tpa dp suy rdng dfl, (ii) he phflong trinh vi phdn chuyen ddng viet cho cdc tpa dp suy rdng du, (ui) phflong phap va ket qua tinh toan qui luat md men ddn ddng cac khdp robot-tay mdy Cac ket qud tren cho phep phat trien cdc luat dieu khien mdt cdch thudn lpi nhfl: Ludt dieu khien tinh toan md men, luat dieu khien trUpt, v.v Ta hoan toan cd CO sd de phdt trien cac phdn tich ve dp ben ca cau, ve tdi flu hda cau true vd phdt trien cdc ludt dieu khien theo cdc phflong phap khae • Ngdy nhan bdi: 02/3/2013 Ngdy phan bien: 05/4/2013 Ngfldi phan bien: TS Nguyen Hong Thai, Bd mdn Co sd thiet ke Mdy va Robot, Vien Co khi, Trfldng Dai hpe Bach Khoa Ha Ndi Tai lieu tham khdo: Hinh Do thi mo men ddn dgng khdp noi tay mdy vdi robot Hinh 3, 4, cho ta biet sU thay ddi cua vi tri, van tde gde vd gia tdc cua cac khdp, ket qua thu dflpc phu hpp vdi quy dao dau vdo Theo dd, cac dd thi d hinh 6, va cho ta biet quy ludt md men dan dpng cdc bdnh, dan ddng khdp ndi tay mdy vdi robot biet trfldc quy ludt chuyen dpng Di^u ndy, cho phep cdc nhd thiet ke de dang Idm can cfl de tinh chpn cde dpng ca dan ddng lap vdo robot [ ] Chu Anh My, el a!, "Mechanical Design and Dynamics Modelling of RoPC Robot", Proceedings of International Symposium on Robotics and Jvlechatronics, Hanoi, Vietnam, 2009 pp, 92-96 [21 Qing Yu St I-Ming Chen, "A General Approach to the Dynamics of Nonholonomic Mobile Manipulator Systems" Journal of Dynamic Systems, Measurement, and Control., Vol 124, 2002, pp 513-21, [3], G Wiens, "Effects of Dynamic Coupling in Mobile Robotic Systems" Proc of 1989 Worid Conf on Robot, Res, Detroit, MI, SME, pp 43-57 [4] N Hootsmans, "The Motion Control of Manipulators on Mobile Vehicles," Ph.D dissertation, MIT, Cambridge, MA, 1992 [5) K Liu and F Lewis, "Decentralized Continuous Robust Controller for Mobile Robots" Proc of IEEE Int, Conf on Robotics and Automation, Cincinnati, OH, 1990 pp 18221827 [6] Y Yamamoto and X Yun, "Coordinating locomotion and mampulation of a mobile manipulator" IEEE Transaction on Automatic Control, Vol 39 No pp 1326-1332, (7) Y Yamamoto and X Yun, "Effect of the dynamic interaction on coordinated control of mobile manipulators" IEEE Transactions on Robotics and Automation, Vol !2, No 5, 1996, pp 816-824 TAP CHl CO KHf VlfiT NAM So (Thdng nam 2013) ... van tdc gdc cua robot (thdn robot di d0ng) O,,, vd quan he van tdc gifla than robot vd cde bdnh robot, khdng gian khdp cua cd he, ta viet dflpc he phflpng trinh lien ket phi holonom: 04^ vdi:... hinh bpc; R: Bdn kinh cua bdnh xe robot; ''^Gp : la vec to van tdc khoi tdm cua than robot 0^; icGp: la vec to van tde gdc eua than robot O^ inuig t m klioi iam cua robot di d^og tryc x, song song... ke, che tao va dieu khien cdc he robot- tay mdy cd md hinh ca he dfl dan ddng va phi holomon (mdt tay may n khau dU dan dpng gd tren mot robot di ddng nhfl cdc robot qudn sU cd gd md dun ban sung,