Unmanned vehicle ground point cloud segmentation method in unstructured environment

Abstract

The invention discloses an unmanned vehicle ground point cloud segmentation method in an unstructured environment, and belongs to the technical field of vehicle automatic driving. The ground point cloud segmentation method comprises the following steps: S1, preprocessing original laser radar point cloud data; iMU pre-integration is adopted to carry out motion distortion correction on the laser radar point cloud, and the point cloud height and the reflection intensity are utilized to remove ground noise; and S2, segmenting the ground point cloud. The method comprises the following steps of: dividing a radar sensing area by using a dynamic Concentric Zone Model (DCZM); and segmenting the ground point cloud through ground initial fitting point screening and a ground fitting algorithm. According to the invention, more accurate ground point cloud segmentation in a non-structural environment is realized.

Description

Technical Field

[0001] The present invention belongs to the technical field of vehicle automatic driving, and in particular relates to a ground point cloud segmentation method for an unmanned vehicle in an unstructured environment. Background Art

[0002] Autonomous driving vehicles mainly include four modules: perception, planning, control, and decision-making. These modules work together to ensure that unmanned vehicles can drive safely and efficiently in complex environments. The environmental perception module is responsible for collecting and processing information about the surrounding environment, and is a key technology to ensure safe and stable driving of vehicles. 3D LiDAR has become the core sensor for environmental perception of unmanned vehicles due to its high detection accuracy, wide perception range, and strong anti-interference ability. LiDAR-based perception tasks mainly include ground point cloud segmentation and obstacle detection, identification, and tracking, among which ground point cloud segmentation is the basis for subsequent perception tasks.

[0003] At present, significant progress has been made in dealing with ground point cloud segmentation problems in structured environments with flat terrain. In complex and changeable unstructured environments with irregular obstacles, existing LiDAR ground point cloud segmentation methods have poor real-time performance and problems of over-segmentation and under-segmentation of point clouds. However, in complex and changeable unstructured environments, unmanned vehicles usually travel on multi-plane rugged roads, which leads to poor real-time performance and over-segmentation and under-segmentation of ground point clouds in existing ground point cloud segmentation algorithms. Therefore, the ground point cloud segmentation problem in this environment is still challenging. Summary of the invention

[0004] A ground point cloud segmentation method for an unmanned vehicle in an unstructured environment comprises the following steps:

[0005] Preprocessing includes: using IMU pre-integration to correct motion distortion of the original lidar point cloud and using point cloud height and reflection intensity to remove ground noise;

[0006] The specific steps are as follows:

[0007] S1, preprocessing the original lidar point cloud data;

[0008] The original LiDAR point cloud data is obtained by laser scanner;

[0009] Preprocessing includes: using IMU pre-integration to correct motion distortion of the original lidar point cloud and using point cloud height and reflection intensity to remove ground noise;

[0010] The specific steps are as follows:

[0011] S1.1. Use IMU pre-integration to correct motion distortion of the original lidar point cloud; the details are as follows:

[0012] The angular velocity ω of the smart car measured by IMU k , acceleration a k To update the posture of smart cars k and speed v k ;

[0013] Specifically, the position of the smart car is obtained by velocity integration; the expression is as follows:

[0014]

[0015] In the formula, R k is the rotation matrix at the kth moment; [ω k ] × is the angular velocity ω k The antisymmetric matrix of Δt k is the time interval from the kth to the k+1th moment; v k is the speed at the kth moment; a k is the acceleration at the kth moment; g is the local gravity acceleration; s k is the position at the kth moment;

[0016] After completing the integration of the smart car's posture and velocity, the original lidar point cloud data is converted to the IMU coordinate system; the expression is as follows:

[0017] p I (k) = R L→I p L,raw (k)+T L→I

[0018] In the formula, p I (k) is the result of transforming the laser radar point cloud into the IMU coordinate system at the kth moment; R L→I is the rotation matrix from radar to IMU; P L,raw is the original point cloud in the radar coordinate system at the kth moment; T L→I The translation matrix from radar to IMU;

[0019] After the original LiDAR point cloud coordinates are converted to the IMU coordinate system, the IMU pre-integration results are used to correct the point cloud motion distortion; the expression is as follows:

[0020] P I,corrected (k) = R I (k)P I (k)+s I (k)

[0021] In the formula, R I (k) is the rotation matrix of the point cloud sampling time in the IMU coordinate system at the kth moment; s I (k) is the displacement of the point cloud sampling time in the IMU coordinate system at the kth moment; PI,corrected (k) is the radar point cloud after distortion correction in the IMU coordinate system at the kth moment;

[0022] The corrected point cloud is transformed into the radar coordinate system; the expression is as follows:

[0023] P L,corrected (k) = R I→L P I,corrected (k)+T I→L

[0024] In the formula, R I→L T is the rotation matrix from IMU to radar; I→L is the translation matrix from IMU to radar; P L,corrected (k) is the point cloud after distortion correction at the kth moment;

[0025] S1.2, using point cloud height and reflection intensity to remove ground noise;

[0026] LiDAR’s perception of the surrounding environment relies heavily on the reflection of the laser beam from the surface of the object;

[0027] The definition of ground noise is as follows:

[0028]

[0029] Where P i represents the i-th point; N is the set of ground noise points; z i is the height coordinate of the i-th point; h s is the installation height of the laser radar; δ is the safety offset; I i is the reflection intensity of the i-th point; η thr is the reflection intensity threshold;

[0030] Traverse all point cloud data, delete all defined ground noise points, and complete the ground noise removal operation;

[0031] S2. Project the preprocessed point cloud data into the dynamic concentric area model, segment the ground point cloud through the ground initial fitting point screening and ground fitting algorithm, and obtain the final result; the details are as follows:

[0032] S2.1, projecting the preprocessed radar point cloud data into a dynamic concentric zone model (DCZM);

[0033] The dynamic concentric zone model is defined as follows:

[0034]

[0035] Where DC represents the dynamic concentric zone model; Zm represents the mth region; n represents the number of regions;

[0036] Furthermore, Z m The expression is as follows:

[0037] Z m = {P k |L min,m ≤P k <L max,m}

[0038] Where P k Z m Point in the area; L min,m With L max,m Respectively represent the minimum and maximum radial boundaries of the mth region; Z m is further divided into N r,m ×N θ,m grids, N r,m Z m The number of rings in the area. Different areas have different numbers of rings. N θ,m Z m The radial number of the region is expressed as follows:

[0039]

[0040] Where N sp,m To be assigned to Z m The total number of point clouds in the area; N p,m Z m The number of point clouds contained in each grid cell of the region; the size of the grid cells in each region is different, and as the environment changes, the area of ​​the grid cells in each region will change accordingly;

[0041] Z m Each grid cell S of the region B,C,m , defined as follows:

[0042]

[0043] In the formula, Indicates point P k The radial distance to the origin; θ k =arctan2(y k ,x k ) represents the polar angle of a point; ΔL m =L max,m -L min,m ; B, C represent the position of the laser radar point cloud in the grid, B represents the ring where the laser radar point cloud is located, and C represents the radial direction where the laser radar point cloud is located;

[0044] S2.2. For the grid unit S in the dynamic concentric zone model B,C,m The point cloud in the grid is used to screen the initial ground fitting points;

[0045] Specifically, from S B,C,m The first W points with the smallest z value are selected from the inner point set to calculate the representative height of the ground seed point; the expression is as follows:

[0046]

[0047] In the formula, h LPR The ground seed point represents the height; z i is the height of the i-th point among the first W points with the smallest z value. The W value is different in different regions.

[0048] The ground seed point set is defined as follows:

[0049] S seeds ={p i ∈S B,C,m Z i <h LPR +Δh seeds}

[0050] In the formula, S seeds is the ground seed point set; Δh seeds is the height threshold of the ground seed point. This parameter can improve the robustness of the initial ground fitting point set.

[0051] S2.3, for the grid unit S in the dynamic concentric zone model B,C,m The point cloud within is ground fitted;

[0052] Take a single grid cell S B,C,m As an example, the ground point cloud segmentation is performed using the ground fitting algorithm;

[0053] Furthermore, let C be S B,C,m The covariance matrix of the midpoint cloud:

[0054]

[0055] Where P i is the three-dimensional coordinate vector of the i-th point; μ is the mean vector of the point cloud data; r is the grid unit S B,C,m The number of point clouds in ;

