UNIVERSIDAD CATÓLICA DE SANTA MARIA

FACULTAD DE CIENCIAS E INGENIERIAS

FÍSICAS Y FORMALES

ESCUELA PROFESIONAL DE INGENIERÍA

MECÁNICA, MECÁNICA-ELÉCTRICA Y MECATRÓNICA

CURSO:
ROBOTICA 1

LABORATORIO 9:

MODELO DINÁMICO – ALGORITMOS

COMPUTACIONALES

PROFESOR:

Ing. JUAN CARLOS CUADROS

ALUMNOS:

 Richard Laura Huarcaya

 Juan Jose Cordova Gonzales

AREQUIPA-PERÚ
2015
I. OBJETIVO

I.1. Obtener el modelo dinámico de un robot aplicando las formulaciones de Newton-Euler y

Lagrange.

I.2. Obtener el modelo dinámico de un robot aplicando los algoritmos computacionales de

Lagrange y Newton.

I.3. Resolver problemas de dinámica del robot aplicando los conceptos de modelo dinámico y los
algoritmos computacionales de Lagrange y Newton, empleando en la solución herramientas
matemáticas y de software.

II. MARCO TEORICO

NEWTON-EULER

La obtención del modelo dinámico se basa fundamentalmente en el planteamiento del equilibrio

de fuerzas establecido en la segunda ley de Newton, o su equivalente para movimientos de
rotación, la denominada ley de Euler:

LAGRANGE
El modelo dinámico del robot manipulador se obtiene por medio de las ecuaciones de Euler-Lagrange , como
primer paso se deben obtener las energías cinéticas de los eslabones del robot manipulador. En un robot
manipulador la energía cinética se encuentra conformada por suma de la energía cinética traslacional y la
energía cinética rotacional.

Enseguida, se obtienen las energías potenciales de los eslabones del robot manipulador.

Como tercer paso se obtiene el Lagrangiano del sistema. El cual se encuentra determinado por la diferencia
entre la energía cinética total1 y la energía potencial total.

Enseguida, se desarrolla la siguiente ecuación para cada una de las coordenadas articulares.
II.1. MODELO DINÁMICO

II.1.1. Formulaciones de Newton_Euler y Lagrange

II.1.2. Algoritmo de Lagrange

II.1.3. Algoritmo de Newton

(Desarrollar un breve resumen teórico del tema)

III. MATERIAL Y EQUIPO

III.1. Una PC con SO Windows XP y MATLAB

IV. PROCEDIMIENTO

IV.1. Obtener las ecuaciones del movimiento del robot de la figura, considerando que la masa de
cada uno de los enlaces se concentra en su centro de masas. Emplear los algoritmos
computacionales de Lagrange y Newton en la solución. Aplicar MATLAB.

ALGORITMO COMPUTACIONALE DE LAGRANGE Y NEWTON

DENAVIT HARTENBERG
MATLAB

NEWTON-EULER

Ecuaciones que rigen el sistema:

d d
∑ F= dt ( m v ) ;∑ T = dt (I ω)

2 d
F 2=m2 d̈ 2 +m 2 g S1−m2 d 2 θ̈ 1 ; T 1=
dt
( I θ̇1 ) + m2 g d 2 C 1
Momento de inercia de la masa 2:
2
I =m 2 d 2
d
¿
dt

F 2=m2 d̈ 2 +m 2 g S1−m2 d 2 θ̇ 21 ; T 1=2 m2 d 2 θ̇ 1 ḋ 2 +m2 d 22 θ̈1 +m2 g d 2 C1

Forma matricial:

[ ][
T1
F2
m d2 0 θ̈1
= 2 2
0
+
0
][ ] [
m2 d̈ 2 −m2 d2 θ̇1
2 m2 d2 θ̇1 θ̇ 1 m2 g d 2 C 1
0 ḋ 2
+
m2 g S 1 ][ ] [ ]

LAGRANGE
Ecuaciones que rigen el sistema:

 Energía cinética:

1
Ec= I∗θ̇ =m2∗( ḋ 2 +d 2 θ̇ 1 )
2 2 2 2
2

 Energía potencial:

E p=m2∗¿

LAGRANGE

1
L= ∗m ∗( ḋ 2+ d 2 θ̇1 ) −[m2∗(d 2∗S 1∗g)]
2 2 2
2 2

