Obstacle point cloud processing method, device, equipment and readable storage medium

Abstract

The present invention provides a method, apparatus, device, and readable storage medium for processing obstacle point clouds. The method comprises: determining a ground point cloud and a non-ground point cloud in a corrected point cloud transmitted by a laser radar; determining multi-line laser radar roadside points and solid-state blind spot-filling laser radar roadside points based on the ground point cloud and the non-ground point cloud; determining roadside obstacle information based on the multi-line laser radar roadside points and the solid-state blind spot-filling laser radar roadside points; and removing the obstacle point cloud based on the roadside obstacle information. By eliminating obstacle information from the roadside, the technical solution of this application improves the accuracy of the point cloud map, thereby resolving the problem of low point cloud map accuracy.

Description

Technical Field

[0001] The present invention relates to the field of radar data processing technology, and in particular to a method, device, equipment and readable storage medium for processing obstacle point clouds. Background Art

[0002] Current outdoor point cloud map construction methods primarily rely on SLAM (Simultaneous Localization and Mapping) through the fusion and optimization of multiple sensors, including LiDAR, IMU (Inertial Measurement Unit), wheel odometry, and GNSS (Global Navigation Satellite System). However, during LiDAR matching, obstacles on the road will also generate point clouds containing corners or planar points when the LiDAR extracts features. These obstacle point clouds can affect the accuracy of LiDAR feature matching and the generation of feature descriptors. Furthermore, dynamic obstacle point clouds can cause artifacts in the point cloud map, resulting in low point cloud map accuracy.

[0003] The above content is only used to assist in understanding the technical solution of the present invention and does not constitute an admission that the above content is prior art. Summary of the Invention

[0004] The main purpose of the present invention is to provide a method for processing obstacle point clouds, aiming to solve the problem of low accuracy of point cloud maps.

[0005] To achieve the above-mentioned object, the present invention provides a method for processing an obstacle point cloud, the method comprising:

[0006] Determine the ground point cloud and non-ground point cloud in the rectified point cloud sent by the lidar;

[0007] Determine a multi-line laser radar roadside point and a solid-state blind spot laser radar roadside point according to the ground point cloud and the non-ground point cloud;

[0008] Determine the roadside obstacle information according to the multi-line laser radar roadside point and the solid-state blind spot filling laser radar roadside point;

[0009] The obstacle point cloud is removed according to the roadside obstacle information.

[0010] Optionally, before the step of determining the corrected point cloud sent by the laser radar as a ground point cloud and a non-ground point cloud according to the ground height, the method further includes:

[0011] Upon receiving data sent by the inertial sensor, obtaining the inertial sensor data corresponding to the current timestamp, and determining the high-frequency posture, angular velocity, and acceleration of the robot based on the inertial sensor data;

[0012] Motion compensation is performed on the point cloud collected by the laser radar according to the high-frequency posture, the angular velocity, and the acceleration to obtain the corrected point cloud.

[0013] Optionally, the step of determining the corrected point cloud sent by the laser radar as a ground point cloud and a non-ground point cloud according to the ground height includes:

[0014] Get the ground height threshold;

[0015] The corrected cloud points that are less than the ground height threshold in the corrected cloud points are regarded as ground point clouds, and the corrected cloud points that are greater than or equal to the ground height threshold in the corrected cloud points are regarded as non-ground point clouds.

[0016] Optionally, the step of determining a multi-line laser radar roadside point and a solid-state blind spot filling laser radar roadside point based on the ground point cloud and the non-ground point cloud includes:

[0017] Get the height difference between the ground and the curb;

[0018] The ground point cloud and the non-ground point cloud are fitted with the ground equation by the random forest algorithm of the solid-state blind spot laser radar to obtain the solid-state blind spot laser radar roadside point; and the ground point cloud and the non-ground point cloud are passed through the multi-line laser radar to obtain the roadside curvature of the roadside, and the multi-line laser radar roadside point is determined according to the roadside curvature and the height difference.

[0019] Optionally, the step of determining obstacle information based on the multi-line laser radar roadside points and the solid-state blind spot compensation laser radar roadside points includes:

[0020] The multi-line laser radar roadside points and the solid-state blind spot laser radar roadside points are subjected to a preset quadratic function to obtain a curved roadside function;

[0021] The roadside obstacle information is determined according to the curved roadside function.

[0022] Optionally, the step of removing the obstacle point cloud according to the roadside obstacle information includes:

[0023] Determining the obstacle point cloud according to the roadside obstacle information;

[0024] The obstacle point cloud is removed from the roadside obstacle information by using an extended Kalman filter method.

[0025] Optionally, after the step of removing the obstacle point cloud according to the roadside obstacle information, the method further includes:

[0026] Acquire feature points of the roadside, and determine each key frame of the robot according to the feature points;

[0027] Determine the key frame pose of each key frame through loop closure detection;

[0028] The key frame pose is determined to optimize the point cloud map through the laser odometry factor, the pre-integration factor of the inertial sensor, the global navigation satellite system factor and the closed-loop factor, wherein the optimized point cloud map is the point cloud map after removing the obstacle point cloud.

[0029] In addition, to achieve the above-mentioned object, the present invention further provides an obstacle point cloud processing device, the obstacle point cloud processing device comprising:

[0030] Point cloud extraction module, used to determine the rectified point cloud sent by the lidar into ground point cloud and non-ground point cloud according to the ground height;

[0031] A roadside point determination module, configured to determine a multi-line laser radar roadside point and a solid-state blind spot laser radar roadside point based on the ground point cloud and the non-ground point cloud;

[0032] An obstacle information determination module, configured to determine roadside obstacle information based on the multi-line laser radar roadside points and the solid-state blind spot-filling laser radar roadside points;

[0033] The obstacle point cloud removal module is used to remove the obstacle point cloud according to the roadside obstacle information.

[0034] In addition, to achieve the above-mentioned purpose, the present invention also provides an obstacle point cloud processing device, which includes a memory, a processor, and an obstacle point cloud processing program stored in the memory and runnable on the processor. When the obstacle point cloud processing program is executed by the processor, the steps of the obstacle point cloud processing method described above are implemented.

[0035] In addition, to achieve the above-mentioned purpose, the present invention also provides a computer-readable storage medium, on which a processing program for an obstacle point cloud is stored. When the processing program for the obstacle point cloud is executed by a processor, the steps of the obstacle point cloud processing method as described above are implemented.

[0036] The embodiments of the present invention provide a method, apparatus, device and computer-readable storage medium for processing obstacle point clouds. By determining the ground point cloud and non-ground point cloud in the correction point cloud sent by the laser radar, after determining the multi-line laser radar roadside points and solid-state blind-filling laser radar roadside points based on the ground point cloud and the non-ground point cloud, the multi-line laser radar roadside points and the solid-state blind-filling laser radar roadside points are used to determine the roadside obstacle information. Finally, the obstacle point cloud is removed according to the roadside obstacle information, thereby eliminating the obstacle information in the roadside, improving the accuracy of the laser radar feature matching and the generation of feature descriptors, improving the accuracy of the point cloud map, and solving the problem of low accuracy of the point cloud map. BRIEF DESCRIPTION OF THE DRAWINGS

[0037] Figure 1 Schematic diagram of the hardware architecture of an obstacle point cloud processing device according to an embodiment of the present invention;

[0038] Figure 2 1 is a flow chart of a first embodiment of a method for processing an obstacle point cloud according to the present invention;

[0039] Figure 3 2 is a flow chart of a second embodiment of a method for processing an obstacle point cloud according to the present invention;

[0040] Figure 4 This is a detailed flowchart of step S10 in the third embodiment of the method for processing obstacle point clouds of the present invention;

[0041] Figure 5 This is a detailed flowchart of step S20 in the fourth embodiment of the method for processing obstacle point clouds of the present invention;

[0042] Figure 6 This is a detailed flowchart of step S30 in the fifth embodiment of the method for processing obstacle point clouds of the present invention;

[0043] Figure 7 This is a detailed flowchart of step S40 in the fourth embodiment of the method for processing obstacle point clouds of the present invention;

[0044] Figure 8 4 is a flow chart of an eighth embodiment of a method for processing an obstacle point cloud according to the present invention;

[0045] Figure 9 Schematic diagram of the architecture of the obstacle point cloud processing device of the present invention.

[0046] The purpose, features and advantages of the present invention will be further described with reference to the accompanying drawings and in conjunction with the embodiments. DETAILED DESCRIPTION

[0047] It should be understood that the drawings of the present invention show exemplary embodiments of the present invention, and that the present invention can be implemented in various forms and should not be limited by the embodiments set forth herein. On the contrary, these embodiments are provided to enable a more thorough understanding of the present invention and to fully convey the scope of the present invention to those skilled in the art.

[0048] As an implementation solution, the obstacle point cloud processing device can be as follows Figure 1 shown.

[0049] The embodiment of the present invention relates to an obstacle point cloud processing device, which includes a processor 101, such as a CPU, a memory 102, and a communication bus 103. The communication bus 103 is used to achieve connection and communication between these components.

