Geometric Visual Servo Via Optimal Transport

Titolo: Controllo Visivo Geometrico tramite Trasporto Ottimale

1. Problema e Contesto

Il lavoro affronta le sfide nel controllo dei robot manipolatori, in particolare nel Visual Servoing (controllo basato sulla visione). I sistemi di controllo esistenti si dividono tradizionalmente in due categorie:

• PBVS (Pose-Based Visual Servoing): Utilizza le caratteristiche estratte per calcolare una posa 3D e minimizzare l'errore di posa.
• IBVS (Image-Based Visual Servoing): Traccia direttamente le caratteristiche nell'immagine per minimizzare l'errore di immagine.

Limitazioni attuali:

• Le metodologie attuali spesso ignorano le caratteristiche probabilistiche estratte dalle immagini, affidandosi invece a metodi di estrazione delle caratteristiche "a mano" (hand-tuned).
• Richiedono conoscenze specifiche del dominio per affinare quali caratteristiche tracciare.
• Spesso perdono le proprietà fisiche e geometriche intrinseche della piattaforma robotica durante la derivazione delle dinamiche.
• L'uso di cinematica inversa e controllo PD a livello di coordinate può compromettere le garanzie di prestazione.

L'obiettivo è sviluppare un legge di controllo geometrico unificato che combini la posa e l'errore basato sull'immagine, trattando le caratteristiche visive non come punti discreti, ma come distribuzioni di probabilità nello spazio delle operazioni.

2. Metodologia

L'approccio proposto integra la teoria dei gruppi di Lie, la meccanica Hamiltoniana e il Trasporto Ottimale (Optimal Transport - OT) in un unico framework di controllo.

A. Dinamiche Port-Hamiltoniane su SE(3)

Il sistema è modellato nello spazio delle operazioni come un gruppo di Lie SE(3) (il gruppo delle trasformazioni rigide 3D, composto da rotazioni e traslazioni).

• Le dinamiche del manipolatore sono formulate in forma Port-Hamiltoniana, preservando le proprietà geometriche e di conservazione dell'energia.
• La funzione di Hamiltoniana $H(g, p)$ rappresenta l'energia totale (cinetica + potenziale) del sistema, dove $g$ è la posa e $p$ è il momento coniugato.
• Viene utilizzato un controllo basato sulla passività (Passivity-Based Control - PBC) per modellare il sistema come un sistema di conservazione dell'energia, permettendo l'iniezione di energia per raggiungere un punto di equilibrio desiderato.

B. Mappatura delle Nuvole di Punti come Misure di Probabilità

Invece di trattare i dati della telecamera (RGB-D) come semplici punti, l'approccio definisce la mappa di profondità come una misura di probabilità $\mu$ sullo spazio SE(3).

• L'obiettivo è trasportare la distribuzione di probabilità iniziale (posa corrente) alla distribuzione target (posa desiderata).
• Viene utilizzato il Trasporto Ottimale (Optimal Transport) per calcolare la distanza tra queste due distribuzioni.
• Poiché le nuvole di punti possono avere masse diverse (a causa di occlusioni o limiti del campo visivo), viene utilizzata una versione sbilanciata (unbalanced) del trasporto ottimale, basata sulla rilassazione di Kantorovich, che permette di gestire variazioni di massa.

C. Legge di Controllo Ibrida

Il segnale di controllo totale $u$ è la somma di tre componenti:

1. $u_{ES}$ (Energy Shaping): Modella la forma dell'energia potenziale per guidare il sistema verso la posa desiderata (simile a un controllo proporzionale).
2. $u_{DI}$ (Damping Injection): Aggiunge smorzamento per garantire la stabilità asintotica (simile a un controllo derivativo).
3. $u_{DC}$ (Disturbance Compensation): Un termine innovativo calcolato tramite la mappa di trasporto ottimale dinamica. Questa componente utilizza la mappa di trasporto calcolata tra la nuvola di punti corrente e quella target per generare un input che minimizza l'energia necessaria per allineare le distribuzioni di profondità.

Il risultato è un controllore che agisce come un PID geometrico con compensazione di gravità, operando simultaneamente negli spazi della posa e dell'immagine.

3. Contributi Chiave

1. Riformulazione Geometrica: Una nuova formulazione delle dinamiche del manipolatore in forma Port-Hamiltoniana che combina la geometria dello spazio delle operazioni con il controllo nello spazio articolare.
2. Trattamento Probabilistico delle Immagini: Utilizzo dell'output di una telecamera RGB-D come misura di probabilità sul gruppo di Lie SE(3), eliminando la necessità di estrarre manualmente caratteristiche specifiche.
3. Controllo basato sul Trasporto Ottimale: Ridefinizione dell'errore di controllo basandosi sulla distanza di Wasserstein tra le mappe di profondità di riferimento e correnti, utilizzando flussi geodetici per generare segnali di controllo a minima energia.
4. Validazione Hardware: Implementazione e test su hardware reale, dimostrando la capacità di generalizzazione su diverse pose iniziali.

4. Risultati Sperimentali

I risultati sono stati ottenuti su un manipolatore robotico reale con un compito di "peg-in-hole" (inserimento di un perno in un foro), utilizzando diverse pose iniziali casuali.

• Convergenza: Il controllore ha dimostrato una rapida risposta iniziale quando l'errore (sia di posa che di mappa di profondità) è grande, rallentando fluidamente man mano che il robot si avvicina alla posa target.
• Traiettoria Geodetica: Il percorso tracciato dall'end-effector nello spazio 3D corrisponde alla geodetica più breve nello spazio SE(3), confermando l'efficienza energetica del controllo.
• Robustezza: Il sistema è riuscito a gestire diverse pose iniziali e rotazioni, convergendo a un errore nullo. Ha mostrato capacità di recupero anche in presenza di instabilità locali (quando la matrice Jacobiana diventa singolare).
• Conservazione dell'Energia: L'analisi dell'energia cinetica ha mostrato che l'energia del sistema tende a zero man mano che il momento coniugato si annulla, indicando che il sistema raggiunge l'equilibrio stabile guidato dal vettore di gravità.
• Tempo Reale: Il ciclo di controllo è stato eseguito in circa 300ms, dimostrando la fattibilità in tempo reale nonostante la complessità dei calcoli di trasporto.

5. Significato e Implicazioni

Questo lavoro rappresenta un passo significativo verso il controllo robotico autonomo più robusto e geometricamente coerente:

• Unificazione: Supera la dicotomia tra PBVS e IBVS, offrendo un framework unificato che gestisce sia la posa che le caratteristiche dell'immagine in modo naturale.
• Gestione dell'Incertezza: Trattando le caratteristiche visive come distribuzioni di probabilità, il metodo è intrinsecamente più robusto al rumore e alle variazioni ambientali rispetto ai metodi basati su punti discreti.
• Efficienza Energetica: L'uso della geometria di Lie e del trasporto ottimale garantisce che il robot segua percorsi a minima energia, un fattore cruciale per l'efficienza operativa e la durata della batteria.
• Futuro: Il lavoro apre la strada all'applicazione di tecniche di trasporto ottimale dinamico in scenari più complessi, come il controllo di sistemi con pose target non statiche (in movimento) e l'uso di reti neurali per accelerare il calcolo delle mappe di trasporto per nuvole di punti ad alta dimensionalità.

In sintesi, il paper propone un cambio di paradigma nel visual servoing, passando da un approccio basato su errori di punti a un approccio basato sul flusso di masse (distribuzioni), garantendo stabilità geometrica ed efficienza energetica.