Diseño de robot RRP con asignación de los marcos referenciales

Figura 1 Robot RRP con marcos referenciales correpondientes

Tabla 1 de parámetros

CM1 CM1 0 0 q1
i ai ❑i di ❑i

1 a1 Pi/2 0 q2
CM2 Xcm2 0 Ycm2 q2
2 a3 Pi/2 0 q2+pi/
2
CM3 0 0 a2-cm3+q3 0
3 0 0 a2+q3 0
 Cinemática Directa

Utilizando la matriz generalizada.

[ ]
i−1
cos ( θ i ) −sin ( θ i ) cos ( α i ) sin ( θi ) sin ( α i ) a i cos ( θi )
sin ( θ i) cos ( θi ) cos ( α i ) −cos ( θi ) sin ( α i ) ai sin ( θ i )
T= (1.0)
0 sin ( α i ) cos ( α i ) di
i 0 0 0 1

Sustituyendo para los casos siguientes.

[ ]
0
cos (q 1) 0 sin(q 1) a 1∗cos (q 1)
sin(q 1) 0 −cos (q 1) a 1∗sin (q 1)
T= (1.1)
0 1 0 0
1 0 0 0 1

[ ]
1
−sin (q 2) 0 cos (q 2) −a 3∗sin(q 2)
T = cos (q 2) 0 sin(q 2) a 3∗cos (q 2) (1.2)
0 1 0 0
2 0 0 0 1

[ ]
2
1 0 0 0
0 1 0 0
T= (1.3)
0 0 1 a 2+ q 3
3 0 0 0 1

