Graph-based method for the holistic fusion of measured data

Abstract

A method for fusing state data via a control unit. State data of a first mobile unit and of an object ascertained via a sensor system of the first mobile unit are received. State data of an object ascertained via a sensor system of a second mobile unit and/or state data of the second mobile unit, transmitted via a communication link from the second mobile unit to the first mobile unit, are received. A node is created in a time-position diagram for each set of received state data of the first mobile unit, the second mobile unit, and the objects. A data optimization of the state data ascertained by the first mobile unit and/or by the second mobile unit is carried out. An optimization problem is created based on the optimized state data ascertained by the first mobile unit and the optimized state data received from the second mobile unit.

Claims

1. A method of a first vehicle, the method comprising: obtaining, by a control unit of the first vehicle and using a sensor system of the first vehicle, a dataset including first state data that includes state data of the first vehicle and state data of at least one object detected by the sensor system of the first vehicle; receiving, by the control unit, a dataset including second state data ascertained by a second vehicle, the second state data including: state data of at least one object ascertained via a sensor system of the second vehicle, the second state data being transmitted via a communication link from the second vehicle to the first vehicle; creating, by the control unit, a respective one of a plurality of nodes in a time-position diagram for each of the datasets; carrying out, by the control unit, a data optimization of the first state data and of second the state data; and executing, by the control unit, an autonomous driving of the first vehicle based on the data optimization of the first state data and the second state data; wherein: that of the first and second state data that is respectively associated with a timestamp indicating a respective age that is above a threshold age value is removed from the time-position diagram; and the threshold age value is varied over time depending on a number of objects of the time-position diagram or depending on an available computing capacity.

2. The method as recited in claim 1, wherein the position of the first vehicle, and/or an ego position of the second vehicle, and/or the position of the at least one object ascertained via the sensor system of the first vehicle, and/or the position of the at least one object ascertained via the sensor system of the second vehicle, and/or pieces of relative distance information between the first vehicle, the second vehicle, and the at least one object, are ascertained from the first state data of the first vehicle and from the second state data of the second vehicle, the position of the first vehicle being created within the surroundings model based on the first state data and the second state data.

3. The method as recited in claim 1, wherein the optimization problem is created and solved each time further state data is received or at regular time intervals or in response to predefined events.

4. The method as recited in claim 1, wherein each of the nodes of the time-position diagram includes an identification of a respective object location and an identification of a respective object state.

5. The method as recited in claim 1, wherein the data optimization includes generating a model of positions of the at least one object and gradually modifying the positions of the model by an iterative minimization of an accumulated distance between (1) positions of all of the at least one object in the model and (2) a combination of positions of all of the at least one object respectively detected by the first vehicle and by the second vehicle.

6. The method as recited in claim 1, wherein the data optimization increases an accuracy of a position of the first vehicle used by the control unit of the first vehicle for the autonomous driving.

7. The method as recited in claim 1, wherein the varying over time of the threshold age value depends on the number of objects of the time-position diagram.

8. The method as recited in claim 1, wherein the varying over time of the threshold age value depends on the available computing capacity.

9. The method as recited in claim 1, further comprising: creating, by the control unit, an optimization problem based on the optimized first state data and based on the optimized second state data; and solving, by the control unit, the optimization problem via a solution algorithm that increases an accuracy of the first state data and the second state data, wherein the execution of the autonomous driving is performed based on the first and second state data with the increased accuracy.

10. A control unit of a first vehicle, the control unit configured to: obtain, using a sensor system of the first vehicle, a dataset including first state data that includes state data of the first vehicle and state data of at least one object detected by the sensor system of the first vehicle; receive a dataset including second state data ascertained by a second vehicle, the second state data including: state data of at least one object ascertained via a sensor system of the second vehicle, the second state data being transmitted via a communication link from the second vehicle to the first vehicle; create a respective one of a plurality of nodes in a time-position diagram for each of the datasets; carry out a data optimization of the first state data and of second the state data; and execute an autonomous driving of the first vehicle based on the data optimization of the first state data and the second state data; wherein: that of the first and second state data that is respectively associated with a timestamp indicating a respective age that is above a threshold age value is removed from the time-position diagram; and the threshold age value is varied over time depending on a number of objects of the time-position diagram or depending on an available computing capacity.

