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 (Forward

Dynamic Programming). Aunque estan mas desarrollados los basados enRRT, 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

30

CAPITULO 4. TRABAJO REALIZADO 31

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 enentornos bidimensionales y tridimensionales. Ha sido desarrollada porla universidad de Carolina del Norte. Para conseguirla, tuvimos quemandar un email a dicha universidad para conseguir descargar elcodigo fuente. La universidad nos lo facilito sin problemas y con granprontitud.

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ıasno las hemos probado, sirven para obtener mejores resultados enlas simulaciones renderizadas. Existe una version de OpenInventortotalmente software 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 optimizada.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 laMSL sin la GUI por lo tanto prescinderemos de dicha parte, aunque losprimeros experimentos los hicimos con la ayuda de la GUI. Hemos tenidoque introducir clases nuevas tanto en los solucionadores, donde hemosdesarrollado nuevas variantes de PRM basado en visibilidad y de RRT, comoen la parte de geometrıa y del modelo.

CAPITULO 4. TRABAJO REALIZADO 32

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

Modelo: Contiene los simuladores incrementales que modelan elcomportamiento cinematico y dinamico de una variedad de sistemasmecanicos. Estas clases permiten a los algoritmos planificadoresestimar el estado futuro del sistema, dandole el estado actual, unintervalo de tiempo(dado por ∆t) y la entrada de control aplicada alrobot en ese intervalo. Se ha implementado un nuevo modelo parasimular el comportamiento del Romeo 4R, como se detalla en elapartado 4.1.2.4.

Geometrıa: Esta parte se encarga de definir las representacionesgeometricas 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. Hemos tenido quedesarrollar una nueva clase de geometrıa en la que tanto el robot comoel entorno se describen con una retıcula.

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

CAPITULO 4. TRABAJO REALIZADO 33

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

Solucionador: Actualmente solo hay un tipo de solucionador, lasvariantes son clases hijas del mismo. Se inicializa con una instanciade un problema y tiene un metodo que resuelve dicho problema(Solve()). Para los algoritmos basados en mapas de carretera,tambien es necesario el metodo (Construct()) que construye el mapade carreteras sobre el que se efectuara la busqueda.

Escena: Es una clase interfaz que computa las configuraciones detodos los cuerpos para que sean representados graficamente porla clase de renderizado. Esta clase recibe la mayor parte de lainformacion directamente del problema, pero incluye informacionadicional concerniente a la representacion, como el punto de vista dela camara.

Renderizado: Esta jerarquıa de clases contiene diferentesimplementaciones de peticiones de renderizado grafico. Por ejemplo,cuando la GUI pide que se anime un camino solucion, un metodode esta clase muestra los cuerpos en movimiento usando lasconfiguraciones obtenidas por la clase Escena. Cada clase derivada derenderizado usa un diferente sistema grafico. En el presente existenrenderers que utilizan las librerıas SGI IRIS Performer, Open Inventory Open GL.

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

4.1.2.1. Generando nuevos problemas en la MSL

Tal y como viene implementada la MSL, los constructores de las clasesnecesitan 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 siguiente tablas, se indican los archivos masimportantes y su funcionalidad.

CAPITULO 4. TRABAJO REALIZADO 34

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 planificacionmultirobotX es la dimension del espacio.

LowerWorld Contiene con las coordenadasmınimas.

Vector Sı

UpperWorld Contiene con las coordenadasmı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

esConjunto Sı

un obstaculo. Ej →(0, 0)(20, 15)(30, 0).

de puntos

Robot[X] Forma del robot. Si hay mas de Conjunto Sıuno, se indica el no(de 0 a N-1). de puntos

LowerState 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 NoGapError Diferencia permitida al considerar

dos estados iguales.Vector No

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

CAPITULO 4. TRABAJO REALIZADO 35

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

CAPITULO 4. TRABAJO REALIZADO 36

Nombre de la clase Descripcion Cabecera

PRM PRM. prm.h

RRT RRT simple. prm.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 lainformacion de cada intento deexpansion.

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.2.2. Rotado de retıculas

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

En el caso de la MSL, como vimos anteriormente en el apartado 4.1.2,la deteccion de colisiones se realiza usando la librerıa PQP, que no sepodra 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.

CAPITULO 4. TRABAJO REALIZADO 37

Existen muchas formas de implementar esta rotacion, desarrolladasprincipalmente por investigadores en el campo del tratamiento de imagenes.

