Hokuyo

L'Hokuyo c'est cool, mais l'Hokuyo c'est quoi ?
L'Hokuyo est un télémètre laser, un LiDar. Il faut imaginer un capteur qui crée une “nappe laser” autour de lui et qui renvoi la position des objets dans ce plan. Plus précisément, nous possédons un modèle URG-04LX-UG01 de la marque Hokuyo (d’où le nom). Vous pouvez trouver les caractéristiques techniques ici : https://www.hokuyo-aut.jp/search/single.php?serial=166

Comme je l'ai expliqué plus haut, un télémètre laser balaye un plan à l'aide d'un laser rotatif, créant une “nappe laser”. Tout objet dans ce plan va renvoyer le laser et permettre au télémètre d'en connaitre la distance. Notre Hokuyo par exemple, balaye sur 240° avec une résolution de 1024 points par tour, soit environ 700 points sur 240°.
On se doit d'évoquer deux points problématiques de ces télémètres (ils sont magiques, mais pas trop) :

1. Un télémètre laser n'est pas TRES précis. J'entends par la que la précision est de quelques centimètres (plus ou moins 3% pour notre hokuyo - soit plus ou moins 9cm à 3 mètres). Certes en pratique les télémètres s'avèrent bien plus précis que ces valeurs extrêmes quand ils sont utilisés dans des conditions optimales mais cette précision reste tout de même de l'ordre du centimètre. C'est plus que suffisant pour détecter les adversaires mais pas assez pour remplacer l'odométrie.
2. Un télémètre possèdes des angles morts. En effet comme disait Coluche “un train peut en cacher un autre” (quoi ya pas que lui qui le dit ?). Avec un seul hokuyo, si un robot se trouve juste devant la balise, un va masquer tout le reste de la carte et nous deviendrons alors aveugles. Le problème peut être résolu par l'ajout d'un deuxième hokuyo.
3. La nappe de distance est discrète : à une certaine distance, un obstacle peu large peut se retrouver entre deux tirs laser. Ça n'empêchera pas de détecter un robot mais on peut louper un élément de décor ou un de nos cylindres de PVC si le nombre d'échos retournés n'est pas suffisant (cf recherche_des_robots). Il suffit de calculer la largeur angulaire de l'objet à 3m divisée par la résolution angulaire du LiDar. De ce fait, nous avons jusqu'ici affecté chacun des deux LiDar à environ une demi-table.

Jusqu'en 2017, les Hokuyos étaient placés sur les tourelles en bord de table. Cela a posé les problèmes suivants :

• on ne peut détecter que la position des adversaires
• cela peut demander beaucoup de temps de mise en place
• contrainte d'encombrement dans la tourelle
• contrainte d'autonomie/d'alimentation séparée
• contrainte de communication sans fils (cf wifi) causant des latences

D'un autre côté, un hokuyo embarqué dans un robot permettrait les choses suivantes :

• pas de contrainte d'alimentation (5V/750mA)
• communication câblée
• algorithme plus simple pour la détection d'obstacle basé sur la distance
• possibilité d'utiliser des algorithmes plus poussés (SLAM, recalage,…)

Les principaux problèmes sont l'angle de vue et l'encombrement : dans l'idéal, il faudrait positionner le lidar dans le bas du robot (pour “voir” le bord de la table) ce qui est possible dans le gros robot mais pas dans le petit.

L'idée actuelle et donc de positionner un hokuyo dans le bas du gros robot (la “tête” en bas si besoin) et un autre dans le pavé droit d'environ 8x8x10 sous la plateforme dans le haut du petit robot.

Parlons de l'Hokuyo en particulier, et plus précisément de notre modèle. Comment communique-t-on avec lui et sous quelle forme ?

L'hokuyo est relié à un ordinateur par câble USB. Il va créer une interface serial dans les périphériques (/dev/ttyACMx). Nous communiquons avec l'hokuyo à l'aide d'une librairie C : v0.8.18. Nous avons utilisé la version 1.0.4 auparavant mais elle beugue avec deux Hokuyo.

Toute la documentation est disponible sur le site. La documentation est plutôt faible et incomplète. Je vais donc essayer de fournir les informations essentielles à l'utilisation de l'hokuyo et les quelques pièges dans lesquels il ne faut pas tomber.

L'hokuyo renvoit simplement un tableau de long. Cela correspond à la distance des points. Nous avons donc des distance associées à des index. Il est facile de transformer index en angle à l'aide d'une fonction présente dans la librairie. On peut donc conclure que l'hokuyo nous renvoi des coordonnées radiales. Un simple cosinus/sinus les transformera en coordonnées cartésiennes. Je ne m'étend pas sur le sujet, les exemples de la documentation sont facile à comprendre.

