Laser SLAM method based on ground segmentation

Abstract

The invention relates to the technical field of laser SLAM, and particularly discloses a laser SLAM method based on ground segmentation, and the method comprises the steps: obtaining laser radar point cloud data information, carrying out the point cloud preprocessing of the laser radar point cloud data, and obtaining ground point cloud features and non-ground point cloud features; respectively carrying out feature extraction, feature matching and pose estimation on the ground point cloud features and the non-ground point cloud features to obtain a pose nonlinear optimization constraint result; selecting a key frame according to a pose nonlinear optimization constraint result, constructing a local map according to pose information of the key frame, and obtaining a local map construction result; loopback detection is carried out according to the local map construction result, and a loopback detection constraint result is obtained; and performing global pose optimization processing according to a loopback detection constraint result and a local map construction result to obtain a global pose and global map construction result. The laser SLAM method based on ground segmentation provided by the invention can improve the positioning precision and the operation stability.

Description

Technical Field

[0001] The present invention relates to the field of laser SLAM technology, and in particular to a laser SLAM method based on ground segmentation. Background Art

[0002] Laser SLAM technology has been widely used in recent years in fields such as autonomous driving, mobile robotics, and augmented reality. Currently, mainstream laser SLAM algorithms include LOAM and its variants such as A-LOAM, LeGO-LOAM, and LIO-SAM.

[0003] LOAM, a classic laser SLAM solution, calculates the curvature of a raw point cloud to separate it into planar and edge features. It then estimates the sensor pose through point-to-point and point-to-plane registration. Its core concept is to decompose the SLAM problem into two processes: odometry (high-frequency but low-precision) and mapping (low-frequency but high-precision). Feature extraction reduces computational effort and enables real-time processing. A-LOAM is a ROS-adapted version of LOAM, retaining its core algorithmic framework but simplifying and optimizing its implementation. LeGO-LOAM builds on LOAM by adding a ground segmentation module that distinguishes between ground and non-ground points, improving feature extraction speed and computational efficiency. LIO-SAM represents a further development of laser SLAM technology, integrating lidar and IMU (inertial measurement unit) data and employing a factor graph framework for tightly coupled optimization. LIO-SAM pre-integrates IMU measurements and leverages inertial information to compensate for lidar's limitations in fast-moving or feature-sparse environments. It also introduces a loop closure detection mechanism based on scan matching, effectively reducing accumulated errors over long runs.

[0004] The current limitations of laser SLAM are mainly manifested in the following aspects: the LOAM system lacks a loop detection mechanism and cannot correct the accumulated errors caused by the passage of time, sensor noise, or limitations of the estimation method. Feature extraction relies on the geometric characteristics of the point cloud, performing poorly in feature-sparse environments, being sensitive to the speed of sensor movement, and performance degrading in fast-moving scenes. The shortcoming of LeGO-LOAM is that its ground segmentation is based on relatively simple rules and performs poorly in complex terrain or undulating ground environments. Naive ground segmentation methods will also greatly reduce the number of feature points, resulting in sparse and discontinuous global maps in feature-sparse areas, affecting the overall mapping quality, especially in large-scale environments. The limitation of LIO-SAM is the lack of an effective dynamic object detection and filtering mechanism. It is easily disturbed in environments with dense populations or frequent vehicles, and has high computing resource requirements.

[0005] Therefore, how to provide a laser SLAM method to improve positioning accuracy and operation stability has become a technical problem that needs to be solved urgently by those skilled in the art. Summary of the Invention

[0006] The present invention provides a laser SLAM method based on ground segmentation, which solves the problem of low laser SLAM positioning accuracy existing in the related art.

[0007] As one aspect of the present invention, a laser SLAM method based on ground segmentation is provided, which includes:

[0008] Acquire laser radar point cloud data information, and perform point cloud preprocessing on the laser radar point cloud data to obtain ground point cloud features and non-ground point cloud features, wherein the point cloud preprocessing at least includes ground segmentation processing;

[0009] Perform feature extraction, feature matching, and pose estimation on the ground point cloud features and the non-ground point cloud features, respectively, to obtain pose nonlinear optimization constraint results;

[0010] Selecting a key frame according to the pose nonlinear optimization constraint result, and constructing a local map according to the pose information of the current key frame to obtain a local map construction result;

[0011] Perform loop closure detection based on the local map construction result to obtain a loop closure detection constraint result;

[0012] A global pose optimization process is performed based on the loop detection constraint result and the local map construction result to obtain a global pose and a global map construction result.

[0013] Furthermore, the laser radar point cloud data is pre-processed, including:

[0014] Performing filtering and denoising on the laser radar point cloud data to obtain filtered laser radar point cloud data;

[0015] The filtered lidar point cloud data is subjected to ground segmentation processing to obtain ground point cloud features and non-ground point cloud features.

[0016] Furthermore, ground segmentation processing is performed on the filtered lidar point cloud data to obtain ground point cloud features and non-ground point cloud features, including:

[0017] Divide the filtered lidar point cloud data into concentric areas according to a multi-layer annular area division method to obtain multiple annular areas, wherein the annular areas include a core area, a short-range area, a medium-range area, and a long-range area according to the distance from the lidar from near to far;

[0018] Each annular area is divided into sectors and a grid is designed to obtain multiple grid units;

[0019] For each grid cell, ground plane feature extraction and ground plane fitting are performed to obtain preliminary screening results of ground points;

[0020] A ground feature evaluation is performed on the preliminary screening results of the ground points to obtain ground point cloud features and non-ground point cloud features.

[0021] Furthermore, feature extraction, feature matching, and pose estimation are performed on the ground point cloud features and the non-ground point cloud features to obtain pose nonlinear optimization constraint results, including:

[0022] Performing plane feature extraction on the ground point cloud feature to obtain a first plane feature, and performing edge feature and ground feature extraction on the non-ground point cloud feature to obtain an edge feature and a second plane feature;

[0023] Performing point-to-plane registration on the first plane feature and the second plane feature to obtain a point-to-plane registration result, and performing point-to-line registration on the edge feature to obtain a point-to-line registration result;

[0024] Performing posture constraint processing according to the point-plane registration result and the point-line registration result to obtain a posture constraint result;

[0025] The posture constraint result is subjected to nonlinear optimization processing to obtain a posture nonlinear optimization constraint result.

[0026] Furthermore, performing point-to-plane registration on the first plane feature and the second plane feature to obtain a point-to-plane registration result includes:

[0027] Selecting a point in the first plane feature as a first plane feature point, and selecting a point in the second plane feature as a second plane feature point;

[0028] Constructing point-to-plane residuals from the first plane feature point and the second plane feature point to a preset plane respectively;

[0029] A point-to-surface registration result for constraining the roll angle rotation, pitch angle rotation, and z-direction translation of the laser radar is obtained according to the point-to-surface residual.

[0030] Furthermore, performing point-line registration on the edge feature to obtain a point-line registration result includes:

[0031] Selecting a point in the edge feature as an edge feature point;

[0032] Constructing point-to-line residuals from the edge feature points and the preset line features;

[0033] A point-line registration result for constraining the yaw angle, x-direction translation, and y-direction translation of the laser radar is obtained according to the point-to-line residual.

[0034] Furthermore, loop closure detection is performed based on the local map construction result to obtain loop closure detection constraint results, including:

[0035] Identify loop candidate frames based on the current key frame in the local map construction result;

[0036] Perform FPFH feature extraction and matching based on the current key frame and the loop candidate frame to obtain the initial matching result;

[0037] Pruning abnormal points in the initial matching result according to the point cloud registration residual to obtain a final matching result;

[0038] Rotation estimation and component translation estimation are performed on the final matching result to obtain a loop detection constraint result.

[0039] Furthermore, outliers in the initial matching result are trimmed according to the point cloud registration residual to obtain a final matching result, including:

[0040] Randomly select a minimum subset based on the initial matching result to calculate the initial transformation;

[0041] Calculate the residuals of all point pairs in the initial matching result according to the initial transformation, and count the number of inliers that meet the preset conditions;

[0042] Repeat the above process until the initial transformation with the largest number of retained inliers is obtained;

[0043] The number of inliers is formed into an optimal inlier set, and the pose is optimized according to the weighted least squares method to obtain the final matching result.

[0044] Furthermore, a global pose optimization process is performed based on the loop detection constraint result and the local map construction result to obtain a global pose and a global map construction result, including:

[0045] Constructing a factor graph based on the loop detection constraint result and the pose constraint result of the current key frame in the local map construction result to obtain a factor graph construction result;

[0046] Factor graph optimization is performed on the factor graph construction result to obtain construction results of the global pose and the global map.

[0047] Furthermore, a factor graph is constructed according to the loop detection constraint result and the pose constraint result of the current key frame in the local map construction result to obtain a factor graph construction result, including:

[0048] The loop detection factor is determined according to the pose estimation residual between the loop candidate frame and the current key frame in the loop detection constraint result, wherein the optimization constraint expression of the loop detection factor is:

[0049]

[0050] Among them, e l represents the loop detection factor, and Both represent the pose of the associated frame of loop closure detection;

[0051] The lidar factor is determined according to the pose estimation residual of the current key frame in the local map construction result, wherein the optimization constraint expression of the lidar factor is:

[0052]

[0053] Among them, e y represents the lidar factor, ΔT ij Represents the relative transformation obtained by point cloud matching, T i w and All represent keyframe lidar poses;

[0054] A factor graph is constructed according to the loop detection factor and the lidar factor, wherein the optimization objective function of the factor graph is expressed as:

[0055]

[0056] The laser SLAM method based on ground segmentation provided by the present invention performs point cloud segmentation and feature extraction and matching from laser radar data, and then performs global optimization and map correction through loop detection, thereby obtaining the construction results of global pose and global map. Therefore, this laser SLAM method based on ground segmentation of the present invention effectively improves the positioning accuracy, environmental adaptability and stability of laser SLAM during long-term operation by integrating advanced feature extraction, point cloud processing, factor graph optimization and loop detection technologies, ultimately achieving more efficient and reliable SLAM performance. BRIEF DESCRIPTION OF THE DRAWINGS

[0057] The accompanying drawings are used to provide further understanding of the present invention and constitute a part of the specification. Together with the following specific embodiments, they are used to explain the present invention, but do not constitute a limitation of the present invention.

[0058] Figure 1 This is a flow chart of the laser SLAM method based on ground segmentation provided by the present invention.

[0059] Figure 2This is a flowchart of a specific implementation of the ground segmentation-based laser SLAM method provided by the present invention.

[0060] Figure 3 This is a flowchart of preprocessing lidar point cloud data provided by the present invention.

[0061] Figure 4a This is a schematic diagram of the concentric area division results provided by the present invention.

[0062] Figure 4b Schematic diagram of traditional point cloud segmentation results.

[0063] Figure 5 This is the ground segmentation flow chart provided by the present invention.

[0064] Figure 6 Flowchart of the method for feature extraction, feature matching and pose estimation provided by the present invention.

[0065] Figure 7 This is a flow chart of the loop detection method provided by the present invention.

[0066] Figure 8 This is a flowchart of the optimized point cloud registration during loop closure detection provided by the present invention.

