Universidad Católica de Santa

María
Facultad de Ciencias e Ingenierías Físicas y
Formales

Programa Profesional de Ingeniería Mecánica,

Mecánica Eléctrica y Mecatronica
Curso:
Robótica I

Profesor:
Ing. Juan Carlos Cuadros

Tema:
Examen de Tercera Fase

Alumnos:
Valencia Nieto, Jonathan Melvin
Zárate Cáceres, Mario Eduardo

Fecha de Presentación:
09/07

Arequipa –2012

Robótica 1: Examen de tercera Fase

Problema 1
1. Los vínculos de un manipulador RP que se muestran en la figura siguiente, tienen los siguientes
tensores de inercia referidos a sus centros de masa:

1
Y las masas totales m1 y m2. Como se muestra en la figura, el centro de masas del vínculo 1 se
encuentra una distancia L1 del eje de la articulación 1, el centro de masas del vínculo 2 se
encuentra a una distancia variable d2 del eje de la articulación 1. Use dinámica Lagrangiana
(Algoritmo) para determinar el modelo dinámico inverso y las ecuaciones de movimiento del
manipulador. Compruebe los resultados obtenidos empleando el algoritmo recursivo de Newton-
Euler.

Para desarrollar este ejemplo, asumiremos los dos

elementos de traslación como uno solo. Esta
suposición tiene validez, porque la traslación del
segundo elemento implica introducirse en el primero,
lo cual afectaría en el centro de gravedad de este, lo
cual complicaría notablemente la solución del
problema. Para solucionarlo sumaremos sus masas y
lo consideraremos como un solo elemento y la única
distancia a considerar será (distancia del origen al
centro de gravedad de ambos elementos
considerados como uno solo).

Además se observa que es constante y el que varia es el , asi que para fines de análisis solo
consideraremos el , que representara el movimiento de ambos, como ya se ha explicado.
Así se tiene:

2
Las expresiones de Newton-Euler:

Se tiene:

El momento inercial de la masa total es:

Por lo que se tendrá:

Remplazando:

En forma matricial:

Formulación de Lagrange:

 Posiciones:

3
  Velocidades:

 Energía cinética:

 Energía Potencial:

 Lagrange:

 Derivadas respecto a :

 Derivadas respecto a :

Formula de Lagrange

4
Expresado matricialmente:

Esta expresión es la misma hallada por el método de Newton-Euler.

Utilizando métodos Computacionales basados en DH

Algoritmo de Newton Euler [1.1]

Parámetros DH del robot:

Articulación
1 0 0
2 0 0 0

No se tienen fuerzas extremas:

Inercias:

Por estar ambas masas concentradas en su respectivo centro de gravedad.

Las matrices de Rotación son:

Se procedió a evaluarlo en Matlab, simulando una entrada:

5
st1='sin(3*t)*t+pi/3'; %Entrada de q1.
sd2='cos(5*t)+t+4'; %Entrada de d2.
% Parámetros dimensionales
L1=5;
m1=8;
m2=3;
g=9.81;

Y se obtuvieron los siguientes resultados:

Algoritmo de Lagrange [1.2]

Para este caso, son necesarias las MTH por D-H.

% Parámetros dimensionales
l1=5;
m1=8;
m2=3;
g=9.81;
G=[g 0 0 0];
r11=[0 0 l1 1]';
r22=[0 0 0 1]';

6
  Matrices Pseudoinversas:
Como nuestro plano esta en 2-D, solo tendremos inercias en .

Como se puede ver, el error en ambos métodos es mínimo, obteniéndose una muy buena
aproximación.

Problema 2

7
2. Derive el modelo dinámico inverso y las ecuaciones de movimiento para el manipulador PR de la
figura siguiente. Ignore la fricción pero incluya la gravedad. Los tensores de inercia de los vínculos
son diagonales, con los momentos Ixx1, Iyy1, Izz1 y Ixx2, Iyy2, Izz2.

Los centros de masa para los vínculos se proporcionan mediante:

