Robotic Comanipulation with Active Impedance Control

Transcription

1 Robotic Comanipulation Active Impedance Control Rui Cortesão 1, Brian Zenowich 2, Rui Araújo 1, William Townsend 2 1 University of Coimbra, Institute of Systems Robotics, 33 Coimbra, Portugal 2 Barrett Technology Inc., Massachusetts, USA s: cortesao@isr.uc.pt, bz@barrett.com, rui@isr.uc.pt, wt@barrett.com Abstract The paper presents active impedance control for robotic comanipulation tasks, enabling virtual contact interactions. Computed torque control in the task space powered by multiple-output active observers (AOBs) is proposed, enhancing haptic perception. Forces force derivatives are artificially measured from position data around an equilibrium point that can move time. Control techniques to deal critical impedances are introduced, taking into account the noise distribution along the system. Stochastic design is discussed. A dynamic model of the redundant lightweight 7-DOF WAM TM arm is derived evaluated, playing a key role in the control design. Experiments for small high impedances are presented, highlighting merits limitations of the approach. A comparative study between active non-active impedance control is made. I. INTRODUCTION Robotic manipulation haptic feedback can be divided into two major groups: telemanipulation comanipulation. In the first group, the human operator comms the robotic arm out physical contact, in the second one the operator grabs directly the robot to perform a task. Explicit force sensing is not matory to achieve haptic feedback, which can be provided by position errors through virtual coupling 6. Moreover, virtual haptic walls variable compliance can be added to the task scenario not only to restrict the workspace but also to ensure safe manipulation. Overlaying calibrating such walls on top of geometric information (e.g. vision) can greatly improve manipulation skills, being particularly useful for critical delicate tasks. Comanipulation has several paradigms 1, such as: 1) Degrees of freedom (DOF) sharing. For a redundant arm respect to the robot task space, the operator robot independently control some DOF. 2) Space sharing. Haptic constraints to avoid forbidden zones, signaling dangerous workspaces. 3) Motion filtering. Equivalent to frequency domain sharing, where commed human motions are properly filtered by the control design. 4) Gravity compensation. The robot only compensates for the robot mass, including its tool. Therefore, the human operator feels a free-floating device. 5) Active guidance. The robot applies active (by opposition to resistive) forces to indicate to the operator desired motion directions. 6) Active impedance. This paradigm is discussed in this paper. A desired robot impedance is specified in task space, actively controlled by AOB design. Explicit force sensing is not required. In general, comanipulation can resort from measured computed forces to accomplish a desired interaction behavior. Force sensors are usually attached to the end-effector to provide high sensitivity resolution to tool interactions. Artificial forces can be computed applied to the robot, taking into account posture task constraints, as well as human-robot-environment interactions. Comanipulation architectures are excellent cidates for human-robot skill transfer, since part of the robot motion is based on human skills. Such skills can be extracted through sensor-based learning to boost intelligent behaviors 5. The paper is organized as follows. Section III discusses active impedance control AOBs. The control architecture is introduced, taking into account computed torque techniques, task space formulation AOB design. Dynamic model assessment comanipulation experiments are presented in Section IV. Section V concludes the paper. II. RELATED WORK Comanipulation techniques can be divided into two major classes, reflecting the presence or absence of force sensors in the loop. There is a growing interest of such techniques in the medical field, particularly for surgical robotics, rehabilitation robotics assistive technologies, where a lot of applications dem direct human-robot interaction. Several force control algorithms have been analyzed in 11 for comanipulation, where adaptive force control techniques are addressed. In the medical field, Taylor co-authors presented micro-surgical augmentation small interaction forces in 13. In 9, a comanipulation system out force sensor is presented, where there is only one DOF left to perform needle insertion. More recently, in 14 force control for kinematically constrained manipulators is analyzed in the context of minimally invasive surgery. None of these techniques have used model reference adaptive control (MRAC) strategies to impose desired impedance behaviors. In our control scheme AOBs, MRAC is tuned by stochastic parameters, which is not the approach of classical MRAC techniques, leaving space to shape impedances control techniques. III. ACTIVE IMPEDANCE CONTROL According to Hogan 7, the idea of impedance control is to assign a prescribed robot dynamic behavior in the presence of external interactions, matching the dynamics of massspring-damper systems. The end-effector velocity Ẋ the interaction force F h are related by a mechanical impedance

