Filtrowanie prędkości

[ursus_arctos]

Witam,
Podczas prac nad sterownikiem serwo napotkałem następujący problem: enkoder pozwala odczytać pozycję (w miarę dokładnie), jednak jego użyteczność w zakresie odczytu prędkości jest ograniczona - przy dużej prędkości częstotliwość zmian wskazania pozycji jest porównywalna z krokiem całkowania, ale przy małych prędkościach wygląda to już cieniutko. Kilka rozwiązań chodzi mi po głowie:
1. Filtr Kalmana - ale tutaj jest problem, bo charakterystyka układu jest zależna od prędkości (tzn. przy większej prędkości mamy większą pewność, więc filtrowanie powinno być mniej agresywne).
2. Taki filtr dyskretny:
v - aktualnie estymowana prędkość
t - czas, który upłynął pomiędzy zmianami wskazania enkodera
d - zmiana wskazania enkodera (-1, 0,+1)
η - stała zaniku prędkości (np. 0.99 czy coś w tym guście)
f(t) - funkcja pewności aktualnego odczytu - funkcja malejąca, wartości z przedziału <0, 1>
W każdym kroku pracy układu wykonujemy następującą operację:
v=ηv
jeżeli d!=0, to
v=v*(1-f(t)) + f(t)*d/t

Jak widać, kiedy nie ma zmiany pozycji enkodera, prędkość sobie powoli zanika, zgodnie z η. Jeżeli natomiast mamy zmianę, to poprzednie wskazanie zanika szybciej na rzecz nowego odczytu d/t. Funkcja f(t) przyjmuje małe wartości dla dużych t, aby nie wprowadzać nagłych "skoków" prędkości przy pojedynczych zliczeniach enkodera.

Co sądzicie o takim podejściu? Czy może ktoś implementował filtrowanie prędkości do PIDa? Jeżeli tak, to co zastosował i czy zadziałało?

[markcomp77]

nie implementowałem czegoś takiego...
ale servo to trochę jak kontroler ruchu, więc można (i pewnie trzeba) w wolniejszym wątku przygotowywać wartości statystyczne wszelakiego rodzaju

prędkość chwilową można aproksymować biorąc pod uwagę większą ilość punktów niż dwa ostatnie

aproksymacja może być jakąś funkcją wyższego rzędu (jest obszerny dział matematyki na ten temat)... problem to sprowadzenie abstrakcyjnej matematyki na poziom realizacji z automatu i szybko


krzywa prędkości powinna być w miarę "gładka"... wyeliminowania "nagłych" skoków jako błędów można pewnie zrealizować z pomocą zmiennej, której wartość ustalamy w konfiguracji...

[ Dodano: 2012-11-21, 13:47 ]
hehe... kawałek z opisu Kalmana

Teoretycznie filtr Kalmana jest estymatorem tego co nazywa się problemem liniowo-kwadratowym czyli problemem estymacji natychmiastowego stanu liniowego układu dynamicznego, który narażony jest na perturbacje białego szumu (zob. regulator liniowo-kwadratowy-Gaussa) &#8211; przez użycie pomiarów liniowo związanych ze stanem ale zakłóconych przez biały szum. Wynikowy estymator jest statystycznie optymalny w odniesieniu do dowolnej funkcji estymowanego błędu.

ale chyba bardziej wpasowuje się w zagadnienie przewidywania prędkości chwilowej "Obserwator stanu"

Znajomość stanu układu jest niezbędna przy rozwiązaniu wielu problemów teorii sterowania; na przykład, problemu stabilizacji układu z użyciem lokowania biegunów układu zamkniętego. W większości praktycznych przypadków, nie można poprzez bezpośrednią obserwację ustalić jaki jest stan fizyczny układu. Zamiast tego na wyjściu systemu obserwuje się pośrednie skutki jakie wywołuje stan wewnętrzny układu. Można to zilustrować przykładem z pojazdami w tunelu: częstość i prędkości z jakimi pojazdy wjeżdżają do tunelu i opuszczają go podlegają bezpośredniej obserwacji ale dokładny stan wnętrza tunelu może być tylko estymowany. Jeśli system jest obserwowalny, można, z użyciem obserwatora stanu, na podstawie pomiarów, w pełni zrekonstruować stan układu.

http://pl.wikipedia.org/wiki/Obserwator_stanu

[ Dodano: 2012-11-21, 13:51 ]
na mój gust --- im prościej tym lepiej

[piotr_olbrysz]

przy dużej prędkości częstotliwość zmian wskazania pozycji jest porównywalna z krokiem całkowania

przy dużej prędkości częstotliwość zmian wskazania pozycji jest porównywalna z krokiem całkowania
Dać zmienny krok całkowania, tak naprawdę używać stempli czasu. Dlatego lepsze rozwiązanie to sprawnie działający system przerwań od sygnałów A, B,"0". Każde zdarzenie stemplować czasem i wyjdzie dokradnie. Inna sprawa to silnikiem komutatorowym to dla małych prędkości rzuca niemiłosiernie, dlatego MK125 miał duże koło zamachowe.