Use dinámica lagrangiana (Algoritmo) para determinar el modelo dinámico inverso y las
ecuaciones de movimiento del manipulador. Compruebe los resultados obtenidos empleando el
algoritmo recursivo de Newton-Euler.

Formulación de Lagrange

8
Como se ve en el grafico el valor de , por tanto su velocidad es la misma:

 Energía Cinética

 Energía potencial

Formula de Lagrange

 Derivadas respecto a :

9
  Derivadas respecto a :

 Derivadas respecto a :

 Derivadas respecto a :

 Finalmente se tiene:

Como se observa, el centro de gravedad del objeto 2 esta en el origen, por lo que es igual a 0,
anulándose el Torque y solo quedándonos una Fuerza:

10
Método de Newton-Euler:

Como se explico anteriormente , por tanto su velocidad es la misma por ambos

conforman un mismo eslabón. Con esto se puede afirmar que:

Como se sabe, el centro de gravedad del objeto 2 esta en el origen, por lo que es igual a 0,
haciendo el Torque 1 “0” y solo habiendo una Fuerza.

11
 Utilizando métodos Computacionales basados en DH

Algoritmo de Newton Euler [2.1]

Parámetros DH del robot:

Articulación
1 0 0
2 0

No se tienen fuerzas extremas:

Inercias:

Por estar ambas masas concentradas en su respectivo centro de gravedad.

12
Las matrices de Rotación son:

Se procedió a evaluarlo en Matlab, simulando una entrada:

sd1='sin(3*t)+t+2'; %Entrada de d1.
st2='cos(t)*t+pi/6'; %Entrada de q2
% Parámetros dimensionales
L1=10;
L2=5;
m1=20;
m2=10;
g=9.81;
Y se obtuvieron los siguientes resultados:

Algoritmo de Lagrange [2.2]

% Parámetros dimensionales
l1=10;
l2=5;
m1=20;
m2=10;
g=9.81;
G=[g 0 0 0];

13
r11=[0 0 -l1 1]';
r22=[0 0 0 1]';

 Matrices Pseudoinversas:
Como nuestro plano esta en 2-D, solo tendremos inercias en .

En este caso, no existe error en ninguno de los métodos, con lo cual comprobamos que nuestra
resolución es correcta.

14
 Anexos, Programas en MatLab
Problema 1
[1.1] Método de Newton Euler
clc,clear all
% Algoritmo de Newton Euler
% Para la solución del modelo dinámico inverso de un robot
%----------------------------------------------------------
% Robot cilíndrico 2 GDL (RD) (Rotacion y desplazamiento)
% Problema 1
% Numero de puntos de la trayectoria
n=50;
% Definición de la trayectoria
%------------------------------
% Definición simbólica de la posición
st1='sin(3*t)*t+pi/3';
sd2='cos(5*t)+t+4';

%Definicion de fuerzas en el extremo

%--------------------------------
sf3='[0 00 0]'; % Sustituir en cada eje en caso de querer aplicar
sn3='[0 0 0]'; %Fuerzas o pares extremos

% Obtención simbólica de la velocidad y aceleración

svt1=diff(st1,'t');
svd2=diff(sd2,'t');
sat1=diff(svt1,'t');
sad2=diff(svd2,'t');

% Parámetros dimensionales
L1=5;
m1=8;
m2=3;
g=9.81;

%Parametros iniciales
w0=[0 0 0]';
dw0=[0 0 0]';
v0=[0 0 0]';
dv0=-[0 -g 0]';%Gravedad en -y
z0=[0 0 1]';

%Matriz de rotacion entre {s2} y TCP {s3}

R23=eye(3);

% Bucle de paso de tiempo

%--------------------------
for tk=1:1:n;
t=(tk-1)/10;
% Evaluación numérica de posición, velocidad y aceleración

15
% y trayectoria del TCP
%--------------------------------------------------------
t1=eval(st1);
vt1=eval(svt1);
at1=eval(sat1);
d2=eval(sd2);
vd2=eval(svd2);
ad2=eval(sad2);

