Commande de vitesse des roues en boucle fermée avec encodeurs

La PWM est une tension, pas une vitesse

analogWrite(motorPin, 180) ne demande pas une vitesse. Il demande environ 70 % de la tension de batterie, et la vitesse produite dépend de la charge sur la roue, du frottement dans ce réducteur, de l’état de charge, et de la température des balais.

C’est pourquoi un robot à deux moteurs identiques et PWM identiques roule en arc. Ils n’ont jamais été identiques — un réducteur est plus serré, un jeu de balais est plus neuf, et à la même tension ils tournent à 3–8 % d’écart. Sur trois mètres cela fait une courbe visible.

Sur un robot à entraînement différentiel, on peut masquer cela avec une constante de trim. Sur une base mecanum on ne peut pas, parce que la cinématique inverse remet aux quatre roues des cibles follement différentes — dans l’exemple calculé là-bas, une roue reçoit 0,020 m/s et une autre 0,580 m/s pour la même commande — et ces rapports sont ce qui décide de la direction où va le robot. Une roue lente de 5 % ne rend pas le robot légèrement lent ; elle l’envoie ailleurs.

Boucler chaque roue sur sa vitesse mesurée est ce qui corrige cela, et c’est le même correctif pour chaque robot à plus de deux roues ou tout besoin d’arriver quelque part.

Mesurez la vitesse que vous avez vraiment

Partez des impulsions d’encodeurs en quadrature. La conversion est de la géométrie fixe :

counts per wheel revolution = CPR_motor × gear ratio × 4     (×4 for quadrature edges)
metres per count            = 2πr / counts per revolution

Pour un encodeur moteur 12 CPR derrière un réducteur 30:1 sur des roues de 97 mm : 12 × 30 × 4 = 1440 impulsions/tour, et 2π × 0,0485 / 1440 = 212 µm par impulsion.

Puis la partie que tout le monde rate :

// DON'T: counts since last loop, divided by loop time.
float speed = (count - lastCount) * METRES_PER_COUNT / dt;

À 100 Hz et 0,05 m/s cela fait une impulsion par échantillon. Votre signal de vitesse est 0, 212, 0, 212 µm — un signal carré avec ±100 % d’ondulation, et un terme D l’amplifiera en un moteur qui bourdonne. Trois sorties, et vous voulez généralement la première :

1. Échantillonnez plus lentement que vous ne commandez. Calculez la vitesse sur 20 ms (50 Hz) et faites tourner la boucle à 50 Hz. Plus d’impulsions par fenêtre, moins de quantification, et pas de retard de filtre.
2. Mesurez le temps entre fronts plutôt que les fronts dans un temps. Excellent à basse vitesse, gênant à haute vitesse, et cela exige des horodatages d’interruption.
3. Filtrez passe-bas la vitesse. Le moins cher, et cela vous coûte de la marge de phase — la chose dont vous avez le moins.

// A 50 Hz velocity estimate, with the counter read atomically because an
// encoder ISR is writing it.
noInterrupts();
long count = encoderCount;
interrupts();

float dt = (now - lastTime) * 1e-6f;             // micros(), so no ms rounding
float v = (count - lastCount) * METRES_PER_COUNT / dt;
lastCount = count; lastTime = now;

volatile long encoderCount et le crochet noInterrupts() ne sont pas optionnels : une lecture 32 bits fait plusieurs instructions sur AVR, et une ISR tombant au milieu vous remet un nombre qui n’a jamais été vrai.

L’anticipation d’abord, le PID ensuite

L’instinct est de saisir le PID. Faites d’abord les faciles 90 %.

La vitesse stabilisée d’un moteur CC est presque linéaire en tension appliquée, donc si vous connaissez la vitesse voulue, vous pouvez calculer l’essentiel de la PWM :

pwm ≈ kS·sign(v) + kV·v

• kV — la pente. Combien de PWM achète un m/s.
• kS — l’ordonnée. La PWM juste pour vaincre le frottement statique, avant que la roue bouge.

Mesurez les deux en dix minutes :

1. Mettez le robot sur cales pour que les roues tournent libres.
2. Montez la PWM lentement jusqu’à ce que la roue commence juste à tourner. C’est kS, typiquement 25–45 sur 255.
3. Échelonnez la PWM à 100, 150, 200, 250 et notez la vitesse stabilisée à chacun.
4. Ajustez une droite. Sa pente est kV, et elle doit extrapoler vers à peu près votre kS.

Maintenant la boucle n’a qu’à corriger l’erreur entre cette estimée et la réalité — qui est petite, donc les gains peuvent être petits, donc le tout est stable et silencieux :

float feedforward = kS * sign(target) + kV * target;
float output = feedforward + kP * error + kI * integral;

