Informe de Laboratorio 7 - Modelamiento Dinámico Del Robot PDF

UNIVERSIDAD NACIONAL DE TRUJILLO
Facultad de Ingeniería
Escuela Profesional de Ingeniería Mecatrónica
Dinámica del Robot – Método de Lagrange Euler
Informe de Laboratorio N° 7
Robótica
AUTORES : Aguilar Valdiviezo, Hans.

Juarez Mercedes, Luis.
DOCENTE : Ing. Josmell H. Alva Alcántara
CICLO : Noveno
Trujillo - Perú
Junio de 2019
Resumen
Este presente informe consiste en el reforzamiento y aplicación práctica de lo realizado

en la teoría sobre dinámica del robot para poder realizar el modelamiento dinámico del robot
haciendo uso del método de Lagrange – Euler. Para lograr esto se hace uso del software
MATLAB 2018 para la implementación del algoritmo que se debe de seguir para obtener las
ecuaciones que rigen la dinámica del robot en un script que condense la solución de cada uno
de los ejercicios propuestos en el presente laboratorio.
Palabras clave:
Estática del robot, modelamiento dinámico, Método Lagrange – Euler.
Abstract
This report is based on reinforcement and practice. What takes place The theory about robot
dynamics. To achieve this, the MATLAB 2018 software can be used for the implementation
of the algorithm that must be followed to obtain the equations that govern the dynamics of the
robot in a script that condense the solution of each one of the exercises proposed in this
laboratory.
Keywords:
Robot static, dynamic modeling, Lagrange - Euler method.
INGENIERÍA MECATRÓNICA 1
Tabla de contenido
Resumen ..................................................................................................................................... 1
Abstract ...................................................................................................................................... 1
Capítulo1 .................................................................................................................................... 4
Introducción ............................................................................................................................... 4
Objetivos ................................................................................................................................ 4
Objetivo General. ............................................................................................................... 4
Objetivos Específicos......................................................................................................... 4
Materiales ............................................................................................................................... 4
Capítulo 2 ................................................................................................................................... 5
Marco Teórico ............................................................................................................................ 5
Capítulo 3 ................................................................................................................................... 7
Metodología ............................................................................................................................... 7
Capítulo 4 ................................................................................................................................. 27
Conclusiones ............................................................................................................................ 27
Referencias Bibliográficas ....................................................................................................... 27
Tabla de Figuras
Figura 1. Péndulo esférico. ........................................................................................................ 7

Figura 2. Manipulador RP........................................................................................................ 10
Figura 3. Sistemas de coordenadas orientados de la cinemática directa.................................. 11
Figura 4. Manipulador RR. ...................................................................................................... 14
Figura 6. Robot SCARA de 4GDL. ......................................................................................... 17
Lista de tablas
Tabla 1. Parámetros de Denavit Hartenberg. ........................................................................... 11
Capítulo1
Introducción
En el proceso del diseño de un robot además de la cinemática directa e inversa también

se debe estudiar la dinámica del robot para poder obtener los torques en cada articulación que
servirán en un futuro dentro del proceso de diseño para la selección del material a usar en los
eslabones entre otras consideraciones adicionales necesarias.
Objetivos
Objetivo General.
- Formular la Dinámica del Robot, por el método de LaGrange Euler.
Objetivos Específicos.
- Realizar un script en Matlab R2018a que condense la solución de los ejercicios

propuestos.
- Resolver los ejercicios mediante el uso de Método de Lagrange Euler.
- Verificar los resultados.
Materiales
- Computadora personal
- Software MATLAB R2018a
Capítulo 2
Marco Teórico
Para el desarrollo de este laboratorio es necesario definir las matrices que conforman
las ecuaciones de la dinámica del robot de Lagrange – Euler junto con su respectiva función en
Matlab.
𝑀(𝑞)𝑞̈ + 𝐶(𝑞, 𝑞̇ )𝑞̇ + 𝑔(𝑞) = 𝜏
Donde
𝑛 𝑚11 ⋯ 𝑚1𝑛
𝑇 𝑇
𝑀(𝑞) = ∑ 𝑚𝑖 (𝐽𝑣𝑖 ) 𝐽𝑣𝑖 + (𝐽𝜔𝑖 ) 0𝑅𝑖 𝑖𝐼 ( 0𝑅𝑖 )𝑇 𝐽𝜔𝑖 =[ ⋮ ⋱ ⋮ ]
𝑖=1 𝑚𝑛1 ⋯ 𝑚𝑛𝑛
% Matriz Masa
function [output] = matrizMasa(m,I,R0,Jv,Jw)
sumD = 0;
for i = 1:2
Di = m(i)*(Jv(3*i-2:3*i,1:n).')*Jv(3*i-2:3*i,1:n) + (Jw(3*i-
2:3*i,1:n).')*R0(3*i-2:3*i,1:3)*I(3*i-2:3*i,1:3)*(R0(3*i-
2:3*i,1:3).')*Jw(3*i-2:3*i,1:n);
sumD = sumD + Di;
D = sumD;
end
output = D;
end
𝑐11 ⋯ 𝑐1𝑛 𝑛
𝐶(𝑞, 𝑞̇ ) = [ ⋮ ⋱ ⋮ ], 𝐶𝑖𝑗 = ∑ 𝐶𝑖𝑗𝑘 𝑞̇ 𝑘

𝑐𝑛1 ⋯ 𝑐𝑛𝑛 𝑘=1
1 𝜕𝑚𝑖𝑗 𝜕𝑚𝑖𝑘 𝜕𝑚𝑗𝑘

𝐶𝑖𝑗𝑘 = ( + − )
2 𝜕𝑞𝑘 𝜕𝑞𝑗 𝜕𝑞𝑖
% Matriz de Coriolis o fuerzas centrifugas

