Dagli scan points agli oggetti: velocità propria con RANSAC, raggruppamento DBSCAN, forma e velocità degli oggetti

Dopo la rivelazione, un radar moderno non restituisce “un bersaglio”, ma una nuvola di punti: decine o centinaia di rivelazioni per scansione, ciascuna con distanza, angolo, velocità radiale e intensità. Un’automobile ne produce più d’una (paraurti, ruote, specchietti, targa), un guardrail ne produce decine, il rumore qualcuna sparsa. Per un sistema di guida assistita servono invece oggetti: “un’auto a 40 m nella mia corsia, lunga 4,5 m, che va a 15 m/s”. Questa lezione descrive la catena che trasforma i punti in oggetti, come avviene nei radar delle automobili e, in forma simile, nei radar di sorveglianza: separazione dei punti fermi da quelli in movimento con la velocità propria, raggruppamento con DBSCAN, stima di forma e velocità completa, inseguimento e classificazione. Una simulazione di una scena stradale percorre tutti i passi.

L’idea in parole semplici

Il radar di un’auto non “vede” automobili: vede tanti puntini luminosi, come le stelle in un cielo notturno. Ogni puntino è un pezzo di metallo che ha riflesso l’onda: un paraurti, una ruota, un palo, un tratto di guardrail. Il compito di questa lezione è lo stesso di chi guarda il cielo e riconosce le costellazioni: raggruppare i puntini che stanno insieme e capire che cosa formano.

Il radar ha però un aiuto che le stelle non danno: per ogni punto conosce la velocità con cui si avvicina o si allontana. Questo permette tre trucchi. Primo, gli oggetti fermi (strada, guardrail, alberi) si riconoscono subito, perché si muovono tutti in modo coerente con la nostra velocità. Secondo, due oggetti vicini ma con velocità diverse non si confondono. Terzo, combinando le velocità radiali di più punti dello stesso oggetto, visti da angoli un po’ diversi, si ricava la velocità completa dell’oggetto, anche quella di traverso, che una singola misura Doppler non vede.

I punti di scansione

Per ogni rivelazione il radar fornisce:

• distanza e azimut (e nei radar “4D” anche l’elevazione), da cui la posizione , nel riferimento del veicolo;
• velocità radiale , dal Doppler: la componente della velocità relativa lungo la linea di vista. È la grandezza più preziosa, misurata direttamente e con precisione di qualche centimetro al secondo;
• intensità (o sezione radar stimata), che aiuta a distinguere un camion da un pedone.

La precisione angolare è il punto debole (qualche decimo di grado nel migliore dei casi, cioè decine di centimetri a 50 m), quella in distanza e in velocità radiale è ottima.

1. Fermo o in movimento? La velocità propria

Un oggetto fermo, visto da un veicolo che avanza a , ha velocità relativa , quindi velocità radiale

Tutti i punti fermi della scena (guardrail, cartelli, auto parcheggiate, il terreno) stanno su questa curva nel piano ; i punti in movimento stanno fuori. Si può anche fare il contrario: stimare la velocità del proprio veicolo dal radar, cercando la curva che passa per il maggior numero di punti. Poiché i punti in movimento sono “intrusi” che rovinerebbero una semplice regressione, si usa un metodo robusto come RANSAC: si prende un punto a caso, si calcola la che lo renderebbe fermo, si contano i punti coerenti con quella ipotesi, si ripete molte volte e si tiene l’ipotesi con più consensi; infine si rifà la stima ai minimi quadrati sui soli punti coerenti. È una stima indipendente dai sensori delle ruote, utile quando le ruote slittano, e permette anche di verificare la taratura del montaggio del radar.

2. Il raggruppamento: DBSCAN

I punti in movimento vanno raggruppati per oggetto. Non si sa quanti oggetti ci siano, e ci sono punti di rumore isolati che non appartengono a nessuno: l’algoritmo più usato è DBSCAN (density-based spatial clustering, Ester e altri, 1996). Due parametri: un raggio e un numero minimo di punti .

• Un punto è centrale se entro ha almeno punti (se stesso compreso).
• Un gruppo è formato da punti centrali raggiungibili l’uno dall’altro con passi di lunghezza al più , più i punti di bordo vicini a un punto centrale.
• I punti che non sono né centrali né di bordo sono rumore.

