UNIVERSIDAD NACIONAL DE INGENIERÍA

FACULTAD DE INGENIERÍA MECÁNICA

INFORME FINAL

CURSO: Análisis y Control de Robots (MT517)

PROFESOR: Calle Flores, Iván Arturo
ALUMNOS: García De La Cruz Pablo 20142127E
Espinoza Velásquez, Jhon 20140076D

SECCION: “A"

2018 - I
 PROYECTO DEL CURSO ANÁLISIS Y CONTROL DE ROBOTS
1. OBJETIVO

 Diseñar e implementar el control de posición de un manipulador de dos grados

de libertad, usando un control PID y PD con compensación de gravedad

2. DISEÑO MECÁNICO Y ELECCIÓN DE MOTORES

Para la realización del brazo de 2 GDL se ha decidido usar como material de los links el
acrílico de 2.5 mm de espesor. Las razones de la elección de este material son que este
es ligero, presenta un costo relativamente bajo y se adapta a nuestros requerimientos
para el diseño.
Las medidas estimadas para el tamaño del brazo son de 13 x 4 cm el primer link y de 8
x 4 cm el segundo link, con esto podemos hacer los cálculos para ver el torque mínimo
requerido para los motores.
2.1. CÁLCULO APROXIMADO DEL TORQUE PARA LOS MOTORES
El diseño realizado en Solidworks para el robot es el mostrado en la figura 1, en el dibujo
se puede apreciar que el segundo motor poco torque necesitará ya que moverá muy
poco peso, es por eso que con un de 1kg-cm es suficiente. De todos modos se usará
un motor DC de mayor torque, como se indicará en los siguientes apartados.
El primer motor sí requerirá de un torque considerablemente alto, ya que se encargará
de mover prácticamente todo el brazo, es por ello que para este caso se estimará un
torque aproximado que pueda necesitar este motor.

1
 Figura 1. Diseño realizado en Solidworks del brazo robótico de 2GDL
Para realizar este cálculo, primero se aísla toda parte del robot que moverá el primer
motor, como se muestra en la figura 2. Luego, con las densidad de todos los materiales
establecidos y sabiendo que el segundo motor tiene un peso de 105 gramos, se procede
a ver las características brindadas en Solidworks como es mostrado en la figura 3.

Figura 2. Parte superior del robot de 2GDL

(Colocar foto de las características que muestra un Ixx de 4992793.87 g*mm^2)
Figura 3. Características de la parte superior del robot
De la figura anterior obtenemos que:

0
𝐼𝑧𝑧 = 4992793.87 𝑔 − 𝑚𝑚2 ⋯ (1)
Sabemos que:

0
∑ 𝑀 = 𝑇𝑚𝑜𝑡𝑜𝑟 = 𝐼𝑧𝑧 ∙𝛼 ⋯ (2)

2 ∙ Δ𝜃
𝛼= ⋯ (3)
𝑡2

2
Teniendo en cuenta los parámetros que suelen tener los motores DC, podemos suponer
uno que trabajará a 180 RPM, es decir dará una vuelta en 1/3 de segundo:

1
𝑡= 𝑠 Δ𝜃 = 2𝜋 ⋯ (4)
3
Reemplazando (1), (3) y (4) en (2) y trabajando en las unidades del SI tendremos:
𝑇𝑚𝑜𝑡𝑜𝑟 = 0.56 𝑁 − 𝑚 = 5.6 𝑘𝑔 − 𝑐𝑚
Por tanto se necesitará un motor de al menos 6 𝑘𝑔 − 𝑐𝑚 para poder trabajar, sin
embargo esta aproximación está relativamente alejada de lo normal, por lo que con un
motor que ofrezca 10 𝑘𝑔 − 𝑐𝑚 será más que suficiente para poder mover sin problema
todo el robot.

2.2. MOTORES SELECCIONADOS

Ya se vio que el primer motor deberá ser uno con un torque relativamente alto para que
pueda soportar y mover toda la estructura del robot. El mínimo torque deberá ser de
6 𝑘𝑔 − 𝑐𝑚.
Para el motor de la junta 2 no se requiere un torque alto, es por ello que – como se
mencionó anteriormente – bastará con un torque de 1𝑘𝑔 − 𝑐𝑚.
Así también se requiere que los motores tengan encoder de 2 canales para que sea
posible poder controlar la posición del brazo de 2 GDL en todo momento. Así mismo se
requerirá que las RPM que entregan las cajas reductoras de los motores sean bajas
(menores a 300 RPM).
En base a estos requerimientos se realizó la selección y compra de los motores. A
continuación, detallaremos las especificaciones y planes de los motores.
2.2.1. MOTOR JUNTA 1
Nombre: Motor 12 V 320 RPM / 12 V 107 RPM / 6 V 160 RPM DC Gear motor encoder
El motor con reductor de velocidad y encoder presenta las siguientes características:
trabaja a 12 V, posee un torque de 20 𝑘𝑔 − 𝑐𝑚 y 107 RPM. Además, presenta un
encoder de efecto Hall, el cual tiene una resolución de 990 pulsos por revolución.
También se muestra las características brindadas por el fabricante.

3
 Figura 4. Motor para el primer link adquirido
Tabla 1. Especificaciones del prime motor, brindadas por el fabricante

Figura 5. Planos del primer motor brindados por el fabricante

2.2.2. MOTOR JUNTA 2
Nombre: 6 V 100/210/300 RPM encoder Motores DC Gear Motores
El motor con reductor de velocidad y encoder presenta las siguientes características:
Además, presenta un encoder de efecto Hall, el cual tiene una resolución de 823.1
pulsos por revolución. También se muestra las especificaciones brindadas:

4
 Figura 6. Motor para el segundo link adquirido
Tabla 2. Especificaciones del segundo motor, brindadas por el fabricante

5
 Figura 7. Planos del segundo motor brindados por el fabricante

2.3. DISEÑO EN SOLIDWORKS

A continuación, se muestra el diseño del brazo de 2 GDL
2.3.1. MOTOR JUNTA 1

Figura 8. Diseño en Solidworks del primer motor

2.3.2. MOTOR JUNTA 2

6
 Figura 9. Diseño en Solidworks del segundo motor
2.3.3. VISTAS DEL ROBOT

Figura 10. Vista frontal del robot de 2GDL

7
 Figura 11. Vista lateral del robot de 2GDL
a. LINK 1

Figura 12. Diseño del link 1 en Solidworks

b. LINK 2

8
 Figura 13. Diseño del link 2 en Solidworks
c. Soporte del segundo motor

Figura 14. Diseño del soporte del segundo motor en Solidworks

2.4. PLANOS DE DISEÑO

a. LINKS 1 y 2

9
 Figura 15. Planos de los link 1 y 2

3. CINEMÁTICA DIRECTA
3.1. DETERMINACIÓN DE LOS SISTEMAS COORDENADOS

10
 Figura 16. Sistemas coordenados del robot
3.2. DETERMINACIÓN DE LOS PARÁMETROS DE DENAVIT HARTENBERG

Tabla 3. Parámetros DH
JUNTA 𝜽𝒊 𝒅𝒊 𝒂𝒊 𝜶𝒊
𝜋
1 𝜃1 𝐿1 0
2
2 𝜃2 𝐿2 0 0

Figura 16. Sistemas coordenados del robot para el

cálculo de los parámetros DH

11
3.3. DETERMINACIÓN DE LAS MATRICES DE TRANSFORMACIÓN

⋯ (5)

⋯ (6)

Reemplazando los valores de la tabla 3 a (6) tendremos:

cos 𝜃1 0 𝑠𝑖𝑛𝜃1 0
𝑠𝑖𝑛𝜃1 0 −𝑐𝑜𝑠𝜃1 0
𝑇10 = [ ]
0 0 1 𝐿1
0 0 0 1
𝑐𝑜𝑠𝜃1 ∙ 𝑐𝑜𝑠𝜃2 − 𝑐𝑜𝑠 𝜃1 ∙ 𝑠𝑖𝑛𝜃2 𝑠𝑖𝑛𝜃1 𝐿2 ∙ 𝑠𝑖𝑛 𝜃1
𝑐𝑜𝑠𝜃2 ∙ 𝑠𝑖𝑛𝜃1 −𝑠𝑖𝑛𝜃1 ∙ 𝑠𝑖𝑛𝜃2 −𝑐𝑜𝑠𝜃1 − 𝐿2 ∙ 𝑐𝑜𝑠𝜃1
𝑇20 = [ ]
𝑠𝑖𝑛𝜃2 𝑐𝑜𝑠𝜃2 0 𝐿1
0 0 0 1
4. CINEMÁTICA INVERSA

De la figura 17
vemos
fácilmente que:
𝑧𝑐 = 𝐿1
Esto es una
primera
restricción.

Figura 17. Vista isométrica del robot de 2GDL

12
 De la figura 18 tenemos que:
𝑥𝑐 = 𝐿2 ∙ 𝑠𝑖𝑛𝜃1 ⋯ (7)
𝑦𝑐 = −𝐿2 ∙ 𝑐𝑜𝑠𝜃1 ⋯ (8)

Haciendo (7)2 + (8)2 :

𝑥𝑐2 + 𝑦𝑐2 = 𝐿22 (sin2 𝜃1 + cos 2 𝜃1 )
Despejando:
2 2
𝑥𝑐2 + 𝑦𝑐2
sin 𝜃1 + cos 𝜃1 = =𝐷
𝐿22
Segundo criterio de restricción:
𝐷=1
Hacemos (7)/(8):
𝑥𝑐
𝑡𝑎𝑛𝜃1 =
−𝑦𝑐
𝑥𝑐
𝜃1 = tan−1
Figura 18. Vista superior del robot de 2GDL −𝑦𝑐

El ángulo 𝜃2 solo determinará el giro de la pinza por lo que no hay una relación para ella
en la cinemática inversa.

5. CÓDIGOS EN MATLAB
5.1. CINEMÁTICA DIRECTA
%**** CINEMATICA DEL ROBOT DE 2 GDL ****%
clc, clear all, close all;

% ------------------------------------- %
% 1. DEFINICION DE VARIABLES SIMBOLICAS %
% ------------------------------------- %
syms theta1 d1 a1 alpha1;
syms theta2 d2 a2 alpha2;
syms q1 q2;
syms L1 L2;

% ------------------------------------- %
% 2. DEFINICION DE SISTEMAS COORDENADOS %
% ------------------------------------- %
% 2.1. Sistema Coordenado 1
theta1 = q1;
d1 = L1;
a1 = 0;
alpha1 = pi/2;
A1 = matriz_homogenea_DH(theta1,d1,a1,alpha1);
% 2.2 Sistema coordenado 2
theta2 = q2;
d2 = L2;
a2 = 0;
alpha2 = 0;
A2 = matriz_homogenea_DH(theta2,d2,a2,alpha2);

% ------------------------------------- %
% 3. CINEMATICA %
% ------------------------------------- %
T01 = A1;
T02 = simplify(A1*A2);

13
% 3.1. Matriz de transformacion T01
disp('Matriz T01');
pretty(T01);
% 3.2. Matriz de transformacion T02
disp('Matriz T02');
pretty(T02);

Figura 19. Resultados de la cinemática directa

5.2. SIMULACIÓN DE LA CINEMÁTICA DIRECTA
%**** SIMULACION CINEMATICA DIRECTA ****%
clc, clear all, close all;

% ------------------------------------- %
% 1. DEFINICION DE PARÁMETROS DEL ROBOT %
% ------------------------------------- %
L1 = 1;
L2 = 1;

