Laser SLAM-based robot localization methods, systems, devices, and media

Abstract

This invention relates to the field of lidar positioning technology, specifically disclosing a robot localization method, system, device, and medium based on lidar SLAM. The invention acquires 3D feature point clouds from lidar and simultaneously receives IMU data. Preliminary pose correction is performed on the feature points through pose pre-transformation. The corrected 3D point cloud is projected onto a 2D plane to form projected feature points containing height information, which are extracted as 2D feature point clouds for registration. The RANSAC point cloud registration algorithm is used to match the current frame's 2D feature points with historical sub-maps, and the sub-maps are updated based on the registration results. Kalman filtering is used to filter the preliminary odometry output to obtain the final odometry output, improving estimation accuracy and producing smooth and high-precision odometry results. This provides support for precise robot localization and navigation in complex environments, achieving robust SLAM mapping capabilities in dynamic, sparse, or violently moving environments.

Description

Technical Field

[0001] This invention relates to the field of lidar positioning technology, and more specifically, to a method, system, device, and medium for robot positioning based on lidar SLAM. Background Technology

[0002] SLAM (Simultaneous Localization and Mapping) is one of the essential technologies in the field of mobile robotics, enabling robots to incrementally build maps in unknown environments and simultaneously locate themselves based on their own state estimates and the map.

[0003] However, for complex environments, such as those with sparse features, environments with more dynamic objects, or scenarios involving vigorous robot movement, 3D laser SLAM can utilize information within three-dimensional space for localization, exhibiting good performance. But for robots operating on a plane, 3D laser SLAM calculates six degrees of freedom, resulting in a waste of computational resources. To address this issue, existing technologies employ a method of converting 3D point cloud slices into 2D lasers and then running 2D laser SLAM. However, this method leads to the loss of information from the spatial environment, reducing the algorithm's performance.

[0004] Therefore, it is necessary to provide a robot localization method, system, device, and medium based on laser SLAM to solve the above-mentioned technical problems. In order to solve the above problems, a technical solution is provided. Summary of the Invention

[0005] To overcome the aforementioned shortcomings of existing technologies, this invention provides a robot localization method, system, device, and medium based on laser SLAM. This addresses the problem that existing 3DSLAM wastes resources calculating redundant degrees of freedom on planar robots, while 2DSLAM suffers from performance degradation due to information loss, making it difficult to balance efficiency and accuracy.

[0006] To achieve the above objectives, the present invention provides the following technical solution:

[0007] The robot localization method based on laser SLAM includes the following steps:

[0008] Collect 3D feature point clouds within the robot's movement space and acquire IMU data based on the inertial measurement unit;

[0009] The pose pre-transformation of the 3D feature point cloud is performed using IMU data. The 3D feature point cloud is then projected onto a 2D plane, and the height attribute is preserved to obtain the 2D feature point cloud. The 2D feature point cloud is then registered, and the point cloud registration result of the current frame is used as the preliminary odometry output.

[0010] The final odometer output is obtained by filtering the initial odometer output using Kalman filtering.

[0011] After each point cloud registration is completed, the sub-map is updated, and the feature point clouds of all registered frames are superimposed to form a local sub-map.

[0012] As a further aspect of the present invention, the RANSAC point cloud registration method is used to register the 2D feature point cloud.

[0013] As a further aspect of the present invention, the registration process for the 2D feature point cloud is as follows:

[0014] Randomly select several feature point clouds, and query the nearest feature points within the stored local sub-map, adding them to a queue as corresponding point pairs. Extract several corresponding point pairs; the feature points of the current frame are called the source point set. ,in, Let i be the feature point of the current frame in the i-th corresponding point pair within the source point set. The number of corresponding point pairs, and the feature points derived from the local submap, are called the target point set. ,in, For the feature points originating from the local submap in the i-th feature point pair within the source point set, calculate the mean for both the source and target point sets, and then perform centering to obtain the first data matrix. and the second data matrix ;

[0015] Based on the first data matrix and the second data matrix Construct the covariance matrix;

[0016] The covariance matrix is ​​decomposed using SVD to obtain the orthogonal matrix. The rotation matrix is ​​then calculated, and the translation vector is solved. A homogeneous transformation matrix is ​​constructed based on the rotation matrix and the translation vector. ;

[0017] Apply rigid body transformation to all feature points in the current frame, find the number of inliers, and iteratively optimize the odometer output.

[0018] As a further aspect of the present invention, the source point set and target point set The process of calculating the mean and then centering the values ​​is as follows:

[0019] For the source set and target point set Calculate the mean separately:

[0020] In the formula, Let be the mean of the source point set. The mean of the target point set;

