METHODS AND SYSTEMS FOR DETERMINING AN INITIAL EGO-POSE FOR INITIALIZATION OF SELF-LOCALIZATION

Abstract

A computer implemented method for determining an initial ego-pose for initialization of self-localization comprises the following steps carried out by computer hardware components: providing a plurality of particles in a map; grouping the particles in a plurality of clusters; performing particle filtering individually for each of the clusters; and determining an initial ego-pose based on the particle filtering.

Claims

1. A computer implemented method for determining an initial ego-pose for initialization of self-localization, the method comprising: providing a plurality of particles in a map; grouping the particles in a plurality of clusters; performing particle filtering individually for each of the clusters; and determining an initial ego-pose based on the particle filtering.

2. The computer implemented method of claim 1, wherein the particle filtering is performed individually for each of the clusters in parallel.

3. The computer implemented method of claim 1, wherein providing the plurality of particles is based on a random distribution over the map.

4. The computer implemented method of claim 1, wherein providing the plurality of particles is based on an estimate of the ego-pose.

5. The computer implemented method of claim 1, wherein performing the particle filtering comprises: sample distribution, prediction, updating, and re-sampling.

6. The computer implemented method of claim 1, wherein grouping the particles into the plurality of clusters is based on at least one of a number of particles in a potential cluster or a number of potential clusters.

7. The computer implemented method of claim 1, comprising exhausting a cluster if it is outside a region of interest.

8. The computer implemented method of claim 1, comprising exhausting a particle of a cluster if the particle is outside a region of interest.

9. The computer implemented method of claim 1, comprising receiving electromagnetic radiation emitted from at least one emitter of a sensor system of a vehicle and reflected in a vicinity of the vehicle towards the sensor system.

10. The computer implemented method of claim 9, wherein performing the particle filtering is based on the received electromagnetic radiation and based on the map.

11. The computer implemented method of claim 1, wherein determining the initial ego-pose is based on at least one of a pre-determined number threshold for the number of clusters or a pre-determined size threshold for the respective spatial sizes of the clusters.

12. The computer implemented method of claim 1, wherein determining the initial ego-pose is based on entropy based monitoring based on a binary grid.

13. A computer system configured to carry out the computer implemented method of claim 1.

14. A vehicle, comprising the computer system of claim 13; and a sensor system adapted to receive electromagnetic radiation emitted from at least one emitter and reflected in a vicinity of the vehicle towards the sensor system.

15. A non-transitory computer readable medium comprising instructions for carrying out the computer implemented method of claim 1.

Description

DRAWINGS

[0026] Exemplary embodiments and functions of the present disclosure are described herein in conjunction with the following drawings, showing schematically:

[0027] FIG. 1 is an illustration of particle clustering of a map with particles according to various embodiments;

[0028] FIG. 2 is an illustration of cluster parallel filtering according to various embodiments;

[0029] FIG. 3 is an illustration of a scenario of ego-pose initialization with particle filtering according to various embodiments in a parking lot with three generated clusters as an example of the parallel filtering according to various embodiments;

[0030] FIG. 4 is an illustration of monitoring of three generated clusters based on their effective sample size and entropy information after a clustering process according to various embodiments;

[0031] FIG. 5 is a flow diagram illustrating a method for determining an initial ego-pose for initialization of self-localization according to various embodiments.

DETAILED DESCRIPTION

[0032] According to various embodiments, a map may be used for finding the initial pose of the vehicle, i.e., where the vehicle starts to move. For example, the map may be an OpenStreetMap and/or occupancy grid map. The map may include information on static objects, such as walls, pillars, tress, houses or guard rails. Information indicated by the map may be provided on a discrete grid (so that the map may also be referred to as a grid). Particle filtering may be used for finding the initial pose of the vehicle. A map may be input into the particle filter and then the filter may be initialized. The initialization process may be the distribution of the samples (in other words: particles) in the entire region, where the initial ego-pose is unknown. While theoretically the region could be the whole world, usually some coarse information about the initial ego-pose is available, such as “The vehicle is in a parking garage” or “The vehicle is in this area of the city”.

[0033] Based on this initial coarse information, samples may be distributed within the map of the area and particle filtering may be performed. Particle filtering may have the following steps: filter initialization (in other words: sample distribution), prediction, updating (in other words: weighting), and re-sampling, like will be described in more detail below.