Voici une liste de ce que l'on peut faire :

• Récupérer un tableau de mesure
• Demander à l'hokuyo de mesurer en continu ou juste un certain nombre de fois.
• Demander à l'hokuyo de “sauter” des mesures
• Définir le champ de mesure en angle
• Définir un champ de mesure en distance (attention, c'est en radial)

• L'hokuyo scanne dans un plan, certes, mais en réalité c'est un ensemble de points, à grande distance, l'écart entre deux points n'est pas négligeable (plusieurs centimètres !), il faut donc en tenir compte dans les algos de filtrage et de reconnaissance de robot.

• Il ne sert à rien de chercher à récupérer les informations d'intensité et de multi-echo, bien que présent dans la librairie, le matériel ne le permet pas.

Angle, Index, Step : Il existe trois manière de représenter l'angle d'une mesure : son angle (ce que l'on utilise), son index (son numéro de mesure) et sa step (son angle en représentation interne). Petit détail au passage : 0° est le point “en face” de l'hokuyo.

En pratique voila ce qu'il faut retenir :

• Nous utilisons l'angle, on peut le récupérer à l'aide d'une fonction type index_to_angle.
• L'index est, comme son nom l'indique, l'index de point dans le tableau de retour.
• Si le domaine de mesure ne change pas, l'angle associé à un index reste le même
• Les steps sont la représentation interne de l'angle, cela ne nous intéresse pas et n'est pas utilisable pour nous. On les utilise uniquement pour définir le domaine de mesures. On se sert alors d'une fonction du type angle_to_index.

Données max :
Un point bien angigu qui m'a vallu de bonnes prises de tête. Une fonction est disponibles pour connaitre le nombre maximal de points que renverra l'hokuyo. Cette fonction renvoit une donnée liée au matériel et non à la configuation de celui-ci.

Ce qu'il faut comprendre est :

• Si j'utilise l'hokuyo “par défaut”, que je demande le nombre max de mesure, l'hokuyo me répondra par exemple 700. Je vais récupérer les données, je récupère 700 données.
• Si je paramètre l'hokuyo pour effectuer des mesures sur uniquement 120° (au lieu de 240) et que le lui demande le nombre max de mesure, il répondra AUSSI 700 mais lorsque je demande les données, je n'en recoit que 350.
• Ainsi si vous souhaitez connaitre le VRAI nombre de données que renvoit l'hokuyo, il faut faire une mesure, et l'hokuyo renverra le nombre de données qu'il a envoyé. Si le domaine de mesure ne change pas, le nombre de données renvoyées ne changera pas non plus.

Multi-echo / Intensité :
Je me répète ici, mais inutile de chercher à récupérer les données de multi-echo et d'intensité. Bien que les fonction existent, le matériel ne le supporte pas.

Quelques précision sur des points importants liés au matériel.

Bien que la consommation de l'hokuyo soit de 2.5W (0.5A à 5V), il est nécessaire d'utiliser le câble double, ou une alimentation externe. Si une alimentation externe est utilisée, attention tout de même aux raccordement, l'expérience m'a appris que si vous branchez cela n'importe comment, les données seront corrompues et il sera impossible d'utiliser l'hokuyo ! Si on utilise un long câble pour les données ou l'alimentation, attention à la résistance interne du câble, qui est loin d'être négligeable au delà de quelques dizaine de cm !

L'hokuyo scanne dans un plan, il est donc important que celui-ci soit bien droit lors des matchs. S'il est de travers, la précision sera moins bonne, et on risque même de ne plus détecter les robots !
Pendant un moment, nous avons monté les Hokuyos sur une base avec une articulation permettant de modifier l'horizontalité et le cap du capteur. En réalité, les plateformes des tables à la coupe sont suffisamment horizontale mais leur cap et leur hauteur ne sont pas forcément très précis.

Attention au temps de mise en place ! A la coupe, on (2 personnes max.) ne dispose que de 3 minutes pour que les robots et les tourelles soient prêts. Avant 2017, les Hokuyos occupaient 2 des 3 tourelles et étaient reliés par câble USB à la Raspberry Pi de la troisième tourelle. Il fallait au moins 3 minutes pour tout mettre en place. En 2017, la sortie de la Raspberry Zero nous a permis de l'intégrer directement à la balise.

Voici les possibilités offertes par un LiDar, nous n'avons pour le moment utilisé que la première technique.

L'hokuyo renvoi une liste d'entiers, chacun étant la distance de l’écho à un certain angle. Il faut alors traiter ces données. La première étape est de grouper ces points en cluster.

Voici l'algo utilisé :

tab[] tableau des distance
culsters[] tableau des clusters
nb_clusters = 0
found_cluster = false

Pour i de 0 à size(tab):
  found_cluster = false
  Pour j de i-1 à max(0, i - search_size): //parcours inverse
    Si distance(tab[i], tab[j]) < distance_max:
      clusters[i] = clusters[j]
      found_cluster = true
      break
      
  Si found_cluster == false :
    clusters[i] = nb_clusters
    nb_cluster += 1
    

On note les variables de réglage search_size et distance_max :

• search_size est un entier désignant le nombre points précédents parmi lesquels chercher un cluster existant
• distance_max désigne la distance en dessous de laquelle un point est rattaché à un cluster existant

Cet algorithme se base sur le fait que les liste renvoyées par l'hokuyo sont triées par angle.

Suite à cela, on dispose de groupes (clusters) de points. Le groupe point étant stocké dans le tableau cluster[]. On peut alors filtrer les groupes ne comporte qu'un faible nombre de points, qui ont de fortes chances d'être des parasites.

Il faut ensuite convertir les coordonnées du repère polaire au repère cartésien. Chaque index correspond à un angle, que l'on peut récupérer par une fonction de la librairie. Selon la manière dont seront traités les points et les groupes, il peut être intéressant de précalculer les sinus/cosinus en fonction de l'angle. En effet, a chaque scan de l'hokuyo, un même index correspondra toujours au même angle. Ainsi il suffira d'initialiser l'hokuyo puis de calculer les tableaux d'équivalence index↔sin et index↔cosinus.

Il suffira alors pour convertir les points dans le repère cartésien d'effectuer :

x[i] = tab[i]*sin_tab[i]
y[i] = tab[i]*cos_tab[i]

au lieu d’appeler pour chaque point, à chaque scan, les fonction sinus et cosinus.

Dans l'absolu, l'hokuyo devrait renvoyer un nombre de cluster égal au nombre de robots sur la carte. Or il peut arriver que l'on détecte un objet en trop (un arbitre qui passerait sa main au dessus de l'aire de jeu par exemple). On se propose donc de filtrer les cluster de la manière suivante :

Calculer la taille de chaque cluster (en millimètres, pas en nombre de points)
Trier les cluster par taille décroissante
Garder les X premiers (X étant le nombre de robots attendus)

Un autre avantage de ce tri est qu'il peut permettre de différencier les deux robots adverses en fonction de la taille de la balise. Par exemple, on peut mettre une balise plus petite sur le petit robot adverse afin de le reconnaitre. Le gros robot sera donc toujours avant le petit dans la liste des robots détectés.

On peut difficilement différencier les balises car elles ne représentent en pratique que quelques points, qu'une balise trop petite peut passer entre les mailles du filet et parce que le bruit et la précision ne permettent pas de différencier le diamètre de la balise.

En 2017, on a donc calculé le diamètre des clusters et on a supprimé ceux qui étaient trop grands (main, bras) ou trop petits (bruit). On a conservé tous les groupes restants.

Afin d'enrichir la technique précédente, il serait intéressant de tracker les robots adverses (s'ils en ont 2) pour différencier leur petit robot du gros. Des tests ont été faits en 2015 pour cela mais n'ont débouché sur rien. D'une époque à l'autre (notre Hokuyo tourne à 10 Hz), les robots ont le temps de beaucoup bouger, ralentir, se croiser, passer dans une zone d'ombre des LiDars. Un algo sur la distance par rapport à l'époque précédente ou une interpolation sur la vitesse ne sont pas vraiment suffisantes.

