Documentos de Académico
Documentos de Profesional
Documentos de Cultura
Bachelorcastanos PDF
Bachelorcastanos PDF
Invertido
3 de febrero de 2003
2
Índice general
1. Introducción 7
1.1. Descripción del péndulo invertido . . . . . . . . . . . . . . . . 7
1.2. Control propuesto . . . . . . . . . . . . . . . . . . . . . . . . 8
1.3. Objetivos . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9
5. Conclusiones 49
5.1. Sobre la implementación . . . . . . . . . . . . . . . . . . . . . 49
5.2. Sobre los algoritmos de control . . . . . . . . . . . . . . . . . 49
5.3. Consideraciones finales . . . . . . . . . . . . . . . . . . . . . . 50
3
4 ÍNDICE GENERAL
Bibliografı́a 51
5
6 ÍNDICE DE FIGURAS
Introducción
7
8 CAPÍTULO 1. INTRODUCCIÓN
Perifericos
Convertidor Actuador Motor
D/A
Algoritmo de Control
1.3. Objetivos
En general, los sistemas de control requieren un tiempo de muestreo
preciso. Esta precisión se consigue fácilmente con un microcontrolador o un
DSP, pero si ha de usarse una PC, se requiere normalmente de un sistema
operativo diseñado especı́ficamente para trabajar en tiempo real. Cualquiera
de estas dos opciones implica un esfuerzo para pasar del diseño a la imple-
mentación. Uno de los objetivos es proporcionar una plataforma sencilla
para que futuros estudiantes de control puedan probar sus diferentes al-
goritmos. La sencillez de dicha plataforma radicarı́a principalmente en la
separación del proceso de diseño y el proceso de implementación. Para veri-
ficar la validez de dicha plataforma, naturalmente será necesario diseñar un
control que estabilice al péndulo.
El sistema, además de ser contener puntos inestables, es no lineal y pre-
senta singularidades. Dado que al estabilizar el péndulo se espera que las
variaciones en el ángulo sean pequeñas, se puede utilizar un modelo linea-
lizado para simplificar el diseño del control que lo estabiliza. El algoritmo
de estabilización requiere que el péndulo se encuentre en la posición vertical
superior, por lo que se propone como objetivo adicional, diseñar un algorit-
mo que levante al péndulo y un conmutador entre el esquema que lo levanta
10 CAPÍTULO 1. INTRODUCCIÓN
y el que lo estabiliza.
Capı́tulo 2
11
12 CAPÍTULO 2. MODELO MATEMÁTICO DEL PÉNDULO
ls
M1 g Φ
M0 g
F Fric
N r
Las ecuaciones 2.1 y 2.2 son la segunda ley de Newton para las compo-
nentes horizontal i y vertical j de la fuerza F y la aceleración a experimenta-
da por el cuerpo rı́gido de masa m. La ecuación 2.3, derivada también de la
segunda ley de Newton, establece que la suma de momentos M de las fuerzas
que actúan sobre un cuerpo rı́gido alrededor de un punto G cualquiera, es
igual al momento de inercia I por la aceleración angular α (con dirección g)
alrededor del cuerpo rı́gido.
En la Figura 2.1 se encuentra el diagrama de cuerpo libre del péndulo
invertido. Sobre el péndulo invertido actúan F , la fuerza de fricción F ric,
los pesos del péndulo y el carro, y la reacción normal N que ejerce el riel
sobre el carro (tercera ley de Newton) M 0 es la masa del carro, M1 la del
péndulo, y g es la aceleración ejercida por la tierra.
Para simplificar el análisis, se puede dividir el péndulo invertido en dos
cuerpos: el carro y el péndulo.
En el lado izquierdo de la Figura 2.2 se puede observar, además del peso
del péndulo, las fuerzas de reacción H y V que actúan sobre su articulación.
Para obtener la aceleración horizontal del péndulo, es necesario tomar en
cuenta que la posición horizontal de su centro de gravedad depende de r y
de Φ. Si se aplica 2.1 al péndulo, se tiene
d2
H = M1 r − l s sen Φ (2.4)
dt2
2.1. ECUACIONES DIFERENCIALES 13
M1 g
Φ V
H
ls
F M0 g Fric
H
N r
V
Figura 2.2: Diagramas de cuerpo libre del péndulo (izquierda) y del carro
(derecha)
y la ecuación 2.2 da
" #
d2
V = M1 l s cos Φ +g (2.5)
dt2
d2 r
F − F ric − H = M0 (2.12)
dt
La fuerza de fricción F ric es proporcional a la velocidad del carro, y F r es
la constante de proporcionalidad. Desarrollando F ric y sustituyendo H
M0 r̈ + Fr ṙ + M1 r̈ + M1 ls Φ̇2 sen Φ − Φ̈ cos Φ = F (2.13)
Si hacemos M = M0 + M1 , entonces
M r̈ + Fr ṙ + M1 ls Φ̇2 sen Φ − Φ̈ cos Φ = F (2.14)
Equilibrio Movimiento
Fc
Fm
Fk
ẋ = Ax + Bu (2.15)
y = Cx + Du (2.16)
x4 Φ̇
el vector u tiene un elemento: la fuerza aplicada al carro.
u=F
El péndulo invertido consta de tres sensores, uno para r, otro para Φ y
otro para ṙ. Por lo que se define a la salida del sistema como
x1
y = x2 ,
x3
con lo que se pueden determinar las matrices C y D fácilmente
1 0 0 0
C = 0 1 0 0 , D = 0.
0 0 1 0
Para encontrar A y B es necesario expresar las ecuaciones 2.11 y 2.14
en la forma
ẋ = f (x, u) (2.17)
para ẋ1 y ẋ2 se tiene
ẋ1 = f1 (x, u) = x3 (2.18)
ẋ2 = f2 (x, u) = x4 (2.19)
De la ecuaciones 2.11 y 2.14, haciendo α = M 1 ls
αẋ4 cos x2 − αx24 sen x2 − Fr x3 + u
ẋ3 = (2.20)
M
αẋ3 cos x2 + αg sen x2 − Cx4
ẋ4 = (2.21)
Θ
sustituyendo 2.21 en 2.20 y despejando ẋ 3
α2 g sen x2 cos x2 Fr Θ
ẋ3 = f3 (x, u) = − x3 −
ΘM − α2 cos2 x2 ΘM − α2 cos2 x2
αC cos x2 α sen x2
− x4 − x2 + (2.22)
ΘM − α2 cos2 x2 ΘM − α2 cos2 x2 4
Θ
+ u
ΘM − α2 cos2 x2
2.2. ECUACIÓN DE ESTADO 17
n
X
ẋi = aij xj + bi u
j=1
4
" #
X ∂f3 (x, u) ∂f3 (x, u)
ẋ3 ≈ ẋ3 = ∆xi + ∆u (2.23)
∂xi ∂u
i=1 x=0,u=0 x=0,u=0
Los elementos n11 a n33 son los factores que relacionan las cantidades
eléctricas con las mecánicas. Como Φ̇ no se puede medir directamente, ha
de ser estimado más tarde. El elemento n 44 , se puede, entonces, elegir arbi-
trariamente. Por simplicidad se eligió n 44 = n22 s.
Si se sustituyen las ecuaciones 2.36 y 2.37 en 2.35 se puede ver que
y al despejar ẋn
ẋn = NAN−1 xn + NBKf us (2.40)
de donde se puede obtener la matriz normalizada del sistema
An = NAN−1 (2.41)
Bn = NBKf (2.42)
Calculando
0 0 1 0
0 0 0 1
A =
0 0.883 −1.916 −0.0056
(2.43)
0 21.53 −3.868 −0.1369
0
0
B =
0.30896
(2.44)
0.62382
y
0 0 −1.95 0
0 0 0 1
An =
0 −0.129 −1.916 0.00082 (2.45)
0 21.53 26.461 −0.1369
0
0
Bn =
−6.137
(2.46)
84.778
Finalmente,
ẋn = An xn + Bn u
yn = Cxn (2.47)
ωs
20 < < 40 (2.48)
ωb
det(sI − An ) = 0 (2.49)
s1 = 0
s2 = 4.52
s3 = −4.84
s4 = −1.73
⇒ ωb ≈ 4.84 [rad/s] (2.50)
23
24 CAPÍTULO 3. ESTABILIZACIÓN DEL PÉNDULO
Planta
Péndulo Sensores
u xn(k) y
xn (k+1)=Ad xn(k)+ B d u (k) y(k)= C xn(k)
Controlador
+ x
−K Σ Observador
−
Ley de
control
Referencia
K(k) = K
Respuesta en frecuencia
1
0
Posición [dB]
−1
−2
−3 ωb
−4
−5
0 2 4 6 8
4
Posición Angular [dB]
3
2
1
0 ωb
−1
−2
−3
−4
−5
0 2 4 6 8
1
0
Velocidad [dB]
−1
−2
−3 ωb
−4
−5
0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 5.5 6 6.5 7 7.5 8
Frecuencia [rad/s]
x n (0)
x (0)
Los dos últimos elementos son conocidos y pueden considerarse como en-
tradas a la dinámica de xb . Si se reordena la parte que describe la dinámica
de xa se tiene que
Las ecuaciones 3.9 y 3.10 tienen la misma forma que las ecuaciones 2.57
y 2.58 respectivamente, por que lo se puede utilizar la ecuación 3.6 si se
hacen las siguientes sustituciones:
xn ← xb
Ad ← Abb
Bd u(k) ← Aba xa (k) + Bb u(k)
yn (k) ← xa (k + 1) − Aaa xa (k) − Ba u(k)
Cd ← Aab
y si se hace
z(k) = x̄b (k) − Lr xa (k) (3.23)
0.04979 0
AL = Abb − Lr Aab =
0 0.04979
−0.20056 −29.92 6.8222
FL = AL Lr + Aba − Lr Aaa =
7.8037 0.00075 −0.2492
0
BL = B b − L r Ba =
0.04979
y(k) BL
xB(k)
z(k) z(k+1) +
+ +
Σ CL Z
−1 Σ FL
+ +
AL
VL
1 0 0
0 1 0
I
VL = = 0 0 1
Lr
0.21107 0.31814 −7.1667
−0.00821 0.00031 0.2781
0 0
0 0
CL =
0 0
1 0
0 1
−5
60
Posición [cm]
40
20
−20
0 0.5 1 1.5 2 2.5 3 3.5 4
5
Posición angular [º]
−5
−10
0 0.5 1 1.5 2 2.5 3 3.5 4
Tiempo [s]
2
Si bien el control óptimo es bastante noble en el sentido de que es insensible a las varia-
ciones de los parámetros del sistema, requiere un tiempo de muestreo preciso, determinado
por hardware, no por software.
3.2. IMPLEMENTACIÓN DEL CONTROL 33
10
−20
−30
0 0.5 1 1.5 2 2.5 3 3.5 4
2
Fricción [N]
Fricción estimada
1
−1 Fricción
−2
0 0.5 1 1.5 2 2.5 3 3.5 4
Tiempo [s]
y(k) BL
xB(k)
z(k) z(k+1) +
+ +
Σ CL Z
−1 Σ FL
+ +
AL
VL
LQR simulado
10
−10
0 5 10 15 20 25
50
Posición [cm]
0
−50
0 5 10 15 20 25
10
Posición Angular [º]
−5
−10
0 5 10 15 20 25
Tiempo [s]
LQR
−10
50
Posición [cm]
−50
Posición Angular [º]
10
−5
−10
5 10 15 20 25 30
Tiempo [s]
−5
−10
0 5 10 15 20 25 30
Posición [cm] 60
40
20
−20
0 5 10 15 20 25 30
10
Posición Angular [º]
−5
−10
0 5 10 15 20 25 30
Tiempo [s]
Figura 3.10: Simulación del algoritmo de control usando LQR sin estimar la
fricción
−5
−10
40
Posición [cm]
20
−20
−40
5
Posici´on angular [º]
−5
−10
0 5 10 15 20 30
tiempo [s]
Figura 3.11: Resultados del algoritmo de control usando LQR sin estimar la
fricción
3.3. ANÁLISIS DE DATOS Y RESULTADOS 37
−1
0 1 2 3 4 5 6 7
2
yn [v], Q2
−2
−4
0 1 2 3 4 5 6 7
2
Posición
0 Posición Angular
yn [v], Q3
Velocidad
−2 Velocidad Angular
−4
−6
−8
0 1 2 3 4 5 6 7
Tiempo [s]
Escalón
d2 x(t)
a1 = −a0 x(t) (4.1)
dt2
donde a0 y a1 son constantes, se dice que el sistema es un oscilador armónico.
La solución a la ecuación diferencial es una función cosenoidal
39
40 CAPÍTULO 4. LEVANTAMIENTO DEL PÉNDULO
R 1er cuadrante
2do cuadrante
3er cuadrante 4to cuadrante
ls
M1 g cos Φ
M1 g
M1 g senΦ
Carro
Riel
Φ
Pendulo
s1,2 = 0
s3,4 = 0 ± 4.6401j. (4.12)
4.1.2. Excitación
En la sección anterior se calculó la frecuencia de resonancia para el sis-
tema linealizado. La frecuencia de resonancia real seguramente es distinta.
Una posibilidad es medir experimentalmente la frecuencia natural del péndu-
lo y aplicar una fuerza que estuviera de acuerdo con la ecuación 4.5. Este
enfoque plantea dos problemas principales: uno, que la frecuencia natural del
péndulo seguramente varı́a debido a la fricción seca, dos, aplicar un voltaje
al motor de DC en lazo abierto no permite controlar la posición del carro
y éste podrı́a rebasar la longitud del riel, chocando contra alguno de los
extremos.
Es necesario encontrar la manera de medir dinámicamente la frecuencia
natural y mantener la posición del carro dentro de los lı́mites del riel. Una
4.1. DISEÑO DEL CONTROL 43
−2
40
Posición Angular [º]
20
−20
−40
10
Velocidad Angular [º/s]
−5
−10
0 1 2 3 4 5 6 7 8 9 10
Tiempo [s]
Inicio
No
¿ Velocidad angular
< umbral ?
Si
¿ Angulo en cuadrantes
No 2o3?
Si
Fin
Si se logra que los polos del controlador del carro sean lo suficientemente
rápidos, se puede esperar que la excitación se aproxime a una señal cuadrada.
Dicha señal cuadrada tendrı́a como frecuencia fundamental, la frecuencia
natural del péndulo.
Lo único que falta es un conmutador entre el sistema que levanta el
péndulo y el que lo estabiliza. Como para ángulos menores o iguales a ±10 ◦ se
cumple con buena aproximación que sen θ ≈ θ, se puede utilizar el algoritmo
para levantar el péndulo cuando el ángulo se encuentre fuera de ese rango
y conmutar al control de estabilización cuando el péndulo entre a ese rango
(ver Figura 4.5).
F = F ric + M r̈ (4.13)
4.1. DISEÑO DEL CONTROL 45
No
¿ Φ < 10 ?
Levantamiento del
Si Péndulo
Algoritmo
de control
F Mg Fric
N r
Figura 4.6: Diagrama de cuerpo libre del carro al despreciar el efecto del
movimiento del péndulo
F ric = Fr ṙ (4.14)
por lo que
F = Fr ṙ + M r̈ (4.15)
Siguiendo un procedimiento similar al del capı́tulo 2, se obtiene la si-
guiente ecuación de estado
donde xnc1 es la posición del carro en volts, x nc2 es la velocidad del carro en
volts y
1 −0.05699 0.0048543
Adc = Bdc =
0 0.94866 −0.1644908
1 0
Cdc = Ddc = 0
0 1
46 CAPÍTULO 4. LEVANTAMIENTO DEL PÉNDULO
−5
−10
0 0.5 1 1.5 2 2.5 3 3.5 4
50
Posición del carro [cm]
−50
0 0.5 1 1.5 2 2.5 3 3.5 4
200
Ángulo [º]
100
Conmutación
Figura 4.7: Simulación del conmutador y los controles para levantar y esta-
bilizar el péndulo invertido
−5
−10
13 14 15 16 17 18 19
40
Posición del carro [cm]
20
−20
−40
13 14 15 16 17 18 19
300
Ángulo [º]
200
100
0
13 14 15 16 17 18 19
Tiempo [s]
Conmutación
Conclusiones
49
50 CAPÍTULO 5. CONCLUSIONES
[6] B.C. Kuo. Discrete-Data Control Systems. Prentice Hall. New Jersey,
E.U.A. 1970
51
52 BIBLIOGRAFÍA
Apéndice A
#include "simstruc.h"
#include "amiraADC.h"
/*=====================================*
* Configuration and execution methods *
*=====================================*/
53
54 APÉNDICE A. INTERFACES MATLAB - DAC 6214
ssSetOptions(S, SS_OPTION_EXCEPTION_FREE_CODE);
// general options (SS_OPTION_xx)
} // end mdlInitializeSampleTimes
float Fposicion = 0;
float Fangulo = 0;
float Fvelocidad = 0;
angulo = posicion + 1;
velocidad = angulo + 1;
amiraADC(0,&error,&Fposicion);
amiraADC(0,&error,&Fposicion);
amiraENC(&Fangulo);
amiraADC(2,&error,&Fvelocidad);
amiraADC(2,&error,&Fvelocidad);
*posicion = Fposicion;
*angulo = Fangulo;
*velocidad = Fvelocidad;
if (error == 1)
ssSetErrorStatus(S,"Tiempo de conversin agotado. No se encuentra
A.2. INTERFAZ PARA EL CONVERTIDOR D/A 55
} // end mdlOutputs
/*=============================*
* Required S-function trailer *
*=============================*/
#include "simstruc.h"
#include "amiraADC.h"
/*=====================================*
* Configuration and execution methods *
*=====================================*/
ssSetOptions(S, SS_OPTION_EXCEPTION_FREE_CODE);
// general options (SS_OPTION_xx)
} // end mdlInitializeSampleTimes
float Fcontrol;
unsigned char Udigital;
Fcontrol = *uPtrs[0];
amiraDAC(&Fcontrol); //Escribimos salida analgica
*control = Fcontrol;
if (Udigital == 1)
amiraRST();
if (error == 1)
ssSetErrorStatus(S,"Tiempo de conversin agotado. No se encuentra tarjeta o probablemente
se encuentra en otra direccin de memoria.");
return;
} // end mdlOutputs
/*=============================*
* Required S-function trailer *
*=============================*/
funciones son las que se comunican directamente con la tarjeta DAC 6214.
Para más información se recomienda ver la documentación incluida con el
péndulo [1]
/************************************************************************
* amiraADC.h *
* *
* Rutinas de lectura y escritura de las tarjetas amira
* para el pndulo invertido *
* *
* *
************************************************************************/
#include <conio.h>
#define TIEMPO_LIMITE 100000 //Tiempo lmite para la conversin
#define HWADR 0x210
#define DATR 0x213
#define PI 3.141593
outp(DIR1,SEL_CANL); //Seleccionamos
outp(DIR2,canal); // canal
outp(DIR1,0);
do {
i++;
if(i>TIEMPO_LIMITE) {
*error = 1;
return;
}
outp(DIR1,ESTADO);
busy = inp(DIR2) & 0x0080; //Preguntamos si ha terminado la conversin
} while (busy == 0); //Termina la conversin
return;
}
return;
}