Control óptimo multivariable punto a punto de un robot ...

8
MEMORIAS DEL XXVII CONGRESO INTERNACIONAL ANUAL DE LA SOMIM 22 al 24 DE SEPTIEMBRE DE 2021 PACHUCA, HIDALGO, MÉXICO Tema A3b.Mecanismos y Robótica: Modelado aplicado a robótica Control óptimo multivariable punto a punto de un robot manipulador tipo SCARACesar Inchaurregui Marín a , René Galindo Orozco a . a Universidad Autónoma de Nuevo León, Facultad de Ingeniería Mecánica y Eléctrica, Av. Pedro de Alba S/N, San Nicolás de los Garza, N.L., México. *Autores contacto, [email protected], [email protected], [email protected] R E S U M E N Los robots manipuladores han sido un área ampliamente estudiada, desde que se introdujeron en la industria para realizar algunas tareas repetitivas o peligrosas para las personas. En este trabajo se propone una metodología para obtener la forma cuadrática de la energía cinética y potencial para el desarrollo del modelo dinámico de un sistema robótico tipo SCARA. En Matlab-SimMechanics se desarrolla un modelo dinámico CAD que tiene como referencia un sistema físico, posteriormente se utiliza para diseñar una ley de control óptima y llevar el sistema de un punto de equilibrio asintótico a otro, teniendo una compensación para las perturbaciones externas al modelo dinámico, como la dinámica del motor, fricción y gravedad en cada grado de libertad. Palabras Clave: Modelado, Control óptimo, modelo dinámico de CAD, manipulador SCARA A B S T R A C T Even with years of study focused on industrial robot manipulators, and due to advancement in computing capability, there are many areas of opportunity and improvement in the study and implementation of control models. This work presents the development and implementation of optimal control, through simulation, applied to a SCARA-type multivariable robotic system. Where the physical prototype is used to obtain each of the parameters and variables that are subsequently introduced in the simulation carried out with the dynamic model of the system, developed in the Matlab-SimMechanics environment. The methodology shown allowed to obtain a control model that uses the quadratic form of the kinetic and potential energy of the system to bring the end effector of the robot to an equilibrium point, and likewise, to cancel external disturbances such as friction, gravity and dynamics of the motors in each degree of freedom. Keywords: Modelling, optimal control, CAD dynamic models, SCARA manipulators. 1. Introducción El control automático ha sido sumamente estudiado y aplicado en robótica ya que, es la manera en que se logra que el sistema realice los movimientos necesarios para seguir una trayectoria y llegar a un punto de interés. Hay diferentes maneras en las que se puede integrar una técnica de control y dinámica del sistema, ya sea mediante la cinemática directa e inversa del robot o bien, como se ha hecho en este trabajo, haciendo uso de las energías del robot de los modelos dinámicos de Euler- Lagrange y su respectivo modelo CAD. Se implementa una ley de control óptima, que utiliza el modelo dinámico Euler-Lagrange del sistema, que define una estabilidad asintótica de cada una de las articulaciones del sistema. En la revisión de bibliografía se encuentra que el diseño de sistemas de control para robots manipuladores [1-4] ha sido un área de interés, para muchos investigadores, desde que se han incorporado cada vez ISSN 2448-5551 MT 27 Derechos Reservados © 2021, SOMIM

Transcript of Control óptimo multivariable punto a punto de un robot ...

Page 1: Control óptimo multivariable punto a punto de un robot ...

MEMORIAS DEL XXVII CONGRESO INTERNACIONAL ANUAL DE LA SOMIM 22 al 24 DE SEPTIEMBRE DE 2021 PACHUCA, HIDALGO, MÉXICO

Tema A3b.Mecanismos y Robótica: Modelado aplicado a robótica

“Control óptimo multivariable punto a punto de un robot manipulador tipo SCARA”

Cesar Inchaurregui Marína, René Galindo Orozcoa.

aUniversidad Autónoma de Nuevo León, Facultad de Ingeniería Mecánica y Eléctrica, Av. Pedro de Alba S/N, San Nicolás de los Garza, N.L., México.

*Autores contacto, [email protected], [email protected], [email protected]

R E S U M E N

Los robots manipuladores han sido un área ampliamente estudiada, desde que se introdujeron en la industria para realizar

algunas tareas repetitivas o peligrosas para las personas. En este trabajo se propone una metodología para obtener la forma

cuadrática de la energía cinética y potencial para el desarrollo del modelo dinámico de un sistema robótico tipo SCARA.

