controllo di un mini segway

Controllo di un mini Segway

Studente/i

Angelo Antonio Fraguglia

Relatore

Roberto Bucher

Correlatore

Mikael Andreas Bianchi

Committente

Roberto Bucher

Corso di laurea

Ingegneria Elettronica

Modulo

C10056

Anno

2018/2019

Data

4 ottobre 2019

i

Indice

1 Introduzione 3

1.1 Motivazioni . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4

1.2 Obiettivi . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4

1.3 Compiti . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4

2 Pianificazione del progetto 7

3 Altium e PCB 9

3.1 Progettazione . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9

3.2 Messa in opera . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13

4 Il motore e la centralina 15

4.1 Il motore BRUSHLESS (senza spazzole) . . . . . . . . . . . . . . . 15

4.1.1 Applicazioni . . . . . . . . . . . . . . . . . . . . . . . . . . . 15

4.1.2 Vantaggi . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16

4.1.3 Svantaggi . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16

4.1.4 Conclusioni . . . . . . . . . . . . . . . . . . . . . . . . . . . 17

4.2 Il Faulhaber 4221 G024 BXT H . . . . . . . . . . . . . . . . . . . . . 18

4.3 La centralina Faulhaber MC 5005 S CO . . . . . . . . . . . . . . . . 19

5 Filtro complementare (Complemtary Filter) 21

5.1 Il Filtro . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21

5.2 Coefficienti . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22

5.3 Implementazione del filtro con Matlab . . . . . . . . . . . . . . . . . 23

5.4 Implementazione Simulink . . . . . . . . . . . . . . . . . . . . . . . 28

6 Modellazione di sistemi elettromeccanici con Lagrange [3] 29

6.1 Definizione della "Lagrangiana" . . . . . . . . . . . . . . . . . . . . . 29

6.2 Definizione delle energie cinetiche del sistema T . . . . . . . . . . . 30


ii INDICE

6.3 Definizione dell’ energia potenziale del sistema U . . . . . . . . . . . 31

6.4 Definizione dell’energia dissipata dal sistema . . . . . . . . . . . . . 32

6.5 La Lagrangiana . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32

6.6 wxMaxima . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 33

7 Definizione dei parametri 35

7.1 Parametri motore . . . . . . . . . . . . . . . . . . . . . . . . . . . . 35

7.2 Python e pysimCoder . . . . . . . . . . . . . . . . . . . . . . . . . . 39

7.3 Parametri body . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 42

8 Il controllo del sistema 47

8.1 Indici di prestazione [4] . . . . . . . . . . . . . . . . . . . . . . . . . 47

8.2 Il controllore LQR (Linear Quadratic Regulator)[4] . . . . . . . . . . . 49

8.3 Python e pysimCoder applicati al sistema . . . . . . . . . . . . . . . 51

8.3.1 Simulazione . . . . . . . . . . . . . . . . . . . . . . . . . . . 51

8.3.2 Sistema reale . . . . . . . . . . . . . . . . . . . . . . . . . . 60

9 Conclusioni 63

10 Costi 65

11 Allegati 69

12 Ringraziamenti 71


iii

Elenco delle figure

3.1 Altium_Piedinatura_STEVALMKI159V1 . . . . . . . . . . . . . . . . 9

3.2 Zoccolino_Passante . . . . . . . . . . . . . . . . . . . . . . . . . . . 10

3.3 Schematic_PCB . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11

3.4 PCB_blue_layer . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12

3.5 PCB_red_layer . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12

3.6 PCB_Finito . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13

3.7 PCB_Montato . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13

4.1 Motore Brushless . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15

4.2 Faulhaber 4221 G024 BXT H . . . . . . . . . . . . . . . . . . . . . . 18

4.3 Faulhaber MC5005 S CO . . . . . . . . . . . . . . . . . . . . . . . . 19

5.1 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 23

5.2 Codice Funzione Ang Matlab . . . . . . . . . . . . . . . . . . . . . . 24

5.3 Angle 1 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25

5.4 Angle 2 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26

5.5 Calcolo Errore . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26

5.6 Angle 3 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 27

5.7 Simulink 1 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 28

5.8 Simulink 2 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 28

6.1 Modello Segway . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 30

7.1 Simulinc_Angle_Motor . . . . . . . . . . . . . . . . . . . . . . . . . 36

7.2 Matlab_Code_Fdt . . . . . . . . . . . . . . . . . . . . . . . . . . . . 37

7.3 /Matlab_Code_Mymot . . . . . . . . . . . . . . . . . . . . . . . . . . 37

7.4 FdT . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 38

7.5 Plot_Dati_Fdt . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 38

7.6 code_py_alpha_k . . . . . . . . . . . . . . . . . . . . . . . . . . . . 39


iv ELENCO DELLE FIGURE

7.7 Grafico_alpha_k . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 40

7.8 Coefficienti_k_α . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 40

7.9 Det_J . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 41

7.10 Det_Dm . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 41

7.11 Segway_Appeso . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 42

7.12 Grafico_Oscillazioni . . . . . . . . . . . . . . . . . . . . . . . . . . . 43

7.13 Teorema_Steiner . . . . . . . . . . . . . . . . . . . . . . . . . . . . 44

7.14 Inertia_cm_Steiner . . . . . . . . . . . . . . . . . . . . . . . . . . . 44

8.1 Simulazioni . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 51

8.2 Import_Librerie . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 51

8.3 Parametri_e_Costanti . . . . . . . . . . . . . . . . . . . . . . . . . . 52

8.4 Matrici_Sistema . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 52

8.5 Schema_Blocchi_Full_Obs . . . . . . . . . . . . . . . . . . . . . . . 53

8.6 Sysc . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 53

8.7 c2d . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 53

8.8 Method_zoh . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 54

8.9 LQR_Controller . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 54

8.10 Poli_Sistema_Controllato . . . . . . . . . . . . . . . . . . . . . . . . 55

