A Position-Based Visual Impedance Control for Robot Manipulators

Transcription

1 2007 IEEE International Conference on Robotics and Automation Roma, Ital, April 2007 ThB21 A Position-Based Visual Impedance Control for Robot Manipulators Vinceno Lippiello, Bruno Siciliano, and Luigi Villani Abstract In this paper, an approach to interaction control of a robot manipulator with a partiall known environment is proposed The environment is a rigid object of known geometr but of unknown and possibl time-varing position and orientation An algorithm for online estimation of the object pose is adopted, based on visual data provided b a camera as well as on forces and moments measured during the interaction with the environment This algorithm is embedded into an impedance control scheme, resulting in a position-based visual impedance control Eperimental results are presented for the case of an industrial robot manipulator in contact with a planar surface I INTRODUCTION Vision and force are two complementar sensing capabilities that can be eploited in a snergic wa to enhance the autonom of a robot manipulator In fact, thanks to the visual perception, a robot ma achieve global information on the surrounding environment that can be used for task planning and obstacle avoidance On the other hand, the perception of the force applied to the end effector allows adjusting its motion so that the local constraints imposed b the environment during the interaction are satisfied In recent ears, several approaches where force and vision measurements are combined in the same feedback control loop have been proposed, such as hbrid visual/force control [1], shared and traded control [2], [3] or visual impedance control [4], [5] The approach adopted in this work is based on the observation that, when the robot moves in free space, a position-based visual servoing strateg [6] can be adopted, where vision is used to estimate the relative pose of the robot with respect to the environment On the other hand, when the robot comes into contact with the environment, a position-based impedance interaction control strateg can be adopted [7] The impedance controller ma take advantage of the estimation of the geometr of the environment computed online from all the available sensor data, ie, visual, force and joint position measurements The estimation algorithm, based on the Etended Kalman Filter (EKF), is an etension of the visual pose estimation approach proposed in [8] to the case that also force and joint position measurements are used Remarkabl, the same estimation algorithm can be adopted both in free space and during the interaction, simpl modifing the measurements set of the EKF The resulting control scheme can be classified as a position-based visual impedance control PRISMA Lab, Dipartimento di Informatica e Sistemistica, Università di Napoli Federico II, Via Claudio 21, Napoli, ITALY {lippiell,siciliano,lvillani}@uninait Desired Task Dnamic Trajector Planner Pose Control Pose feedback (26h) Fig 1 Motion Control Joint feedback (1kh) Pose Estimation Visual Sstem Robot Position-based visual servoing Target Object This scheme was originall proposed in [9], where onl preliminar simulation results are shown In this paper, the position-based impedance control strateg is eperimentall tested on a 6-DOF industrial robot equipped with a standard analog camera and a wrist force/torque sensor II POSITION-BASED VISUAL IMPEDANCE The position-based visual impedance control proposed in this paper can be derived from a tpical position-based visual servoing scheme represented in Fig 1 This scheme requires the estimation of the relative pose of the robot end-effector with respect to a target object b using the vision sstem; the estimated pose is then fed back to a pose controller Notice that pose estimation is a computationall demanding task, because it requires processing of the measurements of some geometric features etracted from the images of one or more cameras Hence, the frequenc of the pose estimation algorithm is usuall lower than the frequenc of the pose control loop (greater or equal to 500 H in standard robot manipulators, to guarantee tracking accurac and disturbance rejection) The bottleneck is usuall represented b the camera frame rate (between 25 H and 60 H for low-cost cameras used in common applications) Pose control is performed through an inner-outer control loop The inner loop implements motion control (independent joint control or an kind of joint space or task space control) In the outer loop, the block named Dnamic Trajector Planner computes the trajector for the end-effector on the basis of the current pose of the robot with respect to the object and of the desired task If the robot interacts with the target object, an interaction control strateg must be adopted, based on force and moment measurements achieved via a force/torque sensor mounted at the end-effector wrist In the presence of uncertainties on the object location, one of the most effective interaction control /07/$ IEEE 2068

