Symulator czujników IMU
- Szczegóły
- Autor: Piotr Laskowski
Dawno temu, gdy dopiero co zacząłem studia, przytrafiła się okazja, aby na weekend wrócić do domu na wieś z dalekim krewnym, który właśnie kończył karierę studencką i na październik została mu obrona pracy magisterskiej. Tego dnia wracał własnym samochodem, więc umówiliśmy się, że zabierze mnie bezpośrednio z Politechniki. Pochwalił się, że zrobił symulację komputerową czegoś związanego z motoryzacją. To był początek lat 90. Komputerów PC praktycznie jeszcze nie było w domach, co najwyżej w instytucjach typu PW. Pojęcie symulacji komputerowej znałem z programów publicystyczno-naukowych typu „Sonda” i kojarzyło mi się z efektowną animacją. Jakież było moje rozczarowanie, kiedy kuzyn przyniósł składankę wydruków z drukarki igłowej, która nie miała nawet obrazków, tylko blade, niezrozumiałe kolumny cyferek, wydrukowane na różowym papierze.
Mam nadzieję, że po takim wstępie będziecie bardziej wyrozumiali dla owoców mojej pracy. Niedawno zacząłem robić filtr Kalmana kątów orientacji, mający zebrać dane z kilku kompletów czujników i wyznaczyć w możliwie najlepszy sposób kąty Eulera. Powstało coś, co działa, ale nie do końca tak, jak bym tego oczekiwał. Czy przyczyną są błędy algorytmu, kalibracji czujników, czy coś innego? Czujniki, a w szczególności magnetometry, na pewno nie działają idealnie w żelbetonowym pomieszczeniu.
Potrzebne jest coś pewnego, co mogłoby posłużyć za punkt odniesienia. Wymyśliłem więc, że utworzę wzorcowe wektory grawitacji i pola magnetycznego. Wektor grawitacji o wartości 9,81 m/s2 skierowany będzie pionowo w dół, a wektor magnetyczny o wartości 50,58 μT skierowany na północ z obrotem w osi Z o deklinację i w osi X o inklinację. Następnie będę nimi obracał, a składowe XYZ wektorów będą odpowiadały idealnym czujnikom. To dosyć szybko ruszyło, ale coś było nie tak jak trzeba. Wtedy zrozumiałem, że obracam światem wokół czujnika, a nie czujnikiem wokół świata. Trzeba było zmienić znak oraz kolejność obrotów z ZYX na XYZ i wreszcie zaczęło się zgadzać. Można więc powiedzieć, że z rozwojem autopilota jestem już na etapie kopernikańskiego, późnego średniowiecza.
Nowa wersja sprzętu
- Szczegóły
- Autor: Piotr Laskowski
Rozbudowa filtra wysokości przez dodanie map wysokości i lidaru pokazała niezdolność pierwszej wersji sprzętu do przechowania zadowalającej ilości map, a interfejs USB FS jest zbyt wolny, aby w rozsądnym czasie wgrać duże ilości danych do pamięci autopilota. Przez ostatnie 1,5 roku, uruchamiając kolejne funkcjonalności, na bieżąco znajdowałem i poprawiałem błędy na schemacie. Teraz nadszedł czas, w którym mam już sprawdzone wszystkie obwody i można hurtowo nanieść poprawki. Dodatkowo przymierzam się do rozpoczęcia lotów i trzeba się liczyć z możliwością uszkodzenia sprzętu, więc przydałaby się nowa wersja, którą można byłoby małoseryjnie produkować, przynajmniej na bieżące potrzeby.
Bez dużego pośpiechu przez ostatnie kilka tygodni zostawałem w pracy po godzinach, aby narysować schemat USB rozbudowanego z FS do HS poprzez interfejs ULPI, oraz większej pamięci na mapy. Efekt wygląda tak, jak na wizualizacjach poniżej:
EKF kątów orientacji
- Szczegóły
- Autor: Piotr Laskowski
Skoro już zacząłem przygodę z filtrem Kalmana, dalszą konsekwencją jest zrobienie przynajmniej dwóch kluczowych filtrów: kątów orientacji, a następnie filtra pozycji w przestrzeni 3D. Obecnie kąty Eulera liczone są dwiema metodami. Pierwsza z nich to najprostszy z możliwych filtr komplementarny, łączący całki prędkości kątowych żyroskopów z kątami obliczonymi metodami trygonometrycznymi z wektorów przyspieszenia i magnetyzmu w proporcji 98% kąta z całki oraz 2% kąta z trygonometrii. Druga metoda liczenia kątów oparta jest na rachunku kwaternionów. W przeciwieństwie do pierwszej jest nieintuicyjna, pewnie dlatego, że w szkole nie uczono nas o kwaternionach, ale bardziej elastyczna, bez miejsc o nieoznaczonej pozycji i prostsza obliczeniowo.
Obie metody liczą kąty z danych, jakie zostaną im dostarczone. Jeżeli dane są złe, bo np. żyroskopy nie trzymają zera albo magnetometr jest źle skalibrowany, to wszystkie te błędy przenoszą się na obliczane kąty orientacji. W pewnym zakresie wady tej nie ma filtr Kalmana, który potrafi na bieżąco estymować niektóre błędy czujnika i usuwać je z pomiaru. W liczonym niedawno filtrze wysokości estymowany był błąd wskazania wysokości obu czujników ciśnienia. Dodatkowo filtr Kalmana potrafi przyjmować dane z wielu czujników, a w APL3 podstawowe czujniki są zdublowane, więc aż prosi się o implementację tej metody. Ze wszystkich wymienionych metod wymaga ona największej mocy obliczeniowej, ale tę mamy dostępną w dużych ilościach. Mając kilka wersji obliczonych kątów, można je ze sobą porównać, próbować ustalić, dlaczego się różnią i poprawić przyczynę problemu. Przykładowo dosyć długo miałem ewidentną różnicę między kątem liczonym filtrem komplementarnym a kątem liczonym kwaternionami. To było 0,8 stopnia w przechyleniu i 0,11 stopnia w pochyleniu. Okazało się, że jedna metoda liczyła z pierwszego zestawu czujników, a druga z drugiego i widocznie takie są błędy ręcznego montażu akcelerometrów.
Mapy i estymacja błędu czujników wysokości
- Szczegóły
- Autor: Piotr Laskowski
Rozwijam filtr Kalmana wysokości. Chcę uzyskać wyznaczenie wysokości MSL i AGL niezależnie od dostępności informacji z GNSS. Doszedłem do rozmiaru wektora stanu x wynoszącego 10 elementów. Oprócz dotychczasowych: wysokości nad poziomem morza, prędkości pionowej, przyspieszenia pionowego i błędu dwóch akcelerometrów doszły: błędy pomiaru wysokości z dwóch czujników ciśnienia, błędy pomiaru wysokości dwóch odbiorników GNSS oraz wysokość terenu. Aktualizacja stanu filtra dotychczas obejmowała pomiary: wysokości MSL i prędkości pionowej z czujników ciśnienia oraz przyspieszenia pionowego z akcelerometrów. Obecnie powiększyła się o pomiary wysokości z dwóch odbiorników GNSS, pomiar wysokości lidarem i odczyt z mapy: Numeryczny Model Terenu lub Numeryczny Model Pokrycia Terenu. Nie mam jeszcze rozpoznania, która z map będzie lepsza do tego zastosowania, ale obie są zbudowane tak samo, więc to kwestia eksperymentów. Mapy są dostępne do pobrania z Geoportalu. Na obrazku po prawej znajduje się autorska wizualizacja mapy wysokościowej losowo wybranego fragmentu Tatr.
Lidar o przyzwoitym zasięgu 30–50 m w zależności od reflektancji celu już do mnie jedzie. Będzie patrzył w dół i mierzył wysokość nad poziomem terenu. Mapa to gruba sprawa wymagająca sporo pracy i przerzucania olbrzymiej, jak na dotychczasowe standardy, ilości danych. Najmniej dokładna mapa o rozdzielczości 5 m obejmująca prostokąt o rozmiarach 2200×2380 m zajmuje 1,4 MB. Mam kostkę pamięci Flash o pojemności 64 MB. Część jest zajęta na zapis konfiguracji i komunikaty głosowe, część zarezerwowana na przechowywanie elementów interfejsu użytkownika, więc zostaje 56 MB dla map i można do niej wgrać 38 kafelków mapy. Chcąc lecieć po linii prostej, byłoby to 84,6 km. Do zdecydowanej większości rozwiązań wystarczy, ale szału nie ma. Testu dalekiego zasięgu, jaki był robiony 11 września 2026, już nie da rady wykonać. Kolejna mapa NMT o rozdzielczości 1 m obejmująca podobny obszar, zajmuje już 36 MB. Tutaj dam radę wgrać do APL jeden kafelek mapy, aby pokręcić się w miejscu. Natomiast kafelek mapy NMPT o rozdzielczości 0,5 m zajmuje 142 MB.
Pomiar wysokości lidarem VL53L1X
- Szczegóły
- Autor: Piotr Laskowski
Następna wersja filtra Kalmana ma wyznaczać wysokości AGL i MSL, więc potrzebuję pomiaru wysokości względem ziemi. Szczególnie jest to potrzebne podczas lądowania, aby móc rozróżnić etap szybkiego schodzenia z wysokości od etapu delikatnego przyziemienia. Jest potrzeba wykrycia poziomu gruntu, kiedy BSP jest jeszcze w powietrzu.
Ponad rok temu wpadła mi w oko seria układów VL53 od ST, zawierająca scalony laser na podczerwień i matrycę lawinowych fotodiod mierzących czas, po jakim fotodioda wykryje odbite światło wysłane przez laser. Są układy jedno- i wielostrefowe. W ostatnich dniach pojawił się układ VL53L9CX, który działa jak kamera, generując obraz 54×42 pikseli, na którym kolor odpowiada nie jasności, jak przy zwykłej kamerze, ale odległości. Świetna sprawa do tego, by zrobić detekcję przeszkód. Rok temu kupiłem próbki układów VL53L1X oraz VL53L5CX i właśnie uruchomiłem najprostszy, jednostrefowy VL53L1X.
Przysłowie mówi, że diabeł tkwi w szczegółach. Pierwszą rzeczą, z jaką trzeba się zmierzyć, to sterownik wymieniający duże ilości danych z układem powolną magistralą I2C. Uzyskanie pomiaru sporo trwa, a biblioteka używa transmisji blokującej, więc przez cały czas pomiaru kontroler nie może zrobić nic innego, co w systemach czasu rzeczywistego, takich jak autopilot jest niedopuszczalne. Podzieliłem bibliotekę na fragmenty kończące się transmisją po to, aby transmisję zamienić z blokującej na DMA. Nie udało mi się jeszcze tego uruchomić, więc na obecnym etapie transmisja jest jeszcze blokująca, ale już jest podzielona na kilka etapów, między którymi autopilot może działać.
Druga sprawa to zasięg. VL53L1X ma deklarowany zasięg do 4 m, ale to dotyczy pomiaru w ciemności względem białego tła. Liczyłem na to, że gdyby udało się uzyskać stabilnie pracującą połowę tego, to by wystarczyło. Jednak maksimum, jakie udało się uzyskać to ok. 700 mm, a w miarę stabilnie pracuje do ok. 300 mm. Jest prawdopodobne, że układ nie jest skonfigurowany optymalnie, ale próby dobierania budżetu czasu i trybu pracy ani razu nie pozwoliły przekroczyć pułapu 0,7 m. Nie robiłem jeszcze kalibracji, jednak mając na uwadze to, że rzeczywiste powierzchnie, względem jakich będzie odbywał się pomiar są znacznie trudniejsze niż to, co ćwiczę w warunkach biurowych, nie mam większych nadziei na praktyczne zastosowanie układu.
Na wykresach po prawej stronie przedstawionych jest kilka parametrów pomiaru. Na górze jest zmierzona odległość. W początkowym etapie pomiaru, czyli na prawym brzegu wykresu, czujnik nie miał przed sobą przeszkody, a pokazywał jakąś minimalną odległość, chociaż powinien pokazać nieskończoność. Status pomiaru (różowy na drugim wykresie) wskazywał poprawny pomiar. To, że coś było nie tak, widać dopiero po odchyleniu standardowym, które było zdecydowanie wyższe niż 3σ. Dla trochę większych odległości, powyżej 0,25 m, status pomiaru wskazuje na jakiś problem, chociaż sam pomiar i odchylenie standardowe wydają się w porządku. Ostatni wykres przedstawia reflektancję celu i natężenie tła. Pomiar był robiony w porannym półmroku, w pomieszczeniu, a czujnik skierowany był w stronę białego sufitu.
Filtr Kalmana z podwójnym zestawem czujników
- Szczegóły
- Autor: Piotr Laskowski
Spełniło się moje marzenie sprzed nastu lat, kiedy chciałem uzyskać lepszy pomiar, uśredniając dane z dwóch czujników. W APL2 istotnym problemem było utrzymanie wysokości. Kopter oscylował wokół zadanej wysokości i nie udawało się tego poprawić regulacją PID ani obróbką sygnałów z czujników. Zrobiłem nawet specjalny moduł ze sprzętowym wariometrem, aby regulator prędkości pionowej miał dobre dane. Próbowałem umieszczać czujniki ciśnienia po obu stronach płytki tak, aby mechaniczne oddziaływanie przyspieszenia w osi Z działało na membrany czujników w przeciwnych kierunkach, licząc na to że uśredniony sygnał będzie mniej zależny od wpływu przyspieszenia. Zacząłem nawet robić niepublikowany na stronie duży moduł, zajmujący 2 gniazda APL, zawierający 2 wariometry i 4 czujniki ciśnienia, po 2 z obu stron obwodu drukowanego. Oprócz tego moduł zawierał komplet czujników inercyjnych i mikrokontroler Cortex-M4, którego jedynym zadaniem było liczenie AHRS aby odciążyć kontroler główny. Nie wystarczyło mi czasu, aby go skończyć, ale wszystkie te próby były tylko półśrodkami. Uśrednienie wskazań dwóch czujników ciśnienia poprawiało sytuację w trudnym do oceny stopniu, ale raczej niewielkim. Wymagało to porządnej kalibracji wszystkich czujników przed taką fuzją. Dużo pomogło dołożenie czujnika ultradźwiękowego, dającego znacznie dokładniejszy pomiar, ale jego zasięg wynosił zaledwie około 1 m. Mimo to najtrudniejsze etapy lotu, czyli start i przyziemienie, miały dzięki temu czujnikowi solidne wsparcie, które powodowało, że wskazanie wysokości z czujników barometrycznych uzyskiwało kalibrację czujnikiem, którego wskazania nie były fałszowane poduszką powietrzną tworzącą się tuż przy ziemi na początkowym etapie lotu oraz przy lądowaniu. Podczas lądowania po dłuższym locie widać było w logach dociąganie wysokości, która w trakcie lotu zdążyła odpłynąć wraz z naturalnym dryftem ciśnienia.
Strona 1 z 17