SLAM Visuel 3D pour robot mobile autonome

Master de Sciences Mention «Imagerie, Robotique et Ingénierie pour le Vivant» - Projet de Fin D étude - SLAM Visuel 3D pour robot mobile autonome Romain Drouilly Encadrants ECA Benoit Morisset Pierrick Daniel Encadrant ENSPS Bernard Bayle Septembre 2011

Résumé Un robot mobile est une machine capable de se mouvoir de manière autonome quelque soit l endroit où elle se trouve et sans la supervision de l homme. Pour cela le robot doit pouvoir cartographier son environnement à mesure qu il l explore. Ce problème de la navigation autonome d un robot dans un environnement inconnu est nommé SLAM (SLAM signifie Localisation et Cartographie Simultanées). C est un domaine de recherche très actif depuis plus de vingt ans et il est maintenant possible de faire du SLAM en 2D. Le nouveau challenge est de faire du SLAM en 3D. C est le but de cette étude. Comme il sera montré il est désormais possible de résoudre le problème du SLAM3D grâce à des capteurs innovants et des algorithmes performants. Nous montrerons qu il est possible de construire en temps réel des cartes en 3D très précises pour modéliser des environnements intérieurs. En ajoutant des textures il est alors possible de créer des vues très réalistes de la zone explorée. Par ailleurs on peut extraire en temps réel des données à partir de la carte fournie par le SLAM, dans le but de fournir au robot des informations sur les objets présents dans la scène. Ceci permet d améliorer significativement la connaissance que le robot a de l environnement et permet de le faire agir intelligemment. Abstract A mobile robot is an intelligent machine able to navigate by itself in any places without human s supervision. To do this robot has to map its environment while it is exploring it. The problem of a robot navigating an unknown environment is called the SLAM problem (SLAM stands for Simultaneous Localization And Mapping). It has been a very active field of research for more than twenty years and we now have solutions to address the 2D SLAM problem. The new challenge is to tackle the 3D SLAM question. This is the goal of this study. As it will be shown in this report it is now possible to address the 3D SLAM problem thanks to cutting edge sensors and very effective algorithms. We will demonstrate that it is possible to build in real-time very precise 3D maps to model indoor environments. Adding texture it is possible to create very realistic views of the explored area. Moreover we can extract data in real-time from the model built by the SLAM to provide the robot with information about objects in the scene. This significantly improves the knowledge the robot has of its environment and allows to make it act in an intelligent way. 1

Remerciements En tout premier lieu je tiens à remercier Benoit Morisset qui a suivit mon travail tout au long de ces six mois. Nos échanges réguliers ont été pour moi une très forte source de motivations et sont pour beaucoup dans la réussite de ce stage. Ses conseils avisés m ont toujours permis d avancer efficacement dans mon travail. Ensuite je tiens à remercier Pierrick Daniel, mon maitre de stage, qui m a permis de travailler sur le sujet passionnant et plus que jamais d actualité qu est le SLAM et grâce à qui j ai découvert ce merveilleux outil qu est la Kinect. Je tiens aussi à remercier Gwenaël Dunand pour m avoir fait découvrir les Design Pattern et m avoir fait partager son expérience de la recherche et son expertise en programmation. Mes sincères remerciements vont à toute l équipe d ECA Saclay pour l accueil chaleureux dont j ai bénéficié et à toutes les personnes avec qui j ai pu partager ma passion pour la robotique. Enfin je tiens à remercier tous ceux qui, à Willow Garage ou ailleurs, au travers d initiatives collectives ou personnelles, mettent à disposition leur travail pour les progrès de tous. 2

Sommaire A. Introduction... 6 B. Objectifs du projet... 7 C. Introduction au problème du SLAM... 8 I. Autonomie des robots... 8 1. Robotique: de la Fiction à la Réalité... 8 2. Qu'est ce qu'un robot autonome?... 9 3. Percevoir son environnement... 9 II. Odométrie Visuelle... 11 1. Points d intérêt... 11 2. RANSAC... 12 3. ICP... 13 III. SLAM... 15 1. Définition du problème... 15 2. Principaux paradigmes... 16 3. Recherches actuelles... 19 IV. TORO : SLAM 3D... 20 1. Principe... 20 2. Graph-SLAM... 21 3. TORO : formalisme... 21 V. Segmentation des nuages de points et reconstruction 3D... 25 1. Principe de la segmentation... 25 2. Mise en œuvre: cas des objets plans... 26 D. Contribution au SLAM... 28 I. Contexte général de l étude... 28 1. ROS... 28 2. Kinect... 29 II. Analyse de l existant... 33 3

