S 5 S 3 S 4 S 1 S 2 P 6 P 5 P 3 TCP

Transcription

1 A BODY-ORIENTED METHOD FOR DYNAMIC MODELING AND ADAPTIVE CONTROL OF FULLY PARALLEL ROBOTS Alan Codourey, Marcel Honegger, Etenne Burdet Insttute of Robotcs, ETH-Zurch ETH-Zentrum, CLA, CH-89 Zurch, Swtzerland E-mal: Abstract: In ths paper we propose a method based on the vrtual work prncple to nd a lnear form of the dynamc equaton of fully parallel robots. Compared to other methods, t has the advantage that t does not need to open the closed loop structure nto a tree-structure robot. It consders rather each body separately usng ts Jacoban matrx to project the forces nto the jont space of the robot. Thus, smplcatons can be made at the very begnnng of the modelng, that s very usefull for real-tme nonlnear adaptve control mplementaton. As an example, the method s appled to the Hexaglde, a parallel machne tool wth degree-of-freedom. Based on ths model, a smulaton of a non-lnear adaptve controller s performed, demonstratng the possblty to apply nonlnear adaptve control to complex parallel robots. Keywords: robot control, robot dynamcs, adaptve control, parameter dentcaton, parallel robot 1. INTRODUCTION The dynamc equaton of robots s lnear n ts dynamc parameters (Khall and Klennger, 198; Khosla, 198). Ths property has been exploted to dentfy the dynamc parameters of robots usng Least-Square technques (An et al., 198; Khosla and Kanade, 198) or nonlnear adaptve control (Crag, 1988; Slotne and L, 198). Many solutons have been proposed to nd a lnear form of the dynamc equaton of seral (Gauter and Khall, 199; Khosla, 198) and tree-structure robots (Khall et al., 1989). For manpulators wth closed loop chans, the dynamc equaton s generally dcult to establsh. It s thus not easy to wrte t n a lnear form. Solutons have been proposed (Walker, 199; Goldenberg et al., 199; Khall and Benns, 199) n whch the model s usually obtaned by openng the loops n order to form a tree-structure robot. The dynamcs equaton s then derved usng classcal technques developed for seral robots (Khall et al., 1989) and a loop closure constrant equaton to elmnate the non-actuated jonts from the tree-structure dynamc model. The method has been appled to planar parallel mechansms, but becomes very cumbersome for spacal parallel robots. The problem has been recently addressed by (Bhattacharya et al., 199) who proposed a soluton based on an energy model of the parallel structure. It results n a parallelsm n the dynamc equatons whch can be exploted ecently n a multprocessor controller structure. It remans however very computatonally ntensve. In ths paper we propose a new method for ndng the dynamc equaton n an explct lnear functon of the the dynamc parameters. It s based on the

2 vrtual work prncple and uses Jacoban matrces to \project" the forces of each body n the space of the actve jonts. It has the followng advantages: The robot does not need to be opened n an equvalent tree-structure mechansm. Smplfyng hypotheses can be used at the very begnnng of the modelng already. It leads to smple equatons that can be used for real tme mplementaton of nonlnear adaptve control. In ths paper, we rst present the proposed method to get a lnear form of the dynamcs equaton. The method s appled to the Hexaglde, a parallel platform wth dof and then used n a nonlnear adaptve control scheme. Fnally, smulatons are performed showng the ecency of nonlnear adaptve control of complex parallel robots..1 Dentons. DYNAMIC EQUATION A robot s a system of N rgd bodes connected together n order to form a knematc chan. Let q be the vector of actve jont varables and X the vector of operatonal space varables descrbng the pose of the end-eector n a reference frame O. A frame C s attached to the centre of mass of body. Let m and I be the mass and nerta tensor of body, and x the poston of ts centre of mass n the reference frame O. _x and! are ts lnear and angular velocty respectvely. The rotaton matrx between body frame C and the nertal reference frame O s noted O R. For each body, the lnear and angular velocty of the center of mass can be expressed as a functon of the jont velocty vector _q by the mean of the basc Jacoban (?) dened by: X x = J _q (1)! Ths basc Jacoban can also be used to transform a vrtual dsplacement of the jont space vector nto a vrtual dsplacement of body. wth F nerta m x ; () I _! +! I! F appled F ; () T and where J s the basc Jacoban as dened n equaton (1) and F, T the appled forces and torques (e.g. motor torques, gravty, frcton).. Extractng the actuator forces In order to wrte equaton () n a lnear form of the dynamc parameters, the actuator forces must rstly be extracted. The wrench of appled forces and torques F appled can be spltted n an actuator wrench F a and an external wrench F ext. Equaton () can thus be rewrtten as follows: or also, =1 J T F a = J = =1 =1 J T?F nerta J T?F nerta? F ext ; ()? F ext () where s the actuator torque vector and J s dened as follows (Codourey and Burdet, 199): J J T A1 1 J T A : : : J T Am m ; () wth m the number of actuators, J A the Jacoban matrx lnkng the velocty of frame A attached to the motor and the jont velocty vector _q, and ; (8). Vrtual work prncple for revolute jont 1 for prsmatc jont ; = 1? : Usng the vrtual work (or d'alembert) prncple, the dynamcs of a system of N rgd bodes can be descrbed by (Codourey and Burdet, 199): =1 J T F nerta? F appled = () Usng ths denton, the dynamc equaton () can nally be wrtten as: = J?1 " =1 J T?F nerta #? F ext : (9)

