Seguire il muro e ruotare con precisione nel labirinto

Introduzione

Un algoritmo per risolvere il labirinto presuppone che il robot si trovi nella casella in cui crede di essere. Il flood fill calcola un percorso perfetto e poi il robot sfiora un muro alla terza svolta; da quel momento ogni decisione successiva riguarda la casella sbagliata.

Quindi è raramente l’algoritmo a fallire. Ciò che fallisce è il movimento: restare centrati in un corridoio e ruotare di esattamente novanta gradi. Questo tutorial riguarda entrambe le operazioni e spiega perché per ciascuna l’approccio ovvio è sbagliato.

Perché la sola stima della posizione non basta

L’approccio allettante è guidare contando soltanto gli incrementi degli encoder: una casella è N incrementi, un quarto di giro è M incrementi e il labirinto è una griglia, quindi basta contare.

Fallisce perché gli errori si accumulano e non correggono mai se stessi. Le ruote slittano in accelerazione. I due motori non sono identici. La distanza tra le ruote che hai misurato è sbagliata di un millimetro, così ogni rotazione è di 88° anziché 90°. Ogni errore è piccolo e nessuno si compensa. Dopo venti caselle, il robot avanza diagonalmente contro i muri con una convinzione che si rivela infondata.

La correzione è usare il labirinto stesso come riferimento. I muri sono rettilinei, paralleli e distanziati in modo noto: una misura gratuita, immune dalla deriva e sempre disponibile.

Il robot seguilinea come ciclo PID

Con un sensore di distanza su ciascun lato, la centralizzazione è un problema di controllo che hai già risolto una volta nel robot seguilinea: calcola un errore e portalo a zero.

// 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 correzione viene applicata in modo differenziale, sommata a una ruota e sottratta all’altra, così la velocità in avanti del robot resta all’incirca costante mentre cambia la direzione. È la stessa struttura di un robot seguilinea; cambia soltanto il termine di errore.

Inizia con Kd a zero, aumenta Kp finché il robot segue il corridoio oscillando leggermente, poi aggiungi Kd finché le oscillazioni si smorzano. Ki quasi non serve mai: un corridoio del labirinto è breve e l’azione integrale aggiunge soprattutto ritardo e overshoot a ogni confine di casella.

Il caso in cui il ciclo naïf sbaglia

Un labirinto ha aperture. Nel momento in cui una parete laterale termina, quel sensore legge la distanza da qualcosa di lontano, la parete opposta di una casella aperta o nulla, e leftDistance - rightDistance diventa un enorme errore fittizio. Il robot si inclina verso l’apertura che avrebbe dovuto attraversare.

Verifica la presenza della parete prima di fidarti della differenza:

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
}

I casi con una sola parete raddoppiano il guadagno perché una singola parete fornisce metà dell’informazione: con due pareti, spostarsi di 5 mm verso sinistra modifica la differenza di 10 mm; con una sola, la modifica di 5.

Nel caso senza pareti, ripiegare su error = 0 fa andare dritto il robot per quanto riguarda i motori, il che nella realtà non significa dritto. Qui entra in gioco il giroscopio.

Ruotare di esattamente 90 gradi

Contare gli incrementi degli encoder durante una rotazione è il primo tentativo naturale, ma non è preciso abbastanza. Una rotazione su se stesso slitta più della marcia in linea retta e un errore di 2° per rotazione diventa 20° dopo dieci rotazioni: un robot rivolto diagonalmente lungo il corridoio.

Integra invece il giroscopio. La velocità di rotazione sull’asse Z dell’MPU6050 è una misura diretta della rapidità con cui il robot ruota e non si cura che le ruote abbiano slittato:

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
}

Tre dettagli decidono se funzioni:

Sottrai il bias del giroscopio. Un giroscopio non calibrato legge un grado o due al secondo mentre è fermo, e tu stai integrando proprio quel valore. Misura la media da fermo, esattamente come nel tutorial sull’inclinazione, e sottraila a ogni lettura.

Fermati presto e lascia scivolare. Il robot ha inerzia rotazionale, quindi interrompere i motori esattamente a 90° causa overshoot. Fermati a circa 85° e lascialo completare la rotazione per inerzia; meglio ancora, riduci gradualmente la velocità di rotazione man mano che l’angolo residuo diminuisce.

Riallineati rispetto alla parete anteriore. Quando una rotazione mette un muro davanti al robot, quel muro è un riferimento perfetto a 90°. Avvicinati lentamente e lascia che il ciclo di controllo del robot seguilinea raddrizzi il robot contro di esso: in questo modo azzeri gratuitamente l’errore di direzione accumulato. Chi costruisce micromouse lo fa a ogni occasione, perché trasforma un robot che deriva in uno che autocorregge.

Riconoscere il passaggio di una casella

Il conteggio delle caselle collega il movimento all’algoritmo del labirinto. La distanza degli encoder ti porta circa nella casella giusta; i muri te lo dicono con esattezza.

Lo stimolo più affidabile è il ** fronte di discesa di una parete laterale**. Quando il robot supera un confine di casella, un sensore laterale che leggeva una parete legge improvvisamente una distanza molto maggiore: la transizione avviene in un punto noto della casella ed è un riferimento fisico, non una stima accumulata.

Combina i due metodi: usa gli encoder per prevedere il confine, poi lascia che il bordo della parete lo confermi o lo corregga. Quando sono in disaccordo, dai ragione alla parete.

Verifica prima di lasciarlo risolvere il labirinto

Quattro test, sul labirinto reale, in quest’ordine:

1. Corridoio rettilineo, dieci caselle. Il robot deve restare centrato e arrivare ancora centrato. Oscillazioni indicano un Kd troppo basso; una deriva verso un lato indica uno scostamento del sensore: misura entrambi i sensori laterali rispetto alla stessa parete e correggi la differenza via software.
2. Corridoio con una sola parete. Rimuovi una parete. Il robot deve continuare dritto invece di inclinarsi verso l’apertura.
3. Dieci rotazioni consecutive. Ruota di 90° dieci volte e verifica che il robot termini rivolto nella direzione iniziale. Se sbaglia di più di pochi gradi, devi correggere il bias del giroscopio o la decelerazione per inerzia.
4. Riallineamento. Vai verso una parete, lascia che il robot si riallinei e verifica che l’errore di direzione sia sparito.

Solo dopo che questi quattro test passano vale la pena eseguire un algoritmo per il labirinto: un pianificatore che riceve dati di posizione errati produce risposte convincenti ma sbagliate. Osserva il livello di pianificazione da solo nel simulatore del risolutore di labirinti, poi combina entrambi nel progetto micromouse.