En [16] 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 rotacionbidimensional en un producto de tres matrices que operan unicamente porfilas o 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 tresconvoluciones 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.

CAPITULO 4. TRABAJO REALIZADO 38

Figura 4.2: Tres trasnsformaciones unidimensionales queconforman 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 undimensionalcomo filtrado FIR.

De entre las funciones de interpolacion, y puesto que para nuestraaplicacion se debe dar mas peso a la rapidez de ejecucion del algoritmoque a la calidad del resultado, elegimos una B-spline de grado 1, que no

CAPITULO 4. TRABAJO REALIZADO 39

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.

La implementacion del algoritmo de rotado resultante puede verse en elcodigo fuente que acompana a este documento en CD-ROM.

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 angulosmayores. Para extender el rango de angulos a θ ∈ [−π, π] se aprovechanlos casos especiales de los giros de θ = {0, π

2, π,−π

2,−π} para los que no

es necesario aplicar el algoritmo de rotado, bastando con reordenar lasceldas adecuadamente. Por lo tanto, rotate es una extension del algoritmoprincipal, que se implementa en rotate45 y que usa otros metodos querealizan los giros de los casos especiales.

4.1.2.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 ymodificar archivos BMP de forma facil. Es una herramienta que intentaser multiplataforma. Ha sido satisfactoriamente probada en Linux(kernels2.4.x y 2.6.x con g++ e icc), Windows(XP Pro, XP Home Y 2K Pro conMinGW/g++ y Borland) y maquinas Sun Sparc 4 (con S.O. Solaris 5.9 ycompilador g++).

Para incluir dicha herramienta, solo tuvimos que anadir al proyectolos archivos EasyBMP.cpp y EasyBMP.h y compilarlo. No tuvimos mayorproblema.

CAPITULO 4. TRABAJO REALIZADO 40

Como se puede ver en el apendice A el mapa en memoria se almacena enla clase C2DOccupancyGrid (por el momento solo trabajamos con mapasbidimensionales). 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.1.2.4. Modelo de ROMEO4R

El modelo de ROMEO4R que se utilizara sera el tıpico para un roboten configuracion Ackerman sin deslizamiento. Una descripcion del mismopuede encontrarse en [17]

CAPITULO 4. TRABAJO REALIZADO 41

Figura 4.4: Robot movil de configuracion tipo Ackerman

La ecuacuion 4.121 de [17] nos da la evolucion en el tiempo de los estadosdel robot

x = −v sin(θ)

y = v cos(θ)

θ = vγ (4.4)

Donde γ es la curvatura, que se calcula como se muestra en la figura 4.4.Ahora bien, estas ecuaciones estan definidas con el angulo θ respecto del

ejey Y en sentido horario. Por mantener la polıtica de la MSL, nosotrosvamos a referir los angulos respecto del eje X en sentido antihorario, yanadiremos dos estados, a saber, el angulo de giro de la rueda virtual centralϕ y la velocidad. Por lo tanto la variable γ la sustituiremos para dejarla enfuncion de ϕ y carlength, la longitud del coche.

x = v cos(θ)

y = v sin(θ)

θ = v tan(ϕ)/car length

ϕ = uϕ

v = uv (4.5)

CAPITULO 4. TRABAJO REALIZADO 42

Siguiendo la polıtica de la MSL, se usaran dos vectores MSLVector pararepresentar el estado del robot y las entradas en cada instante. El vector deestados sera de 5 componentes, en este orden:

x: Coordenada posicional en el eje X

y: Coordenada posicional en el eje Y

a: Angulo respecto del eje X, entre [−π, π], positivo en sentidoantihorario.

s: Angulo de giro de la rueda central imaginaria(s por steer);

v: Velocidad lineal.

El vector de entradas tendra 2 componentes, en el siguiente orden:

uv: Variacion en la referencia de velocidad.

us: Variacion en la referencia de angulo de giro (antihorario si positivo).

4.2. Eleccion de una (pseudo-)metrica adecuada

En el apartado 2.2, la eleccion de una funcion que determine la distanciaentre dos configuraciones del robot es una cuestion importante y complicada,necesaria para un correcto funcionamiento de los algoritmos RRT. En vistade esta necesidad, se han probado tres metricas diferentes, la primera deellas ampliamente conocida (metrica de Mahalanobis), siendo las otras dosaportaciones personales.

