SLAM Visuel Embarqué sur un Robot Quadrupède

Résumé du mémoire de soutenance pour le Diplôme d Ingénieur INSA de Strasbourg - Spécialité Mécatronique - Et le Master des sciences de l université de Strasbourg - Mention Imagerie, Robotique et Ingénierie pour le Vivant - Parcours Automatique et Robotique SLAM Visuel Embarqué sur un Robot Quadrupède Présenté par Anis MEGUENANI Réalisé au sein de l Istituto Italiano di Tecnologia Gênnes ITALIE Septembre 2013

Superviseurs: Dr. Stephane Bazeille Post-doc au sein de l équipe HyQ Department of Advanced Robotics Istituto Italiano di Tecnologia (IIT) Dr. Renaud KIEFER Professeur INSA de Strasbourg Dr. Jacques Gangloff Professeur Université de Strasbourg Dr. Claudio Semini Leadeur de l équipe HyQ Department of Advanced Robotics Istituto Italiano di Tecnologia (IIT) 2

Chapitre I Introduction 1. Context and motivation SLAM (SLAM signifie Localisation et Cartographie Simultanées) est une caractéristique fondamentale pour tout robot destiné à pouvoir naviguer de manière autonome. SLAM est composé de deux parties étroitement liées et complémentaires. La partie localisation permet de traquer les 6 degrés de liberté du robot dans son environnement en temps réel. La partie Cartographie quant à elle permet de récupérer les dimensions et les données géométriques des objets ou obstacles potentielles se trouvant à proximité du robot. Afin d aider HyQ, un robot quadrupède actionné hydrauliquement à naviguer de manière autonome, le but de ce travail durant mon stage de fin d étude à l Istituto Italiano di Tecnologia était de mettre en place un système de perception intégré pour terrains non structurés incluant des obstacles de tailles différentes pour permettre au robot de décider où précisément placer ses pattes et quel chemin emprunter pour atteindre son but. Afin d atteindre cet objectif, une caméra stéréo de type Bumblebee2 ainsi qu une Kinect ont été utilisés pour explorer et tester les algorithmes du SLAM sur HyQ. Le but étant de décider de la solution finale la plus adaptée pour une navigation autonome. Contrairement à la Kinect, la caméra stéréo est plus adaptée pour une utilisation en extérieur mais est cependant moins fiable pour la reconstruction dans des environnements non texturés. Durant ce projet, les packages open source RGBDSLAM et Kinect Fusion ont été testés et adaptés pour être utilisés avec la caméra stéréo au lieu de la Kinect et une approche de cartographie 3D customisée a été développée. Des cartes 3D utilisables pour la navigation ont alors pu être générées en utilisant ces différentes techniques. 1. Le projet HyQ Le projet HyQ 1 a été démarré en 2007 par le docteur Claudio Semini qui en est aujourd hui leadeur sous la supervision du directeur du département de la robotique avancée, le professeur Darwin Caldwell. Le projet est totalement financé par l Istituto Italiano di Tecnologia mais est aussi ouvert aux investissements externes. HyQ est un robot quadrupède actionné hydrauliquement dont toutes les liaisons sont contrôlées en force. Le robot est développé à l Istituto Italiano di Tecnologia (IIT) à Gênnes au sein du département de robotique avancée (voir Figure 1). HyQ a été construit dans le but de pouvoir se mouvoir sur différents types de terrains et de réaliser des tâches hautement dynamiques comme sauter ou courir à une vitesse maximum de 2 m/s. Le robot est utilisé aujourd hui comme plateforme de recherche pour investiguer plusieurs aspects de la locomotion quadrupède tels que la raideur active, le calcul automatique de la position optimale des pattes, calcul automatique du chemin à emprunter, efficacité énergétique, actionneurs hydrauliques embarqués. 1 HyQ est l abréviation de Hydraulic Quadruped 3

Figure 1 Le robot HyQ Chapitre II Les appareils d acquisition Afin de réaliser le SLAM sur HyQ, deux types d appareils d acquisition ont été utilisés. Le premier est la Kinect qui est considérée comme étant un appareil actif et le second une caméra stéréo Bumblebee2 considérée comme un capteur passif. 1. La Kinect La Kinect de chez Microsoft (voir Figure 2) englobe en réalité trois capteurs : Une caméra couleur d une résolution de 640 x 480, un projecteur infrarouge ainsi qu une caméra infrarouge de la même résolution. Une grille infrarouge de points est projetée dans la scène (voir Figure 3) puis par triangulation, en utilisant le capteur infrarouge, la distance de chaque point par rapport à la caméra est calculée. C est ainsi que la Kinect arrive à extraire les données 3D de l environnement cartographié. La fréquence des données 3D est de 20 fps. Figure 3 La Kinect de chez Microsoft Figure 2 Grille infrarouge projetée dans la scène En sortie direct, la Kinect fournit une image de profondeur (depth map) dont chaque pixel encode les données 3D d un point de la grille infrarouge projetée (Voir Figure 4). 4

Figure 4 Image couleur Figure 5 Image de profondeur 1. La camera stereo Bumblebee2 La Bumblebee2 de chez Point Grey est une caméra stéréo grand angle (100 ) qui fournit deux images rectifiées couleurs d une résolution de 1032 x 776 à une fréquence de 20 fps. Contrairement à la Kinect, la caméra stéréo ne génère pas directement de données 3D en sortie. Cependant, les coordonnées 3D des pixels commun entre l image droite et gauche peuvent être calculées avec des algorithmes de traitement d image. Le résultat peut être représenté sous forme d un nuage de points, voir Figure 6 et 7. Les dimensions des objets se trouvant dans la scène peuvent ainsi être évaluées. Durant ce projet deux algorithmes ont été utilisés pour générer les nuages de points : Le premier s appelle Bloc Matching (BM) et le second Sum of Absolute Differences (SAD). Figure 6 Nuage de points généré avec l algorithme Bloc Matching Figure 7 Nuage de points généré avec l'algorithm Sum of Absolute Differences La caméra stéréo est plus adaptée à une utilisation en extérieur que la Kinect. En effet, vu que le capteur de chez Microsoft projette une grille infrarouge dans l environnement cartographié, lorsque celui-ci est exposé à la lumière du soleil, il n est plus possible d en extraire des donnés 3D. D un autre côté, la Kinect est plus précise que la caméra stéréo en ce qui concerne les données 3D extraits. Chapitre III Méthodes et résultats Pour générer des cartes 3D, il faut concaténer les nuages de points pour former un plus grand modèle. Pour cela, la matrice de transformation entre chaque deux nuage de points consécutifs doit être 5

calculée. Dans ce qui suit on présente les différents outils et méthodes qui ont été utilisées pour ce but. 1. Le package RGBDSLAM Le package RGBDSLAM est un code open source disponible sur le site de ROS qui génère des cartes 3D en concaténant les nuages de points acquis. Pour générer un modèle 3D consistent, l algorithme prend en entré des nuages de points ainsi qu un flux d images couleurs (gauche ou droite). Figure 8 Entrées et sortie du package RGBDSLAM Pour concaténer deux nuages de points consécutifs, l algorithme calcule la matrice de transformation entre les deux en déterminant les correspondances entres des points d intérêts extraits de deux images consécutives avec l algorithme SURF puis en utilisant RANSAC (RANdom SAmple Consensus) pour ne garder que la meilleur transformation entre ces données 3D. On montre dans les figures suivantes les différents résultats obtenues avec caméra stéréo et la Kinect à travers le package RGBDSLAM. Figure 9 Carte 3D généré avec des nuages de points calculés avec l'algorithme Bloc Matching (BM) 6

Figure 10 Carte 3D généré avec des nuages de points calculés avec l'algorithme Sum of Absolute Differences (SAD) Figure 11 Carte 3D construite avec des nuages de points générés par la Kinect D après les figures ci-dessus, on peut voir que les trois cartes générées sont consistantes car les nuages de points ont bien été concaténés. La carte générée avec les données 3D de la Kinect est cependant plus précise, ceci peut être constaté dans la forme de la boite cartographiée. La carte générée avec les nuages de points de l algorithme Bloc Matching est distordue car le sol est courbé et pas tout à fait droit (voir Figure 9). Ceci est dû à la qualité de chaque nuage de point constituant le modèle 3D et aussi à la qualité de la calibration de la caméra stéréo. La carte générée avec l algorithme Sum of Absolute Differences (voir Figure 10) quant à elle n a pas de distorsions, le sol est droit; Cependant, cette dernière contient plus de bruit que les deux autres. La moins bruité est bien sûr celle générée avec la Kinect. 2. Pipeline de concaténation de nuages de points utilisant l odométrie visuelle Dans cette approche particulière, on sépare les deux parties localisation et cartographie du SLAM. Le processus de localisation n est plus réalisé en lien total avec la cartographie mais plutôt avec un code open source du nom de FOVIS qui utilise la technique de l odométrie visuelle. D autres techniques de localisation comme par exemple l odométrie basée sur les données d une centrale inertielle et les codeurs incrémentaux des différentes liaisons du robot peuvent être utilisées. L odométrie visuelle est une technique qui permet l estimation des 6 degrés de liberté de la caméra lors de son mouvement. Le principe est basé sur la détection de points d intérêt dans les images de la caméra stéréo en utilisant l algorithme FAST puis le suivi de ces points lors du déplacement de la 7

caméra. Après avoir testé FOVIS avec la Bumblebee2 il s avère que cet algorithme est beaucoup plus fiable en translation qu en rotation. On a donc décidé de n utiliser que les mouvements transrationnels fournis par l odométrie visuelle. Les mouvements rotationnels peuvent très bien être estimés avec une centrale inertielle ou bien tout simplement compensés avec une Pan tilt Unit. Pour construire une première carte 3D en utilisant FOVIS; Chaque nuage de points acquis en utilisant 1 0 0 T x 0 1 0 T la caméra stéréo est multiplié par une matrice de transformation de la forme T = ( y ). 0 0 1 T z 0 0 0 1 Pour cette expérience, la caméra effectue un mouvement purement translationnel. La carte 3D générée peut être observée dans la figure suivante : Figure 12 Carte 3D générée avec l'odométrie visuelle On remarque alors une répétition dans les objets cartographiés qui reflète le degré de drift translationnel de FOVIS. Cependant la carte générée reste cohérente. Pour compenser le drift translationnel, l algorithme ICP (Itérative Closest Point) a été utilisé. Le rôle de cet algorithme est de minimiser la distance entre deux nuages de points consécutifs de la carte 3D. ICP est basé sur un principe d optimisation en gradient; Afin de converger correctement et ne pas être bloqué sur un minimum local, l algorithme a besoin d une bonne estimation initiale de la transformation entre les deux nuages de points. Les étapes suivantes sont utilisées pour rattraper le drift : Des points d intérêts sont calculés sur chaque deux nuages de points consécutifs. Les normales aux points d intérêts sont calculées. Les descripteurs sont ensuite calculés puis les correspondances entre les deux nuages de points (voir Figure 13). La meilleure transformation initiale entre les deux nuages de points consécutifs est estimée en utilisant RANSAC puis alimenter à l ICP pour affiner le recalage. 8

Figure 13 Correspondances calculés entre deux nuages de points consécutifs Le résultat final se voit nettement amélioré : Figure 14 Carte 3D réalisée avec FOVIS après compensation du drift translationnel On remarque sur la Figure ci-dessus que les objets cartographiés ne sont plus répétés. 3. Kinect Fusion Kinect Fusion constitue aujourd hui l état de l art du SLAM. En utilisant une carte graphique NVIDIA comme moyen de calcul, l algorithme est capable de réaliser des cartographies en temps réel à une fréquence maximale de 30 fps. Dans sa version originale l algorithme utilise la Kinect comme capteur de données 3D et prend en entrée des images de profondeur (depth map) (voir Figure 5). Kinect Fusion utilise ICP pour concaténer les nuages de points extraits des images de profondeurs et calcule une moyenne du modèle 3D réalisé. La version de Kinect Fusion disponible dans la version expérimentale 1.7 de la librairie PCL a été testé dans sa version originale avec la Kinect et une carte graphique NVIDIA GTX 660 d architecture Kepler et a donné le résultat suivant : Figure 15 Carte 3D réalisée avec l'algorithme Kinect Fusion et la Kinect 9

On peut voir dans la figure ci-dessus le degré de qualité de la carte 3D réalisée. Cette dernière est parfaitement fidèle à l environnement réel et ne contient aucun bruit ni distorsions. Cette carte sera donc considérée comme réalité terrain (ground truth). Kinect Fusion a aussi été adapté pour pouvoir fonctionner avec la caméra stéréo en recevant l image de profondeur de la Bumblebee2 au lieu de la Kinect. Les deux types de nuages de points calculés avec les algorithmes Bloc Matching et Sum of Absolute Differences ont été utilisées pour construire des cartes 3D. Ces nuages de points sont d abord convertis en images de profondeurs puis alimentés à l algorithme. Avec les données 3D calculées avec l algorithme Bloc matching, le résultat suivant a été obtenu : Figure 16 Carte 3D réalisée avec Kinect Fusion et la Bumblebee2 en utilisant des données 3D calculées avec l'algorithme Bloc Matching - vue perspective- Figure 17 Carte 3D réalisée avec Kinect Fusion et la Bumblebee2 en utilisant des données 3D calculées avec l'algorithme Bloc Matching - vue sur le côté- D après la figure ci-dessus, on peut voir que la carte 3D réalisée est moins précise et plus bruitée que celle obtenue en utilisant la Kinect, ce qui démontre encore une fois la supériorité d un capteur actif par rapport à un capteur passif. La carte réalisée reste consistante mais contient cependant une distorsion visible sous forme de courbure sur le sol de la carte lorsque celle-ci est visualisée sur le côté. Cette courbure revient à la qualité des données 3D alimentant l algorithme et du coup à la qualité de la calibration de la caméra stéréo. Avec les données 3D calculées avec l algorithme Sum of Absolute Diffrences, le résultat suivant a été obtenu : 10

Figure 18 Carte 3D réalisée avec Kinect Fusion et la Bumblebee2 en utilisant des données 3D calculées avec l'algorithme Sum of Absolute Differences - vue perspective- Figure 19 Carte 3D réalisée avec Kinect Fusion et la Bumblebee2 en utilisant des données 3D calculées avec l'algorithme Sum of Absolute Differences - vue sur le côté On remarque une carte 3D consistante dont le sol est droit et non distordu comparé au résultat obtenu avec l algorithme Bloc Matching. Ceci est tout à fait normal car les nuages de points dans ce cas sont basés sur les images calibrées par Triclops, la librairie officielle de la caméra stéréo. 4. Comparaison mathématique entre les différentes cartes réalisées Afin d avoir une comparaison plus objective que la comparaison visuelle précédemment faite, chacune des cartes réalisée précédemment est recalé sur la carte réalisée avec Kinect Fusion et la Kinect qui est considérée comme étant la vérité terrain. Avec un logiciel du nom de CloudCompare on calcule ensuite la distance moyenne entre les deux cartes. Les résultats obtenus pour les différentes cartes réalisées sont résumés dans le tableau suivant: Technique Distance moyenne entre la carte et la vérité térrain (m) RGBDSLAM avec la Kinect 0.0102 Kinect Fusion avec Bumblebee2 (Données 3D SAD) 0.014 RGBDSLAM avec Bumblebee2 (Données 3D SAD) 0.024 Kinect Fusion avec Bumblebee2 (Données 3D BM) 0.040 FOVIS pipeline de concaténation 0.066 (Données 3D BM) RGBDSLAM avec Bumblebee2 (Données 3D BM) 0.093 Table 1 Distance moyenne entre chaque carte 3D et la vérité térrain 11

On remarque d après le tableau ci-dessus que la meilleure carte 3D réalisée après la vérité terrain est celle réalisée avec la Kinect et le package RGBDSLAM, ce qui démontre la supériorité de la Kinect par rapport à la caméra stéréo. On voit aussi que l algorithme SAD (Sum of Absolute Differences) utilisé par Triclops génère des cartes de meilleure qualité que l algorithme BM; Ceci est cependant affecté par la qualité de la calibration utilisée par la librairie officielle de la caméra stéréo. L algorithme BM quant à lui utilise les images calibrées manuellement avec un damier. On retrouve alors les cartes 3D générées avec BM en dernières places. Chapitre IV Conclusion et travail futur Le but de cette dissertation est de fournir une analyse expérimentale de différentes techniques de SLAM visuel afin d aider HyQ, un robot quadrupède actionné hydrauliquement développé à l Istituto Italiano di Tecnologia à Génova à être capable de planifier sont chemin et de calculer les positions optimales de ses pattes lors de son mouvement. En utilisant une caméra stéréo de type Bumblebee2 et une Kinect de chez Microsoft, on a donc pu explorer les limites de chacun de ces capteurs utilisés pour construire des cartes 3D. Premièrement, différents algorithmes constituant l état de l art du SLAM ont été testés pour construire des cartes 3D; Puis une approche customisée utilisant l odométrie visuelle a été mise en place pour faire la même chose. Au final, la planification du chemin et des pas du robot a pu être réalisée en utilisant les différentes cartes 3D générées. En évaluant la qualité des cartes 3D générées en utilisant les différentes techniques présentées, on arrive aujourd hui à avoir une meilleure appréciation de la meilleure technique SLAM qui sera la plus adaptée pour le robot HyQ. Chacun des capteurs utilisés possède ses avantages et inconvénients. Lorsque la Kinect est plus précise pour évaluer les dimensions 3D et plus robuste dans les environnements non-texturés, la caméra stéréo quant à elle peut extraire des données 3D depuis des environnements exposés à la lumière du soleil, ce qui en fait un capteur plus adapté pour une utilisation en extérieur. Une combinaison des deux capteurs utilisés durant ce projet sera la meilleure solution pour ne garder que les avantages de chacun. Aujourd hui, la stratégie de planification du chemin et des pas du robot HyQ repose sur la précision des données 3D extraits de l environnement cartographié. Même en utilisant un capteur actif comme la Kinect, la meilleure précision qu il est possible d avoir est de +/- 2Cm; Avec un capteur passif comme la caméra stéréo, cette précision est encore pire. L approche consistant à calculer les meilleurs positions pour les pas du robot puis de les satisfaire n est pas vraiment la plus optimale lorsque cette précision est prise en compte. Voilà pourquoi la stratégie de planification du chemin et des pas devra être modifiée pour pallier aux incertitudes dans l évaluation de la meilleure position où placer les pattes du robot. Des capteurs de force pourront par exemple être utilisés pour au lieur de seulement pouvoir voir le sol, aussi pouvoir le sentir. 12