Inverse Kinematik eines Zwei-Gelenkarms

Vorwärtskinematik ist leicht: Bei gegebenen Gelenkwinkeln die Gliedtransformationen multiplizieren und man weiß, wo das Werkzeug ist. Inverse Kinematik läuft andersherum – bei gegebener Zielposition Gelenkwinkel finden, die sie erreichen –, und in dieser Richtung leben die interessanten Probleme. Ein planarer Zwei-Gelenkarm ist das kleinste Beispiel, das sie alle zeigt. Im Roboterarmsimulator erkunden.

Der erreichbare Arbeitsraum

Mit Gliedlängen L₁ und L₂ erreicht das Werkzeug nur Punkte in einem Kreisring (einem Ring). Der Außenradius ist L₁ + L₂ (Arm voll gestreckt) und der Innenradius |L₁ − L₂| (Arm voll gefaltet). Alles außerhalb des Rings ist zu fern; alles im Loch ist zu nah. Keine Gelenkwinkel erreichen diese Punkte – die korrekte Antwort lautet schlicht „unerreichbar“.

Zwei Lösungen: Ellbogen oben und Ellbogen unten

Für die meisten Ziele im Ring kann sich der Ellbogen auf zwei Arten beugen und das Werkzeug trotzdem auf denselben Punkt setzen. Das sind die Ellbogen-oben- und Ellbogen-unten-Konfigurationen. Sie stammen aus einem ± im Kosinussatzwinkel des zweiten Gelenks:

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 θ₂)

Weil es zwei gültige Antworten gibt, ist Inverskinematik keine einzelne Funktion – sie ist eine Wahl. Roboter wählen die Konfiguration, die Gelenke von ihren Grenzen fernhält, Bewegung aus der aktuellen Pose minimiert oder einem Hindernis ausweicht.

Singularitäten

An den Arbeitsraumrändern – Arm voll gestreckt oder voll gefaltet – verschmelzen beide Lösungen zu einer. Das sind Singularitäten: Nahe ihnen kann eine kleine Werkzeugbewegung eine riesige Gelenkbewegung verlangen. Planer werden langsam oder routen um singuläre Posen, weil der Arm kurz die Fähigkeit verliert, das Werkzeug in eine Richtung zu bewegen.

Wann gibt es keine Lösung?

Ein Ziel hat keine Lösung, wenn es außerhalb des erreichbaren Rings liegt, wenn es einen Gelenkwinkel verlangen würde, den die Hardware verbietet, oder wenn es eine Orientierung verlangt, die die vorhandenen Gelenke nicht erzeugen können. Ein guter Simulator unterscheidet diese Fälle ehrlich, statt auf die nächste Pose zu springen und Erfolg vorzugeben.

Analytische gegen numerische Löser

Für zwei, drei Glieder hat die Geometrie eine geschlossene (analytische) Lösung – man rechnet die Gelenkwinkel direkt, exakt und schnell. Das macht analytische IK ideal zum Lehren und Testen. Redundante oder voll räumliche Arme haben meist keine geschlossene Form und brauchen numerische Löser, die sich iterativ einer Lösung nähern – allgemeiner, aber langsamer und schwerer zu durchschauen, weil sie Lösungen verfehlen oder nahe Singularitäten hängenbleiben können.

Der Löser, sicher geschrieben

Die Gleichungen oben sind korrekt und erzeugen auf echter Hardware trotzdem NaN. Hier dieselbe Mathematik mit jedem Fehler explizit behandelt:

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

Fünf Details trennen das von der Lehrbuchversion – und jedes entspricht einem echten Fehler:

Erreichbarkeit vor Trigonometrie. Liegt das Ziel jenseits L1 + L2, erzeugt die Kosinusregel ein Argument außerhalb −1 bis 1 und acos liefert NaN. Dieses NaN fließt als Winkelunsinn in den Servobefehl – weshalb ein unerreichbares Ziel so oft eine heftige Bewegung statt Bewegungsverweigerung erzeugt.

Das Epsilon. Ein Ziel, das rechnerisch exakt auf der Grenze sitzt, kann nach Gleitkommarundung c2 = 1,0000001 liefern. Erreichbarkeit mit kleiner Marge zu testen fängt es ab, bevor acos es tut.

atan2f, nicht atan. Einfaches atan nimmt einen einzigen Quotienten und kann ein Ziel vorne nicht von seinem Spiegelbild hinten unterscheiden – der Arm arbeitet also in zwei Quadranten und spiegelt in den anderen beiden. Das ist der häufigste einzelne Fehler in einer ersten IK-Implementierung.

Eine Singularitätswarnung statt Verweigerung. Nahe Vollstreckung ist die Lösung weiter gültig, aber schlecht konditioniert – eine kleine Werkzeugbewegung verlangt eine riesige Gelenkbewegung. Sie zu flaggen lässt den Aufrufer langsamer werden oder darum herumrouten.

Gelenkgrenzen als Status. Ein Arm, dessen Schulter nicht unter −10° darf, hat Ziele, die geometrisch erreichbar und mechanisch nicht sind. Lautloses Klemmen erzeugt einen Arm, der irgendwo nahe dem Ziel landet und Erfolg meldet.

Zwischen beiden Lösungen wählen

Die Mathematik liefert beide Äste und kann nicht sagen, welchen nehmen. Das ist eine Robotikentscheidung – und es gibt vier gängige Regeln:

Minimalbewegung ist die richtige Voreinstellung – und der Grund ist, was ohne geschieht: Wechselt der Löser mitten im Pfad lautlos den Ast, kippt der Arm durch eine große, schnelle, völlig ungeplante Bewegung. An einem echten Arm ist das alarmierend und kann destruktiv sein.

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

Gerade Linien brauchen IK bei jedem Schritt

Zwischen zwei Posen im Gelenkraum zu interpolieren – jedes Gelenk linear von Start- zu Endwinkel bewegen – ist einfach und lässt das Werkzeug einer Kurve folgen. Für viele Aufgaben ist das in Ordnung. Für alles, was geradeaus fahren muss, interpoliert man im kartesischen Raum und löst IK bei jedem Schritt:

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

Zwei Dinge werden beim ersten Mal sichtbar. Gelenkgeschwindigkeit ist nicht konstant, obwohl Werkzeugtempo es ist – nahe der Arbeitsraumkante müssen sich Gelenke für dieselbe Werkzeugbewegung weit schneller bewegen. Und eine Gerade kann durch einen Bereich laufen, den der Arm nicht erreicht, selbst wenn beide Endpunkte in Ordnung sind – weshalb die Statusprüfung in der Schleife zählt statt nur an den Enden.

Von Gelenkwinkeln zu Servozählwerten

Der Löser liefert Gelenkwinkel in einem mathematischen Frame. Ein Servo kennt diesen Frame nicht: Seine Null liegt dort, wo das Horn montiert wurde, und es dreht eventuell andersherum.

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

Diese Struktur ist die gesamte Brücke zwischen Kinematik und Hardware – und sie explizit zu halten ermöglicht, einen Löserfehler von einem Kalibrierfehler zu unterscheiden. Ein Arm, der sich weich bewegt und nirgends nahe dem Ziel landet, ist fast immer das, nicht die Mathematik.

Fehlersuche

Selbst ausprobieren

Im Roboterarmsimulator das Ziel herumziehen und dem Arm live beim Lösen zusehen. Den Ellbogenast umschalten, um die Alternativlösung zu sehen, das Ziel aus dem Ring schieben, um ein ehrliches „unerreichbar“ zu sehen, und zur Arbeitsraumkante fahren, um eine Singularität zu spüren.