% ------------------------------------- %
% 2. DEFINICION DE SISTEMAS COORDENADOS %
% ------------------------------------- %
% 2.1. Sistema coordenado inercial
T0 = eye(4);
figure('name', 'Cinematica Directa', 'NumberTitle', 'off');
title('SIMULACIÓN CINEMÁTICA DIRECTA');
h0 = plot_frame(T0, 'color', 'k', 'view', 'auto');
axis([-4 4 -4 4 -4 4]);
hold on, grid on;
xlabel('x(m)');
ylabel('y(m)');
% 2.2. Sistema coordenado 1
theta1 = 0;
d1 = L1;
a1 = 0;
alpha1 = pi/2;
A1 = matriz_homogenea_DH(theta1,d1,a1,alpha1);

14
T01 = A1;
h1 = plot_frame(T01, 'frame', '1', 'color', 'b');
% 2.3. Sistema coordenado 2
theta2 = 0;
d2 = L2;
a2 = 0;
alpha2 = 0;
A2 = matriz_homogenea_DH(theta2,d2,a2,alpha2);
T02 = T01*A2;
h2 = plot_frame(T02, 'frame', '2', 'color', 'g');

disp('Presione una tecla para empezar la simulación.');

pause();

% ------------------------------------- %
% 3. LAZO DE SIMULACIÓN %
% ------------------------------------- %
MOVE_JUNTA1 = 1;
MOVE_JUNTA2 = 1;
STEPS = 100;

for i = 1:STEPS
if MOVE_JUNTA1 == 1
theta1 = theta1 + pi/200;
end
if MOVE_JUNTA2 == 1
theta2 = theta2 + pi/200;
end
A1 = matriz_homogenea_DH(theta1,d1,a1,alpha1);
A2 = matriz_homogenea_DH(theta2,d2,a2,alpha2);
T01 = A1;
T02 = T01*A2;
plot_frame(h1, T01);
plot_frame(h2, T02);
pause(0.1);
end
clc, disp('Presione una tecla para terminar.');
pause();
clc, close all;

Figura 20. Resultados de la simulación de la cinemática directa

15
5.3. CINEMÁTICA INVERSA
%**** SIMULACION CINEMATICA INVERSA ****%
clc, clear all, close all;

% ------------------------------------- %
% 1. DEFINICION DE PARÁMETROS DEL ROBOT %
% ------------------------------------- %
L1 = 1;
L2 = 1;

% ------------------------------------- %
% 2. POSICIÓN FINAL DESEADA %
% ------------------------------------- %
xc = 0.4; % xc^2 + yc^2 = 1
yc = sqrt(1-0.4^2);
zc = 1; % Fijo

% ------------------------------------- %
% 3. SISTEMAS COORDENADOS INICIALES %
% ------------------------------------- %
% 3.1. Sistema coordenado inercial
T0 = eye(4);
figure('name', 'Cinematica Directa', 'NumberTitle', 'off');
title('SIMULACIÓN CINEMÁTICA INVERSA');
h0 = plot_frame(T0, 'color', 'k', 'view', 'auto');
axis([-4 4 -4 4 -4 4]);
hold on, grid on;
xlabel('x(m)');
ylabel('y(m)');
% 3.2. Sistema coordenado 1
theta1 = 0;
d1 = L1;
a1 = 0;
alpha1 = pi/2;
A1 = matriz_homogenea_DH(theta1,d1,a1,alpha1);
T01 = A1;
h1 = plot_frame(T01, 'frame', '1', 'color', 'b');
% 3.3. Sistema coordenado 2
theta2 = 0;
d2 = L2;
a2 = 0;
alpha2 = 0;
A2 = matriz_homogenea_DH(theta2,d2,a2,alpha2);
T02 = T01*A2;
h2 = plot_frame(T02, 'frame', '2', 'color', 'g');

% ------------------------------------- %
% 4. PLOTEO DEL PUNTO FINAL %
% ------------------------------------- %
plot3(xc, yc, zc, 'ro', 'MarkerFaceColor', 'k', 'MarkerSize', 10);

% ------------------------------------- %
% 5. CÁLCULO DE ÁNGULOS %
% ------------------------------------- %
D = (xc^2 + yc^2)/L2^2;
if (D < 1 - 1E-15 || D > 1+1E-15 || zc ~= L1)
close all, error('Posición imposible de alcanzar');
else
theta1_final = atan2(xc, -yc);

16
 theta2_final = pi/3;
end

% ------------------------------------- %
% 6. LAZO DE SIMULACIÓN %
% ------------------------------------- %
% 6.1. Mensaje
disp('Presione una tecla para empezar la simulación.');
pause();

% 6.2. Parametrización del avance

STEPS = 400;
THETA1 = linspace(theta1, theta1_final, STEPS);
THETA2 = linspace(theta2, theta2_final, STEPS);

% 6.3. Lazo
for i = 1:STEPS
% 6.3.1. Ángulo instantáneo
theta1 = THETA1(i);
theta2 = THETA2(i);
% 6.3.2. Matrices T
A1 = matriz_homogenea_DH(theta1,d1,a1,alpha1);
A2 = matriz_homogenea_DH(theta2,d2,a2,alpha2);
T01 = A1;
T02 = T01*A2;
% 6.3.3. Ploteo del movimiento
plot_frame(h1, T01);
plot_frame(h2, T02);
% 6.3.4. Pausa por movimiento
pause(0.001);
end

Figura 21. Resultados de la simulación de la cinemática inversa

6. CÁLCULO DE LOS JACOBIANOS

Considerando un manipulador de n-links con juntas 𝑞 = [𝑞1 𝑞2 ⋯ 𝑞𝑛 ]𝑇 y con matriz
de transformación:
0
𝑇𝑛0 (𝑞) = [𝑅𝑛 𝑃𝑛0 ]
0 1

17
La cual representa la transformación del sistema del efector final al sistema inercial. Con
el movimiento del robot las variables de las juntas 𝑞𝑖 , el efector final 𝑜𝑛0 y la orientación
𝑅𝑛0 en función de los ángulos.
Se define:

𝑣𝑛0 = 𝐽𝑣 𝑞̇
𝑤𝑛0 = 𝐽𝑤 𝑞̇

Donde 𝐽𝑣 y 𝐽𝑤 son matrices de 3 × 𝑛. Combinando las dos expresiones anteriores

𝐽
obtenemos el vector de velocidades angulares y lineales ( 𝐽 = [ 𝑣 ]).
𝐽𝑤

𝑣𝑛0
𝜀 = 𝐽𝑞̇ = [ ]
𝑤𝑛0

Cálculo de los jacobianos para el link 𝑖:

(𝑙 ) (𝑙𝑖 ) (𝑙 ) (𝑙𝑖 )
𝐽𝑣 𝑖 = [𝐽𝑣1 𝐽𝑣2𝑖 … … 𝐽𝑣𝑖 0 … … 0] ⋯ (1)
(𝑙 ) (𝑙𝑖 ) (𝑙 ) (𝑙𝑖 )
𝐽𝑤 𝑖 = [𝐽𝑤1 𝐽𝑤2𝑖 … … 𝐽𝑤𝑖 0 … … 0] ⋯ (2)

Donde cada columna de este vector se calcula a partir de:

(𝑙 ) 𝑧𝑗−1 × (𝑝𝑛 − 𝑝𝑗−1 ) 𝑗𝑢𝑛𝑡𝑎𝑠 𝑐𝑖𝑙𝑖𝑛𝑑𝑟𝑖𝑐𝑎𝑠