[0067] Figure 9 Flowchart of the method for global pose optimization processing provided by the present invention.

[0068] Figure 10 This is a schematic diagram for comparing the kitti00 trajectories provided by the present invention.

[0069] Figure 11 This is a schematic diagram of the actual test environment provided by the present invention.

[0070] Figure 12 A schematic diagram comparing the real-world mapping effects of entertainment facilities provided by the present invention, with the present invention on the left and LeGO-LOAM on the right.

[0071] Figure 13 This is the rendering of the community road scene provided by the present invention. DETAILED DESCRIPTION

[0072] It should be noted that, in the absence of conflict, the embodiments and features of the embodiments of the present invention may be combined with each other. The present invention will be described in detail below with reference to the accompanying drawings and in combination with the embodiments.

[0073] In order to enable those skilled in the art to better understand the solutions of the present invention, the technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the embodiments described are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative efforts should fall within the scope of protection of the present invention.

[0074] It should be noted that the terms "first," "second," and the like in the specification and claims of the present invention and the accompanying drawings are used to distinguish similar objects and are not necessarily used to describe a particular order or precedence. It should be understood that the terms used in this manner are interchangeable where appropriate for the embodiments of the present invention described herein. In addition, the terms "including," "having," and any variations thereof are intended to cover non-exclusive inclusions. For example, a process, method, system, product, or apparatus comprising a series of steps or units is not necessarily limited to those steps or units explicitly listed, but may include other steps or units that are not explicitly listed or that are inherent to these processes, methods, products, or apparatuses.

[0075] In this embodiment, a laser SLAM method based on ground segmentation is provided. Figure 1 FIG. 1 is a flow chart of a laser SLAM method based on ground segmentation according to an embodiment of the present invention. Figure 1 As shown, including:

[0076] S100, obtaining laser radar point cloud data information, and performing point cloud preprocessing on the laser radar point cloud data to obtain ground point cloud features and non-ground point cloud features, wherein the point cloud preprocessing at least includes ground segmentation processing;

[0077] In an embodiment of the present invention, the laser radar point cloud data may be filtered and then ground segmented to obtain ground point cloud features and non-ground point cloud features.

[0078] S200, performing feature extraction, feature matching, and pose estimation on the ground point cloud features and non-ground point cloud features, respectively, to obtain pose nonlinear optimization constraint results;

[0079] Specifically, edge features and plane feature points are extracted from the segmented point cloud, and the point cloud is divided into plane features and edge features by calculating the curvature of the original point cloud. Based on the extracted edge features and plane features, point-to-point and point-to-plane alignment is performed with the local map generated by the back-end, and the rotation and translation of the current frame are estimated. Finally, the optimal posture of the current frame is obtained through nonlinear optimization of the error function.

[0080] S300, selecting a key frame according to the pose nonlinear optimization constraint result, and constructing a local map according to the pose information of the current key frame to obtain a local map construction result;

[0081] In an embodiment of the present invention, the present invention selects key frames based on spatial distance and rotation angle changes, and constructs a local map based on the posture information of the key frames.

[0082] S400, performing loop detection based on the local map construction result to obtain a loop detection constraint result;

[0083] In an embodiment of the present invention, loop detection is performed based on the pose constraint information of the current key frame in the local map construction result to obtain the pose transformation result of the current key frame.

[0084] S500: Perform global pose optimization processing according to the loop detection constraint result and the local map construction result to obtain a global pose and a global map construction result.

[0085] Specifically, global pose optimization is performed based on loop closure constraints and local map construction results. By fusing loop closure data and laser odometry data, the accuracy of positioning and mapping is improved by minimizing the global error.

[0086] The laser SLAM method based on ground segmentation provided by the present invention performs point cloud segmentation and feature extraction and matching from laser radar data, and then performs global optimization and map correction through loop detection, thereby obtaining the construction results of global pose and global map. Therefore, this laser SLAM method based on ground segmentation of the present invention effectively improves the positioning accuracy, environmental adaptability and stability of laser SLAM during long-term operation by integrating advanced feature extraction, point cloud processing, factor graph optimization and loop detection technologies, ultimately achieving more efficient and reliable SLAM performance.

[0087] It should be noted that if Figure 2 As shown, the ground segmentation-based laser SLAM method of an embodiment of the present invention can specifically include two core processes, namely front-end matching and back-end optimization. The front-end matching is responsible for point cloud segmentation, feature extraction, and matching from the lidar data; the back-end optimization performs global optimization and map correction through factor graph optimization and loop detection.

[0088] As a specific implementation method of front-end matching, in an embodiment of the present invention, Figure 3 As shown, the laser radar point cloud data is pre-processed, including:

[0089] S110, performing filtering and denoising processing on the laser radar point cloud data to obtain filtered laser radar point cloud data;

[0090] Specifically, the lidar scanning data is effectively denoised through statistical outlier filtering and radius outlier filtering.

[0091] S120 , performing ground segmentation processing on the filtered lidar point cloud data to obtain ground point cloud features and non-ground point cloud features.

[0092] For the above-mentioned filtered and denoised lidar point cloud data, ground point cloud features and non-ground point cloud features are obtained by performing ground segmentation.

[0093] Specifically, ground segmentation processing is performed on the filtered lidar point cloud data to obtain ground point cloud features and non-ground point cloud features, including:

[0094] 1) performing concentric area division on the filtered lidar point cloud data according to a multi-layer annular area division method to obtain multiple annular areas, wherein the annular areas include a core area, a short-range area, a medium-range area, and a long-range area according to the distance from the lidar from near to far;

