Planificación de Trayectorias Con El Algoritmo RRT - Aplicación a Robots No Holónomos

PLANIFICACION DE TRAYECTORIAS CON

EL ALGORITMO RRT. APLICACION A

ROBOTS NO HOLONOMOS

D. Lopez F. Gomez-Bravo F. Cuesta

A. Ollero

Grupo de Robotica, Vision y Control. Univ. de Sevilla.Camino de los Descubrimientos, 41092 Sevilla (Spain).

{fede,aollero}@cartuja.us.es

Escuela Politecnica Superior, Universidad de Huelva.Carretera de Palos-La Rabida s/n. 21071 Huelva.{diego.lopez,fernando.gomez}@diesia.uhu.es

Resumen: Dentro de los metodos de planificacion de trayectorias, las tecnicasbasadas en el algoritmo RRT (Rapidly Exploring Random Trees) estan deparandoespecial interes. En este artculo se describen los fundamentos de este novedosoalgoritmo. Asimismo, se detallan las versiones mas significativas aportadas por labibliografa mas reciente. Se ilustra tambien su aplicacion en sistemas roboticos noholonomos. Finalmente se presenta una tecnica basada en el concepto de maniobrarestringida y su aplicacion tanto a vehculos con guiado diferencial como conconfiguracion Ackerman. Copyright c2006 CEA-IFAC

Palabras Clave: Planificacion de caminos, robots moviles, sistemas no holonomos,metodos aleatorios.

1. INTRODUCCION

De forma generica, el problema de planificacion demovimientos consiste en llevar un cuerpo, desdeuna configuracion inicial hasta otra final dentrodel espacio de configuraciones libres de colision.Este problema ha sido ampliamente abordadoen la literatura (Latombe, 1991; Laumont et al.,1994; Munoz, 1995), existiendo un gran numero demetodos efectivos para resolver el problema de laplanificacion en tiempo real, tales como campospotenciales, grafos de visibilidad, diagramas deVoronoi, etc. No obstante, cuanto mas complejoes el entorno, mayor el numero de grados delibertad, o mas restricciones cinematicas presentael robot, mayores son tambien las limitaciones de

los metodos y el tiempo necesario para encontrarla solucion.

Por ejemplo, un conocido metodo de planificaciones el de campo de potenciales, el cual tambienes utilizado en aplicaciones de control reactivo.Este metodo, pese a ser rapido, presenta el pro-blema de la existencia de mnimos locales. Parasolucionar esta limitacion se recurre a algorit-mos de generacion aleatoria que permiten realizarplanificaciones locales hacia un nuevo mnimo(Latombe, 1991).

Esta situacion ha impulsado la busqueda demetodos de planificacion puramente aleatoria conobjeto de lograr mayor velocidad en la obten-cion del resultado. Entre los mas extendidosse encuentra el denominado Rapidly Exploring

JLDIEZPlaced Image

JLDIEZISSN: 1697-7912. Vol. 3, Nm. 3, Julio 2006, pp. 56-67

Random Trees(LaValle, 1998), abreviadamenteRRT. Sobre este han aparecido diferentes ver-siones que intentan mejorarlo, obteniendo efi-cientes avances computacionales, como el RRT-Ext-Con(Kuffner y LaValle, 2000; LaValle yKuffner, 2001), o aportando mejoras para su apli-cacion en escenarios especficos, como es el casodel ERRT (Bruce y Veloso, 2002). Este ultimoresulta especialmente indicado en escenarios en loscuales los obstaculos den lugar a zonas conectadasa traves de pasillos estrechos.

Este artculo introduce el algoritmo RRT. Deeste modo, se detallan las principales implementa-ciones y variantes de dicho metodo. Asimismo seilustra su aplicacion en vehculos no holonomospara facilitar la comprension del metodo. En con-creto, el metodo propuesto se aplica al caso de unrobot movil de conduccion diferencial, si bien, elprocedimiento es extensible con facilidad a robotsde cinematica mas compleja.

El artculo esta organizado como sigue: en primerlugar se introducen los metodos de planificacioncon generacion aleatoria. En la seccion 3 se detallael algoritmo RRT y diversas variantes del mismo.La seccion 4 esta dedicada a ilustrar un metodode postprocesado de la solucion. En el apartado 5se presenta la aplicacion del metodo a sistemas noholonomos. Asimismo, se propone una alternativaa los metodos existentes basada en el conceptode maniobra restringida, incluyendo resultados ycomparativas. El artculo finaliza con las conclu-siones y las referencias.

2. PLANIFICACION CON GENERACIONALEATORIA