d ∂L ∂L
T= −
dt ∂ q̇ 1 ∂ q 1
2
T 1=m 2 d 2 θ̈1 +2 m2 d 2 θ̇ 1 ḋ 2 + g m2 d 2 C1
2
F 2=m2 d̈ 2−m2 d 2 θ̇ 1+ g m2 S 1
En forma matricial:

[ ][ ][ ] [ ][ ] [ ]
2
T1 m2 d2 0 θ̈1 0 2 m2 d2 θ̇1 θ̇ 1 m2 g d 2 C 1
= + +
F2 0 m2 d̈ 2 −m 2 d2 θ̇1 0 ḋ 2 m2 g S 1
Método computacional
syms t
%definicion simbolica de la posicion
%algoritmo de Lagrange para la solucion del modelo dinamico
%definicion de la trayectoria
%definicion simbolica de la trayectoria de posicion
sq1=sin(2*t)+pi/2;
sq2=t^2;
%obtencion simbolica de la velocidad y acelracion
sqd1=diff(sq1,'t');sqdd1=diff(sqd1,'t');
sqd2=diff(sq2,'t');sqdd2=diff(sqd2,'t');
%parametros dimensionales
L1=10;
m1=20;
m2=10;
g=9.8;
G=[g 0 0 0];
r11=[0 0 L1 1]';
r22=[0 0 0 1]';
%Matrices de derivacion
Qr=zeros(4);Qr(1,2)=-1;Qr(2,1)=1;
Qd=zeros(4);Qd(3,4)=1;
%bucle de paso de
tiempo
for tk=1:1:50;
t=(tk-1)/10;
%evaluacion numerica e posicion, velocidad y aceleeracion
q1=eval(sq1);Q1(tk)=q1 ;qd1=eval(sqd1) ;qdd1=eval(sqdd1);
q2=eval(sq2);Q2(tk)=q2 ;qd2=eval(sqd2) ;qdd2=eval(sqdd2);
%pasos 1 y 2 obtencion de las matrices de tranformacion Aij
%matrices A00 y A11 son la identidad
A00=eye(4);
A11=eye(4);
%evalua las matrices A01 y A02
A01 = [cos(q1) 0 -sin(q1) 0;sin(q1) 0 cos(q1) 0; 0 -1 0 0;0 0 0 1];
A12 = [1 0 0 0; 0 1 0 0;0 0 1 q2;0 0 0 1];
%Evalua la matriz A02=A011*A2
A02 = A01*A12;
%PASO 3 evaluacion matrices Uij
U11=A00*Qr*A01;
U12=zeros(4);
U21=A00*Qr*A02;
U22=A01*Qd*A12;

%Paso 4 Evaluacion de las matrices Uijk

%-------------------------------------
U111=A00*Qr*A00*Qr*A01;
U112=zeros(4);
U121=zeros(4);
U122=zeros(4);
U211=A00*Qr*A00*Qr*A02;
U212=A00*Qr*A01*Qd*A12;
U221=A00*Qr*A01*Qd*A12;
U222=A01*Qd*A11*Qd*A12;
%PASO 5 evaluacion mtraices de pseudoinercia Ji
J1=zeros(4);J1(3,3)=L1^2*m1;J1(3,4)=L1*m1;J1(4,3)=L1*m1;J1(4,4)=m1;
J2=zeros(4);J2(4,4)=m2;

%PASO 6 Evaluacion de matriz de Inercias D