En Matlab-SimMechanics se desarrolla un modelo dinámico CAD que tiene como referencia un sistema físico,

posteriormente se utiliza para diseñar una ley de control óptima y llevar el sistema de un punto de equilibrio asintótico a

otro, teniendo una compensación para las perturbaciones externas al modelo dinámico, como la dinámica del motor, fricción

y gravedad en cada grado de libertad.

Palabras Clave: Modelado, Control óptimo, modelo dinámico de CAD, manipulador SCARA

A B S T R A C T

Even with years of study focused on industrial robot manipulators, and due to advancement in computing capability, there

are many areas of opportunity and improvement in the study and implementation of control models. This work presents the

development and implementation of optimal control, through simulation, applied to a SCARA-type multivariable robotic

system. Where the physical prototype is used to obtain each of the parameters and variables that are subsequently introduced

in the simulation carried out with the dynamic model of the system, developed in the Matlab-SimMechanics environment.

The methodology shown allowed to obtain a control model that uses the quadratic form of the kinetic and potential energy

of the system to bring the end effector of the robot to an equilibrium point, and likewise, to cancel external disturbances such

as friction, gravity and dynamics of the motors in each degree of freedom.

Keywords: Modelling, optimal control, CAD dynamic models, SCARA manipulators.

1. Introducción

El control automático ha sido sumamente estudiado y

aplicado en robótica ya que, es la manera en que se logra

que el sistema realice los movimientos necesarios para

seguir una trayectoria y llegar a un punto de interés. Hay

diferentes maneras en las que se puede integrar una

técnica de control y dinámica del sistema, ya sea

mediante la cinemática directa e inversa del robot o bien,

como se ha hecho en este trabajo, haciendo uso de las

energías del robot de los modelos dinámicos de Euler-

Lagrange y su respectivo modelo CAD. Se implementa

una ley de control óptima, que utiliza el modelo dinámico

Euler-Lagrange del sistema, que define una estabilidad

asintótica de cada una de las articulaciones del sistema.

En la revisión de bibliografía se encuentra que el

diseño de sistemas de control para robots manipuladores

[1-4] ha sido un área de interés, para muchos

investigadores, desde que se han incorporado cada vez

ISSN 2448-5551 MT 27 Derechos Reservados © 2021, SOMIM

Page 2: Control óptimo multivariable punto a punto de un robot ...

MEMORIAS DEL XXVII CONGRESO INTERNACIONAL ANUAL DE LA SOMIM 22 al 24 DE SEPTIEMBRE DE 2021 PACHUCA, HIDALGO, MÉXICO

más los sistemas robóticos en nuevas tareas y disciplinas.

En [5] se desarrolla una ley de control óptima, para

sistemas dinámicos de Euler-Lagrange en lazo cerrado,

misma que se aplica en este trabajo, que logra llevar a

sistemas completamente actuados a una estabilidad

asintótica. En [5] también se muestra el funcionamiento

de la ley de control, mediante la simulación de un sistema

de un carro y un péndulo invertido. En [6] se diseña un

sistema de control proporcional, integral, derivativo

(PID) y controladores polinomiales, para cada

articulación independiente, haciendo uso de la

cinemática directa e inversa del sistema, con una

compensación del momento dinámico del sistema

afectando cada articulación durante el movimiento,

ejecutado en un robot manipulador PUMA-560. En [7] se

plantea un sistema de control, derivando funciones

estrictas de Lyapunov, enfocadas en controladores finitos

en el tiempo de robots manipuladores. En [8] se estudian

las capacidades de un sistema de control, en un robot

FANUC M-430iA/4FH, donde se propone un criterio de

calidad de control de posición en modo operativo del

robot; se utilizan dos métodos de control, siendo uno de

ellos un regulador lineal PID para el control de cada uno

de los grados de libertad y, por otro lado, se suscita el uso

de un sistema de control robusto DRC (Disturbance

Rejection Control System).

Existe una variedad de lecturas acerca de trabajos en

el desarrollo de sistemas y modelos de control, dirigidas

a robots manipuladores para diferentes tareas o enfoques.

Este trabajo se enfoca, principalmente, en el desarrollo

del modelo dinámico de un robot tipo SCARA. con el

objetivo de introducir una ley de control óptima

multivariable para el control de posición de punto a punto

del efector final del sistema. El control del robot

manipulador utilizado para este trabajo hace uso de las

energías cinética y potencial del modelo dinámico del

sistema, agregando una compensación de fricción,

gravedad y de la dinámica de sistemas físicos como los

