Force/Vision Based Active Damping Control of Contact Transition in Dynamic Environments

Transcription

1 Fore/Vision Based Ative Damping Control of Contat Transition in Dynami Environments Tomas Olsson, Rolf Johansson, Anders Robertsson Department of Automati Control, Lund Inst. of Teh., Lund University, SE Lund, Sweden {tomas.olsson, anders.robertsson, Abstrat When a manipulator interats with objets with poorly damped osillatory modes, undesired osillations and bouning may result. In this paper, we present a method for observer-based ontrol of a rigid manipulator interating with an environment with linear dynamis. The ontroller injets a desired damping into the environment dynamis, using both visual- and fore sensing for stable ontrol of the ontat transition. Stability of the system is shown using an observer-based bakstepping design method, and simulations are performed in order to validate the hosen approah. I. INTRODUCTION AND PROBLEM FORMULATION Contat fore ontrol has long been an important researh topi in robotis, and a large number of experimental and industrial implementations and appliations have been presented in the literature [1]. Most of these methods fous on the robot dynamis, assuming that the environment an be modeled by ideal onstraints, or as simple massless linear or non-linear) spring-damper systems [2]. Dynamial systems an also be used to model many types of environments and the energy transfer between the robot and its environment [], [4]. For effiient ontrol of suh dynami environments it is neessary to onsider not only the robot dynamis but also the dynami properties of the environment itself, or otherwise undesirable phenomena suh as bouning may our. The ombination of vision and fore sensing is very powerful, in that it allows us to ontrol interation with objets whose loations are not initially known, so that impat an be predited and ontrolled in suh a way that large impat fores are avoided. There exist appliations where fore ontrol in dynami environments is required. As an industrial example we have ompliant tool adapters, whih are mounted between a rigid workpiee and the fixture and serve as both an extra mehanial ompliane and as a fore sensor. Cooperating robots is another example, where one robot may be programmed with a desired ompliane, holding a workpiee on whih some operation is to be performed. Other examples are interation with non-rigid strutures, and loads suspended from ranes. In non-industrial environments, there are examples suh as manipulation of flying, falling and rolling objets. In unertain but stati environments, the fore/vision ontrol problem is often solved by assigning eah available degree of freedom to a speifi task, for instane by speifying it as either position- or fore ontrolled as in [5]. The interation ontrol problem in dynami environments is in general more hallenging. In [4], dynami effets suh as frition in rigid body- and elasti ontat with passive environments were analyzed, however only the ontat phase of the interation was onsidered, thereby avoiding the need for non-ontat sensing suh as vision. For interation with non-stationary objets, for instane, it may be neessary to introdue feed-forward from the measured target motion during the approah phase, as in [6] where fore/vision ontrol was used for an assembly operation involving a moving target. In the most general ase of interating systems, however, the manipulator and environment dynamis are oupled through the fores of interation, and feedbak from multiple sensor signals may affet the overall stability and performane of the system in a omplex way. As a way to overome this diffiulty, in [7] the use of vision/impedane ontrol was used for pegin-hole insertion experiments in a stationary environment. However, as the range of ahievable impedanes is always limited by fore sensor noise and dynami effets suh as gear box- and fore sensor elastiity, as well as the presene of inner position feedbak loops, a purely impedanebased approah may not be suffiient for effiient ontrol in non-stationary environments. A. Problem Formulation In this paper, we present a method for observer-based ontrol of a system onsisting of a rigid manipulator interating with an environment with linear dynamis, and demonstrate how vision and fore an be used together in order to influene and ontrol the dynamis of the environment, by ative ontrol during both the approahand ontat phases. An observer-based bakstepping proedure is used to obtain a ontrol law whih makes the manipulator/environment system asymptotially stable. The method is validated in simulations on a model of a simple three-link serial robot interating with a massspring-damper system.

