A robust laser slam relocalization and mapping method for embodied intelligent robots

Abstract

The application provides a kind of robust laser SLAM repositioning and mapping method for embodied intelligent robot in the technical field of robot autonomous navigation, comprising: receiving original laser radar point cloud and IMU data, pre-processing point cloud to extract feature points;IMU pre-integration provides initial pose constraint;Loop closure detection is through the two-stage cascade of ScanContext global search and LinK3D local matching, obtains repositioning constraint;Front-end odometry matches feature points with local voxel map, combines IMU optimization to output pose estimation, filters key frame to generate laser odometry factor;With repositioning constraint as initial value, use improved ICP to accurately register to obtain repositioning factor;Finally, construct factor graph, jointly optimize IMU, laser odometry and repositioning factor, solve global consistent pose sequence and map.The application has the advantages that: it greatly improves the system robustness, repositioning accuracy and global map consistency of quadruped robot in severe motion and large-scale scene, while meeting the real-time constraint of embedded platform.

Description

Technical Field

[0001] This invention relates to the field of robot autonomous navigation technology, and in particular to a robust laser SLAM relocalization and mapping method for embodied intelligent robots. Background Technology

[0002] Simultaneous Localization and Mapping (SLAM) technology is the core of mobile robots' autonomous navigation and environmental perception. In recent years, the Lidar Inertial Odometry and Mapping (LIO-SAM) scheme, which integrates Lidar and Inertial Measurement Unit (IMU), has demonstrated superior performance in complex environments due to its ability to effectively utilize the high-precision ranging information of Lidar and the short-term motion prediction capability of IMU. Among these, the LIO-SAM framework, by combining Lidar odometry, IMU pre-integration, and factor graph optimization, achieves high-precision, real-time state estimation and mapping, and has become one of the mainstream solutions in this field.

[0003] However, when such advanced laser SLAM systems are applied to quadruped robot platforms (such as "robot dogs"), they face a series of severe challenges in long-distance, large-scale, and complex environmental tasks, especially in relocalization and global localization. Figure 1 There are obvious limitations in terms of consistency, specifically in the following aspects: 1. Drastic Movements and Perspective Changes Lead to Feature Matching Degradation: During movement, the torso of a quadruped robot often undergoes dramatic pitching and rolling motions. This results in significant differences and distortions in the perspective of the point cloud data acquired by the LiDAR, making traditional Iterative Closest Point (ICP) matching methods based on local geometric features (such as edge points and planar points) extremely sensitive to initial values. In scenarios with sparse features or repetitive structures, these methods are prone to getting stuck in local optima, failing to match, or even generating incorrect pose estimations, severely impacting the robustness of the front-end odometry.

[0004] 2. The Challenges of Accumulated Errors and Global Consistency: During long-distance operation, the zero-bias error of IMU pre-integration and the matching error of laser odometry accumulate over time, leading to a significant drift in pose estimation. Although frameworks such as LIO-SAM introduce backend optimization based on loop closure detection to correct this drift, existing methods have shortcomings in the loop closure detection module. Traditional loop closure detection relies heavily on local point cloud feature descriptors and spatial neighborhood search, lacking efficient global scene representation capabilities and fast retrieval mechanisms. This makes it difficult for the system to achieve effective long-distance loop closure recognition in large-scale environments, failing to provide sufficient and accurate global constraints for backend optimization, thus making it difficult to suppress accumulated drift over long periods and affecting the global consistency of the final map.

[0005] 3. Performance conflict between existing algorithms and embedded platforms: To improve the accuracy of loop closure detection, some studies have introduced more discriminative local or global descriptors. However, these algorithms often have high computational complexity and require significant computing resources. Quadruped robots, on the other hand, are typically equipped with embedded computing platforms with limited computing and storage resources. Therefore, how to significantly reduce algorithm overhead while ensuring relocation accuracy and robustness, and thus meet the real-time requirements of embedded platforms, is a critical problem that urgently needs to be solved for engineering implementation.

[0006] In summary, existing SLAM methods based on laser-inertial fusion, when directly applied to quadruped robots, struggle to simultaneously ensure the robustness of the front-end odometry, the accuracy and efficiency of loop closure detection, and the global consistency of the back-end mapping in situations involving drastic posture changes, long-distance operation, large-scale scenes, and resource-constrained embedded platforms.

[0007] Therefore, how to provide a robust laser SLAM relocalization and mapping method for embodied intelligent robots to improve the system robustness, relocalization accuracy, and global mapping of quadruped robots in violent motion and large-scale scenes? Figure 1 Achieving consistency while meeting the real-time constraints of embedded platforms has become a pressing technical problem that needs to be solved. Summary of the Invention

[0008] The technical problem this invention aims to solve is to provide a robust laser SLAM relocalization and mapping method for embodied intelligent robots, thereby improving the system robustness, relocalization accuracy, and global mapping of quadruped robots in violent motion and large-scale scenes. Figure 1 It achieves consistency while meeting the real-time constraints of the embedded platform.

[0009] This invention is implemented as follows: A robust laser SLAM relocalization and mapping method for embodied intelligent robots, comprising the following steps: Step S1: Receive raw lidar point cloud data and IMU data from the quadruped robot; Step S2: Perform preprocessing on the original lidar point cloud data, including at least motion distortion removal, ground point segmentation, and voxel downsampling, to obtain basic point cloud data; Step S3: Extract feature points from the basic point cloud data and send the feature points to the front-end laser odometry module and loop closure detection module respectively; at the same time, perform pre-integration on the IMU data to obtain pre-integration results, and send the pre-integration results to the front-end laser odometry module to provide initial pose constraints for laser point cloud matching; simultaneously, generate IMU pre-integration factors based on the pre-integration results. Step S4: In the loop closure detection module, based on the feature points extracted from the current frame, a two-stage cascaded matching is performed, including fast retrieval of ScanContext global descriptors and precise matching of lightweight LinK3D local descriptors, to obtain high-precision relocation pose constraints. Step S5: In the front-end laser odometry module, the feature points of the current frame are matched with the maintained local voxel map, and the matching results are iteratively optimized in combination with the pre-integration results to output high-frequency laser odometry pose estimation results. Historical key frames are selected according to preset conditions and sent to the loop closure detection module for global retrieval to obtain loop closure candidate frames. At the same time, laser odometry factors are generated based on the laser odometry pose estimation results. Step S6: Using the relocation constraint pose as the initial value for the improved ICP registration, perform fine registration between the current frame and the loop closure candidate frame to obtain the relocation constraint factor; wherein, the loss function of the ICP registration introduces an adaptive weight term based on the point-pair registration residual. Step S7: Construct a factor graph by adding the IMU pre-integration factor, laser odometry factor, and relocation constraint factor to the factor graph for joint optimization, and solve to obtain a globally consistent quadruped robot pose sequence and map.

