Seguimiento de paredes y giros precisos para robots en laberintos

Introducción

Un algoritmo de resolución de laberintos asume que el robot está en la celda que cree que está. Flood fill calcula una ruta perfecta, y luego el robot roza una pared en el tercer giro y cada decisión posterior se toma sobre la celda equivocada.

Así que rara vez lo que falla es el algoritmo. Lo que falla es el movimiento: mantenerse centrado en un pasillo, y girar exactamente noventa grados. Este tutorial trata de esas dos cosas, y de por qué el enfoque obvio para cada una es el equivocado.

Por qué la navegación por dead reckoning sola no basta

El enfoque tentador es conduzirse solo por conteos de encoder: una celda son N pulsos, un cuarto de vuelta son M pulsos, y el laberinto es una rejilla, así que solo cuenta.

Falla porque los errores se acumulan y nunca se autocorrigen. Las ruedas patinan al acelerar. Los dos motores no son idénticos. El entrejejes que mediste se desvía un milímetro, así que cada giro son 88° en vez de 90°. Cada error es pequeño; ninguno se cancela. Veinte celdas dentro, el robot va en diagonal contra las paredes con total confianza.

El arreglo es usar el propio laberinto como referencia. Las paredes son rectas, paralelas y a una separación conocida — una medida gratuita, a prueba de deriva y disponible de forma continua.

Seguir la pared como un lazo PID

Con un sensor de distancia a cada lado, centrarse es un problema de control que ya has resuelto una vez en el seguidor de línea: calcula un error, llévalo a cero.

// Positive error = too close to the left wall = steer right.
float error = leftDistance - rightDistance;

float correction = Kp * error + Kd * (error - previousError);
previousError = error;

int base = 120;
setMotors(base + correction, base - correction);

La corrección se aplica de forma diferencial —sumada a una rueda, restada de la otra— para que la velocidad de avance del robot se mantenga más o menos constante mientras cambia su rumbo. Esa es la misma estructura que un seguidor de línea; solo cambia el término de error.

Empieza con Kd a cero, sube Kp hasta que el robot siga el pasillo mientras serpentea un poco, y luego añade Kd hasta que el serpenteo se amortigüe. Ki casi nunca es útil aquí: un pasillo de laberinto es corto, y la acción integral sobre todo añade retardo y sobreimpulso en cada frontera de celda.

El caso que el bucle ingenuo resuelve mal

Un laberinto tiene aberturas. En el momento en que una pared lateral se acaba, ese sensor lee la distancia hasta algo muy lejano — la pared opuesta de una celda abierta, o nada— y leftDistance - rightDistance se convierte en un error enorme y falso. El robot se lanza hacia el hueco que debía dejar pasar.

Comprueba la presencia de pared antes de fiarte de la diferencia:

const int WALL_THRESHOLD = 100;   // mm — beyond this there is no wall

bool leftWall  = leftDistance  < WALL_THRESHOLD;
bool rightWall = rightDistance < WALL_THRESHOLD;

float error;
if (leftWall && rightWall) {
  error = leftDistance - rightDistance;        // centre between both
} else if (leftWall) {
  error = 2 * (leftDistance - IDEAL_SIDE);     // hold one wall at a set distance
} else if (rightWall) {
  error = 2 * (IDEAL_SIDE - rightDistance);
} else {
  error = 0;                                    // open cell — hold heading instead
}

Los casos de un solo lado duplican la ganancia porque una sola pared te da la mitad de la información: con dos paredes, derivar 5 mm a la izquierda cambia la diferencia en 10 mm; con una, la cambia en 5.

En el caso sin paredes, recurrir a error = 0 hace avanzar recto hasta donde llegan los motores — que no es recto en la realidad. Ahí es donde entra el giroscopio.

Girar exactamente 90 grados

Contar pulsos de encoder durante un giro es el primer intento natural y no es lo bastante preciso. Un giro sobre el sitio patina más que la marcha recta, y un error de 2° por giro son 20° tras diez giros — un robot mirando en diagonal a lo largo de un pasillo.

Integra el giroscopio en su lugar. La tasa del eje Z del MPU6050 es una medida directa de lo rápido que gira el robot, y no le importa si las ruedas patinaron:

float turnDegrees(float target) {
  float heading = 0;
  unsigned long last = micros();

  while (fabs(heading) < fabs(target)) {
    unsigned long now = micros();
    float dt = (now - last) / 1000000.0;
    last = now;

    heading += (readGyroZ() - gyroBias) * dt;   // bias subtraction is essential

    int speed = (target > 0) ? TURN_SPEED : -TURN_SPEED;
    setMotors(speed, -speed);                   // pivot in place
  }
  setMotors(0, 0);
  return heading;                               // report the real angle achieved
}

Tres cosas deciden el éxito o el fracaso:

Resta el sesgo del giroscopio. Un giroscopio sin calibrar lee uno o dos grados por segundo estando quieto, y lo estás integrando. Mide la media mientras está parado, exactamente como en el tutorial de inclinación, y réstrala en cada lectura.

Frena antes y deja rodar. El robot tiene momento rotacional, así que cortar los motores exactamente en 90° se pasa. Para sobre los 85° y déjalo rodar hasta dentro — o mejor, baja la velocidad de giro a medida que el ángulo restante se encoge.

Reescuadra contra la pared frontal. Cuando un giro pone una pared delante del robot, esa pared es una referencia de 90° perfecta. Acércate despacio y deja que el bucle de seguimiento de pared te enderece contra ella, lo que pone a cero gratis el error de rumbo acumulado. Los constructores de micromouse hacen esto en cada oportunidad, porque convierte un robot que deriva en uno que se autocorrige.

Saber cuándo ha pasado una celda

El conteo de celdas es lo que conecta el movimiento con el algoritmo del laberinto. La distancia del encoder te lleva aproximadamente hasta allí; las paredes te lo dicen exactamente.

El disparador más fiable es el flanco de bajada de una pared lateral. Cuando el robot pasa una frontera de celda, un sensor lateral que estaba leyendo una pared de repente lee lejos — esa transición ocurre en un punto conocido de la celda, y es una referencia física en vez de una estimación acumulada.

Combina las dos cosas: usa los encoders para predecir la frontera, y deja que el flanco de pared la confirme o la corrija. Cuando discrepen, cree a la pared.

Verifica antes de dejarle resolver

Cuatro pruebas, en el laberinto real, en este orden:

1. Pasillo recto, diez celdas. El robot debería quedarse centrado y llegar sigue centrado. Si serpentea, Kd es demasiado baja; si se va hacia un lado, hay un desvío de sensor — mide ambos sensores laterales contra la misma pared y corrige la diferencia por software.
2. Pasillo de un solo lado. Quita una pared. El robot debe seguir recto en vez de lanzarse hacia el hueco.
3. Diez giros seguidos. Gira 90° diez veces y comprueba que el robot acaba mirando en su dirección original. Más de unos pocos grados de error y tu sesgo de giroscopio o tu inercia necesitan trabajo.
4. Reescuadre. Conduce contra una pared, deja que se escuadre, y confirma que el error de rumbo ha desaparecido después.

Solo cuando esas cuatro pasan merece la pena ejecutar un algoritmo de laberinto encima — un planificador al que le des datos de posición malos produce insensatez con toda la confianza. Vigila la capa de planificación por separado en el simulador de solucionadores de laberintos, y luego junta ambas cosas en el proyecto micromouse.