UNIVERSIDAD CATOLICA DE SANTA MARIA

FACULTAD DE INGENIERIAS FISICAS Y FORMALES

PROGRAMA PROFESIONAL DE INGENIERIA
MECANICA, MECANICA ELECTRICA Y
MECATRONICA

“LABORATORIO DE ROBOTICA I”
NOMBRES Y APELLIDOS:
Alviz Aragon, Emmanuel
Diaz Reinoso, Pamela Stefanny
Ponce Orellana, David Jonathan
Semestre IX
2015
 Tema: MODELO DINÁMICO – Algoritmos Computacionales

I. OBJETIVO

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

Lagrange.
 Obtener el modelo dinámico de un robot aplicando los algoritmos computacionales de
Lagrange y Newton.
 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

Introducción
La dinámica del robot relaciona el movimiento del robot y las fuerzas implicadas en el mismo. El
modelo dinámico establece relaciones matemáticas entre las coordenadas articulares (o las
coordenadas del extremo del robot), sus derivadas (velocidad y aceleración), las fuerzas y pares
aplicados en las articulaciones (o en el extremo) y los parámetros del robot (masas de los
eslabones, inercias, etc).

Siguiendo con la filosofía de este libro, se recomienda al lector que quiera profundizar sobre la
dinámica de robots, la lectura de los textos donde se estudian varias formulaciones clásicas como
Lagrange-Euler o las ecuaciones generalizadas de D’Alembert. Hay que tener en cuenta que las
ecuaciones de movimiento obtenidas con estas formulaciones son equivalentes en el sentido que
describen la conducta dinámica del robot, sin embargo, cada una de ellas presenta características
diferentes que la hacen más apropiada para ciertas tareas. Por ejemplo, la formulación de
Lagrange-Euler presenta un modelo simple y elegante, dando como resultado una serie de
ecuaciones diferenciales no lineales de 2º orden acopladas útiles para el estudio de estrategias de
control en el espacio de estados de las variables articulares del robot, pero que se presentan
ineficaces para aplicaciones en tiempo real dado el elevado tiempo de computación que requieren
las operaciones con matrices de transformación homogénea.

Los modelos dinámicos que se estudian en esta práctica están basados en el algoritmo recursivo
de Newton-Euler (N-E) desarrollado por Luh . Aunque las formulaciones recursivas destruyen la
estructura del modelo dinámico analítico y dan lugar a la falta de ecuaciones cerradas necesarias
para el análisis del control, la dificultad de un análisis clásico es enorme debido a que se obtienen
expresiones fuertemente no-lineales que constan de cargas inerciales, fuerzas de acoplo entre las
articulaciones y efectos de las cargas de gravedad, con la dificultad añadida de que los
pares/fuerzas dinámicos dependen de los parámetros físicos del manipulador, de la configuración
instantánea de las articulaciones, de la velocidad, de la aceleración y de la carga que soporta el
robot. Aunque las ecuaciones del movimiento son equivalentes ya sean analíticas o recursivas, los
diferentes planteamientos dependen de los objetivos que se quieran conseguir con ellos. En
algunos casos es necesario solucionar el problema dinámico de un robot para lograr tiempos de

1
calculo rápidos en la evaluación de los pares y fuerzas articulares para controlar el manipulador, y
en otros casos son necesarios planteamientos para facilitar el análisis y la síntesis del control.
La formulación de Newton-Euler

El método de Newton-Euler permite obtener un conjunto de ecuaciones recursivas hacia delante

de velocidad y aceleración lineal y angular las cuales están referidas a cada sistema de referencia
articular. Las velocidades y aceleraciones de cada elemento se propagan hacia adelante desde el
sistema de referencia de la base hasta el efector final. Las ecuaciones recursivas hacia atrás
calculan los pares y fuerzas necesarios para cada articulación desde la mano (incluyendo en ella
efectos de fuerzas externas), hasta el sistema de referencia de la base

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.

