Aspects d’implémentation

a Implémentation de l’EKF

Nous avons utilisé la bibliothèque d’algèbre linéaire Boost uBLAS pour implémenter le filtre de Kalman étendu et toutes les opérations vectorielles et matricielles nécessaires. Il s’agit essentiellement d’un choix historique puisqu’elle était déjà utilisée par l’environnement de développement Jafar dans lequel RT-SLAM a été développé, et qu’aucun besoin ne justifiait de changer pour une autre bibliothèque telle que Eigen par exemple.

Nous avons en particulier fait un usage intensif des structures creuses d’indexation indirecte indirect_vectoretindirect_matrix, qui permettent :

— De gérer dynamiquement l’attribution des élements de l’état du filtre (pour l’état du robot, des capteurs, et des amers).

— De manipuler en toute transparence des sous-vecteurs pour plus de clarté : l’état du robot est un sous-vecteur de l’état du filtre, la position du robot est un sous-vecteur de l’état du robot, les données n’étant stockées qu’à un seul endroit, l’état du filtre.

— D’optimiser de façon simple les calculs pour tenir compte du caractère creux des ma-trices d’évolution et d’observation, en ne les faisant que sur les blocs non nuls.

L’utilisation de la structure de matrice symétrique symmetric_matrixpermet également de ne stocker que la moitié de la matrice, de garantir une stabilité numérique accrue, et d’optimiser les calculs en conséquence.

b Gestion des grandeurs estimées

Puisque RT-SLAM a vocation à être générique vis à vis des capteurs à intégrer, il est nécessaire de définir dynamiquement les grandeurs à estimer en fonction des besoins de tous les capteurs ou modèles de mouvement utilisés. Cette tâche doit être centralisée, et ne peut pas être distribuée aux différents capteurs, afin de ne pas dupliquer des grandeurs dans le filtre. Par exemple si deux capteurs ont besoin que la vitesse angulaire soit estimée, cette dernière ne doit se trouver qu’à un endroit dans le filtre, et le modèle ou le capteur qui effectue la prédiction doit savoir qu’elle est estimée pour la mettre à jour.

3. Notons que cela implique cependant de normaliser le quaternion lorsque l’on souhaite l’utiliser en dehors du filtre après la correction, mais seule sa covariance propre doit être mise à jour donc le coût est négligeable par rapport à mettre à jour la covariance de tout l’état.

3.2. RT-SLAM 39 Nous avons donc implémenté une bibliothèque de grandeurs à estimer concernant le robot dans sa globalité : position, orientation, vitesse, vitesse angulaire, accélération, accélération angulaire, gravité, norme de la gravité, . . . Chaque capteur ou modèle doit ensuite déclarer desquels il a besoin, en gardant la possibilité de réserver de l’espace dans le filtre pour son usage privé, par exemple les biais de la centrale inertielle.

c Outils d’analyse

La facilité de débogage est un aspect primordial dans un projet complexe, qui dans notre cas tourne principalement autour de la notion de répétabilité de l’exécution. En effet on le verra beaucoup de mécanismes utilisés impliquent l’utilisation de nombres aléatoires. Ils sont indispensables à certains algorithmes, mais ont également été volontairement utilisés dans beaucoup de mécanismes où il n’étaient pas nécessaires : partout où aucun critère valide ne pouvait motiver une décision particulière, la décision est prise de façon aléatoire. Un exemple typique est dans quelle zone de l’image commencer à chercher des amers : rien ne permet de dire qu’il vaut mieux commencer en bas à droite ou en haut à gauche, donc on le fait aléatoirement. Cela présente un gros avantage en terme d’évaluation de la robustesse du logiciel, puisque l’éxécuter plusieurs fois sur les mêmes données permet de vérifier la sensibilité à ces choix, ce qui revient à simuler des données légèrement différentes. L’inconvénient est que lorsqu’un problème est remarqué dans l’exécution du logiciel, on souhaite pouvoir le répéter en rejouant l’exécution à l’identique. Pour cela il suffit d’initialiser la graine du générateur aléatoire à la même valeur, ce que notre interface permet. Afin d’avoir la garantie que le générateur de nombre aléatoires n’est pas influencé par des bibliothèques extérieures, nous utilisons un générateur séparé pour tous les appels de RT-SLAM dont nous stockons l’état (fonctionrand_r). En fonctionnement standard, si aucune graine aléatoire n’a été spécifiée, on en génère une au début de l’exécution du programme à partir de la date et de l’identifiant du processus (afin que deux instances démarrées en même temps utilisent une graine différente), puis elle est affichée et enregistrée avec le résultat pour permettre sa réutilisation.

Si cela est suffisant pour faire du débogage en ligne, cela ne l’est pas pour pour identifier un problème qui a eu lieu pendant le fonctionnement en ligne. Pour cela il faut pouvoir rejouer hors ligne exactement la même chose qu’en ligne, ce qui pose plusieurs difficultés :

— pouvoir enregistrer sur le disque l’ensemble des données utilisées sans perturber le fonc-tionnement en ligne (essentiellement sans le ralentir).

— pouvoir exécuter à l’identique le programme sur ces données, en utilisant les mêmes et en faisant exactement les mêmes calculs avec.

Ces difficultés sont résolues avec les techniques suivantes :

— Afin de ne pas augmenter la charge de calcul du processeur et de garantir la répétabilité, les données sont enregistrées sans compression et sans pertes (format PGM pour les images, format binaire pour les nombres flottants).

— Si enregistrer les données est assez simple, il faut cependant savoir que les entrées-sorties sur le disque sont génératrices de blocages plus ou moins longs (le disque étant partagé par tous les processus du système), et on doit donc utiliser un thread différent

pour réaliser ces écritures si on ne veut pas bloquer l’exécution du programme principal.

Nous avons donc implémenté un gestionnaire d’enregistrement qui centralise dans une file d’attente toutes les données à enregistrer et réalise les enregistrements dans un thread séparé.

— Certains problèmes d’exécution que l’on souhaite déboguer sont des plantages provocant l’arrêt brutal du programme, y compris du thread d’enregistrement même s’il n’a pas terminé d’enregistrer toutes les données, notamment celles qui ont causé le problème. Il faut donc intercepter les signaux du système d’exploitation pour laisser les différentes tâches, notamment celle d’enregistrement, terminer leur travail proprement lorsqu’un plantage est signalé.

— Si toutes les données sont enregistrées, toutes ne sont pas nécessairement utilisées lors d’une exécution en ligne, pour cause de retard des données ou de manque de temps de calcul. Il est donc nécessaire d’enregistrer également un journal spécifiant quelles données ont été utilisées, ainsi que les traitements réalisés pour les capteurs pour lesquels on adapte le traitement des données au temps disponible.

— Il est également nécessaire de désactiver dans le compilateur les optimisations des calculs en virgule flottante réalisées par le processeur qui ne garantissent pas la répétabilité.

— Enfin bien sûr comme expliqué auparavant on doit enregistrer la graine aléatoire utilisée.

Une fois cette capacité de répétabilité atteinte, d’autres éléments comme l’écriture de journaux de l’exécution, la possibilité dans l’interface de réaliser le traitement image par image, et d’interagir avec les éléments d’affichage pour obtenir des informations plus détaillées, ont également une grande importance pour permettre de comprendre ce qu’il se passe, détecter et identifier les éventuels problèmes. La figure3.3 illustre l’affichage de RT-SLAM.

Figure3.3 –Affichage 2D et 3D de RT-SLAM. Les couleurs dans les tons rouges et bleus représentent différentes paramétrisations d’amers, tandis que les couleurs brillantes contre celles plus foncées si-gnalent que l’amer est couramment observé. Les segments illustrent l’incertitude de profondeur quand elle est grande.