Servo ontrol vsual para tareas de segumento dnámo trdmensonal, medante la utlzaón de una ámara en un root Delta Alerto raslosheros atraslosheros@ets.upm.es José María Seastán jseas@ets.upm.es Luís Ángel langel@ets.upm.es Flavo Roert froert@naut.unsj.edu.ar Rardo. Carell rarell@naut.unsj.edu.ar Departamento de Automáta, Ingenería Eletróna e InformátaIndustral (DISAM). Esuela éna Superor de Ingeneros Industrales,Unversdad Polténa de Madrd. C/ José Gutérrez Aasal, 2, 28006Madrd, España Faultad de Ingenería Eletróna, Unversdad Pontfa Bolvarana.Km. 7 Vía de Pe de uesta, Buaramanga, Coloma. Insttuto de Automáta, Unversdad Naonal de San Juan. Av. San Martín. Oeste 1109, 5400 San Juan, Argentna. RESUMEN En el presente artíulo se desre el nuevo servo ontrol vsual utlzado en tareas de segumento dnámo en el sstema denomnado Rootens. Esta plataforma es un root paralelo dotado de un sstema de adqusón y proesamento vsual. Su prnpal araterísta es que posee una arqutetura de ontrol totalmente aerta, planeada on el ojeto de dseñar, mplementar, proar y omparar estrategas y algortmos de ontrol a nvel artular y vsual. Bajo esta onepón, en las sguentes seones se muestra una nueva estratega de ontrol vsual que tene omo ojetvo el segumento dnámo de ojetos en el espao trdmensonal. A dferena de las estrategas mostradas en traajos anterores, atualmente se onsdera la posldad de modfar las onsgnas del segumento, permtendo el agarre o el golpeo del ojeto a segur. Igualmente se onsdera la estaldad de Lyapunov de este nuevo ontrol servo vsual. Palaras Clave: Segumento dnámo, root paralelo, ontrol servo vsual, estaldad de Lyapunov. 1 INRODUCCIÓN Los sstemas de vsón en las aplaones de roóta se pueden nlur para otener nformaón de los ojetos presentes en el área de traajo, esta nformaón por sus araterístas dee proesarse y extraer la que resultará útl para la aplaón, en las áreas de roóta resulta partularmente nteresante el uso de la posón y orentaón de los ojetos on los que el sstema nteratuará. Sn emargo la ntegraón de estos sstemas en tareas dnámas presenta muhos aspetos aún no resueltos orretamente y que son motvos de nvestgaón en los prnpales entros de nvestgaón [9]. Entre ellos ae destaar el dseño de estrategas de ontrol vsual de roots en tareas de segumento de ojetos a alta velodad, omo las desarrolladas en el departamento de Informaton Physs and Computng de la Unversdad de okyo [10]. Fg.(1) Sstema Rootens. Con el fn de poder estudar e mplementar dstntas estrategas de ontrol vsual, el Grupo de Vsón por Computador de la Unversdad Polténa de Madrd optó por el dseño y la onstruón de la plataforma Rootens. Dha plataforma (se muestra en la Fg.(1)) es un root paralelo de tres grados de lertad, on una estrutura aerta para el ontrol, que ha permtdo la realzaón de tareas dnámas a alta velodad. En el presente artíulo se plantea un nuevo algortmo de ontrol servo vsual para tareas
de segumento dnámo. El algortmo propuesto nvolura un estudo de la presón de medones artesanas que se logran utlzando una sola ámara prevamente alrada. En térmnos generales, un root paralelo onsta de dos plataformas undas por más de una adena nemáta errada. Esta estrutura presenta dversas ventajas on respeto a un root sere, omo son una mayor rgdez [6], presón, apadad de arga, velodad y una menor nera de los motores. La prnpal desventaja rada en la dsmnuón del espao de traajo [1]. La estrutura meána del sstema Rootens está nsprada en el root DELA [5]. El modelo nemáto, la matrz Jaoana y el dseño óptmo del sstema Rootens han sdo presentados en traajos prevos [3]. La estrutura del root ha sdo optmzada tanto desde el punto de vsta nemáto omo dnámo. Así el método de dseño resuelve dos dfultades: La determnaón de las dmensones del root y la seleón de los atuadores. El análss dnámo y el ontrol artular han sdo presentados en Ángel [2] y [4]. El modelo dnámo esta asado en los multpladores de Lagrange e norpora la nfluena de las neras de los anterazos en el desarrollo de las estrategas de ontrol. El sstema posee dos lazos de ontrol. Uno nterno asado en la nformaón artular, que se ejeuta ada 0.5 ms, y que norpora el modelo dnámo en un lazo de prealmentaón y un PD en el lazo de realmentaón. Otro externo, asado en la nformaón vsual, que se ejeuta ada 8.33 ms, y que onsttuye el presente traajo. El artíulo se estrutura de la sguente forma: ras la presente ntroduón, en el apartado segundo se desre revemente el sstema Rootens, ahondándose en el apartado terero se desre el algortmo de ontrol vsual propuesto y temas de su mplementaón. En el apartado uarto se desren algunos de los resultados que se referen a la medón de la posón de la pelota y onsderaones prátas que se deerán tomar en uanta durante la mplementaón del ontrolador. Por últmo en el apartado qunto se destaan las onlusones otendas. 2 DESCRIPCIÓN DEL SISEMA En este apartado se desre el entorno de prueas, los elementos que omponen al sstema Rootens y sus araterístas funonales. Más nformaón se puede onsultar en [1]. 2.1 ENORNO DE PRUEBAS El ojetvo defndo para el sstema es que la pnza del root sea apaz de segur a una dstana varale una pelota de png pong sujeta on un hlo de la estrutura fja (Fg.(2)) y que se mueve a una velodad erana a 1 m/s. A fn de faltar la ejeuón del ontrol vsual se ha optado por emplear una pelota de olor negro sore fondo lano. 2.2 SISEMA DE VISIÓN El sstema Rootens posee una ámara en el extremo del root, Fg.(2). La uaón de la ámara omna la adqusón de un amplo ampo vsual, uando el root está alejado del ojeto, on una adqusón más presa uando el root está a una dstana meda del ojeto. Este aspeto es muy mportante para futuras aplaones del sstema, omo puede ser golpear o oger una pelota. A destaar los sguentes aspetos: 2.2.1 Cámara. Se usó la Sony XC-HR50, que permte la adqusón de mágenes en 8.33 ms, on resoluón de 240 x 640 píxeles, on esaneado progresvo y un tempo de ntegraón de 1 ms. Fg.(2) Cámara y entorno del root. 2.2.2 arjeta de adqusón Se utlzó la Matrox Meteor 2-MC/4. Permte la adqusón en modo dole uffer (adqusón smultánea on el proesamento de la magen preva), fundamental para ejeutar el ule de ontrol vsual en 8.33 ms. 2.2.3 Proesamento de mágenes. Una vez adqurda la magen el sstema realza la segmentaón de la pelota sore el fondo lano. Como araterístas vsuales se utlzan las provenentes de realzar dos análss: En el prmero se alulan el entrode de la pelota y el dámetro de la msma on presón supxel. En el segundo se ajusta una elpse on datos provenentes del perímetro de la pelota y de gual manera se utlza presón supxel. Estos dos análss se utlzan porque uando la pelota está lejos de la ámara, se otene mejor presón on el prmer análss que on el segundo. Sn emargo uando la ámara solo oserva una parte de la pelota se utlza el segundo método que puede otener el dámetro y entro a partr de solo tres puntos del perímetro. Para realzar medones espaales de la pelota on una ámara es neesaro haer una alraón preva de esta. El algortmo de ontrol mpla el
onomento de la velodad de la pelota, que se estma medante un fltro de Kalman [1]. 2.3 SISEMA DE CONROL El hardare de ontrol está formado por una tarjeta dspace 1103. En ella se ejeutan: la generaón de trayetoras, el álulo de los modelos nemáto y dnámo del manpulador, así omo la mplementaón de los algortmos de ontrol. El sstema de aonamento utlzado está ompuesto por servomotores rushless AC (Unmotor), varadores de velodad (Undrve SP) y elementos redutores [1]. 2.4 CARACERÍSICAS FUNCIONALES El ontrol vsual desarrollado está ondonado por las araterístas propas de la aplaón y del sstema Rooens. Así es neesaro destaar la alta nertdumre en los datos sumnstrados por el sstema de vsón. El ajo período de muestreo (8,33 mlsegundos) onverte un error de 0.25 píxeles en una varaón de 1 m/s en la velodad de la pelota, muy elevada para la neesara ontnudad del movmento del root. Igualmente el sstema Rooens posee, omo ualquer sstema real, dversas lmtaones que deen de ser tendas en uenta en la estratega de ontrol para poder asegurar el éxto de la msma. Las dos prnpales son los retrasos y las saturaones. Exste un retraso entre los datos sumnstrados por la ámara y los otendos por los sensores artulares, estmado en 2 perodos de muestreo (16.66 mlsegundos). Está motvado por la ntegraón de la nformaón lumnosa, su transmsón y proesamento. 3 CONROL SERVO VISUAL Los sstemas de oordenadas defndos se muestran en la Fg.(3). Σ, Σ e, y Σ son los sstemas del mundo, del extremo del root y de la ámara. p es la posón relatva de la pelota on respeto al sstema de oordenada de la ámara, y p e es la posón relatva del extremo del root on respeto al sstema de oordenadas del mundo, onoda a través de la nemáta dreta. Se suponen onodas o onstantes las matres R e, R, e R y e, a través de las alraones y modelados del sstema. Nótese que el sstema rootens responde al esquema de ámara en mano. Por esta razón las referenas de ámara y extremo del root son el msmo punto para efetos del presente traajo. De entre las dstntas alternatvas exstentes, se ha elegdo la utlzaón de un ontrol asado en posón. Como se aprea en el esquema áso de la Fg.(4), la funón de error se otene omparando la posón deseada del ojeto ( p ( k )) on la posón otenda por el sstema de vsón ( pˆ ( k )),amas expresadas en el sstema de oordenadas de la ámara. En el presente artíulo la posón deseada es varale, la señal de ontrol y la estaldad son otendas y analzadas ajo este supuesto. Sn emargo en traajos anterores la posón deseada se ha mantendo onstante y se han heho los análss menonados [11]. p Fg.(3) Esquema del root y los sstemas de referena utlzados. p k Planfador Control + + Cnemáta PD + ek vsual nversa - - ( X, Y, Z) Fg.(4) Esquema de ontrol del sstema Rootens. El ontrolador proporona la velodad deseada para el efetor fnal. A través de un planfador en el espao artesano y empleando la matrz Jaoana del root se otene la varaón artular del root. La varale k nda el nstante de muestreo. 3.1 DISEÑO DEL CONROLADOR Para el dseño del ontrolador se onsdera que de auerdo a la Fg.(3) el error entre la posón deseada y la medda de la pelota respeto a la ámara se tene omo: ek = p k p k (1) uk X Z C Y C Z Σ, &,&& q k q k q k d d d Sstema de vsón Dnáma nversa τ( k) Root Donde p es la posón deseada y p es la posón medda por el sstema vsual, amas en el sstema oordenado de la ámara. Reesrendo la e. (1) se tene: X C Σ Y p p +
ek = p k R p k p k (2) Dervamos el error y se otene: = ek & v k R v k v k (3) De la e. (2) y (3) se oserva que p ya no es una posón fja puesto que se varará para aerarse a la pelota, y esta velodad de aeramento es llamada v ( k ). La velodad de aeramento varará o no en todo el trayeto, sn emargo es deseale que tenga erto valor en el momento de olsón on la ola y la posón deseada puede alularse omo: = + ( 1) p( k) p( k1) = p k v k p k v k (4) Forzamos a que el error tenga un omportamento omo el que se defne en la e. (5). =λek ek & (5) Susttuyendo (1) y (3) en (5) se tene: ( ) ( p p( k) ) v k R v k v k λ = (6) Despejando la velodad sore la que podemos atuar al tener el ontrol del root: = v( k) λ v k ( ) + R p k p k v k (7) Asumendo que la velodad de la ámara es la msma que la del extremo del root, la usamos omo nuestra ley de ontrol. La velodad y posón del ojeto es medda por medo del sstema vsual, sn emargo son medones on alto ontendo de rudo, por esto, las señales usadas en el ontrolador son las estmadas: pˆ ( k ) y vˆ ( k ). = ˆ ˆ = λ ˆ u k v k v k ( ) + R p k p k v k 3.2 OBENCIÓN DE λ (8) Supondremos que para n sufentemente pequeña se umple: p k+ n p k + n v k (9) Y tamén: p k+ n p k + n v k (10) Y tamén: p k+ n p k + n v k (11) Olgando a anular el error en el nstante k+ n de la euaón (2): 0 = p k+ n R p k+ n p k+ n (12) Susttuyendo (9), (10) y (11) en (12) se tene: n v( k) + 0= p k + R p k n v k p k n v k Reaomodando logramos la expresón: + n v k R p k n v k = + + p k p k n v k (13) (14) enendo en uenta que las medones vsuales pˆ ( k ) y vˆ ( k ) son estmadas y onsderando que pˆ ( k) p( k) puede expresarse omo R ( ˆ p( k )), susttumos estas expresones en (14) y tenemos: ˆ = v( k) 1 ( ˆ ) + v k R p k p k v k n (15) Reordando la euaón (8) se oserva que un uen valor para λ es haer sufentemente pequeña. 1 λ = on una n n 3.3 ESABILIDAD DEL SISEMA Por medo del análss de Lyapunov es posle proar la estaldad del sstema; se puede demostrar que el error onverge a ero s se onsderan ondones deales s estas ondones no se umplen, puede proarse que el error fnalmente quedará aotado. Para el análss de estaldad, onsderaremos (3), y (7) para otener la expresón en lazo errado: + λek = 0 ek & (16) Esogemos una funón de Lyapunov omo: 1 V e k e k 2 = (17)
λ 0 & = & = < (18) V e k e k e k e k La euaón (18) mpla que ek 0 uando k pero saemos que el error no es ero por los errores nherentes al sstema vsual. De esta forma la expresón v e= v u no es erta y un error ρ dee ser onsderado. vˆ ( k) u( k) v( k) ρ( k) Donde en el vetor ( k) = = + (19) ρ se tenen en uenta los errores de las estmaones norretas y de la dnáma no modelada. La expresón real de la dervada del error se otene a partr de la euaón (3), susttuyendo el valor de v por el otendo omo ley de ontrol vˆ ( k) = u( k) de la euaón (19) = ρ ek & v k R v k v k k (20) Y tenendo en uenta la euaón (7): ρ R R λ p ( k) p ( k) v k ek & = v k R v k v k k + Smplfando: ρ λ( p( k) p( k) ) = R ρ( k) λe( k) e& k = v k v k + R k (21) (22) Consderando nuevamente la funón de Lyapunov propuesta en la euaón (17). V& = e ( k) e& ( k) (23) λ ρ = + e k e k e k R k La ondón sufente para que V & < 0 se umpla es: ρ e > (24) λ ρ k S onsderamos que se puede umplr que 0 entones se logrará anular el error y ek 0.En aso ontraro, la e. (24) no se umple, el error no deree pero queda fnalmente aotado por: ρ e < (25) λ Esto sgnfa que el error reerá o dereerá en medda que las estmaones nluyan mayor o menor error. S onsderamos que los errores de estmaón de la posón y velodad de la pelota son muho mayores que los errores atrudos a la dnáma del sstema, entones podemos otener un valor de ρ ( k) s gualamos las euaones (7), (8) y (19): λ ˆ + vˆ ( k) = ρ( k) + v( k) λ uk= R p k p k + v k R p k p k + v k Por lo que ρ ( k) se podrá expresar omo: ρ ( k) = vˆ ( k) v( k) + R λ pˆ ( k) p ( k) (26) (27) Fg.(5) Root en tareas de golpeo. El root dee tener una velodad fnal al momento de golpear el ojeto [p Rf, v Rf, a Rf =0] el golpeo se produe en un punto de la trayetora del ojeto [p oj, v oj, a oj ]. 4 ESRAEGIA DE GOLPEO p, v, a = 0 Rf Rf Rf En el apartado anteror, el ontrolador servo vsual propone una nteraón dnáma on el ojeto pero exsten argumentos que se deen tomar en uenta. El algortmo aquí propuesto demuestra que es posle aerarse dnámamente al ojeto sn afetar la estaldad del sstema. A pesar de esto, es neesaro ndar uanto se dee aerar y on que velodad o aeleraón deerá haerlo. Se deen defnr estas estrategas dependendo s se desea que el root golpee, agarrare o solamente se aerque a un ojeto.
La tarea propuesta se refere al juego de png pong ontra una pared. Dada la omplejdad vsual que este juego puede suponer, se han heho smplfaones vsuales para aglzar el proesamento vsual. Estas onsderaones nluyen la utlzaón de lumnaón ontrolada, un esenaro on fondo lano y una pelota negra. La tarea nluye el golpeo de la pelota on una determnada dreón y sentdo (Fg.(5)) por tanto es mportante tener en uenta aspetos: El modelo de movmento del ojeto, la trayetora del root para produr el movmento deseado en la pelota o la nertdumre de los datos adqurdos por el sstema vsual. El efeto de golpeo de la pelota tene por ojetvo llevar a la pelota a un punto deseado y de esta manera faltar el sguente golpeo. Dreón deseada después del golpeo Umral de golpeo X Y Z Γ 0 Γ 1 Γ 2 Umral omenzo de predón... p, v, a = 0 Rf Rf Rf Pelota detetada en el umral externo. Perodos de muestreo del lazo vsual. Predón de la uaón de la pelota. rayetoras aluladas para ada predón. rayetora generada. Predón en la posón del ojeto. Fg.(6) Generaón de trayetoras ada perodo servo vsual. El modelo de movmento del ojeto puede ser planteado de varas maneras, sn emargo se omenzará on el modelo más senllo. Este modelo se propondrá omo una prmera aproxmaón y se omparará on modelos más omplejos y aproxmados al movmento real. En la Fg.(6) se muestra un esquema donde la posón de la pelota se extrapola lnealmente hasta la posón de golpeo. En ada perodo del ontrol servo vsual esta medón de la posón se atualza, y el planfador de trayetora del órgano fnal se realula orrgendo la trayetora. En la Fg.(6) se oserva que esta orreón de trayetora puede redurse s se onsdera un modelo de movmento (de la pelota) más ompleto. Como requstos mínmos el modelo de movmento de la pelota deerá proporonar al planfador la posón y velodad de la pelota. Cuando en ada perodo vsual se realula el planfador de trayetoras la velodad fnal (en sentdo ontraro al de la pelota) del extremo del root se dee mantener hasta el momento del hoque Γ n on la pelota. Durante esta operaón, los límtes de operaón del root no deen sorepasarse. Para la planfaón del extremo del root se propone un planfador de qunto orden. Este planfador puede ondonar los extremos del movmento hasta su segunda dervada. Cada trayetora Γ se alula tenendo en uenta los extremos deseados. Los extremos nales del movmento p, v y a dependen de la posón o movmentos anterores y están defndos en todo momento. Los extremos fnales del movmento p f = punto donde se golpeará a la pelota, v f = velodad de golpeo y a f = 0, Fg.(6). Otra stuaón mportante a tomar en uenta por el planfador es que uando la pelota se aera hay un punto de desón donde se dee optar por usar el golpear o no la pelota. Para esto se traza una línea (aja) vrtual que uando es ruzada por la pelota planfador evalúa s logrará ntereptar la pelota on la máxma velodad y aeleraón permtda (y tomando en uenta la predón del punto de golpeo). Otro fator a onsderarse se enuentra nmerso en tareas de golpeo o agarre; espeífamente en la apadad del sstema para nteratuar on el ojeto uando no es detetado por el sstema vsual. Para esto se propone preder el omportamento del ojeto durante el perodo en que no se enuentre en el espao vsual de la ámara hasta el momento del agarre o golpeo. Al pronostar el omportamento del ojeto, se desea ontar on datos hstóros del ojeto que sean araterístos de su movmento. Por esto se realzará un estudo de los datos otendos por el sstema vsual. En traajos anterores, se han utlzado las medones de este sstema vsual y se sae que las estmaones tenen un error mayor en el eje Z de la ámara. Para realzar este estudo de los datos vsuales se dee onsderar que solo se uenta on una ámara y es neesaro ontar on la alraón de la msma y de gual forma las dmensones del ojeto. 5 CONCLUSIONES En el presente traajo se presentó un ontrol servo vsual para segumento dnámo de ojetos en el espao trdmensonal. En traajos anterores se muestran expermentos donde el segumento es a una dstana fja del ojeto y on uenos resultados. Por este motvo se pensa que el nuevo algortmo aquí planteado logrará resultados smlares. Para reforzar esta onlusón se demuestra la estaldad del sstema y el aotamento del error ante nertdumres de medón. En este últmo punto s no se onsdera la dnáma del sstema, el error fnalmente estará aotado por el error de la
estmaón de las medones de posón y velodad hehas por el sstema vsual. Como últmo punto se planteó la estratega que se segurá en el golpeo de ojetos. Esta estratega orgna dferentes retos omo pueden ser: ntereptar la pelota en un determnado punto on un determnado vetor velodad, ajuste del modelo de movmento del ojeto golpeado y tener en uenta las lmtantes físas del sstema. Agradementos El sstema de expermentaón en ontrol servo vsual, llamado Rootens no haría sdo posle sn las suvenones proporonadas por los proyetos: "Control y Perepón Colaoratva en Sstemas eleoperador para Aones Cooperatvas de Roots en Entornos Dnámos", fnanado por la Agena Española de Cooperaón Internaonal (A/6512/06). "Roots de servos para la mejora de la aldad de vda de los udadanos en áreas metropoltanas", fnanado por la Comundad de Madrd (Ref. S- 0505/DPI/0176) Referenas [1] Ángel, L. (2005a). Control Vsual de Roots Paralelos. Análss, Desarrollo y Aplaón a la Plataforma Rootens. ess Dotoral de la Unversdad Polténa de Madrd. [2] Ángel, L., Seastán, J.M., Saltarén, R., Aral, R., Gutérrez, R. (2005). Rooens: Desgn, Dynam Modelng and Prelmnary Control. IEEE/ASME AIM2005, Monterey, USA. [3] Ángel, L., Seastán, J.M., Saltarén, R., Aral, Sanpedro, J. (2005). Rooens: Optmal Desgn of a Parallel Root th Hgh Performane. IEEE/RSJ Internatonal Conferene on Intellgent Roots ans Systems (IROS). Alerta, Canadá. [4] Ángel, L., Seastán, J.M., Saltarén, R., Aral. (2005d). Rooens System. Part II: Dynams and Control. 44 IEEE Conferene on Deson and Control and European Control Conferene (CDC-ECC 05), Sevlla, España. [5] Clavel, R. (1988). DELA: a fast root th parallel geometry. 18 th Internatonal Symposum on Industral Root, pp. 91-100, Sydney, Australa [6] Franços Perrot, Olver Company, Séasten Krut (LIRMM CNRS, Unversty of Montpeller 2) Vnent Naat, Agustn Saenz (Fundaón Fatronk), Seema Gupta, (Adept ehnology, In.). Quattro A ne root manpulator for ultra-hgh-speed pakagng applatons. ICRA, 2007. [7] Gutérrez, D., Seastán, J.M., Ángel, L. (2005). Estmaón de la posón y velodad de un ojeto móvl. Aplaón al sstema Rootens. XXVI Jornadas de Automata, Alante [8] Kaneko, M., Hgashmor, M., akenaka, R., Namk, A., Ishkaa, M. (2003). he 100 G apturng root - too fast to see. IEEE/ASME ransatons on Mehatrons. Volume 8, Issue 1, pp 37 44. [9] Krag, D. y Chrstensen, H.I. (2005). Advanes n root vson. Roots and Autonomous Systems. 52 (1), 1-3. [10] Morkaa, Sho Senoo, aku Namk, Ako Ishkaa, Masatosh ; Realtme ollson avodane usng a root manpulator th lghteght small hgh-speed vson systems ; Roots and Automaton, 2007 IEEE Internatonal Conferene on. Pulaton Date: 10-14 Aprl 2007. On page(s): 794-799. ISSN: 1050-4729. ISBN: 1-4244-0601-3. Dgtal Ojet Identfer: 10. 1109 / ROBO. 2007. 363083. Posted onlne: MAY-2007. [11] raslosheros A., Seastán J. M., Ángel L., Roert F., Carell R. Vsual Servong usng a Parallel Root: Prelmnary Results. Advaned ntellgent mehatrons. 2007 eee/asme nternatonal onferene on. Pulaton Date: 4-7 Sept. 2007. page(s): 1-8. ISBN: 978-1- 4244-1264-8. Date Pulshed n Issue: 2007-12-26 09:56:27.0