IROS2019国际学术会议论文集 2597

IEEE TRANSACTIONS ON ROBOTICS VOL 34 NO 6 DECEMBER 20181651 On the Covariance ofXinAX XB Huy Nguyenand Quang Cuong Pham Abstract Hand eye calibration which consists in identifying the rigid body transformation between a camera mounted on the robot end effector and the end effector itself is a fundamental problem in robot vision Math ematically thisproblemcanbeformulatedas solveforX inAX XB Inthispaper weprovidearigorousderivationofthecovarianceofthesolu tionX whenAandBarerandomlyperturbedmatrices Thisfi ne grained information is critical for applications that require a high degree of percep tion precision Our approach consists in applying covariance propagation methods in SE 3 Experiments involving synthetic and real calibration data confi rm that our approach can predict the covariance of the hand eye transformation with excellent precision Index Terms Calibration and identifi cation hand eye calibration uncertainty I INTRODUCTION Hand eye calibration which consists in identifying the rigid body transformation between a camera eye mounted on the robot end effector and the end effector hand itself is a fundamental problem in robot vision Mathematically this problem can be formulated as solve for X in AX XB where X is the unknown 4 4 hand eye transformation matrix and A and B are known 4 4 transformation matrices see details in Section II A Starting from the late 1980s a large amount of the literature has been devoted to this problem and a number of effi cient methods have been developed see e g 1 6 Inthispaper weareinterestednotmerelyinsolvingforX butmore comprehensively in evaluating the covariance of X from those of A and B where A and B are now randomly perturbed transformation matrices This fi ne grained information is critical in high precision robotics applications for several reasons A Motivations Theuncertaintyoftheobjectposeestimationcomesfromthreemain sources 1 theuncertaintyoftheobjectposeestimationinthecameraframe 2 the uncertainty of the hand eye calibration 3 the uncertainty of the robot end effector positioning In practice source 2 arguably contributes the most For instance a tiny orientation error of 0 05 in the hand eye calibration already implies an error of 0 6 mm in object position if the latter is 700 mm away from the camera typical viewing distance for commodity 3 D Manuscript received January 6 2018 revised April 4 2018 and May 28 2018 accepted June 12 2018 Date of publication August 20 2018 date of current version December 4 2018 This paper was recommended for publica tion by Associate Editor F Boyer and Editor T Murphey upon evaluation of the reviewers comments This work was supported in part by NTUitive Gap Fund NGF 2016 01 028 and in part by the SMART Innovation under Grant NG000074 ENG Corresponding author Huy Nguyen The authors are with the School of Mechanical and Aerospace Engi neering Nanyang Technological University Singapore 639798 e mail huy nguyendinh09 cuong pham normalesup org Color versions of one or more of the fi gures in this paper are available online at http ieeexplore ieee org Digital Object Identifi er 10 1109 TRO 2018 2861905 Fig 1 Hand eye calibration problem consists in identifying the rigid body transformationeTcbetween a camera mounted on the end effector of a robot and the end effector itself cameras In turn having a precise knowledge of the uncertainty of the object pose estimation is critical as follows 1 Inhigh precisionmanufacturing itisimportantnotonlytoknow the pose of an object but also to guarantee that the pose estima tion error is within some tolerance For instance when drilling holes in the fuselage of an aircraft the hole position tolerance is 0 5 mm which would be violated by an error of 0 05 in the hand eye calibration even when assuming that the object pose estimation in the camera frame is perfect aforementioned 2 The precise knowledge of the object pose covariance matrix allows one to intelligently refi ne the object pose estimation by other perception modes For instance in visuo tactile sensor fusion 7 knowing that the covariance of the object pose is comparatively large in the translation along say the X axis will prompt us to touch the object along that axis in order to best reduce the uncertainty In addition knowing the covariance of X allows improving the calibration process itself by e g choosing the appropriate number of measurements to achieve a desired level of precision or choosing the appropriate matrices A and B that minimize the covariance of X B Related Works Finding the covariance of X is challenging for several reasons First as X A and B represent rigid body transformations they live in SE 3 a subset of the space of 4 4 matrices endowed with a non trivial Lie group structure 8 Second how to represent and calculate uncertaintiesinSE 3 isbyitselfacomplexissue whichhasprompted advanced mathematical developments 9 Finally merely solving for X in AX XB is already a diffi cult problem 3 5 let alone evaluating the uncertainty of the solution There are a number of works dealing with the uncertainty of hand eye calibration In 10 based on a sensitivity analysis of closed form 1552 3098 2018 IEEE Personal use is permitted but republication redistribution requires IEEE permission See http www ieee org publications standards publications rights index html for more information IEEE Transactions on Robotics T RO paper presented at IROS 2019 It should be cited as a T RO paper 1652IEEE TRANSACTIONS ON ROBOTICS VOL 34 NO 6 DECEMBER 2018 solutions some critical factors and criteria infl uencing the accuracy of theresultareanalyzed Forinstance onemaytrytomaximizetheangle between rotation axes of relative movement to reduce the infl uence on error in rotation or to minimize the distance between the optical center of the camera and the calibration pattern to reduce the infl uence on error in translation Based on this analysis Shi et al 11 present an algorithm to select movement pairs automatically from a series of measurements to reduce the error of the estimate Schmidt et al also introducesimilarapproachbasedonavectorquantizationmethod 12 In 13 Aron et al present an error estimation method of the rotation part of X based on an Euler angles parameterization The authors do not discuss how that error propagates to the translation part of X and their vision tracking measurements are also assumed to be noise free More fundamentally the Euler angles formulation as opposed to the SE 3 formulation is well known to involve singularities In 6 14 15 authors introduced the concept of using mean and covariance of distributions on SE 3 to formulate the hand eye cali brationproblem Aninformation theoreticapproachisproposedin 14 by viewing the problem in terms of distributions on the group SE 3 and minimizing the Kullback Leibler divergence of these distributions with respect to the unknown X This allows them to handle the prob lemwithoutaprioriknowledge ofthecorrespondence ofthegivendata sets In 15 the algorithm is further improved by introducing two new probabilistic approaches built on top of the Batch method Our work exploits the benefi ts of applying optimization techniques directly on SE 3 andfocusesonthederivationofthecovarianceofthehand eye transformation which is different from the mentioned works as they only focus on fi nding a solution to X The idea of estimating explicitly uncertainties in the system is also by no means new While many have studied the problem of uncertainty in the camera model intrinsic and extrinsic parameters 16 and the propagation of uncertainties through the camera model 9 we place emphasis on the hand eye transformation and its uncertainty in this work C Contribution and Organization of the Paper Itcanbenotedthatnoneoftheaforementionedworkshasprovideda derivation of the covariance of X which is ultimately the most generic and relevant quantifi cation of the uncertainty of the hand eye calibra tion process The goal of this paper is to rigorously work out such a derivation Specifi cally we transpose methods for forward and back ward propagation of covariance 16 into the framework of uncertainty in SE 3 1The structure of the hand eye calibration equation raises specifi c technical diffi culties which we shall address in detail Theremainderofthispaperisorganizedasfollows InSectionII we state the hand eye calibration problem and introduce the mathematical background of the work which includes the representation of uncer tainty in SE 3 and methods for forward and backward propagation of covariance In Section III we present our method to estimate the ro tation and translation parts of the hand eye transformation matrix and their associated covariance matrices In Section IV we show that the method can indeed predict with excellent precision these covariances in synthetic and real calibration data sets and uses this information to compute the covariance of the object pose estimation in a real setting Finally in Section V we conclude by discussing the advantages and drawbacks of our approach and sketch some future research directions 1Many works have provided rigoros treatments for representing and asso ciating uncertainty in SE 3 and SO 3 Common notations terminologies and theoretical tools can be found in 9 17 20 In this work to model the uncertainty on SE 3 we choose to follow the approach proposed in 9 as we fi nd it accessible and easy to implement II BACKGROUND A Formulation of the Hand Eye Calibration Problem The classical hand eye calibration method consists in looking at a fi xed pattern from two different viewpoints say 1 and 2 giving rise to the following equation bT e 1eT ccTo 1 bT o bT e 2eT ccTo 2 1 where we have the following r bT e i is the transformation of the end effector with respect to the fi xed robot base at confi guration i r eT c is the constant transformation of the camera with respect to the end effector r cT o i is the transformation of the pattern object with respect to the camera at confi guration i r bT o is the constant transformation of the pattern with respect to the robot base see Fig 1 Next one can transform the above mentioned equation into bT e 2 1 bT e 1eT c eT ccTo 2cT o 1 1 2 which has the form of AX XB where X eT c is the un known hand eye transformation and A bT e 2 1 bT e 1 and B cT o 2cT o 1 1 can be computed from respectively the robot kinematics and pattern pose estimation 3 Next if the fi xed pattern is viewed from a large number of viewpoints one can collect many differ ent A s and B s Suppose that we have a set of k measurements A1 B1 A2 B2 Ak Bk Since in practice these mea surements are perturbed by actuator sensor noise the exact solution for the set of k equations AiX XBiwill not exist Instead the problem is commonly framed as an optimization problem in which X is found as the transformation that best fi ts the k equalities In some cases the hand eye calibration problem is also formulated asthe A X ZB problem whereA bT e j B cT o j 1 and Z bT o ThesolutionofX inthisformulationhasbeenproposedby manyworks e g 5 21 However weonlyfocusontheAX XB formulation in this paper In other cases the camera is not mounted on the end effector but on a fi xed stand In these cases fi nding the relative transformation between the camera and the robot base can also be formulated as the AX XB problem and can be treated by the same method B Representation of Rigid body Transformations and of Their Uncertainties We fi rst recall some facts and formulae involving the rotation group SO 3 and the special Euclidean group SE 3 8 Elements of SO 3 are given by the 3 3 real matrices R satisfying R R I and detR 1 The group SE 3 is defi ned to be SE 3 R t 0 1 R SO 3 t R 3 3 SO 3 is a matrix Lie group and its associated Lie algebra denoted so 3 is given by the set of 3 3 real skew symmetric matrices The operator turns R3into a 3 3 member of the Lie algebra so 3 0 3 2 30 1 2 10 4 We will use as the inverse operation of By using the matrix expo nential exp on elements of so 3 we can obtain group elements of SO 3 asR exp Theinverseoftheexponential orlogarithm IEEE Transactions on Robotics T RO paper presented at IROS 2019 It should be cited as a T RO paper IEEE TRANSACTIONS ON ROBOTICS VOL 34 NO 6 DECEMBER 20181653 isusedtoobtainthevectorofexponentialcoordinates fromthegroup element R SO 3 as logR To model the uncertainty on SE 3 we follow the approach pro posed in 9 Since there is in general no bi invariant distance on SE 3 22 fi nding the rotation and translation components of X simultaneously would require a non trivial rotation translation weight ing in any cases Instead we choose to solve them separately which entails a number of simplifi cations 3 As a consequence the un certainties of the rotation and the translation parts are also modeled separately Specifi cally we assume that the rotation parts of the observations Aiand Biare corrupted as follows RAi exp RAi RAi RBi exp RBi RBi 5 where RAi RBi SO 3 are the means of RAi RBi and RAi RBi R3are zero mean Gaussian perturbations with covari ance matrices RAi RBi respectively The translation parts of the Aiand Biare corrupted as follows tAi tAi tAi tBi tBi tBi 6 where tAi tBi R3are the means of tAi tBi and tAi tBi R3are zero mean Gaussian perturbations with covariance matrices tAi tBi respectively Note that the above mentioned assumptions imply that rotation and translation noises are independent which on some occasions e g manipulator with large defl ections on the robots links might require further considerations Because of this assumptions the space we con sider is strictly speaking SO 3 R3 rather than SE 3 C Forward and Backward Propagation of Covariance Forward propagation Let P be a random vector in RMwith mean P and covariance matrix Consider a function f RM RNthat is differentiable in a neighborhood of P Then at the fi rst order of approximation f P is a random variable with mean f P and co variance matrix f J J 7 where J is the Jacobian matrix of f at P Backward propagation Assume now that P the parameter is un known but that V f P the measurement is known and deter mined to be a random variable with mean V and covariance matrix V Then the best estimate for P is given by P min P V f P V To estimate the covariance of P one can approximate f by an affi ne function f P f P J P P which yields V f P V V V J P P V 8 Using the weighted pseudo inverse one has P P J V 1J 1J V 1 V V 9 From 7 the covariance of P can now be approximated at the fi rst order by J V 1J 1J V 1 V J V 1J 1J V 1 J V 1J 1 10 In practice when performing an iterative least squares optimization one can use 10 at the last iteration to obtain the estimation of the covariance of P Note that the quality of the approximations given by 7 and 10 depends in particular on the quality of the linear approximation of f III DERIVATION OF THECOVARIANCE OFX Equation AiX XBican be decomposed as RAiR RRBi 11 RAit tAi RtBi t 12 where R t denote respectively the rotation and translation parts of X A Covariance of the Rotation Part of X We fi rst consider the rotation part R of X Let i i so 3 denote the logarithms of RAiand RBi respectively i e i logRAi i logRBi 13 Note that the covariance matrices of iand ican be obtained by applying the forward propagation of covariance i J i 1 RAiJ i 1 14 i J i 1 RBiJ i 1 15 where J i denotes the left Jacobian of SO 3 at i see 9 for more details Next via logarithm mapping 11 can be written as logRAi logRRBiR R i R 16 ApplyingtheruleR R R forR SO 3 and so 3 one has i R i 17 In order to use the uncertainty model in SO 3 we defi ne a random variable Rthat represents the difference between R and the current estimate R by R exp R R Next to apply the backward propagation of covariance one needs the measurement vectors iand ito appear on the same side of the equation To achieve this without making it too complex we use a trick from 16 which consists in copying the i s on both sides as follows 1 1 k k V 1 exp R R 1 k exp R R k f P 18 Now the measurement vector is given by V V1 Vk where Vi i i and the parameter vector is given by P R 1 k Since the noises of is and is are independent is caused by robot kinematics while iis caused by object pose estimation in the camera frame the covariance matrix of the measurement vector is given by V diag V 1 V2 Vk 19 with V i diag i i IEEE Transactions on Robotics T RO paper presented at IROS 2019 It should be cited as a T RO paper 1654IEEE TRANSACTIONS ON ROBOTICS VOL 34 NO 6 DECEMBER 2018 Now the covariance weighted minimization is given by min P V f P min P k i Vi f P i 1 Vi Vi f P i This minimization problem can be solved by iteratively updating the estimate of the parameter vector by the rules R j 1 exp R R j 20 i j 1 i j i 21 where at each step j the update vector R 1 k is found by solving the normal equation J f 1 V Jf J f 1 V V f P 22 The Jacobian of f has the form Jf J R J J R 1 J 1 J R 2 J 2 J R k J k 23 where J R i 0 R i J i I R 24 The set of 22 may now written in block form as J R 1 V J R J R 1 V J J 1 V J R J 1 V J R J R 1 V V f P J 1 V V f P 25 To simplify the left hand side of 25 let U J R 1 V J R k i J R i 1 ViJ R i 26 W J R 1 V J W1 Wk 27 Z J 1 V J diag Z1 Zk 28 where Wi J R i 1 ViJ i and Zi J i 1 ViJ i 29 As for the right hand side of 25 let R J R 1 V V f P k i J R i 1 Vi Vi f P i 30 J 1 V V f P 1 k 31 where i J i 1 Vi Vi f P i 32 To solve 25 one can left multiply both sides by I WZ 1 0I which yields U WZ 1W R R WZ 1 33 Z W R 34 The above mentioned equations can now be solved to fi nd the updating vectors Rand Applying backward propagation of covariance a fi rst order approx imation of the covariance of P is given the following matrix taken at the last iteration J f V 1J f 1 R RWZ 1 RWZ 1 WZ 1 RWZ 1 Z 1 Truncation of the covariance matrix gives covariance matrices for parameters Rand iseparately The covariance of Ris given by the top left block of i e R U WZ 1W 1 U k i WiZi 1Wi 1 35 The covariance of i is given by i WiZ 1 i RWiZ 1 i Z 1 i 36 The cross covariances of Rand i are R i