Odometria differenziale con encoder sulle ruote

Un robot a trazione differenziale ha due ruote azionate indipendentemente e separate dalla larghezza della carreggiata. Quando entrambe le ruote percorrono la stessa distanza, il robot avanza rettilineo. Quando una percorre una distanza maggiore, il telaio segue un arco. L’odometria integra questi piccoli movimenti nel tempo per stimare la posa: posizione orizzontale x, posizione verticale y e orientamento θ.

La stima è locale e cumulativa. Parte da una posa scelta e non può correggersi usando soltanto gli encoder delle ruote. Piccoli errori di diametro delle ruote, larghezza della carreggiata, temporizzazione e slittamento si accumulano; l’obiettivo è quindi ottenere una stima utile a breve termine con limiti misurati, non una verità di riferimento perfetta.

Definisci una convenzione di coordinate

Scegli un sistema di riferimento prima di scrivere le equazioni. Una convenzione bidimensionale comune è:

• x punta in avanti dalla posa iniziale.
• y punta a sinistra.
• Un θ positivo produce una rotazione antioraria.
• Le distanze delle ruote sinistra e destra sono positive quando il robot avanza.

Memorizza gli angoli in radianti all’interno. Convertili in gradi soltanto per la visualizzazione. Normalizza l’orientamento se avere valori limitati semplifica la diagnostica, ma seno e coseno funzionano anche con angoli non limitati.

Indica con dL e dR le distanze delle ruote sinistra e destra dall’aggiornamento precedente e con b la distanza effettiva fra le linee di contatto delle ruote.

Converti i conteggi in metri

Tutto quanto precede presume che tu abbia già dL e dR in metri. Gli encoder non producono metri, ma conteggi. Tra i due c’è una conversione ed è sbagliarla a far scalare ogni valore calcolato nel resto della pagina.

countsPerWheelTurn = PPR x edgesDecoded x gearRatio
metresPerCount     = (pi x wheelDiameter) / countsPerWheelTurn

edgesDecoded vale 4 solo se decodifichi entrambi i fronti di entrambi i canali, ossia la quadratura completa. Decodificare il fronte di salita di un solo canale dà 1 e scarta anche l’informazione sulla direzione, quindi il robot non può distinguere la retromarcia dall’avanzamento.

L’ultima riga è quella che sorprende le persone. Un disco fessurato legato alla ruota con fascette è economico, ma 10 mm di quantizzazione significa che uno spostamento di 200 mm è noto con un’approssimazione del 5% e nessuna taratura recupera un’informazione che il sensore non ha mai acquisito. Il tutorial sugli encoder in quadratura tratta la decodifica in modo completo.

La risoluzione è un limite inferiore della precisione, non una promessa di accuratezza. Un conteggio di 73 um non significa nulla se il diametro della ruota è sbagliato del 2%.

Calcola il movimento lineare e angolare

Il centro dell’asse si sposta della distanza media delle ruote:

dCenter = (dR + dL) / 2

L’orientamento varia in funzione della loro differenza:

dTheta = (dR - dL) / b

Distanze uguali producono dTheta = 0. Distanze uguali e opposte producono dCenter = 0, quindi il robot ruota sul posto attorno al punto medio dell’asse.

Per aggiornamenti piccoli, applicare la traslazione con l’orientamento del punto medio è accurato ed evita un caso speciale per il moto rettilineo:

headingMid = theta + dTheta / 2
x = x + dCenter * cos(headingMid)
y = y + dCenter * sin(headingMid)
theta = theta + dTheta

Questa integrazione al punto medio è abbastanza semplice per un controllore integrato e si comporta bene quando l’anello viene eseguito frequentemente. L’integrazione esatta di un arco è utile per passi ampi, ma non corregge misurazioni delle ruote errate.

Implementa un aggiornamento ripetibile

Conserva i totali grezzi degli encoder e converti in metri le loro differenze. Non azzerare i contatori hardware a ogni iterazione; le istantanee rendono più semplici la gestione del superamento del limite e la diagnostica.

struct Pose { float x, y, theta; };

void updateOdometry(Pose& pose, float dLeft, float dRight, float trackWidth) {
  const float dCenter = 0.5f * (dLeft + dRight);
  const float dTheta = (dRight - dLeft) / trackWidth;
  const float headingMid = pose.theta + 0.5f * dTheta;

  pose.x += dCenter * cosf(headingMid);
  pose.y += dCenter * sinf(headingMid);
  pose.theta += dTheta;
}

