PRACTICA No 4

ANALISIS Y CONTROL DE ROBOTS

CONTROL DE POSICIN Y TRAYECTORIA DE UN BRAZO ROBTICO MEDIANTE CONTROL DE TORQUE COMPUTADO

PROBLEMA
Cada articulacin es sometida a torsiones exteriores 1, 2, 3 y 4 y desarrollan desplazamientos angulares 1, 2, 3 y 4 con sus respectivas velocidades 1, 2, 3 y 4. Todas las articulaciones del sistema
se ven afectadas por expresiones no lineales de friccin, entre otras.

FIG. 1 GRADOS DE LIBERTAD DEL ROBOT

Las expresiones de los componentes involucrados son:

( )

( )

( )
,

( )
,

CLCULO DE LOS PARMETROS DEL SISTEMA

UNIVERSIDAD NACIONAL DE INGENIERA | FACULTAD DE INGENIERA MECNICA

PRACTICA No 4
ANALISIS Y CONTROL DE ROBOTS

En el cuadro mostrado se observa la tabla de parmetros de Denavit Hartemberg:

I

AI

1
2
3
4

280.366
874.312
107.5
0

I
90
0
90
0

I
q1
q2
q3
0

DI

160.366
0
0
q4

De los datos de la tabla se calculan las matrices de transformacin:

( )

( )

( )

Durante el proceso de modelamiento y simulacin se utilizo el Simmechanics Toolbox, con el fin de reducir los calculos de momentos de Inercia, y facilitar una mejor simulacin grfica.
El resultado de la complacin de los scripts de matlab y los modelos de Simulink se mostrarn en la partes
siguientes.
Momentos de Inercia respecto a su respectivo centro de masa:

Momento de Inercia respecto al referencial X0, Y0, Z0 trasladados a sus centros de masa.
Centros de masa respecto al origen de su referencial; Vectores desde el origen hasta el centro de masa respectivo; Matrices que relacionan la velocidad lineal cartesiana del centro de masa con el vector velocidad
articular.

Hallando los vectores

( ):

( )

( )

[ ]

( )

UNIVERSIDAD NACIONAL DE INGENIERA | FACULTAD DE INGENIERA MECNICA

PRACTICA No 4
ANALISIS Y CONTROL DE ROBOTS

( )

( )

( )

]
[

( )

( )

( )

( )

( )

]
[

( )

]
[

JACOBIANOS DE VELOCIDAD LINEAL

Para el primer eslabn:
( )

Para el segundo eslabn:

( )
[

Para el tercer eslabn:

( )
[

Y para el cuarto eslabn:

( )
[

JACOBIANOS DE VELOCIDAD ANGULAR:

( )

( )

( )

UNIVERSIDAD NACIONAL DE INGENIERA | FACULTAD DE INGENIERA MECNICA

PRACTICA No 4
ANALISIS Y CONTROL DE ROBOTS
( )

CLCULO DE MATRICES
Teniendo las matrices
(

( )

( )

( )

( )

]
( )
(

( )

( )

( )

)
(

(
( )

( )
(

( )

( )
)
(

)
(

]
( )
( )

( )

( )

Dnde:

Adems:

Pasando a calcular las matrices

( ):

UNIVERSIDAD NACIONAL DE INGENIERA | FACULTAD DE INGENIERA MECNICA

PRACTICA No 4
ANALISIS Y CONTROL DE ROBOTS
( )

, ( )-

( )

, ( )- ( ), ( )-

, ( )-

, ( )-

]
(

, ( )- ( ), ( )-

, ( )-

)
(

[
[

]
( )

, ( )-

])
(
]

)
(

)
(

, ( )- ( ), ( )-

, ( )-

([

(
(

Finalmente, para el eslabn 4:

( )

, ( )-

, ( )-

, ( )- ( ), ( )-

UNIVERSIDAD NACIONAL DE INGENIERA | FACULTAD DE INGENIERA MECNICA

PRACTICA No 4
ANALISIS Y CONTROL DE ROBOTS

][

][

+ ..

( )

( )

( )

( )

( )

FORMULACION DE LAGRANGE
(

PARA q1:

( (

( ))

( )

PARA q2:

( )

UNIVERSIDAD NACIONAL DE INGENIERA | FACULTAD DE INGENIERA MECNICA

PRACTICA No 4
ANALISIS Y CONTROL DE ROBOTS
(

)))

)
(

)
(

) (

(
(

(
(

)
))

)
(
))

)))

))

)))

) (

) (

))

))

) (

))

) (

)
(

))