DBSCAN trova gruppi di forma qualunque, non chiede il numero di gruppi e scarta da solo i punti isolati. Nel radar conviene raggruppare in uno spazio che contiene anche la velocità radiale (con una scala, per esempio 2 m/s “valgono” 1 m): due auto affiancate alla stessa distanza ma con velocità diverse restano separate, come nella mappa distanza-velocità della lezione sul radar FMCW. La scelta di dipende dalla distanza (a distanze maggiori i punti di uno stesso oggetto sono più sparsi in angolo), e nei sistemi reali si adatta.

3. Dall’insieme di punti all’oggetto

Per ogni gruppo si stimano:

• posizione: il baricentro dei punti, o meglio il punto più vicino (il retro del veicolo), che è quello che conta per la distanza di sicurezza;
• orientamento e dimensioni: la direzione principale della nuvola di punti con l’analisi delle componenti principali (l’autovettore della matrice di covarianza con autovalore maggiore), e lunghezza e larghezza come estensione dei punti lungo i due assi. Nella pratica si usano anche modelli a “L” (due lati visibili di un rettangolo), perché il radar vede solo i lati rivolti verso di sé;
• velocità completa. Un punto misura solo la componente radiale, ma i punti di uno stesso oggetto rigido hanno tutti la stessa velocità e la vedono sotto angoli leggermente diversi: . Con almeno due punti si risolve ai minimi quadrati.

Il limite della velocità laterale. Quel sistema è bene condizionato solo se gli angoli sono abbastanza diversi. Un’auto a 40 m, larga meno di due metri, si vede sotto pochi gradi: la componente lungo la linea di vista () è determinata benissimo, quella di traverso () male. Con un errore di 0,1 m/s su ogni velocità radiale, l’incertezza sulla velocità laterale è , che cresce rapidamente al diminuire dell’apertura angolare. La velocità laterale si ricava meglio nel tempo, inseguendo la posizione dell’oggetto con il filtro di Kalman (lezione sull’inseguimento), o combinando più radar che lo vedono da direzioni diverse.

La simulazione di una scena stradale

Il nostro veicolo va a 20 m/s. La scena: un guardrail fermo a destra (38 punti), due auto parcheggiate a sinistra (12 punti), un’auto davanti nella nostra corsia a 40 m che va a 15 m/s (12 punti), un’auto che si immette in diagonale da sinistra a 30 m con velocità (4; −4) m/s (10 punti), e 8 falsi allarmi sparsi. Rumore di misura: 10 cm in distanza, 0,5° in angolo, 0,1 m/s in velocità radiale.

• Velocità propria stimata dal radar con RANSAC: 20,03 m/s; punti classificati fermi 50 (esattamente guardrail e auto parcheggiate), in movimento 30.
• DBSCAN sui 30 punti in movimento: 2 gruppi, e gli 8 falsi allarmi scartati come rumore.
• Oggetti:

I valori veri erano 4,5 × 1,8 m, orientamenti 180° e 135°, velocità (15; 0) e (4; −4) m/s. Posizione, forma, orientamento e velocità longitudinale sono ottimi; la velocità laterale ha errori dell’ordine della sua incertezza (1,75 m/s per l’auto lontana vista sotto 2,7°), esattamente come previsto: in un solo istante il radar non la misura bene, l’inseguimento sì. Le dimensioni sono un po’ sottostimate perché i punti non coprono tutto il veicolo.

4. Inseguimento e classificazione

