AnuaL Bol2 2010CV 1

UNIVERSIDAD NACIONAL DEL CALLAO
FACULTAD DE INGENIERIA ELECTRICA Y ELECTRONICA
L DE ROBTICA
TEMA:
BRAZO ROBTICO 2 GDL
PROFESOR:
MSc. Ing. Ricardo Rodrguez Bustinza
ALUMNOS:
CONDOR CORDOVA JAVIER ALAN 072059B
DURAN SALAS JOHAN AUGUSTO 072654H
ARANA URIARTE CHRISTIAN LUIS 072673B
CAMPOS TARAZONA OSCAR 080614A

BRAZO ROBTICO 2 GDL

INTRODUCCIN:
Las mquinas conocidas como robots son importantes en la vida del hombre y tienen la
finalidad de ayudar en diferentes actividades como trabajos repetitivos, manejo de
materiales peligrosos y actividades que superan las capacidades naturales del ser
humano. A fin de dar una solucin a estas actividades el ser humano disea y construye
dispositivos para estas tareas, adems busca mejorar su desempeo con la aplicacin de
tecnologa de vanguardia (visin artificial y respuesta inmediata). Se estudia la
morfologa y caractersticas para entender cules son las diferentes partes de que consta
un robot as como las consideraciones que se deben de tomar en cuenta en el modelo
mecnico considerando la cinemtica de todas las piezas mviles que lo componen, el
modelo dinmico del robot es quiz la parte ms compleja de este modelo, como el
lector se dar cuenta cuando se estudie con ms profundad este tema, el modelo
matemtico y la cantidad de recurso informtico que se requiere para resolver este
problema guiara el mtodo ms apropiado para su implementacin. Hasta este punto la
maquina no interacta con el medio y solo dndole caractersticas humanas como lo son
la visin y audicin los robots siguen siendo meras maquinas destinadas a ejecutar
tareas repetitivas en aplicaciones como las celdas de manufactura industrial.
El objetivo final de la robtica es el de construir sistemas inteligentes capaces de

mostrar comportamientos racionales y complejos en la ejecucin de tareas especficas,
caracterizadas por la interaccin fsica con un mundo real y dinmico a travs de un
cuerpo fsico. Un robot autnomo es una mquina capaz de operar en un ambiente
parcialmente desconocido e impredecible. En contraste a este tipo de robots estn los
usados en la industria donde el ambiente es muy controlado y predecible. Hoy en da el
campo de la robtica mvil est recibiendo una gran atencin, sta tiene un amplio
rango de posibilidades en la industria. El inters en los robots no est dirigido
completamente a las aplicaciones industriales. Muchos bilogos, psiclogos y
etimlogos estn interesados en el uso de robots mviles para validar estructuras de
control observadas en el mundo biolgico.
Los robots son entidades mecnicas fsicas o virtuales que por apariencia y movimientos
parecen tener vida, pues suelen parecerse a entidades biolgicas y realizan actividades
que parecen guiadas por algn tipo de inteligencia. Si bien muchos mecanismos son
llamados robots, La Robtica identifica como robot a un mecanismo dotado de
movimiento que es capaz de interactuar dinmicamente y en tiempo real con su entorno
para desarrollar una variedad de tareas que le son encargadas mediante enseanza o
programacin. Es el objetivo del presente trabajo el diseo de un robot de 2 grados de
libertad segn los lineamientos de La Robtica.
ROBTICA Pgina 1
CINEMTICA DIRECTA:
El brazo robtico de 2GDL que modelaremos estar formado por 2 eslabones rectos y
2articulaciones de tipo revoluta. Comenzando desde el eslabn unido al origen y
continuando la cadena cinemtica la longitud de los eslabones ser: 3 y 1,
respectivamente.
El moldeamiento matemtico de la posicin y orientacin de las articulaciones se

realizan en base a las recomendaciones de Denavit-Hartenberg.
Calculo de los parmetros Denavit-Hartenberg
Theta: Sera el ngulo que tenemos que girar respecto al eje Z(i-1)para alinear el eje
X(i-1) con el eje X(i).
d: Es la distancia desde el origen (i-1) hasta el punto de interseccin del eje X(i) con
Z(i-1)
a: Distancia entre la perpendicular comn a Z(i-1) a Z(i)
Alpha: Angulo de Z(i-1) a Z(i) medido alrededor de X(i)
De donde se determina la siguiente tabla:
Parmetros Denavit-Hartenberg
Articulacin Theta D A Alpha

1 Q1 0 L1 0
2 Q2 0 L2 0
Donde:
L1=3; L2=1;
ROBTICA Pgina 2
Ahora utilizando la matriz de transformacin homognea:
i
A i1 = T z , theta(i) * T z , d (i) * T x ,a (i )T x ,alpha (i) =
[cos(theta) -cos(alpha)*sin(theta) sin(alpha)*sin(theta) a*cos(theta)
sin(theta) cos(alpha)*cos(theta) -sin(alpha)*cos(theta) a*sin(theta)
0 sin(alpha) cos(alpha) d
0 0 0 1 ];
Utilizando ahora lo valores de tabla D-H hallamos A 10 , A 21 y A32 .
Finalmente multiplicando estas tres matrices tenemos:
1 2 3
T= A 0 A 1 A 2 =
[ cos(q1)*cos(q2) - sin(q1)*sin(q2), - cos(q1)*sin(q2) - cos(q2)*sin(q1), 0, l1*cos(q1)

+ l2*cos(q1)*cos(q2) - l2*sin(q1)*sin(q2)]
[ cos(q1)*sin(q2) + cos(q2)*sin(q1), cos(q1)*cos(q2) - sin(q1)*sin(q2), 0, l1*sin(q1) +

l2*cos(q1)*sin(q2) + l2*cos(q2)*sin(q1)]
[ 0, 0, 1, 0]
[ 0, 0, 0, 1]
Donde la posicin del efector final ser:
[x,y,z]=[ l1*cos(q1) + l2*cos(q1+q2) , l1*sin(q1) + l2*sin(q1+q2), 0]
ROBTICA Pgina 3
Cdigo del programa para la cinemtica directa:

clear all; close all; clc
%---------------------------------------------------------------------
% INTEGRANTES:
% CONDOR CORDOVA JAVIER ALAN 072059B
% ARANA URIARTE CHRISTIAN LUIS 072673B
% DURAND SALAS JOHANN AUGUSTO 072654H
% CAMPOS TARAZONA OSCAR 080614A
%---------------------------------------------------------------------
%LOS DATOS QUE SE ENCUENTRAN EN LA HOJA SON LOS SIGUIENTES:
%LONGITUD DE LOS ESLABONES => l1=3; l2=1;
%LOS ANGULOS DE GIRO SON => theta1=pi/3; theta2=pi/3;
%SEGUN EL GRAFICO DE LA HOJA SE PUEDEN OBTENER LOS PARAMETROS DE
%DENAVIT-HARTENBERG
%PARAMETROS PARA EL DENAVITH
%Longitud de los eslabones
l1=3; l2=1;
q1=pi/3; q2=pi/3;
%Coordenadas
q=[q1 q2];
d=[0 0];
a=[l1 l2];
alpha=[0 0];
%Parametros Denavit-Hartenberg
disp('PRIMERA MATRIZ DENAVIT-HARTENBERG --> A01:')
disp('-------------------------------------------')
A01=denavit_h(q(1),d(1),a(1),alpha(1))
disp('-------------------------------------------')
disp('SEGUNDA MATRIZ DENAVIT-HARTENBERG --> A12:')
disp('-------------------------------------------')
A12=denavit_h(q(2),d(2),a(2),alpha(2))
disp('-------------------------------------------')
disp('MATRIZ DE TRANSFORMACION HOMOGENEA --> MTH:')
disp('-------------------------------------------')
MTH=A01*A12
disp('-------------------------------------------')
%LA GRAFICA Y LA SIMULACION DEL PROBLEMA
L(1)=Link([0 d(1) a(1) alpha(1) 0]); %DH de longitud del

brazo 1
L(2)=Link([0 d(2) a(2) alpha(2) 0]); %DH de longitud del
brazo 2
twoLink=SerialLink(L,'name','ROBOT RR');
twoLink.n; % n=2 DOF
twoLink.config; % RR configuracion
ROBTICA Pgina 4
%LA GRAFICA DEL PROBLEMA
figure(1)
q1=pi/3;
q2=pi/3;
twoLink.plot([q1 q2]); hold on
disp('MATRIZ DE TRANSFORMACION HOMOGENEA --> N:')
disp('-------------------------------------------')
N=twoLink.fkine([q1 q2])
disp('-------------------------------------------')
plot3(N(1,4),N(2,4),N(3,4),'ro')
% SIMULACION DEL ROBOT RR