[0021] Based on the mean calculation results, the source point set and the target point set are centered respectively:

[0022] In the formula, For the feature points of the new frame in the i-th corresponding point pair within the centered source point set, The feature points originating from the local submap are the i-th feature point pair within the centered source point set.

[0023] A first data matrix is ​​obtained by constructing data matrices based on the centralized source and target point sets. and the second data matrix .

[0024] As a further aspect of the present invention, based on the first data matrix and the second data matrix The expression for constructing the covariance matrix is ​​as follows: In the formula, Let covariance matrix be the variance matrix. For the feature points of the new frame in the i-th corresponding point pair within the centered source point set, For the i-th feature point pair in the centered source point set, the feature points originating from the local submap are... This is for the transpose operation.

[0025] As a further aspect of this invention, the covariance matrix is ​​decomposed using SVD to obtain an orthogonal matrix. Then, the rotation matrix is ​​calculated and the translation vector is solved. Based on the rotation matrix and the translation vector, a homogeneous transformation matrix is ​​constructed. The specific process is as follows: The formulas for performing SVD decomposition on the covariance matrix to obtain the orthogonal matrix and the singular value diagonal matrix are:

[0026] In the formula, It is an orthogonal matrix. It is a singular value diagonal matrix;

[0027] Calculate the rotation matrix based on orthogonal matrices:

[0028] In the formula, It is a rotation matrix;

[0029] Calculate the translation vector using the rotation matrix:

[0030] In the formula, It is a translation vector;

[0031] The homogeneous transformation matrix is ​​obtained by using the translation vector and the rotation matrix:

[0032] In the formula, It is a homogeneous transformation matrix.

[0033] As a further aspect of the present invention, a rigid body transformation is applied to all feature points in the current frame to find the number of inliers and iteratively optimize the odometer output, specifically including:

[0034] Apply rigid body transformation to all feature points in the current frame, find the number of feature points in the sub-map within a fixed distance of all feature points in the feature point cloud of the current frame after the transformation, and use this as the number of inliers.

[0035] Repeatedly and randomly select corresponding point pairs and apply rigid body transformations. When the number of interior points exceeds the number of interior points generated by previous transformations, receive a new transformation. Simultaneously, the transformed feature point cloud is stored to construct a local sub-map;

[0036] When a new transformation is received, the formula for calculating the new robot odometry output is:

[0037] In the formula, Let be the transformation matrix at time i. Let be the transformation matrix at the (i-1)th moment.

[0038] A robot localization system based on laser SLAM, the system comprising:

[0039] The acquisition and measurement module is used for the 3D feature point cloud of the mobile robot, and acquires IMU data based on the inertial measurement unit; the transformation and registration module is used to perform pose pre-transformation on the 3D feature point cloud through IMU data, project the 3D feature point cloud onto a 2D plane, retain the height attribute to obtain the 2D feature point cloud, register the 2D feature point cloud, and output the point cloud registration result of the current frame as a preliminary odometry.

[0040] The output filtering module is used to filter the initial odometer output based on Kalman filtering to obtain the final odometer output;

[0041] The update overlay module is used to update the sub-map after each point cloud registration and overlay the feature point clouds of all registered frames to form a local sub-map.

[0042] An electronic device includes a memory and a processor, wherein the memory stores a computer program, and when the processor runs the computer program stored in the memory, the processor performs the steps of the laser SLAM-based robot localization method described above.

[0043] A readable storage medium storing a computer program, which, when executed by a processor, is used to implement the steps of the laser SLAM-based robot localization method described above.

[0044] The technical effects and advantages of this invention regarding the robot localization method, system, equipment, and medium based on laser SLAM are as follows: This invention acquires 3D feature point clouds from a lidar radar and simultaneously receives IMU data. Preliminary attitude correction is performed on the feature points through pose pre-transformation. The corrected 3D point cloud is projected onto a 2D plane to form projected feature points containing height information, which are extracted as 2D feature point clouds for registration. The RANSAC point cloud registration algorithm is used to match the current frame's 2D feature points with historical sub-maps, and the sub-maps are updated based on the registration results. Kalman filtering is used to filter the preliminary odometry output to obtain the final odometry output, improving estimation accuracy and producing smooth and high-precision odometry results. This provides support for precise robot localization and navigation in complex environments, achieving robust SLAM mapping capabilities in dynamic, sparse, or violent motion environments.

[0045] This invention presents a robot localization method based on laser SLAM that balances the spatial information integrity of 3D point clouds with the computational efficiency of 2D algorithms. It can reduce redundant calculations of six degrees of freedom while preserving key height features, significantly improving the system's localization robustness and real-time performance in environments with sparse features, dynamic object interference, or violent motion. At the same time, it avoids the performance degradation caused by information loss in traditional 2D SLAM, making it an efficient solution for mapping and localization in complex scenarios for ground mobile robots. Attached Figure Description

