Un prototype de recalage par arucos

Chaque année, notre équipe utilise un système de positionnement par odométrie pour déterminer la position du robot. Ce système, si il est relativement précis, a au moins deux désavantages bien connus des équipes participant à la coupe de France de robotique :

1. L’erreur de positionnement s’accumule au cours du temps.
2. En cas de glissement du robot, les encodeurs des roues mesurent un déplacement qui ne correspond pas à la réalité.

Nous avons déjà discuté auparavant d’un système de positionnement par lidar, mais la difficulté de traquer les balises nous a conduit a explorer de nouvelles solutions. À l’occasion de la semaine inter-UT, nous avons décidé d’expérimenter une solution permettant le recalage du robot en utilisant les arucos statiques présents sur le terrain. Il existe 4 arucos sur le terrain comme montré en rouge ci dessous :

Et il est également possible aux équipes de rajouter des arucos sur leurs balises / mats de calculs (tant que ces arucos rentrent dans les plages réservées par le règlement).

Ce court article ne vise pas à présenter une solution complète, mais relate plutôt notre expérience lors de la construction d’un premier prototype.

Solution proposée

Nous avions pour première idée de réaliser ce recallage en utilisant la position absolue connue des marqueurs arucos sur le terrain. En voyant un aruco, le robot peut utiliser sa position et son orientation pour se localiser.

Si cette solution est tentante, elle souffre de deux défauts :

1. Il faut connaître à priori la position des arucos dans le repère global du terrain, ce qui demande de maintenir une configuration avec ces positions. De plus, il faut travailler dans le repère global du terrain, mais notre robot n’a connaissance que de son propre repère (dont l’origine est son point de départ sur le terrain).
2. Les tolérances de construction de la table s’ajoutent aux imprécisions de mesures.

Nous avons donc opté pour une autre tactique : lorsque le robot observe un aruco statique pour la première fois, il sauvegarde la position de celui-ci à ce moment en se fondant sur son odométrie. On fait la supposition que l’odométrie n’a pas trop dérivé, et on prend cette mesure comme vérité terrain. Lors des prochaines observations d’un même aruco, le robot va de nouveau calculer sa position. Cette nouvelle position différera de la position initiale, ce qui nous permettra d’apprécier le décalage de l’odométrie et d’inférer la position réelle du robot. Cette solution esquive les limitations précédemment énoncées, mais suppose que la position estimée par l’odométrie est parfaite au moment d’observer un tag aruco pour la première fois, ce qui constitue évidemment une approximation.

Détection des et positionnement des arucos dans l’espace

Pour donner des yeux au robot, nous utilisons une caméra stéréo Luxonis OAK-D Lite, qui permet de récupérer des informations de profondeurs en plus d’une simple image en 2D.

Notre logiciel sharpsight est chargé (entre autres fonctionnalités) de publier sur le réseau les positions tridimensionnelles des arucos détectés par la caméra, afin que la carte contrôlant le robot y ait accès. Nous détectons les arucos sur les images transmises par la caméra à l’aide des bindings Python de la célèbre librairie de vision OpenCV. Les informations de profondeur de la caméra nous permettent ensuite de déterminer la position tridimensionnelle des arucos dans le repère de la caméra. Nous filtrons à ce moment là certaines observations aberrantes (par exemple, la caméra observe parfois par erreur des arucos avec une position de (0,0,0)).

Une fois un aruco positionné dans le repère de la caméra \(C\), nous appliquons une transformation pour positionner l’aruco dans le repère local du robot \(L\), dont l’origine correspond à son centre odométrique :

\[x_{L} = Rx_{C} + t,\]

où \(R\) représente une matrice dont les colonnes sont les vecteurs de base du repère de la caméra dans le repère local du robot, et \(t\) la position de la caméra dans le repère local du robot. Puis, nous publions ces informations sur le réseau.

Recalage

La carte gérant le contrôle du robot reçoit périodiquement des positions d’arucos dans le repère local du robot. À chaque exécution de la tâche de contrôle (cadencée en temps réel à 200Hz), nous appliquons l’algorithme (volontairement naïf et simplifié) suivant :

def get_pose_from_arucos(aruco_db, odometry):
    # on récupère sur le réseau les arucos actuellement observés
    arucos = retrieve_arucos()

    # on met à jour la base de données d'arucos vus (aruco_db) si
    # nécéssaire, en sauvegardant la position des arucos qu'on observe
    # pour la première fois
    for aruco in arucos:
        if not aruco in aruco_db:
            aruco_db = save_aruco(aruco_db, aruco, odometry)

    # on récupère l'aruco qui a été sauvegardé en premier (en principe, sa
    # position est plus sûre)
    aruco = min(arucos, key=aruco_db.get_save_time)

    # on déduit notre orientation de l'orientation de l'aruco
    saved_pose = aruco_db.saved_pose(aruco)
    Δθ = aruco.θ - saved_pose.θ
    robot_θ = saved_pose.odometry.θ - Δθ

    # on traduit la position de l'aruco dans le repère global du robot
    # par un changement de repère
    R = [[cos(robot_θ), -sin(robot_θ)], [sin(robot_θ, cos(robot_θ))]]
    aruco_pose = R @ aruco.pose + odometry.xy

    # on met à jour la position du robot en fonction de la différences
    # entre l'observation courante et la première observation de la
    # position de l'aruco
    robot_x = odometry_x - (aruco_pose.x - saved_pose.x)
    robot_y = odometry_y - (aruco_pose.y - saved_pose.y)

    return robot_x, robot_y, robot_θ

Mesures expérimentales et conclusion

Nous observons ci-dessus, lors d’un test sur un court déplacement, la différence de positionnement entre l’odométrie et notre méthode par observation d’arucos¹. On peut supposer que l’odométrie ne dérive pas sur un mouvement aussi court. À l’arrêt, les mesures semblent proches. La mesure visuelle varie quelque peu même sans mouvement, mais reste relativement stable : nous observons une amplitude d’environ 4mm dans la mesure des positions. En mouvement, notre technique estime mal la position, et on note également une latence. Cette latence provient au moins en partie de la fréquence d’échantillonage plus faible de la caméra comparée à l’odométrie, tandis que les problèmes d’estimation peuvent provenir de la qualité des images capturés en mouvements.

Le problème d’estimation se confirme en observant les trajectoires 2D du robot prédites par l’odométrie et notre méthode par aruco : la mesure semble correcte à l’arrêt, mais inutilisable en mouvement. Pour conclure, il nous appartient donc de corriger ce problème pour utiliser notre technique en déplacement, ou de ne l’utiliser que lorsque le robot est à l’arrêt.

1. L’axe temporel n’est pas complètement fidèle à la réalité, puisque chaque mesure correspond à une observation d’aruco par le robot, mais celui-ci ne voyait pas systématiquement un aruco pendant le test. ↩