Traction : estimateur de roue et odométrie

App/traction.c est le module le plus dense du firmware, et celui qui a coûté le plus de séances. Il fait trois choses : reconstruire la position de chaque roue à partir d’un capteur qui ment une fois par tour, asservir leur vitesse, et intégrer l’odométrie.

Tout y est sans ROS : la tâche traction tourne à 100 Hz et ne dépend ni de l’agent, ni du réseau, ni de la Raspberry.

Le capteur de l’AX-18A, et son trou

L’AX-18A rapporte une position absolue sur 1024 pas, mais seulement sur 300 degrés de rotation. Les 60 degrés restants sont un secteur aveugle où le curseur est hors piste.

AX_POS_MAX      1024        pas visibles
AX_BLIND_COUNTS  205        pas équivalents dans le trou (1024 × 60/300)
AX_TURN_COUNTS  1229        un tour complet
AX_RANGE_RAD    5,236 rad   les 300 degrés visibles

Important

Un tour de roue vaut 1229 pas, pas 1024. Recaler sur 1024 revient à dire qu’un tour fait 300 degrés : on perd un sixième du tour à chaque passage.

Le défaut ne se voyait pas à basse vitesse — la roue reste plusieurs échantillons collée à la butée, l’aveuglement est détecté, et le delta fautif est jeté. Vite, elle saute par-dessus la butée entre deux lectures : plus d’aveuglement détecté, et le recalage retranche 1024 pas à un mouvement qui n’en valait que 240. La vitesse mesurée devenait négative alors que la roue avançait (mesure du 2026-09-12 : −0,26 rad/s à gauche pour une consigne de 4,8), et la boucle poussait la puissance à fond en voyant une roue qui recule.

Pourquoi la position, et pas `Present Speed`

En mode roue, l’AX rapporte dans Present Speed un rapport de puissance (de l’ordre de 0,1 %), pas des tours par minute. Le publier comme une vitesse mettait un rapport de couple dans un champ normalisé en rad/s : faux, et silencieusement faux.

La vitesse est donc la dérivée de la position, filtrée (VEL_FILTER_ALPHA = 0,25).

Traverser le secteur aveugle

Dans le trou, le registre ne saute pas : il sature, à 0 ou à 1023. Un capteur figé ressemble à une roue arrêtée. Le correcteur voyait une vitesse tomber à zéro, poussait à fond, puis la position ressortait d’un coup — une pointe à −94 rad/s mesurée, et un à-coup visible à chaque tour.

On détecte donc la saturation, pas le saut, avec une hystérésis :

AX_BLIND_ENTER   8     on entre si  raw <= 8   ou  raw >= 1015
AX_BLIND_EXIT   28     on sort si   raw >= 28  et  raw <= 995

Pendant la traversée : l’estime

blind_estimate() fait avancer la position à la dernière vitesse connue, avec deux garde-fous.

La borne physique. Tant que le capteur ne voit rien, la roue n’a pas pu dépasser le bord opposé du trou. On plafonne donc l’estime à ce bord, plus une marge de trois échantillons de course. Quand on l’atteint sans ressortir, c’est que la roue s’est arrêtée dedans : sa vitesse passe à zéro.

Sans cette borne, un robot à l’arrêt avec une roue garée dans le trou se croyait en train de rouler : 600 rad en cinq minutes, puis un bond de 95 tours au redémarrage. Et ça arrive une fois sur six par roue, à chaque arrêt.

La consigne prime en décélération. Si |cmd_rads| < |velocity_rads|, on prend la consigne : l’asservissement fait suivre la rampe à la roue, et supposer qu’elle continue à sa vitesse d’avant l’ordre d’arrêt la faisait courir jusqu’au bord du trou alors qu’elle était déjà immobile — 107 plafonnements sur une seule roue en un seul arrêt.

À la sortie : le crédit

L’estime seule sous-compte, et l’erreur se répète à chaque tour : −8 % de biais permanent sur la roue gauche, mesuré le 2026-09-12.

Or à la sortie on sait ce que la roue a fait : elle est entrée à un compte connu, elle ressort à un autre, et entre les deux il y a exactement les 60 degrés aveugles. On recale donc la position sur cette somme, et la vitesse sur le temps mis à la parcourir.

