Anti-degradation LiDAR-SLAM method and device, terminal and medium

Abstract

The invention provides an anti-degradation LiDAR-SLAM method and device, a terminal and a medium, and belongs to the technical field of computer vision, and the method comprises the steps: continuously obtaining original point cloud data collected by a three-dimensional laser radar on target equipment, and carrying out the processing of the original point cloud data, and obtaining a target intensity map and a target depth map; removing a dynamic target in the original point cloud data, and performing ground segmentation to obtain ground point cloud data and non-ground point cloud data; and respectively extracting features of the target intensity map, the target depth map, the ground point cloud data and the non-ground point cloud data, constructing a residual function based on all the features, and solving the residual function to obtain a pose estimation result of the target equipment. According to the method, the residual function is constructed by extracting the multi-modal features, and the pose estimation result is solved, so that the accuracy of pose estimation can be effectively improved.

Description

Technical Field

[0001] The present invention relates to the field of computer vision technology, and in particular to an anti-degradation LiDAR-SLAM method, device and medium. Background Art

[0002] With the rapid development of robotics and mobile mapping, the simultaneous localization and mapping (SLAM) technology based on LiDAR has been widely used in drones, autonomous vehicles, and indoor and outdoor mobile robots because of its insensitivity to lighting conditions and high data accuracy. However, as the density of LiDAR data continues to increase, the computational burden of point cloud registration also increases.

[0003] In this context, the classic feature-based LiDAR-SLAM method reduces the computational pressure through feature extraction strategies, but the multi-edge-dependent features and plane features are not sensitive enough to the vertical posture changes, and it is impossible to extract effective geometric features for constraints in the vertical direction. In degraded environments such as corridors and tunnels, the pose estimation accuracy is prone to low. At the same time, most LiDAR-SLAM algorithms only use geometric features and do not fully exploit the laser radar intensity information and the intensity and depth images generated by the point cloud. When the environmental features are insufficient or there are dynamic targets (such as pedestrians and vehicles), the pose estimation accuracy is also low.

[0004] Therefore, the prior art has defects and needs to be improved and developed. Summary of the invention

[0005] The technical problem to be solved by the present invention is to provide an anti-degradation LiDAR-SLAM method, device and storage medium in view of the above-mentioned defects of the prior art, aiming to solve the problem of low accuracy of pose estimation of the feature-based LiDAR-SLAM method in the prior art.

[0006] The technical solution adopted by the present invention to solve the technical problem is as follows:

[0007] In a first aspect, an embodiment of the present invention provides an anti-degradation LiDAR-SLAM method. The anti-degradation LiDAR-SLAM method comprises:

[0008] Continuously acquiring original point cloud data collected by a three-dimensional laser radar on a target device, and processing the original point cloud data to obtain a target intensity map and a target depth map;

[0009] Eliminating dynamic targets in the original point cloud data, and performing ground segmentation to obtain ground point cloud data and non-ground point cloud data;

[0010] The features of the target intensity map, the target depth map, the ground point cloud data and the non-ground point cloud data are extracted respectively, and a residual function is constructed based on all the features, and the residual function is solved to obtain a pose estimation result of the target device.

[0011] In one embodiment, the preprocessing of the original point cloud data to obtain a target intensity map and a target depth map includes:

[0012] Performing intensity correction on the original point cloud data to obtain second point cloud data;

[0013] Projecting the second point cloud data onto a two-dimensional plane to obtain an initial intensity map and an initial depth map;

[0014] Upsampling and data completion are performed on the initial intensity map and the initial depth map using a preset interpolation method to obtain a second intensity map and a second depth map;

[0015] The second intensity map and the second depth map are processed by an adaptive histogram equalization method to obtain a target intensity map and a target depth map.

[0016] In one embodiment, the removing of dynamic targets from the original point cloud data and performing ground segmentation to obtain ground point cloud data and non-ground point cloud data includes:

[0017] Inputting the original point cloud data into a pre-trained 3D point cloud semantic segmentation model to obtain a labeled point cloud;

[0018] Calculate the pixel intensity change between adjacent frames in the initial depth map to obtain a residual image;

[0019] Inputting the residual image, the initial depth map and the annotated point cloud into a preset deep learning network, and obtaining a point cloud corresponding to the dynamic pixel point through fusion analysis by the deep learning network;

[0020] Eliminate the point cloud corresponding to the dynamic pixel points in the original point cloud data to obtain an intermediate point cloud;

[0021] The intermediate point cloud is subjected to ground segmentation to obtain ground point cloud data and non-ground point cloud data.

[0022] In one implementation, performing ground segmentation on the intermediate point cloud to obtain ground point cloud data and non-ground point cloud data includes:

[0023] Dividing the intermediate point cloud into blocks according to different preset scales to obtain a plurality of point cloud blocks;

[0024] Use the RANSAC algorithm to fit each point cloud block and obtain the candidate plane of each point cloud block;

[0025] Use the Patchwork++ algorithm to fuse each point cloud block to obtain the fused ground candidate point cloud;