[0050] The memory 102 may be a high-speed RAM memory or a stable memory (non-volatile memory), such as a disk memory. Figure 1 As shown, the memory 102 as a computer-readable storage medium may include a program for processing an obstacle point cloud; and the processor 101 may be used to call the obstacle point cloud processing program stored in the memory 102 and perform the following operations:

[0051] Determine the ground point cloud and non-ground point cloud in the rectified point cloud sent by the lidar;

[0052] Determine a multi-line laser radar roadside point and a solid-state blind spot laser radar roadside point according to the ground point cloud and the non-ground point cloud;

[0053] Determine the roadside obstacle information according to the multi-line laser radar roadside point and the solid-state blind spot filling laser radar roadside point;

[0054] The obstacle point cloud is processed according to the roadside obstacle information.

[0055] In one embodiment, the processor 101 may be configured to call an obstacle point cloud processing program stored in the memory 102 and perform the following operations:

[0056] Upon receiving data sent by the inertial sensor, obtaining the inertial sensor data corresponding to the current timestamp, and determining the high-frequency posture, angular velocity, and acceleration of the robot based on the inertial sensor data;

[0057] Motion compensation is performed on the point cloud collected by the laser radar according to the high-frequency posture, the angular velocity, and the acceleration to obtain the corrected point cloud.

[0058] In one embodiment, the processor 101 may be configured to call an obstacle point cloud processing program stored in the memory 102 and perform the following operations:

[0059] Get the ground height threshold;

[0060] The corrected cloud points that are less than the ground height threshold in the corrected cloud points are regarded as ground point clouds, and the corrected cloud points that are greater than or equal to the ground height threshold in the corrected cloud points are regarded as non-ground point clouds.

[0061] In one embodiment, the processor 101 may be configured to call an obstacle point cloud processing program stored in the memory 102 and perform the following operations:

[0062] Get the height difference between the ground and the curb;

[0063] The ground point cloud and the non-ground point cloud are fitted with the ground equation by the random forest algorithm of the solid-state blind spot laser radar to obtain the solid-state blind spot laser radar roadside point; and the ground point cloud and the non-ground point cloud are passed through the multi-line laser radar to obtain the roadside curvature, and the multi-line laser radar roadside point is determined according to the roadside curvature and the height difference.

[0064] In one embodiment, the processor 101 may be configured to call an obstacle point cloud processing program stored in the memory 102 and perform the following operations:

[0065] The multi-line laser radar roadside points and the solid-state blind spot laser radar roadside points are subjected to a preset quadratic function to obtain a curved roadside function;

[0066] The roadside obstacle information is determined according to the curved roadside function.

[0067] In one embodiment, the processor 101 may be configured to call an obstacle point cloud processing program stored in the memory 102 and perform the following operations:

[0068] Determining an obstacle point cloud based on the roadside obstacle information;

[0069] The obstacle point cloud is removed from the roadside obstacle information by using an extended Kalman filter method.

[0070] In one embodiment, the processor 101 may be configured to call an obstacle point cloud processing program stored in the memory 102 and perform the following operations:

[0071] Acquire feature points of the roadside, and determine each key frame of the robot according to the feature points;

[0072] Determine the key frame pose of each key frame through loop closure detection;

[0073] The key frame pose is determined to optimize the point cloud map through the laser odometry factor, the pre-integration factor of the inertial sensor, the global navigation satellite system factor and the closed-loop factor, wherein the optimized point cloud map is the point cloud map after removing the obstacle point cloud.

[0074] Based on the hardware architecture of the obstacle point cloud processing device based on the above radar data processing technology, an embodiment of the obstacle point cloud processing method of the present invention is proposed.

[0075] SLAM can be divided into front-end processing and back-end optimization. The front-end processing is to perform distortion correction, feature extraction, feature matching, generate key frames, and obtain pose estimation on the point cloud data collected by the lidar; the back-end optimization uses factor graph optimization, adding the IMU's pre-integration factor, wheel odometry factor, GNSS factor, and loop detection factor to the factor graph, thereby optimizing and updating all keyframe positions, and finally obtaining a globally consistent global pose, which is then spliced ​​together to generate a point cloud map. First, the corresponding IMU data is found based on the timestamp. The high-frequency attitude, angular velocity, and acceleration information of the IMU is used to perform motion compensation on each LiDAR point cloud, i.e., point cloud distortion correction, to obtain a corrected point cloud. The ground point cloud and non-ground point cloud are obtained from the corrected point cloud based on the ground height. The multi-line LiDAR roadside points are extracted based on the height difference between the roadside and the ground and the different curvatures of the ground points and roadside points of each beam of the multi-line LiDAR. The ground point cloud and non-ground point cloud are then fitted with the ground equation using the random forest algorithm of the solid-state blind spot LiDAR to obtain the solid-state blind spot LiDAR roadside points. The two types of roadside points are fitted to a curved roadside using a quadratic function to extract the non-ground point cloud. Based on the known roadside information, the obstacle point cloud on the road surface is removed. The feature points of each LiDAR are then calculated and classified into corner points and plane points according to the curvature of each roadside point. The corner points and plane points of the current LiDAR frame are extracted and matched with the local keyframe corner point map and plane point map, respectively. After iterative optimization, the pose of the current frame is obtained. Furthermore, obstacle information extracted from the curb is combined with the current frame pose through an EKF (Extended Kalman Filter) to generate a keyframe. Loop closure detection is then used to match the current keyframe with all keyframes within a certain range of the closest and furthest matching frames in the lidar's historical keyframes to obtain the keyframe pose. Wheel odometry factors, IMU pre-integration factors, GNSS factors, and loop closure factors are added to the keyframe pose to generate a factor graph. All keyframe poses are then optimized and updated to ultimately obtain a globally consistent pose, which is then stitched together to create a point cloud map.

