DE102016205193A1

DE102016205193A1 - Marginalize a posse graph

Info

Publication number: DE102016205193A1
Application number: DE102016205193.8A
Authority: DE
Inventors: Christian Merfels; Moritz Schack
Original assignee: Volkswagen AG
Current assignee: Volkswagen AG
Priority date: 2016-03-30
Filing date: 2016-03-30
Publication date: 2017-10-05

Abstract

Die Erfindung betrifft ein Verfahren zum Marginalisieren eines Posen-Graphen (661). Um ein besonders flexibles Marginalisieren des Posen-Graphen (661) zu ermöglichen wird vorgeschlagen, nach dem Optimieren des Posen-Graphen (661) einen Knoten (971) des Posen-Graphen (661) auszuwählen und für den ausgewählten Knoten (971) einen Fixknoten des Posen-Graphen (661) zu bestimmen. Der ausgewählte Knoten (971) wird dann aus dem Posen-Graphen (661) entfernt.The invention relates to a method for marginalizing a pose graph (661). In order to allow a particularly flexible marginalization of the pose graph (661), it is proposed, after optimizing the pose graph (661), to select a node (971) of the pose graph (661) and a fixed node for the selected node (971) of the Poznan graph (661). The selected node (971) is then removed from the pose graph (661).

Description

TECHNISCHES GEBIET TECHNICAL AREA

Die Erfindung betrifft ein Verfahren und ein Steuergerät. Insbesondere betreffen verschiedene Ausführungsformen Techniken, um Odometrie-Positionsdaten und Absolut-Positionsdaten einer Maschine beim Erhalten einer geschätzten Position der Maschine zu berücksichtigen. Dabei wird ein Posen-Graph verwendet. Insbesondere betreffen verschiedene Ausführungsformen Techniken, um einen ausgewählten Knoten aus dem Posen-Graphen zu entfernen. The invention relates to a method and a control unit. In particular, various embodiments relate to techniques to account for odometry position data and absolute position data of a machine in obtaining an estimated position of the machine. A pose graph is used. In particular, various embodiments relate to techniques for removing a selected node from the pose graph.

HINTERGRUND BACKGROUND

Es sind Techniken bekannt, um Karten von Gebieten zu erstellen. Z.B. ist aus DE 10 2013 208 521 A1 ein Verfahren zum kollektiven Erlenen und Erstellen eines digitalen Straßenmodells bekannt. Dabei werden Trajektoriendaten und Perzeptionsdaten für eine Mehrzahl von Trajektorien von einem oder mehreren Fahrzeugen zu erfassen. Ein Informationsgraph wird gebildet, wobei Trajektorienpunkte der Mehrzahl von Trajektorien Knoten des Informationsgraphen bilden und basierend auf Odometriemessungen und Positionsmessungen Kanten zwischen bzw. an den Knoten des Informationsgraphen gebildet werden. Der Informationsgraph wird optimiert. Eine solche Technik weist den Nachteil auf, dass die Modellbildung aufwendig ist und viel Zeit benötigt. Eine Echtzeitanwendung, z.B. um eine Fahrerassistenzfunktionalität zu steuern, ist typischerweise nicht möglich. Techniques are known to create maps of areas. For example, it is off DE 10 2013 208 521 A1 a method for collective learning and creating a digital road model known. In doing so, trajectory data and perception data for a plurality of trajectories of one or more vehicles will be collected. An information graph is formed, with trajectory points of the plurality of trajectories forming nodes of the information graph, and based on odometry measurements and position measurements, edges are formed between and at the nodes of the information graph. The information graph is optimized. Such a technique has the disadvantage that the modeling is complicated and takes a lot of time. A real-time application, eg to control driver assistance functionality, is typically not possible.

Andererseits sind auch Techniken bekannt, um im Zusammenhang mit einer Fahrerassistenzfunktionalität eine geschätzte Position eines Kraftfahrzeugs zu bestimmen (Positionsbestimmung). Insbesondere sind Techniken der Positionsbestimmung bekannt, welche Absolut-Positionsdaten und Odometrie-Positionsdaten beim Bestimmen der geschätzten Position des Kraftfahrzeugs berücksichtigen. Um die Fahrerassistenzfunktionalität zu gewährleisten, erfolgt dies in Echtzeit. On the other hand, techniques are also known for determining an estimated position of a motor vehicle in connection with driver assistance functionality (position determination). In particular, techniques of position determination are known which take into account absolute position data and odometry position data in determining the estimated position of the motor vehicle. To ensure the driver assistance functionality, this is done in real time.

Dabei indizieren die Absolut-Positionsdaten die gemessene Position des Kraftfahrzeugs zu einem bestimmten Zeitpunkt in absoluten Werten, beispielsweise in einem UTM oder WGS84 Referenzkoordinatensystem. Optional können die Absolut-Positionsdaten auch mit einer Orientierung versehen sein, welche z.B. eine aktuelle Bewegungsrichtung des Kraftfahrzeugs indizieren. Eine Kombination aus Position und Orientierung wird häufig auch als Pose bezeichnet. In this case, the absolute position data indicate the measured position of the motor vehicle at a specific point in time in absolute values, for example in a UTM or WGS84 reference coordinate system. Optionally, the absolute position data may also be provided with an orientation which is e.g. indicate a current direction of movement of the motor vehicle. A combination of position and orientation is often referred to as a pose.

Die Odometrie-Positionsdaten hingegen indizieren eine Eigenbewegung bzw. Relativbewegung des Kraftfahrzeugs, zum Beispiel in einem willkürlich festgelegten Referenzkoordinatensystem oder im sogenannten Fahrzeugkoordinatensystem (engl. body frame). Dabei wird die Eigenbewegung z.B. jeweils relativ in Bezug auf eine vorangegangene Position des Kraftfahrzeugs indiziert. The odometry position data, however, indicate a self-motion or relative movement of the motor vehicle, for example in an arbitrarily set reference coordinate system or in the so-called vehicle coordinate system (English: body frame). The proper motion is e.g. each indexed relative to a previous position of the motor vehicle.

Um verschiedenste Positionsdaten beim Bestimmen der geschätzten Position des Kraftfahrzeugs zu berücksichtigen, sind Techniken der Positionsbestimmung bekannt, die ein oder mehrere Odometrie-Positionsdaten und ein oder mehrere Absolut-Positionsdaten miteinander kombinieren (Sensorfusion). Die verschiedenen Positionsdaten können von unterschiedlichen Positioniersystemen ausgegeben werden. Die Positioniersysteme können unterschiedliche Techniken zur Positionsbestimmung einsetzen, sodass die Positionsdaten der unterschiedlichen Positioniersysteme voneinander unabhängige Schätzungen der Position darstellen können. In order to take into account a wide variety of positional data in determining the estimated position of the motor vehicle, positioning techniques are known which combine one or more odometry position data and one or more absolute position data (sensor fusion). The various position data can be output from different positioning systems. The positioning systems can use different positioning techniques so that the position data of the different positioning systems can represent independent position estimates.

Durch das Berücksichtigen mehrerer voneinander unabhängiger Positionsdaten, die als Ausgabe von verschiedenen Positioniersystemen erhalten werden, kann eine genauere und ausfallsicherere Schätzung für die Position des Kraftfahrzeugs bereitgestellt werden. Insbesondere für Anwendungen im Bereich des autonomen Fahrens ist es erforderlich, dass eine möglichst genaue Schätzung der Position des Kraftfahrzeugs mit einer hohen Ausfallsicherheit bereitgestellt werden kann. By considering a plurality of independent positional data obtained as output from different positioning systems, a more accurate and more fail-safe estimate of the position of the motor vehicle can be provided. In particular, for applications in the field of autonomous driving, it is necessary that the most accurate possible estimation of the position of the motor vehicle with a high reliability can be provided.

Aus DE 10 2014 211 178 A1 ist ein Verfahren zur Korrektur von Messdaten eines ersten Sensorsystems bekannt. Das erste Sensorsystem und/oder ein zweites Sensorsystem erfassen jeweils unterschiedliche Messdaten, die jeweils direkt und/oder indirekt Navigationsdaten beschreiben. Fehlerwerte werden korrigiert, wenn sie beobachtbar sind, und werden als konstant angenommen, wenn sie nicht beobachtbar sind. Die Fehlerwerte können mittels eines Error-State-Space Kalman-Filters bestimmt werden. Das Verwenden eines Kalman-Filters weist den Nachteil auf, dass lediglich aktuelle Messdaten berücksichtigt werden können. Insbesondere kann es nicht oder nur eingeschränkt möglich sein, weiter zurückliegende Messdaten zu berücksichtigen. Deshalb sind entsprechende Techniken oftmals vergleichsweise ungenau. Out DE 10 2014 211 178 A1 a method for correcting measurement data of a first sensor system is known. The first sensor system and / or a second sensor system each detect different measurement data, which respectively directly and / or indirectly describe navigation data. Error values are corrected if they are observable and are assumed to be constant if they are not observable. The error values can be determined by means of an error-state-space Kalman filter. The use of a Kalman filter has the disadvantage that only current measurement data can be taken into account. In particular, it may not be possible, or only to a limited extent, to take into account past measured data. Therefore, corresponding techniques are often relatively inaccurate.

Aus der deutschen Patentanmeldung DE 10 2015 219 577 sind Techniken bekannt, um Sensorfusion basierend auf einen Posen-Graphen durchzuführen. Dabei wird der Posen-Graph wiederholt optimiert. From the German patent application DE 10 2015 219 577 Techniques are known to perform sensor fusion based on a pose graph. In doing so, the pose graph is repeatedly optimized.

Solche Techniken weisen den Nachteil auf, dass mit zunehmender Laufzeit der Posen-Graph immer größer wird und damit die Optimierung rechenintensiver wird. Such techniques have the disadvantage that as the propagation time increases, the pose graph becomes ever larger and thus the optimization becomes more computationally intensive.

Es sind Techniken zur Marginalisierung eines Posen-Graphen unter Verwendung eines Schur-Komplements bekannt, siehe z.B. Sibley G. et al., J. Field Robotics 27 (2010) 587–608 . Solche Techniken weisen den Nachteil auf, dass eine Anpassung des Schur-Komplements nicht oder nur eingeschränkt möglich ist. Techniques for marginalizing a pose graph using a Schur complement are known, see eg Sibley G. et al., J. Field Robotics 27 (2010) 587-608 , Such techniques have the disadvantage that an adaptation of the Schur complement is not possible or only to a limited extent.

ZUSAMMENFASSUNG SUMMARY

Deshalb besteht ein Bedarf für verbesserte Techniken zur Marginalisierung eines Posen-Graphen. Insbesondere besteht ein Bedarf für flexible Techniken zur Marginalisierung eines Posen-Graphen. Therefore, there is a need for improved techniques for marginalizing a pose graph. In particular, there is a need for flexible techniques for marginalizing a pose graph.

Diese Aufgabe wird von den Merkmalen der unabhängigen Patentansprüche gelöst. Die abhängigen Patentansprüche definieren Ausführungsformen. This object is solved by the features of the independent claims. The dependent claims define embodiments.

Gemäß einem Aspekt betrifft die vorliegende Erfindung ein Verfahren zur Marginalisierung eines Posen-Graphen, welches für jeden von mehreren Zeitpunkten umfasst: jeweils Ermitteln von Odometrie-Positionsdaten einer Maschine basierend auf einer Ausgabe mindestens eines Odometrie-Positioniersystems. Das Verfahren umfasst weiterhin für jeden der mehreren Zeitpunkte: jeweils Ermitteln von Absolut-Positionsdaten der Maschine basierend auf einer Ausgabe mindestens eines Absolut-Positioniersystems. Das Verfahren umfasst weiterhin das Erzeugen eines Posen-Graphen. Kanten des Posen-Graphen entsprechen den Odometrie-Positionsdaten. Knoten des Posen-Graphen entsprechen den Absolut-Positionsdaten. Das Verfahren umfasst weiterhin das Optimieren des Posen-Graph zum Erhalten einer geschätzten Position der Maschine. Das Verfahren umfasst weiterhin das Auswählen eines Knotens des Posen-Graphen nach dem Optimieren. Das Verfahren umfasst weiterhin, für den ausgewählten Knoten: Bestimmen eines Fixknotens des Posen-Graphen. Das Verfahren umfasst weiterhin das Entfernen des ausgewählten Knotens aus dem Posen-Graph. In one aspect, the present invention relates to a method for marginalizing a pose graph comprising for each of a plurality of points in time: respectively determining odometry position data of a machine based on an output of at least one odometry positioning system. The method further comprises for each of the plurality of times: respectively determining absolute position data of the machine based on an output of at least one absolute positioning system. The method further includes generating a pose graph. Edges of the posse graph correspond to the odometry position data. Nodes of the pose graph correspond to the absolute position data. The method further comprises optimizing the pose graph to obtain an estimated position of the machine. The method further includes selecting a node of the pose graph after optimizing. The method further comprises, for the selected node: determining a fixed node of the pose graph. The method further includes removing the selected node from the pose graph.

Zum Beispiel kann der Posen-Graph vorzugsweise einen Knoten für jeden der mehreren Zeitpunkte aufweisen. Die mehreren Zeitpunkte können also ein Zeitfenster von der IST-Zeit bis in die Vergangenheit abdecken (engl. sliding window). Entsprechend können beim Erzeugen des Posen-Graphen Ausgaben des mindestens einen Odometrie-Positioniersystems und des mindestens einen Absolut-Positioniersystems berücksichtigt werden, die einen aktuellen Zustand der Maschine, sowie Zustände in der näheren Vergangenheit indizieren. Z.B. kann beim Erzeugen des Posen-Graph vorzugsweise eine fest vorgegebene Anzahl von Zeitpunkten berücksichtigt werden. Die Verwendung des Posen-Graphen kann eine Alternative zur Verwendung der vorbekannten Kalman-Filter zur Sensorfusion darstellen. For example, the pose graph may preferably have a node for each of the multiple times. The multiple times can thus cover a time window from the actual time to the past (sliding window). Accordingly, in generating the pose graph, outputs of the at least one odometry positioning system and the at least one absolute positioning system can be taken into account, which indicate a current state of the machine as well as states in the more recent past. For example, For example, in generating the pose graph, a fixed predetermined number of times may be considered. The use of the posse graph may be an alternative to using the previously known Kalman sensor fusion filters.

Grundlagen zum Posen-Graph und eine entsprechende Optimierung sind beispielsweise aus Kümmerle, R. et al., g2o2o: A General Framework for Graph Optimization, in Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), 2011 bekannt. Dort ist auch der Aufbau einer entsprechenden Systemmatrix beschrieben. Der Posen-Graph umfasst Knoten, welche zu optimierenden Zustandsvariablen (hier: Absolut-Positionen der Maschine zu einem bestimmten Zeitpunkt) entsprechen, und Kanten, welche einer paarweise Verknüpfung der Zustände der durch die jeweilige Kante verbundenen Knoten entsprechen. Es ist zum Beispiel vorteilhafter möglich, dass die Knoten des Posen-Graphen mit einer Richtung (engl. heading) assoziiert sind; insoweit können die Knoten Posen darstellen. Der Posen-Graph gemäß den verschiedenen hierin offenbarten Szenarien umfasst die Ausgaben des mindestens einen Odometrie-Positioniersystems und des mindestens einen Absolut-Positioniersystems. Fundamentals of the Posen graph and a corresponding optimization are for example Kümmerle, R. et al., G2o2o: A General Framework for Graph Optimization, in Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), 2011 known. There is also described the structure of a corresponding system matrix. The pose graph comprises nodes which correspond to state variables to be optimized (here: absolute positions of the machine at a specific time), and edges which correspond to a pairwise linking of the states of the nodes connected by the respective edge. For example, it is more advantageously possible for the nodes of the pose graph to be associated with a heading; in this respect, the nodes can pose poses. The pose graph according to the various scenarios disclosed herein comprises the outputs of the at least one odometry positioning system and the at least one absolute positioning system.

Z.B. können in vorteilhaften Ausführungsformen die Schätzungen für die Position der Maschine, wie sie von dem mindestens einen Absolut-Positioniersystem bereitgestellt werden, im Zusammenhang mit Knoten des Posen-Graph berücksichtigt werden. Z.B. können die verschiedenen Knoten des Posen-Graphen über Randbedingungen mit den verschiedenen Absolut-Positionsdaten des mindestens einen Absolut-Positioniersystems verknüpft sein – manchmal werden auch solche Verknüpfungen in Form von Randbedingungen als Kanten des Posen-Graphen bezeichnet; diese Terminologie wird hierin aber zur Vermeidung von Doppeldeutigen nicht übernommen. Dabei können z.B. vorteilhafterweise die Absolut-Positionsdaten formal als Fixknoten berücksichtigt werden, die durch die Optimierung nicht beeinflusst werden, und die zu optimierenden Knoten des Posen-Graphen können als Optimierungsknoten bezeichnet werden. Ein Optimierungsknoten kann mit mehreren Fixknoten, deren Position nicht durch die Optimierung verändert wird, über Randbedingungen verknüpft sein. For example, in advantageous embodiments, the estimates for the position of the machine as provided by the at least one absolute positioning system may be considered in connection with nodes of the pose graph. For example, the various nodes of the pose graph may be linked via boundary conditions to the different absolute position data of the at least one absolute positioning system - sometimes such links in the form of boundary conditions are referred to as edges of the pose graph; however, this terminology is not incorporated herein to avoid ambiguity. In this case, for example, the absolute position data may advantageously be formally designated as fixed nodes which are not affected by the optimization, and the nodes of the pose graph to be optimized may be referred to as optimization nodes. An optimization node can be linked to several fixed nodes whose position is not changed by the optimization via boundary conditions.

Z.B. können die Eigenbewegungsschätzungen, wie sie als die Ausgabe des mindestens einen Odometrie-Positioniersystems in Form der Odometrie-Positionsdaten bereitgestellt werden, vorteilhafterweise im Zusammenhang mit den Kanten des Posen-Graphen berücksichtigt werden. Wiederum können vorteilhafterweise Optimierungskanten und Fixkanten berücksichtigt werden, die in Form von Randbedingungen verknüpft sind. Die Optimierungskanten können vorzugsweise durch zwei benachbarte Optimierungsknoten vorgegeben sein. Zum Beispiel sind Szenarien denkbar, bei denen mehr als ein Odometrie-Positioniersystem Ausgaben bereitstellt; in einem solchen Szenario ist es möglich, dass mehr als eine Kante zwischen zwei benachbarten Knoten des Posen-Graphen vorhanden ist. For example, For example, the eigenmovement estimates provided as the output of the at least one odometry positioning system in the form of the odometry position data may advantageously be taken into account in connection with the edges of the pose graph. Again, advantageously, optimization edges and fixed edges can be taken into account, which are linked in the form of boundary conditions. The optimization edges may preferably be predefined by two adjacent optimization nodes. For example, scenarios are conceivable in which more than one odometry positioning system provides outputs; in such a scenario, it is possible for there to be more than one edge between two adjacent nodes of the pose graph.

