Ermittlung der von einem Fahrzeug gefahrenen Trajektorie, insbesondere um eine hochgenaue digitale (HD) Karte für ein Straßennetz zu generieren.

Ein Fahrzeug kann für ein sicheres, komfortables und effizientes automatisiertes Fahren hochgenaue digitale Karten verwenden, um die Fahrzeug-Sensorik zu erweitern und/oder zu ergänzen. Durch eine HD-Karte wird a-priori Wissen über das Fahrzeugumfeld bereitgestellt, wodurch eine verbesserte Lokalisierung des Fahrzeugs und/oder die Detektion von Ausreißern der Fahrzeug-Sensorik ermöglicht werden. Außerdem können HD-Karten als Rückfallebene im Falle eines Sensor-Versagens dienen und den Komfort einer automatisierten

Fahrfunktion durch weitere Voraussicht verbessern. Des Weiteren können HD- Karten Informationen über Befahrbarkeit und Konnektivität von Fahrspuren sowie über gültige Verkehrsregeln umfassen, durch die das automatisierte Fahren verbessert wird.

Zur Erstellung einer HD-Karte können dedizierte Kartierungsfahrzeuge verwendet werden, die mit einer Reihe von hochgenauen Sensoren wie LID AR (engl. Light detection and ranging) und/oder hochgenauem GPS Empfängern ausgestattet sind. Die Verwendung von dedizierten Kartierungsfahrzeugen ist jedoch mit relativ hohen Kosten verbunden. Des Weiteren ist die Anzahl von Kartierungsfahrzeugen typischerweise begrenzt, so dass nur ein relativ kleiner Anteil des Straßennetzes regelmäßig befahren werden kann und/oder so dass die Befahrungsfrequenz für ein einzelnes Straßensegment relativ gering ist. Als Folge daraus können Änderungen im Straßennetz möglicherweise nicht immer zeitnah erkannt und in einer HD-Karte berücksichtigt werden. Durch die Verwendung von nicht ausreichend aktuellen HD-Karten kann wiederum die Güte einer automatisierten Fahrfunktion eines Fahrzeugs beeinträchtigt werden.

Um den Aufwand der Überprüfung und/oder der Anpassung einer HD-Karte zu reduzieren und/oder um die Frequenz der Überprüfung und/oder der Anpassung einer HD-Karte zu erhöhen, können Fahrdaten von (herkömmlichen) Fahrzeugen erfasst und ausgewertet werden. Die Fahrdaten eines Fahrzeugs können dabei Sensordaten von unterschiedlichen Sensoren des Fahrzeugs umfassen. Auf Basis der Fahrdaten der einzelnen Fahrzeuge können dann Trajektorien der einzelnen Fahrzeuge ermittelt werden, aus denen wiederum auf den Verlauf von Fahrbahnen geschlossen werden kann. Die Fahrdaten von Fahrzeugen können somit dazu genutzt werden, eine HD-Karte anzupassen.

Die Genauigkeit und/oder die Auflösung der Sensordaten der ein oder mehreren Sensoren eines Fahrzeugs ist typischerweise geringer als die Genauigkeit und/oder die Auflösung der Sensordaten der Sensoren eines Kartierungsfahrzeugs. Das vorliegende Dokument befasst sich mit der technischen Aufgabe, auch bei Verwendung von Sensordaten mit einer relativ geringen Genauigkeit, die Trajektorie eines Fahrzeugs mit möglichst hoher Genauigkeit ermitteln zu können, insbesondere um die ermittelte Trajektorie für die Erstellung und/oder die Anpassung einer digitalen Karte verwenden zu können.

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 daraufhingewiesen, 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 Schätzung einer tatsächlichen (Fahr-)Trajektorie eines Fahrzeugs beschrieben. Die Trajektorie kann dabei auf Basis von Sensordaten von ein oder mehreren Sensoren des Fahrzeugs geschätzt werden. Die Vorrichtung kann zumindest teilweise (oder vollständig) innerhalb des Fahrzeugs und/oder zumindest teilweise (oder vollständig) außerhalb des Fahrzeugs (z.B. in einem Backend-Server) angeordnet sein. Die Vorrichtung kann eingerichtet sein, auf Basis der Schätzung der tatsächlichen Trajektorie des Fahrzeugs eine digitale Karte in Bezug auf ein Straßennetz zu erstellen und/oder anzupassen. Die digitale Karte kann dann zur Bereitstellung einer automatisierten Fahrfunktion (insbesondere für hochautomatisiertes Fahren) verwendet werden. Die digitale Karte kann den Verlauf von Straßen und/oder Fahrbahnen und/oder Fahrspuren des Straßennetzes relativ zu einem Referenzkoordinatensystem (z.B. relativ zu dem Weltkoordinatensystem) beschreiben. Die Vorrichtung kann eingerichtet sein, auf Basis der Sensordaten eines Positionssensors (des Fahrzeugs) eine Sequenz von gemessenen Positionswerten in Bezug auf die jeweilige Position des Fahrzeugs an einer entsprechenden Sequenz von Zeitpunkten zu ermitteln. Ein gemessener Positionwert kann dabei Koordinaten, insbesondere GPS Koordinaten, in Bezug auf die Position des Fahrzeugs innerhalb des Referenzkoordinatensystems (z.B. innerhalb des Weltkoordinatensystems) für das Straßennetz umfassen. Die Sequenz von gemessenen Positionswerten kann somit eine gemessene Trajektorie des Fahrzeugs innerhalb des Referenzkoordinatensystems beschreiben. Aufgrund von Messfehlern und/oder aufgrund von Messungenauigkeiten kann es dabei zu stochastischen und/oder systematischen Abweichungen der einzelnen gemessenen Positionswerte von der jeweiligen tatsächlichen Position des Fahrzeugs kommen.

Die Vorrichtung ist ferner eingerichtet, auf Basis von Sensordaten von ein oder mehreren Fahrzeugsensoren (des Fahrzeugs), eine Sequenz von Odometriewerten für zumindest einen Teil der Sequenz von Zeitpunkten zu ermitteln. Dabei kann ein Odometriewert für einen Zeitpunkt anzeigen, wie sich die Pose (d.h. die Position und/oder die Orientierung des Fahrzeugs) im Vergleich zu einem (direkt) vorhergehenden Zeitpunkt verändert hat. Beispielhafte Fahrzeugsensoren zeigen die Fahrgeschwindigkeit, den Lenkwinkel, die Beschleunigung und/oder die Gierrate des Fahrzeugs an. Ein Posewert kann die Pose des Fahrzeugs anzeigen.

Zu diesem Zweck kann ein Posewerte Koordinaten in Bezug auf die Position des Fahrzeugs innerhalb eines Koordinatensystems des Fahrzeugs umfassen. Ein Odometriewert kann die Änderung der Position (und/oder der Orientierung) des Fahrzeugs zwischen zwei Zeitpunkten relativ zu dem Koordinatensystem des Fahrzeugs anzeigen. Dabei kann das Koordinatensystem des Fahrzeugs gegenüber dem Referenzkoordinatensystem verschoben sein. Die Odometriewerte können somit anzeigen, wie sich die Position (und ggf. die Orientierung) des Fahrzeugs an der Sequenz von Zeitpunkten (relativ zu dem Koordinatensystem des Fahrzeugs) zwischen zwei Zeitpunkten jeweils verändert hat. Folglich kann auf Basis der Odometriewerte eine Sequenz von Posewerten ermittelt werden, die eine Trajektorie anzeigt, die gegenüber der tatsächlichen Trajektorie des Fahrzeugs verschoben (und ggf. rotiert) ist. Des Weiteren können die Odometriewerte (aufgrund von Messungenauigkeiten der ein oder mehreren Fahrzeugsensoren) Fehler aufweisen, so dass sich die allein auf Basis der Odometriewerte ermittelten Posewerte (zusätzlich zu einer Translation und/oder Rotation) typischerweise von den tatsächlichen Posewerten des Fahrzeugs unterscheiden.

