Scan een ultrasone sensor met een servo om vrije ruimte te vinden

Eén vooruitgerichte afstandssensor kan een robot vertellen dat er iets voor hem ligt, maar niet welke kant vrij is. Dus draait hij blind, vaak recht in een ander obstakel of een doodlopende gang. Het monteren van de HC-SR04 op een SG90-servo verhelpt dat: de robot stopt, kijkt links en rechts, en draait naar de richting die het meest vrij is — dezelfde truc die een mens op een blinde hoek doet.

Waarom één vaste sensor de robot vastzet

Stel je een robot voor die alleen recht vooruit ziet. Hij stuit op een muur, weet dat hij moet draaien, maar heeft geen idee of links of rechts beter is. Kiest hij verkeerd, dan rijdt hij in de hoek waar hij net vandaan kwam. Een sensor op een servo maakt van één getal (afstand vooruit) een kleine kaart van de ruimte rond de voorkant van de robot, zodat de draai een onderbouwde keuze is en geen kopsmunt.

De sensor op de servo monteren

Zet de HC-SR04 vast aan het servohoren en monteer de servo in het midden aan de voorkant van het chassis, zo dat het hoorn de sensor van links naar rechts zwaait (niet op en neer). In de middenstand van de servo moet de sensor recht vooruit wijzen. Dat geeft je een schone sweep van de ene kant, recht door, naar de andere kant.

Zwaaien en bemonsteren

Stap de servo over een handvol hoeken en neem op elke hoek een gefilterde meting. Gebruik de schone afstandsfunctie uit de HC-SR04 uitlezen — een ruwe ping halverwege de sweep is veel te ruisvol om op te sturen.

#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
}

Lees niet tijdens de beweging

De meest voorkomende bug hier is lezen voordat de servo is aangekomen. servo.write() geeft meteen terug, maar de SG90 heeft tijd nodig om naar de nieuwe hoek te zwaaien. Ping je te vroeg, dan hoort de afstand bij de plek waar de sensor was, niet waar je hem naartoe wees. Een korte delay na elke write() — genoeg om de servo te laten bezinken — maakt elke meting vertrouwenswaardig.

Van de scan naar een beslissing

De hoek met de grootste afstand is de meest vrije richting. Twee verfijningen houden het verstandig:

• Kies bij gelijkspel recht vooruit. Lezen twee richtingen vergelijkbaar, neig dan naar het midden zodat de robot niet onnodig slingert.
• Alle richtingen dicht? Dan is het een doodlopende gang — stapel terug en scan opnieuw in plaats van verder de boek in te rijden.

Snelheid tegenover dekking

Meer hoeken geven een fijner beeld maar een tragere lus, en een trage lus betekent dat de robot te laat reageert. Drie tot vijf hoeken — links, vooruit, rechts, plus een paar ertussenin — is de gebruikelijke zoete plek. Geef die beslissing “meest vrij” aan het algoritme voor obstakelvermijding, en probeer het hele gedrag in de obstakelvermijdingssimulator voordat je het bouwt.

Hoe lang doet de servo er werkelijk over?

“Een korte vertraging” is de vagigste instructie in de code hierboven, en het is de moeite waard die te vervangen door een getal dat je hebt gemeten. Een SG90 is gespecificeerd op ongeveer 0.1 s per 60° bij 4.8 V, onbelast — dus de bezinktijd hangt af van hoe ver hij reist:

Twee dingen maken het werkelijke getal slechter dan de specificatie. Belasting: een HC-SR04 met zijn kabel op het hoorn is klein maar niet nul, en het vertraagt de servo en veroorzaakt overschwing. Spanning: bij 4.0 V uit een inzakkend pakket is dezelfde servo merkbaar trager dan bij 6 V.

Meet de jouwe in plaats van de tabel te vertrouwen. Geef een stap van 60° op en meet hoe lang het duurt voordat de afstandsmeting niet meer verandert:

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

De meting bezinkt op een duidelijk moment. Gebruik dat getal plus een kleine marge, en je hebt de grootste bron van onzin in het hele gedrag verwijderd.

In één richting zwaaien is sneller dan heen en weer springen

De code hierboven stapt 30 → 60 → 90 → 120 → 150, en dat is al een enkele sweep, en dat is met opzet. Hoeken in een verstrooide volgorde uitlezen betekent dat elke stap een grote is, en de bezinkkosten zijn per stap:

Een volledige scan is een blind moment voor de robot — hij staat stil en boekt geen voortgang — dus de duur halveren is een echte verbetering in hoe de robot aanvoelt. Zwaai in volgorde, en als je herhaaldelijk scant, wissel dan van richting zodat je nooit voor de terugweg betaalt.

Niet-blokkerend scannen

De delay(180) in elke stap betekent dat een scan over vijf hoeken bijna een seconde blokkeert. Op een robot die toch al stilstaat is dat te verdragen, maar het betekent ook dat botsingsschakelaars niet worden ingelezen en al het andere gedrag is bevroren.

Dezelfde aanpak met een toestandsmachine als bij de ping geldt 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;
  }
}

De hoofdlus roept updateScan() aan en gaat verder. Botsingsschakelaars blijven ingelezen worden, de toestandsmachine blijft draaien, en de scan is klaar wanneer hij klaar is.

Interpoleren tussen hoeken

Vijf metingen geven vijf richtingen, maar de werkelijke opening staat zelden precies op een van die. Een gewogen zwaartepunt over de vrije metingen geeft een richting tussen de monsters, op dezelfde manier waarop een reflectiearray een lijnpositie tussen sensoren geeft:

// 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
}

Dit is een echte verbetering ten opzichte van “neem het maximum”. Het maximum nemen betekent dat een deuropening die net naast het midden wordt bemonsterd de robot naar de rand van de opening stuurt; het zwaartepunt stuurt hem naar het midden. Het degradeert ook netjes — zijn twee aangrenzende hoeken allebei vrij, dan ligt het resultaat ertussenin in plaats van willekeurig op een van beide.

De kegel bepaalt wat een scan je kan vertellen

De bundel van een HC-SR04 heeft ongeveer een halve hoek van 15°, wat op 1 m een kegel van ongeveer 53 cm doorsnede is. Bemonsteren op intervallen van 30° overlapt dus sterk — aangrenzende metingen kijken grotendeels naar dezelfde ruimte.

Het belangrijke gevolg is dat een scan je geen hoekresolutie geeft die beter is dan de bundel. Een meting van 68 cm op 120° betekent “iets staat 68 cm verderop, ergens binnen ongeveer 15° van 120°” — prima om een richting om in te draaien, en waardeloos om de breedte van een opening te meten. Heb je dat laatste nodig, dan geeft een VL53L0X op dezelfde servo een effectief smalle vlek en levert hij een echt opgeloste scan.

Storingzoeklijst