Von der Theorie auf die Rennstrecke: Kalman-Filter für die Antriebsschlupfregelung

Wie schon viele von euch habe ich mich im Rahmen meines eigenen Projektes mit dem Kalmanfilter beschäftigt und dabei viel von den Blogeinträgen KF 1/KF 2/EKF gelernt. Da ich auch aufgrund des Standortvorteils direkt mit Paul über die Gestaltung fachsimpeln konnte, ist zwischen uns die Idee enstanden euch meine Umsetzung nicht vorzuenhalten. Deswegen möchte ich im Folgenden genauer auf die Bedatung der verschiedenen Matrizen, aber weniger auf die Berechnungen eingehen. Dafür sind ja schon die genannten Einträgen da.

Das Projekt: SophE

Mein Projekt hört auf den Namen Formula Student und ist der größte studentische Konstruktionswettbewerb weltweit (Formula Student/Formula Student Germany). Ziel jedes teilnehmenden Teams ist es unter Betrachtung wirtschaftlicher und technischer Gesichtspunkte innerhalb eines Jahres ein einsitziges Rennfahrzeug mit offenem Cockpit zu entwickeln.

Für die Fahrdynamikregelung unserer SophE (das 2016er Fahrzeug von meinem Team Elbflorace) ist die Längsgeschwindigkeit von enormer Bedeutung. Das Fahrzeugkonzept eines Allradfahrzeuges mit elektrischen Radnabenmotoren macht deren Bestimmung definitv zu einer interessanten Aufgabe. Beim Beschleunigen erzeugen die Elektromotoren ein so enormes Drehmoment, dass die Räder massiven Längsschlupf erleiden. Dabei reduziert sich sowohl das maximale Beschleunigungsvermögen als auch die Querstabilität. Der Längsschlupf \(\lambda\) bestimmt sich aus der Radumfangsgeschwindigkeit \(v_R\) und der tatsächlichen Fahrzeuglängsgeschwindigkeit \(v_F\):

\[\lambda = \frac{v_R – v_F}{v_F}\]

Zur Bestimmung der Fahrzeug-Längsgeschwindigkeit \(v_F\) stehen verschieden Sensoren zur Verfügung. Allerdings kann keiner das liefern, was eine Fahrdynamikregelung benötigt: ein möglichst rauschfreies, exaktes Signal mit hoher Updatefrequenz.

GPS	IMU	Raddrehzahlsensoren
stellt über den Dopplereffekt eine genau Messung der absoluten Geschwindigkeit zur Verfügung	misst die Beschleunigung und kann über die Integration kurzfristig Geschwindigkeitsverläufe mit hoher Dynamik darstellen	geben genauen und hochfrequenten Aufschluss über die aktuelle Umfangsgeschwindigkeit eines jeden Rades
ABER: es ist verzögert (in unserem Fall ca 250 ms) und hat eine zu geringe Updaterate (5 Hz)	ABER: bei jeder Integration kommt es zu einem Driftfehler, was die von der IMU ermittelte Geschwindigkeit mittelfristig unbrauchbar macht	ABER: aufgrund der Natur der Kraftübertragung von Reifen und Strecke, kann die Umfangsgeschwindigkeit bei stärkeren Schlupf signifikant vom Geschwindigkeitsvektor der Radnabe abweichen. Außerdem soll die Raddrehzahl für die Traktionskontrolle als Regelgröße herhalten. Wie war das gleich noch mit dem Hund, der seinen eigenen Schwanz jagt?

 

Gibt es nicht noch andere Möglichkeiten? Ja, es käme noch die Verwendung von optischer Sensorik, wie dem Correvit in Frage. Dieser ist jedoch deutlich schwerer und größer als alle anderen Sensoren zusammen, was zu Packaging-Problemen führt. Und zu guter Letzt lässt ein fünfstelliges Preisschild  fragen, ob es nicht doch anders geht. Ähnliches gilt für Radarsensoren die mittels Dopplereffekt den Boden abtasten.

Ich entscheide mich also die vorhanden Sensoren zu nutzen. Ich muss ja nur die Vorteile aller Sensoren kombinieren und in ein verwendbares Signal umwandeln. An der Stelle kommt der Kalmanfilter mit Sensorfusion und Filterfunktion ins Spiel.

Kalman-Filter zur Sensordatenfusion und Schätzung der Fahrzeuggeschwindigkeit

Zustandsraumvektor

Zu Beginn einer jeden Kalmanfilterkonzeptionierung steht die Frage, welche Zustandswerte gefiltert werden. Für unsere Fahrdynamikregelung interessieren ich mich für die Fahrzeuglängsgeschwindigkeit. Aufgrund der hohen Dynamik des Fahrzeugverhaltens erweitere ich diese um die Längsbeschleunigung.   Damit ergibt sich der Zustandsraumvektor \(x\) aus der Längsgeschwindigkeit \(v\) und der Längsbeschleunigung \(a_x\) für jeden Zeitschritt \(k\)

\[x_k = \begin{bmatrix} v \\ a_x \end{bmatrix}_k\]

