Filtre complémentaire : fusionner accéléromètre et gyroscope

Introduction

Vous disposez d’un accéléromètre juste en moyenne mais faux à l’instant présent, et d’un gyroscope juste à l’instant présent mais faux sur la durée. Le filtre complémentaire part du constat que ces deux profils d’erreur sont opposés, et qu’on peut donc prendre la bonne moitié de chacun.

C’est une ligne d’arithmétique. C’est aussi, pour un robot équilibriste, presque toujours la bonne réponse — un filtre de Kalman apporte un petit gain de précision pour un grand coût en complexité et en temps CPU sur un microcontrôleur 8 bits.

L’idée

Chaque capteur est fiable dans une bande de fréquences différente.

• L’accéléromètre est fiable en basse fréquence. Moyenné sur une seconde, il indique correctement où se trouve le bas ; échantillonné sur 10 ms, il mesure surtout le mouvement propre du robot.
• Le gyroscope est fiable en haute fréquence. Sur 10 ms, sa vitesse intégrée est pratiquement exacte ; sur une minute, le biais accumulé l’a ruinée.

Donc : filtre passe-bas sur l’accéléromètre, passe-haut sur le gyroscope, puis addition. Les filtres sont conçus pour que leurs réponses somment exactement à un sur toutes les fréquences — ils se complètent, d’où le nom. Aucune bande n’est comptée deux fois ni oubliée.

L’implémentation

const float ALPHA = 0.98;
float angle = 0;

void updateAngle(float accelAngle, float gyroRate, float dt) {
  angle = ALPHA * (angle + gyroRate * dt) + (1.0 - ALPHA) * accelAngle;
}

C’est tout le filtre. Lisez-le ainsi : prenez la position indiquée par le gyroscope depuis la dernière fois, et rapprochez-la de 2 % vers ce qu’annonce actuellement l’accéléromètre.

Le terme gyroscopique apporte la réactivité. Le petit terme accélérométrique apporte une correction lente et permanente qui empêche la dérive du gyroscope de s’accumuler — il ramène constamment l’estimation vers la vérité physique, assez doucement pour que les pics d’accélération transitoires la déplacent à peine.

Choisir la constante de temps

ALPHA n’est pas un réglage arbitraire. Il correspond à une constante de temps :

τ = (ALPHA · dt) / (1 − ALPHA)

τ est la frontière entre les bandes. Les variations plus rapides que τ viennent du gyroscope ; plus lentes que τ, l’accéléromètre gagne. Avec une boucle de 5 ms et ALPHA = 0.98, τ = 0,245 s — environ un quart de seconde d’autorité au gyroscope avant que l’accéléromètre ne reprenne le dessus.

Notez ce qu’implique la formule : ALPHA dépend de votre fréquence de boucle. Copiez une valeur issue d’un projet à 100 Hz dans votre boucle à 500 Hz et vous avez rendu votre filtre cinq fois plus lent sans le savoir. Si la temporisation de votre boucle change, recalculez ALPHA à partir du τ réellement voulu plutôt que de garder le nombre.

La temporisation doit être réelle

dt doit être le temps réellement écoulé, pas celui prévu :

unsigned long now = micros();
float dt = (now - lastUpdate) / 1000000.0;
lastUpdate = now;

Coder dt = 0.005 en dur parce que vous avez appelé delay(5) est un raccourci courant qui empoisonne discrètement le résultat — la seule lecture I2C prend près d’une milliseconde, votre boucle réelle dure donc 6 ms et chaque intégration est 17 % trop courte. L’angle dérive alors d’une façon qui ressemble exactement à un biais du gyroscope et qu’aucun étalonnage ne corrige.

Mieux encore, cadencez la boucle par une minuterie matérielle à fréquence fixe plutôt que par delay(). Une boucle d’équilibrage a bien plus besoin d’une temporisation régulière que d’une fréquence particulière.

Bien régler les signes

La panne la plus fréquente est une incohérence de signe entre les deux sources. Si l’accéléromètre annonce +3° pour une inclinaison avant et que le gyroscope annonce une vitesse négative pour le même mouvement, les deux termes se battent : la sortie du filtre traîne, oscille et se stabilise à la mauvaise valeur, et aucun réglage de ALPHA n’y changera rien.

Vérifiez directement. Affichez accelAngle et l’intégrale courante de gyroRate côte à côte, puis inclinez lentement le robot vers l’avant à la main :

• Les deux doivent évoluer dans la même direction.
• Les deux doivent atteindre à peu près la même amplitude.

S’ils évoluent en sens opposé, inversez le signe de la vitesse du gyroscope. S’ils diffèrent de 90°, vous lisez les mauvais axes de l’accéléromètre pour le plan de chute de votre robot.

Pourquoi pas un filtre de Kalman ?

Un filtre de Kalman est la réponse statistiquement optimale si vous connaissez les caractéristiques de bruit de vos capteurs — et il fonctionne légèrement mieux, en particulier pendant une accélération soutenue.

Pour un robot équilibriste, la différence justifie rarement son coût. Le filtre complémentaire coûte une multiplication-addition, possède un seul paramètre réglable au sens physique clair, et tombe en panne de façons qu’on diagnostique en affichant deux nombres. Un filtre de Kalman coûte une arithmétique matricielle à chaque cycle, possède une matrice de covariance à régler, et quand il se comporte mal la raison est vraiment difficile à voir.

Faites d’abord tenir le robot en équilibre sur un filtre complémentaire. S’il tient bien, vous avez votre réponse. Il existe un traitement plus large des compromis dans bruit, filtrage et fusion des capteurs.

Vérifiez avant de boucler

Deux contrôles attrapent presque tout, et prennent chacun une minute :

Tenez-le immobile pendant deux minutes. L’angle filtré doit rester fixe. S’il dérive, votre étalonnage du biais du gyroscope est faux — le terme accélérométrique devrait justement empêcher cela, donc une dérive signifie que ALPHA est trop élevé pour votre biais.

Secouez-le sans le tourner. Déplacez le robot latéralement en le gardant vertical. L’accéléromètre seul oscillera fortement ; l’angle filtré devrait à peine bouger. Si l’angle filtré oscille aussi, ALPHA est trop faible et vous faites confiance à l’accéléromètre pendant une accélération — précisément la situation où il est le plus mauvais.

Une fois que l’angle reste fixe à l’arrêt et reste honnête quand on le secoue, il est assez bon pour tenir en équilibre. Fermez la boucle dans commande d’équilibre PID en cascade, ou observez une boucle réglée dans le simulateur d’équilibre. La même forme de filtre — un signal qui dérive mais reste lisse, corrigé par un autre bruité mais borné — permet à un robot d’extérieur de transformer une position GPS à 1 Hz en position utilisable.