Jakie są metody radzenia sobie z opóźnieniem kompasu (histereza zależna od prędkości)?

Mam robota napędzanego bieżnikiem, z enkoderami kół o niskiej precyzji do śledzenia odległości i elektronicznym kompasem do określania kierunku. Kompas ma znaczne (> 1 sekundę) opóźnienie, gdy robot obraca się szybko, np. Po osiągnięciu punktu trasy - obraca się w miejscu, aby wskazać nowy kurs.

Jakie są sposoby radzenia sobie z opóźnieniem? Wydaje mi się, że można wykonać wiele pomiarów i modelować odpowiedź kompasu. Wydaje się to jednak problematyczne, ponieważ jest zależne od stawki i nie znam stawki chwilowej.

Jako proste, ale powolne podejście, mam obrót robota, aż zostanie on z grubsza skierowany we właściwym kierunku, a następnie wykonuję bardzo małe skręty przyrostowe z krótkimi przerwami pomiaru, aż zostanie skierowany we właściwą stronę. Czy istnieją inne sposoby radzenia sobie z tym?

sensors compass

— ViennaMike
źródło

Opóźnienie w kompasie wynika z filtra dolnoprzepustowego, który tłumi szumy o wysokiej częstotliwości.

Istnieją droższe magnetometry, które mają mniejszy hałas, a zatem mniejsze opóźnienie.
Możliwe jest również użycie żyroskopu w celu poprawy dokładności. W rzeczywistości to właśnie robią jednostki pomiaru bezwładnościowego (IMU). Można to osiągnąć za pomocą filtra Kalmana. Poprawa dokładności pomaga zmniejszyć opóźnienie, ponieważ zwiększona dokładność zmniejsza zależność od filtra dolnoprzepustowego w celu tłumienia hałasu. Filtr Kalmana łączy dane z magnetometru, a także żyroskopu (który mierzy szybkość zmian kursu).

Jeśli trzymasz się obecnego kompasu, istnieją dwa możliwe rozwiązania (uwaga, to staje się coraz bardziej zaawansowane, ale opcja 1 powinna być dostępna dla większości ludzi bez zbytniej pracy).

Możesz spróbować anulować filtr. Może to usunąć opóźnienie, ale także zwiększa szum o wysokiej częstotliwości. Po wykonaniu tej czynności możesz spróbować sterować robotem w oparciu o nowe oszacowanie kursu. Aby to zrobić, musisz poeksperymentować z parametrami filtra dolnoprzepustowego. Na przykład w dyskretnym czasie możesz znaleźć:

$\hat{θ} (t) = a_{0} θ (t) + a_{1} θ (t - 1) + \dots + a_{k} θ (t - k)$ $\hat\theta(t)=a_0\theta(t)+a_1\theta(t-1)+\cdots+a_k\theta(t-k)$ gdzie to szacowany kurs (wyjście kompasu) na czas , to rzeczywisty kurs (podstawowa prawda) w czasie . $\hat\theta(t)$ $t$ $\theta$ $t$
Możesz znaleźć parametry , wykonując eksperyment, w którym mierzysz prawdę gruntową za pomocą innych środków zewnętrznych. Biorąc pod uwagę próbek, masz następujące równanie: $a_i$ $n+k+1$
$[\begin{matrix} \hat{θ} (k) \\ ⋮ \\ \hat{θ} (k + n) \end{matrix}] = [\begin{matrix} θ (k) & θ (k - 1) & \dots & θ (0) \\ ⋮ & ⋮ & ⋮ \\ θ (k + n) & θ (k + n - 1) & \dots & θ (n) \end{matrix}] [\begin{matrix} a_{0} \\ a_{1} \\ ⋮ \\ a_{k} \end{matrix}]$ $\left[\matrix{\hat\theta(k)\\\vdots\\\hat\theta(k+n)}\right]=\left[\matrix{\theta(k)&\theta(k-1)&\cdots&\theta(0)\\\vdots&\vdots&&\vdots\\\theta(k+n)&\theta(k+n-1)&\cdots&\theta(n)}\right]\left[\matrix{a_0\\a_1\\\vdots\\a_k}\right]$
I możesz rozwiązać, znajdując: gdzie jest pseudo-inwerso macierz . Nie ma ostatecznego sposobu na wypracowanie , więc prawdopodobnie zgadniesz. W przypadku punktów bonusowych zakłada się, że hałas jest biały i niezależny, ale możesz go najpierw wybielić, aby usunąć błąd, a tym samym poprawić swoje oszacowanie parametrów.
$[\begin{matrix} a_{0} \\ a_{1} \\ ⋮ \\ a_{k} \end{matrix}] = {[\begin{matrix} θ (k) & θ (k - 1) & \dots & θ (0) \\ ⋮ & ⋮ & ⋮ \\ θ (k + n) & θ (k + n - 1) & \dots & θ (n) \end{matrix}]}^{+} [\begin{matrix} \hat{θ} (k) \\ ⋮ \\ \hat{θ} (k + n) \end{matrix}]$ $\left[\matrix{a_0\\a_1\\\vdots\\a_k}\right]=\left[\matrix{\theta(k)&\theta(k-1)&\cdots&\theta(0)\\\vdots&\vdots&&\vdots\\\theta(k+n)&\theta(k+n-1)&\cdots&\theta(n)}\right]^{+}\left[\matrix{\hat\theta(k)\\\vdots\\\hat\theta(k+n)}\right]$ $M^+$ $M$ $k$
Możesz przekonwertować to na funkcję przesyłania (znaną również jako transformacja Z w dyskretnej dziedzinie czasu):

$\frac{\hat{Θ} (z)}{Θ (z)} = a_{0} + a_{1} z^{- 1} + . . . + a_{k} z^{- k}$ $\frac{\hat\Theta(z)}{\Theta(z)}=a_0+a_1 z^{-1}+...+a_k z^{-k}$
Aby to anulować, możemy przyjąć odwrotność (gdzie jest naszym nowym oszacowaniem kursu): $\bar\theta(t)$

$\frac{\bar{Θ} (z)}{\hat{Θ} (z)} = \frac{1}{a_{0} + a_{1} z^{- 1} + \dots + a_{k} z^{- k}}$ $\frac{\bar\Theta(z)}{\hat\Theta(z)}=\frac{1}{a_0+a_1 z^{-1}+\cdots+a_k z^{-k}}$
Konwertowanie z powrotem do dziedziny czasu:

$a_{0} \bar{θ} (t) + a_{1} \bar{θ} (t - 1) + \dots + a_{k} \bar{θ} (t - k) = \hat{θ} (t)$ $a_0\bar\theta(t)+a_1\bar\theta(t-1)+\cdots+a_k \bar\theta(t-k)=\hat\theta(t)$
$\bar{θ} (t) = \frac{\hat{θ} (t) - a_{1} \bar{θ} (t - 1) - \dots - a_{k} \bar{θ} (t - k)}{a_{0}}$ $\bar\theta(t)=\frac{\hat\theta(t)-a_1\bar\theta(t-1)-\cdots-a_k \bar\theta(t-k)}{a_0}$
możemy następnie użyć do sterowania robotem. $\bar\theta$

Będzie to bardzo hałaśliwe, więc przed użyciem możesz nadal przefiltrować przez filtr dolnoprzepustowy (chociaż być może taki, który ma mniejsze opóźnienie). $\bar\theta$
Powyższe rozwiązanie wciąż nie jest najlepszym sposobem. Głośne oszacowanie może nie być bardzo przydatne. Jeśli umieścimy to w równaniu przestrzeni stanu, możemy zaprojektować filtr Kalmana i kontroler sprzężenia zwrotnego w pełnym stanie za pomocą LQR (liniowy regulator kwadratowy). Kombinacja filtra Kalmana i kontrolera LQR jest również znana jako kontroler LQG (liniowy kwadratowy gaussian), a do odzyskania dobrego kontrolera wykorzystuje się odzyskiwanie w pętli.

