Improvement of Robot's Simultaneous Localization and Mapping Using an Effective Transformation to Achieve Linear Model

Sintesi Tecnica: Miglioramento della Localizzazione e Mappatura Simultanea di un Robot mediante una Trasformazione Efficace per Ottenere un Modello Lineare

Definizione del Problema
La Localizzazione e Mappatura Simultanea (SLAM) è un compito fondamentale per i robot mobili, tuttavia le implementazioni standard basate sul Filtro di Kalman Esteso (EKF) soffrono di divergenza e inconsistenza. Le cause primarie di questi fallimenti sono la non linearità inerente ai modelli di moto e di osservazione del robot, che rende necessaria la linearizzazione. Questa linearizzazione introduce errori che si accumulano nel tempo, portando ad aggiornamenti errati del vettore di stato, salti nelle posizioni stimate e una riduzione della matrice di covarianza stimata che non riflette l'incertezza reale. Sebbene siano stati proposti vari metodi come Fast SLAM, Unscented Fast SLAM (UnFS) e varianti del Cubature Kalman Filter (CKF) per mitigare questi problemi, essi spesso si affidano ad approssimazioni complesse o faticano ancora a mantenere la consistenza in ambienti estesi o in presenza di alta incertezza dei sensori.

Metodologia
Il documento propone un approccio innovativo denominato LMKF SLAM (Linear Model Kalman Filter SLAM). Il nucleo della metodologia consiste nel trasformare il modello standard dello spazio degli stati, non lineare, in un modello strettamente lineare, consentendo l'applicazione diretta del filtro di Kalman standard (KF) senza la necessità di approssimazioni di linearizzazione o calcoli di Jacobiani.

1. Trasformazione dello Spazio degli Stati:

   • Modello di Moto: Gli autori assumono che il robot sia dotato di una semplice bussola elettronica per misurare l'angolo assoluto ($\theta_k$) del robot ad ogni passo. Incorporando questa misurazione, il vettore di stato viene ridotto escludendo l'angolo di orientamento. Le equazioni di moto, originariamente non lineari a causa delle funzioni trigonometriche di $\theta_k$, vengono trasformate. I termini di rumore vengono linearizzati attorno allo zero, risultando in un modello di moto lineare in cui l'aggiornamento dello stato dipende linearmente dallo stato precedente e dagli input di controllo.
   • Modello di Osservazione: Allo stesso modo, il modello di osservazione, che mette in relazione la distanza ($r_k$) e l'azimut ($\phi_k$) con le posizioni dei landmark, viene trasformato. Utilizzando l'angolo misurato del robot $\theta_k$, le coordinate polari dei landmark vengono convertite in coordinate cartesiane rispetto al frame globale. Questa trasformazione ($\zeta_k$) linearizza la relazione tra i dati osservati e il vettore di stato (posizioni del robot e dei landmark).

2. Algoritmo di Filtraggio:

   • Con entrambi i modelli di moto e di osservazione linearizzati, viene applicato direttamente il filtro di Kalman standard.
   • L'algoritmo impiega l'elaborazione sequenziale per gli aggiornamenti dei landmark per aumentare la velocità computazionale, trattando le osservazioni di diversi landmark come indipendenti.
   • Il vettore di stato consiste nelle coordinate $(x, y)$ del robot e nelle coordinate di tutti i landmark osservati.

Contributi Chiave

• Linearizzazione tramite Trasformazione: Il documento introduce una semplice trasformazione che converte i modelli SLAM non lineari in modelli lineari utilizzando solo una semplice bussola elettronica, eliminando la necessità di tecniche di linearizzazione complesse come i Jacobiani dell'EKF o le trasformazioni Unscented.
• Convergenza Teorica: A differenza dei metodi basati su EKF dove le prove di convergenza sono difficili a causa delle non linearità, il modello lineare proposto permette una rigorosa prova di convergenza, garantendo che l'errore di posizionamento assoluto dei landmark tenda verso un limite inferiore.
• Robustezza: Il metodo è progettato per essere altamente stabile rispetto alle incertezze dei sensori (velocità, angolo di sterzata e angolo del robot) e ai cambiamenti dei parametri di sistema.

Risultati Sperimentali
Gli autori hanno valutato LMKF SLAM rispetto ai metodi standard EKF, UnFS e Improved CKF (ICKF) utilizzando sia simulazioni artificiali che il dataset del mondo reale di Victoria Park.

• Simulazione (Ambiente Artificiale):

  • I test sono stati condotti sia su percorsi a ciclo chiuso che a ciclo aperto.
  • Accuratezza: LMKF ha dimostrato un errore quadratico medio (RMSE) significativamente più basso sia per il posizionamento del robot che per quello dei landmark rispetto a EKF, UnFS e ICKF.
  • Robustezza: Le prestazioni di LMKF sono rimaste stabili anche quando la velocità del robot ($v$) e il rumore del sensore ($\sigma_G, \sigma_\theta$) sono stati aumentati. Al contrario, l'RMSE di EKF e ICKF è cresciuto rapidamente con l'aumento del rumore e della velocità.
  • Dipendenza dal Percorso: Mentre altri metodi hanno mostrato una significativa riduzione dell'errore nei percorsi a ciclo chiuso (grazie alla correzione della chiusura del ciclo), LMKF ha mantenuto un'alta accuratezza nei percorsi a ciclo aperto, dimostrando che le sue prestazioni sono meno dipendenti dall'esistenza di cicli chiusi.
  • Stabilità: La deviazione standard dell'RMSE ($\sigma_{RMSE}$) è stata la più bassa per LMKF, indicando una maggiore affidabilità attraverso diverse sequenze di rumore.

• Dati del Mondo Reale (Dataset Victoria Park):

  • Utilizzando un dataset con uno scanner laser 2D e GPS, gli autori hanno stimato i dati mancanti dell'angolo del robot utilizzando il GPS e le informazioni sul percorso.
  • Nonostante la stima imprecisa dell'angolo del robot (uno svantaggio per il metodo proposto), LMKF ha ottenuto l'RMSE più basso (1,2 m) e l'Errore Assoluto Massimo più basso (3,46 m) rispetto a EKF (2,58 m), UnFS (2,1 m) e ICKF (2,33 m).
  • LMKF ha inoltre richiesto il minor tempo di esecuzione (45,43 secondi) rispetto agli altri metodi.

Significatività e Rivendicazioni
Il documento afferma che il metodo proposto LMKF SLAM rappresenta un miglioramento significativo rispetto ai metodi di filtraggio allo stato dell'arte. Raggiungendo un modello lineare attraverso una semplice trasformazione, il metodo garantisce la convergenza e offre accuratezza e stabilità superiori. Gli autori sottolineano che LMKF è particolarmente robusto contro le incertezze dei sensori e i cambiamenti dei parametri di sistema, superando EKF, UnFS e ICKF anche nelle condizioni in cui i metodi concorrenti erano teoricamente favoriti (ad esempio, quando i parametri di simulazione sono stati tarati per beneficiare i modelli non lineari). Lo studio conclude che questo approccio affronta efficacemente i problemi di divergenza comuni nello SLAM basato su EKF senza richiedere un carico computazionale complesso.