[0010] Furthermore, in step S2, the preprocessing further includes: The original lidar point cloud data is subjected to neighborhood smoothness calculation based on scan lines, and candidate points for subsequent feature extraction are selected according to the set smoothness threshold as the basic point cloud data.

[0011] Furthermore, in step S3, the extracted feature points include edge feature points and planar feature points.

[0012] Furthermore, step S4 specifically includes: Step S41: In the loop closure detection module, a ScanContext global descriptor is generated based on the feature points extracted from the current frame, and a rotation-invariant Ring Key vector is extracted from it. Step S42: Use the Ring Key vector to quickly search in the constructed historical keyframe KD-Tree to obtain the Top-K candidate historical keyframes with similar global positions; Step S43: For each candidate historical keyframe, calculate the column distance between its ScanContext global descriptor and the ScanContext global descriptor of the current frame, and compensate for the orientation difference through a cyclic shift operation to filter out the candidate historical keyframes that meet the similarity threshold. Step S44: Extract lightweight improved LinK3D key points and their lightweight LinK3D local descriptors from the current frame and the filtered candidate historical key frames. Step S45: Based on the distance of the lightweight LinK3D local descriptor, perform feature matching on the LinK3D key points of the current frame and each candidate frame, and use the RANSAC algorithm to remove incorrect matching pairs. Step S46: For the set of matching point pairs filtered by the RANSAC algorithm, calculate the initial relative pose transformation between the feature points of the two frames through singular value decomposition to obtain high-precision relocation pose constraints.

[0013] Furthermore, step S41 specifically includes: In the loop closure detection module, the feature points extracted from the current frame are divided into a grid in polar coordinates, which is divided into Nr rings radially and Ns sectors azimuthally, forming an NrxNs two-dimensional matrix. For each grid cell in the two-dimensional matrix, the maximum height value of all points within it is taken as the encoding value of the grid cell. If the grid cell is empty, the encoding value is zero, thereby generating a ScanContext descriptor matrix containing the ScanContext global descriptor. For each row of the ScanContext descriptor matrix, count the number of non-zero encoded values ​​and divide by the total number of sectors Ns to obtain the density value of the ring. The density values ​​of the Nr rings are concatenated sequentially to form an Nr-dimensional Ring Key vector.

[0014] Furthermore, step S44 specifically includes: Step S441: Extract edge points from the feature points based on local surface smoothness; Step S442: Divide the XoY plane of the LiDAR into N sectors, where N is less than the number of sectors in the original LinK3D, and cluster the extracted edge points into the corresponding sectors according to their azimuth angles. Step S443: For each non-empty sector, calculate the centroid of all edge points within it, and use the centroid as a LinK3D key point; Step S444: Using the currently described aggregated key point as the center, calculate the vector between it and the nearest neighbor aggregated key point, and use it as the main direction; Step S445: For the other aggregated key points, calculate their angle relative to the main direction, and classify them into the corresponding dimension of the preset M-dimensional descriptor according to the angle, where M is less than the descriptor dimension of the original LinK3D. Step S446: For each occupied descriptor dimension, assign its value to the Euclidean distance between the LinK3D keypoint to be described and the nearest neighbor LinK3D keypoint in that dimension, and assign zero to unoccupied dimensions, thereby generating an M-dimensional lightweight LinK3D local descriptor.

[0015] Furthermore, in step S442, the value of N is 60; In step S445, the value of M is 128.

[0016] Furthermore, in step S5, the step of filtering historical keyframes according to preset conditions specifically involves: Based on the preset conditions that the pose change between the current frame and the previous historical key frame exceeds a set translation threshold or rotation threshold, or the time interval from the previous historical key frame exceeds a set interval threshold, historical key frames are selected from the laser odometry pose estimation results. The constructed historical keyframe KD-Tree is updated based on the selected historical keyframes.

[0017] Furthermore, in step S6, the formula for the loss function is: ; in, and Let R represent a pair of matched feature points; R and t represent the rotation matrix and translation vector to be solved; m represents the index of the matched feature pair. The adaptive weight term for the m-th pair of matching feature points is calculated using the following formula: ; in, σ represents the registration residual of the m-th pair of matched feature points; σ represents the preset noise standard deviation parameter.

[0018] Furthermore, in step S7, the construction of the factor graph also includes: The GPS observation factor is added to the factor graph and jointly optimized with the IMU pre-integration factor, laser odometry factor, and relocation constraint factor.

[0019] The advantages of this invention are: 1. Under intense motion conditions, IMU pre-integration is used to compensate for point cloud motion distortion and provide high-quality initial values ​​for laser matching. Combined with local map matching, the robustness of the front-end odometry is enhanced. In large-scale scenes, the ScanContext global descriptor is used for fast scene retrieval, followed by precise matching using a lightweight LinK3D local descriptor. Improved ICP with adaptive weights is introduced for fine registration, thereby obtaining high-precision relocalization constraints. These constraints, along with IMU and laser odometry factors, are then added to a factor map for joint optimization, effectively correcting long-distance cumulative drift and ensuring global accuracy. Figure 1 Consistency; To meet the real-time constraints of the embedded platform, a keyframe management strategy is adopted to control the data scale throughout the process, and the computation and storage overhead of the algorithm is significantly reduced through a cascaded process of "fast global retrieval + precise local matching" and lightweight design such as reducing the descriptor dimension.

[0020] 2. A two-stage back-linked detection mechanism that balances efficiency and accuracy significantly improves the robustness and success rate of relocalization: Fast global descriptor retrieval via ScanContext is combined with precise matching using lightweight LinK3D local descriptors. First, a fast global coarse search is performed in the KD-Tree using ScanContext and its derived RingKey vectors, efficiently filtering out a small number of candidate frames from a massive amount of historical keyframes, significantly reducing the search space and ensuring real-time performance. Second, the candidate frames are further refined using the improved lightweight LinK3D local descriptors for fine feature matching, and optimized using the RANSAC algorithm to obtain high-precision relative pose transformations. This "coarse-to-fine" strategy effectively solves the problems of slow retrieval speed, susceptibility to interference, or insufficient matching accuracy of traditional single methods in large, complex, or dynamic environments, making the relocalization process both fast and reliable, which is key to the system's robustness.

[0021] 3. Targeted lightweight improvements to the local descriptor LinK3D to effectively balance computational overhead and descriptive capabilities: The original LinK3D has been "lightweighted" by reducing the number of sector partitions (e.g., N=60) and the descriptor dimension (e.g., M=128). While maintaining its core advantage of using neighborhood spatial structure relationships for description, this improvement significantly reduces the computational complexity and memory consumption during feature extraction and descriptor matching. It can better adapt to the limited computing resources of mobile platforms such as quadruped robots, ensuring that the whole system can run in real time without sacrificing necessary feature discrimination, thus enhancing the practicality and deployment feasibility of the technical solution.