• Inseguimento: gli oggetti di ogni scansione vengono associati alle tracce esistenti e filtrati (Kalman, IMM, lezione sull’inseguimento). Il modello di moto per un veicolo è spesso quello a velocità e sterzata costanti (il modello cinematico a bicicletta della lezione sulla dinamica del veicolo), che vincola la direzione del moto all’orientamento del veicolo. Con l’inseguimento migliorano la velocità laterale, le dimensioni (accumulando punti su più scansioni) e la robustezza ai punti mancanti.
• Classificazione: auto, camion, moto, bicicletta, pedone. Indizi: dimensioni, intensità dell’eco, e soprattutto il micro-Doppler: le parti che si muovono rispetto al corpo (gambe e braccia di un pedone, pedali e ruote di una bicicletta, ruote di un’auto) aggiungono intorno alla velocità del corpo uno “spettro” di velocità caratteristico. Un pedone che cammina mostra nel tempo un’oscillazione periodica di circa 1-2 Hz delle velocità delle gambe, che una rete neurale riconosce bene.
• Reti neurali sulla nuvola di punti: architetture come PointNet, o reti convoluzionali sulla mappa distanza-velocità-angolo (prima della rivelazione, senza perdere informazione), sostituiscono sempre più spesso la catena di regole. Rimane però essenziale saperla costruire e verificare passo per passo: è quella che si usa come riferimento, per generare dati di addestramento e per i sistemi che devono essere certificati.
• Fusione dei sensori: gli oggetti del radar vengono fusi con quelli della telecamera (migliore in angolo e nella classificazione) e del LiDAR (geometria precisa): ogni sensore compensa i punti deboli degli altri.

In sintesi

• Il radar produce una nuvola di punti con distanza, angolo, velocità radiale e intensità; servono oggetti.
• I punti fermi soddisfano : con RANSAC si stima la velocità propria e si separano i punti in movimento.
• DBSCAN raggruppa i punti vicini (anche in velocità) senza sapere quanti oggetti ci sono e scarta i punti isolati.
• Da ogni gruppo si ricavano centro, dimensioni e orientamento (PCA) e la velocità completa (minimi quadrati sulle velocità radiali); la componente trasversale è tanto più incerta quanto più l’oggetto è piccolo in angolo.
• Inseguimento (Kalman, IMM) e classificazione (micro-Doppler, reti neurali) completano la catena; la fusione con telecamere e lidar dà l’immagine finale.

Il codice

import numpy as np

rng = np.random.default_rng(1)
v_ego = 20.0                                          # la nostra auto va avanti a 20 m/s lungo x

# ---------------- la scena (coordinate del veicolo: x avanti, y a sinistra) ----------------
punti = []                                            # (x, y, vx, vy) di ogni punto riflettente, velocità assolute
for x in np.arange(5, 80, 2.0):                       # guardrail fermo a destra
    punti.append((x, -4.0, 0, 0))
for cx in (25.0, 48.0):                               # auto parcheggiate a sinistra
    for _ in range(6):
        punti.append((cx + rng.uniform(-2, 2), 5.0 + rng.uniform(-0.8, 0.8), 0, 0))
def veicolo(cx, cy, vx, vy, lung, larg, n):
    th = np.arctan2(vy, vx)
    for _ in range(n):
        a, b = rng.uniform(-lung / 2, lung / 2), rng.uniform(-larg / 2, larg / 2)
        punti.append((cx + a * np.cos(th) - b * np.sin(th), cy + a * np.sin(th) + b * np.cos(th), vx, vy))
veicolo(40.0, 0.0, 15.0, 0.0, 4.5, 1.8, 12)           # auto davanti, nella nostra corsia, più lenta: 15 m/s
veicolo(30.0, 8.0, 4.0, -4.0, 4.5, 1.8, 10)           # auto che si immette in diagonale da sinistra
P = np.array(punti)

# ---------------- che cosa misura il radar: distanza, angolo, velocità radiale ----------------
R = np.hypot(P[:, 0], P[:, 1]); az = np.arctan2(P[:, 1], P[:, 0])
vrel = P[:, 2:4] - [v_ego, 0]                         # velocità relativa al nostro veicolo
vr = (vrel[:, 0] * P[:, 0] + vrel[:, 1] * P[:, 1]) / R   # componente radiale (positiva se si allontana)
R += rng.normal(0, 0.1, len(R)); az += rng.normal(0, np.radians(0.5), len(az)); vr += rng.normal(0, 0.1, len(vr))
fx = np.c_[rng.uniform(5, 80, 8), rng.uniform(-10, 10, 8)]   # falsi allarmi sparsi
R = np.r_[R, np.hypot(fx[:, 0], fx[:, 1])]; az = np.r_[az, np.arctan2(fx[:, 1], fx[:, 0])]; vr = np.r_[vr, rng.uniform(-25, 5, 8)]
x, y = R * np.cos(az), R * np.sin(az)
print(f"{len(R)} punti di scansione")