[0046] Figure 1 A flowchart illustrating a robot localization method based on laser SLAM provided in an embodiment of the present invention;

[0047] Figure 2 A flowchart for odometer calculation provided in an embodiment of the present invention;

[0048] Figure 3 A system block diagram of a robot localization system based on laser SLAM provided in an embodiment of the present invention;

[0049] Figure 4 This is a schematic diagram of the hardware structure of an electronic device provided in an embodiment of the present invention. Detailed Implementation

[0050] The technical solutions of this invention will now be clearly and completely described with reference to the accompanying drawings. Obviously, the described technical solutions are only a part of this invention, and not all of it. All other technical solutions obtained by those skilled in the art based on the technical solutions of this invention without inventive effort are within the scope of protection of this invention.

[0051] like Figure 1 The diagram shown is a flowchart of a robot localization method based on laser SLAM provided in an embodiment of the present invention. Figure 1 The execution entity of the method shown can be a software and / or hardware device. The execution entity of this application can include, but is not limited to, at least one of the following: user equipment, network equipment, etc. User equipment can include, but is not limited to, computers, smartphones, personal digital assistants (PDAs), and the aforementioned electronic devices. Network equipment can include, but is not limited to, a single network server, a server group consisting of multiple network servers, or a cloud based on cloud computing consisting of a large number of computers or network servers. Cloud computing is a type of distributed computing, consisting of a super virtual computer composed of a group of loosely coupled computers. This embodiment does not limit this. Steps S1 to S4 are detailed as follows:

[0052] S1, collect 3D feature point cloud within the robot's movement space, and acquire IMU data based on the inertial measurement unit;

[0053] First, a 3D LiDAR sensor deployed on a mobile robot platform scans the surrounding environment to collect a 3D feature point cloud at the current moment. To improve processing efficiency and robustness, the original 3D feature point cloud is filtered, distorted, and feature-extracted to extract key feature points with geometric significance, such as edge points and face points, forming the 3D feature point cloud of the current frame.

[0054] At the same time, the onboard inertial measurement unit (IMU) is used to collect the angular velocity and acceleration information at the current moment, and the IMU data and laser point cloud data are synchronized through timestamps to provide inertial information support for subsequent attitude estimation.

[0055] S2, the pose pre-transformation of the 3D feature point cloud is performed using IMU data, the 3D feature point cloud is projected onto the 2D plane, the height attribute is preserved to obtain the 2D feature point cloud, the 2D feature point cloud is registered, and the point cloud registration result of the current frame is used as the preliminary odometry output.

[0056] After acquiring 3D feature point cloud and IMU data, the system performs pose pre-transformation processing on the current frame's 3D feature point cloud based on the angular velocity and acceleration information provided by the IMU data to obtain an initial pose estimate. Specifically, the pose change from the previous moment to the current moment can be estimated using IMU pre-integration to obtain the predicted pose transformation matrix for the current frame. Subsequently, the 3D feature point cloud is projected onto a 2D horizontal plane according to this pose prediction transformation matrix to form a 2D feature point cloud. To preserve the height features in the 3D structural information, the generated 2D point cloud retains the Z-axis height value in the attributes of each point, achieving 2.5D feature representation. Based on this 2D feature point cloud, the system uses a robust point cloud matching method to register the current frame's point cloud with the previous frame or a local map to obtain the pose increment estimate of the current frame relative to the previous frame. This registration result is the preliminary odometry output for this frame, used to represent the robot's relative motion information in the 2D plane.

[0057] S3, the initial odometer output is filtered using Kalman filtering to obtain the final odometer output;

[0058] To improve robustness to noise interference, dynamic changes, and sensor errors, a Kalman filter is introduced to fuse and optimize the initial odometry output. The predicted values ​​from the IMU data are used as the state prediction term, and the pose obtained from point cloud registration is used as the observation term, which is then input into the Kalman filter. By executing a prediction update loop, the filter can optimally estimate the system state, thereby suppressing estimation jitter caused by local registration errors, motion blur, or missing features. The final odometry output is smoother and more continuous for the current frame, and also includes the robot's position and heading information in the 2D plane at the current moment, which can be used by subsequent navigation, mapping, or control modules.

[0059] S4 updates the sub-map after each point cloud registration and overlays the feature point clouds of all registered frames to form a local sub-map.

