CN112184736A - Multi-plane extraction method based on European clustering - Google Patents

Abstract

Translated fromChinese

本发明公开一种基于欧式聚类的多平面提取方法，可以适用于多种野外复杂环境。首先，针对树冠、隧道、矿洞等一类具有悬挂物的环境，提出基于欧式聚类的混合高度图方法。通过将映射到二维栅格中三维点云进行聚类，并利用混合高度图法区分地面点和悬挂物点，实现了对地面上方障碍物的准确检测和下方对应可通行区域的正确提取。除此之外，针对野外崎岖环境，本发明还提出了基于随机采样一致性的多平面提取方法。通过在gazebo虚拟仿真环境和真实校园环境中进行了实验验证。实验结果表明，本发明方法与已有算法相比在点云分割的精度和效率方面均有明显提高。

The invention discloses a multi-plane extraction method based on Euclidean clustering, which can be applied to various field complex environments. Firstly, a hybrid height map method based on Euclidean clustering is proposed for environments with hanging objects such as tree canopies, tunnels, and mines. By clustering the three-dimensional point cloud mapped to the two-dimensional grid, and using the hybrid height map method to distinguish the ground point and the hanging object point, the accurate detection of obstacles above the ground and the correct extraction of the corresponding passable area below are realized. In addition, for the rugged environment in the wild, the present invention also proposes a multi-plane extraction method based on random sampling consistency. It is verified by experiments in gazebo virtual simulation environment and real campus environment. The experimental results show that, compared with the existing algorithm, the method of the present invention has obvious improvements in the accuracy and efficiency of point cloud segmentation.

Description

Multi-plane extraction method based on European clustering

Technical Field

The invention belongs to the technical field of point cloud segmentation and mobile robot passable domain extraction, and particularly relates to a multi-plane extraction method based on Euclidean clustering.

Background

At present, the autonomous mobile robot [1] is widely applied to various fields, and related technologies thereof also become research hotspots. The environment perception technology is the key for the mobile robot to realize autonomous navigation. The application fields of the outdoor robot include unmanned vehicles, survey robots, rescue robots and the like [2-4], and the accurate detection of obstacles in the surrounding environment and the identification of the area which can be safely traversed by the robot are one of the most basic requirements of the robots. Achieving autonomous navigation of a robot [5] is complicated by the large terrain variations, irregular obstacle shapes, and the like in outdoor unstructured environments. Besides detecting conventional obstacles such as trees, rocks, steep slopes, etc. to avoid collision with them, pits, caves, etc. increase the difficulty of detection. These all make the detection of passable areas a primary task for robot navigation [2,6 ].

The unstructured nature of the outdoor environment requires the robot to perceive the surrounding environment in three-dimensional space. In recent years, multi-line three-dimensional laser scanners (e.g., Velodyne) have found widespread use due to their high speed and accuracy in acquiring distance data. Because the number of data points returned by each frame of three-dimensional (3D) range scan is huge, and the occupied grid method can realize fast compression and efficient processing of data, the method becomes the most commonly used method for representing environments. Occupancy grids [7-9] are a 2.5D representation that is achieved by mapping a three-dimensional point cloud onto a two-dimensional grid fixed on the ground. Thrun et al propose a grid-based method [10] that divides grid cells into ground cells and non-ground cells based on the maximum absolute difference between the heights of the point clouds within the grid cells. In the city challenge race of the national defense department advanced research program office in 2007, many teams have adopted this method with great success in order to segment the point cloud and detect other vehicles on the track. Although this representation method is computationally efficient, there are some problems as follows. First, the method is susceptible to under-segmentation, i.e. obstacles are considered together with the nearby ground as impassable areas, influenced by the grid size. Secondly, because the three-dimensional point cloud is mapped to a 2.5D grid structure, a dimension reduction technology is introduced, and a free vertical space between any two points is not modeled, the method cannot identify the suspended obstacle. For example, the top of the tree and the ground under the tree would be considered as impassable objects due to being in the same grid, which presents new challenges to point cloud segmentation. For the processing of suspended objects, a method for expanding a height map is proposed in [11 ]. In addition, the multi-scale height map [12] proposed by p.pfaff et al is a variation of the height map for processing suspended and vertical structures, which consists of a two-dimensional grid, each containing a series of planar tiles. However, this method has a low processing efficiency because of a large amount of data to be stored.

In addition to the grid-based approach described above, segmentation of the ground and obstacle points also includes model-based approaches [13-16 ]. Himmelsbach et al [17, 18] maps the three-dimensional point cloud to a polar grid map, and selects a point with the lowest height as a candidate ground point. And performing straight line fitting in the cylindrical coordinate system by using the distribution of the candidate points, wherein the points on the straight line segment meeting a certain gradient threshold value are regarded as ground points. Douillard et al [19] project 64-line laser scanner data into a grid map, and a two-dimensional gaussian process regression algorithm is used to directly perform ground fitting and obstacle extraction on the entire map, which can be used to describe rough roads, but only approximately real-time effects can be obtained because the algorithm is relatively complex to calculate. Chen et al [20] use a non-stationary covariance function to determine surface points that adapt to terrain relief, making them robust to rough environments. However, erroneous classification results may occur when detecting small obstacles, for example, a step is regarded as a passable ground and an uphill road is regarded as an obstacle. In addition, the RANSAC method is used to extract planes in many documents [21, 22], but the method still lacks the ability to handle suspended objects. Text [23] fits the floor of the incomplete plane by multiple disjoint planar regions. The method can approximately find the optimal region division and plane hypothesis under the RANSAC scheme with higher real-time computation complexity.

