Kaskada zamiast jednego regulatora#

W multirotorze pętla attitude zamienia błąd orientacji na zadaną prędkość kątową, a pętla rate steruje momentem na podstawie żyroskopu. Pętla wewnętrzna musi być szybsza. Próba sterowania bezpośrednio kątem przy wysokiej dynamice miesza dwie skale czasu i utrudnia ograniczenia.

W dyskretnej implementacji dla kroku dt:

e[k] = r[k] - y[k]
I[k] = clamp(I[k-1] + Ki · e[k] · dt)
D[k] = -Kd · filtered_derivative(y[k])
u[k] = Kp · e[k] + I[k] + D[k] + feedforward(r)

Różniczkowanie pomiaru zamiast błędu ogranicza derivative kick przy skoku setpointu. dt musi pochodzić z zegara monotonicznego. Wstawienie stałej wartości mimo jitteru zmienia efektywne wzmocnienie I i D.

Znaczenie członów#

P tworzy moment proporcjonalny do bieżącego błędu. Zbyt małe P daje miękką odpowiedź, zbyt duże może wywołać oscylację. I kompensuje stałe zaburzenia, np. przesunięcie środka ciężkości; bez anti-windup narasta podczas saturacji. D reaguje na szybką zmianę pomiaru i zwiększa tłumienie, ale wzmacnia szum.

Feedforward nie jest czwartym składnikiem PID. Wykorzystuje żądaną zmianę, aby sterować zanim powstanie duży błąd. Jego sens zależy od jakości setpointu i modelu dynamiki.

Saturacja i anti-windup#

Po osiągnięciu maksimum napędu dodatkowe żądanie regulatora nie jest realizowane. Integrator, który nadal rośnie, magazynuje błąd i po wyjściu z saturacji powoduje przeregulowanie. Proste ograniczenie I jest lepsze niż brak ochrony, lecz back-calculation uwzględnia różnicę między wyjściem żądanym a rzeczywiście zaalokowanym.

Mikser powinien przekazywać regulatorowi informację o ograniczeniu osi. Samo sprawdzanie, czy dowolny silnik osiągnął 100%, jest niewystarczające: możliwa jest desaturacja przez zmianę ciągu zbiorczego.

Filtr D i opóźnienie#

D-term jest szczególnie wrażliwy na drgania. Filtr PT1 ma łagodną fazę i tłumienie, biquad daje bardziej strome zbocze kosztem większej zmiany fazy w pobliżu odcięcia. Dynamiczny notch i filtr RPM celują w harmoniczne napędu. Każdy dodatkowy filtr zwiększa opóźnienie, dlatego konfigurację ocenia się łącznie.

Strojenie na danych#

Bezpieczny proces zaczyna się w symulacji, a próby fizyczne wykonuje się na nieuzbrojonej platformie w kontrolowanym środowisku. Log powinien zawierać setpoint, pomiar, P/I/D/FF, wyjścia po mikserze, RPM oraz znaczniki saturacji.

  1. Usunąć luzy, niewyważenie i nasycenie IMU.
  2. Ustawić filtrację na podstawie widma, nie przez kopiowanie profilu.
  3. Oceniać odpowiedź rate osobno dla każdej osi.
  4. Zwiększać P do uzyskania wymaganej odpowiedzi z zapasem stabilności.
  5. Dodawać D dla tłumienia, kontrolując temperaturę silników i szum.
  6. Ustawić I według odrzucania wolnych zaburzeń i zachowania przy saturacji.
  7. Dopiero potem stroić feedforward i pętlę attitude.

„Propwash” nie jest pojedynczym parametrem. Może ujawniać niedostateczny autorytet napędu, opóźnienie filtrów, rezonans mechaniczny albo saturację. Zwiększanie D bez diagnozy może tylko nagrzać silniki.

Co odczytać z logu#

Gdy pomiar podąża za setpointem z opóźnieniem i bez oscylacji, problemem może być zbyt małe pasmo. Dzwonienie po skoku wskazuje małe tłumienie. Oscylacja o stałej częstotliwości niezależna od setpointu sugeruje rezonans lub zbyt duże wzmocnienie. Powolny błąd w jednym kierunku wskazuje na I, asymetrię napędu albo środek ciężkości.

