Dealing with constraints in sensor-based robot control

Transcription

1 Dealng wth constrants n sensor-based robot control Olver Kermorgant, Franços Chaumette To cte ths verson: Olver Kermorgant, Franços Chaumette. Dealng wth constrants n sensor-based robot control. IEEE Transactons on Robotcs, IEEE,, (), pp.-7. <.9/TRO..86>. <hal-87> HAL Id: hal-87 Submtted on 9 Aug HAL s a mult-dscplnary open access archve for the depost and dssemnaton of scentfc research documents, whether they are publshed or not. The documents may come from teachng and research nsttutons n France or abroad, or from publc or prvate research centers. L archve ouverte plurdscplnare HAL, est destnée au dépôt et à la dffuson de documents scentfques de nveau recherche, publés ou non, émanant des établssements d ensegnement et de recherche franças ou étrangers, des laboratores publcs ou prvés.

2 Dealng wth constrants n sensor-based robot control Olver Kermorgant, Franços Chaumette Abstract A framework s presented n ths paper for the control of a mult-sensor robot under several constrants. In ths approach, the features comng from several sensors are treated as a sngle feature vector. The core of our approach s a weghtng matrx that balances the contrbuton of each feature, allowng to take constrants nto account. The constrants are consdered as addtonal features that are smoothly njected n the control law. Mult-sensor modelng s ntroduced for the desgn of the control law, drawng smlartes wth lnear quadratc control. The man propertes are exposed and we propose several strateges to cope wth the man drawbacks. The framework s valdated on a complex experment, llustratng varous aspects of the approach. The goal s the postonng of a 6 DOF robot arm wth D vsual servong. The consdered constrants are both eye-n-hand and eye-to-hand vsblty, together wth jont lmts avodance. The system s thus hghly overdetermned, yet the task can be performed whle ensurng several combnaton of constrants. Index Terms Sensor-based control, sensor fuson, vsual servong, vsblty constrant, jont lmts avodance I. INTRODUCTION NAVIGATION or manpulaton tasks are often subject to several constrants. They can be nherent to the controlled system (jont lmts, lmted velocty), related to the sensors (vsblty constrant) or comng from the envronment (obstacles). In ths perspectve, the goal s thus to perform the desred task whle ensurng the constrants. A popular approach n ths feld s path plannng. The potental feld method [], [] s a common technque to generate collson-free trajectores. Ths method has been appled to vsual servong n [], where the trajectory s planned n the mage space and allows ensurng the vsblty and the jont lmts constrants. Predctve control has also been used n vsual servong []. In ths case, the whole trajectory s not planned but the objectve functon takes nto account the predcton over a fnte horzon. Path plannng n sensor space has also been desgned through LMI optmzaton [6], []. The man drawback of such schemes s that they requre a model of the envronment, and may not cope wth unexpected obstacles. On the other hand, reactve schemes such as sensor-based control have been used to cope wth the constrants. They are often less complex to desgn than path plannng schemes, and requre less knowledge of the envronment. The task functon approach [9] s a popular technque to buld sensor-based control laws. When dealng wth several sensors, each sensor Ths work was presented n part at ICRA and IROS. The authors are wth Inra Rennes-Bretagne Atlantque, Rennes, France. Olver Kermorgant s now afflated to ICube, Unversty of Strasbourg, France. kermorgant@unstra.fr, francos.chaumette@rsa.fr sgnal s gven a reference sgnal and consdered as an ndependent component of the global task functon. Each sensor thus corresponds to a partcular task. A classcal scheme, often named gradent projecton method (GPM), s to draw a herarchy between the dfferent tasks and to buld a control scheme that prevents lower subtasks to dsturb hgher ones [7]. Ths s a classcal way to combne sensor-based tasks and constrants such as jont lmts avodance n redundant systems [], []. However, a common ssue s when upper tasks constran all the robot degrees of freedom (DOF), preventng lower subtasks from beng performed. A soluton can be to buld a new operator that projects a subtask on to the norm of the man tasks [], freeng some DOF that can then be used by secondary tasks. Task sequencng technques [9] can also be used to make the task herarchy dynamc. Wth another formulaton, sensor-based control laws can be desgned wthout mposng a strct herarchy between the tasks. Here the data comng from dfferent sensors are treated as a unque, hgher-dmensonal sgnal. Ths s the approach chosen n [8] to fuse two cameras, and a force sensor and a camera, where the desgned control law s equvalent to a weghted sum of the subtask control laws. In the general case, usng several sensors rases the queston of balancng ther contrbutons n the control law durng the servong. Optmal methods such as Lnear Quadratc (LQ) control [7], [6] can be appled n ths approach, however the balance s often tuned by hand after several trals []. As we wll see, our approach shares a smlar formulaton but avods the manual tunng of the weghts. More recently, several schemes have been desgned wth a weghtng at the level of the features: n [9] t allows addressng the problem of outlers n robust vsual servong, whle n [] t defnes a task n terms of a desred regon nstead of a desred poston. In [], the vsual features are deactvated n the case of vsblty lost. Recently, the framework of varyng-feature-set [] has unfed these approaches, wth an emphass on the contnuty of the control law n the case of Jacoban rank change whle sgnal components are added or removed from the control law. Yet, all these schemes were ntally desgned for only one sensor and to cope wth specfc ssues n vsual servong. In ths paper ths framework s naturally extended to the mult-sensor case. Recent methods have been proposed to perform a sensor-based task under several unlateral constrants, wth GPM framework [], [] or cascade of quadratc programs [9]. We wll show that our non-herarchcal control law ensures several constrants whle performng a mult-sensor task. In the presented paper, there s no concept of prorty between the dfferent tasks: only the global error s taken nto account. Ths allows defnng a real