function [output] = matrizCoriolis(q,dq,D)
sumC = 0;
for i = 1:2
for j = 1:2
sumC = 0;
for k = 1:2
Cijk = 1/2*(diff(D(i,j),q(k)) + diff(D(i,k),q(j)) -
diff(D(j,k),q(i)));
sumC = sumC + Cijk*dq(k);
Cij = sumC;
end
C(i,j) = Cij;
end
end
output = C;
end
𝑔1
𝑇
𝐺(𝑞) = − ∑(𝐽𝑣𝑖 ) 𝑚𝑖 ⃗ =[ ⋮ ]
𝐠0
𝑖=1 𝑔𝑛
% Matriz de gravedad
function [output] = matrizGravedad(m,g0,Jv)
sumG = 0;
for i = 1:2
Gi = (Jv(3*i-2:3*i,1:n).')*m(i)*g0;
sumG = sumG + Gi;
G = sumG;
end
output = G;
end
Capítulo 3
Metodología
Con el fin de estudiar y comprender el modelo dinámico del robot, nos valemos de
ejercicios que se presentan a continuación. Para este estudio se hace uso de una herramienta de
software MATLAB para lograr resolverlos.
Ejercicios
1. La figura 1 muestra un péndulo esférico. Dicho péndulo está formado por una masa puntual
m, unida a una articulación esférica a través de una barra de longitud “ℓ” la cual posee una
masa despreciable. La configuración de la masa puntual estará parametrizada por 𝑞 = (𝜃,𝜙),
dónde 𝜃 mide la distancia angular del eje z, y 𝜙 mide la distancia angular del eje x.
y
x
θ
ℓ
ϕ m
mg
Figura 1. Péndulo esférico.
a) Determinar las ecuaciones que representan la dinámica del péndulo, suponiendo que no
existe actuación externa, usando el método de Euler – Lagrange.
b) Usando el resultado anterior, encontrar la matriz 𝑀(𝑞), 𝐶(𝑞,𝑞̇) y el vector g(q) para que
la dinámica del péndulo quede expresada de forma compacta como 𝑀(𝑞)𝑞̈+
𝐶(𝑞,𝑞̇)𝑞̇+𝑔(𝑞)=0.
Solución:
a) Método de Euler – Lagrange.
Para dar solución al sistema dinámico mediante el método de Euler – Lagrange primero se
debe de identificar las variables independientes del sistema y expresar los sistemas de
coordenadas en función a ellos. En este caso estas variables son 𝜃 y 𝜙.
𝑥 = ℓ sin 𝜃 cos 𝜙
𝑦 = ℓ sin 𝜃 sin 𝜙
𝑧 = −ℓ cos 𝜃
Luego es necesario realizar operaciones previas. Primero derivamos cada ecuación respecto
al tiempo
𝑥̇ = ℓ(cos 𝜃 cos 𝜙 𝜃̇ − sin 𝜃 sin 𝜙 𝜙̇)
𝑦̇ = ℓ(cos 𝜃 sin 𝜙 𝜃̇ + sin 𝜃 cos 𝜙 𝜙̇)
𝑧̇ = ℓ sin 𝜃 𝜃̇
Y elevamos al cuadrado
𝑥̇ 2 = ℓ2 (cos2 𝜃 cos2 𝜙 𝜃̇ 2 + sin2 𝜃 sin2 𝜙 𝜙̇ 2 − cos 𝜃 sin 𝜃 cos 𝜙 sin 𝜙 𝜃̇ 𝜙̇)
𝑦̇ 2 = ℓ2 (cos2 𝜃 sin2 𝜙 𝜃̇ 2 + sin2 𝜃 cos 2 𝜙 𝜙̇ 2 + cos 𝜃 sin 𝜃 cos 𝜙 sin 𝜙 𝜃̇ 𝜙̇)
𝑧̇ 2 = ℓ2 sin2 𝜃 𝜃̇ 2
Luego se halla la energía cinética del sistema
1 1
𝐾 = 𝑚‖𝑥̇ 2 + 𝑦̇ 2 + 𝑧̇ 2 ‖ = 𝑚ℓ2 (cos 2 𝜃 𝜃̇ 2 + sin2 𝜃 𝜙̇ 2 + sin2 𝜃 𝜃̇ 2 )
2 2
1
𝐾 = 𝑚ℓ2 (𝜃̇ 2 + sin2 𝜃 𝜙̇ 2 )
2
Y también la energía potencial del sistema, en este caso solo existe energía potencial
gravitatoria.
𝑈 = 𝑈𝑔 = 𝑚𝑔𝑧 = −𝑚𝑔ℓ 𝑐𝑜𝑠 𝜃
𝑈 = −𝑚𝑔ℓ cos 𝜃
Y obtenemos el Lagrangiano
𝐿{𝑞, 𝑞̇ } = 𝐾(𝑞, 𝑞̇ ) − 𝑈(𝑞)
1
𝐿{𝜃, 𝜃̇ , 𝜙, 𝜙̇} = 𝑚ℓ2 (𝜃̇ 2 + sin2 𝜃 𝜙̇ 2 ) + 𝑚𝑔ℓ cos 𝜃
2
Finalmente se halla las ecuaciones de Euler – Lagrange para las cuales tendrán como
fuerzas generalizadas los momentos que se ejercen en el sistema para hacer girar el péndulo,
en este caso no existen fuerzas generalizadas por lo que ambas ecuaciones se igualan a cero.
𝜕 𝜕𝐿 𝜕𝐿
( )− = 𝜏𝑖
𝜕𝑡 𝜕𝑞̇ 𝑖 𝜕𝑞𝑖
Primera ecuación, para 𝑞1 = 𝜃 y 𝜏1 = 0
𝜕
(𝑚ℓ2 𝜃̇ ) + 𝑚𝑔ℓ sin 𝜃 − 𝑚ℓ2 sin 𝜃 cos 𝜃 𝜙̇ 2 = 0
𝜕𝑡
𝑚ℓ2 𝜃̈ + 𝑚𝑔ℓ sin 𝜃 − 𝑚ℓ2 sin 𝜃 cos 𝜃 𝜙̇ 2 = 0
ℓ𝜃̈ − ℓ𝜙̇ 2 sin 𝜃 cos 𝜃 + 𝑔 sin 𝜃 = 0
1
ℓ𝜃̈ − ℓ𝜙̇ 2 sin 2𝜃 + 𝑔 sin 𝜃 = 0
2
Segunda ecuación, para 𝑞2 = 𝜃 y 𝜏2 = 0
𝜕
(𝑚ℓ2 sin2 𝜃 𝜙̇) = 0
𝜕𝑡
𝑚ℓ2 sin2 𝜃 𝜙̈ + 2𝑚ℓ2 sin 𝜃 cos 𝜃 𝜃̇ 𝜙̇ = 0
sin2 𝜃 𝜙̈ + 2 sin 𝜃 cos 𝜃 𝜃̇ 𝜙̇ = 0
𝜙̈ sin2 𝜃 + 𝜃̇ 𝜙̇ sin 2𝜃 = 0
b) Forma compacta.
De las ecuaciones anteriormente hallas se puede construir una expresión matricial de la