Da ich weiß, dass das Fahrzeug beim Start des Filters immer still steht kann ich den Zustandsraum wie folgt initialiseren.

\[x_{0} = \begin{bmatrix} 0 \\ 0 \end{bmatrix}\]

Prognose Zustandsraum

Die Prognose des Zustandsraums und der Fehlerkovarianz folgen den bereits in den anderen Einträgen diskutierten mathematischen Formulierungen. Zur Modellierung des Fahrzeugverhaltens dient das physikalische Modell der gleichmäßig beschleunigten Geradeausfahrt. Dies führt zwar in Kurven zu Abweichungen kann aber als hinreichend genau angesehen werden. Mit dem Zeitdifferenz zwischen zwei Berechnungsschritten\(\Delta t\)  und den Annahmen

\[v_k = v_{k-1} + \Delta t \, a_{x_{k-1}}\]

und

\[a_{x_k} = a_{x_{k-1}}\]

ergibt sich die Dynamikmatrix \(A\) und die Zustandsraumprognose \( x_{k_{Prognose}}\) zu

\[A = \begin{bmatrix} 1 & \Delta t \\ 0 & 1 \end{bmatrix}\]

\[x_{k_{Prognose}} = \begin{bmatrix} v \\ a_x \end{bmatrix}_{k_{Prognose}} = \begin{bmatrix} 1 & \Delta t \\ 0 & 1 \end{bmatrix} \begin{bmatrix} v \\ a_x \end{bmatrix}_{k-1}\]

Prognose Fehlerkovarianz

Die Fehlerkovarianzmatrix \(P\) berechnet sich aus der Dynamikmatrix und der Prozessrauschkovarianzmatrix \(Q\).

\[P_{k_{Prognose}} = A \, P_{k-1} \, A^T + Q  \]

mit der Prozessunsicherheit \(\sigma_{P}\)  und

\[ Q = \begin{bmatrix} \sigma_{P_{11}} & 0 \\
0 & \sigma_{P_{22}} \\\end{bmatrix}\]

Laut der Literatur lässt sich \(Q\) mathematisch oder aber experimentell ermitteln.
Ich habe mich für den experimentellen Anzatz entschieden und damit auch gleich sichergestellt, dass das Zusammenspiel von \(Q\) und der Messrauschkovarianzmatrix \(R\) meiner Vorstellung von Dynamik und Filterwirkung entspricht. Da das Fahrzeug nach dem Start der Filterlaufzeit immer noch einige Sekunden still steht kann ich \(P\) mit hohen Zahlenwerten, also hoher Unsicherheit initialisieren.

Messdatenaufbereitung

Für die Korrektur des Zustandsraumes stehen die Signale der IMU, des GPS und die Umfangsgeschwindigkeiten der Räder \(v_{i_k}\) zur Verfügung. Daher fasse ich diese für den Kalmanfilter wie folgt zusammen:

\[ Z = \begin{bmatrix} v_{GPS} \\ a_x \\ v_{1_k} \\ v_{2_k}\\ v_{3_k} \\ v_{4_k} \end{bmatrix} \]

Jedoch müssen sowohl das Signal des GPS als auch der Raddrehzahlen aufbereitet werden um von Nutzen zu sein.
Bei bekannter Verzugsdauer des GPS kann dieses für Phasen geringen Ruckes durch das Beschleunigungssignal ausgeglichen werden. Dafür verwende ich das SMA gefilterte Beschleunigungssignal um das Rauschen zu reduzieren. Im Bild ist die Verbesserung durch die Aufbereitung im Vergleich zur vom Correvit gemessene Längsgeschwindigkeit (ASS) zu sehen.

\[v_{GPS,korr} = v_{GPS} + a_x \, \Delta t\]

Jedes einzelne Rad beschreibt bei einer Kurvenfahrt einen eigenen Kurvenradius. Gemessen am Fahrzeugschwerpunkt, weichen die Raddrehzahlen eines einzelnen Rades mit abnehmenden Kurvenradius zunehmend von der Fahrzeuggeschwindigkeit ab. Um die Raddrehzahlen dennoch nutzen zu können, transformiere ich sie über eine Gierratenkorrektur auf den Schwerpunkt des Fahrzeuges. Dadurch kann auch während der Kurvenfahrt eine korrekte Aussage über den tatsächlichen Schlupf eines Rades getroffen werden.  Mit dem dynamischen Rollradius \(r_{dyn}\), der Raddrehzahlen \(N_{w,i}\), der Getriebeübersetzung \(i\) sowie der vorzeichenbehafteten halben Spurweite \(w_i\) und der Gierrate \(\dot{\psi}\) ergibt sich:

\[v_{i_k} = \frac{2 \, \pi \, r_{dyn} \, N_{w,i}}{i} + 2 \, \pi \, w_i \, \dot{\psi} \]

Korrektur Zustandsraum und Fehlerkovarianzmatrix