8.11 Full_Obs . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 55

8.12 Sys_Completo . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 56

8.13 Settings_Template . . . . . . . . . . . . . . . . . . . . . . . . . . . . 56

8.14 Block_Complete . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 57

8.15 Ass_Funz_Comp . . . . . . . . . . . . . . . . . . . . . . . . . . . . 57

8.16 Ass_Funz_sysc . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 58

8.17 Grafico_Simulazione . . . . . . . . . . . . . . . . . . . . . . . . . . 59

8.18 Prove_Laboratorio . . . . . . . . . . . . . . . . . . . . . . . . . . . . 60

8.19 Tx_Rasp . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 60

8.20 Rx_Ctrl . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 61

8.21 Prove_Coefficienti . . . . . . . . . . . . . . . . . . . . . . . . . . . . 62


v

Elenco delle tabelle

10.1 Tabella_costi . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 65


vi ELENCO DELLE TABELLE


1

Abstract

The purpose of this project was to realize the control system of a mini Segway using

a Raspberry Pi 3B + microcontroller.

It has been necessary to create a small PCB on which the inertial platform tested in

the previous project has been mounted.

To control the motor driver it is needed an intarface between the microcontroller

(which uses an SPI interface to dialogue with the sensor) and the driver (which

uses BUS CANopen interface). A dedicated card has been made in SUPSI in the

previous years. The three cards has been assembled.

Some tests have been made to check the proper functioning of the system.

Than a suitable control system has been projected ad simulated.

Finally the control system has been tested on the real model.


2 ELENCO DELLE TABELLE


3

Capitolo 1

Introduzione

Il controllo di un segway propone la soluzione di diversi problemi di controllo e sen-

sorica. Un sistema meccanico, da completare, è già presente in laboratorio.

Con questo progetto si vuole preparare il sistema per essere controllato, ed even-

tualmente, arrivare al controllo vero e proprio.

La piattaforma di controllo è basata su un microcalcolatore Raspberry PI 3B+, a

cui dovranno essere aggiunte una scheda per il controllo del bus CAN (schema già

presente in SUPSI) ed una piattaforma inerziale per la lettura di velocità angolari ed

accelerazioni frutto di un precedente lavoro di semestre.

Una volta realizzata la piattaforma HW, si potrà passare alla gestione dei sensori ed

al controllo del sistema. Un motore Faulhaber di nuova generazione permetterà la

movimentazione di tutto il sistema.


4 Introduzione

1.1 Motivazioni

Una vecchia pubblicità di pneumatici recitava:"La potenza è nulla senza controllo".

Riuscire a far fare alle macchine quello che si desidera sembra molto semplice visto

dall’utente finale ma non è di sicuro elementare da parte di chi progetta questo tipo

di sistemi.

L’incremento di automatismi nella vita quotidiana spinge a credere che in futuro sia

sempre più importante avere una solida base su questa materia.

Frequentando i corsi di Dinamica e stabilità e Progettazione di controllori, mi

sono reso conto di quanto poco sapessi in materia, ma ho finalmente capito come

si utilizzano le conoscenze matematiche apprese con fatica durante tutto il percorso

formativo applicandole alla fisica.

Sono convinto che immergermi ulteriormente nella materia sia un’ ottima occasione

per aggiungere qualcosa di significativo al curriculum.

Il progetto impone inoltre di effettuare il design di un piccolo PCB, cosa che non ho

avuto ancora l’occasione di fare e (secondo me) di significativa importanza per un

ingegnere.

1.2 Obiettivi

• Integrazione del sensore e lettura degli angoli tramite accelerometro e giro-

scopio, insieme ad un filtro complementare o ad un filtro di Kalmann

• Modellare il sistema

• Realizzare un sistema di controllo per mantenere in equilibrio il segway

• Eventualmente utilizzare un sistema per comandare le posizioni dall’esterno

• Documentazione finale del sistema

1.3 Compiti

• Integrare una piattaforma inerziale nel sistema formato dal microcontrollo-

re Raspberry PI 3B+, assieme alla scheda già realizzata in SUPSI per la

gestione del bus CAN


5

• Leggere i sensori e realizzare il filtro complementare o filtro di Kalmann per la

definizione dell’angolo di inclinazione

• Realizzare il sistema di controllo, basandosi sulla piattaforma libera di CACSD

basata su Python e realizzata presso la SUPSI


6 Introduzione


7

Capitolo 2

Pianificazione del progetto

Si comincia col progettare il PCB. Appena pronto si effetua l’ordinazione e viene

assemblata la scheda.

Si lavora alla taratura del motore con il software dedicato Faulhaber e l’interfaccia

BUS CAN.

Viene poi assemblato tutto e si comincia a lavorare sul controllo studiando le equa-

zioni che regolano il moto del Segway.

Si eseguono varie prove di precisione della posizione, cambiando i valori del filtro

complementare per cercare in modo empirico il coefficiente che ottimizza la preci-

sione dell’angolo.

Viene assemblato il segway.

Si comincia a lavorare sul controllo e si esguono varie prove con il sistema montato.


8 Pianificazione del progetto


9

Capitolo 3

Altium e PCB

3.1 Progettazione

La progettazione del PCB con i 3 componenti risulta relativamte semplice.

L’unica difficoltà che si incontra è con le dimensioni non standard del sensore a

causa della distanza (15.24 mm) tra le due file di pin più larghe di quelle dei nor-

mali IC già presenti nel dabase di Altium, per cui si deve provvedere a disegnare

manualmente un apposito componente ed inserirlo nella libreria del progetto.

Figura 3.1: Altium_Piedinatura_STEVALMKI159V1


10 Altium e PCB

Si decide di utilizzare per il micro uno zoccolo con i pin particolarmente lunghi che

restituisca la piedinatura dello stesso in modo che sia possibile collegare a "cascata"

