Introduzione all'ingegneria del controllo: osservatori di stato e filtro di Kalman lineare

Un robot raramente legge direttamente lo stato di cui ha bisogno. Un encoder può fornire la posizione ma non la velocità; un'IMU riporta accelerazione e velocità angolare con un errore sistematico; la localizzazione tramite telecamera arriva in ritardo o scompare. Un osservatore di stato utilizza un modello del sistema e la differenza tra l'output previsto e quello misurato per stimare lo stato nascosto. Il filtro di Kalman lineare (KF) è l'osservatore standard che include una matrice di covarianza dell'errore e utilizza il rumore di processo e di misura ipotizzato per decidere quanto fidarsi della previsione rispetto alla misurazione.

L'idea chiave non è "mediare i sensori". Si tratta di specificare quale istante di tempo fisico rappresenta un dato, quale modello di errore viene assunto e cosa accade quando i sensori non concordano. Uno stimatore che è sicuramente errato spinge PID, LQR e MPC verso la direzione sbagliata. Per informazioni su temporizzazione e limiti di esecuzione di ROS 2, consultare ROS 2 Primer; per un'integrazione più ampia dei sensori, consultare Sensor Fusion Primer.

Conclusione pratica

• Un osservatore aggiorna \hat{x} in base alle previsioni e alle innovazioni del modello. Uno stato non osservabile non può essere ricostruito in modo univoco semplicemente selezionando un filtro più intelligente.

• Il filtro di Kalman (KF) utilizza la covarianza dell'errore di stato P e la covarianza del processo Q e la covarianza della misurazione R per calcolare il guadagno di Kalman K.

• Q,R sono ipotesi sugli errori del modello e del sensore, non "parametri di precisione" arbitrari. È necessario convalidarle con innovazioni, covarianza, timestamp e dati di guasto.

• Se una stima è obsoleta, la covarianza è eccessiva, l'innovazione è anomala o le trasformazioni falliscono, passare uno stato degradato al controllo e ridurre o arrestare il movimento in base al rischio.

Osservatore nello spazio degli stati e osservabilità

Per un sistema lineare discreto,

x rappresenta lo stato reale, u l'ingresso noto, z la misurazione, w il disturbo di processo e v il rumore di misurazione. Un osservatore di Luenberger ha la forma

L'ultimo termine rappresenta l'innovazione. Un elevato guadagno dell'osservatore L segue rapidamente la misurazione ma trasmette più rumore; un basso L si fida del modello più a lungo ma corregge i disturbi lentamente.

La matrice di osservabilità \mathcal O=[C^T,(CA)^T,\ldots]^T deve avere rango massimo per ricostruire ogni stato nel caso lineare ideale. Le misurazioni di sola posizione di un doppio integratore possono rendere la velocità osservabile nel tempo, ma la frequenza di campionamento, la quantizzazione, il ritardo e il dropout determinano se la stima risultante è utile. "Osservabile" non sostituisce un requisito di latenza e accuratezza massime.

Previsione e aggiornamento di Kalman

Supponendo distribuzioni gaussiane bianche a media zero w\sim\mathcal N(0,Q) e v\sim\mathcal N(0,R), la distribuzione a priori è:

P rappresenta l'incertezza dell'errore di stima, non semplicemente la varianza dello stato. Durante un'interruzione della misurazione, la propagazione del modello normalmente fa sì che P^- cresca attraverso Q.

Per Per ogni misurazione in arrivo, calcolare l'innovazione \nu_k=z_k-C\hat{x}_k^- e la sua matrice di covarianza.

quindi aggiornare con:

La precisione finita può compromettere la simmetria o la semidefinità positiva di P. La forma di Joseph, P^+=(I-KC)P^-(I-KC)^T+KRK^T, è spesso preferita; risolvere sistemi lineari fattorizzati è spesso più sicuro che formare esplicitamente un'inversa. La matrice di covarianza P deve essere controllata dopo ogni aggiornamento.

Scelta di Q e R

R dovrebbe riflettere l'errore misurato in condizioni reali di vibrazione, temperatura, illuminazione, portata, ritrasmissione dei pacchetti e quantizzazione, non solo un valore di accuratezza di catalogo. Q rappresenta quanto può essere errato il modello. In un periodo: accelerazione sconosciuta, random walk con bias, variazione del carico utile o dinamica non modellata. Un valore minimo di Q si basa eccessivamente sul modello; un valore eccessivo di Q segue misurazioni rumorose.

Esempio numerico: stima della velocità a partire da misurazioni di posizione

Per uno stato unidimensionale a velocità costante x=[p\ v]^T con periodo di campionamento T,