Da die unterschiedlichen Odometrie-Positionsdaten und Absolut-Positionsdaten Abweichungen voneinander aufweisen können, kann es mittels des Optimierens des Posen-Graph möglich sein, die geschätzte Position als besten Kompromiss zwischen den verschiedenen Absolut-Positionsdaten und Odometrie-Positionsdaten zu erhalten. Im Rahmen des Optimierens des Posen-Graphen können die Knoten und Kanten verschoben werden, d.h. gegenüber den aus den Ausgaben der Positioniersystem erhaltenen Werten verändert werden. Die zu optimierenden Knoten werden häufig als Optimierungsknoten bezeichnet. Die Optimierungsknoten sind durch Randbedingungen mit den aus den Ausgaben der Positioniersysteme erhaltenen Knoten und Kanten verknüpft. Since the different odometry position data and absolute position data may differ from each other, by optimizing the pose graph, it may be possible to obtain the estimated position as the best compromise between the various absolute position data and odometry position data. As part of optimizing the pose graph, the nodes and edges can be shifted, i. be changed with respect to the values obtained from the outputs of the positioning system. The nodes to be optimized are often referred to as optimization nodes. The optimization nodes are linked by constraints to the nodes and edges obtained from the outputs of the positioning systems.

Es ist möglich, dass die Absolut-Positionsdaten und/oder die Odometrie-Positionsdaten mit einer gewissen Unsicherheit behaftet sind. Eine solche Unsicherheit kann auch als Varianz bezeichnet werden. Zum Beispiel kann die Varianz der Odometrie-Positionsdaten und/oder der Absolut-Positionsdaten beim Optimieren des Posen-Graph berücksichtigt werden. It is possible that the absolute position data and / or the odometry position data are subject to some uncertainty. Such uncertainty can also be called variance. For example, the variance of the odometry position data and / or the absolute position data may be taken into account when optimizing the pose graph.

Im Zusammenhang mit dem Posen-Graph werden oftmals Fehlerterme definiert, welche umso größer (kleiner) sind, umso weiter (weniger weit) sich die durch das Optimieren geschätzte Positionen von den Ausgaben der Positioniersysteme entfernen und je kleiner (größer) die Varianz der entsprechenden Ausgaben der Positioniersysteme ist. Das Optimieren kann vorzugsweise eine Konfiguration des Posen-Graphen bereitstellen, welche einem extremalen Wert der Fehlerterme entspricht. Vorzugsweise wird ein globaler extremaler Wert der Fehlerterme angestrebt. Das Optimieren kann in vorteilhafter Weise numerisch erfolgen. Das Optimieren kann in vorteilhafter Weise iterativ erfolgen, d.h. mehrere Optimierungsiterationen umfassen. Zum Beispiel kann das Optimieren vorteilhafterweise eine Technik nichtlinearer minimaler Quadrate (engl. nonlinear least-squares) umfassen. Zum Beispiel kann das Optimieren vorteilhafterweise mittels einer Gauß-Newton-Technik erfolgen und / oder mittels einer Levenberg-Marquardt-Technik erfolgen. In the context of the pose graph, error terms are often defined which are larger (smaller), the further (less) the positions estimated by optimizing move away from the outputs of the positioning systems, and the smaller (larger) the variance of the corresponding outputs the positioning systems is. The optimizing may preferably provide a configuration of the pose graph which corresponds to an extremal value of the error terms. Preferably, a global extremal value of the error terms is desired. The optimization can advantageously be done numerically. The optimization can advantageously be iterative, i. include several optimization iterations. For example, optimization may advantageously comprise a nonlinear least-squares technique. For example, optimizing can advantageously be done by means of a Gauss-Newton technique and / or by means of a Levenberg-Marquardt technique.

Solche Techniken ermöglichen eine Echtzeitanwendung. Z.B. kann die Maschine ein Kraftfahrzeug sein. Vorzugsweise umfasst weiterhin das Steuern einer Fahrerassistenzfunktionalität basierend auf der geschätzten Position des Kraftfahrzeugs. Z.B. kann die Fahrerassistenzfunktionalität vorzugsweise autonomes Fahren des Kraftfahrzeugs betreffen. Es wäre auch möglich, dass die Fahrerassistenzfunktionalität vorteilhafterweise teilautomatisches oder hochautomatisches Fahren betrifft. Es wäre auch möglich, dass die Fahrerassistenzfunktionalität vorteilhafterweise einen Stauassistent betrifft. Es ist zum Beispiel vorzugsweise möglich, dass die Fahrerassistenzfunktionalität die Position des Kraftfahrzeugs beeinflusst. Insoweit kann basierend auf der geschätzten Position die Position des Kraftfahrzeugs beeinflusst werden. Z.B. kann die Fahrerassistenzfunktionalität einen elektrischen Lenkstrang des Kraftfahrzeugs steuern. Z.B. kann die Fahrerassistenzfunktionalität vorzugsweise die Bremsen und den Motor des Kraftfahrzeugs steuern. Z.B. kann die Fahrerassistenzfunktionalität umfassen: einen Spurhalteassistenten; einen Abstandsassistenten; einen Einparkassistenten; etc. Such techniques enable real-time application. For example, the machine may be a motor vehicle. Preferably, further comprising controlling driver assistance functionality based on the estimated position of the motor vehicle. For example, For example, the driver assistance functionality may preferably relate to autonomous driving of the motor vehicle. It would also be possible for the driver assistance functionality to advantageously concern semi-automatic or highly automatic driving. It would also be possible for the driver assistance functionality to advantageously concern a traffic jam assistant. For example, it is preferably possible for the driver assistance functionality to influence the position of the motor vehicle. In that regard, based on the estimated position, the position of the motor vehicle can be influenced. For example, For example, the driver assistance functionality may control an electric power train of the motor vehicle. For example, For example, the driver assistance functionality may preferably control the brakes and engine of the motor vehicle. For example, the driver assistance functionality may include: a lane departure warning assistant; a distance assistant; a parking assistant; Etc.

Durch das Verwenden des Posen-Graphen ist es möglich, vorzugsweise eine unterschiedliche Anzahl von Odometrie-Positionsdaten und/oder eine unterschiedliche Anzahl von Absolut-Positionsdaten zu berücksichtigen. Insbesondere ist es möglich, dass die Anzahl der Odometrie-Positionsdaten und/oder die Anzahl der Absolut-Positionsdaten, die beim Erzeugen des Posen-Graph berücksichtigt wird, als Funktion der Zeit variiert. Durch die generische Verwendung der Odometrie-Positionsdaten und der Absolut-Positionsdaten können die zu fusionierenden Ausgaben verschiedener Positioniersysteme modular ausgewechselt werden. Derart kann es vorteilhafterweise möglich sein, immer sämtliche verfügbaren Ausgaben von Positioniersysteme zu berücksichtigen. Derart kann es vorzugsweise möglich sein, auch in Übergangssituationen, bei denen Ausgaben von bestimmten Positioniersysteme gerade nicht mehr oder nur noch eingeschränkt verfügbar sind, jedoch Ausgaben von weiteren Positioniersystemen verfügbar werden, die geschätzte Position der Maschine mit einer besonders hohen Zuverlässigkeit zu erhalten. By using the posse graph, it is possible to preferably consider a different number of odometry position data and / or a different number of absolute position data. In particular, it is possible that the number of odometry position data and / or the number of absolute position data taken into account in generating the pose graph varies as a function of time. The generic use of the odometry position data and the absolute position data means that the outputs of different positioning systems to be merged can be exchanged in a modular manner. Thus, it may be advantageously possible to always consider all available outputs of positioning systems. In this way, it may preferably be possible, even in transitional situations where expenses of certain positioning systems are no longer or only partially available, however, outputs from other positioning systems become available to obtain the estimated position of the machine with a particularly high reliability.

Darüber hinaus erlaubt die Verwendung des Posen-Graphen im Vergleich zu Referenzimplementierungen basierend auf Kalman-Filtern eine genauere Schätzung der Position der Maschine nicht nur für die IST-Zeit, sondern auch für die Vergangenheit. Moreover, the use of the Poznan graph, compared to reference implementations based on Kalman filters, allows a more accurate estimation of the position of the machine not only for the actual time, but also for the past.

In verschiedenen vorteilhaften Szenarien kann das Optimieren des Posen-Graphen weiterhin eine Zuverlässigkeit der geschätzten Position der Maschine bereitstellen. Es ist möglich, dass die Fahrerassistenzfunktionalität der Maschine weiterhin basierend auf der Zuverlässigkeit der geschätzten Position gesteuert wird. In various advantageous scenarios, optimizing the pose graph may further provide reliability of the estimated position of the machine. It is possible that the driver assistance functionality of the engine continues to be controlled based on the reliability of the estimated position.

Die Zuverlässigkeit kann z.B. vorzugsweise die Summe der Fehlerterme für die verschiedenen Knoten des optimierten Posen-Graphen berücksichtigen. Aktuellere (ältere) Knoten können einen größeren (geringeren) Einfluss auf die Zuverlässigkeit der geschätzten Position aufweisen. Zum Beispiel kann in einem Szenario, bei dem die durch das Optimieren erhaltene Zuverlässigkeit der geschätzten Position vergleichsweise groß wird, ein manueller Fahrereingriff von der Fahrerassistenzfunktionalität gefordert werden. The reliability can e.g. preferably consider the sum of the error terms for the various nodes of the optimized pose graph. More recent (older) nodes can have a greater (lesser) impact on the reliability of the estimated position. For example, in a scenario where the reliability of the estimated position obtained by the optimization becomes comparatively large, manual driver intervention by the driver assistance functionality may be required.

Durch das Berücksichtigen der Zuverlässigkeit der geschätzten Position kann die Fahrerassistenzfunktionalität mit einer großen Sicherheit gesteuert werden; insbesondere kann vermieden werden, dass die Fahrerassistenzfunktionalität auf Grundlage von nicht oder nur eingeschränkt belastbaren Schätzungen für die Position des Kraftfahrzeugs gesteuert wird. By considering the reliability of the estimated position, the driver assistance functionality can be controlled with a high degree of safety; In particular, it can be avoided that the driver assistance functionality is controlled on the basis of no or limited loadable estimates for the position of the motor vehicle.

In verschiedenen vorteilhaften Szenarien kann das Optimieren des Posen-Graphen weiterhin eine geschätzte Odometrie der Maschine bereitstellen. Es ist möglich, dass die Fahrerassistenzfunktionalität des Kraftfahrzeugs weiterhin basierend auf der geschätzten Odometrie gesteuert wird. In various advantageous scenarios, optimizing the pose graph may still provide an estimated odometry of the machine. It is possible that the driver assistance functionality of the motor vehicle is still controlled based on the estimated odometry.

Die Odometrie kann z.B. vorteilhafterweise eine aktuelle Orientierung / Fahrrichtung der Maschine indizieren. Zusammen mit der geschätzten Position des Kraftfahrzeugs kann so eine geschätzte Position und Orientierung (Pose) des Kraftfahrzeugs bereitgestellt werden. The odometry can e.g. advantageously index a current orientation / direction of travel of the machine. Together with the estimated position of the motor vehicle, such an estimated position and orientation (pose) of the motor vehicle can be provided.

Zum Beispiel kann vorteilhafterweise durch das Optimieren des Posen-Graphen die geschätzte Position der Maschine zu einem in Bezug auf die mehreren Zeitpunkte zukünftigen Zeitpunkt erhalten werden. For example, advantageously, by optimizing the pose graph, the estimated position of the machine may be obtained at a future time with respect to the multiple times.

In anderen Worten kann es in verschiedenen vorteilhaften Ausführungsformen möglich sein, dass die Position für einen zukünftigen Zeitpunkt abgeschätzt wird. Es kann also eine Vorhersage für die Position der Maschine getroffen werden. In other words, in various advantageous embodiments, it may be possible for the position to be estimated for a future point in time. So it can be made a prediction for the position of the machine.

Das Erzeugen und Optimieren des Posen-Graph benötigt eine gewisse Zeitdauer (Optimierungszeitdauer). Die Optimierungszeitdauer kann durch das Bereitstellen der geschätzten Position für den zukünftigen Zeitpunkt kompensiert werden. The generation and optimization of the pose graph takes a certain amount of time (optimization period). The optimization period can be compensated by providing the estimated position for the future time.

Dadurch kann es möglich sein, die Fahrerassistenzfunktionalität besonders zielgerichtet zu steuern. Insbesondere kann es möglich sein, mit einer besonders geringen Latenzzeit eine Information über die aktuelle Position der Maschine bereitzustellen. Echtzeitanwendungen sind möglich. As a result, it may be possible to control the driver assistance functionality particularly purposefully. In particular, it may be possible to provide information about the current position of the machine with a particularly low latency. Real-time applications are possible.

Die Optimierungszeitdauer kann durch Begrenzen der Anzahl von Knoten des Posen-Graphen reduziert werden. Durch das Entfernen des ausgewählten Knoten aus dem Posen-Graph kann die Anzahl der Knoten des Posen-Graphen reduziert werden. Indem jedoch gleichzeitig der Fixknoten für den Posen-Graph bestimmt wird, kann ein nachfolgendes Optimieren des Posen-Graphen trotzdem eine hohe Genauigkeit aufweisen. The optimization period can be reduced by limiting the number of nodes of the pose graph. By removing the selected node from the pose graph, the number of nodes of the pose graph can be reduced. However, by simultaneously determining the fix node for the pose graph, subsequent optimization of the pose graph may still have high accuracy.

Zum Beispiel könnte das Verfahren also weiterhin, nach dem Entfernen des ausgewählten Knotens umfassen: erneutes Optimieren des Posen-Graphen zum Erhalten einer weiteren geschätzten Position der Maschine. Die Position des Fixknotens kann gegenüber dem erneuten Optimieren unveränderlich sein. Der Fixknoten kann über eine Kante mit einem benachbarten Knoten des Posen-Graphen verbunden sein. Indem die Position des Fixknotens gegenüber dem erneuten Optimieren unveränderlich bzw. fix ist, kann die Komplexität des erneuten Optimierens vergleichsweise begrenzt sein. Dadurch kann die Optimierungszeitdauer reduziert werden. For example, after removal of the selected node, the method could further include: re-optimizing the pose graph to obtain another estimated position of the machine. The position of the fixed node may be immutable to the re-optimization. The fix node may be connected via an edge to an adjacent node of the pose graph. Since the position of the fixed node is immutable or refixed to the re-optimization, the complexity of the re-optimization may be comparatively limited. This can reduce the optimization period.

Solchen Techniken liegt die Erkenntnis zugrunde, dass es aufgrund von Restriktionen in der Komplexität der Optimierung des Posen-Graphen oftmals erforderlich sein kann, die Anzahl von Knoten und Kanten des Posen-Graphen zu reduzieren. Dies kann durch Entfernen bzw. Löschen einzelner Knoten und Kanten des Posen-Graphen erfolgen. Zum Beispiel können die ältesten Knoten und Kanten des Posen-Graphen gelöscht werden, d.h. es kann derjenige Knoten des Posen-Graphen ausgewählt werden, der dem ältesten Zeitpunkt entspricht. Ein solches Löschen von alten Knoten und Kanten bewirkt herkömmlicherweise, dass alte Positionsdaten keinen Einfluss mehr auf die aktuelle Schätzung der Position aufweisen. Dies führt dazu, dass die Schätzungen eine begrenzte Genauigkeit aufweisen. Zum Beispiel können die Schätzungen für die Position eine vergleichsweise hohe Ungenauigkeit aufweisen. Such techniques are based on the recognition that, due to restrictions in the complexity of optimizing the pose graph, it may often be necessary to reduce the number of nodes and edges of the pose graph. This can be done by removing or deleting individual nodes and edges of the pose graph. For example, the oldest nodes and edges of the pose graph can be deleted, ie, that node of the pose graph corresponding to the oldest point in time can be selected. Such deletion of old nodes and edges conventionally causes old position data to no longer affect the current estimate of the position. As a result, the estimates have limited accuracy. For example, the estimates for the position may have a comparatively high degree of inaccuracy.

Deshalb sind in vorbekannten Techniken Ansätze zur sogenannten Marginalisierung bekannt. Vorbekannte Ansätze zur Marginalisierung umfassen das Bilden des Schur-Komplements auf die Systemmatrix. Solche Techniken haben keine modellhafte Entsprechung im Posen-Graph. Solche Techniken resultieren häufig in einer dichter besetzten Systemmatrix; dadurch kann wiederum der Rechenaufwand zum Durchführen der Optimierung erhöht werden, wodurch wiederum die Optimierungszeitdauer vergleichsweise lang sein kann. Therefore, approaches to so-called marginalization are known in previously known techniques. Previous approaches to marginalization include forming the Schur complement on the system matrix. Such techniques have no model correspondence in the pose graph. Such techniques often result in a denser system matrix; This in turn can increase the computational effort required to perform the optimization, which in turn can make the optimization period relatively long.

Die hierin beschriebenen Techniken ermöglichen es, durch Vorsehen des Fixknotens anstelle des ausgewählten Knotens, beispielsweise desjenigen Knotens, der dem ältesten Zeitpunkt entspricht, einerseits eine dichtere Besetzung der Systemmatrix – und damit eine verlängerte Optimierungszeitdauer – zu vermeiden und gleichzeitig die Genauigkeit bei der Abschätzung der Position durch Optimieren des Posen-Graphen nicht oder nicht signifikant herabzusetzen. Die hierin beschriebenen Techniken ermöglichen daher eine besonders genaue Marginalisierung des Posen-Graphen im Vergleich zu vorbekannten Techniken. Mittels der hier beschriebenen Techniken kann es also möglich sein, dass auch nach mehreren Marginalisierungen des Posen-Graphen das Optimierungsproblem handhabbar bleibt und nicht als Funktion der Zeit degeneriert. The techniques described herein make it possible, by providing the fixed node instead of the selected node, for example the node which corresponds to the oldest time, to avoid a denser occupation of the system matrix and thus an extended optimization period while at the same time ensuring accuracy in the estimation of the position by optimizing the posse graph not or not significantly reduce. The techniques described herein therefore allow a particularly accurate marginalization of the posse graph compared to prior art techniques. By means of the techniques described here, it may thus be possible for the optimization problem to remain manageable and not degenerate as a function of time even after several marginalizations of the posse graph.