III. Solutions retenues... 40 IV. RGBD Mapping... 41 1. L algorithme... 41 2. Estimation de mouvement... 42 3. Optimisation globale du graphe et fermeture de boucle... 45 4. Implémentation... 46 5. Schéma de fonctionnement du SLAM... 48 V. Descripteur NARFs... 49 1. Tests du détecteur... 49 2. Conclusion générale sur les NARFs... 50 VI. Segmentation et Reconstruction 3D... 51 1. Introduction... 51 2. Détection de plans dans un nuage de points... 52 3. Détection des contours d un plan... 55 4. Appariement de plans... 57 5. Problème lié à la reconstruction 3D temps réel... 59 6. Recollage des textures... 59 7. Organisation du code pour la reconstruction 3D... 60 E. Résultats... 61 I. Matériel de tests... 61 II. Test du SLAM... 62 1. Fidélité de la reconstruction 3D... 62 2. Amélioration due à ICP... 63 3. performances avec et sans supervision... 65 4. Robustesse aux variations de luminosité... 66 III. Test de la reconstruction 3D... 67 1. Test de la détection du sol en temps réel... 67 2. Test de la reconstruction 3D... 68 IV. Discussion sur le SLAM et la reconstruction 3D... 73 F. Conclusion de l étude... 75 G. Bibliographie... 77 4

H. Annexes... 78 I. ECA... 78 1. Présentation du groupe ECA... 78 2. Les Activités du Pôle de Saclay... 79 II. Kd-tree... 83 III. SIFT... 84 1. Détection des points d'intérêt... 84 2. Calcul des descripteurs... 86 IV. SURF (Speeded Up Robust Feature)... 87 1. Détection de points d'intérêts... 87 2. Calcul des descripteurs... 88 V. Précision subpixellique... 89 5

A. Introduction ECA conçoit des robots pour l intervention en situations extrêmes : incendie, tremblement de terre, accident nucléaire, déminage, intervention policière, renseignements etc. Dans toutes ces situations le robot vient en aide à l Homme en explorant des zones inaccessibles ou trop dangereuses pour envoyer, au moins dans un premier temps, des êtres vivants. Les robots doivent donc d une part être suffisamment autonomes pour remplir leur mission sans la supervision de l Homme et d autre part être capable de fournir à l utilisateur des informations sur la topographie du lieu, le plus souvent sous la forme d une carte. L objectif du projet est donc de développer un module permettant de rendre le robot autonome dans la tâche d exploration. Plus précisément, le module doit permettre au robot de dresser la carte d un environnement inconnu au fur et à mesure qu il l explore. Cette carte servira à la fois au robot à se localiser pour évoluer de manière autonome et à l utilisateur humain pour récupérer des informations sur les lieux explorés. Le problème de l autonomie d un robot dans la tâche d exploration d un lieu inconnu est appelé SLAM 1. C est un domaine de recherche très actif depuis plusieurs décennies et beaucoup de solutions existent pour la cartographie 2D. Le défi est ici de réaliser la modélisation en 3D de l environnement et ce uniquement à partir d informations visuelles. Après une présentation des objectifs du projet, les fondements du problème du SLAM seront présentés en détails en les replaçant dans leur contexte originel. Une présentation formelle du problème sera introduite en insistant plus particulièrement sur le paradigme qui sera utilisé dans l étude. C est ensuite le travail de stage luimême qui sera abordé avec la présentation successive des études sur lesquels il est fondé, des solutions proposées et des résultats obtenus. Chaque fois que nécessaire les choix effectués seront justifiés. Finalement les résultats seront discutés et un certain nombre d observations seront faite sur la robotique moderne. 1 Simultaneous Localization and Mapping 6

