Adaptive H Tracking Control Design via Neural Networks of a Constrained Robot System

Transcription

1 Proceedigs of the 44th IEEE Coferece o Decisio ad Cotrol, ad the Europea Cotrol Coferece 25 Seville, Spai, December 12-15, 25 WeIB196 Adaptive H Trackig Cotrol Desig via Neural Networks of a Costraied Robot System Adre Petroilho, Adriao A G Siqueira ad Marco H Terra* Abstract I this paper, a oliear adaptive eural etwork trackig cotrol a guarateed H performace is proposed for a costraied robot maipulator plat ucertaities The eural etwork is used to lear the ukow dyamics by a adaptive algorithm Moreover, a force sesor is built to measure the forces ad torques betwee the experimetal robot UArm II ed-effector ad the eviromet Fially, results obtaied from the implemetatio of the proposed cotroller i the maipulator UArm II, uder a costraied movemet, are preseted I INTRODUCTION With the growth of automatio i idustries, the umber of robots used to perform tasks such as writig, scribig ad gridig, where the robot ed-effector keeps cotact its eviromet, has bee cotiuously icreasig I these mechaical systems, amed costraied robots, there exist variables that deteriorate the represetativity of the omial model, such as parametric ucertaities, umodelled dyamics, ad exteral disturbaces Therefore, the study of cotrol techiques to deal this kid of problem deserves special attetio A adaptive eural etwork trackig cotrol a guarateed H performace is developed i [3] for ucostraied maipulators; the eural etwork is employed first to approximate a cotiuous fuctio defied by the robot dyamic equatio ad torque disturbaces; the H cotroller is desiged to atteuate the effect of the approximatio error (geerated by the eural etwork algorithm ad cosidered as the disturbace to be atteuated) o the trackig error A compariso of this adaptive eural etwork trackig cotrol model-based cotrollers, based o results obtaied from a actual maipulator, is performed i [6] A fuzzy logic cotroller equipped a adaptive algorithm is preseted i [4] to achieve H performace for both holoomic ad oholoomic costraied robots As i [3], the approximatio error icludes the robot dyamics ad torque disturbaces All these H cotrol strategies are difficult to be implemeted i a actual robot The approximatio errors of these cotrollers iclude the robot dyamics ad it is difficult to verify, as assumed i [3], the L 2 property of these errors, that is essetial to guaratee the H atteuatio criterio I [2], a hybrid motio/force trackig cotrol desig a adaptive scheme that guaratees H performace for holoomic costraied robots is proposed For this approach *The authors are the Electrical Egieerig Departmet - Uiversity of São Paulo at São Carlos, CP359, São Carlos, SP, , Brazil s: apetroi,siqueira,terra@seleescuspbr it is required a precise kowledge of the etire robot dyamic model i order to guaratee the best performace of this cotroller I this paper, a adaptive eural etwork cotroller H performace is proposed to holoomic costraied robots It icludes the variable structure cotrol (VSC) proposed i [1] to elimiate the coditio of square-itegrability of the approximatio error Moreover, it is cosidered that a omial part of the robot dyamics is kow for the cotrol algorithm I this case, the eural etwork is employed to approximate oly the ucertai part of the robot dyamics The proposed adaptive eural etwork-based cotroller is implemeted i the experimetal maipulator UArm II [7], which performs a costraied movemet I order to measure the forces betwee the UArm II ed-effector ad the eviromet, a suitable force sesor device, which uses oe-directioal piezoelectric force sesors, was desiged ad built i our laboratory This paper is orgaized as follows: the reduced order model of a costraied robot is preseted i Sectio II; a adaptive eural etwork-based cotroller guarateed H performace for costraied robots is proposed i Sectio III; ad, fially, a descriptio of the force sesor device ad experimetal results are preseted i Sectio IV II REDUCED ORDER MODEL FOR HOLONOMIC CONSTRAINED ROBOTS Cosider a robot maipulator subject to holoomic costraits o its ed-effector The equatio of the m costraits is of the form φ(q)= where q R are the joit positios ad φ(q) : R R m is a smooth fuctio The dyamic equatio of the costraied robot is give from Lagrage theory as M(q) q +C(q, q) q + G(q)=τ + J T (q)λ + τ d (1) where M(q) R is the symmetric positive defiite iertia matrix, C(q, q) R is the Coriolis ad cetripetal matrix, G(q) R are the gravitatioal torques, τ R are the applied torques, λ R m is a vector of geeralized Lagragia multipliers associated the costraits, J(q)= ( φ/ q) R m is the Jacobia matrix, ad τ d R deotes the exteral disturbaces If the Jacobia matrix J(q) has full row rak m for all q R, the the vector q ca be partitioed as q = [(q 1 ) T (q 2 ) T ] T,whereq 1 =[q 1 1 q1 m ]T describes the costraied motio of the maipulator ad q 2 =[q 2 1 q2 m] T /5/$2 25 IEEE 5528

