IMU w dronie — Drony

IMU jest podstawowym źródłem informacji o szybkim ruchu bezzałogowego statku powietrznego. Bez jego danych kontroler nie wie, że podmuch rozpoczął przechylenie, silnik wywołał obrót wokół osi yaw albo skrzydło weszło w krótkotrwałe przeciążenie. Sam układ scalony nie podaje jednak gotowej orientacji. Dostarcza próbki prędkości kątowej i siły właściwej, obciążone biasem, szumem, błędem skali, wpływem temperatury, drganiami oraz niepewnością czasu. Dopiero poprawnie zaprojektowany tor pomiarowy zamienia je w dane użyteczne dla estymatora i regulatora.

W praktyce jakość IMU jest właściwością całego systemu: struktury MEMS, ustawień wewnętrznych filtrów, płytki drukowanej, zasilania, mocowania kontrolera, sterownika programowego, synchronizacji zegarów i mechaniki statku. Droższy sensor na źle zaprojektowanej płytce może dawać gorszy wynik niż układ konsumencki zamontowany i obsługiwany poprawnie.

IMU, AHRS i INS — trzy różne pojęcia#

IMU, czyli inertial measurement unit, jest jednostką pomiarową. W typowym kontrolerze lotu zawiera trójosiowy żyroskop i trójosiowy akcelerometr, dlatego mówi się o sześciu stopniach pomiaru albo „6-DoF”. Określenie „9-osiowy IMU” zwykle oznacza dołączenie trójosiowego magnetometru. Nie są to jednak trzy dodatkowe osie ruchu; magnetometr mierzy pole magnetyczne i z punktu widzenia architektury autopilota jest osobnym, wolniejszym sensorem środowiskowym.

AHRS (attitude and heading reference system) łączy pomiary inercyjne z obserwacjami grawitacji, pola magnetycznego lub innymi źródłami kursu i wyznacza orientację. INS (inertial navigation system) idzie dalej: propaguje również prędkość i pozycję. W małym UAV granice implementacyjne mogą być ukryte w jednym module firmware, lecz warto zachować rozróżnienie:

ruch statku
    ↓
żyroskop + akcelerometr (IMU)
    ↓ próbki, timestamp, status
estymator orientacji / AHRS
    ↓ attitude, bias, jakość
estymator nawigacyjny / INS + GNSS, barometr, wizja...
    ↓ pozycja, prędkość, orientacja
regulatory lotu

IMU nie jest zatem „elektronicznym poziomem”. Bez dodatkowych założeń nie zna bezwzględnego kursu, pozycji ani wysokości. Przez krótki czas bardzo dobrze opisuje zmianę ruchu, ale całkowanie każdego błędu powoduje dryf.

Co naprawdę mierzy żyroskop#

Żyroskop raportuje wektor prędkości kątowej sensora:

ωm = [ωx, ωy, ωz]ᵀ

Jednostką SI jest rad/s, chociaż datasheety często używają °/s. Nieruchomy idealny żyroskop wskazuje zero niezależnie od orientacji. Rzeczywisty pokazuje sumę prawdziwego ruchu, biasu i szumu. Aby otrzymać zmianę orientacji, firmware całkuje prędkość kątową w czasie. W jednej osi uproszczona zależność ma postać:

θ[k+1] = θ[k] + (ωm[k] - bω[k]) · Δt

W trzech wymiarach nie należy niezależnie całkować roll, pitch i yaw. Kolejność obrotów ma znaczenie, dlatego estymator aktualizuje macierz obrotu albo kwaternion. Dokładny czas Δt jest równie istotny jak wartość próbki. Błąd biasu 0,1°/s daje przy naiwnej integracji około 6° błędu po minucie, natomiast błąd zegara skaluje każdy zmierzony obrót.

Żyroskop jest podstawą szybkiej wewnętrznej pętli regulacji. Regulator prędkości kątowej porównuje żądane ω z pomiarem i koryguje moment wytwarzany przez silniki lub powierzchnie sterowe. Opóźnienie oraz szum tego toru bezpośrednio ograniczają możliwe pasmo pętli. Fizykę i parametry samego przetwornika szerzej omawia artykuł Żyroskop MEMS w UAV.

Co naprawdę mierzy akcelerometr#

Akcelerometr nie mierzy bezpośrednio „przyspieszenia pojazdu względem ziemi”. Mierzy siłę właściwą, czyli reakcję niegrawitacyjnych sił działających na masę czujnika. Uproszczony model w układzie sensora można zapisać:

fm = Rᵀ(a - g) + ba + na

gdzie a jest przyspieszeniem translacyjnym w układzie nawigacyjnym, g wektorem grawitacji, R orientacją, ba biasem, a na szumem. Zależnie od przyjętej konwencji znak może być zapisany inaczej, dlatego interfejs danych musi jawnie określać ramkę i znaki.

Sensor nieruchomy na stole wskazuje normę około 1 g, ponieważ podłoże wywiera na niego siłę podtrzymującą. W idealnym swobodnym spadku wskazanie zbliża się do zera. Podczas dynamicznego lotu pomiar zawiera jednocześnie informację o grawitacji i ruchu. Stwierdzenie „akcelerometr pokazuje, gdzie jest dół” jest poprawne tylko po uśrednieniu w warunkach, w których przyspieszenie translacyjne jest małe albo estymator potrafi je modelować.

Po obróceniu siły właściwej do układu nawigacyjnego i dodaniu grawitacji można wyznaczyć przyspieszenie, następnie całkować je do prędkości i pozycji. Bias rzędu zaledwie 10 mg odpowiada około 0,098 m/s²; bez korekcji powoduje błąd prędkości bliski 1 m/s po dziesięciu sekundach i błąd położenia rosnący kwadratowo z czasem. To jedna z przyczyn, dla których mały MEMS INS nie zastępuje długookresowo GNSS, systemu wizyjnego czy innych obserwacji zewnętrznych. Osobny artykuł Akcelerometr MEMS w UAV rozwija fizykę tego pomiaru.

Układy współrzędnych i znak pomiaru#

Surowe osie układu scalonego rzadko są od razu osiami bryły UAV. Trzeba rozróżnić co najmniej:

układ struktury MEMS zdefiniowany w datasheetcie;
układ obudowy i oznaczenia pinu 1;
układ płytki kontrolera;
układ bryły statku;
układ nawigacyjny, na przykład NED albo ENU.