B. Objectifs du projet Lorsqu un robot est employé pour explorer une zone inaccessible à l homme, il doit être capable de fournir des informations à l utilisateur sur la topologie des lieux visités. Il faut donc qu il construise une carte de son environnement. Il doit en outre faire preuve d une autonomie suffisante pour pouvoir explorer la zone et trouver son chemin même si les communications sont interrompues. Le robot doit donc être capable de se localiser en permanence dans l environnement qu il explore. L objectif du stage est donc de mettre en place un module de SLAM pour un robot d intervention en milieu hostile. Plus précisément, le projet vise à doter le robot de la capacité à cartographier en 3D un environnement inconnu. Un capteur innovant a été retenu pour cette tâche : la Kinect de Microsoft. Il s agit d une caméra RGBD, autrement dit d un système d acquisition fournissant à la fois des images et des données métriques. Il faudra donc exploiter au mieux la richesse des informations fournies pour construire un modèle 3D de l environnement visuellement réaliste. Les contraintes principales sont donc de : - réaliser un modèle précis de l environnement permettant au robot de se localiser en vue de planifier sa trajectoire - d offrir un rendu réaliste de la scène pour un observateur humain - d effectuer la reconstruction en temps réel Une contrainte secondaire est d offrir une bonne robustesse aux variations de luminosité. 7

C. Introduction au problème du SLAM L objectif de ce chapitre est de présenter le contexte général du stage et d expliquer ce qui a motivé la problématique en la replaçant dans son contexte originel. Plusieurs questions seront abordées concernant les développements présents et futurs de la robotique, notamment: quels sont les verrous technologiques qui empêchent aujourd hui un robot de conduire une mission de manière parfaitement autonome? Quelles sont les pistes de recherche actuelles pour résoudre ces problèmes? Quels sont les succès déjà remportés? I. Autonomie des robots 1. Robotique: de la Fiction à la Réalité Loin des images des films de science fiction, montrant hommes et robots cohabitant harmonieusement dans des environnements complexes, la robotique s est pendant longtemps limitée à ses applications industrielles, se contentant de remplacer l homme dans l exécution de tâches simples et répétitives. Cette limitation venait notamment de l impossibilité de créer des systèmes capables d évoluer dans un environnement réel. En effet gérer l extrême diversité des situations dans lesquels un robot peut se trouver est très difficile. Il est clairement impossible de prévoir tous les cas de figure pour préparer des comportements types en fonction des situations rencontrées. Il faut dès lors que le robot soit à même de percevoir son environnement intelligemment pour apprendre et comprendre cet environnement en vu de planifier ses actions. Loin d avoir résolu totalement ce problème la robotique a cependant connu ces dernières années plusieurs victoires importantes qui ont conduit à de profonds changements. Qu ils soient à pattes ou à roues, volant ou rampant, les robots sont désormais capables de se mouvoir dans des environnements aussi complexes que le sol martien ou un hall de musée avec très peu de supervision de la part de l homme. Voyons comment ces succès ont étés rendus possibles. 8

2. Qu'est ce qu'un robot autonome? L'autonomie des robots est un enjeu clé de la robotique moderne. Un système robotisé n'a de raison d'être que s'il est capable d'effectuer un maximum de tâches sans la supervision de l'homme. Un système téléguidé représente bien moins d'intérêt puisque l'opérateur doit prendre en charge les tâches de bas niveau comme la navigation au lieu de se concentrer sur les objectifs essentiels de sa mission. Si l'on veut définir clairement ce qu'est un robot mobile autonome on peut dire qu'il s'agit d'un système capable de: se localiser dans son environnement. Ceci répond à la question «où suis-je?». trouver des zones d'intérêt à explorer ou des objets dans son environnement et liés à sa mission. Ceci répond à la question «où dois-je aller?». de planifier ses actions, pour par exemple définir une trajectoire pour se rendre d'un point A à un point B. Ceci répond à la question «comment dois-je y aller?». d'interagir, le cas échéant, avec son environnement pour réaliser certaines tâches. Il peut s'agir par exemple de trouver et d'actionner une poignée de porte pour pouvoir passer d'une pièce à une autre. Ceci répond à la question «Quelle est la fonction de cet objet?» On distingue donc deux types principaux de contraintes pour l'autonomie des robots: la capacité d'extraire des données de l'environnement (localiser des objets, des obstacles, le robot etc.) et la capacité à traiter ces informations intelligemment (prendre des décisions, savoir comment interagir avec tel ou tel objet). A ceci il faudrait encore ajouter d'autres aspects concernant notamment la sécurité du robot ou des êtres vivants avec lesquels il cohabite. Cependant c'est surtout le premier point qui nous intéressera dans la suite. 3. Percevoir son environnement Quelque soit la mission qui lui est confiée, un robot mobile doit être capable de percevoir intelligemment son environnement. C'est à dire qu'il doit être en mesure de capter l'information et de la traiter en vue de produire des données exploitables. 9

Le robot doit notamment être capable de construire sa propre représentation de l'environnement. Cette tâche est nommée la cartographie. Il doit pouvoir fabriquer, sur la base des informations fournies par ses capteurs, une carte lui permettant de définir des zones d'intérêt à explorer, des zones dangereuses à éviter ou encore trouver les zones navigables. Cette carte pourra aussi servir de modèle de l'environnement à un opérateur humain qui voudrait intervenir dans une zone a priori inconnue. Or lorsque le robot explore une zone inconnue il doit aussi être capable de se localiser sur la carte à mesure qu'il la construit. La réalisation simultanée de la cartographie et de la localisation est un problème fondamental en robotique mobile connu sous le nom de SLAM (Simultaneous Localisation and Mapping). Sa résolution est un préalable indispensable à l'automatisation totale des systèmes, sans quoi les robots seraient incapables de se mouvoir dans un environnement inconnu. Dans la suite nous verrons donc quelles sont les étapes nécessaires à la réalisation du SLAM. 10

II. Odométrie Visuelle Pour la réalisation d une carte sur la base d informations purement visuelles, il faut pouvoir estimer le mouvement de la caméra pour mettre en correspondance les différentes vues. Dans le cas d un système monoculaire cette tâche est rendue difficile par l impossibilité d estimer les distances à partir d une seule image. Il faut donc utiliser des séquences d images pour initialiser les distances. Ici la Kinect fournit directement la mesure de distance, on peut donc connaitre la position des points observés dans chaque image. 1. Points d intérêt Pour estimer le mouvement de la caméra entre deux images il faut pouvoir déterminer comment ont bougés les objets observés. Pour cela on ne peut utiliser que des objets visibles dans les deux images et que l on est capable de retrouver facilement dans chacune d elles. Pour des raisons de simplicité, on utilise en général des points d intérêt comme objets de référence. Un point d'intérêt est un point qui caractérise de façon unique une partie de l'image. Plus précisément il s agit d un point localisé finement, augmenté de son voisinage. Pour être utile ce point doit être très bien identifié à l aide d un descripteur unique et il doit être possible de le retrouver facilement. Il existe une multitude de méthode de détection/description de points d intérêt parmi lesquelles SIFT et SURF. Il s agit de méthodes particulièrement appréciées pour leur robustesse et leur efficacité. Elles sont décrites en détails en annexe. Le principe de détection correspond à rechercher des zones d intensité lumineuse particulière et offrant une grande stabilité. Pour cela on utilise le plus souvent des méthodes basées sur le calcul du gradient de luminosité. Les extrema correspondent aux points où s annule le gradient. Il faut noter que ces points d intérêt ne sont pas nécessairement des pixels. En effet on peut obtenir la position des points avec une précision subpixellique par des méthodes d interpolation 2. Une fois les points identifiés il faut décrire l'information locale dans l'image de façon unique et aussi invariante que possible de l'échelle d observation, des rotations, des variations de luminosité etc. Pour cela on calcule un descripteur pour chaque point. On commence par déterminer une orientation privilégiée basée sur l observation de l entourage du point. Le descripteur est alors simplement une suite 2 Voir annexes 11

de valeurs (64 ou 128 le plus souvent) qui décrivent le voisinage à partir de cette direction privilégiée. Ceci permet de rester invariant par rotation. Figure 1: Points d'intérêt détectés entre deux images. Les traits verts symbolisent les déplacements entre deux itérations. Les traits rouges signalent les déplacements non conformes par rapport aux autres points 2. RANSAC Une fois les points d intérêts calculés dans chaque image il faut pouvoir déterminer quel est le mouvement global de la caméra. Pour cela on doit trouver quelle transformation permet de recoller correctement les deux nuages de points formés par les points d intérêt. La recherche de cette transformation est une tâche complexe, d'autant plus que les nuages sont bruités. Il existe plusieurs types de méthodes qui présentent toutes des avantages et des inconvénients, la rapidité de calcul étant souvent incompatible avec la précision. L'algorithme RANSAC (RANdom SAmple Consensus) est classiquement utilisé pour extraire un modèle d'un ensemble de données bruitées. Dans le cas présent RANSAC est utilisé pour déterminer quels sont les points qui permettent d estimer la transformation entre les deux nuages le plus correctement possible. Autrement dit RANSAC permet de filtrer les points trop bruités. Concrètement trois points dans chaque nuage sont tirés au hasard pour déterminer une base. La transformation liant les deux bases est alors calculée. Les nuages sont recollés suivant cette transformation et la distance entre chaque couple de point est estimée. Un score est alors attribué à la transformation : si beaucoup de couples de points se superposent le score est élevé, sinon il est faible. Finalement la 12

transformation qui possède le plus haut score est conservée : c est elle qui permet de recoller le plus de points ensemble, c est donc elle qui a le plus de chance d être juste. Cette méthode permet en plus d évaluer la transformation liant les deux nuages de point de filtrer les points qui n entrent pas dans le modèle principal. L estimation de mouvement avec RANSAC est en générale assez bonne mais dans le cadre de la construction d une carte il est préférable d affiner cette estimation avec une autre méthode comme ICP. 3. ICP ICP (Iterative Closest Point) est un algorithme qui permet de minimiser la distance entre deux nuages de points A et B. Il est utilisé notamment lorsque l'on veut reconstruire des objets 3D à partir de différentes vues, pour la construction de cartes etc. L'avantage d'icp par rapport à RANSAC est qu'il permet d'estimer les mouvements avec une grande finesse. Il nécessite cependant en entrée une estimation de la transformation a trouver sans quoi l'algorithme peut rester bloqué dans une configuration non optimale (minimum local). Le principe de l'algorithme est assez simple. Il consiste à appareiller les points deux à deux en utilisant une fonction de cout quadratique (distance euclidienne). A chaque itération on estime la transformation entre les deux nuages A et B, la source et la cible, à partir de ces couples de points. On déplace le nuage cible conformément à la transformation calculée et la distance entre les deux nuages est alors réévaluée. L'algorithme stoppe lorsque la distance minimale entre les deux nuages est atteinte. Celle-ci est fixée au début de l'algorithme en fonction de la précision souhaitée. De cette manière on obtient une estimation fine de la transformation entre les deux nuages de points qui permet un recollage presque parfait. Avec ces méthodes combinées on peut donc estimer le déplacement de la caméra entre deux images successives. On reconstruit alors au fur et à mesure des déplacements la trajectoire de la caméra tout en modélisant l espace 3D. Cependant bien que la précision de ces méthodes soit bonne, on constate une dérive de la position au cours du temps due à l accumulation des erreurs de mesure. Ceci s observe notamment lorsque le robot après avoir parcouru un certain trajet revient à une position déjà visitée. La localisation courante du robot est alors souvent différente de la localisation initiale. Il est donc nécessaire de créer une boucle de supervision qui détecte lorsque le robot repasse par un endroit déjà exploré. L algorithme permet alors en constatant l erreur sur l estimation de position de corriger la trajectoire et de la rendre plus cohérente. Cette étape est nommée fermeture de boucle. Par ailleurs la carte créée par le robot peut être optimisée en fonction de la connaissance que l on a des erreurs de mesure. L optimisation de la 13

carte et la détection de la fermeture de boucle sont essentielle à l obtention d une carte de qualité. C est l algorithme de SLAM qui réalise ces étapes. 14

III. SLAM Le SLAM est considéré comme l'un des problèmes les plus fondamentaux de la robotique moderne. Comme évoqué plus haut il permet de corriger les erreurs accumulées pendant la phase d estimation de mouvement en vue d obtenir un modèle cohérent de l environnement. Cette partie présente le problème formellement ainsi que ses trois principaux paradigmes. 1. Définition du problème Un robot mobile, doté de la capacité de percevoir son environnement, explore un milieu inconnu. Il part d'un point de coordonnées connues x0. L'estimation du mouvement du robot est entachée d'incertitudes à chaque mesure, qui rendent l'estimation de position de plus en plus imprécise. Le SLAM vise à résoudre le problème de la création d'une carte non biaisée de l'environnement dans ce contexte. Formellement on utilise les probabilités pour décrire le problème. Soit x t la position du robot à l'instant t (en 2D ou en 3D), on note ={ 0, 1,..., } La trajectoire du robot jusqu'à l'instant T. Soit ut l'estimation de mouvement faite entre les instant t-1 et t par l'odométrie. On note: ={ 0, 1,..., } L ensemble du chemin parcouru jusqu'à l'instant T. Enfin soit m la carte de l'environnement réel, considéré comme statique. Les mesure faites par le robot mettent en relation les estimations de déplacement x t et la carte observée, m. Si l'on considère que le robot fait une mesure en chaque point alors on peut noter la séquence de mesures: = { 1, 2, 3,..., } Le problème est maintenant de retrouver un modèle de l'environnement m ainsi que la trajectoire XT du robot à partir de l'odométrie UT et des observations ZT. 15

Il existe deux formes de SLAM. Le premier nommé «online problem» consiste à calculer à chaque instant la position actuelle du robot en fonction des estimations de mouvements et des observations ce qui se note: (,, ) La seconde est nommée «full SLAM problem» et consiste cette fois à calculer à chaque instant l'ensemble de la trajectoire à partir de l'odométrie et des observations. On définit alors ce problème par: (,, ) Enfin pour résoudre le problème du SLAM, le robot a besoin de deux informations supplémentaires que sont : le modèle reliant les mesures issues de l'odométrie avec les positions du robot le modèle permettant de relier les mesures zt avec la position du robot. Ces modèles sont exprimés respectivement par les formules ( 1, ) qui traduit la probabilité d'être à un point de coordonnées xt en partant du point xt-1 et en parcourant le chemin ut ; et (, ) qui est la probabilité de faire l'observation zt en étant à la position donnée xt connaissant m. 2. Principaux paradigmes Il existe trois manières principales de traiter le problème du SLAM dont dérivent beaucoup d'algorithmes. 2.1 EKF-SLAM La première méthode apparue est basée sur le filtrage de Kalman Etendue. Elle utilise un vecteur d'état pour représenter la position du robot et des amers dans la scène, auquel est associé une matrice d'erreur représentant les incertitudes sur les positions, les observations et les corrélations entre les différentes variables du vecteur d'état. Alors que le robot se déplace le vecteur d'état et la matrice d'erreurs sont mis à jour en utilisant un filtre de Kalman étendu. A chaque nouvel amer observé de nouvelles variables d'état sont ajoutés au vecteur d'état du système; la taille de la matrice de covariance croit quadratiquement. 16