[0022] 4. An adaptive ICP loss function is introduced in the loop closure fine registration stage to improve the accuracy and stability of registration in complex point clouds: In the loss function of the standard Iterative Closest Point (ICP) algorithm, an adaptive weight term based on the registration residual of point pairs is innovatively introduced. This weight term can dynamically adjust its importance in the total loss according to the error of each pair of matched points, and automatically assign lower weights to outlier point pairs with large errors. This mechanism makes the optimization process more robust to erroneous point matching (such as that caused by dynamic objects, noise or local similar structures), and can suppress their negative impact on the final pose solution, thereby obtaining a more accurate and stable relocalization constraint pose and improving the global consistency of the map of the entire SLAM system.

[0023] 5. Deeply integrate multi-source sensor information to construct a tightly coupled optimization framework, effectively suppressing cumulative errors: By integrating LiDAR and IMU data and reserving a GPS interface, a factor graph containing IMU pre-integration factors, LiDAR odometry factors, repositioning constraint factors (and optional GPS factors) is constructed for joint optimization. This tightly coupled optimization framework fully utilizes the high-frequency, short-time accuracy of IMU data to provide good initial values ​​for LiDAR matching, uses the absolute ranging information of LiDAR to correct IMU drift, and uses the repositioning factor to provide global closed-loop constraints. The combined effect of multiple constraints can continuously and effectively correct the cumulative errors generated during motion, thereby outputting a globally consistent, high-precision robot pose sequence and map, which is the core of achieving high-precision long-term navigation and mapping.

[0024] 6. The system features a modular design and end-to-end optimization, enhancing the overall reliability and practicality of the solution: From point cloud preprocessing (such as motion distortion removal and ground segmentation), feature point extraction and classification, to front-end odometry, loop closure detection, and factor map optimization, each module has a clear division of labor and works collaboratively. For example, the preprocessing step improves data quality; the keyframe filtering mechanism ensures system efficiency; and the interaction between the front-end and back-end (loop closure) forms a complete data flow and optimization loop. This systematic design not only improves the overall performance of positioning and mapping but also makes the solution structure clear, stable, reliable, easy to implement in engineering, debug, and expand its functionality (such as adding new sensors like GPS), demonstrating good engineering practical value.

[0025] 7. First, strictly adhering to the standard LIO-SAM process of front-end odometry—loop closure detection—back-end factor graph optimization, the system features clear module division, closed-loop data flow, and enhanced compatibility and portability. Second, by introducing the ScanContext global descriptor, rapid retrieval of historical keyframes in large-scale environments is achieved, effectively improving the efficiency of long-distance loop closure candidate screening and enabling the system to achieve cross-regional relocalization capabilities, thus solving the problem of limited retrieval range caused by the reliance on local neighborhood search in traditional methods. Third, by using a lightweight LinK3D structure matching method, the geometric consistency constraints of point cloud matching are enhanced while ensuring computational efficiency. This allows the system to maintain high matching accuracy even in scenarios with changes in viewpoint (such as pitch and roll) and repetitive structures, significantly reducing the probability of mismatches. Simultaneously, by constraining the number of keypoints and the size of the neighborhood, computational complexity is effectively reduced, meeting the real-time operation requirements of the robot dog platform. Furthermore, combining the global heading prior provided by ScanContext with the initial pose estimated by LinK3D provides a better initial value for ICP registration, improving registration convergence speed and stability, and avoiding getting trapped in local optima. The loop closure factor constructed based on this can more accurately reflect the true spatial constraints between frames, and global consistency correction is achieved through factor graph optimization, effectively suppressing accumulated errors during long-term operation. In summary, this invention significantly improves loop closure detection accuracy, relocation success rate, and overall system performance in complex environments while ensuring system real-time performance. Figure 1 It is highly consistent and particularly suitable for autonomous navigation tasks of robot dogs under long-distance, large-scale, and highly dynamic conditions. Attached Figure Description

[0026] The present invention will be further described below with reference to the accompanying drawings and embodiments.

[0027] Figure 1 This is a flowchart of a robust laser SLAM relocalization and mapping method for embodied intelligent robots according to the present invention. Detailed Implementation

[0028] The overall approach of the technical solution in this application is as follows: First, IMU data is used at the front end to compensate for point cloud motion distortion and provide high-quality initial values ​​for laser matching. Combined with local map matching, the stability of the odometry under dynamic conditions is ensured. Then, to correct the cumulative drift over long-term operation, the ScanContext global descriptor is innovatively used for fast coarse scene retrieval. Then, the lightweight LinK3D local descriptor is used to accurately match candidate frames. And the improved ICP algorithm with adaptive weights is used for fine registration, thereby efficiently and reliably obtaining high-precision relocation constraints. Finally, the IMU pre-integration factor, laser odometry factor and relocation constraint factor are jointly incorporated into the factor graph for joint optimization to deeply fuse multi-source information, continuously correct errors, and finally output a globally consistent pose and map. At the same time, through keyframe management, descriptor lightweighting and other designs, the entire algorithm is ensured to meet the real-time requirements of the embedded platform.

[0029] Please refer to Figure 1 As shown, a preferred embodiment of the robust laser SLAM relocalization and mapping method for embodied intelligent robots of the present invention includes the following steps: Step S1: Receive raw lidar point cloud data and IMU data from the quadruped robot; Step S2: Perform preprocessing on the original lidar point cloud data, including at least motion distortion removal, ground point segmentation, and voxel downsampling, to obtain basic point cloud data; Step S3: Extract feature points from the basic point cloud data and send the feature points to the front-end laser odometry module and loop closure detection module respectively; at the same time, perform pre-integration on the IMU data to obtain pre-integration results, and send the pre-integration results to the front-end laser odometry module to provide initial pose constraints for laser point cloud matching; simultaneously, generate IMU pre-integration factors based on the pre-integration results. Step S4: In the loop closure detection module, based on the feature points extracted from the current frame, a two-stage cascaded matching is performed, including fast retrieval of ScanContext global descriptors and precise matching of lightweight LinK3D local descriptors, to obtain high-precision relocation pose constraints. The two-stage (ScanContext global retrieval + LinK3D local matching) cascading matching mechanism is one of the core innovations of this invention, aiming to balance the contradiction between "retrieval speed" and "matching accuracy".