Es können unterschiedliche Techniken zum Bestimmen des Fixknotens eingesetzt werden. Zum Beispiel kann der Fixknoten basierend auf Elementen bestimmt werden, die aus folgender Gruppe ausgewählt sind: die Position des ausgewählten Knotens; mindestens eine Kante, die mit dem ausgewählten Knoten verbunden ist; Absolut-Positionsdaten, die dem ausgewählten Knoten entsprechen; und ein weiterer Fixknoten, der über eine Kante mit dem ausgewählten Knoten verbunden ist. Zum Beispiel können die Absolut-Positionsdaten über einen Fixknoten repräsentiert werden. Solche Fixknoten, die über eine Kante als Randbedingung mit dem ausgewählten Knoten verbunden sind, können beim Bestimmen des Fixknotens berücksichtigt werden; andere Absolut-Positionsdaten können unberücksichtigt bleiben. Zum Beispiel kann der weitere Fixknoten im Rahmen einer vorangegangenen Marginalisierung bestimmt worden sein. Zum Beispiel können alle weiteren vorhandenen entsprechenden weiteren Fixknoten berücksichtigt werden. Derart kann es zum Beispiel möglich sein, dass der Fixknoten Informationen in Bezug auf den ausgewählten und entfernten Knoten in sich bündelt. Vergangene Information wird dann durch das Entfernen des ausgewählten Knotens nicht einfach verworfen, sondern auf den Fixknoten übertragen. Deshalb könnte der Fixknoten auch als Prior-Fixknoten bezeichnet werden. Different techniques can be used to determine the fixed node. For example, the fix node may be determined based on elements selected from the group: the position of the selected node; at least one edge connected to the selected node; Absolute position data corresponding to the selected node; and another fix node connected to the selected node via an edge. For example, the absolute position data may be represented via a fix node. Such fix nodes, which are connected via an edge as a boundary condition with the selected node, can be taken into account when determining the fixed node; other absolute position data can be disregarded. For example, the further fix node may have been determined as part of a previous marginalization. For example, all other existing corresponding further fix nodes can be taken into account. For example, it may be possible for the fix node to bundle information related to the selected and remote nodes. Past information is then not simply discarded by removing the selected node, but is transferred to the fix node. Therefore, the fixed node could also be referred to as prior-fixed node.

Da ein solcher Fixknoten im Graphen direkt dargestellt werden kann, ist eine semantische Interpretation einfach möglich – im Gegensatz zur herkömmlichen Marginalisierung auf Grundlage des Schur-Komplements. Eine solche semantische Interpretation könnte zum Beispiel lauten: „die gebündelten Positionsinformationen, die bisher aus dem Posen-Graphen entfernt wurden, führen zu der Schätzung, dass die Maschine sich an der Position des Fixknoten befunden hat“. Vorteilhafterweise wäre es also möglich, dass der Fixknoten das Schur-Komplement des ausgewählten Knotens darstellt. Damit kann erreicht werden, dass der Fixknoten eine Reduktion der Genauigkeit der Optimierung durch Entfernen des ausgewählten Knotens vermeidet. In anderen Ausführungsformen sind aber auch andere Techniken zum Bestimmen des Fixknotens möglich. Since such a fixed node can be represented directly in the graph, a semantic interpretation is easily possible - in contrast to the conventional marginalization based on the Schur complement. Such a semantic interpretation could be, for example, "the bundled position information previously removed from the pose graph leads to the estimate that the machine was at the fix node location." Advantageously, it would thus be possible for the fixed node to represent the Schur complement of the selected node. This can be achieved that the fixed node avoids a reduction in the accuracy of the optimization by removing the selected node. In other embodiments, however, other techniques for determining the fixed node are possible.

Der Posen-Graph, der den Fixknoten aufweist, nicht jedoch den ausgewählten Knoten, kann weiterhin das vollständige bzw. ursprüngliche Optimierungsproblem repräsentieren. Jedoch ist es entbehrlich, die Matrix-Darstellung des Posen-Graphen zum vollständigen Verständnis zu berücksichtigen. Dies verbessert das Potenzial zum Problemverständnis des Entwicklers/Nutzers, Analysemöglichkeiten und Manipulationsmöglichkeiten zur Lösung des Problems. The pose graph, which has the fix node but not the selected node, can continue to represent the complete or original optimization problem. However, it is not necessary to consider the matrix representation of the pose graph for complete understanding. This improves the potential problem understanding of the developer / user, possibilities of analysis and manipulation possibilities for the solution of the problem.

Als weiterer Effekt ermöglicht es der Fixknoten, die Unsicherheit der alten Informationen anzupassen und die Gewichtung zu beeinflussen, falls dies erforderlich ist. Zum Beispiel könnte das Verfahren weiterhin umfassen, vor dem Durchführen einer erneuten Optimierung: Anpassen einer Verlustfunktion der Kante des Posen-Graphen, die zwischen dem Fixknoten und dem benachbarten Knoten verläuft, und/oder Anpassen einer Unsicherheit des Fixknotens. Besonders unplausible Information, die in dem Fixknoten abgebildet ist, kann dann mit einer größeren Unsicherheit berücksichtigt werden, was eine Toleranz bei der Optimierung erhöhen kann. Es ist auch möglich eine robuste Kosten- oder Verlustfunktion auf den Fehlerterm der Randbedingung, die durch den Fixknoten repräsentiert wird, anzuwenden. As a further effect, the fix node allows the uncertainty of the old information to be adjusted and the weighting to be influenced if necessary. For example, before performing a re-optimization, the method could further include: adjusting a loss function of the edge of the pose graph passing between the fix node and the neighboring node, and / or adjusting an uncertainty of the fixed node. Particularly implausible information that is mapped in the fixed node can then taken into account with greater uncertainty, which can increase tolerance for optimization. It is also possible to apply a robust cost or loss function to the error term of the constraint represented by the fix node.

Zum Beispiel wäre es möglich, dass das Entfernen des ausgewählten Knotens selektiv in Abhängigkeit einer Anzahl von Knoten des Posen-Graphen durchgeführt wird. Überschreitet zum Beispiel die Anzahl der Knoten des Posen-Graphen einen vorgegebenen Schwellenwert, so kann jeweils der älteste Knoten aus dem Posen-Graphen ausgewählt und entfernt werden, wobei gleichzeitig der entsprechende Fixknoten bestimmt wird. Dadurch kann einerseits erreicht werden, dass die Optimierungszeitdauer durch eine besonders große Anzahl von Knoten nicht unverhältnismäßig lange wird; gleichzeitig kann jedoch eine unnötig häufige Marginalisierung des Posen-Graphen vermieden werden. For example, it would be possible for the removal of the selected node to be selectively performed in response to a number of nodes of the pose graph. If, for example, the number of nodes of the pose graph exceeds a predefined threshold value, then in each case the oldest node can be selected from the pose graph and removed, at the same time the corresponding fixed node being determined. As a result, on the one hand, it can be achieved that the optimization period does not become disproportionately long due to a particularly large number of nodes; At the same time, however, an unnecessarily frequent marginalization of the posse graph can be avoided.

Gemäß verschiedener vorteilhafter Beispiele ist es möglich, dass die mehreren Zeitpunkte basierend auf einer vorgegebenen Taktrate bestimmt werden. Dann ist es vorzugsweise möglich, dass jeweils mit zugehörigen Abtastraten Roh-Odometrie-Positionsdaten als die Ausgabe des mindestens einen Odometrie-Positioniersystems empfangen werden. Entsprechend ist es vorzugsweise möglich, dass jeweils mit zugehörigen Abtastraten Roh-Absolut-Positionsdaten als die Ausgabe des mindestens einen Absolut-Positioniersystems empfangen werden. Dabei kann die vorgegebene Taktrate verschieden von den Abtastraten sein. Die Abtastraten können als Funktion der Zeit variieren. According to various advantageous examples, it is possible that the plurality of times are determined based on a predetermined clock rate. Then, it is preferably possible for raw odometry position data to be received in each case with associated sampling rates as the output of the at least one odometry positioning system. Accordingly, it is preferably possible that, in each case with associated sampling rates, raw absolute position data are received as the output of the at least one absolute positioning system. In this case, the predetermined clock rate may be different from the sampling rates. Sample rates may vary as a function of time.

Zum Beispiel wäre es möglich, dass das Verfahren weiterhin umfasst: Interpolieren der Roh-Absolut-Positionsdaten basierend auf der vorgegebenen Taktrate und basierend auf der jeweiligen Abtastrate zum Ermitteln der Absolut-Positionsdaten für jeden der mehreren Zeitpunkte. Zum Beispiel wäre es vorzugsweise möglich, dass das Verfahren weiterhin umfasst: Interpolieren der Roh-Odometrie-Positionsdaten basierend auf der vorgegebenen Taktrate und basierend auf der Abtastrate zum Ermitteln der Odometrie-Positionsdaten für die verschiedenen Zeitpunkte. Das Interpolieren kann also vorzugsweise im Orts-Zeitraum erfolgen. Z.B. kann das Interpolieren in einem Referenzkoordinatensystem des Posen-Graphen erfolgen. For example, it would be possible for the method to further include: interpolating the raw absolute position data based on the predetermined clock rate and based on the respective sampling rate to determine the absolute position data for each of the plurality of times. For example, it would be preferable for the method to further include: interpolating the raw odometry position data based on the predetermined clock rate and based on the sampling rate to determine the odometry position data for the different times. The interpolation can therefore preferably take place in the local time period. For example, the interpolation can take place in a reference coordinate system of the Poznan graph.

In verschiedenen vorteilhaften Varianten ist es möglich, dass eine Koordinatentransformation der Roh-Absolut-Positionsdaten und/oder der Roh-Odometrie-Positionsdaten durchgeführt wird, um jeweils die Absolut-Positionsdaten und/oder die Odometrie-Positionsdaten zu erhalten. Zum Beispiel könnte eine Koordinatentransformation zwischen einem UTM-Koordinatensystem und einem WGS84-Koordinatensystem durchgeführt werden. Es wäre vorteilhafterweise auch möglich, dass die Koordinatentransformation eine Koordinatenrotation umfasst. Zum Beispiel könnte eine Rotation zwischen einem proprietären Referenzsystem, beispielsweise Geschwindigkeit und Gierrate des Kraftfahrzeugs als Maschine, und dem Referenzsystem des Posen-Graphen stattfinden. Zum Beispiel kann vorzugsweise ein Modell des Kraftfahrzeugs berücksichtigt werden, um Fahrzeug-spezifische Eigenschaften bei der Koordinatentransformation zu berücksichtigen. In various advantageous variants, it is possible that a coordinate transformation of the raw absolute position data and / or the raw odometry position data is carried out in order to obtain the absolute position data and / or the odometry position data in each case. For example, a coordinate transformation between a UTM coordinate system and a WGS84 coordinate system could be performed. It would also be advantageously possible for the coordinate transformation to include a coordinate rotation. For example, rotation could take place between a proprietary reference system, such as the speed and yaw rate of the motor vehicle as a machine, and the Poznań's reference system. For example, a model of the motor vehicle may preferably be considered to account for vehicle-specific characteristics in the coordinate transformation.

Mittels solcher Techniken ist es möglich, dass unterschiedliche Zeitverhalten der verschiedenen Positioniersysteme flexibel berücksichtigt werden. Insbesondere ist es nicht notwendig, dass die verschiedenen Positioniersysteme die Ausgaben mit einer fest vorgegebenen oder statischen Abtastrate bereitstellen. Es muss keine a-priori Annahme über das Zeitverhalten der bei dem Erzeugen des Posen-Graph berücksichtigten Positionsdaten getroffen werden. By means of such techniques, it is possible that different time behavior of the various positioning systems are flexibly taken into account. In particular, it is not necessary for the various positioning systems to provide the outputs at a fixed or static sampling rate. There is no need to make a priori assumptions about the timing of the position data taken into account in generating the pose graph.

Zum Beispiel wäre es vorteilhafterweise möglich, dass die Roh-Odometrie-Positionsdaten und/oder die Roh-Absolut-Positionsdaten jeweils mit einem Zeitstempel versehen sind. Zum Beispiel könnte vorteilhafterweise der Zeitstempel einen Zeitpunkt indizieren, zu welchem das jeweilige Positioniersystem die Odometrie-Positionsdaten und/oder die Roh-Absolut-Positionsdaten gemessen hat. For example, it would be advantageously possible that the raw odometry position data and / or the raw absolute position data are each time stamped. For example, advantageously, the time stamp could indicate a point in time at which the respective positioning system has measured the odometry position data and / or the raw absolute position data.

Zum Beispiel kann es vorteilhafterweise möglich sein, dass Positionsdaten unterschiedlicher Positioniersysteme nicht in einer zeitsequenziellen Abfolge bereitgestellt werden, sondern in einer Reihenfolge, die gegenüber den entsprechenden Zeitstempeln vertauscht ist. Dies kann der Fall sein, da unterschiedliche Positioniersysteme unterschiedliche Abtastraten und / oder unterschiedliche Latenzzeiten aufweisen können. Zum Beispiel können die Odometrie-Positionsdaten und die Absolut-Positionsdaten für die mehreren Zeitpunkte in einem Speicher zwischengespeichert werden. For example, it may be advantageously possible that position data of different positioning systems are not provided in a time sequential order, but in an order that is reversed to the corresponding time stamps. This may be the case since different positioning systems may have different sampling rates and / or different latencies. For example, the odometry position data and the absolute position data for the multiple points in time may be cached in a memory.

Beim Erzeugen des Posen-Graphen können solche nicht zeitsequenziell empfangenen Roh-Positionsdaten in der Vergangenheit einfach eingefügt werden. Dies erhöht die Flexibilität beim Fusionieren der Ausgaben unterschiedlichster Positioniersysteme. Eine Echtzeit-Erzeugung und Optimierung des Posen-Graphen kann entbehrlich sein, da eine Vorhersage für die geschätzte Position zu dem zukünftigen Zeitpunkt getroffen werden kann. When generating the pose graph, such non-time sequentially received raw position data may simply be inserted in the past. This increases the flexibility in fusing the outputs of a variety of positioning systems. A real-time creation and optimization of the posse graph may be dispensable because a prediction of the estimated position at the future time can be made.

Es ist zum Beispiel vorzugsweise möglich, dass jeweils eine Iteration des Erzeugens und Optimierens des Posen-Graphen für jeden Takt einer vorgegebenen Optimierungstaktrate durchgeführt wird. Für jede Iteration kann vorteilhafterweise die entsprechende geschätzte Position der Maschine erhalten werden. For example, it is preferably possible that one iteration each of generating and optimizing the pose graph is performed for each clock of a given optimization clock rate. For each iteration advantageously the corresponding estimated position of the machine can be obtained.

Es ist möglich, dass die Optimierungstaktrate verschieden von der Taktrate, welche die mehreren Zeitpunkte bestimmt, gewählt wird. Es wäre aber auch möglich, dass die Optimierungstaktrate gleich der Taktrate zum Bestimmen der mehreren Zeitpunkte gewählt wird. In beiden Szenarien kann es möglich sein, dass der Posen-Graph iterativ erzeugt wird, d.h. dass für verschiedene Iterationen des Erzeugens und Optimierens des Posen-Graphen auf vorangegangen erzeugte Posen-Graphen zurückgegriffen werden kann. It is possible that the optimization clock rate is selected differently from the clock rate which determines the multiple timings. However, it would also be possible for the optimization clock rate to be chosen equal to the clock rate for determining the multiple points in time. In both scenarios, it may be possible for the pose graph to be generated iteratively, i. that for different iterations of generating and optimizing the pose graph on previous generated pose graphs can be used.

Zum Beispiel kann die Optimierungstaktrate im Bereich zwischen 0,1 Hz und 100 Hz liegen, bevorzugt im Bereich zwischen 1 Hz und 50 Hz. In verschiedenen Szenerien kann es erstrebenswert sein, die Optimierungstaktrate möglichst konstant zu halten. Dies kann es ermöglichen, dass nachgeschaltete Logik wie insbesondere die Fahrerassistenzfunktionalität besonders zuverlässig betrieben bzw. gesteuert werden kann. For example, the optimization clock rate may be in the range between 0.1 Hz and 100 Hz, preferably in the range between 1 Hz and 50 Hz. In various scenarios, it may be desirable to keep the optimization clock rate as constant as possible. This can make it possible for downstream logic, in particular the driver assistance functionality, to be operated or controlled particularly reliably.

Durch das Verwenden des Posen-Graphen kann ein besonders effizientes Optimieren zum Erhalten der geschätzten Position der Maschine implementiert werden. Deshalb können vergleichsweise hohe Optimierungstaktraten gewählt werden. Dadurch kann die geschätzte Position besonders zügig bestimmt werden. By using the pose graph, a particularly efficient optimization for obtaining the estimated position of the machine can be implemented. Therefore comparatively high rates of optimization can be selected. As a result, the estimated position can be determined particularly quickly.

Es ist zum Beispiel vorzugsweise möglich, dass die Zeitdauer, die zumindest für das Optimieren des Posen-Graphen in den verschiedenen Iterationen benötigt wird, überwacht wird. Dann ist es vorzugsweise möglich, dass die vorgegebene Taktrate und/oder die Anzahl der Zeitpunkte und / oder eine Genauigkeit des Optimierens in Abhängigkeit des Überwachens der Zeitdauer bestimmt wird. For example, it is preferably possible that the amount of time required for at least optimizing the pose graph in the various iterations is monitored. Then it is preferably possible for the predetermined clock rate and / or the number of times and / or an accuracy of the optimization to be determined as a function of the monitoring of the time duration.

Zum Beispiel kann vorteilhafterweise die Genauigkeit des Optimierens einer Anzahl der Optimierungsiterationen entsprechen. Zum Beispiel kann eine höhere (geringere) Genauigkeit einer größeren Anzahl (geringeren Anzahl) von Optimierungsiterationen entsprechen. For example, advantageously, the accuracy of optimizing may correspond to a number of the optimization iterations. For example, a higher (lower) accuracy may correspond to a larger number (fewer number) of optimization iterations.

Das Überwachen kann vorzugsweise das wiederholte Messen der Optimierungszeitdauer umfassen. Zum Beispiel kann vorzugsweise die gesamte Zeitdauer überwacht werden, die zum Erzeugen und Optimieren des Posen-Graph benötigt wird. Zum Beispiel wäre es möglich, kleinere (größere) Taktraten und / oder eine kleinere (größere) Anzahl von Zeitpunkten und/oder eine geringere (größere) Genauigkeit des Optimierens zu wählen, wenn das Überwachen eine größere (kleinere) Optimierungszeitdauer indiziert. Kleinere (größere) Taktraten und eine kleinere (größere) Anzahl von Zeitpunkten können in einer kleineren (größeren) Zeitdauer, die zumindest für das Optimieren des Posen-Graph in den verschiedenen Iterationen benötigt wird, resultieren. The monitoring may preferably include repeatedly measuring the optimization period. For example, it may be preferable to monitor the total amount of time needed to create and optimize the pose graph. For example, it would be possible to choose smaller (larger) clock rates and / or a smaller (larger) number of times and / or a lower (greater) accuracy of optimization if the monitoring indicates a larger (smaller) optimization period. Smaller (larger) clock rates and a smaller (larger) number of times may result in a smaller (larger) amount of time, which is needed at least for optimizing the pose graph in the various iterations.