3 . LINEAR FORM OF THE DYNAMIC EQUATION Assumng that the knematcs of the system of N rgd bodes s known, ts dynamc equaton can be wrtten n a lnear form of the dynamc parameters (Khosla, 198; Scavcco and Sclano, 199): = (q; _q; q) p ; (1) where s the dynamc matrx and p the dynamc parameter vector contanng functons of the lengths, masses and nertas..1 Inertal forces on a rgd body Inertal forces are usually descrbed relatvely to the center of mass of a body. If the body s descrbed relatvely to an arbtrary frame A attached to body, a transformaton has to be realzed 1. We assume that the Jacoban matrx J A lnkng the jont velocty _q to the lnear and angular veloctes of frame A s known, as well as ts veloctes _x A,! A and acceleratons. Accordng to equaton (), nertal forces C F C actng at the centre of mass of the body are calculated usng the followng equaton: C m FC = C x C C C IC _! C + C! C C C (11) I C!C They can be rewrtten n a lnear form of the dynamc parameters of body wth respect to an arbtrary reference frame A attached to the body (Codourey and Burdet, 199): where wth A FA = ~! = A x A? A^x A! m m r I p ; (1) = A ^! A A^! A + A ^_! A ; (1)! = A ^! A A ~! A + A ~_! A ; (1)! x! y! z! x! y! z! x! y! z (1) and where a hat (^a) over a vector ndcates ts skew symmetrc matrx notaton such that ^ab = 1 ndex s omtted n the followng to smplfy the notaton a b for any vector a = [a x ; a y ; a z ] T, and s dened as: ^r =?a z a y a z?a x : (1)?a y a x m s the mass of the body, m r ts rst order moment vector and I p [I xx ; I xy ; I xz ; I yy ; I zy ; I zz ] T a vector composed of the second order moments (nerta) of the body. Expresson (1) s smlar to the one obtaned by (Goldenberg et al., 199). The wrench A F A can nally be calculated n reference frame O as follows:. Lnear form O FA = OA R O A R A FA (1) Assumng that the external forces are composed of gravtatonal forces only, equaton (9) can be rewrtten wth respect to body ponts A, = 1:::; N, as: = J?1 " =1 J T A O F A? =1 J T O m G # ;(18) where G s the vector of gravtatonal acceleraton. By addng the acceleraton of gravty to the acceleraton of body, ths equaton can be rearranged as follows: where wth and = J?1 J T A1 1 JA T : : : JA T N N OA R O A R p 1 p. p N ; A a? A^a ; (19)! A a A x A? A G; p = [m ; m r T ; IT p] T : Equaton (19) s a lnear representaton of the dynamc equaton. If a column of the dynamc matrx s composed of zeros only t can be cancelled together wth the correspondng parameter, snce t has no nuence on the dynamcs. Furthermore, f columns of the dynamc matrx can be