[0030] The first stage (fast coarse search): Utilizing the ScanContext global descriptor and its derived RingKey vector, a fast search is performed across all historical keyframes. The RingKey vector has low dimensionality (e.g., Nr=20 dimensions) and rotation invariance, making it ideal for constructing a KD-Tree for millisecond-level nearest neighbor search. It can quickly filter out the Top-K (e.g., K=10) most likely loop-like "candidate keyframes" from tens of thousands of historical frames, thus limiting the computationally intensive fine-grained matching to a very small candidate set.

[0031] The second stage (precise fine-grained matching) involves precise matching on a small number of candidate frames using an improved lightweight LinK3D local descriptor. The LinK3D descriptor is constructed based on the spatial relative relationships between keypoints, exhibiting good robustness to viewpoint changes and point cloud sparsity. By comparing the distances between descriptors and performing RANSAC to remove outliers, a high-precision initial relative pose between the current frame and candidate frames can be calculated. This "coarse-to-fine" process ensures that the system can quickly locate loop closure regions and obtain accurate pose constraints even in large-scale environments.

[0032] Step S5: In the front-end laser odometry module, the feature points of the current frame are matched with the maintained local voxel map, and the matching results are iteratively optimized in combination with the pre-integration results to output high-frequency laser odometry pose estimation results. Historical key frames are selected according to preset conditions and sent to the loop closure detection module for global retrieval to obtain loop closure candidate frames. At the same time, laser odometry factors are generated based on the laser odometry pose estimation results. Step S6: Using the relocation constraint pose as the initial value for the improved ICP registration, perform fine registration between the current frame and the loop closure candidate frame to obtain the relocation constraint factor; wherein, the loss function of the ICP registration introduces an adaptive weight term based on the point-pair registration residual. Step S7: Construct a factor graph by adding the IMU pre-integration factor, laser odometry factor, and relocalization constraint factor to the factor graph for joint optimization, thereby obtaining a globally consistent quadruped robot pose sequence and map. The objective of the joint optimization is: ; in, Represents the set of optimal poses; Indicates the IMU pre-integration factor; Indicates the laser odometry factor; Indicates the relocation constraint factor; The loop closure detection factor refers to a constraint factor that characterizes the precise relative pose relationship between two keyframes, obtained by initializing with cascaded matching and refining it using an improved ICP algorithm. Through joint optimization, the relocalization result can stably suppress long-term drift, enabling the robot dog to maintain continuous high-precision localization in long-term, wide-range, and complex scenarios.

[0033] In step S2, the preprocessing further includes: The original lidar point cloud data is subjected to neighborhood smoothness calculation based on scan lines, and candidate points for subsequent feature extraction are selected according to the set smoothness threshold as the basic point cloud data.

[0034] Preprocessing is fundamental to ensuring the accuracy of subsequent feature matching and state estimation. Specifically: Motion distortion removal: Using synchronously acquired IMU data, the motion of the lidar itself during a frame scan is estimated by interpolation or integration, and motion compensation is performed on each laser point in the frame to eliminate point cloud distortion caused by the robot's own movement (especially when the quadruped robot moves violently).

[0035] Ground point segmentation: Simple methods based on normal vectors or height thresholds can be used to identify and temporarily remove ground points in the point cloud. This reduces the number of unstructured features and highlights stable features such as walls, corners, and obstacle edges, which is beneficial for improving the robustness of feature matching in dynamic or open environments.

[0036] Voxel downsampling: This involves uniformly downsampling the point cloud while preserving its geometry. This significantly reduces the number of points required for subsequent processing, lowering the computational load and serving as a key step in meeting the real-time constraints of embedded platforms.

[0037] Smoothness filtering: In the preprocessing stage, the local smoothness of each point on its respective laser scanning line can be further calculated (e.g., by analyzing its curvature variation with neighboring points). By setting a smoothness threshold, a candidate set of potential feature points (such as high-curvature edge points and low-curvature plane points) can be pre-filtered, providing high-quality input for accurate feature extraction in step S3 and improving efficiency.

[0038] In step S3, the extracted feature points include edge feature points and planar feature points.

[0039] Edge features and planar features are two classic types of geometric features in laser SLAM. Edge features are typically located in areas with high curvature, such as object outlines and corners, and are characterized by being distributed along only one main direction within their local neighborhood. Planar features, on the other hand, are typically located in flat areas such as walls and the ground. Extracting these two types of features from the "base point cloud data" is to leverage their stable and distinguishable geometric properties for efficient matching.

[0040] Step S4 specifically includes: Step S41: In the loop closure detection module, a ScanContext global descriptor is generated based on the feature points extracted from the current frame, and a rotation-invariant Ring Key vector is extracted from it. Step S42: Use the Ring Key vector to quickly search in the constructed historical keyframe KD-Tree to obtain the Top-K candidate historical keyframes with similar global positions; Step S43: For each candidate historical keyframe, calculate the column distance between its ScanContext global descriptor and the ScanContext global descriptor of the current frame, and compensate for the orientation difference through a cyclic shift operation to filter out the candidate historical keyframes that meet the similarity threshold. Step S44: Extract lightweight improved LinK3D key points and their lightweight LinK3D local descriptors from the current frame and the filtered candidate historical key frames. Step S45: Based on the distance of the lightweight LinK3D local descriptor, perform feature matching on the LinK3D key points of the current frame and each candidate frame, and use the RANSAC algorithm to remove incorrect matching pairs. Step S46: For the set of matching point pairs filtered by the RANSAC algorithm, calculate the initial relative pose transformation between the feature points of the two frames through singular value decomposition to obtain high-precision relocation pose constraints.

[0041] This invention combines ScanContext global retrieval with lightweight LinK3D local matching to construct a coarse-fine two-level cascaded matching architecture, providing high-quality initial pose and stable point correspondences for subsequent ICP registration. The first level is global coarse matching, which retrieves the Top-K historical keyframes in the KD-Tree using the current frame's RingKey. SC similarity calculation is used to filter out candidate frames with the closest global position, eliminating a large number of irrelevant keyframes and narrowing the relocalization search scope to the local space. The second level is local fine matching, which extracts lightweight LinK3D keypoints and 128-dimensional descriptors for each candidate keyframe and the current frame, and completes the matching based on descriptor distance. Similarity calculation only considers non-zero dimensions; when the absolute difference of the corresponding dimension is less than 0.2, the similarity score is incremented by 1, and a score higher than the threshold Thscore=3 is considered a valid match.

[0042] After obtaining the initial matching pairs, the RANSAC algorithm is used to remove outliers, retaining the interior point set for coarse SVD registration. Let the filtered matching point pairs be... ,in For key points in the current frame, For key points in historical frames, the optimization objective is to find the optimal rigid transformation. : ; Where R represents the rotation matrix; t represents the translation vector; This represents the optimal transformation. SVD decomposition allows for rapid closure of R and t, outputting the initial pose for repositioning. This cascaded architecture utilizes ScanContext for fast global localization and leverages LinK3D for precise local constraints, significantly reducing the initial pose error and providing highly reliable input for subsequent ICP registration.