più schede.

Figura 3.2: Zoccolino_Passante


11

Lo schematico del circuito risulta poi abbastanza semplice, si devono creare le 4

piste per la comunicazione tramite SPI, si deve inoltre pensare all’alimentazione ed

alla massa per il sensore che vengono fornite direttamente dal micro.

Si decide di aggiungere un jumper per decidere con quale Chp_Select lavorare,

lasciando la possibilità di poter attivare anche il magnetometro.

Figura 3.3: Schematic_PCB


12 Altium e PCB

I 2 layers del PCB vengono utilizzati come piani di massa per il circuito, si provvede

anche a prevedere i fori per l’accoppiamento meccanico tra la scheda ed il micro.

Figura 3.4: PCB_blue_layer

Figura 3.5: PCB_red_layer


13

3.2 Messa in opera

Una volta arrivati i componenti, si eseguono le saldature e la scheda è pronta.

Figura 3.6: PCB_Finito

Il corretto funzionamento viene collaudato connettendo la scheda al micro e man-

dando in esecuzione il programma di lettura dei dati.

Viene anche controllato l’accoppiamento meccanico in modo da per poter montare

poi la scheda superiore.

Figura 3.7: PCB_Montato


14 Altium e PCB


15

Capitolo 4

Il motore e la centralina

4.1 Il motore BRUSHLESS (senza spazzole)

Figura 4.1: Motore Brushless

4.1.1 Applicazioni

Esistono applicazioni navali di tali motori con tecnologie a superconduzione che

hanno potenze dell’ordine di MW. Ultimamente sono molto utilizzati in campo indu-

striale e impiegati nelle macchine automatiche che necessitano di movimenti precisi

e veloci con ingombri decisamente contenuti e grande versatilità, offerta anche da

apparecchiature di controllo programmabili molto sofisticate che danno un completo

controllo e diagnostica del motore. Questa tipologia di motore viene utilizzata nella


16 Il motore e la centralina

Formula E.

Recentemente, grazie alla spinta commerciale legata alla richiesta di prodotti ad

elevata efficienza energetica nel settore civile, questi motori sono stati introdotti sul

mercato del condizionamento e della refrigerazione, dove possono ottenere elevati

benefici in termini di consumi a favore dei grandi utilizzatori (ad esempio: la grande

distribuzione organizzata che li può installare sui ventilatori che operano sugli scam-

biatori dei banchi frigo; gli uffici di ampie dimensioni, che li possono montare nei fan

coil utilizzati nei sistemi di condizionamento distribuito).

Con la diminuzione dei costi dei sistemi di controllo elettronici, prodotta da economie

di scala, è divenuta di rilievo la maggiore convenienza e conseguente estensione di

utilizzo dei motori brushless anche per l’azionamento di bicicli, motocicli, e autovei-

coli a trazione elettrica.[1]

4.1.2 Vantaggi

Il primo notevole vantaggio riguarda la vita attesa del motore, dato che le spazzole

sono il "punto debole" di un motore elettrico. L’assenza di spazzole elimina anche la

principale fonte di rumore elettromagnetico presente nei motori elettrici in corrente

continua.

L’ingombro è limitato rispetto alla potenza e soprattutto rispetto alla coppia che que-

sti motori riescono ad erogare. In termini di efficienza, i motori brushless lavora-

no sempre in condizioni di rendimento ottimali. Non dovendo generare il campo

magnetico rotorico, hanno efficienze maggiori. L’assenza di scintille è fondamen-

tale quando il motore opera in ambienti saturi di composti organici volatili come i

carburanti.[1]

4.1.3 Svantaggi

Il principale svantaggio di questo tipo di motori sta nel maggiore costo. A differenza

dei motori a spazzole, infatti, il controllo viene effettuato elettronicamente da un con-

troller, un dispositivo elettronico fornito dal costruttore del motore o da terze parti,

quindi al costo del motore va aggiunto il costo del sistema di controllo. Per i moto-

ri a spazzole il "controllo" è realizzato da un potenziometro o un reostato (sistemi

poco efficienti, ma estremamente economici) per la regolazione della velocità. La

resistenza variabile, potenziometro o un reostato che sia, consente di variare l’inten-

sità di corrente assorbita dai motori. Questo sistema di controllo può anche essere


17

utilizzato per più motori analoghi in parallelo, mentre ogni motore brushless deve

essere controllato da un suo dispositivo "controller".[1]

4.1.4 Conclusioni

In conclusione questo tipo di motore si rivela ideale per questo tipo di applicazione

in quanto fornisce un’ottima coppia in avvio e velocità di inversione del senso di

marcia.



4.2 Il Faulhaber 4221 G024 BXT H

Figura 4.2: Faulhaber 4221 G024 BXT H

Caratterizzato da dimensioni molto ridotte ma dotato di una coppia molto generosa

(max 112mNm) per un consumo di 40W, la versione a 24V di questo motore sembra

essere ideale per l’applicazione sul mini Segway.


19

4.3 La centralina Faulhaber MC 5005 S CO

Figura 4.3: Faulhaber MC5005 S CO

La centralina lavorando con l’iterfaccia CANopen, interpreta i dati che le vengono

passati dal microprocessore in modo da poter pilotare in maniera ottimale il motore.

Quindi, oltre alla scheda preparata nel lavoro precedente aggiungiamo una scheda

di conversione tra SPI e CANopen.


21

Capitolo 5

Filtro complementare (Complemtary

Filter)

5.1 Il Filtro

Giroscopi ed accelerometri sono sensori progettati per misurare la velocità angolare

e l’accelerazione. Le versioni attuali possono fornire in uscita i relativi dati già con-

vertiti in digitale, nel caso specifico sui tre assi x,y,z.

Volendo invece avere informazioni relative all’angolo che viene a formarsi rispetto ad

uno degli assi, le misure effettuate non bastano in quanto i giroscopi sono soggetti

