Odometria, come Arianna sa dov’è

Nella prima puntata ci siamo detti che dobbiamo vedere col il punto di vista di Arianna,  l’odometria è una tecnica che usa sensori per stimare dove ci si trova nello spazio. Se pensiamo alla nostra esperienza di vita è la vista il senso che ci fornisce il grosso delle informazioni per stabilire dove siamo. Stabilire dove siamo significa posizionarci nello spazio in relazione ad altri oggetti, quindi in maniera relativa. Messo in un spazio senza oggetti mi sento perso!
La visione ci permette di vedere a grandi distanze, ci risulta difficile provare questa senza sensazione a meno di entrare in camere appositamente realizzate in maniera da annullare la percezione di forme e ombre, Villa Panza a Varese ne ha ospitato sino a qualche anno fa un esempio http://www.aisthesis-fai.it/Ganzfeld.htm .
Possiamo provare una situazione di perdita di posizione se chiudiamo gli occhi, ci mettiamo al centro di una stanza senza toccare nulla e in un luogo senza sorgenti sonore, oppure all’aperto su un sentiero in un parco. Dopo qualche tempo con gli occhi chiusi iniziamo a perdere i riferimenti.  Se ci muoviamo il nostro cervello inizia a stimare il nostro movimento e noi costantemente immaginiamo la nostra posizione salvo poi, aprendo gli occhi trovarci da tutt’altra parte. Quello che fa il nostro cervello è fondere tutte le informazioni provenienti dai nostri sensi e ricostruire il nostro movimento, mancando la vista che gli permette una verifica continua della posizione, deve aspettare di trovare un punto di riferimento e sperare di riconoscerlo.

L’odometria in base ai sensori di movimento posti sulle ruote, gli encoder, tenta di ricostruire la posizione di Arianna da quando inizia il suo percorso.

Usiamo come riferimento per questo post l’introduzione che troviamo all’indirizzo: odomtutorial.pdf.

La posizione di Arianna

La posizione di Arianna è rappresentata in un piano cartesiano dalle sue coordinate x e y. Questo piano è il pavimento su cui si muove. L’origine è il punto da cui parte per mostrarci il libro, la x è allineata con la libreria, la y rappresenta la distanza da questa. Ci accorgiamo però che queste coordinate non bastano, manca dove è orientata, dove sta guardando. Questa terza variabile è teta, l’angolo che forma l’asse longitudinale del robot, centrato tra le ruote che va dal dietro al davanti, e l’asse delle x. Questo angolo vale quindi 0 se Arianna si muove lungo la libreria allontanandosi dalla base, vale 180 se torna verso la base.

Geometria. Su un periodo sufficientemente piccolo il moto del robot si approssima a un arco. Il problema della odometria è calcolare (x’, y’, teta’) a partire a (x, y, teta). Nella figura il robot si muove in senso antiorario. d_baseline è la distanza tra le due ruote (carreggiata).

La figura illustra il movimento delle due ruote del robot, d_left la distanza percorsa dalla ruota sinistra e d_right dalla destra. L’arco di cerchio percorso ha centro nel punto P. Essendo un moto circolare con centro P in ogni punto  sul raggio r la distanza percorsa d vale:

d1 = φ*r1

Dagli encoder, ammesso di non avere slittamento, abbiamo la misura della distanza percorsa dalle singole ruote. Ci serve una formula che a partire dalla posizione attuale ci dia la prossima posizione.
Ripetendo il calcolo continuamente potremo stimare dove siamo.
Arianna calcolerà questo nel loop di controlloche  viene eseguito ogni 100 ms, all’interno di questo periodo possiamo supporre che nel velocità dei due motori siano costanti anche se diverse. Vale quindi il disegno della figura 1. Da evidenziare che viaggiare in linea retta significa avere le due velocità uguali.

La formula

Lo sviluppo della formula è riportato nell’introduzione indicata sopra. Concentriamoci sull’algoritmo che ne deriva.