[0026] For each point cloud in the ground candidate point cloud, the distance between the point cloud and the ground is calculated by using the corresponding ground plane equation, and the distance is substituted into a preset formula for screening to obtain a ground point cloud and a non-ground point cloud;

[0027] Wherein, the ground plane equation is determined based on the candidate plane.

[0028] In one embodiment, respectively extracting features of the target intensity map, the target depth map, the ground point cloud data, and the non-ground point cloud data, and constructing a residual function based on all the features includes:

[0029] Combine the ORB feature detection algorithm, the RANSAC algorithm and the optical flow method to extract and screen the target intensity map and the target depth map to obtain a number of visual feature points;

[0030] Extracting geometric features from the ground point cloud data and the non-ground point cloud data to obtain a plurality of geometric features;

[0031] A residual function is constructed based on all the visual feature points and the geometric features.

[0032] In one implementation, the geometric feature extraction is performed on the ground point cloud data to obtain a number of geometric features, including:

[0033] Calculating the curvature between each point and adjacent points in the ground point cloud data and the non-ground point cloud data;

[0034] Determining a continuity relationship between adjacent point clouds based on the curvature;

[0035] Processing a point cloud having a continuous spatial relationship within a frame of point cloud, extracting a first geometric feature and a second geometric feature, wherein the first geometric feature represents a feature of an angle, and the second geometric feature represents a feature of a surface;

[0036] Point clouds with discontinuous spatial relationships within a frame of point clouds are processed to extract third geometric features, where the third geometric features represent edge features.

[0037] In one implementation, solving the residual function to obtain a pose estimation result of the target device includes:

[0038] Solving the residual function using an iterative optimization algorithm with adaptive weights;

[0039] When the iterative optimization algorithm executes a preset number of iterations, a pose estimation result of the target device is obtained.

[0040] In a second aspect, an embodiment of the present invention further provides an anti-degradation LiDAR-SLAM device, the device comprising:

[0041] A first point cloud processing module is used to continuously acquire original point cloud data collected by a three-dimensional laser radar on a target device, and process the original point cloud data to obtain a target intensity map and a target depth map;

[0042] A second point cloud processing module is used to remove dynamic targets from the original point cloud data and perform ground segmentation to obtain ground point cloud data and non-ground point cloud data;

[0043] The pose estimation module is used to extract the features of the target intensity map, the target depth map, the ground point cloud data and the non-ground point cloud data respectively, and construct a residual function based on all the features, and solve the residual function to obtain the pose estimation result of the target device.

[0044] In a third aspect, an embodiment of the present invention further provides a terminal, comprising: a memory, a processor, and an anti-degradation LiDAR-SLAM program stored in the memory and executable on the processor, wherein the anti-degradation LiDAR-SLAM program implements the steps of the anti-degradation LiDAR-SLAM method as described above when executed by the processor.

[0045] In a fourth aspect, an embodiment of the present invention further provides a computer-readable storage medium, wherein the computer-readable storage medium stores an anti-degradation LiDAR-SLAM program, and the anti-degradation LiDAR-SLAM program can be executed to implement the steps of the anti-degradation LiDAR-SLAM method as described above.

[0046] Beneficial effects of the present invention: The present invention continuously acquires the original point cloud data collected by the three-dimensional laser radar on the target device, and processes the original point cloud data to obtain the target intensity map and the target depth map; obtains the ground point cloud data; removes the dynamic targets in the original point cloud data, and performs ground segmentation to obtain the ground point cloud data and non-ground point cloud data; respectively extracts the features of the target intensity map, the target depth map, the ground point cloud data and the non-ground point cloud data, and constructs a residual function based on all the features, and solves the residual function to obtain the pose estimation result of the target device. The present invention constructs the residual function and solves the pose estimation result by extracting multi-modal features, which can effectively improve the accuracy of pose estimation. BRIEF DESCRIPTION OF THE DRAWINGS

[0047] Figure 1It is a flow chart of a preferred embodiment of the anti-degradation LiDAR-SLAM method in the present invention.

[0048] Figure 2 It is a schematic diagram of the original point cloud data in the present invention.

[0049] Figure 3 It is a schematic diagram of the target intensity diagram in the present invention.

[0050] Figure 4 It is a schematic diagram of the target depth map in the present invention.

[0051] Figure 5 It is a schematic diagram of dynamic target detection in the present invention.

[0052] Figure 6 It is a schematic diagram of the ground segmentation principle in the present invention.

[0053] Figure 7 It is a schematic diagram of the environment of the real scene 1 in the present invention.

[0054] Figure 8 It is a schematic diagram of the ground segmentation result for the real scene 1 in the present invention.

[0055] Fig. 9 It is a schematic diagram of the segmentation result of the real scene 1 using the LeGO-LOAM segmentation method in the present invention.

[0056] Fig.10 It is a schematic diagram of the framework of the anti-degradation LiDAR-SLAM method in the present invention.

[0057] Fig.11 It is a structural schematic diagram of a preferred embodiment of the anti-degradation LiDAR-SLAM device in the present invention.

[0058] Fig.12 It is a terminal principle block diagram of the present invention. DETAILED DESCRIPTION