ad un errore di deriva dovuto all’integrazione della velocità angolare (è integrando

la velocità angolare che si ottiene l’angolo). Nell’accelerometro l’errore è dovuto a

forze di traslazione. Specificando meglio, i giroscopi lavorano bene con veloci varia-

zioni dell’angolo mentre gli accelerometri con variazioni lente.

Al fine di migliorare la precisione della stima dell’angolo assoluto si ricorre all’utilizzo

di filtri. Per esempio il filtro di Kalman è molto efficacie ma ha come svantaggio la

complessità di calcolo e quindi il tempo computazionale molto lungo.

Il filtro complementare (più semplice) sfrutta quanto di meglio si riesce ad ottene-

re da ogni sensore: la misura tramite giroscopio ha una buona precisione ad alte

frequenze, mentre quella tramite accelerometro ha buona precisione con variazioni

lente. Segue lo schema a blocchi del filtro.


22 Filtro complementare (Complemtary Filter)

θacc

θgyro

LP_Filter G1

∑θf

∫θgyro HP_Filter G2

5.2 Coefficienti

Calcolare i coefficienti di questo filtro può non essere facile a causa della differnza

tra le due frequenze. Inoltre, trasformando tutto in Laplace e definendo G1 la fun-

zione di trasferimento del filtro passa basso e G2 quella del passa alto deve anche

valere

G1 +G2 = 1 (5.1)

in modo da non alterare il valore in uscita.

L’equazione che si ricava risulta:

θf =1

s

[θgyro −

(Kp +

Ki

s

)(θf − θacc)

](5.2)

risolvendo per θf nel dominio di Laplace

θf =s2

s2 +Kps+Ki

(1

sθgyro

)+

Kp +Ki

s2 +Kp +Ki

θacc (5.3)

Semplificando si ottengono 2 coefficienti. Il primo moltiplica l’angolo ottenuto inte-

grando il valore ricevuto dal giroscopio ed il secondo l’angolo ottenuto dall’accelero-

metro in una formula di questo tipo

θf(t+1) = K1

(θf(t) + θgyro(t+1) ∗∆t

)+K2

(tan−1

accz(t+1)

accy(t+1)

)(5.4)

dove deve valere

K1 +K2 = 1 (5.5)

Nel caso specifico, per migiorare le prestazioni, al posto di usare il punto dell’acce-

lerazione sull’asse delle y viene utilizzata la retta tra tra i 2 punti sugli assi x e y e


23

l’equazione diventa

θf(t+1) = K1

(θf(t) + θgyro(t+1) ∗∆t

)+K2

tan−1 accz(t+1)√acc2y(t+1) + acc2x(t+1)

(5.6)

5.3 Implementazione del filtro con Matlab

Viene programmato un file in Matlab in modo da poter analizzare i dati, effettuando

delle prove con il sensore montato su una ruota di test.

Figura 5.1:



Figura 5.2: Codice Funzione Ang Matlab

Partendo da un punto certo, indicato dalla livella che segna lo zero, si fa girare la

ruota nei due sensi e si analizzano i dati raccolti dall’encoder montato direttamente

sul motore e quelli raccolti dall’IMU.

Creando delle apposite matrici e moltiplicando i dati per i coefficienti in modo da

poter avere il valore di entrambi gli angoli in radianti, si può infine calcolare l’errore

e si possono confrontare i grafici.


25

Figura 5.3: Angle 1

Si nota subito che sopra un certo valore i due segnali divergono. Osservando atten-

tamente si nota che cominciano a divergere a π2

.

Analizzando dettagliatamente le matrici, ci si accorge che l’accelerometro sull’asse

z non cambia di segno se viene capovolto il sensore, si risolve il problema imple-

mentando i calcoli e facendo in modo che quando il valore dell’angolo supera giusto

il punto di cambio del segno il calcolatore lo moltiplichi per -1.

Operazione comunque superflua nell’applicazione attuale in quanto π2

è ben al di

fuori del campo di oscillazione previsto per il Segway.

Si rieseguono prove e calcoli.



Figura 5.4: Angle 2

Figura 5.5: Calcolo Errore


27

Come si nota dal grafico, rimanendo sotto il valore di π2

non vi è più il problema

riscontrato nelle prove precedenti. Si nota inoltre come variando a bassa velocità i

2 grafici convergono quasi perfettamente mentre aumentado quest’ultima tendano

a divergere.

L’errore medio (media matematica del vettore RMSE) in questo caso risulta:

mean∑√

(angenc − angIMU)2 = 0.07[rad] (5.7)

Si effetua un’altra prova muovendo lentamente la ruota:

Figura 5.6: Angle 3

Si osserva come ora le curve sono quasi sovrapposte e l’errore diventa:

mean∑√

(angenc − angIMU)2 = 0.0182[rad] (5.8)



5.4 Implementazione Simulink

Viene creato anche un file in Simulink da prendere come spunto per poter imple-

mentare poi il sistema con pysimCoder.

Figura 5.7: Simulink 1

Figura 5.8: Simulink 2


29

Capitolo 6

Modellazione di sistemi

elettromeccanici con Lagrange [3]

6.1 Definizione della "Lagrangiana"

Nella modellazione con il metodo di Lagrange al posto di utilizzare le equazioni

newtoniane per descrivere le forze nel sistema ci si basa sulle energie e si utilizza-

no coordinate indipendenti qi (coordinate generalizzate). Vengono definite:

U Energia potenziale associata al sistema

T Energia cinetica associata al sistema

D Energia dissipata da sistema

L La "Lagrangiana" del sistema: L = T - U

Definita la Lagrangiana, le equazioni differenziali associate al sistema diventano

d

dt

δLδqi

+δD

δqi− δLδqi

= Fext (6.1)


30 Modellazione di sistemi elettromeccanici con Lagrange [3]

6.2 Definizione delle energie cinetiche del sistema T

