DE102016205193B4 - Marginalizing a Pose Graph - Google Patents

Abstract

Method for marginalizing a pose graph (661), comprising:- for each of a plurality of points in time (t.1 - t.12): determining odometry position data (615) of a movable machine based on an output of at least one odometry positioning system (111, 112),- for each of the plurality of points in time (t.1 - t.12): determining absolute position data (605, 606) of the machine based on an output of at least one absolute positioning system (101, 102),- generating (152) the pose graph (661) with edges (672) that correspond to the odometry position data (615) and with nodes (671, 971) that correspond to the absolute position data (605, 606),- optimizing (153, 1002) the pose graph (661) for obtaining an estimated position (190) of the machine, characterized in that the method further comprises:- after optimizing (153, 1002): selecting a node (971) of the pose graph (661),- for the selected node (971): determining a fixed node (972) of the pose graph (661),- removing the selected node (971) from the pose graph (661) and replacing the selected node (971) with the fixed node (972).

Description

TECHNICAL AREA

The invention relates to a method and a control device. In particular, various embodiments relate to techniques for taking odometry position data and absolute position data of a machine into account when 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.

BACKGROUND

Techniques are known to create maps of areas. For example, EN 10 2013 208 521 A1 a method for collectively learning and creating a digital road model is known. Trajectory data and perception data for a plurality of trajectories of one or more vehicles are recorded. An information graph is formed, with trajectory points of the plurality of trajectories forming nodes of the information graph and edges between or at the nodes of the information graph being formed based on odometry measurements and position measurements. The information graph is optimized. Such a technique has the disadvantage that modeling is complex and time-consuming. A real-time application, e.g. to control a driver assistance function, is typically not possible.

On the other hand, techniques are also known for determining an estimated position of a motor vehicle in connection with a driver assistance functionality (position determination). In particular, position determination techniques are known which take absolute position data and odometry position data into account when determining the estimated position of the motor vehicle. In order to ensure the driver assistance functionality, this takes place in real time.

The absolute position data indicates 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 can also be provided with an orientation, which indicates, for example, a current direction of movement of the motor vehicle. A combination of position and orientation is often also referred to as a pose.

The odometry position data, on the other hand, indicate the vehicle's own movement or relative movement, for example in an arbitrarily defined reference coordinate system or in the so-called vehicle coordinate system (body frame). The vehicle's own movement is indicated relative to a previous position of the vehicle, for example.

In order to take a wide variety of position data into account when determining the estimated position of the motor vehicle, positioning techniques are known that combine one or more odometry position data and one or more absolute position data (sensor fusion). The various position data can be output by different positioning systems. The positioning systems can use different techniques for position determination, so that the position data from the different positioning systems can represent independent estimates of the position.

By taking into account several independent position data obtained as output from different positioning systems, a more accurate and more reliable estimate of the position of the motor vehicle can be provided. Especially for applications in the field of autonomous driving, it is necessary that the most accurate possible estimate of the position of the motor vehicle can be provided with a high level of reliability.

Out of EN 10 2014 211 178 A1 A method for correcting measurement data from a first sensor system is known. The first sensor system and/or a second sensor system each record different measurement data, each of which directly and/or indirectly describes 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 using an error-state-space Kalman filter. Using 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 possible to a limited extent to take measurement data from further back into account. For this reason, corresponding techniques are often comparatively inaccurate.

From the German patent application EN 10 2016 211 805 A1 Techniques are known to perform sensor fusion based on a pose graph. The pose graph is repeatedly optimized.

Such techniques have the disadvantage that with increasing runtime the pose graph is getting bigger and the optimization is becoming more computationally intensive.

Techniques for marginalizing a Posen graph using a Schur complement are known, see e.g. Sibley, G. et al.: Sliding window filter with application to planetary landing. In: Journal of Field Robotics, Vol. 27, 2010, No. 5, pp. 587-608. - ISSN 1556-4959 Such techniques have the disadvantage that an adjustment of the Schur complement is not possible or only possible to a limited extent.

Out of Rehder, Joern; Global Pose Estimation with Limited GPS and Long Range Visual Odometry. In Robotics and Automation (ICRA), 2012 IEEE International Conference on Robotics and Automation, pp. 627-633 , techniques are known to estimate the global pose of a vehicle. Techniques for reducing the dimension of a pose graph are also known there. The poses have boundary conditions with respect to measurements, which are modeled by edges in the graph. A virtual zero point node at the origin of the coordinate system is introduced into the graph. The marginalization of nodes is carried out taking into account a Schur complement.

Out of Grisetti, Giorgio; A Tutorial on Graph-Based SLAM. In IEEE Intelligent Transportation Systems Magazine, Vol. 4, Winter 2010, pp. 31-43 , techniques for simultaneous localization and mapping (SLAM) are known, which affect mobile robots when navigating in unknown environments. US 8 442 765 B1 Techniques for marginalizing unnecessary states of a joint distribution of a conditionally independent filter are known.

Out of US 2014/0249752 A1 Techniques for locating a vehicle along a route are known. Orthographic images of the route are captured.

SUMMARY

Therefore, there is a need for improved pose graph marginalization techniques. In particular, there is a need for flexible pose graph marginalization techniques.

This object is solved by the features of the independent patent claims. The dependent patent claims define embodiments.

According to one aspect, the present invention relates to a method for marginalizing a pose graph, which comprises for each of a plurality of points in time: 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 points in time: determining absolute position data of the machine based on an output of at least one absolute positioning system. The method further comprises generating a pose graph. Edges of the pose 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 comprises 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 comprises removing the selected node from the pose graph and replacing the selected node with the fixed node.

For example, the pose graph can preferably have a node for each of the multiple points in time. The multiple points in time can therefore cover a time window from the current time into the past (sliding window). Accordingly, when 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 recent past. For example, when generating the pose graph, a fixed number of points in time can preferably be taken into account. The use of the pose graph can represent an alternative to using the previously known Kalman filters for sensor fusion.

Basics of the pose graph and a corresponding optimization are for example from Kümmerle, R. et. al.: G2O: A General Framework for Graph Optimization. In: 2011 IEEE International Conference on Robotics and Automation (ICRA), pp. 3607 - 3613. - http://dx.doi.org/10.1109/ICRA.2011.5979949 The structure of a corresponding system matrix is also described there. The pose graph comprises nodes that correspond to state variables to be optimized (here: absolute positions of the machine at a certain point in time), and edges that correspond to a pairwise connection of the states of the nodes connected by the respective edge. For example, it is advantageous for the nodes of the pose graph to be associated with a heading; in this respect, the nodes can represent 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.

For example, in advantageous embodiments, the estimates for the position of the machine, as provided by the at least one absolute positioning system, can be taken into account in connection with nodes of the pose graph. For example, the various nodes of the pose graph can be linked to the various absolute position data of the at least one absolute positioning system via boundary conditions - sometimes such links in the form of boundary conditions are also referred to as edges of the pose graph; however, this terminology is not adopted here to avoid ambiguity. For example, the absolute position data can advantageously be formally taken into account as fixed nodes that are not influenced by the optimization, and the nodes of the pose graph to be optimized can be referred to as optimization nodes. An optimization node can be linked to several fixed nodes, the position of which is not changed by the optimization, via boundary conditions.

For example, the egomotion estimates, as provided as the output of the at least one odometry positioning system in the form of the odometry position data, can advantageously be taken into account in connection with the edges of the pose graph. Again, optimization edges and fixed edges linked in the form of constraints can advantageously be taken into account. The optimization edges can preferably be specified 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 that more than one edge is present between two adjacent nodes of the pose graph.

Since the different odometry position data and absolute position data can deviate from each other, by optimizing the pose graph it may be possible to obtain the estimated position as the best compromise between the different absolute position data and odometry position data. When optimizing the pose graph, the nodes and edges can be moved, i.e. changed compared to the values obtained from the outputs of the positioning systems. The nodes to be optimized are often referred to as optimization nodes. The optimization nodes are linked by boundary conditions to the nodes and edges obtained from the outputs of the positioning systems.