2 deotes the remaiig joits variables Moreover, there exist a ope set Ω c R m ad a fuctio σ : Ω c R m such that φ(q 1,σ(q 1 )) = Therefore, the reduced form of costraied robot (1) is give as ([2], [4] ad [8]) M(q 1 )L(q 1 ) q 1 +C L (q 1, q 1 ) q 1 + G(q 1 )=τ + J T (q)λ + τ d (2) where L(q 1 )= [ I m σ(q 1 ) q 1 ], I m meas idetity matrix ad C L (q 1, q 1 ) q 1 = M(q 1 )L(q 1 )+ C(q 1, q 1 )L(q 1 ) The matrices M(q 1 ), C(q 1, q 1 ) ad G(q 1 ) i (2) are obtaied by substitutig q 2 = σ(q 1 ) ad q = L q 1 i M(q),C(q, q) ad G(q) i (1), respectively III ADAPTIVE NEURAL NETWORK-BASED H CONTROL To obtai the error dyamic equatio, it is defied the followig trackig error x(t), see [2] ad refereces therei for more details, x(t) = [ ] [ x1 (t) = q 1 (t) q 1 d (t) ] x 2 (t) q 1 (t) q 1 d (t)+p(q1 (t) q 1 d (t)) (3) for some costat p >, where q 1 d (t) is a desired joit trajectory for q 1 (t) its time derivatives q 1 d (t), q1 d (t) ad d 3 (q 1 d (t))/dt3 bouded, ad satifyig φ(σ(q 1 (t)),q 1 (t)) = Premultiplyig both sides of (2) by L T (q 1 ),weget A L (q 1 ) q 1 + L T (q 1 )C L (q 1, q 1 ) q 1 + +L T (q 1 )G(q 1 )=L T (q 1 )(τ + τ d ) where A L (q 1 )=L T (q 1 )M(q 1 )L(q 1 ) Observe that L T (q 1 ) is formulated such that L T (q 1 )J T (q)= (4) The the error dyamic equatios ca be obtaied as ẋ 1 = px 1 + x 2 A L (q 1 )ẋ 2 = L T (q 1 )C L (q 1, q 1 )x 2 + L T (q 1 )( F(x e )+τ + τ d ) (5) where x e =[(q 1 ) T ( q 1 ) T (q 1 ) T d ( q 1 ) T d ( q 1 ) T d ]T ad F(x e ) = M(q 1 )L(q 1 )( q 1 d pẋ 1 )+C L (q 1, q 1 )( q 1 d px 1)+G(q 1 ) A parametric ucertaity ca be itroduced i (1) dividig the parameter matrices M(q), C(q, q), ad G(q) ito a omial ad a perturbed part M(q)=M (q)+ M(q) C(q, q)=c (q, q)+ C(q, q) G(q)=G (q)+ G(q) where M (q), C (q, q), adg (q) are the omial matrices ad M(q), C(q, q), ad G(q) are the parametric ucertaities The, F(x e ) ca be expressed as F(x e )=F (x e )+ F(x e ) (6) where F (x e ) is the omial part of F(x e ) computed M (q), C (q, q), adg (q) ad F(x e ) is the ucertai part computed M(q), C(q, q), ad G(q) A adaptive eural etwork, F(x e,θ), is used to estimate the term F(x e ) i (6), where Θ is a vector cotaiig the tuable etwork parameters The procedure to adjust the eural etwork, preseted i this sectio, was developed i [3] ad [1] Defie eural etworks F k (x e,θ k ), k = 1,, composed of oliear euros i every hidde layers ad liear euros i the iput ad output layers, the adjustable weights Θ k i the output layers The sigle-output eural etworks are of the form ) ad F k (x e,θ k ) = ξ k = p k H ( 5 w k ij x e j + m k i j=1 Θ ki = ξ T k Θ k (7) ( ) H 5 j=1 wk 1 j x e j + m k 1 ( ) H 5 j=1 wk p k j x e j + m k p k Θ k = Θ k1 Θ kp k where p k is the umber of euros i the hidde layers, the weights w k ij ad the biases mk i for 1 i p k,1 j 5 ad 1 k are assumed to be costat ad specified by the desiger ad H() is the hyperbolic taget fuctio H(z)= ez e z e z + e z The complete eural etwork ca be deoted by F(x e,θ) = F 1 (x e,θ 1 ) F (x e,θ ) = ξ1 T Θ 1 ξ T Θ ξ T 1 Θ 1 = ξ2 T Θ 2 ξ T Θ = ΞΘ (8) The followig assumptios, defied i [1], are used to guaratee the cotrol law stability based o eural etwork developed i Theorem 31: 5529

3 1) There exists a parameter value Θ Ω θ such that F(x e,θ ) ca approximate F(x e ) as close as possible, where Ω θ is a pre-assiged costrait regio defied as Ω θ = {Θ Θ T Θ M θ, M θ > } 2) There exists a fuctio k(x e ) > such that (δf(x e )) i k(x e ), for all 1 i, where δf(x e )= F(x e,θ ) F(x e ) Ad the followig defiitios are made λ c = λd k λ (λ λ d ) ad E = [ ] I( m) ( m) (9) m ( m) for some k λ >, where λ d (t) is a desired multiplier related to a desired costraied force f d (t), thatis, f d (t) = J T (q d (t))λ d (t) Theorem 31: Cosider the reduced model (2) plat ucertaities ad exteral disturbaces, ad desired referece trajectories q d (t) ad λ d (t) The adaptive eural etworkbased cotroller give by ρξ T Lx 2 if Θ < M θ or ( Θ = M θ ad x Θ T 2 = LT ΞΘ ) ρξ T Lx 2 + ρ xt 2 LT ΞΘ Θ if Θ = M Θ 2 θ ad x T 2 LT ΞΘ < (1) τ = F (x e )+ΞΘ k Ex 2 k(x e )sg(lx 2 ) J T λ c (11) ((q(), q()) are cosidered bouded ad satisfy Φ(q()) = ad J(q() q() =) achieves the followig performace for a suitable choice of the costat gai k : (1) Θ(t) Ω θ ad all the variables q(t), q(t) ad τ(t) are bouded for all t (2) The followig H performace holds x(t) 2 Q V ()+γ 2 τ d (t) 2, T (12) τ d () L 2 [, ), whereq is a weightig matrix ad γ is the atteuatio level (3) The steady-state force error λ λ d is iversely proportioal to the value of k λ + 1 (4) If τ d () L 2 [, ) L [, ), the we ca coclude that lim t (q 1 (t) q 1 d (t)) = adlim t ( q 1 (t) q 1 d (t)) = Proof This proof follows the lie of the proof preseted i [2] for adaptive trackig cotrol, where the liear parametrizatio property is used istead of the adaptive eural etwork approach The Lyapuov fuctio cadidate is chose as V (t,x, Θ)= α 2 xt 1 x xt 2 A L x ρ Θ T Θ (13) for some α >, where Θ = Θ Θ Takig ito accout the cotrol law (11) ad usig the property of skew-symmetry of Ȧ L 2L T C L x 2, the derivative of V(t,x, Θ) alog (5) is give as V(t,x, Θ)= α px T 1 x 1 + αx T 1 x 2 k x T 2 x 2 + x T 2 L T τ d +(x T 2 LT Ξ + 1 Θ T ) Θ + x T 2 ρ LT ( k(x e )sg(lx 2 )+δf(x e )) (14) The above equatio is differet from that oe preseted i [2], sice the eural etwork matrix Ξ is used i place of the regressio matrix Y ad the VSC term ( k(x e )sg(lx 2 )) is added to elimiate the limitatio o the approximatio error discussed i [1] From the defiitio of the update law (1), which is a stadard projectio algorithm [5], it ca be coclude that (x T 2 LT Ξ + 1 ρ Θ T ) Θ adθ(t) Ω θ,forall t ifθ() Ω θ Cosiderig Assumptio (2), it ca be guarateed that x T 2 LT ( k(x e )sg(lx 2 )+δf(x e )) k(x e ) (Lx 2 ) i + (δf(x e ))) i (Lx 2 ) i Hece, cosiderig the costrais obtaied i [2] for k ad p, (14) becomes V x 2 Q + γ2 τ d 2 (15) Itegratig from t = tot = T,siceV(T,x(T), Θ(T )), the above iequality leads to x 2 Q dt V(,x(), Θ()) + γ 2 τ d 2 dt Hece, the H performace is achieved The rest of the proof follows [2] IV RESULTS I this sectio we are goig to show the experimetal results obtaied the applicatio of the cotroller developed i Sectio III o our experimetal maipulator UArm II (Uderactuated Arm II), desiged ad built by H Be Brow, Jr of Pittsburgh, PA, USA [7] This 3- lik plaar maipulator cotais i each joit a DC motor, a break ad a optical ecoders quadrature decodig used to measure the joit positios, Fig 1 Joit velocities are obtaied by umerical differetiatio ad filterig The maipulator kiematic ad dyamic omial parameters, which are used to compute the term F (x e ),areshowi Table I The matrices M (q) ad C (q, q) ca be see i the Appedix TABLE I ROBOT PARAMETERS i m i I i l i lc i f i (kg) (kgm 2 ) (m) (m) (kgm 2 /s) Sice the forces betwee the robot ed-effector ad the eviromet are used i the cotrol law, a force sesor device was desiged ad built to measure the ormal ad tagetial 553

