Adaptive Visual Servoing of Micro Aerial Vehicle with Switched System Model for Obstacle Avoidance

Transcription

1 3 IEEE Ieraioal Coferece o Syses Ma ad Cyereics Adapive Visual Servoig of Micro Aerial Vehicle wih Swiched Syse Model for Osacle Avoidace Cheg-Mig Huag Depare of Elecrical Egieerig Naioal aipei Uiversiy of echology aipei aiwa R.O.C. Mig-Li Chiag Depare of Elecrical Egieerig Naioal aiwa Uiversiy aipei aiwa R.O.C. Li-Che Fu Depares of Elecrical Egieerig Naioal aiwa Uiversiy aipei aiwa R.O.C. Asrac I his paper a visio ased adapive coroller is proposed for he osacle avoidace of a icro aerial vehicle (MAV) y usig he opical flow iforaio. I order o eploy he opical flow for idicaig he disace ewee he MAV ad surroudig he uli-hread processig algorih is proposed o relialy oai he opical flow iforaio uiforly diffused i he whole iage frae. he MAV syse is reaed as a swiched syse while aipulaig he differe odes durig he osacle avoidace ask. Based o he desired flyig direcio of osacle avoidace oaied y he opical flow esiaio we desig a adapive coroller such ha he desired raecory ca e racked uder differe swichig odes. he siulaios prese he rackig respose of he adapive coroller of a swiched syse ad he experies of he overall syse validae he collisio-avoidace perforace of he MAV. Keywords-Visual servoig; MAV; swiched syses;opical flow. I. INRODUCION I rece years a growig ieres has ee show i 3D rooics. For 3D rooics a wide area of his research is dedicaed o aerial plafors. he icro aerial vehicle (MAV) is oe kid of he uaed air vehicle which could e desiged o auooously fly i dagerous siuaios such as he scee of a fire cheical or uclear disaser area ad alefield ec. I order o avoid osacles durig flyig he disace could e esiaed y he ragig sesors such as he soar ifrared ad laser o oserve he surroudigs of he MAV. However hese ragig sesors are geerally oo expesive or oo large o add oo he MAV. he pihole caera wih low cos ad sall size is cooly applied o he MAV. Usig he sereo caera o he MAV [4 5] ca cosruc he deph iforaio aroud he caeras for osacle avoidace. he disace esiaio wih a sigle caera geerally uilizes he opical flow [6] which easures he oec displacee ewee he iages whe here is a relaive oio ewee he caera ad oecs. he opical flow iforaio also ca e uilized o esiae he oio of he caera or uild he ap of he eviroe [4]. Several researches develop he parallel copuig algorih o he graphic processig ui (GPU) [] o ge he opical flow rapidly. Here he opical flow iforaio i fro of he MAV which would geerae he ipu coad for coroller desig laer is fas ad relialy exraced wih parallel processig o he CPU copuig plafor. I addiio ay papers recely discuss he corol proles of MAV usig differe odels ad approaches. I [7] he auhors propose a oliear corol approach for he MAV wih ucerai paraeers. A deail discussio of odelig ad corol of he cosidered MAV is proposed i [8]. I [] ad [] rous adapive corollers are proposed for he MAV. I [3] a visio ased adapive coroller is discussed where he auhors cosidered oh he iage dyaics ad he vehicle dyaics o desig he coroller. Mos of he exisig resuls use sae feedack ad assue ha all he sae easurees are availale fro he sesors. As iroduced i previous his is expesive ad soeies ay o e availale. I his paper we odel he syse as a lack ox wih swiched rasfer fucios ad use he easuree for he caera oly. More specifically we cosider oupu feedack corol of a swiched liear syse usig he iforaio fro a sigle caera which is pracical o he MAV syse. I his paper he osacle avoidace of MAV oued wih a fro caera is accoplished y a adapive coroller ased o he opical flow. Firs he Kaade-Lucas-oasi (KL) [] feaure poi racker is applied o yield he opical flow iforaio. A uli-hread processig schee is also applied here o oai he opical flow relialy. Sice he flyig direcio for he origial ask of MAV is o desired o e chaged durig he osacle avoidace he MAV flyig sraegy is desiged y perforig he forward ad laeral acios wihou he yaw oio. hus he dyaics of he MAV is odeled as swiched syses. he ai coriuio of his paper is ha we uilize he iforaio oaied fro visio sesor o desig a rous coroller ased o he oupu of he ukow rasfer fucio which ay have swichig paraeers. he syse is regarded as a lack ox ad he corol of MAV is odeled as a swiched syse durig differe operaio odes. he orgaizaio of his paper is as follows. Secio II iproves he radiioal KL feaure poi racker o yield ore sufficie ad reliale opical flow iforaio. he collisio-avoidace flyig sraegy ased o he opical flow ad he cosidered prole are he descried i Secio III. he adapive corol ased visual servoig of he MAV which is operaed as a swiched syse is also provided here. Soe his work was suppored i par y he Naioal Sciece Coucil aiwa uder he gras: NSC --E ad -8-E--9-MY /3 $3. 3 IEEE DOI.9/SMC

