GRID MAP OBSTACLE DETECTION METHOD FUSING PROBABILITY AND HEIGHT INFORMATION

Abstract

The present invention discloses a grid map obstacle detection method fusing probability and height information, and belongs to the field of image processing and computer vision. A high-performance computing platform is constructed by using a GPU, and a high-performance solving algorithm is constructed to obtain obstacle information in a map. The system is easy to construct, the program is simple, and is easy to implement. The positions of obstacles are acquired in a multi-layer grid map by fusing probability and height information, so the robustness is high and the precision is high.

Claims

1. A grid map obstacle detection method fusing probability and height information, comprising the following steps: 1) Bayesian inference in the case where the existing state of a grid and the grid stat of this measurement are known, obtaining the grid state after this measurement by Bayesian inference, and if the probability P.sub.i of the grid is greater than P.sub.min, restoring virtual points of the grid using an H map; 2) extracting virtual points, restoring three-dimensional boundary calculating the X and Z coordinates of the grid according to the position where the grid is located; restoring all highest points of the grid according to the H map, which are called virtual points in the algorithm; and if the probability corresponding to the grid is less than P.sub.min or the Y coordinate is less than h.sub.min, not restoring; 3) clustering virtual points 3-1) dividing field of view, selecting initial values using sliding window first, dividing the current field of view into f parts, and then, sampling in each part using a sliding window; setting the sliding window to W and the size of the window to a×b; if the number N.sub.w of grids with virtual points in the sliding window is greater than N.sub.min, selecting the virtual points of the grid with the maximum probability in the sliding window as alternative initial values C.sub.i, the probability value being P.sub.i; 3-2) consolidating initial values if there is a weighted distance between the alternative initial values C.sub.i and C.sub.j, and the weight value is probability, setting the two to the same category k within d.sub.1; if the probability of some virtual points C.sub.k.sup.m in the category k is obviously greater than that of other points C.sub.k.sup.n, that is, P.sub.k.sup.m>P.sub.k.sup.n+ε, ε being a constant less than 1 and greater than 0, using the points with higher probability to calculate a category center C.sub.k, otherwise, using all points; taking the probability of the virtual points as the weight, the category center C.sub.k being the weighted mean $\frac{1}{\Sigma P_i}.Math.P_i{C}_i$  of the selected virtual points, the center probability P.sub.k being $\frac{1}{n}.Math.P_i;$  stopping after K centers are selected; 3-3) clustering and extracting bounding boxes using the KMeans clustering algorithm for all virtual points, calculating the weighted distance P.sub.k|C.sub.i−C.sub.k|.sub.p from all virtual points to the clustering center C.sub.k in the clustering process, ∥.sub.p being a p norm, if the weighted distance from a certain virtual point to any clustering center exceeds d.sub.max, eliminating the virtual point; when updating the clustering center, calculate the weighted mean of all samples in the category as a new clustering center, the clustering center being the probability mean of virtual points in the category; extracting bounding boxes of all categories after clustering, the bounding boxes being described respectively by 7 values: clustering center C.sub.k, probability P.sub.k, maximum Y coordinate Y.sub.max, maximum and minimum grid serial numbers in the X direction X.sub.max and X.sub.min, and maximum and minimum grid serial numbers in the Z direction Z.sub.max and Z.sub.min; 3-4) merging categories 3-5) correcting bounding boxes after merging, there may be two situations where the bounding boxes are overlapped and the bounding boxes are separated; if the bounding boxes are overlapped, traversing the bounding boxes and eliminating overlapping regions; and if the bounding boxes are separated, checking the bounding boxes around empty regions, incorporating if the sum of increase of a bounding box in the X and Z directions does not exceed T when an empty region is incorporated into the bounding box, otherwise, checking empty regions adjacent to the empty region, if the number of empty regions that cannot be incorporated exceeds num, creating a new bounding box around these empty regions, the bounding box being described by 5 values: center B.sub.i, maximum and minimum grid serial numbers in the X direction X.sub.max and X.sub.min, and maximum and minimum grid serial numbers in the Z direction Z.sub.max and Z.sub.min.