2 Z. In Laplace domain, In terms of position, where F h (s) Z(s)Ẋ(s). (1) F h (s) sz(s)x(s), (2) sz(s) As 2 Bs K (3) A, K B are the parameters of a mass-spring-damper system, respectively. Equation (3) is based on physical systems, having no active components. Therefore, (3) can be generalized to sz(s) n 2 in 1 ν i s i, (4) n 1, n 2, ν i impedance parameters. If n 1 >, (4) represents an active impedance system, due to integral actions on X(s). A. Robot Dynamics in Task Space Given a set of generalized coordinates q (usually, joint angles for revolute joints) describing robot s posture, the well-known robot dynamics is given by M(q) q v( q, q) g(q) τ (5) where M(q) is the mass matrix, v( q, q) is the vector of Coriolis centripetal forces, g(q) is the gravity term, τ is the generalized torque acting on q. Making the precompensation of v( q, q) g(q), M(q) q τ, (6) where 1 τ τ ˆv( q, q) ĝ(q). (7) Equation (6) can be represented in task space by Λ t (q)ẍt Λ t (q) J t (q) q F t, (8) Ẋ t J t (q) q, (9) Λ 1 t (q) J t (q)m 1 (q)j T t (q), (1) τ t J T t (q)f t. (11) Λ t, X t, J t, F t τ t are respectively the operational space mass matrix, Cartesian position, Jacobian matrix, Cartesian force task torque. F t includes all Cartesian forces referred to the end-effector, such as computer commed, friction, coupling, human environment forces. J t is a truncated non-squared matrix (3 7), since our task space is the 3D Cartesian space. 1 The symbolˆmeans estimate. u(t) f t ˆΛ t Ẍ t f t Fig. 1. K 2 F t B. Feedback Linearization J T t ˆv( q, q) ĝ(q) τ t ˆΛ t (q) J t (q) q q, q Robot Ẋ t X t q, q System plant G(s) for the task space. An active impedance controller can be designed around the equilibrium point generating reactions to external forces. Without disturbance forces 2, F t in (8) is equal to the commed force F c,t. It is computed by y(t) F c,t ˆΛ t (q) J t (q) q ˆΛ t f t, (12) to achieve the desired decoupled system Ẍ t f t. (13) Equation (13) represents the dynamics of a unitary mass for each Cartesian dimension. ft is an acceleration (see (12)), being an input parameter. Inserting a desired Cartesian stiffness necessary to compute a virtual force from position displacements 3, taking the system delay T d into account (mainly due to signal processing), as well as a damping factor K 2 (explained in the sequel), the overall system plant G(s) represented in Figure 1 is obtained. Since T d is small, for each Cartesian dimension G(s) e st d s(s K 2 ). (14) Its equivalent temporal representation is ÿ(t) K 2 ẏ(t) u(t T d ), (15) where y(t) is the computed Cartesian force, based on position displacements around the equilibrium point, u(t) is the plant input. Defining the state variables x 1 (t) y(t) x 2 (t) ẏ(t), (15) can be written as ẋ1 (t) ẋ 2 (t) 1 K 2 x1 (t) x 2 (t) In compact form, { ẋ(t) Ax(t) Bu(t Td ) y(t) Cx(t) u(tt d ). (16). (17) 2 In a real scenario there are disturbance forces due to modeling errors. However, the control architecture needs to know the desired model, which is disturbance free, to generate proper model reference adaptive control actions. 3 For comanipulation in contact, should not be too small, compared to the system stiffness. See 6 for stability robustness analysis under contact.

3 Discretizing (17) sampling time h, the equivalent discrete time system is of form 4 { xr,k Φ r x r,k1 Γ r u k1 y k C r x r,k. (18) In our case, x k has dimension three. The first two states of the discretized system are the measured ones, representing the end-effector force its derivative, respectively 5. The other state appears due to T d, being equal to u k1, the discrete version of u(t T d ). C. AOB Design for Active Impedance Control 1) AOB Concept: To accomplish model-reference adaptive control, the AOB reformulates the Kalman filter, based on 6, 4: A desired closed loop system (reference model) that enters in the state estimation. An active state p k (extra state) to describe an equivalent disturbance referred to the system input. The stochastic design of the Kalman matrices for the AOB application. 2) AOB Control Scheme: Fig. 2 represents the AOB control scheme for comanipulation active impedance control. The value F i,k represents the desired force at equilibrium. Changing F i,k, the equilibrium point moves. The motion dynamics due to F i,k, the active impedance during comanipulation depend on the closed loop poles. The measurement noise of the Cartesian position η Xt affects the control design, being the major limitation to achieve high impedance behaviors. Theoretically, the damping feedback through K 2 can be implemented through the state feedback gain L a. However, this solution is more noisy if is high, due to the noise amplification in F Xt through. Another solution would be to reformulate the state space variables to accommodate Ẋ instead of F Xt (not addressed in this paper). With the AOB in the loop, the overall system can be represented by 6 xr,k Φr Γ r xr,k1 where p k The state x r,k is 1 Γr p k1 u k1 ξ k (19) y k C a xr,k1 p k1 T ηk, (2) u k1 r k1 L r x r,k F Xt,k 1 x r,k1 1. (21) F Xt,k u k1 T, (22) where F Xt,k F Xt,k are the discrete versions of F Xt F Xt, respectively. The stochastic inputs ξ k T ξ xr,k ξ pk 4 See 1, 6 for further details on the discrete matrices. 5 y(t) is equal to the first state, while y k has two states. η k η Fk η represent respectively model measure uncertainties. Since two outputs are measured, 1 C a. (23) 1 F k T The state estimate of (19) is ˆxr,k Φr Γ r L r ˆxr,k1 1 1 Γr r k1 K k (y k ŷ k ), ŷ k C a ( Φr Γ r L r 1 y k F Xt,k ˆxr,k1 1 F Xt,k Γr (24) r k1 ) (25) T. (26) The Kalman gain K k reflects the uncertainty associated to each state, which is a function of ξ k η k 3, 8. It is given by K k P 1k C T a C a P 1k C T a R k 1, (27) P 1k Φ n P k1 Φ T n Q k (28) P k P 1k K k C a P 1k. (29) Φ n is the augmented open loop matrix, Φr Γ Φ n r. (3) 1 The system noise matrix Q k is Qxr,k Q k Q pk. (31) P k R k are respectively the mean square error measurement noise matrices. 3) Stochastic Design: The stochastic parameters of the AOB have to be designed for the comanipulation context. The noise associated the measures is correlated. The measured force can be written as F Xt,k F s,k η Fk, (32) where F s,k is the signal corrupted by noise η Fk. In the same way, F Xt,k is η F k Since F k F s,k F s,k1 ηf h k, (33) given by η F k { R k E η F k η Fk1. (34) h η 2 F k η F k η Fk η Fk η F k η 2 F k }, (35)

