Controlador difuso para problemas de navegación en presencia de obstáculos fijos

CONTROLADOR DIFUSO PARA PROBLEMAS DE NAVEGACIÓN EN PRESENCIA DE OBSTÁCULOS FIJOS M. B ox A. Ge sno iez S. Sánchez-Solano I. Ba u one Ins i u o de Mic oelec ónica de Se illa, CNM-CSIC, Se illa, España. [email p o ec ed].es and e[email p o ec ed].es san [email protected] lum[email p o ec ed] Resumen En es a comunicación se desc ibe un sis ema de con ol di uso pa a aplicaciones de na egación de obo s mó iles au ónomos en p esencia de obs áculos ijos. Las he amien as de CAD del en o no X uzzy 3, desa ollado en el IMSE, han acili ado el diseño del con olado . En la comunicación se p ocede a la e i icación del con olado diseñado ope ando en un lazo ce ado con el modelo del obo mó il au ónomo eléc ico Romeo 4R, diseñado y cons uido en la Escuela Supe io de Ingenie os de la Uni e sidad de Se illa. Las simulaciones ealizadas demues an la e iciencia del con olado desa ollado. Palab as Cla e: Robo s mó iles, Con ol di uso, Obs áculos ijos. 1 INTRODUCCIÓN En la li e a u a se han epo ado di e sas al e na i as pa a esol e el p oblema de e i ación de obs áculos ijos con obo s mó iles au ónomos. Es as soluciones se engloban en es g andes g upos que co esponden a es a egias de na egación delibe a i as, eac i as e híb idas [3]. Las delibe a i as se basan en el conocimien o a p io i del en o no [7]. Se ca ac e izan po consegui una na egación en un iempo mínimo o con un meno consumo ene gé ico aunque, como des en aja, p esen an p oblemas en la adap ación a en o nos dinámicos pa cialmen e conocidos. Las es a egias eac i as consis en en p o oca una eacción en e a la in o mación senso ial que se ecibe. En e és as se encuen an las basadas en el seguimien o del con o no del obs áculo, en campos po enciales [2], en op imización local [6] y en el uso de la lógica di usa [4]. Se ca ac e izan po su lexibilidad y, como con apa ida, p esen an la al a de capacidad de na ega de o ma óp ima. Finalmen e, las es a egias híb idas [5] usan esquemas mix os que combinan écnicas delibe a i as y eac i as pa a ap o echa las en ajas y educi los incon enien es de ambas opciones. En es a comunicación se desc ibe un sis ema de con ol que emplea una es a egia de na egación eac i a basada en lógica di usa, que combina eglas de ac uación undamen adas en el conocimien o heu ís ico del p oblema con da os numé icos ob enidos del análisis geomé ico del mismo. El sis ema a con ola es el obo mó il au ónomo Romeo 4R, do ado de una se ie de senso es que acili an el cálculo del es ado del ehículo, así como la de ección de los posibles obs áculos [8]. Los alo es de posición, o ien ación y elocidad se ob ienen a pa i de los da os p opo cionados po un encode asociado al mo o de acción y po un gi óscopo. A su ez, los da os del encode del mo o de di ección pe mi en ob ene los alo es de la cu a u a del ehículo. Finalmen e, un láse pe mi e ealiza un ba ido angula de 0º a 180º, p opo cionando, pa a cada ángulo, la dis ancia comp endida en e el obo y el posible obs áculo. Las ases de desc ipción y e i icación se han lle ado a cabo con las di e en es he amien as de CAD p opo cionadas po el en o no de desa ollo de sis emas di usos X uzzy 3 [10]. Las he amien as de desc ipción de dicho en o no acili an la especi icación del sis ema de con ol median e una es uc u a je á quica que combina bloques di usos con bloques de p ocesado con encional. Po o o lado, las he amien as de e i icación pe mi en ajus a los pa áme os del sis ema y comp oba su compo amien o median e simulación. Es a comunicación es á es uc u ada como a con inuación se desc ibe. La sección 2 mues a un es udio eó ico del p oblema de na egación de obo s mó iles au ónomos con e i ación de obs áculos. En ella se explica cómo se a on a el p oblema de de ección de obs áculos y cómo modi ica el ehículo su ayec o ia cuando el obs áculo es de ec ado. La sección 3 p esen a el diseño y la implemen ación del con olado di uso, usando la me odología del en o no X uzzy 3; en ella se mues a cómo las he amien as de CAD del en o no han acili ado su desa ollo. En la sección 4 se p ocede a la e i icación del con olado di uso ope ando en un lazo ce ado con el modelo del obo Romeo 4R; en ella se mues an los esul ados ob enidos as la simulación ealizada con la he amien a x sim del en o no X uzzy 3. Po úl imo, la sección 5 ecoge las conclusiones y las líneas de es udio hacia las que es a in es igación se a a encamina en el u u o. 2 EL PROBLEMA DE NAVEGACIÓN EN PRESENCIA DE OBSTÁCULOS Supongamos que en un ins an e dado 0 el ehículo iene la con igu ación (x0, y0, φ0, 0, γ0), donde x e y hacen e e encia a las coo denadas de posición del ehículo, φ a la o ien ación del mismo con espec o al eje x = 0, a la elocidad y γ a la cu a u a. Las elaciones en e las a iables implicadas en es a con igu ación pueden ob ene se u ilizando el modelo cinemá ico de la bicicle a pa a desc ibi el compo amien o del obo [9]: () () ⎪ ⎪ ⎩ ⎪ ⎪ ⎨ ⎧ ⋅= ⋅= ⋅= ⋅ ⋅ ⋅ y x γφ φ φ cos sin En unción de la con igu ación inicial y de la posición inal ijada como obje i o, el sis ema de con ol de conducción debe p opo ciona los alo es de elocidad y cu a u a que se án aplicados en un ins an e pos e io 0 + ∆ . Pa a alcanza la con igu ación inal e i ando colisiones con los posibles obs áculos se conside a án dos si uaciones. Si no exis e obs áculo se ac i a á un con olado di uso cuya inalidad se á la de hace na ega al ehículo has a la con igu ación inal. Sin emba go, si exis e algún obs áculo, se ac i a á un segundo con olado di uso cuya inalidad se á e i a lo. De es a o ma, es necesa io con empla dos p oblemas: po un lado la na egación sin obs áculos y, po o o, la modi icación de la ayec o ia del ehículo pa a e i a los obs áculos que se de ec en. 2.1. DETECCIÓN DE OBSTÁCULOS Dis ingui emos dos si uaciones en unción de que el ehículo es é desc ibiendo una ayec o ia ec a (γ = 0) o ci cula (γ = γ0). Si el obo es á desc ibiendo una ayec o ia ec a, se cons uye un pasillo ec o en o no al coche y se a es udiando si es á lib e de obs áculos. Se conside a que el pasillo iene una anchu a de 2m espec o al eje del ehículo. Teniendo en cuen a que Romeo iene una anchu a de 1m, es o signi ica que la sepa ación en e los la e ales del ehículo y la pa ed del pasillo se á de 1,5m. La cu a u a máxima que puede oma Romeo es de 0,4m-1, lo cual supone que puede desc ibi una ayec o ia con un adio mínimo de 2,5m. Es a ayec o ia, ep esen ada como cu a 1 en la Figu a 1, pod á lle a se a cabo si el obo ya enía una cu a u a de 0,4m-1 o no la enía pe o es aba pa ado. En o o caso, pues o que las ansiciones en la cu a u a no pueden se ins an áneas, una ayec o ia como la ep esen ada po la cu a 1 no se á ealis a, sino que lo se án ayec o ias como la ep esen ada po la cu a 2, cuyo adio de cu a u a es supe io a 4m. Si ap oximamos la cu a 2 de la Figu a 1 po su secan e se ob iene el sec o ci cula de la Figu a 2. Los alo es ap oximados de los pa áme os h0 y α de la Figu a 2 son 4,2m y 30º espec i amen e. Po an o, la ampli ud angula del sec o ci cula (2α) es ap oximadamen e 60º. En es e caso, el obs áculo queda ca ac e izado po los pa áme os h, ФR y ФL. Se de ine h como la dis ancia mínima exis en e en e el ehículo y el obs áculo. Se de ine ФR como el ángulo comp endido en e el bo de de echo del obs áculo y la pe pendicula al eje del ehículo. De o ma análoga se de ine ФL como el ángulo comp endido en e el bo de izquie do del obs áculo y la pe pendicula al eje del ehículo. Conside a emos que el con olado di uso pa a e i ación de obs áculos en a á en juego si el alo de h es meno que el alo de h0 y ФR o ФL es án comp endidos en e (π/2 + α) y (π/2 –α). Todo Figu a 1. Cons ucción del pasillo ec o Fi g u a 2. Sec o de isión del p asillo obs áculo cuyos pa áme os ca ac e ís icos es én comp endidos en el sec o ci cula , p oduci á una modi icación en la ayec o ia que lle a ía el obo si no hubie a obs áculos. Po an o, los alo es de h0 y α son los alo es lími es que se han es ablecido pa a que el obs áculo p o oque un cambio en la ayec o ia. Po o o lado, si el obo es á desc ibiendo una ayec o ia ci cula , se cons uye un pasillo cu o de anchu a 2m y se es udia si es á lib e de obs áculos. Las coo denadas (xc, yc) de la Figu a 3 hacen e e encia al cen o de la ci cun e encia que desc ibe el ehículo en su ayec o ia; (xp, yp) son las coo denadas de posición del láse del ehículo si uado en la pa e delan e a del mismo; po úl imo, (xm, ym) hacen e e encia a las coo denadas del pun o del obs áculo que se encuen a más p óximo al obo . Como se obse a en la igu a, el ec o se puede exp esa como suma de los ec o es p ly m l: ()( ) () cmcmpmpmcpcpmp yyxxyyxxyyxxll −−=−−+−−=+= ,,, (1) La ausencia de obs áculos que hagan p eciso modi ica la ayec o ia que lle aba el obo iene, en es e caso, de e minada po la siguien e condición: ()() 2 0 22 2 1⎟ ⎟ ⎠ ⎞ ⎜ ⎜ ⎝ ⎛+≥−+− γ cmcm yyxx ó ()() 2 0 22 2 1⎟ ⎟ ⎠ ⎞ ⎜ ⎜ ⎝ ⎛−≤−+− γ cmcm yyxx (2) En los casos en que la ecuación (2) no se e i ique, el obo debe á modi ica su ayec o ia. 2.2. MODIFICACIÓN DE LA TRAYECTORIA DEL VEHÍCULO PARA EVITAR OBSTÁCULOS Pa a de e mina la magni ud y el signo de la cu a u a que debe adop a el ehículo pa a esqui a el obs áculo, se han de cuan i ica c i e ios como la can idad y si uación espacial de pasillo que es á siendo ocupado po el obs áculo. Dependiendo de que el pasillo sea ocupado en mayo o meno medida po el obs áculo, el ehículo debe á aplica una cu a u a mayo o meno pa a pode esqui a lo. También es p eciso conside a la zona de pasillo que es á siendo ocupada pa a sabe qué signo debe oma la cu a u a. Pa a pode cuan i ica odo lo an e io han de e alua se la magni ud y el signo de los ángulos ФR y ФL. En la Figu a 4 se mues an seis casos en los que se analizan las dis in as si uaciones en que puede encon a se el obs áculo y la cu a u a a adop a po el ehículo. En el p ime caso el obs áculo se encuen a poco me ido en el pasillo in adiendo la zona izquie da, con lo que con una cu a u a no muy ele ada y posi i a se ía su icien e pa a esqui a lo. Se conside a como cu a u a posi i a Fi g u a 3. Cons ucción del p asillo cu o Figu a 4a. Caso 1 Figu a 4b. Caso 2 Figu a 4c. Caso 3 Figu a 4d. Caso 4 Figu a 4e. Caso 5 Figu a 4 . Caso 6 aquella cuyo sen ido es el de las agujas del eloj y nega i a la que oma el sen ido con a io. La Figu a 4b ep esen a el caso de un obs áculo bas an e me ido en el pasillo po la zona izquie da, con lo que se necesi a una cu a u a ele ada y posi i a pa a esqui a lo. El caso mos ado en la Figu a 4c ep esen a un obs áculo muy pequeño que se encuen a en medio del pasillo y en en e del ehículo, con lo que no se necesi a una cu a u a ele ada pa a esqui a lo. Como el obs áculo se encuen a en medio del pasillo, se á (en p incipio) indi e en e el signo que adop e la cu a u a. En las Figu as 4d y 4e podemos obse a dos si uaciones simé icas a las de los casos 1 y 2, con lo cual, la cu a u a esul an e pa a e i a el obs áculo se á igual en módulo, pe o de signo con a io. Po úl imo, en la Figu a 4 , se obse a un obs áculo muy g ande que se encuen a en medio del pasillo y en en e del ehículo, con lo que se necesi a una cu a u a ele ada pa a esqui a lo. El signo que debe oma la cu a u a del ehículo se discu i á más adelan e. 3 DISEÑO DEL SISTEMA DE CONTROL Las ideas an e io es sob e de ección y e i ación de obs áculos han sido aplicadas en el desa ollo de un sis ema de na egación pa a obo s mó iles que combina dos con olado es di usos: uno pa a guia al ehículo desde una con igu ación dada has a la con igu ación obje i o (x= y = γ = φ = = 0), y o o pa a modi ica su ayec o ia en p esencia de obs áculos. Mien as que pa a el diseño del p ime con olado se han analizado las es icciones geomé icas del p oblema con obje o de op imiza la ayec o ia seguida po el ehículo [1], en el diseño del segundo se emplea un esquema de azonamien o simila al que segui ía un conduc o humano. Tan o en un caso como en el o o la lógica di usa pe mi e exp esa el compo amien o del sis ema de o ma lingüís ica median e eglas del ipo si - en onces. El sis ema de con ol diseñado sigue un esquema je á quico cuya es uc u a se puede desc ibi edi ando di ec amen e un a chi o “.x l” o g á icamen e con la he amien a x edi del en o no X uzzy 3, al y como se mues a en la Figu a 5. En la es uc u a del con olado se obse an los bloques “in e polación”, “ es a”, “sua izado_wheel” y “sua izado_ ” que se enca gan del cálculo de la ayec o ia del obo has a la con igu ación obje i o. Las ayec o ias conseguidas con es os bloques son ec as angen es que unen ci cun e encias de cu a u a máxima azadas en el pun o de inicio y de llegada al y como mues a la Figu a 6. Figu a 5. Visualización de la es uc u a del con olado di uso median e la en ana p incipal de x edi Figu a 6. T ayec o ias desc i as po el ehículo Los bloques “modi icacion_R” y “modi icacion_L” se enca gan de modi ica la ayec o ia del ehículo en el caso de que se de ec e la p esencia de obs áculos. Como se indicó en la sección 2, la modi icación de las ayec o ias se hace en base a la can idad y zona de pasillo ocupado po el obs áculo; pa a e es o se es udian los alo es de phi_R y phi_L. Según los alo es de es os ángulos, se dis inguen los seis casos comen ados en la sección 2 y pa a cada uno de ellos se p opone una cu a u a di e en e; es o se puede o mula a pa i de eglas del ipo si-en onces. La he amien a x edi pe mi e edi a la base de eglas de es os bloques como se obse a en la igu a 7. La di e encia en e la base de eglas de ambos bloque adica en que pa a los casos dudosos de la sección 2 en los que sea indi e en e el signo de la cu a u a pa a esqui a el obs áculo, “modi icacion_R” oma cu a u a posi i a mien as que “modi icacion_L” oma nega i a. El conjun o de ope ado es implicados en un bloque ( igu a 8b), así como el ipo de las a iables se puede de ini ambién a pa i de la he amien a x edi , como se obse a pa a el caso del ipo ang de las a iables phi_R y phi_L en la igu a 8a. Median e es a he amien a se puede especi ica el núme o de unciones de pe enencia, los angos de las mismas, así como el ipo ( iangula es, apezoidales, gaussianas, single on, e c.). En la es uc u a ambién apa ece “Eleccion_RL” que es un bloque no di uso que se enca ga de oma cu a u a posi i a o nega i a en los casos dudosos dependiendo de con cuál de ellas se consigue una ayec o ia mínima. Pa a ello se es udia cuál es la di ección que lle aba el obo an es de de ec a el obs áculo, e aluando el signo de la cu a u a p opo cionada po “sua izado_wheel”, wheel1, y se decide así el signo de cu a u a más con enien e. Si su alo es posi i o, la a iable bina ia eleccion oma á alo ce o, mien as que si es nega i o, oma á alo uno. El alo omado po la a iable eleccion se gua da y no su e cambios has a e un nue o obs áculo. Pa a de ec a cuándo se dan los casos dudosos se de ine una a iable llamada indecision, siendo su alo uno cuando se encuen a en uno de es os casos y ce o en caso con a io. Finalmen e, en la es uc u a del con olado apa ece “decision” que nue amen e es un bloque no di uso que oma como salida la del bloque “sua izado_wheel” o la de “eleccion_RL” según el alo de la a iable obs acle; es a a iable oma alo ce o cuando el con olado no de ec a ningún obs áculo y oma alo uno cuando la de ección es posi i a. Pa a calcula el alo de la a iable obs acle se u ilizan las condiciones de obs áculo de pasillo ec o y cu o de la sección 2. 4 VERIFICACIÓN DEL CONTROLADOR Pa a es udia si el compo amien o del con olado di uso es co ec o, hemos empleado las he amien as de e i icación del en o no X uzzy 3, p incipalmen e la he amien a de simulación x sim. Es impo an e e i ica el compo amien o de un con olado con he amien as de simulación an es de expe imen a con la plan a eal pa a así de ec a posibles e o es. Pa a ealiza las simulaciones se u iliza un modelo de la plan a cuyo código es á esc i o en Ja a. En el código ambién se incluye el p ep ocesado de las señales p opo cionadas po los senso es así como el cálculo de a iables como obs acle, eleccion e indecision. Los esul ados de algunas de las simulaciones ealizadas se pueden isualiza en la Figu a 9. Las igu as 9a y 9b co esponden a casos con un único obs áculo. En ellas se obse a cómo el obo de ec a pe ec amen e el obs áculo ep esen ado po un amo de línea con inua de g oso mayo . Desde el momen o en el que el obs áculo es de ec ado, se e cómo el ehículo comienza a modi ica su ayec o ia sepa ándose de la línea discon inua, que ep esen a la seguida po el ehículo cuando no hay obs áculo. Las igu as 9c y 9d co esponden a casos más complejos con dos obs áculos. En ellas se obse a cómo el ehículo de ec a en p ime luga el obs áculo más ce cano modi icando su ayec o ia pa a pode e i a lo. Seguidamen e cuando e el siguien e obs áculo uel e a modi ica la pa a e i a la colisión con el segundo. Figu a 8a. Especi icación de los ipos de a iables Figu a 8b. De inición del con j un o de o p e ado es Figu a 7. Base de eglas del bloque modi icacion _ R median e x edi 5 CONCLUSIONES En es a comunicación se ha p esen ado una solución al p oblema de na egación de obo s mó iles au ónomos e i ando obs áculos es á icos, basada en la combinación del análisis geomé ico del p oblema con conocimien o heu ís ico modelado median e lógica di usa. Pa a la e i icación expe imen al se ha u ilizado el ehículo Romeo 4R, eniendo en cuen a sus es icciones cinemá icas y u ilizando a ios de los senso es y ac uado es de los que es á p o is o. En el desa ollo del sis ema se han u ilizado di e sas he amien as de CAD del en o no X uzzy 3 pa a el diseño e implemen ación del con olado . En pa icula , se ha hecho amplio uso de la he amien a x sim, que pe mi e simula el con olado ope ando en lazo ce ado con un modelo del obo . Las líneas de abajo u u o de es a in es igación se cen a án en la implemen ación del con olado siguiendo una es a egia híb ida Ha dwa e-So wa e. Ag adecimien os Es e abajo ha sido pa cialmen e inanciado po los p oyec os TEC2005-04359/MIC (CICYT), TIC2006-635 (Jun a de Andalucía) y DPI2005-02293. Re e encias [1] I. Ba u one, F. J. Mo eno Velo, S. Sánchez Solano, A. Olle o. Au oma ic design o uzzy con olle s o ca -like au onomous obo s, IEEE T ansac ions on Fuzzy Sys ems, Vol. 12, No. 4, 2004. [2] C. I. Connoly, J. B. Bu ns, R. Weiss. Pa h planning using Laplace’s equa ion, IEEE Con e ence on Robo ics and Au oma ion, Cincina i, 1990. [3] R. Fe nández Ramos. Na egación au ónoma de obo s mó iles en en o nos pa cialmen e conocidos, Tesis Doc o al, Málaga 2001. [4] D. Fox, W. Bu ga d, S. Thu n. The dynamic window app oach o collision a oidance, IEEE Robo ics & Au oma ion Magazine, 1997. [5] K. Kan , S. Zucke . Towa d e icien ajec o y planning: The pa h- eloci y decomposi ion, The In e na ional Jou nal o Robo ics Resea ch, 1986. [6] T. Kubo a, H. Hashimo o. A s a egy o collision a oidance among mobile obs acle o a mobile obo , 11 h In e na ional Wo ld Cong ess, Fallin, Es onia, 1990. [7] N. J. Nilsson. P inciples o a i icial in elligence, Palo Al o: Tioga, 1980. [8] A. Olle o, B. C. A ue, J. Fe uz, G. He edia, F. Cues a, F. López-Pichaco y C. Nogales. Con ol and pe cep ion componen s o au onomous ehicles guidance. Applica ion o he Romeo Vehicles, Con ol Enginee ing P ac ice, Vol. 7, No. 10, pp 1291-1299, Oc obe 1999. [9] A. Olle o. Robó ica, manipulado es y obo s mó iles, Ed: Ma combo, 2001. [10] X uzzy: He amien as de CAD pa a Lógica Di usa (h p://www.imse.cnm.es/X uzzy/) Fi g u a 9a Fi g u a 9b Fi g u a 9d Fi g u a 9c