Transcription

1 IEEE Coyright Notice Personal use of this material is ermitted. However, ermission to rerint/reublish this material for advertising or romotional uroses or for creating new collective works for resale or redistribution to servers or lists, or to reuse any coyrighted comonent of this work in other works must be obtained from the IEEE. Contact: Manager, Coyrights and Permissions / IEEE Service Center / 445 Hoes Lane / P.O. Box 11 / Piscataway, NJ , USA. Telehone: + Intl

2

3 Worksho on New Trends in Image-based Robot Servoing, IROS'97,. 45-5, Grenoble, France, Setember D 1/ visual servoing with resect to a lanar object Francois Chaumette Ezio Malis Sylvie Boudet IRISA / INRIA Rennes EDF-DER Camus de Beaulieu 6 Quai Watier 5 4 Rennes, France Chatou, France fchaumett, emalisg@irisa.fr, sylvie.boudet@edfgdf.fr Abstract In this aer, we roose a new vision-based robot control aroach which is halfway between the classical osition-based and image-based visual servoings. It allows to avoid their resective disadvantages. For a lanar object, the homograhy between the feature oints extracted from two images (corresonding to the current and desired camera oses) is comuted at each iteration of the control law. From this homograhy, an aroximate artial-ose, where the translational term is known only u to a scale factor, is deduced. Using arameters of this artial ose and image features, it is ossible to design a closed-loo control law controlling the six camera d.o.f. Contrarily to the osition-based visual servoing, our scheme does not need any geometric D model of the object. Furthermore and contrarily to the image-based visual servoing, our aroach ensures the convergence of the control law in all the task sace. 1 Introduction Vision-based robot control using an eye-in-hand system is generally erformed using two dierent aroaches [11, 8, 9]: osition-based and image-based control systems. In a osition-based control system, the control error function is comuted in the D Cartesian sace [1] (for this reason this aroach can be called D visual servoing). The ose of the target with resect to the camera, which describes its D osition and D orientation, is estimated from image features corresonding to the ersective rojection of the target in the image. Numerous methods exist to recover the ose of an object (see [4] for examle). They are all based on the knowledge of a erfect geometric model of the object and necessitate a calibrated camera to obtain unbiased results. The main advantage of D visual servoing is that it controls the camera trajectory in the Cartesian sace, which allows to easily combine the visual ositioning task with obstacles avoidance and/or joint limits and singularities avoidance. On the other hand, in an image-based control system, the control error function is comuted in the D image sace (for this reason this aroach can be called D visual servoing). This local aroach is based on the use of an image Jacobian (also called interaction matrix [5]) and the control laws rovide at each iteration the camera velocity for minimizing the observed error in the image. This aroach is known to be very robust with resect to camera and robot calibration errors [6]. However, the convergence is ensured only in a region (quite imossible to determine) around the desired osition. Furthermore, the Cartesian trajectory of the camera is uncontrolled, and some image features may get out of the camera eld of view during the servoing, which generally gives rise to its failure. The urose of this aer is to design a new visual servoing system which combines the advantages of D and D visual servoings and avoids their drawbacks. We oint out our attention to one of the tyical alications of visual servoing: ositioning a camera mounted on a robot end-eector relative to a target, for a grasing task for instance. The ositioning task is divided into two stes. In the rst o-line learning ste, the camera is moved to its desired osition. The image of the target corresonding to this osition is acquired and the extracted desired features are stored. In the second on-line ste, after the camera and/or the target have been moved, the camera is commanded so that the current features reach their desired osition in the image. In such alication, the target geometry is not always recisely known. Furthermore, the convergence has to be obtained in all the task sace. Our aroach can be called D 1/ visual servoing since the control error function is comuted in art in the D Cartesian sace and in art in the D image sace. More recisely, the homograhy between the feature oints extracted from two images (corresonding to the current and desired camera oses) is comuted at each iteration. From the homograhy, the rotation of the camera between the two views is estimated. Consequently, the rotational control loo 45

