Navegacion por puntos de paso GPS para un robot de exterior

Los grados son la unidad equivocada para un robot

Un modulo GPS te entrega latitud y longitud en grados. Todos los controladores que tienes quieren metros. La conversion es el primer trabajo y es mas facil de lo que sugiere internet.

A lo largo de los pocos cientos de metros que cubre un rover de aficionado, la Tierra es lo bastante plana para que una aproximacion equirectangular sea exacta muy por dentro del propio ruido del receptor:

norte (m) = (lat − lat₀) × 111 320
este  (m) = (lon − lon₀) × 111 320 × cos(lat₀)

Elige un origen lat₀, lon₀ una sola vez (el inicio de la mision) y cada punto de paso se convierte en un par (este, norte) en metros al que pure pursuit puede gobernar sin saber que es un grado.

La formula del haversine es mas correcta y no la necesitas. Importa a lo largo de cientos de kilometros; a 500 m las dos coinciden con unos milimetros, tres ordenes de magnitud por debajo de tu error.

El numero que hay que recordar: un grado de latitud son 111,3 km, asi que cinco decimales son 1,1 m y seis son 11 cm. Guarda los puntos de paso con seis. Cualquier cosa mas alla es guardar ruido, y un float en un AVR solo transporta unos siete digitos significativos: suficiente para 19.076123 y no suficiente para nada mas, y por eso conviertes a metros relativos a un origen en lugar de hacer aritmetica con grados en bruto.

Tres cosas malas de una posicion

Llega tarde. Un NEO-6M tipico informa a 1 Hz. A 1,5 m/s el robot recorre 1,5 m entre posiciones, y la posicion que recibe describe donde estaba cuando se calculo la solucion, no donde esta ahora.

Tiene ruido. El GPS de consumo aterriza dentro de unos 2,5 m de la verdad la mitad de las veces, en campo abierto y con cielo despejado. Bajo arboles, junto a un edificio o con una geometria de satelites pobre es mucho peor, y el fallo no es una perdida de senal que puedas detectar, sino una respuesta equivocada y creible.

No tiene rumbo. Una posicion GPS es una posicion. No puede decirte hacia donde apunta el robot, porque un robot quieto apuntando al norte y el mismo robot apuntando al sur producen posiciones identicas. El curso sobre el terreno se deriva de posiciones consecutivas y solo tiene sentido mientras se mueve de verdad; por debajo de 1 m/s es sobre todo ruido.

Que le hace eso a un seguidor de trayectoria

Dale al mismo controlador pure pursuit el mismo circuito dos veces, una con odometria perfecta y otra con una posicion a 1 Hz de ±1,5 m mantenida entre actualizaciones:

Fijate en lo que no ocurre: el robot nunca se pierde. Se mantiene mas o menos en el circuito y llega al final. La navegacion solo con GPS funciona; simplemente es enormemente peor de lo que parece en la hoja de datos, y el coste recae sobre todo en el actuador.

Las tres soluciones, por orden de valor

1 - Fusiona, no sigas. Integra la odometria de rueda y una fuente de rumbo a la frecuencia de tu bucle, y usa cada posicion GPS para corregir esa estimacion en lugar de reemplazarla. Incluso una mezcla complementaria tosca transforma el resultado:

// Entre posiciones: navegacion a estima. Esto se ejecuta a la frecuencia del bucle.
x += cos(heading) * v * dt;
y += sin(heading) * v * dt;

// Al llegar una posicion: atrae la estimacion hacia ella, con suavidad. Un alpha cercano a 0,1
// confia en la odometria durante el segundo entre posiciones, que es exactamente el instinto correcto.
if (newFix && fix.hdop < 2.5) {
  x += ALPHA * (fixX - x);
  y += ALPHA * (fixY - y);
}

Eso es un filtro complementario haciendo el mismo trabajo que hace con la inclinacion: una senal que deriva pero es suave, corregida por otra que tiene ruido pero esta acotada.

2 - Consigue el rumbo en otro sitio. Un magnetometro da rumbo absoluto estando quieto, y necesita calibracion de hierro duro y blando ademas de distancia a tus motores. Un giroscopio da un rumbo a corto plazo excelente y deriva. Juntos son lo que usa todo rover real; el curso sobre el terreno del GPS corrige entonces al par cuando se mueve lo bastante rapido como para ser fiable.

3 - Sube la frecuencia antes de subir la precision. Un modulo de 10 Hz arregla la obsolescencia que causa la mayor parte del serpenteo. Suele ser una primera mejora mejor que perseguir precision centimetrica, y es mucho mas barata.

Leer el modulo

Los modulos GPS hablan NMEA 0183 por un puerto serie a 9600 baudios: frases separadas por comas, una por linea. Quieres dos de ellas.

• $GPRMC: hora, validez, latitud, longitud, velocidad sobre el terreno y curso sobre el terreno.
• $GPGGA: calidad de la posicion, satelites usados y HDOP, la dilucion horizontal de la precision.

Interpreta la posicion, pero filtra por calidad. Dos reglas que evitan la mayoria de los desastres en exterior:

• Rechaza una posicion con estado V (void) o calidad de posicion 0. Aun contendra numeros de aspecto creible.
• Rechaza un HDOP por encima de unos 2,5. El HDOP describe la geometria de los satelites, y una mala geometria produce una respuesta segura de si misma que se desvia decenas de metros.

Usa una libreria de interpretacion en lugar de escribir la tuya, pero lee lo suficiente de las frases para saber que estas descartando. Ademas: un arranque en frio tarda de 30 segundos a varios minutos sin almanaque. No trates un robot que aun no tiene posicion como un robot que esta en el origen. Espera, con una indicacion visible de que estas esperando.

Ponle una geovalla antes de probarlo

Un robot de exterior que se porta mal se marcha. Antes de la primera prueba de campo, anade un limite duro en software: una distancia maxima desde el punto de partida y una parada si se supera.

if (hypotf(x, y) > FENCE_RADIUS) { stop(); return; }   // antes de cualquier otra cosa

Diez lineas, y son la diferencia entre un fallo de software y un robot en un canal. Acompáñalo de un tiempo maximo de mision y un corte manual que puedas alcanzar: un interruptor de choque en la cubierta superior sirve y no necesita enlace de radio.

Cuando algo sale mal

Conduce el mismo circuito con las dos fuentes de posicion en el simulador de seguimiento de trayectorias y despues construye el rover por puntos de paso GPS, donde el modulo en si y la fusion anterior son el proyecto entero.