Point cloud clustering and enclosure method based on binary occupancy grid
Through the point cloud clustering and surrounding method based on binary occupancy grid, the problem of poor real-time point cloud clustering and insufficient tightness of the bounding box in the existing technology is solved, fast clustering and precise surrounding are achieved, and robot obstacle avoidance is improved real-time and accuracy.
Patent Information
- Application Number
- CN202111524987.3
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2021-12-13
- Publication Date
- 2024-11-22
- Estimated Expiration
- 2041-12-13
AI Technical Summary
In the existing robot obstacle avoidance technology, the point cloud clustering algorithm has poor real-time performance and cannot effectively capture objects with faster motion state changes in the scene. The AABB enclosure box has poor tightness, especially the inaccurate surround of obstacles with irregular geometric shapes.
Point cloud clustering and surrounding methods based on binary occupancy grid are adopted to realize point cloud clustering through polar coordinate mapping, expansion image processing and connectivity area analysis, and identify obstacles and establish a compact surrounding box.
The clustering time is greatly reduced, the real-time and clustering efficiency of moving objects are improved, the built-in enclosure box is more compact, and can more accurately respond to obstacle sizes, providing a good theoretical basis for robots to avoid obstacles.
Smart Images

Figure CN114187425B_ABST
Abstract
Description
Technical Field
[0001] The present invention belongs to the technical field of robot autonomous obstacle avoidance, and in particular relates to a point cloud clustering and encirclement method based on binary occupancy grid. Background Art
[0002] A point set in three-dimensional space is usually called a "point cloud". The smallest point cloud contains only one data point, and a high-density point cloud contains millions of data points. At present, robot obstacle avoidance mostly uses laser radar to collect the surrounding environment and obtain point cloud data. The point cloud is then segmented according to different obstacles, and several scattered points of the obstacle are grouped into a point cloud cluster to distinguish different obstacles.
[0003] In order to improve the robot's obstacle avoidance ability, it is usually necessary to cluster the point cloud data to obtain the obstacle point cloud. Existing point cloud clustering generally uses a density-based clustering algorithm, which finds high-density points and then gradually connects similar high-density points into a piece to generate various clusters. In fact, it is an iterative operation. Clustering takes a long time and has poor real-time performance. It cannot capture objects with fast motion state changes in the scene.
[0004] The AABB bounding box encloses the obstacle through the minimum hexahedron. The AABB bounding box directly uses the maximum and minimum values of the obstacle point cloud in the three-dimensional direction as vertices to establish the bounding box, but the compactness is poor, especially for irregular geometric shapes, the redundant space is large and the compactness is poor. Summary of the invention
[0005] In view of the deficiencies in the prior art, the technical problem to be solved by the present invention is to propose a point cloud clustering and encirclement method based on binary occupancy grid.
[0006] The technical solution adopted by the present invention to solve the technical problem is as follows:
[0007] A point cloud clustering and encirclement method based on binary occupancy grid, characterized in that the method comprises the following steps:
[0008] Step 1: Collect environmental point cloud data;
[0009] Step 2: remove the ground point cloud to obtain point cloud data after removing the ground point cloud;
[0010] Step 3: for the point cloud data after removing the ground point cloud, generate a polar coordinate grid in the point cloud space, and map the point cloud data to the polar coordinate grid; assign 0 / 1 to the polar coordinate grid, generate a binary occupancy grid, and map the point cloud data to the polar coordinate grid;
[0011] Step 4, dilate the binary occupancy grid to obtain a dilated image;
[0012] Step 5: Perform connected region analysis on the dilated image to obtain all connected regions. Each connected region is an obstacle point cloud, and the clustering of the obstacle point cloud is completed.
[0013] Step 6: encircling the point cloud based on the binary occupancy grid, including identifying the main direction of the obstacle point cloud, establishing a bounding box according to the main direction of the obstacle point cloud, and determining the vertex coordinates of the bounding box.
[0014] Compared with the prior art, the present invention has the following beneficial effects:
[0015] 1. For the point cloud data after removing the ground point cloud, a binary occupancy grid is established in the point cloud space. The binary occupancy grid is a binary image. Therefore, the clustering operation based on the binary occupancy grid is faster and takes less time. The reason is that, on the one hand, mapping the point cloud data to the binary occupancy grid is actually a dimensionality reduction process for the point cloud, that is, converting the three-dimensional point cloud processing into two-dimensional image processing; on the other hand, compared with conventional Euclidean clustering, this method does not require iteration, but only needs to scan the binary occupancy grid, which greatly reduces the clustering time and has good real-time performance for objects with fast motion state changes in the scene.
[0016] 2. For the ground point cloud, the point cloud space is divided into multiple fan-shaped sub-planes, and the points in the fan-shaped sub-planes are screened to select the points that are most likely to constitute the ground point cloud; then RANSAC sampling is performed on the total point set composed of these points to obtain the ground point cloud, which shortens the number of iterations of the RANSAC operation and improves the efficiency of ground point cloud segmentation.
[0017] 3. Compared with the AABB bounding box, the AABB bounding box directly uses the maximum and minimum values of the obstacle point cloud in the three-dimensional direction as the vertices to establish the bounding box. From the expression of the coordinates of each vertex of the bounding box of the present invention, it can be seen that the length and width of the bounding box established by the present application are smaller than those of the AABB bounding box. Therefore, the bounding box structure established by this method is more compact and can more accurately reflect the size of the obstacle, providing a good theoretical basis for robot obstacle avoidance. BRIEF DESCRIPTION OF THE DRAWINGS
[0018] Figure 1 is an overall flow chart of the method of the present invention;
[0019] Figure 2 is the collected environment image;
[0020] Figure 3 It is the environmental point cloud data;
[0021] Figure 4 The result diagram of mapping the point cloud data to the polar coordinate grid;
[0022] Figure 5 is the dilated image;
[0023] Figure 6 This is a schematic diagram of the expanded image marked with white pixels;
[0024] Figure 7 This is the point cloud clustering result diagram;
[0025] Figure 8 The result image of the obstacle point cloud projected onto the xy plane;
[0026] Fig. 9 Schematic diagram of the obstacle point cloud bounding box;
[0027] Fig.10 The grid image of the obstacle point cloud border;
[0028] Fig.11 Binary occupancy grid map for point cloud bounding boxes;
[0029] Fig.12 This is a schematic diagram of the main direction of the obstacle point cloud;
[0030] Fig.13 It is a schematic diagram that the main direction of the obstacle point cloud is parallel to the x-axis of the point cloud space base coordinate system;
[0031] Fig.14 It is a schematic diagram of the circumscribed rectangle of the rotated obstacle point cloud;
[0032] Fig.15 is the circumscribed rectangle after clockwise rotation;
[0033] Fig.16 It is a schematic diagram of the bounding box;
[0034] Fig.17 Schematic diagram of the positions of the vertices of the bounding box. DETAILED DESCRIPTION
[0035] The technical solution of the present invention is described in detail below in conjunction with the accompanying drawings and specific implementation methods, but the protection scope of the present application is not limited thereto.
[0036] The present invention is a point cloud clustering and encirclement method based on binary occupancy grid (hereinafter referred to as method), which comprises the following steps:
[0037] Step 1: Collect environmental point cloud data;
[0038] Use RS-LiDAR-M1 laser radar to collect environmental point cloud data. Figure 2 , 3 Environmental images and environmental point cloud data. The scene contains four obstacles, two roadblocks, a large truck, and pedestrians, numbered 1 to 4.
[0039] Step 2: remove the ground point cloud to obtain point cloud data after removing the ground point cloud;
[0040] The rectangular coordinate system of the point cloud space is transformed into a polar coordinate system to obtain a polar coordinate plane; according to formula (1), from point p g The coordinates in the xy plane (x g ,y g ) to obtain the azimuth angle θ g ;
[0041] θ g =atan2(y g ,x g ) (1)
[0042] where atan2 is the inverse tangent function that takes the quadrant into account;
[0043] The polar coordinate plane is divided into m1 sector-shaped sub-planes using equation (2), and the central angle of each sector-shaped sub-plane is Δα. In this embodiment, Δα=2°;
[0044]
[0045] According to formula (3), point p is determined g The fan-shaped sub-plane s(p g ), are the lower and upper bounds of each sector sub-plane respectively;
[0046]
[0047] In the tth (t=1,2,…,m1)th sector subplane, all points are reordered from low to high in the z direction as follows: Then we have:
[0048]
[0049] In the t-th fan-shaped sub-plane, the first r points with the lowest height in the z direction are selected. According to the characteristics of the ground point cloud, these r points are considered to be the points most likely to constitute the ground point cloud. The average height H of the first r points is calculated according to formula (5): L :
[0050]
[0051] According to formula (6), the points in the t-th fan-shaped sub-plane are screened, and all points that satisfy formula (6) constitute a point set P t ;
[0052]
[0053] in, is the point p in the t-th sector subplane g The z-direction height, H0 is the ground height, h L 、h g They are the minimum height threshold in the z direction and the ground height threshold respectively;
[0054] The point set of m1 fan-shaped sub-planes constitutes the total point set
[0055] Perform RANSAC sampling on the total point set P0: randomly select three non-collinear points p1 = (x1, y1, z1), p2 = (x2, y2, z2) and p3 = (x3, y3, z3) to form a plane model. The general mathematical form of the plane model can be written as:
[0056] A0x+B0y+C0z+D0=0 (7)
[0057] According to the three-point equation of the plane, there is only one plane passing through these three points, and its equation is written as:
[0058]
[0059] Expand the determinant of formula (8) and find the parameters of the plane model according to formula (9);
[0060]
[0061] Traverse each point in the total point set P0 and calculate the distance d from each point to the plane model according to formula (10);
[0062]
[0063] The points whose distance d is less than the RANSAC threshold are recorded as inliers. RANSAC sampling is repeated on the total point set P0 until the number of inliers reaches the set value. Sampling is stopped to obtain the ground plane model. The points contained in the ground plane model are the ground point cloud, and the ground point cloud is removed.
[0064] Step 3: for the point cloud data after removing the ground point cloud, generate a polar coordinate grid in the point cloud space, and map the point cloud data to the polar coordinate grid; assign 0 / 1 to the polar coordinate grid to generate a binary occupancy grid;
[0065] The polar coordinate plane where the point cloud space is located is divided into m2 sector-shaped areas, and the central angle of each sector-shaped area is α pl , then:
[0066]
[0067] Let the maximum radius of the sector be r max, the segmentation step is r pl , divide each sector area into n sub-areas along the radial direction, then:
[0068]
[0069] In this way, the point cloud space is divided into a polar coordinate grid containing m×n grids; in this embodiment, α pl =0.65°, r max =200, r pl =0.2;
[0070] Map the point cloud data to the polar coordinate grid, the result is as follows Figure 4 ; Since point cloud data has three-dimensional information, mapping point cloud data to polar coordinate grid is actually dimensionality reduction storage of point cloud data, that is, directly simplifying the coordinates (x, y, z) of the point to (x, y). The reason is that, on the one hand, point cloud clustering does not care about the arrangement and order of point cloud clusters in the z direction. If the x and y values of two points are the same but the z values are different, they are regarded as superimposed in the z direction, and the two points belong to the point cloud of the same obstacle; on the other hand, it can speed up the clustering speed and meet the real-time requirements of clustering algorithms in dynamically changing environments;
[0071] According to equations (13) and (14), point p is determined i =(x i ,y i ,-) belongs to the sector area s(p i );
[0072]
[0073]
[0074] in, are the lower and upper bounds of each sector area respectively;
[0075] Determine the sector area s(p i ), then calculate point p according to equations (15) and (16) i The sub-region b(p i );
[0076]
[0077]
[0078] in, are the lower and upper bounds of each sub-region respectively;
[0079] After mapping each point to the polar coordinate grid, assign a value to each grid. If the grid does not contain a point, the pixel value of the grid is 0 and the pixel color is black; if the grid contains a point, the pixel value of the grid is 1 and the pixel color is white; traverse all grids to generate a binary occupancy grid, which is actually a binary image; in this way, the processing of three-dimensional point cloud data is converted into two-dimensional image processing, and the binary occupancy grid is simple in composition, occupies a small space, and the processing difficulty is greatly reduced.
[0080] Step 4, dilate the binary occupancy grid to obtain a dilated image;
[0081] Define a square kernel of size 3×3, with the kernel origin located at the center of the square; let the pixel set of the binary occupied grid be A, the kernel pixel set be B, and the origin be the center point, then the binary occupied grid is expanded by equation (17);
[0082]
[0083] Where x represents the area; Represents the convolution operation;
[0084] The origin of the kernel starts from the origin of the binary occupancy grid, and the moving step is 1, so that the kernel moves pixel by pixel on the binary occupancy grid to scan the pixels within the kernel range; if the pixel values of all the pixels scanned by the kernel are 0, the pixel value at the anchor point in the kernel is recorded as 0, and the pixel color is black, otherwise it is recorded as 1, and the pixel color is white; traverse each pixel of the binary occupancy grid to get the following Figure 5 The dilated image shown has larger and more continuous white areas compared to the binary occupancy grid.
[0085] Step 5: Perform a connected region analysis on the dilated image to obtain all connected regions, each of which is an obstacle point cloud; specifically, the following steps are included:
[0086] 5.1) Define a four-neighborhood pixel adjacency relationship, which means starting from the target pixel, the four adjacent pixels above, below, left and right of the target pixel are detected;
[0087] 5.2) Starting from the origin of the dilated image, scan the dilated image pixel by pixel until the current scanned pixel p0 is a white pixel. Then assign a label to the position of the white pixel. The label is a natural number that increases gradually from 1 and is used to mark the white pixel. See Figure 6 ;
[0088] 5.3) The pixel point p in the neighborhood of the current scanned pixel point p0 01 ,p02 ,p 03 ,p 04 Put them into the set M in order, and get the result shown in formula (8). From left to right, they represent the order of putting them in, and the last pixel put in is p5;
[0089] M={p 01 ,p 02 ,p 03 ,p 04} (18)
[0090] 5.4) Set pixel p 04 Take out and detect, if the pixel p 04 is a white pixel, then for pixel p 04 Assign the same label as the currently scanned pixel p0 and 04 All pixels in the neighborhood are added to the set M in order, and the last pixel added is taken out to repeat the detection operation; if the pixel p 04 If it is a black pixel, then the pixel p is discarded. 04 And take out the pixel p 03 Repeat the detection operation until all the pixels in the set M are taken out. At this time, the set of points marked by label forms a connected area. The result is as follows Figure 7 As shown;
[0091] 5.5) Repeat steps 5.2) to 5.4) to scan all pixels of the dilated image. Multiple connected regions are obtained in the dilated image. Each connected region represents a set of points marked by a label. Each connected region corresponds to an obstacle point cloud. Point cloud clustering is completed to obtain each obstacle point cloud.
[0092] At this point, the clustering of obstacle point cloud is completed.
[0093] Step 6: Point cloud encirclement based on binary occupancy grid;
[0094] 6.1) Use Hough line detection to identify the main direction of the obstacle point cloud;
[0095] The main direction of the point cloud refers to the direction in which the point cloud data changes the most. It determines the direction and size of the bounding box. The key to determining the main direction of the obstacle point cloud is how to obtain the maximum distribution of the obstacle point cloud border in the xy plane.
[0096] First, extract the obstacle point cloud frame: Since the point cloud is a kind of image similar to an empty shell, it only reflects the surface information of the photographed object. For general obstacles, the size of the point cloud in the x direction does not change much. Project the obstacle point cloud into the xy plane and get Figure 8The result shown in Figure 1 is: Retain all points whose height in the z direction satisfies equation (19), and we can get Fig. 9 The obstacle point cloud bounding box shown;
[0097] z(p i )-z min <γ(z max -z min ) (19)
[0098] Among them, z(p i ) represents point p i The z-direction height, z max 、z min They represent the upper and lower limits of the height of the obstacle point cloud in the z direction, and γ represents a coefficient. In this embodiment, the obstacle point cloud border is clearest when γ=0.7;
[0099] Secondly, the obstacle point cloud border is rasterized in the xy plane to obtain Fig.10 The grid image shown in the figure records the maximum values of all pixels in the obstacle point cloud border in the x and y directions. and minimum value According to equations (20) and (21), the range r of the grid image in the x and y directions is calculated x 、r y ; The size of the raster image is r x × y ;
[0100]
[0101]
[0102] Map all pixels on the obstacle point cloud border to the grid image, and assign 0 / 1 to the grid image, record the pixel value of the included pixel as 1, and the pixel value of the excluded pixel as 0, and then generate the point cloud border binary occupancy grid, see Fig.11 ;
[0103] Then, read the binary occupancy grid of the point cloud border, scan the pixel points with pixel value 1 in the binary occupancy grid of the point cloud border, and use the grid side length g as the transformation step length of Hough line detection. The total detection length is the total perimeter of the obstacle point cloud border, so the number of detections n t Satisfying formula (22):
[0104] n t =2(r x +r y ) / g (22)
[0105] Set the accumulator for the parameter space and initialize the accumulator Num(θ,ρ) = 0, where ρ and θ represent the polar radius and polar angle of the pixel point in polar coordinates respectively; for the pixel point (x j ,y j ), find all the parameters in the parameter space that satisfy ρ = x j cosθ+y j The coordinate value of sinθ (θ, ρ) completes the pixel point (x j ,y j ) Hough transform; then let Num(θ,ρ)=Num(θ,ρ)+1, and perform Hough transform on the next pixel until all pixels in the binary occupancy grid of the point cloud border have completed Hough transform; count the sizes of all Num(θ,ρ), and the straight line with the coordinate value corresponding to the maximum Num(θ,ρ) is the straight line corresponding to the main direction of the obstacle point cloud; calculate the angle α between the straight line corresponding to the main direction of the obstacle point cloud and the base coordinate system, then α is the main direction of the obstacle point cloud, such as Fig.12 As shown;
[0106] 6.2) Establish a bounding box based on the main direction of the obstacle point cloud and determine the vertex coordinates of the bounding box;
[0107] First, the coordinates of the center point of the obstacle point cloud (x c ,y c ), and the vector starting from the center point and pointing to each point of the obstacle point cloud is calculated by formula (24):
[0108]
[0109]
[0110] Among them, x max 、x min They represent the maximum and minimum values of the obstacle point cloud in the x direction and y direction, respectively. max ,y min Respectively represent the maximum and minimum values of the obstacle point cloud in the y direction;
[0111] Secondly, the vector Rotate -α angle around the z axis at the center point to get the vector Make the main direction of the obstacle point cloud parallel to the x-axis of the point cloud space base coordinate system, then we have equation (25), see Fig.13 ;
[0112]
[0113] After all vectors are rotated, the obstacle point cloud is actually rotated to obtain the rotated obstacle point cloud;
[0114] Then, traverse the rotated obstacle point cloud and get the maximum value x' of the rotated obstacle point cloud in the x, y, and z directions max ,y' max 、z' max and the minimum value x' min ,y' min 、z' min , which is equivalent to finding a bounding rectangle in each face to frame the rotated obstacle point cloud; according to formula (26), the vertex coordinates of the bounding box are calculated as box' k ;
[0115]
[0116] Finally, the vertex coordinates of the bounding box box' are converted into k After the rotation transformation, we return to the obstacle point cloud before the rotation and get the original vertex coordinates of the bounding box. k , that is, rotate the circumscribed rectangle clockwise by angle α, see Fig.15 ;
[0117]
[0118] For each obstacle point cloud, a bounding box is built according to step 6. The result is as follows Fig.16 As shown, Fig.17 This is a schematic diagram of the positions of each vertex of the bounding box. At this point, the point cloud encirclement based on the binary occupancy grid is completed.
[0119] The experimental results show that this method can effectively distinguish the number of obstacles in the scene. As the number of frames changes, the labels of each obstacle point cloud remain consistent, and there will be no missed or over-judgment with the change of the number of frames; the bounding box can always follow and envelop the obstacle point cloud, so the bounding box has good tracking performance. For static obstacles, since the main direction of the obstacle point cloud does not change, the direction and size of the bounding box do not change. For dynamic obstacles, since the position and speed change during walking, the main direction of the obstacle point cloud also changes. This method can track the change of the main direction of the obstacle point cloud in real time, so that the direction and size of the bounding box change in real time, ensuring that the bounding box always tightly envelops the obstacle point cloud.
[0120] In summary, this method can greatly improve the computing speed while ensuring the clustering accuracy. The established bounding box can more accurately reflect the size of obstacles and has good real-time and tracking performance, which provides a theoretical basis for the robot's autonomous obstacle avoidance.
[0121] Any matters not described in the present invention are applicable to the prior art.
Claims
1. A point cloud clustering and enclosure method based on binary occupancy grid, characterized in that: The method comprises the following steps: Step 1: Collect environmental point cloud data; Step 2: remove the ground point cloud to obtain point cloud data after removing the ground point cloud; Step 3: For the point cloud data after removing the ground point cloud, generate a polar coordinate grid in the point cloud space, and map the point cloud data to the polar coordinate grid; assign 0 / 1 to the polar coordinate grid, generate a binary occupancy grid, and map the point cloud data to the polar coordinate grid; specifically: The polar coordinate plane where the point cloud space is located is divided into m2 sector-shaped areas, and the central angle of each sector-shaped area is α pl , then: Let the maximum radius of the sector be r max , the segmentation step is r pl , divide each sector area into n sub-areas along the radial direction, then: In this way, the point cloud space is divided into a polar coordinate grid containing m×n grids; Map the point cloud data to the polar coordinate grid and determine the point p according to equations (13) and (14): i =(x i ,y i ,-) belongs to the sector area s(p i ); in, are the lower and upper bounds of each sector area respectively; Determine the sector area s(p i ), then calculate point p according to equations (15) and (16) i The sub-region b(p i ); in, are the lower and upper bounds of each sub-region respectively; After mapping each point to the polar coordinate grid, assign a value to each grid. If the grid does not contain a point, the pixel value of the grid is 0 and the pixel color is black; if the grid contains a point, the pixel value of the grid is 1 and the pixel color is white. Traverse all grids to generate a binary occupancy grid. Step 4, dilate the binary occupancy grid to obtain a dilated image; Step 5: Perform connected region analysis on the dilated image to obtain all connected regions. Each connected region is an obstacle point cloud, and the clustering of the obstacle point cloud is completed. Step 6: encircling the point cloud based on the binary occupancy grid, including identifying the main direction of the obstacle point cloud, establishing a bounding box according to the main direction of the obstacle point cloud, and determining the vertex coordinates of the bounding box.
2. The point cloud clustering and encirclement method based on binary occupancy grid according to claim 1, characterized in that: In step 6, the specific process of identifying the main direction of the obstacle point cloud is: Project the obstacle point cloud into the xy plane, retain all points whose height in the z direction satisfies equation (19), and obtain the obstacle point cloud bounding box; from(p i )-With min <γ(z max -With min ) (19) Among them, z(p i ) represents point p i The z-direction height, z max 、z min They represent the upper and lower limits of the obstacle point cloud height in the z direction, and γ represents the coefficient; Rasterize the obstacle point cloud border in the xy plane to obtain a grid map, and record the maximum values of all pixels in the obstacle point cloud border in the x and y directions. and minimum value According to equations (20) and (21), the range r of the grid image in the x and y directions is calculated x 、r y ; Map all pixels on the obstacle point cloud border to the grid image, and assign 0 / 1 to the grid image, record the pixel value of the included pixel as 1, and the pixel value of the excluded pixel as 0, to generate the binary occupancy grid of the point cloud border; Scan the pixel points with a pixel value of 1 in the binary occupancy grid of the point cloud border, and obtain the main direction α of the obstacle point cloud through Hough line detection; The specific process of establishing a bounding box is: First, the coordinates of the center point of the obstacle point cloud (x c ,y c ), and the vector starting from the center point and pointing to each point of the obstacle point cloud is calculated by formula (24): Among them, x max 、x min They represent the maximum and minimum values of the obstacle point cloud in the x direction and y direction, respectively. max ,y min Respectively represent the maximum and minimum values of the obstacle point cloud in the y direction; Secondly, the vector Rotate -α angle around the z axis at the center point to get the vector Make the main direction of the obstacle point cloud parallel to the x-axis of the point cloud space base coordinate system, then we have formula (25): After all vectors are rotated, the rotated obstacle point cloud is obtained; Then, traverse the rotated obstacle point cloud and get the maximum value x' of the rotated obstacle point cloud in the x, y, and z directions max ,y' max 、z' max and the minimum value x' min ,y' min 、z' min , calculate the vertex coordinates of the bounding box box' according to formula (26) k ; Finally, the vertex coordinates of the bounding box box' are converted into k After the rotation transformation, we return to the obstacle point cloud before the rotation and get the original vertex coordinates of the bounding box. k ; A bounding box is built for each obstacle point cloud according to the above process.
3. The point cloud clustering and encirclement method based on binary occupancy grid according to claim 2, characterized in that: When γ = 0.7, the boundary of the obstacle point cloud is the clearest.
4. The point cloud clustering and encirclement method based on binary occupancy grid according to claim 1, characterized in that: Step 2 is as follows: The rectangular coordinate system of the point cloud space is transformed into a polar coordinate system to obtain a polar coordinate plane; according to formula (1), from point p g The coordinates in the xy plane (x g ,y g ) to obtain the azimuth angle θ g ; i g =atan2(y g ,x g ) (1) where atan2 is the inverse tangent function that takes the quadrant into account; Use formula (2) to divide the polar coordinate plane into m1 sector-shaped sub-planes, and the central angle of each sector-shaped sub-plane is Δα; According to formula (3), point p is determined g The fan-shaped sub-plane s(p g ), are the lower and upper bounds of each sector sub-plane respectively; In the tth (t=1,2,…,m1)th sector subplane, all points are reordered from low to high in the z direction as follows: Then we have: Select the first r points with the lowest height in the z direction in the t-th sector sub-plane, and calculate the average height H of the first r points according to formula (5): L : According to formula (6), the points in the t-th fan-shaped sub-plane are screened, and all points that satisfy formula (6) constitute a point set P t ; in, is the point p in the t-th sector subplane g The z-direction height, H0 is the ground height, h L 、h g They are the minimum height threshold in the z direction and the ground height threshold respectively; The point set of m1 fan-shaped sub-planes constitutes the total point set Perform RANSAC sampling on the total point set P0 to obtain a ground plane model, and remove the points contained in the ground plane model to remove the ground point cloud.