Die Vorrichtung kann eingerichtet sein, die Sequenz von gemessenen Positionswerten und die Sequenz von Odometriewerten zu kombinieren und/oder zu überlagern, um eine Schätzung der tatsächlichen Trajektorie zu ermitteln.

Dabei kann ein (systematischer) Positionsoffset berücksichtigt werden, um die gemessenen Positionswerte in einheitlicher Weise zu verschieben, und um damit einen systematischen Fehler bei der Messung der gemessenen Positionswerte (insbesondere der gemessenen GPS bzw. Satelliten Koordinaten) zu berücksichtigen und zu kompensieren.

Insbesondere kann die Vorrichtung eingerichtet sein, als Schätzung der tatsächlichen Trajektorie des Fahrzeugs eine Sequenz von geschätzten Posewerten für die Sequenz von Zeitpunkten zu ermitteln. Ein geschätzter Posewert für einen bestimmten Zeitpunkt kann dabei die Pose des Fahrzeugs an dem bestimmten Zeitpunkt relativ zu dem Referenzkoordinatensystem anzeigen. Des Weiteren kann der (einheitliche) Wert des Positionsoffsets für die Sequenz von gemessenen Positionswerten ermittelt werden. Die Sequenz von geschätzten Posewerten und der Wert des Positionsoffsets können dabei derart ermittelt werden, dass ein Optimierungskriterium verbessert, insbesondere zumindest lokal optimiert, wird.

Das Optimierungskriterium kann einen Positionswert- Abweichungsterm für jeden der Sequenz von Zeitpunkten umfassen. Der Positionswert-Abweichungsterm für einen Zeitpunkt kann von der (z.B. quadratischen) Abweichung des geschätzten Posewertes für den jeweiligen Zeitpunkt von dem um den (einheitlichen und/oder systematischen) Positionsoffset verschobenen gemessenen Positionswert für den jeweiligen Zeitpunkt abhängen. So kann die Sequenz von geschätzten Posewerten (und damit die Schätzung der Trajektorie) in besonders präziser Weise ermittelt werden.

Alternativ oder ergänzend kann das Optimierungskriterium einen Odometrie- Abweichungsterm für zumindest einen Teil der Zeitpunkte der Sequenz von Zeitpunkten umfassen (insbesondere für N-l Zeitpunkte, für den Fall, dass die Sequenz von Zeitpunkten N Zeitpunkte umfasst). Dabei kann der Odometrie- Abweichungsterm für einen Zeitpunkt von der (ggf. quadratischen) Abweichung der sich aus der Sequenz von geschätzten Posewerten ergebenden Änderung der Pose des Fahrzeugs an dem Zeitpunkt von dem Odometriewert für den Zeitpunkt abhängen.

Das Optimierungskriterium kann somit darauf ausgelegt sein, die Sequenz von geschätzten Posewerten derart zu ermitteln, dass die (mittlere) Abweichung der geschätzten Posewerte zu den gemessenen Odometriewerten und zu den (um den Positionsoffset verschobenen) gemessenen Positionswerten reduziert, insbesondere minimiert, wird. So kann die tatsächliche Trajektorie des Fahrzeugs in besonders präziser Weise geschätzt werden.

Die Vorrichtung kann eingerichtet sein, die Schätzung der tatsächlichen Trajektorie des Fahrzeugs mittels einer Graph-SLAM, d.h. Simultaneous Localization and Mapping, Methode zu ermitteln. Dabei kann der Positionsoffset in einem bevorzugten Beispiel als verschiebbarer Anker-Knoten im Rahmen der Graph-SLAM Methode berücksichtigt werden. Dies ermöglicht eine besonders effiziente und präzise Schätzung der tatsächlichen Trajektorie. Wie bereits oben dargelegt, weisen die gemessenen Positionswerte typischerweise eine bestimmte Positions-Messungenauigkeit auf. Als Folge daraus kann für jeden gemessenen Positionswert eine Wahrscheinlichkeitsverteilung von möglichen Werten um den jeweils gemessenen Positionswert vorliegen. Das Optimierungskriterium kann von der Positions-Messungenauigkeit (insbesondere von der Wahrscheinlichkeitsverteilung) der gemessenen Positionswerte abhängen. Die Abhängigkeit kann dabei derart sein, dass eine Erhöhung der Positions- Messungenauigkeit (bzw. eine Verbreiterung der Wahrscheinlichkeitsverteilung) zu einer Reduzierung der Auswirkung der Positionswert-Abweichungsterme auf das Optimierungskriterium führt. So kann die Genauigkeit der Schätzung der tatsächlichen Trajektorie weiter erhöht werden.

In entsprechender Weise weisen auch die Odometriewerte typischerweise eine Odometrie-Messungenauigkeit auf. Als Folge daraus kann für jeden gemessenen Odometriewert eine Wahrscheinlichkeitsverteilung von möglichen Werten um den jeweils gemessenen Odometriewert vorliegen. Das Optimierungskriterium kann von der Odometrie-Messungenauigkeit (insbesondere von der Wahrscheinlichkeitsverteilung) der Odometrie abhängen, insbesondere derart, dass eine Erhöhung der Odometrie-Messungenauigkeit (bzw. eine Verbreiterung der Wahrscheinlichkeitsverteilung) zu einer Reduzierung der Auswirkung der Odometrie-Abweichungsterme auf das Optimierungskriterium führt. So kann die Genauigkeit der Schätzung der tatsächlichen Trajektorie weiter erhöht werden.

Die Vorrichtung kann eingerichtet sein, den Positionsoffset für die Sequenz von gemessenen Positionswerten derart zu ermitteln, dass der Wert des Positionsoffsets für alle gemessenen Positionswerte der Sequenz von gemessenen Positionswerten und/oder für alle Zeitpunkte der Sequenz von Zeitpunkten gleich ist. Mit anderen Worten, es kann ein gemeinsamer (systematischer)

Positionsoffset für alle gemessenen Positionswerte der Trajektorie berücksichtigt werden. So kann in effizienter und präziser Weise ein über die gesamte Trajektorie wirkender systematischer Fehler bei der Messung der gemessenen Positionswerte (z.B. ein auf die Atmosphäre zurückzuführender systematischer Fehler) berücksichtigt und kompensiert werden.

Die Vorrichtung kann eingerichtet sein, den Wert des Positionsoffsets auf einen Maximalwert zu beschränken. Alternativ oder ergänzend kann die Vorrichtung eingerichtet sein, den Wert des Positionsoffsets unter Berücksichtigung einer Kovarianz und/oder einer Wahrscheinlichkeitsverteilung für den Positionsoffset zu ermitteln. Alternativ oder ergänzend kann das Optimierungskriterium einen Term umfassen, durch den eine betragliche Erhöhung des Wertes des Positionsoffsets bestraft wird. So kann in effizienter und zuverlässiger Weise eine präzise Lokalisierung der Schätzung der tatsächlichen Trajektorie innerhalb des Referenzkoordinatensystems (insbesondere innerhalb des Weltkoordinatensystems) bewirkt werden.

