Obstakelvermijding: waarnemen, scannen, beslissen, draaien

Zodra een robot een schone afstand kan lezen en de open richting kan scannen, resteert er één stuk: de logica die beslist wat er moet gebeuren. Dit is het brein van een obstakelvermijdende robot—een korte lus die vooruit rijdt, merkt wanneer er iets te dichtbij is, om zich heen kijkt en naar de ruimte draait. Het is eenvoudiger dan het klinkt, en het goed krijgen is vooral een kwestie van niet vastlopen.

Reactief, niet gepland

Deze robot heeft geen kaart. Hij reageert op wat er op dit moment vóór hem is: waarnemen, beslissen, handelen, herhalen. Dat heet reactieve navigatie, en het is de juiste eerste aanpak—goedkoop, robuust, en genoeg om zonder iets te raken door een drukke kamer te zwerven. Het is niet hetzelfde als trajectplanning, waarbij een robot met een kaart vooraf een route kiest (dat is het terrein van A* op een bezettingsraster). Reactieve vermijding is de basis; planning komt later.

De stopdrempel

Alles hangt aan één getal: hoe dicht is te dicht? Kies een afstand waarop de robot stopt en opnieuw nadenkt. Te klein en hij stoot obstakels voordat hij kan reageren; te groot en hij stopt bij alles en schuifelt nerveus. De juiste waarde hangt af van de snelheid—een snellere robot moet eerder reageren, want hij overbrugt meer grond voordat hij kan stoppen. Voor een kleine robot op een rustig tempo is 15–25 cm een verstandig begin.

De toestandsmachine

Een afstandssensor is wat de meeste obstakels vermijdt; een bumpschakelaar vangt de dingen die hij niet kan zien, en elke robot die onbeheerd draait wil allebei.

De schoonste manier om het gedrag uit te drukken is een toestandsmachine: een paar benoemde toestanden met heldere regels om tussen door te gaan. Dat vervangt een kluwen van geneste ifs door iets dat je kunt lezen en debuggen. (Voor het algemene patroon, zie toestandsmachines voor robotgedrag.)

enum State { DRIVE, SCAN, TURN, REVERSE };
State state = DRIVE;
const int STOP_CM = 20;

void loop() {
  switch (state) {
    case DRIVE:
      driveForward();
      if (medianCm() < STOP_CM) { stopMotors(); state = SCAN; }
      break;

    case SCAN: {
      int a = bestAngle();            // servo sweep from the scan tutorial
      long open = distanceAt(a);
      if (open < STOP_CM)      state = REVERSE;   // nowhere to go
      else { turnToward(a); state = TURN; }
      break;
    }

    case TURN:
      if (turnComplete()) state = DRIVE;          // opening cleared, resume
      break;

    case REVERSE:
      backUp(400);                                // ms
      state = SCAN;                               // then look again
      break;
  }
}

De draairichting kiezen

De scan heeft het zware werk al gedaan: hij geeft de meest open richting terug. Draai die kant op. De aanroep turnToward(angle) vertaalt “links is open” naar een draai naar links en “rechts is open” naar een draai naar rechts, met differentiaalaandrijving—één wiel vooruit, het andere terug, zodat hij ongeveer op de plek draait.

Oscilleren en doodlopende wegen vermijden

Twee storingsvormen vangen naïeve robots, en beide hebben een eenvoudige oplossing:

• Heen-en-weer slingeren tussen links en rechts op een hoek waar beide er hetzelfde uitzien. Voeg een minimumdraaitijd toe zodat de robot zich aan een draai committeert in plaats van elke lus opnieuw te beslissen.
• Vastlopen in een doodlopende weg waar elke richting geblokkeerd is. Daar is de toestand REVERSE voor: achteruit, dan opnieuw scannen. Een kleine vastloopteller—harder terug als je meerdere keren hebt gescand zonder vooruitgang—breekt de ergste vallen.

Afstellen: snelheid tegenover reactieafstand

Twee knoppen bepalen of de robot soepel of panisch voelt: rijsnelheid en stopdrempel. Verhoog je de snelheid, dan moet je de drempel ook verhogen om de remafstand veilig te houden—v·t voor de latentie en v²/2a voor het remmen—en daarom verdubbelt verdubbeling van de snelheid de drempel meer dan. De snelste manier om een goed paar te vinden is ze te veranderen op een plek waar een fout niets kost—in de simulator voor obstakelvermijding—en de waarden pas daarna over te zetten op hardware.

Hysterese: twee drempels, niet één

De toestandsmachine hierboven gebruikt één STOP_CM, en dat is het eerste dat je moet repareren. Een robot die rond één drempel hangt, wisselt veelvuldig per seconde tussen DRIVE en SCAN, omdat piepkleine meetveranderingen de vergelijking omgooien. Hij stottert in plaats van zich vast te leggen.

const int STOP_CM   = 20;   // stop when closer than this
const int RESUME_CM = 30;   // do not drive again until clearer than this

De ruimte ertussen heet hysterese, en ze dwingt het gedrag ertoe zich vast te leggen. Hetzelfde patroon repareert vrijwel elke drempelgestuurde oscillatie in robotcode, niet alleen deze—een flankdetector, een waarschuwing bij een bijna lege batterij, een aanloop naar een dok.

