Vous êtes sur la page 1sur 6

Implementacion y Control de un Mecanismo de

Dos Grados de Libertad Tipo Pendulo Invertido.


Oscar E. Alvarado Martnez. Ricardo E. Campa Cocom. E. Javier Ollervides Vazquez.
Instituto Tecnologico de La Laguna
Torreon Coahuila 27000, Mexico
oscareduardoalvarado@gmail.com
Telefono: (52)-871-7129404

ResumenEn este artculo se discuten la implementacion tal manera que actualmente es empleado para explicar
y control de un prototipo mecatronico tipo pendulo invertido diversas ideas emergentes en el campo del control no lineal.
de dos grados de libertad, el cual esta compuesto por un
pendulo esferico y un mecanismo de cuatro eslabones con
restricciones holonomicas. Se considera al mecanismo como Aunque existe un numero considerable de sistemas
dos pendulos de un grado de libertad desacoplados y se utiliza basados en pendulos invertidos, todos ellos tienen un
el modelo dinamico linealizado para disenar
un controlador objetivo de control similar: llevar el centro de masa del
PD para cada uno de ellos. Las ganancias del controlador pendulo a su equilibrio inestable y/o mantenerlo ah a
son obtenidas mediante la tecnica LQR. La efectividad del pesar de perturbaciones externas.
controlador utilizado es mostrada mediante experimentacion.

Keywords: Metodo de Control LQR, Sistemas Desacopla- Entre los pendulos invertidos mas comunes son el
dos. sistema carro-pendulo (Aguilar-Ibanez C. y Gutierrez
F., 2005) y el pendulo rotatorio (Sukontanakarn V. &
I. .
I NTRODUCCI ON Parnichkun M., 2009) conocido tambien como pendulo de
Una gran cantidad de temas relacionados con cinematica, Furuta (Astrom, K.J. y K. Furuta, 2000).
dinamica, analisis de sistemas no lineales y teora de
control, entre otros, pueden presentarse ante los estudiantes Un caso especial es el pendulo invertido de dos
como conceptos matematicos puramente abstractos y grados de libertad, comunmente conocido como pendulo
carentes de significado fsico. esferico. Consiste en una varilla delgada montada en
una base movil, unida a un plano horizontal a traves de
La aplicacion de dichos conceptos en un prototipo de una articulacion universal. La base movil, considerada
laboratorio provee una motivacion extra a los estudiantes, idealmente como un punto, puede deplazarse libremente
ademas de dar un significado practico a dichos conceptos. bajo la accion de una fuerza planar. Debido a la accion de
la gravedad el pendulo tiene dos equilibrios, uno estable en
En los u ltimos anos el desarrollo de prototipos la posicion vertical hacia abajo y uno inestable hacia arriba.
mecatronicos ha tenido un gran auge, ya que perminte a
los estudiantes tener un balance adecuado entre la teora
matematica y la experimentacion practica. II. DEL S ISTEMA T IPO P E NDULO
D ESCRIPCI ON
I NVERTIDO DE D OS G RADOS DE L IBERTAD .
El objetivo de este articulo es construir un dispositivo El sistema estudiado consiste en una base fija con
mecatronico tipo pendulo invertido para evaluar de forma dos actuadores(Q1 y Q2), sobre la cual esta montado
practica algoritmos de control. un mecanismo de cuatro eslabones con restricciones
holonomicas, que a su vez, soporta la articulacion universal
Siendo un sistema mecanico subactuado, inherentemente del pendulo esferico. Se utilizan encoders o pticos
inestable en lazo abierto y con una dinamica altamente no para medir la posicion angular de los actuadores y la
lineal, un pendulo invertido es una aplicacion mecatronica inclinacion de la varilla . La posicion de la plataforma
muy popular. movil es determinada mediante la cinematica inversa
del mecanismo de cuatro eslabones. Las velocidades son
El pendulo invertido ha sido utilizado mundialmente obtenidas numericamente por diferenciacion.
para probar numerosas tecnicas de control. Originalmente
fue usado para ilustrar ideas sobre teora de control El sistema completo (el pendulo mas la base movil)
lineal, sin embargo, su naturaleza no lineal ha servido posee cuatro grados de libertad, los cuales son: la posicion
para mantener su utilidad a traves de los anos, de de la base movil en el plano horizontal(X,Y), determinada
q1 = 0 grados y q2 = 90 grados.

Figura 2. Configuracion inicial del mecanismo de cinco barras.

A continuacion se describen los diferentes elementos


utilizados para la implementacion del sistema fsico.

Se utilizaron el 2 DOF Joint de la compania Quanser