It is possible that the absolute position data and/or the odometry position data may have some uncertainty. Such uncertainty may also be referred to as 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.

In connection with the pose graph, error terms are often defined which are larger (smaller), the further (less far) the positions estimated by the optimization are from the outputs of the positioning systems and the smaller (larger) the variance of the corresponding outputs of the positioning systems. The optimization can preferably provide a configuration of the pose graph which corresponds to an extremal value of the error terms. A global extremal value of the error terms is preferably aimed for. The optimization can advantageously be carried out numerically. The optimization can advantageously be carried out iteratively, i.e. comprise several optimization iterations. For example, the optimization can advantageously comprise a nonlinear least squares technique. For example, the optimization can advantageously be carried out using a Gauss-Newton technique and/or a Levenberg-Marquardt technique.

Such techniques enable real-time application. For example, the machine can be a motor vehicle. Preferably, it also includes controlling a driver assistance functionality based on the estimated position of the motor vehicle. For example, the driver assistance functionality can preferably relate to autonomous driving of the motor vehicle. It would also be possible for the driver assistance functionality to advantageously relate to partially automated or highly automated driving. It would also be possible for the driver assistance functionality to advantageously relate to a traffic jam assistant. For example, it is preferably possible for the driver assistance functionality to influence the position of the motor vehicle. In this respect, the position of the motor vehicle can be influenced based on the estimated position. For example, the driver assistance functionality can control an electric steering system of the motor vehicle. For example, the driver assistance functionality can preferably control the brakes and the engine of the motor vehicle. For example, the driver assistance functionality can include: a lane keeping assistant; a distance assistant; a parking assistant; etc.

By using the pose graph it is possible to preferably use a different number of odometry position data and/or a different number of absolute position data to be taken into account. In particular, it is possible for the number of odometry position data and/or the number of absolute position data taken into account when generating the pose graph to vary as a function of time. Through the generic use of the odometry position data and the absolute position data, the outputs of different positioning systems to be merged can be exchanged modularly. In this way, it may advantageously be possible to always take into account all available outputs of positioning systems. In this way, it may preferably be possible to obtain the estimated position of the machine with a particularly high level of reliability even in transitional situations in which outputs from certain positioning systems are no longer available or only available to a limited extent, but outputs from other positioning systems become available.

Furthermore, the use of the pose graph allows a more accurate estimation of the position of the machine not only for the current time but also for the past, compared to reference implementations based on Kalman filters.

In various advantageous scenarios, optimizing the pose graph may still provide reliability of the estimated position of the machine. It is possible that the driver assistance functionality of the machine may still be controlled based on the reliability of the estimated position.

For example, the reliability may preferentially take into account the sum of the error terms for the different nodes of the optimized pose graph. More recent (older) nodes may have a larger (smaller) influence on the reliability of the estimated position. For example, in a scenario where the reliability of the estimated position obtained by optimization becomes comparatively large, manual driver intervention may be required by the driver assistance functionality.

By taking into account the reliability of the estimated position, the driver assistance functionality can be controlled with a high degree of certainty; in particular, it can be avoided that the driver assistance functionality is controlled on the basis of unreliable or only partially reliable estimates of the position of the motor vehicle.

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 may still be controlled based on the estimated odometry.

The odometry can, for example, advantageously indicate a current orientation/direction of travel of the machine. Together with the estimated position of the motor vehicle, an estimated position and orientation (pose) of the motor vehicle can thus be provided.

For example, by optimizing the pose graph, the estimated position of the machine at a future time with respect to the plurality of time points can advantageously be obtained.

In other words, in various advantageous embodiments it may be possible for the position to be estimated for a future point in time. A prediction can therefore be made for the position of the machine.

Generating and optimizing the pose graph requires a certain amount of time (optimization time). The optimization time can be compensated by providing the estimated position for the future time.

This can make it possible to control the driver assistance functionality in a particularly targeted manner. In particular, it can be possible to provide information about the current position of the machine with a particularly low latency. Real-time applications are possible.

The optimization time 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 can still have high accuracy.

For example, after removing the selected node, the method could further comprise: re-optimizing the pose graph to obtain another estimated position of the machine. The position of the fixed node can be invariant with respect to the re-optimization. The fixed node can be connected to an adjacent node of the pose graph via an edge. By making the position of the fixed node invariant or fixed with respect to the re-optimization, the complexity of the re-optimization can be comparatively limited. This can reduce the optimization time.

Such techniques are based on the realization 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, i.e. the node of the pose graph that corresponds to the oldest point in time can be selected. Deleting old nodes and edges in this way usually means that old position data no longer has any influence on the current estimate of the position. This means that the estimates have limited accuracy. For example, the estimates for the position can have a comparatively high level of inaccuracy.

Therefore, approaches to so-called marginalization are known in previously known techniques. Previously known approaches to marginalization include forming the Schur complement on the system matrix. Such techniques have no model equivalent in the Posen graph. Such techniques often result in a more densely populated system matrix; this in turn can increase the computational effort required to carry out the optimization, which in turn can make the optimization time comparatively long.

The techniques described here make it possible to avoid a denser population of the system matrix - and thus a longer optimization time - by providing the fixed node instead of the selected node, for example the node that corresponds to the oldest point in time, and at the same time not or not significantly reducing the accuracy of the position estimation by optimizing the pose graph. The techniques described here therefore enable a particularly precise marginalization of the pose graph compared to previously known techniques. Using the techniques described here, it may therefore be possible for the optimization problem to remain manageable even after several marginalizations of the pose graph and not to degenerate as a function of time.

Different techniques can be used to determine the fixed node. For example, the fixed node can be determined based on elements selected from the following 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 fixed node connected to the selected node via an edge. For example, the absolute position data can be represented by a fixed node. Such fixed nodes that are connected to the selected node via an edge as a boundary condition can be taken into account when determining the fixed node; other absolute position data can be disregarded. For example, the additional fixed node can have been determined as part of a previous marginalization. For example, all other existing corresponding additional fixed nodes can be taken into account. In this way, it can be possible, for example, for the fixed node to bundle information relating to the selected and removed node. Past information is then not simply discarded by removing the selected node, but transferred to the fixed node. Therefore, the fixed node could also be called a prior fixed node.

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, for example, be: "the bundled position information that has so far been removed from the pose graph leads to the estimate that the machine was at the position of the fixed node." It would therefore be advantageous for the fixed node to represent the Schur complement of the selected node. This means that the fixed node can avoid 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 also possible.

The pose graph that has the fixed node but not the selected node can still 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 for the developer/user to understand the problem, analyze and manipulate the problem.

As a further effect, the fixed node allows to adjust the uncertainty of the old information and to influence the weighting if necessary. For example, the method could further comprise, before performing a new optimization: adjusting a loss function of the edge of the pose graph that runs between the fixed node and the neighboring node, and/or adjusting an uncertainty of the fixed node. Particularly implausible information contained in the fixed node can then be taken into account with a larger uncertainty, which can increase a tolerance in the optimization. It is also possible to apply a robust cost or loss function to the error term of the constraint represented by the fixed node.

For example, it would be possible for the removal of the selected node to be carried out selectively depending on the number of nodes in the pose graph. If, for example, the number of nodes in the pose graph exceeds a predetermined threshold, the oldest node in the pose graph can be selected and removed, while the corresponding fixed node is determined at the same time. On the one hand, this can ensure that the optimization time does not become disproportionately long due to a particularly large number of nodes; at the same time, however, unnecessarily frequent marginalization of the pose graph can be avoided.

According to various advantageous examples, it is possible for the plurality of points in time to be determined based on a predetermined clock rate. It is then preferably possible for raw odometry position data to be received as the output of the at least one odometry positioning system at respective sampling rates. Accordingly, it is preferably possible for raw absolute position data to be received as the output of the at least one absolute positioning system at respective sampling rates. The predetermined clock rate can be different from the sampling rates. The sampling rates can vary as a function of time.