D(1,1)=trace(U11*J1*U11')+trace(U21*J2*U21');
D(1,2)=trace(U22*J2*U21');
D(2,1)=D(1,2);
D(2,2)=trace(U22*J2*U22');

%PASO 7-8 Evaluacion de la matriz Coriolis h

h111=trace(U111*J1*U11')+trace(U211*J2*U21');
h112=trace(U212*J2*U21');
h121=trace(U221*J2*U21');
h122=trace(U222*J2*U21');
h211=trace(U211*J2*U22');
h212=trace(U212*J2*U22');
h221=trace(U221*J2*U22');
h222=trace(U222*J2*U22');

H(1,1)=h111*qd1*qd1+h112*qd1*qd2+h121*qd2*qd1+h122*qd2*qd2;
H(2,1)=h211*qd1*qd1+h212*qd1*qd2+h221*qd2*qd1+h222*qd2*qd2;

%PASO 9 evaluacion de la matriz de Gravedad C

C(1,1)=-m1*G*U11*r11-m2*G*U21*r22;
C(2,1)=-m1*G*U12*r11-m2*G*U22*r22;

%PASO 10 Evaluacion de los pares

PARES(:,tk)=D*[qdd1 qdd2]'+H+C;

%FIN DEL ALGORITMO

%VERIFICACION DE RESULTADOS

%Este codigo es unicamente a efectos de verificar

%que la programacion del algoritmo es correcto
%obtiene la solucion analitica obtenidad manualmente
%(expresion [5.18]

T1=(m1*L1^2+m2*q2^2)*qdd1+2*q2*m2*qd1*qd2;
T1=T1+g*(m1*L1*cos(q1)+m2*q2*cos(q1));
F2= m2*qdd2-q2*m2*qd1^2+m2*g*sin(q1);
PARES2(1,tk)=T1;
PARES2(2,tk)=F2;
end
% fin del bucle de paso de tiempo

%presentacion grafica de los resultados

figure(1);clf
subplot(2,2,1),plot(Q1),grid,legend(strcat('Q1:','sin(2*t)+pi/2'),0)
subplot(2,2,2),hold,
plot(PARES(1,:)),plot(PARES2(1,:),'rx')
grid,title(' PAR Q1 Alg.Lagrange'),hold

subplot(2,2,3),plot(Q2),grid,legend(strcat('Q2:','t'),0)
subplot(2,2,4),hold
plot(PARES(2,:)),plot(PARES2(2,:),'rx')
grid,title(' PAR Q2 Alg.Lagrange'),hold
Obtener las ecuaciones del movimiento del robot de la figura, considerando que la masa de cada
uno de los enlaces se concentra en su centro de masas.

LAGRANGE

Primer Grado

1 2
∗M 1∗l
2 2 M ∗g∗l
L= ∗θ̇1 − 1 ∗sin ⁡(θ1 )
4 2
d dL dL
− =τ
dt d θ̇1 d θ 1

M 1∗l 2 M 1∗g∗l
∗θ̈1− ∗cos ( θ1 )=τ
4 2
Segundo grado

1
∗M 2∗l 2
2
L= ∗θ̇22−M 2∗g∗(l∗sin ( θ1 ) +l∗sen ( θ2 ) )
4
d dL dL
− =τ
dt d θ̇1 d θ 1
2 2
M 1∗l M ∗g∗l M ∗l M ∗g∗l
∗θ̈1+ 1 ∗cos ( θ 1) + 2 ∗θ¨2 − 2 ∗cos ( θ 2 )=τ
4 2 4 2

EULER

∑ τ=I∗θ̈+ θ̇∗I∗θ̇
M 1∗l 2 M 1∗g∗l
∗θ̈1− ∗cos ( θ1 )=τ
4 2
 2 2
M 1∗l M 1∗g∗l M 2∗l ¨ M 2∗g∗l
∗θ̈1+ ∗cos ( θ 1) + ∗θ2 − ∗cos ( θ 2 )=τ
4 2 4 2
Sumando terminos

M 1∗l 2 M 1∗g∗l M 2∗l 2 M 2∗g∗l

∗θ̈1+ ∗cos ( θ 1) + ∗θ¨2 − ∗cos ( θ 2 )+
4 2 4 2
2
M 1∗l M 1∗g∗l
∗θ̈1− ∗cos ( θ1 )=τ
4 2

Modelo Computacional
clc
clear all
close all
syms q1 q2 l1 l2 lc1 lc2 m1 m2 real
syms v1 v2 a1 a2 g real
%TABLA DE PARAMETROS
MP=[ q1-pi/2 0 l1 0; q2 0 l2 0 ]
%MATRICES DE TRANSFORMACION
A01=[ cos(MP(1,1)), (-1)*cos(MP(1,4))*sin(MP(1,1)),
sin(MP(1,4))*sin(MP(1,1)), MP(1,3)*cos(MP(1,1)) ; sin(MP(1,1)),
(cos(MP(1,4)))*cos(MP(1,1)), -sin(MP(1,4))*cos(MP(1,1)),
(MP(1,3))*sin(MP(1,1)) ; 0 (sin(MP(1,4))) cos(MP(1,4)) (MP(1,2)) ; 0 0 0
1 ]
A12= [cos(MP(2,1)) -cos(MP(2,4))*sin(MP(2,1)) (sin(MP(2,4)))*sin(MP(2,1))
(MP(2,3))*cos(MP(2,1)) ; sin(MP(2,1)) (cos(MP(2,4)))*cos(MP(2,1)) -
sin(MP(2,4))*cos(MP(2,1)) MP(2,3)*sin(MP(2,1)) ; 0 sin(MP(2,4))
cos(MP(2,4)) MP(2,2) ;0 0 0 1 ]
%MATRIZ DE TRANFORMACION HOMEGEA FINAL
A02=A01*A12;
A02=simple(A02)
% PASO 3 Evaluacion matrices Uij
%--------------------------------
%Matrices de Derivación Qr y Qd
Qr=zeros(4);Qr(1,2)=-1;Qr(2,1)=1;
Qd=zeros(4);Qd(3,4)=1;
% Matrices A00 y A11 son la identidad
A00=eye(4);
A11=eye(4);
A22=eye(4);
%DESARROLLO
U11=A00*Qr*A01
U12=zeros(4)
U21=A00*Qr*A02
U22=A01*Qr*A12
%PASO 4 Evaluación de las matrices Uijk
%--------------------------------------
U111=A00*Qr*A00*Qr*A01
U112=zeros(4)
U121=zeros(4)
U122=zeros(4)
U211=A00*Qr*A00*Qr*A02
U212=A00*Qr*A01*Qr*A12
U221=A00*Qr*A01*Qr*A12
U222=A01*Qr*A11*Qr*A12
% PASO 5 Evaluación matrices de pseudoinercia Ji
%-----------------------------------------------
xc2=-(l2-lc2)
yc2=0
zc2=0
xc1=-(l1-lc1)
yc1=0
zc1=0
%J1=zeros(4);J1(1,1)=xc1*m1;J1(1,4)=xc1*m1;J1(4,1)=xc1*m1;J1(4,4)=m1;
J1=[xc1^2*m1 xc1*yc1*m1 xc1*zc1*m1 xc1*m1;
xc1*yc1*m1 yc1^2*m1 yc1*zc1*m1 yc1*m1;
zc1*xc1*m1 zc1*yc1*m1 zc1^2*m1 zc1*m1;
xc1*m1 yc1*m1 zc1*m1 m1]
%J2=zeros(4);J2(1,1)=xc2*m2;J2(1,4)=xc2*m2;J2(4,1)=xc2*m1;J2(4,4)=m2;
J2=[xc2^2*m2 xc2*yc2*m2 xc2*zc2*m2 xc2*m2;
xc2*yc2*m2 yc2^2*m2 yc2*zc2*m2 yc2*m2;
zc2*xc2*m2 zc2*yc2*m2 zc2^2*m2 zc2*m2;
xc2*m2 yc2*m2 zc2*m2 m2]
% PASO 6 Evaluación de la matriz de Inercias D
D(1,1)=trace(U11*J1*U11')+trace(U21*J2*U21')
D(1,2)=trace(U22*J2*U21')
D(2,1)=D(1,2)
D(2,2)=trace(U22*J2*U22')
% PASO 7-8 Evaluación de la matriz de Coriolis H
h111=trace(U111*J1*U11')+trace(U211*J2*U21')
h112=trace(U212*J2*U21')
h121=trace(U221*J2*U21')
h122=trace(U222*J2*U21')
h211=trace(U211*J2*U22')
h212=trace(U212*J2*U22')
h221=trace(U221*J2*U22')
h222=trace(U222*J2*U22')
H(1,1)=h111*v1*v1 + h112*v1*v2 + h121*v2*v1 + h122*v2*v2;
H(2,1)=h211*v1*v1 + h212*v1*v2 + h221*v2*v1 + h222*v2*v2;
% PASO 9 Evaluación de la matriz de Gravedad C
G=[0 -g 0 0];
r11=[ xc1 0 0 1]';
r22=[xc2 0 0 1]';
C(1,1)=-m1*G*U11*r11-m2*G*U21*r22;
C(2,1)=-m1*G*U12*r11-m2*G*U22*r22;
% PASO 10 Evaluación de los pares
PARES=D*[a1 a2]'+H+C
simple(PARES)
COMCLUSIONES

 El método computación nos permite un cálculo mucho más rápido y efectivo para sus
operación y cálculos deseados de un manipulador.
 Con el algoritmo computacional nos permite visualizar el movimiento y el valor de los
torques del brazo, para poder analizar y dimensionar efectivamente.
 Podemos desarrollar este análisis para cualquier brazo de n grados pero el inconveniente
es resolverlo manualmente la solución está en el algoritmo computacional
 El algoritmo permite un análisis rápido y efectivo para acelerar los cálculos y poder simular
su comportamiento.