% CONTROL MANUAL Y AUTOMÁTICO DE BALANCEO DE UN PÉNDULO % % Este script ejecuta la simulación de un péndulo cuyo balanceo puede ser controlado manualmente (accionando % una barra deslizante o "slider") o de forma automática, mediante lazos de control PID. El control % automático incorpora un lazo con control del ángulo del péndulo para que se sitúe en % posición vertical. % % El script puede ser utilizado tanto como un "equipo de prácticas", en el que pueden % estudiarse diversos conceptos de control, así como servir de base para que el profesor (o el % alumno) elaboren otros ejemplos interactivos totalmente distintos mediante el procedimiento % de "cortar y pegar". % % % Fecha: 2006-11-02 (última modificación: 2008-04-04) % Autor: Ignacio Díaz % Area de Ingeniería de Sistemas y Automática % Universidad de Oviedo clear; close all; clc; disp('Instrucciones:'); disp('- Pulsar ''0'' para desactivar el sistema de control'); disp('- Pulsar otra tecla para restaurar el sistema de control'); disp(' '); disp('Control manual: actuar sobre la barra deslizante para modificar la base del péndulo.'); disp('Control automático: actuar sobre la barra deslizante para modificar la base del péndulo.'); disp(' En este modo, puede apreciarse cómo la acción de control'); disp(' hace que el péndulo oscile mucho menos que en el control manual'); pause(2); % PARAMETROS DE LA SIMULACION lineal = 0; % PARÁMETROS DEL PÉNDULO l = 2; % Longitud del péndulo m = 1; % Masa del péndulo J = m*l^2; % Momento de inercia referido al eje b = 1; % Coeficiente de fricción g = 10; % Aceleración de la gravedad % PARÁMETROS DEL CARRO Bc = 10; % Coeficiente de fricción del carro Mc = 10; % Masa del carro % ESTADO INICIAL DEL PÉNDULO x = [0.0;0;0;0]; % Para que se vea el efecto del control, empezamos % con el péndulo casi vertical (theta = pi +/- "algo") posc_1 = 0; posc_2 = 0; % DEFINICIÓN DE UN "SLIDER" PARA CONTROLAR MANUALMENTE EL PÉNDULO f = figure(1); set(f,'pos',[426 697 700 700]); h = uicontrol('style','slider','pos',[20 20 680 20],'min',-4,'max',4); Tm = 0.03; % Período de muestreo xmin = -5; xmax = +5; r = 0; % DISEÑO DEL SISTEMA DE CONTROL % Modelo linealizado en EE A = [0 1 0 0; % x(1) = th -g/l -b/(m*l^2) 0 Bc/(Mc*l); % x(2) = w 0 0 0 1; % x(3) = x 0 0 0 -Bc/Mc]; % x(4) = v B = [0; -1/(Mc*l); 0 1/Mc]; C = [1 0 0 0; % y(1) = th 0 0 1 0]; % y(2) = x D = [0;0]; % DISEÑO DEL CONTROLADOR N = inv([A B;C(2,:) D(2)])*[0;0;0;0;1]; % ASIGNACIÓN DE POLOS % K = place(A,B,-[1 2 3 4]); % LQR rho = 5*diag([.2 10 10 1]); Q = rho*ones(4,4)*rho; K = lqr(A,B,rho*Q,1), Nx = N(1:4); Nu = N(5); k = 2; % Empezamos en k=2 para tener acceso al menos a dos muestras anteriores for kk = 1:1000, k = k + 1; % SELECCIÓN DE CONTROL MANUAL / CONTROL AUTOMÁTICO tecla = get(f,'currentchar'); % tecla = '0'; switch tecla case '0' posc = get(h,'value'); u = 0.25*(posc-2*posc_1+posc_2)/Tm^2; posc_1= posc; posc_2= posc_1; otherwise r = get(h,'value'); % r = 1; % Poner esto para simular un escalón u = -K*(x-Nx*r)+Nu*r; end % PERTURBACION % if (kk>100)&(kk<110) % u = u + 100; % end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % ECUACIONES EN ESPACIO DE ESTADOS (NO LINEALES) DEL PÉNDULO % Ecuación de estados if lineal % Modelo lineal x = x + Tm*(A*x+B*u); else % Modelo no lineal (más realista) x(1) = x(1) + Tm*x(2); % x(1) = th x(2) = x(2) + Tm*(-(g/l)*sin(x(1)) - b/(m*l^2)*x(2) + Bc/(Mc*l)*x(4)*cos(x(1)) - 1/(Mc*l)*u*cos(x(1)) ); % x(2) = w x(3) = x(3) + Tm*x(4); % x(3) = x x(4) = x(4) + Tm*(-Bc/Mc*x(4) + (1/Mc)*u); % x(4) = v end % Ecuación de salida y(1) = x(1); y(2) = x(3); th = x(1); % angulo de la carga pos = x(3); % posición del carro %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % REPRESENTACIÓN GRÁFICA DE LA SIMULACIÓN figure(1); plot(pos,0,'.'); hold on; p1 = pos; p2 = pos + l*exp(j*(th-pi/2)); line(real([p1,p2]),imag([p1,p2])); plot(real(p2),imag(p2),'.','markersize',20); plot(r,0,'r+'); hold off; % Sugerencia: pueden dibujarse también otras flechas indicando en tiempo real las fuerzas reales % o de inercia que actúan en cada elemento del sistema % Centrado automático de la perspectiva sobre el objeto de control if pos>xmax-0.5 xmin = xmin + 0.1; xmax = xmax + 0.1; elseif pos