Gracias al desarrollo del metodo de campos poten-ciales surgio una primera aproximacion a los meto-dos de planificacion aleatoria (Latombe, 1991). Eneste tipo de metodos, se parte de un escenariodonde el espacio de configuraciones se encuentradividido en porciones discretas estableciendo unarejilla regular. A cada casilla se le concede unvalor de potencial dependiendo de su proximidada un obstaculo o la cercana a los puntos origeno destino. El planificador genera una trayectoriamerced a un vector gradiente derivado del campopotencial. Esta tecnica permite que el sistema sigasiempre la direccion que minimiza el valor delcampo potencial. El objetivo es alcanzar el mni-mo absoluto que estara situado en la configuraciondestino. Sin embargo, el campo resultante puedetener una serie de mnimos locales, superioressiempre al mnimo absoluto. El problema aparececuando el sistema alcanza un mnimo local. Eneste caso, el potencial no es nulo, pero el gradientemantiene al sistema en la configuracion alcanzada.

Para resolver este inconveniente se recurre a unmetodo de generacion aleatoria. Se planificanmovimientos aleatorios que permitan que el sis-tema abandone el mnimo y, a continuacion, seaplica de nuevo el metodo del gradiente. Esteproceso continuara hasta hallar un nuevo mnimo(Latombe, 1991).

El modo de generar caminos consista en crearuna sucesion de desplazamientos aleatorios. Enestos, cada coordenada poda experimentar unincremento positivo o negativo de una longitudconstante dada. Seguidamente, se comprobaba laexistencia o no de colision y en funcion de ello seagregaban al camino. Este proceso se repeta unnumero de iteraciones determinado y, a partir delultimo punto, se volva al algoritmo de campo depotencial original.

El camino generado deba ser lo suficientementelargo como para abandonar cualquier mnimo lo-cal. No obstante es deseable que dicho camino nosea excesivamente largo, para evitar costes com-putacionales innecesarios. Por tanto, el metodoutilizado determinaba tambien un tamano aleato-rio para la longitud del camino de salida. Deeste modo se mantena la capacidad de escape decualquier mnimo, al tiempo que se proporcionabamayor probabilidad a las longitudes menores. Es-tos metodos utilizan una distribucion de Laplace,por ser de maxima entropa para la seleccionaleatoria de las magnitudes (Latombe, 1991).

El exito de los metodos aleatorios sugirio la posi-bilidad de usar dichas tecnicas de forma exclusi-va, eliminando el coste del procesamiento para elcalculo del potencial. Haban de ser mas simples,para competir en velocidad y suplir la carencia deuna inteligencia en la busqueda de caminos. Unode estos metodos es el llamado Rapidly ExploringRandom Trees, RRT (LaValle, 1998).

El RRT no precisa el establecimiento de un campode potencial, con el consecuente ahorro del proce-samiento. Al mismo tiempo, el RRT asegura unaexploracion equiprobable de todo el espacio deconfiguraciones. Por ultimo, es sencillo, rapido yde facil extension a escenarios complejos.

3. RAPIDLY EXPLORING RANDOM TREES(RRT)

El RRT original pronto dio lugar a varias versionesmas sofisticadas y de diferente comportamientosegun el entorno. En esta seccion se describira,en primer lugar, el algoritmo original y, poste-riormente, se presentaran distintas modificacionesefectuadas sobre el mismo.

MarinaText BoxD. Lpez, F. Gmez-Bravo, F. Cuesta, A. Ollero

JLDIEZLine

MarinaText Box57

3.1 Algoritmo RRT

El algoritmo RRT original se basa en la construc-cion de un arbol de configuraciones que crece bus-cando a partir de un punto origen. Para entenderel algoritmo se usaran los siguientes conceptos:

C es el conjunto de todas las configuracionesposibles del robot en un espacio dado.Cfree es el subconjunto de C de las confi-guraciones que no intersectan ninguno de losobstaculos existentes en dicho espacio.R metrica definida dentro de C. Puede serdistancia eucldea u otra ponderacion deproximidad que pueda interesar.qini es la configuracion inicial (en el casode un robot que se mueve en un plano, lascoordenadas x e y de un punto de referenciay la orientacion del vehculo respecto a unode los ejes del sistema de referencia).qfin es la configuracion que se desea alcanzar.qrand es una configuracion aleatoria que ge-nera el algoritmo.qnear es la configuracion mas proxima aqrand, en el sentido definido por R, de entrelas existentes en un arbol.qnew es la configuracion que se va a anadir alarbol. es la longitud del segmento de crecimiento.En realidad, es la distancia entre un punto delarbol y el siguiente con el que esta conectado.

El objetivo original del metodo RRT consista enconstruir un arbol de exploracion que cubrierauniformemente todo el espacio libre de colision.Para ello, se desarrollo el algoritmo que se muestraen la figura 1 (LaValle, 1998). Dicho algoritmotiene como mision seleccionar un punto (qrand)de forma aleatoria y extender hacia el el arbol deconfiguraciones. Observese como se hace uso de lafuncion Extiende. Dicha funcion tiene el cometidode ampliar el arbol en el sentido que marca qranddeterminando si existe un camino libre de colision.El esquema de dicha funcion se presenta en lafigura 2.