11. A non-transitory machine-readable memory medium on which is stored a computer program for a control unit of a first vehicle, the computer program being executable by a computer of the control unit and causing the computer, when executing the computer program, to perform the following steps: obtaining, using a second system of the first vehicle, a dataset including first state data that includes state data of the first vehicle and state data of at least one object detected by the sensor system of the first vehicle; receiving a dataset including second state data ascertained by a second vehicle, the second state data including: state data of at least one object ascertained via a sensor system of the second vehicle, the second state data being transmitted via a communication link from the second vehicle to the first vehicle; creating a respective one of a plurality of nodes in a time-position diagram for each of the datasets; carrying out a data optimization of the first state data and of second the state data; and executing an autonomous driving of the first vehicle based on the data optimization of the first state data and the second state data; wherein: that of the first and second state data that is respectively associated with a timestamp indicating a respective age that is above a threshold age value is removed from the time-position diagram; and the threshold age value is varied over time depending on a number of objects of the time-position diagram or depending on an available computing capacity.

Description

BRIEF DESCRIPTION OF THE DRAWINGS

(1) Preferred exemplary embodiments of the present invention are explained in greater detail below with reference to highly simplified schematic representations.

(2) FIG. 1 shows a top view of a traffic situation illustrating a method for redundantly carrying out a localization according to one specific embodiment of the present invention.

(3) FIG. 2 schematically shows a representation of a time-position diagram illustrating a method for fusing state data, in accordance with an example embodiment of the present invention.

(4) FIG. 3 shows a top view of a surroundings model having a localization inaccuracy, in accordance with an example embodiment of the present invention.

(5) FIG. 4 shows a top view of the surroundings model from FIG. 3 after an implemented data optimization, in accordance with an example embodiment of the present invention.

(6) FIG. 5 shows a top view of a surroundings model recalculated after the data optimization, in accordance with an example embodiment of the present invention.

DETAILED DESCRIPTION OF EXAMPLE EMBODIMENTS

(7) FIG. 1 shows a top view of a traffic situation 1 for illustrating a method for redundantly carrying out a localization according to one specific embodiment of the present invention.

(8) Traffic situation 1 includes a first mobile unit 2 and a second mobile unit 4. In addition, a dynamic object 6, which is designed as a vehicle, is situated in surroundings U of first mobile unit 2 and of second mobile unit 4.

(9) First mobile unit 2 and second mobile unit 4 are designed as motor vehicles and may be operated in an automated manner. For this purpose, first mobile unit 2 and second mobile unit 4 each include a sensor system 8, 10. Sensor system 8, 10 may, for example, include radar sensors, GNSS sensors, camera sensors, LIDAR sensors, and the like.

(10) Via sensor system 8, 10, it is possible to scan surroundings U and, for example, to detect object 6. In addition, mobile units 2, 4 may be mutually detected.

(11) In the illustrated exemplary embodiment, first mobile unit 2 is designed as a receiver and second mobile unit 4 is designed as a transmitter. Second mobile unit 4 may, in particular, transmit measured data ascertained by its sensor system 10 via a communication link K to first mobile unit 2. The transmitted measured data may, for example, include a position of second mobile unit 4 and/or relative positions. The relative position may, for example, be a distance A between second mobile unit 4 and first mobile unit 2, as well as between second mobile unit 4 and object 6, which is measured by sensor system 10.

(12) First mobile unit 2 may calculate its own position with the aid of the position of second mobile unit 4 received via communication link K and of relative positions A.

(13) To establish communication link K, mobile units 2, 4 include communication units 12, 14. Communication units 12, 14 are linked to respective onboard control units 16, 18 in a data-transmitting manner.

(14) Control unit 16 of first mobile unit 2 is able to receive and evaluate the measured data of sensor system 8 and transmit these or receive extraneously ascertained measured data via communication unit 12 via communication link K. Similarly, control unit 18 of second mobile unit 4 is able to receive and evaluate the measured data of sensor system 10.

(15) Control unit 16, 18 may ascertain a feature extraction 20, an ego localization 22 and nearby vehicles and objects 6 from the measured data of sensor system 8, 10.

