Impedance MPC for Physical Human-Robot Interaction: Predictive Disturbance Rejection with Joint-Limit Safety

Sintesi Tecnica: MPC di Impedenza per l'Interazione Fisica Uomo-Robot

Enunciato del Problema
L'interazione fisica uomo-robot (pHRI) richiede che i robot mantengano simultaneamente un'elevata precisione di traiettoria e garantiscano una sicurezza conforme durante contatti imprevisti. Il controllo di impedenza classico, pur essendo efficace per la sicurezza, soffre di un limite strutturale fondamentale: sotto una forza umana sostenuta ($F_h$), presenta un errore di posizione a regime non nullo definito come $e_\infty = K_d^{-1}F_h$. Ad esempio, una spinta di 15 N contro una rigidezza di 300 N/m risulta in una deflessione di 50 mm. Sebbene l'aumento della rigidezza ($K_d$) riduca questo errore, esso viola i vincoli di sicurezza aumentando le forze di contatto. Viceversa, l'aggiunta di un'azione integrale per eliminare l'errore a regime è severamente limitata dai requisiti di stabilità, che ne restringono la velocità di convergenza. Gli approcci esistenti di Model Predictive Control (MPC) per la pHRI spesso mantengono la dinamica non lineare completa del sistema, limitando le frequenze di controllo, o non riescono a fornire garanzie formali di errore a regime nullo sotto disturbi persistenti.

Metodologia
Gli autori propongono un'architettura Impedance MPC a due strati progettata per risolvere la tensione tra precisione di traiettoria e sicurezza conforme.

• Strato 1: Inversione Non Lineare Analitica
  Il primo strato impiega una strategia di controllo feedforward basata sulla decomposizione dello spazio operativo. Esso cancella analiticamente gravità, termini di Coriolis e di inerzia dello spazio di task. Questo trasforma la dinamica residua del sistema in un doppio integratore lineare indipendente dalla configurazione.

  • La dinamica dell'errore è ridotta a $\ddot{e} = -\Lambda^{-1}(q) F_{mpc} + d(t)$, dove $d(t)$ rappresenta i disturbi aggregati (forza umana, attrito, errori di modello).
  • Crucialmente, questa linearizzazione produce una matrice di transizione di stato costante ($A_d$) per il sistema in tempo discreto, indipendentemente dalla configurazione del robot. Ciò consente alla matrice di "risposta libera" di essere precomputata una sola volta, riducendo significativamente il carico computazionale online.

• Strato 2: QP Convesso con Augmentation del Disturbo
  Il secondo strato risolve un problema di ottimizzazione a orizzonte recedente a 100 Hz (con un loop interno a 1 kHz per gli aggiornamenti feedforward).

  • Stima del Disturbo: Un filtro di Kalman aumentato stima uno stato di disturbo persistente ($\hat{d}$) modellato come un cammino casuale (random walk). Questo stato è integrato nel modello di predizione MPC, consentendo un tracking senza offset (errore a regime n zero) per disturbi costanti o asintoticamente costanti.
  • Ottimizzazione: Il controllore risolve un Programma Quadratico (QP) convesso a 30 variabili soggetto a vincoli di box sulla forza di controllo ($F_{mpc}$). La funzione di costo minimizza l'errore di tracking su un orizzonte temporale rispettando i limiti articolari.
  • Meccanismi di Sicurezza: La sicurezza dei limiti articolari è garantita tramite un approccio a doppio barriera: un potenziale di barriera inversa nello spazio nullo e una proiezione dello spazio di lavoro. Questi meccanismi impediscono le violazioni dei limiti articolari senza degradare il tracking del task nelle regioni sicure.

Contributi Chiave

1. Architettura a due strati con $A_d$ costante: Cancellando analiticamente le non linearità, gli autori riducono il sistema a un sistema lineare tempo-variante con una matrice di stato costante. Ciò consente di risolvere un QP a 30 variabili in meno di 1 ms, rendendo fattibile l'implementazione in tempo reale.
2. Connessione Analitica con l'Impedenza Classica: Il documento dimostra (Teorema 1) che, nel limite non vincolato e privo di disturbi, l'MPC proposto è matematicamente equivalente a un classico controllore di impedenza nello spazio di task. La rigidezza e lo smorzamento realizzati sono determinati dai guadagni LQR derivati dai pesi del costo. Questo posiziona il metodo come un'estensione del controllo di impedenza classico che aggiunge gestione dei vincoli e reiezione dell'offset, piuttosto che come un suo sostituto.
3. Garanzia Formale di Errore a Regime Nullo: Attraverso l'augmentation del disturbo tramite Kalman, il sistema ottiene una garanzia basata sul Principio del Modello Interno di errore a regime nullo per disturbi persistenti, estendendo la precedente teoria MPC senza offset ai manipolatori controllati in coppia.
4. Sicurezza dei Limiti Articolari a Doppia Barriera: L'integrazione di potenziali dello spazio nullo e proiezioni dello spazio di lavoro impedisce empiricamente le violazioni dei limiti articolari attraverso i workspace testati, inclusi i test di stress ai bordi.

Risultati Sperimentali
Gli esperimenti sono stati condotti su un robot Franka FR3 a 7 DOF utilizzando la simulazione MuJoCo. Il metodo proposto (Impedance MPC con augmentation di Kalman) è stato confrontato con l'impedenza classica, il controllo di ammettenza e l'impedenza PI.

• Errore a Regime: Sotto una forza sostenuta di 15 N, il metodo proposto ha ottenuto un errore a regime <0,05 mm, rispetto ai 44,8 mm dell'impedenza classica (una riduzione di oltre 800 volte).
• Prestazioni Transitorie: Nel tracking di traiettorie circolari, l'MPC da solo (senza Kalman) ha ridotto l'errore RMS nella finestra di contatto di 19 volte rispetto all'impedenza classica. L'aggiunta del filtro di Kalman ha eliminato completamente l'errore a regime.
• Robustezza: Il sistema ha dimostrato una robustezza graduale al rumore di misura (l'errore a regime è rimasto <1 mm anche con un rumore di 5 mm) e al mismatch del modello inerziale (l'RMSE di tracking è rimasto invariato fino al 30% di errore di inerzia quando il filtro di Kalman era attivo).
• Sicurezza: Nei test di confine in cui l'impedenza classica violava i limiti articolari, l'MPC ha mantenuto un margine di sicurezza (margine articolare minimo di 0,084 rispetto a 0,048 per l'impedenza).

Significatività e Rivendicazioni
Il documento afferma che l'architettura proposta riesce a unificare le richieste contrastanti della pHRI: un elevato tracking della traiettoria e una sicurezza conforme. La principale significatività risiede nell'intuizione strutturale secondo cui la cancellazione feedforward analitica consente una struttura lineare costante, la quale a sua volta permette:

1. Precomputazione: La matrice di risposta libera viene calcolata una sola volta, disaccoppiando la complessità del QP dalla configurazione del robot.
2. Garanzie Formali: La struttura lineare consente dimostrazioni rigorose di equivalenza dell'impedenza e di tracking senza offset.
3. Fattibilità in Tempo Reale: L'approccio raggiunge elevati tassi di controllo (QP a 100 Hz, feedforward a 1 kHz) su hardware standard, superando i colli di bottiglia computazionali dei precedenti approcci MPC non lineari.

Gli autori concludono che questo metodo fornisce una soluzione robusta e teoricamente fondata per la pHRI, che elimina l'errore a regime inerente al controllo di impedenza classico mantenendo al contempo i vincoli di sicurezza, con lavori futuri pianificati per la validazione su hardware fisico e l'aumento della passività tramite "energy tank".