[0034] After initialization, the movement of each particle may be predicted based on the vehicle movement information (for example yaw rate and velocity) and a vehicle model. Based on the updated sample poses, each sample may be weighted based on a comparison between the sensor observation (for example radar, camera, or LiDAR) and the map. Due to this weighting, some samples may get a higher weight than the other samples. The domination of the particles with a higher weight to the other samples may lead to a problem, called “degeneracy”. To avoid this problem, re-sampling may be performed which focuses the samples to the regions where the sample weights are higher, since the vehicle is more likely to be located in these regions. After some sample times, the particles are more concentrated in one region and the initial ego-pose is considered as found. The size of the recognized area can be defined by the user, for example the user can define an area of 5 m.sup.2 for the initialization success. If all particles are concentrated in an area smaller or equal to that value, the filtering process for the initialization may be considered done.

[0035] However, some factors may lead to filtering problems or even filter divergence, which means that the filter converges to a wrong initial pose. Some common problems, which may be faced during the filtering process are impoverishment (which refers to a fast and high concentration of the particles in a small region), degeneracy (which refers to a situation where the weights of many samples are close to zero, so that there is a large difference between sample weights), or filter divergence (which refers to a complete divergence of the filter, so that initialization fails).

[0036] The source of particle divergence may be sparse and noisy measurements, for example in a case of using radars, when the observations are sparse.

[0037] For avoiding the particle filter divergence, strategies such as particle injection based on different sensor system may be used, for example based on radars, LiDAR, camera or a combination of these sensors. If the divergence is recognized, new particles are injected into the filter in the entire initialization area. However, this particle injection into the filter in the entire initialization area is considered a filter reset, which should be avoided.

[0038] According to various embodiments, clustering of particles may be applied, which may overcome the divergence problem of the particle filtering in case of noisy and sparse measurements or inaccurate map. A binary grid may be provided over the entire region with a pre-determined resolution. A binary clustering may be performed for all particles in each sample time. Clusters which have a number of samples over a pre-determined threshold may be considered. The number of clusters may also have a threshold and if the number of clusters reaches the threshold, then the clusters may be tracked in parallel which is explained in more detail below. All clusters may represent the map regions where the probability of the ego-pose is high according to the measurements until the clustering time.

[0039] FIG. 1 shows an illustration 100 of particle clustering of a map 102 with particles according to various embodiments, and the generated clusters 106, 108, 110 for an example scene with several obstacles 112 (for example walls), so as to provide a clustered map 104.

[0040] The particles of the map 102 may be clustered with a binary grid (for example with a resolution of 10 cm in x direction and 10 cm in y direction). The three clusters 106, 108, 110 may be generated after the clustering process. Each cluster is considered and processed as a separate particle filter. Only particles in the clusters are considered, and particles which are not included in any cluster are not taken into consideration for particle filtering.

[0041] Each cluster may be continuously monitored by the effective sample size and entropy. If the effective sample size and entropy of one cluster meet certain conditions, considering all clusters, then the particle filter is initialized and other clusters are eliminated.

[0042] According to various embodiments, based on the clusters, a cluster parallel filtering (for parallel processing of all clusters) may be provided. Each cluster may be processed separately, after the clusters reach a certain number equal or smaller than a threshold.

[0043] FIG. 2 shows an illustration 200 of cluster parallel filtering according to various embodiments. The clusters of the left map 202 (at clustering time) may be processed independently and updated, so as to arrive at the clusters of the right map 204 (after updating clusters with motion model). The distribution of each particle cluster may change with time as a separate filter.

[0044] Each of the different clusters of the left map 202 may be processed within the tracked trajectory independently. The cluster particles may be tracked using the motion parameters and the vehicle model. The cluster size and the number of particles may be changed, depending on the re-sampling method. The clusters after processing within some sample times are illustrated in the right map 204.

[0045] According to various embodiments, all of the filtering processes (prediction, update, re-sampling) may be performed for each cluster independently from the other clusters. Clusters which move outside of the valid region may not be considered anymore and may be extinguished. The valid clusters (in valid area in which the belief is searched) may be monitored by their effective sample size and entropy in each sample time. The filter may be converged if the conditions

ESS(C.sub.i)>k.sub.1SS(C.sub.i)

ESS(C.sub.i)>k.sub.2ESS(C.sub.j)

are fulfilled for the cluster i, wherein SS may be the sample size, ESS may be the effective sample size, k.sub.1 and k.sub.2 may be thresholds, 1<i, j<N.sub.clusters, and N.sub.clusters may be the number of clusters.