2 x 2) f 2) y 2) d x 1) f 1) Fig. 1. Setup with multiple point ontats between robot and environment. Loal oordinates x i),y i),z i) ) are attahed to the workpiee at eah ontat point, while the dynamis of the environment is modeled by a linear mass-spring-damper system. II. MODELING OF THE ROBOT AND ENVIRONMENT We assume the setup shown in Fig. 1. A rigid robot is in ontat with a dynami environment, where the interation is modeled by ontat fores in the surfae normal diretion at the ontat points. The dynamis of a rigid robot an be modeled by the system Mq) q+cq, q) q+gq) = τ J T q)f 1) where f is the interation environment) fore given in the hosen fore spae. By using a feedbak linearizing ontrol law it is possible to obtain a deoupled system of double integrators from a new ommand signal u to x [8]. Assuming robot double integrator dynamis with position z and veloity z 4, and that the environment dynamis in one diretion an be modeled as a stable linear seond order system with position z 1 and veloity z 2, we an write the oupled dynamis of the robot and environment as k y 1) ż 1 = z 2 2) ż 2 = kz 1 dz 2 + Ψz z 1 ) ) ż = z 4 4) ż 4 = u 5) where the salar ontat fore is f = Ψδ) def = Kδ)δ, 6) where for onveniene we have defined δ def = z z 1, 7) and where the ontat stiffness Kδ) is assumed to be a differentiable funtion of δ, satisfying the properties Kx) 0, x 8) Kx) = 0, x < 0 9) Kx) K, x 10) K x)x+kx) > 0, x > 0 11) The final inequality expresses the ondition that the ontat fore f = Kδ)δ is inreasing with respet to the deformation distane δ. This also implies that the inverse Ψ 1 f ) is well-defined when δ > 0. Eq. 6) together with properties 8) 11) an be used as an approximate global model for a linear spring mehanism, or a loal model for ompletely elasti non-linear ontat of the type f = k δ n with n > 1 as desribed in [2]. Although the following setion will use the simple model in Eq. 2) 5), the method presented is straightforward to generalize to a larger lass of systems, suh as any passive lumped-multiple-mass environment model. The robot dynamis may in general be any linear seondorder system with relative degree two, whih an always be transformed into a double integrator by a suitable state feedbak. Suh a model ould for instane be used to model a position ontrolled manipulator with PD-ontrol. A. Full state feedbak III. CONTROLLER DESIGN The aim of the ontroller is to obtain a suffiiently damped impat transition, whih an be ahieved by ontrolling the interation fores suitably. The form of the system in Eq. 2) 5) is similar to the so alled strit feedbak- or triangular form [9]. For suh systems, the bakstepping design method an be used to find a ontrol law and a Lyapunov funtion in a reursive fashion. Due to the speial struture of the passive environment, it makes sense to try to find a ontroller that aims to ontrol the energy flow between the robot and environment suh that the environment dynamis is suffiiently damped. If diret ontrol of the interation fore f was possible, we ould introdue extra damping into the environment by hoosing a virtual ontrol signal of the form with f = Kα 1 z 1,z 2 ) z 1 ) α 1 z 1,z 2 ) z 1 ) 12) α 1 z 1,z 2 ) = z 1 + hz 2 ) 1) where the damping funtion hz 2 ) is twie ontinuously differentiable and hosen to satisfy the properties hx) 0, x 14) hx) = 0, x > 0 15) dhx)/dx < h, x 16) In this way, extra damping is introdued by a suitable dissipation of energy by appliation of a ontat fore in the opposite diretion of motion during the part of the motion when the ontat point veloity z 2 < 0. This an be seen by introduing the energy-based Lyapunov funtion V 1 z 1,z 2 ) = 1 2 kz z2 2 17)

