AMR - WAREHOUSE
Silvia Calvo Cabello
Silvia Calvo Cabello
- Envié el robot a varios puntos de referencia distribuidos por el almacén (esquinas y puntos intermedios).
- Registré sus coordenadas reales.
- Localicé los píxeles correspondientes en la imagen del mapa.
- Colisión automática con modelos .dae: Esta es la forma más precisa: OMPL carga el modelo del robot y del mapa en formato DAE y calcula colisiones físicas reales y el controlador necesario. Sin embargo, en Unibotics no está disponible, porque no se incluyen las extensiones necesarias de OMPL y no existe un modelo DAE del robot holonómico. Y aunque en ackerman si, la libreria necesaria para usarlo no se encunetra disponible en unibotics.
- Colisión manual (la opción usada en esta práctica): Se indica manualmente qué zonas del mapa son obstáculos. Para ello:
- Se calculan las dimensiones del robot en píxeles.
- Se calculan también las dimensiones del robot con estantería sobre la bandeja.
- La variable with_shelf indica a isStateValid() qué tamaño debe usar en cada momento.
- OTROS_CODIGOS
- VIDEO DE MUESTRA
La práctica consiste en el control de un robot móvil AGV dentro de un almacén para realizar tareas de logística. En este caso, el robot debe desplazarse hasta las estanterías, recogerlas utilizando la bandeja elevadora (lift) y transportarlas a otra zona. Para planificar las rutas entre puntos y evitar colisiones se utiliza el planificador OMPL.
Para trabajar con el mapa del almacén, primero fue necesario relacionar las coordenadas reales del robot (en metros) con las coordenadas en píxeles de la imagen del mapa.
Para ello:
Con estos pares de puntos, calculé la matriz de transformación entre ambos sistemas mediante el método de mínimos cuadrados. El código se divide en dos funciones: una que obtiene la matriz de transformación y otra que aplica dicha matriz a cualquier punto para convertirlo entre coordenadas del mundo y del mapa.
Además, el mapa incluye las estanterías dibujadas originalmente. Como el robot las mueve, es necesario modificar dinámicamente el mapa: cuando el robot recoge una estantería, se eliminan sus píxeles como obstáculo; cuando la deja en otro lugar, se añaden al mapa.
OMPL es un planificador de movimiento que permite generar trayectorias libres de colisiones. Para esta práctica se emplea RRT*, configurado para buscar el camino más corto y válido entre la posición actual del robot y su objetivo. El planificador utiliza la función de validez isStateValid() para asegurarse de que ninguna pose propuesta esté en colisión.
OMPL dispone de dos métodos principales para detectar colisiones:
Como cuando el robot lleva una estantería deja de ser cuadrado, la orientación (yaw) es importante. Por eso la función genera un rectángulo con el tamaño adecuado, lo rota según el yaw del robot y comprueba si todos los píxeles ocupados por ese rectángulo están libres en el mapa.
Explicación del programa
Este programa implementa un sistema autónomo de planificación y movimiento para un robot que recoge estanterías de un almacén y las transporta a otra zona. Se basa en un mapa extraído de una imagen, la simulación robótica proporcionada por HAL y WebGUI, y en el uso del planificador RRT* del framework OMPL para generar trayectorias que eviten colisiones. El funcionamiento se divide en varias etapas: carga del mapa, transformación entre coordenadas, evaluación de colisiones, planificación óptima de trayectorias, seguimiento del camino y actualización del mapa al mover las estanterías. A continuación se describen las principales partes del código.Funciones del programa
transformation() y transform_point()
El sistema trabaja simultáneamente en dos espacios de coordenadas:Coordenadas del mundo: donde se mueve el robot (metros).
Coordenadas del mapa: donde se dibujan obstáculos en la imagen (píxeles).
La función transformation() calcula una matriz de transformación afín a partir de un conjunto de puntos de calibración en dos espacios diferentes (puntos en el mundo y puntos en el mapa). Esta matriz se utiliza para transformar cualquier punto de un espacio a otro.
La función transform_point() aplica la matriz de transformación a un punto específico, convirtiéndolo de las coordenadas del mundo a las coordenadas del mapa o viceversa.
Ambas funciones permiten que el planificador use el mapa correctamente para detectar colisiones al realizar la planificación de la ruta.
isStateValid()
La función isStateValid() verifica si el robot se encuentra en un estado válido, es decir, si no colisiona con obstáculos o estanterías en el mapa. El proceso se lleva a cabo en los siguientes pasos:Se transforma la posición del robot (en coordenadas del mundo) a coordenadas del mapa usando la función transform_point().
Se genera un conjunto de puntos que representa el contorno del robot (basado en su tamaño y orientación).
Se rota este conjunto de puntos de acuerdo a la orientación (yaw) del robot.
Se comprueba si alguno de estos puntos cae sobre una zona ocupada del mapa.
Si alguno de los puntos del robot cae fuera del mapa o sobre una zona ocupada, la función devuelve False, indicando que el estado no es válido. Si no hay colisión, devuelve True.
create_numpy_path()
La función create_numpy_path() toma la cadena de texto de los estados generados por el planificador y la convierte en dos tipos de coordenadas:Coordenadas de mundo: Estos son los puntos que el robot debe seguir en el mundo real (en metros).
Coordenadas de píxeles: Estos son los puntos transformados al espacio del mapa, para ser mostrados en WebGUI (en píxeles).
Esta función facilita la visualización del camino generado y también permite al robot seguirlo de manera precisa en el mundo real.
get_path()
La función get_path() es responsable de crear un problema de planificación y encontrar una trayectoria óptima desde la posición actual del robot hasta una meta dada.Crea un espacio de información (SpaceInformation) que incluye el espacio SE(2) y la función de validez del estado (isStateValid()).
Define el estado inicial del robot (start) y el objetivo (goal) a alcanzar, configurando las posiciones y orientaciones.
Configura el optimizador para la minimización de la longitud del camino y selecciona el planificador RRTstar para encontrar el camino.
Ejecuta la búsqueda del camino durante un tiempo máximo de 10 segundos.
Si se encuentra un camino, la función devuelve la ruta encontrada en las coordenadas del mundo y las coordenadas de píxeles, que luego se visualizan en WebGUI.
Si no se encuentra un camino en el tiempo asignado, la función indica que no se ha encontrado solución.
orientated_to()
La función orientated_to() ajusta la orientación del robot para que apunte hacia un ángulo objetivo.El robot gira hasta que la diferencia entre su orientación actual (yaw) y la orientación objetivo (target_yaw) sea menor que un umbral dado (yaw_threshold).
Durante este proceso, la velocidad angular (w) se ajusta en función del error angular, y el robot no avanza hasta estar correctamente orientado.
Cuando la orientación deseada se alcanza, el robot deja de girar y la función termina.
Esta función asegura que el robot esté correctamente orientado antes de moverse hacia el objetivo.
go_to()
La función go_to() mueve al robot hacia un objetivo específico (x_target, y_target).Calcula la distancia al objetivo y el ángulo (yaw) necesario para dirigir al robot hacia él.
La velocidad lineal (v) se ajusta en función de la distancia al objetivo, y la velocidad angular (w) se ajusta en función de la diferencia entre la orientación actual y la deseada.
El robot se mueve hacia el objetivo hasta que la distancia sea menor que el umbral threshold, momento en el cual la velocidad lineal se establece a 0 y el movimiento se detiene.
Si es necesario, también ajusta la orientación para asegurar que el robot siga el camino correcto.
next_target()
La función next_target() ejecuta todo el proceso de planificación para mover al robot hacia un objetivo dado, y luego lo sigue a lo largo de la ruta generada.Genera una ruta desde la posición actual del robot hasta el objetivo especificado (x_target, y_target, yaw_target) mediante el planificador de caminos get_path().
Una vez que se encuentra un camino, el robot sigue cada punto de la ruta utilizando la función go_to() y se orienta correctamente con orientated_to().
El robot se mueve de un punto al siguiente, corrigiendo su orientación y avanzando hasta llegar al destino final.
Esta función permite al robot seguir un camino óptimo de manera eficiente y precisa.
paint_map()
La función paint_map() se encarga de actualizar dinámicamente el mapa para reflejar las estanterías que el robot ha recogido o dejado en su destino.Modifica el mapa del almacén para marcar una zona ocupada cuando el robot recoge una estantería, y la marca como libre cuando la deja en un nuevo lugar.
Se asegura de que el planificador tenga en cuenta estos cambios al generar nuevas rutas, actualizando el mapa para evitar futuras colisiones.
Esta función es fundamental para mantener el mapa actualizado y garantizar que el robot navegue de manera eficiente.
Flujo principal del programa
El flujo del programa se realiza en varias etapas:El mapa se carga y se inicializan las variables globales.
El robot comienza sin transportar estanterías.
Para cada estantería en la lista:
Se planifica y ejecuta un camino hacia la estantería.
El robot la recoge y la elimina del mapa.
Se planifica un nuevo camino hacia el área de descarga.
Se actualiza el mapa marcando la estantería como depositada en el nuevo destino.
Cuando todas las estanterías han sido movidas, el programa termina y el robot detiene su movimiento.
Mapa “engordado”
La idea es bastante similar, pero enis_valid el robot se considera como un punto.
Al generar el mapa, este se “engorda” según si el robot lleva estantería o no.
Este método no es muy eficiente: si el robot tiene estantería, hay que engordar los obstáculos según la medida más grande. Como resultado, el robot puede pensar que ciertas zonas son inaccesibles, aunque en realidad podría pasar con la orientación adecuada.
Código Ackerman
La lógica del programa sigue siendo la misma, pero cambia el planificador para que tenga en cuenta que el robot es Ackerman. Además, en el control ya no puede girar sobre sí mismo, por lo que se añade un control diferente para manejar esta limitación.A continuacion se muestra el video del funcioinamineto donde el AGV holonomico calcula la ruta para llegar a la estanteria, la levanta, elimina los obstaculos guardando una copia de los rectangulos al rededor para poder calcular la nueva ruta a dejar la estanteria y al dejarla en su sitio coloca de nuevo esa imagen de copia donde corresponde para calcular el siguiente punto teniwndo en cuenta la nueva posicion de la estanteria
Ver el video
Para mostrar el funcinamiento del AGV ackerman se muestra el video del funcioinamineto donde el robot calcula la ruta para llegar a la estanteria y se dirige hacia ella. El problema es que se buggeo el simulador y estaba la plataforma elevada pero el simulador no lo detecta, por lo que se choca con la estanteria.
Ver el video