For example, it would be possible for the method to further comprise: 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 multiple points in time. For example, it would preferably be possible for the method to further comprise: 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 points in time. The interpolation can therefore preferably take place in the spatial time period. For example, the interpolation can take place in a reference coordinate system of the pose graph.

In various advantageous variants, it is possible for a coordinate transformation of the raw absolute position data and/or the raw odometry position data to be carried out in order to obtain the absolute position data and/or the odometry position data, respectively. For example, a coordinate transformation could be carried out between a UTM coordinate system and a WGS84 coordinate system. It would also advantageously be possible for the coordinate transformation to include a coordinate rotation. For example, a rotation could take place between a proprietary reference system, for example speed and yaw rate of the motor vehicle as a machine, and the reference system of the pose graph. For example, a model of the motor vehicle can preferably be taken into account in order to take vehicle-specific properties into account in the coordinate transformation.

Using such techniques, it is possible to flexibly take into account different timing behaviors of the various positioning systems. In particular, it is not necessary for the various positioning systems to provide the outputs with a fixed or static sampling rate. No a priori assumption needs to be made about the timing behavior of the position data taken into account when generating the pose graph.

For example, it would advantageously be possible for the raw odometry position data and/or the raw absolute position data to each be provided with a time stamp. For example, the time stamp could advantageously indicate a point in time at which the respective positioning system measured the odometry position data and/or the raw absolute position data.

For example, it may advantageously be possible for position data from different positioning systems to be provided not in a time-sequential sequence, but in an order that is swapped with respect to the corresponding time stamps. This may be the case because 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 temporarily stored in a memory.

When generating the pose graph, such raw position data that is not received sequentially in the past can simply be inserted. This increases the flexibility when merging the outputs of different positioning systems. Real-time generation and optimization of the pose graph can be dispensed with, since a prediction for the estimated position can be made at the future point in time.

For example, it is preferably possible that one iteration of the generation and optimization optimization of the pose graph is performed for each cycle of a given optimization cycle rate. For each iteration, the corresponding estimated position of the machine can advantageously be obtained.

It is possible that the optimization clock rate is chosen to be different from the clock rate that determines the multiple points in time. However, it would also be possible that the optimization clock rate is chosen to be the same as 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.e. that previously generated pose graphs can be used for different iterations of generating and optimizing the pose graph.

For example, the optimization clock rate can 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 enable downstream logic, such as the driver assistance functionality in particular, to be operated or controlled particularly reliably.

By using the pose graph, a particularly efficient optimization can be implemented to obtain the estimated position of the machine. Therefore, comparatively high optimization clock rates can be selected. This allows the estimated position to be determined particularly quickly.

For example, it is preferably possible for the time period required at least for optimizing the pose graph in the various iterations to be monitored. Then it is preferably possible for the predetermined clock rate and/or the number of points in time and/or an accuracy of the optimization to be determined as a function of monitoring the time period.

For example, advantageously, the accuracy of optimizing may correspond to a number of optimization iterations. For example, a higher (lower) accuracy may correspond to a larger number (lower number) of optimization iterations.

The monitoring may preferably comprise repeatedly measuring the optimization time duration. For example, the total time duration required to generate and optimize the pose graph may preferably be monitored. For example, it would be possible to choose smaller (larger) clock rates and/or a smaller (larger) number of time points and/or a lower (larger) accuracy of optimization if the monitoring indicates a larger (smaller) optimization time duration. Smaller (larger) clock rates and a smaller (larger) number of time points may result in a smaller (larger) time duration required at least for optimizing the pose graph in the different iterations.

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.

For example, the clock rate can be in the range of 1 Hz - 1,000 Hz, or in the range of 10 Hz - 100 Hz, or in the range of 25 - 50 Hz.

In principle, it is possible for at least some of the sampling rates to have a time dependency. For example, preferably at least some of the sampling rates can have temporal fluctuations. In other words, it can advantageously be possible to take into account outputs from positioning systems when generating the pose graph that 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 of 0.5 Hz to 10 Hz, or in the range of 0.5 Hz to 2 Hz.

In the various scenarios described herein, it may also be possible that the output of at least one of the positioning systems does not provide the associated position data in 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 such non-time-sequentially obtained position data into account when generating the pose graph.

In particular, it may be possible, for example, that at least one of the sampling rates has interruptions. For example, the interruptions may comprise more than two sampling intervals of the corresponding sampling rate. For example, the interruptions may comprise more than 10 sampling intervals. For example, the interruptions may comprise more than 100 sampling intervals.

The interruptions may represent outages during which no or no reliable position data from the corresponding positioning system is available.

In other words, it may be possible to compensate for interruptions in the provision of outputs from the various positioning systems. The pose graph can be generated with a flexible number of outputs from different positioning systems.

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 for a plurality of points in time in a first time interval; the absolute position data can preferably be determined based on the output of a second absolute positioning system for a plurality of points in time in a second time interval. The first absolute positioning system can advantageously be different from the second absolute positioning system. The absolute position data can preferably not be determined based on the output of the second absolute positioning system for the plurality of points in time in the first time interval.

Therefore, for example, the first time interval may represent an interruption with respect to the output of the second absolute positioning system. The method may, in advantageous scenarios, comprise: 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 various preferred scenarios, it may therefore be possible for different outputs of positioning systems to be taken into account depending on the point in time. 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 transitional situations it may be possible in this way to estimate the position of the machine with a high degree of reliability.

According to a further aspect, the present invention relates to a control device. The control device comprises at least one interface. The at least one interface is configured to receive outputs from at least one odometry positioning system and to receive outputs from at least one absolute positioning system. The control device further comprises at least one processor. The at least one processor is configured to determine odometry position data of the machine for each of a plurality of points in time based on the outputs of the at least one odometry positioning system. The at least one processor is further configured to determine absolute position data of the machine for each of the plurality of points in time 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 pose 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 to obtain an estimated position of the motor vehicle. The at least one processor is further configured to select a node of the pose graph after optimization, to determine a fixed node of the pose graph for the selected node, and to remove the selected node from the pose graph.

The control device according to the currently discussed aspect is arranged to carry out the method according to a further aspect of the present invention.

For such a control device according to the presently discussed aspect, effects can be achieved that are comparable to the effects that can be achieved for the method according to another aspect of the present invention.

The features set out above and features described below may be used not only in the corresponding explicitly set out combinations, but also in further combinations or in isolation without departing from the scope of the present invention.

BRIEF DESCRIPTION OF THE CHARACTERS

• 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.
• 2 illustriert schematisch die Verfügbarkeit der unterschiedlichen Positionsdaten für Zeitpunkte in unterschiedlichen Zeitintervallen in einer Übergangssituation gemäß verschiedener Ausführungsformen.
• 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.
• 4 illustriert schematisch ein Steuergerät gemäß verschiedener Ausführungsformen.
• 5 ist ein Flussdiagramm eines Verfahrens gemäß verschiedener Ausführungsformen.
• 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.
• 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.
• 8 illustriert schematisch den Posen-Graph der 6 und 7, bevor der Posen-Graph optimiert wird.
• 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.
• 10 illustriert schematisch einen Posen-Graphen gemäß verschiedener Ausführungsformen.
• 11 illustriert schematisch eine Systemmatrix für den Posen-Graphen der 10 gemäß verschiedener Ausführungsformen.
• 12, 13, 14 und 15 illustrieren schematisch die Marginalisierung eines Posen-Graphen unter Verwendung eines Fixknotens.
• 16 ist ein Flussdiagramm eines Verfahrens gemäß verschiedener Ausführungsformen.

The present invention is explained in more detail below using preferred embodiments with reference to the drawings. In the figures, identical reference numerals designate identical or similar elements.

