Guida introduttiva alla navigazione a stima (Dead Reckoning): Integrazione di ruote, IMU e GNSS per tracciare il movimento di un robot.

Il metodo più elementare per un robot per sapere di essersi spostato di "1 metro a destra e 2 metri in avanti" consiste nel sommare costantemente i propri spostamenti. Integra nel tempo il numero di rotazioni delle ruote, la velocità dei motori e la velocità angolare dell'IMU per stimare la sua posizione attuale. Questo metodo è chiamato navigazione inerziale (dead reckoning). Continua a funzionare anche quando il GPS o una telecamera non sono temporaneamente disponibili, ma piccoli slittamenti delle ruote o errori di misurazione si traducono in un errore di posizione che aumenta nel tempo.

Osservazione GNSS assoluta

Esempio di IMU a basso costo

Immagini: Surveyor Utilizzo di un ricevitore GNSS con soluzione RTK (SweetCanadianMullet, CC BY 4.0) / GY-521 Modulo MPU-6050 (Nevit Dilmen, CC BY-SA 3.0), Wikimedia Commons. Sensori rappresentativi, non un abbinamento hardware obbligatorio.

Riepilogo di 30 secondi

• La navigazione inerziale (dead reckoning) prevede lo stato attuale a partire dallo "stato precedente più lo spostamento misurato". Può aggiornarsi rapidamente anche senza osservazioni.

• In un robot a trazione differenziale, la differenza tra lo spostamento delle ruote sinistra e destra determina l'angolo di sterzata, e la loro media determina la distanza percorsa in avanti. L'errore nel raggio della ruota e nella larghezza del battistrada si manifesta come una curva sistematica.

• L'errore di odometria si accumula tramite integrazione. Per ottenere una posizione assoluta è necessaria una correzione basata su osservazioni come GNSS, LiDAR, telecamera o marcatori riflettenti.

• Un'IMU è robusta per le variazioni di assetto a breve termine, ma la posizione varia rapidamente quando l'accelerazione viene integrata due volte. L'approccio comune consiste nell'integrare le ruote e un'IMU tramite un filtro di Kalman esteso (EKF) o un grafo fattoriale.

• In fase di implementazione, timestamp, sistemi di coordinate, convenzioni di segno sinistra/destra, raggio della ruota, rilevamento dello slittamento e covarianza devono essere definiti correttamente fin dall'inizio: un errore in questo ambito non sarà sufficiente nemmeno con sensori costosi.

1. Cinematica della trazione differenziale

Sia r il raggio della ruota sinistra e destra, b la distanza tra le ruote (carreggiata) e \omega_L,\omega_R le velocità angolari sinistra/destra. Le velocità lineari delle ruote sono v_L=r\omega_L e v_R=r\omega_R, quindi la velocità di traslazione del centro del veicolo v e la velocità di imbardata \dot{\theta} sono:

Indicando con (x_k,y_k,\theta_k) la posizione al tempo k e con \Delta t il periodo di campionamento, una semplice integrazione di Eulero fornisce:

Quando si percorre un arco a velocità costante, l'utilizzo dell'integrazione esatta (la mappa esponenziale nelle coordinate del corpo) mantiene l'errore ridotto anche a bassa velocità e con elevata velocità di sterzata.

Figura 1 — Nella trazione differenziale, la differenza tra la velocità a sinistra e a destra crea la velocità di imbardata. Fermando una ruota, il robot ruota attorno ad essa.

2. Perché l'odometria presenta una deriva

Se il raggio della ruota presenta un errore dell'1% e si presume che la sinistra e la destra siano uguali durante la guida, il robot girerà gradualmente anche quando pensa di andare dritto. La risoluzione dell'encoder sinistra/destra, il gioco degli ingranaggi, le irregolarità del pavimento, l'usura degli pneumatici e la deformazione indotta dal carico diventano tutti errori sistematici. Se una ruota slitta su un pavimento bagnato o ghiaioso, la rotazione misurata e lo spostamento effettivo non corrispondono più.

L'errore di assetto \delta\theta produce un errore di posizione laterale. Per una distanza in avanti s, un'approssimazione a breve termine fornisce

Percorrendo 1 km con un errore angolare di 0,5 gradi, lo scostamento laterale raggiunge circa 8,7 m. Esprimere semplicemente l'errore di posizione come "tanti centimetri al secondo" non è in grado di cogliere le diverse strutture di errore che si verificano durante la guida in rettilineo, in curva e in slittamento.

La covarianza dell'odometria delle ruote propaga l'incertezza dello spostamento laterale nella posizione e la aggiorna di conseguenza. Nell'implementazione, la covarianza viene aumentata durante le curve o quando viene rilevato uno slittamento, in modo che la fusione dei sensori a valle non si fidi eccessivamente dell'odometria.

1. Aggiunta di un IMU

L'integrazione della velocità angolare \omega_z dal giroscopio dell'IMU fornisce

