En automatique, on appelle “observateur” la partie de logiciel qui permet d’évaluer l’état du système qu’on essaye d’asservir. Pour mon drone, l’observateur calcule les 3 angles d’Euler et la position 3D dans l’espace. Il y a donc 6 composantes à recalculer. C’est de loin la partie la plus gourmande en code et en ressource. Cette page décrit l'intégralité de l'observateur embarqué dans le drone.
1.1) Echantillonnage:
Les calculs (observateur et contrôleur) tournent 20 fois par seconde, il n'y a pas besoin de plus. Mais j'ai volontairement poussé l’échantillonnage des gyromètres et accéléromètres à 120 fois par secondes. L’objectif est d’exploiter au maximum les informations des capteurs. C’est très important si le signal est bruité. Ca équivaut à filtrer les informations issues des capteurs.
Pour les gyromètres, comme l'information "vitesse de rotation" qu'ils délivrent est utilisée dans les algorithmes, il faut appliquer un filtre passe bas. La fréquence du filtre correspond à peu près à la réccurence de 20Hz des calculs.
Pour les accéléromètres, comme les informations "accélération" qu'ils délivrent ne sont PAS utilisées par les algorithmes (les contrôleurs PID utilisent la vitesse et la position), le filtre n'est pas nécessaire. Du coup, la seule opération réalisée 120 fois par secondes est l'intégrale mathématique "simplifiée" de l'accélération qui servira à renseigner les vitesses de déplacement 3D dans les calculs. L'opération "intégrale mathématique" est une simple addition, donc moins gourmande en temps de calcul que le filtre passe bas.
1.2) orientation 3D à partir des gyros :
Le calcul consiste à faire une intégrale mathématique des informations issues des gyro. Là où ça se complique, c’est qu’il faut faire ça en 3D, où une rotation simple peut avoir un impact sur les 3 angles en même temps. Pour obtenir les angles d’Euler, je m’inspire d’algorithmes « DCM » (direction cosine matrix). J’utilise des formules simplifiées, pour gagner du temps de calcul. En effet, mon drone ne dépassera jamais 35° d’inclinaison en vol, ce qui permet d’ignorer certains termes du calcul.
J’en déduis les angles suivants :
angle de lacet (orientation à plat)
angle de roulis (inclinaison sur le côté)
angle de tangage (inclinaison avant-arrière)
Pour les angles de tangage et de roulis, comme le drone est normalement en moyenne à l'horizontale, j'applique un filtre passe haut avec un temps de 20 secondes. Ca évite que les angles divergent. C'est nécessaire car ces 2 angles (tangage et roulis) sont les seules données parmis les 6 données (3 angles d'euler et 3 positions) qui ne peuvent pas être recalées avec d'autres capteurs (sonar, caméra wii, boussole). Lorsque les servos sont à des positions élevées qui inclinent volontairement l'hélicoptère, ce filtre est temporairement désactivé.
1.3) Orientation 3D - la boussole:
J’avais tout d’abord essayé d’utiliser une boussole 2D. Je n’ai jamais réussi à interpréter correctement, même si c’est en théorie faisable. La tâche est beaucoup plus simple avec une boussole 3D, qui mesure les 3 composantes (x, y, et z) du champ magnétique.
Le calcul consiste à réaliser une estimation de l’angle de lacet. On fait converger lentement l’angle de lacet de l’observateur vers l’angle de lacet estimé à partir de la boussole. Donc les gyro renseignent la partie "haute fréquence" de l’information angle de lacet, et la boussole 3D renseigne la partie "basse fréquence".
Maintenant, comment fait-on pour estimer l’angle de lacet avec les informations de la boussole ? On utilise pour cela le vecteur « champ magnétique », ainsi que les angles de roulis et de tangage calculés à l’étape précédente. En fait, il faut appliquer une rotation "inverse" au vecteur champ magnétique. L’objectif est d’obtenir un vecteur champ magnétique "à plat", qui correspond au vecteur lu par la boussole 3D si le drone était à plat et non incliné. En fait, seules les composantes longitudinales et latérales doivent être calculées. A partir de là, il est très simple de déduire l’angle de lacet, comme on le ferait avec une boussole 2D posée bien à plat.
1.4) Position 3D - Accéléromètres :
Là, c’est compliqué, et ça ne fonctionne pas très bien. Sans doute du fait de la piètre qualité des capteurs, et du bruit. En théorie, c’est assez simple ; on connaît déjà l’orientation 3D, il faut reconstruire la position, ou plutôt le déplacement 3D.
On prend le vecteur accélération 3D mesuré dans le repère du drone. On y applique les 3 rotations roulis tangage lacet pour remettre ce vecteur dans le repère absolu. Puis on y soustrait la gravité (une constante selon l’axe vertical). On a alors le vecteur correspondant en théorie à l’accélération 3D, au sens "dérivée de la vitesse".
Compensation d’offset.
Ce vecteur est malheureusement assez erroné, avec une erreur d’offset qui varie dans le temps. Pour corriger en temps réel cet offset, je calcule le vecteur gravité 3D (estimé) dans le repère du drone. C’est réalisé uniquement à l’aide des informations des angles de roulis et de tangage. Après, pour chacun des accéléromètres x, y, et z, on considère que la moyenne temporelle de la différence [accélération mesurée – gravité selon l’axe] doit être nulle. Malheureusement, cette partie fonctionne assez mal, c’est vraiment difficile à réaliser.
Calcul de la vitesse et de la position :
En intégrant l’accélération selon chacun des axes, on en déduit la vitesse.
En intégrant la vitesse selon chacun des axes, on en déduit la position absolue.
Malheureusement, ces informations position et vitesse dérivent très vite. Dès la moindre petite erreur de mesure, l’erreur est amplifiée, car elle passe au travers d’une intégrale double. En moins d’une seconde, l’erreur est déjà très importante. Ce sont donc les autres capteurs (sonars et caméra wii) qui vont permettre de recaler en temps réel.
1.5) Position 3D – Sonars :
Le drone possède 4 sonars répartis à 90° les uns des autres. Malheureusement, à cause de limitations incompréhensibles de la carte Embedded Master (impossibilité d’ouvrir 2 connexions I2C en même temps), je ne peux mesurer chaque direction que toutes les 100ms. Je réalise 2 mesures sonars en même temps, en m’assurant bien de ne pas mesurer des sonars parallèle en même temps (le droite et le gauche) pour ne pas qu’ils se perturbent mutuellement.
L’objectif est de mesurer la distance entre le drone et un plan connu (mur, plancher). Tous les plans de référence sont connus, donc la géométrie de la pièce dans laquelle évolue le drone est connue à l’avance. On ne retient une mesure sonar que si le sonar est perpendiculaire par rapport à un des plans connus, avec une tolérance de 18°. Le sonar déterminant l’altitude, lui, est tout le temps retenu, car les angles de roulis et tangage dépassent rarement 20° en vol.
Si une mesure sonar est retenue (critère d’orientation), alors on calcule la distance "projetée" entre le plan (mur, plancher) et le drone à l’aide de l’orientation 3D et de la mesure. Cette mesure va permettre de recaler le drone dans une seule des 3 directions X, Y ou Z. Mais il y a un autre critère pour que cette mesure soit retenue : que la position et la vitesse qui en est déduite soit crédible.
Le recalage :
On recale 2 choses :
L’estimation de position
L’estimation de vitesse
L’estimation de vitesse est recalée en fonction de la « vitesse sonar ». La vitesse sonar est la vitesse calculée entre 2 mesures sonars réalisées avec le même sonar et le même plan, pourvu que ces 2 mesures soient consécutives (réalisées à 100ms d’intervalle).
Enfin, que ce soit pour la position ou pour la vitesse, on ne recale pas l’estimation à 100% à chaque fois. En effet, les mesures sonars peuvent être bruitées, et un petit obstacle sur le mur peut faire croire que le drone se déplace (vitesse erronée). Donc on recale la position de 50% à chaque fois, et la vitesse de 30% à chaque fois.
Encore une fois, on a donc 2 type de capteurs pour renseigner une seule et même information (la position 3D X, Y, Z) :
Les accéléromètres renseignent la partie "haute fréquence" de la position 3D
Les sonars renseignent la partie "basse fréquence" de la position 3D
1.6) Position 3D - Caméra Wii Remote :
La caméra Wii est un capteur génial. Contrairement aux sonars, elle permet d’actualiser simultanément 2 axes de mesure. Mes algos ne tournent que 20 fois par secondes, mais cette caméra peut très bien monter à 100Hz !
A partir de la mesure d’un spot (coordonnées 2D dans l’image), de l’altitude, et de l’orientation 3D estimée, on peut recaler simultanément la position du drone dans les 2 directions X et Y (pas Z l’altitude).
La position de tous les spots est connue à l’avance, exactement comme pour la position des murs avec les sonars. Un nouveau spot n’est prix en compte dans le champ de vision que si sa position est crédible (à moins de 20cm d’un spot théorique) et que ce spot est vu pendant que la position 3D du drone est confirmée par d’autres informations (sonar ou autre spot).
La caméra Wii est capable de mesurer jusqu’à 4 spots simultanément. Chaque spot renvoyé par la caméra (entre 0 et 4 spots) est ainsi comparé à un spot théorique, pour déterminer s’il peut être utilisé. On élimine donc les mesures parasites.
Le recalage suit exactement les mêmes règles que pour les sonars : on ne retient que les mesures crédibles, et on ne recale pas de 100%.
1.7) Détection de "posé au sol" :
En plus de tout ça, j’ai programmé une détection de posé au sol. Si la puissance des moteurs est suffisamment faible, ET que l’altitude estimée est faible, c’est qu’on est posé au sol, et en théorie bien à plat. C’est une bonne source d’information, très fiable. Tant que le drone est posé au sol, j’en profite pour réaliser les opérations suivantes:
Compenser les offsets de tous les capteurs accéléro et gyro : le drone ne bouge pas, c’est le moment idéal pour refaire le « zéro » des capteurs
Remettre rapidement les angles de roulis et tangage à Zero