RRT (qini){

Arbol[0] = qini;Para k = 1 hasta Kmax{

qrand = ConfiguracionAleatoria();Extiende(Arbol, qrand);

}Devuelve Arbol;

}

Figura 1. Algoritmo RRT

El algoritmo comienza inicializando la tabla aso-ciada al arbol con la configuracion origen. Seguida-mente, se entra en un bucle, limitado por un valorKmax, cuya funcion es finalizar el algoritmo unavez se ha realizado un numero prefijado de itera-

Funcion Extiende(Arbol, qrand){

qnear= VecinoMasProximo(qrand, Arbol);Si ( NuevaConfiguracion(qrand, qnear, qnew) ){

A~nadeVertice(Arbol, qnew);Si ( qnew=qrand ){

Devuelve alcanzado ;}Si no{

Devuelve avanzado ;}

}Si no{

Devuelve rechazado ;}

Figura 2. Funcion Extiende

ciones. Este valor, se utilizara posteriormente paraparar el algoritmo en el caso en que no se alcancela configuracion final. Es importante resaltar quela determinacion de dicho valor dependera de lascaractersticas del problema (numero de obstacu-los, tiempo lmite del algoritmo, etc...).

Dentro del bucle del algoritmo RRT hay dosinstrucciones. Con la primera se obtiene un puntoal azar dentro del espacio libre de colision (Cfree);la segunda hace crecer el arbol en direccion a laconfiguracion aleatoria anteriormente obtenida.

Figura 3. Crecimiento del arbol

El crecimiento del arbol se consigue con la fun-cion Extiende. La estructura de dicha funcioncomienza con el calculo de qnear. Esto se reali-za gracias a la funcion VecinoMasProximo queaplica la metrica R definida anteriormente a to-dos los vertices del arbol, obteniendo el puntomas cercano a qrand. Seguidamente, la funcionNuevaConfiguracion calcula qnew, el nuevo pun-to a agregar, mediante un salto de tamano par-tiendo de qnear en direccion hacia qrand (ver figura3). Para la obtencion de qnew se tiene en cuentasi hay alguna colision en dicho desplazamiento,devolviendo verdadero o falso segun sea unmovimiento posible o, por el contrario, colisionecon algun obstaculo.

Si no se ha detectado colision, se agrega el nuevopunto al arbol distinguiendo entre dos casos. Si elpunto aleatorio se encuentra dentro de un crculode centro qnear y radio , entonces se considera quese ha alcanzado qrand y, por tanto, dicho punto hasido alcanzado. El algoritmo notificara entoncestal circunstancia. Si por el contrario (caso mas

MarinaText BoxPlanificacin de Trayectorias con el Algoritmo RRT. Aplicacin a Robots No Holnomos

MarinaText Box58

JLDIEZLine

usual), no se ha producido el alcance, entoncesdevolvera el valor avanzado. Por ultimo, en casode que la funcion NuevaConfiguracion haya ad-vertido de la existencia de algun obstaculo en elcamino de qnear a qnew, se informa de que no hahabido nuevas ramas y el punto no es agregado alarbol.

El comportamiento de este algoritmo con respectoa otros es mejor en cuanto a la homogeneidad delespacio explorado.

La naturaleza del RRT le hace avanzar con masavidez en aquellas zonas donde haya un mayor es-pacio libre, pues es all donde hay mas posibilidadde encontrar puntos qrand factibles. Esto puedecomprenderse observando el crecimiento del arbolen distintos entornos. Observese, por ejemplo, laprogresion del algoritmo representado a traves delas figuras 4, 5, 6 y 7. En este caso, el espaciolibre consiste en un crculo. Esto significa, quedesde el punto central, no hay ninguna preferenciaen cuanto a la direccion de crecimiento. Efecti-vamente, puede verse que las ramas iniciales delarbol surgen en direcciones aleatorias y crecen sinpredominio entre ellas. Otro hecho a observar es lainexistencia de una alta densidad de ramas en elorigen. La naturaleza del RRT evita precisamenteesto. Es mas, se puede contemplar que el arboltiende a lograr una densidad uniforme de ramassobre el espacio libre.

Figura 4. RRT tras 100 iteraciones.

Observese ahora la figura 8. En esta ocasion serepresenta la progresion del algoritmo RRT sobreun espacio diferente. Se observa que, desde el pun-to origen, situado dentro del rectangulo pequeno,parte una rama que crece con acusada prepon-derancia con respecto a las demas. La causa es laalta frecuencia con que aparecen puntos aleatoriossobre el rectangulo de la derecha debido a que quepresenta mayor area libre de obstaculos.





JLDIEZLine

MarinaText Box59

Figura 8. Evolucion en entorno asimetrico.

Otro detalle relevante consiste en el crecimientode ramas principales hacia las esquinas de losrectangulos. Si se compara con el crculo, uncuadrado presenta mas puntos desde su centro acualquiera de sus vertices que en otras direcciones.Esto significa, igualmente, una mayor densidadde puntos aleatorios en dichas direcciones y, portanto, una predominancia de crecimiento.

Al igual que en el caso anterior, tras las suficientesiteraciones, puede observarse que la densidad deramas resulta razonablemente homogenea a pesardel desequilibrio inicial existente entre los dosrectangulos.

Por ultimo, se advierte que este algoritmo solo seocupa de generar un arbol capaz de explorar de

modo equiprobable el espacio libre. Por tanto, noes el metodo mas adecuado para hallar el caminoentre dos puntos. En las siguientes secciones sedetallan algunas extensiones y mejoras del RRTque permiten establecer un camino entre configu-racion origen y destino.

3.2 Algoritmo RRT-Bidireccional basico

La adaptacion del algoritmo RRT para conseguirconectar una configuracion inicial con una final selogra sustituyendo el algoritmo original RRT porel algoritmo RRT-Bidireccional basico mostradoen la figura 9.

RRT-Bidireccional basico(qini, qfin){

ArbolA[0]= qini;ArbolB[0]= qfin;Para k = 1 hasta Kmax{

qrand= ConfiguracionAleatoria( );Si (Extiende(ArbolA, qrand)6=rechazado ){

Si (Extiende(ArbolB, qrand)=alcanzado YExtiende(ArbolA, qrand)=alcanzado )

{Devuelve Camino(ArbolA, ArbolB);

}}Intercambiar(ArbolA, ArbolB)

}Devuelve Error

}

Figura 9. Algoritmo RRT-Bidireccional basico

Este algoritmo se basa en la construccion de dosarboles que parten de los puntos origen y destinosimultaneamente. El algoritmo concluye en cuantodichos arboles conectan. Es decir, cuando la fun-cion Extiende devuelve alcanzado para el segundoarbol. Si alcanzado el valor de Kmax ambos arbolesno han coincidido, se devuelve un mensaje deerror.

Es importante destacar que ambos arboles com-parten el mismo punto qrand para crecer. Es-to se hace para ahorrar coste computacional sinmenoscabar las propiedades de los RRT. Debido aello, sendos arboles tenderan a explorar el espaciovaco, aumentando con el tiempo la probabilidadde conexion.

Observese el uso de la funcion Intercambiar, queintercambia el orden de los arboles de maneraque el crecimiento de ambos sea equilibrado. Siesta funcion no existiera, el ArbolB solo creceracuando la extension del ArbolA no presentaracolision.

Este algoritmo admite ciertas mejoras. Por unaparte, presenta una condicion muy restrictiva yaque exige que en una misma iteracion coincidanambos arboles en un mismo punto. Para relajar es-ta restriccion se ha desarrollado el algoritmo RRT-Ext-Con (Kuffner y LaValle, 2000). Por otra, el


JLDIEZLine

MarinaText Box60

crecimiento de los arboles puede ser dirigido deforma que tiendan el uno hacia el otro, en lugar decrecer por zonas donde la interconexion es difcil.Para conseguir esto se ha desarrollado el algoritmoRRT-Ext-Ext (LaValle y Kuffner, 2001). Seguida-mente se presentan ambos metodos.

3.3 RRT-Ext-Ext

Una mejora que permite agilizar la conexion entrelos arboles en el anterior algoritmo se logra en elRRT-Ext-Ext. Con una muy pequena variacion,es posible atribuirle una capacidad para que ca-da arbol pueda dirigir su crecimiento hacia suhomologo. Esto lo hace mas competitivo frente aotros algoritmos puramente aleatorios.

RRT-Ext-Ext (qini, qfin){


qrand= ConfiguracionAleatoria( );Si (Extiende(ArbolA, qrand) 6=rechazado ){

Si (Extiende(ArbolB, qnew)=alcanzado ){

Devuelve Camino(ArbolA, ArbolB);}

}Intercambiar(ArbolA,ArbolB);

}Devuelve Error;

}

