SISTEMA DE CONTROL DE TORQUE DE LOS ACTUADORES DEL BRAZO ROBÓTICO

Se considera el brazo robótico como un manipulador de dos grados de libertad. Los péndulos
se mueven en el plano XY como se observa en la Figura 1. Las longitudes de los péndulos son
L2=0.60 m y L3=0.40 m. Se considera las masas de los péndulos concentradas en sus extremos
como m2=40 kg y m3= 15 kg.

Figura 1. Movimiento del brazo del robot en el plano XY

El modelo dinámico de este sistema se obtiene a través de las siguientes definiciones:

Posiciones:

𝒓𝟏 = (𝑥1, 𝑦1); 𝒓𝟐 = (𝑥2, 𝑦2)

Velocidad de las masas concentradas:

𝒗 = 𝒓;̇
̇
𝒗𝟏 = [ 𝐿2𝑐𝑜𝑠𝜃1. 𝜃1 ] ; (𝟏)
−𝐿2𝑠𝑒𝑛𝜃1. 𝜃1̇
̇
𝐿2𝑐𝑜𝑠𝜃1. 𝜃1̇ + 𝐿3𝑐𝑜𝑠(𝜃1 + 𝜃2). (𝜃1̇ + 𝜃2)
𝒗𝟐 = [ ]; (𝟐)
−𝐿2𝑠𝑒𝑛𝜃1. 𝜃1̇ − 𝐿3𝑠𝑒𝑛(𝜃1 + 𝜃2). (𝜃1̇ + 𝜃2) ̇
1 1
Energía cinética: 𝐾 = 2 𝑚𝑣 2 + 2 𝐼𝜃̇ 2

Momentos de inercia I:

𝐼1 = 𝑚2 ∗ 𝐿22 ; (3)

𝐼2 = 𝑚3 ∗ 𝐿32 ; (4)
La energía cinética para brazo con (1), (2), (3) y (4) es:
1 1
𝐾1 = 𝑚2 ∗ 𝑣12 + 𝐼1 ∗ 𝜃̇ 12
2 2
1 1
𝐾1 = . 𝑚2. 𝐿22 . 𝜃̇ 12 + 𝑚2. 𝐿22 . 𝜃̇ 12 ; (5)
2 2
𝐾1 = 14,4𝜃̇12

1 1 2
𝐾2 = 𝑚2 ∗ 𝑣22 + 𝐼2(𝜃1̇ + 𝜃̇ 2) ;
2 2

