On-vehicle visual tracker with disturbance rejection

ON-VEHICLE VISUAL TRACKER WITH DISTURBANCE REJECTION F. G´omez-Es e n ∗F.R. Rubio ∗J. A acil ∗ ∗Dep . Ingenie ´ıa de Sis emas y Au om´a ica Escuela Supe io de Ingenie os. Uni e sidad de Se illa Camino de los Descub imien os s/n. 41092-Se illa. SPAIN e-mails: gs@ca uja.us.es,{ ubio, a acil}@esi.us.es TEL: +34 954487346 FAX: +34 954487340 Abs ac : This pape p esen s he syn hesis and implemen a ion o a passi i y based con olle designed o isual acking o mo ing objec s in a dis u bance ejec ion scheme. The pu pose o his applica ion is o con ol he line o sigh o a 2-DOF senso pla o m in a mo ing coo dina e ame, when he mo emen o he ehicle ca ying he pla o m ac s as a dis u bance o fini e ene gy. Ex e nal o ques due o ehicle mo emen downg ade he isual acking pe o mance and can cause ins abili y and o al loss o sigh o he objec i e. These dis u bances can be compensa ed o in an effec i ely manne in oducing a classical L2-gain dis u bance ejec ion scheme, widely deployed in ine ial ame obo ic sys ems. The p oposed con olle has been es ed in a eal 2-DOF pla o m moun ed on a mo ing es bench ha emula es diffe en ypes o non-ine ial ehicle su aces. Keywo ds: Nonlinea sys ems, Visual acking, Mobile obo ics, Passi i y–based con ol 1. INTRODUCTION In ecen wo ks, he heo ies o passi e and po Hamil- onian sys ems ha e ui ully gi en ise o a se o ools ha succes ully ackle he dis u bance supp es- sion p oblem in elec omechanical sys ems. In (Van de Scha , 1989) he p ope ies o po -con olled Hamil o- nian sys ems (PCH) ha e been s a ed (see also mo e spe- cific app oaches (Wang and Li, In e nal epo 2002) o mul imachine powe sys ems and (Slo ine and Li, 1988) o obo ic sys ems). These ools p o ide a bounded- dis u bance bounded-ou pu con ol in he sense o he L2gain. On he o he hand, on- ehicle senso s like sa elli e an en- nas and isual acking sys ems a e inhe en ly affec ed by undesi ed pe u ba ions due o mo emen s in he coo dina e ame whe e hey li e. These pe u ba ions appea as Co iolis and cen i ugal o ces ypical o non- ine ial ames. Consequen ly, he p oblem o keeping a s a ic line o sigh in his ype o senso is a dis u bance ejec ion p oblem, whe e he dis u bance can be caused by ehicle mo emen s o by displacemen s o he a ge poin (e.g. non-geos a iona y sa elli e). When acking objec i es a e a , he o me case is he one ha mo e se e ely affec s he s abili y o he line o sigh and hese will be he ones s udied in his wo k. In essels, he a o emen ioned dis u bance is mainly due o sea wa es and a e usually app oxima ed wi h sinusoidal unc ions. This ype o signals, when ampli udes a e low, a e well handled in he L2dis u bance a enua ion amewo k. We will s udy a 2-DOF pla o m designed o essel guidance and hence pe u bed by wa e-induced mo e- men s o he ship su ace. Wi h a labo a o y se up ha physically emula es diffe en ypes o pe u ba ion, we will p esen and analyze a isual se oing sys em based on he Slo ine and Li con olle (Slo ine and Li, 1988) ha gua an ees L2dis u bance ejec ion p ope ies. IAV2004 - PREPRINTS 5 h IFAC/EURON Symposium on In elligen Au onomous Vehicles Ins i u o Supe io Técnico, Lisboa, Po ugal July 5-7, 2004 316 Wi h he aim o s udying he p ope ies and effec s o on- ehicle pe u ba ions, a wo deg ee o eedom pla o m has been se up on a des abilizing 2-DOF desk. A isual se oing sys em has been implemen ed wi h a CCD came a. When analyzing expe imen ally he effec o he eal dis u bances on he isual acking loop, a subs an ial imp o emen o he o e all beha io has been achie ed wi h he p oposed con olle . These esul s a e p esen ed g aphically. This pape is o ganized as ollows: in Sec ion 2, he simplified sys em model o he pla o m is gi en. In Sec ion 3, he new con ol s uc u e is p esen ed, di ided in isual es ima o and ajec o y acke . In Sec ion 4, echnical de ails o he physical se up and con olle implemen a ion a e gi en. Sec ion 5 p esen s some ex- pe imen al esul s. Finally, a se o conclusi e ema ks a e gi en in Sec ion 6. 2. SYSTEM MODEL The sys em consis s o a pla o m wi h wo deg ees o eedom imme sed in a non ine ial e e ence sys- em emula ed by a des abilizing desk. The syn hesis o con olle s o his pla o m aises impo an challenges like he obus s abiliza ion ejec ing dis u bances, isual acking o mo ing objec s and gy oscopic s abiliza ion o compensa e o he coo dina e ame mo emen s. The model used (G´omez-Es e n e al., 2000) co esponds o he 2-DOF pla o m shown in Fig.1. I is composed o wo main bodies: he base, whose posi ion is de e mined by he azimu h angle ϕand he head body whose coo di- na e is he ele a ion angle θ. In ou expe imen al se up, his sys em is moun ed on a mechanical desk capable o u ning in wo axes. The whole sys em has ou deg ees o eedom, bu only he wo local coo dina es ( hose o he acke pla o m) a e a ailable om he buil -in encode s. The e o e only he 2-DOF pla o m kinema ics a e conside ed, while any coupling effec due o he mo ing su ace will be ea ed as a dis u bance (G´omez- Es e n e al., 2003) . The load o he ele a ion axis is concen a ed along he sha , hence he po en ial ene gy is in a ian and he e o e disca ded om he equa ions. The o igin o he azimu h angle is a bi a y. The a iable θis ze o when bo h bodies a e pe pendicula . The gene alized coo dina es o he sys em a e (ϕ,θ). The equa ions o mo ion a e easily ob ained in Hamil o- nian o m. Unde he assump ion s a ed abo e, he o al ene gy o he plan is educed o he kine ic ene gy: 1 2˙ϕ˙ θΓ(θ)0 0Iyy2˙ϕ ˙ θ(1) The e m Γ(θ)s ands o Γ(θ)Izz1+Ixx2sin2(θ)+Izz2cos2(θ)(2) Fig. 1. Two deg ee-o - eedom pla o m. whe e Izz1is he zaxis ine ia o he base, and Ixx2,Iyy2 and Izz2a e he second body (ele a ion) ine ia momen a wi h espec o he x,yand zaxes espec i ely. 3. VISUAL TRACKING IN NON INERTIAL VEHICLES The isual acking p oblem in his pla o m can educed in some cases o he s abiliza ion o equilib ia. Indeed, i he objec o be acked (e.g. s a iona y sa elli e) is fixed o has ela i ely slow mo ion, and he pla o m is ins alled on a s a ic su ace, an equilib ium s abiliza ion con olle wi h slowly a ying se poin will do he job. The se poin is es ima ed by an image p ocessing algo i hm. Howe e i he sys em is imme sed in a as o a ing coo dina e ame, as is he case o a ehicle an enna, he pe o mance o a se poin s abiliza ion con olle wi h a ying e e ence is quickly downg aded e en o slow mo ions, as small line o sigh e o s due o dis u bances a eamplifiedwhen he ackedobjec sa edis an .The la e case is a a mo e challenging acking p oblem. Summa izing, he angles o he line o sigh o he a ge wi h espec o he came a mo e by one o wo causes •Visual acking o mobile objec s om a pla o m on a fixed g ound. This p oblem can be ackled as apa icula case o he ollowing. •Fixed o mobile objec acking om a pla o m moun ed on boa d o a g ound ehicle o a essel. The objec i e is o ake o ze o he angula eloci y ec o o he line- o- a ge when i is e e ed o he came a coo dina e ame. This ec o is ac ually he composi ion o he mo ion o he ehicle (o sea wa es) and he eac ion o he 2-DOF pla o ms in o de o compensa e i . As he second o hese si ua ions is mo e gene al, his will be he one s udied in his pape . 317 Rada Video Encode /gy o Passi i y based ajec o y acke Visual algo i hm o ajec o y compu a ion D( ) x( ),y( ) q( ),q'( ) qd( ),q'd( ) Dis u bance (wa e mo ion) + + u Senso Pla o m Fig. 2. Passi e isual acking scheme. The con ol p oblem will be di ided in wo pa s (G´omez-Es e n, 2003). Fi s , a passi i y-based ajec- o y acking sys em wi h dis u bance supp ession is designed exploi ing de ailed knowledge o he mechanical s uc u e o he pla o m. Then, a isual acking algo- i hm es ima es he de ia ion o he acked objec om he line o sigh and, based on geome ic conside a ions, i gene a es he e e ence ajec o y o be acked by he passi e sys em. The p oposed scheme is illus a ed in Fig. 2. 3.1 Visual acking The design o he fi s block consis o calcula ing he desi ed ajec o y o he pla o m coo dina es, qd( )= (ϕd( ),θ d( )) in such a way ha he image o he mo ing objec s abilizes a he cen e o he sc een. The inpu o his block is a 2D ec o wi h he (x, y)coo dina esin he sc een, e e ed o he o igin a he cen e poin . A simple image scanning algo i hm compu es his ec o a a 10ms a e. We will assume ha he e is a unique ajec o y qd( )∈C 2in he local pla o m coo dina es such ha q( )≡qd( )⇒(x( ),y( )) = (0,0),∀ >0 This ajec o y is ob iously unknown, as i depends on he mo ion o he coo dina e ame o he ehicle and he mobile a ge .Howe e ,aswillbeseen, heknowledge o he p ojec ion o he a ge on he sc een is enough o compu e he desi ed ajec o y qd( ) and an es ima e o he de i a i es a any ins an .Onceqd( )isknown, a passi i y-based con olle will be designed in o de o ensu e asymp o ic s abili y o he ajec o y and, hence o h lim →∞ (x( ),y( )) = (0,0) The sc een coo dina es o he acked objec (x,y)canbe iewed as a measu e o he acking e o , since hese a e ze o i and only i he objec is pe ec ly acked. Howe e i is necessa y o exp ess his e o in e ms o he angles o he pla o m join s, (ϕ, θ). These can be easily ob ained om (x,y)as ollows.Assume(X, Y, Z) a e he 3D coo dina es o he a ge in he came a coo dina e ame (see Fig. 3). By iangle equi alence, we ha e By iangle equi alence, we ha e x d=X Z y d=Y Z whe e dis he dis ance o he plane o he image o he iewe ( his pa ame e can be calib a ed ollowing came a ins uc ions). A e some manipula ions, he 3D coo dina es a e isola ed X=xD d2+x2+y2 Y=yD d2+x2+y2 The azimu h and ele a ion e o s o he came a in e ms o xand ya e gi en by eϕ= a c an X Z=a c anx d=ϕd−ϕ eθ= a c an Y Z=a c any d=θd−θ De i ing his exp ession wi h espec o ime, we ha e ˙eϕ=d d a c an X Z=d d a c an x d =˙xd d2+x2=˙ϕd−˙ϕ ˙eθ=d d a c an Y Z=d d a c an y d =˙yd d2+y2=˙ θd−˙ θ, F om whe e we ob ain he desi ed ajec o y o he pla o m angles a any ins an qd( )=ϕ( ) θ( )+eϕ( ) eθ( )(3) ˙qd( )=˙ϕ( ) ˙ θ( )+˙eϕ( ) ˙eθ( )(4) 318 Y Z d Side iew Uppe iew Z d X y x Image p ojec ion plane Image p ojec ion plane Fig. 3. Sc een and eal coo dina es. The gene alized coo dina es (ϕ( ),θ( )) a e measu ed in eal ime wi h he buil -in encode s o he pla o m independen ly o he Eule angles o he ehicle. This eal ime compu ed a ge ajec o y will be he e e ence o he passi i i y-based ajec o y acking con olle whose design will be illus a ed in he ollowing. 3.2 T ajec o y acking subsys em In o de o ge he ajec o y acking we will s a om he Eule –Lag ange equa ions, and use he celeb a ed esul s o Slo ine and Li (Slo ine and Li, 1988). Gi en he nolinea ine ia ma ix o he pla o m M=Γ(θ)0 0Iyy2(5) we ob ain he dynamic equa ions in open loop using he Eule -Lag ange desc ip ion o obo ic sys ems M(q)¨q+C(q, ˙q)˙q=τ, (6) whe e no po en ial ene gy e ms a e being conside ed be- cause he head o he pla o m is mechanically balanced by cons uc ion (in o de o educe ene gy demand). I hey we e nonze o he ea men would be equi alen ex- cep in he ac ha an inclinome e would be necessa y o measu e he eal angles o he pla o m wi h espec o an ine ial ame, in o de o p ope ly measu e and compensa e g a i y effec s. In o de o asymp o ically ack he e e ence ajec o y qd( ) we will apply he con ol law τ=M(q)˙ ξ+C(q, ˙q)ξ+ν(7) whe e ξ=˙qd−Λ(q−qd)(8) and Λ = ΛT>0. Subs i u ing in (7) yields M(q)˙s+C(q, ˙q)s=ν(9) whe e s˙q−ζ. Defining he ene gy unc ion as H(s, q)=1 2sTM(q)s(10) hen, along he ajec o ies (9) d d H=sTM(q)˙s+1 2sT˙ Ms =−sTCs +1 2sT˙ Ms+sTν=sTν(11) The las exp ession is due o he skew-symme y o he ma ix ( ˙ M−2C). The sys em is dissipa i e wi h espec o (sTν) and ep esen s a passi e ν→ sbecause H≥0, o any ini ial condi ion. I , addi ionally, we define ˆν=Ks (12) whe e K=KT>0 is a posi i e defini e ma ix ha Fig. 4. Con ol s uc u e o ajec o y acking appea s in he eedback block o Fig.4. Then we ha e ha P oposi ion 1. The con ol law 319 τ=M(q)[¨qd−Λ( ˙q−˙qd)] +C(q, ˙q)( ˙qd−Λ(q−qd)) +Kd[˙q−˙qd+Λ(q−qd)] whe e Kd=KT d>0andΛ=Λ T>0 a e uning pa am- e e s, and qd( ) is he compu ed ajec o y ha mus be acked, asymp o ically s abilizes he e o e( )=q( )− qd( )in heo igin Fo mo e de ails on he p oo we e e he eade o (Van de Scha , 1989). 3.3 Dis u bance ejec ion in ajec o y acking Thep oposedschemeisbasedon hewellknownSlo- ine and Li con olle (Slo ine and Li, 1988), which in p inciple lacks o quan ifiable dis u bance supp ession p ope ies when he posi ion e o o he obo is aken as he ou pu o he sys em. A known ac om he li e a u e (see (Sadegh and Ho owi h, 1990)) is ha he addi ion o a e m p opo ional o he posi ion e o o he Slo ine and Li con olle p oduces a dis u bance supp ession beha oi in he L2-gain sense om he inpu o he ou pu e( )=qd( )−q( ) wi h a a enua ion le el γa bi a ily small in he case o ully ac ua ed sys ems. Hence he p e ious p oposi ion is ans o med in o P oposi ion 2. Conside he 2-DOF pla o m wi h he con ol law τ=M(q)[¨qd−Λ( ˙q−˙qd)] +C(q, ˙q)( ˙qd−Λ(q−qd)) +Kd[˙q−˙qd+Λ(q−qd)] + Kp(q−qd) wi h he same cons an s defined in p oposi ion 1, and suppose, addi ionally, ha he ollowing condi ions hold: Kd=1 21 γ2+1+λ Kp=1 2γ2+1+λ. Then, he L2-gain om he inpu τ o he ou pu (qd−q) is less o equal o he a bi a ily defined gain γ,and he ansien beha io can be adjus ed using he posi i e scala λ. The p oo o his esul is based in ha o (Sadegh and Ho owi h, 1990). We will use i o i s implemen a ion in he eal 2-DOF pla o m inside he isual eedback loop. Bu fi s ly we will desc ibe, in he ollowing sec ion, how he in o ma ion is con eyed among he diffe en con olle blocks. 4. IMPLEMENTATION Figu e 5 shows he expe imen al se up. The pla o m u ns in wo axis powe ed by wo b ushless DC mo o s. The des abilizing desk is also 2-DOF and is powe ed by h ee-phase mo o s con olled wi h a eloci y a ia o . Themeasu emen so heposi ionsenso s(diffe en ial encode s o he desk and inclinome e s o he desk) and eloci ies (gy oscopes) a e ed back in o a PC ha con ains wo dSpaceTM DSP ca ds (one o each axis) based on he DSP TMS320C31 p ocesso and equipped wi h a con ol de elopmen en i onmen Con olDesk. Fig. 5. Expe imen al se up. Fo he isual acke we ha e de eloped and simple al- go i hm o image scanning unning on a spa e PC. Such algo i hm is a C++ ou ine ha de e mines he sc een coo dina es o he acking a ge (x,y), in ou expe i- men jus a black ci cle p in ed on a whi e backg ound. This image is fi s used o calib a e he pa ame e s o he came a. The acqui ed da a is packed in o an UDP da ag am wi h a ime s amp and sen ia E he ne o he pee compu e , whe e he dSpace ca ds a e unning he acking con olle in connec ion wi h he pla o m senso s and ac ua o s. In o de o check he alidi y o he p oposed scheme, i has been obse ed ha he o al ime in acqui ing an image, iden i ying he angles o he a ge , and sending he da ag am, does no exceed a o al ime o 20ms. Thus, wi h a sample pe iod o 100ms he ji e due o unmodeled delays is no la ge han 20% o he sampling ime. The PC ac ing as con olle ecei es he UDP packe s (no packe losses ha e been accoun ed o ), finds ou he e o angles om he ecei ed (x,y) coo dina es, and con eys he da a o he DSP ca ds, whe e he alue o qdis compu ed acco ding o encode measu emen s and equa ion (3). In hese p ocesso s, a SimulinkTM 320 RTW 1algo i hm applies he ol age he o que o he DC se omo o o he co esponding axis . The physical cons an s o he model ha e been ob ained h ough leas squa es iden ifica ion o e se e al eco ds on he s ep esponse cap u ed by he DSP boa ds. As ic ion will be o some ex en compensa ed o in he final con olle . he LuG e model pa ame e s ha e been iden ified as indica ed in (As om K.J., 1995). 5. EXPERIMENTAL RESULTS The quali y o he expe imen al esul s significan ly in- c eases when ou con olle is enhanced wi h a eed o - wa d compensa o o he ic ion phenomena based on he LuG e model. Theelemen sdepic edin(6)a e heslowsinusoidal e e ence p o ided by he isual acke when he des a- bilizing desk oscilla es in sinusoidal slow mo ion (ma ine ehicle emula ion), oge he wi h he posi ion gi en by he encode s. The acking con olle is also a ificially pe u bedwi ha as sinusoidalsignal(includedin he figu e) di ec ly added o he DC mo o inpu s. The small de ia ions be ween he posi ion and he e e ence a e due o his dis u bance, which is he one o be a en- ua ed wi h he obus scheme. The L2-gaino he as dis u bance- o-ou pu map has been adjus ed acco ding o p oposi ion 2. Al hough he alue o γcan be made a bi a ily small, in expe imen s we ha e obse ed ha i should no be chosen below a ce ain alue due o he appea ance o a sus ained oscilla ion, possibly due o communica ion delays inducing a limi cycle. As a ma e o ac any unmodeled oscilla ion is amplified h ough he de i a i e e m o he con olle , which is in u n inc eased as pa ame e γgoes smalle . Fig. 6. Visual acking o a fixed a ge wi h sinusoidal desk mo ion and addi i e whi e noise dis u bance. 6. CONCLUSIONS In his wo k we ha e p esen ed he design and p ac i- calimplemen a iono acon olle basedon heSlo ine 1Real Time Wo kshopTM. and Li scheme imme sed in a isual eedback loop wi h E he ne da a ansmission. The con olle is spli be- ween image p ocessing, a ge ajec o y compu a ion and ajec o y acking. The la e block is a passi e con olle specifically designed o supp ession o unde- si ed dis u bances up o a le el ha , in heo y, can be made a bi a ily small. The main con ibu ion lies in he use o passi i y and obus ness echniques in a isual se oing amewo k. In he expe imen al esul s we ha e checked ha well known con ol echniques designed o o he domains ha e p o ed o be effec i e in his challenging ype o applica ion. The ex ension o he echniques o isual se oing sys ems is na u al and only elies on he com- pu a ion o a ajec o y o be acked as a esul o a as image analysis C++ algo i hm. The powe ul expe imen al se up consis ing o a 2-DOF senso pla o m moun ed on a 2-DOF des abilizing desk has allowed us o e i y expe imen al esul s in a mos ealis ic en i onmen , and his makes us belie e ha he con olle can be easily upg aded in o he eal g ound and wa e ehicle wo ld. Acknowledgemen s. This wo k has been pa ially unded by he Spanish Minis y o Science and Tech- nology unde g an s DPI2001-2424, DPI2003-00429 and HF2001-0126. REFERENCES As om K.J., Canudas de Wi C., Olsson H. Lischin- scky P. (1995). A new model o con ol on sys ems wi h ic ion. IEEE Tansac ions o Au oma ic Con- ol 40(3), 419–425. G´omez-Es e n, F. (2003). PhD Thesis: Con ol de Sis- emas no Lineales basado en la Es uc u a Hamil o- niana.. Dep . Ing de Sis emas y Au om´a ica. Uni- e sidad de Se illa. G´omez-Es e n, F., J. A acil and F.R. Rubio (2000). P ac ical implemen a ion o a hamil onian–based 2– DOF pla o m. In: P oc. IFAC Symposium on Robo Con ol. Vienna, AU. G´omez-Es e n, F., M.G. Co dones and F.R. Rubio (2003). Seguimien o isual basado en pasi idad en sis emas no ine ciales. In: P oc. o he XXIV Jo - nadas de Au om´a ica. Leon, Spain. Sadegh, N. and R. Ho owi h (1990). S abili y and o- bus ness analysis o a class o adap i e con olle s o obo manipula o s. In . Jou nal. ob. Res. 9, 74–92. Slo ine, J.J. and W. Li (1988). Adap i e manipula o con ol: a case s udy. IEEE T ans. on Au oma ic Con ol 13(5), 995–1003. Van de Scha , A.J. (1989). L2-Gain and Passi i y Techniques in Nonlinea Con ol. Sp inge -Ve lag. Wang, Y., Cheng D. and Ge Y. Li, C. (In e nal e- po 2002). Dissipa i e hamil onian ealiza ion and ene gy-based l2-dis u bance a enua ion con ol o mul imachine powe sys ems. 321