4 Bz S Bx S S By S S 1 S P P Px Tx P P P P 1 Py TCP Ty Fg. 1. Drawng of the Hexaglde parallel machne tool expressed as lnear combnatons of other columns, the correspondng parameters can be grouped and only one column kept n the dynamc matrx (Gauter, 199; Goldenberg et al., 199; Khall and Benns, 199). In nonlnear adaptve control, t s necessary to compute the nverse dynamc model n real-tme. For fully parallel robots, an ecent soluton s to neglect the nertas of the legs. It has been shown n (Codourey, 1991) that for most fully parallel robots ths assumpton leads to a smple model wthout too much loss. In ths case, ndng a lnear form of the dynamcs s hghly smpled by usng the method proposed n ths paragraph. Indeed, the complcated knematcs of the legs does not need to be computed, and the robot s reduced to few bodes. As an llustraton, the method descrbed n ths paragraph s appled to the Hexaglde, a dof parallel machne.. MODELING OF THE HEXAGLIDE MACHINE The Hexaglde s a concept developed at the Insttute of Machne Tools of the Swss Federal Insttute of Technology Zurch (Wegand et al., 199). It has a fully parallel structure wth dof and s ntended to be used as a hgh speed mllng machne. A drawng of the machne s shown n gure 1. It s smlar to a Stewart platform but nstead of varable length bars, t has constant length bars lnkng the platform to lnear motors dstrbuted on lnear rals. In ths secton, we rst derve the knematcs of the Hexaglde and then use t for calculatng a lnear form of the dynamc equaton. Fg.. Denton of the frames and geometrc parameters of the Hexaglde.1 Knematcs of the Hexaglde For modelng the Hexaglde structure, a base reference frame B s dened as shown n gure. A frame P s attached to the center of gravty of the platform. A thrd frame T s attached to the tool-center-pont (TCP) of the robot. The ponts lnkng the legs to the platform are noted P, = 1; :::;, and each leg s attached to the lnear motor at the ponts S, = 1; :::;. The pose of the TCP s represented by the vector x T [x; y; z; ; ; ] T ; () where x; y; z are the cartesan postons of the TCP and ; ; the xed angles ZYX representaton of ts rotaton (Crag, 1989). The rotaton matrx between the frames T and B s gven by: B T R = R x() R y () R z () (1) The nerta of the legs wll be neglected n order to smplfy the computaton of the dynamc model. Wth ths assumpton, the robot can be consdered as composed of bodes,.e. motors and the platform. In order to compute the dynamc equaton, the velocty and acceleraton of each body, as well as the Jacoban relatng ts cartesan velocty to the jont velocty of the robot, have to be known. In the followng, the motors wll be represented by the ndces 1 to and the platform by ndex. Let q denote the jont coordnates, and the jont vector be: q [q 1 ; q ; q ; q ; q ; q ] T : () To smplfy the notaton, the Jacoban matrces of the motors J can be decomposed n two parts: one part aectng lnear veloctes (J v ) and another aectng angular veloctes only (J! ): We have J Jv J! ()

5 and x 1 = J v1 = 1 ;. J v = 1 ; () q 1 ; ; x = q : () The lnear acceleraton of body s gven by x T whch s calculated by numercal derentaton of _x T. The angular velocty and acceleraton are further calculated as follows: B!T = E + r _ ; () B _! T = d dt (B! T ) : (). Lnear form of the dynamc equaton Because no rotaton occurs at the motor level, the correspondng Jacoban matrces, angular veloctes and acceleratons are all equal to zero: J! = ; ()! = ; _! = ; = 1; :::; : () The Jacoban matrx J T of the TCP relates the tme dervatve of the operatonal coordnates and the jonts velocty vector: _x T J T _q; (8) wth x T dened n () and q n (). The Jacoban J T has been derved n (Honegger, 199) usng the partal derentaton of the nverse geometrc model of the machne. J T s representaton dependant. In order to nd the nstantaneous lnear and angular veloctes of the TCP, J T has to be multpled by the representaton dependant matrx E + (x T ) (Crag, 1989): wth and _x T = E + J T _q = J T _q ; (9) _x T [v T T ;!T T ] T ; E + 1 E + r E r + = 1 sn cos? cos sn : () sn cos cos It has to be noted that matrx E + s sngular for = =. Ths s however not a problem n our case, snce ths happens outsde of the workspace of the robot. From (9), t follows for the Jacoban matrx of the platform (body ): J = J T = E + J T : (1) Usng equaton (19), and notng that the use of equaton () leads to the dentty matrx for J, the dynamc model of the Hexaglde can be wrtten n a lnear form of the dynamc parameters as follows: = p () where the matrx and correspondngly the parameter vector p can be spltted n three parts: ; () p 1:: b p1:: p b p T : () In ths representaton, the ndex b corresponds to the frcton, ndex 1:: to the dynamcs of the motors and to the dynamcs of the platform. We evaluate these terms n the next three sectons...1. Determnaton of 1:: and p 1:: The Jacobans J ; = 1; :::;, have been calculated above (,, ). The matrces are dened as follows: Ba? B^a ; = 1; :::; ; ()! where a s the lnear acceleraton of the body, ^a the cross product matrx form of a as dened n equaton (1), and and as dened n equatons (1) and (1). Usng equatons () and (), we can compute : = q g 1 q?q (8) We can now determne the elements J T of the matrx 1:::