Figura 10. Algoritmo RRT-Ext-Ext.

Este sutil cambio con respecto al algoritmo an-terior (figura 9) es posible observarlo en la figura10, donde el punto qrand se sustituye en el segundoarbol por qnew, es decir, el punto recien agregadoal arbol precedente. Finalizada la iteracion, lafuncion Intercambiar se ocupa de que la proximavez, sea el ultimo arbol el que crezca hacia qrandy el primero hacia qnew. As pues, las accionesresultantes de dos iteraciones consecutivas seran:

1. ArbolA crece hacia qrand (1a iteracion).

2. ArbolB crece hacia qnew de ArbolA.3. ArbolB crece hacia qrand (2

a iteracion).4. ArbolA crece hacia qnew de ArbolB.

De esta forma cada arbol invierte la mitad desu tiempo en explorar el espacio libre, y la otramitad, en buscar a su companero.

3.4 Algoritmo RRT-Ext-Con

En este algoritmo (figura 11) se sustituye la fun-cion Extiende, que agregaba un nuevo segmentoal arbol, por otra denominada Conecta (ver figura12), mas ambiciosa, que va agregando segmentosconsecutivos hasta alcanzar la configuracion obje-tivo o un obstaculo (Kuffner y LaValle, 2000).

RRT-Ext-Con (qini, qfin){


qrand= ConfiguracionAleatoria( );Si (Extiende(ArbolA, qrand)6=rechazado ){

Si (Conecta(ArbolB, qnew)=alcanzado ){

Devuelve Camino(ArbolA, ArbolB);}

}Intercambiar(ArbolA,ArbolB);

}Devuelve Error;

}

