DE102020101837A1

DE102020101837A1 - Verfahren und Vorrichtung zur Ermittlung des Zustands eines Fahrzeugs

Info

Publication number: DE102020101837A1
Application number: DE102020101837.1A
Authority: DE
Inventors: Yuran Liang
Original assignee: Bayerische Motoren Werke AG
Current assignee: Bayerische Motoren Werke AG
Priority date: 2020-01-27
Filing date: 2020-01-27
Publication date: 2021-07-29

Abstract

Es wird eine Vorrichtung (200) zur Ermittlung eines Schätzwertes (215) eines Zustandsvektors eines Fahrzeugs (100) mit einer Mehrzahl von Zustandsvariablen beschrieben. Die Vorrichtung (200) ist eingerichtet, auf Basis der Sensordaten (211) einer Inertialen Messeinheit (201) und auf Basis eines Bewegungsmodells des Fahrzeugs (100) einen Basis-Schätzwert (212) des Zustandsvektors zu ermitteln, sowie anhand von ein oder mehreren Beobachtungssensoren (202, 203, 204, 205, 206) ein oder mehrere Messwerte (213) in Bezug auf ein oder mehrere der Mehrzahl von Zustandsvariablen zu ermitteln. Des Weiteren ist die Vorrichtung (200) eingerichtet, auf Basis der ein oder mehreren Messwerte (213) und unter Verwendung zumindest eines Error State Extended Kalman Filters einen Schätzwert (214) eines Korrekturvektors zu ermitteln, und den Schätzwert (215) des Zustandsvektors auf Basis des Basis-Schätzwertes (212) des Zustandsvektors und auf Basis des Schätzwertes (214) des Korrekturvektors zu ermitteln.

Description

Die Erfindung betrifft ein Verfahren und eine entsprechende Vorrichtung zur Ermittlung des Zustands, insbesondere zur Ermittlung eines Schätzwertes eines Zustandsvektors, eines Fahrzeugs. Des Weiteren betrifft die Erfindung die Steuerung und/oder Regelung der Fahrdynamik eines Fahrzeugs.
Ein Fahrzeug weist typischerweise ein oder mehrere Systeme zur Steuerung und/oder Regelung der Fahrdynamik des Fahrzeugs auf. Derartige Fahrdynamiksysteme, wie z.B. ABS (Antiblockiersystem), ASR (Antriebsschlupfregelung), ESP (elektronisches Stabilitätsprogramm), CDC (Continuous Damper Control), ARC (Active Roll Control), etc., führen automatische Eingriffe auf eine Bremsvorrichtung, eine Antriebsvorrichtung, eine Lenkvorrichtung und/oder eine Feder-Dämpfer-Vorrichtung des Fahrzeugs durch, um einen stabilen und/oder komfortablen Fahrbetrieb des Fahrzeugs zu ermöglichen.
Ein Fahrdynamiksystem verwendet typischerweise Messwerte oder Schätzwerte von ein oder mehreren Zustandsgrößen, insbesondere von der Fahrzeugposition, der Fahrzeugorientierung und/oder der Fahrzeuggeschwindigkeit, um den Zeitpunkt, das Ausmaß und/oder die Form eines automatischen Eingriffs zu bestimmen, und/oder um das Fahrzeug zumindest teilweise automatisiert längs- und/oder querzuführen.
Das vorliegende Dokument befasst sich mit der technischen Aufgabe, aktuelle Werte von ein oder mehreren Zustandsgrößen eines Fahrzeugs in besonders effizienter und präziser Weise zu ermitteln.
Die Aufgabe wird durch jeden der unabhängigen Ansprüche gelöst. Vorteilhafte Ausführungsformen werden u.a. in den abhängigen Ansprüchen beschrieben. Es wird darauf hingewiesen, dass zusätzliche Merkmale eines von einem unabhängigen Patentanspruch abhängigen Patentanspruchs ohne die Merkmale des unabhängigen Patentanspruchs oder nur in Kombination mit einer Teilmenge der Merkmale des unabhängigen Patentanspruchs eine eigene und von der Kombination sämtlicher Merkmale des unabhängigen Patentanspruchs unabhängige Erfindung bilden können, die zum Gegenstand eines unabhängigen Anspruchs, einer Teilungsanmeldung oder einer Nachanmeldung gemacht werden kann. Dies gilt in gleicher Weise für in der Beschreibung beschriebene technische Lehren, die eine von den Merkmalen der unabhängigen Patentansprüche unabhängige Erfindung bilden können.
Gemäß einem Aspekt wird eine Vorrichtung zur Ermittlung eines Schätzwertes eines Zustandsvektors eines (Kraft-) Fahrzeugs beschrieben. Der Zustandsvektor x umfasst eine Mehrzahl von Zustandsvariablen, insbesondere die Position p des Fahrzeugs, die Orientierung θ des Fahrzeugs und/oder die Fahrgeschwindigkeit v des Fahrzeugs. Dabei können die Position, die Orientierung und/oder die Fahrgeschwindigkeit jeweils mehrere Dimensionen aufweisen.
Die Vorrichtung kann eingerichtet sein, auf Basis der Sensordaten einer Inertialen Messeinheit (insbesondere einer Inertial Measurement Unit, IMU) und auf Basis eines Bewegungsmodells des Fahrzeugs einen Basis-Schätzwert des Zustandsvektors zu ermitteln. Insbesondere kann die Vorrichtung eingerichtet sein, auf Basis der Sensordaten der Inertialen Messeinheit ein oder mehrere Eingangswerte u des Bewegungsmodells für einen Zeitpunkt k - 1 zu ermitteln. Die Eingangswerte können insbesondere die (mehrdimensionale) Beschleunigung des Fahrzeugs und/oder eine (mehrdimensionale) Drehrate des Fahrzeugs (z.B. die Nickrate, die Wankrate und/oder die Gierrate) umfassen. Das Bewegungsmodell kann ausgebildet sein, auf Basis des Schätzwertes x̂ _k-1 des Zustandsvektors x für den (vorhergehenden) Zeitpunkt k - 1 und auf Basis der ein oder mehrere (gemessenen) Eingangswerte u für den Zeitpunkt k - 1 den Basis-Schätzwert x̌_k des Zustandsvektors x für einen (nachfolgenden bzw. aktuellen) Zeitpunkt k zu ermitteln. Es kann somit iterativ für eine Sequenz von Zeitpunkten jeweils ein (Basis-) Schätzwert des Zustandsvektors ermittelt werden.
Der Basis-Schätzwert x̌_k des Zustandsvektors für den Zeitpunkt k kann insbesondere anhand ein oder mehrerer der folgenden Formeln des Bewegungsmodells ermittelt werden:

$Ř_{k}^{N E D} = e u l 2 d c m (Δ t * {\underline{w}}_{k - 1}) * {\hat{R}}_{k - 1}^{N E D}$
Dabei sind:

• p̂k-1: der Schätzwert der Position des Fahrzeugs für den Zeitpunkt k - 1;
• Δt der: zeitliche Abstand zwischen den Zeitpunkten k - 1 und k;
• v̂k-1: der Schätzwert der Geschwindigkeit des Fahrzeugs für den Zeitpunkt k-1;
•: der Schätzwert der Rotationsmatrix des Fahrzeugs für den Zeitpunkt k-1 von Fahrzeugkörperkoordinaten auf NED-Koordinaten (wobei die Rotationsmatrix die Orientierung des Fahrzeugs anzeigt bzw. dieser entspricht);
• ak-1: die für den Zeitpunkt k - 1 gemessene (mehrdimensionale) Beschleunigung des Fahrzeugs (100);
• g die: Erdbeschleunigung;
• p̌k der: Basis-Schätzwert;
• der: Position des Fahrzeugs für den Zeitpunkt k;
• v̌k: der Basis-Schätzwert der Geschwindigkeit des Fahrzeugs für den Zeitpunkt k;
• wk-1: die für den Zeitpunkt k - 1 gemessene (mehrdimensionale) Drehrate des Fahrzeugs;
• eul2dcm(): eine Transformation von Eulerwinkeln auf die Rotationsmatrix in NED-Koordinaten;
•: der Basis-Schätzwert der Rotationsmatrix des Fahrzeugs für den Zeitpunkt k - 1 von Fahrzeugkörperkoordinaten auf NED-Koordinaten; dcm2eul() eine Transformation von der Rotationsmatrix in NED-Koordinaten auf Eulerwinkel; und/oder
• θ̌k: der Basis-Schätzwert der Orientierung des Fahrzeugs für den Zeitpunkt k ist.