Derart können zu lange Optimierungszeitdauern vermieden werden. In anderen Worten kann es vorzugsweise möglich sein, die Optimierungszeitdauer flexibel anzupassen. In this way, optimization periods that are too long can be avoided. In other words, it may preferably be possible to adapt the optimization period flexibly.

Zum Beispiel kann die Taktrate im Bereich von 1 Hz–1.000 Hz liegen oder im Bereich von 10 Hz–100 Hz liegen, oder im Bereich von 25–50 Hz liegen. For example, the clock rate may be in the range of 1 Hz-1000 Hz, or in the range of 10 Hz-100 Hz, or in the range of 25-50 Hz.

Grundsätzlich ist es möglich, dass zumindest einige der Abtastraten eine Zeitabhängigkeit aufweisen. Z.B. können vorzugsweise zumindest einige der Abtastraten zeitliche Schwankungen aufweisen. In anderen Worten kann es vorteilhafterweise möglich sein, Ausgaben von Positioniersysteme beim Erzeugen des Posen-Graphen zu berücksichtigen, die Positionsdaten nicht mit einer konstanten Abtastrate bereitstellen. Zum Beispiel ist es vorzugsweise möglich, dass zumindest eine der Abtastraten im Bereich von 0,1 Hz–100 Hz liegt, oder im Bereich 0,5 Hz bis 10 Hz liegt, oder im Bereich von 0,5 Hz bis 2 Hz liegt. In principle, it is possible that at least some of the sampling rates have a time dependence. For example, For example, at least some of the sampling rates may have temporal variations. In other words, it may be advantageously possible to consider outputs from positioning systems in generating the pose graph which do not provide position data at a constant sampling rate. For example, it is preferably possible for at least one of the sampling rates to be in the range of 0.1 Hz-100 Hz, or in the range 0.5 Hz to 10 Hz, or in the range of 0.5 Hz to 2 Hz.

In den verschiedenen hierein beschriebenen Szenarien kann es auch möglich sein, dass die Ausgabe von zumindest einem der Positioniersysteme die zugehörigen Positionsdaten nicht in zeitsequentieller Reihenfolge bereitstellt (engl. out-of-order). Es kann in vorteilhaften Szenarien möglich sein – z.B. im Gegensatz zu Referenzimplementierungen basierend auf einem Kalmanfilter – solche nicht-zeitsequentiell erhaltenen Positionsdaten beim Erzeugen des Posen-Graphen zu berücksichtigen. In the various scenarios described herein, it may also be possible that the output from at least one of the positioning systems does not provide the associated position data in a time sequential order (out-of-order). It may be possible in advantageous scenarios - e.g. in contrast to reference implementations based on a Kalman filter - to take into account such non-time sequentially obtained position data when generating the pose graph.

Insbesondere kann es zum Beispiel möglich sein, dass zumindest eine der Abtastraten Unterbrechungen aufweist. Zum Beispiel können die Unterbrechungen mehr als zwei Abtastintervalle der entsprechenden Abtastrate umfassen. Zum Beispiel können die Unterbrechungen mehr als 10 Abtastintervalle umfassen. Zum Beispiel können die Unterbrechungen mehr als 100 Abtastintervalle umfassen. In particular, it may be possible, for example, for at least one of the sampling rates to have interruptions. For example, the interrupts may be more than two sample intervals of the corresponding ones Sample rate include. For example, the interrupts may include more than 10 sample intervals. For example, the interrupts may include more than 100 sample intervals.

Die Unterbrechungen können Aussetzer darstellen, während welchen keine oder keine belastbaren Positionsdaten des entsprechenden Positioniersystems vorhanden sind. The interruptions may represent dropouts during which no or no loadable position data of the corresponding positioning system are present.

In anderen Worten kann es also möglich sein, dass Aussetzer beim Bereitstellen von Ausgaben der verschiedenen Positioniersysteme kompensiert werden. Der Posen-Graph kann mit einer flexiblen Anzahl von Ausgaben verschiedener Positioniersysteme erzeugt werden. In other words, it may thus be possible to compensate for dropouts in providing outputs of the various positioning systems. The pose graph can be generated with a flexible number of outputs from different positioning systems.

Zum Beispiel ist es in vorteilhaften Szenarien möglich, dass die Absolut-Positionsdaten basierend auf der Ausgabe eines ersten Absolut-Positioniersystems jeweils für mehrere Zeitpunkte in einem ersten Zeitintervall ermittelt werden; die Absolut-Positionsdaten können vorzugsweise basierend auf der Ausgabe eines zweiten Absolut-Positioniersystems jeweils für mehrere Zeitpunkte in einem zweiten Zeitintervall ermittelt werden. Das erste Absolut-Positioniersystem kann vorteilhafterweise verschieden von dem zweiten Absolut-Positioniersystem sein. Die Absolut-Positionsdaten können vorzugsweise für die mehreren Zeitpunkte in dem ersten Zeitintervall nicht basierend auf der Ausgabe des zweiten Absolut-Positioniersystems ermittelt werden. For example, in advantageous scenarios it is possible for the absolute position data to be determined based on the output of a first absolute positioning system in each case for a plurality of points in time in a first time interval; The absolute position data may preferably be determined based on the output of a second absolute positioning system for a plurality of times in a second time interval. The first absolute positioning system may advantageously be different from the second absolute positioning system. The absolute position data may preferably be determined for the plurality of times in the first time interval based on the output of the second absolute positioning system.

Deshalb kann z.B. das erste Zeitintervall einer Unterbrechung in Bezug auf die Ausgabe des zweiten Absolut-Positioniersystems darstellen. Das Verfahren kann in vorteilhaften Szenarien umfassen: Umschalten zwischen der Ausgabe des ersten Absolut-Positioniersystems und der Ausgabe des zweiten Absolut-Positioniersystems zum Ende des ersten Zeitintervalls. Therefore, e.g. represent the first time interval of an interrupt with respect to the output of the second absolute positioning system. The method may include, in advantageous scenarios: switching between the output of the first absolute positioning system and the output of the second absolute positioning system at the end of the first time interval.

In verschiedenen bevorzugten Szenarien kann es also möglich sein, dass je nach Zeitpunkt unterschiedliche Ausgaben von Positioniersystemen berücksichtigt werden. Z.B. kann es in vorteilhaften Implementierungen möglich sein, flexibel zwischen den Ausgaben der ersten und zweiten Absolut-Positioniersysteme beim Erzeugen des Posen-Graphen umzuschalten. Das Umschalten kann graduell erfolgen. Insbesondere in Übergangssituationen kann es derart möglich sein, die Position der Maschine mit einer hohen Zuverlässigkeit zu schätzen. In various preferred scenarios, it may therefore be possible that, depending on the time, different outputs of positioning systems are taken into account. For example, For example, in advantageous implementations, it may be possible to flexibly switch between the outputs of the first and second absolute positioning systems when generating the pose graph. The switching can be done gradually. In particular, in transient situations, it may be possible to estimate the position of the machine with high reliability.

Gemäß einem weiteren Aspekt betrifft die vorliegende Erfindung ein Steuergerät. Das Steuergerät umfasst mindestens eine Schnittstelle. Die mindestens eine Schnittstelle ist eingerichtet, um jeweils Ausgaben von mindestens einem Odometrie-Positioniersystem zu empfangen und um jeweils Ausgaben von mindestens einem Absolut-Positioniersystem zu empfangen. Das Steuergerät umfasst weiterhin mindestens einen Prozessor. Der mindestens eine Prozessor ist eingerichtet, um jeweils für jeden von mehreren Zeitpunkten Odometrie-Positionsdaten der Maschine basierend auf den Ausgaben des mindestens einen Odometrie-Positioniersystems zu ermitteln. Der mindestens eine Prozessor ist weiterhin eingerichtet, um jeweils für jeden der mehreren Zeitpunkte Absolut-Positionsdaten der Maschine basierend auf den Ausgaben des mindestens einen Absolut-Positioniersystems zu ermitteln. Der mindestens eine Prozessor ist weiterhin eingerichtet, um einen Posen-Graph zu erzeugen. Kanten des Posen-Graphen entsprechen den Odometrie-Positionsdaten. Knoten des Posen-Graphen entsprechen den Absolut-Positionsdaten. Der mindestens eine Prozessor ist weiterhin eingerichtet, um den Posen-Graph zum Erhalten einer geschätzten Position des Kraftfahrzeugs zu optimieren. Der mindestens eine Prozessor ist weiterhin eingerichtet, um nach dem Optimieren einen Knoten des Posen-Graphen auszuwählen, für den ausgewählten Knoten einen Fixknoten des Posen-Graphen zu bestimmen und den ausgewählten Knoten aus dem Posen-Graph zu entfernen. In another aspect, the present invention relates to a controller. The control unit comprises at least one interface. The at least one interface is arranged to receive outputs from at least one odometry positioning system and to receive outputs from at least one absolute positioning system, respectively. The controller further includes at least one processor. The at least one processor is configured to determine, for each of a plurality of times, odometry position data of the machine based on the outputs of the at least one odometry positioning system. The at least one processor is further configured to determine, for each of the plurality of times, absolute position data of the machine based on the outputs of the at least one absolute positioning system. The at least one processor is further configured to generate a pose graph. Edges of the posse graph correspond to the odometry position data. Nodes of the pose graph correspond to the absolute position data. The at least one processor is further configured to optimize the pose graph for obtaining an estimated position of the motor vehicle. The at least one processor is further configured to select a node of the pose graph after optimizing, to determine a fixed node of the pose graph for the selected node, and to remove the selected node from the pose graph.

Das Steuergerät gemäß dem gegenwärtig diskutierten Aspekt ist eingerichtet, um das Verfahren gemäß einem weiteren Aspekt der vorliegenden Erfindung auszuführen. The controller according to the presently discussed aspect is arranged to carry out the method according to another aspect of the present invention.

Für ein solche Steuergerät gemäß den gegenwärtig diskutierten Aspekt können Effekte erzielt werden, die vergleichbar sind mit den Effekten, die für das Verfahren gemäß einem weiteren Aspekt der vorliegenden Erfindung erzielt werden können. For such a controller according to the presently discussed aspect, effects comparable to those achievable for the method according to another aspect of the present invention can be achieved.

Die oben dargelegten Merkmale und Merkmale, die nachfolgend beschrieben werden, können nicht nur in den entsprechenden explizit dargelegten Kombinationen verwendet werden, sondern auch in weiteren Kombinationen oder isoliert, ohne den Schutzumfang der vorliegenden Erfindung zu verlassen. The features and features set out above, which are described below, can be used not only in the corresponding combinations explicitly set out, but also in other combinations or isolated, without departing from the scope of the present invention.

KURZBESCHREIBUNG DER FIGUREN BRIEF DESCRIPTION OF THE FIGURES

Nachfolgend wird die vorliegende Erfindung anhand bevorzugter Ausführungsformen unter Bezugnahme auf die Zeichnungen näher erläutert. In den Figuren bezeichnen gleiche Bezugszeichen gleiche oder ähnliche Elemente. Hereinafter, the present invention will be described with reference to preferred embodiments with reference to the drawings. In the figures, like reference characters designate the same or similar elements.

1 ist eine schematische Illustration des logischen Informationsflusses im Rahmen der Fusion von Positionsdaten mehrerer Positioniersysteme auf Grundlage eines Posen-Graphen gemäß verschiedener Ausführungsformen. 1 is a schematic illustration of the logical flow of information in the context of the fusion of position data of several positioning systems based on a pose graph according to various embodiments.

2 illustriert schematisch die Verfügbarkeit der unterschiedlichen Positionsdaten für Zeitpunkte in unterschiedlichen Zeitintervallen in einer Übergangssituation gemäß verschiedener Ausführungsformen. 2 schematically illustrates the availability of the different position data for times in different time intervals in a transition situation according to various embodiments.

3 illustriert schematisch die Verfügbarkeit bestimmter Positionsdaten zu verschiedenen Zeitpunkten gemäß verschiedener Ausführungsformen, wobei in 3 eine Unterbrechung, bei welcher temporär keine Positionsdaten verfügbar sind, illustriert ist. 3 schematically illustrates the availability of certain position data at different times according to various embodiments, wherein 3 an interruption in which no position data is temporarily available is illustrated.

4 illustriert schematisch ein Steuergerät gemäß verschiedener Ausführungsformen. 4 schematically illustrates a controller according to various embodiments.

5 ist ein Flussdiagramm eines Verfahrens gemäß verschiedener Ausführungsformen. 5 FIG. 10 is a flowchart of a method according to various embodiments. FIG.

6 illustriert schematisch das Erzeugen von Knoten eines Posen-Graphen auf Grundlage von Roh-Absolut-Positionsdaten gemäß verschiedener Ausführungsformen, wobei die Roh-Absolut-Positionsdaten von zwei Absolut-Positioniersystemen mit unterschiedlichen Abtastraten empfangen werden. 6 schematically illustrates generating nodes of a pose graph based on raw absolute position data according to various embodiments, wherein the raw absolute position data is received from two absolute positioning systems having different sampling rates.

7 illustriert schematisch das Erzeugen von Kanten des Posen-Graphen der 6 auf Grundlage von Roh-Odometrie-Positionsdaten gemäß verschiedener Ausführungsformen, wobei die Roh-Odometrie-Positionsdaten von einem Odometrie-Positioniersystem mit einer bestimmten Abtastrate empfangen werden. 7 schematically illustrates the generation of edges of the pose graph of 6 based on raw odometry position data according to various embodiments, wherein the raw odometry position data is received from an odometry positioning system at a particular sampling rate.

8 illustriert schematisch den Posen-Graph der 6 und 7, bevor der Posen-Graph optimiert wird. 8th schematically illustrates the pose graph of 6 and 7 before the pose graph is optimized.

9 illustriert schematisch den optimierten Posen-Graph der 6–8 und illustriert weiterhin eine aus dem optimierten Posen-Graph erhaltene geschätzte Position eines Kraftfahrzeugs. 9 schematically illustrates the optimized pose graph of 6 - 8th and further illustrates an estimated position of a motor vehicle obtained from the optimized pose graph.

10 illustriert schematisch einen Posen-Graphen gemäß verschiedener Ausführungsformen. 10 schematically illustrates a pose graph according to various embodiments.

11 illustriert schematisch eine Systemmatrix für den Posen-Graphen der 10 gemäß verschiedener Ausführungsformen. 11 schematically illustrates a system matrix for the pose graph of 10 according to various embodiments.

12, 13, 14 und 15 illustrieren schematisch die Marginalisierung eines Posen-Graphen unter Verwendung eines Fixknotens. 12 . 13 . 14 and 15 illustrate schematically the marginalization of a posse graph using a fixed node.

16 ist ein Flussdiagramm eines Verfahrens gemäß verschiedener Ausführungsformen. 16 FIG. 10 is a flowchart of a method according to various embodiments. FIG.

DETAILLIERTE BESCHREIBUNG VON AUSFÜHRUNGSFORMEN DETAILED DESCRIPTION OF EMBODIMENTS

Nachfolgend wird die vorliegende Erfindung anhand bevorzugter Ausführungsformen unter Bezugnahme auf die Zeichnungen näher erläutert. In den Figuren bezeichnen gleiche Bezugszeichen gleiche oder ähnliche Elemente. Die Figuren sind schematische Repräsentationen verschiedener Ausführungsformen der Erfindung. In den Figuren dargestellte Elemente sind nicht notwendigerweise maßstabsgetreu dargestellt. Vielmehr sind die verschiedenen in den Figuren dargestellten Elemente derart wiedergegeben, dass ihre Funktion und genereller Zweck dem Fachmann verständlich wird. In den Figuren dargestellte Verbindungen und Kopplungen zwischen funktionellen Einheiten und Elementen können auch als indirekte Verbindung oder Kopplung implementiert werden. Eine Verbindung oder Kopplung kann drahtgebunden oder drahtlos implementiert sein. Funktionale Einheiten können als Hardware, Software oder eine Kombination aus Hardware und Software implementiert werden. Hereinafter, the present invention will be described with reference to preferred embodiments with reference to the drawings. In the figures, like reference characters designate the same or similar elements. The figures are schematic representations of various embodiments of the invention. Elements shown in the figures are not necessarily drawn to scale. Rather, the various elements shown in the figures are reproduced in such a way that their function and general purpose will be understood by those skilled in the art. Connections and couplings between functional units and elements illustrated in the figures may also be implemented as an indirect connection or coupling. A connection or coupling may be implemented by wire or wireless. Functional units can be implemented as hardware, software or a combination of hardware and software.

Nachfolgend werden Techniken zur Positionsbestimmung einer Maschine, wie z. B. einem Kraftfahrzeug beschrieben, welche es erlauben, Odometrie-Positionsdaten mindestens eines Odometrie-Positioniersystems und Absolut-Positionsdaten mindestens eines Absolut-Positioniersystems zu fusionieren. Der Einfachheit wegen wird nachfolgend auf ein Kraftfahrzeug Bezug genommen, obwohl entsprechende Techniken auch für andere Maschinen angewendet werden können. Hereinafter, techniques for determining the position of a machine, such. B. a motor vehicle described, which allow odometry position data of at least one odometry Positioning system and absolute position data of at least one absolute positioning system to merge. For the sake of simplicity, reference will now be made to a motor vehicle, although corresponding techniques may be applied to other machines.

Insbesondere werden nachfolgend Techniken geschrieben, welche es erlauben, eine geschätzte Position des Kraftfahrzeugs zu bestimmen, welche für eine Fahrerassistenzfunktionalität verwendet werden kann. Z.B. kann die Fahrerassistenzfunktionalität autonomes Fahren betreffen. Es sind auch andere Fahrerassistenzfunktionalitäten denkbar. In particular, techniques are described below which allow an estimated position of the motor vehicle to be determined which can be used for driver assistance functionality. For example, For example, the driver assistance functionality may concern autonomous driving. Other driver assistance functions are also conceivable.

Verschiedene hierin offenbarte Beispiele erlauben es, die Fusion der verschiedenen Positionsdaten auf Grundlage eines Posen-Graphen durchzuführen. Der Posen-Graph kann optimiert werden, um die Position des Kraftfahrzeugs abzuschätzen. Various examples disclosed herein allow the fusion of the various position data based on a pose graph. The pose graph can be optimized to estimate the position of the motor vehicle.

Dabei werden für jeden von mehreren Zeitpunkten Odometrie-Positionsdaten eines Kraftfahrzeugs basierend auf der Ausgabe mindestens eines Odometrie-Positioniersystems ermittelt und weiterhin Absolut-Positionsdaten des Kraftfahrzeugs basierend auf einer Ausgabe mindestens eines Absolut-Positioniersystems ermittelt. Der Posen-Graph wird erzeugt, wobei Knoten des Posen-Graphen den Absolut-Positionsdaten entsprechen und wobei Kanten des Posen-Graphen den Odometrie-Positionsdaten entsprechen. Der Posen-Graph wird dann optimiert. In this case, odometry position data of a motor vehicle based on the output of at least one odometry positioning system are determined for each of a plurality of points in time, and further absolute position data of the motor vehicle is determined based on an output of at least one absolute positioning system. The pose graph is generated, where nodes of the pose graph correspond to the absolute position data, and where edges of the pose graph correspond to the odometry position data. The pose graph is then optimized.

