Einen Ultraschallsensor per Servo scannen, um Freiraum zu finden

Ein einzelner geradeaus schauender Entfernungssensor kann einem Roboter sagen, dass etwas voraus ist, aber nicht, wohin frei ist. Also dreht er blind – oft gerade in ein weiteres Hindernis oder eine Sackgasse. Den HC-SR04 auf ein SG90-Servo zu montieren behebt das: Der Roboter stoppt, schaut links und rechts und dreht dorthin, wo es am offensten ist – derselbe Trick, den ein Mensch an einer blinden Ecke tut.

Warum ein fester Sensor den Roboter fängt

Man stelle sich einen Roboter vor, der nur geradeaus sieht. Er trifft eine Wand, weiß, dass er drehen muss, hat aber keine Ahnung, ob links oder rechts besser ist. Falsch gewählt und er fährt in die Ecke, aus der er gerade kam. Ein servomontierter Sensor verwandelt eine Zahl (Entfernung voraus) in eine kleine Karte des Raums vor dem Roboter – die Drehung wird also eine informierte Wahl, kein Münzwurf.

Den Sensor aufs Servo montieren

Den HC-SR04 ans Servohorn schrauben und das Servo vorne-mittig aufs Chassis setzen – ausgerichtet, sodass das Horn den Sensor links-rechts schwenkt (nicht hoch-runter). In Servomittenstellung sollte der Sensor genau geradeaus zeigen. Das ergibt einen sauberen Schwenk von einer Seite über geradeaus zur anderen.

Schwenken und abtasten

Das Servo über eine Handvoll Winkel schreiten und an jedem eine gefilterte Messung nehmen. Die saubere Entfernungsfunktion aus den HC-SR04 auslesen wiederverwenden – ein roher Ping mitten im Schwenk ist viel zu verrauscht, um danach zu lenken.

#include <Servo.h>
Servo scanner;

const int ANGLES[] = {30, 60, 90, 120, 150};
const int N = 5;

int bestAngle() {
  long best = -1;
  int bestA = 90;
  for (int i = 0; i < N; i++) {
    scanner.write(ANGLES[i]);
    delay(180);                 // let the servo REACH the angle first
    long d = medianCm();        // filtered read from the previous tutorial
    if (d > best) { best = d; bestA = ANGLES[i]; }
  }
  scanner.write(90);            // re-centre for driving
  return bestA;                 // > 90 = open to the left, < 90 = right
}

Nicht mitten in der Bewegung lesen

Der häufigste einzelne Fehler hier ist Lesen, bevor das Servo ankommt. servo.write() kehrt instant zurück – doch das SG90 braucht Zeit, um auf den neuen Winkel zu schwenken. Zu früh gepingt und die Entfernung gehört dorthin, wo der Sensor war, nicht wohin man ihn zeigen ließ. Ein kurzes delay nach jedem write() – genug, damit sich das Servo setzt –, macht jede Messung vertrauenswürdig.

Den Scan in eine Entscheidung verwandeln

Der Winkel mit der größten Entfernung ist die offenste Richtung. Ein paar Verfeinerungen halten sie vernünftig:

• Bei Gleichstand geradeaus bevorzugen. Lesen zwei Richtungen ähnlich, zur Mitte neigen, damit der Roboter nicht unnötig webt.
• Alle Richtungen nah? Das ist eine Sackgasse – zurücksetzen und erneut scannen statt weiter hineinzuschnüffeln.

Tempo gegen Abdeckung

Mehr Winkel liefern ein feineres Bild, aber eine langsamere Schleife – und eine langsame Schleife heißt: Der Roboter reagiert spät. Drei bis fünf Winkel – links, voraus, rechts, plus ein paar dazwischen – sind der übliche Sweet Spot. Diese „am offensten“-Entscheidung an den Hindernisvermeidungsalgorithmus übergeben und das Gesamtverhalten erst im Hindernisvermeidungssimulator ausprobieren, bevor man es baut.

Wie lange braucht das Servo wirklich?

„Ein kurzes Delay“ ist die vageste Anweisung im Code oben – und lohnt Ersetzen durch eine gemessene Zahl. Ein SG90 ist mit grob 0,1 s pro 60° bei 4,8 V spezifiziert, unbelastet – die Beruhigungszeit hängt also davon ab, wie weit es fährt:

Zwei Dinge machen die echte Zahl schlechter als die Angabe. Last: Ein HC-SR04 mit Kabel am Horn ist klein, aber nicht null – und verlangsamt das Servo und fügt Überschwingen hinzu. Spannung: Bei 4,0 V aus sackendem Pack ist dasselbe Servo merklich langsamer als bei 6 V.

Eigenes messen statt der Tabelle trauen. Einen 60°-Schritt befehlen und stoppen, wie lange bis die Entfernungsmessung aufhört, sich zu ändern:

void measureSettle() {
  scanner.write(60);  delay(500);            // known start
  scanner.write(120);                        // command the step
  const unsigned long t0 = millis();
  long prev = -1;
  for (int i = 0; i < 40; i++) {             // sample every 10 ms
    const long d = pingCm();
    Serial.print(millis() - t0); Serial.print(' '); Serial.println(d);
    delay(10);
  }
}

Die Messung setzt sich in einem bestimmten Moment. Diese Zahl plus kleine Marge verwenden – und die größte Unsinnsquelle im Gesamtverhalten ist beseitigt.

In eine Richtung schwenken ist schneller als herumspringen

Der Code oben schreitet 30 → 60 → 90 → 120 → 150 – bereits ein einzelner Schwenk, und das ist Absicht. Winkel in gestreuter Reihenfolge zu lesen heißt: Jeder Schritt ist ein großer – und die Beruhigungskosten fallen pro Schritt an:

Ein vollständiger Scan ist ein blinder Moment für den Roboter – er steht und kommt nicht voran: Seine Dauer zu halbieren ist also eine echte Verbesserung, wie sich der Roboter anfühlt. Der Reihe nach schwenken – und bei wiederholtem Scannen Richtung abwechseln, damit man die Rückfahrt nie bezahlt.

Nicht blockierend scannen

Das delay(180) in jedem Schritt heißt: Ein Fünf-Winkel-Scan blockiert fast eine Sekunde. An einem Roboter, der ohnehin steht, erträglich – heißt aber auch: Bumper bleiben ungelesen und jedes andere Verhalten friert ein.

Dieselbe Zustandsmaschinenbehandlung wie für den Ping gilt hier:

enum ScanState { SCAN_IDLE, SCAN_MOVING, SCAN_READING };

ScanState scanState = SCAN_IDLE;
uint8_t   scanIndex = 0;
unsigned long scanSince = 0;
long      scanResult[N];
bool      scanComplete = false;

void updateScan() {
  switch (scanState) {
    case SCAN_IDLE:
      break;

    case SCAN_MOVING:
      if (millis() - scanSince < SETTLE_MS) return;      // measured, above
      scanState = SCAN_READING;
      break;

    case SCAN_READING:
      scanResult[scanIndex] = medianCm();
      if (++scanIndex >= N) { scanComplete = true; scanState = SCAN_IDLE; scanner.write(90); }
      else { scanner.write(ANGLES[scanIndex]); scanSince = millis(); scanState = SCAN_MOVING; }
      break;
  }
}

Die Hauptschleife ruft updateScan() auf und fährt fort. Bumper werden weiter gelesen, die Zustandsmaschine läuft weiter, und der Scan beendet sich, wenn er sich beendet.

Zwischen Winkeln interpolieren

Fünf Messwerte liefern fünf Peilungen – doch die wahre Öffnung liegt selten exakt auf einer davon. Ein gewichteter Schwerpunkt über die offenen Messwerte liefert eine Peilung zwischen den Messwerten – so wie ein Reflexionsarray eine Linienposition zwischen Sensoren liefert:

// Weight each angle by how open it is, ignoring anything below the stop threshold.
float openBearing() {
  float weighted = 0, total = 0;
  for (int i = 0; i < N; i++) {
    if (scanResult[i] < STOP_CM) continue;             // blocked, contributes nothing
    const float w = scanResult[i] - STOP_CM;           // how much clearance beyond the threshold
    weighted += w * ANGLES[i];
    total    += w;
  }
  return (total > 0) ? (weighted / total) : 90.0f;     // 90 = straight ahead
}

Das ist eine echte Verbesserung gegenüber „das Maximum nehmen“. Das Maximum zu nehmen heißt: Eine leicht außermittig abgetastete Türöffnung schickt den Roboter an die Türkante; der Schwerpunkt schickt ihn in die Türmitte. Es degradiert außerdem graceful: Sind zwei Nachbarwinkel beide offen, liegt das Ergebnis dazwischen statt willkürlich auf einem davon.

Der Kegel begrenzt, was ein Scan sagen kann

Der Strahl eines HC-SR04 hat grob 15° Halbwinkel – was bei 1 m ein Kegel von etwa 53 cm Durchmesser ist. Bei 30°-Abstand abzutasten überlappt also stark – Nachbarmessungen schauen großteils in denselben Raum.

Die wichtige Folge: Ein Scan liefert keine Winkelauflösung besser als der Strahl. Eine Messung von 68 cm bei 120° heißt „etwas ist 68 cm entfernt, irgendwo innerhalb etwa 15° um 120°“ – in Ordnung, um eine Drehrichtung zu wählen, und nutzlos, um die Breite einer Lücke zu messen. Wer Letzteres braucht: Ein VL53L0X auf demselben Servo hat einen effektiv engen Fleck und liefert einen wirklich aufgelösten Scan.

Fehlersuche