3 mult-sensor task that s performed n all sensor spaces at the same tme, as presented n [] n the case of mult-camera vsual servong. The man contrbuton of ths paper s to propose a canoncal weghtng at the level of the features wth an automatc computaton of the weghts. It allows avodng any dffcult and cumbersome manual tunng. Instead of balancng between the tasks, a mult-sensor task s defned, then the features themselves are balanced wth a weghtng functon that takes nto account the several sensors and constrants. As we wll see, balancng at the level of the features allows focusng on the most crtcal constrants, whch s not the case f all the constrants are consdered as a sngle task and share the same weght. Ths approach does not requre any herarchy between the tasks and show nce propertes n the sensors space and n the robot behavor. The proposed approach s a generc framework that embeds our prevous works about mult-camera vsual servong [], robot postonng whle ensurng the vsblty constrant [] and avodng the jont lmts []. In ths paper, all these ssues are addressed wthn an homogeneous framework. As we wll show, ths allows regroupng very easly all the tasks and constrants nto a sngle experment. The robot can thus perform eye-n-hand/eye-to-hand cooperaton, together wth jont lmts avodance whle ensurng the vsblty constrant n both mages. As far as we know, ths s the frst tme such a complete and complex confguraton s consdered. The paper s organzed as follows. The general modelng of a mult-sensor robot s presented n Secton II. We also show how the proposed weghtng of the sgnal error can take unlateral constrants nto account. Then, the control law, ts stablty analyss and ts man propertes are descrbed n Secton III. Several addtonal strateges are presented n Secton IV for specfc ssues that may occur n practce. Fnally, expermental results are presented n Secton V. II. MULTI-SENSOR MODELING Ths secton presents the general modelng of a mult-sensor robot. Frst, we defne the global knematc model, then we ntroduce the weghted sgnal error that wll be used n the control law. We propose a generc weghtng functon that allows both balancng the sensor features and takng nto account unlateral constrants. A. Knematc model We consder a robotc system equpped wth k sensors provdng data about the robot pose n ts envronment. The robot jont postons are denoted q and we defne n = dm(q). Each sensor S delvers a sgnal s of dmenson m wth k = m = m and we assume m n. A sgnal component s called a sensor feature. In the case of a motonless envronment, the sgnal tme dervatve s drectly related to the sensor velocty v expressed n the sensor frame by: ṡ = L v () where L s named the nteracton matrx of s [9], [] and s of dmenson (m 6). Its analytcal form can be derved for many features comng from exteroceptve sensors. It depends Fg.. control frame Mult-sensor model. s s sensory data Object of nterest sensor sgnal object frame manly on the type of consdered sensory data s and on the sensor ntrnsc parameters. L may also depend on other data: for nstance the nteracton matrx of an mage pont observed by a camera depends on the depth of that pont, whch s not actually measured n the mage []. Now, we consder a reference frame F e n whch the robot velocty can be controlled. Ths frame can be for nstance the end-effector frame for a robot arm as shown n Fg.. The screw transformaton matrx allows expressng the sensor velocty v wrt. the robot velocty v e : v = W e v e () W e s gven by [8]: [ [ R ] e t ] e R W e = e () R e where R e SO() and t e R are respectvely the rotaton matrx and the translaton vector between F e and F s. [ t e ] s the ( ) skew-symmetrc matrx related to t e. Denotng e J q R m n the robot Jacoban, we have: ṡ = L W e e J q q () Denotng s = (s,..., s k ) the m-dmensonal sgnal of the mult-sensor set, () allows lnkng the sgnal tme varaton wth the jont velocty: wth: L... J s = LW e e J q = L k ṡ = J s q () W e. k W e e J q (6) where L R m 6k contans the nteracton matrces of the sensors and W e R 6k 6 contans the transformaton matrces. In the sequel we assume J s s of full rank n. We wll menton n Secton III-B that ths assumpton could be relaxed, but ths artcle focuses on the full rank case. We now defne the weghted error that wll be used n the control law. B. Weghted error The goal of sensor-based control s to desgn a control law that makes the robot reach a desred value s of the sensor features. Ths desred value may be obtaned by teachng-byshowng, or through a model at the desred pose: for example n [], vsual servong s performed wth the desred value beng the projecton of the object model at the desred camera pose. ) Weghted error: We defne the weghted mult-sensor sgnal error as: e H = He (7)

4 where e s the sensor error defned as e = s s and H s a dagonal postve sem-defnte weghtng matrx that depends on the current value of s. As n all varyng-feature-set schemes [], each component h of H may vary n order to ensure specfc constrants, manage prortes or add or remove a sensor or a feature from the control law. In the case of k sensors, H yelds: H... H = H k where H s the weghtng matrx for sensor S. (8) ) Weghtng canoncal form: Weghtng can be performed for several purposes. Frst, the most smple goal s to balance the dsparate sensor contrbutons durng the scheme. As n Lnear Quadratc (LQ) control, ths amounts to optmzng the system behavor by defnng a specfc weght for each sensor feature. In ths paper, we propose to also use the weght of a sensor feature to take nto account unlateral constrants on that feature. We thus defne a generc weghtng by: [, m] : h = h t + h c (9) where h t s tuned for the general balance of the feature and where h c allows takng potental constrants nto account. Classcal control laws such as vsual servo schemes usually use the smplest weghtng that corresponds to H = I m, that s:, h t = and h c = () Each weght (h t ) may also be tuned ndependently, as n LQ control. In practce, several trals are often necessary to determne the best weghtng []. In varyng-feature-set schemes [], [9], the weghts h t vary between and dependng on the confdence n each sensor feature. In ths paper we do not focus on the tunng of ths weght term, and we set h t = f the feature s s always used for the actual navgaton task, and h t = f the feature s only corresponds to a constrant to be ensured. We now explct the generc formulaton for the term h c handlng the constrants. A constrant s usually expressed by an nequalty on the value of a sensor feature. Ths s typcally the case for jont lmts or mage vsblty, and s also vald when range sensors are measurng the dstance to the obstacles. Sngularty avodance can also be consdered by settng a lower bound for det(js J s ). In all cases, ths corresponds to havng to keep the feature value s n an nterval [s, s+ ]. In that case, a safe nterval [s s, s s+ ] can be defned by: { s s =s +ρ (s + s ) s s+ =s + ρ (s + s ) () where ρ [,.] s a tunng parameter. The weghtng term h c handlng the constrant s then gven by: s s s+ f s h c s + > s s+ s = s s s f s s s < s s () otherwse h c s represented n Fg.. Smlarly to a repulsve feld [], the weght s null n the safe regon and contnuously ncreases to as the feature approaches the lmt. A constrant s sad 8 6 Fg.. s s s s s+ s + Feature value Generc weghtng h c for the basc constrants to be actve when ts weght h c s non-null. The next secton presents the control law and ts man propertes. In partcular, the weghts n case of one or several actve constrants are studed n Secton III-C. III. CONTROL LAW We now present the generc control law assocated wth the weghted error defned n (7) and ts lnk wth the lnear quadratc (LQ) approach. The man propertes for sensor fuson are then presented. In partcular, when only one constrant s consdered we show n Secton III-C that a suffcent weght ensures the correspondng feature error s decreasng, and that a mnmal weght can be determned. We then expose addtonal strateges that can be used for specfc ssues. A. Weghted control scheme In the task functon approach [9], the task error e task R n s defned by: e task = C(s s ) = Ce () where C R n m s named the combnaton matrx and allows to take nto account the redundancy between the sensor features. A classcal controller s then: q = λe task = λce () A popular choce that tres to ensure an exponental decrease of e task s C = Ĵs+, that s an estmaton of the Moore-Penrose pseudo-nverse of J s. In our case, J + s = (J s J s ) J s snce J s s full rank. Ths strategy can be seen as a partcular case of LQ control [6]. In ths framework, a cost functon F has to be mnmzed and s defned wth: F = (s s ) Q(s s ) + q R q () where Q and R are weghtng matrces that are usually tuned n order to obtan an optmal behavor of the robot. The selecton of the elements of Q and R may be computed from a pole placement tunng or consderatons on the varance of observed data []. In practce, several trals are often necessary to obtan the desred behavor. The correspondng control nput yelds [6]: q = λ(ĵ s QĴs + R) Ĵ s Q(s s ) (6) Ths control law s the same as () for the partcular weghtng Q = I m and R =. When consderng the weghted error e H nstead of e, the assocated Jacoban s J H = HJ s. In ths case, control law () yelds: q = λ(hĵs) + e H = λ(hĵs) + He (7)

5 The combnaton matrx of e s thus gven by: C = (HĴs) + H (8) When compared wth LQ control, ths combnaton matrx corresponds to the partcular weghtng Q = H and R =. Indeed, n ths case the LQ scheme (6) yelds: q = λ(ĵ s H Ĵ s ) Ĵ s H (s s ) (9) ( = λ (HĴs) (HĴs)) (H Ĵ s ) H(s s ) () = λ(hĵs) + H(s s ) = λc(s s ) () The man dfference between our scheme and classcal LQ control s about the use of the weghtng matrx Q. In LQ control t s usually tuned n order to obtan an optmal behavor of the robot. In the proposed scheme, we focus on the balance between the dfferent features and sensors and potental constrants. Actually, the two strateges may be used n a complementary way: f both H and Q are defned from ther respectve frameworks, a global scheme can be desgned by usng the weghtng matrx H QH. Also, a control cost matrx R could be used f needed. As for the estmaton of J s, the most popular choces are summarzed n [], showng the nduced behavors. From (6), computng J s amounts to choosng how to estmate the nteracton matrces L and the transformaton matrces W e. a) Interacton matrces: Several possbltes exst for the nteracton matrces []. Two classcal choces are to use the current nteracton matrx, or ts value at the desred pose L. In ths case the nteracton matrx s constant. Another popular strategy s the mean nteracton matrx /(L + L ), whch was recently shown as an approxmaton of secondorder mnmzaton []. b) Transformaton matrces: If the sensors are rgdly attached to the effector, then all transformaton matrces are constant and can usually be estmated n an offlne calbraton step. In the other case, for nstance n eye-to-hand confguraton, W e s not constant and the desred value We depends on the fnal D pose of the sensors wrt. the effector. Ths pose s generally unknown n sensor-based control. The most plausble choce s thus to estmate the current transformaton matrces from the robot geometrcal model and calbraton. In the sequel we assume that an estmaton of the current matrces L and W e s avalable, allowng to estmate J s n realtme. We now study the propertes of the proposed scheme. B. Control scheme propertes Ths secton explores the basc propertes of the control law. Frst we expose the condton for control law contnuty and study the case of null weghts. We then show local asymptotc stablty. ) Contnuty and nfluence of null weghts: The contnuty of varyng-feature-set control laws has been studed n []. In the general case, contnuty s ensured under three condtons: H and J s are contnuous and the pseudo-nverse operator s contnuous for HJ s. The latter s ensured under the assumpton that HJ s s full-rank, whch mples n partcular that there are always at least n non-null weghts. The case of rank change s solved n [] wth a generalzed pseudo-nverse, however n ths paper we use the classcal pseudo-nverse and assume HJ s s full-rank. Usual sensor features have a contnuous Jacoban J s. The formulaton of the weghtng matrx n Secton II-B s also contnuous, hence the control law s contnuous. Control law (7) s desgned to ensure that Hė = λhe, whch s dfferent from classcal desgn ė = λe. Ths dfference clearly appears for confguratons wth null weghts. Assumng s = (s, s ) where features s have null weghts (H = ), the control law (7) can be wrtten: [ ] +[ H q= λ Ĵ H e H Ĵ H e [ = λ (H Ĵ ) + ] H Ĵ = λ[ ] + [ H e ] = λ(h Ĵ ) + H e () ] [ H e The scheme s thus equvalent to the control law for actve features only. In partcular, t s dfferent from not takng nto account the weghtng matrx n the pseudo-nverse. More precsely, n [6] the combnaton matrx s defned as C = Ĵs+ H, nducng the followng: q = λĵs+ He () [ ] +[ ] Ĵ H e = λ () Ĵ The zeroed error components are thus stll taken nto account and the system behaves exactly as f the desred values for e had been reached, whch nduces an undesred conservatve behavor to ensure the useless constrants e =. Ths s not the case wth our approach. ) Local asymptotc stablty: Varyng-feature-set schemes usually neglect the tme varaton of H by assumng the weghtng matrx s varyng slowly, or that t s null at the convergence as n regon-reachng vsual servong []. Actually, when H s ntegrated nto the combnaton matrx and assumed to be varyng wrt. s, the stablty analyss s the same as wth a varyng J s []. As for classcal IBVS schemes, a drect consequence s that global asymptotc stablty cannot be proven as soon as redundant features are nvolved (m > n). From (6) and (8), the task error varaton yelds: ė task = Cė + Ċe = (CJ s + O) q = λ(cj s + O)e task () where O R n n = when e task = []. Wth the combnaton matrx from (8), ths scheme s known to be locally asymptotcally stable (LAS) n a neghborhood of e = f [8]: CJ s = (HĴs) + HJ s > (6) The system s thus LAS when HJ s and HĴs are full rank and when the Jacoban J s s suffcently well estmated, whch s the case n general. In ths case, potental local mnma correspond to confguratons where H (s s ) Ker Ĵs. We wll see n Secton IV-A how to deal wth ths ssue. Let us also note that determnng theoretcally the convergence doman seems to be out of reach. However, as we wll see n Secton V, t reveals to be surprsngly large n practce. ]