Hierin werden Techniken beschrieben, die es ermöglichen, den Posen-Graphen effizient und ohne Genauigkeitsverlust zu marginalisieren. Dabei wird ein zu optimierender Knoten durch einen Fixknoten ersetzt. Herein, techniques are described which make it possible to marginalize the posse graph efficiently and without loss of accuracy. In this case, a node to be optimized is replaced by a fixed node.

In 1 sind verschiedene Aspekte in Bezug auf einen Arbeitsablauf zur Positionsbestimmung mittels Sensorfusion illustriert. Es werden Roh-Absolut-Positionsdaten 105, 106 als Ausgaben von zwei Absolut-Positioniersystemen 101, 102 empfangen. Ferner werden Roh-Odometrie-Positionsdaten 115, 116 als Ausgaben von zwei Odometrie-Positioniersystemen 111, 112 empfangen. Zum Beispiel könnte ein Software-Modul 151 die entsprechenden Positionsdaten 105, 106, 115, 116 auf einem dedizierten Ausführungsstrang (engl. thread) empfangen. Ein Speicher (in 1 nicht gezeigt) kann die empfangenen Positionsdaten 105, 106, 115, 116 zwischenspeichern, bis sie verworfen werden. In 1 Several aspects relating to a sensor fusion positioning workflow are illustrated. It will be raw absolute position data 105 . 106 as outputs of two absolute positioning systems 101 . 102 receive. Furthermore, raw odometry position data 115 . 116 as outputs from two odometry positioning systems 111 . 112 receive. For example, a software module could 151 the corresponding position data 105 . 106 . 115 . 116 received on a dedicated thread. A memory (in 1 not shown) can receive the received position data 105 . 106 . 115 . 116 cache them until they are discarded.

Es ist dann möglich, in regelmäßigen, einstellbaren Zeitintervallen – das heißt mit einer Optimierungstaktrate – einen Konstruktionsprozess zum Erzeugen des Posen-Graphen durch eine Graphenverwaltung 152 anzustoßen. Dabei kann zum Beispiel der in einer vorangehenden Iteration der Optimierungstaktrate erzeugte Posen-Graph jeweils um neue Knoten und Kanten erweitert werden. It is then possible, at regular, adjustable time intervals - that is, with an optimization clock rate - a construction process for generating the pose graph by a graph manager 152 to initiate. In this case, for example, the pose graph generated in a preceding iteration of the optimization clock rate can be extended in each case by new nodes and edges.

Der Posen-Graph kann in verschiedenen Szenarien Knoten aufweisen, die in festen Zeitabständen angeordnet sind. Entsprechend ist es möglich, dass mehrere Zeitpunkte, für die Knoten zu dem Posen-Graph hinzugefügt werden, basierend auf einer vorgegebenen Taktrate ermittelt werden. Für jeden der mehreren Zeitpunkte können dann entsprechende Odometrie-Positionsdaten und Absolut-Positionsdaten basierend auf den Roh-Odometrie-Positionsdaten und den Roh-Absolut-Positionsdaten ermittelt werden und jeweils mit zu optimierenden Kanten bzw. Knoten des Posen-Graphen assoziiert werden, z.B. in Form von Randbedingungen. The pose graph may have nodes arranged at fixed intervals in various scenarios. Accordingly, it is possible that a plurality of times for which nodes are added to the pose graph are determined based on a predetermined clock rate. For each of the multiple time points, corresponding odometry position data and absolute position data can then be determined based on the raw odometry position data and the raw absolute position data and associated with respective edges or nodes of the pose graph, e.g. in the form of boundary conditions.

Z.B. kann die Optimierungstaktrate gleich oder ungleich der Taktrate der Knoten des Posen-Graphen sein. For example, For example, the optimization clock rate may be equal to or different from the clock rate of the nodes of the pose graph.

Nachdem der Posen-Graph erzeugt wurde, kann die Optimierung durchgeführt werden. Dazu wird der erzeugte Posen-Graph an ein Backend 153 übergeben. Das Backend 153 führt dann die Optimierung aus. Als Ergebnis der Optimierung wird eine IST-Position des Kraftfahrzeugs erhalten. After the pose graph has been generated, the optimization can be performed. To do this, the generated pose graph is applied to a backend 153 to hand over. The backend 153 then performs the optimization. As a result of the optimization, an actual position of the motor vehicle is obtained.

Dabei kann in den verschiedenen hierin offenbarten Szenarien die IST-Position eine Vorhersage für einen zukünftigen Zeitpunkt sein, um Latenzzeiten aufgrund der Erzeugung des Posen-Graphen durch die Graphenverwaltung 152 und das Durchführen der Optimierung durch das Backend 153 zu kompensieren. Zum Beispiel wäre es möglich, dass die IST-Position eine Vorhersage der Position des Kraftfahrzeugs für den nächsten Takt der vorgegebenen Optimierungstaktrate ist. Herein, in the various scenarios disclosed herein, the actual position may be a prediction for a future time, latencies due to the generation of the pose graph by the graph manager 152 and performing the optimization by the backend 153 to compensate. For example, it would be possible for the ACTUAL position to be a prediction of the position of the motor vehicle for the next clock of the predetermined optimization clock rate.

Mittels solcher Techniken zur Positionsbestimmung auf Grundlage des Posen-Graphen ist es möglich, flexibel Positionsdaten 105, 106, 115, 116 zu berücksichtigen, die aus unterschiedlichsten Positioniersystemen 101, 102, 111, 112, 113 stammen, siehe 2. Dabei ist der konkrete Typ des verwendeten Positioniersystems 101, 102, 111–113 nicht wesentlich für die Funktionsweise der hierin beschriebenen Techniken zur Positionsbestimmung; es können unterschiedlichste Typen und Arten von Positioniersystemen 101, 102, 111–113 in den verschiedenen hierin offenbarten Szenarien verwendet werden. By means of such techniques for determining the position on the basis of the pose graph, it is possible to flexibly position data 105 . 106 . 115 . 116 to take into account the most diverse positioning systems 101 . 102 . 111 . 112 . 113 come, see 2 , Here is the specific type of positioning system used 101 . 102 . 111 - 113 not essential to the operation of the positioning techniques described herein; Different types and types of positioning systems can be used 101 . 102 . 111 - 113 in the various scenarios disclosed herein.

Zum Beispiel können Satelliten-basierte Positioniersysteme zum Bereitstellen der Absolut-Positionsdaten 105, 106 verwendet werden; ein Beispiel ist das Global Positioning System (GPS). Zum Beispiel kann eine Erkennung von maschinenlesbaren Zeichen 101 als Positioniersystem zum Bereitstellen der Absolut-Positionsdaten 105, 106 verwendet werden. Zum Beispiel könnte eine Laser-basierte Abstandsmessung 111 (engl. Light detection and ranging, LIDAR) zum Bestimmen von Absolut-Positionsdaten 105, 106 und / oder Odometrie-Positionsdaten 115, 116 verwendet werden. Auch die visuelle Posenerkennung 112 kann basierend auf Unterschieden in sequentiell aufgenommenen Bildern, die eine Landmarke abbilden, die Odometrie-Positionsdaten 115, 116 bereitstellen. Informationen aus der Fahrwerk-Odometrie 113 können zum Bereitstellen der Odometrie-Positionsdaten 115, 116 verwendet werden; zum Beispiel können in diesem Zusammenhang ein Lenkwinkel und/oder eine Drehgeschwindigkeit der Räder berücksichtigt werden. Es könnten auch Beschleunigungssensoren zum Bereitstellen der Odometrie-Positionsdaten 115, 116 verwendet werden; zum Beispiel könnte ein elektronisches Stabilitätssystem (ESP) als Beschleunigungssensor verwendet werden. For example, satellite-based positioning systems can provide the absolute position data 105 . 106 be used; an example is the Global Positioning System (GPS). For example, recognition of machine-readable characters 101 as a positioning system for providing the absolute position data 105 . 106 be used. For example, a laser-based distance measurement 111 (Light detection and ranging, LIDAR) for determining absolute position data 105 . 106 and / or odometry position data 115 . 116 be used. Also the visual pose detection 112 may map the odometry position data based on differences in sequentially captured images that map a landmark 115 . 116 provide. Information from the chassis odometry 113 can be used to provide the odometry position data 115 . 116 be used; For example, in this context, a steering angle and / or a rotational speed of the wheels can be considered. Accelerometers could also be used to provide the odometry position data 115 . 116 be used; For example, an electronic stability system (ESP) could be used as the acceleration sensor.

Eine weitere Quelle für die Positionsdaten könnten car2car-Systeme sein: diese können entweder direkt die Position eines Fahrzeugs bestimmen oder eine indirekte Positionsbestimmung über die Position eines dritten Fahrzeugs ermöglichen, wobei dann die Relativpositionierung zum dritten Fahrzeug berücksichtigt werden kann. Another source for the position data could be car2car systems: these can either directly determine the position of a vehicle or allow indirect positioning via the position of a third vehicle, in which case the relative positioning to the third vehicle can be taken into account.

Stereo- und/oder Monokameras können nicht nur zur Bereitstellung von Odometrie-Positionsdaten genutzt werden, sondern alternativ oder zusätzlich zur Bereitstellung von Absolut-Positionsdaten. Hierfür gibt es verschiedene Ansätze: es ist z.B. möglich, eine visuelle Karte einer Strecke zu erstellen und beim erneuten Durchfahren eine Lokalisierung in der Karte vorzunehmen. Stereo and / or mono cameras can be used not only to provide odometry position data, but alternatively or in addition to providing absolute position data. There are various approaches for this: it is e.g. possible to create a visual map of a route and make a localization in the map when driving through again.

Techniken der simultanen Lokalisierung und Kartenerstellung (engl. Simultaneous Localization and Mapping, SLAM) können auch als Quelle für Positionsdaten dienen. Z.B. können SLAM-Techniken Absolut-Positionsdaten in einem globalen Koordinatensystem bereitstellen oder alternativ oder zusätzlich Odometrie-Positionsdaten bereitstellen. Simultaneous Localization and Mapping (SLAM) techniques can also serve as a source of positional data. For example, For example, SLAM techniques may provide absolute position data in a global coordinate system or alternatively or additionally provide odometry position data.

Weitere Beispiele für Positioniersysteme umfassen: Techniken basierend auf RFID-Markern. Techniken basierend auf WLAN-Netzen, die aktuell empfangen werden und / oder basierend auf einer Triangulation in einer bekannten WLAN-Netzwerke-Karte. Es können auch Mobilfunkmasten ausgelesen werden. Other examples of positioning systems include: techniques based on RFID markers. Techniques based on WLAN networks currently being received and / or based on triangulation in a known WLAN network card. It can also be read cell towers.

In den verschiedensten hierin beschriebenen Szenarien kann es jeweils auch möglich sein, durch Differenzbildung von zwei zeitsequentiellen Absolut-Positionsdaten Odometrie-Positionsdaten zu erzeugen. In the most diverse scenarios described herein, it may also be possible in each case to generate odometry position data by differentiating two time-sequential absolute position data.

Aus 2 ist ersichtlich, dass einerseits die Zeitintervalle 201, in welchen die verschiedenen Positioniersysteme 101, 102, 111–113 die zugehörigen Positionsdaten bereitstellen, verschieden voneinander sind. 2 stellt eine sogenannte Übergangssituation dar: die von LIDAR 111, der Erkennung von maschinenlesbaren Zeichen 101 und der visuellen Posenerkennung 112 bereitgestellten Positionsdaten 105, 106, 115, 116 brechen ab; dafür werden Positionsdaten des GPS-Systems 106 verfügbar. Eine entsprechende Situation kann sich zum Beispiel beim Verlassen eines Parkhauses ergeben. Dies bedeutet, dass zunächst die Absolut-Positionsdaten 105 aus der Erkennung maschinenlesbarer Zeichen 101 erhalten werden und nicht von dem GPS-System 106; anschließend werden die Absolut-Positionsdaten 106 von dem GPS-System 102 erhalten. In den verschiedenen hierin beschriebenen Szenarien kann also zwischen unterschiedlichen Quellen für die Positionsdaten 105, 106, 115, 116 umgeschaltet werden; dies bedeutet, dass in den verschiedenen hierin beschriebenen Szenarien zwischen den unterschiedlichen Positioniersysteme 101, 102, 111–113 umgeschaltet werden kann. Das Umschalten kann graduell erfolgen. Das Umschalten kann Situationsangepasst erfolgen. Das Umschalten kann unter Berücksichtigung von Übergangsphasen zwischen den verschiedenen Positioniersystemen erfolgen. Out 2 it can be seen that, on the one hand, the time intervals 201 in which the different positioning systems 101 . 102 . 111 - 113 provide the associated position data, are different from each other. 2 represents a so-called transitional situation: that of LIDAR 111 , the detection of machine-readable characters 101 and the visual pose detection 112 provided position data 105 . 106 . 115 . 116 cancel; for this position data of the GPS system 106 available. A similar situation can arise, for example, when leaving a parking garage. This means that first the absolute position data 105 from the recognition of machine-readable characters 101 be obtained and not from the GPS system 106 ; then the absolute position data 106 from the GPS system 102 receive. Thus, in the various scenarios described herein, different sources of position data may be used 105 . 106 . 115 . 116 be switched; this means that in the various scenarios described herein between the different positioning systems 101 . 102 . 111 - 113 can be switched. The switching can be done gradually. Switching can be done according to the situation. The switching can take place taking into account transition phases between the various positioning systems.

In 2 ist ferner illustriert, dass sich die Abtastraten 202, mit welchen die verschiedenen Positioniersysteme 101, 102, 111–113 die zugehörigen Roh-Positionsdaten bereitstellen, voneinander abweichen. In 2 is further illustrated that the sampling rates 202 with which the different positioning systems 101 . 102 . 111 - 113 provide the associated raw position data, differ from one another.

In verschiedenen Szenarien ist es auch möglich, dass die Abtastrate 202 eine Unterbrechung 203 aufweist, siehe 3. In 3 ist die Abtastrate 202 als Funktion der Zeit aufgetragen. In einen solchen Szenario ist es möglich, dass während der Unterbrechung 203 das entsprechende Positioniersystem 101, 102, 111–113 keine oder keine signifikanten Ausgaben bereitstellt. Eine Unterbrechung 203 kann insbesondere dann vorliegen, wenn wesentlich weniger Roh-Positionsdaten bereitstehen, als durch eine Taktrate, mit der Knoten in den Posen-Graph eingefügt werden, gefordert. Kurze Unterbrechungen 203 können kompensiert werden, beispielsweise durch Interpolation. Grundsätzlich ist es aber möglich, dass die Unterbrechung 203 vergleichsweise lang ist. Zum Beispiel kann die Unterbrechung 203 mehrere Abtastintervalle der Abtastrate 202 umfassen. In dem Szenario der 3 beträgt die Abtastrate 202 in etwa 1 Hz vor und nach der Unterbrechung 203. Dies bedeutet, dass ein Abtastintervall eine Länge von ca. 1 Sekunde aufweist. Die Unterbrechung 203 weist eine Zeitdauer von ca. 12 Sekunden auf, umfasst also mehr als 10 Abtastintervalle. In different scenarios, it is also possible that the sampling rate 202 a break 203 see, see 3 , In 3 is the sampling rate 202 plotted as a function of time. In such a scenario, it is possible that during the interruption 203 the corresponding positioning system 101 . 102 . 111 - 113 provides no or no significant expense. A break 203 may be present when substantially less raw position data is available than required by a clock rate with which nodes are inserted into the pose graph. Short breaks 203 can be compensated, for example by interpolation. In principle, it is possible that the interruption 203 is comparatively long. For example, the interruption 203 several sampling intervals of the sampling rate 202 include. In the scenario of 3 is the sampling rate 202 in about 1 Hz before and after the interruption 203 , This means that a sampling interval has a length of about 1 second. The interruption 203 has a duration of about 12 seconds, so includes more than 10 sampling intervals.

Grundsätzlich ist es auch möglich, dass die Abtastrate 202 – außerhalb einer Unterbrechung 203 – als Funktion der Zeit variiert. Zum Beispiel könnte je nach verfügbaren Signalqualität das Satelliten-basierte Positionssystem 106 eine höhere oder geringere Abtastrate 202 implementieren. Basically, it is also possible that the sampling rate 202 - outside of an interruption 203 - varies as a function of time. For example, depending on the available signal quality, the satellite-based positioning system could 106 a higher or lower sampling rate 202 to implement.

Nachfolgend werden Techniken beschrieben, um die IST-Position 190 des Kraftfahrzeugs durchgängig und zuverlässig mit einer hohen Genauigkeit zu bestimmen. Diese Techniken können auch in Übergangssituationen oder bei voneinander abweichenden bzw. mit Unterbrechungen 203 versehenen Abtastraten 201 zuverlässig und genau die IST-Position 190 bestimmen. The following describes techniques for the actual position 190 of the motor vehicle consistently and reliably to determine a high accuracy. These techniques can also be used in transitional situations or in divergent or intermittent situations 203 provided sampling rates 201 reliable and exactly the actual position 190 determine.

4 illustriert ein Steuergerät 120 welches zum Durchführen entsprechender Techniken eingerichtet ist. Das Steuergerät 120 umfasst eine Schnittstelle 121, welche mit den verschiedenen Positioniersystemen 101, 102, 111, 112 kommuniziert. Ferner umfasst das Steuergerät 120 einen Prozessor 123, der mit der Schnittstelle 121 und einem Speicher 124 gekoppelt ist. Zum Beispiel kann der Speicher 124 die verschiedenen Positionsdaten 105, 106, 115, 116 zwischenspeichern bzw. puffern. Der Prozessor 123 kann eingerichtet sein, um verschiedenste Techniken, die hierin im Zusammenhang mit dem Ermitteln der Odometrie-Positionsdaten und der Absolut-Positionsdaten auf Grundlage jeweils der Roh-Odometrie-Positionsdaten und der Roh-Absolut-Positionsdaten, dem Erzeugen des Posen-Graphen und dem Optimieren des Posen-Graphen, sowie dem Steuern der Fahrerassistenzfunktionalität beschrieben sind, durchzuführen. Zum Steuern der Fahrerassistenzfunktionalität 129 umfasst das Steuergerät 120 eine weitere Schnittstelle 122. Die verschiedenen Schnittstellen 121, 122 können eingerichtet sein, um zum Beispiel über ein Bus-System des Kraftfahrzeugs mit den verschiedenen Einheiten zu kommunizieren. Es sind auch direkte Datenverbindungen möglich. 4 illustrates a control unit 120 which is set up to implement appropriate techniques. The control unit 120 includes an interface 121 , which with the different positioning systems 101 . 102 . 111 . 112 communicated. Furthermore, the control unit comprises 120 a processor 123 that with the interface 121 and a memory 124 is coupled. For example, the memory 124 the different position data 105 . 106 . 115 . 116 cache or buffer. The processor 123 may be arranged to implement various techniques referred to herein in connection with determining the odometry position data and the absolute position data based on each of the raw odometry position data and the raw absolute position data, generating the pose graph, and optimizing of the pose graph, as well as controlling the driver assistance functionality. To control the driver assistance functionality 129 includes the controller 120 another interface 122 , The different interfaces 121 . 122 may be arranged to communicate with the various units, for example via a bus system of the motor vehicle. There are also direct data connections possible.