Richiama questa funzione dopo aver letto atomicamente entrambi gli encoder. Le istantanee sinistra e destra devono rappresentare quasi lo stesso istante; un intervallo lungo tra le due crea uno sterzo apparente durante un movimento rapido.

Se un altro sottosistema richiede la velocità, dividi gli incrementi per il tempo trascorso misurato. L’integrazione della posa usa direttamente la distanza, quindi le variazioni del tempo di ciclo non cambiano direttamente la stima della distanza percorsa.

Leggi entrambi gli encoder senza perdere conteggi

La funzione precedente è pura aritmetica. Alimentarla correttamente è il punto in cui l’odometria integrata si rompe davvero e tre dettagli contengono quasi tutto il rischio.

Cattura entrambi i contatori nello stesso istante. Le routine di interrupt continuano a incrementarli mentre l’anello li legge. Leggere il sinistro, eseguire qualche operazione e poi leggere il destro attribuisce il movimento avvenuto nel frattempo soltanto a una ruota e le equazioni lo interpretano fedelmente come una rotazione.

Non azzerare mai i contatori hardware. I totali che avanzano liberamente, insieme a un valore precedente memorizzato, forniscono il delta e lasciano disponibile il conteggio grezzo per la diagnostica.

Calcola il delta in aritmetica senza segno. In C++ il traboccamento con segno è comportamento indefinito; la sequenza di conversioni seguente è definita, non costa nulla e resta corretta anche al superamento del limite.

volatile int32_t leftCount = 0, rightCount = 0;   // written only by the ISRs

void isrLeft()  { leftCount  += digitalRead(L_CHAN_B) ? +1 : -1; }
void isrRight() { rightCount += digitalRead(R_CHAN_B) ? +1 : -1; }

constexpr float METRES_PER_COUNT = (3.14159265f * 0.065f) / 2800.0f;   // 72.9 um
constexpr float TRACK_WIDTH      = 0.152f;                             // calibrated, not measured

void odometryTick(Pose& pose) {
  int32_t nowLeft, nowRight;
  noInterrupts();                      // one instant, both wheels
  nowLeft  = leftCount;
  nowRight = rightCount;
  interrupts();

  static int32_t prevLeft = 0, prevRight = 0;
  // Unsigned subtraction wraps in a defined way; the cast back recovers the
  // signed delta even across the 2^32 boundary.
  const int32_t dLeftCounts  = (int32_t)((uint32_t)nowLeft  - (uint32_t)prevLeft);
  const int32_t dRightCounts = (int32_t)((uint32_t)nowRight - (uint32_t)prevRight);
  prevLeft  = nowLeft;
  prevRight = nowRight;

  updateOdometry(pose,
                 dLeftCounts  * METRES_PER_COUNT,
                 dRightCounts * METRES_PER_COUNT,
                 TRACK_WIDTH);
}

A 73 um per conteggio, un contatore con segno a 32 bit supera il limite dopo circa 157 km, quindi su un robot da scrivania è un problema soltanto teorico. Costa comunque due conversioni garantirne la correttezza, e lo stesso schema è obbligatorio su un contatore a 16 bit, che supera il limite dopo 2.4 m.

Svolgi una rotazione di esempio

Supponi che la ruota sinistra percorra 0.20 m, quella destra 0.30 m e che la larghezza della carreggiata sia 0.25 m:

• dCenter = 0.25 m
• dTheta = (0.30 - 0.20) / 0.25 = 0.4 rad
• dall’orientamento zero, l’orientamento al punto medio è 0.2 rad
• dx ≈ 0.25 cos(0.2) = 0.245 m
• dy ≈ 0.25 sin(0.2) = 0.050 m

Il robot termina leggermente a sinistra del suo asse iniziale di marcia e ruotato di circa 22.9°. Verificare un calcolo manuale di questo tipo intercetta errori di segno e di unità prima che il codice raggiunga l’hardware.

calibra i due parametri dominanti

Il diametro effettivo delle ruote controlla la scala delle distanze. Percorri un lungo tratto rettilineo misurato, confronta la distanza riportata con quella reale e moltiplica la scala del diametro per actual / reported. Calibra ciascun lato se il robot curva nonostante comandi uguali.

