[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] – Sistema target-chaser con misure di posizione sul chaser

Ecco uno script per verificare l’osservabilità di sistema target-chaser, ovvero il problema del docking tra due navette.

Si hanno a disposizione le misure di posizione sul velivolo inseguitore.

t_c_sys2.m

function t_c_sys2()

m=input(‘inserire il valore della massa: \n’); % chiede in input il valore della massa

%zdot= A*z+Bu
% z=[x; xdot; y; ydot; utx; uty]
% u= [ucx; ucy]
% u è il vettore controllo, t=target, c=chaser

A=[0 1 0 0 0 0 ;
0 0 0 0 1/m 0;
0 0 0 1 0 0;
0 0 0 0 0 1/m;
0 0 0 0 0 0 ;
0 0 0 0 0 0 ];

C = [1 0 0 0 0 0;
0 0 1 0 0 0]; % misure su x e y

n=length(A);      % n=numero di colonne di A. Qui indica il numero di variabili dello stato (xdot=Ax+Bu)
oss=obsv(A,C);    % oservabilità

disp(sprintf(‘\n la massa vale %d’ , m))

% calcola il ranghi e stampa il risultato.

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