INSTITUTO TECNOLGICO DE NUEVO

LAREDO

Ing. Mecatrnica
Robtica 2
Dr. Jos Luis Ortiz Simn

Trabajo Final
Robot PRR
Karina Gutirrez Rodrguez
1

N C. 10100145

Robot PRR

a2

a3

b2

b2

Cinemtica Directa
i

i ai
1

s 23=sin ( q 2+ q3 )

2
3

di

q1 0

c 2=cos q2
s 2=sin q 2

a2 0

q2

a3 0

q3

c 3=cos q3
s 3=sin q 3

c 23=cos ( q2 +q 3 )

[ ][ ][ ]

c2 s 2 0 a 2 c 2
c 3 s3 0 a3 c 3
1000
0
1
0
0
1
0
2
T =
T 2= s 2 c2 a2 s 2 T 3 = s 3 c 3 0 a3 s3
0 0 1 q1
0010
0010
000 1
0001
0001
0
1

[ ]

c 2 s2 0 a2 c 2
0
T 02 = s 2 c 2 a2 s2
0 0 1 q1
000 1

c 23s23 0 a2 c 2+ a3 c23
0
T 03 = s 23 c 23 a2 s 2+ a3 s23
0 0 1 q1
000 1

Cinemtica Inversa
1

cos

a2 + a3 ( Px + Py )
2 a 2 a3

q1 =P z

Px
Py

=tan1

cos

q3 =+

a32 + ( X d 2+Y d2 )a22

2

(X

2
d

+Y d2 ) a 3

q3 =+

Primer Cuadrante
|Px|>=|Py|

|Px|<|Py|

q 2= +

q 2=

q3 =+

q3 = +

Tercer Cuadrante
|Px|>=|Py|

q 2= + +
Segundo Cuadrante
|Px|>=|Py|

q 2= +

|Px|<|Py|

q3 =+

|Px|<|Py|

q 2= +
q3 = +

q 2= +
Cuarto Cuadrante

|Px|>=|Py|

|Px|<|Py|

q 2=2

q 2=2 +

q3 = +

q3 =+

Modelo Dinmico
a) Calculo de energas cinticas.

i=1

1
K 1= m1 q 12
2

V c 1=

[][]

0
d Oc 1 d 0
=
0 = 0
dt
dt
q1
q 1

i=2

2
c1

0
1

=[ 0 0 q 1 ]

W =0

[]

0
2
0 =q1
q 1

02
1

W = 0

[ ][ ]
[ ]

d Oc 2 d b2 c 2 b2 s 2 q2
V c 2=
=
b s = b2 c 2 q 2
dt
dt 2 2
q1
q 1

2
c2

=[b2 s 2 q 2 b2 c 2 q 2 q 1 ]

b 2 s2 q 2
2 2 2
2 2
2
2
2
2
2
b2 c 2 q 2 =b2 s 2 q 2 +b 2 c 2 q2 + q 1 =b2 q 2 + q 1
q 1

[] [ ]
[]

0
0
W =z 1 q 1= 0 q 2= 0
1
q 2
0
2

0
W 02
=
0
0
q

22
[
2
2] 0 =q
q 2
K 2=

1
2
2 1
2
m2 b2 + I 2 ] q 2 + m2 q1
[
2
2

i=3

][

a c +b c
a2 s 2 q2b 3 s23 ( q 2+ q 3 )
d Oc 3 d 2 2 3 23
V c 3=
=
a s + b s = a2 c2 q 2 +b 3 c 23 ( q 2+ q 3 )
dt
dt 2 2 3 23
q1
q 1

Vc3

]
[

a2 s 2 q2b 3 s23 ( q 2+ q 3 )
=[a2 s 2 q 2b3 s23 ( q 2+ q 3 ) a2 c 2 q2 +b3 c 23 ( q 2 + q 3) q 1 ] a2 c 2 q 2 +b 3 c 23 ( q 2+ q 3 )
q 1

[ ][ ][ ]

0
0
0
W 03= 0 + 0 = 0
q2 + q 3
q 2 q 3

W 02
q 2+ q 3 ]
3 =[ 0 0

a22 q 22 +b32 ( q 2+ q 3 )2+ 2 a2 b 3 c 3 ( q 2+ q 3 ) q 2 + q12

[ ]

0
2
0 =( q 2+ q 3 )
q 2+ q 3

K 3=

1
2
2
2
2
2
2
m a q + b3 (q 2+ q3 ) +2 a2 b3 c 3 ( q 2 + q 3 ) q 2 + q 1 } + I 3 ( q 2 + q 3)
2 3{ 2 2

K=K 1 + K 2 + K 3

K=

m1 q 12 + [ m 2 b22+ I 2 ] q 22 +m2 q 12
1
2 + m { a 2 q 2+ b 2 ( q + q )2 +2 a b c ( q + q ) q + q 2 }+ I ( q + q )2
3
2
2
3
2
3
2 3 3 2
3 2
1
3
2
3

b) Calculo de Energas Potenciales.