Transformacja stała sensor → body powinna być zapisana jawnie jako macierz obrotu lub element skończonego zbioru dozwolonych orientacji. Nie należy rozrzucać zamian osi i negacji po sterowniku, estymatorze i loggerze. Taki kod może wyglądać poprawnie dla jednego montażu, a po obróceniu kontrolera o 90° utworzyć dodatnie sprzężenie zwrotne.

Minimalny test osi wykonuje się bez śmigieł. Ręczny dodatni obrót wokół osi bryły musi dać dodatnią prędkość w tej samej osi po transformacji. Następnie sprawdza się wskazanie grawitacji w sześciu orientacjach oraz ruch złożony, ponieważ trzy testy jednoosiowe nie wykrywają każdej zamiany kolejności transformacji. Konwencje ROS REP 145 przypominają dodatkowo, że sterownik IMU powinien publikować dane zgodnie z ramką sensora, a transformacja statyczna ma opisywać jego montaż; autopilot może stosować własne konwencje i wymaga oddzielnego mapowania. Więcej przykładów zawiera artykuł Układy współrzędnych w UAV.

Model błędu sześciu osi#

Użyteczny model pomiaru powinien być wektorowy. Dla żyroskopu:

ωm = Mω · ω + bω(T,t) + nω + rvω

a dla akcelerometru:

fm = Ma · f + ba(T,t) + na + rva

Macierze M obejmują błąd skali, nieortogonalność i sprzężenia osi. b(T,t) opisuje bias zależny od temperatury i czasu, n szum krótkookresowy, a rv dodatkowy błąd wywołany drganiami i nieliniowością. W pełnym modelu dochodzą kwantyzacja, opóźnienie, jitter, clipping, histereza termiczna i zmiana parametrów po lutowaniu.

Składnik błędu	Typowy skutek w UAV	Jak go ujawnić
bias żyroskopu	narastający błąd orientacji, stała korekcja regulatora	średnia nieruchomego, rozgrzanego sensora
bias akcelerometru	dryf prędkości i pozycji, zły wektor grawitacji	kalibracja wielopozycyjna i porównanie normy
szum biały	jitter attitude i wejścia PID, wzrost sygnału D	PSD, odchylenie standardowe, noise density
random walk / wolny dryf	zmiana najlepszego oszacowania z czasem	odchylenie Allana z długiego zapisu
błąd skali	proporcjonalnie zły obrót albo przyspieszenie	stół obrotowy, ±g i wzorce wielopozycyjne
nieortogonalność	przeciek ruchu jednej osi do pozostałych	obrót wokół wzorcowej osi, kalibracja macierzowa
zależność od temperatury	zmiana biasu od startu do lotu	kontrolowany cykl grzania i chłodzenia
vibration rectification	pozorny bias podczas drgań	shaker lub sweep RPM, porównanie spoczynku i napędu
clipping	utrata rzeczywistych szczytów ruchu	licznik saturacji i surowe kody ADC
błąd czasu	zła integracja mimo poprawnych wartości	capture timestamp, analiza `Δt` i latency

Jedna liczba „dokładności IMU” nie opisuje tych zjawisk. Dwa układy mogą mieć podobną gęstość szumu, ale różną stabilność biasu, odporność na drgania albo opóźnienie filtra. Do regulatora małego multirotora ważne są pasmo i odporność na napęd; do mapowania i nawigacji długookresowej większą wagę mogą mieć stabilność temperaturowa, synchronizacja oraz powtarzalność skali.

Zakres pomiarowy, rozdzielczość i clipping#

Zakres pełnoskalowy określa największą wartość, którą sensor może zakodować bez nasycenia. Popularne IMU pozwalają wybierać zakresy żyroskopu do około ±2000°/s i akcelerometru do ±16 g; dokładne opcje zależą od modelu. Mniejszy zakres daje więcej kodów na jednostkę fizyczną, lecz mniejszy margines na gwałtowny manewr, twarde lądowanie i drgania.

Dobór wykonuje się na podstawie dynamiki platformy i logów, nie według zasady „najmniejszy zakres jest najdokładniejszy”. Dla żyroskopu margines musi obejmować najwyższą zamierzoną prędkość obrotu oraz krótkie zaburzenia. Dla akcelerometru istotne są manewry, uderzenia i lokalne przyspieszenia wywołane odsunięciem sensora od środka obrotu. Sensor zamontowany daleko od środka masy mierzy dodatkowe składowe styczne i dośrodkowe.

Clipping jest utratą informacji, której filtr później nie odtworzy. Odcinek kolejnych próbek równych wartości maksymalnej nie opisuje rzeczywistego kształtu impulsu. Estymator powinien otrzymać flagę saturacji, a logger liczniki per oś. Jeśli wiadomo, że zakres zmieniono w locie, rekord danych musi zawierać generację konfiguracji lub już przeliczoną wartość SI z informacją o progu clippingu.

Liczba bitów wyjścia nie jest dokładnością. Jeżeli szum zajmuje kilka najmłodszych bitów, nominalne 16 albo 20 bitów nie oznacza tylu bitów informacji. Z drugiej strony nadmiernie mały zakres może zmniejszyć kwantyzację, ale prowadzić do częstego nasycenia — znacznie groźniejszego niż dodatkowy szum pojedynczego LSB.

ODR, pasmo i częstotliwość pętli#

Trzy częstotliwości bywają błędnie utożsamiane:

ODR sensora — tempo tworzenia nowych próbek wewnątrz IMU;
częstotliwość odczytu — tempo transferów przez SPI albo opróżniania FIFO;
częstotliwość estymatora lub PID — tempo wykorzystania danych przez algorytm.

Odczyt rejestru 8 tysięcy razy na sekundę nie daje 8 kHz informacji, jeśli sensor produkuje dane z ODR 1 kHz. Host może pobierać tę samą próbkę kilka razy. Odwrotnie, polling 1 kHz przy ODR 8 kHz bez FIFO pomija dane w sposób zależny od fazy. Dla każdej konfiguracji trzeba znać tor wewnętrzny: częstotliwość struktur MEMS i ADC, filtry, decymację, ODR wyjścia oraz opóźnienie grupowe.

