Aller au contenu
ELECTRONICS ENGINEERINGelectronics-engineering.com
Automobile

ACC 3 : fusion radar et caméra avec le filtre de Kalman

Fusion centralisée et piste à piste, modèle du mouvement relatif, gain de Kalman démontré, pourquoi deux capteurs valent mieux qu'un, fenêtres de validation, association, gestion des pistes et une erreur de réglage.

https://www.electronics-engineering.com/fr/acc-fusion-radar-camera-filtre-de-kalman-association-pistes/

Pas encore de vote : soyez le premier à noter cette page.

Radar et caméra envoient, chacun à son rythme, des listes de mesures bruitées, parfois manquantes, parfois fausses. La fusion les transforme en une liste d’objets pistés : pour chacun une estimation de la position et de la vitesse avec son incertitude, mise à jour à chaque cycle. L’outil est le filtre de Kalman, que nous avons présenté dans la leçon sur le filtre de Kalman et retrouvé parmi les observateurs discrets ; ici nous l’appliquons à plusieurs capteurs, nous ajoutons l’association des mesures aux objets et la gestion des pistes, et nous voyons une erreur de réglage qui rend la fusion pire qu’un seul capteur.

Deux architectures

Fusion centralisée et fusion piste à piste
A : fusion centralisée, chaque mesure du radar et de la caméra met à jour le même filtre. B : chaque capteur a son propre pisteur et les pistes sont fusionnées ensuite (T2T), avec l’intersection des covariances (CI) parce que les erreurs des deux pistes ne sont pas indépendantes.
  • centralisée (A) : la plus précise, car le filtre utilise chaque mesure avec sa vraie incertitude ; elle demande des mesures brutes ou presque, horodatées ;
  • piste à piste (B) : chaque calculateur capteur fait déjà son propre pistage (fréquent avec des capteurs de fournisseurs différents) et le calculateur ADAS fusionne les pistes. Deux pistes du même objet ont des erreurs corrélées (elles partagent le même modèle de mouvement), et les traiter comme indépendantes rendrait l’estimation trop optimiste. L’intersection des covariances est sûre même sans connaître la corrélation : 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), avec \omega \in [0,1] choisi pour minimiser la trace de P.

Dans le code nous utilisons la fusion centralisée.

Le modèle du mouvement relatif

L’état de chaque objet est relatif à notre voiture : \mathbf{s} = [x,\ v_x,\ y,\ v_y]^T. Longitudinalement l’accélération relative est \ddot x = a_t - a_{ego} : celle de la cible a_t est inconnue et nous la traitons comme un bruit, la nôtre a_{ego} est connue et nous l’utilisons comme entrée. En intégrant sur un pas T à accélération constante (voir la cinématique du mouvement rectiligne) :

    \[ 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, \]

et de même latéralement, sans entrée. Sous forme matricielle \mathbf{s}_{k+1} = F\mathbf{s}_k + B u + G\mathbf{w} avec

    \[ 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 et q_y sont les écarts types des accélérations que le modèle ne connaît pas : avec quelle force la cible peut freiner ou accélérer, à quelle vitesse elle peut se déplacer latéralement. Leur valeur compte énormément, comme nous allons le voir.

Le filtre de Kalman

Filtre de Kalman en schéma-blocs
Le filtre en schéma-blocs : l’estimation précédente passe par le retard z^{-1} et le modèle F (avec l’entrée u = -a_{ego}) et devient la prédiction \hat x_{k|k-1} ; la mesure z_k moins la mesure prédite H\hat x_{k|k-1} est l’innovation \nu, qui multipliée par le gain K_k corrige la prédiction.

À chaque mesure le filtre fait deux pas :

  • prédiction jusqu’à l’instant de la mesure : \hat{\mathbf{s}} \leftarrow F\hat{\mathbf{s}} + Bu, P \leftarrow F P F^T + Q ;
  • mise à jour : innovation \nu = z - H\hat{\mathbf{s}}, sa covariance S = H P H^T + R, gain K = P H^T S^{-1}, puis \hat{\mathbf{s}} \leftarrow \hat{\mathbf{s}} + K\nu, P \leftarrow (I - KH)P(I-KH)^T + K R K^T.

Pourquoi ce gain. Après la correction l’erreur est e^+ = (I - KH)e^- - K v, avec e^- l’erreur de prédiction et v le bruit de mesure, indépendants. Sa covariance est

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

(c’est la forme de Joseph, que nous utilisons aussi dans le code car elle reste symétrique et positive avec les nombres à virgule flottante). Nous cherchons le K qui minimise la somme des variances, la trace de P^+. En développant et en dérivant par rapport à K, avec \partial\,\mathrm{tr}(KAK^T)/\partial K = 2KA pour A symétrique :

    \[ \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}. \]

Pourquoi deux capteurs valent mieux qu’un

Le cas le plus simple dit tout. Deux estimations indépendantes de la même grandeur, x_1 de variance \sigma_1^2 et x_2 de variance \sigma_2^2 : leur moyenne pondérée \hat x = w x_1 + (1-w) x_2 a pour variance w^2\sigma_1^2 + (1-w)^2\sigma_2^2, minimale pour w = \sigma_2^2/(\sigma_1^2+\sigma_2^2). En remplaçant :

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

