Simulateur de trajectoire : poursuite pure, Stanley

Commandes

Controller bascule entre poursuite pure et Stanley. Lookahead est le seul bouton de la poursuite pure et est désactivé pour Stanley, qui n’en utilise pas — c’est en soi le propos.

Speed et la visée interagissent, ce qui est la chose à explorer : réglez 1 m/s et trouvez la meilleure visée, puis réglez 3 m/s et retrouvez-la.

Position from échange l’odométrie parfaite contre un point à 1 Hz avec un bruit métrique, maintenu entre les mises à jour. L’erreur est toujours notée face à la vérité, donc la pénalité est réelle plutôt que masquée.

Tirez n’importe quel point de passage, ou appuyez sur 1–8 pour en sélectionner un et le déplacer avec les flèches du clavier. Tout l’essai se recalcule pendant que vous tirez.

Théorie

Les deux contrôleurs répondent à la même question — étant donné où je suis et où est la trajectoire, quel angle de direction ? — et y répondent tout à fait différemment.

La poursuite pure est géométrique. Trouvez le point sur la trajectoire à une distance de visée L devant, et roulez l’arc de cercle qui y passe :

κ = 2y / L_d²          δ = atan(2 b sin α / L_d)

avec y l’écart latéral du point but dans le repère du robot et b l’empattement. Un paramètre, et c’est une distance plutôt qu’un gain — donc il se transfère entre robots.

Stanley est de la commande. Référencez l’essieu avant et ajoutez deux termes :

δ = θ_e + atan(k · e / v)

erreur de cap plus un arc-tangente de l’écart transversal. Diviser par la vitesse permet à un gain de fonctionner sur une plage de vitesses. k est un vrai gain de commande et ne se transfère pas.

Le labo ajoute ce que les démonstrations de manuels omettent : 0,4 s de retard total, comme une réponse de direction du premier ordre de 0,25 s plus 0,15 s de retard de détection et de calcul. Sans lui, la poursuite pure s’améliore de façon monotone quand la visée rétrécit et toute la question du réglage disparaît.

Algorithme

À chaque pas, pour les deux contrôleurs :

1. Projetez sur la trajectoire, en cherchant dans une fenêtre autour de l’abscisse curviligne dernière atteinte plutôt que sur toute la trajectoire. Une recherche globale apparie le segment le plus proche, donc un parcours qui rebrousse chemin téléporte la progression du robot vers un virage passé il y a une minute.
2. Résolvez une égalité à un sommet vers le segment sortant. Après un virage, les deux segments adjacents se projettent au même point à la même distance ; garder l’entrant confie au contrôleur une erreur de cap et un écart latéral tous deux proches de zéro pendant qu’il quitte la trajectoire tout droit, silencieusement.
3. Calculez l’angle de direction selon la propre règle du contrôleur, et saturez-le à la limite mécanique.
4. Appliquez le retard, puis intégrez le modèle bicyclette.
5. Notez face à la vérité, jamais face à l’estimation.

L’étape 2 n’est pas une optimisation. C’était la différence entre Stanley qui fonctionne et Stanley qui quitte le parcours avec assurance exactement à 78 % à chaque essai.

Code source

La poursuite pure, complète, sous la forme qu’elle prend sur un vrai robot :

float lastS = 0;   // arc length last reached — state, and it only moves forward

float pursue(float x, float y, float heading, float speed) {
  // The lookahead is scheduled on speed, with a hard floor: at a standstill a
  // proportional schedule asks for zero, which is a division by zero in disguise.
  float lookahead = constrain(0.7f * speed + 0.4f, 0.4f, 3.0f);

  float s = projectOntoPath(x, y, lastS);      // windowed around lastS
  lastS = max(lastS, s);
  Point goal = pointAtArcLength(lastS + lookahead);

  float dx = goal.x - x, dy = goal.y - y;
  float lateral = -sinf(heading) * dx + cosf(heading) * dy;
  float ld = max(0.3f, hypotf(dx, dy));        // clamp, or curvature blows up

  float curvature = 2.0f * lateral / (ld * ld);
  return curvature;                            // omega = v * curvature
}

Les trois lignes qui comptent sont le plancher du programme, la monotonicité de lastS, et la saturation de ld. Tout le reste est la formule.

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

Le labo donne au robot une carte parfaite de la trajectoire, un empattement parfaitement connu, aucun patinage des roues, et un retard qui est un modèle propre du premier ordre. Un vrai robot n’a rien de tout cela.

Le patinage des roues fausse l’estimation de position d’une façon qu’aucun contrôleur ne peut détecter. Le jeu du réducteur ajoute une course morte à chaque inversion de direction, qui se comporte comme un retard supplémentaire et pousse la visée optimale vers le haut. Et une propulsion différentielle n’a aucun angle de direction — elle a une vitesse de rotation commandée, donc ω = v κ remplace l’atan et l’empattement ne compte que pour l’étroitesse de ses virages.

Ce qui survit à tout cela, c’est l’allure : un optimum intérieur, qui se déplace vers la droite avec la vitesse, et une erreur à cet optimum qui croît avec la vitesse quoi qu’il arrive.

Schéma de câblage

Le contrôleur est la partie bon marché. Ce dont il a besoin du reste du robot est la partie chère.

Position. Codeurs en quadrature sur les deux roues et odométrie à propulsion différentielle qui les intègre, à la fréquence de boucle. En intérieur, cela suffit pour quelques dizaines de mètres.

Cap. Un gyroscope pour le court terme, un magnétomètre pour la référence absolue, ou les deux. L’erreur de cap est le terme que les deux contrôleurs pondèrent le plus, et c’est celui que l’odométrie perd en premier.

Une correction absolue, en extérieur. Un module GPS fusionné dans l’estimation plutôt que suivi directement — la différence est de 0,09 m d’erreur RMS contre 1,25.

Pour la théorie complète, voir poursuite pure et écart transversal ; pour construire, le rover GPS à points de passage démarre en intérieur sur codeurs et ne sort que lorsque le suiveur fonctionne.