figure(2)
% HACIENDO LA PRIMERA REVOLUCION
for q1=0:pi/30:pi/3;
q2=0;
T=twoLink.fkine([q1 q2]);
view(0,90)
plot3(T(1,4),T(2,4),T(3,4),'ro')
end
%HACIENDO LA SEGUNDA REVOLUCION
for q2=0:pi/30:pi/3;
T=twoLink.fkine([q1 q2]);
view(0,90)
plot3(T(1,4),T(2,4),T(3,4),'g*')
end
disp('---------------------------------------------------')
disp(' MATRIZ DE TRANSFORMACION HOMOGENEA')
disp('PARA CONDICIONES FINALES DESPUES DE LOS MOVIMIENTOS')
disp('---------------------------------------------------')
disp('**T_FINAL:')
disp('-----------------------------------------')
disp(T)
disp('-----------------------------------------')
Las matrices del Denavit-Hartenberg
PRIMERA MATRIZ DENAVIT-HARTENBERG --> A01:
-------------------------------------------
A01 =
0.5000 -0.8660 0 1.5000
0.8660 0.5000 0 2.5981
0 0 1.0000 0
0 0 0 1.0000
-------------------------------------------
ROBTICA Pgina 5
SEGUNDA MATRIZ DENAVIT-HARTENBERG --> A12:
-------------------------------------------
A12 =
0.5000 -0.8660 0 0.5000
0.8660 0.5000 0 0.8660
0 0 1.0000 0
0 0 0 1.0000
-------------------------------------------
MATRIZ DE TRANSFORMACION HOMOGENEA --> MTH:
-------------------------------------------
MTH =
-0.5000 -0.8660 0 1.0000
0.8660 -0.5000 0 3.4641
0 0 1.0000 0
0 0 0 1.0000
-------------------------------------------
MATRIZ DE TRANSFORMACION HOMOGENEA --> N:
-------------------------------------------
N=
-0.5000 -0.8660 0 1.0000
0.8660 -0.5000 0 3.4641
0 0 1.0000 0
0 0 0 1.0000
-------------------------------------------
ROBTICA Pgina 6
---------------------------------------------------
MATRIZ DE TRANSFORMACION HOMOGENEA
PARA CONDICIONES FINALES DESPUES DE LOS MOVIMIENTOS
---------------------------------------------------
**T_FINAL:
-----------------------------------------
-0.5000 -0.8660 0 1.0000
0.8660 -0.5000 0 3.4641
0 0 1.0000 0
0 0 0 1.0000
-----------------------------------------
La grafica de la posicin final del robot:
ROBTICA Pgina 7
CINEMTICA INVERSA:
Por simple inspeccin:
ROBTICA Pgina 8
Despejando:
Definimos r^2=Rcua=x^2+y^2;
Y para nuestro diseo:
a1=3 y a2=1
Reemplazando tenemos
q(2)=acos((Rcua-10)/6);
De forma similar obtenemos para q(1):
q(1)=atan2(y,x)-atan2(sin(q(2)),3+cos(q(2)));
Cdigo del programa para la cinemtica inversa:

function q=cinversa_RR(T)
%Posicion de la mano del robot
p=T(1:3,4); %T es la matriz DH
%Condiciones iniciales
q1=0; q2=0;
%parametros
d1=0; d2=0;
a1=3; a2=1;
%Parametros DH
teta=[q1 q2];
d= [d1 d2];
a= [a1 a2];
alfa=[0 0];
%cinematica inversa
%ecuaciones
%Solucion articulacion 1 y 2
R=sqrt(p(1)^2+p(2)^2);
cosq2=(R^2-(a(1)^2+a(2)^2))/(2*a(1)*a(2));
sinq2=sqrt(1-cosq2^2);
q2=atan2(sqrt(1-cosq2^2),cosq2); % articulacion 2
beta=atan2(p(2),p(1));
phi=atan2((a(2)*sinq2),(a(1)+a(2)*cosq2));
q1=beta-phi; % articulacion 1
q=[q1 q2];
end
ROBTICA Pgina 9
%Incovacando cinversa
% qci=cinversa_2g(T);
qci=cinversa_RR(T);
qci1=qci*180/pi;
disp('LOS ANGULOS DESEADOS DE CINEMATICA INVERSA EN RADIANES')
disp(' ')
disp('------------------------------------------')
disp(qci)
disp('LOS ANGULOS DESEADOS DE CINEMATICA INVERSA')
disp(' ')
disp('------------------------------------------')
disp(qci1)
LOS ANGULOS DESEADOS DE CINEMATICA INVERSA EN RADIANES
------------------------------------------
1.0472 1.0472
------------------------------------------
LOS ANGULOS DESEADOS DE CINEMATICA INVERSA
------------------------------------------
60.0000 60.0000
------------------------------------------
DINMICA DEL ROBOT RR:
Los movimientos de un robot son predecibles por la estructura de su

concepcin de realizar movimientos repetitivos pero tambin existen
posibilidades de tener movimientos aleatorios que procedan de una
estructura racional provocado por estmulos externos con
independencia de accin que hacen que el robot adquiera habilidades
de tomar decisiones que dependen totalmente de su entorno y que
cada experiencia sirva para un aprendizaje guiado. En este captulo se
describen las diferentes partes que componen a un robot,
caractersticas que deben cumplir para obtener movilidad, destreza,
exactitud, repetitividad, siendo parmetros importantes para la
movilidad de un robot y que cumpla con la tarea que se le programe,
como se discutir existen diferentes coordenadas espaciales que le
darn al robot la ubicacin de su entorno y los movimientos que
tenga que ejecutar para realizar tareas. Los grados de libertar de las
ROBTICA Pgina 10
partes mviles del robot son consideraciones importantes que deben

tomarse en cuenta para disear la mecnica y el control de
movimientos por medio de motores que movern las articulaciones, a
medida que aumenta el grado de libertad el robot se vuelve ms
complejo como se discutir ampliamente en el desarrollo del captulo.
Una de las partes mviles mas importantes del robot son los efectores
finales los cuales le dan capacidad pare efectuar una serie de tareas y
movimientos emulando las manos de los seres humanos, existen
varias herramientas terminales para robots destinados para la
industria igual importancia tienen los manipuladores que se encargan
de agarrar y manipular objetos con dedos accionados por medio de
cilindros neumticos discutiendo cuales son los que tienen ms
ventaja dependiendo de la aplicacin que se le programe. Los
movimientos de los robots no son independientes siendo muy
rutinarios destinados a efectuar tareas que para los humanos serian
tediosas y peligrosas.
La dinmica se ocupa de la relacin entre las fuerzas que actan sobre un cuerpo y el
movimiento que en l se origina. Por lo tanto, el modelo dinmico de un robot tiene por
objeto conocer la relacin entre el movimiento del robot y las fuerzas implicadas en el
mismo. Esta relacin se obtiene mediante el denominado modelo dinmico, que
relaciona matemticamente:
1 La localizacin del robot definida por sus variables articulares o por las
coordenadas de localizacin de su extremo, y sus derivadas: velocidad y
aceleracin.
2 Las fuerzas pares aplicados en las articulaciones (o en el extremo del robot).
3 Los parmetros dimensinales del robot, como longitud, masa e inercias de sus
elementos.
La obtencin de este modelo para mecanismos de uno o dos grados de libertad no es

excesivamente compleja, pero a medida que el nmero de grados de libertad aumenta, el
planteamiento y obtencin del modelo se complica enormemente. Por este motivo no
siempre es posible obtener un modelo dinmico expresado de una forma cerrada, esto
es, mediante una serie de ecuaciones, normalmente del tipo diferencial de segundo
orden, cuya integracin permita conocer que el movimiento surge al aplicar unas
fuerzas o que fuerzas hay que aplicar para obtener un movimiento determinado. El
modelo dinmico debe ser resuelto entonces de manera iterativa mediante la utilizacin
de un procedimiento numrico.
ROBTICA Pgina 11
Modelo Dinmico Directo:
Expresa la evolucin temporal de las coordenadas articulares del robot en funcin de las
fuerzas y pares que intervienen.
Modelo Dinmico Inverso:
Expresa las fuerzas y pares que intervienen en funcin de la evolucin de las

coordenadas articulares y su derivadas.
Resolucin del Modelo Dinmico:
La obtencin del modelo analtico no es factible para robots de 3 a ms GDL, es por ello
que se debe recurrir a los algoritmos computacionales que obtiene la solucin de manera
numrica sin necesidad de obtener una expresin explicita del modelo. Estos algoritmos
en el caso de una trayectoria determinista (discretizada en pasos), van evaluando los
pares a desarrollar por cada articulacin paso a paso.
El modelo dinmico es muy importante en el captulo del diseo de un robot ya que se

pueden presentar estos dos casos:
Articulacin Simple Lineal: Articulacin Rotacional

Simple:
F= a*m = *J
ROBTICA Pgina 12
Dependiendo el caso es que debemos elegir nuestra dinmica y nuestro control ya que
en alguno de ellos es ms fcil un mtodo que el otro.
Dentro de estos algoritmos computacionales tenemos a los que destacaron:
Lagrange:
- Basado en la representacin D-H.

