Tasks Sequencing for visual servoing

Transcription

1 acceped o he IEEE In. Conf on Ineligen Robos and Sysems, Sendai, Japan, Sepember 28 - Ocober Tasks Sequencing for visual servoing Nicolas Mansard, François Chaumee IRISA - ENS Cachan and INRIA Rennes Campus de Beaulieu, 3542 Rennes, France Firsname.Lasname@irisa.fr Absrac Classical visual servoing approaches end o consrain all degrees of freedom (DOF) of he robo during a ask s execuion. In his aricle a ne approach is proposed. The key idea is o conrol he robo ih a very underconsrained ask hen i is far of he desired posiion, and o incremenally consrain he global ask by adding furher asks as he robo moves closer o he goal. A mehod is firs proposed ha sacks elemenary asks unil he robo is fully consrained. To insure he coninuiy of he aricular velociies hen adding consrains, a ne conrol la is hen proposed. Experimens ha prove he ineres of he approach are also provided. I. INTRODUCTION isual servoing provides very efficien soluions o conrol robo moions from an iniial posiion o a precise goal [7]. I supplies high accuracy for he final pose, and good robusness o noise in image processing, camera calibraion and oher seing parameers. Hoever, if he iniial error is large, such a conrol may become erraic [1]. Approaches such as 2-1/2-D [8] or pah planning [3], [1] provide soluions ha enlarge he region here he sysem converges. Bu hey each consrain all available robo DOF from he beginning. This imposes an unique rajecory, hile in his paper, e propose o raher use very lo consrains hen he robo is far from he goal, in order o enlarge he rajecories available. Consrains are progresively added as he robo approches he required posiion. This paper deals ih asks sequencing [11], [12], [14], and describes a soluion o sack elemenary asks one on op of he oher unil all degrees of freedom of he robo are consrained, and he desired posiion is reached. A vas number of rajecories are usually available o reach he goal. Indeed, by consraining all DOF from he beginning, he classical conrols choose a paricular rajecory, ihou knoing if i is valid or no. In some paricular cases, his can lead o singulariy or insabiliy problems. To alays obain an ideal execuion, a firs soluion is o plan he rajecory. For example by using he poenial field mehod [3], [1]. The idea is o choose an opimal rajecory among all he available rajecories. This provides a complee soluion, hich ensures opimaliy, sabiliy and physical feasibiliy unil he goal hen i is reachable. Pah planning solves he deficiencies of basic approaches, bu by applying even greaer consrains o he rajecory of he robo. This means, hoever, ha his soluion is less reacive o changes of he goal, environmen or consrains. Raher han decide in advance hich pah should be used o reach he goal, siched sysems use a se of subsysems along ih a discree siching conrol [5], [2]. The robo ill hen avoid difficul regions by siching from a firs conrol la (a paricular rajecory) o anoher one hen necessary. This enlarges he sable area o he union of he sable area of each ask used. The key idea of his sudy is o use he leas amoun of DOF hen he robo is far from he goal, and o add consrains only as he goal comes closer. A each sep, he robo moves o achieve an elemenary ask, mainaining all he elemenary asks already compleed. A he end, he robo is enirely consrained by he sum of he consrains of each elemenary ask. Addiional consrains such as join limi avoidance can also be added using he remaining DOF. Our scheme consiss of hree phases: Firs, he elemenary asks are chosen. These asks do no have o be properly decoupled. The only resricion is ha each DOF of he robo should be consrained by a leas one ask. This is a difficul choice, currenly done off-line by he programmer. He also has o choose he naure and order of he asks o be applied. A conrol la hen has o be compued from he seleced elemenary ask. The idea is o mainain he elemenary asks already achieved hen moving he robo according o he las elemenary ask. This is done by a sack of asks, using he redundancy formalism inroduced in [13]. I guaranees ha every ne ask added ill no disurb he ones already achieved. The las sep consiss of insuring a smooh ransiion hen adding a ne elemenary ask. As shon in [14], one canno insure a coninuous siching by using a firs order conrol la like in classical servoing. One can alernaively use a second order conrol la [14], or a non homogeneous firs order conrol la as described belo. Secion II ill recall he redundancy formalism, and presens he adapaion done o apply i o a sack of several asks (more han o). In Secion III, a ay o insure a coninuous conrol la is given. The experimens and resuls are finally se ou in Secion I. II. STACK OF TASKS USING REDUNDANCY FORMALISM This secion presens he redundancy formalism [13], and he ay i is used o sack elemenary asks. I has firs been used for visual servoing in [4], and in numerous applicaions since (e.g. avoiding visual feaures occulaion

2 Y & F s u [9], or human-machine cooperaion using vision conrol [6]). The idea is o use he DOF lef by a firs ask having prioriy, o realize a secondary ask a bes ihou disurbing he firs one. A. Redundancy formalism Le be he aricular vecor of he robo. Le and be o asks, and heir jacobian. The robo is conroled in speed, i.e. by compuing he aricular velociy vecor. We use he command (1) here is a cusom parameer chosen o regulae he convergence speed of he global ask. This equaion ill be explained in deail in Secion III. We an o combine he o asks, using as a secondary goal realized under he consrain. This clause can be formulaed mahemaically:! (2) here is he variaion of he ask using he conrol la #" hich does no ake he second ask ino accoun, and! is he variaion of he ask using a conrol la "% hich realizes boh asks. A ask ha realizes (2) is '&)(* + (3) here ( is he orhogonal projecion operaor on he null space of,. Inroducing (3) in (1), one ges: -. #"/ (*. 3 (4) Since ( is in he null-space of,, he second par of he sum is. Thus! (5) The ask inroduced in (3) realizes a bes ihou alering, as specified. B. Using redundancy formalism ih hree asks and more Equaion (3) enables o sack o elemenary asks. Hoever e an o add as many asks as needed, unil here is no DOF lef. Le be : differen asks, and heir jacobians. One may hink ha he soluion is simply o projec he las ask : on he null space of he previous one, and hen projec his composed ask on he null space of he ask. The resul ould be: Tha is: '&;(* +=< 3 &;(?>%777@&;( 93A + 39 B%C (6) &)( &)( D( 3E &777 F A " G HJI " (LK= 9 (7) Hoever, since he projecion operaors do no commue, ( M( 3E is no in he null space of. Thus, ask ill be modified by E and by each supplemenary ask, hich is no an adequae behaviour. To beer soluions are proposed. Le i be assumed ha a ask ONPNPN 9, realizes he : firs asks hile respecing he prioriy consrains (ask Q should no disurb ask R, hen RTSUQ ). A supplemenary ask 9W is o be added ih respec o he : firs asks. The firs idea is o consider he firs : elemenary asks as a big one, named ONPNPN 9, and o projec 9W in he null space X " NPNPN F of his ask. The orhogonal projecion operaor 9 ono X " NPNPN F is compued from he jacobian ' ONPNPN 9 of he ask, hich 9 ZY Tha is: ONPNPN 9 [Y > '&;(*. 93A 9B Y (8) 9 ^ ^ 9 f 9 ( 9 ONPNPN &)(*. D\] &UD&)(* ONPNPN D\]_^ `ab 93A + 9 &UD& (9) \cd^ The are very difficul o compue, herefore hey are assumed o be, hich seems o be a saisfacory approximaion around he posiion is finally he projecion operaor ono X " NPNPN F Xihkj6j <, &l( 93A 9 C (1) This approximaion inroduces lags. When he conrol due o 9W is srong (i.e. 9? is high), he : firs errors do no remain a as required (see Fig. 1.a). A beer soluion is o projec he ask ino he space X " NPNPN F of he moions lef available by he firs : asks, here X " NPNPN F is defined as he inersecion of he null spaces of each ask. X " NPNPN F n o I " Xlhkj6j > qpb (11) The inersecion is compued by sacking he : jacobians. v X " NPNPN F Xlhrj!j. x (12) 9 and can be easily obained ih a S..D. The projecion operaor compued from he jacobians is no longer an approximaion. No more lag occurs, excep hose due o modeling errors in he jacobian marices 8p (see fig 1.b). C. Compuing he conrol la using a sack of asks We used a sack srucure for our experimens. Tasks are sacked one by one. The boom of he sack (ask ) has prioriy. The op of he sack (ask 9 ) consrains he DOF lef by he boom. Adding or removing a consrain is as easy as puing a ne ask in he sack.