Porównanie samego wykresu gyro z setpointem nie wystarcza. Należy sprawdzić, czy silniki nie są ograniczone, czy napięcie nie spadło i czy estymator nie zmienił trybu. Regulator może generować poprawne żądanie, którego platforma fizycznie nie potrafi wykonać.

Błędy implementacyjne#

  • używanie zmiennego dt bez ograniczenia po pauzie debuggera;
  • całkowanie, gdy układ nie jest w stanie aktywnym;
  • różniczkowanie zaszumionego błędu bez filtra;
  • reset integratora przy każdej zmianie trybu, powodujący skok momentu;
  • strojenie przed sprawdzeniem znaków osi;
  • liczenie regulatora na starych próbkach IMU;
  • pomijanie różnicy między wyjściem PID a wyjściem po mikserze.

Test jednostkowy powinien obejmować skok setpointu, stałe zaburzenie, saturację, zmianę dt, NaN w pomiarze i przejście trybu. Test integracyjny musi sprawdzić znaki całego łańcucha sensor–estymator–regulator–mikser.

Od równania ciągłego do kodu#

Klasyczny zapis ciągły ma postać:

u(t) = Kp·e(t) + Ki·∫e(t)dt + Kd·de(t)/dt

Firmware nie oblicza jednak całki i pochodnej w sposób ciągły. Pracuje na próbkach, a wybór dyskretyzacji zmienia odpowiedź przy wysokich częstotliwościach. Najprostszy integrator Eulera jest łatwy do audytu, ale przy dużym dt może wnosić istotny błąd. Metoda trapezów lepiej odwzorowuje całkę:

I[k] = I[k−1] + Ki · (e[k] + e[k−1]) · dt / 2

Pochodna liczona z różnicy dwóch zaszumionych próbek wymaga filtra. Praktyczna implementacja często łączy różniczkowanie z filtrem dolnoprzepustowym w jednym równaniu, zamiast najpierw obliczać surową pochodną o bardzo dużej amplitudzie. Współczynniki filtra muszą być przeliczane, jeżeli zmienia się częstotliwość pętli.

Jednostki są częścią kontraktu. Jeżeli błąd rate jest wyrażony w rad/s, wzmocnienie nie jest liczbowo wymienne z konfiguracją używającą °/s. To samo dotyczy znormalizowanego wyjścia −1…1, momentu w N·m i wartości protokołu silnikowego. Kopiowanie liczb między firmware bez sprawdzenia definicji nie jest przenoszeniem strojenia.

Pętle kaskadowe#

Typowy multirotor stosuje co najmniej dwie pętle:

zadany kąt ─→ regulator attitude ─→ zadana prędkość kątowa
                                            │
żyroskop ─────────→ regulator rate ←────────┘
                              │
                              └→ żądany moment → alokator → silniki

Pętla rate korzysta bezpośrednio z żyroskopu i musi reagować szybciej niż pętla attitude. Zewnętrzna pętla nie może żądać dynamiki, której wewnętrzna nie potrafi wykonać. Jeszcze wolniejsze regulatory pozycji i prędkości generują żądania przechylenia oraz ciągu. Hierarchia pasm zapobiega sytuacji, w której kilka regulatorów jednocześnie próbuje korygować to samo zjawisko w podobnej skali czasu.

W samolocie fixed-wing kaskada może prowadzić od wysokości do kąta toru lotu, pochylenia i wychylenia steru. Dynamika zależy wtedy mocno od prędkości opływu. Te same wzmocnienia przy prędkości bliskiej przeciągnięcia i podczas szybkiego przelotu dają inną skuteczność. Stosuje się skalowanie z prędkością, harmonogramowanie wzmocnień albo regulatory uwzględniające model płatowca.

Człon proporcjonalny#

P odpowiada za natychmiastową reakcję na błąd. W pętli rate większe P zwiększa moment przeciwdziałający rozbieżności między setpointem a pomiarem. Zbyt małe daje opóźnioną, „gumową” odpowiedź. Zbyt duże zmniejsza zapas fazy i może wywołać oscylację, szczególnie gdy w torze występują filtry, wolny napęd lub opóźnienie komunikacji z ESC.