Die Vorrichtung kann eingerichtet sein, die Sequenz von Zeitpunkten in eine Mehrzahl von aufeinanderfolgenden Teilsequenzen zu unterteilen. Die gemessenen Positionswerte einer Teilsequenz können dann zur Ermittlung der Schätzung der tatsächlichen Trajektorie jeweils mit einem Teil-Positionsoffset für die Teilsequenz und mit dem Positionsoffset für die gesamte Sequenz verschoben werden. Insbesondere kann die Vorrichtung eingerichtet sein, die Sequenz von geschätzten Posewerten für die Sequenz von Zeitpunkten, den Positionsoffset für die gesamte Sequenz von gemessenen Positionswerten und die Mehrzahl von Teil-Positionsoffsets für die Mehrzahl von Teilsequenzen derart zu ermitteln, dass das Optimierungskriterium verbessert, insbesondere zumindest lokal optimiert, wird. Durch die Berücksichtigung von Teil-Positionsoffsets für unterschiedliche Teilbereiche der Trajektorie des Fahrzeugs können in effizienter Weise lokal wirkende systematische Fehler (wie z.B. lokale Multipath-Effekte) bei der Messung der gemessenen Positionswerte berücksichtigt und kompensiert werden. So kann die Güte der Schätzung der tatsächlichen Trajektorie weiter erhöht werden. Wie bereits oben dargelegt, kann die Vorrichtung eingerichtet sein, die Schätzung der tatsächlichen Trajektorie mittels einer Graph-SLAM Methode zu ermitteln. Zu diesem Zweck kann die Vorrichtung eingerichtet sein, einen Graphen zu erstellen, wobei der Graph die einzelnen geschätzten Posewerte der Sequenz von geschätzten Posewerten als Knoten umfasst (die in diesem Dokument als Posevariable-Knoten bezeichnet werden). Dabei können die geschätzten Posewerte auf Basis der gemessenen Positionswerte und/oder auf Basis der (gemessenen) Odometriewerte initialisiert werden. Der Graph mit den initialisierten, geschätzten Posewerten kann dann als Ausgangspunkt für (iterative) Optimierung des Graphen und damit für die Schätzung der tatsächlichen Trajektorie verwendet werden.

Der Graph kann als Kante zwischen zwei Posevariable-Knoten einen Odometrie- Faktor umfassen, der von den Sensordaten der ein oder mehreren Fahrzeugsensoren abhängig ist, und/oder der eine Bewegung des Fahrzeugs zwischen den zwei Knoten anzeigt. Insbesondere kann der Odometrie-Faktor zwischen dem Posevariable-Knoten für einen bestimmten Zeitpunkt und dem Posevariable-Knoten für einen vorhergehenden Zeitpunkt von dem gemessenen Odometriewert für den bestimmten Zeitpunkt abhängen. Dabei kann der Odometrie-Faktor eine Wahrscheinlichkeitsverteilung in Bezug auf die Änderung der Posewerte zwischen zwei Posevariable-Knoten anzeigen.

Der Graph kann ferner für jeden gemessenen Positionwert einen Positionsvariable-Knoten aufweisen. Des Weiteren kann der Graph für jeden der Posevariable-Knoten eine Assoziation über einen Positions-Faktor (d.h. über eine Kante) zu einem entsprechenden Positionsvariable-Knoten (bzw. zu einem entsprechenden gemessenen Positionswert) umfassen. Dabei kann über den Positions-Faktor angezeigt werden, wie der Positionsvariable-Knoten relativ zu dem j eweiligen Posevariable-Knoten angeordnet ist. Der Positions-Faktor kann dabei von dem gemessenen Positionswert für den jeweiligen Positionsvariable- Knoten abhängen. Insbesondere kann der Positionsvariable-Knoten für einen bestimmten Zeitpunkt an einer Position innerhalb des Graphen angeordnet sein, die dem gemessenen Positionswert für den bestimmten Zeitpunkt entspricht. Der Positions-Faktor (d.h. die Kante) zwischen einem Posevariable-Knoten und einem entsprechenden Positionsvariable-Konten kann mit einer Wahrscheinlichkeitsverteilung assoziiert sein, die von der Positions- Messungenauigkeit abhängt.

Auf Basis der gemessenen Positionswerte können somit Positionsvariable-Knoten und auf Basis der gemessenen Odometriewerte (initialisierte) Posevariable- Knoten eines Graphen bereitgestellt werden. Die Knoten können über Kanten miteinander verbunden sein, wobei die Kanten jeweils mit einer Wahrscheinlichkeitsverteilung bzw. einer Kovarianz assoziiert sein können, die von der Positions-Messungenauigkeit und/oder von der Odometrie- Messungenauigkeit abhängt. So können die verfügbaren Sensordaten in Bezug auf die Position des Fahrzeugs in effizienter Weise innerhalb eines Graphen repräsentiert werden.

Die Positionsvariable-Knoten des Graphen können derart (in steifer Weise) mit einem Positions-Anker (d.h. mit einem Anker-Knoten) verbunden sein, dass eine Verschiebung des Positions-Ankers zu einer entsprechenden Verschiebung der Positionsvariable-Knoten des Graphen führt. So kann der Positionsoffset in effizienter Weise innerhalb des Graphen berücksichtigt werden.

Die Vorrichtung kann eingerichtet sein, die Sequenz von geschätzten Posewerten (d.h. den Schätzwert der tatsächlichen Trajektorie) durch eine Optimierung des Graphen zu ermitteln. Die Optimierung des Graphen kann dabei ein Verschieben des Positions-Ankers und/oder ein Ändern von ein oder mehreren Posewerten der Sequenz von Posewerten und/oder ein Verschieben der Posevariable-Knoten umfassen. Die Optimierung des Graphen kann derart erfolgen, dass ein bestimmtes Optimierungskriterium verbessert, insbesondere zumindest lokal optimiert, wird. Das Optimierungskriterium kann, wie weiter oben beschrieben, Positions-Abweichungsterme und/oder Odometrie- Abweichungsterme umfassen. Durch die Beschreibung des Optimierungsproblems als Graph kann die Sequenz von geschätzten Posewerten in besonders effizienter und präziser Weise ermittelt werden.

Die Vorrichtung kann eingerichtet sein, im Rahmen der Optimierung des Graphen eine Pose des Positions-Ankers in Abhängigkeit von einer

Wahrscheinlichkeitsverteilung für die Pose des Positions-Ankers zu ändern. Dabei kann die Wahrscheinlichkeitsverteilung derart sein, dass eine Translation des Positions-Ankers ermöglicht wird und dass eine Rotation des Positions-Ankers im Wesentlichen unterbunden wird. So kann ein Positionsoffset und damit ein systematischer Fehler der Messung der gemessenen Positionswerte in besonders zuverlässiger Weise berücksichtigt und kompensiert werden.

Wie bereits oben dargelegt, kann die Sequenz von Zeitpunkten eine Mehrzahl von aufeinanderfolgenden Teilsequenzen umfassen. Die Positionsvariable-Knoten des Graphen einer Teil Sequenz können dann jeweils mit einem Zwischen- Anker verbunden sein, so dass eine Verschiebung des Zwischen- Ankers zu einer entsprechenden Verschiebung der Positionsvariable-Knoten der Teilsequenz führt. Des Weiteren können die Zwischen- Anker der Mehrzahl von Teil Sequenzen derart mit dem Positions-Anker verbunden sein, dass eine Verschiebung des Positions-Ankers zu einer entsprechenden Verschiebung der Zwischen- Anker der Mehrzahl von Teilsequenzen führt. Durch die Berücksichtigung von ein oder mehreren Zwischen-Ankem können auch lokal wirkende systematische Fehler bei der Messung der gemessenen Positionswerte in zuverlässiger und präziser Weise ermittelt und kompensiert werden.

Die Vorrichtung kann eingerichtet sein, auf Basis von Sensordaten von ein oder mehreren Umfeldsensoren (z.B. Kameras und/oder Lidarsensoren) des Fahrzeugs, für einen bestimmten Zeitpunkt der Sequenz von Zeitpunkten eine Messposition einer Landmarke in einem Umfeld des Fahrzeugs zu ermitteln. Es kann dann ein Landmarken-Abweichungsterm in dem Optimierungskriterium berücksichtigt werden, wobei der Landmarken-Abweichungsterm von der Abweichung der Messposition der Landmarke von einer Referenzposition der Landmarke abhängen kann. Durch die Berücksichtigung von ein oder mehreren Landmarken kann die Güte der Schätzung der tatsächlichen Trajektorie weiter erhöht werden.

