|
PROJET ROBOTIQUE MOBILE LOCALISATION PAR FILTRAGE PARTICULAIRE |
Mercredi 21 Février 2007
L'objectif de ce projet a été pour nous de coder un filtre particulaire sur le simulateur PlayerStage. Nous voulions d'abord tester notre algorithme sur une plateforme tel qu'un simulateur afin de s'affranchir des contraintes du monde réel et de ses mesures bruitées. La simulation a été effectuée sur le simulateur PlayerStage avec un robot Pioneer doté d'une ceinture laser. Nous avons commencé par réaliser un tracking du robot sur le simulateur avec les particules, avant de réaliser la localisation globale sur la carte.
Une méthode de localisation par suivi continu implicite de plusieurs hypothèses.
C'est une des méthodes les plus efficaces pour la localisation.
Plusieurs hypothèses : Au lieu de s'appuyer sur la position estimée à l'instant t pour calculer la position à l'instant t + 1 (Filtre de Kalman par exemple), les méthodes multi-hypothèses ont la capacité de gérer un ensemble de telles positions. Chacune de ces positions fera office d'hypothèse dont la crédibilité va évoluer.
Suivi implicite : Par opposition au suivi explicite qui met à jour une liste donné d'hypothèse, cette méthode remplace les différentes hypothèses par une distribution de probabilité.
Continu : La carte n'est pas discrétisée.
Filtrage particulaire : On introduit des particules afin d'approcher au mieux la distribution de probabilité que nous cherchons à déterminer.
La position du robot dans son environnement est un vecteur, et sera notée X.
Dans notre cadre, qui est celui de l'environnement 2D de player Stage, ce vecteur est tridimensionnel : Abscisse x, Ordonnée y et orientation Thêta.
On estime X de la manière suivante :
X = Espérance [ Bel ( x ) ] avec Bel ( x ) le belief state : La probabilité d'être dans un état x sachant une suite d'actions et de perceptions .
Nos particules P(i) , nous l'avons vu, vont approcher cette loi Bel ( x ) . Nous avons alors, d'après la loi forte des grands nombres :
X = Espérance [ Bel ( x ) ]
~= ( 1 / NumParticules ) ( w(1)P(1) + w(2)P(2) + ... + w(NumParticules)P(NumParticules) )
Avec NumParticules grand et w(i) le poids associé à la particule P(i).
En utilisant la formule du filtrage Bayésien, le poids se calcule grâce au modèle de capteur :
w ( i ) = k * P ( y | P(i) ), k étant une constante de normalisation.
Pour ré-échantillonner les particules on procède en deux étapes : Tout d'abord, évolution de chaque particule P ( i ) en suivant le modèle proprioceptif ( à savoir p ( x | odométrie ) ) .
Ensuite, on effectue un tirage qui nous permettra, à chaque itération, d'être en accord avec la loi Bel(x) que nous voulons approcher. Nous utilisons le Stochastic Universal Sampling. Ce tirage va dupliquer les particules de poids élevé et éliminer les autres.
Ce processus, après un nombre variable d'itération converge vers une situation où la population de particules se concentre autour de la position réelle du robot. Cette concentration sera d'autant plus fine que le capteur est précis et l'odométrie fiable.
La convergence dépendra de la taille de la carte, du perceptual aliasing présent, des facteurs dynamiques éventuellement mis en jeu et du nombre de particules initial. Cette convergence n'est pas assurée à priori. Nous verrons dans notre cas comment les chances de convergences sont augmentées en jouant sur certains paramètres.
Dans cette partie nous verrons le passage de la théorie à la pratique et l'implémentation du filtre particulaire que nous avons réalisé en C++ interfacé avec PlayerStage.
La carte est fournie au programme et est stockée dans la classe Map. Il faut donc tout d'abord créer une carte à partir de l'url du fichier image bicolore N&B en .pgm. Le constructeur de la carte se charge de lire le fichier et de le transformer en une représentation interne utilisable par le programme, à savoir une matrice booléenne (true pour indiquer la présence d'un mur).
À partir de cette carte, on peut créer un ScanCorrelating dont la méthode correlate permet de donner la corrélation (un nombre entre 0 et 1) entre la perception prévue suivant la position de la particule (passée en paramètre) dans la carte et la perception effectivement recueillie (passée en paramètre). Les détails de cet algorithme seront donnés dans la partie Perception.
La base de l'algorithme se trouve dans les particules qui sont représentées par la classe Particule. Elle regroupe toutes les caractéristiques d'une particule à savoir: la position, l'angle et le poids. La méthode updateConf permet de mettre à jour la particule suivant le modèle d'odométrie. Plus de détails seront fournis dans la partie Odométrie.
A partir des particules on créait un ensemble de particules dans la classe ParticuleSet. Cette classe fournie en sus toutes les méthodes nécessaires à la réalisation du filtre particulaire. La fonction updateConf applique l'odométrie sur toutes les particules; reSample rééchantillonne les particules suivant leur poids suivant l'algorithme de la roue; updateWeight met à jour les poids suivant la correspondance de la perception prévu et observée effectivement en s'appuyant sur la classe ScanCorrelating (attribut de la classe); normalizeWeight permet de normaliser les poids afin d'avoir une somme égale à 1.
La classe Filter se contente, dans sa méthode evoluate d'organiser suivant l'ordre présenté dans la partie Principe du filtre particulaire les différentes parties de l'algorithme du filtrage particulaire, correspondant aux différentes fonctions de ParticuleSet.
La classe Fenetre permet l'affichage des particules, ainsi que de la carte, dans une fenêtre externe à PlayerStage par l'appel à la fonction affichage. Fenetre est liée à l'instance de Map et de ParticuleSet afin de pouvoir afficher les données demandées.
ExampleGraphicPlayer permet l'interfaçage avec PlayerStage.
Algorithme :
On récupère les valeurs de l'odométrie entre deux pas de temps successifs.
Ces données sont transmises à la fonction evoluate de la classe Filter qui effectue l'application de l'algorithme du filtre particulaire.
On appelle la fonction de rafraichissement de l'affichage de la fenêtre.
Nous allons rentrer plus précisément dans les entrailles du code en explicitant certains points clés et délicats de l'implémentation de l'algorithme.
Lors du lancement de Playerstage le robot est placé dans l'environement en fonction des paramètres contenus dans un fichier.
Le robot possède sont propre repère de déplacement qui est différent de celui de playerstage et différent de celui utilisé par nos particules. En effet, le robot initialise son repère dans la position et l'orientation où il se trouve lors du lancement de playerstage, et s'y place à l'origine. Il se déplacera par la suite dans se repère et nous renverra ses valeurs de positions dans ce repère.
Pour palier au problème de la position et de l'orientation de départ inconnue du robot et donc du repère 0, nous utilisons des variation de positions dx, dy et dθ. Nos particules possèdent des coordonnés (x,y,θ), dans le repère 1. Leurs positions vont être mise à jour en appliquant le déplacement du robot aux particules.
A partir du déplacement du robot, on en déduit la norme de celui-ci. On considère alors un déplacement des particules dans la direction pointée par celles-ci d'une valeur correspondant à cette norme de déplacement. Puis, nous mettons à jour l'angle de la particule en lui ajoutant la variation de l'angle du robot au cours du déplacement.
A ces valeurs de positon des particules, on rajoute un bruit gaussien pour compenser le bruit du capteur, qui est dans notre cas, simulé par playerstage. La variance de ce bruit est calculée en fonction des résultats obtenus.
Dans cette partie, nous allons détailler le fonctionnement de la méthode correlate de la classe ScanCorrelating. Pour pouvoir donner la corrélation entre la valeur prévu et la valeur effective, il faut dans un premier temps calculer la valeur prévue (Prévision de la perception), puis la corréler suivant le modèle du capteur (Modèle du capteur). Cette opération est effectuée pour chaque valeur donnée par le laser, la corrélation globale de la particule avec le robot étant donnée par le produit de toutes les corrélations de toutes les scans.
Le challenge résidant dans cette partie est le passage du monde continu des particules (coordonnées et angle pointés par le laser) dans le monde discret de la carte. Ce passage se fait par l'algorithme suivant :
Algorithme :
- On avance d'1 dans la direction de x pour calculer x1 (x+1 ou x-1 suivant les cas) et on regarde la valeur y2 correspondante.
- Si |y2-y|<=1 alors on ne "saute" pas de pixels dans la discrétisation et on prend ces valeurs x1 et y2 pour la suite.
- Sinon on prend la valeur y1, déplace d'1 de y (y-1 ou y+1) et on prend la valeur x2 correspondante (on est sur que l'un ou l'autre des choix nous garanti de ne pas "rater" de pixels)
- On regarde si il y a un mur pour les x et y choisis (x1,y2 ou x2,y1).
- Si oui, l'algorithme est fini et on renvoie la distance entre le x,y actuel et le x,y d'origine.
- Sinon on réitère.
Le raisonnement et l'algorithme sont évidemment identiques peu importe le cadran dans lequel se trouve l'angle du laser. Le choix de commencer par x est arbitraire et le choix est symétrique.
Remarque :
L'algorithme teste préalablement si la particule se trouve dans un mur. Si tel est le cas, l'algorithme renvoie 0 ce qui fait que la corrélation (=poids) de la particule (calculée comme produit de toutes les corrélations de tous les scans) sera nulle. Cela permet d'éliminer les particules se trouvant dans un mur.
Nous utilisons ici des lasers dont la précision est relativement bonne. Le modèle du capteur est représenté par une gaussienne centrée autour de la valeur réelle de variance assez faible, due à la précision du capteur. Le reste de la courbe est une courbe affine de pente faible. La partie finale correspond à la saturation en distance du capteur laser. L'allure du modèle est présenté ci dessous.
Algorithme :
- On calcule la distance prévue au mur (dont le calcul a éte présenté dans la partie Prévision de la perception)
- Si cette mesure est nulle on renvoie 0 (pour traiter les murs)
- Si elle vaut la valeur maximale du capteur et que le capteur a également saturé, on renvoie 1 (partie droite de la courbe)
- Sinon on calcule la valeur de la gaussienne centrée sur la valeur prévue, au point correspondant à la valeur réellement mesurée, et la valeur de l'affine au même point. On renvoie la plus forte des deux valeurs.
Évolution des paramètres
La variance est fonction du nombre de pas de temps que le filtre a effectué. Au départ, la variance, dont la valeur initiale est un paramètre du module, est diminuée de façon rapide jusqu'à un certain point, où la décroissance se ralentit pour finalement se stabiliser. Cette allure permet de conserver au départ des particules même avec une mauvaise vision afin de ne pas supprimer des particules qui ne seraient pas loin de la position du robot. Au fur et à mesure du temps, le positionnement des particules se précise et donc leur perceptions sont de plus en plus concordantes avec celles du robot, d'où la diminution de la variance, tolérance aux erreurs.
Ce processus est la pierre angulaire de notre algorithme de localisation. En effet, un algorithme de navigation trop simpliste (simple évitement d'obstacle par exemple) ne permettra pas au robot de couvrir une partie assez importante de la carte. Dans ce cas, la localisation s'effectue seulement sur une partie restreinte, ce qui peut poser des problèmes perceptual aliasing.
Notre algorithme, en restant simple, doit présenter deux caractéristiques : Robustesse, c'est à dire éviter les situations de blocage, et long rayon d'action, c'est à dire : Passer par le plus de lieux possibles.
Principe : Explorer les entrées latérales. Le robot, en longeant des couloirs, aura la possibilité de quitter le couloir pour bifurquer.
Ce programme implémente un module d'évitement d'obstacle simple : Si un des trois télémètres centraux détecte un obstacle à proximité du robot, le robot évitera l'obstacle en tournant du côté où il y a le plus de place : Ce côté sera choisi en prenant le maximum des valeurs renvoyés par les lasers latéraux. Il est important de noter que le sens de rotation est vérouillé afin d'éviter des situations de blocage. Ce processus est bloquant : Quand il s'exécute, aucun autre comportement ne peut être pris en compte.
Le deuxième comportement, à savoir la possibilité de quitter les couloirs fonctionne de la manière suivante : Si le gradient des données d'un des deux lasers latéraux dépasse un certain seuil, on considère qu'une ouverture est détecté. On fera le choix de bifurquer en suivant deux règles : Attendre au moins trois détections entre deux bifurcations, et, toujours prendre une bifurcation avec une probabilité de un tiers.
Nous allons préciser les embûches auxquelles nous nous sommes heurtés au cours de la réalisation de notre projet afin de sensibiliser quiconque voudrait refaire notre travail aux points sur lesquels il faut porter attention.
Les repères utilisés dans les différentes parties, à savoir particules, carte et PlayerStage (voir figure ci-dessous) sont différents et il faut donc bien faire attention à faire la conversion entre tous ces repères.
L'interfaçage avec PlayerStage n'est pas une chose aisée. Il faut tout d'abord programmer et surtout adapter le comportement du robot. En effet celui ci se déplace pendant les calculs effectués par l'algorithme et avait une fâcheuse tendance à foncer dans les murs. Ensuite il faut faire en sorte de bien récuperer les données d'odométrie (les dernières en date) afin qu'elles soient exploitables par le programme. Enfin, il faut faire attention au fait que le monde de PlayerStage est en mètre, alors que notre programme calculait sur des pixels, et donc faire les conversions adéquates.
Les différents modèles à savoir l'odométrie et la perception, dépendent de nombreux paramètres qui doivent être ajustés "au feeling". En effet ces paramètres jouent un rôle assez important dans l'évolution des particules et donc la convergence de l'algorithme. Il faut donc bien les ajuster en fonction du robot afin que l'algorithme fonctionne.
En particulier avec des variances constantes, le tracking fonctionne mais la localisation globale ne marche que de façon aléatoire. Il faut démarrer avec des variances assez grandes pour pouvoir faire une exploration importante des différentes configurations possibles. Au fil de la décroissance, les particules vont converger vers la position exacte du robot.
D'autres problèmes de toute sortes ont également jalonné notre parcours.
Un petit écueil est le nombre de particules limités (3000) que peut suppporter PlayerStage, ce qui limite la probabilité de bon positionnement des particules et nous a obligé à adapter nos variances.
Le plus fourbe a été lors du calcul de la correspondance entre mètres et pixels. Nous calculions : échelle = taille_carte_en pixel / taille_carte_en_metre. Le problème étant que les deux tailles de cartes étaient en entier ce qui donnait un résultat faux pour la division malgré le fait que l'échelle soit un double. Cela avait pour conséquence de ralentir la progression de nos particules sans que nous ne comprenions pourquoi...
Notre filtre particulaire fonctionne que ce soit en tracking ou pour une localisation globale. La convergence a été obtenue sur différentes cartes, avec différentes positions initiales si la variance initiale et le nombre de particules initiales sont suffisamment importants.
Exemple classique:
Notre projet est dans l'ensemble satisfaisant. Malgré de nombreuses embûches auxquelles nous nous sommes heurtés, en particulier les différents repères et le paramétrage de l'algorithme, nous avons réussi à implémenter un algorithme de filtrage particulaire en interfaçage avec le simulateur PlayerStage. Notre algorithme réalise un suivi de position mais également une localisation globale du robot sur la carte. Il est robuste (convergence systématique sur tous les environnements testés et toutes les positions initiales), précis et paramétrable (la plupart des paramètres critiques sont en paramètre du programme).
Une première amélioration possible pour ce projet pourrait être son implémentation sur le pioneer réel. La robustesse de notre algorithme, qui semble bonne d'après la simulation avec des capteurs très bruités, pourrait ainsi être mise à l'épreuve. Pour ce faire, il faudrait recréer un environnement ainsi que sa carte correspondante.
Une seconde amélioration, algorithmique celle ci, pourrait être l'utilisation de notre filtre particulaire dans un contexte plus large, notamment le fast SLAM qui utilise le même procédé.
Il pourrait également être envisagé la mise en place d'un processus de cartographie par grille d'occupation indépendant, afin de le valider, en amont avec le filtre particulaire.
Une dernière amélioration, plus technique, pourrait viser au réglage fin des paramètres afin d'accélérer la vitesse de convergence : décroissance des variances et bruits d'odométrie mieux adaptée. Un tel réglage viserait sans doute les mêmes résultats avec un nombre plus limité de particules. Le temps de calcul s'en trouverait réduit.
|
polycopié du cours de robotique de David Filliat site du simulateur PlayerStage site en anglais sur la localisation/planification / page sur le filtre particulaire |