Was sind Methoden zum Umgang mit Kompassverzögerung (ratenabhängige Hysterese)?

Ich habe einen Roboter mit Profilantrieb, mit Radcodierern geringer Präzision für die Verfolgung der Entfernung und einem elektronischen Kompass für die Bestimmung der Fahrtrichtung. Der Kompass hat eine erhebliche Verzögerung (> 1 Sekunde), wenn sich der Roboter schnell dreht, z. B. nach Erreichen eines Wegpunkts.

Wie kann man mit dem Lag umgehen? Ich würde denken, man könnte viele Messungen durchführen und die Kompassantwort modellieren. Dies scheint jedoch problematisch zu sein, da es ratenabhängig ist und ich die momentane Rate nicht kenne.

Als einfache, aber langsame Annäherung lasse ich den Roboter drehen, bis er sehr grob in die richtige Richtung zeigt, und mache dann sehr kleine inkrementelle Drehungen mit kurzen Messpausen, bis er in die richtige Richtung zeigt. Gibt es andere Möglichkeiten, damit umzugehen?

sensors compass

— ViennaMike
quelle

Die Verzögerung im Kompass beruht auf einem Tiefpassfilter, um hochfrequentes Rauschen zu unterdrücken.

Es gibt teurere Magnetometer, die weniger Rauschen und daher weniger Verzögerung aufweisen.
Es ist auch möglich, ein Gyroskop zu verwenden, um die Genauigkeit zu verbessern. Genau das tun Inertial Measurement Units (IMUs). Dies kann durch Verwendung eines Kalman-Filters erreicht werden. Die Verbesserung der Genauigkeit trägt zur Verringerung der Verzögerung bei, da eine höhere Genauigkeit die Abhängigkeit von einem Tiefpassfilter zur Unterdrückung von Rauschen verringert. Der Kalman-Filter verschmilzt die Daten des Magnetometers und des Gyroskops (das die Geschwindigkeit der Kursänderung misst).

Wenn Sie bei Ihrem aktuellen Kompass bleiben, gibt es zwei mögliche Lösungen (Achtung, dies wird immer fortgeschrittener, aber Option 1 sollte für die meisten Menschen ohne zu viel Arbeit zugänglich sein).

Sie können versuchen, den Filter aufzuheben. Dies kann Verzögerungen beseitigen, erhöht aber auch das Hochfrequenzrauschen. Anschließend können Sie versuchen, den Roboter anhand der neuen Steuerkursschätzung zu steuern. Dazu müssen Sie experimentieren, um die Tiefpassfilterparameter zu ermitteln. In diskreter Zeit könnten Sie beispielsweise Folgendes finden:

$\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)$ wobei die geschätzte Richtung (Kompassausgabe) bei ist Zeit , ist die tatsächliche Richtung (Grundwahrheit) zur Zeit . $\hat\theta(t)$ $t$ $\theta$ $t$
Sie können die Parameter indem Sie ein Experiment durchführen, bei dem Sie die Grundwahrheit mit anderen externen Mitteln messen. Bei Abtastwerten haben Sie folgende Gleichung: $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]$
Und Sie können lösen, indem Sie finden: wobei die pseudoinverse Matrix von . Es gibt keinen definitiven Weg, um zu , also werden Sie wahrscheinlich nur raten. Bei Bonuspunkten wird davon ausgegangen, dass das Rauschen weiß und unabhängig ist. Sie können es jedoch zuerst aufhellen, um die Verzerrung zu beseitigen und die Schätzung der Parameter zu verbessern.
$[\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$
Sie können dies in eine Übertragungsfunktion umwandeln (im diskreten Zeitbereich auch als Z-Transformation bezeichnet):

$\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}$
Um dies aufzuheben, können wir die Umkehrung nehmen (wobei unsere neue Schätzung der Überschrift ist): $\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}}$
Zurück in die Zeitdomäne konvertieren:

$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}$
Wir können dann , um den Roboter zu steuern. $\bar\theta$

Dies ist sehr laut, daher sollten Sie vor der Verwendung möglicherweise noch durch einen Tiefpassfilter leiten (obwohl möglicherweise einer mit geringerer Verzögerung). $\bar\theta$
Die obige Lösung ist immer noch nicht der beste Weg. Die Schätzung des Rauschens ist möglicherweise nicht sehr nützlich. Wenn wir dies in eine Zustandsraumgleichung setzen, können wir ein Kalman-Filter und einen Vollzustands-Rückkopplungsregler unter Verwendung von LQR (linearer quadratischer Regler) entwerfen. Die Kombination aus einem Kalman-Filter und einem LQR-Regler wird auch als LQG-Regler (lineares quadratisches Gauß) bezeichnet und verwendet die Rückgewinnung durch Schleifenübertragung, um einen guten Regler zu erhalten.

Überlegen Sie sich dazu die (zeitdiskreten) Zustandsraumgleichungen:

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

