trabajo realizadobibing.us.es/proyectos/abreproy/11509/fichero/volumen1... · la versi´on m´as...

Capıtulo 4

Trabajo realizado

4.1. Analisis y adaptacion de la Motion StrategyLibrary(MSL) (comun)

La Librerıa de Estrategia de Movimientos (MSL) permite desarrollar yhacer pruebas de algoritmos de planificacion de movimiento para una granvariedad de aplicaciones. La arquitectura software esta orientada a objetossiendo su diseno general altamente modular. Esta desarrollada en un sistemaLinux usando GNU C++, STL (Standard Template Library de C++) eincluye una interfaz grafica (GUI) desarrollada con la FOX GUI Toolkit.

La MSL se encuentra disponible como Codigo Abierto (Open Source),su distribucion es gratuita tanto para uso academico como comercial. Estoes ası pues su objetivo es difundir el interes y contribuir al desarrollo de losalgoritmos de planificacion de movimientos.

La MSL incluye planificadores basados en RRT, PRM y FDP (For-ward Dynamic Programming). Aunque estan mas desarrollados los basadosen RRT, incluyendo una cantidad de variantes (algoritmos bidireccionales,etc.).

La version mas actual que existe es la MSL 2.1, que soluciona algunosproblemas de compatibilidad con la librerıa de desarrollo de GUIs FOX.

4.1.1. Instalacion de la MSL

Tal y como la vamos a usar para integrarla en la arquitectura CROMAT,solo hace falta anadir el codigo fuente de los planificadores, la geometrıa y losmodelos a usar. Una vez adjuntados se compilan con ayuda de un Makefiley ya con eso se obtienen los objetos que seran necesarios incluir.

En cambio, para usar la MSL con todas sus caracterısticas, es necesarioinstalar una serie de librerıas que son:

El compilador GCC. Es un compilador de software libre de c++.Vital por supuesto a la hora de construir el ejecutable. La version con

39

CAPITULO 4. TRABAJO REALIZADO 40

la que se ha desarrollado la librerıa MSL es la 3.4.

La librerıa PQP. Es una librerıa para deteccion de colisiones en en-tornos bidimensionales y tridimensionales. Ha sido desarrollada por launiversidad de Carolina del Norte. Para conseguirla, tuvimos que man-dar un email a dicha universidad para conseguir descargar el codigofuente. La universidad nos lo facilito sin problemas y con gran pronti-tud.

La librerıa FOX. Es una librerıa muy extendida para el diseno deGUIs. Lo malo de esta librerıa es que existen versiones que no tienenretrocompatibilidad. Para instalar la msl-2.0 tuvimos que conseguiruna version bastante antigua de la librerıa FOX (v 1.0.53). Con laversion msl-2.1 ya se puede utilizar las versiones nuevas de FOX(1.2+).

La librerıa OpenGL. La GUI de la MSL puede dar resultados muyvistosos de planificacion de rutas en entornos tridimensionales (verfigura ...). Para ello, se necesita una librerıa de renderizado. Antes denada, debes haber configurado la targeta grafica. Nosotros disponemosde una targeta grafica ATI Mobility Radeon 9000 e instalamos losdrivers en linux que estaban disponibles en la pagina de la tarjeta(www.ati.com). Una vez hecho esto, instalamos la librerıa Mesa (creo)disponible en los repositorios de Debian.

Librerıas OpenInventor y OpenGLPerformer Estas librerıas nolas hemos probado, sirven para obtener mejores resultados en las simu-laciones renderizadas. Existe una version de OpenInventor totalmentesoftware libre llamada Coin3d.

Una vez instaladas estas herramientas, debemos configurar la MSL paraque sepa donde se hallan ubicadas. Existe una herramienta ./configure

que deberıa hacer este trabajo, lamentablemente, no esta muy optimiza-da. La mejor manera de configurar la MSL es editando a mano el archivoMakefile.conf en la que le tienes que especificar la ruta de cada una de laslibrerıas ası como las opciones de compilacion pertinentes.

4.1.2. Analisis de la MSL

La estructura del programa, que no el diagrama de clases, se muestraen la figura 4.1 y se detalla a continuacion. Nosotros vamos a usar la MSLsin la GUI por lo tanto prescinderemos de dicha parte, aunque los primerosexperimentos los hicimos con la ayuda de la GUI. Hemos tenido que intro-ducir clases nuevas tanto en los solucionadores, donde hemos desarrolladonuevas variantes de PRM basado en visibilidad y de RRT, como en la partede geometrıa y del modelo. Todo esto se detalla en el apartado 4.1.3.


Figura 4.1: Estructura de la librerıa MSL. Los modulos con fondo sepia hansido modificadas.

Modelo: Contiene los simuladores incrementales que modelan el com-portamiento cinematico y dinamico de una variedad de sistemas mecanicos.Estas clases permiten a los algoritmos planificadores estimar el estadofuturo del sistema, dandole el estado actual, un intervalo de tiem-po(dado por ∆t) y la entrada de control aplicada al robot en ese inter-valo. Se ha implementado un nuevo modelo para simular el compor-tamiento del Romeo 4R, como se detalla en el proyecto fin de carrerade Roberto Ojeda [23].

Geometrıa: Esta parte se encarga de definir las representaciones ge-ometricas del robot y de los obstaculos del entorno. Estos metodospermiten a los algoritmos de planificacion determinar si existe colisionentre robots o entre un robot y los obstaculos. Se ha desarrollado unanueva clase de geometrıa en la que tanto el robot como el entorno sedescriben con una retıcula. Se detalla en el apartado 4.1.3.

Problema: Es una clase que sirve como interfaz al planificador. Conesto se consigue que el disenador del algoritmo se abstraiga de losdetalles del detector de colisiones y las simulaciones dinamicas. Cadainstancia de un problema incluye referencias a instancias tanto de unmodelo como de una geometrıa en particular. Ademas, para completar


el problema, se debe especificar tanto el estado inicial del sistema comoel estado final deseado.

Solucionador: Actualmente solo hay un tipo de solucionador, las vari-antes son clases hijas del mismo. Se inicializa con una instancia de unproblema y tiene un metodo que resuelve dicho problema (Solve()).Para los algoritmos basados en mapas de carretera, tambien es nece-sario el metodo (Construct()) que construye el mapa de carreterassobre el que se efectuara la busqueda.

Escena: Es una clase interfaz que computa las configuraciones de to-dos los cuerpos para que sean representados graficamente por la clasede renderizado. Esta clase recibe la mayor parte de la informaciondirectamente del problema, pero incluye informacion adicional con-cerniente a la representacion, como el punto de vista de la camara.

Renderizado: Esta jerarquıa de clases contiene diferentes implementa-ciones de peticiones de renderizado grafico. Por ejemplo, cuando la GUIpide que se anime un camino solucion, un metodo de esta clase mues-tra los cuerpos en movimiento usando las configuraciones obtenidaspor la clase Escena. Cada clase derivada de renderizado usa un difer-ente sistema grafico. En el presente existen renderers que utilizan laslibrerıas SGI IRIS Performer, Open Inventor y Open GL.

GUI:Permite al usuario formular gran variedad de tipos de problemasusando solucionadores diferentes. Ademas tiene ventanas que dan in-formacion del camino conseguido y muestran animaciones del mismo.

