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:
θ (t)tθt
θ^(t)=a0θ(t)+a1θ(t−1)+⋯+akθ(t−k)
wobei die geschätzte Richtung (Kompassausgabe) bei ist Zeit , ist die tatsächliche Richtung (Grundwahrheit) zur Zeit .θ^(t)tθ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:
ain+k+1
⎡⎣⎢⎢⎢θ^(k)⋮θ^(k+n)⎤⎦⎥⎥⎥=⎡⎣⎢⎢θ(k)⋮θ(k+n)θ(k−1)⋮θ(k+n−1)⋯⋯θ(0)⋮θ(n)⎤⎦⎥⎥⎡⎣⎢⎢⎢⎢a0a1⋮ak⎤⎦⎥⎥⎥⎥
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.M+Mk
⎡⎣⎢⎢⎢⎢a0a1⋮ak⎤⎦⎥⎥⎥⎥=⎡⎣⎢⎢θ(k)⋮θ(k+n)θ(k−1)⋮θ(k+n−1)⋯⋯θ(0)⋮θ(n)⎤⎦⎥⎥+⎡⎣⎢⎢⎢θ^(k)⋮θ^(k+n)⎤⎦⎥⎥⎥
M+Mk
Sie können dies in eine Übertragungsfunktion umwandeln (im diskreten Zeitbereich auch als Z-Transformation bezeichnet):
Θ^(z)Θ(z)=a0+a1z−1+...+akz−k
Um dies aufzuheben, können wir die Umkehrung nehmen (wobei unsere neue Schätzung der Überschrift ist):θ¯(t)
Θ¯(z)Θ^(z)=1a0+a1z−1+⋯+akz−k
Zurück in die Zeitdomäne konvertieren:
a0θ¯(t)+a1θ¯(t−1)+⋯+akθ¯(t−k)=θ^(t)
θ¯(t)=θ^(t)−a1θ¯(t−1)−⋯−akθ¯(t−k)a0
Wir können dann , um den Roboter zu steuern.θ¯
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).θ¯
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:
x⃗ (t)=Ax⃗ (t−1)+Bu⃗ (t−1) ,y⃗ (t)=Cx⃗ (t)
oder:
x⃗ (t)=⎡⎣⎢⎢⎢⎢θ(t)θ(t−1)⋯θ(t−k)⎤⎦⎥⎥⎥⎥=⎡⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢A110⋮00A201⋮00⋯⋯⋯⋯⋯000⋮10000⋮01000⋮00⎤⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥x⃗ (t−1)+⎡⎣⎢⎢⎢⎢⎢⎢⎢⎢⎢B0B10⋮00⎤⎦⎥⎥⎥⎥⎥⎥⎥⎥⎥u⃗ (t−1)
y⃗ (t)=[θ^(t)]=⎡⎣⎢⎢⎢⎢a0a1⋮ak⎤⎦⎥⎥⎥⎥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).u⃗ (t−1)A0A1B0B1BA
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).QoRo
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:QcRcQc=⎡⎣⎢⎢⎢⎢10⋮000⋮000⋮0⋯⋯⋯00⋮0⎤⎦⎥⎥⎥⎥Rc=[1]J=∑(x⃗ TQx⃗ +u⃗ TRu⃗ )
Dann setzen Sie es einfach durch die diskrete zeitalgebraische Riccati-Gleichung:
P=Q+AT(P−PB(R+BTPB)−1BTP)A
und löst für eine positiv definite Matrix .P
So kann Ihr Kontrollgesetz gegeben sein durch:
u⃗ (t)=−K(x⃗ (t)−x⃗ ref(t))
wobeiK=(R+BTPB)−1(BTPA)
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.θ¯
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.Qo=Q0+q2BVBTQ0QVV=Iqq→∞Qo
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.qRcQc
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.
quelle