Technique d’appariement de points 2D

Cette section décrit la technique d’appariement de points 2D qui sera à la base du modèle d’observation utilisé dans le filtrage de Kalman de la section suivante. Elle comprend le problème de registration et de recalage géométrique.

8.3.1 Registration

La registration de points 2D établit la correspondance entre l’information mesurée et représen- tée. L’information mesurée est liée aux points d’occupation réels ˜P_{= { ˜p}i, i = 1, . . . , N}, lesquels sont exprimés dans le repère de la carte en connaissance de la pose prédite de la plate-forme. La technique des plus proches voisins est utilisée : chaque point réel ˜pi est couplé avec le point de l’environnement virtuel ˜qide distance minimale.

8.3.2 Recalage géométrique de points 2D

Définition du problème

Le recalage géométrique de points 2D consiste à trouver les matrices de transformation homogène liant deux ensembles de points.

L’ensemble P_{= {p}i, i = 1, . . ., N}, dit réel, comprend les positions des points réellement perçus dans l’environnement, lesquelles sont relative à la pose estimée de la plate-forme. L’ensemble Q= {qi, i = 1, . . . , N}, dit virtuel, comprend les mêmes points physiques que ceux de l’ensemble P, mais sur la grille d’occupation. Comme l’erreur sur l’estimée de la pose rend l’ensemble P décalé de l’ensemble, Q. chaque point qipeut être obtenu en appliquant une transformation (T, R) sur le

point correspondant pi, où T est un vecteur de translation et R une matrice de rotation :

qi= T + Rpi ∀i ∈ {1,...,N} (8.1)

Dans les contextes réels, les mesures sensorielles sont bruitées, ce qui produit l’ensemble de points ˜P _{= { ˜p}i, i = 1, . . . , N}, ne correspondant pas exactement aux points physiques détec- tés dans l’environnement. Par ailleurs, la correspondance entre les points extraits des mesures et ceux de la grille est inconnue, ce qui introduit des erreurs d’association produisant l’ensemble

˜

mesure sur pietδi est l’erreur de pairage affectant{pi, qi}, on peut écrire : ˜

pi = pi+δMi ∀i ∈ {1,...,N} (8.2)

˜

qi = qi+δCi ∀i ∈ {1,...,N} (8.3)

En insérant ces équations dans (8.1), l’expression de l’erreur de pairageδC_i est obtenue : δC

i = −T − R ˜pi−δMi
+ ˜qi ∀i ∈ {1,...,N} (8.4)

La figure 8.2 schématise les relations entre les points réels de P, leurs estimations bruités de ˜P, les points virtuels de Q et leurs estimations de ˜Q.

Calcul des matrices de transformation homogène

Le calcul des matrices de transformation homogène se fonde sur les hypothèses suivantes :

1. Le bruit de mesure et l’erreur de pairage sont considérés comme étant des processus gaussiens centrés zéro, de varianceσ2_M etσ_C2 respectivement :

δM

i −→ N(0,σ2M) δC

i −→ N(0,σC2)

2. Il n’existe pas de matrice_{{R,T } qui minimise davantage l’erreur quadratique moyenne que}

{R∗, T∗_{} ;}

3. L’espérance mathématique d’une variable aléatoire tend vers sa moyenne : χi= E [χi] = 1 N N

∑

Étape 1 : exprimer T∗en fonction de R∗ En insérant les expressions (8.2) et (8.3) dans (8.1), le vecteur de translation ˜tiest donné par :

˜ti = T∗+δCi − R∗δMi (8.5)

= ˜qi− R∗p˜i (8.6)

et son espérance mathématique par :

E[˜ti] = T∗

= q˜i− R∗p˜i (8.7)

Étape 2 : déduction de R∗ Partant de (8.5), l’expression de l’erreur de pairage δC_i est calculée comme une fonction de T∗et de R∗:

δC

i = ˜ti− T∗+ R∗δMi (8.8)

Comme R∗et T∗minimisent l’erreur quadratique moyenne :

J∗ = min Eh(δC_i)TδC_ii (8.9)

En insérant l’équation (8.8) dans (8.9), en remplaçant E[(δM

i )TδMi ] par σ2M et en notant que E[(δC

i )TR∗δMi ] =σ2M, puisqueδCi etδMi sont corrélés via (8.8), l’expression suivante est obtenue : J∗= E(˜ti− T∗)T(˜ti− T∗) −σ2M (8.10) Ce résultat montre que la variance du vecteur de translation ˜ti, que l’on note∆T2, correspond à la variance minimale de l’erreur de pairage additionnée au bruit de mesure :

∆T2

= E(˜ti− T∗)T(˜ti− T∗)

= Eh(δC_i)TδC_ii+σ2_M. (8.11) En insérant les expressions (8.5) et (8.7) dans (8.11), puis en utilisant l’angleφ∗ associé à la matrice de rotation R∗, l’expression de la fonction de coût peut être récrite comme suit :

J∗ = ˜qT i q˜i+ ˜pTi p˜i− ˜q T i q˜i− ˜p T i p˜i−σ2M (8.12)

−2cos(φ∗) ˜pixqix˜ + ˜piyqiy˜ − ˜pixq˜ix− ˜piyq˜iy

−2sin(φ∗) ˜piyq˜ix− ˜pixq˜iy− ˜pixq˜iy+ ˜piyq˜ix

où ˜pi= [ ˜pix p˜iy] et ˜qi= [ ˜qix q˜iy] .