𝐽𝑣𝑗𝑖 = {
𝑧𝑗−1 𝑗𝑢𝑛𝑡𝑎𝑠 𝑝𝑟𝑖𝑠𝑚á𝑡𝑖𝑐𝑎𝑠
(𝑙𝑖 ) 𝑧𝑗−1 𝑗𝑢𝑛𝑡𝑎𝑠 𝑐𝑖𝑙𝑖𝑛𝑑𝑟𝑖𝑐𝑎𝑠
𝐽𝑤𝑗 = {
0 𝑗𝑢𝑛𝑡𝑎𝑠 𝑝𝑟𝑖𝑠𝑚á𝑡𝑖𝑐𝑎𝑠

Para los motores, su velocidad lineal se representa de la siguiente forma:

𝑚
𝑝̇𝑚𝑖 = 𝐽𝑣 𝑖 𝑞̇

El jacobiano para los motores se representa de la siguiente manera:

𝑚 𝑚 𝑚 𝑖𝑚
𝐽𝑣 𝑖 = [𝐽𝑣1𝑖 𝐽𝑣2𝑖 … . 𝐽𝑣,𝑖−1 0 … 0] ⋯ (3)

Donde:

(𝑚) 𝑧𝑗−1 × (𝑝𝑚 − 𝑝𝑗−1 ) 𝑗𝑢𝑛𝑡𝑎𝑠 𝑐𝑖𝑙𝑖𝑛𝑑𝑟𝑖𝑐𝑎𝑠

𝐽𝑣𝑗 = {
𝑧𝑗−1 𝑗𝑢𝑛𝑡𝑎𝑠 𝑝𝑟𝑖𝑠𝑚á𝑡𝑖𝑐𝑎𝑠

Para los motores, su velocidad angular se representa de la siguiente forma:

𝑚
𝑤𝑚𝑖 = 𝐽𝑤 𝑖 𝑞̇

18
El jacobiano:

𝑚 𝑚 𝑚 𝑚
𝐽𝑤 𝑖 = [𝐽𝑤1𝑖 𝐽𝑤2𝑖 …. 𝐽𝑤,𝑖𝑖 0 … 0]

Donde:
(𝑙 )
(𝑚 ) 𝐽𝑤𝑗𝑖 𝑗 = 1,2, … … . 𝑖 − 1
𝐽𝑤𝑗 𝑖 = { ⋯ (4)
𝑘𝑟𝑖 𝑧𝑚𝑖 𝑗=𝑖

Figura 1. Posición de un motor en un link

6.1. CÁLCULO DE LOS JACOBIANOS DE LOS LINK

19
 Tabla 1. Parámetros DH
JUNTA 𝜽𝒊 𝒅𝒊 𝒂𝒊 𝜶𝒊
𝜋
1 𝜃1 𝐿1 0
2
2 𝜃2 𝐿2 0 0

Tabla 2. Dimensiones del robot

𝑳𝟏
𝑳𝟐

Figura 2. Dimensionamiento del manipulador

A partir de la cinemática, calculamos las coordenadas del centro de masa de los links.

20
Figura 3. Ubicación del centro de masa del link 1

21
 Figura 4. Ubicación del centro de masa del link 2

 Cálculo de la posición del 𝐶𝑀1

1 1]𝑇
𝑟𝑐𝑚 1
= [0 −𝐿𝐶𝑀1𝑦 𝐿𝐶𝑀1𝑧

0
𝑟𝑐𝑚 1
= 𝑇10 . 𝑟𝑐𝑚
1
1
= [𝐿𝐶𝑀1𝑧 ∙ sin 𝜃1 −𝐿𝐶𝑀1𝑧 ∙ cos 𝜃1 𝐿1 − 𝐿𝐶𝑀1𝑦 1]𝑇

 Cálculo de la posición del 𝐶𝑀2

22
 2 0 −𝐿𝐶𝑀2𝑧 1]𝑇
𝑟𝑐𝑚 2
= [𝐿𝐶𝑀2𝑥
(𝐿2 − 𝐿𝐶𝑀2𝑧 ) ∙ sin 𝜃1 + 𝐿𝐶𝑀2𝑥 ∙ cos 𝜃1 ∙ cos 𝜃2
−(𝐿2 − 𝐿𝐶𝑀2𝑧 ) ∙ cos 𝜃1 + 𝐿𝐶𝑀2𝑥 ∙ sin 𝜃1 ∙ cos 𝜃2
0
𝑟𝑐𝑚 2
= 𝑇20 . 𝑟𝑐𝑚
2
2
=[ ]
𝐿1 + 𝐿𝐶𝑀2𝑥 ∙ sin 𝜃2
1

 Jacobianos de los link

Haciendo uso de las ecuaciones (1) y (2) con los valores hallados previamente tenemos:

0 0 𝐿𝐶𝑀1𝑧 ∙ cos 𝜃1 0
𝐽𝑤1 = [0 0] 𝐽𝑣𝐶𝑀1 = [ 𝐿𝐶𝑀1𝑧 ∙ sin 𝜃1 0]
1 0 0 0
𝐴 𝐶
𝐽𝑣𝐶𝑀2 = [𝐵 𝐷 ]
0 𝐵 ∙ cos 𝜃1 − 𝐴 ∙ sin 𝜃1
0 sin 𝜃1
𝐽𝑤2 = [0 − cos 𝜃1 ] 𝐴 = (𝐿2 − 𝐿𝐶𝑀2𝑧 ) ∙ cos 𝜃1 − 𝐿𝐶𝑀2𝑥 ∙ cos 𝜃2 ∙ sin 𝜃1
1 0 𝐵 = (𝐿2 − 𝐿𝐶𝑀2𝑧 ) ∙ sin 𝜃1 + 𝐿𝐶𝑀2𝑥 ∙ cos 𝜃2 ∙ cos 𝜃1
𝐶 = −𝐿𝐶𝑀2𝑥 ∙ cos 𝜃1 ∙ sin 𝜃2
𝐷 = −𝐿𝐶𝑀2𝑥 ∙ sin 𝜃1 ∙ sin 𝜃2

6.2. CÁLCULO DE LOS JACOBIANOS DE LOS MOTORES

Figura. Posición de los centros de masa de los motores

 Posición del centro de masa del primer motor

𝑟𝑚0 1 = [0 0 −𝐿𝑀1 1]𝑇

 Posición del centro de masa del segundo motor

1 −𝐿𝑀2 1]𝑇
𝑟𝑐𝑚 2
= [0 0

𝑟𝑚0 2 = 𝑇10 . 𝑟𝑚1 2 = [−𝐿𝑀2 ∙ sin 𝜃1 𝐿𝑀2 ∙ cos 𝜃1 𝐿1 1]𝑇

 Jacobianos de los motores

Haciendo uso de las ecuaciones (3) y (4) con los valores hallados previamente tenemos:

0 0 0 0
𝐽𝑤𝑀1 = [ 0 0] 𝐽𝑣𝑀1 = [0 0]
𝐾𝑟1 0 0 0

0 𝐾𝑟2 ∙ sin 𝜃1 −𝐿𝑀2 ∙ cos 𝜃1 0

𝐽𝑤𝑀2 = [0 𝐾𝑟2 ∙ cos 𝜃1 ] 𝐽𝑣𝑀2 = [ −𝐿𝑀2 ∙ sin 𝜃1 0]
1 0 0 0

23
7. DINÁMICA
7.1. CÁLCULO DE LOS CENTROS DE MASA
 Link 1

Figura 5. Vista isométrica del link 1 y la ubicación de su centro de masa

Tabla 3. Parámetros del Link 1

𝑳𝑪𝑴𝟏𝒚
𝑳𝑪𝑴𝟏𝒛
𝑴𝒂𝒔𝒂

24
 Link 2

Figura 6. Vista isométrica del link 2 y la ubicación de su centro de masa

Tabla 4. Parámetros del Link 2

𝑳𝑪𝑴𝟐𝒙
𝑳𝑪𝑴𝟐𝒛
𝑴𝒂𝒔𝒂

7.2. CÁLCULO DE INERCIAS

25
 Link 1

Figura 7. Tensor de inercia del link 1 – ver parámetros L

 Link 2

Figura 8. Tensor de inercia del link 2 – ver parámetros L

26
7.3. MÉTODO DE LAGRANGE
El análisis del modelo dinámico puede ser útil para diseño y cálculo de las fuerzas y
pares requerido para la ejecución de movimientos típicos, proporciona información útil
para diseñando juntas, transmisiones y actuadores. Para ello usaremos el método que
se basa en la formulación de Lagrange y es conceptualmente simple y sistemático.
El modelo dinámico de un manipulador proporciona una descripción de la relación entre
los pares de accionamiento del conjunto y el movimiento de la estructura. Con la
formulación de Lagrange. El Lagrangiano del sistema mecánico se puede definir como:
𝐿 =𝑇−𝑈
Donde T y U denotan la energía cinética y potencial del sistema. La ecuación de
Lagrange puede ser expresada por:
𝑑 𝜕𝐿 𝜕𝐿
− = 𝜀𝑖 𝑃𝑎𝑟𝑎 𝑖 = 1, 2, ⋯ , 𝑛
𝑑𝑡 𝜕𝑞𝑖̇ 𝜕𝑞𝑖
Donde 𝜀𝑖 es la fuerza generalizada asociada con el movimiento generalizado 𝑞𝑖 , esta
ecuación puede ser escrita de manera compacta de la forma:

𝑑 𝜕𝐿 𝑇 𝜕𝐿 𝑇
( ) −( ) =𝜀 ⋯ (5)
𝑑𝑡 𝜕𝑞̇ 𝜕𝑞

Las contribuciones a las fuerzas generalizadas están dadas por las fuerzas no
conservativas, es decir, la articulación, pares de torsión del actuador, pares de fricción
de la junta, así como pares de torsión inducidos por fuerzas efectores finales en el
contacto con el medio ambiente. La ecuación anterior establece la relación existente
entre el vector de fuerzas aplicadas al manipulador y las posiciones de las
articulaciones, las velocidades y aceleraciones. Por lo tanto, permiten la derivación del
modelo dinámico del manipulador a partir de la determinación de la energía cinética y
potencial del sistema mecánico.
7.3.1. Cálculo de la energía cinética
Considerando un manipulador con 𝑛 juntas, la energía cinética total está dada por:
𝑛

𝑇 = ∑(𝑇𝑙𝑖 + 𝑇𝑚𝑖 ) ⋯ (6)

𝑖=1

Donde 𝑇𝑙𝑖 es la energía cinética de cada link 𝑖 y 𝑇𝑚𝑖 es la energía cinética de los motores
de las juntas 𝑖. Luego la contribución de la energía cinética de los links es:
1 1
𝑇𝑙𝑖 = 𝑚𝑙𝑖 . 𝑝𝑙̇ 𝑖 𝑇 . 𝑝̇ 𝑙𝑖 + 𝑤𝑖 𝑇 . 𝑅𝑖 . 𝐼 𝑖 𝑙𝑖 . 𝑅𝑖 𝑇 . 𝑤𝑖
2 2
Además:

𝑝𝑙̇ 𝑖 = 𝐽(𝑙𝑖) 𝑣 . 𝑞̇
𝑤̇ 𝑖 = 𝐽(𝑙𝑖) 𝑤 . 𝑞̇

Donde 𝐽(𝑙𝑖) 𝑣 y 𝐽(𝑙𝑖) 𝑤 son los jacobianos del link 𝑖.

27
Luego la energía cinética del rotor 𝑖 es:
1 1
𝑇𝑚𝑖 = 𝑚𝑚𝑖 . 𝑝𝑚̇ 𝑖 𝑇 . 𝑝𝑚̇ 𝑖 + 𝑤𝑚𝑖 𝑇 . 𝐼𝑚𝑖 . 𝑤𝑚𝑖 ⋯ (7)
2 2

Donde 𝑚𝑚𝑖 es la masa del rotor y 𝑝𝑚̇ 𝑖 la velocidad lineal del centro de masa del rotor,
𝐼𝑚𝑖 es el tensor de inercia del rotor relativo al centro de masa y 𝑤𝑚𝑖 la velocidad angular
del rotor. Además:

𝑝̇𝑚𝑖 = 𝐽(𝑚𝑖) 𝑣 . 𝑞̇
𝑤̇𝑚𝑖 = 𝐽(𝑚𝑖) 𝑤 . 𝑞̇

Donde 𝐽(𝑚𝑖) 𝑣 y 𝐽(𝑚𝑖) 𝑤 son los jacobianos del motor 𝑖.

7.3.2. Cálculo de la energía cinética para eslabones

La energía cinética de cada link se representa de la siguiente manera:
1 𝑇 1 𝑇
𝑇𝑙𝑖 = 𝑚𝑙𝑖 𝑞̇ 𝑇 𝐽𝑣 (𝑙𝑖) 𝐽𝑣 (𝑙𝑖) 𝑞̇ + 𝑚𝑙𝑖 𝑞̇ 𝑇 𝐽𝑤 (𝑙𝑖) 𝑅𝑖 𝐼𝑙𝑖 𝑖 𝑅𝑖 𝑇 𝐽𝑤 (𝑙𝑖) 𝑞̇ ⋯ (8)
2 2

7.3.3. Cálculo de la energía cinética para motores

Asimismo la energía cinética contribuida por cada motor se representa de la siguiente
manera:
1 𝑇 1 𝑇
𝑇𝑚𝑖 = 𝑚𝑚𝑖 𝑞̇ 𝑇 𝐽𝑣 (𝑚𝑖) 𝐽𝑣 (𝑚𝑖) 𝑞̇ + 𝑚𝑚𝑖 𝑞̇ 𝑇 𝐽𝑤 (𝑚𝑖) 𝑅𝑚𝑖 𝐼𝑚𝑖 𝑚𝑖 𝑅𝑚𝑖 𝑇 𝐽𝑤 (𝑚𝑖) 𝑞̇ ⋯ (9)
2 2

Matriz de rotación para llevar la inercia de los motores al sistema base.

Figura. Matriz de transformación del momento de inercia del motor 𝑚𝑖

28
Se debe tener en cuenta de que la matriz de rotación del motor con respecto a tierra es
diferente a la matriz de rotación del link 𝑖 con respecto a tierra, es decir:
0
𝑅𝑚𝑖 ≠ 𝑅𝑙𝑖0

Para encontrar la matriz de rotación adecuada se plantea:

𝑖−1
𝑅𝑚𝑖 = 𝑅𝑜𝑡𝑧,𝜃𝑚𝑖 = 𝑅𝑜𝑡𝑧, 𝑞𝑖
𝑘𝑟𝑖

Para finalmente por una composición respecto al eje actual se obtendrá:

0 0 𝑖−1 0
𝑅𝑚𝑖 = 𝑅𝑖−1 . 𝑅𝑚𝑖 = 𝑅𝑖−1 . 𝑅𝑜𝑡𝑧, 𝑞𝑖 ⋯ (10)
𝑘𝑟𝑖

Luego de la aclaración anterior finalmente calculamos la suma de las contribuciones de

cada link y de cada rotor, la energía cinética total del manipulador está definida con la
siguiente forma cuadrática:
𝑛 𝑛
1 1
𝑇 = ∑ ∑ 𝑑𝑖𝑗 (𝑞)𝑞̇ 𝑖 𝑞̇ 𝑗 = 𝑞̇ 𝑇 𝐷(𝑞)𝑞̇
2 2
𝑖=1 𝑗=1

Donde:
𝑛
𝑇 𝑇 𝑇
𝐷(𝑞) = ∑(𝑚𝑙𝑖 𝐽𝑣 (𝑙𝑖) 𝐽𝑣 (𝑙𝑖) + 𝐽𝑤 (𝑙𝑖) 𝑅𝑖 𝐼𝑙𝑖 𝑖 𝑅𝑖 𝑇 𝐽𝑤 (𝑙𝑖) + 𝑚𝑚𝑖 𝐽𝑣 (𝑚𝑖) 𝐽𝑣 (𝑚𝑖)
𝑖=1
⋯ (11)
(𝑚𝑖 ) 𝑇 𝑚𝑖 𝑇 (𝑚𝑖 )
+ 𝐽𝑤 𝑅𝑚𝑖 𝐼𝑚𝑖 𝑅𝑚𝑖 𝐽𝑤 )

7.3.4. Cálculo de la energía potencial

De la misma manera que se vio en la energía cinética, la energía potencial será la suma
de 2 contribuciones, una relativa al link y otra relativa al motor.
𝑛

𝑈 = ∑(𝑈𝑙𝑖 + 𝑈𝑚𝑖 ) ⋯ (12)

𝑖=1

Asumiendo eslabones rígidos, la contribución debida solo a la fuerza gravitacional es

definida como:
.
𝑈𝑙𝑖 = − ∫ 𝑔𝑜 𝑇 𝑝∗ 𝑖 𝜌𝑑𝑉 = −𝑚𝑙𝑖 𝑔𝑜 𝑇 𝑝𝑙𝑖
𝑣𝑙 𝑖

De la misma manera para la contribución de los rotores:

𝑈𝑚𝑖 = −𝑚𝑚𝑖 𝑔𝑜 𝑇 𝑝𝑚𝑖

Entonces la energía potencial total será:

𝑛

𝑈 = − ∑(𝑚𝑙𝑖 ∗ 𝑔0𝑇 ∗ 𝑝𝑙𝑖 + 𝑚𝑚𝑖 ∗ 𝑔0𝑇 ∗ 𝑝𝑚𝑖 ) ⋯ (13)

𝑖=1

29
Podemos notar que esta es solo una función de las posiciones y no de las velocidades.
7.4. ECUACIONES DEL MOVIMIENTO
Entonces reemplazando nuestros valores en el Lagrangiano:
𝐿(𝑞, 𝑞̇ ) = 𝑇(𝑞, 𝑞̇ ) − 𝑈(𝑞)
Aplicamos el método de Euler – Lagrange y tendremos:
𝑑 𝜕𝐿 𝜕𝐿
( )− = 𝜀𝑘
𝑑𝑡 𝜕𝑞𝑘̇ 𝜕𝑞𝑘
Primer término:
𝑛 𝑛 𝑛 𝑛 𝑛
𝑑 𝑑𝐿 𝑑 𝑑𝑇 𝑑𝐷𝑘𝑗 (𝑞) 𝑑𝐷𝑘𝑗 (𝑞)
( )= ( ) = ∑ 𝐷𝑘𝑗 (𝑞). 𝑞𝑗̈ + ∑ . 𝑞̇ 𝑗 = ∑ 𝐷𝑘𝑗 (𝑞). 𝑞𝑗̈ + ∑ ∑ . 𝑞̇ 𝑖 𝑞̇ 𝑗
𝑑𝑡 𝑑𝑞𝑘̇ 𝑑𝑡 𝑑𝑞𝑘̇ 𝑑𝑡 𝑑𝑞𝑖
𝑗=1 𝑗=1 𝑗=1 𝑖=1 𝑗=1

Segundo término:
𝑛 𝑛
𝑑𝑇 1 𝑑𝐷𝑖𝑗 (𝑞)
( ) = ∑∑ . 𝑞̇ 𝑖 𝑞̇ 𝑗
𝑑𝑞𝑖̇ 2 𝑑𝑞𝑘
𝑗=1 𝑘=1
𝑛 𝑛
𝑑𝑈 𝑑𝑝𝑙𝑗 𝑑𝑝𝑚𝑗 (𝑙 ) (𝑚 )
( ) = − ∑ (𝑚𝑙𝑗 . 𝑔0 𝑇 + 𝑚𝑚𝑗 . 𝑔0 𝑇 ) = − ∑ (𝑚𝑙𝑗 . 𝑔0 𝑇 𝐽𝑣𝑖𝑗 + 𝑚𝑚𝑗 . 𝑔0 𝑇 𝐽𝑤𝑖 𝑗 ) = 𝑔𝑘 (𝑞)
𝑑𝑞𝑘̇ 𝑑𝑞𝑘 𝑑𝑞𝑘
𝑗=1 𝑗=1

𝑛 𝑛
𝑑𝐿 1 𝑑𝐷𝑖𝑗 (𝑞)
( ) = ∑∑ . 𝑞̇ 𝑖 𝑞̇ 𝑗 + 𝑔𝑘 (𝑞)
𝑑𝑞𝑘̇ 2 𝑑𝑞𝑘
𝑗=1 𝑘=1

Finalmente, reemplazando:
𝑛 𝑛 𝑛

∑ 𝐷𝑘𝑗 (𝑞). 𝑞𝑗̈ + ∑ ∑ ℎ𝑘𝑗𝑖 (𝑞). 𝑞̇ 𝑖 𝑞̇ 𝑗 + 𝑔𝑘 (𝑞) = 𝜀𝑘 𝑘 = 1,2, … . , 𝑛

𝑗=1 𝑖=1 𝑗=1

Donde:
𝑑𝐷𝑘𝑗 (𝑞) 1 𝑑𝐷𝑖𝑗 (𝑞)
ℎ𝑘𝑗𝑖 (𝑞) = −
𝑑𝑞𝑖 2 𝑑𝑞𝑘
El lagrangiano puede ser escrito de la siguiente forma despreciando la fricción y fuerza
de contacto externo:
𝐷(𝑞). 𝑞⃛ + 𝐶(𝑞, 𝑞̇ )𝑞̈ + 𝑔𝑘 (𝑞) = 𝜏
Donde 𝐶(𝑞, 𝑞̇ ) se calcula de la siguiente forma:
𝑛

𝐶𝑘𝑗 (𝑞) = ∑ 𝐶𝑖𝑗𝑘 . 𝑞̇ 𝑖

𝑖=1

1 𝑑𝐷𝑘𝑗 𝑑𝐷𝑘𝑖 𝑑𝐷𝑖𝑗

𝐶𝑖𝑗𝑘 = [ + − ]
2 𝑑𝑞𝑖 𝑑𝑞𝑗 𝑑𝑞𝑘

30
7.5. DINÁMICA DE NUESTRO MANIPULADOR
Tenemos los siguientes parámetros del manipulador de 2 GDL.
Tabla 5. Parámetros del robot de 2 GDL

 Matriz de inercia de translación (link 1)

2
𝐷1𝑣 = 𝑚1 ∙ 𝐽𝑣𝑐1 𝑇 ∙ 𝐽𝑣𝑐1 = [𝐿𝐶𝑀1𝑧 ∙ 𝑚1 0]
0 0
 Matriz de inercia de translación (link 2)

𝐷2𝑣 = 𝑚2 ∙ 𝐽𝑣𝑐2 𝑇 ∙ 𝐽𝑣𝑐2

((𝐿 − 𝐿𝐶𝑀2𝑧 )2 + 𝐿2𝐶𝑀2𝑥 ∙ cos2 𝜃2 ) ∗ 𝑚2 −𝐿𝐶𝑀2𝑥 ∙ 𝑚2 ∙ sin 𝜃2 ∙ (𝐿2 − 𝐿𝐶𝑀2𝑧 )

𝐷2𝑣 = [ 2 ]
−𝐿𝐶𝑀2𝑥 ∙ 𝑚2 ∙ sin 𝜃2 ∙ (𝐿2 − 𝐿𝐶𝑀2𝑧 ) 𝐿𝐶𝑀2𝑥 2 ∙ 𝑚2

 Matriz de inercia de translación (motor 1)

𝐷𝑚1𝑣 = 𝑚𝑚1 ∙ 𝐽𝑣𝑚1 𝑇 ∙ 𝐽𝑣𝑚1

0 0
𝐷𝑚1𝑣 = [ ]
0 0
 Matriz de inercia de translación (motor 2)

𝐷𝑚2𝑣 = 𝑚𝑚2 ∙ 𝐽𝑣𝑚2 𝑇 ∙ 𝐽𝑣𝑚2

2
𝐷𝑚2𝑣 = [𝐿𝑚2 ∙ 𝑚𝑚2 0]
0 0
 Matriz de inercia rotacional (link 1)
𝐼𝑥𝑥1 0 0
𝐼̃1 = [ 0 𝐼𝑦𝑦1 𝐼𝑦𝑧1 ]
0 𝐼𝑦𝑧1 𝐼𝑧𝑧1

𝐼1 = 𝑅10 ∙ 𝐼̃1 ∙ (𝑅10 )𝑇

𝐼𝑥𝑥1 − 𝐼𝑧𝑧1
𝐼𝑥𝑥1 ∙ cos2 𝜃1 + 𝐼𝑧𝑧1 ∙ sin2 𝜃1 sin 2𝜃1 ∙ ( ) 𝐼𝑦𝑧1 ∙ sin 𝜃1
2
𝐼1 = 𝐼𝑥𝑥1 − 𝐼𝑧𝑧1
sin 2𝜃1 ∙ ( ) 𝐼𝑥𝑥1 ∙ sin2 𝜃1 + 𝐼𝑧𝑧1 ∙ 𝑐𝑜𝑠 2 𝜃1 −𝐼𝑦𝑧1 ∙ cos 𝜃1
2
[ 𝐼𝑦𝑧1 ∙ sin 𝜃1 −𝐼𝑦𝑧1 ∙ cos 𝜃1 𝐼𝑦𝑦1 ]

𝑇 𝐼𝑦𝑦1 0
𝐷1𝑤 = 𝐽𝑤1 ∙ 𝐼1 ∙ 𝐽𝑤1 = [ ]
0 0
 Matriz de inercia rotacional (link 2)
𝐼𝑥𝑥2 0 𝐼𝑥𝑧2
̃
𝐼2 = [ 0 𝐼𝑦𝑦2 0 ]
𝐼𝑥𝑧2 0 𝐼𝑧𝑧2

𝐼2 = 𝑅20 ∙ 𝐼̃2 ∙ (𝑅20 )𝑇

31
 𝑇 𝐼𝑥𝑥2 ∙ sin2 𝜃2 + 𝐼𝑦𝑦2 ∙ cos 2 𝜃2 𝐼𝑥𝑧2 ∙ sin 𝜃2
𝐷2𝑤 = 𝐽𝑤2 ∙ 𝐼2 ∙ 𝐽𝑤2 = [ ]
𝐼𝑥𝑧2 ∙ sin 𝜃2 𝐼𝑧𝑧2

 Matriz de inercia rotacional (motor 1)

𝐼𝑚1𝑥𝑥 0 0
𝐼𝑚1 = [ 0 𝐼𝑚1𝑦𝑦 0 ]
0 0 𝐼𝑚1𝑧𝑧

𝐼𝐼𝑚1 = 𝑅1 ∙ 𝐼𝑚1 ∙ 𝐼𝑚1 𝑇

𝐷𝑚1𝑤 = 𝐽𝑤𝑚1 𝑇 ∙ 𝐼𝐼𝑚1 ∙ 𝐽𝑤𝑚1

2
𝐼𝑚1𝑦𝑦 ∙ 𝑘𝑟1 0
𝐷𝑚1𝑤 = [ ]
0 0
 Matriz de inercia rotacional (motor 2)
𝐼𝑚2𝑥𝑥 0 0
𝐼𝑚2 =[ 0 𝐼𝑚2𝑦𝑦 0 ]
0 0 𝐼𝑚2𝑧𝑧

𝐼𝐼𝑚2 = 𝑅2 ∙ 𝐼𝑚2 ∙ 𝐼𝑚2 𝑇

𝐷𝑚2𝑤 = 𝐽𝑤𝑚2 𝑇 ∙ 𝐼𝐼𝑚2 ∙ 𝐽𝑤𝑚2

𝐼𝑚2𝑦𝑦 ∙ cos 2 𝜃2 + 𝐼𝑚2𝑥𝑥 ∙ sin2 𝜃2 0
𝐷𝑚2𝑤 = [ 2
]
0 𝐼𝑚2𝑧𝑧 ∙ 𝑘𝑟2
Luego en base a estos hallaremos la matriz de inercia total, que es la suma de los
valores calculados anteriormente:
𝐷 = 𝐷1𝑣 + 𝐷2𝑣 + 𝐷𝑚1𝑣 + 𝐷𝑚2𝑣 + 𝐷1𝑤 + 𝐷2𝑤 + 𝐷𝑚1𝑤 + 𝐷𝑚2𝑤
2 2
𝐷11 = −2 ∙ 𝐿𝐶𝑀2𝑧 ∙ 𝑚2 ∙ 𝐿2 + 𝐿𝐶𝑀2𝑥 ∙ sin 𝜃2 ∙ 𝑚2 + 𝐼2𝑦𝑦 ∙ sin2 𝜃2 + 𝐼2𝑥𝑥 ∙ sin2 𝜃2 + 𝐼𝑚2𝑥𝑥 ∙ sin2 𝜃2
− 𝐼𝑚2𝑦𝑦 ∙ sin2 𝜃2 + 𝐿𝑚22 ∗ 𝑚𝑚2 + 𝐿𝑐𝑚2𝑧 2 ∙ 𝑚2 + 𝐿𝑐𝑚2𝑥 2 ∙ 𝑚2 + 𝐿𝑐𝑚1𝑧 2 ∙ 𝑚1 + 𝐿22 ∙ 𝑚2
+ 𝐼𝑚1𝑦𝑦 ∙ 𝑘𝑟12 + 𝐼𝑦𝑦1 + 𝐼𝑦𝑦2 + 𝐼𝑚2𝑦𝑦

𝐷12 = 𝐷21 = 𝐼𝑥𝑧2 ∙ sin 𝜃2 − 𝐿𝐶𝑀2𝑥 ∙ 𝑚2 ∙ (𝐿2 − 𝐿𝐶𝑀2𝑧 ) ∙ sin 𝜃2

𝐷22 = 𝐿𝐶𝑀2𝑥 2 ∗ 𝑚2 + 𝐼𝑧𝑧2 + 𝐼𝑚2𝑧𝑧 ∗ 𝑘𝑟22

Para calcular la matriz C, tenemos que usar la siguiente formula:

𝑛

𝑐𝑖𝑗 = ∑ 𝑐𝑖𝑗𝑘 ∗ 𝑞𝑘̇

𝑘=1

Donde:
1 𝜕𝐷𝑖𝑗 𝜕𝐷𝑖𝑘 𝜕𝐷𝑗𝑘
𝑐𝑖𝑗𝑘 = ( + − )
2 𝜕𝑞𝑘 𝜕𝑞𝑗 𝜕𝑞𝑖
Entonces:

32
 (𝐼𝑥𝑥2 + 𝐼𝑦𝑦2 + 𝐼𝑚𝑦𝑦2 + −𝐼𝑚𝑥𝑥2 + 𝐿𝐶𝑀2𝑥 2 ∗ 𝑚2 ) ∗ 𝑆(2 ∗ θ2 ) ∗ 𝑞2̇ /2 𝐶12
𝐶=[ ]
(−𝐼𝑥𝑥2 + 𝐼𝑦𝑦2 + 𝐼𝑚𝑦𝑦2 + −𝐼𝑚𝑥𝑥2 + 𝐿𝐶𝑀2𝑥 2 ∗ 𝑚2 ) ∗ 𝑆(2 ∗ θ2 ) ∗ 𝑞1̇ /2 0

𝐶12 = (−𝐼𝑥𝑥2 + 𝐼𝑦𝑦2 + 𝐼𝑚𝑦𝑦2 − 𝐼𝑚𝑥𝑥2 + 𝐿𝐶𝑀2𝑥 2 ∗ 𝑚2 ) ∗ 𝑆(2 ∗ θ2 ) ∗ 𝑞1̇ /2 + (𝐼𝑥𝑧2

− 𝐿2 ∗ 𝐿𝐶𝑀2𝑥 ∗ 𝑚2 + 𝐿𝐶𝑀2𝑥 ∗ 𝐼𝐶𝑀2𝑧 ∗ 𝑚2 ) ∗ 𝐶(θ2 ) ∗ 𝑞2̇

Por último, calcularemos el efecto de la energía potencial en la dinámica del

manipulador:

𝑔1 = 0

𝑔2 = 𝐿𝑐𝑚2𝑥 ∗ 𝑔 ∗ 𝑚2 ∗ 𝐶θ2

Luego:
0
𝑔=[ ]
𝐿𝑐𝑚2𝑥 ∗ 𝑔 ∗ 𝑚2 ∗ 𝐶θ2

Observaciones:
 Se observó en el cálculo de la dinámica que esta no depende de IL1X , IL1y
IL2X, IL2y, Im1xx, Im2xx, Im1yy, Im2yy

6.6. OBTENCIÓN DEL MODELO DINÁMICO DE UN MOTOR DC

6.6.1. JACOBIANOS

% Demo_01
% Dinamica de un manipulador planar de 2GDL
clc; clear all; close all
%-------------------------------------------------------------------%
% 1. DEFINICION DE VARIABLES SIMBOLICAS
%---------------------------------------------------------------------
-%
syms theta1 d1 a1 alpha1
syms theta2 d2 a2 alpha2
syms q1 q2
syms dq1 dq2
syms L1 L2
syms m1 m2 mm1 mm2
syms Lcm1y Lcm1z Lcm2x Lcm2z Lm1 Lm2
syms Ixx1 Iyy1 Izz1 Iyz1 Ixx2 Iyy2 Izz2 Ixz2
syms Imxx2 Imyy2 Imzz2 Imxx1 Imyy1 Imzz1
syms g
syms Kr1 Kr2
q = [q1; q2];

33
dq = [dq1; dq2];
n = 2;

%---------------------------------------------------------------------
-%
% 2. SISTEMAS COORDENADOS Y CINEMATICA
%---------------------------------------------------------------------
-%
% 2.1. SISTEMA COORDENADO 1
% -> Parametros D-H
theta1 = q1;
d1 = L1;
a1 = 0;
alpha1 = pi/2;
% -> Matriz A
A1 = matriz_homogenea_DH(theta1, d1,a1,alpha1);
% -> MATRIZ T
T01 = A1

% 2.2. SISTEMA COORDENADO 2

% -> Parametros D-H
theta2 = q2;
d2 = L2;
a2 = 0;
alpha2 = 0;
% -> Matriz A
A2 = matriz_homogenea_DH(theta2, d2,a2,alpha2);
% -> MATRIZ T
T02 = T01*A2;
T02 = simplify(T02)

%---------------------------------------------------------------------
-%
% 3. CALCULO DE LOS JACOBIANOS
%---------------------------------------------------------------------
-%
% 3.1. DEFINICION DE ALGUNOS PARAMETROS
% -> Origenes de coordenadas

34
p0 = [0;0;0];
p1 = T01(1:3,4)
% -> Vectores unitarios "z"
z0 = [0;0;1];
z1 = T01(1:3,3);
% 3.2. CALCULO DE LAS COORDENADAS DE LOS C.M
% -> Del eslabon 1
pCM1 = [0; -Lcm1y; Lcm1z; 1];
pCM1 = T01*pCM1;
pCM1 = simplify( pCM1(1:3) );
disp('pCM1')
disp(pCM1)
% -> Del eslabon 2
pCM2 = [Lcm2x; 0; -Lcm2z; 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) ]
% -> 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)) ]
% -> Calculo de "Jw"
Jw2 = [ z0 z1 ]
% 3.5. CALCULO DE LOS JACOBIANOS DE LOS MOTORES
pM1 = [0; 0; -Lm1];
pM2 = [0; 0; -Lm2; 1];
pM2 = simplify(T01*pM2);
pM2 = pM2(1:3);
Jvm1 = [ cross(z0, (pM1-p0)) zeros(3,1) ]
Jwm1 = [ z0*Kr1 zeros(3,1) ]
Jvm2 = [ cross(z0, (pM2-p0)) cross(z1, (pM2-p1)) ]
Jwm2 = [ z0 z1*Kr2 ]
%---------------------------------------------------------------------
-%
% 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);
% 4.3. MOTOR "1"
Dm1v = mm1*transpose(Jvm1)*Jvm1;
Dm1v = simplify(Dm1v);
% 4.4. MOTOR "2"
Dm2v = mm2*transpose(Jvm2)*Jvm2;
Dm2v = simplify(Dm2v);
%---------------------------------------------------------------------
-%
% 5. CALCULO DE LA MATRIZ DE INERCIA (COMPONENTE ROTACIONAL)
%---------------------------------------------------------------------
-%
% 5.1. ESLABON "1"
% -> Tensor de inercia en el centro de masa