Sans anticipation, l’intégrateur doit construire toute la commande depuis zéro à chaque changement de cible. C’est lent, cela dépasse, et c’est la raison pour laquelle tant de boucles moteur réglées à la main finissent avec un kI énorme et un robot qui tressaute.

Le contrôleur

P et I. Pas D.

struct WheelLoop {
  float kS, kV, kP, kI;
  float integral = 0, target = 0;

  int update(float measured, float dt) {
    float error = target - measured;
    float ff = kS * (target > 0 ? 1 : target < 0 ? -1 : 0) + kV * target;
    float unclamped = ff + kP * error + kI * (integral + error * dt);
    int   output    = (int)constrain(unclamped, -255, 255);

    // Conditional integration: only accumulate if the output is NOT saturated,
    // or if the error would wind the integrator back toward the middle.
    // Otherwise a stalled wheel builds a command it cannot use and then keeps
    // driving for a second after you let go of the stick.
    if (fabsf(unclamped) < 255.0f || (error * unclamped) < 0) integral += error * dt;

    return output;
  }
};

Pourquoi pas de dérivée : le terme D dérive le signal de vitesse, qui est déjà une différence d’impulsions d’encodeurs. Vous dérivez deux fois, donc amplifiez le bruit de quantification deux fois, et sur une roue il n’y a presque rien à amortir de toute façon. Une boucle de vitesse avec PI et bonne anticipation est la réponse standard, et ajouter D est l’une des façons les plus courantes de rendre une boucle qui marche pire. (Une boucle de position est différente — voir la commande en cascade.)

L’anti-emballement est la partie qu’on ne peut sauter. Tenez le robot contre un mur avec une commande de vitesse appliquée, et un intégrateur naïf monte à pleine échelle en quelques secondes. Lâchez et le robot bondit. L’intégration conditionnelle, un if, corrige cela.

Le réglage, dans le bon ordre

1. Anticipation seule. kP = kI = 0. Commandez 0,3 m/s et mesurez ce que vous obtenez. À environ 10 % près signifie que kV est juste. Corrigez-le avant de toucher à quoi que ce soit d’autre.
2. kP jusqu’à ce que cela bourdonne, puis reculez d’un tiers. Vous guettez un sifflement qui apparaît avec le gain et disparaît sans lui.
3. kI jusqu’à effacer l’erreur permanente. Une roue à anticipation correcte en veut très peu. Commencez à kP/10 par seconde et montez doucement.
4. Vérifiez en charge, pas sur cales. Tout le propos est la roue à qui on demande le plus.
5. Testez les quatre ensemble, en roulant une diagonale. C’est la commande avec le plus grand écart de cibles de roues, et c’est là qu’une roue sous-réglée apparaît d’abord.

Le simulateur PID est un chemin plus rapide que le robot pour construire l’intuition des étapes 2 et 3, et le guide de réglage a la procédure générale.

Fréquence d’échantillonnage

La fréquence de boucle doit être bien au-dessus de la chose commandée. La vitesse d’un petit moteur CC à réducteur se stabilise avec une constante de temps d’environ 50–150 ms ; dix échantillons dans cette constante de temps est la règle usuelle, donc 50–200 Hz.

Faites-la tourner sur un horaire fixe, pas « aussi vite que loop() va », parce que dt figure directement dans l’estimée de vitesse et l’intégrateur :

const unsigned long PERIOD_US = 10000;   // 100 Hz
static unsigned long next = 0;

void loop() {
  unsigned long now = micros();
  if ((long)(now - next) < 0) return;    // signed compare survives rollover
  next += PERIOD_US;
  // ... read encoders, run four loops, write four PWMs
}

La comparaison signée est le même piège de bouclage de millis() dans sa forme micros(), où il mord après 71 minutes plutôt que 49 jours.

Ce que cela apporte, mesuré

Même robot, même commande, même sol :

Cette dernière ligne est la sous-estimée. Une boucle fermée sait quand elle a échoué : une roue dont la sortie est coincée à 255 et toujours loin de la cible rapporte quelque chose d’utile, et vous pouvez le mettre sur une ligne de télémétrie, refuser la commande, ou ralentir tout le robot pour garder les rapports honnêtes.

Dépannage

Suite

Les roues délivrent désormais les vitesses demandées. Pointez le tout où le pilote veut avec la commande orientée terrain, et donnez-lui des cibles lisses au lieu d’échelons avec des profils de mouvement. Puis regardez ce qu’une boucle de vitesse peut et ne peut vous sauver dans le simulateur de traction mecanum — une vitesse de roue parfaitement tenue ne signifie toujours rien si le sol lâche.