(Quanser:2DOF Joint User Manual, ) como articulacion
universal para el pendulo esferico y el 2 DOF Robot
Figura 1. Mecanismo tipo pendulo invertido de dos grados de libertad.
(Quanser:2DOF Robot User Manual, ) del mismo
fabricante como mecanismo de cinco barras.
por los a ngulos de los eslabones q1 y q2 , y los a ngulos Dos servomotores de CD 2224U006SR MICROMO con
de inclinacion pitch() y roll() de la varilla. El reductor de velocidad fueron empleados para transmitir
objetivo del control es mantener el pendulo en su equilibrio
potencia al mecanismo. Los motores fueron controlados
inestable, con la base movil del pendulo en el punto de mediante dos servoamplificadores BE15A8 de Advanced
origen del plano horizontal. Motion Controls, que funcionan mediante modulacion de
ancho de puslo (PWM), operados en modo velocidad.Se
La figura 1 muestra una representacion esquematica del utilizaron dos filtros FC10100 del mismo fabricante para
mecanismo estudiado. incrementar la inductancia de los motores y hacerlos
compatibles con los servoamplificadores.
El sistema de pendulo esferico pertenece a la clase VIII
(Olfati-Saber R., 2001)de sistemas subactuados que poseen La interfaz del sistema para realizar el control en tiempo
menos entradas de control que grados de libertad. real fue creada utilizando Matlab/Simulink en conjunto
con Real Time Windows Target.
Como se menciono anteriormente, la base movil del
pendulo esferico esta montada sobre un mecanismo de La comunicacion Sistema-Computadora se realizo a
cuatro eslabones, comunmente conocido como mecanismo traves de una tarjeta adquisitora Sensoray Modelo 626.
de cinco barras. Este tipo de mecanismo ha sido objeto
de numerosos estudios (Mundo D., Gatti G. y Dooner
D.B, 2008) debido a su relativa simplicidad y a la riqueza III. M ODELADO DEL S ISTEMA .
matematica que ofrece su analisis. El mecanismo de cinco El modelo no lineal del pendulo esferico ha sido
barras es un robot paralelo, por lo que puede presentar estudiado en (Liu G., NeSic D. y Mareels I, 2008) y
diferentes configuraciones para una postura determinada. en (Olfati-Saber R., 2001). En (Yang R., Kuen Y. y Li
La configuracion utilizada en este artculo fue seleccionada Z., 2000) se presenta un modelo lineal que considera
arbitrariamente tomando como base las ecuaciones de la al sistema como dos pendulos invertidos desacoplados.
cinematica inversa y el espacio de trabajo del mecanismo Esto es posible siempre y cuando la desviacion de la
fue delimitado de tal forma que se asegura que no existe varilla con respecto a la posicion vertical sea pequena
un cambio de configuracion mientras el sistema esta en (+/- 10 grados). En este artculo con fines de evaluar el
operacion. prototipo fsico implementado se utilizo el modelo obtenido
La configuracion inicial del mecanismo de cinco barras en (Quanser: Rotary Experiment#8 Inverted Pendulum
es mostrada en la figura 2. Los valores iniciales son: Control-Instructor Manual, ) . De esta forma el sistema
TABLA I
puede ser tratado como dos pendulos invertidos rotatorios
PAR AMETROS DEL M ECANISMO T IPO P E NDULO I NVERTIDO .
de un grado de libertad, donde el actuador Q1 controla el
mp 0,127kg
a ngulo roll() y el actuador Q2 controla la inclinacion lp 0,3365m
del a ngulo pitch(). El acoplamiento dinamico entre los L 0,127m
dos pendulos es despreciado. Jp 0,000441kgm2
Jr 0,00597kgm2
g 9,81m/s2
Una vista esquematica de uno de estos pendulos se
puede observar en la figura 3.

(t)= Inclinacion angular de la varilla respecto al eje x


[rad].

q1 (t)= Velocidad angular del eslabon q1 [rad/s].

(t)=
Velocidad angular de la varilla respecto al eje x
[rad/s]

Las matrices del sistema linealizado estan dadas por:



0 0 1 0
0 0 0 1
m2p l2p Lg

A=
0 Jr Jp mp l2 Jr +Jp mp L2 0 0 .

p
l m g(J +m L2 )
0 Jr Jpp mpp l2 Jrr +Jppmp L2 0 0
p


0

0

B= Jp +mp l2p .
Jr Jp mp l2p Jr +Jp mp L2


l m L
Jr Jp mp lp2 Jrp+Jp mp L2
p