dinámica del sistema.
𝑀(𝑞)𝑞̈ + 𝐶(𝑞, 𝑞̇ )𝑞̇ + 𝑔(𝑞) = 0
1
ℓ 0 𝜃̈ 0 − ℓ𝜙̇ sin 2𝜃 𝜃̇ 𝑔 sin 𝜃 ⃗
[ 2 ] [ ̈] + [ 2 ] [ ̇] + [ ]=𝟎
0 sin 𝜃 𝜙 𝜙 0
𝜙̇ sin 2𝜃 0
2. Determinar el modelo dinámico del robot manipulador RP de 2 GDL mostrado en la figura

2. Asumir que el sistema de referencia se encuentra en la base del robot (origen de la
primera articulación) con el eje x orientado horizontalmente y el y verticalmente. La masa
del primer eslabón es 𝑚1 y la masa del segundo eslabón es 𝑚2. Los tensores de inercia del
primer y segundo eslabón con respecto a sus centros de masa son 𝐼1 𝑒 𝐼2, respectivamente,
y están dados por:
𝐼𝑥𝑥1 0 0 𝐼𝑥𝑥2 0 0
𝐼1 = [ 0 𝐼𝑦𝑦1 0 ], 𝐼2 = [ 0 𝐼𝑦𝑦2 0 ]
0 0 𝐼𝑧𝑧1 0 0 𝐼𝑧𝑧2
m2
ℓ
m1
θ1 d2
Figura 2. Manipulador RP.
Solución:
Para poder determinar el modelo dinámico del robot es necesario primero realizar la
cinemática directa del robot con el fin de obtener las matrices de transformación
homogénea y con de ellas extraer los elementos necesarios para el cálculo, por ejemplo, las
matrices de rotación respecto a base.
Además, por inspección en el dibujo consideramos que la posición del efector final y del
centro de masa del segundo eslabón no son iguales, para ello creamos dos variables, q2 que
vendría a ser el segundo grado de libertad y d1 que viene a ser una distancia desde el efector
final al centro de masa, pudiéndose así definir la posición del efector final en función de q1
y q2. Pero para mayor practicidad hacemos d1 = 0.
q2
g
m2
ℓ
y0 d1 =0
m1
x0
q1 d2
z0, y1
Figura 3. Sistemas de coordenadas orientados de la cinemática directa.
Art. 𝑑𝑖 𝜃𝑖 𝑎𝑖 𝛼𝑖
1 q1+90° 0 0 90°
2 0 q2 0 0
Tabla 1. Parámetros de Denavit Hartenberg.
Con ayuda del software Matlab hallamos las matrices de rotación de cada eslabón respecto
a base con el siguiente código. Además, considerando 𝑞1 = 𝜃1 y 𝑞2 = 𝑑2
syms q1 q2 dq1 dq2 ddq1 ddq2 L m1 m2 g pi

syms Ixx1 Iyy1 Izz1 Ixy1 Ixz1 Iyz1
g0 = [0;-g;0];
m = [m1;m2];
q = [q1;q2];
dq = [dq1;dq2];
ddq = [ddq1;ddq2];
% Parámetros de Denavit Hartenberg

theta = [q1+pi/2;0];
d = [0;q2];
a = [0;0];
alpha = [pi/2;0];
MDH = [theta d a alpha] % Tabla de parámetros de DH.
% Matriz de Transformación Homogénea entre sistemas de coordenadas
T01 = simplify(denavit(theta(1),d(1),a(1),alpha(1)));
T02 = simplify(simplify(T01*T12))
% Posicion
P2 = T02(1:3,4);
% Matrices de Rotación
R00 = eye(3)
R01 = T01(1:3,1:3)
R02 = T02(1:3,1:3)
Obteniendo las matrices de rotación
1 0 0 − sin 𝑞1 0 cos 𝑞1 − sin 𝑞1 0 cos 𝑞1

0 0 0
𝑅0 = [0 1 0] , 𝑅1 = [ cos 𝑞1 0 sin 𝑞1 ] , 𝑅2 = [ cos 𝑞1 0 sin 𝑞1 ]
0 0 1 0 1 0 0 1 0
Luego de ello se calcula la posición de los centros de masa de cada eslabón respecto a masa
para determinar el jacobiano de velocidad lineal. Y también se halla el jacobiano de
velocidad angular a partir de los vectores de la tercera columna de las matrices de rotación.
Esto se realiza con ayuda de Matlab.
% MTH del centro de masa de la barra 1 respecto a base

Tcm01 = simplify(denavit(q1,0,L,0));
Pcm1 = Tcm01(1:3,4)
Pcm2 = T02(1:3,4)
% Jacobianos de Velocidad lineal
Jv1 = [diff(Pcm1(1),q1) 0; % Jacobiano eslabon 1
diff(Pcm1(2),q1) 0;
diff(Pcm1(3),q1) 0]
Jv2 = [diff(Pcm2(1),q1) diff(Pcm2(1),q2); % Jacobiano eslabon 2
diff(Pcm2(2),q1) diff(Pcm2(2),q2);
diff(Pcm2(3),q1) diff(Pcm2(3),q2)]
% Columnas para matriz Jacobiana de velocidad angular
z00 = zeros(3,1); % Vector Cero.
z0 = R00(1:3,3); % Vector Columna de matriz R00
% Jacobianos de Velocidad angular
Jw1 = [z0 z00] % Jacobiano eslabon 1
Obteniendo los vectores posición de centro de masa y jacobianos
𝐿 cos 𝑞1 𝑞2 cos 𝑞1
𝑃𝑐𝑚1 = [ 𝐿 sin 𝑞1 ], 𝑃𝑐𝑚2 = [ 𝑞2 sin 𝑞1 ]
0 0
−𝐿 sin 𝑞1 0 −𝑞2 sin 𝑞1 cos 𝑞1

