Calculation of Robot Gripper Centroid, Mass, and Force Sensor Offsets

Transcription

1 CENTER FOR MACHINE PERCEPTION CZECH TECHNICAL UNIVERSITY IN PRAGUE Calculation of Robot Gripper Centroid, Mass, and Force Sensor Offsets Jan Mrňa, Matej Murgaš, Vladimír Smutný September 3th, 213 RESEARCH REPORT The authors were supported by EC project FP CloPeMa. Any opinions expressed in this paper do not necessarily reflect the views of the European Community. The Community is not liable for any use that may be made of the information contained herein. Center for Machine Perception, Department of Cybernetics Faculty of Electrical Engineering, Czech Technical University Technická 2, Prague 6, Czech Republic fax , phone , www:

2

3 Calculation of Robot Gripper Centroid, Mass, and Force Sensor Offsets Jan Mrňa, Matej Murgaš, Vladimír Smutný September 3th, 213 Abstract This report explains how robot gripper mass, centroid position and force/torque sensor offsets can be calculated using robot joint angle data and force data measured by sensor ATI Mini45. 1 Introduction Lifting clothes with robot arm requires precise force and torque measuring. Force sensor measures sum of gravity forces affecting gripper and object, which is being held by the gripper. Therefore, mass of the gripper must be calculated first if we want to measure force affecting object (eg. clothing) itself. Also, there is some (unknown) offset in measurement caused by temperature. This offset needs to be calculated. When calculating torque offset, knowledge of centroid position is needed. Following sections explains how all these calculations can be done. In following sections, all values indexed with W are in world coordinate system (hereinafter referred to as WCS); all values indexed with H are in robot gripper coordinate system (hereinafter referred to as GCS). 1

4 Figure 1: Transformation of WCS to GCS. The figure illustrates relationship between WCS, GCS, and important vectors. Vector r G is vector pointing from GCS origin to centroid of robot gripper. Vector g is gravity vector. R(q) represents transformation from WCS to GCS (q is vector of joint angles). 2 System Description In this section general equations leading to desired equation system will be discussed. Gravity force affecting gripper can be calculated using formula: F = mg (1) where F is the gravity force vector, m represents unknown mass of gripper, and g is the gravity vector. Similarly, torque affecting gripper can be calculated using formula: M = mr g (2) where r is a unknown vector from GCS origin to the gripper centroid, M and F represent force and torque vectors: F x F = F y (3) F z M x M = M y (4) M z 2

5 Eq. (1) and (2) in matrix notation: [ ] F M = m 3 System Analysis r z r y r z r x r y r x g = mqg (5) Orientation of a gripper in WCS is given by rotation matrix R(q), which is in the form: r 11 r 12 r 13 R(q) = r 21 r 22 r 23 (6) r 31 r 32 r 33 Gravity vector in the world coordinate system g W is known to be: g W = (7) g Multiplication of the vectors in world coordinate system gives the vector in gripper coordinate system. Gravity vector in the gripper coordinate system g G can be calculated using R(q) : g G = R(q)g W = R(q) (8) g Vector r G points from the gripper coordinate system origin to the centroid of the gripper: r G = r y (9) r z It is constant in the gripper coordinate system. Both m and r G are unknown. The force sensor ATI Mini 45 is negatively influenced by many factors, including noise from electrical grid, mechanic vibrations (cars, trams, subway), temperature, drift, and even light. The influences could be divided into high frequency effects (mechanical vibrations, electrical noise) which are best handled by properly set low-pass filter and low frequency effects (temperature, drifts,...) which could be modeled in the short time by an offset:: r x F = F m F o (1) 3

6 M = M m M o (11) M and F are real values affecting the robot gripper, M m and F m are data measured by sensor and M o and F o are (slowly changing offsets. Combining matrix eq. (5) with equations (8), (1) and (11) results in: [ ] [ ] [ ] F Fm Fo = M M m M o = mqr(q) g (12) Eq. (12) contains 1 unknowns: offset values for both force and torque, the centroid position r x, r y, r z in matrix Q, and mass m. Separating eq. (12) gives us 2 new equations: F = F m F o = mr(q) (13) g r z r y M = M m M o = m r z r x R(q) (14) r y r x g Rearranging eq. (13) will result in equation: Where unknown x F represents following matrix: A F represents: And b F represents: A F x F = b F (15) x F = F xo F yo F zo m Similarly, rearranging (14) will result in equation: (16) 1 r 13 g A F = 1 r 23 g (17) 1 r 33 g F x b F = F y (18) F z A M x M = b M (19) 4

7 Where unknown x M represents following matrix: x M = M xo M yo M zo r x r y r z (2) A M represents: 1 mr 33 g mr 23 g A M = 1 mr 33 g mr 13 g (21) 1 mr 23 g mr 13 g And measured b M represents: M x b M = M y (22) M z 4 Solution 4.1 Force offsets and mass Since matrix A M includes unknown m, it is necessary to calculate force equation A F x F = b F first. Solution can be found with help of linear least squares solved by orthogonal decomposition (in this case, singular value decomposition method - SVD - is used). Applying SVD on matrix A F gives us three matrices U F, S F, V F. Force equation system has solution in form: x F = V F S + F UT F b F (23) 4.2 Torque offsets and centroid position With knowledge of m torque equation system A M x M = b M can be solved. Again linear least square method could be applied. Similarly as in (23), solution will be in form: x M = V M S + M UT Mb M (24) Where U M, S M, V M are matrices created by applying SVD on matrix A M. 5

8 4.3 Conclusion Both Matlab and Python (numpy library) have SVD and pseudo-inverse calculation functions, therefore this procedure is quite easy to implement. However, noise affecting force sensor may cause severe errors in results. Best precaution against these high frequency errors are well configured low-pass filters. For proper results, measurements must be made in different positions/rotations. 6