Sobald alle Signale erstmal beisammen sind, muss man dem Filter zeigen wie er diese interpretieren soll. Dies geschieht mithilfe der Messmatrix \(H\). Diese hat soviele Zeilen wie es Messsignale gibt und die Spaltenanzahl entspricht der Elemente im Zustandsraum. Über diese Matrix bestimmt man die Zugehörigkeit eines Messsignals zu einem Zustandraumelement. Für einen direkten Zusammenhang verwende man eine 1, ohne Zusammenhang eine 0. Dass ich \(H\) dynamisch variieren kann hilft mir bei der Verwendung des GPS Signals. Der Filter läuft mit einer Frequenz von 100Hz, das GPS Signal liegt aber nur mit einer Frequenz von 5Hz vor. Daher nutze ich eine adaptive Messmatrix um die beiden Fälle abzudecken. Die Messmatrix ergibt sich für einen Zeitschritt

mit GPS-Messwert zu	für Messschritte ohne GPS Messwert
\[H = \begin{bmatrix} 1 & 0 \\ 0 & 1 \\ 1 & 0 \\ 1 & 0 \\ 1 & 0 \\ 1 & 0 \end{bmatrix}\]	\[H = \begin{bmatrix} 0 & 0 \\ 0 & 1 \\ 1 & 0 \\ 1 & 0 \\ 1 & 0 \\ 1 & 0 \end{bmatrix}\]

 

Neben der Information ob ein Messwert vorhanden ist, benötigt der Kalmanfaktor auch die Auskunft darüber, wie zuverlässig die Messdaten sind. Diese wird von der diagonalen Messrauschkovarianzmatrix \(R\) bereitgestellt. Dabei ist \(\sigma\) die Varianz des jeweiligen Messwertes.

\[R_k = \begin{bmatrix} \sigma_{GPS} & 0 & 0 & 0 & 0 & 0 \\
0 & \sigma_{AMU} & 0 & 0 & 0 & 0\\
0 & 0 & \sigma_{Motor,1_k} & 0 & 0 & 0\\
0 & 0 & 0 & \sigma_{Motor,2_k} & 0 & 0\\
0 & 0 & 0 & 0 & \sigma_{Motor,3_k} & 0\\
0 & 0 & 0 & 0 & 0 & \sigma_{Motor,4_k}\end{bmatrix}\]

Diese Varianz bezieht sich nicht nur auf die nominellen Messtoleranzen der Messwerte, sondern auch auf die systematischen Fehler der Messung im Bezug auf die Modellierung im Prädiktorschritt. Während der Messfehler des GPS und der IMU konstant bleiben, führt Schlupf an den Rädern zu variierenden systematischen Messfehlern. Deswegen habe ich mich entschieden auch die \(R\)-Matrix adaptiv zu gestalten. Als Bezugsgröße für die Varianz der Motordrehzahlen wird daher \(\delta v_i\) eingeführt. \(\delta v_i\) beschreibt die Geschwindigkeitsdifferenz eines jeden Rades zur vom Kalmanfilter im vorherigen Schritt berechneten Geschwindigkeit.

\[\Delta v_{i_k} = v_{i_k} – v_{k-1}\]

Mithilfe der Geschwindigkeitsdifferenz und einem Totbereich um die tatsächliche Fahrzeuggeschwindigkeit berechnet sich die Varianz eines Drehzahlsignals \(\sigma_{Motor,i}\) zu

\[ \sigma_{Motor,i_k}(\Delta v_{i_k}) =
\begin{cases}
\sigma_{Motor,statisch} & \text{für } |\Delta v_{i_k}| \le 0,5 \\
\sigma_{Motor,statisch} + \sigma_{Motor,dynamisch} \cdot |\Delta v_{i_k}| & \text{für } |\Delta v_{i_k}| > 0,5. \\
\end{cases}\]

Mithilfe dieser Formulierung erkennt der Kalmanfilter auffälligen Schlupf an einem Rad und setzt dessen Vertrauenswürdigkeit über die adaptive \(R\)-Matrix herab.
Unter der Vorraussetzung, dass das Kalmansignal schon ungefähr der realen Fahrzeuggeschwindigkeit entspricht, kann so das Ergebnis noch weiter verbessert werden.

Ergebnis

Wenn man es dann geschafft hat alle Matrizen zu definieren, überlässt man die weiteren Berechnungen einfach den vom Herrn Kalman erdachten Algorithmen. Auch wenn ich bis zu dem hier präsentierten Stand einige Iterationen benötigt habe, kann sich das Ergebnis im Vergleich zum vom Correvit gemessenen Längsgeschwindigkeitssignal sehen lassen.

Aber wie es nun mal mit solchen Projekten ist, kann man immer noch etwas verbessern. Wie wäre es zum Beispiel mit einer besseren Abbildung der Querdynamik und -geschwindigkeit?

---

 

Autor: Stephan Heidrich arbeitete 2015/2016 im Formula Student Team der TU Dresden für das Modul Virtual Vehicle Performance. Er verantwortete im Team die Entwicklung und Applikation einer Antriebsschlupfregelung incl. Absolutgeschwindigkeitschätzer für ein elektrisches Allradfahrzeug. Außerdem arbeitete er an der Konzeptionierung einer Gesamtfahrzeugsimulation als Grundlagen für Konzeptentscheidungen und Regelungsauslegung.