P1=m1 g q 1

P2=m2 g q 1

P3=m3 g q1

m
( 1+m2+ m3 ) q1
P=P1 + P2 + P3=g

c) Calculo de Lagrangiano

L=K P

m
( 1+ m2 +m3 )q 1
L=

1
m1 q 12 + [ m2 b22+ I 2 ] q 22 +m2 q 12 +m3 { a22 q 22 +b32 ( q 2 + q 3 )2 +2 a2 b3 c3 ( q 2+ q 3 ) q 2+ q 12 }+ I 3 ( q 2 + q 3 )2 g
2

d) Aplicacin de ecuacin de Euler-Lagrange

i=1

1=

[ ]

d L
L

dt q 1 q 1

m
( 1+m2+ m3 ) q 1
L
=
q 1

m
( 1+m2+ m3 ) q 1
d L
=
dt q 1

[ ]

m
( 1+m2+ m3 ) g
L
=
q1

m
q
( 1+ g)
( 1+m 2+ m3 )
1=

i=2

2=

[ ]

d L
L

dt q 2 q 2

L
2
2
2
=( m2 b2 + I 2 +m3 a2 +2 m3 a2 b3 c 3) q 2+ m3 a2 b3 c 3 q 3 + I 3 ( q 2+ q 3 ) + m3 b3 ( q 2 + q 3 )
q2

[ ]

d L
=[ m 2 b22+ I 2 +m3 a22+ 2m3 a2 b3 c 3+ m3 b32 + I 3 ] q2 + [ m3 a2 b 3 c3 q 3 + m3 b32 + I 3 ] q3m3 a2 b3 s 3 q 3 (2 q 2 + q 3)
dt q 2
L
=0
q2

2= [ m2 b 22 + I 2+ m3 a22 +2 m3 a 2 b 3 c 3 +m3 b32+ I 3 ] q 2+ [ m3 a2 b3 c 3 q 3 +m3 b32+ I 3 ] q 3m3 a2 b 3 s3 q 3( 2 q 2+ q 3 )

i=3

3=

[ ]

d L
L

dt q 3 q 3

L
2
2
=( m3 a2 b3 c 3+ m3 b3 + I 3 ) q 2 +(m3 b 3 + I 3) q 3
q3

[ ]

d L
=( m3 a2 b3 c 3+ m3 b 32 + I 3 ) q 2 +(m3 b 32+ I 3 )q 3m3 a2 b3 s 3 q 2 q 3
dt q 3

L
=m3 a2 b3 s 3 q 2 (q 2+ q 3 )
q3

10

 3 =( m3 a2 b 3 c3 +m3 b32 + I 3 ) q 2 +(m3 b32 + I 3) q 3 m3 a 2 b3 s3 q 22

m
q
( 1+ g)
( 1+m 2+ m3 )
1=
2= [ m2 b 22 + I 2+ m3 a22 +2 m3 a 2 b 3 c 3 +m3 b32+ I 3 ] q 2+ [ m3 a2 b3 c 3 q 3 +m3 b32+ I 3 ] q 3m3 a2 b 3 s3 q 3( 2 q 2+ q 3 )
3 =( m3 a2 b 3 c3 +m3 b32 + I 3 ) q 2 +(m3 b32 + I 3) q 3 m3 a 2 b3 s3 q 22
e) Obtencin la matriz de inercias D, la matriz de Coriolis C y el vector de gravedad g a partir de los