1 1 2
𝐾2 = 𝑚2 ∗ 𝑣22 + 𝑚3. 𝐿32 . (𝜃1̇ + 𝜃̇ 2) ;
2 2
𝑚2 2
𝐾2 = (𝐿22 𝜃1̇2 + 𝐿32 (𝜃1̇ + 𝜃̇ 2) + 2𝐿2𝐿3. (𝜃1̇2 + 𝜃1̇𝜃̇2)𝑐𝑜𝑠𝜃2
2
1 2
+ 𝑚3. 𝐿32 . (𝜃1̇ + 𝜃̇ 2) ; (6)
2
2
𝐾2 = 7,2. 𝜃1̇2 + 4,4(𝜃1̇ + 𝜃̇ 2) + 9,6(𝜃1̇2 + 𝜃1̇𝜃̇ 2)𝑐𝑜𝑠𝜃2

𝐾 = 𝐾1 + 𝐾2
2
𝐾 = 21,6𝜃1̇2 + 4,4(𝜃1̇ + 𝜃̇ 2) + 9,6(𝜃1̇2 + 𝜃1̇𝜃̇2)𝑐𝑜𝑠𝜃2; (7)

Energía potencial: U=mgh

𝑈1 = 𝑚2. 𝑔. 𝐿2. 𝑐𝑜𝑠𝜃1

𝑈1 = 235,2𝑐𝑜𝑠𝜃1; (8)

𝑈2 = 𝑚3. 𝑔. (𝐿2𝑐𝑜𝑠𝜃1 + 𝐿3 cos(𝜃1 + 𝜃2));

𝑈2 = 88,2𝑐𝑜𝑠𝜃1 + 58,8 cos(𝜃1 + 𝜃2) ; (9)

𝑈 = 𝑈1 + 𝑈2
𝑈 = 232,4𝑐𝑜𝑠𝜃1 + 58,8 cos(𝜃1 + 𝜃2) ; (10)
Lagrangiano: L=K-U
2
𝐿 = 21,6𝜃1̇2 + 4,4(𝜃1̇ + 𝜃̇ 2) + 9,6(𝜃1̇2 + 𝜃1̇𝜃̇ 2)𝑐𝑜𝑠𝜃2 − 232,4𝑐𝑜𝑠𝜃1
− 58,8 cos(𝜃1 + 𝜃2) ; (11)
Ecuación de Euler Lagrange:
𝑑 𝜕𝐿 𝜕𝐿
( )− = 𝜏1
𝑑𝑡 𝜕𝜃1̇ 𝜕𝜃1
𝑑 𝜕𝐿 𝜕𝐿
( )− = 𝜏2
𝑑𝑡 𝜕𝜃2̇ 𝜕𝜃2
A partir de (11) y la Ecuación de Euler Lagrange se obtiene entonces el siguiente sistema:
 𝜏1 = (52 + 19,2 cos 𝜃2)𝜃1̈ + (8,8 + 9,6𝑐𝑜𝑠𝜃2)𝜃2̈ − (19,2𝑠𝑒𝑛𝜃2)𝜃1̇𝜃2̇ − (9,6𝑠𝑒𝑛𝜃2)𝜃̇ 22
− 232,4𝑠𝑒𝑛𝜃1 − 58,8 sen(𝜃1 + 𝜃2) ; (12)

𝜏2 = (8,8 + 9,6𝑐𝑜𝑠𝜃2)𝜃1̈ + (8,8)𝜃2̈ + (9,6𝑠𝑒𝑛𝜃2)𝜃̇12 − 58,8 sen(𝜃1 + 𝜃2) ; (13)

𝜏1 52 + 19,2 cos 𝜃2 8,8 + 9,6𝑐𝑜𝑠𝜃2 𝜃1̈

[ ]=[ ][ ] +
𝜏2 8,8 + 9,6𝑐𝑜𝑠𝜃2 8,8 𝜃2̈
−19,2𝑠𝑒𝑛𝜃2. 𝜃2̇ −9,6𝑠𝑒𝑛𝜃2. 𝜃2̇ 𝜃1̇ −232,4𝑠𝑒𝑛𝜃1 − 58,8 sen(𝜃1 + 𝜃2)
[ ][ ] + [ ]; (14)
9,6𝑠𝑒𝑛𝜃2. 𝜃1̇ 0 𝜃2̇ −58,8 sen(𝜃1 + 𝜃2)

La ecuación (14) es la ecuación del manipulador en estudio y está conformado por las
siguientes tres matrices:
52 + 19,2 cos 𝜃2 8,8 + 9,6𝑐𝑜𝑠𝜃2
𝑀=[ ] ; 𝑚𝑎𝑡𝑟𝑖𝑧 𝑑𝑒 𝑖𝑛𝑒𝑟𝑐𝑖𝑎
8,8 + 9,6𝑐𝑜𝑠𝜃2 8,8
−19,2𝑠𝑒𝑛𝜃2. 𝜃2̇ −9,6𝑠𝑒𝑛𝜃2. 𝜃2̇
𝐶=[ ] ; 𝑚𝑎𝑡𝑟𝑖𝑧 𝑑𝑒 𝑐𝑜𝑟𝑖𝑜𝑙𝑖𝑠
9,6𝑠𝑒𝑛𝜃2. 𝜃1̇ 0
−232,4𝑠𝑒𝑛𝜃1 − 58,8 sen(𝜃1 + 𝜃2)
𝐺=[ ] ; 𝑚𝑎𝑡𝑟𝑖𝑧 𝑑𝑒 𝑝𝑜𝑡𝑒𝑛𝑐𝑖𝑎𝑙𝑒𝑠
−58,8 sen(𝜃1 + 𝜃2)
Modelo en espacio de estados:

La ecuación (14) es:

[𝜏] = [𝑀][𝜃]̈ + [𝐶][𝜃]̇ + [𝐺]

Se reordena para un modelo en espacio de estados así:

[𝜃̈ ] = (−[𝑀]−1 [𝐶][𝜃]̇ − [𝑀]−1 [𝐺]) + [𝑀]−1 [𝜏]