6 C. Partcular case of one constrant In ths secton we focus on the case where only one actve constrant s nvolved. In that case, we show that a suffcently hgh weght nduces the decreasng of the correspondng feature error. In partcular, we determne the mnmal weght ensurng the correspondng constrant s respected. Dealng wth several actve constrants smultaneously s fnally dscussed at the end of ths secton. ) Suffcent weght: We assume the reference value s of the feature s s n the confdence nterval. A suffcent condton for the assocated constrant to be ensured s that the error e = s s decreases. We now show that ths can be ensured at each teraton f the assocated weght s hgh enough. A classcal Lyapunov functon assocated wth the weghted error s V (e H ) = e H e H. Assumng we are n the doman of local stablty, the tme dervatve of V yelds: V = V m ė H = h e ė < (7) e H = The error e decreases f e ė <, whch s equvalent to: h > h e ė je j ė j (8) Hence, n any confguraton there exsts a suffcently hgh weght h that ensures the correspondng feature error norm s decreasng. Note that f ė = or e =, the correspondng constrant s de facto ensured. Ths property on suffcent weghts has been recently hghlghted n [], where the parallel s drawn wth the GPM approach and constraned optmzaton. Isolatng the partcular feature s, the control law (7) can be wrtten as the mnmum-norm soluton to: mn H J q H ė + h J q ė (9) q where ė s such that Hė = λhe and X denotes a value related to all features except s. It has been shown n [] that the soluton to (9) when h tends to nfnty s exactly the soluton to the constraned { mnmzaton: mn q H J q H ė s.t. J q = ė () Note that () corresponds to the GPM approach wth feature s used as the prorty task and the other features as the secondary task. At a gven teraton and f the Jacoban J s suffcently well estmated, the condton ė e < s ensured wth the system () snce n ths case, ė e ė e = λe <. Hence, comng back to (9), there exsts a value h mn such that: h > h mn, ė e < () The decrease of the error, hence the correspondng constrant, can be thus ensured wth a fnte weght at any gven teraton. We now explct the computaton of ths mnmal weght. ) Mnmal weght: We denote s the feature correspondng to the consdered constrant. The goal here s to dsturb the task e as lttle as possble by determnng the weght h that s as small as possble, yet suffcently hgh to ensure the correspondng constrant. j The tme varaton of the constraned feature s gven by: ṡ = J q = λj (HJ s ) + H(s s ) () = λj (J s H HJ s ) (J H e + h J e ) () We now show that ensurng ṡ = leads to a lnear condton on h. We have: (Js H HJ s ) = adj(j s H J s ) det (Js H J s ) = A(h) () D(h) where D(h) s a strctly postve polynomal of (h ), beng the determnant of a symmetrc nvertble matrx, and A(h) s the adjugate matrx of Js H J s, that s the matrx of the cofactors. A s a (n n) full rank symmetrc matrx. The constraned feature tme varaton thus yelds, up to the scale factor λ D : ṡ J A(J H e + h J e ) () We now show that J A does not depend on h. To do so, t s suffcent to show that J A does not depend on h, as any permutaton of the rows of s, J and H would lead to the same control law. Let Q and R be the QR decomposton of J. We have: JA = R Q adj(qrh R Q ) (6) = R Q adj(q )adj(rh R )adj(q) (7) = R adj(rh R )adj(q) (8) as Q adj(q ) = det(q )I n = I n. R beng upper trangular, h only appears n the frst element of RH R. From the adjugate matrx propertes, the frst row of adj(rh R ) does not depend on h. Thus, as R s lower trangular, the frst row of JA does not depend on h, whch concludes the demonstraton that can be extended to all ndexes. We denote the two h -ndependent scalars: { c = J AJ H e a = J AJ e (9) From (), ṡ s thus lnear wrt. h and can be wrtten: ṡ c h a () Ths leads to two confguratons C and C that are represented n Fg. : Approachng the constrant (C): f c and a have the same sgn, the robot s gong towards the constrant. In that case there exsts a postve h such that ṡ s null. Avodng the constrant (C): f c and a do not have the same sgn, the robot s movng away from the constrant: selfavodance occurs and the avodance scheme can be gnored. From ths observaton, we defne the values s a and s a+ where the feature has to stop: { s a = s + ρ a (s + s ) s a+ = s + ρ a (s + s ) () where ρ a < ρ s a tunng parameter. The mnmal value can thus be computed analytcally from (): { c a h mn = f c a > (C) else (C) () where h mn = corresponds to the confguratons where selfavodance occurs. Such mnmal weght ensures that the constrant s ensured at least when s = s a snce n ths case we have ṡ =.

7 s s a s s Feature value s s+ s a+ s + C C Fg.. Actvaton functon for lower and upper bounds. Fg. 6. Weghts for confguraton C (upper bound) and C (lower bound). In C (red), the feature s gong towards ts lmt and a non-null weght has to be used (here h mn =.). In C (green), the other features nduce the avodance, hence the weght can be null n the actvaton area. If the feature stll approaches the lmts, the generc weghtng h c s used n both cases. Fg.. Confguratons C (left) and C (rght). The feature approaches the nearest lmt n C, whle t goes away n C. wall x x x (a) Trajectory to x.6 h (b) Weght to x Fg.. Mnmal weght n smulaton. Robot trajectory (a) and correspondng weght (b). The dotted lne shows the trajectory wth the generc weght (). The mnmal weght value s. at the begnnng, then ncreases as the robot approaches the wall. The mnmal weght s null once the robot has passed the wall. However, as we do not need to ensure the constrant before s = s a, the mnmal weght s smoothly taken nto account wth an njecton functon. ) Injecton functon: We address the njecton of h mn wth the followng form of the weghts:, h c = µ (s )h mn () where µ (s ) [, ] s a contnuous functon. To ensure contnuty of HJ s and He, weghts must be null at feature actvaton and deactvaton, and ncreasng as the constraned feature values vary from the safe lmt to the physcal lmt. In our case, the njecton functon s null when s = s s and equal to when s = s a. Such a functon can be defned wth a sgmod: ( ) +tanh( ) f s s+ s a+ s s s s+ <s <s a+ ( ) µ = tanh( ) f s a s a s s s s <s <s s else () µ s C and smoothly ncreases the weght as the feature reaches the lmt, wth µ (s a ) = µ (s a+ ) = and µ (s s ) = µ (s s+ ) =. The proposed njecton functon s represented on Fg.. Ths allows actvatng the feature as progressvely as possble, hence wth the smallest dsturbance on the man task. ) Example n smulaton: The proposed mnmal weght s llustrated n smulaton. The smulaton setup s voluntarly smple and conssts n a D Cartesan robot that has to reach a pont. The task Jacoban s thus I. The constrant s to keep a mnmum dstance to the wall that s present. The smulaton s represented n Fg.. The robot starts n x (, ). The measured dstance to the wall s d = whle the desred dstance has been set to d =. The actvaton values are defned as d a = and d s =. Denotng e x the task error and e d the error related to the constrant, the varables from (9) yeld: J x = I J d = [ ] A = I e d = () From (9), we thus have a = and c = [ ]e x. If the desred poston s x (, ) we have c =. From (), the mnmum weght s thus h mn =. As d =, the actual weght wll be h = µ(d)h mn =. h mn =.. As long as d > d a the robot wll thus approach the wall f the task requres so. The weght s represented n Fg. b. We can see the ntal value s ndeed., then ncreases as the robot comes nearer to the wall. Ths means the weght s not hgh enough to have d = at ths poston, whch s the desred behavor as we want the robot to stop approachng the wall only at d a =. If the desred poston s x (, ) we have c = : selfavodance occurs, as t can be guessed n Fg. a. Ths also occurs at the end of the task to x once the wall s passed, nducng a null weght and a straght lne trajectory. ) Ensurng several constrants: In the case of several constrants havng to be ensured smultaneously, couplng terms appear snce a system of equatons () s hghly non-lnear. A soluton stll exsts to stop all the endangered constrants (for nstance q = s always a soluton) but t would be dffcult to compute analytcally the correspondng set of optmal weghts. The mnmal weghtng () can thus be used together wth the generc weghtng (). Wth ths strategy, the weghtng s mnmal n [s a, s a+ ] but s stll robust to multple avodance. Such a weghtng s represented n Fg. 6. We have assumed that C holds for the upper bound wth an optmal weght of h mn =. and that C holds for the lower bound, hence the optmal weght s null. If the feature goes out of [s a, s a+ ] the generc weghtng s used for both bounds. In ths case, an endangered constrant wll have ts weght ncreased untl t reaches a suffcent value, whch explans why the generc weghtng () s not bounded. As the suffcent and mnmal weghts for one constrant depend on the other constrants (see (8) and ()), ths can lead to a general ncreasng of the weghts correspondng to all the endangered constrants untl avodance. In the general case the nduced behavor s