# ---------------- 1) velocità propria dai punti fermi: per un punto fermo vr = -v_ego cos(az) ----------------
migliore = 0
for _ in range(200):                                  # RANSAC: ipotesi da un punto, conteggio dei punti coerenti
    i = rng.integers(len(vr))
    v_ip = -vr[i] / np.cos(az[i])
    coerenti = np.abs(vr + v_ip * np.cos(az)) < 0.3
    if coerenti.sum() > migliore:
        migliore, fermi = coerenti.sum(), coerenti
v_stima = -np.sum(vr[fermi] * np.cos(az[fermi])) / np.sum(np.cos(az[fermi]) ** 2)   # minimi quadrati sui coerenti
print(f"velocità propria stimata dal radar: {v_stima:.2f} m/s (vera {v_ego}); punti fermi {fermi.sum()}, in movimento {(~fermi).sum()}")

# ---------------- 2) raggruppamento DBSCAN dei punti in movimento ----------------
def dbscan(Xf, eps, min_pts):
    n = len(Xf); etichetta = np.full(n, -1); c = 0
    vicini = [np.where(np.linalg.norm(Xf - Xf[i], axis=1) <= eps)[0] for i in range(n)]
    for i in range(n):
        if etichetta[i] != -1 or len(vicini[i]) < min_pts:
            continue
        etichetta[i] = c; coda = list(vicini[i])
        while coda:                                   # si espande il gruppo dai punti "centrali"
            j = coda.pop()
            if etichetta[j] == -1:
                etichetta[j] = c
                if len(vicini[j]) >= min_pts:
                    coda.extend(vicini[j])
        c += 1
    return etichetta
mob = ~fermi
Xf = np.c_[x[mob], y[mob], vr[mob] / 2.0]             # la velocità radiale conta come una coordinata (2 m/s = 1 m)
lab = dbscan(Xf, eps=2.5, min_pts=3)
print(f"gruppi trovati: {lab.max() + 1}; punti scartati come rumore: {(lab == -1).sum()}")

# ---------------- 3) ogni gruppo diventa un oggetto: posizione, dimensioni, orientamento, velocità ----------------
for g in range(lab.max() + 1):
    sel = np.where(mob)[0][lab == g]
    pts = np.c_[x[sel], y[sel]]
    centro = pts.mean(0)
    autoval, autovet = np.linalg.eigh(np.cov(pts.T))  # direzione principale dei punti (PCA)
    asse = autovet[:, -1]
    proiez = (pts - centro) @ np.c_[asse, [-asse[1], asse[0]]]
    lung, larg = np.ptp(proiez[:, 0]), np.ptp(proiez[:, 1])
    # velocità completa: ogni punto vede vr = vx cos(az) + vy sin(az) (relativa); minimi quadrati
    A = np.c_[np.cos(az[sel]), np.sin(az[sel])]
    v_rel = np.linalg.lstsq(A, vr[sel], rcond=None)[0]
    v_ass = v_rel + [v_stima, 0]
    incert = 0.1 * np.sqrt(np.diag(np.linalg.inv(A.T @ A)))   # deviazione standard delle due componenti (0,1 m/s per punto)
    print(f"oggetto {g}: {len(sel)} punti, centro ({centro[0]:5.1f}, {centro[1]:5.1f}) m, {lung:.1f} x {larg:.1f} m, "
          f"orientamento {np.degrees(np.arctan2(asse[1], asse[0])) % 180:5.1f} gradi, velocità ({v_ass[0]:5.1f}, {v_ass[1]:5.1f}) m/s "
          f"+- ({incert[0]:.2f}, {incert[1]:.2f}), apertura angolare {np.degrees(np.ptp(az[sel])):.1f} gradi")

L’esecuzione stampa 80 punti, la velocità propria 20,03 m/s con 50 punti fermi e 30 in movimento, 2 gruppi e 8 punti di rumore, e le due righe della tabella.