[0060] After acquiring the final odometry output, the registered and corrected feature point cloud of the current frame is transformed into the sub-map coordinate system and fused with the existing local map to complete the sub-map update operation. Specifically, the 2D feature point cloud of the current frame is transformed into a unified reference coordinate system according to the final pose of the current frame, and then superimposed with the point clouds of several historical frames after registration to form a local sub-map for the current time window. The local sub-map is used for point cloud matching and registration in subsequent frames, and also supports the construction of a sliding window mechanism to control the map size, thereby improving system operating efficiency and long-term operational stability. By continuously superimposing and updating the sub-map, structural information in complex environments can be preserved and expanded, enhancing positioning and tracking capabilities.

[0061] The RANSAC (Random Sample Consistency) point cloud registration method was used to register the 2D feature point cloud.

[0062] The RANSAC algorithm is specifically designed for processing data containing a large number of outliers. In point cloud registration tasks, there are often mismatched point pairs caused by dynamic obstacles or perception errors in the environment. By using iterative sampling and "inlier count" as a scoring criterion, RANSAC can effectively remove these outliers, improving the stability and accuracy of the registration results.

[0063] The registration process for 2D feature point clouds is as follows:

[0064] Randomly select several feature point clouds, and query the nearest feature points in the stored local sub-map, add them to a queue as corresponding point pairs, and extract several corresponding point pairs;

[0065] The feature points of the current frame are called the source point set. ,in, Let i be the feature point of the current frame in the i-th corresponding point pair within the source point set. The number of corresponding point pairs, and the feature points derived from the local submap, are called the target point set. ,in, For the feature points originating from the local submap in the i-th feature point pair within the source point set, calculate the mean for both the source and target point sets, and then perform centering to obtain the first data matrix. and the second data matrix ;

[0066] Based on the first data matrix and the second data matrix Construct the covariance matrix;

[0067] The covariance matrix is ​​decomposed using SVD to obtain the orthogonal matrix. The rotation matrix is ​​then calculated, and the translation vector is solved. A homogeneous transformation matrix is ​​constructed based on the rotation matrix and the translation vector. ;

[0068] Apply rigid body transformation to all feature points in the current frame, find the number of inliers, and iteratively optimize the odometer output.

[0069] The covariance matrix is ​​decomposed by SVD (singular value decomposition). The orthogonal matrix is ​​obtained through the covariance matrix decomposition. Then, the rotation matrix is ​​calculated and the translation vector is solved. A homogeneous transformation matrix is ​​constructed based on the rotation matrix and the translation vector.

[0070] Apply rigid body transformation to all feature points in the current frame, find the number of inliers, and iteratively optimize the odometer output.

[0071] In this embodiment, to achieve robust registration of two-dimensional feature point clouds, a point cloud registration method based on the RANSAC algorithm is adopted. This method estimates the relative pose of the current frame by establishing the correspondence between feature points in the current frame and historical local sub-maps, estimating the transformation model, and evaluating the number of interior points. By centering the source point set P and the target point set Q, and performing SVD decomposition based on the covariance matrix, the obtained rotation matrix and displacement vector conform to the minimum mean square error criterion, exhibiting good numerical stability and analytical solution accuracy, thus avoiding the problem of gradient descent methods potentially getting trapped in local optima.

[0072] By combining the obtained rotation matrix and translation vector, a homogeneous transformation matrix is ​​constructed relative to the previous frame or sub-map, which is used to represent the robot's motion transformation in the local coordinate system.

[0073] After obtaining the homogeneous transformation matrix, it is applied to all feature points in the current frame to perform a rigid body transformation, resulting in the transformation of the current frame's point cloud in the sub-map reference frame. Next, the number of point pairs whose distances to the transformed feature points and their corresponding points in the sub-map are within a set threshold range is counted and used as an evaluation metric for the number of interior points in the current iteration.

[0074] To achieve better registration results, the RANSAC process aims to maximize the number of inliers by iteratively executing the above steps several times. Typically, a maximum number of iterations is set or an early convergence condition is met. Finally, the transformation matrix that maximizes the number of inliers is selected as the optimal registration result for the current frame. The optimal transformation result is used to output the relative pose estimate of the current frame, i.e., the preliminary odometry output of this frame.

[0075] For the source set and target point set The process of calculating the mean and then centering the values ​​is as follows:

[0076] Calculate the mean for the source point set and the target point set respectively:

[0077] In the formula, Let be the mean of the source point set. The mean of the target point set;

[0078] Based on the mean calculation results, the source point set and the target point set are centered respectively: where, For the feature points of the new frame in the i-th corresponding point pair within the centered source point set, The feature points originating from the local submap are the i-th feature point pair within the centered source point set.