- Poca eficiencia computacional.
- Ecuaciones finales bien estructuradas.
Este algoritmo se basa en estos dos parmetros:
Energa cintica del enlace i Energa Potencial del enlace i
1 1
k i= mi v ci T v ci+ i Ti ci I ii i ui=moi gT o Pci +u ref
2 2
n n
Total k=
i=n
ki Total k= ui
i=n
Donde la ecuacin de movimiento tambin se puede expresar por:
En un robot manipulador la energa cintica se encuentra conformada por suma de la

energa cintica traslacional y la energa cintica rotacional.
1 1 &2
K i q, q& mi v 2 I i&
2 2
Enseguida, se obtienen las energas potenciales de los eslabones del robot manipulador.
U i q mi gh
Como tercer paso se obtiene el Lagrangiano del sistema. El cual se encuentra

determinado por la diferencia entre la energa cintica total1 y la energa potencial total2.
ROBTICA Pgina 13
L q, q& K q, q& U q
Enseguida, se desarrolla la siguiente ecuacin para cada una de las coordenadas

articulares.
d L L
&
dt i i
A continuacin se detalla el desarrollo del modelo dinmico del robot por medio de las
ecuaciones de Euler-Lagrange. Como primer paso se obtienen las energas cinticas de
cada uno de los eslabones. Como el primer eslabn, solamente rota alrededor del eje z,
nicamente cuenta con energa cintica rotacional.
1 2

K1 &, I1&
2
1
Para los siguientes eslabones las ecuaciones de sus energas cinticas son las siguientes:
1 1

K 2 &, m2 lc 2 2&2 2 lc 2 2 cos 2 2&
2
2
1
I 2 &
2
2
&2
1 2
1 1 2 && 1 &2 1 m l 2 cos 2 &2

K 3 &, m3lc 32&
2
2
2
2
m3lc 32&3 m3lc 3 2 3
2
2
2 m3l2 lc 3 cos 2 cos 2 3 1
m3l2 2&2
2
3 c3 2 3 1
1 & & & 1 I &2 & & 2

m3l2 2 cos 2 2 &
2

1 m3 l2 lc 3 cos 3 2 2 3
2
2
3 1 2
3

1 1 2 && 1 &2 1 m l 2 cos 2 &2

K 4 &, m f l32&
2
2
2
2
m f l32&3 m f l3 2 3
2
2
2 m f l2 l3 cos 2 cos 2 3 1
m f l2 2&2
2
f 3 2 3 1
1
m f l2 2 cos 2 2 &
2

1 m f l2 l3 cos 3 2 2 3
2 & & &
Siguiendo con el proceso de modelado, se deben obtener las expresiones de las energas
potenciales.
ROBTICA Pgina 14
U1 m1 gh
U 2 m2 g h lc 2 sen 2
U 3 m3 g h l2 sen 2 lc 3 sen 2 3
U 4 m f g h l2 sen 2 l3 sen 2 3
El Lagrangiano del sistema es el siguiente:
1 1 &2 1 I &2 &2 1 m l 2&2 1 m l 2&2 m l 2&& 1

L &, I1&
2
1
2
2
m2 lc 2 2& 2 lc 2 cos 21
2 2 2
2
2 1 2 2
3 c3 2
2
3 c3 3 3 c3 2 3
2
m3l2 2&
2
2
1 1
m3l2lc 3 cos 2 cos 2 3 & 1
2
2
m3lc 32 cos 2 2 3 &
1
2
2
m3l22 cos 2 2 & 2

1 m3l2 lc 3 cos 3 2 2 3
& & &
1 & & 2 1 m l 2&2 1 m l 2&2 m l 2&& &2 1 m l 2&2
I 3 &
2

1 2 3
2

2
f 3 2
2
f 3 3 f 3 2 3 m f l2 l3 cos 2 cos 2 3 1
2
f 2 2
1 1
m f l32 cos 2 2 3 &
2
1
2
2
m f l2 2 cos 2 2 &2

& & &
1 m f l2l3 cos 3 2 2 3 m1 gh m2 g h lc 2 sen 2
m3 g h l2 sen 2 lc 3 sen 2 3 m f g h l2 sen 2 l3 sen 2 3
Desarrollando las ecuaciones para cada una de las articulaciones del robot manipulador
se obtienen:
I1& 2 1 I 31 m2 lc 2 cos 21 2 m2 lc 2 cos 2 sen 21 2 2 m3 l2 lc 3 m f l2 l3 sen 2 cos 2 3 1 2

& I &
& & & 2 2 && 2 && &&
1
2 m3l2 lc 3 m f l2 l3 cos 2 sen 2 3 &

1 2 3 2 m3l2 lc 3 m f l2 l3 cos 2 cos 2 3 1
& & &&
2 m3lc 3 m f l3 sen 2 3 cos 2 3 &

2 2

1 2 3 m3 lc 3 m f l3 cos 2 3 1
& & 2 2 2 &&
2 m3 m f l2 sen 2 cos 2&&

2
1 2 m3 m f l2 cos 2 1 1
2 && 2
ROBTICA Pgina 15
m l m l m l m l m l && m l m l && 2 m l l m l l sen &&

2 c2
2
3 c3
2
3 2
2
f 3
2
f 2
2
2
3 c3
2
f 3
2
3 3 2 c3 f 2 3 3 2 3
m l l m l l sen & m l l m l l sen cos & m m l cos sen &

3 2 c3 f 2 3 3 3
2
3 2 c3 f 2 3 2 2 3 1
2
3 f 2
2
2 2 1
2
m l l m l l cos &
3 2 c3
& m l l m l l cos sen & m m gl cos
f 2 3 3 3 3 2 c3 f 2 3 2 2 3 1
2
3 f 2 2
m l cos sen & m l m l cos sen & 2 m l l m l l cos &

2 c2
2
2 2 1
2
3 c3
2
f 3
2
2
&
3 2 3 1
2
3 2 c3 f 2 3 3 2
I &
3
& m l m l g cos m gl cos I &
& &
2 3 3 c3 f 3
&
2 3 2 c2 2 2 2 2
& m l l sen &&

m3lc 32& 2 3 m f l3 3 m f l3 2 m f l2 l3 sen 3 2 3 m f l2 l3 cos 3 2
& m l 2& 2&& 2&& && &&
3 3 c3 2 3 2 c3 3
m3l2 lc 3 cos 2 sen 2 3 &

1 m3l 2 lc 3 cos 3 2 m3lc 3 cos 2 3 sen 2 3 1 m f gl3 cos 2 3
2 && 2 &2
m3l2 lc 3 sen 3 & & &

2 2 3 m f l2 l3 cos 2 sen 2 3 1 m f l3 cos 2 3 sen 2 3 1
&2 2 &2
m f l2 l3 sen 3 &
2 2 3 m3 glc 3 cos 2 3 I 3 2 3 3
& & && & &
El modelo formado por las ecuaciones anteriores puede descomponerse en la siguiente

forma:
M &
& C ,&& g
Newton-Euler:
- Basado en operaciones vectoriales.

- Mayor eficiencia computacional.
- Ecuaciones poco estructuradas.
Este algoritmo se basa en estos dos parmetros:
Ecuacin de newton Ecuacin de Euler
vc c
I
+I c
F=m* N=
Asignar a cada eslabn un sistema de referencia de acuerdo a las normas de D-H, luego
obtener las matrices de rotacin i-1Ri y sus inversas iRi-1 siendo:
i-1Ri =
Ci -Ci Si Si Si
Si Ci Ci -Si Ci
0 Si Ci
ROBTICA Pgina 16
Establecer las condiciones iniciales.

Para el sistema de la base S0:
00 : velocidad angular = (0,0,0)exp T
0d0 : aceleracin angular = (0,0,0)exp T
0v0 : velocidad lineal = (0,0,0)exp T
0dv0 : aceleracin lineal = (gx, gy, gz)exp T
00, 0d0 y 0v0 son tpicamente nulos salvo que la base del robot este en
movimiento.
Para el extremo del robot se conocer la fuerza y el par ejercidos externamente n+1
Fn+1 y n+1 N n+1.
Z0 = (0,0,1)exp T
iPi = coordenadas del origen del sistema Si respecto a Si-1.= ( ai, di, Si, di, Ci ).
iSi = coordenadas del centro de masas del eslabn i respecto del sistema Si.
iIi = matriz de inercia del eslabn i respecto de su centro de masas expresado en Si.
ii =
iRi-1 (i-1 i-1 + Z0 dq1) si el eslabn i es de rotacin

