Mejora de nodo de grafo de visibilidad

Mejora de grafo de visibilidad FMS

Esta semana he estado mejorando el nodo de grafo de visibilidad. Ahora posee 5 estados la la FMS:

1. BUILD GRAPH. Se carga el mapa, se detectan los polígonos y se genera o carga el grafo de visibilidad dependiendo si está generado o no. Una vez termina, pasa al estado WAIT GOAL.

2. WAIT GOAL. El robot está parado esperando un objetivo. Entra en este estado cuando:
   • Termina BUILD GRAPH
   • No encuentra una ruta al objetivo
   • La ruta está vacía
   • Se han completado todos los waypoints

3. EXTEND GRAPH. Una vez tenemos un goal, se extiende el grafo de visibilidad añadiendo los nodos de start y goal.

4. PLAN ROUTE. Cuando se comprueba que existe un camino, se entra en este estado. Se calcula la ruta óptima.

5. PILOTING. Entra en este estado cuando existe una ruta válida. El robot va siguiendo los waypoints hasta el destino

Mejora de navegación del robot

Otro punto que estuve mejorando esta semana fue la navegación del robot. Inicialmente el robot giraba sobre sí mismo para orientarse al objetivo y una vez orientado navegaba hasta él siguiendo un control proporcional. Ahora, hemos conseguido un pilotaje mucho más suave y natural. Usamos dos controladores proporcionales P para:

1. Error angular. El robot gira más cuanto mayor es el error angular hacia el objetivo, y gira menos conforme se alinea.

2. Error de distancia. Usando la distancia euclídea al objetivo, cuanto más lejos está, más acelera.

En ambos casos las velocidades angular y lineal están limitadas. La lógica usada para la navegación es que si el robot está muy girado respecto al objetivo, no avanza rápido. La velocidad se multiplica por un factor que se acomoda a esa lógica. Así:

• Error angular ≈ 0°. Avanza a máxima velocidad.
• Error angular ≈ 45°. Avanza a la mitad de velocidad mientras corrige.
• Error angular >= 90°. Solo giro.