Pasmo użytecznego sygnału powinno być niższe od częstotliwości Nyquista, ale sama nierówność f < ODR/2 nie wystarcza. Potrzebny jest filtr antyaliasingowy o odpowiednim tłumieniu przed decymacją. Jeżeli regulator działa z 1 kHz, a próbki żyroskopu powstają z 8 kHz, blok redukujący częstotliwość musi filtrować pasmo powyżej 500 Hz przed przekazaniem danych do pętli 1 kHz.

Wybór ODR jest kompromisem. Wyższe ODR zmniejsza odstęp czasowy i może uprościć kontrolę aliasingu, lecz zwiększa ruch na magistrali, obciążenie CPU i ilość logów. Nie zwiększa rzeczywistego pasma, jeśli wewnętrzny filtr albo mechanika już je ogranicza. Niższe ODR oszczędza zasoby, ale wymaga prawidłowego filtra i może wprowadzić zbyt duże opóźnienie dla szybkiego multirotora.

Aliasing — dlaczego filtr po fakcie nie wystarcza#

Aliasing zachodzi, gdy składowa powyżej połowy częstotliwości próbkowania zostaje odwzorowana jako inna, niższa częstotliwość. Dla próbkowania fs sygnał o częstotliwości f może pojawić się w paśmie podstawowym jako:

falias = |f - k · fs|

dla takiej liczby całkowitej k, która sprowadza wynik do obserwowanego pasma. Harmoniczna napędu 1,2 kHz może więc po niewłaściwym próbkowaniu wyglądać jak wolniejsze zaburzenie, które estymator interpretuje jako rzeczywisty ruch kadłuba. Cyfrowy low-pass zastosowany dopiero do zaliasowanych próbek nie wie, że energia pochodziła z innej częstotliwości.

Obrona jest warstwowa:

usuwa się źródło nadmiernych drgań: niewyważone śmigło, uszkodzone łożysko, luzy i giętką ramę;
ogranicza się transmisję mechaniczną bez tworzenia nowego rezonansu izolatora;
wybiera się wewnętrzny filtr sensora i ODR zapewniające tłumienie przed decymacją;
zachowuje się prawidłowe filtrowanie przy każdej kolejnej zmianie częstotliwości;
dopiero potem stosuje się low-pass i notch w torze programowym.

ArduPilot wprost rozróżnia drgania leżące w paśmie pomiarowym od drgań, które zaliasowały się do pasma: te drugie trzeba przede wszystkim ograniczyć mechanicznie lub przed próbkowaniem. Jest to ważniejsze niż kosmetyczne „wygładzenie” końcowego wykresu.

Czas próbki, opóźnienie i jitter#

Estymator potrzebuje czasu fizycznego pomiaru, a nie tylko chwili, w której MCU skończył transfer SPI. W torze występuje kilka różnych momentów:

zjawisko fizyczne
  → próbkowanie/konwersja MEMS
  → filtr wewnętrzny
  → zapis do rejestru lub FIFO
  → data-ready
  → początek transferu
  → zakończenie DMA
  → publikacja do estymatora

Stałe opóźnienie można zmierzyć i uwzględnić. Zmienny jitter jest trudniejszy, ponieważ każda próbka ma inną relację czasową do pozostałych sensorów. Najlepszy projekt wykorzystuje data-ready podłączone do wejścia capture timera, licznik czasu samego IMU albo synchronizację sprzętową. Timestamp w przerwaniu jest zwykle lepszy niż timestamp po zakończeniu DMA, o ile znane jest opóźnienie wewnętrznego filtra.

FIFO nie usuwa potrzeby timestampów. Jeśli w jednym transferze odczytano 16 próbek, nadanie wszystkim czasu callbacku DMA tworzy sztuczne Δt = 0 i jedną dużą lukę. Czas rekonstruuje się z licznika sensora, znanego ODR oraz znacznika zakotwiczonego w zegarze MCU. Trzeba obsłużyć przepełnienie licznika, utratę ramek i zmianę ODR.

Synchronizacja ma znaczenie również między żyroskopem a akcelerometrem. W BMI088 części gyro i accel są funkcjonalnie rozdzielone i mogą pracować z różnymi ODR oraz FIFO; Bosch publikuje osobną notę dotyczącą synchronizacji danych. Estymator nie powinien zakładać, że wartości odczytane jednym wywołaniem reprezentują dokładnie tę samą chwilę.

Data-ready, FIFO, SPI i DMA#

Polling typu read() wykonywany z pętli głównej jest wystarczający do prototypu, ale łatwo prowadzi do zduplikowanych i pominiętych próbek. Produkcyjny tor zwykle wykorzystuje:

pin data-ready → capture/przerwanie → transakcja SPI DMA
                                      ↓
                         walidacja ramki / FIFO
                                      ↓
                 jednostki SI + kalibracja + alignment
                                      ↓
                       kolejka estymatora i loggera

SPI jest zazwyczaj przewidywalniejsze dla szybkiego IMU niż współdzielone I²C: zapewnia większą przepustowość, pełny dupleks i oddzielny chip-select. I²C nadal może być właściwe dla wolniejszych sensorów albo małej konstrukcji, lecz blokada SDA przez jedno urządzenie zatrzymuje całą magistralę. W obu przypadkach trzeba kontrolować poziomy napięć, czasy narastania, integralność sygnału, timeout i recovery.

FIFO buforuje próbki podczas krótkiego opóźnienia CPU i pozwala odczytywać paczki. Nie jest nieskończonym magazynem. Sterownik powinien znać rozmiar rekordu, watermark, semantykę trybu stop-at-full lub stream, licznik overflow oraz zachowanie po błędnym nagłówku. Przepełnienie w trybie stream nadpisuje najstarsze dane; bez licznika sekwencji wykres może wyglądać płynnie mimo utraty czasu.

ISR powinna wykonywać tylko pracę ograniczoną czasowo:

void imu_data_ready_isr(void)
{
    const uint64_t sample_time = timer_capture_us();

    if (!imu_dma_busy()) {
        imu_pending_time = sample_time;
        imu_start_spi_dma();
    } else {
        imu_stats.data_ready_while_busy++;
    }
}

Przeliczanie macierzy kalibracji, filtracja i zapis na kartę nie należą do przerwania. Na MCU z cache, na przykład Cortex-M7, bufor DMA musi być umieszczony w odpowiedniej pamięci albo obsługiwany operacjami clean/invalidate. Dokładną architekturę warstwy urządzenia opisuje Sterownik IMU od zera.

