Chapitre 4 : consigne

Le calcul des erreurs avant d'asservir

Dans les chapitres précédents, nous avons écrit l’algorithme pour le calcul d’odométrie et nous l’avons calibré.
Notre robot connait donc maintenant sa position XY et son angle Téta à tout instant.

Dans notre système à asservir, les précédents calculs d’odométrie correspondent à la mesure :

Cette mesure donne le point X0, Y0, T0.

La consigne, c’est la destination du robot (demandé par le programme de stratégie) >> On souhaite aller au point X2, Y2, T2

L’erreur du système est donc très simple à calculer :

  • Xerr = X2-X0
  • Yerr = Y2-Y0
  • Terr = T2-T0

On va commencer par mettre de côté l’angle final Terr, il se fera « tout seul » une fois la position XY obtenue. On se penche alors sur le calcul de la consigne en position :

Comme le robot fonctionne en polaire, on ne va pas travailler avec l’erreur cartésienne, mais avec une erreur polaire :

  • Dist: c’est la distance entre la position du robot et son objectif. Un calcul du niveau collège (comme disait mon prof de maths), grâce à Pythagore : Dist = RACINE(Xerr²+Yerr²)
  • Cap: comme son nom l’indique, c’est l’orientation que doit prendre le robot pour atteindre son point. Toujours un peu de trigonométrie  : Cap =ArcTan(Yerr/Xerr) – T0

Il faut être très vigilent avec les angles. Le calcul du cap peut amener l’ArcTan à jongler autour de PI et –PI… et donc (avec la soustraction de T0) calculer un Cap qui fait plus d’un tour. Il est extrêmement important de borner la consigne en angle. Je conseille donc de ramener immédiatement le calcul du Cap entre PI et –PI avec une fonction récursive (un simple -2PI si le Cap est supérieur à PI ou +2PI si le Cap est inférieur à –PI).

Avec tout ça, on peut déjà commencer à écrire un morceau d’algorithme pour calculer la consigne pour le robot (l’erreur du système à asservir) :

Avec ça, on a déjà une consigne pas trop mal pour déplacer le robot. Pour clôturer ce chapitre, on va simplement ajouter un tout petit bout de code pour gérer la marche arrière. En effet, avec le précédent calcul Dist est toujours positif (on demande toujours au robot d’avancer), alors si son objectif est derrière lui il va y aller de cette façon :

Alors qu’une simple marche arrière toute droite serait tellement plus efficace !

Pour s’occuper de cette marche arrière, on va inverser le calcul de la consigne si l’objectif dépasse la moitié du robot (90° ou -90°). Concrètement, si le Cap dépasse PI/2 ou est inférieur à –PI/2 on inverse le signe de Dist et on ajoute PI au calcul du Cap. L’algo devient alors :

Si on est en marche arrière, il faut de nouveau penser à placer le Cap entre PI et –PI.

Avec cette simple condition de marche arrière, on obtient bien cette ligne droite au plus court :

Laisser un commentaire

Votre adresse de messagerie ne sera pas publiée. Les champs obligatoires sont indiqués avec *