Académique Documents
Professionnel Documents
Culture Documents
Ec es la energia cinetica.
E p es la energia potencial
I.
INTRODUCCIN
1
L=T P= mv 2mgh
2
(4)
D ( q ) q +C ( q , q ) q + G+ F f =
En donde: D es la matriz de inercia
C es la matriz de Coriolis
G es la gravedad
F f fuerza de friccin
(5)
ET =Ec +E p
Ec =T
E p=P
(1)
(2)
(3)
[]
0
1= 0
l1
(6)
[ ]
[
[
(7)
]
]
(9)
2
3
q 180
l1 z
(14)
Despues de encontrar la ecuacin (14) se procede a
encontrar la matriz jacobiana la cual permite localizar las
posiciones singulares del robot.
El cual queda definido por la siguiente matriz.
l2 S1 l3 S12
J l2C1 l2C12
2
2
(13)
Y por ltimo de (11),(12),(13) se obtiene la siguiente el
siguiente vector para calcular las posiciones angulares del
robot.
(8)
l 2 C 1 +l 3 C 12
4 = l2 S 1 +l 3 S12
l 1q3
(12)
l l d
2l2l3
cos1
l2 C1
2 = l 2 S1
l1
l 2 C 1+l 3 C12
3= l 2 S 1+l 3 S 12
l1
d 2 l22 l32
cos
2l2 d
l3 S12
l3C12
0
0
0
1
(15)
l3C12
l3 S12
l C l C
2 1 3 12 l2 S1 l3S12
0
0
J 1
l2l3 S 2
0
0
l2l3 S 2
(16)
d x2 y2
Asi como para obtener
(10)
queda de las siguiente manera
y
tan
x
{ }
d dL dL
=T
dt d q
dq
1
L1= m v12m1 g h1=m1 g l 1
2 1
(11)
la
(17)
v 1=
[]
0
d 0
l1
dt
(18)
(19)
1q
(
3)
[]
0
2
v 1= 0 =0
0
(20)
1
1
L4= m v 24m4 g h3 = m3 l 22 q 21 +2 l 2 l 3 C2 ( q 21+ q 1 q 2 ) +l 23 ( q 1+ q
2 4
2
(28)
1
1
L2= m v 22m2 g h2= m 2 l 22 q 21m2 g l 1
2 2
2
v 2=
[ ]
l2 C 1
d l2 S 1
l1
dt
(21)
v 4=
[ ]
l 2 S1 q 1
= l 2 C 1 q 1
0
l 2 C 1+l 3 C12
d l 2 S 1+l 3 S 12
l 1q 3
dt
(22)
l 2 S 1 q 1l 3 S12 ( q 1 + q 2 )2
= l 2 C 1 q1 +l 3 C 12 ( q 1+ q 2 )2
q23
(29)
2
v =l q 21 +2l 2 l 3 C 2 ( q 21 + q 1 q 2 ) +l 23 ( q 1 + q 2 ) + q 23
2
4
2
2
(30)
(23)
4
1
1
2
2 2
2
2
2
L3= m v 3m 3 g h 3= m3 [l 2 q 1 +2 l 2 l 3 C 2 ( q 1+ q 1 q 2 ) +l 3 ( q 1+ q2 ) ]m3 g l1 LT = Li
(31)
2 3
2
i=1
(24)
L1=m1 g l 1
(32)
l 2 C 1 +l 3 C 12
1
L2= m2 l 22 q 21m2 g l 1
(33)
d l 2 S1 +l 3 S12
2
2
l2 S 1 q 1l 3 S 12 ( q 1+ q 2 )
1
2 2
2
2
2
l1
L3= m3 l 2 q 1+2 l 2 l 3 C2 ( q 1 + q 1 q 2) + l 3 ( q 1 + q 2 ) m3 g l 1
v 3=
= l 2 C1 q 1 +l 3 C12 ( q 1 + q 2 )2 ( 25)
2
dt
(34)
0
l
2
Para obtener v 3 es necesario reducir terminos para
( 1q 3)
simplificar la ecuacin y nos queda de la siguiente manera:
1
2
2 2
2
2
L4= m3 l 2 q 1 +2 l2 l 3 C 2 ( q 1 + q 1 q2 )+l 3 ( q 1 + q 2 ) m3 g
2
2
v 23=l22 S 21 q 21 +2 l 2 l 3 S1 S 12 ( q 21 + q 1 q 2 ) +l 23 S 212 ( q 1+ q2 )
(35)
2
2 2 2
2
2 2
Y
de
este
modo
se
obtiene
el
lagrangiano
total
del
robot
y
+l 2 C 1 q 1 +2l 2 l 3 C 1 C 12 (q 1 + q 1 q 2) + l 3 C 12 ( q 1+ q 2 )
(26)
1
2 2 1
2 2
LT =( m1 +m 2 +m 3+ m4 ) g l 1 + m2 l 2 q 1 + ( m 3 +m 4 ) l 2 q 1 +2 l 2 l 3
Donde: S 21+C 21=1
2
2
S 12 =S 1 C 2 +C1 S 2
(36)
C12=C 1 C 2 +S 1 S2
Al cual de la misma manera se le aplica la derivada temporal
de la derivada parcial de cada uno de las variables articulares.
S 212 +C 212=1
12 .
A continuacion
se presenta2 para q
2
v 23=l22 q 21+2 l 2 l 3 S 1 ( S 1 C 2+C 1 S 2) ( q 21 + q 1 q 2 ) +2 l 2 l 3 C 1 ( C1 C2 +
SL1 S 2) ( q 12+ q 1 q 2 ) +l 3 ( q 1 +2q 2 )
2 + ( m3 +m4 ) [ l 2 q 1 +l 2 l 3 C2 ( 2 q 1 + q 2 )+ l 23 ( q 1+ q 2 ) ]
2=m l q
v 23=l22 q 21+2 l 2 l 3 ( S21 C2 +C 1 S1 S 2+ C21 C2C 1 S 1 S2 )( q21 + q 1 q 2) +l
q13 ( q1 +2 q2 2) 1
(37)
2
(27)
{ }
d L
=m2 l 22 q1 + ( m3+ m4 ) l 22 q 1l 2 l 3 S2 ( 2 q 1 q 2 + q 22 ) +l 2 l 3 C2 (
dt q 1
(38)
L
=0
q1
Se realizan las mismas operaciones para
(39)
q 2 .
L
=( m3 +m4 ) (l 2 l 3 C 2 q 1+l 23 ( q 1+ q 2 ) )
q2
q
( 1+ q 2 )
l 2 l 3 S2 q 1 q 2+l 2 l 3 C 2 q 1+l 23
d L
=( m3 +m 4 )
dt q 2
G=
(41)
{ }
(51)
(52)
(53
(42)
V. RESULTADOS
En esta seccin se procede a llevar a cabo el control de nuestro
robot mediante la ayuda de SCILAB que es una herramienta
computacional que nos permite visualizar cmo se comporta
el robot y cambios presenta al aplicar un control Proporcional,
Derivativo Integral (PID).
(43)
{ }
(44)
(45)
D ( q ) q +C ( q , q ) q + G+ F f =
(46)
d
D=C+C T
dt
D=
(50)
L
=( m3+ m4 ) (l 2 l 3 S 2 ( q 21 + q 1 q 2 ) )
q2
3
Y tambien para q
L
=m4 q 3
q3
d L
=m4 q3
dt q 3
L
=m4 g
q2
[ ]
0
0
g m 4
F f = q + Ksign( q )
D ( q ) q +C ( q , q ) q + G ( q ) + F f =
Despejando a q de (52) nos queda:
1
q=D [ CGF f ]
(40)
(47)
(m3 +m4 )l
2l 3 S 2 q 2
( m3 + m4 ) l 2 l 3 S2 ( q 1+ q 2 )
( m3 +m 4 ) l2 l3 S 1 q1
C=[ 0 0000 ]
(48)
(49)
0
0
m4
5
La simulacin del robot se puede observar a continuacin en
la Fig. 5
VII. BIBLIOGRAFA
[1]tesis.ipn.mx/jspui/bitstream/123456789/5023/1/DISENOYCON
SBRAZO.pdf
[2] Fundamentos de Robtica, Antonio Barrientos.
Fig. 6 Robot SCARA control PID