Warning

Pas de plafond sur la durée de traversée. Il y en a eu un (1 s) et c’était le bug du 2026-09-12 : blind_ms n’est capturé qu’à l’entrée dans le trou, donc dt_trou ne fait que croître tant qu’on ne sort pas. Un plafond fixe transforme la première traversée un peu lente en blocage permanent — plus aucune sortie ne peut jamais repasser dessous, et la roue reste aveugle le reste de la session.

La seule garde qui compte est physique, sur la vitesse déduite (WHEEL_MAX_RADS = 15 rad/s).

Rejeter les fantômes

Dans ses 60 degrés aveugles, l’AX ne sature pas proprement : le curseur est hors piste et le registre rend ce qu’il veut. Séquence mesurée, roue montant vers 1023 : 1012, 1023, 565, 0. Le 565 est une valeur fantôme en plein milieu de la plage. Prise pour une sortie, elle créditait −2 radians en 150 ms, soit −13 rad/s à contresens.

On sait pourtant de quel côté la roue doit ressortir : elle tournait dans un sens en entrant, elle n’a pas pu s’inverser en un dixième de seconde.

Sens à l’entrée	Sortie acceptée
comptes croissants	`raw < AX_EXIT_NEAR` (200) — juste après zéro
comptes décroissants	`raw > 1023 − AX_EXIT_NEAR` — juste avant 1023
roue à l’arrêt	près de l’un ou l’autre bord

Le dernier cas acceptait autrefois n’importe quelle valeur, et c’était le trou : une roue arrêtée dans le secteur aveugle au bout d’une ligne droite, puis relancée pour pivoter, prenait un fantôme de milieu de course pour sa sortie. Mesuré le 2026-09-24 sur le premier carré UMBmark : des sauts de −3,4 rad au démarrage des virages, et des virages physiques de 180 à 230 degrés pour 120 dans l’odométrie.

Note

La position n’est écrite que si le crédit tient debout. Elle l’était avant le contrôle de vitesse : un fantôme rejeté laissait quand même un saut de 3,4 radians dans l’odométrie — invisible sur la vitesse publiée, bien réel sur la distance.

Remise à zéro pendant la traversée

Dernier cas, trouvé le 2026-09-26. Le crédit compte tout ce que la roue a fait depuis son entrée dans le trou, remise à zéro de l’odométrie comprise.

Or entre deux essais on porte le robot, roues libres : ce qu’on leur fait faire dans le trou est invisible, et tombait dans l’essai suivant. Mesure du troisième carré UMBmark : 0,66 rad de trop à la roue droite dès le départ, soit +6,9 degrés de cap en un quart de seconde pour un gyroscope qui n’en voyait que 0,8. Tout le carré s’en trouvait décalé.

traction_reset_odometry() incrémente donc un compteur que la tâche traction observe. Une roue déjà dans le trou au moment de la remise à zéro oublie son point d’entrée : à la sortie, on ne compte que le chemin fait après.

L’asservissement de vitesse

PI avec terme direct, par roue, à 50 Hz chacune (la tâche alterne à 100 Hz).

Constante	Valeur	Rôle
`VEL_KFF`	—	terme direct, en unités servo par rad/s : l’essentiel de la commande
`VEL_KP`	15	gain proportionnel de régime
`VEL_KP_BRAKE`	120	gain de freinage, uniquement en décélération
`VEL_KI`	90	gain intégral
`VEL_I_MAX`	450	écrêtage de l’intégrale, en second rideau
`ROBOT_ACCEL_MS2`	1,0	rampe d’accélération au sol, dans les deux sens
`DXL_TORQUE_LIMIT`	800/1023	plafond de couple du servo

Anti-emballement conditionnel

On n’intègre pas dans deux cas, et l’écrêtage seul ne suffisait pas.