4 Fig 1 Maipulator UArm II Fig 3 Force Sesor Device coupled to UArm II ed-effector Fig 2 Force Sesor Device forces ad the z-directio momet applied i the UArm II ed-effector by the costrait surface described i the followig The device has a alumium-made sesor board where six FSG-15N1A force sesors from Hoeywell ad the acquisitio board are fixed As the FS series force sesor measures oly positive force values, two sesors are disposed i a opposite way for each directio to measure positive ad egative values, see Fig 2 Two force sesor pairs are used to measure the ormal force ad the z-directio momet, give, respectively, by F = F 1 +F 2 ad M z =(F 2 F 1 )r If oly traslatioal movemet is performed o the ormal directio, F 1 = F 2,adthez-directio momet is zero Otherwise, if oly rotatioal movemet is performed, F 1 = F 2 ad the ormal force is zero Figure 3 shows the experimetal maipulator UArm II the force sesor device coupled The costrait surface for the robot ed-effector is a segmet of a straight lie o the X-Y plae, ed-effector orietatio perpedicular to the costrait lie, that is, the orietatio must remai i a costat value c givebythe lie icliatio β The equatios of the m = 2 costraits are [ ] l1 s φ(q)= 1 l 2 s 12 l 3 s β [l 1 c 1 + l 2 c 12 + l 3 c 123 ]+b q 1 + q 2 + q 3 c [ ] = where b is the liear coefficiet of the costrait lie, s ik = si(q i + + q k ), c ik = cos(q i + + q k ), q i is the agular positio of joit i, adl i is the legth of the i-lik Hece, φ : R 3 R 2, ad the Jacobia matrix, J(q) = φ/ q, is give as [ ] J11 J J = 12 J 13 J 21 J 22 J 23 J 11 = l 1 c 1 l 2 c 12 l 3 c 123 β [l 1 s 1 + l 2 s 12 + l 3 s 123 ] J 12 = l 2 c 12 l 3 c 123 β [l 2 s 12 + l 3 s 123 ] J 13 = l 3 c 123 β [l 3 s 123 ] J 21 = J 22 = J 23 = 1 Defiig q 1 =[q 1 ] ad q 2 =[q 2 q 3 ], the matrix L(q) of the costrait lie is 1 L(q)= [cos(q 1)+cos(q 1 +q 2 )+β (si(q 1 )+si(q 1 +q 2 ))] [cos(q 1 +q 2 )+β si(q 1 +q 2 )] [cos(q 1 )+cos(q 1 +q 2 )+β (si(q 1 )+si(q 1 +q 2 ))] [cos(q 1 +q 2 )+β si(q 1 +q 2 )] 1 Observe that l 1 = l 2 = l 3 ad J(q) T L(q) T = The iitial ad fial coordiates of the movemet are (x,y )= (45,3) m ad (x(t ),y(t )) = (49,22) m, respectively I this case, β = 2, b = 12, ad c = 266 The referece trajectory for the joit variables q d (t) is a fifthdegree polyomial, trajectory duratio time T = 4 s It is desired that o force acts o the ormal directio of the costrai lie ad o momet acts o the z-directio, that is, λ d =[(F ) d (M z ) d ] T =[] T The level of atteuatio ad the weightig matrix Q defied for the cotroller are γ = 3adQ = 5I 2, respectively The selected cotrol gais are p = 2, k = 15, k λ = 8, ad ρ = 45, M θ = 1 The fuctio k(x e ) is defied as k(x e )=2 x x