q(tk,1)=t1;
qd(tk,1)=vt1;
qdd(tk,1)=at1;
q(tk,2)=d2;
qd(tk,2)=vd2;
qdd(tk,2)=ad2;
XY(tk,1)=L1*cos(t1)+d2*cos(t1);
XY(tk,2)=L1*sin(t1)+d2*sin(t1);

%Evaluacion numerica de las fuerzas y pares en el TCP

%---------------------------------------
n3=eval(sn3)';
f3=eval(sf3)';
n_3(tk,:)=n3;
f_3(tk,:)=f3;

% PASO 1-2 Evaluacion de los vectores p,s y de la matriz de inercias

%---------------------------------------------------------
p1=[0 0 0]';
p2=[0 0 d2]';
s1=[0 0 L1]';
s2=[0 0 0]';
I1=zeros(3);
I2=zeros(3);

%PASO 3
%----------------------------------------------
R01=[cos(t1) 0 -sin(t1);sin(t1) 0 cos(t1); 0 -1 0];
R12=[1 0 0;0 1 0; 0 0 1];

%PASO 4
%----------------------------------------------
w1=R01'*(w0+z0*vt1);%Rotacion
w2=R12'*w1;%Traslacion

%PASO 5
%----------------------------------------------
dw1=R01'*(dw0+z0*at1)+cross(w0,z0*vt1);%Rotacion
dw2=R12'*dw1;%Traslacion

%PASO 6
%----------------------------------------------
dv1=cross(dw1,p1)+cross(w1,cross(w1,p1))+R01'*dv0;%Rotacion
dv2=R12'*(z0*ad2+dv1)+cross(w2,p2)+cross(2*w2,R12*z0*vd2)+cross(w2,cross(
w2,p2));%Traslacion

%PASO 7

16
%----------------------------------------------
a1=cross(dw1,s1)+cross(w1,cross(w1,s1))+dv1;
a2=cross(dw2,s2)+cross(w2,cross(w2,s2))+dv2;

%PASO 8
%----------------------------------------------
f2=R23*f3+m2*a2;
f1=R12*f2+m1*a1;

