Controllo di un robot monociclo
Transcript
Controllo di un robot monociclo
Università degli Studi di Napoli Federico II Facoltà di Ingegneria Tesina di Sistemi di Controllo Multivariabile Controllo di un robot monociclo Candidati Davide Falanga M58/48 Andrea Fontanelli M58/64 Anno Accademico 2012/2013 Docente Ch.mo Prof. Ing. Francesco Amato INDICE INDICE Indice 1 Analisi e modellistica 1.1 Descrizione del robot . . . . . . . . . . . . . . . . . 1.2 Considerazioni preliminari . . . . . . . . . . . . . . 1.3 Equazioni del moto longitudinale . . . . . . . . . . 1.3.1 Grandezze e schema di riferimento . . . . . 1.3.2 Equazioni di Lagrange . . . . . . . . . . . . 1.3.3 Linearizzazione . . . . . . . . . . . . . . . . 1.4 Equazioni del moto laterale . . . . . . . . . . . . . 1.4.1 Grandezze e schema di riferimento . . . . . 1.4.2 Equazioni di Lagrange . . . . . . . . . . . . 1.4.3 Linearizzazione . . . . . . . . . . . . . . . . 1.5 Dinamica dei motori . . . . . . . . . . . . . . . . . 1.6 Modello matematico nello spazio di stato . . . . . 1.6.1 Modello per il moto longitudinale . . . . . . 1.6.2 Modello per il moto laterale . . . . . . . . . 1.7 Proprietà strutturali: osservabilità e controllabilità . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4 4 4 4 4 5 6 7 7 8 8 9 9 9 10 10 2 Controllo 2.1 Controllo mediante LQR . . . . . . . . . . . . . . . . . . . . . . . 2.1.1 Il controllo LQR . . . . . . . . . . . . . . . . . . . . . . . 2.1.2 Scelta delle matrici di peso . . . . . . . . . . . . . . . . . 2.1.3 Moto longitudinale . . . . . . . . . . . . . . . . . . . . . . 2.1.4 Moto laterale . . . . . . . . . . . . . . . . . . . . . . . . . 2.2 Controllo mediante assegnamento dell’autostruttura . . . . . . . 2.2.1 L’assegnamento dell’autostruttura . . . . . . . . . . . . . 2.2.2 Moto longitudinale . . . . . . . . . . . . . . . . . . . . . . 2.2.3 Moto laterale . . . . . . . . . . . . . . . . . . . . . . . . . 2.3 Schemi di controllo e confronti . . . . . . . . . . . . . . . . . . . 2.3.1 Schemi di controllo . . . . . . . . . . . . . . . . . . . . . . 2.3.2 Risposta del sistema con controllo LQR . . . . . . . . . . 2.3.3 Risposta del sistema con assegnamento dell’autostruttura 2.3.4 Azioni di controllo . . . . . . . . . . . . . . . . . . . . . . 2.4 Filtro di Kalman . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12 12 12 12 12 13 13 13 14 14 15 15 16 18 20 20 3 Realizzazione fisica del prototipo 3.1 Componenti utilizzati . . . . . . . . . . . . . . . . . 3.1.1 Microcontrollore: Arduino Due . . . . . . . . 3.1.2 IMU: Invesense MPU-6050 . . . . . . . . . . 3.1.3 Motori DC: Pololu 12V, 19:1 con encoder . . 3.1.4 Driver: Pololu Dual 12 A , 24 V Motor Shield 3.1.5 Encoder: ATM102-V . . . . . . . . . . . . . . 3.1.6 Ruota . . . . . . . . . . . . . . . . . . . . . . 3.1.7 Disco . . . . . . . . . . . . . . . . . . . . . . 3.2 Caratterizzazione dei motori . . . . . . . . . . . . . . 3.3 Progettazione mecccanica . . . . . . . . . . . . . . . 3.4 Produzione dei componenti e assemblaggio . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22 22 22 22 23 24 24 24 25 25 26 27 2 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . INDICE INDICE A Appendice A.1 Codice Matlab . . . . . . . . . . . . . . A.1.1 Controllo del moto longitudinale A.1.2 Controllo del moto laterale . . . A.2 Codice Arduino . . . . . . . . . . . . . . A.2.1 Controllore . . . . . . . . . . . . A.2.2 Gestione encoder . . . . . . . . . A.2.3 Libreria Kalman . . . . . . . . . 3 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 29 29 29 30 31 31 36 39 1 1 1.1 ANALISI E MODELLISTICA Analisi e modellistica Descrizione del robot Nel presente lavoro si intende descrivere le metodologie utilizzate per il controllo di un monociclo robot, il cui comportamento è simile a quello di un essere umano a cavallo di un classico monociclo. Trattasi di un sistema instabile che può essere analizzato considerando separatamente due dinamiche differenti: una longitudinale (pitch) ed una laterale (roll). Il robot in analisi è composto da una ruota, un corpo ed un disco inerziale. La ruota ha l’obiettivo di equilibrare il sistema nei movimenti di pitch, mentre il disco permette di equilibrare i movimenti di roll. Si assume quindi che il movimento di imbardata sia impedito a causa della struttura stessa del sistema, riportata di seguito: Figura 1: schema del robot e sistemi di riferimento 1.2 Considerazioni preliminari Per lo studio del robot in oggetto è stato utilizzato il modello matematico proposto in [1]. Tale modello dinamico è stato ricavato applicando le equazioni di Eulero-Lagrange al sistema meccanico, supponendo che gli unici gradi di libertà a disposizione siano quello longitudinale e quello laterale. Alla base dell’intera analisi vi è l’ipotesi di disaccoppiamento tra i due movimenti, la quale consente di studiare il comportamento del sistema in due piani differenti. Eventuali effetti di accoppiamento sono da considerarsi trascurabili. 1.3 1.3.1 Equazioni del moto longitudinale Grandezze e schema di riferimento Per l’analisi del robot nel suo movimento longitudinale è stato tenuto in considerazione il seguente schema di riferimento: 4 1.3 Equazioni del moto longitudinale 1 ANALISI E MODELLISTICA Figura 2: schema e grandezze in gioco nel moto longitudinale Le grandezze in gioco risultano essere le seguenti: α1 (t) angolo tra la ruota ed il corpo α2 (t) angolo tra la il corpo e l’asse verticale τ (t) coppia applicata dal motore all’asse della ruota xw (t) posizione orizzontale della ruota xb (t) posizione orizzontale del corpo yb (t) posizione verticale del corpo mw massa della ruota mbt massa del corpo Jw momento di inerzia della ruota rispetto al suo baricentro Jbt momento di inerzia del corpo rispetto al suo baricentro lbt distanza tra il centro della ruota ed il baricentro del corpo rbt raggio della ruota g accelerazione di gravità 1.3.2 Equazioni di Lagrange Per ottenere le equazioni del moto sono state applicate le equazioni di Lagrange al sistema meccanico precedentemente descritto. In particolare, queste ultime risultano essere le seguenti: ∂T ∂V d ∂T ( )− + = Qk dt ∂ q̇k ∂qk ∂qk 5 (1) 1.3 Equazioni del moto longitudinale 1 ANALISI E MODELLISTICA dove T e V sono rispettivamente l’energia cinetica e l’energia potenziale del sistema, mentre Qk rappresentano le forze generalizzate per il k-esimo grado di libertà. L’energia cinetica può essere vista come formata da due contributi: il primo, Tw , legato al moto della ruota; il secondo, Tb , legato al moto del corpo. Quest’ultimo, inoltre, può essere visto a sua volta come composto da tre parti: Tx , componente orizzontale dell’energia cinetica; Ty , componente verticale; Tr ot, componente rotazionale. Tenendo conto di tutti i suddetti contributi, l’energia cinetica del sistema risulta essere la seguente: 1 1 2 2 2 mbt (2rw lbt (α̇1 + α̇2 )α̇2 cosα2 + rw (α̇1 + α̇2 )2 + lbt α̇2 ) + Jbt α̇22 2 2 Mentre l’energia potenziale è fornita da : T = V = mbt glbt cosα2 (2) (3) Le forze generalizzate, infine, sono le seguenti: Q = Q1 δα1 + Q2 δα2 (4) Q1 = τ (5) Q2 = 0 (6) Grazie a tale analisi, dunque, è stato possibile giungere alle seguenti equazioni per il moto longitudinale: 2 2 2 (mbt rw + mw rw + Jw )α̈1 + (mbt rw lbt cosα2 + mbt rw + 2 mw rw + Jw )α̈2 − mbt rw lbt α̇22 sinα2 = τ (7) 2 2 (mbt rw lbt cosα2 + mbt rw + mw rw + Jw )α̈1 + (2mbt rw lbt cosα2 + 2 2 2 mbt rw + mbt lbt + Jbt + mw rw + Jw )α̈2 − mbt rw lbt α̇22 sinα2 − (8) mbt glbt sinα2 = 0 1.3.3 Linearizzazione Le equazioni (7) e (8) descrivono un sistema chiaramente non lineare. Ai fini del controllo è invece conveniente avere a disposizione equazioni lineari. Una prima soluzione può essere quella di considerare il punto α2 ' 0 come punto di equilibrio intorno al quale effettuare la linearizzazione. In tali circostanze è possibile ritenere valide le approssimazioni sinα2 ' α2 e cos α2 ' 0. Trascurando inoltre i termini di ordine superiore al primo è possibile ottenere cosı̀ un modello linearizzato approssimato. Per migliorare invece il processo di linearizzazione si è fatto uso del software Matlab. Nello specifico, le equazioni (7) e (8) sono state inserite all’interno dell’editor di equazioni differenziali DEE, esplicitando le espressioni delle variabili α̈1 e α̈2 . In questo modo è stato ottenuto un blocco Simulink rappresentante il sistema non lineare. Avendo a disposizione tale blocco è stato quindi possibile utilizzare la funzione linmod fornita da Matlab per effettuare la linearizzazione nell’intorno del punto di equilibrio. 6 1.4 Equazioni del moto laterale 1.4 1.4.1 1 ANALISI E MODELLISTICA Equazioni del moto laterale Grandezze e schema di riferimento Per l’analisi del robot nel suo movimento laterale è stato tenuto in considerazione il seguente schema di riferimento: Figura 3: schema e grandezze in gioco nel moto laterale Le grandezze in gioco risultano essere le seguenti: β1 (t) angolo tra il corpo e l’asse verticale β2 (t) angolo tra la il disco ed il corpo τ (t) coppia applicata dal motore all’asse del disco xw (t) posizione orizzontale della disco yw (t) posizione orizzontale del disco m1 massa del corpo m2 massa del disco J1A momento di inerzia del corpo rispetto al punto A J2C momento di inerzia del disco rispetto al suo centro l1 distanza tra il punto A e il centro del disco l1C distanza tra il punto A e il baricentro del corpo r raggio del disco g accelerazione di gravità 7 1.4 Equazioni del moto laterale 1.4.2 1 ANALISI E MODELLISTICA Equazioni di Lagrange Le equazioni (1) sono state applicate nuovamente al sistema per ottenere, a partire dall’energia cinetica e dall’energia potenziale, le equazioni del moto laterale. In particolare, in entrambi i casi è stato possibile distinguere un contributo dovuto al corpo ed uno dovuto al disco inerziale. Tenendo conto di tutti i suddetti contributi, l’energia cinetica del sistema risulta essere la seguente: 1 1 1 J1A β̇12 + J2C (β̇1 + β̇2 )2 + m2 l12 β̇12 2 2 2 Mentre l’energia potenziale è fornita da : T = V = (m1 l1C + m2 l1 )gcosβ1 (9) (10) Le forze generalizzate, infine, sono le seguenti: Q = Q1 δβ1 + Q2 δβ2 (11) Q1 = 0 (12) Q2 = τ (13) Grazie a tale analisi, dunque, è stato possibile giungere alle seguenti equazioni per il moto longitudinale: 1.4.3 (J1A + J2C + m2 l12 )β̈1 + J2C β̈2 − (m1 l1C + m2 l1 )gsinβ1 = 0 (14) J2C β̈1 + J2C β̈2 = τ (15) Linearizzazione Le equazioni (14) e (15) descrivono un sistema chiaramente non lineare. Ai fini del controllo è invece conveniente avere a disposizione equazioni lineari. Una prima soluzione può essere quella di considerare il punto β1 = 0 come punto di equilibrio intorno al quale effettuare la linearizzazione. In tali circostanze è possibile ritenere valida l’approssimazione sinβ1 ' β1. Per migliorare invece il processo di linearizzazione si è fatto uso del software Matlab. Nello specifico, le equazioni (14) e (15) sono state inserite all’interno dell’editor di equazioni differenziali DEE, esplicitando le espressioni delle variabili β̈1 e β̈2 . In questo modo è stato ottenuto un blocco Simulink rappresentante il sistema non lineare. Avendo a disposizione tale blocco è stato quindi possibile utilizzare la funzione linmod fornita da Matlab per effettuare la linearizzazione nell’intorno del punto di equilibrio. 8 1.5 Dinamica dei motori 1.5 1 ANALISI E MODELLISTICA Dinamica dei motori Il modello matematico descritto delle equazioni (7), (8), (14) e (15) prevede come ingressi al sistema due coppie: quella all’asse del disco e quella all’asse della ruota. Entrambe vengono fornite da un motore DC a magneti permanenti, per il quale possiamo tenere valida la seguente legge: τ= kkm (Vm − kke ω) Rm (16) dove: k rapporto di riduzione km costante di coppia del motore ke costante elettromagnetica del motore Rm resistenza elettrica del motore Vm tensione in ingresso ω velocità di rotazione dell’albero τ coppia disponibile all’asse È evidente dunque l’assenza di fenomeni induttivi, i quali possono essere trascurati. L’equazione (16) illustra chiaramente quale sia il legame tra la tensione fornita in ingresso al motore e la coppia sviluppata da quest’ultimo all’asse. Il controllo della coppia erogata da ciascun motore si riconduce quindi in sostanza al controllo della tensione fornita ai capi dell’armatura dello stesso. 1.6 Modello matematico nello spazio di stato Utilizzando le tecniche di linearizzazione è stato possibile dunque giungere ad un modello nello spazio di stato per ciascun moto. I due modelli risultano essere entrambi caratterizzati da quattro variabili di stato (due posizioni angolari e due velocità angolari), ed un ingresso ciascuno (la coppia fornita dal rispettivo motore). Nei modelli matematici riportati nei seguenti paragrafi è stata già effettuata la sostituzione dell’equazione (16) nelle equazioni (7), (8), (14) e (15). L’ingresso non sarà più dunque la coppia richiesta all’asse, bensı̀ la tensione fornita ai capi dell’armatura del motore al fine di ottenere tale coppia. 1.6.1 Modello per il moto longitudinale Per il moto longitudinale, il modello matematico nello spazio di stato risulta essere descritto dalle seguenti equazioni: 2 2 2 2 (mbt rw + mw rw + Jw )α̈1 + (mbt rw lbt + mbt rw + mw rw + Jw )α̈2 + k 2 km ke kkm α̇1 = Vm Rm Rm 2 2 (mbt rw lbt + mbt rw + mw rw + Jw )α̈1 + (2mbt rw lbt + 2 2 2 mbt rw + mbt lbt + Jbt + mw rw + Jw )α̈2 − mbt glbt α2 = 0 Nello spazio di stato, dunque, si ha: 9 (17) (18) 1.7 Proprietà strutturali: osservabilità e controllabilità 0 α̇1 α̇2 0 = α̈1 0 α̈2 0 0 0 1 K7 + K5KK62K−K 6 K1 K1 K7 − K5 K2 −K6 K1 ANALISI E MODELLISTICA 0 0 α1 0 1 α2 + V K4 K6 − K5 K2 −K6 K1 m 0 α̇1 K4 K5 0 α̇2 K5 K2 −K6 K1 1 0 K7 K5 1 K6 K3 K5 K2 −K6 K1 K5 − K5 KK23−K 6 K1 (19) dove: 2 2 K1 = (mbt rw + mw rw + Jw ) 2 2 2 K2 = (2mbt rw lbt + mbt rw + mbt lbt + Jbt + mw rw + Jw ) K3 = k2 km ke Rm K4 = kkm Rm 2 2 K5 = (mbt rw lbt + mbt rw + mw rw + Jw ) 2 2 2 K6 = (2mbt rw lbt + mbt rw + mbt lbt + Jbt + mw rw + Jw ) K7 = mbt glbt 1.6.2 Modello per il moto laterale Per il moto laterale, il modello matematico nello spazio di stato risulta essere descritto dalle seguenti equazioni: (J1A + J2C + m2 l12 )β̈1 + J2C β̈2 − (m1 l1C + m2 l1 )gβ1 = 0 J2C β̈1 + J2C β̈2 Nello spazio di stato, dunque, 0 β̇1 0 β̇2 = m1 l1C +m2 l1 β̈1 g Jeq m1 l1C +m2 l1 β̈2 − g Jeq 1.7 kkm k 2 km ke β̇2 = Vm Rm Rm (20) (21) si ha: 0 1 0 0 0 0 0 0 0 β1 β 0 2 k2 km ke + − kKm Vm β̇1 Jeq Rm Jeq Rm 2 kKm Jeq +J2C ke Jeq +J2C β̇ − kJ km 2 Jeq Rm J2C R J 0 1 eq m (22) 2C Proprietà strutturali: osservabilità e controllabilità Prima di procedere alla fase di progettazione del controllore mediante le tecniche dell’assegnamento dell’autostruttura e del controllo ottimo, è necessario verificare alcune proprietà strutturali del sistema. In particolare, sono state verificate l’osservabilità e la controllabilità di entrambi i sistemi. Per verificare l’osservabilità è necessario analizzare la matrice di osservabilità del sistema, definita come: O = CT AT C T (A2 )T C T ... (An−1 )T C T (23) Affinché il sistema sia completamente osservabile, tale matrice deve essere di rango pieno, ovvero deve avere rango pari all’ordine del sistema. 10 1.7 Proprietà strutturali: osservabilità e controllabilità 1 ANALISI E MODELLISTICA Per verificare la controllabilità è necessario analizzare la matrice di controllabilità del sistema, definita come: Cc = B AB A2 B ... An−1 B (24) Affinché il sistema sia completamente controllabile, tale matrice deve essere di rango pieno, ovvero deve avere rango pari all’ordine del sistema. Per verificare entrambe le proprietà strutturali è stato utilizzato il codice Matlab riportato in appendice per la linearizzazione dei due modelli. Sia il moto longitudinale che quello laterale sono completamente osservabili e controllabili. 11 2 2 CONTROLLO Controllo 2.1 2.1.1 Controllo mediante LQR Il controllo LQR Dato un sistema LTI del tipo ẋ(t) = Ax(t) + Bu(t) (25) assegnato lo stato x(t0 ), il problema del controllo ottimo LQ consiste nel trovare una legge di controllo in retroazione di stato del tipo: u(t) = −kx(t) (26) tale da minimizzare il seguente funzionale di costo (o cifra di merito): Z +∞ V = (x(t)T Qx(t) + u(t)T Ru(t))dt (27) 0 dove Q ≥ 0 e R > 0 sono matrici opportunamente scelte. I termini sotto segno di integrale rappresentano i costi di transizione, mentre è assente il costo terminale in quanto affinché tale integrale converga, all’infinito lo stato deve tendere necessariamrnte a zero. Il controllore cosı̀ sintetizzato dipende dalla soluzione dell’equazione algebrica di Riccati: Q − P (t)BR−1 B T P (t) + AT P (t) + P (t)A = 0 (28) Fine ultimo di tale tecnica di controllo è quindi quello di minimizzare l’energia spesa ai fini del controllo, pesando quest’ultima secondo un’opportuna matrice di pesi. Allo stesso modo è indispensabile pesare il vettore degli stati, affinché si raggiunga un giusto compromesso tra la necessità di stabilizzare il sistema e quella di ridurre al minimo l’azione di controllo sullo stesso. Un particolare problema LQ è il cosiddetto Linear Quadratic Regulator (LQR), ossia un problema di controllo ottimo LQ il cui obiettivo è quello di mantenere lo stato del sistema in prossimità dell’origine dello spazio di stato. 2.1.2 Scelta delle matrici di peso La scelta delle matrici di peso ha rappresentato uno dei principali problemi nell’implementazione del controllo LQR. In particolare, è stato necessario individuare un giusto compromesso tra requisiti quali la stabilità del sistema, i vincoli imposti dagli attuatori a disposizione e la necessità di limitare il più possibile l’azione di controllo. 2.1.3 Moto longitudinale Per il moto longitudinale, la scelta finale è ricaduta sulle 2 0 0 0 2000 0 Q= 0 0 2 0 0 0 R = 2000 12 seguenti matrici: 0 0 0 1 (29) (30) 2.2 Controllo mediante assegnamento dell’autostruttura 2 CONTROLLO Note le matrici di peso, dunque, il guadagno del controllore è stato calcolato in Matlab utilizzando il comando K = lqr(sistema, Q, R). Il regolatore sintetizzato è dunque il seguente: K = −0.0316 2.1.4 −4.6202 −0.0557 −0.5364 (31) Moto laterale Per il moto laterale, la scelta finale è ricaduta sulle seguenti matrici: 5000 0 0 0 0 2 0 0 Q= 0 0 20 0 0 0 0 1 R = 50000 (32) (33) Note le matrici di peso, dunque, il guadagno del controllore è stato calcolato in Matlab utilizzando il comando K = lqr(sistema, Q, R). Il regolatore sintetizzato è dunque il seguente: K = −9.1340 2.2 2.2.1 −0.0063 −1.4605 −0.0099 (34) Controllo mediante assegnamento dell’autostruttura L’assegnamento dell’autostruttura L’assegnamento dell’autostruttura è una tecnica per il controllo di sistemi multivariabile nella forma di retroazione di stato. In particolare, essa ha l’obiettivo di individuare un guadagno K tale che i poli del sistema a ciclo chiuso si trovino in una particolare regione del piano complesso: tipicamente si fa in modo che tali poli risultino essere a sinistra di un’assegnata ascissa. Alla base dell’assegnamento dell’autostruttura vi è il profondo legame tra poli e autovalori, cosı̀ come tra zeri ed autovettori del sistema. Grazie alla decomposizione spettrale, infatti, è possibile scrivere: e At = n X eλi t vi wiT (35) i=1 dove λi costituisce l’i-esimo autovalore della matrice dinamica, mentre vi e wiT rappresentano rispettivamente l’i-esimo autovettore destro e autovettore sinistro. La quantità eAt figura anche nell’espressione della risposta forzata nello stato, cosı̀ come nella risposta forzata nell’uscita. Difatti: xf (t) = Z tX n eλi (t−τ ) vi wiT Bu(τ )dτ (36) 0 i=1 Z yf (t) = t C 0 n X eλi (t−τ ) vi wiT Bu(τ )dτ (37) i=1 In entrambi i casi, dunque, la risposta forzata legata all’i-esimo modo di evoluzione dipende temporalmente da eλi t , in termini di ampiezza dall’autovettore wi ed è diretta nella direzione dell’autovettore vi . Ciò vuol dire che la posizione dei poli determina l’andamento temporale dei modi di evoluzione, mentre la posizione degli zeri determina il coefficiente di ampiezza degli stessi. Il controllo mediante assegnamento dell’autostruttura prevede dunque uno schema di controllo come quello riportato di seguito: 13 2.2 Controllo mediante assegnamento dell’autostruttura r(t) + 2 u(t) CONTROLLO y(t) Sistema x(t) Kx(t) K La legge di controllo è dunque nella forma: u(t) = Kx(t) + r(t) (38) Dove il guadagno K è tale che, assegnata una matrice P definita positiva, si verifica che: AP + BKP + P AT + P AT B T + 2αP < 0 (39) con α che dipende dalla velocità con cui si intende far esaurire i modi di evoluzione in transitorio. In tal modo il sistema a ciclo chiuso possiede poli la cui parte reale risiede nel semipiano complesso a sinistra dell’ascissa α. 2.2.2 Moto longitudinale Per il controllo del moto longitudinale si è scelto di imporre al vettore α i seguenti valori, relativi ciascuno ad una dinamica del sistema: α = −0.5 −50 −0.6 (40) −0.6269 (41) −10 Il regolatore sintetizzato è dunque il seguente: K = −0.0095 2.2.3 −4.9821 −0.0361 Moto laterale Per il controllo del moto laterale si è scelto di imporre al vettore α i seguenti valori, relativi ciascuno ad una dinamica del sistema: α = −20 −0.8 −5 −0.5 (42) Il regolatore sintetizzato è dunque il seguente: K = −14.0029 −0.0033 14 −2.4199 −0.0117 (43) 2.3 Schemi di controllo e confronti 2.3 2.3.1 2 CONTROLLO Schemi di controllo e confronti Schemi di controllo Dopo aver sintetizzato il controllore mediante le due tecniche presentate in precedenza, sono state effettuate delle simulazioni in ambiente Simulink al fine di verificarne l’efficacia, visualizzare l’andamento delle grandezze di stato e rilevare eventuali problematiche o anomalie. Nello specifico, lo schema di riferimento adottato per il controllo del moto longitudinale è il seguente: Figura 4: lo schema Simulink per il controllo del moto longitudinale Mentre nel caso del moto laterale lo schema Simulink è il seguente: Figura 5: lo schema Simulink per il controllo del moto laterale In entrambi i casi il sistema fisico è stato modellato mediante le matrici A, B e C utilizzando un apposito blocco fornito dal Simulink. Per verificare la bontà del controllore si è imposta una leggera perturbazione sulla condizione iniziale, pari ad’inclinazione di circa 6 gradi sull’angolo tra il riferimento verticale ed il corpo del robot. Lo schema Simulink presenta inoltre alcuni blocchi che consentono di introdurre un disturbo nel sistema, mediante il quale è possibile modellare ad esempio eventuali azioni esterne atte a compromettere la stabilità del sistema. 15 2.3 Schemi di controllo e confronti 2.3.2 2 CONTROLLO Risposta del sistema con controllo LQR Di seguito sono riportate le evoluzioni delle grandezze di stato nel caso di controllo LQR, rispettivamente per il moto longitudinale e per il moto laterale. (a) angolo tra ruota e corpo (b) angolo tra corpo e asse verticale (c) velocità ruota (d) velocità corpo Figura 6: evoluzione delle variabili di stato del moto longitudinale 16 2.3 Schemi di controllo e confronti 2 CONTROLLO (a) angolo tra corpo e asse verticale (b) angolo tra disco e corpo (c) velocità corpo (d) velocità disco Figura 7: evoluzione delle variabili di stato del moto laterale 17 2.3 Schemi di controllo e confronti 2.3.3 2 CONTROLLO Risposta del sistema con assegnamento dell’autostruttura Di seguito sono riportate le evoluzioni delle grandezze di stato nel caso di controllo mediante assegnamento dell’autostruttura, rispettivamente per il moto longitudinale e per il moto laterale. (a) angolo tra ruota e corpo (b) angolo tra corpo e asse verticale (c) velocità ruota (d) velocità corpo Figura 8: evoluzione delle variabili di stato del moto longitudinale 18 2.3 Schemi di controllo e confronti 2 CONTROLLO (a) angolo tra corpo e asse verticale (b) angolo tra disco e corpo (c) velocità corpo (d) velocità disco Figura 9: evoluzione delle variabili di stato del moto laterale 19 2.4 Filtro di Kalman 2.3.4 2 CONTROLLO Azioni di controllo Per entrambe le tecniche di controllo, poi, si riportano gli andamento delle azioni di controllo. (a) tensione ai capi del motore della ruota (b) tensione ai capi del motore del disco Figura 10: evoluzione delle azioni di controllo (LQR) (a) tensione ai capi del motore della ruota (b) tensione ai capi del motore del disco Figura 11: evoluzione delle azioni di controllo (pole-placement) 2.4 Filtro di Kalman Al fine di realizzare il prototipo fisico del robot si è reso necessario l’utilizzo di un osservatore dello stato. Per stimare quest’ultimo, infatti, è stato adottato un sistema di misurazione inerziale composto da un accelerometro e da un giroscopio. La necessità di fondere le informazioni provenienti da tali sensori, cosı̀ come quella di pulire le misure dagli inevitabili rumori che si sovrappongono al segnale durante le misure, hanno fatto ricadere la scelta sul filtro di Kalman. Un osservatore è un sistema che consente di utilizzare le informazioni a disposizione riguardo l’ingresso e l’uscita del sistema dinamico da controllare per ottenere una stima del suo stato. Tale stima deve convergere rapidamente al valore reale dello stato, motivo per cui i poli dell’osservatore devono necessariamente essere più veloci di quelli del sistema stesso, facendo attenzione a non 20 2.4 Filtro di Kalman 2 CONTROLLO amplificare eccessivamente il rumore in alta frequenza. Il filtro di Kalman è un particolare osservatore dello stato, utilizzato soprattutto quando sullo stato e sull’uscita sono presenti dei segnali rumorosi. In particolare, questi ultimi sono supposti essere rumori bianchi incorrelati a media nulla. Nella sua forma fondamentale, il filtro di Kalman è caratterizzato dalla seguente equazione: ζ̇(t) = Aζ(t) + Bu(t) + Kf (y(t) − Cζ(t)) (44) dove Kf è il cosiddetto guadagno di Kalman. Quest’ultimo viene scelto in maniera tale da minimizzare il funzionale di costo cosı̀ definito: V (x, t) = min E[eT (t)c(t)cT (t)e(t)] e (45) dove e(t) = x(t) − ζ(t) è l’errore di stima. Si dimostra che se la coppia (AT , GT ) è stabilizzabile e la coppia (AT , C T ) è rivelabile, allora il guadagno è esprimibile nella forma: Kf = S ? C T N −1 (46) con S ? soluzione di un’equazione algebrica di Riccati (ARE) e N matrice di correlazione del rumore di uscita. 21 3 3 3.1 3.1.1 REALIZZAZIONE FISICA DEL PROTOTIPO Realizzazione fisica del prototipo Componenti utilizzati Microcontrollore: Arduino Due Il cuore del sistema di controllo è il microcontrollore Arduino Due, ultima edizione della board. Arduino Due è basato sulla CPU Atmel SAM3X8E, realizzata con architettura ARM e primo processore a 32 bit nella storia di questo progetto open-source. Figura 12: la board Arduino Due La board mette a disposizione 54 pin digitali di I/O (di cui 12 utilizzabili come output PWM), 12 input analogici, 4 porte seriali hardware, un clock a 84 MHz, uno slot USB, 2 convertitori DAC, un jack per la connessione all’alimentazione, un pulsante di reset e diversi altri tra connettori e pulsanti vari. A differenza delle edizioni precedenti, Arduino Due funziona a 3.3 V. 3.1.2 IMU: Invesense MPU-6050 Per ottenere informazioni sullo stato del robot quali la posizione angolare o la velocità è stata utilizzata una Inertial Measurement Unit (IMU) MPU-6050 realizzata dalla società Invesense. Essa fornisce su di un unico chip un accelerometro ed un giroscopio, ciascuno a tre assi, con collegamento digitale mediante protocollo I2C (pin 20 e 21 della board Arduino Due). Figura 13: la IMU Invesense MPU-6050 Le specifiche tecniche disponibili grazie al datasheet fornito dall’azienda produttrice sono le seguenti: 22 3.1 Componenti utilizzati 3 Grandezza Dimensioni Peso Alimentazione Range Velocità a vuoto Corrente a vuoto Corrente in stallo Coppia in stallo REALIZZAZIONE FISICA DEL PROTOTIPO Valore 20 x 15.5 x 3.5 mm 4g 3.3 o 5 V ± 250, ± 500, ± 1000, ± 2000, deg/s ± 2g, ± 4g, ± 8g, ± 16g 500 rpm 300 mA 5000 mA 5 Kg-cm Tabella 1: datasheet della IMU Come descritto nel capitolo precedente, per evitare problemi di stima della posizione dovuti alla deriva del giroscopio, le misure ottenute mediante il presente sensore sono state filtrate mediante un filtro di Kalman. 3.1.3 Motori DC: Pololu 12V, 19:1 con encoder Al fine di fornire alla ruota e al disco inerziale la coppia richiesta per il controllo del robot sono stati utilizzati due motori in corrente continua sviluppati dall’azienda Pololu, con tensione di ingresso massima di 12 V e rapporto di riduzione 19:1. Essi presentano inoltre un encoder in quadratura con risoluzione pari a 64 CPR, che grazie al rapporto di riduzione diventano circa 1200 CPR a valle del riduttore. Figura 14: il motore DC Pololu 12V, 19:1 con encoder Le specifiche tecniche disponibili grazie al datasheet fornito dall’azienda produttrice sono le seguenti: Grandezza Dimensioni Peso Diametro asse Rapporto di riduzione Velocità a vuoto Corrente a vuoto Corrente in stallo Coppia in stallo Valore 37D x 64L mm 213 g 6 mm 19:1 500 rpm 300 mA 5000 mA 5 Kg-cm Tabella 2: datasheet dei motori DC 23 3.1 Componenti utilizzati 3.1.4 3 REALIZZAZIONE FISICA DEL PROTOTIPO Driver: Pololu Dual 12 A , 24 V Motor Shield Al fine di fornire ai motori la tensione di alimentazione necessaria ad esercitare l’azione di controllo sul robot è stato utilizzato un driver di potenza realizzato dalla società Pololu, il quale fornisce fino ad un massimo di 12 A di corrente e 24 V di tensione. Tale driver monta sulla propria board un chip VNH5019 realizzato dalla medesima casa produttrice e particolarmente diffuso nel campo degli azionamenti elettrici in corrente continua. Figura 15: il driver Pololu per l’alimentazione dei motori Uno dei principali vantaggi offerti da tale chip è la presenza di due driver di potenza a bordo della stessa scheda, il che ha reso possibile il controllo in parallelo dei due motori. 3.1.5 Encoder: ATM102-V La fase di testing del robot ha evidenziato un problema relativo alla non sufficiente risoluzione dell’encoder presente sui motori per quanto concerne la stabilizzazione del moto longidutinale. Per tale motivo tale encoder è stato sostituito con un ATM102-V, il quale offre una risoluzione pari a 2048 CPR e dunque consente di stimare la posizione e la velocità della ruota con maggiore precisione. Per migliorare ulteriormente tali letture, l’encoder viene letto ad una velocità 4X, per cui gli impulsi a giro diventano oltre 8000. Figura 16: l’encoder utilizzato sulla ruota 3.1.6 Ruota La scelta della ruota da utilizzare per bilanciare il moto longitudinale è stata fortemente influenzata da diversi parametri. Aspetti legati ad un’eccessiva sollecitazione dei motori, ad un dimensionamento coerente con il resto del sistema e a problematiche nel controllo del moto longitudinale 24 3.2 Caratterizzazione dei motori 3 REALIZZAZIONE FISICA DEL PROTOTIPO hanno infatti imposto dei limiti superiori ed inferiori alle dimensioni della ruota. A valle di diversi test, quindi, è stata scartata l’ipotesi di utilizzare una ruota con superficie di contatto piatta, cosı̀ come si è preferita la gomma alla plastica in termini di materiali. La scelta finale è ricaduta quindi su una ruota distribuita dalla catena Decathlon come pezzo di ricambio per monopattini, avente un diametro pari a 100 mm e rivelatasi particolarmente funzionale al fine di stabilizzare il robot. Figura 17: la ruota utilizzata per stabilizzare il moto longitudinale 3.1.7 Disco Il disco inerziale è stato realizzato lavorando una lastra di plexiglass adoperando una macchina a controllo numerico. In tal modo è stato possibile realizzare tanto la forma complessiva del disco quanto le scanalature interne. La corona circolare esterna è stata inoltre perforata in maniera simmetrica al fine di poter aggiungere con facilità dei pesi (bulloni metallici): in tal modo è stato possibile regolare a piacere il peso complessivo del disco, di particolare importanza nell’economia del bilanciamento del moto laterale. 3.2 Caratterizzazione dei motori L’equazione (16) consente di trovare un collegamento tra la coppia sviluppata all’asse da un motore in corrente continua a magneti permanenti e la tensione fornita in ingresso all’armatura. Ai fini del controllo si è rivelato dunque indispensabile individuare i parametri che figurano in tale equazione, non forniti dal produttore del motore utilizzato. Il datasheet messo a disposizione da Pololu per i motori utilizzati non fornisce direttamente i valori delle costanti di coppia ed elettromagnetica, cosı̀ come la resistenza di armatura. I dati disponibili sono tuttavia in parte sufficienti al fine di determinarne i valori mediante semplici calcoli, i quali sono stati in seguito verificati sperimentalmente al fine di ottenere una stima più precisa delle suddette grandezze. Per determinare il valore della resistenza di armatura si è tenuto conto del fatto che, in stallo, la caduta di tensione è interamente resistiva, per cui risulta Vn = Ra Is . Da questa equatione è stato possibile quindi ricavare il valore di Ra alimentando il motore a tensione nominale (12 V). Il dato è stato verificando facendo variare la tensione di alimentazione tra 0 e 3 V, cosı̀ da avere a disposizione differenti stime della resistenza di armatura. I valori cosı̀ ottenuti discostano di poco da quello ricavato invertendo la precedente equazione, per cui dei dati a disposizione è stato calcolato il valore medio. Per il calcolo della costante di coppia, invece, si è considerato nuovamente il motore in condizione di stallo, per la quale sono forniti i valori di coppia e corrente assorbita. Tali grandezze sono collegate proprio dalla costante Km , la quale può essere semplicemente calcolata come rapporto 25 3.3 Progettazione mecccanica 3 REALIZZAZIONE FISICA DEL PROTOTIPO tra la coppia e la corrente assorbita. Il risultato cosı̀ ottenuto è stato verificato sperimentalmente collegando all’albero una massa nota ad una distanza nota dall’asse. Nota cosı̀ la coppia resistente, affinché la massa fosse rimasta in equilibrio il motore avrebbe dovuto sviluppare una coppia di pari modulo ma segno opposto: i valori di corrente ottenuti nelle condizioni di equilibrio meccanico sono risultati dunque coerenti con la costante di coppia stimata. Infine, è stata calcolata la costante elettromagnetica del motore ricordando che per il rotore vale la relazione Va − Ra Ia = Ea , dove Ea rappresenta la tensione indotta. Quest’ultima è proporzionale alla velocità di rotazione dell’albero proprio secondo tale costante, la quale può essere determinata a Ia . Alimentando il motore a diversi valori di tensione e misurando con l’espressione Ke = V a−R wr la velocità di rotazione dell’albero utilizzando l’encoder a disposizione sono stati stimati diversi valori di Ke , dei quali è stato poi calcolato il valore medio. In definitiva, i parametri stimati del motore sono i seguenti: Grandezza Ra Km Ke Valore 2.4 Ω 0.0063 0.0099 Tabella 3: parametri dei motori 3.3 Progettazione mecccanica Al fine di realizzare un prototipo fisico del robot descritto in precedenza ci si è avvalsi del software Autodesk Inventor per la progettazione di un modello CAD tridimensionale, cosı̀ come per l’ottimizzazione e la determinazione di alcuni parametri meccanici che caratterizzano il sistema. Il modello CAD ha consentito inoltre l’individuazione di una serie di problematiche strutturali e di numerosi dettagli realizzativi da tenere in considerazione durante la fase di assemblaggio. I parametri meccanici del robot (masse, distanze e momenti di inerzia) forniti dal software sono quindi i seguenti: Grandezza mw = m1 mbt m2 l1 lbt l1C r J1A J2C Jw Jbt Valore 0.281Kg 1.475Kg 0.342Kg 0.2916m 0.103m 0.153m 0.05m 0.051625Kgm2 0.003303Kgm2 0.000262Kgm2 0.017097Kgm2 Tabella 4: parametri meccanici del robot Il risultato del lavoro di progettazione al calcolatore è il seguente: 26 3.4 Produzione dei componenti e assemblaggio 3 REALIZZAZIONE FISICA DEL PROTOTIPO (a) vista frontale (b) vista posteriore Figura 18: il prototipo 3D al computer 3.4 Produzione dei componenti e assemblaggio Una volta terminato il lavoro di progettazione del modello CAD al computer, con l’ausilio di quest’ultimo sono stati realizzati i principali componenti della struttura meccanica. In particolare, questi sono stati realizzati utilizzando una macchina a controllo numerico in grado di lavorare differenti materiali. Nello specifico, il corpo del robot è stato realizzato in alluminio, mentre il disco inerziale, i supporti per l’IMU, i cuscinetti ed altri piccoli componenti in plexiglas. 27 3.4 Produzione dei componenti e assemblaggio 3 REALIZZAZIONE FISICA DEL PROTOTIPO Il risultato finale è visibile nelle seguenti immagini: (a) vista laterale (b) vista posteriore Figura 19: il prototipo fisico 28 A A A.1 A.1.1 % % % % % % Appendice Codice Matlab Controllo del moto longitudinale Script Matlab per la simulazione di un robot monociclo File per la linearizzazione del moto longitudinale Falanga Davide − M58/48 Fontanelli Andrea − M58/64 clear all; close all; clc; %Tempo di campionamento Tc=2*10ˆ−3; % Imposto i parametri del sistema Mbt = 1.475; % Massa del corpo M1 = 1.475; % Massa del corpo M2 = 0.342; % Massa del disco Mw = 0.281; % Massa della ruota Rw = 0.05; % Raggio della ruota Jw = 0.000262; % Momento di inerzia della ruota intorno al baricentro Jbt = 0.017097; % Momento di inerzia del corpo intorno al baricentro J1a = 0.051625; % Momento di inerzia del corpo intorno al punto A J2c = 0.003303; % Momento di inerzia del disco intorno al suo centro L1 = 0.2916; % Distanza tra il punto A ed il centro del disco L1c = 0.153; % Distanza tra il punto A ed il baricentro del corpo Lbt = 0.103; % Distanza tra il centro della ruota ed il baricentro del corpo g = 9.81; % Accelerazione di gravita' % Dati del motore kr = 19; % Rapporto di riduzione Vp = 12; % Tensione di alimentazione [V] I max = 1.33; % Corrente massima [A] La = 0.556 *1e−3 ; % Induttanza [H] Ra = 2.4; % Resistenza [Ohm] Ta = La/Ra; % Costante di tempo elettrica [s] Kv = 1/(992.0*2*pi/60); % Costante di velocita' [Vs/rad] Kt = 6.3 *1e−3; % Costante di coppia [Nm/A] Tm = 14.6 * 1e−3; % Costante di tempo meccanica [s] Jm = 45.3 * 1e−3*1e−4; % Inerzia del motore [kg mˆ2] C max = I max * Kt * kr; % Massima coppia continuativa [Nm] Fm = 0; % Coefficiente di frizione del rotore [kg/smˆ2] C max = I max * Kt * kr; %Maximum continuos torque [Nm] % Effettuo la linearizzazione x0=[0;0;0;0]; u0=[0]; [Ar,Br,Cr,Dr]=linmod('ruota dee',x0,u0); ruota=ss(Ar,Br,Cr,Dr) % Osservabilita' obsv mat=obsv(ruota); no obsv=length(Ar)−rank(obsv mat); if no obsv == 0 disp 'Sistema osservabile.'; 29 APPENDICE A.1 Codice Matlab A else disp 'Sistema osservabile.'; end % Controllabilita' contr mat=ctrb(ruota); no contr=length(Ar)−rank(contr mat); if no contr == 0 disp 'Sistema controllabile.'; else disp 'Sistema non controllabile.'; end % Controllo LQR Qr=diag([2,20000,2,1]); Rr=diag([2000]); Kr=lqr(ruota,Qr,Rr); %Assegnamento dei poli Pr=[−0.5,−50,−0.6,−10]; Kr p=place(Ar,Br,Pr); %Controllo assegnamento dei poli eig(Ar−Br* Kr p) A.1.2 % % % % % % Controllo del moto laterale Script Matlab per la simulazione di un robot monociclo File per la linearizzazione del moto laterale Falanga Davide − M58/48 Fontanelli Andrea − M58/64 clear all; close all; clc; %Tempo di campionamento Tc=2*10ˆ−3; % Valori thesis.pdf nostri Mbt = 1.475; % Massa del corpo M1 = 1.475; % Massa del corpo M2 = 0.342; % Massa del disco Mw = 0.281; % Massa della ruota Rw = 0.05; % Raggio della ruota Jw = 0.000262; % Momento di inerzia della ruota intorno al baricentro Jbt = 0.017097; % Momento di inerzia del corpo intorno al baricentro J1a = 0.051625; % Momento di inerzia del corpo intorno al punto A J2c = 0.003303; % Momento di inerzia del disco intorno al suo centro L1 = 0.2916; % Distanza tra il punto A ed il centro del disco L1c = 0.153; % Distanza tra il punto A ed il baricentro del corpo Lbt = 0.103; % Distanza tra il centro della ruota ed il baricentro del corpo g = 9.81; % Accelerazione di gravita' % Dati del motore kr = 19; % Rapporto di riduzione Vp = 12; % Tensione di alimentazione [V] I max = 1.33; % Corrente massima [A] La = 0.556 *1e−3 ; % Induttanza [H] 30 APPENDICE A.2 Codice Arduino A Ra = 2.4; % Resistenza [Ohm] Ta = La/Ra; % Costante di tempo elettrica [s] Kv = 1/(992.0*2*pi/60); % Costante di velocita' [Vs/rad] Kt = 6.3 *1e−3; % Costante di coppia [Nm/A] Tm = 14.6 * 1e−3; % Costante di tempo meccanica [s] Jm = 45.3 * 1e−3*1e−4; % Inerzia del motore [kg mˆ2] C max = I max * Kt * kr; % Massima coppia continuativa [Nm] Fm = 0; % Coefficiente di frizione del rotore [kg/smˆ2] C max = I max * Kt * kr; %Maximum continuos torque [Nm] % Effettuo la linearizzazione x0=[0;0;0;0]; u0=[0]; [Ad,Bd,Cd,Dd]=linmod('disco dee',x0,u0); disco=ss(Ad,Bd,Cd,Dd) % Osservabilita' obsv mat disco=obsv(disco); no obsv disco=length(Ad)−rank(obsv mat disco); if no obsv disco == 0 disp 'Sistema osservabile.'; else disp 'Sistema osservabile.'; end % Controllabilita' contr mat disco=ctrb(disco); no contr disco=length(Ad)−rank(contr mat disco); if no contr disco == 0 disp 'Sistema controllabile.'; else disp 'Sistema controllabile.'; end % Controllo LQR Qd=diag([5000, 2, 20, 1]); Rd=diag([50000]); Kd=lqr(disco,Qd,Rd); %Assegnamento dei poli Pd=[−20,−0.8,−5,−0.5]; Kd p=place(Ad,Bd,Pd); %Controllo assegnamento dei poli eig(Ad−Bd* Kd p) A.2 A.2.1 Codice Arduino Controllore #include <Wire.h> #include "Kalman.h" #define periodo 0 #define tempo calcolo 1.69 int c; float pi=3.1415; uint32 t timer; // Settaggio dei PIN del encoder 31 APPENDICE A.2 Codice Arduino //Encoder 8192 ppr int encoder1PinA = int encoder1PinB = //Encoder 1216 ppr int encoder2PinA = int encoder2PinB = A 52; 53; 48; 49; // Settaggio pin motore int MYINA = 2; int MYINB = 4; int MYENA = 6; int MYPWM = 9; int MXINA = 7; int MXINB = 8; int MXENA = 12; int MXPWM = 10; int dutyX=0; int dutyY=0; Kalman kalmanX; Kalman kalmanY; const uint8 t IMUAddress = 0x68; /* IMU Data */ int16 t accX; int16 t accY; int16 t accZ; int16 t gyroX; int16 t gyroY; int16 t gyroZ; double double double double double double accXangle; accYangle; gyroXangle gyroYangle compAngleX compAngleY double double double double kalAngleX=0; kalAngleY=0; gyroXrate=0; gyroYrate=0; = = = = 180; 180; 180; 180; long int posX=0; long int posY=0; double speedX=0; double speedY=0; double gyroXrate hold=0; double gyroYrate hold=0; double double double double coppia ruota; coppia disco; tensione ruota; tensione disco; double IntAlpha2=0; //double KI=0.15; double KI=0.25; //Parametri motore 32 APPENDICE A.2 Codice Arduino double double double double A Ra=2.4; Km=0.0066; Ke=0.0099; k=19; //Stato double stato ruota[4][1]; double stato disco[4][1]; double guadagni ruota[1][4]; double guadagni disco[1][4]; //////////////////////////////////////////////////////////////// // // // // // FUNZIONE DI SETUP // // // //////////////////////////////////////////////////////////////// void setup() { //Setup IMU Serial.begin(115200); Wire.begin(); i2cWrite(0x6B,0x00); if(i2cRead(0x75,1)[0] != 0x68) { Serial.print("MPU−6050 with address 0x"); Serial.print(IMUAddress,HEX); Serial.println(" is not connected"); while(1); } kalmanX.setAngle(180); kalmanY.setAngle(180); timer = micros(); // Gestione PIN per l'encoder pinMode (encoder1PinA,INPUT); pinMode (encoder1PinB,INPUT); pinMode (encoder2PinA,INPUT); pinMode (encoder2PinB,INPUT); pinMode(MXINA, pinMode(MXINB, pinMode(MXENA, pinMode(MXPWM, pinMode(MYINA, pinMode(MYINB, pinMode(MYENA, pinMode(MYPWM, OUTPUT); OUTPUT); OUTPUT); OUTPUT); OUTPUT); OUTPUT); OUTPUT); OUTPUT); //Abilitazione delle interruzioni sui pin 2 e 3 attachInterrupt(encoder1PinA, GestInt1, CHANGE); attachInterrupt(encoder1PinB, GestInt1, CHANGE); // Vettore dei guadagni ruota ottenuti mediante LQR guadagni ruota[0][0]=−0.0316; guadagni ruota[0][1]=−4.6202; guadagni ruota[0][2]=−0.0557; guadagni ruota[0][3]=−0.5364; // Vettore dei guadagni ruota ottenuti mediante P−P //guadagni ruota[0][0]=−0.0095; //guadagni ruota[0][1]=−4.9821; 33 APPENDICE A.2 Codice Arduino A APPENDICE //guadagni ruota[0][2]=−0.0361; //guadagni ruota[0][3]=−0.6269; // Vettore dei guadagni disco ottenuti mediante LQR guadagni disco[0][0]=−9.1340; guadagni disco[0][1]=−0.0063; guadagni disco[0][2]=−1.4605; guadagni disco[0][3]=−0.0099; // Vettore //guadagni //guadagni //guadagni //guadagni dei guadagni disco ottenuti mediante P−P disco[0][0]=−14.0029; disco[0][1]=−0.0033; disco[0][2]=−2.4199; disco[0][3]=−0.0117; Serial.println("Avvio in corso..."); } //////////////////////////////////////////////////////////////// // // // // // FUNZIONE DI LOOP // // // //////////////////////////////////////////////////////////////// void loop() { // Lettura IMU e filtraggio con Kalman uint8 t* data = i2cRead(0x3B,12); accX = ((data[0] << 8) | data[1]); accY = ((data[2] << 8) | data[3]); accZ = ((data[4] << 8) | data[5]); gyroX = ((data[8] << 8) | data[9]); gyroY = ((data[10] << 8) | data[11]); // Calcolo l'angolo accYangle = (atan2(accX,accZ)+PI)* RAD TO DEG; accXangle = (atan2(accY,accZ)+PI)* RAD TO DEG; // Calcolo la v e l o c i t gyroXrate = (double)gyroX/131.0; gyroYrate = −((double)gyroY/131.0); gyroXangle += kalmanX.getRate()*((double)(micros()−timer)/1000000); gyroYangle += kalmanY.getRate()*((double)(micros()−timer)/1000000); // Effettuo il filtraggio kalAngleX = kalmanX.getAngle(accXangle, gyroXrate, (double)(micros()−timer)/1000000); kalAngleY = kalmanY.getAngle(accYangle, gyroYrate, (double)(micros()−timer)/1000000); timer = micros(); // Encoder speedY=(double)Calc speedY(); speedX=(double)Calc speedX(); posY=Calc posY(); posX=Calc posX(); //Filtro la gyroRate gyroXrate=(gyroXrate+gyroXrate hold)/2; gyroYrate=(gyroYrate+gyroYrate hold)/2; gyroXrate hold=gyroXrate; gyroYrate hold=gyroYrate; // Vettori di stato stato ruota[0][0]=(((double)posY/8192))*2*pi; 34 A.2 Codice Arduino A APPENDICE stato ruota[1][0]=((kalAngleY−180)/360)*2*pi−0.0424+0.0025; stato ruota[2][0]=(speedY)*2*pi/60; stato ruota[3][0]=(gyroYrate/360)*2*pi; stato stato stato stato disco[0][0]=((kalAngleX−180)/360)*2*pi−0.0424+0.0025; disco[1][0]=(((double)posX/8192))*2*pi; disco[2][0]=(gyroXrate/360)*2*pi; disco[3][0]=(speedX)*2*pi/60; // Calcolo delle azioni di controllo int i=0; double temp ruota=0; double temp disco=0; for (i=0;i<4;i++){ temp ruota=stato ruota[i][0]*guadagni ruota[0][i]+temp ruota; temp disco=stato disco[i][0]*guadagni disco[0][i]+temp disco; } coppia ruota=temp ruota; coppia disco=temp disco; //Spengo il controllo superato un certo angolo if(stato ruota[1][0]>0.3||stato ruota[1][0]<−0.3){digitalWrite(MYENA,LOW);}else{ digitalWrite(MYENA,HIGH) if(stato disco[0][0]>0.3||stato disco[0][0]<−0.3){digitalWrite(MXENA,LOW);}else{ digitalWrite(MXENA,HIGH) //Converto la coppia in tensione tensione ruota=(Ra*coppia ruota/(Km*k))+((Ke*k)*stato ruota[2][0]); tensione disco=−(Ra*coppia disco/Km)+(Ke*stato disco[3][0]); // Traduco le azioni di controllo if(tensione disco>=0){ digitalWrite(MXINA,HIGH); digitalWrite(MXINB,LOW); }else{ digitalWrite(MXINA,LOW); digitalWrite(MXINB,HIGH); } if(tensione ruota>=0){ digitalWrite(MYINA,HIGH); digitalWrite(MYINB,LOW); }else{ digitalWrite(MYINA,LOW); digitalWrite(MYINB,HIGH); } //Traduco la tensione in douty cycle dutyY=(double)abs(tensione ruota)/12*255; dutyX=(double)abs(tensione disco)/12*255; //Saturazione if(dutyX>255){dutyX=255;} if(dutyY>255){dutyY=255;} // Esercito le azioni di controllo analogWrite(MXPWM, dutyX); analogWrite(MYPWM, dutyY); //Stampa dati if(c==50){ c=0; Serial.print("Tempo: "); 35 A.2 Codice Arduino A Serial.println(micros()); Serial.print("Angolo x (disco): "); Serial.print(stato disco[0][0]);Serial.print("\t"); Serial.print("Angolo y (ruota): "); Serial.print(stato ruota[1][0],4);Serial.print("\t"); Serial.print("Pos disco: "); Serial.print(stato disco[1][0]);Serial.print("\t"); Serial.print("Pos ruota: "); Serial.print(stato ruota[0][0]);Serial.print("\t"); Serial.print(" gyro disco: "); Serial.print(stato disco[2][0]);Serial.print("\t"); Serial.print(" gyro ruota: "); Serial.print(stato ruota[3][0]);Serial.print("\t"); Serial.print("speed disco: "); Serial.print(stato disco[3][0]);Serial.print("\t"); Serial.print("speed ruota: "); Serial.print(stato ruota[2][0]);Serial.print("\t"); Serial.print("contr disco: "); Serial.print(tensione disco);Serial.print("\t"); Serial.print("contr ruota: "); Serial.print(tensione ruota);Serial.print("\n"); }else{c++;} delay(periodo); } // Funzioni per la gestione della IMU void i2cWrite(uint8 t registerAddress, uint8 t data){ Wire.beginTransmission(IMUAddress); Wire.write(registerAddress); Wire.write(data); Wire.endTransmission(); } uint8 t* i2cRead(uint8 t registerAddress, uint8 t nbytes) { uint8 t data[nbytes]; Wire.beginTransmission(IMUAddress); Wire.write(registerAddress); Wire.endTransmission(false); Wire.requestFrom(IMUAddress, nbytes); for(uint8 t i = 0; i < nbytes; i++) data[i] = Wire.read(); return data; } A.2.2 int int int int int int int int Gestione encoder encoder1PinALast1 encoder1PinBLast1 encoder1PinALast2 encoder1PinBLast2 n1 = LOW; n2 = LOW; n3 = LOW; n4 = LOW; = = = = LOW; LOW; LOW; LOW; 36 APPENDICE A.2 int int int int int int int int Codice Arduino encoder2PinALast1 encoder2PinBLast1 encoder2PinALast2 encoder2PinBLast2 n5 = LOW; n6 = LOW; n7 = LOW; n8 = LOW; A = = = = LOW; LOW; LOW; LOW; long int encoder1Pos = 0; long int encoder2Pos = 0; double n impulsi1=0; double speed1=0; int temp1=0; double n impulsi2=0; double speed2=0; int temp2=0; //////////////////////////////////////////////////////////////// // // // // // Algoritmo di lettura dati dall'encored // // // //////////////////////////////////////////////////////////////// //Encoder // Encoder 1 void GestInt1(){ n1 = digitalRead(encoder1PinA); if ((encoder1PinALast1 == LOW) && (n1 == HIGH)) { if (digitalRead(encoder1PinB) == LOW) { encoder1Pos−−; } else { encoder1Pos++; } } encoder1PinALast1 = n1; n2 = digitalRead(encoder1PinB); if ((encoder1PinBLast1 == LOW) && (n2 == HIGH)) { if (digitalRead(encoder1PinA) == HIGH) { encoder1Pos−−; } else { encoder1Pos++; } } encoder1PinBLast1 = n2; n3 = digitalRead(encoder1PinA); if ((encoder1PinALast2 == HIGH) && (n3 == LOW)) { if (digitalRead(encoder1PinB) == HIGH) { encoder1Pos−−; } else { 37 APPENDICE A.2 Codice Arduino A encoder1Pos++; } } encoder1PinALast2 = n3; n4 = digitalRead(encoder1PinB); if ((encoder1PinBLast2 == HIGH) && (n4 == LOW)) { if (digitalRead(encoder1PinA) == LOW) { encoder1Pos−−; } else { encoder1Pos++; } } encoder1PinBLast2 = n4; //Serial.println(encoder1Pos);; } // Encoder 2 void GestInt2(){ n5 = digitalRead(encoder2PinA); if ((encoder2PinALast1 == LOW) && (n5 == HIGH)) { if (digitalRead(encoder2PinB) == LOW) { encoder2Pos−−; } else { encoder2Pos++; } } encoder2PinALast1 = n5; n6 = digitalRead(encoder2PinB); if ((encoder2PinBLast1 == LOW) && (n6 == HIGH)) { if (digitalRead(encoder2PinA) == HIGH) { encoder2Pos−−; } else { encoder2Pos++; } } encoder2PinBLast1 = n6; n7 = digitalRead(encoder2PinA); if ((encoder2PinALast2 == HIGH) && (n7 == LOW)) { if (digitalRead(encoder2PinB) == HIGH) { encoder2Pos−−; } else { encoder2Pos++; } } encoder2PinALast2 = n7; n8 = digitalRead(encoder2PinB); if ((encoder2PinBLast2 == HIGH) && (n8 == LOW)) { if (digitalRead(encoder2PinA) == LOW) { 38 APPENDICE A.2 Codice Arduino A encoder2Pos−−; } else { encoder2Pos++; } } encoder2PinBLast2 = n8; //Serial.println(encoder2Pos); } //////////////////////////////////////////////////////////////// // // // // // Calcolo delle v e l o c i t // // // //////////////////////////////////////////////////////////////// // Encoder 1 double Calc posY(){ return encoder1Pos; } double Calc speedY(){ n impulsi1=encoder1Pos−temp1; speed1=((n impulsi1/(periodo+tempo calcolo))*1000*60)/8192; temp1=encoder1Pos; return speed1; } // Encoder 2 double Calc posX(){ return encoder2Pos; } double Calc speedX(){ n impulsi2=encoder2Pos−temp2; speed2=((n impulsi2/(periodo+tempo calcolo))*1000*60)/1216; temp2=encoder2Pos; return speed2; } A.2.3 #ifndef #define Libreria Kalman Kalman h Kalman h class Kalman { public: Kalman() { Q angle = 0.001; Q bias = 0.003; R measure = 0.03; bias = 0; P[0][0] = P[0][1] = P[1][0] = P[1][1] = // Resetto il bias 0; 0; 0; 0; }; 39 APPENDICE A.2 Codice Arduino A double getAngle(double newAngle, double newRate, double dt) { /* FASE DI TIME UPDATE */ // Aggiorno il bias e l'angolo rate = newRate − bias; angle += dt * rate; // Aggiorno la stima della covarianza dell'errore P[0][0] += dt * (dt*P[1][1] − P[0][1] − P[1][0] + Q angle); P[0][1] −= dt * P[1][1]; P[1][0] −= dt * P[1][1]; P[1][1] += Q bias * dt; /* FASE DI MEASUREMENT UPDATE */ // Calcolo il guadagno di Kalman S = P[0][0] + R measure; K[0] = P[0][0] / S; K[1] = P[1][0] / S; // Calcolo angolo e bias con le misure y = newAngle − angle; angle += K[0] * y; bias += K[1] * y; // Calcolo P[0][0] −= P[0][1] −= P[1][0] −= P[1][1] −= la covarianza dell'errore K[0] * P[0][0]; K[0] * P[0][1]; K[1] * P[0][0]; K[1] * P[0][1]; return angle; }; void setAngle(double newAngle) { angle = newAngle; }; // Setto l'angolo double getRate() { return rate; }; // Restituite la v e l o c i t /* Funzioni per il tuning del filtro */ void setQangle(double newQ angle) { Q angle = newQ angle; }; void setQbias(double newQ bias) { Q bias = newQ bias; }; void setRmeasure(double newR measure) { R measure = newR measure; }; private: /* Variabili membro */ double Q angle; // Varianza del rumore di processo dell'accelerometro double Q bias; // Varianza del rumore di processo del giroscopio double R measure; // Varianza del rumore di misura double angle; // Angolo double bias; // Bias double rate; // V e l o c i t double double double double P[2][2]; // Matrica della covarianza dell'errore K[2]; // Guadagno di Kalman y; // DIfferenza angolo S; // Errore di stima }; #endif 40 APPENDICE RIFERIMENTI BIBLIOGRAFICI RIFERIMENTI BIBLIOGRAFICI Riferimenti bibliografici [1] Jasha van Pommeren. “The Unibot - Design and Control of a Self-Balancinf Unicycle Robot”. In: (2007). 41