论文2G精华版本 ICRA 2018 files 2391

Perceived Stiffness Estimation for Robot Force Control Lu s Santos and Rui Cortes o Abstract Typical robot force control architectures have a positive force feedback loop to decouple robot dynamics from contact dynamics Due to the noisy profi le of force measurements it is common to fi lter force signals by low pass fi lters This paper shows that when force feedback is fi ltered robot and environment dynamics are no longer decoupled affecting force control performance Additionally the perceived stiffness from the force control perspective is correlated with the robot effective mass To cope with this issue a force based stiffness estimation strategy that also includes the inertial properties effective mass in the estimation algorithm is proposed allowing to adapt control gains based on the robot effective mass In this way the perceived stiffness can be seen as a control optimization parameter rather than a well defi ned physical property Simulation and experimental results with a 1 DoF robot and 7 DoF manipulator respectively validate the estimation strategy showing better force control results with the perceived stiffness in the control loop as compared to the real environment stiffness I INTRODUCTION Contact is a complex physical phenomenon In the litera ture different formulations have been followed to model it ranging from simple single parameter linear model to non linear multi parameter models growing in model complexity when the desired estimation accuracy and or the environment complexity increases After a proper model selection it is necessary to assign values to model parameters which is not a trivial task The environment model1parameter estimation remains an open problem in robotics playing a crucial role in interaction tasks since proper estimation improves interaction control performance enabling environment perception in tele manipulation tasks and allowing computer simulation of contact scenarios In 1 the performance of an impedance controller is improved by including an estimation of the environment stiffness in the impedance control parameters The environment stiffness is estimated offl ine by a recursive least squares RLS algorithm Adaptive control techniques are used for stiffness estimation in 2 3 and for stiffness and damping estimation in 4 In these approaches stability is as sessed by Lyapunov techniques while parameter convergence can only be ensured for frequency rich signals Kikuuwe and Yoshikawa presented in 5 a RLS algorithm to estimate the mechanical impedance perceived by a robot when interacting This work was supported by the project CENTRO 01 0247 FEDER 017958 funded by CENTRO 2020 Portugal 2020 and the European Union by FEDER Lu s Santos is with the Institute of Systems and Robotics University of Coimbra Portugal email luiscvsantos isr uc pt Rui Cortes o is with the Electrical and Computer Engineering Department Institute of Systems and Robotics University of Coimbra Portugal email cortesao isr uc pt 1In this paper contact and environment model are used interchangeably in a constrained environment A speed dependent forgetting factor together with a discontinuity detection approach allows to quickly detect environment transitions improving online estimation performance A similar approach is proposed in 6 The authors use time varying forgetting factors in the RLS algorithm claiming fast tracking performance when the environment parameters change together with increased robustness to noisy measurements In 7 the performance of several contact models for biological soft tissue modeling are assessed being the contact parameters estimated by an offl ine linear least squares method The authors conclude that the Kelvin Boltzmann and the Hunt Crossley HC models are the ones that achieve the best results choosing the Kelvin Boltzmann model to be used in a model reference adaptive controller due to its linearity In 8 a comparative study is performed between the methods presented in 1 2 4 and an offl ine approach proposed by the authors in which the Kelvin Voight KV model parameters are estimated by an offl ine signal processing method Nonlinear contact models present a better physical consistency addressing some of the inconsistencies of linear models 9 11 requiring however more complex estimation algorithms Most contact model estimation algorithms require the availability of force position and sometimes velocity mea surements in addition with a precise knowledge of contact point Geometrical uncertainty of contact position is the biggest obstacle to achieve high quality estimations especially in contact with stiff environments where a small error in contact location leads to large force errors A large number of approaches either assume that the environment is static and the contact position is well known 1 or start the measurements with the robot at rest in uncompressed contact with the environment 10 For most practical robotic applications such data or setup is almost unpractical The geometrical uncertainty problem can be addressed by estimating the contact parameters resorting only to force data avoiding using geometric information in the estimation algorithm The offl ine signal processing technique in 8 was one of the fi rst approaches to estimate the contact parameters KV contact model resorting only to force data In 12 the contact stiffness is estimated based on the force error of two force observers tuned with different nominal stiffnesses while in 13 the estimation is performed by an artifi cial neural network In this paper we propose a perceived stiffness estimation algorithm tightly connected to force control which is not necessarily equivalent to the real stiffness due to coupling between robot and environment In highly unstructured environments as is the case in medical ultrasound tasks 2018 IEEE International Conference on Robotics and Automation ICRA May 21 25 2018 Brisbane Australia 978 1 5386 3080 8 18 31 00 2018 IEEE1667 Mr X 0 Ks F Fe Fig 1 Mass spring contact model of a 1 DoF robot 14 where free space motion interchanges with soft and stiff contact scenarios simpler but less accurate contact models might be preferable Lumping the contact dynamics into a single parameter simplifi es estimation algorithms allowing for real time contact parameter adaptation In this way the choice of a linear spring contact model is convenient In addition to force data the perceived stiffness estimation algorithm also includes information about robot inertial properties referred to the end effector i e its effective mass 15 In force control tasks due to the noisy profi le of force sensor measurements it is common to fi lter force measurements by a low pass fi lter 16 When fi ltering the feedback force robot dynamics is no longer decoupled from environment dynamics leading to different force responses for different contact end effector orientations since the effective mass is confi guration dependent It is going to be shown that when the effective mass is included in the estimation algorithm the stiffness perceived by the controller can be anticipated improving force control performance in dynamic interactions specially with stiff surfaces Clearly the elastic properties of an object do not change during interaction however the controller perception of the object does change existing a strong correlation between the effective mass and the perceived stiffness when the feedback force is fi ltered From a control standpoint rather than aiming to obtain a model that perfectly emulates contact dynamics the critical issue is to boost force control performance In this way the perceived stiffness can be seen not as a well defi ned physical property but as an optimization parameter to improve force control The remaining of this paper is organized as follows Sec tion II shows the coupling between robot and environment dy namics when force feedback signals are fi ltered In Section III the new stiffness estimation algorithm is presented Section IV assesses through a 1 DoF robot example the impact of inertial parameters in stiffness estimation when force feedback is fi ltered while Section V present experimental results Section VI concludes the paper II FILTEREDFORCEFEEDBACK Under ideal conditions when the system is linearized the controller sees an unitary mass interacting with the environment being the system response independent from the robot dynamic parameters However since a model is always an idealized and often simplifi ed view of a physical system the feedback linearization may not perfectly compensate the manipulator dynamics introducing a perturbation in the controller Furthermore force measurements are usually Mr KsF Fe Fe X 0 Force Control Law Fd F a Fe Fd F X Robot Dynamics Contact Dynamics 1 Mrs2 Mr Mass Normalization Computed Force Control Law Ks reaction force physics b Fig 2 Force control with a positive feedback loop Feeding back the contact force the controller is able to see the robot dynamics decoupled from the contact dynamics a Generic force control scheme with a positive feedback loop b Force control scheme with mass normalization from the control point of view corrupted with noise having a poor signal to noise ratio SNR A common strategy to enhance SNR is to fi lter the measured force by a low pass fi lter 16 This section analyzes through a 1 DoF example how the introduction of a low pass fi lter in the feedback loop changes the force control performance and the contact dynamics Fig 1 shows a 1 DoF robot interacting with an environment that is assumed to behave as a linear spring According to Newton s2ndlaw the equation of motion of the 1 DoF robot in contact with environment is given by Mr x f t Ksx t 1 whereKsis the environment stiffness Mrthe robot mass andf t the applied force x t is the robot penetration in the environment2 In the Laplace domain the contact dynamics is given by X s F s 1 Mrs2 Ks 2 whereX s andF s are respectively the Laplace domain representation ofx t andf t LetFe s be the contact force given by Fe s KsX s 3 Solving 3 forX s and replacing the result in 2 the open loop force response is given by Fe s F s Ks Mrs2 Ks 4 Fig 2 shows the 1 DoF robot interacting with the envi ronment controlled by a force controller Feeding back the contact force with positive feedback the control variable 2In this section it is assumed that the robot is always in contact with the environment i e no free space motion 1668 Fe Fd F X Robot Dynamics Contact Dynamics 1 Mrs2 MrKs Mass Normalization Computed Low Pass Filter Force Control Law c s c reaction force physics a Fe F X Robot Dynamics Contact Dynamics 1 Mrs2 MrKs Mass Normalization Computed Low Pass Filter c s c reaction force physics b Fig 3 Force controller with a low pass fi lter in the positive feedback loop a Generic force control architecture with fi ltered feedback force b System normalization with fi ltered feedback force F s sees the robot dynamics decoupled from contact dynamics see Fig 2a and Fig 2b X s F s 1 Mrs2 5 If the dynamic model is known Mrfor the 1 DoF robot it is possible to perform the system normalization with the controller seeing the robot as an unitary mass interacting with the environment X s F s 1 s2 6 A Force Feedback Filtering Fig 3a shows a generic force control with a low pass fi lter with cutoff frequency cin the force feedback loop When fi ltering the feedback force the controller no longer sees the interaction with the environment as see Fig 2b Fe s F s Ks s2 7 but as see Fig 3b Fe s F s Ks s c s3 cs2 Ks Mrs 8 highlighting that when the contact force is fi ltered the robot and the environment dynamics are no longer decoupled III PERCEIVEDSTIFFNESSESTIMATION This section presents the perceived stiffness estimation algorithm from the control point of view which might be different from the environment stiffness The perceived stiffness estimation is given by the contribution of two terms Ks n Ks 1 Ks 2 9 Ks 1is given by the transitory relation between desired measured and estimated forces based on the idea presented in 17 Ks 2is proportional to the end effector effective mass A Force Relations Letfd fmandfebe desired measured and estimated forces respectively withfe given by the AOB fi rst state estimate see 17 for details on AOB WhenKs Ks n the controller expects a more compliant environment than it actually is A lowKs nmeans higher feedback gains which leads to an underdamped response from the force controller fmpresents an oscillatory behavior thatfeis not able to follow averaging outfmoscillations As result the difference betweenfdandfeis relatively small when compared to the difference betweenfmandfe On the other hand whenKs Ks n the environment is more compliant than expected by the controller leading to a sluggish force control performance since control gains are too low to track fd fefollowsfmbut due to the low control gains is unable to trackfd being the difference betweenfmandfesmaller than the difference betweenfdandfe Whenever the system presents an underdamped response the nominal stiffness value is increased which reduces the control feedback gains stabilizing the system see 14 for details on Ks 1algorithm B Effective Mass and Perceived Stiffness From 8 when the force feedback is fi ltered contact and manipulator dynamics are no longer decoupled despite the feedback linearization and as it is shown in sections IV and V the stiffness perceived by the controller rather than changing with the contact force changes with the end effector posture Letmjbe the effective mass i e the mass that is perceived at the end effector along the j direction 15 1 mj Jpj q B 1 q JT pj q 10 whereJpi q R1 nis the linear Jacobiani th row and B q Rn nthe inertia matrix Since bothJp q andB q change with the manipulator posture when changing the end effector orientation the inertial properties perceived at the end effector will also change changing the contact dynamics and consequently the control performance Using this information a new perceived stiffness estimation algorithm can be devised C Perceived Stiffness Estimation for Robot Force Control The perceived stiffness can be anticipated by introducing a second term in the estimation algorithm that updates the estimation based on the end effector effective mass Ks 2 km j 11 jis thejthmain diagonal element of the end effector inertia matrix p q p q Jp q B 1 q JT p q 1 12 withJp q R3 nthe linear Jacobian jis closely related to the effective massmj 10 presenting slower dynamics 1669 Fe FdF X Robot Dynamics Contact Dynamics Mr Mass Normalization Computed Low Pass Filter Kp 1 s 1 Mr 1 s Kd X Force Controller Ks c s c reaction force physics Fig 4 Force controller with dampingKdprovided by velocity feedback with force feedback fi ltering Fdis the reference force whileFeis the contact force which can be seen as a fi ltered version of the effective mass Ks 2can be seen as an anticipative envelop of Ks 1 which improves the overall control performance IV SIMULATION This section assesses the impact that inertial mass parameters have in force control performance when force feedback is fi ltered by a low pass fi lter see 8 A simplifi ed 1 DoF robot is controlled in contact by a PD controller Fig 4 The system transfer function is given by Fe Fd s c KpKs s3 Kd c s2 Ks Mr Kd c s KpKs c 13 It is hard to analyze how the pole placement behavior changes with the robot mass from the closed form solution of 13 In this way the system response is inferred by computing the root locus using the robot mass as the variable parameter Fig 5 show the root locus when the robot mass is changed from 0 001 to 10 Kg forKp 1 the root locus pattern remains the same for differentKp Kdis set to assign a critically damped behavior assuming that there is no fi lter in the feedback loop The unfi ltered transfer function is given by Fe s Fd s KpKs s2 Kds KpKs 14 with Kdgiven by Kd 2pKpKs 15 The remaining parameters are kept constant throughout the simulation cis given by c 2 fc 16 with the fi lter cutoff frequency set atfc 5 Hz and the environment is modeled to behave as an elastic wall with stiffness Ks 2000 Nm 1 A Discussion Simulations show that despite linearization systems with different masses have different performances For the same control design setups with small masses display a sluggish response while large mass setups present an overdamped behavior It can be inferred when force feedback is fi ltered to have similar force control performances with different contact postures the robot effective mass needs to be taken 100 80 60 40 200 100 50 0 50 100 Real Axis Imaginary Axis polezero a 0510 0 5 10 time s Force N fdfm b 60 40 200 500 0 500 Real Axis Imag Axis zeropole c 0510 0 5 10 time s Force N fdfm d 60 40 200 100 0 100 Real Axis Imag Axis zeropole e 0510 0 5 10 time s Force N fdfm f 60 40 200 10 0 10 Real Axis Imag Axis zeropole g 0510 0 5 10 time s Force N fdfm h 100 75 50 25 0 20 0 20 Real Axis Imag Axis zeropole i Fig 5 Root locus evolution for 0 001 Kg Mr 10 Kg andKp 1 a Root locus The arrows point the root locus evolution as the mass increases from 0 001 to 10 Kg For lowMr the non dominant complex pole pairs have a high imaginary component and are not represented in the plot b d f and h show afd 5 N step response forMr 0 01 0 1 1 and 10 Kg respectively fdandfmare reference and applied forces respectively c e g and i show poles and zero locations of the closed loop system for Mr 0 01 0 1 1 and 10 Kg respectively into account in the perceived stiffness parameter which in turn affects force control gains V EXPERIMENTS Two experiments are performed to assess and validate the stiffness estimation algorithm The relation between effective mass and perceived stiffness is investigated using a real robot manipulator interacting with a virtual wall The system linearization is performed through nonlinear feedback linearization The manipulator is controlled following a hierarchical control approach where explicit force control arises as the pr