[0095] Specifically, the concentric regions are firstly divided for the filtered LiDAR point cloud data, wherein the concentric region division results are obtained based on the density, accuracy and ground characteristics of the point cloud data as the distance from the sensor increases. Figure 4a The different colored areas shown show the results of concentric area division, which is designed with non-uniform area sizes centered on the sensor. Figure 4b The traditional point cloud division method shown above usually uses a uniform grid. The concentric area division of the embodiment of the present invention can avoid the point cloud in the distant area being too sparse, thereby achieving accurate ground fitting. In addition, the grid in the near area of the embodiment of the present invention will not be too small, thereby being able to express the overall ground characteristics. Figure 5 As shown in the figure, the ground segmentation of the present invention adopts multi-layer annular area division, and divides the point cloud S into four main areas with the position of the lidar sensor as the center: the core area Z1, the area closest to the sensor; the close-range area Z2, the middle close-range area; the medium-range area Z3, the middle long-range area; and the long-range area Z4, the area farthest from the sensor.

[0096] 2) Each annular area is divided into sectors and a grid is designed to obtain multiple grid units;

[0097] Specifically, within each annular area, the sector area is subdivided by different azimuth angles N θ Set radial lines to further divide the ring into several sector-shaped areas. N of the core area and the distant area θThe large,sector-shaped areas are set relatively large to address the expressiveness and,sparsity issues, while the sector-shaped areas in the middle,area are smaller to provide finer ground representation.

[0098] In addition, the grid design process is specifically to set different area interval numbers N for different areas. r For the close-range area, fewer grid cells are set, and for the far-range area, more grid cells are set to maintain the geometric balance of each area. The final divided grid cell is recorded as S n .

[0099] Therefore, through this partitioning strategy, the embodiments of the present invention significantly reduce the total number of grid cells, thereby improving computational efficiency while maintaining accurate representation of ground morphology. Compared to traditional uniform grids, the embodiments of the present invention can better adapt to the variation of point cloud density with distance.

[0100] 3) For each grid cell, ground plane feature extraction and ground plane fitting are performed to obtain preliminary ground point screening results;

[0101] In the embodiment of the present invention, based on the concentric area division, regional ground plane extraction is performed on each grid unit.

[0102] The specific steps of regional ground plane extraction are as follows: (1) Determine the seed point. For each grid cell S n , select the lowest 20 points with the lowest height as the initial seed points. This is based on an assumption: in a local area, the point with the lowest height is most likely to belong to the ground. In order to prevent abnormal low point interference caused by problems such as signal reflection, the embodiment of the present invention introduces an adaptive seed selection strategy, especially for the core area, by setting a height threshold to filter out abnormal points that are significantly lower than the sensor (set according to the actual radar installation height). (2) Plane feature extraction. For each candidate ground point of the grid unit, the main direction and eigenvalue are calculated through feature analysis, and the normal vector and plane equation are extracted. It is defined here: is the covariance matrix of the point cloud, and the corresponding eigenvalue λ α and the eigenvector v α The calculation is as follows: Cv α =λ α v α , where α = 1, 2, 3. Assuming that the eigenvectors λ1 ≥ λ2 ≥ λ3, the eigenvector λ3 corresponding to the minimum eigenvalue can be expressed as the normal vector of the ground plane. This is because the distribution of point cloud data relative to the ground plane usually has the smallest deviation in the vertical direction. Here, we define n = v3 = [abc] T Represents the normal vector, and then the plane coefficient can be expressed like this Indicates that Sn The average position of the midpoint where the direction vector corresponding to the smallest eigenvalue is most likely to represent the normal vector to the ground plane.

[0103] After the ground plane feature extraction, the loop optimization is performed. After the initial ground plane is determined, the height margin h is set. m Estimate initial ground points in Represents the mean height of the seed points initially selected. Through multiple cycles of optimization, the ground point set is continuously adjusted. In each cycle, the distance from each point to the current estimated plane is calculated, and the plane numerical difference M is set. d , points that meet the distance conditions are classified into a new ground point set:

[0104]

[0105] in, Represents the plane coefficient fitted at each point.

[0106] Through this regional processing, the embodiment of the present invention can adapt to terrain changes in different regions and effectively solve the complex terrain problem that is difficult to deal with by global plane fitting.

[0107] 4) Performing ground feature evaluation on the preliminary screening results of the ground points to obtain ground point cloud features and non-ground point cloud features.

[0108] In the embodiment of the present invention, based on the preliminary ground point screening by using the feature analysis method (PCA), in order to effectively process a large amount of noise and outlier data, so as to accurately extract ground information and obtain a more ideal segmentation result, the embodiment of the present invention is to select the ground points extracted in the previous step. Refine the verticality and height. The verticality can be solved by the feature analysis method in the previous step. The included angle of the vector perpendicular to the ground is calculated. If it is less than the set threshold, it can be considered to pass the verticality. This is because if Most of the points do belong to the ground, so this S n The v3 in the image will tend to be perpendicular to the ground plane. Verticality alone is not enough. For example, when there is a car in front of the radar, the point cloud data of the roof may pass the test in terms of verticality. However, these point clouds are obviously not ground points, so they need to be refined according to the height. When there is a slope far away from the radar, the points in this area are not perpendicular to the ground plane. The height may also be higher, so according to the four spaces Z divided by the concentric areas in the first step, different height thresholds h are given thresholdFor example, the threshold of Z1 is set to the radar installation height, and the threshold of Z4 is set to a larger value (changed according to the actual test environment), which satisfies the requirement of less than the height threshold. Continue to keep it.

[0109] In the embodiment of the present invention, feature extraction, feature matching and pose estimation are performed on the ground point cloud features and non-ground point cloud features respectively to obtain pose nonlinear optimization constraint results, such as Figure 6 As shown, including:

[0110] S210, performing plane feature extraction on the ground point cloud feature to obtain a first plane feature, and performing edge feature and ground feature extraction on the non-ground point cloud feature to obtain an edge feature and a second plane feature;

[0111] Specifically, edge features and plane feature points are extracted from the segmented point cloud, and the point cloud is divided into plane features and edge features by calculating the curvature of the original point cloud. The curvature formula is calculated as follows:

[0112]

[0113] Simplified expression: where X i represents the current point, c represents X i The curvature value of X i The coordinates of adjacent points on the same radar scan line as the center and n represents the selected neighborhood radius.

[0114] In this embodiment of the present invention, feature extraction specifically targets non-ground points after ground segmentation to obtain both planar and edge features, while only planar features are extracted for ground points. This feature extraction approach takes into account the generally high flatness and uniformity of ground points, so extracting planar features alone provides sufficient information to describe ground structure while avoiding excessive computation. Because non-ground points contain more complex structures, extracting edge features helps better capture this geometric information. This is particularly true in complex or dynamic environments, where edge features contribute to increased system stability and robustness.

[0115] S220, performing point-to-plane registration on the first plane feature and the second plane feature to obtain a point-to-plane registration result, and performing point-to-line registration on the edge feature to obtain a point-to-line registration result;

[0116] Specifically, based on the extracted edge features and plane features, point-to-point and point-to-plane alignment is performed with the local map generated by the back-end, and the rotation and translation of the current frame are estimated. Finally, the optimal posture of the current frame is obtained through a nonlinear optimization error function.

[0117] As a specific implementation manner, performing point-to-plane registration on the first plane feature and the second plane feature to obtain a point-to-plane registration result includes:

[0118] 1) selecting a point in the first plane feature as a first plane feature point, and selecting a point in the second plane feature as a second plane feature point;

[0119] 2) constructing point-to-plane residuals between the first plane feature point and the second plane feature point and a preset plane;

[0120] 3) Obtaining a point-to-surface registration result for constraining the roll angle rotation, pitch angle rotation, and z-direction translation of the laser radar based on the point-to-surface residual.

[0121] Specifically, we focus on the plane features extracted from the point cloud and estimate the motion of the sensor by establishing a point-to-plane correspondence. Assume that point Q represents a plane feature point in the current frame, and after converting it to the historical map coordinate system, it is recorded as Q′. The plane is represented by the normal vector and a point C on the plane, then the residual equation from point to surface is: Similarly, expand to the pose transformation level: Point-to-plane registration is mainly used to constrain roll and pitch rotations as well as z-direction translation.

[0122] As another specific implementation, performing point-line registration on the edge feature to obtain a point-line registration result includes:

[0123] 1) Selecting a point in the edge feature as an edge feature point;

[0124] 2) constructing point-to-line residuals from the edge feature points and the preset line features;

[0125] 3) Obtaining a point-line registration result for constraining the yaw angle, x-direction translation, and y-direction translation of the laser radar based on the point-to-line residual.

[0126] In this embodiment of the present invention, point-line registration primarily targets edge features extracted from the point cloud, estimating sensor motion by establishing point-to-line correspondences. Assuming point P is an edge feature point in the current frame, converted to the historical map coordinate system and denoted as P′, and the line feature is represented by two points A and B, the point-to-line residual equation is: × represents the cross product, |·| represents the modulus, and the result is expanded to the pose transformation level:

[0127]

[0128] In the embodiment of the present invention, point-line registration is mainly used to constrain yaw angle rotation and translation in the x and y directions.

[0129] S230, performing posture constraint processing according to the point-plane registration result and the point-line registration result to obtain a posture constraint result;

[0130] S240: Perform nonlinear optimization processing on the posture constraint result to obtain a posture nonlinear optimization constraint result.

[0131] Specifically, in the nonlinear optimization stage, the error function is minimized: Solve to get the pose of the current frame.

[0132] In an embodiment of the present invention, the specific local map construction process is as follows: when no loop detection occurs, the present invention selects key frames based on changes in spatial distance and rotation angle. Specifically, relevant thresholds can be set. When the moving distance exceeds the threshold (such as 0.2m) or the rotation angle exceeds the threshold (such as 5 degrees), it is selected as a key frame, and a sliding window method is used to retain N key frames to form a local map. Before the point cloud data of each key frame is added to the local map, it needs to be converted from its local coordinate system {B} to the global map coordinate system {W}. This ensures that all point clouds are correctly aligned in the same reference coordinate system. When loop detection occurs, the loop frame is added as a key frame, and the poses of all key frames of the current local map are updated through the global pose optimization results.

[0133] In an embodiment of the present invention, loop detection is performed based on the local map construction result to obtain loop detection constraint results, such as Figure 7 As shown, including:

[0134] S410, identifying loop candidate frames according to the current key frame in the local map construction result;

[0135] Specifically, a radius search is used to determine whether a loop has occurred, that is, whether the current frame overlaps with a historical frame. First, the radius search parameters are set, including the search radius (e.g., 15m) and the time threshold (to exclude multiple recently added keyframes). Then, the poses of the historical keyframes are constructed into a KD tree and a nearest neighbor search is performed to find potential loop candidates.

[0136] S420, extracting and matching FPFH features based on the current key frame and the loop candidate frame to obtain an initial matching result;

[0137] Specific as Figure 8As shown in the figure, the current keyframe is used as the source point cloud, and the loop closure candidate frame is used as the target point cloud. This step first downsamples the source and target point clouds (by default, 0.3m) to reduce computational complexity. Normals are then calculated for the downsampled point clouds, and the FPFH descriptors are calculated using the estimated normals. The FPFH descriptors of the target point cloud are constructed into a KD tree, and the source point cloud FPFH descriptors are then used to query this KD tree to find the nearest neighbors.