2 siulaios ad he experieal resuls are preseed i Secio IV. Fially he coclusio is give i Secio V. II. OPICAL FLOW ESIMAION Sice he MAV cosidered here is oly equipped wih he caera sesor he feasile flyig pah has o e desiged fro he iage sequece. Here he scee is assued o e saic. he Kaade-Lucas-oasi (KL) feaure poi racker [] is applied o ideify he poi cosisecy ewee fraes. he poi displacee i.e. opical flow iplies he relaive disace ewee he MAV ad oec i he iage fraes. However he coveioal KL racker cao oai auda opical flow i he surroudigs ad is easily isled y disracers. Hece a iproved uli-hread KL racker is proposed i his secio o fas ad cosisely exrac he opical flow wih real-ie perforace. I order o relialy rack he pois across fraes he coveioal KL racker us selecs he sigifica pois such as he Harris corer scale-ivaria feaure rasfor (SIF) o e racked. As show i Fig. (a) he exraced corers would e rously ideified for a log period y he pyraid Lucas-Kaade ehod [3]. Oviously he resulig opical flow is oly resriced i he exured regio. he opical flow will o e origiaed fro he oecs wih uifor appearace. Cosiderig he visual flyig avigaio here is o coplee iforaio of he relaive disace for akig he decisio of MAV availale flyig space. (a) Harris corer () uifor disriuio Fig.. Copariso of poi disriuios. (a) iiially disriued pois; () rackig resul afer 5 fraes; Fig.. KL poi rackig resul o a lue oard uder caera viraio. Hece he opical flow iforaio is desired o fill i he full iage frae. he pois which would e racked y he KL algorih are uiforly seleced i he iage i he egiig as show i Fig. (). Usig he sraegy of uiforly arragig pois soe pois draw o he fla iage regio coai he iage paches which are siilar o he eighorhood. As illusraed i Fig. he pois o he fla regio ay e easily isled o he visual disracers ad cao edurigly resul i correc opical flow. Due o he lifeie of he idisiguishale poi rackig is shor he pois geeraed fro uifor disriuio have o e re-draw rapidly o iprove he usailiy of he. Figure 3 shows ha he KL algorih could perfor accepale rackig resuls for os of he pois wihi a shor period eve i he exureless regio. Moreover durig he MAV flyig he oserved pois would disappear due o ou he caera field of view. he racked pois ay also e disured ad shifed y he overlappig oecs. Fas re-drawig frequecy ca quickly copesae he pois wih opical flow iforaio for he sparse regio. Frae # Frae # Frae #3 Frae #4 Fig. 3. KL poi rackig wih rapid poi re-drawig o a whie wall while he caera is ovig oward he wall. he racked pois are uiforly redraw every hree fraes. he KL racker wih high poi re-drawig frequecy also preses he drawack ha all of he poi rackig raecories are erased whe he pois are re-draw. A ha oe he MAV has o ases for desigig he visual servoig. I order o repleish he opical flow iforaio he KL racker wih uli-hread processig is proposed here. akig he advaage of he curre CPU uli-core archiecure each hread of KL rackig for oe iage frae could e assiged o oe core of he CPU. he opical flow i he whole caera field of view he ca e exraced coiuously ad relialy as show i Fig. 4(a) wih he real-ie perforace. Sice he oec closer o he caera resuls i he larger displacee i he iage plae he opical flow iforaio oaied here could e uilized o avoid he osacles i fro of he MAV. he regio aroud he opical flow whose legh is saller ha he axiu adissile value will e regarded as he feasile flyig space as preseed i Fig. 4. he ceroid ( xc y c) of he feasile flyig space is also defied o idicae he flyig raecory of he MAV laer. (a) oe iage frae wih opical flow; () feasile flyig space (lue regio); Fig. 4. Feasile flyig space decided fro he legh of opical flow. 45

