ViSP: A Software Environment for Eye-in-Hand Visual Servoing

Transcription

1 VSP: A Software Envronment for Eye-n-Hand Vsual Servong Érc Marchand IRISA - INRIA Rennes Campus de Beauleu, F-3542 Rennes Cedex Emal: Erc.Marchand@rsa.fr Abstract In ths paper, we descrbe a modular software that allows fast development of eye-n-hand mage-based vsual servong applcatons (VSP states for Vsual Servong Platform ). Vsual servong conssts n specfyng a task as the regulaton n the mage of a set of vsual features. Varous ssues have thus to consdered n the desgn of such applcaton: among these ssues we fnd the control of camera motons and the trackng of vsual features. Our envronment features a wde class of control sklls as well as a lbrary of real-tme trackng processes. Some applcatons that used ths modular archtecture on a sx dof cartesan robot are fnally presented. 1 Overvew Vsual servong s a very mportant research area n vson-based robotcs (see [4] for an extensve revew). Despte all the research n ths feld, t seems that there are no software envronment that allows fast prototypng of vsual servong tasks. The man reason s certanly that t usually requres specfc hardware (the robot and, most of the tme, dedcated mage processng boards). The consequence s that the resultng applcatons are not portable and can be merely adapted to other envronment. Today s software desgn allows to propose elementary components that can be combned to buld portable hgh-level applcatons. Furthermore, the ncreasng speed of mcro-processors allows the development of real-tme mage processng algorthms on a smple workstaton. Chaumette, Rves and Espau [3] proposed to consttute a lbrary of canoncal vson-based tasks for eye-n-hand vsual servong that contents the most classcal lnkages that are used n practce. Toyama and Hager [9] descrbe what such a system should be n the case of stereo vsual servong. The proposed system s specfed usng the same phlosophy as the XVson system [7]. Ths system (called Servomatc) would have been ndependent from the robot and the trackng algorthms. Followng these ways, VSP, the software envronment we present n ths paper features all these capabltes: ndependence wth respect to the hardware, smplcty, extendblty, portablty. Moreover, VSP features a large lbrary of elementary postonng tasks wrt. varous basc control features (ponts, lnes, crcles, spheres, cylnders, etc.) that can be combned together, and an mage processng lbrary that allows the trackng of vsual cues (dot, segment, ellpse, splne, etc.). Ths modular platform has been developed n C++ on Unx Workstatons. The remander of ths paper presents the background dealng wth the eye-n-hand vsual servong and the trackng algorthms. Software desgn s then presented n the lght of a smple example. Expermental results carred out on a sx dof cartesan robots are fnally presented. 2 Image Based Vsual Servong The mage-based vsual servong conssts n specfyng a task as the regulaton n the mage of a set of vsual features [6][8]. Embeddng vsual servong n the task functon approach allows us to take advantage of general results helpful for the analyss and the synthess of effcent closed loop control schemes. These control ssues are now well known, we just gve a rapd overvew of the vsual servong process. 2.1 Control ssues Let us denote P the current value of the set of selected vsual features 1 used n the vsual servong task and measured from the mage at each teraton of the control law. To ensure the convergence of P to ts desred value P d, we need to know the nteracton matrx (also called mage Jacoban) L T P defned by the classcal equaton [6]: Ṗ = L T P(P,p)T c (1) where Ṗ s the tme varaton of P due to the camera moton T c. The parameters p nvolved n L T P(P,p) represent the depth nformaton between the consdered objects and the camera frame. A vson-based task e s defned by: e = W + C(P P d ) + (I W + W)g T s (2) 1 In the remnder of ths paper, we wll call these features control features by opposton to the features tracked n the mage sequence (the vsual cues ) from whch the control features are extracted. 3224