[0138] S430, trimming abnormal points in the initial matching result according to the point cloud registration residual to obtain a final matching result;

[0139] Specifically, dynamic objects and noise can lead to mismatching. For the correspondence obtained by the above FPFH detection, the minimum consensus iterative sampling method is applied to define the residual function of point cloud registration as:

[0140] e i =p i -(Rq i +t) 2 ,

[0141] Among them, p i and q i Represent the corresponding points in the candidate loop frame point cloud and the current frame point cloud respectively. The goal is to filter out abnormal points whose residual exceeds the threshold τ by maximizing the consistency of the internal points: I inlier ={e i <τ}.

[0142] More specifically, the abnormal points in the initial matching result are trimmed according to the point cloud registration residual to obtain the final matching result, including:

[0143] 1) randomly selecting a minimum subset based on the initial matching result to calculate the initial transformation;

[0144] Specifically, a minimum subset (e.g., 3 pairs of points) is randomly selected from the matching point pairs and the initial transformation is calculated.

[0145] 2) calculating the residuals of all point pairs in the initial matching results according to the initial transformation, and counting the number of inliers that meet the preset conditions;

[0146] Specifically, the initial transformation is used to calculate the residuals of all point pairs, and the statistics satisfy e i < the number of interior points N of τ inlier .

[0147] 3) Repeat the above process until the initial transformation with the largest number of retained inliers is obtained;

[0148] Repeat the above for K times and keep the transformation with the largest number of interior points (R * ,t *).

[0149] 4) The number of interior points is formed into an optimal interior point set, and the pose is optimized according to the weighted least squares method to obtain the final matching result.

[0150] Based on the optimal interior point set I inlier , and the weighted least squares method is used to optimize the pose.

[0151] S440: Perform rotation estimation and component translation estimation on the final matching result to obtain a loop detection constraint result.

[0152] In this embodiment of the present invention, a graduated non-convexity (GNC) strategy is adopted to decompose the rotation estimation into a multi-stage convex optimization problem, gradually approaching the global optimal solution. The rotation error function is defined as:

[0153]

[0154] Among them, ρ(·) is the GNC robust kernel function, which is expressed as:

[0155]

[0156] The parameters μ and λ control the degree of non-convexity and are dynamically adjusted with the number of iterations k:

[0157] μ (k) =μ0·γ k ,λ (k) =λ0·γ k .

[0158] By gradually tightening the threshold, the algorithm transitions from a loose convex approximation to a precise non-convex optimization, effectively avoiding local minima.

[0159] Based on the rotation estimation, the component-wise translation estimation (COTE) is used to estimate the translation vector t = [t x ,t y ,t z ] T Considering the difference in measurement accuracy of lidar in different directions (for example, the vertical direction accuracy is usually lower than the horizontal direction), the covariance matrix Σ is introduced. t Weight the translational component:

[0160]

[0161] Finally, the pose transformation between the loop detection module loop frame and the current frame is obtained.

[0162] In an embodiment of the present invention, a global pose optimization process is performed based on the loop detection constraint result and the local map construction result to obtain a global pose and a global map construction result, such as Figure 9 As shown, including:

[0163] S510, constructing a factor graph according to the loop detection constraint result and the pose constraint result of the current key frame in the local map construction result to obtain a factor graph construction result;

[0164] Specifically, the present invention uses a factor graph optimization framework to construct a factor graph that globally optimizes the odometry constraints (the pose estimation residuals from point cloud registration) and the loop detection constraints obtained by the loop detection module (the pose estimation residuals between the looped frame and the current frame). Factor graph optimization improves the accuracy of localization and mapping by minimizing the global error by fusing loop detection data with laser odometry data.

[0165] More specifically, constructing a factor graph according to the loop detection constraint result and the pose constraint result of the current key frame in the local map construction result to obtain a factor graph construction result includes:

[0166] 1) determining a loop detection factor according to the pose estimation residual between the loop candidate frame and the current key frame in the loop detection constraint result, wherein the optimization constraint expression of the loop detection factor is:

[0167]

[0168] Among them, e l represents the loop detection factor, and Both represent the pose of the associated frame of loop closure detection;

[0169] In the embodiment of the present invention, historical key frames are detected by feature point or point cloud matching, and global optimization constraints are calculated.

[0170] 2) Determine a lidar factor based on the pose estimation residual of the current key frame in the local map construction result, wherein the optimization constraint of the lidar factor is expressed as:

[0171]

[0172] Among them, e y represents the lidar factor, ΔT ij Represents the relative transformation obtained by point cloud matching, T i w and All represent keyframe lidar poses;

[0173] In an embodiment of the present invention, the inter-frame pose is calculated by point cloud matching to obtain an expression of the optimization constraint of the above-mentioned lidar factor.

[0174] 3) Constructing a factor graph based on the loop detection factor and the lidar factor, wherein the optimization objective function of the factor graph is expressed as:

[0175]

[0176] Among them, the error term corresponds to the measurement constraints of the odometry and loop closure detection.

[0177] S520: Perform factor graph optimization on the factor graph construction result to obtain a construction result of a global pose and a global map.

[0178] In the embodiment of the present invention, the final global pose optimization is the implementation process of factor graph optimization. Factor graph optimization is a nonlinear least squares problem, so it needs to be linearized. The core is to perform Taylor expansion on the error function of each factor to obtain a first-order approximation. This step is achieved by calculating the Jacobian matrix of the error function. To achieve this, the linear system is solved by the optimization method in the gtsam library to obtain the increment and finally the state variables are updated to obtain the final global pose and map.