Die Vorrichtung kann eingerichtet sein, anhand von ein oder mehreren Beobachtungssensoren ein oder mehrere Messwerte in Bezug auf ein oder mehrere der Mehrzahl von Zustandsvariablen zu ermitteln. Dabei können insbesondere drei oder mehr unterschiedliche Beobachtungssensoren verwendet werden. Die unterschiedlichen Beobachtungssensoren können dabei Messwerte mit jeweils unterschiedlichen Messfrequenzen bereitstellen. Des Weiteren können die Bereitstellungszeitpunkte der Messwerte der unterschiedlichen Beobachtungssensoren zeitlich asynchron zueinander sein. Als Folge daraus kann es vorkommen, dass zwischen zwei direkt aufeinanderfolgenden Zeitpunkten k - 1 und k Messwerte von null, ein oder mehreren Beobachtungssensoren bereitgestellt werden. Die in diesem Dokument beschriebene Vorrichtung ist dabei ausgebildet, zeitlich asynchron bereitgestellte Messwerte einer beliebigen Anzahl von Beobachtungssensoren in flexibler und präziser Weise zu berücksichtigen, um einen besonders präzisen Schätzwert des Zustandsvektors zu ermitteln.
Die einzelnen Beobachtungssensoren können jeweils eingerichtet sein, Sensordaten in Bezug auf ein oder mehrere Beobachtungsgrößen zu erfassen. Dabei sind die Beobachtungsgrößen von unterschiedlichen Beobachtungssensoren typischerweise zumindest teilweise unterschiedlich zueinander. Die Vorrichtung kann eingerichtet sein, mittels eines Sensormodells des jeweiligen Beobachtungssensors auf Basis der Sensordaten des jeweiligen Beobachtungssensors jeweils ein oder mehrere Messwerte in Bezug auf ein oder mehrere der Mehrzahl von Zustandsvariablen zu ermitteln. Diese ein oder mehreren Messwerte können dann dazu genutzt werden, die Genauigkeit des Schätzwertes des Zustandsvektors zu erhöhen.
Die ein oder mehreren Beobachtungssensoren können eine Mehrzahl von Höhenstandssensoren umfassen, die jeweils eingerichtet sind, Sensordaten in Bezug auf den Höhenstand eines Rades des Fahrzeugs zu erfassen. Das Sensormodell für die Mehrzahl von Höhenstandssensoren kann ausgebildet sein, auf Basis der Sensordaten der Mehrzahl von Höhenstandssensoren Messwerte in Bezug auf die Orientierung des Fahrzeugs zu ermitteln. Auf Basis der Sensordaten und des Sensormodells kann z.B. eine Rotationsmatrix R_HS ermittelt werden, die insbesondere den Nickwinkel θ_y und/oder den Wankwinkel θ_x (d.h. allgemein Drehwinkel) des Fahrzeugs anzeigt. Die Drehwinkel des Fahrzeugs können z.B. ermittelt werden, als $[\begin{matrix} θ_{x} \\ θ_{y} \\ θ_{z} \end{matrix}] = d c m 2 e u l (R_{H S})$
und es kann als Messwert ${\underline{y}}_{k} = [\begin{matrix} θ_{x} \\ θ_{y} \end{matrix}] \in R^{2}$
bereitgestellt werden.
Aus dem Sensormodell kann ferner ein Messwert geschätzt werden, als
und es kann eine Beobachtungsmatrix bereitgestellt werden, als $H_{x, k} = [\begin{matrix} 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 \end{matrix}]$
Alternativ oder ergänzend können die ein oder mehreren Beobachtungssensoren einen (satellitenbasierten) Positionssensor umfassen, der eingerichtet ist, Sensordaten in Bezug auf die Position und/oder die (Doppler-) Geschwindigkeit des Fahrzeugs zu erfassen. Das Sensormodell für den Positionssensor kann ausgebildet sein, auf Basis der Sensordaten des Positionssensors Messwerte in Bezug auf die Position und/oder die Geschwindigkeit des Fahrzeugs zu ermitteln.
Als Sensordaten kann insbesondere die globale Position und/oder Geschwindigkeit bereitgestellt werden, die mittels des Sensormodells auf das NED Koordinatensystem umgerechnet werden kann. Als Messwert kann dann bereitgestellt werden, ${\underline{y}}_{k} = [\begin{matrix} {\underline{p}}_{s} \\ {\underline{v}}_{s} \end{matrix}] \in R^{6}$
Ferner kann anhand des Sensormodells ein Messwert geschätzt werden, z.B. anhand von
Außerdem kann folgende Beobachtungsmatrix bereitgestellt werden, $H_{x, k} = [\begin{matrix} I_{3} & 0_{3} & 0_{3} \\ 0_{3} & I_{3} & 0_{3} \end{matrix}]$
wobei I₃ eine 3x3 Identitäts- bzw. Einheitsmatrix und 0₃ eine 3×3 Nullmatrix ist.
Alternativ oder ergänzend können die ein oder mehreren Beobachtungssensoren einen Radarsensor umfassen. Das Sensormodell für den Radarsensor kann ausgebildet sein, auf Basis der Sensordaten des Radarsensors Messwerte in Bezug auf die Geschwindigkeit, die Rotationsbewegung und/oder die Translationsbewegung des Fahrzeugs zu ermitteln.
Auf Basis der Sensordaten und des Sensormodells des Radarsensors können z.B. die lokale Geschwindigkeit v _R, die Rotationsmatrix R _R und/oder die Position t _R relativ zu einem Köperfestkoordinatensystem ermittelt werden. Beispielhafte Messwerte sind somit ${\underline{y}}_{k} = [\begin{matrix} \underline{t_{R}} \\ \underline{v_{R}} \\ \underline{θ_{R}} \end{matrix}] \in R^{9}$
wobei sich die Orientierung des Fahrzeugs (relativ zu einem Köperfestkoordinatensystem) z.B. aus θ _R=dcm2eul (R _R) ergibt.
Ein geschätzter Messwert kann ermittelt werden als
Außerdem kann folgende Beobachtungsmatrix bereitgestellt werden,
wobei der Operator [z] _xeine schiefsymmetrische Matrix von Vektor z = [Z₁ Z₂ Z_3] ^T ist, d.h. ${[\underline{z}]}_{x} ≜ [\begin{matrix} 0 & - z_{3} & z \\ z_{3} & 0 & - z_{1} \\ - z & z_{1} & 0 \end{matrix}] \in R^{3 \times 3}$
Alternativ oder ergänzend können die ein oder mehreren Beobachtungssensoren einen Lidarsensor umfassen. Das Sensormodell für den Lidarsensor kann ausgebildet sein, auf Basis der Sensordaten des Lidarsensors Messwerte in Bezug auf die Rotationsbewegung und/oder die Translationsbewegung des Fahrzeugs zu ermitteln.
Auf Basis der Sensordaten und des Sensormodells des Lidarsensors können z.B. die lokale Rotationsmatrix R _L und/oder die Position t _L relativ zu einem Köperfestkoordinatensystem als Messwerte bereitgestellt werden, d.h. ${\underline{y}}_{k} = [\begin{matrix} \underline{t_{L}} \\ \underline{θ_{L}} \end{matrix}] \in R^{6}$
wobei θ _L = dcm2eul (R _L).
Ein geschätzter Messwert kann ermittelt werden als
Des Weiteren kann folgende Beobachtungsmatrix bereitgestellt werden $H_{x, k} = [\begin{matrix} {\hat{R}}_{k - 1}^{N E D}^{^{T}} & 0_{3} & 0_{3} \\ 0_{3} & 0_{3} & 0_{3} \\ 0_{3} & 0_{3} & I_{3} \end{matrix}]$
Alternativ oder ergänzend können die ein oder mehreren Beobachtungssensoren eine Kamera umfassen, die ausgebildet ist, Sensordaten (insbesondere Bilddaten) in Bezug auf das Umfeld des Fahrzeugs zu erfassen. Das Sensormodell für die Kamera kann ausgebildet sein, auf Basis der Sensordaten der Kamera Messwerte in Bezug auf die Rotationsbewegung und/oder die Translationsbewegung des Fahrzeugs zu ermitteln.
Auf Basis der Sensordaten und des Sensormodells der Kamera können z.B. die lokale Rotationsmatrix R _K und/oder die Position t _K relativ zu einem Köperfestkoordinatensystem als Messwerte bereitgestellt werden, d.h. ${\underline{y}}_{k} = [\begin{matrix} \underline{t_{K}} \\ \underline{θ_{K}} \end{matrix}] \in R^{6}$
wobei θ _K = dcm2eul(R _K).
Ein geschätzter Messwert kann ermittelt werden als
Des Weiteren kann folgende Beobachtungsmatrix bereitgestellt werden $H_{x, k} = [\begin{matrix} {\hat{R}}_{k - 1}^{N E D}^{^{T}} & 0_{3} & 0_{3} \\ 0_{3} & 0_{3} & 0_{3} \\ 0_{3} & 0_{3} & I_{3} \end{matrix}]$
Es können somit Messwerte von mehreren Beobachtungssensoren ermittelt und bei der Ermittlung des Schätzwertes des Zustandsvektors berücksichtigt werden. Insbesondere kann die Vorrichtung eingerichtet sein, auf Basis der ein oder mehreren Messwerte von ein oder mehreren Beobachtungssensoren und unter Verwendung zumindest eines Error State Extended Kalman Filters, kurz ESEKF, einen Schätzwert δx̂ _k eines Korrekturvektors zu ermitteln. Der Schätzwert x̂ _k des Zustandsvektors kann dann in präziser Weise auf Basis des Basis-Schätzwertes x̂ _k des Zustandsvektors und auf Basis des Schätzwertes δx̂ _k des Korrekturvektors ermittelt werden, insbesondere als x̂ _k = x̌ _k + δx̂ _k. So kann auch bei asynchron bereitgestellten Messwerten ein präziser Schätzwert des Zustandsvektors ermittelt werden (an einer Sequenz von Zeitpunkten k).
Die Vorrichtung kann ferner eingerichtet sein, das Fahrzeug in Abhängigkeit von dem ermittelten Schätzwert des Zustandsvektors zumindest teilweise automatisiert längs- und/oder querzuführen, und/oder einen Eingriff in die Fahrdynamik des Fahrzeugs zu bewirken. So kann eine besonders zuverlässige automatisierte Längs- und/oder Querführung (insbesondere zum voll- oder hochautomatisierten Fahren) ermöglicht werden.
Die Vorrichtung kann eingerichtet sein, einen Schätzwert der Fehlerkovarianz des Bewegungsmodells zu ermitteln. Der Schätzwert der Fehlerkovarianz kann dabei iterativ über der Zeit angepasst werden. Ferner kann der Schätzwert der Fehlerkovarianz mit einem bestimmten Wert (z.B. mit einer Einheitsmatrix) initialisiert werden. Die (iterative) Ermittlung des Schätzwertes der Fehlerkovarianz kann auf Basis des Bewegungsmodells (insbesondere auf Basis einer Ableitung des Bewegungsmodells) ermittelt werden.
Die Vorrichtung kann eingerichtet sein, den Schätzwert der Fehlerkovarianz zu ermitteln, anhand der Formel
Dabei sind

