Hallo,
ich bin gerade an einem regelungstechnischen Projekt, in dem ich einen
Lego-Roboter auf 2 Rädern stabilisiere. Dazu wird ein Raspberry Pi mit
Lego-Sensor/Aktuator-Interface verwendet.
Für die Messung des Winkels wird ein Lego Gyro-Sensor verwendet, der
eine Sampling-Rate von 1 kHz und eine WInkelauflösung von 1° hat.
Der Sensor gibt neben dem absoluten Winkel auch die
Winkelgeschwindigkeit aus, welche beide auch als Zustände definiert
sind.
Ich habe nun das Problem, dass die Geschwindigkeitswerte sehr stark
verrauscht sind. Bisher habe ich es so gehandhabt, dass ich den
Differenzenquotienten der aboluten Winkeländerungen durch die
Programmlaufzeit berechnet habe und darüber noch mal einen gleitenden
Mittelwert gebildet habe (da die Auflösung sehr schlecht und die Werte
daher groß sind). Also im Prinzip so (mit Python):
1 | import time
|
2 | from collections import deque
|
3 |
|
4 | winkelwert_alt=0
|
5 | liste=deque(maxlen=21)
|
6 | while True:
|
7 | start=time.time
|
8 |
|
9 | s=(winkelwert_aktuell - winkelwert_alt)/abtastzeit #Differenzenquotient
|
10 | winkelwert_alt=winkelrwert_aktuell
|
11 | liste.append(s) #Erweitere Liste
|
12 |
|
13 | winkel_ableitung= sum(liste)/len(liste) #gleitender Mittelwert
|
14 |
|
15 | ende=time.time
|
16 |
|
17 | if (ende-start)<0.01:
|
18 | time.sleep(0.01-(ende-start)) # feste Schleifenlaufzeit von 10 ms
|
Also ich bilde pro Schleifenschritt, der mit einer festen Abtastzeit von
10 ms erfolgt den gleitenden Mittelwert über 21 Änderungswerte.
Das läuft so eigentlich auch ganz gut, ich habe aber das Problem, dass
sich der Roboter nach einer längeren Zeit der Stabilisierung wieder
selbst hochschaukelt und die Ableitungswerte schleichend wieder größer
werden.
Ich möchte daher nun innerhalb der Schleife eine weitere Schleife
realisieren, in der der Sensor mehrmals ausgelesen wird und somit mehr
Werte pro Schritt zur Verfügung stehen.
Habt ihr noch bessere Vorschläge?