4 can be decouled from the translational one. A such decouled system allows to obtain the convergence in all the task sace. It must be emhasized that the use of an homograhy does not need neither a D model of the target (the method we actually use to estimate the homograhy is however only valid for lanar objects) nor a recisely calibrated camera. As far as the camera translational dislacement is concerned, it can only be estimated u to a scale factor. It is therefore controlled using D image features and the ratio, obtained from the homograhy, of the unknown desired and current distances between the camera and the target. Finally, in site of the artial-ose estimation, exeriments show that the aroach is robust to calibration errors. The aer is organized as follows. In Section and Section we briey recall D and D visual servoings resectively. In Section 4 we resent the D 1/ visual servoing aroach. The exerimental results are given in Section 5. D Visual Servoing Let F be the coordinate frame attached to the target, F 1 and F be the coordinate frames attached to the camera in its desired and current osition resectively (see Figure 1). 1 T ose estimation always aear in the camera eld of view. This can not be guaranteed if the camera or the robot are only coarse-calibrated. Let us also note that if the camera is not accurately calibrated, or if the D model of the considered object is not erfectly known, the estimated current and desired camera oses will be biased. However, this is not a drawback if a closedloo scheme is erformed. The corresonding block diagram of the D visual servoing aroach is given in Figure. T + - T Cartesian Control law Pose Estimation ; V Model of the target Features Extraction Robot Camera Figure : Block diagram of the D visual servoing D Visual Servoing The control error function is now exressed directly in the D image sace (see Figure ). target oint target oint F F1 F1 s? s T F F Figure 1: Modelisation of camera dislacement for D visual servoing Knowing the coordinates, exressed in F, of at least four oints of the target [4], it is ossible from their measure in the image to comute the desired camera ose (reresented in Figure 1 by the transformation matrix 1 T ) and the current camera ose (reresented in Figure 1 by the transformation matrix T ). The camera dislacement T 1 to reach the desired osition is thus easily obtained ( T 1 =? T 1?1 T ), and can be erformed either in oen loo or, more robustly, in closed loo by comuting T at each iteration of the control law. Finally, the control law has to be designed in order that the image features used in the Figure : Modelisation of camera dislacement for D visual servoing Let s be the current value of visual features observed by the camera and s be the desired value of s to be reached in the image. The time variation of s is related to camera velocity by [5]: _s = L(s; Z)T (1) where T = V is the camera velocity screw and L(s; Z) is the interaction matrix related to s. For examle, if the chosen features are the coordinates s = u v T = X=Z Y =Z T in the image of a D oint P of coordinates X Y Z T in the 46

5 camera frame, the interaction matrix related to u and v is given by: where: L! (s) = L(s; Z) = 1 Z L v(s) L! (s) () L v (s) =?1 u?1 v uv?(1 + u ) v (1 + v )?uv?u () (4) The interaction matrix for more comlex image features can be found in [5]. The vision-based task e (to be regulated to ), corresonding to the regulation of s to s, is dened by: e = C(s? s ) (5) where C is a matrix which has to be selected such that CL(s; Z) > in order to ensure the convergence of the control law. The otimal choice seems to consider C as the seudo-inverse L(s; Z) + of the interaction matrix. The matrix C thus deends on the deth Z of each target oint used in the visual servoing. An estimation of the deth can be obtained using, as in D visual servoing, a ose determination algorithm (if a D target model is available), or using a structure from known motion algorithm (if the camera motion can be measured). However, using this choice for C may lead the system to near, or even reach, a singularity of the interaction matrix. Furthermore, the convergence may also not be attained due to local minima reached because of the comutation by the control law of unrealizable motions in the image []. Another choice is to consider C as a constant matrix equal to L(s ; Z ) +, the seudo-inverse of the interaction matrix comuted for s = s and Z = Z, where Z is an aroximate value of Z at the desired camera osition. In this simle case, the condition for convergence is however only satised in the neighborhood of the desired osition, which means that the convergence may not be ensured if the initial camera osition is too far away from the desired one. If an exonential convergence is desired (_e =?e where is a ositive scalar), a simle control law is given by [5]: T =?e =?C(s? s ) (6) The block diagram of the D visual servoing aroach is given in Figure 4. 4 D 1/ Visual Servoing If n D oints P j on a lanar target are used, it is well known that each image oint j, of coordinates s + - s Features Control law Features Extraction ; V Deth Estimation or aroximation Robot Camera Figure 4: Block diagram of the D visual servoing u v 1 T in the current camera frame F, is related to the corresonding image oint j, of coordinates u v 1 T in the desired camera frame F 1, by an homograhy such that [7]: j j = H j fj = ; 1; ; :::; n? 1g (7) where j is a ositive scalar and H is a ( ) homograhy matrix. More recisely, H can be written [7]: H = R + n t T d (8) where R and t are the rotational matrix and the translational vector between the current camera frame F and the desired camera frame F 1 resectively (see Figure 5), n = n x n y n T z is the unit vector normal to the target lane exressed in F 1 and d is the distance between the origin of F 1 and. F1 H d F target oint Figure 5: Modelisation of camera dislacement for D 1/ visual servoing Let us remark that H is dened u to a scalar factor, therefore one of the unknown j can be set to 1 without loss of generality (for examle = 1). Equation (7) is available for each feature oint in the image, d 47