Filtracja jako część pętli sterowania#

Filtr ma dwa skutki: tłumi wybrane składowe i zmienia fazę sygnału. Obniżenie częstotliwości odcięcia redukuje szum, ale zwiększa opóźnienie. W pętli zamkniętej opóźnienie zmniejsza zapas fazy i może wymusić łagodniejsze nastawy regulatora. Nie istnieje więc jedna „najczystsza” konfiguracja niezależna od dynamiki statku.

W torze mogą występować kolejne warstwy:

analogowy front-end oraz filtr przed ADC;
filtr cyfrowy/DLPF wewnątrz IMU;
filtr antyaliasingowy przed zmianą częstotliwości;
low-pass sygnału gyro lub accel;
statyczny notch dla stałego rezonansu;
notch dynamiczny śledzący częstotliwość napędu;
osobne filtrowanie wejścia estymatora i regulatora.

Każda warstwa powinna mieć uzasadnienie w widmie i budżecie opóźnienia. Kilka filtrów o podobnym odcięciu kumuluje fazę. Filtr medianowy dobrze usuwa pojedyncze impulsy, ale jest nieliniowy i może nieprzewidywalnie zmieniać szybki tor żyroskopu. Bezwarunkowe odrzucanie „nierealnych” próbek również jest ryzykowne: gwałtowny obrót albo uderzenie może być prawdziwe. Lepiej publikować status jakości i zachować surowe dane diagnostyczne.

Notch śledzący napęd#

W multirotorze dominujące linie widma często odpowiadają częstotliwości obrotowej silników i jej harmonicznym. Stały notch trafia w szczyt tylko dla jednego RPM. Telemetria dwukierunkowego DShot, pomiar RPM albo analiza FFT pozwalają przesuwać częstotliwość środkową wraz z napędem.

Betaflight wykorzystuje RPM filtering jako bank filtrów przypisanych do silników i harmonicznych. ArduPilot udostępnia harmonic notch sterowany między innymi telemetrią ESC, czujnikiem RPM, przepustnicą lub analizą FFT. Te mechanizmy nie zwalniają z naprawy mechaniki: nie usuną wszystkich rezonansów ramy, szumu łożysk ani energii, która zaliasowała się wcześniej.

Konfigurację dobiera się z logów przed i po filtracji. Zbyt szeroki lub zbyt nisko ustawiony notch dodaje opóźnienie w paśmie ważnym dla sterowania. Zbyt wiele harmonicznych zużywa czas CPU; dokumentacja ArduPilot pokazuje, że liczba aktywnych filtrów szybko rośnie przy osobnym śledzeniu wielu silników. Zmiany należy wprowadzać pojedynczo, zachować konfigurację bazową i po każdym kroku kontrolować temperaturę napędu oraz widmo.

Drgania mechaniczne i vibration rectification#

IMU jest akustycznym i mechanicznym odbiornikiem konstrukcji. Do sensora docierają wymuszenia od niewyważenia śmigieł, bicia wału, komutacji, łożysk, przekładni, powierzchni sterowych, turbulencji oraz przewodów działających jak sprężyny. Rama i mocowania mają własne mody drgań, więc ta sama jednostka napędowa daje inne widmo po zmianie payloadu albo sztywności ramienia.

Wibracje mogą powodować:

szerokopasmowy wzrost szumu;
wąskie linie i harmoniczne w PSD;
clipping akcelerometru lub żyroskopu;
aliasing do niskiego pasma;
pozorną zmianę biasu wskutek nieliniowości, zwaną vibration rectification error;
wzajemne pobudzenie konstrukcji i regulatora.

Miękkie mocowanie nie jest uniwersalnym lekarstwem. Sensor, płytka i izolator tworzą układ masa–sprężyna–tłumik. Poniżej i w pobliżu częstotliwości własnej izolator może przenosić albo nawet wzmacniać ruch; dopiero odpowiednio powyżej niej zapewnia izolację. Zbyt miękkie mocowanie pozwala też kontrolerowi poruszać się względem kadłuba, a napięty przewód omija elastomer i tworzy boczną drogę transmisji.

Kolejność diagnostyki powinna być fizyczna: inspekcja śmigieł i łożysk, dokręcenie ramy, pomiar bicia, próba różnych RPM, PSD/spektrogram surowego IMU, ocena clippingu, a dopiero potem zmiana izolacji i filtrów. Praktyczne metody opisują Diagnostyka logów i wibracji oraz Luzy i rezonanse ramy.

Montaż na PCB i w pojeździe#

Położenie IMU na płytce wpływa na wynik. Dobre praktyki obejmują:

umieszczenie z dala od ciężkich elementów i krawędzi płytki o dużym ugięciu;
ograniczenie naprężeń od śrub, złączy i procesu depanelizacji;
lokalne, czyste zasilanie oraz kondensatory zgodne z dokumentacją;
krótkie SPI nad ciągłą płaszczyzną odniesienia;
odsunięcie od cewek przetwornic, źródeł ciepła i silnych pól;
jednoznaczną orientację obudowy oraz znak pinu 1;
zachowanie zaleceń producenta dotyczących land pattern, lutowania i czyszczenia.

Naprężenie obudowy może zmienić offset MEMS. Parametry zmierzone na płytce ewaluacyjnej nie muszą powtórzyć się po lutowaniu na cienkim, skręconym PCB. Kalibrację końcową wykonuje się na zmontowanym kontrolerze, a dla dokładniejszych systemów — po zamontowaniu w docelowej obudowie i z ustalonym momentem dokręcenia.

Na pojeździe kontroler powinien znajdować się możliwie blisko środka obrotu. Jeżeli nie jest, estymator może uwzględniać ramię dźwigni, ale musi znać jego geometrię i dokładną prędkość kątową. Montaż musi wytrzymać pełen zakres temperatur, drgań i przeciążeń bez zmiany orientacji.

Temperatura, rozgrzewanie i naprężenia#

Bias oraz skala zmieniają się z temperaturą struktury MEMS. Wbudowany termometr mierzy temperaturę układu, nie powietrza. Po włączeniu sensor, MCU i przetwornice nagrzewają płytkę; w locie dochodzą słońce i przepływ powietrza. Kalibracja wykonana natychmiast po starcie może przestać pasować kilka minut później.