dove b_g è il bias del giroscopio e n_g è il rumore. Anche nell'istante in cui una ruota slitta, la variazione a breve termine della velocità angolare può comunque essere misurata, fornendo una stima dell'assetto più precisa rispetto alle sole ruote. Quando si esegue una doppia integrazione dell'accelerazione per ottenere velocità e posizione, l'errore dovuto al bias b_a cresce come

pertanto un IMU da solo non può fungere da sensore di posizione a lungo termine.

Combinazione di ruote e IMU: le ruote gestiscono lo spostamento a bassa frequenza, mentre l'IMU gestisce la velocità angolare e l'accelerazione ad alta frequenza. Il rollio e il beccheggio vengono inizializzati dalla direzione della gravità e, quando è disponibile un magnetometro o un GNSS, viene aggiunto anche un riferimento di imbardata assoluta.

4. Correzione con un EKF o un grafo fattoriale

Un esempio di vettore di stato è:

Nella fase di predizione, viene calcolato un modello di movimento non lineare f a partire dalle ruote e dall'IMU; quando arriva un'osservazione, il residuo viene utilizzato per correggerla.

Il filtro di Kalman esteso (EKF) linearizza localmente il modello non lineare tramite la matrice jacobiana. La posizione GNSS, la corrispondenza della scansione LiDAR, i punti caratteristici della telecamera e le pose dei marker noti possono essere tutti importati come osservazioni \mathbf{z}. Per cicli o rivisitazioni di grandi dimensioni, le pose e le osservazioni precedenti vengono memorizzate in un grafico fattoriale e l'intero processo viene riottimizzato successivamente.

Per ROS 2 (ipotizzando le definizioni dei messaggi standard di Jazzy), i messaggi nav_msgs/msg/Odometry della ruota, sensor_msgs/msg/Imu dell'IMU e la trasformazione TF odom → base_link dovrebbero utilizzare una base temporale comune. Questi tipi di messaggio corrispondono alle sigle ROS 1 nav_msgs/Odometry e sensor_msgs/Imu, ma i dettagli di implementazione, la QoS e le impostazioni della distribuzione per ROS 2 devono essere verificati separatamente. L'EKF di robot_localization è un buon punto di partenza per l'implementazione, ma azzerare le covarianze o inserire le stesse informazioni due volte rende la stima innaturalmente rigida.

5. Come viene utilizzato nei robot attuali

I robot mobili autonomi per interni (AMR) utilizzano costantemente encoder sulle ruote e un'IMU, compensando la deriva tramite LiDAR o mappatura con telecamera. Nella guida autonoma, velocità delle ruote, angolo di sterzata, IMU, GNSS e LiDAR vengono integrati in modo che la stima rimanga valida per secondi o decine di secondi, anche quando il segnale GNSS si interrompe in una galleria o sotto una struttura sopraelevata. Nei robot agricoli, fango, solchi e pendenze aumentano lo slittamento delle ruote, quindi RTK-GNSS, IMU e sensori di forza dell'attrezzo vengono combinati.

I robot sottomarini e i veicoli spaziali possono operare per lunghi tratti senza GNSS. Un log di velocità Doppler (DVL), un inseguitore stellare o la mappatura del terreno vengono aggiunti come osservazioni assolute a bassa frequenza per tenere sotto controllo la covarianza della navigazione inerziale. Ciò che conta è trasmettere la stima alla pianificazione e al controllo insieme al livello di affidabilità.

6. Lista di controllo per l'implementazione

1. Convertire i timestamp dei sensori in un orologio comune e misurare la latenza.

2. Disegnare i sistemi di coordinate base_link, odom e map e le relative convenzioni di segno destrorso.

3. Identificare il raggio della ruota, la carreggiata e la risoluzione dell'encoder tramite prove di guida reali.

4. Valutare l'errore separatamente per la guida in linea retta, le curve a velocità costante, i percorsi a otto, le asperità e le superfici a bassa aderenza.

5. Aumentare la covarianza quando vengono rilevati slittamenti o interruzioni del segnale e correggerla quando si ottiene un'osservazione assoluta.

6. Riprodurre i log per confermare che lo stesso input produca la stessa stima.

Durante le prove, registrare non solo l'errore di posizione del punto finale, ma anche l'errore di assetto, l'errore massimo, il 95° percentile e la velocità di deriva durante le interruzioni del segnale. Una stima che di solito è accurata ma occasionalmente molto errata non può essere utilizzata per la pianificazione di percorsi sicuri.

Riepilogo

La navigazione inerziale è la tecnica fondamentale per stimare il movimento di un robot con il minor ritardo possibile. Integra lo spostamento delle ruote e la velocità angolare dell'IMU, sopprimendo la deriva con osservazioni assolute da GNSS, LiDAR o telecamera. Prima di aggiungere altri sensori, è fondamentale definire correttamente il modello di movimento, i timestamp, i sistemi di coordinate e la covarianza, elementi che determinano la precisione e la ripetibilità.