5 The followig auxiliary variable is defied for computatio of F k (x e,θ k ) xx = (q i q d i )+ ( q i q d i ) The matrix Ξ ca be computed as Ξ = ξ 1 T ξ2 T ξ3 T ad ξ 1 =[ξ 11,,ξ 17 ] T, ξ 2 =[ξ 21,,ξ 27 ] T, ξ 3 =[ξ 31,,ξ 37 ] T ξ 11 = ξ 21 = ξ 31 = exx 15 e xx+15 e xx 15 + e xx+15 ξ 12 = ξ 22 = ξ 32 = exx 1 e xx+1 e xx 1 + e xx+1 ξ 13 = ξ 23 = ξ 33 = exx 5 e xx+5 e xx 5 + e xx+5 ξ 14 = ξ 24 = ξ 34 = exx e xx e xx + e xx ξ 15 = ξ 25 = ξ 35 = exx+5 e xx 5 e xx+5 + e xx 5 ξ 16 = ξ 26 = ξ 36 = exx+1 e xx 2 e xx+2 + e xx 2 ξ 17 = ξ 27 = ξ 37 = exx+15 e xx 15 e xx+15 + e xx 15 q i (16) Note that, these defiitios, 7 euros i the hidde layer are selected for the eural etworks the weights w k ij assumig the values 1 or 1 ad the biases m i assumig the values 15, 1, 5,, 5, 1, 15 The etwork parameters Θ are defied by Θ = Θ 1 Θ 2 Θ 3 Θ 1 =[Θ 11 Θ 12 Θ 13 Θ 14 Θ 15 Θ 16 Θ 17 ] T Θ 2 =[Θ 21 Θ 22 Θ 23 Θ 24 Θ 25 Θ 26 Θ 27 ] T Θ 3 =[Θ 31 Θ 32 Θ 33 Θ 34 Θ 35 Θ 36 Θ 37 ] T The experimetal results (joit positio ad torque) for the H cotrol are show i Figures 4 ad 5, respectively Figure 6 shows the measured ormal force ad momet at the robot ed-effector Fially, the ed-effector X Y positio is showifigure7 Joit Positio (º) Torque (Nm) Joit 1 Joit 2 Joit 3 Desired Time (s) Joit 1 Joit 2 Joit 3 Fig 4 Joit Positio ( ) Time (s) Fig 5 Torque (Nm) V CONCLUSIONS The most importat cotributio of this paper are the experimetal results obtaied a costraied robot maipulator subject to parametric ucertaities ad exteral disturbaces A adaptive eural etwork-based cotrol trackig performace is developed to atteuate the effects of the disturbaces o the trackig errors Also, a force cotrol is employed to track the costrait forces actig at the robot ed-effector to a desired value The force sesor device developed i this paper, that was built i our laboratory, presets simple desig ad structure, that ca be a ecoomical advatage i compariso available force sesors REFERENCES [1] Y C Chag, Neural etwork-based H trackig cotrol for robotic systems, IEE Proceedigs o Cotrol Theory Applicatios, vol147, o 3, 2, pp [2] Y C Chag ad B S Che, Adaptive Trackig Cotrol Desig of Costraied Robot Systems, Iteratioal Joural of Adaptive Cotrol Sigal Processig, vol 12, 1998, pp [3] Y C Chag ad B S Che, A Noliear Adaptive H Trackig Cotrol Desig i Robotic Systems via Neural Networks, IEEE Trasactios o Cotrol Systems Techology, vol 5, 1997, pp [4] YCChagadBSChe,RobustTrackigDesigsforBothHoloomic ad Noholoomic Costraied Mechaical Systems: Adaptive Fuzzy Approach, IEEE Trasactios o Fuzzy Systems, vol 8, 2, pp [5] H K Khalil, Adaptive ooutput feedback cotrol of oliear systems represeted by iput-output models, IEEE Trasactio o Automatic Cotrol, vol 41, 1996, pp