35
I1 = [Ixx1 0 0;0 Iyy1 Iyz1; 0 Iyz1 Izz1];
% -> 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 = [Ixx2 0 Ixz2;0 Iyy2 0; Ixz2 0 Izz2];
% -> 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);
% 5.3. MOTORES
% -> Tensor de inercia en el centro de masa
Im1 = [Imxx1 0 0; 0 Imyy1 0; 0 0 Imzz1];
Im2 = [Imxx2 0 0; 0 Imyy2 0; 0 0 Imzz2];
IIm1 = simplify(R1*Im1*transpose(R1));
IIm2 = simplify(R2*Im2*transpose(R2));
Dwm1 = transpose(Jwm1)*IIm1*Jwm1;
Dwm1 = simplify(Dwm1);
Dwm2 = transpose(Jwm2)*IIm2*Jwm2;
Dwm2 = simplify(Dwm2);
%---------------------------------------------------------------------
-%
% 6. CALCULO DE LA MATRIZ "D"
%---------------------------------------------------------------------
-%
% 6.1. SUMA DE LOS TERMINOS HALLADOS ANTERIORMENTE
D = D1v + D2v + D1w + D2w + Dm1v + Dm2v + Dwm1 + Dwm2;
D = simplify(D);
% 6.2. MENSAJES
disp('**********')
disp(' MATRIZ D ')
disp('**********')
for i=1:n
for j=1:n
fprintf('D%d%d: \n', i, j)
pretty( D(i,j))
end
end