Pendant le secteur aveugle. La vitesse y est gelée, pas mesurée : intégrer une erreur calculée sur une valeur figée charge l’intégrale pour rien, et elle se décharge d’un coup au retour — un à-coup visible, une fois par tour de roue.
Quand la sortie est déjà saturée et que l’erreur pousse dans le même sens. À pleine vitesse, la limite de couple empêche d’atteindre la consigne, l’erreur reste positive, et l’intégrale montait à sa borne en quatre secondes. On demandait alors 3 rad/s et la sortie valait encore 648 — terme direct 258, intégrale +450. La roue continuait à pousser une seconde et demie après l’ordre de ralentir, le temps que l’intégrale se décharge, et devait même passer négative pour freiner. Invisible à vitesse constante, très visible dès que la consigne change — donc en permanence sous navigation.

Freiner, puis couper

À l’arrêt, on ne coupe pas le couple : on freine avec VEL_KP_BRAKE, puis on coupe quand la roue est vraiment arrêtée (WHEEL_STOP_RADS = 0,3 rad/s), ou au bout de WHEEL_BRAKE_MAX_MS (400 ms), ou tout de suite si l’échantillon n’est pas valide.

Ce dernier cas compte : dans le secteur aveugle la mesure est figée à sa dernière valeur, et freiner contre elle pousserait une roue déjà arrêtée en arrière.

Sans le gain de freinage, les virages dépassaient de 13 à 22 degrés sur leur élan.

L’odométrie

On intègre les positions, pas les vitesses : la position déroulée est déjà ce que l’estimateur reconstruit au mieux, secteur aveugle compris. Intégrer la vitesse ajouterait une seconde source de dérive par-dessus la première.

d_left  = Δposition_gauche * cfg->wheel_radius_left;
d_right = Δposition_droite * cfg->wheel_radius_right;

d_center = 0.5f * (d_left + d_right);
d_theta  = (d_right - d_left) / cfg->wheel_separation;

/* Point MILIEU : on avance selon le cap a mi-pas. */
mid      = pose_theta + 0.5f * d_theta;
pose_x  += d_center * cosf(mid);
pose_y  += d_center * sinf(mid);

Un rayon par roue depuis le 2026-09-26 : c’est là qu’atterrit la correction UMBmark. Quatre endroits utilisent le rayon — odométrie, vitesses publiées, cinématique inverse et rampe d’accélération — et ils passent tous par wheel_radius_of() pour qu’aucun ne puisse rester sur l’autre roue.

La procédure de mesure est décrite côté utilisateur, page Calibrer l’odométrie : https://docs.ratbot.org/calibration.html

Note

/odom est de l’odométrie de roues pure. La centrale inertielle n’y est fusionnée nulle part, et c’est délibéré : elle reste un témoin indépendant. Elle a démasqué deux défauts à ce titre — les 12 degrés de dépassement par virage dus à la QoS de cmd_vel, et les 6,9 degrés du secteur aveugle.

C’est aussi ce qui rend la calibration UMBmark valide : la méthode exige que le robot soit piloté sur sa propre odométrie, sans référence extérieure.

Les compteurs de diagnostic

Le bloc wheel estimator de /diagnostics expose, roue par roue, ce que l’estimateur a fait de chaque échantillon. Les compteurs globaux mélangeaient les deux roues, alors que le 2026-09-24 la roue droite comptait 91 degrés de trop sur 20 tours et la gauche 17.

Compteur	Ce qu’il signale
`credits`	traversées du trou créditées à la sortie — le fonctionnement normal, environ une par tour de roue
`ghosts`	sorties rejetées comme fantômes
`rejects`	échantillons écartés pour vitesse aberrante
`resyncs`	reprises après trois écarts d’affilée
`clamps`	estime plafonnée au bord du trou — normal à l’arrêt, suspect en roulant
`stale`	lectures espacées de plus d’une seconde

Tout à zéro sauf credits, c’est un estimateur qui ne fait que son travail. Un compteur qui grimpe sur une seule roue désigne le chemin à regarder.

Le XL430-W250

L’autre servo de roue, au choix : wheel_servo_model vaut 18 (AX-18A) ou 1060 (XL430-W250), lu une fois par traction_init() (use_xl). Écrit le 2026-10-19 avant d’avoir les moteurs : testé au banc avec un faux XL430, jamais encore sur un vrai.

Ce qui change

	AX-18A	XL430-W250