Figura 6.1: Modello Segway

Il modello d’esempio in figura mostra quali sono le grandezze del sistema.

Vengono definite

M = massa del carrello

m = massa del pendolo

L = distanza del centro di massa dal fulcro

ϕb = angolo tra il pendolo e l’asse y

ϕw = angolo della ruota del carrello


31

Rw = raggio delle ruote

xw = Rwϕw = posizione del carrello

xb = Rwϕw − L sin(ϕb) = posizione del pendolo asse x

yb = L cos(ϕb) = posizione del pendolo asse y

Tqw = forza in ingresso

d = coefficiente d’attrito delle ruote

Si definisccono l’energia cinetica totale, l’energia del carrello ruote (wheels) Tw e

quella del pendolo (body) Tb

TW =Θw

(ddtϕw)2

2+M Rw

2(ddtϕw)2

2

Tb =m((Rw

(ddtϕw)− L cos (ϕb)

(ddtϕb))2

+ L2 sin (ϕb)2( d

dtϕb)2)

2+

Θb

(ddtϕb)2

2

Ttot = TW + Tb (6.2)

quindi:

Ttot =m((Rw


(ddtϕb))2

+ L2 sin (ϕb)2( d

dtϕb)2)

2+

+Θw

(ddtϕw)2

2+M Rw

2(ddtϕw)2

2+

Θb

(ddtϕb)2

2

6.3 Definizione dell’ energia potenziale del sistema U

L’energia potenziale nel sistema è dovuta al solo body:

U = Lgm cos (ϕb)



6.4 Definizione dell’energia dissipata dal sistema

L’unica componente dissipata dal sistema è dovuta all’attrito delle ruote e viene

definita:

D =d(ddtphiw

)22

6.5 La Lagrangiana

Ai fini di semplificare il modello viene deciso di ignorare le perdite, quindi la Lagran-

giana risulta:

L = T − U

L =m((Rw


(ddtϕb))2

+ L2 sin (ϕb)2( d

dtϕb)2)

2+

+Θw

(ddtϕw)2

2+M Rw

2(ddtϕw)2

2+

Θb

(ddtϕb)2

2− Lgm cos (ϕb)

La quale inserita in apposito software di calcolo (wxMaxima) con anche le perdite

del sistema D (che però non vengono utilizzate poi nei calcoli) e definite le tutte le

variabili ci resituisce direttamente le matrici Jacobiane con le quali, una volta inserito

il punto di lavoro desiderato ed i valori delle variabili, si può cominciare a studiare

un sistema di controllo.


33

6.6 wxMaxima

Maxima è un software matematico di tipo CAS (Computer Algebra System) in grado

di eseguire calcoli numerici ma anche simbolici che è la parte che qui verrà utilizza-

re per estrarre le matrici dalla Lagrangiana.

È open source e può essere utilizzato da linea di comando oppure tramite un’inter-

faccia grafica chiamata appunto wxMaxima.

Queso software offre una serie di comandi molto utili tra i quali:

• expand: espande l’equazione eseguendo tutte le operazioni e cercando quin-

di di semplificare il più possibile

• trigreduce: combina prodotti ed esponaziali delle funzioni trigonometriche in

modo da poter semplificare (formile di prostaferesi e Werner)

• trigsimp: semplifica con l’obiettivo di far appare nelle espressini sin2 e cos2

in modo da poterli sommare

• linsolve: risolve l’equazione in funzione della variabile desiderata

• jacobian: passando a questa funzione le equazioni e le variabili questa ri-

torna direttamente la matrice Jacobiana necessaria per la linearizzazone del

sistema, essendo questo di secondo grado

• at: valuta l’espressione assegnando alle variabili i valori desiderati. Viene

utilizzata per impostare il punto di lavoro del sistema

• ratsimp: semplifica se possibile anche le funzioni irrazionali

In allegato il codice utilizzato per il sistema in analisi di seguito le matrici A e B

ricavate dopo tutte le semplificazioni:

A =

0 1 0 0

0 0 − L2Rwgm2

(L2 Thetaw+Rw2Thetab+L2M Rw

2)m+ThetabThetaw+M Rw2Thetab

0

0 0 0 1

0 0LRw

2gm2+(LThetaw+LM Rw2)gm



0



B =

0

L2m+Thetab



0

− LRwm




35

Capitolo 7

Definizione dei parametri

7.1 Parametri motore

Si procede alla ricerca dei parametri non definiti (gli attriti e le inerzie) partendo dalle

equazioni che governano il sistema. Successivamente si fanno delle simulazioni

fisiche. Infine con i dati ottenuti vengono restituiti i coefficienti dal calcolatore.

J

(d

dtωw

)=∑

τn = KtIa −Dmωw

dove il termine

J

(d

dtωw

)indica il totale delle forze agenti sul sistema che sono l’attrito (J = momento d’iner-

zia)

Dmωw

e l’energia in ingresso

ktIa

questo termine tiene conto anche della corrente in ingresso , ma in questo tipo

di sistema la corrente la determina la centralina che pilota il motore alla quale in

ingresso viene quindi dato solo un valore numerico proporzionale a kt.

Portata l’equazione nel dominio di Laplace, troviamo una funzione di trasferimento:

g =k

s2 + αs


36 Definizione dei parametri

dove :

k =kt

J

ed:

α =Dm

J

dalla quale si vogliono ricavare i valori di k ed α.

Si procede quindi a dare uno step ben definito in ingresso e si raccolgono i dati di

posizione in uscita forniti dall’encoder. Successivamente si inviano i dati ad un’appo-

sita funzione di Matlab la quale ritorna la funzione di trasferimento con i coefficienti

k ed α.

Quindi si procede alla raccolta dei dati con piccolo programma Similink

Figura 7.1: Simulinc_Angle_Motor

Questo permette di vedere in tempo reale l’andamento degli angoli in funzione del

