Simulateur de robot auto-équilibré : pendule inversé PID

Commandes

Start, Pause et Reset pilotent la boucle. Shove applique une impulsion comme le ferait un doigt, et Loop on/off désactive les deux contrôleurs pour que vous puissiez voir le système nu tomber.

La commande la plus directe est le robot lui-même : tirez le corps jusqu’à n’importe quel angle et lâchez-le. Quelle que soit la pose où vous le laissez, le contrôleur prend le relais — il n’y a pas d’étape de réarmement, et les curseurs de gains s’appliquent immédiatement à la simulation en cours. Mettez le gain dérivé à zéro pendant qu’il s’équilibre et il commence à osciller dans la même seconde.

Le menu Tuning charge quatre préréglages : un qui fonctionne, et trois qui échouent des trois façons dont les vrais robots d’équilibre échouent.

Placez le focus sur le canevas et utilisez Espace, S, L, R et F pour les mêmes commandes au clavier.

Théorie

Tenir un robot debout sur deux roues signifie maintenir un pendule inversé à la verticale. Le centre de masse se trouve au-dessus de l’axe, donc la verticale est un équilibre au même sens qu’un crayon en équilibre sur sa pointe : mathématiquement réel, physiquement sans espoir sans correction permanente. Inclinez-le d’un angle quelconque et la gravité produit un couple qui l’incline davantage. Plus il s’écarte, plus il s’écarte vite.

Le robot n’a qu’un seul moyen de combattre ce couple — il peut déplacer l’axe. Accélérer les roues vers l’avant repousse la base sous le centre de masse, ce qui ramène le corps vers la verticale. C’est pourquoi la correction semble inversée au premier abord : pour cesser de tomber vers l’avant, il faut avancer, comme lorsque vous déplacez votre main sous un balai en équilibre plutôt qu’à l’opposé.

Cela impose au robot une limite stricte. Au-delà d’environ 35°, l’accélération horizontale nécessaire dépasse ce que les roues peuvent fournir avant que le corps ne touche le sol, et aucun gain n’y change rien. Les lignes en pointillés sur le tracé marquent cette frontière.

Algorithme

Le contrôleur est en cascade — deux boucles PID, l’une alimentant l’autre.

La boucle interne fonctionne vite et maintient un angle d’inclinaison. Elle compare l’angle mesuré à une cible et commande le couple des moteurs :

error   = measured_angle − target_angle
motor   = Kp·error + Ki·∫error + Kd·d(measured)/dt

Notez que l’erreur est la mesure moins la cible, et non la classique cible moins mesure. Rattraper une chute vers l’avant signifie piloter les roues vers l’avant, donc une inclinaison positive doit produire une commande moteur positive. Ce signe est la raison la plus fréquente pour laquelle un premier robot d’équilibre se plaque au sol dès sa mise sous tension.

La boucle externe fonctionne plus lentement et maintient une position. Elle ne peut pas commander les moteurs — ils sont déjà pris — elle commande donc une cible d’inclinaison pour la boucle interne :

target_angle = Kp_pos·(desired_position − position) − Kd_pos·velocity

En arrière de l’endroit où il veut être, elle demande une légère inclinaison vers l’avant ; la boucle interne pilote alors les roues pour tenir cette inclinaison, et le robot avance. L’inclinaison est volontairement limitée à quelques degrés. Une boucle externe autorisée à demander un grand angle confierait à la boucle interne une cible intenable, et les deux boucles perdraient ensemble.

Le terme intégral compte plus ici que dans la plupart des boucles, car c’est lui qui absorbe l’écart entre l’endroit où la centrale inertielle croit que se trouve la verticale et l’endroit où elle se trouve réellement. C’est aussi pourquoi l’intégrale doit cesser d’accumuler pendant que les moteurs sont saturés — sinon elle s’emballe pendant une chute qu’elle n’a pas la puissance d’empêcher, puis s’oppose à la récupération.

Code source