%---------------------------------------------------------------------
-%
% 7. CALCULO DE LA MATRIZ "C"
%---------------------------------------------------------------------
-%
% 7.1. CALCULO DE LOS TERMINOS "Cijk"
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

36
 cijk{i,j,k} = 1/2*( term_01 + term_02 - term_03);
end
end
end
% cijk{1,1,2} es igual a c112 ???
% 7.2. MATRIZ "C"
disp('**********')
disp(' MATRIZ C ')
disp('**********')
C = cell(n,n);
for k=1:n
for j=1:n
% Calculo de "ckj"
sum = 0;
for i=1:n
sum = sum + cijk{i,j,k}*dq(i);
end
C{k,j} = simplify(sum);
% Mensajes
fprintf('C%d%d:\n', k, j)
pretty( C{k,j} )
end
end

%---------------------------------------------------------------------
-%
% 8. CALCULO DE LA MATRIZ "g"
%---------------------------------------------------------------------
-%
% 8.1.CALCULO DE LA ENERGIA POTENCIAL
P1 = m1*g*(L1-Lcm1z)
P2 = m2*g*(L1+Lcm2x*sin(q2))
P1M= mm1*[0;0;g].'*pM1
P2M= mm2*[0;0;g].'*pM2
P = P1 + P2 +P1M+P2M
% 8.2. HALLAMOS "gi"
disp('**********')
disp(' MATRIZ g ')
disp('**********')
for i=1:n
a = diff(P, q(i));
a = simplify(a);
% Mensajes
fprintf('g%d: \n', i)
pretty(a)
end