[0056] Through eigenvalue decomposition, the eigenvalues ​​and eigenvectors corresponding to the covariance matrix C are obtained; the expression is as follows:

[0057] Cv A =λ A v A

[0058] In the formula, λ A is the eigenvalue of C; v A is A The corresponding eigenvectors, A = 1, 2, 3; assuming λ 1 ≤λ 2 ≤λ 3 , eigenvalue λ 1 The corresponding eigenvector ν 1 represents the normal vector of the ground plane; according to the eigenvector ν 1 , the ground plane coefficient d can be expressed as:

[0059] d=-ν 1 T μ

[0060] Where μ is the mean vector of point cloud data;

[0061] The obtained ground seed point set S seeds Set as Assume that the estimated ground point set in the tth iteration is Then the estimated ground point set in the t+1th iteration is The expression is as follows:

[0062]

[0063] In the formula, Represents the grid unit S B,C,m The value of the ground plane coefficient d at the tth iteration; represents the distance from point i to the ground; τ d Indicates the ground plane boundary distance;

[0064] τ d The introduction of enhances the robustness of the system in unstructured environments. The ground in unstructured environments is not an ideal geometric plane, but has certain height changes. For example, soil and grass have undulating characteristics. d The introduction of will make the system retain these natural characteristics;

[0065] S2.4. Merge the ground point sets of all grids in the dynamic concentric area model to obtain the ground point set of the entire point cloud space.

[0066] Beneficial effects of the present invention

[0067] The present invention combines lidar and IMU data by adopting multi-sensor fusion technology to effectively remove radar point cloud distortion; obtains point cloud spatial distribution characteristics by projecting radar point cloud onto a dynamic concentric area model, thereby reducing algorithm calculation complexity and improving algorithm real-time performance; adopts an iterative ground fitting algorithm to segment the ground point cloud, reducing the problems of over-segmentation and under-segmentation of the ground point cloud. BRIEF DESCRIPTION OF THE DRAWINGS

[0068] Figure 1 is a flow chart of the present invention;

[0069] Figure 2 It is a schematic diagram of the method of the present invention;

[0070] Figure 3 Generate a schematic diagram for ground noise points;

[0071] Figure 4 It is a schematic diagram of the dynamic concentric zone model changes;

[0072] Figure 5 It is a schematic diagram of the ground fitting algorithm iteration;

[0073] Figure 6 It is the experimental platform described in this embodiment;

[0074] Figure 7 This is a comparison chart of different ground point cloud segmentation algorithms in this embodiment, where yellow represents ground points and purple represents non-ground points. The marks in the figure are all ground points that are mistakenly identified as non-ground points or non-ground points that are mistakenly identified as ground points; among them, part (a) is the experimental scene, part (b) is the LineFit ground point cloud segmentation algorithm; part (c) is the GPF ground point cloud segmentation algorithm; part (d) is the Patchwork ground point cloud segmentation algorithm; part (e) is the Patchwork++ ground point cloud segmentation algorithm; part (f) is the ground point cloud segmentation result chart of Example 1 of the present invention. DETAILED DESCRIPTION

[0075] The present invention is further described in detail below in conjunction with specific embodiments.

[0076] like Figure 1 , Figure 2 As shown, a ground point cloud segmentation method for an unmanned vehicle in an unstructured environment includes the following steps:

[0077] S1, preprocessing the original lidar point cloud data;

[0078] The original LiDAR point cloud data is obtained by laser scanner;

[0079] Preprocessing includes: using IMU pre-integration to correct motion distortion of the original lidar point cloud and using point cloud height and reflection intensity to remove ground noise;

[0080] The specific steps are as follows:

[0081] S1.1. Use IMU pre-integration to correct motion distortion of the original lidar point cloud; the details are as follows:

[0082] The angular velocity ω of the smart car measured by IMU k , acceleration a k To update the posture of smart cars k and speed v k ;

[0083] Specifically, the position of the smart car is obtained by velocity integration; the expression is as follows:

[0084]