Although the above methods have made great progress, research on point cloud segmentation in field environment is far from mature. Although the mesh-based method can realize the rapid segmentation of the point cloud, it is difficult to accurately distinguish ground points and suspension obstacle points in an environment with suspension objects, which causes the accuracy of segmentation to be reduced. And the model-based method is difficult to adapt to the field environment with variable characteristics. Summarizing the literature, it can be known that the existing method has a defect in the precision of point cloud segmentation, and affects the extraction of the passable domain of the mobile robot and the implementation of the navigation task.

Disclosure of Invention

The invention aims to overcome the defects in the prior art and provides a multi-plane extraction method based on Euclidean clustering.

The purpose of the invention is realized by the following technical scheme:

a multi-resolution mixed height map method based on Euclidean clustering comprises the following steps:

(1) representing the map by adopting a polar coordinate grid;

dividing the map by a two-dimensional polar grid as follows: uniformly dividing the map into m fan-shaped blocks in the anticlockwise direction by taking the position of the robot as the center and the advancing direction as the starting boundary line, and for each fan-shaped block s_iEqually spaced along the radius direction to be divided into n sub-blocks; projecting three-dimensional point cloud data obtained by a three-dimensional laser scanner on a robot to a two-dimensional polar coordinate grid;

(2) traversing all grids in the map, and performing preliminary judgment on the grid attributes by using a hybrid height map method based on Euclidean clustering segmentation; the method comprises the following specific steps:

when the height difference of a certain grid is less than a given thresholdValue T₁Adding all point clouds in the grid into a quasi-ground point set; if the height difference of the grids is greater than a given threshold T₁If so, judging that the corresponding grid is an uncertain grid;

grouping the point clouds in the uncertain grids by an Euclidean clustering segmentation method, calculating the height average value of the point clouds in each group, selecting one group of point clouds with the minimum height average value from the point clouds in each group as a reference group, and taking other groups as comparison groups; then, comparing the value with the minimum point cloud height in each comparison group with the value with the maximum point cloud height in the reference group; if the height difference between all comparison groups and the reference group is greater than the passable clearance threshold T₂Adding the point clouds in the reference group into the quasi-ground point set, and adding the point clouds in all the other comparison groups into the obstacle point set;

(3) performing secondary filtering processing on the quasi-ground points obtained by the Euclidean clustering segmentation method through a multi-plane extraction method with random sampling consistency, wherein the method specifically comprises the following steps:

(301) taking quasi-ground points obtained based on a Euclidean clustering segmentation method as an input measuring point sequence of a random sampling consistency method to extract a plane model, acquiring plane parameters and establishing the plane model as shown in formula (3):

ax+by+cz+d＝0 (3)

the normal vector of the fitting plane is obtained as follows:

unit vector in z-axis direction of robot body coordinate system

As a reference normal vector; calculating the normal vector of the fitting plane

And a reference normal vector

Angle theta therebetween, angle thetaThe calculation method is shown in formula (4):

if the included angle theta is smaller than the maximum gradient threshold value theta_thAdding the interior points of the fitting plane into the final ground point set;

(302) and the outer points of the fitting plane are continuously used as an input measuring point sequence to carry out plane model extraction, if the constraint condition is met, the processing is carried out according to the step (301), otherwise, the inner points of the fitting plane are added into the obstacle point set;

(303) repeating the operation according to the step (302) until the number of the remaining exterior points is less than the set threshold value T_num。

Further, in the step (1), assume that disordered three-dimensional point cloud data obtained by the three-dimensional laser scanner at the current time t is P_t＝{p₁,…,p_m,…,p_NI.e. a total of N laser data points; projecting the three-dimensional point cloud data into a two-dimensional polar coordinate grid, and aiming at each laser data point p_m＝(x_m,y_m,z_m)^TThe mapping method is as follows:

where θ is the three-dimensional data point p_mThe included angle between the projection plane and the positive direction of the X axis is in the range of 0,2 pi](ii) a Δ th and Δ r represent angular resolution and radial resolution, respectively, which together determine the size of the grid; floor is a floor rounding function for calculating the index position(s) of the projection grid in the map_i,b_j)；

Further, in step (2), first at low resolutionJudging by using a maximum and minimum height difference method under the rate grid, and if the height difference of the corresponding grid is smaller than a given threshold value T₁Directly adding the point cloud in the grid into the quasi-ground point set, otherwise switching to a high-resolution grid for judging again; if the maximum and minimum height difference value of the point clouds in the grids is still larger than a given threshold value T till the highest resolution layer₁If so, judging that the corresponding grid is an uncertain grid; and (3) further determining the grid attribute according to the Euclidean clustering-based mixed height map method provided in the step (2) for the uncertain grid.

Compared with the prior art, the technical scheme of the invention has the following beneficial effects:

the method overcomes the difficulty that the prior method can not process the suspension, groups the point clouds by using the mixed height map method based on Euclidean clustering, realizes the separation of the ground point clouds and the suspension point clouds, improves the segmentation precision of the point clouds, and achieves good point cloud segmentation effect. Moreover, the method can also be adopted in the case of sparse point cloud, which is different from the document [25] which can only be applied to the processing of dense point cloud. In addition, the method does not take a common single-resolution map as a map expression, but adopts a new multi-resolution map mode, which solves the problem of contradiction between under-segmentation and time complexity caused by the size of the grid to a certain extent. Finally, the invention also adopts a novel multi-plane extraction method based on random sampling consistency, adaptively extracts plane points meeting terrain constraint conditions in an iteration mode, and can be used for point cloud segmentation in complex environments (particularly uneven terrains with fluctuation). The method is adaptive according to terrain and can tolerate unevenness of the ground. The method solves the problem that most of the existing point cloud segmentation mainly aiming at the flat roads of cities and villages still has technical vacancy under challenging field rugged terrains. Compared with various existing algorithms in the set-up virtual environment and experimental environment, the algorithm has the advantages that the precision and the efficiency of point cloud segmentation are obviously improved.