[0059] In order to make the purpose, technical solution and advantages of the present invention clearer and more specific, the present invention is further described in detail below with reference to the accompanying drawings and examples. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not used to limit the present invention.

[0060] With the rapid development of robotics and mobile mapping, the simultaneous localization and mapping (SLAM) technology based on LiDAR has been widely used in drones, autonomous vehicles, and indoor and outdoor mobile robots because of its insensitivity to lighting conditions and high data accuracy. However, as the density of LiDAR data continues to increase, the computational burden of point cloud registration also increases.

[0061] In this context, the classic feature-based LiDAR-SLAM method reduces the computational pressure through feature extraction strategies, but the multi-edge-dependent features and plane features are not sensitive enough to the vertical posture changes, and it is impossible to extract effective geometric features for constraints in the vertical direction. In degraded environments such as corridors and tunnels, the pose estimation accuracy is prone to low. At the same time, most LiDAR-SLAM algorithms only use geometric features and do not fully exploit the laser radar intensity information and the intensity and depth images generated by the point cloud. When the environmental features are insufficient or there are dynamic targets (such as pedestrians and vehicles), the pose estimation accuracy is also low.

[0062] In view of the above-mentioned defects of the prior art, the present invention provides a degradation-resistant LiDAR-SLAM method, device, terminal and medium, the method comprising: continuously acquiring the original point cloud data collected by the three-dimensional laser radar on the target device, and processing the original point cloud data to obtain a target intensity map and a target depth map; removing dynamic targets from the original point cloud data, and performing ground segmentation to obtain ground point cloud data and non-ground point cloud data; respectively extracting the features of the target intensity map, target depth map, ground point cloud data and non-ground point cloud data, and constructing a residual function based on all the features, and solving the residual function to obtain the pose estimation result of the target device. The present invention constructs a residual function and solves the pose estimation result by extracting multi-modal features, which can effectively improve the accuracy of pose estimation.

[0063] See also Figure 1 The anti-degradation LiDAR-SLAM method described in the embodiment of the present invention comprises the following steps:

[0064] Step S100: continuously acquire original point cloud data collected by the three-dimensional laser radar on the target device, and process the original point cloud data to obtain a target intensity map and a target depth map.

[0065] Specifically, the target device can be a robot, a vehicle with intelligent driving function, a drone, etc., all equipped with a 3D laser radar. It can be understood that as the target device moves, the surrounding environment is continuously collected to generate raw point cloud data, which is then processed for simultaneous positioning and map construction. Figure 2 shown.

[0066] In one implementation, preprocessing the original point cloud data to obtain a target intensity map and a target depth map includes:

[0067] Performing intensity correction on the original point cloud data to obtain second point cloud data;

[0068] Projecting the second point cloud data onto a two-dimensional plane to obtain an initial intensity map and an initial depth map;

[0069] Upsampling and data completion are performed on the initial intensity map and the initial depth map using a preset interpolation method to obtain a second intensity map and a second depth map;

[0070] The second intensity map and the second depth map are processed by an adaptive histogram equalization method to obtain a target intensity map and a target depth map.

[0071] Specifically, in the point cloud data output by the 3D LiDAR, the intensity is affected by many factors such as the reflectivity of the object surface, the laser incident angle, the ranging distance, and the transmission power. In order to obtain more stable intensity information and ensure the accuracy of subsequent point cloud data applications, intensity correction is required. In the intensity correction process, the intensity of point p is calculated by first comparing it with the adjacent point p on the same scan line. a 、p b The local normal vector n is formed. The formula of the local normal vector n is as follows:

[0072]

[0073] Where × is the vector cross product operation and |·| represents the vector norm.

[0074] Then, the incident angle is calculated using the p coordinate and n, the point cloud with low signal-to-noise ratio is eliminated according to the incident angle, and the intensity information is corrected to obtain the second point cloud data.

[0075] After completing the intensity correction, the second point cloud data is projected onto a two-dimensional plane according to formula (2) to obtain an initial intensity map and an initial depth map. Figure 2 As shown. Formula (2) is expressed as:

[0076]

[0077] Where x, y, and z are the point cloud coordinates, r is the distance from the point to the origin of the laser radar coordinates, and w and h are the width and height of the projected image, respectively. Through this process, the three-dimensional point cloud can be mapped into a two-dimensional intensity map and a two-dimensional depth map related to the three-dimensional laser radar acquisition direction, that is, the initial intensity map and the initial depth map, thereby providing a visualization basis for subsequent feature extraction.

[0078] Since the distribution of laser point clouds in the horizontal and vertical directions is often sparse and uneven, the image obtained after simple projection has holes and discrete problems. To this end, the present invention uses bilinear interpolation and distance-weighted inverse distance interpolation (IDW) methods to upsample and complete the projected image. Specifically, for the interpolated pixel point (x, y), formula (3) is used to calculate:

[0079]

[0080] In the formula, Ii is the intensity value of the four nearest known points around the pixel, is the distance between the known point i and the pixel to be interpolated p. This method assigns higher weights to known points that are closer, thereby ensuring smooth transition and spatial consistency of image intensity information.

