Cinemática inversa de um braço robótico de dois elos

A cinemática direta é fácil: dados os ângulos das articulações, multiplique as transformações dos elos e você saberá onde está a ferramenta. A cinemática inversa faz o caminho oposto — dada uma pose-alvo, encontre os ângulos das articulações que a alcancam — e é nessa direção que estão os problemas interessantes. Um braço planar de dois elos é o menor exemplo que reúne todos eles. Explore-o no simulador de braço robótico.

O espaço de trabalho alcançável

Com comprimentos de elo L₁ e L₂, a ferramenta só alcança pontos dentro de uma região anular. O raio externo é L₁ + L₂, com o braço totalmente estendido, e o raio interno é |L₁ − L₂|, com o braço totalmente dobrado. Os pontos fora do anel estão longe demais; os pontos no interior do buraco estão perto demais. Nenhum conjunto de ângulos das articulações alcança esses pontos, portanto a resposta correta é simplesmente “inacessível”.

Duas soluções: cotovelo para cima e para baixo

Para a maioria dos alvos dentro do anel, o cotovelo pode dobrar em duas direções e ainda colocar a ferramenta no mesmo ponto. Essas são as configurações de cotovelo para cima e cotovelo para baixo. Elas resultam do sinal ± no ângulo calculado pela lei dos cossenos para a segunda articulação:

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

Como existem duas respostas válidas, a cinemática inversa não é uma única função, mas uma escolha. Os robôs selecionam a configuração que mantém as articulações longe dos limites, minimiza o movimento em relação à pose atual ou evita um obstáculo.

Singularidades

Nas bordas do espaço de trabalho — com o braço totalmente estendido ou dobrado — as duas soluções se fundem em uma. Esses pontos são singularidades; perto deles, um pequeno movimento da ferramenta pode exigir um movimento enorme das articulações. Os planejadores desaceleram ou contornam poses singulares porque o braço perde temporariamente a capacidade de mover a ferramenta em determinada direção.

Quando não há solução?

Um alvo não tem solução se estiver fora da região alcançável, se exigir um ângulo de articulação proibido pelo hardware ou se exigir uma orientação que as articulações disponíveis não conseguem produzir. Um bom simulador distingue esses casos com clareza, em vez de se ajustar à pose mais próxima e fingir que a solução foi encontrada.

Solucionadores analíticos versus numéricos

Para dois ou três elos, a geometria admite uma solução em forma fechada, ou analítica. Isso permite calcular diretamente os ângulos das articulações, com exatidão e rapidez, e torna a IK analítica ideal para ensino e testes. Braços redundantes ou totalmente tridimensionais normalmente não admitem solução em forma fechada e exigem solucionadores numéricos que convergem para uma solução. Esses solucionadores são mais gerais, mas mais lentos e difíceis de analisar, pois podem deixar de encontrar soluções ou parar próximos de singularidades.

O solucionador, escrito com segurança

As equações acima estão corretas, mas ainda produzirão NaN em hardware real. Esta é a mesma matemática com cada falha tratada explicitamente:

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

Cinco detalhes separam esta versão da versão do livro didático, e cada um corresponde a uma falha real:

Verificação do alcance antes da trigonometria. Se o alvo estiver além de L1 + L2, a lei dos cossenos produz um argumento fora do intervalo −1 a 1 e acos retorna NaN. Esse NaN chega ao comando do servo como um valor de ângulo inválido — e é por isso que um alvo inacessível tantas vezes provoca um movimento violento, em vez de uma recusa explícita.

A tolerância epsilon. Um alvo calculado para ficar exatamente no limite pode, após o arredondamento de ponto flutuante, produzir c2 = 1.0000001. Testar a condição de alcance com uma pequena margem captura esse caso antes que acos o faça.

atan2f, não atan. O atan comum recebe um único quociente e não distingue um alvo à frente de seu correspondente espelhado atrás. Por isso, o braço funciona em dois quadrantes e se comporta como uma reflexão nos outros dois. Este é o erro mais comum na primeira implementação de IK.

Um aviso de singularidade em vez de uma recusa. Perto da extensão máxima, a solução ainda é válida, mas está mal condicionada — um pequeno movimento da ferramenta exige um movimento enorme das articulações. Sinalizar essa condição permite que o código que solicitou a solução desacelere ou contorne a região.

Limites das articulações como status. Um braço cujo ombro não consegue ir abaixo de −10° tem alvos geometricamente alcançáveis, mas mecanicamente inalcançáveis. Limitar os valores silenciosamente faz o braço ir para algum ponto próximo do alvo e, ainda assim, informar sucesso.

Escolhendo entre as duas soluções

A matemática fornece os dois ramos e não pode dizer qual usar. Essa é uma decisão de robótica, e há quatro regras comuns:

O menor movimento é a opção padrão, e o motivo fica claro sem essa regra: se o solucionador muda de ramo silenciosamente no meio de uma trajetória, o braço executa uma virada grande, rápida e completamente não planejada. Em um braço real, isso é preocupante e pode causar danos.

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

Linhas retas exigem IK em cada passo

Interpolar entre duas poses no espaço das articulações — movendo cada articulação linearmente do ângulo inicial ao ângulo final — é simples, mas faz a ferramenta seguir uma curva. Para muitas tarefas, isso é suficiente. Para qualquer movimento que exija um deslocamento em linha reta, interpole no espaço cartesiano e resolva a IK em cada 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);
  }
}

Duas coisas ficam evidentes na primeira vez que você faz isso. A velocidade das articulações não é constante, embora a velocidade da ferramenta seja — próximo à borda do espaço de trabalho, as articulações precisam se mover muito mais para produzir o mesmo deslocamento da ferramenta. Além disso, uma linha reta pode atravessar uma região fora do alcance do braço, mesmo quando ambas as extremidades estão corretas. Por isso, é importante verificar o status dentro do laço, e não apenas nas extremidades.

Dos ângulos das articulações aos comandos do servo

O solucionador retorna ângulos das articulações em um referencial matemático. Um servo não conhece esse referencial: seu zero é definido pela posição de montagem do chapéu do servo, e ele pode girar no sentido oposto ao esperado.

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

Essa estrutura faz toda a ponte entre a cinemática e o hardware. Mantê-la explícita permite distinguir um erro do solucionador de um erro de calibração. Um braço que se move suavemente, mas termina longe do alvo, quase sempre está nesse caso, e não em uma falha da matemática.

Solução de problemas

Experimente você mesmo

No simulador de braço robótico, arraste o alvo pela área de trabalho e veja o braço resolver a cinemática inversa em tempo real. Alterne o ramo do cotovelo para observar a solução alternativa, leve o alvo para fora do anel para ver a indicação explícita de “inacessível” e aproxime o alvo da borda do espaço de trabalho para perceber uma singularidade.