Drawings

FIG. 1 is a schematic flow chart of the overall algorithm of the present invention;

fig. 2 is a polar grid representation map. Performing environment representation by projecting the three-dimensional point cloud to a two-dimensional polar grid;

fig. 3a to 3d are schematic diagrams of a hybrid height map algorithm based on euclidean cluster segmentation. Fig. 3a shows the preliminary judgment of the grid attribute by using the maximum and minimum height map method, fig. 3b is used for calculating the distance between the suspended obstacle and the ground below the suspended obstacle, fig. 3c shows the special situation that only one group of point clouds may exist after the euclidean clustering segmentation, and fig. 3d is used for judging whether the obstacle which obstructs the robot from passing through exists on the ground;

fig. 4 is a multi-resolution based polar grid map.

Fig. 5 is a schematic diagram of extracting planar features by using a random sampling consistency method.

Fig. 6a to 6c are schematic diagrams of a multi-plane extraction algorithm process based on random sampling consistency. Wherein fig. 6a shows some false detection situations existing in the hybrid height map method based on euclidean clustering, fig. 6b shows the filtering process performed by the RANSAC method, and fig. 6c shows the filtering process performed by the RANSAC-based multi-plane extraction method;

fig. 7a to 7d are simulation results of the husky mobile platform in the gazebo simulation environment. FIG. 7a is a schematic diagram of a first type of simulation environment; fig. 7b to 7d respectively show a maximum and minimum height difference method, a mean height map method and a hybrid height map method based on euclidean clustering according to the present invention;

fig. 8a to 8d show experimental results of the summit-XL robot in a campus environment. Wherein FIG. 8a is a schematic diagram of a first type of experimental environment; fig. 8b to 8d respectively show a maximum and minimum height difference method, a mean height map method and a hybrid height map method based on euclidean clustering according to the present invention;

FIGS. 9a to 9c are simulation results of the husky mobile platform in a gazebo simulation environment. FIG. 9a is a diagram of a second type of simulation environment; FIGS. 9b and 9c show the three-segment fitting method used in document [17] and the RANSAC-based adaptive multi-plane extraction method proposed by the present invention, respectively;

fig. 10a to 10c show experimental results of the summit-XL robot in a campus environment. Wherein FIG. 10a shows a schematic diagram of a second type of experimental environment; FIGS. 10b and 10c show the three-segment fitting method used in document [17] and the RANSAC-based adaptive multi-plane extraction method proposed by the present invention, respectively;

fig. 11a and 11b are schematic diagrams for comparison of the time performance of a single-resolution grid map and a multi-resolution grid map.

Note: in the cloud point diagrams referred to in the drawings of the specification, light-colored points all represent ground points, and dark-colored points all represent obstacle points.

Detailed Description

The invention is described in further detail below with reference to the figures and specific examples. It should be understood that the specific embodiments described herein are merely illustrative of the invention and are not intended to limit the invention.

The invention provides a multi-plane point cloud segmentation method based on Euclidean clustering. The passable area is preliminarily identified by utilizing the height difference between the point clouds in the grids by projecting the three-dimensional point cloud into the two-dimensional polar coordinate grid. And for the grids which do not meet the height difference constraint condition, further checking whether a free vertical space exists by adopting a Euclidean clustering-based method. That is, the point clouds in the grid are grouped, and a part belonging to the ground is selected from the point clouds, and the height between the suspended object part located above and the ground determines the trafficability of the grid. In order to improve the efficiency of point cloud segmentation, the invention adopts a multi-resolution grid map method, so that the calculation and storage can be effectively reduced. In addition, by iteratively extracting points with plane features, the multi-plane extraction method based on random sampling consistency can be well adapted to uneven ground. The whole algorithm flow is shown in the attached figure 1. The virtual environment simulation and the real campus environment experiment result show that compared with the existing method, the method has higher point cloud segmentation precision and accuracy.

Specifically, the multi-plane point cloud segmentation method based on Euclidean clustering provided by the invention comprises the following steps:

the method comprises the following steps: representing the map by adopting a polar coordinate grid;

a single frame of data of a three-dimensional laser scanner usually contains tens of thousands or even hundreds of thousands of measurement points, which would cause a huge time consumption if directly operated on. Before a point cloud is segmented, reasonable representation of data is generally required to improve segmentation efficiency. Raster map-based representation is one of the most common three-dimensional data representation methods. The data characteristics of the multi-line laser scanner are represented as dense at near and sparse at far. To accommodate the above feature, the present embodiment represents the map in the form of a grid of polar coordinates. The method can overcome the problem of uneven data distribution caused by that the measuring points of the laser scanner become more and more sparse along with the increase of the distance to a certain extent. The polar grid divides the map as follows: uniformly dividing the map into m fan-shaped blocks in the anticlockwise direction by taking the position of the robot as the center and the advancing direction as the starting boundary line, and for each fan-shaped block s_iThe sub-blocks are equally spaced in the radial direction and the specific division result is shown in fig. 2.