[0081] After interpolation and upsampling, the second intensity map and the second depth map are processed by an adaptive histogram equalization method (CLAHE). The adaptive histogram equalization method can enhance the local contrast and texture details of the image and generate Figure 3 The target intensity diagram shown in Figure 4 The target depth map is shown.

[0082] See also Figure 1 The anti-degradation LiDAR-SLAM method described in the embodiment of the present invention further includes the following steps:

[0083] Step S200: remove dynamic targets from the original point cloud data, and perform ground segmentation to obtain ground point cloud data and non-ground point cloud data.

[0084] Specifically, during the point cloud acquisition process, there are often dynamic targets such as people and vehicles in the environment. The movement of these objects will affect the stability of the lidar point cloud, thereby reducing the accuracy of registration and positioning. The present invention removes the point cloud corresponding to the dynamic target and retains the relatively static point cloud. This process helps to reduce noise and interference in subsequent feature extraction and optimization, and improve the overall robustness and accuracy stability of the system.

[0085] In one implementation, the removing of dynamic targets from the original point cloud data and performing ground segmentation to obtain ground point cloud data and non-ground point cloud data includes:

[0086] Inputting the original point cloud data into a pre-trained 3D point cloud semantic segmentation model to obtain a labeled point cloud;

[0087] Calculate the pixel intensity change between adjacent frames in the initial depth map to obtain a residual image;

[0088] Inputting the residual image, the initial depth map and the annotated point cloud into a preset deep learning network, and obtaining a point cloud corresponding to the dynamic pixel point through fusion analysis by the deep learning network;

[0089] Eliminate the point cloud corresponding to the dynamic pixel points in the original point cloud data to obtain an intermediate point cloud;

[0090] The intermediate point cloud is subjected to ground segmentation to obtain ground point cloud data and non-ground point cloud data.

[0091] Specifically, the 3D point cloud semantic segmentation model can be a Minkowski SR-UNet model. The original point cloud data is classified and annotated through the 3D point cloud semantic segmentation model, and each point is assigned a corresponding category (for example: static background point, pedestrian point, vehicle point). Then, the segmented and annotated point cloud (i.e., the annotated point cloud) is mapped to a two-dimensional plane. A second depth map is obtained, and the pixel intensity changes between adjacent frames in the second depth map are calculated to obtain a residual image. The pixel intensity change calculation formula is as follows:

[0092]

[0093] In the formula, r m is the intensity value of the mth pixel in the jth frame, is the intensity value of the pixel after being mapped from the j frame to the i frame position. If it is larger, it means that there is a significant temporal change at the pixel position and there is a potential dynamic target.

[0094] Next, the present invention inputs the residual image together with the initial depth map and the annotated point cloud into a two-dimensional deep learning network (such as SalsaNext) for fusion analysis. SalsaNext can comprehensively utilize spatial, semantic and temporal residual information to more accurately detect dynamic target areas. Figure 5 As shown. After identifying the point cloud coordinates (i.e., dynamic targets) corresponding to these dynamic pixel points, the point clouds corresponding to the dynamic pixel points in the original point cloud data are removed to obtain an intermediate point cloud; the intermediate point cloud is ground segmented to obtain ground point cloud and non-ground point cloud data. The present invention removes such dynamic point cloud data and only retains relatively static scene points. This process helps to reduce noise and interference in subsequent feature extraction and optimization, and improve the overall robustness and precision stability of the system.

[0095] In one implementation, performing ground segmentation on the intermediate point cloud to obtain ground point cloud and non-ground point cloud data includes:

[0096] Dividing the intermediate point cloud into blocks according to different preset scales to obtain a plurality of point cloud blocks;

[0097] Use the RANSAC algorithm to fit each point cloud block and obtain the candidate plane of each point cloud block;

[0098] Use the Patchwork++ algorithm to fuse each point cloud block to obtain the fused ground candidate point cloud;

[0099] For each point cloud in the ground candidate point cloud, the distance between the point cloud and the ground is calculated by using the corresponding ground plane equation, and the distance is substituted into a preset formula for screening to obtain a ground point cloud and a non-ground point cloud;

[0100] Wherein, the ground plane equation is determined based on the candidate plane.

[0101] Specifically, ground extraction plays a key role in three-dimensional environment modeling, because the ground is usually the largest and most stable planar structure, which can provide a vertical reference for pose solution. After obtaining the intermediate point cloud, it is processed in blocks at different preset scales, and the candidate plane is fitted for each point cloud block using the RANSAC method. In the present invention, the multi-scale strategy can adapt to the height fluctuation and unevenness of the ground, and improve the accuracy and robustness of ground extraction. In the real environment, the ceiling, floor or other upper parallel structures may be approximately parallel to the ground, resulting in misjudgment. This environment is also called a degraded environment. In order to reduce misjudgment, the present invention needs to screen the ground candidate point cloud after obtaining it, so as to obtain more accurate ground point cloud data and non-ground point cloud data.

[0102] The present invention utilizes formula (5):

[0103]

