Bewegingsprofielen: trapezium- en S-curveversnelling

Inleiding

Zeg tegen een motor dat hij naar een positie moet gaan, en hij probeert daar onmiddellijk te komen. Niet snel—onmiddellijk. Het commando bevat geen informatie over hoe snel, dus de enige beschikbare interpretatie is “zo snel mogelijk”, en wat er daarna gebeurt wordt beslist door wat als eerste op is: het koppel, de stroomgrens, de tractie, of het vermogen van de stappenmotor om bij te benen.

Een bewegingsprofiel is de ontbrekende informatie. Het maakt van “ga naar 100 mm” een schema—waar moet de as op elk moment tussen nu en aankomst staan—en geeft de regelaar een doel dat beweegt met een snelheid die de machine daadwerkelijk kan leveren.

Dit is het verschil tussen een robotarm die aankomt en een die aankomt en blijft stotteren.

Wat een kaal positiecommando werkelijk vraagt

Neem een sprong: op t = 0 springt het doel van 0 mm naar 100 mm.

Om dat exact te volgen zou de as oneindige snelheid nodig hebben, waarvoor oneindige versnelling nodig is, waarvoor oneindige kracht nodig is. Dat kan niet, dus er geeft iets:

• Een stappenmotor krijgt een stapfrequentie opgelegd die zijn koppel bij die snelheid niet kan leveren, slaat stappen over en weet—in open lus—niet eens dat hij dat deed. De positie is nu permanent fout. Waarom dat gebeurt, en waarom het altijd vier stappen tegelijk zijn.
• Een gelijkstroommotor met een PID-regellus ziet een enorme momentane fout, verzadigt zijn uitvoer, loopt zijn integraalterm op en schiet door.
• Een servo slaat aan op zijn maximale snelheid en komt aan terwijl het hele mechanisme nog in beweging is.

Geen van dit alles zijn afstelproblemen. Het is allemaal hetzelfde probleem: het commando was fysiek onmogelijk.

Het trapeziumprofiel

De oplossing is snelheid op te leggen, en die snelheid te veranderen met een tempo dat de motor aankan. Versnel met een constant tempo tot een kruissnelheid, houd hem vast, rem daarna met hetzelfde tempo af. Plot je de snelheid tegen de tijd, dan krijg je een trapezium, en daar komt de naam vandaan.

Drie getallen bepalen het: de afstand d, een topsnelheid v_max en een versnelling a.

t_accel = v_max / a                     time spent ramping
d_accel = v_max² / (2a)                 distance used by one ramp
t_total = v_max / a  +  d / v_max       whole move, if it reaches v_max

Voor een verplaatsing van 100 mm met 100 mm/s en a = 500 mm/s²: elke helling duurt 0,2 s en legt 10 mm af, wat 80 mm kruissnelheid overlaat, voor een totaal van 1,2 s.

Dit is het hele verhaal als functie van de tijd—de positie die je regellus op tijdstip t moet nastreven:

// Target position t seconds into a trapezoidal move of `d` mm.
float profilePosition(float t, float d, float vMax, float a) {
  float tAcc = vMax / a;
  float dAcc = vMax * vMax / (2 * a);

  if (2 * dAcc > d) {                      // too short to reach vMax
    float tPeak = sqrtf(d / a);            // triangular profile
    if (t >= 2 * tPeak) return d;
    if (t < tPeak) return 0.5f * a * t * t;
    float left = 2 * tPeak - t;
    return d - 0.5f * a * left * left;
  }

  float tCruise = (d - 2 * dAcc) / vMax;
  float tTotal  = 2 * tAcc + tCruise;
  if (t >= tTotal) return d;
  if (t < tAcc)           return 0.5f * a * t * t;
  if (t < tAcc + tCruise) return dAcc + vMax * (t - tAcc);

  float left = tTotal - t;
  return d - 0.5f * a * left * left;
}

