Simulatie van baanvolging: pure pursuit en Stanley

Bediening

Regelaar schakelt tussen pure pursuit en Stanley. Vooruitkijkafstand is de enige parameter van pure pursuit en is uitgeschakeld voor Stanley, dat er geen gebruikt — dat is zelf al het punt.

Snelheid en vooruitkijkafstand werken op elkaar in, en dat is waar je mee kunt spelen: stel 1 m/s in en zoek de beste vooruitkijkafstand, stel dan 3 m/s in en zoek hem opnieuw.

Positie uit vervangt perfecte odometrie door een fix van 1 Hz met ruis op meterschaal, die tussen de updates wordt vastgehouden. De fout wordt altijd tegen de waarheid gescoord, dus de straf is echt en niet verborgen.

Sleep een willekeurig waypoint, of druk op 1–8 om er een te selecteren en verplaats het met de pijltjestoetsen. De hele run wordt opnieuw berekend terwijl je sleept.

Theorie

Beide regelaars beantwoorden dezelfde vraag — gegeven waar ik ben en waar het pad loopt, welke stuurhoek? — en beantwoorden die volledig verschillend.

Pure pursuit is geometrie. Zoek het punt op het pad op een vooruitkijkafstand L verderop en rij de cirkelboog daardoorheen:

κ = 2y / L_d²          δ = atan(2 b sin α / L_d)

met y de laterale offset van het doelpunt in het robotframe en b de wielbasis. Eén parameter, en het is een afstand in plaats van een versterking — dus hij is overdraagbaar tussen robots.

Stanley is regeltechniek. Neem de vooras als referentie en tel er twee termen bij op:

δ = θ_e + atan(k · e / v)

richtingsfout plus een arctangens van de dwarslijnfout. Delen door de snelheid is wat één versterking over een snelheidsbereik laat werken. k is een echte regelversterking en is niet overdraagbaar.

Het lab voegt toe wat klassieke afleidingen weglaten: 0,4 s totale latentie, als een eerste-ordere stuurrespons van 0,25 s plus 0,15 s detectie- en rekenvertraging. Zonder die latentie wordt pure pursuit monotonisch beter naarmate de vooruitkijkafstand kleiner wordt, en verdwijnt de hele afstemvraag.

Algoritme

Elke stap, voor beide regelaars:

1. Projecteer op het pad, zoekend in een venster rond de laatst bereikte booglengte in plaats van over het hele pad. Een globale zoekactie matcht het dichtstbijzijnde segment, dus een parcours dat omkeert stuurt de voortgang van de robot naar een hoek die hij een minuut geleden al passeerde.
2. Los een gelijkspel op een knooppunt op naar het uitgaande segment. Na een hoek projecteren beide aangrenzende segmenten naar hetzelfde punt op dezelfde afstand; het binnenkomende segment vasthouden geeft de regelaar een richtingsfout en een laterale offset die beide vrijwel nul zijn terwijl hij in rechte lijn van het pad af rijdt, zonder enig signaal.
3. Bereken de stuurhoek volgens de eigen regel van de regelaar en begrens hem op de mechanische grens.
4. Pas de latentie toe en integreer daarna het fietsmodel.
5. Score tegen de waarheid, nooit tegen de schatting.

Stap 2 is geen optimalisatie. Het was het verschil tussen een werkende Stanley en een Stanley die bij elke enkele run op precies 78% zelfverzekerd het parcours verlaat.

Broncode

Pure pursuit, compleet, in de vorm die het op een echte robot aanneemt:

float lastS = 0;   // arc length last reached — state, and it only moves forward

float pursue(float x, float y, float heading, float speed) {
  // The lookahead is scheduled on speed, with a hard floor: at a standstill a
  // proportional schedule asks for zero, which is a division by zero in disguise.
  float lookahead = constrain(0.7f * speed + 0.4f, 0.4f, 3.0f);

  float s = projectOntoPath(x, y, lastS);      // windowed around lastS
  lastS = max(lastS, s);
  Point goal = pointAtArcLength(lastS + lookahead);

  float dx = goal.x - x, dy = goal.y - y;
  float lateral = -sinf(heading) * dx + cosf(heading) * dy;
  float ld = max(0.3f, hypotf(dx, dy));        // clamp, or curvature blows up

  float curvature = 2.0f * lateral / (ld * ld);
  return curvature;                            // omega = v * curvature
}

De drie regels die ertoe doen zijn de ondergrens van het schema, de monotone lastS en de begrenzing van ld. Al het andere is de formule.

Aannames en hardwareverschillen

Het lab geeft de robot een perfecte kaart van het pad, een perfect bekende wielbasis, geen wielslip en een latentie die een zuiver eerste-ordemodel is. Een echte robot heeft daar geen van allemaal.

Wielslip maakt de positieschatting fout op een manier die geen enkele regelaar kan detecteren. Tandwielspeling voegt bij elke stuurterugslag doodlopende verplaatsing toe, wat zich gedraagt als extra latentie en de optimale vooruitkijkafstand omhoog duwt. En een differentiaalaandrijving heeft helemaal geen stuurhoek — die heeft een gevraagde draaisnelheid, dus ω = v κ vervangt de atan en de wielbasis is alleen nog van belang voor hoe strak hij kan draaien.

Wat daarvan overblijft is de vorm: een optimum binnen het bereik, dat met de snelheid naar rechts schuift, en een fout bij dat optimum die met de snelheid groeit, hoe dan ook.

Schakelschema

De regelaar is het goedkope deel. Wat hij van de rest van de robot nodig heeft, is het dure deel.

Positie. Kwadratuurencoders op beide wielen en differentiaale odometrie die ze integreert, op de snelheid van de regelkring. Binnenshuis is dat voldoende voor enkele tientallen meters.

Koers. Een gyro voor de korte termijn, een magnetometer als absolute referentie, of allebei. De richtingsfout is de term die beide regelaars het zwaarst wegen, en het is de eerste die de odometrie verliest.

Een absolute correctie, buiten. Een GPS-module die in de schatting wordt gefuseerd in plaats van rechtstreeks te worden gevolgd — het verschil is 0,09 m RMS-fout tegenover 1,25.

Voor de volledige theorie, zie pure pursuit en dwarslijnfout; om het te bouwen, de GPS-waypointrover begint binnenshuis op encoders en gaat pas naar buiten zodra de volger werkt.