[0079] Data matrices are constructed based on the centralized source and target point sets to obtain the first data matrix and the second data matrix.

[0080] To estimate the rigid body transformation of 2D point clouds between the current frame and the sub-map, the two key sets for point cloud registration—the source point set and the target point set—are first mean-calculated and centered to eliminate the influence of global translation. This allows subsequent calculations to focus on attitude and rotation information, thereby improving the accuracy and stability of rigid body registration. In practical applications, if there are 3 matching point pairs (n=3), then the source point set... target point set The mean of the source point set is obtained by calculating the mean of the source point set and the target point set respectively. The mean of the target point set Then, based on the mean calculation results, the source point set and the target point set are centered respectively to obtain... , At this point, the first data matrix is ​​obtained. and the second data matrix .

[0081] Centering effectively eliminates overall translational bias by transforming the point set into a coordinate system centered on its respective mean, resulting in more accurate subsequent rotation estimation. Centering the source and target point sets makes the construction of the covariance matrix more consistent with the minimum mean square error criterion, improving the numerical stability and robustness of the SVD solution process. The point cloud registration process after centering primarily estimates attitude rotation information, avoiding unnecessary translational interference and making the geometric meaning of the rotation matrix clearer and more independent. Compared to directly processing the original point cloud, centering helps reduce the impact of single-point distribution offsets and has a certain suppression effect on outlier interference, thereby improving the final registration quality. Centered feature points are easier to process uniformly during sub-map construction and multi-frame fusion, contributing to the construction of smoother and more accurate local maps.

[0082] The expression for constructing the covariance matrix based on the first and second data matrices is as follows:

[0083] In the formula, Let covariance matrix be the variance matrix. For the feature points of the new frame in the i-th corresponding point pair within the centered source point set, For the i-th feature point pair in the centered source point set, the feature points originating from the local submap are... This is for the transpose operation.

[0084] By performing joint statistics on the centered source and target point sets, the covariance matrix effectively captures the overall correspondence and spatial structure change trend of the two sets of points in two-dimensional or three-dimensional space, thus providing a stable foundation for subsequent pose estimation. Specifically, the covariance matrix can accurately express the spatial alignment direction between the current frame point cloud and the corresponding points in the map. By performing SVD on this matrix, the optimal rotation matrix can be obtained, and then the translation vector can be solved in conjunction with the mean offset, thereby constructing a rigid body transformation matrix and achieving high-precision point cloud pose estimation. The introduction of the covariance matrix not only improves the accuracy and stability of pose estimation, but also has good robustness. Especially when the feature point distribution is sparse or there are some mismatches, the main direction can still be extracted through global statistical characteristics, thus significantly improving the robustness and practicality of the point cloud registration algorithm in complex environments.

[0085] The covariance matrix is ​​decomposed using SVD to obtain an orthogonal matrix. Then, the rotation matrix is ​​calculated and the translation vector is solved. Finally, a homogeneous transformation matrix is ​​constructed based on the rotation matrix and the translation vector. The specific process is as follows:

[0086] The formulas for performing SVD decomposition on the covariance matrix to obtain orthogonal matrices and singular value diagonal matrices are as follows:

[0087] In the formula, It is an orthogonal matrix. It is a singular value diagonal matrix;

[0088] Calculate the rotation matrix based on orthogonal matrices:

[0089] In the formula, It is a rotation matrix;

[0090] Calculate the translation vector using the rotation matrix:

[0091] In the formula, It is a translation vector;

[0092] The homogeneous transformation matrix is ​​obtained by using the translation vector and the rotation matrix:

[0093] In the formula, It is a homogeneous transformation matrix.

[0094] By performing singular value decomposition (SVD) on the covariance matrix and sequentially calculating the rotation matrix and translation vector, the homogeneous transformation matrix is ​​finally constructed. This process essentially achieves the optimal rigid body transformation estimation between the source point set and the target point set. Its technical effect is reflected in the high-precision, low-drift, stable and robust point cloud registration capability.

[0095] By decomposing the covariance matrix, the extracted orthogonal matrix describes the principal directions and spatial distribution characteristics of the two sets of points. The constructed rotation matrix precisely aligns the pose relationships of the point cloud. The obtained translation vector corrects the relative positions of the point cloud in space. The final homogeneous transformation matrix unifies rotation and translation into a single four-dimensional matrix, facilitating efficient use in multi-frame data fusion, map stitching, and robot motion estimation.

[0096] Overall, the covariance matrix solution process not only avoids the local extremum traps common in iterative optimization, but also has high mathematical stability and engineering practicality, making it one of the key steps in realizing high-precision laser SLAM algorithms, point cloud map construction, and motion estimation.