8 + q a+ q s+ q s q a - Data: α =, α + >, α < whle convergence has not been reached do compute optmal weghts H = Dag(H c, H t ); compute v e usng (7); f v e < v ɛ and H(s s ) > e ɛ then α α + α; else α + α (α ); end apply control law usng weghts Dag(αH c, H t ); end Algorthm : Escape from local mnma 6 (a) Jont postons q (b) Jont weghts H q Fg. 7. Jont poston and weghts whle escapng from a local mnmum. Oscllatons appear n h (red), nducng small oscllatons n the robot moton. satsfactory even f t remans possble to defne a task under constrants that would be mpossble to perform. In such a case, weghts cannot be proven to be fnte anymore snce the system s no more stable and (7) does not hold. Fnally, the sole generc weghtng may also be used, leadng to a less optmal behavor as seen wth the dotted trajectory n Fg. a. We now hghlght practcal ssues for the presented system. IV. POTENTIAL ISSUES Three undesred behavors may be encountered n the presented system. Frst, as n all sensor-based approaches, local mnma may appear as soon as the system s overdetermned, that s m > n. Reachng a desred poston where the constrants are actve s a second ssue. Fnally, havng potentally hgh weghts may cause oscllatons n some cases. In ths secton, we propose several strateges for each of these ssues. A. Escapng from local mnma The man drawback of the proposed scheme, as for all redundant reactve sensor-based schemes, s the potental exstence of local mnma. Indeed, as soon as m > n only local stablty can be proven. As no plannng s consdered wth a hgher-level controller, the approach that has been nvestgated s to detect that a local mnmum has been reached, and try to escape from t. A local mnmum s easly detected as t s necessarly a confguraton where the end-effector velocty s almost null, whle some of the weghted error components H(s s ) are not null. The detecton condton can thus be defned by two parameters v ɛ and e ɛ such as a local mnmum corresponds to a confguraton where: v e < v ɛ and H(s s ) > e ɛ (6) 6 Once a local mnmum has been detected, we allow the system to perform non-optmal moton n terms of the sensor-based task, by ncreasng the weghts correspondng to the actve constrants. Ths can be seen as a random walk [] where we use the structure to compute the escapng moton. We denote e c the set of features that regroups the actve constrants, and e t the other features. The correspondng strategy to modfy the weghtng matrx s descrbed n Algorthm : the weghts H c are artfcally ncreased by a multplcatve factor α, untl reachng a confguraton where: [ ] Hc (s s H + ) Ker Ĵs [ t ] (7) αhc (s s H + ) / Ker Ĵs t In ths case, the obtaned moton s null f α = whle t s not wth the obtaned α >. Ths may not be true for any gven α but n ths case Algorthm wll carry on ncreasng α and eventually lead to a confguraton that s out of the null space. Meanwhle, f α makes the weghts reach very hgh values, the system s slowed down by the adaptve gan detaled n Secton IV-C. α s always equal to as long as no local mnmum has been reached. Durng normal convergence, α s slowly set back to. The proposed algorthm makes the actve constrants more repulsve, whch can be seen as a temporary herarchy between the actve constrants and the other features. Stll, such a herarchy seems natural as constrants have of course to be ensured. On the opposte, gong out of a local mnmum often prevents from performng optmally the postonng task, as the escapng moton s usually opposed to the moton that s nduced by the task. That s why we temporarly ncrease the weghts of the constrants. The tunng of α + and α may be dffcult. In practce, α has to reach a suffcently hgh value n order to ensure that the robot wll not go back to the same local mnmum. A condton to allow the escape from a local mnmum s that α + α >. Ths corresponds to α ncreasng faster than t decreases back to. The value we used are α + =. and α =.99. Ths strategy s nspred by smulated annealng [], where the parameter α acts as the annealng temperature. We now show two smulaton examples of the proposed algorthm. a) Jont lmts: The nduced behavor s represented n Fg. 7, for a smulaton of jont lmts avodance n vsual servong. A local mnmum occurs around teraton. We can see n Fg. 7a that the jont postons are barely evolvng from teraton to 7, and that jont lmt avodance s actve for jont. The jont weghts are artfcally ncreased as seen n Fg. 7b. As the most crtcal jont s q, the correspondng weght s far more mportant than the others. Ths allows escapng from the local mnmum and nduced oscllatons are very small n practce. Fnally, t s nterestng to notce that local mnma rarely occur wrt. the number of features compared to the avalable DOFs. In partcular, n [] we have performed exhaustve smulatons fusng D and D vsual servong. No local mnma have been found n ths confguraton. b) D Cartesan robot: We use the robot setup presented n Secton III-C. Walls are set up such that a local mnmum