Są trzy poziomy rozwiązania:

kalibracja startowa po ustabilizowaniu — prosta, ale nie kompensuje zmian podczas lotu;
model temperaturowy — tabela lub funkcja biasu i skali dla każdej osi;
stabilizacja temperatury IMU — grzejnik utrzymuje powtarzalny punkt pracy kosztem energii i czasu rozgrzewania.

Model temperaturowy buduje się z kontrolowanego cyklu, gdy prawdziwy ruch jest znany. Dla żyroskopu może to być nieruchomy sensor; dla akcelerometru potrzebne są ustalone orientacje. Ogrzewanie i chłodzenie warto analizować oddzielnie, ponieważ bezwładność i histereza powodują różnicę przy tej samej chwilowej temperaturze. Parametry kalibracji muszą być przypisane do konkretnego egzemplarza, zakresu, rewizji płytki i konfiguracji sensora.

Kalibracja żyroskopu#

Najprostsza kalibracja żyroskopu estymuje bias z nieruchomego zapisu:

b̂ω = (1/N) Σ ωm[k]

Warunkiem jest rzeczywisty brak ruchu. Uśrednianie podczas kołysania stołu albo powolnego obracania platformy wprowadza błąd systematyczny. Detektor nieruchomości powinien sprawdzać wariancję żyroskopu, stabilność normy akcelerometru i brak dużej zmiany temperatury. Sam długi czas nie gwarantuje dobrej kalibracji.

Pełniejsza kalibracja obejmuje skalę i macierz osi. Potrzebny jest wzorzec prędkości lub znanego kąta, na przykład stół obrotowy. Ręczny obrót o „około 360°” jest dobrym testem zgrubnym, lecz nie pozwala oddzielić błędu operatora od błędu sensora.

Wariancja lub odchylenie Allana z długiego, równomiernego zapisu pomaga rozróżnić biały szum i wolne procesy biasu. Minimum krzywej nie jest uniwersalną dokładnością autopilota — zależy od warunków pomiaru, temperatury, montażu i czasu. Nota Analog Devices AN‑1041 pokazuje, jak dobierać czas uśredniania i interpretować stabilność biasu bez założenia, że nieskończenie dłuższa średnia zawsze pomaga.

Kalibracja akcelerometru#

Popularna kalibracja sześciopozycyjna ustawia każdą oś kolejno w przybliżeniu zgodnie i przeciwnie do grawitacji. Dla idealnego sensora wyniki odpowiadają +g i -g, a pozostałe osie zero. Z par ekstremów można wyznaczyć offset i skalę jednej osi:

offset = (r+ + r-) / 2
scale  = 2g / (r+ - r-)

Taki model diagonalny nie koryguje nieortogonalności. Lepsza kalibracja zbiera wiele statycznych orientacji i dopasowuje elipsoidę do sfery o promieniu g, wyznaczając macierz Ma i bias. Orientacje muszą pokrywać przestrzeń; dziesiątki próbek w prawie tej samej pozycji nie poprawiają obserwowalności.

Lokalna wartość g zależy od szerokości geograficznej i wysokości, ale dla większości małych UAV błąd prostego 9,80665 m/s² jest mniejszy niż błędy taniego sensora i stanowiska. W systemie metrologicznym trzeba jednak jawnie określić użyty wzorzec oraz niepewność ustawienia.

Kalibracja na stole nie naprawia vibration rectification, clippingu, ugięcia PCB ani złej transformacji do bryły. Po kalibracji statycznej wykonuje się test dynamiczny i porównanie normy siły właściwej w różnych orientacjach.

Bias uczony w locie#

Estymator może utrzymywać bias żyroskopu i akcelerometru jako zmienne stanu. Nie oznacza to, że każdy błąd jest zawsze obserwowalny. Podczas długiego, jednostajnego ruchu część biasu akcelerometru może być trudna do odróżnienia od nachylenia lub rzeczywistego przyspieszenia. Bias yaw żyroskopu bez wiarygodnej obserwacji kierunku przekłada się na dryf kursu.

Aktualizacja biasu powinna mieć ograniczenia tempa i niepewności. Nagły skok oszacowania może być objawem złego sensora zewnętrznego, drgań albo błędu czasu, nie prawdziwą zmianą MEMS. Po resecie IMU, zmianie zakresu lub przełączeniu na inny egzemplarz estymator musi wiedzieć, że poprzedni bias może nie pasować.

Kalibracja startowa i estymacja w locie pełnią różne role. Pierwsza daje bezpieczny punkt początkowy i test zdrowia, druga śledzi wolne odchylenia przy wykorzystaniu dodatkowych obserwacji.

Od próbek IMU do orientacji#

Żyroskop zapewnia szybką propagację orientacji. Akcelerometr wnosi obserwację kierunku grawitacji, ale tylko z niepewnością zależną od dynamiki. Magnetometr, GNSS z dwiema antenami, kurs z ruchu albo system wizyjny mogą obserwować yaw. Estymator łączy dane, uwzględniając czas, kowariancje i model ruchu.

Najprostszy filtr komplementarny rozdziela role częstotliwościowo: szybkie zmiany pochodzą z żyroskopu, wolna korekta roll/pitch z akcelerometru. EKF propaguje stan z danych IMU i koryguje go innymi sensorami. W obu przypadkach błędnie ustawiony noise, delay albo bias może sprawić, że algorytm będzie wyglądał stabilnie na stole, lecz zawiedzie przy przyspieszeniu.

Akcelerometr nie daje bezpośredniej obserwacji yaw, a sama grawitacja nie rozróżnia obrotu wokół pionu. W locie bez magnetometru kurs może być obserwowany z GNSS lub wizji tylko przy odpowiednim ruchu. Artykuły Filtr komplementarny, Filtr Kalmana w UAV i INS w UAV opisują kolejne poziomy estymacji.

IMU dla estymatora i IMU dla regulatora#

Ten sam fizyczny sensor obsługuje dwa odbiorniki o innych potrzebach:

Odbiorca	Priorytet	Typowe wymagania
regulator rate	małe opóźnienie, duże pasmo	równy sampling, kontrola fazy, niski jitter, notch napędu
estymator attitude	znany czas i model szumu	timestamp pomiaru, bias, kowariancja, poprawne osie
INS/nawigacja	stabilność i synchronizacja	skala, temperatura, długookresowy bias, korekcje zewnętrzne
diagnostyka	możliwość odtworzenia toru	raw i filtered, status, konfiguracja, temperatura, clipping