𝐽𝑣1 = [ 𝐿 cos 𝑞1 0], 𝐽𝑣2 = [ 𝑞2 cos 𝑞1 sin 𝑞1 ]
0 0 0 0
0 0 0 0
𝐽𝜔1 = [0 0], 𝐽𝜔2 = [0 0]
1 0 1 0
Ahora haciendo uso de las funciones ya creada en Matlab para las matrices M, C y G,
hallamos el modelo dinámico del robot RP.
% Tensores
I1 = [Ixx1 0 0;
0 Iyy1 0;
0 0 Izz1];
I2 = [Ixx2 0 0;
0 Iyy2 0;
0 0 Izz2];
% Matrices necesarias para calculo
R0 = [R01;R02];
Jv = [Jv1;Jv2];
Jw = [Jw1;Jw2];
I = [I1;I2];
% Matriz Euler-Lagrange
M = matrizMasa(m,I,R0,Jv,Jw)
C = matrizCoriolis(q,dq,M)
G = matrizGravedad(m,g0,Jv)
% Ecuacion Euler-Lagrange
tau = M*ddq+C*dq+G
Obteniendo
𝑚1 ℓ2 + 𝑚2 𝑞22 + 𝐼𝑦𝑦1 + 𝐼𝑦𝑦2 0 𝑚 𝑞̇ 𝑞 𝑚2 𝑞2 𝑞1̇

𝑀=[ ], 𝐶=[ 2 2 2 ]
0 𝑚2 −𝑚2 𝑞2 𝑞̇ 1 0
−𝑔 cos 𝑞1 (ℓ𝑚1 + 𝑑2 𝑚2 )
𝐺=[ ]
−𝑔𝑚2 sin 𝑞1
𝑀(𝑞)𝑞̈ + 𝐶(𝑞, 𝑞̇ )𝑞̇ + 𝑔(𝑞) = 𝜏
𝑚 ℓ2 + 𝑚2 𝑞22 + 𝐼𝑦𝑦1 + 𝐼𝑦𝑦2 0 𝑞̈ 2 𝑞̇ 𝑞1̇ 𝑞1̇ 𝑔 cos 𝑞1 (ℓ𝑚1 + 𝑑2 𝑚2 )

[ 1 ] [ ] + 𝑚2 𝑞2 [ 2 ][ ] − [ ⃗
]=𝟎
0 𝑚2 𝑞̈ 2 −𝑞̇ 1 0 𝑞2̇ 𝑔𝑚2 sin 𝑞1
3. Determinar las ecuaciones que describen la dinámica del robot manipulador RR, mostrado
en la figura 4. Considerar que masa de cada eslabón se encuentra concentrada al final del
mismo con valores 𝑚1 𝑦 𝑚2. Las longitudes de los eslabones son ℓ1 𝑦 ℓ2. Asumir que existe
fricción viscosa actuando sobre cada articulación con coeficientes 𝑓𝑣1 𝑦 𝑓𝑣2. El sistema de
la base se encuentra sobre la primera articulación, con el eje z apuntado hacia arriba, “Y” y
el eje “X” en la horizontal (hacia la derecha). Los tensores de inercia tienen la misma forma
que el problema anterior.
m2
ℓ2
θ1
θ2
m1
ℓ1
Figura 4. Manipulador RR.
Solución:
m2
ℓ2
z0 y1
θ1
y0 θ2
x0 m1 x1
z1
ℓ1
1 q1 0 ℓ1 90°
2 q2 0 ℓ2 0
a base con el siguiente código. Además, considerando 𝑞1 = 𝜃1 y 𝑞2 = 𝜃2
syms q1 q2 dq1 dq2 ddq1 ddq2 L1 L2 Lc1 Lc2 m1 m2 g pi

g0 = [0;-g;0];
m = [m1;m2];
q = [q1;q2];
dq = [dq1;dq2];
ddq = [ddq1;ddq2];
theta = [q1;q2];
d = [0;0];
a = [L1;L2];
alpha = [pi/2;0];
T02 = simplify(simplify(T01*T12));
R00 = eye(3)
R01 = T01(1:3,1:3)
R02 = T02(1:3,1:3)
Obteniendo las matrices de rotación
1 0 0 cos 𝑞1 0 − sin 𝑞1
0 0
𝑅0 = [0 1 0] , 𝑅1 = [ sin 𝑞1 0 cos 𝑞1 ],
0 0 1 0 1 0
cos 𝑞1 cos 𝑞2 − cos 𝑞1 sin 𝑞2 sin 𝑞1

0
𝑅2 = [ sin 𝑞1 cos 𝑞2 − sin 𝑞1 sin 𝑞2 − cos 𝑞1 ]
sin 𝑞2 cos 𝑞2 0
Luego de ello se calcula la posición de los centros de masa de cada eslabón respecto a masa
para determinar el jacobiano de velocidad lineal. Y también se halla el jacobiano de
velocidad angular a partir de los vectores de la tercera columna de las matrices de rotación
para luego hallar las matrices M, C y G, que forman el modelo dinámico del robot. Esto se
realiza con ayuda de Matlab.
% MTH del centro de masa de cada barra respecto a base

Pcm1 = T01(1:3,4)
Pcm2 = T02(1:3,4)
Jv1 = [diff(Pcm1(1),q1) 0; % Jacobiano eslabon 1
diff(Pcm1(2),q1) 0;
diff(Pcm1(3),q1) 0]
Jv2 = [diff(Pcm2(1),q1) diff(Pcm2(1),q2); % Jacobiano eslabon 2
diff(Pcm2(2),q1) diff(Pcm2(2),q2);
diff(Pcm2(3),q1) diff(Pcm2(3),q2)]
% Tensores
I1 = [Ixx1 0 0;
0 Iyy1 0;
0 0 Izz1];
I2 = [Ixx2 0 0;
0 Iyy2 0;
0 0 Izz2];
R0 = [R01;R02];
Jv = [Jv1;Jv2];
Jw = [Jw1;Jw2];
I = [I1;I2];
tau = M*ddq+C*dq+G
Obteniendo como resultado las matrices que componen el modelo dinámico
M =
[ Ixx2 + Iyy1 + L1^2*m1 + L1^2*m2 - Ixx2*cos(q2)^2 + Iyy2*cos(q2)^2 +
L2^2*m2*cos(q2)^2 + 2*L1*L2*m2*cos(q2), 0]
[
0, m2*L2^2 + Izz2]
C =
[ -dq2*((m2*sin(2*q2)*L2^2)/2 + L1*m2*sin(q2)*L2 - (Ixx2*sin(2*q2))/2
+ (Iyy2*sin(2*q2))/2), -dq1*((m2*sin(2*q2)*L2^2)/2 + L1*m2*sin(q2)*L2
- (Ixx2*sin(2*q2))/2 + (Iyy2*sin(2*q2))/2)]
[ dq1*((m2*sin(2*q2)*L2^2)/2 + L1*m2*sin(q2)*L2 - (Ixx2*sin(2*q2))/2
+ (Iyy2*sin(2*q2))/2),
0]
G =
g*cos(q1)*(L1*m1 + L1*m2 + L2*m2*cos(q2))