motores en los grados de libertad rotacionales. Cabe

mencionar que las variables utilizadas tanto en el modelo

dinámico del sistema, como las variables usadas en los

modelos de sistemas físicos han sido tomadas del

prototipo real del robot.

2. Modelo Euler-Lagrange

Dado que la ley de control descrita en [1,5], tiene un

enfoque a sistemas dinámicos no lineales de Euler-

Lagrange, el sistema que se desea controlar debe cumplir

con la forma de la ecuación de Euler-Lagrange:

𝑀(𝑞)�̈� + 𝐶(𝑞, �̇�)�̇� + 𝐺(𝑞) + 𝐹(�̇�) + 𝜏𝑑(𝑡) = 𝜏(𝑡) (1)

Siendo en la ec. (1), 𝑀(𝑞) una matriz de inercia

generalizada, 𝐶(𝑞, �̇�) la matriz Coriolis generalizada,

𝐺(𝑞) el vector de gravedad generalizado del sistema,

𝐹(�̇�) la fricción generalizada, 𝜏𝑑(𝑡) una perturbación

externa acotada respecto al tiempo y 𝜏(𝑡) el vector de

entradas generalizadas. También 𝑞, �̈� 𝑦 𝑞 son los

vectores de posición, velocidad y aceleración

respectivamente. La matriz 𝑀(𝑞) se determina en la

siguiente sección, dado que 𝐺(𝑞), 𝐹(�̇�) y 𝜏𝑑(𝑡) son

establecidas mediante mediciones y parámetros físicos

del sistema.

2.1 Ley de control

Se considera un esquema de control donde las

perturbaciones externas, la fricción del sistema y la

gravedad son compensadas para estabilizar el sistema a

su punto de origen. Definiendo 𝑥1 = 𝑞 y 𝑥2 = �̇�, se

tiene la ley de cancelación:

𝜏(𝑡) = 𝑢 + 𝜏𝑑(𝑡) + 𝐹(𝑥2) + 𝐺(𝑥1) (2)

Y la ley de control óptima [5]:

𝑢∗ = −𝑥1

2−

𝑀(𝑥1)𝑥2

2 (3)

Que minimiza el criterio de la ec. (4):

𝐽 = ∫[𝑥1𝑇𝑄𝑥1 + 𝑢𝑇𝑅𝑢 + 𝑥2

𝑇𝑀(𝑥1)𝑅𝑢]𝑑𝑡 (4)

Donde 𝑅 = 𝑅𝑇 = (𝑀(𝑥1))−1 > 0 𝑦 𝑄 = 𝐼2.

Las ecuaciones de estado son:

�̇�1 = 𝑥2

�̇�2 = (𝑀(𝑥1))−1[𝑢 − 𝐶(𝑥1, 𝑥2)𝑥2] (5)

En [1], se diseña la ley de control para llevar al sistema

a la estabilidad asintótica al origen. Sin embargo, en lugar

de 𝑞, se puede utilizar la forma:

𝛥𝑞(𝑡) = 𝑞(𝑡) − �̃� (6)

Donde �̃�, son las coordenadas donde se desea

estabilizar el efector final. Siendo 𝛥q, el nuevo punto de

equilibro del sistema, este punto es diferente al origen.

3. Modelo dinámico

En este trabajo se hace énfasis en el modelo dinámico

Euler-Lagrange con base en las energías cinéticas y

potenciales del mecanismo. Este enfoque nos permite un

mejor estudio de las propiedades dinámicas y el esquema

de control. Considerando la expresión de la energía

cinética [9]:

ISSN 2448-5551 MT 28 Derechos Reservados © 2021, SOMIM

Page 3: Control óptimo multivariable punto a punto de un robot ...

MEMORIAS DEL XXVII CONGRESO INTERNACIONAL ANUAL DE LA SOMIM 22 al 24 DE SEPTIEMBRE DE 2021 PACHUCA, HIDALGO, MÉXICO

𝐸𝑘 = 𝐸𝑘𝑡𝑟𝑎𝑠 + 𝐸𝑘𝑟𝑜𝑡 = 1

2�̇�𝑇𝑀(𝑞)�̇� (7)

Donde 𝑀(𝑞) es la matriz de inercia, 𝑞 el vector de

coordenadas generalizadas para rotación y traslación,

también se tiene que:

𝐸𝑘𝑡𝑟𝑎𝑠 = 1

2 ∑ 𝑚𝑖 (𝑣𝑖)