tempo e contemporaneamente salva i dati in una matrice in modo da poterli poi

riplottare in una funzione del tipo: y = f(t).

Di seguito i codici utilizzati:


37

Figura 7.2: Matlab_Code_Fdt

Figura 7.3: /Matlab_Code_Mymot



La funzione lsqcurvefit risulta molto utile in quanto in base agli array di dati che

le vengono forniti ed al tipo di funzione di trasferimento che si vuole ottenere trova

i coefficienti in modo tale che la funzione di trasferimento trovata approssimi nel

modo migliore possibile risolvendo il problema dei minimi quadrati.

Cioè facendo in modo che lo scarto quadratico sia il minimo possibile.

Si ottiene così la FdT:

Figura 7.4: FdT

Come si può osservare dal grafico i coefficienti trovati approssimano molto bene la

curva fornita dai dati delle misure.

Figura 7.5: Plot_Dati_Fdt


39

7.2 Python e pysimCoder

Si determinano i coefficienti anche utilizzando python, che è il software CAS che

viene utilizzato da pysimCoder per le simulazioni.

Viene definita una funzione molto simile a quella utilizzata precedentemente con

Matlab, la quale viene passata al comando leastsq che provvede a calcolare i

coefficienti.

Figura 7.6: code_py_alpha_k



Si controlla che anche in questo caso il grafico sia coerente coi dati misurati

Figura 7.7: Grafico_alpha_k

Quindi vengono trovati i coefficienti che permettono l’identificazione dei parametri

Dm e J .

Si ottiene così un parametro k molto più piccolo di quello calcolato in precedenza.

Ed a partire da questo, conoscendo il parametro kt, si determinano il momento

d’inerzia J ed il coefficiente d’attrito DM

Figura 7.8: Coefficienti_k_alpha

dove il primo termine dell’array è il coefficiente k ed il secondo α. Quindi partendo

da questi e conoscendo il valore di kt, si determinano il momento d’inerzia J ed il

coefficiente d’attrito dinamico Dm.


41

J :

Figura 7.9: Det_J

Dm:

Figura 7.10: Det_Dm



7.3 Parametri body

Per il body si procede in modo analogo ma invece che far girare il motore lo si tiene

fermo e viene movimentato il corpo del segway.

Quindi vengono fissate le ruote e fatto oscillare il corpo in modo da poter misurare i

parametri riguardanti lo stesso.

Figura 7.11: Segway_Appeso


43

Una volta fissato il sistema, il metodo per raccogliere i dati è il medesimo delle prove

riguardanti il motore, semplicemente non si aziona il medesimo. In questo caso le

equazioni necessitano di un altro tipo di soluzione, quindi il metodo per trovare i

coefficienti cambia.

In questo caso viene cercata l’inerzia nel centro di massa del body.

Quindi viene fatto oscillare lo stesso, tenendo ferme le ruote fino a quando non

ritrova la sua posizione di riposo, come in figura 7.11.

Vengono poi raccolti i dati e plottati in un grafico che ci permette di contare il numero

di oscillazioni ed il tempo in modo da poter ricavare la frequenza di oscillazione.

Figura 7.12: Grafico_Oscillazioni

Nota la frequenza di oscillazione viene utilizzata la formula:

ω2oscill =

mgRcm

Jp

quindi

Jp =mgRcm

ω2oscill

dlla quale viene ricavata l’inerzia nel punto dove il body è collegato meccanicamente

alle ruote.

Si procede quindi tramite il teorema di Steiner a trovare l’inerzia nel punto desiderato



Figura 7.13: Teorema_Steiner_Wiki [1]

quindi con i dati a disposizione

Jcm = Jp −mR2cm

i dati vengono inseriti nel calcolatore

Figura 7.14: Inertia_cm_Steiner

Vengono così ottenuti tutti i parametri di non semplice misurazione.


45

Ci si affida invece alle misure per quello che riguarda le 2 masse (vengono separati

e pesati carrello e body) ed il centro di massa.


47

Capitolo 8

Il controllo del sistema

8.1 Indici di prestazione [4]

Un processo di controllo classico determina i valori di sovraelongazione (overshoo-

ting) oppure tempo di setting.

È però possibile anche determinare il controllo di un sistema in funzione di alcuni

"indici di prestazione". Questi possonono essere ottimizzati per il tempo:

J =

∫ t∞

t0

dt = tf − t0

oppure per il consumo:

J =

∫ t∞

t0

r∑i=1

|u| dt

energia minima:

J =

∫ t∞

t0

uT ·R · u dt

minima differenza rispetto all’equilibrio

J =

∫ t∞

t0

xT ·Q · x dt

forma quadratica (compromesso tra energia ed errore):

J =1

2

∫ t∞

t0

xT ·Q · x + uT ·R · u dt

Quest’ultima verrà utilizzata per imporre il controllo del sistema in esame. In pratica


48 Il controllo del sistema

si tratta di un problema di minimazzione dell’indice di prestazione cioè la ricerca di

massimi e minimi di una funzione a più variabili.

Come studiato nel corso di Analisi 2, questo viene fatto calcolando le derivate prime

della funzione in esame, quindi creando la matrice Hessiana, dopo di che in funzione

del valore del determinante della stassa nei punti di interesse si trovano i punti di

massimo, minimo oppure sella.

Così facendo però non si tiene conto degli eventuali vincoli che possono riguardare

il sistema, quindi occore aggiungere all’equazione principale un’altra equazione,

detta di vincolo, in modo da creare la funzione detta di Lagrange dove i coefficienti

λi sono moltiplicatori di Lagrange.


49

8.2 Il controllore LQR (Linear Quadratic Regulator)[4]

Questo sistema, essendo stato linearizzato, risulta non controllabile con i poli po-

sizionati sull’asse reale, uno positivo ed uno negativo. Per poter controllare questo

