DE102022200142A1 - Schätzen eines Bewegungszustands eines Fahrzeugs aus Kameradaten mithilfe von maschinellem Lernen - Google Patents

Abstract

Ein Verfahren zum Schätzen eines Bewegungszustands eines Fahrzeugs (1), das mit einer Kamera (5) zum Erfassen einer Umgebung des Fahrzeugs (1) ausgestattet ist, umfasst: Empfangen von Kameradaten (8), die von der Kamera (5) erzeugt wurden; Eingeben der Kameradaten (8) in einen Algorithmus (10), der mit historischen Kameradaten und den historischen Kameradaten zugeordneten Referenzbewegungsdaten trainiert wurde, um die Kameradaten (8) in Bewegungsdaten (12) umzuwandeln, die einen aktuellen Bewegungszustand des Fahrzeugs (1) beschreiben; und Bestimmen eines geschätzten Bewegungszustands des Fahrzeugs (1) aus den Bewegungsdaten (12) durch einen Zustandsschätzer (23).

Description

• Gebiet der Erfindung
• Die Erfindung betrifft ein Verfahren zum Schätzen eines Bewegungszustands eines Fahrzeugs. Des Weiteren betrifft die Erfindung ein Steuergerät und ein Computerprogramm zum Ausführen eines solchen Verfahrens und ein computerlesbares Medium, auf dem ein solches Computerprogramm gespeichert ist.
• Stand der Technik
• Messdaten kostengünstiger Inertialsensoren, wie sie zum Erfassen eines Bewegungszustands eines Fahrzeugs eingesetzt werden, enthalten häufig Messfehler, die sich infolge von Temperaturänderungen verstärken können. Um die Messfehler klein zu halten, können die Messdaten beispielsweise mithilfe einer sogenannten Strapdown-Rechnung korrigiert werden. Dabei können über einen längeren Zeitraum hinweg Integrationsfehler auftreten, die zu Ungenauigkeiten bei der Bestimmung von Geschwindigkeits- oder Drehwinkelwerten führen können. Aus diesem Grund werden häufig zusätzlich Positionsdaten eines globalen Navigationssatellitensystems und/oder Messdaten eines oder mehrerer Raddrehzahlsensoren herangezogen, um die Ergebnisse der Strapdown-Rechnung zu korrigieren. Die Korrektur kann beispielsweise in einem Kalman-Filter stattfinden.
• Offenbarung der Erfindung
• Vor diesem Hintergrund werden nachstehend ein Verfahren zum Schätzen eines Bewegungszustands eines Fahrzeugs, ein entsprechendes Steuergerät, ein entsprechendes Computerprogramm und ein entsprechendes computerlesbares Medium gemäß den unabhängigen Ansprüchen vorgestellt. Vorteilhafte Weiterbildungen und Verbesserungen des hier vorgestellten Ansatzes ergeben sich aus der Beschreibung und sind in den abhängigen Ansprüchen beschrieben.
• Vorteile der Erfindung
• Ausführungsformen der vorliegenden Erfindung ermöglichen eine hochgenaue Schätzung eines Bewegungszustands eines Fahrzeugs basierend auf Kameradaten, ohne dass hierzu Sensordaten einer zusätzlichen Sensorik, etwa eines Inertialsensors oder eines satellitengestützten Ortungssensors, zwingend erforderlich sind.
• Ein erster Aspekt der Erfindung betrifft ein computerimplementiertes Verfahren zum Schätzen eines Bewegungszustands eines Fahrzeugs, das mit einer oder mehreren Kameras zum Erfassen einer Umgebung des Fahrzeugs ausgestattet ist. Das Verfahren umfasst zumindest die folgenden Schritte: (i) Empfangen von Kameradaten, die von der Kamera erzeugt wurden; (ii) Eingeben der Kameradaten in einen Algorithmus, der mit historischen Kameradaten und den historischen Kameradaten zugeordneten Referenzbewegungsdaten trainiert wurde, um die Kameradaten in Bewegungsdaten umzuwandeln, die einen aktuellen Bewegungszustand des Fahrzeugs beschreiben; und (iii) Bestimmen eines geschätzten Bewegungszustands des Fahrzeugs aus den Bewegungsdaten durch einen Zustandsschätzer.
• Beispielsweise können die Kameradaten in Schritt (i) in mehreren aufeinanderfolgenden Zeitschritten empfangen werden, wobei in jedem Zeitschritt Schritt (ii) bis (iii) ausgeführt werden. Beispielsweise können die Bewegungsdaten in einem aktuellen Zeitschritt in den Zustandsschätzer eingegeben werden, um Schätzdaten im aktuellen Zeitschritt zu erhalten, die den geschätzten Bewegungszustand des Fahrzeugs zu einem dem aktuellen Zeitschritt nachfolgenden Zeitschritt beschreiben. Dabei können die Schätzdaten beispielsweise Werte für eine geschätzte Geschwindigkeit und/oder einen geschätzten Drehwinkel des Fahrzeugs in Bezug auf eine oder mehrere Raumachsen umfassen.
• Der Zustandsschätzer kann beispielsweise ein Kalman-Filter, eine Variante eines Kalman-Filters, ein Partikelfilter oder eine Kombination aus mindestens zwei dieser Beispiele sein.
• Unter „Algorithmus“ kann ein Klassifikations- und/oder Regressionsalgorithmus verstanden werden, der mit einer Methode des maschinellen Lernens konfiguriert wurde, um einem bestimmten Bild, wie es von einer Kamera aufgenommen wird, d. h. einer zwei- oder mehrdimensionalen Matrix aus bestimmten Farb- und/oder Helligkeitswerten, und/oder einer Folge mindestens zweier solcher Bilder, einen oder mehrere Schätzwerte für mindestens eine Bewegungsgröße, die geeignet ist, um den aktuellen Bewegungszustand des Fahrzeugs zu charakterisieren, zuzuordnen. Eine geeignete Bewegungsgröße kann beispielsweise eine Beschleunigung, Drehrate, Geschwindigkeit oder ein Drehwinkel sein. Die Bewegungsgrößen können jeweils auf eine, zwei oder drei verschiedene Raumachsen eines Koordinatensystems, in dem sich das Fahrzeug bewegt und/oder das sich mit dem Fahrzeug bewegt, bezogen sein.
• Die historischen Kameradaten können von einer oder mehreren realen Kameras bereitgestellt worden sein, beispielsweise von der Kamera des Fahrzeugs und/oder mindestens einer anderen Kamera mindestens eines anderen Fahrzeugs. Die historischen Kameradaten können aber auch aus einer Simulation der Kamera, des Fahrzeugs und/oder der Umgebung des Fahrzeugs stammen. Die Referenzbewegungsdaten können beispielsweise Geschwindigkeitswerte, Beschleunigungswerte, Drehwinkelwerte und/oder Drehratenwerte umfassen, die Bildern in den historischen Kameradaten zugeordnet sein können.
• Das Verfahren kann beispielsweise automatisch durch einen Prozessor eines Steuergeräts des Fahrzeugs ausgeführt werden. Das Steuergerät kann zusätzlich konfiguriert sein, um eine satellitengestützte Navigationsfunktion und/oder eine oder mehrere Fahrerassistenzfunktionen wie beispielsweise ABS oder ESP auszuführen, mit denen das Fahrzeug in Abhängigkeit vom geschätzten Bewegungszustand gelenkt, beschleunigt und/oder abgebremst werden kann. Hierzu kann das Fahrzeug eine entsprechende Aktorik umfassen, beispielsweise in Form eines Lenkaktors, eines Bremsaktors, eines Motorsteuergeräts, eines elektrischen Antriebsmotors oder einer Kombination aus mindestens zwei dieser Beispiele.
• Das Fahrzeug kann ein Kraftfahrzeug sein, etwa in Form eines Pkw, Lkw, Busses oder eines Motorrads. Das Fahrzeug kann von einem Menschen geführt sein oder ganz oder teilweise autonom bewegt werden. Im weiteren Sinn kann unter einem Fahrzeug auch ein Flugzeug, ein Schiff, ein Kran oder ein autonomer, mobiler Roboter verstanden werden. Beispielsweise kann das vor- und nachstehend beschriebene Verfahren genutzt werden, um den Bewegungszustand eines Hakens des Krans zu schätzen.
• Die Kamera kann beispielsweise in einen Front-, Heck- und/oder Seitenbereich des (Kraft-) Fahrzeugs eingebaut sein. Alternativ kann es sich bei der Kamera um eine Komponente eines Smartphones oder einer Smartwatch handeln.
• Ein zweiter Aspekt der Erfindung betrifft ein Steuergerät, das einen Prozessor umfasst, der konfiguriert ist, um das vor- und nachstehend beschriebene Verfahren auszuführen. Das Steuergerät kann Hardware- und/oder Softwaremodule umfassen. Zusätzlich zum Prozessor kann das Steuergerät einen Speicher und Datenkommunikationsschnittstellen zur Datenkommunikation mit Peripheriegeräten umfassen. Merkmale des Verfahrens können auch als Merkmale des Steuergeräts aufgefasst werden und umgekehrt.
• Weitere Aspekte der Erfindung betreffen ein Computerprogramm und ein computerlesbares Medium, auf dem das Computerprogramm gespeichert ist.
• Das Computerprogramm umfasst Befehle, die einen Prozessor bei Ausführung des Computerprogramms durch den Prozessor veranlassen, das vor- und nachstehend beschriebene Verfahren auszuführen.
• Das computerlesbare Medium kann ein flüchtiger oder nicht flüchtiger Datenspeicher sein. Beispielsweise kann das computerlesbare Medium eine Festplatte, ein USB-Speichergerät, ein RAM, ROM, EPROM oder Flash-Speicher sein. Das computerlesbare Medium kann auch ein einen Download eines Programmcodes ermöglichendes Datenkommunikationsnetzwerk wie etwa das Internet oder eine Datenwolke (Cloud) sein.
• Merkmale des vor- und nachstehend beschriebenen Verfahrens können auch als Merkmale des Computerprogramms und/oder des computerlesbaren Mediums aufgefasst werden und umgekehrt.
• Mögliche Merkmale und Vorteile von Ausführungsformen der Erfindung können unter anderem, und ohne die Erfindung einzuschränken, als auf den nachstehend beschriebenen Ideen und Erkenntnissen beruhend angesehen werden.
• Gemäß einer Ausführungsform können die Bewegungsdaten eine aktuelle Geschwindigkeit, eine aktuelle Beschleunigung, einen aktuellen Drehwinkel und/oder eine aktuelle Drehrate des Fahrzeugs anzeigen. Zusätzlich oder alternativ kann der Zustandsschätzer eine geschätzte Geschwindigkeit und/oder einen geschätzten Drehwinkel des Fahrzeugs bestimmen. Wie bereits erwähnt, können die Kameradaten Farbwerte und/oder Helligkeitswerte umfassen, die vom Algorithmus in Schätzwerte für die vorgenannten Bewegungsgrößen umgesetzt werden können.
• Gemäß einer Ausführungsform können die Bewegungsdaten zusätzlich Varianzen bezüglich des aktuellen Bewegungszustands anzeigen. Zusätzlich oder alternativ kann der Zustandsschätzer zusätzlich Varianzen bezüglich des geschätzten Bewegungszustands bestimmen. Beispielsweise können die Bewegungsdaten aktuelle Werte für eine oder mehrere Bewegungsgrößen umfassen, wobei jedem aktuellen Wert ein entsprechender Varianzwert und/oder mindestens ein sonstiger statistischer Wert, der eine Unsicherheit bezüglich des jeweiligen Werts quantifiziert, zugeordnet sein kann. Diese Varianzen können vom Zustandsschätzer ausgewertet werden, um den geschätzten Bewegungszustand noch genauer zu bestimmen. Dabei kann der Zustandsschätzer geschätzte Varianzen bestimmen, die bei der Bestimmung des geschätzten Bewegungszustands berücksichtigt und/oder zur externen Weiterverarbeitung vom Zustandsschätzer ausgegeben werden können.
• Gemäß einer Ausführungsform kann der Zustandsschätzer ein Kalman-Filter sein. Unter dem Begriff „Kalman-Filter“ kann auch eine Variante eines Kalman-Filters, etwa ein extended Kalman filter oder ein unscented Kalman filter, oder eine Kombination aus mindestens zwei solcher Varianten verstanden werden. Damit kann der geschätzte Bewegungszustand besonders effizient bestimmt werden.
• Gemäß einer Ausführungsform kann der Algorithmus ein künstliches neuronales Netz sein. Das künstliche neuronale Netz kann beispielsweise ein mehrlagiges Perzeptron, ein faltendes neuronales Netz (convolutional neural network) und/oder eine LSTM-Architektur (long short-term memory) umfassen. Damit kann der aktuelle Bewegungszustand des Fahrzeugs sehr genau bestimmt werden. insbesondere kann die Bestimmung genauer erfolgen, als dies mit gängigen Inertialsensoren der Fall wäre. Beispielsweise können damit Ungenauigkeiten aufgrund von Messfehlern, wie sie für Inertialsensoren typisch sind, vermieden werden. Entsprechend genauer kann der geschätzte Bewegungszustand des Fahrzeugs vom Zustandsschätzer bestimmt werden.
• Gemäß einer Ausführungsform kann zum Trainieren des Algorithmus eine Kostenfunktion, die eine Abweichung zwischen den Bewegungsdaten und den Referenzbewegungsdaten quantifiziert, minimiert worden sein. Dabei kann die Kostenfunktion eine Likelihood-Funktion, insbesondere eine Log-Likelihood-Funktion, sein. Anders ausgedrückt kann der Algorithmus unter Berücksichtigung von Unsicherheiten bezüglich der Ein- und/oder Ausgabedaten des Algorithmus und/oder der Referenzbewegungsdaten, mit denen die Ausgabedaten verglichen werden, optimiert worden sein. Der Algorithmus kann beispielsweise in einem Gradientenverfahren durch Backpropagation optimiert worden sein. Möglich ist aber auch eine Optimierung mithilfe von Normalgleichungen. Damit kann das Verfahren weiter verbessert werden.
• Gemäß einer Ausführungsform können die Referenzbewegungsdaten von einem Inertialsensor, einem satellitengestützten Ortungssensor und/oder einem Raddrehzahlsensor erzeugt worden sein. Beispielsweise können die Referenzbewegungsdaten von einem besonders hochwertigen und entsprechend genauen Inertialsensor bereitgestellt worden sein, der aus Kostengründen in einem Fahrzeug üblicherweise nicht verbaut wird. Anders ausgedrückt kann der Bewegungszustand des Fahrzeugs mit einer Genauigkeit geschätzt werden, die mindestens genauso hoch ist, wie wenn die Bewegungsdaten von einem solchen hochwertigen Inertialsensor bereitgestellt werden, jedoch ohne dass der Inertialsensor tatsächlich im Fahrzeug verbaut zu werden braucht.
• Gemäß einer Ausführungsform können zusätzlich Sensordaten, die von einer zusätzlichen Sensorik des Fahrzeugs erzeugt wurden, empfangen und in den Zustandsschätzer eingegeben werden. Auf diese Weise kann die Genauigkeit des Verfahrens erhöht werden. Darüber hinaus kann damit die Ausfallsicherheit des Systems erhöht werden. Beispielsweise kann der geschätzte Bewegungszustand auch dann noch bestimmt werden, wenn die Kamera ausfällt oder in ihrer Funktion beeinträchtigt ist.
• Gemäß einer Ausführungsform kann die zusätzliche Sensorik mindestens einen Inertialsensor, einen satellitengestützten Ortungssensor und/oder einen Raddrehzahlsensor umfassen. In diesem Fall kann für die Bereitstellung der Sensordaten eine deutlich kostengünstigere Sensorik verwendet werden, als wenn der geschätzte Bewegungszustand ausschließlich aus den Sensordaten, d. h. ohne den aus den Kameradaten gewonnenen Bewegungsdaten, bestimmt wird. Zudem ermöglicht dies eine Prognose und Korrektur der Sensordaten auf Basis der Kameradaten. Beispielsweise können somit Fehler in den Ausgabedaten eines Inertialsensors zuverlässig erkannt werden.
• Figurenliste
• Nachfolgend werden Ausführungsformen der Erfindung unter Bezugnahme auf die beigefügten Zeichnungen beschrieben, wobei weder die Zeichnungen noch die Beschreibung als die Erfindung einschränkend auszulegen sind.
• 1 zeigt ein Fahrzeug mit einem Fahrzeugsystem, das eine Kamera und ein Steuergerät gemäß einer Ausführungsform der Erfindung umfasst.
• 2 zeigt das Fahrzeugsystem aus 1 im Detail.
• 3 veranschaulicht einen Algorithmus in Form eines künstlichen neuronalen Netzes zur Verwendung in einem Verfahren gemäß einer Ausführungsform der Erfindung.
• Die Figuren sind lediglich schematisch und nicht maßstabsgetreu. Gleiche Bezugszeichen bezeichnen in den Figuren gleiche oder gleichwirkende Merkmale.
• Ausführungsformen der Erfindung
• 1 zeigt ein Fahrzeug 1, das mit einem Fahrzeugsystem 3 bestehend aus einer Kamera 5 zum Erfassen einer Umgebung des Fahrzeugs 1 und einem Steuergerät 7 zum Auswerten von Kameradaten 8 der Kamera 5 in einem nachstehend beschriebenen Verfahren ausgestattet ist.
• 2 zeigt mögliche Komponenten des Fahrzeugsystems 3 bzw. des Steuergeräts 7.
• In diesem Beispiel umfasst das Steuergerät 7 einen Machine-Learning-Algorithmus 10, nachfolgend kurz Algorithmus 10 genannt, der mit historischen Kameradaten und Referenzbewegungsdaten trainiert wurde, um die Kameradaten 8 in Bewegungsdaten 12 umzuwandeln, die Werte für eine oder mehrere Bewegungsgrößen umfassen. Dementsprechend können die Referenzbewegungsdaten Referenzwerte für diese Bewegungsgröße(n) umfassen, die mit den historischen Kameradaten in geeigneter Weise zeitlich korreliert sein können.
• Der Algorithmus 10 ist hier beispielhaft als ein künstliches neuronales Netz ausgeführt, dessen prinzipieller Aufbau in 3 gezeigt ist. Das Netz umfasst eine Eingabeschicht 15, in die die Kameradaten 8 eingegeben werden, und eine Ausgabeschicht 17 mit einer Mehrzahl von Ausgabeneuronen 19, die die Bewegungsdaten 12 ausgeben, hier Werte für eine Längsgeschwindigkeit v_x, eine Quergeschwindigkeit v_y, eine Vertikalgeschwindigkeit v_z, eine Längsbeschleunigung a_x, eine Querbeschleunigung a_y, eine Vertikalbeschleunigung a_z, einen Rollwinkel ϕ, einen Nickwinkel θ, einen Gierwinkel ψ, eine Rollrate ω_x, eine Nickrate ω_y und eine Gierrate ω_z. Die Eingabeschicht 15 und die Ausgabeschicht 17 sind über eine Mehrzahl hintereinanderliegender Zwischenschichten 21 miteinander verknüpft, die jeweils mehrere Neuronen mit trainierbaren Gewichten umfassen können, wobei die Ausgaben einer Zwischenschicht 21 als Eingaben in die jeweils nachfolgende Zwischenschicht 21 eingegeben werden.
• Zusätzlich kann der Algorithmus 10 für jede der vorgenannten Bewegungsgrößen Unsicherheiten ausgeben, beispielsweise in Form entsprechender Varianzen $${\sigma }_{vx}^2,{\sigma }_{vy}^2,{\sigma }_{vz}^2,{\sigma }_{ax}^2,{\sigma }_{ay}^2,{\sigma }_{az}^2,{\sigma }_{\varphi }^2,{\sigma }_{\theta }^2,{\sigma }_{\psi }^2,{\sigma }_{\omega x}^2,{\sigma }_{\omega y}^2,{\sigma }_{\omega z}^2.$$

  
• Zudem läuft auf dem Steuergerät 7 ein Zustandsschätzer 23, beispielsweise ein Kalman-Filter, der die Bewegungsdaten 12 vom Algorithmus 10 in jedem Zeitschritt empfängt und daraus Schätzdaten 25 erzeugt, die einen geschätzten Bewegungszustand des Fahrzeugs 1 zu einem dem jeweiligen Zeitpunkt nachfolgenden zukünftigen Zeitpunkt beschreiben. Die Schätzdaten 25 können beispielsweise einem ABS-Modul 27, einem ESP-Modul 28 und/oder einem Navigationsmodul 29 zum satellitengestützten Navigieren des Fahrzeugs 1 basierend auf Sensordaten 31 eines entsprechenden Ortungssensors 33 zur Verfügung gestellt werden.
• In diesem Beispiel werden in den Zustandsschätzer 23 zusätzlich Sensordaten 35 eines Inertialsensors 37 des Fahrzeugs 1 eingegeben. Die Sensordaten 35 und die Kameradaten 12 können dabei in geeigneter Weise zeitlich miteinander korreliert sein.
• Im Folgenden wird die Berechnung der Schätzdaten 25 näher erläutert.
• Zunächst berechnet der Algorithmus 10 aus den Kameradaten 8 erste Modellwerte z = [v_x,k v_y,k v_z,k ϕ_x θ_x ψ_k]^T , umfassend erste Geschwindigkeitswerte [v_x,k v_y,k v_z,k]^T und erste Roll-, Nick- und Gierwinkelwerte $${\left[{\varphi }_k{\theta }_k{\psi }_k\right]}^T.$$

  
• Außerdem berechnet der Algorithmus 10 aus den Kameradaten 8 die ersten weiteren Modellwerte u = [a_x,k a_y,k a_z,k ω_x,k ω_y,k ω_z,k] ^T , umfassend Beschleunigungswerte [a_x,k a_y,k a_z,k]^T und Winkeländerungswerte $${\left[{\omega }_{x,k}{\omega }_{y,k}{\omega }_{z,k}\right]}^T.$$

  
• Mithilfe einer Strapdown- oder Kalman-Filter-Rechnung können aus den ersten weiteren Modellwerten u und den zweiten Modellwerten x̂ = [v_x,k v_y,k v_z,k ϕ̂_k θ̂̂_k ψ̂_k] ^T , umfassend zweite Roll-, Nick- und Gierwinkelwerte [ϕ̂_k θ̂̂_k ψ̂_k]^T , die zweiten Modellwerte x̂_k+1 = [v̂_x,k+1 v̂_y,k+1 v̂_z,k+1 ϕ̂_k+1 θ̂_k+1 ψ̂_k+1]^T des nachfolgenden Zeitschritts k + 1, umfassend zweite Geschwindigkeitswerte [v̂_x,k+1 v̂_y,k+1 v̂_z,k+1]^T sowie zweite Roll-, Nick- und Gierwinkelwerte [ϕ̂_k+1 θ̂_k+1 ψ̂_k+1]^T , durch diskrete Integration über die Zeit dt berechnet werden: $$\left[\begin{array}{c}{\widehat{v}}_{x,k+1}\\ {\widehat{v}}_{y,k+1}\\ {\widehat{v}}_{z,k+1}\end{array}\right]$$

  

  $$\begin{array}{l}\left[\begin{array}{c}{\widehat{v}}_{x,k}\\ {\widehat{v}}_{y,k}\\ {\widehat{v}}_{z,k}\end{array}\right]+dt\left(\left[\begin{array}{c}{a}_{x,k}\\ a_{y,k}\\ a_{z,k}\end{array}\right]-\underset{\text{Zentrifugalbeschleunigung}}{\underbrace{\left[\begin{array}{c}{\omega }_{x,k}\\ {\omega }_{y,k}\\ {\omega }_{z,k}\end{array}\right]\times \left[\begin{array}{c}{\widehat{v}}_{x,k}\\ {\widehat{v}}_{y,k}\\ {\widehat{v}}_{z,k}\end{array}\right]}}-g\underset{\text{Erdbeschleunigung}}{\underbrace{\left[\begin{array}{c}-\text{sin}\left({\widehat{\theta }}_k\right)\\ \text{cos}\left({\widehat{\theta }}_k\right)\cdot \text{sin}\left({\widehat{\varphi }}_k\right)\\ \text{cos}\left({\widehat{\theta }}_k\right)\cdot \text{cos}\left({\widehat{\theta }}_k\right)\end{array}\right]}}\right)\\ \left[\begin{array}{c}{\widehat{\varphi }}_{k+1}\\ {\widehat{\theta }}_{k+1}\\ {\widehat{\psi }}_{k+1}\end{array}\right]=\left[\begin{array}{c}{\widehat{\varphi }}_k\\ {\widehat{\theta }}_k\\ {\widehat{\psi }}_k\end{array}\right]+\text{d}t\left[\begin{array}{ccc}1& \text{sin}\left({\widehat{\varphi }}_k\right)\cdot \text{tan}\left({\widehat{\theta }}_k\right)& \text{cos}\left({\widehat{\varphi }}_k\right)\cdot \text{tan}\left({\widehat{\theta }}_k\right)\\ 0& \text{cos}\left({\widehat{\varphi }}_k\right)& -\text{sin}\left({\widehat{\varphi }}_k\right)\\ 0& \frac{\text{sin}\left({\widehat{\varphi }}_k\right)}{\text{cos}\left({\widehat{\theta }}_k\right)}& \frac{\text{cos}\left({\widehat{\varphi }}_k\right)}{\text{cos}\left({\widehat{\theta }}_k\right)}\end{array}\right]\cdot \left[\begin{array}{c}{\omega }_{x,k}\\ {\omega }_{y,k}\\ {\omega }_{z,k}\end{array}\right]\end{array}$$

  