-L2*g*m2*sin(q1)*sin(q2)
Las cuales se reemplazan en la ecuación
𝑀(𝑞)𝑞̈ + 𝐶(𝑞, 𝑞̇ )𝑞̇ + 𝑔(𝑞) = 𝜏
4. Se muestra un robot SCARA de 4GDL.
q1 q2
ℓ3
ℓ2
q3
ℓ4
ℓ1
q4
Figura 6. Robot SCARA de 4GDL.
Donde: 𝐿1=400𝑚𝑚, 𝐿2=400𝑚𝑚, 𝐿3=300𝑚𝑚 y 𝐿4=30𝑚𝑚
a) Realizar un archivo script (simbólico) para la solución de la dinámica con el método de

Lagrange Euler, asumir como entrada posiciones angulares q1, q2, q3, q4, velocidades
articulares, q1p, q2p, q3p, q4pp, y aceleraciones articulares q1pp, q2pp, q3pp, considerar
las masas de cada eslabón m1, m2, m3, m4 que se encuentran en el punto medio del
eslabón, considerar la matriz de Inercia:
𝐼𝑥𝑥𝑖 𝑖𝑥𝑦𝑖 𝑖𝑥𝑧𝑖

𝐼𝑖 = [𝑖𝑥𝑦𝑖 𝐼𝑦𝑦𝑖 𝑖𝑦𝑧𝑖 ] , 𝑖 = 1, 2, 3, 4.
𝑖𝑥𝑧𝑖 𝑖𝑦𝑧𝑖 𝐼𝑧𝑧𝑖
b) Hacer una función numérica para evaluar los pares que se producen para las siguientes
entradas. 𝑞1=30°; 𝑞2=−15°; 𝑞3=30𝑚𝑚; 𝑞4=20°, 𝑞1̇=10°𝑠; 𝑞2̇=−5°𝑠;𝑞3̇=−8𝑚𝑚/𝑠;
𝑞4̇=2°/𝑠; 𝑞1̈=1.5°/𝑠2; 𝑞2̈=-1.5°/𝑠2; 𝑞3̈=1mm/𝑠2; 𝑞1̈=1°/𝑠2.
Solución a):
z3
q1 q2 y3
x3
y1 ℓ3
z1
ℓ2
z2 q3
y2
x1 x
y2 ℓ4
ℓ1 x2
z0 z2
q4
y0
x0
1 𝑞1 L1 L2 0°
2 𝑞2 0 L3 0°
3 0 -q3 0 0°
4 𝑞4 -L4 0 0°
a base con el siguiente código. Luego de ello se calcula la posición de los centros de masa
de cada eslabón respecto a masa para determinar el jacobiano de velocidad lineal. Y
también se halla el jacobiano de velocidad angular a partir de los vectores de la tercera
columna de las matrices de rotación para luego hallar las matrices M, C y G, que forman
el modelo dinámico del robot. Esto se realiza con ayuda de Matlab.
Código:
syms q1 q2 q3 q4 q1p q2p q3p q4p q1pp q2pp q3pp q4pp

