Robot auto-équilibré sur ESP32 : trois boucles de régulation

Ce robot vient de sauter d’un pèse-personne, et il est resté debout. Cinq jours plus tôt, il n’arrivait même pas à rester sur place. Entre-temps, rien n’a changé dans son matériel : le même châssis, les mêmes moteurs, le même capteur. Seul le programme a changé. Une boucle de régulation est devenue deux, puis trois, et chaque nouvelle boucle a corrigé ce que la précédente ne savait pas faire.

C’est un robot auto-équilibré à deux roues : un châssis à trois étages imprimé en 3D, posé sur deux motoréducteurs à courant continu, une carte ESP32 et un capteur de mouvement. Nous l’avons construit pour comparer, sur le même matériel, différentes façons de maintenir un robot debout, en commençant par la méthode classique, le PID. Cet article suit les dix jours qui séparent le premier test du saut, dans l’ordre où ils se sont déroulés : ce qu’il y a à l’intérieur, comment le robot sait où est le haut, les trois boucles une par une, et ce que nous a appris son pilotage par Wi-Fi. Les chiffres viennent des journaux du robot lui-même.

Le robot

Le châssis se compose de trois étages imprimés, empilés les uns sur les autres, chacun étant une pièce distincte. Nous l’avons dessiné dans Fusion 360 en y intégrant les composants achetés, si bien que les moteurs et les drivers avaient leur place avant que la moindre pièce soit imprimée.

De bas en haut : les moteurs sont suspendus sous l’étage inférieur, sur les équerres fournies dans leur kit. Le premier étage porte les deux drivers de moteur, dissipateurs vers le bas. Le deuxième étage est le cerveau : la carte ESP32 avec son petit écran sur une plaque à trous, le capteur de mouvement et un buzzer. L’étage du haut porte une batterie à trois cellules avec sa carte de protection, un interrupteur et un module abaisseur 5 V. Le corps mesure 164 × 128 × 60 mm, 235 mm de large avec les roues, et le robot complet pèse 1025 g.

Deux particularités de la transmission ont déterminé tout ce qui a suivi. La première est le retard. Le capteur fournit un nouvel échantillon toutes les 2 ms, et la commande qui en découle atteint les drivers en moins d’une milliseconde, mais une roue a besoin d’environ un dixième de seconde pour répondre à un changement de commande. La seconde est le réducteur : il a du jeu, si bien qu’après chaque inversion de sens, le moteur tourne un instant avant que la roue ne suive.

Et les deux moteurs ne sont pas jumeaux. Sur des images filmées au ras du sol, une seule et même commande PWM a fait bouger la roue la plus éloignée de 18 mm et la plus proche de 8 mm. La commande était identique ; pourquoi la réponse ne l’est pas, nous ne l’avons pas établi. Livrée à elle-même, cette différence fait lentement tourner le robot sur place pendant qu’il garde l’équilibre, et une petite correction de cap, décrite dans la partie sur le pilotage, s’en charge.

Où est le haut

Tout ce que fait un robot auto-équilibré dépend d’un seul nombre : son inclinaison. Le MPU-6500 offre deux façons de l’obtenir, et aucune ne fonctionne seule.

Le gyroscope mesure la vitesse à laquelle le corps tourne. Additionnez ses mesures toutes les 2 ms et vous obtenez un angle lisse et rapide, mais qui dérive : sans correction du décalage propre du gyroscope, nous avons mesuré environ 15° par minute. L’accéléromètre sait où est la gravité, si bien que son angle ne peut pas dériver, mais il ressent aussi chaque secousse. Moteurs en marche, son angle sautait en tous sens sur une plage de 18,7°.

Un filtre complémentaire prend la bonne moitié de chacun. À chaque pas, le gyroscope fait avancer l’angle, puis l’angle est tiré vers la mesure de l’accéléromètre d’une infime part de l’écart, un millième. Autrement dit : 99,9 % de gyroscope, 0,1 % d’accéléromètre, ce qui correspond à une constante de temps de deux secondes. Sur le banc, l’angle filtré n’a pas varié de plus de six centièmes de degré.