Zum Beispiel können in dem Speicher 124 Steuerdaten hinterlegt sein, die von dem Prozessor 123 ausgeführt werden können. Ausführen der Steuerdaten durch den Prozessor 123 kann bewirken, dass der Prozessor 123 ein Verfahren ausführt, wie es im Zusammenhang mit dem Flussdiagramm der 5 illustriert ist. For example, in the memory 124 Control data may be deposited by the processor 123 can be executed. Executing the control data by the processor 123 can cause the processor 123 performs a method as described in connection with the flow chart of 5 is illustrated.

Zunächst wird in Schritt 1 überprüft, ob die nächste Iteration für das Erzeugen und Optimieren eines Posen-Graphen durchgeführt werden soll; dafür kann eine vorgegebene Optimierungstaktrate berücksichtigt werden. Für jede Iteration wird eine IST-Position 190 erhalten. First, in step 1 checks if the next iteration is to be done for creating and optimizing a pose graph; For this purpose, a predetermined optimization rate can be taken into account. For each iteration becomes an actual position 190 receive.

Optional kann in allen hierin beschriebenen Beispielen für jede Iteration auch eine IST-Odometrie erhalten werden. Z.B. kann es möglich sein, die IST-Odometrie auf Grundlage der beiden letzten Optimierungsknoten zu berechnen, d.h., unter Berücksichtigen der zurückgelegten Strecke und der erfolgten Drehung. Daraus kann z.B. auf die Geschwindigkeit und die Drehrate zurückgeschlossen werden. Optionally, in all examples described herein for each iteration, an actual odometry may also be obtained. For example, For example, it may be possible to calculate the actual odometry based on the last two optimization nodes, i.e. taking into account the distance covered and the rotation made. This may be e.g. be deduced the speed and the rotation rate.

Wenn die nächste Iteration durchgeführt werden soll, werden in Schritt 2 zunächst für mehrere Zeitpunkte Absolut-Positionsdaten auf Grundlage von Roh-Absolut-Positionsdaten 105, 106 ermittelt. Dies ist in 6 illustriert. In 6 ist der Posen-Graph 661 lediglich in Bezug auf Knoten 671 illustriert. Der Posen-Graph 661 ist in 6 im zweidimensionalen xy-Ortsraum dargestellt. Der Posen-Graph 661 umfasst Optimierungsknoten 671 zu den Zeitpunkten t.1, t.3, t.5, t.8 und t.11. Die Zeitpunkte t.1, t.3, t.5, t.8 und t.11 weisen einen festen Zeitabstand zueinander auf und sind entsprechend einer Taktrate bestimmt. If the next iteration is to be performed, in step 2 first for several times absolute position data based on raw absolute position data 105 . 106 determined. This is in 6 illustrated. In 6 is the pose graph 661 only in terms of nodes 671 illustrated. The pose graph 661 is in 6 shown in two-dimensional xy-space. The pose graph 661 includes optimization nodes 671 at times t.1, t.3, t.5, t.8 and t.11. The times t.1, t.3, t.5, t.8 and t.11 have a fixed time interval from each other and are determined according to a clock rate.

In den verschiedenen hierin offenbarten Szenarien können unterschiedlichste Taktraten berücksichtigt werden. Zum Beispiel kann die Taktrate im Bereich von 1 Hz bis 1000 Hz liegen, bevorzugt im Bereich von 10 Hz bis 100 Hz, besonders bevorzugt im Bereich von 25 Hz bis 50 Hz. Derart kann die IST-Position zügig und mit einer hinreichend hohen Zuverlässigkeit bestimmt werden. Various clock rates may be considered in the various scenarios disclosed herein. For example, the clock rate may be in the range of 1 Hz to 1000 Hz, preferably in the range of 10 Hz to 100 Hz, more preferably in the range of 25 Hz to 50 Hz. Thus, the actual position can be determined quickly and with a sufficiently high reliability become.

In den verschiedenen hierin offenbarten Szenarien ist es möglich, dass die Taktrate veränderlich ist und flexibel bestimmt wird. Zum Beispiel kann eine höhere Taktrate einer größeren Anzahl von Knoten 671 entsprechen, sodass wiederum die Optimierungszeitdauer zunimmt; deshalb kann es erstrebenswert sein, in Abhängigkeit von einem Überwachen der Optimierungszeitdauer die Taktrate zu bestimmen. Derart kann eine bestimmte Optimierungstaktrate auch für rechenintensive Posen-Graphen 661 gewährleistet werden. In the various scenarios disclosed herein, it is possible that the clock rate is variable and flexibly determined. For example, a higher clock rate may result in a larger number of nodes 671 so that in turn the optimization period increases; therefore it may be worthwhile in Dependence on monitoring the optimization period to determine the clock rate. In this way, a certain optimization clock rate can also be used for calculation-intensive posse graphs 661 be guaranteed.

Die Knoten 671 entsprechen den Absolut-Positionsdaten 605, 606 (in 6 mit gestricheltgefüllten Kreisen und Quadraten dargestellt). Die Absolut-Positionsdaten 605, 606 bilden insbesondere Randbedingungen für die Knoten 671. Die Absolut-Positionsdaten 605, 606 werden basierend auf den Roh-Absolut-Positionsdaten 105, 106 bestimmt. Dabei werden die Roh-Absolut-Positionsdaten 105, 106 mit Abtastraten erhalten, die sich von der Taktrate unterscheiden; z.B. werden Roh-Absolut-Positionsdaten 105, 106 zu den Zeitpunkten t.2, t.4, t.4a, t.6, t.7, t.9, t.10 und t.12 erhalten. Die Roh-Absolut-Positionsdaten 105, 106 weisen entsprechende Zeitstempel auf. Deshalb werden die Roh-Absolut-Positionsdaten 105, 106 zum Erhalten der Absolut-Positionsdaten 605, 606 interpoliert (in 6 durch die durchgezogenen Linien 662 dargestellt). Zum Beispiel werden die Roh-Absolut-Positionsdaten 105 zu den Zeitpunkten t.2 und t.4 zum Erhalten der Absolut-Positionsdaten 605 zum Zeitpunkt t.3 interpoliert. Die Interpolation stellt also eine Schätzung dar, wo das entsprechende Positioniersystem zum Zeitpunkt t.3 das Kraftfahrzeug vermutet hätte. The knots 671 correspond to the absolute position data 605 . 606 (in 6 shown with dashed circles and squares). The absolute position data 605 . 606 form in particular boundary conditions for the nodes 671 , The absolute position data 605 . 606 are based on the raw absolute position data 105 . 106 certainly. This will be the raw absolute position data 105 . 106 obtained at sampling rates that differ from the clock rate; For example, raw absolute position data becomes 105 . 106 at the times t.2, t.4, t.4a, t.6, t.7, t.9, t.10 and t.12. The raw absolute position data 105 . 106 have appropriate timestamps. Therefore, the raw absolute position data 105 . 106 for obtaining the absolute position data 605 . 606 interpolated (in 6 through the solid lines 662 shown). For example, the raw absolute position data 105 at times t.2 and t.4 for obtaining the absolute position data 605 interpolated at time t.3. The interpolation thus represents an estimate of where the corresponding positioning system would have suspected the motor vehicle at time t.3.

Alle Zeitpunkte t.1–t.13 liegen innerhalb des Zeitfensters [t0–T; t0], wobei t0 die IST-Zeit darstellt. T bezeichnet eine Zeitdifferenz. Roh-Absolut-Positionsdaten 105, 106, die nicht innerhalb dieses Zeitfensters liegen, werden verworfen, d.h. nicht in den Posen-Graph 661 übernommen und z.B. aus dem Speicher gelöscht. Dadurch kann die Menge der Knoten 671 des Posen-Graphen 661 begrenzt werden; dadurch wird die zum Optimieren benötigte Zeitdauer reduziert. Gleichzeitig wird die jüngere Vergangenheit der Posen des Kraftfahrzeugs berücksichtigt, so dass die Zuverlässigkeit, mit der die IST-Position 190 abgeschätzt werden kann, vergleichsweise hoch ist. All times t1-t.13 lie within the time window [t0-T; t0], where t0 represents the actual time. T denotes a time difference. Crude absolute position data 105 . 106 that are not within this time window are discarded, ie not in the pose graph 661 taken over and deleted eg from the memory. This can reduce the amount of nodes 671 of the posse graph 661 be limited; This reduces the time required for optimization. At the same time, the recent past of the poses of the motor vehicle is taken into account, so that the reliability with which the actual position 190 can be estimated is relatively high.

In manchen Szenarien kann es vorkommen dass z.B. Odometrie-Positioniersysteme, die Ausgaben mit einer vergleichsweise sehr hohen Abtastrate bereitstellen, Messungen durchführen, die gegenüber dem Zeithorizont t0 in der Zukunft liegen. Dies kann aufgrund einer internen Vorhersage des Odometrie-Positioniersystems vorkommen. Es ist möglich, entsprechende Roh-Odometrie-Positionsdaten beim Erzeugen des Posen-Graphen 661 zu berücksichtigen. In some scenarios, it may happen that, for example, odometry positioning systems that provide outputs at a comparatively very high sampling rate perform measurements that are opposite to the time horizon t0 in the future. This may occur due to an internal prediction of the odometry positioning system. It is possible to obtain corresponding raw odometry position data when generating the pose graph 661 to take into account.

Ferner werden die Roh-Absolut-Positionsdaten 105, 106 basierend auf einer Datendichte der Roh-Absolut-Positionsdaten 105, 106 im Zeitraum und basierend auf der Taktrate selektiv verworfen. Zum Beispiel sind im Zeitbereich t.1–t.6 nur sehr wenige Roh-Absolut-Positionsdaten 106 vorhanden – insbesondere im Vergleich zur Taktrate. Deshalb wären denn die Roh-Absolut-Positionsdaten 106 zum Zeitpunkt t.4a verworfen und beim Erzeugen des Posen-Graphen 661 nicht als Randbedingung z.B. für die Knoten 671 zu den Zeitpunkten t.3 oder t.5 berücksichtigt. Further, the raw absolute position data become 105 . 106 based on a data density of the raw absolute position data 105 . 106 selectively discarded in the period and based on the clock rate. For example, in the time domain t.1-t.6, there are very few raw absolute position data 106 present - especially in comparison to the clock rate. That's why the raw absolute position data would be 106 discarded at time t.4a and when generating the pose graph 661 not as a boundary condition eg for the nodes 671 taken into account at times t.3 or t.5.

In anderen Beispielen können auch die Positionsdaten 106 zum Zeitpunkt t.4a zunächst im Speicher gepuffert verbleiben, aber nicht für das Erzeugen des Posen-Graphen 661 in der gegenwärtigen Iteration berücksichtigt werden. Das Löschen aus dem Speicher kann in einer späteren Iteration erfolgen, sofern keine nicht-zeitsequentiellen Positionsdaten 106 mehr erhalten werden. In other examples, the position data may also be 106 initially remain buffered in memory at time t.4a, but not for generating the pose graph 661 be considered in the current iteration. The erasure from the memory can be done in a later iteration, provided no non-time sequential position data 106 more will be obtained.

In verschiedenen weiteren Szenarien wäre es zum Beispiel möglich, dass das selektive Verwerfen der verschiedenen Roh-Absolut-Positionsdaten 105, 106 auf der vorgegebenen Optimierungstaktrate basiert; zum Beispiel könnte für höhere (niedrigere) Optimierungstaktrate und eine größere (kleinere) Anzahl von Roh-Absolut-Positionsdaten 105, 106 verworfen werden. In various other scenarios, for example, it would be possible to selectively discard the various raw absolute position data 105 . 106 based on the given optimization clock rate; For example, for higher (lower) optimization clock rate and a larger (smaller) number of raw absolute position data 105 . 106 be discarded.

Wieder Bezug nehmend auf 5, werden in Schritt 3 Odometrie-Positionsdaten ermittelt. Entsprechende Aspekte sind in Bezug auf 7 illustriert. In 7 sind die aus den Roh-Absolut-Positionsdaten 105, 106 ermittelten Knoten 671 dargestellt, die den Absolut-Positionsdaten 605, 606 entsprechen (wobei in 7 aus Gründen der Übersichtlichkeit die Randbedingungen nicht illustriert sind). Ferner sind in 7 Roh-Odometrie-Positionsdaten 115 dargestellt, die mit einer vergleichsweise hohen Abtastrate erhalten werden; die Abtastrate ist größer als die Taktrate. In verschiedenen beispielhaften Szenarien kann es erforderlich sein, die Roh-Odometrie-Positionsdaten 115 vor dem Einfügen in den Posen-Graph 661 durch eine Koordinaten-Rotation in das Koordinatensystem des Posen-Graph 661 zu überführen (in 7 nicht gezeigt). Die Roh-Odometrie-Positionsdaten 115 werden wiederum durch Interpolation in Odometrie-Positionsdaten 615, die den Kanten 672 des Posen-Graphen 661 entsprechen, überführt. Again referring to 5 , be in step 3 Odometry position data determined. Corresponding aspects are regarding 7 illustrated. In 7 are the ones from the raw absolute position data 105 . 106 determined node 671 representing the absolute position data 605 . 606 correspond to (in 7 for reasons of clarity, the boundary conditions are not illustrated). Furthermore, in 7 Raw odometry position data 115 shown obtained at a comparatively high sampling rate; the sampling rate is greater than the clock rate. In various example scenarios, it may be necessary to have the raw odometry position data 115 before pasting into the poses graph 661 by a coordinate rotation in the coordinate system of the pose graph 661 to convict (in 7 Not shown). The raw odometry position data 115 in turn, by interpolating into odometry position data 615 that the edges 672 of the posse graph 661 correspond, transferred.

Entsprechende Techniken, die obenstehend in Bezug auf das Verwerfen von Roh-Absolut-Positionsdaten 105, 106 offenbart sind, können auch auf die Roh-Odometrie-Positionsdaten 115, 116 angewendet werden. Corresponding techniques above with respect to rejecting raw absolute position data 105 . 106 can also be disclosed on the raw odometry position data 115 . 116 be applied.

Wieder Bezug nehmend auf 5, wird anschließend in Schritt 4 der Posen-Graph 661, d.h. die Knoten 671 und Kanten 672, auf Grundlage der in Schritten 2 und 3 erzeugten Positionsdaten 105, 106, 115, 116 erzeugt. Dazu werden die zu optimierenden Knoten 671 in gemäß der vorgegeben Taktrate bestimmten, zeitlich festen Abständen eingefügt. Again referring to 5 , will then step in 4 the pose graph 661 ie the nodes 671 and edges 672 , based on in steps 2 and 3 generated position data 105 . 106 . 115 . 116 generated. These are the nodes to be optimized 671 inserted in according to the predetermined clock rate certain, fixed time intervals.

In 8 ist der zu optimierende Posen-Graph 661 dargestellt; insbesondere ist in Bezug auf die Knoten 661 durch die Verbindung zwischen den schematisch illustrierten Absolut-Positionsdaten 605 und den Optimierungsknoten 671 der Aspekt der Randbedingung grafisch illustriert. Ziel der Optimierung ist es, unter Berücksichtigung der Randbedingungen eine entsprechende Fehlerfunktion global zu minimieren. Die Kanten 672 bilden auch Randbedingungen für die Optimierung der Knoten 671. Der Graph 661 wird zur Optimierung an das Backend 153 übergeben. In 8th is the pose graph to be optimized 661 shown; in particular, in terms of nodes 661 by the connection between the schematically illustrated absolute position data 605 and the optimization node 671 the aspect of the boundary condition graphically illustrated. The aim of the optimization is to globally minimize a corresponding error function taking into account the boundary conditions. The edges 672 also form boundary conditions for the optimization of the nodes 671 , The graph 661 will be sent to the backend for optimization 153 to hand over.

Wieder Bezug nehmend auf 5: anschließend wird in Schritt 5 der Posen-Graph 661 optimiert. Dann kann angenommen werden, dass der optimierte Posen-Graph 661 einen bestmöglichen Kompromiss aus den verschiedenen Positionsdaten 105, 106, 115, 116 der zu fusionierenden Positioniersysteme 101, 102, 111–113 darstellt. Again referring to 5 : then in step 5 the pose graph 661 optimized. Then it can be assumed that the optimized poses graph 661 a best possible compromise from the different position data 105 . 106 . 115 . 116 the positioning systems to be merged 101 . 102 . 111 - 113 represents.

In 9 sind Aspekte des optimierten Posen-Graph 661 dargestellt. In 9 sind die nunmehr optimierten Knoten 671 dargestellt, sowie die abgeschätzt IST-Position 190 des Kraftfahrzeugs. Aus 9 ist ersichtlich, dass die IST-Position 190 für den zukünftigen Zeitpunkt t.13 vorhergesagt wird (in 9 durch die gepunktete Linie illustriert). Dadurch kann eine Latenzzeit für das Optimieren des Posen-Graphen 661 kompensiert werden. Durch die Vorhersage der IST-Position 190 kann erreicht werden, dass zu Beginn einer nächsten Iteration gemäß Schritt 1 der 5 bereits eine aktuelle Abschätzung der Position 190 vorliegt. In 9 are aspects of the optimized pose graph 661 shown. In 9 are the now optimized nodes 671 shown, as well as the estimated actual position 190 of the motor vehicle. Out 9 it can be seen that the actual position 190 for the future time t.13 is predicted (in 9 illustrated by the dotted line). This can provide a latency for optimizing the pose graph 661 be compensated. By predicting the actual position 190 can be achieved at the beginning of a next iteration according to step 1 of the 5 already an up to date estimate of the position 190 is present.

Zur Vorhersage der IST-Position 190 kann in verschiedenen Beispielen eine Interpolation der letzten beiden optimierten Knoten 671 erfolgen. Daraus kann eine Eigenbewegung des Kraftfahrzeugs bestimmt werden und optional ausgegeben werden. Unter der Annahme, dass sich das Kraftfahrzeug in naher Zukunft genauso verhält, wie in der letzten bekannten Vergangenheit, wird diese Eigenbewegung an den jüngsten optimierten Knoten 671 per Vektoraddition angehangen. To predict the actual position 190 In various examples, an interpolation of the last two optimized nodes 671 respectively. From this, a proper movement of the motor vehicle can be determined and optionally output. Assuming that the motor vehicle behaves in the near future in the same way as in the last known past, this self-motion will be at the youngest optimized node 671 attached by vector addition.