37
6.6.2. RESULTADOS:

JACOBIANO DE LOS MOTRES

MOTOR 1 MOTOR 2

MATRIZ D

38
 MATRIZ C

MATRIZ g

Curso: 39
Análisis y Control de Robots (MT - 517)
8. OBTENCIÓN DEL MODELO DINÁMICO DE UN MOTOR DC
8.1. Obtención de parámetros
Con este método obtendremos el modelo dinámico de un motor dc aplicando solamente
leyes físicas.
Tabla 1. Parámetros de un motor DC

Parámetros Descripción Unidades SI

𝑅𝑎 Resistencia de armadura Ω

𝐿𝑎 Inductancia de armadura 𝐻

𝐽 Momento de inercia rotacional 𝐾𝑔 ∙ 𝑚2

𝑁∙𝑚∙𝑠
𝐵 Coeficiente de fricción viscosa
𝑟𝑎𝑑
𝑁∙𝑚
𝐾𝑡 Constante de torque
𝐴
𝑉∙𝑠
𝐾𝑏 Constante electromotriz
𝑟𝑎𝑑

El funcionamiento del motor DC se basa en una serie de bobinas enrolladas en un

núcleo ferromagnético expuestas en un campo magnético. Para el caso de los motores
DC de imán permanente, el campo magnético es generado por imanes permanentes
(como dice su nombre), por lo que al aplicar una diferencia de tensión a la bobina se
genera una fuerza magnetomotriz que hace generar un movimiento mecánico…
mientras que el sentido rotacional lo da la flecha.
8.1.1. Análisis eléctrico
Analicemos un motor DC de imán permanente como la que se muestra en la figura 1.

Figura 1. Modelamiento de un motor DC de imán permanente

Sabemos que:

𝐸𝑔 = 𝑤𝑟𝑜𝑡𝑜𝑟 ∙ 𝐾𝑏 ⋯ (1)
También:

40
 𝑉 − 𝑉𝑅 − 𝑉𝐿 − 𝐸𝑔 = 0
𝑑𝑖 ⋯ (2)
𝑉 − 𝑅𝑎 ∙ 𝑖 − 𝐿𝑎 ∙ − 𝑤𝑟𝑜𝑡𝑜𝑟 ∙ 𝐾𝑏 = 0
𝑑𝑡
Si despreciamos 𝐿 (por ser casi siempre pequeño comparado con los demás valores):

𝑉 − 𝑅𝑎 ∙ 𝑖 − 𝑤𝑟𝑜𝑡𝑜𝑟 ∙ 𝐾𝑏 = 0
Si bloqueamos el roto (𝑤𝑟𝑜𝑡𝑜𝑟 = 0):

𝑉 − 𝑅𝑎 ∙ 𝑖 = 0
𝑉 ⋯ (3)
𝑅𝑎 =
𝑖
También:

𝑤𝑟𝑜𝑡𝑜𝑟 ∙ 𝐾𝑏 = 𝑉 − 𝑅𝑎 ∙ 𝑖
𝑉 − 𝑅𝑎 ∙ 𝑖 ⋯ (4)
𝐾𝑏 =
𝑤𝑟𝑜𝑡𝑜𝑟
8.1.2. Análisis mecánico
Analizaremos en un primer momento la potencia del motor DC, que debería ser la misma
en la entrada como en la salida.

𝐸𝑔 = 𝐾𝑏 ∙ 𝑤𝑟𝑜𝑡𝑜𝑟
𝑇𝑚 = 𝐾𝑡 ∙ 𝑖
𝑃𝑜𝑡𝑒𝑛𝑡𝑟𝑎𝑑𝑎,𝑓𝑒𝑚 = 𝐸𝑔 ∙ 𝑖 = 𝑇𝑚 ∙ 𝑤𝑟𝑜𝑡𝑜𝑟 = 𝑃𝑜𝑡𝑠𝑎𝑙𝑖𝑑𝑎
Luego:

𝐾𝑡 = 𝐾𝑏 ⋯ (5)
Cuando se analiza el motor sin carga tendremos:
𝑑𝑤𝑟𝑜𝑡𝑜𝑟
𝑇𝑚 = 𝐽 ∙ + 𝐵 ∙ 𝑤𝑟𝑜𝑡𝑜𝑟
𝑑𝑡
𝑑𝑤𝑟𝑜𝑡𝑜𝑟
𝐾𝑡 ∙ 𝑖 = 𝐽 ∙ + 𝐵 ∙ 𝑤𝑟𝑜𝑡𝑜𝑟
𝑑𝑡
Ahora, si consideramos que el sistema se encuentra en un punto estacionario (𝑤̇ = 0):

𝐾𝑡 ∙ 𝑖 − 𝐵 ∙ 𝑤𝑟𝑜𝑡𝑜𝑟 = 0
𝐾𝑡 ∙ 𝑖 ⋯ (6)
𝐵=
𝑤𝑟𝑜𝑡𝑜𝑟

8.1.3. Análisis de la respuesta del sistema

Con estas mediciones se buscará calcular el parámetro 𝐽, existe otras maneras de
calcular este parámetro, pero se tendría que desarmar el motor y medir detalladamente
su geometría interna, pero esto resulta muy complicado… por ello se opta por calcular
la inercia despejando de la fórmula de tiempo de subida.
𝐽 ∙ 𝑅𝑎
𝑡𝑚 =
𝐾𝑡 ∙ 𝐾𝑏

41
 𝐾𝑡 ∙ 𝐾𝑏 ⋯ (7)
𝐽= ∙ 𝑡𝑚
𝑅𝑎
Hay diferentes definiciones del tiempo de subida, pero la que use usará es la siguiente:
“El tiempo de subida es el tiempo que le toma al sistema para llegar del 10% al 90% de
su valor final”.
8.2. Realización del experimento
8.2.1. Cálculo de la resistencia de armadura (𝑹𝒂 )

 Medir diferentes valores de voltaje y corriente en el motor

 Valores de voltaje pequeño de modo que no gire el rotor
Finalmente aplicamos la ecuación (3).
8.2.2. Cálculo de la constante electromotriz y de torque (𝑲𝒃 , 𝑲𝒕 )

 Medir diferentes valores de voltaje y corriente en el motor

 Valores de voltaje suficiente para que empiece a girar el rotor
Finalmente aplicamos las ecuaciones (4) y (5).
8.2.3. Cálculo del coeficiente del fricción viscosa (𝑩)
Con los mismos datos obtenidos en el punto (1.2.2) y usando la ecuación (6).
8.2.4. Cálculo del momento de inercia rotacional (𝑱)

 Alimentar al motor con un pulso lo suficientemente ancho para que llegue a su

velocidad nominal constante
 Calcular gráficamente el tiempo
Finalmente aplicamos la ecuación (7).
8.3. Identificación de los parámetros de los motores
Tabla 2. Características de los motores
Motor 𝑛 (Caja reductora) CPR (Resolución encoder)
𝑀1 (pequeño) 1: 34 10
𝑀2 (grande) 1: 90 11

8.3.1. Motor 1 (pequeño)

El motor es el mostrado en la figura 2.

42
 Figura 2. Motor DC de 6V

 Cálculo de la resistencia de armadura

Tabla 3. Voltaje vs. Corriente del motor pequeño
I (𝒎𝑨) V (𝑽)
15.89 0.235
34.4 0.505
6 0.135
35.9 0.524
45 0.618
70.8 0.881
52.5 0.659
23.7 0.293
6.79 0.125
𝑅𝑝𝑒𝑞𝑢𝑒ñ𝑜 = 7.5 Ω
Cabe resaltar que para mejor precisión medimos la resistencia del multímetro, el cual
resultó con un valor de 𝑅𝑚𝑢𝑙𝑡í𝑚𝑒𝑡𝑟𝑜 = 4.4 Ω.

MOTOR PEQUEÑO
1
0.9
0.8 y = 0.0119x + 0.056
0.7 R² = 0.987
Voltaje (V)

0.6
0.5
0.4
0.3
0.2
0.1
0
0 10 20 30 40 50 60 70 80
Corriente I (mA)

Figura 3. Voltaje vs. Corriente en el motor 1 – Prueba roto bloqueado

 Cálculo de las constantes del motor

43
Tabla 4. Parámetros medidos y calculados para el cálculo de la constante
Valores medidos Valores calculados
𝑽 (𝑽) 𝑰 (𝒎𝑨) 𝒇 (𝑯𝒛) 𝒘𝒓𝒐𝒕𝒐𝒓 (𝒓𝒂𝒅/𝒔) 𝑬𝒈 (𝑽)
3.82 75.8 694 436.0530603 2.91798
4.037 77.1 741 465.5840313 3.11951
4.355 79.2 798 501.3981875 3.41252
4.664 80 867 544.7521661 3.712
4.996 81.3 946 594.3893301 4.02853
5.2 81.6 1000 628.3185307 4.22896
5.295 82.3 1030 647.1680866 4.31563
5.405 82.3 1055 662.8760499 4.42563
5.568 83.4 1091 685.495517 4.57554
5.755 83.8 1115 700.5751618 4.75778
𝐾𝑏 = 𝐾𝑡 = 0.0073

MOTOR PEQUEÑO
5
4.5 y = 0.0073x + 0.0356
4 R² = 0.997
3.5
3
Eg (V)

2.5
2
1.5
1
0.5
0
0 100 200 300 400 500 600 700
w_rotor (rad/s)

Figura 4. Voltaje electromotriz vs. Velocidad angular del rotor – Prueba Kb, Kt

 Cálculo de la fricción viscosa

Tabla 5. Parámetros medidos y calculados para el cálculo de la fricción viscosa
Valores medidos Valores calculados
𝑽 (𝑽) 𝑰 (𝒎𝑨) 𝒇 (𝑯𝒛) 𝒘_𝒔𝒂𝒍𝒊𝒅𝒂(𝒓𝒐𝒕𝒐𝒓) 𝑲𝒃 ∙ 𝑰
3.82 75.8 694 436.0530603 0.00050786
4.037 77.1 741 465.5840313 0.00051657
4.355 79.2 798 501.3981875 0.00053064
4.664 80 867 544.7521661 0.000536
4.996 81.3 946 594.3893301 0.00054471
5.2 81.6 1000 628.3185307 0.00054672
5.295 82.3 1030 647.1680866 0.00055141
5.405 82.3 1055 662.8760499 0.00055141
5.568 83.4 1091 685.495517 0.00055878
5.755 83.8 1115 700.5751618 0.00056146

44
 𝑁∙𝑚∙𝑠
𝐵 = 2.03 ∙ 10−7
𝑟𝑎𝑑

MOTOR PEQUEÑO
0.00057

0.00056 y = 2E-07x + 0.0004

0.00055 R² = 0.9678
0.00054
Kb.I

0.00053

0.00052

0.00051

0.0005
0 100 200 300 400 500 600 700 800
w_rotor (rad/s)

Figura 5. Kb*I vs. Velocidad angular del motor – Prueba viscosidad

 Cálculo de la inercia del rotor

Tenemos el tiempo de subida de la figura 12: 𝑡𝑚 = 0.262 𝑠𝑒𝑔

Luego: 𝐽 = 1.57 ∙ 10−6 𝐾𝑔 ∙ 𝑚2

A partir de los datos hallados tendremos la planta:

𝑛 = 34, 𝑅𝑎 = 7.5, 𝐾𝑡 = 0.0073, 𝐵 = 2.03 ∙ 10−7 , 𝐽 = 1.57 ∙ 10−6

