Cinematica inversa di un braccio robotico a due segmenti

La cinematica diretta è semplice: dati gli angoli dei giunti, moltiplica le trasformazioni dei segmenti e sai dove si trova l’organo terminale. La cinematica inversa procede nell’altra direzione—dato un bersaglio, trova gli angoli dei giunti che lo raggiungono—ed è proprio lì che si trovano i problemi interessanti. Un braccio a due segmenti sul piano è il minimo esempio che li mostra tutti. Esploralo nel simulatore del braccio robotico.

Lo spazio di lavoro raggiungibile

Con lunghezze dei segmenti L₁ e L₂, l’organo terminale può raggiungere solo i punti dentro una corona circolare. Il raggio esterno è L₁ + L₂ (bracio completamente esteso) e quello interno è |L₁ − L₂| (bracio completamente ripiegato). Tutto ciò che è fuori dalla corona è troppo lontano; tutto ciò che è dentro il foro è troppo vicino. Nessuna combinazione di angoli dei giunti può raggiungere questi punti, quindi la risposta corretta è semplicemente «irraggiungibile».

Due soluzioni: gomito in alto e in basso

Per la maggior parte dei bersagli dentro la corona, il gomito può piegarsi in due modi e comunque posizionare l’organo terminale nello stesso punto. Sono le configurazioni con gomito in alto e gomito in basso. Derivano dal segno ± nell’angolo del teorema del coseno per il secondo giunto:

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

Poiché esistono due risposte valide, la cinematica inversa non è una singola funzione: è una scelta. I robot scelgono la configurazione che mantiene i giunti lontani dai limiti, riduce il movimento dalla posa corrente o evita un ostacolo.

Singolarità

Ai bordi dello spazio di lavoro—bracio completamente esteso o completamente ripiegato—le due soluzioni si fondono in una. Sono singolarità; in prossimità di esse, un piccolo movimento dell’organo terminale può richiedere un enorme movimento dei giunti. I pianificatori rallentano o aggirano le pose singolari perché il braccio perde temporaneamente la capacità di muovere l’organo terminale in una direzione.

Quando non esiste una soluzione?

Un bersaglio non ha soluzioni quando cade fuori dalla corona raggiungibile, quando richiederebbe un angolo dei giunti vietato dall’hardware o quando imporrebbe un orientamento che i giunti disponibili non possono produrre. Un buon simulatore distingue onestamente questi casi invece di scattare alla posa più vicina e fingere di aver avuto successo.

Solutori analitici e numerici

Per due o tre segmenti la geometria ha una soluzione in forma chiusa (analitica), quindi calcoli direttamente, esattamente e rapidamente gli angoli dei giunti. Questo rende la cinematica inversa analitica ideale per l’insegnamento e i test. I bracci ridondanti o completamente spaziali raramente hanno una forma chiusa e richiedono solutori numerici che iterano verso una soluzione—più generali, ma più lenti e difficili da valutare, perché possono perdere soluzioni o bloccarsi vicino alle singolarità.

Il solutore, scritto in modo sicuro

Le equazioni precedenti sono corrette e su hardware reale produrranno comunque NaN. Ecco la stessa matematica con ogni errore gestito esplicitamente:

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

Cinque dettagli distinguono questa versione da quella dei libri di testo, e ciascuno corrisponde a un guasto reale:

Raggiungibilità prima della trigonometria. Se il bersaglio supera L1 + L2, il teorema del coseno produce un argomento esterno all’intervallo da −1 a 1 e acos restituisce NaN. Questo NaN finisce poi nel comando del servomotore come angolo spazzatura: è il motivo per cui un bersaglio irraggiungibile produce spesso un movimento violento anziché il rifiuto di muoversi.

L’epsilon. Un bersaglio calcolato in modo da trovarsi esattamente sul bordo può, dopo l’arrotondamento in virgola mobile, dare c2 = 1.0000001. Verificare la raggiungibilità con un piccolo margine intercetta il problema prima di acos.

atan2f, non atan. Il semplice atan accetta un solo quoziente e non distingue un bersaglio davanti dal suo specchio dietro, quindi il braccio funziona in due quadranti e si rispecchia negli altri due. È il bug più comune nella prima implementazione della cinematica inversa.

Un avviso di singolarità anziché un rifiuto. In prossimità dell’estensione completa la soluzione è ancora valida, ma mal condizionata: un piccolo movimento dell’organo terminale richiede un movimento enorme dei giunti. Segnalarlo permette al chiamante di rallentare o aggirare la zona.

I limiti dei giunti come stato. Un braccio la cui spalla non può scendere sotto −10° ha bersagli geometricamente raggiungibili ma meccanicamente irraggiungibili. Limitarli silenziosamente produce un braccio che si ferma vicino al bersaglio e segnala comunque il successo.

Scegliere tra le due soluzioni

La matematica ti dà entrambi i rami e non può indicarti quale usare. È una decisione robotica, e ci sono quattro regole comuni:

Il movimento minimo è l’opzione predefinita corretta, e il motivo è ciò che accade senza di essa: se il solutore cambia ramo silenziosamente a metà percorso, il braccio compie una deviazione ampia, rapida e completamente non pianificata. Su un braccio reale è allarmante e può essere distruttivo.

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

Le rette richiedono la cinematica inversa a ogni passo

L’interpolazione tra due pose nello spazio dei giunti — lo spostamento lineare di ogni giunto dal suo angolo iniziale a quello finale — è semplice e fa seguire all’organo terminale una curva. Per molte attività va bene. Per tutto ciò che deve percorrere una retta, interpola nello spazio cartesiano e risolvi la cinematica inversa a ogni passo:

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

La prima volta che fai questo emergono due fatti. La velocità dei giunti non è costante anche se la velocità dell’organo terminale lo è: vicino al bordo dello spazio di lavoro i giunti devono muoversi molto più velocemente per lo stesso movimento dell’organo terminale. E una retta può attraversare una zona che il braccio non può raggiungere anche quando entrambe le estremità sono valide; ecco perché è importante controllare lo stato all’interno del ciclo, non solo alle estremità.

Dagli angoli dei giunti ai conteggi del servomotore

Il solutore restituisce gli angoli dei giunti in un sistema matematico. Un servomotore non sa nulla di quel sistema: il suo zero dipende da dove è stata montata l’aletta e potrebbe ruotare nel verso opposto.

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

Quella struttura è l’intero ponte fra la cinematica e l’hardware; mantenerla esplicita ti permette di distinguere un bug del solutore da uno di calibrazione. Un braccio che si muove in modo fluido ma si ferma lontano dal bersaglio è quasi sempre un problema di questo tipo, non della matematica.

Risoluzione dei problemi

Provalo tu stesso

Nel simulatore del braccio robotico, trascina il bersaglio e osserva il braccio che lo risolve in tempo reale. Cambia il ramo del gomito per vedere la soluzione alternativa, sposta il bersaglio fuori dalla corona per vedere un onesto «irraggiungibile» e avvicinati al bordo dello spazio di lavoro per sentire una singolarità.