Elemento de Masa 1

1 2 1 2 2
Ec = ∗I∗θ̇ = ∗M 1∗L1 ∗θ̇1
2 2
E p =¿ 0

2
 L=E c −E p

1
L= ∗M 1∗L1 ∗θ˙1 −0
2 2
2
∂L
=0
∂θ
∂L 2
=M 1 ¿ L1 ∗θ̇1
∂ θ̇
∂ ∂L 2
=M 1 ¿ L1 ∗θ̈1
∂ t ∂ θ̇
∂ ∂L ∂L
τ= −
∂t ∂ θ̇ ∂ θ
2
τ =M 1 ¿ L1 ∗θ̈1 −0
2
τ =M 1 ¿ L1 ∗θ̈1

Elemento de Masa 2

1 2 1
Ec = ∗I∗θ̇ = ∗M 2∗d˙2
2
2 2
E p =¿ 0

L=E c −E p

1
L= ∗M 2∗d˙2 −0
2
2
∂L
=0
∂θ
∂L
=M 2∗ḋ 2
∂ ḋ
∂ ∂L
=M 2∗d¨2
∂ t ∂ ḋ
∂ ∂L ∂L
τ= −
∂t ∂ ḋ ∂ θ
τ =M 2∗d¨2−0

τ =M 2∗d¨2

Lagrange
2
τ =M 1 ¿ L1 ∗θ̈1 + M 2∗ d̈2

3
Algoritmo Computacional de Lagrange
clear all, close all, clc

syms t
sq1='sin(2*t)+ pi/2';
sq2='sin(5*t)+t+1';

% Obtencion simbolica de la velocidad y aceleracion

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 de posicion, velocidad y aceleracion
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 matrices de transformacion Aij
% matrices A00 y A11 son la identidad

A00=eye(4);
A11=eye(4);
% Evalua la matriz 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*Qr*A11*Qd*A12;

4
% PASO 5 Evaluacion Matrices Pseudoinversa 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 la Matrices 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');

%PASOS 7-8 EVALUACION DE LA MATRIZ DE CORIOLIS R

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 Evalucion 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 Evalucion de los pares

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

%FIN DEL ALGORITMO

%VERIFICACION DEL RESULTADO

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

%Presentacion Grafica de los resultados

figure(1);clf

subplot(2,2,1),plot(Q1),grid,legend(strcat('Q1:',sq1),0)
subplot(2,2,2),hold,
plot(PARES(1,:)),plot(PARES2(1,:),'rx')
grid,title('PAR Q1 Alg.Lagrange'),hold

5
subplot(2,2,3),plot(Q2),grid,legend(strcat('Q2:',sq2),0)
subplot(2,2,4),hold,
plot(PARES(2,:)),plot(PARES2(2,:),'rx')
grid,title('PAR Q2 Alg.Lagrange'),hold

Probando trayectos aleatorios

6
IV.2. 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.

Elemento de Masa 1

x 1=d 1 ¿C 1

x˙1=−d 1∗S 1∗q˙1

y 1=d1 ¿ S 1

7
 ẏ 1=d1∗C1∗q̇1
2 2 2
v1 = x˙1 + ẏ 1

v1 =(−d 1 ¿ S 1 ¿ ˙q 1) +( d 1∗C1∗q̇1 )
2 2 2

2
˙1
2
v1 =d 1 ¿ q
2

Elemento de Masa 2

x 2=l 1 ¿ C 1+ d 2 ¿ C 12

x˙2=−( l 1 ¿ S 1+ d 2 ¿ S 12 ) ¿ ˙q1−d 2∗S 12∗q̇ 2

y 2=l 1 ¿ S 1+ d 2 ¿ S 12
˙ 1 +d 2∗C 12∗q˙2
ẏ 2=( l 1 ¿ C1 +d 2 ¿ C12 ) ¿ q
2 2 2
v 2 = ẋ1 + ẏ 1

