[Matlab] – Script di un sistema PID con disturbo (ode della dinamica complessiva del sistema esteso)

Ecco uno script matlab per modellare un sistema PID, ovvero con un controllo Proporzionale-Derivativo-Integratore, nel quale è presente un disturbo.

Viene utilizzato l’integratore ode45 sulla dinamica complessiva del sistema esteso.

File pid_function.m (contiene la funzione chiamata dalla ode)

function zdot = pid_function(t,z)

global A B K D d Q R % importa le var. globali

%K = [kp kd ki];
[K,P] = lqr(A,B,Q,R); % ricava K

%K(1,3)= 0; % Annulla il termine integrale

u = -K*z;             % ricava u da usare riga sotto
%udot=theta
%zdot= [xdot; udot]

%Dinamica complessiva del sistema esteso
zdot = A*z + B*u + D*d;

end

Il file pid_ode.m invece è il file principale da richiamare per eseguire lo scirpt.

clc           %pulisce il workspace
clear all     %canc variabili
%close all     %chiude finestre e refresha
% commentato per permettere l’hold on delle figure

global A B K D d Q R
%——————————-
% Costanti
I = 1;
A = [0 1 0; 0 0 0 ; 1 0 0 ];
B = [0;1/I; 0];
D = [0; 1; 0];

q1 = 1;
q2 = 1;
q3 = 1;
r = 1;
Q = [q1 0 0;0 q2 0; 0 0 q3];
R = [r];

%——————————
% Intervallo di integrazione
tin = 0;
tfin = 15;
tspan = [tin tfin];

%——————————
% Valori iniziali

d = 10; % disturbo costante
x0 = [2 1];
u0 = [0];
z0=[x0';u0'];

%——————————

esegui_ode=1;  % variabile booleana di debug per ode su zdot
if (esegui_ode==1)

options = odeset(‘abstol’,1e-10,’reltol’,1e-5); %tolleranzaassoluta=10^-10 reltol=10^-5;
[t,z]=ode45(‘pid_function’,tspan,z0,options);

%Calcolo del vettore controllo u
for i=1:length(t)
% u=-K*x
u(i,:)=-K*(z(i,1:3))’;
end

end

disegna_figure=1; % variabile booleana di debug per la stampa delle figure
if (disegna_figure==1)

figure(1);
set(1, ‘NextPlot’, ‘add’)          % imposta i parametri dell’immagine
set(1,’Units’, ‘normalized’)
set(1,’Position’, [.2 0 0.6 0.8])
set(1,’NumberTitle’, ‘off’)
set(1,’Name’, ‘Spostamento e velocità angolare, controllo ed errori’)

subplot(3,1,1); %Spost. Angolare
plot(t,z(:,1))
hold all
grid on
xlabel(‘Tempo’,'fontsize’,8,’fontname’,'tahoma’,'fontweight’,'bold’)
ylabel(‘Spost. Angolare’,'fontsize’,8,’fontname’,'tahoma’,'fontweight’,'bold’)

subplot(3,1,2); %Velocità Angolare
plot(t,z(:,2))
hold all
grid on
xlabel(‘Tempo’,'fontsize’,8,’fontname’,'tahoma’,'fontweight’,'bold’)
ylabel(‘Vel, Angolare’,'fontsize’,8,’fontname’,'tahoma’,'fontweight’,'bold’)

subplot(3,1,3); %Vettore Controllo
plot(t,u)
hold all
grid on
xlabel(‘Tempo’,'fontsize’,8,’fontname’,'tahoma’,'fontweight’,'bold’)
ylabel(‘Controllo’,'fontsize’,8,’fontname’,'tahoma’,'fontweight’,'bold’)

end

K  % => K =    2.4142    2.4142    1.0000

disp(‘Script eseguito.’)

OSSERVAZIONI

Come si vede dal grafico seguente, lo spostamento angolare nel sistema P.I.D. è
nullo (curve blu e azzurra), mentre nel sistema PD, senza integratore (curve
rossa e verde) si ha uno sfasamento residuo theta_finale= 4.142 = D/Kp (il sistema pd è ottenuto impostando a zero la K dell’integratore nel file pid_function.m, decommentando la riga 8).
Cambiando le condizioni iniziali sulla x0 i risultati non cambiano.