2. The grid map obstacle detection method fusing probability and height information according to claim 1, wherein P.sub.min, h.sub.min, N.sub.min, d.sub.1, d.sub.2, d.sub.3, p.sub.1, T and num in the method are set thresholds.

3. The grid map obstacle detection method fusing probability and height information according to claim 1, wherein in the step 3-4) of merging categories: if the weighted distance between probabilities P.sub.i and P.sub.j of centers C.sub.i and C.sub.j of two categories is less than d.sub.2, merging the two categories, and if the distance between the bounding boxes of the two categories in one of the X and Z directions is less than d.sub.3, merging the two categories.

Description

DESCRIPTION OF DRAWINGS

[0024] FIG. 1 shows the overall flow of the algorithm.

[0025] FIG. 2 shows the detailed flow of clustering inference.

[0026] FIG. 3 shows the input grid map.

[0027] FIG. 4(a) shows clustering result, and FIG. 4(b) shows merged result.

DETAILED DESCRIPTION

[0028] The present invention proposes a grid map obstacle detection method fusing probability and height information, which is described in detail with reference to the accompanying drawings and embodiments as follows:

[0029] The overall flow is shown in FIG. 1. First, inputting a grid map, FIG. 3 representing a simulation map of the input grid map, which may be divided into two parts: a probabilistic grid map (P map) and an elevation grid map (H map). Then, updating the grid state using the P map in combination with Bayesian inference, and completing obstacle clustering detection using the H map and the updated result. Finally, outputting results.

[0030] In order to illustrate that the specific algorithm, there are the following settings in the present invention: setting that there is a spatial rectangular coordinate system XYZ, the X axis is horizontal to the right, the Y axis is upright, and the Z axis is forward; and setting that the grid map is created on the XOZ plane, and XOZ reflects the current horizontal plane. P represents a probabilistic grid map, and H represents an elevation grid map. On this basis, the grid map obstacle detection method is described. As shown in FIG. 2, the method comprises the following steps:

[0031] 1) Bayesian Inference

[0032] As shown in FIG. 2, only the P map enters the Bayesian inference stage. Bayesian inference refers to combining the grid state at time t−1 with the grid state measured at current time t, to obtain the measured grid state. In Bayesian inference, there is a need to obtain the posterior probability p(m|z.sub.1:t) of each grid, where p(m) represents the probability of map initialization, and p(z.sub.1:t) represents the probability from 1 to time t, the form of log-odds ratio is applied to Bayesian inference, so a time recursion form may be formed, and the local map probability value at the time t is finally calculated as

[00001]$p\left(m.Math.z_{1\text{:}t}\right)=1\frac{1}{1+\exp \left\{l_t\right\}},\mathrm{where}$$l_t=\log \frac{p\left(m.Math.z_{1\text{:}t}\right)}{1-p\left(m.Math.z_{1\text{:}t}\right)}.$

If the probability of the grid is greater than P.sub.min, restoring virtual points of the grid using an H map.

[0033] 2) Extracting Virtual Points, Restoring Three-Dimensional Boundary

[0034] As shown in FIG. 2, the H map and the updated grid state are jointly used to extract virtual points. The H map includes height information about the grid, namely a Y coordinate. Calculating the X and Z coordinates of the grid according to the position where the grid is located. Restoring the highest points of the grid according to the H map, which are called virtual point in the algorithm, the virtual points describing the upper boundary in the scenario. If the probability corresponding to the grid is less than P.sub.min or the Y coordinate is less than h.sub.min, not restoring.

[0035] 3) Clustering Virtual Points

[0036] 3-1) Dividing Field of View, Selecting Initial Values Using Sliding Window