(16) First mobile unit 2 in this case may also be designed as a transmitter and second mobile unit 4 may be designed as a receiver. Multiple first mobile units 2 and multiple second mobile units 4 may also be provided.

(17) FIG. 2 schematically shows a representation of a time-position diagram for illustrating a method for fusing state data. First mobile unit 2, second mobile unit 4 and three objects 6, 7 are represented, which are ascertained and entered in the diagram as a function of time t or the point in time of the measurement and of the relative position A. Each measurement is stored in the form of a node in the diagram and connected to older nodes. This forms a probabilistic graphic model, which may be formulated as an optimization problem and solved via a solver.

(18) In the time-position diagram, X.sup.E.sub.K represents nodes of ascertained positions of first mobile unit 2 including corresponding state data Z.sup.E.sub.K at point in time k. X.sup.V1-4.sub.K form the nodes of tracked objects 4, 6 including the ascertained state data Z.sup.V1-4.sub.K at point in time k, node X.sup.V3.sub.K mapping the position of second mobile unit 4 with state data Z.sup.V3.sub.K ascertained by second mobile unit 4. Node X.sup.V2.sub.K is detected and tracked by both mobile units 2, 4 and is dynamic object 6 from FIG. 1, for example.

(19) In a classic state estimation or localization, the object is to estimate the profile of the system state such as, for example, a position, an orientation, a velocity, an acceleration of a mobile unit 2, 4 from provided measurements of sensor system 8, 10. This is necessary because, on the one hand, the measurements are inexact and noisy and, on the other hand, some of the state variables are not directly observable and therefore must be derived. For example, the global position of a mobile unit 2, 4 without a GPS signal must be estimated based on its velocity data and acceleration data. If mobile unit 2, 4 has a sensor system 8, 10 for identifying objects 6, 7 in surroundings U, then the states of these objects 6 may also be tracked beyond time t. Moreover, objects 6, 7 may be temporarily invisible as a result of a missing detection or a temporary occlusion of sensor system 8, 10 and thus are compensated for by tracking.

(20) An estimation of the state data by tracking detected objects 6, 7 or so-called interference may take place for the ego state data of first mobile unit 2 with the aid of a recursive filter such as, for example, an extended Kalman filter, an unscented Kalman filter or a particle filter. In this case, a multi-target tracking for multiple objects 4, 6, 7 may also be implemented.

(21) For a simple case, in which merely first mobile unit 2, E with a detected object 7, V1 is considered, the probability density p may be calculated via the following equation.

(22) $p(x_{0:k}^E,x_{o:k}^{V1}.Math.z_{1:k}^E,z_{1:k}^1)=\frac{1}{c}p\left(x_0^E\right)p\left(x_0^{V1}\right)\underset{t=1}{\overset{k}{.Math.}}p(z_t^1.Math.x_t^{V1},x_t^E)p(x_t^{V1}.Math.x_{t-1}^{V1})p(z_t^E.Math.x_t^E)p(x_t^E.Math.x_{t-1}^E)$

(23) In this locally ascertained probability density p, the density of the ego state of mobile unit 2 is independent from the density of the state of the vehicle or object 7, but not vice versa. Probability density p in this case need not necessarily be calculated for an entire time period or observation time period of t=0 . . . k.

(24) If, in addition to the self-ascertained state data from sensor system 8, first mobile unit 2 then receives state data and/or sensor date via V2X communication link K from further vehicles 4, the estimation problem and the calculation of probability density p become more complicated, since it may be possible that the received state data enable conclusions to be drawn about the ego state of first mobile unit 2. Such a scenario is schematically represented in FIG. 2.

(25) In this representation, second mobile unit 4, V3 transmits measurements of ego state Z.sup.V3.sub.K in the form of cooperative awareness messages and measurements of state data Z.sup.V4.sub.K of vehicle V4 detected by it in the form of cooperative perception messages.

(26) First mobile unit 2 or the exemplary ego vehicle receives the cooperative awareness messages and the cooperative perception messages and detects further objects V2 and V1 with sensor system 8 and has measurements of its state Z.sup.E.sub.K.