Multiplicando las matrices y aplicando propiedades.

 [
0
−cos ( q 1 )∗sin ( q 2 ) sin ( q 1 ) cos ( q 1 )∗cos ( q 2 ) cos ( q 1 )∗( a 1+ a 2∗cos ( q 2 )−a 3∗sin ( q 2 )+ q 3∗c
T =01T 12T 23T = −sin ( q 1 )∗sin ( q 2 ) −cos ( q 1 ) cos ( q 2 )∗sin ( q 1 ) sin ( q 1 )∗( a 1+ a 2∗cos ( q 2 )−a 3∗sin ( q 2 )+ q 3∗co
cos ( q 2 ) 0 sin ( q 2 ) sin ( q 2 )∗( a 2+ q 3 ) +a 3∗cos ( q 2 )
3 0 0 0 1

Matriz de Transformacion - T0CM1

T0CM1 =

[ cos(q1), -sin(q1), 0, cm1*cos(q1)]

[ sin(q1), cos(q1), 0, cm1*sin(q1)]

[ 0, 0, 1, 0]

[ 0, 0, 0, 1]

Matriz de Transformacion - T1CM2

T1CM2 =

[ cos(q2), -sin(q2), 0, Xcm2*cos(q2)]

[ sin(q2), cos(q2), 0, Xcm2*sin(q2)]

[ 0, 0, 1, Ycm2]

[ 0, 0, 0, 1]

Matriz de Transformacion - T2CM3

T2CM3 =

[ 1, 0, 0, 0]

[ 0, 1, 0, 0]

[ 0, 0, 1, a2 - cm3 + q3]

[ 0, 0, 0, 1]
Matriz de Transformacion - T0CM2

T0CM2 =

[ cos(q1)*cos(q2), -cos(q1)*sin(q2), sin(q1), a1*cos(q1) + Ycm2*sin(q1) + Xcm2*cos(q1)*cos(q2)]

[ cos(q2)*sin(q1), -sin(q1)*sin(q2), -cos(q1), a1*sin(q1) - Ycm2*cos(q1) + Xcm2*cos(q2)*sin(q1)]

[ sin(q2), cos(q2), 0, Xcm2*sin(q2)]

[ 0, 0, 0, 1]

Matriz de Transformacion - T0CM3

T0CM3 =

[ -cos(q1)*sin(q2), sin(q1), cos(q1)*cos(q2), a1*cos(q1) - a3*cos(q1)*sin(q2) + cos(q1)*cos(q2)*(a2 -cm3 + q3)]

[ -sin(q1)*sin(q2), -cos(q1), cos(q2)*sin(q1), a1*sin(q1) + cos(q2)*sin(q1)*(a2 - cm3 + q3) - a3*sin(q1)*sin(q2)]

[ cos(q2), 0, sin(q2), sin(q2)*(a2 - cm3 + q3) + a3*cos(q2)]

[ 0, 0, 0, 1]

Comprobaciones
Cuando

[ ]
90°
q⃗ = 90° ( 2.0 )
10

La matriz de rotación es la siguiente

[ ]
0
0 1 0
R= −1 0 0 (2.1)
3 0 0 1

En cuanto el vector de posición es el siguiente

[ ]
0
0
P= a 1−a3 (2.2)
3 a 2+10

Sustituyendo q⃗ en la ecuación 1.4 se obtiene la matriz de transformación 3T que

0

da como resultado la matriz de la ecuación 2.3:

[ ]
0
0 1 0 0
−1 0 0 a1−a 3
T= (2.3)
0 0 1 a 2+10
3 0 0 0 1

Comentarios. Como se puede apreciar tanto el modelo de cinematica directa

como el de cinematica inversa son correctos
 Modelo diferencial de velocidad lineal

Al momento de obtener el modelo diferencial de velocidad lineal se utiliza la

siguiente ecuación junto con la matriz Jacobiana.

V (t )=J v q̇ (4.0)

Se utiliza la matriz de transformación de la ecuación del vector de posición.

Mediante derivadas parciales se utiliza la siguiente ecuación:

[ ]
d d d
d q1
( p x(q ) )
d q2
(t )
( p x(q ) ) … ( px )
d q n (q )
( t) (t )

d d d
d q 1 ( y(q ) ) d q 2 ( y(q ) ) d q n ( y(q ) )
Jv = p (t )
p … p
(t )
(4.2)
( t)

d d d
( p ) ( p ) …+ d q ( p z(q ) )
d q 1 z(q ) d q 2 z(q )
(t ) ( t) (t )
n

De tal manera que la matriz jacobiana es de la siguiente manera:

J v =¿(4.3)

[ -sin(q1)*(a1 + a2*cos(q2) - a3*sin(q2) + q3*cos(q2)), -cos(q1)*(sin(q2)*(a2

+ q3) + a3*cos(q2)), cos(q1)*cos(q2)]
[ cos(q1)*(a1 + a2*cos(q2) - a3*sin(q2) + q3*cos(q2)), -sin(q1)*(sin(q2)*(a2 +
q3) + a3*cos(q2)), cos(q2)*sin(q1)]
[ 0, - sin(q1)*(a1*sin(q1) - sin(q1)*(a1 + a2*cos(q2) - a3*sin(q2) + q3*cos(q2))) -
cos(q1)*(a1*cos(q1) - cos(q1)*(a1 + a2*cos(q2) - a3*sin(q2) +
q3*cos(q2))) ,sin(q2)]
Por lo cual, el modelo de velocidad lineal es:
0
3 V =(4.4)

[qp3*cos(q1)*cos(q2) - qp1*sin(q1)*(a1 + a2*cos(q2) - a3*sin(q2) + q3*cos(q2)) - qp2*cos(q1)*(sin(q2)*(a2 +

q3) + a3*cos(q2))]

[qp1*cos(q1)*(a1 + a2*cos(q2) - a3*sin(q2) + q3*cos(q2)) - qp2*sin(q1)*(sin(q2)*(a2 + q3) + a3*cos(q2)) +

qp3*cos(q2)*sin(q1)]

[qp3*sin(q2) + a2*qp2*cos(q2) - a3*qp2*sin(q2) + q3*qp2*cos(q2)]

Modelo diferencial de la velocidad angular

El modelo diferencial de la velocidad angular se obtiene por la ecuación de

manera generalizada:

z n−1(4.5)
0 0 0
n ω =ρ1 ∙ q˙(1 ) ⃗
z 0 + ρ2 ∙ 1 R (t ) q˙(2 ) ⃗
z 1+ …+ ρn ∙ n−1 R (t ) q˙(n )⃗

De una manera más generalizada para unión revoluta y prismática, la ecuación

(4.5) se reescribe de la siguiente manera:

[]
q˙(1)
0
n ω =[ ρ1 ⃗
z0 ρ 2 01 R( t ) ⃗ z n−1 ] q˙(2) (4.6)
z 1 ⋯ ρ n n−10 R( t )⃗
⋮
q˙( n)

En donde,

J ω=¿ [ ρ 1 ⃗
z 0 ρ2 01 R(t ) ⃗ z n−1 ]
z 1 ⋯ ρ n n−10 R(t ) ⃗

es conocido como el jacobiano de velocidad angular para un robot que tiene

uniones prismáticas y revolutas.
Por lo tanto, el modelo diferencial velocidad angular obtenido es:

[ ]
qp 2∗sin (q 1)
0
3 ω = −qp 2∗cos (q 1) (4.7)
qp 1

Matriz jacobiana

La matriz jacobiana J es dada por:

Jv
J= (4.8)
Jω

Sustituyendo la ecuación (4.8):

J v =¿(4.9)

[-sin(q1)*(a1 + a2*cos(q2) - a3*sin(q2) + q3*cos(q2)), -cos(q1)*(sin(q2)*(a2 + q3) +

a3*cos(q2)), cos(q1)*cos(q2)]
[cos(q1)*(a1 + a2*cos(q2) - a3*sin(q2) + q3*cos(q2)), -sin(q1)*(sin(q2)*(a2 + q3) +
a3*cos(q2)), cos(q2)*sin(q1)]
[0, - sin(q1)*(a1*sin(q1) - sin(q1)*(a1 + a2*cos(q2) - a3*sin(q2) + q3*cos(q2))) -
cos(q1)*(a1*cos(q1) - cos(q1)*(a1 + a2*cos(q2) - a3*sin(q2) + q3*cos(q2))), sin(q2)]

Jacobiano de Velocidad Angular - Jw

[ 0, sin(q1), 0]
[ 0, -cos(q1), 0]
[ 1, 0, 0]
 Matrices de transformación de centros de masa
Con la tabla 2 se realiza las matrices de transformación de los centros de masa
usando la ecuación 1.0.

(6.0)

(6.1)

(6.2)

(6.3)

(6.4)

Ecuación Euler Lagrange

La ecuación Euler Lagrange se utiliza para conocer cuál es el torque o fuerza

necesaria de un actuador para producir el movimiento deseado en esta. Para
encontrar esto es necesario obtener el lagrangiano (ecuación 6.5) del eslabón.
 ʆ =E k −E p ( 6.5 )

Las ecuaciones que se utilizan para obtener el lagrangiano son las que se
muestran a continuación:
1 0 T 0 10 T 0 i 0 T 0
E ki = mi ⋅ . vi ⋅ . v i + . ωi ⋅ i R ⋅ . I ⋅ i R ⋅ . ωi ( 6.6 )
2 2

T
E pi =mi ∙ g ∙ pi ( 6.7 )

Eslabón I
La matriz de transformación del primer centro de masa está dada por la ecuación
6.0.

Energía cinética y energía potencial del eslabón 1

Para conocer la energía cinética es necesario conocer las velocidades lineales y
angulares del centro de masa del eslabón I. Calculos realizados en Matlab
Energía cinética del eslabón 1
La ecuación 6.6 se utiliza para obtener la energía cinética en este caso se
realizaron los cálculos en Matlab.
1 0 T 0 10 T 0 0 T 0
E k1 = m1 ⋅ c m V ⋅ c m V + 1ω ⋅ 0 R ⋅ I 1 ⋅ 0 R ⋅ 1ω ( 6.6 .1 )
2 1 1
2

Energía potencial del eslabón 1

Para la energía potencial se requiere el vector gravedad junto con el vector de
posición del cm1, donde se utiliza la ecuación 6.7, realizado en Matlab.
T
E pi =mi ∙ g ∙ pi ( 6.7 )

Eslabón 2
La matriz de transformación del centro de masa del eslabón 2 esta dada por la
ecuación 6.3.
Energía cinética y energía potencial del eslabón 2
Para conocer la energía cinética es necesario conocer las velocidades lineales y
angulares del centro de masa del eslabón.

Energía cinética del eslabón 2

La ecuación 6.6 se utiliza para obtener la energía cinética en este caso se
realizaron los cálculos en Matlab.
1 0 T 0 10 T 0 0 T 0
E k2 = m2 ⋅ c m V ⋅ c m V + 2ω ⋅ 1 R ⋅ I 1 ⋅ 1R ⋅ 2ω (6.6 .2)
2 2 2
2

Ek2 =

(Iyy2*qp1^2)/2 + (Izz2*qp2^2)/2 + (Xcm2^2*m2*qp2^2)/2 +

(Ycm2^2*m2*qp1^2)/2 + (a1^2*m2*qp1^2)/2 +
(Xcm2^2*m2*qp1^2*cos(q2)^2)/2 + Xcm2*a1*m2*qp1^2*cos(q2) -
Xcm2*Ycm2*m2*qp1*qp2*sin(q2)

Energía potencial del eslabón 2

Para la energía potencial se requiere el vector gravedad junto con el vector de
posición del cm2, donde se utiliza la ecuación 6.7, realizado en Matlab.
T
E pi =mi ∙ g ∙ pi ( 6.7 )

Eslabón 3
La matriz de tranformación del centro de masa del eslabón 3 esta dada por la
ecuación 6.4.

Energía cinética y energía potencial del eslabón 3

Para conocer la energía cinética es necesario conocer la velocidades lineales y
angulares del centro de masa del eslabón.

Vcm3 =
qp3*cos(q1)*cos(q2) - qp2*(cos(q1)*sin(q2)*(a2 - cm3 + q3) +
a3*cos(q1)*cos(q2)) - qp1*(a1*sin(q1) + cos(q2)*sin(q1)*(a2 - cm3 + q3) -
a3*sin(q1)*sin(q2))
qp1*(a1*cos(q1) - a3*cos(q1)*sin(q2) + cos(q1)*cos(q2)*(a2 - cm3 + q3)) -
qp2*(sin(q1)*sin(q2)*(a2 - cm3 + q3) + a3*cos(q2)*sin(q1)) +
qp3*cos(q2)*sin(q1)
 qp3*sin(q2) + qp2*(cos(q2)*(a2 - cm3 + q3) - a3*sin(q2))

Energía cinética del eslabón 3

La ecuación 6.6 se utiliza para obtener la energía cinética en este caso se
realizaron los cálculos en Matlab.
1 0 T 0 10 T 0 0 T 0
E k3 = m2 ⋅ c m V ⋅ c m V + 2ω ⋅ 1R ⋅ I 1 ⋅ 1R ⋅ 2ω (6.6 .3)
2 2 2
2

Ek3 =

(Iyy3*qp2^2)/2 + (Izz3*qp1^2)/2 + (m3*qp3^2)/2 + (a1^2*m3*qp1^2)/2

+ (a2^2*m3*qp2^2)/2 + (a3^2*m3*qp1^2)/2 + (a3^2*m3*qp2^2)/2 +
(cm3^2*m3*qp2^2)/2 + (m3*q3^2*qp2^2)/2 +
(Ixx3*qp1^2*cos(q2)^2)/2 - (Izz3*qp1^2*cos(q2)^2)/2 - a3*m3*qp2*qp3 -
a2*cm3*m3*qp2^2 + a2*m3*q3*qp2^2 - cm3*m3*q3*qp2^2 +
(a2^2*m3*qp1^2*cos(q2)^2)/2 - (a3^2*m3*qp1^2*cos(q2)^2)/2 +
(cm3^2*m3*qp1^2*cos(q2)^2)/2 + (m3*q3^2*qp1^2*cos(q2)^2)/2 +
a1*a2*m3*qp1^2*cos(q2) - a1*cm3*m3*qp1^2*cos(q2) -
a1*a3*m3*qp1^2*sin(q2) + a1*m3*q3*qp1^2*cos(q2) -
a2*cm3*m3*qp1^2*cos(q2)^2 - (a2*a3*m3*qp1^2*sin(2*q2))/2 +
a2*m3*q3*qp1^2*cos(q2)^2 + (a3*cm3*m3*qp1^2*sin(2*q2))/2 -
cm3*m3*q3*qp1^2*cos(q2)^2 - (a3*m3*q3*qp1^2*sin(2*q2))/2

Energía potencial del eslabón 3

Para la energía potencial se requiere el vector gravedad junto con el vector de
posición del cm3, donde se utiliza la ecuación 6.7, realizado en Matlab.
 T
E pi =mi ∙ g ∙ pi ( 6.7 )

Lagrangiano
Para obtener el lagrangiano se utilizan las energías cinéticas y potenciales de los
centros de masa del robot.
ʆ =Ek t −Ept ( 6.8 )

2 2 2 2 2 2 2
L=(qp 1 ∗(m1∗cm 1 + Izz 1))/2+( Iyy 2∗qp 1 )/ 2+( Iyy 3∗qp 2 )/ 2+( Izz 2∗qp 2 )/ 2+( Izz 3∗qp 1 )/ 2+( m3∗qp 3 )/2

En este caso la ecuación queda para el lagrangiano es la anterior 6.9.

Vector τ
Una vez obtenido el lagrangiano podemos obtener el vector tao que nos muestra
cuales son las fuerzas torques necesarios para cada eslabón.
Los elementos de este vector se pueden obtener con la ecuación 7.0.
d ∂ʆ ∂ʆ
Tao i= − ( 7.0 )
dt ∂ q̇ i ∂ qi
%Vector Tao
for i=1:3
dL_dQp=simplify(diff(L,Qp(i)));
dt_dL_dQp=diff(dL_dQp,qp1)*qpp1 + diff(dL_dQp,qp2)*qpp2 + diff(dL_dQp,qp3)*qpp3
+ diff(dL_dQp,q1)*qp1 + diff(dL_dQp,q2)*qp2 + diff(dL_dQp,q3)*qp3;
dL_dq=simplify(diff(L,Q(i)));
Tao(i)=simplify(dt_dL_dQp-dL_dq);
end

Tao =

[ Iyy2*qpp1 + Izz1*qpp1 + Izz3*qpp1 + cm1^2*m1*qpp1 + Ixx3*qpp1*cos(q2)^2 -

Izz3*qpp1*cos(q2)^2 + Ycm2^2*m2*qpp1 + a1^2*m2*qpp1 + a1^2*m3*qpp1 +
a3^2*m3*qpp1 + Xcm2^2*m2*qpp1*cos(q2)^2 + a2^2*m3*qpp1*cos(q2)^2 -
a3^2*m3*qpp1*cos(q2)^2 + cm3^2*m3*qpp1*cos(q2)^2 + m3*q3^2*qpp1*cos(q2)^2
- Ixx3*qp1*qp2*sin(2*q2) + Izz3*qp1*qp2*sin(2*q2) - Xcm2*Ycm2*m2*qp2^2*cos(q2)
- 2*a2*cm3*m3*qpp1*cos(q2)^2 - a2*a3*m3*qpp1*sin(2*q2) +
2*a2*m3*q3*qpp1*cos(q2)^2 + 2*a2*m3*qp1*qp3*cos(q2)^2 +
a3*cm3*m3*qpp1*sin(2*q2) - 2*cm3*m3*q3*qpp1*cos(q2)^2 -
2*cm3*m3*qp1*qp3*cos(q2)^2 - a3*m3*q3*qpp1*sin(2*q2) -
a3*m3*qp1*qp3*sin(2*q2) + 2*m3*q3*qp1*qp3*cos(q2)^2 + 2*a2*a3*m3*qp1*qp2 -
2*a3*cm3*m3*qp1*qp2 + 2*a3*m3*q3*qp1*qp2 - Xcm2^2*m2*qp1*qp2*sin(2*q2) -
a2^2*m3*qp1*qp2*sin(2*q2) + a3^2*m3*qp1*qp2*sin(2*q2) -
cm3^2*m3*qp1*qp2*sin(2*q2) - m3*q3^2*qp1*qp2*sin(2*q2) +
2*Xcm2*a1*m2*qpp1*cos(q2) - Xcm2*Ycm2*m2*qpp2*sin(q2) +
2*a1*a2*m3*qpp1*cos(q2) - 2*a1*cm3*m3*qpp1*cos(q2) -
2*a1*a3*m3*qpp1*sin(q2) + 2*a1*m3*q3*qpp1*cos(q2) +
2*a1*m3*qp1*qp3*cos(q2) - 2*a1*a3*m3*qp1*qp2*cos(q2) -
2*Xcm2*a1*m2*qp1*qp2*sin(q2) - 2*a1*a2*m3*qp1*qp2*sin(q2) +
2*a1*cm3*m3*qp1*qp2*sin(q2) - 2*a1*m3*q3*qp1*qp2*sin(q2) -
4*a2*a3*m3*qp1*qp2*cos(q2)^2 + 4*a3*cm3*m3*qp1*qp2*cos(q2)^2 -
4*a3*m3*q3*qp1*qp2*cos(q2)^2 + 2*a2*cm3*m3*qp1*qp2*sin(2*q2) -
2*a2*m3*q3*qp1*qp2*sin(2*q2) + 2*cm3*m3*q3*qp1*qp2*sin(2*q2), Iyy3*qpp2 +
Izz2*qpp2 + cm3^2*m3*qpp2 + m3*q3^2*qpp2 - a3*m3*qpp3 +
(Ixx3*qp1^2*sin(2*q2))/2 - (Izz3*qp1^2*sin(2*q2))/2 + Xcm2^2*m2*qpp2 +
a2^2*m3*qpp2 + a3^2*m3*qpp2 - 2*a2*cm3*m3*qpp2 + 2*a2*m3*q3*qpp2 +
2*a2*m3*qp2*qp3 - 2*cm3*m3*q3*qpp2 - 2*cm3*m3*qp2*qp3 +
2*m3*q3*qp2*qp3 + Xcm2*g*m2*cos(q2) + a2*g*m3*cos(q2) - cm3*g*m3*cos(q2) -
a3*g*m3*sin(q2) + g*m3*q3*cos(q2) + (Xcm2^2*m2*qp1^2*sin(2*q2))/2 +
(a2^2*m3*qp1^2*sin(2*q2))/2 - (a3^2*m3*qp1^2*sin(2*q2))/2 +
(cm3^2*m3*qp1^2*sin(2*q2))/2 + (m3*q3^2*qp1^2*sin(2*q2))/2 +
a1*a3*m3*qp1^2*cos(q2) + Xcm2*a1*m2*qp1^2*sin(q2) + a1*a2*m3*qp1^2*sin(q2)
- a1*cm3*m3*qp1^2*sin(q2) + a1*m3*q3*qp1^2*sin(q2) +
a2*a3*m3*qp1^2*cos(2*q2) - a3*cm3*m3*qp1^2*cos(2*q2) +
a3*m3*q3*qp1^2*cos(2*q2) - a2*cm3*m3*qp1^2*sin(2*q2) +
a2*m3*q3*qp1^2*sin(2*q2) - cm3*m3*q3*qp1^2*sin(2*q2) -
Xcm2*Ycm2*m2*qpp1*sin(q2), -m3*(a3*qpp2 - qpp3 + a2*qp2^2 - cm3*qp2^2 +
q3*qp2^2 - g*sin(q2) + a1*qp1^2*cos(q2) + a2*qp1^2*cos(q2)^2 -
cm3*qp1^2*cos(q2)^2 - (a3*qp1^2*sin(2*q2))/2 + q3*qp1^2*cos(q2)^2)]

TaoA =

Iyy2*qpp1 + Izz1*qpp1 + Izz3*qpp1 + cm1^2*m1*qpp1 + Ixx3*qpp1*cos(q2)^2 - Izz3*qpp1*cos(q2)^2 +

Ycm2^2*m2*qpp1 + a1^2*m2*qpp1 + a1^2*m3*qpp1 + a3^2*m3*qpp1 + Xcm2^2*m2*qpp1*cos(q2)^2 +
a2^2*m3*qpp1*cos(q2)^2 - a3^2*m3*qpp1*cos(q2)^2 + cm3^2*m3*qpp1*cos(q2)^2 + m3*q3^2*qpp1*cos(q2)^2 -
Ixx3*qp1*qp2*sin(2*q2) + Izz3*qp1*qp2*sin(2*q2) - Xcm2*Ycm2*m2*qp2^2*cos(q2) - 2*a2*cm3*m3*qpp1*cos(q2)^2 -
a2*a3*m3*qpp1*sin(2*q2) + 2*a2*m3*q3*qpp1*cos(q2)^2 + 2*a2*m3*qp1*qp3*cos(q2)^2 +
a3*cm3*m3*qpp1*sin(2*q2) - 2*cm3*m3*q3*qpp1*cos(q2)^2 - 2*cm3*m3*qp1*qp3*cos(q2)^2 -
a3*m3*q3*qpp1*sin(2*q2) - a3*m3*qp1*qp3*sin(2*q2) + 2*m3*q3*qp1*qp3*cos(q2)^2 + 2*a2*a3*m3*qp1*qp2 -
2*a3*cm3*m3*qp1*qp2 + 2*a3*m3*q3*qp1*qp2 - Xcm2^2*m2*qp1*qp2*sin(2*q2) - a2^2*m3*qp1*qp2*sin(2*q2) +
a3^2*m3*qp1*qp2*sin(2*q2) - cm3^2*m3*qp1*qp2*sin(2*q2) - m3*q3^2*qp1*qp2*sin(2*q2) +
2*Xcm2*a1*m2*qpp1*cos(q2) - Xcm2*Ycm2*m2*qpp2*sin(q2) + 2*a1*a2*m3*qpp1*cos(q2) -
2*a1*cm3*m3*qpp1*cos(q2) - 2*a1*a3*m3*qpp1*sin(q2) + 2*a1*m3*q3*qpp1*cos(q2) + 2*a1*m3*qp1*qp3*cos(q2) -
2*a1*a3*m3*qp1*qp2*cos(q2) - 2*Xcm2*a1*m2*qp1*qp2*sin(q2) - 2*a1*a2*m3*qp1*qp2*sin(q2) +
2*a1*cm3*m3*qp1*qp2*sin(q2) - 2*a1*m3*q3*qp1*qp2*sin(q2) - 4*a2*a3*m3*qp1*qp2*cos(q2)^2 +
4*a3*cm3*m3*qp1*qp2*cos(q2)^2 - 4*a3*m3*q3*qp1*qp2*cos(q2)^2 + 2*a2*cm3*m3*qp1*qp2*sin(2*q2) -
2*a2*m3*q3*qp1*qp2*sin(2*q2) + 2*cm3*m3*q3*qp1*qp2*sin(2*q2)

Iyy3*qpp2 + Izz2*qpp2 + cm3^2*m3*qpp2 + m3*q3^2*qpp2 - a3*m3*qpp3 + (Ixx3*qp1^2*sin(2*q2))/2 -

(Izz3*qp1^2*sin(2*q2))/2 + Xcm2^2*m2*qpp2 + a2^2*m3*qpp2 + a3^2*m3*qpp2 - 2*a2*cm3*m3*qpp2 +
2*a2*m3*q3*qpp2 + 2*a2*m3*qp2*qp3 - 2*cm3*m3*q3*qpp2 - 2*cm3*m3*qp2*qp3 + 2*m3*q3*qp2*qp3 +
Xcm2*g*m2*cos(q2) + a2*g*m3*cos(q2) - cm3*g*m3*cos(q2) - a3*g*m3*sin(q2) + g*m3*q3*cos(q2) +
(Xcm2^2*m2*qp1^2*sin(2*q2))/2 + (a2^2*m3*qp1^2*sin(2*q2))/2 - (a3^2*m3*qp1^2*sin(2*q2))/2 +
(cm3^2*m3*qp1^2*sin(2*q2))/2 + (m3*q3^2*qp1^2*sin(2*q2))/2 + a1*a3*m3*qp1^2*cos(q2) +
Xcm2*a1*m2*qp1^2*sin(q2) + a1*a2*m3*qp1^2*sin(q2) - a1*cm3*m3*qp1^2*sin(q2) + a1*m3*q3*qp1^2*sin(q2) +
a2*a3*m3*qp1^2*cos(2*q2) - a3*cm3*m3*qp1^2*cos(2*q2) + a3*m3*q3*qp1^2*cos(2*q2) -
a2*cm3*m3*qp1^2*sin(2*q2) + a2*m3*q3*qp1^2*sin(2*q2) - cm3*m3*q3*qp1^2*sin(2*q2) -
Xcm2*Ycm2*m2*qpp1*sin(q2)

-m3*(a3*qpp2 - qpp3 + a2*qp2^2 - cm3*qp2^2 + q3*qp2^2 - g*sin(q2) + a1*qp1^2*cos(q2) + a2*qp1^2*cos(q2)^2 -

cm3*qp1^2*cos(q2)^2 - (a3*qp1^2*sin(2*q2))/2 + q3*qp1^2*cos(q2)^2)

TaoB =

Iyy2*qpp1 + Izz1*qpp1 + Izz3*qpp1 + cm1^2*m1*qpp1 + Ixx3*qpp1*cos(q2)^2 - Izz3*qpp1*cos(q2)^2 +

Ycm2^2*m2*qpp1 + a1^2*m2*qpp1 + a1^2*m3*qpp1 + a3^2*m3*qpp1 + Xcm2^2*m2*qpp1*cos(q2)^2 +
a2^2*m3*qpp1*cos(q2)^2 - a3^2*m3*qpp1*cos(q2)^2 + cm3^2*m3*qpp1*cos(q2)^2 + m3*q3^2*qpp1*cos(q2)^2 -
Ixx3*qp1*qp2*sin(2*q2) + Izz3*qp1*qp2*sin(2*q2) - Xcm2*Ycm2*m2*qp2^2*cos(q2) - 2*a2*cm3*m3*qpp1*cos(q2)^2 -
a2*a3*m3*qpp1*sin(2*q2) + 2*a2*m3*q3*qpp1*cos(q2)^2 + 2*a2*m3*qp1*qp3*cos(q2)^2 +
a3*cm3*m3*qpp1*sin(2*q2) - 2*cm3*m3*q3*qpp1*cos(q2)^2 - 2*cm3*m3*qp1*qp3*cos(q2)^2 -
a3*m3*q3*qpp1*sin(2*q2) - a3*m3*qp1*qp3*sin(2*q2) + 2*m3*q3*qp1*qp3*cos(q2)^2 + 2*a2*a3*m3*qp1*qp2 -
2*a3*cm3*m3*qp1*qp2 + 2*a3*m3*q3*qp1*qp2 - Xcm2^2*m2*qp1*qp2*sin(2*q2) - a2^2*m3*qp1*qp2*sin(2*q2) +
a3^2*m3*qp1*qp2*sin(2*q2) - cm3^2*m3*qp1*qp2*sin(2*q2) - m3*q3^2*qp1*qp2*sin(2*q2) +
2*Xcm2*a1*m2*qpp1*cos(q2) - Xcm2*Ycm2*m2*qpp2*sin(q2) + 2*a1*a2*m3*qpp1*cos(q2) -
2*a1*cm3*m3*qpp1*cos(q2) - 2*a1*a3*m3*qpp1*sin(q2) + 2*a1*m3*q3*qpp1*cos(q2) + 2*a1*m3*qp1*qp3*cos(q2) -
2*a1*a3*m3*qp1*qp2*cos(q2) - 2*Xcm2*a1*m2*qp1*qp2*sin(q2) - 2*a1*a2*m3*qp1*qp2*sin(q2) +
2*a1*cm3*m3*qp1*qp2*sin(q2) - 2*a1*m3*q3*qp1*qp2*sin(q2) - 4*a2*a3*m3*qp1*qp2*cos(q2)^2 +
4*a3*cm3*m3*qp1*qp2*cos(q2)^2 - 4*a3*m3*q3*qp1*qp2*cos(q2)^2 + 2*a2*cm3*m3*qp1*qp2*sin(2*q2) -
2*a2*m3*q3*qp1*qp2*sin(2*q2) + 2*cm3*m3*q3*qp1*qp2*sin(2*q2)

Iyy3*qpp2 + Izz2*qpp2 + cm3^2*m3*qpp2 + m3*q3^2*qpp2 - a3*m3*qpp3 + (Ixx3*qp1^2*sin(2*q2))/2 -

(Izz3*qp1^2*sin(2*q2))/2 + Xcm2^2*m2*qpp2 + a2^2*m3*qpp2 + a3^2*m3*qpp2 - 2*a2*cm3*m3*qpp2 +
2*a2*m3*q3*qpp2 + 2*a2*m3*qp2*qp3 - 2*cm3*m3*q3*qpp2 - 2*cm3*m3*qp2*qp3 + 2*m3*q3*qp2*qp3 +
Xcm2*g*m2*cos(q2) + a2*g*m3*cos(q2) - cm3*g*m3*cos(q2) - a3*g*m3*sin(q2) + g*m3*q3*cos(q2) +
(Xcm2^2*m2*qp1^2*sin(2*q2))/2 + (a2^2*m3*qp1^2*sin(2*q2))/2 - (a3^2*m3*qp1^2*sin(2*q2))/2 +
(cm3^2*m3*qp1^2*sin(2*q2))/2 + (m3*q3^2*qp1^2*sin(2*q2))/2 + a1*a3*m3*qp1^2*cos(q2) +
Xcm2*a1*m2*qp1^2*sin(q2) + a1*a2*m3*qp1^2*sin(q2) - a1*cm3*m3*qp1^2*sin(q2) + a1*m3*q3*qp1^2*sin(q2) +
a2*a3*m3*qp1^2*cos(2*q2) - a3*cm3*m3*qp1^2*cos(2*q2) + a3*m3*q3*qp1^2*cos(2*q2) -
a2*cm3*m3*qp1^2*sin(2*q2) + a2*m3*q3*qp1^2*sin(2*q2) - cm3*m3*q3*qp1^2*sin(2*q2) -
Xcm2*Ycm2*m2*qpp1*sin(q2)

- m3*cos(q2)*(a1 + a2*cos(q2) - cm3*cos(q2) - a3*sin(q2) + q3*cos(q2))*qp1^2 - m3*(a2 - cm3 + q3)*qp2^2 + m3*qpp3 +

g*m3*sin(q2) - a3*m3*qpp2

Obtención de las matrices D, C y G

Las matrices D, C y G son de utilidad a la hora de comprobar y simular el robot.

 τ =D (q̇ ) q̈+C (q̇ , q ) q̇+ G¿ 7.4)

[][ ][ ] [ ][ ] [ ]
f1 D11 D12 D13 q¨1 C 11 C12 C 13 q̇ 1 G1
f2 = D21 D22 D23 q¨2 + C 21 C22 C 23 q̇ 2 + G2 ( 7.5 )
τ3 D31 D32 D33 q¨3 C 31 C32 C 33 q̇ 3 G3

Tao i=( Di 1 ) q̈1 + ( Di 2 ) q¨2+ ( Di3 ) q¨3 + ( C i1 ) q˙1+ ( Ci 2 ) q̇2 + ( C i 3 ) q̇3 +Gi ( 7.6 )

Matriz D
La matriz D es una matriz única y simétrica la cual nos puede servir para
comprobar el robot.

[ ][ ]
D11 D12 D13 q̈ 1
D21 D22 D23 q̈ 2 ( 7.7 )
D31 D32 D33 q̈3
2
D11 =(Iyy 2+ Izz 1+m 2∗(a 1∗cos (q 1)+Ycm2∗sin(q 1)+ Xcm 2∗cos (q 1)∗cos(q 2)) +m2∗( a 1∗sin (q 1)−Ycm2∗co
D12=−Xcm 2∗Ycm 2∗m 2∗sin (q 2) ( 7.7 .2 ) D13=0 ( 7.7 .3 ) D21=−Xcm 2∗Ycm 2∗m 2∗sin (q 2) ( 7.7 .4 )
2 2 2 2 2
D22=m 2∗Xcm 2 + m3∗a 2 −2∗m3∗a 2∗cm3+ 2∗m 3∗a 2∗q 3+ m3∗a 3 +m 3∗cm3 −2∗m 3∗cm3∗q 3+m 3∗q 3
D23=−a3∗m 3 ( 7.7.6 ) D31=0 ( 7.7 .7 ) D32=−a 3∗m 3 ( 7.7 .8 ) D33=m 3 (7.7 .9 )

Matriz C

[ ][ ]
C11 C 12 C 13 q̇1
C21 C 22 C 23 q̇2 ( 8.0 )
C31 C 32 C 33 q̇3

C 11=m 3∗qp 3∗cos(q 2)∗(a1+ a 2∗cos (q 2)−cm3∗cos (q 2)−a 3∗sin(q 2)+ q 3∗cos (q 2))−qp 2∗((Ixx 3∗sin(2∗q 2
2 2
C 12=−qp 1∗(( Ixx 3∗sin(2∗q 2))/2−(Izz 3∗sin( 2∗q 2))/2+(Xcm 2 ∗m 2∗sin(2∗q 2))/2+(a 2 ∗m3∗sin(2∗q 2))/2−
C 13=m3∗qp 1∗cos(q 2)∗(a 1+ a 2∗cos(q 2)−cm3∗cos (q 2)−a3∗sin (q 2)+ q 3∗cos( q 2)) ( 8.1.3 )
2 2
C 21=qp 1∗((Ixx 3∗sin(2∗q 2)) /2−(Izz 3∗sin(2∗q 2))/2+(Xcm 2 ∗m2∗sin( 2∗q 2))/2+(a 2 ∗m3∗sin(2∗q 2))/2−(
C 22=m3∗qp 3∗(a 2−cm 3+q 3) ( 8.1.5 )C 23=m3∗qp 2∗(a2−cm 3+q 3) ( 8.16 )
C 31=−m3∗qp 1∗cos(q 2)∗(a 1+ a 2∗cos (q 2)−cm3∗cos (q 2)−a3∗sin(q 2)+ q 3∗cos(q 2)) ( 8.1.7 )
C 32=−m3∗qp 2∗(a2−cm 3+q 3) ( 8.1.8 ) C 33=0 ( 8.1.9 )

Vector G

[]
G1
G2 ( 8.2 )
G3

G1=0 (8.2 .3)G2=g∗m3∗(cos(q 2)∗(a 2−cm 3+q 3)−a 3∗sin(q 2))+ Xcm 2∗g∗m 2∗cos (q 2)(8.2 .4)
G3=g∗m3∗sin (q 2) ( 8.2 .5 )

Simulink
Figura 2: Tao=0

Figura 3: Tao=1
 Figura 4: Tao=0 ; Friccion=1

CONTROL PD

Figura 5: DIAGRAMA DE CONTROL PD

Figura 6: CONTROLADOR PD

Figura 7: GRAFICA 3D
 Figura 8: GRAFICA 2D

Como se observa en la figura 8 q1 y q2 son igual a 90 grados por lo que se

demuestra en la gráfica 2D que se mantienen en el 1.5 que vienen siendo la línea
azul y la línea amarilla y q3 que viene siendo la línea naranja hace su movimiento
y después de mantiene en 1 como se muestra en la imagen.
En la figura 7 en la gráfica 3D se puede ver que inicia la curva por el movimiento
del robot debido a que son las revolutas que van en el eje x, finalizando en el eje
z que va en una trayectoria continua de un lado a otro porque es un prismático.
 Figura 9: MODELO DINAMICO
LINEALIDAD EN LOS PARAMETROS
Regresor ( Y ( q , q̇ , q̈ ))

[ ]
q̈1 ¿ 2 ¿ 3 ¿ 4 ¿ 5 ¿ 6 ¿ 7 ¿ 8 ¿ 9 ¿10 0 0
Y ( q , q̇ , q̈ )= 0 ¿ 14 ¿ 15 q̈ 1 s2 ¿ 17 ¿ 18 ¿ 19 ¿ 20 ¿ 21 ¿ 22 q̈2 g c 2 (9.0)
0 0 ¿ 27 0 0 ¿ 30 ¿ 31 0 0 ¿ 34 0 0
 2
¿ 2= q̈1 c 2 −q̇1 q̇2 s 22
2 2 2 2
¿ 3=q3 q̈1 c 2 +2 q 3 q̇1 q̇3 c 2 −q3 q̇ 1 q̇ 2 s22
2
¿ 4= q̇2 c2 + q̈ 2 s 2

2 1
¿ 5= q̇1 q̇2−2 q̇ 1 q̇2 c 2 − q̈1 s 22
2
2 2
¿ 6=q3 q̈ 1 c 2 + q̇ 1 q̇ 3 c 2 −q 3 q̇1 q̇2 s 22
2
¿ 7=q3 q̈ 1 s22 + q̇ 1 q̇ 3 s22−2 q 3 q̇ 1 q̇2 +4 q3 q̇1 q̇ 2 c 2

¿ 8=q̈1 c 2−q̇ 1 q̇ 2 c2

¿ 9=q̈1 s 2 + q̇ 1 q̇ 2 c 2

¿ 10=q3 q̈1 c 2 + q̇1 q̇ 3 c 2−q3 q̇1 q̇2 s 2

1
¿ 14= q̈1 s 22
2

2 1 2 2
¿ 15=q3 q̈2 +2 q 3 q̇ 2 q̇ 3 + g q3 c 2+ q3 q̇1 s 22
2

1 2
¿ 17= q̇1 c 22
2

1 1 2
¿ 18=q3 q̈ 2+ q̇2 q̇3 + g c 2+ q 3 q̇ 1 s22
2 2
2
¿ 19= q̈3 + g s 2−q 3 q̇1 c22

1 2
¿ 20= q̇1 s 2
2

−1 2
¿ 21= q̇ c
2 1 2

1 2
¿ 22= q 3 q̇1 s 2
2
2 2 2
¿ 27=q̈3 −q3 q̇ 2 + g s2−q3 q̇1 c 2

−1 2 1 2 2
¿ 30= q̇ − q̇ c
2 2 2 1 2
 2
¿ 31= q̈2−q̇ 1 c 2 s 2

−1 2
¿ 34= q̇ c
2 1 2

Vector de Parámetros Lineales (θ )

[ ]
2 2 2 2 2
I yy2 + I zz1 + I zz3 +cm1 m1 +Y cm2 m2+ a1 m2 +a 1 m3+ a3 m3
2 2 2 2
I xx3−I zz3 + X cm2 m2 + a2 m3−a3 m3 +cm3 m3 −2 a2 cm3 m3
m3
−X cm Y cm m22 2

2 a 2 a 3 m3−2 a 3 cm3 m3
2 a 2 m3−2 cm3 m3
θ= (9.1)
−a 3 m3
2 X cm a 1 m2 +2 a1 a2 m3 −2 a1 cm3 m3
2

−2 a1 a3 m3
2 a1 m 3
2 2 2 2
I yy3 + I zz2 +cm3 m3 + X cm2 m2 +a2 m3+ a3 m3 −2 a2 cm3 m3
X cm2 m2

COMPROBACION EN MATLAB
%REGRESOR Y VECTOR DE PARAMETROS LINEALES
R=[qpp1,qpp1*cos(q2)^2 - qp1*qp2*sin(2*q2),q3^2*qpp1*cos(q2)^2 +
2*q3*qp1*qp3*cos(q2)^2 - q3^2*qp1*qp2*sin(2*q2),qp2^2*cos(q2) +
qpp2*sin(q2),(-1/2)*qpp1*sin(2*q2) + qp1*qp2 -
2*qp1*qp2*cos(q2)^2,q3*qpp1*cos(q2)^2 + qp1*qp3*cos(q2)^2 -
q3*qp1*qp2*sin(2*q2),q3*qpp1*sin(2*q2) + qp1*qp3*sin(2*q2) - 2*q3*qp1*qp2
+ 4*q3*qp1*qp2*cos(q2)^2,qpp1*cos(q2) - qp1*qp2*sin(q2),qpp1*sin(q2) +
qp1*qp2*cos(q2),q3*qpp1*cos(q2) + qp1*qp3*cos(q2) -
q3*qp1*qp2*sin(q2),0,0;
0,(1/2)*qp1^2*sin(2*q2), q3^2*qpp2 + 2*q3*qp2*qp3 + g*q3*cos(q2) +
(1/2)*q3^2*qp1^2*sin(2*q2),qpp1*sin(q2),(1/2)*qp1^2*cos(2*q2),q3*qpp2 +
qp2*qp3 + (1/2)*g*cos(q2) + (1/2)*q3*qp1^2*sin(2*q2),qpp3 + g*sin(q2) -
q3*qp1^2*cos(2*q2),(1/2)*qp1^2*sin(q2),(-1/2)*qp1^2*cos(q2),
(1/2)*q3*qp1^2*sin(q2),qpp2,g*cos(q2);
0,0,qpp3 - q3*qp2^2 + g*sin(q2) - q3*qp1^2*cos(q2)^2,0,0,(-1/2)*qp2^2
+ (-1/2)*qp1^2*cos(q2)^2,qpp2 - qp1^2*cos(q2)*sin(q2),0,0,(-
1/2)*qp1^2*cos(q2),0,0]
V=[Iyy2 + Izz1 + Izz3 + cm1^2*m1 + Ycm2^2*m2 + a1^2*m2 + a1^2*m3 +
a3^2*m3;Ixx3 - Izz3 + Xcm2^2*m2 + a2^2*m3 - a3^2*m3 + cm3^2*m3 -
2*a2*cm3*m3;m3;-Xcm2*Ycm2*m2;2*a2*a3*m3 - 2*a3*cm3*m3;2*a2*m3 -
2*cm3*m3;-a3*m3;2*Xcm2*a1*m2 + 2*a1*a2*m3 - 2*a1*cm3*m3;-
2*a1*a3*m3;2*a1*m3;Iyy3 + Izz2 + cm3^2*m3 + Xcm2^2*m2 + a2^2*m3 + a3^2*m3
- 2*a2*cm3*m3;Xcm2*m2]
TaoC=simplify(R*V)

Figura 10: Algoritmo en Matlab donde se demuestra el cálculo.

Figura 11: Comprobación de la resta de los Tao.

Se puede observar en las figuras anteriores el cálculo que se

utilizó para la resta de los Taos en el cual su resultado fue
igual a 0 por lo que se demuestra que el regresor se realizó
de una manera correcta