Con riferimento alla figura sopra le formule da applicare a ogni giro sono le seguenti:

Gli ingressi sono dati dalle coordinate attuali (x, y, θ) e dallo spostamento delle due ruote d_left e d_right. La d_baseline è una costante meccanica, la carreggiata. Applicando le formule ne deriva il nuovo set di coordinate (x’, y’, θ’) che diventano a loro volta le nuove coordinate attuali. Il ciclo va dunque a ripetersi.

Codice

Il codice raggruppa quanto visto nei post precedenti. Un sensore encoder per ogni ruota e quindi due routine di interrupt che agiscono su due contatori diversi.

#define GIRO_RUOTA  		2.625	// mm per impulso*0.5 = sviluppo ruota[mm]/ppr (pulsi per rivoluzione)
#define GIRO_RUOTA_DX  		2.625		
#define GIRO_RUOTA_SX  		2.615 		


	// odometro 
	// pin 21 (atMega) int 2	
	attachInterrupt(digitalPinToInterrupt(GIRO_DX_PIN), odometroDxMisuraHW, CHANGE);
	// pin 20 (atMega) int 3	
	attachInterrupt(digitalPinToInterrupt(GIRO_SX_PIN), odometroSxMisuraHW, CHANGE);


/*
	pin 21 (atMega) int 2
	sotto interrupt incremmento contatore interi
	solo se R != 0. cioè se in movimento
*/
void odometroDxMisuraHW(void){
unsigned long pulseTime;

	// se gli impulsi sono troppo vicini è dovuto a un disturbo
	if ((millis() - pulseTime) < MIN_TIME_TRA_PULSE) return;
	pulseTime = millis();

	if (statoRun == 0) return;

	if (direzione == AVANTI)  odometroDxCnt ++;
	else                      odometroDxCnt --;
    
}

/*
	pin 20 (atMega) int 3
	sotto interrupt incremmento contatore interi
	solo se R != 0. cioè se in movimento
*/
void odometroSxMisuraHW(void){
unsigned long pulseTime;

	// se gli impulsi sono troppo vicini è dovuto a un disturbo
	if ((millis() - pulseTime) < MIN_TIME_TRA_PULSE) return;
	pulseTime = millis();

	if (statoRun == 0) return;

	if (direzione == AVANTI)  odometroSxCnt ++;
	else                      odometroSxCnt --;

}




void updatePosition(void){

static long SxCnt_k_1 = 0;		// valore cnt a k-1
static long DxCnt_k_1 = 0;		// valore cnt a k-1
static long dDxCnt, dSxCnt;		// delta cnt
static long letturaDx;			// congelo cnt
static long letturaSx;			// congelo cnt
static float deltaC;			// delta cnt

	// valore complessivo: usato temporaneamente
	odometro = (odometroDxCnt + odometroSxCnt)*GIRO_RUOTA;

	// calcolo evoluzione nel periodo 
	
	// blocco gli interrupt per fare le letture coerenti
	noInterrupts();
		letturaDx= odometroDxCnt;
		letturaSx= odometroSxCnt;
	interrupts();
	
	dDxCnt   = letturaDx - DxCnt_k_1;				// delta sx e dx in count
	dSxCnt   = letturaSx - SxCnt_k_1;
	
	deltaC   = (dDxCnt + dSxCnt)*GIRO_RUOTA;		// avanzamento del centro nel periodo in mm
	
	DxCnt_k_1= letturaDx;							// memoria per prossimo ciclo
	SxCnt_k_1= letturaSx;
	
	// integro teta
	teta 	+= ((float)dDxCnt*GIRO_RUOTA_DX - (float)dSxCnt*GIRO_RUOTA_SX)*2.0/BASELINE;
	
	// integro posizioni
	xpos    +=  deltaC*cos(teta);
	ypos    +=  deltaC*sin(teta);
	
	// monitor dati
	if (monitorDati){
		Serial1.print(dDxCnt);
		Serial1.print(", ");
		Serial1.print(dSxCnt);
		Serial1.print(", ");
		Serial1.print(deltaC);
		Serial1.print(", ");
		Serial1.print(teta);
		Serial1.print(", ");
		Serial1.print(xpos);
		Serial1.print(", ");
		Serial1.print(ypos);
		Serial1.print(", ");
		Serial1.print(errore);
		Serial1.println();
	}

}


 