3 Task errors Task 1 Task 2 Task 3 Task errors a b Fig. 1. Adding a ne ask : (a) he firs ask errors do no remain o using he approximaion (1) o compue he projecion operaor. (b) The errors remain o using (12). (See Secion I for deails abou he chosen asks.) III. SMOOTH TRANSITION The robo is conrolled by he aricular velociy. The conrol la has o be coninuous. Since, a break of coninuiy means an infinie acceleraion during a shor period of ime, hich implies ha he conrol ill no be correcly applied. Disconinuiies may occur hen e add a ne elemenary ask ino he sack. Usually, he conrol is compued from he folloing equaion ha consrains he behavior of he ask funcion: Since, e obain: " > kb 1 (13) 4 (14) here is an approximaion of he pseudo-inverse of and is used as a parameer o conrol he robo speed. The " funcion in (13) is chosen by he programmer o link and. One chooses generally " > rb 2 o se an exponenial decoupled decreasing of he error. The ask is so ha a good approximaion of is he ideniy maix I. Equaion (14) is hus equivalen o (1). The problem of coninuiy is due o he lack of consrains on he iniial value of. Le be a global ask, used o drive he robo unil ime. A ime, he conrol la siches o a second ask. Since and are linked linearly, no coninuiy guaranee can be ensured on. A ime =, is no coninuous. A firs soluion as proposed in [12], using a mixed conrol during a shor ransien ime afer = o ensure he coninuiy ( > > %BdB &=> %B here is a decreasing coninuous funcion of ime hich akes on values beeen ). The obained coninuiy as perfec. Hoever here as no guaranee for he corresponding ask o be ell condiioned or o correspond o a correc moion of he robo. A. Using a second order differenial equaion Soueres e al. proposed a soluion o his problem in [14]. They used a second order linear dynamics insead of (14) o ake ino accoun o iniial condiions > >! B >! B%B : &i &T U (15) Task 1 Task 2 Task 3 here he o parameers and are used o conrol boh he robo speed and he lengh of he ransien ime reponse. The main draback is he difficuly in choosing hese o parameers o obain he desired behavior. B. A simple paricular case : non homogeneous firs order differenial equaion I is choosen o link he ask funcion and is derivaive ih a non homogeneous firs order differenial equaion. In he general case, he equaion is: > rb 4 &k> %B (16) here k> %B has o be chosen so ha i ensures he coninuiy consrain, and equals o afer he ransien period: r>! B >! B &; >! B and!#" % k> %B (17) The funcion used for he experimens is r> %B < & >6 B & & >! B C (' A )&*! (18) here + is used o se he lengh of he ransien ime, and o se he decreasing speed of he error. This equaion is equivalen o a second order one: & >61& +#B &U> +#B (19) Neverheless, unlike >,- B, his couple of parameers > +#B is properly decoupled. In paricular, he end of he ransien ime is only se by +. Indeed, he ransien period ends hen " (see (13)) and (see (16)) are numerically equivalen, ha is o say hen k> %B is insignifican compared o > %B, i.e. > %B " > %B > %B J " > %B J k>6 B /' A )&*! (2) The erm. is exponenially decreasing, ih a speed se by +. The ask funcion > %B is equivalen o a decreasing exponenial funcion se by. I is simply necessary o choose + bigger han o ensure a shor ransien ime reponse, in comparison ih he decreasing ime of he ask error. The bigger he value +, he shorer he ransien ime, bu he sronger he acceleraion. Experimenally + D.D is chosen. I. EXPERIMENTS AND RESULTS This secion presens he experimenal resuls obained ih a six DOF eye-in-hand robo hich is o be posiionned ih respec o a square objec. The iniial image is given in Fig. 2.a. The desired image corresponds o he square cenered in he image, a a deph of.8m. The camera displacemen e have considered is very large ( 21 3 D5464, 27 98:8;8<4=4, 2>? <85@54=4, > h B 1 4BA 8<C;D, > h B 7 98<C;D, > h B > 3 (@:85C5D ). The resuls obained for a basic servoing ask using he coordinaes of four poins as visual feaures are quickly presened. The resuls of he experimen using our mehod

4 % " % s u " " + + & x D are hen provided in par I-B. Three elemenary asks have been chosen, using he cenre of graviy, he angle of one diagonal and he second order momens respecively. A las ask, using he four poins, complees he ask. The ask funcions used in he remainder of he ex are compued from he visual feaures [4]: > B (21) here is he curren value of he visual feaures, heir desired value and is combinaion marix. The ineracion marix relaed o is defined so ha, here is he kinemaic camera scre, hich is considered as inpu of he lo level robo conroller (i.e. ). has o be chosen so ha be full rank [13]. From (21), i is clear ha he ineracion marix and he ask jacobian are linked by he relaion: (22) Since is of full rank, (22) means ha he kernel of he o marices and are equal, and ha hey can be used indiscriminaely o compue he projecion operaors in (12). In pracice, he beer choice for is [4]. A. Using a 4-poins ask The ask based on 4-Poins is presened here by ay of a comparison ih nex resuls. The feaures used are he coordinaes of he four poins in he image. The pose is no compued a each ieraion of he conrol la. Thus, he rue values of, he feaures dephs, are no knon. An approximaion of he ineracion marix is used insead: > B (23) Due o he high roaion around he X- and Z-axis, he servo does no converge. The feaures are in fac quickly los, hich means obviously ha he desired posiion canno be reached (see Fig. 2.b). B. Four elemenary asks o consrain he six DOF The four elemenary asks ha have been chosen for conrolling he robo moion are presened here. As explained in he previous pars, here is no need o choose hem independen, hanks o he redundancy formalism. Neverheless, in order o have a beer and easier conrol over he robo rajecory, decoupled asks are chosen. Choosing a ask consiss of selecing specific visual feaures, and compuing he associaed ineracion marix. Feaures and ineracion marices are given for each elemenary seleced ask. A each ieraion, le H > H H B be he posiion of he four poins in he image (Q 7775 ). Le H > H H H B heir posiion in he 3-D space. The firs ask "!# is based on he posiion of he cenre of graviy. The associaed feaures are easy o compue: % " '& +, H I " H (&*) +, HJI " H (24) ) Since he projecive projecion does no preserve he barycener, his poin G in he image does no correspond o any physical poin. Neverheless his approximaion is chosen o be made and G is considered as he cenre of graviy of our 3D-objec. The approximae ineracion marix is hus obained from [4]: -./ 1 /./ '& (& 3,1 (&. / 7 /. / &2 & 3'&45& 36&4) (25) Afer compleion of his ask, he objec ill be cenered in he image, hich is really desirable for o reasons. Firs of all, he objec is in he middle of he camera field of vie, as far as possible from he border of he image. And, since "!# has prioriy over he oher asks, i is hus highly unlikely ha any poin be los during he execuion. In he second place, he problem has been ell linearized by cenering he objec. Indeed, by riing (1), he servo is considered as a linear problem. A good esimaor of he validiy of his approximaion is he disance beeen he ineracion marices a curren and desired posiions. As shon in Fig. 3, he disances for he ineracion marices of he four asks chosen are almos a he end of he cenering ask. The eakness of he nex asks ill be hus raher beer. The second ask 7 roaes he camera around he opical axis, so ha he objec ill be correcly oriened in he image. The feaure ha is used is he angle of a segmen in he image (e used he diagonal of he 4-poins arge). The approximaed ineracion marix is [4] 7 98;: =< T&2>8'?A@5:B: C< 98'?A@: B: =< i 68;?A@5: 3 v (26) here j is he lengh of he segmen, and 8L g> 68(8 B is cener. Afer compleing he firs o asks, he recangle is cenered in he image, and properly oriened around he opical axis. In his posiion, he res of he execuion is easier, and could be realized using he basic 4-poins ask ih a very high probabiliy of success. As shon in Fig. 3, he disance beeen curren and desired ineracion marices of ask 4 have again decreased. Hoever, i is preferable o scale he recangle properly before. The hird ask E uses he secondary cenered momens o conrol he range beeen he robo and he arge. The mos inuiive soluion is o consider he quadrilaeral

5 F area, i.e. he firs momen of he coninuous objec. The area can also be compued geomerically from he discree recangle. Since he considered objec is discree, e have used discree cenered momens of second order. The momens are compued using [15]. The cenered momen + H of a se of X poins is defined by + H o I " > o 1 &B > o 1 &qb (27) here > 6&B5& B is he cenre of graviy of he se. The feaure F considered is compued from he second order momens + and + : F, (28) here + & +. The associaed ineracion marix is also given in [15]. When he objec is parallel o he image plane, i has he folloing form: E < 3 " C (29) " (&i& 7 / * ) 1 / * ) here 1 / * ) _7 / * ) 6 & & The experimens have shon ha one of he four poins may be los during his paricular ask, even if he objec remains cenered. A firs soluion o his problem is o sop he moion on he opical axis a he middle of he disance, i.e. o servo on he desired value raher han. In his posiion, here is a loer probabiliy of loosing he visual feaures during he final ask. Hoever, his ould only pach-up he problem. According o us, he good soluion is o use he DOF remaining in order o keep he poins in he field of vie by adding a supplemenary ask based on a cos funcion [9]. More aenion ill be paid o his avenue in he near fuure. The las ask is he one used in Secion I-A. The eigh feaures are he verex coordinaes. The ineracion marix is easily compued from (25). When his ask is added o he sack, he objec is cenered in he camera field of vie, properly oriened and a he desired disance. The o remaining moions are combinaions of ranslaion along and roaion around X- and Y-axis. The las seleced ask is hus no opimal. I shos, neverheless, ha he redundancy formalism is poerful, even ihou properly decoupled elemenary asks. C. Resuls This secion commens he resuls obained ih our mehod on he example presened in Secion I-A. As in his secion, he deph is no knon. The approximaion is used insead. The visual ineracion marix K and he projecion operaor are hus approximaed oo. I has been noiced experimenally ha his approximaion inroduces racking errors. For a beer conrol, a specific a Fig. 2. Experimen ih he 4-Poins ask. (a) Iniial image. (b) Feaures rajecories in he image. The feaures leave he camera field. gain H is hus compued for each elemenary ask K. This gain is lo hen he corresponding error is high, and is high hen he ask is nearly compleed. The gain applied o he las ask added is much loer han he ones applied on he asks already compleed. The lags are hus srongly reduced. The conrol la is finally: > %B H I " H > K B K A > K K B & r> %B (3) here : is he curren size of he sack. The experimenal resuls are presened in Figures 4 and 5. The servo is compleed ihou difficuly. The errors decrease exponenially as specified. When a ask error is close enough o, he nex one is added. The asks already compleed say a during he remainder of he convergence. The firs hree asks are ell decoupled. Each of hem correspond o a favorie DOF. Iniialy using he X and Y ranslaions as ell as he pan-il, he objec is cenered in he middle of he image. A long roaion around Z-axis is hen realized o orien properly he square. The deph is hen adjused using he Z-ranslaion. Each moion corresponds o a recognizable par of he rajecories in he image (Fig. 5). The behavior during he las ask is no as good as in he firs hree. One can noice ha he curves are a lile bi more noisy. This is due o he approximaions in he projecors. A small disurbance appears on he compleed asks, hich is compensaed by an addiional moion in he opposie ay a he nex sep of he servo. This noise can be off se by using a loer gain on he las ask. Hoever he convergence is hen sloer. One can finally noice he coninuiy of he velociies. Using he classical conrol la (13), he velociies ould have been proporional o he error, meaning ha hey ould be highly disconinuous. Using (16), he velociies increase coninuously afer each sich.. CONCLUSION In his paper, a ne approach o conrol a robo using visual servoing has been presened. Raher han choosing a specific rajecory among all he possible rajecories from he beginning, he robo is lef as free as possible hen i is far from he desired posiion. Addiional consrains are added only hen i approaches he goal. A ay o implemen his general idea has been proposed, using b

6 L L* L1 L1* L2 L2* L3 L3* L4 L4* Fig. 3. Differences beeen visual jacobians a curren and desired posiion. Afer having compleed ask 1, he four marices are approximaely equal o heir value a desired posiion. Task 2 makes decreases once again. Trans. Speed Ro. speed.1.2 Task errors Task 1 Task 2 3 Task 3 Task Fig. 4. Second experimen using ask sequencing. (a) Translaional and (b) roaional velociies (cm/s and dg/s). (c) Tasks errors decreasing. redundancy formalism and a sack of asks. I guaranees ha, a he end of he execuion, he robo has reached he expeced posiion. Addiional ork has been se up o guaranee he conrol la coninuiy ih respec o ime. Experimens presened in he las secion have shon he ineres of he approach. Using very simple feaures and asks, very good rajecories have been obained even in he case of a difficul iniial posiion. Furher ork is necessary o explore in deph his ne approach. In paricular, far from he goal, DOF are available bu no used ye. I is neverheless one of he aims of he proposed approach in order o ake ino accoun oher Fig. 5. Final image and verexes rajecories in he image space. Tx Ty Tz Rx Ry Rz consrains such as joins limi avoidance. Furhermore, a poerful use of he ask sequencing mehod could be o move around elemenary asks ihin he sack during a shor period of ime in order o gain DOF ha can be used o avoid obsacles, join limis, ec. Anoher fuure perspecive of his research is he auomaion of he elemenary asks choice. I ould be useful for he robo o deermine auomaically alone hen a specific ask should be added. ACKNOWLEDGMENT The auhors hank he parners of he French ROBEA projec Egocenre, and especially Philippe Soueres. We hank also Anne-Sophie Tranchan and Omar Tahri for subsanial help during he realizaion of his ork. I. REFERENCES [1] F. Chaumee. Poenial problems of sabiliy and convergence in image-based and posiion-based visual servoing. In D. Kriegman, G. Hager, and A.S. Morse, ediors, The Confluence of ision and Conrol, pages LNCIS Series, No 237, Springer-erlag, [2] G. Chesi, K. Hashimoo, D. Praichizzo, and A. icino. A siching conrol la for keeping feaures in he field of vie in eye-in-hand visual servoing. IEEE In. Conf. on Roboics and Auomaion (ICRA 3), 3: , Sepember 23. [3] N.J. Coan, J.D. Weingaren, and D.E. Kodischek. isual servoing via navigaion funcions. IEEE Trans. on Roboics and Auomaion, 18(4): , Augus 22. [4] B. Espiau, F. Chaumee, and P. Rives. A ne approach o visual servoing in roboics. IEEE Trans. on Roboics and Auomaion, 8(3): , June [5] N. R. Gans and S. A. Huchinson. An experimenal sudy of hybrid siched approaches o visual servoing. IEEE In. Conf. on Roboics and Auomaion (ICRA 3), 3: , Sepember 23. [6] G. D. Hager. Human-machine cooperaive manipulaion ih vision-based moion consrains. Workshop on visual servoing, IROS 2, Ocober 22. [7] S. Huchinson, G. Hager, and P. Corke. A uorial on visual servo conrol. IEEE Trans. on Roboics and Auomaion, 12(5):651 67, Ocober [8] E. Malis, F. Chaumee, and S. Boude. 2 1/2 D visual servoing. IEEE Trans. on Roboics and Auomaion, 15(2):238 25, April [9] E. Marchand and G.-D. Hager. Dynamic sensor planning in visual servoing. In IEEE/RSJ In. Conf. on Inelligen Robos and Sysems (IROS 98), volume 3, pages , Lueven, Belgium, May [1] Y. Mezouar and F. Chaumee. Pah planning for robus imagebased conrol. IEEE Trans. on Roboics and Auomaion, 18(4): , Augus 22. [11] L. Peerson, D. Ausin, and D. Kragic. High-level conrol of a mobile manipulaor for door opening. IEEE In. Conf. on Roboics and Auomaion (ICRA 3), 3: , Sepember 23. [12] R. Pissard-Gibole and P. Rives. Applying visual servoing echniques o conrol a mobile hand-eye sysem. IEEE In. Conf. on Roboics and Auomaion (ICRA 96), pages , May [13] C. Samson, M. Le Borgne, and B. Espiau. Robo Conrol: he Task Funcion Approach. Clarendon Press, Oxford, Unied Kingdom, [14] P. Soueres,. Cadena, and M. Djeddou. Dynamical sequence of muli-sensor based asks for mobile robos navigaion. 7h Symp. on Robo Conrol (SYROCO 3), 2: , Sepember 22. [15] O. Tahri and F. Chaumee. Image momen: generic descripors for decoupled image-based visual servo. IEEE In. Conf. on Roboics and Auomaion (ICRA 4), April 24.