Die IST-Position 190 wird mit einer bestimmten Genauigkeit bestimmt, welche invers proportional zu einer Zuverlässigkeit 191 (in 9 mittels der Fehlerbalken illustriert) ist. Die Zuverlässigkeit 191 kann auch zum Steuern der Fahrerassistenzfunktionalität 129 berücksichtigt werden. The actual position 190 is determined with a certain accuracy, which is inversely proportional to a reliability 191 (in 9 illustrated by the error bar). The reliability 191 can also control the driver assistance functionality 129 be taken into account.

10 illustriert einen Posen-Graphen 661, der mittels der voranstehend beschriebenen Techniken bestimmt werden kann. Der Posen-Graph 661 weist mehrere Knoten 671 auf, die in einem festen zeitlichen Abstand zueinander an den Zeitpunkten t.1, t.2 und t.3 angeordnet sind. 10 illustrates a posse graph 661 which can be determined by the techniques described above. The pose graph 661 has several nodes 671 on, which are arranged at a fixed time interval to each other at the times t.1, t.2 and t.3.

Den verschiedenen Knoten 671 sind jeweils Absolut-Positionsdaten 605, 606 zugeordnet; diese Zuordnung kann in Form von Randbedingungen erfolgen (in 10 durch die vertikalen Kanten des Posen-Graphen 661 dargestellt). Die Absolut-Positionsdaten 605, 606 entsprechen dabei Fixknoten, da ihre Position im Rahmen der Optimierung des Posen-Graphen 661 nicht verändert wird. Dabei können Techniken der Interpolation angewendet werden; Roh-Absolut-Positionsdaten 105, 106 können auf die Absolut-Positionsdaten 605, 606 abgebildet werden, die an den Zeitpunkten t.1, t.2 und t.3 angeordnet sind, die den zu optimierenden Knoten 671 entsprechen. Anschließend können die Kanten 672 des Posen-Graphen 661 erzeugt werden, z.B. basierend auf den Odometrie-Positionsdaten 615. The different nodes 671 are each absolute position data 605 . 606 assigned; this assignment can take the form of boundary conditions (in 10 through the vertical edges of the posse graph 661 shown). The absolute position data 605 . 606 Fix nodes correspond to their position as part of the optimization of the pose graph 661 not changed. Techniques of interpolation can be used; Crude absolute position data 105 . 106 can be on the absolute position data 605 . 606 mapped, which are arranged at the times t.1, t.2 and t.3, the node to be optimized 671 correspond. Subsequently, the edges can 672 of the posse graph 661 generated, eg based on the odometry position data 615 ,

11 illustriert Aspekte in Bezug auf eine Systemmatrix 901. Die Systemmatrix 901, H ist die diejenige Matrix, die während der Optimierung invertiert wird, zum Beispiel mittels eines Gauss-Newton-Verfahrens oder eines Levenberg-Marquardt-Verfahrens. Dies kann dem Lösen folgender Gleichung entsprechen: H∆x = –b (1) wobei ∆x den Aktualisierungsvektor und b den Koeffizientenvektor bezeichnet. Typischerweise wird die Lösung der Gleichung 1 durch Invertieren schneller, wenn die Systemmatrix 901 dünn besetzt ist. 11 illustrates aspects related to a system matrix 901 , The system matrix 901 , H is the one matrix which is inverted during optimization, for example by means of a Gauss-Newton method or a Levenberg-Marquardt method. This may correspond to solving the following equation: HΔx = -b (1) where Δx denotes the update vector and b the coefficient vector. Typically, the solution of Equation 1 becomes faster by inverting when the system matrix 901 thin is occupied.

Die Systemmatrix 901 umfasst Diagonaleinträge 902, welche von den Absolut-Positionsdaten 605, 606 und den Odometrie-Positionsdaten 615 jeweils als Randbedingungen beeinflusst werden. Die Nicht-Diagonaleinträge 903 werden nur von den Odometrie-Positionsdaten 615 als Randbedingungen beeinflusst. The system matrix 901 includes diagonal entries 902 which of the absolute position data 605 . 606 and the odometry position data 615 each be influenced as boundary conditions. The non-diagonal entries 903 are only from the odometry position data 615 influenced as boundary conditions.

Sofern neue Knoten 671 jeweils dann eingefügt werden, wenn neue Absolut-Positionsdaten 605, 606 und/oder neue Odometrie-Positionsdaten 615 verfügbar sind, resultiert eine verzweigter Struktur des Posen-Graphen 661 und damit auch eine verzweigter Struktur der Systemmatrix 901. Deshalb ist es in verschiedenen Ausführungsformen erstrebenswert, von Zeit zu Zeit eine Marginalisierung des Posen-Graphen 661 durchzuführen. Unless new nodes 671 are inserted whenever new absolute position data 605 . 606 and / or new odometry position data 615 are available results in a branched structure of the pose graph 661 and thus also a branched structure of the system matrix 901 , Therefore, it is desirable in various embodiments, from time to time, to marginalize the posse graph 661 perform.

Im Folgenden werden Techniken zur Marginalisierung im Detail beschrieben. Es wird ferner dargelegt, dass ein Fixknoten als Ersatz für einen ausgewählten bisherigen Knoten verwendet werden kann. Der Fixknoten kann die gleiche Information beinhalten wie das Schur-Komplement. Gleichzeitig kann ein solcher Fixknoten den Vorteil aufweisen, dass dieser Fixknoten explizit im Design des Posen-Graphen 661 wieder gefunden werden kann und dadurch gezielt manipuliert werden kann. In the following, techniques for marginalization are described in detail. It is further stated that a fixed node may be used as a replacement for a selected previous node. The fix node may contain the same information as the Schur complement. At the same time, such a fixed node can have the advantage that this fixed node is explicitly in the design of the pose graph 661 can be found again and thus can be manipulated specifically.

Herkömmlicherweise wird das Schur-Komplement auf die Gleichung 1 angewendet, um Zustandsvariablen aus dem Optimierungsproblem zu entfernen. Im Posen-Graphen 661 entspricht diese Operation dem Entfernen von Knoten 671. Durch das Schur-Komplement ist sichergestellt, dass vor und nach der Anwendung die restlichen Knoten 671 den gleichen Schätzwert behalten. Würde man die Zustandsvariablen einfach löschen, hätte man einen anderen Schätzwert; dies bedeutet, dass das einfache Entfernen eines Knoten 671 die Abschätzung für die Position des Fahrzeugs verfälschen würde. Der Zustandsvektor besteht in einem Zeitfenster-Verfahren aus m Zustandsvariablen. Es wird nachfolgend gezeigt, wie man einen einzelnen Knoten 671/eine einzelne Zustandsvariable marginalisieren kann. Sofern mehrere Knoten 671/Zustandsvariablen marginalisiert werden sollen, ist es möglich, solche Techniken mehrfach hintereinander anzuwenden. Conventionally, the Schur complement is applied to Equation 1 to remove state variables from the optimization problem. In the Poznań graphene 661 This operation is equivalent to removing nodes 671 , The Schur complement ensures that before and after the application, the remaining nodes 671 keep the same estimate. If you simply delete the state variables, you would have a different estimate; This means that simply removing a node 671 would distort the estimate for the position of the vehicle. The state vector consists of m state variables in a time window method. It will be shown below how to make a single node 671 / can marginalize a single state variable. If several nodes 671 / State variables are marginalized, it is possible to apply such techniques several times in succession.

Nachfolgend wird eine beispielhafte Herleitung einer Möglichkeit, den Fixknoten zu bestimmen, beschrieben. Diese ist rein beispielhaft. Es sind auch andere Techniken zum Bestimmen des Fixknotens möglich. Es ist nützlich, für die Herleitung zunächst mit einem Posen-Graphen 661 zu beginnen, der lediglich zwei Knoten 671 und mehrere Randbedingungen aufgrund von Odometrie-Positionsdaten 615 und Absolut-Positionsdaten 605, 606 aufweist. Eine solche Situation ist in 12 dargestellt. In dem Beispiel der 12 wird ein Knoten 971 (zum Beispiel in 13 mit x_a bezeichnet) ignoriert, vergleiche 12 und 13. Dies wird getan, um zu sehen, wie die verschiedenen Knoten 671, 971 sich auf die Systemmatrix 901 auswirken. Im resultierenden Posen-Graphen 661 G^small für 12, in dem die übrigen Knoten 671, x_b, x_c miteinander verbunden sind und die zusätzliche Randbedingungen 605, 606, 615 aufweisen, ist die zugehörige Optimierungsgleichung:

An exemplary derivation of a way to determine the fix node will now be described. This is purely exemplary. Other techniques for determining the fixed node are also possible. It is useful for derivation first with a posse graph 661 to start, the only two nodes 671 and several constraints due to odometry position data 615 and absolute position data 605 . 606 having. Such a situation is in 12 shown. In the example of 12 becomes a node 971 (for example in 13 ignored by x _a ), compare 12 and 13 , This is done to see how the different nodes 671 . 971 on the system matrix 901 impact. In the resulting posse graph 661 G ^small for 12 in which the remaining nodes 671 , x _b , x _c are interconnected and the additional constraints 605 . 606 . 615 , the corresponding optimization equation is:

Wenn der zusätzliche Knoten 971, x_a berücksichtigt wird, verändert sich die Struktur des Posen-Graphen (vergleiche 13), als auch die Struktur der Systemmatrix 901. Sei zusätzlich x_a mit mehreren globalen Fixknoten x _i verbunden, die Absolut-Positionsdaten 605 entsprechen, die die Informationsmatrizen Ω _i besitzen, sodass Ω = Σ _i Ω _i If the additional node 971 , x _{a is} taken into account, the structure of the posse graph changes (cf. 13 ), as well as the structure of the system matrix 901 , In addition, let x _{a be} multiple global fix nodes x _i connected, the absolute position data 605 match the information matrices Ω _i own, so Ω = Σ _i Ω _i

Außerdem soll es eine oder mehrere Kanten 672 zwischen x_a und x_b mit den Informationsmatrizen Ω ^ j / a,b geben, so dass Ω ^ = Σ_jΩ ^ j / a,b . Der resultierende Posen-Graph 661 G^full für das Beispiel der 13 wird definiert durch die Systemmatrix 901:

und dem Koeffizientenvektor

wobei

e – / i,a

der Fehler ist zwischen den Fixknoten

x _i

für die Absolut-Positionsdaten 605, 606 und dem zu optimierenden Knoten 971 x_a. Der Einfachheit halber werden alle Randbedingungen aufgrund von Odometrie-Positionsdaten 615 abgekürzt mit

b^rel = Σ_jΩ ^ j / a,be_a,b

It should also have one or more edges 672 between x _a and x _b with the information matrices

Ω ^ j / a, b

give, so that

Ω ^ = Σ _j Ω ^ j / a, b

, The resulting posse graph 661 G ^full for the example of 13 is defined by the system matrix 901 :

and the coefficient vector

in which

e - / i, a

the error is between the fix nodes

x _i

for the absolute position data 605 . 606 and the node to be optimized 971 x _a . For the sake of simplicity, all constraints will be due to odometry position data 615 abbreviated with

b ^rel = Σ _j Ω ^ j / a, be _{a, b}

Der herkömmliche Weg, um den Knoten 971, x_a „weg zu marginalisieren“, ist es, das Schur-Komplement auf H^full und b^full zu berechnen. Dies führt zu der marginalisierten Systemmatrix 901 H^marg, welche identisch ist zu H^small außer dass der linke obere Block sich ändert zu H marg / bb = H small / bb + Ω ^ – Ω ^(Ω + Ω ^)^–1Ω ^ (5) The conventional way to the node 971 To "marginalize" x _a is to compute the Schur complement to H ^full and b ^full . This leads to the marginalized system matrix 901 H ^marg , which is identical to H ^small except that the upper left block changes too H marg / bb = H small / bb + Ω ^ - Ω ^ ( Ω + Ω ^) ^-1 Ω ^ (5)

Der dazugehörige marginalisierte Koeffizientenvektor ist gegeben durch

The associated marginalized coefficient vector is given by

Das bisherige Teilergebnis gibt vor, wie die Systemmatrix 901 nach der Marginalisierung aussehen kann. Dies wird benutzt, um den Fixknoten 972 (cf. 14) herzuleiten. The previous partial result specifies how the system matrix 901 after the marginalization can look like. This is used to fix the node 972 (Cf. 14 ) derive.

In einem solchen Fall kann der Fixknoten 972 das Schur-Komplement 972 des Knotens 971 darstellen. In such a case, the fix node can 972 the Schur complement 972 of the node 971 represent.

Nachfolgend wird eine Gleichung hergeleitet, mit der die Pose und die Unsicherheit des Fixknotens 972, x _p berechnet werden kann, um ihn in den Posen-Graphen 661 anstelle des Knotens 971, x_a einzufügen. Wenn entweder Knoten 971, x_a nicht mit mindestens einem Fixknoten, der Absolut-Positionsdaten 605, 606 entspricht, oder wenn Knoten 971, x_a nicht über eine Kante 672 mit dem benachbarten Knoten 671, x_b verbunden ist, beeinflusst Knoten 971, x_a den Knoten 671, x_b nicht. In diesem Fall muss kein Fixknoten 972 berechnet werden, da auch die Marginalisierung äquivalent dazu ist, dass der Knoten 971 einfach gelöscht wird. Im Allgemeinen Fall jedoch ist dies nicht der Fall. The following is an equation used to derive the pose and uncertainty of the fixed node 972 . x _p can be calculated to put it in the posse graph 661 instead of the node 971 to insert x _a . If either node 971 , x _a does not have at least one fixed node, the absolute position data 605 . 606 corresponds, or if node 971 , x _a does not have an edge 672 with the neighboring node 671 , x _{b is} connected, affects nodes 971 , x _a the node 671 , x _b not. In this case, no fixed node 972 since the marginalization is equivalent to that of the node 971 simply deleted. In the general case, however, this is not the case.

Als erstes wird der Knoten 971, x_a und alle zugehörigen Fixknoten, die Absolut-Positionsdaten 605, 606 entsprechen, und alle Kanten 672 zwischen dem Knoten 971, x_a und dem Knoten 671, x_b, entfernt. Wird nun der Fixknoten 972, x _p mit der Informationsmatrix Ω _p zu dem Knoten 671, x_b, verbunden, wird der Graph G^prior erhalten (siehe 14). Der vorangestellte Fixknoten 972, x_p führt zu einem zusätzlichen Summanden in der Systemmatrix H^prior von G^prior:

First, the node 971 , x _a and all associated fix nodes, the absolute position data 605 . 606 match, and all edges 672 between the node 971 , x _a and the node 671 , x _b , removed. Will now be the fix node 972 .

x _p

with the information matrix

Ω _p

to the node 671 , x _b , the graph G ^{prior is} obtained (see 14 ). The preceding fixed node 972 , x _p leads to an additional addend in the system matrix H ^prior to G ^prior :

Nun wid gezeigt, dass die Wahl von Ω –1 / p = Ω ^^–1 + Ω ^–1 (8) äquivalent ist zum Anbringen des Schur-Komplements bezüglich der resultierenden Unsicherheit. Es wird Äquivalenz zwischen den oberen linken Blöcken von H^marg und H^prior gefordert:

Well wid shown that the choice of

Ω -1 / p = Ω ^ ^-1 + Ω ^-1 (8)

Equivalent to attaching the Schur complement to the resulting uncertainty. Equivalence is required between the upper left blocks of H ^marg and H ^prior :

Diese Herleitung zeigt, dass die obige Wahl für Ω _p unsere Anforderungen erfüllt. Die nächste Frage ist, mit welcher Posenschätzung bzw. Position der Fixknoten 972, x _p initialisiert wird. This derivation shows that the above choice for Ω _p meets our requirements. The next question is with which pose estimation or position of the fixed node 972 . x _p is initialized.

Nun wird die Position x _p des Fixknotens 972, x _p so festlegt, dass alle Knoten 671 nach der Marginalisierung noch auf die selben Positionen wie vorher geschätzt werden. Dafür wird Gleichheit zwischen b^marg und b^prior gefordert. Eine genauere Analyse von b^marg zeigt

Now the position

x _p

of the fixed node 972 .

x _p

so fix that all nodes 671 after the marginalization are still estimated to the same positions as before. For this, equality is required between b ^marg and b ^prior . A more detailed analysis of b ^marg shows

Das Hinzufügen eines Fixknotens 972, x _p mit der Posenschätzung x _p und Unsicherheit Ω –1 / p = Ω ^–1 + Ω ^^–1 resultiert in

The addition of a fixed node 972 .

x _p

with the pose estimate

x _p

and uncertainty

Ω -1 / p = Ω ^-1 + Ω ^ ^-1

results in

Fordern von Gleichheit zwischen b^marg und b^prior lasst uns nach x _p auflösen: x _p = x_b – Ω –1 / p[b marg / b – b small / b]. (11) Demanding equality between b ^marg and b ^prior let us go x _p dissolve: x _p = x _b - Ω -1 / p [b marg / b - b small / b]. (11)

Dies ist die analytische Lösung für x _p . Damit kann der Fixknoten 972, x _p konstruiert werden, nämlich seine Unsicherheit und seine Position. Das Anfügen des Fixknotens 972 mit dieser Position und Unsicherheit an den Posen-Graphen 661 führt zu den gleichen Effekten wie das Anwenden der Marginalisierung mit Hilfe des Schur-Komplements. This is the analytical solution for x _p , This allows the fix node 972 . x _p its uncertainty and position. The attachment of the fixed node 972 with this position and uncertainty on the posse graph 661 leads to the same effects as applying the marginalization using the Schur complement.

Die nach Gleichung 11 berechnete Position des Fixknotens 972 kann im Allgemeinen abweichen von der Position des gelöschten Knotens 971. The position of the fixed node calculated according to Equation 11 972 may generally differ from the location of the deleted node 971 ,

