Warning: "continue" targeting switch is equivalent to "break". Did you mean to use "continue 2"? in /htdocs/wiki/inc/parser/handler.php on line 1552
asservissement_mbed [EsialRobotik]

Outils pour utilisateurs

Outils du site


asservissement_mbed

Asservissement

Introduction

Le principe de l'asservissement est un système bouclé, c'est à dire qu'à chaque itération nous avons besoin d'un feedback sur certaines grandeurs. Dans notre cas, il s'agit de la variation de la distance, et la variation du cap, qui nous permettent de contrôler avec précision les déplacements linéaires et angulaires du robot.

L'asservissement est composée de plusieurs parties :

  • l'Odométrie, qui mesure les déplacements du robot,
  • le CommandManager, qui gère une file d'ordre de déplacement,
  • le ConsignController, qui gère l'exécution de la commande courante,
  • le MotorController, qui donne des ordres aux moteurs,
  • le Régulateur qui calcule la consigne des moteurs en fonction de la consigne filtrée à atteindre et de l'erreur filtrée,
  • le filtre QuadRampDérivée, qui traduit la consigne à exécuter en fonction de différs paramètres,
  • le filtre PID, qui traduit l'erreur entre la position réelle et la position cible en fonction des paramètres d'asservissement

On pourrait se poser une question simple : « Et si on prenait 2 moteurs identiques, et qu'on les alimentait avec la même puissance, cela devrait permettre d'avancer tout droit, non ? »

La réponse est oui, mais uniquement dans le pays des Bisounours ! En réalité, les cartes de puissance n'envoient jamais exactement la même puissance aux moteurs, et même si cela était le cas, les moto-réducteurs n'opposeraient pas les mêmes frottements, les surfaces pourraient être différentes sous chaque roue…

En somme, pour viser le haut du classement à la coupe, un asservissement est obligatoire !

Odométrie

WTF ?

L'odométrie consiste à mesurer le déplacement de chaque roue du robot afin de déterminer sa position actuelle par rapport à sa position initiale. Pour cela, on utilise des roues codeuses : ce sont des capteurs mesurant la rotation de chaque roue en tics codeurs (plusieurs centaines de tics par tour). Il est très important de s'assurer de l'adhérence complète des roues codeuses : on ne mesure pas la rotation des roues motrices, qui peuvent glisser, on ne mesure pas la rotation de l'arbre moteur qui n'est pas moto-réducté, et on plaque les roues codeuses au sol (suspension POWAAAAA!). La méthode calculant la position du robot est l'une des trois méthodes appelée par la boucle d'asservissement, soit 200 fois par seconde environ.

Unité odométrique

Dans tout l'asservissement, on travaille en unités odométrique (ou UO). Cette unité est un multiple arbitraire (une puissance de deux) d'un tic codeur, dans le but de manipuler des grands nombres afin de réduire l'impact des erreurs d'approximation, car le tic codeur est la seule grandeur sûre.

Paramètres à régler

Dans le fichier “config.h” se trouvent les quatre paramètres à régler en fonction du robot :

  • frontParMetreCodeurG : nombre de tics mesurés par la roue codeuse gauche lorsqu'elle parcourt un mètre,
  • frontParMetreCodeurD : idem, pour le codeur droit,
  • distRoues : distance en mm entre les centres des deux roues codeuses,
  • uOParFront : nombre d'UO par tic codeur (puissance de deux à choisir arbitrairement)

Grandeurs mesurées