[0037] First, dividing the current field of view into f parts, and then, sampling in each part using a sliding window. Setting the sliding window to W and the size of the window to a×b. If the number N.sub.w of grids with virtual points in the sliding window is greater than N.sub.min, selecting the virtual points of the grid with the maximum probability in the sliding window as alternative initial values C.sub.i, the probability value being P.sub.i.

[0038] 3-2) Consolidating Initial Values

[0039] As shown in FIG. 2, after using the obtained alternative initial values, entering the stage of consolidating the initial values. If there is a weighted distance between the alternative initial values C.sub.i and C.sub.j, and the weight value is probability, setting the two to the same category k within d.sub.1; if the probability of some virtual points C.sub.k.sup.m in the category k is obviously greater than that of other points C.sub.k.sup.n, that is, P.sub.k.sup.m>P.sub.k.sup.n+ε, ε being a constant less than 1 and greater than 0, using the points with higher probability to calculate a category center C.sub.k, otherwise, using all points. Taking the probability of the virtual points as the weight, the category center C.sub.k being the weighted mean

[00002]$\frac{1}{\Sigma P_i}.Math.P_i{C}_i$

of the selected virtual points, the probability P.sub.k of the center being

[00003]$\frac{1}{n}.Math.P_i.$

Stopping after K centers are selected.

[0040] 3-3) Clustering and Extracting Bounding Boxes

[0041] Entering the clustering stage after selecting the initial values, using the KMeans clustering algorithm for all virtual points, calculating the weighted distance P.sub.k|C.sub.i−C.sub.k|.sub.p from all virtual points to the clustering center C.sub.k in the clustering process, ∥.sub.p being a p norm, if the weighted distance from a certain virtual point to any clustering center exceeds d.sub.max, eliminating the virtual point. When updating the clustering center, calculating the weighted mean of all samples in the category as a new clustering center, the clustering center being the probability mean of all virtual points in the category. Extracting bounding boxes of all categories after clustering, the bounding boxes being described respectively by 7 values: clustering center C.sub.k, probability P.sub.k, maximum Y coordinate Y.sub.max, maximum and minimum grid serial numbers in the X direction X.sub.max and X.sub.min, and maximum and minimum grid serial numbers in the Z direction Z.sub.max and Z.sub.min; visualizing the bounding boxes on the XOZ plane. FIG. 4(a) shows the expected effect in this stage.

[0042] As shown in FIG. 2, the last two stages of the clustering algorithm are secondary aggregation stages, including merging categories and correcting bounding boxes.

[0043] 3-4) Merging Categories

[0044] After KMeans clustering, quite close categories may occur, so a merging operation is required at this time. If the weighted distance between probabilities P.sub.i and P.sub.j of centers C.sub.i and C.sub.j of two categories is less than d.sub.2, merging the two categories, and if the distance between the bounding boxes of the two categories in one of the X and Z directions is less than d.sub.3, merging the two categories.

[0045] 3-5) Correcting Bounding Boxes

[0046] After merging, there may be two situations where the bounding boxes are overlapped and the bounding boxes are separated. If the bounding boxes are overlapped, traversing the bounding boxes and eliminating overlapping regions; and if the bounding boxes are separated, checking the bounding boxes around empty regions, incorporating if the sum of increase of a bounding box in the X and Z directions does not exceed T when an empty region is incorporated into the bounding box, otherwise, checking empty regions adjacent to the empty region, if the number of empty regions that cannot be incorporated exceeds num, creating a new bounding box around these empty regions, the bounding box being described by 5 values: center B.sub.i, maximum and minimum grid serial numbers in the X direction X.sub.max and X.sub.min, and maximum and minimum grid serial numbers in the Z direction Z.sub.max and Z.sub.min, the new bounding box being called a fuzzy obstacle because the probability value of the empty region is extremely low. Such box is used to mark regions, as shown in FIG. 4(b).

[0047] P.sub.min, h.sub.min, N.sub.min, d.sub.1, d.sub.2, d.sub.3, p.sub.1, T and num in the algorithm are set thresholds.