Let also U =  up  +Uo  (14) represent a potential elastic energy with U  -ipTKpp p-2 U, = 2CdTK,Zd,  (16) where Kp  = KT  and  KO  = KT  are positive definite matrices.  In  particular, Up expresses the elastic energy of  a  three-dimensional linear spring of  constant stiffness Kp  acting on the position displacement p.  As  for  the contribution U,,  even though this is expressed in terms of the vector part of  a quaternion, it can be seen that it has a clear geometrical meaning.  In fact, the following equality can be proven 2ZdTK,Zd  = -Tr(RzAoR,)  +%(Ao)  (17) with KO  = A, -  Tr(Ao)I,  (18) where  the  term on  the  right-hand  side  of (17) is  the rotational elastic  energy defined in [  101  associated to a  torsional spring of  stiffness KO  acting as to align frame Rd  with R,;  such stiffness is assumed to be constant  in the desired frame. Force and moment contributions can be derived by  consid- ering the associated powers.  Taking the time derivative of (1  1)  and accounting  for (12) and (1  3) yields (19) where are respectively the inertial force and moment. Also, takmg the time derivative of (14) and accounting  for (15) and (16) yields ir=  fgp+pE  dT-d  w where fE = Kpp pi =  2ET(fj,  Zd)KaZd are respectively the elastic force and moment. Suitable dissipative contributions  can be added as fD = Dpp d pD  -DOG  -  , where D, = DF  and Do  = DT  are positive definite con- stant matrices characterizing a translational and rotational damping  at the end effector,  respectively. Therefore, a mechanical impedance  at the end effector can be defined in terms of its translational part and its rotational part: MPp  + Dpp  + Kpp  =  f  (27) (28) .d Ma& + DLGd + KLZd = pd, where DL  = Do  - MaS(w,d) KL  =  2ET(fj,  Zd)Ka.  (30) (29) are the resulting time-varying  rotational damping  and stiff- ness matrices. The  above  energy-based formulation guarantees that the mapping  between the vector of end-effector linear (angular) velocity and the vector of contact force (moment)  is strictly passive.  In fact, let u,=I,+u,  (31) ua=I,+ua  (32) respectively represent the Hamiltonian contributions asso- ciated to the translational and rotational motion, which are positive definite functions.  Taking  the time derivative of (3  l), (32) and accounting  for (12), (13) and (15), (16) along with (27)-(30)  yields -T '&!,  =  -p  Dpp  +  f  'p '&!,  = -WdTDac;rd  +  pdTGd (33) (34) which implies strict passivity of  the mappings f c-)  and pd  I+  Gd  [14]. It can be shown that the impedance  in (27) and (28) guaran- tees asymptotic convergence  to zero of end-effector position and orientation errors in the case of free motion. If  f =  0, from (33) it is $ =  0 and thus it is immediate  to see in (27) that p  asymptotically tends to 0 from La Salle's  theorem. On the other hand,  if p  = 0, from (34) it is Gd = 0 and thus it can be seen in  (28) that Ed  asymptotically  tends to the invariant set described  by (35) It can be proven that the only stable equilibrium  is Zd  =  0, implying Rd =  I  [6]. 3. Inverse dynamics control The dynamic  model of an n-dof rigid robot manipulator  can be written in the well-known form ,U$  = 2BT(fi,  Zd)KaZd  =  0. B(q)ii+C(q,  4)4+4q, 4)+9(4) =  u-JT(q)k  (36) where q is the  (n x  1)  vector of joint variables, B  is the (n x n)  symmetric positive definite inertia matrix, C4 is the (n  x 1)  vector of Coriolis and centrifugal torques, d is the (n  x 1)  vector of friction torques, g  is the (n  x 1)  vector of gravitational torques, U  is the (n  x  1)  vector of driving torques, h = [ f  pT  IT  is the (6 x  1)  vector of contact forces  exerted by the end effector on  the  environment,  and J is the (6  x n)  Jacobian matrix relating  joint velocities 4. to  the (6 x 1)  vector of end-effector velocities v =  [ +T  wT IT, i.e. which is assumed to be nonsingular. According to &e well-known concept  of inverse dynamics, the driving  torques are chosen as 'U  =  J(q)O,  (37) U  = B(s)Jt(n)(.  -  4q,  $4) + C(q,  414 + &,  4) +  S(Q)  +  JT(q)h,  (38) where Jt  is the right pseudoinverse of  J, ^d  denotes the available estimate of the friction torques, and h  is the mea- sured contact  force.  Notice  that it  is reasonable to  as- sume accurate compensation of  the terms in the dynamic model (36), e.g. as obtained by  a parameter identification technique, except for the friction torques. Substituting the control law (38) in (36) and accounting for the time derivative of (37)  gives ir=a-?J  (39) that  is a resolved end-effector acceleration for which  the term (40) can be  regarded as a disturbance. In  the case of mismatching on other terms in the dynamic model (36), such a distur- bance would include additional contributions. In  order to match the desired impedance behavior es- tablished  by  (27)  and  (28),  t 六自由度电阻机器人英文文献和中文翻译(2):http://www.751com.cn/fanyi/lunwen_35660.html