[ursus_arctos]

Dlatego lepsze rozwiązanie to sprawnie działający system przerwań od sygnałów A, B,"0"

Eeeeee.... PID ma całkować nawet, jak uchyb się nie zmienia - wystarczy, że jest niezerowy.

A czy rzuca niemiłosiernie? No, dynamikę to on ma, nie powiem. Ale na małych prędkościach chodzi naprawdę gładziutko - mogę mu zadać prędkość 0.1obr/s i chodzi bezgłośnie.

[piotr_olbrysz]

Eeeeee.... PID ma całkować nawet, jak uchyb się nie zmienia - wystarczy, że jest niezerowy.

teraz "eeee"e a przdtem było "cieniutko".
tylko ze jak złe stemplowanie czasu to i kiepska matematyka

"mogę mu zadać prędkość 0.1obr/s i chodzi bezgłośnie"
ciekawe ile pól ma komutator

[ursus_arctos]

Komutator ma raczej sporo pól - ale chyba ważniejsze, że tak na wyczucie, to wirnik ma skośne szczeliny - bo silnik nie przeskakuje w żaden zauważalny sposób jak się nim kręci.

teraz "eeee"e a przdtem było "cieniutko".
tylko ze jak złe stemplowanie czasu to i kiepska matematyka

Chyba się słabo rozumiemy. Samo stemplowanie czasu wydarzeń nie wystarczy. Również pomiędzy nimi regulator musi zmieniać swoje wyjście, więc musi pracować na jakimś timerze - praca w pętli głównej programu nie jest najlepszym pomysłem, bo jak przyjdą przerwania komunikacyjne*, to procek może dość długo do pętli głównej nie powrócić.

* komunikację robię tak: jak w FIFO od USARTa znajdę znak końca pakietu, to wywołuję softwareowo przerwanie EXTI4 o niskim priorytecie, na którym pakiet jest przetwarzany. Priorytet jest niski dlatego, że przetwarzanie takiego pakietu może trwać długo.

[piotr_olbrysz]

praca w pętli głównej programu nie jest najlepszym pomysłem

Przepraszam najmocniej, ja tego nie powiedziałem, ani nie napisałem.
Jak już, to napisałem że wyliczanie kolejnego kroku zaczyna się od zdarzenia typu zmiana sygnału A lub B. Czy cała operacja przeliczania filtru jest w przerwaniu, wątku czy pętli głównej nie ma to znaczenia jak jest zrobiona poprawnie, ale to już szczegóły algorytmu.

Jeżeli pomiędzy zderzeniami ( sygnałami A,B) przeliczamy filtr, to na jego wejściu musimy dać dane z palca czyli np. zakładamy że prędkość obrotowa jest stała, albo jednostajnie zmienna,albo..., czyli zachowuję się tak jak poprzednio. Przy małych prędkościach, zmiennym obciążeniu będzie to prawda? chyba nie. Ale to już dyskusja jaki algorytm jest lepszy. Teraz kolegi jest lepszy bo działa, a ja jeszcze nie zrobiłem serwa więc mój jest gorszy.

[ursus_arctos]

Jeżeli pomiędzy zderzeniami ( sygnałami A,B) przeliczamy filtr, to na jego wejściu musimy dać dane z palca

Właściwie, to z palca jest tylko prędkość - i tego "palca" dotyczy ten wątek - uchyb (z dokładnością do rozdzielczości enkodera) przecież znamy. Swoją drogą, to uchyb nie musi wcale być stały - przecież zadana pozycja też może ulegać zmianom w czasie - maszyna ma się poruszać po jakiejś krzywej, to punkt zadany jest zmienny. Ja zamierzam utrzymać krok całkowania krzywej poniżej okresu PWM - u mnie jest to coś rzędu 30µs.

[piotr_olbrysz]

Właściwie, to z palca jest tylko prędkość

Przy małych prędkościach, tzn. gdy odległość czasowa pomiedzy znacznikami enkodera jest znacznie wieksza niż czestość próbkowania to błąd wyznaczania predkosci jest pomijalnie mały, natomiast przy dużych prędkościach to okres 30us chyba już jest porównywalny z okresem sygnału A lub B.

[ursus_arctos]

czasowa pomiedzy znacznikami enkodera jest znacznie wieksza niż czestość próbkowania to błąd wyznaczania predkosci jest pomijalnie mały

Właśnie wtedy błąd jest olbrzymi Przy trzymaniu stałej pozycji między impulsami mogą mijać nawet sekundy - nikt nie wie, co się wówczas napradę dzieje z silnikiem. Natomiast jak enkoder daje impulsy 10kHz, to już w ciągu 1ms mamy aż 10 próbek, z których można liczyć prędkość.
No nic, zaczynami implementację...