6 thus, for n oints, we have to solve a linear system of n equations and n + 8 unknowns: that is n? 1 unknowns j (fj = 1; ; :::; n? 1g since = 1), and 9 unknown elements of H. A minimum of four oints is needed to erform a linear estimation. Let us emhasize that the estimation of H does not necessitate the knowledge of the D osition of the target oints on lane, which makes unnecessary a D model of the target. After matrix H is comuted, R, t=d and n can be estimated [7]. Unfortunately, in the most general case we have two dierent solutions. The indetermination is eliminated by choosing the solution which is such that the vector n is as co-linear as ossible with the desired orientation of the camera otical axis. Let us note that the translational vector t is estimated only u to a scale factor since the desired distance d is unknown. D visual servoing can thus not be emloyed. It is ossible to design a control law such that R and t=d resectively have to reach the identity matrix I and T (which thus imlies the achievement of the ositioning task). However, such a control law does not ensure that the considered object will always remain in the camera eld of view since it is only based on D estimated arameters. Getting out of the image may thus occur in the resence of imortant errors in the intrinsic arameters of the camera or in the robot Jacobian. We have therefore referred another more robust solution. The block diagram of the D 1/ visual servoing aroach is given in Figure 6. We now describe its dierents arts. Our control scheme is based on the D estimated rotation between F and F 1 (which has to reach the identity matrix). We also use the ratio r which controls the deth between the camera and the target (and which has to reach the desired value r = 1). Indeed, the distances d and d are unknown, but the ratio r = d=d can easily be estimated using R, t=d and n. We remark that the current normal vector n to the lane, exressed in F, and the current distance d between frame F and can be written in function of vector n and distance d : From equations (9) and (1) we get: n = Rn (9) d = d + n T t (1) r = d = 1 + t d nt = 1 + t R d nt T (11) d Finally, in order to control the 6 camera d.o.f and to maintain the target in the camera eld of view, we also introduce the use of two indeendent visual features, such as the image coordinates of a target oint, which can be controlled by using classical D visual servoing. s r R + -+ s -+ r - R Partial ose Estimation Features and Deth Rotation Control law Features Extraction V Robot Camera Figure 6: Block diagram of the D 1/ visual servoing We rst consider the control of the camera orientation. Let u be the rotation axis and the rotation angle obtained from the revious artial-ose estimation. The rotational velocity of the camera can be exressed in function of the angular velocity _ around the axis of rotation u: u _ = (1) The translational degrees of freedom are used to maintain the target in the image and to control the ratio r between the current distance d and the desired distance d. A oint is chosen in the desired image, such as for examle the nearest from the image center. As done for the D visual servoing, the chosen features are its coordinates s = u v T in the image. By using equations (1) and (), the relationshi between the time variation of the chosen features and the velocity of the camera is: _s = 1 Z L v(s)v + L! (s) (1) where Z is the deth of the corresonding D oint. Since the D oint belongs to the target lane, Z can be written as: Z = d n T and from equation (11), we get: (14) 1 = nt = (15) Z rd d where = nt can be comuted from image measurement and the artial-ose algorithm. Furthermore, by r derivating equation (11) with resect to time, we get: _r = nt d (RT _t + _R T t) (16) = nt d RT (_t + R _R T t) (17) = nt d ( _t + t) (18) =? nt d V (19) 48

