UNIVERSIDAD NACIONAL DE INGENIERÍA

FACULTAD DE INGENIERÍA MECÁNICA

ANÁLISIS Y CONTROL DE ROBOTS (MT1517)

CINEMÁTICA DE UN MANIPULADOR DE
2GDL

PROFESOR:
-Ing. CALLE FLORES, Iván Arturo
ALUMNOS:
-CASTILLO ALVARADO, David 20140012F
-MUÑOZ RUIZ, Moisés 20140119E
-SALVADOR CARRILLO, Martín 20140295H
SECCIÓN:
-“A”

2017
Universidad Nacional de Ingeniería Analisis y control de Robots(MT517)
Facultad de Ingeniería Mecánica

CINEMÁTICA DE UN MANIPULADOR DE 2GDL

En el presente informe se mostrara como realizar la cinemática de un
manipulador de 2 grados de libertar (GDL) de la siguiente forma:

Figura 1. Manipulador de 2GDL a trabajar

1. CINEMÁTICA DIRECTA

1.1. Ubicación de ejes

Para trabajar de una manera más cómoda con nuestro manipulador
trabajaremos con la versión simplificada.

Figura 2. Forma simplificada del manipulador

Los ejes se ubicaran de tal manera que facilite el cálculo de los

parámetros de Denavit-Hartenberg.

2
Universidad Nacional de Ingeniería Analisis y control de Robots(MT517)
Facultad de Ingeniería Mecánica

Figura 2. Manipulador con ejes ubicados

Figura 3. Simulación con ejes ubicados

Figura 4. Manipulador con parámetros ubicados

3
 Universidad Nacional de Ingeniería Analisis y control de Robots(MT517)
Facultad de Ingeniería Mecánica

Calculamos los parámetros D-H

Tabla 1. Parámetros D-H

Θi d1 ai αi

1 Θ1 L1 L2 π/2

2 Θ2 -L3 L4 0

Calculamos las matrices D-H, para eso recordamos los siguientes conceptos

Cos (1 ) 0 Sin(1 ) L2Cos(1 ) 

 Sin( ) 0 Cos(1 ) L 2Sin(1 ) 
A10   1

 0 1 0 L1 
 
 0 0 0 1 

Cos ( 2 )  Sin( 2 ) 0 L 4 Cos( 2 ) 

 Sin( ) Cos( ) 0 L 4 Sin( 2 ) 
A2  
1 2 2

 0 0 1  L3 
 
 0 0 0 1 

Cos (1 )Cos( 2 ) Cos(1 ) Sin( 2 ) Sin(1 ) L2Sin(1 )  L3Sin(1 )  L4 Cos(1 )Cos( 2 ) 
 Sin( )Cos( )  Sin( ) Sin( ) Cos( ) L2 Sin( )  L3Cos( )  L4 Sin( )Cos( ) 
T20  A10 A21   1 2 1 2 2 1 1 2 

 Sin( 2 ) Cos( 2 ) 0 L 4Sin( 2 )  L1 

 
 0 0 0 1 
4
Universidad Nacional de Ingeniería Analisis y control de Robots(MT517)
Facultad de Ingeniería Mecánica

2. CINEMÁTICA INVERSA

Figura 5. Parametros para hallar la cinematica inversa

Figura 6. Vista del plano XY para calcular parámetros

    1
  a tan 2( x, y)
  a tan 2( ( x)2  ( y)2  L23 , L3 )
  a tan 2( x, y)  a tan 2( ( x) 2  ( y) 2  L23 , L3 )

5
 Universidad Nacional de Ingeniería Analisis y control de Robots(MT517)
Facultad de Ingeniería Mecánica

Z  L3  L4Sin(2 )
 Z  L1 
 2  A sin   ; Z  L1  L 4
 L 4 

Figura 7. Vista del plano XZ para calcular parámetros

3. PROGRAMACIÓN CINEMÁTICA DIRECTA

clc
%1. definicion de vairables simbolicas
syms theta1 alpha1
syms theta2 alpha2
syms L1 L2 L3 L4
syms q1 q2
%%2. definicion de sustemas ciirdenads
%%2.1. Sistemas coordenados1

theta1=q1;
%%L1 = 90.95;
%%L2 =28;
alpha1= pi/2;
A1=matriz_homogenea_DH(q1, L1,L2, alpha1);
A1
%%%2.2. Sistemas coordenados2

theta2=q2;
%%L3 = 20.03;
%%L4 = 136;
alpha2= 0;
A2=matriz_homogenea_DH(theta2,-L3,L4,alpha2);
A2
%%3. Cinematica
T1=A1;
T2=simplify(A1*A2);
disp('Matriz T2')
disp(T2)

6
Universidad Nacional de Ingeniería Analisis y control de Robots(MT517)
Facultad de Ingeniería Mecánica

3.1 Resultados

4. PROGRAMACIÓN DE LA CINEMÁTICA INVERSA

function q = cinematica_inversa(this, xc, yc, zc)

%Metodo para la cinematica inversa
L1=this.LL1;
L2=this.LL2;
L3=abs(this.LL3);
L4=this.LL4;