iRi (i-1 i-1) si el eslabn i es de traslacin.
Obtener la aceleracin angular del sistema si:
idi =
iRi-1 (i-1 di-1 + Z0 dq1) si el eslabn i es de rotacin
iRi (i-1 di-1) si el eslabn i es de traslacin.
Obtener la aceleracin lineal del sistema i:
idvi =idi (iPi) + ii (iPi) + iRi-1 (i-1 dvi-1)
Si el eslabn i es de rotacin.
(Z0 dqi + i-1 dvi-1) + idi (iPi) + 2i (iRi-1) Z0 (dqi) + ii (ii)(iPi)
Si l es de traslacin.
Obtener la aceleracin lineal del centro de gravedad del eslabn i:
iAi = idi (iSi) + ii (iSi) + idvi
ROBTICA Pgina 17
Para i = n...1 realizar los pasos 8 a 10.
Obtener la fuerza ejercida sobre el eslabn i:
iFi = iRi+1 (i+1 Fi+1) + mi ai
Obtener el par ejercido sobre el eslabn i:
iNi = iRi+1 (i+1ni + (i+1Ri)(iPi)(i+1 Fi+1)) + (iPi + iSi)(mi)(iai) + iIi (id i) + i i (iIi)
(ii).
Obtener la fuerza o par aplicado a la articulacin i.
i =
(iNi)exp T (iRi-1) Z0. Si el eslabn i es de rotacin.
(iFi)exp T (iRi-1) Z0. Si el eslabn i es de traslacin.
Donde es el par o fuerza efectivo (par motor menos pares de rozamiento o

perturbacin).
4.2 Modelo dinmico en variables de estado: la siguiente ecuacin

establece el modelo dinmico inverso de un robot, dado los pares y
fuerzas que deben proporcionar los actuadores para que las variables
articulares sigan una determinada trayectoria q(t):
= D dq + H + C
En esta expresin conviene recordar que la matriz de inercias D y la matriz columna de

gravedad C dependen de los valores de q, y que la matriz columna de fuerzas de
Coriolis y centrpetas H depende de q y dq. Asimismo, hay que tener presente que el
vector de pares generalizados t, presupone pares efectivos, por lo que de existir pares
perturbadores o de rozamiento (viscoso o seco) estos debern ser tenidos en cuenta,
siendo:
= (motor) - (perturbador) - (rozamiento viscoso) -

(rozamiento seco).
La expresin anterior es por tanto no lineal, no siendo trivial obtener a partir de ella el
modelo dinmico directo que proporciona la trayectoria seguida como consecuencia de
la aplicacin de unos pares determinados .
ROBTICA Pgina 18
Como vimos anteriormente, es posible modelar la dinmica del robot usando el mtodo
de Lagrange-Euler o el mtodo de Newton-Euler, en ambos caso nos lleva a obtener el
mismo resultado de ecuaciones dinmicas, donde el control dinmico del sistema se rige
a travs de este algoritmo:

=M ( ) +C ( , ) +G(
)
Con la ayuda del toolbox HEMERO, lo podemos desarrollar de este modo calculando
los parmetros que estn incluidos en la ecuacin general.
Donde el comando dyn del toolbox de Hemero, contiene los parmetros cinemticos y
dinmicos del manipulador. La matriz est compuesta de estos parmetros:
1. Alpha(i-1): Parmetros Denavit-Hartenberg

2. a(i-1)
3. theta(i)
4. d(i)
5. sigma (i): Tipo de articulacin, 0 rotacional y 1 prismtica.
6. masa: Masa del enlace i
7. rx: Centro de masa del enlace respecto al marco de referencia.
8. ry
9. rz
10. Ixx: Elementos del tensor de inercia ref. al centro de masa.
11. Iyy
12. Izz
13. Ixy
14. Iyz
15. Ixz
16. Jm: Inercia de la armadura
17. G: Velocidad de la articulacin, velocidad del enlace
18. B: Friccin viscosa referida al motor.
19. Tc+: Friccin de Coulomb, rotacin positiva.
20. Tc-: Friccin de Coulomb, rotacin negativa.
De este modo para un robot de n enlaces, la matriz dyn tendra nx20. Todos los
ngulos deben ser introducidos en radianes.
Cdigo del modelamiento dinmico:
ROBTICA Pgina 19
close all;clear all ;clc
syms q1 q2 qd1 qd2 qdd1 qdd2 l1 l2 m1 m2 g l3 d2 real
dyn=[ -pi/2 l1 q1 0 0 m1 l1 0 0 0 0 0 0 0 0 0 1 0
0 0
0 l2 q2 0 0 m2 l2 0 0 0 0 0 0 0 0 0 1 0 0
0];
q= [q1 q2];
qd = [qd1 qd2];
qdd=[qdd1 qdd2];
grav = [0 0 -g];
disp('Par total sobre las articulaciones');
tau = rne(dyn, q, qd, qdd, grav);
tau = simplify(tau);
disp('Matriz de masas del manipulador');
M = inertia(dyn, q);
M = simplify(M)
disp('Trminos centrfugos y de coriolis')

C = coriolis(dyn, q, qd);
C = simplify(C)
disp('Trmino gravitatorio')
G = gravity(dyn, q, grav);
G = simplify(G)
La solucin:
Par total sobre las articulaciones
Matriz de masas del manipulador
M=
[ m1*l1^2 + 4*m2*(cos(q2)/2 + 1/2)*l2^2, l2^2*m2*(cos(q2) + 1)]
[ l2^2*m2*(cos(q2) + 1), l2^2*m2]
Trminos centrfugos y de coriolis
C=
[ -l2^2*m2*qd2*sin(q2)*(2*qd1 + qd2), l2^2*m2*qd1^2*sin(q2)]
Trmino gravitatorio
G=
[ -g*(l1*m1*cos(q1) + l2*m2*cos(q1) + l2*m2*cos(q1 + q2)), -g*l2*m2*cos(q1 + q2)]
Cdigo del modelamiento usando Lagrange-Euler:
clear all;close all; clc

%Calculo del modelo dinamico de un Robot Manipulador de 2DOF
%usando el metodo de Lagrange-Euler
disp('--------------------------------------------------------------')
ROBTICA Pgina 20
disp('DINAMICA DE UN ROBOT 2DOF USANDO EL METODO DE LAGRANGE - EULER')
disp('--------------------------------------------------------------')
syms q1 q2 dq1 dq2 ddq1 ddq2;
syms l1 d2; %longitud de los eslabones
syms g; %parametro de gravedad [m/seg^2]
syms m1 m2 x1 x2 y1 y2 z1 z2
%------------------------------------------------------------
disp('1.- Tabla de Parametros DH')
disp('-a---alpha---d---theta---')
DH=[ 0 -pi/2 0 q1
0 0 q2 0 ]
%------------------------------------------------------------
disp('-------------------------------------------------------------')
disp('2.- Calculo de las matrices de transformacion: Aij')
disp('-------------------------------------------------------------')
%------------------------------------------------------------
%T=rotz(theta)*transl(0,0,d)*transl(a,0,0)*rotx(alpha)
disp('Matriz de TH del primer eslabon movil desde la base del robot')
disp('-------------------------------------------------------------')
A01=simple(simplify(denavit(DH(1,1),DH(1,2),DH(1,3),DH(1,4))))
A12=simple(simplify(denavit(DH(2,1),DH(2,2),DH(2,3),DH(2,4))));
%Matriz de transformacion de la base al efector final