Een nuttige vuistregel voor de grootte van die ruimte: maak hem groter dan je meetruis plus de afstand die de robot in één lus aflegt. Voor een robot op 0,3 m/s met een sensorcyclus van 60 ms is dat 18 mm extra reis plus een paar centimeter ultrasone verstrooiing—dus een gat van 10 cm is ruim en een gat van 3 cm ratelt nog.

Je committeren aan een draai

De tweede oscillatie zit op beslissingsniveau: de robot draait naar links, de scan ziet er nu anders uit, hij draait naar rechts, en hij staat ter plekke te wiebelen. Twee mechanismen lossen dat op:

unsigned long turnStartedAt = 0;
const unsigned long MIN_TURN_MS = 250;    // commit for at least this long

case TURN:
  if (millis() - turnStartedAt < MIN_TURN_MS) break;   // no re-deciding yet
  if (medianCm() > RESUME_CM) { stopTurn(); state = DRIVE; }
  break;

Een minimumdraaitijd voorkomt dat de robot opnieuw beoordeelt voordat de draai iets heeft veranderd, en het is de hervatdrempel in plaats van de stopdrempel waar hij tegen toetst.

Detecteren dat je vastzit

REVERSE handelt een doodlopende weg vóór je af. Het handelt niet het lastigere geval: een robot die technisch gezien beweegt, scant en draait, en nergens komt—aan een concave hoek, of een stoel waar hij om blijft cirkelen.

uint8_t scanCount = 0;
unsigned long lastProgressAt = 0;

// Called whenever the robot successfully drives forward for a while.
void notedProgress() { scanCount = 0; lastProgressAt = millis(); }

void escalate() {
  scanCount++;
  if (scanCount >= 3) {
    backUp(800);                    // back further
    pivot(random(120, 240));        // and turn a large, RANDOM amount
    scanCount = 0;
  } else if (millis() - lastProgressAt > 15000) {
    backUp(1200);
    pivot(180);                     // full about-turn
    lastProgressAt = millis();
  }
}

Twee ontwerpprincipes hier zijn het noemen waard.

Escalatie. De eerste reactie op een obstakel is een kleine draai; de derde reactie op dezelfde situatie is een grote. Een vaste reactie die je herhaalt is wat een robot in een hoek houdt.

Willekeur. Een deterministische robot in een symmetrische val kiest elke keer hetzelfde en loopt eindeloos rond. Een willekeurige component in de ontsnappingsdraai verbreekt die symmetrie, en het is de reden dat de vroegste stofzuigerrobots willekeurig botsten in plaats van een slimme regel te volgen—het is werkelijk robuust tegen vallen die je niet had voorzien.

De bumpschakelaars in elke toestand uitlezen

De toestandsmachine zoals hierboven geschreven kijkt alleen naar afstand, en elke afstandssensor heeft oppervlakken die hij niet kan zien. Een robot zonder aanrakingsdetectie duwt een gordijn voor zich uit tot de motoren vastlopen.

Aanraking moet boven de toestandsmachine worden afgehandeld, niet binnen één toestand:

void loop() {
  updateBumpers();                       // debounced, from the bump-switch tutorial

  // Contact overrides whatever the robot thought it was doing.
  if (bumpLeft || bumpRight) {
    stopMotors();
    backUp(300);
    pivot(bumpLeft ? +60 : -60);         // turn AWAY from the side that hit
    state = SCAN;
    return;
  }

  switch (state) { /* ... as before ... */ }
}

Let op het teken: draai weg van de schakelaar die aansloeg. Dat is precies waarom twee schakelaars hun plek verdienen boven één—één centrale schakelaar vertelt je dat je ergens tegenaan bent gekomen en laat de draairichting aan het toeval.

Snelheid en drempel, als tabel

De stopdrempel is geen constante; hij is een functie van de snelheid, want de remafstand groeit met het kwadraat van de snelheid. Voor een robot met 80 ms detectie- en beslissingstijd, die remt met 1,0 m/s², een straal van 90 mm en een marge van 50 mm:

Verdubbeling van de snelheid van 0,3 naar 0,6 m/s verdubbelt de drempel niet—maar hij groeit wel met 76%, en bij 0,9 m/s moet de robot reageren op meer dan een halve meter. Daarom gaat een robot die bij looptempo betrouwbaar vermijdt dingen raken zodra je hem versnelt, en daarom is de oplossing een snelheidsafhankelijke drempel en niet één grote vaste waarde die de robot overal timide maakt.

int stopThresholdCm(float speedMps) {
  const float latency = speedMps * 0.08f;             // 80 ms
  const float braking = (speedMps * speedMps) / 2.0f; // a = 1.0 m/s^2
  return (int)((latency + braking + 0.09f + 0.05f) * 100.0f);
}

De toestand printen is het debuggereedschap

De nuttigste regel die je kunt toevoegen:

if (state != lastReportedState) {
  Serial.print(millis()); Serial.print(F(" -> "));
  Serial.println(STATE_NAMES[state]);
  lastReportedState = state;
}

Printen bij verandering in plaats van elke lus houdt de uitvoer leesbaar, en het maakt van een robot die “soms vreemd doet” een verslag dat je kunt lezen. Vrijwel elke bug in dit gedrag is zichtbaar als een toestandsreeks die geen logica heeft—een TURN die nooit eindigt, een SCAN die veertig keer per seconde vuurt, een REVERSE dat meteen opnieuw betreden wordt.

Kun je niet uit één variabele beantwoorden “wat doet de robot op dit moment”, dan doet de toestandsmachine zijn werk niet.

Problemen oplossen