syms L1 L2 L3 L4 Lc1 Lc2 Lc3 Lc4 m1 m2 m3 m4 g pi
g0 = [0;-g;0];
m = [m1;m2;m3;m4];
q = [q1;q2;q3;q4];
dq = [q1p;q2p;q3p;q4p];
ddq = [q1pp;q2pp;q3pp;q4pp];
%----------------------------------------------------------------------
---
theta = [q1;q2;0;q4];
d = [L1;0;-q3;-L4];
a = [L2;L3;0;0];
alpha = [0;0;0;0];
%----------------------------------------------------------------------
---
T01 = denavit(theta(1),d(1),a(1),alpha(1));
T02 = T01*T12;
T03 = T01*T12*T23;
T04 = T01*T12*T23*T34;
R00 = eye(3)
R01 = T01(1:3,1:3);
R12 = T12(1:3,1:3);
R23 = T23(1:3,1:3);
R34 = T34(1:3,1:3);
R02 = T02(1:3,1:3);
R03 = T03(1:3,1:3);
R04 = T04(1:3,1:3);
R30 = R03.';
R20 = R02.';
R10 = R01.';
Tcm01 = simplify(denavit(q1,L1/2,L2/2,0));
Pcm1 = Tcm01(1:3,4)
Tcm12 = simplify(denavit(q2,0,L3/2,0));
Tcm02 = simplify(T01*Tcm12);
Pcm2 = Tcm02(1:3,4)
Tcm23 = simplify(denavit(0,-q3/2,0,0));
Tcm03 = simplify(T01*T12*Tcm23);
Pcm3 = Tcm03(1:3,4)
Tcm34 = simplify(denavit(q4,-L4/2,0,0));
Tcm04 = simplify(T01*T12*T23*Tcm34);
Pcm4 = Tcm04(1:3,4)
%----------------------------------------------------------------------
---
Jv1 = [diff(Pcm1(1),q1) 0 0 0; % Jacobiano eslabon 1
diff(Pcm1(2),q1) 0 0 0;
diff(Pcm1(3),q1) 0 0 0]
Jv2 = [diff(Pcm2(1),q1) diff(Pcm2(1),q2) 0 0; % Jacobiano eslabon 2
diff(Pcm2(2),q1) diff(Pcm2(2),q2) 0 0;
diff(Pcm2(3),q1) diff(Pcm2(3),q2) 0 0]
Jv3 = [diff(Pcm3(1),q1) diff(Pcm3(1),q2) diff(Pcm3(1),q3) 0; %
Jacobiano eslabon 3
diff(Pcm3(2),q1) diff(Pcm3(2),q2) diff(Pcm3(2),q3) 0;
diff(Pcm3(3),q1) diff(Pcm3(3),q2) diff(Pcm3(3),q3) 0]
Jv4 = [diff(Pcm4(1),q1) diff(Pcm4(1),q2) diff(Pcm4(1),q3)

diff(Pcm4(1),q4); % Jacobiano eslabon 4
diff(Pcm4(2),q1) diff(Pcm4(2),q2) diff(Pcm4(2),q3)
diff(Pcm4(2),q4);
diff(Pcm4(3),q1) diff(Pcm4(3),q2) diff(Pcm4(3),q3)
diff(Pcm4(3),q4)]
z2 = R02(1:3,3) % Vector Columna de matriz R02
Jw1 = [z0 z00 z00 z00] % Jacobiano eslabon 1
% Tensores
I1 = [Ixx1 Ixy1 Ixz1;
Ixy1 Iyy1 Iyz1;
Ixz1 Iyz1 Izz1];
Ixy2 Iyy2 Iyz2;
Ixz2 Iyz2 Izz2];
Ixy3 Iyy3 Iyz3;
Ixz3 Iyz3 Izz3];
Ixy4 Iyy4 Iyz4;
Ixz4 Iyz4 Izz4];
R0 = [R01;R02;R03;R04];
Jv = [Jv1;Jv2;Jv3;Jv4];
Jw = [Jw1;Jw2;Jw3;Jw4];
I = [I1;I2;I3;I4];
%----------------------------------------------------------------------
---
tau = M*ddq+C*dq+G
Obteniendo como resultado las matrices que componen el modelo dinámico
M =
[ Izz1 + Izz2 + Izz3 + Izz4 + (L2^2*m1)/4 + L2^2*m2 + L2^2*m3 +
(L3^2*m2)/4 + L2^2*m4 + L3^2*m3 + L3^2*m4 + L2*L3*m2*cos(q2) +
2*L2*L3*m3*cos(q2) + 2*L2*L3*m4*cos(q2), Izz2 + Izz3 + Izz4 +
(L3^2*m2)/4 + L3^2*m3 + L3^2*m4 + (L2*L3*m2*cos(q2))/2 +
L2*L3*m3*cos(q2) + L2*L3*m4*cos(q2), 0, Izz4]
[ Izz2 + Izz3 +
Izz4 + (L3^2*m2)/4 + L3^2*m3 + L3^2*m4 + (L2*L3*m2*cos(q2))/2 +
L2*L3*m3*cos(q2) + L2*L3*m4*cos(q2),
Izz2 + Izz3 + Izz4 + (L3^2*m2)/4 + L3^2*m3 + L3^2*m4, 0,
Izz4]
[
0,
0, m3/4 + m4, 0]
[
Izz4,
Izz4, 0, Izz4]
C =
[ -(L2*L3*q2p*sin(q2)*(m2 + 2*m3 + 2*m4))/2, -
(L2*L3*sin(q2)*(q1p + q2p)*(m2 + 2*m3 + 2*m4))/2, 0, 0]
[ (L2*L3*q1p*sin(q2)*(m2 + 2*m3 + 2*m4))/2,
0, 0, 0]
[ 0,
0, 0, 0]
[ 0,
0, 0, 0]
G =
g*m3*(L3*cos(q1 + q2) + L2*cos(q1)) + g*m4*(L3*cos(q1 + q2) +
L2*cos(q1)) + g*m2*((L3*cos(q1 + q2))/2 + L2*cos(q1)) +
(L2*g*m1*cos(q1))/2
(L3*g*cos(q1 + q2)*(m2 + 2*m3 + 2*m4))/2
Las cuales se reemplazan en la ecuación
𝑀(𝑞)𝑞̈ + 𝐶(𝑞, 𝑞̇ )𝑞̇ + 𝑔(𝑞) = 𝜏
Solución b):
El procedimiento para hallar esto reemplazar valores numéricos en los simbólicos usados
en el código de Matab. Sin embargo, no se nos proporciona valores para los tensores y las
masas, por lo que se asume 𝐼1 = 𝐼2 = 𝐼3 = 𝐼4 = ⃗𝟏, y 𝑚1 = 𝑚2 = 𝑚3 = 𝑚4 = 1.
Se implementa en código en Matlab. Con la consideración de que como se hace un calculo

numerico y en el jacobiano de velcodiades lineales se usa derivada se ha ingresado en ves
de la expresión en derivadas la expresión simbólica anteriormente hallada:
Jv1 =
[ -(L2*sin(q1))/2, 0, 0, 0]
[ (L2*cos(q1))/2, 0, 0, 0]
[ 0, 0, 0, 0]
Jv2 =
[ - (L3*sin(q1 + q2))/2 - L2*sin(q1), -(L3*sin(q1 + q2))/2, 0, 0]
[ (L3*cos(q1 + q2))/2 + L2*cos(q1), (L3*cos(q1 + q2))/2, 0, 0]
[ 0, 0, 0, 0]
Jv3 =
[ - L3*sin(q1 + q2) - L2*sin(q1), -L3*sin(q1 + q2), 0, 0]
[ L3*cos(q1 + q2) + L2*cos(q1), L3*cos(q1 + q2), 0, 0]
[ 0, 0, -1/2, 0]
Jv4 =
[ - L3*sin(q1 + q2) - L2*sin(q1), -L3*sin(q1 + q2), 0, 0]
[ L3*cos(q1 + q2) + L2*cos(q1), L3*cos(q1 + q2), 0, 0]
[ 0, 0, -1, 0]
Jw1 =
0 0 0 0
0 0 0 0
1 0 0 0
Jw2 =
[ 0, 0, 0, 0]
[ 0, 0, 0, 0]
[ 1, 1, 0, 0]
Jw3 =
[ 0, 0, 0, 0]
[ 0, 0, 0, 0]
[ 1, 1, 0, 0]
Jw4 =
[ 0, 0, 0, 0]
[ 0, 0, 0, 0]
[ 1, 1, 0, 1]
Además, se toma en cuenta que la matriz Masa requiere de derivadas, por lo que también
en la matriz C colocamos la ecuación simbólica del script de la parte a.
C =
[ -(L2*L3*q2p*sin(q2)*(m2 + 2*m3 + 2*m4))/2, -(L2*L3*sin(q2)*(q1p +
q2p)*(m2 + 2*m3 + 2*m4))/2, 0, 0]
[ (L2*L3*q1p*sin(q2)*(m2 + 2*m3 + 2*m4))/2,
0, 0, 0]
[ 0,
0, 0, 0]
[ 0,
0, 0, 0]
Código:
% Datos
L1 = 400/1000;
L2 = 400/1000;
L3 = 300/1000;
L4 = 30/1000;
q1 = deg2rad(30);
q2 = deg2rad(-15);
q3 = 30/1000;
q4 = deg2rad(20);
q1p = deg2rad(10);
q2p = deg2rad(-5);
q3p = -8/1000;
q4p = deg2rad(2);
q1pp = deg2rad(1.5);
q2pp = deg2rad(-1.5);
q3pp = 1/1000;
q4pp = deg2rad(1);
m1 = 1;
m2 = 1;
m3 = 1;
m4 = 1;
g = 9.81;
Ixx1 = 1; Ixx2 = 1; Ixx3 = 1; Ixx4 = 1;
Iyy1 = 1; Iyy2 = 1; Iyy3 = 1; Iyy4 = 1;
Izz1 = 1; Izz2 = 1; Izz3 = 1; Izz4 = 1;
Ixy1 = 1; Ixy2 = 1; Ixy3 = 1; Ixy4 = 1;
Ixz1 = 1; Ixz2 = 1; Ixz3 = 1; Ixz4 = 1;
Iyz1 = 1; Iyz2 = 1; Iyz3 = 1; Iyz4 = 1;
% Matrices
g0 = [0;-g;0];
m = [m1;m2;m3;m4];
q = [q1;q2;q3;q4];
dq = [q1p;q2p;q3p;q4p];
ddq = [q1pp;q2pp;q3pp;q4pp];
%----------------------------------------------------------------------
---
theta = [q1;q2;0;q4];
d = [L1;0;-q3;-L4];
a = [L2;L3;0;0];
alpha = [0;0;0;0];
%----------------------------------------------------------------------
---
T02 = T01*T12;
T03 = T01*T12*T23;
T04 = T01*T12*T23*T34;
R00 = eye(3)
R01 = T01(1:3,1:3);
R12 = T12(1:3,1:3);
R23 = T23(1:3,1:3);
R34 = T34(1:3,1:3);
R02 = T02(1:3,1:3);
R03 = T03(1:3,1:3);
R04 = T04(1:3,1:3);
R30 = R03.';
R20 = R02.';
R10 = R01.';
Tcm01 = denavit(q1,L1/2,L2/2,0);
Pcm1 = Tcm01(1:3,4)
Tcm12 = denavit(q2,0,L3/2,0);
Tcm02 = T01*Tcm12;
Pcm2 = Tcm02(1:3,4)

