Facultad Ingeniería Electrónica y Mecatrónica

Mag.W. Castellanos
Robtica II
Laboratorio 4 : Algoritmo de Lagrange

Objetivo:
- Analizar la dinámica inversa del robot de 2 grados de libertad utilizando el algoritmo de
Lagrange.
- Analizar la respuesta para diferentes trayectorias.

Marco teórico.
El análisis dinámico de un robot se efectúa aplicando el algoritmo computacional de Lagrange
para una trayectoria deseada de posición, velocidad y aceleración. Esto es posible manteniendo
constante un grado de libertad y realizando movimientos alternativos en el otro.
La respuesta del par obtenido utilizando el algoritmo debe mostrar que al ser constante el grado
de libertad, no aparecen pares de Coriolis y el momento de inercia debe ser constante.

Algoritmo computacional para la obtención de modelo dinámico de un robot mediante la

formulación de Lagrange

L1. Asignar a cada eslabón un sistema de referencia de acuerdo al algoritmo DH

L2. Obtener las matrices de transformación 0Ai para cada elemento i
L3. Obtener las matrics Uij de finidas por:

 0Ai
U ij 
q j

L4. Obtener las matrices Uikj definidas por:

U ij
U ijk 
q k

L5. Obtener las matrices de pseudoinercia J para cada elemento que viene definida por:

 x1 2 dm
  x y dm
i i  x z dm
i i  x dm
i

Ji     y dm  y z dm  y dm
2
 yi xi dm 1 i i i

  z i y i dm  z y dm  z dm  z dm 
2
i i 1 i
 x dm
  i  y dm i  z dm
i  dm 

1
Donde las integrales están extendidas al elemento i considerado, y (xi,yi, zI)son las
coordenadas del diferencial de masa dm respecto al sistema de coordenadas del elemento.

L6. Obtener la matriz de inercias D= [d ij ] cuyos elementos vienen definidos por

d ij   Traza (U k j J k U T kj )

L7 Obtener los términos h ikm definidos por

hikm   Traza (U jkm J jU ji )

T

L8 Obtener la matriz de fuerzas de coriolis y centrípeta H= [h] T cuyos elementos vienen

definidos por :

hi    U ikm q k q m

L8
L9 Obtener la matriz columna de fuerza de gravedad C = [c] T cuyos elementos vienen
definidos por :
ci  (  m j gU ji ri )

g: es el vector de gravedad
r:es el vector de coordenadas homogéneas

L10 La ecuación dinámica del sistema será

Proceedimiento.
1. Aplicar el algoritmo computacional de Lagrange para la obtención del modelo dinámico
del robot de 2 grados de libertad (q1, d2) con base fija mostrado en la figura 1.

Figura 1
L1. Se asignan los sistemas de referencia y se hallan los parámetros Denavit-Hartemberg.

2
Articulación i di ai i
1 1 0 0 -90
2 0 d2 0 0

%Algoritmo de Lagrange
%Solución del modelo dinámico inverso Robot polar de 2GDL
%Definición de la trayectoria
%Definición simbólica de la trayectoria
sq1='sin(2*t)+pi/2'
sq2='1'
%obtención simbólica de la velocidad y aceleración
sqd1='2*cos(2*t)';
sqdd1='-4*sin(2*t)';
sqd2='0';
sqdd2='0';
%Parámetros 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 derivación
Qr=zeros(4); Qr(1,2)= -1; Qr(2,1)= 1;
Qd=zeros(4); Qd(3,4)= 1
%Bucle de tiempo
for tk= 1:1:50
t= (tk-1)/10
%Evaluación de posición, velocidad y aceleración.
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 Evaluación de matrices de transformación Aij
A00=eye(4)
A11=eye(4)
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]
A02= A01*A12
% PASO 3 Evaluación de matrices Uij
U11=A00*Qr*A01
U12=zeros(4)
U21=A00*Qr*A02
U22=A01*Qd*A12
% PASO 4 Evaluación de 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

3
% PASO 5 Evaluación de matrices de pseudoinercia J1
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 inercia 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*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 Evaluación de la matriz de Gravedad
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*[qdd1 qdd2]'+ H+ C
end

2. Aplicar el algoritmo para diferentes trayectorias manteniendo uno de los grados de

libertad constante y luego ambos variables.
3. Trayectorias deseadas:
q1= pi/2 + sin 2t
q2= 1

q1= 0
q2= sin 2t

q1= pi/2 + t sin 2t

q2= t + 1+ sin 5t

4. Graficar los resultados para las diferentes trayectorias.

Plot (Q1), Plot (Q2),
plot(PARES(1,:))

5. Analizar los resultados