L'idée peut donc être d'utiliser un filtre de Kalman (vu en SY15). Il simule les systèmes à surveiller (les positions, vitesses, accélération des robots adverses ici) pour pouvoir estimer leur position à l'époque suivante.

Cette technique, bien documentée sur le Net, permet -comme son nom l'indique- de créer/enrichir une carte de l'environnement et de se positionner sur celle-ci. Cela ne concerne que le cas où l'Hokuyo est placé dans le robot à une hauteur suffisamment basse pour percevoir des éléments de la table.

Si l'hokuyo est suffisamment bas, il est possible de recaler l'odométrie sans aller taper les bords. C'est surtout intéressant pour le décalage en angle, car c'est ce qui arrive en premier (voir notre technique d'odométrie) et qui a pour conséquence la création d'un grand décalage en position.

On peut imaginer réaliser un recalage à chaque rotation du robot (éventuellement supérieur à un seuil) ou bien faire de la fusion de capteur : un filtre de Kalman (encore lui !) se sert de l'incertitude sur chaque capteur pour estimer une valeur plus proche du mesurande à partir de plusieurs mesures. Cette technique suppose deux choses : qu'on puisse considérer que la mesure de l'odométrie par roues codeuses et par LiDar soit simultanée (vrai à l'arrêt) et qu'on puisse estimer l'incertitude sur l'odométrie.