𝐾𝑡
𝑤(𝑠) 𝑛 ∗ (𝑅𝑎 ∗ 𝐵 + 𝐾𝑡 2 ) 𝟒. 𝟗𝟐
= =
𝑉(𝑠) 𝑅𝑎 ∗ 𝐽
∗ 𝑠 + 1 𝟎. 𝟐𝟏𝟓 ∙ 𝒔 + 𝟏
𝑅𝑎 ∗ 𝐵 + 𝐾𝑡 2
8.3.2. Motor 2 (grande)
El motor es el mostrado en la figura 6.

45
 Figura 6. Motor DC de 12V

 Cálculo de la resistencia de armadura

Tabla 6. Voltaje vs. Corriente del motor pequeño
𝑰 (𝒎𝑨) 𝑽 (𝑽)
2.08 0.034
13.24 0.211
7.05 0.114
17.35 0.277
24.81 0.395
30.02 0.478
25.28 0.402
32.6 0.517
38.7 0.615
55.3 0.874
𝑅𝑔𝑟𝑎𝑛𝑑𝑒 = 11.4 Ω
Cabe resaltar que para mejor precisión medimos la resistencia del multímetro, el cual
resultó con un valor de 𝑅𝑚𝑢𝑙𝑡í𝑚𝑒𝑡𝑟𝑜 = 4.4 Ω.

MOTOR GRANDE
1
0.9
y = 0.0158x + 0.0025
0.8
R² = 1
0.7
Voltaje (V)

0.6
0.5
0.4
0.3
0.2
0.1
0
0 10 20 30 40 50 60
Corriente I (mA)

Figura 6. Voltaje vs. Corriente en el motor 1 – Prueba roto bloqueado

 Cálculo de las constantes del motor

Tabla 4. Parámetros medidos y calculados para el cálculo de la constante
Valores medidos Valores calculados
𝑽 (𝑽) 𝑰 (𝒎𝑨) 𝒇 (𝑯𝒛) 𝒘_𝒔𝒂𝒍𝒊𝒅𝒂(𝒓𝒐𝒕𝒐𝒓) 𝑬𝒈 (𝑽)
2.305 48.6 242 138.2300768 1.53712
2.838 53 318 181.6411752 2.0006
3.382 57.6 400 228.4794657 2.47192
4.065 61.5 505 288.4553255 3.0933
4.94 66.2 602 343.8615959 3.89404
5.498 70.4 700 399.839065 4.38568
6.19 74.4 807 460.9573221 5.01448

46
 7.11 79.8 948 541.4963337 5.84916
7.55 82 1027 586.6210282 6.2544
8.54 86.4 1168 667.1600399 7.17488
9.52 90 1314 750.5550449 8.098
10.62 94.6 1480 845.3740231 9.12532
11.21 95.4 1574 899.0666976 9.70268
12 95.2 1694 967.6105373 10.49584
𝐾𝑏 = 𝐾𝑡 = 0.0107

MOTOR GRANDE
12

10
y = 0.0107x + 0.0531
8 R² = 0.9996
Eg (V)

0
0 200 400 600 800 1000 1200
w_rotor (rad/s)

Figura 4. Voltaje electromotriz vs. Velocidad angular del rotor – Prueba Kb, Kt

 Cálculo de la fricción viscosa

Tabla 5. Parámetros medidos y calculados para el cálculo de la fricción viscosa
V (V) I (mA) f (Hz) w_salida(rotor) Eg (V) Kb*I
2.305 48.6 242 138.2300768 1.53712 0.00052002
2.838 53 318 181.6411752 2.0006 0.0005671
3.382 57.6 400 228.4794657 2.47192 0.00061632
4.065 61.5 505 288.4553255 3.0933 0.00065805
4.94 66.2 602 343.8615959 3.89404 0.00070834
5.498 70.4 700 399.839065 4.38568 0.00075328
6.19 74.4 807 460.9573221 5.01448 0.00079608
7.11 79.8 948 541.4963337 5.84916 0.00085386
7.55 82 1027 586.6210282 6.2544 0.0008774
8.54 86.4 1168 667.1600399 7.17488 0.00092448
9.52 90 1314 750.5550449 8.098 0.000963
10.62 94.6 1480 845.3740231 9.12532 0.00101222
11.21 95.4 1574 899.0666976 9.70268 0.00102078
12 95.2 1694 967.6105373 10.49584 0.00101864
𝑁 ∙ 𝑚 ∙ 𝑠
𝐵 = 6.21 ∙ 10−7
𝑟𝑎𝑑

47
 MOTOR GRANDE
0.0012
y = 6E-07x + 0.0005
0.001 R² = 0.9682
0.0008
Kb.I

0.0006

0.0004

0.0002

0
0 200 400 600 800 1000 1200
w_rotor (rad/s)

Figura 5. Kb*I vs. Velocidad angular del motor – Prueba viscosidad

 Cálculo de la inercia del rotor

Tenemos el tiempo de subida de la figura 13: 𝑡𝑚 = 0.314 𝑠𝑒𝑔

Luego: 𝐽 = 3.15 ∙ 10−6 𝐾𝑔 ∙ 𝑚2

A partir de los datos hallados tendremos la planta:

𝑛 = 90, 𝑅𝑎 = 11.4, 𝐾𝑡 = 0.0107, 𝐵 = 6.21 ∙ 10−7 , 𝐽 = 3.15 ∙ 10−6

𝐾𝑡
𝑤(𝑠) 𝑛 ∗ (𝑅𝑎 ∗ 𝐵 + 𝐾𝑡 2 ) 𝟐. 𝟏𝟖
= =
𝑉(𝑠) 𝑅𝑎 ∗ 𝐽 𝟎. 𝟑𝟎 ∙ 𝒔 + 𝟏
2∗𝑠+1
𝑅𝑎 ∗ 𝐵 + 𝐾𝑡

9. IDENTIFICACIÓN DEL SISTEMA

En el punto anterior mostramos una manera de identificar el modelo dinámico del motor
a partir de ecuaciones matemáticas. Ahora presentaremos un método más práctico, en
el que adquirimos datos de la respuesta del motor (y de la entrada también) para
finalmente identificar la planta con funciones que nos provee MatLab.
9.1. Desarrollo del método al sistema
El algoritmo que se usa para hallar la planta será el ARX (autoregressive exogenous
model), que presenta la siguiente ecuación de transferencia y un diagrama de bloques
mostrado en la figura 6.
𝐵 ⋯ (8)
𝑦(𝑠) = 𝑢(𝑠) ∗ + 𝐸(𝑠)
𝐴

48
 Figura 6. Diagrama de bloques de la estructura ARX
Para la realización de la prueba usamos los siguientes materiales:

 DAQ 6002
 Arduino UNO
 Puente H – L298N
 Circuito conversor frecuencia – voltaje (LM331)
 Fuente de voltaje
 Multímetro
9.1.1. Explicación del funcionamiento
El DAQ se encargará de generar de una señal en forma de pulso de 0 – 5V como la
mostrada en la figura, esta señal activará el puente H para que el motor empieza a girar
a su tensión nominal y la frecuencia generada en el encoder por uno de sus canales es
leída en forma de voltaje (haciendo uso del conversor, calibrado previamente para que
arroje un máximo de 5V) por el DAQ… así mismo la misma señal generada es leída por
el encoder en otro de sus pines.
El esquema resumido para obtener los datos es el mostrado en la figura 7.

Figura 7. Esquema resumido de la conexión para la obtención de la planta

 Calibración del conversor y conversión voltaje – velocidad angular

Como mencionamos previamente, primero procedimos a calibrar el conversor para que
al voltaje nominal del motor nos arroje 5V de salida; además se hizo una tabla para
convertir el voltaje en velocidad angular (de encoder).
Tabla 6. Obtención de la ecuación para convertir voltaje del conversor a velocidad (𝑀1 )
𝒇 (𝑯𝒛) 𝑽𝒄𝒐𝒏𝒗𝒆𝒓𝒔𝒐𝒓 (𝑽) 𝑽𝒎𝒐𝒕𝒐𝒓𝑷𝑾𝑴 (𝑽)
1348 5.09 6
1260 4.77 5.6
1116 4.23 5
978 3.708 4.395
805 3.062 3.654

49
 635 2.41 2.912
528 2.01 2.45
397 1.51 1.872
297 1.131 1.43
135 0.47 0.7
0 0 0.1

MOTOR PEQUEÑO
1600
1400 y = 263.7036226002x + 1.1082227963
1200 R² = 0.9999232886
Frecuencia (Hz)

1000
800
600
400
200
0
0 1 2 3 4 5 6
Voltaje (V)

Figura 8. Ecuación de la conversión frecuencia – voltaje del conversor del motor

pequeño
Tabla 7. Obtención de la ecuación para convertir voltaje del conversor a velocidad (𝑀2 )
𝒇 (𝑯𝒛) 𝑽𝒄𝒐𝒏𝒗𝒆𝒓𝒔𝒐𝒓 (𝑽) 𝑽𝒎𝒐𝒕𝒐𝒓𝑷𝑾𝑴 (𝑽)
1830 5 12
1595 4.45 10.58
1408 3.91 9.28
1192 3.31 7.93
980 2.73 6.6
775 2.146 5.26
565 1.56 3.93
373 0.99 2.606
0 0 0.1

50
 MOTOR GRANDE
2000
1800 y = 361.1253903099x + 1.813621677
Frecuencia (Hz) 1600 R² = 0.9996472431
1400
1200
1000
800
600
400
200
0
0 1 2 3 4 5 6
Voltaje (V)

Figura 9. Ecuación de la conversión frecuencia – voltaje del conversor del motor

pequeño

 Programación en LabView

Figura 10. Panel frontal del programa

51
 Figura 11. Diagrama de bloques del programa

 Programación en Arduino para convertir voltaje analógico en PWM

int input_pot = A1;

int temp = 0;
int temp1;
int output_pwm = 9;

void setup() {
// put your setup code here, to run once:
TCCR1A = 0x00; // Setea timer modo de genera PWM
TCCR1B = 0x12; // Preescaler N = 8
ICR1 = 50; // Valor máximo de analogWrite
TIMSK1 = 0; // Deshabilita cualquier interrupción
pinMode(9, OUTPUT);
Serial.begin(9600);
}

void loop() {
// put your main code here, to run repeatedly:
temp = analogRead(input_pot);
temp1 = map(temp, 0, 1023, 0, 50);
analogWrite(output_pwm, temp1);
Serial.print(temp);
Serial.print(" - ");
Serial.println(temp1);
}

 Programa en MatLab para identificar la planta por el método ARX

clcear all; close all;clc

52
 % Adquisicion Continous Sample
