Pendolo inverso Simulazione modello non lineare Controllo in retroazione dello stato e Osservatore dello stato

Transcript

1 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: θ Base Pendolo Motore CC + Riduttore α Figure 1: Schema e foto del pendolo inverso. Il modello dinamico del pendolo inverso può essere scritto, sfruttando la formulazione di Eulero- Lagrange, nel seguente modo: M(q) q + C( q, q) q + D q + g(q) = τ τ f (1) dove q = [α θ] T è 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) è la matrice di inerzia del sistema, C( q, q) è la matrice degli effetti centripeti e di Coriolis, D è la matrice degli attriti viscosi e g(q) è il vettore delle coppie dovute agli effetti gravitazionali. In particolare, per questo sistema si ha: [ I1 + m M(q) = 2 (l1 2 + ] l2 2 sin2 (θ)) m 2 l 1 l 2 cos(θ) (2) m 2 l 1 l 2 cos(θ) I 2 [ l2 cos(θ) C( q, q) = m 2 l 2 sin(θ) θ ] l 2 cos(θ) α + l 1 θ (3) l 2 cos(θ) α 1

2 Descrizione Simbolo Valore Unità di misura Costante di coppia del motore K m.767 N m A 1 Resistenza di armatura R m 2.6 Ω Rapporto di riduzione del motore G r 6.5 Raggio della base l 1.2 m Massa della base m Kg Momento d inerzia della base I 1.44 Kg m 2 Coeff. d attrito viscoso della base d α.1 N m 2 s 1 Posizione baricentro del pendolo l 2.43 m (rispetto al punto di ancoraggio) Massa del pendolo m 2.14 Kg Momento d inerzia del pendolo I 2.19 Kg m 2 Coeff. d attrito viscoso del pendolo d θ.152 N m 2 s 1 Sensibilità del potenziometro (sensore P s rad V 1 di posizione della base) Risoluzione dell encoder (sensore di posizione del pendolo) E r rad step 1 D = [ ] [ dα, g(q) = d θ Table 1: Parametri del pendolo inverso. m 2 l 2 g sin(θ) ] [ τm, τ = ] [ ], τ f = dove τ m è 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 = ). Data l entità delle dinamiche meccaniche di questo sistema, viene adottato il modello semplificato del motore elettrico in corrente continua tramite il quale è possibile muovere la base: τ m = K mg r v K2 m G2 R α = K v v K α α (5) R m R m dove v e la tensione in ingresso al motore (uscita della scheda di acquisizione). Si riscriva l eq. (1) nella forma 1 : q = M(q) 1 [ C( q, q) q D q g(q) + τ τ f ] (6) Dalle eq. (2)-(5), assumendo come vettore di stato x = [α θ α θ] T e come vettore di ingresso u = [v], è possibile scrivere il modello del sistema nella forma: (4) ẋ = 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 può quindi essere scritta nel seguente modo: y = [ y1 ] = y 2 [ 1/Ps 1/E r ] x (8) Il sistema presenta infiniti punti di equilibrio, di cui stabili quelli corrispondenti a x eqs = [α (2n + 1)π ] T (pendolo rivolto verso il basso) e instabili corrispondenti a x eqi = [α 2nπ ] T (pendolo rivolto verso l alto), con n intero e α una qualsiasi posizione della base. 2 Analisi e sintesi del controllo Obiettivo del controllo è 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 proprietà del modello dinamico di Eulero-Lagrange, la matrice M(q) è sempre invertibile. 2

3 1. Effettuare la simulazione del sistema non lineare; 2. Si linearizzi il sistema nell intorno di un punto di equilibrio instabile; 3. Verificare la raggiungibilità, l osservabilità 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 TRACCIA DI SOLUZIONE 3.1 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. ingresso di equilibrio u MATLAB Function modello non lineare del pendolo dotx 1 s x o Integratore x C y stato reale uscita [a+da 2*n*pi+dteta dota dotteta] Stato iniziale [a 2*n*pi ] Punto di equilibrio 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 è la derivata dello stato ẋ = f(x, u). Il blocco integratore provvede al calcolo della soluzione del sistema. Notare che le condizione iniziali dell integratore (stato iniziale del sistema x ) sono state inserite in un blocco esterno. È 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 è stato linearizzato (che in generale può essere diverso dallo stato iniziale). Questo perchè una volta che il sistema è stato stabilizzato, l unico punto di equilibrio per il sistema linearizzato sarà lo stato x f = [ ] T, ovvero l origine dello spazio degli stati. È quindi necessario traslare lo stato di equilibrio (o, più in generale, la triettoria di equilibrio) del modello non lineare nell origine. Data la linearità della funzione di uscita, è 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