[0043] Step S41 specifically involves: In the loop closure detection module, the feature points extracted from the current frame are divided into a grid in polar coordinates, which is divided into Nr rings radially and Ns sectors azimuthally, forming an NrxNs two-dimensional matrix. For each grid cell in the two-dimensional matrix, the maximum height value of all points within it is taken as the encoding value of the grid cell. If the grid cell is empty, the encoding value is zero, thereby generating a ScanContext descriptor matrix containing the ScanContext global descriptor. For each row of the ScanContext descriptor matrix, count the number of non-zero encoded values ​​and divide by the total number of sectors Ns to obtain the density value of the ring. The density values ​​of the Nr rings are concatenated sequentially to form an Nr-dimensional Ring Key vector.

[0044] To achieve fast global relocalization retrieval for the robot dog, a ScanContext global descriptor is introduced, constructing a two-stage search mechanism of "RingKey coarse retrieval - SC fine verification," which significantly reduces the global matching time while ensuring recall. First, the preprocessed point cloud is divided into polar coordinate grids, with Nr rings radially and Ns sectors azimuthally, forming an Nr×Ns two-dimensional ScanContext matrix. Each grid cell is denoted as a Bin. Each Bin is height-encoded using the maximum z-coordinate of all points within the Bin. ; in, This represents the Bin corresponding to the j-th sector of the i-th ring; This represents the height value of point p; This represents the encoded value of Bin. An empty Bin is directly assigned the value 0.

[0045] To achieve fast retrieval, rotation-invariant RingKey vectors are extracted from the ScanContext. The effective point density is calculated for each row of the SC matrix (corresponding to one ring): ; in, This represents the density value of the i-th ring; This indicates the number of non-zero elements in the i-th row; This indicates the total number of sectors.

[0046] Concatenate the densities of all rings in order to obtain an Nr-dimensional RingKey vector. This vector is rotation-invariant and can be directly fed into a KD-Tree for fast nearest neighbor search, outputting Top-K candidate keyframes. After obtaining the candidate keyframes, perform fine-grained matching using ScanContext. Let the query frame SC be... Candidate frame SC is The sum of the cosine distances for each column is used as the overall similarity. ; in, and These represent the j-th column vectors of the query frame and the candidate frame, respectively. This indicates the SC distance between two frames.

[0047] To overcome column misalignment caused by changes in lidar orientation, one frame is processed from 0 to... The columns are cyclically shifted, and the minimum distance is taken as the final similarity. When the distance is lower than the set threshold, it is determined to be globally similar.

[0048] Step S44 specifically includes: Step S441: Extract edge points from the feature points based on local surface smoothness; Step S442: Divide the XoY plane of the LiDAR into N sectors, where N is less than the number of sectors in the original LinK3D, and cluster the extracted edge points into the corresponding sectors according to their azimuth angles. Step S443: For each non-empty sector, calculate the centroid of all edge points within it, and use the centroid as a LinK3D key point; Step S444: Using the currently described aggregated key point as the center, calculate the vector between it and the nearest neighbor aggregated key point, and use it as the main direction; Step S445: For the other aggregated key points, calculate their angle relative to the main direction, and classify them into the corresponding dimension of the preset M-dimensional descriptor according to the angle, where M is less than the descriptor dimension of the original LinK3D. Step S446: For each occupied descriptor dimension, assign its value to the Euclidean distance between the LinK3D keypoint to be described and the nearest neighbor LinK3D keypoint in that dimension, and assign zero to unoccupied dimensions, thereby generating an M-dimensional lightweight LinK3D local descriptor.

[0049] The core idea of ​​LinK3D is to construct linear descriptors using the relative spatial relationships of keypoint neighborhoods, offering advantages such as robustness to sparse point clouds, real-time CPU operation, and high matching accuracy. The original LinK3D, employing a 180-dimensional descriptor, 120-sector clustering, and a three-nearest-neighbor main direction fusion strategy, still suffers from significant storage and computational overhead on the embedded computing platform of a robot dog. To address this, this invention implements lightweight improvements in five aspects: edge point extraction, keypoint aggregation, descriptor dimension, main direction calculation, and lookup table structure, significantly reducing time consumption while maintaining matching performance.

[0050] In the edge point extraction stage, the screening method based on local smoothness is used, and for any point i in the point cloud, the set of neighborhood points evenly distributed along its scan line is selected. Calculate the smoothing term: ; in, This represents the local surface smoothness at point i; Represents the set of points in the same line of reference centered at point i; Indicates the number of neighboring points; and Let i and j represent the three-dimensional coordinate vectors of points i and j respectively; when When it is determined to be an edge point, among which The threshold was adjusted to 8 based on typical robot dog scenarios to reduce redundant candidate points while preserving effective structural edges.

[0051] In step S442, the value of N is 60; In the key point aggregation stage, the original sector clustering is simplified and lightweighted: the number of sectors is reduced from... The clustering overhead is reduced to 60; the fixed distance threshold is changed to an adaptive range of 0.3~0.5 that varies with radial distance to improve the stability of long-distance keypoints; at the same time, the effective cluster selection conditions are simplified, retaining only the constraint on the number of points within a cluster and removing the constraint on the number of scan lines. Finally, the centroid of each effective cluster is used as the aggregated keypoint to output a robust keypoint set. This is used for subsequent descriptor generation.

[0052] In step S445, the value of M is 128.

[0053] During the descriptor generation stage, the original 180-dimensional descriptor is compressed to 128 dimensions, reducing the computational cost of matching while maintaining descriptive power. All aggregated keypoints are projected onto the LiDAR coordinate system XoY plane to eliminate the influence of uneven Z-axis distribution, using the current keypoints... The plane is evenly divided into 128 sectors centered on a point. To ensure rotational invariance, the distance is... Recent key points Construct the principal direction vector: ; in, This represents the main direction vector of the current key point; Indicates the key points to be described; Show the nearest neighbor key point.

[0054] For any other key point Calculate its angle relative to the principal direction: ; The direction sign discrimination term is: ; in, express point to ; express The angle relative to the main direction; Used to determine the quadrant in which the angle is located; and This represents the two-dimensional coordinates of the reference key point (or aggregation seed point) on the XoY plane of the LiDAR coordinate system. This point is the center point where we are currently trying to aggregate other points. and This represents the two-dimensional coordinates of the adjacent key point (index j) to be judged on the XoY plane of the LiDAR coordinate system. The algorithm will traverse multiple such points and determine whether they should be aligned with the reference point. , )polymerization.