[0104] Perform point cloud screening processing, where d i For point p i The distance to the ground, p0 is the projection point of the sensor platform on the ground, θ max is the preset angle threshold. Through this formula, points in the ground candidate point cloud that meet this condition can be regarded as non-ground point cloud data. After eliminating the non-ground point cloud data, the ground point cloud data can be obtained. Through the above process, the present invention can still quickly and accurately extract ground points in a variety of complex scenes, providing a stable and reliable vertical reference for subsequent posture optimization. The schematic diagram of the ground segmentation principle of the present invention is shown in Figure 6 As shown. Figure 7 As shown, Figure 7 is a schematic diagram of the environment of the real scene 1. The method of the present invention can be used to obtain the following Figure 8 The ground segmentation diagram shown in Figure 1. The segmentation result diagram of the real scene 1 using the LeGO-LOAM segmentation method is shown in Figure 2. Fig. 9 As shown, it can be seen that the ground segmentation effect of the method of the present invention is more accurate.

[0105] See also Figure 1 The anti-degradation LiDAR-SLAM method described in the embodiment of the present invention further includes the following steps:

[0106] Step S300: extract the features of the target intensity map, the target depth map, the ground point cloud data and the non-ground point cloud data respectively, construct a residual function based on all the features, and solve the residual function to obtain a pose estimation result of the target device.

[0107] Specifically, most LiDAR-SLAM algorithms only use geometric features, and do not fully exploit the laser radar intensity information and the intensity and depth images generated by the point cloud. When the environmental features are insufficient or there are dynamic targets (such as pedestrians and vehicles), the positioning and mapping performance of traditional methods are easily affected. The present invention obtains a rich, diverse and robust feature set by fusing multi-source features from images and point clouds, providing sufficient constraint information for subsequent pose optimization. When the environmental scene is single or the features are insufficient, these additional intensity, depth and ground features can effectively make up for the shortcomings of pure geometric features.

[0108] In one implementation, respectively extracting features of the target intensity map, the target depth map, the ground point cloud data, and the non-ground point cloud data, and constructing a residual function based on all the features includes:

[0109] Combine the ORB feature detection algorithm, the RANSAC algorithm and the optical flow method to extract and screen the target intensity map and the target depth map to obtain a number of visual feature points;

[0110] Extracting geometric features from the ground point cloud data to obtain a number of geometric features;

[0111] A residual function is constructed based on all the visual feature points and the geometric features.

[0112] Specifically, the influencing features of the target intensity map and the target depth map are first extracted. During the extraction process, the ORB (Oriented FAST and Rotated BRIEF) feature detection algorithm is first used to extract the initial feature points. The ORB algorithm has efficient and robust feature detection and description capabilities. Then, the RANSAC (RANdom SAmple Consensus) algorithm is used to pair the initial feature points to obtain a number of matching feature point pairs; the optical flow method is used to track the matching feature point pairs, and the matching feature point pairs with stable relationships are retained to obtain a number of visual feature points. In this way, it is beneficial to maintain a high level of feature density and matching success rate in a degraded environment. In addition to extracting visual feature points, geometric features are also extracted. By fusing multi-source features from images and point clouds, the present invention obtains a rich, diverse, and robust feature set, providing sufficient constraint information for subsequent pose optimization. The pose parameter to be optimized is defined as ξ (including translation and rotation components), and a residual function including point-to-line, point-to-surface, intensity difference, and ground direction constraints is constructed and expressed as:

[0113] min∑ i ρ i ∥D(T(p i ,ξ))∥2, where p iis the feature point, T(·) is the coordinate transformation function, D(·) is the residual measurement function, ρ i is the adaptive weight factor. The adaptive weight mechanism can improve the contribution of ground features and intensity features when geometric features are insufficient, thereby maintaining the stability and accuracy of pose solution.

[0114] In one implementation, the geometric features of the ground point cloud data and the non-ground point cloud data are extracted to obtain a number of geometric features, including:

[0115] Calculating the curvature between each point and adjacent points in the ground point cloud data and the non-ground point cloud data;

[0116] Determining a continuity relationship between adjacent point clouds based on the curvature;

[0117] Processing a point cloud having a continuous spatial relationship within a frame of point cloud, extracting a first geometric feature and a second geometric feature, wherein the first geometric feature represents a feature of an angle, and the second geometric feature represents a feature of a surface;

[0118] Point clouds with discontinuous spatial relationships within a frame of point clouds are processed to extract third geometric features, where the third geometric features represent edge features.

[0119] Specifically, for point cloud data, the present invention determines the discontinuous and continuous relationship based on the change in distance between a point and its adjacent points. For point clouds with a continuous relationship, the first geometric feature and the second geometric feature are distinguished by using the normalization of odd adjacent points and the scatter matrix feature ratio. The first geometric feature represents the feature of the angle, and the second geometric feature represents the feature of the surface. For point clouds with a discontinuous relationship, the points where the distance change between discontinuous points exceeds a preset distance threshold are calculated and used as the third geometric feature. The third geometric feature represents the point where the distance change is significant in the feature of the edge.

[0120] In one implementation, solving the residual function to obtain a pose estimation result of the target device includes:

[0121] Solving the residual function using an iterative optimization algorithm with adaptive weights;