[0179] The present invention compares the widely used A-LOAM and LeGO-LOAM in the prior art, and evaluates the performance differences of the three SLAM methods through qualitative and quantitative analysis.

[0180] This paper uses the KITTI dataset 00-10 sequence for experimental verification, and the results are as follows Figure 10 and as shown in Table 1.

[0181] Table 1 KITTI sequence ATE comparison (units m and rad)

[0182]

[0183] Table 1 shows the absolute trajectory error (ATE) comparison results of the embodiment of the present invention, A-LOAM, and LeGO-LOAM on the 00-10 sequence of the KITTI dataset. As can be seen from Table 1, the embodiment of the present invention achieves optimal performance on most sequences, with an average ATE of 3.299 m and 3.129 rad, significantly outperforming A-LOAM's 7.309 / 2.086 and LeGO-LOAM's 19.163 / 63.111. In particular, on the 00 sequence, the ATE of the embodiment of the present invention is only 3.299, while A-LOAM and LeGO-LOAM have 7.309 and 19.163, respectively, demonstrating a significant advantage.

[0184] Figure 10 The trajectory comparison results of the KITTI 00 sequence are shown. From the visualization results, it can be seen that the trajectory generated by the embodiment of the present invention ( Figure 10 middle red line) and Ground Truth trajectory ( Figure 10 The match of the trajectory (the middle dashed line) is closer, especially in complex terrain and turning parts, with smaller errors, reflecting higher precision. Compared with the LeGO-LOAM trajectory, it can effectively identify the loop and correct the global pose.

[0185] In addition, the ground segmentation method of the embodiment of the present invention reduces noise and improves the quality of subsequent feature extraction by more accurately distinguishing ground points and non-ground points, providing more reliable data support for the traditional point cloud registration (ICP) of the subsequent odometer and the optimized point cloud registration during loop detection. Combined with the optimized point cloud registration during loop detection, the present invention can correct the accumulated error and optimize the global trajectory, ensuring the stability and accuracy of SLAM during long-term operation. Therefore, the laser SLAM method based on ground segmentation in the embodiment of the present invention significantly improves the performance of the global map, especially the adaptability in complex environments, showing obvious advantages.

[0186] The effect of the laser SLAM method based on ground segmentation in the embodiment of the present invention is further explained below in combination with an actual car real environment test.

[0187] Specifically, if Figure 11 As shown, the actual environment is carried out on an Ackerman car equipped with a Velodyne VLP-16 lidar sensor and a Jetson orin nx computing unit with a 6-core Arm Cortex-A78AE CPU and 8GB of memory.

[0188] Compared with the LeGO-LOAM algorithm which also has the ground segmentation function, the results are as follows Figure 12 shown. Figure 12 (top left) and Figure 12 (Upper right) shows the overall mapping effects of the present invention and LeGO-LOAM respectively. Through comparison, it can be clearly observed that when processing scenes containing complex elements such as curbs, trees and entertainment facilities, the point cloud map generated by the present invention presents a more coherent structure and a clearer environmental outline. Especially in terms of edge expression and road continuity in dense vegetation areas, the present invention shows obvious advantages, and the spatial layering of the overall environment is stronger. Further analysis Figure 12 (lower left) and Figure 12(Bottom right) In areas with high point cloud density, such as those around entertainment facilities, the proposed method presents continuous, smooth lines and precise, clear boundaries, preserving structural details more completely. This difference is primarily due to the proposed method's optimized ground segmentation algorithm, which effectively filters out noise points while preserving key environmental features. This allows subsequent feature matching and point cloud registration processes to be based on more reliable feature points, thereby improving the accuracy and consistency of the overall map.

[0189] For the complex road environment inside the community, this invention shows good mapping effect on the outdoor environment data collected by the mobile robot experimental platform, such as Figure 13 As shown in Figure 2, the fine detail structure of the environment is successfully reconstructed, and the outlines of buildings and the distribution of dense vegetation can be clearly distinguished ( Figure 13 The green portion of the image (center) demonstrates the robust performance of our feature matching algorithm when handling diverse environmental features. The continuity of roads across the entire map also demonstrates robust registration across multiple scans, maintaining consistency even in areas with repeated or similar features. This demonstrates the superiority of our feature matching algorithm in handling challenging scenarios. Figure 13 In the enlarged image in the upper right corner, the outlines of multiple cars can be clearly observed, and the structural details of the cars, including the body outline and position, are successfully captured, which shows that the present invention also has good capabilities in identifying and reconstructing objects in the environment. Figure 13 The enlarged portion in the lower right corner clearly demonstrates the ability to preserve environmental features, particularly in areas densely populated with trees and shrubs, accurately distinguishing the position and shape of individual trees. Compared to existing SLAM methods, which often suffer from blurred or distorted mapping due to difficulties in feature extraction, the present invention can accurately distinguish the position and shape of individual trees, demonstrating its adaptability when handling objects with varying morphologies.

[0190] It will be understood that the above embodiments are merely exemplary embodiments for illustrating the principles of the present invention, and the present invention is not limited thereto. Those skilled in the art will appreciate that various modifications and improvements can be made without departing from the spirit and substance of the present invention, and such modifications and improvements are also considered to be within the scope of protection of the present invention.

Claims

1. A laser SLAM method based on ground segmentation, characterized in that: include: Acquire laser radar point cloud data information, and perform point cloud preprocessing on the laser radar point cloud data to obtain ground point cloud features and non-ground point cloud features, wherein the point cloud preprocessing at least includes ground segmentation processing; Perform feature extraction, feature matching, and pose estimation on the ground point cloud features and the non-ground point cloud features, respectively, to obtain pose nonlinear optimization constraint results; Selecting a key frame according to the pose nonlinear optimization constraint result, and constructing a local map according to the pose information of the current key frame to obtain a local map construction result; Perform loop closure detection based on the local map construction result to obtain a loop closure detection constraint result; A global pose optimization process is performed based on the loop detection constraint result and the local map construction result to obtain a global pose and a global map construction result.

2. The laser SLAM method based on ground segmentation according to claim 1, characterized in that Performing point cloud preprocessing on the laser radar point cloud data includes: Performing filtering and denoising on the laser radar point cloud data to obtain filtered laser radar point cloud data; The filtered lidar point cloud data is subjected to ground segmentation processing to obtain ground point cloud features and non-ground point cloud features.

3. The laser SLAM method based on ground segmentation according to claim 2, characterized in that Performing ground segmentation processing on the filtered lidar point cloud data to obtain ground point cloud features and non-ground point cloud features, including: Divide the filtered lidar point cloud data into concentric areas according to a multi-layer annular area division method to obtain multiple annular areas, wherein the annular areas include a core area, a short-range area, a medium-range area, and a long-range area according to the distance from the lidar from near to far; Each annular area is divided into sectors and a grid is designed to obtain multiple grid units; For each grid cell, ground plane feature extraction and ground plane fitting are performed to obtain preliminary screening results of ground points; A ground feature evaluation is performed on the preliminary screening results of the ground points to obtain ground point cloud features and non-ground point cloud features.

4. The laser SLAM method based on ground segmentation according to claim 1, characterized in that Feature extraction, feature matching and pose estimation are performed on the ground point cloud features and non-ground point cloud features respectively to obtain pose nonlinear optimization constraint results, including: Performing plane feature extraction on the ground point cloud feature to obtain a first plane feature, and performing edge feature and ground feature extraction on the non-ground point cloud feature to obtain an edge feature and a second plane feature; Performing point-to-plane registration on the first plane feature and the second plane feature to obtain a point-to-plane registration result, and performing point-to-line registration on the edge feature to obtain a point-to-line registration result; Performing posture constraint processing according to the point-plane registration result and the point-line registration result to obtain a posture constraint result; The posture constraint result is subjected to nonlinear optimization processing to obtain a posture nonlinear optimization constraint result.

5. The laser SLAM method based on ground segmentation according to claim 4, characterized in that: Performing point-to-plane registration on the first plane feature and the second plane feature to obtain a point-to-plane registration result includes: Selecting a point in the first plane feature as a first plane feature point, and selecting a point in the second plane feature as a second plane feature point; Constructing point-to-plane residuals from the first plane feature point and the second plane feature point to a preset plane respectively; A point-to-surface registration result for constraining the roll angle rotation, pitch angle rotation, and z-direction translation of the laser radar is obtained according to the point-to-surface residual.

6. The laser SLAM method based on ground segmentation according to claim 4, characterized in that: Performing point-line registration on the edge feature to obtain a point-line registration result includes: Selecting a point in the edge feature as an edge feature point; Constructing point-to-line residuals from the edge feature points and the preset line features; A point-line registration result for constraining the yaw angle, x-direction translation, and y-direction translation of the laser radar is obtained according to the point-to-line residual.

7. The laser SLAM method based on ground segmentation according to claim 1, characterized in that: Performing loop closure detection based on the local map construction result to obtain loop closure detection constraint results, including: Identify loop candidate frames based on the current key frame in the local map construction result; Perform FPFH feature extraction and matching based on the current key frame and the loop candidate frame to obtain the initial matching result; Pruning abnormal points in the initial matching result according to the point cloud registration residual to obtain a final matching result; Rotation estimation and component translation estimation are performed on the final matching result to obtain a loop detection constraint result.

8. The laser SLAM method based on ground segmentation according to claim 7, characterized in that: The abnormal points in the initial matching result are trimmed according to the point cloud registration residual to obtain the final matching result, including: Randomly select a minimum subset based on the initial matching result to calculate the initial transformation; Calculate the residuals of all point pairs in the initial matching result according to the initial transformation, and count the number of inliers that meet the preset conditions; Repeat the above process until the initial transformation with the largest number of retained inliers is obtained; The number of inliers is formed into an optimal inlier set, and the pose is optimized according to the weighted least squares method to obtain the final matching result.

9. The laser SLAM method based on ground segmentation according to claim 1, characterized in that: Performing global pose optimization processing according to the loop detection constraint result and the local map construction result to obtain a global pose and a global map construction result, including: Constructing a factor graph based on the loop detection constraint result and the pose constraint result of the current key frame in the local map construction result to obtain a factor graph construction result; Factor graph optimization is performed on the factor graph construction result to obtain construction results of the global pose and the global map.

10. The laser SLAM method based on ground segmentation according to claim 9, characterized in that: A factor graph is constructed according to the loop detection constraint result and the pose constraint result of the current key frame in the local map construction result to obtain a factor graph construction result, including: The loop detection factor is determined according to the pose estimation residual between the loop candidate frame and the current key frame in the loop detection constraint result, wherein the optimization constraint expression of the loop detection factor is: Among them, e l represents the loop detection factor, and Both represent the pose of the associated frame of loop closure detection; The lidar factor is determined according to the pose estimation residual of the current key frame in the local map construction result, wherein the optimization constraint expression of the lidar factor is: Among them, e y represents the lidar factor, ΔT ij Represents the relative transformation obtained by point cloud matching, T i w and All represent keyframe lidar poses; A factor graph is constructed according to the loop detection factor and the lidar factor, wherein the optimization objective function of the factor graph is expressed as:

Images