L’information (l’inverse de la variance) s’additionne : l’estimation fusionnée est toujours meilleure que la meilleure des deux. Et c’est exactement ce que fait le filtre de Kalman, composante par composante : pour la distance il pondère surtout le radar, pour la position latérale surtout la caméra (les formules de la gaussienne donnent le même résultat avec le théorème de Bayes).

Les mesures des deux capteurs

La caméra donne z_c = [x,\ y] avec la covariance R_c = \mathrm{diag}(\sigma_x^2, \sigma_y^2) obtenue dans la leçon 2 : H_c prend la première et la troisième composante de l’état. Le radar mesure en coordonnées polaires (r, \theta, \dot r) ; pour utiliser un filtre linéaire on convertit la mesure en coordonnées cartésiennes, x = r\cos\theta, y = r\sin\theta, et on propage la covariance avec la matrice jacobienne :

    \[ 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. \]

Pour une cible devant (\theta \approx 0) l’ellipse d’erreur est étroite en x (\sigma_r) et large en y (r\sigma_\theta). La vitesse radiale \dot r \approx v_x est la troisième mesure. Les capteurs ne sont pas synchronisés (radar à 20 Hz, caméra à 15 Hz) : le filtre prédit chaque objet jusqu’à l’instant de la mesure et l’utilise aussitôt, dans l’ordre chronologique.

Associer les mesures aux objets

Avant la mise à jour il faut savoir quelle mesure appartient à quel objet.

Fenêtres de validation et association
Chaque piste (carré) a une fenêtre de validation, l’ellipse où sa prochaine mesure tombe avec une probabilité de 99 %. Les mesures (croix) dans une fenêtre sont candidates pour cette piste ; celle qui est hors de toutes les fenêtres (rouge) crée une nouvelle piste.
  • fenêtre de validation (gating) : la distance de Mahalanobis d^2 = \nu^T S^{-1}\nu d’une mesure correcte suit une loi du \chi^2 avec autant de degrés de liberté que de composantes de la mesure. Nous acceptons la mesure si d^2 \mathchar"313C  \gamma, avec \gamma = 9.21 (2 degrés, caméra) ou 11,34 (3 degrés, radar) pour 99 %. Contrairement à la distance euclidienne, la fenêtre s’élargit là où la piste est incertaine ;
  • association globale (Global Nearest Neighbour) : parmi toutes les affectations possibles (chaque piste au plus une mesure, chaque mesure au plus une piste) on choisit celle qui a le plus de paires et la plus petite somme des distances. Avec peu d’objets on peut toutes les essayer ; avec beaucoup l’algorithme hongrois le fait en O(n^3). Dans un trafic dense ou avec beaucoup de faux échos on utilise des méthodes probabilistes (JPDA, MHT), vues dans la leçon sur la poursuite radar.

Gérer les pistes

Une mesure non associée crée une piste provisoire ; elle devient confirmée quand elle a été associée dans au moins 3 des 5 derniers cycles (logique M sur N), et elle est supprimée après 6 cycles sans mesure. Seules les pistes confirmées vont à la sélection de la cible : un écho isolé (une réflexion, un insecte sur le radôme) n’atteint jamais le planificateur.

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

Une erreur qui rend la fusion pire qu’un capteur

Dans la simulation une voiture devant, de 30 à 110 m, change de voie à un moment donné. Avec le premier réglage, q_y = 0.5 m/s², le résultat était paradoxal : erreur latérale quadratique moyenne de 0,32 m en fusionnant radar et caméra, 0,11 m avec la caméra seule. La fusion était pire que le meilleur capteur.

La cause : avec beaucoup de mesures et un q_y petit le filtre devenait trop sûr de lui, \sigma_y \approx 2 cm. Pendant le changement de voie (une vraie accélération latérale d’environ 1 m/s²) les mesures de la caméra, correctes, tombaient hors de la fenêtre de validation d’un filtre aussi sûr de lui et étaient rejetées : 36 sur 222. Il ne restait que le radar, imprécis latéralement, et la piste suivait la manœuvre en retard.

Pistage avec radar, caméra et fusion
Avec q_y = 1.5 m/s² : en haut, l’erreur sur la distance (radar et fusion 0,03 m, caméra 1,1 m) ; en bas, la position latérale estimée pendant le changement de voie entre 6 et 10 s. La fusion (vert) suit la manœuvre mieux que les deux capteurs : erreur de 0,08 m contre 0,19 m pour le radar et 0,11 m pour la caméra.

Avec q_y = 1.5 m/s², une valeur qui couvre une vraie manœuvre, la fusion refait son travail : 0,03 m en distance (comme le radar) et 0,08 m latéralement (mieux que la caméra). La leçon générale : le bruit de processus n’est pas un paramètre de lissage, c’est une affirmation sur la façon dont les objets peuvent bouger ; s’il est trop petit le filtre rejette la réalité. Les systèmes de série utilisent souvent plusieurs modèles ensemble (IMM, Interacting Multiple Models) : un à vitesse constante pour les passages tranquilles et un de manœuvre, pondérés selon celui qui explique le mieux les mesures.

Le pisteur donne maintenant une liste d’objets avec leur état et leur covariance. Lequel suivre ? C’est le problème de la prochaine leçon, et c’est là qu’un ACC gagne la confiance du conducteur ou la perd.

S’abonner
Notifier de
guest
0 commentaires
Le plus ancien
Plus récent Les plus votés
0
Vous avez apprécié cet article ? Merci de laisser un commentaire.x