Les étapes de l’algorithme

L’algorithme1présente les différentes étapes impliquées dans la phase d’évolution du filtre. Il se découpe en deux parties : le partitionnement des particules (instructions??à??) et l’ap-plication du modèle d’évolution sur celles-ci (instructions ?? à ??). Le partitionnement sert à créer une mixture de gaussiennes intermédiaire dans laquelle, les particules dont l’incerti-tude d’orientation était importante ont été divisées en plusieurs particules tout en conservant un

Tester

l’incertitude ^Prédire

Partitionner l’état valide

non valide pour chaque particule étape d’évolution estimation courante de l’état mesure d’odométrie nouvelle estimation a prioride l’état Tester la linéarité Mettre à jour

(modèle normal) ^Réduire

Partitionner la mesure Mettre à jour (modèle spécifique) valide non valide

pour chaque particule étape de mise à jour

estimation courante de l’état mesure de balise estimation a posteriori de l’état

Figure 4.4 – Schéma représentant les étapes impliquées dans le calcul d’une nouvelle estimation d’état par notre approche.

bon recouvrement de la densité de probabilité initiale. Cette mixture intermédiaire est ensuite utilisée pour générer la mixture représentant l’estimation a priori de l’état. Ce calcul est une adaptation de l’étape d’évolution du filtre à intersection de covariance partitionné (SCIF) pour une utilisation avec un état représenté par une mixture de gaussienne.

L’algorithme2correspond aux étapes de la phase de mise à jour du filtre. Il fonctionne pour les observations de balises mais également pour d’autres types d’observation qui ne concernent que des informations propres au robot. Cependant le partitionnement d’une particule (instruc-tion ??) est spécifique aux mesures de distance avec une balise. Il survient lorsque le test de non-linéarité (instructions ?? à ??) entre la mesure et une particule de l’état indique que la linéarisation est mauvaise. Ce test correspond à l’utilisation d’un critère permettant de quanti-fier l’écart entre l’approximation calculée et la vraissemblance réelle de la particule. De façon générale, il est possible d’utiliser la divergence de Cullback-Leibler comme critère de compa-raison, mais il est rarement possible de trouver une forme analytique adéquate. Nous verrons dans la section 4.5.1le critère que nous avons développé spécifiquement pour les observations de balises.

S’il n’y a pas de problème de linéarité, la mesure est fusionnée à la particule en cours de traitement en utilisant la procédure d’une mise à jour d’un SCIF avec le modèle d’observation présenté dans la section section4.2.2ainsi qu’un calcul de pondération. Le nombre de particules de l’estimation de l’état peut fortement augmenter après le partitionnement de l’étape de mise

Entrée : L’ensemble I des particules de l’estimation précédente n (wⁱ_k−1|k−1, xi k−1|k−1, Pi k−1|k−1)^o i∈I

Entrée : La mesure d’évolution

Sortie : L’ensemble O des particules de l’estimation de l’état a priori n

(wⁱ_k|k−1, xi k|k−1, Pi

k|k−1)^o

i∈O

Créer un ensemble de particules T vide. pour chaque particule i de l’ensemble I

si l’incertitude d’orientation de la particule i est trop importante alors Partitionner la particule i en un nouvel ensemble de particules Ajouter les particules obtenues à l’ensemble T

sinon

Ajouter la particule i à l’ensemble T fin

fin

Créer un ensemble de particules O vide. pour chaque particule i de l’ensemble T

Calculer une nouvelle particule grâce à la particule i, la mesure d’évolution et les formules d’évolution du SCIF

Ajouter cette nouvelle particule à l’ensemble O fin

Algorithme 1 : Étape d’évolution

à jour. Il est cependant possible de réduire ce nombre lorsque les particules sont dans certaines configurations particulières :

— si le poids d’une particule est très faible, elle n’a alors que peu d’influence dans la mixture de gaussiennes. Ces particules sont donc supprimées (instruction??).

— si des particules sont très proches les unes des autres, il est alors possible de les fusionner afin d’éviter les calculs similaires. Pour cela, nous appliquons un algorithme de réduction de mixture de gaussiennes (instruction??).

Les deux algorithmes présentés ici regroupent l’essentiel des étapes impliquées dans le filtre développé. Lorsqu’une nouvelle mesure sera disponible, le choix de l’algorithme utilisé va dé-pendre du type de mesure. Les mesures d’odométries seront traitées avec l’algorithme 1et les autres mesures seront gérées dans l’algorithme2. Cependant, les temps de traitements des don-nées ne peuvent être négligés dans les approches de localisation coopérative à cause du temps de latence des outils de communication sans fil et du déplacement des robots.

4.4 Calcul de l’évolution

L’objectif de l’algorithme d’évolution est de fournir une estimation a priori de l’état du robot en se servant d’un modèle d’évolution des robots. Puisque la plupart des robots ne sont

Entrée : L’ensemble I des particules de l’estimation a prioriⁿ(wⁱ_k|k−1, xi k|k−1, Pi

k|k−1)^o

i∈I

Entrée : La mesure (observation de balise ou autre)

Sortie : L’ensemble O des particules de l’estimation a posterioriⁿ(wi k|k, xi

k|k, Pi k|k)^o

i∈O

Créer un ensemble de particules O vide. pour chaque particule i de l’ensemble I

si c’est une mesure de balise et la non-linéarité de la mesure sur la particule i est trop importante alors

Générer un ensemble de particules M correspondant à la conversion de la mesure de balise dans le plan 2D.

Calculer un nouvel ensemble de particules représentant la fusion entre la particule i et chaque particule de M.

Ajouter les particules obtenues à l’ensemble O sinon

Appliquer l’étape de mise à jour du SCIF sur la particule i

Calculer le nouvel état de la particule i grâce à la mesure et les formules d’observation du SCIF.

Ajouter cette nouvelle particule à l’ensemble O fin

fin

Supprimer les particules de l’ensemble O dont le poids est négligeable Appliquer l’algorithme de réduction de mixture sur l’ensenble O Normaliser le poids des particules de l’ensemble O

Algorithme 2 : Étape de mise à jour

pas holonomes, l’évolution de leur position est fortement contrainte par leur orientation. Le problème des modèles de direction courants des robots mobiles, comme le modèle différentiel ou encore le modèle Ackermann, est qu’ils ne sont pas linéaires par rapport à l’orientation. De plus, cette erreur se cumule avec le temps. Cela signifie que lorsque l’incertitude d’orientation est importante, une modélisation gaussienne ne sera pas une bonne approximation de l’état du robot. C’est pourquoi notre solution met en place une étape de partitionnement de la mixture de gaussiennes représentant l’état afin d’assurer une meilleure représentation de la densité de probabilité de l’état d’un robot lorsqu’il effectue un long trajet sans observation externe.