%PASO 9
%----------------------------------------------
n2=R23*(n3+cross((R23'*p2),f3))+cross((p2+s2),m2*a2)+I2*dw2+cross(w2,
(I2*w2));
n1=R12*(n2+cross((R12'*p1),f2))+cross((p1+s1),m1*a1)+I1*dw1+cross(w1,
(I1*w1));

%PASO 10
%----------------------------------------------
P1=n1'*R01'*z0;
P2=f2'*R12'*z0;
PARES(tk,1)=P1;
PARES(tk,2)=P2;

%Fin del algoritmo

%------------------------------------------------

%Verificacion del resultado

T1=(at1*d2^2+2*vt1*vd2*d2)*m2+at1*L1^2*m1;
F2=m2*ad2-d2*m2*vt1^2+m2*g*sin(t1);
PARES2(tk,1)=T1;
PARES2(tk,2)=F2;

end % Fin del bucle de tiempo

figure(1);clf
subplot(2,2,1),hold;
grid,plot(q(:,1)),title('Q1'),legend(strcat('Q1:',st1),0),hold;
subplot(2,2,2),hold;
grid,title('PAR Q1 Alg,NE (-), SOl. analitica (x)')
plot(PARES(:,1)),plot(PARES2(:,1),'rx'),hold;
subplot(2,2,3),hold;
grid,plot(q(:,2)),title('Q2'),legend(strcat('Q2:',sd2),0),hold;
subplot(2,2,4),hold;
grid,title('Fuerza Q2 Alg,NE (-), SOl. analitica (x)')
plot(PARES(:,2)),plot(PARES2(:,2),'rx'),hold;

[1.2] Método de Lagrange

clc,clear all
% Algoritmo de Lagrange
% Para la solución del modelo dinámico inverso de un robot
%----------------------------------------------------------
% Robot cilíndrico 2 GDL (RD)
% Problema 1
% Definición de la trayectoria
%------------------------------
% Definición simbólica de la posición
st1='sin(3*t)*t+pi/3';

17
sd2='cos(5*t)+t+4';

% Obtención simbólica de la velocidad y aceleración

svt1=diff(st1,'t');
svd2=diff(sd2,'t');
sat1=diff(svt1,'t');
sad2=diff(svd2,'t');
% Parámetros dimensionales
l1=5;
m1=8;
m2=3;
g=9.81;
G=[g 0 0 0];
r11=[0 0 l1 1]';
r22=[0 0 0 1]';
%Matrices de Derivación
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;
% Evaluación numérica de posición, velocidad y aceleración
%--------------------------------------------------------
t1=eval(st1);Q1(tk)=t1;
d2=eval(sd2);Q2(tk)=d2;
vt1=eval(svt1);
vd2=eval(svd2);
at1=eval(sat1);
ad2=eval(sad2);
% PASO 1-2 Obtención de las matrices de transformación Aij
%---------------------------------------------------------
% Matrices A00 y A11 son la identidad
A00=eye(4);
A11=eye(4);
%Evalua las matrices A01 y A02
A01 =[cos(t1) 0 -sin(t1) 0;
sin(t1) 0 cos(t1) 0;
0 -1 0 0;
0 0 0 1];
A12 =[1 0 0 0 ;
0 1 0 0 ;
0 0 1 d2;
0 0 0 1 ];
%Evalua la matriz A02=A01*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 Evaluación de las matrices Uijk
%--------------------------------------
U111=A00*Qr*A00*Qr*A01;
U112=zeros(4);
U121=zeros(4);

18
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 Evaluación matrices 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 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*vt1*vt1 + h112*vt1*vd2 + h121*vd2*vt1 + h122*vd2*vd2;
H(2,1)=h211*vt1*vt1 + h212*vt1*vd2 + h221*vd2*vt1 + h222*vd2*vd2;
% PASO 9 Evaluación 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 Evaluación de los pares
%----------------------------------
pares(:,tk)=D*[at1 ad2]'+H+C;
%end
%findelalgoritmo
%---------------
%Verificacion del resultado
T1=(m1*l1^2+m2*d2^2)*at1+2*d2*m2*vt1*vd2;
T1=T1+g*(m1*l1*cos(t1)+m2*d2*cos(t1));
F2=m2*ad2-d2*m2*vt1^2+m2*g*sin(t1);
pares2(1,tk)=T1;
pares2(2,tk)=F2;
end
% fin del bucle de paso de tiempo
% Presentación Gráfica de los resultados
figure(2);clf
subplot(2,2,1),plot(Q1),title('Q1'),grid,legend(strcat('Q1:',st1),0)
subplot(2,2,2),hold on,
plot(pares(1,:)),plot(pares2(1,:),'rx')
grid,title('PAR Q1 Alg. Lagrange'),hold

19
subplot(2,2,3),plot(Q2),title('Q2'),grid,legend(strcat('Q2:',sd2),0)
subplot(2,2,4),hold
plot(pares(2,:)),plot(pares2(2,:),'rx')
grid,title(' Fuerza Q2 Alg.Lagrange'),hold

Problema 2
[2.1] Método de Newton Euler
clc,clear all
% Algoritmo de Newton Euler
% Para la solución del modelo dinámico inverso de un robot
%----------------------------------------------------------
% Robot cilíndrico 2 GDL (DR) (Desplazamiento y Rotacion)
% Problema 2
% Numero de puntos de la trayectoria
n=50;
% Definición de la trayectoria
%------------------------------
% Definición simbólica de la posición
sd1='sin(3*t)+t+2';
st2='cos(t)*t+pi/6';

%Definicion de fuerzas en el extremo

%--------------------------------
sf3='[0 00 0]'; % Sustituir en cada eje en caso de querer aplicar
sn3='[0 0 0]'; %Fuerzas o pares extremos

% Obtención simbólica de la velocidad y aceleración

svd1=diff(sd1,'t');
svt2=diff(st2,'t');
sad1=diff(svd1,'t');
sat2=diff(svt2,'t');

% Parámetros dimensionales
L1=10;
L2=5;
m1=20;
m2=10;
g=9.81;

%Parametros iniciales
w0=[0 0 0]';
dw0=[0 0 0]';
v0=[0 0 0]';
dv0=-[-g 0 0]';%Gravedad en -x
z0=[0 0 1]';

%Matriz de rotacion entre {s2} y TCP {s3}

R23=eye(3);

% Bucle de paso de tiempo

%--------------------------
for tk=1:1:n;
t=(tk-1)/10;

20
% Evaluación numérica de posición, velocidad y aceleración
% y trayectoria del TCP
%--------------------------------------------------------
d1=eval(sd1);
vd1=eval(svd1);
ad1=eval(sad1);
t2=eval(st2);
vt2=eval(svt2);
at2=eval(sat2);

q(tk,1)=d1;
qd(tk,1)=vd1;
qdd(tk,1)=ad1;
q(tk,2)=t2;
qd(tk,2)=vt2;
qdd(tk,2)=at2;
XY(tk,1)=L1 + d1 +L2*cos(t2);
XY(tk,2)=L2*sin(t2);

%Evaluacion numerica de las fuerzas y pares en el TCP

%---------------------------------------
n3=eval(sn3)';
f3=eval(sf3)';
n_3(tk,:)=n3;
f_3(tk,:)=f3;

% PASO 1-2 Evaluacion de los vectores p,s y de la matriz de inercias

%---------------------------------------------------------
p1=[0 0 d1]';
p2=[0 0 0]';
s1=[0 0 L1]';
s2=[0 0 0]';
I1=zeros(3);
I2=zeros(3);

%PASO 3
%----------------------------------------------
R01=[1 0 0;0 0 1; 0 -1 0];
R12=[sin(t2) cos(t2) 0;-cos(t2) sin(t2) 0; 0 0 1];

%PASO 4
%----------------------------------------------
w1=R01'*w0;%Traslacion
w2=R12'*(w1+z0*vt2);%Rotacion

%PASO 5
%----------------------------------------------
dw1=R01'*dw0;%Traslacion
dw2=R12'*(dw1+z0*at2)+cross(w1,z0*vt2);%Rotacion

%PASO 6
%----------------------------------------------
dv1=R01'*(z0*ad1+dv0)+cross(w1,p1)+cross(2*w1,R12*z0*vd1)+cross(w1,cross(
w1,p1));%Traslacion
dv2=cross(dw2,p2)+cross(w2,cross(w2,p2))+R12'*dv1;%Rotacion

21
%PASO 7
%----------------------------------------------
a1=cross(dw1,s1)+cross(w1,cross(w1,s1))+dv1;
a2=cross(dw2,s2)+cross(w2,cross(w2,s2))+dv2;

%PASO 8
%----------------------------------------------
f2=R23*f3+m2*a2;
f1=R12*f2+m1*a1;

%PASO 9
%----------------------------------------------
n2=R23*(n3+cross((R23'*p2),f3))+cross((p2+s2),m2*a2)+I2*dw2+cross(w2,
(I2*w2));
n1=R12*(n2+cross((R12'*p1),f2))+cross((p1+s1),m1*a1)+I1*dw1+cross(w1,
(I1*w1));

%PASO 10
%----------------------------------------------
P1=f1'*R01'*z0;
P2=n2'*R12'*z0;
PARES(tk,1)=P1;
PARES(tk,2)=P2;

%Fin del algoritmo

%------------------------------------------------

%Verificacion del resultado

F1=(m1+m2)*ad1;
T2=0;
PARES2(tk,1)=F1;
PARES2(tk,2)=T2;

end % Fin del bucle de tiempo

figure(1);clf
subplot(2,2,1),hold;
grid,plot(q(:,1)),title('Q1'),legend(strcat('Q1:',sd1),0),hold;
subplot(2,2,2),hold;
grid,title('Fuerza Q1 Alg,NE (-), SOl. analitica (x)')
plot(PARES(:,1)),plot(PARES2(:,1),'rx'),hold;
subplot(2,2,3),hold;
grid,plot(q(:,2)),title('Q2'),legend(strcat('Q2:',st2),0),hold;
subplot(2,2,4),hold;
grid,title('PAR Q2 Alg,NE (-), SOl. analitica (x)')
plot(PARES(:,2)),plot(PARES2(:,2),'rx'),hold;

[2.2] Método de Lagrange

clc,clear all
% Algoritmo de Lagrange
% Para la solución del modelo dinámico inverso de un robot
%----------------------------------------------------------
% Robot cilíndrico 2 GDL (DR) (Desplazamiento y Rotacion)
% Problema 2
% Definición de la trayectoria
%------------------------------
% Definición simbólica de la posición

22
sd1='sin(3*t)+t+2';
st2='cos(t)*t+pi/6';

% Obtención simbólica de la velocidad y aceleración

svd1=diff(sd1,'t');
svt2=diff(st2,'t');
sad1=diff(svd1,'t');
sat2=diff(svt2,'t');
% Parámetros dimensionales
L1=10;
L2=5;
m1=20;
m2=10;
g=9.81;
G=[g 0 0 0];
r11=[0 0 -L1 1]';
r22=[0 0 0 1]';
%Matrices de Derivación
Qr=zeros(4);%Matriz de rotacion
Qr(1,2)=-1;
Qr(2,1)=1;
Qd=zeros(4);%Matriz de traslacion
Qd(3,4)=1;
% Bucle de paso de tiempo
%--------------------------
for tk=1:1:50;
t=(tk-1)/10;
% Evaluación numérica de posición, velocidad y aceleración
%--------------------------------------------------------
d1=eval(sd1);Q1(tk)=d1;
t2=eval(st2);Q2(tk)=t2;
vd1=eval(svd1);
vt2=eval(svt2);
ad1=eval(sad1);
at2=eval(sat2);
% PASO 1-2 Obtención de las matrices de transformación Aij
%---------------------------------------------------------
% Matrices A00 y A11 son la identidad
A00=eye(4);
A11=eye(4);
%Evalua las matrices A01 y A02
A01=[1 0 0 0;0 0 1 0; 0 -1 0 d1;0 0 0 1];
A12=[sin(t2) cos(t2) 0 0;-cos(t2) sin(t2) 0 0; 0 0 1 0;0 0 0 1];

%Evalua la matriz A02=A01*A2

A02=A01*A12;
% PASO 3 Evaluacion matrices Uij
%--------------------------------
U11=A00*Qd*A01;
U12=zeros(4);
U21=A00*Qd*A02;
U22=A01*Qr*A12;
%PASO 4 Evaluación de las matrices Uijk
%--------------------------------------
U111=A00*Qd*A00*Qd*A01;
U112=zeros(4);
U121=zeros(4);

23
U122=zeros(4);
U211=A00*Qd*A00*Qd*A02;
U212=A00*Qd*A01*Qr*A12;
U221=A00*Qr*A01*Qd*A12;
U222=A01*Qr*A11*Qr*A12;

% PASO 5 Evaluación matrices 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 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*vd1*vd1 + h112*vd1*vt2 + h121*vt2*vd1 + h122*vt2*vt2;
H(2,1)=h211*vd1*vd1 + h212*vd1*vt2 + h221*vt2*vd1 + h222*vt2*vt2;
% PASO 9 Evaluación 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 Evaluación de los pares
%----------------------------------
pares(:,tk)=D*[ad1 at2]'+H+C;
%fin del algoritmo
%---------------
%Verificacion del resultado
F1=(m1+m2)*ad1;
T2=0;
pares2(1,tk)=F1;
pares2(2,tk)=T2;
end
% fin del bucle de paso de tiempo
% Presentación Gráfica de los resultados
figure(2);clf
subplot(2,2,1),plot(Q1),grid,legend(strcat('Q1:',sd1),0)
subplot(2,2,2),hold on,
plot(pares(1,:)),plot(pares2(1,:),'rx')
grid,title('PAR Q1 Alg. Lagrange'),hold

24
subplot(2,2,3),plot(Q2),grid,legend(strcat('Q2:',st2),0)
subplot(2,2,4),hold
plot(pares(2,:)),plot(pares2(2,:),'rx')
grid,title(' PAR Q2 Alg.Lagrange'),hold

25