(27) Since first mobile unit 2, E and second mobile unit 4, V3 both detect object 6, V2 and second mobile unit 4, V3 transmits state data Z.sup.V3.sub.K, Z.sup.V4.sub.K of its sensor system 10 to first mobile unit 2, E, the estimation of state Z.sup.E.sub.K is no longer a function exclusively of the ego measurements of first mobile unit 2, E, but also of measurements Z.sup.V3.sub.K, Z.sup.V4.sub.K of second mobile unit 4, V3 and vice versa. This means that probability density p of ego state Z.sup.E.sub.K is a function of probability density p of state Z.sup.V3.sub.K of second mobile unit 4, V3 and vice versa.

(28) This mutual dependency means that the classic multi-target tracker application requires approximations for simplifying. This approximation may be implemented via a graph-based approach.

(29) The approach may, for example, be described as follows: With each new measurement, all nodes X.sup.E,V.sub.K that are older than a particular threshold value are discarded from the instantaneous time-position diagram. This threshold value is a parameter, which may, for example, be selected as a function of an available computing capacity and of the number of objects 6, 7. In addition, the threshold value may be variable and thus may temporally vary. For each new measurement, a new node X.sup.E,V.sub.K for received objects 2, 4, 6, 7 is inserted into the graph so that time stamp k of inserted object node X.sup.E,V.sub.K corresponds to point in time t of the measurement. In a further step, an optimization problem is constructed made up of a quadratic matrix and a vector. This takes place using algorithmic adaptations based on the use of data of moved objects 2, 4, 6, 7. The optimization problem is subsequently solved using a suitable solver or a solution algorithm.

(30) The solution of the optimization problem is efficiently possible, even for high-dimensional optimization problems due to the exploitation of the sparsity of the optimization problem.

(31) Moreover, it is not absolutely necessary to have to solve the optimization problem with each new measurement. The time-position diagram may be expanded with each measurement, but solved only when this is required. This may, for example, take place at scanning points in time of control unit 16, 18, which carries out a situation analysis.

(32) The method is also able to handle delayed data. This is possible with the method according to the present invention without additional effort in that nodes X are inserted at corresponding points in times k into the time-position diagram and the time-position diagram is expanded via connections between existing nodes X and these new nodes X. The time-position diagram may thus serve as a buffer for state data and the like. Finally, it is not absolutely necessary immediately to discard nodes X that are older than the threshold value. Instead, it is possible to incorporate the previously calculated node X as prior information into the optimization problem.

(33) FIG. 3 shows a top view of a surroundings model that includes a localization inaccuracy. For example, the measurements described in FIG. 2 and received via communication link K exhibit a localization inaccuracy. The localization inaccuracy is, for example, illustrated by erroneously oriented positions of second mobile unit 4.

(34) To correctly set up the graph-based optimization problem described in FIG. 2, a correct data association must be carried out beforehand. In the process, the object measurements from ego sensor system 8 of first mobile unit 2 and the object measurements that have been received via communication link K are implemented in a surroundings model and correctly assigned to objects 24 in the surroundings model. To carry out a data optimization, an association of onboard measurements of first mobile unit 2 of perceived objects 4, 6, 7 and the object measurements received via communication link K are used via an ICP (iterative closest point)-like method. In one step, the state data or the object measurement of detected objects 4, 6, 7 as well as measuring object 2, which have been ascertained by sensor system 8 of first mobile unit 2 and/or by sensor system 10 of second mobile unit 2, are transformed into a coordinate system of the surroundings model. The entire measurement is iteratively shifted in such a way that a minimum for an accumulated distance H between objects 2, 4, 6, 7 in the measurement and objects 24 in the surroundings model is achieved. This process is illustrated in FIG. 4.

(35) As a result of the data optimization, a newly calculated surroundings model is formed, which is depicted in FIG. 5. Within the scope of the fusion, the state of vehicles 24, with which one measurement each has been associated, has been updated and applied as new objects 25 in the surroundings model.

(36) The surroundings model is subsequently used as the basis for formulating or setting up an optimization problem, which may be handled similarly to a SLAM problem, the optimization problem taking dynamic objects 24, 25 and non-static landmarks into account. In this way, a graph-based approach may also be provided with exclusively dynamic objects.