Wandfolge und präzise Drehungen für einen Labyrinthroboter

Einführung

Ein Labyrinthlösealgorithmus nimmt an, der Roboter stehe in der Zelle, in der er sich wähnt. Flood Fill rechnet eine perfekte Route – und dann streift der Roboter an der dritten Drehung eine Wand und jede folgende Entscheidung gilt der falschen Zelle.

Der Algorithmus scheitert also selten. Was scheitert, ist Bewegung: in einem Korridor zentriert bleiben und exakt neunzig Grad drehen. Hierum geht es in diesem Tutorial – und darum, warum der naheliegende Ansatz zu jedem der falsche ist.

Warum Koppelnavigation allein nicht reicht

Der verlockende Ansatz: allein per Encoderzählwerten fahren: Eine Zelle sind N Ticks, eine Vierteldrehung M Ticks, und das Labyrinth ist ein Raster – also einfach zählen.

Es scheitert, weil Fehler sich summieren und nie selbst korrigieren. Räder rutschen beim Beschleunigen. Beide Motoren sind nicht identisch. Der vermessene Radstand liegt einen Millimeter daneben – jede Drehung sind also 88° statt 90°. Jeder Fehler ist klein; keiner hebt sich auf. Zwanzig Zellen weiter fährt der Roboter diagonal in Wände, in völliger Selbstgewissheit.

Die Lösung: das Labyrinth selbst als Referenz nutzen. Die Wände sind gerade, parallel und in bekanntem Abstand – eine gratis, driftfreie Messung, fortlaufend verfügbar.

Wandfolge als PID-Schleife

Mit Entfernungssensor auf jeder Seite ist Zentrieren ein Regelungsproblem, das man schon einmal im Linienfolger löste: einen Fehler rechnen, auf null fahren.

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

Die Korrektur wird differenziell angewandt – auf ein Rad addiert, vom anderen subtrahiert –, sodass die Vorwärtsgeschwindigkeit des Roboters grob konstant bleibt, während sich sein Kurs ändert. Dieselbe Struktur wie ein Linienfolger; nur der Fehlerterm ist anders.

Mit Kd bei null anfangen, Kp erhöhen, bis der Roboter dem Korridor leicht webend folgt, dann Kd dazu, bis das Weben abdämpft. Ki nützt hier fast nie: Ein Labyrinthkorridor ist kurz – und Integralwirkung fügt meist Verzögerung und Überschwingen an jeder Zellgrenze hinzu.

Der Fall, den die naive Schleife falsch hat

Ein Labyrinth hat Öffnungen. In dem Moment, wo eine Seitenwand endet, liest dieser Sensor die Entfernung zu etwas Fernem – der Gegenwand einer offenen Zelle, oder gar nichts –, und leftDistance - rightDistance wird ein riesiger Scheinfehler. Der Roboter wirft sich in die Lücke, an der er vorbeifahren sollte.

Erst Wandpräsenz prüfen, bevor man der Differenz traut:

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
}

Die einseitigen Fälle verdoppeln die Verstärkung – denn eine einzelne Wand liefert halbe Information: Mit zwei Wänden ändert 5-mm-Drift nach links die Differenz um 10 mm; mit einer ändert sie um 5.

Im wandlosen Fall fährt Fallback error = 0 geradeaus, soweit es die Motoren betrifft – was in der Realität nicht geradeaus ist. Dort übernimmt das Gyro.

Exakt 90 Grad drehen

Encoderticks durch eine Drehung zu zählen ist der natürliche erste Versuch – und nicht genau genug. Eine Drehung auf der Stelle rutscht mehr als Geradeausfahrt – und 2° Fehler pro Drehung sind 20° nach zehn Drehungen: ein Roboter, der diagonal einen Korridor hinunterschaut.

Stattdessen das Gyro integrieren. Die Z-Achsen-Rate des MPU6050 ist eine direkte Messung, wie schnell sich der Roboter dreht – und es kümmert sie nicht, ob die Räder rutschten:

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
}

Drei Dinge entscheiden darüber:

Den Gyrobias abziehen. Ein unkalibriertes Gyro liest im Stand ein, zwei Grad pro Sekunde – und man integriert es. Den Mittelwert im Stillstand messen, exakt wie im Neigungstutorial, und bei jeder Lesung abziehen.

Früh stoppen und ausrollen lassen. Der Roboter hat Drehimpuls – wer die Motoren bei exakt 90° kappt, schießt darüber. Bei etwa 85° stoppen und einrollen lassen – oder besser: Die Drehgeschwindigkeit rampen, während der Restwinkel schrumpft.

Gegen die Vorderwand neu ausrichten. Wenn eine Drehung eine Wand vor den Roboter stellt, ist diese Wand eine perfekte 90°-Referenz. Langsam darauf zufahren und die Wandfolgeschleife den Roboter dagegen gerade richten lassen – was aufgestauten Kursfehler gratis nullt. Micromouse-Bauer tun das bei jeder Gelegenheit – denn es macht aus einem driftenden Roboter einen selbstkorrigierenden.

Wissen, wann eine Zelle vorbei ist

Zellenzählen verbindet Bewegung mit dem Labyrinthalgorithmus. Encoderstrecke bringt dich ungefähr dorthin; die Wände sagen es exakt.

Der verlässlichste Auslöser ist die fallende Flanke einer Seitenwand. Während der Roboter eine Zellgrenze passiert, liest ein Seitensensor, der eine Wand las, plötzlich weit – dieser Übergang geschieht an bekanntem Punkt in der Zelle, und er ist eine physische Landmarke statt aufgestauter Schätzung.

Beide kombinieren: Encoder zur Vorhersage der Grenze nutzen, dann die Wandkante bestätigen oder korrigieren lassen. Bei Dissens der Wand glauben.

Verifizieren, bevor man lösen lässt

Vier Tests, auf dem echten Labyrinth, in dieser Reihenfolge:

1. Gerader Korridor, zehn Zellen. Der Roboter soll zentriert bleiben und zentriert ankommen. Weben heißt: Kd zu niedrig; einseitiges Driften heißt: Sensoroffset – beide Seitensensoren gegen dieselbe Wand vermessen und die Differenz per Software korrigieren.
2. Einseitiger Korridor. Eine Wand entfernen. Der Roboter muss geradeaus weiter statt in die Lücke zu werfen.
3. Zehn Drehungen hintereinander. Zehnmal 90° drehen und prüfen, dass der Roboter am Ende in Ausgangsrichtung zeigt. Mehr als wenige Grad daneben und Gyrobias oder Ausrollen brauchen Arbeit.
4. Neu ausrichten. Auf eine Wand zufahren, sich aufstellen lassen und bestätigen, dass der Kursfehler danach weg ist.

Erst wenn diese vier bestehen, lohnt es, einen Labyrinthalgorithmus obendrauf zu fahren – ein Planer mit schlechten Positionsdaten erzeugt selbstbewussten Unsinn. Die Planungsschicht allein im Labyrinthlösersimulator beobachten, dann beides zusammen im Micromouse-Projekt vereinen.