La larghezza effettiva della carreggiata controlla la scala delle rotazioni. Comanda diverse rotazioni complete sul posto, misura la variazione reale dell’orientamento con un riferimento esterno e correggi la larghezza della carreggiata. Più rotazioni amplificano l’errore e semplificano la misura. Lo scorrimento laterale degli pneumatici può far sì che il valore effettivo differisca dalla distanza tra le ruote misurata con un metro.

Mantieni entrambi i parametri configurabili. Superficie, compressione degli pneumatici, carico e usura possono modificarli; i modelli hardware devono avere manopole di taratura.

Scopri rapidamente l’errore

La taratura non elimina l’errore, ne riduce il coefficiente. Conoscere la grandezza di ciò che rimane indica per quanto tempo puoi fidarti della stima prima che serva una correzione esterna.

Il valore del quadrato deriva dall’integrazione di un quadrato di 1 m con quattro rotazioni sul posto da 90 deg e nessun errore tranne la larghezza della carreggiata. Tutto il resto è esatto e il robot termina comunque a 8.7 cm dal punto di partenza.

Confronta l’ultima riga con le prime due. Il calcolo dell’integrazione non è mai il tuo errore dominante. È tre ordini di grandezza sotto i termini di taratura e quattro sotto lo slittamento, ed è per questo che aumentare la frequenza dell’anello per inseguire la precisione è un’ottimizzazione sbagliata. Ecco anche perché la pagina sullo slittamento delle ruote conta più di qualsiasi elemento di questa pagina per la posa finale.

Nota inoltre che gli errori sistematici e casuali si comportano in modo completamente diverso. Uno scorrimento sistematico si accumula linearmente e vale la pena eliminarlo con una taratura. Uno casuale cresce con la radice quadrata del numero di rotazioni, quindi 50 rotazioni costano circa 6 deg anziché 45: è fastidioso, ma nessuna regolazione dei parametri può risolverlo.

Scegli una frequenza di aggiornamento

L’arco tra due campioni viene approssimato con una corda rettilinea all’orientamento del punto medio, quindi l’errore dipende da quanto cambia l’orientamento a ogni passo. Per un passo di dTheta radianti, la corda è più breve di circa dTheta^2 / 24:

Scegli 50 Hz e smetti di pensarci. Spendi l’attenzione risparmiata per sincronizzare le due letture degli encoder, cosa che a qualunque frequenza conta più della frequenza stessa: uno sfasamento di 1 ms fra le istantanee sinistra e destra, mentre le ruote differiscono di 0.5 m/s, inventa 0.5 mm di differenza fra le ruote a ogni iterazione.

Aggiungi un giroscopio senza confondere i sistemi di riferimento

Un giroscopio spesso fornisce una variazione di orientamento a breve termine migliore della differenza fra gli encoder delle ruote. Puoi usare l’orientamento del giroscopio per theta continuando a usare la distanza media delle ruote per la traslazione. Applica la traslazione usando la media del vecchio e del nuovo orientamento del giroscopio.

Questo non rende assoluta la posizione. I giroscopi hanno un bias e la distanza delle ruote continua a scivolare. Per correggere la posa accumulata serve un sensore globale: punti di riferimento visivi, localizzazione con lidar o un altro riferimento esterno.

Nella terminologia ROS, una stima locale continua appartiene naturalmente al sistema odom. Un sistema di localizzazione può aggiornare map → odom senza forzare la trasformazione localmente continua odom → base_link a saltare.

Misura la deriva anziché nasconderla

Esegui tre prove ripetibili:

1. Avanza e indietro fino al punto di partenza.
2. Esegui diverse rotazioni e torna all’orientamento iniziale.
3. Percorri un quadrato grande e confronta la posa finale con il segnale di partenza.

Registra i delta degli encoder, la posa calcolata, la tensione della batteria e la superficie. Ripeti ogni prova in entrambe le direzioni. Un errore di scala costante suggerisce una questione di taratura; un errore che cambia con la direzione suggerisce gioco o asimmetria; un errore che cambia con la superficie suggerisce slittamento.

Risoluzione dei problemi

Prova le conseguenze sullo sterzo nel Simulatore di robot seguilinea e confronta le stime locali del movimento con i concetti di mappatura in Fondamenti di SLAM. Per l’attività che dipende più direttamente da questa stima, percorrere file parallele su un’intera stanza, consulta pianificazione del percorso di copertura e il simulatore di copertura.