Vai al contenuto
ELECTRONICS ENGINEERINGelectronics-engineering.com
Automotive

ACC 3: fusione radar e camera con il filtro di Kalman

Fusione centralizzata e traccia a traccia, modello del moto relativo, guadagno di Kalman dimostrato, perché due sensori sono meglio di uno, gating, associazione, gestione delle tracce e un errore di taratura.

https://www.electronics-engineering.com/acc-fusione-radar-camera-filtro-di-kalman-associazione-tracce

Nessun voto ancora: sii il primo a votare questa pagina.

Radar e camera mandano, ciascuno al proprio ritmo, liste di misure rumorose, a volte mancanti, a volte false. La fusione le trasforma in una lista di oggetti tracciati: per ognuno una stima di posizione e velocità con la sua incertezza, aggiornata ogni ciclo. Lo strumento è il filtro di Kalman, che abbiamo introdotto nella lezione sul filtro di Kalman e ritrovato tra gli osservatori discreti; qui lo applichiamo a più sensori, aggiungiamo l’associazione delle misure agli oggetti e la gestione delle tracce, e vediamo un errore di taratura che rende la fusione peggiore di un sensore da solo.

Due architetture

Fusione centralizzata e fusione traccia a traccia
A: fusione centralizzata, ogni misura di radar e camera aggiorna lo stesso filtro. B: ogni sensore ha il suo tracciatore e le tracce si fondono dopo (T2T), con l’intersezione delle covarianze (CI) perché gli errori delle due tracce non sono indipendenti.
  • centralizzata (A): la più precisa, perché il filtro usa ogni misura con la sua vera incertezza; richiede misure grezze o quasi, sincronizzate;
  • traccia a traccia (B): ogni centralina sensore fa già il suo tracciamento (succede spesso con sensori di fornitori diversi) e la centralina ADAS fonde le tracce. Due tracce dello stesso oggetto hanno errori correlati (hanno lo stesso modello di moto), e trattarle come indipendenti renderebbe la stima troppo ottimista. L’intersezione delle covarianze è sicura anche senza conoscere la correlazione: P^{-1} = \omega P_1^{-1} + (1-\omega) P_2^{-1}, \hat x = P\,(\omega P_1^{-1}\hat x_1 + (1-\omega)P_2^{-1}\hat x_2), con \omega \in [0,1] scelto per minimizzare la traccia di P.

Nel codice usiamo la fusione centralizzata.

Il modello del moto relativo

Lo stato di ogni oggetto è relativo alla nostra auto: \mathbf{s} = [x,\ v_x,\ y,\ v_y]^T. Longitudinalmente l’accelerazione relativa è \ddot x = a_t - a_{ego}: quella del bersaglio a_t non la conosciamo e la trattiamo come rumore, la nostra a_{ego} la conosciamo e la usiamo come ingresso. Integrando su un passo T con accelerazione costante (vedi la cinematica del moto rettilineo):

    \[ x_{k+1} = x_k + v_{x,k} T - \tfrac12 a_{ego} T^2 + \tfrac12 w_x T^2, \qquad v_{x,k+1} = v_{x,k} - a_{ego} T + w_x T, \]

e lo stesso lateralmente senza ingresso. In forma matriciale \mathbf{s}_{k+1} = F\mathbf{s}_k + B u + G\mathbf{w} con

    \[ F = \begin{bmatrix}1&T&0&0\\0&1&0&0\\0&0&1&T\\0&0&0&1\end{bmatrix},\quad G = \begin{bmatrix}T^2/2&0\\T&0\\0&T^2/2\\0&T\end{bmatrix},\quad Q = G\,\mathrm{diag}(q_x^2, q_y^2)\,G^T. \]

q_x e q_y sono le deviazioni standard delle accelerazioni che il modello non conosce: quanto può frenare o accelerare il bersaglio, quanto in fretta può spostarsi di lato. Il loro valore conta moltissimo, come vedremo.

Il filtro di Kalman

