Experiments of Spatial Impedance Control

1. I n t r o d u c t i o n One of the most reliable strategies to manage the interaction of the end effector of a robot manipulator with a compliant environment is impedance control [1]. The major i ty of interaction control schemes refer to three-degree-of-freedom (dof) tasks in that they handle end-effector position and contact linear force. On the other hand, in order to perform six-dof tasks, not only is a representation of end-effector orientation required, but also a suitable definition of end-effector orientation displacement to be related to the contact moment should be sought. The usual minimal representation of orientation is given by a set of Euler angles. These three coordinates, together with the three position coordinates, allow the description of end-effector tasks in the so-called operational space [2]. A drawback of this description is the occurrence of representation singularities of the analytical Jacobian [3]. These can be avoided by resorting to a four-parameter singularity-free description of end-effector orientation, e.g. in terms of a unitary quaternion. Such a description has already been successfully used for the at t i tude motion control problem of spacecrafts [4, 5] and manipulators [6]. The goal of this work is to present six-dof impedance controllers, where the impedance equation is characterized for both its translational part and its rotat ional part . In the framework of an operational space formulation, an analytical ai~proach is pursued first where the end-effector orientation displacement is merely given by the difference between the actual and the desired set of Euler angles. 94 Then, in order to relate the rotational parameters of the impedance to the task geometry, a different approach is followed where the end-effector orientation displacement is extracted from the mutual rotation matrix between the actual and .the desired end-effector frame. This can be obtained in terms of either three Euler angles or a unitary quaternion [7], where the latter allows the mechanical impedance to be derived from suitable energy contributions of clear physical interpretation [8]. In order to obtain a configuration-independent desired impedance, an inverse dynamics strategy with contact force and moment measurement is adopted leading to a resolved acceleration scheme. A modification of the basic scheme by the inclusion of an inner loop acting on the end-effector position and orientation error is devised to ensure good disturbance rejection [9], e.g. unmodeled friction. The proposed controllers are tested and critically compared in a number of experiments on a setup comprising a 6-joint industrial robot Comau SMART-3 S with open control architecture and a 6-axis force/torque wrist sensor ATI F T 130/10. 2. R e p r e s e n t a t i o n s o f O r i e n t a t i o n The location of a rigid body in space is typically described in terms of the (3 x 1) position vector p and the (3 x 3) rotation matrix / / describing the origin and the orientation of a frame attached to the body with respect to a fixed reference frame. A minimal representation of orientation can be obtained by using a set of three Euler angles ¢ = [~ 0 ¢]T. Among the 12 possible definitions of Euler angles, the X Y Z representation is considered leading to the rotation matr ix R(¢) = R~(~o)R~(O)R~(V,) (~) where R , , P~, Rz are the matrices of the elementary rotations about three independent axes of successive frames. The relationship between the time derivative of the Euter angles ¢ and the body angular velocity ca is given by w = T ( ¢ ) ¢ (2) where the transformation matrix T corresponding to the above X Y Z representation is i 0 sin ~) T ( ¢ ) --cos ~ sin ~ cos ~) (3) sin qo cos ~ cos Notice that a representation singularity occurs whenever 0 = ~7r/2. Also, in view of the choice of Euler angles X Y Z , it is T(0) = I (4) which will be useful in the following. 95 A singularity-free description of orientation can be obtained by resorting to a four-parameter representation in terms of a unitary quaternion 0 = cos (5) 2 0 e ---sin ~ r , (6) where 0 and r are respectively the rotation and the unit vector of an equivalent angle/axis description. The relationship between the time derivative of the quaternion and the body angular velocity is established by the so-called quaternion propagation: = _ ~ T ~ (7) 1 E = ~ (7, ~)~ (8) with E = ~ I + S(e), (9) being S(-) the skew-symmetric matrix operator performing vector product. 3. S p a t i a l I m p e d a n c e When the manipulator moves in free space, the end-effector is required to match a desired frame specified by Pd and Rd. Instead, when the end effector interacts with the environment, it is worth considering another frame specified by Pc and Re; then, a mechanical impedance can be introduced which is aimed at imposing a dynamic behaviour for the position and orientation displacements between the above two frames. The mutual position between the compliant and the desired frame can be characterized by the position displacement A p = Pc Pal. (10) Then, the translational part of the mechanical impedance at the end effector can be defined as M p A ~ + D p A p + K v A p = f (11) where Mp, D v, Kp are symmetric positive definite matrices describing the generalized mass, translational damping, translational stiffness, respectively, and f is the contact force at the end effector; all the above quantities are referred to a common base frame. On the other hand, the mutual orientation between the compliant and the desired frame can be characterized in different ways. With reference to an operational space formulation, the end-effector orientation displacement can be computed as A¢ = ¢¢ Cd (12) 96 where ¢¢ and Cg denote the set of Euler angles that can be extracted from Re and Rd, respectively. Then, the rotational part of the mechanical impedance at the end effector can be defined as Mo,nA~b + Do,nA~b + Ko,~A¢ = TT(¢c)tt (13) where Mo,n, Do,n, Ko,n are symmetric positive definite matrices describing the generalized inertia, rotational damping, rotational stiffness, respectively, tt is the contact moment at the end effector; all the above quantities are referred to a common base frame, and the matrix TT(¢c) is needed to transform the moment into an equivalent operational space quantity. Notice that, differently from (11), the impedance behaviour for the rotational part depends on the actual orientation of the end effector through the matrix TT(¢c). Equation (13) becomes ill-defined in the neighbourhood of a representation singularity for ¢~; in particular, at such a singularity, moment components in the null space of T T do not generate any contribution to the dynamics of the orientation displacement, leading to a possible build-up of high values of generalized forces at the contact. The effect of the matrix TW(¢c) is best understood by considering the elastic contribution of the moment, i.e. ttE= T-T(¢~)Ko,nA¢. (14) In the case of small orientation displacement about a constant Cd, at first approximation it is A¢ ~ T-l(¢d)Wcdt (15) where w~dt is the infinitesimal angular displacement of the compliant frame. Plugging (15) into (14) gives t tE= T-T(¢d)Ko,z~T-l(¢d)wcdt , (16) revealing that the equivalent rotational stiffness between the angular displacement and the physical moment depends on the desired end-effector orientation. A geometrically consistent expression for the end-effector orientation displacement can be derived by characterizing the mutual orientation between the compliant and the desired frame directly in terms of the rotation matrix = R Rc (17) where the superscript evidences that the matrix is referred to the desired frame. If a minimal representation of the end-effector orientation displacement is sought, a set of Euler angles ~b can be extracted from /~d. Then, the rotational part of the mechanical impedance at the end effector can be defined a s Mo,¢~ + Do,¢~ + Ko,¢¢ = TT(¢)tt d (18) where Mo,¢, Do,c, Ko,¢ are defined as in (13), tt ~ is expressed in the desired frame, and T w (¢) is needed to transform the moment into a quantity consistent 97 with ~ via a kineto-static duality concept. An advantage with respect to (13) is that the impedance behaviour for the rotational part does not depend on the actual end-effector orientation but only on the orientation displacement through the matrix TT(~). If the X Y Z representation of Euler angles in (1) is adopted, the transformation matrix T satisfies (4)" for ¢ = 0, i.e. when the compliant frame is aligned with the desired frame. Also, representation singularities have a mitigated effect since they occur for large end-effector orientation displacements. By developing an infinitesimal analysis similar to (14)-(16), the orientation displacement de j~ddt (19) can be considered where (4) has been exploited. This corresponds to the angular velocity (20) /'~'c With = (21) Then, the elastic contribution of the moment is given by iza~ = T T ( d ~ ) K o , ¢ d ¢ ~ K o , ¢ d ~ (22) where (4) has been exploited again. Equation (22) with (19) shows how the equivalent rotational stiffness has a clear geometric meaning and is constant on condition that both the angular displacement and the contact moment are expressed in the desired frame. An alternative representation of the end-effector orientation displacement can be extracted from (17) by resorting to the unitary quaternion defined in (5),(6), i.e. 7) = cos(23) 2 0~d (24) ~a = sin ~ , where the vector part of the quaternion has been referred to the desired frame for consistency. It can be shown that the rotational part of the mechanical impedance at the end effector can be defined as [7] t d , -d ~ ptd Mo,~d, a + Do,~O., + Ko,~e D',~ = Do,~ Mo,~S(o. ,~) K' = 2ET(~,~d)Ko~ o ~ where (25) (26) (27) are the resulting time-varying rotational damping and stiffness matrices derived from an energy-based formulation, with E as in (9). 98 By developing an infinitesimal analysis similar to the above, the elastic contribution of the moment is given by UdE ~-Ko,¢~ddt (28) which allows the rotational stiffness to be clearly related to the task geometry, as for (22),(19). Compared to the previous Euler angles descriptions, though, a breakthrough is represented by the avoidance of representation singularities thanks to the use of a unitary quaternion. 4 . C o n t r o l I m p l e m e n t a t i o n For a 6-dof rigid robot manipulator, the dynamic model can be written in the well-known form B(q)i@ + C(q, O)q + d(q, q) + g(q) = u jT (q)h~ , (29) where q is the (6 × 1) vector of joint variables, B is the (6 x 6) symmetric positive definite inertia matrix, CO is the (6 x 1) vector of Coriolis and centrifugal torques, d is the (6 x 1) vector of friction torques, g is the (6 x 1) vector of gravitational torques, u is the (6 x 1) vector of driving torques, h~ = [ f T t tT]T is the (6 x 1) vector of contact forces exerted by the end effector on the environment, and J is the (6 x 6) Jacobian matrix relating joint velocities 0 to the (6 x 1) vector of end-effector velocities v~ = [lb T wTl w ~ j , i.e. v ~ = J ( q ) o , (30) which is assumed to be nonsingular. According to the well-known concept of inverse dynamics, the driving torques are chosen as u = B ( q ) J l ( q ) ( a ~I(q,O)O) + C(q,q)O +~l(q,o) + g(q) + jT(q)h~ , (31) where d denotes the available estimate of the friction torques, and h~ is the measured contact force. Notice that it is reasonable to assume accurate compensation of the terms in the dynamic model (29), e.g. as obtained by a parameter identification technique [10], except for the friction torques. Substituting the control law (31) in (29) and accounting for the time derivative of (30) gives /J -a ~ (32) that is a resolved end-effector acceleration for which the term 5 = J B l ( d ~l) (33) can be regarded as a disturbance. In the case of mismatching on other terms in the dynamic model (29), such a disturbance would include additional contributions. 99 The new control input can be chosen as a = [a T a T IT where ap and ao are designed to match the desired impedance for the translational and the rotational part, respectively. In view of (11), ap is taken as ap = Pc + kvp(pc p~) + kpp(pc Pc) (34) where k yp, kpp are suitable positive gains of the position loop, while p~ and its associated derivatives can be computed by forward integration of the differential equation (11). As regards the orientation loop, instead, ao can be taken according to the three different representations of orientation displacement illustrated above. Then, with reference to (13), it is ao,/~ = T(C~)(~)c +kyo , , ,~ (qb~-~p , )+kpo ,A(Oc-¢ , ) ) + T(O~)dpe (35) where kvo,z~, kpo,,~ are suitable positive gains, ¢~ is the set of Euler angles that can be extracted from the rotation matrix Re expressing the orientation of the end-effector frame with respect to the base frame; note that the presence of T and its time derivative is originated from the time derivative of (2). Further, ¢c and its associated derivatives can be computed by forward integration of the differential equation (13). Next, with reference to (18), it is ao,¢ = &d + T d ( ~ ) ( ~ + kvo.¢(~ ~ ) + kpo.¢(dp ~be)) + Td(~b~)~b~ (36) where kvo,¢, kpo,¢ are suitable positive gains, ¢~ is the set of Euler angles that can be extracted from R T R e , and the matrix Tg(~be) = R~T(~)~) (37) is needed to refer the angular velocity in (21) to the base frame, which then generates the presence of Td in (36) too. Further, ¢ and its associated derivatives can be computed by forward integration of the differential equation (18). Finally, with reference to (25), it is ao,~ = (Oc + kvo,~(wc we) + kpo,~ece (38) where e~ is the vector part of the quaternion that can be extracted from RTR~ when referred to the base frame. Further, Re, we, &~ can be computed by forward integration of the differential equation (25). Notice that the inner position and orientation loops in the previous equations are used in order to provide robustness to the disturbance in (33), which otherwise could not be effectively counteracted through the impedance parameters [9]. 5. Experiments The laboratory setup consists of an industrial robot Comau SMART-3 S. The robot manipulator has a six-revolute-joint anthropomorphic geometry with