3 III. VISUAL SERVOING WIH ADAPIVE CONROL FOR SWICHED SYSEMS A. Prole Forulaio Fig. 5. MAV ody coordiae syse. Cosider a MAV of he quadroor ype wih he caera oued for capurig he fro view of he MAV. he caera opical ceer is assued o e he sae as he MAV ceroid ad he caera coordiae is cosise wih he MAV ody coordiae ( x y z ) as illusraed i Fig. 5. I order o aiai he origial flyig direcio of MAV ask he headig agle will o e chaged durig he osacle avoidace. he corol ipus eploy he forward velociy U alog he x axis he laeral velociy V alog he y axis ad he up-addow velociy W alog he z axis. he horizoal iage posiio x c ca e used o decide he MAV laeral oio ad he verical iage posiio y c ca draw he logiudial oio. Here he up-ad-dow velociy is us coaded o aiai he cosa heigh wihou cosiderig he iforaio of y c.give he iage oservaio x c of ad he caera cosa f he age fucio of he desired flyig direcio for osacle avoidace i he laeral plae ca e deoed as c a x Y f X () where X ad Y are he desired forward ad laeral displacee a curre ie respecively. Accordig o he operaio ehaviors ad dyaics of he MAV we assue ha he dyaic odel of he MAV ca e approxiaed as a hird order rasfer fucio wih relaive degree oe. his is ased o he oservaio fro experieal seup wih piecewise sep refereces. he ode of he quadroor ca e separaely odeled as differe rasfer fucios as follows hrough he syse ideificaio where he forward ode F of forward raslaio over he velociy U is G () p s ad he laeral ode F of he raslaio over he velociy V is G () s. p hrough he oservaio execuig he coads of oh odes siulaeously a every ie isa would easily ierfere wih he MAV y is airflow i he idoor eviroe. Hece he flyig odes are desired o e acivaed i urs a every ie isa. Whe he forward ode is acivaed he pla is F ad whe he laeral ode is acivaed he pla is F. Sice he approxiaed odel is o accurae ad ay have uceraiies he MAV pla paraeers are regarded as ukow ad a rous adapive coroller is proposed o corol his swiched plas wih he ucerai paraeers i he differe odes. he overall process of he closed-loop syse is show i Fig. 6. he syse is decoupled o x ad y axis respecively. For x axis whe he syse is i Forward ode is pla will e F. Whe he syse swiches o Laeral ode is dyaics will e F. I is he sae for y axis. Oe ipora poi is ha for x axis he referece raecory X d will e zero whe i Laeral ode (F) ad e cosa i Forward ode (F). Corarily for y axis he referece raecory Y d will e zero whe i Forward ode (F) ad e cosa i Laeral ode (F). he lock C x ad C y are corollers of x ad y axis respecively. Fig. 6. he lock diagra of he closed-loop syse. B. Adapive Corol of a Swiched Liear Syse As show i Fig. 6 we will desig a rous adapive coroller for he MAV which has differe dyaics i differe ode ad is regarded as a swiched syse. For sipliciy i he followig we will discuss he geeral syse i (3) ad i ca e easily applied o he cosidered case i Fig. 6. Cosider he swiched syse odeled y he rasfer fucio Zp () () s y Wp () ( s)[ u] [ u] () R () s p () where () {} is he swichig sigal ad Z () s s s s pi i i i R () s s a s a sa i. pi i i i he oupu y i () ay represe U or V respecively. X or Y ad he ipu u are he rasfer fucios Wpi () s i i () are sricly proper ad paraeers of he are ukow. he corol 45