6 Force ad Momet Positio Y (m) Force (N) Momet (Nm) Time (s) Fig Force ad Momet Applied at the robot ed-effector Real Desired Positio X (m) Fig 7 Positio X, Y of the robot ed-effector [6] A A G Siqueira, A Petroilho ad M H Terra, Adaptive oliear H techiques applied to a robot maipulator, Proceedigs of the IEEE Coferece o Cotrol Applicatios, Istambul, Turquia, 23, pp [7] A A G Siqueira ad M H Terra Noliear ad Markovia H cotrols of uderactuated maipulators IEEE Trasactios o Cotrol Systems Techology, v 12, 6, 24, p [8] C Y Su, Y Stepaeko ad T P Leug Combied adaptive ad varible strucuture cotrol of costraied robots, Automatica, vol 31, 1995, pp APPENDIX The matrices M (q) ad C (q, q) for the plaar maipulator are give by: M (q)= M 11(q) M 12 (q) M 13 (q) M 21 (q) M 22 (q) M 23 (q) M 31 (q) M 32 (q) M 33 (q) M 11 (q)=m 1 lc m 2 (l1 2 + lc l 1 l c2 cos(q 2 )) + m 3 (l1 2 + l2 2 + lc l 1 l 2 cos(q 2 )+2l 2 l c3 cos(q 3 )) + 2m 3 l 1 l c3 cos(q 2 + q 3 )+I 1 + I 2 + I 3, M 12 (q)=m 2 (lc l 1 l c2 cos(q 2 )) + m 3 (l2 2 + lc l 1 l 2 cos(q 2 )) + m 3 (l 1 l c3 cos(q 2 + q 3 ) + 2l 2 l c3 cos(q 3 )) + I 2 + I 3, M 13 (q)=i 3 + m 3 (lc l 1 l c3 cos(q 2 + q 3 )+l 2 l c3 cos(q 3 )), M 21 (q)=m 12 (q), M 22 (q)=i 2 + I 3 + m 2 (lc 2 2 )+m 3 (l2 2 + l2 c 3 + 2l 2 l c3 cos(q 3 )), M 23 (q)=i 3 + m 3 (lc l 2 l c3 cos(q 3 )), M 31 (q)=m 13 (q), M 32 (q)=m 23 (q), M 33 (q)=i 3 + m 3 (lc 2 3 ), ad C (q, q)= C 11(q, q) C 12 (q, q) C 13 (q, q) C 21 (q, q) C 22 (q, q) C 23 (q, q), C 31 (q, q) C 32 (q, q) C 33 (q, q) C 11 (q, q)= [(m 2 l 1 l c2 si(q 2 )+m 3 l 1 l 2 si(q 2 ) + m 3 l 1 l c3 si(q 2 + q 3 )) q 2 +(m 3 l 1 l c3 si(q 2 + q 3 ) + m 3 l 2 l c3 si(q 3 )) q 3 ], C 12 (q, q)= [(m 2 l 1 l c2 si(q 2 )+m 3 l 1 l 2 si(q 2 ) + m 3 l 1 l c3 si(q 2 + q 3 ))( q 2 ) +(m 3 l 1 l c3 si(q 2 + q 3 )+m 3 l 2 l c3 si(q 3 )) q 3 ], C 13 (q, q)= [(m 3 l 1 l c3 si(q 2 + q 3 )+m 3 l 2 l c3 si(q 3 )) ( q 2 + q 3 )], C 21 (q, q)=(m 2 l 1 l c2 si(q 2 )+m 3 l 1 l 2 si(q 2 ) + m 3 l 1 l c3 si(q 2 + q 3 )) q 1 m 3 l 2 l c3 si(q 3 ) q 3, C 22 (q, q)= m 3 l 2 l c3 si(q 3 ) q 3, C 23 (q, q)= m 3 l 2 l c3 si(q 3 )( q 2 + q 3 ), C 31 (q, q)=(m 3 l 1 l c3 si(q 2 + q 3 )+m 3 l 2 l c3 si(q 3 )) q 1 + m 3 l 2 l c3 si(q 3 ) q 2, C 32 (q, q)=m 3 l 2 l c3 si(q 3 )( q 2 ), C 33 (q, q)=, where m i, l i, l ci, I i, q i ad q i, are the mass, the legth, the ceter of mass, the iertia mometum, the agular positio ad the agular velocity of the i-lik, respectively 5533