[0076] Reference Figure 2 In a first embodiment, the method for processing the obstacle point cloud includes the following steps:

[0077] Step S10: determining the ground point cloud and the non-ground point cloud in the rectified point cloud sent by the laser radar;

[0078] In this embodiment, the server receives the corrected point cloud sent by the LiDAR. This corrected point cloud refers to the laser point cloud after distortion correction. For example, assuming the LiDAR output frequency is 10Hz and each laser beam generates 2000 points during one revolution, 2000 data points need to be generated within 0.1 seconds. Motion compensation is then performed on these 2000 points to eliminate errors introduced by the LiDAR during robot motion. Based on the height of the corrected point cloud above the ground, the corrected point cloud is identified as a ground point cloud, while the remaining point clouds are considered non-ground point clouds.

[0079] Step S20: determining a multi-line laser radar roadside point and a solid-state blind spot laser radar roadside point according to the ground point cloud and the non-ground point cloud;

[0080] In this embodiment, after determining the ground point cloud and non-ground point cloud, the roadside points are determined based on the two ground point clouds. In this embodiment, two different types of radars, a multi-line laser radar and a solid-state laser blind spot radar, are used to obtain the two types of roadside points. Compared to a single-line laser radar, a multi-line laser radar (which can be a 16-line, 32-line, or 64-line radar, etc.) differs in that it uses multiple laser emitters for polling, with a sensing range of 360°. The more laser beams a laser radar has, the better the object detection effect. After a laser radar polling cycle, a frame of laser point cloud data is obtained. The roadside points obtained by these two types of radar are processed and used as a judgment basis for distinguishing obstacle point clouds from other necessary point cloud images in the roadside point cloud.

[0081] Step S30: determining roadside obstacle information according to the multi-line laser radar roadside points and the solid-state blind spot filling laser radar roadside points;

[0082] In this embodiment, after determining the solid-state blind spot laser radar roadside points of the multi-line laser radar, since the point cloud data obtained by the solid-state blind spot laser radar is short and dense, the point cloud within the height range from the ground is intercepted, and the ground equation is fitted using the random forest algorithm to extract the point cloud, and a section of solid-state blind spot laser radar roadside points is extracted based on the ground points and non-ground points; while the point cloud data obtained by the multi-line laser radar is long and sparse, and the multi-line laser radar roadside points are extracted based on the height difference between the roadside and the ground and the different curvatures of the ground points and roadside points of each beam based on the multi-line laser radar, and then the obstacle information of the roadside is determined based on these two types of roadside points.

[0083] Step S40: removing the obstacle point cloud according to the roadside obstacle information.

[0084] In this embodiment, after the roadside obstacle information is determined, the obstacle information is removed from the known roadside information.

[0085] In the technical solution provided in this embodiment, the ground point cloud and non-ground point cloud are distinguished by correcting the height of the point cloud from the ground, and the two different point clouds are substituted into two different laser radars to extract two types of roadside points. The obstacle information in the roadside information is determined based on the two roadside points, and finally the obstacle information is eliminated. This method improves the accuracy of the point cloud map and solves the problem of low accuracy of the point cloud map.

[0086] Reference Figure 3 In the second embodiment, based on the first embodiment, before step S10, the following steps are further included:

[0087] Step S50: upon receiving the data sent by the inertial sensor, obtaining the inertial sensor data corresponding to the current timestamp, and determining the high-frequency posture, angular velocity, and acceleration of the robot based on the inertial sensor data;

[0088] Step S60: performing motion compensation on the point cloud collected by the laser radar according to the high-frequency posture, the angular velocity, and the acceleration to obtain the corrected point cloud.