3 whih gives V 1 = kz 1 z 2 + z 2 kz 1 dz 2 + Kδ)ε + hz 2 )) = dz z 2 Kδ)hz 2 ) +z 2 Kδ)ε 18) }{{} def = Wz 2,ε ) 0 where the term z 2 Kδ)hz 2 ) 0 introdues extra damping due to properties 8), 14) 15), and where ε = z α 1 z 1,z 2 ) = z z 1 hz 2 ), 19) is interpreted as the error between the tool tip position orresponding to the virtual ontrol signal and the true position. We an now write Eq. ) as ż 2 = kz 1 dz 2 + Kδ)hz 2 )+Kδ)ε def = f 2 z 1,z 2,ε ) 20) Furthermore, we have ε = z 4 α 1 = z 4 z 2 h z 2 ) f 2 z 1,z 2,ε ) 21) By augmenting V 1 with a quadrati term in ε, we obtain V 2 z 1,z 2,ε ) = V 1 z 1,z 2 )+ 1 2 p ε 2, p > 0 22) whih gives V 2 = Wz 2,ε )+ε Kδ)z 2 + p ε z 4 α 1 ) = Wz 2,ε )+ p ε p 1 Kδ)z ) 2 + z 4 α 1 = Wz 2,ε ) p k ε 2 + p ε ε 4 2) with k > 0 and the new error signal ε 4 given by Eqs. 21) and 24) give ε 4 = z 4 α 1 + k ε + p 1 Kδ)z 2. 24) ε = z 4 α 1 = k ε + ε 4 p 1 Kδ)z 2 def = f z,ε). 25) Differentiating Eq. 24) gives with ε 4 = u α 1 + k f z,ε)+ p 1 Kδ) f 2z 1,z 2,ε )+ +K δ) f z,ε)+h z 2 ) f 2 z 1,z 2,ε ) ) z 2 ) 26) α 1 = f 2 z,ε )+h z 2 ) f 2 z,ε ) 2 + h z 2 ) kz 2 d f 2 z,ε )+ K δ)ε + hz 2 ))+Kδ) ) f z,ε) ). 27) By augmenting V 2 with a quadrati term in the error ε 4 V z 1,z 2,ε,ε 4 ) = V 2 z 1,z 2,ε )+ 1 2 p 4ε 2 4, p 4 > 0 28) we get V = V 2 + p 4 ε 4 ε 4 = Wz 2,ε ) k p ε 2 + p 4 ε 4 p p 1 4 ε + u α 1 k 2 ε + k ε 4 k p 1 Kδ)z 2+ Kδ) f2 + K δ) f z,ε)+h ) )) z 2 ) f 2 z2 29) p 1 We an now hoose the ontrol signal u = k + k 4 )ε 4 + α 1 + k 2 ε + k p 1 Kδ)z 2 p 1 Kδ) f 2 z,ε )+K δ) k ε + ε 4 p 1 Kδ)z 2 + h z 2 ) f 2 z 1,z 2,ε ) ) z 2 ) 0) with k 4 > 0 and obtain V = Wz 2,ε ) k p ε 2 k 4 p 4 ε p ε ε 4 1) whih is negative semidefinite if the free parameter p 4 satisfies p 4 p /4k k 4 ). Asymptoti stability of the origin z 1 = z 2 = ε = ε 4 = 0 follows from LaSalle s theorem [9], sine the largest invariant set in {z,ε) V = 0} is the origin. B. Observer-based design Extension an be made to the ase when the environment state is not diretly measurable, assuming that the ontat fore f is measurable, and when a potentially noisy) position measurement z 1 is available from the amera. In this ase, a linear globally exponentially onvergent full state observer for the environment an be onstruted as ẑ 1 = ẑ 2 + l 1 z 1 ẑ 1 ) def = f 1 z 1,ẑ) 2) ẑ 2 = kẑ 1 dẑ 2 + f + l 2 z 1 ẑ 1 ) def = f 2 z 1,ẑ, f ) ) whih gives the error dynamis z 1 = z 2 l 1 z 1 4) z 2 = k z 1 d z 2 l 2 z 1 5) whih an be made to onverge exponentially to zero by hoosing the possibly time-varying) observer gains l 1 and l 2 suitably. By defining and α 1 ẑ 1,ẑ 2 ) = ẑ 1 + hẑ 2 ) 6) ε = z α 1 ẑ 1,ẑ 2 ) = z ẑ 1 hẑ 2 ) 7) we an write Eq. ) as ż 2 = kz 1 dz 2 + Kδ)ε + hẑ 2 ) z 1 ) = kz 1 dz 2 + Kδ)hz 2 )+Kδ)ε + Kδ)[hẑ 2 ) hz 2 )] Kδ) z 1 8) Using the Lyapunov funtion V 1 of Eq. 17) gives V 1 = Wz 2,ε )+z 2 Kδ)[hẑ 2 ) hz 2 )] z 2 Kδ) z 1 + }{{} W z z z, z) + z 2 Kδ)ε, 9) and using V 2 in Eq. 22) we find V 2 = W +W z z + p ε p 1 Kδ)z ) 2 + z 4 α 1 def = W p k ε 2 + p ε ε 4 +W z z + ε Kδ) z 2 40)

