PCB Quadrotor (borstlösa) (15 / 20 steg)

Steg 15: IMU del 2: kompletterande Filter

Min metod för val av kombinera accelerometer och gyro signalerna till en ren och stabil vinkel uppskattning är den kompletterande Filter. Det är beräkningsmässigt effektiv och det behandlar direkt både övergående horisontell acceleration och långsiktiga gyro drift. Tanken är att använda gyrot för kort sikt vinkel uppskattar, numerisk integration, men växla till accelerometer för långsiktiga beräkningar, som genomsnittet. För att göra detta i ett fortlöpande långt, används lågpass och högpass filter. Detta är identisk med metoden jag använde i Seg... stick Instructable, steg 10.

Bilden visar den ett kompletterande Filter i block diagram form. Sensorn signaler, omvandlas till grader och grader per sekund, är ingångarna. Accelerometer vinkeln är låg-pass filtrerad, minska påverkan av kortvariga signaler men att upprätthålla en långtidsmedelvärdet (från allvar). Den gyro vinkelformiga hastigheten är första integrerat, för att få en uppskattning av vinkel och sedan hög-pass filtrerad, för att ta bort någon långsiktig drift. De två filtrerade signalerna summeras ihop för att skapa en enda vinkel uppskattning som kombinerar de bästa delarna av både sensorn signaler.

Det är en av de få fall där koden är kortare än blockdiagram eller den skriftliga förklaringen:

Utdrag från pd_digitalIMU.pde i 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;
// -------------------------------------------------------------------

Den första raden omvandlar signalen raw gyro, g_pitch, i º / s, som diskuterades i föregående steg. I slutet av den andra raden är numerisk integration. DT är slingan tid, 0.01s. En liten förändring i vinkel, rate_pitch * DT, läggas till den föregående vinkel uppskattningen.

Resten av den andra raden och alla tredje raden bildar de hög-pass och låg-pass filter, i diskreta form. Hög-pass fungerar på den integrerade vinkel uppskattningen och det low-passet på signalen accelerometer, a_pitch. Konstanten AA bestämmer tidsskalan av high - och low-pass filtrerar, och är ett värde som du kan justera beroende på din quadrotor. Större AA förtroende integrerad gyro signalen för längre. Kortare AA sammanslagningar accelerometer signalen i snabbare.

Värdet för AA används i 4pcb motsvarar en tidskonstant av 1s. Detta är ungefärliga längden av tid som gyro vinkel uppskattningen är betrodd, och den ungefärliga tid som accerometer signalen är genomsnitt. Eftersom dessa är oändlig-impulssvar (IIR) filter, övergången är jämna och kontinuerliga och tidskonstanten representerar bara där viktningen av de två filtren är lika.

Jag skrev upp en mer grundlig förklaring av kompletterande Filter som tillämpas för Self balancing system i denna vitbok. Det omfattar vissa detaljer av hur AA är relaterad till tidskonstanten lågpass och högpass filter, hur betygsätta sensor partiskhet påverkar filtret, och lite mer information.