6 J T 1 1 =. J T = q q ; : (9) As expected, for the bodes 1 to, there s no contrbuton of nertal terms. These terms can be cancelled, reducng correspondngly the parameter vector. Ths gves: 1:: q 1 q q q q q ; () p 1:: [m 1 ; m ; :::; m ] T : (1)... Frcton terms For the Hexaglde, we wll assume that the frcton s located at the motorzed jonts and s composed of vscous and Coulomb frcton: b ( _q ) b sgn( _q ) + c _q ; = 1; :::; : () Usng the Jacoban J of the motor dened n equatons (,, ), we nd and b sgn( _q 1 ) _q 1 : : : sgn( _q ) _q () p b [b 1 ; c 1 ; :::; b ; c ] T : ()... Determnaton of and p For body (platform), the parameters (x T,! T, _! T ) needed to nd have been calculated n equatons (8), () and (). We have: BaT B = T R? B^a B T T R B T R ; ()! wth B a T = B x T? G, G = [; ;?g] T, and! and calculated usng the components of T! T and T _! T n frame T. The Jacoban matrx J comes from equaton (1). We obtan nally and J T () p [m ; m r ;x ; m r ;y ; m r ;z ;... Complete form We nally nd = p I ;xx ; I ;xy ; :::; I ;zz ] T : () 1:: b p 1:: p b p (8) wth 1:: and p 1:: dened by equatons (, 1), b and p b n (, ), and and p n (, ). Ths relatvely compact dynamc equaton can be used for nonlnear control of the Hexaglde. Ths wll be shown n the next secton.. NONLINEAR ADAPTIVE CONTROL Algorthms from nonlnear adaptve control theory based on the mnmzaton of the trackng error functon (Crag, 1988; Slotne and L, 198; Sadegh and Horowtz, 199) provde the nonlnear control and the dynamcal calbraton smultaneously. In the followng, we wll use the Adaptve FeedForward Controller (AFFC) of (Sadegh and Horowtz, 199). The control law of the AFFC s (gure ): = (q d ; _q d ; q d ) p + F B ; (9) where F B s the jont lnear feedback porton of the controller: F B K p e + K d _e () where e q d? q s the error vector between the desred and actual jont postons and K p and K d are dagonal matrces wth postve terms. In the AFFC, the adaptaton s performed by gradent descent of the trackng error functon whch gves the learnng rule E = (? p) ; (1) ep new ep old + p ; p? T F B ; ()

7 q d, q d, q d nverse dynamcs I,yy q d + - q e PD τ FB + τ FF + τ robot q, q, q Fg.. Functonal scheme of the adaptatve feedforward controller. nerta [kgm] 1 1 I,xx I,zz mass [kg] / frcton [N] 1 1 c1..c m1.. mrz tme [s] m b1..b Fg.. Learnng the frcton and masses usng a translatonal movement wth? a postve dente matrx composed of the learnng factors tme [s] Fg.. Learnng the nerta tensor by performng pure rotatons m 1:: b 1:: (mn/max) c 1:: (mn/max) m [kg] [N] [N] [kg] actual learned. 11./1.9 1./ m r z I ;xx I ;yy I ;zz [kgm] [kgm] [kgm] [kgm] actual learned Table 1. Comparson of learned wth actual parameters p [m 1:: ; b 1 ; c 1 ; :::; b ; c ; m ; m r z ; I ;xx ; I ;yy ; I ;zz ] T : (). SIMULATION RESULTS Ths secton presents some smulatons performed n order to test the nonlnear adaptve controller approach desgned for the Hexaglde. To smplfy the learnng of the parameters, the sx masses of the slders whch should all be equal are collected together n a sngle parameter m 1::. Because they are more subject to varaton, the frcton terms are however consdered ndvdually. The frame attached to the TCP s supposed to be orented such that the nerta moments I ;xy, I ;xz and I ;yz are equal to zero. We further suppose that r = [; ; r z ] T,.e. that only the poston of the center of mass n the z drecton of frame T has to be learned. Thus, the matrx and the parameter vector p can be rewrtten as q 1 q q b J T q q q ; () wth b and J T dened n equaton () and () respectvely, and Fgures and show how the parameters have been learned. Table 1 compares the learned parameters wth ther real value. In ths computer-smulaton the dynamc parameters are learned n a relatvely short tme and wth a good precson. It has however to be mentoned that a smpled model of the dynamcs of the machne has been used, ncludng however vscous and Coulomb frcton terms. Furthermore, no other perturbatons such as nose n the measurement data have been used. Because such eects are nherent to a real applcaton, the learnng process won't probably be so fast. Ths smulaton presents therefore only an optmal case, but demonstrates the possblty to apply nonlnear adaptve control to complex parallel robots.. CONCLUSION Fndng a lnear form of the dynamc equaton of parallel robots s a dcult task, especally when a real-tme mplementaton s needed. In ths paper, we presented a new method based on the vrtual work prncple. It consders each body separately n a body-orented approach. Jacoban