Dans le firmware, cela tient en quelques lignes. La carte du capteur est montée sur la tranche, avec un roulis de 1,4°, si bien qu’avant toute chose le code sépare la vitesse de tangage, dont le filtre a besoin, de la vitesse de lacet, dont le code des virages se sert plus loin :

// Leaning forward, the forward axis dips below the horizontal:
// its reading is -sin(theta).
att.thetaAcc = atan2f(-(float)s.accel[1], (float)s.accel[0])
               * DEG_PER_RAD;

// The board is rolled by 1.4 deg: take pitch and yaw rates apart.
const float gx = s.gyro[0] / GYRO_LSB_PER_DPS - att.gyroBiasDps[0];
const float gz = s.gyro[2] / GYRO_LSB_PER_DPS - att.gyroBiasDps[2];
att.omegaPitch = gz * rollCos - gx * rollSin;
att.omegaYaw = gx * rollCos + gz * rollSin;

// Complementary filter: the gyroscope carries the angle from
// sample to sample, the accelerometer pulls it back to gravity.
// FILTER_K = dt / (tau + dt) = 0.002 / 2.002, about 0.1 %.
att.theta = wrapDeg(att.theta + att.omegaPitch * IMU_DT);
att.theta = wrapDeg(att.theta
                    + FILTER_K * wrapDeg(att.thetaAcc - att.theta));

Le pas de calcul suit l’horloge propre du capteur. Un timer matériel réveille la tâche de commande juste avant chaque échantillon attendu, la tâche le lit et exécute tout le régulateur, tandis que l’écran et le journal d’essai attendent leur tour avec une priorité plus basse. Le Wi-Fi tourne sur l’autre cœur. Cette organisation vient de notre précédent balancier à hélice, construit sur la même carte et le même capteur.

Première boucle : l’angle

La première expérience utilise une seule boucle, qui ne surveille que l’angle. C’est un régulateur PID qui tourne 500 fois par seconde, une fois par échantillon du capteur, et sa sortie est la puissance des deux moteurs, en pourcentage :

• P, le terme proportionnel : kp = 30, donc un degré d’erreur demande 30 % de la pleine puissance.
• I, le terme intégral : ki = 30 cumule l’erreur dans le temps et continue de pousser tant qu’elle dure.
• D, le terme dérivé : kd = 0,3 calme l’oscillation. Il lit la vitesse de rotation directement sur le gyroscope au lieu de dériver un angle bruité.

La consigne n’est pas exactement zéro. Le centre de masse ne se trouve pas parfaitement au-dessus de l’axe des roues, si bien que le robot s’équilibre à θ_ref = 0,3°.

float pidStep(float theta, float omegaPitch, float thetaRef,
              const Params &p, float dt, float *raw) {
    const float error = theta - thetaRef;
    const float summed = integral + p.ki * error * dt;
    integral = constrain(summed, -p.uMax, p.uMax);
    const float u = p.kp * error + integral + p.kd * omegaPitch;
    *raw = u;
    return constrain(u, -p.uMax, p.uMax);
}

Le premier essai au sol a échoué d’une façon que le banc n’aurait pas pu révéler. En l’air, toutes les vérifications avaient réussi : les roues tournaient, les encodeurs comptaient, le capteur lisait l’inclinaison. Au sol, les roues sont parties à l’opposé de l’inclinaison au lieu de passer dessous, et le robot est tombé. L’angle était juste ; ce qui était inversé, c’était le sens « avant » des moteurs et des encodeurs. Une fois les deux inversés dans la configuration, le robot a tenu debout, et avec ces gains, il est ensuite resté en équilibre plus de deux minutes, avec un écart type de 0,2° sur l’angle.

