Planficación de trayectorias para robots autónomos en entornos industriales

UNIVERSIDAD DE SEVILLA Facultad de F ´ ısica Doble Grado en F´ısica e Ingenier´ıa de Materiales Departamento de Ingenier´ıa de Sistemas y Autom´atica Departamento de Electr´onica Planificaci´on de Trayectorias para Robots Aut´onomos en Entornos Industriales Trabajo Fin de Grado presentado por Rafael Rey Arcenegui Dirigido por: Fernando Caballero Ben´ıtez, Gloria Huertas S´anchez. Sevilla, Junio de 2019 ´ Indice general 1. Introducci´on 1 1.1. El problema de la planificaci´on . . . . . . . . . . . . . . . . . . . . . . 2 1.2. Planificaci´on de caminos . . . . . . . . . . . . . . . . . . . . . . . . . . 3 1.3. Marcodeltrabajo.............................. 3 2. Metodolog´ıas para la Planificaci´on de Caminos 5 2.1. Algoritmos basados en el muestro (Sampling-Based motion planning) . 6 2.1.1. Probabilistic Roadmaps (PRM) . . . . . . . . . . . . . . . . . . 7 2.1.2. Rapidly-exploring Random Trees (RRT) . . . . . . . . . . . . . 8 2.2. M´etodos de campos potenciales . . . . . . . . . . . . . . . . . . . . . . 10 2.3. Algoritmos heur´ısticos . . . . . . . . . . . . . . . . . . . . . . . . . . . 11 2.3.1. Dijkstra ............................... 12 2.3.2. A* .................................. 13 2.3.3. Theta*................................ 16 2.3.4. LazyTheta*............................. 17 3. Implementaci´on y uso del algoritmo Lazy Theta* en un planificador global y local 22 3.1. Arquitectura del sistema . . . . . . . . . . . . . . . . . . . . . . . . . . 23 3.2. PlanificadorGlobal............................. 24 3 3.3. PlanificadorLocal.............................. 26 3.4. SeguidordeCaminos............................ 28 4. Resultados y Discusi´on 31 4.1. Planificador global en simulaci´on . . . . . . . . . . . . . . . . . . . . . 31 4.2. Lazy Theta* sensible a los costes . . . . . . . . . . . . . . . . . . . . . 34 4.3. Planificadores y Lazy Theta* sensible a los costes sobre una plataforma rob´otica en un escenario industrial real . . . . . . . . . . . . . . . . . . 38 5. Conclusiones 41 Cap´ıtulo 1 Introducci´on Actualmente en el campo de la rob´otica encontramos dos ramas bien diferenciadas en lo que respecta a escala de uso, costes y usuarios finales. Por un lado existe la rob´otica industrial, un conjunto de tecnolog´ıas presentes en el mercado desde hace d´ecadas que ha permitido la automatizaci´on de innumerables procesos de fabricaci´on industriales con alto nivel de especializaci´on. Un sector que se ha transformado profundamente gracias a esta rob´otica industrial es la industria automovil´ıstica, en la cual se usan l´ıneas de montaje con brazos rob´oticos en casi todo el proceso de fabricaci´on de los veh´ıculos. Por otro lado, tenemos el sector de la rob´otica de servicios. A´un no existe total consenso en cuanto a la definici´on de robot de servicios, pero seg´un la IFR1se trata de un robot que opera de forma semi o totalmente aut´onoma para realizar servicios ´utiles para el bienestar de los seres humanos y equipos, excluyendo las operaciones de fabricaci´on [1]. Dentro de los robots de servicios, existen los de uso profesional y los de uso personal seg´un la aplicaci´on final a la que se destinen. Un ejemplo de robot de servicio comercial de uso profesional ser´ıa por ejemplo el prototipo de robot repartidor de Amazon (Fig. 1.1a ), mientras que un robot de servicios de uso personal ser´ıa la aspiradora autom´atica Roomba (Fig. 1.1b ) Dentro de esto, la planificaci´on de caminos o trayectorias es un parte fundamental del problema del movimiento aut´onomo del robot. Para esta tarea existen numerosos 1IFR: International Federation of Robotics 1 Cap´ıtulo 1. Introducci´on (a) Prototipo de robot repartidos de la compa˜nia Amazon (b) Robot aspiradora autom´atico Roomba 980 Figura 1.1: Ejemplos de robots de servicios enfoques, nosotros concretamente usaremos un algoritmo heur´ıstico, pero tambi´en existen otras aproximaciones, como los algoritmos probabil´ısticos por ejemplo. En el Cap´ıtulo 2 se describir´a el estado del arte de las diferentes metodolog´ıas existentes para abordar la resoluci´on de tal problem´atica. 1.1. El problema de la planificaci´on El problema de la planificaci´on de forma general estudia como un sistema puede pasar de estado inicial a un estado final o goal dada una serie de posibles acciones simples en un espacio dado. As´ı pues, la resoluci´on de un cubo de Rubbik es un ejemplo de problema de planificaci´on en el cual el estado inicial puede ser cualquier configuraci´on aleatoria del cubo, el estado final el cubo resuelto y las acciones, los giros de las caras. La soluci´on a este problema consistir´ıa en una secuencia ordenada de giros que partiesen del estado aleatorio y terminasen en el estado ordenado o resuelto. De forma m´as concreta, en el campo de la rob´otica, la planificaci´on se refiere a la automatizaci´on de sistemas mec´anicos con sensores, actuadores y capacidad de computaci´on [2]. Esto crea la necesidad de encontrar algoritmos eficientes que transformen ´ordenes de alto nivel en comandos de movimiento de bajo nivel. La necesidad de eficiencia suele venir impuesta por las limitaciones computacionales del sistema. 2 Cap´ıtulo 1. Introducci´on 1.2. Planificaci´on de caminos La planificaci´on de caminos es una de las partes esenciales involucradas en el funcionamiento de un robot aut´onomo. Sin embargo, esto es una aplicaci´on concreta de la planificaci´on de caminos, ya que el objetivo general de ´esta es encontrar el camino m´as corto entre dos puntos o en general el camino ´optimo. De esta forma, dependiendo de como se defina la m´etrica, podremos aplicarlo a diferentes campos. Es por esto que este tipo de algoritmos tambi´en son importantes en network routing, videojuegos o en la comprensi´on de como se doblan las prote´ınas [3]. En el campo de la rob´otica, la planificaci´on de caminos o trayectorias responde a la necesidad de ejecutar una serie de acciones para ir de un estado inicial hasta un estado final de forma realizable y segura. Enti´endase aqu´ı por camino o trayectoria realizable aquella que es capaz de ejecutar la plataforma concreta, dadas sus limitaciones mec´anicas. En el Cap´ıtulo 2 describiremos las metodolog´ıas existentes para planificar tanto caminos como trayectorias as´ı como sus virtudes y defectos, ya que no existe una soluci´on universal al problema, si no que debe elegirse la que mas convenga seg´un los requerimientos de la aplicaci´on considerada. La diferencia entre planificaci´on de caminos y trayectorias radica en que las trayectorias son una sucesi´on de estados temporales, mientras que los caminos son una sucesi´on de puntos espaciales separados una distancia que puede ser arbitrariamente peque˜na (en la pr´actica la separaci´on m´axima ´util es la resoluci´on del mapa usado). La planificaci´on de trayectorias se suele referir al problema de como moverse a lo largo de un camino dado (sucesi´on de puntos en el espacio sin obst´aculos entre ellos) respetando las limitaciones mec´anicas del sistema rob´otico (restricciones de velocidad y aceleraci´on por ejemplo). 1.3. Marco del trabajo El contenido de este trabajo se enmarca dentro de las tareas del proyecto ARCO2. 2Autonmous Robot Co-Worker 3 Cap´ıtulo 1. Introducci´on Se trata de un proyecto europeo financiado por el proyecto HORSE (n´umero de subvenci´on 680734) que a su vez se enmarca en el programa Horizon2020. El proyecto tiene como objetivo el dise˜no de robot aut´onomo destinado a ayudar en el transporte de materiales dentro de peque˜nos y/o medianos almacenes o l´ıneas de producci´on con vistas a la Industria 4.0 y el uso de IoT3. Este sistema permitir´ıa combinar la extrema flexibilidad y adaptaci´on de los humanos con la seguridad y control de los robots terrestres. Esta plataforma rob´otica permitir´ıa reducir la carga de trabajo f´ısico que actualmente soportan los operarios que trasladan por el almac´en las numerosas mercanc´ıas. Adem´as de esta reducci´on en el trabajo lesivo, esta plataforma rob´otica permitir´ıa la paralelizaci´on de tareas de forma que mientras el robot desplaza materiales, los operarios del almac´en podr´ıan desempe˜nar otras tareas de forma simult´anea, incrementando la productividad. Este sistema es particularmente interesante para almacenes y fabricas peque˜nas y medianas donde el coste de la robotizaci´on total no es asumible. En concreto, el proyecto ARCO se validar´a en la sede en Lisboa de la empresa portuguesa Tintas Robbialac S.A., un fabricante de pinturas perteneciente al grupo Cromology, uno de los l´ıderes mundiales en el ´area de pinturas. En el Cap´ıtulo 4 se mostrar´an varios resultados de los experimentos realizados en dicha planta. El contenido de este trabajo se centra en el apartado de la planificaci´on de caminos, uno de los elementos centrales en la navegaci´on aut´onoma. Esta navegaci´on aut´onoma consta de dos partes bien diferenciadas, la planificaci´on del camino y la ejecuci´on de este. Esto ´ultimo ser´ıa tarea de un m´odulo de path-tracking. Una descripci´on completa del sistema puede verse en [4]. Existen ciertos requisitos de funcionamiento relativos a la seguridad que condicionan el enfoque que se la ha dado en este trabajo a la planificaci´on de trayectorias. Para asegurar que el sistema responde r´apidamente a la aparici´on de obst´aculos inesperados, se adoptar´a una estrategia de replanificaci´on local continua. Esta replanificaci´on ser´a solo de caminos, y luego el seguidor de caminos se encargar´a de enviar comandos de velocidad siguiendo un algoritmo simple, por lo que las trayectorias ser´an creadas en tiempo real a partir del camino. 3Internet of Things 4 Cap´ıtulo 2 Metodolog´ıas para la Planificaci´on de Caminos Como se ha dicho anteriormente, existen diferentes m´etodos que se diferencian por el enfoque del que parten. A continuaci´on introduciremos tres grandes grupos, a saber: las metodolog´ıas probabil´ısticas (RRT por ejemplo), los m´etodos de campos potenciales y los m´etodos heur´ısticos. Como ya hemos dicho, el algoritmo usado en este trabajo, Lazy Theta* pertenece a ´este ´ultimo grupo. Por otro lado, en este trabajo hablaremos mas propiamente de planificaci´on de caminos que de trayectorias, entendi´endose por camino el conjunto de traslaciones y rotaciones necesarias para mover un objeto (el robot en nuestro caso) entre dos puntos. La planificaci´on de trayectorias partir´ıa del camino resultante anterior y resolver´ıa el problema de mover el robot a lo largo de estos puntos respetando las limitaciones mec´anicas del robot, de esto ´ultimo se encargar´a el m´odulo de seguimiento de caminos descrito en la secci´on 3.4. En general, cualquier enfoque, necesita una informaci´on m´ınima para calcular un camino o trayectoria entre un estado A y un estado B. Por un lado se necesita un mapa del entorno, que puede ser est´atico o din´amico. Por otro lado necesitamos saber las coordenadas del objetivo en ese mapa y finalmente pero no menos importante la propia posici´on y orientaci´on del robot. Para tener una estimaci´on fiable de esta ´ultima se utilizan diferentes sensores (odometr´ıa, l´aseres, c´amaras, sistemas RF, IMU) 5 Cap´ıtulo 2. Metodolog´ıas para la Planificaci´on de Caminos pr´acticos. En este tipo de m´etodos el problema se reduce a buscar un camino dentro de un grafo que representa el espacio donde se mueve el robot. Una forma com´un de representar el espacio para este tipo de algoritmos es discretiz´andolo en forma de una malla de v´ertices. Uno de los problemas que suelen surgir en este tipo de algoritmos es que el camino resultante requiere un post-procesado para suavizar los caminos obtenidos lo que repercute en el tiempo de computaci´on total necesario. Existen multitud de algoritmos dentro de esta categor´ıa y cada uno con multitud de variantes, algunos de ellos son Dijkstra, A*, Theta*, Lazy Theta*, D*, ARA*. En esta secci´on explicaremos concretamente los algoritmos Dijkstra, A*, Theta* y finalmente Lazy Theta*, el usado en la implementaci´on de los planificadores descritos en el Cap´ıtulo 3. 2.3.1. Dijkstra Fue publicado en 1959 por E.W. Dijkstra [10]. Se trata de un algoritmo de b´usqueda con coste uniforme, o dicho de otra forma, emplea una heur´ıstica de valor 0. En lo que sigue nos referiremos al valor de la heur´ıstica como la estimaci´on de la distancia al punto objetivo. Esta heur´ıstica cero hace que explore todos los nodos contiguos al nodo inicial y as´ı sucesivamente de forma uniforme hasta que llega al nodo final. A continuaci´on se describen los pasos en el algoritmo. Sea A=Scon Sel nodo inicial y Ael nodo actual: 1. Se marcan todos los nodos como no visitados y se crea el conjunto correspondiente 2. Se inicializan todas las distancias relativas como infinito ya que son desconocidas salvo la distancia del nodo inicial a si mismo que es 0. 3. Para el nodo A, se calculan todas las distancias de sus vecinos no visitados al nodo inicial. Se comparan las distancias resultantes con las ya calculadas anteriormente. Si la calculada en esta iteraci´on para un nodo dado es menor que la distancia que constaba anteriormente, se sustituye ´esta por la nueva. 12 Cap´ıtulo 2. Metodolog´ıas para la Planificaci´on de Caminos 4. Una vez recorridos todos los vecinos sin visitar de A, se marca el nodo actual como visitado y se elimina del conjunto de nodos no visitados. Un nodo visitado no se volver´a a analizar nunca. 5. Si el nodo objetivo se ha marcado como visitado o si la menor distancia entre nodos del conjunto de nodos sin visitar es infinita, el algoritmo ha terminado. En el caso contrario toma como pr´oximo nodo Aaquel con la menor distancia y se regresa al paso 3 mientras existan nodos no visitados. Como puede verse se trata de un algoritmo simple, f´acil de implementar. 2.3.2. A* Este algoritmo es una extensi´on del Dijkstra, fue publicado en 1968 por Peter Hart, Nils Nilsson y Bertram Raphael [11]. Usando una heur´ıstica no trivial (diferente de cero) obtiene un mejor rendimiento que su predecesor, pues realiza una exploraci´on dirigida al nodo objetivo. Se trata de uno de los algoritmos m´as extendidos. La diferencia fundamental respecto al Dijkstra, es que en cada iteraci´on A* toma como nodo siguiente aquel con el menor valor de una funci´on f(n) definida como sigue: f(n) = g(n) + h(n) (2.2) Donde: g(n) es la distancia del nodo nal nodo inicial siguiendo el camino generado hasta el momento para llegar a n h(n) es la estimaci´on de la distancia desde el nhasta el nodo objetivo o goal. Esta es la funci´on que se conoce com´unmente como heur´ıstica Como vemos, el algoritmo Dijkstra es un caso concreto de A* con h(n) = 0 para todo n. La dificultad en este tipo de algoritmos reside en seleccionar o dise˜nar la heur´ıstica adecuada. En la figura 2.9 podemos ver el pseudo-c´odigo, que nos ser´a ´util para explicar las diferencias de A* con Theta* y Lazy Theta*. 13 Cap´ıtulo 2. Metodolog´ıas para la Planificaci´on de Caminos Como vemos en el pseudo-c´odigo, el algoritmo parte de una lista “abierta” y otra “cerrada”. El nodo de partida se introduce en la lista abierta y mientras esta lista no est´e vac´ıa se toma el nodo sde menor fen ella. Este nodo sse saca de la lista abierta y se generan sus ocho vecinos, asign´andoles como padre s. A continuaci´on para cada vecino s0, se comprueba si es el nodo objetivo, en este caso se ha terminado la b´usqueda. En caso contrario, si el nodo no est´a en la lista cerrada se comprueba si tampoco est´a en la lista abierta. En este caso se le asigna g=∞y se pone a nulo su padre. A continuaci´on se actualiza el valor de gdel nodo comprobando si la distancia del nodo inicial al nodo sm´as la distancia entre sys0es menor que la distancia del nodo inicial a s0. En caso afirmativo se actualiza el valor g(s0) y se le asigna a s0el nodo scomo padre. 2.3.2.1. Ejemplos de heur´ısticas Seg´un la libertad de movimiento en una malla de puntos o grid bidimensional, podemos distinguir tres casos generales para lo que existe consenso en cuanto a la heur´ıstica ´optima que elegir. A continuaci´on se describen a modo de ejemplo. Movimiento en cuatro direcciones (horizontal y vertical): Distancia Manhattan h(n) = |nx−goalx|+|ny−goaly|(2.3) Movimiento en ocho direcciones (horizontal, vertical y diagonal): Distancia diagonal, el m´aximo de los valores absolutos de las diferencias de las coordenadas x e y del nodo actual y el objetivo h(n) = max(|nx−goalx|,|ny−goaly|) (2.4) Movimiento en cualquier direcci´on: Distancia eucl´ıdea h(n) = q(nx−goalx)2+ (ny−goaly)2(2.5) Estas heur´ısticas tienen la propiedad de ser consistentes. Es decir la estimaci´on que hacen de la distancia a un nodo Nes siempre igual o menor a la estimaci´on de 14 Cap´ıtulo 2. Metodolog´ıas para la Planificaci´on de Caminos Figura 2.9: Pseudo-c´odigo del algoritmo A* distancia desde cualquier vecino directo de Nal objetivo m´as la distancia de alcanzar este vecino. h(N)≤c(N, P) + h(P) (2.6) Esto no es m´as que la desigualdad triangular. Por otro lado, otro concepto relevante 15 Cap´ıtulo 2. Metodolog´ıas para la Planificaci´on de Caminos Figura 2.10: Ejemplo de exploraci´on realizada por A* en un malla con un obst´aculo y posibilidad de movimiento diagonal aqu´ı es el de heur´ıstica admisible, esto es, una heur´ıstica que nunca sobrestima el coste o distancia al nodo objetivo. Todas las heur´ısticas admisibles son consistentes, pero lo contrario no es cierto [12]. 2.3.3. Theta* Se trata de una variaci´on de A*, propuesto en 2010 por K.Daniel et al. [13]. Esta variaci´on surge del hecho de que el algoritmo A* siempre encuentra el camino m´as corto en el grafo, sin embargo este camino m´as corto en el grafo no tiene por qu´e coincidir con el camino m´as corto en el entorno continuo. Esto se debe a que el algoritmo A* restringe el camino a los bordes del grafo. El algoritmo Theta* parte de la base de A* pero elimina esta restricci´on, lo que convierte a Theta* en un algoritmo de Any-angle seg´un los autores. En la figura 2.11 se ilustra la diferencia entre los caminos encontrados por Theta* y A*. La diferencia entre ambos algoritmos radica en que Theta* permite que el padre de un v´ertice sea cualquier v´ertice visible, al contrario que A*, donde el padre tiene que ser obligatoriamente un vecino desocupado. La diferencia en el pseudo-c´odigo de ambos es m´ınima, afectando solo a la funci´on de c´alculo de costes, la figura 2.12 muestra la funci´on de c´alculo de costes usada por Theta*. 16 Cap´ıtulo 2. Metodolog´ıas para la Planificaci´on de Caminos Figura 2.11: A la izquierda, el camino m´as corto con la restricci´on de pasar por los bordes del grafo, resultado del algoritmo A*. A la derecha, camino calculado por Theta* sin la restricci´on mencionada. Figura 2.12: Funci´on ComputeCosts del algoritmo Theta* 2.3.4. Lazy Theta* Este es el algoritmo usado en la implementaci´on del sistema planificaci´on que se ha realizado. Se trata de una variaci´on m´as reciente de Theta*. Al igual que Theta* es un algoritmo de any-angle. Esta variante del algoritmo Theta* surge del hecho de que este ´ultimo realiza muchos m´as an´alisis de visibilidad o line of sight checks de los necesarios como puede verse en la figura 2.13. Lo que sucede es que si Theta* comprueba la visibilidad entre un v´ertice y su padre, y este v´ertice nunca llega a expandirse (es decir, no se llegan a analizar sus vecinos), la 17 Cap´ıtulo 2. Metodolog´ıas para la Planificaci´on de Caminos Figura 2.13: Diferencia entre los an´alisis de linea de visibilidad realizados por Theta* y Lazy Theta*. Fuente: AiGameDev.Com comprobaci´on ha malgastado tiempo de c´alculo. De ah´ı la denominaci´on de la variante lazy, ya que realiza menos c´alculos pero devuelve pr´acticamente los mismos caminos que Theta* (Fig. 2.13). Dados tres v´ertices, s,s0y el padre de s, Lazy Theta* asume que hay linea de visibilidad ente s0y el padre de s, pudiendo omitir en el camino al v´ertice s. Esta suposici´on puede ser por supuesto incorrecta, de ah´ı que Lazy Theta* lo compruebe en el procedimiento SetV ertex (algoritmo 2,14), justo despu´es de expandir el v´ertice s0. Si resulta que efectivamente hay l´ınea de visi´on entre s0y el padre de s, no cambia nada. En el caso contrario recalcula el valor gy el padre de s0. El ahorro en n´umero de c´alculos proviene de retrasar la comprobaci´on de la l´ınea de vista. 2.3.4.1. Lazy Theta* con optimizaciones Se denomina as´ı a una modificaci´on adicional de este algoritmo que mejora notablemente la eficiencia. En este caso, la optimizaci´on se traduce en una heur´ıstica modificada. 18 Cap´ıtulo 2. Metodolog´ıas para la Planificaci´on de Caminos Figura 2.14: Algoritmo Lazy Theta* 19 Cap´ıtulo 2. Metodolog´ıas para la Planificaci´on de Caminos (a) w= 1, t= 525ms, l= 28,2m (b) w= 1,1, t= 484ms, l= 28,2m (c) w= 1,5, t= 368ms, l= 28,5m Figura 2.15: Comparativa entre tres valores diferentes de goal weight con sus correspondientes resultados de tiempos y distancias. En verde los nodos explorados y en rojo los puntos del camino resultante. La resoluci´on del mapa es de 0.05m/pixel. Como ya se describi´o en la secci´on 2.3, esta clase de algoritmos siempre encuentran una soluci´on si existe ya que usan heur´ısticas consistentes (esto es que obedecen la desigualdad de los tri´angulos, la distancia eucl´ıdea por ejemplo). Sin embargo, existe la opci´on de usar heur´ısticas inconsistentes ponderando los valores hcalculados por el algoritmo. De esta forma, Lazy Theta* con optimizaciones obtiene caminos ligeramente m´as largos pero reduce notablemente el tiempo de c´alculo. En la ecuaci´on 2.7 se muestra la heur´ıstica, donde w > 1 es el peso. h(s) = w·c(s, sgoal) (2.7) Como vemos en la figura 2.15 los tiempos se reducen un 8 % y un 30 % para pesos de 1.1 y 1.5 respectivamente lo que constituye una mejora notable. Visualmente tambi´en se aprecia como de izquierda a derecha disminuye el n´umero de nodos explorados, que es proporcional al tiempo. El n´umero de nodos explorados es una indicaci´on m´as relevante que el tiempo, puesto que el tiempo depende de la implementaci´on realizada as´ı como de la velocidad del procesador sobre el que se ha ejecutado el algoritmo. En cuanto a la longitud del camino ´esta no se ve pr´acticamente afectada, lo que es beneficioso, sin embargo en el laberinto usado como mapa en ´esta figura los caminos 20 Cap´ıtulo 2. Metodolog´ıas para la Planificaci´on de Caminos (a) w= 1, t= 6362ms, l= 38,10m (b) w= 1,1, t= 4950ms, l= 38,25m (c) w= 1,5, t= 2624ms, l= 39,50m Figura 2.16: Comparativa entre tres valores diferentes de goal weight con sus correspondientes resultados de tiempos y distancias en un mapa real. En verde los nodos explorados y en rojo los puntos del camino resultante. La resoluci´on del mapa es de 0.05m/pixel. est´an muy restringidos por lo que era de esperar que las longitudes de los caminos fuesen similares. En la figura 2.16 podemos ver m´as claramente la diferencia entre los resultados con diferentes ponderaciones. En este caso se trata de un mapa de un escenario real, donde los caminos no est´an tan restringidos como en el laberinto anterior. Vemos que para los pesos de 1.0 y 1.1 el algoritmo produce un camino similar con un n´umero de nodos explorados similar. Sin embargo para un peso de 1.5 pasa por una zona diferente, lo que conlleva un aumento de la distancia del camino en mas de 1m(entorno a un 2 % superior) con una reducci´on de tiempo de c´alculo de casi un 60 %. Ambas series de figuras han sido obtenidas a partir de la implementaci´on propia del planificador global que se detalla en la secci´on 3.2. 21 Cap´ıtulo 3. Implementaci´on y uso del algoritmo Lazy Theta* en un planificador global y local 3.4. Seguidor de Caminos Este m´odulo recibe los puntos del camino local devuelto por el planificador local y en funci´on de una serie de par´ametros y restricciones cinem´aticas del robot, env´ıa comandos de velocidad para seguir los puntos, obteni´endose de esta forma una trayectoria en el dominio espacio temporal. La particularidad de este sistema es que la trayectoria se obtiene en l´ınea, es decir va cambiando seg´un cambia el escenario local. Esto se realiza en condiciones de seguridad dada la alta frecuencia de re-planificaci´on local (entre 20-23 Hz) de forma que el sistema reacciona r´apidamente si aparece un obst´aculo inesperado. Aqu´ı se presentar´a el planteamiento propuesto para el caso de un robot hol´onomo, es decir omnidireccional. Esto es posible gracias a las ruedas tipo Mecanum Wheels (Fig. 3.6). En la figura 3.7 se muestra como se obtiene el movimiento en x,yy de rotaci´on. El m´odulo de seguimiento de caminos o path-tracker env´ıa comandos de velocidad, Figura 3.6: Rueda tipo Mecanum Wheel. Este tipo de rueda permite un movimiento omnidireccional Figura 3.7: Principio de funcionamiento de una plataforma omnidireccional con cuatro ruedas Mecanum Wheel [14]. 28 Cap´ıtulo 3. Implementaci´on y uso del algoritmo Lazy Theta* en un planificador global y local esto es, valores de Vx,VyyWzal driver, que de acuerdo con las ecuaciones de la cinem´atica de este sistema (En [15] podemos ver un desarrollo formal de esta cinem´atica) env´ıa comandos de velocidad de rotaci´on individual a cada rueda. El sistema de seguimiento de puntos consiste en tomar los puntos devueltos por el planificador local y enviar comandos de velocidad en VxyVydada una velocidad m´axima Vmax de la siguiente forma: Vx=Vmax cos(θ) (3.1) Vy=Vmax sin(θ) (3.2) Donde el ´angulo θes aquel formado entre la direcci´on frontal del robot (en el sistema de referencia base-link, el eje x en sentido positivo) y la direcci´on hacia el punto considerado. A primera vista, esto conducir´ıa a movimientos cualesquiera ya que el robot es libre de moverse en cualquier direcci´on por ser omnidireccional. Sin embargo esto no siempre es beneficioso por diversas razones. Por un lado, por el principio de funcionamiento de las ruedas, que producen m´as deslizamiento al mover el sistema lateralmente (en direcci´on y) o en las rotaciones, lo que empeora la estimaci´on de localizaci´on por odometr´ıa. Por otro lado, un robot que puede moverse de forma repentina en cualquier direcci´on resulta poco predecible y esto puede no ser lo m´as adecuado en un entorno din´amico con personas. Es por ello que adem´as de los comandos de velocidad en x e y, se mandan tambi´en comandos de rotaci´on Wz, para de forma simult´anea al movimiento de traslaci´on omnidireccional, orientar el robot hacia el punto siguiente, de forma que el robot vaya “encarando” los puntos que debe seguir. Finalmente, este m´odulo tambi´en se comunica con el planificador local para indicarle cuando se ha llegado al punto objetivo para que el planificador local pare de enviar caminos, y vuelva a esperar un camino global que seguir por parte del planificador global. Como medida adicional de seguridad, se ha implementado un sistema de seguridad que para autom´aticamente el env´ıo de comandos de velocidad si una medida del l´aser es menor o igual a una cierta distancia de seguridad, configurable. Esto permite evitar 29 Cap´ıtulo 3. Implementaci´on y uso del algoritmo Lazy Theta* en un planificador global y local colisiones en caso de que la replanificaci´on no fuese lo suficientemente r´apida. Este sistema funciona a la misma frecuencia que los l´aseres (40 Hz). ´ Unicamente cuando el obst´aculo se ha alejado una cierta distancia (mayor que la primera, tambi´en configurable) el sistema contin´ua siguiendo el camino devuelto por el planificador local. En el Cap´ıtulo 4 se mostrar´an los resultados conjuntos al usar este sistema de seguimiento de caminos con los planificadores global y local descritos anteriormente. 30 Cap´ıtulo 4 Resultados y Discusi´on Para validar el funcionamiento de los planificadores se han realizado tanto experimentos en simulaci´on (para el caso del planificador global) como con una plataforma real omnidireccional para el caso de los dos planificadores funcionando simult´aneamente con el seguidor de caminos. En primer lugar se mostrar´an los caminos devueltos por el planificador global en simulaci´on para diferentes escenarios de pruebas y un breve an´alisis de su funcionamiento. En segundo lugar se mostrar´an los resultados de modificar el algoritmo Lazy Theta* con optimizaciones para hacerlo sensible a un costmap como el descrito en la secci´on 3.2. Por ´ultimo se mostrar´an los resultados del sistema completo en funcionamiento, planificadores global y local con algoritmo modificado y seguidor de caminos en un entorno industrial real. 4.1. Planificador global en simulaci´on Dado que este m´odulo de planificaci´on solo requiere un mapa est´atico y las posiciones inicial y final del camino, puede evaluarse su funcionamiento simplemente por simulaci´on. Para verificar su correcto funcionamiento usaremos mapas de laberintos rectangula- 31 Cap´ıtulo 4. Resultados y Discusi´on res obtenidos de MazeGenerator1 Este generador nos permite obtener laberintos con una ´unica soluci´on y variar la complejidad del laberinto mediante los siguientes dos par´ametros: Factor R: Controla la tendencia elitista del algoritmo generador de laberintos. Un laberinto elitista es aquel con una soluci´on corta respecto a las dimensiones de este mientras que uno no elitista tiene una soluci´on que atraviesa una mayor parte del laberinto Factor E: Controla la tendencia a la ramificaci´on. Un alto valor de este factor tiene pocas calles sin salida pero largas, mientras que con un valor bajo sucede lo contrario. En las Figuras 4.1a y 4.1b se ilustra el efecto de usar valores extremos de estos par´ametros y como afectan a la cantidad de nodos explorados y por tanto al tiempo de c´alculo del camino. En las Figuras 4.2a, 4.2b y 4.2c se ilustran casos de combinaciones de valores intermedios de estos par´ametros. Visualmente puede apreciarse la diferencia entre los nodos explorados, aprenci´andose casos en los para hallar la soluci´on el algoritmo solo explora una m´ınima parte del mapa, como en la figura 4.2c mientras que en otros casos explora casi la totalidad de este (Fig. 4.1a. El objetivo aqu´ı es poner a prueba el planificador global en laberintos con diferentes factores R y E para verificar el correcto funcionamiento de este. Adem´as los tiempos y el n´umero de nodos explorados visualmente deber´an estar correlaciones con los valores de R y E en cierta medida. Adem´as los laberintos se han explorado dando como punto inicial la salida superior y como punto final la inferior. Esto es importante para comparar resultados ya que estos pueden variar seg´un de que punto se parta debido a la ausencia de simetr´ıa. Estos mapas tipo laberinto resultan ´utiles para hacer an´alisis iniciales del comportamiento del algoritmo. Sin embargo en la Secci´on 4.2 se usar´a un mapa real para poner a prueba el algoritmo modificado con el objetivo de que los resultados ilustren mejor el comportamiento en un entorno realista. 1http://www.mazegenerator.net/ : Generador de laberintos online 32 Cap´ıtulo 4. Resultados y Discusi´on (a) E= 0, R= 0, L= 98,4m, t= 710ms (b) E= 100, R= 100, L= 44,1m,t= 595ms Figura 4.1: A la izquierda un laberinto con muchas calles sin salida cortas y una soluci´on muy larga respecto a sus dimensiones. Puede verse como el algoritmo necesita explorar casi la totalidad del laberinto. A la derecha, el caso opuesto, al haber menos calles sin salida y ser la soluci´on m´as corta respecto a las dimensiones del laberinto, el algoritmo necesita explorar menos nodos. (a) E= 0, R= 50, L= 64,7m,t= 714ms (b) E= 50, R= 0, L= 23,7m,t= 195ms (c) E= 50, R= 50, L= 29.m,t= 100ms Figura 4.2: A la izquierda laberinto poco ramificado pero con soluci´on muy larga, en el centro, lo contrario y a la derecha un caso intermedio. Puede verse como en estos dos ´ultimos casos el algoritmo solo necesita explorar una peque˜na fracci´on del laberinto para encontrar la soluci´on. 33 Cap´ıtulo 4. Resultados y Discusi´on 4.2. Lazy Theta* sensible a los costes Como ya se explic´o anteriormente, esta modificaci´on esta motivada por el uso de un costmap, ya que por defecto el algoritmo solo distingue entre nodos ocupados y no ocupados. La motivaci´on del uso del costmap es la de obtener caminos m´as seguros, entendi´endose como caminos seguros aquellos que que pasan por los puntos m´as alejados de los obst´aculos en cada momento, por ejemplo por el centro de un pasillo. Adem´as de obtener caminos m´as seguros, un camino que discurra por las zonas de m´ınimo coste posible aumenta tambi´en la suavidad de este. Como hemos visto en los ejemplos anteriores, los caminos devueltos por el algoritmo sin tener en cuenta costes son bruscos en cuanto al ´angulo que forman los puntos entre ellos. Esta suavidad resulta muy necesaria en aplicaciones como la navegaci´on aut´onoma. La soluci´on propuesta consiste en modificar la funci´on g(s), la cual originalmente es la distancia del nodo sal nodo inicial por el camino calculado hasta el momento al nodo s. Esta modificaci´on afecta a la forma de calcular distancias entre nodos, a˜nadiendo un termino aditivo proporcional al coste del nodo siguiente. Es decir, cuando el algoritmo este situado en un nodo y calcule la distancia a los vecinos m´as pr´oximos, esta se ver´a distorsionada, favoreciendo a los vecinos de coste menor frente a los de coste mayor. Dados 3 nodos, s,s0yp(s) el padre de s: D(s, s0) = Cw·c(s0) + q(sx−s0 x)2+ (sy−s0 y)2(4.1) D(start, s) = D(p(s), start) + D(p(s), s) (4.2) Con start el nodo de partida y D(p(s), start) calculada de forma acumulativa a lo largo del camino desde start ap(s) mediante la ec. 4.1. Esto hace que de forma acumulada la nueva distancia al punto inicial se vea afectada favoreciendo caminos por zonas de menor coste. Por otro lado, para que ´esta modificaci´on surta efecto, debe modificarse tambi´en la distancia de linea de vista. Esto se debe a que el propio algoritmo tiende a unir los 34 Cap´ıtulo 4. Resultados y Discusi´on Figura 4.3: Comparativa entre n´umero de nodos explorados respecto al caso sin cost weight(algoritmo sin modificar) frente a distancia de l´ınea de vista para diferentes valores de cost weights. nodos que tienen linea de vista con los padres de otros nodos, de forma que aunque se fuerce al algoritmo a buscar caminos por las zonas de menor coste alterando la distancia eucl´ıdea entre ellos, si hay linea de vista entre un nodo y el padre de otro, los unir´a y el hecho de haber tenido en cuenta los costes no habr´a servido de nada. A continuaci´on se muestra un breve an´alisis del resultado de estas modificaciones. Los resultados presentados a continuaci´on se han realizado bajo el mismo sistema con el planificador global en simulaci´on, con un mapa de la planta de Tintas Robbialac S.A. con sede en Lisboa. En todos los ensayos se han usado los mismos puntos como inicial y final. En las figuras 4.5 y 4.6 pueden visualizarse algunos caminos obtenidos con par´ametros representativos. En todos ellos el punto inicial es aquel de la esquina inferior derecha y el punto final el m´as cercano a la esquina superior derecha del mapa. En las Figura 4.3 se muestra la proporci´on entre el n´umero de nodos explorados con 35 Cap´ıtulo 4. Resultados y Discusi´on Figura 4.4: Comparativa de n´umero de nodos explorados frente a cost weight para diferentes distancias de l´ınea de vista. el algoritmo modificado y sin modificar, para diferentes valores del par´ametro cost weight en funci´on de la restricci´on de l´ınea de vista. Como era de esperar, al aumentar la l´ınea de vista, el cost weight deja de tener efecto por lo que se explic´o anteriormente. Adem´as se observa como al aumentar el cost weight para una misma l´ınea de vista, aumentan los nodos explorados, esto puede deberse a que cuanto m´as grande es ´este, mas se distorsiona el mapa que ve el algoritmo. En la Figura 4.4 se muestran datos an´alogos, pero usando como variable independiente el cost weight. Puede observarse como los nodos explorados dependen no linealmente de la restricci´on de l´ınea de vista. Por un lado, tal y como era de esperar, la modificaci´on realizada reduce la eficiencia del algoritmo, aumentando la cantidad de nodos explorados para el mismo caso. Como se ve en la figura 4.5a este efecto se reduce al aumentar la l´ınea de vista. Sin embargo este aumento de la l´ınea de vista, como ya se ha dicho, reduce la efectividad de tener en cuenta los costes. En la figura 4.5b puede verse como efectivamente esta modificaci´on conduce a caminos m´as suaves ,aunque ligeramente m´as largos, que respecto al caso sin modificar (Fig. 4.5a). Puede observarse tambi´en que pr´acticamente 36 Cap´ıtulo 4. Resultados y Discusi´on (a) L= 41m, 62000 nodos explorados (b) L= 43,3m, 320000 nodos explorados Figura 4.5: A la izquierda, camino resultante sin modificar el algoritmo, a la derecha, algoritmo con par´ametros cost weight = 0.25 y linea de vista = 1 m. independientemente del valor del cost weight, al aumentar la l´ınea de vista, el n´umero de nodos explorados converge al caso con cost weight cero. Esto refleja la necesidad de restringir la l´ınea de vista. Por otro lado vemos como el n´umero de nodos explorados es creciente con el cost weight y decreciente con la distancia de l´ınea de vista (Fig. 4.5b) como era de esperar. Estos an´alisis permiten acotar las posibilidades en la elecci´on del valor de estos par´ametros seg´un la geometr´ıa del escenario y otros requerimientos como el tiempo de ejecuci´on, que esta relacionado directamente con el n´umero de nodos explorados. En las figuras 4.5 y 4.6 podemos ver una serie de caminos ilustrativos de los efectos de los par´ametros estudiados anteriormente. Los puntos verdes corresponden a los nodos explorados, los puntos rojos a los nodos que pertenecen al camino resultante. Las zonas de color cyan representan puntos ocupados para el algoritmo, las zonas m´as cercanas a estas(rojas) son las de mayor coste. Puede verse como claramente el camino m´as suave (Fig. 4.5b) es aquel que explora m´as nodos. Tambi´en en la figura 4.6a puede verse como el par´ametro cost weight resulta in´util sin la restricci´on de l´ınea de vista y viceversa en la figura 4.6b. 37 Bibliograf´ıa [1] IFR. Service robots - definition and classification. https://www.ifr.org/servicerobots/, 2016. [2] Steven M. Lavalle. Planning Algorithms. Cambridge University Press, 2006. [3] Nikolas Correl. Introduction to Autonomous Robots, v1.7. Magellan Scientific, 2016. [4] J.A. Cobano F. Caballero L. Merino R. Rey, M. Conflan. Human-robot co-working system for industry 4.0 warehouse automation. In Proceedings of the 24th IEEE Conference on Emerging Technologies and Factory Automation, 2019. [5] L. E. Kavraki, P. Svestka, J. . Latombe, and M. H. Overmars. Probabilistic roadmaps for path planning in high-dimensional configuration spaces. IEEE Transactions on Robotics and Automation, 12(4):566–580, Aug 1996. [6] Roland Geraerts and Mark H. Overmars. A Comparative Study of Probabilistic Roadmap Planners, pages 43–57. Springer Berlin Heidelberg, Berlin, Heidelberg, 2004. [7] Steven M. Lavalle. Rapidly-exploring random trees: A new tool for path planning. Technical report, Iowa State University, 1998. [8] Seth Hutchinson George Kantor Wolfram Burgard Lydia Kavraki Sebastian Thrun Howie Choset, Kevin Lynch. Principles of Robot Motion: Theory, Algorithms, and Implementation. The MIT Press, 2005. 44 [9] S. Arimoto H. Noborio, S. Wazumi. An implicit approach for a mobile robot running on a force field without generation of local minima. IFAC Proceedings Volumes, 23:87–92, Aug 1990. [10] E.W. Dijkstra. A note on two problems in connexion with graphs. Numerische Mathematik, 1:269–271, 1959. [11] Nilsson N. J. Raphael B. Hart, P. E. A formal basis for the heuristic determination of minimum cost paths. IEEE Transactions on Systems Science and Cybernetics, SSC-4(2), Jul 1968. [12] Richard E. Korf. Recent progress in the design and analysis of admissible heuristic functions. In Berthe Y. Choueiry and Toby Walsh, editors, Abstraction, Reformulation, and Approximation, pages 45–55, Berlin, Heidelberg, 2000. Springer Berlin Heidelberg. [13] S. Koenig K. Daniel, A. Nash and A. Felner. Theta*: Any-angle path planning on grids. Journal of Artificial Intelligence Research, 39:533–579, 2010. [14] Thomas Br¨aunl. Embedded Robotics. Springer, 2006. [15] Hamid Taheri, Bing Qiao, and Nurallah Ghaeminezhad. Kinematic model of a four mecanum wheeled mobile robot. International Journal of Computer Applications, 113(3):6–9, March 2015. 45