SLAM Visuel 3D pour robot mobile autonome - Master de Sciences
←
→
Transcription du contenu de la page
Si votre navigateur ne rend pas la page correctement, lisez s'il vous plaît le contenu de la page ci-dessous
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é SLAM1. 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 lui-
mê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.
9Le 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.
10II. 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’interpolation2.
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
11de 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
12transformation 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
13carte 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.
14III. 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 xt 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.
déplacement xt et la carte observée, m.
Les mesure faites par le robot mettent en relation les estimations de
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.
15Il 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.
( | − 1 , ) qui traduit la probabilité d'être à un point de coordonnées xt en
Ces modèles sont exprimés respectivement par les formules
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.
16Cette 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.
17A 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
données (odométrie plus observations), ce qui s’écrit ( , | , ).
une trajectoire et un modèle de l'environnement pour un ensemble de mesures
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.
183. 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.
19IV. TORO : SLAM 3D
Cette partie présente TORO, l’algorithme de SLAM utilisé dans la suite. Après
un rapide descriptif des principes
principes utilisés dans TORO, le formalisme est présenté en
détail.
1. Principe
Lorsqu'un robot se déplace, il observe des amers dont il évalue la position dans
la scène. Entre deux poses successives le robot peut estimer sa trajectoire en
observant comment les positions des amers dans son champ d'observation ont
évoluées relativement à lui. Il créer donc une contrainte entre deux poses
successives.
Figure 3 : Position du robot (triangles bleus) et des amers (étoiles) au cours du temps.
temp Les flèches
jaunes sont les contraintes dues à l’odométrie ou à la vision.
Figure 4 : schématisation de principe de base exploité dans TORO : les contraintes visuelles à différents
instants sont transformées pour créer des contraintes
contraintes entre des poses non successives (flèches jaunes
pales).
20Cependant, il arrive fréquemment que le robot observe plus de deux fois le
même amer. Il est alors possible de créer de nouvelles contraintes tout au long de la
trajectoire entre des poses non successives. Ceci permet de réévaluer la trajectoire à
chaque itération en prenant en compte l'ensemble des observations. C'est cette idée
qui est exploitée dans TORO.
2. Graph-SLAM
TORO utilise une représentation des données sous forme de graphe.
Autrement dit il n'existe pas de carte métrique au sens usuel du terme.
Pour construire le graphe TORO réduit l'ensemble des données à une
succession de nœuds, les poses du robot, reliées par des contraintes entre ses nœuds
correspondant aux déplacements estimés. Les contraintes sont déterminées à partir
des observations faites à chaque itération.
Ici on ne considère pas la position d'un amer comme une contrainte. On déduit
à partir de la position estimée à différentes poses des relations entre les nœuds qui
elles sont prises en compte comme des contraintes. L'avantage principal de cette
représentation est que les contraintes modélisent mieux les dépendances entre les
données que dans une représentation classique. En effet avec cette représentation
sont mis en relation un ensemble d'observations faites à un instant t avec un autre
ensemble d'observations faites à un instant ultérieur, et non pas les observations
elles-mêmes.
Par ailleurs dans cette représentation l'optimisation est complètement
découplée du modèle sous-jacent (optimisation de graphe), ce qui permet une
grande souplesse d'utilisation de l'algorithme.
La contrepartie est cependant que pour un amers observé N fois il existe N²
arrêtes, donc la complexité de la résolution croît rapidement.
3. TORO : formalisme
L'algorithme consiste à trouver la configuration des nœuds du graphe qui
minimise l'erreur introduite par les contraintes.
Ainsi en notant :
x = (x1 · · · xn ) le vecteur des paramètres décrivant la configuration des
nœuds (qui ne sont nécessairement des relations métriques);
21δji la contrainte entre le nœud i et j, qui correspond à l'arrête ij ;
Ωji la matrice modélisant les incertitudes sur les contraintes;
fji (x) la fonction définissant la contrainte non bruitée entre les points i et j;
On peut définir l'erreur sur la contrainte entre les nœuds j et i, notée eji, par :
678 () = 978 () – ;78
On peut aussi définir le résidu rij par :
*78 () = −678 ().
L'intérêt de l'utilisation du résidu est que ce vecteur donne la 'direction' dans
laquelle il faut changer le graphe (l’erreur diminue dans ce sens).
Si on fait l'hypothèse d'un bruit gaussien on peut exprimer le log négatif de la
probabilité3 de fij 4 par :
78 (978 () − ;78 )
= 678 () >78 678 ()
= *78 () >78 *78 ().
En supposant que les observations sont indépendantes5, le log négatif de la
probabilité pour une configuration x est :
3
L’intérêt de prendre le log négatif est de ramener la probabilité sur une plage
de 0 à l’infini, de transformer les produits en sommes et de se ramener à une
fonction monotone.
4
Voir chapitre consacré au Graph-SLAM
5
Ceci suppose que la probabilité de A et B est la probabilité de A fois celle de B
22< () = ? 78 *78 ()
@A,BCDE
Finalement l'objectif est de trouver la configuration x* qui maximise la
probabilité des observations, ce qui s'écrit :
∗ = )*'8" < ()6
Où * est le symbole pour désigner la solution optimale.
3.1 Descente de gradient stochastique(SGD)
Pour minimiser la fonction F une approche présenté dans par Olson [7]
consiste à sélectionner de manière itérative les contraintes et à déplacer les nœuds
dans le sens permettant une diminution de l'erreur. Les nœuds sont mis à jour selon
la formule :
HIJ = H + K ・L MJ NBA
,
>BA *BA
Où x représente l’ensemble des variables décrivant la localisation de la pose
dans le graphe. H est la hessienne du système qui représente la courbure de la
fonction d’erreur. J est la jacobienne. Le facteur lambda sert simplement à éviter des
oscillations dans l'algorithme en diminuant progressivement la part du résidu utilisée
dans la mise à jour des variables.
3.2 Parametrisation
La qualité de la parametrisation joue un rôle important pour l'efficacité de
l'algorithme. En effet avec la méthode SGD il faut à chaque itération définir quelle
partie du graphe doit être mise à jour. Avec la méthode présentée par Olson [7] le
6
On démontre qu’en prenant le log négatif rechercher la probabilité maximum
revient à calculer argmin
23problème est que pour l'optimisation d'une contrainte entre les nœuds i et k, il
faudra mettre à jour tous les nœuds j pour j=i à k.
Dans TORO une autre méthode exploitant au mieux la topologie du graphe a
été développée utilisant un arbre pour la représentation des données. Ceci permet
de décomposer le problème en plusieurs sous problèmesproblèmes plus simples qui
correspondent soit à des contraintes de l'arbre soit des contraintes additionnelles
(fermeture de boucle).
Figure 5:: passage du graphe à un arbre pour la phase d'optimisation
Chaque équation du SGD résout réso l'un de ces sous-problèmes.
problèmes. Dans cet
algorithme on traite les rotations et les translations séparément. Si l'on considère les
rotations, elles induisent des non linéarités importantes dans le problème. Le SGD
répartit simplement le résidu le long de la trajectoire. Ceci se fait au travers du
terme :
O = K ・L MJ NBA
,
>BA *BA
Pour ce qui est de la translation la solution très intuitive consiste
simplement à distribuer le déplacement le long de la trajectoire.
Figure 6: distribution des erreurs en rotation
rotation et translation le long de la trajectoire
24V. Segmentation des nuages de points et
reconstruction 3D
Pour représenter une scène en 3D il faut pouvoir mesurer la position des
objets observés dans l’espace. Pour l'heure les capteurs permettant de déterminer
les positions en 3D utilisent soit des lasers soit des systèmes vidéos et fournissent
une représentation de la scène sous forme de nuages de points. La visualisation
d’une carte sous cette forme n’est pas optimale d’une part parce que les points n’ont
pas de signification physique réelle, ils sont simplement une vision échantillonnée de
la réalité sous-jacente, et d’autre part parce que pour représenter des surfaces il faut
un très grand nombre de points.
Cette manière de représenter les données impose donc une étape de
reconstruction de la scène qui vise à extraire du nuage de point les données
véritablement pertinentes, à savoir les primitives géométriques. Prenons l'exemple
d'une sphère. Lorsqu'elle est observée par un Laser le résultat fourni est un groupe
de points répartis dans l'espace selon une relation bien précise donnée par la
géométrie de l'objet, ici une surface sphérique. La question qui se pose est donc de
savoir comment retrouver les propriétés de la surface à partir de l’ensemble des
points qui la forment. Dans la pratique un objet n'est jamais observé isolement, il est
inclus dans un nuage représentant la scène globale et sa taille relativement au reste
du nuage peut être faible. Il faut donc dans le même temps segmenter le nuage de
point pour en extraire les sous-ensembles formés par les objets.
1. Principe de la segmentation
Pour pouvoir identifier un ensemble de points correspondant à un objet
particulier il faut connaître la relation liant les points. Il faut donc disposer d'un
modèle de l'objet que l’on cherche. Le principe de la recherche consiste dans un
premier temps à identifier un ensemble minimal décrivant correctement le modèle
(par exemple trois points pour un plan) puis à essayer d’étendre le support de ce
modèle en cherchant les points qui s’accordent avec lui.
Pour l’ensemble des formes géométriques simples il est possible de définir un
modèle paramétrique donc cette technique ne pose pas de problème. Cependant
pour des objets plus complexes il n'existe pas toujours de modèle mathématiques
simples. On peut alors imaginer décomposer l'objet en formes simples puis inclure
dans un second temps des contraintes entre ces formes élémentaires. Par exemple si
l'on souhaite détecter un cube on peut chercher chacune de ces faces, des plans, et
25ensuite vérifier qu'elles sont bien perpendiculaires ou parallèles les unes aux autres.
L’avantage de cette méthode est qu’elle permet de s’appuyer sur les techniques de
détection classiques. En revanche elle n’utilise pas toute l’information disponible
directement pour la détection.
2. Mise en œuvre: cas des objets plans
Ici on se limite au cas des objets plans. Ceci se justifie du fait de
l’environnement urbain dans lequel se trouve le robot. Les objets ont en majorité des
formes simples composées de faces planes : sol, murs, portes, meubles, ordinateurs
etc. On cherche à identifier un plan dans un nuage. Il faut trouver l'ensemble des
points dont les positions remplissent le critère:
) + PQ + + R = 0
Pour a, b, c et d choisis.
nuage permettant de calculer un premier jeu de paramètres du modèle (a, b, c et d).
La première étape consiste à tirer un nombre de point minimal (trois) dans le
coordonnées sont les paramètres a, b et c. d est identifié en calculant ) + PQ +
Avec ces trois points est formé le vecteur perpendiculaire au plan dont les
pour l’un des points. On évalue alors le nombre de point qui concordent avec le
modèle. Pour cela on vérifie que les coordonnées de chaque point candidat
respectent l’équation paramétrique (à un seuil près). Si c’est le cas alors le point est
ajouté à l’ensemble et les coefficients du plan sont réévalués. L’évaluation des
coefficients du plan optimal pour un ensemble de point se fait en plusieurs étapes.
Premièrement on calcule le barycentre du nuage
W
1
= ? 8
"
AXJ
Où pi est la position d’un point.
Ensuite on calcule la matrice :
ГW (, ) ГW (, Q) ГW (, )
Y = ZГW (Q, ) ГW (Q, Q) ГW (Q, )\
ГW (, ) ГW (, Q) ГW (, )
Où ГW (), P) = ∑WA() − ] )(P − ^ ) .
Le vecteur normal optimal à ce plan est le vecteur correspondant à la valeur
propre la plus petite de la matrice C. Finalement on trouve le coefficient d comme
précédemment en utilisant le centre de masse comme point. L’ajout de point se fait
de manière itérative jusqu’à ce qu’il n’y ait plus aucun point à ajouter. Si le nombre
de point est trop faible le modèle est rejeté pour éviter les fausses détections. Sinon
26Vous pouvez aussi lire