(
)

)
)

))

) (

(
(

)))

))

UNIVERSIDAD NACIONAL DE INGENIERA | FACULTAD DE INGENIERA MECNICA

PRACTICA No 4
ANALISIS Y CONTROL DE ROBOTS

((

) (

(
(

)
)

(
(

(
(

))

))

))

))

(
(

(
)

))

))

))
( )

PARA q3:

(
(

)
(

(
(

((

(
(

)
) (

)
)

))

)))

))

(
(

) (

(
(

) (

)
(
(

)
))

))

))
(

))
)

(
(

(
)

)
(

UNIVERSIDAD NACIONAL DE INGENIERA | FACULTAD DE INGENIERA MECNICA

PRACTICA No 4
ANALISIS Y CONTROL DE ROBOTS
(

((

(
(

)
) (
(

((

(
(

((

))

(
(

)
)

(
)

(
(

))

))

(
)

))

)
(
) (

))

) (

))

(
(

))

))

(
)

(
(

) (

(
)

)
)

(
( ) (
( ) )

) (

)))

))

))

))

(
(

))

))

))

(
(

(
)

))

))

))
( )

PARA q4:

)
(

))

UNIVERSIDAD NACIONAL DE INGENIERA | FACULTAD DE INGENIERA MECNICA

PRACTICA No 4
ANALISIS Y CONTROL DE ROBOTS
(

(
(

)
))

)
(
))

))

((

(
(

((

) (

(
(
(

))

))

))

))

))

))

))

))

))

)
(

))

(
(

))

(
)

(
(

) (

(
(

) (

(
)

(
(

((

))

)
)

( ) (
( ) )

) (

) (

(
(

) (

) (

))

)
(
) (

))

))

(
)

)
)

))

))

))

))

))

))

(
(

))

UNIVERSIDAD NACIONAL DE INGENIERA | FACULTAD DE INGENIERA MECNICA

PRACTICA No 4
ANALISIS Y CONTROL DE ROBOTS
(

) (

)
(
(

))
))

) (

(
(

(
(

) (

((

( )

)))

))

)))

(
)

))

( ))
) (

))

))

)
(

(
)

)
)

ENERGIA CINETICA:

-[

][ ]

ENERGIA POTENCIAL:
( )

( )

( )

( )

(
( )

( )
( )

( )

( )

( )
)

( ))

( ))

