Introduction
Après un article sur comment Améliorer vos régulateurs PID, et l’explication des principaux concepts sur l’asservissement d’un robot autonome, nous allons maintenant vous présenter notre choix final concernant l’architecture, le mode de fonctionnement polaire de l’asservissement en vitesse et position, et dans la 2ème partie, le code associé, et les explications comment l’utiliser et le mettre en œuvre.
Il nous faut ici 2 moteurs et roues motrices, avec 2 roues codeuses indépendantes ou encodeurs, le tout aligné sur l’axe principal du robot.
Cela fait plusieurs années que nous avons travaillé ensemble avec EsialRobotik sur ce sujet, nous avions l’ambition de créer cet article, ainsi que fournir les sources mutualisées, qui sont disponibles sur le github EsialRobotik/asserv_chibios, pour le bonheur des roboticiens qui en auraient besoin.
Spéciale dédicace à Jeff Guyonneau (Cheff pour les intimes) pour toute son implication et son travail sur ce sujet, et à Christophe Chaudelet (Cho) d’avoir testé et retesté et re-retesté, ainsi on peut vraiment en conclure que : “Tester, ce n’est pas Douter, c’est Fiabiliser !” 🙂
=> N’hésitez pas à nous poser des questions (sur le Discord Eurobot-CDR par exemple), nos réponses serviront immanquablement à améliorer cet article au fur et à mesure !
L’idée
Créer un asservissement performant et flexible pour piloter nos robots autonomes, avec des unités de mesure SI, en mode polaire, tout en loguant des graphiques en temps réel, avec une gestion “sans s’arrêter” à des points de passage désirés, que chacun pourrait instancier et réutiliser!
SOMMAIRE
1ère partie – Asservissement et architecture – Cet article
2. Architecture matérielle du robot
3. Architecture logicielle du robot
4. Architecture de l’asservissement (Motion System)
5. La génération de trajectoire du robot
6. La technique du GOTO enchainé
7. La technique de la détection du blocage
8. La notification des données
2ème partie – Instancier son code d’asservissement – Page suivante
3. Comment régler son “asserv” ?
1. Quels objectifs
De par notre expérience en tant que participant à la coupe de France de robotique, nous savons qu’un asservissement est une brique de base obligatoire pour accéder ensuite à des travaux intéressants sur un robot.
Si le robot ne sait pas où il est, ou ne sait pas aller à un endroit précis sur la table, le bras le plus classe du monde ou la mécanique la plus (“chiadée”) complexe, ne servent à rien 🙂
Malheureusement cette brique n’est pas facile à appréhender :
- Il y a peu de personnes ou de ressources qui permettent de s’initier “simplement” à cette question.
- La transmission de connaissances sur le sujet est difficile entre générations
Petit souvenir de Jeff, ému à son arrivée dans le club de robotique de notre école où on lui a montré un PCB rouge avec marqué “carte d’asserv” dessus et ZERO documentation !
- Cela nécessite des connaissances en langage de programmation, en mathématiques, électronique et mécanique. C’est normal en robotique, vous allez nous dire, mais pour des équipes “jeunes” qui commencent, atteindre le niveau minimale dans toutes ces disciplines peut être très, très long et la finalité pas forcément très motivante.
Donc, lorsque Jeff a décidé, fin 2019, de refondre tout le code de l’asservissement, on s’est donné quelques objectifs que voici :
- Faire aussi accessible que possible : que ce soit en terme de compréhension du fonctionnement ou du réglage.
- Fournir un système de visualisation de données, de façon à pouvoir comprendre facilement le comportement des formules mathématiques utilisées et donc le comportement final du robot.
- Avoir une base de code qui soit pensée pour être extensible et partageable avec d’autres équipes. Et par conséquent, un code qui soit instanciable facilement sur d’autres microcontrôleurs et en utilisant un matériel différent du nôtre.
- Bien-sûr, avoir un asservissement performant. L’idée étant que si une équipe, ou une personne (le code est public) veut être UTILISATEUR de cet asservissement en quasi “boite noire”, cela est possible. Et dans l’autre cas, si quelqu’un veut implémenter des choses plus avancées et devenir un “power user”, c’est tout à fait possible.
Quels caractéristiques ?
Donc si on devait compiler une liste de caractéristiques de ce projet façon liste à la Prévert:
- Que des unités SI. Ici, pas d’unités incompréhensibles, donc tous les calculs se font en flottant (d’où la nécessité d’une FPU et d’une carte performante)
- Un premier étage d’asservissement des roues en vitesse, puis par-dessus, un asservissement polaire. L’avantage est de pouvoir gérer plus facilement les courbes d’accélération (et aussi la décélération, mais moins facilement…)
- Un décodage hardware des encodeurs (géré dans l’instance souhaitée, par des Timers directement sur le microprocesseur)
- Un système de visualisation en temps réel des données calculés par les boucles de contrôle.
- Un OS temps réel (RTOS).
- Une gestion de points de passage “sans s’arrêter“.
- Des documentations sur le fonctionnement minimal à connaître, et sur la façon de régler cet asservissement.
- Des documentations plus avancées sur le fonctionnement interne, les méthodes alternatives existantes, ….
Pourquoi faire ?
Soyons honnête, l’objectif premier était pour nos équipes d’avoir un meilleur asservissement, moins buggé et plus compréhensible pour tout le monde. Le second objectif était simplement pour montrer à Planète Sciences qu’en tant que personnes trop vieilles pour ces conneries anciens de la coupe de France de robotique, nous jouons le jeu de la vulgarisation scientifique. Le but n’est pas d’écraser la concurrence avec un “chacun pour soi”, “chacun garde ses secrets”, ou “n’a pas le temps pour les explications détaillées”, mais tout à fait l’inverse : il faut de la concurrence qui apprend, s’améliore, et que cela contribue à la montée en connaissance au fur et à mesure de toute la communauté.
Lorsque nous avons commencé à travailler sur un asservissement de robot de coupe de France de robotique, “Aversive” fournit par “Microb Technology 2008” nous a énormément appris et aidé. A nous maintenant de faire de même, avec l’évolution des années et de la technologie.
2. Architecture matérielle
Nous souhaitions créer une base roulante performante, mais avec des solutions du marché qui restent dans un prix acceptable pour nos finances d’association.
Notre matériel
Nous avons donc choisi ce qui suit:
- Motoréducteur DC 12V 30:1 330RPM / 13 kg.cm 37Dx65L CQRobot – 37€
- Motoréducteur DC 12V 43.8:1 228RPM / 18 kg/cm 37Dx65L CQRobot – 37€
- Plusieurs choix pour les encodeurs, ceux qu’on a testé:
- Encodeur ESB cyclon B106 500PPR (étanche poussières) – 200€
- Encodeur KUBLER 05.2400 1024PPR (étanche poussières) – 225€
- Encodeur magnétique fait maison: voir ici Roues codeuses magnétiques à très faible encombrement et haute résolution
- NUCLEO F446-RE – 15€
- Module de commande de moteurs MD22 – 72€
- ou Module de commande de moteurs MD25 – 60€
- Les roues et pneus sont un sujet à aborder prochainement.
- Une carte linux (type Raspberry pi ou Armadeus/Opossom) permet de commander les ordres de haut niveau par liaison série.
- N’oublions pas l’interface dédiée USB (série) pour Plotjuggler afin de débuguer et calibrer l’asservissement avec les données (vitesse, position, etc.) en direct/live.
Schéma d’architecture
Flexibilité
Cependant l’architecture a été pensée pour être flexible. Ainsi, il est tout à fait possible d’utiliser d’autres moteurs, encodeurs ou même une autre carte de puissance.
La seule adhérence forte concerne notre RTOS : Chibios. On peut donc utiliser un autre microcontrôleur du moment qu’il existe une HAL ChibiOS (voir plus bas).
Concernant la gestion de cet asservissement, d’autres cartes sont à l’étude:
Dans la gamme STM32 Nucleo, il est possible d’utiliser une carte Nucleo de la gamme 64 comme NUCLEO-F411RE ou NUCLEO-F40RE (même si l’intérêt est nul), ou une carte Nucleo de la gamme 144 si vous avez besoin de RAM et/ou de flash pour rajouter des fonctionnalités.
On doit également pouvoir utiliser une NUCLEO-L432KC qui a un facteur de forme intéressant. Il faudrait simplement vérifier qu’on ne perd pas de fonctionnalités cruciales et si la capacité de calcul est suffisante.
Une autre nouveauté intéressante est la Raspberry PICO W qui a un facteur de forme intéressant, ne coûte pas chère, et embarque le WIFI, ce qui permettrait d’envoyer les données vers Plotjuggler sans fil. Malheureusement, il n’y a pas de FPU, donc je n’ai pas la certitude que tous les calculs peuvent se faire en respectant les contraintes de temps réel. Sinon il faudrait répartir les calculs entre les 2 cœurs, mais je ne sais pas si cela peut se faire sans casser toute l’architecture logicielle existante ? Si une personne veut se lancer là-dedans, contactez-nous 😉
3. Architecture logicielle
Comme vous pouvez le voir ci-dessous, il y a un certain nombre d’interfaces qui permettent de s’adapter à différentes instances hardware ou à des besoins différents selon ce que l’on veut faire de son robot.
Par exemple, l’implémentation d’EsialRobotik est différente de celle de PM-ROBOTIX et utilise:
- des encodeurs en direct, classe QuadratureEncoder, alors que chez PM-ROBOTIX, nous utilisons les MagEncoders (avec la classe ams_as5048b)
- un contrôleur vmh5019 et un contrôleur MD22 en i²c, alors que chez PM, on utilisait un contrôleur MD25 en i²c et on a changé aussi pour une MD22.
Tout cela pour dire, que le matériel importe peut, chacun peut mapper son matériel (hardware) et créer la classe d’implémentation qui convient.
Langage C/C++
Pour coder les applications sur les cartes STM32 Nucleo, le langage de haut niveau est le C/C++, voir la plateforme de développement mbed.
Comment compiler le code
Tout est sur le Wiki EsialRobotik.
ChibiOS
ChibiOS est un RTOS, c’est-à-dire un système d’exploitation temps réel.
Si les systèmes d’exploitation n’étaient pas si utiles, nous ne les exécuterions pas sur chacun de nos systèmes de bureau. Dans le même ordre d’idées, les systèmes d’exploitation embarqués offrent des fonctionnalités similaires à celles de ces OS de bureau, tout en ciblant un marché plus spécialisé. Certaines d’entre elles sont des versions adaptées d’OS de bureau (par exemple Yocto Linux), tandis que d’autres sont construites à partir de zéro pour des applications embarquées, comme VxWorks et QNX. Cependant, peu de ces systèmes d’exploitation peuvent fonctionner sur un microcontrôleur (MCU). Lorsque vous devez exécuter un système d’exploitation sur quelque chose comme un AVR 8 bits ou un microcontrôleur Cortex-M 32 bits, vous avez besoin de quelque chose de plus petit.
Quelque chose comme ChibiOS («Chibi» signifiant «petit» en japonais), ou FreeRTOS (ici pas de points pour l’originalité). Peut-être plus précisément, FreeRTOS pourrait être résumé comme un cadre multi-threading ciblant les systèmes à faible puissance, tandis que ChibiOS est plus un système d’exploitation complet, comprenant une couche d’abstraction matérielle (HAL) et d’autres subtilités.
Chibi: Peut-être pas si petit
Comme évoqué précédemment, ChibiOS est (ironiquement) beaucoup plus grand que FreeRTOS en termes de fonctionnalités. Cela devient également évident lorsqu’il s’agit de simplement démarrer avec un nouveau projet ChibiOS. Alors que FreeRTOS, peut confortablement être juste le RTOS dans un HAL comme CMSIS-RTOS, ChibiOS a beaucoup de fonctionnalités qui ne sont pas couvertes par cette API. Pour cette raison, le projet ChibiOS a son propre IDE (basé sur Eclipse) sous la forme de ChibiStudio, qui est livré avec des projets de démonstration préinstallés.
Sur le site Play Embedded, un grand nombre de tutoriels et d’articles sur ChibiOS peuvent être trouvés, comme cet article d’introduction, qui couvre également la prise en main de ChibiStudio. La complexité de ChibiOS apparaît également dans les fichiers de configuration, qui comprennent:
chconf.h, pour configurer les options du noyau.halconf.h, pour configurer le HAL.mcuconf.h, contenant des informations relatives à la MCU spécifique ciblée.
L’exemple de projet ‘Blinky’ fourni avec le package de téléchargement ChibiOS pour le MCU STM32F042K6 (tel que trouvé sur la carte ST Nucleo-F042K6) donne un aperçu assez solide de ce à quoi ressemble un projet ChibiOS. A noter ici l’utilisation du module ChibiOS / HAL, qui permet d’utiliser le périphérique UART2, en utilisant le pilote série de ChibiOS.
ChibiOS prend en charge CMSIS-RTOS et propose également deux noyaux différents: le noyau RT (temps réel) et NIL, qui essaie simplement d’être le plus petit possible en termes de taille de code. Ce compromis ne semble pas trop affecter les performances si l’on en croit leurs benchmarks, ce qui en fait une option intéressante pour un nouveau projet de système d’exploitation intégré (RT).
Référence : premiers-pas-avec-freertos-et-chibios
PlotJuggler
Un des objectifs de cette asservissement pour robot autonome, est de pouvoir visualiser en temps réel un maximum de données calculées par les boucles de contrôles. Pour cela, nous utilisons la capacité du chip STM32F446RE de gérer un port USB pour transmettre les informations en temps réel. D’autre part, côté PC nous utilisons PlotJuggler pour visualiser nos données.
Pour comprendre l’utilisation de PlotJuggler, je vous conseille de regarder le cheatsheet: https://www.plotjuggler.io/cheatsheet
Pour récupérer les données de la carte d’asservissement, Jeff a développé un plugin PlotJuggler. Il vous faudra donc compiler le fork de PlotJuggler qui contient le plugin Asserv_stream, à savoir celui-ci: https://github.com/EsialRobotik/PlotJuggler
Un template est disponible ici afin de visualiser les données avec les onglets et les graphiques déjà prédéfinis (oui, en fait, ça ne change pas tant que cela ^^)
4. Architecture Asservissement (Motion System)
Venons-en au point primordial, le fonctionnement de l’asservissement ou motion system pour un robot autonome.
Ici, nous avons fait un choix contraignant dès le départ, il se retrouve dans l’architecture de l’asservissement:
Petite histoire…
Jusqu’à maintenant, nous (et pas mal d’autres équipes autant que je sache) utilisions une régulation basée sur 2 correcteurs uniquement, [Proportionnel + Dérivée], pour réguler en polaire (Si vous ne savez pas de quoi il s’agit, voir notre 1er article “asservissement et pilotage d’un robot autonome” sera fortement utile). Cela apportait effectivement une certaine facilité à l’implémentation et au réglage, mais il nous a toujours été très compliqué de contrôler correctement l’accélération et la décélération.
Globalement même en utilisant des quadRampDerivee d’Aversive, nous avons toujours eu des comportements “moyens” et des réglages qui ne fonctionnaient pas tout le temps. Comprenez par cela que les réglages étaient aussi différents en fonction de la vitesse du robot, et la gestion en devenait très compliquée. Nous souhaitions trouver une solution simplifiée et plus efficace sur ces problèmes rencontrés.
La solution
Donc Jeff a pris le parti de faire une régulation basée sur 2 correcteurs [Proportionnels] pour la régulation polaire. Ceux-ci donnent des consignes en vitesse aux 2 régulateurs [Proportionnel + Intégral] qui contrôlent les roues en vitesse.
Cette astuce a en effet pour conséquence de pouvoir contrôler finement la dérivée des consignes de vitesse, et par conséquent, contrôler l’accélération et la décélération, ce que nous souhaitions pour éviter tous les comportements hétéroclites précédemment cités.
Au départ, on pensait que cette solution ajouterait de la difficulté de réglage supplémentaire (2 paramètres PI à régler en plus), mais au final le bilan est plutôt nuancé. Certes, on doit régler 2 PI supplémentaires, mais le réglage est assez facile à faire, les différents réglages sont plutôt indépendants, et le réglage des 2 proportionnels est simplissime. Nous le verrons plus tard dans la partie “Comment régler son asserv?“.
5. La génération de trajectoire du robot
Trajectoire et garder le cap
L’algorithme fonctionne en 4 étapes :
- Tant que le robot n’est pas suffisamment aligné vers le point cible, on donne uniquement des consignes d’angle. Par exemple, si on donne une cible derrière nous, cela évite de faire un grand arc de cercle non maîtrisé sur la table.
- Calculer en temps réel une consigne d’angle et de distance en fonction de notre position actuelle par rapport à la cible.
- Une fois que le robot est en dessous d’une certaine distance, on ne calcule plus que des consignes d’angle. Sans cette étape, si on passe un millimètre à droite de la cible, le delta de distance sera de 1mm mais le delta d’angle sera de 90°, et donc le robot va se mettre à tourner autour de la cible, et ce n’est pas du tout un bon comportement du robot.
- Une fois que la distance par rapport au point cible est en dessous d’un seuil, on se considère comme arrivé, et on peut donc passer à la commande suivante.
6. La technique du Goto Enchainé
Le principe d’une commande GOTO est simple et utilisée par tout le monde en mode polaire, le robot se tourne de r radians, pour se mettre face à sa cible, puis avance tout droit jusqu’à la position cible. Il y a forcément des réglages de tolérance sur ces 2 mouvements (voir dans la partie réglage de l’asservissement).
A première vue, comme mentionné dans notre premier article, on pourrait croire qu’une trajectoire composée de ligne droite et d’arc de cercle est réalisable, mais lorsqu’on y réfléchit plus attentivement, on s’aperçoit que ce type de mouvement en clothoïde implique que les deux roues subissent pendant un bref instant une accélération infinie, ce qui est impossible. Une trajectoire qui n’assure pas la continuité des accélérations (pour la rotation et l’avancement) du robot est donc théoriquement irréalisable.
Donc il a fallu trouver une autre solution.
Le gain de temps à ne pas s’arrêter aux points de passage est, de notre point vu, très crucial. Les accélérations et décélérations successives pour s’arrêter pile au point de passage, voir s’ajuster sur les petites distances (dans le cas des PID qui ne sont pas très bien configurés) sont très fréquentes. En fait, le robot n’arrive jamais à la vitesse constante max du trapèze de génération de trajectoire moteur.
On arrive donc à la situation : accélération, décélération, arrêt. accélération, décélération, arrêt. accélération, décélération, arrêt. accélération, décélération, arrêt. accélération, décélération, arrêt. accélération, décélération, arrêt. accélération, décélération, arrêt. accélération, décélération, arrêt. accélération, décélération, arrêt. accélération, décélération, arrêt. accélération, décélération, arrêt….etc.
Et ça prend un temps foooouuuuu!
Petite histoire
La première version naïve de cet algorithme était de définir un cercle de rayon r_enchaine autour du robot.
Si la cible n’est pas dans le cercle, on génère une consigne vers l’intersection de la ligne du centre du robot vers la cible et le cercle de rayon r_enchaine.
Si la cible est dans le cercle, on génère une consigne vers la cible, ou on change de cible si la prochaine commande est enchaînable.
Le soucis étant que le rayon r_enchaine donne une vitesse constante au robot (cela génère donc une erreur de distance constante et le régulateur de distance est un simple proportionnel), et celle-ci est potentiellement en dessous de la vitesse max du robot.
Si on augmente le rayon de r_enchaine pour aller plus vite, on va passer à la consigne suivante plus tôt et passer plus loin des cibles. De plus, même si r_enchaine permet d’atteindre la vitesse max, le régulateur de vitesse tenterait d’aller à vitesse max même lors de virages serrés, ce qui n’est pas forcément une bonne chose; exemple, si le robot glisse sur le côté pendant un virage, on se décale, mais les roues codeuses ne peuvent pas le capter !
La solution
Pour cela, une 2ème version de cet algorithme a été testé à la coupe de robotique 2022. On définit 3 éléments:
- Un cercle centré sur le robot de rayon r_vMax = VitesseMax / Kp_distance permettant de générer une erreur de distance qui va entrainer une vitesse max en sortie de régulateur d’angle.
- Un cercle centré sur le robot de rayon r_next inférieur à r_vMax qui permet de décider de passer à la commande suivante si elle est enchaînable.
- Un angle ang_enchaine qui permet de passer à la commande suivante si elle est enchaînable tout en restant à vitesse max.
L’algorithme fonctionne de la même manière que précédemment avec r_enchaine = r_vMax. La principale différence tient dans les conditions d’enchainement de la cible. On utilise la prochaine commande enchaînable si une de ces conditions est vérifiées :
- Si la prochaine commande est dans le cercle de rayon r_vMax et que l’erreur de cap (aka deltaTheta) est inférieure à ang_enchaine
- Si la prochaine commande est dans le cercle de rayon r_next
Autrement dit, on enchaine la prochaine commande au loin si c’est presque tout droit, sinon on ralenti pour tourner.
TODO schéma <<
7. La technique de la détection du blocage
Comme nous utilisons des intégrales dans nos régulateurs de vitesse, une détection automatique du blocage est assez importante ! Si le robot autonome cogne dans quelque chose, ou se bloque, cet asservissement doit le détecter et le notifier.
Un robot bloqué qui n’arrive pas à sa consigne, finira inexorablement par mettre la puissance de ses moteurs à fond, fera un trou dans la table, ou cramera un moteur :-/
Et même s’il n’y a pas de fumée, le robot “attendra” à l’infini d’arriver à cette position inatteignable.
La petite histoire
Cette année, on a réfléchi à une solution qui semblait élégante : calculer l’intégrale glissante de l’erreur de la consigne de distance. Et si cette intégrale dépasse un seuil, on considérerait que l’on est bloqué.
De loin, ça semblait bien, sauf que cela utilise énormément de RAM (intégrale glissante) et le seuil, ainsi que l’horizon de l’intégrale est quasi-impossible à régler car c’est finalement dépendant aussi du type de consigne. Les résultats n’étaient donc pas concluants.
La solution
Nous avons déterré une solution cryptique et qui s’apparente à du bricolage, mais qui a le bon goût de fonctionner 🙂
L’idée étant qu’un blocage comporte les « caractéristiques » suivantes :
- Au moins un des moteurs doit tourner à plus de x% de sa puissance (nous avions souvent mis 10%)
- Si le robot est en train de tourner, la différence d’angle par rapport au dernier tour de boucle d’asservissement doit être supérieure à une valeur block_angle_speed_threshold
- Si le robot est en train d’avancer, la différence de distance par rapport au dernier tour de boucle d’asservissement doit être supérieure à une valeur block_dist_speed_threshold
Si toutes ces conditions sont réunies, on incrémente un compteur de la durée de la boucle d’asservissement, et sinon on remet à zéro ce compteur.
Si ce compteur dépasse un certain temps blocking_detected_duration_threshold, le blocage est détecté et peut être notifié.
8. La notification des données
Concernant la transmission des données, tout ce passe dans le main.cpp. Cela signifie que chacun peut choisir son implémentation à sa convenance.
Dans notre cas, nous avons un thread asservPositionSerial (séparé dans une classe raspIO.cpp) qui tourne en parallèle et qui informe la carte linux principale de commande toutes les 100ms, avec les informations intéressantes à remonter au niveau supérieur, à savoir :
- la position odométrique (x,y,theta),
- le status de l’asservissement (BLOCKED, HALTED, IDLE, RUNNING),
- s’il y a encore des commandes en cours,
- et la vitesse des roues droite et gauche.
Pour chaque instance de l’asservissement (donc chaque robot), il est possible de faire différemment et d’implémenter sa propre communication adéquate.
9. Le shell de debug
Afin de débuguer, chaque instance de robot (main.cpp) peut ou non utiliser le debug USB en ligne de commande asservCommandUSB. C’est aussi un Thread à activer en plus sur son implémentation.
Dès que la carte est branchée en USB serial, L’outputStream est présent et des commandes directes sont disponibles du type “asserv enablemotor 0|1” ou “asserv adddist [x]” afin de faire rouler le robot de [x]mm.