[0097] Apply rigid body transformation to all feature points in the current frame, find the number of inliers, and iteratively optimize the odometry output, specifically including:

[0098] Apply rigid body transformation to all feature points in the current frame, find the number of feature points in the sub-map within a fixed distance of all feature points in the feature point cloud of the current frame after the transformation, and use this as the number of inliers.

[0099] Repeatedly and randomly select corresponding point pairs and apply rigid body transformations. When the number of interior points exceeds the number of interior points generated by previous transformations, receive a new transformation. Simultaneously, the transformed feature point cloud is stored to construct a local sub-map;

[0100] When receiving a new transformation The formula for calculating the new robot odometry output is: In the formula, Let be the transformation matrix at time i. Let be the transformation matrix at the (i-1)th moment.

[0101] By applying rigid body transformation to feature points of the current frame and iteratively optimizing the process by evaluating the number of inliers, a robust point cloud registration verification and optimization mechanism is essentially established. Its core technical effectiveness lies in adaptively filtering and refining the uncertainty of the initial transformation estimate, thereby improving the stability and accuracy of the odometry output. Specifically, the rigid body transformation is first applied to all feature points of the current frame, and matching points within a certain threshold distance are searched in the sub-map. These matching points are counted as the "number of inliers" to measure the registration effect. When a new transformation is generated by randomly selecting point pairs, resulting in an increase in the number of inliers, the system automatically replaces the current transformation, demonstrating the random optimal filtering characteristic of RANSAC and effectively eliminating erroneous pose estimations caused by mismatches. Subsequently, through... To achieve temporal updates of the robot's pose, the formula continuously multiplies the incremental pose of each frame with the cumulative pose of the previous frame, constructing the robot's continuous motion trajectory in the global coordinate system, thereby realizing a robust SLAM process.

[0102] Overall, this process effectively enhances the system's resistance to abnormal matching, local occlusion, and dynamic interference, and improves the accuracy of motion estimation through the interior point maximization criterion. It is one of the key steps in building a robust and high-performance laser SLAM algorithm.

[0103] like Figure 2 The diagram shows a flowchart of odometry calculation provided in this embodiment of the invention. First, a 3D feature point cloud is acquired from a LiDAR sensor, and IMU data is received simultaneously. The feature points undergo preliminary attitude correction via a "pose pre-transformation" module. The corrected 3D point cloud is projected onto a 2D plane, forming "projected feature points" containing height information, which are then extracted as a 2D feature point cloud for registration. Next, the RANSAC point cloud registration algorithm is used to match the current frame's 2D feature points with historical sub-maps, and the sub-maps are dynamically updated based on the registration results. Subsequently, the preliminary pose information obtained from registration is fused using a Kalman filter for multi-source fusion and noise suppression, improving estimation accuracy. Finally, a smooth and high-precision odometry result is output, providing support for precise positioning and navigation of the robot in complex environments. The entire process combines the dynamic stability of the IMU with the spatial geometric characteristics of the LiDAR point cloud, achieving robust SLAM mapping capabilities in dynamic, sparse, or violently moving environments.

[0104] A robot localization system based on laser SLAM, the system comprising:

[0105] The data acquisition and measurement module is used for the 3D feature point cloud of the mobile robot, and acquires IMU data based on the inertial measurement unit;

[0106] The transformation and registration module is used to perform pose pre-transformation on the 3D feature point cloud using IMU data, project the 3D feature point cloud onto the 2D plane, retain the height attribute to obtain the 2D feature point cloud, register the 2D feature point cloud, and output the point cloud registration result of the current frame as the preliminary odometry.

[0107] The output filtering module is used to filter the initial odometer output based on Kalman filtering to obtain the final odometer output;

[0108] The update overlay module is used to update the sub-map after each point cloud registration and overlay the feature point clouds of all registered frames to form a local sub-map.

[0109] like Figure 3 The diagram shown is a system block diagram of a robot localization system based on laser SLAM according to an embodiment of the present invention, which can be used to perform corresponding tasks. Figure 1 The steps in the method embodiments shown are implemented in a similar manner and have similar technical effects, and will not be repeated here.

[0110] An electronic device includes a memory and a processor, wherein the memory stores a computer program, and when the processor runs the computer program stored in the memory, the processor performs the steps of the laser SLAM-based robot localization method described above.

[0111] like Figure 4 The diagram shown is a hardware structure schematic of an electronic device according to an embodiment of the present invention. The electronic device includes: a processor 41, a memory 42, and a computer program; wherein:

[0112] The memory 42 is used to store the computer program, and the memory may also be flash memory. The computer program is, for example, an application program or functional module that implements the above method.