• 1 is a schematic illustration of the logical information flow in the context of the fusion of position data from multiple positioning systems based on a pose graph according to various embodiments.
• 2 schematically illustrates the availability of the different position data for points in time in different time intervals in a transition situation according to various embodiments.
• 3 schematically illustrates the availability of certain position data at different times according to different embodiments, wherein in 3 an interruption during which position data is temporarily unavailable is illustrated.
• 4 schematically illustrates a control device according to various embodiments.
• 5 is a flowchart of a method according to various embodiments.
• 6 schematically illustrates the generation of 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 with different sampling rates.
• 7 schematically illustrates the generation of edges of the pose graph of the 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 certain sampling rate.
• 8 schematically illustrates the pose graph of the 6 and 7 before the pose graph is optimized.
• 9 schematically illustrates the optimized pose graph of the 6-8 and further illustrates an estimated position of a motor vehicle obtained from the optimized pose graph.
• 10 schematically illustrates a pose graph according to various embodiments.
• 11 schematically illustrates a system matrix for the pose graph of the 10 according to various embodiments.
• 12 , 13 , 14 and 15 schematically illustrate the marginalization of a pose graph using a fixed node.
• 16 is a flowchart of a method according to various embodiments.

DETAILED DESCRIPTION OF EMBODIMENTS

The present invention is explained in more detail below using preferred embodiments with reference to the drawings. In the figures, identical reference numerals denote identical or similar elements. The figures are schematic representations of various embodiments of the invention. Elements shown in the figures are not necessarily shown to scale. Rather, the various elements shown in the figures are shown in such a way that their function and general purpose is understandable to those skilled in the art. Connections and couplings between functional units and elements shown in the figures can also be implemented as an indirect connection or coupling. A connection or coupling can be implemented wired or wirelessly. Functional units can be implemented as hardware, software or a combination of hardware and software.

Techniques for determining the position of a machine, such as a motor vehicle, are described below, which allow odometry position data from at least one odometry positioning system and absolute position data from at least one absolute positioning system to be merged. For the sake of simplicity, reference is made below to a motor vehicle, although corresponding techniques can also be applied to other machines.

In particular, techniques are described below that allow an estimated position of the motor vehicle to be determined, which can be used for a driver assistance functionality. For example, the driver assistance functionality can relate to autonomous driving. Other driver assistance functionalities are also conceivable.

Various examples disclosed herein allow the fusion of the various position data to be performed based on a pose graph. The pose graph can be optimized to estimate the position of the motor vehicle.

For each of several points in time, odometry position data of a motor vehicle is determined based on the output of at least one odometry positioning system, and absolute position data of the motor vehicle is also determined based on an output of at least one absolute positioning system. The pose graph is generated, with nodes of the pose graph corresponding to the absolute position data and with edges of the pose graph corresponding to the odometry position data. The pose graph is then optimized.

This paper describes techniques that allow the pose graph to be marginalized efficiently and without loss of accuracy. A node to be optimized is replaced by a fixed node.

In 1 various aspects are illustrated in relation to a workflow for position determination using sensor fusion. Raw absolute position data 105, 106 are received as outputs from two absolute positioning systems 101, 102. Furthermore, raw odometry position data 115, 116 are received as outputs from two odometry positioning systems systems 111, 112. For example, a software module 151 could receive the corresponding position data 105, 106, 115, 116 on a dedicated thread. A memory (in 1 not shown) can temporarily store the received position data 105, 106, 115, 116 until they are discarded.

It is then possible to initiate a construction process for generating the pose graph by a graph management system 152 at regular, adjustable time intervals - that is, at an optimization clock rate. For example, the pose graph generated in a previous iteration of the optimization clock rate can be expanded by new nodes and edges.

In various scenarios, the pose graph can have nodes arranged at fixed time intervals. Accordingly, it is possible for several points in time at which nodes are added to the pose graph to be determined based on a predetermined clock rate. For each of the several points in time, 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 each associated with edges or nodes of the pose graph to be optimized, e.g. in the form of boundary conditions.

For example, the optimization clock rate can be equal to or different from the clock rate of the nodes of the Posen graph.

After the pose graph has been generated, the optimization can be carried out. For this purpose, the generated pose graph is passed to a backend 153. The backend 153 then carries out the optimization. As a result of the optimization, an actual position of the motor vehicle is obtained.

In the various scenarios disclosed herein, the actual position may be a prediction for a future point in time in order to compensate for latencies due to the generation of the pose graph by the graph management 152 and the performance of the optimization by the backend 153. For example, it would be possible for the actual position to be a prediction of the position of the motor vehicle for the next cycle of the predetermined optimization cycle rate.

Using such techniques for determining positions based on the pose graph, it is possible to flexibly take into account position data 105, 106, 115, 116 that originate from a wide variety of positioning systems 101, 102, 111, 112, 113, see 2 The specific type of positioning system 101, 102, 111-113 used is not essential for the functioning of the position determination techniques described herein; a wide variety of types and kinds of positioning systems 101, 102, 111-113 can be used in the various scenarios disclosed herein.

For example, satellite-based positioning systems may be used to provide the absolute position data 105, 106; one example is the Global Positioning System (GPS). For example, machine-readable character recognition 101 may be used as a positioning system to provide the absolute position data 105, 106. For example, laser-based distance measurement 111 (light detection and ranging, LIDAR) could be used to determine absolute position data 105, 106 and/or odometry position data 115, 116. Also, visual pose recognition 112 may provide the odometry position data 115, 116 based on differences in sequentially captured images depicting a landmark. Information from the chassis odometry 113 may be used to provide the odometry position data 115, 116; for example, a steering angle and/or a rotational speed of the wheels may be taken into account in this context. Acceleration sensors could also be used to provide the odometry position data 115, 116; for example, an electronic stability system (ESP) could be used as an acceleration sensor.

Another source for the position data could be car2car systems: these can either directly determine the position of a vehicle or enable indirect position determination via the position of a third vehicle, whereby the relative positioning to the third vehicle can then be taken into account.

Stereo and/or mono cameras can be used not only to provide odometry position data, but also alternatively or additionally to provide absolute position data. There are various approaches to this: for example, it is possible to create a visual map of a route and to localize it on the map when driving through it again.

Simultaneous localization and mapping (SLAM) techniques can also serve as a source of position data. For example, SLAM techniques can provide absolute position data in a global coordinate system or alternatively or additionally provide odometry position data.

Other examples of positioning systems include: Techniques based on RFID markers. Techniques based on WLAN networks that are currently being received and/or based on triangulation in a known WLAN network map. Cell phone towers can also be read.

In the various scenarios described herein, it may also be possible to generate odometry position data by taking the difference between two time-sequential absolute position data.

Out of 2 It can be seen that, on the one hand, the time intervals 201 in which the various positioning systems 101, 102, 111-113 provide the associated position data are different from one another. 2 represents a so-called transition situation: the position data 105, 106, 115, 116 provided by LIDAR 111, the recognition of machine-readable characters 101 and the visual pose recognition 112 break off; instead, position data from the GPS system 106 becomes available. A corresponding situation can arise, for example, when leaving a parking garage. This means that first the absolute position data 105 is obtained from the recognition of machine-readable characters 101 and not from the GPS system 106; then the absolute position data 106 is obtained from the GPS system 102. In the various scenarios described here, it is therefore possible to switch between different sources for the position data 105, 106, 115, 116; This means that in the various scenarios described here, switching can take place between the different positioning systems 101, 102, 111-113. The switching can take place gradually. The switching can be adapted to the situation. The switching can take place taking into account transition phases between the different positioning systems.

In 2 It is further illustrated that the sampling rates 202 with which the various positioning systems 101, 102, 111-113 provide the associated raw position data differ from one another.

In various scenarios it is also possible that the sampling rate 202 has an interruption 203, see 3 . In 3 the sampling rate 202 is 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 outputs. An interruption 203 can occur in particular when significantly less raw position data is available than required by a clock rate with which nodes are inserted into the pose graph. Short interruptions 203 can be compensated, for example by interpolation. In principle, however, it is possible that the interruption 203 is comparatively long. For example, the interruption 203 can include several sampling intervals of the sampling rate 202. In the scenario of the 3 the sampling rate 202 is approximately 1 Hz before and after the interruption 203. This means that a sampling interval has a length of approximately 1 second. The interruption 203 has a duration of approximately 12 seconds, thus comprising more than 10 sampling intervals.

