Controllo di movimento tramite encoder

Quando ho cominciato ad interessarmi di robotica, non molto tempo fa, ho cominciato a frequentare le gare del settore. La prima cosa che ho notato è stato il sistema di movimento dei vari robot in gara, qualsiasi tipo di gara: quasi tutti i motori erano pilotati ON/OFF, la linearità del movimento non era l'interesse principale dei progettisti. Molto spesso i bot rimanevano incastrati in angoli morti o ritornavano più volte sullo stesso percorso.

Da allora ho deciso che, appena fossi riuscito a far muovere il mio bot, avrei fatto di tutto per controllarne il movimento. ("Where am I" [1] )

Già nelle mie prime esperienze con Monty modificai i motori per inserire un encoder di movimento, come si può vedere nella pagina linkata a lato che scrissi all'epoca. L'encoder funzionava benissimo, ma mi accorsi presto che il PIC16F84 è troppo piccolo per contenere una routine completa di controllo del movimento oltre al resto del programma.

immagine1.gif
immagine2.gif

Allora cominciai a costruire Dino, con un PIC16F877, introducendo subito gli encoder (in quadratura) come descritto nella pagina linkata a lato. ("Encoders" [2] )

La risoluzione dell'encoder descritta nella pagina era sufficiente ad un semplice controllo di movimento ma quando cominciai ad implemetare il PID mi resi conto che l'errore di quantizzazione nel calcolo della velocità era troppo elevato.

Utilizzando un'encoder con 40 finestrelle ed un sistema di decodifica in quadratura x2, ho ottenuto 1600 impulsi per ogni giro di ruota, campionando la velocità ogni 50 mSec ho ottenuto una misura abbastanza precisa per i miei scopi. ("Encoders" [2] )

Gli impulsi degli encoder sono catturati tramite interrupt, nella "Interrupt Services Routine", attivata sia dagli encoder sia dal TIMER0, si misura quindi lo spazio percorso da ogni ruota (tenendo conto della direzione), il tempo trascorso e di conseguenza anche la velocità.

Applicando i concetti studiati diverso tempo fa (vedi link a lato), riguardanti lo sviluppo di un sistema operativo multitasking real time, non è stato difficile costruire una routine che, lavorando in background, tenga conto dei parametri misurati per mantenere la velocità desiderata. Più difficile è stato trovare l'algoritmo che questa routine doveva applicare. In Rete si trovano migliaia di pagine sul controllo PID, io ne ho lette molte; grazie a queste e all'aiuto degli amici della mailing list "Roboteck" su Yahoo! (it.groups.yahoo.com/group/roboteck/) che mi hanno dato molte "dritte" e che ni hanno indicato il documento del prof. Orsini ( [3] ), sono riuscito ad implementare un algoritmo semplificato ma sufficiente per i miei scopi. La taratura dei parametri di retroazione non è stata facilissima, ma questo era preventivato, è la prima cosa che spiega chiunque parli di PID. In ogni caso rientra nella possibilità umane medie! Ci vuole solo un po' di pazienza.

Il sorgente in C della parte di programma che controlla il movimento: dino.c

Nel programma ci sono moltissimi commenti, chi è interessato potrà comprendere molto leggendo il listato, anche se non è molto pratico di C; il programma è scritto in modo abbastanza semplice, senza usare le possibilità di "code masking" tipiche del linguaggio C. Altrimenti clicca qui per maggiori dettagli.
Sono comunque sempre a disposizione per qualsiasi charimento.

La teoria sul PID la lascio ai tanti testi che ne parlano molto meglio di quello che potrei fare io, ( PID [4] ).

In particolare voglio citare ancora il documento del prof. Orsini ( [3] ) nel quale ho trovato il giusto compromesso tra teoria e pratica .

Nel programma troverete le semplificazioni che ho usato, in parte dovute ad un criterio generale di risparmio di risorse, in parte ai limiti della mia preparazione. In particolare, si può notare l'esclusione assoluta delle matematica in floating point, dove avevo bisogno di una precisione maggiore nei calcoli intermedi ho moltiplicato per 10 o per 100 costanti e variabili, per poi troncare il risultato finale all'intero. Ripeto: lo scopo è quello di ottenere un buon controllo con le poche risorse a disposizione. Forse sarebbe meglio usare il termine "ingegnerizzazione" piuttosto che "semplificazione"!