load DatosMotorPequeno2.lvm; % Ingresar data adquirida xxxx
t=DatosMotorPequeno2(:,1);
u=DatosMotorPequeno2(:,2);
y=DatosMotorPequeno2(:,4)/341.2;
%Ploteo de la data
subplot(2,1,1),plot(t,u,'r'), grid on; title('Señal generada');
xlabel('Tiempo [seg]'), ylabel('Voltaje [V]'), axis([0 14.4 -inf
inf]);
subplot(2,1,2),plot(t,y,'b'), grid on, title('Respuesta del motor
pequeño');
xlabel('Tiempo [seg]'), ylabel('W [rad/s]');
axis([0 max(t) min(y) max(y)+10])
%Identificacion ARX
fs=1000;
T=1/fs;
data=iddata(y,u,T);
th=arx(data,[1 1 1]);
Gpd=tf(th.b,th.a,T); % F.T. en T.D.
Gpc=d2c(Gpd); % F.T. en T.C.
disp('Planta Identificada en continuo')
[nc,dc]=tfdata(Gpc,'v');
Gpc

9.2. Adquisición de datos y obtención de las plantas de los motores

9.2.1. Motor 1 (pequeño)
Los datos obtenidos se muestran a continuación.

53
 Figura 12. Señal generada y respuesta del motor pequeño adquiridos por el DAQ
Y la planta es:

9.2.2. Motor 2
Los datos obtenidos se muestran a continuación.

54
 Figura 13. Señal generada y respuesta del motor grande adquiridos por el DAQ
Y la planta es:

9.3. Comparación entre modelos hallados

 Motor 1 (pequeño)
Usando el método físico (medición de parámetros) obtuvimos:
𝑤(𝑠) 𝟒. 𝟗𝟐
=
𝑉(𝑠) 𝟎. 𝟐𝟏𝟓 ∙ 𝒔 + 𝟏
Y mediante adquisición de datos:
𝑤(𝑠) 𝟐𝟓. 𝟓𝟐
=
𝑉(𝑠) 𝒔 + 𝟓. 𝟎𝟕𝟓

55
 Figura 14. Respuesta ante un step para el motor pequeño

 Motor 2 (grande)
Usando el método físico (medición de parámetros) obtuvimos:
𝑤(𝑠) 𝟐. 𝟏𝟖
=
𝑉(𝑠) 𝟎. 𝟑𝟎 ∙ 𝒔 + 𝟏
Y mediante adquisición de datos:
𝑤(𝑠) 𝟗. 𝟗𝟓𝟐
=
𝑉(𝑠) 𝒔 + 𝟒. 𝟑𝟏𝟏

56
 Figura 15. Respuesta ante un step para el motor grande
Las diferencias se dieron por diversas razones:

 Las diferencias entre las plantas radican principalmente en el tiempo de respuesta

del sistema, esto se debe que en el análisis de la adquisición de datos, se agregaron
distintos componentes para poder tomar la medición por lo tanto el tiempo de
respuesta es mayor al tiempo de respuesta del primer método ya que este solo
considera el tiempo de respuesta del motor.
 Las plantas obtenidas reflejan mucha similitud por lo cual se optara por cualquiera
de estas.
 Para el siguiente informe se procederá a usar la planta obtenida por adquisición de
datos ya que este incluye algunos de los componentes usados para el control, por
lo tanto incluye en la planta los pequeños efectos de retardos.

10. ELECTRÓNICA DEL MANIPULADOR

 Arduino UNO

57
 Figura 16. Arduino UNO

 Puente H L298N

Figura 17. Módulo L298N

 Motores con encoder de dos canales

Figura 18. Motores con encoder

La conexión de los componentes será el siguiente:

58
 Figura 19. Esquema de conexión para la implementación del circuito

11. CÓDIGO DE LECTURA DE ENCODERS

11.1. Lectura de velocidad y posición en el encoder

#define EncoderB 3

volatile int cont = 0;

unsigned long int t = 0;
float t_pasado = 0.0;
float t_actual = 0.0;
const float angulo = 1.0/375.0;
float rpm = 0;
const float PPR = 375.0;

void setup() {
// put your setup code here, to run once:
attachInterrupt(0, PosicionMotor, RISING);
pinMode(EncoderB, INPUT);
Serial.begin(9600);
//Serial.println(0);
}

void loop() {
// put your main code here, to run repeatedly:
if ((millis() - t) > 500){
Serial.print((float)cont*360.0/375.0);
Serial.print(" ");
Serial.println(rpm, 6);
t = millis();
}
if (rpm < 10 || rpm > 210){
rpm = 0;
}

59
}

void PosicionMotor(){
//cont++;
//t_actual = millis();
rpm = angulo/(micros() - t_pasado)*1000000.0*60.0;
t_pasado = micros();
if (digitalRead(3)){
cont--;
}
else{
cont++;
}
if (cont == PPR || cont == -PPR){
cont = 0;
}
//Serial.println(cont);
//t_pasado = millis();
}

12. CÓDIGOS DE CONTROL

#define IN1 5 // Entrada 1 al puente H

#define IN2 6 // Entrada 2 al puente H
#define IN3 8
#define IN4 9

// ------ 1. DECLARACIÓN DE VARIABLES ------ //

// 1.1. Motor 1
const int EncA = 2; // Canal A como interrupción
const int EncB = 4; // Canal B como entrada digital
const float PPR = 374; // Resolución del encoder
// 1.1. Motor 2
const int EncA2 = 3; // Canal A como interrupción
const int EncB2 = 10; // Canal B como entrada digital
const float PPR2 = 980; // Resolución del encoder
int POSICION = 90;
int POSICION2 = 270;
// 1.2. Parámetros para el motor
volatile float cont = 0.0; // Contador de pulsos por interrupción, 0.5 en CHANGE
volatile float cont2 = 0.0;
const float angulo = 1.0/PPR; // Giro en vueltas del eje por contador en aumentado en
1
volatile unsigned long t_pasado = 0; // Inicialización del tiempo de ocurrido el pulso
anterior
volatile unsigned long t_actual = 0; // Inicialización del tiempo de ocurrido el pulso
actual
unsigned long t = 0; // Inicialización del tiempo para escribir en el puerto serial
// 1.3. Parámetros del control
float kp = 1.5, kd = 0.1, ki = 0.0; // Parámetros del controlador PID
float cp = 0.0; // cp = kp * e
float cd = 0.0; // cd = kd * (e - ea) / dt
float ci = 0.0; // ci = cia + ki * e * dt
float ea = 0.0; // Error anterior

60
float cia = 0.0; // Anterior ci
float e = 0.0; // Error
float u = 0.0; // Señal del controlador
const float dt = 10.0;
float g = 0, l = 3;

float cp2 = 0.0; // cp = kp * e

float cd2 = 0.0; // cd = kd * (e - ea) / dt
float ci2 = 0.0; // ci = cia + ki * e * dt
float ea2 = 0.0; // Error anterior
float cia2 = 0.0; // Anterior ci
float e2 = 0.0; // Error
float u2 = 0.0; // Señal del controlador
// ----------------------------------------- //

// ---- 2. INICIALIZACIÓN DEL PROGRAMA ---- //

void setup() {
attachInterrupt(0, DetectaFlanco, CHANGE); // Interrupción del canal A del encoder
attachInterrupt(1, DetectaFlanco2, CHANGE);
pinMode(EncB, INPUT); // Canal B como entrada digital
pinMode(EncB2, INPUT); // Canal B como entrada digital
Serial.begin(9600); // Baudios (velocidad) de la comunicación serial
//Serial.setTimeout(100);

pinMode(IN1, OUTPUT);
pinMode(IN2, OUTPUT);
digitalWrite(IN1, LOW);
digitalWrite(IN2, LOW);

pinMode(IN3, OUTPUT);
pinMode(IN4, OUTPUT);
digitalWrite(IN3, LOW);
digitalWrite(IN4, LOW);
}
// ---------------------------------------- //

// ---- 3. BUCLE PRINCIPAL DEL PROGRAMA ---- //

void loop() {
if(Serial.available() > 0){ // Si se escribe un dato en el puerto serie
//POSICION = (Serial.readString().toInt());
if(POSICION > 180){
POSICION = -(360 - POSICION);
}
POSICION = POSICION * PPR / 360;
//cia = 0;
t = millis();
}

if((millis() - t) > dt){ // Cada 50 ms se realiza el control

t = millis();
Control();
Control2();
//Serial.write((int)cont);
}
MandarPWM();

61
 MandarPWM2();
}
// ----------------------------------------- //

// ---- 4. PROGRAMA DE LA INTERRUPCIÓN ---- //

void DetectaFlanco(){
if(digitalRead(EncA)){ // Detecta flanco de subida
if(digitalRead(EncB)){
cont = cont -0.5;
}
else{
cont = cont + 0.5;
}
}
else{ // Detecta flanco de bajada
if(digitalRead(EncB)){
cont = cont + 0.5;
}
else{
cont = cont - 0.5;
}
}
if (cont == PPR || cont == -PPR){ // Si dio una vuelta horaria o antihoraria
cont = 0;
}
}
// ---------------------------------------- //

// ---- 4. PROGRAMA DE LA INTERRUPCIÓN ---- //

void DetectaFlanco2(){
if(digitalRead(EncA2)){ // Detecta flanco de subida
if(digitalRead(EncB2)){
cont2 = cont2 - 0.5;
}
else{
cont2 = cont2 + 0.5;
}
}
else{ // Detecta flanco de bajada
if(digitalRead(EncB2)){
cont2 = cont2 + 0.5;
}
else{
cont2 = cont2 - 0.5;
}
}
if (cont2 == PPR2 || cont2 == -PPR2){ // Si dio una vuelta horaria o antihoraria
cont2 = 0;
}
}
// ---------------------------------------- //

// ------- 5. CONTROL DE LA PLANTA ------- //

void Control(){
e = ((float)POSICION) - cont;

62
 cp = kp * e;
cd = kd * (e - ea) / dt * 1000.0;
ci = cia + ki * e * dt / 1000.0;
ea = e;
cia = ci;
g = 9.81*cos(cont*360/PPR*PI/180)*l*18.51*0.01275;
u = cp + cd + ci + g;
if (u > PPR/2){
u = PPR/2;
}
if (u < -PPR/2){
u = -PPR/2;
}
u = u*dt/PPR*2;
/*if (cont > (POSICION - 1) && cont < (POSICION + 1)){
u = 0.0, cia = 0;
}
//MandarPWM();*/
}
// --------------------------------------- //

// ------- 5. CONTROL DE LA PLANTA ------- //

void Control2(){
e2 = ((float)POSICION2)*PPR2/360.0 - cont2;
cp2 = kp * 5 * e2;
cd2 = kd * (e2 - ea2) / dt * 1000.0;
ci2 = cia2 + ki * e2 * dt / 1000.0;
ea2 = e2;
cia2 = ci2;
//g = 9.81*cos(cont*360/PPR*PI/180)*l*18.51*0.01275;
u2 = cp2 + cd2 + ci2;
if (u2 > PPR2/2){
u2 = PPR2/2;
}
if (u2 < -PPR2/2){
u2 = -PPR2/2;
}
u2 = u2*dt/PPR2*2;
/*if (cont > (POSICION - 1) && cont < (POSICION + 1)){
u = 0.0, cia = 0;
}
//MandarPWM();*/
}
// --------------------------------------- //

// ---------- 6. GENERADOR PWM ---------- //

void MandarPWM(){
if (u >= 0){
//digitalWrite(IN1, LOW);
digitalWrite(IN2, LOW);
if((millis() - t) < u){
digitalWrite(IN1, HIGH);
}
else{
digitalWrite(IN1, LOW);

63
 }
}
else{
digitalWrite(IN1, LOW);
//digitalWrite(IN2, LOW);
if((millis() - t) < -u){
digitalWrite(IN2, HIGH);
}
else{
digitalWrite(IN2, LOW);
}
}
}
// ------------------------------------- //

// ---------- 6. GENERADOR PWM ---------- //

void MandarPWM2(){
if (u2 >= 0){
//digitalWrite(IN1, LOW);
digitalWrite(IN4, LOW);
if((millis() - t) < u2){
digitalWrite(IN3, HIGH);
}
else{
digitalWrite(IN3, LOW);
}
}
else{
digitalWrite(IN3, LOW);
//digitalWrite(IN2, LOW);
if((millis() - t) < -u2){
digitalWrite(IN4, HIGH);
}
else{
digitalWrite(IN4, LOW);
}
}
}
// ------------------------------------- //

13. BIBLIOGRAFÍA

[1] “Determinación de los parámetros de un motor CD por medición física directa” -

Universidad Tecnológica de Mixteca

[2] “Simulador de efectos inerciales de 2DOF” - Juan E. Camarena - UNI - FIM

64