In principle, it is also possible for the sampling rate 202 - outside of an interruption 203 - to vary as a function of time. For example, depending on the available signal quality, the satellite-based positioning system 106 could implement a higher or lower sampling rate 202.

Techniques are described below for determining the actual position 190 of the motor vehicle consistently and reliably with a high degree of accuracy. These techniques can also determine the actual position 190 reliably and precisely in transitional situations or when sampling rates 201 differ from one another or are provided with interruptions 203.

4 illustrates a control unit 120 which is set up to carry out corresponding techniques. The control unit 120 comprises an interface 121 which communicates with the various positioning systems 101, 102, 111, 112. The control unit 120 further comprises a processor 123 which is coupled to the interface 121 and a memory 124. For example, the memory 124 can temporarily store or buffer the various position data 105, 106, 115, 116. The processor 123 can be set up to carry out a variety of techniques described herein in connection with determining the odometry position data and the absolute position data based on the raw odometry position data and the raw absolute position data, respectively, generating the pose graph and optimizing the pose graph, as well as controlling the driver assistance functionality. To control the driver assistance functionality 129, the control unit 120 includes a further interface 122. The various interfaces 121, 122 can be set up to communicate with the various units, for example via a bus system of the motor vehicle. Direct data connections are also possible.

For example, control data can be stored in the memory 124 that can be executed by the processor 123. Execution of the Control data by the processor 123 may cause the processor 123 to execute a method as described in connection with the flow chart of the 5 is illustrated.

First, in step 1, it is checked whether the next iteration for generating and optimizing a pose graph should be carried out; for this purpose, a predetermined optimization clock rate can be taken into account. An actual position 190 is obtained for each iteration.

Optionally, in all examples described here, an actual odometry can also be obtained for each iteration. 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 that has taken place. From this, for example, the speed and the rotation rate can be deduced.

If the next iteration is to be carried out, in step 2 absolute position data are first determined for several points in time based on raw absolute position data 105, 106. This is shown in 6 illustrated. In 6 the pose graph 661 is illustrated only with respect to nodes 671. The pose graph 661 is in 6 shown in two-dimensional xy-position space. The pose graph 661 comprises optimization nodes 671 at the 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 the various scenarios disclosed herein, a wide variety of clock rates can be taken into account. For example, the clock rate can be in the range from 1 Hz to 1000 Hz, preferably in the range from 10 Hz to 100 Hz, particularly preferably in the range from 25 Hz to 50 Hz. In this way, the actual position can be determined quickly and with a sufficiently high level of reliability.

In the various scenarios disclosed herein, it is possible that the clock rate is variable and is determined flexibly. For example, a higher clock rate may correspond to a larger number of nodes 671, which in turn increases the optimization time period; therefore, it may be desirable to determine the clock rate depending on monitoring the optimization time period. In this way, a certain optimization clock rate can be guaranteed even for computationally intensive pose graphs 661.