4 where we have defined with k > 0 and where ε 4 = z 4 α 1 + k ε + p 1 Kδ)ẑ 2. 41) ε = z 4 α 1 = ε 4 k ε p 1 Kδ)ẑ 2 def = f z,ε,ẑ). 42) With V as in Eq. 28) we obtain where V = Wz 2,ε ) p k ε 2 +W z z z, z)+ε Kδ) z 2 + p ε ε 4 + p 4 ε 4 u+α 2 ) 4) α 2 = k z 4 α 1 ) α 1 + p 1 Using the ontrol signal d dt [Kδ)ẑ 2]. 44) u = k 4 ε 4 ˆα 2, k 4 > 0 45) we an rewrite Eq. 46) on the form V = Wz 2,ε ) p k ε 2 p 4 k 4 ε p ε ε W z z z, z)+ε Kδ) z 2 + p 4 ε 4 α 2 ˆα 2 ) 46) where the term ˆα 2 should approximate α 2. We hoose ˆα 2 = k z 4 α 1 ) ˆ α 1 + p 1 Kδ) f 2 + K ) δ)z 4 ẑ 2 )ẑ 2 47) where ˆ α 1 = f 2 + h ẑ 2 ) f h ẑ 2 ) k f 1 d f 2 + Kδ)+K δ)ε + hẑ 2 )) ) f + h )) ẑ 2 )ẑ 2. 48) With this hoie and using 44), 47), 48) and 2) 5), the error term α 2 ˆα 2 ) in Eq. 46) an be written α 2 ˆα 2 = l 1 +h ẑ 2 )l 2 ) z 2 l 1 z 1 ) p 1 Kδ) z 2z 2 z 2 ). 49) Furthermore, sine the observer error in Eq. 4) 5) is exponentially stable, we an use the Lyapunov funtion V 4 z 1,z 2,ε,ε 4, z) = V + z T P z z, z = [ z 1 z 2 ] T 50) whih gives V 4 = Wz 2,ε ) p k ε 2 p 4 k 4 ε 2 4 z T Q z z+ p ε ε 4 +W z z z, z)+ε Kδ) z 2 + p 4 ε 4 α 2 ˆα 2 ) 51) where Q z > 0 is hosen freely and P z > 0 satisfies the Lyapunov equation A T z P z + P z A z = Q z 52) where A z is defined by expressing 4) 5) as z = A z z. The negative semi-definiteness of V 4 in Eq. 51) ould be established as in the state feedbak ase in Setion III- A, exept for the presene of the last three extra rossterms in Eq. 51). Using the properties of K and hx), and that the exponential onvergene of the observer error gives that z 2 t) v, t 0 for some v > 0, the ross-terms an be bounded by W z z z, z) z 2 K h z 2 + z 2 K z 1 ε Kδ) z 2 ε K z 2 p 4 ε 4 α 2 ˆα 2 ) ε 4 p 4 h l + p 1 K v) z ε 4 p 4 h l l 1 z 1 + ε 4 p 4 p 1 K vz 2 with h l = l 1 + hl 2. By hoosing Q z suffiiently large, the ross-terms ontaining z 1 and z 2 an be dominated by the diagonal terms in V 4. Straightforward alulations then show that if p k k 2 4 K v) 2 /4d) we an make V 4 negative semi-definite by hoosing p 4 = 2k 4 p 2 d/ K v) 2. Asymptoti stability of the origin follows from LaSalle s theorem [9], as in Setion III-A. A. Vision based observer IV. IMPLEMENTATION In pratie, the observer in Eqs. 2) ) should be able to ompensate for positioning errors in all degrees of freedom. For this purpose, a dynami model is obtained by extending the system in Eqs. 2) ) with a number of stati states x s with ẋ s = 0, and interpret z 1 in Eq. 2) as the deviation from the nominal position x s. The extended model with x = x s,z 1,z 2 ) T is then used in an observer ˆx = Fˆx+Gf + K o t)j py h p x s,z 1 )) 5) where f is the vetor of measured ontat fores, and J p is the pseudo inverse of the Jaobian of the projetion equation h p for a standard pinhole amera. In the ase of edge measurements, only the distanes between the predited and real edges in the normal diretion of the ontour are measurable, and the orresponding equations are obtained by projeting the image spae errors onto the normal as in [10], [11]. Edge positions are found using a robust sub-pixel algorithm whih loalizes features at three different sales in the image, where visible features are predited from frame to frame using a Binary Searh Partitioning tree desription of the objet. The observer gain K o t) is obtained by using a time-varying Kalman filter. A multi-rate disrete-time approximation of the Kalman filter is used, where the dynamis and input fores are sampled at a signifiantly faster rate than the amera. It is assumed that the errors in the image measurements ε y an be modeled as Gaussian, spatially unorrelated white noise with variane σ 2, so that an effetive measurement error ovariane ε x = J pε y an be obtained from E[ε x ε T x ] = E[J pε y J pε y ) T ] = J T p J p ) 1 σ 2 54)