Die Vorrichtung kann eingerichtet sein, für eine Mehrzahl von Fahrten an einem Segment eines Straßennetzes (und ggf. innerhalb eines begrenzten Zeitintervalls) jeweils eine Sequenz von gemessenen Positionswerten an einer entsprechenden Sequenz von Zeitpunkten zu ermitteln. Des Weiteren kann die Vorrichtung eingerichtet sein, für die Mehrzahl von Fahrten jeweils eine Sequenz von Odometriewerten für zumindest einen Teil der Sequenz von Zeitpunkten zu ermitteln. Außerdem kann die Vorrichtung eingerichtet sein, für die Mehrzahl von Fahrten jeweils eine Sequenz von geschätzten Posewerten für die Sequenz von Zeitpunkten und einen Positionsoffset zu ermitteln. Dabei können die Sequenzen von geschätzten Posewerten (d.h. die Schätzungen der tatsächlichen Trajektorien für die unterschiedlichen Fahrten) und der Positionsoffset derart ermittelt werden, dass jeweils ein Optimierungskriterium verbessert, insbesondere zumindest lokal optimiert wird. Des Weiteren können die Sequenzen von geschätzten Posewerten (d.h. die Schätzungen der tatsächlichen Trajektorien für die unterschiedlichen Fahrten) und der Positionsoffset derart ermittelt werden, dass der Positionsoffset für die Mehrzahl von Fahrten gleich ist (insbesondere dann, wenn die Mehrzahl von Fahrten innerhalb eines relativ kurzen Zeitintervalls erfolgt sind). So kann in effizienter Weise berücksichtigt werden, dass sich ein systematischer Fehler bei der Positionsmessung in gleicher Weise auf unterschiedliche Fahrten auswirkt. Dadurch kann die Güte der ermittelten Schätzungen der tatsächlichen Fahrten weiter erhöht werden.

Andererseits kann ggf. für die unterschiedlichen Fahrten zumindest teilweise ein spezifischer Positionsoffset ermittelt werden (insbesondere, wenn die unterschiedlichen Fahrten zeitlich relativ weit auseinander liegen). So können Änderungen bei Vorliegen von systematischen Messfehlern in zuverlässiger Weise erkannt und berücksichtigt werden.

Es sei daraufhingewiesen, dass die Vorgabe eines einheitlichen Positionsoffsets für mehrere Fahrten an einem Segment des Straßennetzes in entsprechender Weise auf Teil-Positionsoffsets (für Teilsegmente des Segments), auf einen Positions-Anker (für das Segment) und/oder auf die Zwischen-Anker (für Teilsegmente des Segments) angewendet werden kann. So können insbesondere auch lokale systematische Fehler bei der Positionsmessung in effizienter und präziser Weise berücksichtigt und kompensiert werden.

Die Vorrichtung kann eingerichtet sein, alternative Daten in Bezug auf die Position des Fahrzeugs zu ermitteln und auf Basis der alternativen Daten einen Wert zumindest eines weiteren Abweichungsterms für das Optimierungskriterium zu ermitteln. Dabei kann der zumindest eine weitere Abweichungsterm derart sein, dass durch den weiteren Abweichungsterm eine betragliche Erhöhung des Wertes des Positionsoffsets bewirkt wird. Der weitere Abweichungsterm kann z.B. (wie oben dargelegt) von einer Landmarke abhängen und/oder von den gemessenen Positionswerten, die bei einer weiteren Fahrt erfasst wurden, abhängen. Die alternativen Daten können somit z.B. die Messposition einer Landmarke und/oder gemessene Positionswerte einer weiteren Fahrt umfassen. Durch die Berücksichtigung von alternativen Daten in Bezug auf die Position des Fahrzeugs kann ein systematischer Fehler bei der Messung von Positionsdaten in besonders zuverlässiger und präziser Weise ermittelt und kompensiert werden.

Die Vorrichtung kann eingerichtet sein, die Sensordaten des Positionssensors des Fahrzeugs oder davon abgeleitete Daten von dem Fahrzeug zu empfangen. Des Weiteren kann die Vorrichtung eingerichtet sein, die Sensordaten der ein oder mehreren Fahrzeugsensoren des Fahrzeugs oder davon abgeleitete Daten von dem Fahrzeug zu empfangen. Die Daten können z.B. über eine (drahtlose) Kommunikationsverbindung empfangen werden. Die Sequenz von geschätzten Poseweiten für die Sequenz von Zeitpunkten kann dann von der (Fahrzeug extemen Vorrichtung) auf Basis der empfangenen Daten ermittelt werden.

Gemäß einem weiteren Aspekt wird ein (Straßen-)Kraftfahrzeug (insbesondere ein Personenkraftwagen oder ein Lastkraftwagen oder ein Bus oder ein Motorrad) und/oder eine Fahrzeug-externe Einheit (z.B. ein Server) beschrieben, das bzw. die die in diesem Dokument beschriebene Vorrichtung umfasst.

Gemäß einem weiteren Aspekt wird ein (Computer-implementiertes) Verfahren zur Schätzung einer tatsächlichen Trajektorie eines Fahrzeugs beschrieben. Das Verfahren umfasst das Ermitteln, auf Basis der Sensordaten eines Positionssensors, einer Sequenz von gemessenen Positionswerten in Bezug auf eine jeweilige Position des Fahrzeugs an einer entsprechenden Sequenz von Zeitpunkten. Die gemessenen Positionswerte können die Position des Fahrzeugs relativ zu einem Referenzkoordinatensystem anzeigen. Des Weiteren umfasst das Verfahren das Ermitteln, auf Basis von Sensordaten von ein oder mehreren Fahrzeugsensoren, einer Sequenz von Odometriewerten für zumindest einen Teil der Sequenz von Zeitpunkten. Dabei kann ein Odometriewert für einen Zeitpunkt anzeigen, wie sich die Pose des Fahrzeugs im Vergleich zu einem (direkt) vorhergehenden Zeitpunkt verändert hat.

Das Verfahren umfasst ferner das Ermitteln (als Schätzung der tatsächlichen Trajektorie des Fahrzeugs) einer Sequenz von geschätzten Posewerten für die Sequenz von Zeitpunkten und das Ermitteln eines (systematischen und/oder einheitlichen) Positionsoffsets für die Sequenz von gemessenen Positionswerten, so dass ein Optimierungskriterium verbessert, insbesondere zumindest lokal optimiert, wird. Dabei kann das Optimierungskriterium einen Positionswert- Abweichungsterm für jeden der Sequenz von Zeitpunkten umfassen. Der Positionswert-Abweichungsterm für einen Zeitpunkt kann von der Abweichung des geschätzten Posewertes für den Zeitpunkt von dem um den Positionsoffset verschobenen gemessenen Positionswert für den Zeitpunkt abhängen. Des Weiteren kann das Optimierungskriterium einen Odometrie-Abweichungsterm für zumindest einen Teil der Zeitpunkte der Sequenz von Zeitpunkten umfassen. Dabei kann der Odometrie-Abweichungsterm für einen Zeitpunkt von der Abweichung einer sich aus der Sequenz von geschätzten Posewerten ergebenden Änderung der Pose des Fahrzeugs an dem Zeitpunkt von dem Odometriewert für den Zeitpunkt abhängen.

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 und/oder auf einem Server) ausgeführt zu werden, und um dadurch das in diesem Dokument beschriebene 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 das in diesem Dokument beschriebene 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

Figur 1 beispielhafte Komponenten eines Fahrzeugs;

Fig. 2a beispielhafte Sensordaten entlang einer Fahrzeugtrajektorie; Fig. 2b eine beispielhafte, auf Basis der Sensordaten der Fig. 2a geschätzte,

F ahrzeugtraj ektorie;

Fig. 3a einen beispielhaften Positions- Anker für Positionswerte;

Fig. 3b eine beispielhafte, durch Verschieben des Positions-Ankers geschätzte,

F ahrzeugtraj ektorie;

Fig. 4a einen beispielhaften Baum von Positions- Ankern für unterschiedliche Teil Sequenzen von Positionswerten;

Fig. 4b eine beispielhafte, durch Verschieben der Positions-Anker geschätzte,

F ahrzeugtraj ektorie; und

Fig. 5 ein Ablaufdiagramm eines beispielhaften Verfahrens zur Ermittlung einer Traj ektorie eines Fahrzeugs.