Allo scattare del tempo viene chiamata la updatePosition. Qui viene eseguito l’algoritmo descritto, per risparmiare tempo si usano gli interi per i contatori sotto interrupt, le trasformazioni in float vengono fatte solo quando necessario per i conti difficili. Le costanti meccaniche per ogni impulso sono definite separatamente  per ogni ruota.  Durante la fase di taratura una differenza di 1 centesimo di mm per ogni impulso rendeva il sistema più preciso. Il valore equivale a 4 decimi di mm di differenza sullo sviluppo tra le due ruote.

 

Risultato e Problemi

Grazie all’odometria è stato aggiunto un nuovo meccanismo di guida, è possibile definire l’angolo cioè la direzione del movimento. Il regolatore confronta l’angolo desiderato con il valore stimato dall’odometria. L’errore influisce sullo scorrimento tra le due ruote.
Il meccanismo funziona! Sotto il grafico dei valori x e y restituiti da Arianna con il monitorDati attivo.

Il grafico x, y mostra i punti stimati in un viaggio con varie curve. Il tragitto partiva da 0, 0 con direzione 0°, poi 180°, di nuovo 0°, -90° e per finire 180°. Le distanze da percorrere erano diverse nei vari tratti. È curioso come per passare da -90 a 180° Arianna abbia fatto un intero giro su se stessa, in realtà ha fatto quanto richiesto, infatti per curvare ancora verso destra avremmo dovuto dirgli di andare a -180°!! Per capirlo occorre ragionare un poco sugli angoli!

Una nota sulla posizione y, come si vede durante le curve si sposta dallo zero. anche qui nessuno stupore, quella che viene controllata è la direzione non le posizioni x e y.

Gli ondeggiamenti che si vedono sono dovuti alla dinamica del sistema e del regolatore, possiamo paragonarlo a quando guidando l’auto dobbiamo correggere più del dovuto, ad esempio uscendo velocemente da una curva o essendo lenti nelle correzioni.

Altra fonte di errore sono gli scivolamenti delle ruote sul terreno. La base del principio usato è la misura dello spostamento se questa è falsata il risultato è falsato. Il pavimento della biblioteca ha reso necessari degli ulteriori accorgimenti per evitare questo fenomeno.

Altro errore è la bassa risoluzione degli encoder usati, questi fanno si che la risoluzione minima dell’angolo sia circa 3.5°, valore di per se non male per l’angolo. Purtroppo questo errore riportato sulle y ad esempio da luogo a deviazioni notevoli. Possiamo dire che mentre l’angolo e quindi l’orientamento è sufficiente lo stesso non vale per le coordinate cartesiane.

Per finire rammentiamo che il meccanismo di stima della posizione per integrazione dei piccoli passi a lungo andare accumula forzatamente errori, questo rende necessari dei riallineamenti quando si passa in prossimità di punti noti. Riprendendo la metafora iniziale abbiamo che  viaggiando ad occhi chiusi dobbiamo ogni tanto tastare le pareti alla ricerca di un punto di riferimento.

 

Annunci

Rispondi

Inserisci i tuoi dati qui sotto o clicca su un'icona per effettuare l'accesso:

Logo WordPress.com

Stai commentando usando il tuo account WordPress.com. Chiudi sessione / Modifica )

Foto Twitter

Stai commentando usando il tuo account Twitter. Chiudi sessione / Modifica )

Foto di Facebook

Stai commentando usando il tuo account Facebook. Chiudi sessione / Modifica )

Google+ photo

Stai commentando usando il tuo account Google+. Chiudi sessione / Modifica )

Connessione a %s...