Sintesi Tecnica: Assemblaggio di Corpi Rigidi Robusto tramite Controllo Ottimale con Contatto Implicito e Derivate del Secondo Ordine Esatte

Definizione del Problema

La pianificazione efficiente dei movimenti di assemblaggio rimane una sfida significativa nella robotica. Gli approcci tradizionali si affidano pesantemente al reinforcement learning (RL) senza derivate e ai metodi basati sul campionamento, che richiedono estese simulazioni fisiche e spesso mancano di efficienza campionaria. Sebbene i metodi basati sulle derivate abbiano mostato risultati promettenti in altri domini (ad esempio, la locomozione), essi faticano nei compiti di assemblaggio a causa della natura intrinsecamente non regolare della dinamica di contatto.

La difficoltà principale risiede nel fatto che i sistemi dinamici ricchi di contatti sono tipicamente formulati come Problemi di Complementarità Non Lineare (NCP) o Problemi di Complementarità Lineare (LCP). Le soluzioni a questi problemi sono non differenziabili rispetto agli stati e agli input di controllo perché variazioni infinitesimali possono causare commutazioni discrete nei set di contatto attivi (ad esempio, i punti di contatto che si spostano tra facce, spigoli o vertici, o i cambiamenti nelle modalità di attrito). Questa non regolarità impedisce l'applicazione diretta di efficienti metodi di ottimizzazione del secondo ordine, che si basano su derivate esatte per ottenere una rapida convergenza. Inoltre, le esistenti simulazioni di fisica differenziabile spesso trascurano la non differenziabilità introdotta dal rilevamento delle collisioni o si affidano ad approssimazioni (differenze finite, smoothing randomizzato) che non corrispondono alla valutazione nominale, ostacolando la convergenza dell'ottimizzazione.

Metodologia

Il paper propone un approccio di controllo ottimale robusto e campionario che utilizza una simulazione fisica differenziabile capace di fornire derivate analitiche del secondo ordine esatte. La metodologia integra tre componenti principali:

1. Rilevamento delle Collisioni Differenziabile e Smooth

Gli autori formulano il rilevamento delle collisioni come un Programma Lineare (LP) utilizzando le distanze di crescita (una variante delle funzioni di distanza con segno).

• Formulazione: Invece di scalare i poliedri in una forma stabile, applicano una scalatura uniforme in tutte le direzioni per garantire problemi di ottimizzazione ben scalati.
• Smoothing: Per gestire la non differenziabilità, impiegano metodi a punti interni. Risolvendo il sistema KKT dell'LP con un parametro di barriera $\tau > 0$, ottengono un'approssimazione smooth della funzione di distanza con segno (SDF), $\Phi_\tau(q)$.
• Derivate: Utilizzando il Teorema della Funzione Implicita (IFT), derivano le derivate esatte del primo e secondo ordine della SDF e del vettore normale di contatto.
• Approssimazione del Gradiente: Viene proposta una specifica approssimazione per il vettore normale di contatto, derivata dalle variabili primal-dual dell'LP. Ciò evita la necessità di derivate del terzo ordine della SDF, che sarebbero necessarie se il gradiente esatto venisse utilizzato direttamente.
• Robustezza: Viene introdotta una strategia di scalatura euristica per gestire i casi in cui il solver fallisce la convergenza a causa di matrici KKT mal condizionate, garantendo una generazione affidabile delle informazioni di contatto.

2. Fisica Multi-Contatto Differenziabile

Il problema della risoluzione del contatto è modellato utilizzando una formulazione della dinamica di contatto senza attrito basata sullo schema di time-stepping di Moreau, che risulta in un LCP di contatto.

• Smoothing/Relaxation: Le condizioni di complementarità vengono regolarizzate utilizzando lo smoothing di Scholtes (sostituendo $a \perp b$ con $ab = \sigma$) o la relaxation di Scholtes ($ab \leq \sigma$). Questo trasforma l'LCP non liscio in un insieme di vincoli di uguaglianza e disuguaglianza smooth.
• Integrazione: Le dinamiche di contatto smooth sono direttamente trascritte nel problema di controllo ottimale (OCP). Il solver ottimizza simultaneamente gli input di controllo e le dinamiche di sistema.
• Procedura di Omotopia: Per risolvere le dinamiche di contatto rigide (dove $\tau, \sigma \to 0$), viene utilizzata una procedura di omotopia. L'ottimizzazione inizia con parametri di smoothing ampi (fisica rilassata) e li restringe sequenzialmente, utilizzando le soluzioni precedenti per il warm-start del solver. Ciò consente all'algoritmo di convergere da ipotesi iniziali banali verso soluzioni che approssimano strettamente le formulazioni non differenziabili originali.