𝑇 𝑣𝑖 𝑛𝑖=1 (8)

𝐸𝑘𝑟𝑜𝑡 = 1

2 ∑ 𝑤𝑖

𝑇 𝐼𝑖 𝑤𝑖 𝑛𝑖=1 (9)

Siendo para la ec. (8), 𝑚𝑖 cada una de las masas del

sistema y 𝑣𝑖 son las velocidades tangenciales de los

centros de masas. Para la ec. (9), Ii es la matriz de inercias

de cada eslabón y 𝑤𝑖 denota las velocidades angulares

absolutas.

Dado que se ha utilizado un sistema físico como

referencia para la obtención de todas aquellas constantes

del sistema, se tiene que las matrices de inercias de cada

eslabón han sido determinadas mediante el modelo CAD

del sistema físico, teniendo la forma:

𝐼𝑖 = [

𝐼𝑋 𝐼𝑋𝑌 𝐼𝑋𝑍

𝐼𝑋𝑌 𝐼𝑌 𝐼𝑌𝑍

𝐼𝑋𝑍 𝐼𝑌𝑍 𝐼𝑍

] (10)

Donde IX, IY e Iz son los momentos de inercia y IXY,

IXZ e IYz son los productos de inercia.

3.1 Convención Denavit-Hartenberg

Haciendo uso de la convención Denavit-Hartenberg [10],

en la tabla 1, se desplaza el marco de referencia desde la

base del robot, pasando por cada uno de los centros de

masa, mostrados en el diagrama de CAD de la Fig. 1,

hasta la posición del efector final.

Figura 1 - Esquema del robot

En la Fig. 1, se muestra la configuración del robot y la

posición de sus centros de masa que posteriormente

servirán para el cálculo de las transformaciones

homogéneas, en función de las rotaciones y traslaciones

del marco de referencia correspondiente. Se sabe

mediante el modelo CAD, la posición de los centros de

masa de cada eslabón.

Tabla 1 – Parámetros Denavit - Hartenberg.

𝑻𝒊𝒊−𝟏 𝜽𝒊 𝒅𝒊 𝒂𝒊 𝜶𝒊

𝑻𝟎𝟏 𝜃1

∗ 𝑑1 0 0

𝑻𝟏𝟐 0 𝑑2

∗ 𝑎1 0

𝑻𝟐𝟑 0 0 𝑎2 0

𝑻𝟑𝟒 𝜃2

∗ 0 𝑎3 0

Donde 𝜃𝑖∗, 𝑑𝑖

∗, 𝑎𝑖∗ y 𝛼𝑖

∗, son las variables de traslación y

rotación del marco de referencia correspondiente a cada

transformación homogénea respecto al tiempo, y que 𝜃𝑖,

𝑑𝑖, 𝑎𝑖 y 𝛼𝑖 son datos ya conocidos del sistema físico.

Efectuando la multiplicación de las matrices de

transformaciones homogéneas compuestas de las

rotaciones y de los vectores de posición; definidos en la

convención de Denavit-Hartenberg, mostrados en la

Tabla 1. Se obtienen las transformaciones homogéneas

con la forma:

𝑇𝑖 𝑖−1 = [𝑅0

𝑖 𝜊0𝑖

0 1] (11)

Donde 𝑅0i denota la orientación y 𝜊0

𝑖 la posición del

marco de referencia respecto al tiempo.

3.2 Velocidades tangenciales

Usando como referencia la forma de la transformación

homogénea, sabemos que se obtiene la velocidad

tangencial mediante �̇�𝑜𝑖 . Se sabe, por la ec. (8), que la

velocidad utilizada para calcular la energía cinética

traslacional se determina mediante la ec. (12).

𝑣𝑖 = �̇�𝑜𝑖 (12)

Aplicando la ec. (12) para cada centro de masa, se

obtuvieron las velocidades angulares (13)-(15).

𝑣1 = 0 (13)

𝑣2 = 𝑎12�̇�1

2 + �̇�12 (14)

𝑣3 = (𝑎1 + 𝑎2)2�̇�1

2 + 𝑎32(�̇�1 + �̇�2)

2 + 2𝑎3(𝑎1 +

𝑎2)𝐶𝜃2(�̇�1

2 + �̇�1�̇�2) + �̇�22 (15)

Haciendo uso de las velocidades tangenciales, y

aplicando la ec. (8), se determina la energía cinética

traslacional del sistema:

ISSN 2448-5551 MT 29 Derechos Reservados © 2021, SOMIM

