Murenvolgen en precieze bochten voor een doofhofrobot

Inleiding

Een doofhofoplossend algoritme neemt aan dat de robot in de cel zit waar hij denkt dat hij is. Flood fill berekent een perfecte route, en dan raakt de robot bij de derde bocht een muur en wordt elke volgende beslissing over de verkeerde cel genomen.

Het algoritma is dus zelden wat faalt. Wat faalt is beweging: gecentreerd blijven in een gang en precies negentig graden draaien. Deze tutorial gaat over die twee, en over waarom de voor de hand liggende aanpak voor elk ervan de verkeerde is.

Waarom doodse schatting alleen niet genoeg is

De verleidelijke aanpak is puur op encodertellingen te rijden: één cel is N tikken, een kwart draai is M tikken, en het doofhof is een raster, dus gewoon tellen.

Het faalt omdat de fouten zich opstapelen en nooit zelf corrigeren. Wielen slippen bij versnelling. De twee motoren zijn niet identiek. De wielbasis die je gemeten hebt is een millimeter ernaast, dus elke draai is 88° in plaats van 90°. Elke fout is klein; geen ervan heffen elkaar op. Na twintig cellen rijdt de robot met volledig vertrouwen diagonaal tegen de muren in.

De oplossing is het doofhof zelf als referentie te gebruiken. De muren zijn recht, evenwijdig en op bekende afstand — een gratis, driftvrije meting die continu beschikbaar is.

Muren volgen als PID-lus

Met een afstandssensor aan elke kant is gecentreerd zitten een regelprobleem dat je al eens hebt opgelost bij de lijnvolger: bereken een fout en stuur hem naar nul.

// 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);

De correctie wordt differentieel toegepast — opgeteld bij het ene wiel en afgetrokken van het andere — zodat de voorwaartse snelheid van de robot ongeveer constant blijft terwijl de richting verandert. Dat is dezelfde structuur als bij een lijnvolger; alleen de foutterm verschilt.

Begin met Kd op nul, verhoog Kp tot de robot de gang volgt terwijl hij licht slingert, en voeg dan Kd toe tot dat nalopen verdwijnt. Ki is hier vrijwel nooit nuttig: een doofhofgang is kort, en integraal werk voegt vooral vertraging en overschrijding toe bij elke celgrens.

Het geval dat de naïeve lus verkeerd krijgt

Een doofhof heeft openingen. Zodra een zijmuur eindigt, leest die sensor de afstand tot iets ver weg — de overstaande muur van een open cel, of helemaal niets — en leftDistance - rightDistance wordt een enorme onzin-fout. De robot schiet naar voren in de opening langs.

Controleer op de aanwezigheid van een muur voordat je het verschil vertrouwt:

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
}

De eenkantige gevallen verdubbelen de versterking omdat één muur je maar de helft van de informatie geeft: met twee muren verandert 5 mm naar links het verschil met 10 mm; met één met 5 mm.

In het geval zonder muur betekent terugvallen op error = 0 rechtdoor rijden voor zover het de motoren betreft — en dat is in werkelijkheid niet recht. Daar neemt de gyroscope het over.

Precies 90 graden draaien

Encoderschakels door een draai tellen is de vanzelfsprekende eerste poging, en hij is niet nauwkeurig genoeg. Een draai op de plaats slipt meer dan rechtdoor rijden, en 2° fout per draai is 20° na tien draaibewegingen — een robot die diagonaal een gang in kijkt.

Integreer in plaats daarvan de gyroscope. De Z-asratio van de MPU6050 is een directe meting van hoe snel de robot draait, en het maakt niet uit of de wielen slipten:

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
}

Drie dingen maken of breken dit:

Trek de gyrobias af. Een ongekalibreerde gyroscope leest een à twee graden per seconde terwijl hij stilstaat, en je integreert die. Meet het gemiddelde in stilstand, precies zoals in de tutorial over kanteling, en trek het bij elke meting af.

Stop vroeg en laat uitglijden. De robot heeft draaimomentum, dus de motoren precies op 90° afschakelen betekent doorrijven. Stop rond 85° en laat hem uitschuiven — of nog beter: bouw de draaisnelheid af naarmate de resterende hoek kleiner wordt.

Lijn weer uit tegen de voorste muur. Zet een draai een muur voor de robot, dan is die muur een perfecte 90°-referentie. Rij er langzaam tegenaan en laat de murenvolglus de robot ertegenaan uitlijnen, wat opgebouwde gierfout gratis nul maakt. Micromouse-bouwers doen dit bij elke gelegenheid, want het maakt van een driftende robot een zelfcorrigerende.

Weten wanneer er een cel is gepasseerd

Celtellen is wat beweging met het doofhofalgoritme verbindt. Encoderafstand brengt je er ongeveer; de muren vertellen je het precies.

De betrouwbaarste trigger is de dalende flank van een zijmuur. Terwijl de robot een celgrens passeert, leest een zijsensor die een muur zag plots een grote afstand — die overgang gebeurt op een bekend punt in de cel, en het is een fysiek referentiepunt in plaats van een opgebouwde schatting.

Combineer de twee: gebruik encoders om de grens te voorspellen en laat de muurflank hem bevestigen of corrigeren. Botsen ze, dan geloof je de muur.

Verifieer voordat je hem laat oplossen

Vier tests, op het echte doofhof, in deze volgorde:

1. Rechte gang, tien cellen. De robot moet gecentreerd blijven en ook gecentreerd aankomen. Slingeren betekent dat Kd te laag is; naar één kant drijven betekent een sensor-offset — meet beide zijsensors tegen dezelfde muur en corrigeer het verschil in software.
2. Gang met aan één kant een muur. Haal een muur weg. De robot moet recht blijven rijden in plaats van naar de opening te schieten.
3. Tien draaibewegingen achter elkaar. Draai tien keer 90° en controleer of de robot in zijn oorspronkelijke richting eindigt. Meer dan een paar graden afwijking betekent dat je gyrobias of uitglijden aandacht nodig heeft.
4. Opnieuw uitlijnen. Rijdt op een muur af, laat hem vierkant zetten en bevestig dat de gierfout daarna verdwenen is.

Pas wanneer die vier slagen is het de moeite waard er een doofhofalgoritme bovenop te draaien — een planner die slechte positiegegevens krijgt levert zelfverzekerde onzin. Kijk naar de planningslaag op zichzelf in de doofhofoplosser-simulator, en zet daarna alles samen in het micromouse-project.