Można rozgałęzić pipeline i używać różnych filtrów, ale oba tory muszą pochodzić z tej samej jawnie wersjonowanej próbki. Regulator nie powinien przypadkiem korzystać z opóźnionych danych estymatora, a EKF z danych po agresywnym filtrze o nieznanej charakterystyce.

W regulatorze szum żyroskopu jest szczególnie widoczny w członie D i komendach silników. Nadmierny sygnał wysokoczęstotliwościowy podnosi temperaturę ESC i silników oraz może pobudzać konstrukcję. Z kolei nadmierne filtrowanie opóźnia reakcję na prawdziwe zaburzenie. Strojenie PID powinno następować dopiero po ustabilizowaniu mechaniki i pipeline IMU.

Jak czytać datasheet IMU#

Porównanie nazw produktów bez analizy warunków testu jest niewystarczające. W datasheetcie trzeba znaleźć co najmniej:

dostępne zakresy gyro i accel;
ODR oraz legalne kombinacje z trybami zasilania;
pasmo i opóźnienie każdego filtra;
noise density oraz warunki jej pomiaru;
initial zero-rate output, repeatability i stabilność biasu;
współczynniki temperaturowe;
cross-axis sensitivity i błąd skali;
rozmiar, format i tryby FIFO;
timestamp, data-ready i opcje synchronizacji;
wymagania VDD/VDDIO, startup, reset i decoupling;
limity SPI/I²C oraz format burst;
zakres temperatur, udar i drgania;
zalecenia montażowe, lutownicze i erraty.

Wartości typical opisują zwykle typowy egzemplarz w określonych warunkach, nie gwarantowany limit wszystkich sztuk. Parametr może być podany osobno dla osi albo jako najgorszy przypadek. Noise density nie jest tym samym co RMS po filtrze; przybliżenie ma postać:

σRMS ≈ noise_density · √(effective_noise_bandwidth)

Efektywne pasmo szumowe nie musi równać się częstotliwości -3 dB. Jeśli producent nie podaje charakterystyki, pomiar gotowej konfiguracji jest konieczny.

BMI088 i ICM‑42688‑P jako dwa przykłady#

BMI088 i ICM‑42688‑P pojawiają się w wielu kontrolerach, lecz reprezentują nieco inne podejścia. Nie należy traktować poniższej tabeli jako rankingu — konkretne wartości i opcje trzeba zawsze sprawdzić w aktualnej rewizji datasheetu.

Cecha systemowa	BMI088	ICM‑42688‑P
organizacja	części accel i gyro funkcjonalnie rozdzielone	zintegrowany sześcioosiowy tor
pozycjonowanie producenta	odporność na środowisko o dużych drganiach	niski szum, rozbudowany tor cyfrowy
interfejsy	SPI i I²C, osobna konfiguracja części	SPI/I²C/I3C zależnie od trybu i dokumentacji
buforowanie	FIFO i mechanizmy przerwań; synchronizacja wymaga uwagi	rozbudowane FIFO, formaty ramek i timestamp
konsekwencja integracyjna	trzeba jawnie wiązać czasy gyro i accel	trzeba poprawnie parsować format, zegar i konfigurację FIFO

Bosch publikuje dla BMI088 zakres ODR obejmujący różne tryby aż do 2 kHz, programowalne pasma i osobne materiały o synchronizacji. TDK opisuje w ICM‑42688‑P między innymi programowalne filtry antyaliasingowe, FIFO oraz rozszerzone formaty danych. Te funkcje dają korzyść tylko wtedy, gdy sterownik je poprawnie konfiguruje i weryfikuje po resecie.

Układy klasy przemysłowej, na przykład moduły Analog Devices ADIS, mogą oferować kalibrację fabryczną, lepszą stabilność i jawniejsze charakterystyki, ale są większe, droższe i mają inne wymagania mechaniczne oraz energetyczne. Nie ma sensu dobierać ich wyłącznie według ceny: platforma wyścigowa, autopilot mapujący i statek latający w szerokim zakresie temperatur mają różne funkcje celu.

Kryteria doboru sensora do platformy#

Dobór warto zacząć od wymagań systemu, a nie od katalogu:

Pytanie	Dlaczego jest ważne
jaka jest maksymalna prędkość obrotu i przeciążenie?	wyznacza zakres oraz margines clippingu
jakie jest pasmo regulatora?	wyznacza ODR, filtr i budżet opóźnienia
jakie widmo ma napęd?	określa odporność na drgania i anti-aliasing
jak długo system ma działać bez korekcji zewnętrznej?	zwiększa wagę stabilności biasu i temperatury
czy potrzebna jest synchronizacja z kamerą/lidarem?	wymaga timestampu, FSYNC lub wspólnego zegara
jaki jest zakres temperatur?	określa model termiczny albo potrzebę grzania
czy dopuszczalny jest reset lub utrata jednej magistrali?	wpływa na redundancję i podział zasilania
jaki jest budżet CPU, energii i logowania?	ogranicza ODR, liczbę filtrów i FIFO

Dla małego multirotora priorytetem bywa niski delay, duży zakres żyroskopu i zachowanie w drganiach. Dla fixed-wing o spokojnej dynamice można użyć mniejszego pasma, ale większego znaczenia nabierają stabilność długookresowa i współpraca z GNSS. Dla fotogrametrii oraz lidar mappingu krytyczna jest synchronizacja czasu i ekstrynsyka IMU–kamera/lidar. Dla platformy przemysłowej wymagającej fault tolerance dochodzą kwalifikowane redundantne sensory, niezależne zasilanie i diagnostyka.

Redundancja i różnorodność#

Dwa IMU na jednej płytce pomagają wykryć awarię pojedynczego układu, ale nie usuwają wspólnych przyczyn. Oba widzą te same drgania, temperaturę, błąd orientacji płytki i zakłócenia zasilania. Jeśli korzystają z tego samego SPI, DMA i kodu sterownika, awaria wspólnej warstwy może unieważnić oba kanały.

Porównanie sensorów wymaga:

transformacji do wspólnej ramki bryły;
wyrównania czasowego;
uwzględnienia odmiennych filtrów i opóźnień;
osobnych modeli biasu i temperatury;
progów zależnych od dynamiki, nie stałego porównania próbka do próbki.

Trzy sensory umożliwiają głosowanie, ale większość nie zawsze ma rację, jeśli dwa kanały dzielą błąd wspólny. Różne modele układów zwiększają różnorodność, lecz komplikują charakterystyki opóźnienia i szumu. Architektury autopilotów mogą prowadzić osobne „lanes” estymatora związane z różnymi IMU i źródłami nawigacji, dzięki czemu przełączenie jest decyzją na poziomie spójności stanu, a nie tylko medianą surowych liczb.

Przełączenie primary IMU może spowodować skok biasu. Estymator musi albo utrzymywać osobne stany, albo wykonać kontrolowane ponowne wyrównanie. Samo podmienienie wskaźnika do bufora jest niewystarczające. Szerzej temat omawia Redundancja sensorów flight controllera.

Monitorowanie zdrowia IMU#

Stan „układ odpowiada na WHO_AM_I” jest zbyt słabym kryterium. Monitor zdrowia powinien obserwować co najmniej:

ciągłość numerów sekwencji i Δt;
liczbę overflow FIFO oraz błędnych ramek;
timeouty i błędy magistrali;
rejestry krytycznej konfiguracji po starcie i okresowo;
clipping per oś;
temperaturę oraz jej dopuszczalne tempo zmiany;
wariancję i bias w stanie nieruchomym;
rozbieżność redundantnych sensorów;
wiek ostatniej prawidłowej próbki;
wynik self-testu, jeśli układ go oferuje.

Użyteczna maszyna stanów może wyglądać tak:

OFF → STARTING → WARMING → CALIBRATING → HEALTHY
                         ↘ DEGRADED → RECOVERING
                                      ↘ FAILED

Po resecie brownout identyfikator układu może nadal być poprawny, ale zakres i ODR wracają do wartości domyślnych. Dlatego sterownik odczytuje konfigurację, posiada licznik jej generacji i nie publikuje danych o niepewnej skali. Recovery ma ograniczoną liczbę prób oraz backoff, aby uszkodzony sensor nie zajmował całego CPU.

Stara próbka nie może być po cichu używana jako nowa. Utrata IMU jest zdarzeniem krytycznym dla regulatora; reakcja zależy od redundancji i klasy pojazdu, ale musi być jawnie zaprojektowana i przetestowana w HIL.

Co logować#

Wartość diagnostyczna logu zależy od punktów obserwacji. Minimalny zestaw obejmuje:

raw gyro i accel przed filtrami programowymi;
dane po kolejnych istotnych filtrach;
dokładny timestamp próbki i czas publikacji;
temperaturę sensora;
zakres, ODR, konfigurację filtra i identyfikator egzemplarza;
clipping, overflow, błędy magistrali i reset;
wybrane primary IMU oraz przyczynę przełączenia;
RPM silników albo sygnał pozwalający powiązać widmo z napędem;
wyjście regulatora i komendy aktuatorów.

Nie zawsze da się stale logować pełne 8 kHz sześciu osi. Można stosować bufor kołowy, krótkie zrzuty raw na zdarzenie, decymację z prawidłowym anti-aliasingiem i osobne statystyki ciągłe. Ważne, aby plik zawierał konfigurację potrzebną do interpretacji. Wykres bez informacji o skali, filtrze i ODR może prowadzić do fałszywych wniosków.

Przydatne analizy:

przebieg Δt i histogram latency;
średnia, wariancja i histogram w spoczynku;
PSD każdej osi przy wyłączonym i pracującym napędzie;
spektrogram podczas płynnej zmiany RPM;
korelacja temperatury z biasem;
różnica między redundantnymi IMU po wyrównaniu czasu;
liczba clippingów względem manewru i komend;
porównanie danych przed i po filtrze wraz z opóźnieniem.

Procedura laboratoryjna dla nowego kontrolera#

1. Identyfikacja i konfiguracja#

Odczytaj identyfikator, rewizję i wartości resetowe. Ustaw zakres, ODR, filtr, FIFO oraz przerwania, a następnie odczytaj krytyczne rejestry i porównaj je z żądaną konfiguracją. Sam sukces zapisu SPI nie dowodzi, że sensor przyjął ustawienia.

2. Pomiar czasu#

Policz rzeczywiste zbocza data-ready w długim oknie, zmierz rozkład Δt, czas transferu i opóźnienie publikacji. Wymuś obciążenie CPU, loggera i pozostałych magistral. Liczniki utraconych próbek nadal muszą pozostać zerowe albo system ma jawnie przejść w degradację.

3. Osie i skala#

Wykonaj dodatni ruch każdej osi, sześć statycznych orientacji i ruch złożony. Sprawdź surowe kody oraz jednostki SI. Zapisz transformację sensor → board → body jako część konfiguracji sprzętowej.

4. Szum i rozgrzewanie#

Zarejestruj długi postój od zimnego startu do stanu ustalonego. Oddziel pierwsze minuty rozgrzewania od stabilnego fragmentu. Oblicz bias, wariancję, PSD i — jeśli wymagania tego potrzebują — odchylenie Allana.

5. Zakres i clipping#

Na bezpiecznym stanowisku pobudź sensor do wartości zbliżonych do projektowych granic. Potwierdź flagę clippingu i brak zawijania liczby ze znakiem. Test udarowy nie powinien przekraczać limitów mechanicznych układu ani płytki.

6. Wibracje#

Najpierw użyj kontrolowanego wzbudnika albo silnika bez śmigła, następnie zabezpieczonego stanowiska napędowego. Wykonaj sweep RPM, rejestrując raw IMU i obroty. Poszukaj rezonansów, aliasingu oraz zmiany średniej pod wpływem drgań.

7. Temperatura#

Przeprowadź cykl w docelowym zakresie temperatur z kontrolowanym ruchem. Osobno oceń bias, skalę, czas stabilizacji i histerezę. Nie używaj punktów z okresu, w którym stanowisko mechanicznie się poruszało.

8. Błędy i recovery#

W HIL lub na stanowisku zasymuluj brak data-ready, timeout SPI, złą ramkę FIFO, reset układu i opóźnienie zadania. Sprawdź, czy stare próbki nie są publikowane jako aktualne, konfiguracja zostaje odtworzona, a estymator otrzymuje zmianę stanu zdrowia.

