Commande PID en cascade pour robot équilibriste

Introduction

Vous disposez d’un angle d’inclinaison fiable. Il faut maintenant garder le robot debout dessus — puis, à terme, le faire rester sur place plutôt que dériver dans la pièce tout en restant techniquement vertical.

Ce sont deux tâches différentes, et l’erreur que presque tous commettent d’abord est de tenter les deux avec une seule boucle. Ce tutoriel les construit en deux boucles imbriquées, comme sont commandés les vrais robots équilibristes.

Pourquoi une seule boucle ne peut pas marcher

Ce que vous voulez, c’est un robot qui reste où vous l’avez posé. Ce que vous pouvez commander, c’est le couple moteur. Entre les deux se trouve un fait gênant : la seule façon de changer la position du robot est de changer d’abord son inclinaison.

Tentez de commander directement la position — pilotez les moteurs proportionnellement à l’erreur de position — et les roues fusent sous le centre de masse. Le corps tourne en sens inverse et le robot tombe instantanément, après s’être déplacé du mauvais côté en chutant. Ce n’est pas un problème de réglage ; le signe de la réponse initiale est vraiment inversé, une propriété dite à phase non minimale.

La sortie est de commander les deux grandeurs à des vitesses différentes, en cascade.

La structure

   position error → [ OUTER PID ] → tilt target
                                        ↓
   tilt error     → [ INNER PID ] → motor command → robot
                                        ↑
                                   measured tilt

La boucle interne n’a qu’un travail : tenir l’angle d’inclinaison commandé. Elle tourne vite — 200 Hz ou plus — car elle lutte contre la gravité, et la gravité n’attend pas.

La boucle externe fait l’autre travail : amener le robot à une position. Elle ne peut pas toucher les moteurs, elle demande donc à la boucle interne une petite inclinaison. En arrière de là où elle veut être, elle demande une inclinaison avant ; la boucle interne pilote les roues pour maintenir cette inclinaison ; maintenir une inclinaison avant signifie accélérer continuellement vers l’avant. Elle tourne environ dix fois plus lentement que la boucle interne.

Cette séparation des vitesses est toute la conception. Si la boucle externe est proche de la vitesse de l’interne, elles se battent — l’externe change la consigne plus vite que l’interne ne peut la suivre, et le robot s’oscille jusqu’au sol.

La boucle interne

float angleError = angle - tiltTarget;      // measured MINUS target
angleIntegral   = constrain(angleIntegral + angleError * dt, -3, 3);

float command = Kp * angleError
              + Ki * angleIntegral
              + Kd * gyroRate;

command = constrain(command, -255, 255);

Trois détails y comptent plus que les gains.

L’erreur vaut mesuré moins consigne, l’inverse du consigne − mesure des manuels. Pour rattraper un robot qui tombe en avant, on pilote les roues vers l’avant, sous la masse qui chute, comme on déplace la main sous un balai. Une inclinaison positive doit donc produire une commande moteur positive. Inversez ce signe et le contrôleur plaque le robot au sol dès la mise sous tension — une panne distinctive et violente qui est presque toujours ce signe.

Le terme dérivé utilise directement la vitesse du gyroscope, pas une différence d’angles successifs. Le gyroscope mesure nativement une vitesse angulaire. Différencier un signal d’angle bruité amplifie exactement le bruit le moins voulu, et c’est pourquoi beaucoup de premiers essais exigent un gain D suspectement faible pour rester stables.

L’intégrale est bornée. Pendant une chute, les moteurs saturent et l’erreur continue de croître, donc une intégrale non bornée s’accumule en un énorme nombre qui lutte ensuite contre la récupération au retour. Bornez-la, et cessez toute accumulation pendant que la sortie sature.

La boucle externe

if (++slowTick >= 10) {                     // ten times slower
  slowTick = 0;
  float positionError = targetPosition - wheelPosition;
  tiltTarget = constrain(KpPos * positionError - KdPos * wheelVelocity,
                         -0.12, 0.12);      // radians, about ±7°
}

wheelPosition vient de la somme de vos impulsions d’encodeurs. Sans encodeurs, vous pouvez intégrer la commande moteur comme pis-aller, mais la boucle sera nettement moins bonne.

La borne n’est pas optionnelle. Une boucle externe autorisée à demander 30° donne à l’interne une consigne physiquement intenable — le robot s’engage dans une inclinaison dont il ne peut pas se relever et tombe tout en obéissant parfaitement. Quelques degrés suffisent ; une inclinaison de 7° accélère vivement un robot.

Notez que le terme de vitesse est soustrait. Il amortit l’approche pour que le robot ne dépasse pas sa cible et doive revenir.

Réglage, dans l’ordre qui marche

Réglez d’abord la boucle interne seule. Mettez KpPos et KdPos à zéro pour rendre l’externe inerte et maintenir tiltTarget à zéro. Le robot tiendra en équilibre sur place et dérivera lentement — c’est attendu et normal pour l’instant.

1. Ki et Kd à zéro. Montez Kp jusqu’à ce que le robot réagisse vivement à une poussée. Trop bas et il s’affaisse au sol sans vraiment lutter ; trop haut et il grésille.
2. Montez Kd jusqu’à ce que l’oscillation s’amortisse. C’est le terme qui transforme un robot qui se fait vibrer jusqu’à tomber en un robot qui se stabilise. Dans le simulateur d’équilibre, vous pouvez mettre D à zéro sur un robot qui tient debout et compter les oscillations avant la chute.
3. Montez Ki en dernier, et doucement. Il retire l’inclinaison lente due au désalignement de montage de l’IMU. Trop fort et le robot développe une lente oscillation croissante quand l’intégrale dépasse puis se dévide.

Ensuite seulement, activez la boucle externe, et partez avec KpPos autour d’un dixième de ce qui semble raisonnable. Son effet est indirect — observez si le robot cesse de dériver, pas une réponse franche.

Modes de panne et leur sens

Coupez les moteurs passé le point de récupération

if (fabs(angle) > FALL_ANGLE) {             // about 35 degrees
  driveMotors(0, 0);
  angleIntegral = 0;
  return;
}

Passé environ 35°, l’accélération horizontale requise pour repasser sous le centre de masse dépasse ce que les roues peuvent fournir avant que le corps touche le sol. Le contrôleur l’ignore et forcera à plein couple jusqu’en bas, ce qui fatigue les réducteurs et rend la panne plus bruyante que nécessaire.

La coupure réinitialise aussi l’intégrale, donc relever le robot redonne une boucle propre plutôt qu’encore chargée d’une correction accumulée pendant la chute.

Où aller ensuite

Les gains interagissent ici plus fortement que dans un PID à boucle unique, car le système est instable — il n’existe pas de « à peu près juste » simplement moins performant. Soit ça tient debout, soit non.

Cela rend un simulateur inhabituellement utile : vous pouvez mettre D à zéro sur un robot qui tient en équilibre, observer exactement la panne, et l’annuler sans rien réparer. Essayez les préréglages du simulateur d’équilibre, puis construisez le vrai dans le projet de robot équilibriste.