Suppose that the disordered point cloud sequence obtained by the three-dimensional laser scanner at the current moment t is P_t＝{p₁,…,p_m,…,p_NI.e. a total of N laser data points. Projecting the three-dimensional point clouds into a two-dimensional polar grid, for each laser data point p_m＝(x_m,y_m,z_m)^TThe mapping method is as follows:

where θ is the three-dimensional data point p_mThe included angle between the projection plane and the positive direction of the X axis is in the range of 0,2 pi]. Δ th and Δ r represent angular resolution and radial resolution, respectively, which together determineThe size of the grid. Floor is a floor rounding function for calculating the index position(s) of the projection grid in the map_i,b_j)。

Step two, a mixed height map method based on Euclidean clustering segmentation;

the grid-based method only retains partial data information in each grid, which reduces the amount of data to be processed and increases the processing efficiency. At present, the method becomes the most common three-dimensional point cloud segmentation method. The grid-based method may be further classified into a mean height map, a maximum and minimum height map, and the like according to the difference of data information included in each grid. Wherein only one value, namely the average value of the height information of the terrain position, is stored in each grid of the average height map. By comparing the average value with a threshold value, it is determined whether the state of each grid is an obstacle or a non-obstacle. In the maximum-minimum height map, however, each grid contains only two values — the maximum and minimum of all measurement point heights projected into the same grid. The grid height difference method is that the difference value between the maximum height value and the minimum height value is calculated and compared with a set threshold value, if the difference value is larger than the threshold value, all points in the grid are considered to belong to the obstacle point, otherwise, the points are judged to be the ground points. Although the method is widely applied to the point cloud segmentation technology, the method shows good segmentation effect to a certain extent. They do not have the ability to handle suspended structures (tree crowns, tunnels, caverns, etc.).

Algorithm 1 hybrid height map method based on Euclidean clustering segmentation

Therefore, the ground points and the suspended object points are distinguished based on the hybrid height map method of Euclidean clustering segmentation, and accurate detection of the obstacles above the ground and accurate extraction of the corresponding passable areas below the obstacles are realized. The flow chart of the hybrid height map method of the euclidean cluster segmentation is shown inalgorithm 1.

First traverse polar coordinateAll grids in the graph are subjected to preliminary judgment on the grid attribute by using a maximum and minimum height graph method (as shown in FIG. 3 a). When the height difference of a certain grid is less than a given threshold value T₁Adding all point clouds in the grid into a quasi-ground point set; if the height difference of the grids is greater than a given threshold T₁If the grid is determined to be an indeterminate grid, the attribute of the indeterminate grid needs to be further determined. Since there is a significant distance between the suspended object and the ground below it, the present invention relies on this property to determine whether there is a vertical free space through which the robot can safely pass. At line 6 ofAlgorithm 1, by the Euclidean clustering segmentation method [24 ]]The point clouds in the "uncertain grid" are grouped. And the function Group () is used for calculating the height average value of the point clouds in each Group, and selecting one Group of point clouds with the minimum average value as a reference Group and other groups as comparison groups. The value of minimum point cloud height in each comparison set is then compared to the value of maximum point cloud height in the reference set. If the height difference between all comparison groups and the reference group is greater than the passable clearance threshold T₂And adding the point clouds in the reference group into the quasi-ground point set, and adding the point clouds in all the other comparison groups into the obstacle point set. In lines 11-16 ofalgorithm 1, the present embodiment employs a maximum minimum height map method for calculating the distance between the suspended obstacle and the ground below it, as shown in fig. 3 b.

However, when the grid attribute is further determined by using the hybrid height map method based on euclidean clustering, special cases may occur. For example, after the euclidean segmentation, only one group of point clouds may exist, which indicates that the point clouds in the grid are distributed more densely. This may occur because the multi-line laser scanner does not simultaneously obtain laser measurement points from the same location of the ground and the suspension above it. At this time, the group of point clouds is all divided into a set of obstacle points for the following reasons: (1) when the maximum and minimum height difference method is adopted for preliminary judgment, the height difference of the group of point clouds is larger than a threshold value T1. If it returns from the ground, it indicates that there is a ground obstacle at that location. (2) If the set of point clouds is returned from the suspension location, it also belongs to the set of obstacle points (as shown in FIG. 3 c). In addition to this, situations may arise in which the distance between the ground and the suspension in the grid is greater than a threshold value T₂But there are obstacles at the ground that impede the passage of the robot. In addition to calculating the distance between the contrast set and the reference set, the point cloud of the reference set needs to be detected. That is, the maximum and minimum height difference method is adopted again inside the reference group to determine whether there is a ground obstacle (as shown in fig. 3 d). As long as one of the two measured distances does not satisfy the threshold condition, the point clouds in the grid are all classified into the set of obstacle points, see lines 14-15 inalgorithm 1. In this embodiment, the threshold T₁Set to 0.2m, T₂Set to 1 m.

The invention combines various grid height graph methods on the basis of the European cluster segmentation method, and realizes the correct segmentation of the point cloud in the environment with a suspension structure. The method not only can effectively distinguish the ground part from the suspension part, but also has accurate detection and identification capability on the obstacles on the ground.

Step three: multi-resolution grid map method;

although the method based on the grid map can realize the preliminary segmentation of the point cloud to a certain extent, the selection of the grid size directly affects the precision of the point cloud segmentation. Obviously, the larger the grid is, the less segmentation condition occurs, and the accuracy of point cloud segmentation is influenced; however, too small a grid will increase the time consumption and affect the real-time performance of the determination. It can thus be seen that the accuracy of point cloud segmentation and the time cost it takes are contradictory at a single resolution.