Viene misurata solo la posizione. Come aggiornamento concreto adimensionale, consideriamo T=1, \hat{x}^-=[0\ 0]^T, P^-=[[2,1],[1,1]], z=1, R=1 e C=[1\ 0]. Allora S=3 , K=[2/3\ 1/3]^T e \hat{x}^+=[2/3\ 1/3]^T ; la covarianza a posteriori è P^+=(I-KC)P^-=[[2/3,1/3],[1/3,2/3]] . Questo illustra un aggiornamento e non costituisce una dichiarazione di accuratezza del sensore. Un valore iniziale elevato di P_{vv} esprime incertezza sulla velocità. Le innovazioni di posizione ripetute inferiscono la velocità stimata \hat v ; un dropout mantiene la previsione in esecuzione ma ne aumenta l'incertezza. Un controllore che necessita della velocità deve verificare P_{vv} , l'età della misurazione e lo stato residuo, quindi rallentare o arrestarsi quando superano l'inviluppo definito.

L'aggiunta di un'IMU aiuta a tracciare l'accelerazione a breve termine, ma omettere il bias dallo stato consente l'accumulo dell'errore di integrazione. Le osservazioni della telecamera o del LiDAR possono vincolare la posizione assoluta quando sono collegate a una mappa nota, un marker o un altro riferimento; il solo tracciamento relativo non garantisce la posizione globale. Anche il GNSS è Condizionato dalla visibilità del satellite, dal multipath, dalla frequenza di aggiornamento, dalla latenza e dall'allineamento del frame. Per osservazioni non lineari di assetto o distanza, è possibile utilizzare EKF o UKF, ma permangono le problematiche del filtro di Kalman lineare relative a Q,R, alla covarianza della misurazione R, al tempo, agli outlier e all'osservabilità.

Il quadrato dell'innovazione normalizzata \nu^TS^{-1}\nu aiuta a rilevare una misurazione incoerente con la sua distribuzione prevista. Un valore elevato può indicare un outlier, un frame errato, un errore di timestamp, un guasto del sensore o una discrepanza del modello. Il gating di un dato non è un dispositivo per nascondere gli errori: dovrebbe fornire informazioni diagnostiche e una modalità degradata sicura.

Rappresentativa IMU/INS

Telecamera di profondità rappresentativa

Immagini: Xsens MTi-G (Kallap85, CC BY-SA 4.0) e Intel RealSense D435 (Marc Auledas, CC BY-SA 4.0), Wikimedia Commons. Si tratta di sensori rappresentativi, non di dati misurati o di un sistema combinato raccomandato.

Fornitura di PID, LQR e MPC

Il PID può utilizzare il feedback filtrato grezzo, ma il suo termine derivativo e un anello di velocità interno spesso traggono vantaggio da una stima. LQR Presuppone un sistema multistato \hat{x}; nelle condizioni del principio di separazione, un guadagno LQR e uno stimatore di Kalman formano un'architettura LQG. Saturazione, ritardo e non linearità possono comunque compromettere la robustezza nel mondo reale. L'MPC dipende anche da una stima tempestiva dello stato iniziale e del disturbo; l'ottimizzazione basata su una stima obsoleta o su una covarianza falsamente piccola crea un piano ottimizzato per un mondo non adatto.

Temporizzazione del robot e sicurezza dai guasti Funzionamento

In ROS 2, distinguere il tempo di misurazione dal tempo di arrivo del messaggio. Aggiornare il presente con un dato della telecamera in ritardo come se fosse corrente trascina il robot verso il suo passato. Scegliere e documentare una politica: riavvolgere/ripropagare la cronologia, rifiutare i dati ritardati o separare l'odometria di basso livello dalle correzioni della mappa più lente. Le trasformazioni di trasporto e di coordinate di ROS 2 descritte in ROS 2 Primer e Sensor Fusion Primer sono precondizioni esterne alle equazioni del filtro.

Monitorare timeout, limiti di covarianza, ripetuti errori del gate di innovazione, bias divergente, trasformazioni non disponibili e superamento delle scadenze. Se tutte le misurazioni scompaiono, non eseguire indefinitamente basandosi solo sulla previsione. Selezionare limitazione della velocità, arresto, ridondanza o handover dell'operatore in base all'applicazione. Il reset dell'estimatore può saltare di stato, quindi riferimenti del controllore, stati integrali e uscite Necessità di una gestione senza urti.

Lista di controllo per l'implementazione

1. Definire stato, misurazione, input, frame, tempo di misurazione e tempo di ricezione.

2. Quantificare l'osservabilità, l'accuratezza richiesta e la latenza massima per ogni stato.

3. Registrare le prove per Q,R,P_0 da specifiche, dati e test.

4. Monitorare la simmetria/definitezza di P, innovazione, gate e timeout.

5. Iniettare casi di dropout, outlier, rilocalizzazione e variazione di bias.

6. Definire cosa riceve PID/LQR/MPC quando la qualità della stima si degrada e verificare l'azione di sicurezza indipendente.

Riferimenti

• Kalman, 1960: A New Approach to Linear Filtering and Prediction Problems
• [MIT Underactuated Robotics: LQR]( https://underactuated.mit.edu/lqr.html - Introduzione a ROS 2, Introduzione alla fusione di sensori