4 F i,k L c r k K 1 u k ft Ẋ t e std 1 1 s s X t s F Xt F Xt Multiple Output AOB K 2 L a η Xt ˆx r,k T Fig. 2. AOB control scheme for comanipulation active impedance control. Decoupled control scheme for each task dimension. The feedback gain is L a L 1 L 2 L 3 L 4. L c L 1, the state estimate is ˆx r,k ˆF ˆ Xt,k F Xt,k û k1. For our 3D Cartesian task, ft f x fy fz T. straightforward analysis of (34) (35) gives 6 R k E { η 2 F k } 1 1/h 1/h 2/h 2. (36) The Kalman gains only depend on the relations between R k Q k 4. Hence, a normalized value for R k, Rk, has been used in the design. From (36), R k 1 1/h 1/h 2/h 2. (37) A sensor-based estimation strategy is adequate for the measured states, enabling to accurately track comanipulation forces. Therefore, the uncertainty associated those states, coded in Q k, is relatively high, making the estimates more sensitive to measures. The scaling factor of stochastic parameters should make observer dynamics faster than system dynamics. In the design, Q k (38) The Kalman gains K k are not only a function of R k Q k, but also of the system plant, which includes the control parameters K 2 (derived in the sequel). The stochastic parameters (e.g. K k, R k Q k ) the control gains (e.g. L a K 2 ) are all based on SI units. 4) Control Design: The feedback gains have to be properly designed based on the desired active impedance for comanipulation. The critical scenario happens for high values of, necessary for comanipulation on hard surfaces. In this case, the derivative gain should come mainly from K 2, not from the force derivative feedback. A virtual PD controller (K 1 K 2 gains) can be designed to achieve a desired closed loop response. This procedure assigns the K 2 6 The η F k noise is not white, since it correlates the previous sample. It is approximated by white noise in the system b (Kalman design). gain 7. The same desired response 8 is used to compute the state feedback gain, taking also into account the deadtime discretization procedures. In this way, the state feedback gain for the derivative term (L 2 ) will be very small, reducing the effect of noise in the system. Moreover, L 1 plays the role of K 1 referred to the force data, L 1 K 1. (39) In the presence of force sensors, the feedback from L 1 instead of K 1 disables position-controlled behaviors. For moderate or small values of, the feedback distribution of the derivative terms (the critical ones) can be balanced between K 2 the state feedback. This analysis does not take into account signal to noise ratios but only noise values, since the noise distribution always exists even out signal inputs (i.e., zero values for the signal). D. Active Impedance Perception Without loss of generality, let s consider the equilibrium point at F i,k X t (see Fig. 2). When the human operator applies an external force F h to the robot, the controller reacts through f t K 2 Ẋ t L a ˆF Xt ˆ F Xt û(t T d ) ˆp(t) T, (4) where ˆp(t) is the continuous version of. From the stochastic design (sensor-based strategies), knowing that the term L 3 u(t T d ) has negligible influence in f t, f t (K 2 L 2 )Ẋt L 1 X t p(t). (41) Therefore, from (8) (12), Λ t Ẍt (K 2 L 2 )Ẋt L 1 X t p(t) F h. (42) Without considering the influence of p(t), (42) corresponds to the classical impedance given by (2) (3). The gains due 7 The K 1 gain is never used in the design. 8 In the design we have used a critically damped response time constant τ f.