[0085] In the formula, R k is the rotation matrix at the kth moment; [ω k ] × is the angular velocity ω k The antisymmetric matrix of Δt k is the time interval from the kth to the k+1th moment; v k is the speed at the kth moment; a k is the acceleration at the kth moment; g is the local gravity acceleration; s k is the position at the kth moment;

[0086] After completing the integration of the smart car's posture and velocity, the original lidar point cloud data is converted to the IMU coordinate system; the expression is as follows:

[0087] p I (k) = R L→I p L,raw (k)+T L→I

[0088] Where P I (t k ) is the result of transforming the laser radar point cloud into the IMU coordinate system at the kth moment; R L→I is the rotation matrix from radar to IMU. P L,raw is the original point cloud in the radar coordinate system at the kth moment; T L→I is the translation matrix from radar to IMU. In this embodiment, T L→I =[0.002-0.0070.400];

[0089] After the original LiDAR point cloud coordinates are converted to the IMU coordinate system, the IMU pre-integration results are used to correct the point cloud motion distortion; the expression is as follows:

[0090] P I,corrected (k) = R I (k)P I (k)+s I (k)

[0091] In the formula, p I (k) is the rotation matrix of the point cloud sampling time in the IMU coordinate system at the kth moment; s I (k) is the displacement of the point cloud sampling time in the IMU coordinate system at the kth moment; P I,corrected (k) is the radar point cloud after distortion correction in the IMU coordinate system at the kth moment;

[0092] The corrected point cloud is transformed into the radar coordinate system; the expression is as follows:

[0093] P L,corrected (k) = R I→L P I,corrected (k)+T I→L

[0094] In the formula, R I→L is the rotation matrix from IMU to radar. In this embodiment T I→L is the translation matrix from IMU to radar. In this embodiment, T I→L =[0.005-0.010-0.400]; P L,corrected (k) is the point cloud after distortion correction at the kth moment;

[0095] S1.2, using point cloud height and reflection intensity to remove ground noise;

[0096] LiDAR’s perception of the surrounding environment relies heavily on the reflection of the laser beam from the surface of the object;

[0097] like Figure 3 As shown, the generation of ground noise points is usually due to certain materials in the unstructured environment, such as leaves and plastics, which cause part of the laser beam to penetrate the ground and propagate below the ground, thereby generating false points below the actual ground. These noise points below the ground have the characteristics of low ground height values ​​and low reflection intensity.

[0098] The definition of ground noise is as follows:

[0099]

[0100] Where P i represents the i-th point; N is the set of ground noise points; z iis the height coordinate of the i-th point; h s is the installation height of the laser radar. In this embodiment, h s =1.732; δ is the safety offset, in this embodiment, δ=1.0; I i is the reflection intensity of the i-th point; η thr is the reflection intensity threshold, in this embodiment, η thr =0.2;

[0101] Traverse all point cloud data, delete all defined ground noise points, and complete the ground noise removal operation;

[0102] S2. Project the preprocessed point cloud data into the dynamic concentric area model, segment the ground point cloud through the ground initial fitting point screening and ground fitting algorithm, and obtain the final result; the details are as follows:

[0103] S2.1, projecting the preprocessed radar point cloud data into a dynamic concentric zone model (DCZM);

[0104] Based on the geometric characteristics of radar point cloud data, DCZM constructs Figure 4 As shown in the figure, the radar sensing area is divided into several sub-areas, each of which is composed of grid units of different sizes. As the environment changes, the size of each grid unit will be dynamically adjusted;

[0105] The dynamic concentric zone model is defined as follows:

[0106]

[0107] Where DC represents the dynamic concentric zone model; Z m represents the mth region, in this embodiment, m={1,2,3,4}; n represents the number of regions, in this embodiment, n=4 is set based on experience;

[0108] Furthermore, Z m The expression is as follows:

[0109] Z m = {P k |L min,m ≤P k <L max,m}

[0110] Where P k Z m Point in the area; L min,m With L max,m Respectively represent the minimum and maximum radial boundaries of the mth region; Z m is further divided into N r,m ×Nθ,m grids, N r,m Z m The number of rings in the area. Different areas have different numbers of rings. In this embodiment, it is set to {2, 3, 3, 4} based on experience; N θ,m Z m The radial number of the region is expressed as follows:

[0111]

