Lego Mindstorms Line follower PID ( controllo proporzionale

Lego Mindstorms
Line follower PID ( controllo proporzionale, integrativo, derivativo)
A cosa serve un PID
Ogni volta che un dispositivo deve mantenere costante un determinato valore, ad esempio una velocità,
una temperatura, un livello, una rotta o il valore misurato da un sensore serve un sistema detto regolatore.
Qualcosa che corregga eventuali ed inevitabili errori rispetto al valore di consegna (set point).
Se pensiamo al pilota automatico di una nave che deve lavorare per mantenere la rotta, ci è facile
comprendere come venti, correnti e onde siano fonti di errore che il regolatore deve contrastare.
Ma ci può essere anche il caso in cui si debba seguire un valore che cambia come ad esempio seguire un
percorso che varia, come succede ad un robot tipo line follower o se vogliamo ad un missile che segue
l’aereo da colpire.
La descrizione matematica di un sistema di regolazione PID è molto complessa,
(http://it.wikipedia.org/wiki/Controllori_PID) ma nel nostro caso cerchiamo di capire solo i concetti generali
per poter poi implementare in modo sperimentale un regolatore.
Line follower
il problema
Dobbiamo far seguire al nostro robot una linea di forma qualsiasi di colore
nero su fondo bianco, o meglio, vedremo che in realtà quello che il robot
deve seguire è il bordo della linea nera, delimitazione tra il nero ed il bianco.
La struttura semplificata della macchina da realizzare è riportata in figura
È un robot con due motori indipendenti A e C.
Un sensore di luminosità che legge l’intensità della luce riflessa dalla
superficie; la luce è prodotta da un diodo led montato sul robot (cerchio
rosso).
Una macchina così fatta, anche se molto semplice ha tutte le caratteristiche di un robot; il sensore “legge”
il mondo esterno e in base alle informazioni ricevute la macchina cambia il suo comportamento.
Il sensore di luminosità
Il sensore di luminosità usato è quello in figura.
La prima operazione da eseguire è la sua calibrazione; con il
led acceso dobbiamo misurare l’intensità della luce riflessa
dalla superficie bianca che corrisponde al valore max che
viene posto pari a 100; stessa procedura per la luce riflessa
dalla linea nera, valore minimo posto uguale a 0.
In tutte l altre situazioni intermedie il sensore leggerà un
valore di luce riflessa compresa tra il valore minimo 0 e
quello massimo 100.
Attenzione alle condizioni di luce ambientali, se cambiano occorre calibrare
sensore.
di nuovo il
Analisi del problema
Il robot deve seguire la linea nera? Non conviene lavorare in
questo modo perché se la macchina perde la linea e va verso il
bianco non sappiamo da che parte è uscito e potremmo perdere
per sempre il percorso.
Se invece facciamo seguire il bordo, ad esempio sinistro, della
linea sappiamo che se il sensore riporta un valore maggiore del
precedente è andato verso il bianco (verso sinistra) mentre al
contrario ha deviato verso il nero ( a destra); possiamo così
determinare la direzione verso cui far ruotare il robot per
cercare di ritrovare il percorso.
Un controllo della direzione di questo tipo è detto on off in
quanto prevede due soli stati possibili riguardo il
comportamento dei motori
1. Gira a destra se luce > 50
2. Gira a sinistra se luce < 50
Un’altra possibilità è quella di prevedere non solo due stati possibili ma 3, 4 5 in funzione del valore
dell’intensità della luce letta dal sensore
1.
2.
3.
4.
5.
Luce >> 50? Allora gira tanto a destra
Luce > 50? Allora gira un po’ a destra
Luce = 50? Vai dritto
Luce < 50? Gira un po’ a sinistra
Luce >> 50? Gira tanto a sinistra
È intuitivo capire che il comportamento di un robot così
programmato sarà migliore del precedente; l’andamento sarà
meno a zig zag e con linea retta il moto sarà praticamente
continuo se scegliamo in maniera corretta i valori di soglia tra
una stato e l’altro.
Aumentando i possibili stati ci stiamo avviando alla
realizzazione di un regolatore proporzionale nel quale l’entità
della rotazione è proporzionale alla differenza tra il valore
rilevato dal sensore ed il valore 50 fissato come separazione
nero bianco.
Quanto detto può essere schematizzato nel grafico seguente
Il primo grafico è relativo alla situazione a due stati: il robot non va mai dritto, gira a destra se il sensore
riporta un valore >50 gira a sinistra nell’altro caso.
Il secondo fa riferimento ad una situazione a tre stati: il robot va dritto se il sensore riporta un valore
intorno a 50 (lungo la separazione bianco nero); gira a destra se il valore letto dal sensore è >50 gira a
sinistra nel caso opposto.
Il terzo grafico mostra il comportamento d un regolatore proporzionale: se il sensore riporta il valore 50 il
robot va dritto, se però il valore letto dal sensore tende ad aumentare il robot inizia a girare verso destra e
tanto più il valore è diverso da 50 tanto più “forte” è la sterzata verso destra; in maniera analoga se il
sensore misura valori minori di 50.
Proporzionale significa che la “forza della sterzata” sarà tanto maggiore quanto più il valore letto dal
sensore si allontana da 50.
La curva rappresentata nel grafico è quindi una retta che in generale ha equazione
Nel nostro caso la y rappresenta il raggio di sterzata (legato alla differente potenza applicata ai due motori)
mentre la x è sostanzialmente la differenza tra il valore misurato dal sensore e quello di riferimento (set
point) legato alla separazione bianco nero della linea da seguire.
Fissiamo 50 il valore letto dal sensore quando è esattamente
sulla linea di separazione
Definiamo
; naturalmente
se il robot si sposta verso il bianco mentre
se il robot si sposta vero il nero
Sull’asse della y, in unità arbitrarie riportiamo la “forza della
sterzata” che ha il valore max +1 se verso destra e il valore
max negativo -1 se verso sinistra.
L’equazione della retta rappresentata sul grafico è
Dove Turn e la “forza della sterzata” positiva se verso destra,
negativa se verso sinistra, m è il coefficiente angolare della
retta cioè un valore legato alla sua pendenza
Nel caso specifico
))
))
I valori riportati nel grafico sono totalmente arbitrari; nel caso reale bisognerà determinare
sperimentalmente il range in cui cade il valore dell’errore; in maniera analoga occorre legare i valori
compresi tra +1 e -1 alla potenza dei due motori.
Il coefficiente angolare m, nella letteratura dei sistemi di controllo, prende il nome di Kp costante di
proporzionalità, quindi
Applichiamo quanto detto al robot.
Pa=potenza motore A; Pb=potenza motore B
Se Pa=Pb moto rettilineo a velocità costante
Se Pa>Pb il robot gira a destra
Se Pa<Pb il robot gira a sinistra
La potenza applicata al motore assume valori compresi tra 0 (motore fermo) e 100 (max
potenza).
Fissiamo P valore di potenza applicata ai due motori per seguire la linea retta (turn=0 perché errore=0)
direzione
0
P
P
dritto
>0
>P
<P
Gira a destra
<0
<P
>P
Gira a sinistra
Ad esempio fissiamo:



P=50% (motori a metà potenza)
Valore di soglia (offset) = 50 (valore letto dal sensore se sul bordo bianco nero; valore medio tra
lettura tutto bianco e tutto nero)
Kp=5 ; ipotesi
; se errore=0 Pot motori =50; se errore = 10 pot motore = 0;
allora
Pseudo codice:
Kp=5;
P=50;
offset=50;
luce, errore, turn
Loop infinito
Luce=lettura del sensore
inizializzazione variabili; i valori devono essere
regolati in funzione della situazione sperimentale.
La variabile luce serve per memorizzare il valore
letto dal sensore
Ripeti per sempre
Sensore misura luce e memorizza il valore
nella variabile luce
Errore=offset-luce
Calcolo dell’errore
Turn=Kp*errore
Calcolo di turn
Pa=P+turn; Pb=P-turn
Calcola potenza da applicare ai motori
motoreA=direzione Avanti, Potenza Pa
Comanda motore A
motoreB=direzione Avanti, Potenza Pb
Comanda motore B
Fine loop
torna all’inizio e ripeti
I valori di P e Kp sono da aggiustare in funzione del comportamento del robot in base al percorso.
Solo osservando il suo movimento si può giungere ad una scelta ottimale dei due parametri.