Figura 11. Algoritmo RRT-Ext-Con.

Funcion Conecta(Arbol, q){

Repetir{

S=Extiende(Arbol, q);} Mientras (S=avanzado );Devuelve S;

}

Figura 12. Funcion Conecta

Se atenua as el problema de la excesiva restric-tividad del algoritmo Bidireccional basico. Ahora,los segmentos agregados pueden ser multiples, conlo que para conectar basta con que exista solo uncamino rectilneo libre entre el punto qnew y elvertice mas proximo del otro arbol.

La unica desventaja consiste en el mayor costecomputacional de la funcion Conecta, que secompensa en muchos escenarios con la mayoreficiencia del algoritmo.

Este algoritmo presenta una gran eficacia cuan-do se trata de planificar trayectorias para sis-temas holonomos. Por el contrario, como se ilustraen (Cheng y LaValle, 2001), el algoritmo RRT-Ext-Ext representa la mejor opcion cuando setrata de planificar trayectorias para sistemas noholonomos.

4. POSTPROCESADO

Los arboles obtenidos en la aplicacion de los dis-tintos algoritmos RRT pueden resultar comple-jos para ser recorridos. Normalmente, los arbolesson susceptibles de simplificacion (por ejemplo,en algunos casos los extremos de ambos arbolespodran unirse con una simple lnea recta).

Por consiguiente, en las aplicaciones practicas delmetodo RRT resulta conveniente aplicar a losarboles obtenidos un postprocesado que permitareducir su irregular topologa. En este sentido, aligual que ocurre en otras tecnicas de planificacion,se suelen aplicar algoritmos de simplificacion, muyrapidos y sencillos, que parten de la informacion


JLDIEZLine

MarinaText Box61

suministrada por el planificador y generan, deforma iterativa, un camino mas simple. Dichosalgoritmos suelen tener un coste computacionalbajo.

Notese, que el algoritmo de postprocesado de-bera ser disenado en funcion de la aplicacion con-creta, constituyendo un elemento mas del sistemade planificacion.

As, por ejemplo, para el caso de planificacion deuna trayectoria del objeto de planta rectangularmostrado en la figura 14, se ha utilizado la funcionque se muestra en la figura 13, inspirada enun algoritmo de postprocesado introducido en(Laumont et al., 1994).

Funcion Postprocesado(CaminoOriginal){

fin = 0;Mientras ( no fin ){

colision = 1;i=NumeroDeVertices;Mientras ( colision ){

Si ( CuerdaValida(1, i) ){

EliminaArco(1,i,CaminoOriginal,CaminoFinal);colision=0;

}Si no{

i=i/2;}Si ( i 2 ){

EliminaArco(1,2,CaminoOriginal,CaminoFinal);colision=0;

}}Si ( NumeroDeVertices 2 ){

fin=1;}

}Devuelve(CaminoFinal);

}

Figura 13. Funcion Postprocesado.

Este algoritmo permite obtener un camino finalsimplificado a partir del arbol original suministra-do por el RRT. La idea del algoritmo consisteen eliminar aquellos tramos del arbol que puedanser sustituidos por una trayectoria recta libre decolision.

Como se puede ver, NumeroDeVertices es unafuncion que devuelve el numero de vertices quecomponen el camino original e i representa elnumero de vertices que se pretende sustituir. Enun principio, el algoritmo prueba a sustituir todoel camino original por una trayectoria recta. Siesto no es posible, i va adquiriendo un valor cadavez menor (tpicamente se escoge el punto mediodel tramo de camino con el que se prueba).

En cada iteracion, la funcion CuerdaValida(1,j) establece una ruta recta entre el vertice 1 y el j,y genera un conjunto de puntos alternativos sobrelos que se comprueba la posibilidad de colision. Si

la ruta presenta colisiones, se intenta estableceruna nueva ruta recta, pero ahora entre el primervertice y el punto medio del trozo de camino conel que se acaba de probar.

Si en una de las iteraciones la ruta es valida,es decir es libre de colisiones, entonces se dapaso a la funcion EliminaArco, que sustrae delcamino original los vertices que van desde el1 al j, disminuyendo, por tanto, el valor quedevolvera NumeroDeVertices e incorporando lanueva ruta al camino final. Por tanto, el caminooriginal queda configurado con un nuevo verticeinicial y el camino final con un nuevo tramo recto.