Cette approche a été la première développée. Elle est de moins en moins utilisée aujourd'hui notamment du fait des temps de calcul qui la rendent moins intéressante que les autres. 2.2 SLAM basé sur les filtres particulaires Une seconde approche pour traiter le problème du slam est basée sur l'utilisation de filtres particulaires. Le principe est de suivre un grand nombre d'hypothèses en parallèle qui sont autant de trajectoires possibles. Ces différentes hypothèses correspondent à un échantillonnage de la distribution de probabilité des trajectoires. Pour chacune d'elles on construit la carte en fonction des perceptions du robot à l'aide d'un filtre de Kalman. Cependant dans ce cas le traitement est simplifié puisque la trajectoire est connue: les perceptions successives des différents amers ne sont plus corrélées et la matrice de covariance se simplifie puisqu'on ne mémorise plus que les variances individuelles des amers. La complexité des calculs passe ainsi de 0(N²) à O(N). Le problème principal de cette technique est que la représentation de la carte et de la trajectoire du robot devient vite très lourde. En effet il faut que le nombre de particules soit suffisant pour échantillonner correctement la distribution de probabilité des trajectoires. Par ailleurs lors de fermeture de boucle, seules les trajectoires correctes sont retenues ce qui entraine un ré-échantillonnage du filtre particulaire, conduisant à une forte perte d'information. Ce problème est d'autant plus important que l'environnement contient plusieurs cycles (plusieurs ré échantillonnages successifs). 2.3 Graph-SLAM La troisième méthode est basée sur la théorie des graphes. Elle consiste à considérer les positions successives du robot et des différents amers comme les nœuds d'un graphe. Les arrêtes sont alors constituées des contraintes fournies par l'odométrie ou par l'observation des amers. Pour illustrer ce propos voyons comment procède le robot pour construire la carte. 2.3.1 Construction d'un graphe A l'instant de départ le robot observe le amer 1. Le graphe est donc constitué de deux nœuds, la position du robot et celle du amer, ainsi que d'une arrête : la contrainte observationnelle entre le robot et le amer 1. 17

A l'instant t2 le robot a avancé d'une distance u2 fournie par l'odométrie et observe les amers 1 et 2. Le graphe est maintenant constitué de quatre nœuds: les positions du robot à t1 et t2, reliées par l'estimation de déplacement (odométrie); les positions des amers, reliées aux positions du robot par les contraintes observationnelles. Figure 2 : exemple de création d un graphe 2.3.2 Optimisation du graphe Une fois le graphe construit on cherche à l'optimiser en minimisant l'erreur sur les contraintes du graphe. Du fait des incertitudes sur les mesures il existe en effet des erreurs dans l'estimation de position du robot et des amers. Le «full SLAM problem» a été défini plus haut comme la probabilité d'avoir une trajectoire et un modèle de l'environnement pour un ensemble de mesures données (odométrie plus observations), ce qui s écrit (,, ). En prenant le log on obtient: log (,, ) = + log ( 1, ) + (, ) Chacun des éléments de cette somme correspond à un mouvement du robot et forme une arrête du graphe. L'optimisation du graphe consiste donc finalement à calculer :, =, (,, ) Où * est le symbole pour la solution optimale. Le gros avantage du Graph-SLAM est qu'il permet de gérer les cartes composées d'un très grand nombre de nœuds (>10⁸ à ce jour) ce qui est impossible avec les autres techniques. Cependant l'optimisation du graphe peut être très lourde. 18

3. Recherches actuelles Le SLAM a fait l'objet de nombreux développements ces dernières années qui ont permis d'arriver à des solutions variées plus ou moins performantes. Il existe aujourd'hui plusieurs algorithmes ayant fait leurs preuves pour la cartographie 2D avec des caméras ou des Lasers et plusieurs logiciels proposent leur propre module de SLAM 2D. L'objectif est aujourd'hui de passer à la 3D. Reconstruire l'environnement intégralement permettrait de surpasser les performances des robots actuels en leur permettant d'accéder à des données beaucoup plus riches. La cartographie 3D autorise en effet de rapprocher considérablement la perception des robots de la vision animale et donc d'améliorer sensiblement leur potentielle compréhension de leur environnement. Pour évoluer dans un environnement complexe il faut en effet être capable d'en percevoir la complexité. La cartographie 3D permettrait en outre de surmonter des problèmes classiques rencontrés en 2D comme la détection d'une table ou d'obstacles situés à différentes hauteurs, d'envisager l'exploration de milieux en plusieurs dimensions (plusieurs étages d'un bâtiment, milieu naturel etc) d'ajouter d'autres types d'informations aux cartes que les informations métriques, comme par exemple la couleur. Le passage à la 3D présente bien sur des défis supplémentaires notamment en termes de volume de données à traiter. Cependant comme nous le verrons dans la suite ceci est aujourd'hui possible. 19