[0046] With the cluster parallel filtering method according to various embodiments, a divergence may be avoided in a computational efficient way. No sample are added to the filter, but the strategy may be solely to keep the samples which represent the region with high probability for the belief of the ego-pose. As described above, the clusters may not be processed as one particle filter, but each cluster may be processed separately (and, for example, in parallel). In such a way, no additional computation time may be added to the filtering process, and a filter reset may not be necessary.

[0047] FIG. 3 shows an illustration 300 of a scenario of ego-pose initialization with particle filtering according to various embodiments in a parking lot with three generated clusters (denoted as “1”, “2”, and “3”) as an example of the parallel filtering according to various embodiments.

[0048] FIG. 4 shows an illustration 400 of monitoring of three generated clusters based on their effective sample size and entropy information after a clustering process according to various embodiments. If one cluster meets the pre-defined convergence condition, the filter is initialized successfully. As an example, FIG. 4 illustrates the result of the cluster parallel filtering for the scenario of FIG. 3. The top portion of FIG. 4 shows the maximum entropies as solid lines and the entropies as dashed lines. Solid line 402 represents the maximum entropy of the first cluster, solid line 404 represents the maximum entropy of the second cluster, solid line 406 represents the maximum entropy of the third cluster, dashed line 408 represents the entropy of the first cluster, dashed line 410 represents the entropy of the second cluster, and dashed line 412 represents the entropy of the third cluster.

[0049] The bottom portion of FIG. 4 shows the effective sample sizes as solid lines and the sample size as dashed lines. Solid line 414 represents the effective sample size of the first cluster, solid line 416 represents the effective sample size of the second cluster, solid line 418 represents the effective sample size of the third cluster, dashed line 420 represents the sample size of the first cluster, dashed line 422 represents the sample size of the second cluster, and dashed line 424 represents the sample size of the third cluster.

[0050] The symmetrical form of the parking lot and accordingly the ambiguity of the observations in opposite side of the map presents a major challenge for the particle filtering. The symmetry leads to survival of the clusters within the re-sampling process which is also observable in FIG. 4. The effective sample size of the first cluster reduces continuously with time as it moves towards the map boundaries from the time 11 s. Due to the affinity of the observations on two corners of the parking lot for the second cluster and the third cluster, their samples obtain almost alike weights within the time between 10.7 s and 10.8 s. With more dense measurements from the lower right corner of the map, an effective samples size increment is observed for the second cluster.

[0051] FIG. 5 shows a flow diagram 500 illustrating a method for determining an initial ego-pose for initialization of self-localization according to various embodiments. At 502, a plurality of particles may be provided in a map. At 504, the particles may be grouped in a plurality of clusters. At 506, particle filtering may be performed individually for each of the clusters. At 508, an initial ego-pose may be determined based on the particle filtering.

[0052] According to various embodiments, the particle filtering may be performed individually for each of the clusters in parallel.

[0053] According to various embodiments, the plurality of particles may be provided based on at least one of a random distribution over the map, or an estimate of the ego-pose.

[0054] According to various embodiments, performing the particle filtering may include: sample distribution, prediction, updating, and re-sampling.

[0055] According to various embodiments, the particles may be grouped into the plurality of clusters based on at least one of a numbers of particles in a potential cluster, or a numbers of potential clusters.

[0056] According to various embodiments, a cluster may be exhausted if it is outside a region of interest. According to various embodiments, a particle of a cluster may be exhausted if the particle is outside a region of interest.

[0057] According to various embodiments, electromagnetic radiation emitted from at least one emitter of a sensor system of a vehicle and reflected in a vicinity of the vehicle towards the sensor system may be received.

[0058] According to various embodiments, the particle filtering may be performed based on the received electromagnetic radiation and based on the map.

[0059] According to various embodiments, the initial ego-position may be determined based on at least one of a pre-determined number threshold for the number of clusters or a pre-determined size threshold for the respective spatial sizes of the clusters.

[0060] According to various embodiments, the initial ego-position may be determined based on entropy based monitoring based on a binary grid.

[0061] Each of the steps 502, 504, 506, 508 and the further steps described above may be performed by computer hardware components.

[0062] It will be understood that the individual (or parallel) filtering according to various embodiments is not to be confused with parallel filtering implementation in the literature, wherein the particle filter is parallelized in the software to use the complete capacity of the processor or to map the particle filter on a graphic processing unit (GPU), and which is an implementation method to speed up the filtering process by parallel implementation.

[0063] The preceding description is illustrative rather than limiting in nature. Variations and modifications to the disclosed examples may become apparent to those skilled in the art that do not necessarily depart from the essence of this invention. The scope of legal protection given to this invention can only be determined by studying the following claims.