En la figura 14a se presenta un escenario dondeaun no se ha aplicado el postprocesado. Se mues-tran los arboles generados por el RRT as como,las distintas posiciones y orientaciones del robotmovil a lo largo del camino solucion. Una vezobtenido dicho camino se procede a su postproce-sado. En la figura 14b se muestra el camino sinlos arboles de generacion. La figura 14c ilustrael postprocesado aplicado a la primera mitad delcamino, mientras que la figura 14d muestra elcamino final resultado del proceso de postproce-sado.

5. APLICACION DE ALGORITMO RRT ENSISTEMAS NO HOLONOMOS

5.1 Sistemas no holonomos

Todos los vehculos rodados presentan restric-ciones no holonomas. Considerese, por ejemplo,un vehculo de planta rectangular y conducciondiferencial cuya configuracion puede determinarsemediante las coordenadas de un punto de referen-cia (se suele escoger el centro del eje de las ruedas,ver figura 15) y el valor de , la orientacion deleje longitudinal. El movimiento del vehculo puedemodelarse mediante la ecuacion (Ollero, 2001):

x

y

=

r cos

2

r cos

2

r sin

2

r sin

2

r

2l

r

2l

1

2

. (1)

En estos vehculos, ver figura 15b, la direccion delmovimiento esta controlada por las velocidades derodado de cada una de las ruedas (1, 2).

Este vehculo presenta la siguiente restriccion cin-ematica (Ollero, 2001):

x0 sin y0 cos = 0. (2)

Esta ecuacion implica la imposibilidad de moverel vehculo en una direccion distinta a la determi-


JLDIEZLine

MarinaText Box62

a)

b)

c)

d)

Figura 14. Planificacion y postprocesado.

Figura 15. Vehculo con conduccion diferencial.

nada por la orientacion del vehculo; es decir, solose puede mover en el sentido del eje longitudinalde las ruedas.

5.2 RRT y sistemas no holonomos

La aplicacion del algoritmo RRT a sistemas conrestricciones no holonomas presenta algunos pro-blemas. As, en muchas ocasiones, el caminoobtenido viola las restricciones cinematicas delsistema (el camino resultante es tal que el vehcu-lo ha de moverse en direcciones distintas a ladeterminada por su orientacion, ver figura 14).Ademas, no es inmediato determinar la secuenciade movimientos necesarios para conseguir que elmovil se desplace segun el camino planificado.

Los metodos existentes en la actualidad se basanen una modificacion sobre el RRT original (LaValle,1998). Consisten en considerar un conjunto dis-creto de ordenes de control. A la hora de extenderel arbol, se escogen los vertices mas cercanos aqrand y se les aplican todas y cada una de lasposibles senales de control. De entre los puntosas generados se escoge el mas proximo y se agregaal arbol (LaValle y Kuffner, 1999).

Estos metodos tienen varios inconvenientes (LaValley Kuffner, 1999). Por una parte, es necesariodefinir la metrica que permita generar trayectoriafactibles, lo cual es un problema de cierta relevan-cia (LaValle y Kuffner, 1999; Cheng et al., 2001).


JLDIEZLine

MarinaText Box63

En muchos casos es necesario hacer uso de unaheurstica particular, con la que hay que tenerun especial cuidado debido a la alta sensibilidaddel algoritmo respecto de este elemento (Chengy LaValle, 2001; Cheng et al., 2001). Por otra,el coste computacional se eleva: el primero delos metodos necesita aplicar al camino obtenidotecnicas de optimizacion que permitan filtrar laaparicion de fluctuaciones inutiles en las senales decontrol aplicadas (Cheng y LaValle, 2001; Chenget al., 2001); en el caso del segundo metodo, seplantea el problema del retardo que supone aplicartodas las ordenes de control.

Los mejores resultados obtenidos al aplicar estosmetodos se han conseguido considerando ordenesde control que generan lneas rectas y giros a laderecha, y a la izquierda, ambos con curvaturaacotada. Si durante el movimiento solo se utilizanvalores de velocidad positiva, se trata de unaplanificacion basada en los resultados de Dubins(1957); si por el contrario se consideran valoresde velocidad positivos y negativos se trata de unaplanificacion basada en los resultados de Reeds yShepp (1990).

5.3 Maniobras restringidas y RRT

La solucion propuesta en este artculo se basa enla aplicacion del concepto de maniobra restringida(Gomez-Bravo et al., 2001). Este tipo de trayec-torias representan un conjunto de maniobras quepermiten variar una y solo una de las coorde-nadas del espacio de configuracion. As, es posibleestablecer una trayectoria valida desde cualquierpunto del arbol hasta el nuevo punto a agregar consolo obtener la correspondiente combinacion demaniobras. Dado que son maniobras predefinidas,es extremadamente sencillo obtener una planti-lla del espacio que ocupara el vehculo durantesu ejecucion. Por tanto, es posible establecer laposibilidad de colision sin necesidad de computarexplcitamente la trayectoria.