Wie eingangs dargelegt, befasst sich das vorliegende Dokument mit der präzisen Ermittlung der Trajektorie eines Fahrzeugs auf Basis der (ggf. fehlerbehafteten) Sensordaten von ein oder mehreren Sensoren des Fahrzeugs. Dabei soll insbesondere eine präzise Lokalisierung der Trajektorie innerhalb eines (globalen bzw. Welt-) Referenzkoordinatensystems ermöglicht werden.

Fig. 1 zeigt beispielhafte Komponenten eines Fahrzeugs 100. Das Fahrzeug 100 umfasst ein oder mehrere Elmfeldsensoren 102, die eingerichtet sind, Sensordaten (in diesem Dokument auch als Elmfelddaten bezeichnet) in Bezug auf das Elmfeld des Fahrzeugs 100 zu erfassen. Beispielhafte Elmfeldsensoren 102 sind: eine Bildkamera, ein Radarsensor, ein Lidarsensor, ein EU traschall sensor, etc. Eine Steuereinheit 101 des Fahrzeugs 100 kann eingerichtet sein, auf Basis der Elmfelddaten (insbesondere auf Basis von Bilddaten einer Kamera) ein oder mehrere Objekte in dem Elmfeld des Fahrzeugs 100 zu detektieren. Insbesondere kann auf Basis der Umfelddaten eine Landmarke (z.B. ein Gebäude) im Umfeld des Fahrzeugs 100 detektiert werden, für die die genaue (Referenz-)Position (innerhalb des Referenzkoordinatensystems) bekannt ist. Des Weiteren kann auf Basis der Umfelddaten eine Messposition der Landmarke ermittelt werden. Die Messposition kann dann mit der Referenzposition der Landmarke verglichen werden, um das Fahrzeug 100 (innerhalb des Referenzkoordinatensystems) zu lokalisieren.

Das Fahrzeug 100 umfasst ferner einen Positionssensor 103, der eingerichtet ist, Sensordaten (in diesem Dokument auch als Positionsdaten bezeichnet) in Bezug auf die Position des Fahrzeugs 100 zu erfassen. Die Positionsdaten können wiederholt an einer Sequenz von Zeitpunkten erfasst werden. Insbesondere können die Positionsdaten Positionswerte für unterschiedliche Zeitpunkte umfassen, wobei der Positionswert für einen bestimmten Zeitpunkt die Position des Fahrzeugs 100 an dem bestimmten Zeitpunkt anzeigt (innerhalb des Referenzkoordinatensystems). Ein (mehrdimensionaler) Positionswert kann z.B. die GPS Koordinaten des Fahrzeugs 100 umfassen. Dabei ist ein Positionswert typischerweise mit einer bestimmten Unsicherheit (z.B. mit einer bestimmten Standardabweichung) behaftet.

Des Weiteren kann das Fahrzeug 100 ein oder mehrere Fahrzeugsensoren 104 umfassen, die eingerichtet sind, Sensordaten (in diesem Dokument auch als Fahrzeugdaten bezeichnet) in Bezug auf einen Zustand des Fahrzeugs 100 zu erfassen. Die Fahrzeugdaten können z.B. anzeigen: die Geschwindigkeit, die Orientierung, die Gierrate, die Beschleunigung, etc. Die Steuereinheit 101 kann eingerichtet sein, auf Basis der Fahrzeugdaten eine Pose des Fahrzeugs 100, d.h. eine (auf Basis der Odometrie ermittelte) Position und Orientierung des Fahrzeugs 100, zu ermitteln. Beispielsweise können auf Basis der Fahrzeugdaten für eine Sequenz von Zeitpunkten jeweils (mehrdimensionale) Posewerte in Bezug auf die Pose des Fahrzeugs 100 ermittelt werden. Dabei weisen die einzelnen Posewerte typischerweise eine bestimmte Unsicherheit (z.B. eine bestimmte Standardabweichung) auf.

Außerdem kann das Fahrzeug 100 eine Kommunikationseinheit 105 umfassen, die eingerichtet ist, über eine (drahtlose) Kommunikationsverbindung mit einer externen Einheit (z.B. mit einem Backend-Server) Daten auszutauschen. Das Fahrzeug 100 kann eingerichtet sein, die bei einer Fahrt des Fahrzeugs 100 erfassten Sensordaten (insbesondere die Umfelddaten, die Positionsdaten und/oder die Fahrzeugdaten) an die externe Einheit zu senden, um es der externen Einheit zu ermöglichen, die von dem Fahrzeug 100 gefahrene Trajektorie (innerhalb des Referenzkoordinatensystems) zu ermitteln. Alternativ oder ergänzend kann die Steuereinheit 101 des Fahrzeugs 100 eingerichtet sein, auf Basis der erfassten Sensordaten (zumindest teilweise) die von dem Fahrzeug 100 gefahrene Trajektorie zu ermitteln, und über die Kommunikationseinheit 105 an die externe Einheit zu senden.

Fig. 2a veranschaulicht beispielhafte Sensordaten, die bei der Fahrt eines Fahrzeugs 100 erfasst wurden. Insbesondere zeigt Fig. 2a eine Sequenz von Positionswerten 201 an einer Sequenz von aufeinanderfolgenden Zeitpunkten. Die einzelnen Positionswerte 201 weisen eine bestimmte Unsicherheit auf, die durch die Kreise 202 um die einzelnen Positionswerte 201 veranschaulicht wird. Wie oben dargelegt, können die einzelnen Positionswerte 201 (ggf. allein) anhand des Positionssensors 103 des Fahrzeugs 101 erfasst worden sein. Die einzelnen Positionswerte 201 können als Positionsvariable-Konten eines Graphen 215 betrachtet werden.

Des Weiteren zeigt Fig. 2a Posewerte 211 für die Sequenz von Zeitpunkten. Wie oben dargelegt, können die Posewerte 211 (ggf. allein) auf Basis der Fahrzeugdaten der ein oder mehreren Fahrzeugsensoren 104 ermittelt worden sein. Die Posewerte 211 können Posevariable-Knoten des Graphen 215 sein, wobei der Graph 215 eine Trajektorie des Fahrzeugs 100 beschreibt.

Die Posewerte 211 können auf Basis von Odometriewerten ermittelt werden, wobei dein Odometriewert die Veränderung der Pose des Fahrzeugs 100 zwischen zwei Posewerten 211 beschreibt. Die Odometriewerte können auf Basis der Fahrzeugdaten ermittelt werden. Im Falle einer exakten Messung und/oder Ermittlung der Posewerte 211 und der Positionswerte 201 sollten die Position des Posewertes 211 und der Positionswert 201 für einen bestimmten Zeitpunkt jeweils übereinstimmen (unter der Annahme, dass die Position der einzelnen Posewerte 211 relativ zu dem Referenzkoordinatensystems angegeben ist). Fehler und/oder Ungenauigkeiten bei der Messung und/oder der Ermittlung der Posewerte 211 (insbesondere der Odometriewerte) und der Positionswerte 201 bewirken jedoch, dass die Position des Posewertes 211 und der Positionswert 201 für einen bestimmten Zeitpunkt typischerweise voneinander abweichen. In Fig. 2a ist jedoch die Assoziation 213 des Posewertes 211 und des Positionswertes 201 für einen gleichen Zeitpunkt veranschaulicht (durch eine gestrichelte Gerade). Die Assoziation 213 kann in dem Graphen 215 durch eine Kante berücksichtigt werden.

Des Weiteren können bei der Fahrt des Fahrzeugs 100 an isolierten Zeitpunkten Landmarken 220 detektiert werden (auf Basis der Umfelddaten). Die (Referenz-) Position einer Landmarke 220 (z.B. eines Verkehrsschildes) ist dabei typischerweise bekannt (z.B. aus einer zuvor ermittelten digitalen Karte). Andererseits kann der Messwert 221 der Position der Landmarke 220 mit einem bestimmten Fehler und mit einer bestimmten Ungenauigkeit 222 behaftet sein.