oder:
$\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)$
Dabei steht für die Leistung der Motoren zum Drehen des Roboters und , , , Auswirkung auf die Fahrtrichtung auf der Grundlage von Position und Geschwindigkeit (Sie können Werte ungleich Null auswählen für die anderen Elemente der Matrix und auch für die erste Zeile der Matrix). $\vec{u}(t-1)$ $A_0$ $A_1$ $B_0$ $B_1$ $B$ $A$

Anschließend können Sie Ihren Beobachter (Kalman-Filter) , indem Sie die und für das Prozessrauschen und das . Der Kalman-Filter kann dann die optimale Kursschätzung unter Berücksichtigung dieser Annahmen über das Rauschen finden. Nach der Auswahl der Rauschschätzungen hängt die Implementierung nur von der Implementierung von Code für den Kalman-Filter ab (Gleichungen sind auf Wikipedia verfügbar, daher werde ich hier nicht darauf eingehen). $Q_o$ $R_o$

Danach können Sie einen LQR Controller - Design, dieses Mal, die Wahl und die Gewichtung darstellt , um die Regelung der Überschrift gegeben, und zu versuchen , die Verwendung der Aktoren zu begrenzen. In diesem Fall können Sie und . Dies geschieht, weil LQR den optimalen Controller findet, um eine Kostenfunktion zu minimieren: $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})}$

Dann setzen Sie es einfach durch die diskrete zeitalgebraische Riccati-Gleichung:

$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$
und löst für eine positiv definite Matrix . $P$

So kann Ihr Kontrollgesetz gegeben sein durch:

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

Letztendlich funktioniert dies nicht sehr gut und ist wahrscheinlich aufgrund des Rauschens instabil. Dies bedeutet in der Tat, dass Option 1 wahrscheinlich erst funktioniert, wenn Sie durch einen Tiefpassfilter geschickt haben (allerdings nicht unbedingt mit einer so langen effektiven Verzögerungszeit). Dies liegt daran, dass LQR zwar garantiert stabil ist, die Garantie jedoch verloren geht, sobald Sie einen Kalman-Filter verwenden. $\bar\theta$

Um dies zu beheben, verwenden wir die Loop Transfer Recovery-Technik, bei der Sie den Kalman-Filter anpassen und stattdessen ein neues wählen , wobei Ihre ursprüngliche Matrix ist und so abgestimmt ist, dass der Kalman-Filter optimal ist . ist eine beliebige positiv definierte symmetrische Matrix, die Sie einfach als Identitätsmatrix auswählen können ( ). Dann wählen Sie einfach einen Skalar . Der resultierende Controller sollte (stabiler) werden als , obwohl die Matrix verstimmt wird, was bedeutet, dass sie weniger optimal ist. $Q_o = Q_0 + q^2BVB^T$ $Q_0$ $Q$ $V$ $V=I$ $q$ $q \rightarrow \infty$ $Q_o$

Erhöhen Sie daher einfach bis es stabil ist. Eine weitere Möglichkeit , die Sie versuchen können , um es stabil zu machen, ist zu erhöhen (oder verringern ) die LQR Controller langsamer zu machen. $q$ $R_c$ $Q_c$

Die Konzepte in diesem Beitrag sind noch recht weit fortgeschritten. Wenn Sie jedoch Dinge wie die Riccati-Gleichung lösen müssen, können Sie MATLAB oder eine andere Software verwenden, um dies zu tun. Möglicherweise gibt es auch Bibliotheken, die den Kalman-Filter bereits implementieren (ich glaube, MATLAB tut dies auch).

Für eine eingebettete Anwendung müssen Sie den Kalman-Filter möglicherweise selbst implementieren, obwohl es wahrscheinlich eine C-Implementierung gibt.

— ronalchn
quelle

Vielen Dank für Ihre ausgezeichnete und ausführliche Antwort. Ich folge dem Kern Ihrer ersten Lösung und bin mir sicher, dass ich sie durcharbeiten kann. Der zweite ist, wie Sie sagen, herausfordernder und ich muss arbeiten, um zu sehen, ob ich ihm alles folgen kann.

— ViennaMike

Ein Kreisel ist die einfache Antwort. Ich habe immer gehört, Gyro für die kurzen Maße, Kompass für die langen. Und realistisch eine Tasse Kallman-Filter zwischen den beiden die meiste Zeit. Der Preis für ein 6DOF-Gyro / acc-Board liegt heutzutage unter 20 US-Dollar, viel zu billig, um eines nicht zu verwenden.

Einmal habe ich den Kallman-Filter einer anderen Person durchgearbeitet . und habe es funktioniert. Ein Kallman-Filter ist eigentlich eher ein Ansatz, keine exakte Implementierung, und im Fall eines Kreisels muss für das Endergebnis keine Matrixmathematik verwendet werden. Dies vereinfacht den Code erheblich.

— Spiked3
quelle