4 oecive is o desig a adapive coroller such ha he syse oupu ca rack he desired oupu y. Noe ha for he x axis he referece y Xd ad for he y axis he referece y Yd. o desig he rous adapive coroller for he swiched pla () we ade he followig assupios: (A) All he rasfer fucios have he sae order ad he relaive degree is. (A) he firs derivaives of he referece oupu y are kow ad ouded. (A3) All he plas are copleely corollale ad oservale. (A4) he sigs of i are all kow ad he sae for all i P. (A5) All he ukow paraeers lie i a kow copac se. A variale srucure ased ackseppig coroller will e desiged such ha he rackig error will coverge o zero for arirary swichig sigals. Firs he adapive oserver wih K- filers [9] is used o esiae he sae. We rewrie () as ( ) x A xf y u (3) y C x where I A ad ( ) ( a ) a a ( ) ( ) F( y u) I y I ( ) ( ) ( ) Hereafer we will deoe e i he ih coordiae vecor ha is he vecor wih all zero copoes excep he ih copoe which is. Le k [ k... k ] such ha A A ke is Hurwiz ad hece here exiss a posiive defiie arix P P which saisfies A PPA I. (4) he K-filers o desig he adapive oserver is as follows: A ky (5) A F( yu ) Le he virual sae esiaor e xˆ (6) ad i is clear ha he esiaio error x xˆ will vaish expoeially wih A. Accordig o he srucure of F( y u ) we ca deoe [ v... v ] where v R... are colu vecors ha ca e oaied. fro a filer of ipu u ad R ca e oaied fro a filer of oupu y. Ipleeaio of he K-filers are suarized as follows [9]: A ey Aeu [ v... v ] v ( A )... [( A)... A ]. (7) ( A ) Now we will desig he adapive ackseppig coroller usig he K-filers ad uig fucio. Sice he relaive degree y (3) we have x x a y u x ye a u. (8) Fro (3) ad (8) we ca represe x x [ v v... v ] (9) as () where he su-idex i he righ had side deoes he secod copoe of he colu vecor v... ad () deoes he secod row of. he x [ v v... v ye ] u. () () v u Le he oupu error e z y y he z v u y. () Desig he corol u ˆ () where ˆ is he esiaio of /( ) ad cz dz v ˆ y (3) a where va [ v... v ]. o copesae he effecs caused y he swichig paraeers he coroller paraeers are decided y he followig variale srucure adapive laws: ˆ i sg( z) i i i (4) ˆ sg( )sg( z ) / for i.... he coroller is desiged ased o he Lyapuov aalysis as follows. Cosider he Lyapuov fucio cadidae ) V ( z P. (5) d he he ie derivaive of V is V ( ) ( ˆ c z v v ) a z z 4d (6) d( z) z 4d 453

5 c ( z ) ( v v ˆ ) z z a 4d z ( i izi i iz c ( ) v v ) ( z ) 4d c( z ) 4d Here ˆ ˆ ˆ i addiio we use (5) ad he equaliy z v y (7) i he derivaios. Fro aove aalysis we have achieve ha z y y ad covergece o zero expoeially which also iplies he oudedess of y x. Now we proceed o show he oudedess of he reaiig saes x... x y he defiiios of he filers. Fro (3) ad oudedess of z we kow ha he corol ipu u is ouded. he sice A is sale we kow ha is ouded which i ur iplies ha v... are ouded. Moreover is also ouded sice A is sale ad y is ouded. he y () x is ouded. Siilarly x 3... x are ouded sice is ouded. Accordig o aove aalysis he closed-loop syse is sale ad he oupu ca rack he referece oupu asypoically. Reark. Noe ha for he geeral relaive degree case of adapive corol wih uig fucio approach he iiu phase assupio is eeded [9]. I his paper sice we oly cosider he relaive degree oe case his assupio is o eeded as proved i previous discussios. IV. SIMULAIONS AND EXPERIMENAL RESULS A. Siulaios For he proposed coroller we perfor he followig siulaios o validae he resuls i Secio III. o show he rousess of he coroller o he swichig ode he x axis referece X d he x axis raecory ad he pla ode swich every oe secod as show i Fig. 7. he rasfer fucios which is oaied fro experie is as follows:.377s 3.3S GP() s ( F) 3 S 6.537S.77S S.9S.7 GP () s ( F) 3 S 5.975S 8.97S.47 We ca see ha he oupu racks he referece asypoically o aer how he pla ode swiches. However here are soe rasies due o he swichig of he referece raecory. We wa o ephasize ha he rasie is o due o pla swichig u is caused y he referece raecory. he respose of y is siilar. Moreover i he corol law () we use y for he cosidered case sice he o-differeiale pois are easure zero. Swichig ode (F) ie (sec) ie (sec) Fig. 7. rackig perforace of x axis. B. Expeies he visual osacle deecio eploys he opical esiaio eioed i secio which ca e ipleeed y he OpeCV iage lirary o he Iel 3.3GHz CPU wih four cores. he uli-hread processig of opical esiaio akes four hreads ad each hread re-draws he pois every four fraes y urs. he MAV uilizes he Parro AR.Droe. o es he proposed visual servoig algorihs. he overall process of he visual deecio ad servoig ca e accoplished wih real-ie perforace. he lue circles i Fig. 8 dicae he desired flyig direcio decided fro he opical flow iforaio a each frae ad he correspodig MAV flyig sapshos capured fro he hird view are preseed i Fig. 9. he origial flyig direcio of he MAV is se o pass hrough he idoor corridor. Durig he flyig he MAV firs faces he oxes i he iddle he corridor. he desired flyig direcio is he adused lef ad he MAV acivaes he forward ode ad laeral ode y urs o cross he oxes. However his avoidace leads he MAV oward he wall a frae #56. Sice he wall resuls i he larger opical flow whe he MAV ovig forward he feasile flyig direcio is revised o righ a frae #585. he MAV also ries o avoid he ale i he followig fraes of frae #6. By uilizig he visual collisio-avoidace sraegy ad he adapive visual servoig of he swiched flyig odes proposed i secio ad 3 respecively he MAV ca successfully cross he osacles ad aiai he flyig direcio o pass hrough he corridor a las. V. CONCLUSION his paper preses a visual collisio-avoidace syse for MAV flyig which icludes he visual osacle deecio ad visual flyig corol. he osacle deecio eploys he X d X 454

6 frae #463 frae #53 frae #57 frae #54 frae #56 frae #575 frae #585 frae #6 frae #646 frae #68 frae #779 frae #87 Fig. 8. Osacle avoidace capured fro he caera view. frae #463 frae #53 frae #57 frae #54 frae #56 frae #575 frae #585 frae #6 frae #646 frae #68 frae #779 frae #87 Fig. 9. Osacle avoidace capured fro he hird view. he su-widow i each frae is he correspodig view fro he caera o MAV. opical flow o decide he ifeasile flyig space. I order o oai eough opical flow iforaio fro he KL feaure poi rackig he feaure pois are desired o e uiforly diffused i he whole iage frae. he uli-hread processig of KL feaure poi rackig wih he fas poi re-drawig y urs is developed o exrac reliale opical flow. Based o he opical flow iforaio he desired flyig direcio wih he forward ad laeral displacees of MAV is seleced o avoid he osacle. Moreover a adapive visual servoig corol for MAV is proposed i his paper. For he osacle avoidace ask he MAV is odeled as a swiched syse ad he he rous adapive coroller is desiged o achieve he oecive. Whereas os of he exisig resulig assue sae feedack ad eploy ay sesors we use a sigle caera as a sesor ad he proposed oupu feedack corol syse uses his easuree oly. REFERENCES [] J. Shi ad C. oasi Good feaures o rack IEEE Cof. Copuer Visio ad Paer Recogiio pp [] M. Adriluka S. Roh B. Schiele Moocular 3D pose esiaio ad rackig y deecio i Proc. IEEE Cof. Copuer Visio ad Paer Recogiio vol. pp [3] J. Y. Bougue Pyraidal ipleeaio of he Lucas Kaade feaure racker descripio of he algorih Iel Corporaio Microprocessor Research Las. [4] F. Fraudorfer L. Heg D. Hoegger G. H. Lee P. askae M. Pollefeys Visio-ased auooous appig ad exploraio usig a quadroor MAV IEEE Ieraioal Coferece o Iellige Roos ad Syses pp [5] H. Yu R. W. Beard ad J. Byre Visio-ased local uli-resoluio appig ad pah plaig for iiaure air vehicles Aerica Corol Coferece pp [6] S. Zigg D. Scarauzza S. Weiss ad R. Siegwar MAV avigaio hrough idoor corridors usig opical flow IEEE Ieraioal Coferece o Rooics ad Auoaio (ICRA) pp [7] C. Diao B. Xia X. Gu B. Zhao ad J. Guo Noliear Corol for a uderacuaed quadroor uaed aerial vehicle wih paraeric uceraiies Proc. 3 s Chiese Corol Coferece pp [8] M. Schreier Modelig ad adapive corol of a Quadroor IEEE Ieraioal Coferece o Mechroics ad Auoaio (ICRA) pp [9] M. Krsic I. kaellakopoulos ad P. Kokoovic Noliear ad Adapive Corol Desig Joh Wiley & Sos 995. [] K. Pauwels M. oasi J. Diaz Aloso E. Ros M. M. Va Hulle A copariso of FPGA ad GPU for real-ie phase-ased opical flow sereo ad local iage feaures IEEE rasacios o Copuers vol. 6 o. 7 pp [] C. Nicol C.J.B. Maca A. Rairez-Serrao Rous adapive corol of a quadroor helicoper vol. o. 6 pp Mecharoics. []. Haela R. Mahoy Iage ased visual servo corol for a class of aerial rooic syses Auoaica 7. [3] D. Lee C. Naara. C. Burg ad D.. M. Dawso Adapive rackig corol of a uderacuaed aerial vehicle Proc. Aerica Corol Coferece. [4] M. Krsic I. kaellakopoulos ad P. Kokoovic Noliear ad Adapive Corol Desig Joh Wiley & Sos