9 x x local mnmum (a) Trajectory (b) Weghts Fg. 8. D Cartesan robot escapng from a local mnmum (a). Wthout the proposed strategy the robot s stuck n the ndcated poston. The correspondng weghts (one per wall) (b) are quckly ncreasng at the begnnng, before slowly decreasng. exsts, as shown n Fg. 8a. Wthout the proposed algorthm the robot ends up n the ndcated poston. The proposed strategy allows the robot escapng the local mnmum, and uses the structure of the task to fnd an ext. In Fg. 8b we can see that the correspondng weghts are quckly ncreased, before slowly decreasng. Ths llustrates the balance between α + and α. Of course, as t s only a reactve scheme some local mnma stll exst, partcularly when the stuaton s symmetrcal. Indeed, n ths case ncreasng the weght would only lead to gong backwards. Complex traps such as U-shapes may not be escaped ether: n such cases a plannng strategy should be used. B. Reachng an unsafe poston If the desred poston s outsde the safe area, that s s ]s, s s ] ]s s+, s + ], the man task cannot be perfectly performed as t does not correspond to the global mnmum of the complete weghted task. Indeed, denotng s = (s t, s c ) where s t corresponds to the man task and s c to the constrants, the desred poston s defned by: q ( = arg mn e t H ) ( t e t arg mn e H e ) (8) q q where e H e = e t H t e t + e c H ce c. A suffcent condton to overcome the nequalty (8) s to ensure that H c = n a neghborhood of the desred poston. To do so, we ntroduce a progress parameter ξ( e t ) smoothly makng the constrant weghts null when the man task gets close to completon. f e t e ξ( e t )= ( ) f e t e +tanh( e e t e t e ) else (9) where e and e are defned so that the constrants are totally gnored when the man task s close to completon, that s e t < e. The correspondng weghtng matrx yelds H = Dag(H t, ξ( e t )H c ) and s equal to H = Dag(H t, ) at the vcnty of the desred poston. The desred poston can thus be reached. Fnally, dependng on the stuaton, one may or may not use ths progress parameter: ndeed n some confguratons t s preferable to converge to a compromse between the desred poston and the constrants, typcally f the desred poston les outsde of the boundares of the h h x Adapt No adapt Fg. 9. Oscllatons n a corrdor. Wthout the adaptve gan, the robot oscllates between the two walls (green lne). The adaptve gan allows drawng a smooth trajectory (dotted blue lne). constrants. C. Avodng oscllatons The generc weghts () ncrease when approachng the constrants. When several constrants are beng reached, ths may lead to oscllatons or even to volatng the constrants due to dscretzaton. In our case, an effcent way to cope wth ths ssue s an adaptve gan dependng on H that slows the system n the vcnty of the constrants. The LAS analyss n Secton III-B s of course stll vald wth a varyng gan, snce t can be consdered as part of the varyng combnaton matrx. The control gan λ nvolved n (7) s gven by: where: λ( H ) = (λ λ )e λ λ H + λ () λ = λ() s the gan n, that s for very small weghts λ = lm H λ( H ) s the gan to nfnty, that s for very hgh weghts λ s the slope of λ at H =. In practce we have used the values λ =, λ =. and λ =.. The proposed strategy s llustrated n smulaton n Fg. 9, wth the D Cartesan robot setup. Ths tme the walls draw a corrdor. If the gan s too hgh, oscllatons appear (green lne). Ths s not the case f the adaptve gan s used (dotted blue lne). Fnally, n the case of opposed constrants, hence several ncreasng weghts, such an adaptve gan would eventually make the robot stop f no soluton exst. Ths seems an acceptable behavor n such a bad stuaton. We now present the expermental results llustratng varous aspects of the proposed scheme. λ V. EXPERIMENTAL RESULTS In order to llustrate the proposed approach, experments are carred on a 6-DOFs Gantry robot. The control laws are mplemented usng VSP software []. We frst detal the expermental setup and ts calbraton. The sensors and constrants are then ntroduced one after the other n the control law. The eye-n-hand camera observes a fxed object, the CAD model of whch s known. Its edges are tracked to allow for x