C’est là qu’une seule boucle atteint sa limite. Pendant cette poussée, le corps ne s’est jamais incliné de plus d’un demi-degré, et le robot a roulé sur 27 cm puis est resté là. Pour une boucle d’angle, c’est une réussite, puisque l’angle est bon. Elle ne sait pas distinguer l’immobilité d’un roulement à vitesse constante : sur notre sol, maintenir une vitesse v demande environ 4,5 % de puissance plus 78 % par mètre par seconde, et l’intégrale garde simplement la puissance, quelle qu’elle soit, que la poussée lui a laissée. Pour elle, n’importe quelle vitesse ressemble à l’équilibre.

Deuxième boucle : la vitesse

La deuxième boucle surveille les roues. Elle tourne 100 fois par seconde, lit les deux encodeurs et compare la vitesse du robot à celle qu’il devrait avoir, zéro pour l’instant. Elle ne touche pas du tout aux moteurs : sa sortie est l’angle que la première boucle doit tenir. Le robot roule vers l’avant ? Qu’il se penche un peu en arrière, et la boucle d’angle, en poursuivant le nouvel angle, le freine. C’est un régulateur PI, dont l’intégrale est bornée à la même plage de quelques degrés que la sortie :

float pidSpeed(float vRef, float v, const Params &p, float dt,
               bool hold) {
    const float error = vRef - v;
    const float summed = hold ? speedIntegral
                              : speedIntegral + p.kvi * error * dt;
    speedIntegral = constrain(summed, -p.thetaMax, p.thetaMax);
    return constrain(p.kv * error + speedIntegral,
                     -p.thetaMax, p.thetaMax);
}

La première tentative tremblait cinq fois par seconde. Son gain venait d’un modèle qui supposait une boucle d’angle trois fois plus faible que la vraie : kv = 10 avait été calculé pour kp = 10, alors que le robot fonctionne avec kp = 30. Le modèle ignorait aussi le dixième de seconde que mettent les roues à répondre. Le remède n’a pas été un gain plus petit, mais une mesure plus calme : la vitesse est désormais moyennée sur 200 ms au lieu de 40 ms, et il se trouve qu’une moyenne sur 200 ms annule complètement une oscillation à 5 Hz.

Puis le robot s’est mis à dériver lentement. Avec une boucle de vitesse purement proportionnelle, kvi = 0, huit essais sur dix se terminaient à plus de 10 cm de leur point de départ. Une petite intégrale, kvi = 1, a réglé le problème : sur les onze essais suivants, aucun n’a dérivé de plus de 2,7 cm.

Et puis il a fait quelque chose que personne ne lui avait demandé : poussé, il est revenu.

L’explication est dans l’intégrale. L’intégrale de la vitesse, c’est simplement la distance parcourue, si bien que pour maintenir à vitesse nulle un robot qu’on a déplacé, l’intégrale de la vitesse finit par tenir une petite inclinaison, environ un degré par mètre d’écart au point de départ. C’est un ressort caché, et faible, qui tire le robot vers son point de départ. Faible, c’est bien le mot : le robot revient dans une bande d’environ 30 cm de large plutôt qu’en un point, et dans cette prise, deux de ses positions de repos étaient à 34 cm l’une de l’autre. Nous supposons qu’à l’intérieur de la bande, l’inclinaison est trop faible pour rattraper le jeu du réducteur, mais c’est une supposition, pas une mesure. La prise ci-dessus a été faite avec kv = 5 ; l’étape s’est conclue avec kv = 7.

Troisième boucle : la position

La troisième boucle retient l’endroit d’où le robot est parti, et nous avons marqué ce point avec un petit bateau imprimé. Vingt fois par seconde, la boucle mesure la distance au point de départ et demande à la boucle de vitesse une vitesse pour y revenir. Son gain est kx = 1,5, si bien qu’à dix centimètres du point de départ, elle demanderait 15 cm/s, mais elle ne demande jamais plus de 10 cm/s. La position fixe la vitesse, la vitesse fixe l’angle, et l’angle fixe les moteurs.