Der Fehler des Messwerts 221 der Position der Landmarke 220 kann dabei durch einen Fehler in Bezug auf den Schätzwert der Position des Fahrzeugs 100 verursacht werden. In Fig. 2a ist die Assoziation 223 zwischen dem Messwert 221 der Position und der tatsächlichen Position einer Landmarke 220 veranschaulicht (durch eine gestrichelte Gerade). Die Assoziation 223 kann wiederum durch eine Kante in dem Graphen 215 berücksichtigt werden.

Die in Fig. 2a veranschaulichten Sensordaten können dazu verwendet werden, die Lage und/oder den Verlauf der tatsächlichen Trajektorie des Fahrzeugs 100 innerhalb des Referenzkoordinatensystems zu schätzen. Zu diesem Zweck kann die sogenannte Graph-SLAM (simultaneous localization and mapping) Methode verwendet werden. Auf Basis der Posewerte 211 kann ein Graph 215 bereitgestellt werden, der die Pose des Fahrzeugs 100 an einer Sequenz von Zeitpunkten beschreibt. Dabei entsprechen die einzelnen Posevariable-Knoten des Graphen 215 den einzelnen Posewerten 211. Direkt benachbarte Posevariable-Knoten sind über Kanten miteinander verbunden, wobei die Kanten ein oder mehrere Randbedingungen bzw. Odometrie-Faktoren 214 für den Übergang eines Posewertes 211 zu dem direkt nachfolgenden Posewert 211 umfassen. Die ein oder mehreren Randbedingungen bzw. Odometrie-Faktoren 214 hängen dabei typischerweise von technischen Eigenschaften des Fahrzeugs 100 (wie z.B. einen maximalen Lenkwinkel, etc.) und/oder von Messungenauigkeiten der Fahrzeugdaten ab. Insbesondere können die Odometrie-Faktoren 214 von der Messungenauigkeit der Odometriewerte zwischen zwei Posewerten 211 abhängen.

Die Graph-SLAM Methode kann darauf ausgerichtet sein, den Graphen 215 zu ermitteln, durch den ein bestimmtes Fehlerkriterium (z.B. ein mittlerer quadratischer Fehler) in Bezug auf die Abweichung des ermittelten Graphen 215 von dem verfügbaren Sensordaten reduziert, insbesondere minimiert, wird. Der Schätzwert der Position der Fahrzeugs 100 an einem bestimmten Zeitpunkt n kann z.B. als x(n) bezeichnet werden, der Positionswert 201 an dem Zeitpunkt n kann als p(n) bezeichnet werden. Es ergibt sich somit an dem Zeitpunkt n ein Positionswert-Abweichungsterm (x(n) — p(n)) ² in Bezug auf die Abweichung des Schätzwertes der Position von dem Positionswert 201. Die Änderung zwischen zwei geschätzten Positionen des Fahrzeugs 100 an einem bestimmten Zeitpunkt n kann als d(n) bezeichnet werden, und der Odometriewert an dem bestimmten Zeitpunkt n (durch den die gemessene Änderung der Position wiedergegeben wird) kann als o(n) bezeichnet werden. Es ergibt sich dann ein Odometrie-Abweichungsterm (d(n) — o(n)) ² in Bezug auf die Abweichung der geschätzten Positionsänderung (insbesondere der geschätzten Poseänderung) von der gemessenen Positionsänderung (insbesondere der gemessene Poseänderung). Die Abweichungsterme können gewichtet werden, wobei das Gewicht für den jeweiligen Term von der Unsicherheit des jeweiligen Messwertes der Position bzw. der Positionsänderung abhängt (und mit steigender Unsicherheit sinkt). Des Weiteren können die (gewichteten) Abweichungsterme für alle Zeitpunkte n =

1, JV des betrachteten Ausschnitts der zu ermittelten Trajektorie berücksichtigt, z.B. addiert, werden.

An isolierten Zeitpunkten k (z.B. an den Zeitpunkten n = 1 und n = N in dem in Fig. 2a dargestellten Beispiel) können zusätzliche Messwerte der Position des Fahrzeugs 100 auf Basis einer Landmarke 220 ermittelt werden. Dabei kann der Schätzwert der Position des Fahrzeugs 100 derart angepasst werden, dass die Abweichung zwischen dem Messwert 221 der Position der Landmarke 220 und dem (z.B. aus der digitalen Karte) bekannten Referenzwert der Position der Landmarke 220 reduziert wird. Da der Referenzwert der Position der Landmarke 220 typischerweise mit relativ hoher Genauigkeit ermittelt werden kann, weist der Abweichungsterm in Bezug auf eine Landmarke 220 typischerweise ein relativ großes Gewicht innerhalb des Gesamt-Fehler- bzw. Optimierungskriteriums auf.

Es kann dann ein Graph 215 mit einer Sequenz von Schätzwerten der Position des Fahrzeugs 100 bzw. mit einer Sequenz von geschätzten Posewerten 231 des Fahrzeugs 100 an der Sequenz von Zeitpunkten n = 1, ... , N ermittelt werden, durch den das Gesamt-Fehler- bzw. Optimierungskriterium reduziert, insbesondere minimiert, wird. Fig. 2b zeigt einen beispielhaften optimierten Graph 215 mit den geschätzten Posewerten 231 des Fahrzeugs 100 an der Sequenz von Zeitpunkten n = 1, Der optimierte Graph 215, insbesondere die Sequenz von geschätzten Posewerten 231, stellt eine Schätzung der tatsächlichen Trajektorie des Fahrzeugs 100 dar.

Wie aus Fig. 2b zu entnehmen ist, kann die Berücksichtigung von isolierten Landmarken 220 dazu führen, dass der optimierte Graph 215 in der direkten Umgebung einer Landmarke 220 eine relativ gute Schätzung der tatsächlichen Trajektorie des Fahrzeugs 100 darstellt. Aufgrund von systematischen Fehlem der Positionsdaten können jedoch in einem Bereich 230 zwischen zwei Landmarken 220 relativ starke Abweichungen der geschätzten Trajektorie (d.h. der Sequenz von geschätzten Posewerten 231) von der tatsächlichen Trajektorie des Fahrzeugs 100 entstehen. Die Abweichungen werden insbesondere dadurch bewirkt, dass in einem Bereich 230 zwischen zwei Landmarken 220 keine Messwerte 221 der Position einer Landmarke 220 verfügbar sind und/oder berücksichtigt werden, und der Graph 215 (insbesondere die Sequenz von geschätzten Posewerten 231) daher derart ermittelt wird, dass die Abweichung zu den verfügbaren Positionswerten 201 reduziert wird.

Der systematische Fehler der Positionsdaten ist in Fig. 2b durch die relativ starke Abweichung zwischen dem Schätzwert 231 der Position und dem Positionswert 201 in unmittelbarer Nähe zu den beiden Landmarken 220 ersichtlich. Die relativ starke Abweichung der geschätzten Trajektorie (d.h. der Sequenz von geschätzten Posewerten 231) von der tatsächlichen Trajektorie des Fahrzeugs 100 in dem Bereich 230 zwischen zwei Landmarken 220 führt, wie in Fig. 2b dargestellt, zu einem „Durchhängen“ der Sequenz von geschätzten Posewerten 231.

Der systematische Fehler bei der Messung der Positionswerte 201 kann dadurch berücksichtigt werden, dass eine zusätzliche Variable (d.h. ein zusätzlicher Knoten) bei der Ermittlung des Graphen 215 eingeführt wird, durch die alle zu berücksichtigenden Positionswerte 201 gemeinsam verschoben werden können. Insbesondere können die einzelnen Positionswerte 201 (d.h. die Positionsvariable- Knoten) über jeweils eine feste Kante 312 an einem gemeinsamen Positions- Anker 312 befestigt werden, der durch eine Translationsbewegung 323 verschoben werden kann, um dabei die einzelnen gemessenen Positionswerte 201 (d.h. die Positionsvariable-Knoten) jeweils in gleicher Weise verschiebt. Der Positions-Anker 321 kann ggf. gemäß einer vordefinierten Kovarianz bzw. Wahrscheinlichkeitsverteilung 322 verschoben werden.