En posant la dérivée égale à zéro, nous obtenons : φ∗ _{= atan2 ˜p}

ixq˜iy− ˜piyq˜ix− ˜pixq˜iy+ ˜piyq˜ix,

˜

pixq˜ix+ ˜piyq˜iy− ˜pixq˜ix− ˜piyq˜iy
. (8.13) La matrice de rotation optimale R∗est déduite directement de cette expression :

R∗= "

cos(φ∗_{) −sin(}φ∗)

sin(φ∗) cos(φ∗) #

Connaissant R∗, on peut calculer T∗via l’équation (8.7).

Algorithme

1. Évaluer les moyennes :

˜

pixq˜iy, p˜iyq˜ix, p˜ixq˜ix, p˜iyq˜iy, ˜

pix, p˜iy, q˜ix, q˜iy 2. Évaluer l’angleφ∗via l’équation (8.13) ;

3. Évaluer la matrice de rotation R∗(φ∗) ;

4. Évaluer le vecteur de translation T∗(8.7).

Complexité de l’algorithme de recalage géométrique

La première étape de l’algorithme implique des calculs de moyennes de complexité O(n). Les étapes 2,3 et 4 ne dépendent pas de la taille des données. Ainsi, la complexité générale du recalage géométrique est O(n).

Incertitude des matrices de transformation homogène

Cette sous-section vise l’obtention de l’incertitude∆T∗du vecteur de translation T∗ainsi que l’in- certitude∆φ∗de l’écart angulaireφ∗. Ces valeurs, qui fournissent une mesure pratique de la corré- lation entre deux ensembles de points, seront indispensables dans le modèle d’observation du filtre de Kalman.

Matrice de covariance du vecteur de translation La covariance du vecteur de translation,Ω2_T, est : Ω2 T = E h (˜ti− T∗) (˜ti− T∗)T i = " σ2 x σxσy σyσx σ2 y # , (8.14)

et correspond à la covariance réorientée du bruit de mesure additionnée à celle de l’erreur de pai- rage : Ω2 T = E h δC i(δCi)T i + R∗EδM_i (δM_i )T (R∗₎T_{. (8.15)}

Incertitude sur l’orientation Comme l’orientation optimaleφ∗minimise l’incertitude de la trans- lation∆T , cette orientation est d’autant plus plausible que l’amplitude de∆T est faible. Considérant que la valeur maximale de l’erreur angulaire est_±π, nous avons :

lim ∆T→0∆φ ∗ _{= 0 (8.16)} lim ∆T→∞∆φ ∗ _{= π (8.17)}

La formule empirique suivante fournit une estimation pratique de l’incertitude sur l’écart d’orien- tation :

∆φ∗₌ π∆T

∆T+∆Thigh

(8.18) où ∆Thigh est une erreur de translation impliquant nécessairement des erreurs de pairage appré- ciables.

Robustesse

Au cours des développements précédents, il était présumé que les erreurs de mesures et de pairage varient sous des lois normales de moyennes nulles.

En réalité, la présence d’éléments inconnus dans l’environnement engendrera des erreurs de correspondance de moyenne possiblement non nulle. Néanmoins, si la proportion des points erronés (issus d’objets inattendus) reste minoritaire, l’équation (8.4) fournira une bonne approximation de l’erreur de correspondance associée à chaque paire. Ayant réalisé une première estimation de T∗et R∗via (8.13) et (8.7), nous obtenons en négligeant le bruit de mesure :

˜ δC

i = −T∗− R∗p˜i+ ˜qi ∀i ∈ {1··· ,N}

influencer les valeurs de R et T . Partant de ce principe, chaque paire est pondérée par un poidsγi tel que :

γi(σci) = exp{−β(σci)2}, (8.19)

où(σc_i)2= ˜δc_{i∗ ˜}δc_i est le coefficient d’incertitude. Le paramètreβ, défini positif, peut être ajusté de manière empirique et détermine le taux de rejet des mauvaises paires. Cette expression permet une distribution flexible de leur pondération relative. Les valeurs de R∗et T∗peuvent ensuite être mise à jour en utilisant les moyennes pondérées suivantes dans les étapes 1 et 2 de l’algorithme(8.3.2) :

˜

pixq˜iyγi, p˜iyq˜ixγi, p˜ixq˜ixγi, p˜iyq˜iyγi, ˜

pixγi, p˜iyγi, q˜ixγi, q˜iyγi . 8.3.3 Sommaire de l’algorithme

1. L’ensemble ˜Q est obtenu sous le principe des plus proches voisins de P : ˜

qi= arg min

1≤ j≤Mdist ˜pi, ci j

où les points d’occupation entourant ˜pisont{ci j, j = 1, ..., M} et dist représente la distance euclidienne ;

2. Trouver R∗ et T∗par la méthode de recalage (8.3.2) ;

3. Pondérer chaque paire via (8.19), recalculer R∗ et T∗ avec les moyennes pondérées (voir section 8.3.2).

Complexité de l’algorithme Nous avons démontré à la sous-section (8.3.2) que le recalage géo- métrique est de complexité O(n). Comme chaque point additionnel implique M comparaisons sup- plémentaires, la complexité de l’étape 1 est directement proportionnelle au nombre de points, ce qui cause une complexité O_{(n · m). L’étape 3 nécessite un calcul de moyenne pour chaque point,} causant une complexité O(n). La complexité totale est donc O_{(n · m).}