4.1.2.1. Generando nuevos problemas en la MSL

Tal y como viene codificada la MSL, los constructores de las clases nece-sitan una serie de archivos que definen el problema que se desea abordar.Puedes intentar resolverlos una vez definidos usando la GUI que te vienedesarrollada (plangl), o usando otro ejecutable sin GUI (tests/nogui.C)que te devuelve el camino solucion directamente impreso en pantalla. Ambosejecutables usan por defecto el planificador RRTConCon, para usar otro, esnecesario modificar el codigo fuente.

Cada problema nuevo de la MSL debe estar localizado en un directoriopropio, a su vez, cada dato del problema se adjunta en un fichero propio, deforma que los datos que son opcionales no tienen porque tener su archivo.Los datos se introducen en modo texto, por lo que pueden ser generadoscon cualquier editor. En las siguientes tablas, se indican los archivos masimportantes y su funcionalidad.


Archivo Significado Valor Ob

GeomDim Dimension del espacio geometrico 2 o 3 SıGeomPQPXDRigid[Multi] Librerıa para detectar colisiones. Vacıo Sı

Multi se usa para planificacion multirobotX es la dimension del espacio.

LowerWorld Contiene con las coordenadas mınimas. Vector SıUpperWorld Contiene con las coordenadas mınimas. Vector SıGoalState Estado a alcanzar.Si hay mas de un Vector Sı

robot, se ponen los vectores de todosen la misma lınea.

InitialState Estado inicial. Vector SıObst Forma de los obstaculos. Cada lınea es Conjunto Sı

un obstaculo. Ej → (0, 0)(20, 15)(30, 0). de puntosRobot[X] Forma del robot. Si hay mas de Conjunto Sı

uno, se indica el no(de 0 a N-1). de puntosLowerState Estado ınfimo de los robots. Vector NoUpperState Estado maximo de los robots. Vector NoModel... Indica el comportamiento del robot. Vacıo SıModelDeltaT Incremento de tiempo del modelo. Numero NoPlannerDeltaT Tiempo de variacion de la entrada. Numero No

Cuadro 4.1: Tabla de archivos para configurar un problema con la MSL


Archivo Descripcion Cabecera

ModelLinear Robot rıgido holonomico model2d.h

Model2DPoint Robot puntual holonomico model2d.h

Model2DRigid Robot rıgido holonomico model2d.h

Model2DRigidCar Coche rıgido holonomico model2d.h

Model2DRigidCarSmooth Coche rıgido holonomico con model2d.h

variacion de angulo suaveModel2DRigidCarSmoothTrailer Como el anterior cargado model2d.h

Model2DRigidCarSmooth2Trailer Con 2 cargas model2d.h

Model2DRigidCarSmooth3Trailer Con 3 cargas model2d.h

Model2DRigidDyncar Modelo dinamico poco realista model2d.h

Model2DRigidDyncarNtire Mejora del anterior model2d.h

Model2DRigidLander Modulo de alunizaje model2d.h

ModelCarDynRollover Modelo avanzado de coche modelcar.h

Presiones de las ruedas por separadoModelCarDynSmoothRollover Mejora del anterior modelcar.h

Model3DRigid Modelo 3D basico model3d.h

Model3DRigidMulti Modelo 3D multiples robots model3d.h

Model3DRigidChain Modelo de manipuladores model3d.h

Model3DRigidTree Para cuerpos arboreos model3d.h

Model3DDyn Cohete con 3 propulsores model3ddyn.h

Cuadro 4.2: Modelos implementados por defecto en la MSL


Nombre de la clase Descripcion Cabecera

PRM PRM. prm.h

RRT RRT simple. rrt.h

RRTCon RRT que usa Connect. rrt.h

RRTGoalBias RRT que toma como muestr con unacierta probabilidad la configuracionobjetivo.

rrt.h

RRTGoalZoom Muestrea con cierta probabilidad enuna region cada vez mas pequena enel entorno del objetivo.

rrt.h

RRTConCon Connect para explorar y conectarlos arboles.

rrt.h

RRTBidirBalanced RRTConCon que de prioridad deexploracion al arbol mas pequeno encada iteracion.

rrt.h

RRTExtCon Extend para explorar, Connect paraconectar los arboles.

rrt.h

RRTExtExt Extend para explorar y conectar losarboles.

rrt.h

RCRRT RRT que aprovecha la informacionde cada intento de expansion.

rcrrt.h

RCRRTExtExt RRTExtExt que aprovecha la infor-macion de cada intento de expan-sion.

rcrrt.h

Cuadro 4.3: Planificadores PRM y RRT implementados por defecto en laMSL

Hemos desarrollado unos cuantos ejemplos de problemas que puedenser resueltos con ayuda de la librerıa MSL. Hemos probado con diferentestipos de modelos, con entornos bi y tridimensionales y con problemas decoordinacion de robots.

4.1.3. Adaptacion de la MSL

4.1.3.1. Geometrıa basada en retıculas

La librerıa MSL usa el paquete PQP (Proximity Query Package) para ladeteccion de colisiones. Este paquete indica las posibles colisiones entre dosconjuntos de polıgonos (en 2D) de cualquier numero de lados o de poliedros(en 3D) cuyas caras deben estar definidas por triangulos. Dentro de la MSLexiste los archivos geompqp.h y geompqp.cpp que sirven de interfaz entrela PQP y la MSL.

Lamentablemente, nosotros no vamos a trabajar con una descripcion del


entorno basada en polıgonos sino en una retıcula, por lo que no podemos usardicho paquete para detectar colisiones. Hemos desarrollado dos archivos degeometrıa basada en retıculas(grids) (geomgrid2d.h y geomgrid2d.cpp)que se incluiran en nuestra version de la MSL.

Estos archivos cargan el mapa y la forma del robot cuando esta orientadohacia el norte que estaran almacenados en formato ASC A e implementanlas funciones que indican si una configuracion del robot es valida. Hemostenido que implementar una funcion de rotado para detectar correctamentelas colisiones, ya que el robot puede tener una orientacion cualquiera.

4.1.3.2. Rotado de retıculas

Todo planificador por muestreo necesita de un modulo de deteccion decolisiones para independizar la planificacion del espacio de obstaculos, comose mostro en la figura 1.2.

En el caso de la MSL, como vimos anteriormente en el apartado 4.1.3.1,la deteccion de colisiones se realiza usando la librerıa PQP, que no se po-dra aprovechar, tal como vimos en dicho apartado. En nuestro proyecto elespacio de trabajo se encuentra codificado como una retıcula bidimensional,lo que hace necesario implementar un algoritmo de deteccion de colisionesespecıfico para el cual es necesario un algoritmo de reotacion de retıculas.

Existen muchas formas de implementar esta rotacion, desarrolladas prin-cipalmente por investigadores en el campo del tratamiento de imagenes.

En [21] se expone un algoritmo rapido de rotacion que mantiene unabuena calidad tras las transformaciones.

El algoritmo se basa en la descomposicion de la matriz de rotacion bidi-mensional en un producto de tres matrices que operan unicamente por filaso columnas.

R(θ) =

[

cos(θ) − sin(θ)sin(θ) cos(θ)

]

=

[

1 − tan( θ2)

0 1

]

×

[

1 0sin(θ) 1

]