protocole	1.0, somme de contrôle	2.0, CRC-16 et bourrage (`dxl_frame2.c`)
capteur	1024 pas sur 300°, secteur aveugle	4096 pas sur 360°, multitour en mode vitesse
commande	une puissance en boucle ouverte	une vitesse, que le servo régule lui-même
asservissement	terme direct, PI, gain de freinage (ci-dessus)	celui du servo ; seule la rampe reste à la carte
vitesse lue	dérivée filtrée de la position	`Present Velocity`, mesurée par le servo
lecture par pas	position, puis charge toutes les 50 ms	PWM, charge, vitesse et position en un échange (12 octets à l’adresse 124)

Tout le chapitre du secteur aveugle — estime, crédit, fantômes — est sans objet : aucun de ces chemins n’est emprunté. Restent deux gardes :

un écart de position au-delà de WHEEL_MAX_RADS n’est pas crédité. Il a une cause connue : un servo qui redémarre (chute de tension) reprend sa position multitour entre 0 et 4095, et le saut passerait pour des dizaines de tours. Il revient aussi couple coupé : la roue est déclarée absente (rejects) et remise en service comme une roue qui revient. Limite : un servo qui redémarre avant d’avoir fait un tour ne produit aucun saut, et n’est pas vu ;
plus d’une seconde entre deux lectures : rien n’est crédité (stale).

La différence de positions est calculée sur 32 bits en non signé, puis convertie : elle reste juste au repli de 2³¹, sans comportement indéfini.

La consigne

Goal Velocity est en unités de 0,229 tr/min (0,02398 rad/s). Une consigne au-delà de Velocity Limit est refusée par le servo, pas écrêtée : la carte relit cette limite à la mise en service (265 d’usine, 6,4 rad/s) et écrête avant d’envoyer. Une consigne inchangée n’est pas renvoyée.

Profile Acceleration est mis à 0, sans rampe côté servo : la rampe de la carte (ROBOT_ACCEL_MS2) reste la seule, pour que l’accélération au sol soit la même avec les deux servos. À l’arrêt, la consigne nulle fait tenir la roue à vitesse nulle : c’est le freinage, sans gain dédié.

La mise en service

Une instruction refusée par le servo (champ ERR, par exemple une consigne hors limite) est un échec de l’échange, pas un succès : la consigne serait sinon tenue pour appliquée et jamais renvoyée. Elle est comptée dans refused (bloc dynamixel bus). La réponse est cherchée derrière l’écho par son instruction (0x55), pas par sa position (dxl2_find_status(), testée au banc).

wheel_setup_xl() : ping 2.0, modèle vérifié (1060, sinon rien n’est écrit), couple coupé, mode vitesse écrit seulement s’il le faut (EEPROM, refusée couple actif), limite relue, rampe du servo à 0, consigne nulle, couple. Le débit (1 Mbps) et les identifiants se règlent avant le montage, au Dynamixel Wizard ; la carte ne les écrit pas.

Si aucune roue ne répond, le balayage essaie aussi le protocole 2.0, aux deux débits : un XL430 d’usine apparaît comme conn2 id1 57k p2 dans le bloc dynamixel bus.

Ce que lit la sécurité

output_ratio est, pour l’AX-18A, la puissance commandée ; pour le XL430, le PWM mesuré (Present PWM / 885). Les deux montent quand la roue force. La sécurité inertielle (MOTION_GUARD_STALL_POWER 0,40, MOTION_GUARD_DRIVE_POWER 0,05) et le zéro du gyroscope (STILL_WHEEL_POWER 0,01) ont été réglés sur l’AX-18A : à revérifier sur le XL430, en particulier le PWM d’une roue tenue à l’arrêt.

À vérifier à l’arrivée des moteurs

l’échange 2.0 sur le vrai bus : délai de réponse, aucune trame perdue (succeeded = exchanges dans dynamixel bus) ;
la tension : 12 V maximum annoncés, 12,6 V pour une 3S pleine ;
la vitesse : 57 tr/min à vide sous 11,1 V, max_linear à régler ;
les seuils de la sécurité, ci-dessus ;
la calibration UMBmark, à refaire.

Voir aussi

Le banc de test sur PC — les vérifications qui rejouent tout ce qui précède sur PC, avec du faux matériel (AX-18A et XL430).
Architecture de l’application firmware — l’architecture des tâches.