[0122] When the iterative optimization algorithm executes a preset number of iterations, a pose estimation result of the target device is obtained.

[0123] Specifically, the present invention integrates the extracted geometric features and visual feature points into the same least squares optimization framework, and solves the posture through an iterative optimization algorithm with adaptive weights (such as Levenberg-Marquardt).

[0124] In summary, if Fig.10As shown, the framework diagram of the anti-degradation LiDAR-SLAM method. The present invention pre-processes the LiDAR point cloud, uses deep learning and point cloud semantic segmentation model combined with residual image detection and removes dynamic targets, uses Patchwork++ for ground segmentation, and then extracts multimodal features (including visual feature points from intensity / depth images, corner features, surface features and intensity difference features in point clouds), and uses iterative optimization algorithms (such as Levenberg-Marquardt) to uniformly solve the residual pose containing multiple constraints, thereby maintaining high precision and low drift in positioning and mapping in degraded and dynamic environments.

[0125] In one embodiment, if Fig.11 As shown, based on the above-mentioned anti-degradation LiDAR-SLAM method, the present invention also provides an anti-degradation LiDAR-SLAM device, which includes:

[0126] The first point cloud processing module 100 is used to continuously acquire the original point cloud data collected by the three-dimensional laser radar on the target device, and process the original point cloud data to obtain a target intensity map and a target depth map;

[0127] The second point cloud generation module 200 is used to remove dynamic targets from the original point cloud data and perform ground segmentation to obtain ground point cloud data and non-ground point cloud data;

[0128] The pose estimation module 300 is used to respectively extract the features of the target intensity map, the target depth map, the ground point cloud data and the non-ground point cloud data, and construct a residual function based on all the features, and solve the residual function to obtain the pose estimation result of the target device.

[0129] In one embodiment, the device further comprises:

[0130] A correction unit, used for performing intensity correction on the original point cloud data to obtain second point cloud data;

[0131] A point cloud mapping unit, used for projecting the second point cloud data onto a two-dimensional plane to obtain an initial intensity map and an initial depth map;

[0132] An interpolation processing unit, configured to perform upsampling and data completion on the initial intensity map and the initial depth map using a preset interpolation method to obtain a second intensity map and a second depth map;

[0133] The image quality improving unit is used to process the second intensity map and the second depth map by using an adaptive histogram equalization method to obtain a target intensity map and a target depth map.

[0134] In one embodiment, the ground point cloud generation module includes:

[0135] A point cloud annotation unit, used for inputting the original point cloud data into a pre-trained 3D point cloud semantic segmentation model to obtain an annotated point cloud;

[0136] A residual image generating unit, which calculates the pixel intensity change between adjacent frames in the initial depth map to obtain a residual image;

[0137] A dynamic point cloud recognition unit, used for inputting the residual image, the initial depth map and the annotated point cloud into a preset deep learning network, and obtaining a point cloud corresponding to a dynamic pixel point through fusion analysis by the deep learning network;

[0138] A point cloud removal unit, used to remove point clouds corresponding to dynamic pixel points in the original point cloud data to obtain an intermediate point cloud;

[0139] The ground segmentation unit is used to perform ground segmentation on the intermediate point cloud to obtain ground point cloud data and non-ground point cloud data.

[0140] In one embodiment, the device further comprises:

[0141] A point cloud segmentation unit, configured to segment the intermediate point cloud into blocks according to different preset scales to obtain a plurality of point cloud blocks;

[0142] A fitting unit is used to fit each point cloud block using the RANSAC algorithm to obtain a candidate plane for each point cloud block;

[0143] The fusion unit is used to fuse each point cloud block using the Patchwork++ algorithm to obtain the fused ground candidate point cloud;

[0144] The point cloud screening unit is used to calculate the distance between each point cloud in the ground candidate point cloud and the ground through the corresponding ground plane equation, and substitute the distance into a preset formula for screening to obtain ground point cloud data and non-ground point cloud data, wherein the ground plane equation is determined based on the candidate plane.

[0145] In one embodiment, the device further comprises:

[0146] A first feature extraction unit is used to extract and screen the target intensity map and the target depth map by combining an ORB feature detection algorithm, a RANSAC algorithm and an optical flow method to obtain a plurality of visual feature points;

[0147] A second feature extraction unit is used to extract geometric features from the ground point cloud data and the non-ground point cloud data to obtain a plurality of geometric features;

[0148] A function construction unit is used to construct a residual function based on all the visual feature points and the geometric features.

[0149] In one embodiment, the device further comprises:

[0150] A curvature calculation unit, used for calculating the curvature between each point and adjacent points in the ground point cloud data and the non-ground point cloud data;

[0151] a relationship judgment unit, configured to judge a continuous relationship between adjacent point clouds based on the curvature;

[0152] A third feature extraction unit is used to process a point cloud having a continuous spatial relationship in a frame of point cloud, and extract a first geometric feature and a second geometric feature, wherein the first geometric feature represents a feature of an angle, and the second geometric feature represents a feature of a surface;

[0153] The fourth feature extraction unit is used to process the point cloud with discontinuous relationship in the space within a frame of point cloud, and extract the third geometric feature, where the third geometric feature represents the feature of the edge.