Bei der Ermittlung des optimierten Graphen 215 (und damit der Sequenz von geschätzten Posewerten 231) kann somit als zusätzlicher Freiheitsgrad der Positions-Anker 321 verschoben (insbesondere per Translation 323 verschoben) werden. Die Verschiebung kann um einen Positionsoffset 330 erfolgen. Als Folge daraus kann implizit (durch das Verschieben des Positions- Ankers 321) ein systematischer Fehler der gemessenen Positionswerte 201 ermittelt und kompensiert werden. Die Größe des systematischen Fehlers wird dabei durch den Positionsoffset 330 beschrieben. Des Weiteren können so Abweichungen des optimierten Graphen 215 (insbesondere der Sequenz von geschätzten Posewerten 231) von der tatsächlichen Trajektorie des Fahrzeugs 100, die auf einen systematischen Fehler der gemessenen Positionswerte 201 zurückzuführen sind, vermieden werden. Fig. 3b zeigt einen optimierten Graph 215 (insbesondere eine optimierte Sequenz von geschätzten Posewerten 231) und die im Rahmen der Optimierung bewirkte Verschiebung des Positions- Ankers 321.

Der systematische Fehler bei der Erfassung der gemessenen Positionswerte 201 kann von der direkten Umgebung des Fahrzeugs 100 abhängen. Beispielsweise kann ein relativ hohes Gebäude in der direkten Umgebung des Fahrzeugs 100 (z.B. durch Multipath-Effekte) zu einer Änderung des systematischen Fehlers führen. Um unterschiedliche systematische Fehler in unterschiedlichen Segmenten einer Trajektorie berücksichtigen zu können, kann, wie in Fig. 4a dargestellt, ein Baum bzw. eine Hierarchie von Ankern 321, 421 verwendet werden.

Insbesondere kann für jedes Segment der Trajektorie ein Zwischen -Anker 421 definiert werden, an den die gemessenen Positionswerte (bzw. eine den Positionswerten entsprechende Positionsvariable 301) des jeweiligen Segments fest angebunden sind. Die Zwischen- Anker 421 können dann gemäß einer Kovarianz bzw. Wahrscheinlichkeitsverteilung 422 des jeweiligen Zwischen- Ankers 421 relativ zu einem Ankerpunkt 424 verschoben werden (dargestellt durch die Verbindung 423). Die Ankerpunkte 424 der einzelnen Zwischen- Anker können dann mit dem (Haupt-) Positions-Anker 321 verbunden sein (ggf. über ein oder mehrere weitere Ebenen von Zwi sehen -Ankems 421). Wie aus Fig. 4b zu entnehmen ist, können so in den einzelnen Segmenten der Trajektorie systematische Fehler der Positionswerte 201 berücksichtigt werden, um in besonders präziser Weise einen optimierten Graph 215, d.h. eine Schätzung der tatsächlichen Trajektorie, zu ermitteln.

In diesem Dokument wird somit ein Verfahren beschrieben, mit dem ein systematischer Fehler bei der Positionsmessung, insbesondere bei einer GPS- basierten Positionsmessung, berücksichtigt und zumindest teilweise kompensiert werden kann. Der systematische Fehler bei der Positionsmessung kann durch Atmosphäreneffekte oder durch lokale Effekte, wie die Abschirmung durch eine Hauswand oder einen Wald, hervorgerufen werden. Der systematische Fehler kann dazu führen, dass Messungen miteinander korreliert und nicht voneinander unabhängig sind. Für den auf Atmosphäreneffekte beruhenden systematischen Fehler betrifft dies alle Messungen in einem bestimmten Zeitraum, z.B. bei einer bestimmten Durchfahrung bzw. Fahrt des Fahrzeugs 100. Daneben können noch lokale Effekte auftreten, bei denen in einem gewissen Bereich, beispielsweise vor einem hohen Haus, Abschattungs- und Multipath-Effekte auftreten können, die lokal zu einem systematischen Fehler führen.

Bei dem in diesem Dokument beschriebenen Verfahren wird eine Graph-SLAM Methode um eine Modellierung von systematischen Fehlem der Positionsdaten erweitert. Dabei wird jeder gemessene Positionswert 201 (bzw. ein dem gemessenen Positionswert 201 entsprechender Positionsvariable-Knoten 301) über eine steife Kante 312 mit einem Positions-Anker 321 verbunden. Der Positions-Anker 321 kann derart definiert sein, dass der Positions-Anker (Konten) 321 über einen Prior mit festgelegten Kovarianzen 322 (ausschließlich) durch eine Translation 323 verschoben werden kann. Dadurch ist eine systematische Verschiebung aller gemessenen Positionswerte 201 bzw. Positionsvariable- Knoten 301 implizit in den Faktorgraphen 215 integriert. Die Graph-SLAM Optimierung berücksichtigt also systematische Fehler der Positionsdaten bei der Bestimmung der Fahrtrajektorie und ermittelt dabei die wahrscheinlichste systematische Verschiebung der gemessenen Positionswerte 201 bzw. der Positionsvariable-Knoten 301. Durch ein hierarchisches System mit mehreren Zwischen- Ankern 421 können auch lokale systematische Fehler der Positionsmessung modelliert und bestimmt werden.

Das beschriebene Verfahren kann bei der Lokalisierung von Fahrtrajektorien verwendet werden, wie sie insbesondere in der Änderungserkennung und für das Bereitstellen von Map-Patches benötigt wird. Es wird zunächst ein Faktorgraph 215 aufgebaut. Dabei werden die einzelnen gemessenen Positionswerte 201 nicht direkt als Prior für die Fahrzeugposition an den jeweiligen Zeitpunkten verwendet. Stattdessen wird für jede Positionsmessung eine entsprechende Positionsvariable als Positionsvariable-Knoten 301 in den Faktorgraphen 215 eingefügt. Diese Positionsvariable-Knoten 301 können über jeweils einen Positions-Faktor 313 (d.h. über entsprechende Kanten) mit zugehörigen Fahrzeugpose bzw. Posevariable-Knoten 211 verbunden werden. Die ursprünglich im Positions-Prior enthaltene Unsicherheit der Positionsmessungen kann durch den Positions-Faktor 313 modelliert werden. Zusätzlich werden alle Positionsvariable-Knoten 301 über virtuelle ideal steife Kanten 312 mit einer virtuellen Positions-Anker-Variable 321 verbunden. Diese Positions- Anker- Variable 321 kann eine Pose (Position + Orientierung) darstellen und ist typischerweise wiederum mit einem Prior (d.h. mit einer Wahrscheinlichkeitsverteilung 322 in Bezug auf die Pose der Positions-Anker- Variable 321) versehen. Um die systematische Verschiebung aller Positionsvariable-Knoten 301 zu ermöglichen, weist dieser Prior bevorzugt eine Translations-Kovarianz 322 auf, die die entsprechenden Annahmen bezüglich systematischer Verschiebung kodiert. Die Orientierung der Anker-Pose kann durch eine ideal kleine Orientierungs-Kovarianz fixiert sein, um eine Drehung der Positions-Anker-Variablen 321 zu vermeiden (und um so eine zuverlässige Modellierung des systematischen Fehlers der Positionsmessung zu gewährleisten). Durch die steifen Faktoren 312 zwischen den Positionsvariable-Knoten 301 und dem Anker-Knoten 321 bleibt die innere Geometrie zwischen den Positionsvariable-Knoten 301 erhalten und „beliebige“ systematische Translationen können im Faktorgraphen 215 modelliert und optimiert werden. Dies führt zu einer verbesserten Lokalisierung der Fahrtrajektorie und zu einer systematischen Verschiebung der Positionsmessungen.