%Angulo Theta1
if(zc>(L1+L4))
error('POSICION IMPOSIBLE DE ALCANZAR z muy alto')
end
q2_star=asin((zc-L1)/L4);
%Angulo theta2
r2=xc^2+yc^2;
%%r=sqrt(r2);
k2=(L4*cos(q2_star))^2+L3^2;
k=sqrt(k2);
num=r2-L2^2-k2;
den=2*L2*k;
D=num/den;
if(abs(D)>1)
error('POSICION IMPOSIBLE DE ALCANZAR por X o Y ')
end

q1_star=atan2(yc,xc)-atan2(L3,(r2-L3^2)^0.5);

%Vector de Angulos
q=[q1_star q2_star];
end

7
Universidad Nacional de Ingeniería Analisis y control de Robots(MT517)
Facultad de Ingeniería Mecánica

5. PROGRAMACIÓN MIXTA, CINEMÁTICA INVERSA Y DIRECTA

%-------------------
% 1. Configuracion inicial
clc; clear all; close all;
xc=4.02812;
yc=5.26573;
zc=22.23159;

%1.2. Ploteo del entorno

T0=eye(4);
plot_frame(T0,'color','k','view','auto')
axis([-15 15 -15 15 0 25])
hold on
grid on
xlabel('x(m)')
ylabel('y(m)')
%Ploteo del punto a alcanzar
plot3(xc, yc, zc, 'ro','MarkerFaceColor', 'k', 'MarkerSize', 10)

% 2. Parametros Mecanicos del Robot

L1=9.095;
L2=2.8;
L3=-2.003;
L4=13.6;

my_robot = robot_RRR();
my_robot.set_dimensiones(L1, L2, L3, L4)

% 3. Cnematica Inversa
%3.1. USAMOS EL METODO "CInematica inversa"
q = my_robot.cinematica_inversa(xc, yc, zc);
q1d = q(1);
q2d = q(2);
%3.2. SALIDAs
disp('angulos')
disp(q)
disp(q*180/pi)

%---------------------------------------------
%4. Configuracion Incial del Robot

%4.1. Eslabor 1
% -> Hallamos A1
theta1=0;
A1 = my_robot.compute_matrix_A1(theta1);
% -> Ploteamos T101
T01 =A1;
h1 = plot_frame(T01, 'frame', '1', 'color', 'b');
disp(T01)

%4.2. Eslabon 2
theta2 = 0;
A2 = my_robot.compute_matrix_A2(theta2);
T02 = T01*A2;
% .> Ploteamso T02
h2 = plot_frame(T02, 'frame', '2', 'color', 'r');
disp(T02)

8
Universidad Nacional de Ingeniería Analisis y control de Robots(MT517)
Facultad de Ingeniería Mecánica

%---------------------------------------------------------------
% 5. Lazo de simulacion
% 5.1. Mensaje

disp('Robot del tipo RR de 2 GDL')

disp('Presione una tecla para empezar simulacion')
pause

% 5.2. Definimos los angulos

STEPS = 400;
THETA1 = linspace(theta1, q1d, STEPS);
THETA2 = linspace(theta2, q2d, STEPS);

% 5.3. LAZO PRINCIPAL

for i=1:STEPS

%A. OBTENEMOS LOS ANGULOS

theta1 = THETA1(i)
theta2 = THETA2(i)

%B. Matrices de Transformacion Homoghenea

A1 = my_robot.compute_matrix_A1(theta1);
A2 = my_robot.compute_matrix_A2(theta2);

T01 = A1;
T02 = A1*A2;

%C. Ploteamos los sistemas coordenados

plot_frame(h1, T01);
plot_frame(h2, T02);

% D. Delta de Tiempo
pause(0.001)
end

5.1 Resultados

9
Universidad Nacional de Ingeniería Analisis y control de Robots(MT517)
Facultad de Ingeniería Mecánica

....

Figura 8. Resultado de programa, fase inicial

10
Universidad Nacional de Ingeniería Analisis y control de Robots(MT517)
Facultad de Ingeniería Mecánica

Figura 9. Resultado de programa, fase intermedia

Figura 10. Resultado de programa, fase final

11
Universidad Nacional de Ingeniería Analisis y control de Robots(MT517)
Facultad de Ingeniería Mecánica

Materiales del robot a implementar:

 Juego de poleas

 Caja de engranajes

12
Universidad Nacional de Ingeniería Analisis y control de Robots(MT517)
Facultad de Ingeniería Mecánica

 Motor 1

 Motor2

 Engranajes

13
Universidad Nacional de Ingeniería Analisis y control de Robots(MT517)
Facultad de Ingeniería Mecánica

 Engranaje1

 Engranaje 2

 Engranaje 3

14
Universidad Nacional de Ingeniería Analisis y control de Robots(MT517)
Facultad de Ingeniería Mecánica

 Engranaje 4

 Poleas

15
Universidad Nacional de Ingeniería Analisis y control de Robots(MT517)
Facultad de Ingeniería Mecánica

 Enconder

16