[0113] The processor 41 is configured to execute the computer program stored in the memory to implement the various steps performed by the device in the above method. For details, please refer to the relevant descriptions in the preceding method embodiments.

[0114] Alternatively, the memory 42 can be either standalone or integrated with the processor 41.

[0115] When the memory 42 is a device independent of the processor 41, the device may further include:

[0116] Bus 43 is used to connect the memory 42 and the processor 41.

[0117] A readable storage medium storing a computer program, which, when executed by a processor, is used to implement the steps of the laser SLAM-based robot localization method described above.

[0118] The readable storage medium can be a computer storage medium or a communication medium. A communication medium includes any medium that facilitates the transfer of computer programs from one location to another. A computer storage medium can be any available medium accessible to a general-purpose or special-purpose computer. For example, a readable storage medium is coupled to a processor, enabling the processor to read information from and write information to the readable storage medium. Of course, the readable storage medium can also be a component of the processor. The processor and the readable storage medium can reside in an Application-Specific Integrated Circuit (ASIC). Alternatively, the ASIC can be located in a user equipment. Of course, the processor and the readable storage medium can also exist as discrete components in a communication device. The readable storage medium can be a read-only memory (ROM), random access memory (RAM), CD-ROM, magnetic tape, floppy disk, and optical data storage device, etc.

[0119] The present invention also provides a program product including executable instructions stored in a readable storage medium. At least one processor of the device can read the executable instructions from the readable storage medium, and the at least one processor executes the executable instructions to cause the device to implement the methods provided in the various embodiments described above.

[0120] In the embodiments of the above-described device, it should be understood that the processor can be a Central Processing Unit (CPU), or other general-purpose processors, digital signal processors (DSPs), application-specific integrated circuits (ASICs), etc. The general-purpose processor can be a microprocessor or any conventional processor. The steps of the method disclosed in this invention can be directly manifested as execution by a hardware processor, or execution by a combination of hardware and software modules within the processor.

[0121] Through the above embodiments, this invention, using a laser SLAM-based robot localization method, acquires 3D feature point clouds from a lidar radar and simultaneously receives IMU data. Preliminary pose correction is performed on the feature points through pose pre-transformation. The corrected 3D point cloud is projected onto a 2D plane to form projected feature points containing height information, which are extracted as 2D feature point clouds for registration. The RANSAC point cloud registration algorithm is used to match the current frame's 2D feature points with historical sub-maps, and the sub-maps are updated based on the registration results. Kalman filtering is used to filter the preliminary odometry output to obtain the final odometry output, improving estimation accuracy and producing smooth and high-precision odometry results. This provides support for precise robot localization and navigation in complex environments, achieving robust SLAM mapping capabilities in dynamic, sparse, or violent motion environments.

[0122] The robot localization method based on laser SLAM in this invention combines the spatial information integrity of 3D point clouds with the computational efficiency of 2D algorithms. It can reduce redundant calculations of six degrees of freedom while preserving key height features, significantly improving the localization robustness and real-time performance of the system in environments with sparse features, dynamic object interference, or violent motion. At the same time, it avoids the performance degradation caused by information loss in traditional 2D SLAM, making it an efficient solution for mapping and localization in complex scenarios for ground mobile robots.

[0123] The above description is merely a specific embodiment of this application, but the scope of protection of this application is not limited thereto. Any changes or substitutions that can be easily conceived by those skilled in the art within the scope of the technology disclosed in this application should be included within the scope of protection of this application.

[0124] Finally: The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the protection scope of the present invention.

Claims

1. A robot localization method based on laser SLAM, characterized in that, Includes the following steps: Collect 3D feature point clouds within the robot's movement space and acquire IMU data based on the inertial measurement unit; The pose pre-transformation of the 3D feature point cloud is performed using IMU data. The 3D feature point cloud is then projected onto a 2D plane, and the height attribute is preserved to obtain the 2D feature point cloud. The 2D feature point cloud is then registered, and the point cloud registration result of the current frame is used as the preliminary odometry output. The registration process for the 2D feature point cloud is as follows: Randomly select several feature point clouds, and query the nearest feature point in the stored local sub-map. Add the feature point and the nearest feature point in the local sub-map to a queue as a corresponding point pair, and extract several corresponding point pairs. The feature points of the current frame are called the source point set. ,in, Let i be the feature point of the current frame in the i-th corresponding point pair within the source point set. The number of corresponding point pairs, and the feature points derived from the local submap, are called the target point set. ,in, For the feature points originating from the local submap in the i-th feature point pair within the source point set, calculate the mean for both the source and target point sets, and then perform centering to obtain the first data matrix. and the second data matrix ; Based on the first data matrix and the second data matrix Construct the covariance matrix; The covariance matrix is ​​decomposed using SVD to obtain the orthogonal matrix. The rotation matrix is ​​then calculated, and the translation vector is solved. A homogeneous transformation matrix is ​​constructed based on the rotation matrix and the translation vector. ; Apply rigid body transformation to all feature points in the current frame, find the number of inliers, and iteratively optimize the odometry output. The final odometer output is obtained by filtering the initial odometer output using Kalman filtering. After each point cloud registration is completed, the sub-map is updated, and the feature point clouds of all registered frames are superimposed to form a local sub-map.

