Algoritmo de navegación RRT y unificación de nodos del ejercicio de grafo de visibilidad
Algoritmo RRT
Estas semanas he avanzado en la implementación de un nodo planificador de ruta basado en RRT (Rapidly–Exploring Random Tree). Este algoritmo construye un árbol de búsqueda a partir de la posición inicial del robot. La idea principal de cómo funciona el nodo es:
Genera un punto aleatorio en el espacio de búsqueda.
Encuentra el nodo del árbol más cercano al punto.
Expande el árbol desde ese nodo hacia la muestra, avanzando una distancia limitada (step size) si la muestra está más lejos que el step size.
Si el nuevo segmento no colisiona con ningún obstáculo, se añade el nuevo nodo al árbol.
Si el nodo está lo suficientemente cerca de la meta, se intenta conectar con ésta para finalizar.
Véase que este algoritmo no optimiza la ruta, sino que encuentra un camino válido. En los siguientes vídeos podemos observar el algoritmo en acción. Se observa que para dos goals iguales, se obtienen rutas distintas a causa de la aleatoriedad en la que se basa RRT.
Posibles mejoras que veo es que los parámetros sean configurables por comando en la terminal.
Unificación en un nodo del ejercicio de grafo de visibilidad
Durante estas semanas estuve trabajando también en conseguir unificar en un nodo el planificador de ruta basada en grafo de visibilidad y el nodo que la sigue.
- Carga el mapa de polígonos y se genera el grafo de visibilidad si no existe.
- Espera odometría. Hasta que no se recibe /odom el nodo no puede generar una ruta.
- Recibe un goal. Este será el objetivo al que deberá moverse el robot.
- Planificación de ruta con el start y goal.
- Seguimiento de la ruta.
Ahora funciona como una máquina de estados finita (FMS). Así, los estados que tiene son:
IDLE. El robot está quieto. Espera a que haya un goal y odometría válida.
ROTATING. El robot se orienta hacia el siguiente waypoint de la ruta. Una vez que el ángulo está dentro de la tolerancia fijada, el robot pasa a MOVING.
MOVING. El robot avanza hacia el waypoint. Una vez alcanza el objetivo, si hay más waypoints en la ruta, pasa a ROTATING, y si no, a IDLE.
En el siguiente vídeo podemos ver el funcionamiento del nodo. Nótese que el goal propuesto es el mismo que en el algoritmo RRT. Obtenemos una ruta mucho más óptima.