tipo di sistemi si ricorre al "controllo ottimo", ossia si progetta un controllore LQR.

Questo tipo di controllore minimizza una delle variabili del sistema in funzione di co-

sa si vuole ottenere come risultato finale, ottimizzando l’errore quadratico minimo.

Si può per esempio ottimizzare per il consumo di energia oppure per evitare over-

shooting.

Per descrivere bene questo tipo di controllo, il paragone che meglio spiega è quello

della strada per andare al lavoro. Se si immagina di poter andare al lavoro a 30Km

di distanza da casa in auto, a piedi oppure in elicottero, è possibile poi e dare impor-

tanza al tempo che ci si mette, al costo del trasporto, oppure pesare l’uno e l’altro e

quindi trovare un giusto compromesso.

Ovviamente, ottimizzando per il tempo vince l’elicottero, per il costo a piedi, ma l’au-

to sembra il compromesso migliore.

Il controllo "OTTIMO" permette appunto di decidere quale peso dare a ciascuna del-

le variabili.

Nel caso del sistema in esame si cercherà di dare poco peso al consumo di energia

in modo da migliorare l’accuratezza.

Quindi, avendo la funzione dell’indice di prestazione nella forma quadratica:

J =1

2

∫ t∞

t0

xT ·Q · x + uT ·R · u dt

e la funzione di vincolo che è data dal modello matematico del sistema nella forma

matriciale:

x = A · x+B · u

y = C · x+D · u(8.1)

Quindi si può ora formare l’Hamiltoniano

H(x, λ, t) =1

2· (xT ·Q · x + uT ·R · u) + λ′ · (A · x+B · u) (8.2)

Quest’ultima, dopo tutti i passagi dovuti, porta ad un’equazione detta di ”Riccati”,

la quale risolta dà come risultato un vettore denominato K(t), cioè un controllore di



stato con vettore di retroazione. Nel prossimo capitolo verrà descritto come tramite

il calcolatore ed i software adatti questo viene fatto.


51

8.3 Python e pysimCoder applicati al sistema

8.3.1 Simulazione

Figura 8.1: Simulazioni

Una volta definiti tutti i parametri e deciso che tipo di controllore utilizzare, si comin-

cia ad applicare questo al sistema. Viene per prima cosa creato un file di scrittura

(nel caso specifico con emacs in modo da potergli dare l’estensione .py) nel quale

vengono riportati dati ed equazioni che vengono poi controllate tramite l’esecutivo

jupyter-qtconsol ed infine simulate con pysimCoder fino ad arrivare a poter simulare

il controllo al sistema vero e proprio.

Per prima cosa come sempre vengono importate le librerie:

Figura 8.2: Import_Librerie



Si noti come, oltre alle solite, due sono specifiche dell’università dedicate appunto

al controllo. Quindi si passano al sistema le costanti, cioè i parametri fisici misurati

o calcolati come descritto nel paragrafo precedente:

Figura 8.3: Parametri_e_Costanti

Infine le matrici che descrivono il sistema:

Figura 8.4: Matrici_Sistema

Una volta passati tutti i parametri, si comincia a progettare il controllo. Avendo

a disposizione le misure dei soli angoli ed avendo bisogno anche delle velocità

angolari, si decide di aggiungere un osservatore al sistema. Quindi si progetta un

sistema di controllo ad anello chiuso come quello in figura:


53

Figura 8.5: Schema_Blocchi_Full_Obs

Dove il blocco denominato system è quello che simula il sistema, i due blocchi

Full_Obs e K_LQR_Ctrl sono quelli dedicati al controllo ed il blocco Plot permet-

te di simulare l’andamento del sistema in funzione del tempo.

Ora si procede ad inserire le funzioni nello script in modo da poterle successiva-

mente assegnare ai blocchi dedicati.

Per prima cosa con le matrici A,B,C e D si crea il sistema in continuo:

Figura 8.6: Sysc

Lo si converte in digitale passando all’apposita funzione, oltre al sistema, sia Ts,

cioè il periodo di sampling, che il metodo da utilizzare per la conversione:

Figura 8.7: c2d

il metodo deciso è quello di default (zoh = zero horder hold)



Figura 8.8: zoh_Method

Quindi si incomincia a calcolare il vettore ~K dei coefficienti del controllore che

sposterà i poli del sistema in una posizione stabile:

Figura 8.9: LQR_Controller

Una volta determinati i poli si prende quello col valore assoluto maggiore, lo si mol-

tiplica per 5 e si utilizza per realizzare l’osservatore, in quanto questo deve essere

più veloce del sistema stesso:


55

Figura 8.10: Poli_Sistema_Controllato_Per_Osservatore

Vengono quindi calcolati i poli discreti moltiplicando quelli trovati in continuo per Ts

e si può infine chiamare la funzione che crea l’osservatore completo passandole il

sistema ed i poli desiderati:

Figura 8.11: Full_Obs

Si decide di semplificare il sistema creando un blocco unico che unisca controllore

ed osservatore in modo che questo il sistema assuma la forma :



Figura 8.12: Sys_Completo

Una volta creato il sistema dal Block Settings (chiave e cacciavite sull’interfaccia

grafica), si assegna al file lo sript da utilizzare per le simulazioni (Parameter script)

ed il tipo di file che si vuole ottenere (Template Makefile), ques’ultimo perchè è

da qui che si seleziona (se si desidera) un file in C per lavorare col PC oppure è

anche possibile compilare per una versione in tempo reale di Raspbian (il SO del

Raspberry):

Figura 8.13: Settings_Template

Nello script si crea il blocco completo con l’apposita funzione:


57

Figura 8.14: Block_Complete

E si assegna con un doppio click sul blocco stesso la funzione desiderata:

Figura 8.15: Ass_Funz_Comp



Figura 8.16: Ass_Funz_sysc

Nel blocco del sistema si assegnano anche le condizioni iniziali, in questo caso

