Ground point cloud extraction method based on multiple sensors

Abstract

The invention discloses a ground point cloud extraction method based on multiple sensors, and the method comprises the following steps: S1, carrying out the ground plane information estimation of the point cloud data of each sensor in parallel through employing a method based on singular value decomposition; s2, fusing each piece of ground plane information into unified ground plane information; and S3, traversing all sensor point clouds, calculating the distance between the sensor point clouds and the unified ground plane, if the distance is smaller than a threshold value, considering that the point belongs to the ground point cloud, otherwise, not considering that the point belongs to the ground point cloud. The ground point cloud information of each sensor is calculated in parallel, and the real-time performance of the method is ensured; ground point cloud information is efficiently fused to form a unified ground plane, and the ground plane estimation is accurate and reliable; ground point cloud information is maintained based on a threshold value, and efficient and accurate ground point cloud extraction is ensured.

Description

Technical Field

[0001] The invention relates to the technical field of navigation map construction, and in particular to a ground point cloud extraction method based on multiple sensors. Background Art

[0002] The multi-sensor based ground point cloud extraction method is a technology that uses data obtained by multiple sensors (such as lidar, camera, radar, etc.) and combines the information of multiple sensors to extract ground point cloud information. This method is usually used in map making, 3D modeling, urban planning and other fields. The multi-sensor based ground point cloud extraction method can obtain ground information more comprehensively and improve the efficiency and accuracy of map making and urban planning. However, this method also faces challenges such as high complexity of data fusion, algorithm selection and parameter tuning. In practical applications, it is necessary to select appropriate methods and tools according to specific scenarios and needs to achieve ground point cloud extraction.

[0003] Existing ground point cloud extraction methods mainly focus on single sensor devices. The number of point clouds after multi-sensor fusion is large and there are high requirements for the time synchronization of sensor point clouds. In order to realize the obstacle avoidance function by extracting ground point clouds for autonomous platforms, the present invention proposes a ground point cloud extraction method based on multi-sensors, which can complete the extraction of ground point clouds with high efficiency. Summary of the invention

[0004] In order to solve the above technical deficiencies, the present invention adopts a modified technical solution, a ground point cloud extraction method based on multiple sensors, comprising the following steps:

[0005] S1, uses a method based on singular value decomposition to estimate the ground plane information of each sensor's point cloud data in parallel;

[0006] S2, fuse each ground plane information into a unified ground plane information;

[0007] S3, traverses all sensor point clouds and calculates their distance to the unified ground plane. If the distance is less than the threshold, the point is considered to belong to the ground point cloud, otherwise it is not considered to belong to the ground point cloud.

[0008] As a further preferred embodiment of the present invention, in step S1, a laser radar, several depth cameras, and several sensors are included, the laser radar coordinate system is the main coordinate system, and the depth camera coordinate system is the auxiliary coordinate system, wherein the relative pose transformation from the i-th depth camera coordinate system to the laser radar coordinate system is The point cloud of the laser radar is represented by P L , and the point cloud of the depth camera is represented as arrive Point cloud from a single sensor To segment the ground point cloud, first sort it in ascending order Select the point with smaller z value as the seed point cloud Assume that the seed point cloud has N points and calculate the mean point of the seed point cloud and the covariance matrix C L,seed , perform singular value decomposition on the covariance matrix, where the vector corresponding to the minimum singular value is the vector perpendicular to the initial estimated ground plane, and the corresponding ground plane estimation equation is a X x+b X y+c X z+d X =0, the specific calculation process is as follows:

[0009]

[0010] C X,seed =USV T ,U=[u1,u2,u3]

[0011] (a X ,b X ,c X ) T =u3

[0012]

[0013] As a further preferred embodiment of the present invention, in step S2, traverse P X If the distance between the point and the estimated ground plane is lower than the threshold Th d , then add it to the seed point cloud, repeat the above steps, continuously update the ground plane estimation equation of the ground point cloud estimation, iterate IterNum times to get a relatively stable ground plane estimation, perform the above operations on multiple sensors in parallel, and finally get a stable estimated ground plane center point for each sensor The estimated ground plane center point is fitted into a unified ground plane estimation equation through linear fitting, which is in the form of As shown, the specific formula is as follows:

[0014]

[0015] As a further preferred embodiment of the present invention, in step S3, the points of each sensor point cloud are traversed to calculate the distance information between the points and the unified ground plane equation. If the distance information exceeds the threshold value Th, d , then the point is not considered to belong to the ground point cloud, otherwise it is considered to be a member of the ground point cloud.

[0016] Beneficial Effects

[0017] The present invention calculates the ground point cloud information of each sensor in parallel to ensure the real-time performance of the method; efficiently fuses the ground point cloud information to form a unified ground plane to ensure accurate and reliable ground plane estimation; and maintains the ground point cloud information based on a threshold to ensure efficient and accurate extraction of the ground point cloud.

[0018] The multi-sensor parallel processing of the present invention: adopts the singular value decomposition method to estimate the ground plane information of the point cloud data of each sensor in parallel, uses the information of multiple sensors for comprehensive processing, improves the efficiency and accuracy of ground point cloud extraction, and realizes effective fusion and processing of different sensor data by taking the lidar coordinate system as the main coordinate system and the depth camera coordinate system as the auxiliary coordinate system and considering the relative posture transformation between them. Through the selection and update of the seed point cloud and the singular value decomposition of the covariance matrix, a more accurate ground plane estimation equation is obtained, thereby improving the segmentation effect and stability of the ground point cloud. BRIEF DESCRIPTION OF THE DRAWINGS

[0019] Figure 1 , which is a schematic diagram of the principle of the present invention. DETAILED DESCRIPTION