Para el analisis de la bondad de la metrica se observaran las curvasde nivel equidistantes para varias configuraciones. En todos los casos setomara como configuracion inicial al robot en reposo en el centro de laimagen y orientado hacia arriba. Las configuraciones finales tendran encomun la velocidad y el angulo de las ruedas, que seran nulos. Se barreranlos ejes X e Y , mostrando diferentes graficas para diferentes angulos deorientacion final.

Ademas debera tenerse en cuenta el tiempo de computo de la distancia,que se tomara como un promedio de las 160.000 medidas realizadas algenerar la grafica. Las pruebas se han realizado en un Dell Inspiron 6400,

CPU Intel Core 2 Duo T5600 1.83Ghz.

4.2.1. Metrica de Mahalanobis

Se define la distancia de Mahalanobis de dimension n como

ρ(~x, ~y) =√

k1(x1 − y1)2 + k2(x2 − y2)2 + · · ·+ kn(xn − yn)2

CAPITULO 4. TRABAJO REALIZADO 43

Como puede verse, la metrica euclıdea es un caso particular de la deMahalanobis en la que todos los pesos valen uno. Ademas esta medida dedistancia cumple con la propiedad de simetrıa.

Figura 4.5: Curvas de nivel equidistantes en la metrica deMahalanobis. Orientacion final hacia arriba.

Como era de esperar y puede observarse en la grafica, la metrica deMahalanobis tiene simetrıa radial en el plano (x, y). Al ser independientecada componente de los vectores de configuracion, en esta metrica cualquiervariacion en la orientacion final tendra como unica repercusion en las graficasde curvas de nivel una variacion de la oscuridad de todas las curvas por igual,que no cambiaran su forma. Esto es un problema, como ya vimos en la figura2.3, debido a que en las distancias para un robot con restricciones comoRomeo4R las componentes de orientacion y posicion son interdependientes.

4.2.2. Pseudometrica Bezier cubica

Para solucionar el problema de la independencia de las componentes dela metrica de Mahalanobis, se intento elaborar una metrica mas ajustada ala realidad. Definiremos la distancia Bezier cubica en funcion de la longitudde una curva Bezier de cuatro puntos P1,P2,P3,P4, en la que:Sea la distancia euclıdea de P1 a P4 dP1−P4 =√

(xP1 − xP4)2 + (xP1 − xP4)2

P1 se corresponde con la configuracion inicial en el plano (x, y)

CAPITULO 4. TRABAJO REALIZADO 44

P2 es un punto de control alejado dP1−P4/2 de P1 en direccion de lavelocidad inicial.

P3 es un punto de control alejado dP1−P4/2 de P4 en direccioncontraria a la velocidad final.

P4 se corresponde con la configuracion final en el plano (x, y).

La distancia se computa incrementalmente, aproximando por lıneasrectas cada tramo de la curva. Aunque en la eleccion de los puntos de controlse ha pretendido minimizar la curvatura, habra configuraciones en las queen ciertos tramos de la curva, se superara la curvatura maxima posible deRomeo4r. A la hora de computar la distancia, en estos tramos, en lugar deaproximar por una linea recta, se calcula el mınimo arco que podrıa trazarRomeo tal como se muestra en la figura:

Figura 4.6: Correccion a la longitud de los tramos de la curvaBezier con curvatura mayor que la posible.

Ası, en los tramos donde la curvatura supere kmax, en lugar de sumar ladistancia del segmento que une los puntos, se sumarıa θ/kmax.

A continuacion se muestran los resultados para diferentes angulos deorientacion final.

CAPITULO 4. TRABAJO REALIZADO 45

Figura 4.7: Curvas de nivel equidistantes en la metrica Beziercubica. Orientacion final hacia arriba.

Figura 4.8: Curvas de nivel equidistantes en la metrica Beziercubica. Orientacion final hacia la derecha.

CAPITULO 4. TRABAJO REALIZADO 46

Figura 4.9: Curvas de nivel equidistantes en la metrica Beziercubica. Orientacion final hacia abajo.

El tiempo de computo de estas distancias es dependiente del numero depuntos de las curvas Bezier que se tomen, siendo aproximadamente de 3.125microsegundos por cada 10 puntos. Las figuras presentadas se obtienen con100 puntos por curva de resolucion. Con 20 puntos ( 6.25µs por medida) se obtienen graficas muy parecidas, utiles para la implementacion de losalgoritmos.

4.2.3. Pseudometrica Bezier-aproximada

