Description de l`algorithme de reconstruction de terrain
publicité
2RT3D Présentation de l’algorithme de reconstruction de terrain de Robosoft Référence document Révision Date RSP/6184/TEC/2009/01 1.1 28/10/2009 Auteur Visa Yves QUEMENER pg:es Présentation de l'algorithme de reconstruction de terrain de Robosoft Réf. RSP/6184/TEC/2009/01 Révision 1.1 Date 28/10/2009 DIFFUSION DU DOCUMENT Société / Organisme Nom Distrib. Copie Robosoft PGES (RSP) Aubert CARREL X Robosoft PGES (RSP) Francis MARTINEZ X USAR – CNRS / PSIROB Michel GUGLIELMI X LAAS Simon LACROIX X LAAS Naveed MUHAMMAD X THALES Optronique (TOSA) Joël MORILLON X THALES Optronique (TOSA) Jean Sébastien BENOIST X HISTORIQUE DU DOCUMENT DATE AUTEUR PRINCIPALES MODIFICATIONS 0.1 22/07/2009 Y. QUEMENER Création 1.1 28/10/2009 Y. QUEMENER Modifications N° Révision Page 2 MOTIFS Version préliminaire Prise en compte remarques de lecture F.Martinez et A.Carrel Présentation de l'algorithme de reconstruction de terrain de Robosoft Réf. RSP/6184/TEC/2009/01 Révision 1.1 Date 28/10/2009 Table des matières 1. INTRODUCTION 4 1.1 Objet du document 4 1.2 Terminologie 4 1.3 Objet de l’algorithme 4 2. ALGORITHME 5 2.1 Principe général 5 2.2 Principes détaillés 5 2.2.1 Création du MNT 5 2.2.2 Recherche de chemin 8 3. PERFORMANCES 9 3.1 Qualité et finesse du MNT obtenu 9 3.2 Algorithme de recherche de chemin 9 3.3 Sensibilité aux erreurs d’orientation 9 4. INTERFACE GRAPHIQUE 10 Page 3 Présentation de l'algorithme de reconstruction de terrain de Robosoft Réf. RSP/6184/TEC/2009/01 Révision 1.1 Date 28/10/2009 1. Introduction 1.1 Objet du document Ce document a pour but de présenter l’algorithme de reconstruction de modèle numérique de terrain proposé par Robosoft dans le cadre du projet 2RT3D. 1.2 Terminologie MNT Modèle numérique de terrain Repère global Repère utilisé par la base de localisation du véhicule. Repère local Repère relatif au point zéro de l’imageur laser. Il est donc mobile par rapport au terrain et solidaire au véhicule. Impact Point de mesure retourné par l’imageur laser dans le repère local. Un tel point correspond à l’impact entre un faisceau laser de mesure et un objet de l’environnement. LNP Lidar Network Protocol. Protocole de transmission réseau mis au point dans le cadre de 2RT3D qui permet de transmettre des séries d’impact ainsi que la position et l’orientation du capteur. Ce protocole est publié dans le document « Description of the network protocol used to transmit lidar data » 1.3 Objet de l’algorithme Le but de l’algorithme de reconstruction de MNT de Robosoft est de convertir une série d’impacts reçus par Ethernet grâce au protocole LNP en une grille d’élévations utilisable pour la navigation autonome. Il a été convenu que Robosoft présenterait un algorithme basique, par comparaison à ceux mis au point par le LAAS mais qu’il permettrait d’effectuer rapidement des essais de reconstruction avec des données réelles et simulées, voire d’effectuer des tests en boucle fermée dans le simulateur. Optionnellement, l’algorithme développé peut également donner des consignes de guidage point à point qui permettent au robot d’éviter des obstacles tels que les murs et les fortes pentes. Page 4 Présentation de l'algorithme de reconstruction de terrain de Robosoft Réf. RSP/6184/TEC/2009/01 Révision 1.1 Date 28/10/2009 2. Algorithme 2.1 Principe général L’algorithme développé crée une carte d’élévation sur une grille de 80 x 80 cases de 2 mètres de coté (pour une grille de 160x160 m donc) centrée sur le véhicule, et alignée sur les axes du repère « monde ». Ces nombres sont précisés dans un script et sont donc aisément modifiables. Ces dimensions ont été choisies car 80 mètres est la distance maximale théorique de l’imageur laser utilisé dans le cadre de ce projet. On va attribuer chacun des impacts à un nœud du MNT afin de déterminer une altitude. Une fois la carte d’élévation déterminée, on en déduit une carte de traversabilité qui servira à son tour à déterminer un chemin praticable grâce à un algorithme de recherche de chemin A*. 2.2 2.2.1 Principes détaillés Création du MNT Chaque impact reçu est associé au nœud de la grille du MNT dont il est le plus proche (en ne considérant que les coordonnées XY). L’altitude de cet impact vient ensuite contribuer aux multiples caractéristiques associées à chaque nœud. A chaque nœud du maillage du MNT correspond un ensemble de caractéristiques : • Date à laquelle le dernier impact associé à ce nœud à été reçu • Altitude minimum des impacts reçus • Altitude maximum des impacts reçus • Altitude moyenne des impacts reçus • Altitude du dernier impact reçu • Moyenne des incertitudes des données liée à ce nœud. • Altitude moyenne des impacts reçus pondérés par les incertitudes • Variance de l’altitude des impacts Il est possible de visualiser ces différentes données dans l’interface graphique (cf. 4.Interface). Ces informations vont être utilisées pour construire le MNT final qui attribue une altitude finale à chaque nœud du MNT. Le principe de l’algorithme est le suivant : • Si la variance de l’altitude des impacts dépasse un certain seuil (paramétrable et empiriquement fixé à 0.1, soit un écart-type de 31 cm) on considère que le voisinage du nœud comporte une surface verticale. L’altitude finale attribuée au nœud est alors l’altitude maximum enregistrée. Le nœud est également classé comme non traversable. • Si la variance de l’altitude des impacts est inférieure à ce seuil, on considère que le voisinage du nœud est une surface à peu près plane. On utilise alors l’altitude moyenne. Un modèle simple d’incertitudes de mesure a été utilisé : on suppose un certain pourcentage d’erreur dans les données de localisation, en position et en orientation, ainsi que dans les mesures de distance de l’imageur. Cela se traduit par une incertitude de position de l’impact d’autant plus grande que l’impact est éloigné de l’imageur. Page 5 Présentation de l'algorithme de reconstruction de terrain de Robosoft Réf. RSP/6184/TEC/2009/01 Révision 1.1 Date 28/10/2009 Ne disposant pas encore d’un modèle d’erreur précis de l’imageur, des valeurs arbitraires ont été choisies : 10 cm d’erreur de position, 0.01 radian d’erreur d’orientation et 1% d’erreur dans la mesure de distance. Toutes ces valeurs sont paramétrables pour pouvoir s’adapter à d’autres types de capteurs. Figure 1. Exemple de MNT. La couleur correspond à l'élévation des nœuds Une carte de traversabilité peut ensuite être établie. Deux critères sont considérés pour la traversabilité : la variance des altitudes (c'est-à-dire la présence d’un objet vertical) et l’incertitude des données. Une zone ayant une trop grande incertitude sera considérée comme « inconnue ». Si une zone n’est pas inconnue, on considère la variance de l’altitude de ses impacts. Une zone ayant une forte variance d’altitude sera classée « non traversable » et une zone avec une faible variance sera « traversable ». On effectue ensuite une dilatation des zones non traversables afin de permettre au véhicule de disposer d’une marge de sécurité lors du contournement d’un mur par exemple. Cette dilatation est désactivable via l’interface graphique. Page 6 Présentation de l'algorithme de reconstruction de terrain de Robosoft Réf. RSP/6184/TEC/2009/01 Révision 1.1 Date 28/10/2009 Figure 2. Carte de traversabilité après dilatation Page 7 Présentation de l'algorithme de reconstruction de terrain de Robosoft 2.2.2 Réf. RSP/6184/TEC/2009/01 Révision 1.1 Date 28/10/2009 Recherche de chemin Afin de valider la carte de traversabilité obtenue par l’algorithme ci-dessus, un algorithme de recherche d’itinéraire a été développé et testé avec le simulateur de Robosoft. Après désignation d’une destination à la souris, l’algorithme essaiera de l’atteindre en envoyant au véhicule géré par le simulateur les consignes de guidage appropriées. Figure 3. Recherche de chemin (filtrage des nœuds atteignables) L’algorithme de recherche de chemin utilise une recherche A* pour atteindre la destination (croix noire). Il retourne donc dans un premier temps une liste de nœuds traversables (en vert) formant un chemin vers l’objectif. Toutefois envoyer cette liste de coordonnées comme consignes de guidage rendrait la trajectoire irrégulière et hachée. Au lieu de cela, on effectue un lancer de rayon sur chacun des points de la trajectoire et on utilise le point le plus avancé de la trajectoire qui n’a pas rencontré d’obstacle (en rouge). Ainsi la trajectoire finale est composée du minimum de segments linéaires amenant à sa destination. Page 8 Présentation de l'algorithme de reconstruction de terrain de Robosoft Réf. RSP/6184/TEC/2009/01 Révision 1.1 Date 28/10/2009 3. Performances 3.1 Qualité et finesse du MNT obtenu Les paramètres par défaut utilisent une grille dont les nœuds sont espacés de deux mètres afin d’assurer au véhicule un passage si un chemin est trouvé. Il est possible de réduire ce paramètre afin d’améliorer la finesse du modèle, au risque de voir des passages trop étroits détectés par l’algorithme. Les obstacles à la navigation sont généralement bien repérés sur la grille par défaut. Il ressort des tests qu’une grande taille de maille a tendance à donner à un obstacle une taille horizontale plus importante que dans la réalité mais qu’elle permet de donner une grille de navigation plus sûre. 3.2 Algorithme de recherche de chemin L’algorithme de recherche de chemin, utilisé pour valider la reconstruction, est élémentaire : il s’agit d’une recherche A* utilisant la distance euclidienne comme heuristique. Il considère les zones marquée « verticales » comme non-traversables mais s’autorise à traverser des zones « inconnues », car dans la plupart des cas une zone inconnue dénote une zone cachée par un obstacle qui pourra se révéler traversable une fois l’obstacle contourné ou le véhicule plus proche. Cependant, cette façon de considérer les zones inconnues classe les plans d’eau et les dénivelés descendants par rapport au véhicule (les trous) comme des zones traversables. De plus, cet algorithme considère également une case à pente nulle et une case à pente moyenne (mais en deçà du seuil de verticalité) avec le même coût. A ces égards, il est améliorable mais a cependant permis une évaluation d’un guidage dans le MNT reconstruit. 3.3 Sensibilité aux erreurs d’orientation Une conséquence attendue et vérifiée de l’algorithme utilisé est une grande sensibilité aux erreurs des mesures d’orientation retournées par la base de localisation. En effet, une faible erreur sur la composante de tangage peut résulter en une erreur de plusieurs mètres à une grande distance de l’imageur. Le grand nombre d’impacts modère l’erreur , dans une certaine mesure, mais dans sa forme actuelle, l’algorithme de reconstruction est très sensible à une mesure aberrante de l’orientation. A noter que les erreurs de cap sont mieux absorbées : la grande taille des mailles rend ces erreurs moins visibles. Page 9 Présentation de l'algorithme de reconstruction de terrain de Robosoft Réf. RSP/6184/TEC/2009/01 Révision 1.1 Date 28/10/2009 4. Interface graphique Figure 4.Interface graphique L’interface graphique permet de colorer le modèle numérique de terrain en fonctions de divers paramètres des nœuds, de changer le mode de calcul de l’altitude des points, ainsi que d’activer la recherche de chemin par A*. Les différents modes de calcul de l’altitude des nœuds de la carte sont les suivants : • Altitude minimum. • Altitude maximum. • Altitude du dernier impact reçu. • Altitude moyenne. • Altitude pondérée en fonction de l’incertitude de mesure. • Classification en nœuds verticaux et en nœuds horizontaux: on utilise l’altitude maximum pour les nœuds verticaux et l’altitude moyenne pour les nœuds horizontaux. C’est le mode sélectionné par défaut. Page 10 Présentation de l'algorithme de reconstruction de terrain de Robosoft Réf. RSP/6184/TEC/2009/01 Révision 1.1 Date 28/10/2009 Il est également possible de colorer la carte en fonction de plusieurs données relatives aux nœuds : • Altitude • Incertitude des données • Âge des données • Nombre d’impacts • Variance de l’altitude des impacts • Traversabilité (calculée en seuillant la variance de l’altitude des impacts) • Distance minimum à l’objectif selon l’algorithme A* (permet de visualiser le plus court chemin à l’objectif) Une case à cocher permet d’activer la recherche d’itinéraire. Lorsque la recherche d’itinéraire est activée, l’application tente d’établir une communication via le port UDP 2323 avec le véhicule simulé par le Simulateur et lui envoie les consignes de guidage. Si la communication ne peut s’établir, il reste possible via l’interface d’entrer manuellement la coordonnée d’un point à atteindre. Page 11