[0020] The following will be combined with the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0021] See also Figure 1 The present invention provides a technical solution: a multi-sensor based ground point cloud extraction method, comprising the following steps:

[0022] S1, uses a method based on singular value decomposition to estimate the ground plane information of each sensor's point cloud data in parallel;

[0023] S2, fuse each ground plane information into a unified ground plane information;

[0024] S3, traverses all sensor point clouds and calculates their distance to the unified ground plane. If the distance is less than the threshold, the point is considered to belong to the ground point cloud, otherwise it is not considered to belong to the ground point cloud.

[0025] First, assume that the autonomous platform has multiple sensors, including a lidar and multiple depth cameras. The lidar coordinate system is the primary coordinate system, and the depth camera coordinate system is the auxiliary coordinate system. The relative pose transformation from the i-th depth camera coordinate system to the lidar coordinate system is: The point cloud of the laser radar is represented by P L , and the point cloud of the depth camera is represented as arrive Point cloud from a single sensor To segment the ground point cloud, first sort it in ascending order Select the point with smaller z value as the seed point cloud Assume that the seed point cloud has N points and calculate the mean point of the seed point cloud and the covariance matrix C L,seed , perform singular value decomposition on the covariance matrix, where the vector corresponding to the minimum singular value is the vector perpendicular to the initial estimated ground plane, and the corresponding ground plane estimation equation is a X x+b X y+c X z+d X =0, the specific calculation process is as follows:

[0026]

[0027] C X,seed =USV T ,U=[u1,u2,u3]

[0028] (a X ,b X ,c X ) T =u3

[0029]

[0030] Traverse P X If the distance between the point and the estimated ground plane is lower than the threshold Th d , then add it to the seed point cloud, repeat the above steps, continuously update the ground plane estimation equation of the ground point cloud estimation, iterate IterNum times to get a relatively stable ground plane estimation, perform the above operations on multiple sensors in parallel, and finally get a stable estimated ground plane center point for each sensor The estimated ground plane center point is fitted into a unified ground plane estimation equation through linear fitting, which is in the form of As shown, the specific formula is as follows:

[0031]

[0032] Traverse the points of each sensor point cloud and calculate the distance information between it and the unified ground plane equation. If it exceeds the threshold Th d , then the point is not considered to belong to the ground point cloud, otherwise it is considered to be a member of the ground point cloud.

[0033] The pseudocode table is as follows:

[0034]

[0035]

[0036] The above shows and describes the basic principles and main features of the present invention and the advantages of the present invention. It is obvious to those skilled in the art that the present invention is not limited to the details of the above exemplary embodiments, and the present invention can be implemented in other specific forms without departing from the spirit or basic features of the present invention. Therefore, no matter from which point of view, the embodiments should be regarded as exemplary and non-restrictive. The scope of the present invention is defined by the appended claims rather than the above description, and it is intended that all changes that fall within the meaning and scope of the equivalent elements of the claims are included in the present invention.

[0037] In addition, it should be understood that although the present specification is described according to implementation modes, not every implementation mode contains only one independent technical solution. This description of the specification is only for the sake of clarity. Those skilled in the art should regard the specification as a whole. The technical solutions in each embodiment may also be appropriately combined to form other implementation modes that can be understood by those skilled in the art.

Claims

1. A multi-sensor based ground point cloud extraction method, characterized in that: The method comprises the following steps, S1, uses a method based on singular value decomposition to estimate the ground plane information of each sensor's point cloud data in parallel; S2, fuse each ground plane information into a unified ground plane information; S3, traverses all sensor point clouds and calculates their distance to the unified ground plane. If the distance is less than the threshold, the point is considered to belong to the ground point cloud, otherwise it is not considered to belong to the ground point cloud.

2. The multi-sensor based ground point cloud extraction method according to claim 1, characterized in that: In step S1, a laser radar, several depth cameras, and several sensors are included. The laser radar coordinate system is the main coordinate system, and the depth camera coordinate system is the auxiliary coordinate system. The relative pose transformation from the i-th depth camera coordinate system to the laser radar coordinate system is: The point cloud of the laser radar is represented by P L , and the point cloud of the depth camera is represented as arrive Point cloud from a single sensor To segment the ground point cloud, first sort it in ascending order Select the point with smaller z value as the seed point cloud Assume that the seed point cloud has N points and calculate the mean point of the seed point cloud and the covariance matrix C L,seed , perform singular value decomposition on the covariance matrix, where the vector corresponding to the minimum singular value is the vector perpendicular to the initial estimated ground plane, and the corresponding ground plane estimation equation is a X x+b X y+c X z+d X =0, the specific calculation process is as follows: C X,seed =USV T ,U=[u1,u2,u3] (a X ,b X ,c X ) T =u3 3. The multi-sensor based ground point cloud extraction method according to claim 1, characterized in that: In step S2, traverse P X If the distance between the point and the estimated ground plane is lower than the threshold Th d , then add it to the seed point cloud, repeat the above steps, continuously update the ground plane estimation equation of the ground point cloud estimation, iterate IterNum times to get a relatively stable ground plane estimation, perform the above operations on multiple sensors in parallel, and finally get a stable estimated ground plane center point for each sensor The estimated ground plane center point is fitted into a unified ground plane estimation equation through linear fitting, which is in the form of As shown, the specific formula is as follows:

4. The multi-sensor based ground point cloud extraction method according to claim 1, characterized in that: In step S3, the points of each sensor point cloud are traversed to calculate the distance information between them and the unified ground plane equation. If the distance exceeds the threshold Th d , then the point is not considered to belong to the ground point cloud, otherwise it is considered to be a member of the ground point cloud.

Images