Hybrid force-position control for manipulators with 4 degrees of freedom

Transcription

1 Hybrid forc-position control for manipulators with 4 dgrs of frdom Alxandru GAL Institut of Solid Mchanics of th Romanian Acadmy C-tin Mill 5, Bucharst, Romania galxandru@yahoo.com Abstract: his papr taks th alrady known hybrid forc-position control, and us it for a hybrid control manipulator with four dgrs of frdom, from which rotational and on translational. Control systm usd, us dirct kinmatics for finding th currnt position of th nd-ffctor, and calculats th positioning rror using th transposd Jacobian matrix. o obsrv th ffctivnss of th control loops and controllrs was modld using Matlab Simulink, th proposd control systm. Basd on th obtaind graphs and prsntd in this papr, th control systm was tstd in a virtual nvironmnt, whr thr ar no risks of damag or injury to prsonnl. Using this tst mod, on can achiv bttr control mthods with varid prformancs, bfor moving to hardwar implmntation. Kywords: hybrid control, manipulators, position control, forc control. Introduction Combining forc and position information into a singl ovrall control schm for nd-ffctor motion in indfinit nvironmnt was first proposd by Raibrt and Craig []. h control schm mad by thm was calld hybrid forc/position control, currntly calld hybrid control. Aftr Raibrt and Craig s paprs on hybrid control, Zhang and Paul[] followd with a hybrid control schm that changd from a Cartsian spac form to on in joint spac, using th sam mthod for th sparation of th position and forc rstrictions in Cartsian spac for th motion of th manipulator/robot. In both cass, th advantag of position and forc hybrid control is that th informations ar analyzd indpndntly to tak advantag of wll-known tchniqus that control th position and forc, and ar thn combind (addd-up) in th last round, whn both wr convrtd to valus of joint spac. An and Hollrbach[] hav shown that crtain mthods of forc control, including hybrid control ar unstabl. Zhang[4] showd that th hybrid control schm can bcom unstabl in crtain configurations of manipulators using rotational joints [5]. A mthod to liminat th instabilitis du to invrs matrix computation for motion gnration on robot axis is to us th transpos matrix [6,, ]. Mor rcntly, wr prsntd numrous systm architcturs for th hybrid forc-position control, which ld to incrasd prformancs in walking robot control [8, 9, ]. In this papr it will b prsntd a hybrid forcposition control systm for a manipulator with four dgrs of frdom. his systm was tstd using Matlab Simulink, and th rsults ar shown and discussd. Also, th main goal of this papr is to find th bst and asy to us and implmnt control systm for a manipulator.. Systm dscription Manipulator structur chosn for study in this papr, prsnts svral construction lmnts shown in figur. Sgmnts d, a and a ar rgardd as finit and a =, d = 6 and d = 5, whr valus gnrally chosn so th systm can b simulatd. Fig. Manipulator structur with four dgrs of frdom h systm also shows four dgrs of frdom, of which thr ar rotational and on translational. h thr rotational dgrs of frdom ar prsnt in th manipulator joints, and th translation is ISBN:

2 found in th structur of nd-ffctor for picking up th pics. o fully dscrib th systm is ncssary to writ th dirct kinmatic quations for finding th coordinats of point M corrsponding to th manipulator s nd-ffctor. Dirct kinmatics: cos cos cos sin () cos cos sin cos sin sin xm d l l a a y M d l l a a z a l l M Sinc th constants d, a and a ar zro th quations bcom: xm l cos l cos cos y M l cos l cos sin () z a l sin l sin M hs quations for finding th coordinats of point M, of th nd-ffctor, ar usd in th control loops in ordr to calculat th positioning rror of th manipulator, starting from th joint angls. Invrs kinmatics: Invrs kinmatics involvs knowing th coordinats of point M and tris to find th joint angls for th known nd-ffctor position. h systm to b solvd is that of rlation (). What is diffrnt from th dirct kinmatics is that th valus known ar on th lft sid and th unknown on th right sid. Unknowns bing in trigonomtric rlationships, to solv th systm, w nd to solv ordr 4 quations, making it vry difficult to find an optimal solution. Mx Mx Mx M My My My () J Mz Mz Mz It was proposd that th first vrsion will us th invrs Jacobian matrix. o find it, first th Jacobian matrix is computd using partial drivativs from rlation (). sin l cos l cos l cos sin l cos sin J cos l cos l cos l sin sin l sin sin (4) l cos l cos Using th Jacobian matrix to rplac th difficult calculations of invrs kinmatics, rquirs invrs of th Jacobian matrix, rsulting th algorithm writtn blow. his algorithm is much asir to us compard to th calculation of th rlationship systm of quations () in which th unknown variabls ar changd with th known ons. But this systm shows instability bcaus solving th problm of calculating th invrs matrix can prsnt singularitis, lading to an inability to calculat th positioning rror. Bcaus of this, th following sction will prsnt an algorithm that rplacs th computation of th invrs Jacobian matrix with th transposd Jacobian matrix.. Calculat J. Calculat. Calculat J Updat th joint and nd positions basd in nw θ. Rpat until is within tolranc of currnt or itration count xhaustd.. Background of th hybrid forcpsition control. For any task assignd to th manipulator, th position constraints ar sparatd from thos in forc. Aftr that, th first stp is dtrmining th rlvant positioning rror in position control x Sx (5) s In which, th rror vctor x is th diffrnc btwn dsird and actual position of th ndffctor in Cartsian spac []. h nxt stp is to transform into joint coordinats using th invrs Jacobian matrix, rsulting s goal (joint positioning rror) for controlling th manipulator. h quation x J (6) is usd to dtrmin th rrors of th joints, corrsponding to th Cartsian rrors x. hus, rrors in th joints ar calculatd from th Cartsian rrors as follows: J x (7) And corrlatd with Raibrt and Craig's approach to hybrid control schm, th rrors of th joint ar J x (8) s Whn th matrix J is singular, or clos to a singularity, a mor gnral and numrically stabl mthod is ndd for th calculation of in th quation (8). his quation is a solution that causs th hybrid control schm to b unstabl [,6] o control th forc, th Cartsian forc rror f is calculatd as th diffrnc btwn th dsird and masurd forc. h main concpt of hybrid control is to control th forc in th dirctions that ar not controlld in position. Using th orthogonal s s ISBN:

3 complmnt of matrix S, (I-S) th forc rrors ar f I S f (9) givn by th rlationship: ransposd Jacobian matrix, transforms th forcs from Cartsian coordinats, in joint torqus [7,] as follows: J f () his rlationship is valid for any J and f, and is not an approximat calculation of. In particular, using th Cartsian forc rrors from th rlation (), w asily obtain that th joint torqu rrors ar givn by: s () s J f s In trms of kinmatic quations, ths forc quations prsntd ar clar and prcis mathmatical, gomtrical and algbraic rprsntations. hy dirctly modl, th physical bhavior of th nvironmnt. hrfor, th forc control for th hybrid control schm is always kinmatically stabl. h positioning rror, is calculatd (fig. 4) using th Jacobian matrix alrady calculatd, and th two valus of nd-ffctor s position (point M) goal and ral. his computation follows th algorithm from rlation (), to calculat th positioning rror of th thr motors. Bcaus th convrgnc was too slow, th thr rrors,,, and, wr amplifid by a factor of 6, to incras th acclration of th motors. 4. Hybrid forc-position control simulation, using Matlab Simulink From th analysis of th control systm from [, ] th us of th invrs Jacobian matrix can introduc singularitis lading to systm instability. o liminat this problm, it is proposd to us th algorithms such as DLS (dampd Last Squars) or thos that us th transposd Jacobian matrix. h application rsulting from th rsarch prsntd in this papr, uss th algorithm basd on th transpos Jacobian matrix: goal ral JJd J J JJd JJd JJd J () In which, th transposd Jacobian matrix is computd mor asily than th invrs Jacobian, and it shows no singularitis. h only ngativ diffrnc compard to othr algorithms that may hav singularitis, is that J convrgs slowly to th solution. Constant α is chosn to minimiz th nw valu of aftr th systm is rfrshd with nw calculatd valus of th joint angls. o simulat in Matlab Simulink th hybrid forc-position control, a first stp is to tst and us th hybrid control in simulations. For this w nd a rfrnc signal. his rfrnc was gnratd for th trajctory of moving th manipulator s ndffctor (fig.). In ordr to obtain th D motion path, it was gnratd a trajctory for ach axis (fig.). Fig. End-fctor s position rfrnc gnration Aftr calculating th thr corrsponding positioning rrors of th thr motors, thy ar transmittd to th control loops, which will dirctly rciv th rror, no longr bing ncssary to calculat. Fig. Dlta thta calculation Using th calculatd rrors, thy ar takn to control th motor position (fig. 4). his control uss two proportional-intgral (PI) control moduls, on for spd and on for ngin torqu. With ths rgulators, along with th DC motor modl from figur 5, it is obtaind th motors position control that movs th manipulator joints. ISBN:

4 h motor is simulatd using th diagram from figur 5[4], that uss as rfrnc th armatur currnt, and th load torqu. Using ths, th motor diagram will rturn signals (angular spd, armatur currnt and position). h position is usd in calculating th Jacobian, th angular spd is usd in calculating th rror for th PI-spd controllr and th armatur currnt is usd in calculating th rror for th PI-torqu controllr. Also for th raction loop, w hav two signal amplifirs that multiply th thta rror by 5 and th rfrnc armatur currnt by. Fig. 4 Control functions for th thr rotational dgrs of frdom h motor systm from figur 5, was modld by summing th torqus action on th rotor inrtia and intgrating th acclration to giv th vlocity, and intgrating th vlocity to gt th position. Also, Kirchoff s laws will b applid to th armatur circuit. For th hybrid forc-position control of th nd-ffctor, th schm usd is in figur 6. o control th slction of position and forc control, it uss a signal that simulats a contact snsor btwn th nd-ffctor s grippr and th workpic that nds to b movd. h signal valus of or, maks th slction of position or forc control. h position control is sparat from th on in forc, but th rgulators ar similar. Fig. 5 DC motor modl usd h rgulators usd in this control loop ar calculatd for a simpl motor control, and adaptd for th hybrid control, whr th position control is amplifid by a factor of compard to th forc control. hy wr dsignd by a root locus mthod and th discrt-tim transfr function from th motor position output to th voltag input is: z,z, () V z Z,945 Z,945 By compnsating th systm using th locus root, w ll gt th motor control rgulators from figur 6. On notics a block that simulats th DC choppr driv (a Simulink gnratd block), for which, th rfrnc is th sum of th two rfrncs (th position and forc control). Position control of th nd-ffctor is limitd to th command to clos or opn th grippr, whr w hav a translational movmnt. his movmnt to clos th grippr, stops whn it coms to contact with th grippd workpic, and th forc control taks ovr, which aims to control th contact forc btwn th nd-ffctor and workpic. o slct which control is usd, a fuzzy control block multiplis th positon and rfrnc valus so th final rfrnc in position plus forc will hav th rfrnc corrsponding to th dsird control of th grippr. Fig.6 Hybrid control schm ISBN:

5 5. Rsults and conclusions Following th simulation, ths graphs wr obtaind (fig.7 - fig. 9): Fig.7 Dlta hta Fig.8 Dlta hta Fig.9 Dlta hta In th abov figurs ar graphs of variation of positioning rrors. It may b notd that bcaus th simulation motors starting angls ar and th initial position of th manipulator nd-ffctor (,,) dos not match th angls of, initially appars a larg positioning rror until th manipulator rachs th position rfrnc. h larg rror that w can s starting at th tim 5ms is du to th unxpctd slop of th trajctory, slop that also occurs at th nd of th trajctory, whn it bcoms a constant valu, at tim 95ms. hraftr, th positioning rror is rlativly small, and th following graphs will show th diffrnc btwn rfrnc and tracking. h diffrnc that appars in th figurs, and is du to th continuous incras in position rfrnc, that th motors hav to compnsat. h positioning diffrnc achivd can b vry larg if a wrong rgulator is usd. hs figurs also shows positioning graphs on th thr axs of ndffctor, in which th rfrnc is th blu lin and th ral position on th thr axs is rprsntd by th rd lin. Figur shows th chart positions in XOY plan. Figur shows th chart positions in ZOY plan. And in figur w chart OY axis position in rlation to tim. 4 Fig. Movmnt in XOY plan (blu-rf; rdactual motion) Fig. Movmnt in ZOY plan (blu-rf; rdactual motion) Fig. Movmnt in OY(t) plan (blu-rf; rdactual motion) It may b notd that tracking is good for th rgulators usd, but thr is still room for improvmnt in futur rsarch. If w calculat th distanc th nd-ffctor has travlld and w compar this with th maximum distanc rror of th position, w obtain an rror that is lowr than %, du to th abrupt slop changing. As I said arlir it can b sn in th initial stat, th manipulator s control functions is lading th ndffctor to th rfrnc stat, and this is amplifid bcaus th dlta thta angls wr amplifid by a factor of 6, to achiv a bttr rspons tim. his was also ndd bcaus th solution of using th transpos Jacobian matrix, convrgs vry slowly to th givn position rfrnc, and th amplifir of angl rrors tris to ovrcom this problm. ISBN:

6 Fig. Hybrid control of th nd-fctor For th hybrid control th motion is prsntd in figur, from bottom to top: torqu, forc control valus, th armatur currnt and th rfrnc rcivd by th motor, corrsponding to th position modul from th hybrid control schm in figur 6. It can b sn in th figur that th driv rfrnc changs according to th position rfrnc and thn with th forc rfrnc whn a contact with th workpic is mad. hus, th driv rfrnc incrass, rsulting in a highr grip forc. h figur shows th rfrnc rcivd by th driv bcaus th position of th grippr should rmain th sam if th workpic is rigid. Bcaus of this, th driv will try to compnsat th positioning rror and it will giv a highr acclration and spd of th grippr, rsulting in an incrasd grippr forc. his papr tris to ovrcom th problm that occurs whn th control uss th invrs of th Jacobian matrix or th difficult solving of th quations (). By using th transpos Jacobian w avoid th singularitis of th invrs and th nd to choos th optimal solutions of th quation systm. Also, by amplifying th angular rrors w try to compnsat th slowly convrgnc to th rfrnc path. In th hybrid control, th us of simpl fuzzy logic allows th chang btwn th rgulators of forc and position control. Acknowldgmnts: h author wish to xprss his gratitud to Romanian Acadmy for its continuous support. h work took plac as a continuity of th rsarch projct no. 589/999 in th framwork of Romanian Acadmy s Grants on yar 999. Rfrncs: [] J. J. Craig and M. H. Raibrt, "A Systmatic Mthod of Hybrid Position/Forc Control of a Manipulator", in Computr Softwar and Applications Confrnc, IEEE Computr Socity, Novmbr 979, pp [] H. Zhang and R. P. Paul, "Hybrid Control of Robot Manipulators," in Intrnational Conf. on Rob and Auto, IEEE Computr Socity, March 985. St. Louis, Missouri, pp [] C. H. An and J. M. Hollrbach, "Kinmatic Stability Issus in Forc Control of Manipulators" in Int Conf on Rob and Aut, IEEE Rob and Aut Socity, April 987, pp [4] H. Zhang, "Kinmatic Stability of Robot Manipulators undr Forc Control," in Intrnational Conf on Rob and Aut, IEEE Rob and Aut Socity, ISBN: , May 989, pg [5] William D. Fishr, Dr. M. Shahid Mujtaba, Hybrid Position/Forc Control: A Corrct Formulation, Mas. & Manuf. Systms Laboratory HPL-9-4, Octobr, 99. [6] Vladaranu L, Vasil A, Ral im Control Opn Systms of 5 DOF Nano-Manipulators, Confrnc on Advancd opics in Optolctronics, Microlctronics, and Nanotchnologis,, Constanta, WOS: , SN X. [7] R. P. Paul, Robot Manipulators: Mathmatics, Programming and Control. Cambridg : MI Prss, 98. [8] Ion I, Vladaranu L, Adrian C, Vasil A, Nw Modl of MERO Modular Walking Robots Support for Havy Load ransportation, 8th Procdings o th 8th Intrnational Confrnc on Applications of Elctrical Enginring, pg. 4-46, WOS: 66649, 9, [9] JASON CLARK, Invrs Kinmatics, Powrpoint prsntation, Essntial Math for Gams. [] ont G, Vladaranu L, Muntanu MS, ont DG, Hirarchical Baysian Rliability Analysis of Complx Dynamical Systms, Procdings of th 9th WSEAS Intrnational Confrnc on Applications of Elctrical Enginring,, Pnang, Malaysia, pg. 8-86, WOS: 7877, SN [] S. R. BUSS, Introduction to invrs kinmatics with Jacobian transpos, psudoinvrs, and dampd last squars mthods, ypst manuscript, April 4. [] Vladaranu L, Ion I, Diaconscu E, ont G, Vla LM, Mitro D, h Hybrid Position and Forc Control of Robots with Complianc Function, Procdings of th th WSEAS Intrnational Confrnc on Mathmatical and Computational Mthods in Scinc and Enginring, NOV 7-9, 8, Bucharst, pg , WOS: 64688, [] S. R. BUSS AND J. KIM, Slctivly Dampd Last Squars for Invrs Kinmatics, Octobr 5, 4. [4] 5 ISBN: