Fermeture de boucle

Dans cette première expérimentation, nous avons choisi de confronter l’algo- rithme au problème de la fermeture de boucle. Pour ce faire, une longue boucle d’approximativement 100 mètres a été faite par le VIPALab. Le véhicule a continué sur environ 10 mètres après avoir rejoint le point de départ de la trajectoire afin de rendre possible la détection de la boucle. Des amers ont été cartographiés tout au long de la trajectoire. Seulement ceux suffisamment précis ont été utilisés dans le SLAM haut niveau afin d’identifier la fermeture de boucle. Le seuil de convergence des amers est fixé ici comme un cumul des racines carrées des variances suivant les 3 dimensions de l’amer : σconvergence =  σ2 a_ix +  σ2 a_iy +  σ2 a_iz < τ (4.42)

où τ est le seuil d’acceptabilité d’un amer, ici fixé à 0, 5m.

Pour cette trajectoire, environ 60 amers ont convergé et sont donc utilisés dans le SLAM gérant la dérive. Afin de bien comprendre l’importance de la prise en compte de la dérive, la figure 4.20 présente les résultats de localisation issus du SLAM bas niveau, c’est-à-dire sans l’intégration du biais. Sur cette figure, la fermeture de boucle n’est également pas accomplie.

Le point important à constater sur cette figure est que la trajectoire calculée par le SLAM monoculaire est sujette à une dérive en translation non négligeable. Les angles sont en revanche plutôt bien estimés, sauf vers la fin de la trajectoire où le véhicule commence tout doucement à dévier. L’incertitude estimée associée à la pose du véhicule est très faible, même après 100 mètres parcourus. Cela rend la détection de la fermeture de boucle impossible avec cette seule information. La localisation calculée est précise mais inconsistante. C’est cet état de fait qui a été décrit tout au long de ce chapitre et qui affecte les algorithmes de SLAM. On peut ainsi constater que, rapidement après le départ, l’intégrité de l’EKF est perdue car la vérité n’est plus englobée par l’incertitude calculée par le SLAM. La dérive de translation maximale mesurée ici est de plus de 4 mètres lors du dernier virage.

Figure _{4.20 – Inconsistance et dérive du SLAM bas niveau. La courbe noire est la} vérité terrain obtenue par le GPS RTK. La courbe rouge est la localisation calculée par le SLAM monoculaire. Les petites ellipses rouges sur cette trajectoire sont les incertitudes associées aux différentes poses du véhicule. Le point noir est le point de départ de la trajectoire (le véhicule part vers la droite). Le carré noir est la fin de trajectoire pour le GPS RTK et le carré rouge la fin de cette même trajectoire mais estimée par le SLAM.

En reprenant la même trajectoire mais en activant le haut niveau, il est possible de conserver l’intégrité de la localisation. Ces résultats sont exposés dans la figure 4.21. Comme pour la figure précédente, la fermeture de boucle n’est pas effective ici, expliquant ainsi que la dérive ne soit pas corrigée.

Figure _{4.21 – Prise en compte de la dérive sans fermeture de boucle. Le code} couleur est le même que pour la figure précédente. Les ellipses bleues correspondent à l’incertitude du véhicule donnée par le haut niveau en intégrant la dérive.

La prise en compte du biais montre que, dans ce cas, la consistance du SLAM est assurée : les ellipses grandissent tout au long de la trajectoire et inclues toujours la position réelle du véhicule. Bien entendu, le biais a été calibré pour le véhicule en fonction de la divergence moyenne observée sur plusieurs trajectoires. La recherche de fermetures de boucle est facilitée car un a priori fort est donné par la dérive. Il convient ensuite d’identifier des amers précédemment vus afin de pouvoir s’assurer de la fermeture de boucle et ainsi affiner la trajectoire calculée par le véhicule. Le résultat de localisation avec bouclage est disponible en figure 4.22.

Figure _{4.22 – Prise en compte de la dérive avec fermeture de boucle. Le code} couleur est le même que pour la figure précédente. La trajectoire bleue est celle calculée avec la fermeture de boucle. Le carré bleu est le point d’arrêt calculé après fermeture de boucle.

On peut voir que la trajectoire avec bouclage vient beaucoup plus près de la vérité. L’angle est correctement corrigé à la fin de la trajectoire et les erreurs de translation ont quasiment disparu et ce sur toute la trajectoire grâce à la rétro- action naturelle causée par l’estimation du biais. Les sauts soudains, visibles dans la trajectoire, sont liés au fait que seulement quelques estimées de biais (16 au total) sont utilisées dans cette trajectoire. Ainsi, chaque changement de biais donne ce petit saut dans la localisation. Ce problème peut être résolu en initialisant plus de biais tout au long de la trajectoire, bien que cela engendrerait un surcoût au niveau du temps de calcul. Un autre aspect important est que l’incertitude concernant la pose du véhicule est grandement réduite. Cela est particulièrement visible autour du point de fermeture de la boucle. Il est important de noter que malgré la diminution de l’incertitude, la consistance est toujours assurée sur cette trajectoire.

Afin de vérifier la qualité de la localisation obtenue, celle-ci a été comparée à la position donnée par le GPS RTK. Ces résultats sont montrés en figure 4.23.

Figure _{4.23 – Écart de position entre le GPS RTK et le SLAM dans le cadre d’une} fermeture de boucle. En bleu, l’écart longitudinal en fonction de l’abscisse curviligne. En rouge, la même chose pour l’écart latéral. Le point d’exclamation sur l’abscisse est l’endroit où la boucle a été fermée.

dans le SLAM. L’erreur maximale est de 55 centimètres et est en moyenne autour de 20 centimètres. Les pics les plus importants ont eu lieu durant un changement d’estimée de biais. Il est important de garder en tête que la précision de la localisation est étroitement liée à la précision des amers. Celle-ci ayant été fixée à 50 centimètres, il est donc difficile d’arriver à avoir une meilleure estimation de la pose véhicule dans ce cas. Cela nécessiterait d’abaisser le seuil de convergence des amers. Pour ce faire, il faudrait également augmenter le nombre d’amers à suivre afin de pouvoir assurer leur convergence. Ici, uniquement 60 amers ont été utilisés. Enfin, en continuant la trajectoire plus longtemps, il aurait certainement été possible de trouver plus d’associations pour améliorer la qualité de la fermeture de boucle.

Ces premiers résultats montrent clairement l’intérêt qu’il y a à prendre en compte la dérive naturelle du SLAM. La fermeture de boucle a ainsi pu être facilement identifiée. De plus, le maintien de l’intégrité du filtre tout au long de la trajectoire renforce encore l’avantage que l’on peut avoir à intégrer le biais de localisation dans le calcul de la pose du véhicule.