[ ]
[]

d 11 d 12 d 13
D= d 21 d 22 d 23
d 31 d 32 d 33

c11 c 12 c 13
C= c 21 c 22 c 23
c 31 c 32 c 33

g1
g= g 2
g3

d 11=m1 +m2 +m3

2

d 23 =d 32=m3 a2 b3 c3 q 3+ m3 b3 + I 3
2

d 22 =m2 b 2 + I 2 +m3 a2 +2 m3 a2 b3 c3 +m3 b3 + I 3

d 13=d 31=d 12=d 21=0

d 33=m3 b32+ I 3

11

c 11=c12=c13=c 21=c 31=c 33=0

c 23=m3 a 2 b3 s3 q 3
c 32=m3 a 2 b 3 s3 q 2

c 22=2m3 a2 b3 s 3 q 3

1+m
(
2+ m3 ) g
g1=

12

g2=g3 =0 Modelo Dinmico por Jacobianos

i=1

J v 1=z 1

[]

0
z 1= 0
1

[ ] [ ]
[ ]

0 0 0
J v= 0 0 0
1 0 0

I1 J w 1

0 0 0
J w= 0 0 0
0 0 0

[ ][ ] [ ]

0 0 1 0 0 0
1 0 0
J v 2= 0 0 0 0 0 0 = 0 0 0
0 0 0 1 0 0
0 0 0

[ ]

0 0 0
J w 2= 0 0 0
0 0 0

0 0 0
=0 0 0
0 0 0

i=2

[ ] [] [ ]
[ ]

b2 c2
O c 2 = b2 s 2
q 1

[ ]

b2 c 2
O
O
=
( c 2 1 ) b 2 s2
0

[]

0
J v 1= 0
1

0
O 0= 0
0

b 2 s2
J v 2=z 1 ( Oc 2O1 )= b2 c 2
0

0
O 1= 0
q 1

[]

0
z 0=z 1= 0
1

[ ]

m1 0 0
2
m1 J v1 = 0 0 0
0 0 0

0 b 2 s2 0
J v = 0 b2 c 2 0
1
0
0

[ ]

0 0 0
J w= 0 0 0
0 1 0

[
][ ] [
[ ][ ] [ ]

1 0 0
0
0
1 0 b2 s2 0
J v = b 2 s2 b2 c2 0 0 b 2 c 2 0 = 0 b22 0
0
0
0 1
0
0
0 0 0
2

0 0 0 0 0 0
0 0 0
J w 2= 0 0 1 0 0 0 = 0 1 0
0 0 0 0 1 0
0 0 0

m2
0
0
2
m2 J = 0 m2 b 2 0
0
0
0
2
v

[ ]

0 0 0
I 2 J w 2= 0 I 2 0
0 0 0

i=3

0 a 2 s2b 3 s23 b 3 s23

J v = 0 a2 c2 +b 3 c23
b3 c 23
1
0
0

]
][

][

0
0
0
0
1 0 a2 s2 b3 s 23 b3 s 23 0
2
2
2
J v = a 2 s2b3 s 23 a2 c 2+ b3 c23 0 0 a2 c 2+ b3 c 23
b 3 c23 = 0 a 2 +b 3 +2 a 2 b3 c 3 b 3 + a2 b3 c3 0
b 3 s23
b3 c 23
0 1
0
0
0
b32+ a2 b3 c 3
b 32
2

0
0
0
2
2
2
m 3 J = 0 m3 (a2 + b3 +2 a2 b3 c3 ) m3 (b 3 + a2 b3 c3 )
2
v

m 3 (b3 +a 2 b 3 c 3 )

m3 b 3

[ ][ ] [ ]

0 0 0 0 0 0
0 0 0
Jw = 0 0 1 0 0 0 = 0 1 1
0 0 1 0 1 1
0 1 1
2

]
[ ]

0 0 0
I 3 J w 2= 0 I 3 I 3
0 I3 I3

d 11=m1 +m2 +m3

d 22=m2 b 22+ I 2 +m3 a22+2 m3 a2 b3 c3 +m3 b32 + I 3
d 23 =d 32=m3 a2 b3 c3 q 3+ m3 b32 + I 3
d 13 =d 31=d 12=d 21 =0