// Inner loop: tilt → motors. Runs every 5 ms off a timer, not in loop().
float angleError = angle - tiltTarget;          // measured minus target
angleIntegral   = constrain(angleIntegral + angleError * dt, -3, 3);
float command   = Kp * angleError
                + Ki * angleIntegral
                + Kd * gyroRate;                // rate straight from the gyro

command = constrain(command, -255, 255);
if (fabs(angle) > FALL_ANGLE) command = 0;      // past saving; cut the motors
driveMotors(command, command);

// Outer loop: position → tilt target. Runs ten times slower, and may only
// ever ask for a small lean.
if (++slowTick >= 10) {
  slowTick = 0;
  float positionError = 0 - wheelPosition;
  tiltTarget = constrain(KpPos * positionError - KdPos * wheelVelocity,
                         -0.12, 0.12);          // radians, about ±7°
}

Deux détails se transposent directement depuis ce simulateur. Prenez le terme dérivé de la vitesse du gyroscope plutôt qu’en différenciant l’angle : le gyroscope mesure cette vitesse directement et sans le bruit qu’amplifie la différentiation. Et coupez les moteurs au-delà de l’angle de récupération plutôt que de laisser le contrôleur forcer — cela épargne les réducteurs et rend l’échec net au lieu d’être violent.

Matériel

Le capteur est un MPU6050, qui contient à la fois un accéléromètre et un gyroscope, car aucun des deux n’est utilisable seul. L’accéléromètre sait où se trouve le bas mais est noyé par les propres mouvements du robot ; le gyroscope suit proprement la rotation mais dérive en quelques secondes. Un filtre complémentaire les fusionne en un angle à la fois stable et rapide.

Les moteurs doivent être assez démultipliés pour le couple et assez rapides pour rattraper une chute — des moto-réducteurs N20 autour de 200 tr/min sont un choix courant, pilotés par un TB6612FNG, dont la faible chute de tension compte lorsque la boucle demande de petites corrections des milliers de fois par seconde.

Construisez-le pour de vrai dans le projet de robot auto-équilibré, ou commencez par le tutoriel de lecture d’inclinaison si la centrale inertielle est nouvelle pour vous.

Hypothèses et différences avec le matériel

Le modèle est un pendule inversé 2D avec un seul axe d’inclinaison, des liaisons rigides, aucun patinage des roues et une centrale inertielle qui rend compte de l’angle et de la vitesse vrais avec seulement la dérive et le bruit que vous réglez. Un vrai robot d’équilibre subit un couple moteur qui varie avec la tension de la batterie, la quantification des codeurs, l’écart de montage de la centrale et la dérive en température, et il peut aussi tomber sur le côté — un degré de liberté que ce labo supprime entièrement. La structure PID en cascade et la dérivée issue du gyroscope présentées ici se transfèrent directement, mais l’écart de montage, le réglage du filtre et le seuil d’angle de chute doivent être retrouvés sur le châssis physique.

Schéma de câblage

Deux règles de câblage décident si ce robot fonctionne.

La batterie des moteurs atteint la broche VM du pont et rien d’autre. Le courant des moteurs ne doit jamais traverser l’Arduino, et toutes les masses — carte, pont, centrale inertielle, moins de la batterie — doivent être reliées entre elles, sinon les signaux de commande n’ont aucune référence commune et la panne est silencieuse plutôt qu’évidente.

Le codeur est alimenté depuis la logique, pas depuis le rail des moteurs. Alimenter un codeur par l’alimentation des moteurs le détruit ou remplit les comptages de bruit de commutation. Placez aussi la voie A du codeur sur une broche compatible interruption ; l’interroger depuis loop() fait perdre des impulsions, et des impulsions perdues signifient que la boucle externe perd lentement la trace de l’endroit où se trouve le robot.

Montez la centrale inertielle près de l’axe. La monter en hauteur multiplie l’accélération linéaire qu’elle voit à chaque correction, ce qui complique le problème de fusion sans aucun bénéfice.