Oscylacja nie zawsze oznacza nadmierne P. Podobny wykres może spowodować luźne ramię, rezonans śmigła, aliasing IMU albo błędna estymacja czasu. Najpierw należy sprawdzić, czy częstotliwość zjawiska koreluje z setpointem i modelem sterowania, czy raczej z RPM.

Integrator i błąd ustalony#

I gromadzi błąd, dzięki czemu usuwa stałe odchylenie wynikające z niesymetrycznej masy, wiatru albo rozrzutu napędu. Integrator powinien mieć jawne warunki aktywacji. Nie powinien rosnąć przed uzbrojeniem, przy niewiarygodnym pomiarze ani wtedy, gdy dana oś nie ma autorytetu wykonawczego.

Popularne strategie anti-windup to:

Strategia Działanie Ograniczenie
clamp ogranicza wartość I nie rozróżnia przyczyny saturacji
conditional integration zatrzymuje całkowanie pogłębiające saturację wymaga informacji o znaku ograniczenia
back-calculation koryguje I różnicą między wyjściem żądanym a wykonanym wymaga strojenia szybkości „odwijania”
tracking prowadzi stan regulatora za rzeczywistym wyjściem przy zmianie trybu bardziej rozbudowana logika przejść

Back-calculation można zapisać koncepcyjnie jako:

I += (Ki·e + Kaw·(u_applied − u_requested)) · dt

u_applied musi pochodzić zza ograniczeń i alokatora. Jeżeli regulator zna tylko swoje wyjście przed mikserem, nie widzi utraty autorytetu spowodowanej nasyceniem konkretnego silnika.

D-term, szum i derivative kick#

Różniczkowanie błędu obejmuje również pochodną setpointu. Skok polecenia daje wtedy duży impuls, zwany derivative kick. D liczone od pomiaru przeciwdziała szybkiemu ruchowi platformy, ale nie reaguje bezpośrednio na skok zadania:

D = −Kd · d(measurement)/dt

Nie usuwa to szumu. D wzmacnia składowe proporcjonalnie do częstotliwości, dlatego jest najbardziej wrażliwe na drgania mechaniczne, błędy kwantyzacji i niesynchronizowane próbki. Temperatura silników po locie jest użytecznym sygnałem ostrzegawczym, ale nie zastępuje widma żyroskopu i wykresu składnika D.

Filtracja tworzy kompromis. Mocne tłumienie szumu zmniejsza ryzyko wzbudzenia, lecz opóźnia informację używaną przez regulator. Opóźnienie fazowe może wymusić mniejsze P i D, przez co sterowanie wolniej odrzuca zaburzenia. Najlepszym „filtrem” jest często usunięcie źródła: wyważenie śmigła, usztywnienie mocowania, poprawa łożyska lub odsunięcie IMU od rezonansu.

Feedforward i kształtowanie setpointu#

Feedforward szacuje sterowanie potrzebne do wykonania polecenia, zanim powstanie błąd. W pętli rate może być proporcjonalne do setpointu lub jego zmiany. Nie zwiększa sam z siebie odporności na nieznane zaburzenia — za nią odpowiada sprzężenie zwrotne.

Surowy ruch drążka nie musi być dobrym setpointem. Ograniczenie prędkości zmian, krzywa expo i filtr setpointu wyznaczają kompromis między odczuciem operatora a pobudzaniem rezonansów. Jeżeli po zmianie kształtowania setpointu pozostawi się stare feedforward, odpowiedź może się znacząco zmienić mimo niezmienionych P/I/D.

Mikser i alokacja sterowania#

PID nie steruje silnikami niezależnie. Generuje żądany ciąg zbiorczy i momenty osi, które mikser przelicza na aktuatory. Dla idealnego quada X jest to operacja macierzowa, lecz rzeczywista platforma ma różne stałe ciągu, ograniczenia minimalne, martwe strefy i dynamikę ESC.