d 11 d 12 d 13
D= d 21 d 22 d 23
d 31 d 32 d 33

d 33=m3 b32+ I 3

Matriz de Coriolis por Cristofel

k =1

c 11=

j=1

1 d 11 d 11 d 11
1 d 11 d 12 d 21
1 d 11 d 13 d31
+

q 1 +
+

q 2 + (
+

) q
2 q1 q 1 q 1
2 q2 q 1 q1
2 q 3 q1 q 1 3

) (

i=1i=2i=3

c 11=0

k =1
c 12=

j=2
d
d
1 d 12 d 11 d12
1 d 12 d 12 d 22
1 d
+

q 1 +
+

q 2 + ( 12 + 13 32 ) q 3
2 q1 q 2 q 1
2 q2 q2 q1
2 q3 q2 q1

) (

i=1i=2i=3
c 12=0

k =1

c 13=

j=3

1 d 13 d 11 d 13
1 d 13 d12 d 23
1 d 13 d13 d 33
+

q 1+
+

q 2+ (
+

)q
2 q1 q 3 q 1
2 q 2 q 3 q 1
2 q 3 q 3 q 1 3

) (

i=1i=2i=3

c 13=0

k =2

c 21=

j=1

1 d 21 d 21 d 11
1 d21 d 22 d21
1 d 21 d 23 d 31
+

q 1+
+

q 2 + (
+

) q
2 q1 q 1 q 2
2 q 2 q1 q 2
2 q3 q1 q 2 3

) (

i=1i=2i=3

c 21=0

k =2
c 22=

j=2
d
d
1 d 22 d 21 d 12
1 d 22 d 22 d 22
1 d
+

q 1+
+

q 2+ ( 22 + 23 32 ) q 3
2 q1 q 2 q2
2 q 2 q 2 q 2
2 q3 q2 q2

) (

i=1i=2i=3
c 22=m3 a2 b3 s 3 q 3

k =2

c 23=

j=3

1 d 23 d 21 d 13
1 d 23 d 22 d 23
1 d 23 d 23 d 33
+

q 1 +
+

q 2 + (
+

) q
2 q1 q 3 q2
2 q2 q3 q2
2 q3 q3 q 2 3

) (

i=1i=2i=3

q 2 + q
( 3)
c 23=m3 a 2 b3 s3

k =3

c 31=

j=1

1 d 31 d 31 d 11
1 d 31 d 32 d 21
1 d 31 d 33 d 31
+

q 1 +
+

q 2 + (
+

) q
2 q1 q 1 q 3
2 q2 q1 q3
2 q3 q1 q3 3

) (

i=1i=2i=3

c 31=0

k =3
c 32=

j=2
d
d
1 d 32 d 31 d 12
1 d 32 d32 d 22
1 d
+

q 1+
+

q 2+ ( 32 + 33 32 ) q 3
2 q1 q 2 q 3
2 q2 q2 q3
2 q3 q2 q 3

i=1i=2i=3
c 32=m 3 a 2 b 3 s3 q 2

) (

k =3

c 33=

j=3

1 d 33 d 31 d 13
1 d 33 d 32 d 23
1 d 33 d 33 d 33
+

q 1 +
+

q 2 + (
+

) q
2 q1 q 3 q3
2 q2 q3 q3
2 q3 q3 q3 3

) (

i=1i=2i=3

c 33=0

c11 c 12 c 13
C= c 21 c 22 c 23
c 31 c 32 c 33
Vector Gravedad

m
( 1+m2+ m3 ) g
g= 0
0

[ ]

Control PD Robot PRR

Modelo matemtico.

Planta ++=
Ley de control PD
=

En lazo cerrado
++=
=(r)

=()

Como se quiere la estabilizacin la velocidad deseada es igual a cero (=0)

=
Entonces:

+++(r)+ ()=0

Modelo en Simulink (R2_T10_145)

Posicin de los eslabones. (grafica 1.1)

Tipo de unin
Numero de grafica y
eslabn
Posicin deseada
Posicin real final
t=15 s

Prismtica

Revoluta

Revoluta

Grafica #1. (q1)

Grafica #2 (q2)

Grafica #3 (q3)

.5
.467

.29
.29

.35
.35

Velocidad de los eslabones. (grafica 1.2)

Tipo de unin
Numero de grafica y
eslabn
Velocidad real final
Velocidad deseada
t=15 s

Prismtica

Revoluta

Revoluta

Grafica #1. (q1 punto)

Grafica #2 (q2 punto)

Grafica #3 (q3 punto)

0
0

0
0

0
0

Aceleracin de los eslabones. (grafica 1.3)

Tipo de unin
Numero de grafica y
eslabn
Aceleracin deseada
Aceleracin real final

Prismtica
Grafica #1. (q1 punto
punto)
0
0