Page 4: Control óptimo multivariable punto a punto de un robot ...

MEMORIAS DEL XXVII CONGRESO INTERNACIONAL ANUAL DE LA SOMIM 22 al 24 DE SEPTIEMBRE DE 2021 PACHUCA, HIDALGO, MÉXICO

𝐸𝑘𝑡𝑟𝑎𝑠 =1

2 𝑚2(𝑎1

2�̇�12 + �̇�1

2) +1

2 𝑚3((𝑎1 + 𝑎2)

2�̇�12 +

𝑎32(�̇�1 + �̇�2)

2 + 2𝑎3(𝑎1 + 𝑎2)𝐶𝜃2(�̇�1

2 + �̇�1�̇�2) + �̇�22)

d (16)

3.3 Velocidades angulares

En esta sección, se calculan las velocidades angulares de

cada centro de masa, respecto a un marco de referencia

fijo, según las ecs. (17) y (18) descritas en [10].

𝑊0𝑖 = 𝜌1𝑤0

1 + 𝜌2𝑅01𝑤1

2 + ⋯+ 𝜌𝑖𝑅0𝑖−1𝑤𝑖−1

𝑖

𝑊0𝑖 = 𝜌1𝐽𝑤1�̇�1 + 𝜌2𝐽𝑤2�̇�2 + ⋯ + 𝜌𝑖𝐽𝑤𝑖�̇�𝑖 (17)

𝑊𝑖 = (𝑅0𝑖 )𝑇𝑊0

𝑖 (18)

Donde 𝜌𝑖 = 1 para rotación y 𝜌𝑖 = 0 para

traslación, 𝐽𝑤𝑖�̇�𝑖 = [0 0 �̇�𝑖]𝑇 si el eje de giro es z y

𝐽𝑤𝑖�̇�𝑖 = [�̇�𝑖 0 0]𝑇 si el eje de giro es x, 𝑊0𝑖 es la

velocidad angular resultante debido a la rotación relativa

de varios marcos de coordenadas. La ec. (18) nos sirve

para el cálculo de las velocidades angulares absolutas de

los centros de masa, donde 𝑅0𝑖 denota la matriz de

orientación de la transformación homogénea 𝑇0𝑖 y 𝑊0

𝑖

denota la velocidad angular relativa de los centros de

masa. Aplicando la ec. (17), se consiguen las velocidades

angulares relativas:

𝑊01 = [

00�̇�1

] ,𝑊02 = [

00

�̇�1 + �̇�2

] (19)

Utilizando la ec. (18), se obtienen las velocidades

absolutas:

𝑊1 = [

00�̇�1

] , 𝑊2 = [

00

�̇�1 + �̇�2

] (20)

Se hace notar que en, este caso, las velocidades

angulares relativas y absolutas del sistema son idénticas.

Esto se debe a la configuración del robot manipulador

tipo SCARA, ya que ambas rotaciones están sobre el

plano horizontal, causando que la posición del efector

final sea el resultado de �̇�1 + �̇�2.

𝐸𝑘𝑟𝑜𝑡 = 1

2 𝐼𝑍1�̇�1

2 + 𝐼𝑍2�̇�12 + 𝐼𝑍3(�̇�1 + �̇�2)

2 (21)

3.4 Modelo dinámico de Euler-Lagrange

Como se mencionó en la sección 2, la ley de control

adoptada hace uso de la energía cinética del sistema, para

introducir la matriz de inercias generalizadas a la ley de

control. Con las ecs. (7), (16) y (21), se llega a la energía

cinética total señalada en la ec. (1).

Se sabe que M(q), es una matriz simétrica [1] y que es

la representación cuadrática de la energía cinética del

modelo dinámico Euler-Lagrange del sistema. Entonces:

1

2�̇�𝑇𝑀(𝑞)�̇� = [

�̇�1

�̇�2

�̇�2

]

𝑇

[

𝑀11 𝑀12 𝑀13

𝑀21 𝑀22 𝑀23

𝑀31 𝑀32 𝑀33

] [

�̇�1

�̇�2

�̇�2

] (22)

Donde:

𝑀11 = 𝑚₂𝑎₁² + 𝑚₃(𝑎₁ + 𝑎₂)² + 𝑚₃𝑎₃² + 2𝑚₃𝑎₃(𝑎₁ +

𝑎₂)𝑐𝑜𝑠 (𝜃2) + 𝐼𝑧1 + 𝐼𝑧2 + 𝐼𝑧3