disp('--------------------------------------------------------------')
disp('Matriz de TH del efector final desde la base del robot')
disp('-------------------------------------------------------------')
A02=simple(simplify(A01*A12))
%------------------------------------------------------------
disp('-------------------------------------------------------------')
disp('3.- Calculo de las matrices : Uij')
disp('-------------------------------------------------------------')
%------------------------------------------------------------
U11=simple(simplify(diff(A01,q1)))
%------------------------------------------------------------
disp('-------------------------------------------------------------')
disp('4.- Calculo de las matrices : Uijk')
disp('-------------------------------------------------------------')
%------------------------------------------------------------
U111=simple(simplify(diff(U11,q1)))
%------------------------------------------------------------
disp('-------------------------------------------------------------')
disp('5.- Matrices de Pseudoinercia : Ji')
disp('-------------------------------------------------------------')
%------------------------------------------------------------
%Para el primer eslabon: J1[Kg.m^2]
%m1=1.4Kg
%Coordenadas del centro de gravedad 1 respecto al frame 1
%x1=0;y1=0;z1=l1=0.5m;
disp('Para el Primer Eslabon')
disp('-------------------------------------------------------------')
ROBTICA Pgina 21
J1=[ 0 0 0 0
0 0 0 0
0 0 (m1*l1^2) (m1*l1)
0 0 (m1*l1) m1 ]
%------------------------------------------------------------
%Para el primer eslabon: J1[Kg.m^2]
%m2=1Kg
%Coordenadas del centro de gravedad 2 respecto al frame 2
%x2=0;y2=0;z2=0;
disp('-------------------------------------------------------------')
disp('Para el segundo eslabon')
disp('-------------------------------------------------------------')
J2=[ 0 0 0 0
0 0 0 0
0 0 0 0
0 0 0 m2]
%---------------------------------------------------------------------
-----
disp('-------------------------------------------------------------')
disp('6.- Calculo de las matrices de inercia : D = [dij]')
disp('-------------------------------------------------------------')
%---------------------------------------------------------------------
-----
%MATRIZ D11
d11=simple(simplify(trace(U11*J1*U11.')))
+simple(simplify(trace(U21*J2*U21.')))
%MATRIZ D12
%MATRIZ D21
%MATRIZ D22
%Matriz general
D = [d11 d12;d21 d22]
%---------------------------------------------------------------------
-----
disp('-------------------------------------------------------------')
disp('7.- Calculo del vector hikm')
disp('-------------------------------------------------------------')
%---------------------------------------------------------------------
-----
h111=simplify(trace(U111*J1*U11.'))+simplify(trace(U211*J2*U21.'))
h112=simplify(trace(U212*J2*U21.'))
%---------------------------------------------------------------------
-----
disp('----------------------------------------------------------------
-------')
disp('8.-Calculo del vector columna de fuerza centrifuga y de
coriolis:H=[hi]')
disp('----------------------------------------------------------------
-------')
%---------------------------------------------------------------------
-----
h1=h111*dq1^2+h112*dq1*dq2+h121*dq2*dq1+h122*dq2^2;
ROBTICA Pgina 22
h2=h211*dq1^2+h212*dq1*dq2+h221*dq2*dq1+h222*dq2^2;
%---------------------------------------------------------------------
-----
%Matriz de Coriolis
H=[h1;h2]
%---------------------------------------------------------------------
-----
disp('--------------------------------------------------------------')
disp('9.-Calculo de la matriz columna de fuerzas de gravedad: C=[ci]')
disp('--------------------------------------------------------------')
%---------------------------------------------------------------------
-----
g1=[g 0 0 0]; % Segun el frame 0 , gravedad actuando en el eje X
%---------------------------------------------------------------------
-----
% Vector de compuesto por coordenadas de posicion del centro de masas
% para cada eslabon
r11=[0;0;l1;1]; %CG para el eslabon 1
r22=[0;0;0;1]; %CG para el eslabon 2
c1=-m1*g1*U11*r11 - m2*g1*U21*r22;
c2=-m1*g1*U12*r11 - m2*g1*U22*r22;
%---------------------------------------------------------------------
-----
% Matriz de gravedad
C=[c1;c2]
%---------------------------------------------------------------------
-----
disp('-------------------------------------------------------------')
disp('10.- Ecuacion dinamica del robot')
disp('-------------------------------------------------------------')
%---------------------------------------------------------------------
-----
tau=simplify(D*[ddq1;ddq2]+H+C);
disp('El torque o par para la primera articulacion revoluta es: ')
disp('-------------------------------------------------------------')
T1 = tau(1) % Juntura Revoluta
disp('-------------------------------------------------------------')
disp('La fuerza para al articulacion prismatica es:')
disp('-------------------------------------------------------------')
F2 = tau(2) % Juntura Prismatica
disp('-------------------------------------------------------------')
disp('La notacion general de la ecuacion dinamica es:')
disp('-------------------------------------------------------------')
tau=[tau(1); tau(2)]
La solucin:
DINAMICA DE UN ROBOT 2DOF USANDO EL METODO DE LAGRANGE -

EULER
ROBTICA Pgina 23
1.- Tabla de Parametros DH
-a---alpha---d---theta---
DH =
[ 0, -pi/2, 0, q1]
[ 0, 0, q2, 0]
-------------------------------------------------------------
2.- Calculo de las matrices de transformacion: Aij
-------------------------------------------------------------
Matriz de TH del primer eslabon movil desde la base del robot
-------------------------------------------------------------
A01 =
[ cos(q1), 0, -sin(q1), 0]
[ sin(q1), 0, cos(q1), 0]
[ 0, -1, 0, 0]
[ 0, 0, 0, 1]
Matriz de TH del efector final desde la base del robot
-------------------------------------------------------------
A02 =
[ cos(q1), 0, -sin(q1), -q2*sin(q1)]
[ sin(q1), 0, cos(q1), q2*cos(q1)]
[ 0, -1, 0, 0]
[ 0, 0, 0, 1]
3.- Calculo de las matrices : Uij
------------------------------------------------------------
U11 =
ROBTICA Pgina 24
[ -sin(q1), 0, -cos(q1), 0]
[ cos(q1), 0, -sin(q1), 0]
[ 0, 0, 0, 0]
[ 0, 0, 0, 0]
U12 =
[ 0, 0, 0, 0]
[ 0, 0, 0, 0]
[ 0, 0, 0, 0]
[ 0, 0, 0, 0]
U21 =
[ -sin(q1), 0, -cos(q1), -q2*cos(q1)]
[ cos(q1), 0, -sin(q1), -q2*sin(q1)]
[ 0, 0, 0, 0]
[ 0, 0, 0, 0]
U22 =
[ 0, 0, 0, -sin(q1)]
[ 0, 0, 0, cos(q1)]
[ 0, 0, 0, 0]
[ 0, 0, 0, 0]
4.- Calculo de las matrices : Uijk
-------------------------------------------------------------
U111 =
ROBTICA Pgina 25
[ -cos(q1), 0, sin(q1), 0]
[ -sin(q1), 0, -cos(q1), 0]
[ 0, 0, 0, 0]
[ 0, 0, 0, 0]
U112 =
[ 0, 0, 0, 0]
[ 0, 0, 0, 0]
[ 0, 0, 0, 0]
[ 0, 0, 0, 0]
U121 =
[ 0, 0, 0, 0]
[ 0, 0, 0, 0]
[ 0, 0, 0, 0]
[ 0, 0, 0, 0]
U122 =
[ 0, 0, 0, 0]
[ 0, 0, 0, 0]
[ 0, 0, 0, 0]
[ 0, 0, 0, 0]
U211 =
[ -cos(q1), 0, sin(q1), q2*sin(q1)]
[ -sin(q1), 0, -cos(q1), -q2*cos(q1)]
ROBTICA Pgina 26
[ 0, 0, 0, 0]
[ 0, 0, 0, 0]
U212 =
[ 0, 0, 0, -cos(q1)]
[ 0, 0, 0, -sin(q1)]
[ 0, 0, 0, 0]
[ 0, 0, 0, 0]
U221 =
[ 0, 0, 0, -cos(q1)]
[ 0, 0, 0, -sin(q1)]
[ 0, 0, 0, 0]
[ 0, 0, 0, 0]
U222 =
[ 0, 0, 0, 0]
[ 0, 0, 0, 0]
[ 0, 0, 0, 0]
[ 0, 0, 0, 0]
5.- Matrices de Pseudoinercia : Ji
-------------------------------------------------------------
Para el Primer Eslabon
ROBTICA Pgina 27
J1 =
[ 0, 0, 0, 0]
[ 0, 0, 0, 0]
[ 0, 0, l1^2*m1, l1*m1]
[ 0, 0, l1*m1, m1]
-------------------------------------------------------------
Para el segundo eslabon
J2 =
[ 0, 0, 0, 0]
[ 0, 0, 0, 0]
[ 0, 0, 0, 0]
[ 0, 0, 0, m2]
6.- Calculo de las matrices de inercia : D = [dij]
d11 =
m1*l1^2 + m2*q2^2
d12 = 0
d21 = 0
d22 = m2
D=
[ m1*l1^2 + m2*q2^2, 0]
[ 0, m2]
7.- Calculo del vector hikm
h111 = 0
h112 = m2*q2
ROBTICA Pgina 28
h121 = m2*q2
h122 = 0
h211 = -m2*q2
h212 = 0
h221 = 0
h222 = 0
8.-Calculo del vector columna de fuerza centrifuga y de coriolis:H=[hi]
H=
2*dq1*dq2*m2*q2
-dq1^2*m2*q2
9.-Calculo de la matriz columna de fuerzas de gravedad: C=[ci]
C=
g*l1*m1*cos(q1) + g*m2*q2*cos(q1)
g*m2*sin(q1)
10.- Ecuacion dinamica del robot
El torque o par para la primera articulacion revoluta es:
T1 =
ddq1*(m1*l1^2 + m2*q2^2) + 2*dq1*dq2*m2*q2 + g*l1*m1*cos(q1) +

g*m2*q2*cos(q1)
La fuerza para al articulacion prismatica es:
F2 =
m2*(- q2*dq1^2 + ddq2 + g*sin(q1))
La notacion general de la ecuacion dinamica es:

ROBTICA Pgina 29
tau =
ddq1*(m1*l1^2 + m2*q2^2) + 2*dq1*dq2*m2*q2 + g*l1*m1*cos(q1) +

g*m2*q2*cos(q1)
m2*(- q2*dq1^2 + ddq2 + g*sin(q1))
Realizando el diagrama de bloques de la dinmica directa del manipulador, se

tendra de esta manera:
DINAMICA DIRECTA DEL ROBOT RR
ROBTICA Pgina 30
PARTE INTERNA DE LA DINAMICA
LAS SEALES DE ENTRADA DEL TORQUE Y FUERZA
ROBTICA Pgina 31
LAS SEALES DE SALIDA POSICION, VELOCIDAD Y ACELERACION
Realizando el diagrama de bloques de la dinmica directa del manipulador, se

tendra de esta manera:
DINMICA INVERSA DEL ROBOT RR
ROBTICA Pgina 32
PARTE INTERNA DE LA DINAMICA INVERSA
EL TORQUE DESEADO
ROBTICA Pgina 33
LA FUERZA DESEADA
CONTROL CINEMTICO:
El control cinemtica establece las trayectorias que debe seguir cada articulacin del
robot para cumplir con los requerimientos del usuario. Dichas trayectorias se
seleccionaran dependiendo de las restricciones fsicas propias de los accionamientos y a
criterios de calidad como suavidad o precisin de la misma.
Funciones de control cinemtico.
El robot recibe como entradas los datos procedentes del programa del robot escrito por
el usuario, (punto de destino, precisin, tipo de trayectoria, deseada, velocidad, etc.),
luego establece la trayectoria para cada articulacin como funciones del tiempo
ROBTICA Pgina 34
El control cinemtico selecciona cuatro puntos de la trayectoria, a continuacin trata de

unir estos cuatro puntos con algn tipo de funcin que pasando por todos ellos garantice
la suavidad y no supere las velocidades y aceleraciones mximas permisibles para cada
accionador .El resultado final del movimiento del extremo del robot es una trayectoria
que se aproxima en mayor o menor medida a la lnea recta deseada, el principal
inconveniente del procedimiento descrito radica en la necesidad de resolver
repetidamente la transformacin homognea inversa, lo que conlleva un elevado costo
computacional. Otra alternativa, puede ser utilizar un procedimiento basado en la
utilizacin de la matriz Jacobiana, la matriz Jacobiana establece las relaciones
diferenciales entre variables articulares y cartesianas, relacin que nos entrega la
localizacin la cual a su vez a travs de su derivada, permite establecer las velocidades a
partir de las correspondientes articulaciones. Luego los puntos finitos obtenidos de la
derivada deben ser interpolados
Generacin de trayectorias.
Normalmente el usuario del robot indica el movimiento que ste debe realizar
especificando las localizaciones especiales por las que debe pasar el extremo, junto con
otros datos, como instantes de paso, velocidades o tipos de trayectorias. As, por
ejemplo, es frecuente especificar que el robot debe ir de un punto inicial hasta otro final,
siguiendo en cartesianas una lnea recta a velocidad constante.
Puesto que estos puntos estn excesivamente separados es preciso seleccionar

puntos intermedios suficientemente cercanos como para que el control consiga ajustar
no slo el punto final especificado, sino tambin la trayectoria seguida a la indicada en
el programa.
ROBTICA Pgina 35
Para ello es preciso establecer un interpolador entre las localizaciones

expresadas en el espacio de la tarea que dar como resultado una expresin analtica de
la evolucin de cada coordenada. La interpolacin ms frecuente es la lineal, en la que
cada coordenada evoluciona a velocidad constante desde su valor inicial hasta su valor
final.
Para evitarlas discontinuidades de velocidad en el caso de paso por varios

puntos, pueden utilizarse las tcnicas de interpoladores a tramos o interpoladores
cbicos, los cuales se describirn ms adelante para el caso de variables articulares.
Evolucin de la orientacin: la especificacin de la posicin se realiza

habitualmente en coordenadas cartesianas. Sin embargo, la especificacin de la
orientacin puede realizarse mediante diferentes herramientas, como son: matrices de
rotacin, ngulos de Euler (en sus diferentes versiones), par de rotacin o cuaternios.
La alternativa ms adecuada para unir dos posiciones en el espacio de la tarea es

la evolucin lineal, a velocidad constante, de cada coordenada cartesiana desde su valor
inicial hasta el final, resultando un movimiento sencillo, fcilmente interpretable por el
usuario y de calidad; Sin embargo para el caso de la orientacin, esta orientacin lineal
puede ser planteada en trminos de matrices de rotacin, ngulos de Euler, par de
rotacin o cuaternios, resultando en cada caso trayectorias diferentes.
La utilizacin de ngulos de Euler, en cualquiera de sus formas, adems de ser la

representacin ms compacta, no representa este inconveniente. As, para pasar de una
orientacin inicial (i i i ) a una final (f f f ) se podran utilizar las funciones
lineales:
(t) = (f - i) t ti + i
tf ti
( t) = (f i) t ti+i
tf ti
(t) = (f - i) t ti + i
tf ti
Donde ti y tf son los instantes de tiempo en los que se pretende estar en la

orientacin inicial y final, respectivamente. El inconveniente de esta trayectoria es que
desde el punto de vista del usuario, es una trayectoria no intuitiva, con extraas
evoluciones de la orientacin. La evolucin ms natural desde una orientacin
inicial hasta otra final, ser aquella que hace girar de manera progresiva al efector final
(u objeto manipulado por el robot) desde su orientacin inicial hasta la final en torno a
un eje de giro fijo. Por este motivo, la utilizacin del par de rotacin, o su equivalente,
los cuaternios, es la manera ms adecuada para generar la trayectoria cartesiana de
ROBTICA Pgina 36
orientacin.
Dado un sistema ortogonal inicial y otro final rotado respecto del primero, existe
un nico eje k que permite pasar del sistema inicial al final girando un ngulo respecto
a l. Por lo tanto, para que el extremo del robot evoluciones desde la orientacin inicial
hasta el final, se podr buscar cual es el par ( k ) que relaciona los sistemas de
coordenadas ortogonales asociadas a ambas orientaciones, y realizar la evolucin
temporal mediante un giro en torno al eje k de valor:
(t) = t - ti
tf - ti
A partir del valor de (t) para instantes concretos de tiempo ser inmediato conocer el
cuaternio correspondiente.
Interpolacin de trayectorias
Como ser ha indicado, una de las funciones del control cinemtica es la de unir
una sucesin de puntos en el espacio articular por los que se quiere que pasen las
articulaciones del robot en un instante determinado .Adems, junto con las condiciones
de posicin- tiempo, es conveniente aadir restricciones en la velocidades y aceleracin
de paso por los puntos, de manera que se asegure la suavidad de la trayectoria y se
limiten las velocidades y aceleraciones mximas. Estas restricciones garantizarn que
los actuadores estn capacitados para implementar la trayectoria final.
Para ello deber seleccionarse algn tipo de funcin cuyos parmetros o

coeficientes se ajustarn al imponer las condiciones de contorno: posiciones,
velocidades y aceleraciones. En la seleccin de esta funcin debe considerarse que tanto
el clculo de sus parmetros, como su posterior utilizacin para generar puntos de
consigna de control dinmico, deben hacerse en tiempo real, por lo que la simplicidad
de la funcin ser un factor a valorar.
Se presentarn a continuacin las funciones interpoladoras utilizadas con mayor

frecuencia. Cada una de ella ha sido desarrollada para un solo grado libertad, debiendo
quedar claro que el mismo clculo deber repetirse para cada uno de los grados de
libertad del robot. Cabe indicar que si articular son igualmente aplicables para el espacio
de la tarea.
Interpoladores lineales: Supongamos que se pretende que una de las articulaciones q del
robot, pase sucesivamente por los valores qi en los instantes ti. Una primera solucin a
este problema consistira en mantener constante la velocidad de movimiento entre cada
dos valores sucesivos (qi-1, qi) de la articulacin.
ROBTICA Pgina 37
La trayectoria entre dos puntos q i-1, qi sera entonces:
Q (t) = (qi q i-1) t ti-1 + qi+1 ti-1 < t < ti

T
T = ti ti-1
Esta trayectoria asegura la continuidad de la posicin, pero origina saltos

bruscos en la velocidad (dq) de la articulacin, y consecuentemente precisa de
aceleraciones d2q) de valor infinito, lo que en la prctica no es posible.
La seleccin de los instantes de paso ti por los puntos qi podr haberse hecho segn los
diferentes criterios expuestos a continuacin:
Intentando que cada articulacin q alcance el punto de destino en el menor

tiempo posible sin considerara las dems articulaciones, lo que resultar en
velocidades dq constantes e iguales a la mxima.
Ajustando los instantes de paso a los de la articulacin que ms tiempo precise,

resultando movimientos coordinados.
DE LA EXPERIENCIA:
Primero se crea el ploteo del brazo en el plano XY:
El cdigo es:
function dibuja_RR(q)
L1=3; L2=1;
q1=q(1); q2=q(2);
x(1)=0;
y(1)=0;
x(2)=L1*cos(q1);
y(2)=L1*sin(q1);
x(3)=L1*cos(q1)+L2*cos(q1+q2);
y(3)=L1*sin(q1)+L2*sin(q1+q2);
hold on;
plot(x,y,'b','linewidth',2);
plot(x,y,'r.','MarkerSize',20);
grid on;
axis([-4 4 -4 4]);
hold off;
end
luego creamos la inversa de la trayectoria
el cdigo es:
function dibuja_RR(q)
ROBTICA Pgina 38
L1=3; L2=1;
q1=q(1); q2=q(2);
x(1)=0;
y(1)=0;
x(2)=L1*cos(q1);
y(2)=L1*sin(q1);
x(3)=L1*cos(q1)+L2*cos(q1+q2);
y(3)=L1*sin(q1)+L2*sin(q1+q2);
hold on;
plot(x,y,'b','linewidth',2);
plot(x,y,'r.','MarkerSize',20);
grid on;
axis([-4 4 -4 4]);
hold off;
end
Finalmente generamos las trayectorias que va a recorrer el brazo RR desde un punto

inicial hasta un punto final.
El cdigo es:
close all; clear all; clc;

x0=4; y0=0;
xf=0; yf=3;
[q0 k0]=inversa_trayectoriaRR(x0,y0);
[qf kf]=inversa_trayectoriaRR(xf,yf);
if k0*kf
clf;
hold on;
plot([x0 xf],[y0 yf],'c','LineWidth',2); %recta entre los puntos
inicial y final
plot([x0 xf],[y0 yf],'g.','MarkerSize',20); %tamao de los puntos
t=-pi:0.01:pi;
plot(2*cos(t),2*sin(t),'k','LineWidth',1); %circunferencia menor
plot(4*cos(t),4*sin(t),'k','LineWidth',1); %circunferencia mayor
grid on;
axis([-4 4 -4 4]);
hold off;
P=30;
[q dq ddq]=jtraj(q0,qf,P);
for k=1:length(q)
dibuja_RR(q(k,:));
pause(0.1);
end
figure
subplot 311
plot(q); %la posicion
subplot 312
plot(dq); %la velocidad
subplot 313
plot(ddq); %la aceleracion
end
ROBTICA Pgina 39
la grafica es el siguiente:
Grafica para un paso de N=30
Grfica del ngulo, velocidad y aceleracin para N=30
ROBTICA Pgina 40
ROBTICA Pgina 41
ROBTICA Pgina 42
Ahora generamos las trayectorias que va a recorrer el brazo RR desde un punto inicial
hasta un punto final.
El cdigo nmero 2 es:
close all; clear all; clc;
x0=3; y0=-2;
xf=1; yf=3.5;
[q0 k0]=inversa_trayectoriaRR(x0,y0);
[qf kf]=inversa_trayectoriaRR(xf,yf);
if k0*kf
A=[xf-x0 yf-y0 0];
B=[-x0 -y0 0];
d=norm(cross(A,B))/norm(A);
clf;
hold on;
plot([x0 xf],[y0 yf],'y','LineWidth',2);
plot([x0 xf],[y0 yf],'b.','MarkerSize',20);
t=-pi:0.01:pi;
plot(2*cos(t),2*sin(t),'k','LineWidth',1);
plot(4*cos(t),4*sin(t),'k','LineWidth',1);
grid on;
axis([-4 4 -4 4]);
hold off;
if d>2
N=20;
x=linspace(x0,xf,N);
y=linspace(y0,yf,N);
for k=2:N
[q dq ddq]=jtraj(inversa_trayectoriaRR(x(k-1),y(k-
1)),inversa_trayectoriaRR(x(k),y(k)),3);
for k=1:length(q)
dibuja_RR(q(k,:));
pause(0.1);
end
end
end
figure
subplot 311
plot(q);
subplot 312
plot(dq);
subplot 313
plot(ddq);
end
ROBTICA Pgina 43
ROBTICA Pgina 44
ROBTICA Pgina 45
CONTROL USANDO TORQUE COMPUTADO BAJO LA ACCION DE UN

CONTROL PD CON COMPENSACION DE GRAVEDAD:
Objetivo: control el movimiento del robot de 2 grados de libertad tipo RR como se

muestra en la siguiente figura
Determinando sus parmetros DH:
N Art d a
1 Q1 pi/2 0 L1
2 Q2 0 0 L2
Matriz de transformacin homognea del manipulador (ultimo efector respecto a la base

del robot):
Ahora procedemos a sacar las matrices de masa, coriolis y gravedad para

posteriormente realizar el control PD como sigue utilizamos el toolbox de Hemero para
calcular dichas matrices:
Definimos parmetros del robot
m1 = 1 kg, m2=1 kg,
l1=2, l2 =2,
g=9.81
Cm1=l1; Cm2=l2 (definimos los centros de masa en los extremos de los eslabones)
M=
[ m1l 12+ 4m 2 ( cos2( q 2) + 12 )l2
l22m2(cos ( q 2 )+1)
2
l22m2(cos ( q 2 )+1)
l 22m2 ]
C= [l22m2qd 2sin ( q 2 )(2qd 1+ qd 2 ) l 2 m2qd 1 sin ( q 2) ]
2 2
ROBTICA Pgina 46
G=
[g( l 1m1cos ( q 1 ) +l 2m2cos ( q 1 ) +l 2m 2cos ( q 1+q 2 ) ) gl2m2cos ( q 1+q 2) ]
ALGORITMO DE CONTROL PD CON COMPENSACION DE GRAVEDAD
Este tipo de control pertenece al conjunto de controladores por torque computado como
se muestra en la tabla
Donde la accin de control viene dada por la ecuacin resaltada en rojo.
Motivo del uso de este tipo de control:
El motivo del uso de este tipo de control es el de su simpleza cuando se

implementa y la anulacin del error en rgimen permanente de la posicin del ngulo
del eslabn, a causa de la compensacin que realiza la matriz G(q) ya que en rgimen
permanente solo rige la matriz C(q) y los valores q y q son nulos por ende al
adicionarle el termino G(q) a la ley de control har que elimine el error compensando
con la matriz C(q) que solo depende de la gravedad.
Determinacin de las ganancias Kp y Kv:
Para calcular se toma el principio de respuesta de los sistemas, que pueden ser
crticamente subamortiguados o sobre amortiguados, entonces para esto debemos definir
una frecuencia natural de trabajo:
Wn, y la constante Kp:
Kp=wn2 y para el caso crticamente amortiguado
ROBTICA Pgina 47
Kv=2 Kp
Algoritmo:
Seteando las condiciones iniciales de la posicin y velocidad de los eslabones:
q1 q2 dq 1 dq 2
Angulos y pi/4 pi/3 0 0

velocidades iniciales
Seteo de la posicin deseada:
xd= 2.8 / yd = 2.5
Aplicando la cinemtica inversa para la obtencin de los ngulos deseados:
q1d=arctan(yd,xd);
q2d=acos(((xd/cos(q1d))-l1)/l2);
q1d=0.7289 / q2d= 0.5016 (rad/s)
Definimos el error que entrara al controlador:
[
error= q 1 dq 1
q 2 dq 2 ]
Clculo del torque aplicado a los eslabones:
Kp= [ 60 0
0 50 ] Y [
Kv=
40 0
0 30 ]
Tau=Kp*Error - Kv*[dq1; dq2]+G;
Calculo de la aceleracin de rotacin de los revolutas ( q )
ROBTICA Pgina 48
q =inv. (M)*(Tau-C*[dq1; dq2]-G);
Actualizacin de los valores de q yq :
q1=q1+dt*q1_p;
q2=q2+dt*q2_p;
dq1=dq1+t* q (1);
dq2=dq2+t* q (2);
y aplicando la cinemtica directa para obtener la posicin final en funcin de los

ngulos finales tenemos:
x= (l1+l2*cos (q2))*cos (q1);
y= (l1+l2*cos (q2))*sin (q1);
Entonces tenemos la ubicacin final de los ngulos y de la posicin de la ltima

articulacin como sigue:
Cdigo de la controlador PD para un brazo robtico RR.
clear all; close all; clc

%
----------------------------------------------------------------------
--
% programa que simula el control de un robot de 2DOF con un
controlador PD
% con compensacion de gravedad
%
----------------------------------------------------------------------
--
% Posiciones incial-final
l1=3;l2=1; m1=2;m2=1; g=9.81; I1=1; I2=0;
lc1=l1; lc2=l2;
% Valores iniciales
q1 = pi/5;
q2 = pi/5;
q1_p = 0;
q2_p = 0;
% Definiendo las ganancias proporcional y derivativa del controlador

% se desea un amortiguamiento critico: Kp=w^2 y Kv=2sqrt(Kp)
% w1=1; w2=2;
% Kp1=w1^2;
% Kp2=w2^2;
ROBTICA Pgina 49
% Kv1=2*sqrt(Kp1);
% Kv2=2*sqrt(Kp2);
% Kp=[Kp1 0
% 0 Kp2];
% Kv=[Kv1 0
% 0 Kv2];
Kp=[50 0
0 50];
Kv=[30 0
0 30];
% Definiendo lo valores deseados

xd = 2.8;
yd = 2.5;
% Generacin de los angulos articulares deseados
% Cinemtica inversa
q1d=atan2(yd,xd);
q2d=acos(((xd/cos(q1d))-l1)/l2);
% Definicion de los tiempos de simulacion

ti=0;
dt=0.01;
tf=30;
k=1;
for t=ti:dt:tf
m11=m1*l1^2+4*m2*(cos(q2)/2 +1/2)*l2^2;
m12=l2^2*m2*(cos(q2)+1);
m21=m12;
m22=m2*l2^2;
M=[m11 m12
m21 m22];
c11= l2^2*m2*q2_p*sin(q2)*(2*q1_p + q2_p);
c12= l2^2*m2*q1_p^2*sin(q2);
c21=0;
c22=0;
C=[c11 c12
c21 c22];
g1=-g*(l1*m1*cos(q1) + l2*m2*cos(q1) + l2*m2*cos(q1 + q2));
g2=-g*l2*m2*cos(q1 + q2);
G=[g1
g2];
% Controlador
Error = [q1d-q1
q2d-q2];
tau=Kp*Error - Kv*[q1_p;q2_p]+G;
% Sistema
Q_2p=inv(M)*(tau-C*[q1_p;q2_p]-G);
q1=q1+dt*q1_p;
q2=q2+dt*q2_p;
q1_p=q1_p+dt*Q_2p(1);
q2_p=q2_p+dt*Q_2p(2);
% Cinematica directa
x=(l1+l2*cos(q2))*cos(q1);
y=(l1+l2*cos(q2))*sin(q1);
% Tomando datos
ROBTICA Pgina 50
ang1(k)=q1;
ang2(k)=q2;
tiempo(k)=t;
exis(k)=x;
ye(k)=y;
k=k+1;
end
figure(1)
%subplot(311)
plot(tiempo,ang1*180/pi,'b','linewidth',1.5)
title('Angulo de articulacin 1')
xlabel('Tiempo (seg)')
ylabel('Angulo (grad)')
%subplot(312)
figure(2)
plot(tiempo,ang2*180/pi,'r','linewidth',1.5)
title('Angulo de articulacin 2')
xlabel('Tiempo (seg)')
ylabel('Angulo (grad)')
%subplot(313)
figure(3)
plot(exis,ye), hold
plot([0 l1*cos(ang1(1)) exis(1)],[0 l1*sin(ang1(1))
ye(1)],'k','linewidth',1.5);
title('Movimiento del robot')
xlabel('X (m)')
ylabel('Y (m)')
for i=1:4
plot([0 l1*cos(ang1(i)) exis(i)],[0 l1*sin(ang1(i)) ye(i)]);
end
ROBTICA Pgina 51
Angulo de la primera articulacin
ROBTICA Pgina 52
Angulo de la segunda articulacin
Movimiento del robot RR
CODIGO EN LABVIEW
ROBTICA Pgina 53
El programa fue diseado para ingresar las coordenadas finales y los ngulos iniciales
de las articulaciones.
Al ingresar la posicin final se generan los ngulos finales a esa posicin y el programa
se detendr una vez se establezca el sistema, iniciando nuevamente el programa
solicitando se ingresen los DATOS INICIALES para correr el programa y realizar la
siguiente simulacin.
Coordenadas Finales: xd=1 y yd=1

ngulos iniciales: q1=0 y q2=30
Coordenadas finales: xd=-1 y yd=3.1

Angulos iniciales: q1=0 y q2=90
Coordenadas finales: xd=0 y yd=0
ROBTICA Pgina 54
Angulos iniciales: q1=60 y q2=30
ROBTICA Pgina 55

AnuaL Bol2 2010CV 1

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

AnuaL Bol2 2010CV 1

Cargado por

Copyright:

Formatos disponibles

UNIVERSIDAD NACIONAL DEL CALLAO

FACULTAD DE INGENIERIA ELECTRICA Y ELECTRONICA

CONDOR CORDOVA JAVIER ALAN 072059B

DURAN SALAS JOHAN AUGUSTO 072654H

ARANA URIARTE CHRISTIAN LUIS 072673B

CAMPOS TARAZONA OSCAR 080614A

BRAZO ROBTICO 2 GDL

El objetivo final de la robtica es el de construir sistemas inteligentes capaces de

El moldeamiento matemtico de la posicin y orientacin de las articulaciones se

Calculo de los parmetros Denavit-Hartenberg

a: Distancia entre la perpendicular comn a Z(i-1) a Z(i)

Alpha: Angulo de Z(i-1) a Z(i) medido alrededor de X(i)

De donde se determina la siguiente tabla:

Articulacin Theta D A Alpha

Ahora utilizando la matriz de transformacin homognea:

[cos(theta) -cos(alpha)*sin(theta) sin(alpha)*sin(theta) a*cos(theta)

sin(theta) cos(alpha)*cos(theta) -sin(alpha)*cos(theta) a*sin(theta)

Utilizando ahora lo valores de tabla D-H hallamos A 10 , A 21 y A32 .

Finalmente multiplicando estas tres matrices tenemos:

[ cos(q1)*cos(q2) - sin(q1)*sin(q2), - cos(q1)*sin(q2) - cos(q2)*sin(q1), 0, l1*cos(q1)

[ cos(q1)*sin(q2) + cos(q2)*sin(q1), cos(q1)*cos(q2) - sin(q1)*sin(q2), 0, l1*sin(q1) +

Donde la posicin del efector final ser:

[x,y,z]=[ l1*cos(q1) + l2*cos(q1+q2) , l1*sin(q1) + l2*sin(q1+q2), 0]

Cdigo del programa para la cinemtica directa:

%PARAMETROS PARA EL DENAVITH

%Longitud de los eslabones

%LA GRAFICA Y LA SIMULACION DEL PROBLEMA

L(1)=Link([0 d(1) a(1) alpha(1) 0]); %DH de longitud del

% SIMULACION DEL ROBOT RR

Las matrices del Denavit-Hartenberg

PRIMERA MATRIZ DENAVIT-HARTENBERG --> A01:

0.5000 -0.8660 0 1.5000

0.8660 0.5000 0 2.5981

SEGUNDA MATRIZ DENAVIT-HARTENBERG --> A12:

0.5000 -0.8660 0 0.5000

0.8660 0.5000 0 0.8660

MATRIZ DE TRANSFORMACION HOMOGENEA --> MTH:

-0.5000 -0.8660 0 1.0000

0.8660 -0.5000 0 3.4641

MATRIZ DE TRANSFORMACION HOMOGENEA --> N:

-0.5000 -0.8660 0 1.0000

0.8660 -0.5000 0 3.4641

MATRIZ DE TRANSFORMACION HOMOGENEA

PARA CONDICIONES FINALES DESPUES DE LOS MOVIMIENTOS

-0.5000 -0.8660 0 1.0000

0.8660 -0.5000 0 3.4641

La grafica de la posicin final del robot:

Por simple inspeccin:

Y para nuestro diseo:

De forma similar obtenemos para q(1):

Cdigo del programa para la cinemtica inversa:

LOS ANGULOS DESEADOS DE CINEMATICA INVERSA EN RADIANES

LOS ANGULOS DESEADOS DE CINEMATICA INVERSA

DINMICA DEL ROBOT RR:

Los movimientos de un robot son predecibles por la estructura de su

partes mviles del robot son consideraciones importantes que deben

La obtencin de este modelo para mecanismos de uno o dos grados de libertad no es

Modelo Dinmico Directo:

Modelo Dinmico Inverso:

Expresa las fuerzas y pares que intervienen en funcin de la evolucin de las

Resolucin del Modelo Dinmico:

El modelo dinmico es muy importante en el captulo del diseo de un robot ya que se

Articulacin Simple Lineal: Articulacin Rotacional

[cos(theta) -cos(alpha)sin(theta) sin(alpha)sin(theta) a*cos(theta)

sin(theta) cos(alpha)cos(theta) -sin(alpha)cos(theta) a*sin(theta)

[ cos(q1)cos(q2) - sin(q1)sin(q2), - cos(q1)sin(q2) - cos(q2)sin(q1), 0, l1*cos(q1)

[ cos(q1)sin(q2) + cos(q2)sin(q1), cos(q1)cos(q2) - sin(q1)sin(q2), 0, l1*sin(q1) +

[x,y,z]=[ l1cos(q1) + l2cos(q1+q2) , l1sin(q1) + l2sin(q1+q2), 0]

[ m1l1^2 + 4m2(cos(q2)/2 + 1/2)l2^2, l2^2m2(cos(q2) + 1)]

[ l2^2m2(cos(q2) + 1), l2^2*m2]

[ -l2^2m2qd2sin(q2)(2qd1 + qd2), l2^2m2qd1^2sin(q2)]

[ -g(l1m1cos(q1) + l2m2cos(q1) + l2m2cos(q1 + q2)), -gl2m2cos(q1 + q2)]