5 0.05 Approximation error Predited edge y i N) y i z 1, z [m] 0 y i+1 N) 0.05 y i t[s] Fig. 2. Edge detetion in the normal diretion of the predited edges. Fig.. Environment position z 1 dashed) and robot tool tip position z solid) during simulated ontat transition with ative damping. B. Control law The ontroller in Eq. 45) will damp the system by ontrolling the relative position of the robot tip and the ontat point, whih makes sense during the approah phase. One ontat has been established, it is neessary to swith to diret ontrol of the ontat fore. The ontrol law in Eq. 45) is given by u = k 4 ε 4 k z 4 α 1 )+ ˆ α 1 p 1 d dt [Kδ)ẑ 2] = k + k 4 )z 4 α 1 ) k k 4 z α 1 ) + ˆ α 1 k 4 p 1 Kδ)ẑ 2 p 1 d dt [Kδ)ẑ 2] 55) where the first and seond terms on the right hand side of 55) onsist of feedbak from the veloity- and position errors, respetively, while the third term an be seen as a ompensation of the estimated environment aeleration. We now replae the position error z α 1 ) in 55) with the ontrol error e p defined as e p = θψ 1 f ))Ψ 1 f )+θẑ 1 z ) z ẑ 1 ) hẑ 2 ) 56) where θx) is the step funtion θx) = 1,x 0, θx) = 0,x < 0. This leads to a swithed ontrol law approximating Eq. 45) whih inorporates a proportional feedbak from the measured fore during the ontat phase. V. RESULTS Simulations were arried out on a model of a three-link robot with a single point ontat with the environment, and with ontat fores ontrolled in the vertial diretion only. The environment dynamis was given by a poorly damped mass-spring-damper system with mass 10 kg, stiffness 2000 N/m, and linear damping of N/m/s). The real ontat stiffness Kδ) was set to N/m for δ > 0. The damping funtion hẑ 2 ) was omposed of pieewise seond f [N] t[s] Fig. 4. Contat fore f during simulated ontat transition and ative damping. order polynomials and hosen to orrespond roughly to an additional damping of d ative = 200 N/m/s) in order to obtain a ritially damped response for small veloities, while hẑ 2 ) saturates at high veloities for a maximum desired ontat fore of 5 N. An additional onstant term was also added to the ontrol signal in order to obtain a ontat fore f = 15 N in stationarity. The ontroller parameters were set to k = k 4 = 20, p = The Kalman filter was set to trak the translation of the workpiee, a textured ubi box with side 40 m, from syntheti pixels amera images rendered in real time using the standard D graphis API OpenGL, see Fig. 5 for an example image. In order to analyze the robustness of the system against modeling errors, a number of extra error soures were added in the simulation:

6 The method works well in simulation, inluding some robustness to unertainties in the model parameters k, d and Kδ). The resulting ontroller has a relatively simple struture, as seen from Eqs. 55) and 56). Different hoies of the damping funtion hx) will give different properties of the system. In pratie, it is often possible and benefiial to hoose a damping funtion that violates property 15), espeially in order to obtain a suffiient damping for osillations with small amplitude. The role of the fore sensor is twofold. Firstly, it is used to stiffen the ontrolled robot by fore feedforward, seondly to measure the urrent ontat fore for feedbak. In order to perform fore feed-forward, the assumption of a rigid manipulator restrits the method to environments whih are signifiantly more ompliant than the manipulator itself, or where the effetive inertia of the robot is so large that the effet of interation fore an be ignored. Additionally, for very high ontat stiffness between the robot and environment large ontat fores an build up quikly, espeially during the transition phase, meaning that additional physial ompliane or padding may be neessary in order to derease the ontat stiffness. Fig. 5. Example of a rendered image, with superimposed wireframe model showing the estimated position of the ubi box. The model of Kδ) is a smoothed version of the true stiffness funtion, and the modeled elasti stiffness was 100% higher than the true value. Additional spatial noise was added to the syntheti images before the feature extration step. The estimated value of parameter d in the ontroller was 100% higher than the true value, while the stiffness k was assumed to be known. The amera data and the observer were sampled at 40 ms, while the fore signal sampling and the damping ontrol law were exeuted at a shorter period of 4 ms. A time delay of 40 ms for image apture and proessing was added for the amera data, and modeled in the Kalman filter. For the motion ontrol in the unonstrained diretions a standard omputed torque ontroller was used, with feedforward from the estimated environment position. The environment osillation mode was exited, and at time t = 2 s the damping ontroller was started. Fig. shows the resulting robot- and environment positions, while Fig. 4 shows the resulting ontat fore. The limiting of the ontat fore just below the desired value of 5 N an be seen at t = 2.4 s, whih is the reason why the desired ritially damped dynamis is not ahieved initially. VI. DISCUSSION VII. CONCLUSIONS In this paper, we have presented a method for vision/fore-feedbak ontrol of a system onsisting of a rigid manipulator interating with an environment with linear dynamis. An observer-based bakstepping ontrol approah was used to find a ontroller that injets a desired damping into the dynamis of the environment, using both visual feedbak and fore sensing. Simulations were used to validate the approah. The hoie of the ontroller parameters and damping funtion gives a onsiderable design flexibility, whih an for instane be used to design damping ontrollers that attempt to limit the applied interation fore. REFERENCES [1] B. Siiliano and L. Villani, Robot Fore Control, Kluwer Aademi Publishers, [2] N. Dioiaiti, C. Melhiorri, and S. Stramigioli, Contat impedane estimation for roboti systems, in Pro. IEEE/RSJ Int. Conf. Intelligent Robots and Systems, Sendai, Japan, 2004, vol., pp [] A. de Lua and C. Manes, Modeling of robots in ontat with a dynami environment, IEEE Trans. Robotis and Automation, vol. 10, no. 4, pp , August [4] M. Vukobratovi, V. Potkonjak, and A. Rodi, Contribution to the dynami study of humanoid robots interating with dynami environment, Robotia, vol. 22, no. 4, pp , [5] J. Baeten, H. Bruyninkx, and J. De Shutter, Combining eye-inhand visual servoing and fore ontrol in roboti tasks using the task frame, in Pro. IEEE Int. Conf. Multisensor Fusion, Taipei, Taiwan, August 1999, pp [6] S. Jörg, J. Langwald, J. Stelter, G. Hirzinger, and C. Natale, Flexible robot-assembly using a multi-sensory approah, in IEEE Int. Conf. Robotis and Automation, San Franiso, CA, USA, 2000, pp [7] G. Morel, E. Malis, and S. Boudet, Impedane based ombination of visual and fore ontrol, in Pro. IEEE Int. Conf. Robotis and Automation, Leuven, Belgium, May 1998, pp [8] J. Park and K Khatib, Multi-link multi-ontat fore ontrol for manipulators, in Pro. IEEE Int. Conf. Robotis and Automation, Barelona, Spain, April 2005, pp [9] M. Krstić, I. Kanellakopoulos, and P. V. Kokotović, Nonlinear and Adaptive Control Design, Wiley, New York, NY, [10] T. Drummond and R. Cipolla, Real-time traking of omplex strutures with on-line amera alibration, in British Mahine Vision Conf., 1999, vol. 2, pp [11] F. Martin and R. Horaud, Multiple amera traking of rigid objets, Int. J. of Robotis Researh, vol. 21, no. 2, pp , February 2002.