Cinemática inversa de un robot de seis ejes en forma cerrada

El problema, dicho sin adornos

La cinemática directa es cómoda: dados los seis ángulos de articulación, se componen seis matrices homogéneas y sale la pose del efector. Una multiplicación, ninguna ambigüedad, siempre hay respuesta.

La inversa es la que sirve para trabajar y la que da problemas. El operador o el CAD entregan un punto en el espacio y una orientación de herramienta; hay que devolver los seis ángulos que llevan ahí. Es un sistema de ecuaciones trigonométricas acopladas, puede no tener solución —el punto está fuera del alcance—, o tener varias.

Por qué el robot está diseñado para que esto se pueda resolver

No todo manipulador de seis ejes tiene solución analítica. La tiene si cumple la condición de Pieper: tres ejes consecutivos que se cortan en un punto, o tres ejes paralelos. La primera variante —la muñeca esférica— es la que montan casi todos los robots industriales.

Eso no es casualidad ni herencia mecánica: es una decisión de diseño para que el controlador pueda resolver la inversa en tiempo acotado, miles de veces por minuto, sin iterar. Cuando alguien construye un brazo «a medida» sin respetar esa condición, se encuentra con que hay que meter un solver numérico en el lazo y con que el tiempo de ciclo deja de ser predecible.

La tabla DH del robot

Todo arranca de la tabla Denavit-Hartenberg, que resume la geometría del brazo en cuatro números por eslabón. Esta es la de un KUKA KR6, en convención modificada (Craig), donde cada fila lleva el ángulo y la distancia del eslabón anterior más la traslación y el giro del eje propio:

Los cinco números que van a aparecer en todas las ecuaciones son esos: 260, 680, −35, 670 y 115 mm. El −35 no es un error de signo, es un desplazamiento real del eslabón; y el 115 es la distancia del centro de muñeca al punto de trabajo de la herramienta, que es la que permite el primer paso.

Paso 1 — desacoplar la muñeca

El centro de muñeca es el punto donde se cortan los ejes 4, 5 y 6. Girar la muñeca no lo mueve. Por tanto su posición depende sólo de θ₁, θ₂ y θ₃: retrocediendo d₆ desde el TCP a lo largo del eje de la herramienta se obtiene un problema de tres incógnitas en lugar de seis.

% El centro de muñeca: se retrocede d6 = 115 mm desde el TCP
P04 = T06 * [0; 0; -115; 1];
 
% Con la muñeca fuera del problema, theta1 es inmediato
th1 = atan2(P04(2), P04(1));
 
% La otra rama: el robot llega al mismo punto girando 180°
% de base y plegándose hacia atrás
th1_alt = atan2(-P04(2), -P04(1));

Ese atan2 ya entrega dos soluciones válidas. La segunda corresponde a la pose «hombro atrás»: el robot gira media vuelta de base y alcanza el mismo punto plegado hacia el otro lado. Es perfectamente válida en el papel y frecuentemente imposible en planta, porque ahí está la columna, la valla o el operador.

Paso 2 — el codo sale de una ecuación de segundo grado

Con θ₁ conocido, el problema se reduce al plano que contiene el brazo. Ahí, la distancia del hombro al centro de muñeca es un dato geométrico fijo, y de imponerla sale una ecuación cuadrática en el coseno de θ₃:

% Se impone la distancia hombro-muñeca y queda A·cos²+B·cos+C = 0
K    = (1/1360)*((P04(1)/cos(th1)-260)^2 + P04(3)^2 ...
                 - 35^2 - 670^2 - 680^2);
A    = (670^2 + 35^2)/K^2;
B    = -2*35/K;
C    = 1 - (670/K)^2;
 
% Las dos raíces son las dos poses del codo
cos3 = (-B + sqrt(B^2 - 4*A*C))/(2*A);   % codo arriba
sin3 = -K/670 - 35*cos3/670;
th3  = atan2(sin3, cos3);
 
% Con theta3 fijo, theta2 sale de un sistema lineal 2x2
sol2 = A2 \ b2;
th2  = atan2(sol2(1), sol2(2));

El discriminante hace de comprobación de alcance sin trabajo extra: si B² − 4AC sale negativo, no existe combinación de ángulos que llegue a ese punto. No hace falta un chequeo aparte del volumen de trabajo — la propia ecuación lo dice.

Las dos raíces son las dos poses clásicas: codo arriba y codo abajo. Y θ₂ ya no necesita trigonometría inversa a ciegas: con θ₃ fijo el sistema queda lineal, dos ecuaciones y dos incógnitas, y de ahí salen seno y coseno por separado. Eso importa: recuperar el ángulo con atan2 de seno y coseno, y no con un acos, conserva el cuadrante.

Paso 3 — la orientación, leída de la matriz

Los tres primeros ángulos fijan dónde está la muñeca. Lo que falta es cómo está girada la herramienta. Conocidos θ₁, θ₂ y θ₃ se puede componer la rotación de la base al eslabón 3 y despejar lo que queda:

R³₆ = (R⁰₃)ᵀ · R⁰₆

De los nueve elementos de esa submatriz salen directamente θ₄, θ₅ y θ₆, cada uno con un atan2 sobre dos componentes. Y aparece la segunda familia de soluciones dobles: para cada θ₅ positivo hay un θ₅ negativo con θ₄ y θ₆ girados 180°, que deja la herramienta exactamente igual orientada.

Ocho soluciones: cuál se elige

Multiplicando las tres ambigüedades —base, codo y muñeca— salen hasta ocho configuraciones que llevan la herramienta exactamente a la misma pose. La cinemática no elige: elige el ingeniero, y por criterios que no están en las ecuaciones.

1. Continuidad. Entre dos puntos consecutivos de una trayectoria hay que quedarse en la misma rama. Saltar de codo arriba a codo abajo a mitad de camino significa que el robot se reconfigura entero: movimiento amplio, lento y a menudo hacia donde no hay sitio.
2. Límites articulares. Una solución matemáticamente correcta puede pedir 400° a un eje que sólo gira 350°.
3. Distancia a singularidades. Entre dos ramas válidas se prefiere la que deja más margen antes de que la muñeca se alinee.
4. Entorno. La celda tiene vallas, mesas y otros equipos. Media de las soluciones suele estar ocupada por hierro.

Por eso el planificador no puede ser sólo una función que devuelve ángulos: tiene que devolver ángulos y quedarse con la rama elegida para el punto siguiente.

Verificar sobre el sólido, no sobre el papel

Una inversa correcta puede seguir dando un programa inviable. La comprobación útil es resolver una trayectoria completa punto a punto, exportar los seis ángulos como series de tiempo y meterlos en un modelo multicuerpo con la geometría real del robot:

% Hélice de prueba: circunferencia de 100 mm que sube de 400 a 900
I = linspace(0, 2*pi, 100);
x = 100*cos(I);
y = 100*sin(I);
z = linspace(400, 900, length(I));
 
for i = 1:length(I)
   [th1(i), th2(i), th3(i), th4(i), th5(i), th6(i)] = ...
        invKuka1(x(i), y(i), z(i));
end
 
% Al modelo multicuerpo como series de tiempo
tth1 = timeseries(th1, linspace(1, 10, length(I)));

Con los sólidos importados en STEP, el modelo responde a las preguntas que de verdad cuestan dinero: ¿el brazo llega a todos los puntos?, ¿choca con el utillaje?, ¿cambia de configuración a mitad de trayectoria?, ¿cuánto tarda el ciclo? Todo eso antes de que el robot exista en la planta.