2 where C, called combnaton matrx, has to be chosen such that CL T P(P,p) s full rank about the desred trajectory q r(t). It can be defned as C = WL T+ P (P,p) (L + denotes the pseudo nverse of L). In that case, we set W as a full rank matrx such that Ker W = Ker L T P. If the vsonbased task does not constran all the n robot degrees of freedom, a secondary task g s can also be performed. g s s the gradent of a cost functon h s to be mnmzed (g s = h s ). Ths cost functon s mnmzed under the constrant r that P = P d. The two projecton operators W + and I W + W guarantee that the camera moton due to the secondary task s compatble wth the regulaton of P to P d. In order to make e exponentally decrease and then behave lke a frst order decoupled system, we get: where: T c = λe W d + e 1 t (I W+ W) gt s t T c s the camera velocty; (3) λ s the proportonal coeffcent nvolved n the exponental convergence of e; d e 1 t (wth e1 = C(P P d)) represents an estmaton of a possble autonomous target moton. 2.2 A lbrary of vsual servong sklls One of the dffculty n vsual servong s to derved the nteracton matrx L T correspondng to the selected control features. A systematc method has been proposed to derved analytcally the nteracton matrx of a set of control features defned upon geometrcal prmtves [6][3]. Any knd of vsual nformaton can be consdered wthn the same vsual servong task (coordnates of ponts, lne orentaton, surface (or more generally nertal moments), dstance, etc.) Knowng these nteracton matrces, the constructon of elementary vsual servong tasks s straghtforward. As explaned n [3] a large lbrary of elementary sklls can be proposed. The current verson of VSP allows to defne X-to-X feature-based tasks wth X = {pont, lne, sphere, cylnder, crcle, etc.}. Usng these elementary postonng sklls more complex tasks can be consdered by stackng the elementary matrces. For example f we want to buld a postonng task wrt. to a segment, defned by two ponts P 1 and P 2, the resultng nteracton matrx wll be defned by:» L T L T P = P1 L T P 2 where L T P s defned, f P = (X, Y ) and z s ts depth, by: L T 1/z X/z XY (1 + X P = 2 «) Y 1/z Y/z 1 + Y 2 XY X Ths way, more feature-based tasks can be smply added to the lbrary. An other mportant feature s the capablty to ntroduce a secondary task. Ths secondary task can be a soluton to the mpossblty to plan the camera trajectory by ntroducng some constrants n ts moton. We proposed a large lbrary of secondary tasks from trajectory trackng to occlusons avodance and jont lmts/sngulartes avodance. Trackng capabltes have also been ntegrated. When the target s movng, an estmaton of the autonomous target moton s requred n order to avod trackng errors. 3 Trackng Vsual Cues Parallel to the development of the platform wth respect to control part of vsual servong, we have to develop algorthms dealng wth trackng ssues. The vsual servong formalsm allows to use more complex features or combnaton of features. VSP allows to consder, besdes the usual dots, vsual cues such as lnes, ellpses, or more complex curves. Informaton avalable n the descrpton of these shapes are then used to descrbe the control features. We have developed fast real-tme trackng processes. Few systems features real-tme capabltes on a smple workstaton. The XVson system [7] s a good example of such systems, however, t does not features all the trackng capabltes we wanted 2. In our case, we decded to use the ME (movng edges) algorthm [2] adapted to the trackng of parametrc curves. It s a local approach that allows to match movng contours. Prevous works have been done to use ths algorthm to track lne segments [1] on a dedcated IP board. 3.1 General Algorthm One of the advantages of the method s that t does not requre any edge extracton, furthermore t can be mplemented wth convoluton effcency and can therefore ensure a real-tme computaton [2]. As we want an algorthm that s fast, relable, robust to partal occlusons and to false matches, we decded to track only a lst L t of pxels along the consdered edge and then to determne, by a robust least square approach, the equaton of the support prmtve that fts these data. We wll not descrbed the ntalzaton process n detals, let us just say that we are able to ntalze the pxels lst L t. Then, for each pxel we estmate the drecton of the tangent to the edge θ. b The process conssts n searchng for the correspondent P t+1 n mage I t+1 of each pxel P t L t. We determne a 1D search area Q j, j [ J, J] n the drecton of the normal to the contour δ. For each pxel P t of the lst L t, and for each poston Q j lyng n the drecton δ we compute the matchng crteron correspondng to a log-lkelhood rato ζ j. Ths s nothng but the absolute sum of the convoluton values computed at P and Q j usng a pre-determned mask M θ functon of the orentaton of the contour. New poston P t+1 s gven by: Q j = arg max ζ j wth ζ j = I(P ) M θ +I(Q j ) M θ j [ J,J] 2 Even f, n many way, t s far more complete that the system we propose.