𝑀13 = 𝑚3𝑎32 + 𝑚3𝑎3(𝑎1 + 𝑎2)𝑐𝑜𝑠 (𝜃2) + 𝐼𝑍3

𝑀22 = 𝑚2 + 𝑚3

𝑀31 = 𝑚3𝑎32 + 𝑚3𝑎3(𝑎1 + 𝑎2)𝑐𝑜𝑠 (𝜃2) + 𝐼𝑍3

𝑀33 = 𝑚3𝑎32 + 𝐼𝑍3

𝑀12 = 𝑀21 = 𝑀23 = 𝑀32 = 0

En la sección 5, se hace uso de la matriz M(q) en la

simulación del sistema, incorporada en la ley de control

óptima dada en la ec. (3).

4. Modelo de motores DC

Se agregó al modelo de control una compensación de la

dinámica de los motores en las uniones rotacionales. Se

considera un motor DC como un sistema de primer orden,

de esta manera se desprecia la inductancia de armadura.

Figura 2 - Modelo del motor

Donde 𝑣 es el voltaje aplicado, K denota la ganancia

de la función de transferencia y 𝜏 denota la constante de

tiempo. Aplicando el Teorema del valor final [11], 𝐾 = 𝑤(𝑠𝑠) /𝑣(𝑠𝑠) , donde 𝑤(𝑠𝑠) y 𝑣(𝑠𝑠) denotan valores en

estado estacionario y que al sistema le toma 5𝜏 en llegar

a su velocidad nominal en estado estacionario, entonces

𝜏 = 5𝜏/5.

Se realizaron mediciones para obtener el valor de 5𝜏

y la zona muerta (𝑣(𝑑𝑧)) de arranque que se tiene en los

motores. Todas estas mediciones se llevaron a cabo con

ISSN 2448-5551 MT 30 Derechos Reservados © 2021, SOMIM

Page 5: Control óptimo multivariable punto a punto de un robot ...

MEMORIAS DEL XXVII CONGRESO INTERNACIONAL ANUAL DE LA SOMIM 22 al 24 DE SEPTIEMBRE DE 2021 PACHUCA, HIDALGO, MÉXICO

una respuesta a la entrada en escalón y se han utilizado

para recrear el comportamiento real de la dinámica de los

motores. Además, se considera una saturación (𝑣(𝑠𝑎𝑡))

para la entrada de 𝑣(𝑠𝑠) que ha sido tomada de la hoja de

especificaciones del fabricante y se integra en la

simulación mostrada en secciones posteriores.

5. Modelo de control implementado

A manera de formular la ley de control para el

mecanismo robótico de interés, se aplican las ecs. (2) y

(3) en la ec. (22). Se tiene la ley de control:

[

𝜏1

𝜏2

𝜏3

] =

[ −

1

2𝜃1 − (𝑀11�̇�1 + 𝑀11�̇�2 + 𝑀11�̇�2) + 𝑏1�̇�1

−1

2𝑑2 − (𝑀21�̇�1 + 𝑀22�̇�2 + 𝑀23�̇�2) + 𝑏2�̇�2 + 𝐸𝑝

−1

2𝜃2 − (𝑀31�̇�1 + 𝑀32�̇�2 + 𝑀33�̇�2) + 𝑏3�̇�2 ]

j (23)

En la ec. (23), se aprecia la ley de control que

estabilizará al sistema al origen. Donde Ep = 𝑔(𝑚2 +𝑚3), en las ecs. (23) y (24).

[

𝜏1

𝜏2

𝜏3

] =

[ −

1

2𝛥𝜃1 − (𝑀11�̇�1 + 𝑀11�̇�2 + 𝑀11�̇�2) + 𝑏1�̇�1

−1

2𝛥𝑑2 − (𝑀21�̇�1 + 𝑀22�̇�2 + 𝑀23�̇�2) + 𝑏2�̇�2 + 𝐸𝑝

−1

2𝛥𝜃2 − (𝑀31�̇�1 + 𝑀32�̇�2 + 𝑀33�̇�2) + 𝑏3�̇�2 ]

j (24)

Como se menciona en la sección 2, a la ec. (24), se le

ha hecho el cambio de variable que permitirá estabilizar

al robot manipulador del origen a un punto de interés.

Para realizar la atenuación de la dinámica de los motores

CD, se tiene un esquema de cancelación como se muestra

en la Fig. 3, donde 𝐺(𝑠) = 𝐾/(𝜏𝑠 + 1). Para establecer

este esquema en la simulación, al ser el resultado la