Aby to zrobić, wymyśl równania przestrzeni czasowej (dyskretne):

$\vec{x}(t)=A\vec{x}(t-1)+B\vec{u}(t-1)$ , $\vec{y}(t)=C\vec{x}(t)$

lub:
$\vec{x} (t) = [\begin{matrix} θ (t) \\ θ (t - 1) \\ \dots \\ θ (t - k) \end{matrix}] = [\begin{matrix} A_{1} & A_{2} & \dots & 0 & 0 & 0 \\ 1 & 0 & \dots & 0 & 0 & 0 \\ 0 & 1 & \dots & 0 & 0 & 0 \\ ⋮ & ⋮ & ⋮ & ⋮ & ⋮ \\ 0 & 0 & \dots & 1 & 0 & 0 \\ 0 & 0 & \dots & 0 & 1 & 0 \end{matrix}] \vec{x} (t - 1) + [\begin{matrix} B_{0} \\ B_{1} \\ 0 \\ ⋮ \\ 0 \\ 0 \end{matrix}] \vec{u} (t - 1)$ $\vec{x}(t)=\left[\matrix{\theta(t)\\\theta(t-1)\\\cdots\\\theta(t-k)}\right]= \left[\matrix{ A_1&A_2&\cdots&0&0&0\\ 1&0&\cdots&0&0&0\\ 0&1&\cdots&0&0&0\\ \vdots&\vdots&&\vdots&\vdots&\vdots\\ 0&0&\cdots&1&0&0\\ 0&0&\cdots&0&1&0 }\right] \vec{x}(t-1) + \left[\matrix{B_0\\B_1\\0\\\vdots\\0\\0}\right]\vec{u}(t-1)$
$\vec{y} (t) = [\begin{matrix} \hat{θ} (t) \end{matrix}] = [\begin{matrix} a_{0} \\ a_{1} \\ ⋮ \\ a_{k} \end{matrix}] \vec{x} (t)$ $\vec{y}(t)=\left[\matrix{\hat\theta(t)}\right]=\left[\matrix{a_0\\a_1\\\vdots\\a_k}\right]\vec{x}(t)$
gdzie reprezentuje moc w silnikach do obracania robota, a , , , to wpływ, jaki wpływa na kurs w oparciu o pozycję i prędkość (możesz wybrać wartości niezerowe dla innych elementów macierzy , a także pierwszego rzędu macierzy ). $\vec{u}(t-1)$ $A_0$ $A_1$ $B_0$ $B_1$ $B$ $A$

Następnie możesz zbudować swojego obserwatora (Filtr Kalmana), wybierając oszacowania hałasu i dla szumu procesowego i szumu pomiarowego. Filtr Kalmana może następnie znaleźć optymalne oszacowanie kursu, biorąc pod uwagę te założenia dotyczące hałasu. Po wybraniu oszacowań hałasu, implementacja zależy tylko od implementacji kodu dla filtru Kalmana (równania można znaleźć na Wikipedii, więc nie będę tutaj go omawiać). $Q_o$ $R_o$

Następnie możesz zaprojektować kontroler , tym razem wybierając i reprezentujące wagę przypisaną do regulacji i próbując ograniczyć użycie siłowników. W takim przypadku możesz wybrać i . Dzieje się tak, ponieważ LQR znajduje optymalny kontroler, aby zminimalizować funkcję kosztów: $Q_c$ $R_c$ $Q_c = \left[\matrix{1&0&0&\cdots&0\\0&0&0&\cdots&0\\\vdots&\vdots&\vdots&&\vdots\\0&0&0&\cdots&0}\right]$ $R_c = \left[1\right]$ $J = \sum{(\vec{x}^T Q\vec{x} + \vec{u}^T R \vec{u})}$