×

×

[

1 − tan( θ2)

0 1

]

(4.1)

Esta transformacion permite descomponer la rotacion en tres convolu-ciones unidimensionales que realizan un simple desplazamiento ∆x = −y ∗tan( θ

2), ∆y = −x ∗ tan( θ

2) de cada fila/columna proporcional al tamano

vertical/horizontal de la retıcula. Ademas, al tener el determinante de lasmatrices valor 1, las transformaciones no escalan en ningun momento laretıcula.

En la siguiente figura se pueden apreciar las tres transformaciones quedan lugar al rotado de la retıcula.


Figura 4.2: Tres trasnsformaciones unidimensionales que confor-man la rotacion bidimensional

Cada una de las tres convoluciones pueden representarse como el pasopor un filtro digital tipo FIR como el de la figura:

Figura 4.3: Representacion de la convolucion undimensional co-mo filtrado FIR.

De entre las funciones de interpolacion, y puesto que para nuestra apli-cacion se debe dar mas peso a la rapidez de ejecucion del algoritmo que ala calidad del resultado, elgimos una B-spline de grado 1, que no necesita


prefiltro, al ser b0 = 1.

ϕ = β1 =

{

1 − |x|, |x| < 10, en otro caso.

(4.2)

El filtro de transformacion resultante t∆ = β1(k − ∆) aplicado a unafila o columna de la retıcula da como resultado una simple ponderacion delos valores de dos celdas contiguas en cada celda resultante, realizando portanto solo dos multiplicaciones y una suma por cada celda.

El metodo de la clase C2DOccupancyGrid

bool rotate(θ, *j, *i) (4.3)

gira la retıcula de la instancia desde la que se llama un angulo θ ∈ [−π, π]en sentido antihorario, devolviendo false en caso de error y modificandoconvenientemente la cabecera donde se almacenan los datos del tamano dela retıcula. Este metodo recibe ademas dos punteros que pueden usarse pararealizar un rastreo de un celda concreta de la retıcula, que variara de posicionpor la naturaleza de la transformacion.

La implemetencion del algoritmo es valida solo para giros de θ ∈ [−π4, π

4]

lo cual no supone un problema a la hora de realizar giros con angulos may-ores. Para extender el rango de angulos a θ ∈ [−π, π] se aprovechan los casosespeciales de los giros de θ = {0, π

2, π,−π

2,−π} para los que no es necesario

aplicar el algoritmo de rotado, bastando con reordenar las celdas adecuada-mente. Por lo tanto, rotate es una extension del algoritmo principal, quese implementa en rotate45 y que usa otros metodos que realizan los girosde los casos especiales.

4.1.3.3. La herramienta BMP2ASC

Para facilitar la tarea de creacion y el visionado de mapas, decidimosgenerar una herramienta capaz de convertir archivos de formato ASC aBMP y viceversa.

Para leer y escribir archivos BMP hemos usado el proyecto EasyBMP

(http://easybmp.sourceforge.net/) que permite leer, escribir y modi-ficar archivos BMP de forma facil. Es una herramienta que intenta ser mul-tiplataforma. Ha sido satisfactoriamente probada en Linux(kernels 2.4.x y2.6.x con g++ e icc), Windows(XP Pro, XP Home Y 2K Pro con MinGW/g++y Borland) y maquinas Sun Sparc 4 (con S.O. Solaris 5.9 y compilador g++).

Para incluir dicha herramienta, solo tuvimos que anadir al proyecto losarchivos EasyBMP.cpp y EasyBMP.h y compilarlo. No tuvimos mayor prob-lema.

Como hemos visto en el apendice A el mapa en memoria se almacena enla clase C2DOccupancyGrid (por el momento solo trabajamos con mapas


bidimensionales). Y esta clase tiene metodos para leer y escribir archivoscon formatos ASC y BMP.

Por tanto a la hora de disenar este programa, solo tenemos que usardichos metodos de forma adecuada para transferir la informacion entre unoy otro formato. El programa puede leer archivos BMP de 8, 16 y 24 y 32 bitspor pixel, generando un valor de probabilidad en coma fija sin parte enteracon dichos bits. A la hora de escribir archivos BMP, se generan archivos enescala de grises con 8 bits por pıxel.

El formato de las llamadas a los ejecutables ASC2BMP y BMP2ASC es:

ASC2BMP <origen> <destino>

BMP2ASC <origen> <destino> [<xllcorner> <yllcorner>

<cellsize>]

Si no se especifica el valor de los parametros de la cabecera en BMP2ASC,los valores de xllcorner e yllcorner son 0.0 y el de cellsize es de 1.0.

Por ultimo destacar que como la informacion almacenada en cada celdaes una probabilidad de ocupacion de la misma, su valor esta comprendidoentre 0 y 1. A la hora de convertir dicho valor en formato BMP, usamos unarepresentacion en coma fija, en la que los bits del valor del pıxel indican laparte fraccionaria del numero. Lo hemos hecho ası por simplicidad y porqueera mas que suficiente para nuestros propositos.

4.2. Implementacion de las variantes mas aptaspara el proyecto

Como se ha visto en 4.1.3, la implementacion del algoritmo PRM dentrode la MSL se reduce al algoritmo basico (seccion 2.2), en el que puedes selec-cionar diferentes muestreos, como el basado en cuadrıcula Sukharev (2.3.2),los basados en las secuencias Halton y Hammersley (2.3.3) y muestreo aleato-rio.

Ya se han visto los inconvenientes del algoritmo PRM basico para nue-stros propositos. Basicamente, su fase de preprocesado genera grafos condemasiados nodos y pocas conexiones entre sı. Esto hace que los caminos

seleccionados sean un tanto aleatorios y bastante poco optimos. Esto sedetalla en la figura 4.4.


Figura 4.4: Grafo generado por el algoritmo PRM original y un ejemplo deun camino solucion.

Como se puede observar en la figura, el grafo generado tiene demasiadosnodos y pocas conexiones. Esto puede ser bueno para situaciones en las quese manejen robots con altos grados de libertad, pues los chequeos de colisionson los que mas carga computacional exigen. En nuestro caso, dada la bajadimensionalidad del espacio en el que trabajamos (trabajamos en el plano)queremos un grafo mas reducido, con menos nodos y mas conexiones entreellos.

De entre las opciones que se han barajado, hemos optado por implemen-tar el algoritmo PRM basado en visibilidad.

4.2.1. Implementacion del algoritmo PRM basado en visibil-idad

Para obtener grafos con menos nodos y mas interconexiones hemos deci-dido implementar la variante del PRM basado en visibilidad (vease seccion2.5.1). Este metodo permite la generacion de grafos mas pequenos y conmas conexiones. Ademas, en el trabajo en el que se propuso, se sugirio queen lugar de realizar un muestreo aleatorio, se realizara uno basado en lasecuencia Halton 2.3.3.

Un grafo generado con el metodo antes descrito se muestra en la figura4.5.


Figura 4.5: Grafo generado por el algoritmo PRM basado en visibilidad conmuestreo basado en secuencia Halton. A la derecha, un ejemplo de un caminosolucion.

Se ve que el grafo generado tiene muchas mejores propiedades. Es ungrafo con bastantes menos nodos y con una mejor interconectividad entreellos. Ademas, la solucion proporcionada al problema es bastante mejor quela anterior, no tiene ya bucles siendo por tanto mucho mas directa.

El grafo generado todavıa tiene algunas propiedades que mejorar, porejemplo, como se vio en 2.5.1 no tiene bucles, lo cual hace que no se optim-icen los caminos del todo. Para solucionar esto, se ha disenado una funcionRefine() que una vez realizado el grafo, intenta hacer interconexiones en-tre los nodos y los nodos vecinos al mismo. Para que sea util, debemos deaumentar la distancia de vecindad.

Figura 4.6: Grafo antes y despues de hacer el proceso Refine (cerrar losbucles).


No obstante, el grafo se ha trazado muestreando el mapa de una for-ma cuasi-aleatoria, sin tener en consideracion la informacion que se tienedel terreno. Por tanto, lo siguiente que se implemento fueron metodos demuestreo que tengan en cuenta la informacion almacenada en el mapa.

4.2.2. Muestreo en los bordes

El grafo generado con la secuencia Halton, puede tener puntos demasiadopegados a los obstaculos, lo cual no es deseable a la hora de guiar un robot.Ademas nos pueden dar caminos solucion en los que el robot va dandobandazos (ver figura 4.5. Por tanto hemos disenado algoritmos que tenganen cuenta la geometrıa de los obstaculos.

Al hacer muestreo de acuerdo con la geometrıa, los grafos ganan enpropiedades de seguridad, pero necesitamos incluir un procesado del mapadel terreno. Esto hace que el tiempo empleado para generar los grafos seincremente bastante (5).

Vimos en los tipos de muestreo 2.4.5 que si eliges puntos para introducir-los al grafo de forma que esten muy cerca de los obstaculos obtienes caminosoptimos en distancia. En nuestro caso elegimos un muestreo en los bordesde los obstaculos.

Ahora bien, el tamano del camino no es lo unico que importa. Queremosrealizar un muestreo que tenga unas buenas propiedades de seguridad y deefectividad, por tanto no muestrearemos directamente en los bordes de losobstaculos, si no que haremos un engordado artificial de los mismos unadistancia de seguridad.

La eleccion de la distancia de seguridad es un asunto peliagudo. Debeser suficiente para que las rutas trazadas sean viables. Pero no puede serdemasiado, ya que puede hacer que dos obstaculos se solapen.

Esto, que en principio es un problema, se ha aprovechado en el algoritmo.Si existe un solapamiento entre dos obstaculos, entonces no se muestrea enel borde del mismo, si no que se muestrea en la zona de conflicto. Haciendoel muestreo ası, aunque se incumpla la premisa de distancia de seguridad, seconsigue un muestreo en las areas mas lejanas a dos obstaculos demasiadoproximos.


Figura 4.7: Obstaculos antes y despues de la expansion. En negro, las zonasdonde existe un solapamiento entre obstaculos.

Se ha realizado el mismo experimento que en los dos metodos anterioresy el resultado obtenido es:

Figura 4.8: Grafo generado por el algoritmo PRM basado en visibilidad conmuestreo basado en secuencia Halton. A la derecha, un ejemplo de un caminosolucion.

4.2.3. Muestreo en las zonas solapadas

Esta clase de muestreo disenado se basa en llevar al extremo el muestreoanterior.

Su mentalidad es simple: ir inflando los obstaculos hasta que haya so-lapamiento y una vez hecho esto, muestrear solo en la zona solapada. Con


esto se consigue una aproximacion del eje medio 2.4.4 para muestrear soloen dicha zona que es la mas alejada de los obstaculos.

Figura 4.9: Proceso de obtencion de la region mas alejada de los obstaculospor el solape de los mismos.

El algoritmo desarrollado va inflando los obstaculos poco a poco obte-niendo zonas solapadas, ası se van muestreando primero las zonas mas con-flictivas. Se para cuando ha inflado los obstaculos un numero indicado deveces. El grafo obtenido se indica en la figura 4.10.


Figura 4.10: Grafo generado por el algoritmo PRM basado en visibilidadcon muestreo basado en el solape entre obstaculos. A la derecha, ejemplo decamino solucion.

Tanto el grafo como la solucion se puede observar que tienen una calidadmas que aceptable. Tiene muy pocos nodos y los caminos que genera son lomas seguros posible. Esto es muy importante a la hora de guiar fiablementeun robot.

Figura 4.11: Grafo generado por el algoritmo PRM basado en visibilidadcon muestreo basado en el solape entre obstaculos. A la derecha, ejemplo decamino solucion.

4.2.4. Camino mas corto

Esta forma de construir es bastante diferente a las anteriores. No tieneen cuenta el algoritmo del PRM basado en visibilidad.


En primer lugar forma los nodos del grafo formando anillos bastantedensos de puntos entorno a los obstaculos con una distancia de seguridadconseguida de la misma forma que en 4.2.2. Entre punto y punto hay hayuna distancia mınima que puede ser configurada. Esta parte del proceso seobserva en la parte izquierda de la figura 4.10.

Despues usa la funcion Refine() para unir todos los nodos que se puedanentre sı y por ultimo usa la funcion Refine2() para completar los buclesque sean necesarios.

Figura 4.12: Proceso de generacion del grafo por el metodo del camino mascorto.

El problema de esta forma de generar el grafo es que se generan de-masiados nodos y conexiones, se podrıa refinar eliminando los que sean masredundantes.

4.2.5. Otras mejoras del grafo

Ya hemos visto como los tipos de muestreo implementados y el tipo dealgoritmo de generacion de los mismos que vamos a usar. Se han mostradoejemplos de grafos generados con cada tecnica de muestreo y visto que losresultados son bastante aceptables.

A la hora de generar un grafo completo para integrarlo en la arquitec-tura CROMAT, hacemos varias llamadas al constructor de grafos. Primerogeneramos un grafo con alguno de los tipos de muestreo vistos en las sec-ciones anteriores, despues anadimos los caminos posibles para formar buclesy por ultimo se hace otra llamada al constructor de grafos pero esta vez conun muestreo basado en cuadrıcula Sukharev (2.3.2). Esta parte sirve paraobtener muestras de zonas en las que no haya mucha densidad de obstacu-los, ya que con muestreo en las zonas solapadas o en los bordes te dejas


muchas zonas sin muestrear. En caso de muestreo Halton se realiza ası paracompletar posibles deficiencias en el grafo.

Algoritmo 8 Preprocesado Complejo (T ipodemuestreo,N,∆T )

1: Preprocesado Visibilidad (T ipomuestreo,N,∆T );2: Anadido de caminos3: Preprocesado visibilidad (Sukharev,N,∆T ;4: Anadido de caminos5: Completado de bucles

La ultima parte del algoritmo se usa porque aunque anadamos caminosredundantes con el metodo descrito en 4.2, es posible que tengamos la malasuerte de que necesitemos anadir un nodo para completar un bucle satisfac-toriamente, esto se muestra en la figura 4.13.

Para solucionar este problema, se ha disenado un ultimo metodo llamadoRefine2(k) para completar los bucles. Su filosofıa es parecida a la empleadaen el algoritmo Reachability Roadmap (2.5.2), se intenta anadir un nodoentre otros dos nodos inconexos que cumplan:

k ∗ ρ(v, v′) < G(v, v′) (4.4)

La zona en la que se intentan anadir los nuevos nodos es un cuadradocentrado en el punto medio de los dos nodos y que tiene como lado unacuarta parte mas que la distancia entre ellos.

Es decir, que la relacion entre el camino recorriendo el grafo entre ambosy su distancia sea mayor que una cierta constante, cuyo valor recomendamosque este entre 4 y 10, segun el tamano de los obstaculos, para que el procesono sea demasiado largo. En la figura 4.13 se muestra un ejemplo de aplicacionde la anterior funcion.


Figura 4.13: Grafo antes y despues de hacer el proceso Refine 2(cerrar losbucles). Se detalla el bucle incompleto.

4.2.6. Algoritmo de consulta

Se ha tenido que implementar una variante del algoritmo de busquedade Dijkstra 2.2.2 para obtener el camino optimo en distancia recorrida.

En la implementacion inicial, basicamente el algoritmo es igual con unasencilla diferencia: los puntos inicial y el final de la consulta no puedenpertenecer al grafo. Por tanto, debemos ver los nodos vecinos al grafo deambos e intentar conectar dichos puntos a sus vecinos. Se encuentra en lafuncion Plan(mincost) del archivo MSL/prm visibility.cpp y te devuelvela longitud total del camino hallado.

Una vez conectado el inicial a sus vecinos, a dichos nodos se les pone comocoste la distancia entre ellos y el punto inicial. Despues pasa a ejecutar elalgoritmo de Dijkstra, aunque no hay un unico nodo objetivo, si no los nodosobjetivo son los vecinos al punto final. El algoritmo se acabara cuando sehaya obtenido el coste de todos dichos nodos.

Al final, solo queda recuperar el camino, lo que se consigue con ayu-da de un mapa de conceptos que liga cada nodo del grafo con su prece-dente(parentMap). Y ver el camino que al final te da menos coste teniendoen cuenta la distancia desde los nodos vecinos al objetivo.

Aunque el algoritmo presentado sirve para resolver problemas de tipoholonomo, se han introducido unas cuantas mejoras en la funcion de con-sulta para hacer mas probable que con un suavizado o con una consultaal algoritmo RRT se consiga hallar un camino solucion. La idea es que elalgoritmo de soluciones en las que se tenga un poco en cuenta algunas delas restricciones de los robots con configuracion Ackerman.

La primera modificacion tiene en cuenta la orientacion inicial del robota la hora de hallar los nodos vecinos al punto inicial. Solo podran ser nodos


vecinos al incial los puntos que esten en el semiplano indicado. Ademas, seexige que tengan una distancia mınima con el punto inicial.

La ultima modificacion tiene como objetivo reducir la aparicion de caminosexcesivamente angulados penalizando aquellos tramos entre los cuales elangulo entre los dos tramos del camino es mayor que un cierto angulo. Estose puede ver en la figura 4.14.

Figura 4.14: Ejemplo de camino excesivamente angulado.

4.3. Integracion en la arquitectura CROMAT

El escenario donde se han implementado los algoritmos elegidos es laarquitectura CROMAT [22]. Se trata de una arquitectura disenada para elcontrol, comunicacion y coordinacion entre multiples robots heterogeneosestructurada en capas o niveles de abstraccion que permite que desde losniveles mas altos se trate de forma similar a los diferentes robots, a pesarde sus diferencias.

Esta arquitectura, disenada por Antidio Viguria e Ivan Maza, se presentaesquematizada en la siguiente figura:


Figura 4.15: Diagrama de bloques de la arquitectura CROMAT

En la figura se observa en detalle la division en capas de un robot, ası co-mo las partes que componen el resto de la arquitectura, a saber el ControlCenter, el Time Server y el proceso 3DRepresentation.

Para la realizacion del proyecto, exceptuando las capas de cada robot,solo ha sido necesario modificar levemente la entidad Control Center paraincluir un nuevo tipo de tarea de movimiento dentro de las posibles tareas, latarea PRM GOTOXYZ. Recibe como parametros las coordenadas de la posicionobjetivo y una velocidad media deseada en m/s. Su mision es llevar al robotdesde la posicion actual hasta la final (notese que no se indica la orientacionfinal) evitando los posibles obstaculos usando los algoritmos PRM y RRT.

Para hacer esto posible, se ha incluıdo en la interfaz GRI dicha tarea yse han modificado los archivos robot architecture/comms/GRI data.h y


robot architecture/control centre/source/HMITerminal.cpp.La parte mas importante, sin embargo, es la de los niveles de abstraccion

de cada robot donde se han ubicado los diferentes modulos que implementanlos algoritmos PRM y RRT para su uso como planificadores de movimientodentro de la arquitectura.

El algoritmo PRM se ha implementado en la capa RAL de la arquitec-tura. Esta es la capa con mayor abstraccion de la arquitectura y se pretendeque su implementacion sea independiente del robot en la medida de lo posi-ble. El algoritmo disenado es casi totalmente independiente del robot si estees terrestre, aunque se puede configurar para darle una distancia de seguri-dad del grafo a los obstaculos que sera mayor en robots mayores y masrapidos.

A continuacion se muestra un esquema sencillo de la ubicacion de losmodulos en la arquitectura con una breve descripcion de las senales quedichos modulos se envıan:


Figura 4.16: Diagrama de bloques de la ubicacion en la arquitec-tura CROMAT de los modulos PRM y RRT.

La ubicacion de cada modulo viene determinada por la dependencia deestos con el nivel fısico del robot. Ası, el modulo PRM, independiente delrobot, se ubica en la capa RAL(Robot Abstraction Layer) mientras que elmodulo RRT se ubica en la MML (Module Management Layer), que dependedel robot, pero no se ocupa de la implementacion del control del mismo sitaen la RIL (Robot Implementation Layer) sino que usa los modulos que estaproporciona.

Como puede verse en la figura, los modulos PRM y RRT no tienencomunicacion directa puesto que esta no es necesaria. Recordemos que el


modulo PRM genera un grafo de nodos de visibilidad a partir de un mapadel entorno, y posteriormente con la informacion de la posicion del roboty los parametros de la tarea PRM GOTOXYZ obteniene una lista de puntosdel grafo que lleva desde la posicion actual del robot hasta la objetivo. Elalgoritmo RRT no necesita mas informacion para cumplir su trabajo quedicha lista de puntos, pues tiene acceso al mapa del entorno1.

Por lo tanto la estrategia que se sigue es la independizacion de am-bos modulos mediante los modulos ya existentes Task Manager y Module

Manager. Estos dos modulos se encargan de la gestion de las tareas y losmodulos PRM y RRT tienen la unica funcion de calcular un camino seguropara el robot.

De este modo, el procedimiento habitual de realizacion de una tarea demovimiento PRM GOTOXYZ se divide en:

GRI → Task Manager : Peticion de nueva tarea PRM GOTOXYZ.

Task Manager → Modulo PRM: Peticion de nueva tarea de movimien-to

Modulo PRM → Task Manager : Lista de puntos solucion a la tarearequerida.

Task Manager → Module Manager : Peticion de tarea en forma de listade puntos.

Module Manager → Modulo RRT: Peticion de tarea de lista de puntos.

Modulo RRT → Module Manager : Camino solucion de la tarea re-querida.

Module Manager → HAM: Ordenes necesarias para completar la tareafısicamente.

Informacion hacia arriba del estado de la tarea.

Este serıa el desarrollo normal de funcionamiento a la hora de resolver elproblema. Sin embargo, cabe esperar que ocurran sucesos en el transcursode la misma que modifiquen o anulen por completo la tarea por distintasrazones, como pueden ser la imposibilidad de llevarla a cabo por cuestionesfısicas, la indisponibilidad de algun modulo o una peticion del usuario o elsistema de abortar la tarea.

Ademas puede ocurrir que dentro de la lista de puntos solucion del modu-lo PRM, al ser esta independiente de la configuracion del robot, existan dospuntos consecutivos imposibles de conectar para el robot concreto (en nue-stro caso Romeo4R). En estas situaciones resulta conveniente que el modulo

1El acceso al mapa del entorno queda fuera del ambito de este proyecto, por lo que se

tomara dicho mapa desde un fichero en formato ASC.


RRT informe al modulo PRM de cuales han sido esos puntos, para que seprocuren evitar al reintentar la tarea de movimiento.

Por lo tanto se necesita un protocolo para la comunicacion entre losdistintos modulos que intervienen que tenga en cuenta estas posibles situa-ciones. En el apartado 4.3.3 se describira con detalle.

4.3.1. Implementacion del PRM Module

Como cualquier otro modulo perteneciente a la capa RAL, debe de imple-mentarse como un hilo que se comunica con los demas mediante una seccioncrıtica. El comportamiento del hilo se define en robot architecture/RAL/source/PRM modulethread.cpp

y llama a los metodos de la clase PRMM(modulo PRM) definida en robot architecture/RAL/source/PRM

Su comportamiento es:

Algoritmo 9 Comportamiento del modulo PRM

1: Configuracion del modulo indicado en el archi-vo robot architecture/RAL/prm module.conf

(STATE=GENERATING GRAPH)2: loop

3: if Llega peticion then

4: Obtener camino optimo5: Mandar respuesta

4.3.2. Configuracion del PRM Module

Como hemos visto en la descripcion del algoritmo PRM 2.2, tiene unafase de preprocesado en la que se genera el mapa de carreteras con el quese resolveran los problemas. Esta fase se realiza nada mas ejecutar la arqui-tectura y el que se genere un grafo correcto es indispensable para un buenfuncionamiento del modulo. Para configurar la forma de realizar el grafoexiste un archivo de ruta: robot architecture/RAL/prm module.conf.

El algoritmo y los muestreos implementados se detallan en 4.2.1 basica-mente se hara un grafo PRM de visibilidad. El algoritmo de generacion delgrafo completo viene en 4.2.5.

Es posible configurar dicho modulo para que genere un grafo segun tecni-cas diferentes de muestreo, como muestreo en los bordes o muestreo en el ejemedio. Tambien es posible configurar la distancia de seguridad, el tamanodel robot que se probara y el numero de puntos que se intentaran anadir almapa. Ası como la maxima distancia de interconexion entre nodos del grafo.

En el caso de que se elija el muestreo SHORTEST PATH


Opcion Descripcion

MinDistance Distancia mınima entre dos nodos del grafoConnectionDistRefine Distancia maxima para anadir un caminoMaxNeighborDist Distancia maxima entre dos nodos vecinosNumNodes Numero de nodos que se intentaran anadir en la

fase del muestreoType of Sample Tipo de muestreo en la primera fase

Los valores posibles se indican en la tabla 4.5Points Per Axis Sukharev Puntos por eje en la

segunda parte del muestreoSecurity Distance Distancia con la que se inflaran los obstaculos

en la primera parte del muestreo(solo muestreo en los bordes)

Number of inflates Numero de veces que se inflaran los obstaculosen la primera parte del muestreo solosi se elige muestreo en el eje medio

Inflate Distance Distancia de inflado con muestreo en eje medio

Cuadro 4.4: Opciones disponibles en el archivo de configuracion del moduloPRM

Opcion Descripcion

RANDOM Muestreo aleatorioSUKHAREV Basado en cuadrıcula SukharevHALTON Muestreo usando la secuencia HaltonHAMMERSLEY Muestreo usando la secuencia HammersleyCOLLIDING EDGE SAMPLING Muestreo en el eje medioEDGE SAMPLING Muestreo en los bordes con distancia de seguridadSHORTEST PATH Tecnica del camino mas corto(muestreo en los bordes

con anillos muy densos

Cuadro 4.5: Tipos de muestreo disponibles en el modulo PRM


# Config file for the PRM module

# Configures the type of sample utilized to generate

# the graph and its characteristics

# Minimum distance between two nodes (if possible)

MinDistance: 3.5

# Maximum distance between two neighbors nodes

MaxNeighborDist: 10.0

# Maximum distance in the connection phase of the generation

ConnectionDistRefine: 40.0

# Number of nodes that will be tried to add

# in the first phase of the construction

NumNodes: 1000

# Type of sampling used in the first phase

# (in the next phases will be Sukharev)

# Can be: RANDOM, HAMMERSLEY, HALTON, SUKHAREV,

# EDGE_SAMPLING, COLLIDING_EDGE_SAMPLING

Type of Sampling: EDGE_SAMPLING

# In the edge sampling, the obstacles

# will be inflated a security distance

Security Distance: 5.0 # ONLY FOR EDGE_SAMPLING & SHORTEST_PATH

# In the colliding edge sampling the obstacles will be inflated a

# specified number of times

# Number of inflates: 8 # ONLY FOR COLLIDING_EDGE_SAMPLING

# Inflate Distance: 1.5 # ONLY FOR COLLIDING_EDGE_SAMPLING

# Points per axis in the sukharev grid. Used in the second sampling

Points Per Axis Sukharev: 400

Figura 4.17: Ejemplo de archivo de configuracion del modulo PRM.

En la seccion 5 se detallan los tiempos medios de calculo de cada al-goritmo y la varianza de los mismos en escenarios diferentes ası como lacomplejidad del grafo resultante. Es recomendable consultar dicho apartadopara una configuracion optima del modulo segun el tipo de escenarios.

A la hora de trabajar con el grafo, se han tenido que incluir varios meto-dos de gestion de los grafos que no se incluıan en la MSL. Por ejemplo, cuando


el modulo RRT indica que un camino es imposible de seguir, se deben elimi-nar nodos del grafo, lo cual no estaba implementado. Se han realizado variaspruebas a los metodos de forma que no existen errores ni fugas de memoriapudiendose modificar el grafo totalmente de forma simple.

4.3.3. Estructura de comunicacion

En el ambito de este proyecto, la capa RAL funciona como una cajanegra que tiene como entradas y salidas bidireccionales al Module Managerde la capa MML y al Control Centre.

En la siguiente figura se pueden ver los tipos de datos usados para lascomunicaciones internas de la RAL entre los modulos que nos interesan(Task Manager y PRM Module) ası como de esta capa con el exterior (lacapa inferior y el Control Centre, se entiende).

Figura 4.18: Estructuras de datos para la comunicacion por losmodulos de la RAL.

A continuacion explicaremos cada estructura de datos en detalle:


typedef struct

{

CromatTaskOperation request;

CromatTaskName taskName;

int sequenceNumber;

TimeSpec timeOut; /* task time-out */

union /* task parameters depending on the task*/

{

CromatWaypoint wp; /* CROMATTASK_GOTO_XYZ */

CromatWaypointsList wpl; /* CROMATTASK_GOTOLIST_XYZ */

CromatWaitParams wait; /* CROMATTASK_WAIT */

CromatHomeParams hom; /* CROMATTASK_HOME */

CromatPanTiltParams pt; /* CROMATTASK_PAN_TILT */

CromatTakeoffParams to; /* CROMATTASK_TAKEOFF */

CromatLandingParams lan; /* CROMATTASK_LANDING */

CromatGetImagesParams gi; /* CROMATTASK_GET_IMAGES */

CromatDetectParams det; /* CROMATTASK_DETECT*/

CromatTrackingParams track; /* CROMATTASK_TRACKING */

} taskParam;

int preconditions[MAX_TASK_DEPENDENCIES];

int postconditions[MAX_TASK_DEPENDENCIES];

} CromatTaskRequest;

Figura 4.19: CromatTaskRequest.Definida enrobot architecture/comms/GRI data.h

En nuestro caso:

request solo puede tomar por valores START o ABORT

taskName tendra siempre el valor CROMATTASK PRM GOTO XYZ

sequenceNumber tendra un numero unico para identificar la tarea.

taskParam.wp contendra el waypoint objetivo.


typedef struct{

int sequenceNumber; /* task id */

CromatTaskName taskName; /* task name */

CromatTaskStatus state; /* task status */

int errorCode;

bool error_wp[GOTOLIST_XYZ_MAXLENGTH]; /* Waypoints with errors in the list */

}TaskState;

typedef struct

{

TaskState state[CROMATBBCSTASK_MAXSTATE];

} CromatTasksState;

typedef enum

{

CROMATTASKSTATUS_EMPTY, /* empty task slot */

CROMATTASKSTATUS_SCHEDULED,/* task ready and about to start */

CROMATTASKSTATUS_RUNNING,/* task running */

CROMATTASKSTATUS_ABORTING,/* task about to abort */

CROMATTASKSTATUS_ABORTED,/* task terminated by an abort */

CROMATTASKSTATUS_ENDED, /* task terminated normally */

} CromatTaskStatus;

Figura 4.20: CromatTaskState y sus subestructuras. Definida enrobot architecture/comms/GRI data.h

La estructura de datos CromatTaskStatus se compone de un array deTaskState. Esto se debe a que en la arquitectura CROMAT se distinguen lasposibles tareas segun su naturaleza, de modo que se puede ejecutar mas deuna tarea a la vez si son de naturaleza diferente. Actualmente se distinguentareas de Percepcion, movimiento(las de nuestro tipo) y pan & tilt.

Nosotros nos fijaremos en el TaskState de la clase de tareas que nosconcierne (movimiento).

En nuestro caso:

sequenceNumber debe ser el mismo que el recibido en la CromatTaskRequestcorrespondiente.

taskName valdra siempre CROMATTASK PRM GOTO XYZ

state tendra uno de los valores posibles definidos en la estructuraCromatTaskStatus

errorCode valdra por defecto GRI NO ERROR


error wp No se usa.

typedef struct {

int sequenceNumber;

PRM_type_of_request type;

// The following field has effect if the type of request is a RETRY

CromatWaypoint wp_to_avoid;

CromatWaypoint wp;/* CROMATTASK_PRM_GOTO_XYZ */

}PRM_task;

typedef enum {

PRIMARY_TASK,

RETRY,

}PRM_type_of_request;

typedef struct {

int errorCode;

int sequenceNumber;

CromatWaypointsList path;

}PRM_path;

Figura 4.21: Estructuras de datos PRMTask, PRMPath y sus subestructuras.Definidos en robot architecture/comms/InternalRALComm.h

En nuestro caso:

type valdra PRIMARY TASK o RETRY

sequenceNumber sera el mismo que el de la CromatTaskRequest cor-respondiente.

wp waypoint objetivo del CromatTaskRequest correspondiente.

error valdra por defecto GRI NO ERROR.

path indicara la lista de nodos del camino si se ha encontrado solucional problema.


typedef struct AtomTaskRequest {

AtomTaskOperation request;

AtomTaskName taskName;

int sequenceNumber;

union {/* Parametro que depende del tipo de tarea*/

[...]

CromatWaypointsList wpl;

[...]

} taskParam;

} AtomTaskRequest;

Figura 4.22: AtomTaskRequest.Definida enrobot architecture/comms/MRI data.h

En nuestro caso:

request solo puede tomar por valores START o ABORT

taskName tendra siempre el valor TASK ATOM RRT GOTO LIST XYZ

sequenceNumber tendra un numero unico para identificar la tarea.

wpl contendra la lista de waypoints que se han calculado en el moduloPRM.

typedef struct AtomTasksState {

int sequenceNumber;

AtomTaskName taskName;

AtomTaskStatus state;

int errorCode;

bool error_wp[GOTOLIST_XYZ_MAXLENGTH];

} AtomTaskState;

typedef enum AtomTaskStatus {

ATOMTASKSTATUS_EMPTY,/* empty task slot */

ATOMTASKSTATUS_SCHEDULED,/* task ready and about to start */

ATOMTASKSTATUS_RUNNING,/* task running */

ATOMTASKSTATUS_ABORTING,/* task about to abort */

ATOMTASKSTATUS_ABORTED,/* task terminated by an abort */

ATOMTASKSTATUS_ENDED /* task terminated normally */

} AtomTaskStatus;

Figura 4.23: AtomTaskState.Definida en robot architecture/comms/MRI data.h


En nuestro caso:

sequenceNumber debe ser el mismo que el recibido en la AtomTaskRequestcorrespondiente.

taskName valdra siempre TASK ATOM RRT GOTO LIST XYZ

state tendra uno de los valores posibles definidos en la estructuraAtomTaskStatus

errorCode valdra por defecto RRT NO ERROR

error wp valdra true unicamente para los ındices correspondientes awaypoints en los que el modulo del algoritmo RRT no encontro solu-cion, si los hubiere. Para el resto valdra false.

4.3.3.1. Funcionamiento normal

En un funcionamiento normal, llega una tarea del tipo PRM GOTO XYZ

desde el Control Centre. Dentro de la tarea se indicara el punto objetivoal que se desea ir. El Modulo PRM calcula un camino con todos sus nodosvisibles entre sı y se lo indica al Task Manager. Este modulo lo envıa a lascapas bajas, si no existe otra tarea de movimiento en ejecucion, y espera aque se termine. Cuando esto ocurre, lo indica al Control Centre.

Control Centre → Task Manager : CromatTaskRequest con request

= START.

Task Manager → Modulo PRM : PRM Task con PRM task.type = PRIMARY TASK

Y wp indica el punto objetivo.

Modulo PRM → Task Manager : PRM Path con el camino almacenadoen el campo path, errorCode=NO ERROR GRI.

Task Manager → Module Manager : AtomTaskRequest . request =

START.

Module Manager → Modulo RRT: RRTRequest con request=START.

Module Manager → Task Manager : AtomTaskState con AtomTaskStatus

= ATOMTASKSTATUS RUNNING. Aquı empieza un proceso interno en laMML, que se describe en [23].


= ATOMTASKSTATUS ENDED


4.3.3.2. Cancelacion manual

Una tarea puede ser cancelada en cualquier momento mediante una ordenABORT indicando el numero de tarea a abortar. Si todavıa no habıa sidoprocesada, la tarea se aborta inmediatamente. En cambio, si el modulo Task

Manager ya ha enviado la tarea a las capas bajas, se les tiene que informarde que se va a interrumpir la tarea y esperar que la aborten.

Control Centre → Task Manager : AtomTaskRequest con request =

ABORT.

Task Manager → Module Manager : AtomTaskRequest con request =

ABORT. El estado de la tarea pasa a CromatTaskStatus = CROMATTASKSTATUS ABORTING

para indicar que se esta gestionando la cancelacion.


= ATOMTASKSTATUS ABORTED

Task Manager → Control Centre: AtomTaskState con CromatTaskStatus

= CROMATTASKSTATUS ABORTED. Se confirma que se han acabado todoslos procesos de cancelacion.

4.3.3.3. Tramo imposible

Como indicamos anteriormente, es util que el modulo RRT nos indiqueque pares de nodos son difıciles de conectar teniendo en cuenta las restric-ciones cinematicas del robot, para obtener una nueva solucion sin pasarpor dichos nodos. Nos encontramos por lo tanto en el transcurso de unfuncionamiento normal, en el que el algoritmo RRT falla un determinadonumero de veces al encontrar la solucion de un tramo de la solucion. Noes necesario detener a las capas bajas porque aun no se han enviado lasordenes para que entren en funcionamiento. Una vez que llega el error alTask Manager, este informa al Modulo PRM de que vuelva a calcular unaruta e indica que nodos han originado el error. El Modulo PRM elimina di-chos nodos del grafo y vuelve a consultarlo. Obtendra por tanto, una rutadistinta a la inicial que se espera que sea factible.


= ATOMTASKSTATUS ABORTED, errorCode = RRT WP NOT REACHED y error wp

con los nodos conflictivos a true.

Task Manager → Modulo PRM : PRM Task con PRM task.type = RETRY,wp to avoid tiene los puntos a evitar y wp punto objetivo.

Modulo PRM → Task Manager : PRM Path con el camino almacenadoen el campo path, errorCode=NO ERROR GRI.


Task Manager → Module Manager : AtomTaskRequest con request

= START, sequenceNumber el mismo que la tarea anterior que ha sidoabortada. wpl no deberıa contener los waypoints conflictivos si esto esposible(si no es posible se aborta la tarea).

[...]

4.3.3.4. Otros errores

En caso de que ocurran otros errores no especificados aquı, se sigue el mis-mo protocolo de actuacion que en la arquitectura CROMAT [22], abortandola tarea y especificando al Control Centre el error en el CromatTaskStatecon los valores definidos enrobot architecture/comms/GRI error code.h

4.3.4. Implementacion de las comunicaciones

En la arquitectura CROMAT, cada nivel de abstraccion (RAL, MML oRIL) se implementa mediando un proceso, mientras que cada modulo dentrode una capa se implementa mediante un hilo. De esta manera, cada una delas “cajas” de la figura 4.18 del modulo PRM y el Task Manager se ejecutancomo hilos que pertenecen al proceso RAL, mientras que Module Manageres un hilo que se ejecuta dentro del proceso MML. Por otro lado el moduloControl Centre es un proceso independiente.

Podemos dividir por tanto nuestras comunicaciones en comunicacionesdentro del mismo proceso y comunicaciones interprocesales.

Para las comunicaciones dentro del mismo proceso se usara una seccioncrıtica, implementada como una clase global, que podra ser accedida portodos los hilos del proceso, controlandose su acceso mediante mutex paraevitar condiciones de carrera.


Figura 4.24: La comunicacion en CROMAT.

Aunque en el momento de escribir estas lıneas se esta comenzando a mi-grar a YARP (Yet Another Robot Platform), las comunicaciones entre proce-sos se implementaron mediante BBCS(BlackBoard Communication System)a traves de un hilo de comunicaciones que todo proceso lanza expresamentepara este cometido. Ademas cada entidad (robots, Control Center, TimeServer...) incluye un repetidor, llamado Relay Node que se encarga de enviary recibir los datos de todos los slots conectados con el resto de entidades.

Hay dos tipos de slots entre hilos de comunicaciones, seguro, cuyosdatos no se borran hasta que son recibidos, almacenandose en un buffermientras tanto; y los no seguros, que se actualizan cada vez que se envıa un


nuevo dato, independientemente de si este ha sido tomado.Aunque existe una cierta independencia del hilo de comunicaciones re-

specto del resto de hilos, a la hora de programar esto no es tan evidente yresulta algo engorroso poner en funcionamiento lo que se denomina un slot

de comunicaciones. Estos son las modificaciones que hay que realizar:

Hilo de comunicaciones:

• XXXcommunicactionthread.cpp: Anadir al bucle infinito las lla-madas pertienentes a los metodos de la clase communication, tıpi-camente sendNOMBREDATO y/o receiveNOMBREDATO.

• XXXcommnication.cpp: Modificar el constructor y destructor dela clase para los nuevos tipos de datos que debera usar. Abrirel slot de comunicaciones, en el metodo Init. Implementar lasfunciones sendNOMBREDATO y receiveNOMBREDATO, quedeberan tomar el dato de la SC y enviarlo a otro hilo en el slotcorrespondiente, o recibirlo del slot y meterlo en la SC(diferentepara slot seguro o no).

• XXXcommunication.h: Definir los metodos y datos anteriores.

Seccion crıtica:

• XXXcriticalsection.cpp:Definicion de los metodos de acceso ala SC(diferente para slot seguro o no).

• XXXcriticalsection.h:Declarar los tipos de datos que se van ausar(diferente para slot seguro o no) y los metodos de acceso.

Relay Node (si la comunicacion sale de la entidad):

RN Communication.cpp: Anadir el slot correspondiente.

Ademas, al crear un slot nuevo debe asignarsele un identificador que de-bera definirse en una cabecera X slots.h donde X correspondie a la interfazdonde se ubique (GRI,MRI, IMI).

Una vez hecho esto solo hay que acceder a la seccion crıtica con los meto-dos de acceso que hemos generado. Como se puede ver no es muy comodoestablecer comunicaciones entre hilos con este sistema, razon por la cual semigrara a YARP.

Para las comunicaciones internas de una capa, solo es necesario imple-mentar los metodos de acceso y estructuras de datos en la clase de la seccioncrıtica, para luego llamar a dichos metodos para enviar o recibir los datos.

En este proyecto, aunque inicialmente se abrieron otros slots para re-alizar pruebas, solo ha sido necesario abrir un nuevo slot para comunicarsecon el modulo Path Follower del RIL, cosa concerniente al proyecto [23].Ademas, para las comunicaciones entre hilos de la misma capa en la MML,


se han modificado pertinentemente los archivos correspondientes de la sec-cion crıtica. Todas las modifificaciones a los archivos ya existentes se hanmarcado con comentarios del tipo:

//PRMModuleChanges

[...]

//End of PRMModuleChanges.

trabajo realizadobibing.us.es/proyectos/abreproy/11509/fichero/volumen1... · la versi´on m´as...

Documents