7 since, from the fundamental kinematics equation, we have _t =?V? t. Combining equation 19 to equation (1) and equation (1), we get the following system: where: 4 M! = _s _r u _ 5 = M v = 4 4 " 1 d M v M! I? u? v?n x?n y?n z # V uv?(1 + u ) v (1 + v ) uv?u () 5 (1) 5 () Equation () is similar to equation (1). The task function aroach [5] can thus be alied. If we desire an exonential convergence of s towards s, of r towards r = 1 and u towards u = (with decreasing velocity ), we obtain: 4 _s _r u _ 5 =? 4 s? s r? 1 u 5 () From equations () and (), the control law is given by: V =? where M?1 v M?1 v =?1 n T d M?1 v?d M?1 v M! I 4 s? s r? 1 5 u (4), the inverse matrix of M v, is given by: n z + n y v? n yu u ? n xv n z + n x u v 7 7 5?n x?n y 1 (5) The determinant of matrix M v is? (n T ). This determinant is null only if the otical axis of the camera belongs to lane (in this singular case, all the image oints are collinear), which ensures the stability of the system in all the task sace (more recisely, the half sace in front of the target lane) if and n are correctly estimated. We note that the ositive scalar only inuences the time-to-convergence of the translational control loo. Indeed, if is estimated as the ositive scalar and vector n is well-estimated, the matrix M?1 v ()M v () = I is always denite ositive. We can also note that the camera translational velocity is roortional to the desired distance d between F and. An aroximate value has thus to be chosen during the o-line learning stage. However, this value has not to be recisely determined (by hand in the following exeriments) since it does not inuence the stability of the system, but only the time-to-convergence of the translational velocity and the amlitude of the ossible tracking error due to a wrong comensation of the rotational motion. As far as the tracking error is concerned, it is roortional to the rotational velocity and thus disaears when the camera is correctly oriented. 5 Exerimental results The control law has been tested on a seven d.o.f. industrial robot MITSUBISHI PA1 (at EDF DER Chatou) and a six d.o.f. cartesian robot AFMA (at IRISA - see Figure 7). The camera is mounted on the endeector of the robot. The target is a black board with seven white marks (see Figure 8). The extracted visual features are the image coordinates of the center of gravity of each mark. As far as camera calibration is concerned, we have used the ixel and focal lengths given by the constructor in order to comute the image coordinates u and v from their measured values in the image. The center of the image has been used for the rojection of the otical axis in the image. The images corresonding to the desired and initial camera osition are lotted in Figure 8a and 8b resectively. Figure 7: Exerimental cell at IRISA Classical D visual servoing has rst been tested, but without success due to the too much imortant 49