Filtro di Kalman come schema a blocchi
Il filtro come schema a blocchi: la stima precedente passa per il ritardo z^{-1} e il modello F (con l’ingresso u = -a_{ego}) e diventa la previsione \hat x_{k|k-1}; la misura z_k meno la misura prevista H\hat x_{k|k-1} è l’innovazione \nu, che moltiplicata per il guadagno K_k corregge la previsione.

A ogni misura il filtro fa due passi:

  • previsione fino all’istante della misura: \hat{\mathbf{s}} \leftarrow F\hat{\mathbf{s}} + Bu, P \leftarrow F P F^T + Q;
  • aggiornamento: innovazione \nu = z - H\hat{\mathbf{s}}, sua covarianza S = H P H^T + R, guadagno K = P H^T S^{-1}, poi \hat{\mathbf{s}} \leftarrow \hat{\mathbf{s}} + K\nu, P \leftarrow (I - KH)P(I-KH)^T + K R K^T.

Perché quel guadagno. Dopo la correzione l’errore è e^+ = (I - KH)e^- - K v, con e^- errore della previsione e v rumore di misura, indipendenti. La sua covarianza è

    \[ P^+ = (I-KH)P(I-KH)^T + K R K^T \]

(è la forma di Joseph, che usiamo anche nel codice perché resta simmetrica e positiva con i numeri in virgola mobile). Cerchiamo il K che rende minima la somma delle varianze, la traccia di P^+. Sviluppando e derivando rispetto a K, con \partial\,\mathrm{tr}(KAK^T)/\partial K = 2KA per A simmetrica:

    \[ \frac{\partial\,\mathrm{tr}P^+}{\partial K} = -2 P H^T + 2K (H P H^T + R) = 0 \quad\Rightarrow\quad K = P H^T S^{-1}. \]

Perché due sensori sono meglio di uno

Il caso più semplice dice tutto. Due stime indipendenti della stessa grandezza, x_1 con varianza \sigma_1^2 e x_2 con \sigma_2^2: la loro media pesata \hat x = w x_1 + (1-w) x_2 ha varianza w^2\sigma_1^2 + (1-w)^2\sigma_2^2, minima per w = \sigma_2^2/(\sigma_1^2+\sigma_2^2). Sostituendo:

    \[ \frac{1}{\sigma^2} = \frac{1}{\sigma_1^2} + \frac{1}{\sigma_2^2}. \]

L’informazione (l’inverso della varianza) si somma: la stima fusa è sempre migliore della migliore delle due. Ed è esattamente quello che fa il filtro di Kalman, componente per componente: per la distanza pesa soprattutto il radar, per la posizione laterale soprattutto la camera (le formule della gaussiana danno lo stesso risultato con il teorema di Bayes).

Le misure dei due sensori

La camera dà z_c = [x,\ y] con covarianza R_c = \mathrm{diag}(\sigma_x^2, \sigma_y^2) ricavata nella lezione 2: H_c prende la prima e la terza componente dello stato. Il radar misura in coordinate polari (r, \theta, \dot r); per usare un filtro lineare si converte la misura in coordinate cartesiane, x = r\cos\theta, y = r\sin\theta, e si propaga la covarianza con la matrice jacobiana:

    \[ J = \begin{bmatrix}\cos\theta & -r\sin\theta\\ \sin\theta & r\cos\theta\end{bmatrix},\qquad R_{xy} = J\,\mathrm{diag}(\sigma_r^2, \sigma_\theta^2)\,J^T. \]

Per un bersaglio davanti (\theta \approx 0) l’ellisse d’errore è stretta in x (\sigma_r) e larga in y (r\sigma_\theta). La velocità radiale \dot r \approx v_x è la terza misura. I sensori non sono sincronizzati (radar a 20 Hz, camera a 15 Hz): il filtro prevede ogni oggetto fino all’istante della misura e la usa subito, in ordine di tempo.

Associare le misure agli oggetti

Prima di aggiornare bisogna sapere quale misura appartiene a quale oggetto.