[0112] Where N sp,m To be assigned to Z m The total number of point clouds in the area; N p,m Z m The number of point clouds contained in each grid unit in the region, in this embodiment, N p,m ={700,150,40,20}; The size of the grid cells in each region is different, and as the environment changes, the area of ​​the grid cells in each region will change accordingly;

[0113] Z m Each grid cell S of the region B,C,m , defined as follows:

[0114]

[0115] In the formula, Indicates point P k The radial distance to the origin; θ k =arctan2(y k ,x k ) represents the polar angle of a point; ΔL m =L max,m -L min,m , when m=1,2,3, L max,m =L min,m+1 , L max,4 =L max , L min,1 =L min , L max With L min are the global maximum and minimum boundaries respectively; B and C represent the position of the LiDAR point cloud in the grid, B represents the ring where the LiDAR point cloud is located, and C represents the radial direction where the LiDAR point cloud is located;

[0116] S2.2. For the grid unit S in the dynamic concentric zone model B,C,m The point cloud in the ground is used to screen the initial fitting points;

[0117] Specifically, from S B,C,m The first W points with the smallest z value are selected from the inner point set to calculate the representative height of the ground seed point; the expression is as follows:

[0118]

[0119] In the formula, h LPR The ground seed point represents the height; z i is the height of the i-th point among the first W points with the smallest z value. In different regions, the W value is different. In this embodiment, it is set to {30, 20, 8, 5} based on experience;

[0120] The ground seed point set is defined as follows:

[0121] S seeds ={p i ∈S B,C,m Z i <h LPR +Δh seeds}

[0122] In the formula, S seeds is the ground seed point set; Δh seeds is the height threshold of the ground seed point. This parameter can improve the robustness of the initial ground fitting point set. In this embodiment, Δh is set based on experience. seeds = -2;

[0123] S2.3, for the grid unit S in the dynamic concentric zone model B,C,m The point cloud within is ground fitted;

[0124] like Figure 5 As shown, a single grid unit S B,C,m As an example, the ground point cloud segmentation is performed using the ground fitting algorithm;

[0125] Furthermore, let C be S B,C,m The covariance matrix of the midpoint cloud:

[0126]

[0127] Where P i is the three-dimensional coordinate vector of the i-th point; μ is the mean vector of the point cloud data; r is the grid unit S B,C,m The number of point clouds in ;

[0128] Through eigenvalue decomposition, the eigenvalues ​​and eigenvectors corresponding to the covariance matrix C are obtained; the expression is as follows:

[0129] Cv A =λ A v A

[0130] In the formula, λ A is the eigenvalue of C; v A is A The corresponding eigenvectors, A = 1, 2, 3; assuming λ1 ≤λ 2 ≤λ 3 , eigenvalue λ 1 The corresponding eigenvector ν 1 represents the normal vector of the ground plane; according to the eigenvector ν 1 , the ground plane coefficient d can be expressed as:

[0131] d=-ν 1 T μ

[0132] Where μ is the mean vector of point cloud data;

[0133] The obtained ground seed point set S seeds Set as Assume that the estimated ground point set in the tth iteration is Then the estimated ground point set in the t+1th iteration is The expression is as follows:

[0134]

[0135] In the formula, Represents the grid unit S B,C,m The value of the ground plane coefficient d at the tth iteration; represents the distance from point i to the ground; τ d Indicates the ground plane boundary distance. In this embodiment, τ is set based on experience. d =-0.2; τ d The introduction of enhances the robustness of the system in unstructured environments. The ground in unstructured environments is not an ideal geometric plane, but has certain height changes. For example, soil and grass have undulating characteristics. d The introduction of will make the system retain these natural characteristics;

[0136] In this embodiment, three iterations are performed, and the estimated ground point set in the second iteration is but The expression is as follows:

[0137]

[0138] S2.4, merging the ground point sets of all grids in the dynamic concentric area model to obtain the ground point set of the entire point cloud space;

[0139] use Figure 6The experimental platform shown in the figure verifies the effect of the ground point cloud segmentation algorithm for intelligent vehicles in unstructured environments described in the present invention. The platform is an independently designed all-terrain unmanned vehicle equipped with a monocular camera, a 16-line laser radar, an inertial measurement system IMU, and an industrial computer. All sensor data has been connected to the industrial computer and can be obtained through the topic communication method of ROS. The experimental scene is selected as follows Figure 7 (a) shows a jungle environment with a significant slope.