Por ultimo, se presenta una nueva metrica que intenta ser unaaproximacion gruesa de la anterior, pero con un tiempo de computoconsiderablemente inferior. Esta metrica tiene en cuenta los resultados de lapseudometrica Bezier cubica presentada en el apartado anterior, e intentaemular los resultados obtenidos con esta, sin calcular la curva.

El metodo de aproximacion usado ha sido la inspeccion de las curvas denivel generadas dividiendo el plano en cuatro zonas dependiendo del angulode las configuraciones inicial y final ası como del vector que une ambasconfiguraciones en el plano XY, y aplicando condiciones de continuidadcuando ha sido necesario.

CAPITULO 4. TRABAJO REALIZADO 47

Algoritmo 17 Pseudometrica Simple(x1, x2)

1: distancia← Mahalanobis(x1, x2);2: angmed← AnguloMedio(x1.ang, x2.ang);3: angx1x2←angulo(x1, x2, angmed);4: if Zona(angx1x2)=1 then

5: distance← distance + Zona1(angx1x2);6: else

7: if Zona(angx1x2)=2 then

8: distance← distance + Zona2(angx1x2);9: else

10: if Zona(angx1x2)=3 then

11: distance← distance + Zona3(angx1x2);

Como podemos ver en el pseudocodigo, el algoritmo calcula la distanciade mahalanobis, para luego, sumarle ciertos valores dependiendo de la zonadonde se encuentre el objetivo.

Figura 4.10: Curvas de nivel equidistantes en la metricaBezier-aproximada. Orientacion final hacia arriba.

CAPITULO 4. TRABAJO REALIZADO 48

Figura 4.11: Curvas de nivel equidistantes en la metricaBezier-aproximada. Orientacion final hacia la derecha.

Figura 4.12: Curvas de nivel equidistantes en la metricaBezier-aproximada. Orientacion final hacia abajo.

CAPITULO 4. TRABAJO REALIZADO 49

4.3. Metodos de muestreo

Como vimos en 2.7, una buena polıtica de muestreo puede hacer que elplanificador RRT mejore considerablemente. Del mismo modo, la eleccionrepetitiva de muestras inutiles provocara un descenso del rendimiento delplanificador, pudiendo incluso hacerlo inservible.

Por defecto, los planificadores incluıdos en la librerıa MSL realizan unmuestreo uniforme de todo el espacio de configuraciones. En nuestro casopor ejemplo estarıamos hablando de una zona zona de 100x100m con cadaconfiguracion posible de Romeo4r, incluyendo los espacios ocupados porobstaculos.

Esta polıtica de muestreo resulta idonea para tareas difıciles, en las quehay que sortear obstaculos y alcanzar configuraciones complicadas, porquetrata de explorar todos los caminos posibles.

Sin embargo, recordando que nuestro problema se cine a conectar dosconfiguraciones con visibilidad en el plano XY, no es necesario explorar todoel entorno, pues ya tiene una guıa de las zonas por donde se deberıa dirigirel robot.

Por esta razon, se han implementado dos nuevas formas de muestreo:

4.3.1. Muestreo zonal

Se definen tres zonas de muestreo en el plano XY:

Zona inicial: Es una zona cuadrada situada delante de la configuracioninicial, teniendo en cuenta por lo tanto la orientacion inicial. Su ladodepende proporcionalmente de la distancia de la configuracion iniciala la final y del parametro InitZone.

Zona final: Es una zona cuadrada situada detras de la configuracionfinal, teniendo en cuenta por lo tanto la orientacion final. Su ladodepende proporcionalmente de la distancia de la configuracion iniciala la final y del parametro GoalZone.

Zona Media: Es una zona rectangular que se forma como el mınimorectangulo que rodea a las zonas inicial y final.

En las siguientes figuras se pueden observar las zonas de muestreo para dosproblemas habituales:

CAPITULO 4. TRABAJO REALIZADO 51

Es decir cada muestra se tomara como :

ymuestra = xcurva + rand(xcurva − offset, xcurva + offset);

ymuestra = ycurva + rand(ycurva − offset, ycurva + offset); (4.6)

Siendo xcurva, ycurva un punto aleatorio de la curva S y offset unparametro que determina el entorno del punto de la curva sobre el quese muestreara.

4.4. Tabulacion de la integracion del modelo

Uno de las partes a las que mas tiempo de computacion se dedica enlos planificadores RRT es a la integracion del modelo del robot. Cada vezque se ejecuta MejorActuacion(vease pseudocodigo 1), presente en todaslas variantes RRT que se han estudiado, se integra el modelo para todas laentradas posbiles, para saber a que nueva configuracion lleva y ası elegir laentrada adecuada.