Tcm23 = denavit(0,-q3/2,0,0);
Tcm03 = T01*T12*Tcm23;
Pcm3 = Tcm03(1:3,4)
Tcm34 = denavit(q4,-L4/2,0,0);
Tcm04 = T01*T12*T23*Tcm34;
Pcm4 = Tcm04(1:3,4)
%----------------------------------------------------------------------
---
Jv1 = [ -(L2*sin(q1))/2 0 0 0;
(L2*cos(q1))/2 0 0 0;
0 0 0 0]
Jv2 = [ - (L3*sin(q1 + q2))/2 - L2*sin(q1) -(L3*sin(q1 + q2))/2 0 0;
(L3*cos(q1 + q2))/2 + L2*cos(q1) (L3*cos(q1 + q2))/2 0 0;
0 0 0 0]
Jv3 = [ - L3*sin(q1 + q2) - L2*sin(q1) -L3*sin(q1 + q2) 0 0;
L3*cos(q1 + q2) + L2*cos(q1) L3*cos(q1 + q2) 0 0;
0 0 -1/2 0]
Jv4 = [ - L3*sin(q1 + q2) - L2*sin(q1) -L3*sin(q1 + q2) 0 0;
L3*cos(q1 + q2) + L2*cos(q1) L3*cos(q1 + q2) 0 0;
0 0 -1 0]
% Tensores
Ixy1 Iyy1 Iyz1;
Ixz1 Iyz1 Izz1];
Ixy2 Iyy2 Iyz2;
Ixz2 Iyz2 Izz2];
Ixy3 Iyy3 Iyz3;
Ixz3 Iyz3 Izz3];
Ixy4 Iyy4 Iyz4;
Ixz4 Iyz4 Izz4];
R0 = [R01;R02;R03;R04];
Jv = [Jv1;Jv2;Jv3;Jv4];
Jw = [Jw1;Jw2;Jw3;Jw4];
I = [I1;I2;I3;I4];
%----------------------------------------------------------------------
---
C = [ -(L2*L3*q2p*sin(q2)*(m2 + 2*m3 + 2*m4))/2 -(L2*L3*sin(q2)*(q1p +
q2p)*(m2 + 2*m3 + 2*m4))/2 0 0;
(L2*L3*q1p*sin(q2)*(m2 + 2*m3 + 2*m4))/2
0 0 0;
0
0 0 0;
0
0 0 0]
tau = M*ddq+C*dq+G
Obteniéndose los pares
19.0639
7.1295
𝜏=[ ]𝑁 ∙ 𝑚
0.0013
0.0175
Capítulo 4
Conclusiones
- Se estudió y aplicó la dinámica del robot adecuadamente.

- Se logró realizar correctamente un script en Matlab 2018 que condensa la solución de
los ejercicios propuestos.
Referencias Bibliográficas
[1] Barrientos, A. (2da Edición). (2007). Fundamentos de Robótica. Madrid, España:

McGraw-Hill/Interamericana.
[2] Reyes, F. (1era Edición). (2011). Robótica, control de robots manipuladores. C. V.,
México: Alfaomega Grupo Editor.
[3] Corke, P. (2da Edición). (2017). Robotics, Vision and Control. Berlín, Alemania: Springer

Informe de Laboratorio 7 - Modelamiento Dinámico Del Robot PDF

Cargado por

Información del documento

Título original

Derechos de autor

Formatos disponibles

Compartir este documento

Compartir o incrustar documentos

Opciones para compartir

¿Le pareció útil este documento?

¿Este contenido es inapropiado?

Copyright:

Formatos disponibles

Informe de Laboratorio 7 - Modelamiento Dinámico Del Robot PDF

Cargado por

Copyright:

Formatos disponibles

UNIVERSIDAD NACIONAL DE TRUJILLO

Dinámica del Robot – Método de Lagrange Euler

AUTORES : Aguilar Valdiviezo, Hans.

DOCENTE : Ing. Josmell H. Alva Alcántara

Este presente informe consiste en el reforzamiento y aplicación práctica de lo realizado

Estática del robot, modelamiento dinámico, Método Lagrange – Euler.