ECUACION DE LAGRANGE
(
(

( )

Finalmente luego de las operaciones mostradas se obtienen los parmetros para el modelo no lineal del sistema:
( )

( )

( )

UNIVERSIDAD NACIONAL DE INGENIERA | FACULTAD DE INGENIERA MECNICA

PRACTICA No 4
ANALISIS Y CONTROL DE ROBOTS

Para realizar el acoplamiento de los motores se debe tener en cuenta que el motor debe ser idealizado para
despreciar la inductancia del motor:

MODELO LEY DE CONTROL NO LINEAL Y LINEAL

El sistema completo fue explicado durante la busqueda de los parametros del sitema; para fines de estudio se
genera una trayectoria predefinida en la compilacion del programa principal [Script del MatLab], esto debido
a que de esa manera reducimos el tiempo de compilacin del Simulink.
El sistema mecanico modelado y controlado, es simulado mediante el toolbox Simmechanics de Simulink;
junto con el software SolidWorks.
Los problemas que se pueden observar son basicamente por efectos de no linealidad, ello se podr ver
compensado en el informe de Control por Torque Computado.
A continuacin se muestra las partes internas del sistema modelado, asi como los Subsystems que permiten la
optimizacin en el tiempo de compilacin.
Para el control de trayectoria se usa la ley e control de trayectoria:
( )

* (
(

( )
)

( )+
)

CLCULO DE PARMETROS
Para este caso, realizar los calculos de
y
se vuelven simples, pero lo que resulta complicado es la
arbitrariedad con la que se definen las raices objetivas de cada sistema; por ello para el presente informe se
toma en cuenta el enfoque aleatorio de los algoritmos geneticos.
Para una sintonizacin PID usando algoritmos genticos los valores de Kp, Ki y Kd van variando
aleatoriamente encontrando en cada generacin el mejor valor y reduciendoel rango de bsqueda.
Incluso del pruebas similares que se realizaron con control PID se obtuvieron los siguientes resultados:
Con el controlador PID se obtuvo resultados muy pobres, al realizar cambios en la referencias o trayetorias
complejas, la nolinealidad del sistema era muy grande de esta manera el controlador fallaba.
Con el control por torque computado se obtubieron resultados mucho mejores, las trayectorias definidas
fueron realizadas sin mayores problemas, por ello este metodo resulto ms efectivo a excepcin de
sintonizacin del PID, la cual era muy tediosa, pero esto se soluciona mediante el uso de algoritmos geneticos.
Se espera que con otros controladores no lineales se logren mejores resultados a los ya obtenidos con el
control de torque calculado. Se pueden usar controladores Adaptivos, Fuzzy Control (Control FIS),
Adaptative Neural Fuzzy Interface Systems (Control ANFIS) , con lo que se reduciria la carga computacional
del sistema.

UNIVERSIDAD NACIONAL DE INGENIERA | FACULTAD DE INGENIERA MECNICA

PRACTICA No 4
ANALISIS Y CONTROL DE ROBOTS

FUENTE PRINCIPAL DE LA SIMULACIN; ES MUY DEPENDIENTE DE LAS FUNCIONES SCRIPT QUE NECESITA PARA REDUCIR LA CARGA COMPUTACIONAL

1. GRFICA DE LAS VARIABLES

Los resultados obtenidos por el Control de torque Computado con Sintonizacin manual fueron
Kd=diag([0.1 0.1 0.5 0.1]); Ki=diag([1 1 0.01 0.01]); Kp=diag([100 100 100 10]).
Estos resultados pueden observarse en las grficas siguientes:
UNIVERSIDAD NACIONAL DE INGENIERA | FACULTAD DE INGENIERA MECNICA

PRACTICA No 4
ANALISIS Y CONTROL DE ROBOTS

CONTROL
DE TRAYECTORIA MEDIANTE TORQUE CALCULADO CON PID SINTONIZADO

UNIVERSIDAD NACIONAL DE INGENIERA | FACULTAD DE INGENIERA MECNICA

PRACTICA No 4
ANALISIS Y CONTROL DE ROBOTS

CONTROL DE TORQUE

Los resultados obtenidos por el Control de torque Computado con Sintonizacin genetica fueron
Kd=diag([138 78 79.1 15.4]); Ki=diag[12.2 1.79 0.309 0.496]); Kp=diag([0.269 0.499 0.979 0.119]).
Estos resultados pueden observarse en las grficas siguientes:

CONTROL DE POSICIN POR TORUE COMPUTADO Y SINTONIZACIN GENETICA

UNIVERSIDAD NACIONAL DE INGENIERA | FACULTAD DE INGENIERA MECNICA

PRACTICA No 4
ANALISIS Y CONTROL DE ROBOTS

UNIVERSIDAD NACIONAL DE INGENIERA | FACULTAD DE INGENIERA MECNICA