Se ha estudiado la posibilidad de evitar estas integraciones en tiempode ejecucion, tabulando el resultado de dichas integraciones y obteniendoası el incremento diferencial de un paso de integracion en cada una de lasconfiguraciones posibles, a la que luego solo hay que acceder, en lugar devolver a integrar.

Hay que tener en cuenta que el tamano de la tabla de integracion puedeser excesivamente grande, pues el tamano sera de :

size(tabla) = size(estado)× estados posibles× numero de entradas

Para el computo de los estados posibles se puede prescindir de laorientacion del robot, que se puede volver a reorientar con un simple cambiode sistema de referencia. Se podrıa estudiar la inclusion de la orientacionen la tabla para evitar usar las funciones seno y coseno necesarias para elcambio de sistema, sin embargo esto implicarıa tambien la discretizacion deesta componente en el modelo y un tamano de la tabla aun mayor.

Para nuestro modelo de robot (vease el apartado 4.1.2.4), contamos con:

5 entradas:

• 2 de variacion de velocidad(positiva y negativa).

• 2 de variacion de angulo de giro(derecha e izquierda).

• 1 entrada nula.

21 valores posibles de velocidad, en pasos de 0.1m/s de 0 a 2m/s

17 valores posibles de orientacion, en pasos de 0.1rad desde -0.8rad a0.8rad.

CAPITULO 4. TRABAJO REALIZADO 52

Por lo tanto una tabla que contenga los resultados de la integracionpara nuestro modelo tendra 21 × 17 × 5 = 1785 entradas del tamano deun vector de estados (5 valores de tipo double). Si ademas guardamos lasintegraciones hacia atras, utiles para los planificadores de dos arboles, elnumero se duplicarıa. Esto no es un gran problema para las maquinasactuales.

El sistema para evitar las integraciones en tiempo de ejecucion delplanificador consiste entonces en realizar todas las integraciones, guardarlasen una tabla, y luego, cuando haya que integrar el modelo, se accede al valorcorrespondiente, y se aplica al modelo como sigue:

x = x + cos(θ)xTabla− sin(θ)yTabla

y = x + cos(θ)yTabla + sin(θ)xTabla

θ = θ + Tabla

ϕ = ϕ + uϕ

v = v + uv (4.7)

4.5. Discretizacion de los angulos de giro a efectosde deteccion de colisiones

Una de las tareas que mas tiempo de calculo consume en losplanificadores por muestreo es la deteccion de colisiones. En nuestro caso, ladeteccion de colisiones se realiza, como vimos en el apartado 4.1.2y 4.1.2.2,mediante superposicion de reticulas, previa rotacion de la retıcula del robot.De manera analoga al apartado anterior, si discretizamos el angulo de giroposible del robot y guardamos los grids del robot girado, evitaremos calculosinnecesariamente reiterados. Para hacer esto se guardaran en memoria uncierto numero de retıculas con la informacion de las celdas de ocupacion delrobot girado en cada una de los angulos discretos, para luego al detectarcolisiones aproximar el angulo real del robot por uno de estos angulos.

4.6. Integracion en la arquitectura CROMAT

El escenario donde se han implementado los algoritmos elegidos es laarquitectura CROMAT [15]. Se trata de una arquitectura disenada para elcontrol y comunicacion entre multiples robots heterogeneos estructurada encapas o niveles de abstraccion que permite que desde los niveles mas altosse trate de forma similar a los diferentes robots, a pesar de sus diferencias.

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

CAPITULO 4. TRABAJO REALIZADO 53

Figura 4.14: Diagrama de bloques de la arquitectura CROMAT

En la figura se observa en detalle la division en capas de un robot,ası como las partes que componen el resto de la arquitectura, a saber elControl Center, 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 y sehan modificado los archivos robot architecture/comms/GRI data.h yrobot architecture/control centre/source/HMITerminal.cpp.

La parte mas importante, sin embargo, es la de los niveles de abstraccionde cada robot donde se han ubicado los diferentes modulos que implementan

CAPITULO 4. TRABAJO REALIZADO 54

los algoritmos PRM y RRT para su uso como planificadores de movimientodentro de la arquitectura.

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.15: Diagrama de bloques de la ubicacion en laarquitectura 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 el

CAPITULO 4. TRABAJO REALIZADO 55