inversa de la función de transferencia del motor de primer

orden, se utiliza un controlador PD.

Figura 3 – Esquema de compensación simplificado

Integrando la cancelación de la dinámica del motor al

sistema, tenemos un esquema de control [5], como se

observa en la Fig. 4. Donde 𝑢∗ es la ley de control,

“Robot” es la planta del sistema, 𝐺(𝑥1) el vector de

gravedad generalizado y 𝐹(𝑥2) el vector de fricción.

Figura 4 – Esquema de control

Este esquema de control ha sido ejecutado en la

siguiente sección.

6. Simulación

La simulación se realizó con las herramientas MATLAB-

Simulink y SolidWorks. Con el toolbox Simpscape

multibody, se logró exportar el modelo 3D del sistema

desde SolidWorks a un entorno de Simulink. Dentro de

este entorno se llevó a efecto, inicialmente, la ley de

control de la ec. (23) para estabilizar los grados de

libertad del robot al punto de origen, posteriormente se

realizó el cambio de variable, para estabilizar cada grado

de libertad en una posición deseada mediante la ley de

control (24). En la Fig. 4, se muestra el esquema de

control óptimo para un robot manipulador tipo SCARA.

Los datos utilizados para la simulación son: 𝑚1 = 1,

𝑚2 = 0.40, 𝑚3 = 0.12, 𝐼𝑧1 = 0.0029, 𝐼𝑧2 = 0.0040,

𝐼𝑧3= 0.0008, 𝑎1 = 0.15, 𝑎2 = 0.15, 𝑎3 = 0.15, 𝑏1 =0.1, 𝑏2 = 0.1, 𝑏3 = 0.1, 𝑔 = 9.81, 𝜏 = .0374, 𝑘 =0.0872, 𝑣(𝑑𝑧) = ±0.47, 𝑣(𝑠𝑎𝑡) = ±12𝑣. Todos los

datos han sido tomados del prototipo físico, exceptuando

el coeficiente de fricción utilizado en la ec. (2) y las

matrices de inercia de cada eslabón, ya que se intenta

realizar una simulación apegada al sistema físico.

Se encuentra en las Figs. (5-7), que el sistema se

estabiliza al punto de origen desde una condición inicial

diferente a este (90,20,30). Se presenta una respuesta

suave para la primera rotación y el prismático, siendo la

segunda rotación, de la Fig. 7, la que presenta una

perturbación debida al movimiento de la primera

rotación. Sin embargo, la ley de control logra atenuar esta

perturbación y logra la estabilidad asintótica del sistema

en lazo cerrado.

En las Figs. (9-11), se muestra el comportamiento del

sistema al llevarlo a una posición deseada (45, 20, 80),

desde una condición inicial (0, 0, 0). Se examina un

funcionamiento satisfactorio de la ley de control, al llevar

al punto de equilibro sin mostrar un comportamiento con

sobre impulsos o alguna otra anomalía. Pese a esta

perturbación generada por la primera rotación, la ley de

control logra disminuirla y llegar a la posición deseada

en la segunda rotación.

ISSN 2448-5551 MT 31 Derechos Reservados © 2021, SOMIM

Page 6: Control óptimo multivariable punto a punto de un robot ...

MEMORIAS DEL XXVII CONGRESO INTERNACIONAL ANUAL DE LA SOMIM 22 al 24 DE SEPTIEMBRE DE 2021 PACHUCA, HIDALGO, MÉXICO

Figura 5 – Ángulo respecto al tiempo con punto inicial diferente al

origen

Figura 6 – Posición respecto al tiempo con punto inicial diferente

al origen

Figura 7 – Ángulo respecto al tiempo con punto inicial diferente al

origen

En las Figs. (8) y (12), se aprecia que la ley de control

tiene respuestas suaves, a excepción de 𝜏3, y sin

sobrepasar los niveles de saturación del sistema. Además,

se observa que se minimiza el criterio descrito en la ec.

(4).

En la Fig. 13, se aprecia el diagrama en Simulink

utilizado para efectuar la simulación de la ley de control

óptima en el modelo SimMechanics del sistema robótico

de tipo SCARA. Dentro de la ley óptima de control, se

tienen las ecs. (23) y (24) para tener una estabilidad

asintótica al origen, o bien, una estabilidad a un punto

establecido que no converge en el origen del sistema.

Figura 8 – Comportamiento de la ley de control con punto inicial

diferente al origen

Figura 9 – Ángulo respecto al tiempo, punto de equilibro deseado