Grafico blu :      PID con x0=[0,0]
Grafico azzurro :  PID con x0=[2,1]

Grafico verde :    PD  con x0=[2,1]
Grafico rosso :    PD  con x0=[0,0]

K =    2.4142    2.4142    1.0000

[Matlab] – Energia potenziale e cinetica in funzione della quota

Ecco uno script che calcola l’energia potenziale,  cinetica e totale per un corpo posto in un orbita circolare uniforme (velocità=costante):

en_potenziale_e_cinetica.m

clc
clear all
close all

r=[6300:100:40000]‘; % supposte orbite circolari
mu=398604.3;

% T= 0.5*v^2 = 0.5*(sqrt(mu./r)).^2 =0.5*mu./r; % r>0
T=0.5*mu./r;
U=-mu./r;
E=T+U; % E= -mu/2a ; a= r ;

hold all
grid on

figure (1)
set(1,’Name’, ‘Energia Cinetica, Potenziale e Totale’,'NumberTitle’, ‘off’)        %

% Energia potenziale U
plot(r,U)

% Energia cinetica T
plot(r,T)

% Energia totale T+U
plot(r,E)

legend(‘Energia potenziale T’,'Energia cinetica U’,'Energia totale T+U’)
xlabel(‘Raggio (10^4 km)’,'fontweight’,'bold’)
ylabel(‘Energia (10^6 J)’,'fontweight’,'bold’)

OSSERVAZIONI

Come si nota dalla figura l’energia cinetica è sempre positiva e quella  potenziale sempre negativa. L’energia totale resta comunque negativa e  pari a -mu/2r.

Nota

Per questo esercizio sono state utilizzate le convenzioni dell’astrodinamica, per cui si ha che l”energia potenziale è negativa e si annulla all’infinito. Imponendo invece la U(0)=0 si ottiene una en. potenziale positiva.

Per vedere il grafico in dimensioni piene clicca su di esso.

[Matlab] – Script per verificare l’osservabilità e la controllabilità delle matrici

Ecco a voi uno script per verificare l’osservabilità e la controllabilità delle matrici:

function oss_e_cntrl.m

function oss_e_cntrl(A,B,C)

n=length(A);      % n=numero di colonne di A. Qui indica il numero di variabili dello stato (xdot=Ax+Bu)
cntrl=ctrb(A,B);  % controllabilità
oss=obsv(A,C); % oservabilità
disp(‘oss_e_cntrl(A,B,C)’) % mostra come vengono chiamate le matrici variabili usate

if rank(cntrl) == n
disp(sprintf(‘il sistema è controllabile: n=%d, rank(ctrb(A,B))=%d’,n,rank(cntrl)))
else
disp(sprintf(‘il sistema non è controllabile: n=%d, rank(ctrb(A,B))=%d’,n,rank(cntrl)))
end

if rank(oss)== n
disp(sprintf(‘il sistema è osservabile: n=%d, rank(obsv(A,C))=%d’,n,rank(oss)))
else
disp(sprintf(‘il sistema non è osservabile: n=%d, rank(obsv(A,C))=%d’,n,rank(oss)))
end

SPIEGAZIONE

Lo script è molto facile:

• Innanzitutto calcolo i ranghi delle matrici controllabilità e  osservabilità (queste matrici sono calcolate in modo automatico da matlab tramite i comandi ctrb e oss).
• Quindi verifico (tramite if) che il rango sia pari alla lunghezza n del vettore di stato.
• Infine  stampa a schermo il risultato (dal ramo if o dall’else).

il comando  sprintf permette di stampare nel testo le variabili indicate con %d, specificando successivamente le variabili o i valori con cui sostuitirle nella stringa.

sprintf puo’ essere associato ad una variabile a=sprintf(‘stringa da stampare’) che produce nell’esecuzione :

>> ans = stringa da stampare

Per evitare l’ans= annido il tutto in un disp (comando display) che stampa direttamente la stringa:

>> stringa da stampare