[0055] According to the angle Assign a sector to the corresponding sector and a value to each sector: ; in, This represents the value of the d-th dimension of the descriptor; This represents the Euclidean distance between key points.

[0056] This results in a 128-dimensional lightweight LinK3D local description. To further accelerate the process, the original three-nearest-neighbor descriptor fusion is simplified to a single principal direction descriptor, and the priority calculation of multiple sets of descriptors is eliminated. Temporal consistency and RANSAC outlier removal are used to replace multi-direction fusion, which brings robustness gains. At the same time, the distance table and direction table are lightweightly reconstructed, retaining only the relationships between key points and using hash table indexing to meet the real-time operation requirements of the robot dog embedded platform.

[0057] In step S5, the step of filtering historical keyframes according to preset conditions specifically involves: Based on the preset conditions that the pose change between the current frame and the previous historical key frame exceeds a set translation threshold or rotation threshold, or the time interval from the previous historical key frame exceeds a set interval threshold, historical key frames are selected from the laser odometry pose estimation results. The constructed historical keyframe KD-Tree is updated based on the selected historical keyframes.

[0058] In step S6, the formula for the loss function is: ; in, and Let R represent a pair of matched feature points; R and t represent the rotation matrix and translation vector to be solved; m represents the index of the matched feature pair. The adaptive weight term for the m-th pair of matching feature points is calculated using the following formula: ; in, σ represents the registration residual of the m-th pair of matched feature points; σ represents the preset noise standard deviation parameter. The larger the residual, the lower the weight, which can effectively suppress dynamic targets, occlusion, and outside point interference.

[0059] The native LIO-SAM uses ICP registration based on edge and planar features, which is sensitive to initial values ​​and easily gets stuck in local optima or even fails to match in degenerate, dynamic, and low-feature scenes. This invention embeds the cascaded matching results into the ICP matching stage of LIO-SAM, systematically improving the registration process from four aspects: initial pose, point correspondence, loss function, and factor graph constraints.

[0060] During the registration initialization phase, the SVD pose output by the LinK3D-SC concatenated matching directly replaces the default initial value of LIO-SAM, significantly reducing the dependence of ICP on the initial value and enabling it to converge quickly even in scenarios with large pose deviations. During the point correspondence construction phase, robust matching pairs provided by LinK3D replace the traditional neighborhood search for corresponding points, reducing registration deviations caused by incorrect correspondences and improving the stability of ICP in sparse and degenerate scenarios.

[0061] In step S7, the construction of the factor graph further includes: The GPS observation factor is added to the factor graph and jointly optimized with the IMU pre-integration factor, laser odometry factor, and relocation constraint factor.

[0062] The composition and optimization objective of the factor graph: A factor graph is a probabilistic graphical model that integrates information from multiple sources. Nodes in the graph represent the robot's pose state to be optimized, and edges (factors) represent constraints imposed by observations from various sensors. Specifically, this includes: IMU pre-integration factor: constrains the relationship between continuous poses and provides high-frequency short-term motion prediction.

[0063] Laser odometry factor: generated by the front-end odometry, constrains the relative pose between adjacent keyframes, and provides local consistency.

[0064] Relocation constraint factor (loop closure factor): generated by improved loop closure detection and ICP fine registration, connecting non-adjacent but spatially reunited keyframes, providing crucial global loop closure constraints to correct for accumulated drift.

[0065] GPS observation factor: When the robot is outdoors and has access to GPS signals, this factor can constrain the pose to a global coordinate system, provide an absolute position reference, and further suppress drift.

[0066] The joint optimization process: The task of the back-end optimizer (such as iSAM2) is to find a set of most probable pose state sequences that minimizes the sum of squares of the observation errors of all factors (such as IMU pre-integration errors, laser matching errors, loop closure errors, etc.). Through this tightly coupled joint optimization, the high-frequency information of the IMU, the precise geometric information of the LiDAR, and the globally consistent information provided by the loop closure are deeply fused, thereby outputting a globally consistent, high-precision robot motion trajectory and environment map.

[0067] This invention integrates a lightweight LinK3D local descriptor with a ScanContext global descriptor and embeds it into the LIO-SAM laser inertial odometry framework, forming a standardized relocalization architecture of front-end odometry—loop closure detection—back-end factor graph optimization. Using raw LiDAR point cloud data and IMU data as input, the raw LiDAR point cloud data undergoes distortion correction, ground segmentation, and voxel downsampling preprocessing. Edge and planar feature points are then extracted uniformly, and these feature points are simultaneously fed into the front-end laser odometry module and the loop closure detection module. The IMU data, after IMU pre-integration, provides initial pose constraints for the front-end laser odometry module. In the loop closure detection module, a ScanContext global descriptor is first generated from the current frame features. A KD-Tree is constructed using Ring Keys to perform rapid retrieval, obtaining Top-K candidate historical keyframes. Subsequently, lightweight LinK3D local descriptor generation and feature matching are performed on the candidate historical keyframes. After RANSAC outlier removal and SVD geometric verification, stable relocalization pose constraints are output. In the front-end laser odometry module, the current frame features are matched with the voxel map, and the laser-IMU initial pose provided by IMU pre-integration is used for iterative optimization. This outputs high-frequency laser odometry results and completes keyframe filtering. Historical keyframes are fed back to the loop closure detection module for global retrieval. The back-end incorporates IMU pre-integration factors, laser odometry factors, loop closure constraint factors, and optional GPS factors into joint optimization. The initial pose output by cascaded matching replaces the original initial value. ICP registration is improved, and adaptive weight terms are added to enhance robustness in dynamic environments. Finally, global pose optimization outputs a globally consistent map and high-precision positioning results. This invention implements lightweight improvements to LinK3D in five aspects: edge point extraction, key point aggregation, descriptor dimension, main direction calculation, and lookup table structure. It reduces the number of sectors, compresses the descriptor dimension, and simplifies the main direction fusion strategy, significantly reducing computation time while maintaining matching performance. Simultaneously, it employs a two-stage search mechanism of RingKey coarse retrieval and SC fine verification, balancing global retrieval efficiency and recall. Through a cascaded architecture of ScanContext coarse matching and LinK3D fine matching, it provides high-quality initial poses for ICP registration, reducing dependence on initial values ​​and avoiding local optima. Finally, it incorporates relocalization constraints as global factors into factor graph joint optimization, effectively suppressing IMU cumulative errors and laser matching errors, and improving the relocalization accuracy, system robustness, and construction performance of the robot dog in long-distance, large-scene, and highly dynamic complex environments. Figure 1 To the point of being responsive.