5 to velocity position feedback affect respectively damping spring parameters. The mass matrix Λ t shapes all impedance parameters, generating couplings among Cartesian axes. When the off-diagonal terms of Λ t are small compared to the main diagonal ones, a good decoupling behavior is accomplished 9. Nevertheless, an external force F ext can be added to F h to compensate couplings in (42). F ext is given by F ext ˆΛ d t Ẍt (K 2 L 2 )Ẋt L 1 X t p(t). (43) The matrix ˆΛ d t corresponds to ˆΛ t the main diagonal set to zero. Equation (42) becomes then decoupled for each task dimension. Let s analyze now the role of p(t). In discrete terms, 1 K k (F Xt,k ˆF Xt,k) ( F Xt,k F ˆ T Xt,k). (44) From (24), (25) r k1, ˆxr,k k j1 Φ kj e K j FXt,k F Xt,k Φr Γ Φ e I K k C a r L r 1, (45). (46) It can be inferred from (45) that the state estimate is a linear function of all past measured variables F Xt,k F Xt,k. Therefore, (44) performs integral actions on 1 X t,k Ẋt,k, making (42) an active impedance system. From (7), (11), (12), (4) (43) the commed torque for the task τ c,t is τ c,t ĝ(q) ˆv(q, q) J T t ˆΛ t J t q J T t ˆΛ t f t J T t F ext, (47) Each component of f t is independently computed by f t r k L a ˆx r,k T K 2 Ẋ t, (48) where the control gains (i.e the active impedance parameters) may vary for each task dimension. IV. EXPERIMENTS This section analyses the quality of the 7-DOF WAM dynamic model, followed by comanipulation experiments. A. Experimental Setup A picture of the lightweight 7-DOF WAM is depicted in Fig. 3. The robot, joint-torque controlled, is connected by CAN to a shuttle PC (AMD Duron Applebred x86 processor at 1.8 GHz) running RTAI Linux. The roundtrip communication time is about.8 ms. The control sampling time was set to h 1.4 ms. The overall system deadtime T d h. 9 For lightweight robots, the Λ t values are small. 1 There is a linear relation between F Xt,k, F Xt,k X t,k Ẋt,k. Fig DOF WAM arm. B. Dynamic Model of the 7-DOF WAM The dynamic model of (5) can be computed either by recursive Newton-Euler (RNE), Euler-Lagrange (EL) or a combination of both methods. In our setup, M(q) is based on EL 12. The terms v( q, q) g(q) come from RNE 11. The value of M(q) is given by 12 M(q) n mi J vi (q) T J vi (q) (49) i1 J wi (q) T R i (q)i i R i (q) T J wi (q), where m i, J vi (q), J wi (q), R i (q) I i are respectively the mass of each link, the Jacobians associated linear angular velocities, the rotation matrix in base coordinates the inertia tensor in link coordinates. All these quantities are referred to the center of mass of link i. For the 7-DOF WAM arm, the values of m i I i obtained SolidWorks TM are in Barrett s manual 2, which take into account specific characteristics of the WAM design. J vi (q) J wi (q) can be numerically computed at each time step by Jvi (q) J wi (q) a... a i1... z... z i1... (5) a k z k (o ci o k ) k 1,..., i 1. (51) o ci is the center of mass of link i, o k is the origin of link frame k, i.e. o k x k y k z k T. (52) Using this procedure, the computation of M(q) in the shuttle PC takes µs. With RNE, the computation times of M(q), v(q, q) g(q) are respectively 13 µs, 2 µs below 1 µs. For manipulation techniques that require the partial Jacobians J vi (q) J wi (q), there might be an advantage of using (49), instead of RNE methods. 11 Setting q, v( q, q) g(q) τ. Additionally, g(q) τ for q q (v( q, q) can be written in the form C( q, q) q). 12 Each column of M(q) can also be computed by RNE techniques. For example, the first column of M(q) is the value of τ for q 1... q subtracted by g(q). 13 Computation times vary, depending on compilation flags, hardware, coding techniques. We were able to achieve 2 µs for (49) a recent PC.

