FACULTAD DE INGENIERÍA MECANICA

CONTROL DE ROBOT MANIPULADOR DE 2GDL

INFORME 2

Profesor: MSc. Calle Flores, Iván Arturo

Curso: Análisis y Control de Robots Sección: A

Alumnos:

Trucios Ruiz, Luis Enrique 20152053D

Lima, 05 de octubre del 2018

Análisis y Control de Robots
1ER INFORME

Rincón Virhuez, Angélica Mercedes 20152032G

Soel Huamaní, Juan Diego 20152003G

INDICE
1. CINEMÁTICA DIFERENCIAL ................................................................................................. 3
A. PARAMETROS DENAVIT – HATENBERG ......................................................................... 3
B. JACOBIANOS DEL EFECTOR FINAL................................................................................. 6
C. JACOBIANOS CENTRO DE MASA DE ESLABONES ...................................................... 10
D. JACOBIANOS CENTRO DE MASA MOTORES .............................................................. 17
2. DINÁMICA........................................................................................................................... 23
A. DINÁMICA SIN CONSIDERAR MOTORES .........................Error! Bookmark not defined.
B. DINÁMICA CONSIDERANDO MOTORES..........................Error! Bookmark not defined.
C. SIMULACIÓN SIN CONSIDERAR MOTORES .....................Error! Bookmark not defined.
D. SIMULACIÓN CONSIDERANDO MOTORES .....................Error! Bookmark not defined.

~2~
Análisis y Control de Robots
1ER INFORME

1. CINEMÁTICA DIFERENCIAL
A. PARAMETROS DENAVIT – HATENBERG

Según la teoría de Denavit – Hartenberg

Link 𝜽𝒊 𝒅𝒊 𝒂𝟏 𝜶𝟏

1 𝜃1 𝑑1 0 𝜋/2

2 𝜃2 𝑑2 𝑎2 0

Reemplazando. -

Link 𝜽𝒊 𝒅𝒊 𝒂𝟏 𝜶𝟏

1 𝜃1 14.4 cm 0 𝜋/2

2 𝜃2 6.8 𝑐𝑚 3.6 cm 0

~3~
Análisis y Control de Robots
1ER INFORME

Calculo de la matriz de transformación: 𝐴10

