Pendolo inverso Simulazione modello non lineare Controllo in

Pendolo inverso
Simulazione modello non lineare
Controllo in retroazione dello stato e Osservatore dello stato
Esercitazioni di Controlli Automatici LS (Prof. C. Melchiorri)
1
Modello del pendolo inverso
Si consideri il modello di un pendolo inverso montato su di una base rotante mostrato in Figura 1:
θ
Pendolo
Base
Motore CC
+
Riduttore
α
Figure 1: Schema e foto del pendolo inverso.
Il modello dinamico del pendolo inverso può essere scritto, sfruttando la formulazione di EuleroLagrange, nel seguente modo:
M (q)q̈ + C(q̇, q)q̇ + Dq̇ + g(q) = τ − τf
(1)
dove q = [α θ]T è il vettore delle coordinate generallizate (rispettivamente l’angolo fra la base rotante e
il punto riferimento sulla parte fissa e l’angolo del pendolo rispetto alla posizione verticale rivolta verso
l’alto), τ e τf sono i vettori delle coppie e degli attriti agenti sui giunti, M (q) è la matrice di inerzia del
sistema, C(q̇, q) è la matrice degli effetti centripeti e di Coriolis, D è la matrice degli attriti viscosi e g(q)
è il vettore delle coppie dovute agli effetti gravitazionali. In particolare, per questo sistema si ha:
I1 + m2 (l12 + l22 sin2 (θ)) −m2 l1 l2 cos(θ)
(2)
M (q) =
−m2 l1 l2 cos(θ)
I2
l2 cos(θ)θ̇ l2 cos(θ)α̇ + l1 θ̇
(3)
C(q̇, q) = −m2 l2 sin(θ)
−l2 cos(θ)α̇
0
1
Descrizione
Costante di coppia del motore
Resistenza di armatura
Rapporto di riduzione del motore
Raggio della base
Massa della base
Momento d’inerzia della base
Coeff. d’attrito viscoso della base
Posizione baricentro del pendolo
(rispetto al punto di ancoraggio)
Massa del pendolo
Momento d’inerzia del pendolo
Coeff. d’attrito viscoso del pendolo
Sensibilità del potenziometro (sensore
di posizione della base)
Risoluzione dell’encoder (sensore di posizione del pendolo)
Simbolo
Km
Rm
Gr
l1
m1
I1
dα
l2
Valore
0.00767
2.6
60.5
0.2
0.128
0.0044
0.0001
0.43
Unità di misura
N m A−1
Ω
m2
I2
dθ
Ps
0.14
0.019
0.0152
1.637
Kg
Kg m2
N m2 s−1
rad V −1
Er
1.534 · 10−3
rad step−1
m
Kg
Kg m2
N m2 s−1
m
Table 1: Parametri del pendolo inverso.
D=
dα
0
0
dθ
,
g(q) =
0
−m2 l2 g sin(θ)
,
τ=
τm
0
,
τf =
0
0
(4)
dove τm è la coppia applicata dal motore elettrico in corrente continua che muove la base. I Parametri
del sistema sono riportati in Tab. 1. Notare che viene trascurato l’attrito statico (τf = 0).
Data l’entità delle dinamiche meccaniche di questo sistema, viene adottato il modello semplificato del
motore elettrico in corrente continua tramite il quale è possibile muovere la base:
τm =
K 2 G2
Km Gr
v − m R α̇ = Kv v − Kα α̇
Rm
Rm
(5)
dove v e la tensione in ingresso al motore (uscita della scheda di acquisizione).
Si riscriva l’eq. (1) nella forma1 :
q̈ = M (q)−1 [−C(q̇, q)q̇ − Dq̇ − g(q) + τ − τf ]
(6)
Dalle eq. (2)-(5), assumendo come vettore di stato x = [α θ α̇ θ̇]T e come vettore di ingresso u = [v],
è possibile scrivere il modello del sistema nella forma:
ẋ = f (x, u)
(7)
Sul sistema sono disponibili due sensori, un potenziometro ed un encoder ottico, per la misura della
posizione della base e del pendolo, rispettivamente α e θ. L’equazione d’uscita del pendolo inverso può
quindi essere scritta nel seguente modo:
1/Ps
0
0 0
y1
=
y=
x
(8)
0
1/Er 0 0
y2
Il sistema presenta infiniti punti di equilibrio, di cui stabili quelli corrispondenti a xeqs = [α0 (2n +
1)π 0 0]T (pendolo rivolto verso il basso) e instabili corrispondenti a xeqi = [α0 2nπ 0 0]T (pendolo
rivolto verso l’alto), con n intero e α0 una qualsiasi posizione della base.
2
Analisi e sintesi del controllo
Obiettivo del controllo è quello di stabilizzare il sistema nell’intorno di un qualsiasi punto di equilibrio
instabile corrispondente alla posizione del pendolo rivolta verso l’alto.
Si risponda quindi ai seguenti quesiti:
1 Per
le proprietà del modello dinamico di Eulero-Lagrange, la matrice M(q) è sempre invertibile.
2
1. Effettuare la simulazione del sistema non lineare;
2. Si linearizzi il sistema nell’intorno di un punto di equilibrio instabile;
3. Verificare la raggiungibilità, l’osservabilità e la scomposizione di Kalman del sistema linearizzato;
4. Si realizzi una retroazione statica dello stato tale che il sistema in retroazione risulti asintoticamente
stabile. Si verifichi la risposta del sistema per diverse condizioni iniziali del sistema non lineare;
5. Si realizzi una legge di controllo ottimo a tempo infinito che minimizzi gli scostamenti del pendolo
dalla posizione eretta. Si verifichi la risposta del sistema per diverse condizioni iniziali del sistema
non lineare;
6. Dal momento che non si possiede una misura di tutto lo stato del sistema, si realizzi un osservatore
dello stato del sistema linearizzato e lo si inserisca nello schema di controllo;
7. Si consideri il problema della regolazione della posizione della base mantenendo il pendolo in posizione eretta (inseguimento del setpoint);
8. Si implementi il regolatore progettato in ambiente RTAI e si confronti la risposta del sistema con
quella di Matlab.
3
3.1
TRACCIA DI SOLUZIONE
Simulazione del sistema non lineare
Per la simulazione del sistema non lineare, si utilizza l’ambiente Simulink. Si faccia riferimento allo
schema il fig. 2.
u
0
ingresso di equilibrio
MATLAB
Function
stato reale
dotx
modello non lineare
del pendolo
1
x
xo s
Integratore
[a0+da 2*n*pi+dteta dota0 dotteta0]
[a0 2*n*pi 0 0]
Stato iniziale
Punto di equilibrio
C
y
uscita
stato modello linearizzato
Figure 2: Schema Simulink per la simulazione del modello non lineare del pendolo inverso.
Il blocco “MATLAB Function” che rappresenta il modello non lineare del pendolo inverso implementa
l’eq. (7). I suoi ingressi sono quindi la tensione di alimentazione del motore (ingresso di controllo u = [v])
e lo stato attuale del sistema x. L’uscita è la derivata dello stato ẋ = f (x, u). Il blocco integratore
provvede al calcolo della soluzione del sistema. Notare che le condizione iniziali dell’integratore (stato
iniziale del sistema x0 ) sono state inserite in un blocco esterno. È quindi necessario, prima di tutto,
scrivere il modello dato dalle eq. (5) e (6) nella forma data dall’eq. (7).
Per effettuare il controllo sulla base del sistema linearizzato, allo stato del sistema non lineare viene
sottratto lo stato nell’intorno del quale il modello è stato linearizzato (che in generale può essere diverso
dallo stato iniziale). Questo perchè una volta che il sistema è stato stabilizzato, l’unico punto di equilibrio
per il sistema linearizzato sarà lo stato xf = [0 0 0 0]T , ovvero l’origine dello spazio degli stati. È
quindi necessario traslare lo stato di equilibrio (o, più in generale, la triettoria di equilibrio) del modello
non lineare nell’origine.
Data la linearità della funzione di uscita, è possibile applicare la funzione di uscita eq. (8) direttamente
anche allo stato del modello linearizzato, ottenendo lo scostamento del segnale di uscita del sistema
rispetto al punto di equilibrio considerato.
3
3.2
Linearizzazione del modello dinamico del sistema
Si vuole linearizzare il sistema nell’intorno dello stato di equilibrio instabile x0 = [0 0 0 0]T con ingresso
di controllo nullo u0 = 0. Prendendo in considerazione l’eq. (7), si determini in modello linearizzato:
∂f (x, u) ∂f (x, u) x
+
u = Al x + Bl u
(9)
ẋ =
∂x (x0 ,u0 )
∂u (x0 ,u0 )
1/Ps
0
0 0
y =
x = Cl x
(10)
0
1/Er 0 0
3.3
Raggiugibilità e Osservabilità
Per lo studio della raggiugibilità e della osservabilità del sistema linearizzato, si faccia uso delle funzioni
matlab ctrb, obsv, mininv, maxinv e kalmcd. Si verifichi se possibile realizzare un osservatore asintotico
dello stato utilizzando le misure di uno solo dei due sensori a disposizione.
3.4
Retroazione dello stato
Per il progetto della retroazione dello stato, si procede considerando il sistema linearizzato dato dalle matrici Al , Bl , Cl , Dl . La matrice K può quindi essere ricavata con il comando K=-place(Al,Bl,autovalori)
(oppure con il comando acker).
3.5
Osservatore dello stato
Per il progetto dell’osservatore dello stato per il sistema linearizzato, si procede considerando il sistema
duale ATl , ClT , BlT , DlT . A questo punto, il progetto della matrice di retroazione dell’osservatore si
riduce al problema di sintesi della matrice di retroazione dello stato del sistema duale. La matrice L può
quindi essere ricavata con il comando L=-place(Al’,Cl’,autovalori)’.
Gli autovalori assegnati al sistema tramite la legge di controllo ottimo possono essere ricavati tramite
il comando matlab eig(Al+Bl*K).
3.6
Inseguimento del setpoint
Si consideri il problema di controllo con retroazione dello stato con setpoint non-nullo. Nel caso in esame
il sistema ha un solo ingresso e due uscite. Non quindi possibile, in maniera diretta, progettare la matrice
di ingresso per il servomeccanismo. È possibile procedere con il progetto della matrice d’ingresso Ki se
si considera esclusivamente la relazione fra ingresso di controllo u e posizione della base del pendolo y1 .
3.7
Implementazione del controllore
Viene esaminato il problema relativo all’implementazione in ambiente Simulink e RTAI-Linux dell’osservatore
e della retroazione dello stato per il pendolo inverso.
3.7.1
Discretizzazione del controllore e dell’osservatore
Una volta determinate le matrici di retroazione dello stato e dell’osservatore, si procede alla discretizzazione del sistema linearizzato tramite il comando matlab sysd=c2d(ss(Al,Bl,Cl,Dl),Tc), dove T c è
il tempo di campionamento scelto sulla base dell’autovalore maggiore in modulo del sistema. La struttura
sysd contiene quindi le matrici del sistema discretizzato Ad=sysd.a, Bd=sysd.b, Cd=sysd.c e Dd=sysd.d.
In particolare le matrici Cl e Dl restano invariate mentre cambiano le matrici Al e Bl :
e Al T c
Z Tc
eAl τ Bl dτ
Ad
=
Bd
=
Cd
=
Cl
Dd
=
Dl
0
4
Anche la matrice di retroazione dello stato K resta invariata ma cambia la matrice di retroazione
dell’osservatore L, che si modifica allo stesso modo della matrice Bl :
Kd
=
Ld
=
K
Z Tc
eAl τ L dτ
0
Per la sintesi della matrice Ld con Matlab può essere utilizzato, anche in questo caso, il comando
sysd2=c2d(ss(Al,L,Cl,Dl),Tc). La struttura sysd2 contiene quindi la matrice di retroazione dell’osservatore discretizzata Ld=sysd.b.
3.7.2
Condizionamento dei segnali di ingresso ed uscita
Allo scopo di riprodurre il più fedelmente possibile la risposta di un sistema di controllo reale utilizzando
attraverso la simulazione in ambiente simulink, è necessario tenere esplicitamente in considerazione, nello
schema del controllore, della scalatura, limitazione (saturazione) e discretizzazione sia delle variabili di
ingresso che di quelle di uscita. Questi (ed altri) effetti, dovuti alla realizzazione pratica dei sistemi
di misura e attuazione, vengono comunemente indicati con il termine “condizionamento” dei segnali di
ingresso ed uscita.
In figura 3 viene riportato lo schema simulink del sistema di controllo del pendolo inverso. In questa
figura, i blocchi DAC, ADC ed Encoder effettano il condizionamento dei segnali, mentre il blocco di simulazione del sistema contiene il modello dinamico non lineare tempo continuo del pendolo inverso. Questi
blocchi, racchiusi nel rettangolo tratteggiato in figura 3, rappresentano come il sistema viene “visto” dal
controllore real-time. Notare che la variabile di controllo viene saturata prima di essere messa in ingresso
al sistema e all’osservatore dello stato.
Pendolo Inverso
In Out
Ps
posizione
base
ADC
Ki
Setpoint
1/Kin
In Out
Saturation
DAC
u
y
modello non lineare
del pendolo inverso
In Out
Er
posizione
pendolo
Encoder
Kin
u(k)
Kd
x(k)
azione di
controllo
y(k)
Retroazione dello stato
Osservatore dello stato
tempo discreto
Figure 3: Schema di controllo del pendolo inverso.
Il sistema si interfaccia con il calcolatore tramite una scheda di acquisizione dati di tipo PCI modello
626 prodotta dalla Sensoray.
In particolare, il blocco DAC è caratterizzato da:
• risoluzione: 14 bit;
• range variabili di uscita: ±10 [V ];
Il blocco ADC ha le seguenti caratteristiche:
• risoluzione: 16 bit;
• range variabili di uscita: ±5, ± 10 [V ];
Il blocco Encoder ha le seguenti caratteristiche:
5
• lunghezza di parola: 24 bit;
• tipo variabile d’uscita: unsigned int;
• valore iniziale di default: 0x000000;
Il blocco Encoder rappresenta l’encoder incrementale collegato al giunto posto alla base del pendolo e
restituisce in uscita un valore intero corrispondente al numero di passi (steps, ovvero frazioni dell’angolo
giro) compiuti a partire dalla posizione iniziale. Il valore della variabile d’uscita dell’encoder viene quindi
2π
incrementato di uno ad ogni variazione di 4096
[rad] dell’angolo θ e decrementato di uno se la variazione
2π
avviene di − 4096 [rad]. Notare che, partendo dal valore d’uscita 0x000000, una variazione negativa
dell’angolo θ porta l’uscita dell’encoder al valore massimo 0xFFFFFF (overflow).
Nel sistema in esame, si ha Kin = 1 e Kout [0] = 1/P s, Kout [1] = 1/Er.
3.7.3
Simulazione in ambiente Simulink
Allo scopo di verificare l’efficacia del sistema di controllo e la risposta del sistema, si esegua la simulazione
del pendolo inverso con retroazione dello stato ed osservatore. Si faccia riferimento alla figura 2 per
l’implementazione del modello del pendolo e alla figura 3 per lo schema di controllo.
3.7.4
Implementazione del controllore in ambiente RTAI
L’algoritmo di controllo realtime, ad ogni istante di campionamento, dovrà calcolare l’innovazione,
l’evoluzione dello stato dell’osservatore e l’azione di controllo. Viene riportato di seguito uno schema
per l’implementazione del controllo in retrozione e dell’osservatore per il modello linearizzato del pendolo inverso. Si noti la particolare sequenza di operazioni necessarie per le configurazione e la lettura
dell’encoder.
...
comedi_insn instr;
lsampl_t initvalue=10000;
/* open comedi device */
cf=comedi_open("/dev/comedi0");
/* impostazione istruzione di configurazione dell’encoder */
instr.insn=INSN_CONFIG;
instr.n=1;
instr.data=&initvalue; /* valore iniziale del conteggio per evitare l’overflow */
instr.subdev=5;
instr.chanspec=CR_PACK(0,0,AREF_OTHER);
/* configurazione dell’encoder */
comedi_do_insn(cf,&instr);
/* impostazione istruzione di lettura dell’encoder */
instr.insn=INSN_READ;
instr.n=1;
instr.data=&ai_data;
...
/* discrete time controller output, plant input */
double pendolo_ctrl_input[INPUT_DIM]={0.0};
/* plant output y(k) */
double pendolo_ctrl_output[OUTPUT_DIM]={0.0, 0.0};
/* discrete time observer matrices */
6
static double Ad[STATE_DIM][STATE_DIM]={...};
static double Bd[STATE_DIM][INPUT_DIM]={...};
static double Cd[OUTPUT_DIM][STATE_DIM]={...};
static double Ld[STATE_DIM][OUTPUT_DIM]={...};
static double Kd[INPUT_DIM][STATE_DIM]={...};
static double Ki[INPUT_DIM][OUTPUT_DIM]={...};
...
/* inizializzazione dello stato e del setpoint */
...
while(isrunning){
/* calcolo dell’azione di controllo, da attuare al ciclo successivo */
// u(k) = Kd*x(k) + ki*r(k)
/* attesa dell’istante di campionamento */
rt_task_wait_period();
/*********************************/
/* attuazione del controllo u(k) */
/*********************************/
/* scalatura delle variabili di controllo */
// kin[0]=1;
// pendolo_ctrl_input[0] = u[0]/kin[0];
/* conversione da grandezza fisica a registro del DAC */
ao_data=comedi_from_phys(pendolo_ctrl_input[0],ao_rangetype,ao_maxdata);
/* scrittura sul registro del DAC */
comedi_data_write(cf,ao_subdev,ao_chan,ao_range,ao_aref,ao_data);
/* saturazione e discretizzazione delle variabili di controllo */
pendolo_ctrl_input[0]=comedi_to_phys(ao_data,ao_rangetype,ao_maxdata);
/* riscalatura delle variabili di controllo */
// kin[0]=1;
// u[0] = pendolo_ctrl_input[0]*kin[0];
/**********************************/
/* lettura dell’output y(0)=alpha */
/**********************************/
/* lettura del registro dell’ADC */
comedi_data_read(cf,ai_subdev,ai_chan,ai_range,ai_aref,&ai_data);
/* conversione registro ADC a grandezza fisica (tensione del potenziometro) */
pendolo_ctrl_output[0]=comedi_to_phys(ai_data,ai_rangetype,ai_maxdata);
/* scalatura della variabile di uscita alpha */
// y[0] = pendolo_ctrl_output[0]/kout[0]; // kout[0]=1/Ps;
7
/**********************************/
/* lettura dell’output y(1)=theta */
/**********************************/
/* lettura registro dell’encoder, il valore viene salvato in ai_data */
comedi_do_insn(cf,&instr);
/* conversione registro encoder a grandezza fisica (angolo theta) */
/* NOTARE IL VALORE INIZIALE DELL’ANGOLO (-M_PI) */
// kout[1]=1/Er;
pendolo_ctrl_output[1]= -M_PI + (((double)ai_data)-((double)initvalue))/kout[1];
/* calcolo dell’evoluzione libera */
// x(k+1) = Ad*x(k)
/* calcolo dell’errore di inseguimento dello stato */
// e(k) = Cd*x(k) + Dd*u(k) - y(k)
/* calcolo dell’innovazione */
// x(k+1) += Ld*e(k)
/* calcolo dell’evoluzione forzata */
// x(k+1) += Bd*u(k)
/* aggiornamento dello stato */
// x(k) = x(k+1)
/* calcolo e aggiornamento dello setpoint */
// r(k+1) = ...
// r(k) = r(k+1)
}
...
3.7.5
Comparazione dei risultati simulativi e sperimentali
Viene di seguito riportato il confronto fra la risposta del sistema simulato e del sistema reale. Nello
schema utilizzato, il controllore in grado di portare il sistema dallo stato iniziale di equilibrio stabile
x0 = [0 − π2 0 0] allo stato instabile x1 = [α1 − π 0 0] (il valore di α1 non è di interesse), e di
stabilizzare il sistema nell’intorno dello stato instabile xd = [αd − π 0 0], con αd definibile dall’utente.
8
Real and Simulated Positions
1
0
Position [rad]
−1
Arm (Real)
Pendulum (Real)
Arm (Simulated)
Pendulum (Simulated)
−2
−3
−4
−5
−6
−7
0
1
2
3
4
5
Time [s]
6
7
8
9
10
Figure 4: Confronto fra le posizioni del sistema reale e di quello simulato.
Difference in position
0.2
Arm
Pendulum
rad
0.1
0
−0.1
−0.2
0
1
2
3
4
5
Time [s]
6
7
8
9
10
Figure 5: Differenza fra le posizione del sistema reale e di quello simulato.
Real and Simulated Velocities
Pendulum velocity [rad/s]
Arm velocity [rad/s]
10
5
0
−5
−10
0
10
Real velocity
Simulated velocity
1
2
3
4
5
6
7
8
9
10
5
0
−5
−10
0
Real velocity
Simulated velocity
1
2
3
4
5
Time [s]
6
7
8
9
10
Figure 6: Confronto fra le velocità del sistema reale e di quello simulato.
9