• Die ersten Modellwerte können für die Korrektur der zweiten Modellwerte herangezogen werden. Die Korrektur kann mithilfe eines Kalman-Filters erfolgen.
• Für das Training des Algorithmus 10 können Daten eines Inertialsensors 37, etwa Winkelgeschwindigkeiten, Beschleunigungen und gegebenenfalls ein Neigungswinkel, und zusätzlich Daten eines Satellitennavigationssystems 33, etwa Geschwindigkeiten und gegebenenfalls ein Neigungswinkel, als Referenz verwendet werden. Der Algorithmus 10 kann dann die ersten Modellwerte und die ersten weiteren Modellwerte ausgeben. Darüber hinaus ist es besonders von Vorteil, wenn der Algorithmus 10, wie in 3 gezeigt, zusätzlich zu jedem ersten Modellwert und jedem ersten weiteren Modellwert einen zugehörigen ersten Konfidenzwert bzw. einen zugehörigen ersten weiteren Konfidenzwert wie etwa eine Varianz σ² ausgibt. Jeder Ausgabe µ ∈ {u,z} = {v_x, v_y, ...ω_z} ist dann eine zusätzliche Varianzausgabe ${\sigma }^2\in \left\{{\sigma }_{vx}^2,{\sigma }_{vy}^2\mathrm{,...}{\sigma }_{\omega z}^2\right\}$

  

  zugeordnet.
