Filtr Kalmana - cześć pierwsza
- Szczegóły
- Autor: Piotr Laskowski
Jeszcze za czasów rozwoju APL2 próbowałem zbudować filtr Kalmana i na podstawie kilku przykładów praktycznie udało mi się uruchomić filtr wysokości, wariometru i przyspieszenia w osi Z. Już nie pamiętam szczegółów, ale sprawiało to wrażenie działania, chociaż było dalekie od oczekiwań. Wtedy nie byłem w stanie zrozumieć, jak to działa, a bez zrozumienia trudno jest dostosować się do konkretnego zastosowania, więc tamte próby traktuję jako ćwiczenia praktyczne. Pomimo intensywnych poszukiwań nie udało mi się wtedy znaleźć opracowania, które by w sposób dla mnie zrozumiały tłumaczyło podstawy działania filtra.
W czasie tegorocznego urlopu, gdy przeczytałem już wszystkie zabrane na wypoczynek książki, przejrzałem sieć pod kątem literatury dotyczącej filtra i znalazłem świetne opracowanie Alexa Beckera „Kalman Filter from the Ground Up”. Pod linkiem znajduje się strona typu „landing page” reklamująca książkę, zawierająca kilka podstawowych rozdziałów. Polecam jej lekturę, bo genialnie opisuje zasadę działania, zaczynając od prostych przykładów, stopniowo wprowadzając kolejne zagadnienia i wyprowadzając wzory. Książka już do mnie jedzie, ale informacje znajdujące się na powyższej stronie były wystarczające do uruchomienia kolejnych kilku wersji liniowego filtra na wcześniej wspomnianych danych.
Obecnie pierwsze podejście do filtra zawierało taki sam zestaw danych: wysokość, prędkość pionową i przyspieszenie w osi Z, tworzących wektor stanu:
x = [h, v, a]
Wysokość i przyspieszenie potrafimy zmierzyć z ograniczoną dokładnością, prędkość możemy tylko wyliczyć. Filtr dodatkowo wykorzystuje zależność między elementami wektora stanu, tworząc model matematyczny wiążący je ze sobą. Model jest zawarty w macierzy przejścia F:
Czy warto wspominać o błędach?
- Szczegóły
- Autor: Piotr Laskowski
Uważam, że podczas rozwoju jakiegokolwiek projektu, występowanie błędów jest czymś normalnym. Jest wartością zasilającą sprzężenie zwrotne w pętli doskonalenia produktu. Im efektywniejszy będzie proces ich znajdowania, tym szybciej projekt osiągnie dojrzałość. Już w trakcie implementacji obsługi błędów znalazłem kilka problemów, z których wcześniej nie zdawałem sobie sprawy. Debugowanie wymusza ponowny przegląd kodu mającego nierzadko kilka lat. Spojrzenie na niego jeszcze raz z nowej perspektywy pozwala zauważyć nowe rzeczy. Można to porównać do gry, w której trzeba znaleźć drogę w labiryncie pełnym pułapek. W pierwszej kolejności trzeba zbudować sobie w głowie mapę zauważonych pułapek. Nigdy nie wiadomo, czy wszystkie już zostały znalezione. Powstający kod jest opisem przejścia przez ten labirynt. W kodzie opis pułapek jest zwykle pomijany, czasami jest coś wspomniane w komentarzu, ale w ogólności przy powtórnej wizycie w tym miejscu trzeba zbudować mapę jeszcze raz. Ponieważ ścieżka jest już wyznaczona, spojrzenie na labirynt jest zupełnie inne, niż wtedy gdy patrzyliśmy na niego za pierwszym razem. Nierzadko można znaleźć lepszą drogę. Dlatego warto wracać do kodu, a już na pewno wtedy, gdy stwarza on zauważalne problemy.
W złożonym systemie, jakim jest APL3, mającym wiele trybów pracy i obsługującym wiele poleceń, jest prawdopodobne, że mogą nastąpić okoliczności, w których pewne mechanizmy nie będą działały tak, jak powinny. Przyczyną może być nieoczekiwana konfiguracja, błąd czujnika czy nieprzystosowanie kodu do zmienionych warunków pracy. Wystąpienie takich sytuacji jest ewidentnym błędem i warto byłoby mieć świadomość, że taka sytuacja ma miejsce. Można wtedy zareagować, poprawiając oprogramowanie lub zmieniając warunki pracy. W przypadku rdzenia CM7, mającego swój interfejs użytkownika, informację o błędzie można wyświetlić jako komunikat na ekranie, ale jak poinformować o sytuacji z błędem w rdzeniu CM4?
Regulatory PID
- Szczegóły
- Autor: Piotr Laskowski
Obiektem odpowiedzialnym za utrzymanie stabilności lotu jest regulator proporcjonalno-całkująco-różniczkujący, nazywany skrótowo PID od pierwszych liter angielskich nazw tych trzech członów. Regulator porównuje wartość zadaną wz, czyli taką wartość regulowanego parametru, jaką chcemy osiągnąć, z wartością wejściową x otrzymaną z układu pomiarowego, reprezentującą rzeczywistą wartość regulowanego parametru. Regulator oblicza różnicę między tym, co ma być, a tym, co jest, nazywaną błędem lub uchybem regulacji e. Błąd jest następnie przetwarzany w poszczególnych członach regulatora, aby obliczyć wartość wyjściową y, przekazywaną na układy wykonawcze sterujące regulowanym parametrem. W bezzałogowcach czujniki znajdują się w jednostce inercyjnej mierzącej prędkości kątowe i kąty orientacji w przestrzeni. Czujniki w centralce barometrycznej mierzą wysokość, na podstawie której obliczana jest prędkość zmiany wysokości, oraz prędkość względem powietrza. Jeszcze inne mierzą położenie w przestrzeni i prędkość zmiany tego położenia. Układami wykonawczymi w bezzałogowcu są silniki lub powierzchnie sterowe. Wartości zadane w zależności od trybu regulacji pochodzą z aparatury RC w przypadku sterowania ręcznego, z układu automatyki lotu, który wyznacza zadane wartości parametrów lotu na podstawie zdefiniowanej trasy lotu, lub z członów nadrzędnych w przypadku kaskady.
Regulator pracuje z rzeczywistymi wartościami regulowanych parametrów. Jeżeli stabilizuje prędkość zmiany wysokości, to jego wartość zadana i wartość wejściowa są podawane w jednostkach pomiaru tej prędkości, czyli w metrach na sekundę [m/s]. Jeżeli stabilizuje wysokość, to wartości podane są w metrach [m]. Jeżeli regulator stabilizuje kąt, to obie wchodzące do niego wartości są podane w radianach [rad]. Prędkości kątowe są podane w radianach na sekundę [rad/s]. Współrzędne geograficzne są również kątem podanym w radianach [rad]. Analogicznie prędkość zmiany położenia jest wyrażona w radianach na sekundę [rad/s]. Dlaczego radiany, a nie stopnie? Ponieważ jednostką miary kąta w układzie SI jest radian i w tej jednostce wykonywane są obliczenia trygonometryczne. Przyjąłem, że wszystkie kąty będą wyskalowane w radianach a ewentualne przeliczenie ich na bardziej intuicyjne stopnie odbywa się w interfejsie użytkownika.
Wartość wyjściowa regulatora jest wynikiem przekształceń różnicy parametrów wejściowych. Ponieważ obiekty wykonawcze nie są wyskalowane w konkretnych jednostkach, więc wyjście regulatora jest normalizowane do zakresu ustalonego przez użytkownika, domyślnie ±100,0.
Nowe funkcjonalności regulatora PID – regulowany filtr członu D
- Szczegóły
- Autor: Piotr Laskowski
Na studiach uczyłem się, że regulator (wtedy jeszcze pneumatyczny) parametryzuje się wzmocnieniem Kp, czasem zdwojenia Ti, oraz czasem wyprzedzenia Td. Wyobraźcie sobie moje zdziwienie i irytację, gdy wreszcie zrozumiałem, że wartości Ki i Kd jakimi parametryzuje się regulatory cyfrowe, to nie są znane ze szkoły czasy Ti i Td określające dynamikę regulatora, tylko statyczne wzmocnienia tych członów dynamicznych, określające, jaka część ich odpowiedzi ma wchodzić na wyjście.
O ile nie odczuwam dysonansu przy członie całkującym, gdzie całkowana, czyli w praktyce sumowana, jest wartość błędu od początku pracy do chwili bieżącej. Moje idealistyczne wyobrażenie czasu zdwojenia jako parametru powodującego, że po tym czasie wartość całki podwaja się, nie gryzie się ze współczynnikiem wzmocnienia Ki. Zmieniając współczynnik Ki, mnożymy przez niego błąd e, jaki wchodzi do całki, i wartość całki rośnie szybciej lub wolniej. Całkowanie samo w sobie jest dynamiczne, więc wyprowadzony na zewnątrz współczynnik nadal steruje dynamiką regulatora. Znając dynamikę procesu, można pokusić się o wyskalowanie tego parametru w sekundach.
Natomiast mam dysonans poznawczy z członem różniczkującym. W regulatorach dyskretnych, bo takie implementuje się w sterownikach lotu, wejście członu D to różnica wartości bieżącej i poprzedniej. Dopóki pętla regulacji była dosyć wolna, to wartość poprzednia była przyzwoicie oddalona w czasie od wartości bieżącej i jakoś to działało. Teraz, gdy pętla regulacji przyspieszyła do prawie 2 tys. obrotów na sekundę, porównywane wartości są tak małe, że nikną w szumie pochodzącym z czujnika i taki człon praktycznie nie działa jako różniczkujący, ale jest generatorem szumu z niewielką domieszką różniczkowania. Coś trzeba z tym zrobić, aby móc wyeliminować szum i sięgnąć do wcześniejszych niż poprzednie pomiarów.
Czytaj więcej: Nowe funkcjonalności regulatora PID – regulowany filtr członu D
Wymiana informacji między rdzeniami kontrolera
- Szczegóły
- Autor: Piotr Laskowski
Każdy współczesny komputer osobisty czy smartfon ma procesor z więcej niż jednym rdzeniem. Prognozuję, że wkrótce podobnie będzie z autopilotami, więc już teraz zdecydowałem się na dwurdzeniowy mikrokontroler, pomimo że współczesne autopiloty radzą sobie z jednym. Dzisiaj sobie radzą, ale jutro mogą potrzebować więcej, więc teraz jest czas na zdobycie doświadczenia i wypracowanie narzędzi do obsługi wieloprocesorowości.
Mikrokontroler w APL3 ma dwa rdzenie: CM7, czyli Cortex-M7 – szybki i nowoczesny jak na kontrolery rdzeń, pracujący z częstotliwością 480 MHz, obsługujący dużą ilość pamięci, oraz CM4, czyli Cortex-M4 – tradycyjny rdzeń będący rozszerzeniem popularnego Cortex-M3 o jednostkę zmiennoprzecinkową, pracujący z częstotliwością 240 MHz. Jako punkt odniesienia weźmy System FPV w wersji 2.x, gdzie kontroler miał rdzeń Cortex-M3 pracujący na 72 MHz, albo APL2 również z Cortex-M3 pracującym na 120 MHz, gdzie ich wydajność spokojnie wystarczała, aby można było latać.
Podstawowym wyznacznikiem wydajności autopilota jest prędkość aktualizacji jego pętli głównej, czyli cyklu obsługi wszystkich czujników, a w szczególności czujników jednostki inercyjnej IMU, pozwalającej na wyznaczenie orientacji w przestrzeni i aktualizacji swojej pozycji przez mechanizmy wykonawcze, czyli serwa lub silniki. W APL3 pętla główna obecnie jest aktualizowana z częstotliwością ok. 1900 Hz. Rdzeń CM4 obsługuje właściwy autopilot z wyłączeniem obsługi loggera na karcie SD i telemetrii. Rdzeń CM7 obsługuje multimedialny interfejs użytkownika, kamerę i obróbkę obrazu oraz czujniki zbliżeniowe: sonary i lidary. Docelowo CM7 ma również wyznaczać trasę lotu dla CM4 na podstawie danych ze swoich czujników, w szczególności obrazowego rozpoznania trasy lotu. Ze względu na różnorodność obsługiwanych funkcjonalności pracuje pod kontrolą systemu operacyjnego RTOS, obsługując wiele wątków, więc tutaj trudno mówić o częstotliwości obsługi, gdyż jest ona inna dla każdego wątku.
W sytuacji, gdy CM4 ma komplet informacji o stanie autopilota, a CM7 obsługuje interfejs użytkownika, w tym logger i telemetrię, pojawia się potrzeba wymiany informacji między nimi. Zostało to zrobione na bazie wspólnej pamięci SRAM4. W 2 kB zarezerwowanej w tym celu pamięci są zdefiniowane 2 tablice danych typu uint32_t: nBuforWymianyCM4[] i nBuforWymianyCM7[]. Każdy z rdzeni ma swoją kopię tych buforów w postaci unii uDaneCM7 i odpowiednio uDaneCM4, łączących powyższe tablice uint32_t oraz struktury danych wychodzących z każdego rdzenia. Unia jest po to, aby z jednej strony móc zapisać w niej dane o dowolnym typie i rozmiarze, a z drugiej strony móc efektywnie wykonać transfer danych do i ze wspólnej pamięci pełnymi 32-bitowymi słowami. Co kilka obiegów pętli oba rdzenie zapisują swoje dane do wspólnego bufora wymiany i odczytują dane drugiego rdzenia.
Czytaj więcej: Wymiana informacji między rdzeniami kontrolera
Pomiary FFT Pentakoptera
- Szczegóły
- Autor: Piotr Laskowski
Jakiś czas temu uruchomiłem analizę FFT danych z akcelerometrów i żyroskopów. Od tego czasu oprogramowanie APL3 wzbogaciło się o obsługę protokołu DShot do sterowania silnikami i zostały wyeliminowane błędy, ograniczające prędkość pętli głównej rdzenia CM4 obsługującego kod autopilota, więc prędkość pozyskiwania danych wzrosła z 270 Hz do okolic 1800 Hz. To sprawia, że na wodospadzie FFT powinno być widać dużo szerszy zakres częstotliwości. Rozbudowuje się też docelowa platforma, na której chcę testować APL3, czyli Pentakopter. Pierwszy pomiar zdjęty z Pentakoptera uzbrojonego już w silniki wygląda jednak bardzo ubogo, co znaczy dobrze :-)
Test jest jeszcze bez śmigieł, same 2 tylne silniki. Platforma stoi na własnych nóżkach. W pętli dwukrotnie odczytuję czujniki IMU, więc FFT, które pokazuje zakres częstotliwości równy połowie częstotliwości akwizycji danych, tutaj pokrywa częstotliwość pracy pętli głównej, czyli okolice 1800 Hz. FFT jest zrobione dla 2048 punktów z oknem Hamminga. Silniki sterowane od 0 na górze wykresu do 100% wysterowania na dole (wysterowanie na wodospadzie nietypowo rośnie w dół, częstotliwość normalnie w prawą stronę). Trzy pierwsze wykresy to dane z akcelerometrów w osiach X, Y, Z, kolejne 3 to dane z żyroskopów P, Q, R.
To, co widać, to przede wszystkim poziome paski mówiące tyle, że przy pewnych prędkościach obrotowych silniki drgają bardziej niż przy innych. Ciekawa jest ostra granica między niebieskim obszarem o małych drganiach i czerwonym o dużych.
W tle, głównie na wodospadach przyspieszenia, widać 3 figury typu „X”. Kreska opadająca od lewej do prawej jest czymś normalnym. Zwiększa się wysterowanie i rośnie częstotliwość. Druga kreska od X, ta narastająca, jest dla mnie zagadką. Tak samo zagadką jest występowanie większej liczby tych X-ów. Może to są jakieś harmoniczne? Jak na razie nie widać żadnych rezonansów ramy.
Strona 2 z 17