Quello che non sono riuscito a semplificare più di tanto è stato proprio l'algoritmo PID. Su alcuni testi ho letto che per i nostri scopi sarebbe stata sufficiente la componente P(roporzionale), altri dicevano di aggiungere almeno la I(ntegrativa). Io ho fatto tutte le prove nell'ordine P-I-D: il sistema si è stabilizzato solo quando ho inserito anche la D(erivativa).

In uscita dal PID abbiamo il valore da impostare nel circuito di controllo dei motori per ottenere la velocità desiderata. I motori sono pilotati da un ponte ad H SN754410 tramite il generatore di PWM hardware del PIC16F877 ( [5] ). Lo schema classico per questi scopi è quello a lato, anche la controller board del MarkIII (http://www.junun.org) funziona così: il segnale PWM è applicato al pin di Enable, e la direzione si cambia abilitando in opposizione di fase i due rami del ponte tramite un inverter.
Ho cercato per giorni di farlo funzionare in questo modo sui miei motorini ( "PWM" [6] ). Con generatore di segnali ed oscilloscopio ho provato tutte le combinzioni di frequenze, filtri e diodi che mi sono venute in mente ma non c'è stato verso di avere, a frequenze superiori ai 20Hz, una regolazione continua e per un range sufficiente di velocità!! Decisamente poco.

Prendendo spunto da altri lavori, ho cominciato a cercare metodi alternativi. Qualcuno consigliava il metodo "Locked Anti Phase" (LAP [7] ), con il quale i motori non sono lasciati senza carico in alcuna fase del ciclo PWM. In pratica si fa scorrere la corrente in un verso per una parte del ciclo e nel verso opposto per la parte rimanente. Ci ho messo un po' a digerire la teoria ma la pratica è stata una passeggiata, ho tagliato un paio di piste, messo un paio di ponticelli, ed ho realizzato lo schema visibile a lato.

Miracolo! Funziona bene con un ampio range di frequenze, anche se la migliore è intorno a 2KHz, i motori girano regolarmente dal 20-30% in su e, se non ho bisogno dell'enable, risparmio un pin di I/O per ogni motore, non mi sembrava vero!

La direzione dei motori si inverte regolando il duty-cycle del PWM: 0% significa indietro a piena velocità, 50% motori fermi, 100% avanti tutta. Io per semplicità ho usato solamento il byte alto dei registri PWM del PIC ottenendo una dinamica di 255 valori, quindi da 0 a 127 per la retromarcia, da 128 a 255 per la marcia avanti. Mi sembra ancora una buona regolazione .

Insomma, come ho già detto, ho adottato un po' di compromessi ed un po' di semplificazioni, lo scopo era anche quello di far entrare tutto nelle capacità del PIC16F877, lo scopo è stato raggiunto:
Program statistics:
Total ROM used 1939 words (23.7%)
Total RAM used 113 bytes (30.7%)

ed il risultato mi sembra buono come si vede nel filmato che mostra il comportamento del bot programmato con: quadrato.c . Questo si muove sui lati di un quadrato di un metro, girando quattro volte di 90° e ritornando al punto di partenza. La precisione mi sembra buona, tenendo anche conto della non eccelsa precisone dei motoriduttori.

Da notare il controllo di movimento ad una velocità di 10 cm/sec, circa la metà della velocità massima del bot.

immagine2.gif
Versione ridotta (1.6MB)
Probabilmente, raffinando un po' gli algoritmi e perdendo ancora un po' di tempo nella taratura del PID, otterrei una precisione maggiore ed un movimento più regolare, ma avrei anche bisogno di motori molto più costosi con dei gruppi riduttori molto più precisi.

Mi ritengo soddisfatto, il bot va dritto, alla velocità che voglio io e indipendentemente dal carico e dallo stato delle batterie, gira di quanti gradi voglio, so di quanto ha camminato, funziona in modo regolare anche a velocità molto basse.

Senza PID, le differenze tra i motori fanno si che sia impossibile far camminare il mezzo a pochissimi centimetri al secondo, o mandarlo dritto a diverse velocità: a volte prevale un motore a volte l'altro.

Ora posso cominciare a studiare il "Dead-reckoning", la stima della posizione senza riferimenti esterni: un po' di matematica, un po' di sensori e... ( [8] )

immagine4.gif In questo filmato si può notare l'effetto di un semplice algoritmo per uscire dagli angoli morti

Il codice in linguaggio C delle modifiche rispetto al programma precedente : bumpers.c

Versione ridotta (0,5MB)
------------------------------------------------------------------Links
"Where am I" sensors and methods for mobile robot positioning [1]
ftp://ftp.eecs.umich.edu/people/johannb/pos96rep.pdf
Encoders [2]
Understanding quadrature www.gpi-encoders.com/Understanding_Quadrature.pdf
Square wave and pulses www.gpi-encoders.com/Square_vs_Pulse_Waves.pdf
Pulsed sensor usage mobots.com/EeC194/Notes/1999_10_14.html
Description of shaft encoders www.mil.ufl.edu/imdl/papers/IMDL_Report_Spring_02/louis_brandy/shaft.pdf
PID & Motion control [4]
Manuale propedeutico sulle tecniche di regolazione, Prof. Livio S. Orsini [3]
www.plcforum.info/didattica/conreg/pagina1.htm
www.plcforum.info/didattica/conreg/pagina2.htm
www.plcforum.info/didattica/conreg/pagina3.htm
www.plcforum.info/didattica/conreg/pagina6.htm
www.plcforum.info/didattica/conreg/appendice.htm
Single motor speed control abrobotics.tripod.com/ControlLaws/singlemotor.htm
The Dilbert motion control www.barello.net/Papers/Motion_Control/
PID Without a PhD www.embedded.com/2000/0010/0010feat3.htm
Designing a PID Motor Controller www.seattlerobotics.org/encoder/200205/PIDmc.html
Control Theory Terminology www.newport.com/file_store/PDFs/tempPDFs/Control_Theory_e3987.pdf
Proportional - Integral - Derivative (PID) Theory and Fabrication www.mechatronics.me.vt.edu/book/Section3/PID.html
PID in detail www.mechatronics.me.vt.edu/book/Section5/PIDindetail.html
PID academic1.bellevue.edu/robots/mogotut/PID.htm
A Simple DC Motor Controller academic1.bellevue.edu/robots/mogotut/simplemotor.htm
Quick and Easy Motor Control www.circuitcellar.com/
PID motor control with Zilog Z8PE003 www.zilog.com/docs/z8/appnotes/pid_motor_control.pdf
PWM [6]
Pololu micro dual serial motor controller www.pololu.com/products/pololu/0401/
Choosing the right PWM frequency www.seattlerobotics.org/encoder/200011/pwm.html
PWM speed control www.4qdtec.com/pwm-01.html
Some power PWM drivers for DC motors www.picotech.com/applications/pwm_drivers/
Locked Anti Phase & sign magnitude[7] www.national.com/an/AN/AN-694.pdf
PWM forum www.seattlerobotics.org/encoder/sep99/pwmmail.html
Microchip AN594 ( usingCCP modules) [5] www.microchip.com/
H-Bridge Demystified www.barello.net/Papers/H-Bridge.ppt
Piccoli motori DC www.vincenzov.net/tutorial/motoridc/motoridc.htm
Robot Circuits www.geocities.com/robodave2000/circuits.htm
[DPRG] PWM wanderings nimon.ncc.com/pipermail/dprglist/2000-November/001560.html
Dead reckoning [8]
Implementing Dead Reckoning by Odometry… www.seattlerobotics.org/encoder/200010/dead_reckoning_article.html
A Tutorial and Elementary Trajectory Model… rossum.sourceforge.net/papers/DiffSteer/DiffSteer.html