viene assegnato un valore diverso da zero all’angolo del body in quanto si vuole

simulare per vedere se, ed in che modo il controllore lo riporta a zero.

Si lancia quindi la simulazione e, cliccando 2 volte sul Plot, in un grafico viene

visualizzata la risposta:


59

Figura 8.17: Grafico_Simulazione

Ora si è preparati per il sistema reale.



8.3.2 Sistema reale

Si tratta di applicare lo stesso modello funzionante in simulazione al sistema vero e

proprio sempre utilizzando pysimCoder, con le differenze che in questo caso i dati

non sono quelli simulati ma quelli inviati dai sensori sul sistema.

Figura 8.18: Prove_Laboratorio

Per prima cosa si progetta in pysimCoder la parte di software da mandare al micro-

controllore che gestisce il sensore per inviare i dati.

Figura 8.19: Tx_Rasp

Questo prende semplicemente i segnali di giroscopio ed accelerometro, i quali pas-

sano attraverso il filtro complementare e vano ad un trasmettitore il quale tramite


61

la rete LAN li manda ad un ip ben specifico. Con due click sul filtro si determina il

coefficiente α del filtro mentre con un doppio click sul trasmetitore UDP si inserisce

l’idirizzo di ricezione. Il blocco con il gain serve ad invertire un segnale in funzione

dell’orientamento del sensore.

La parte di ricezione e controllo è decisamente più complessa:

Figura 8.20: Rx_Ctrl

Subito dopo aver ricevuto il segnale lo si fa passare in un filtro passa basso in modo

da eliminare parte delle oscillazioni ad alta frequenza dovute ai sensori ed alla loro

posizione.

L’angolo misurato del body viene poi sottratto a quello che deriva dalla posizione

dell’encoder sul motore in modo da ottenere l’angolo esatto delle ruote.

In questo modo otteniamo i due segnali da passare al controllore: ϕb, ϕw. Il blocco

con la costante fornisce il riferimento (in questo caso zero).

Il segnale in uscita del controllore viene fatto passare attraverso un blocco il quale ci

permette di decidere se aggiungere o togliere gain, poi in un altro che limita i valori

in uscita in modo che la richiesta di energia non sia superiore a quella disponibile.

Vengono inoltre inseriti degli step in modo da generare un ritardo che dia tempo di

far partire il sistema e mettersi in posizione per poter fisicamente agire su di esso.

Ora vengono lanciati gli eseguibili creati dal sistema e si osserva come esso reagise

tendando di trovare dei parametri idonei allo scopo prefittosi, cioè tenere il segway

in equilibrio.



Figura 8.21: Prove_Coefficienti

Da un lato si tiene aperto il file con lo script in modo da poter modificare i coefficienti

ad ogni tentativo e dall’altra il terminale per lanciare gli eseguibili.

Si eseguono numerose prove ma non si riesce a trovare una soluzione soddisfacen-

te.


63

Capitolo 9

Conclusioni

Dopo numerosi tentativi, anche con controllori diversi da quello descritto in questo

progetto non si riesce comunque a controllare il sistema.

Si esegue quindi un’approfondita analisi dello stesso per cercare di capire quali pos-

sano essere le cause di questa non controllabilità.

Si riparte dalla ricerca del modello matematico e dalle equazioni alla base del siste-

ma, ma tutto sembra combaciare.

Vengono determinati alcuni punti che potrebbero essere causa del malfunziona-

mento:

• Calibrazione del sensore: è forse possibile studiare una calibrazione migliore

che eviti le fluttuazioni anche da fermo del segnale

• Inserire gli attriti nelle equazioni: anche se questi andrebbero ad inserire nella

matrice di controllo valori molto vicini allo zero (10−4, 10−5)

• La posizione del sensore: posizionandolo più in basso si otterrebbe probabil-

mente un risultato migliore

• Una meccanica migliore: anche l’eccessivo attrito statico non aiuta

In particolare la meccanica sembra il punto più problematico del sistema.

A volte, durante le prove, si ha avuto l’impressione che il sistema per partire avesse

bisogno di talmente tanta energia per vincere l’attrito statico da portare lo stesso a

divergere.


64 Conclusioni


65

Capitolo 10

Costi

Nella tabella che segue i costi sostenuti:

DESCRIZIONE QUANTITÀ PREZZO UNITARIO TOTALEConnettore passante Raspberry 5 2.89 14.45

PCB 10 0.80 8.00Trasporto PCB 1 20.00 20.00

Controller MC5005 s CO 1 418.65 418.65Motore 4221G024BTXH IE3-1024 1 195.20 195.20

Steckerset MC50xx V3.0-Motorseite 1 7.85 7.85Steckerset MC5005/5010 V3-Versorg. 1 14.70 14.70

TOTALE CHF 678.85

Tabella 10.1: Tabella_costi

I costi sono da intendersi iva esclusa.


66 Costi


67

Bibliografia

[1] Wikipedia.org

[2] Prof Roberto Bucher, "Python for control purposes", 2019.

[3] Prof Roberto Bucher, "Modellazione di sistemi elettromeccanici con Lagrange",

2011.

[4] Prof.Silvano Balemi, Docente Ivan Furlan, "Progettazione di controllori", 1998-

2008.


68 BIBLIOGRAFIA


69

Capitolo 11

Allegati

Allegato_1 : Codice wxMaxmima delle equazioni

Allegato_2 : Codice python del sistema


70 Allegati


71

Capitolo 12

Ringraziamenti

Ringrazio:

Il professor Bucher per l’assistenza e la disponibilità

Il professor Bianchi per l’aiuto

Tutti i professori che mi hanno aiutato e sostenuto

Andrea Bernaschina per l’aiuto sopratutto col PCB

La mia famiglia che mi ha sopportato e supportato in questi 3 anni

I miei compagni tutti in particolare Flavio Valoti per l’aiuto


controllo di un mini segway

Documents