Let op wat deze functie niet is: het is geen regelaar. Hij genereert een setpoint. Wat je had—een stapplanner voor stappenmotoren, een PID-regellus, een servo-aansturing—doet nog steeds het volgen. Het profiel verandert alleen wat er gevolgd wordt.

De beweging die je vroeg is niet altijd de beweging die je krijgt

Die 2 * dAcc > d-tak is het geval waar mensen als eerste mee te maken krijgen en het verkeerd diagnosticeren.

Is de verplaatsing korter dan de afstand die nodig is om te versnellen en weer af te remmen, dan bereikt de as nooit de snelheid die je hebt ingesteld. Hij versnelt, raakt het midden en remt meteen weer af—een driehoek, geen trapezium.

De grens is d < v_max² / a. Voor de getallen hierboven is dat 20 mm, dus elke verplaatsing korter dan 20 mm bereikt hoe dan ook niet meer dan 100 mm/s.

Daarom verandert het verhogen van v_max op een machine met veel korte bewegingen niets meetbaars. De bewegingen waren nooit snelheidsbegrensd; ze waren versnellingsbegrensd. Een 3D-printer met dichte vulling is het alledaagse voorbeeld.

De hoekpunten zijn nog steeds discontinu

Het trapeziumprofiel repareerde de snelheid. Kijk je naar de versnelling, dan is het probleem alleen opgeschoven.

Op t = 0 springt de versnelling van 0 naar 500 mm/s². Boven aan de helling zakt hij meteen terug naar 0. Elk van die twee is een sprong in de versnelling—en de snelheid van verandering van de versnelling is jerk, dus een sprong in de versnelling is een oneindige jerkimpuls.

Oneindig is ook geen getal dat een machine kan produceren. Wat het in plaats daarvan oplevert:

• Een hoorbare klop aan het begin en het einde van elke helling.
• Natrillen—banden rekken, koppelingen worden opgerold en de constructie heeft compliantie, dus een plotselinge krachtverandering wekt die op en hij blijft daarna trillen. Bij een printer is dat de echo die je naast scherpe vormen ziet.
• Koppelpieken die stroomgrenzen overschrijden die het gemiddelde koppel nog als ruim binnen de grenzen had afgemeld.

De S-curve

Een S-curveprofiel begrenst ook de jerk. In plaats van direct naar de volle versnelling te stappen, loopt hij er in over een jerk-tijd t_j = a_max / j_max, houdt hem vast en loopt er weer uit. De snelheidskromme krijgt afgeronde hoeken—vandaar de naam—en de versnelling wordt zelf een trapezium.

Een volledige S-curveverplaatsing heeft zeven fasen: jerk omhoog, versnelling vasthouden, jerk omlaag, kruissnelheid, en het spiegelbeeld om te stoppen.

Wat het kost: precies één t_j

Dit is het stuk dat je uit je hoofd moet leren, want het maakt de afweging concreet.

Wanneer beide profielen dezelfde v_max en dezelfde a_max halen, duurt de S-curvebeweging precies t_j langer—één jerk-tijd, niet twee, en ongeacht hoe ver de beweging is:

trapezoidal   T = v_max/a + d/v_max
S-curve       T = v_max/a + d/v_max + t_j          where t_j = a_max / j_max

Voor de uitgewerkte beweging: 1,2 s wordt 1,3 s. Een straf van 8% op deze beweging, en een kleiner percentage op langere, in ruil voor begrensde jerk.

De reden dat het er één is en niet twee: het versnellingssegment van de S-curve is langer, dus hij legt meer af voordat hij gaat kruisen, dus de kruisfase wordt korter en geeft het meeste van de tijd terug. Alleen de jerk-tijd zelf is onherstelbaar.

Daar zit ook een randvoorwaarde in verscholen. De versnelling haalt alleen a_max als v_max ≥ a_max² / j_max. Daaronder haalt de beweging ook de volle versnelling niet en krijg je een driehoekig versnellingsprofiel—hetzelfde verhaal van “geen afstand meer”, één afgeleide hogerop.