Gdy jeden silnik osiąga maksimum, nie wszystkie żądania mogą zostać spełnione. Alokator musi ustalić priorytety: zachować momenty roll/pitch kosztem ciągu, chronić yaw albo ograniczyć wszystkie osie. Ta decyzja wpływa na integratory. Analiza logu przed mikserem bez wyjść po ograniczeniach może błędnie przypisać problem regulatorowi.

W hexacopterach, tilt-rotorach i platformach z serwami alokacja może zależeć od geometrii i aktualnego stanu mechanizmu. Macierz sterowania powinna być weryfikowana testem znaków: wymuszenie dodatniego momentu każdej osi ma spowodować fizycznie właściwą kombinację aktuatorów.

Częstotliwość pętli i synchronizacja IMU#

Szybsza pętla nie zawsze daje lepsze sterowanie. Jeżeli IMU dostarcza nowe dane z częstotliwością 1 kHz, obliczanie PID osiem razy na tej samej próbce nie wnosi informacji. Dobre rozwiązanie synchronizuje obliczenia z gotowością próbki, mierzy czas rzeczywisty i nadzoruje przekroczenia deadline.

Aliasing występuje, gdy drganie powyżej częstotliwości Nyquista pojawia się po próbkowaniu jako niższa, fałszywa częstotliwość. Filtr cyfrowy zastosowany dopiero po aliasingu nie odzyska pierwotnego sygnału. Znaczenie ma analogowy filtr IMU, jego tryb ODR oraz mechanika platformy.

Jitter zmienia efektywną dyskretyzację. Firmware powinien logować czas cyklu, liczbę przekroczeń i wiek próbki. Po przerwie debuggera dt może być setki razy większe; bez ograniczenia integrator wykona ogromny skok, a pochodna stanie się bezwartościowa.

Metodyczne strojenie#

Strojenie rozpoczyna się od modelu i kontroli mechanicznej, nie od losowego zmieniania trzech suwaków.

1. Ustalenie warunków#

Zapisuje się masę, śmigła, akumulator, wersję firmware, częstotliwość pętli, filtry i ograniczenia aktuatorów. Próby porównawcze mają tę samą konfigurację i podobny stan energii. Zmiana śmigła lub napięcia może zmienić dynamikę bardziej niż niewielka korekta wzmocnienia.

2. Weryfikacja toru pomiarowego#

Sprawdza się orientację FC, znaki żyroskopu, nasycenie zakresu, clipping akcelerometru, widmo drgań i znaczniki czasu. Regulator nie skompensuje odwróconej osi ani brakujących próbek.

3. Identyfikacja ograniczeń#

Log powinien pokazać, przy jakich poleceniach dochodzi do minimum lub maksimum napędu. Test poza obszarem liniowym nie służy do oceny małosygnałowego strojenia. Zbyt mały thrust-to-weight albo przeciążone serwo jest ograniczeniem konstrukcji.

4. Pętla wewnętrzna#

Porównuje się setpoint rate i gyro dla oddzielnych pobudzeń roll, pitch i yaw. Zwiększa się P do uzyskania wymaganej szybkości z zachowanym marginesem, następnie D do odpowiedniego tłumienia. I ustawia się według odpowiedzi na trwałe zaburzenie. Feedforward dobiera się na końcu, obserwując zarówno błąd, jak i przeregulowanie.

5. Pętle zewnętrzne#

Dopiero stabilna pętla rate pozwala ocenić attitude, velocity i position. Zbyt agresywna pętla zewnętrzna może stale nasycać wewnętrzną, co na wykresie wygląda jak słaby PID rate.

Jak czytać odpowiedź#

Objaw Możliwe przyczyny Co sprawdzić najpierw
wolne podążanie bez oscylacji małe pasmo, limit rate, słaby napęd setpoint po ograniczeniu, saturację
przeregulowanie po skoku małe tłumienie, windup, feedforward D, stan I, wyjście po mikserze
oscylacja wysokiej częstotliwości rezonans, duże P/D, aliasing FFT gyro/D, RPM, mocowanie FC
wolne „pływanie” osi małe I, bias, asymetria integrator, temperaturę IMU, środek masy
odbicie po wyjściu z saturacji windup różnicę u_requested/u_applied
poprawny PID, zła trajektoria pętla zewnętrzna lub estymator setpoint rate, innowacje EKF

