Commande orientée terrain : faire qu'avant veuille dire avant

Le problème que vous aurez dans les dix secondes

Construisez un robot mecanum, saisissez la manette, poussez le manche vers l’avant. Le robot s’éloigne de vous. Bien.

Maintenant faites-le pivoter de 180° et poussez à nouveau le manche vers l’avant. Le robot fonce sur vous, parce qu’avant signifie avant pour le robot et que le robot vous fait désormais face. Le cerveau de chaque pilote fait la traduction en miroir pendant une seconde ou deux, se trompe, et le robot heurte quelque chose.

C’est déjà mauvais sur un robot à entraînement différentiel, où au moins le robot avance toujours le long de son nez. Sur une base holonome c’est bien pire, parce que le robot peut désormais pointer d’un côté et se déplacer d’un autre, sans aucun indice visuel de l’endroit où « le côté » a atterri. Dix secondes de rotation et personne dans la pièce ne peut le piloter.

Le correctif est une matrice de rotation.

Deux repères, une transformée

Il y a deux façons d’interpréter le manche :

• Orienté robot (égocentré robot). Le manche est une commande dans le repère propre du robot — base_link, si vous parlez ROS. Poussez vers l’avant, le robot roule le long de son nez.
• Orienté terrain (égocentré terrain). Le manche est une commande dans le repère du sol. Poussez vers l’avant, le robot va au nord, quel que soit l’endroit où pointe son nez.

La transmission ne comprend jamais que le repère robot — la cinématique mecanum y est écrite. Donc la commande orientée terrain consiste à tourner la commande du pilote vers le repère robot avant que la cinématique la voie.

Si le cap du robot est θ (mesuré dans le sens antihoraire depuis l’axe x du terrain), tournez la commande de −θ :

vx_robot = vx_field · cos(θ) + vy_field · sin(θ)
vy_robot = −vx_field · sin(θ) + vy_field · cos(θ)

Moins thêta, pas thêta. Se tromper de signe donne un robot qui semble correct en pointant vers l’avant et roule exactement dans la mauvaise direction une fois tourné de 90°, ce qui fait dix minutes enrageantes.

La rotation n’est pas touchée. ω signifie la même chose dans les deux repères — un pivot est un pivot — donc elle passe directement.

Le tout, en code

#include <math.h>

// stickX: forward on the stick, positive = away from the driver (field +x)
// stickY: left on the stick,    positive = to the driver's left (field +y)
// stickW: twist, positive = counter-clockwise
void driveFieldOriented(float stickX, float stickY, float stickW) {
  float theta = imuYawRadians() - headingOffset;   // see "the reset" below

  float c = cosf(theta);
  float s = sinf(theta);
  float vx =  stickX * c + stickY * s;
  float vy = -stickX * s + stickY * c;

  driveMecanum(vx, vy, stickW);   // the inverse kinematics, unchanged
}

Six lignes, et c’est la différence entre un robot que les gens peuvent piloter et un qu’ils ne peuvent pas. Notez que driveMecanum() n’apprend jamais rien de tout ceci — l’orientation terrain est un chapeau posé sur la commande, pas un changement de la transmission.

Obtenir θ

Il faut un angle de lacet, et il y a trois façons de l’obtenir, par ordre croissant de durée d’honnêteté.

Le lacet issu d’un gyro est une intégrale, donc le biais devient de la dérive linéairement :

// Integrate yaw rate, with the bias you measured while it sat still.
float rate = gyroZ() - gyroBiasZ;             // rad/s
if (fabsf(rate) < 0.004f) rate = 0.0f;        // deadband: kill the last of the creep
yaw += rate * dt;

Deux détails qui comptent plus que le choix du capteur :

• Mesurez le biais à chaque démarrage, robot immobile. Moyennez 500 échantillons sur une ou deux secondes avant tout mouvement. Un MPU6050 brut sans suppression du biais dérive de dizaines de degrés par minute ; avec, d’une poignée.
• Une bande morte sur la vitesse, pas sur l’angle. Mettre une bande morte sur l’angle perd les virages lents réels. En mettre une sur la vitesse ne jette que le plancher de bruit, qui est exactement ce qui s’intègre en dérive.

Un accéléromètre ne peut pas aider ici. Il donne roulis et tangage en observant la gravité, et c’est pourquoi un filtre complémentaire corrige l’inclinaison magnifiquement — mais la gravité est parallèle à l’axe de lacet, donc elle ne dit rien de la direction où vous regardez. Le lacet exige un magnétomètre ou il doit dériver.

La réinitialisation, qui n’est pas optionnelle

Deux choses imposent un zéro manuel :

1. La dérive. Après quelques minutes, le nord du robot et celui du pilote se sont séparés, et le correctif est devenu le problème.
2. L’orientation de démarrage. Le zéro de l’IMU est là où le robot pointait à la mise sous tension, rarement là où se tient le pilote.

Gardez donc un décalage et un bouton :

float headingOffset = 0.0f;

void resetFieldHeading() {          // bind to a button the driver can hit blind
  headingOffset = imuYawRadians();  // "whatever you are pointing at is now forward"
}

La convention vers laquelle tout le monde converge : pointez le robot loin du pilote, appuyez sur le bouton, pilotez. Cela prend une demi-seconde et corrige les deux problèmes à la fois. Chaque robot holonome téléopéré sérieux a ce bouton, et chaque équipe qui l’a sauté l’a ajouté après le premier match.

Orienté terrain pour l’autonomie aussi

Ce n’est pas seulement une aide au pilotage. Tout comportement autonome dont la commande s’exprime naturellement sur le sol veut la même transformée :

• « Avancez de 300 mm vers le but » — le but est en coordonnées terrain, donc la commande aussi.
• « Rippez pour garder la cible caméra centrée en l’orbitant » — l’orbite est en repère terrain, le pivot en repère robot, et la commande orientée terrain permet d’écrire les deux dans la même ligne.
• Tout ce qui est en aval d’un planificateur de trajectoire, qui planifie sur une carte donc en repère terrain.

En ROS c’est la transformée odom → base_link qui fait le même travail, et c’est pourquoi TF2 existe et pourquoi cmd_vel est explicitement défini dans base_link. La commande orientée terrain est la version artisanale d’une recherche que vous demanderiez sinon à TF2.

Le voir

Le simulateur de traction mecanum a une seule manœuvre qui établit le point sans un mot : tenez le manche vers l’avant, continuez de pivoter, et changez de repère.

Même manche, mêmes roues, même cinématique. La seule différence est six lignes de trigonométrie.

Dépannage

Suite

L’orientation terrain fait signifier ce qu’il faut à la commande. La commande de vitesse des roues en boucle fermée fait que les roues la délivrent vraiment — sur une base holonome les deux vont par paire, parce qu’un robot dont les moteurs s’affaissent inégalement est hors cap avant que l’IMU ait son mot à dire.