2. The robot localization method based on laser SLAM according to claim 1, characterized in that, The RANSAC point cloud registration method was used to register the 2D feature point cloud.

3. The robot localization method based on laser SLAM according to claim 1, characterized in that, For the source set and target point set The process of calculating the mean and then centering the values ​​is as follows: For the source set and target point set Calculate the mean separately: In the formula, Let be the mean of the source point set. The mean of the target point set; Based on the mean calculation results, the source point set and the target point set are centered respectively: In the formula, For the feature points of the new frame in the i-th corresponding point pair within the centered source point set, The feature points originating from the local submap are the i-th feature point pair within the centered source point set. A first data matrix is ​​obtained by constructing data matrices based on the centralized source and target point sets. and the second data matrix .

4. The robot localization method based on laser SLAM according to claim 1, characterized in that, Based on the first data matrix and the second data matrix The expression for constructing the covariance matrix is ​​as follows: In the formula, Let covariance matrix be the variance matrix. For the feature points of the new frame in the i-th corresponding point pair within the centered source point set, For the i-th feature point pair in the centered source point set, the feature points originating from the local submap are... This is for the transpose operation.

5. The robot localization method based on laser SLAM according to claim 1, characterized in that, The covariance matrix is ​​decomposed using SVD to obtain the orthogonal matrix. Then, the rotation matrix is ​​calculated and the translation vector is solved. Based on the rotation matrix and translation vector, a homogeneous transformation matrix is ​​constructed. The specific process is as follows: The formulas for performing SVD decomposition on the covariance matrix to obtain orthogonal matrices and singular value diagonal matrices are as follows: In the formula, It is an orthogonal matrix. It is a singular value diagonal matrix; Calculate the rotation matrix based on orthogonal matrices: In the formula, It is a rotation matrix; Calculate the translation vector using the rotation matrix: In the formula, It is a translation vector; The homogeneous transformation matrix is ​​obtained by using the translation vector and the rotation matrix: In the formula, It is a homogeneous transformation matrix.

6. The robot localization method based on laser SLAM according to claim 1, characterized in that, Apply rigid body transformation to all feature points in the current frame, find the number of inliers, and iteratively optimize the odometry output, specifically including: Apply rigid body transformation to all feature points in the current frame, find the number of feature points in the sub-map within a fixed distance of all feature points in the feature point cloud of the current frame after the transformation, and use this as the number of inliers. Repeatedly and randomly select corresponding point pairs and apply rigid body transformations. When the number of interior points exceeds the number of interior points generated by previous transformations, receive a new transformation. Simultaneously, the transformed feature point cloud is stored to construct a local sub-map; When receiving a new transformation The formula for calculating the new robot odometry output is: In the formula, Let be the transformation matrix at time i. Let be the transformation matrix at the (i-1)th moment.

7. A robot localization system based on laser SLAM, applied to the robot localization method based on laser SLAM as described in any one of claims 1-6, characterized in that, The system includes: The data acquisition and measurement module is used for the 3D feature point cloud of the mobile robot, and acquires IMU data based on the inertial measurement unit; The transformation and registration module is used to perform pose pre-transformation on the 3D feature point cloud using IMU data, project the 3D feature point cloud onto the 2D plane, retain the height attribute to obtain the 2D feature point cloud, register the 2D feature point cloud, and output the point cloud registration result of the current frame as the preliminary odometry. The output filtering module is used to filter the initial odometer output based on Kalman filtering to obtain the final odometer output; The update overlay module is used to update the sub-map after each point cloud registration and overlay the feature point clouds of all registered frames to form a local sub-map.

8. An electronic device comprising a memory and a processor, wherein the memory stores a computer program, characterized in that, When the processor runs the computer program stored in the memory, the processor performs the steps of the laser SLAM-based robot localization method as described in any one of claims 1-6.

9. A readable storage medium storing a computer program, characterized in that, When the computer program is executed by the processor, it is used to implement the steps of the robot localization method based on laser SLAM as described in any one of claims 1-6.

Images