Cinematica Inversa

Robótica y Visión por Computador - UMH

Práctica

2

Cinemática inversa del robot 4 gdl
Ejemplo Solución del robot cilíndrico de 4 grados de libertad

En este caso particular, la solución geométrica es inmediata. Se parte de que la posición del extremo del robot es conocida (px, py, pz) y se va a calcular los valores de las coordenadas articulares. Articulación 1 Para obtener el valor deθ1 (TETA1 en el código de Matlab®.) se proyecta el punto del extremo del robot (px, py, pz) sobre el plano (x0, y0, z0) obteniendo una sencilla relación angular. Sabiendo que θ1 es el ángulo entre x0 y x1, se obtienen las siguientes gráficas.

px, py y0 d3+l4 R r β px,py,pz orientación a2 θ1 x0 z0 φ

x0

y0

Figura-2.5 Cinemática inversa del robot de 4 gdl.

De las que se deducen lassiguientes relaciones:

Práctica 2 .- Pág. 1

Robótica y Visión por Computador. UMH

R=

2 px + p2 y

2 r = R 2 − a2 = d 3 + l4 py sinφ = cos φ = R a cos β = sinβ = 2 R

px R r R

utilizando la función atan2 de Matlab® se calculan los valores de los ángulos:

φ = atan 2( sinφ , cos φ ) β = atan2( sinβ , cos β )
que permiten el cálculo de θ1 como φ-β. Articulación 2 De la figura 2.3se obtiene la siguiente fórmula:
l1 + d 2 = p z ⇒ d 2 = p z − l1

Articulación 3 De la figura 2.5:
2 r = R 2 − a2 = d 3 + l4

Articulación 4 Para calcular la última articulación se necesita el cálculo previo del sistema de referencia (x3, y3, z3), que se resolverá mediante la cinemática directa explicada en el ejemplo 2.3. Dado que el vector a de aproximación es necesariamente paralelo a z4se deben cumplir las siguientes relaciones:
y3 x4 y4 θ4 x3

sinθ 4 = n·y 3 cosθ 4 = s· y 3

θ 4 = atan 2( sinθ 4 , cosθ 4 )

donde n y s son vectores de orientación del extremo del robot.

Práctica 2 .- Pág. 2

Robótica y Visión por Computador - UMH

Código en Matlab®. La función INVERSEKINEMATIC4 resuelve la cinemática inversa del robot cilíndrico de 4 gdl. Para ello toma comoparámetros la matriz homogénea T, que representa la orientación y posición del extremo del robot y devuelve el vector de coordenadas articulares.
% % % % % % Q = INVERSEKINEMATIC4(T) devuelve el vector de coordenadas articulares correspondiente a la solución cinemática inversa de la mano del manipulador en la posición y orientación expresadas en la matriz T. See also DIRECTKINEMATIC4, DENAVIT.function q = inversekinematic4(T) p = T(1:3,4); % Posición de la mano del manipulador

% Inicialización de las variables articulares a calcular q1 = 0; q2 = 0; q3 = 0; q4 = 0; % Parámetros Denavit-Hartenberg del robot teta = [q1 0 0 q4 ]; d = [0.4 q2 q3 0.2]; a = [0 -0.1 0 0 ]; alfa = [0 -pi/2 0 0 ]; % Solución de la primera articulación: q1 R = sqrt(p(1)^2+p(2)^2); r = sqrt(R^2-a(2)^2); sphi =-p(1)/R; cphi = p(2)/R; phi = atan2(sphi, cphi); sbeta = -a(2)/R; cbeta = r/R; beta = atan2(sbeta, cbeta); q1 = phi - beta; % Solución de la segunda articulación: q2 q2 = p(3) - d(1); % Solución de la tercera articulación: q3 q3 = r - d(4); % Solución de la cuarta articulación: q4 % Cálculo de la matriz de transformación A03 A01 = denavit(q1, d(1), a(1), alfa(1)); A12 = denavit(teta(2), q2, a(2),alfa(2)); A23 = denavit(teta(3), q3, a(3), alfa(3)); A03 = A01 * A12 * A23; y3 = A03(1:3,2);

Práctica 2 .- Pág. 3

Robótica y Visión por Computador. UMH

sq4 = dot(T(1:3,1), y3); cq4 = dot(T(1:3,2), y3); q4 = atan2(sq4, cq4);

% Vector orientación n: T(1:3,1) % Vector orientación s: T(1:3,2)

% Vector de variables articulares q = [q1 q2 q3 q4]';

⇒ Se observa como la cinemática directaestá incluida en los cálculos necesarios para obtener la matriz 0A3. En el ejemplo mostrado a continuación se puede comprobar como después de asignar un vector de coordenadas articulares aleatorio, y obtener la matriz homogénea del extremo de robot correspondiente a este vector, si sobre esta matriz se aplica la función INVERSEKINEMATIC4 se obtiene el vector q original.
» q=rand(4,1) q = 0.8913...