Finestre di validazione e associazione
Ogni traccia (quadrato) ha una finestra di validazione, l’ellisse dove la sua prossima misura cade con probabilità 99%. Le misure (croci) dentro una finestra sono candidate per quella traccia; quella fuori da tutte (rossa) fa nascere una nuova traccia.
  • finestra di validazione (gating): la distanza di Mahalanobis d^2 = \nu^T S^{-1}\nu di una misura corretta segue una distribuzione \chi^2 con tanti gradi di libertà quante le componenti della misura. Accettiamo la misura se d^2 \mathchar"313C  \gamma, con \gamma = 9.21 (2 gradi, camera) o 11.34 (3 gradi, radar) per il 99%. A differenza della distanza euclidea, la finestra si allarga dove la traccia è incerta;
  • associazione globale (Global Nearest Neighbour): tra tutte le assegnazioni possibili (ogni traccia al massimo una misura, ogni misura al massimo una traccia) si sceglie quella con più coppie e somma delle distanze minima. Con pochi oggetti si possono provare tutte; con molti l’algoritmo ungherese lo fa in O(n^3). Nel traffico denso o con molti falsi echi si usano metodi probabilistici (JPDA, MHT), visti nella lezione sul radar ad inseguimento.

Gestire le tracce

Una misura non associata fa nascere una traccia provvisoria; diventa confermata quando è stata associata in almeno 3 degli ultimi 5 cicli (logica M su N), e viene cancellata dopo 6 cicli senza misure. Solo le tracce confermate vanno alla selezione dell’obiettivo: un eco isolato (una riflessione, un insetto sul radome) non arriva mai al pianificatore.

def update(self, z, R, H):
    y = z - H @ self.s                   # innovation
    S = H @ self.P @ H.T + R             # innovation covariance
    K = self.P @ H.T @ np.linalg.inv(S)  # Kalman gain
    self.s = self.s + K @ y
    I_KH = np.eye(4) - K @ H
    self.P = I_KH @ self.P @ I_KH.T + K @ R @ K.T   # Joseph form
    return y, S

Un errore che rende la fusione peggiore di un sensore

Nella simulazione un’auto davanti, da 30 a 110 m, a un certo punto cambia corsia. Con la prima taratura, q_y = 0.5 m/s², il risultato era paradossale: errore laterale quadratico medio 0.32 m fondendo radar e camera, 0.11 m con la sola camera. La fusione era peggiore del sensore migliore.

La causa: con tante misure e un q_y piccolo il filtro diventava troppo sicuro, \sigma_y \approx 2 cm. Durante il cambio di corsia (un’accelerazione laterale vera di circa 1 m/s²) le misure della camera, corrette, finivano fuori dalla finestra di validazione di un filtro così sicuro di sé e venivano scartate: 36 su 222. Restava solo il radar, impreciso di lato, e la traccia inseguiva la manovra in ritardo.

Tracciamento con radar, camera e fusione
Con q_y = 1.5 m/s²: sopra, errore sulla distanza (radar e fusione 0.03 m, camera 1.1 m); sotto, la posizione laterale stimata durante il cambio di corsia tra 6 e 10 s. La fusione (verde) segue la manovra meglio di entrambi i sensori: errore 0.08 m contro 0.19 m del radar e 0.11 m della camera.

Con q_y = 1.5 m/s², un valore che copre una manovra reale, la fusione torna a fare il suo mestiere: 0.03 m in distanza (come il radar) e 0.08 m di lato (meglio della camera). La lezione generale: il rumore di processo non è un parametro di lisciatura, è un’affermazione su quanto si possono muovere gli oggetti; se è troppo piccolo il filtro scarta la realtà. Nei sistemi di serie si usano spesso più modelli insieme (IMM, Interacting Multiple Models): uno a velocità costante per i tratti tranquilli e uno di manovra, pesati in base a quale spiega meglio le misure.

Il tracciatore dà ora una lista di oggetti con stato e covarianza. Quale di questi seguire? È il problema della prossima lezione, ed è il punto in cui un ACC si guadagna la fiducia del guidatore o la perde.

Iscriviti
Notificami
guest
0 commenti
Vecchi
Più recenti Le più votate
0
Esprimete la vostra opinione commentando.x