[0140] By comparing the LineFit algorithm, GPF algorithm, Patchwork algorithm, Patchwork++ algorithm and the algorithm described in the present invention: during the experiment, the experimental platform is controlled to collect data around the experimental scene, and then the LineFit algorithm, GPF algorithm, Patchwork algorithm, Patchwork++ algorithm and the algorithm described in the present invention are used for processing respectively. Figure 7 The point cloud segmentation images obtained by different methods can be seen. Compared with other algorithms, the algorithm of the present invention has a lower probability of ground mis-segmentation and is more robust. It also shows that the ground point cloud segmentation method for intelligent vehicles in unstructured environments described in the present invention has high application value, can provide more accurate environmental perception for autonomous vehicles, and promote the development of autonomous vehicles.

[0141] Finally, it should be noted that the above embodiments are only used to illustrate the technical solution of the present invention rather than to limit the scope of protection of the present invention. Although the present invention has been described in detail with reference to the preferred embodiments, those skilled in the art should understand that the technical solution of the present invention can be modified or replaced by equivalents without departing from the essence and scope of the technical solution of the present invention.

Claims

1. A ground point cloud segmentation method for unmanned vehicles in an unstructured environment, characterized in that: The steps include: S1, preprocessing the original lidar point cloud data; The preprocessing includes: using IMU pre-integration to correct motion distortion of the original laser radar point cloud and using point cloud height and reflection intensity to remove ground noise; S2. Project the preprocessed point cloud data into the dynamic concentric area model, segment the ground point cloud through the ground initial fitting point screening and ground fitting algorithm to obtain the final result.

2. The method for ground point cloud segmentation of an unmanned vehicle in an unstructured environment according to claim 1, characterized in that: The use of IMU pre-integration to correct motion distortion of the original lidar point cloud is specifically as follows: S1.

1. Use IMU pre-integration to correct motion distortion of the original lidar point cloud; the details are as follows: The angular velocity ω of the smart car measured by IMU k , acceleration a k To update the posture of smart cars k and speed v k ; Specifically, the position of the smart car is obtained by velocity integration; the expression is as follows: In the formula, R k is the rotation matrix at the kth moment; [ω k ] × is the angular velocity ω k The antisymmetric matrix of Δt k is the time interval from the kth to the k+1th moment; v k is the speed at the kth moment; a k is the acceleration at the kth moment; g is the local gravity acceleration; s k is the position at the kth moment; After completing the integration of the smart car's posture and velocity, the original lidar point cloud data is converted to the IMU coordinate system; the expression is as follows: p I (k)=R L→I p L,raw (k)+T L→I In the formula, p I (k) is the result of transforming the laser radar point cloud into the IMU coordinate system at the kth moment; R L→I is the rotation matrix from radar to IMU; P L,raw is the original point cloud in the radar coordinate system at the kth moment; T L→I The translation matrix from radar to IMU; After the original LiDAR point cloud coordinates are converted to the IMU coordinate system, the IMU pre-integration results are used to correct the point cloud motion distortion; the expression is as follows: P I,corrected (k)=R I (k)P I (k)+s I (k) In the formula, R I (k) is the rotation matrix of the point cloud sampling time in the IMU coordinate system at the kth moment; s I (k) is the displacement of the point cloud sampling time in the IMU coordinate system at the kth moment; P I,corrected (k) is the radar point cloud after distortion correction in the IMU coordinate system at the kth moment; The corrected point cloud is transformed into the radar coordinate system; the expression is as follows: P L,corrected (k)=R I→L P I,corrected (k)+T I→L In the formula, R I→L T is the rotation matrix from IMU to radar; I→L is the translation matrix from IMU to radar; P L,corrected (k) is the point cloud after distortion correction at the kth moment.

