Pose-estimation and control in a 3D visual servoing system

POSE-ESTIMATION AND CONTROL IN A 3D VISUAL SERVOlN ... 14th World Congress ofIFAC Copyright © 1999 IF AC B-ld-09-6 14th Triennial World Congress, Beijing, P.R. China POSE-ESTIMATION AND CONTROL IN A 3D VISUAL SERVOING SYSTEM M. Vargas, F.R. Rubio and A.R. Malpesa 1 Dpto. Ingenieria de Sistemas y Automatica Escuela Superior de Ingenieros, Universidad de Sevilla Camino de Los Descubrimientos sin, 41092-Sevilla (Spain) Ph. :+34 954487350, Fax: + 34954487340, E-mail:[email protected] Abstract: The present work studies the case of a 3D visual servoing system composed by a robot manipulator, a CCD camera rigidly mounted all the end-effector of the robot. and other computer-based hardware. The aim of the system iH to keep a desired relative position and orientation between the object and t.he camera, compensating for the autonomous object motion. Two existing algorithms for object's pose estimation have been implemented. Several controllers are designed and its performance evaluated along the paper. Copyright© 1999 IFAC Keywords: Robotic manipulators, Matrix equations, Image processing, Control system design, Digital Signal Processor 1. INTRODUCTION The interaction between vision sensors and robotic systems has been subject of great effort in the recent research. It has special interest in applications ,,,here a robot has to interact with moving parts or objects, whose position and orientation is unpredictable or not known with precision. The term visual servoing is applied to visually guided systems that make use of one or ' several, static or moving cameras to provide visual information a...'l a feedback signal to control the manipulator in accomplishing its task. Some examples of applicat.ions for visual servoing are: fruit picking, teleoperation, missile tracking, pur s uer autonomous vehicles, etc. The present work studies the case of a singlecamera, visual servoing system using the hand-eye configuration, composed by a robot manipulator, a CCD camera rigidly mounted on the end-effector of th e robot, and other hardware co mponents that 1 Partially sllpported by CICYT. under gra.nt TAP 98- O<~41 Copyright 1999 IF AC will be detailed later in the paper. The aim of the system is to keep a desired relative position and orientation between the object and the camera, compensating for the autonomous object motion. The implementation and performance analysis of this system in the planar case can be found in a previuus work (Vargas et al., 1998). In the present paper, the extension to the 6-DOF case is presented. Corke and other authors (Corke, 1996), (Hashimoto, 1993), distinguish several approaches when handling a visual servoing implementation. The one used here is the Position-Based Approach, in which visual features are used to estimate the relative object-cam e ra position, t.hat. is actually used to control the system. Since one of the critical issues in the positionbased approach is the estimation of the object position and orientation relative to the camera, two different existing methods have been essayed in this work: the first one is a variation of the Tsai's method (Tsai, 1987), by S0rensen (Sorensen, 1994), and the other one is an in- ISBN: 0 08 043248 4 802 POSE-ESTIMATION AND CONTROL IN A 3D VISUAL SERVOlN ... ere mental method due to Heuring and Murray (Heuring and Munay, 1996b), (Heuring and Murray, 1996a). The structure of the paper is the following: First of all, in section 2, the deduced model of the complete 6-DOF visual servoing system is detailed. In section 3, the two different, existing methods, used for the relative object-camera pose estimation, are mentioned. After that, in section 4, the design of several controllers is handled, starting from a simplified model of the system. This section includes real experiments to compare the response of each controller and the performance using both pose estimation methods. Finally, the paper describes the specific components of our system in section 5, and concludes with a summary in section 6. 2. VISUAL SERVOING MODEL One of the main problems in the case of the position-based approach, is the precise estimation of the relative object-camera pose (position and orientation, entailing up to 6 coordinates, which can be handled in the form of homogeneous matrices). The model used in the present work was built up starting from the one-dimensional case as described in detail in (Vargas et al., 1998). Briefly, the system can be viewed at two levels: • Low-level Model: That models the robot arm behaviour. The robot manipulator used is the PUMA 560, commanded via serial port, in cartesian coordinates, making use of the ALTER facility (Unimation , 1986). The model derived in (Vargas et al., 1998), for each cartesian degree of freedom, is shown in figure 1. 'Where, the sampling time is TI = 28ms and: 1 Garm(s) = ( )( ) (1) 1 + rIs 1 + r2S \Vith: 1'1'"" 0.003 and 1'2 = 0.005 Fig. 1. Detailed robot model • High-level Model: Several coordinate syst.ems and geometric transformations are involved in this model. In figure 2, the position of the robot arm, camera and object are shown in two consecutive sampling intervals. In this figure, {W} is the world coordinate frame, usually at the base of the robot . {O} is the coordinate frame bound to the moving object (time-varying {Od, k: sampling period). {Cd is the camera coordinate system, with axes X and Y parallel to the horizontal and vertical image plane Copyright 1999 IF AC 14th World Congress ofIFAC ------ ... . ---.--"yw {W Xw Fig. 2. Nomenclature of the coordinate systems and matrix transformations axes, and the Z axis, passing through the center of the camera lens. {Gk} is the gripper (or robot end-effector) coordinate system. The high-level model describes the behavior of the fu1l6 - DOF version of our servoing system, and can be seen in figure 3. In this block diagram, the object position is an external , unpredictabl e input: WHOk+1 2. This is compared with the current camera pose, WH Ck +1, giving the relative object-camera pose: (2) The image capturing mechanism is modeled as the World-To-Screen module in the figure, and converts the set of feature points present on the real object to their corresponding positions in the frame-grabber memory. Then, after some image processing and algebraic computations, included in the block Pose-Estimation (which the following section is devoted to), the estimated objectcamera pose is obtained: Ck+lHo k +1' This estimated pose is compared with the desired relative pose, giving the necessary camera displacement and rotation: Then, the camera-gripper transformation ekc, which has been estimated off-line, using some calibration method (Tsai and Lenz, 1989)) is used to get the necessary motion of the robot endeffector: 2 VVhere the usual notation AHB means for the homogeneous matrix that transforms coordinate system {A} into coordinate system {B}. ISBN: 0 08 043248 4 803 POSE-ESTIMATION AND CONTROL IN A 3D VISUAL SERVOlN ... Fig. 3. Complete 6-DOF model. This resulting matrix is the input to the controller module (to be described later). The controller output is given as an input to the low-level subsystem, the output of which is the current gripper pose. Finally, the motion of the robot implies motion of the camera, according to the true camera-gripper transformation. The sampling interval of this higher-level model is T2 = 8 . Tl, and, hence, the whole system has a multi-rate structure. It is worth pointing out that modules (1) and (2) in figure 3 are ideally complementary. In the World-To-Screen transformation the real internal parameters of the camera are involved, but in the Pose-Estimation module, the estimated values of these parameters are used instead. This estimation was made off-line, using a camera-calibration algorithm (Tsai, 1987), (Sorensen, 1994). Other factors that can disturb the pose estimation are the precision in the estimation of the feature positions in the image plane, the accuracy in the geometric model of the object, the relative speed between the object and the camera, etc. 3. FEATURE EXTRACTIO::-r AND POSE ESTIMATION It is necessary to select some distinguishing visual features of the object to be tracked. In our case, the object used in the experiments was a planar template made of 25 circular, black spots, arranged in matrix form, on a white background. The centers of the spots are the features to be extracted. The position of these centers must be accurately knOVIlU respect to the local coordinate system placed on the object, {D}, and will be denoted by: p::", with i = I..n, where n is the number of feature points. The corresponding projections onto the image plane, Pj, must be obtained through image analysis. To avoid the blurring effect in the grabbed image, due to eventual high speeds in the relative cameraobject motion, only one of the two fields in eaeh interlaced image is used. Two different methods have been implemented in order to get the relative orientation and position between the camera and the object of interest. Copyright 1999 IF AC 14th World Congress oflFAC The first one is a modification of Tsai's calibrat.ion method (Tsai, 1987) due to S0resen (Sorensen, 1994). The application of the method gives an estimation of the relative object-camera rotation and translation, matrix R and vector T, respectively. The second method essayed was originally due to Heuring and Murray (Heuring and Murray, 1996b) and (Heuring and Murray, 1996a). The main disadvantage respect to Tsai's method is that the rotation matrix is obtained in an incremental way (oR instead R). On the other hand it gives rise to a simpler formulation and more equations are obtained for the same number of points. Heuring-Murray's method, by construction, introduces an error in the estimation of the rotation parameters. This error is larger as larger are the rotation angles from the reference orientation. After a first estimation of matrix r5R, we have angles close to the real ones, if we modify the reference matrix with this first solution, and apply again the algorithm, a better approximation will be obtained. According to this, an iterative scheme has been chosen to estimate the current object-camera orientation. This iterative process should be finished when the incremental rotation matrix reaches the identity matrix. In our implementation, due to the real-time requirements of the system, the nUlnber of iterations has been limited to 4, ,,,hich has been sho 'wn to give enough precision in normal conditions. 4. CONTROLLER DESIGN AND EXPERIMENTS 4.1 Coordinate Decomposition for Control The more usual way to control robot manipulators is at low level, in joint coordinates. In our implementation, there is not direct access to the servomotors. Then, it is necessary to program the control laws at a higher level, in cartesian coordinates, and at a relatively large sampling period: Tl = 0.0288. For implementation of the Controller module in figure 3, the transformation matrix must be decomposed in six parameters, that fully determine it: three for translation aud three for rotation. ISBN: 0 08 043248 4 804 POSE-ESTIMATION AND CONTROL IN A 3D VISUAL SERVOTN ... If we denote by x each one of the six coordinates, and Xli the output of the controller for the corresponding coordinate, x, the following algebraic expresion implements a general discrete controller: x~ =al X~_l + ... + an x;_n + bo Xk + ... + bm. Xk-17l. (5) For the aditivity property being more consistent with the variables llsed, the coordinates chosen should be refered to a stationary reference system. Then, before decomposing matrix D.HCk' it should be transformed into ~H~~?), which represents the same transformation but refered to the set of axes of the world coordinate system {W}, the rotation and translation of which can be obtained as: Obviously, in the case of the translation parameters, the three components of the translation vector are chosen. The inmediate selection for the rotation parameters could be Euler's angles representation of the D.Rg!') matrix. However, another parameterization of the rotation matrix has been chosen in order to avoid the strong cross-coupling showed by Euler's angles, and to fullfil the aditivity property required in the control variables. The parameterization for the l"otation matI'ix finally used is the axis-angle specification, (k, B) f- R, where k is the unitary vector on the rotation axis, and (J is the angle rotated about this axis, in radians. The additivity property of this rotation parameters derives from Varignon's theorem, known from kinematics theory, which has to do with the combination of simultaneous angular velocities in continuous time: • Varignon's Theorem The following relation: I)w, 1\ O~P) = w 1\ 6p (where: w = I:i Wt, Ot: point on the corresponding rotation axis, P: feature point), holds when all the rotation axes intersect in one point O. In our discrete-time case, if a rotation matrix, 6.R Ok , must be applied in cycle k, the corresponding average angular velocity during this cycle is: Copyright 1999 IF AC B - Wk = T2 k, 14th World Congress ofTFAC Where: (k,O) f- D.Rok, and T2 is the sampling period. If the components of w" are used as control variables (x in equation (5) can be replaced by w). Hence, angular velocities are used whose rotation axes converge in the origin of the current {Ok} coordinate system, and are acting simultaneously to obtain the wJ: as a linear combination of them. The rotation matrix corresponding to the output of the controller is: As a conclusion, the control variables can be considered as independent variables and will be put together in a row vector, in the form: The contwl law (the general form of which is in equation (5), replacing x by Q) gives Q~ as an output. To come back again to homogeneous matrix formulation: Qr(1 : 3) --t .6.Tuh:1 Q'U.(4: 6) -7 ~Ru{W) k Ok Finally, the converse transformation to (6) is made: 4.2 Controller Design W' ROk As the response of each one of the 6 chosen control variables is close to the response of the onedimensional model of Ollr visual tracking system, this pararneterization of the transformation matrices will allow to develope the control design stage based on the I-DOF model, and, afterwards, on the 6-DOF system, make some minor modifications in the controller parameters for the final tuning of the system response. Then, the design of the controllers has been carried out looking for the best performance of the one-dimensional model. This one-dimensional model was simplified as detailed in (Vargas et al., 1998), in order to make easier the control design stage. The approximated one-dimensional, single-rate block diagram is shown in figure 4. In this simplification it has been assumed that the pair of modules (1) and (2) in figure 3 are exactly complementary, and the same holds for modules (3) and (4). ISBN: 008 0432484 805 POSE-ESTIMATION AND CONTROL IN A 3D VISUAL SERVOlN ... Fig. 4. Approximate model used for controller design. G~obot(Z) in the figure is: 7z + 1 G~obot(Z) = ( ) zz-l In general, the performance of the controller designed using the I-DOF model, degrades when applied to the 6-DOF system. The reason is not strictly the unsuitability of the controller extrapolation, but the poorer object's pose estimation achievable in 6-DOF. Another factor that contributes to deteriorate the response in the 6-DOF case is the existing uncertainty in the cameragripper transformation matrix, CfJ o, that can slightly deflect the camera motion from the exact desired transformation. 4.3 Control Experiments In the previously mentioned paper, (Vargas et al., 1998), only one or two dimensional experiments were included. In the present work, step responses when all the 6 DOF are controlled will be shown, using both pose-estimation algorithms refered in section 3. Proportional Controller The first controller essayed was the proportional controller, which the other ones will be compared with. In figure 5 appears the experimental response of the 6-DOF system, under step input in translation (upper plots) and rotation (lower plots) coordinates, and for both pose-estimation methods. Pole-Placement Controller The second controller tested in our visual servoing system was a discrete controller with the general st.ructure: qo + qI Z-l + q2 z-2 1 + PI Z-l + pz Z-2 (7) designed using the pole-placement method. The specified poles for the closed-loop syst.em were placed in: Po == O.15~ (Pl,P2) == 0.36 ± O.15i, (PS,P4) = 0.17 ± O.175i. Smith's Predictor Controller Other controller included for comparison in this work is made of a proportional controller with a Smith's predictor. Copyright 1999 IF AC 14th World Congress oflFAC The transfer function of this controller is of the form: k(l - Z-l) 8 (1 + ak) + (bk -1) Z-l - ak Z-2 -bk Z-3 ( ) with: a = 7, b = 1 and k = 0.08. The most significant improvement provided by the use of this controller, can be apreciated in figure 6, where its step response is compared with the two other controllers, in the case of Tsai's poseestimation method being used and steps inputs only in t.he three translation components. Several other experimental tests were made , comparing the behavior of the Smith's predictor controller (also for the pole-placement controller) and the proportional one, but no meaningful differencies were noticed. 5. IMPLEME::-JTATION The visual servoing system implementation described in this paper, is made of the following main components: • UNlMATlON robot system: Composed by the V AL II controller module and the PUMA 560 robotic arm. • Pulnix monocrome, CCD camera. • Personal Computer. Pentium 160MHz. Plugged into the PC, there is a multiple Digital Signal Processors Motherboard, with three DSP modules, one of w'hich is an image adquisition module. The parallelizing capabilities of this system have not been exploited yet. The physical link between the personal computer and the robot controller is the serial line using the protocol RS-232C. 6. CONCLUSIONS This paper introduces a model for a visual servoing system, using the position-based approach. The system is based on the PUMA 560 robot manipulator, operated using the ALTER facility. \Vhen operating in ALTER mode, the wbot is commanded in cartesian positions; thus, it is not necessary to solve the inverse kinematics problem, and no direct control on the servomotors is available. The obtained model (which happens to be a multirate one) has been validated in some experimental tests, comparing its response to the real response of the system. Then, several controllers have been designed, starting from an approximat.ed single-rate simplified model. The step response of the cont.rollers with best performance is showed. ISBN: 0 08 043248 4 806 POSE·ESTIMATION AND CONTROL IN A 3D VISUAL SERVOIN . .. 14th World Congress oflFAC --------- --------- - - Translac. ox •••. Translac. QY •• Translac. oz Tsa i. Proportional. Kp=D.04 t .' ., t (s) -- ROlac.QZ - - - Rolac. QY •• Rotac. ox ;;----;~-~ -~- 7--;,.';-S ---;- -~3~_L - 4"--c O-----! 1(5) -- Translac. OX •••. Traoslac.OY -Translac. OZ Heurlng. Proportional. Kp=O.04 -' .. ~~~-~-~-~-~,~,-~-~~~-~~~ I (s) He.ring. Proportional. Kp=O.D4 -- RDtac.OZ - - - Rotac. OY •• Rotac. ox >< t ($) Fig . 5. Response to step input under proportional control '§" ,!!., " ..82.5 3 a. 2 . , Smith Predictor Controller Pole Placement Controller •• Proportional Controller X Translation Component H; I(s) . . . , . , . , . . . , " . . : -- Smith Predictor Contr'oller - - - , Pole Placement Controller .. PropMlonal Controller . , . , . , . , . , . , . , . " Y Translation Component 0.' ... 1(.) -- Smith Predictor Controller - - - - Pole PlacemE:lnt Controher •• Proportional Controlrer • , 2 Translatiofl Component ,. t(s) Fig. 6. Comparison of the response in each t.ranslation compon e nt Copyright 1999 IF AC ISBN: 0 08 043248 4 807 POSE-ESTIMATION AND CONTROL IN A 3D VISUAL SERVOTN ... Current and future work is focused on the generalization of the kind of features to be extracted for the object pose estimation. Besides this, a similar visual servoing system is being implemented on a mobile robot. 7. REFERENCES Corke, P.!' (1996). Visual Control of Robots. Research Studies. Press Ltd. Hashimoto, K. (1993). Visual Servoing: Real- Time Control of Robot Manipulators Based on Visual Sensory Feedback. \Vorld Scientific. Heuring, J.J. and D.W. Murray (1996a). Slaving head and eye movements for visual telepresence. British Mach-ine Vision Confer-ence, BMVC'96. Heuring, J.J. and D.W. Murray (1996b). Visual head tracking and slaving for visual telepresence. Proc. IEEE Int. Conf. on Robotics and Automation. Sorensen, A.T. (1994). Analysis of Real-Time Vision in Control and Automation of Systems. Vol. Ph.D. Thesis. Dept. of Automation, IAt:', Denmark Technical t:'niversity. Tsai, R.Y. (1987). A versatile camera calibration technique for high-accuracy 3d machine vision metrology using ofl'-the-shelf tv cameras and lenses. IEEE Tran. on Robotics and A utomation. Tsai, R.Y. and R.K. Lenz (1989). A new technique for fully autonomous and efficient 3d robotics hand/eye calibration. IEEE Tmn. on Robotics and Automation. Unimation (1986). Unimate Industrial Robot. User's Guide to VAL II. 'Cnimation. Vargas, M., A.R. Malpesa and F.R. Rubio (1998). Modelling and control of a visual servoing system. Proceedings of the Third European Robotics, Intelligent Systems and Control Conference, EURISCON'98. Copyright 1999 IF AC 14th World Congress ofTFAC ISBN: 008 0432484 808