[0089] Optionally, this embodiment provides a method for determining the correction of the point cloud. When the server receives data sent by the inertial sensor, it obtains the data in the inertial sensor IMU at that moment. The IMU is mainly used to detect and measure acceleration, tilt, impact, vibration, rotation, and multi-degree-of-freedom motion. It is an important component for solving navigation, orientation, and motion carrier control. In this embodiment, the inertial sensor installed on the robot obtains high-frequency attitude, angular velocity, acceleration and other data at the current timestamp, and performs motion compensation on the point cloud collected by the lidar based on this data (which can be global motion compensation or block motion compensation).

[0090] In the technical solution provided in this embodiment, the point cloud collected by the lidar is motion compensated by the inertial sensor, thereby eliminating the point cloud distortion caused by the robot movement and improving the accuracy of the point cloud map.

[0091] refer to Figure 4 In the third embodiment, based on the above embodiment, step S10 includes:

[0092] Step S11: obtaining a ground height threshold;

[0093] Step S12: The corrected cloud points that are less than the ground height threshold in the corrected cloud points are regarded as ground point clouds, and the corrected cloud points that are greater than or equal to the ground height threshold in the corrected cloud points are regarded as non-ground point clouds.

[0094] Optionally, this embodiment provides a method for determining ground point clouds and non-ground point clouds from a corrected point cloud. In this embodiment, the ground height threshold can be a height threshold automatically determined by the server, or the operator can store the threshold as a preset file in the server's database. After determining the corrected point cloud, the server calls the threshold to determine the parts of the corrected point cloud that are closer to and farther from the ground, and regards the point clouds in the corrected point cloud that are less than the ground height threshold as ground point clouds, and regards the point clouds that are greater than or equal to the ground height threshold as non-ground point clouds. In addition, there is another determination scheme, in which the server can only determine the ground point clouds that are less than the threshold based on the ground height threshold, and directly regard the remaining point clouds in the corrected point cloud as non-ground point clouds. This can also relatively quickly identify the ground point cloud and non-ground point cloud parts in the corrected point cloud.

[0095] In the technical solution provided in this embodiment, a height threshold is set to quickly distinguish the ground point cloud part and the non-ground point cloud part in the corrected point cloud according to the height threshold, so that the subsequent server can extract the curb points based on the corrected point cloud.

[0096] refer to Figure 5 In the fourth embodiment, based on the above embodiment, step S20 includes:

[0097] Step S21: Obtain the height difference between the ground and the curb;

[0098] Step S22: Fitting the ground point cloud and the non-ground point cloud to the ground equation through the random forest algorithm of the solid-state blind spot laser radar to obtain the solid-state blind spot laser radar roadside point; and obtaining the roadside curvature of the ground point cloud and the non-ground point cloud through the multi-line laser radar, and determining the multi-line laser radar roadside point based on the roadside curvature and the height difference.

[0099] Optionally, this embodiment provides a method for extracting roadside nodes. After determining the ground point cloud and the non-ground point cloud, the server obtains the height difference between the ground and the roadside, and uses the height difference as a preset parameter for the server to extract the roadside nodes. After determining the height difference, the ground point cloud and the non-ground point cloud are fitted with the ground equation by a solid-state laser blind spot radar using a random forest algorithm to obtain a roadside point determined by the radar, that is, a solid-state blind spot laser radar roadside point. In this embodiment, a random forest (RF) algorithm is used to allow machine learning to automatically identify roadside points. The RF algorithm is an algorithm that integrates multiple decision trees through the idea of ​​ensemble learning. It has high flexibility and a wide range of application scenarios. Through the RF algorithm, the server can automatically fit the surface equation of the ground (the fitting method can be the common least squares method, which is not limited in this embodiment). After obtaining the surface equation of the ground, the server can substitute the corresponding parameters to quickly determine the roadside nodes. As for multi-line laser radar, since the line beams it generates are relatively sparse and narrow, the curvature of the ground point cloud and the roadside nodes between each line beam is quite different. In this embodiment, the roadside points are extracted from the point cloud data through the curvature and the preset parameter height difference. As the multi-line laser radar roadside points, the extraction method is relatively existing and will not be repeated in this embodiment.

[0100] In the technical solution provided in this embodiment, the solid-state laser blind spot compensation radar and RF algorithm are used to obtain the ground equation to determine the solid-state laser blind spot compensation radar roadside points, and the multi-line laser radar roadside points are extracted according to the height difference and the curvature between the roadside points through the multi-line laser radar, so that the server can automatically identify and generate roadside points based on the ground point cloud and non-ground point cloud.

[0101] refer to Figure 6 In the fifth embodiment, based on the above embodiment, step S30 includes:

[0102] Step S31: obtaining a curved roadside function by applying a preset quadratic function to the multi-line laser radar roadside points and the solid-state blind spot-filling laser radar roadside points;

[0103] Step S32: Determine the roadside obstacle information according to the curved roadside function.

[0104] Optionally, this embodiment provides a method for determining obstacle information at curb points. Two curb points are applied to a preset quadratic function to determine the curb's surface equation. The surface equation includes characteristic points of the curb, and obstacle information can be determined based on the characteristic points of the surface equation. For example, an interval value is set, and the corner points and flat points within the interval obtained by the surface equation in the current frame are used as obstacle information parameters.

[0105] In the technical solution provided in this embodiment, a surface function of the curb is obtained by substituting the multi-line laser radar curb points and the solid-state blind spot laser radar curb points into a preset quadratic function, and the curb obstacle information is determined according to the surface function, so that the server can identify the obstacle information of the curb points.

[0106] refer to Figure 7 In the sixth embodiment, based on the above embodiment, step S40 includes:

[0107] Step S41: determining an obstacle point cloud based on the roadside obstacle information;

[0108] Step S42: removing the obstacle point cloud from the roadside obstacle information by using an extended Kalman filter method.

[0109] Optionally, this embodiment provides a method for eliminating obstacle point clouds. Obstacle information may include coordinate parameters of the obstacle. Point cloud data representing the obstacle is determined based on the coordinate parameters, and then the point cloud data belonging to the obstacle is filtered out using an EKF filter. The EKF is an extended form of the Kalman filter (KF) in a nonlinear situation. It linearizes the nonlinear system using a Taylor series expansion and then filters the signal using a Kalman filter framework. This is a highly efficient recursive filtering method. By substituting the curb information into the EKF for iterative optimization, the obstacle information can be quickly removed from the curb information, leaving the keyframes in the updated and optimized curb information.

[0110] In the technical solution provided in this embodiment, the obstacle point cloud that affects the accuracy of the point cloud map is removed by removing the obstacle point cloud from the curb information through EKF. At the same time, it can avoid the generation of ghosting by dynamic obstacles in the point cloud map, thereby improving the accuracy of the point cloud map.

[0111] refer to Figure 8 In the seventh embodiment, based on the above embodiment, step S40 includes:

[0112] Step S70: Acquire feature points of the roadside, and determine each key frame of the robot according to the feature points;

[0113] Step S80: determining the key frame pose of each key frame through loop closure detection;

[0114] Step S90: Determine an optimized point cloud map based on the key frame pose by using a laser odometry factor, a pre-integration factor of an inertial sensor, a global navigation satellite system factor, and a closed-loop factor, wherein the optimized point cloud map is a point cloud map after removing obstacle point clouds.

[0115] Optionally, this embodiment provides a method for generating an optimized point cloud map after removing the obstacle point cloud. In this embodiment, after removing the obstacle information in the roadside, the feature points of the roadside are extracted. The feature points may include the corner points and plane points of each roadside point. The corner points and plane points of the current frame of the laser radar are extracted and matched with the local key frame corner point map and plane point map respectively. The pose of the current frame is obtained after iterative optimization. Then, the current frame of the laser radar is determined as a key frame by a certain distance or a certain angle of rotation through the laser radar odometer, and the key frame is made into a feature descriptor. The current key frame of the laser radar is matched with all key frames within a certain range of the matching frame with the closest distance and the farthest time interval in the historical key frames of the laser radar through loop detection to obtain the key frame pose. Then, the relative posture obtained by angular rate and angular rate integration between the two key frames of the laser radar is pre-integrated. Finally, the laser odometer factor, the pre-integration factor of the IMU, the GNSS factor and the closed-loop factor are added to the factor graph. The open source code GSTAM (Georgia Tech Smoothing and Mapping, Georgia Institute of Technology smoothing and mapping) optimizes and updates the poses of all key frames, finally obtains a globally consistent pose, and finally splices it to generate an optimized point cloud map.

[0116] In the technical solution provided in this embodiment, the roadside points after removing the obstacle information are extracted and key frame poses are generated, and the factor graph is optimized and updated according to the factors in the key frame poses, so that the server can reconstruct the point cloud map after removing the obstacle data.

[0117] In addition, refer to Figure 9 This embodiment further provides a device for processing an obstacle point cloud, the device comprising:

[0118] The point cloud extraction module 100 is used to determine the rectified point cloud sent by the laser radar into a ground point cloud and a non-ground point cloud according to the ground height;

[0119] A roadside point determination module 200 is configured to determine a multi-line laser radar roadside point and a solid-state blind spot laser radar roadside point based on the ground point cloud and the non-ground point cloud;