modulo 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 elmodulo 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 ambosmodulos mediante los modulos ya existentes Task Manager y Module

Manager. De este modo, el procedimiento habitual de realizacion de unatarea de movimiento PRM GOTOXYZ se divide en:

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

Task Manager → Modulo PRM: Peticion de nueva tarea demovimiento

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 tarearequerida.

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.

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.

CAPITULO 4. TRABAJO REALIZADO 56

Ademas puede ocurrir que dentro de la lista de puntos solucion delmodulo PRM, al ser esta independiente de la configuracion del robot, existandos puntos consecutivos imposibles de conectar para el robot concreto (ennuestro caso Romeo4R). En estas situaciones resulta conveniente que elmodulo RRT informe al modulo PRM de cuales han sido esos puntos, paraque se procuren evitar al reintentar la tarea de movimiento.

Por lo tanto se necesita un protocolo para la comunicacion entrelos distintos modulos que intervienen que tenga en cuenta estas posiblessituaciones. En el siguiente apartado se describira con detalle.

4.6.1. Estructura de comunicacion

En el ambito de este proyecto, la MML funciona como una caja negraque tiene como entradas y salidas bidireccionales al Task Manager de lacapa RAL y al Path Follower Module del RIL.

En la siguiente figura se pueden ver los tipos de datos usados para lascomunicaciones internas de la MML ası como de esta capa con el exterior(las capas superior e inferior, se entiende).

CAPITULO 4. TRABAJO REALIZADO 57

Figura 4.16: Estructuras de datos para la comunicacion por losmodulos de la MML.

A continuacion explicaremos cada estructura de datos en detalle:

typedef struct AtomTaskRequest {

AtomTaskOperation request;

AtomTaskName taskName;

int sequenceNumber;

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

[...]

CromatWaypointsList wpl;

[...]

} taskParam;

} AtomTaskRequest;

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

CAPITULO 4. TRABAJO REALIZADO 58

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 define el problema.

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.18: AtomTaskState.Definida en robot architecture/comms/MRI data.h

En nuestro caso:

sequenceNumber debe ser el mismo que el recibido en laAtomTaskRequest correspondiente.

taskName valdra siempre TASK ATOM RRT GOTO LIST XYZ

state tendra uno de los valores posibles definidos en AtomTaskStatus

errorCode valdra por defecto RRT NO ERROR

error wp valdra true unicamente para los ındices correspondientesa waypoints en los que el modulo del algoritmo RRT noencontro solucion, si los hubiere. Para el resto valdra false.

CAPITULO 4. TRABAJO REALIZADO 59

typedef struct

{

AtomTaskOperation request;

int sequenceNumber;

CromatWaypointsList wpl;

}RRTRequest;

typedef struct

{

int error;

RRTSolution solution;

AtomTaskState rrt_state;

}RRTResponse;

Figura 4.19: RRTRequest y RRTResponse. Definidos enromeo/comms/ROMEO InternalMMLComms.h

En nuestro caso:

request valdra START o ABORT

sequenceNumber sera el mismo que el de la AtomTaskRequest

correspondiente.

wpl es la contenida en la peticion AtomTaskRequest correspondiente.

error valdra por defecto RRT NO ERROR.

solution indicara si se ha encontrado solucion al problema yproveera el camino solucion union de todos los caminos consecutivosen su caso.

RRTProblem contiene una serie de parametros del problema que no sonrelevantes en este apartado. Basta saber que sera un problema single query

obtenido de dos puntos consecutivos de la lista de waypoints wpl.

typedef struct{

bool solved;

list <MSLVector> solution;

}RRTSolution;

Figura 4.20: RRTSolution. Definido enromeo/comms/ROMEO InternalMMLComms.h

CAPITULO 4. TRABAJO REALIZADO 60

Donde solved vale true en caso de haberse encontrado solucion alproblema definido en RRTProblem, en cuyo caso se incluira como una listade vectores de configuracion.

4.6.1.1. Funcionamiento normal

En un funcionamiento normal, el problema llega hasta el modulo enforma de lista de waypoints, y este lo divide en single queries que envıaal algoritmo RRT para solucionarlos. Cuando todos se han solucionado, elmodulo RRT une las soluciones y las envıa hacia las capas bajas vıa Module

Manager

Task Manager → Module Manager : AtomTaskRequest . request =

START.

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

Module Manager → Task Manager : AtomTaskState conAtomTaskStatus = ATOMTASKSTATUS RUNNING

