CINEMÁTICA INVERSA PARA EL ROBOT DE CUATRO GRADOS DE LIBERTAD

Ing. Carlos H. Esparza

El siguiente trabajo muestra todo el procedimiento necesario para obtener la cinemática inversa del
robot que se muestra en la figura 1 propuesto como trabajo de curso.

Figura 1. Robot de 4 GDL propuesto.y

noname

1. En primera instancia se obtiene la matriz D-H que describe el comportamiento del robot, para eso
se dibuja la orientación de cada uno de los ejes sobre cada articulación como se muestra en la
figura 2.

Figura 2. Orientación de los ejes cartesianos para obtener la matriz D-H.

20

5 15

10
0
5

2. Se
-5 obtiene la matriz D-H que describe el comportamiento completo 0
del robot. Se debe tener en
cuenta que la posición en la que está dibujado el robot es la que corresponde a ángulos cero para
-10 -5
cada articulación, lo que conlleva a que la articulación 2 se mueva de 0° a 180°, y las
-10
articulaciones
-15 3 y 4 se muevan de -90° a 90°. Esto para servomotores de 180° de movimiento
X
claro está. La matriz que se obtiene es la-15que se muestra en la tabla 1.
-20
-20

Tabla 1. Matriz D-H para el robot de la figura 2.

Articulación Rot z Transl z Transl x Rot x
1 q1 L1 0 90°
2 q2 0 L2 0°
3 q3 0 L3 0°
4 q4 0 L4 0°
3. Ahora el problema inverso consiste en obtener el valor de las articulaciones para una posición
final dada, la cual se determina mediante métodos geométricos y desacople cinemático. Teniendo
en cuenta que la orientación final del brazo va a ser siempre paralela al plano XY, se puede
obtener un punto desacoplado con ubicación en la cuarta articulación de la siguiente manera.

Figura 3. Obtención del punto Pz’ a partir del punto Pz

El punto es el mismo punto gracias a que la orientación final es paralela al plano XY.

Figura 4. Obtención del punto (Px’, Py’) a partir del punto (Px,Py)

El punto Px’ y Py’ se puede obtener a partir del ángulo q1 que se obtiene directamente de los
puntos Px y Py originales.

Es muy importante tener en cuenta en que cuadrante está el vector que describe el ángulo q1, ya
que el resultado de la no lo diferencia, y por ejemplo un ángulo que está en el segundo
cuadrante lo coloca como si estuviera en el cuarto. De aquí la importancia de que las
articulaciones trabajen en el rango de -90° a +90°.

De nuevo, gracias a la simplificación hecha en la última articulación se pueden obtener los puntos
en x y en y mediante las ecuaciones siguientes.
4. Una vez calculado el punto P’ se pueden aplicar las ecuaciones geométricas que determinan los
ángulos q2 y q3. El ángulo q1 ya se halló antes.

5. Por último en la cinemática inversa del robot planteado se debe obtener el ángulo q4. Analizando
la figura 5 se tiene que la suma de los ángulos q2 q3 y q4 debe ser cero para que se cumpla la
condición que la orientación final siempre sea paralela al plano XY.

Figura 5. Obtención del ángulo q4

De esta manera el ángulo q4 es:

6. A continuación se copia una función en matlab que desarrolla la cinemática inversa del robot de 4
GDL y un programa que la aplica y comprueba los resultados al aplicar la cinemática directa.

FUNCIÓN

function [q1,q2,q3,q4]=cinem_inver4GDL(Px,Py,Pz,L1,L2,L3,L4)
% Esta función determinará la cinemática inversa
% del robot de cuatro grados de libertad propuesto
% para la asignatura
q1=atan(Py/Px);

Px1=Px-L4*cos(q1);
Py1=Py-L4*sin(q1);
r=sqrt(Px1^2+Py1^2);

PZ=Pz-L1;
cosq3=(Px1^2+Py1^2+PZ^2-L2^2-L3^2)/(2*L2*L3);
senq3=sqrt(1-cosq3^2);
q3=atan(senq3/cosq3);
Beta=atan(PZ/r);
Alfa=atan((L3*senq3)/(L2+L3*cosq3));
q2=Beta-Alfa;

q4=-(q2+q3);

q1=q1*180/pi;
q2=q2*180/pi;
q3=q3*180/pi;
q4=q4*180/pi;

PROGRAMA
% CINEMÁTICA DIRECTA
% METODO DENAVIT HARTENBERG D-H
% GRAFICA DEL ROBOT

clear all; close all; clc

L1=4; L2=7; L3=7; L4=4;

Px=8;
Py=9;
Pz=15;
P=[Px Py Pz]

[q1,q2,q3,q4]=cinem_inver4GDL(Px,Py,Pz,L1,L2,L3,L4)

% verificación de los ángulos que resultan de la cinemática inversa

disp('Verificación de los resultados')
q1=q1*pi/180;
q2=q2*pi/180;
q3=q3*pi/180;
q4=q4*pi/180;

A01=MDH(q1, L1, 0, pi/2);

A12=MDH(q2, 0, L2, 0);
A23=MDH(q3, 0, L3, 0);
A34=MDH(q4,0,L4,0);

T=A01*A12*A23*A34