Por ejemplo, para el caso del vehculo no holonomode conduccion diferencial, se consideran dos ma-niobras restringidas. La primera consiste en elcambio de orientacion del vehculo, manteniendola posicion cartesiana del punto de referencia.Suponiendo que el vehculo tiene una planta rec-tangular, la plantilla que define esta maniobra esun crculo centrado en el punto de referencia y deradio igual a la distancia de los vertices a dichopunto. La segunda maniobra consiste en desplazarlongitudinalmente el vehculo, variando en estecaso la coordenada x del sistema de referenciade igual orientacion a la del robot. La plantillaasociada a esta maniobra es un rectangulo, delongitud igual al desplazamiento efectuado y deanchura identica a la de la planta del vehculo.

Cualquier movimiento generico del vehculo es-tara compuesto de un giro para orientarse a laposicion de destino, un desplazamiento hasta elpunto destino y una nueva reorientacion para al-canzar la orientacion final deseada.

a)

b)

Figura 16. RRT aplicado a un robot de conducciondiferencial.

En este artculo, en lugar de utilizar el algoritmoRRT-Ext-Ext con un conjunto discreto de ordenesde control, se propone emplear el algoritmo RRT-Ext-Ext original junto con un postprocesado basa-do en el concepto de maniobra restringida. As,el RRT sigue manteniendo su velocidad de com-putacion, y es la progresiva simplificacion de latrayectoria la que depara que el camino no violelas restricciones no holonomas.

As, partiendo del algoritmo de postprocesadopropuesto en la Seccion 4, la idea consiste encomprobar, en las sucesivas iteraciones, la posibili-dad de conexion entre vertices del arbol mediantealguna maniobra restringida. De este modo, lafuncion CuerdaValida, en lugar de establecer unalnea recta entre vertices, comprueba que la plan-tilla asociada a la maniobra no presenta colisiones.

Por tanto, el camino completo se segmenta aldescomponerlo en una secuencia de movimientosacordes con las restricciones no holonomas. En lafigura 16 se ilustran los distintos pasos seguidos enla obtencion de un camino libre de colisiones paraun vehculo de conduccion diferencial. En la figura16a se presentan los arboles y el camino obtenidoal aplicar el algoritmo RRT-Ext-Ext. Por su parte,en la figura 16b, se muestra el resultado de aplicarel suavizado mediante las maniobras descritas.


JLDIEZLine

MarinaText Box64

Para evaluar la tecnica propuesta con los meto-dos existentes se han realizado diversas compara-ciones, incluyendo distintos tipos de vehculos.As, la figura 17 presenta una comparacion entreuna trayectoria para un vehculo de conducciondiferencial generada utilizando el algoritmo RRTcon un conjunto discreto de consignas de control(ver figura 17a) y una trayectoria generada me-diante el metodo propuesto (ver figura 17b). Elentorno considerado es el presentado en LaValley Kuffner (2001), y se ha elegido para facilitar lacomparacion.

a)

b)

Figura 17. Comparacion de aplicacion a un robotcon conduccion diferencial. a) RRT con con-junto discreto de consignas de control. b)RRT basado en maniobras restringidas.

El conjunto discreto de consignas de control consi-derado en la figura 17a, esta compuesta de catorceposibilidades, incluyendo, por ejemplo, marchahacia adelante, marcha hacia atras y giros de

diversa amplitud. Tras realizar una serie de diezexperimentos se obtuvieron los datos mostradosen la tabla 1. Los algoritmos estan implementadosen JAVA y las simulaciones se realizaron con unprocesador AMD K7 1700 a 1,4MHz.

Tabla 1. Comparacion aplicacion RRTa vehculo diferencial.

Tiempo Desv. Long. Desv.

medio tpica media tpicacalculo tiempo camino long.(ms) (ms) (m) (m)

RRT

consignas 7620,0 3563,8 179,7 34,1

discretas

RRT

maniobras 652,7 93,8 51,9 4,7

retringidas

Asimismo, la figura 18 muestra una comparacionentre una trayectoria para un vehculo con con-figuracion Ackerman, similar a un coche (Ollero,2001), con radio de curvatura mnimo de 2,5 me-tros, utilizando el algoritmo RRT con un conjuntodiscreto de consignas de control (ver figura 18a)y una trayectoria generada mediante el metodopropuesto (ver figura 18b).

En este caso las maniobras restringidas son lasdescritas en (Gomez-Bravo et al., 2001). Por suparte, el conjunto de consignas de control estabacompuesto tambien por catorce posibilidades, in-cluyendo giros de curvatura acotada. Tras realizaruna serie de diez experimentos se obtuvieron losdatos mostrados en la tabla 2.

Tabla 2. Comparacion aplicacion RRTa vehculo con configuracion Ackerman.