8 matrces are used to project the forces and torques of each body onto the space of actuated jonts. An advantage of the method s that smplfyng hypothess can be used at the very begnnng already. As an example, the method was appled to the Hexaglde, a dof fully parallel platform. It led to compact equatons whch can be used n a nonlnear adaptve control scheme. Smulaton results are presented demonstratng the possblty to apply nonlnear adaptve control to complex parallel robots. 8. REFERENCES An, C.H., C.G. Atkeson and J.M. Hollerbach (198). Estmaton of nertal parameters of rgd body lnks of manpulators. In: th Conference on Decson and Control. Bhattacharya, S., H. Hatwal and A. Ghosh (199). An on-lne parameter estmaton scheme for generalzed stewart platform type parallel manpulators. Mech. Mach. Theory (1), 9{89. Codourey, A. (1991). Contrbuton a la commande des robots rapdes et precs, applcaton au robot Delta a entranement drect. PhD thess. Ecole Polytechnque Federale de Lausanne. Codourey, A. and E. Burdet (199). A bodyorented method for ndng a lnear form of the dynamc equaton of fully parallel robots. In: IEEE Internatonal Conference on Robotcs and Automaton. Crag, J.J. (1988). Adaptve Control of Mechancal Manpulators. Addson-Wesley. Crag, J.J. (1989). Introducton to Robotcs, Mechancs and Control. Addson-Wesley. Gauter, M. (199). Numercal calculaton of the base nertal parameters. In: IEEE Internatonal Conference on Robotcs and Automaton. pp. 1{1. Gauter, M. and W. Khall (199). Drect calculaton of mnmum set of nertal parameters of seral robots. IEEE Transactons on Robotcs and Automaton (), 8{. Goldenberg, A.A., X. He and S.P. Ananthanarayanan (199). Identcaton of nertal parameters of a manpulator wth closed knematc chans. IEEE Transactons on Systems, Man, and Cybernetcs (), 99{8. Honegger (199). Steuerung und regelung ener stewart-plattform als werzeugmaschne. Techncal report. Insttut fuer Robotk, ETH- Zuerch. Khall, W. and F. Benns (199). Symbolc calculaton of the base nertal parameters of closed-loop robots. The Internatonal Journal of Robotcs Research 1(), 11{118. Khall, W. and J.F. Klennger (198). Mnmum operatons and mnmum parameters of the dynamc models of tree structure robots. IEEE Transactons on Robotcs and Automaton (), 1{. Khall, W., F. Benns and M. Gauter (1989). Calculaton of the mnmum nertal parameters of tree structure robots. In: rd ICAR Conference. pp. 189{1. Khosla, P.K. (198). Real-tme Control and Identcaton of Drect-drve Manpulators. PhD thess. Carnege-Mellon Unversty. Khosla, P.K. and T. Kanade (198). Parameter dentcaton of robot dynamcs. In: th Conference on Decson and Control. Sadegh, N. and R. Horowtz (199). Stablty and robustness analyss of a class of adaptve controller for robotc manpulators. Internatonal Journal of Robotcs Research 9(), {9. Scavcco, L. and B. Sclano (199). Modelng and Control of Robot Manpulators. The McGraw- Hll Companes, Inc. Slotne, J.-J.E. and W. L (198). Adaptve manpulator control, a case study. In: IEEE Internatonal Conference on Robotcs and Automaton. Walker, M.W. (199). Adaptve control of manpulators contanng closed knematc loops. IEEE Transactons on Robotcs and Automaton (1), 1{19. Wegand, A., M. Hebsacker and M. Honegger (199). Parallele knematk und lnearmotoren: Hexaglde - en neues, hochdynamsches werkzeugmaschnenkonzept. Technsche Rundschau Transfer.