8 (a) (b) On the obtained lots, we can note the divergence of the visual features, which induces the getting out of the target outside the camera eld of view, and thus the failure of the D visual servoing. The corresonding translational and rotational velocity are given together in Figure Figure 8: Images of the target for the desired (a) and the initial (b) camera osition 1.5 dislacement between the initial and desired ositions. Matrix C of equation (5) was chosen always equal to L + (s ; Z ) because Z cannot be calculated without a geometric model of the target. The errors on the coordinates of the center of gravity of each mark are given in Figure 9. The corresonding trajectory in the image is given in Figure Figure 9: Error on the coordinates of the image features (ixels) versus iteration number using D visual servoing Figure 1: Trajectory of target oints in the image using D visual servoing Figure 11: Translational velocity V (cm/s), rotational velocity (dg/s) versus iteration number using D visual servoing We now resent the results obtained using our new D 1/ visual servoing scheme. In the resented exeriment, d is set to cm. From the estimated homograhy, we get a artial estimation of the camera dislacement. For examle, the estimated rotational dislacement, using the initial and desired images, was r x = 9.6 dg, r y = -.5 dg, r z = 95.9 dg (while the real dislacement was r x = 8.1 dg, r y = -4. dg, r z = 96.1 dg). Similarly, the estimated direction of translation was tx =?:19, t y = :95, t z =?: (while jtk jtk jtk the real dislacement was t x = -.8 cm, t y =.6 cm, t z = -5.9 cm, which imlies tx =?:15, t y = :95, ktk jtk t z =?:. The used algorithm is thus quite recise ktk (maximal rotational error is around dg, as well as the angle error on the direction of translation) in desite of the coarse calibration which has been used. The exonential decreasing of the estimated rotation, of ratio r = d=d and of the coordinates u and v of the target oint in the image are given in Figure 1, Figure 1 and Figure 14 resectively. As can be seen on the lots, the obtained results are articularly stable and robust. In this exeriment, is set to :1. A smaller value just imlies a slower timeto-convergence. Higher values could also be chosen. However, the system (whose rate is 1 Hz) becomes unstable when > :5. A good aroximation of d reduces the amlitude of the tracking error due to the rotational movement. A such small erturbation can be observed in Figure 14 from iteration 1 to 5 where the tracking error is aroximately ixels. Let us note that the tracking 5

9 1 8 error comletely disaears (after iteration 5) when the rotational motion has no more inuence. The oututs of the control law, V and are given in Figure Figure 1: Rotation u (dg) versus iteration number Figure 15: Translational velocity V (cm/s), rotational velocity (dg/s) versus iteration number using D 1/ visual servoing Once again, we can note the stability and the robustness of the control law. Finally, the error on the image coordinates of each target oint is given in Figure 16 and the corresonding trajectory in the image is given in Figure Figure 1: Ratio r = d=d versus iteration number Figure 14: Error on the coordinates of the target center of gravity (ixels) versus iteration number Figure 16: Error on the coordinates of the image features (ixels) versus iteration number using D 1/ visual servoing We can note the convergence of the coordinates to their desired values (the control scheme is stoed when maximal error is less than.5 ixels), which demonstrates the correct achievement of the ositioning task. Let us recall that these image coordinates are only used to comute the homograhy between initial and nal camera ositions. 51

10 Figure 17: Trajectory of target oints in the image using D 1/ visual servoing 6 Conclusion In this aer, we have roosed a new aroach to vision-based robot control. Contrarily to the osebased aroach, the roosed control law does not need any geometric model of the target. Furthermore, the domain of convergence for the D 1/ visual servoing (the half sace in front of the target) is larger than for the D visual servoing. Finally, exeriments show that a recise camera calibration is not needed. This is due to the fact that the homograhy estimation is iteratively erformed in conjunction with a closedloo control law. The main hyothesis of our actual scheme is that the image oints used to estimate the homograhy corresond to colanar D oints. Future work will thus concern the generalization of homograhy estimation in the case of any non-lanar target using algorithms such as those resented in [] and [1]. Acknowledgements This work was suorted by the national French Comany of Electricity Power: EDF. We are grateful to the team manager and the researchers of the Teleoeration/Robotics grou, at DER Chatou, for their articiation and hel. [] B. Couael, K.E. Bainian. Stereo vision with the use of a virtual lane in the sace. Chinese Journal of Electronics, 4():{9, Aril [4] D. Dementhon, L.. Davis. Model-based object ose in 5 lines of code. International Journal of Comuter Vision, 15:1{141, [5] B. Esiau, F. Chaumette, P. Rives. A new aroach to visual servoing in robotics. IEEE Trans. on Robotics and Automation, 8():1{6, June 199. [6] B. Esiau. Eect of camera calibration errors on visual servoing in robotics. In rd International Symosium on Exerimental Robotics, Kyoto, Jaan, October 199. [7] O. Faugeras, F. Lustman. Motion and structure from motion in a iecewise lanar environment. International Journal of Pattern Recognition and Articial Intelligence, ():485{58, [8] K. Hashimoto, ed. Visual Servoing: Real Time Control of Robot maniulators based on visual sensory feedback, volume 7 of World Scientic Series in Robotics and Automated Systems. World Scientic Press, Singaore, 199. [9] S. Hutchinson, G.D. Hager, P.I. Corke. A tutorial on visual servo control. IEEE Trans. on Robotics and Automation, 1(5):651{67, October [1] W.J. Wilson, C.C. Williams Hulls, G.S. Bell. Relative end-eector control using cartesian ositionbased visual servoing. IEEE Trans. on Robotics and Automation, 1(5):684{696, October [11] L.E. Weiss, A.C. Sanderson, C.P. Neuman. Dynamic sensor-based control of robots with visual feedback. IEEE Journal of Robotics and Automation, (5):44{417, October References [1] B. Boufama, R. Mohr. Eiole and fundamental matrix estimation using the virtual arallax roerty. In IEEE International Conference on Comuter Vision, ages 1{16, Cambridge, USA, [] F. Chaumette. Potential roblems of stability and convergence in image-based and osition-based visual servoing. Worksho on Vision and Control, Block Island, USA, To aear, June