Seguimiento visual mediante esquema por par calculado. Aplicación a plataforma de 2-GDL

SEGUIMIENTO VISUAL BASADO EN PASIVIDAD EN SISTEMAS NO INERCIALES Fabio G´ omez-Es e n, Miguel G. Co dones and F.R. Rubio 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,co dones, ubio}@ca uja.us.es TEL: +34 954487346 FAX: +34 954487340 Abs ac En es e abajo se p esen a la s´ ın esis e implemen aci´ on de un con olado basado en pasi idad des inado al seguimien o de isual de obje os m´ o iles den o de un es- quema de echazo de pe u baciones. El p op´ osi o de la apli- caci´ on es con ola la l´ ınea de isi´ on de una pla a o ma de senso es de dos g ados de libe ad asen ada sob e un sis- ema de e e encia m´ o il, cuando el mo imien o del eh´ ıculo e es e o acu´ a ico que sopo a la pla a o ma ac ´ ua como un e ec o pe u bado de ene g´ ıa limi ada. Los pa es ex e - nos debidos al mo imien o del sis ema de e e encia de la base deses abilizan la imagen y pueden causa una p´ e dida del obje i o isual. Dichas pe u baciones se pueden com- pensa de mane a e ec i a inco po ando un esquema cl´ asico de echazo de pe u baciones con ganancia L2basado en pasi idad, inicialmen e dise˜ nado pa a sis emas ine ciales. La p incipal apo aci´ on de es e abajo consis e en hace - los ex ensi os a sis emas no ine ciales con ealimen aci´ on isual. Los con olado es dise˜ nados han sido p obados en una pla a o ma de labo a o io ins alada sob e una mesa de- ses abilizado a que emula el oleaje o el mo imien o de un eh´ ıculo e es e, in oduciendo el e ec o pe u bado de mane a ealis a. Palab as cla e: Sis emas no lineales, Robo s m´ o iles, Pasi idad, Con ol obus o, Seguimien o isual. 1 In oducci´ on En ecien es abajos, la eo ´ ıa de los sis emas hamil onianos y la pasi idad han pe mi ido p o undiza en el a amien o de la elaci´ on pe u baciones-salida de los sis emas ealimen a- dos. En [4] se han demos ado las p opiedades de echazo de pe u baciones pa a sis emas PCH ( ´ ease ambi´ en un en- oque ecien e m´ as p ´ ac ico en [5]) al impone unas de e mi- nadas condiciones en la ma iz de disipaci´ on de un sis ema hamil oniano, de modo que en bucle ce ado p esen e una ganancia L2 educida, el algunos casos cuan i icable, en la elaci´ on pe u baci´ on-salida. Los senso es de eh´ ıculos o obo s m´ o iles como an enas sa ´ eli e o sis emas de seguimien o isual, se en a ec ados de mane a con inua po pe u baciones no deseadas debidas al mo imien o sis ema de e e encia sob e el que se asien- an. Las pe u baciones se e lejan como ue zas de co iolis y cen ´ ı ugas p opias de los sis emas no ine ciales. Es os sis emas encuen an la di icul ad de man ene una l´ ınea de isi´ on es able en e a los mo imien os del obje i o isual o los del p opio sis ema de e e encia del senso . Pa a obje- i os alejados la segunda de las pe u baciones es la que m´ as e ec o iene en el seguimien o y equie e un a amien o espe- cial. En el caso de eh´ ıculos acu´ a icos es as pe u baciones ienen ca ´ ac e sinusoidal y po an o es de espe a que su ene g´ ıa es ´ e aco ada, hecho impo an e pa a la e ec i idad del esquema de echazo de pe u baciones L2. Mo i ado po ello, en es e a ´ ıculo se p opone el empleo de un con olado de Slo ine y Li con echazo de pe u baciones en sen ido L2 basado en pasi idad, inme so en un sis ema de seguimien o isual que p opo ciona la ayec o ia de e e encia a dicho con olado . A in de es udia de o ma ealis a las ca ac e ´ ıs icas y el e ec o de las pe u baciones, se ha cons uido una pla a o ma de labo a o io de dos g ados de libe ad sob e una mesa deses abilizado a de dos g ados de libe ad y se ha imple- men ado un sis ema de seguimien o isual median e c´ ama a CCD. Al analiza el e ec o de las pe u baciones de la mesa deses abilizado a sob e la calidad del seguimien o isual, se ha obse ado expe imen almen e una sus ancial mejo a al in- co po a el nue o esquema de echazo de pe u baciones al sis ema de con ol. Es os esul ados se ilus an de o ma g ´ a ica. El p esen e a ´ ıculo se o ganiza del siguien e modo: En la secci´ on 2, se p esen a el modelo simpli icado de la pla a o ma. En la secci´ on 3, se p opone la es uc u a de con- ol deseada, di idida en subsis ema isual y subsis ema de con ol. En la secci´ on 4 se ilus a la implemen aci´ on de lab- o a o io de los con olado es. La secci´ on 5 p esen a algunos esul ados expe imen ales. Finalmen e se apo an una se ie de conclusiones en el apa ado 6. 2 Modelo del sis ema El sis ema consis e en una pla a o ma de dos g ados de lib- e ad inme sa en un sis ema de e e encia no ine cial. La s´ ın esis de con olado es pa a es a pla a o ma p esen a e os como la es abilizaci´ on obus a con echazo de pe u baciones y el seguimien o isual de obje os m´ o iles. El modelo empleado co esponde a la pla a o ma de dos g a- dos de libe ad mos ada en la igu a 1. Se compone de dos cue pos p incipales: la base, cuya posici´ on es ´ a de e minada po el ´ angulo de acimu ϕy el cue po p incipal que con iene la c´ ama a y cuya coo denada gene alizada es el ´ angulo de ele aci´ on θ. En nues a ins alaci´ on expe imen al, es e sis- ema se asien a en una mesa mecanizada capaz de gi a en dos ejes. El sis ema comple o e e ido a ie a posee cua- o g ados de libe ad, aunque en nues o modelo conside - a emos s´ olo la cinem´ a ica de la pla a o ma de dos g ados de libe ad en un sis ema ijo, ya que el sis ema de e e encia de la mesa se supone desconocido en aplicaciones eales. Po an o cualquie e ec o de acoplamien o debido al mo imien o de es e sis ema se ´ a a ado como pe u baci´ on ex e na. El peso del cue po de ele aci´ on se concen a a lo la go del eje de gi o, po lo que la ene g´ ıa po encial es in a ian e y po an o desapa ece de las ecuaciones. El o igen del ´ angulo de acimu es a bi a io. La a iable θes ce o cuando ambos cue pos son pe pendicula es. Las coo denadas gene alizadas del sis ema son (ϕ,θ). Figu e 1: Pla a o ma de dos g ados de libe ad. Las ecuaciones de mo imien o se ob ienen de mane a sen- cilla en o ma hamil oniana. Bajo la suposici´ on an e io , la ene g´ ıa mec´ anica o al se educe a la ene g´ ıa cin´ e ica: 1 2˙ϕ˙ θΓ(θ) 0 0Iyy2˙ϕ ˙ θ(1) que coincide al mismo iempo con el lag angiano y el hamil- oniano del sis ema. El ´ e mino Γ(θ) ep esen a la siguien e exp esi´ on: Γ(θ),Izz1+Ixx2sen2(θ) + Izz2cos2(θ)(2) donde Izz1es el momen o de ine cia en o no al eje zde la base, y Ixx2,Iyy2eIzz2lo son espec o a los ejes x,yand z espec i amen e. 3 Seguimien o de obje os m´ o iles en supe - ices no ine ciales El p oblema de con ol de es a pla a o ma puede se a ado en p ime a ap oximaci´ on como una es abilizaci´ on de pun os de equilib io. En e ec o, si el obje i o isual no es m´ o il o su desplazamien o es len o ( .g . sa ´ eli e de comunicaciones no geos aciona io), el e o en posici´ on puede se es imado po la c´ ama a y co egido median e linealizaci´ on po eali- men aci´ on en e o as opciones. Sin emba go si el obje i o de con ol es el seguimien o i- sual de un obje o m´ o il, el p oblema c ece en in e ´ es. El mo imien o del obje i o espec o a la l´ ınea de isi´ on de la c´ ama a puede da se en una o ambas de las siguien es si ua- ciones: •Seguimien o de obje os m´ o iles desde una pla a o ma apoyada en ie a. La soluci´ on a es e p oblema, al se un caso pa icula , es ´ a con enida en el siguien e supues o. •Seguimien o de obje os ijos o m´ o iles desde una pla a o ma apoyada en la cubie a de un buque u o a base m´ o il. En es e caso el obje o a segui end ´ a un ec o de elocidad no nulo con espec o al sis ema de e e encia ligado a la cabeza del pedes al, an o si el ob- je o es ´ a en mo imien o como si no. Es a elocidad se ´ a la composici´ on del mo imien o del p opio obje o con el mo imien o de la pla a o ma debido a las oscilaciones p o ocadas en el buque po el oleaje. Siendo el segundo caso el m´ as gene al, se ´ a el que abo - da emos a con inuaci´ on. Se plan ea ´ a el con ol como un seguimien o de ayec o ias basado en pasi idad con echazo de pe u baciones. Las ayec o ias de e e encia se ´ an aque- llas que hagan nulas las coo denadas del obje o en la imagen cap ada po la c´ ama a y sus de i adas. Pa a ello se inclui ´ a un m´ odulo de p ec´ alculo de la ayec o ia de e e encia en i ud de los mo imien os del obje i o en la imagen de la c´ ama a. El esquema de con ol p opues o apa ece en la igu a 2. 3.1 Seguimien o isual El dise˜ no del p ime bloque consis e en calcula la ayec o- ia deseada pa a las coo denadas de la pla a o ma, qd( ) = (ϕd( ), θd( )) de modo que la imagen del obje o m´ o il se es abilice en el cen o de la pan alla de la imagen cap u ada po la c´ ama a CCD. La en ada de es e bloque es ´ a o mada po la posici´ on 2D del obje o en la imagen, e e ida al cen o Rada Video Encode /gy o Seguimien o de ayec o ia basado en pasi idad Cálculo isual de la ayec o ia de e e encia D( ) x( ),y( ) q( ),q'( ) qd( ),q'd( ) Pe u bación (oleaje) + + u Pla a o ma de senso es Figu e 2: Esquema de con ol pa a seguimien o isual. de la pan alla. Deno a emos es as coo denadas (x( ), y( )). Supongamos que exis e una ayec o ia qd( )∈ C2´ unica en las coo denadas a icula es de la pla a o ma, al que q( ) = qd( )⇒(x( ), y( )) = (0,0),∀ Es a ayec o ia es, e iden emen e desconocida, pues de- pende de los mo imien os del buque y del obje o m´ o il. Sin emba go, como se e ´ a a con inuaci´ on, bas a ´ a con conoce las ayec o ias de las coo denadas del obje o en la imagen cap u ada po la c´ ama a, pa a es ima qd( )y sus de i adas en cada ins an e . Conocidas ´ es as, se dise˜ na ´ a un con o- lado basado en pasi idad pa a log a es abilidad asin ´ o ica de la ayec o ia, y consecuen emen e lim →∞ (x( ), y( )) = (0,0) Es admisible a i ma que las coo denadas en pan alla (x,y), as´ ı como las coo denadas idimensionales en el sis ema de e e encia de la c´ ama a, (X,Y,Z), son medidas del e o en seguimien o. Sin emba go, es necesa io exp esa dicho e o en coo denadas a icula es (ϕ, θ), y as´ ı se ob end ´ a a con in- uaci´ on. La posici´ on 2D del obje i o en la imagen cap u ada se supone es imada po un algo i mo de isi´ on po compu a- do es ´ anda . El c´ alculo de las coo denadas (X, Y, Z)del ob- je o e e idas al sis ema de e e encia de la c´ ama a se de i a de las p opiedades geom´ e icas ilus adas en la igu a 3. Po semejanza de i´ angulos, se iene x d=X Z y d=Y Z siendo dla dis ancia ocal al plano de la imagen, un pa ´ ame o ca ac e ´ ıs ico (y a menudo ajus able) de la c´ ama a. T as algunas manipulaciones se despejan las coo denadas 3D X=xD √d2+ 2 Y=yD √d2+ 2 donde 2=x2+y2. El e o de o ien aci´ on de la c´ ama a en unci´ on de xey iene dado po eϕ= a c an(X Z) = a c an x d=ϕd−ϕ eθ= a c an(Y Z) = a c an y d=θd−θ Si de i amos es a exp esi´ on, se iene ˙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−˙ θ, De donde ob enemos ayec o ia a icula deseada y sus de i adas en el ins an e qd( ) = ϕ( ) θ( )+eϕ( ) eθ( )(3) ˙qd( ) = ˙ϕ( ) ˙ θ( )+˙eϕ( ) ˙eθ( ) (4) Es a ayec o ia obje i o calculada en iempo eal se ´ a la e - e encia del bloque de es abilizaci´ on de ayec o ias que se dise˜ na ´ a a con inuaci´ on con m´ e odos basados en pasi idad. 3.2 Subsis ema de es abilizaci´ on de ayec o ias Pa a log a el seguimien o de ayec o ias pa i emos de las ecuaciones de Eule –Lag ange, y emplea emos los esul a- dos de [4]. Dada la ma iz de ine cia del pedes al M=Γ(θ) 0 0Iyy2(5) Y Z d Vis a la e al Vis a supe io Z d X y x Plano de la Imagen Plano de la Imagen Figu e 3: Coo denadas en pan alla y coo denadas eales. se esc ibe la din´ amica en bucle abie o en la es uc u a de Eule -Lag ange pa a sis emas ob´ o icos M(q)¨q+C(q, ˙q) ˙q=τ. (6) donde los ´ e minos de ene g´ ıa po encial no son conside a- dos en es a aplicaci´ on po azones ya expues as, aunque si exis ie an el a amien o se ´ ıa equi alen e. Pa a es abiliza la ayec o ia de e e encia qd( )aplica emos la ley de con ol τ=M(q)˙ ξ+C(q, ˙q)ξ+ν(7) donde ξ= ˙qd−Λ(q−qd)(8) yΛ=ΛT>0. Sus i uyendo en (7) se ob iene M(q) ˙s+C(q, ˙q)s=ν(9) donde s,˙q−ζ. De iniendo la unci´ on de ene g´ ıa como H(s, q) = 1 2sTM(q)s(10) en onces a lo la go de las ayec o ias (9) d d H=sTM(q) ˙s+1 2sT˙ Ms =−sTCs +1 2sT˙ Ms +sTν=sTν(11) La ´ ul ima exp esi´ on se debe a la an isime ´ ıa de la ma iz (˙ M-2C). El sis ema es disipa i o espec o a (sTν) y un mapa pasi o ν7→ spo que H≥0, po cualquie condici´ on inicial. Si de inimos, adicionalmen e ˆν=Ks (12) Figu e 4: Es uc u a de con ol pa a seguimien o de ayec- o ias donde K=KT>0es una ma iz de inida posi i a que apa ece en la es uc u a de con ol (Fig.4) en la pa e de eal- imen aci´ on. En onces se iene que P oposi ion 3.1. La ley de con ol τ=M(q)[¨qd−Λ( ˙q−˙qd)] +C(q, ˙q)( ˙qd−Λ(q−qd)) +Kd[ ˙q−˙qd+ Λ(q−qd)] donde Kd=KT d>0yΛ=ΛT>0a e son pa ´ ame os de ajus e, y qd( )es la ayec o ia calculada que se ha de segui , es abiliza asin ´ o icamen e el e o e( ) = q( )−qd( )en el o igen La p ueba es conocida as los esul ados de [4], y no se p e- sen a ´ a en es e luga . 3.3 Rechazo de pe u baciones en seguimien o de ayec o ias El esquema p opues o se basa en el celeb ado con olado de Slo ine y Li [2], que en p incipio ca ece de p opiedades de echazo de pe u baciones cuan i icable cuando se oma el e o en posici´ on del obo como salida del sis ema. Un hecho conocido de la li e a u a ( ´ ease [3]) es que la adici´ on de un ´ e mino p opo cional al e o al con olado de Slo ine y Li p opo ciona un m´ e odo de echazo de pe u baciones en sen ido L2hacia la salida e( ) = q( )−qd( )con un ni el de a enuaci´ on γa bi a iamen e bueno en caso de sis emas com- ple amen e ac uados. La p oposici´ on an e io se ans o ma en P oposi ion 3.2. Consid´ e ese la pla a o ma de dos g ados de libe ad con ley de con ol τ=M(q)[¨qd−Λ( ˙q−˙qd)] +C(q, ˙q)( ˙qd−Λ(q−qd)) +Kd[ ˙q−˙qd+ Λ(q−qd)] + Kp(q−qd) con odas las cons an es de inidas en la p oposici´ on 3.1, y sup´ ongase adicionalmen e que se cumplen las siguien es condiciones: Kd=1 21 γ2+1+λ Kp=1 2γ2+1+λ. En onces, la a enuaci´ on L2desde la en ada a la salida τ la salida (q−qd)es in e io a la ganancia a bi a iamen e de inida γ, y el compo amien o ansi o io puede se ajus- ado usando el escala posi i o λ. La p ueba de es e esul ado puede lee se en [3]. Lo em- plea emos pa a su implemen aci´ on en la pla a o ma eal de dos g ados de libe ad con ealimen aci´ on isual, pa a lo que se desc ibi ´ a la conexi´ on en e el sis ema de a amien o de imagen y la es uc u a aqu´ ı desc i a. 4 Implemen aci´ on En la igu a 5 apa ece el sis ema de expe imen aci´ on de lab- o a o io. La pla a o ma gi a en dos ejes impulsados po mo- o es de co ien e con inua ipo b ushless. La mesa deses abi- lizado a cons a de dos mo o es i ´ asicos con olados con un a iado de elocidad. La medida de los senso es de posici´ on (encode s inc emen ales e inclin´ ome os pa a la mesa) y e- locidad (gi ´ oscopos) se ealimen a al PC que con iene dos a je as (una po eje) de adquisici´ on y con ol dSpace basadas en el DSP TMS320C31 y un en o no de con ol en iempo eal Con olDesk. Pa a la ealimen aci´ on isual se ha desa ollado un algo- i mo sencillo de a amien o de im´ agenes que se ejecu a en un PC dis in o del de con ol de la pla a o ma. Dicho al- go i mo de e mina las coo denadas xeyde un c´ ı culo ne- g o en la imagen cap u ada y las ajus a en unci´ on de los Figu e 5: Pla a o ma expe imen al pa ´ ame os calib ados de la c´ ama a. A con inuaci´ on se cal- culan los dos ´ angulos de la l´ ınea que apun a al obje o, e e i- dos al eje longi udinal de la c´ ama a. Es e eje coincide con el de la pa e supe io de la pla a o ma. Conocidas es as coo denadas, se ansmi en po medio de da ag amas UDP sob e una ed E he ne desconges ionada hacia el o denado de con ol de la pla a o ma. En es a m´ aquina se ha imple- men ado una aplicaci´ on de memo ia compa ida que ecibe los da ag amas y ansmi e los alo es a las a je as DSP. El iempo de mues eo log ado con es e esquema es de 20 milisegundos, sin habe se ap eciados e ec os signi ica i os elacionados con el e aso de comunicaci´ on. La iden i icaci´ on de los pa ´ ame os del modelo ´ ısico de la pla a o ma se han ob enido median e ajus e m´ ınimo cuad ´ a ico sob e los da os cap u ados po el PC. La compen- saci´ on de icci´ on ( e apa ado siguien e) es posible medi- an e la iden i icaci´ on de los coe icien es del modelo de LuG e al y como se indica en [1]. 5 Resul ados expe imen ales La calidad de los esul ados inc emen a conside ablemen e al inco po a a nues o con olado el esquema de p eali- men aci´ on pa a la compensaci´ on de icci´ on basado en el modelo de LuG e p esen ado en [1]. Con el con olado p esen ado, la igu a 6 mues a los e- sul ados expe imen ales. La ganancia L2del mapa pe u - baci´ on-salida ha sido ajus ada de acue do con la p oposici´ on 3.2. Aunque el alo de γpuede hace se a bi a iamen e peque˜ no, en los expe imen os se ha obse ado que no se puede educi po debajo de cie o alo debido a la apa ici´ on de un ib aci´ on sos enida. Es a ib aci´ on, debida a din´ amicas no modeladas, se e ampli icada al aumen a el ´ e mino de i a i o (disipa i o) del con olado , y es o se p o- duce p ecisamen e al educi el alo de γ. Figu e 6: Seguimien o isual de obje i o ijo con mo imien o de mesa senoidal y pe u baci´ on adi i a. 6 Conclusiones En es e abajo se ha p esen ado la implemen aci´ on p ´ ac ica de un con olado basado en la es uc u a hamil oniana con ealimen aci´ on de isual y a enuaci´ on de pe u baciones. El dise˜ no se basa en el con olado de Slo ine and Li mejo- ado pa a inco po a un esquema de echazo de pe u ba- ciones. Como apo aci´ on no edosa es a es uc u a ha sido in- me sa en con ex o poco habi ual en abajos de pasi idad: el seguimien o isual. La ex ensi´ on es na u al y iene dada po el hecho de que la e e encia m´ o il del con olado de Slo ine y Li es calculada bas´ andose en conside aciones geom´ e icas po un subsis ema de seguimien o isual. Se ha es udiado un sis ema eal o mado po una pla a o ma de dos g ados de libe ad sob e una mesa deses abilizado a emulando la supe icie de un buque. Se ha ealimen ado el sis ema median e encode s, gi ´ oscopos y una c´ ama a CCD pa a ealimen aci´ on isual. De o ma expe imen al se ha ilus ado el compo amien o del sis ema en e a la p esen- cia de pe u baciones ex e nas. Acknowledgemen s. Los au o es desean exp esa su ag adecimien o al minis e io espa˜ nol de ciencia y ec- nolog´ ıa pa ocina es e abajo en el ma co de los p oyec os DPI2001-2424-C02-01 y DPI2000-1218-C04-01. Re e ences [1] Olsson H. Lischinscky P. As om K.J., Canudas de Wi C. A new model o con ol on sys ems wi h ic ion. IEEE Tansac ions o Au oma ic Con ol, ol.40,No.3, pp. 419-425, 1995. [2] Slo ine J.J. and Li W. Adap i e manipula o con- ol: a case s udy. IEEE T ans. on Au oma ic Con ol, 13(5):995–1003, 1988. [3] Sadegh N. and Ho owi h R. S abili y and obus ness analysis o a class o adap i e con olle s o obo ma- nipula o s. In . Jou nal. ob. Res., 9:74–92, 1990. [4] A. Van de Scha . L2-Gain and Passi i y Techniques in Nonlinea Con ol. Sp inge -Ve lag, 1989. [5] Cheng D. Wang, Y. and Ge Y. Li, C. Dissipa i e hamil- onian ealiza ion and ene gy-based l2-dis u bance a en- ua ion con ol o mul imachine powe sys ems. In e nal epo 2002.