Cartographie et Localisation Simultanées d`un Robot

Introduction à la Recherche en Laboratoire
Cartographie et Localisation
Simultanées d'un Robot Golfeur
Présenté par
Sink Yeow CHEW
Préparé au sein du
Laboratoire G-SCOP
Du 1 Février 2016 au 2 Mai 2016
Encadré par
Gautier STAUFFER
Thierry FRAICHARD
Remerciement
Je souhaite tout d’abord remercier Gautier STAUFFER, professeur à Grenoble INP Ensimag
et chercheur au laboratoire G-SCOP, de m’avoir accepté dans son équipe de robot golfeur..
Ses qualités professionnelles, sa pédagogie et son optimisme ont été les facteurs dominants
pour réussir dans ce projet. Il a toujours été disponible pour m’aider à avancer au mieux dans
mon travail. En plus, il m’a tout le temps fait confiance, j’avais donc une réelle volonté de ne
pas le décevoir. Cette confiance était pour moi une source de motivation aussi importante que
la réussite de mon projet.
Je suis également reconnaissant à Thierry FRAICHARD pour l’idée de partir dans la direction
de SLAM. Il a parfaitement complété mon encadrement avec sa gentillesse et sa générosité de
partager ses expériences en robotique. Les discussions que nous avons eues sont les clefs de
réussite de ce projet.
Je pense avoir eu des encadrants exceptionnels et j’en ai conscience. C’est pour cela que je
leurs en suis extrêmement reconnaissant.
Il y a aussi ceux que j’ai côtoyés ou avec lesquels j’ai travaillé pendant ces trois mois, merci à
Alexandre CAULIER, Hugo MATHIAS et Yassine YAAKOUBI. Toutes ces personnes
m’ont apporté bien plus qu’elles ne le pensent et je les remercie. J’ai aimé la super ambiance
dans la salle.
Enfin, je remercie toutes personnes qui ont partagé avec moi cette aventure et qui a toujours
été à mes côtés depuis trois mois.
1
Sommaire
1.
2.
Introduction ................................................................................................................................... 3
1.1.
Contexte de travail ................................................................................................................ 3
1.2.
Problématique........................................................................................................................ 3
Etat de l’art .................................................................................................................................... 4
2.1.
2.1.1.
Ultrasons et Infrarouge .................................................................................................... 4
2.1.2.
Télémètre et système de triangulation ............................................................................. 5
2.2.
3.
Utilisation des balises ............................................................................................................ 4
Cartographie et Localisation Simultanées ............................................................................... 7
2.2.1.
Introduction à la méthode SLAM .................................................................................... 7
2.2.2.
SLAM visuel ................................................................................................................... 7
ORB-SLAM ................................................................................................................................... 9
3.1.
Principe.................................................................................................................................... 9
3.2.
Utilisation de l’ORB –SLAM sous ROS ............................................................................... 10
3.2.1.
Introduction au Robot Operating System (ROS) ........................................................... 10
3.2.2.
Notion de bases du ROS ................................................................................................ 10
3.2.3.
Intérêt de l’utilisation du ROS ....................................................................................... 11
4.
Implantation du système de positionnement sous ROS................................................................. 11
5.
Résultats et interprétation .......................................................................................................... 14
6.
7.
8.
5.1.
Conditions de l’expérience .................................................................................................... 14
5.2.
Visualisation sous rviz.......................................................................................................... 14
5.3.
Optimisation des nuages de points ........................................................................................ 15
5.4.
Reconstruction du terrain ...................................................................................................... 16
Conclusion .................................................................................................................................... 17
6.1.
Limites ................................................................................................................................... 17
6.2.
Comparaison à l’état de l’art ................................................................................................. 17
6.3.
Améliorations possibles ........................................................................................................ 17
Annexes......................................................................................................................................... 18
7.1.
Annexe A : Détails de calculs du système de triangulation................................................... 18
7.2.
Annexe B : Calibrage de la caméra ....................................................................................... 19
Bibliographie................................................................................................................................ 23
2
1. Introduction
1.1.
Contexte de travail
Ce projet a été réalisé dans le cadre du module Introduction à la Recherche en
Laboratoire (IRL) dans le laboratoire G-SCOP. Ce sujet a été proposé et encadré par Gautier
STAUFFER, chercheur au GSCOP.
Le but est de développer un robot capable de se repérer sur un green et de faire rentrer
la balle dans le trou. Dans un premier temps, le robot a été décomposé en plusieurs blocs
fonctionnels tels que la motricité, le système de positionnement et le mécanisme de vision
permettant de repérer la balle de golf et le trou. Nous sommes 3 étudiants chacun focalisant
sur les différentes fonctions citées précédemment. J’ai choisi de m’occuper du système de
positionnement parce que j’ai déjà eu des expériences lors de la participation à la Coupe
Robotique de la France.
La première partie de ce document est consacrée à l’état de l’art sur les techniques
permettant de localiser un robot mobile dans un environnement inconnu. Nous verrons de
manière théorique les principaux systèmes existants, leurs avantages et les inconvénients.
Ensuite, nous étudierons en détail un système basé sur la méthode SLAM (Cartographie et
Localisation Simultanées) qui sera utilisée dans notre projet.
1.2.
Problématique
Un robot mobile est une machine capable de se déplacer de manière semi-autonome ou
complètement autonome quel que soit l’endroit où elle se trouve.
Dans le cadre de ce projet, nous avons pour but de localiser notre robot golfeur sur un
green inconnu. Le robot doit être capable de se déplacer sur un green tout en cherchant la
balle de golf et le trou. Donc, à tout moment de sa navigation, nous avons besoin de connaitre
notre position, celle de la balle et du trou par rapport à un repère.
Cette problématique fait souvent le sujet de recherche dans les compétitions
robotiques par exemple la Coupe Robotique de la France. Dans la plupart des cas, la
résolution de ce problème est basée sur l’utilisation de l’odométrie ou des balises comme les
repères. Nous verrons par la suite les avantages et les inconvénients de ces systèmes. En
même temps, nous étudierons la problématique de SLAM qui ressemble beaucoup à la nôtre.
3
2. Etat de l’art
2.1.
Utilisation des balises
2.1.1.
Ultrasons et Infrarouge
Principe
Source [ 1]:
Sébastien GROS, “Système de
positionnement pour un robot mobile”,
Club ROBOTIK2000, 2007
Figure 1 : Schéma de principe d'un système à ultrason
Nous avons besoin de 2 signaux : 1 signal ultrasonique Su dont la vitesse de propagation Vu
𝑉𝑆𝑢 ≈ 340 𝑚 𝑠 −1
et 1 signal infrarouge SIR dont sa vitesse est très supérieure à celle des ultrasons.
A partir du robot, on envoie en même temps les 2 signaux dans toutes les directions. Etant
plus rapide que le signal ultrasonique Su, le signal infrarouge SIR atteindra les balises avant le
signal Su. Ceci a pour but de signaler aux balises le départ du signal Su.
Ti
t
Signal SIR reçu
Signal Su reçu
Figure 2 : Mesure de temps Ti
Après la réception du signal SIR, on attend pour recevoir les ultrasons afin de déterminer le
temps, Ti mis par le signal Su pour arriver jusqu’à la balise i. Ensuite, cette donnée est
renvoyée vers le robot. Ayant connu le temps de parcours des ultrasons et leur vitesse dans
l’air, nous pouvons donc déterminer les distances Di séparant le robot et la balise i.
𝐷𝑖 = 𝑇𝑖 ∗ 𝑉𝑆𝑢
4
Cette distance Di calculée peut être vue comme le rayon du cercle ayant la balise i comme le
centre. Afin de déterminer les coordonnées (X,Y) du robot, il suffit de résoudre l’équation à 2
inconnus suivante:
(𝑋 − 𝑥𝑖 )2 + (𝑋 − 𝑦𝑖 )2 = 𝐷𝑖 2
xi : Coordonnée X de la balise i
y i : Coordonnée Y de la balise i
Avantages et inconvénients
Cette solution a pour avantages d’être simple à mettre en œuvre et les composants
électroniques sont courants et bons marchés. Il n’y a pas de communication entre les balises,
ce qui évite d’avoir à mettre en place une liaison ou un protocole de communication évolué.
En revanche, dans le contexte de notre projet, le système de positionnement dont nous avons
besoin doit être capable de localiser notre robot sur un terrain qui ne sera pas forcément tout
plat. En plus, ce système à ultrasons ne nous permet pas de positionner notre robot sur un
grand green car la portée des ultrasons est de l’ordre 0.2m à 5m. Donc, la précision du
système dépend du nombre de récepteur (balises) qu’il est possible d’installer.
2.1.2.
Télémètre et système de triangulation
Principe
Le principe consiste à déduire le positionnement d’un robot situé autour des balises dont on
connait la position.[ 1] Pour ce faire, nous allons faire tourner un télémètre (partie mobile) à
vitesse constante sur le robot. L’utilisation des moteurs pas à pas permet d’obtenir une grande
précision sur l’angle de rotation. Le système de positionnement du robot aura l’air suivant.
0
0
Télémètre
Moteur pas à pas
Figure 3 : Modèle du robot équipé d’un télémètre
Ensuite, les balises sont équipées des photodiodes. Il s’agit d’un composant électronique
ayant la capacité de détecter un faisceau lumineux et de le transformer en signal électrique.
Lorsqu’une balise détecte le passage du laser, elle renvoie l’information au système de
localisation situé sur le robot qui peut ensuite en déduire les angles entre les balises grâce au
moteur pas à pas.
5
Lorsqu’au moins 2 balises détectent le passage du laser, l’ensemble de robot et des balises
forme un triangle.
Balise
B1
W
i
d1
r
d2
k
U
p
q
(0,0)
s
m
Robot
V
Origine
considérée
B2
j
o
d3
t
B3
Figure 4 : Principe du système de triangulation
Puisque la position des 3 balises, l’angle de rotation des émetteurs laser (i,j,k,o,p,q,r,s,t) et les
distances entre les balises (V,W,U) sont connus, il nous reste à connaitre les distances d1, d2 et
d3 afin d’en déduire la position du robot par rapport à ces 3 repères. Ces distances peuvent
être obtenues facilement à l’aide du télémètre.
Grace à ces données, nous pouvons procéder au calcul de la position (x,y) du robot suivant les
différents triangles. On remarque qu’il suffit de ne calculer qu’un seul des trois angles pour
connaître (x,y). Mais, par souci de précision, il est conseillé de faire une moyenne sur au
moins 2 valeurs de (x,y). Les détails de calculs sont précisés en Annexe A.
Avantages et inconvénients
Ce système est d’une part très peu sensible à l’interférence, et d’autre part permet d’obtenir à
la fois les angles entre les balises et les distances séparant le robot et les balises avec une
grande précision. Nous avons la possibilité de déterminer l’orientation du robot si nous
sommes capables de connaître l’angle entre une balise et l’avant du robot, ce qui revient à être
capable de connaître à tout instant l’orientation du laser par rapport à l’avant du robot.
En revanche, cette solution est compliquée à mettre en œuvre car contrairement au système
précédent, elle nécessite une communication non pas seulement entre les balises et le robot,
mais aussi entre les balises elles-mêmes. De plus, un télémètre de bonne qualité est de prix de
l’ordre 200 euros, ce qui est très cher par rapport à la solution précédente, sans compter le prix
des autres composants électroniques nécessaires à implanter ce système de positionnement.
6
2.2.
2.2.1.
Cartographie et Localisation Simultanées
Introduction à la méthode SLAM
La Cartographie et Localisation Simultanées, connue sous le nom anglais SLAM
(Simultaneous Localization And Mapping) permet à un robot autonome de se localiser dans
un environnement inconnu et de construire et améliorer une carte de l’environnement. La
problématique de SLAM peut être décomposée en 2 sous-problèmes :
 Représenter l'environnement à partir de l'interprétation des données fournies par les
capteurs (ex : caméra, odomètre)
 La localisation du robot par rapport à un plan.
Evidemment, résoudre ces 2 problèmes consiste à traiter le paradoxe de la poule et de l'œuf
car nous avons besoin d’une carte pour définir la localisation et la position est nécessaire pour
construire une carte.
Récemment, de nombreuses recherches ont été faites, ayant pour but de traiter la
problématique de SLAM. Les approches les plus couramment utilisées sont basées sur des
méthodes d’estimation statistiques. Il s’agit des filtres statistiques permettant d’estimer l’état
d’un système dynamique grâce aux données fournies par les capteurs. Les SLAMs sont
catégorisés selon les algorithmes de calcul appliqués, le type d’informations reçue (ex :
images, laser), la représentation de l’environnement, etc. Par exemple, coreSLAM est famille
de SLAM capable de localiser un robot et construire la carte de l’environnement à partir des
données de laser.[ 3]
Dans le cadre de ce projet, nous avons choisi les algorithmes basés sur la vision par
ordinateur à l’aide d’une caméra.
2.2.2.
SLAM visuel
L’ajustement de faisceaux (Bundle Adjustment) est une méthode connue capable de
fournir la localisation de caméra avec une bonne précision [ 2]. Le PTAM est l’une des
premières familles de SLAM basées sur l’application temps réel de l’ajustement de
faisceaux.[ 4] Il permet de faire une sélection d’image et de trouver une correspondance entre
les images. Après avoir effectué une triangulation des points, il est capable de localiser la
caméra voire la relocaliser après que ci-dernière se perde dans l’espace. Or, l’application de
cet algorithme est assez limitée dû à un manque de détection de fermeture de boucle (loop
closure detection). La détection de fermeture de boucle est un élément crucial de la robustesse
du SLAM. Il consiste à détecter si le robot repasse par l’endroit où il est passé précédemment.
Cela nous permet d’améliorer la précision de la position actuelle du robot.
Dans le cadre de ce projet, nous avons choisi d’utiliser l’ORB-SLAM. Il s’agit d’une
autre famille de SLAM ayant toutes les propriétés essentielles présentées dans le PTAM. En
plus, des nouvelles fonctionnalités sont également ajoutées. Nous allons les préciser dans le
prochain chapitre.
7
Avantages et inconvénients
La méthode de l’ORB-SLAM est une problématique qui a déjà été traitée avec des résultats
garantis de bonne précision. Par rapport aux systèmes de positionnement présentés
précédemment, cette méthode ne nécessite qu’une caméra (dans le cas d’un mono SLAM), ce
qui est beaucoup plus simple à implanter en termes de matériel. En plus, il existe un module
de l’ORB-SLAM publié en open source et il est prêt à être employé.
En revanche, l’ORB-SLAM nécessite un processeur suffisamment puissant afin d’effectuer le
traitement des images et les calculs complexes. Cela implique que l’algorithme de SLAM sera
lancé dans un ordinateur mais pas sur un microcontrôleur d’Arduino. Les résultats seront
ensuite envoyés vers le robot à l’aide d’un système de communication comme bluetooth ou
wifi.
8
3. ORB-SLAM
3.1.
Principe
L’ORB signifie en anglais Oriented Fast and Rotated BRIEF, qui est un détecteur visuel
couramment utilisé dans la vision par ordinateur [ 5]. Un détecteur visuel permet de détecter
et de décrire les caractéristiques élémentaires d’une image : la forme, la couleur, etc
Figure 5 : Fonctionnement de l’ORB SLAM
Source:
Raul Mur-Artal, J. M. M.
Montiel and Juan D. Tardos
“ORB-SLAM: a Versatile and
Accurate Monocular SLAM
System”, IEEE, 2015
L’ORB-SLAM [ 6] consiste à recevoir en entrée les images et exécuter 3 threads en parallèle:
la localisation(Tracking), la construction de carte (Local Mapping) et la détection de
fermeture de boucle (Loop Closing).
La Tracking permet de localiser la caméra à partir des images. Ce thread décide également si
une nouvelle image sera considérée comme la référence ou rejetée. Dans le cas où la Tracking
perd sa référence, le module de reconnaissance (Place Recognition) sert de refaire une
localisation globale afin de retrouver la position de la caméra.
La Local Mapping a pour objectif de traiter les nouvelles images de référence. Elle permet de
faire une correspondance entre les nouveaux points ajoutés en faisant une triangulation. Lors
de cette étape, un algorithme d’élimination est appliqué dans le but de ne garder que les points
de bonnes qualités selon les critères de Recent Map Points Culling [ 6].
La Loop Closing recherche dans toutes les nouvelles images ajoutées s’il existe une
fermeture de boucle. Dans le cas où une fermeture est détectée, les 2 terminaux de boucle sont
alignés et les points redondants sont fusionnés. Cela nous permet d’estimer l’erreur accumulée
et de corriger l’erreur.
9
3.2.
Utilisation de l’ORB –SLAM sous ROS
3.2.1. Introduction au Robot Operating System (ROS)
Robot Operating System (ROS) est un système d'exploitation complet pour le service
robotique [ 7]. Aujourd’hui, Ubuntu est le seul système officiellement supporté. De nombreux
packages sont publiés pour ROS avec diverses licences open source (BSD, MIT). Ces
packages permettent de faire fonctionner des applications, des algorithmes ou encore des
programmes pour interfacer ROS avec des robots.
Le principe de base du ROS est de lancer en parallèle un grand nombre d'exécutables qui
peuvent échanger de l'information de manière synchrone ou asynchrone.
3.2.2. Notion de bases du ROS
Cette partie du document est consacrée à l’explication des notions de bases du ROS. Les mots
de clef comme le Master, le Node, les topics, les messages et les bags seront réutilisés dans la
suite du document.
Source :
Robot Operating System
https://fr.wikipedia.org/wiki
/Robot_Operating_System
Figure 6 : Communication des nœuds via les topics
 Le Master est le maitre contrôleur qui permet à des nœuds de se connaître et
d'échanger de l'information via les topics.
 Un Node est une instance d'un exécutable. Il peut correspondre à un capteur, un
moteur, un algorithme de traitement, etc.
 Un Topic est un canal de communication basé sur le système de publication et
d’abonnement (publication and subscription). ROS permet à un ou plusieurs nœuds de
publier de l'information sur un topic. En même temps, il peut y avoir un ou plusieurs
autres nœuds qui lisent de l'information sur ce topic.
 Les Messages sont les informations qui sont publiées dans un topic. Il est composé
des types élémentaires (entiers, flottants, chaînes de caractères, etc) ou d’autres
messages.
 Les Bags sont des formats destinés à stocker et rejouer les messages publiés dans un
topic.
Sur un schéma décrivant les relations entre les nœuds et la publication des messages dans les
topics, un nœud est représenté par un cercle alors qu’un rectangle correspond à un topic. Une
flèche partante d’un nœud A vers un topic B signifie la publication des messages du nœud A
dans le topic B. Cette notation sera utilisée dans la suite de ce document.
10
3.2.3. Intérêt de l’utilisation du ROS
L’ORB-SLAM que nous avons abordé précédemment nécessite de travailler sous
l’environnement de ROS. Il a été testé sous les versions Fuerte, Groovy, Hydro et Indigo.
Dans le cadre de ce projet, nous avons choisi la version Indigo car elle est la plus récente et la
plus stable parmi les 4 citées ci-dessus [ 7].
L’idée principale d’un OS robotique est de proposer des fonctionnalités standardisées faisant
abstraction du matériel, tout comme un OS classique pour PC. Dans notre cas, l’utilisation du
ROS nous permet de communiquer entre les différents blocs fonctionnels du robot golfeur: la
motricité, le système de positionnement et le mécanisme de vision. Nous pouvons ainsi
développer chacune de ces 3 parties indépendamment à condition que les bons types de
messages soient publiés sur le bon topic.
4. Implantation du système de positionnement sous ROS
Nœud caméra
Nœud ORB
SLAM
Nœud convertisseur
markers-pointcloud
Nœud convertisseur
pointcloud-pointcloud2
Sauvegarde
pcd
Figure 7 : Système de positionnement sous ROS
Le principe du système de positionnement est de cartographier le terrain sous forme des
nuages de points à partir des images reçues. Ces nuages de points seront traités ailleurs afin de
produire un modèle du terrain qui pourra ensuite servir dans la simulation. Le schéma cidessus montre l’ensemble des nœuds exécutés et des topics établis.
11
i.
Nœud caméra (/usb_cam) :
Rôle
Il communique avec les webcams standards et publie les images sous forme
sensor_msgs::Image.
Abonnement
Rien
Publication
/usb_cam/image_raw
Avant l’utilisation, une caméra doit être calibrée dans l’objectif de fournir une bonne
précision. Dans le cadre de notre projet, la caméra utilisée est la webcam Logitech C270.
Les explications théoriques sur le modèle sténopé et les démarches techniques du
calibrage sont précisées en Annexe B.
ii.
Nœud ORB SLAM :
Rôle
Etant fourni des images du terrain, l’ORB SLAM extrait ses caractéristiques ORB.
Lorsqu’il découvre des nouveaux coordonnées, une nouvelle carte de référence
prenant en compte des nouveaux repères sera publiée dans le topic
/ORB_SLAM/Map. Cette référence, sauvegardée internement sous le module de
reconnaissance (Place Recognition) (Figure 5), est renouvelé à tout moment lorsqu’il y
a de nouvelles images ajoutées. La position actuelle de la caméra est envoyée dans le
topic /tf. Ces informations sont importantes pour pouvoir visualiser la création de la
carte sous l’afficheur rviz.
Abonnement
/usb_cam/image_raw
Publication
/ORB_SLAM/Map
/ORB_SLAM/Frame
/tf
12
iii.
Nœud convertisseur markers-pointcloud (/marker2pointcloud) :
Rôle
A la sortie de l’ORB SLAM, la carte de référence est publiée sous forme des markers
qui ne servent que pour l’affichage. Or, ce qui nous intéresse dans le cadre de ce projet
est de pouvoir les manipuler voire les importer dans les logiciels de simulation
physique comme Gazebo. L’exécution de ce nœud a pour objectif d’extraire les
coordonnées 3D des markers et fabriquer une première forme de nuages de points.
Abonnement
/ORB_SLAM/Map
Publication
/marker2pointcloud/PointCloud
iv.
Nœud convertisseur pointcloud-pointcloud2 (/point_cloud_converter) :
Rôle
Le ROS possède son propre type de nuages de points : sensor_msgs/PointCloud et
sensor_msgs/PointCloud2. Ils sont de l’origine de la bibliothèque de nuages de points
(PCL : Point Cloud Library). Depuis des années, la PCL a remplacé le type
PointCloud par PointCloud 2 à cause de son inefficacité [ 8].
Ce deuxième convertisseur de nuages de point a pour objectif de convertir les
messages sensor_msgs/PointCloud en sensor_msgs/PointCloud2 pour pouvoir utiliser
les fonctions qui nécessitent de ne travailler qu’avec PointCloud 2 sous PCL.
Abonnement
/marker2pointcloud/PointCloud
Publication
/point_cloud_converter/PointCloud2
v.
Sauvegarde en PCD :
Sauvegarder les nuages de points dans les fichiers PCD [ 9] qui seront réutilisés dans
la suite.
Abonnement
/point_cloud_converter/PointCloud2
Publication
Rien
13
5. Résultats et interprétation
5.1.
Conditions de l’expérience
L’expérience a été conduite à l’extérieur sur de l’herbe. Dans un premier temps, nous ne nous
occupons du terrain quasi plat d’aire 8m*8m. La caméra utilisée est la webcam Logitech
C270 (3Mp) dont la qualité des images (publiées à 30 fps) dépend beaucoup de l’intensité de
lumière. L’ensemble du système de positionnement est implanté sur le processeur Intel(R)
Core(TM) i5-2410M [email protected].
5.2.
Visualisation sous rviz
La Figure 8 montre une image publiée dans le topic /ORB_SLAM/Frame. Nous constatons qu’elle est
Figure 8 : Image du terrain
Figure 9 : Représentation 3D de la carte
sous rviz
noire et blanche. Cela vient du fait que l’ORB SLAM ne garde que les caractéristiques ORB. A part
cela, les markers verts sur l’image représentent les coordonnées ORB qui ont été extraites de l’image.
Ces repères sont rangés dans le module de reconnaissance (Place Recognition) (Figure 5), qui
sera mis à jour pour chaque nouvelle image.
Ces mêmes repères sont représentés sur la Figure 9 dans un espace 3D. Les points en rouge
signifient les nouveaux caractéristiques ORB détectés. Les noirs représentent les coordonnés
qui ont été découverts et gardés comme la référence de la cartographie. Lors de l’insertion
d’une nouvelle image, la position actuelle de la caméra (cadre vert) est mise à jour sur la
carte. Les cadres bleus représentent le parcours que la caméra a suivi.
Figure 10 : Détection de fermeture de boucle
14
Comme expliqué précédemment, la référence de l’ORB SLAM est enregistrée dans le
module intégré Place Recognition. Cette référence est corrigée tout au long du parcours de la
caméra dans l’espace. Les nouveaux points insérés doivent satisfaire les conditions citées dans
Recent Map Points Culling [ 6]. En plus de cela, la détection d’une fermeture de boucle (loopclosure) permet d’estimer l’erreur accumulée et ainsi corriger l’erreur. La Figure 10 montre le
cas où une fermeture est détectée. Les 2 terminaux de boucle sont alignés et les points
redondants sont fusionnés. Par conséquent, nous observons qu’un nombre significatif de
points ont été éliminés.
5.3.
Optimisation des nuages de points
Vu de caméra
Figure 12 : Mur collé des papiers pointés
Figure 11 : Nuage de points
représentant le mur
Dans le cadre de ce projet, nous nous intéressons à la création d’une carte en construisant la
surface du terrain à partir des nuages de points. Or, afin d’obtenir une bonne précision, l’ORB
SLAM a besoin de repasser plusieurs fois par le même endroit et de refaire une estimation.
Donc, il se peut que les nuages de points créés à la première vue de la caméra ne soient pas
corrects. Il peut également arriver que l’ORB SLAM publie sa carte de référence sans que ci
dernière ne soit corrigée.
La Figure 12 montre l’expérience ayant pour but de vérifier l’hypothèse ci-dessus. Pour ce
faire, nous avons collés des papiers pointés sur un mur et ensuite parcouru la caméra sur les
zones de papiers. Sur la Figure 11, nous constatons que les points sont concentrés autour d’un
plan. On peut en déduire que ce plan représente le mur. En plus de cela, nous avons également
constaté qu’il existe des points qui ne sont pas appartenus au mur. Ceci peut être expliqué par
l’imprécision introduite par les points qui ne sont pas encore corrigés.
L’imprécision ci-dessus pose des problèmes lorsque nous allons faire une triangulation des
points. Pour la remédier, nous avons procédé par la méthode d’extraction de plan (Plane
model segmentation) [ 10]. Cette technique consiste à extraire tous les points qui se trouvent
sur un plan avec une épaisseur DistanceThreshold. La distance DistanceThreshold détermine
l’écart maximal entre un point et le plan considéré pour être pris en compte. A la fin du calcul,
nous n’allons garder que le plan contenant le plus de points.
15
5.4.
Reconstruction du terrain
Figure 13 : Représentation du terrain
3D
Figure 14 : Existence d’une bosse sur
le terrain 3D
L’étape finale dans notre système de positionnement consiste à reconstruire la surface du
terrain à partir de la carte de référence présentée sous forme des nuages de points Il existe
sous la bibliothèque de PCL une fonction permettant de trianguler les nuages de points [ 11].
La Figure 13 désigne la surface de l’herbe sur laquelle la caméra a fait son trajet. Nous
observons que la distribution de triangle n’est pas tout à fait équilibrée. Le centre de la carte
est plus dense par rapport à ses côtés parce que le fait de faire plusieurs tours permet à la
caméra de revoir et corriger la partie milieu de la carte plus souvent que ses côtés. Par
conséquent, la surface de cotés n’est pas plate, ce qui ne correspond pas à la réalité.
La Figure 14 montre les bosses que nous avons observés lorsqu’un zoom est fait sur la Figure
13. Ces bosses représentent la condition actuelle du terrain où il existe des cailloux ou des
zones de l’herbe qui sont plus élevées que les autres.
16
6. Conclusion
Dans le cadre de ce projet, nous avons développé un système de positionnement pour un robot
golfeur en nous basant sur l’ORB SLAM [ 6]. Il est démontré que notre système est capable
de reconstruire approximativement la surface du terrain. La précision dépend fortement de
celle de l’ORB SLAM. L’expérience montre que l’ORB SLAM pourra atteindre la précision
de l’ordre 1cm dans une salle et de l’ordre quelques mètres quand le taille du terrain
augmente.
6.1.
Limites
Comme la précision, les limites de notre système reste celles de l’ORB SLAM [ 6]. Dans la
plupart des cas, nous retrouverons les résultats ressemblant à ceux que nous avons obtenus
lors de l’expérience. Or, dans les cas suivants, notre système pourrait se comporter
différemment des résultats présentés précédemment.


Pas de translation ou trop de rotation pure dans l’espace lors de l’initialisation du
système
Pure rotation lors de la navigation dans l’espace
En plus de cela, lors de l’expérience, nous avons fait l’hypothèse que notre terrain de l’herbe
est quasiment plat, d’où nous avons filtré les nuages de points par rapport à un plan. Nous
n’avons pas testé notre système avec toutes les conditions du terrain (bosses, trou etc). Le
comportement du système ne sera donc pas garanti.
6.2.
Comparaison à l’état de l’art
Les techniques présentées dans l’état de l’art sont des systèmes électroniques ayant besoin de
la mise en œuvre des balises électroniques. Dans le cas de notre projet, seule une caméra est
nécessaire pour capter les images. Mais, l’efficacité de notre système dépend fortement de la
qualité des images fournies par la caméra.
L’utilisation des balises dans l’état de l’art implique la prédéfinition de la taille du terrain.
Dans le cas du système à ultrasons [ 1], la taille du terrain est limitée à 5m*5m. Or,
l’expérience montre que notre système fonctionne bien sur un terrain de 8m*8m.
Nous n’avons pas pu comparer exactement la précision de notre système et celle de l’état de
l’art car elle n’est pas détaillée dans la documentation [ 1].
6.3.
Améliorations possibles
La performance de notre système peut être améliorée selon les différentes méthodes de SLAM
utilisées. L’ORB SLAM 2 sorti en Janvier 2016 possède les mêmes caractéristiques que
l’ORB SLAM utilisé dans notre projet [ 12]. En plus, l’ORB SLAM 2 applique un ajustement
de faisceaux complet. L’extraction des caractéristiques ORB est améliorée et le procès de la
tracking prendra moins de temps.
A part cela, la construction de la surface de terrain devrait être améliorée en affinant les
détails de surface (trou, bosse etc).
Finalement, dans le but d’adapter notre robot et d’améliorer la précision du système, nous pouvons
effectuer une simulation de terrain avec le robot à l’aide de l’outil Gazebo [ 13].
17
7. Annexes
7.1.
Annexe A : Détails de calculs du système de triangulation
Triangle (B1, R, B3)
𝑤
𝑥 = + cos(180 − (𝑝 + 𝑞)) ∗ 𝑑3
2
𝑦 = sin(180 − (𝑝 + 𝑞)) ∗ 𝑑3
Triangle (B2, R, B3)
𝑤
𝑥 = + cos(𝑜 + 𝑡 ) ∗ 𝑑3
2
𝑦 = sin(𝑜 + 𝑡 ) ∗ 𝑑3
Triangle (B1, R, B2)
𝑥 = cos(𝑖) ∗ 𝑑1
𝑦 = L + sin(𝑖) ∗ 𝑑1
18
7.2.
Annexe B : Calibrage de la caméra
Objectif
La calibration d'une caméra a pour but de déterminer la relation mathématique entre les
coordonnées 3D observée via la caméra et les coordonnées 2D projetés sur son image
correspondante. Afin de fournir des mesures dimensionnelles précises en entrée de l’ORB
SLAM, il est indispensable de prendre en compte des distorsions induites par le système
optique de la caméra.
Modèle sténopé
Calibrer une caméra consiste à la modéliser. Aujourd’hui, il existe plusieurs modèles décrivant
le processus de formation des images dans une caméra. Le modèle sténopé (pin-hole) est le
modèle le plus couramment utilisé.
𝑢
𝑓𝑥
𝑠 [𝑣 ] = [ 0
1
0





0
𝑓𝑦
0
𝑐𝑥 𝑟11
𝑐𝑦 ] [𝑟21
1 𝑟31
𝑟12
𝑟22
𝑟32
𝑟13 𝑡1 𝑋
𝑟23 𝑡2 ] [𝑌 ]
𝑟33 𝑡3 𝑍
1
(X, Y, Z) : Coordonnées des points dans l’espace 3D
(u, v) : Coordonnées des points projetés sur l’image 2D
S : Factor d’agrandissement de l’image
Matrice [FC] modélise les paramètres intrinsèques de la caméra, d’où
 (cx, cy) : Centre de l’image
 fx, fy : Distances focales exprimées en pixels
Matrice [RT] représente les paramètres extrinsèques de la caméra.
Dans ce modèle, on cherche à estimer les paramètres intrinsèques de la caméra, et sa position
et orientation par rapport au repère du monde réel (paramètres extrinsèques). La matrice [RT]
décrit le mouvement de la caméra dans un environnement statique. Elle transforme les
coordonnés (X, Y, Z) en (x, y, z). En tenant en compte des distorsions, cette transformation
peut être exprimée sous forme :
𝑥
𝑋
[ 𝑦 ] = [𝑅 ] [ 𝑌 ] + [𝑇 ]
𝑧
𝑍
19
Avec
𝑦′ =
𝑦
𝑧
′′
𝑥 = 𝑥′
𝑦 ′′ = 𝑦
1+𝑘1 𝑟 2 +𝑘2 𝑟 4 +𝑘3 𝑟 6
1+𝑘4 𝑟 2 +𝑘5 𝑟 4 +𝑘6 𝑟 6
2
4
6
′ 1+𝑘1 𝑟 +𝑘2 𝑟 +𝑘3 𝑟
1+𝑘4 𝑟 2 +𝑘5 𝑟 4 +𝑘6 𝑟 6
2
+ 2𝑝1 𝑥 ′ 𝑦 ′ + 𝑝2 (𝑟 2 + 2𝑥 ′ )
2
+ 𝑝1 (𝑟 2 + 2𝑦 ′ ) + 2𝑝2 𝑥 ′ 𝑦 ′
2
2
𝑟2 = 𝑥′ + 𝑦′
𝑢 = 𝑓𝑥 𝑥 ′′ + 𝑐𝑥
𝑣 = 𝑓𝑦 𝑦 ′′ + 𝑐𝑦
Les coefficients ki représentent les distorsions radiales et les pi modélisent les distorsions
tangentielle. Dans notre cas, l’ORB SLAM ne demande que la précision des coefficients
jusqu’à l’ordre 2 (k1, k2, p1, p2).
Algorithmes de calibration
Aujourd’hui, il existe plusieurs moyens permettant de calculer ces paramètres. Nous résumons
ci-dessous les algorithmes les plus connus en traitement d’image.
No
1
2
3
4
Algorithmes
Méthode Transformation directe linéaire (DLT method)
Méthode de Zhang
Méthode de Selby **
Méthode de Tsai
Dans le cadre de notre projet, l’algorithme utilisé est l’approche de Zhang car il existe sous
l’environnement de ROS le paquetage camera_calibration dans lequel est intégré cet
algorithme, qui permet de calibrer les mono-caméras.
Calibration sous ROS
Protocole de calibration
1. Avant de commencer, il nous faut


un échiquier imprimé en noir et blanc. Dans notre cas, il s’agit d’un 8*6 échiquier avec les
carrés de coté 10.8 cm. [Annexe Checkerboard]
une caméra. J’ai utilisé la webcam Logitech C270.
L’expérience est faite dans un environnement sans obstacle.
2. Démarrer le maitre de ROS.
Roscore
20
3. Lancer le nœud permettant de publier les images dans le topic /usb_cam/image_raw. Pour
ce faire, j’ai écrit un fichier launch usb-cam.launch.
roslaunch usb-cam.launch
4. Maintenant, on peut démarrer le nœud camera_calibration en spécifiant les caractéristiques
de l’échiquier utilisé.
rosrun camera_calibration cameracalibrator.py --size NbR*NbC --square L
image:=/topic_qui_publie
NbR: Nombre de rangées
NbC: Nombre de colonnes
L : La largeur d’un carré en mètre
Dans mon cas, j’ai utilisé un 8*6 échiquier avec les carrés de largeur 4 cm.
rosrun camera_calibration cameracalibrator.py --size 8*6 --square 0.04
image:=/usb_cam/image_raw
Si tout se passe bien, une fenêtre s’ouvre comme suivant.
Figure 15 : Fenêtre du calibrage de caméra
Source:
How to Calibrate a Monocular Camera
http://wiki.ros.org/camera_calibration/
Tutorials/MonocularCalibration
Signification des barres X,Y,Skew et Size.
 X : Les informations récupérées sur l’axe X
 Y: Les informations récupérées sur l’axe Y
 Size : Les informations récupérées sur l’axe Z
 Skew : Les informations récupérées sur la rotation
4. Afin d’obtenir une bonne calibration, il faut déplacer l’image autour de la fenêtre avec
toutes les orientations possibles. Pour chaque déplacement de l’échiquier dans l’espace, on
verra l’avancement de l’accumulation des infos sur les barres X,Y,Skew et Size.
21
Une fois que les informations sont suffisantes pour les calculs, toutes les 4 barres seront en
vertes. On peut donc démarrer la calibration avec les données récupérées.
Figure 16 : Déplacement de l’échiquier autour de la fenêtre
Utilisation des résultats
Dès que la calibration est terminée, nous allons trouver les informations ci-dessous affichées
dans le terminal et aussi sauvegardées dans un fichier .yaml.
...
camera matrix
832.419750 0.000000
308.340942
0.000000 835.157342
230.981013
0.000000 0.000000 1.000000
distortion
0.013499 -0.189840 0.001753
0.001687 0.000000
…
Lors de l’expérience, je me suis rendu compte que les résultats varient selon l’intensité
de lumière. Donc, plusieurs expériences sont fait avec les mêmes grilles et la moyenne des
valeurs sera utilisée.
Finalement, on remplit le fichier ORB_SLAM/Data/Settings.yaml avec les informations cidessus.
22
8. Bibliographie
[ 1 ] Sébastien GROS, “Système de positionnement pour un robot mobile”, Club
ROBOTIK2000, 2007
[ 2 ] B. Triggs, P. F. McLauchlan, R. I. Hartley, and A. W. Fitzgibbon, “Bundle adjustment a
modern synthesis” Vision algorithms: theory and practice, 2000
[ 3 ] Oussama El Hamzaoui, “Localisation et cartographie simultanées pour un robot mobile
équipé d’un laser à balayage : CoreSLAM ”,École nationale supérieure des mines de Paris,
2014
[ 4 ] G. Klein and D. Murray, “Parallel tracking and mapping for small AR workspaces,” in
IEEE and ACM International Symposium on Mixed and Augmented Reality (ISMAR), Nara,
Japan, November 2007
[ 5 ] Ethan Rublee Vincent Rabaud Kurt Konolige Gary Bradski, “ORB: an efficient
alternative to SIFT or SURF”,Willow Garage, Menlo Park, California
[ 6] Raul Mur-Artal, J. M. M. Montiel and Juan D. Tardos “ORB-SLAM: a Versatile and
Accurate Monocular SLAM System”, IEEE, 2015
[ 7] ROS
https://fr.wikipedia.org/wiki/Robot_Operating_System
[ 8] Remplacement du PointCloud par PointCloud2
http://wiki.ros.org/pcl?action=AttachFile&do=get&target=PCL_March_2010.pdf
[ 9] Format PCD
http://pointclouds.org/documentation/tutorials/pcd_file_format.php
[ 10] Plane model segmentation
http://www.pointclouds.org/documentation/tutorials/planar_segmentation.php
[ 11] Alexandru-Eugen Ichim,“PCL - SURFACE RECONSTRUCTION”, TOYOTA CODE
SPRINT
[ 12] Raul Mur-Artal, Juan D. Tardos, J. M. M. Montiel and Dorian Galvez-Lopez, “RealTime SLAM for Monocular, Stereo and RGB-D Cameras, with Loop Detection and
Relocalization Capabilities”, Willow Garage, Menlo Park, California
[ 13] Simulateur physique Gazebo
http://wiki.ros.org/gazebo
23