6 m.8 measured x t desired x t measured y t measured z t m.8 measured y t desired y t measured x t measured z t m.9 measured z t desired z t measured x t measured y t time s time s (a) (b) (c) time s Fig. 4. Step responses in the task space to evaluate the dynamic model. (a) fx 1 m/s 2, fy fz. (b) fx, fy 1 m/s 2 fz. (c) fx, fy fz 1 m/s 2. A resistive acceleration (referred to ft ) of 7 m/s2 was considered. C. Dynamic Model Assessment To assess the dynamic model, (12) is applied to the robot f t equal to a step function starting at T, f t A f Step(t T ). (53) Perfect modeling will make each Cartesian position follow a double integrator response (see (13)), (t T ) 2 X t A f. (54) 2 Fig. 4 presents results. A f 1 m/s 2, a resistive (by opposition to commed) acceleration due to friction of 7 m/s 2 acting against ft was considered to plot the desired responses. From Fig. 4 it can be inferred that at the beginning of the step, the dynamic model tracks well the real response couplings between axis are small. Due to the open-loop nature of this test, a poor matching performance as time goes by was expected. D. Comanipulation Experiments The comanipulation control design follows the procedure of Sections III-C.3 III-C.4, respectively. This section reports control results for small high impedances. A comparative study is also made, showing differences between classical active impedances. 1) Design of Small High Active Impedances: Setting 5 N/m, critically damped responses different time constants τ f entail different active impedances. For small impedance, τ f.2 s, giving K 2 1., (55) L a , (56) K k (57).94. For high impedance, τ f.3 s, K , (58) mm,m/s 2,m/s 2 mm,m/s 2,m/s x t f x time s x t f x (a) time s Fig. 5. Comanipulation data in the x direction. (a) Small impedance. (b) High impedance. L a , (59) K k (b) (6) Smaller time constants generate higher feedback gains, increasing the system impedance (see (42)). Fig. 5 presents comanipulation results. In Fig. 5.a the human operator is able to move the arm a couple of centimeters, feeling soft impedance around the equilibrium point. In Fig. 5.b the arm only moves a few millimeters. In both situations, f x is mainly due to. For the high impedance case (Fig.

7 mm,n position measured F x s Fig. 6. Comanipulation data classical impedance. The measured force F x represents the human force applied to the robot. Position data are relative to the equilibrium point. mm,n position measured F x s Fig. 7. Comanipulation data active impedance. The measured force F x represents the human force applied to the robot. Position data are relative to the equilibrium point. 5.b) there are however more high frequency components in f x due to the K 2 gain that amplifies the Cartesian position noise. 2) Comparative Study: This section compares the active impedance control scheme proposed in this paper classical impedance control. The same controller is kept for both cases, eliminating only active state actions in the classical control. A force sensor was attached to the end-effector to measure human forces. Figure 6 presents comanipulation results non-active impedance. The relations between force position are degraded respect to the active impedance case (see Figure 7). After comanipulation, the robot position does not go to the equilibrium point due to the lack of integral actions (see Figure 6 around 85 s). The main control parameters were 5 N/m, K 2 6, 66 L a (61) V. CONCLUSIONS The concept of active impedance has been introduced. It extends the formulation of mechanical impedance to accommodate integral actions between applied force measured position. A multiple output active observer has been designed on top of operational space techniques to achieve active impedance control for comanipulation. The proposed paradigm does not include force sensors in the loop. Virtual forces are computed from position displacements around an equilibrium point. Control techniques that address the distribution of feedback gains based on measurement noise have been introduced, as well as the stochastic design for comanipulation. Noise amplification by derivative terms are the practical limitation for very stiff behaviors. The active impedance parameters are shaped by the operational space mass matrix, control gains stochastic gains. Impedance decoupling techniques have been introduced, enabling decentralized design for each Cartesian dimension. Experimental results have validated the approach for small high impedances. A comparative study between active non-active impedance control for comanipulation has been presented, highlighting the merits of the approach. Moreover, a dynamic model of the 7-DOF WAM robot has been tested in task space to assess not only the quality of feedback linearization techniques but also control robustness to modeling errors. VI. ACKNOWLEDGMENTS This work was supported in part by the Portuguese Science Technology Foundation (FCT) Project PTDC/EEA- ACR/72253/26. REFERENCES 1 K. J. Åström B. Wittenmark. Computer Controlled Systems: Theory Design. Prentice Hall, Barrett Technology, Inc. WAM Arm: Inertial Specifications, S. M. Bozic. Digital Kalman Filtering. Edward Arnold, London, R. Cortesão. On kalman active observers. Int. J. of Intelligent Robotic Systems, 48: , R. Cortesão, R. Koeppe, U. Nunes, G. Hirzinger. Data fusion for robotic assembly tasks based on human skills. IEEE Trans. on Robotics, 2(6): , R. Cortesão, J. Park, O. Khatib. Real-time adaptive control for haptic telemanipulation kalman active observers. IEEE Trans. on Robotics, 22(5): , N. Hogan. Impedance control: An approach to manipulation. Trans. of ASME, J. of Dynamic Systems, Measurement Control, 17:1 24, A. Jazwinsky. Stochastic Processes Filtering Theory, volume 64 of Mathematics In Science Engineering. Academic Press, 197. (Edited by R. Bellman). 9 S. Lavallee, J. Troccaz, L. Gaborit, P. Cinquin, A. Benabid, D. Hoffmann. Image guided operating robot: a clinical application in stereotactic neurosurgery. In Proc. of the IEEE Int. Conf. on Robotics Automation, (ICRA), pages , France, G. Morel. Applications of force feedback in medical surgical robotics. In Summer School on Medical Robotics. France, J. Roy, D. L. Rothbaum, L. Whitcomb. Haptic feedback augmentation through position based adaptive force scaling: Theory experiment. In Proc. of the Int. Conf. on Intelligent Robots Systems (IROS), pages , Switzerl, M. Spong M. Vidyasagar. Robot Dynamics Control. John Wiley & Sons, R. Taylor, P. Jensen, L. Whitcomb, A. Barnes, R. Kumar, D. Stoianovici, P. Gupta, Z. Wang, E. Juan, L. Kavoussi. A steady-h robotic system for microsurgical augmentation. Int. J. of Robotics Research, 18: , N. Zemiti, G. Morel, B. Cagneau, D. Bellot, A. Micaelli. A passive formulation of force control for kinematically constrained manipulators. In Proc. of the Int. Conf. on Robotics Automation (ICRA), pages , USA, 26.