3. Controllo Ottimale Multi-Scenario Robusto

Per garantire un'esecuzione di successo sull'hardware reale nonostante i mismatch modello-realtà, gli autori formulano un OCP basato su multi-scenario.

• Tracciamento dell'Impedenza: Una traiettoria di riferimento viene ottimizzata, la quale viene tracciata da molteplici traiettorie "compliant" tramite un controllore di impedenza cartesiana.
• Perturbazione: Ogni scenario corrisponde a una traiettoria compliant con un offset posizionale randomizzato rispetto alla riferimento. L'ottimizzatore deve trovare una traiettoria di riferimento che completi con successo il compito di assemblaggio per tutti gli scenari simultaneamente.
• Obiettivo: Questo approccio ottimizza efficacementamente la robustezza, garantendo che il movimento pianificato rimanga fattibile anche se il robot reale devia leggermente dal modello.

Contributi Chiave

1. Formulazione Modificata della Distanza di Crescita: Una nuova formulazione dell'LP per le funzioni di distanza con segno che garantisce derivate ben scalate e approssima strettamente la distanza euclidea, includendo un'approssimazione del gradiente adatta per i vettori normali di contatto.
2. Implementazione Efficiente delle Derivate: Un'implementazione che utilizza il solver a punti interni ad alte prestazioni HPIPM e la libreria di algebra lineare BLASFEO per calcolare le derivate esatte del primo e secondo ordine del rilevamento delle collisioni e della risoluzione del contatto, interfacciate con la libreria di controllo ottimale CasADi.
3. Pianificazione Multi-Scenario Robusta: Una formulazione che combina il controllo ottimale con contatto implicito e la robustezza multi-scenario, garantendo movimenti eseguibili su robot reali con controllo di impedenza cartesiana.
4. Validazione Empirica: Esperimenti nel mondo reale che dimostrano alti tassi di successo (>99%) in compiti di inserimento perugina-foro (peg-in-hole) con tolleranza quasi nulla, e risultati di simulazione che mostrano i benefici delle Hessiane esatte rispetto alle approssimazioni (Gauss-Newton, L-BFGS) e l'impatto dei parametri di smoothing.

Risultati

• Prestazioni nel Mondo Reale: Gli esperimenti su un robot UR10e che esegue compiti di assemblaggio peg-in-hole hanno raggiunto oltre il 99% di esecuzioni di successo. Il metodo ha gestito con successo sostanziali mismatch modello-realtà senza ulteriori tuning.
• Efficienza Computazionale: La proposta implementazione di SDF calcola le derivate in modo efficiento. Sebbene sia leggermente più lenta degli algoritmi GJK allo stato dell'arte per la valutazione nominale, la capacità di calcolare derivate esatte del secondo ordine rende il processo di ottimizzazione complessivo significativamente più efficiente in termini di campionamento.
• Convergenza: I risultati numerici indicano che l'uso di Hessiane esatte migliora significativamente la velocità di convergenza rispetto alle appzioni di Hessiana come Gauss-Newton o L-BFGS.
• Impatto dello Smoothing: Lo studio investiga il trade-off tra i parametri di smoothing e i tassi di successo, mostrando come la procedura di omotopia colmi efficacemente il divario tra approssimazioni smooth e facili da risolvere e le dinamiche di contatto precise e rigide richieste per l'esecuzione nel mondo reale.

Significato

Il paper sostiene di colmare il divario tra l'ottimizzazione basata su derivate e i complessi compiti di assemblaggio. Fornendo un framework che utilizza derivate esatte del secondo ordine sia per il rilevamento delle collisioni che per la risoluzione del contatto, il metodo consente l'uso di efficienti solver basati su Newton per problemi ricchi di contatto. Ciò porta a:

• Migliore Efficienza Campionaria: Sono necessari molti meno step di simulazione fisica durante la pianificazione rispetto ai metodi senza derivate.
• Robustezza: La formulazione multi-scenario assicura che le traiettorie generate siano robuste alle incertezze, rendendole direttamente eseguibili su hardware reale tramite controllo di impedenza cartesiana.
• Generalità: L'approccio è applicabile a qualsiasi problema di assemblaggio di corpi rigidi in cui le forme sono rappresentate come unioni di poliedri convessi.

Il lavoro dimostra che, con un attento smoothing e il calcolo di derivate esatte, il controllo ottimale può risolvere efficacemente compiti di assemblaggio impegnativi che sono tradizionalmente stati il dominio del reinforcement learning e dei metodi basati sul campionamento.