• Der Algorithmus 10 kann mithilfe der jeweiligen Ausgaben µ_k ∈ {v_x,k, v_y,k, ... ω_z,k} und ${\sigma }_k^2\in \left\{{\sigma }_{vx,k}^2,{\sigma }_{vy,k}^2\mathrm{,...}{\sigma }_{\omega z,k}^2\right\}$

  

  sowie der zugehörigen Referenzwerte µ_ref,k ∈ {v_x,ref,k, v_y,ref,k, ... ω_z,ref,k} und ${\sigma }_{ref,k}^2\in \left\{{\sigma }_{vx,ref,k}^2,{\sigma }_{vy,ref,k}^2\mathrm{,...}{\sigma }_{\omega z,ref,k}^2\right\}$

  

  so trainiert werden, dass die zweifach negative Log-Likelihood-Funktion $$-2\text{ln}\left(L\right)={\displaystyle \sum _k^n\left(\text{ln}\left(2\pi \left({\sigma }_k^2+{\sigma }_{ref,k}^2\right)\right)+\frac{{\left({\mu }_k-{\mu }_{\text{ref}\text{,}k}\right)}^2}{{\sigma }_k^2+{\sigma }_{ref,k}^2}\right)}$$

  

  über insgesamt n Berechnungsschritte k hinweg minimiert wird.