cos(𝜃1 ) 0 sin(𝜃1 ) 0
sin(𝜃1 ) 0 −cos(𝜃1 ) 0]
𝐴10 = [
0 1 1 𝑑1
0 0 0 1

cos(𝜃1 ) 0 sin(𝜃1 )
𝑅10 = [ sin(𝜃1 ) 0 −cos(𝜃1 )]
0 1 1
0 0
𝑂10 = [ 0 ] = [ 0 ]
𝑑1 14.4
sin(𝜃1 )
𝑍10 = [−cos(𝜃1 )]
1

Calculo de la matriz de transformación: 𝐴12

cos(𝜃2 ) −sin(𝜃2 ) 0 𝑎2 ∗ cos(𝜃2 )

sin(𝜃2 ) cos(𝜃2 ) 0 𝑎2 ∗ sin(𝜃2 )
𝐴12 = [ ]
0 1 1 𝑑2
0 0 0 1

Calculo de la matriz de transformación respecto al origen

𝑇20 = 𝐴10 ∗ 𝐴12

cos(𝜃1 ) ∗ cos(𝜃2 ) −cos(𝜃1 ) ∗ sin(𝜃2 ) sin(𝜃1 ) 𝑎2 ∗ cos(𝜃1 ) ∗ cos(𝜃2 ) + 𝑑2 ∗ sin(𝜃1 )

sin(𝜃1 ) ∗ cos(𝜃2 ) −sin(𝜃1 ) ∗ sin(𝜃2 ) −cos(𝜃1 ) 𝑎2 ∗ sin(𝜃1 ) ∗ cos(𝜃2 ) − 𝑑2 ∗ cos(𝜃1 )
[ ]
sin(𝜃2 ) cos(𝜃2 ) 0 𝑎2 ∗ sin(𝜃2 ) + 𝑑1
0 0 0 1
cos(𝜃1 ) ∗ cos(𝜃2 ) −cos(𝜃1 ) ∗ sin(𝜃2 ) sin(𝜃1 )
𝑅20 = [ sin(𝜃1 ) ∗ cos(𝜃2 ) −sin(𝜃1 ) ∗ sin(𝜃2 ) −cos(𝜃1 )]
sin(𝜃2 ) cos(𝜃2 ) 0

3.6 ∗ cos(𝜃1 ) ∗ cos(𝜃2 ) + 6.8 ∗ sin(𝜃1 )

𝑂20 = [3.6 ∗ sin(𝜃1 ) ∗ cos(𝜃2 ) − 6.8 ∗ cos(𝜃1 )]
3.6 ∗ sin(𝜃2 ) + 14.4

sin(𝜃1 )
𝑍20 = [−cos(𝜃1 )]
1

~4~
Análisis y Control de Robots
1ER INFORME

~5~
Análisis y Control de Robots
1ER INFORME

B. JACOBIANOS DEL EFECTOR FINAL

Calculo del Jacobiano Traslacional (𝑱𝒗𝟏 , 𝑱𝒗𝟐 )

𝑱𝒗𝟏 = 𝒁𝟎𝟎 ∗ (𝑶𝟎𝟐 − 𝑶𝟎𝟎 )

0 𝑎2 ∗ cos(𝜃1 ) ∗ cos(𝜃2 ) + 𝑑2 ∗ sin(𝜃1 ) 0

𝑱𝒗𝟏 = [0] ∗ ([𝑎2 ∗ sin(𝜃1 ) ∗ cos(𝜃2 ) − 𝑑2 ∗ cos(𝜃1 )] − [0])
1 𝑎2 ∗ sin(𝜃2 ) + 𝑑1 0

−𝑎2 ∗ sin(𝜃1 ) ∗ cos(𝜃2 ) + 𝑑2 ∗ cos(𝜃1 )

𝑱𝒗𝟏 = [ 𝑎2 ∗ cos(𝜃1 ) ∗ cos(𝜃2 ) + 𝑑2 ∗ sin(𝜃1 ) ]
0

−3.6 ∗ sin(𝜃1 ) ∗ cos(𝜃2 ) + 6.8 ∗ cos(𝜃1 )

𝑱𝒗𝟏 = [ 3.6 ∗ cos(𝜃1 ) ∗ cos(𝜃2 ) + 6.8 ∗ sin(𝜃1 ) ]
0

𝑱𝒗𝟐 = 𝒁𝟎𝟏 ∗ (𝑶𝟎𝟐 − 𝑶𝟎𝟏 )

sin(𝜃1 ) 𝑎2 ∗ cos(𝜃1 ) ∗ cos(𝜃2 ) + 𝑑2 ∗ sin(𝜃1 ) 0

𝑱𝒗𝟐 = [− cos(𝜃1 )] ∗ ([𝑎2 ∗ sin(𝜃1 ) ∗ cos(𝜃2 ) − 𝑑2 ∗ cos(𝜃1 )] − [ 0 ])
1 𝑎2 ∗ sin(𝜃2 ) + 𝑑1 𝑑1

−𝑎2 ∗ sin(𝜃2 ) ∗ cos(𝜃1 )

𝑱𝒗𝟐 = [ −𝑎2 ∗ sin(𝜃1 ) ∗ sin(𝜃2 ) ]
𝑎2 ∗ cos(𝜃2 )

−3.6 ∗ sin(𝜃2 ) ∗ cos(𝜃1 )

𝑱𝒗𝟐 = [ −3.6 ∗ sin(𝜃1 ) ∗ sin(𝜃2 ) ]
3.6 ∗ cos(𝜃2 )

𝑱𝒗 = [𝑱𝒗𝟏 , 𝑱𝒗𝟐 ]

−3.6 ∗ sin(𝜃1 ) ∗ cos(𝜃2 ) + 6.8 ∗ cos(𝜃1 ) −3.6 ∗ sin(𝜃2 ) ∗ cos(𝜃1 )

𝑱𝒗 = [ 3.6 ∗ cos(𝜃1 ) ∗ cos(𝜃2 ) + 6.8 ∗ sin(𝜃1 ) −3.6 ∗ sin(𝜃1 ) ∗ sin(𝜃2 ) ]
0 3.6 ∗ cos(𝜃2 )

Calculo del Jacobiano Rotacional (𝑱𝒘𝟏 , 𝑱𝒘𝟐 )

0
𝑱𝒘𝟏 = 𝒁𝟎𝟎 = [0]
1

sin(𝜃1 )
𝑱𝒘𝟐 = 𝒁𝟎𝟏 = [− cos(𝜃1 )]
1

𝑱𝑤 = [𝑱𝒘𝟏 , 𝑱𝒘𝟐 ]

~6~
Análisis y Control de Robots
1ER INFORME

0 sin(𝜃1 )
𝑱𝒘 = [0 − cos(𝜃1 )]
1 1

CM2

CM1

~7~
Análisis y Control de Robots
1ER INFORME

~8~
Análisis y Control de Robots
1ER INFORME

SIMULACIÓN EN MATLAB
% Tema: Jacobiano de un manipulador de 2GDL en el espacio
clc; clear all; close all

%----------------------------------------------------------------------%
% 1. DEFINICION DE VARIABLES SIMBOLICAS
%----------------------------------------------------------------------%
syms th1 d1 a1 alpha1
syms th2 d2 a2 alpha2
syms q1 q2
syms dq1 dq2
dq = [dq1;dq2];

%----------------------------------------------------------------------%
% 2. CINEMATICA DIRECTA
%----------------------------------------------------------------------%
% 2.1. SISTEMA COORDENADO 1
% -> Parametros D-H
th1 = q1;
a1 = 0;
alpha1 = pi/2;
% -> Matriz A
A1 = matriz_homogenea_DH(th1, d1,a1,alpha1);
% -> Matriz T
T01 = A1;
% 2.2. SISTEMA COORDENADO 2
% -> Parametros D-H
th2 = q2;
d2 = 0;
alpha2 = 0;
% -> Matriz A
A2 = matriz_homogenea_DH(th2, d2,a2,alpha2);
% -> Matriz T
T02 = T01*A2;
T02 = simplify(T02);

%----------------------------------------------------------------------%
% 3. CALCULO DEL JACOBIANO
%----------------------------------------------------------------------%
% 3.1. DEFINICION DE LOS PARAMETROS
% -> Origenes de coordenadas
p0 = [0;0;0];
p1 = T01(1:3,4);
p2 = T02(1:3,4);
% -> Vectores unitarios
z0 = [0;0;1];
z1 = T01(1:3,3);
z2 = T02(1:3,3);
% 3.2. CALCULO DEL TERMINO 'Jv'
Jv = [ cross(z0, (p2-p0)) cross(z1,(p2-p1)) ];
Jv = simplify(Jv);
disp('Jv')
disp(Jv)
% 3.3. CALCULO DEL TERMINO 'Jw'
Jw = [z0 z1];
disp('Jw')
disp(Jw)
% 3.4. HALLANDO LAS VELOCIDADES
v = Jv*dq;
v = simplify(v);
w = Jw*dq;
w = simplify(w);
disp('velocidad lineal')
disp(v)
disp('velocidad angular')

~9~
Análisis y Control de Robots
1ER INFORME

C. JACOBIANOS CENTRO DE MASA DE ESLABONES

Eslabón 2

CM1

Calculo del centro de masa respecto al eje 1

0 0
𝑪𝑴𝒆𝒔𝒍𝒂𝒃𝒐𝒏 𝟏 = [𝐿𝑦1] = [−(𝑑1 − 𝑍1)]
𝐿𝑧1 𝑌1
0
𝑪𝑴𝒆𝒔𝒍𝒂𝒃𝒐𝒏 𝟏 = [ 5.05 ]
−0.486

Calculo del centro de masa respecto a tierra.

𝑝𝑪𝑴𝒆𝒔𝒍𝒂𝒃𝒐𝒏 𝟏 = 𝑇10 ∗ 𝑪𝑴𝒆𝒔𝒍𝒂𝒃𝒐𝒏 𝟏

𝐿𝑧1 ∗ sin(𝜃1 )
𝑝𝑪𝑴𝒆𝒔𝒍𝒂𝒃𝒐𝒏 𝟏 = [−𝐿𝑧1 ∗ cos(𝜃1 )]
𝐿𝑦1 + 𝑑1

Reemplazando

−0.486 ∗ sin(𝜃1 )
𝑝𝑪𝑴𝒆𝒔𝒍𝒂𝒃𝒐𝒏 𝟏 = [ 0.486 ∗ cos(𝜃1 ) ]
19.45

Calculo del Jacobiano Traslacional (𝑱𝒗𝒄𝟏 )

𝑱𝒗𝒄𝟏_𝟏 = 𝒁𝟎𝟎 ∗ (𝑝𝐶𝑀𝐸𝑠𝑙𝑎𝑏𝑜𝑛 1 𝟎𝟐 − 𝑶𝟎𝟎 )

𝐿𝑧1 ∗ cos(𝜃2 )
𝑱𝒗𝒄𝟏_𝟏 = [ 𝐿𝑧1 ∗ sin(𝜃1 ) ]
0

~ 10 ~
Análisis y Control de Robots
1ER INFORME

−0.486 ∗ cos(𝜃2 )
𝑱𝒗𝒄𝟏_𝟏 = [ −0.486 ∗ sin(𝜃1 ) ]
0

𝑱𝒗𝒄𝟏_𝟐 = 𝒁𝟎𝟏 ∗ (𝑝𝐶𝑀𝐸𝑠𝑙𝑎𝑏𝑜𝑛 1 𝟎𝟐 − 𝑶𝟎𝟏 )

0
𝑱𝒗𝒄𝟏_𝟐 = [ 0]
0

𝑱𝒗𝒄𝟏 = [𝑱𝒗𝒄𝟏_𝟏 , 𝑱𝒗𝒄𝟏_𝟐 ]

𝐿𝑧1 ∗ cos(𝜃2 ) 0
𝑱𝒗𝒄𝟏 = [ 𝐿𝑧1 ∗ sin(𝜃1 ) 0]
0 0

Reemplazando

−0.486 ∗ cos(𝜃2 ) 0
𝑱𝒗𝒄𝟏 = [ −0.486 ∗ sin(𝜃1 ) 0]
0 0

Calculo del Jacobiano Rotacional (𝑱𝒘𝒄𝟏 )

0
𝑱𝒘𝒄𝟏_𝟏 = 𝒁𝟎𝟎 = [0]
1
0
𝑱𝒘𝒄𝟏_𝟐 = 𝒁𝟎𝟏 = [0]
0

𝑱𝑤𝑐1 = [𝑱𝒘𝒄𝟏_𝟏 , 𝑱𝒘𝒄𝟏_𝟐 ]

0 0
𝑱𝒘𝒄𝟏 = [0 0]
1 0

Calculo las velocidades: (𝑽𝒄𝟏 , 𝒘𝒄𝟏 )

𝑽𝒄𝟏 = 𝐉𝐯𝐜𝟏 ∗ [θ̇ 1 θ2̇ ]

θ̇ 1 ∗ (−0.486 ∗ cos(𝜃2 ))
𝑽𝒄𝟏 = [ θ̇ 1 ∗ (−0.486 ∗ sin(𝜃1 )) ]
0

~ 11 ~
Análisis y Control de Robots
1ER INFORME

𝒘𝒄𝟏 = 𝐉𝐰𝐜𝟏 ∗ [θ̇ 1 θ2̇ ]

0
𝒘𝒄𝟏 = [0]
𝜃̇1

~ 12 ~
Análisis y Control de Robots
1ER INFORME

Eslabón 2

CM2

Calculo del centro de masa respecto al eje 2

𝐿𝑥2 −𝑋2
𝑪𝑴𝒆𝒔𝒍𝒂𝒃𝒐𝒏 𝟐 = [ 0 ] = [ 0 ]
0 0
−2.69
𝑪𝑴𝒆𝒔𝒍𝒂𝒃𝒐𝒏 𝟐 = [ 0 ]
0

Calculo del centro de masa respecto a tierra.

𝑝𝑪𝑴𝒆𝒔𝒍𝒂𝒃𝒐𝒏 𝟐 = 𝑇20 ∗ 𝑪𝑴𝒆𝒔𝒍𝒂𝒃𝒐𝒏 𝟐

𝑑2 ∗ sin(𝜃1 ) + 𝐿𝑥2 ∗ cos(𝜃1 ) ∗ cos(𝜃2 ) + a2 ∗ cos(𝜃1 ) ∗ cos(𝜃2 )

𝑝𝑪𝑴𝒆𝒔𝒍𝒂𝒃𝒐𝒏 𝟐 = [ 𝐿𝑥2 ∗ cos(𝜃2 ) ∗ sin(𝜃1 ) − 𝑑2 ∗ cos(𝜃1 ) + a2 ∗ cos(𝜃2 ) ∗ sin(𝜃1 ) ]
𝑑1 + 𝐿𝑥2 ∗ sin(𝜃1 ) + 𝑎2 ∗ sin(𝜃2 )

Reemplazando

6.8 ∗ sin(𝜃1 ) − 2.69 ∗ cos(𝜃1 ) ∗ cos(𝜃2 ) + 3.6 ∗ cos(𝜃1 ) ∗ cos(𝜃2 )

𝑝𝑪𝑴𝒆𝒔𝒍𝒂𝒃𝒐𝒏 𝟐 = [−2.69 ∗ cos(𝜃2 ) ∗ sin(𝜃1 ) − 6.8 ∗ cos(𝜃1 ) + 3.6 ∗ cos(𝜃2 ) ∗ sin(𝜃1 )]
14.4 − 2.69 ∗ sin(𝜃1 ) + 3.6 ∗ sin(𝜃2 )

Calculo del Jacobiano Traslacional (𝑱𝒗𝒄𝟐 )

𝑱𝒗𝒄𝟐_𝟏 = 𝒁𝟎𝟎 ∗ (𝑝𝐶𝑀𝐸𝑠𝑙𝑎𝑏𝑜𝑛 2 𝟎𝟐 − 𝑶𝟎𝟎 )

𝑑2 ∗ cos(𝜃1 ) − 𝐿𝑥2 ∗ cos(𝜃2 ) ∗ sin(𝜃1 ) − 𝑎2 ∗ cos(𝜃2 ) ∗ sin(𝜃1 )

𝑱𝒗𝒄𝟐_𝟏 = [𝑑2 ∗ sin(𝜃1 ) + 𝐿𝑥2 ∗ cos(𝜃1 ) ∗ cos(𝜃2 ) + 𝑎2 ∗ cos(𝜃2 ) ∗ cos(𝜃1 )]
0

6.8 ∗ cos(𝜃1 ) + 2.69 ∗ cos(𝜃2 ) ∗ sin(𝜃1 ) − 3.6 ∗ cos(𝜃2 ) ∗ sin(𝜃1 )

𝑱𝒗𝒄𝟐_𝟏 = [6.8 ∗ sin(𝜃1 ) − 2.69 ∗ cos(𝜃1 ) ∗ cos(𝜃2 ) + 3.6 ∗ cos(𝜃2 ) ∗ cos(𝜃1 )]
0

~ 13 ~
Análisis y Control de Robots
1ER INFORME

𝑱𝒗𝒄𝟐_𝟐 = 𝒁𝟎𝟏 ∗ (𝑝𝐶𝑀𝐸𝑠𝑙𝑎𝑏𝑜𝑛 1 𝟎𝟐 − 𝑶𝟎𝟏 )

− cos(𝜃1 ) ∗ sin(𝜃2 ) ∗ (𝐿𝑥2 + 𝑎2)

𝑱𝒗𝒄𝟐_𝟐 = [ − sin(𝜃1 ) ∗ sin(𝜃2 ) ∗ (𝐿𝑥2 + 𝑎2) ]
cos(𝜃2 ) ∗ (𝐿𝑥2 + 𝑎2)

𝑱𝒗𝒄𝟐 = [𝑱𝒗𝒄𝟐_𝟏 , 𝑱𝒗𝒄𝟐_𝟐 ]

𝑑2 ∗ cos(𝜃1 ) − 𝐿𝑥2 ∗ cos(𝜃2 ) ∗ sin(𝜃1 ) − 𝑎2 ∗ cos(𝜃2 ) ∗ sin(𝜃1 ) − cos(𝜃1 ) ∗ sin(𝜃2 ) ∗ (𝐿𝑥2 + 𝑎2)
[𝑑2 ∗ sin(𝜃1 ) + 𝐿𝑥2 ∗ cos(𝜃1 ) ∗ cos(𝜃2 ) + 𝑎2 ∗ cos(𝜃2 ) ∗ cos(𝜃1 ) − sin(𝜃1 ) ∗ sin(𝜃2 ) ∗ (𝐿𝑥2 + 𝑎2) ]
0 cos(𝜃2 ) ∗ (𝐿𝑥2 + 𝑎2)

Calculo del Jacobiano Rotacional (𝑱𝒘𝒄𝟐 )

0
𝑱𝒘𝒄𝟐_𝟏 = 𝒁𝟎𝟎 = [0]
1

sin(𝜃1 )
𝑱𝒘𝒄𝟐_𝟐 = 𝒁𝟎𝟏 = [−cos(𝜃1 )]
0

𝑱𝑤𝑐2 = [𝑱𝒘𝒄𝟐_𝟏 , 𝑱𝒘𝒄𝟐_𝟐 ]

Reemplazando

0 sin(𝜃1 )
𝑱𝒘𝒄𝟐 = [0 −cos(𝜃1 )]
1 0

Calculo las velocidades: (𝑽𝒄𝟐 , 𝒘𝒄𝟐 )

𝑽𝒄𝟐 = 𝐉𝐯𝐜𝟐 ∗ [θ̇ 1 θ2̇ ]

𝒘𝒄𝟐 = 𝐉𝐰𝐜𝟐 ∗ [θ̇ 1 θ2̇ ]

~ 14 ~
Análisis y Control de Robots
1ER INFORME

SIMULACIÓN EN MATLAB
% Demo_04
% Jacobiano de los C.M del manipulador 2GDL en el espacio
clc; clear all; close all

%---------------------------------------------------------------------
-%
% 1. DEFINICION DE VARIABLES SIMBOLICAS
%---------------------------------------------------------------------
-%
syms th1 d1 a1 alpha1
syms th2 d2 a2 alpha2
syms q1 q2
syms dq1 dq2
syms Lz1 Ly1 Lx2 %Respecto al eje1 y eje2 respectivamente
%Lz2=Ly2=0
q = [q1; q2];
dq = [dq1; dq2];

%---------------------------------------------------------------------
-%
% 2. CINEMATICA DIRECTA
%---------------------------------------------------------------------
-%
% 2.1. SISTEMA COORDENADO 1
% -> Parametros D-H
th1 = q1;
a1 = 0;
d1 = d1;
alpha1 = pi/2;
% -> Matriz A
A1 = matriz_homogenea_DH(th1,d1,a1,alpha1);
% -> Matriz T
T01 = A1;
% 2.2. SISTEMA COORDENADO 2
% -> Parametros D-H
th2 = q2;
d2 = d2;
a2 = a2;
alpha2 = 0;
% -> Matriz A
A2 = matriz_homogenea_DH(th2,d2,a2,alpha2);
% -> Matriz T
T02 = T01*A2;
T02 = simplify(T02);

%---------------------------------------------------------------------
-%
% 3. CALCULO DE LOS JACOBIANOS PARA CADA
% CENTRO DE MASA DE LOS ESLABONES
%---------------------------------------------------------------------
-%
% 3.1. DEFINICION DE LOS PARAMETROS
% -> Origenes de coordenadas
p0 = [0;0;0];
p1 = T01(1:3,4);
p2 = T02(1:3,4);
% -> Vectores unitarios
z0 = [0;0;1];
z1 = T01(1:3,3);
z2 = T02(1:3,3);

% 3.2. COORDENADAS DE LOS C.M

% -> Del eslabon 1
pCM1 = [0;Ly1;Lz1;1];

~ 15 ~
Análisis y Control de Robots
1ER INFORME

pCM1 = T01*pCM1;
pCM1 = simplify( pCM1(1:3) );
disp('pCM1')
disp(pCM1)
% -> Del eslabon 2
pCM2 = [Lx2;0;0;1];
pCM2 = T02*pCM2;
pCM2 = simplify( pCM2(1:3) );
disp('pCM2')
disp(pCM2)

% 3.3. CALCULO DE LOS JACOBIANOS PARA EL ESLABON 1

% -> Calculo de "Jv"
Jvc1 = [ cross(z0, (pCM1-p0)) zeros(3,1) ];
Jvc1 = simplify(Jvc1);
% -> Calculo de "Jw"
Jw1 = [ z0 zeros(3,1) ];

% 3.4. CALCULO DE LOS JACOBIANOS PARA EL ESLABON 2

% -> Calculo de "Jv"
Jvc2 = [ cross(z0, (pCM2-p0)) cross(z1, (pCM2-p1)) ];
Jvc2 = simplify(Jvc2);
% -> Calculo de "Jw"
Jw2 = [ z0 z1 ];

~ 16 ~
Análisis y Control de Robots
1ER INFORME

D. JACOBIANOS CENTRO DE MASA MOTORES

Motor 1

CM1

Calculo del centro de masa respecto al eje 0

0 0
𝑪𝑴𝒎𝒐𝒕𝒐𝒓 𝟏 = [𝐿𝑦𝑚1] = [−𝑍]
𝐿𝑧𝑚1 𝑌
0
𝑪𝑴𝒎𝒐𝒕𝒐𝒓 𝟏 = [ 0 ]
−5.561

Calculo del Jacobiano Traslacional (𝑱𝒗𝒎𝟏 )

𝑱𝒗𝒎𝟏_𝟏 = 𝒁𝟎𝟎 ∗ (𝑝𝐶𝑀𝐸𝑠𝑙𝑎𝑏𝑜𝑛 1 𝟎𝟐 − 𝑶𝟎𝟎 )

0
𝑱𝒗𝒎𝟏_𝟏 = [0]
0

𝑱𝒗𝒎𝟏_𝟐 = 𝒁𝟎𝟏 ∗ (𝑝𝐶𝑀𝐸𝑠𝑙𝑎𝑏𝑜𝑛 1 𝟎𝟐 − 𝑶𝟎𝟏 )

0
𝑱𝒗𝒎𝟏_𝟐 = [ 0]
0

𝑱𝒗𝒎𝟏 = [𝑱𝒗𝒎𝟏_𝟏 , 𝑱𝒗𝒎𝟏_𝟐 ]

0 0
𝑱𝒗𝒄𝟏 = [0 0]
0 0

~ 17 ~
Análisis y Control de Robots
1ER INFORME

Calculo del Jacobiano Rotacional (𝑱𝒘𝒄𝟏 )

0
𝑱𝒘𝒄𝟏_𝟏 = 𝒁𝟎𝟎 = [ 0 ]
𝐾𝑟1
0
𝑱𝒘𝒄𝟏_𝟐 = 𝒁𝟎𝟏 = [0]
0

𝑱𝑤𝑐1 = [𝑱𝒘𝒄𝟏_𝟏 , 𝑱𝒘𝒄𝟏_𝟐 ]

0 0
𝑱𝒘𝒄𝟏 =[ 0 0]
𝐾𝑟1 0

Calculo las velocidades: (𝑽𝒄𝟏 , 𝒘𝒄𝟏 )

𝑽𝒄𝟏 = 𝐉𝐯𝐜𝟏 ∗ [θ̇ 1 θ2̇ ]

0
𝑽𝒄𝟏 = [0]
0
𝒘𝒄𝟏 = 𝐉𝐰𝐜𝟏 ∗ [θ̇ 1 θ2̇ ]

0
𝒘𝒄𝟏 =[ 0 ]
𝜃̇1 ∗ 𝐾𝑟1

~ 18 ~
Análisis y Control de Robots
1ER INFORME

Motor 2

CM2

Calculo del centro de masa respecto al eje 1

0 0
𝑪𝑴𝒎𝒐𝒕𝒐𝒓 𝟐 = [𝐿𝑦𝑚2] = [ −𝑍 ]
𝐿𝑧𝑚2 𝑑2 + 𝑌
0
𝑪𝑴𝒎𝒐𝒕𝒐𝒓 𝟐 = [ 0 ]
1.239

Calculo del centro de masa respecto a tierra.

𝑝𝑪𝑴𝒎𝒐𝒕𝒐𝒓 𝟐 = 𝑇10 ∗ 𝑪𝑴𝒎𝒐𝒕𝒐𝒓 𝟐

𝐿𝑚𝑧2 ∗ sin(𝜃1 )
𝑝𝑪𝑴𝒎𝒐𝒕𝒐𝒓 𝟐 = [−𝐿𝑚𝑧2 ∗ cos(𝜃1 )]
𝑑1

Reemplazando

1.239 ∗ sin(𝜃1 )
𝑝𝑪𝑴𝒆𝒔𝒍𝒂𝒃𝒐𝒏 𝟏 = [−1.239 ∗ cos(𝜃1 )]
14.4

Calculo del Jacobiano Traslacional (𝑱𝒗𝒎𝟐 )

𝑱𝒗𝒎𝟐_𝟏 = 𝒁𝟎𝟎 ∗ (𝑝𝐶𝑀𝐸𝑠𝑙𝑎𝑏𝑜𝑛 1 𝟎𝟐 − 𝑶𝟎𝟎 )

𝐿𝑚𝑧2 ∗ cos(𝜃1 )
𝑱𝒗𝒎𝟐_𝟏 = [ 𝐿𝑚𝑧2 ∗ sin(𝜃1 ) ]
0

𝑱𝒗𝒎𝟐_𝟐 = 𝒁𝟎𝟏 ∗ (𝑝𝐶𝑀𝐸𝑠𝑙𝑎𝑏𝑜𝑛 1 𝟎𝟐 − 𝑶𝟎𝟏 )

~ 19 ~
Análisis y Control de Robots
1ER INFORME

0
𝑱𝒗𝒎𝟐_𝟐 = [0]
0

𝑱𝒗𝒎𝟐 = [𝑱𝒗𝒎𝟐_𝟏 , 𝑱𝒗𝒎𝟐_𝟐 ]

𝐿𝑚𝑧2 ∗ cos(𝜃1 ) 0
𝑱𝒗𝒎𝟐 = [ 𝐿𝑚𝑧2 ∗ sin(𝜃1 ) 0]
0 0

Calculo del Jacobiano Rotacional (𝑱𝒘𝒄𝟏 )

0
𝑱𝒘𝒎𝟐_𝟏 = 𝒁𝟎𝟎 = [0]
1

𝐾𝑟2 ∗ sin(𝜃1 )
𝑱𝒘𝒎𝟐_𝟐 = 𝒁𝟎𝟏 = [−𝐾𝑟2 ∗ sin(𝜃1 )]
0

𝑱𝑤𝑚2 = [𝑱𝒘𝒎𝟐_𝟏 , 𝑱𝒘𝒎𝟐_𝟐 ]

0 𝐾𝑟2 ∗ sin(𝜃1 )
𝑱𝒘𝒎𝟐 = [0 −𝐾𝑟2 ∗ sin(𝜃1 )]
1 0

Calculo las velocidades: (𝑽𝒎𝟐 , 𝒘𝒎𝟐 )

𝑽𝒎𝟐 = 𝐉𝐯𝐦𝟐 ∗ [θ̇ 1 θ2̇ ]

𝜃̇1 ∗ (𝐿𝑚𝑧2 ∗ 𝑐𝑜𝑠(𝜃1 ))

𝑽𝒎𝟐 = [ 𝜃̇1 ∗ (𝐿𝑚𝑧2 ∗ 𝑠𝑖𝑛(𝜃1 )) ]
0
𝒘𝒎𝟐 = 𝐉𝐰𝐦𝟐 ∗ [θ̇ 1 θ2̇ ]

𝜃̇2 ∗ 𝐾𝑟2 ∗ 𝑠𝑖𝑛(𝜃1 )

𝒘𝒎𝟐 = [𝜃̇2 ∗ (−𝐾𝑟2 ∗ 𝑠𝑖𝑛(𝜃1 ))]
𝜃̇1

~ 20 ~
Análisis y Control de Robots
1ER INFORME

SIMULACIÓN EN MATLAB
% Demo_06
% Jacobiano de los C.M del manipulador 2GDL en el espacio
% Se asume que:
% - Los motores tienen su CM en el eje z de los sistemas
% de coordenadas
% - Los zm del motor son iguales a los z de las juntas
clc; clear all; close all

%----------------------------------------------------------------------
%
% 1. DEFINICION DE VARIABLES SIMBOLICAS
%----------------------------------------------------------------------
%
syms th1 d1 a1 alpha1
syms th2 d2 a2 alpha2
syms q1 q2
syms dq1 dq2
syms Lmz1 Lmz2
syms kr1 kr2 % Factores de reduccion de los motores
q = [q1; q2];
dq = [dq1; dq2];

%----------------------------------------------------------------------
%
% 2. CINEMATICA DIRECTA
%----------------------------------------------------------------------
%
% 2.1. SISTEMA COORDENADO 1
% -> Parametros D-H
th1 = q1;
a1 = 0;
d1 = d1;
alpha1 = pi/2;
% -> Matriz A
A1 = matriz_homogenea_DH(th1,d1,a1,alpha1);
% -> Matriz T
T01 = A1;
% 2.2. SISTEMA COORDENADO 2
% -> Parametros D-H
th2 = q2;
d2 = d2;
a2 = a2;
alpha2 = 0;
% -> Matriz A
A2 = matriz_homogenea_DH(th2,d2,a2,alpha2);
% -> Matriz T
T02 = T01*A2;
T02 = simplify(T02);

%----------------------------------------------------------------------
%
% 3. CALCULO DE LOS JACOBIANOS PARA CADA
% CENTRO DE MASA DE LOS ESLABONES
%----------------------------------------------------------------------
%
% 3.1. DEFINICION DE LOS PARAMETROS
% -> Origenes de coordenadas
p0 = [0;0;0];
p1 = T01(1:3,4);
p2 = T02(1:3,4);
% -> Vectores unitarios
z0 = [0;0;1];
z1 = T01(1:3,3);

~ 21 ~
Análisis y Control de Robots
1ER INFORME

z2 = T02(1:3,3);
% 3.2. COORDENADAS DE LOS C.M
% -> Del motor1 (mueve a la junta 1)
pCM_m1 = [0;0;Lmz1];
zm1 = z0;
disp('pCM_m1')
disp(pCM_m1)

% -> Del motor2 (mueve a la junta 2)

pCM_m2 = [0;0;Lmz2;1]; % Esta ubicado en el origen del sistema 1
pCM_m2 = T01*pCM_m2;
pCM_m2 = simplify( pCM_m2(1:3) );
zm2 = z1;
disp('pCM_m2')
disp(pCM_m2)

% 3.3. CALCULO DE LOS JACOBIANOS PARA EL MOTOR 1

% -> Calculo de "Jv"
Jvc_m1 = [ zeros(3,1) zeros(3,1) ];
% -> Calculo de "Jw"
Jw_m1 = [ kr1*zm1 zeros(3,1) ];

% 3.4. CALCULO DE LOS JACOBIANOS PARA EL MOTOR 2

% -> Calculo de "Jv"
Jvc_m2 = [ cross(z0, (pCM_m2-p0)) zeros(3,1) ];
% -> Calculo de "Jw"
Jw_m2 = [ z0 kr2*zm2 ];

~ 22 ~
Análisis y Control de Robots
1ER INFORME

2. DINÁMICA

MODELO DINÁMICO
Modelo de Lagrange:
𝐷(𝑞)𝑞̈ + 𝐶(𝑞, 𝑞̇ )𝑞̈ + 𝑔𝑘 (𝑞) = 𝜏

1. Cálculo de la Matriz de Inercia.

1.1 Componente Traslacional

Eslabón 1.

𝐷1𝑉 = 𝑚1 ∗ 𝐽𝑉𝐶1 𝑇 ∗ 𝐽𝑉𝐶1

Eslabón 2.

𝐷2𝑉 = 𝑚2 ∗ 𝐽𝑉𝐶2 𝑇 ∗ 𝐽𝑉𝐶2

% 4. CALCULO DE LA MATRIZ DE INERCIA - COMPONENTE
TRANSLACIONAL
%--------------------------------------------------------
--------------%
% 4.1. ESLABON "1"
D1v = m1*transpose(Jvc1)*Jvc1;
D1v = simplify(D1v)
% 4.2. ESLABON "2"
D2v = m2*transpose(Jvc2)*Jvc2;
D2v = simplify(D2v)

2
𝐷1𝑉 = [𝐿𝑧1 ∗ 𝑚1 0]
0 0

~ 23 ~
Análisis y Control de Robots
1ER INFORME

(−𝑚2 ∗ (𝐿𝑥2 + 𝑎2 )2 ∗ sin(𝑞2)2 ) 0

𝐷2𝑉 = [ ]
0 𝑚2 ∗ (𝐿𝑥2 + 𝑎2 )2
Reemplazando
m1: 0.1013 kg

m2=0.00353 kg

~ 24 ~
Análisis y Control de Robots
1ER INFORME

Motor1
𝐷𝑚1𝑉 = 𝑚𝑚1 ∗ 𝐽𝑉𝑚1 𝑇 ∗ 𝐽𝑉𝑚1

Motor1
𝐷𝑚2𝑉 = 𝑚𝑚2 ∗ 𝐽𝑉𝑚2 𝑇 ∗ 𝐽𝑉𝑚2

~ 25 ~
Análisis y Control de Robots
1ER INFORME

1.2 Componente Rotacional.

Eslabón 1

𝐼𝑙1 = 𝑅1 ∗ 𝐼1 ∗ 𝑅1 𝑇
R1:

Cálculo de 𝐼1 :

~ 26 ~
Análisis y Control de Robots
1ER INFORME

𝐷1𝑤 = 𝐽𝑤1 𝑇 ∗ 𝐼𝑙1 ∗ 𝐽𝑤1

Eslabón 2
Cálculo de 𝐼2 :

𝐼𝑙2 = 𝑅2 ∗ 𝐼2 ∗ 𝑅2 𝑇
cos 𝜃1 0 sin 𝜃1
𝑅2 = [ sin 𝜃1 0 cos 𝜃1 ]
0 1 0

𝐷2𝑤 = 𝐽𝑤2 𝑇 ∗ 𝐽𝑉𝐶2 ∗ 𝐽𝑤2

% 5. CALCULO DE LA MATRIZ DE INERCIA (COMPONENTE

ROTACIONAL)

~ 27 ~
Análisis y Control de Robots
1ER INFORME

%--------------------------------------------------------
--------------%
% 5.1. ESLABON "1"
% -> Tensor de inercia en el centro de masa
I1 = [0 0 0;0 I1 0; 0 0 I1];
% -> Tensor de inercia en el sistema inercial
R1 = T01(1:3, 1:3);
II1 = R1*I1*transpose(R1);
II1 = simplify(II1);
% -> Componente "D"
D1w = transpose(Jw1)*II1*Jw1;
D1w = simplify(D1w)
% 5.2. ESLABON "2"
% -> Tensor de inercia en el centro de masa
I2 = [0 0 0;0 I2 0; 0 0 I2];
% -> Tensor de inercia en el sistema inercial
R2 = T02(1:3, 1:3);
II2 = R2*I2*transpose(R2);
% -> Componente "D"
D2w = transpose(Jw2)*II2*Jw2;
D2w = simplify(D2w)

Motor 1
𝐼𝑙𝑚1 = 𝑅𝑚1 ∗ 𝐼𝑚1 ∗ 𝑅𝑚1 𝑇

𝐷𝑚1𝑤 = 𝐽𝑤𝑚1 𝑇 ∗ 𝐼𝑙𝑚1 ∗ 𝐽𝑤𝑚1

Motor 2
𝐼𝑙𝑚2 = 𝑅𝑚2 ∗ 𝐼𝑚2 ∗ 𝑅𝑚2 𝑇

𝐷𝑚2𝑤 = 𝐽𝑤𝑚2 𝑇 ∗ 𝐽𝑙𝑚2 ∗ 𝐽𝑤𝑚2

1.3 Cálculo de la matriz D

𝐷 = 𝐷1𝑉 + 𝐷2𝑉 + 𝐷𝑚1𝑉 + 𝐷𝑚2𝑉 + 𝐷1𝑤 + 𝐷2𝑤 + 𝐷𝑚1𝑤 + 𝐷𝑚2𝑤

2. Cálculo de la Matriz de Coriolis

Símbolos de Christoffel
1 𝜕𝐷𝑘𝑗 𝜕𝐷𝑘𝑖 𝜕𝐷𝑖𝑗
𝑐𝑖𝑗𝑘 = [ + − ]
2 𝜕𝑞𝑖 𝜕𝑞𝑗 𝜕𝑞𝑘

% 7.1. CALCULO DE LOS TERMINOS "Cijk"

~ 28 ~
Análisis y Control de Robots
1ER INFORME

cijk = cell(n,n,n);
for i=1:n
for j=1:n
for k=1:n
% Terminos individuales
term_01 = diff(D(k,j), q(i));
term_02 = diff(D(k,i), q(j));
term_03 = diff(D(i,j), q(k));
% Suma total
cijk{i,j,k} = 1/2*( term_01 + term_02
- term_03);
end
end
end

3. Cálculo del Vector de Gravedad

𝑃(𝑞) = −𝑚1 ∗ 𝐽𝑉𝐶1 𝑇 ∗ 𝑔 − 𝑚2 ∗ 𝐽𝑉𝐶2 𝑇 ∗ 𝑔 − 𝑚𝑚1 ∗ 𝐽𝑉𝑚1 𝑇 ∗ 𝑔 − 𝑚𝑚2 ∗ 𝐽𝑉𝑚2 𝑇 ∗ 𝑔

𝜕𝑃(𝑞)
𝐺(𝑞) =
𝜕𝑞𝑘

~ 29 ~