4 3.2 Linearizzazione del modello dinamico del sistema Si vuole linearizzare il sistema nell intorno dello stato di equilibrio instabile x = [ ] T con ingresso di controllo nullo u =. Prendendo in considerazione l eq. (7), si determini in modello linearizzato: f(x, u) ẋ = f(x, u) x x + (x,u ) u u = A l x + B l u (9) (x,u ) [ ] 1/Ps y = x = C 1/E r l x (1) 3.3 Raggiugibilità e Osservabilità Per lo studio della raggiugibilità e della osservabilità 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 A l, B l, C l, D l. La matrice K può quindi essere ricavata con il comandok=-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 A T l, CT l, BT l, DT l. 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 può 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. È possibile procedere con il progetto della matrice d ingresso K i se si considera esclusivamente la relazione fra ingresso di controllo u e posizione della base del pendolo y 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 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 Tc è 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 C l e D l restano invariate mentre cambiano le matrici A l e B l : A d = e A ltc B d = Tc C d = C l D d = D l e A lτ B l dτ 4

5 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 B l : K d = K L d = Tc e A lτ L dτ Per la sintesi della matrice L d con Matlab può 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 Condizionamento dei segnali di ingresso ed uscita Allo scopo di riprodurre il più fedelmente possibile la risposta di un sistema di controllo reale utilizzando attraverso la simulazione in ambiente simulink, è 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 Ki 1/Kin In Out u y ADC posizione base Setpoint Saturation DAC modello non lineare del pendolo inverso In Out Er Encoder posizione pendolo Kin Kd Retroazione dello stato x(k) u(k) y(k) Osservatore dello stato tempo discreto azione di controllo 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 è caratterizzato da: risoluzione: 14 bit; range variabili di uscita: ±1 [V ]; Il blocco ADC ha le seguenti caratteristiche: risoluzione: 16 bit; range variabili di uscita: ±5, ± 1 [V ]; Il blocco Encoder ha le seguenti caratteristiche: 5

6 lunghezza di parola: 24 bit; tipo variabile d uscita: unsigned int; valore iniziale di default: x; 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 496 [rad] dell angolo θ e decrementato di uno se la variazione avviene di 2π 496 [rad]. Notare che, partendo dal valore d uscita x, una variazione negativa dell angolo θ porta l uscita dell encoder al valore massimo xffffff (overflow). Nel sistema in esame, si ha K in = 1 e K out [] = 1/Ps, K out [1] = 1/Er 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 Implementazione del controllore in ambiente RTAI L algoritmo di controllo realtime, ad ogni istante di campionamento, dovrà 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=1; /* open comedi device */ cf=comedi_open("/dev/comedi"); /* 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(,,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]={.}; /* plant output y(k) */ double pendolo_ctrl_output[output_dim]={.,.}; /* discrete time observer matrices */ 6

7 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[]=1; // pendolo_ctrl_input[] = u[]/kin[]; /* conversione da grandezza fisica a registro del DAC */ ao_data=comedi_from_phys(pendolo_ctrl_input[],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[]=comedi_to_phys(ao_data,ao_rangetype,ao_maxdata); /* riscalatura delle variabili di controllo */ // kin[]=1; // u[] = pendolo_ctrl_input[]*kin[]; /**********************************/ /* lettura dell output y()=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[]=comedi_to_phys(ai_data,ai_rangetype,ai_maxdata); /* scalatura della variabile di uscita alpha */ // y[] = pendolo_ctrl_output[]/kout[]; // kout[]=1/ps; 7

8 /**********************************/ /* 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) 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 x = [ π 2 ] allo stato instabile x 1 = [α 1 π ] (il valore di α 1 non è di interesse), e di stabilizzare il sistema nell intorno dello stato instabile x d = [α d π ], con α d definibile dall utente. 8

9 1 Real and Simulated Positions Position [rad] Arm (Real) Pendulum (Real) Arm (Simulated) Pendulum (Simulated) Time [s] Figure 4: Confronto fra le posizioni del sistema reale e di quello simulato..2.1 Difference in position Arm Pendulum rad Time [s] Figure 5: Differenza fra le posizione del sistema reale e di quello simulato. 1 Real and Simulated Velocities Arm velocity [rad/s] Pendulum velocity [rad/s] 5 5 Real velocity Simulated velocity Real velocity Simulated velocity Time [s] Figure 6: Confronto fra le velocità del sistema reale e di quello simulato. 9