2 Desired Task Dnamic Trajector Planner Fig 2 Pose Control Pose feedback (26h) Impedence Control Joint feedback (1kh) Pose Estimation Robot Force feedback (1kh) Visual Sstem Position-based visual impedance control Target Object strategies is impedance control, which, in addition, is able to manage both contact and free-space motion phases [7] When the robot is in contact with the target object, the force/torque measurements, as well as the joint position measurements, can be suitabl eploited, together with the visual measurements, to improve the accurac of the estimation of the target object pose The resulting control algorithm, represented in Fig 2, can be classified as a position-based visual impedance control A position-based impedance control is adopted, based on the concept of compliant frame [7] In detail, the Dnamic Trajector Planner generates a pose trajector for a desired end-effector frame d specified in terms of the position of the origin p d and orientation matri R d Moreover, a compliant frame r is introduced, specified in terms of p r and R r Then, a mechanical impedance between the desired and the compliant frame is considered, so as to contain the values of the interaction force h and moment m In other words, the desired position and orientation, together with the measured contact force and moment, are input to the impedance equation which, via a suitable integration, generates the position and orientation of the compliant frame to be used as a reference for the motion controller of the robot end effector As far as the compliant frame is concerned, the position p r can be computed via the translational impedance equation M p p dr + D p ṗ dr + K p p dr = h, (1) where p dr = p d p r, and M p, D p and K p are positive definite matrices representing the mass, damping, and stiffness characteriing the impedance The orientation of the reference frame R r is computed via a geometricall consistent impedance equation similar to (1), in terms of an orientation error based on the (3 1) vector r ɛ dr, defined as the vector part of the unit quaternion that can be etracted from r R d = R T r R d The corresponding mass, damping and inertia matrices are M o, D o and K o respectivel More details about the geometricall consistent impedance based on the unit quaternion can be found in [7] The estimation of the object pose is crucial in order to compute the desired absolute end-effector motion from the desired task assigned in terms of the relative motion with respect to the target object In the following, the main issues concerning pose estimation are considered in detail III MODELLING Consider a robot in contact with an object, a wrist force sensor and a camera mounted on the end-effector (ee-inhand) or fied in the workspace (ee-to-hand) In this tion, some modelling assumptions concerning the object, the robot and the camera are presented A Object The position and orientation of a frame attached to a rigid object O o o o o with respect to a base coordinate frame O can be epressed in terms of the coordinate vector of the origin o o = [ ] T o o o and of the rotation matri R o (ϕ o ), where ϕ o is a (p 1) vector corresponding to a suitable parametriation of the orientation In the case that a minimal representation of the orientation is adopted, eg, Euler angles, it is p =3, while it is p =4if unit quaternions are used Hence, the (m 1) vector o = [ ] o T o ϕ T T o defines a representation of the object pose with respect to the base frame in terms of m =3+p parameters The homogeneous coordinate vector p = [ p T 1 ] T of a point P of the object with respect to the base frame can be computed as p = H o ( o ) o p, where o p is the homogeneous coordinate vector of P with respect to the object frame and H o is the homogeneous transformation matri representing the pose of the object frame referred to the base frame: [ ] Ro (ϕ H o ( o )= o ) o o 0 T, 3 1 where 0 3 is the (3 1) null vector It is assumed that the geometr of the object is known and that the interaction involves a portion of the eternal surface which satisfies the continuousl differentiable scalar equation ϕ( o p)=0 The unit vector normal to the surface at the point o p and pointing outwards can be computed as: o n( o p)= ( ϕ(o p)/ o p) T ( ϕ( o p)/ o p, (2) where o n is epressed in the object frame Notice that the object pose o is assumed to be unknown and ma change during the task eecution As an eample, a compliant contact can be modelled assuming that o changes during the interaction according to an elastic law A further assumption is that the contact between the robot and the object is of point tpe and frictionless Therefore, when in contact, the tip point P q of the robot instantaneousl coincides with a point P of the object, so that the tip position o p q satisfies the constraint equation: ϕ( o p q )=0 (3) Moreover, the (3 1) contact force o h is aligned to the normal unit vector o n B Robot The case of a n-joints robot manipulator is considered, with n 3 The tip position p q can be computed via the direct kinematics equation: 2069

3 p q = k(q), (4) where q is the (n 1) vector of the joint variables Also, the velocit of the robot s tip v Pq can be epressed as v Pq = J(q) q where J = k(q)/ q is the robot Jacobian matri The vector v Pq can be decomposed as o v Pq = o ṗ q + Λ( o p q ) o ν o, (5) with Λ( ) =[I 3 S( )], where I 3 is the (3 3) identit matri and S( ) denotes the (3 3) skew-smmetric matri operator In Eq (5), o ṗ q is the relative velocit of the tip point P q with respect to the object frame while o ν o =[ o v T O o o ω T o ] T is the velocit screw characteriing the motion of the object frame with respect to the base frame in terms of the translational velocit of the origin v Oo and of the angular velocit ω o ; all the quantities are epressed in the object frame When the robot is in contact to the object, the normal component of the relative velocit o ṗ q is null, ie: o n T ( o p q ) o ṗ q =0 (6) C Camera A frame O c c c c attached to the camera (either in eein-hand or in ee-to-hand configuration) is considered B using the classical pin-hole model, a point P of the object with coordinates c p = [ ] T with respect to the camera frame is projected onto the point of the image plane with coordinates [ ] X = λ [ ] c (7) Y where λ c is the focal length of the lens of the camera Let H c denote the homogeneous transformation matri representing the pose of the camera frame referred to the base frame For ee-to-hand cameras, the matri H c is constant, and can be computed through a suitable calibration procedure, while for ee-in-hand cameras this matri depends on the camera current pose c and can be computed as: H c ( c )=H e ( e ) e H c where H e is the homogeneous transformation matri of the end effector frame e with respect to the base frame, and e H c is the homogeneous transformation matri of camera frame with respect to end effector frame Notice that e H c is constant and can be estimated through suitable calibration procedures, while H e depends on the current end-effector pose e and ma be computed using the robot kinematic model Therefore, the homogeneous coordinate vector of P with respect to the camera frame can be epressed as c p = c H o ( o, c ) o p (8) where c H o ( o, c )= c H 1 ( c )H o ( o ) Notice that c is constant for ee-to-hand cameras; moreover, the matri c H o does not depend on c and o separatel but onl on the relative pose of the object frame with respect to the camera frame The velocit of the camera frame with respect to the base frame can be characteried in terms of the translational velocit of the origin v Oc and of angular velocit ω c These vectors, epressed in camera frame, define the velocit screw c ν c = [ c v T O c c ω T c ] T Analogousl to (5), the absolute velocit of the origin O o of the object frame can be computed as c v Oo = c ȯ o + Λ( c o o ) c ν c, (9) where c o o is the vector of the coordinates of O o with respect to camera frame and c ȯ o is the relative velocit of O o with respect to camera frame; all the quantities are epressed in camera frame On the other hand, the absolute angular velocit c ω o of the object frame epressed in camera frame can be computed as c ω o = c ω o,c + c ω c (10) where c ω o,c represents the relative angular velocit of the object frame with respect to the camera frame The two equations (9) and (10) can be rewritten in the compact form c ν o = c ν o,c + Γ( c o o ) c ν c (11) where c ν o = [ c v T O c o ω T o ] T is the velocit screw corresponding to the absolute motion of the object frame, c ν o,c = [ c ȯ T o c ω T o,c ]T is the velocit screw corresponding to the relative motion of the object frame with respect to camera frame, and the matri Γ( ) is defined as [ I3 S( ) Γ( ) = O 3 I 3 where O 3 denotes the (3 3) null matri The velocit screw r ν s of a frame s with respect to a frame r can be epressed in terms of the time derivative of the vector s representing the pose of frame s through the equation r ν s = r L( s )ẋ s (12) where r L( ) is a Jacobian matri depending on the particular choice of coordinates for the orientation The epressions of r L( ) for different kinds of parametriation of the orientation can be found, eg, in [10] IV OBJECT POSE ESTIMATION When the robot moves in free space, the unknown object pose can be estimated online b using the data provided b the camera; when the robot is in contact to the target object, also the force measurements and the joint position measurements can be used In this tion, the equations mapping the measurements to the unknown position and orientation of the object are derived Then, the estimation algorithm based on the EKF is presented A Vision Vision is used to measure the image features, ie, an structural feature that can be etracted from an image, corresponding to the projection of a phsical feature of the object onto the camera image plane ], 2070

4 An image feature can be characteried b a set of scalar parameters f j that can be grouped in a vector f = [f 1 f k ] T, where k is the dimension of the image feature parameter space The mapping from the position and orientation of the object to the corresponding image feature vector can be computed using the projective geometr of the camera and can be written in the form f = g f ( c H o ( o, c )), (13) where onl the dependence from the relative pose of the object frame with respect to camera frame has been eplicitl evidenced For the estimation of the object pose, the computation of the Jacobian matri J f = g f o is required To this purpose, the time derivative of (13) can be computed in the form ḟ = g f ẋ o + g f ẋ c, (14) o c where the ond term in the right hand side is null for ee-to-hand cameras On the other hand, the time derivative of (13) can be epressed also in the form ḟ = J o,c c ν o,c (15) where the matri J o,c is the Jacobian mapping the relative velocit screw of the object frame with respect to the camera frame into the variation of the image feature parameters The epression of J o,c depends on the choice of the image features; eamples of computation can be found in [10] Taking into account the velocit composition (11), Eq (15) can be rewritten in the form ḟ = J o,c c ν o J c c ν c (16) where J c = J o,c Γ( c o o ) is the Jacobian corresponding to the contribution of the absolute velocit screw of the camera frame, known in the literature as interaction matri [11] Considering Eq (12), the comparison of (16) with (14) ields J f = J o,c c L( o ) (17) B Force and joint measurements In the case of frictionless point contact, the measure of the force h at the robot tip during the interaction can be used to compute the unit vector normal to the object surface at the contact point o p q, ie, n h = h h (18) On the other hand, vector n h can be epressed as a function of the object pose o and of the robot position p q in the form n h = R o o n( o p q )=g h ( o, p q ), (19) being o p q = R T o (p q o o ) For the estimation of the object pose, the computation of the Jacobian matri J h = g h o is required To this purpose, the time derivative of (19) can be epressed as ṅ h = g h ẋ o + g h ṗ o p q (20) q On the other hand, the time derivative of (19) can be computed also in the form ṅ h = Ṙo o n( o p q )+R o o N( o p q ) o ṗ q, (21) where o N( o p q )= o n/ o p q depends on the surface curvature and o ṗ q can be computed from (5) Hence, comparing (20) with (21) and taking into account (12) and the equalit Ṙo o n( o p q )= S(n h )ω o, the following epression can be found: J h = [N S(n h ) NS(p q o o )]L( o ), (22) where N = R o o N( o p q )R T o The measurement of the joint position vector q can be used to evaluate the position of the point P of the object when in contact to the robot s tip point P q, using the direct kinematics equation (4) In particular, it is significant computing the scalar δ hq = n T h p q = g hq ( o, p q ), (23) using also the force measurements via Eq (18) For the estimation of the object pose, the computation of the Jacobian matri J hq = g hq o is required As in the previous subtion, the time derivative of δ hq can be epressed as δ hq = g hq ẋ o + g hq ṗ o p q (24) q On the other hand, the time derivative of δ hq can be computed also as δ hq = ṅ T h p q + n T h R o ( o ṗ q + Λ( o p q ) o ν o ) where the epression of the absolute velocit of the point P q in (5) has been used Using the identit (6), the above equation can be rewritten as δ hq = p T q ṅh + n T h Λ(p q o o )ν o (25) Hence, comparing (24) with (25) and taking into account (21), (22) and (12), the following epression can be found J hq = p T q J h + n T h Λ(p q o o )L( o ) (26) C Etended Kalman Filter The pose vector o of the object with respect to the base frame can be estimated using an Etended Kalman Filter 2071

5 To this purpose, a discrete-time state space dnamic model has to be considered, describing the object motion The state vector of the dnamic model is chosen as w = [ ] T T o ẋ T o For simplicit, the object velocit is assumed to be constant over one sample period T s This approimation is reasonable in the hpothesis that T s is sufficientl small The corresponding dnamic modeling error can be considered as an input disturbance γ described b ero mean Gaussian noise with covariance Q The discrete-time dnamic model can be written as w k = Aw k 1 + γ k, (27) where A is the (2m 2m) block matri [ ] Im T A = s I m O m I m The output of the Kalman filter, in the case that all the available data can be used, is the vector of the measurements at time kt s ζk = [ ζ T f,k ζ T T h,k ζ ] T hq,k, where ζ f,k = f k + µ f,k, ζ h,k = h k + µ h,k, and ζ hq,k = δ k +µ hq,k, being µ the measurement noise The measurement noise is assumed to be ero mean Gaussian noise with covariance Π Taking into account the Eqs (13), (19), and (23), the output model of the Kalman filter can be written in the form: ζ k = g(w k )+µ k, where [µ T f,k µ T h,k µ T hq,k] T and g(w k )= [ g T f (w k) g T h (w k) g T hq (w k) ] T (28) where onl the eplicit dependence on the state vector w k has been evidenced Since the output model is nonlinear in the sstem state, the EKF must be adopted, which requires the computation of the Jacobian matri of the output equation C k = g(w) [ ] g(w) w = O, w=ŵ k,k 1 o w=ŵ k,k 1 where O is a null matri of proper dimensions corresponding to the partial derivative of g with respect to the velocit variables, which is null because function g does not depend on the velocit The Jacobian matri g(w)/ o, in view of (17), (22), and (26) has the epression g(w) = [ J T f J T h J T ] T hq o The equations of the recursive form of the EKF are standard and are omitted here Notice that the proposed algorithm can be used to estimate online the pose of an object in the workspace; hence it allows the computation of the constraint Eq (3) with respect to the base frame in the form ϕ(r T o (p q o o )) = 0 This information can be suitabl eploited to implement an kind of interaction control strateg Fig 3 Eperimental setup V EXPERIMENTS The eperimental setup (Fig 3) is composed b a 6-DOF industrial robot Comau SMART-3 S with an open control architecture based on RTAI-Linu operating sstem A siais force/torque sensor ATI FT with force range of ±130 N and torque range of ±10 N m is mounted at the arm s wrist, providing readings of si components of generalied force at 1 ms A visual sstem composed b a PC equipped with a Matro GENESIS board and a Son 8500CE B/W camera is available The camera is fied and calibrated with respect to the base frame of the robot The environment is a planar wooden horiontal surface, described b the equation o n To p =0, assuming that the origin O o of the object frame is a point of the plane and the ais o is aligned to the normal o n The plane has an estimated stiffness (along o n) of about N/m The object features are 8 landmark points ling on the plane at the corners of two rectangles of cm sie (as in Fig 3) The end-effector tool is a rigid stick of 15 cm length ending with a small sphere, so that the hpothesis of point contact is reasonable The impedance parameters are chosen as: M p =40I 3, D p = 263I 3 and K p = 1020I 3, M o = 15I 3, D o = 174I 3 and K o = 3I 3 ;a1 ms sampling time has been selected for the impedance and pose control Notice that the stiffness of the object is much higher that the positional stiffness of the impedance, so that the environment can be considered rigid The desired task is planned in the object frame and consists in a straight-line motion of the end-effector along the o - ais keeping a fied orientation with the stick normal to the o o -plane The final position is o p f =[ ] T m, which is chosen to have a normal force of about 50 Nat the equilibrium, with the selected value of the impedance positional stiffness A trapeoidal velocit profile time-law is adopted, with a cruise velocit of 001 m/s The absolute trajector is computed from the desired relative trajector using the current object pose estimation The final position of the end-effector is held for 2 s; after, a vertical motion in the opposite direction is commanded In the EKF, the non null elements of the matri Π have been set equal to 25 piel 2 for f, for n h and 10 6 m 2 for δ hq The state noise covariance matri has been selected so as to give a rough measure of the errors due to the 2072

6 N N Nm Nm Fig 4 Measured force (top) and moment (bottom) in the 1 st eperiment Fig 5 Measured force (top) and moment (bottom) in the 2 nd eperiment simplification introduced on the model (constant velocit), b considering onl velocit disturbance, ie Q = diag{0, 0, 0, 0, 0, 0, 0, 10, 10, 10, 1, 1, 1} 10 9 Notice that the unit quaternion has been used for the orientation in the EKF, to avoid an occurrence of representation singularities Moreover a 38 ms sampling time has been used for the estimation algorithm, corresponding to the tpical camera frame rate of 26 H Two different eperiments are presented, to show the effectiveness of the use of force and joint position measurements, besides visual measurements In the first eperiment onl the visual measurements are used The time histor of the measured force and moments in the sensor frame are reported in Fig 4 Notice that the force is null during the motion in free space and becomes different from ero after the contact The impedance control keeps the force limited during the transient while, at stead state, the force component along the ais reaches a value of about 65 N, which is different from the desired value of 50 N This is caused b the presence of estimation errors on the position of the plane due to calibration errors of the camera and to the use of a monocular vision sstem Moreover, the moment measured b the sensor is different from ero due to the misalignment between the estimated and the real normal direction of the plane The same task is repeated using also the contact force and the joint position measurements for object pose estimation; the results are reported in Fig 5 It can be observed that the benefit of using additional measurements in the EKF produces a force in the vertical direction which is ver close to the desired value, due to the improved estimation of the position of the plane; moreover, the contact moment is also reduced because of the better estimation of the normal direction of the plane VI CONCLUSION A 6-DOF position-based visual impedance control scheme was proposed in this paper The environment is a rigid object of known geometr but of unknown and possibl time varing pose A pose estimation algorithm is adopted, based on visual, force and joint positions data The proposed approach can be cast into an kind of interaction control strateg Moreover, it can be applied also to the case of multiple cameras in hbrid ee-in-hand/ee-to-hand configuration REFERENCES [1] K Hosoda, K Igarashi, and M Asada, Adaptive hbrid control for visual and force servoing in an unknown environment, Robotics & Automation Magaine, vol 5, no 4, pp 39 43, 1998 [2] B J Nelson, J D Morrow, and P K Khosla, Improved force control through visual servoing, in 1995 American Control Conference, pp [3] J Baeten and J D Schutter, Integrated Visual Servoing and Force Control The Task Frame Approach Heidelberg, D: Springer, 2004 [4] G Morel, E Malis, and S Boudet, Impedance based combination of visual and force control, in 1998 IEEE Int Conf on Robotics and Automation, pp [5] T Olsson, R Johansson, and A Robertsson, Fleible force-vision control for surface following using multiple cameras, in 2004 IEEE/RSJ Int Conf on Intelligent Robots and Sstem, pp [6] WJ Wilson, CCW Hulls, and GS Bell, Relative end-effector control using cartesian position based visual servoing, IEEE Transactions on Robotics and Automation, vol 12, pp , 1996 [7] B Siciliano and L Villani, Robot Force Control Boston, MA: Kluwer Academic Publishers, 1999 [8] V Lippiello and L Villani, Managing redundant visual measurements for accurate pose tracking, Robotica, vol 21, pp , 2003 [9] V Lippiello, B Siciliano, L Villani, Robot interaction control using force and vision, 2006 IEEE/RSJ Int Conf on Intelligent Robots and Sstems, [10] V Lippiello, B Siciliano, and L Villani, 3D pose estimation for robotic applications based on a multi-camera hbrid visual sstem, in 2006 IEEE Int Conf on Robotics and Automation, [11] B Espiau, F Chaumette, and P Rives, A new approach to visual servoing in robotics, IEEE Transactions on Robotics and Automation, vol 8, pp ,