The nodes 671 correspond to the absolute position data 605, 606 (in 6 with dashed-filled 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 determined based on the raw absolute position data 105, 106. The raw absolute position data 105, 106 are obtained with sampling rates that differ from the clock rate; e.g., raw absolute position data 105, 106 are obtained at 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 corresponding time stamps. Therefore, the raw absolute position data 105, 106 are interpolated to obtain the absolute position data 605, 606 (in 6 represented by the solid lines 662). For example, the raw absolute position data 105 at times t.2 and t.4 are interpolated to obtain the absolute position data 605 at time t.3. The interpolation thus represents an estimate of where the corresponding positioning system would have expected the motor vehicle to be at time t.3.

All times t.1-t.13 are within the time window [t0-T; t0], where t0 represents the actual time. T denotes a time difference. Raw absolute position data 105, 106 that are not within this time window are discarded, i.e. not included in the pose graph 661 and e.g. deleted from the memory. This allows the number of nodes 671 of the pose graph 661 to 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 comparatively high.

In some scenarios, it may happen that, for example, odometry positioning systems that provide outputs with a comparatively very high sampling rate perform measurements that lie in the future compared to the time horizon t0. This may occur due to an internal prediction of the odometry positioning system. It is possible to take corresponding raw odometry position data into account when generating the pose graph 661.

Furthermore, the raw absolute position data 105, 106 are selectively discarded based on a data density of the raw absolute position data 105, 106 in the time period and based on the clock rate. For example, in the time range t.1-t.6, only very few raw absolute position data 106 are present - especially in comparison to the clock rate. Therefore, the raw absolute position data 106 would be discarded at time t.4a and not taken into account as a boundary condition when generating the pose graph 661, e.g. for the nodes 671 at the times t.3 or t.5.

In other examples, the position data 106 at time t.4a may initially remain buffered in memory, but may not be taken into account for generating the pose graph 661 in the current iteration. Deletion from memory may occur in a later iteration, provided that no more non-time-sequential position data 106 is obtained.

In various other scenarios, it would be possible, for example, for the selective discarding of the various raw absolute position data 105, 106 to be based on the predetermined optimization clock rate; for example, for higher (lower) optimization clock rate and a larger (smaller) number of raw absolute position data 105, 106 could be discarded.

Referring again to 5 , odometry position data are determined in step 3. Corresponding aspects are related to 7 illustrated. In 7 the nodes 671 determined from the raw absolute position data 105, 106 are shown, which correspond to the absolute position data 605, 606 (where in 7 for reasons of clarity the boundary conditions are not illustrated). Furthermore, 7 Raw odometry position data 115 are shown, which are obtained with a comparatively high sampling rate; the sampling rate is greater than the clock rate. In various exemplary scenarios, it may be necessary to transfer the raw odometry position data 115 into the coordinate system of the pose graph 661 by means of a coordinate rotation before inserting it into the pose graph 661 (in 7 not shown). The raw odometry position data 115 are in turn converted by interpolation into odometry position data 615 which correspond to the edges 672 of the pose graph 661.

Corresponding techniques disclosed above with respect to discarding raw absolute position data 105, 106 may also be applied to the raw odometry position data 115, 116.

Referring again to 5 , the pose graph 661, ie the nodes 671 and edges 672, is then generated in step 4 on the basis of the position data 105, 106, 115, 116 generated in steps 2 and 3. For this purpose, the nodes 671 to be optimized are inserted at fixed intervals determined according to the predetermined clock rate.

In 8 the pose graph 661 to be optimized is shown; in particular, with respect to the nodes 661, the aspect of the boundary condition is graphically illustrated by the connection between the schematically illustrated absolute position data 605 and the optimization nodes 671. The aim of the optimization is to globally minimize a corresponding error function while taking the boundary conditions into account. The edges 672 also form boundary conditions for the optimization of the nodes 671. The graph 661 is passed to the backend 153 for optimization.

Referring again to 5 : subsequently, in step 5, the pose graph 661 is optimized. It can then be assumed that the optimized pose graph 661 represents the best possible compromise between the various position data 105, 106, 115, 116 of the positioning systems 101, 102, 111-113 to be merged.

In 9 Aspects of the optimized pose graph 661 are shown. In 9 the now optimized nodes 671 are shown, as well as the estimated actual position 190 of the motor vehicle. 9 it can be seen that the actual position 190 is predicted for the future time t.13 (in 9 illustrated by the dotted line). This can compensate for a latency for optimizing the pose graph 661. By predicting the actual position 190, it can be achieved that at the beginning of a next iteration according to step 1 of the 5 A current estimate of position 190 is already available.

In order to predict the actual position 190, an interpolation of the last two optimized nodes 671 can be carried out in various examples. From this, the motor vehicle's own motion can be determined and optionally output. Assuming that the motor vehicle behaves in the same way in the near future as it did in the last known past, this own motion is appended to the most recently optimized node 671 by vector addition.

The actual position 190 is determined with a certain accuracy, which is inversely proportional to a reliability 191 (in 9 illustrated by the error bars). The reliability 191 can also be taken into account to control the driver assistance functionality 129.

10 illustrates a pose graph 661 that can be determined using the techniques described above. The pose graph 661 has several nodes 671 that are arranged at a fixed time distance from each other at times t.1, t.2 and t.3.

The various nodes 671 are each assigned absolute position data 605, 606; this assignment can be made in the form of boundary conditions (in 10 represented by the vertical edges of the pose graph 661). The absolute position data 605, 606 correspond to fixed nodes, since their position is not changed during the optimization of the pose graph 661. Interpolation techniques can be used; raw absolute position data 105, 106 can be mapped to the absolute position data 605, 606 arranged at the times t.1, t.2 and t.3 that correspond to the nodes 671 to be optimized. The edges 672 of the pose graph 661 can then be generated, e.g. based on the odometry position data 615.

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 matrix that is inverted during optimization, for example by means of a Gauss-Newton method or a Levenberg-Marquardt method. This can correspond to solving the following equation: $$H\Delta x=-b$$

where Δx denotes the update vector and b the coefficient vector. Typically, the solution of equation 1 becomes faster by inversion when the system matrix 901 is sparse.

The system matrix 901 includes diagonal entries 902, which are each influenced by the absolute position data 605, 606 and the odometry position data 615 as boundary conditions. The non-diagonal entries 903 are only influenced by the odometry position data 615 as boundary conditions.

If new nodes 671 are inserted whenever new absolute position data 605, 606 and/or new odometry position data 615 are available, a branched structure of the pose graph 661 and thus also a branched structure of the system matrix 901 results. Therefore, in various embodiments, it is desirable to carry out a marginalization of the pose graph 661 from time to time.

In the following, techniques for marginalization are described in detail. It is also explained that a fixed node can be used as a replacement for a selected previous node. The fixed node can contain the same information as the Schur complement. At the same time, such a fixed node can have the advantage that this fixed node can be explicitly found in the design of the Posen graph 661 and can therefore be manipulated in a targeted manner.

Traditionally, the Schur complement is applied to equation 1 to remove state variables from the optimization problem. In the Posen graph 661, this operation corresponds to removing nodes 671. The Schur complement ensures that the remaining nodes 671 retain the same estimate before and after the application. If the state variables were simply deleted, a different estimate would be obtained; 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 is shown below how a single node 671/a single state variable can be marginalized. If several nodes 671/state variables are to be marginalized, it is possible to apply such techniques several times in succession.

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: $$\left[\begin{array}{cc}{H}_{bb}^{\text{small}}& H_{bc}^{\text{small}}\\ H_{cb}^{\text{small}}& H_{cc}^{\text{small}}\end{array}\right]\Delta x^{small}=-\left[\begin{array}{l}{b}_b^{\text{small}}\\ b_c^{\text{small}}\end{array}\right].$$

An exemplary derivation of a possibility for determining the fixed node is described below. This is purely exemplary. Other techniques for determining the fixed node are also possible. For the derivation, it is useful to start with a pose graph 661 that has only two nodes 671 and several boundary conditions based on odometry position data 615 and absolute position data 605, 606. Such a situation is in 12 In the example of 12 a node 971 (for example in 13 denoted by x _a ) is ignored, compare 12 and 13 . This is done to see how the different nodes 671, 971 affect the system matrix 901. In the resulting pose graph 661 $G^{\text{small}}$

for 12 , in which the remaining nodes 671, x _b , x _c are connected to each other and have the additional boundary conditions 605, 606, 615, the corresponding optimization equation is: $$\left[\begin{array}{cc}{H}_{bb}^{\text{small}}& H_{bc}^{\text{small}}\\ H_{cb}^{\text{small}}& H_{cc}^{\text{small}}\end{array}\right]\Delta x^{small}=-\left[\begin{array}{l}{b}_b^{\text{small}}\\ b_c^{\text{small}}\end{array}\right].$$

If the additional node 971, x _a is taken into account, the structure of the Posen graph changes (compare 13 ), as well as the structure of the system matrix 901. In addition, let x _a with several global fixed nodes x _i corresponding to the absolute position data 605 having the information matrices Ω ^̅ _i such that Ω ^̅ = ^- ∑ _i Ω̅ _i .

geben, so dass $\widehat{\Omega }={\displaystyle \sum {}_j{\widehat{\Omega }}_{a,b}^j}.$

Der resultierende Posen-Graph 661 $G^{\text{full}}$

für das Beispiel der 13 wird definiert durch die Systemmatrix 901: $$H^{\text{full}}=\left[\begin{array}{ccc}{H}_{aa}^{\text{full}}& H_{ab}^{\text{full}}& \\ H_{ba}^{\text{full}}& H_{bb}^{\text{full}}& H_{bc}^{\text{full}}\\ & H_{cb}^{\text{full}}& H_{cc}^{\text{full}}\end{array}\right]=\left[\begin{array}{ccc}\overline{\mathrm{\Omega }}+\widehat{\Omega }& -\widehat{\Omega }& \\ -\widehat{\Omega }& H_{bb}^{\text{small}}+\widehat{\Omega }& H_{bc}^{\text{small}}\\ & H_{cb}^{\text{small}}& H_{cc}^{\text{small}}\end{array}\right]$$

und dem Koeffizientenvektor $$b^{full}=\left[\begin{array}{l}{b}_a^{\text{full}}\\ b_b^{\text{full}}\\ b_c^{\text{full}}\end{array}\right]=\left[\begin{array}{c}{\displaystyle \sum {}_i{\overline{\mathrm{\Omega }}}^i{e}_{\overline{i},a}+{\displaystyle \sum {}_j{\widehat{\Omega }}_{a,b}^j{e}_{a,b}}}\\ b_b^{\text{small}}+{\displaystyle \sum {}_j\mathrm{-}{\widehat{\Omega }}_{a,b}^j{e}_{a,b}}\\ b_c^{\text{small}}\end{array}\right],$$

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^{\text{rel}}={\displaystyle \sum {}_j{\widehat{\Omega }}_{a,b}^j{e}_{a,b}}.$

In addition, there should be one or more edges 672 between X _a and X _b with the information matrices ${\widehat{\Omega }}_{a,b}^j$

so that $\widehat{\Omega }={\displaystyle \sum {}_j{\widehat{\Omega }}_{a,b}^j}.$

The resulting pose graph 661 $G^{\text{full}}$

for the example of 13 is defined by the system matrix 901: $$H^{\text{full}}=\left[\begin{array}{ccc}{H}_{aa}^{\text{full}}& H_{ab}^{\text{full}}& \\ H_{ba}^{\text{full}}& H_{bb}^{\text{full}}& H_{bc}^{\text{full}}\\ & H_{cb}^{\text{full}}& H_{cc}^{\text{full}}\end{array}\right]=\left[\begin{array}{ccc}\overline{\mathrm{\Omega }}+\widehat{\Omega }& -\widehat{\Omega }& \\ -\widehat{\Omega }& H_{bb}^{\text{small}}+\widehat{\Omega }& H_{bc}^{\text{small}}\\ & H_{cb}^{\text{small}}& H_{cc}^{\text{small}}\end{array}\right]$$

and the coefficient vector $$b^{eull}=\left[\begin{array}{l}{b}_a^{\text{full}}\\ b_b^{\text{full}}\\ b_c^{\text{full}}\end{array}\right]=\left[\begin{array}{c}{\displaystyle \sum {}_i{\overline{\mathrm{\Omega }}}^i{e}_{\overline{i},a}+{\displaystyle \sum {}_j{\widehat{\Omega }}_{a,b}^j{e}_{a,b}}}\\ b_b^{\text{small}}+{\displaystyle \sum {}_j\mathrm{-}{\widehat{\Omega }}_{a,b}^j{e}_{a,b}}\\ b_c^{\text{small}}\end{array}\right],$$

where e _i̅, a is the error between the fixed nodes x̅ _i for the absolute position data 605, 606 and the node 971 to be optimized X _a . For the sake of simplicity, all boundary conditions based on odometry position data 615 are abbreviated to $b^{\text{rel}}={\displaystyle \sum {}_j{\widehat{\Omega }}_{a,b}^j{e}_{a,b}}.$

The conventional way to “marginalize away” node 971, X _a , is to compute the Schur complement on 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 to $$H_{bb}^{\text{margin}}=H_{bb}^{\text{small}}+\widehat{\Omega }-\widehat{\Omega }{\left(\overline{\Omega }+\widehat{\Omega }\right)}^{-1}\widehat{\Omega }$$

The corresponding marginalized coefficient vector is given by $$b^{\text{margin}}=\left[\begin{array}{l}{b}_b^{\text{full}}+\widehat{\Omega }{\left(\overline{\mathrm{\Omega }}+\widehat{\Omega }\right)}^{-1}{b}_a^{\text{full}}\\ b_c^{\text{small}}\end{array}\right].$$

The partial result so far indicates how the system matrix 901 can look after marginalization. This is used to determine the fixed node 972 (cf. 14 ).

In such a case, the fixed node 972 may represent the Schur complement 972 of the node 971.

An equation is derived below that can be used to calculate the pose and uncertainty of the fixed node 972, x̅ _p in order to insert it into the pose graph 661 instead of the node 971, X _a . If either node 971, X _a is not connected to at least one fixed node that corresponds to absolute position data 605, 606, or if node 971, X _a is not connected to the neighboring node 671, X _b via an edge 672, node 971, X _a does not influence the node 671, X _b . In this case, no fixed node 972 needs to be calculated, since the marginalization is also equivalent to simply deleting the node 971. In the general case, however, this is not the case.

erhalten (siehe 14). Der vorangestellte Fixknoten 972, X_p führt zu einem zusätzlichen Summanden in der Systemmatrix H^prior von ${\mathcal{G}}^{prior}:$

. $$H^{\text{prior}}=\left[\begin{array}{cc}{H}_{bb}^{\text{small}}+{\overline{\Omega }}_p& H_{bc}^{\text{small}}\\ H_{cb}^{\text{small}}& H_{cc}^{\text{small}}\end{array}\right].$$

First, the node 971, X _a and all associated fixed nodes that correspond to absolute position data 605, 606, and all edges 672 between the node 971, X _a and the node 671, X _b , are removed. If the fixed node 972, x̅ _p is now connected to the information matrix Ω ^̅ _p to the node 671, X _b , the graph is $G^{\text{prior}}$

received (see 14 ). The preceding fixed node 972, X _p leads to an additional summand in the system matrix H ^prior of ${\mathcal{G}}^{priOr}:$

. $$H^{\text{prior}}=\left[\begin{array}{cc}{H}_{bb}^{\text{small}}+{\overline{\Omega }}_p& H_{bc}^{\text{small}}\\ H_{cb}^{\text{small}}& H_{cc}^{\text{small}}\end{array}\right].$$

ä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: $$\begin{array}{rr}\hfill \widehat{\Omega }-\widehat{\Omega }{\left(\overline{\Omega }+\widehat{\Omega }\right)}^{-1}\widehat{\Omega }& \hfill \\ \hfill \iff \widehat{\Omega }-\widehat{\Omega }{\left(\overline{\Omega }+\widehat{\Omega }\right)}^{-1}\widehat{\Omega }& \hfill \\ \hfill \iff \widehat{\Omega }{\left(\widehat{\Omega }+\overline{\Omega }\right)}^{-1}\left(\widehat{\Omega }+\overline{\Omega }\right)& \hfill \\ \hfill \iff \widehat{\Omega }& \hfill \end{array}\begin{array}{ll}=\hfill & {\left({\widehat{\Omega }}^{-1}+{\overline{\Omega }}^{-1}\right)}^{-1}\widehat{\Omega }\hfill \\ =\hfill & \overline{\Omega }{\left(\widehat{\Omega }+\overline{\Omega }\right)}^{-1}\widehat{\Omega }\hfill \\ =\hfill & \widehat{\Omega }\hfill \\ =\hfill & \widehat{\Omega }.\hfill \end{array}$$

Now it is shown that the choice of $${\overline{\Omega }}_p^{-1}={\widehat{\Omega }}^{-1}+{\widehat{\Omega }}^{-1}$$

is equivalent to applying the Schur complement with respect to the resulting uncertainty. Equivalence is required between the upper left blocks of H ^marg and H ^prior : $$\begin{array}{rr}\hfill \widehat{\Omega }-\widehat{\Omega }{\left(\overline{\Omega }+\widehat{\Omega }\right)}^{-1}\widehat{\Omega }& \hfill \\ \hfill \iff \widehat{\Omega }-\widehat{\Omega }{\left(\overline{\Omega }+\widehat{\Omega }\right)}^{-1}\widehat{\Omega }& \hfill \\ \hfill \iff \widehat{\Omega }{\left(\widehat{\Omega }+\overline{\Omega }\right)}^{-1}\left(\widehat{\Omega }+\overline{\Omega }\right)& \hfill \\ \hfill \iff \widehat{\Omega }& \hfill \end{array}\begin{array}{ll}=\hfill & {\left({\widehat{\Omega }}^{-1}+{\overline{\Omega }}^{-1}\right)}^{-1}\widehat{\Omega }\hfill \\ =\hfill & \overline{\Omega }{\left(\widehat{\Omega }+\overline{\Omega }\right)}^{-1}\widehat{\Omega }\hfill \\ =\hfill & \widehat{\Omega }\hfill \\ =\hfill & \widehat{\Omega }.\hfill \end{array}$$

This derivation shows that the above choice for Ω ^̅ _p satisfies our requirements. The next question is with which pose estimate or position of the fixed node 972, x̅ _p is initialized.

Now the position x̅ _p of the fixed node 972, x̅ _p is determined in such a way that all nodes 671 are estimated to have the same positions after marginalization as before. For this, equality between b ^marg and b ^prior is required. A more detailed analysis of b ^marg shows $$b^{\text{margin}}=\left[\begin{array}{l}{b}_b^{\text{small}}+b^{\text{rel}}+\widehat{\Omega }{\left(\overline{\Omega }+\widehat{\Omega }\right)}^{-1}{b}_a^{\text{full}}\\ {\text{b}}_c^{\text{small}}\end{array}\right].$$

Adding a fixed node 972, x̅ _p with the pose estimate x̅ _p and uncertainty Ω ^̅ _p ^-1 =Ω ^̅-1 + Ω ^̂-1 results in $$b^{\text{prior}}=\left[\begin{array}{l}{b}_b^{\text{small}}+{\overline{\Omega }}_p\left(x_b-{\overline{x}}_p\right)\\ {\text{b}}_c^{\text{small}}\end{array}\right].$$

Requiring equality between b ^marg and b ^prior let us solve for x̅ _p : $${\overline{x}}_p=x_b-{\overline{\Omega }}_p^{-1}\left[b_b^{\text{margin}}-b_b^{\text{small}}\right].$$

This is the analytical solution for x̅ _p . This allows the fixed node 972, x̅ _p , namely its uncertainty and its position, to be constructed. Attaching the fixed node 972 with this position and uncertainty to the Posen graph 661 leads to the same effects as applying marginalization using the Schur complement.

The position of the fixed node 972 calculated according to equation 11 may generally differ from the position of the deleted node 971.

Since the fixed node 972 is stored transparently in the pose graph 661, it may be possible to change various properties of the fixed node 972 before re-performing an optimization. For example, it would be possible to adjust the loss function of the edge 973 and/or the uncertainty of the fixed node 972. Examples of loss functions include: Huber loss function and m-estimator. Such an adjustment can make it possible to achieve particularly accurate sensor fusion results. For example, the optimization can take the fixed node 972 into account more the lower its uncertainty. In general, the location result after optimizing the fixed node 972 can be closer to sources that have a lower uncertainty. The loss function is a technique that makes it possible to change the uncertainty. Based on how well certain position data fits with other position data, the uncertainty can be scaled.

A loss function is just one example of how to scale the uncertainty. Other techniques for determining this uncertainty include, for example, map-based techniques: the prior knowledge about a position, which is expressed by the fixed node 972, can be plotted on a road map. It can then be analyzed how well this prior knowledge fits the map. If the fixed node 972 is located next to the road, for example in a lake, then the uncertainty of this fixed node 972 would be greatly increased. This is the case because the corresponding position is implausible. The uncertainty would be assessed differently if the position of the fixed node 972 is on a lane and this lane is assigned to a road that was traveled on in the past. Then, for example, the uncertainty would not be increased. It is therefore possible to adjust the uncertainty taking into account expert knowledge and/or context knowledge.

From the above it can be seen that the pose graph 661 can be marginalized with respect to the node 971 by determining the fixed node 972. In the example which is shown with respect to the 12-14 described, node 971 was selected, which corresponds to the oldest time t.1. In other cases, however, another node 671, 971 could be selected.

The techniques described above with reference to the 12-14 described above may be executed repeatedly one after the other to remove further nodes 671. For example, with respect to 15 a scenario is shown in which the node 671, x _b was selected and removed and another fixed node 972 was added to the pose graph 661.

16 is a flow chart of a method according to various embodiments. First, in step 1001, a pose graph 661 is generated. In particular, in step 1001, a pose graph 661 with a chain structure can be generated, as described above.

Then, in step 1002, the pose graph 661 is optimized. The position of the nodes 671 can be changed. The optimization takes into account boundary conditions that are determined by the edges 672 or the fixed nodes that correspond to the absolute position data 605, 606. During optimization, the pose of fixed nodes is not changed.

Subsequently, in step 1003, it is checked whether further position data 605, 606, 615 have become available in the meantime. For example, an interim sampling rate with which the position of their systems 101, 102, 111, 112 provide raw position data 105, 106, 115, 116 can be taken into account.

If new position data 605, 606, 615 are available, a new node 671 and/or a new edge 672 is added to the pose graph 661, see step 1004.

Then, in step 1005, it is checked whether a marginalization of the pose graph 661 should be carried out. For example, in step 1005, a number of nodes 671 of the pose graph 661 can be taken into account. If, for example, the number exceeds a predetermined threshold value, the marginalization can be carried out.

If it is determined in step 1005 that the marginalization should not be performed, steps 1002-1004 are performed again. Otherwise, step 1006 is performed. In step 1006, the fixed node 972 is determined for a selected node 971 and the selected node 971 is removed from the pose graph 661. The fixed node 972 is added to the pose graph 661. Then the optimization is performed again in step 1002, whereby the pose of the fixed node 972 is not changed during optimization.

Der erstere wiederum beinhaltet b^relI, der aus den Nebenbedingungen der Odometriekanten, die mit dem zu marginalisierenden Knoten verbunden sind, gewonnen wurden. Außerdem enthält er $b_a^{full}$

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 connected to the selected node; absolute position data 605, 606 corresponding to the selected node 971 and; optimization nodes to which the selected node 971 is directly connected (see equation 11). Equation 11 contains the terms $b_b^{marG}und{b}_b^{small}$

The former in turn contains b ^relI , which is obtained from the constraints of the odometry edges connected to the node to be marginalized. It also contains $b_a^{eull}$

which was obtained from the fixed nodes and the odometry edges that are connected to the node to be removed. It would also be possible for the fixed node 972 to be determined based on another fixed node that is connected to the selected node 971 via an edge 973 (cf. 14 and 15 ).

Of course, the features of the embodiments and aspects of the invention described above can be combined with one another. In particular, the features can be used not only in the combinations described, but also in other combinations or on their own, without departing from the scope of the invention.

For example, various examples were described above with regard to obtaining the estimated position from optimizing the pose graph. Alternatively or additionally, it may be possible to obtain an estimated orientation or odometry of the motor vehicle from optimizing the pose graph and to take this into account, for example, when controlling the driver assistance functionality.

For example, various examples were described above with regard to controlling a driver assistance functionality for autonomous driving. Alternatively or additionally, other driver assistance functionalities can also be controlled, e.g. partially or highly automated driving solutions, etc.

List of reference symbols

t.1 - t.13

time

1 - 5

Step

101

Absolute positioning system

102

Absolute positioning system

111

Odometry positioning system

112

Odometry positioning system

113

Odometry positioning system

121

interface

122

interface

123

processor

124

memory

129

Driver assistance

151

Location fusion

152

Graph management

153

Backend, optimization

190

Actual position

191

reliability

201

Time interval

202

Sampling rate

203

Interruption

105, 106

Raw absolute position data

115, 116

Raw odometry position data

615

Odometry position data

661

Pose graph

662

interpolation

671

node

672

Edge

605, 606

Absolute position data

901

System matrix

902, 903

Entries of the system matrix

971

Selected node

972

Fixed knot

973

Edge

1001-1006

Step

Claims (10)

Method for marginalizing a pose graph (661), comprising: - for each of a plurality of points in time (t.1 - t.12): determining odometry position data (615) of a movable machine based on an output of at least one odometry positioning system (111, 112), - for each of the plurality of points in time (t.1 - t.12): determining absolute position data (605, 606) of the machine based on an output of at least one absolute positioning system (101, 102), - generating (152) the pose graph (661) with Edges (672) corresponding to the odometry position data (615) and with nodes (671, 971) corresponding to the absolute position data (605, 606), - optimizing (153, 1002) the pose 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 pose graph (661), - for the selected node (971): determining a fixed node (972) of the pose graph (661), - removing the selected node (971) from the pose graph (661) and replacing the selected node (971) with the fixed node (972). Procedure according to Claim 1 , characterized in that the fixed node (972) is determined based on elements selected from the following group: the position of the selected node (971); at least one edge (672) connected to the selected node (971); absolute position data (605, 606) corresponding to the selected node (971); and the position of another fixed node (972) connected to the selected node (971) via an edge (973). Procedure according to Claim 1 or 2 , characterized in that the fixed node (972) represents 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 and replacing the selected node (971) by the fixed node (972): re-optimizing (153, 1002) the pose graph (661) to obtain a further estimated position (190) of the machine, wherein the position of the fixed node (972) is invariant with respect to the re-optimization, wherein the fixed node (972) is connected via an edge (973) to an adjacent node (671) of the pose graph (661). Procedure 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 pose graph (661) which runs between the fixed node (972) and the adjacent 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 pose graph (661) is selected which corresponds to the oldest point in time (t. 1-t. 12). Method according to one of the preceding claims, characterized in that the removal and replacement of the selected node (971) by the fixed node (972) is carried out selectively depending on a number of nodes (671, 971) of the pose graph (661). 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 device (120) comprising: - at least one interface (121, 122) which is configured to receive outputs from 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 configured to determine odometry position data (615) of a machine for each of a plurality of points in time (t.1 - t.12) based on the outputs of the at least one odometry positioning system (111, 112), wherein the at least one processor (123) is further configured to determine absolute position data (605, 606) of the machine for each of the plurality of points in time (t.1 - t.12) based on the outputs of the at least one absolute positioning system (101, 102), wherein the at least one processor (123) is further configured, to generate a pose graph (661), wherein edges (672) of the pose graph (661) correspond to the odometry position data (615), wherein nodes (671) of the pose graph (661) correspond to the absolute position data (605, 606), wherein the at least one processor (123) is further configured to optimize the pose graph (661) to obtain an estimated position (190) of the machine (153, 1002), characterized in that the at least one processor (123) is further configured to select a node (971) of the pose graph (661) after the optimization (153, 1002), to determine a fixed node (972) of the pose graph (661) for the selected node (971), to remove the selected node (971) from the pose graph (661) and to delete the selected node (971) by the fixed node (972). Control unit (120) to Claim 9 , wherein the control device (120) is configured to carry out a method according to one of the Claims 2 - 8 to carry out.

Images