[0120] The obstacle information determination module 300 is used to determine the roadside obstacle information based on the multi-line laser radar roadside points and the solid-state blind spot laser radar roadside points;

[0121] The obstacle point cloud removal module 400 is configured to remove the obstacle point cloud according to the roadside obstacle information.

[0122] Optionally, the obstacle point cloud processing device may further perform the following steps:

[0123] Upon receiving data sent by the inertial sensor, obtaining the inertial sensor data corresponding to the current timestamp, and determining the high-frequency posture, angular velocity, and acceleration of the robot based on the inertial sensor data;

[0124] Motion compensation is performed on the point cloud collected by the laser radar according to the high-frequency posture, the angular velocity, and the acceleration to obtain the corrected point cloud.

[0125] Optionally, the obstacle point cloud processing device may further perform the following steps:

[0126] Get the ground height threshold;

[0127] The corrected cloud points that are less than the ground height threshold in the corrected cloud points are regarded as ground point clouds, and the corrected cloud points that are greater than or equal to the ground height threshold in the corrected cloud points are regarded as non-ground point clouds.

[0128] Optionally, the obstacle point cloud processing device may further perform the following steps:

[0129] Get the height difference between the ground and the curb;

[0130] The ground point cloud and the non-ground point cloud are fitted with the ground equation by the random forest algorithm of the solid-state blind spot laser radar to obtain the solid-state blind spot laser radar roadside point; and the ground point cloud and the non-ground point cloud are passed through the multi-line laser radar to obtain the roadside curvature of the roadside, and the multi-line laser radar roadside point is determined according to the roadside curvature and the height difference.

[0131] Optionally, the obstacle point cloud processing device may further perform the following steps:

[0132] The multi-line laser radar roadside points and the solid-state blind spot laser radar roadside points are subjected to a preset quadratic function to obtain a curved roadside function;

[0133] The roadside obstacle information is determined according to the curved roadside function.

[0134] Optionally, the obstacle point cloud processing device may further perform the following steps:

[0135] Determining the obstacle point cloud according to the roadside obstacle information;

[0136] The obstacle point cloud is removed from the roadside obstacle information by using an extended Kalman filter method.

[0137] Optionally, the obstacle point cloud processing device may further perform the following steps:

[0138] Acquire feature points of the roadside, and determine each key frame of the robot according to the feature points;

[0139] Determine the key frame pose of each key frame through loop closure detection;

[0140] The key frame pose is determined to optimize the point cloud map through the laser odometry factor, the pre-integration factor of the inertial sensor, the global navigation satellite system factor and the closed-loop factor, wherein the optimized point cloud map is the point cloud map after removing the obstacle point cloud.

[0141] In addition, the present invention also provides an obstacle point cloud processing device, which includes a memory, a processor, and an obstacle point cloud processing program stored in the memory and runnable on the processor. When the obstacle point cloud processing program is executed by the processor, the various steps of the obstacle point cloud processing method described above are implemented.

[0142] In addition, the present invention also provides a computer-readable storage medium, which stores an obstacle point cloud processing program. When the obstacle point cloud processing program is executed by a processor, the various steps of the obstacle point cloud processing method described in the above embodiment are implemented.

[0143] It should be noted that, in this document, the terms "comprises," "includes," or any other variations thereof are intended to encompass non-exclusive inclusion, such that a process, method, article, or apparatus comprising a series of elements includes not only those elements but also other elements not explicitly listed, or elements inherent to such process, method, article, or apparatus. In the absence of further limitations, an element defined by the phrase "comprising a ..." does not exclude the presence of other identical elements in the process, method, article, or apparatus comprising the element.

[0144] Through the description of the above embodiments, those skilled in the art can clearly understand that the above embodiment methods can be implemented by means of software plus the necessary general hardware platform, and of course can also be implemented by hardware, but in many cases the former is a better embodiment. Based on this understanding, the technical solution of the present invention is essentially or the part that contributes to the prior art can be embodied in the form of a software product, which is stored in a computer-readable storage medium (such as ROM / RAM, magnetic disk, optical disk) as described above, and includes a number of instructions for enabling a terminal device (which can be a mobile phone, computer, server, air conditioner, or network device, etc.) to execute the methods described in each embodiment of the present invention.

[0145] The above are only preferred embodiments of the present invention and are not intended to limit the patent scope of the present invention. Any equivalent structure or equivalent process transformation made using the contents of the present invention description and drawings, or directly or indirectly applied in other related technical fields, are also included in the patent protection scope of the present invention.

Claims