Następnie po prostu przeprowadźmy przez algebraiczne równanie Riccati z czasem dyskretnym:

$P = Q + A^{T} (P - P B {(R + B^{T} P B)}^{- 1} B^{T} P) A$ $P = Q + A^T \left( P - P B \left( R + B^T P B \right)^{-1} B^T P \right) A$
i rozwiązania dla pozytywnej określonej macierzy . $P$

Zatem twoje prawo kontrolne może zostać wydane przez:

$\vec{u} (t) = - K (\vec{x} (t) - {\vec{x}}_{r e f} (t))$ $\vec{u}(t)=-K(\vec{x}(t)-\vec{x}_{ref}(t))$
gdzie $K = (R + B^T P B)^{-1}(B^T P A)$

Wreszcie, samo wykonanie tego nie będzie działać zbyt dobrze i prawdopodobnie będzie niestabilne z powodu hałasu. Rzeczywiście, oznacza to, że opcja 1 prawdopodobnie nie zadziała, dopóki najpierw nie przełączysz przez filtr dolnoprzepustowy (choć niekoniecznie przy tak długim efektywnym czasie opóźnienia). Wynika to z faktu, że chociaż LQR ma zagwarantowaną stabilność, to natychmiast po użyciu filtra Kalmana gwarancja zostaje utracona. $\bar\theta$

Aby to naprawić, wykorzystujemy technikę Loop Transfer Recovery, w której dostosowujesz filtr Kalmana, a zamiast tego wybierasz nową , gdzie to twoja pierwotna macierz , dostrojona tak, aby filtr Kalmana był optymalny . to dowolna dodatnio określona macierz symetryczna, którą można po prostu wybrać jako macierz tożsamości ( ). Następnie wybierz skalar . Wynikowy kontroler powinien stać się (bardziej) stabilny jako , chociaż matryca zostaje dostrojona, co oznacza, że staje się mniej optymalna. $Q_o = Q_0 + q^2BVB^T$ $Q_0$ $Q$ $V$ $V=I$ $q$ $q \rightarrow \infty$ $Q_o$

Dlatego po prostu zwiększaj aż będzie stabilny. Innym sposobem, aby spróbować go ustabilizować, jest zwiększenie (lub zmniejszenie ), aby spowolnić kontroler . $q$ $R_c$ $Q_c$

Pojęcia zawarte w tym poście są dość zaawansowane, ale jeśli potrzebujesz rozwiązać takie rzeczy jak równanie Riccati, możesz użyć MATLAB lub innego oprogramowania, aby to zrobić. Być może biblioteki już implementują filtr Kalmana (ponownie, sądzę, że MATLAB również to robi).

W przypadku aplikacji osadzonej może być konieczne samodzielne wdrożenie filtru Kalmana, chociaż prawdopodobnie istnieje implementacja C.

— ronalchn
źródło

Dziękuję za doskonałą i dogłębną odpowiedź. Podążam za sednem twojego pierwszego rozwiązania i jestem pewien, że dam radę. Drugi, jak mówisz, jest trudniejszy i będę musiał pracować, aby sprawdzić, czy mogę to wszystko wykonać.

— ViennaMike,

Żyroskop to prosta odpowiedź. Zawsze słyszałem, żyroskop dla krótkich pomiarów, kompas na długo. I realistycznie filiżanka Kallmana filtruje między nimi przez większość czasu. Cena płyty żyroskopowej 6DOF jest obecnie mniejsza niż 20 USD, zdecydowanie zbyt tania, aby z niej nie korzystać.

Kiedyś pracowałem przez filtr Kallmana innej osoby . i działało. Filtr Kallmana jest w rzeczywistości bardziej podejściem, a nie dokładną implementacją, aw przypadku żyroskopu wynik końcowy nie wymaga użycia matematyki matematycznej. To sprawia, że kod jest znacznie prostszy.

— Kolczasty 3
źródło