Revoluta
Grafica #2 (q2 punto
punto)
0
0

Revoluta
Grafica #3 (q3 punto
punto)
0
0

Como se puede apreciar en las graficas, el control PD fue realizado

satisfactoriamente con un margen de error en el primer eslabn de 6% de

error, lo cual es de esperarse pues la fuerza de gravedad acta en dicho

eslabn, lo cual al momento de disear el modelo a escala real es un factor
en tomar en cuenta. En lo que se refiere en los otros dos eslabones si se
llega a la posicin deseada en menos de 5 segundos (lo cual se puede
apreciar en la grafica).
Como se planteo en el modelo matemtico, la velocidad final deseada en
los eslabones es cero y en la simulacin se puede comprobar que fue logrado
el propsito. Al igual que la velocidad, la aceleracin final debe de ser cero y
en las graficas se comprueba que tambin este aspecto fue cubierto.

Control PD con Friccin Robot PRR

Modelo matemtico.

Planta con friccin.

D q +C q + g+ B q = + g+ B q

Ley de control PD
=

En lazo cerrado
D q +C q + g+ B q =Kp qKv q + g+ B q

=(r)

=()

Como se quiere la estabilizacin la velocidad deseada es igual a cero (=0)

=
Entonces:

+++(r)+ ()

gB q

=0

Modelo en Simulink (R2_T11_145)

Posicin de los eslabones (grafica 2.1)

Tipo de unin
Numero de grafica y
eslabn
Posicin deseada
Posicin real final
t=15 s

Prismtica

Revoluta

Revoluta

Grafica #1. (q1)

Grafica #2 (q2)

Grafica #3 (q3)

.5
.5

/3
/3

Velocidad de los eslabones. (grafica 2.2)

Tipo de unin
Numero de grafica y
eslabn
Velocidad real final
Velocidad deseada
t=15 s

Prismtica

Revoluta

Revoluta

Grafica #1. (q1 punto)

Grafica #2 (q2 punto)

Grafica #3 (q3 punto)

0
0

0
0

0
0

Aceleracin de los eslabones. (grafica 2.3)

Tipo de unin
Numero de grafica y
eslabn
Aceleracin deseada
Aceleracin real final
t=15 s

Prismtica
Grafica #1. (q1 punto
punto)
0
0

Revoluta
Grafica #2 (q2 punto
punto)
0
0

Revoluta
Grafica #3 (q3 punto
punto)
0
0

En este paso del trabajo se implemento el control PD pero considerando

la friccin, por lo que se tuvo que realizar las compensacin necesarias para
poder contrarrestar este factor, como se puede observar en la grafica 2.1 y
comparndola con la grafica 1.1, se tarda ms en llegar a la posicin de los
estabones deseada, y as mismo sucede al comparar las graficas de
velocidad (1.2 y 2.2) y aceleracin (1.3 y 2.3).
En donde tambin se realizaron cambios fueron en las matrices Kv y Kp,
pues se tuvieron que aumentar sus valores para poder tener un mejor
control.

Control PD de robot PRR a partir de la posicion del efector