La distance vient des encodeurs, corrigée de l’inclinaison : quand le corps penche, les roues roulent sous lui même si le robot dans son ensemble reste sur place. Voici la cascade telle qu’elle tourne dans le pas à 500 Hz, un peu raccourcie : la branche qui permet au PC de piloter le robot a été retirée.

// Every step runs at 500 Hz: position every 25th, speed every 5th.
if (positionPhase == 0) {
    vRefNow = pidPosition(xHold + params.xRef, xNow, params);
}
positionPhase = (positionPhase + 1) % POSITION_EVERY;

if (speedPhase == 0) {
    // Hold the speed integral while a hand carries the robot away.
    const bool limited = fabsf(vRefNow) >= params.vMax;
    const bool carried = (vRefNow > 0) ? (vNow < -AWAY_M_S)
                                       : (vNow > AWAY_M_S);
    thetaOffset = pidSpeed(vRefNow, vNow, params,
                           SPEED_EVERY * IMU_DT, limited && carried);
}
speedPhase = (speedPhase + 1) % SPEED_EVERY;

thetaRefNow = params.thetaRef + thetaOffset;
percent = pidStep(a.theta, a.omegaPitch, thetaRefNow, params,
                  IMU_DT, &uCtl);

La ligne « carried » (« emporté ») cache une histoire. Dans la première version, après une poussée, le robot revenait à une vitesse de 20 à 46 cm/s et dépassait sa place de 10 à 24 cm. L’intégrale de la vitesse s’était chargée tant que la main poussait, et elle se souvenait de la main. Figer l’intégrale chaque fois que la boucle de position demandait la pleine vitesse a réduit le dépassement à environ 5 cm, mais les grandes poussées se terminaient alors de 12 à 27 cm avant le point de départ. La règle retenue ne la fige que lorsque la boucle demande la pleine vitesse de retour et que le robot s’éloigne encore de son point de départ à plus de 1 cm/s, autrement dit pendant qu’une main l’emporte. Après cela, des poussées de 68 et 80 cm se sont soldées par un retour à moins de 3 cm, sans dépassement.

Quant à kx, nous l’avons choisi en déplaçant le point cible par Wi-Fi et en regardant le robot le suivre, 36 déplacements pour chaque réglage :

Il ne revient pas exactement au petit bateau. Il s’arrête deux ou trois centimètres en deçà ou au-delà, retenu par le même frottement qui laissait le robot à deux boucles s’immobiliser n’importe où dans sa bande. Après une poussée de 50 cm dans l’autre sens, le retour a pris 8,6 secondes. Mettez les trois étapes côte à côte, et le progrès saute aux yeux :

Le piloter par Wi-Fi

Dès que le robot sait tenir un point, le piloter revient à déplacer ce point. L’ESP32 se connecte au réseau Wi-Fi de la maison, et une petite passerelle Python sur le PC sert une page web avec des flèches : clavier, manette ou manette à l’écran. La passerelle transmet au robot la commande la plus récente en UDP vingt fois par seconde, et le robot répond aussitôt à chaque commande avec sa télémétrie : angle, vitesse, position, cap, puissance, tension de la batterie et force du signal. Tant qu’une touche est maintenue, une vitesse de déplacement lissée par une rampe prend la place de la sortie de la boucle de position ; au relâchement, la boucle de position tient l’endroit où le robot s’est arrêté.

Pour tourner, il a fallu une boucle de plus, hors de la cascade. Une boucle de cap compare l’écart entre les encodeurs des deux roues à une consigne qui avance à la vitesse de rotation demandée, puis ajoute sa correction à une roue et la retranche de l’autre. Cette même boucle empêche le robot de tourner tout seul à cause de ses moteurs dissemblables. La règle, c’est l’équilibre d’abord : le virage ne reçoit que la puissance que la boucle d’angle laisse disponible.