Robot static, dynamic modeling, Lagrange - Euler method.

Objetivo General. ............................................................................................................... 4

Marco Teórico ............................................................................................................................ 5

Referencias Bibliográficas ....................................................................................................... 27

Figura 1. Péndulo esférico. ........................................................................................................ 7

En el proceso del diseño de un robot además de la cinemática directa e inversa también

- Formular la Dinámica del Robot, por el método de LaGrange Euler.

- Realizar un script en Matlab R2018a que condense la solución de los ejercicios

𝑀(𝑞)𝑞̈ + 𝐶(𝑞, 𝑞̇ )𝑞̇ + 𝑔(𝑞) = 𝜏

𝐶(𝑞, 𝑞̇ ) = [ ⋮ ⋱ ⋮ ], 𝐶𝑖𝑗 = ∑ 𝐶𝑖𝑗𝑘 𝑞̇ 𝑘

1 𝜕𝑚𝑖𝑗 𝜕𝑚𝑖𝑘 𝜕𝑚𝑗𝑘

% Matriz de Coriolis o fuerzas centrifugas

a) Método de Euler – Lagrange.

𝑥̇ = ℓ(cos 𝜃 cos 𝜙 𝜃̇ − sin 𝜃 sin 𝜙 𝜙̇)

𝑦̇ = ℓ(cos 𝜃 sin 𝜙 𝜃̇ + sin 𝜃 cos 𝜙 𝜙̇)

𝑥̇ 2 = ℓ2 (cos2 𝜃 cos2 𝜙 𝜃̇ 2 + sin2 𝜃 sin2 𝜙 𝜙̇ 2 − cos 𝜃 sin 𝜃 cos 𝜙 sin 𝜙 𝜃̇ 𝜙̇)

𝑦̇ 2 = ℓ2 (cos2 𝜃 sin2 𝜙 𝜃̇ 2 + sin2 𝜃 cos 2 𝜙 𝜙̇ 2 + cos 𝜃 sin 𝜃 cos 𝜙 sin 𝜙 𝜃̇ 𝜙̇)

Luego se halla la energía cinética del sistema

𝑈 = 𝑈𝑔 = 𝑚𝑔𝑧 = −𝑚𝑔ℓ 𝑐𝑜𝑠 𝜃

𝐿{𝑞, 𝑞̇ } = 𝐾(𝑞, 𝑞̇ ) − 𝑈(𝑞)

Primera ecuación, para 𝑞1 = 𝜃 y 𝜏1 = 0

𝑚ℓ2 𝜃̈ + 𝑚𝑔ℓ sin 𝜃 − 𝑚ℓ2 sin 𝜃 cos 𝜃 𝜙̇ 2 = 0

ℓ𝜃̈ − ℓ𝜙̇ 2 sin 𝜃 cos 𝜃 + 𝑔 sin 𝜃 = 0

Segunda ecuación, para 𝑞2 = 𝜃 y 𝜏2 = 0

𝑚ℓ2 sin2 𝜃 𝜙̈ + 2𝑚ℓ2 sin 𝜃 cos 𝜃 𝜃̇ 𝜙̇ = 0

sin2 𝜃 𝜙̈ + 2 sin 𝜃 cos 𝜃 𝜃̇ 𝜙̇ = 0

De las ecuaciones anteriormente hallas se puede construir una expresión matricial de la

𝑀(𝑞)𝑞̈ + 𝐶(𝑞, 𝑞̇ )𝑞̇ + 𝑔(𝑞) = 0

2. Determinar el modelo dinámico del robot manipulador RP de 2 GDL mostrado en la figura

Figura 2. Manipulador RP.

Figura 3. Sistemas de coordenadas orientados de la cinemática directa.

Tabla 1. Parámetros de Denavit Hartenberg.

syms q1 q2 dq1 dq2 ddq1 ddq2 L m1 m2 g pi

% Parámetros de Denavit Hartenberg

Obteniendo las matrices de rotación

1 0 0 − sin 𝑞1 0 cos 𝑞1 − sin 𝑞1 0 cos 𝑞1

% MTH del centro de masa de la barra 1 respecto a base

Obteniendo los vectores posición de centro de masa y jacobianos

−𝐿 sin 𝑞1 0 −𝑞2 sin 𝑞1 cos 𝑞1

𝑚1 ℓ2 + 𝑚2 𝑞22 + 𝐼𝑦𝑦1 + 𝐼𝑦𝑦2 0 𝑚 𝑞̇ 𝑞 𝑚2 𝑞2 𝑞1̇

𝑀(𝑞)𝑞̈ + 𝐶(𝑞, 𝑞̇ )𝑞̇ + 𝑔(𝑞) = 𝜏

𝑚 ℓ2 + 𝑚2 𝑞22 + 𝐼𝑦𝑦1 + 𝐼𝑦𝑦2 0 𝑞̈ 2 𝑞̇ 𝑞1̇ 𝑞1̇ 𝑔 cos 𝑞1 (ℓ𝑚1 + 𝑑2 𝑚2 )

Figura 4. Manipulador RR.

Figura 5. Sistemas de coordenadas orientados de la cinemática directa.

Tabla 2. Parámetros de Denavit Hartenberg.

syms q1 q2 dq1 dq2 ddq1 ddq2 L1 L2 Lc1 Lc2 m1 m2 g pi

Obteniendo las matrices de rotación

cos 𝑞1 cos 𝑞2 − cos 𝑞1 sin 𝑞2 sin 𝑞1

% MTH del centro de masa de cada barra respecto a base

Obteniendo como resultado las matrices que componen el modelo dinámico

g*cos(q1)*(L1*m1 + L1*m2 + L2*m2*cos(q2))

Las cuales se reemplazan en la ecuación

𝑀(𝑞)𝑞̈ + 𝐶(𝑞, 𝑞̇ )𝑞̇ + 𝑔(𝑞) = 𝜏

4. Se muestra un robot SCARA de 4GDL.

Figura 6. Robot SCARA de 4GDL.

Donde: 𝐿1=400𝑚𝑚, 𝐿2=400𝑚𝑚, 𝐿3=300𝑚𝑚 y 𝐿4=30𝑚𝑚

a) Realizar un archivo script (simbólico) para la solución de la dinámica con el método de

gcos(q1)(L1m1 + L1m2 + L2m2cos(q2))

(L3gcos(q1 + q2)(m2 + 2m3 + 2*m4))/2