1. A method for processing obstacle point clouds, characterized in that: The obstacle point cloud processing method includes: Determine the ground point cloud and non-ground point cloud in the rectified point cloud sent by the lidar; Determining a multi-line laser radar curb point and a solid-state blind spot laser radar curb point according to the ground point cloud and the non-ground point cloud, including: obtaining a height difference between the ground and the curb; fitting the ground point cloud and the non-ground point cloud to a ground equation using a random forest algorithm of a solid-state blind spot laser radar to obtain the solid-state blind spot laser radar curb point; obtaining a curb curvature of the curb by passing the ground point cloud and the non-ground point cloud through the multi-line laser radar, and determining the multi-line laser radar curb point according to the curb curvature and the height difference; Determining the roadside obstacle information according to the multi-line laser radar roadside points and the solid-state blind spot laser radar roadside points, including: applying a preset quadratic function to the multi-line laser radar roadside points and the solid-state blind spot laser radar roadside points to obtain a curved roadside function; and determining the roadside obstacle information according to the curved roadside function; The obstacle point cloud is removed according to the roadside obstacle information.

2. The method for processing obstacle point clouds according to claim 1, wherein: Before the step of determining the ground point cloud and the non-ground point cloud in the rectified point cloud sent by the laser radar, the method further includes: Upon receiving data sent by the inertial sensor, obtaining the inertial sensor data corresponding to the current timestamp, and determining the high-frequency posture, angular velocity, and acceleration of the robot based on the inertial sensor data; Motion compensation is performed on the point cloud collected by the laser radar according to the high-frequency posture, the angular velocity, and the acceleration to obtain the corrected point cloud.

3. The method for processing obstacle point clouds according to claim 1, wherein: The step of determining the ground point cloud and the non-ground point cloud in the rectified point cloud sent by the laser radar comprises: Get the ground height threshold; The corrected point cloud with a height less than the ground height threshold in the corrected point cloud is regarded as a ground point cloud, and the corrected point cloud with a height greater than or equal to the ground height threshold in the corrected point cloud is regarded as a non-ground point cloud.

4. The method for processing obstacle point clouds according to claim 1, wherein: The step of removing the obstacle point cloud according to the roadside obstacle information comprises: Determining an obstacle point cloud based on the roadside obstacle information; The obstacle point cloud is removed from the roadside obstacle information by using an extended Kalman filter method.

5. The method for processing obstacle point clouds according to claim 1, wherein: After the step of removing the obstacle point cloud according to the roadside obstacle information, the method further includes: Acquire feature points of the roadside, and determine each key frame of the robot according to the feature points; Determine the key frame pose of each key frame through loop closure detection; The key frame pose is determined to optimize the point cloud map through the laser odometry factor, the pre-integration factor of the inertial sensor, the global navigation satellite system factor and the closed-loop factor, wherein the optimized point cloud map is the point cloud map after removing the obstacle point cloud.

6. An obstacle point cloud processing device, characterized in that: The obstacle point cloud processing device includes: Point cloud extraction module, used to determine the rectified point cloud sent by the lidar into ground point cloud and non-ground point cloud according to the ground height; A roadside point determination module is used to determine a multi-line laser radar roadside point and a solid-state blind spot laser radar roadside point based on the ground point cloud and the non-ground point cloud, including: obtaining a height difference between the ground and the roadside; fitting the ground point cloud and the non-ground point cloud to the ground equation through the random forest algorithm of the solid-state blind spot laser radar to obtain the solid-state blind spot laser radar roadside point; passing the ground point cloud and the non-ground point cloud through the multi-line laser radar to obtain the roadside curvature of the roadside, and determining the multi-line laser radar roadside point based on the roadside curvature and the height difference; An obstacle information determination module is configured to determine roadside obstacle information based on the multi-line laser radar roadside points and the solid-state blind spot filling laser radar roadside points, comprising: applying a preset quadratic function to the multi-line laser radar roadside points and the solid-state blind spot filling laser radar roadside points to obtain a curved roadside function; and determining the roadside obstacle information based on the curved roadside function. The obstacle point cloud removal module is used to remove the obstacle point cloud according to the roadside obstacle information.

7. An obstacle point cloud processing device, characterized in that: The obstacle point cloud processing device includes: a memory, a processor, and an obstacle point cloud processing program stored in the memory and executable on the processor. When the obstacle point cloud processing program is executed by the processor, the steps of the obstacle point cloud processing method according to any one of claims 1 to 5 are implemented.

8. A computer-readable storage medium, characterized in that The computer-readable storage medium stores an obstacle point cloud processing program, and when the obstacle point cloud processing program is executed by a processor, the steps of the obstacle point cloud processing method according to any one of claims 1 to 5 are implemented.

Images