3 provdng that the value ζ j s greater than a threshold λ. Then pxel P t+1 gven by Q j s stored n L t+1. A new lst of pxels s obtaned. Fnally, gven the lst L t+1 the new parameters of the feature are computed usng a least squares technque. 3.2 Trackng Vsual cues Lne segments. The smplest case we consder s the lne segment [1]. The representaton consdered for the lne are the polar coordnates (ρ,θ): xcos θ + y sn θ ρ = Ths case s very smple as the drecton θ s drectly gven by the parameters of the features. The choce of the convoluton mask s then straghtforward. A ponts nserton processes ether n the mddle of the segment, to deal wth partal occlusons or mss-trackng, and at the extremtes of the segment to deal wth sldng movements has been ntroduced n the trackng method. Ellpses. Dealng wth the ellpse many representaton can be defned, we chose to use the coeffcents K that are obtaned from the polynomal equaton of an ellpse: K x 2 + K 1y 2 + 2K 2xy + 2K 3x + 2K 4y + K 5 = The ellpse correspond to the case K 2 2 < K K 1. The parameters K can be estmated from the pxels of the lst L t usng a least square method. From the parameters K we can derved other representatons as the parameterzaton (X c, Y x, µ 11, µ 2, µ 2) based on the normalzed nertal moments. Splnes. A splne s defned by a parametrc equaton: Q(t) = n 1 X j= d α jb j(t), t [, 1] where the α j are the control knots of the splne, d s the degree of the splne (d = 3 for a cubc splne) and B j splne bass functon. Snce the number p of tracked pont s usually greater than the number of desred control knots n, a least square method s used. Dscusson. The proposed trackng approach algorthms based on the ME algorthm allows a real-tme trackng of geometrc features n an mage sequence. It s robust wth respect to partal occlusons and shadows. However, as a local algorthm, ts robustness cannot be ensure n complex scenes wth hghly textured envronment. 4 Software envronment As already stated, whle developng ths software, our goal was to allow a portable (ndependent from the hardware), fast and relable prototypng of vsual servong applcatons. The frst part of ths secton presents the nternal archtecture of the system and how t has been mplemented. Descrbng the full mplementaton of the software s out of reach n ths paper, therefore, we wll focus on the noton of extendblty and portablty. The second part descrbes how to use the avalable lbrares from a end-user pont of vew. Let us note that all the functonaltes descrbed n ths secton have been mplemented and are fully operatonal. 4.1 An overvew of the VSP archtecture To fulfll the extendblty and portablty requrements we dvded the platform nto three dfferent parts (the trackng parts, a lbrary of control features, and the controller tself) and we wdely use C++ capabltes (templates, dervaton, nhertance, vrtual classes, etc.). As can be seen on the network of Fgure 1, each lbrary of the VSP envronment s ndeed extensble. In the controller lbrary, the CServoAfma class s derved from a CServo vrtual class and s specfc to our sx dof Afma robot. It redefnes some pure vrtual methods defned n CServo such as robot moton orders (.e., the CServoAfma::UpdateCameraVelocty specfc to a gven robot) and nherts all the methods and attrbutes of CServo (.e., generc control ssues). Addng a new robot s then straghtforward. In the same way some specfc frame grabber classes (here CSunvdeo or CEdxa, that are our frame grabbers) can be derved from a generc CFrameGrabber class. Ths two examples also show that the platform s ndependent from the hardware and then portable. The extendblty can be manly seen n the control features lbrary. Each specfc control feature s derved from a vrtual class CBaseFeatures. Ths class manly defnes a few varables (e.g., a vector that descrbes the parameters P and p) and a set of vrtual members functons that are features dependent (e.g., the way to compute the mage Jacoban L or the vrtual functon that allows the trackng of the feature n the mages). It s mportant to note that all the relatons between the controller lbrary and the features lbrary s done through ths class. The vrtual functons defne n CBaseFeatures can be drectly used by the controlled even f they are not yet defned. The consequence s that the controller lbrary never know the nature of the manpulated features. Another consequence s therefore that t s absolutely not necessary to modfy the controller lbrary when addng a new feature. On the other hand, when addng a new feature n the control features lbrary, the programmer must defne the number of vsual nformaton, the way to compute the mage Jacoban, etc. Ths s done at a lower level (e.g., CPont, CLne,...). Some member functons of these derved classes reman vrtual. Indeed a control feature can be defned by many vsual cues, member functons lke Track(...) cannot be defned at ths level. Therefore, a few classes derve from the feature classes n order to supply the users wth many trackng capabltes. For example a pont can be defne as a smple dot (CPontDots n Fgure 1) or as the ntersecton of two lnes (CPontTwoLnes). In any case, usng the nhertance mechansm the controller knows whch functon t has to use. In concluson, addng new capabltes wthn the control features lbrary can therefore be done