Therefore, the present embodiment also provides a multi-resolution polar grid map method (as shown in fig. 4). Firstly, the maximum and minimum height difference method is used for judging under the low-resolution grid, and if the threshold condition T is met₁And directly adding the point cloud in the grid into the quasi-ground point set, otherwise, continuously dividing the grid, and switching to a grid with higher resolution ratio for judging again. If up to the highest resolution layer is to be reached,and (4) if the maximum and minimum height difference value of the point cloud in the grid is still larger than the threshold value, switching to the mixed height map method based on Euclidean clustering segmentation provided in the step two for judgment.

When the grid is large, some flat areas can be directly judged as passable ground, so that the accuracy of point cloud segmentation is not influenced, and unnecessary time consumption is reduced; when the grid is small, the fine division can be realized for some barrier regions, and the probability of under-segmentation is greatly reduced. However, the choice of different levels of resolution is also important. An angular resolution of 2.5/radial resolution of 0.25m is selected as the highest resolution of the grid, and an angular resolution of 20/radial resolution of 2m is selected as the lowest resolution of the grid. Table 1 shows the resolution of each layer of the grid.

TABLE 1 resolution of layers of the grid

Step four, a self-adaptive multi-plane extraction method based on a Random Sample Consensus method (RANSAC);

the point cloud segmentation method in the first step can not only process objects with suspension structures, but also avoid the problem of under-segmentation. The grid-based method only reflects local characteristics, and for high-altitude horizontal objects appearing in the environment, the local height is not obviously changed, so that the high-altitude horizontal objects are easily divided into ground points, and the wrong division occurs; (2) the grid height map method only reflects the change in vertical height and cannot reflect the horizontal characteristic, which may cause some slope surfaces with larger inclination angles to be wrongly classified as drivable road surfaces due to the plane characteristic. As shown in fig. 6a, the point clouds marked by the solid line boxes all have wrong segmentation. For the above situation, the embodiment performs filtering processing again on the quasi-ground points obtained based on the euclidean cluster segmentation method by using a random sampling consistency method.

Random Sample Consensus (RANSAC) is a model extraction method that iteratively estimates the parameters of a mathematical model from a set of observed data sets containing outliers. The basic assumptions of RANSAC are:

a. the observation data set consists of an interior point measurement sequence conforming to a certain mathematical model distribution and an exterior point measurement sequence not conforming to the distribution;

b. given a set of observed data sets, there is a model to describe the interior points therein, while the exterior points, as noise, cannot fit into the model.

Given a set of observation sequences comprising inliers and outliers, the RANSAC method uses a voting procedure to search step by step for the parameters that are best suited for a given mathematical model. The iterative process comprises the following two steps: firstly, assuming that the minimum number of samples required for calculating the parameters of the given mathematical model is n, randomly selecting a group of subsequences with the number of samples being n from an observation data set, and calculating the parameters of the given mathematical model based on the subsequences; and traversing the whole observation sequence, judging whether the sample conforms to the model obtained in the previous step, and recording the number of interior points conforming to the model.

And repeating the two steps until the iteration number reaches a given threshold value, finally finding a model which is in accordance with the model and has the maximum number of interior points, and recording the model parameters.

In the embodiment, a RANSAC method is adopted to extract plane features, and ground point clouds meeting the passable requirement of the mobile robot are obtained by screening, as shown in figure 5. First, the "quasi-ground points" obtained insection 1 are used as a set of observation sequences, and plane model extraction is performed based on the observation data set. Since the RANSAC method obtains a plane model with the largest number of interior points, the plane is generally the main plane centered on the position where the robot is located. After the filtering process, the point cloud marked by the solid line frame in fig. 6b is correctly divided into the obstacle points. At the same time, however, some slope points with smaller angles of possible travel are filtered out as noise due to their greater distance from the principal plane point, as indicated by the dashed box in fig. 6 b.

In order to solve the above problem, the embodiment provides a self-adaptive multi-plane extraction method based on RANSAC, and a plane satisfying a certain gradient constraint is extracted in an iterative manner. Firstly, based on the quasi-ground points obtained in the second step, obtaining plane parameters and establishing a plane model as shown in formula (3):

ax+by+cz+d＝0 (3)

the normal vector of the plane is:

then, the unit vector in the z-axis direction of the vehicle body

As a reference normal vector. The fitted ground plane should satisfy a certain slope constraint, i.e. the normal vector of the fitted plane

And a reference normal vector

Should be less than the maximum slope threshold theta_th. The included angle θ is calculated as shown in equation (4):

the self-adaptive multi-plane extraction method based on random sampling consistency comprises the following steps:

(401) taking quasi-ground points as an observation sequence of a random sampling consistency method to carry out plane model extraction, and obtaining a normal vector of a fitting plane

Calculating the included angle between the normal vector of the ground plane and the normal vector of the reference plane by using the formula (4), and if the included angle is smaller than the maximum gradient threshold value theta_th(in the present embodiment, the threshold value θ_thSet to 15 deg.), then the interior points of the fitted plane are added to the final set of ground points.

(402) And continuously taking the outer points of the fitting plane as an observation sequence to extract a plane model, if the constraint condition is met, processing according to the step (1), and if the constraint condition is not met, adding the inner points of the fitting plane into the obstacle point set.

(403) Repeating the operation according to the step (402) until the number of the remaining exterior points is less than the set threshold value T_num(in the present embodiment, threshold T_numSet to 0.1N).