• Modulo RRT → Algoritmo RRT: RRTProblem con el primerproblema de la lista de puntos.

• Algoritmo RRT → Modulo RRT: RRTSolution con la soluciondel primer problema.

[...]

• Modulo RRT → Algoritmo RRT: RRTProblem con el ultimoproblema de la lista de puntos.

• Algoritmo RRT → Modulo RRT: RRTSolution con la soluciondel ultimo problema.

ModuloRRT → Module Manager : RRTResponse con la solucion delproblema y rrt state.state = ATOMTASKSTATUS ENDED

Module Manager → Path Follower Module: WayPointData con lasolucion completa.

Path Follower Module → Module Manager : ModuleState indicandoque ha terminado (vease [15]).

Module Manager → Task Manager : AtomTaskState conAtomTaskStatus = ATOMTASKSTATUS ENDED

CAPITULO 4. TRABAJO REALIZADO 61

4.6.1.2. Cancelacion manual

Task Manager → Module Manager : AtomTaskRequest con request =

ABORT.

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

Module Manager → Task Manager : AtomTaskState conAtomTaskStatus = ATOMTASKSTATUS ABORTING

ModuloRRT→Module Manager : RRTResponse con rrt state.state

= ATOMTASKSTATUS ENDED

Module Manager → Capas bajas: Instrucciones necesarias para detenerlos modulos. Ver [15].

Capas bajas → Module Manager : Indicacion de que los modulos sehan detenido correctamente. Ver [15].

Module Manager → Task Manager : AtomTaskState conAtomTaskStatus = ATOMTASKSTATUS ABORTED

4.6.1.3. Tramo imposible

Como indicamos anteriormente, es util indicar al modulo PRM de lospares de nodos que no han podido conectarse, para reintentar solucionarel problema evitando dichos nodos. Nos encontramos por lo tanto en eltranscurso de un funcionamiento normal, en el que el algoritmo RRT fallaun determinado numero de veces al encontrar la solucion de un single query.No es necesario detener a las capas bajas porque aun no se han enviado lasordenes para que entren en funcionamiento.

ModuloRRT→Module Manager : RRTResponse con rrt state.state

= ATOMTASKSTATUS ENDED, error = RRT WP NOT REACHED yrrt state.error wp con los nodos conflictivos a true.

Module Manager → Task Manager : AtomTaskState conAtomTaskStatus = ATOMTASKSTATUS ABORTED, errorCode =

RRT WP NOT REACHED y error wp con los nodos conflictivos atrue.

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.

[...]

CAPITULO 4. TRABAJO REALIZADO 62

4.6.1.4. Otros errores

En caso de que ocurran otros errores no especificados aquı,se sigue el mismo protocolo de actuacion que en la arquitecturaCROMAT [15], abortando la tarea y especificando a las capasaltas el error en el AtomTaskState con los valores definidosenrobot architecture/comms/MRI error code.h

4.6.2. Implementacion de las comunicaciones

En la arquitectura CROMAT, cada nivel de abstraccion se implementamediando un proceso, mientras que cada modulo dentro de una capa seimplementa mediante un hilo. De esta manera, cada una de las “cajas” dela figura 4.16 del modulo RRT y el Algoritmo RRT, ası como el Module

Manager representas hilos que se ejecutan en el proceso MML, mientras queTask Manager es un hilo que se ejecuta dentro del proceso RAL. Por otrolado el modulo Path Follower es un hilo que se encuentra en el proceso RIL.

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.

CAPITULO 4. TRABAJO REALIZADO 63

Figura 4.21: La comunicacion en CROMAT.

Aunque en el momento de escribir estas lıneas se esta comenzando amigrar a YARP (Yet Another Robot Platform), las comunicaciones entreprocesos se implementaron mediante BBCS(BlackBoard CommunicationSystem) a traves de un hilo de comunicaciones que todo proceso lanzaexpresamente para este cometido. Ademas cada entidad (robots, Control

Center, Time Server...) incluye un repetidor, llamado Relay Node que seencarga de enviar y recibir los datos de todos los slots conectados con elresto de entidades.

Hay dos tipos de slots entre hilos de comunicaciones, seguro, cuyosdatos no se borran hasta que son recibidos, almacenandose en un buffer

CAPITULO 4. TRABAJO REALIZADO 64

mientras tanto; y los no seguros, que se actualizan cada vez que se envıa unnuevo dato, independientemente de si este ha sido tomado.

