Inverse kinematica van een robotarm met twee links

Voorwaartse kinematica is eenvoudig: geef de hoekwaarden van de gewrichten, vermenigvuldig de transformaties van de links en je weet waar het gereedschap staat. Inverse kinematica werkt de andere kant op—geef een doelpositie en zoek hoekwaarden die hem bereiken—en juist in die richting wonen de interessante problemen. Een vlakke arm met twee links is het kleinste voorbeeld dat ze allemaal laat zien. Verken hem in de Robot Arm Simulator.

Het bereikbare werkgebied

Bij linklengtes L₁ en L₂ kan het gereedschap alleen punten binnen een annulus (een ring) bereiken. De buitenradius is L₁ + L₂ (arm volledig uitgestrekt) en de binnenradius is |L₁ − L₂| (arm volledig opgevouwen). Alles buiten de ring is te ver; alles in het gat is te dicht. Geen enkele hoekwaarde bereikt die punten, dus het juiste antwoord is simpelweg “onbereikbaar”.

Twee oplossingen: elleboog omhoog en elleboog omlaag

Voor de meeste doelen binnen de ring kan de elleboog twee kanten op buigen en het gereedschap toch op hetzelfde punt zetten. Dat zijn de configuraties elleboog omhoog (elbow-up) en elleboog omlaag (elbow-down). Ze ontstaan uit een ± in de hoek volgens de cosinusregel voor het tweede gewricht:

cos(θ₂) = (x² + y² − L₁² − L₂²) / (2·L₁·L₂)
θ₂ = ± acos(cos(θ₂))        # + is one elbow, − is the other
θ₁ = atan2(y, x) − atan2(L₂·sin θ₂, L₁ + L₂·cos θ₂)

Omdat er twee geldige antwoorden zijn, is inverse kinematica geen functie maar een keuze. Robots kiezen de configuratie die de gewrichten weg houdt van hun grenzen, de beweging vanaf de huidige pose minimaliseert, of een obstakel vermijdt.

Singulariteiten

Aan de randen van het werkgebied—arm volledig uitgestrekt of volledig opgevouwen—lopen de twee oplossingen in elkaar over. Dat zijn singulariteiten, en vlak ervoor kan een kleine beweging van het gereedschap een enorme beweging van de gewrichten eisen. Planners remmen af of lopen om singuliere poses heen, omdat de arm kortstondig het vermogen verliest om het gereedschap in één richting te bewegen.

Wanneer is er geen oplossing?

Een doel heeft geen oplossing als het buiten de bereikbare ring valt, als het een hoekwaarde vereist die de hardware verbiedt, of als het een oriëntatie eist die de beschikbare gewrichten niet kunnen maken. Een goede simulator maakt dit onderscheid eerlijk in plaats van naar de dichtstbijzijnde pose te springen en te doen alsof het gelukt is.

Analytische tegenover numerieke solvers

Bij twee of drie links heeft de geometrie een gesloten vorm (analytisch), dus je berekent de hoekwaarden rechtstreeks, exact en snel. Daarmee is analytische IK ideaal om te leren en om te testen. Redundante of volledig ruimtelijke armen hebben meestal geen gesloten vorm en hebben numerieke solvers nodig die naar een oplossing itereren—algemener, maar trager en lastiger om over te redeneren, omdat ze oplossingen kunnen missen of bij singulariteiten vastlopen.

De solver, veilig geschreven

De vergelijkingen hierboven kloppen en produceren op echte hardware nog steeds NaN. Dit is dezelfde wiskunde met elke foutmelding expliciet afgehandeld:

enum IkStatus { IK_OK, IK_TOO_FAR, IK_TOO_CLOSE, IK_JOINT_LIMIT, IK_SINGULAR };

struct IkResult {
  IkStatus status;
  float theta1, theta2;      // radians
};

IkResult solveIk(float x, float y, float L1, float L2, bool elbowUp) {
  IkResult r{};

  const float r2 = x * x + y * y;
  const float reach = sqrtf(r2);

  // 1. Reachability FIRST, before any trigonometry.
  //    A small epsilon keeps a target exactly on the boundary from
  //    producing an acos argument of 1.0000001 through rounding.
  const float eps = 1e-4f;
  if (reach > L1 + L2 - eps)          { r.status = IK_TOO_FAR;   return r; }
  if (reach < fabsf(L1 - L2) + eps)   { r.status = IK_TOO_CLOSE; return r; }

  // 2. Now acos is guaranteed to be in range.
  float c2 = (r2 - L1 * L1 - L2 * L2) / (2.0f * L1 * L2);
  c2 = constrain(c2, -1.0f, 1.0f);              // belt and braces
  const float s2 = sqrtf(1.0f - c2 * c2);

  r.theta2 = elbowUp ? atan2f(s2, c2) : atan2f(-s2, c2);

  // 3. atan2, never atan — this is what gets all four quadrants right.
  r.theta1 = atan2f(y, x) - atan2f(L2 * sinf(r.theta2), L1 + L2 * cosf(r.theta2));

  // 4. Warn near the boundary, where the arm is ill-conditioned.
  if (reach > (L1 + L2) * 0.95f) { r.status = IK_SINGULAR; return r; }

  // 5. Mechanical limits are part of the answer, not an afterthought.
  if (r.theta1 < T1_MIN || r.theta1 > T1_MAX ||
      r.theta2 < T2_MIN || r.theta2 > T2_MAX) { r.status = IK_JOINT_LIMIT; return r; }

  r.status = IK_OK;
  return r;
}