L'odométrie permet de déterminer l'état courant du robot, défini par ces grandeurs :

  • Dd, Dg : déplacements des roues Gauche et Droite entre les instants T et T + deltaT,
  • x, y : coordonnées du point au milieu de l'axe des roues codeuses,
  • theta : cap du robot par rapport à son angle initial
  • deltaDist : vitesse linéaire du robot (en fait, distance parcourue entre T et T + deltaT,
  • deltaTheta : vitesse angulaire du robot.

Calculs

Dd et Dg sont donnés directement par les codeurs. On en déduit :

deltaDist = (Dd + Dg) / 2
deltaTheta = (Dd - Dg) / distRoues

Pour le calcul des paramètres x, y et theta, deux possibilités s'offrent à nous :

Si deltaTheta est nul, le robot se déplace en ligne droite. theta ne change pas, et on en déduit x et y :

x = x + deltaDist * cos(theta) 
y = y + deltaDist * sin(theta)

Si deltaTheta n'est pas nul, alors on considère que le robot décrit un arc de cercle de rayon de courbure R :

R = deltaDist / deltaTheta

On en déduit les coordonnées x et y en utilisant le theta non mis à jour (à l'instant T) :

x = x + R * ( -sin(theta) + sin(theta + deltaTheta ) ) 
y = y + R * ( cos(theta) - cos(theta + deltaTheta ) )

Enfin, on met theta à jour :

theta = theta + deltaTheta

Afin d'éviter les robots “enrouleur de câble”, il est vivement recommandé de maintenir theta entre -pi/2 et pi/2 .

CommandManager

Le CommandManager a pour rôle de gérer la file des différentes instructions données à l'asservissement. L'objectif est de stocker quelques actions d'avance afin de pouvoir déclencher le mouvement suivant quand l'on arrive à la fin du mouvement courant pour obtenir quelque chose de fluide. Le CommandManager gère également l'arrêt d'urgence logiciel du robot, qui vide la file d'instructions et place la position courante comme consigne. Il ne faut pas oublier de faire un reset de l’arrêt d'urgence pour pouvoir repartir.

Il y a quatre types de commandes que l'on peut ajouter à la file:

  • addStraightLine(int64_t valueInmm) : Permet d'ajouter une instruction “va tout droit sur valueInmm mm”
  • addTurn(int64_t angleInDeg) : Permet d'ajouter une instruction “tourne de angleInDeg degrés”
  • addGoTo(int64_t posXInmm, int64_t posYInmm) : Permet d'ajouter une instruction “va à la position x = posXInmm, y = posYInmm”
  • addGoToTurn(int64_t posXInmm, int64_t posYInmm) : Permet d'ajouter une instruction “aligne toi avec la position x = posXInmm, y = posYInmm”

<note important>Tous les paramètres sont à fournir en mm ou en degrés, la conversion vers les UO se fera automatiquement. De plus, pour les GoTo, les positions (x;y) sont calculées dans le plan ayant pour origine la position de départ du robot. Le sens des axes dépend du sens dans lequel sont branchés les codeurs (d'ailleurs faudrait corriger ça un jour … en branchant les codeurs à l'endroit par exemple)</note>

La méthode perform du CommandManager est l'une des trois méthodes appelées par l'interruption d'asservissement. Son rôle est d'ajuster la commande courante à fournir au ConsignManager. S'il détecte l'arrivée à la fin d'une consigne, il doit démarrer la suivante. Si la commande courante est un GoTo ou un GoToTurn, il détermine la distance et l'angle à parcourir pour en faire une consigne pour le ConsignController.

ConsignController

Le ConsignController récupère la position du robot de la part de l'odométrie et demande aux Régulateurs de distance et d'angle de calculer les consignes longitudinale et angulaire à fournir ensuite aux moteurs, on obtient donc deux consignes : dist_output et angle_output. On calcule ensuite assez bêtement les consignes à fournir aux moteurs en donnant à l'un la somme de ces deux consignes et la différence de la consigne de distance et d'angle au second. Afin de ne pas avoir un robot totalement incontrôlable parce que trop rapide, on borne les vitesses envoyées aux moteurs entre V_MAX_NEG_MOTOR et V_MAX_POS_MOTOR.

MotorController

L'objectif du MotorController est, comme son nom l'indique, de contrôler les moteurs du robot. Les fonctions setVitesseG et setVitesseD prennent en paramètre les vitesses à appliquer aux moteurs, calculées à partir des valeurs de sortie des deux régulateurs, dist_output et angle_output :

VmoteurG = dist_output + angle_output 
VmoteurD = dist_output - angle_output

Il faut ensuite s'assurer que les vitesses sont entre les valeurs V_MAX_NEG_MOTOR et V_MAX_POS_MOTOR, pour éviter des robots s'arrêtant brusquement lorsqu'on veut leur donner la vitesse maximale.

Le MotorController n'est, à la base, qu'une interface : il faut coder une implémentation pour chaque modèle de carte moteur que l'on souhaite utiliser. On a également deux paramètres à régler, dépendants eux aussi de la carte moteur :

  • V_MAX_NEG_MOTOR : vitesse minimum à demander aux moteurs, en théorie négative,
  • V_MAX_POS_MOTOR : vitesse maximum.

Régulateur

Un Régulateur gère soit la distance, soit l'angle. Dans les deux cas le fonctionnement est identique. Son rôle est de retenir la distance (respectivement l'angle) totale parcourue depuis la mise en route du robot, d'appeler la QuadRampDerivee qui va lisser la consigne à atteindre pour pouvoir accélérer/décélérer et le PID qui va convertir l'erreur en quelque chose de compréhensible par l'asservissement en fonction de ses paramètres. Une fois que l'on connait l'erreur entre la consigne à suivre et notre état actuel, on peut la corriger en envoyant le résultat du PID au ConsignManager qui va mixer les deux résultats pour les donner aux moteurs.

QuadRampDérivée

But

La QuadRampDerivee filtre la consigne que l'on souhaite atteindre. Mais pourquoi diable aurait-on besoin d'un filtre sur la consigne ? Après tout, la consigne dit : « Va à cet endroit » , ni plus ni moins … Pourtant ce n'est pas satisfaisant !

Prenons un exemple simple : disons que nous donnons comme consigne d'avancer d'un mètre. À la première itération de la boucle d'asservissement, l'erreur va être d'un mètre, et il y a fort à parier que la vitesse de sortie va tout de suite être … à fond ! En d'autres termes, la vitesse des moteurs va passer de rien à tout, et pour peu que les moteurs soient puissants, nous allons déraper !

Pour éviter de déraper nous allons donc augmenter progressivement la vitesse et freiner progressivement en arrivant sur la consigne. Mais comment faire varier la vitesse si l'asservissement est sur la position me direz vous ? Tout simplement en découpant la consigne en petit bout et en augmentant/diminuant la taille de ces bouts. Aussi il faut se demander à partir de quel moment (endroit ?) doit-on commencer à décélérer pour arriver sur notre cible à vitesse nulle. Pour cela, plusieurs techniques sont possibles :

  • La première consiste à calculer le moment idéal théorique. Le problème étant que si le système est perturbé ( ie : un obstacle bloque le robot x secondes ), alors les consignes théoriques seront totalement dans les choux !
  • La seconde méthode calcule cette position en fonction de la vitesse actuelle de robot, mais elle induit une certaine instabilité. C'est cette méthode que nous allons utiliser.

Calculs

Nous allons appeler pivot cette position. Sa valeur est donnée par la formule :

position_pivot = vitesse2 / ( 2 * décélération_max)

Problème : si on applique directement cette formule, le démarrage est propre, mais l'arrêt est relativement brutal ! En fait, il faudrait commencer à décélérer plus tôt. Pour cela, on rajoute de “l'anticipation” :

position_pivot = vitesse2 / ( 2 * décélération_max) + | vitesse | * gain_anticipation

Plus ce gain d'anticipation est élevé, plus la décélération commence tôt, rendant aussi l'arrêt plus long.

Ensuite, suivant la position courante du robot par rapport à ce pivot, on accélère ou on décélère le robot : Si position_pivot > position_actuelle, on accélère :

consigne_accélération = accélération_max

sinon, on décélère :

consigne_accélération = décélération_max

On calcule ensuite une consigne de vitesse :

consigne_vitesse = prev_consigne_vitesse + consigne_accélération

Cette consigne de vitesse doit être bornée, parce qu'il faut éviter de trop déconner en atelier.

Ensuite, on calcule une nouvelle consigne de position, que l'on renverra aux filtres suivants :

nouvelle_consigne = consigne + consigne_vitesse

Nous avons parlé plus tôt d'un risque d'instabilité. Pour l'éviter, on définit une fenêtre autour de la consigne de position. Si le robot est dans cette fenêtre d'arrivée, on ne calcule pas de nouvelle consigne : on renvoie directement la consigne courante et on laisse ensuite le PID se débrouiller.

nouvelle_consigne = consigne
consigne_vitesse = 0

Paramètres à régler

Pour simplifier, nous avons en fait supposé dans les calculs que le robot se comportait de façon identique en marche avant et en marche arrière. Ce n'est malheureusement pas toujours le cas. Par exemple, si quasiment tout le poids de notre robot est situé sur l'arrière (c'était le cas de notre robot de 2011), une même valeur de décélération peut paraître faible lors d'un arrêt en marche avant et suffire à retourner le robot lors d'un arrêt en marche arrière (c'était encore le cas de notre robot de 2011) ! Nous avons donc une série de paramètres pour la marche avant et une autre pour la marche arrière.

Il faut donc régler, pour la QuadRamp de distance :

  • DIST_QUAD_1ST_POS : Vitesse maximale en marche avant
  • DIST_QUAD_AV_2ND_ACC : Accélération maximale en marche avant
  • DIST_QUAD_AV_2ND_DEC : Décélération maximale en marche avant
  • DIST_QUAD_AV_ANTICIPATION_GAIN_COEF : Gain d'anticipation pour la marche avant
  • DIST_QUAD_1ST_NEG : Vitesse maximale en marche arrière
  • DIST_QUAD_AR_2ND_ACC : Accélération maximale en marche arrière
  • DIST_QUAD_AR_2ND_DEC : Décélération maximale en marche arrière
  • DIST_QUAD_AR_ANTICIPATION_GAIN_COEF : Gain d'anticipation pour la marche arrière
  • DIST_TAILLE_FENETRE_ARRIVEE : Taille de la fenêtre d'arrivée (en Unité Odométrique)

et pour la QuadRamp d'angle :

  • ANGLE_QUAD_1ST_POS : Vitesse de rotation maximale
  • ANGLE_QUAD_2ND_ACC : Accélération maximale en rotation
  • ANGLE_QUAD_2ND_DEC : Décélération maximale en rotation
  • ANGLE_QUAD_ANTICIPATION_GAIN_COEF : Gain d'anticipation pour la rotation
  • ANGLE_TAILLE_FENETRE_ARRIVEE : Taille de la fenêtre d'arrivée (en Unité Odométrique)

Voir Réglage de l'asservissement pour le “protocole” de réglage de ces coefficients

PID

But

Avant toute chose, nous devons définir la notion d'erreur. Cette valeur correspond à la soustraction de la position actuelle donné par l'accumulateur à la position donné par la consigne (de manière analogue du cap actuel au cap voulu). L'erreur est la distance (ou l'angle) entre là où on est, et là où on veut aller.