Figura 3. Pendulo desacoplado de un grado de libertad.


Donde:
La obtencion del modelo matematico de un pendulo
rotatorio, a partir de las ecuaciones de Lagrange, ha mp =Masa de la varilla [kg].
sido extensamente tratada en la literatura as que no
es necesario repetir esta tarea. El modelo empleado es lp =Longitud de la varilla [m].
linealizado alredor del punto de operacion, en este caso el
equilibrio inestable, para obtener un sistema en espacio de L=Longitud del eslabon [m].
estados de la forma mostrada en 1:
Jp =Momento de inercia de la varilla[kgm2 ].
x
= Ax + Bu. (1) Jr =Momento de inercia del eslabon[kgm2 ].
t
g=Fuerza de gravedad [kgm/s2 ].
El vector de estados queda definido como:
Los parametros del mecanismo de cuatro eslabones y el
pendulo esferico fueron tomados de (Quanser:2DOF Robot
User Manual, ) y (Quanser:2DOF Joint User Manual, ) y
x = [q1 (t) (t) q1 (t) (t)]

se muestran en la Tabla I.
Donde:
IV. E STRATEGIA DE C ONTROL
q1 (t)=Posicion angular del eslabon q1 [rad]. El objetivo de control es mantener el pendulo esferico
en el equilibrio inestable, para ello es necesario controlar
individualmente los dos pendulos de un grado de libertad.
Debido a la similitud de los dos controladores, en lo T
y(t) = [(t) (t)]
sucesivo se hara referencia solo a uno de ellos.

Idealmente, el control de los estados (t) y (t) sera


Y los estados controlados con el vector:
suficiente para estabilizar el pendulo,sin embargo, debido a
las limitaciones del espacio de trabajo q1 (t) y q1 (t) deben
ser controlados. //
z(t) = [q1 (t) q1 (t)]T
El controlador debe ser disenado de tal forma que la
respuesta del a ngulo de inclinacion roll () tenga una
dinamica mucho mas rapida que la posicion del a ngulo
q1 . Sin embargo, una dinamica demasiado lenta de q1 La ley de control es implementada mediante una matriz
provocara que el mecanismo sobrepase el lmite del espacio de ganancias
de trabajo. K = [Kq1 K Kq1 K ]

Con el sistema linealizado y en forma de espacio de tal como se muestra en la ecuacion 2


estados, es posible utilizar tecnicas como la realimentacion
de estados para disenar un controlador o ptimo. u = K(x xdeseada ) (2)

El controlador utilizado es un Controlador PD, cuyas


ganacias son calculadas utilizando la metodologa de un Se define:
Regulador Cuadratico Lineal (LQR).
xdeseada = [0 0 0 0]T .
La metodologa LQR es una tecnica de diseno que
proporciona ganancias practicas para el controlador, por lo que la dinamica del sistema en lazo cerrado queda
siempre y cuando se cumplan dos requisitos basicos: definida por la ecuacion 3.

1. El sistema debe de estar en la forma de espacio de x = (A BK)x + BKxdeseada (3)


estados.

2. Todos los estados deben de estar disponibles para el


controlador. Una forma efectiva de calcular ganancias para un
controlador LQR es utilizando la instruccion LQR(A,B,Q,R)
Un esquema de control para el pendulo de un grado de de Matlab.
libertad, utilizando LQR, es mostrado en la figura 4
Este comando utiliza las matrices A y B del sistema,
z(t) ademas de unas matrices definidas por el usuario Q y
u(t)
Controlador Pndulo I DOF R conocidas como matrices de peso para minimizar la
y(t) funcion costo definida por:

1
Z
Figura 4. Diagrama de Control LQR.
J= (xT Qx + uT Ru)dt (4)
2 0
Donde:

u(t)=Senal de control. Las matrices de peso utilizadas para calcular las


ganancias del controlador fueron las siguientes:
y(t)=Estados medidos. Son los estados que se quieren
llevar a cero en la menor cantidad de tiempo posible.
1 0 1 0
0 25 0 1
z(t)=Estados controlados. Son los estados que se quieren Q= .
0 0 1 0
mantener dentro de un intervalo controlado.
0 0 0 20
En el caso del sistema estudiado, los estados medidos y
son definidos por el vector:
R=1
Dando como resultado las siguiente ganancias para el La figura 6 muestra la accion conjunta de los dos con-
controlador PD: troladores sobre la varilla. El pendulo parte del reposo en
el equilibrio inestable. Idealmente, el pendulo debera de
permancer ah, sin embargo en la practica esto nunca ocurre.
K = [1 50,4076 1,4146 9,6964 ]T Despues de cierto tiempo se alcanza el objetivo de control:
la permanencia del pendulo en el punto de equilibrio. Una
V. R ESULTADOS DE .
E XPERIMENTACI ON perturbacion externa es aplicada para comprobar la robustez
Debido a las no linealidades que no fueron consideradas del controlador.
en el modelo dinamico, las ganancias adecuadas tuvieron
que ser encontradas mediante experimentacion. 10
Las ganancias utilizadas fueron: 8
6