10 (a) Eye-n-hand ntal mage (b) Eye-to-hand ntal mage Fg.. Intal mages. The object s large n the eye-n-hand mage (a). The D landmark approaches the top of the eye-to-hand mage (b). Fg.. Expermental setup: (a) Eye-n-hand camera wth a D landmark, (b) Observed object, (c) Eye-to-hand camera (a) Case : eye-n-hand mage. (b) Case : eye-to-hand mage Fg.. Wthout the vsblty constrant, the observed object leaves the FoV n case (a). The movng landmark leaves the FoV n case (b). Fg.. Integraton of varous subsystems: hybrd eye-n-hand features for the vsblty constrant, eye-to-hand cooperaton and jont postons to avod jont lmts. the pose estmaton at camera rate (Hz) [8]. The eye-nhand camera carres a landmark that allows ts D trackng n the eye-to-hand vew []. The carred landmark s composed by dots. Both cameras are calbrated. The pose between the eye-n-hand camera and the landmark cmo s roughly calbrated. The eye-to-hand camera pose wrt. the robot reference frame fmc s also roughly calbrated (see Fg. ). Fg. represents the two ntal mages. The robot translaton jonts and are represented n Fg. b. Jont thus corresponds to an horzontal moton, whle jont corresponds to a vertcal moton n the eye-to-hand vew. The ntal and desred poses make t necessary for the robot to move away from the observed object n order to keep t entrely n the FoV. As we wll see, ths backward moton makes the endeffector approach not only the upper lmt of the eye-to-hand mage, but also some jont lmts. As the desred poston s out of the safe jont nterval, the jont weghts are progressvely set to (9) accordng to the strategy exposed n Secton IV. The adaptve gan () s also computed from the actvaton matrx norm. We now present the system behavor whle the constrants are added one after the other. A. Pure poston-based vsual servo (case ) As prevously sad, the pose between the eye-n-hand camera and the object s estmated at each teraton of the control scheme. It s thus possble to perform PBVS []. The correspondng D features are sd = (c tc, c θuc ). They descrbe the transformaton between the current and the desred camera pose. The assocated desred features s a null vector, and the nteracton matrx Ld s known to be bloc-dagonal, nducng decoupled translatonal and rotatonal motons []. In perfect condtons, the correspondng camera trajectory s a D straght lne. The assocated weghtng s classcally constant, that corresponds to Hd = I6. Furthermore, ths ensures the matrx HJs s full rank, whch s a condton for the control law contnuty. The man drawback of PBVS s the lack of control n the mage: control s done only n the D space and does not ensure the observed object stays n the FoV. In our case, ths lack of control clearly appears n Fg. a. After few, the object leaves the FoV and the task cannot be performed anymore. We thus add the vsblty constrant nto the scheme. B. Addng the vsblty constrant (case ) The vsblty constrant n vsual servong has been prevously addressed through swtchng control law [], or vsual plannng [], [], [7], []. Here, we defne a set of D ponts (o x,..., o xp ) that are attached to the observed object, typcally the nodes of the CAD model. As the camera pose c Mo s estmated n real tme, the D coordnates of the projecton of the D ponts can easly be computed together wth ther depth. The vsblty constrant s taken nto account by addng the feature vector sd as the Cartesan coordnates of these D ponts. The well-known analytcal expresson of the nteracton matrx of an mage pont depends both on ts mage coordnates (x, y) and on ts depth Z []. The nteracton matrx of sd can thus be computed n real tme. Smlarly, the

11 6 8 (a) Eye-n-hand weghts H d Fg (b) Eye-to-hand weghts H ext (c) Jont postons q Case. Vsblty constrants n eye-n-hand (a) and eye-to-hand (b) are competng at teraton. At ths tme, jont passes ts upper lmt (c) (a) Vsblty weghts H d (b) Jont postons q Fg.. Case. The weghtng s qute small for the vsblty constrant (a). The jont postons are nsde ther lmts but jont (green) approaches the upper bound (b). (a) Eye-n-hand vew (ter. ) (b) Eye-to-hand vew (ter. ) Fg. 6. Case. Ths tme the camera goes to the rght of the eye-to-hand mage whle ensurng the eye-n-hand vsblty constrant. correspondng desred features s d = (x, y ) are computed from the desred camera pose c M o. Let (x, x +, y, y + ) be the mage borders: a safe regon can be defned as n (). In ths experment we use ρ = %. Fnally, the feature vector s defned by: [ ] sd PBVS (dm. 6) s = () s d Vsblty (dm. ) where the dmenson of the feature vectors are detaled: 6 components for the PBVS, and for the vsblty constrant ( nodes n the object CAD model). The correspondng weghtng matrx s H = Dag(I 6, H d ) where H d s derved from (9) usng h t = and h c gven by (). We can notce that the global mnmum corresponds to the desred pose: ndeed, f s d = s d then c M o = c M o, and s d = s d. Hence, the progress parameter (9) s not used for ths constrant, as the robot wll converge to the desred pose even f some constrants are actve. The resultng mages are shown n Fg. 6. The actve nodes are plotted n orange for the vsblty constrant. Ths tme the object stays n the FoV durng the whole scheme. The vsblty weghts H d are represented n Fg. a. Ther value remans small (h < ) and yet allows ensurng the constrant. Several features are actve around teraton. The maxmum value s obtaned around teraton 6 for only one D feature. Ths corresponds to one of the nodes approachng the left border durng the rotaton around the optcal axs (see the vdeo accompanyng ths paper). Jont postons (Fg. b) stay nsde ther lmts, yet no avodance s specfed n ths experment. Fnally, Fg. b shows the D landmark goes out of the eye-to-hand vew around teraton, that s when the camera moves away from the object to keep t n the FoV. C. Addng the eye-to-hand vsblty constrant (case ) We now take nto account the vsblty constrant n the eye-to-hand vew. The modelng s the same as prevously exposed. The consdered ponts are the ponts from the D landmark. We denote s ext the correspondng D features. The global feature vector s thus s = (s d, s d, s ext ) and the weghtng matrx s H = Dag(H d, H d, H ext ) where H ext s defned exactly as H d. The resultng mages are shown n Fg. 6. Ths tme, the D landmark stays n the eye-to-hand FoV. The eye-n-hand vsblty constrant can stll be ensured as the camera moves to the rght nstead of movng up. As seen n Fg. c, ths makes jont (green) pass ts upper lmt (whch s not the real lmt so that t has been possble to realze ths experment). The correspondng weghts are represented n Fg.. Addng a new constrant makes the vsblty weghts H d ncrease when compared to the prevous secton. Indeed, eyen-hand and eye-to-hand vsblty constrants are competng around teraton whch makes the eye-n-hand weghts pass, whle one of the eye-to-hand weghts reaches. As prevously, the maxmum weght s reached around teraton 6 for the vsblty constrant. D. Addng the jont lmts avodance (case ) We now take nto account the jont postons n the task. The global feature vector yelds: s d PBVS (dm. 6) s = s d Eye-n-hand vsblty (dm. ) s ext () Eye-to-hand vsblty (dm. ) q Jont postons (dm. 6)