v 22=( l 12+ d 22 +2∗l 1∗d 2 ¿C 2 ) ¿ ˙q12 +2∗d 2 (l 1∗C ¿ ¿ 2+ d2 ) ¿ ˙q1 ¿ ˙q2 ¿

Energia cinetica

1 2 2
EC = ∗(m 1 v 1 +m2 v 2 )
2
1 2 1 2 1
EC = ∗( m1 d 1 +m2∗( l1 +d 2 +2∗d 2 ¿ l 1∗C 2) ) ¿ q˙ 1 + ∗( m 2 d 2 ) ¿ ˙q2 + ∗( m2 ¿ 2∗d 2 ∗( l 1 ¿ C 2+ d 2 ) ) ¿ q˙ 1 ¿ q˙ 2
2 2 2 2 2
2 2 2
Energia Potencial

E p =g∗(m1∗h1 +m2∗h2)

E p =g∗(m1∗y 1 +m2∗y 2 )

E p =g∗(m1∗d 1∗S 1+ m2∗l1∗S 1∗m2∗d 2∗S 12 )

Lagrange

L=E c −E p

1 1 1
L= ∗( m1 d 12+ m2∗( l 12 +d 22+ 2∗d 2 ¿ l1∗C2 ) ) ¿ q˙ 12 + ∗( m2 d22 ) ¿ q˙ 22+ ∗( m2 ¿ 2∗d22∗( l 1 ¿ C 2+ d2 ) ) ¿ q˙ 1 ¿ q˙ 2−g∗(m1∗
2 2 2

8
%Reemplazando las matrices en el algoritmo de Lagrange
% Evalua la matriz A01 y A02
A01=[cos(q1) -sin(q1) 0 L1*cos(q1);sin(q1) cos(q1) 0 L1*sin(q1);0 0 1 0;
0 0 0 1];
A12=[cos(-q2) -sin(-q2) 0 L1*cos(-q2);sin(-q2) cos(-q2) 0 L1*sin(-q2);0
0 1 0; 0 0 0 1];
% Evalua la matriz A02=A011*A2
A02=A01*A12;

Probando trayectos aleatorios

9
Algoritmo Computacional de Euler
%DefRej5_6 Define los parametros del Robot del Ejercicio
clear L

%Parametros de DH
L(1)=link([-pi/2 0 0 0 0],'standard');
L(2)=link([0 0 0 0 1],'standard');

%Masas de los eslabones

L(1)*m=20;
L(2)*m=10;

%Coordenadas de los centros de masa de cada eslabon

L(1)*r=[0 0 10];
L(2)*r=[0 0 0];

%Tensiones de Inercia del motor que acciona cada elemento

L(1)*Jm=0;
L(2)*Jm=0;

%Coeficiente de reduccion del actuador del elemento

L(1)*G=1;
L(2)*G=1;

%Rozamiento viscoso del actuador de cada elemento

L(1)*B=0;
L(2)*B=0;

10
%Rozamiento de Coulomb en la direccion positiva
%y negativa del actuador de cada elemento
L(1)*Tc=[0 0];
L(2)*Tc=[0 0];

%Definicion del robot

Rej5_6=robot(L,'Polar RD','UPM','Robot cilindrico de 2 GDL');
Rej5_6.name='Polar RD';
Rej5_6.manuf='UPM';

%Dibuja los pares para una trayectoria concreta

%utilizando la funcion rne

defRej5_6 %Se define el robot

grav=(-9.8,0,0); %Se define la gravedad con respecto al sistema
n=50;

%Genera la trayectoria y la dibuja

%Definicion Simbolica de la posicion
sq1='t*sin(t)+pi/2';
sq2='sin(5*t)+t+1';

%Obtencion simbolica de la velocidad y aceleracion

sqd1=diff(sq1,'t'); sqdd1=diff(sqd1,'t');
sqd2=diff(sq2,'t'); sqdd2=diff(sqd2,'t');

%Evaluacion numerica de posicion, velocidad, aceleracion y XY

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

q(tk,1)=eval(sq1);qd(tk,1)=eval(sqd1);qdd(tk,1)=eval(sqdd1);
q(tk,2)=eval(sq2);qd(tk,2)=eval(sqd2);qdd(tk,2)=eval(sqdd2);

XY(tk,1)=q(tk,2)*cos(q(tk,1));XY(tk,2)=q(tk,2)*sin(q(tk,1));
end

%Dibuja la trayectoria
figure(1)
subplot(2,2,1),plot(q),title('Posicion'),grid
legend(strcat('Q1:',sq1),strcat('Q2:',sq2),0)
legend boxoff
subplot(2,2,3),plot(qd),title('Velocidad'),grid
subplot(2,2,4),plot(qdd),title('Aceleracion'),grid
subplot(2,2,2),plot(XY(:,1),XY(:,2)),title('XY'),grid

%VERIFICACION

%Este codigo es unicamente a efects de verificar

%que la programacion del algoritmo es correcta
%Representa la solucion analitica obtenida manualmente

%Parametros dimensionales
L1=10;m1=20;m2=10;g=9.8;

11
q1=q(:,1);qd1=qd(:,1);qdd1=qdd(:,1);
q2=q(:,2);qd2=qd(:,2);qdd2=qdd(:,2);

T1=(m1*L1^2+m2*q2.*q2).*qdd1+2*q2*m2.*qd1.*qd2+m1*g*L1*cos(q1)+m2*g*q2.*c
os(q1);
F2=m2*qdd2-m2*q2.*qd1.*qd1+m2*g*sin(q1);

%Obtiene los pares y las aportaciones al par por

%inercia,coriolis y gravedad y las dibuja

figure(2)
Tau=rne(Rej5_6,q,qd,qdd,grav);
TauG=GRAVLOAD(Rej5_6,q,grav);
TauC=CORIOLIS(Rej5_6,q,qd);
TauI=ITORQUE(Rej5_6,q,qdd);

subplot(2,4,1),hold
plot(Tau(:,1)),plot(T1,'rx'),title('Par Total Q1 rne'),grid
legend(strcat('Q1:',sq1),'Location','SouthOutside')
legend boxoff, hold

subplot(2,4,2), plot(TauI(:,1)),title('Par Inercia Q1 rne'),grid

subplot(2,4,3), plot(TauC(:,1)),title('Par Coriolis Q1 rne'),grid
subplot(2,4,4), plot(TauG(:,1)),title('Par Gravedad Q1 rne'),grid

subplot(2,4,5),hold
plot(Tau(:,2)),plot(F2,'rx'),title('Par Total Q2 rne'),grid
legend(strcat('Q2:',sq2),'Location','SouthOutside')
legend boxoff, hold

subplot(2,4,6), plot(TauI(:,2)),title('Par Inercia Q2 rne'),grid

subplot(2,4,7), plot(TauC(:,2)),title('Par Coriolis Q2 rne'),grid
subplot(2,4,8), plot(TauG(:,2)),title('Par Gravedad Q2 rne'),grid

CONCLUSIONES y OBSERVACIONES

 Existen complicaciones al realizar el modelo dinamico debido a que a mayor grados de

Libertad más complejidad, existencia de fuerzas de Coriolis, son modelos no lineales, se
deben aplicar procesos iterativos, se considera la dinámica de los actuadores.
 La ecuación final de Lagrange y Newton Euler corresponde a la ecuación del sistema
 Es de gran utilidad ya que obteniendo la dinámica del robot podemos obtener el
dimensionamiento de los actuadores.
 Al hacer la dinámica del robot relacionamos las fuerzas que actúan sobre el cuerpo y el
movimiento que en él se originan.
 Existen dos algoritmos computacionales para la realización del modelo dinamico: Newton
Euler y Lagrange.

12