The RANSAC-based adaptive multi-plane extraction method not only can filter local noise points, but also has a certain constraint effect on the gradient. As shown in fig. 6c, the point clouds marked in the solid line frame are all correctly divided into obstacle points, and the slope points in the dashed line frame are also correctly divided into passable ground points due to the multiple iteration of extracting the plane, which proves that the algorithm has good performance in the point cloud segmentation of the undulating ground.

Specifically, in order to verify the performance of the method provided by the invention, experiments are respectively carried out in a virtual simulation environment and a campus actual environment. On the one hand, the different types of field environments including tree crowns, tunnels, hillsides, rocks and the like are built through the robot simulation platform Gazebo. The outdoor mobile platform husky is adopted to simulate the operation of a field robot, so that the real dynamic simulation can be realized. On the other hand, in the campus environment, a suitable scenario was also chosen to illustrate the performance of the algorithm, namely sidewalk under the tree and meadow near the lake, respectively. The mobile robot platform selects a summit-XL robot which can be applied to various indoor and outdoor complex terrains. In addition, the three-dimensional laser scanner type chosen for both environments was the same, both as the Velodyne VLP-16 laser scanner.

The hybrid height map method based on Euclidean clustering provided by the invention is compared with the maximum and minimum height difference method and the mean height map method, and the three algorithms are combined with the RANSAC method for filtering. The comparison experiment can fully show that the hybrid height map method based on Euclidean clustering has good performance in the aspect of processing suspended obstacles. Fig. 7a to 8d are experimental results of the husky mobile platform in a gazebo simulation environment and the summit-XL robot in a campus environment, respectively. As can be seen from the figure, both the maximum-minimum height difference method and the mean height map method can accurately detect the ground obstacle, but cannot process the suspended object. Neither of the above two methods can effectively identify the passable area under the crown, which will seriously affect the traveling process of the mobile robot. The point clouds marked by solid line boxes in the figure all belong to parts which are falsely detected as obstacles. The method of the invention has obvious advantages in processing suspended objects in both crown and tunnel. Obviously, the method of the invention effectively solves the problems and enhances the trafficability of the road surface.

The experiment result shows that the hybrid height map method based on Euclidean clustering has good performance in the aspect of processing suspension objects, and the performance of the method on uneven road surfaces is further verified through experiments. The multi-plane extraction method based on random sampling consistency proposed in the present invention is compared with the method of extracting a road surface in document [17 ]. Document [17] corrects the wrongly labeled ground points by dividing the initial point cloud into a plurality of segments along the direction of travel of the robot and fitting a plane to each segment. As shown in fig. 9b and 9c, the planes to which the different segments are fitted are labeled with different gray scales. The multi-plane extraction method based on random sampling consistency is to adaptively divide the multi-plane extraction method into a plurality of segments according to terrain and fit a plane to each segment. The comparison results of the two methods are shown in fig. 9a to 9c and fig. 10a to 10c, and obviously, the point cloud segmentation precision of the multi-plane extraction method based on random sampling consistency is higher, and the method has a better effect.

With respect to time performance; to illustrate the advantage of the multi-resolution grid map approach in reducing time consumption, the experimental protocol performed temporal performance tests in the four environments described above. And respectively carrying out segmentation test on the point cloud under the single-resolution grid map and the multi-resolution grid map. Fig. 11a and 11b show the results of a comparison between two grid resolutions. Due to the high complexity of the first class of environments, the run times at both resolutions are nearly close. However, in the second kind of environment, the point cloud segmentation under multi-resolution only needs 20ms, and the method of the invention improves the time performance by nearly 20%.

Reference to the literature

[1].Quan Q,Jianda H.2.5-dimensional angle potential field algorithm for the real-time autonomous navigation of outdoor mobile robots.Science China Information Sciences,2011,54(10):2100-2112.

[2].Satish K R,Prabir K P.Computing an unevenness field from 3D laser range data to obtain traversable region around a mobile robot.Robotics and Autonomous Systems,2016,84:48-63.

[3].Suger B,Steder B,Burgard W.Traversability analysis for mobile robots in outdoor environments:A semi-supervised learning approach based on 3D-lidar data.Proceedings IEEE International Conference on Robotics and Automation,2015:3941-3946.

[4].Zhu B,Xiong G,Di H,et al.A Novel Method of Traversable Area Extraction Fused With LiDAR Odometry in Off-road Environment.2019IEEE International Conference on Vehicular Electronics and Safety(ICVES),2019.

[5].Papadakis P.Terrain traversability analysis methods for unmanned ground vehicles:A survey.Engineering Applications of Artificial Intelligence,2013,26(4):1373-1385.

[6].Byun J,Na K I,Seo B S,et al.Drivable Road Detection with 3D Point Clouds Based on the MRF for Intelligent Vehicle.Springer Tracts in Advanced Robotics,2015,105:49-60.

[7].Himmelsbach M,Luettel T,Wuensche H J.Real-time object classification in 3D point clouds using point feature histograms.IEEE/RSJ International Conference on Intelligent Robots and Systems,2009:994-1000.

[8].Igor B,Cyrill S.Efficient Online Segmentation for Sparse 3D Laser Scans.Photogrammetrie Fernerkundung Geoinformation,2017,85(1):41-52.

[9].Reina A J,Martinez J L,Mandow A,et al.Collapsible cubes:Removing overhangs from 3D point clouds to build local navigable elevation maps.IEEE/ASME International Conference on Advanced Intelligent Mechatronics,2014.

[10].Thrun S,Montemerlo M,Dahlkamp H,et al.Stanley:The Robot that Won the DARPA Grand Challenge.Journal of Field Robotics,2006,23(9):661-692.

