Suivi de mur et virages précis pour robot de labyrinthe

Introduction

Un algorithme de résolution de labyrinthe suppose que le robot est dans la case qu’il croit. Le remplissage par diffusion calcule un itinéraire parfait, puis le robot touche un mur au troisième virage et chaque décision suivante porte sur la mauvaise case.

L’algorithme est donc rarement ce qui échoue. Ce qui échoue, c’est le mouvement : rester centré dans un couloir, et tourner à exactement quatre-vingt-dix degrés. Ce tutoriel traite ces deux points, et explique pourquoi l’approche évidente de chacun est la mauvaise.

Pourquoi l’estime seule ne suffira pas

L’approche tentante est de rouler aux seuls comptes d’encodeurs : une case vaut N impulsions, un quart de tour M impulsions, et le labyrinthe est une grille, donc il suffit de compter.

Elle échoue car les erreurs s’accumulent sans jamais s’autocorriger. Les roues patinent à l’accélération. Les deux moteurs ne sont pas identiques. L’empattement mesuré est faux d’un millimètre, donc chaque virage fait 88° au lieu de 90°. Chaque erreur est petite ; aucune ne s’annule. Vingt cases plus loin, le robot fonce en diagonale dans les murs en toute confiance.

Le correctif est d’utiliser le labyrinthe lui-même comme référence. Les murs sont droits, parallèles et à espacement connu — une mesure gratuite, sans dérive, disponible en continu.

Le suivi de mur comme boucle PID

Avec un capteur de distance de chaque côté, se centrer est un problème de commande déjà résolu une fois dans le suiveur de ligne : calculez une erreur, amenez-la à zéro.

// Positive error = too close to the left wall = steer right.
float error = leftDistance - rightDistance;

float correction = Kp * error + Kd * (error - previousError);
previousError = error;

int base = 120;
setMotors(base + correction, base - correction);

La correction s’applique en différentiel — ajoutée à une roue, retranchée à l’autre — pour que la vitesse avant du robot reste à peu près constante pendant que son cap change. C’est la même structure qu’un suiveur de ligne ; seul le terme d’erreur diffère.

Démarrez avec Kd à zéro, montez Kp jusqu’à ce que le robot suive le couloir en oscillant légèrement, puis ajoutez Kd jusqu’à amortir l’oscillation. Ki n’est presque jamais utile ici : un couloir de labyrinthe est court, et l’action intégrale ajoute surtout retard et dépassement à chaque frontière de case.

Le cas où la boucle naïve se trompe

Un labyrinthe a des ouvertures. Dès qu’un mur latéral se termine, ce capteur mesure la distance vers quelque chose de lointain — le mur opposé d’une case ouverte, ou rien du tout — et leftDistance - rightDistance devient une énorme erreur fictive. Le robot se jette dans l’ouverture qu’il devait longer.

Vérifiez la présence du mur avant de croire la différence :

const int WALL_THRESHOLD = 100;   // mm — beyond this there is no wall

bool leftWall  = leftDistance  < WALL_THRESHOLD;
bool rightWall = rightDistance < WALL_THRESHOLD;

float error;
if (leftWall && rightWall) {
  error = leftDistance - rightDistance;        // centre between both
} else if (leftWall) {
  error = 2 * (leftDistance - IDEAL_SIDE);     // hold one wall at a set distance
} else if (rightWall) {
  error = 2 * (IDEAL_SIDE - rightDistance);
} else {
  error = 0;                                    // open cell — hold heading instead
}

Les cas à un mur doublent le gain car un seul mur donne moitié moins d’information : avec deux murs, dériver de 5 mm à gauche change la différence de 10 mm ; avec un seul, de 5 mm.

Sans mur, retomber sur error = 0 roule droit pour les moteurs — ce qui n’est pas droit en réalité. C’est là que le gyroscope prend le relais.

Tourner à exactement 90 degrés

Compter les impulsions des encodeurs dans un virage est la première tentative naturelle et ce n’est pas assez précis. Un pivot sur place patine plus qu’en ligne droite, et une erreur de 2° par virage fait 20° après dix virages — un robot face à la diagonale d’un couloir.

Intégrez plutôt le gyroscope. La vitesse d’axe Z du MPU6050 mesure directement la vitesse de rotation du robot, sans se soucier du patinage des roues :

float turnDegrees(float target) {
  float heading = 0;
  unsigned long last = micros();

  while (fabs(heading) < fabs(target)) {
    unsigned long now = micros();
    float dt = (now - last) / 1000000.0;
    last = now;

    heading += (readGyroZ() - gyroBias) * dt;   // bias subtraction is essential

    int speed = (target > 0) ? TURN_SPEED : -TURN_SPEED;
    setMotors(speed, -speed);                   // pivot in place
  }
  setMotors(0, 0);
  return heading;                               // report the real angle achieved
}

Trois points font ou défont cela :

Soustrayez le biais du gyroscope. Un gyroscope non étalonné lit un ou deux degrés par seconde à l’arrêt, et vous l’intégrez. Mesurez la moyenne à l’arrêt, exactement comme dans le tutoriel d’inclinaison, et soustrayez-la à chaque lecture.

Arrêtez tôt et laissez rouler. Le robot a une inertie en rotation, donc couper les moteurs à exactement 90° dépasse. Arrêtez vers 85° et laissez finir en roue libre — ou mieux, réduisez la vitesse de virage quand l’angle restant diminue.

Recalez-vous contre le mur avant. Quand un virage place un mur devant le robot, ce mur est une référence parfaite à 90°. Avancez lentement vers lui et laissez la boucle de suivi redresser le robot contre lui, ce qui annule gratuitement l’erreur de cap accumulée. Les concepteurs de micromouse le font à chaque occasion, car cela transforme un robot qui dérive en robot qui s’autocorrige.

Savoir quand une case est passée

Le comptage de cases relie le mouvement à l’algorithme de labyrinthe. La distance des encodeurs y mène approximativement ; les murs l’indiquent exactement.

Le déclencheur le plus fiable est le front descendant d’un mur latéral. Quand le robot franchit une frontière de case, un capteur latéral qui lisait un mur lit soudain loin — cette transition arrive à un point connu de la case, et c’est un repère physique plutôt qu’une estimation accumulée.

Combinez les deux : utilisez les encodeurs pour prédire la frontière, puis laissez le bord du mur confirmer ou corriger. En cas de désaccord, croyez le mur.

Vérifiez avant de laisser résoudre

Quatre essais, sur le vrai labyrinthe, dans cet ordre :

1. Couloir droit, dix cases. Le robot doit rester centré et arriver encore centré. Osciller signifie Kd trop faible ; dériver d’un côté signifie un décalage de capteur — mesurez les deux capteurs latéraux contre le même mur et corrigez l’écart en logiciel.
2. Couloir à un mur. Retirez un mur. Le robot doit continuer droit plutôt que se jeter dans le vide.
3. Dix virages d’affilée. Tournez de 90° dix fois et vérifiez que le robot finit face à sa direction d’origine. Au-delà de quelques degrés, votre biais de gyroscope ou fin de course demande du travail.
4. Recalage. Roulez vers un mur, laissez-le se recaler, et confirmez que l’erreur de cap a disparu après.

Seulement quand ces quatre passent, il vaut la peine d’exécuter un algorithme de labyrinthe par-dessus — un planificateur nourri de mauvaises positions produit des non-sens assurés. Observez la couche de planification seule dans le simulateur de résolution de labyrinthe, puis assemblez les deux dans le projet micromouse.