• p̂k-1: der Schätzwert der Fehlerkovarianz für den Zeitpunkt k - 1;
• Fk-1: die Jacobi-Matrix des Bewegungsmodells und/oder die erste Ableitung des Basis-Schätzwertes des Zustandsvektors für den Zeitpunkt k nach dem Schätzwert des Korrekturvektors für den Zeitpunkt k - 1; F_k-1 kann typischerweise auf Basis des (o.g.) Bewegungsmodells ermittelt werden;
• P̌k: der Schätzwert der Fehlerkovarianz für den Zeitpunkt k;
• Qk-1: die (typischerweise konstante und/oder geschätzte) Kovarianz von Prozessrauschen; und/oder
• Lk-1: eine (applizierbare) Matrix, insbesondere eine konstante Matrix.

Der Schätzwert des Korrekturvektors kann dann in besonders präziser Weise auf Basis des Schätzwerts der Fehlerkovarianz ermittelt werden.
Zur Ermittlung des Schätzwerts der Fehlerkovarianz auf Basis der ein oder mehreren Messwerte eines Beobachtungssensors kann zunächst ein Kalman-Verstärkungsfaktor K_k ermittelt werden (spezifisch für den jeweiligen Beobachtungssensor). Der Kalman-Verstärkungsfaktor kann dabei auf Basis des Schätzwerts der Fehlerkovarianz ermittelt werden. Insbesondere kann die Vorrichtung eingerichtet sein, den Kalman-Verstärkungsfaktor K_k auf Basis des Schätzwertes der Fehlerkovarianz für den Zeitpunkt k zu ermitteln, z.B. unter Verwendung der Formel
wobei H_k die Beobachtungsmatrix des jeweiligen Beobachtungssensors ist (wie beispielhaft oben für unterschiedliche Beobachtungssensoren wiedergegeben). R kann eine (typischerweise konstante) Matrix zur Beschreibung von Rauschen des jeweiligen Beobachtungssensors sein (und für die unterschiedlichen Beobachtungssensoren unterschiedlich sein).
Des Weiteren kann die Vorrichtung eingerichtet sein, den Schätzwert des Korrekturvektors auf Basis des Basis-Schätzwertes des Zustandsvektors, auf Basis des Sensormodells für den jeweiligen Beobachtungssensor und auf Basis der ein oder mehreren Messwerte des jeweiligen Beobachtungssensors zu ermitteln. Dies kann insbesondere auf Basis der folgenden Formel erfolgen:
Dabei sind