// Balance first: the turn gets what is left of the limit, so the
// mean of the two wheels stays the angle loop's command.
static void driveApart(float percent, float turn, float limit) {
    drivePercent = percent;
    const float room = max(limit - fabsf(percent), 0.0f);
    uTurnNow = constrain(turn, -room, room);
    motorDrive(Wheel::Left, percent - uTurnNow);
    motorDrive(Wheel::Right, percent + uTurnNow);
}

Les limites de fonctionnement sont de 0,35 m/s et de 75° par seconde. Le dernier essai du 8 novembre a duré 150 secondes : 28 mètres de trajet avec des pointes jusqu’à 0,57 m/s, deux sauts depuis le pèse-personne et aucune chute. Au premier atterrissage, le corps a basculé à +7° pendant un instant, et le robot est resté debout.

Tous les sauts ne se sont pas aussi bien passés. Plus tôt dans la journée, un atterrissage depuis un petit tremplin a fait basculer le robot de 9° vers l’avant. Pour se rattraper, il est parti à environ un mètre par seconde et a foncé droit dans le lit. Pour que le robot se penche en arrière et freine, ses roues doivent d’abord passer devant le corps, et la commande de puissance était déjà à sa limite de 85 % : à cette vitesse, il ne restait plus rien pour les y amener. La limite a depuis été relevée à 100 %, ce que nous n’avons pas encore essayé au sol.

Ce qui l’empêche de s’abîmer

• Il ne démarre que sur demande. Un appui sur un bouton l’arme avec trois bips, et il s’enclenche une fois que son inclinaison est restée à moins de 5° de la verticale pendant 0,2 s.
• Il abandonne à 25°. Au-delà de cette inclinaison, il ne peut plus se rattraper, alors les moteurs se coupent au lieu de faire patiner les roues sur le sol.
• Son propre chien de garde. Un timer matériel séparé coupe la ligne d’activation (enable) des drivers si aucun échantillon du capteur n’est arrivé depuis 10 ms. Un bus I²C bloqué peut immobiliser la tâche de commande pendant environ une seconde, et un robot qui roule à l’aveugle pendant une seconde, c’est un robot dans le mur.
• Surveillance de la batterie et du capteur. Il s’arrête quand la batterie est faible et en cas de défaut du capteur. Une liaison Wi-Fi perdue n’arrête que le pilotage : le robot s’immobilise et continue de garder l’équilibre.

Toute la construction en vidéo

La vidéo raconte la même histoire en huit minutes et demie, avec les journaux tracés par-dessus les images. Le lecteur ne se charge depuis YouTube que lorsque vous appuyez dessus.

Lire la vidéo : J’ai construit un robot auto-équilibré : trois boucles de régulation 8:40

J’ai construit un robot auto-équilibré : trois boucles de régulation

• Regarder sur YouTube

La vidéo est chargée depuis YouTube lorsque vous lancez la lecture. Politique de confidentialité

La suite

Dix jours, trois boucles, un robot qui tient debout. La cascade PID fonctionne, mais c’est un empilement de pièces réglées à la main, et chaque pièce a été ajoutée pour corriger ce que la précédente ne savait pas faire.

Ensuite, nous voulons remplacer les trois boucles par un seul régulateur LQR, qui calcule ses gains à partir d’un modèle du robot, au lieu qu’une personne les règle un par un. Après cela, nous voulons essayer un petit réseau de neurones qui tourne sur l’ESP32 lui-même. Le robot enregistre déjà ses essais, en partie pour qu’un tel réseau ait de quoi apprendre. Ni l’un ni l’autre n’existe encore. Quand ils existeront, ils subiront les mêmes poussées que le PID ici, et nous publierons les chiffres.