3. The method for ground point cloud segmentation of an unmanned vehicle in an unstructured environment according to claim 1, characterized in that: The method of removing ground noise by using point cloud height and reflection intensity is as follows: S1.2, using point cloud height and reflection intensity to remove ground noise; The definition of ground noise is as follows: Where P i represents the i-th point; N is the set of ground noise points; z i is the height coordinate of the i-th point; h s is the installation height of the laser radar; δ is the safety offset; I i is the reflection intensity of the i-th point; η thr is the reflection intensity threshold; Traverse all point cloud data, delete all defined ground noise points, and complete the ground noise removal operation.

4. The method for ground point cloud segmentation of an unmanned vehicle in an unstructured environment according to claim 1, characterized in that: The specific steps of projecting the pre-processed point cloud data onto the dynamic concentric area model and segmenting the ground point cloud by screening the ground initial fitting points and using the ground fitting algorithm are as follows: S2.1, projecting the preprocessed radar point cloud data into the dynamic concentric zone model; The dynamic concentric zone model is defined as follows: Where DC represents the dynamic concentric zone model; Z m represents the mth region; n represents the number of regions; Furthermore, Z m The expression is as follows: Z m ={P k |L min ,m≤P k <L max,m } Where P k Z m Point in the area; L min,m With L max,m Respectively represent the minimum and maximum radial boundaries of the mth region; Z m is further divided into N r,m ×N θ,m grids, N r,m Z m The number of rings in the area. Different areas have different numbers of rings. N θ,m Z m The radial number of the region is expressed as follows: Where N sp,m To be assigned to Z m The total number of point clouds in the area; N p,m Z m The number of point clouds contained in each grid cell of the region; the size of the grid cells in each region is different, and as the environment changes, the area of ​​the grid cells in each region will change accordingly; Z m Each grid cell S of the region B,C,m , defined as follows: In the formula, Indicates point P k The radial distance to the origin; θ k =arctan2(y k ,x k ) represents the polar angle of a point; ΔL m =L max,m -L min,m ; B, C represent the position of the laser radar point cloud in the grid, B represents the ring where the laser radar point cloud is located, and C represents the radial direction where the laser radar point cloud is located; S2.

2. For the grid unit S in the dynamic concentric zone model B,C,m The point cloud in the grid is used to screen the initial ground fitting points; Specifically, from S B,C,m The first W points with the smallest z value are selected from the inner point set to calculate the representative height of the ground seed point; the expression is as follows: In the formula, h LPR The ground seed point represents the height; z i is the height of the i-th point among the first W points with the smallest z value. The W value is different in different regions. The ground seed point set is defined as follows: S seeds ={p i ∈S B,C,m Z i <h LPR +Δh seeds } In the formula, S seeds is the ground seed point set; Δh seeds is the ground seed point height threshold; S2.3, for the grid unit S in the dynamic concentric zone model B,C,m The point cloud within is ground fitted; When a single grid cell S is used B,C,m When , the ground fitting algorithm is used to segment the ground point cloud; Furthermore, let C be S B,C,m The covariance matrix of the midpoint cloud: Where P i is the three-dimensional coordinate vector of the i-th point; μ is the mean vector of the point cloud data; r is the grid unit S B,C,m The number of point clouds in ; Through eigenvalue decomposition, we can get the eigenvalue and eigenvector corresponding to the covariance matrix C; the expression as follows: Cv A =λ A v A In the formula, λ A is the eigenvalue of C; v A is A The corresponding eigenvectors are A=1, 2, 3. When λ1≤λ2≤λ3, the eigenvector ν1 corresponding to the eigenvalue λ1 represents the ground plane normal vector. According to the eigenvector ν1, the ground plane coefficient d can be expressed as: d=-ν1 T m Where μ is the mean vector of point cloud data; The obtained ground seed point set S seeds Set as Assume that the estimated ground point set in the tth iteration is Then the estimated ground point set in the t+1th iteration is The expression is as follows: In the formula, Represents the grid unit S B,C,m The value of the ground plane coefficient d at the tth iteration; represents the distance from point i to the ground; τ d Indicates the ground plane boundary distance; τ d The introduction of enhances the robustness of the system in unstructured environments. The ground in unstructured environments is not an ideal geometric plane, but has certain height changes. For example, soil and grass have undulating characteristics. d The introduction of will make the system retain these natural characteristics; S2.

4. Merge the ground point sets of all grids in the dynamic concentric area model to obtain the ground point set of the entire point cloud space.

Images