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