PCB Quadrotor (Brushless) (15 / 20 étapes)

Étape 15 : IMU partie 2: filtre complémentaire

Ma méthode de choix pour combiner les signaux accéléromètre et gyroscope dans une estimation d’angle propre et stable est le filtre complémentaire. C’est par le calcul efficace et il aborde directement les accélération horizontale transitoire et à long terme dérive de compas gyroscopique. L’idée est d’utiliser le gyroscope à court terme angle estime, par intégration numérique, mais le passage à l’accéléromètre pour des prévisions à long terme, en faisant la moyenne. Pour ce faire d’une manière continue, filtres passe-bas et passe-haut sont utilisés. C’est identique à la méthode j’ai utilisé dans le Seg... bâton Instructable, étape 10.

L’image montre l’un filtre complémentaire sous forme de diagramme de bloc. Les signaux de capteur, converties en degrés et degrés par seconde, sont les entrées. L’angle de l’accéléromètre est passe-bas filtré, réduire l’influence des signaux de courte durée, mais maintient une moyenne à long terme (de gravité).  Pour obtenir une estimation de l’angle et puis passe-haut filtré, pour supprimer toute dérive à long terme, la vitesse angulaire du gyroscope est tout d’abord intégrée. Les deux signaux filtrés sont additionnés pour créer une estimation de l’angle unique qui combine les meilleurs éléments des deux signaux de capteur.

Il est l’un des rares cas où le code est plus concis que le schéma ou l’explication écrite :

Extrait de pd_digitalIMU.pde dans 4pcb_ARD :
// -------------------------------------------------------------------
rate_pitch = (float) g_pitch * G_GAIN ;
angle_pitch = AA * (angle_pitch + rate_pitch * DT) ;
angle_pitch += (1,0 - AA) * (float) a_pitch * A_GAIN ;
// -------------------------------------------------------------------

La première ligne convertit le signal brut gyro, g_pitch, º / s, comme indiqué dans l’étape précédente. La fin de la deuxième ligne est l’intégration numérique. DT est la boucle du temps, 0.01s. Un petit changement dans l’angle, rate_pitch * DT, est ajouté à l’estimation précédente d’angle.

Le reste de la deuxième ligne et de l’ensemble de la troisième ligne forment les filtres passe-haut et passe-bas, sous une forme discrète. Passe-haut agit sur l’estimation de l’angle intégré et passe-bas agit sur le signal de l’accéléromètre, a_pitch. La constante AA détermine l’échelle de temps des filtres passe-haut et bas et est une valeur que vous pouvez modifier selon vos quadrotor. Plus AA fiducies le signal de gyroscope intégré pour plus longtemps. Plus courte AA fusionne le signal de l’accéléromètre en plus vite.

La valeur AA utilisé dans 4pcb correspond avec une constante de temps de 1 s. Il s’agit de la durée approximative de la durée pendant laquelle l’estimation d’angle gyro est fiable et la durée approximative de temps pendant laquelle le signal d’accerometer est en moyenne. Puisqu’il s’agit de réponse impulsionnelle infinie (IIR) les filtres, la transition est lisse et continue et ne représente que la constante de temps où la pondération des deux filtres est égale.

J’ai écrit une explication plus détaillée du filtre complémentaire tel qu’appliqué à équilibrage automatique système dans ce livre blanc. Elle couvre certains détails de comment AA est liée à la constante de temps des filtres passe-bas et passe-haut, comment noter les biais affecte capteur le filtre et plus de détails.