Ocena nie powinna sprowadzać się do wrażenia pilota. Użyteczne miary to błąd RMS, opóźnienie, czas narastania, przeregulowanie, czas ustalania, udział czasu w saturacji i energia sterowania. Wyniki porównuje się w określonym zakresie częstotliwości oraz warunków lotu.

Odporność implementacji#

Kod regulatora powinien jawnie obsługiwać:

  • brak lub nieświeżą próbkę;
  • NaN i wartości poza zakresem;
  • zmianę trybu oraz bezudarowe przejęcie sterowania;
  • aktywację i rozbrojenie;
  • saturację osi i ograniczenia aktuatorów;
  • reset estymatora, zmianę setpointu i zmianę częstotliwości pętli;
  • utratę jednego aktuatora w platformie redundantnej.

„Bumpless transfer” oznacza takie ustawienie stanów regulatora przy przejściu, aby wyjście nie wykonało sztucznego skoku. Jeżeli tryb manualny oddaje sterowanie trybowi stabilizowanemu, integrator może zostać zainicjalizowany wartością zgodną z aktualnym wyjściem, zamiast zerowany bezwarunkowo.

Walidacja przed lotem#

Najpierw testuje się funkcję PID na sztucznych sygnałach, następnie zamkniętą pętlę w SITL/HIL, a potem stanowisko bez śmigieł lub z ograniczonymi aktuatorami. Symulacja powinna obejmować szum, opóźnienie, saturację i zmienne napięcie. Pierwszy test fizyczny nie może być pierwszym przypadkiem, w którym wykonywany jest failsafe lub przejście trybu.

Wynik strojenia jest ważny dla konkretnej konfiguracji. Zmiana masy, bezwładności, ramy, silników, śmigieł, serw, filtrów albo częstotliwości próbkowania wymaga ponownej oceny. Profile producenta są punktem startowym, nie dowodem stabilności egzemplarza.

Powiązane tematy#

Przypisy#

  1. Betaflight, PID Tuning Guide — znaczenie członów, feedforward i diagnostyka odpowiedzi.
  2. Betaflight, PID Tuning — implementacyjne tło regulatora i filtracji.
  3. PX4, Controller Diagrams — kaskady regulatorów w stosie PX4.
  4. PX4, Multicopter PID Tuning Guide — kolejność strojenia pętli wielowirnikowca.
  5. ArduPilot, Copter Attitude Control — przepływ sygnałów w regulatorze attitude/rate.
  6. Åström K.J., Hägglund T., PID Controllers: Theory, Design, and Tuning, Instrument Society of America — dyskretyzacja, anti-windup i ocena jakości regulacji.

Powiązane tematy

IMU w droniePrzekrojowa inżynieria toru inercyjnego UAV: znaczenie pomiarów gyro i accel, modele błędów, zakresy, ODR, aliasing, timestamp, FIFO i DMA, filtry, drgania, montaż, kalibracja temperaturowa, redundancja, health monitoring oraz kwalifikacja laboratoryjna.Flight controller — jak działaPrzekrojowa techniczna anatomia kontrolera lotu: sprzęt, domeny czasu, sensory, estymacja stanu, regulatory kaskadowe, alokacja sterowania, wyjścia, zasilanie, odporność na awarie, logowanie i uruchomienie platformy.Betaflight — architektura i ścieżka startowaProfil techniczny Betaflight jako stosu dla wielowirnikowców sterowanych ręcznie: harmonogram pętli, sensory, regulatory, mikser i diagnostyka.Diagnostyka logów i wibracjiPraktyczny proces diagnostyczny: synchronizacja czasu, setpoint kontra measurement, widmo drgań, saturacja i rozróżnianie awarii mechanicznej od problemu regulatora.

Źródła z centralnego rejestru

  1. Sumit Sharma, „Drone Development from Concept to Flight” [książka]
  2. Betaflight: PID tuning [dokumentacja]
  3. Betaflight: Blackbox logging internals [dokumentacja]