Posicin [grados]
4 Perturbacin
Para el controlador del servomotor Q1. 2
Externa

0
2

K = [3, 40, 1, 1]T 4


6
Delta [grados].
8 Epsilon [grados].
10
0 5 10 15 20 25 30
Tiempo [s]
Para el controlador del servomotor Q2.
Figura 6. Desviaciones angulares  y .

K = [1, 40, 1, 1,5]T


Los errores maximos de los a ngulos de inclinacion de la
varilla  y son mostrados en las figuras 7 y 8 respectiva-
mente.
El prototipo fsico utilizado para realizar los experimen-
tos es mostrado en la figura 5. 0.1

0.05
Posicin [rad]

0.05

0.1
Error Epsilon [rad]

0.15
0 5 10 15 20 25 30
Tiempo (s)

Figura 7. Error 

0.1

0.05
Posicin [rad]

0.05

Figura 5. Prototipo fsico del Pendulo Invertido. 0.1

Error Delta [rad].

El tiempo total del experimento fue de 30 segundos. 0.15


0 5 10 15 20 25 30
El periodo de muestreo empleado fue de 1 milisegundo Tiempo [s]

y se utilizo el ode5 (Dormand-Prince) para obtener las


Figura 8. Error
velocidades mediante diferenciacion. Debido a lo ruidoso
de estas senales, se utilizaron filtros pasa-bajas de segundo
o rden para obtener una mejor estimacion de la velocidad. Los voltajes de control aplicados a los servomotores son
mostrados en las figuras 9 y 10
3 Quanser:2 DOF Joint User Manual
Quanser: Rotary Experiment#8 Inverted Pendulum Control-Instructor
2
Manual.
Seal de Control [V]

Disponible en linea en:


1
http : //www.meqtronics.com/public/requests/QCI T EM P/2DOF IP/
0

2
Seal de Control Q1 [V]

3
0 5 10 15 20 25 30
Tiempo (s)

Figura 9. Voltaje de Control Servomotor Q1

2
Seal de Control Q2 [V]

2
Seal de Control Q2 [V]

3
0 5 10 15 20 25 30
Tiempo (s)

Figura 10. Voltaje de Control Servomotor Q2

VI. C ONCLUSIONES Y T RABAJO F UTURO .


Se cumplio el objetivo principal del artculo que era
construir un prototipo fsico tipo pendulo invertido con un
costo mucho menor a los que existen actualmente en el
mercado.
Se probo experimentalmente un controlador lineal tipo
PD para evaluar el correcto funcionamiento del prototipo
implementado.
Como trabajo futuro se pretende utilizar el prototipo
fsico para evaluar controladores no lineales.

R EFERENCIAS
Aguilar-Ibanez C. y Gutierrez F.(2005)Lyapunov based controller for the
inverted pendulum cart system. Nonlinear Dynamics 40. (2005) 367-
374.
Astrom, K.J. y K. Furuta (2000) Swinging up a pendulum by energy
control. Automatica 36. (2000) 287-295.
Liu G., NeSic D. y Mareels I.Non local Stabilization of a Spherical
Inverted Pendulum. International Journal of Control 81:7 1035-1052
2008.
Mundo D., Gatti G., y Dooner D.B (2008). Optimize five-bar linkages
with non-circular gears for exact path generation. Mechanism and
c
Machine Theory 44 (2009).751-760 2008 Elsevier
Olfati-Saber R.Non Linear Control of Underactuated Mechanical Systems
with Application to Robotics and Aerospace Vehicles. Ph. D. Tesis,
(2001) Massachusetts Institute of Technology, Department of Elec-
trical Engineering and Computer Science.
Sukontanakarn V. & Parnichkun M.(2009). Real-Time Optimal Con-
trol for Rotary Inverted Pendulum. American Journal of Applied
c
Sciences.1106-115 2009 Science Publications
Yang R., Kuen Y. y Li Z.Stabilization of A 2-DOF Spherical Pendulum
on X-Y Table.Proc. Of the 2000 IEEE International Conference on
Control Applications.(2000) 724-729, USA.
Quanser:2 DOF Joint User Manual