Da der Fixknoten 972 transparent in dem Posen-Graphen 661 hinterlegt ist, kann es möglich sein, vor dem erneuten Durchführen einer Optimierung verschiedener Eigenschaften des Fixknotens 972 zu verändern. Zum Beispiel wäre es möglich, die Verlustfunktion der Kante 973 und/oder die Unsicherheit des Fixknotens 972 anzupassen. Beispiele für Verlustfunktionen umfassen: Huber-Loss-Funktion und m-Estimator. Eine derartige Anpassung kann es ermöglichen, besonders genaue Ergebnisse der Sensorfusion zu erzielen. Zum Beispiel kann die Optimierung den Fixknoten 972 umso stärker berücksichtigen, je geringer dessen Unsicherheit ist. Generell kann das Ortungsresultat nach Optimierung des Fixknotens 972 näher an solchen Quellen liegen, die eine geringere Unsicherheit aufweisen. Die Verlustfunktion ist eine Technik, die es ermöglicht, die Unsicherheit zu verändern. Basierend darauf, wie gut bestimmte Positionsdaten zu weiteren Positionsdaten passen, kann die Unsicherheit skaliert werden. Because the fixed node 972 transparent in the posse graph 661 It may be possible to re-tune various properties of the fixed node before repeating it 972 to change. For example, it would be possible to use the loss function of the edge 973 and / or the uncertainty of the fixed node 972 adapt. Examples of loss functions include: Huber loss function and m-estimator. Such an adaptation can make it possible to obtain particularly accurate results of the sensor fusion. For example, the optimization may be the fix node 972 take into account the more the uncertainty is lower. In general, the location result after optimization of the fixed node 972 closer to sources with less uncertainty. The loss function is a technique that makes it possible to change the uncertainty. The uncertainty can be scaled based on how well certain position data matches other positional data.

Eine Verlustfunktion ist lediglich eine beispielhafte Möglichkeit, um die Unsicherheit zu skalieren. Weitere Techniken, um dies Unsicherheit zu bestimmen, umfassen zum Beispiel Karten-basierte Techniken: dabei kann das Vorwissen über eine Position, das durch den Fixknoten 972 ausgedrückt wird, in eine Straßenkarte eingezeichnet werden. Es kann dann analysiert werden, wie gut dieses Vorwissen zu der Karte passt. Liegt der Fixknoten 972 beispielsweise neben der Straße, zum Beispiel in einem See, dann wäre die Unsicherheit dieses Fixknotens 972 stark erhöht. Dies ist der Fall, da die entsprechende Position unplausibel ist. Anders wäre die Unsicherheit einzuschätzen, wenn die Position des Fixknotens 972 auf einem Fahrstreifen liegt und dieser Fahrstreifen einer Straße zugeordnet ist, die in der Vergangenheit befahren wurde. Dann wäre zum Beispiel die Unsicherheit nicht zu erhöhen. Es ist also möglich, die Unsicherheit unter Berücksichtigung von Expertenwissen und/oder Kontextwissen anzupassen. A loss function is just one example of how to scale the uncertainty. Other techniques used to determine this uncertainty include, for example, map-based techniques: this may involve prior knowledge of a position taken by the fixed node 972 expressed in a road map. It can then be analyzed how well this prior knowledge fits the card. Lies the fixed node 972 For example, next to the road, for example in a lake, then the uncertainty of this fixed node would be 972 greatly increased. This is the case because the corresponding position is implausible. The uncertainty would be different if the position of the fixed node 972 is on a lane and this lane is associated with a road that was traveled in the past. Then, for example, the uncertainty would not increase. It is thus possible to adjust the uncertainty taking into account expert knowledge and / or contextual knowledge.

Aus Obenstehendem ist ersichtlich, dass der Posen-Graph 661 in Bezug auf den Knoten 971 durch Bestimmen des Fixknotens 972 marginalisiert werden kann. In dem Beispiel, welches in Bezug auf die 12–14 beschrieben wurde, wurde dabei der Knoten 971 ausgewählt, der dem ältesten Zeitpunkt t.1 entspricht. In anderen Fällen könnte jedoch auch ein anderer Knoten 671, 971 ausgewählt werden. From the above, it can be seen that the pose graph 661 in relation to the node 971 by determining the fixed node 972 can be marginalized. In the example, which in relation to the 12 - 14 was described, became the node 971 selected, which corresponds to the oldest time t.1. In other cases, however, could also be another node 671 . 971 to be selected.

Dabei können die Techniken, die obenstehend mit Bezug auf die 12–14 beschrieben wurden, wiederholt nacheinander ausgeführt werden, um weitere Knoten 671 zu entfernen. Zum Beispiel ist in Bezug auf 15 ein Szenario dargestellt, bei dem der Knoten 671, x_b ausgewählt und entfernt wurde und dafür ein weiterer Fixknoten 972 zum Posen-Graphen 661 hinzugefügt wurde. The techniques described above with reference to the 12 - 14 have been repeatedly executed consecutively to further nodes 671 to remove. For example, in terms of 15 presented a scenario in which the node 671 , x _{b was} selected and removed, and another fixed node 972 to the posse graph 661 was added.

16 ist ein Flussdiagramm eines Verfahrens gemäß verschiedener Ausführungsformen. Zunächst erfolgt in Schritt 1001 das Erzeugen eines Posen-Graphen 661. Insbesondere kann in Schritt 1001 ein Posen-Graph 661 mit Ketten-Struktur erzeugt werden, wie oben stehend beschrieben. 16 FIG. 10 is a flowchart of a method according to various embodiments. FIG. First, in step 1001 generating a pose graph 661 , In particular, in step 1001 a pose graph 661 be produced with chain structure, as described above.

Dann erfolgt in Schritt 1002 das Optimieren des Posen-Graphen 661. Dabei kann die Position der Knoten 671 verändert werden. Das Optimieren berücksichtigt Randbedingungen, die durch die Kanten 672 bzw. die Fixknoten, die den Absolut-Positionsdaten 605, 606 entsprechen. Beim Optimieren wird die Pose von Fixknoten nicht verändert. Then done in step 1002 optimizing the pose graph 661 , In doing so, the position of the nodes 671 to be changed. Optimizing takes into account boundary conditions caused by the edges 672 or the fixed nodes, which are the absolute position data 605 . 606 correspond. Optimizing does not change the pose of fix nodes.

Anschließend wird in Schritt 1003 überprüft, ob zwischenzeitlich weitere Positionsdaten 605, 606, 615 verfügbar geworden sind. Dabei kann zum Beispiel eine zwischenzeitliche Abtastrate, mit der die Position ihrer Systeme 101, 102, 111, 112 Roh-Positionsdaten 105, 106, 115, 116 bereitstellen, berücksichtigt werden. Subsequently, in step 1003 checks, if in the meantime further position data 605 . 606 . 615 become available. It can, for example, an intermediate sampling rate, with the position of their systems 101 . 102 . 111 . 112 Raw position data 105 . 106 . 115 . 116 be considered.

Sind neue Positionsdaten 605, 606, 615 verfügbar, wird ein neuer Knoten 671 und/oder eine neue Kante 672 zu dem Posen-Graphen 661 hinzugefügt, siehe Schritt 1004. Are new position data 605 . 606 . 615 available, becomes a new node 671 and / or a new edge 672 to the posse graph 661 added, see step 1004 ,

Dann wird in Schritt 1005 überprüft, ob eine Marginalisierung des Posen-Graphen 661 durchgeführt werden soll. Zum Beispiel kann in Schritt 1005 eine Anzahl der Knoten 671 des Posen-Graphen 661 berücksichtigt werden. Überschreitet zum Beispiel die Anzahl einen vorgegebenen Schwellenwert, so kann die Marginalisierung durchgeführt werden. Then in step 1005 checked for a marginalization of the posse graph 661 to be carried out. For example, in step 1005 a number of nodes 671 of the posse graph 661 be taken into account. For example, if the number exceeds a predetermined threshold, the marginalization may be performed.

Wird in Schritt 1005 bestimmt, dass die Marginalisierung nicht durchgeführt werden soll, so werden Schritte 1002–1004 erneut durchgeführt. Andernfalls wird Schritt 1006 durchgeführt. In Schritt 1006 wird der Fixknoten 972 für einen ausgewählten Knoten 971 bestimmt und der ausgewählte Knoten 971 wird aus dem Posen-Graphen 661 entfernt. Der Fixknoten 972 wird dem Posen-Graphen 661 hinzugefügt. Dann wird das Optimieren in Schritt 1002 erneut durchgeführt, wobei die Pose des Fixknotens 972 im Rahmen der Optimierung nicht verändert wird. Will in step 1005 determines that the marginalization should not be performed, so will be steps 1002 - 1004 carried out again. Otherwise, step will be 1006 carried out. In step 1006 becomes the fix node 972 for a selected node 971 determined and the selected node 971 becomes from the posse graph 661 away. The fixed node 972 becomes the posse graph 661 added. Then the optimization will be in step 1002 performed again, with the pose of the fixed node 972 as part of the optimization is not changed.

Der Fixknoten 972 kann z.B. basierend auf Elementen bestimmt werden, die aus folgender Gruppe ausgewählt sind: die Position des ausgewählten Knotens 971; mindestens eine Kante 672, die mit dem ausgewählten Knoten verbunden ist; Absolut-Positionsdaten 605, 606, die dem ausgewählten Knoten 971 entsprechen und; Optimierungs-Knoten, mit denen der ausgewählte Knoten 971 direkt verbunden ist (sh. Gleichung 11). Gl. 11 enthält die Terme b marg / b und b small / b . Der erstere wiederum beinhaltet b^rel, der aus den Nebenbedingungen der Odometriekanten, die mit dem zu marginalisierenden Knoten verbunden sind, gewonnen wurden. Außerdem enthält er b full / a , der aus den Fixknoten und den Odometriekanten, die mit dem zu entfernenden Knoten verbunden sind, gewonnen wurde. Es wäre auch möglich, dass der Fixknoten 972 basierend auf einem weiteren Fixknoten, der über eine Kante 973 mit dem ausgewählten Knoten 971 verbunden ist, bestimmt wird (vgl. 14 und 15). The fixed node 972 can be determined, for example, based on elements selected from the following group: the position of the selected node 971 ; at least one edge 672 which is connected to the selected node; Absolute position data 605 . 606 that the selected node 971 correspond and; Optimization Node with which the selected node 971 directly connected (see Equation 11). Eq. 11 contains the terms b marg / b and b small / b , The former, in turn, contains b ^rel , obtained from the constraints of the odometry edges associated with the node to be marginalized. He also contains b full / a which was obtained from the fix nodes and the odometry edges associated with the node to be removed. It would also be possible for the fix node 972 based on another fix node, over an edge 973 with the selected node 971 is determined (cf. 14 and 15 ).

Selbstverständlich können die Merkmale der vorab beschriebenen Ausführungsformen und Aspekte der Erfindung miteinander kombiniert werden. Insbesondere können die Merkmale nicht nur in den beschriebenen Kombinationen, sondern auch in anderen Kombinationen oder für sich genommen verwendet werden, ohne das Gebiet der Erfindung zu verlassen. Of course, the features of the previously described embodiments and aspects of the invention may be combined. In particular, the features may be used not only in the described combinations but also in other combinations or per se, without departing from the scope of the invention.

Z.B. wurden voranstehend verschiedenste Beispiele in Bezug auf das Erhalten der geschätzten Position aus dem Optimieren des Posen-Graphen beschrieben. Alternativ oder zusätzlich kann es möglich sein, eine geschätzte Orientierung bzw. Odometrie des Kraftfahrzeugs aus dem Optimieren des Posen-Graphen zu erhalten und z.B. im Rahmen des Steuerns der Fahrerassistenzfunktionalität zu berücksichtigen. For example, above, various examples relating to obtaining the estimated position from optimizing the pose graph have been described. Alternatively or additionally, it may be possible to obtain an estimated orientation or odometry of the motor vehicle from the optimization of the pose graph and to take it into account, for example, in the context of controlling the driver assistance functionality.

Z.B. wurden voranstehend verschiedenste Beispiele in Bezug auf das Steuern einer Fahrerassistenzfunktionalität für autonomes Fahren beschrieben. Alternativ oder zusätzlich können auch andere Fahrerassistenzfunktionalitäten gesteuert werden, z.B. teil- oder hochautomatisierte Fahrtlösungen, etc. For example, In the foregoing, various examples have been described in terms of controlling a driver assistance functionality for autonomous driving. Alternatively or additionally, other driver assistance functions may also be controlled, e.g. partially or highly automated travel solutions, etc.

BezugszeichenlisteLIST OF REFERENCE NUMBERS

t.1–t.13 t.1-T.13: Zeitpunkt time
1–5 1-5: Schritt step
101 101: Absolut-Positioniersystem Absolute positioning
102 102: Absolut-Positioniersystem Absolute positioning
111 111: Odometrie-Positioniersystem Odometry positioning system
112 112: Odometrie-Positioniersystem Odometry positioning system
113 113: Odometrie-Positioniersystem Odometry positioning system
121 121: Schnittstelle interface
122 122: Schnittstelle interface
123 123: Prozessor processor
124 124: Speicher Storage
129 129: Fahrerassistenz driver assistance
151 151: Ortungsfusion locating merger
152 152: Graphenverwaltung graphs management
153 153: Backend, Optimierung Backend, optimization
190 190: Ist-Position Actual position
191 191: Zuverlässigkeit reliability
201 201: Zeitintervall time interval
202 202: Abtastrate sampling rate
203 203: Unterbrechung interruption
105, 106 105, 106: Roh-Absolut-Positionsdaten Crude absolute position data
115, 116 115, 116: Roh-Odometrie-Positionsdaten Raw odometry position data
615 615: Odometrie-Positionsdaten Odometry position data
661 661: Posen-Graph Posen-Graph
662 662: Interpolation interpolation
671 671: Knoten node
672 672: Kanten edge
605, 606 605, 606: Absolut-Positionsdaten Absolute position data
901 901: Systemmatrix matrix
902, 903 902, 903: Einträge der Systemmatrix Entries of the system matrix
971 971: Ausgewählter Knoten Selected node
972 972: Fixknoten Fixknoten
973 973: Kante edge
1001–1006 1001-1006: Schritt step

ZITATE ENTHALTEN IN DER BESCHREIBUNG QUOTES INCLUDE IN THE DESCRIPTION

Diese Liste der vom Anmelder aufgeführten Dokumente wurde automatisiert erzeugt und ist ausschließlich zur besseren Information des Lesers aufgenommen. Die Liste ist nicht Bestandteil der deutschen Patent- bzw. Gebrauchsmusteranmeldung. Das DPMA übernimmt keinerlei Haftung für etwaige Fehler oder Auslassungen.This list of the documents listed by the applicant has been generated automatically and is included solely for the better information of the reader. The list is not part of the German patent or utility model application. The DPMA assumes no liability for any errors or omissions.

Zitierte PatentliteraturCited patent literature

DE 102013208521 A1 [0002]
DE 102014211178 A1 [0008]
DE 102015219577 [0009]

Zitierte Nicht-PatentliteraturCited non-patent literature

Sibley G. et al., J. Field Robotics 27 (2010) 587-608 [0011]
Kümmerle, R. et al., G2o2o: A General Framework for Graph Optimization, in Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), 2011 [0016]

Claims

Method of marginalizing a pose graph ( 661 ), comprising: - for each of a plurality of times (t.1-t.12): respectively determining odometry position data ( 615 ) of a mobile machine based on an output of at least one odometry positioning system ( 111 . 112 ), - for each of the several times (t.1-t.12): in each case determination of absolute position data ( 605 . 606 ) of the machine based on an output of at least one absolute positioning system ( 101 . 102 ), - Produce ( 152 ) of the posse graph ( 661 ) with edges ( 672 ), the odometry position data ( 615 ) and with nodes ( 671 . 971 ), the absolute position data ( 605 . 606 ), - Optimize ( 153 . 1002 ) of the posse graph ( 661 ) to obtain an estimated position ( 190 ) of the machine, characterized in that the method further comprises: - after optimizing ( 153 . 1002 ): Selecting a node ( 971 ) of the posse graph ( 661 ), - for the selected node ( 971 ): Determine a Fixed Node ( 972 ) of the posse graph ( 661 ), - remove the selected node ( 971 ) from the pose graph ( 661 ).

Method according to claim 1, characterized in that the fixed node ( 972 ) is determined based on elements selected from the group: the position of the selected node ( 971 ); at least one edge ( 672 ) with the selected node ( 971 ) connected is; Absolute position data ( 605 . 606 ) to the selected node ( 971 ) correspond; and the location of another fixed node ( 972 ), which over an edge ( 973 ) with the selected node ( 971 ) connected is.

Method according to claim 1 or 2, characterized in that the fixed node ( 972 ) a Schur complement of the selected node ( 971 ).

Method according to one of the preceding claims, characterized in that the method further comprises: - after removing the selected node: re-optimizing ( 153 . 1002 ) of the posse graph ( 661 ) to obtain another estimated position ( 190 ) of the machine, whereby the position of the fixed node ( 972 ) is immutable to the re-optimization, the fixed node ( 972 ) over an edge ( 973 ) with an adjacent node ( 671 ) of the posse graph ( 661 ) connected is.

A method according to claim 4, characterized in that the method further comprises: before performing the re-optimization: adjusting a loss function of the edge ( 973 ) of the posse graph ( 661 ) between the fixed node ( 972 ) and the neighboring node ( 671 ), and / or adjusting an uncertainty of the fixed node ( 972 ).

Method according to one of the preceding claims, characterized in that the node ( 971 ) of the posse graph ( 661 ), which corresponds to the oldest time (t.1-t.12).

Method according to one of the preceding claims, characterized in that the removal of the selected node ( 971 ) selectively depending on a number of nodes ( 671 . 971 ) of the posse graph ( 661 ) is carried out.

Method according to one of the preceding claims, characterized in that the machine is a motor vehicle and in that the method further comprises: - controlling a driver assistance functionality ( 129 ) based on the estimated position ( 190 ) of the motor vehicle.

Control unit comprising: - at least one interface ( 121 . 122 ), which is set up in each case for expenditure of at least one odometry positioning system ( 111 . 112 ) and to receive outputs from at least one absolute positioning system ( 101 . 102 ), - at least one processor ( 123 ), which is set up in each case for each of several times (t.1-t.12) odometry position data ( 615 ) of a machine based on the outputs of the at least one odometry positioning system ( 111 . 112 ) to investigate, wherein the at least one processor ( 123 ) is further set up in each case for each of the plurality of times (t.1-t.12) absolute position data ( 605 . 606 ) of the machine based on the outputs of the at least one absolute positioning system ( 101 . 102 ), wherein the at least one processor ( 123 ) is further adapted to generate a pose graph ( 661 ), where edges ( 672 ) of the pose graph ( 661 ) the odometry position data ( 615 ), where nodes ( 671 ) of the pose graph ( 661 ) the absolute position data ( 605 . 606 ), wherein the at least one processor ( 123 ) is further set up to display the pose graph ( 661 ) to obtain an estimated position ( 190 ) to optimize the machine ( 153 . 1002 ), characterized in that the at least one processor ( 123 ) is further set up after optimizing ( 153 . 1002 ) a node ( 971 ) of the posse graph ( 661 ) for the selected node ( 971 ) a fixed node ( 972 ) of the posse graph ( 661 ) and the selected node ( 971 ) from the pose graph ( 661 ) to remove.

A controller according to claim 9, wherein the controller is arranged to perform a method according to any one of claims 1-8.