9. Integracja bez śmigieł#

Zweryfikuj attitude, znaki korekcji regulatora i przełączenie redundantnego IMU. Poruszając kadłubem ręcznie, sprawdź czy odpowiedź wyjść przeciwdziała ruchowi. Dopiero po przejściu tej kontroli można przejść do testów napędu i etapowego programu lotnego.

Kryteria odbioru zamiast „wygląda dobrze”#

Przed testem należy zapisać mierzalne limity. Przykładowy szablon — wartości trzeba wyznaczyć dla konkretnej platformy — obejmuje:

Metryka	Warunek odbioru
ODR zmierzony	w ustalonej tolerancji względem konfiguracji
utracone/zdublowane próbki	zero w teście nominalnym; jawna reakcja w fault injection
jitter `Δt`	poniżej limitu wynikającego z błędu integracji
latency	percentyl i maksimum poniżej budżetu pętli
bias po rozgrzaniu	poniżej limitu na każdej osi
norma accel w orientacjach	zgodna z modelem kalibracji i niepewnością
clipping	brak w locie nominalnym; poprawny licznik w teście granicznym
temperatura	pełny zakres bez utraty konfiguracji i przekroczenia biasu
PSD napędu	brak niedopuszczalnych szczytów w paśmie sterowania
recovery	powrót albo kontrolowana degradacja w określonym czasie

Limit powinien wynikać z budżetu błędów estymacji i sterowania. „Szum mniejszy niż w poprzedniej płytce” nie jest wymaganiem, jeśli poprzednia płytka również nie spełniała potrzeb systemu.

Najczęstsze błędne diagnozy#

„Attitude drży, więc potrzebny jest mocniejszy filtr”#

Przyczyną może być uszkodzone śmigło, luźny silnik, rezonans ramy, clipping albo aliasing. Mocniejszy filtr może ukryć wykres i pogorszyć zapas fazy, nie usuwając źródła.

„WHO_AM_I działa, więc IMU jest sprawne”#

Identyfikator potwierdza jedną transakcję. Nie dowodzi poprawnego ODR, zakresu, zegara, FIFO, osi ani ciągłości danych.

„Akcelerometr w spoczynku powinien wskazywać zero”#

Nie. Na podpartym stole norma siły właściwej wynosi około 1 g. Zero jest oczekiwane w idealnym swobodnym spadku.

„Wyższy ODR zawsze zmniejsza opóźnienie”#

Nie, jeśli wewnętrzny filtr pozostaje wolny, host odczytuje stare próbki albo pipeline buforuje paczki. Trzeba zmierzyć cały tor.

„Dwa IMU dają dwukrotnie większe bezpieczeństwo”#

Nie przy wspólnym zasilaniu, magistrali, mechanice i kodzie. Redundancja musi obejmować analizę wspólnych przyczyn oraz sposób przełączenia stanu estymatora.

„Kalibracja naprawi każdy błąd”#

Kalibracja koryguje modelowalne parametry. Nie naprawia saturacji, utraty próbek, niestabilnego montażu, aliasingu ani nieznanego timestampu.

Lista kontrolna integratora#

[ ] Model i rewizja IMU są jednoznacznie zapisane.
[ ] Zakresy mają margines wynikający z pomiarów, a clipping jest liczony.
[ ] Rzeczywiste ODR, pasmo i opóźnienie zostały zmierzone.
[ ] Każda próbka ma czas pomiaru, sekwencję, temperaturę i status.
[ ] FIFO ma obsługę watermark, overflow, złej ramki i rekonstrukcji czasu.
[ ] Transformacja sensor → body ma testy znaków i ruchu złożonego.
[ ] Kalibracja jest przypisana do egzemplarza i wersji konfiguracji.
[ ] Rozgrzewanie oraz wpływ temperatury mieszczą się w budżecie.
[ ] Widmo sprawdzono przy pełnym zakresie RPM i z docelowym payloadem.
[ ] Filtry mają zapisany cel, odcięcie, opóźnienie i punkt w pipeline.
[ ] Log zawiera dane wystarczające do odtworzenia diagnozy.
[ ] Reset, błąd magistrali i utrata próbek były testowane poza lotem.
[ ] Przełączenie redundantnego sensora nie tworzy niekontrolowanego skoku.
[ ] Strojenie regulatora rozpoczęto dopiero po kwalifikacji toru IMU.

IMU należy traktować jak przyrząd pomiarowy osadzony w pętli czasu rzeczywistego, nie jak sześć rejestrów do okresowego odczytania. Dopiero połączenie mechaniki, filtracji przed aliasingiem, poprawnego czasu, kalibracji, diagnostyki i jawnego kontraktu z estymatorem tworzy sensor, na którym można oprzeć stabilny lot.

Przypisy

Sumit Sharma, „Drone Development from Concept to Flight” [książka]
Terry Kilby, Belinda Kilby, „Make: Drony dla początkujących” [książka]
Bosch Sensortec: BMI088 [datasheet]
TDK InvenSense: ICM-42688-P [datasheet]
Bosch Sensortec: BMI088 product page and technical documents [oficjalny punkt publikacji datasheetu, handling/soldering, shipment oraz application notes IMU]
Analog Devices AN-1041: iSensor IMU Quick Start Guide and Bias Optimization Tips [nota aplikacyjna producenta]
Analog Devices: The Basics of MEMS IMU/Gyroscope Alignment [artykuł techniczny producenta]
ROS REP 145: Conventions for IMU Sensor Drivers [standard projektu open source]
PX4: Using PX4 Navigation Filter EKF2 [dokumentacja projektu]
PX4 Guide: Vibration Isolation [dokumentacja projektu]
ArduPilot: Vibration Damping [dokumentacja projektu]
ArduPilot Copter: Measuring Vibration [dokumentacja projektu open source]
ArduPilot Copter: Managing Gyro Noise with Dynamic Harmonic Notch Filters [dokumentacja projektu open source]
Betaflight: DShot RPM Filtering [dokumentacja projektu]
TDK InvenSense public software organization [repozytoria producenta]
ST AN5543: Guidelines for enhanced SPI communication on STM32 MCUs and MPUs [nota aplikacyjna producenta]