Figura 10 – Posición respecto al tiempo, punto de equilibro

deseado

ISSN 2448-5551 MT 32 Derechos Reservados © 2021, SOMIM

Page 7: Control óptimo multivariable punto a punto de un robot ...

MEMORIAS DEL XXVII CONGRESO INTERNACIONAL ANUAL DE LA SOMIM 22 al 24 DE SEPTIEMBRE DE 2021 PACHUCA, HIDALGO, MÉXICO

Figura 11 – Ángulo respecto al tiempo en condición inicial

diferente al origen

Figura 12 – Comportamiento de la ley de control, punto de

equilibro deseado

7. Conclusiones

Con este trabajo se muestra el comportamiento de un

control óptimo multivariable aplicado a un modelo

dinámico de Lagrange, basado en un prototipo físico que

proporciona las constantes y parámetros para realizar la

simulación. El comportamiento del sistema ha sido

satisfactorio, ya que se ha logrado llevar a cada eslabón

del robot manipulador al punto de equilibro deseado con

una trayectoria suave y sin sobre impulsos. Teniendo una

ley de control que si bien, al inicio muestra una pequeña

variación y sobre impulsos, logra atenuar todas las

perturbaciones externas y llegar a converger

asintóticamente en cero o en un punto de equilibrio

deseado.

Una metodología de este tipo puede dar grandes

beneficios al momento de desarrollar un sistema en el

cuál, se implemente un modelo de control unificado, es

decir, no se requiere la calibración o programación de un

control independiente en cada una de las articulaciones.

También da paso al uso del modelo dinámico de nuestros

sistemas, y así integrar una compensación de

perturbaciones ajenas al sistema que en otros modelos de

control podrían no integrarse de una manera sencilla.

Figura 13 – Diagrama en simulink del sistema y la ley de control óptima

ISSN 2448-5551 MT 33 Derechos Reservados © 2021, SOMIM

Page 8: Control óptimo multivariable punto a punto de un robot ...

MEMORIAS DEL XXVII CONGRESO INTERNACIONAL ANUAL DE LA SOMIM 22 al 24 DE SEPTIEMBRE DE 2021 PACHUCA, HIDALGO, MÉXICO

REFERENCIAS

[1] John Gregory, Alberto Olivares & Ernesto Staffetti.,

Energy-optimal trajectory planning for robot

manipulators with holonomic constraints. Systems &

Control Letters, Volume 61, Issue 2,2012, Pages 279-

291.

[2] Guechi E-H, Bouzoualegh S, Zennir Y, Blažič S., MPC

Control and LQ Optimal Control of A Two-Link Robot

Arm: A Comparative Study. Machines (2018).

[3] J. Shi, J. Xu, J. Sun & Y. Yang., Iterative Learning

Control for Time-Varying Systems Subject to Variable

Pass Lengths: Application to Robot Manipulators.

IEEE Transactions on Industrial Electronics, vol. 67,

no. 10, pp. 8629-8637.

[4] Piltan Farzin, Mehrara Saleh, Bayat Reza & Rahmdel

Sajad, Design New Control Methodology of Industrial

Robot Manipulator: Sliding Mode Baseline

Methodology. International Journal of Hybrid

Information Technology. 5. 41-54.

[5] Galindo R. & Unifo Y., Regulación No-Lineal Óptima

de Sistemas Dinámicos Euler-Lagrange, IEEE (2021).

[6] Tyutikov, V.V., Krasilnikyants, E.V. & Varkov A.A.,

Robot Manipulator Control System with Dynamic

Moment Compensation. Autom Remote Control 80,

189–199 (2019).

[7] Cruz-Zavala, Emmanuel & Nuño, Emmanuel &

Time Control of Robot Manipulators. International

Journal of Control. 1-19.

[8] E. Nikolov, V. Karlova-Sergieva & B. Grasiani.,

Design control system of robot-manipulator {FANUC}

M-430iA/4FH. {IOP} Conference Series: Materials

Science and Engineering.

[9] H. Goldstein, C. Poole and J. Safko, Classical

mechanics, 3ra ed. San Francisco: Addison Wesley,

2002.

[10] M. Spong, S. Hutchinson and M. Vidyasager, Robot

modeling and control, 1ra ed. Hoboken, N.J.: John

Wiley & Sons, 2006 pp. 113-127.

[11] K. Ogata, Ingeniería de Control Moderna (5ta ed.).

Madrid: Pearson (2010).

ISSN 2448-5551 MT 34 Derechos Reservados © 2021, SOMIM