[0068] In summary, the advantages of this invention are as follows: 1. Under intense motion conditions, IMU pre-integration is used to compensate for point cloud motion distortion and provide high-quality initial values ​​for laser matching. Combined with local map matching, the robustness of the front-end odometry is enhanced. In large-scale scenes, the ScanContext global descriptor is used for fast scene retrieval, followed by precise matching using a lightweight LinK3D local descriptor. Improved ICP with adaptive weights is introduced for fine registration, thereby obtaining high-precision relocalization constraints. These constraints, along with IMU and laser odometry factors, are then added to a factor map for joint optimization, effectively correcting long-distance cumulative drift and ensuring global accuracy. Figure 1 Consistency; To meet the real-time constraints of the embedded platform, a keyframe management strategy is adopted to control the data scale throughout the process, and the computation and storage overhead of the algorithm is significantly reduced through a cascaded process of "fast global retrieval + precise local matching" and lightweight design such as reducing the descriptor dimension.

[0069] 2. A two-stage back-linked detection mechanism that balances efficiency and accuracy significantly improves the robustness and success rate of relocalization: Fast global descriptor retrieval via ScanContext is combined with precise matching using lightweight LinK3D local descriptors. First, a fast global coarse search is performed in the KD-Tree using ScanContext and its derived RingKey vectors, efficiently filtering out a small number of candidate frames from a massive amount of historical keyframes, significantly reducing the search space and ensuring real-time performance. Second, the candidate frames are further refined using the improved lightweight LinK3D local descriptors for fine feature matching, and optimized using the RANSAC algorithm to obtain high-precision relative pose transformations. This "coarse-to-fine" strategy effectively solves the problems of slow retrieval speed, susceptibility to interference, or insufficient matching accuracy of traditional single methods in large, complex, or dynamic environments, making the relocalization process both fast and reliable, which is key to the system's robustness.

[0070] 3. Targeted lightweight improvements to the local descriptor LinK3D to effectively balance computational overhead and descriptive capabilities: The original LinK3D has been "lightweighted" by reducing the number of sector partitions (e.g., N=60) and the descriptor dimension (e.g., M=128). While maintaining its core advantage of using neighborhood spatial structure relationships for description, this improvement significantly reduces the computational complexity and memory consumption during feature extraction and descriptor matching. It can better adapt to the limited computing resources of mobile platforms such as quadruped robots, ensuring that the whole system can run in real time without sacrificing necessary feature discrimination, thus enhancing the practicality and deployment feasibility of the technical solution.

[0071] 4. An adaptive ICP loss function is introduced in the loop closure fine registration stage to improve the accuracy and stability of registration in complex point clouds: In the loss function of the standard Iterative Closest Point (ICP) algorithm, an adaptive weight term based on the registration residual of point pairs is innovatively introduced. This weight term can dynamically adjust its importance in the total loss according to the error of each pair of matched points, and automatically assign lower weights to outlier point pairs with large errors. This mechanism makes the optimization process more robust to erroneous point matching (such as that caused by dynamic objects, noise or local similar structures), and can suppress their negative impact on the final pose solution, thereby obtaining a more accurate and stable relocalization constraint pose and improving the global consistency of the map of the entire SLAM system.

[0072] 5. Deeply integrate multi-source sensor information to construct a tightly coupled optimization framework, effectively suppressing cumulative errors: By integrating LiDAR and IMU data and reserving a GPS interface, a factor graph containing IMU pre-integration factors, LiDAR odometry factors, repositioning constraint factors (and optional GPS factors) is constructed for joint optimization. This tightly coupled optimization framework fully utilizes the high-frequency, short-time accuracy of IMU data to provide good initial values ​​for LiDAR matching, uses the absolute ranging information of LiDAR to correct IMU drift, and uses the repositioning factor to provide global closed-loop constraints. The combined effect of multiple constraints can continuously and effectively correct the cumulative errors generated during motion, thereby outputting a globally consistent, high-precision robot pose sequence and map, which is the core of achieving high-precision long-term navigation and mapping.

[0073] 6. The system features a modular design and end-to-end optimization, enhancing the overall reliability and practicality of the solution: From point cloud preprocessing (such as motion distortion removal and ground segmentation), feature point extraction and classification, to front-end odometry, loop closure detection, and factor map optimization, each module has a clear division of labor and works collaboratively. For example, the preprocessing step improves data quality; the keyframe filtering mechanism ensures system efficiency; and the interaction between the front-end and back-end (loop closure) forms a complete data flow and optimization loop. This systematic design not only improves the overall performance of positioning and mapping but also makes the solution structure clear, stable, reliable, easy to implement in engineering, debug, and expand its functionality (such as adding new sensors like GPS), demonstrating good engineering practical value.

[0074] 7. First, strictly adhering to the standard LIO-SAM process of front-end odometry—loop closure detection—back-end factor graph optimization, the system features clear module division, closed-loop data flow, and enhanced compatibility and portability. Second, by introducing the ScanContext global descriptor, rapid retrieval of historical keyframes in large-scale environments is achieved, effectively improving the efficiency of long-distance loop closure candidate screening and enabling the system to achieve cross-regional relocalization capabilities, thus solving the problem of limited retrieval range caused by the reliance on local neighborhood search in traditional methods. Third, by using a lightweight LinK3D structure matching method, the geometric consistency constraints of point cloud matching are enhanced while ensuring computational efficiency. This allows the system to maintain high matching accuracy even in scenarios with changes in viewpoint (such as pitch and roll) and repetitive structures, significantly reducing the probability of mismatches. Simultaneously, by constraining the number of keypoints and the size of the neighborhood, computational complexity is effectively reduced, meeting the real-time operation requirements of the robot dog platform. Furthermore, combining the global heading prior provided by ScanContext with the initial pose estimated by LinK3D provides a better initial value for ICP registration, improving registration convergence speed and stability, and avoiding getting trapped in local optima. The loop closure factor constructed based on this can more accurately reflect the true spatial constraints between frames, and global consistency correction is achieved through factor graph optimization, effectively suppressing accumulated errors during long-term operation. In summary, this invention significantly improves loop closure detection accuracy, relocation success rate, and overall system performance in complex environments while ensuring system real-time performance. Figure 1 It is highly consistent and particularly suitable for autonomous navigation tasks of robot dogs under long-distance, large-scale, and highly dynamic conditions.

[0075] While specific embodiments of the present invention have been described above, those skilled in the art should understand that the specific embodiments described are merely illustrative and not intended to limit the scope of the present invention. Equivalent modifications and variations made by those skilled in the art in accordance with the spirit of the present invention should be covered within the scope of protection of the claims of the present invention.

Claims