12 ... (a) Eye-n-hand vew (ter. ) (b) Eye-to-hand vew (ter. ) Fg. 7. Case. The camera cannot move to the rght anymore when the observed object s large n the eye-n-hand mage (a). Ths tme the D landmark comes towards the eye-to-hand camera whle rotatng around the optcal axs (b).... The correspondng weghtng matrx s thus H = Dag(H d, H d, H ext, H q ) where H q regroups the jont weghts. We use the strategy exposed n Secton III-C: H q corresponds to the optmal weghtng (). The actvaton and safe areas are defned wth ρ = % and ρ a = %. For ths constrant, the progress parameter (9) s used as the desred poston s lkely to le n the jont unsafe area. Fg. 7 shows the eye-n-hand and eye-to-hand mages that correspond to, when the man dffculty occurs: some D ponts are very near to the mage border n both vews. We can see n Fg. 9c that at the same tme one of the jont lmts s beng avoded very closely. Ths corresponds to a confguraton where the camera has to move away from the object n order to keep t n the FoV, but has ts moton lmted by both the eye-to-hand vsblty constrant and the jont lmt. The values of the weghts are represented n Fg. 8 and clearly reflect ths phenomenon. Indeed, all three curves ndcate that the constrants are endangered at the same tme. Several weghts reach ther maxmum around teraton. The correspondng values are hgher than n the prevous cases: the eye-n-hand vsblty constrant has some weghts reachng, whle the eye-to-hand and the jont weghts reach. As prevously announced these values are stll acceptable and do not endanger the condtonnng of H and the whole system s stable. Reducng artfcally the jont lmt would typcally lead to a confguraton where the task could actually not be performed wthout volatng the constrants and the robot would have stopped at ths poston. As prevously, a second peak value s reached for one vsblty weght around teraton 6. Ths tme the correspondng value s less than, compared to n case. Ths constrant s thus less endangered by the trajectory from case than the one from case. As for the general behavor of the robot, Fg. 9a reveals that some oscllatons appear n the velocty setpont. Ths s due to the hgh number of constrants that are near to volaton at the same tme. We can notce n Fg. 8c that the adaptve gan s reduced by at ths tme. Ths s clearly vsble n Fg. 9a that the system slows down around teraton. The measurement of the jont postons n Fg. 9c shows that the general moton remans smooth all along the task and especally even when jont s near ts lmt. Fnally, Fg. 9b represents the PBVS error. Even f the correspondng weghtng matrx s the dentty, the other Fg.. D trajectores for the presented cases, observed from the eyeto-hand camera. Pure PBVS (cyan) begns by a straght lne, before gettng nconsstent when the tracker loses the object. Case (blue) corresponds to the trajectory that reaches the hghest pont, as the eye-to-hand vsblty s not taken nto account. Case (green) makes the camera go to the rght nstead of gong up. Fnally, case (red) forces the camera to draw another trajectory n order to ensure all the constrants. weghts prevents the PBVS from decreasng exponentally. The convergence s stll satsfactory and oscllatons are qute small. E. Comparson between the several cases Fg. compares the trajectores correspondng to the presented experments. Ths shows very clearly that the endeffector has many avalable trajectores to perform the postonng task. However, not all of them respect the constrants: actually only the last confguraton (case, red) does. When performng only PBVS (case, cyan), the trajectory s a straght lne tll the falure due to the tracker losng the object. When not usng the eye-to-hand mage, the camera tends to go up (case, blue). On the opposte, the runs that use the eye-tohand camera share a lower trajectory. In partcular, the camera gong rght nstead of gong up s clearly vsble for case (green). We hghlght that no local mnmum s reached for any combnaton of features. The mplct concurrency allows addng new constrants wthout havng to model the potental couplng. F. Other experments The presented experment llustrates the general propertes of our approach. In complementary works, we have also hghlghted specfc aspects through other types of experments. In partcular, exhaustve smulatons have been carred for the vsblty constrant (case ). We have smulated 99 servongs, that s a large set of combnatons for ntal and fnal poses, showng that 98% converge whle ensurng the vsblty constrant wth the maxmum weght beng less than. The other cases converge wth hgher weghts, or wth a smaller control gan. In [], our approach has also been compared wth other control laws that address the vsblty

13 6 8 (a) Eye-n-hand weghts H d (b) Eye-to-hand weghts H ext λ (c) Jont weghts and adaptve gan Fg. 8. Weghts of the dfferent subsystems: eye-n-hand D ponts (left), eye-to-hand D ponts (mddle) and jont postons (rght). All constrants occur around teraton, makng the weghts have sgnfcatve values. The adaptve gan (black curve) shows the system slows down at ths moment.. v x (a) Camera velocty setpont v y v z ω x ω y ω z... t x t y θu x θu y. t z θu z 6 8 (b) PBVS error (c) Jont postons Fg. 9. General behavor of the robot. Camera velocty (left) shows some oscllatons when passng the vcnty of all constrants. PBVS error (mddle) ndcates that the vsual servong s performed durng the task. Jont postons (rght) hghlght the lmt beng avoded. 6 constrant. The jont lmts avodance has been valdated n [] on a 6-DOF robot arm Adept Vper8 for several postonng tasks n vsual servong. Fnally, the proposed framework has also been used n ultrasound mages n [7], to mantan the vsblty of an anatomc element of nterest durng tele-echography. VI. CONCLUSION Ths paper has proposed a generc approach for mult-sensor and mult-constrants fuson n sensor-based control. The lterature classcally addresses ths ssue by a herarchcal approach or by performng a weghted mean of the veloctes that are computed for each task. We proposed to perform the data fuson at the level of the features, by ntroducng a dynamc weghtng matrx. Whle some tunng aspects are smlar to LQ control, actvaton and deactvaton of the features s part of the varyng-feature-set approach, that has been recently formalzed for one sensor []. The general dea s that a robotc system can handle a hgh-dmensonal task and several constrants wthout havng to explct the herarchy or performng a manual tunng of the feature weghts. The man propertes of the proposed control law have been exposed, concurrng to the classcal condtons on the system rank wth local asymptotc stablty and potental local mnma n the case of redundancy. The scheme s generc even for sensors that are not rgdly attached to the end-effector frame. The man drawbacks of the proposed scheme are related to ts nature beng only reactve. The addtonal strateges that are proposed for the partcular cases of local mnma and unsafe desred poston both consst n modfyng the actvaton matrx ndependently from ts ntal desgn n terms of subsystems ntegraton. Ths can be vewed as the begnnng of a hgher-level controller that takes nto account the global confguraton and balances the weghtng matrx so that the nduced trajectory avods or escapes local mnma. The versatlty of the approach has been llustrated by consderng a mult-sensor, mult-constrant task. Several experments have shown that the proposed approach can handle varous combnatons of sensors and constrants for a postonng task. Future work wll consst n extendng ths framework to other types of sensors such as laser range or haptc devces. Other strateges, such as relaxng some constrants, could also ncrease the convergence doman of the proposed scheme. Ths could be suted for nstance for the vsblty constrant, where some parts of the object could be allowed to leave the feld of vew. ACKNOWLEDGMENT The authors would lke to acknowledge Perre-Brce Weber for hs valuable feedback on ths work. REFERENCES [] G. Allbert, E. Courtal, and F. Chaumette, Predctve control for constraned mage-based vsual servong, IEEE Trans. on Robotcs, vol. 6, no., pp. 9 99,. [] J. Barraquand and J. Latombe, Robot moton plannng: A dstrbuted representaton approach, Int. Journal of Robotcs Research, vol., no. 6, pp , 99. [] S. Brooks and B. Morgan, Optmzaton usng smulated annealng, The Statstcan, vol., no., pp. 7, 99. [] F. Chaumette and S. Hutchnson, Vsual servo control. I. Basc approaches, IEEE Robot. Autom. Mag., vol., no., pp. 8 9, 6. [] C. Cheah, D. Wang, and Y. Sun, Regon-reachng control of robots, IEEE Trans. on Robotcs, vol., no. 6, pp. 6 6, 7.