4 Control features lbrary CBaseFeatures Controller Lbrary CServoAfma CServo CServoRobot2 CPont CLne CCylnder CSphere CServoSmu CPontDot CPontTwoLnes CLneTwoPonts CLneSegment nhertance CDots CStraghtLnes CEllpse CSplne CImage CFrameGrabber use the object Other Trackng lbrary CSunvdeo CEdxa Fgure 1: VSP structure of classes at two levels: addng a new control feature (e.g., a crcle n Fgure 1) or addng a new way to lnk ths feature to the vsual cues (for example, defnes a pont as the center of an ellpse). 4.2 VSP from a end-user pont of vew Our other clam was that VSP s smple to use. We wll therefore descrbe the software envronment, from the end-user pont of vew, n the lght of two smple examples mplemented usng VSP. The frst example s a postonng task wrt. a lne. man() { 1 CImage I ; 2 I.IntAcqImage(Sunvdeo()) ; 3 I.AcqImage() ; 4 5 CLneSegment lne ; // or CLneTwoDots lne ; 7 lne.inttrackng(i) ; 8 9 CLne Ld(,PI/2) ; // Int here the desred vsual 1 // features Ld (centered/horzontal) 1 2 CServoAfma task ; 3 task.addlnk(lne,ld) ; 4 task.intinteractonmatrx() ; 5 6 whle(...) { 7 CColVector Tc(6) ; 8 9 I.AcqImage() ; 2 task.getcurrent(i) ; 1 Tc = -.2 * task.taskfuncton() ; // Tc = -l L^+(s-sd) 2 task.updatecameravelocty(camera,tc) ; 3 } 4 } Lne 2 defnes the mage acquston protocol (here through the sunvdeo frame grabber). The CImage::AcqImage() functon puts the btmap n the computer memory where the trackng process wll be done. A control feature, a lne, s defned (5-6) as a segment (CLneSegment). Ths means that the vsual cue s a lne segment. Lne 9 defnes the desred poston of the vsual feature n the mage. The controlled CServoAfma derved from the CServo vrtual class s defned lne 12. Lne 13 defnes a lnk between the current poston (lne) of the vsual feature n the mage and the desred poston (Ld) whle lne 14 creates the correspondng nteracton matrx (here a 2 6 matrx). Note that more complex tasks can be defned by stackng other lnks usng the CServo::AddLnk method. It s straghtforward to wrte the loop tself. It features the mage acquston (19) and the current vsual features extracton or trackng (2). Fnally the vson based task e s computed usng the CServo::TaskFuncton() method and the new robot velocty orders T c = λe (22). The task may be more complex. Let us consder, a curves followng task (see the expermental results n the next secton). Ths problem can be dvded n two subtasks. The prmary task conssts n servong on the tangent to the curve (.e., mantan ths tangent horzontal and centered n the mage). The postonng skll used n ths experment s therefore a lne-to-lne lnk. Indeed, mage features used here are P = (ρ, θ) where ρ and θ are the parameters of a lne that s nothng but the tangent to the curve (2 dof are then controlled). The secondary task s a trajectory trackng at a gven velocty n the X drecton and s defned by a cost functon h s = X X V xt. Image processng conssts here n trackng a splne n the mage sequence and to compute the equaton of the tangent to the curve from whch we can control the camera moton. From a control pont of vew, ths task s smlar to the prevous one. However, n our software envronment there are no drect relatons between the tangent to a curve (that s, here, the vsual cue) and a lne (that s the control feature). As explaned n the prevous example, VSP usually allows to avod an explct access to the

5 trackers, but the number of relatons vsual cues/control features s vrtually nfnte. Therefore a drect access to the trackers s sometmes necessary. The splne tracker, here, s defned n lne 1 whereas the vsual feature (a straght lne) s assocated to ths tracker n lne 4. Then n the control loop, the splne s tracked n each frame (the CSplne::Track(CImage &I) method n lne 1) and the new control feature s computed (lne 11) and ntroduced n the controller (the CServo::NewCurrent(...) method n lne 12). Ths vsual servong task also features the use of a secondary task. Vector g s the gradent of h s, g = V x and s combned wth the prmary task usng the projecton operaton I W + W (lne 14). These smple examples allow us to show the mportance of the three lbrares: the trackers lbrary (defne by varable n lne 1), the control features lbrary (lne 3), and the controller lbrary (lne 6) and how they nteract wth each other. man() { CImage I ; 1 CSplne S(CUBICSPLINE) ; 2 S.IntTrackng(I) ; Curves followng task. The prncple of ths experment has been descrbed n the prevous paragraph. Let us just add that the goal was to follow a long ppe (2 meters long) that features three 9 o corners. Fgure 3 show three mages acqured durng ths task n the frst corner (note the tracked splne and the tangent n the mage). Fgure 2a depcts the translatonal veloctes. The bottom curves s the due to the secondary task. In order to avod hgh rotatonal velocty n hgh curvature area, the velocty V x has been defned as V x = V max exp( α(θ θ ) 2 ). Ths explan the nose n ths plot and the three T that can be observed (due to the three corners). Fgures 2b and c depct the errors θ θ d and ρ ρ d observed for the selected vsual features. The mportant error n θ whle crossng the hgh curvature area are due to the fact that the curvature s not predcted. Ths problem s very smlar to the trackng errors that can be observed n a target trackng task. Fgure 2d depcts the XY camera trajectory that reflects the shape of the ppe. 3 CLne L ; 4 L = S.Tangent(,) ; 5 CLne Ld(,PI/2) ; 6 CServo task ; 7 task.addlnk(l,ld) ; 8 task.intinteractonmatrx() ; whle(...) { CColVector Tc(6), g(6) 9 I.AcqImage() ; 1 S.Track(I) ; // Track the splne (vsual cue) 11 L = S.Tangent(,) ; // compute the tangent 12 task.newcurrent(l) ; // defne the new control feature 13 g[] = Vx ; // secondary task 14 Tc = -.2* (task.taskfuncton() + task.i_wpw*g); 15 task.updatecameralocaton(camera,tc) ; } } 5 Some Applcatons Table 1 sums up the dfferent elementary postonng tasks that have been mplemented usng VSP. Most of these results are now classc and wll not be descrbed here. We just propose results for less classc experments,.e. postonng wrt. to a sphere and curves followng. Other applcatons mplemented wth VSP are structure from controlled moton (pont, sphere), jont lmts and sngulartes avodance, occlusons avodance,... The applcaton presented n ths secton has been mplemented at IRISA on a sx dof cartesan robot. Image processng (descrbed n Secton 3) and control law (Secton 2) are performed on a Sun Ultra Sparc 1 (17 Mhz). The frame grabber was a Sunvdeo board. Tests performed on the Sun show that a lne can be tracked n 3.6 ms (wth 4 pxels n the lst L t and a maxmum dsplacement J = ±1) whereas an ellpse s tracked n 6.5 ms. A splne wth 4 pxels n L t and 15 control knots s tracked n less than 1 ms. Fgure 3: Ppe followng task: 3 mages acqured durng the task and tracked curve (red) and tangent to the curve (green) Let us note that a smlar problem has already been addressed n [5]. But even f the trackng was done usng snakes, the vsual features used n the control were ponts. Postonng wrt. a sphere. In ths second experment, we want to servo on a sphere (a png-pong ball). The task s to observe the sphere as a centered crcle n the mage wth a gven radus and then to move the camera at a constant dstance from the sphere center. The projecton of a sphere n the mage plane s an ellpse. Image processng conssts n trackng ths ellpse (here on a non-unform envronment). We chose as vsual features the nertal moments of ths ellpse (.e., X c, Y x, µ 11, µ 2, µ 2). Snce the real radus of the png-pong ball s known (19 mm), the secondary moton (along the X axs) does not ntroduce any perturbatons n the prmary task Each teraton s performed n 4 ms. 6 Concluson and future work VSP s a fully functonal modular archtecture that allows fast development of vsual servong applcatons. It s manly composed of three C-callable lbrares: two dedcated to control ssues (one of control processes and one of canoncal vson-based tasks that contans the most classcal lnkage [3]) and one dedcated to real-tme trackng (based on the Movng edges algorthm). Ths

6 Vsual Cues objects Control features (see [6] for detals) dots lnes ellpse B-Splne pont pont P = (X, Y ) lne lne P = (ρ, θ) (tangent) crcle ellpse P = (X c, Y c, µ 11, µ 2, µ 2 ) sphere ellpse P = (X c, Y c, µ 11, µ 2, µ 2 ) cylnder lnes P = (ρ 1, θ 1, ρ 2, θ 2 ) square ponts P = (X, Y ), = 1..4 lnes P = (ρ, θ ), = 1..4 quadrlateral moments/orentatons P = (X c, Y c, µ, α 1, α 2, α 3 ) Table 1: Elementary postonng tasks usng VSP: tracked cues and control features Tx Ty Tz.4 err. err.1 XY a b -12 c d Fgure 2: Ppe followng task: (a) Translatonal camera velocty (b) Error θ θ d and (c) ρ ρ d (d) XY camera trajectory Fgure 4: Trackng ellpse and servo on a sphere: t = 62, t = 126, t = 416, t = 76, t = 162 software has already been used for the development of a large number of applcatons. Let us fnally note that VSP also features a vrtual sx dof robot that allows to smulate vsual servong experments. Dealng wth vson-based control, many new features can be added to ths software. Only eye-n-hand 2D vsual servong wth respect to geometrcal features have been proposed. Therefore other vsual servong techncs can be ntroduced such as stereo vsual servong, or vsual servong wrt. dynamc nformatons or moton, etc. Furthermore, now that a set of basc tools are avalable, t s necessary to be able to deal wth hgh-level msson that combnes many tasks. References [1] S. Boukr, P. Bouthemy, F. Chaumette, and D. Juvn. A local method for contour matchng and ts parallel mplementaton. Machne Vson and Applcaton, 1(5/6):321 33, Aprl [2] P. Bouthemy. A maxmum lkelhood framework for determnng movng edges. IEEE Trans. on PAMI, 11(5): , May [3] F. Chaumette, P. Rves, and B. Espau. Classfcaton and realzaton of the dfferent vson-based tasks. n K. Hashmoto, edtor, Vsual Servong, pp , World Scentfc, Sngapor, [4] P.I. Corke. Vsual control of robot manpulator. K. Hashmoto, edtor, Vsual Servong, pp. 1 32, World Scentfc, Sngapor, [5] E. Coste-Manre, P. Couvgnou, and P. Khosla. Vsual servong n the task-functon framework: a contour followng task Journal of Intellgent and Robotcs Systems, 12:1 21, July [6] B. Espau, F. Chaumette, and P. Rves. A new approach to vsual servong n robotcs. IEEE Trans. on Robotcs and Automaton, 8(3): , June [7] G. Hager and K. Toyama. The XVson system: A generalpurpose substrate for portable real-tme vson applcatons. Computer Vson and Image Understandng, 69(1):23 37, January [8] K. Hashmoto, edtor. Vsual Servong: Real Tme Control of Robot Manpulators Based on Vsual Sensory Feedback. World Scentfc Seres n Robotcs and Automated Systems, Vol 7, World Scentfc Press, Sngapor, [9] K. Toyama, G. Hager, and J. Wang. Servomatc: A modular system for robust postonng usng stereo vsual servong. In Proc. of the Internatonal Conference on Robotcs and Automaton, pp , Mnneapols, Aprl 1996.