Des Weiteren wird ein hierarchisches System mit mehreren Zwischen -Ankern 421 beschrieben. Dadurch können auch lokale systematische Fehler modelliert werden. Der Aufbau der Nachbarschaftsbeziehungen kann zeitlich und/oder räumlich durchgeführt werden. Dadurch können verschiedene lokale systematische Fehler modelliert und berücksichtigt werden. Wird diese Variante zur synchronen Verortung mehrerer Trajektorien (z.B. von mehreren Fahrzeugen 100) verwendet, so können die Zwischen- Anker 421 untereinander aufgrund räumlicher oder zeitlicher Nähe miteinander verknüpft werden, um die räumlichen und/oder zeitlichen Abhängigkeiten der zugrunde liegenden Fehler bei der Positionsmessung über verschiedene Trajektorien hinweg zu modellieren. Ein Beispiel für eine räumliche Abhängigkeit ist ein systematischer Fehler bedingt durch eine Hauswand. Eine zeitliche Abhängigkeit kann durch ionosphärische Zustände zu einem bestimmten Zeitpunkt oder durch identische Satellitenkonstellation für verschiedene Durchfahrungen zum gleichen Zeitpunkt bewirkt werden.

Fig. 5 zeigt ein Ablaufdiagramm eines beispielhaften Verfahrens 500 zur Schätzung einer tatsächlichen Trajektorie eines Fahrzeugs 100. Das Verfahren 500 kann z.B. durch eine Steuereinheit 101 des Fahrzeugs 100 und/oder durch eine Fahrzeug-externe Einheit (z.B. einen Backen- Server) ausgeführt werden.

Das Verfahren 500 umfasst das Ermitteln 501, auf Basis der Sensordaten eines Positionssensors 103 (z.B. auf Basis der Sensordaten eines GPS Empfängers), einer Sequenz von gemessenen Positionswerten 201 (z.B. GPS Koordinaten) in Bezug auf die jeweilige Position des Fahrzeugs 100 an einer entsprechenden Sequenz von Zeitpunkten. Insbesondere kann ein gemessener Positionswert 201 für einen Zeitpunkt die Position des Fahrzeugs 100 an dem Zeitpunkt relativ zu einem Referenzkoordinatensystem anzeigen. Der Positionssensor 103 kann Teil des Fahrzeugs 100 sein. Die gemessenen Positionswerte 201 können eine (mittels der Sensordaten des Positionssensors 103) gemessene Trajektorie des Fahrzeugs 100 wiedergeben.

Des Weiteren umfasst das Verfahren 500 das Ermitteln 502, auf Basis von Sensordaten von ein oder mehreren Fahrzeugsensoren 104, einer Sequenz von Odometriewerten für zumindest einen Teil der Sequenz von Zeitpunkten. Dabei kann der Odometriewert für einen Zeitpunkt anzeigen, wie sich die Pose des Fahrzeugs 100 im Vergleich zu einem vorhergehenden Zeitpunkt verändert hat. Die Sequenz von Zeitpunkten kann z.B. n = 1, ... , N Zeitpunkte umfassen. Es kann dann für die Zeitpunkte n = 2, ... , N jeweils ein Odometriewert bereitgestellt werden, der anzeigt, wie sich die Pose des Fahrzeugs 100 gegenüber dem jeweils direkt vorhergehenden Zeitpunkt n — 1 verändert hat.

Die ein oder mehreren Fahrzeugsensoren 104 können zur Ermittlung der Odometriewerte z.B. die Geschwindigkeit, den zurückgelegten Weg, die Gierrate, die Drehrate, und/oder die Beschleunigung des Fahrzeugs 100 anzeigen.

Alternativ oder ergänzend können die Odometriewerte auf Basis von ein oder mehreren Steuergrößen ermittelt werden, mit denen z.B. der Antriebsmotor, eine Bremsvorrichtung und/oder eine Lenkvorrichtung des Fahrzeugs 100 angesteuert wird. Die einzelnen Odometriewerte können jeweils die Änderung der Position und/oder die Änderung der Orientierung des Fahrzeugs 100 anzeigen. Auf Basis der Sequenz von Odometriewerten kann eine relative Trajektorie des Fahrzeugs 100 wiedergeben werden, die durch Translation und/oder Rotation in die Schätzung der tatsächlichen Trajektorie überführt werden kann. Das Verfahren 500 umfasst ferner das Ermitteln 503 (als Schätzung der tatsächlichen Trajektorie des Fahrzeugs 100) einer Sequenz von geschätzten Posewerten 231 für die Sequenz von Zeitpunkten, sowie das Ermitteln eines (systematischen) Positionsoffsets 330 für die Sequenz von gemessenen Positionswerten 201. Der geschätzte Posewert 231 für einen Zeitpunkt kann die Pose des Fahrzeugs 100 an dem Zeitpunkt relativ zu dem Referenzkoordinatensystem anzeigen. Die Sequenz von geschätzten Posewerte 231 und der Positionsoffset 330 können derart ermittelt werden, dass ein Optimierungskriterium verbessert, insbesondere zumindest lokal optimiert, wird.

Dabei kann das Optimierungskriterium einen Positionswert-Abweichungsterm für jeden der Sequenz von Zeitpunkten umfassen. Der Positionswert- Abweichungsterm für einen Zeitpunkt kann von der Abweichung 313 des geschätzten Posewertes 231 für den Zeitpunkt von dem um den Positionsoffset 330 verschobenen gemessenen Positionswert 201 für den Zeitpunkt abhängen. Es kann somit eine Sequenz von geschätzten Posewerten 231 ermittelt werden, durch die die (mittlere, ggf. quadratische) Abweichung der geschätzten Posewerte 231 von den jeweils um den Positionsoffset verschobenen gemessenen Positionswerte 201 reduziert, insbesondere minimiert, wird. Durch die Berücksichtigung eines Positionsoffsets 330 kann in effizienter und präziser Weise ein systematischer Fehler bei der Messung der gemessenen Positionswerte 201 erkannt und kompensiert werden. So kann die Genauigkeit einer Schätzung der tatsächlichen Trajektorie erhöht werden.

Des Weiteren kann das Optimierungskriterium das Optimierungskriterium einen Odometrie- Abweichungsterm für zumindest einen Teil der Zeitpunkte der Sequenz von Zeitpunkten umfassen (insbesondere für die Zeitpunkte n =

2, Dabei kann der Odometrie- Abweichungsterm für einen Zeitpunkt n von der Abweichung einer sich aus der Sequenz von geschätzten Posewerten 231 ergebenden Änderung der Pose des Fahrzeugs 100 an dem Zeitpunkt von dem Odometriewert für den Zeitpunkt abhängen. Das Optimierungskriterium kann z.B. eine (ggf. gewichtete) Summe der Positionswert-Abweichungsterme und der Odometrie- Abweichungsterme umfassen. Durch die Berücksichtigung von Positionswert-Abweichungstermen und Odometrie-Abweichungstermen kann eine präzise Schätzung der tatsächlichen Trajektorie bereitgestellt werden.

Durch die in diesem Dokument beschriebenen Maßnahmen können die Güte der Verortung einer Trajektorie relativ zu einer bestehenden digitalen Karte und/oder die Güte bei der synchronen Verortung mehrerer Trajektorien mittels gemeinsamer Landmarkensichtungen (z.B. der Sichtung von Schildern, Ampeln, Fahrstreifenmarkierungen etc.) durch die Modellierung systematischer globaler und lokaler Fehler bei der Positionsmessung erhöht werden. Insbesondere kann die Qualität der Lokalisierung der Fahrtrajektorien relativ zu einer Karte bzw. relativ zueinander durch die implizite Modellierung systematischer Fehler verbessert werden. Die in diesem Dokument beschriebenen Verfahren können z.B. für die Änderungserkennung und das Lernen von hochgenauen digitalen Karten (z.B. für das automatisierte Fahren) verwendet werden.

Die vorliegende Erfindung ist nicht auf die gezeigten Ausführungsbeispiele beschränkt. Insbesondere ist zu beachten, dass die Beschreibung und die Figuren nur das Prinzip der vorgeschlagenen Verfahren, Vorrichtungen und Systeme veranschaulichen sollen.