Aunque existe una cierta independencia del hilo de comunicacionesrespecto 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 lasllamadas pertienentes a los metodos de la clase communication,tıpicamente 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 quedebera 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 losmetodos de acceso que hemos generado. Como se puede ver no es muycomodo establecer comunicaciones entre hilos con este sistema, razon porla cual se migrara a YARP.

Para las comunicaciones internas de una capa, solo es necesarioimplementar los metodos de acceso y estructuras de datos en la clase dela seccion crıtica, para luego llamar a dichos metodos para enviar o recibirlos datos.

En este proyecto, aunque inicialmente se abrieron otros slots para realizarpruebas, solo ha sido necesario abrir un nuevo slot para comunicarse con

CAPITULO 4. TRABAJO REALIZADO 65

el modulo Path Follower del RIL. Ademas, para las comunicaciones entrehilos de la misma capa en la MML, se han modificado pertinentemente losarchivos correspondientes de la seccion crıtica. Todas las modifificaciones alos archivos ya existentes se han marcado con comentarios del tipo:

//RRTChanges

[...]

//End of RRTChanges.

4.6.3. Modulo RRT

Como se explico en el apartado 4.6.1, el modulo RRT recibe una lista depuntos que define el problema, y envıa single queries al modulo algortıtmicode la misma capa. Por lo tanto, para implementar el protocolo definidoen dicho apartado, este modulo debe dividir el problema en trozos, quellamaremos subproblemas para evitar confusiones.

Este modulo se encarga ademas de, en el caso de que varios intentosdel mismo subproblema fallen, abortar el problema indicando a las capasaltas cuales fueron los dos nodos consecutivos de la lista que provocaron esasituacion. Se realizan varios intentos porque dada la naturaleza estocasticade los planificadores RRT, a veces es mejor empezar de nuevo la busquedaque continuar con la misma.

Para definir completamente una single query que pueda solucionar unplanificador RRT, hay que especificar las configuraciones inicial y final delrobot, y no solamente su posicion en el plano.

Se hace necesario por lo tanto incluir en la definicion de cadasubproblema la informacion sobre las demas componentes del estado delrobot, y especialmete la de la orientacion del mismo.

Por lo tanto, a la hora de dividir el problema de la lista de puntos ensubproblemas de pares de vectores de configuraciones, conviene diferenciarentre cuatro tipos:

4.6.3.1. Subproblema inicial

La configuracion inicial del problema sera la actual del propio robot,accesible en la seccion crıtica en forma de RobotState

Para la configuracion final, se tomara la orientacion teniendo en cuentala configuracion inicial, la posicion final y la posicion del siguiente punto dela lista. Mediante el metodo getMiddleCubicAngle de la clase RomeoUtils,se obtiene el angulo y curvatura (de la cual se tomara el angulo de giro)que formarıa una funcion cubica que pase por esos puntos y con variacionessuaves en la curvatura.

CAPITULO 4. TRABAJO REALIZADO 66

Figura 4.22: Eleccion de la orientacion objetivo (en azul) de unsubproblema inicial o intermedio.

La velocidad final sera la especificada en el problema global. Otroparametro importante a tener en cuenta es GapError, que define unatolerancia a la hora de considerar que el arbol se ha conectado con el objetivoo con el otro arbol para los planificadores de dos arboles. GapError puedetomar aquı valores grandes para la posicion si el algoritmo es de un unicoarbol, porque el objetivo no es mas que una guıa de por donde deberıa pasarel robot.

4.6.3.2. Subproblema intermedio

La configuracion inicial del problema sera la ultima configuracion de lasolucion al single query anterior.

La configuracion final y el GapError se tomaran de forma analoga a lasdel problema inicial.

4.6.3.3. Subproblema final

La configuracion inicial del problema sera la ultima configuracion de lasolucion al single query anterior.

Para la configuracion final, en este caso no contamos con un puntoposterior en la lista, de modo que se llamara a getMiddleCubicAngle conun punto final ficticio que se colocara como una prolongacion en linea rectade la ultima solucion y el punto objetivo.

CAPITULO 4. TRABAJO REALIZADO 68

Por ultimo, antes de devolver la solucion, para evitar que el tamanodel GapError esconda un obstaculo que se interponga o la imposibilidadde unir el arbol sin incumplir las restricciones no-holonomicas del robot,se intenta realizar un suavizado, tambien con curvas Bezier, para unir losextremos del arbol. En caso de que esto no sea posible, por seguridad seconsiderara tambien que se ha fallado al buscar la solucion.