[0154] In one embodiment, the device further comprises:

[0155] A solving unit, used for solving the residual function by using an iterative optimization algorithm with adaptive weights;

[0156] The result generating unit is used to obtain the pose estimation result of the target device after the iterative optimization algorithm executes a preset number of iterations.

[0157] Based on the above embodiment, the present invention further provides a terminal, whose principle block diagram can be shown as follows: Fig.12 As shown. The terminal includes a processor, a memory, a network interface and a display screen connected via a device bus. The processor of the terminal is used to provide computing and control capabilities. The memory of the terminal includes a non-volatile storage medium and an internal memory. The non-volatile storage medium stores an operating device and an anti-degradation LiDAR-SLAM program. The internal memory provides an environment for the operation of the operating device and the anti-degradation LiDAR-SLAM program in the non-volatile storage medium. The network interface of the terminal is used to communicate with an external terminal through a network connection. When the anti-degradation LiDAR-SLAM program is executed by the processor, the steps of any one of the above-mentioned anti-degradation LiDAR-SLAM methods are implemented. The display screen of the terminal can be a liquid crystal display or an electronic ink display.

[0158] Those skilled in the art will understand that Fig.12 The principle block diagram shown in the figure is only a block diagram of a partial structure related to the scheme of the present invention, and does not constitute a limitation on the terminal to which the scheme of the present invention is applied. The specific terminal may include more or fewer components than those shown in the figure, or combine certain components, or have a different arrangement of components.

[0159] In one embodiment, a terminal is provided, comprising a memory, a processor, and an anti-degradation LiDAR-SLAM program stored in the memory and executable on the processor. When the anti-degradation LiDAR-SLAM program is executed by the processor, the steps of any one of the anti-degradation LiDAR-SLAM methods provided in the embodiments of the present invention are implemented.

[0160] An embodiment of the present invention further provides a computer-readable storage medium, on which an anti-degradation LiDAR-SLAM program is stored. When the anti-degradation LiDAR-SLAM program is executed by a processor, the steps of any one of the anti-degradation LiDAR-SLAM methods provided in the embodiments of the present invention are implemented.

[0161] It should be understood that the serial numbers of the steps in the above embodiments do not imply a sequence of execution, and the execution sequence of each process should be determined by its function and internal logic, and should not constitute any limitation on the implementation process of the embodiments of the present invention.

[0162] Those skilled in the art can clearly understand that for the convenience and simplicity of description, only the division of the above-mentioned functional units and modules is used as an example for illustration. In practical applications, the above-mentioned function allocation can be completed by different functional units and modules as needed, that is, the internal structure of the above-mentioned device can be divided into different functional units or modules to complete all or part of the functions described above. The functional units and modules in the embodiment can be integrated in a processing unit, or each unit can exist physically separately, or two or more units can be integrated in one unit. The above-mentioned integrated unit can be implemented in the form of hardware or in the form of software functional units. In addition, the specific names of the functional units and modules are only for the convenience of distinguishing each other, and are not used to limit the scope of protection of the present invention. The specific working process of the units and modules in the above-mentioned device can refer to the corresponding process in the aforementioned method embodiment, which will not be repeated here.

[0163] In the above embodiments, the description of each embodiment has its own emphasis. For parts that are not described or recorded in detail in a certain embodiment, reference can be made to the relevant descriptions of other embodiments.

[0164] Those skilled in the art will appreciate that the units and algorithm steps of each example described in conjunction with the embodiments disclosed herein can be implemented in electronic hardware, or a combination of computer software and electronic hardware. Whether these functions are performed in hardware or software depends on the specific application and design constraints of the technical solution. Professional and technical personnel can use different methods to implement the described functions for each specific application, but such implementation should not be considered to be beyond the scope of the present invention.

[0165] In the embodiments provided by the present invention, it should be understood that the disclosed apparatus / terminal equipment and method can be implemented in other ways. For example, the apparatus / terminal equipment embodiments described above are only illustrative, for example, the division of the above modules or units is only a logical function division, and in actual implementation, other division methods can be used, for example, multiple units or components can be combined or integrated into another apparatus, or some features can be ignored or not executed.

[0166] The embodiments described above are only used to illustrate the technical solutions of the present invention, rather than to limit the same. Although the present invention has been described in detail with reference to the aforementioned embodiments, it should be understood by those skilled in the art that the technical solutions described in the aforementioned embodiments may still be modified, or some of the technical features therein may be replaced by equivalents. However, these modifications or replacements do not deviate from the spirit and scope of the technical solutions of the embodiments of the present invention, and should be included in the protection scope of the present invention.

Claims

1. A degradation-resistant LiDAR-SLAM method, characterized in that: The method comprises: Continuously acquiring original point cloud data collected by a three-dimensional laser radar on a target device, and processing the original point cloud data to obtain a target intensity map and a target depth map; Eliminating dynamic targets in the original point cloud data, and performing ground segmentation to obtain ground point cloud data and non-ground point cloud data; The features of the target intensity map, the target depth map, the ground point cloud data and the non-ground point cloud data are extracted respectively, and a residual function is constructed based on all the features, and the residual function is solved to obtain a pose estimation result of the target device.