• δx̂k: der Schätzwert des Korrekturvektors für den Zeitpunkt k;
• yk: der Messwert des jeweiligen Beobachtungssensors für den Zeitpunkt k;
• x̌k: der Basis-Schätzwertes für den Zustandsvektor für den Zeitpunkt k; und/oder
• hk(): das Sensormodell für den jeweiligen Beobachtungssensor (wie beispielhafte weiter oben für unterschiedliche Beobachtungssensoren beschrieben.

So kann der Schätzwert des Korrekturvektors in besonders präziser Weise ermittelt werden.
Die Vorrichtung kann eingerichtet sein, auf Basis des Kalman-Verstärkungsfaktors für den Zeitpunkt k und auf Basis der Beobachtungsmatrix des jeweiligen Beobachtungssensors für den Zeitpunkt k den Schätzwert P̂_k der Fehlerkovarianz für den Zeitpunkt k zu aktualisieren, insbesondere als
wobei I eine Einheitsmatrix ist. Der aktualisierte Schätzwert P̂_k der Fehlerkovarianz kann dann dazu verwendet werden, den Schätzwert des Korrekturvektors zu aktualisieren, insbesondere auf Basis der ein oder mehreren Messwerte eines (ggf. anderen) Beobachtungssensors.
Die Vorrichtung kann insbesondere eingerichtet sein, für jeden Beobachtungssensor (bzw. für mehrere Beobachtungsensoren), der (bzw. die) zwischen den Zeitpunkten k - 1 und k einen Messwert bereitstellt (bzw. bereitstellen), nacheinander, jeweils auf Basis des aktuellen Schätzwerts der Fehlerkovarianz einen Kalman-Verstärkungsfaktor zu ermitteln, und auf Basis des Kalman-Verstärkungsfaktors den Schätzwert der Fehlerkovarianz zu aktualisieren. Ferner kann auf Basis des Kalman-Verstärkungsfaktors jeweils ein Schätzwert des Korrekturvektors für den Zeitpunkt k ermittelt werden. So können mit einer besonders hohen zeitlichen Auflösung präzise Schätzwerte des Korrekturvektors bereitgestellt werden. Ferner können so in effizienter und zuverlässiger Weise die Messwerte von unterschiedlichen Beobachtungssensoren berücksichtigt werden, um die Genauigkeit des ermittelten Schätzwertes des Korrekturwertes zu erhöhen.
Die Vorrichtung kann eingerichtet sein, für jeden der Mehrzahl von Beobachtungssensoren (insbesondere der drei oder mehr Beobachtungssensoren) auf Basis des Messwertes des jeweiligen Beobachtungssensors und unter Verwendung jeweils eines (separaten) ESEKFs jeweils einen Teil-Schätzwert für den Korrekturvektor zu ermitteln. Der Schätzwert des Korrekturvektors kann dann in besonders präziser Weise auf Basis der Mehrzahl von Teil-Schätzwerte ermittelt werden. Dabei kann insbesondere ein Weighted Least Squares Filter verwendet werden, um Ausreißer detektieren und eliminieren zu können, und um somit die Güte des ermittelten Schätzwertes des Korrekturwertes weiter zu erhöhen.
Die Vorrichtung kann eingerichtet sein, für die Mehrzahl von Beobachtungssensoren eine entsprechende Mehrzahl von Gewichten zu ermitteln. Die Gewichte der einzelnen Beobachtungssensoren können von der Messgenauigkeit und/oder der Konfidenz des jeweiligen Beobachtungssensors abhängen. Die Gewichte der einzelnen Beobachtungssensoren können sich mit der Zeit ändern. Der Schätzwert des Korrekturvektors kann dann auch auf Basis der Mehrzahl von Gewichten, insbesondere unter Verwendung eines Weighted Least Squares Filters, ermittelt werden. So kann die Genauigkeit des ermittelten Schätzwertes des Korrekturwertes weiter erhöht werden.
Die Vorrichtung kann eingerichtet sein, die Mehrzahl von Teil-Schätzwerte für eine Sequenz von Zeitpunkten bis zu dem Zeitpunkt k zu ermitteln. Der Schätzwert des Korrekturvektors für den Zeitpunkt k kann dann in besonders präziser Weise auf Basis der Mehrzahl von Teil-Schätzwerte für die Sequenz von Zeitpunkten ermittelt werden (z.B. durch Anwendung eines Weighted Least Squares Filters).
Gemäß einem weiteren Aspekt wird ein (Straßen-)Kraftfahrzeug (insbesondere ein Personenkraftwagen oder ein Lastkraftwagen oder ein Bus oder ein Motorrad) beschrieben, das die in diesem Dokument beschriebene Steuereinheit bzw. Vorrichtung umfasst.
Gemäß einem weiteren Aspekt wird ein Verfahren zur Ermittlung eines Schätzwertes eines Zustandsvektors eines Fahrzeugs, wobei der Zustandsvektor eine Mehrzahl von Zustandsvariablen aufweist, beschrieben. Das Verfahren umfasst das Ermitteln, auf Basis der Sensordaten einer Inertialen Messeinheit und auf Basis eines Bewegungsmodells des Fahrzeugs, eines Basis-Schätzwertes des Zustandsvektors. Des Weiteren umfasst das Verfahren das Ermitteln, anhand von ein oder mehreren Beobachtungssensoren, ein oder mehrerer Messwerte in Bezug auf ein oder mehrere der Mehrzahl von Zustandsvariablen. Das Verfahren umfasst ferner das Ermitteln, auf Basis der ein oder mehreren Messwerte und unter Verwendung zumindest eines Error State Extended Kalman Filters (ESEKF), eines Schätzwertes eines Korrekturvektors. Außerdem umfasst das Verfahren das Ermitteln des Schätzwertes des Zustandsvektors auf Basis des Basis-Schätzwertes des Zustandsvektors und auf Basis des Schätzwertes des Korrekturvektors.
Gemäß einem weiteren Aspekt wird ein Software (SW) Programm beschrieben. Das SW Programm kann eingerichtet werden, um auf einem Prozessor (z.B. auf einem Steuergerät eines Fahrzeugs) ausgeführt zu werden, und um dadurch eines der in diesem Dokument beschriebenen Verfahren auszuführen.
Gemäß einem weiteren Aspekt wird ein Speichermedium beschrieben. Das Speichermedium kann ein SW Programm umfassen, welches eingerichtet ist, um auf einem Prozessor ausgeführt zu werden, und um dadurch eines der in diesem Dokument beschriebenen Verfahren auszuführen.
Es ist zu beachten, dass die in diesem Dokument beschriebenen Verfahren, Vorrichtungen und Systeme sowohl alleine, als auch in Kombination mit anderen in diesem Dokument beschriebenen Verfahren, Vorrichtungen und Systemen verwendet werden können. Des Weiteren können jegliche Aspekte der in diesem Dokument beschriebenen Verfahren, Vorrichtungen und Systemen in vielfältiger Weise miteinander kombiniert werden. Insbesondere können die Merkmale der Ansprüche in vielfältiger Weise miteinander kombiniert werden.
Im Weiteren wird die Erfindung anhand von Ausführungsbeispielen näher beschrieben. Dabei zeigen

1 beispielhafte Komponenten eines Fahrzeugs;
2 und 3 beispielhafte Vorrichtungen zur Ermittlung eines Schätzwertes des Zustands eines Fahrzeugs; und
4 ein Ablaufdiagramm eines beispielhaften Verfahrens zur Ermittlung eines Schätzwertes des Zustands eines Fahrzeugs.

Wie eingangs dargelegt, befasst sich das vorliegende Dokument mit der zuverlässigen und effizienten Ermittlung von Werten für ein oder mehrere Zustandsgrößen eines Fahrzeugs. In diesem Zusammenhang zeigt 1 beispielhafte Komponenten eines Fahrzeugs 100. Insbesondere zeigt 1 die Räder 103 der Achsen 101, 102 eines Fahrzeugs 100, wobei die Räder 103 jeweils Radbremsen 110 aufweisen.
Des Weiteren umfasst das Fahrzeug 100 einen Antrieb 107 zum Antrieb zumindest einer Achse 101, 102 des Fahrzeugs 100. Außerdem umfasst das Fahrzeug 100 ein oder mehrere Sensoren 106, die eingerichtet sind, Sensordaten in Bezug auf ein oder mehrere Mess- bzw. Beobachtungsgrößen zu erfassen. Beispielhafte Messgrößen sind die Fahrgeschwindigkeit, die Längsbeschleunigung, die Querbeschleunigung, die Gierrate, den Lenkwinkel der Vorderachse 101, den Lenkwinkel der Hinterachse 102, etc. Alternativ oder ergänzend können die ein oder mehreren Sensoren 106 eingerichtet sein, Sensordaten in Bezug auf das Umfeld des Fahrzeugs 100 zu erfassen. Beispielhafte Sensoren 106 sind eine Bildkamera, ein Radarsensor, ein Lidarsensor, etc.
Eine Steuereinheit 105 des Fahrzeugs 100 kann eingerichtet sein, auf Basis der Sensordaten der ein oder mehrere Sensoren 106 automatische Eingriffe an ein oder mehrere Fahrdynamik-Aktoren 107, 110 (z.B. den Antrieb 107 und/oder die Bremsen 110) zu bewirken, um die Fahrdynamik des Fahrzeugs 100 zu verändern, insbesondere um die Stabilität und/oder den Komfort des Fahrzeugs 100 zu erhöhen. Zu diesem Zweck kann die Steuereinheit 105 auf ein Fahrzeugmodell, insbesondere ein lineares Einspurmodell, zurückgreifen, das z.B. auf einer Speichereinheit 104 des Fahrzeugs 100 gespeichert sein kann. Das Fahrzeugmodell weist typischerweise ein oder mehrere Modellparameter auf. Das Fahrzeugmodell kann ausgebildet sein, den Fahrzeug- und/oder Fahrzustand zu schätzen.
Um eine gute Schätzung von ein oder mehreren Zustandsgrößen bzw. Zustandsvariablen des Fahrzeugs 100 zu erhalten, müssen die ein oder mehreren Modellparameter des Fahrzeugmodells (z.B. ein oder mehrere Fahrwerks- und/oder Reifenparameter) möglichst genau gemessen werde. Abweichungen der Modellparameter durch Ungenauigkeiten und/oder durch Änderungen während der Fahrt (z.B. bei der Schräglaufsteifigkeiten und/oder Schwerpunktlage), können zu Ungenauigkeiten bei der Ermittlung der Werte der ein oder mehreren Zustandsgrößen führen. Weiterhin müssen die Werte der Modellparameter typischerweise bereits vor Applikation des Fahrzeugmodells ausreichend bekannt sein. In relativ frühen Entwicklungsphasen eines Fahrzeugs 100 sind präzise Werte der ein oder mehreren Modellparameter jedoch oftmals noch nicht bekannt, sodass die Applikation eines Fahrzeugmodells erst dann möglich und/oder sinnvoll ist sobald die Werte der ein oder mehreren Modellparameter feststehen.
Ein weiterer Nachteil ist, dass ein Fahrzeugmodell, insbesondere ein Einspurmodell, linear ausgelegt ist und die Fahrzeuggeschwindigkeit mit relativ hoher Genauigkeit geschätzt werden muss. Dabei führt jedoch z.B. eine Erfassung der Längsgeschwindigkeit durch die Raddrehzahlsensorik eines Fahrzeugs 100, insbesondere bei relativ hohem Schlupf und/oder ungenauer Applikation des Radradius, zu Fehlern.
Die im Hinblick auf hoch- und vollautomatisiertes Fahren zunehmend wachsenden Anforderungen an die Genauigkeit und Integrität der Schätzwerte für ein oder mehrere Zustandsgrößen bzw. Zustandsvariablen eines Fahrzeugs 100, wie die Odometrie, die Geschwindigkeit und/oder die Orientierung eines Fahrzeugs 100, können somit durch eine (allein) auf einem Fahrzeugmodell basierenden Schätzmethode nicht immer ausreichend erfüllt werden.
In den 2 und 3 wird jeweils eine Vorrichtung 200 (z.B. als Teil der Steuereinheit 105 des Fahrzeugs 100) zur Ermittlung von Schätzwerten von ein oder mehreren Zustandsgrößen eines Fahrzeugs 100 dargestellt. Die Vorrichtung 200 umfasst eine Modelleinheit 202, die eingerichtet ist, auf Basis von Sensordaten 211 von ein oder mehreren Sensoren 106, insbesondere auf Basis der Sensordaten 211 einer Inertial Measurement Unit (IMU), d.h. einer Inertialen Messeinheit, und unter Verwendung eines Bewegungsmodells einen Basis-Schätzwert x̌_k 212 eines Zustandsvektors in Bezug auf den Zustand des Fahrzeugs 100 an dem aktuellen Zeitpunkt k zu ermitteln. Der Zustandsvektor kann z.B. die Position p, die Geschwindigkeit v und/oder die Orientierung q umfassen (jeweils als mehrdimensionale Untervektoren). Die Messwerte u_k der Eingangsgrößen des Bewegungsmodells können auf Basis der Sensordaten 211 der IMU 201 ermittelt werden. Beispielhafte Eingangsgrößen sind die Beschleunigung α und/oder die rotationalen Drehraten w.
Es kann somit durch die Modelleinheit 202 an einer Sequenz von aufeinanderfolgenden Zeitpunkten k jeweils ein aktueller Basis-Schätzwert x̌_k 212 (alternativ bezeichnet als x̌ _k des Zustandsvektors $\underline{x} = (\begin{matrix} \underline{p} \\ \underline{v} \\ \underline{q} \end{matrix}), z . B . \in R^{10}$
ermittelt werden. Als Eingangsgrößen des Bewegungsmodells können jeweils aktuelle Messwerte 211 des Eingangsvektors $\underline{u} = (\begin{matrix} \underline{a} \\ \underline{w} \end{matrix}) \in R^{6}$
bereitgestellt werden. Ferner kann die Erdbeschleunigung $\underline{g} = (\begin{matrix} 0 \\ 0 \\ - 9,81 \end{matrix})$
berücksichtigt werden. Ein beispielhaftes Bewegungsmodell für ein Fahrzeug 100 ist ${\underline{p}}_{k} = {\underline{p}}_{k - 1} + Δ t * {\underline{v}}_{k - 1} + \frac{Δ t^{2}}{2} * (R_{k - 1}^{N E D} * {\underline{a}}_{k - 1} + \underline{g})$
${\underline{v}}_{k} = {\underline{v}}_{k - 1} + Δ t * (R_{k - 1}^{N E D} * {\underline{a}}_{k - 1} + \underline{g})$
${\underline{q}}_{k} = Ω (\underline{q} ({\underline{w}}_{k - 1} * Δ t), {\underline{q}}_{k - 1})$
Dabei sind Δt die Zeitdauer zwischen zwei direkt aufeinanderfolgenden Zeitpunkten k - 1 und k, $R_{k - 1}^{N E D}$
die Rotationsmatrix von Fahrzeugkörperfestkoordinaten auf NED (North, East, Down)-Koordinaten, Ω(q ₁,q ₂) die Multiplikation von zwei Quaternionen q ₁ und q ₂, und q() die Transformation von Euler auf Quaternionen.
Anhand des o.g. Bewegungsmodells können an jedem Zeitpunkt k jeweils ein Basis-Schätzwert x̌_k 212 des Zustandsvektors des Fahrzeugs 100 bereitgestellt werden. Der (mehrdimensionale) Basis-Schätzwert x̌_k 212 kann in einer Korrektureinheit 230 mit einem (mehrdimensionalen) Korrekturwert δx_k 214 korrigiert werden, um den (korrigierten) Schätzwert x̂_k 215 des Zustandsvektors des Fahrzeugs 100 zu ermitteln, z.B. als
Der Korrekturwert δx_k 214 an dem Zeitpunkt k kann durch Verwendung eines Errors-State Extended Kalman Filters (ESEKF) auf Basis der Sensordaten 211 von ein oder mehreren weiteren Beobachtungssensoren 106, 202, 203, 204, 205, 206 des Fahrzeugs 100 ermittelt werden. Beispielhafte weitere Sensoren 106 sind:

Höhenstandssensoren 202 (an den Rädern 110 des Fahrzeugs 100), die ausgebildet sind, Sensordaten 211 in Bezug auf den Höhenstand h der einzelnen Räder 110 des Fahrzeugs 100 bereitzustellen. Auf Basis der Sensordaten 211 der Höhenstandssensoren 202 kann unter Verwendung eines Höhenstands-Sensormodells 210 des Fahrzeugs 100 ein Messwert R_Hs 213 in Bezug auf eine Rotationsbewegung, insbesondere in Bezug auf eine Wank- und/oder Nickbewegung, des Fahrzeugs 100 ermittelt werden.

Ein GNSS (Global Navigation Satellite System) Sensor 203, der ausgebildet ist, Sensordaten 211 in Bezug auf die Position p _abs in absoluten Koordinaten, und/oder in Bezug auf die Doppler-Geschwindigkeit v _dop zu ermitteln. Unter Verwendung eines GNSS-Sensormodells 210 des Fahrzeugs 100 können GNSS-Messwerte 213 p _s für die Position und/oder v _s für die Geschwindigkeit des Fahrzeugs 100 bereitgestellt werden.
Die Sensordaten 211 eines Radarsensors 204 können dazu verwendet werden, unter Verwendung eines Radar-Sensormodells 210 für das Fahrzeug 100 Radar-Messwerte 213 v _R in Bezug auf die Geschwindigkeit des Fahrzeugs 100, R _R in Bezug auf die Rotationsbewegung des Fahrzeugs 100 und/oder t _R in Bezug auf die Translationsbewegung des Fahrzeugs 100 bereitzustellen.
Die Sensordaten 211 eines Lidarsensors 205 können dazu verwendet werden, unter Verwendung eines Lidar-Sensormodells 210 für das Fahrzeug 100 Lidar-Messwerte 213 R _L in Bezug auf die Rotationsbewegung des Fahrzeugs 100 und/oder t _L in Bezug auf die Translationsbewegung des Fahrzeugs 100 bereitzustellen.
Die Sensordaten 211 einer Kamera 206 können dazu verwendet werden, unter Verwendung eines Kamera-Sensormodells 210 für das Fahrzeug 100 Kamera-Messwerte 213 R _K in Bezug auf die Rotationsbewegung des Fahrzeugs 100 und/oder t _K in Bezug auf die Translationsbewegung des Fahrzeugs 100 bereitzustellen.
Die Messwerte 213 der ein oder mehreren weiteren Sensoren 202, 203, 204, 205, 206 können dann in einer ESEKF-Einheit 220 dazu verwendet werden, den Korrekturwert δx_k 214 zu ermitteln. Dabei kann auch der Basis-Schätzwert x̌_k 212 berücksichtigt werden. Die ESEKF-Einheit 220 kann dabei einen ESEKF verwenden, um iterativ den Korrekturwert δx_k 214 ausgehend von dem vorhergehenden Korrekturwert δx_k-1 und/oder ausgehend von dem Basis-Schätzwert x̌_k 212 auf Basis der verfügbaren Messwerte 213 zu aktualisieren. Der Korrekturwert δx_k 214 kann auch als Fehlerzustand oder als Korrekturzustand bezeichnet werden.
Aus der Linearisierung des Bewegungsmodells ergibt sich ${\underline{x}}_{k} - f_{k - 1} ({\hat{\underline{x}}}_{k - 1}, {\underline{u}}_{k - 1}, 0) = F_{k - 1} ({\underline{x}}_{k - 1} - {\hat{\underline{x}}}_{k - 1}) + L_{k - 1} {\underline{w}}_{k - 1}$
woraus sich ergibt $δ x_{k} = F_{k - 1} δ x_{k - 1} + L_{k - 1} w_{k - 1}$
mit der Jacobi-Matrix F_k-1 des Bewegungsmodells, d.h. der ersten Ableitung des aktuellen Fehlerzustands nach dem vorhergehenden Fehlerzustand. Die Matrix L_k-1 ist die erste Ableitung des aktuellen Fehlerzustands nach dem vorhergehenden Prozessrauschen, und wird typischerweise als konstant angenommen.
Des Weiteren kann ein linearisiertes Beobachtungsmodell um den Fehlerzustand δx_k 214 herum bereitgestellt werden
wobei h_k(x̌_k, 0) der ideale Beobachtungszustand unter der Annahme ist, dass die Messung bzw. Beobachtung rauschfrei ist. H_kist die erste Ableitung des beobachteten Fehlerzustands nach dem idealen Fehlerzustand. M_kist die erste Ableitung des beobachteten Fehlerzustands nach dem Messrausch, und wird typischerweise als konstant angenommen.
Anhand des o.g. Bewegungsmodells kann der Basis-Zustandsvektor bzw. der nominale Zustand x̌ _k bzw. x̌_k berechnet werden
In einem anschließenden Schritt kann die Fehlerkovarianz P̌_k aus dem Bewegungsmodell aktualisiert werden, durch
wobei Q die Kovarianz des Prozessrauschens ist.
Nachfolgenden kann die Kalman-Verstärkung K_k berechnet werden, als
wobei R die Kovarianz des Messrauschens ist.
Es kann dann anhand des o.g. Beobachtungsmodells ein Schätzwert 213 des Fehlerzustands ermittelt werden, als
wobei y _k ein Messwert 213 ist.
Der aktuelle Zustandsvektor ergibt sich dann als
Schließlich kann die geschätzter bzw. korrigierte Fehlerkovarianz P̂_k ermittelt werden, als
Die Sensordaten 211 und/oder die Messwerte 213 können von den unterschiedlichen Sensoren 201, 202, 203, 204, 205, 206 mit einer unterschiedlichen Frequenz und/oder zu unterschiedlichen Zeitpunkten bereitgestellt werden. Beispielsweise kann die IMU 201 ausgebildet sein, Sensordaten 211 mit einer Frequenz von 100Hz bereitzustellen. Beispielhafte Frequenzen für den Höhenstandsensor 202 sind 100Hz, für den GNSS-Sensor 203 10Hz, für den Radarsensor 204 13Hz, für den Lidarsensor 205 25Hz und/oder für die Kamera 206 20Hz.
Durch die Abtast- bzw. Messfrequenz der IMU 201 kann der zeitliche Abstand Δt zwischen zwei Zeitpunkten k - 1 und k festgelegt sein. Die ESEKF-Einheit 220 kann eingerichtet sein, einen aktualisierten Schätzwert δx̂_k des Fehlerzustands auf Basis eines Messwertes 213 zumindest eines weiteren Sensors 202, 203, 204, 205, 206 bereitzustellen, sobald der Messwert 213 verfügbar ist. Dies kann asynchron zu der Taktung durch die IMU 201 erfolgen. Es kann angenommen werden, dass zwischen zwei Zeitpunkten k - 1 und k n der N weiteren Sensoren 202, 203, 204, 205, 206 (z.B. N = 5) Messwerte 213 bereitstellen. Die ESEKF-Einheit 220 kann dann eingerichtet sein, den Schätzwert δx̂_k des Fehlerzustands n Mal zwischen den zwei Zeitpunkten k - 1 und k zu aktualisieren, jeweils auf Basis der Messwerte 213 eines der n Sensoren, für die ein neuer Messwert 213 bereitgestellt wird. Dabei kann jeweils die Fehlerkovarianz P̂_k aktualisiert werden.
Zur Berechnung des aktuellen Zustandsvektors x̂_k kann dann der jeweils aktuellste Schätzwert δx̂_k des Fehlerzustands verwendet werden. So kann eine robuste und zuverlässige Fusion der Sensordaten 211 von mehreren Sensoren ermöglicht werden.
3 zeigt eine weitere Vorrichtung 200 zur Ermittlung eines Schätzwertes 215 für einen Zustandsvektor. Die Vorrichtung 200 umfasst eine Mehrzahl von ESEKF-Einheiten 320 für die entsprechende Mehrzahl von weiteren Sensoren 202, 203, 204, 205, 206. Jede einzelne ESEKF-Einheit 320 ist ausgebildet, auf Basis der Messwerte 213 des jeweiligen Sensors 202, 203, 204, 205, 206 jeweils einen Teil-Schätzwert 314 des Fehlerzustands zu ermitteln. Die jeweils aktuellen Teil-Schätzwerte 314 des Fehlerzustands der einzelnen ESEKF-Einheiten 320 können in einer Speichereinheit 321 gespeichert werden.
Auf Basis der Güte und/oder Genauigkeit der Messwerte 213 der einzelnen Sensoren können Gewichte 322 für die Teil-Schätzwerte 314 des Fehlerzustands der einzelnen ESEKF-Einheiten 320 ermittelt werden. Es kann dann in einer Filtereinheit 323, insbesondere mittels eines Weigthed Least Square (WLSQ) Filters, auf Basis der Gewichte 322 und auf Basis der Teil-Schätzwerte 314 des Fehlerzustands der einzelnen ESEKF-Einheiten 320 ein Schätzwert 214 des Fehlerzustands ermittelt werden, der dann in der Korrektureinheit 230 dazu verwendet werden kann, den aktuellen Zustandsvektor 215 zu bestimmen.
Es werden somit Vorrichtungen 200 beschrieben, mit denen der Schätzwert eines Zustandsvektors eines Fahrzeugs 100 in präziser Weise auf Basis der Sensordaten 211 einer Mehrzahl von Sensoren 106 ermittelt werden kann. Ein oder mehrere Inertialsensoren (IMU) 201 liefern Messwerte bzw. Sensordaten 211 in Bezug auf die translatorischen Beschleunigungen a und die rotationalen Drehraten w (z.B. mit einer Messfrequenz von ca. 100Hz). Der Fahrzustandsvektor x, insbesondere die Fahrzeugposition p , die Fahrzeugorientierung q und/oder die Fahrzeuggeschwindigkeit v, kann durch ein Bewegungsmodell des Fahrzeugs 100 prädiziert werden. Die Integrationen bzw. das Dead Reckoning der Bewegungseingänge $\underline{u} = (\begin{matrix} \underline{a} \\ \underline{w} \end{matrix})$
kann das drifthafte Bewegungsmodell abbilden. Die Prädiktion des Fahrzustandsvektors aus Bewegungsmodell kann als nominaler Zustand 212 bezeichnet werden.
Ein Höhenstandsensor (HS) 202 detektiert die Höheninformationen zwischen dem Fahrzeugaufbau und der Achse (z.B. mit einer Messfrequenz von ca. 100Hz). Die Höhenstandsensoren des Fahrzeugs 100 können genutzt werden, um die Nick- und/oder Wankbewegung des Fahrzeugs 100 zu erfassen. Die Ausgänge des Sensormodells 210 der Höhenstandsensorn 202 bilden eine Rotationsmatrix aus Wank- und/oder Nickwinkel (R_HS) als Messwert 213.
Der Global Navigation Satellite System (GNSS) Sensor 203 des Fahrzeugs 100 liefert die absolute Fahrzeugposition p _abs und die Fahrzeuggeschwindigkeit aus dem Doppler-Effekt (v _dop) in einem globalen Koordinatensystem (z.B. mit einer Messfrequenz von ca. 10Hz). Das GNSS Modell 210 beschreibt den Zusammenhang zwischen den Sensorgrößen und den Zustandsvariablen. Wenn die Zustandsvariablen in dem globalen Koordinatensystem North-East-Down (NED) definiert werden, dann ist das Sensormodell 210 ggf. eine Einheitsmatrix in Bezug auf die Fahrzeugposition und/oder die Fahrzeuggeschwindigkeit.
Der Lidarsensor 205 erfasst die Umgebung und speichert die reflektierten Punkte in sogenannten Punktwolken (z.B. mit einer Messfrequenz von ca. 25Hz). Aus den Punktwolken für zwei verschiedene Zeitpunkte kann die Fahrzeugeigenbewegung extrahiert werden. Die relative Bewegung des Fahrzeugs 100 zwischen verschiedenen Zeitpunkten kann in einem Lidar-Modell 210 durch den Iterative Closest Point (ICP) Algorithmus geschätzt werden. Daraus ergibt sich die Rotations- (R _L) und Translationsbewegung (t _L) zwischen zwei Zeitpunkten.
Der Radarsensor 204 erfasst reflektierte Punkte sowie die radiale relative Geschwindigkeit der Fahrumgebung (z.B. mit einer Messfrequenz von ca. 13Hz). Die Verarbeitung der Punktwolken in einem Radarmodell 210 kann, ähnlich wie im Lidarmodell 210, mit einem ICP Algorithmus erfolgen. Dadurch ergibt sich die Rotations- (R _R) und Translationsbewegung (t _R) des Fahrzeugs 100. Zudem können die Punktwolken mit den Dopplergeschwindigkeiten anhand von ein oder mehreren statistischen Methoden verarbeitet werden, um sogenannte Inlier (d.h. ein oder mehrere stationäre Objekte in der Umgebung) zu identifizieren. Basierend darauf können Geschwindigkeiten des Fahrzeugs in x- und y-Richtung (v _R) ermittelt werden.
Die Kamera 206 liefert fortlaufend Bilder der Fahrzeugumgebung. Dabei kann die Unterscheidung der Bilder zu unterschiedlichen Zeitpunkten stattfinden. Die Bilder werden anschließend abgeglichen und die Verschiebung der Pixel pro Zeit stellt Informationen zu der Geschwindigkeit eines Pixels bzw. Objekts bereit. Der optische Fluss kann durch verschiedene Methoden geschätzt werden (z.B. durch neuronale Netze, die „Lucas Kanade Methode“, die „Farneback Methode“, etc.). Tiefeninformation in Bezug auf Pixel mit entsprechenden Geschwindigkeiten im Pixelkoordinatensystem (insbesondere der optische Fluss) können durch unterschiedliche Schätz- und Projektionsverfahren bereitgestellt werden. Ausgänge des Kameramodells 210 sind die Rotations- (R _K) und Translationsbewegung (t _K) des Fahrzeugs 100.
Es kann ein Error State Extended Kalman Filter (ESEKF) verwendet werden, um die Prädiktion aus Bewegungsmodell und die Beobachtungsgrößen aus den unterschiedlichen Sensormodellen 210 zu fusionieren, und um einen Fehlerzustand δx̂_k als Korrektur der Zustandsvariablen zu ermitteln.
Die Daten der einzelnen Beobachtungssensoren 202. 203. 204. 205. 206 werden typischerweise zu verschiedenen Zeitpunkten abgetastet bzw. bereitgestellt, und es kann basierend darauf der Fehlerzustand geschätzt werden. Solange die ESEKF-Einheit 202 einen Messwert 213 aus zumindest einem Sensormodell 210 erhält, kann der Fehlerzustand und basierend darauf der Fahrzeugzustand ermittelt werden. Dies ermöglicht aufgrund relativ hoher Schätzfrequenzen eine nahezu kontinuierliche Approximation des Fahrzeugzustands. Hierbei kann die ESEKF-Einheit 220 derart skaliert werden, dass die Schätzung auf Basis der Kombination der Messwerte 213 von unterschiedlichen Sensoren 202, 203, 204, 205, 206 durchgeführt werden kann. Dabei sind typischerweise nicht alle Messwerte 312 für die Schätzung erforderlich. Dies hat zur Folge, dass der ESEKF eine relativ hohe Robustheit gegenüber dem Ausfall von ein oder mehreren Sensoren aufweist.
Anstelle der Nutzung eines gemeinsamen ESEKF für die Messwerte 213 von mehreren Sensoren kann (wie in 3 dargestellt), kann für jeden Beobachtungssensor 202-206 ein eigener ESEKF verwendet werden. Dadurch findet die Fehlerschätzung für die unterschiedlichen Beobachtungssensoren 202-206 unabhängig voneinander statt.
Der Fehlerzustand der einzelnen ESEKF kann jeweils für eine bestimmte Zeitsequenz gespeichert werden. Parallel dazu kann eine Gewichtung 322 der einzelnen ESEKF geschätzt werden. Der endgültige Fehlerzustand 214 kann dann mit einem sogenannten Weighted Least Squares (WLSQ) Filter angenähert werden. Die Schätzung anhand von separaten ESEKFs weist eine relativ hohe Robustheit gegenüber Ausreißern einzelner Sensormodelle 210 auf.
4 zeigt ein Ablaufdiagramm eines beispielhaften (Computerimplementierten) Verfahrens 400 zur Ermittlung eines Schätzwertes 215 eines Zustandsvektors eines Fahrzeugs 100, wobei der Zustandsvektor eine Mehrzahl von Zustandsvariablen, insbesondere die Position, die Geschwindigkeit und/oder die Orientierung des Fahrzeugs, umfasst.
Das Verfahren 400 umfasst das Ermitteln 401, auf Basis der Sensordaten 211 einer Inertialen Messeinheit 201, insbesondere einer IMU, und auf Basis eines Bewegungsmodells des Fahrzeugs 100, eines Basis-Schätzwertes 212 des Zustandsvektors. Des Weiteren umfasst das Verfahren 400 das Ermitteln 402, anhand von ein oder mehreren Beobachtungssensoren 202, 203, 204, 205, 206 (insbesondere anhand von drei oder mehr unterschiedlichen Beobachtungssensoren), ein oder mehrerer Messwerte 213 in Bezug auf ein oder mehrere der Mehrzahl von Zustandsvariablen.
Das Verfahren 400 umfasst ferner das Ermitteln 403, auf Basis der ein oder mehreren Messwerte 213 und unter Verwendung zumindest eines Error State Extended Kalman Filters, kurz ESEKF, eines Schätzwertes 214 eines Korrekturvektors (bzw. eines Fehlerzustands). Des Weiteren umfasst das Verfahren 400 das Ermitteln 404 des Schätzwertes 215 des Zustandsvektors auf Basis des Basis-Schätzwertes 212 des Zustandsvektors und auf Basis des Schätzwertes 214 des Korrekturvektors.
Durch die in diesem Dokument beschriebenen Maßnahmen kann eine präzise Schätzung des Fahrzeugzustands eines Fahrzeugs 100 erfolgen, insbesondere unabhängig von Fahrwerkparametern und/oder Reifenparametern, welche appliziert werden müssen und sich während des Fahrens ändern können. Die Schätzung weist dabei eine hohe Robustheit bei Ausfall von ein oder mehreren Beobachtungssensoren 202-206 auf. Des Weiteren können Fehler aus einer Zeitsynchronisierung und aufgrund von Latenzzeiten reduziert werden.
Es sei angemerkt, dass in diesem Dokument Vektoren und/oder Matrizen in austauschbarer Weise mit einem Unterstrich „_“ oder fett dargestellt sind.
Die vorliegende Erfindung ist nicht auf die gezeigten Ausführungsbeispiele beschränkt. Insbesondere ist zu beachten, dass die Beschreibung und die Figuren nur beispielhaft das Prinzip der vorgeschlagenen Verfahren, Vorrichtungen und Systeme veranschaulichen sollen.

Claims

Vorrichtung (200) zur Ermittlung eines Schätzwertes (215) eines Zustandsvektors eines Fahrzeugs (100) mit einer Mehrzahl von Zustandsvariablen; wobei die Vorrichtung (200) eingerichtet ist, - auf Basis der Sensordaten (211) einer Inertialen Messeinheit (201) und auf Basis eines Bewegungsmodells des Fahrzeugs (100) einen Basis-Schätzwert (212) des Zustandsvektors zu ermitteln; - anhand von ein oder mehreren Beobachtungssensoren (202, 203, 204, 205, 206) ein oder mehrere Messwerte (213) in Bezug auf ein oder mehrere der Mehrzahl von Zustandsvariablen zu ermitteln; - auf Basis der ein oder mehreren Messwerte (213) und unter Verwendung zumindest eines Error State Extended Kalman Filters, kurz ESEKF, einen Schätzwert (214) eines Korrekturvektors zu ermitteln; und - den Schätzwert (215) des Zustandsvektors auf Basis des Basis-Schätzwertes (212) des Zustandsvektors und auf Basis des Schätzwertes (214) des Korrekturvektors zu ermitteln.
Vorrichtung (200) gemäß Anspruch 1, wobei - ein Beobachtungssensor (202, 203, 204, 205, 206) eingerichtet ist, Sensordaten (211) in Bezug auf ein oder mehrere Beobachtungsgrößen zu erfassen; und - die Vorrichtung (200) eingerichtet ist, mittels eines Sensormodells (210) des Beobachtungssensors (202, 203, 204, 205, 206) auf Basis der Sensordaten (211) des Beobachtungssensors (202, 203, 204, 205, 206) ein oder mehrere Messwerte (213) in Bezug auf ein oder mehrere der Mehrzahl von Zustandsvariablen zu ermitteln.
Vorrichtung (200) gemäß Anspruch 2, wobei die ein oder mehreren Beobachtungssensoren (202, 203, 204, 205, 206) umfassen, - eine Mehrzahl von Höhenstandssensoren (202), die jeweils eingerichtet sind, Sensordaten (211) in Bezug auf einen Höhenstand jeweils eines Rades (110) des Fahrzeugs (100) zu erfassen; und wobei das Sensormodell (210) für die Mehrzahl von Höhenstandssensoren (202) ausgebildet ist, auf Basis der Sensordaten (211) der Mehrzahl von Höhenstandssensoren (202) Messwerte (213) in Bezug auf eine Orientierung des Fahrzeugs (100) zu ermitteln; - einen satellitenbasierten Positionssensor (203), der eingerichtet ist, Sensordaten (211) in Bezug auf eine Position des Fahrzeugs (100) zu erfassen; und wobei das Sensormodell (210) für den Positionssensor (203) ausgebildet ist, auf Basis der Sensordaten (211) des Positionssensors (203) Messwerte (213) in Bezug auf die Position und/oder die Geschwindigkeit des Fahrzeugs (100) zu ermitteln; - einen Radarsensor (204); wobei das Sensormodell (210) für den Radarsensor (204) ausgebildet ist, auf Basis der Sensordaten (211) des Radarsensors (204) Messwerte (213) in Bezug auf die Geschwindigkeit, die Rotationsbewegung und/oder die Translationsbewegung des Fahrzeugs (100) zu ermitteln; und/oder - einen Lidarsensor (205); wobei das Sensormodell (210) für den Lidarsensor (205) ausgebildet ist, auf Basis der Sensordaten (211) des Lidarsensors (205) Messwerte (213) in Bezug auf die Rotationsbewegung und/oder die Translationsbewegung des Fahrzeugs (100) zu ermitteln; und/oder - eine Kamera (206), die ausgebildet ist, Sensordaten (211) in Bezug auf ein Umfeld des Fahrzeugs (100) zu erfassen; wobei das Sensormodell (210) für die Kamera (206) ausgebildet ist, auf Basis der Sensordaten (211) der Kamera (206) Messwerte (213) in Bezug auf die Rotationsbewegung und/oder die Translationsbewegung des Fahrzeugs (100) zu ermitteln.
Vorrichtung (200) gemäß einem der vorhergehenden Ansprüche, wobei die Mehrzahl von Zustandsvariablen umfassen, - eine Position des Fahrzeugs (100); - eine Orientierung des Fahrzeugs (100); und/oder - eine Fahrgeschwindigkeit des Fahrzeugs (100).
Vorrichtung (200) gemäß einem der vorhergehenden Ansprüche, wobei - die Vorrichtung (200) eingerichtet ist, auf Basis der Sensordaten (211) der Inertialen Messeinheit (201) ein oder mehrere Eingangswerte u des Bewegungsmodells für einen Zeitpunkt k - 1 zu ermitteln; und - das Bewegungsmodell ausgebildet ist, auf Basis des Schätzwertes x _̂k-1 (214) des Zustandsvektors x für den Zeitpunkt k - 1 und auf Basis der ein oder mehrere Eingangswerte u für den Zeitpunkt k - 1 den Basis-Schätzwert x̌_k(212) des Zustandsvektors x für einen nachfolgenden Zeitpunkt k zu ermitteln.
Vorrichtung (200) gemäß Anspruch 5, wobei die Vorrichtung (200) eingerichtet ist, den Basis-Schätzwert (212) x̌ _k des Zustandsvektors für den Zeitpunkt k zu ermitteln, anhand ein oder mehrerer der Formeln des Bewegungsmodells

wobei p̂ _k-1 der Schätzwert (215) der Position des Fahrzeugs (100) für den Zeitpunkt k - 1 ist; wobei Δt ein zeitlicher Abstand zwischen den Zeitpunkten k - 1 und k ist; wobei v̂ _k-1 der Schätzwert (215) der Geschwindigkeit des Fahrzeugs (100) für den Zeitpunkt k - 1 ist; wobei ${\hat{R}}_{k - 1}^{N E D}$
der Schätzwert (215) einer Rotationsmatrix des Fahrzeugs (100) für den Zeitpunkt k - 1 von Fahrzeugkörperkoordinaten auf NED-Koordinaten ist; wobei a _k-1 die für den Zeitpunkt k - 1 gemessene Beschleunigung des Fahrzeugs (100) ist; wobei g die Erdbeschleunigung ist; wobei p̌ _k der Basis-Schätzwert (212) der Position des Fahrzeugs (100) für den Zeitpunkt k ist; wobei v̌ _k der Basis-Schätzwert (212) der Geschwindigkeit des Fahrzeugs (100) für den Zeitpunkt k ist; wobei w _k-1 die für den Zeitpunkt k - 1 gemessene Drehrate des Fahrzeugs (100) ist; wobei eul2dcm() eine Transformation von Eulerwinkeln auf die Rotationsmatrix in NED-Koordinaten ist; wobei $Ř_{k}^{N E D}$
der Basis-Schätzwert (212) der Rotationsmatrix des Fahrzeugs (100) für den Zeitpunkt k - 1 von Fahrzeugkörperkoordinaten auf NED-Koordinaten ist; wobei dcm2eul() eine Transformation von der Rotationsmatrix in NED-Koordinaten auf Eulerwinkel ist; und wobei θ̌ _k der Basis-Schätzwert (212) der Orientierung des Fahrzeugs (100) für den Zeitpunkt k ist.
Vorrichtung (200) gemäß einem der vorhergehenden Ansprüche, wobei die Vorrichtung (200) eingerichtet ist, - einen Schätzwert einer Fehlerkovarianz des Bewegungsmodells zu ermitteln; und - den Schätzwert (214) des Korrekturvektors auf Basis des Schätzwerts der Fehlerkovarianz zu ermitteln.
Vorrichtung (200) gemäß Anspruch 7, wobei die Vorrichtung (200) eingerichtet ist, den Schätzwert der Fehlerkovarianz zu ermitteln, anhand der Formel
wobei p̂ _k-1 der Schätzwert der Fehlerkovarianz für den Zeitpunkt k - 1 ist; wobei F_k-1 die Jacobi-Matrix des Bewegungsmodells und/oder die erste Ableitung des Basis-Schätzwertes (212) des Zustandsvektors für den Zeitpunkt k nach dem Schätzwert (214) des Korrekturvektors für den Zeitpunkt k - 1 ist; wobei P̌_k der Schätzwert der Fehlerkovarianz für den Zeitpunkt k ist; wobei Q_k-1 die Kovarianz von Prozessrauschen ist; und wobei L_k-1 eine Matrix, insbesondere eine konstante Matrix, ist.
Vorrichtung (200) gemäß einem der vorhergehenden Ansprüche, wobei die Vorrichtung (200) eingerichtet ist, den Schätzwert (214) des Korrekturvektors auf Basis des Basis-Schätzwertes (212) des Zustandsvektors, auf Basis eines Sensormodells für einen Beobachtungssensor (202, 203, 204, 205, 206) und auf Basis eines Messwertes (213) des Beobachtungssensors (202, 203, 204, 205, 206) zu ermitteln, insbesondere als
wobei δx̂ _k. der Schätzwert (214) des Korrekturvektors für den Zeitpunkt k ist; wobei y _k der Messwert (213) für den Zeitpunkt k ist; wobei x̌_k der Basis-Schätzwertes (212) für den Zustandsvektor für den Zeitpunkt k ist; wobei h_k() das Sensormodell für den Beobachtungssensor (202, 203, 204, 205, 206) ist; und wobei K_k ein Kalman-Verstärkungsfaktor ist.
Vorrichtung (200) gemäß Anspruch 9 mit Rückbezug auf Ansprüche 7 bis 8, wobei die Vorrichtung (200) eingerichtet ist, den Kalman-Verstärkungsfaktor auf Basis des Schätzwertes der Fehlerkovarianz für den Zeitpunkt k zu ermitteln, insbesondere unter Verwendung der Formel
wobei H_k eine Beobachtungsmatrix ist, die von dem Sensormodell für den Beobachtungssensor (202, 203, 204, 205, 206) abhängt; und wobei R eine Matrix zur Beschreibung von Rauschen des Beobachtungssensors (202, 203, 204, 205, 206) ist.
Vorrichtung (200) gemäß Anspruch 10, wobei die Vorrichtung (200) eingerichtet ist, auf Basis des Kalman-Verstärkungsfaktors für den Zeitpunkt k und auf Basis der Beobachtungsmatrix für den Zeitpunkt k einen Schätzwert P̂_k der Fehlerkovarianz für den Zeitpunkt k zu aktualisieren, insbesondere als
wobei I eine Einheitsmatrix ist.
Vorrichtung (200) gemäß einem der Ansprüche 9 bis 11, wobei die Vorrichtung (200) eingerichtet ist, für jeden Beobachtungssensor (202, 203, 204, 205, 206), der zwischen den Zeitpunkten k - 1 und k einen Messwert (213) bereitstellt, nacheinander, - jeweils auf Basis des aktuellen Schätzwerts der Fehlerkovarianz einen Kalman-Verstärkungsfaktor zu ermitteln; - auf Basis des Kalman-Verstärkungsfaktors jeweils einen Schätzwert (214) des Korrekturvektors für den Zeitpunkt k zu ermitteln; und - auf Basis des Kalman-Verstärkungsfaktors den Schätzwert der Fehlerkovarianz zu aktualisieren.
Vorrichtung (200) gemäß einem der vorhergehenden Ansprüche, wobei die Vorrichtung (200) eingerichtet ist, - für jeden einer Mehrzahl von Beobachtungssensoren (202, 203, 204, 205, 206) auf Basis des Messwertes (213) des jeweiligen Beobachtungssensors (202, 203, 204, 205, 206) und unter Verwendung jeweils eines ESEKFs jeweils einen Teil-Schätzwert (314) für den Korrekturvektor zu ermitteln; und - den Schätzwert (214) des Korrekturvektors auf Basis der Mehrzahl von Teil-Schätzwertes (314) zu ermitteln.
Vorrichtung (200) gemäß Anspruch 13, wobei die Vorrichtung (200) eingerichtet ist, - für die Mehrzahl von Beobachtungssensoren (202, 203, 204, 205, 206) eine entsprechende Mehrzahl von Gewichten (322) zu ermitteln; wobei das Gewicht (322) für einen Beobachtungssensor (202, 203, 204, 205, 206) von einer Messgenauigkeit und/oder einer Konfidenz des Beobachtungssensors (202, 203, 204, 205, 206) abhängt; und - den Schätzwert (214) des Korrekturvektors auch auf Basis der Mehrzahl von Gewichten (322), insbesondere unter Verwendung eines Weighted Least Squares Filters, zu ermitteln.
Vorrichtung (200) gemäß einem der Ansprüche 13 bis 14, wobei die Vorrichtung (200) eingerichtet ist, - die Mehrzahl von Teil-Schätzwerte (314) für eine Sequenz von Zeitpunkten bis zu dem Zeitpunkt k zu ermitteln; und - den Schätzwert (214) des Korrekturvektors für den Zeitpunkt k auf Basis der Mehrzahl von Teil-Schätzwerte (314) für die Sequenz von Zeitpunkten zu ermitteln.
Vorrichtung (200) gemäß einem der vorhergehenden Ansprüche, wobei die Vorrichtung (200) eingerichtet ist, Messwerte (213) von drei oder mehr unterschiedlichen Beobachtungssensoren (202, 203, 204, 205, 206) zu ermitteln und zur Ermittlung des Schätzwertes (214) des Korrekturvektors zu verwenden.
Vorrichtung (200) gemäß einem der vorhergehenden Ansprüche, wobei die Vorrichtung (200) eingerichtet ist, das Fahrzeug (100) in Abhängigkeit von dem ermittelten Schätzwert (215) des Zustandsvektors zumindest teilweise automatisiert längs- und/oder querzuführen.
Verfahren (400) zur Ermittlung eines Schätzwertes (215) eines Zustandsvektors eines Fahrzeugs (100) mit einer Mehrzahl von Zustandsvariablen; wobei das Verfahren (400) umfasst, - Ermitteln (401), auf Basis der Sensordaten (211) einer Inertialen Messeinheit (201) und auf Basis eines Bewegungsmodells des Fahrzeugs (100), eines Basis-Schätzwertes (212) des Zustandsvektors; - Ermitteln (402), anhand von ein oder mehreren Beobachtungssensoren (202, 203, 204, 205, 206), ein oder mehrerer Messwerte (213) in Bezug auf ein oder mehrere der Mehrzahl von Zustandsvariablen; - Ermitteln (403), auf Basis der ein oder mehreren Messwerte (213) und unter Verwendung zumindest eines Error State Extended Kalman Filters, kurz ESEKF, eines Schätzwertes (214) eines Korrekturvektors; und - Ermitteln (404) des Schätzwertes (215) des Zustandsvektors auf Basis des Basis-Schätzwertes (212) des Zustandsvektors und auf Basis des Schätzwertes (214) des Korrekturvektors.