Le but de ce document est de décrire le fonctionnement du futur asservissement qui sera implémenté sur le robot de Microb Technology en 2005.
Ce qui en ressort est inspiré par ce que j'ai réalisé en colaboration avec Tof1.1 en 2003, par ce même Tof en 2004, et grâce à certains conseils avisés du professeur (bien connu) de l'IUT de Ville d'Avray. J'ai aussi essayé de regarder un peu ce qui a été fait par d'autres équipes.
Dans ce chapitre, je vais tenter de décrire les choix généraux qui ont été faits, et de les justifier.
Je travaille depuis 2002 avec ces microcontrôleurs, je les trouve vraiment intéressants par rapport aux autres que j'ai utilisés : les 8051 et 68HC11 (je ne connais pas les PICs). En effet, ils sont à la fois très performants (1 cycle d'horloge = 1 top d'horloge = 1 instruction2.1), et ils disposent d'outils de developpements très aboutis et libres, la suite avr-gcc2.2.
Comme le reste du robot utilisera ce processeur, le choix de cette famille de microcontrôleurs est donc logique. J'ai choisi d'implémenter l'asservissement que je vais décrire ici sur un AtMega128, car je sais que je n'en atteindrai pas les limites, soit 128Ko de flash, 4Ko de RAM et une fréquence de fonctionnement de 16Mhz. De plus, il dispose de nombreuses sorties PWM et ports pour y connecter les compteurs des roues (l'architecture est présentée ci-dessous).
L'architecture de la carte est finalement assez simple. Outre le noyau central composé de l'ATMega128, il y a 3 AT90s2313 dont le rôle est effectuer des comptages/décomptages : les signaux des moteurs arrivent sous la forme de compteurs gray 2 bits (les fameux canaux A et B des roues codeuses). Chaque AT90s2313 prend en entrée 4 signaux (A1, B1, A2, B2) et sort sur un port de 8 bits la valeur du compteur sélectionné en binaire naturel sur 8 bits (la selection s'effectue via une broche).
On peut donc récupérer des informations en provenance de 6 codeurs et envoyer 4 PWM. L'objectif ici est de pouvoir asservir 4 moteurs2.3, dont deux disposants de roues codeuses supplémentaires (voir paragraphe ci-dessous).
La carte se comporte alors comme une boîte récupérant des consignes et sa configuration via la liason I2C, et envoyant les commandes aux moteurs. La carte mère devra aussi être en mesure de récupérer des informations via la liaison I2C, comme la position des moteurs, ou bien tout simplement par l'intermédiaire de flags sur les ports, indiquant la fin de la trajectoire ou un blocage.
Une idée qui me paraît importante à mettre en place est de disposer, en plus des roues de propulsion du robot, de roues non motorisées effectuant la mesure de la position. En effet, les roues motorisées, pour être efficaces, doivent être plutôt larges et molles. Cela est malheureusement incompatible avec une mesure fiable de la position (diamètre de la roue et distance entre les 2 roues imprécis). De plus, ce roues peuvent avoir tendance à patiner lors de blocage contre un obstacle ou d'une forte accélération.
C'est pourquoi plusieurs équipes (dont Eirbot à partir de 2003) utilisent des roues folles non motorisées, et placées sur le même axe que les roues motorisées. Elles sont à l'inverse fines et dures, montées sur suspensions, et ne glissent quasiment pas. Pour avoir vu fonctionner des robots avec ce système (Minitech, Eirbot, Ville d'Avray 2005), je peux affirmer que la précision est redoutable, même en cas de choc.
Un autre choix est celui de l'asservissement en Theta-alpha. Une idée de Tof pour Eirbot en 2004, elle me paraît plutôt bonne. Elle consiste à ne pas asservir, comme habituellement sur les robots de la coupe, chacune des roues avec un PID, mais plutôt d'utiliser un PID pour asservir la différence des positions de roues (l'angle Alpha), et un autre PID pour asservir la distance parcourue (Theta)
Voici le détail de l'architecture du code d'asservissement. Pour l'instant l'architecture est sujette à quelques changements, mais les modules et les liens entre ceux-cis semblent cohérents.
Commençons par détailler les modules auxiliaires, dans un ordre
logique. Le schéma de la figure ![[*]](/usr/share/latex2html/icons/crossref.png){kind=icon}
présenté ici est
légèrement adapté pour ne présenter que la partie propulsion, mais si
d'autres moteurs sont utilisés sur le robot (par exemple pour
actionner un rouleau), l'architecture restera assez semblable à
l'exception des modules Trajectory, Position, Block_detect, et
surtout Theta-Alpha.
Le rôle du scheduler est de planifier l'appel de fonctions à
intervalle de temps régulier. Dans notre cas, il convient d'appeler la
fonction d'asservissement environ toutes les 1 à 10 ms (à définir). En
pratique le scheduler n'appelera pas la fonction
asserv_update() mais une fonction asserv_allow() qui
aura pour effet de mettre un flag à 1. Le code de la fonction
d'asservissement est alors exécuté dans la boucle principale, au lieu
d'être exécuté par le scheduler dans une interruption. Ce même code
remettra alors le flag à 0.
L'interface du module est déjà définie, car utilisée par d'autres projets.
void scheduler_add_single_event(void (*f)(void), u16 time) void scheduler_add_periodical_event(void (*f)(void), u16 period)
L'appel d'une de ces 2 fonctions activera ensuite l'appel de f de manière périodique ou unique.
void scheduler_del_event(u08 id)
Bien que non indispensable dans le cas notre asservissement, on peut imaginer pouvoir supprimer un évènement.
Ce module s'occupe des couches bas-niveau concernant la communication par le bus I2C3.1. Il fournira des fonctions d'interface simples pour émettre des données sur le bus.
Pour l'instant pas d'interface prévue.
Le module de communication utilisera le module I2C. Son objectif est de définir une interface simple pour envoyer et recevoir des commandes, ainsi que partager des données entre les cartes. Cette partie est encore floue à l'heure actuelle, mais elle n'est pas critique dans notre cas.
Pour l'instant pas d'interface prévue.
Ce module reçoit des commandes du modules de communication. En fait, d'une manière plus globale, la carte mère envoie à la carte d'asservissement des commandes de haut-niveau, c'est à dire ``va tout droit, à telle vitesse, sur telle distance''3.2, ou bien encore ``va à telle position x,y,a''. Le module de communication les décode, et appelle les fonctions du module Trajectory. Ce dernier les transforme en commandes de moyen-niveau, c'est à dire en consignes de position, vitesse et accélération pour theta, et position, vitesse et accélération pour alpha.
void trajectory_straight(u16 speed_cm_s, s16 pos_cm) void trajectory_c_swivel(u16 speed_cm_s, s16 angle_degre) void trajectory_l_swivel(u16 speed_cm_s, s16 angle_degre) void trajectory_r_swivel(u16 speed_cm_s, s16 angle_degre) void trajectory_turn(u16 speed_cm_s, s16 circle_center_cm, s16 angle_degre)
Ces fonctions génèrent des paramètres d'asservissement (accélération,
vitesse, position) afin de produire des trajectoires. Pour le moment,
l'interface n'est pas finale, il reste à définir comment les
paramètres sont passés à l'asservissement (a priori ces fonctions
modifieront une structure contenant les paramètres de
l'asservissement).
void trajectory_straight_while(s16 speed_cm_s, u16 time_ms) void trajectory_c_swivel_while(s16 speed_cm_s, u16 time_ms) void trajectory_l_swivel_while(s16 speed_cm_s, u16 time_ms) void trajectory_r_swivel_while(s16 speed_cm_s, u16 time_ms) void trajectory_turn_while(s16 speed_cm_s, s16 circle_center_cm, u16 time_ms)
Ces fonctions, au lieu de prendre comme condition de fin une distance, prennent un temps. Cela signifie qu'il faudra ajouter un paramètre timeout aux paramètres de l'asservissement.
Au passage, il pourrait être intéressant de toujours disposer d'un timeout sur les fonctions, en cas de blocage des roues. L'inconvénient est que le timeout dépend des consignes : le timeout doit être petit si la fonction est une courte distance, et grand dans le cas de longues trajectoires.
void trajectory_goto_xya(u16 speed_cm_s, u16 x, u16 y, s16 a)
Il pourrait être intéressant d'ajouter une ou plusieurs fonctions permettant de se rendre à un point précis sur le terrain, comme ci-dessus. Pour le moment, nous nous contenterons des premières.
Le module d'asservissement reçoit des consignes du module
Trajectory. De manière régulière, la fonction asserv_update()
est appelée par le Scheduler. Cette fonction va d'abord transformer
les consignes reçues en une seule consigne de position, car
l'asservissement effectué n'est qu'un asservissement de position
PID. Le but de ce module est de générer la consigne de position en
fonction des limites de vitesse et d'accélération, et de la position
finale demandée par le module Trajectory. Cela permet de générer des
trajetoires dites trapézoïdales3.3.
Typiquement, si on prend un exemple simple, voici les valeurs envoyées par le module Trajectory :
La suite de consignes générées par le module Asserv sera alors :
V = 1,2,3,4, 5, 6, 7, 8, 9, 10,10,10,10,10, 10, 9, P = 1,3,6,10,15,21,28,36,45,55,65,75,85,95,105,114, V = 8, 7, 6, 5, 4, 3, 2, 1 P = 122,129,135,140,144,147,148,149,150
La courbe de consigne de position générée est de forme trapézoïdale.
Nous allons maintenant voir en détails les étapes de l'algorithme qui va générer en temps réel la consigne de position
Le principe général est de chercher à aller le plus vite possible,
sans dépasser les limites fixées par les consignes. En clair, si on
n'a pas atteint la vitesse maximum, on continue à accélérer, sinon, on
reste à cette vitesse.
On passe en phase de décélération lorsque l'algorithme estime être suffisemment proche de sa cible, en fonction évidemment de sa vitesse actuelle et de ses capacités de décélération.
C'est la partie la plus dure de l'algorithme. Le programme doit être
capable déterminer, à partir de la distance restante et des paramètres
de vitesse et d'accélération, le moment à partir duquel il devra
commencer à freiner.
Par exemple on a Amin = -2 Et on a Vmax = 16, et Vcurrent = 16 La position cible à atteindre est 172 On veut une courbe de décelération de ce type :
-n 9 8 7 6 5 4 3 2 1 0 A 0 0 -2 -2 -2 -2 -2 -2 -2 0 V 16 16 14 12 10 8 6 4 2 0 d restante 88 72 56 42 30 20 12 6 2 0 Pconsigne 100 116 130 142 152 160 166 170 172 172
On a dans la phase de décélération :
Ce qui nous donne :
Sauf qu'on ne connait que d (et A), et qu'on veut récupérer V pour déterminer Pconsigne.
On développe, puis on utilise la forme canonique :
On dispose d'un algo qui donne à peu près la racine carrée d'un entier.
L'algorithme va donc saturer la vitesse max par :
On dispose à ce stade d'une vitesse min et d'une vitesse max, cette dernière ayant éventuellement été diminuée par la partie ci-dessus imposant la rampe.
Or, à chaque instant, il est impossible d'atteindre une vitesse très différente de la vitesse à l'instant précédent : les paramètres d'accélération min et max sont la pour ça. On va donc réduire la plage de vitesse atteignable en fonction de ces paramètres.
Par exemple, si on a :
Amin = -1 Amax = 5 Vcurrent = 10 Vmin = -20 Vmax = 13
En sortie, on aura :
Vmin = 9 Vmax = 13
En effet, l'accélération min est -1, donc lorsque la vitesse courante vaut 10, on ne peut pas ralentir à moins de 9. Par contre, l'accélération max est 5, ce qui signifie qu'à partir d'une vitesse de 10 on pourrait atteindre une vitesse de 15 au coup suivant. Or, la vitesse maximum est de 13, donc Vmax n'est pas modifiée.
On a notre plage de vitesse atteignable, on cherche à atteindre la position cible le plus vite possible en restant dans cette plage. Cependant, dès qu'on a atteint la position ``critique'', c'est à dire le point à partir duquel il faut commencer à ralentir, Vmax est à nouveau limité par l'algorithme générant la rampe de décélération.
Dûe à l'approximation faite sur la rampe de décélération (en fait sur les racines carrées), le trapèze n'est pas parfait : la pente varie légèrement au cours du temps. Il n'empêche que l'algorithme offre un bon compromis vitesse d'exécution sur qualité de la consigne générée.
La figure montre un exemple de consigne générée.
C'est un module qui prend une consigne en entrée, ainsi que des paramètres de gains proportionnel, dérivé et intégral, qui calcule une commande. Ce module est utilisé pour asservir theta, et pour asservir alpha.
Ce module effectue des conversions entre la base theta-alpha, et les
valeurs des compteurs retournées par les roues codeuses. On a :
Ainsi, on peut communiquer avec les modules pwm et counter, qui eux ne sont pas en base theta-alpha.
On trouve deux structures :
struct theta_alpha
{
s32 theta;
s32 alpha;
}
struct left_right
{
s32 left;
s32 right;
}
struct theta_alpha theta_alpha_from_left_right(struct left_right lf) struct left_right theta_alpha_to_left_right(struct theta_alpha ta)
Ces fonctions permettent de changer de base.
Ce module permet d'utiliser l'interface bas-niveau fournie par le microcontrôleur pour générer des signaux à modification de largeur d'impulsion, afin de commander les moteurs.
void pwm_init(u08 mode, u08 prescale) void pwm_set(s16 value)
Pas de difficultés pour ce module.
Ce module communique avec les AT90s2313 pour récupérer les informations provenant des roues codeuses. Les valeurs des compteurs sont stockées dans des entiers de 32 bits.
void counter_init() u32 counter_get()
Pas de problèmes là non plus.
Ce module (facultatif) fait la corrélation entre les valeurs des codeurs des roues motorisées et des roues folles. Si les valeurs sont incohérentes, il peut s'agir d'un blocage, et l'information est remontée.
Plutôt que d'être interfacé avec le module de communication, les flags pourraient remonter vers le module d'asserv, afin d'implémenter une sorte d'antipatinage. Pour le moment ce n'est pas à l'ordre du jour.
Pas d'interface définie pour le moment. J'imagine pour le moment une tâche planifiée qui appelerait une fonction de block_detect de manière régulière, par exemple :
block_detect_update(cod_mot_g,cod_fou_g,cod_mot_d,cod_fou_d)
Il s'agirait ensuite de correler tout ça avec les paramètres physiques du robot ainsi que les précédents updates. La fonction pourrait alors placer un flag à vrai.
Le rôle de ce module est de récupérer les valeurs des codeurs (converties en theta-alpha) afin de tenir à jour une position (x,y,alpha) sur le terrain. Les calculs seront effectués en flottant, peut-être toutes les 10 ms.
De la même manière que pour le module block_detect, rien n'est sûr mais une tâche planifiée qui appelerait une fonction d'update serait l'idée.
position_update(theta,alpha).
Ce code est écrit en assembleur, car il doit tourner le plus vite possible. Le code est déjà disponible sur le CVS.
Voilà en gros comment il fonctionne :
A1(t-1),B1(t-1),A2(t-1),B2(t-1),A1(t),B1(t),A2(t),B2(t).
Il est toujours intéressant de réfléchir à un projet avant de le réaliser, cela permet d'accélérer le développement et de mettre en évidence certains défauts.
L'architecture semble satisfaisante et correspond aux besoins. Ce document sera maintenu à jour au fur et à mesure de l'avancement du projet.
This document was generated using the LaTeX2HTML translator Version 2002-2-1 (1.70)
Copyright © 1993, 1994, 1995, 1996,
Nikos Drakos,
Computer Based Learning Unit, University of Leeds.
Copyright © 1997, 1998, 1999,
Ross Moore,
Mathematics Department, Macquarie University, Sydney.
The command line arguments were:
latex2html -split 0 asservissement2005.tex
The translation was initiated by Olivier MATZ on 2005-02-03