final.
Cinematica Inversa y Directa. (pags. 2 y 3)
Validacion (R2_validacion_145)
display('PRR 10100145');
L1=1;
a2=.5;
a3=.5;
display('Favor de tener en cuenta que las unidades de medidas manejadas son en metros, y las medias maximas y
minimas son 0<=Pz<=1 y sqrt(Px^2+Py^2)<=1')
Xd=input('Ingresa Px=\n');
Yd=input('Ingresa Py=\n');
Zd=input('Ingresa Pz=\n');
if sqrt(Xd^2+Yd^2)>(a2+a3)
display('Error: Debido al diseno del robot no es posible posicionarse en dichos puntos de X y Y, prueba que la
raiz de la suma de los cuadrados de Px y Py sean menores a 1');
elseif Zd>L1 || Zd<0
display('La coordenada deseada de z es mayor a lo que permite el diseno o es de valor negativo');
else
c=sqrt(Xd^2+Yd^2);
teta= acos((a2^2+a3^2-c^2)/(2*a2*a3));
alfa=atan(abs(Yd)/abs(Xd));
beta=acos((c^2+(a3)^2-(a2)^2)/(2*c*a3));
q1=Zd
if Xd==1 && Yd==0
q2=0
q3=0
elseif Xd==-1 && Yd==0
q2=pi
q3=0
elseif Xd==0 && Yd==1
q2=pi/2

q3=0
elseif Xd==0 && Yd==-1
q2=3*pi/2
q3=0
elseif abs(Xd)>=abs(Yd)
if Xd>0 && Yd>0
q2=alfa+beta
q3=teta+pi
elseif Xd<0 && Yd>0
q2=pi-(alfa-beta)
q3=teta+pi
elseif Xd<0 && Yd<0
q2=pi+(alfa+beta)
q3=teta+pi
elseif Xd>0 && Yd<0
q2=2*pi-(alfa+beta)
q3=-teta+pi
end
elseif abs(Xd)<abs(Yd)
if Xd>0 && Yd>0
q2=alfa-beta
q3=-teta+pi
elseif Xd<0 && Yd>0
q2=pi-(alfa-beta)
q3=teta+pi
elseif Xd<0 && Yd<0
q2=pi+(alfa-beta)
q3=-teta+pi
elseif Xd>0 && Yd<0
q2=2*pi-(alfa-beta)
q3=teta+pi
end
end
T01=[1 0 0 0; 0 1 0 0; 0 0 1 q1; 0 0 0 1];
T12=[cos(q2) -sin(q2) 0 a2*cos(q2); sin(q2) cos(q2) 0 a2*sin(q2); 0 0 1 0; 0 0 0 1];
T23=[cos(q3) -sin(q3) 0 a3*cos(q3); sin(q3) cos(q3) 0 a3*sin(q3); 0 0 1 0; 0 0 0 1];
display('Matriz de Transformacion de 0 a 2');
T02=T01*T12
display('Matriz de Transformacion de 0 a 3');
T03=T02*T23

end

Modelo en Simulink (R2_T12_145)

Posicin del efector final (grafica 3.1)

Numero de grafica
Posicin deseada
Posicin real final
t=15 s

Grafica #1. (Px)

.3
.3

Grafica #2 (Py)
.7
.7

Grafica #3 (Pz)
.4
.4

En esta ocasin lo que se desea es posicionar el efector final de acuerdo a

las coordenadas especificadas para x, y, z del mismo, por lo que se hace uso
de la cinemtica inversa para obtener los valores de q1, q2 y q3 para poder
implementarlas en el control PD con friccin previamente realizado, y as
mismo se utiliza la cinemtica directa para que los valores arrojados por el
controlador se conviertan en la posicin del efector final y as ir comparado
para poder lograr el control deseado; y como se puede observar en la grafica
3.1 el resultado fue favorable. Y para hacer la validacin de este sistema se
implemento un archivo .m.

Control cartesiano para seguimiento de trayectorias.

Ecuacion del circulo con centro fuera del origen.
2

( xh) +( y k ) =r

Despejando

y
Py= r 2( xh)2 + k

Px=t /10

Pz=r

t=tiempo de simulacion

Modelo en Simulink(R2_T13_145)

Deseado:

:Real

Como se puede apreciar en las graficas de arriba, nos e pudo lograr

satisfactoriamente lo deseado pues aunque se logra realizar medio circulo,
la otra parte no se logra pues al haber una raz cuadrada en la formula, es
necesario que los valores de y sean tambin negativos al momento de
obtener su valor y as poder tener un circulo.