14 [6] G. Ches, Vsual servong path plannng va homogeneous forms and LMI optmzatons, IEEE Trans. on Robotcs, vol., no., pp. 8 9, 9. [7] G. Ches and Y. Hung, Global path-plannng for constraned and optmal vsual servong, IEEE Trans. on Robotcs, vol., no., pp. 6, 7. [8] A. Comport, E. Marchand, and F. Chaumette, Effcent model-based trackng for robot vson, Advanced Robotcs, vol. 9, no., pp. 97, October. [9], Statstcally robust -D vsual servong, IEEE Trans. on Robotcs, vol., no., pp., 6. [] P. Danes and D. Bellot, Towards an LMI approach to multcrtera vsual servong n robotcs, European journal of control, vol., no., p. 86, 6. [] W. Decré, R. Smts, H. Bruynnckx, and J. De Schutter, Extendng TaSC to support nequalty constrants and non-nstantaneous task specfcaton, n IEEE Int. Conf. on Robotcs and Automaton, 9, pp [] L. Deng, F. Janab-Sharf, and W. Wlson, Hybrd moton control and plannng strateges for vsual servong, IEEE Trans. on Industral Electroncs, vol., no., pp.,. [] F. Fahm, C. Nataraj, and H. Ashrafuon, Real-tme obstacle avodance for multple moble robots, Robotca, vol. 7, no., pp , Mar. 9. [] N. Gans and S. Hutchnson, Stable vsual servong through hybrd swtched-system control, IEEE Trans. on Robotcs, vol., no., pp., 7. [] N. García-Aracl, E. Mals, R. Aracl-Santonja, and C. Pérez-Vdal, Contnuous vsual servong despte the changes of vsblty n mage features, IEEE Trans. on Robotcs, vol., no. 6, pp.,. [6] A. Hafez and C. Jawahar, Vsual servong by optmzaton of a D/D hybrd objectve functon, n IEEE Int. Conf. on Robotcs and Automaton, Roma, Italy, 7, pp [7] K. Hosoda, K. Igarash, and M. Asada, Adaptve hybrd control for vsual and force servong n an unknown envronment, IEEE Robot. Autom. Mag., vol., no., pp. 9, 998. [8] A. Isdor, Nonlnear control systems. Sprnger Verlag, 99. [9] O. Kanoun, F. Lamraux, and P.-B. Weber, Knematc Control of Redundant Manpulators: Generalzng the Task-Prorty Framework to Inequalty Task. IEEE Trans. on Robotcs, vol. 7, no., pp ,. [] M. Kazem, M. Mehrandezh, and K. Gupta, Knodynamc plannng for vsual servong, n IEEE Int. Conf. on Robotcs and Automaton,, pp [] F. Keth, P. Weber, N. Mansard, and A. Kheddar, Analyss of the Dscontnutes n Prortzed Tasks-Space Control Under Dscreet Task Schedulng Operatons, September. [] O. Kermorgant and F. Chaumette, Avodng jont lmts wth a lowlevel fuson scheme, n IEEE/RSJ Int. Conf. on Intellgent Robots and Systems, San Francsco, USA, September, pp [], Combnng IBVS and PBVS to ensure the vsblty constrant, n IEEE/RSJ Int. Conf. on Intellgent Robots and Systems, San Francsco, USA, September, pp [], Mult-sensor data fuson n sensor-based control: applcaton to mult-camera vsual servong, n IEEE Int. Conf. on Robotcs and Automaton, Shangha, Chna, May, pp. 8. [] O. Khatb, Real-Tme Obstacle Avodance for Manpulators and Moble Robots, The Internatonal Journal of Robotcs Research, vol., no., pp. 9 98, 986. [6] F. Lews and V. Syrmos, Optmal control. Wley-Interscence, 99. [7] T. L, O. Kermorgant, and A. Krupa, Mantanng vsblty constrants durng tele-echography wth ultrasound vsual servong, n IEEE Int. Conf. on Robotcs and Automaton, Sant Paul, USA, May. [8] E. Mals, G. Morel, and F. Chaumette, Robot Control Usng Dsparate Multple Sensors, Int. Journal of Robotcs Research, vol., no., pp. 6 77, May. [9] N. Mansard and F. Chaumette, Task sequencng for sensor-based control, IEEE Trans. on Robotcs, vol., no., pp. 6 7, Feb. 7. [] N. Mansard, O. Khatb, and A. Kheddar, A unfed approach to ntegrate unlateral constrants n the stack of tasks, IEEE Trans. on Robotcs, vol., no., pp , 9. [] N. Mansard, A. Remazelles, and F. Chaumette, Contnuty of varyngfeature-set control laws, IEEE Trans. Autom. Control, vol., no., pp. 9, November 9. [] E. Marchand, F. Chaumette, F. Spndler, and M. Perrer, Controllng an unnstrumented manpulator by vsual servong, The Internatonal Journal of Robotcs Research, vol., no. 7, p. 6,. [] E. Marchand, F. Spndler, and F. Chaumette, VSP for vsual servong: a generc software platform wth a wde class of robot control sklls, IEEE Robot. Autom. Mag., vol., no., December. [] M. Marey and F. Chaumette, A new large projecton operator for the redundancy framework, n IEEE Int. Conf. on Robotcs and Automaton, Anchorage, Alaska, May. [] Y. Mezouar and F. Chaumette, Desgn and trackng of desrable trajectores n the mage space by ntegratng mechancal and vsblty constrants, n IEEE Int. Conf. on Robotcs and Automaton, vol.,, pp [6] B. Nelson and P. Khosla, Strateges for ncreasng the trackng regon of an eye-n-hand system by sngularty and jont lmt avodance, The Int. J. of Robotcs Research, vol., no., p., 99. [7] N. Papankolopoulos, P. Khosla, and T. Kanade, Vsual trackng of a movng target by a camera mounted on a robot: A combnaton of control and vson, IEEE Trans. Robot. Autom., vol. 9, no., pp., 99. [8] R. Paul, Robot manpulators: mathematcs, programmng, and control: the computer control of robot manpulators. The MIT Press, 98. [9] C. Samson, M. Le Borgne, and B. Espau, Robot Control : The task functon approach. Clarendon Press, 99. [] F. Schramm and G. Morel, Ensurng vsblty n calbraton-free path plannng for mage-based vsual servong, IEEE Trans. on Robotcs, vol., no., pp. 88 8, 6. [] O. Tahr and Y. Mezouar, On vsual servong based on effcent second order mnmzaton, Robotcs and Autonomous Systems, 9. [] C. Van Loan, On the method of weghtng for equalty-constraned least-squares problems, SIAM Journal on Numercal Analyss, pp. 8 86, 98. [] W. Wlson, W. Hulls, and G. Bell, Relatve end-effector control usng cartesan poston based vsual servong, IEEE Trans. on Robotcs and Automaton, vol., no., pp ,. [] B. Wttenmark, R. Evans, and Y. Soh, Constraned pole-placement usng transformaton and LQ-desgn*, Automatca, vol., no. 6, pp , 987. [] T. Yoshkawa, Basc optmzaton methods of redundant manpulators, Laboratory Robotcs and Automaton, vol. 8, no., pp. 9 6, 996. Olver Kermorgant graduated from École Centrale Pars, France n. For two years he has been a Research Engneer n the Measurement and Control department at Arcelor Research, Metz, France. From 8 to he was wth the Lagadc group at Inra Rennes where he receved the Ph.D. degree n sgnal processng from Unversty of Rennes, France n. He then joned the Ocean Systems Laboratory at Herot-Watt Unversty, Ednburgh, Scotland as a Research Assstant. Snce he has been Assstant Professor at Unversty of Strasbourg, France. Hs research nterests nclude sensor-based robot control, dsturbance rejecton and optmzaton. Franços Chaumette graduated from École Natonale Supéreure de Mécanque, Nantes, France, n 987. He receved the Ph.D. degree n computer scence from the Unversty of Rennes, France, n 99. Snce 99, he has been wth Inra n Rennes where he s now senor research scentst and head of the Lagadc group ( Hs research nterests nclude robotcs and computer vson, especally vsual servong and actve percepton. Dr. Chaumette s IEEE Fellow. He receved the AFCET/CNRS Prze for the best French thess n automatc control n 99. He also receved the Kng-Sun Fu Memoral Best IEEE Transactons on Robotcs and Automaton Paper Award. He has been Assocate Edtor of the IEEE Transactons on Robotcs from to and s now n the Edtoral Board of the Int. Journal of Robotcs Research.