A partir de cette erreur, le PID est capable de nous calculer une vitesse à appliquer aux moteurs via trois sous-calculs : le Proportionnel , l'Intégrale et la Dérivée (d'où le nom PID).

Calculs

Proportionnel

Ce calcul est défini de la façon suivante :

P = erreur * Kp
où Kp est le coefficient de proportionnalité.

Cela représente en quelque sorte la base de notre régulateur, c'est cette valeur qui va donner la direction et la puissance initiale du système. C'est pourquoi on pourra trouver des PID avec Ki nul et/ou Kd nul, mais jamais avec Kp nul ! Comme on peut le constater en regardant le calcul, dans notre cas le proportionnel va donner une vitesse d'autant plus grande que l'erreur est grande.

Intégrale

Ce calcul nécessite un pré-requis, il faut calculer l'intégrale des erreurs jusqu'à présent (une bête somme, si le mot intégrale vous fait peur :-) ).

intégrale += erreur 
I = intégrale * Ki

Tant que l'erreur n'est pas nulle, ce terme augmente : il tend donc à faire diminuer cette erreur en augmentant la vitesse des moteurs. Par contre, ce terme peut augmenter en permanence si l'erreur n'est jamais nulle (ce qui a de forte chance d'arriver.) Il faut donc absolument le limiter :

| intégrale | < maxIntégrale

Malgré ça, ce terme risque toujours d'être en permanence égal à maxIntégrale ou à -maxIntégrale, le robot n'étant pas un système linéaire. Bref, cela rend le système instable, quoi qu'il arrive. Le seul moyen d'éviter ce problème, c'est de mettre Ki à 0. Radical, mais très efficace !

Ah, évidemment, si toi, lecteur, tu as la solution à ce problème, n'hésite pas à coder et à tester, mais sache que de toute façon, ça ne servira pas à grand chose si tout le reste est bien réglé.

Dérivée

Il nous faut pour ce calcul connaître la dérivée de l'erreur. Une manière simple de la calculer est de faire, à chaque itération :

dérivée = erreur – erreurPrecedente 
erreurPrecedente = erreur

A vrai dire, cette valeur n'est pas très pertinente, puisque la consigne change tout le temps, à cause de la QuadRampDérivée. Elle ne représente plus la vitesse du robot, qui est la donnée qui nous intéresse vraiment. On va plutôt utiliser comme valeur le deltaDist récupéré de l'odométrie :

D = - deltaDist * Kd

Ce terme va avoir pour effet d'amortir les oscillations et donc de stabiliser le système. Il ne va pas fortement influencer sur le temps de montée vers la consigne, mais le dépassement va diminuer et le temps d'arrivée au régime stationnaire sera plus court. Ce coefficient n'a aucune influence sur l'erreur statique.

Derniers calculs

La sortie aura pour valeur :

sortie = P + I + D

Problème : cette valeur va être énorme, étant donné que tous les calculs sont fait en unité odométrique. Or, on doit donner aux moteurs une vitesse de l'ordre de quelques dizaines d'unités. Cela imposerait de paraméter les coefficients Kp,Ki et Kd à des valeurs ridiculement petites (inférieures à 1). Pour éviter ça, on multiplie la sortie par un coefficient inférieur à 1 :

sortie = sortie * ratioSortie

On définit enfin une valeur maximale de sortie, telle que :

| sortie | < maxOutput

Paramètres à régler

  • Kp : Le coefficient proportionnel
  • Ki : Le coefficient intégrale
  • Kd : Le coefficient dérivée
  • maxIntégrale : La valeur maximale de l'intégrale (la valeur de I)
  • ratioSortie : Un coefficient inférieur à 1 pour avoir une valeur de sortie cohérente
  • maxOutput : La valeur de sortie maximale du filtre.
asservissement_mbed.txt · Dernière modification: 2017/11/06 14:10 (modification externe)