1. A robust laser SLAM relocalization and mapping method for embodied intelligent robots, characterized in that: Includes the following steps: Step S1: Receive raw lidar point cloud data and IMU data from the quadruped robot; Step S2: Perform preprocessing on the original lidar point cloud data, including at least motion distortion removal, ground point segmentation, and voxel downsampling, to obtain basic point cloud data; Step S3: Extract feature points from the basic point cloud data and send the feature points to the front-end laser odometry module and loop closure detection module respectively; at the same time, perform pre-integration on the IMU data to obtain pre-integration results, and send the pre-integration results to the front-end laser odometry module to provide initial pose constraints for laser point cloud matching; simultaneously, generate IMU pre-integration factors based on the pre-integration results. Step S4: In the loop closure detection module, based on the feature points extracted from the current frame, a two-stage cascaded matching is performed, including fast retrieval of ScanContext global descriptors and precise matching of lightweight LinK3D local descriptors, to obtain high-precision relocation pose constraints. Step S5: In the front-end laser odometry module, the feature points of the current frame are matched with the maintained local voxel map, and the matching results are iteratively optimized in combination with the pre-integration results to output high-frequency laser odometry pose estimation results. Historical key frames are selected according to preset conditions and sent to the loop closure detection module for global retrieval to obtain loop closure candidate frames. At the same time, laser odometry factors are generated based on the laser odometry pose estimation results. Step S6: Using the relocation constraint pose as the initial value for the improved ICP registration, perform fine registration between the current frame and the loop closure candidate frame to obtain the relocation constraint factor; wherein, the loss function of the ICP registration introduces an adaptive weight term based on the point-pair registration residual. Step S7: Construct a factor graph by adding the IMU pre-integration factor, laser odometry factor, and relocation constraint factor to the factor graph for joint optimization, and solve to obtain a globally consistent quadruped robot pose sequence and map.

2. The robust laser SLAM relocalization and mapping method for embodied intelligent robots as described in claim 1, characterized in that: In step S2, the preprocessing further includes: The original lidar point cloud data is subjected to neighborhood smoothness calculation based on scan lines, and candidate points for subsequent feature extraction are selected according to the set smoothness threshold as the basic point cloud data.

3. The robust laser SLAM relocalization and mapping method for embodied intelligent robots as described in claim 1, characterized in that: In step S3, the extracted feature points include edge feature points and planar feature points.

4. A robust laser SLAM relocalization and mapping method for embodied intelligent robots as described in claim 1, characterized in that: Step S4 specifically includes: Step S41: In the loop closure detection module, a ScanContext global descriptor is generated based on the feature points extracted from the current frame, and a rotation-invariant Ring Key vector is extracted from it. Step S42: Use the Ring Key vector to quickly search in the constructed historical keyframe KD-Tree to obtain the Top-K candidate historical keyframes with similar global positions; Step S43: For each candidate historical keyframe, calculate the column distance between its ScanContext global descriptor and the ScanContext global descriptor of the current frame, and compensate for the orientation difference through a cyclic shift operation to filter out the candidate historical keyframes that meet the similarity threshold. Step S44: Extract lightweight improved LinK3D key points and their lightweight LinK3D local descriptors from the current frame and the filtered candidate historical key frames. Step S45: Based on the distance of the lightweight LinK3D local descriptor, perform feature matching on the LinK3D key points of the current frame and each candidate frame, and use the RANSAC algorithm to remove incorrect matching pairs. Step S46: For the set of matching point pairs filtered by the RANSAC algorithm, calculate the initial relative pose transformation between the feature points of the two frames through singular value decomposition to obtain high-precision relocation pose constraints.

5. A robust laser SLAM relocalization and mapping method for embodied intelligent robots as described in claim 4, characterized in that: Step S41 specifically involves: In the loop closure detection module, the feature points extracted from the current frame are divided into a grid in polar coordinates, which is divided into Nr rings radially and Ns sectors azimuthally, forming an NrxNs two-dimensional matrix. For each grid cell in the two-dimensional matrix, the maximum height value of all points within it is taken as the encoding value of the grid cell. If the grid cell is empty, the encoding value is zero, thereby generating a ScanContext descriptor matrix containing the ScanContext global descriptor. For each row of the ScanContext descriptor matrix, count the number of non-zero encoded values ​​and divide by the total number of sectors Ns to obtain the density value of the ring. The density values ​​of the Nr rings are concatenated sequentially to form an Nr-dimensional Ring Key vector.

6. A robust laser SLAM relocalization and mapping method for embodied intelligent robots as described in claim 4, characterized in that: Step S44 specifically includes: Step S441: Extract edge points from the feature points based on local surface smoothness; Step S442: Divide the XoY plane of the LiDAR into N sectors, where N is less than the number of sectors in the original LinK3D, and cluster the extracted edge points into the corresponding sectors according to their azimuth angles. Step S443: For each non-empty sector, calculate the centroid of all edge points within it, and use the centroid as a LinK3D key point; Step S444: Using the currently described aggregated key point as the center, calculate the vector between it and the nearest neighbor aggregated key point, and use it as the main direction; Step S445: For the other aggregated key points, calculate their angle relative to the main direction, and classify them into the corresponding dimension of the preset M-dimensional descriptor according to the angle, where M is less than the descriptor dimension of the original LinK3D. Step S446: For each occupied descriptor dimension, assign its value to the Euclidean distance between the LinK3D keypoint to be described and the nearest neighbor LinK3D keypoint in that dimension, and assign zero to unoccupied dimensions, thereby generating an M-dimensional lightweight LinK3D local descriptor.

7. A robust laser SLAM relocalization and mapping method for embodied intelligent robots as described in claim 6, characterized in that: In step S442, the value of N is 60; In step S445, the value of M is 128.

8. A robust laser SLAM relocalization and mapping method for embodied intelligent robots as described in claim 1, characterized in that: In step S5, the step of filtering historical keyframes according to preset conditions specifically involves: Based on the preset conditions that the pose change between the current frame and the previous historical key frame exceeds a set translation threshold or rotation threshold, or the time interval from the previous historical key frame exceeds a set interval threshold, historical key frames are selected from the laser odometry pose estimation results. The constructed historical keyframe KD-Tree is updated based on the selected historical keyframes.

9. A robust laser SLAM relocalization and mapping method for embodied intelligent robots as described in claim 1, characterized in that: In step S6, the formula for the loss function is: ; in, and Let R represent a pair of matched feature points; R and t represent the rotation matrix and translation vector to be solved; m represents the index of the matched feature pair. The adaptive weight term for the m-th pair of matching feature points is calculated using the following formula: ; in, σ represents the registration residual of the m-th pair of matched feature points; σ represents the preset noise standard deviation parameter.

10. A robust laser SLAM relocalization and mapping method for embodied intelligent robots as described in claim 1, characterized in that: In step S7, the construction of the factor graph further includes: The GPS observation factor is added to the factor graph and jointly optimized with the IMU pre-integration factor, laser odometry factor, and relocation constraint factor.

Images