“Jerk” in printerfirmware is geen jerk

Weten waard vóór je gaat afstellen, want de woordenschat misleidt actief.

De DEFAULT_XJERK van klassieke Marlin is niet de afgeleide van de versnelling. Het is de maximaal toegestane momentane snelheidsverandering op een kruispunt tussen twee bewegingen, in mm/s—een hoekruimte die de planner in staat stelt een hoek te nemen zonder tot nul af te remmen. Nuttig, maar een andere grootheid met de verkeerde naam en de verkeerde eenheid.

Nieuwere Marlin gebruikt standaard Junction Deviation, dat de hoek modelleert als een cirkelboog met een gegeven afwijking in mm. Klipper gebruikt square_corner_velocity—de snelheid waarmee een hoek van 90° wordt genomen. Alle drie zijn antwoorden op “hoe stop ik niet bij elke hoek?”, en geen van alle drie is een jerkbegrenzing in de fysische betekenis.

Zegt een forumbericht dus dat je je jerk moet verlagen om het natrillen te stoppen, controleer dan welke grootheid bedoeld wordt. Junction-snelheid verlagen vermindert de schok in een hoek; het begrenst da/dt binnen één beweging niet.

Je PID is waarschijnlijk prima

Heb je een gesloten lus met overshoot en oscillatie bij elke beweging, dan is de reflex om af te stellen—Kp verlagen, derivaat toevoegen, de overshoot bestrijden.

Probeer eerst de setpoint te profileren.

Een stap als ingang vraagt een PID-regelaar om oneindige uitvoer. De foutterm begint op de volle verplaatsingsafstand, de derivaatterm ziet een momentane sprong, en de integraalterm stapelt op terwijl de uitvoer verzadigd is en de as niet kan reageren. Afstellen maakt de regelaar loom overal, om één onmogelijk moment te overleven.

Met een profiel blijft de fout klein gedurende de hele beweging, want het doel is ergens waar de as werkelijk kan zijn. Dezelfde gains die tegen een stap onbruikbaar waren, gedragen zich vaak prima tegen een helling—en nu corrigeert de regelaar alleen nog voor belasting en wrijving, wat zijn taak is. Je kunt het verschil zien in de PID-simulator door een stapsetpoint te vergelijken met een geleidelijke.

Waar je dit op een echte robot tegenkomt

• Stappenmotoren. setMaxSpeed() en setAcceleration() van AccelStepper geven je een trapeziumprofiel en niets anders. Er is geen S-curveoptie; daarvoor genereer je de setpoint zelf.
• Hobby-servo’s. Een SG90 heeft een interne regelaar die je niet kunt zien of afstellen, dus profileren betekent de opgedragen hoek stap voor stap laten oplopen in plaats van het doel in één aanroep te schrijven—hetzelfde idee van buitenaf toegepast.
• Gelijkstroommotoren met encoders. Profileer de positiesetpoint en laat de encoder-PID hem volgen. Dit is de opstelling waarin profileren het meest oplevert.
• Mobiele robots. Dezelfde grens geldt voor linear.x op een cmd_vel-brug: een snelheidsstap commanderen laat de wielen slippen, wat de odometrie aantast. Begrens de snelheid van de twist.
• Printers en CNC. Marlin, Klipper en GRBL plannen allemaal trapeziumbewegingen met er een hoekregel tussen.

Als het misgaat

Profileren is de goedkoopste grote verbetering die voor de meeste hobby-bewegingssystemen beschikbaar is, want het kost één functie en geen hardware. De robotarm-simulator is een goede plek om te zien waarom het er aan het einde van een lange link toe doet, waar een kleine versnellingsstap aan de basis een zichtbare wiebel bij de grijper wordt. Op een mobiele robot is hetzelfde profiel wat een padvolger in staat stelt af te remmen voor een hoek die scherper is dan zijn draaistraal, in plaats van hem te missen.