[11].Pfaff P,Triebel R,Burgard W.An Efficient Extension to Elevation Maps for Outdoor Terrain Mapping and Loop Closing.International Journal of Robotics Research,2007,26(2):217-230.

[12].Triebel R,Pfaff P,Burgard W.Multi-Level Surface Maps for Outdoor Terrain Mapping and Loop Closing.IEEE/RSJ International Conference on Intelligent Robots and Systems,2006:2276-2282.

[13].Liu B,Chen X,Han Y,et al.Accelerating DNN-based 3D point cloud processing for mobile computing.Science China Information Sciences,2019,62(11):36-46.

[14].Chu P M,Cho S,Park Y W,et al.Fast point cloud segmentation based on flood-fill algorithm.2017 IEEE International Conference on Multisensor Fusion and Integration for Intelligent Systems(MFI),2017.

[15].Wen C,Sun X,Li J,et al.A deep learning framework for road marking extraction,classification and completion from mobile laser scanning point clouds.ISPRS Journal of Photogrammetry and Remote Sensing,2019,147(1):178-192.

[16].Gao B,Xu A,Pan Y,et al.Off-Road Drivable Area Extraction Using 3D LiDAR Data.2019 IEEE Intelligent Vehicles Symposium(IV),2019.

[17].Zermas D,Izzat I,Papanikolopoulos N.Fast segmentation of 3D point clouds:A paradigm on LiDAR data for autonomous vehicle applications.IEEE International Conference on Robotics and Automation,2017.

[18].Himmelsbach M,Hundelshausen F V,Wuensche H J.Fast segmentation of 3D point clouds for ground vehicles.IEEE Intelligent Vehicles Symposium,2010:560-565.

[19].Douillard B,Underwood J,Kuntz N,et al.On the segmentation of 3D LIDAR point clouds.IEEE International Conference on Robotics and Automation,2011:2798-2805.

[20].Chen T,Dai B,Wang R,et al.Gaussian-Process-Based Real-Time Ground Segmentation for Autonomous Land Vehicles.Journal of Intelligent and Robotic Systems,2014,76(3-4):563-582.

[21].

A,Hermans A,Engelmann F,et al.Multi-Scale Object Candidates for Generic Object Tracking in Street Scenes.IEEE International Conference on Robotics and Automation,2016:3180-3187.

[22].Meng X,Cao Z,Liang S,et al.A terrain description method for traversability analysis based on elevation grid map.International Journal of Advanced Robotic Systems,2018.

[23].Li B.On Enhancing Ground Surface Detection from Sparse Lidar Point Cloud.IEEE/RSJ International Conference on Intelligent Robots and Systems,2019.

[24].Spark D N.Euclidean cluster analysis algorithm.Applied Statistics,1973,22(1):126-130.

[25].Douillard B,Underwood J,Melkumyan N,et al.Hybrid elevation maps:3D surface models for segmentation.IEEE/RSJ International Conference on Intelligent Robots and Systems,2010.

The present invention is not limited to the above-described embodiments. The foregoing description of the specific embodiments is intended to describe and illustrate the technical solutions of the present invention, and the above specific embodiments are merely illustrative and not restrictive. Those skilled in the art can make many changes and modifications to the invention without departing from the spirit and scope of the invention as defined in the appended claims.

Claims (3)

Translated fromChinese

1.一种基于欧式聚类的多平面提取方法，其特征在于，包括以下步骤：1. a multi-plane extraction method based on Euclidean clustering, is characterized in that, comprises the following steps:(1)采用极坐标栅格表示地图；(1) The polar coordinate grid is used to represent the map;通过二维极坐标栅格将地图按如下方式进行划分：以机器人所在位置为中心、前进方向为起始边界线，按逆时针方向将地图均匀划分为m个扇形块，对于每一个扇形块s_i，沿半径方向等间距划分为n个子块；将机器人上三维激光扫描仪得到的三维点云数据投影到二维极坐标栅格；The map is divided by the two-dimensional polar coordinate grid as follows: with the position of the robot as the center and the forward direction as the starting boundary line, the map is evenly divided into m sector-shaped blocks in the counterclockwise direction, and for each sector-shaped block s_i , divided into n sub-blocks at equal intervals along the radial direction; project the 3D point cloud data obtained by the 3D laser scanner on the robot to a 2D polar coordinate grid;(2)遍历地图中的所有栅格，运用基于欧式聚类分割的混合高度图方法对栅格属性进行初步判断；具体如下：(2) Traverse all the grids in the map, and use the hybrid height map method based on Euclidean clustering to make a preliminary judgment on the grid attributes; the details are as follows:当某一栅格的高度差小于给定阈值T₁时，将该栅格中所有的点云都加入到准地面点集合中；如果栅格的高度差大于给定阈值T₁，则判定对应栅格为不确定栅格；When the height difference of a grid is less than the given threshold T₁ , all point clouds in the grid are added to the quasi-ground point set; if the height difference of the grid is greater than the given threshold T₁ , it is determined that the corresponding The grid is an indeterminate grid;通过欧式聚类分割方法对不确定栅格中的点云进行分组并计算每组中点云的高度平均值，并从各组点云中选取高度平均值最小的一组点云作为基准组，其他组作为比较组；然后，将各个比较组中点云高度最小的值与基准组中点云高度最大的值进行比较；如果所有比较组和基准组之间的高度差值均大于可通行间隙阈值T₂，则将基准组中的点云加入到准地面点集合中，而将其余所有的比较组中的点云加入到障碍物点集合中；The point clouds in the uncertain grid are grouped by the Euclidean clustering segmentation method, and the average height of the point clouds in each group is calculated, and the group of point clouds with the smallest height average is selected from each group of point clouds as the reference group. The other groups are used as comparison groups; then, the value with the smallest point cloud height in each comparison group is compared with the value with the largest point cloud height in the reference group; if the height difference between all comparison groups and the reference group is greater than the passable gap If the threshold value T₂ is set, the point clouds in the benchmark group are added to the quasi-ground point set, and the point clouds in all other comparison groups are added to the obstacle point set;(3)通过随机采样一致性的多平面提取方法对基于欧式聚类分割的混合高度图方法得到的准地面点进行再次滤波处理，具体如下：(3) The quasi-ground points obtained by the hybrid height map method based on Euclidean clustering segmentation are filtered again by the multi-plane extraction method of random sampling consistency, as follows:(301)针对基于欧式聚类分割方法得到的准地面点，获取平面参数并建立平面模型如式(3)所示:(301) For the quasi-ground points obtained based on the Euclidean clustering segmentation method, obtain plane parameters and establish a plane model as shown in formula (3):ax+by+cz+d＝0 (3)ax+by+cz+d=0 (3)平面的法向量为：

