Repositorio Institucional de Documentos

Proyecto Fin de Carrera Ingenier´ıa Industrial Curso 2011-2012 Navegaci´on de robots aut´onomos en entornos din´amicos Cristina Men´endez Romero Director: Luis Montano Gella Departamento de Inform´atica e Ingenier´ıa de Sistemas Escuela de Ingenier´ıa y Arquitectura Universidad de Zaragoza Junio 2012 3 NAVEGACI´ ON DE ROBOTS AUT´ ONOMOS EN ENTORNOS DIN ´ AMICOS Resumen Uno de los principales objetivos de la rob´otica es desarrollar m´etodos robustos de planificaci´on aut´onoma de trayectorias. En la mayor´ıa de las situaciones pueden aparecer otros obst´aculos m´oviles que dificultan la navegaci´on y con los que es necesario tratar en tiempo real para poder asegurar en todo momento la integridad del robot mientras realiza sus tareas. Este proyecto se enmarca en los proyectos de rob´otica desarrollados por el Grupo de Rob´otica, Percepci´on y Tiempo Real de la Universidad de Zaragoza. El objetivo general es desarrollar una t´ecnica de navegaci´on para un veh´ıculo aut´onomo en un entorno din´amico y no estructurado. El veh´ıculo debe ser capaz de maniobrar con rapidez y seguridad. Todo esto se logra planificando trayectorias en el espacio din´amico de velocidades y tiempo (DVTS) del robot. En trabajos anteriores, se desarrollaron las t´ecnicas para representar la informaci´on de los obst´aculos tanto est´aticos como din´amicos en el espacio de velocidades del robot, considerando los pares de velocidades para los cuales podr´ıa aparecer colisi´on a partir de un tiempo dado. Adem´as, se desarroll´o una t´ecnica de planificaci´on mediante un ´arbol de decisiones, que trabajaba con la informaci´on proyectada en el plano de velocidades (v,w). Se trata de una t´ecnica heur´ıstica basada en el reconocimiento de situaciones con las que se encuentra el robot y a las que asocia un conjunto de acciones. Este proyecto da un paso m´as, desarrollando una t´ecnica que minimiza el tiempo de alcance de los subobjetivos de la trayectoria a la vez que genera movimientos seguros. Para ello emplea adem´as de la informaci´on del espacio de velocidades, la informaci´on de tiempo para obtener trayectorias m´as ajustadas y eficientes. La b´usqueda de la trayectoria ´optima est´a dirigida por una funci´on de coste, en la que se incluyen las restricciones cinem´aticas y din´amicas del robot (no todos los movimientos son posibles, ni todas las velocidades alcanzables de forma instant´anea), criterios de seguridad del robot, criterios de selecci´on de estrategias de evitaci´on y criterios de distancia hasta el objetivo. La obtenci´on de la informaci´on del entorno se realiza mediante un esc´aner l´aser de 180 ode rango y 0.5ode precisi´on. Este dispositivo limita el tiempo de refresco del programa a 250 milisegundos. Para el tratamiento de la informaci´on obtenida a trav´es del l´aser y de la odometr´ıa se emplean algoritmos desarrollados previamente en el laboratorio. El proyecto se desarrolla en el lenguaje de programaci´on C++. La interfaz proporcionada por la plataforma de software para rob´otica Player se emplea para interactuar con los robots en los diferentes experimentos realizados. Estos experimentos constan tanto de simulaciones llevadas a cabo en la plataforma Stage como de experimentos reales empleando los robots Pioneer del laboratorio. ´ Indice general 1. Introducci´on 1 1.1. Motivaci´on.................................. 1 1.2. Estadodelarte ............................... 2 1.3. Estructura de la memoria . . . . . . . . . . . . . . . . . . . . . . . . . 3 2. El espacio de control y la b´usqueda en el mismo 5 2.1. El espacio de velocidades-tiempo . . . . . . . . . . . . . . . . . . . . . 5 2.2. C´alculo de trayectorias en el espacio DVTS . . . . . . . . . . . . . . . . 8 2.3. Discretizaci´on del DVTS . . . . . . . . . . . . . . . . . . . . . . . . . . 10 2.3.1. Resoluci´on de la malla: tama˜no de celda . . . . . . . . . . . . . 10 2.3.2. Horizonte Temporal . . . . . . . . . . . . . . . . . . . . . . . . . 11 2.3.3. Obtenci´on de la superficie completa . . . . . . . . . . . . . . . . 11 3. B´usqueda de trayectorias en DVTS 13 3.1. Introducci´on al Algoritmo A* . . . . . . . . . . . . . . . . . . . . . . . 13 3.2. El operador de expansi´on . . . . . . . . . . . . . . . . . . . . . . . . . . 15 3.3. Costesyseguridad ............................. 17 4. Estrategias de navegaci´on 23 4.1. Heur´ısticas para la selecci´on de subobjetivos . . . . . . . . . . . . . . . 24 4.1.1. Integraci´on de estrategias . . . . . . . . . . . . . . . . . . . . . 25 4.2. Maniobras de adelantamiento . . . . . . . . . . . . . . . . . . . . . . . 27 5. M´odulos del programa 31 5.1. Estructura del programa . . . . . . . . . . . . . . . . . . . . . . . . . . 31 6. Simulaciones 33 6.1. Cruce .................................... 34 6.2. Escenario sin obst´aculos est´aticos . . . . . . . . . . . . . . . . . . . . . 35 6.3. Escenario de interiores con obst´aculos est´aticos y m´oviles . . . . . . . . 36 6.4. Adelantamiento............................... 38 7. Experimentos 41 7.1. Hall ..................................... 42 7.2. Adelantamiento............................... 44 5 6 8. Conclusiones y trabajo futuro 47 8.1. Conclusiones................................. 47 8.2. Limitaciones y Trabajo Futuro . . . . . . . . . . . . . . . . . . . . . . . 48 A. Selecci´on de estrategias 51 A.1. El ´arbol de Decisi´on . . . . . . . . . . . . . . . . . . . . . . . . . . . . 51 A.2.Adelantamiento............................... 52 A.2.1. Clotoides en el cambio de carril . . . . . . . . . . . . . . . . . . 52 A.2.2. Maniobra de adelantamiento . . . . . . . . . . . . . . . . . . . . 53 B. Filtros y estimadores 55 B.1. Filtro de Kalman extendido . . . . . . . . . . . . . . . . . . . . . . . . 55 B.2.Estimadores................................. 56 B.2.1. Clasificaci´on de puntos . . . . . . . . . . . . . . . . . . . . . . . 56 B.2.2. Autolocalizaci´on . . . . . . . . . . . . . . . . . . . . . . . . . . 57 B.2.3. Obst´aculos est´aticos . . . . . . . . . . . . . . . . . . . . . . . . 57 B.2.4. Obst´aculos m´oviles . . . . . . . . . . . . . . . . . . . . . . . . . 58 C. Simulaciones 61 C.1.Cruce .................................... 61 C.2.Caos..................................... 64 C.3.Interiores .................................. 67 C.4.Adelantamiento............................... 72 D. Experimentos 77 D.1.Hall ..................................... 77 D.2.Adelantamiento............................... 81 E. Consideraciones 87 E.1. L´ımites del espacio de velocidades . . . . . . . . . . . . . . . . . . . . . 87 E.2. Pruebas de coeficientes . . . . . . . . . . . . . . . . . . . . . . . . . . . 89 ´ Indice de figuras 93 Bibliograf´ıa 95 8 Cap´ıtulo 1 Introducci´on 1.1. Motivaci´on El objetivo principal de este proyecto es dotar al robot de un planificador local capaz de integrar toda la informaci´on sensorial disponible y con ella seleccionar la trayectoria ´optima para alcanzar el objetivo. Se supone la existencia de un planificador global que se˜nala los objetivos intermedios o puntos v´ıa. El planificador local desarrollado es el encargado de resolver la trayectoria ´optima entre los puntos v´ıa. Concretamente, las etapas necesarias para alcanzar el objetivo de este proyecto son las siguientes: Establecer una discretizaci´on del espacio tridimensional de velocidades-tiempo que permita calcular la trayectoria y tratar la informaci´on de forma r´apida y eficiente. Desarrollar un m´etodo de planificaci´on de trayectorias basado en un algoritmo de b´usqueda A*. Dicho algoritmo incluye unos t´erminos de ponderaci´on que dirigen la b´usqueda hacia trayectorias seguras y r´apidas y trabaja sobre el espacio de velocidades-tiempo. Integrar el algoritmo en un programa que pueda ejecutarse en los robots del laboratorio. La integraci´on implica trabajar con algoritmos de tratamiento de datos sensoriales desarrollados en el laboratorio en trabajos previos. Estos algoritmos son los de detecci´on, seguimiento y modelado de los objetos m´oviles. Validar el planificador desarrollado mediante los resultados obtenidos en experimentos simulados y reales. 1 8 2.2. C´alculo de trayectorias en el espacio DVTS En trabajos previos relativos al c´alculo de trayectorias en entornos din´amicos, el espacio habitual de trabajo es el de configuraciones extendido en el tiempo. En el presente trabajo el espacio de configuraciones se representa en el espacio de velocidadestiempo. De esta forma la planificaci´on de trayectorias consiste en la b´usqueda en este espacio de la trayectoria que minimiza el tiempo de alcance de los objetivos, lo que tiene albunas ventajas sobre los m´etodos cl´asicos al trabajar sobre el espacio de control del robot. N´otese que al considerar el DVTS est´a impl´ıcita la informaci´on de aceleraciones, es decir, las restricciones din´amicas se pueden representar de manera natural en este espacio tal y como se explicar´a m´as adelante con los operadores de expansi´on. El espacio es discretizado en prismas rectangulares, y el espacio delimitado por las superficies prohibidas calculadas se convierte en celdas ocupadas, quedando el resto del espacio como celdas libres en las que se realizar´a la b´usqueda. En la secci´on 2.3.3 se amplia. Para realizar la b´usqueda se trabaja con un algortimo A*(cap´ıtulo 3) que va expandiendo un ´arbol de celdas guiado por una funci´on de costes que pondera t´erminos de seguridad, estrategias de evitaci´on y cercan´ıa al objetivo. Las im´agenes de la figura 2.2 muestran el proceso de trabajo en el DVTS. En primer lugar se calculan las superficies prohibidas para los obst´aculos m´oviles y est´aticos. En la figura 2.2a se observan dos obst´aculos m´oviles y uno est´atico (la superficie gris). A continuaci´on las celdas correspondientes a las superficies de velocidades prohibidas y superiores se marcan como ocupadas en la malla discretizada, figura 2.2b. En la figura 2.2c se muestra la b´usqueda realizada por el A* entre las casillas no ocupadas. En la ´ultima figura, 2.2d, se aprecia como la planificaci´on discurre por debajo de superficies prohibidas, algo que en el trabajo anterior era evitado ya que se intentaba siempre mantener la velocidad en zonas libres. CAP´ ITULO 2. EL ESPACIO DE CONTROL Y LA B´ USQUEDA EN EL MISMO 9 (a) Superficies prohibidas (b) Discretizaci´on del DVTS (c) B´usqueda en el DVTS (d) Vista general Figura 2.2: Espacio de velocidades tiempo 10 2.3. Discretizaci´on del DVTS El espacio de trabajo es el espacio de velocidades lineales, velocidades angulares y tiempo. Es un espacio tridimensional semiacotado por las velocidades m´aximas alcanzables y un horizonte de tiempo. Para trabajar en este espacio, se discretiza con prismas rectangulares de longitudes constantes, deltav,deltawydeltat. Cada celda se identifica mediante sus coordenadas centrales en v y w, y su menor tiempo. Adem´as la celda contiene informaci´on sobre su estado (correspondiente a estar ocupada o al valor que adquiere en el algoritmo de b´usqueda). Todas las celdas se almacenan en una malla, para la que se definen las operaciones de c´alculo de superficies est´aticas y din´amicas as´ı como otras operaciones necesarias en el programa. 2.3.1. Resoluci´on de la malla: tama˜no de celda En toda discretizaci´on, el tama˜no de malla juega un rol importante. La discretizaci´on de una malla puede ser constante o variable. Aunque una discretizaci´on variable permitir´ıa una mayor resoluci´on en zonas conflictivas y una menor resoluci´on en zonas menos interesantes, se opt´o por una discretizaci´on constante para homogeneizar todo el programa, as´ı como los m´etodos de c´alculo. Un razonamiento similar se emple´o para descartar los m´etodos de descomposici´on aproximada de celdas [Latombe(1991)], que adem´as de introducir una discretizaci´on variable aumentaban el tiempo de c´omputo en cada iteraci´on al hacer necesario rehacer el mallado para cada conjunto de superficies. La estructura de celdas permanece constante a lo largo de todo el programa, con la misma discretizaci´on predefinida al lanzarlo. Para definir el tama˜no de las celdas, era necesario llegar a un compromiso entre la precisi´on alcanzada y el coste computacional que conlleva un aumento de la resoluci´on. Con la variaci´on de tiempo deltat, fue sencillo ya que posee un significado f´ısico que corresponde a la duraci´on de cada iteraci´on o actualizaci´on del laser adem´as del periodo entre lanzamientos de ´ordenes. Deltavydeltawse definieron buscando un equilibrio entre la resoluci´on de la malla y la velocidad de c´alculo. Figura 2.3: Tiempos de c´alculo de una superficie seg´un el tama˜no de celda CAP´ ITULO 2. EL ESPACIO DE CONTROL Y LA B´ USQUEDA EN EL MISMO11 En la figura 2.3 aparecen las estad´ısticas del tiempo de c´omputo de la superficie tridimensional para diferentes discretizaciones de la malla. Se observa como emplear mallas m´as grandes favorece considerablemente el tiempo de c´omputo. Para tama˜nos de celda mayores a los que aparecen en la figura 2.3 el espacio de b´usqueda resultaba demasiado restrictivo, lo que conduc´ıa a problemas de b´usquedas no finalizadas. Finalmente la discretizaci´on empleada es de deltav= 0.1 m/s y deltaw= 0.1 rad/s. 2.3.2. Horizonte Temporal Una variable muy importante en este algoritmo es el horizonte temporal hasta el cual se considera la planificaci´on. Planificar a un tiempo demasiado corto deja obst´aculos importantes sin tratar, mientras que horizontes demasiado tard´ıos aumentan el tiempo de c´omputo. Una de las premisas de este programa es su continua actualizaci´on, lo que favorece emplear horizontes de planificaci´on no muy largos. El horizonte temporal m´aximo con el que se trabaja finalmente es de 5 s. No obstante, para cada iteraci´on se calcula el tiempo estimado de alcance del objetivo cartesiano a la velocidad actual y se elige como horizonte de planificaci´on temporal el menor de los dos. Por encima del horizonte temporal no se buscan trayectorias, y en caso de que las superficies de los obst´aculos superen este nivel, se proyecta sobre el ´ultimo plano de casillas la superficie que quedar´ıa por encima. 2.3.3. Obtenci´on de la superficie completa A partir de los pares de velocidades calculados con la Banda de Colisi´on [Owen and Montano(2005)], se interpola la superficie completa. En una primera aproximaci´on, en este trabajo para dos pares de comandos consecutivos se calculaba la superficie comprendida entre los cuatro puntos mediante una factorizaci´on SVD con la librer´ıa ALGLIB [Alglib()], calculando para cada celda su posici´on relativa respecto a la superficie. Sin embargo este m´etodo ten´ıa problemas de convergencia en demasiadas ocasiones, adem´as de resultar muy lento. Por ello se modific´o la estrategia y se pas´o a calcular una serie de puntos intermedios que cubriesen todas las celdas superficie intermedia tal y como se muestra en la figura siguiente. 12 Figura 2.4: Mallado generado entre dos pares de velocidades consecutivos Dado que la superficie prohibida define el l´ımite de los comandos prohibidos, en la malla se proh´ıben las celdas que contienen la superficie y las de tiempos superiores. Cap´ıtulo 3 B´usqueda de trayectorias en DVTS 3.1. Introducci´on al Algoritmo A* El algoritmo de b´usqueda A* pertenece a los algoritmos de b´usqueda en grafos. Bajo unas determinadas condiciones encuentra el camino de menor coste entre un nodo origen y un nodo objetivo. El algoritmo A* se emplea para encontrar un camino de celdas libres desde las velocidades actuales Ninicial hasta el objetivo indicado Ngoal, [Latombe(1991)]. Este algoritmo explora iterativamente las celdas o nodos, abriendo diferentes caminos desde el nodo inicial. Para cada nodo visitado N, el algoritmo ha producido uno o varios caminos conect´andolo con Ninicial pero solo memoriza la representaci´on del camino cuyo coste asociado es m´ınimo. En cada iteraci´on de este algoritmo, el conjunto de nodos expandidos forma un ´arbol de expansi´on con origen en Ninicial. Cada nodo visitado nace de su predecesor, por lo que para obtener la trayectoria desde un nodo Nhasta el inicial, basta con recorrer el ´arbol hacia su origen. El algoritmo A* asigna a cada nodo explorado una funci´on de coste. Los nodos explorados se insertan en una lista denominada Abiertos, en la que se ordenan de menor a mayor coste. En cada iteraci´on se analiza uno de los nodos de Abiertos, el de menor coste. Mediante el operador de expansi´on, introducido en el apartado 3.2 se van abriendo sus casillas vecinas. Para cada celda abierta se comprueba que est´a libre de colisi´on, que es alcanzable en ese nivel temporal y se calcula el coste asociado. Si est´a libre de colisi´on la celda aparecer´a marcada en la malla como vac´ıa ,en cuyo caso se estima el coste asociado mediante una funci´on de coste.B´asicamente, se trata de una estimaci´on heur´ıstica del coste asociado. La funci´on consta de dos t´erminos, f(N) = g(N) + h(N) (3.1) donde g(N) representa el coste asociado al camino desde el nodo inicial Ninicio hasta el nodo actual N,h(N) es una estimaci´on heur´ıstica del coste del camino de menor coste entre NyNobjetivo que incluye el coste asociado a la seguridad. Los detalles del m´etodo de c´alculo de estos t´erminos se encuentran en la secci´on 3.3. 13 14 Una vez se han visitado todos los vecinos, se marca la celda como ya expandida y se a˜nade al ´arbol de expansi´on. A continuaci´on, en la figura 3.1 muestra un esquema del proceso de expansi´on. Desde la celda a se abren las celdas adyacentes. b, d, e y g est´an marcadas como prohibidas por los que no se expanden. De las celdas abiertas y vac´ıas se elige la de menor coste, la celda f y se expande. A continuaci´on se expandir´ıa la de menos coste, en este ejemplo la celda c. Progresivamente se va creando un grafo de expansi´on que conecta las celdas tridimensionales. Figura 3.1: A*: expansi´on y generaci´on de Abiertas y del ´arbol de expansi´on El pseudoc´odigo correspondiente a estas acciones, se encuentra en la figura 3.2. El tiempo de b´usqueda de un camino satisfactorio pod´ıa sobrepasar el tiempo adecuado para esta parte del programa e impedir que la ejecuci´on del programa principal se realizara en el periodo definido. Por ello, al lanzar el algoritmo se inicializa un temporizador. En cada iteraci´on se comprueba que no se ha sobrepasado el tiempo de b´usqueda m´aximo permitido, si se ha sobrepasado se termina la b´usqueda y se devuelve la trayectoria correspondiente al nodo de menor coste del ´arbol de expansi´on. Al lanzar el algoritmo, se introduce la lista de los subobjetivos de velocidad calculados. El c´alculo de los subobjetivos corresponde con las heur´ısticas de evitaci´on y se desarrolla en la secci´on 4. Una vez finalizado el proceso de b´usqueda, si se ha conseguido alcanzar al menos un subobjetivo se reconstruye la trayectoria. Esto se realiza recorriendo en el ´arbol de expansi´on el camino desde el ´ultimo objetivo alcanzado hasta la celda inicial (la posici´on actual). Se guarda la trayectoria completa calculada permitiendo, en caso de que una b´usqueda no pueda finalizar, emplear las velocidades calculadas en los periodos anteriores. Al programa principal se le devuelve el par de velocidades (v,w) de la trayectoria en el primer nivel de tiempo, que ser´a la orden de velocidad del robot para la pr´oxima iteraci´on. CAP´ ITULO 3. B´ USQUEDA DE TRAYECTORIAS EN DVTS 15 Inicializaci´on del temporizador; Inserci´on de los subobjetivos; Inserci´on del par de velocidades actual (v,w) en Abiertos (Ninit); while (No se ha alcanzado el ´ultimo subobjetivo) or (Queda tiempo de c´alculo) {Se procesa la casilla de menor coste de Abiertos; for (Todos los vecinos) {if (No est´a ocupada) and (Es alcanzable) {C´alculo del coste asociado; if (Ya hab´ıa sido visitada) and (Los predecesores coinciden) Se mantiene en Abiertos la de menor coste; else Se inserta en abiertas; } else Se descarta la celda vecina; } Se guarda el nodo expandido en el ´arbol de expansi´on. } Figura 3.2: Estructura del algoritmo A* 3.2. El operador de expansi´on Los robots reales presentan restricciones din´amicas, esto es, las velocidades no son alcanzables instant´aneamente sino que se rigen por unas aceleraciones m´aximas. Para representar esta restricci´on, en [Fox and Thrun(1997)] se present´o la Ventana Din´amica que delimita todas las velocidas alcanzables seg´un la aceleraci´on del robot en un periodo. Adem´as, para robots no holon´omicos es com´un restringir el movimiento a caminos rectos o circulares para cada periodo de muestreo. Esto permite respetar las restricciones cinem´aticas as´ı como mantener una curvatura continua. El tipo de trayectorias descritas, curvas clotoides y anticlotoides obtenidas manteniendo la velocidad lineal o angular constante respectivamente, se presenta en [Owen and Montano.(2006)].As´ı, durante la b´usqueda para cada nivel temporal solo se permite el desplazamiento o en velocidad lineal o en velocidad angular dentro de la Ventana Din´amica. El cumplimiento de estas restricciones se introdujo en el m´etodo de expansi´on de las celdas del A*. El algoritmo de b´usqueda emplea un ´arbol de expansi´on (spanning tree) en el que los nodos hijos se definen mediante un operador de expansi´on. Para determinar las celdas que se consideraban expandibles desde otra se plantearon dos alternativas: La primera opci´on consist´ıa en expandir las cinco celdas adyacentes, esto es las cuatro ortogonales del mismo nivel de tiempo y la inmediatamente superior. Esto permit´ıa una exploraci´on m´as gen´erica de todas las alternativas. Se conservaba la informaci´on del nodo que alcanzaba el nivel de tiempo y en cada expansi´on se 16 comprobaba que la celda fuera accesible en ese nivel de tiempo (que estuviera en el interior de la ventana din´amica), as´ı como que s´olo realizara aceleraciones en velocidad lineal o angular para cumplir las condiciones de curvatura continua.Este m´etodo permit´ıa una expansi´on gen´erica de todas las celdas contiguas, independientemente de la discretizaci´on empleada. Las comprobaciones de cumplimiento de las restricciones cinem´aticas y din´amicas se realizaban al abrir la celda. La figura 3.3a presenta un esquema de este operador de expansi´on. La segunda alternativa se tom´o para el caso concreto de la discretizaci´on empleada. Una vez elegido el tama˜no de celda, se comprob´o que era posible predefinir las celdas que conformaban la ventana din´amica como vecinas directamente en el nivel de tiempo superior, evitando comprobaciones adicionales al realizar la expansi´on. (a) Expansi´on celdas adyacentes (b) Expansi´on ventana din´amica Figura 3.3: operadores de expansi´on Con la primera metodolog´ıa la b´usqueda en el nivel temporal de la casilla a expandir est´a permitida, ya que las comprobaciones se realizan a posteriori, mientras que con la segunda metodolog´ıa se expanden todas las celdas accesibles de la ventana din´amica para la iteraci´on siguiente. CAP´ ITULO 3. B´ USQUEDA DE TRAYECTORIAS EN DVTS 17 3.3. Costes y seguridad Como ya se ha mencionado, la casilla a expandir en cada iteraci´on se determina seleccionando en la lista de abiertas el nodo de menor coste. Para conseguir un resultado ´optimo se debe ponderar tanto la seguridad de la trayectoria como que el objetivo se alcance lo m´as rapido posible. La funci´on de coste presentada en la 3.3 hace referencia a dos tipos de costes diferentes: el coste de llegar desde el nodo inicial al nodo que se est´a estudiando g(N) , el coste estimado de llegar desde el nodo actual hasta el objetivo h(N) . f(N) = g(N) + h(N) (3.2) A continuaci´on se desarrolla la composici´on y m´etodo de c´alculo de estos sumandos. Coste hasta la casilla actual g(N) Indica el coste de llegar desde el nodo inicial hasta el nodo que se est´a estudiando. El tiempo transcurrido desde el nodo inicial hasta el nodo actual aparece directamente representado con la componente de tiempo de la celda. Este tiempo se normaliza al n´umero de iteraciones equivalentes, ya que la discretizaci´on de las celdas en tiempo corresponde a la duraci´on de cada iteraci´on, g(N) = numCasillas (3.3) Coste heur´ıstico hasta el objetivo h(N) La funci´on de costes del algoritmo A* consta de un t´ermino h(N) que es una estimaci´on heur´ıstica del coste h∗(N) del camino de menor coste entre NyNobjetivo. Este coste se dice que es admisible si y s´olo si para cada nodo Ndel grafo Gse satisface : ∀NG : 0 ≤h(N)≤h∗(N) (3.4) Si h(N) es admisible, se garantiza que el algoritmo A* devuelve un camino de m´ınimo coste entre el nodo inicial y el final. Por ello, los factores de coste heur´ısticos deben ser optimistas. El t´ermino de coste heur´ıstico hasta el objetivo consta de tres componentes ponderadas: la primera hace referencia al coste de alcanzar la velocidad m´as id´onea para ese instante hv(N), la segunda introduce el coste asociado en el espacio de configuraciones hasta alcanzar el objetivo cartesiano hdist la tercera componente hsaporta un coste relativo a la seguridad. h(N) = αvhv(N) + αdisthdist(N) + αshs(3.5) A continuaci´on se ampl´ıa el m´etodo de c´alculo de estos tres t´erminos. La evaluaci´on de los factores de ponderaci´on se encuentra en el anexo E.2. 24 selecci´on de objetivos 4.1b la velocidad lineal se mantiene m´as cont´ınua se obtiene una trayectoria m´as suave, logrando adem´as alcanzar el objetivo m´as rapidamente. (a) Empleo del freemotion (b) Integraci´on de heur´ısticas de evitaci´on Figura 4.1: Escenario comparativo de m´etodos de selecci´on de subobjetivos 4.1. Heur´ısticas para la selecci´on de subobjetivos En [Lorente(2011)] se desarroll´o una estrategia para la selecci´on de la estrategia de evitaci´on ´optima mediante un ´ Arbol de Decisiones. En el anexo A.1 se amplia la informaci´on de este trabajo. Para este proyecto, se trata cada obst´aculo de forma individual. Seg´un la proyecci´on de la silueta de las velocidades prohibidas (la forma del obst´aculo en el DVS) , aparecen las siguientes estrategias: Pasar por delante Evitar por detr´as Alineaci´on Dejar pasar CAP´ ITULO 4. ESTRATEGIAS DE NAVEGACI´ ON 25 4.1.1. Integraci´on de estrategias De esta manera, se calculan las ´ordenes de movimiento para los obst´aculos que son relevantes. Aqu´ı conviene hacer un inciso sobre cu´ando se considera un obst´aculo como relevante. Relevancia de obst´aculos •No se consideran los obst´aculos que se encuentran a una distancia mayor de 2/3 el alcance del l´aser. Con esta condici´on se permite que obst´aculos demasiados lejanos sean procesados durante m´as tiempo por los algoritmos de seguimiento (mejorando sus estimaciones) pero no afecten a la planificaci´on de trayectorias. •Para obst´aculos m´as pr´oximos considerados, se calculan las velocidades l´ımites que definen su superficie prohibida del modo indicado en el apartado 2.Una segunda criba se realiza no calculando la orden de evitaci´on para obst´aculos que se encuentran a una distancia mayor que el doble de la m´axima distancia alcanzable en el horizonte temporal considerado por el A*. distancia ≤2∗velocidad m´axima del robot ∗horizonte temporal m´aximo Para los obst´aculos que cumplen ambas restricciones, se calcula la estrategia asociada de evitaci´on. Inserci´on de los objetivos de velocidad As´ı, para cada objeto din´amico considerado, se ha definido la estrategia m´as adecuada de evitaci´on seg´un la silueta de las velocidades prohibidas. Finalmente es preciso integrar el conjunto de todas las estrategias para introducir en el A* subobjetivos que ayuden a dirigir la b´usqueda. Seg´un el tama˜no angular en el DVS: Este tama˜no angular es representativo tanto de la distancia a la que se encuentra el obst´aculo, como de la velocidad y de su direcci´on y sentido relativos al robot. A mayor tama˜no angular, proximidad o velocidad del objeto, es decir menor seguridad y mayor prioridad en su evitaci´on. En la figura siguiente se muestra un tama˜no angular grande , 4.2a, frente a otro menor, 4.2b. Seg´un el tiempo que es posible mantener esa velocidad libre de colisi´on: Si bien trabajar en el espacio velocidades-tiempo permite emplear velocidades que en el plano eran evitados, es necesario tener en cuenta cu´anto tiempo pueden ser mantenidos estos comandos. Por ello, cada vez que un subobjetivo definido por un obst´aculo se incluye en el planificador A*, se comprueba para cu´al es la primera casilla prohibida en la que aparece y se le asocia un tiempo durante el cual ese par de velocidades es posible y seguro (disminuyendo a la primera casilla prohibida un margen de frenado). 26 (a) (b) Figura 4.2: Figura comparativa de dos tama˜nos angulares En esta aplicaci´on el margen de tiempo corresponde a un margen de seguridad definido como el m´aximo tiempo de frenado del robot. Si se desea ampliar la seguridad del sistema, bastar´ıa con modificar este margen. En el caso m´as extremo, en el que todos los subobjetivos correspondientes a obst´aculos fueran eliminados, el A* funcionar´ıa con el ´unico objetivo del freemotion. Este par´ametro permite descartar como subobjetivo las velocidades que no son alcanzables por las restricciones cinem´aticas y din´amicas del robot en el tiempo asociado. Al comienzo de cada iteraci´on, el algoritmo comprueba que cada subobjetivo sea alcanzable antes de su tiempo m´aximo desde el par de velocidades que lleva el robot en ese instante. Si no es as´ı, lo descarta. Como se ha indicado en la secci´on 2.3.2, para limitar el tiempo de b´usqueda se define en cada iteraci´on un techo de b´usqueda temporal u horizonte temporal. Este tiempo corresponde al tiempo que le costar´ıa llegar al objetivo en el espacio de trabajo si mantuviera la velocidad del freemotion. Est´a acotado superiormente por el tama˜no m´aximo de la malla para evitar b´usquedas infinitas o muy largas. Una vez todos los subobobjetivos correspondientes con obst´aculos han sido ordenados, es necesario incluir el freemotion. Para decidir su prioridad de inserci´on, se sigue el siguiente criterio: Si las velocidades de freemotion son libres en el nivel del horizonte temporal, se insertar´a en su posici´on correspondiente de tiempo y se dejar´a de incluir subobjetivos detr´as. Esto es especialemente importante en casos en los que el robot se halla muy cerca de su objetivo cartesiano, ya que la cercan´ıa de otros robots puede indicar trayectorias de evitaci´on, que lo alejar´ıan del objetivo. En caso contrario se a˜nade al final de la lista. Si ninguno de los subobjetivos est´a libre en el horizonte temporal, se a˜nade el m´as cercano al de freemotion. CAP´ ITULO 4. ESTRATEGIAS DE NAVEGACI´ ON 27 Este m´etodo busca trayectorias en las que una vez evitado el obst´aculo m´as pr´oximo fuera posible alcanzar la orden de velocidad que evitaba al siguiente. En la figura 4.3a se aprecian tres superficies prohibidas correspondientes a obst´aculos m´oviles y una (gris) correspondiente a una pared. Con rect´angulos del mismo color aparecen las estrategias asociadas a cada uno de los obst´aculos m´oviles que se ordenan priorizando el tama˜no angular: naranja (evitar por detr´as) , amarillo (libre), granate (evitar por detr´as). La figura 4.3b muestra los objetivos finales que se introducen al A*. Se puede observar c´omo todos a los subobjetivos se les ha asignado un tiempo m´aximo de alcance que los sit´ua por debajo de cualquier superficie prohibida. El subobjetivo amarillo coincide con el de freemotion, pero ninguno de los subobjetivos calculados est´a libre en horizonte temporal. Por ello se a˜nade un ´ultimo subobjetivo cercano al freemotion, que est´a libre en el horizonte temporal (celda verde). (a) Subobjetivos calculados en el DVS para cada obst´aculo (b) Subobjetivos ordenados e introducidos en el DVTS Figura 4.3: Ordenaci´on de subobjetivos En este apartado se ha explicado la t´ecnica general, que se aplica a escenarios poco estructurados con obst´aculos movi´endose en direcciones aleatorias como la navegaci´on en interiores, una plaza o el cruce de una calle. Adicionalmente se consider´o una escena de adelantamiento, en la que la selecci´on de estrategias difiere. 4.2. Maniobras de adelantamiento A diferencia de los escenarios presentados hasta ahora, la circulaci´on de un veh´ıculo en carriles se trata de un entorno muy estructurado. Por ello, la selecci´on de las heur´ısticas de velocidad que se introducen al algoritmo de b´usqueda, se realiza de un modo diferente. B´asicamente se pretende avanzar a m´axima velocidad siguiendo el vial mientras sea posible. Si aparece un veh´ıculo en el mismo carril m´as lento se analiza la 28 opci´on de realizar un adelantamiento, reduciendo la velocidad si este no se puede llevar a cabo de forma segura. Un adelantamiento consta b´asicamente de tres etapas. En primer lugar hay un cambio al carril izquierdo, luego se avanza por dicho carril hasta superar el veh´ıculo a adelantar y finalmente se realiza un cambio al carril original. El anexo A.2 explica la generaci´on de maniobras de cambio de carril, basada en la combinaci´on de pares de clotoides sim´etricas [Mont´es and Tornero(2004)], figura 4.4 que permite un cambio de carril suave, manteniendo la continuidad de la curvatura de la trayectoria. Figura 4.4: Figura obtenida del art´ıculo [Mont´es and Tornero(2004)] en la que se muestra la combinaci´on de cuatro clotoides sim´etricas para el cambio de carriles Al incializar el programa se calcula la maniobra aproximada de adelantamiento. Una vez conocido el tramo y el tiempo estimado de cambio de carril se obtienen las condiciones de las distancias m´ınimas entre los veh´ıculos a adelantar y las distancias de seguridad respecto a los veh´ıculos que vienen de frente. Durante la ejecuci´on del programa, se identifica la situaci´on actual seg´un el esquema presentado en la figura 4.5. Figura 4.5: Esquema adelantamiento CAP´ ITULO 4. ESTRATEGIAS DE NAVEGACI´ ON 29 Este m´etodo calcula para cada iteraci´on un objetivo de velocidad que introduce en el A* en funci´on de la situaci´on en el espacio de trabajo. Esto lo diferencia del m´etodo general que seleccionaba los subobjetivos para cada obst´aculo bas´andose en sus siluetas en el DVS. Sin embargo, en lo referente a la b´usqueda del A* son similares. Una vez seleccionadas las heur´ısticas de velocidad, la b´usqueda se lanza. Esto permite en el adelantamiento tender a las velocidades que marca el adelantamiento te´orico, pero considerando que en todo momento las acciones son seguras. La figura 4.6a representa un caso de necesario adelantar pero no posible, por lo que el robot debe adaptarse a la velocidad del veh´ıculo lento. As´ı la velocidad se mantiene lineal y baja por debajo de las superficies prohibidas (el caso general hubiera desviado al robot a la derecha, generando comportamientos excesivamente oscilatorios). En la figura 4.6b el robot se mantiene paralelo al veh´ıculo a adelantar mientras lo sobrepasa, en lugar de desviarse a la izquierda. Workspace DVTS DVS (a) (b) Figura 4.6: Ejemplos de la selecci´on de estrategias en el adelantamiento En los apartados 6.4 y 7.2 se presentan las pruebas realizadas en este tipo de situaciones, tanto en la simulaci´on como con los robots reales. En el anexo A.2 se desarrollan las pruebas m´as en detalle,y se aclaran las modificaciones que fueron necesarias para realizar la prueba real. 30 Cap´ıtulo 5 M´odulos del programa 5.1. Estructura del programa El programa es un ejecutivo c´ıclico de 250 milisegundos de duraci´on, durante los cuales se realizan todas las operaciones necesarias para planificar la trayectoria. El tiempo de refresco del l´aser es aproximadamente 250 milisegundos, por esta raz´on se ha fijado esta duraci´on de periodo. En la siguiente figura se muestran los m´odulos y dependencia de las acciones realizadas tras las inicializaciones y durante cada iteraci´on hasta la finalizaci´on del programa. Mediante el color granate se indica que partes fueron desarrolladas en este proyec- to. En color azul se representan m´odulos se integraron de trabajos anteriores. Los algoritmos de estimaci´on y filtrado de la informaci´on fueron proporcionados por el departamento, al igual que el m´etodo de c´alculo de los l´ımites de velocidad que conducen a colisi´on. En el anexo B se amplia el funcionamiento de estos filtros. En el caso de los obst´aculos m´oviles (naranja), se modific´o la selecci´on de estrategias del ´arbol de decisiones A para adaptarla y se desarroll´o el m´etodo de selecci´on de estrategias en el adelantamiento a partir del cambio de carril mediante combinaciones de clotoides [Mont´es and Tornero(2004)]. Los m´odulos en verde representan las interacciones del programa con el robot, a trav´es de Player [PlayerStage()]. 31 32 Figura 5.1: M´odulos ejecutados en cada iteraci´on del programa Cap´ıtulo 6 Simulaciones En este cap´ıtulo se presentan los experimentos llevados a cabo sobre la plataforma de simulaci´on Stage. A pesar de que este programa proporciona los datos exactos de posici´on y velocidad de todos los robots, todas las simulaciones toman los datos a trav´es de la percepci´on del l´aser de la simulaci´on. A los datos del laser se les aplican los mismos tipos filtros que se emplean en las pruebas reales para ser lo m´as similares posible. Se dise˜naron varios escenarios para probar la eficacia del m´etodo e intentar reproducir diferentes situaciones comunes en la vida real. En simulaci´on se realizaron cuatro experimentos: Un cruce de carretera, 6.1 . Una situaci´on ca´otica en espacio abierto, 6.2 . El interior de un edificio en el que un planificador global le ha indicado varios subobjetivos 6.3 . Un adelantamiento en una carretera, 6.4 . Se presenta para cada experimento una descripci´on general del escenario y una breve descripci´on de las trayectorias que han resultado, junto con unas capturas del escenario para mejorar su comprensi´on. En el anexo C se amplian estas explicaciones con el razonamiento sobre el espacio de velocidades-tiempo. En todas las simulaciones, el robot que est´a ejecutando el programa es el robot verde. Los dem´as se mueven con velocidades constantes. 33 40 Cap´ıtulo 7 Experimentos Una vez realizados los experimentos en simulaci´on, se procedi´o a montar los experimentos con los robots Pioneer reales. Para realizar estos experimentos fue necesario incluir un filtro para los obst´aculos din´amicos, el filtro de piernas (ve´ase anexo B), que fue facilitado por el departamento. Se decidi´o llevar a cabo dos experimentos, cada uno de ellos presenta alguna variaci´on respecto a sus hom´ologos de simulaci´on: Hall Se desarrolla en un entorno interior. Aparecen otros robots y personas. Adelantamiento Adem´as de un veh´ıculo lento al que debe adelantar junto con otro veh´ıculo en el carril izquierdo que le obliga a retrasar el adelantamiento, en este experimento aparece un tercer veh´ıculo que invade su carril y le obliga a realizar una maniobra de emergencia. 41 42 7.1. Hall Este experimento se llev´o a cabo en la entrada del edificio de I+D. Aparecen seis obst´aculos m´oviles de los cuales dos son robots y cuatro personas. Se ha simulado un planificador global que le indica dos puntos v´ıa hasta el objetivo. El robot parte de su posici´on inicial y le aparece el primer obst´aculo, un robot, al que comienza a evitar por detr´as y luego deja pasar. Reduce su velocidad al llegar al primer punto v´ıa y se dirige hacia el objetivo siguiente. Al comienzo del segundo tramo, se encuentra con un peat´on que se dirige en diagonal hacia ´el, realiza un giro a izquierdas para dejarle pasar y recupera su orientaci´on hacia el objetivo. Otro peat´on entra por la puerta del edificio y se cruza por delante en perpendicular, realiza un leve giro a la derecha para proseguir su camino. Un tercer peat´on le aparece por su lateral. Este peat´on describe una trayectoria curva y se le cruza de derecha a izquierda, el robot realiza una maniobra de giro la derecha para pasar por detr´as de ´el de forma segura y alcanza el segundo punto v´ıa. Un quinto obst´aculo m´ovil, esta vez otro robot, avanza en linea recta obstaculizando el camino directo que conduce a su objetivo final. Mediante un giro a izquierdas se aparta de la trayectoria del otro m´ovil, recuperando su posici´on. Aparece un sexto obst´aculo, un peat´on. Su presencia retrasa la recuperaci´on de la orientaci´on directa al objetivo. Finalmente concluye su recorrido. Figura 7.1: Hall: Escenario La figura anterior reproduce la identificaci´on de puntos est´aticos(azules) y din´amicos(rojos) del robot a lo largo del experimento y la trayectoria que ha descrito (l´ınea verde). CAP´ ITULO 7. EXPERIMENTOS 43 Figura 7.2: Hall: Perfil de Velocidades En la figura 7.4 se observa c´omo el robot consigue mantener la velocidad lineal alta en general. El primer descenso de velocidad corresponde con dejar pasar al pimer obst´aculo, los otros dos se realizan an pasar por los puntos v´ıa. Figura 7.3: Tomas de la prueba del Hall Con este experimento, se ha probado el m´etodo completo en una situaci´on realista. Mediante el filtro de piernas se ha logrado identificar de forma adecuada a las personas. El robot ha sido capaz de tratar con el entorno completo y desarrollar una trayectoria adecuada y segura. 44 7.2. Adelantamiento En este experimento se lleva a cabo un adelantamiento y una maniobra de evitaci´on ante una invasi´on frontal del carril propio. En la figura 7.4 aparece la evoluci´on de las velocidades del robot. En la figura 7.6 se muestran algunas capturas de pantalla del experimento. Comienza identificando un veh´ıculo m´as lento en el carril derecho al que debe adelantar, pero como por el carril izquierdo viene otro de frente y est´a bastante cerca debe esperar a realizar su maniobra reduciendo la velocidad. Esto se aprecia claramente en el momento en el que la velocidad lineal se reduce, en la gr´afica 7.4 marca (1). Cuando el veh´ıculo del carril izquierdo ha pasado comienza el adelantamiento, (2) primero un giro a izquierdas y luego a derechas. Una vez sobrepasa al veh´ıculo lento y estima que ya lo ha superado suficientemente vuelve a su carril,(3) giro a derechas y luego a izquierdas, y corrige su trayectoria para terminar bien alineado en su carril. Al final del tramo aparece un veh´ıculo que le viene de frente en su propio carril, lo que le obliga a pasar al carril izquierdo y volver una vez el veh´ıculo hostil ha pasado (4). Figura 7.4: Adelantamiento real: Perfil de Velocidades La figura 7.5 muestra el escenario completo, siendo los puntos azules puntos clasificados como est´aticos los circulos rojos los obst´aculos identificados y la l´ınea verde la trayectoria seguida por el robot a lo largo del experimento. Figura 7.5: Adelantamiento real: Escenario CAP´ ITULO 7. EXPERIMENTOS 45 Se puede apreciar una ligera deriva en la autolocalizaci´on de la posici´on en Y. Por otra parte se observa que los puntos que forman el obst´aculo lento aparecen clasificados en muchas ocasiones como est´aticos. Gracias al empleo del filtro de piernas en lugar de un clasificador a partir de los puntos din´amicos (anexo B), esto no represente un grave problema. Figura 7.6: Tomas de la prueba de Adelantamiento real 46 Cap´ıtulo 8 Conclusiones y trabajo futuro 8.1. Conclusiones Este proyecto presenta un m´etodo robusto de planificaci´on de trayectorias ´optimas en tiempo y seguridad. Para ello se trabaja sobre un espacio discreto de velocidadestiempo, que permite representar de forma expl´ıcita las restricciones din´amicas del robot y el tiempo. Las principales aportaciones de este trabajo son dos: Trabajar con la componente de tiempo de forma expl´ıcita para realizar la planificaci´on permite realizar maniobras que no eran permitidas en trabajos anteriores que consideraban la proyecci´on bidimensional. Se han integrado de un modo natural la seguridad, la rapidez y la selecci´on de estrategias. A pesar del alto coste computacional que supone trabajar con la componente tiempo, se ha alcanzado un compromiso adecuado entre el nivel de discretizaci´on, el horizonte temporal y el tiempo de c´omputo requerido que permite ejecutar el programa en tiempo real. La trayectoria calculada es una trayectoria ´optima seg´un la funci´on de costes desarrollada en este trabajo, la cual minimiza el tiempo de alcance de los objetivos a la vez que asegura la integridad del robot. Esto se logra gracias a la integraci´on en la funci´on de costes de heur´ısticas de evitaci´on de obst´aculos, de cercan´ıa al objetivo y de seguridad, adem´as de considerar de forma expl´ıcita el tiempo. Realizar una planificaci´on a varios niveles de tiempo de la trayectoria tambi´en aumenta la robustez del m´etodo, ya que permite aplicar ´ordenes de velocidad calculados en iteraciones anteriores en caso de no poder completar una b´usqueda. Adicionalmente, seleccionar de forma independiente las estrategias de evitaci´on de cada obst´aculo e integrarlas al final permite tratar un elevado n´umero de obst´aculos m´oviles con menos restricciones que con el m´etodo anterior que fusionaba las proyecciones de los obst´aculos y los trataba como uno solo. 47 48 El m´etodo fue satisfactoriamente validado en varios escenarios diferentes, tanto reales como de simulaci´on. Los resultados muestran que se ha conseguido implementar un m´etodo general y seguro. 8.2. Limitaciones y Trabajo Futuro La realizaci´on de los experimentos reales supuso la aparici´on de varios problemas: La clasificaci´on de los obst´aculos m´oviles a trav´es de las agrupaciones de puntos din´amicos no permit´ıa identificar algunas situaciones concretas, como las de un obst´aculo m´as lento que se desplaza en la misma direcci´on y sentido. Para solventarlo se emple´o el filtro de identificaci´on y seguimiento de obst´aculos m´oviles a trav´es de patrones tipo piernas, que en lugar de emplear la clasificaci´on de puntos est´aticos y din´amicos buscaba directamente el patr´on de discontinuidad. El empleo de este filtro adem´as permiti´o realizar un experimento con personas reales, lo que de otro modo no hubiera sido posible. En los experimentos reales, especialmente en el caso del adelantamiento, se observ´o que al realizar giros bruscos el algoritmo de autolocalizaci´on de Scan Matching perd´ıa parte de su eficacia, lo que causaba derivas en la estimaci´on de la posici´on del robot respecto a la posici´on real. Otro posible punto de mejora es el tratamiento de obst´aculos est´aticos. La segmentaci´on de los puntos observados en cada iteraci´on por el l´aser puede no ser suficiente para representar el conjunto de los obst´aculos est´aticos que dificultan la navegaci´on. Una futura l´ınea de trabajo podr´ıa completar este m´etodo de planificaci´on con un m´etodo de autolocalizaci´on y generaci´on de mapas tipo SLAM (Simultaneous Localization And Mapping, [Thrun and Fox(2005)]). Mientras los m´etodos SLAM permiten un mejor tratamiento de los obst´aculos est´aticos, el DVTS permite trabajar mejor con obst´aculos m´oviles al plantear en el tiempo las estrategias de evitaci´on. La planificaci´on de caminos sobre el espacio de trabajo es una l´ınea bastante tratada en la rob´otica m´ovil. Emplear un m´etodo SLAM podr´ıa mejorar adem´as el problema de autolocalizaci´on. Una posible soluci´on para integrar ambos m´etodos y aprovechar las principales ventajas de cada uno de ellos, ser´ıa a trav´es de la funci´on de costes. En la funci´on de costes presentada en este proyecto se ha introducido ya un t´ermino relativo al espacio de trabajo (hdist), en el que se realiza una estimaci´on de la posici´on para la celda a expandir, pero esta estimaci´on solo era empleada para realizar una estimaci´on del tiempo desde esa posici´on al objetivo. Este t´ermino se podr´ıa ampliar o redefinir comparando la posici´on estimada con el coste equivalente en el espacio cartesiano empleando por ejemplo el m´etodo de potencial [Latombe(1991)]. 56 Donde ω(t) y ν(t) representan un ruido gausiano de media cero del proceso y la medida, con matrices de covarianza Q(t) y R(t). Las funciones f(x(t), u(t)) y h(x(t)) son funciones no lineales que relacionan el estado en el instante t con el estado en el instante t+1 y la medici´on en el instante t con el estado en ese mismo instante. La predicci´on a priori del estado en el instante actual ˆx(t) queda: ˆx(t)−=f(ˆx(t−1), u(t−1)) con una covarianza a priori del error P(t): P(t)−=A(t−1) ∗P(t−1) ∗A(t−1)T+W(t−1) ∗Q(t−1) ∗W(t−1)T siendo A(t) el jacobiano de frespecto al estado y W(t) la matriz de derivadas parciales de frespecto al ruido ω. La matriz de ganancia de Kalman K(t) se define como: K(t) = P(t)−∗H(t)T∗(H(t)∗P(t)−∗H(t)T+V(t)∗R(t)∗V(t)T)−1 donde H(T) es el jacobiano de hrespecto al estado y V(t) es la matriz de derivadas parciales de frespecto al ruido de medida ν La estimaci´on del estado a posteriori y la covarianza del error finalmente quedan: ˆx(t) = ˆx(t)−+K(t)∗(d(t)−h(ˆx(t)−)) P(t) = (I−K(t)∗H(t)) ∗P(t)− Si R(t) se aproxima a cero, se puede confiar en la medida, mientras si lo hace P(t)− se confia m´as en la estimaci´on. B.2. Estimadores A continuaci´on se describen los principales filtros y estimadores empleados en el programa y proporcionados por el grupo de Rob´otica. B.2.1. Clasificaci´on de puntos Conforme el robot avanza se genera un mapa roboc´entrico que se emplea para mejorar la autolocalizaci´on y para realizar la clasificaci´on en est´aticos, din´amicos o desconocidos de los puntos percibidos por el l´aser. [Montesano and Montano(2005b)] Mediante una malla de probabilidad centrada en el robot, se calcula comparando la percepci´on y la predicci´on sobre el estado anterior, la probabilidad de que ese punto sea est´atico. Si esa probabilidad es alta se clasifica como est´atico, si es baja como din´amico y si no est´a dentro de ninguno de los dos umbrales, como desconocido. AP´ ENDICE B. FILTROS Y ESTIMADORES 57 Adem´as, los puntos din´amicos se introducen como entradas en un filtro de Kalman, que realiza su seguimiento individual. Figura B.1: L´aser: clasificaci´on de puntos est´aticos (rojos) y din´amicos (azules) B.2.2. Autolocalizaci´on Scan Matching [Montesano and Montano(2005a)], con los puntos clasificados como est´aticos se realiza una mejora en la estimaci´on de la posici´on del robot, comparando los desplazamientos de los puntos est´aticos entre dos medidas consecutivas minimizando el error cuadr´atico medio. B.2.3. Obst´aculos est´aticos Segmentaci´on Los puntos est´aticos se agrupan generando los segmentos que se emplear´an para definir los l´ımites de velocidad de los obst´aculos est´aticos. La agrupaci´on de los puntos est´aticos se basa en la distancia m´ınima entre dos puntos consecutivos, [D. Castro and Ruano(2002)]. Si los segmentos generados constan de m´as de cuatro puntos se tratan, eliminando esp´ureos. Posteriormente los segmentos se engordan con el radio del robot. En la figura siguiente se observa a la izquierda el espacio de trabajo y c´omo los segmentos han sido engordados y a la derecha su equivalente en la proyecci´on de la superfice del espacio de velocidades. 58 Figura B.2: Figura obtenida de [Lorente(2011)] en la que se muestra el engorde de los segmentos para el c´alculo de las superficies est´aticas B.2.4. Obst´aculos m´oviles Seguimiento de puntos din´amicos En primer lugar sobre los puntos din´amicos cercanos se agrupan en una misma nube, que se emplea como medida en un filtro de Kalman Extendido, que agrupa los puntos din´amicos cercanos en una que los almacena para realizar su seguimiento. As´ı las nubes de puntos din´amicos son agrupadas en un ´unico obst´aculo din´amico que pasar´a a ser mapeado en el espacio de velocidades tiempo. Clasificador de piernas En las pruebas reales, la estimaci´on de la localizaci´on del robot empeor´o radicalmente, lo que oblig´o a realizar una clasificaci´on de est´aticos m´as rigurosa para mejorarla. Como consecuencia la clasificaci´on de puntos din´amicos se vio afectada y no era posible realizar el seguimiento de los obst´aculos din´amicos por este m´etodo. Por ello fue necesario emplear otro filtro para los obst´aculos din´amicos y se adapt´o el desarrollado en [Urcola and Montano(2011)]. Este filtro no realiza la clasificaci´on sobre puntos est´aticos y din´amicos sino que busca el patron ’piernas’. Se clasifica como pierna una agrupaci´on de puntos consecutiva de un tama˜no acotado, cuyos puntos lim´ıtrofes presentan un salto m´ınimo con los puntos siguientes. Una vez identificado el patr´on pierna se compara con las predicciones de los identificados anteriormente, se agrupa con el vecino m´as cercano y se introduce en el filtro como observaci´on. En la figura B.3a se observan dos patrones de clasificaci´on de piernas. El m´as peque˜no corresponde a una persona y el grande a un robot, ambos fueron correctamente identificados. La figura B.3b presenta dos piernas de la misma persona que tambi´en son correctamente clasificadas. AP´ ENDICE B. FILTROS Y ESTIMADORES 59 (a) (b) Figura B.3: L´aser: Identificaci´on del patr´on pierna 60 Ap´endice C Simulaciones El prop´osito de este anexo es detallar las pruebas introducidas en el cap´ıtulo 6. C.1. Cruce A continuaci´on se muestran im´agenes del espacio de trabajo, del DVTS y de su proyecci´on el DVS del ejemplo presentado en 6.1. El robot comienza avanzando hacia el objetivo, pasando por delante de los obst´aculos mientras le es posible. En las figuras C.1a se observa que aunque el espacio se est´e cerrando al aparecer varios obst´aculos enfrentados, la b´usqueda por debajo de las superficies prohibidas permite pasar por delante. Cuando ´esto ya no es posible, debe evitar a un obst´aculo que viene desde su derecha por detras, por lo que gira a la derecha, figura C.1b. Una vez este obst´aculo ha sido evitado, sin embargo no puede recuperar su alineaci´on al objetivo. Aqu´ı hay otro obst´aculo que le viene por la derecha y al que no le da tiempo de pasar por delante, figura C.2b, por lo que mantiene su direcci´on hasta pasarlo por detr´as, C.2b. Todav´ıa le quedan dos obst´aculos, pero uno ya se est´a alejando, C.2c, y al ´ultimo lo pasa por delante (figura C.2d) para finalmente alcanzar su objetivo. Este escenario es especialmente interesante porque muestra c´omo, a pesar de que gran parte del DVTS est´a prohibido para tiempos lejanos, la planificaci´on se realiza por debajo de las superficies prohibidas permitiendo realizar una trayectoria segura a pesar de la aparici´on de varios obst´aculos de forma simult´anea. 61 62 Workspace DVTS DVS (a) (b) (c) (d) Figura C.1: Carriles I AP´ ENDICE C. SIMULACIONES 63 Workspace DVTS DVS (a) (b) (c) (d) Figura C.2: Carriles II 64 C.2. Caos El escenario Caos se desarrolla en un espacio abierto con un gran n´umero de obst´aculos circulando en todas direcciones, alguno de ellos con trayectorias no lineales. El robot comienza realizando una maniobra de alineaci´on con el primer obst´aculo al que pasa por delante C.3a. Lo pasa por delante y mantiene su direcci´on para pasar por delante de otro obst´aculo que le viene por su derecha C.3b. En C.3c planea pasar entre en veh´ıculo que se le acerca por la derecha y el que se le hacerca por la izquierda. Sin embargo el veh´ıculo de su izquierda describe una trayectoria curva y se le acerca oblig´andole a realizar una alineaci´on m´as dr´astica de alineaci´on con ´el C.3d, una vez alineado y hasta superarlo las velocidades de giro a izquierdas est´an altamente penalizadas. Una vez lo sobrepasa el peligro m´as inmediato es otro veh´ıculo que se le cruza en su trayectoria C.4a. Lo rodea por detr´as, dirigi´endose a su objetivo C.4b. El ´ultimo obst´aculo lo intenta evitar mediante un giro a izquierdas, pero no es suficiente, se le cierra el espacio de b´usqueda y debe reducir la velocidad C.4c. Termina alcanzando su objetivo. Una de las hip´otesis de las que parte el c´alculo del DVTS a trav´es de la Banda de Colisi´on es que los obst´aculos m´oviles se desplanzan con trayectorias lineales en la que se basa, dada la frecuencia de actualizaci´on del programa se trata de una hip´otesis aceptable. Este escenario presenta varios obst´aculos con velocidades angulares que han sido correctamente evitados en un escenario bastante poblado. AP´ ENDICE C. SIMULACIONES 65 Workspace DVTS DVS (a) (b) (c) (d) Figura C.3: Caos I 72 C.4. Adelantamiento En el anexo A.2 se ha presentado m´as en detalle el m´etodo de c´alculo de las estrategias para el caso del adelantamiento. A continuaci´on se desarrolla el escenario correspondiente presentado en el cap´ıtulo 6. Consiste en una carretera en la que el robot se encuentra un veh´ıculo m´as lento en su carril al que no puede adelantar de forma inmediata por dos veh´ıculos en el carril contrario que se lo impiden. En la figura C.9a se muestra el momento en el que se identifica el veh´ıculo m´as lento. Al no poder adelantar se ve obligado a reducir la velocidad tal y como aparece en la figura C.9b. Al decidir la estrategia que se le introduce al A* en su b´usqueda seg´un la situaci´on en el espacio de velocidades, se consigue que el robot se adapte al veh´ıculo lento, en lugar de estar oscilando hacia izquierda y derecha para intentar rodearlo. Una vez el carril frontal est´a despejado, se acelera hasta la velocidad m´axima y se comienza la maniobra de cambio de carril, figura C.9c. El primer tramo coincide con la primera clotoide, que va aumentando su radio de forma progresiva. Esto se traduce en una consigna de giro a izquierdas a la velocidad angular m´axima como muestra la figura C.9d. A continuaci´on se disminuye la velocidad angular (segunda clotoide) y se mantiene a velocidad angular nula durante el tramo de concatenaci´on, figura C.10a. El cuarto y quinto tramo del cambio de carril consisten en un giro a derechas con consigna de velocidad angular m´axima a derechas y velocidad angular nula respectivamente. La figura C.10b muestra el momento en el que la consigna cambia. Una vez terminado el cambio de carril, el veh´ıculo se encuentra alineado. Se mantiene paralelo al carril mientras sobrepasa al veh´ıculo lento, figura C.10c. Una vez lo deja atr´as, vuelve a su carril con una maniobra similar. Comienza con un giro a derechas, figura C.10d. Reduce la velocidad angular a cero C.11a. A continuaci´on realiza un giro a izquierdas, figura C.11b, y termina alineado en su carril, figura C.11c. Mientras estaba realizando la maniobra de vuelta, se acercaba otro obst´aculo por el carril izquierdo. En la figura C.11d se ve c´omo un veh´ıculo en el carril contrario no le afecta para avanzar por su carril. AP´ ENDICE C. SIMULACIONES 73 Workspace DVTS DVS (a) (b) (c) (d) Figura C.9: Adelantamiento: I 74 Workspace DVTS DVS (a) (b) (c) (d) Figura C.10: Adelantamiento: II AP´ ENDICE C. SIMULACIONES 75 Workspace DVTS DVS (a) (b) (c) (d) Figura C.11: Adelantamiento: III 76 Ap´endice D Experimentos D.1. Hall El robot comienza su recorrido cuando le aparece un obst´aculo que se le acerca de derecha a izquierda, figura D.1a que comienza a evitar por detr´as girando hacia la derecha. Cuando ha girado sufiente (la superficie prohibida del obst´aculo, la azul oscura D.1b recupera su alineaci´on girando a la izquierda. Sin embargo al evitar al obst´aculo se ha alejado demasiado en orientaci´on del primer v´ıa de su trayectoria, por lo que debe reducir la velocidad, figura D.1c. Una vez recupera la orientaci´on vuelve a aumentar su velocidad para llegar al primer punto v´ıa. En la figura D.1d se observan adem´as del primer obst´aculo dos falsos positivos devueltos por el filtro que corresponden con las patas de un banco. El filtro de piernas tiene un par´ametro para distinguir las piernas de las patas de sillas o mesas a trav´es del tama˜no, pero la anchura de las patas de este banco es comparable al tama˜no de una pierna. Superado el primer punto v´ıa, el robot contin´ua la trayectoria por la parte m´as larga de la entrada. De izquierda a derecha se le acerca otro obst´aculo m´ovil, granate en el DVS de la figura D.2a. Lo rodea mediante un giro primero a izquierdas y luego a derechas, figura D.2b. Otro obst´aculo le viene de derechas a izquierdas, lo evita con un giro a derechas y luego recuperando su alineaci´on, figura D.2c. Los dos obst´aculos que aparecen en la figura D.2c corresponden a sendas piernas. Aparece otro obst´aculo m´ovil que comienza avanzando paralelo al robot, y luego se le cruza por delante. Lo deja pasar por delante desvi´andose a la derecha para no reducir velocidad, figura D.2d. Cerca de alcanzar su segundo punto v´ıa, otro obst´aculo se dirige de frente hacia ´el, figura D.3a. Al venir de frente, le cierra el espacio de velocidades, oblig´ando al robot a desviarse de su trayectoria con un giro a izquierdas. Una vez se ha salido de la trayectoria del obst´aculo, lo rodea dej´andolo a su derecha, figura D.3b. Detecta otro obst´aculo que se aleja por la izquierda, figura D.3c. Se mantiene a su izquierda hasta que alcanza el objetivo final, figura D.3d. 77 78 Workspace DVTS DVS (a) (b) (c) (d) Figura D.1: Hall: I AP´ ENDICE D. EXPERIMENTOS 79 Workspace DVTS DVS (a) (b) (c) (d) Figura D.2: Hall: II 80 Workspace DVTS DVS (a) (b) (c) (d) Figura D.3: Hall: III AP´ ENDICE D. EXPERIMENTOS 81 D.2. Adelantamiento El robot comienza avanzando con su carril, pero se encuentra con un veh´ıculo m´as lento al que no puede adelantar porque le viene otro de frente. En la figura D.4b se observa c´omo se mantiene a velocidades bajas detr´as del veh´ıculo lento. Una vez desaparece el veh´ıculo del otro carril comienza el adelantamiento. En primer lugar la consigna de velocidad es girar a izquierdas, D.4c,que corresponde con el primer tramo de los cinco que componene el cambio de carril mediante clotoides. En el segundo tramo reduce la velocidad angular hasta cero y la mantiene D.4d hasta completar el tercer tramo, el tramo lineal entre los dos pares de clotoides sim´etricas. El cuarto tramo consiste en girar a derechas, figura D.5a, para finalizar el cambio de carril reduciendo la velocidad angular a cero y terminando alineado, D.5b. Avanza paralelo al veh´ıculo lento (figura D.5c). Una vez deja de verlo y estima que ya lo ha sobrepasado comienza la vuelta al carril derecho. Primero realiza un giro a derechas (figura D.5d), reduce velocidad angular (figura D.6a), gira a izquierdas (figura D.6b) y enlaza con el carril por el que circula normalmente D.6c. Detecta un obst´aculo m´ovil que invade su carril en sentido contrario. Cuando la distancia es demasiado cercana realiza la maniobra de emergencia, figura D.6d. Al igual que en el adelantamiento realiza el cambio de carril girando a izquierdas, luego avanzando (figura D.7a) y finalmente girando a derechas para enlazar con el carril izquierdo(figura D.7b). Una vez el peligro ha desaparecido vuelve a su carril girando a derechas, y a izquierdas para enlazar (figura D.7c) y terminar el recorrido (figura D.7d). 88 la consigna del programa le mandaba girar, pero al ser una velocidad no alcanzable el controlador interno del robot priorizaba la velocidad lineal y despreciaba la angular. Ante este hecho hab´ıa dos alternativas. La primera ser´ıa trabajar en un rango de velocidades inferior, para el que el espacio de velocidades alcanzables estuviera perfectamente acotado en un rect´angulo por una velocidad lineal y una angular. La segunda alternativa correspond´ıa a definir adecuadamente los l´ımites de velocidades alcanzables e introducirlos en el programa para emplear el m´aximo rango de velocidades posible. Para definir los l´ımites de velocidad aceptables se realiz´o un esayo con el robot en el que se comprobaron los pares de velocidad lineal y angular que se manten´ıan estables y con error de posici´on cero ante la consigna. En la figura E.2 se muestran los l´ımites medidos. Figura E.2: L´ımites del espacio de velocidades real AP´ ENDICE E. CONSIDERACIONES 89 E.2. Pruebas de coeficientes En el cap´ıtulo 3.3 se presentaban los distintos t´erminos que componen la funci´on de costes. Para decidir la ponderaci´on de cada uno se llevaron a cabo diferentes ensayos. En esta secci´on se presentan los resultados m´as relevantes y se analiza un poco m´as en profundidad la importancia de cada t´ermino. Importancia del t´ermino de selecci´on de velocidad Algunos m´etodos de b´usqueda de trayectorias ya emplean la informaci´on de seguridad y distancia al objetivo. Una de las contribuciones de este proyecto es la integraci´on de informaci´on del espacio de velocidades y definici´on de estrategias determinadas para realizar la b´usqueda de la trayectoria. A continuaci´on se muestra una serie de ensayos en los que se observa c´omo el m´etodo mejora considerablemente. Si el t´ermino de velocidades se deja con coeficente cero, es decir que no se gu´ıa al robot para elegir la forma de evitaci´on m´as adecuada ni se le gu´ıa hacia las velocidades m´as favorecedoras en ausencia de obst´aculos, los resultados obtenidos son bastante negativos. Para conseguir evitar la colisi´on se debe restringir bastante el margen de actuaci´on. Es necesario aumentar tanto el margen de seguridad como el peso del t´ermino de seguridad y penalizar en mayor medida hallarse debajo de la superficie prohibida ya que en caso contrario, el robot no evita a tiempo trayectorias que le llevan a colisi´on. Este caso se presenta en la figura E.3. Figura E.3: B´usqueda sin gu´ıa de velocidades, αdist = 1, αv= 0, αs= 4 Simplemente incluyendo el freemotion la trayectoria converge de forma m´as adecuada. En la primera figura se observa como el robot debe reducir su velocidad para dejar evitar la colisi´on. Todav´ıa no tiene la selecci´on de estrategias de evitaci´on, por lo que el movimiento resulta algo brusco. Adem´as emplear el t´ermino de velocidades con el freemotion pero sin selecci´on de estrategias resulta o bien demasiado arriesgado por cruzar con poco margen por delante o demasiado conservador. En la figura E.4 se observa como el robot espera a tener despejado el espacio para dirigirse a su objetivo. 90 Figura E.4: B´usqueda con freemotion αdist = 0, αv= 1, αs= 1 Ambos tipos de actuaciones son demasiado pasivas, el robot frena en lugar de realizar maniobras activas de evitaci´on, lo que le puede llevar a situaciones de atrapamiento. La figura muestra todas las maniobras activas. Realiza una maniobra que en este caso resulta m´as lenta que la de freemotion, pero es v´alida de forma m´as general, como se demuestra en los ejemplos siguientes. Figura E.5: B´usqueda con todos los par´ametros activos, αdist = 0.5, αv= 1, αs= 1 A continuaci´on en las figuras E.6 y E.7 se muestran dos bater´ıas de experimentos sobre escenarios similares en los que se comparan diferentes ponderaciones de los coeficientes de coste. Se muestra c´omo aunque la estrategia de freemotion en ocasiones es m´as r´apida, no garantiza trayectorias seguras en diferentes escenarios para los mismos coeficientes de seguridad. En la figura E.7b el coeficiente de seguridad es bajo y conduce a una colisi´on frontal, para unos valores que el caso E.6c hab´ıan dado un buen resultado. AP´ ENDICE E. CONSIDERACIONES 91 (a) B´usqueda sin gu´ıa de velocidades, αdist = 1 , αv= 0, αs= 0.5 (b) B´usqueda sin gu´ıa de velocidades, αdist = 1 , αv= 0, αs= 1 (c) B´usqueda con freemotion,αdist = 0, αv= 1, αs= 0.5 (d) B´usqueda con todo activo, αdist = 0.5 , αv= 1, αs= 1 Figura E.6: Comparaciones coeficientes I 92 (a) B´usqueda sin gu´ıa de velocidades, αdist = 1 , αv= 0, αs= 1 (b) B´usqueda con freemotion,αdist = 0, αv= 1, αs= 0.5 (c) B´usqueda con freemotion,αdist = 0 ,αv= 1, αs= 4 (d) B´usqueda con todo activo, αdist = 0.5 , αv= 1, αs= 1 Figura E.7: Comparaciones coeficientes II ´ Indice de figuras 2.1. Diferentesespacios ............................. 7 2.2. Espacio de velocidades tiempo . . . . . . . . . . . . . . . . . . . . . . . 9 2.3. Tiempos de c´alculo de una superficie seg´un el tama˜no de celda . . . . . 10 2.4. Mallado generado entre dos pares de velocidades consecutivos . . . . . 12 3.1. A*: expansi´on y generaci´on de Abiertas y del ´arbol de expansi´on . . . . 14 3.2. Estructura del algoritmo A* . . . . . . . . . . . . . . . . . . . . . . . . 15 3.3. operadores de expansi´on . . . . . . . . . . . . . . . . . . . . . . . . . . 16 3.4. T´ermino heur´ıstico de velocidad hv.................... 19 3.5. T´ermino heur´ıstico de cercan´ıa en el espacio de trabajo hdist ...... 20 3.6. T´ermino heur´ıstico de seguridad hs.................... 21 4.1. Escenario comparativo de m´etodos de selecci´on de subobjetivos . . . . 24 4.2. Figura comparativa de dos tama˜nos angulares . . . . . . . . . . . . . . 26 4.3. Ordenaci´on de subobjetivos . . . . . . . . . . . . . . . . . . . . . . . . 27 4.4. Figura obtenida del art´ıculo [Mont´es and Tornero(2004)] en la que se muestra la combinaci´on de cuatro clotoides sim´etricas para el cambio de carriles.................................... 28 4.5. Esquema adelantamiento . . . . . . . . . . . . . . . . . . . . . . . . . . 28 4.6. Ejemplos de la selecci´on de estrategias en el adelantamiento . . . . . . 29 5.1. M´odulos ejecutados en cada iteraci´on del programa . . . . . . . . . . . 32 6.1. Escenas de la simulaci´on Cruce ...................... 34 6.2. Carriles: Perfil de Velocidades . . . . . . . . . . . . . . . . . . . . . . . 34 6.3. Escenas de la simulaci´on Caos ....................... 35 6.4. Caos: Perfil de Velocidades . . . . . . . . . . . . . . . . . . . . . . . . . 36 6.5. Escenas de la simulaci´on Interiores .................... 37 6.6. Interiores: Perfil de Velocidades . . . . . . . . . . . . . . . . . . . . . . 37 6.7. Adelantamiento: Perfil de velocidades . . . . . . . . . . . . . . . . . . . 38 6.8. Adelantamiento:Evoluci´on......................... 39 7.1. Hall:Escenario ............................... 42 7.2. Hall: Perfil de Velocidades . . . . . . . . . . . . . . . . . . . . . . . . . 43 7.3. Tomas de la prueba del Hall ........................ 43 93 94 7.4. Adelantamiento real: Perfil de Velocidades . . . . . . . . . . . . . . . . 44 7.5. Adelantamiento real:Escenario ...................... 44 7.6. Tomas de la prueba de Adelantamiento real ............... 45 A.1. Esquema del ´ Arbol de Toma de Decisiones . . . . . . . . . . . . . . . . 51 A.2. Figura obtenida del art´ıculo [Mont´es and Tornero(2004)] en la que se muestra la combinaci´on de cuatro clotoides sim´etricas para el cambio de carriles.................................... 52 A.3. Distancias y condiciones de seguridad durante la maniobra adelantamiento 53 B.1. L´aser: clasificaci´on de puntos est´aticos (rojos) y din´amicos (azules) . . . 57 B.2. Figura obtenida de [Lorente(2011)] en la que se muestra el engorde de los segmentos para el c´alculo de las superficies est´aticas . . . . . . . . . 58 B.3. L´aser: Identificaci´on del patr´on pierna . . . . . . . . . . . . . . . . . . . 59 C.1. Carriles I .................................. 62 C.2. Carriles II ................................. 63 C.3. Caos I.................................... 65 C.4. Caos II ................................... 66 C.5. Interiores:primertramo .......................... 68 C.6. Interiores:segundotramo ......................... 69 C.7. Interiores:tercertramo........................... 70 C.8. Interiores:cuartotramo .......................... 71 C.9.Adelantamiento:I.............................. 73 C.10.Adelantamiento:II ............................. 74 C.11.Adelantamiento: III . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 75 D.1. Hall:I .................................... 78 D.2. Hall:II ................................... 79 D.3. Hall:III ................................... 80 D.4. Adelantamiento real: I . . . . . . . . . . . . . . . . . . . . . . . . . . . 82 D.5. Adelantamiento real: II . . . . . . . . . . . . . . . . . . . . . . . . . . . 83 D.6. Adelantamiento real: III . . . . . . . . . . . . . . . . . . . . . . . . . . 84 D.7. Adelantamiento real: IV . . . . . . . . . . . . . . . . . . . . . . . . . . 85 E.1.RobotPioneer3AT............................. 87 E.2. L´ımites del espacio de velocidades real . . . . . . . . . . . . . . . . . . 88 E.3. B´usqueda sin gu´ıa de velocidades, αdist = 1, αv= 0, αs= 4 . . . . . . 89 E.4. B´usqueda con freemotion αdist = 0, αv= 1, αs=1 ........... 90 E.5. B´usqueda con todos los par´ametros activos, αdist = 0.5, αv= 1, αs= 1 90 E.6. Comparaciones coeficientes I . . . . . . . . . . . . . . . . . . . . . . . . 91 E.7. Comparaciones coeficientes II . . . . . . . . . . . . . . . . . . . . . . . 92 Bibliograf´ıa [Alglib()] Alglib. http://www.alglib.net/. [Brock and Khatib(1999)] Oliver Brock and Oussama Khatib. High-speed navigation using the global dynamic window approach. In In IEEE International Conference on Robotics and Automation, pages 341–346, 1999. [D. Castro and Ruano(2002)] U. Nunes D. Castro and A. Ruano. Reactive local navigation. In IEEE 28th Annual Conference on Industrial Electronics Society, pages 2427–2432, 2002. [Fiorini and Shillert(1998)] Paolo Fiorini and Zvi Shillert. Motion planning in dynamic environments using velocity obstacles. International Journal of Robotics Research, 17:760–772, 1998. [Fox and Thrun(1997)] Burgard Fox and Thrun. The dynamic window approach to collision avoidance. In Robotics & Automation Magazine, IEEE, volume 4 (1), pages 13–23, 1997. [Fraichard(1999)] Th. Fraichard. Trajectory planning in a dynamic workspace: a ‘statetime space’ approach. In Advanced Robotics, 13 (1), pages 75–94, 1999. [Gal and Shiller(2009)] Oren Gal and Zvi Shiller. Mapping obstacles to collision states for on-line motion planning in dynamic environments. In Workshop on Safe navigation of Autonomous vehicles, ICRA, IEEE, Japan, 2009. [J. M´ınguez and Alami(2001)] N. Simeon J. M´ınguez, L. Montano and R. Alami. ”global nearness diagram navigation (gnd)”. In IEEE International Conference on Robotics and Automation (ICRA’01), 2001. [Latombe(1991)] Jean-Claude Latombe. Robot Motion Planning. 1991. [LaValle and Kuffner(2001)] Steven M. LaValle and James J. Kuffner. Randomized kinodynamic planning. In The International Journal of Robotics Research, volume 20 (5), pages 378–400, 2001. [Lorente(2011)] MaTeresa Lorente. Planificaci´on de la navegaci´on de robots en entornos din´amicos. In PFC Universidad de Zaragoza, 2011. 95 96 [Mont´es and Tornero(2004)] N. Mont´es and J. Tornero. Lane changing using s-series clothoidal approximation and dual-rate based on bezier points to controlling vehicle. In Proceedings of the 4th WSEAS International Conference on Systems Theory and Scientific Computation, 2004. [Montesano and Montano(2005a)] J. Montesano, L. Minguez and L. Montano. Probabilistic scan matching for motion estimation in unstructured environments. In IEEE International Conference on Intelligent Robots and Systems (IROS), Edmonton, Canada, 2005a. [Montesano and Montano(2005b)] J. Montesano, L. Minguez and L. Montano. Modeling the static and the dynamic parts of the environment to improve sensorbased navigation. In IEEE International Conference on Robotics and Automation (ICRA), Barcelona, Spain, 2005b. [Owen and Montano(2005)] E. Owen and L. Montano. Motion planning in dynamic environments using the velocity space. In /RSJ International Conference on Intelligent Robots and Systems (IROS’2005), pages 997–1002, Edmonton, Alberta, Canada, 2005. [Owen and Montano.(2006)] E. Owen and L. Montano. A robocentric motion planner for dynamic environments using the velocity space. In IEEE/RSJ International Conference on Intelligent Robots and Systems, Beijing, China, 2006. [PlayerStage()] PlayerStage. http://playerstage.sourceforge.net/. [Stachniss and Burgard(2002)] Cyrill Stachniss and Wolfram Burgard. An integrated approach to goal-directed obstacle avoidance under dynamic constraints for dynamic environments. In IN IEEE-RSJ International Conference on Intelligent Robots and Systems, pages 508–513, 2002. [Thrun and Fox(2005)] Burgard Thrun and Fox. Probabilistic Robotics. 2005. [Urcola and Montano(2011)] P. Urcola and L. Montano. Adapting robot team behavior from interaction with a group of people. In Intelligent Robots and Systems, 2011. IROS 2011. IEEE/RSJ International Conference on, 2011.