Tiempo Desv. Long. Desv.

medio tpica media tpicacalculo tiempo camino long.

(ms) (ms) (m) (m)

RRT

consignas 11408,2 7146,7 173,2 31,0

discretas

RRT

maniobras 701,8 73,5 84,7 3,4

restringidas

Como muestran los datos, el metodo propuesto re-quiere menor tiempo de calculo y genera caminosmas cortos. De este modo, el elevado coste com-putacional requerido por el algoritmo RRT alconsiderar un conjunto discreto de ordenes, secompensa con la utilizacion del algoritmo RRT-Ext-Ext original (mucho mas rapido) y un post-procesado, en el que el ahorro de tiempo se debe,fundamentalmente, a que para la deteccion decolisiones se utiliza la plantilla asociada a cadamaniobra en lugar de los puntos de la trayectoria.

Finalmente, para ilustar la efectividad del meto-do, en la figura 19 se presenta un experimentocon un vehculo Ackerman, cuyo radio mnimo


JLDIEZLine

MarinaText Box65

a)

b)

Figura 18. Comparacion de aplicacion a un robotcon configuracion Ackerman. a) RRT conconjunto discreto de consignas de control. b)RRT basado en maniobras restringidas.

de curvatura es de 6 metros, en un entorno mascomplejo dada su maniobrabilidad.

6. CONCLUSIONES

Los algoritmos puramente aleatorios de planifi-cacion de caminos (tales como RRT y sus exten-siones) constituyen una alternativa a los metodosde planificacion convencionales para determina-dos entornos complejos y robots con restricciones.Los requisitos computacionales hacen viable suimplementacion en tiempo real, incluso para unaposible replanificacion en lnea ante obstaculos nomodelados. En este sentido la inclusion de lasmaniobras restringidas como herramienta en los

Figura 19. Aplicacion de RRT-Ext-Ext con ma-niobras restringidas a un robot con configu-racion Ackerman.

algoritmos existentes puede suponer una mejorainteresante.

7. AGRADECIMIENTOS

Este trabajo ha sido parcialmente financiado porel proyecto DPI2005-02293.

REFERENCIAS

Bruce, J., and M. Veloso (2002). Real-Time Ran-domized Path Planning for Robot Naviga-tion, Proc. IROS 2002, Switzerland.

Cheng, P., and S. M. LaValle (2001). Reducingmetric sensitivity in randomized trajectorydesign. In: Proc. IEEE/RSJ Intl Conf. onIntelligent Robots and Systems, pp. 4348.

Cheng, P., Z. Shien and S. M. LaValle(2001). RRT-Based Trajectory Design for Au-tonomous Automobiles and Spacecraft.

Dubins, L.E. (1957). On curves of minimal lengthwith a constraint on average curvature andwith prescribed initial and terminal posi-tion and tangents. Amer. J. Math., Vol 79,pp. 497516.

Gomez Bravo F., F. Cuesta and A. Ollero. (2001).Parallel and diagonal parking in nonholonom-ic autonomous vehicles. Engineering Aplica-tion of Artificial Inteligence. Vol 14 No. 1,pp. 419434.

Kuffner, J.J., and S. M. LaValle (2000). RRT-connect: An efficient approach to single-querypath planning. In: Proc. IEEE Intl Conf. onRobotics and Automation, pp. 9951001.

Latombe, J.C. (1991). Robot Motion Planning,Kluwer Academic Pulisher.

Laumont, J.P., P.E. Jacobs, M. Taix and M.Murray (1994). A Motion Planner for Non-holonomic Mobile Robots. IEEE Trans. onRobotics and Autom, Vol 10, No 5: pp. 577593.

LaValle, S.M. (1998). Rapidly-exploring randomtrees: A new tool for path planning. ComputerScience Dept., Iowa State University TR 98-11.


JLDIEZLine

MarinaText Box66

LaValle, S. M., and J. J. Kuffner (1999). Ran-domized kinodynamic planning. In: Proc.IEEE Intl Conf. on Robotics and Automa-tion, pp. 473479.

LaValle, S.M., and J.J. Kuffner (2001). Rapidly-Exploring Random Trees: Progress andProspects. In Algorithmic and Computation-al Robotics: New Directions. B. R. Donald, K.M. Lynch, and D. Rus, editors, , pp. 293308.

Munoz, V. (1995). Planificacion de Trayectoriaspara Robots Moviles. Tesis Doctoral. Univer-sidad de Malaga.

Ollero, A. (2001). Robotica: manipuladores yRobots Moviles. Marcombo Boixareu.

Reeds, J.A. and R.A. Shepp, (1990). Optimalpaths for a car that goes both forward andbackward. Pacific J. Math., Vol 145 n 2,pp. 367393.


JLDIEZLine

MarinaText Box67

Documents

Planificación de Trayectorias Con El Algoritmo RRT - Aplicación a Robots No Holónomos