以机器人车体坐标系z轴方向的单位向量

作为参考法向量；拟合平面的法向量

与参考法向量

之间的夹角θ小于最大坡度阈值θ_th；夹角θ的计算方式如式(4)所示：The normal vector of the plane is:

Unit vector in the z-axis direction of the robot body coordinate system

as the reference normal; the normal of the fitted plane

with the reference normal vector

The included angle θ between them is less than the maximum gradient threshold θ_th ; the calculation method of the included angle θ is shown in formula (4):

(302)将基于欧式聚类分割方法得到的准地面点作为随机采样一致性方法的输入测量点序列进行平面模型提取，得到拟合平面的法向量

利用式(4)计算地平面法向量与参考平面法向量之间的夹角，若小于最大坡度阈值θ_th，则将拟合平面的内点加入最终地面点集合中；(302) Use the quasi-ground points obtained based on the Euclidean clustering and segmentation method as the input measurement point sequence of the random sampling consistency method to extract the plane model, and obtain the normal vector of the fitted plane

Use formula (4) to calculate the angle between the ground plane normal vector and the reference plane normal vector, if it is less than the maximum gradient threshold θ_th , then add the interior point of the fitted plane to the final ground point set;(303)而拟合平面的外点则继续作为输入测量点序列进行平面模型提取，如果满足约束条件则按(301)中步骤处理，否则将本次拟合平面的内点加入障碍物点集中；(303) The outer points of the fitted plane continue to be used as the input measurement point sequence to extract the plane model. If the constraints are met, the steps in (301) are followed. Otherwise, the inner points of the fitted plane are added to the obstacle point set. ;(304)剩余的外点则按照步骤(302)重复进行操作，直到剩余的外点数量小于设定的阈值T_num。(304) For the remaining outliers, repeat the operation according to step (302) until the number of remaining outliers is less than the set threshold T_num .2.根据权利要求1所述一种基于欧式聚类的多平面提取方法，其特征在于，步骤(1)中，假设当前时刻t下三维激光扫描仪得到的无序的三维点云数据为P_t＝{p₁,…,p_m,…,p_N}，即共包含N个激光数据点；将上述三维点云数据投影到二维极坐标栅格中，对每一个激光数据点p_m＝(x_m,y_m,z_m)^T，其映射方式如下：2. a kind of multi-plane extraction method based on Euclidean clustering according to claim 1, is characterized in that, in step (1), suppose the disordered three-dimensional point cloud data that three-dimensional laser scanner obtains under current moment t is P_t = {p₁ ,...,p_m ,...,p_N }, that is, it contains N laser data points in total; the above three-dimensional point cloud data is projected into a two-dimensional polar coordinate grid, and for each laser data point p_m ＝(x_m , y_m , z_m )^T , which is mapped as follows:

其中，θ是三维数据点p_m在投影平面内与X轴正方向之间的夹角，大小范围为[0,2π]；Δth和Δr分别表示角度分辨率和半径分辨率，二者共同决定栅格的大小；而floor是一个向下取整函数，用于计算投影栅格在地图中的索引位置(s_i,b_j)。Among them, θ is the angle between the three-dimensional data point p_m in the projection plane and the positive direction of the X axis, and the size range is [0, 2π]; Δth and Δr represent the angular resolution and the radius resolution, respectively, which are jointly determined by the two The size of the raster; while floor is a round-down function that calculates the index position (s_i , b_j ) of the projected raster in the map.3.根据权利要求1所述一种基于欧式聚类的多平面提取方法，其特征在于，步骤(2)中，首先在低分辨率栅格下运用最大最小高度差法进行判断，如果满足对应栅格的高度差小于给定阈值T₁则直接将该栅格内的点云加入到准地面点集合中，否则转入高分辨率的栅格下重新进行判断；如果直至最高分辨率层，栅格内点云的最大最小高度差值仍然大于给定阈值T₁，则判定对应栅格为不确定栅格。3. a kind of multi-plane extraction method based on Euclidean clustering according to claim 1, is characterized in that, in step (2), at first under low-resolution grid, use maximum and minimum height difference method to judge, if satisfy corresponding If the height difference of the grid is less than the given threshold T₁ , the point cloud in the grid is directly added to the quasi-ground point set, otherwise it is transferred to the high-resolution grid for re-judgment; if it reaches the highest resolution layer, If the maximum and minimum height differences of the point clouds in the grid are still greater than the given threshold T₁ , the corresponding grid is determined to be an uncertain grid.

Images