Vijf details onderscheiden dit van de schoolboekversie, en elk correspondeert met een echte storing:

Bereikbaarheid vóór trigonometrie. Ligt het doel voorbij L1 + L2, dan levert de cosinusregel een argument buiten −1 en 1 op en geeft acos NaN terug. Die NaN stroomt vervolgens als een onzin-hoek naar het servocomando—en daarom levert een onbereikbaar doel zo vaak een heftige beweging op in plaats van een weigering om te bewegen.

De epsilon. Een doel dat berekend is om precies op de grens te liggen kan door afronding in drijvende komma c2 = 1.0000001 geven. Bereikbaarheid met een kleine marge testen vangt dat op voordat acos het doet.

atan2f, niet atan. Gewoon atan neemt één quotiënt en kan een doel vóór de robot niet onderscheiden van zijn spiegelbeeld erachter, dus werkt de arm in twee kwadranten en spiegelt hij in de andere twee. Dit is de meest voorkomende bug in een eerste IK-implementatie.

Een singulariteitswaarschuwing in plaats van een weigering. Vlak bij volledige uitrekking is de oplossing nog geldig maar slecht geconditioneerd—een kleine beweging van het gereedschap vraagt een enorme gewrichtsbeweging. Markeren laat de aanroeper afremmen of eromheen lopen.

Gewrichtsgrenzen als status. Een arm waarvan de schouder niet onder −10° kan, heeft doelen die geometrisch bereikbaar en mechanisch onbereikbaar zijn. Stilzwijgend afklemmen levert een arm die ergens in de buurt van het doel komt en succes meldt.

Kiezen tussen de twee oplossingen

De wiskunde geeft je beide takken en kan niet zeggen welke je moet gebruiken. Dat is een robotica-beslissing, en er zijn vier gangbare regels:

Minimale beweging is de juiste standaard, en de reden is wat er gebeurt zonder die regel: schakelt de solver stilzwijgend midden in een pad van tak, dan klapt de arm door een grote, snelle, volledig ongeplande beweging. Op een echte arm is dat alarmerend en kan het beschadigen.

// Pick the branch closest to where the arm already is.
IkResult up   = solveIk(x, y, L1, L2, true);
IkResult down = solveIk(x, y, L1, L2, false);

if (up.status == IK_OK && down.status == IK_OK) {
  const float costUp   = fabsf(up.theta1   - cur1) + fabsf(up.theta2   - cur2);
  const float costDown = fabsf(down.theta1 - cur1) + fabsf(down.theta2 - cur2);
  return (costUp <= costDown) ? up : down;
}
return (up.status == IK_OK) ? up : down;

Rechte lijnen hebben IK bij elke stap nodig

Interpoleren tussen twee poses in gewrichtsruimte—elk gewricht lineair van zijn beginhoek naar zijn eindhoek bewegen—is eenvoudig en laat het gereedschap een kromme volgen. Voor veel taken is dat prima. Voor alles dat rechtdoor moet, interpoleer je in Cartesiaanse ruimte en los je bij elke stap IK op:

void moveStraight(float x0, float y0, float x1, float y1, int steps) {
  for (int i = 1; i <= steps; i++) {
    const float t = (float)i / steps;
    const float x = x0 + (x1 - x0) * t;
    const float y = y0 + (y1 - y0) * t;

    IkResult r = solveClosest(x, y);
    if (r.status != IK_OK) { abortMove(r.status); return; }   // do not skip the point
    commandJoints(r.theta1, r.theta2);
  }
}

Twee dingen worden zichtbaar de eerste keer dat je dit doet. De gewrichtssnelheid is niet constant, ook al is de snelheid van het gereedschap dat wel—vlak bij de rand van het werkgebied moeten de gewrichten veel sneller bewegen voor dezelfde beweging van het gereedschap. En een rechte lijn kan door een gebied lopen dat de arm niet kan bereiken, zelfs als beide eindpunten prima zijn, en daarom is de statuscontrole in de lus belangrijker dan alleen aan de uiteinden.

Van hoekwaarden naar servotellingen

De solver geeft hoekwaarden in een wiskundig stelsel terug. Een servo weet daar niets van: zijn nulpunt is waar het hoornje gemonteerd zit, en hij kan de andere kant op draaien.

struct Joint {
  uint8_t channel;
  float   offsetDeg;   // servo angle when the joint is at mathematical zero
  int8_t  sign;        // +1 or -1
  float   minDeg, maxDeg;
};

int servoAngle(const Joint &j, float jointRad) {
  const float deg = jointRad * 57.2957795f;
  const float clamped = constrain(deg, j.minDeg, j.maxDeg);
  return (int)(j.offsetDeg + j.sign * clamped);
}

Die struct is de hele brug tussen de kinematica en de hardware, en hem expliciet houden is wat het mogelijk maakt een solverbug van een kalibratiefout te onderscheiden. Een arm die soepel beweegt en ver van het doel belandt is bijna altijd dit, niet de wiskunde.

Problemen oplossen

Probeer het zelf

Sleep in de Robot Arm Simulator het doel rond en zie hoe de arm er realtime voor oplost. Schakel tussen de elleboogtakken om de alternatieve oplossing te zien, duw het doel buiten de ring om een eerlijke “onbereikbaar” te zien, en beweeg naar de rand van het werkgebied om een singulariteit te voelen.