2. The degradation-resistant LiDAR-SLAM method according to claim 1, characterized in that: The preprocessing of the original point cloud data to obtain a target intensity map and a target depth map includes: Performing intensity correction on the original point cloud data to obtain second point cloud data; Projecting the second point cloud data onto a two-dimensional plane to obtain an initial intensity map and an initial depth map; Upsampling and data completion are performed on the initial intensity map and the initial depth map using a preset interpolation method to obtain a second intensity map and a second depth map; The second intensity map and the second depth map are processed by an adaptive histogram equalization method to obtain a target intensity map and a target depth map.

3. The degradation-resistant LiDAR-SLAM method according to claim 2, characterized in that: The step of removing dynamic targets from the original point cloud data and performing ground segmentation to obtain ground point cloud data and non-ground point cloud data includes: Inputting the original point cloud data into a pre-trained 3D point cloud semantic segmentation model to obtain a labeled point cloud; Calculate the pixel intensity change between adjacent frames in the initial depth map to obtain a residual image; Inputting the residual image, the initial depth map and the annotated point cloud into a preset deep learning network, and obtaining a point cloud corresponding to the dynamic pixel point through fusion analysis by the deep learning network; Eliminate the point cloud corresponding to the dynamic pixel points in the original point cloud data to obtain an intermediate point cloud; The intermediate point cloud is subjected to ground segmentation to obtain ground point cloud data and non-ground point cloud data.

4. The degradation-resistant LiDAR-SLAM method according to claim 3, characterized in that: The performing ground segmentation on the intermediate point cloud to obtain ground point cloud data and non-ground point cloud data includes: Dividing the intermediate point cloud into blocks according to different preset scales to obtain a plurality of point cloud blocks; Use the RANSAC algorithm to fit each point cloud block and obtain the candidate plane of each point cloud block; Use the Patchwork++ algorithm to fuse each point cloud block to obtain the fused ground candidate point cloud; For each point in the ground candidate point cloud, the distance between the point and the ground is calculated by using the corresponding ground plane equation, and the distance is substituted into a preset formula for screening to obtain ground point cloud data and non-ground point cloud data; Wherein, the ground plane equation is determined based on the candidate plane.

5. The degradation-resistant LiDAR-SLAM method according to claim 1, characterized in that: The extracting the features of the target intensity map, the target depth map, the ground point cloud data and the non-ground point cloud data respectively, and constructing a residual function based on all the features, comprises: Combine the ORB feature detection algorithm, the RANSAC algorithm and the optical flow method to extract and screen the target intensity map and the target depth map to obtain a number of visual feature points; Extracting geometric features from the ground point cloud data and the non-ground point cloud data to obtain a plurality of geometric features; A residual function is constructed based on all the visual feature points and the geometric features.

6. The degradation-resistant LiDAR-SLAM method according to claim 5, characterized in that: The geometric features of the ground point cloud data and the non-ground point cloud data are extracted to obtain a number of geometric features, including: Calculating the curvature between each point and adjacent points in the ground point cloud data and the non-ground point cloud data; Determining a continuity relationship between adjacent point clouds based on the curvature; Processing a point cloud having a continuous spatial relationship within a frame of point cloud, extracting a first geometric feature and a second geometric feature, wherein the first geometric feature represents a feature of an angle, and the second geometric feature represents a feature of a surface; Point clouds with discontinuous spatial relationships within a frame of point clouds are processed to extract third geometric features, where the third geometric features represent edge features.

7. The degradation-resistant LiDAR-SLAM method according to claim 1, characterized in that: The step of solving the residual function to obtain a pose estimation result of the target device includes: Solving the residual function using an iterative optimization algorithm with adaptive weights; When the iterative optimization algorithm executes a preset number of iterations, a pose estimation result of the target device is obtained.

8. A degradation-resistant LiDAR-SLAM device, characterized in that: include: A first point cloud processing module is used to continuously acquire original point cloud data collected by a three-dimensional laser radar on a target device, and process the original point cloud data to obtain a target intensity map and a target depth map; A second point cloud processing module is used to remove dynamic targets from the original point cloud data and perform ground segmentation to obtain ground point cloud data and non-ground point cloud data; The pose estimation module is used to respectively extract the features of the target intensity map, the target depth map, the ground point cloud data and the non-ground point cloud data, and construct a residual function based on all the features, and solve the residual function to obtain the pose estimation result of the target device.

9. A terminal, characterized in that: The terminal includes: a memory, a processor, and an anti-degradation LiDAR-SLAM program stored in the memory and executable on the processor. When the anti-degradation LiDAR-SLAM program is executed by the processor, the steps of the anti-degradation LiDAR-SLAM method according to any one of claims 1 to 7 are implemented.

10. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores an anti-degradation LiDAR-SLAM program, and when the anti-degradation LiDAR-SLAM program is executed by a processor, the steps of the anti-degradation LiDAR-SLAM method according to any one of claims 1 to 7 are implemented.

Images