• Im künstlichen neuronalen Netz des Algorithmus 10 können Schichten mit rekurrenten Berechnungen wie etwa GRU (gated recurrent unit) oder LSTM, Faltungsschichten und insbesondere Schichten mit stochastischen Neuronen zum Einsatz kommen.
• Die Varianzausgaben des Algorithmus 10 können verwendet werden, um die Korrektur der zweiten Modellwerte mithilfe der ersten Modellwerte besonders gut auszuführen. Neben den zweiten Modellwerten können auch zugehörige zweite Konfidenzwerte ${\widehat{\sigma }}_{k+1}^2\in \left\{{\widehat{\sigma }}_{vx,k+1}^2,{\widehat{\sigma }}_{vy,k+1}^2\mathrm{,...}{\sigma }_{\widehat{\psi },k+1}^2\right\}$

  

  berechnet werden.
• Für die Berechnung der zugehörigen zweiten Konfidenzwerte ${\widehat{\sigma }}_{k+1}^2$

  

  kann das Modell $$\dot{\widehat{x}}=ƒ\left(\widehat{x},u\right)=\left[\begin{array}{c}{a}_x-{\omega }_y{\widehat{v}}_z+{\omega }_z{\widehat{v}}_y+g\text{ sin}\left(\widehat{\theta }\right)\\ a_y+{\omega }_x{\widehat{v}}_z-{\omega }_z{\widehat{v}}_x-g\text{ cos}\left(\widehat{\theta }\right)\text{sin}\left(\widehat{\varphi }\right)\\ a_z-{\omega }_x{\widehat{v}}_y+{\omega }_y{\widehat{v}}_x-g\text{ cos}\left(\widehat{\varphi }\right)\text{tan}\left(\widehat{\theta }\right)\\ {\omega }_y\text{cos}\left(\widehat{\varphi }\right)-{\omega }_z\text{sin}\left(\widehat{\varphi }\right)\\ \frac{{\omega }_y\text{sin}\left(\widehat{\varphi }\right)}{\text{cos}\left(\widehat{\theta }\right)}+\frac{{\omega }_z\text{cos}\left(\widehat{\varphi }\right)}{\text{cos}\left(\widehat{\theta }\right)}\end{array}\right]$$

  

  mit den zweiten Modellwerten x̂ = [v̂_x v̂_y v̂_z ϕ̂_k θ̂̂_k ψ̂_k] ^Tund den ersten weiteren Modellwerten u = [a_x a_y a_z ω_x ω_y ω_z]^T linearisiert werden zu $$\dot{\widehat{x}}=ƒ\left(\widehat{x},u\right)=\frac{\partial ƒ\left(\widehat{x},u\right)}{\partial \widehat{x}}\widehat{x}+\frac{\partial ƒ\left(\widehat{x},u\right)}{\partial u}u$$

  

  mit $$\begin{array}{l}\frac{\partial ƒ\left(\widehat{x},u\right)}{\partial \widehat{x}}\\ =[\begin{array}{cccccc}0& {\omega }_z& -{\omega }_y& 0& gcos\left(\widehat{\theta }\right)& 0\\ -{\omega }_z& 0& {\omega }_x& -g\text{ cos}\left(\widehat{\theta }\right)\text{cos}\left(\widehat{\varphi }\right)& g\text{ sin}\left(\widehat{\theta }\right)\text{sin}\left(\widehat{\varphi }\right)& 0\\ {\omega }_y& -{\omega }_x& 0& g\text{ cos}\left(\widehat{\theta }\right)\text{sin}\left(\widehat{\varphi }\right)& g\text{ sin}\left(\widehat{\theta }\right)\text{cos}\left(\widehat{\varphi }\right)& 0\\ 0& 0& 0& \left({\omega }_y\text{cos}\left(\widehat{\varphi }\right)-{\omega }_z\text{sin}\left(\widehat{\varphi }\right)\right)\text{tan}\left(\widehat{\theta }\right)& \left({\omega }_y\text{sin}\left(\widehat{\varphi }\right)+{\omega }_z\text{cos}\left(\widehat{\varphi }\right)\right)\left(1+{\text{tan}}^2\left(\widehat{\theta }\right)\right)& 0\\ 0& 0& 0& -{\omega }_y\text{sin}\left(\widehat{\varphi }\right)-{\omega }_z\text{cos}\left(\widehat{\varphi }\right)& 0& 0\\ 0& 0& 0& \frac{{\omega }_y\text{cos}\left(\widehat{\varphi }\right)}{\text{cos}\left(\widehat{\theta }\right)}-\frac{{\omega }_z\text{sin}\left(\widehat{\varphi }\right)}{\text{cos}\left(\widehat{\theta }\right)}& \frac{{\omega }_z\text{cos}\left(\widehat{\varphi }\right)\text{sin}\left(\widehat{\theta }\right)}{\text{cos}{\left(\widehat{\theta }\right)}^2}+\frac{{\omega }_y\text{sin}\left(\widehat{\varphi }\right)\text{sin}\left(\widehat{\theta }\right)}{\text{cos}{\left(\widehat{\theta }\right)}^2}& 0\end{array}\end{array}$$

  

  und $$\frac{\partial ƒ\left(\widehat{x},u\right)}{\partial u}=\left[\begin{array}{cccccc}1& 0& 0& 0& -v_z& v_y\\ 0& 1& 0& v_z& 0& -v_x\\ 0& 0& 1& -v_y& v_x& 0\\ 0& 0& 0& 1& \text{sin}\left(\widehat{\varphi }\right)\text{tan}\left(\widehat{\theta }\right)& \text{cos}\left(\widehat{\varphi }\right)\text{tan}\left(\widehat{\theta }\right)\\ 0& 0& 0& 0& \text{cos}\left(\widehat{\varphi }\right)& -\text{sin}\left(\widehat{\varphi }\right)\\ 0& 0& 0& 0& \frac{\text{sin}\left(\widehat{\varphi }\right)}{\text{cos}\left(\widehat{\theta }\right)}& \frac{\text{cos}\left(\widehat{\varphi }\right)}{\text{cos}\left(\widehat{\theta }\right)}\end{array}\right]$$

  
• Das linearisierte Strapdown-Modell kann zur Prädiktion der Kalman-Filter-Zustände bzw. der zweiten Modellwerte $${\widehat{x}}_{k+1}={\widehat{x}}_k+\text{d}t\dot{\widehat{x}}=\underset{A}{\underbrace{\left(I+\text{d}t\frac{\partial ƒ\left(\widehat{x},u\right)}{\partial \widehat{x}}\right)}}{\widehat{x}}_k+\underset{B}{\underbrace{\text{d}t\frac{\partial ƒ\left(\widehat{x},u\right)}{\partial u}}}u$$

  

  und zur Prädiktion der Kalman-Filter-Kovarianzmatrix $${\widehat{P}}_{k+1}=A{\widehat{P}}_{k}A+BQB$$

  

  mit $$Q=\left[\begin{array}{cccccc}{\sigma }_{ax}^2& 0& 0& 0& 0& 0\\ 0& {\sigma }_{ay}^2& 0& 0& 0& 0\\ 0& 0& {\sigma }_{az}^2& 0& 0& 0\\ 0& 0& 0& {\sigma }_{\omega x}^2& 0& 0\\ 0& 0& 0& 0& {\sigma }_{\omega y}^2& 0\\ 0& 0& 0& 0& 0& {\sigma }_{\omega z}^2\end{array}\right]$$

  

  und $$\widehat{P}=\left[\begin{array}{cccccc}{\widehat{\sigma }}_{vx}^2& {\widehat{\sigma }}_{vx}{\widehat{\sigma }}_{vy}& {\widehat{\sigma }}_{vx}{\widehat{\sigma }}_{vz}& {\widehat{\sigma }}_{vx}{\widehat{\sigma }}_{\varphi }& {\widehat{\sigma }}_{vx}{\widehat{\sigma }}_{\theta }& {\widehat{\sigma }}_{vx}{\widehat{\sigma }}_{\psi }\\ {\widehat{\sigma }}_{vx}{\widehat{\sigma }}_{vy}& {\widehat{\sigma }}_{vy}^2& {\widehat{\sigma }}_{vy}{\widehat{\sigma }}_{vz}& {\widehat{\sigma }}_{vy}{\widehat{\sigma }}_{\varphi }& {\widehat{\sigma }}_{vy}{\widehat{\sigma }}_{\theta }& {\widehat{\sigma }}_{vy}{\widehat{\sigma }}_{\psi }\\ {\widehat{\sigma }}_{vx}{\widehat{\sigma }}_{vz}& {\widehat{\sigma }}_{vy}{\widehat{\sigma }}_{vz}& {\widehat{\sigma }}_{vz}^2& {\widehat{\sigma }}_{vz}{\widehat{\sigma }}_{\varphi }& {\widehat{\sigma }}_{vz}{\widehat{\sigma }}_{\theta }& {\widehat{\sigma }}_{vz}{\widehat{\sigma }}_{\psi }\\ {\widehat{\sigma }}_{vx}{\widehat{\sigma }}_{\varphi }& {\widehat{\sigma }}_{vy}{\widehat{\sigma }}_{\varphi }& {\widehat{\sigma }}_{vz}{\widehat{\sigma }}_{\varphi }& {\widehat{\sigma }}_{\varphi }^2& {\widehat{\sigma }}_{\varphi }{\widehat{\sigma }}_{\theta }& {\widehat{\sigma }}_{\varphi }{\widehat{\sigma }}_{\psi }\\ {\widehat{\sigma }}_{vx}{\widehat{\sigma }}_{\theta }& {\widehat{\sigma }}_{vy}{\widehat{\sigma }}_{\theta }& {\widehat{\sigma }}_{vz}{\widehat{\sigma }}_{\theta }& {\widehat{\sigma }}_{\varphi }{\widehat{\sigma }}_{\theta }& {\widehat{\sigma }}_{\theta }^2& {\widehat{\sigma }}_{\theta }{\widehat{\sigma }}_{\psi }\\ {\widehat{\sigma }}_{vx}{\widehat{\sigma }}_{\psi }& {\widehat{\sigma }}_{vy}{\widehat{\sigma }}_{\psi }& {\widehat{\sigma }}_{vz}{\widehat{\sigma }}_{\psi }& {\widehat{\sigma }}_{\varphi }{\widehat{\sigma }}_{\psi }& {\widehat{\sigma }}_{\theta }{\widehat{\sigma }}_{\psi }& {\sigma }_{\psi }^2\end{array}\right]$$

  

  herangezogen werden, wobei zusätzlich auch die resultierenden Kovarianzen σ̂_vxσ̂_vy ... σ_̂vxσ̂ψ_̂ ... σ̂_θσ̂_ψ auf den Nebendiagonalen der Kovarianzmatrix P̂̂ prädiziert werden können.
• Die Korrektur der zweiten Modellwerte x̂ = [v̂_x,k v_y,k v̂_z,k ϕ̂_k θ̂_k ψ̂_k] ^T mithilfe der ersten Modellwerte z = [v_x,k v_y,k v_z,k ϕ_k θ̂̂_k ψ_k] ^T kann beim Kalman-Filter mithilfe des Kalman-Gain $$K=\widehat{P}{H}^T{\left(H\widehat{P}{H}^T+R\right)}^{-1}$$

  

  mit H^T = I und $$R=\left[\begin{array}{cccccc}{\sigma }_{vx}^2& 0& 0& 0& 0& 0\\ 0& {\sigma }_{vy}^2& 0& 0& 0& 0\\ 0& 0& {\sigma }_{vz}^2& 0& 0& 0\\ 0& 0& 0& {\sigma }_{\varphi }^2& 0& 0\\ 0& 0& 0& 0& {\sigma }_{\theta }^2& 0\\ 0& 0& 0& 0& 0& {\sigma }_{\psi }^2\end{array}\right]$$

  

  wie folgt vorgenommen werden: $$\widehat{x}\text{'}=\widehat{x}+K\left(z-H\widehat{x}\right)$$

  
• Die Kovarianzmatrix P̂ kann ebenso wie folgt korrigiert werden: $$\widehat{P}\text{'}=\left(I-KH\right)\widehat{P}$$

  
• Abschließend wird darauf hingewiesen, dass Begriffe wie „aufweisend“, „umfassend“ usw. keine anderen Elemente oder Schritte ausschließen und unbestimmte Artikel wie „ein“ oder „eine“ keine Vielzahl ausschließen. Bezugszeichen in den Ansprüchen sind nicht als Einschränkung anzusehen.

Claims (12)

1. Verfahren zum Schätzen eines Bewegungszustands eines Fahrzeugs (1), wobei das Fahrzeug (1) mit einer Kamera (5) zum Erfassen einer Umgebung des Fahrzeugs (1) ausgestattet ist, wobei das Verfahren umfasst: Empfangen von Kameradaten (8), die von der Kamera (5) erzeugt wurden; Eingeben der Kameradaten (8) in einen Algorithmus (10), der mit historischen Kameradaten und den historischen Kameradaten zugeordneten Referenzbewegungsdaten trainiert wurde, um die Kameradaten (8) in Bewegungsdaten (12) umzuwandeln, die einen aktuellen Bewegungszustand des Fahrzeugs (1) beschreiben; und Bestimmen eines geschätzten Bewegungszustands des Fahrzeugs (1) aus den Bewegungsdaten (12) durch einen Zustandsschätzer (23).
2. Verfahren nach Anspruch 1, wobei die Bewegungsdaten (12) eine aktuelle Geschwindigkeit, eine aktuelle Beschleunigung, einen aktuellen Drehwinkel und/oder eine aktuelle Drehrate des Fahrzeugs (1) anzeigen; und/oder wobei der Zustandsschätzer (23) eine geschätzte Geschwindigkeit und/oder einen geschätzten Drehwinkel des Fahrzeugs (1) bestimmt.
3. Verfahren nach einem der vorhergehenden Ansprüche, wobei die Bewegungsdaten (12) zusätzlich Varianzen bezüglich des aktuellen Bewegungszustands anzeigen; und/oder wobei der Zustandsschätzer (23) zusätzlich Varianzen bezüglich des geschätzten Bewegungszustands bestimmt.
4. Verfahren nach einem der vorhergehenden Ansprüche, wobei der Zustandsschätzer (23) ein Kalman-Filter ist.
5. Verfahren nach einem der vorhergehenden Ansprüche, wobei der Algorithmus (10) ein künstliches neuronales Netz ist.
6. Verfahren nach einem der vorhergehenden Ansprüche, wobei zum Trainieren des Algorithmus (10) eine Kostenfunktion, die eine Abweichung zwischen den Bewegungsdaten (12) und den Referenzbewegungsdaten quantifiziert, minimiert wurde; wobei die Kostenfunktion eine Likelihood-Funktion, insbesondere eine Log-Likelihood-Funktion, ist.
7. Verfahren nach einem der vorhergehenden Ansprüche, wobei die Referenzbewegungsdaten von einem Inertialsensor (37) und/oder einem satellitengestützten Ortungssensor (33) erzeugt wurden.
8. Verfahren nach einem der vorhergehenden Ansprüche, wobei zusätzlich Sensordaten (35), die von einer zusätzlichen Sensorik (37) des Fahrzeugs (1) erzeugt wurden, empfangen und in den Zustandsschätzer (23) eingegeben werden; wobei der Zustandsschätzer (23) den geschätzten Bewegungszustand zusätzlich aus den Sensordaten (35) bestimmt.
9. Verfahren nach Anspruch 8, wobei die zusätzliche Sensorik mindestens einen Inertialsensor (37), einen satellitengestützten Ortungssensor (33) und/oder einen Raddrehzahlsensor umfasst.
10. Steuergerät (7), umfassend einen Prozessor, der konfiguriert ist, um das Verfahren nach einem der vorhergehenden Ansprüche auszuführen.
11. Computerprogramm, umfassend Befehle, die einen Prozessor bei Ausführung des Computerprogramms durch den Prozessor veranlassen, das Verfahren nach einem der Ansprüche 1 bis 9 auszuführen.
12. Computerlesbares Medium, auf dem das Computerprogramm nach Anspruch 11 gespeichert ist.

Images